2021-02-12T23:01:13Z* LcmTrn::LcmTrn() - configuration file /home/tethysadmin/Downloads/trncfgs/lcm-trn.cfg 2021-02-12T23:01:13Z* LcmTrn::init() - using configuration file /home/tethysadmin/Downloads/trncfgs/lcm-trn.cfg 2021-02-12T23:01:13Z* ahrs config: AHRS_M2, platform_orientation, platform_pitch_angle, platform_roll_angle 2021-02-12T23:01:13Z* dvl config: RDI_Pathfinder, Beam1Range, Beam2Range, Beam3Range, Beam4Range, BottomVelocityFlag 2021-02-12T23:01:13Z* nav config: DeadReckonUsingMultipleVelocitySources, latitude, longitude 2021-02-12T23:01:13Z* depth config: Depth_Keller, depth, sea_water_pressure 2021-02-12T23:01:13Z* trn config: TRN, trn_mle, trn_mmse, trn_var, trn_reinits, trn_filterstate 2021-02-12T23:01:13Z* LCM timeout=5.00 sec 2021-02-12T23:01:13Z* TRN settings: 2021-02-12T23:01:13Z* period=3.00 sec 2021-02-12T23:01:13Z* coherence=2.00 sec 2021-02-12T23:01:13Z* map = PortTiles cfg = mappingAUV_specs.cfg part= particles.cfg logdir= lrauv 2021-02-12T23:01:13Z* maptype = 2 filtertype = 3 weighting = 0 2021-02-12T23:01:13Z* lowgrade_filter = 1 allow reinit = 1 2021-02-12T23:01:13Z* cmd_estimate = cmd_estimate cmd_reinit = cmd_reinit 2021-02-12T23:01:13Z* LcmTrn::initTrn() - configuration file /home/tethysadmin/Downloads/trncfgs/lcm-trn.cfg 2021-02-12T23:01:13Z* LcmTrn::initTrn() - map: /home/tethysadmin/Downloads/trnmaps/PortTiles 2021-02-12T23:01:13Z* LcmTrn::initTrn() - cfg: /home/tethysadmin/Downloads/trncfgs/mappingAUV_specs.cfg 2021-02-12T23:01:13Z* LcmTrn::initTrn() - part: /home/tethysadmin/Downloads/trncfgs/particles.cfg 2021-02-12T23:01:13Z* TerrainMapOctree::tileDataName = /home/tethysadmin/Downloads/trnmaps/PortTiles/tiles.csv 2021-02-12T23:01:13Z* TerrainMapOctree::numTiles_ = 9. 2021-02-12T23:01:13Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile1.bo 4061250.0 594250.0 . 2021-02-12T23:01:13Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile2.bo 4061250.0 594750.0 . 2021-02-12T23:01:13Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile3.bo 4061250.0 595250.0 . 2021-02-12T23:01:13Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile4.bo 4061750.0 594250.0 . 2021-02-12T23:01:13Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile5.bo 4061750.0 594750.0 . 2021-02-12T23:01:13Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile6.bo 4061750.0 595250.0 . 2021-02-12T23:01:13Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile7.bo 4062250.0 594250.0 . 2021-02-12T23:01:13Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile8.bo 4062250.0 594750.0 . 2021-02-12T23:01:13Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile9.bo 4062250.0 595250.0 . 2021-02-12T23:01:13Z* TerrainMapOctree::loaded 9 useable tiles from /home/tethysadmin/Downloads/trnmaps/PortTiles/tiles.csv. 2021-02-12T23:01:13Z* TerrainMapOctree::Octree tile load /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile1.bo took 0.491314 seconds. 2021-02-12T23:01:13Z* TRN log directory is /home/tethysadmin/Downloads/trnlogs/lrauv-TRN.40 2021-02-12T23:01:13Z* symlink /home/tethysadmin/Downloads/trnlogs/latestTRN to /home/tethysadmin/Downloads/trnlogs/lrauv-TRN.40 OK 2021-02-12T23:01:13Z* parseSensorSpecs parsing sensor of type 1. 2021-02-12T23:01:13Z* Sensor 0 is of type 1. 2021-02-12T23:01:13Z* parseSensorSpecs parsing sensor of type 2. 2021-02-12T23:01:13Z* Sensor 1 is of type 2. 2021-02-12T23:01:13Z* parseSensorSpecs parsing sensor of type 5. 2021-02-12T23:01:13Z* Sensor 2 is of type 5. 2021-02-12T23:01:13Z- File created[/home/tethysadmin/Downloads/trnlogs/latestTRN/trn.log.0000] returned[0x556d3c58d0] 2021-02-12T23:01:13Z- TNavConfig::setParticlesFile: value is now particles.cfg 2021-02-12T23:01:13Z- TNavConfig::setMapFile: value is now /home/tethysadmin/Downloads/trnmaps/PortTiles 2021-02-12T23:01:13Z- TNavConfig::setVehicleSpecsFile: value is now /home/tethysadmin/Downloads/trncfgs/mappingAUV_specs.cfg 2021-02-12T23:01:13Z- TNavConfig::setParticlesFile: value is now /home/tethysadmin/Downloads/trncfgs/particles.cfg 2021-02-12T23:01:13Z- TNavConfig::setLogDir: value is now /home/tethysadmin/Downloads/trnlogs/lrauv-TRN.40 2021-02-12T23:01:13Z- Loading vehicle config file... /home/tethysadmin/Downloads/trncfgs/mappingAUV_specs.cfg 2021-02-12T23:01:13Z- parseSensorSpecs parsing sensor of type 1. 2021-02-12T23:01:13Z- Sensor 0 is of type 1. 2021-02-12T23:01:13Z- parseSensorSpecs parsing sensor of type 2. 2021-02-12T23:01:13Z- Sensor 1 is of type 2. 2021-02-12T23:01:13Z- parseSensorSpecs parsing sensor of type 5. 2021-02-12T23:01:13Z- Sensor 2 is of type 5. 2021-02-12T23:01:13Z- Random noise maker initialized with 1613170873 2021-02-12T23:01:13Z- setDistribToSave(0) 2021-02-12T23:01:13Z- setDistribToSave set to 0 2021-02-12T23:01:13Z- Vehicle name: mappingAUV Number of sensors: 3 2021-02-12T23:01:13Z- Sensor #1: 2021-02-12T23:01:13Z- Sensor name: rdiDVL 2021-02-12T23:01:13Z- Sensor type: 1 2021-02-12T23:01:13Z- Number of beams per measurement: 4 2021-02-12T23:01:13Z- Sensor #1 to vehicle transformation information: 2021-02-12T23:01:13Z- Rotation angles (phi, theta, psi): (0.000000 ,0.000000, 0.000000) 2021-02-12T23:01:13Z- Translation vector [dx, dy, dz]: (0.000000 ,0.000000, 0.000000) 2021-02-12T23:01:13Z- 2021-02-12T23:01:13Z- Sensor #2: 2021-02-12T23:01:13Z- Sensor name: resonSeabat8101 2021-02-12T23:01:13Z- Sensor type: 2 2021-02-12T23:01:13Z- Number of beams per measurement: 11 2021-02-12T23:01:13Z- Sensor #2 to vehicle transformation information: 2021-02-12T23:01:13Z- Rotation angles (phi, theta, psi): (0.000000 ,0.000000, 0.000000) 2021-02-12T23:01:13Z- Translation vector [dx, dy, dz]: (0.000000 ,0.000000, 0.000000) 2021-02-12T23:01:13Z- 2021-02-12T23:01:13Z- Sensor #3: 2021-02-12T23:01:13Z- Sensor name: imagenexdeltat 2021-02-12T23:01:13Z- Sensor type: 5 2021-02-12T23:01:13Z- Number of beams per measurement: 120 2021-02-12T23:01:13Z- Sensor #3 to vehicle transformation information: 2021-02-12T23:01:13Z- Rotation angles (phi, theta, psi): (0.000000 ,0.000000, 0.000000) 2021-02-12T23:01:13Z- Translation vector [dx, dy, dz]: (0.000000 ,0.000000, 0.000000) 2021-02-12T23:01:13Z- 2021-02-12T23:01:13Z- TNavBF::nParticles: 10000 numFilters: 2 PMF Grid Resolution: 1.0 PMF Grid Size: 100 2021-02-12T23:01:13Z- TerrainNav::TNavFilter initialized with type 3 2021-02-12T23:01:13Z- TerrainNav::initVariables finished. 2021-02-12T23:01:13Z- TerrainNav::Constructor finished. 2021-02-12T23:01:13Z- TNavBF::setModifiedWeighting umw: 4 2021-02-12T23:01:13Z- TNavBF::Filter configuration: 0 Filter comparison: 0 Distance threshold 3.0 Weight ratio threshold 0.20 2021-02-12T23:01:13Z- TNavBF::numFilters: 1 2021-02-12T23:01:13Z- TerrainNav::modified weighting set to 0 2021-02-12T23:01:13Z- LcmTrn::initLcm() - configuration file /home/tethysadmin/Downloads/trncfgs/lcm-trn.cfg 2021-02-12T23:01:13Z- LcmTrn::run() 2021-02-12T23:01:13Z- AHRS_M2 msg: 1613170873.83 epoch sec; seqNo:38 2021-02-12T23:01:13Z- AHRS_M2 msg: 5.39 phi; -0.22 theta; -0.16 psi 2021-02-12T23:01:13Z- handleDepth()-Depth_Keller msg: 1613170873837 epoch sec; seqNo:38; depth 0.17 2021-02-12T23:01:14Z- handleDvl() - RDI_Pathfinder msg: 1613170874.52 epoch sec; seqNo:23 2021-02-12T23:01:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.52 , 109.43 , 82.64 , 118.36 2021-02-12T23:01:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.17 , -0.23 2021-02-12T23:01:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170874.53 epoch sec; seqNo:38; 4056073.90 north; 590342.62 east 2021-02-12T23:01:14Z- AHRS_M2 msg: 1613170874.61 epoch sec; seqNo:39 2021-02-12T23:01:14Z- AHRS_M2 msg: 5.37 phi; -0.16 theta; -0.09 psi 2021-02-12T23:01:14Z- handleDepth()-Depth_Keller msg: 1613170874615 epoch sec; seqNo:39; depth 0.15 2021-02-12T23:01:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170874.63 epoch sec; seqNo:39; 4056073.90 north; 590342.62 east 2021-02-12T23:01:15Z- AHRS_M2 msg: 1613170875.00 epoch sec; seqNo:40 2021-02-12T23:01:15Z- AHRS_M2 msg: 5.34 phi; -0.19 theta; -0.02 psi 2021-02-12T23:01:15Z- handleDepth()-Depth_Keller msg: 1613170875008 epoch sec; seqNo:40; depth 0.15 2021-02-12T23:01:17Z- handleDvl() - RDI_Pathfinder msg: 1613170877.06 epoch sec; seqNo:24 2021-02-12T23:01:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.48 , 102.39 , 86.02 , 105.37 2021-02-12T23:01:17Z- handleDvl() - RDI_Pathfinder msg: velocities -0.14 , -0.27 , 0.70 2021-02-12T23:01:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170877.07 epoch sec; seqNo:40; 4056073.14 north; 590342.62 east 2021-02-12T23:01:17Z- AHRS_M2 msg: 1613170877.17 epoch sec; seqNo:41 2021-02-12T23:01:17Z- AHRS_M2 msg: 5.36 phi; -0.07 theta; -0.10 psi 2021-02-12T23:01:17Z- handleDepth()-Depth_Keller msg: 1613170877172 epoch sec; seqNo:41; depth 0.07 2021-02-12T23:01:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170877.18 epoch sec; seqNo:41; 4056073.14 north; 590342.62 east 2021-02-12T23:01:17Z- AHRS_M2 msg: 1613170877.56 epoch sec; seqNo:42 2021-02-12T23:01:17Z- AHRS_M2 msg: 5.36 phi; -0.22 theta; -0.02 psi 2021-02-12T23:01:17Z- handleDepth()-Depth_Keller msg: 1613170877561 epoch sec; seqNo:42; depth 0.08 2021-02-12T23:01:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170877.58 epoch sec; seqNo:42; 4056072.76 north; 590342.63 east 2021-02-12T23:01:18Z- AHRS_M2 msg: 1613170878.07 epoch sec; seqNo:43 2021-02-12T23:01:18Z- AHRS_M2 msg: 5.36 phi; -0.21 theta; -0.03 psi 2021-02-12T23:01:18Z- handleDepth()-Depth_Keller msg: 1613170878084 epoch sec; seqNo:43; depth 0.05 2021-02-12T23:01:18Z- handleDvl() - RDI_Pathfinder msg: 1613170878.11 epoch sec; seqNo:25 2021-02-12T23:01:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.52 , 96.17 , 0.00 , 108.35 2021-02-12T23:01:18Z- handleDvl() - RDI_Pathfinder msg: velocities -0.31 , -0.06 , -0.23 2021-02-12T23:01:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170878.12 epoch sec; seqNo:43; 4056072.76 north; 590342.63 east 2021-02-12T23:01:18Z- AHRS_M2 msg: 1613170878.36 epoch sec; seqNo:44 2021-02-12T23:01:18Z- AHRS_M2 msg: 5.35 phi; -0.13 theta; -0.09 psi 2021-02-12T23:01:18Z- handleDepth()-Depth_Keller msg: 1613170878367 epoch sec; seqNo:44; depth 0.03 2021-02-12T23:01:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170878.38 epoch sec; seqNo:44; 4056072.76 north; 590342.63 east 2021-02-12T23:01:18Z- AHRS_M2 msg: 1613170878.77 epoch sec; seqNo:45 2021-02-12T23:01:18Z- AHRS_M2 msg: 5.35 phi; -0.03 theta; -0.17 psi 2021-02-12T23:01:18Z- handleDepth()-Depth_Keller msg: 1613170878778 epoch sec; seqNo:45; depth 0.02 2021-02-12T23:01:18Z- LcmTrn::handleMessages() - handling 26 messages took 0.54 ms and 115 calls... 2021-02-12T23:01:18Z- waiting for 2.00, time is 1613170878.77 2021-02-12T23:01:18Z- LcmTrn::updateTrn() >>>> pose time 1613170878.77, meas time 1613170878.11 <<<< 2021-02-12T23:01:18Z- TerrainNav::Measurement type 1 from time = 1613170878.11 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:01:18Z- TerrainNav::motionUpdate() - currEstimate = (4056073, 590342.63, 0.02) 2021-02-12T23:01:18Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:01:18Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:01:18Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:01:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:01:18Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:01:18Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:01:18Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:01:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170878.79 epoch sec; seqNo:45; 4056072.76 north; 590342.63 east 2021-02-12T23:01:19Z- AHRS_M2 msg: 1613170879.17 epoch sec; seqNo:46 2021-02-12T23:01:19Z- AHRS_M2 msg: 5.42 phi; 0.07 theta; -0.18 psi 2021-02-12T23:01:19Z- handleDepth()-Depth_Keller msg: 1613170879187 epoch sec; seqNo:46; depth 0.02 2021-02-12T23:01:19Z- handleDvl() - RDI_Pathfinder msg: 1613170879.20 epoch sec; seqNo:26 2021-02-12T23:01:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.21 , 101.04 , 96.98 , 103.75 2021-02-12T23:01:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.38 , -0.19 , 0.17 2021-02-12T23:01:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170879.21 epoch sec; seqNo:46; 4056072.76 north; 590342.63 east 2021-02-12T23:01:19Z- AHRS_M2 msg: 1613170879.58 epoch sec; seqNo:47 2021-02-12T23:01:19Z- AHRS_M2 msg: 5.49 phi; 0.15 theta; -0.02 psi 2021-02-12T23:01:19Z- handleDepth()-Depth_Keller msg: 1613170879580 epoch sec; seqNo:47; depth 0.05 2021-02-12T23:01:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170879.59 epoch sec; seqNo:47; 4056072.76 north; 590342.63 east 2021-02-12T23:01:19Z- AHRS_M2 msg: 1613170879.99 epoch sec; seqNo:48 2021-02-12T23:01:19Z- AHRS_M2 msg: 5.49 phi; 0.14 theta; 0.12 psi 2021-02-12T23:01:20Z- handleDepth()-Depth_Keller msg: 1613170880000 epoch sec; seqNo:48; depth 0.08 2021-02-12T23:01:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170880.04 epoch sec; seqNo:48; 4056072.76 north; 590342.63 east 2021-02-12T23:01:20Z- AHRS_M2 msg: 1613170880.38 epoch sec; seqNo:49 2021-02-12T23:01:20Z- AHRS_M2 msg: 5.42 phi; 0.09 theta; 0.08 psi 2021-02-12T23:01:20Z- handleDepth()-Depth_Keller msg: 1613170880397 epoch sec; seqNo:49; depth 0.10 2021-02-12T23:01:20Z- handleDvl() - RDI_Pathfinder msg: 1613170880.41 epoch sec; seqNo:27 2021-02-12T23:01:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.25 , 102.66 , 97.25 , 97.25 2021-02-12T23:01:20Z- handleDvl() - RDI_Pathfinder msg: velocities -0.01 , 0.46 , 0.74 2021-02-12T23:01:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170880.42 epoch sec; seqNo:49; 4056073.14 north; 590342.62 east 2021-02-12T23:01:20Z- AHRS_M2 msg: 1613170880.79 epoch sec; seqNo:50 2021-02-12T23:01:20Z- AHRS_M2 msg: 5.40 phi; -0.01 theta; -0.08 psi 2021-02-12T23:01:20Z- handleDepth()-Depth_Keller msg: 1613170880797 epoch sec; seqNo:50; depth 0.05 2021-02-12T23:01:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170880.82 epoch sec; seqNo:50; 4056073.14 north; 590342.62 east 2021-02-12T23:01:21Z- AHRS_M2 msg: 1613170881.19 epoch sec; seqNo:51 2021-02-12T23:01:21Z- AHRS_M2 msg: 5.45 phi; -0.18 theta; -0.19 psi 2021-02-12T23:01:21Z- handleDepth()-Depth_Keller msg: 1613170881196 epoch sec; seqNo:51; depth 0.01 2021-02-12T23:01:21Z- handleDvl() - RDI_Pathfinder msg: 1613170881.21 epoch sec; seqNo:28 2021-02-12T23:01:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.24 , 94.48 , 97.18 , 97.18 2021-02-12T23:01:21Z- handleDvl() - RDI_Pathfinder msg: velocities -1.11 , -0.12 , 0.09 2021-02-12T23:01:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170881.23 epoch sec; seqNo:51; 4056072.76 north; 590342.63 east 2021-02-12T23:01:21Z- AHRS_M2 msg: 1613170881.60 epoch sec; seqNo:52 2021-02-12T23:01:21Z- AHRS_M2 msg: 5.45 phi; -0.24 theta; -0.16 psi 2021-02-12T23:01:21Z- handleDepth()-Depth_Keller msg: 1613170881605 epoch sec; seqNo:52; depth 0.03 2021-02-12T23:01:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170881.62 epoch sec; seqNo:52; 4056072.38 north; 590342.63 east 2021-02-12T23:01:22Z- AHRS_M2 msg: 1613170882.02 epoch sec; seqNo:53 2021-02-12T23:01:22Z- AHRS_M2 msg: 5.42 phi; -0.21 theta; -0.09 psi 2021-02-12T23:01:22Z- handleDepth()-Depth_Keller msg: 1613170882026 epoch sec; seqNo:53; depth 0.04 2021-02-12T23:01:22Z- handleDvl() - RDI_Pathfinder msg: 1613170882.04 epoch sec; seqNo:29 2021-02-12T23:01:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.13 , 109.43 , 85.07 , 113.49 2021-02-12T23:01:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.34 , -0.48 , -1.25 2021-02-12T23:01:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170882.07 epoch sec; seqNo:53; 4056072.38 north; 590342.63 east 2021-02-12T23:01:22Z- AHRS_M2 msg: 1613170882.40 epoch sec; seqNo:54 2021-02-12T23:01:22Z- AHRS_M2 msg: 5.45 phi; -0.14 theta; -0.02 psi 2021-02-12T23:01:22Z- handleDepth()-Depth_Keller msg: 1613170882413 epoch sec; seqNo:54; depth 0.07 2021-02-12T23:01:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170882.43 epoch sec; seqNo:54; 4056072.38 north; 590342.63 east 2021-02-12T23:01:22Z- AHRS_M2 msg: 1613170882.81 epoch sec; seqNo:55 2021-02-12T23:01:22Z- AHRS_M2 msg: 5.45 phi; -0.06 theta; 0.09 psi 2021-02-12T23:01:22Z- handleDepth()-Depth_Keller msg: 1613170882813 epoch sec; seqNo:55; depth 0.12 2021-02-12T23:01:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170882.82 epoch sec; seqNo:55; 4056072.38 north; 590342.63 east 2021-02-12T23:01:23Z- AHRS_M2 msg: 1613170883.21 epoch sec; seqNo:56 2021-02-12T23:01:23Z- AHRS_M2 msg: 5.43 phi; -0.08 theta; 0.10 psi 2021-02-12T23:01:23Z- handleDepth()-Depth_Keller msg: 1613170883221 epoch sec; seqNo:56; depth 0.14 2021-02-12T23:01:23Z- handleDvl() - RDI_Pathfinder msg: 1613170883.23 epoch sec; seqNo:30 2021-02-12T23:01:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 97.11 , 97.11 , 103.88 2021-02-12T23:01:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.40 , 0.19 2021-02-12T23:01:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170883.24 epoch sec; seqNo:56; 4056072.37 north; 590341.41 east 2021-02-12T23:01:23Z- AHRS_M2 msg: 1613170883.62 epoch sec; seqNo:57 2021-02-12T23:01:23Z- AHRS_M2 msg: 5.43 phi; -0.16 theta; -0.02 psi 2021-02-12T23:01:23Z- handleDepth()-Depth_Keller msg: 1613170883622 epoch sec; seqNo:57; depth 0.13 2021-02-12T23:01:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170883.63 epoch sec; seqNo:57; 4056072.75 north; 590341.41 east 2021-02-12T23:01:23Z- LcmTrn::handleMessages() - handling 42 messages took 0.69 ms and 128 calls... 2021-02-12T23:01:23Z- waiting for 1613170881.77, time is 1613170883.63 2021-02-12T23:01:23Z- LcmTrn::updateTrn() >>>> pose time 1613170883.63, meas time 1613170883.23 <<<< 2021-02-12T23:01:23Z- TerrainNav:: Throwing out all beams because more than two are equal to 97.115 at t=1613170883.23, ping # 0. 2021-02-12T23:01:23Z- TerrainNav::Measurement type 1 from time = 1613170883.23 sec. not included; vehicle motion has not been initialized. lastMeasValid = 0, beams = 4, ping # 0 2021-02-12T23:01:23Z- TerrainNav::motionUpdate() - currEstimate = (4056073, 590341.41, 0.13) 2021-02-12T23:01:23Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:01:23Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:01:23Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:01:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:01:23Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:01:23Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:01:23Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:01:24Z- AHRS_M2 msg: 1613170884.03 epoch sec; seqNo:58 2021-02-12T23:01:24Z- AHRS_M2 msg: 5.47 phi; -0.20 theta; -0.11 psi 2021-02-12T23:01:24Z- handleDepth()-Depth_Keller msg: 1613170884045 epoch sec; seqNo:58; depth 0.12 2021-02-12T23:01:24Z- handleDvl() - RDI_Pathfinder msg: 1613170884.07 epoch sec; seqNo:31 2021-02-12T23:01:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.18 , 94.48 , 86.36 , 105.30 2021-02-12T23:01:24Z- handleDvl() - RDI_Pathfinder msg: velocities -0.20 , -0.37 , 0.43 2021-02-12T23:01:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170884.08 epoch sec; seqNo:58; 4056072.37 north; 590341.41 east 2021-02-12T23:01:24Z- AHRS_M2 msg: 1613170884.42 epoch sec; seqNo:59 2021-02-12T23:01:24Z- AHRS_M2 msg: 5.49 phi; -0.24 theta; -0.06 psi 2021-02-12T23:01:24Z- handleDepth()-Depth_Keller msg: 1613170884429 epoch sec; seqNo:59; depth 0.12 2021-02-12T23:01:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170884.46 epoch sec; seqNo:59; 4056072.37 north; 590341.41 east 2021-02-12T23:01:24Z- AHRS_M2 msg: 1613170884.83 epoch sec; seqNo:60 2021-02-12T23:01:24Z- AHRS_M2 msg: 5.46 phi; -0.33 theta; 0.02 psi 2021-02-12T23:01:24Z- handleDepth()-Depth_Keller msg: 1613170884833 epoch sec; seqNo:60; depth 0.14 2021-02-12T23:01:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170884.86 epoch sec; seqNo:60; 4056071.99 north; 590341.41 east 2021-02-12T23:01:25Z- AHRS_M2 msg: 1613170885.23 epoch sec; seqNo:61 2021-02-12T23:01:25Z- AHRS_M2 msg: 5.43 phi; -0.40 theta; 0.04 psi 2021-02-12T23:01:25Z- handleDepth()-Depth_Keller msg: 1613170885241 epoch sec; seqNo:61; depth 0.16 2021-02-12T23:01:25Z- handleDvl() - RDI_Pathfinder msg: 1613170885.32 epoch sec; seqNo:32 2021-02-12T23:01:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:01:25Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:01:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170885.34 epoch sec; seqNo:61; 4056071.61 north; 590341.42 east 2021-02-12T23:01:25Z- AHRS_M2 msg: 1613170885.63 epoch sec; seqNo:62 2021-02-12T23:01:25Z- AHRS_M2 msg: 5.41 phi; -0.40 theta; 0.02 psi 2021-02-12T23:01:25Z- handleDepth()-Depth_Keller msg: 1613170885641 epoch sec; seqNo:62; depth 0.16 2021-02-12T23:01:26Z- handleDvl() - RDI_Pathfinder msg: 1613170886.87 epoch sec; seqNo:33 2021-02-12T23:01:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.21 , 101.72 , 83.85 , 131.49 2021-02-12T23:01:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.08 , -0.09 , -0.38 2021-02-12T23:01:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170886.89 epoch sec; seqNo:62; 4056071.99 north; 590341.41 east 2021-02-12T23:01:27Z- AHRS_M2 msg: 1613170887.07 epoch sec; seqNo:63 2021-02-12T23:01:27Z- AHRS_M2 msg: 5.42 phi; -0.23 theta; -0.11 psi 2021-02-12T23:01:27Z- handleDepth()-Depth_Keller msg: 1613170887114 epoch sec; seqNo:63; depth 0.16 2021-02-12T23:01:27Z- handleDvl() - RDI_Pathfinder msg: 1613170887.16 epoch sec; seqNo:34 2021-02-12T23:01:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.24 , 100.70 , 84.33 , 115.59 2021-02-12T23:01:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.46 , -0.13 , 0.05 2021-02-12T23:01:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170887.17 epoch sec; seqNo:63; 4056071.99 north; 590341.41 east 2021-02-12T23:01:27Z- AHRS_M2 msg: 1613170887.40 epoch sec; seqNo:64 2021-02-12T23:01:27Z- AHRS_M2 msg: 5.39 phi; -0.23 theta; -0.08 psi 2021-02-12T23:01:27Z- handleDepth()-Depth_Keller msg: 1613170887401 epoch sec; seqNo:64; depth 0.18 2021-02-12T23:01:28Z- LcmTrn::handleMessages() - handling 24 messages took 0.56 ms and 113 calls... 2021-02-12T23:01:28Z- waiting for 1613170886.63, time is 1613170887.40 2021-02-12T23:01:28Z- LcmTrn::updateTrn() >>>> pose time 1613170887.40, meas time 1613170887.16 <<<< 2021-02-12T23:01:28Z- TerrainNav::Measurement type 1 from time = 1613170887.16 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:01:28Z- TerrainNav::motionUpdate() - currEstimate = (4056072, 590341.41, 0.18) 2021-02-12T23:01:28Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:01:28Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:01:28Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:01:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:01:28Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:01:28Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:01:28Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:01:29Z- handleDvl() - RDI_Pathfinder msg: 1613170889.44 epoch sec; seqNo:35 2021-02-12T23:01:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.06 , 101.18 , 83.58 , 110.65 2021-02-12T23:01:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.04 , -0.11 , 0.18 2021-02-12T23:01:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170889.46 epoch sec; seqNo:64; 4056071.61 north; 590341.42 east 2021-02-12T23:01:29Z- AHRS_M2 msg: 1613170889.58 epoch sec; seqNo:65 2021-02-12T23:01:29Z- AHRS_M2 msg: 5.38 phi; -0.22 theta; -0.02 psi 2021-02-12T23:01:29Z- handleDepth()-Depth_Keller msg: 1613170889616 epoch sec; seqNo:65; depth 0.15 2021-02-12T23:01:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170889.63 epoch sec; seqNo:65; 4056071.61 north; 590341.42 east 2021-02-12T23:01:29Z- AHRS_M2 msg: 1613170889.97 epoch sec; seqNo:66 2021-02-12T23:01:29Z- AHRS_M2 msg: 5.39 phi; -0.19 theta; -0.04 psi 2021-02-12T23:01:29Z- handleDepth()-Depth_Keller msg: 1613170889974 epoch sec; seqNo:66; depth 0.15 2021-02-12T23:01:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170889.98 epoch sec; seqNo:66; 4056071.61 north; 590341.42 east 2021-02-12T23:01:30Z- AHRS_M2 msg: 1613170890.37 epoch sec; seqNo:67 2021-02-12T23:01:30Z- AHRS_M2 msg: 5.40 phi; -0.14 theta; -0.04 psi 2021-02-12T23:01:30Z- handleDepth()-Depth_Keller msg: 1613170890377 epoch sec; seqNo:67; depth 0.10 2021-02-12T23:01:30Z- handleDvl() - RDI_Pathfinder msg: 1613170890.40 epoch sec; seqNo:36 2021-02-12T23:01:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.03 , 96.03 , 85.21 , 110.92 2021-02-12T23:01:30Z- handleDvl() - RDI_Pathfinder msg: velocities -0.06 , -0.20 , 0.12 2021-02-12T23:01:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170890.41 epoch sec; seqNo:67; 4056071.61 north; 590341.42 east 2021-02-12T23:01:30Z- AHRS_M2 msg: 1613170890.78 epoch sec; seqNo:68 2021-02-12T23:01:30Z- AHRS_M2 msg: 5.40 phi; -0.18 theta; -0.03 psi 2021-02-12T23:01:30Z- handleDepth()-Depth_Keller msg: 1613170890780 epoch sec; seqNo:68; depth 0.09 2021-02-12T23:01:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170890.79 epoch sec; seqNo:68; 4056071.61 north; 590341.42 east 2021-02-12T23:01:31Z- AHRS_M2 msg: 1613170891.18 epoch sec; seqNo:69 2021-02-12T23:01:31Z- AHRS_M2 msg: 5.36 phi; -0.23 theta; -0.04 psi 2021-02-12T23:01:31Z- handleDepth()-Depth_Keller msg: 1613170891189 epoch sec; seqNo:69; depth 0.08 2021-02-12T23:01:31Z- handleDvl() - RDI_Pathfinder msg: 1613170891.20 epoch sec; seqNo:37 2021-02-12T23:01:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.26 , 95.96 , 86.49 , 106.79 2021-02-12T23:01:31Z- handleDvl() - RDI_Pathfinder msg: velocities -0.31 , 0.19 , 0.43 2021-02-12T23:01:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170891.21 epoch sec; seqNo:69; 4056071.61 north; 590341.42 east 2021-02-12T23:01:31Z- AHRS_M2 msg: 1613170891.59 epoch sec; seqNo:70 2021-02-12T23:01:31Z- AHRS_M2 msg: 5.34 phi; -0.19 theta; -0.12 psi 2021-02-12T23:01:31Z- handleDepth()-Depth_Keller msg: 1613170891593 epoch sec; seqNo:70; depth 0.05 2021-02-12T23:01:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170891.62 epoch sec; seqNo:70; 4056071.61 north; 590341.42 east 2021-02-12T23:01:32Z- AHRS_M2 msg: 1613170892.00 epoch sec; seqNo:71 2021-02-12T23:01:32Z- AHRS_M2 msg: 5.34 phi; -0.12 theta; -0.17 psi 2021-02-12T23:01:32Z- handleDepth()-Depth_Keller msg: 1613170892017 epoch sec; seqNo:71; depth 0.02 2021-02-12T23:01:32Z- handleDvl() - RDI_Pathfinder msg: 1613170892.03 epoch sec; seqNo:38 2021-02-12T23:01:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.69 , 106.93 , 86.63 , 110.99 2021-02-12T23:01:32Z- handleDvl() - RDI_Pathfinder msg: velocities -0.21 , -0.06 , -0.06 2021-02-12T23:01:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170892.04 epoch sec; seqNo:71; 4056071.61 north; 590341.42 east 2021-02-12T23:01:32Z- AHRS_M2 msg: 1613170892.40 epoch sec; seqNo:72 2021-02-12T23:01:32Z- AHRS_M2 msg: 5.37 phi; -0.08 theta; -0.15 psi 2021-02-12T23:01:32Z- handleDepth()-Depth_Keller msg: 1613170892405 epoch sec; seqNo:72; depth -0.00 2021-02-12T23:01:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170892.43 epoch sec; seqNo:72; 4056071.23 north; 590341.42 east 2021-02-12T23:01:32Z- AHRS_M2 msg: 1613170892.80 epoch sec; seqNo:73 2021-02-12T23:01:32Z- AHRS_M2 msg: 5.39 phi; -0.12 theta; -0.02 psi 2021-02-12T23:01:32Z- handleDepth()-Depth_Keller msg: 1613170892806 epoch sec; seqNo:73; depth 0.01 2021-02-12T23:01:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170892.82 epoch sec; seqNo:73; 4056071.23 north; 590341.42 east 2021-02-12T23:01:33Z- AHRS_M2 msg: 1613170893.20 epoch sec; seqNo:74 2021-02-12T23:01:33Z- AHRS_M2 msg: 5.39 phi; -0.10 theta; 0.04 psi 2021-02-12T23:01:33Z- handleDepth()-Depth_Keller msg: 1613170893211 epoch sec; seqNo:74; depth 0.03 2021-02-12T23:01:33Z- handleDvl() - RDI_Pathfinder msg: 1613170893.22 epoch sec; seqNo:39 2021-02-12T23:01:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 105.30 , 97.18 , 95.83 2021-02-12T23:01:33Z- handleDvl() - RDI_Pathfinder msg: velocities -0.55 , -0.04 , -0.25 2021-02-12T23:01:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170893.29 epoch sec; seqNo:74; 4056071.23 north; 590341.42 east 2021-02-12T23:01:33Z- AHRS_M2 msg: 1613170893.60 epoch sec; seqNo:75 2021-02-12T23:01:33Z- AHRS_M2 msg: 5.36 phi; -0.06 theta; 0.05 psi 2021-02-12T23:01:33Z- handleDepth()-Depth_Keller msg: 1613170893613 epoch sec; seqNo:75; depth 0.04 2021-02-12T23:01:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170893.64 epoch sec; seqNo:75; 4056070.85 north; 590341.43 east 2021-02-12T23:01:33Z- LcmTrn::handleMessages() - handling 39 messages took 0.68 ms and 126 calls... 2021-02-12T23:01:33Z- waiting for 1613170890.40, time is 1613170893.64 2021-02-12T23:01:33Z- LcmTrn::updateTrn() >>>> pose time 1613170893.64, meas time 1613170893.22 <<<< 2021-02-12T23:01:33Z- TerrainNav::Measurement type 1 from time = 1613170893.22 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:01:33Z- TerrainNav::motionUpdate() - currEstimate = (4056071, 590341.43, 0.04) 2021-02-12T23:01:33Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:01:33Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:01:33Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:01:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:01:33Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:01:33Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:01:33Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:01:34Z- AHRS_M2 msg: 1613170894.01 epoch sec; seqNo:76 2021-02-12T23:01:34Z- AHRS_M2 msg: 5.31 phi; -0.07 theta; 0.00 psi 2021-02-12T23:01:34Z- handleDepth()-Depth_Keller msg: 1613170894022 epoch sec; seqNo:76; depth 0.02 2021-02-12T23:01:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170894.06 epoch sec; seqNo:76; 4056070.85 north; 590341.43 east 2021-02-12T23:01:34Z- AHRS_M2 msg: 1613170894.42 epoch sec; seqNo:77 2021-02-12T23:01:34Z- AHRS_M2 msg: 5.33 phi; -0.10 theta; -0.09 psi 2021-02-12T23:01:34Z- handleDepth()-Depth_Keller msg: 1613170894421 epoch sec; seqNo:77; depth 0.04 2021-02-12T23:01:34Z- handleDvl() - RDI_Pathfinder msg: 1613170894.44 epoch sec; seqNo:40 2021-02-12T23:01:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.96 , 93.26 , 89.20 , 95.96 2021-02-12T23:01:34Z- handleDvl() - RDI_Pathfinder msg: velocities -0.10 , -0.29 , -0.48 2021-02-12T23:01:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170894.48 epoch sec; seqNo:77; 4056070.85 north; 590341.43 east 2021-02-12T23:01:34Z- AHRS_M2 msg: 1613170894.82 epoch sec; seqNo:78 2021-02-12T23:01:34Z- AHRS_M2 msg: 5.37 phi; -0.08 theta; -0.12 psi 2021-02-12T23:01:34Z- handleDepth()-Depth_Keller msg: 1613170894825 epoch sec; seqNo:78; depth 0.01 2021-02-12T23:01:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170894.84 epoch sec; seqNo:78; 4056070.47 north; 590341.43 east 2021-02-12T23:01:35Z- AHRS_M2 msg: 1613170895.22 epoch sec; seqNo:79 2021-02-12T23:01:35Z- AHRS_M2 msg: 5.37 phi; -0.04 theta; -0.04 psi 2021-02-12T23:01:35Z- handleDepth()-Depth_Keller msg: 1613170895225 epoch sec; seqNo:79; depth 0.03 2021-02-12T23:01:35Z- handleDvl() - RDI_Pathfinder msg: 1613170895.24 epoch sec; seqNo:41 2021-02-12T23:01:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.82 , 97.59 , 97.59 , 96.24 2021-02-12T23:01:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.61 , -0.38 , -0.70 2021-02-12T23:01:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170895.27 epoch sec; seqNo:79; 4056070.85 north; 590341.43 east 2021-02-12T23:01:35Z- AHRS_M2 msg: 1613170895.62 epoch sec; seqNo:80 2021-02-12T23:01:35Z- AHRS_M2 msg: 5.35 phi; 0.02 theta; -0.02 psi 2021-02-12T23:01:35Z- handleDepth()-Depth_Keller msg: 1613170895633 epoch sec; seqNo:80; depth 0.08 2021-02-12T23:01:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170895.65 epoch sec; seqNo:80; 4056070.85 north; 590341.43 east 2021-02-12T23:01:36Z- AHRS_M2 msg: 1613170896.03 epoch sec; seqNo:81 2021-02-12T23:01:36Z- AHRS_M2 msg: 5.36 phi; 0.05 theta; -0.04 psi 2021-02-12T23:01:36Z- handleDepth()-Depth_Keller msg: 1613170896041 epoch sec; seqNo:81; depth 0.05 2021-02-12T23:01:36Z- handleDvl() - RDI_Pathfinder msg: 1613170896.07 epoch sec; seqNo:42 2021-02-12T23:01:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.04 , 96.10 , 92.04 , 94.75 2021-02-12T23:01:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.35 , 0.30 2021-02-12T23:01:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170896.10 epoch sec; seqNo:81; 4056070.85 north; 590341.43 east 2021-02-12T23:01:36Z- AHRS_M2 msg: 1613170896.43 epoch sec; seqNo:82 2021-02-12T23:01:36Z- AHRS_M2 msg: 5.36 phi; -0.02 theta; -0.01 psi 2021-02-12T23:01:36Z- handleDepth()-Depth_Keller msg: 1613170896437 epoch sec; seqNo:82; depth 0.05 2021-02-12T23:01:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170896.46 epoch sec; seqNo:82; 4056070.84 north; 590340.21 east 2021-02-12T23:01:36Z- AHRS_M2 msg: 1613170896.84 epoch sec; seqNo:83 2021-02-12T23:01:36Z- AHRS_M2 msg: 5.37 phi; -0.14 theta; 0.00 psi 2021-02-12T23:01:36Z- handleDepth()-Depth_Keller msg: 1613170896845 epoch sec; seqNo:83; depth 0.05 2021-02-12T23:01:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170896.87 epoch sec; seqNo:83; 4056070.84 north; 590340.21 east 2021-02-12T23:01:37Z- AHRS_M2 msg: 1613170897.24 epoch sec; seqNo:84 2021-02-12T23:01:37Z- AHRS_M2 msg: 5.40 phi; -0.17 theta; -0.01 psi 2021-02-12T23:01:37Z- handleDepth()-Depth_Keller msg: 1613170897261 epoch sec; seqNo:84; depth 0.04 2021-02-12T23:01:37Z- handleDvl() - RDI_Pathfinder msg: 1613170897.28 epoch sec; seqNo:43 2021-02-12T23:01:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.59 , 97.59 , 93.53 , 93.53 2021-02-12T23:01:37Z- handleDvl() - RDI_Pathfinder msg: velocities -0.30 , -0.28 , 0.82 2021-02-12T23:01:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170897.29 epoch sec; seqNo:84; 4056070.84 north; 590340.21 east 2021-02-12T23:01:37Z- AHRS_M2 msg: 1613170897.64 epoch sec; seqNo:85 2021-02-12T23:01:37Z- AHRS_M2 msg: 5.36 phi; -0.12 theta; 0.04 psi 2021-02-12T23:01:37Z- handleDepth()-Depth_Keller msg: 1613170897650 epoch sec; seqNo:85; depth 0.05 2021-02-12T23:01:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170897.66 epoch sec; seqNo:85; 4056070.46 north; 590340.21 east 2021-02-12T23:01:38Z- AHRS_M2 msg: 1613170898.05 epoch sec; seqNo:86 2021-02-12T23:01:38Z- AHRS_M2 msg: 5.32 phi; -0.11 theta; 0.02 psi 2021-02-12T23:01:38Z- handleDepth()-Depth_Keller msg: 1613170898053 epoch sec; seqNo:86; depth 0.05 2021-02-12T23:01:38Z- handleDvl() - RDI_Pathfinder msg: 1613170898.07 epoch sec; seqNo:44 2021-02-12T23:01:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.10 , 96.10 , 87.98 , 106.93 2021-02-12T23:01:38Z- handleDvl() - RDI_Pathfinder msg: velocities -0.39 , -0.01 , 0.27 2021-02-12T23:01:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170898.08 epoch sec; seqNo:86; 4056070.46 north; 590340.21 east 2021-02-12T23:01:38Z- AHRS_M2 msg: 1613170898.45 epoch sec; seqNo:87 2021-02-12T23:01:38Z- AHRS_M2 msg: 5.34 phi; -0.12 theta; -0.12 psi 2021-02-12T23:01:38Z- handleDepth()-Depth_Keller msg: 1613170898461 epoch sec; seqNo:87; depth 0.02 2021-02-12T23:01:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170898.47 epoch sec; seqNo:87; 4056070.46 north; 590340.21 east 2021-02-12T23:01:38Z- AHRS_M2 msg: 1613170898.86 epoch sec; seqNo:88 2021-02-12T23:01:38Z- AHRS_M2 msg: 5.35 phi; -0.12 theta; -0.17 psi 2021-02-12T23:01:38Z- LcmTrn::handleMessages() - handling 42 messages took 0.75 ms and 127 calls... 2021-02-12T23:01:38Z- waiting for 1613170896.64, time is 1613170898.86 2021-02-12T23:01:38Z- LcmTrn::updateTrn() >>>> pose time 1613170898.86, meas time 1613170898.07 <<<< 2021-02-12T23:01:38Z- TerrainNav::Measurement type 1 from time = 1613170898.07 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:01:38Z- TerrainNav::motionUpdate() - currEstimate = (4056070, 590340.21, 0.02) 2021-02-12T23:01:38Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:01:38Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:01:38Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:01:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:01:38Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:01:38Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:01:38Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:01:38Z- handleDepth()-Depth_Keller msg: 1613170898860 epoch sec; seqNo:88; depth 0.04 2021-02-12T23:01:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170898.87 epoch sec; seqNo:88; 4056070.46 north; 590340.21 east 2021-02-12T23:01:39Z- AHRS_M2 msg: 1613170899.26 epoch sec; seqNo:89 2021-02-12T23:01:39Z- AHRS_M2 msg: 5.37 phi; -0.06 theta; -0.05 psi 2021-02-12T23:01:39Z- handleDepth()-Depth_Keller msg: 1613170899273 epoch sec; seqNo:89; depth 0.09 2021-02-12T23:01:39Z- handleDvl() - RDI_Pathfinder msg: 1613170899.29 epoch sec; seqNo:45 2021-02-12T23:01:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.32 , 100.03 , 97.32 , 104.08 2021-02-12T23:01:39Z- handleDvl() - RDI_Pathfinder msg: velocities -0.48 , -0.11 , -0.46 2021-02-12T23:01:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170899.30 epoch sec; seqNo:89; 4056070.08 north; 590340.21 east 2021-02-12T23:01:39Z- AHRS_M2 msg: 1613170899.66 epoch sec; seqNo:90 2021-02-12T23:01:39Z- AHRS_M2 msg: 5.37 phi; 0.04 theta; 0.05 psi 2021-02-12T23:01:39Z- handleDepth()-Depth_Keller msg: 1613170899668 epoch sec; seqNo:90; depth 0.08 2021-02-12T23:01:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170899.69 epoch sec; seqNo:90; 4056070.10 north; 590341.43 east 2021-02-12T23:01:40Z- AHRS_M2 msg: 1613170900.07 epoch sec; seqNo:91 2021-02-12T23:01:40Z- AHRS_M2 msg: 5.40 phi; 0.04 theta; 0.09 psi 2021-02-12T23:01:40Z- handleDepth()-Depth_Keller msg: 1613170900072 epoch sec; seqNo:91; depth 0.07 2021-02-12T23:01:40Z- handleDvl() - RDI_Pathfinder msg: 1613170900.08 epoch sec; seqNo:46 2021-02-12T23:01:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.05 , 94.34 , 97.05 , 97.05 2021-02-12T23:01:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.13 , -0.10 , -0.08 2021-02-12T23:01:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170900.11 epoch sec; seqNo:91; 4056070.10 north; 590341.43 east 2021-02-12T23:01:40Z- AHRS_M2 msg: 1613170900.47 epoch sec; seqNo:92 2021-02-12T23:01:40Z- AHRS_M2 msg: 5.44 phi; -0.08 theta; 0.11 psi 2021-02-12T23:01:40Z- handleDepth()-Depth_Keller msg: 1613170900479 epoch sec; seqNo:92; depth 0.02 2021-02-12T23:01:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170900.49 epoch sec; seqNo:92; 4056070.08 north; 590340.21 east 2021-02-12T23:01:40Z- AHRS_M2 msg: 1613170900.88 epoch sec; seqNo:93 2021-02-12T23:01:40Z- AHRS_M2 msg: 5.38 phi; -0.11 theta; 0.14 psi 2021-02-12T23:01:40Z- handleDepth()-Depth_Keller msg: 1613170900882 epoch sec; seqNo:93; depth 0.05 2021-02-12T23:01:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170900.89 epoch sec; seqNo:93; 4056070.08 north; 590340.21 east 2021-02-12T23:01:41Z- AHRS_M2 msg: 1613170901.28 epoch sec; seqNo:94 2021-02-12T23:01:41Z- AHRS_M2 msg: 5.30 phi; -0.04 theta; 0.08 psi 2021-02-12T23:01:41Z- handleDepth()-Depth_Keller msg: 1613170901285 epoch sec; seqNo:94; depth 0.09 2021-02-12T23:01:41Z- handleDvl() - RDI_Pathfinder msg: 1613170901.31 epoch sec; seqNo:47 2021-02-12T23:01:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 102.80 , 97.39 , 89.27 , 98.74 2021-02-12T23:01:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.19 , 0.23 , -0.59 2021-02-12T23:01:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170901.33 epoch sec; seqNo:94; 4056070.08 north; 590340.21 east 2021-02-12T23:01:41Z- AHRS_M2 msg: 1613170901.69 epoch sec; seqNo:95 2021-02-12T23:01:41Z- AHRS_M2 msg: 5.27 phi; 0.01 theta; -0.12 psi 2021-02-12T23:01:41Z- handleDepth()-Depth_Keller msg: 1613170901693 epoch sec; seqNo:95; depth 0.07 2021-02-12T23:01:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170901.72 epoch sec; seqNo:95; 4056070.46 north; 590340.21 east 2021-02-12T23:01:42Z- AHRS_M2 msg: 1613170902.09 epoch sec; seqNo:96 2021-02-12T23:01:42Z- AHRS_M2 msg: 5.26 phi; -0.02 theta; -0.25 psi 2021-02-12T23:01:42Z- handleDepth()-Depth_Keller msg: 1613170902093 epoch sec; seqNo:96; depth 0.05 2021-02-12T23:01:42Z- handleDvl() - RDI_Pathfinder msg: 1613170902.40 epoch sec; seqNo:48 2021-02-12T23:01:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.32 , 97.32 , 97.32 , 97.32 2021-02-12T23:01:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.23 , 0.46 2021-02-12T23:01:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170902.41 epoch sec; seqNo:96; 4056070.46 north; 590340.21 east 2021-02-12T23:01:42Z- AHRS_M2 msg: 1613170902.50 epoch sec; seqNo:97 2021-02-12T23:01:42Z- AHRS_M2 msg: 5.25 phi; -0.07 theta; -0.20 psi 2021-02-12T23:01:42Z- handleDepth()-Depth_Keller msg: 1613170902507 epoch sec; seqNo:97; depth 0.09 2021-02-12T23:01:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170902.52 epoch sec; seqNo:97; 4056070.46 north; 590340.21 east 2021-02-12T23:01:42Z- AHRS_M2 msg: 1613170902.90 epoch sec; seqNo:98 2021-02-12T23:01:42Z- AHRS_M2 msg: 5.23 phi; -0.13 theta; -0.09 psi 2021-02-12T23:01:42Z- handleDepth()-Depth_Keller msg: 1613170902906 epoch sec; seqNo:98; depth 0.09 2021-02-12T23:01:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170902.91 epoch sec; seqNo:98; 4056070.46 north; 590340.21 east 2021-02-12T23:01:43Z- AHRS_M2 msg: 1613170903.30 epoch sec; seqNo:99 2021-02-12T23:01:43Z- AHRS_M2 msg: 5.20 phi; -0.21 theta; 0.02 psi 2021-02-12T23:01:43Z- handleDepth()-Depth_Keller msg: 1613170903313 epoch sec; seqNo:99; depth 0.11 2021-02-12T23:01:43Z- handleDvl() - RDI_Pathfinder msg: 1613170903.35 epoch sec; seqNo:49 2021-02-12T23:01:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.42 , 110.78 , 97.25 , 101.31 2021-02-12T23:01:43Z- handleDvl() - RDI_Pathfinder msg: velocities -0.37 , -0.42 , 0.56 2021-02-12T23:01:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170903.39 epoch sec; seqNo:99; 4056070.08 north; 590340.21 east 2021-02-12T23:01:43Z- AHRS_M2 msg: 1613170903.70 epoch sec; seqNo:100 2021-02-12T23:01:43Z- AHRS_M2 msg: 5.19 phi; -0.26 theta; 0.07 psi 2021-02-12T23:01:43Z- handleDepth()-Depth_Keller msg: 1613170903713 epoch sec; seqNo:100; depth 0.15 2021-02-12T23:01:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170903.75 epoch sec; seqNo:100; 4056070.08 north; 590340.21 east 2021-02-12T23:01:43Z- LcmTrn::handleMessages() - handling 43 messages took 0.79 ms and 128 calls... 2021-02-12T23:01:43Z- waiting for 1613170901.86, time is 1613170903.75 2021-02-12T23:01:43Z- LcmTrn::updateTrn() >>>> pose time 1613170903.75, meas time 1613170903.35 <<<< 2021-02-12T23:01:43Z- TerrainNav::Measurement type 1 from time = 1613170903.35 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:01:43Z- TerrainNav::motionUpdate() - currEstimate = (4056070, 590340.21, 0.15) 2021-02-12T23:01:43Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:01:43Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:01:43Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:01:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:01:43Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:01:43Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:01:43Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:01:44Z- AHRS_M2 msg: 1613170904.11 epoch sec; seqNo:101 2021-02-12T23:01:44Z- AHRS_M2 msg: 5.25 phi; -0.29 theta; 0.04 psi 2021-02-12T23:01:44Z- handleDepth()-Depth_Keller msg: 1613170904117 epoch sec; seqNo:101; depth 0.10 2021-02-12T23:01:44Z- handleDvl() - RDI_Pathfinder msg: 1613170904.15 epoch sec; seqNo:50 2021-02-12T23:01:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.18 , 94.41 , 86.29 , 113.36 2021-02-12T23:01:44Z- handleDvl() - RDI_Pathfinder msg: velocities -0.76 , -0.42 , 0.04 2021-02-12T23:01:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170904.19 epoch sec; seqNo:101; 4056069.70 north; 590340.22 east 2021-02-12T23:01:44Z- AHRS_M2 msg: 1613170904.51 epoch sec; seqNo:102 2021-02-12T23:01:44Z- AHRS_M2 msg: 5.28 phi; -0.36 theta; 0.03 psi 2021-02-12T23:01:44Z- handleDepth()-Depth_Keller msg: 1613170904517 epoch sec; seqNo:102; depth 0.13 2021-02-12T23:01:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170904.55 epoch sec; seqNo:102; 4056069.32 north; 590340.22 east 2021-02-12T23:01:44Z- AHRS_M2 msg: 1613170904.91 epoch sec; seqNo:103 2021-02-12T23:01:44Z- AHRS_M2 msg: 5.23 phi; -0.37 theta; 0.05 psi 2021-02-12T23:01:44Z- handleDepth()-Depth_Keller msg: 1613170904921 epoch sec; seqNo:103; depth 0.15 2021-02-12T23:01:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170904.94 epoch sec; seqNo:103; 4056069.32 north; 590340.22 east 2021-02-12T23:01:45Z- AHRS_M2 msg: 1613170905.32 epoch sec; seqNo:104 2021-02-12T23:01:45Z- AHRS_M2 msg: 5.13 phi; -0.34 theta; -0.01 psi 2021-02-12T23:01:45Z- handleDepth()-Depth_Keller msg: 1613170905325 epoch sec; seqNo:104; depth 0.16 2021-02-12T23:01:45Z- handleDvl() - RDI_Pathfinder msg: 1613170905.34 epoch sec; seqNo:51 2021-02-12T23:01:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 105.30 , 95.83 , 83.65 , 129.66 2021-02-12T23:01:45Z- handleDvl() - RDI_Pathfinder msg: velocities -0.36 , 0.43 , -0.63 2021-02-12T23:01:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170905.36 epoch sec; seqNo:104; 4056069.32 north; 590340.22 east 2021-02-12T23:01:45Z- AHRS_M2 msg: 1613170905.72 epoch sec; seqNo:105 2021-02-12T23:01:45Z- AHRS_M2 msg: 5.12 phi; -0.31 theta; -0.24 psi 2021-02-12T23:01:45Z- handleDepth()-Depth_Keller msg: 1613170905729 epoch sec; seqNo:105; depth 0.10 2021-02-12T23:01:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170905.78 epoch sec; seqNo:105; 4056069.34 north; 590341.44 east 2021-02-12T23:01:46Z- AHRS_M2 msg: 1613170906.13 epoch sec; seqNo:106 2021-02-12T23:01:46Z- AHRS_M2 msg: 5.13 phi; -0.25 theta; -0.37 psi 2021-02-12T23:01:46Z- handleDepth()-Depth_Keller msg: 1613170906137 epoch sec; seqNo:106; depth 0.10 2021-02-12T23:01:46Z- handleDvl() - RDI_Pathfinder msg: 1613170906.17 epoch sec; seqNo:52 2021-02-12T23:01:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.79 , 106.72 , 0.00 , 120.12 2021-02-12T23:01:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.32 , -0.06 , -0.46 2021-02-12T23:01:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170906.19 epoch sec; seqNo:106; 4056069.32 north; 590340.22 east 2021-02-12T23:01:46Z- AHRS_M2 msg: 1613170906.53 epoch sec; seqNo:107 2021-02-12T23:01:46Z- AHRS_M2 msg: 5.10 phi; -0.23 theta; -0.25 psi 2021-02-12T23:01:46Z- handleDepth()-Depth_Keller msg: 1613170906541 epoch sec; seqNo:107; depth 0.12 2021-02-12T23:01:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170906.56 epoch sec; seqNo:107; 4056069.32 north; 590340.22 east 2021-02-12T23:01:46Z- AHRS_M2 msg: 1613170906.93 epoch sec; seqNo:108 2021-02-12T23:01:46Z- AHRS_M2 msg: 5.08 phi; -0.27 theta; -0.14 psi 2021-02-12T23:01:46Z- handleDepth()-Depth_Keller msg: 1613170906949 epoch sec; seqNo:108; depth 0.14 2021-02-12T23:01:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170906.97 epoch sec; seqNo:108; 4056069.32 north; 590340.22 east 2021-02-12T23:01:47Z- AHRS_M2 msg: 1613170907.34 epoch sec; seqNo:109 2021-02-12T23:01:47Z- AHRS_M2 msg: 5.11 phi; -0.31 theta; -0.11 psi 2021-02-12T23:01:47Z- handleDepth()-Depth_Keller msg: 1613170907345 epoch sec; seqNo:109; depth 0.17 2021-02-12T23:01:47Z- handleDvl() - RDI_Pathfinder msg: 1613170907.36 epoch sec; seqNo:53 2021-02-12T23:01:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.45 , 116.81 , 0.00 , 115.32 2021-02-12T23:01:47Z- handleDvl() - RDI_Pathfinder msg: velocities -0.06 , -0.30 , 0.12 2021-02-12T23:01:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170907.40 epoch sec; seqNo:109; 4056069.32 north; 590340.22 east 2021-02-12T23:01:47Z- AHRS_M2 msg: 1613170907.74 epoch sec; seqNo:110 2021-02-12T23:01:47Z- AHRS_M2 msg: 5.12 phi; -0.30 theta; -0.06 psi 2021-02-12T23:01:47Z- handleDepth()-Depth_Keller msg: 1613170907748 epoch sec; seqNo:110; depth 0.16 2021-02-12T23:01:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170907.76 epoch sec; seqNo:110; 4056068.95 north; 590340.23 east 2021-02-12T23:01:48Z- AHRS_M2 msg: 1613170908.15 epoch sec; seqNo:111 2021-02-12T23:01:48Z- AHRS_M2 msg: 5.11 phi; -0.28 theta; -0.01 psi 2021-02-12T23:01:48Z- handleDepth()-Depth_Keller msg: 1613170908157 epoch sec; seqNo:111; depth 0.17 2021-02-12T23:01:48Z- handleDvl() - RDI_Pathfinder msg: 1613170908.17 epoch sec; seqNo:54 2021-02-12T23:01:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.76 , 104.69 , 83.85 , 119.58 2021-02-12T23:01:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.15 , -0.44 , -0.58 2021-02-12T23:01:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170908.18 epoch sec; seqNo:111; 4056068.95 north; 590340.23 east 2021-02-12T23:01:48Z- AHRS_M2 msg: 1613170908.55 epoch sec; seqNo:112 2021-02-12T23:01:48Z- AHRS_M2 msg: 5.14 phi; -0.26 theta; -0.02 psi 2021-02-12T23:01:48Z- handleDepth()-Depth_Keller msg: 1613170908565 epoch sec; seqNo:112; depth 0.14 2021-02-12T23:01:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170908.58 epoch sec; seqNo:112; 4056068.95 north; 590340.23 east 2021-02-12T23:01:48Z- LcmTrn::handleMessages() - handling 41 messages took 0.76 ms and 126 calls... 2021-02-12T23:01:48Z- waiting for 1613170906.75, time is 1613170908.58 2021-02-12T23:01:48Z- LcmTrn::updateTrn() >>>> pose time 1613170908.58, meas time 1613170908.17 <<<< 2021-02-12T23:01:48Z- TerrainNav::Measurement type 1 from time = 1613170908.17 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:01:48Z- TerrainNav::motionUpdate() - currEstimate = (4056069, 590340.23, 0.14) 2021-02-12T23:01:48Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:01:48Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:01:48Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:01:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:01:48Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:01:48Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:01:48Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:01:48Z- AHRS_M2 msg: 1613170908.95 epoch sec; seqNo:113 2021-02-12T23:01:48Z- AHRS_M2 msg: 5.17 phi; -0.21 theta; -0.01 psi 2021-02-12T23:01:48Z- handleDepth()-Depth_Keller msg: 1613170908965 epoch sec; seqNo:113; depth 0.15 2021-02-12T23:01:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170908.98 epoch sec; seqNo:113; 4056068.95 north; 590340.23 east 2021-02-12T23:01:49Z- AHRS_M2 msg: 1613170909.36 epoch sec; seqNo:114 2021-02-12T23:01:49Z- AHRS_M2 msg: 5.14 phi; -0.17 theta; -0.03 psi 2021-02-12T23:01:49Z- handleDepth()-Depth_Keller msg: 1613170909364 epoch sec; seqNo:114; depth 0.12 2021-02-12T23:01:49Z- handleDvl() - RDI_Pathfinder msg: 1613170909.38 epoch sec; seqNo:55 2021-02-12T23:01:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.63 , 95.63 , 84.80 , 113.22 2021-02-12T23:01:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , 0.07 , 0.00 2021-02-12T23:01:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170909.40 epoch sec; seqNo:114; 4056068.93 north; 590339.00 east 2021-02-12T23:01:49Z- AHRS_M2 msg: 1613170909.76 epoch sec; seqNo:115 2021-02-12T23:01:49Z- AHRS_M2 msg: 5.08 phi; -0.21 theta; -0.13 psi 2021-02-12T23:01:49Z- handleDepth()-Depth_Keller msg: 1613170909773 epoch sec; seqNo:115; depth 0.11 2021-02-12T23:01:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170909.78 epoch sec; seqNo:115; 4056068.93 north; 590339.00 east 2021-02-12T23:01:50Z- AHRS_M2 msg: 1613170910.18 epoch sec; seqNo:116 2021-02-12T23:01:50Z- AHRS_M2 msg: 5.15 phi; -0.15 theta; -0.30 psi 2021-02-12T23:01:50Z- handleDepth()-Depth_Keller msg: 1613170910190 epoch sec; seqNo:116; depth 0.12 2021-02-12T23:01:50Z- handleDvl() - RDI_Pathfinder msg: 1613170910.20 epoch sec; seqNo:56 2021-02-12T23:01:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.13 , 102.66 , 0.00 , 108.08 2021-02-12T23:01:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.19 , -0.29 , 0.17 2021-02-12T23:01:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170910.21 epoch sec; seqNo:116; 4056068.93 north; 590339.00 east 2021-02-12T23:01:50Z- AHRS_M2 msg: 1613170910.57 epoch sec; seqNo:117 2021-02-12T23:01:50Z- AHRS_M2 msg: 5.19 phi; -0.07 theta; -0.15 psi 2021-02-12T23:01:50Z- handleDepth()-Depth_Keller msg: 1613170910577 epoch sec; seqNo:117; depth 0.08 2021-02-12T23:01:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170910.60 epoch sec; seqNo:117; 4056068.93 north; 590339.00 east 2021-02-12T23:01:51Z- AHRS_M2 msg: 1613170910.99 epoch sec; seqNo:118 2021-02-12T23:01:51Z- AHRS_M2 msg: 5.20 phi; -0.06 theta; 0.00 psi 2021-02-12T23:01:51Z- handleDepth()-Depth_Keller msg: 1613170911014 epoch sec; seqNo:118; depth 0.07 2021-02-12T23:01:51Z- handleDvl() - RDI_Pathfinder msg: 1613170911.03 epoch sec; seqNo:57 2021-02-12T23:01:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 111.93 , 97.05 , 106.52 2021-02-12T23:01:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.55 , -0.05 , 0.61 2021-02-12T23:01:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170911.04 epoch sec; seqNo:118; 4056068.93 north; 590339.00 east 2021-02-12T23:01:51Z- AHRS_M2 msg: 1613170911.38 epoch sec; seqNo:119 2021-02-12T23:01:51Z- AHRS_M2 msg: 5.19 phi; -0.01 theta; 0.02 psi 2021-02-12T23:01:51Z- handleDepth()-Depth_Keller msg: 1613170911389 epoch sec; seqNo:119; depth 0.08 2021-02-12T23:01:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170911.40 epoch sec; seqNo:119; 4056068.93 north; 590339.00 east 2021-02-12T23:01:51Z- AHRS_M2 msg: 1613170911.79 epoch sec; seqNo:120 2021-02-12T23:01:51Z- AHRS_M2 msg: 5.19 phi; 0.13 theta; -0.05 psi 2021-02-12T23:01:51Z- handleDepth()-Depth_Keller msg: 1613170911790 epoch sec; seqNo:120; depth 0.06 2021-02-12T23:01:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170911.80 epoch sec; seqNo:120; 4056069.31 north; 590339.00 east 2021-02-12T23:01:52Z- AHRS_M2 msg: 1613170912.19 epoch sec; seqNo:121 2021-02-12T23:01:52Z- AHRS_M2 msg: 5.21 phi; 0.15 theta; -0.13 psi 2021-02-12T23:01:52Z- handleDepth()-Depth_Keller msg: 1613170912197 epoch sec; seqNo:121; depth 0.01 2021-02-12T23:01:52Z- handleDvl() - RDI_Pathfinder msg: 1613170912.22 epoch sec; seqNo:58 2021-02-12T23:01:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.17 , 93.19 , 97.66 , 94.68 2021-02-12T23:01:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.26 , -0.44 , 1.15 2021-02-12T23:01:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170912.23 epoch sec; seqNo:121; 4056069.30 north; 590337.78 east 2021-02-12T23:01:52Z- AHRS_M2 msg: 1613170912.59 epoch sec; seqNo:122 2021-02-12T23:01:52Z- AHRS_M2 msg: 5.27 phi; -0.03 theta; -0.14 psi 2021-02-12T23:01:52Z- handleDepth()-Depth_Keller msg: 1613170912597 epoch sec; seqNo:122; depth -0.03 2021-02-12T23:01:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170912.61 epoch sec; seqNo:122; 4056068.92 north; 590337.78 east 2021-02-12T23:01:53Z- AHRS_M2 msg: 1613170913.00 epoch sec; seqNo:123 2021-02-12T23:01:53Z- AHRS_M2 msg: 5.33 phi; -0.13 theta; -0.05 psi 2021-02-12T23:01:53Z- handleDepth()-Depth_Keller msg: 1613170913001 epoch sec; seqNo:123; depth 0.02 2021-02-12T23:01:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170913.02 epoch sec; seqNo:123; 4056068.92 north; 590337.78 east 2021-02-12T23:01:53Z- AHRS_M2 msg: 1613170913.40 epoch sec; seqNo:124 2021-02-12T23:01:53Z- AHRS_M2 msg: 5.32 phi; -0.17 theta; 0.00 psi 2021-02-12T23:01:53Z- handleDepth()-Depth_Keller msg: 1613170913413 epoch sec; seqNo:124; depth -0.01 2021-02-12T23:01:53Z- handleDvl() - RDI_Pathfinder msg: 1613170913.44 epoch sec; seqNo:59 2021-02-12T23:01:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.45 , 105.57 , 101.51 , 97.45 2021-02-12T23:01:53Z- handleDvl() - RDI_Pathfinder msg: velocities -1.58 , 0.06 , 0.38 2021-02-12T23:01:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170913.45 epoch sec; seqNo:124; 4056068.55 north; 590339.01 east 2021-02-12T23:01:53Z- AHRS_M2 msg: 1613170913.80 epoch sec; seqNo:125 2021-02-12T23:01:53Z- AHRS_M2 msg: 5.30 phi; -0.15 theta; 0.01 psi 2021-02-12T23:01:53Z- handleDepth()-Depth_Keller msg: 1613170913813 epoch sec; seqNo:125; depth 0.03 2021-02-12T23:01:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170913.84 epoch sec; seqNo:125; 4056068.18 north; 590339.01 east 2021-02-12T23:01:53Z- LcmTrn::handleMessages() - handling 44 messages took 0.80 ms and 128 calls... 2021-02-12T23:01:53Z- waiting for 1613170911.58, time is 1613170913.84 2021-02-12T23:01:53Z- LcmTrn::updateTrn() >>>> pose time 1613170913.84, meas time 1613170913.44 <<<< 2021-02-12T23:01:53Z- TerrainNav::Measurement type 1 from time = 1613170913.44 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:01:53Z- TerrainNav::motionUpdate() - currEstimate = (4056068, 590339.01, 0.03) 2021-02-12T23:01:53Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:01:53Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:01:53Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:01:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:01:53Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:01:53Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:01:53Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:01:54Z- AHRS_M2 msg: 1613170914.21 epoch sec; seqNo:126 2021-02-12T23:01:54Z- AHRS_M2 msg: 5.29 phi; -0.10 theta; 0.01 psi 2021-02-12T23:01:54Z- handleDepth()-Depth_Keller msg: 1613170914213 epoch sec; seqNo:126; depth 0.08 2021-02-12T23:01:54Z- handleDvl() - RDI_Pathfinder msg: 1613170914.23 epoch sec; seqNo:60 2021-02-12T23:01:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.63 , 95.63 , 96.98 , 109.16 2021-02-12T23:01:54Z- handleDvl() - RDI_Pathfinder msg: velocities -0.43 , 0.30 , -1.31 2021-02-12T23:01:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170914.26 epoch sec; seqNo:126; 4056068.18 north; 590339.01 east 2021-02-12T23:01:54Z- AHRS_M2 msg: 1613170914.61 epoch sec; seqNo:127 2021-02-12T23:01:54Z- AHRS_M2 msg: 5.29 phi; -0.08 theta; -0.01 psi 2021-02-12T23:01:54Z- handleDepth()-Depth_Keller msg: 1613170914617 epoch sec; seqNo:127; depth 0.13 2021-02-12T23:01:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170914.64 epoch sec; seqNo:127; 4056068.18 north; 590339.01 east 2021-02-12T23:01:55Z- AHRS_M2 msg: 1613170915.02 epoch sec; seqNo:128 2021-02-12T23:01:55Z- AHRS_M2 msg: 5.29 phi; -0.09 theta; -0.07 psi 2021-02-12T23:01:55Z- handleDepth()-Depth_Keller msg: 1613170915025 epoch sec; seqNo:128; depth 0.17 2021-02-12T23:01:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170915.06 epoch sec; seqNo:128; 4056068.19 north; 590340.23 east 2021-02-12T23:01:55Z- AHRS_M2 msg: 1613170915.42 epoch sec; seqNo:129 2021-02-12T23:01:55Z- AHRS_M2 msg: 5.28 phi; -0.05 theta; -0.10 psi 2021-02-12T23:01:55Z- handleDepth()-Depth_Keller msg: 1613170915434 epoch sec; seqNo:129; depth 0.21 2021-02-12T23:01:55Z- handleDvl() - RDI_Pathfinder msg: 1613170915.46 epoch sec; seqNo:61 2021-02-12T23:01:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.76 , 94.41 , 89.00 , 95.76 2021-02-12T23:01:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.31 , 0.10 , -0.39 2021-02-12T23:01:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170915.47 epoch sec; seqNo:129; 4056068.55 north; 590339.01 east 2021-02-12T23:01:55Z- AHRS_M2 msg: 1613170915.83 epoch sec; seqNo:130 2021-02-12T23:01:55Z- AHRS_M2 msg: 5.26 phi; -0.02 theta; -0.10 psi 2021-02-12T23:01:55Z- handleDepth()-Depth_Keller msg: 1613170915833 epoch sec; seqNo:130; depth 0.23 2021-02-12T23:01:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170915.86 epoch sec; seqNo:130; 4056068.55 north; 590339.01 east 2021-02-12T23:01:56Z- AHRS_M2 msg: 1613170916.23 epoch sec; seqNo:131 2021-02-12T23:01:56Z- AHRS_M2 msg: 5.22 phi; -0.04 theta; -0.07 psi 2021-02-12T23:01:56Z- handleDepth()-Depth_Keller msg: 1613170916233 epoch sec; seqNo:131; depth 0.26 2021-02-12T23:01:56Z- handleDvl() - RDI_Pathfinder msg: 1613170916.24 epoch sec; seqNo:62 2021-02-12T23:01:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.47 , 100.29 , 89.47 , 97.59 2021-02-12T23:01:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , -0.20 , 0.09 2021-02-12T23:01:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170916.28 epoch sec; seqNo:131; 4056068.55 north; 590339.01 east 2021-02-12T23:01:56Z- AHRS_M2 msg: 1613170916.63 epoch sec; seqNo:132 2021-02-12T23:01:56Z- AHRS_M2 msg: 5.21 phi; -0.06 theta; -0.05 psi 2021-02-12T23:01:56Z- handleDepth()-Depth_Keller msg: 1613170916645 epoch sec; seqNo:132; depth 0.26 2021-02-12T23:01:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170916.66 epoch sec; seqNo:132; 4056068.55 north; 590339.01 east 2021-02-12T23:01:57Z- AHRS_M2 msg: 1613170917.05 epoch sec; seqNo:133 2021-02-12T23:01:57Z- AHRS_M2 msg: 5.22 phi; -0.03 theta; -0.04 psi 2021-02-12T23:01:57Z- handleDepth()-Depth_Keller msg: 1613170917074 epoch sec; seqNo:133; depth 0.28 2021-02-12T23:01:57Z- handleDvl() - RDI_Pathfinder msg: 1613170917.09 epoch sec; seqNo:63 2021-02-12T23:01:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.11 , 98.87 , 89.40 , 96.17 2021-02-12T23:01:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.28 , -0.34 , 0.51 2021-02-12T23:01:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170917.10 epoch sec; seqNo:133; 4056068.55 north; 590339.01 east 2021-02-12T23:01:57Z- AHRS_M2 msg: 1613170917.44 epoch sec; seqNo:134 2021-02-12T23:01:57Z- AHRS_M2 msg: 5.18 phi; -0.00 theta; -0.02 psi 2021-02-12T23:01:57Z- handleDepth()-Depth_Keller msg: 1613170917444 epoch sec; seqNo:134; depth 0.30 2021-02-12T23:01:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170917.49 epoch sec; seqNo:134; 4056068.55 north; 590339.01 east 2021-02-12T23:01:57Z- AHRS_M2 msg: 1613170917.85 epoch sec; seqNo:135 2021-02-12T23:01:57Z- AHRS_M2 msg: 5.17 phi; -0.09 theta; -0.02 psi 2021-02-12T23:01:57Z- handleDepth()-Depth_Keller msg: 1613170917850 epoch sec; seqNo:135; depth 0.28 2021-02-12T23:01:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170917.86 epoch sec; seqNo:135; 4056068.55 north; 590339.01 east 2021-02-12T23:01:58Z- AHRS_M2 msg: 1613170918.25 epoch sec; seqNo:136 2021-02-12T23:01:58Z- AHRS_M2 msg: 5.21 phi; -0.23 theta; -0.02 psi 2021-02-12T23:01:58Z- handleDepth()-Depth_Keller msg: 1613170918266 epoch sec; seqNo:136; depth 0.25 2021-02-12T23:01:58Z- handleDvl() - RDI_Pathfinder msg: 1613170918.34 epoch sec; seqNo:64 2021-02-12T23:01:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.88 , 96.24 , 92.18 , 94.88 2021-02-12T23:01:58Z- handleDvl() - RDI_Pathfinder msg: velocities -0.66 , -0.53 , 0.90 2021-02-12T23:01:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170918.35 epoch sec; seqNo:136; 4056068.18 north; 590339.01 east 2021-02-12T23:01:58Z- AHRS_M2 msg: 1613170918.65 epoch sec; seqNo:137 2021-02-12T23:01:58Z- AHRS_M2 msg: 5.23 phi; -0.30 theta; 0.07 psi 2021-02-12T23:01:58Z- handleDepth()-Depth_Keller msg: 1613170918656 epoch sec; seqNo:137; depth 0.29 2021-02-12T23:01:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170918.67 epoch sec; seqNo:137; 4056067.80 north; 590339.02 east 2021-02-12T23:01:58Z- LcmTrn::handleMessages() - handling 41 messages took 0.77 ms and 126 calls... 2021-02-12T23:01:58Z- waiting for 1613170916.84, time is 1613170918.67 2021-02-12T23:01:58Z- LcmTrn::updateTrn() >>>> pose time 1613170918.67, meas time 1613170918.34 <<<< 2021-02-12T23:01:58Z- TerrainNav::Measurement type 1 from time = 1613170918.34 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:01:58Z- TerrainNav::motionUpdate() - currEstimate = (4056068, 590339.02, 0.29) 2021-02-12T23:01:58Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:01:58Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:01:58Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:01:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:01:58Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:01:58Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:01:58Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:01:59Z- AHRS_M2 msg: 1613170919.07 epoch sec; seqNo:138 2021-02-12T23:01:59Z- AHRS_M2 msg: 5.19 phi; -0.31 theta; 0.19 psi 2021-02-12T23:01:59Z- handleDepth()-Depth_Keller msg: 1613170919078 epoch sec; seqNo:138; depth 0.28 2021-02-12T23:01:59Z- handleDvl() - RDI_Pathfinder msg: 1613170919.09 epoch sec; seqNo:65 2021-02-12T23:01:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.58 , 98.87 , 85.34 , 117.82 2021-02-12T23:01:59Z- handleDvl() - RDI_Pathfinder msg: velocities -0.87 , 0.18 , -1.01 2021-02-12T23:01:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170919.11 epoch sec; seqNo:138; 4056067.80 north; 590339.02 east 2021-02-12T23:01:59Z- AHRS_M2 msg: 1613170919.46 epoch sec; seqNo:139 2021-02-12T23:01:59Z- AHRS_M2 msg: 5.17 phi; -0.30 theta; 0.13 psi 2021-02-12T23:01:59Z- handleDepth()-Depth_Keller msg: 1613170919465 epoch sec; seqNo:139; depth 0.23 2021-02-12T23:01:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170919.49 epoch sec; seqNo:139; 4056067.80 north; 590339.02 east 2021-02-12T23:01:59Z- AHRS_M2 msg: 1613170919.86 epoch sec; seqNo:140 2021-02-12T23:01:59Z- AHRS_M2 msg: 5.18 phi; -0.29 theta; -0.04 psi 2021-02-12T23:01:59Z- handleDepth()-Depth_Keller msg: 1613170919869 epoch sec; seqNo:140; depth 0.18 2021-02-12T23:01:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170919.89 epoch sec; seqNo:140; 4056067.80 north; 590339.02 east 2021-02-12T23:02:00Z- AHRS_M2 msg: 1613170920.27 epoch sec; seqNo:141 2021-02-12T23:02:00Z- AHRS_M2 msg: 5.21 phi; -0.22 theta; -0.16 psi 2021-02-12T23:02:00Z- handleDepth()-Depth_Keller msg: 1613170920272 epoch sec; seqNo:141; depth 0.11 2021-02-12T23:02:00Z- handleDvl() - RDI_Pathfinder msg: 1613170920.28 epoch sec; seqNo:66 2021-02-12T23:02:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 105.10 , 96.98 , 84.80 , 119.99 2021-02-12T23:02:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.43 , -0.19 , -1.15 2021-02-12T23:02:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170920.31 epoch sec; seqNo:141; 4056067.80 north; 590339.02 east 2021-02-12T23:02:00Z- AHRS_M2 msg: 1613170920.67 epoch sec; seqNo:142 2021-02-12T23:02:00Z- AHRS_M2 msg: 5.19 phi; -0.19 theta; -0.17 psi 2021-02-12T23:02:00Z- handleDepth()-Depth_Keller msg: 1613170920681 epoch sec; seqNo:142; depth 0.07 2021-02-12T23:02:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170920.69 epoch sec; seqNo:142; 4056067.80 north; 590339.02 east 2021-02-12T23:02:01Z- AHRS_M2 msg: 1613170921.10 epoch sec; seqNo:143 2021-02-12T23:02:01Z- AHRS_M2 msg: 5.14 phi; -0.16 theta; -0.15 psi 2021-02-12T23:02:01Z- handleDepth()-Depth_Keller msg: 1613170921110 epoch sec; seqNo:143; depth 0.10 2021-02-12T23:02:01Z- handleDvl() - RDI_Pathfinder msg: 1613170921.12 epoch sec; seqNo:67 2021-02-12T23:02:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 108.42 , 84.60 , 111.39 2021-02-12T23:02:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.09 , 0.01 2021-02-12T23:02:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170921.14 epoch sec; seqNo:143; 4056067.80 north; 590339.02 east 2021-02-12T23:02:01Z- AHRS_M2 msg: 1613170921.48 epoch sec; seqNo:144 2021-02-12T23:02:01Z- AHRS_M2 msg: 5.13 phi; -0.08 theta; -0.18 psi 2021-02-12T23:02:01Z- handleDepth()-Depth_Keller msg: 1613170921489 epoch sec; seqNo:144; depth 0.11 2021-02-12T23:02:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170921.52 epoch sec; seqNo:144; 4056067.80 north; 590339.02 east 2021-02-12T23:02:01Z- AHRS_M2 msg: 1613170921.88 epoch sec; seqNo:145 2021-02-12T23:02:01Z- AHRS_M2 msg: 5.17 phi; 0.01 theta; -0.16 psi 2021-02-12T23:02:01Z- handleDepth()-Depth_Keller msg: 1613170921889 epoch sec; seqNo:145; depth 0.10 2021-02-12T23:02:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170921.91 epoch sec; seqNo:145; 4056067.78 north; 590337.79 east 2021-02-12T23:02:02Z- AHRS_M2 msg: 1613170922.29 epoch sec; seqNo:146 2021-02-12T23:02:02Z- AHRS_M2 msg: 5.18 phi; 0.06 theta; -0.05 psi 2021-02-12T23:02:02Z- handleDepth()-Depth_Keller msg: 1613170922297 epoch sec; seqNo:146; depth 0.09 2021-02-12T23:02:02Z- handleDvl() - RDI_Pathfinder msg: 1613170922.31 epoch sec; seqNo:68 2021-02-12T23:02:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.42 , 109.43 , 97.25 , 99.96 2021-02-12T23:02:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.60 , 0.06 , 0.84 2021-02-12T23:02:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170922.32 epoch sec; seqNo:146; 4056068.16 north; 590337.79 east 2021-02-12T23:02:02Z- handleDepth()-Depth_Keller msg: 1613170922713 epoch sec; seqNo:147; depth 0.07 2021-02-12T23:02:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170922.73 epoch sec; seqNo:147; 4056068.16 north; 590337.79 east 2021-02-12T23:02:03Z- AHRS_M2 msg: 1613170923.10 epoch sec; seqNo:147 2021-02-12T23:02:03Z- AHRS_M2 msg: 5.04 phi; -0.06 theta; -0.14 psi 2021-02-12T23:02:03Z- handleDepth()-Depth_Keller msg: 1613170923143 epoch sec; seqNo:148; depth 0.03 2021-02-12T23:02:03Z- handleDvl() - RDI_Pathfinder msg: 1613170923.17 epoch sec; seqNo:69 2021-02-12T23:02:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.18 , 95.83 , 93.12 , 97.18 2021-02-12T23:02:03Z- handleDvl() - RDI_Pathfinder msg: velocities -0.27 , -0.50 , 0.57 2021-02-12T23:02:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170923.20 epoch sec; seqNo:148; 4056068.16 north; 590337.79 east 2021-02-12T23:02:03Z- AHRS_M2 msg: 1613170923.50 epoch sec; seqNo:148 2021-02-12T23:02:03Z- AHRS_M2 msg: 5.06 phi; -0.07 theta; -0.23 psi 2021-02-12T23:02:03Z- handleDepth()-Depth_Keller msg: 1613170923505 epoch sec; seqNo:149; depth -0.01 2021-02-12T23:02:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170923.54 epoch sec; seqNo:149; 4056067.78 north; 590337.79 east 2021-02-12T23:02:03Z- AHRS_M2 msg: 1613170923.90 epoch sec; seqNo:149 2021-02-12T23:02:03Z- AHRS_M2 msg: 5.12 phi; -0.05 theta; -0.23 psi 2021-02-12T23:02:03Z- handleDepth()-Depth_Keller msg: 1613170923909 epoch sec; seqNo:150; depth -0.03 2021-02-12T23:02:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170923.93 epoch sec; seqNo:150; 4056067.78 north; 590337.79 east 2021-02-12T23:02:04Z- LcmTrn::handleMessages() - handling 43 messages took 0.77 ms and 128 calls... 2021-02-12T23:02:04Z- waiting for 1613170921.67, time is 1613170923.93 2021-02-12T23:02:04Z- LcmTrn::updateTrn() >>>> pose time 1613170923.93, meas time 1613170923.17 <<<< 2021-02-12T23:02:04Z- TerrainNav::Measurement type 1 from time = 1613170923.17 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:02:04Z- TerrainNav::motionUpdate() - currEstimate = (4056068, 590337.79, -0.03) 2021-02-12T23:02:04Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:02:04Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:04Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:02:04Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:02:04Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:02:04Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:02:04Z- AHRS_M2 msg: 1613170924.31 epoch sec; seqNo:150 2021-02-12T23:02:04Z- AHRS_M2 msg: 5.15 phi; -0.05 theta; -0.11 psi 2021-02-12T23:02:04Z- handleDepth()-Depth_Keller msg: 1613170924317 epoch sec; seqNo:151; depth 0.02 2021-02-12T23:02:04Z- handleDvl() - RDI_Pathfinder msg: 1613170924.34 epoch sec; seqNo:70 2021-02-12T23:02:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 113.62 , 93.33 , 96.03 2021-02-12T23:02:04Z- handleDvl() - RDI_Pathfinder msg: velocities -0.16 , -0.58 , -0.10 2021-02-12T23:02:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170924.36 epoch sec; seqNo:151; 4056067.40 north; 590337.80 east 2021-02-12T23:02:04Z- AHRS_M2 msg: 1613170924.71 epoch sec; seqNo:151 2021-02-12T23:02:04Z- AHRS_M2 msg: 5.19 phi; -0.03 theta; 0.01 psi 2021-02-12T23:02:04Z- handleDepth()-Depth_Keller msg: 1613170924726 epoch sec; seqNo:152; depth 0.08 2021-02-12T23:02:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170924.75 epoch sec; seqNo:152; 4056067.03 north; 590337.80 east 2021-02-12T23:02:05Z- AHRS_M2 msg: 1613170925.13 epoch sec; seqNo:152 2021-02-12T23:02:05Z- AHRS_M2 msg: 5.22 phi; 0.03 theta; 0.05 psi 2021-02-12T23:02:05Z- handleDepth()-Depth_Keller msg: 1613170925149 epoch sec; seqNo:153; depth 0.08 2021-02-12T23:02:05Z- handleDvl() - RDI_Pathfinder msg: 1613170925.18 epoch sec; seqNo:71 2021-02-12T23:02:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.90 , 94.54 , 91.84 , 98.60 2021-02-12T23:02:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.29 , -0.07 , -0.11 2021-02-12T23:02:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170925.21 epoch sec; seqNo:153; 4056067.40 north; 590337.80 east 2021-02-12T23:02:05Z- AHRS_M2 msg: 1613170925.52 epoch sec; seqNo:153 2021-02-12T23:02:05Z- AHRS_M2 msg: 5.21 phi; 0.05 theta; 0.05 psi 2021-02-12T23:02:05Z- handleDepth()-Depth_Keller msg: 1613170925525 epoch sec; seqNo:154; depth 0.11 2021-02-12T23:02:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170925.54 epoch sec; seqNo:154; 4056067.40 north; 590337.80 east 2021-02-12T23:02:05Z- AHRS_M2 msg: 1613170925.92 epoch sec; seqNo:154 2021-02-12T23:02:05Z- AHRS_M2 msg: 5.21 phi; 0.06 theta; -0.05 psi 2021-02-12T23:02:05Z- handleDepth()-Depth_Keller msg: 1613170925930 epoch sec; seqNo:155; depth 0.09 2021-02-12T23:02:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170925.94 epoch sec; seqNo:155; 4056067.40 north; 590337.80 east 2021-02-12T23:02:06Z- AHRS_M2 msg: 1613170926.33 epoch sec; seqNo:155 2021-02-12T23:02:06Z- AHRS_M2 msg: 5.21 phi; 0.06 theta; -0.18 psi 2021-02-12T23:02:06Z- handleDepth()-Depth_Keller msg: 1613170926337 epoch sec; seqNo:156; depth 0.07 2021-02-12T23:02:06Z- handleDvl() - RDI_Pathfinder msg: 1613170926.36 epoch sec; seqNo:72 2021-02-12T23:02:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.10 , 93.39 , 94.75 , 93.39 2021-02-12T23:02:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.16 , 0.20 , 0.46 2021-02-12T23:02:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170926.37 epoch sec; seqNo:156; 4056067.40 north; 590337.80 east 2021-02-12T23:02:06Z- AHRS_M2 msg: 1613170926.73 epoch sec; seqNo:156 2021-02-12T23:02:06Z- AHRS_M2 msg: 5.16 phi; 0.00 theta; -0.21 psi 2021-02-12T23:02:06Z- handleDepth()-Depth_Keller msg: 1613170926736 epoch sec; seqNo:157; depth 0.06 2021-02-12T23:02:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170926.75 epoch sec; seqNo:157; 4056067.40 north; 590337.80 east 2021-02-12T23:02:07Z- AHRS_M2 msg: 1613170927.14 epoch sec; seqNo:157 2021-02-12T23:02:07Z- AHRS_M2 msg: 5.22 phi; -0.10 theta; -0.14 psi 2021-02-12T23:02:07Z- handleDepth()-Depth_Keller msg: 1613170927149 epoch sec; seqNo:158; depth 0.07 2021-02-12T23:02:07Z- handleDvl() - RDI_Pathfinder msg: 1613170927.16 epoch sec; seqNo:73 2021-02-12T23:02:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.05 , 109.70 , 97.52 , 96.17 2021-02-12T23:02:07Z- handleDvl() - RDI_Pathfinder msg: velocities -0.46 , -0.05 , 0.04 2021-02-12T23:02:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170927.17 epoch sec; seqNo:158; 4056067.40 north; 590337.80 east 2021-02-12T23:02:07Z- AHRS_M2 msg: 1613170927.55 epoch sec; seqNo:158 2021-02-12T23:02:07Z- AHRS_M2 msg: 5.29 phi; -0.14 theta; -0.06 psi 2021-02-12T23:02:07Z- handleDepth()-Depth_Keller msg: 1613170927557 epoch sec; seqNo:159; depth 0.04 2021-02-12T23:02:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170927.57 epoch sec; seqNo:159; 4056067.40 north; 590337.80 east 2021-02-12T23:02:07Z- AHRS_M2 msg: 1613170927.94 epoch sec; seqNo:159 2021-02-12T23:02:07Z- AHRS_M2 msg: 5.31 phi; -0.13 theta; 0.03 psi 2021-02-12T23:02:07Z- handleDepth()-Depth_Keller msg: 1613170927953 epoch sec; seqNo:160; depth 0.02 2021-02-12T23:02:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170927.97 epoch sec; seqNo:160; 4056067.40 north; 590337.80 east 2021-02-12T23:02:08Z- AHRS_M2 msg: 1613170928.35 epoch sec; seqNo:160 2021-02-12T23:02:08Z- AHRS_M2 msg: 5.27 phi; -0.05 theta; 0.04 psi 2021-02-12T23:02:08Z- handleDepth()-Depth_Keller msg: 1613170928352 epoch sec; seqNo:161; depth 0.05 2021-02-12T23:02:08Z- handleDvl() - RDI_Pathfinder msg: 1613170928.36 epoch sec; seqNo:74 2021-02-12T23:02:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.48 , 99.96 , 89.13 , 101.31 2021-02-12T23:02:08Z- handleDvl() - RDI_Pathfinder msg: velocities -0.23 , 0.07 , -0.51 2021-02-12T23:02:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170928.39 epoch sec; seqNo:161; 4056067.03 north; 590337.80 east 2021-02-12T23:02:08Z- AHRS_M2 msg: 1613170928.75 epoch sec; seqNo:161 2021-02-12T23:02:08Z- AHRS_M2 msg: 5.21 phi; 0.00 theta; -0.13 psi 2021-02-12T23:02:08Z- handleDepth()-Depth_Keller msg: 1613170928760 epoch sec; seqNo:162; depth 0.06 2021-02-12T23:02:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170928.77 epoch sec; seqNo:162; 4056067.03 north; 590337.80 east 2021-02-12T23:02:09Z- LcmTrn::handleMessages() - handling 41 messages took 0.76 ms and 126 calls... 2021-02-12T23:02:09Z- waiting for 1613170926.93, time is 1613170928.77 2021-02-12T23:02:09Z- LcmTrn::updateTrn() >>>> pose time 1613170928.77, meas time 1613170928.36 <<<< 2021-02-12T23:02:09Z- TerrainNav::Measurement type 1 from time = 1613170928.36 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:02:09Z- TerrainNav::motionUpdate() - currEstimate = (4056067, 590337.80, 0.06) 2021-02-12T23:02:09Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:02:09Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:09Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:02:09Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:02:09Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:02:09Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:02:09Z- AHRS_M2 msg: 1613170929.15 epoch sec; seqNo:162 2021-02-12T23:02:09Z- AHRS_M2 msg: 5.21 phi; -0.02 theta; -0.31 psi 2021-02-12T23:02:09Z- handleDepth()-Depth_Keller msg: 1613170929169 epoch sec; seqNo:163; depth 0.01 2021-02-12T23:02:09Z- handleDvl() - RDI_Pathfinder msg: 1613170929.18 epoch sec; seqNo:75 2021-02-12T23:02:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.10 , 92.04 , 90.69 , 97.45 2021-02-12T23:02:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.31 , -0.02 , -0.02 2021-02-12T23:02:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170929.19 epoch sec; seqNo:163; 4056067.40 north; 590337.80 east 2021-02-12T23:02:09Z- AHRS_M2 msg: 1613170929.56 epoch sec; seqNo:163 2021-02-12T23:02:09Z- AHRS_M2 msg: 5.28 phi; -0.06 theta; -0.37 psi 2021-02-12T23:02:09Z- handleDepth()-Depth_Keller msg: 1613170929564 epoch sec; seqNo:164; depth 0.01 2021-02-12T23:02:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170929.58 epoch sec; seqNo:164; 4056067.40 north; 590337.80 east 2021-02-12T23:02:09Z- AHRS_M2 msg: 1613170929.96 epoch sec; seqNo:164 2021-02-12T23:02:09Z- AHRS_M2 msg: 5.34 phi; -0.07 theta; -0.27 psi 2021-02-12T23:02:09Z- handleDepth()-Depth_Keller msg: 1613170929968 epoch sec; seqNo:165; depth -0.02 2021-02-12T23:02:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170929.98 epoch sec; seqNo:165; 4056067.40 north; 590337.80 east 2021-02-12T23:02:10Z- AHRS_M2 msg: 1613170930.37 epoch sec; seqNo:165 2021-02-12T23:02:10Z- AHRS_M2 msg: 5.36 phi; -0.01 theta; -0.07 psi 2021-02-12T23:02:10Z- handleDepth()-Depth_Keller msg: 1613170930377 epoch sec; seqNo:166; depth -0.01 2021-02-12T23:02:10Z- handleDvl() - RDI_Pathfinder msg: 1613170930.39 epoch sec; seqNo:76 2021-02-12T23:02:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 126.01 , 97.59 , 104.36 2021-02-12T23:02:10Z- handleDvl() - RDI_Pathfinder msg: velocities -0.06 , -0.47 , -0.07 2021-02-12T23:02:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170930.40 epoch sec; seqNo:166; 4056067.03 north; 590337.80 east 2021-02-12T23:02:10Z- AHRS_M2 msg: 1613170930.77 epoch sec; seqNo:166 2021-02-12T23:02:10Z- AHRS_M2 msg: 5.34 phi; 0.06 theta; 0.04 psi 2021-02-12T23:02:10Z- handleDepth()-Depth_Keller msg: 1613170930781 epoch sec; seqNo:167; depth 0.06 2021-02-12T23:02:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170930.81 epoch sec; seqNo:167; 4056067.03 north; 590337.80 east 2021-02-12T23:02:11Z- AHRS_M2 msg: 1613170931.17 epoch sec; seqNo:167 2021-02-12T23:02:11Z- AHRS_M2 msg: 5.33 phi; 0.09 theta; -0.01 psi 2021-02-12T23:02:11Z- handleDepth()-Depth_Keller msg: 1613170931181 epoch sec; seqNo:168; depth 0.08 2021-02-12T23:02:11Z- handleDvl() - RDI_Pathfinder msg: 1613170931.21 epoch sec; seqNo:77 2021-02-12T23:02:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.86 , 97.86 , 97.86 , 97.86 2021-02-12T23:02:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.44 , -0.12 , 0.53 2021-02-12T23:02:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170931.24 epoch sec; seqNo:168; 4056067.03 north; 590337.80 east 2021-02-12T23:02:11Z- AHRS_M2 msg: 1613170931.58 epoch sec; seqNo:168 2021-02-12T23:02:11Z- AHRS_M2 msg: 5.39 phi; 0.07 theta; -0.07 psi 2021-02-12T23:02:11Z- handleDepth()-Depth_Keller msg: 1613170931589 epoch sec; seqNo:169; depth 0.03 2021-02-12T23:02:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170931.63 epoch sec; seqNo:169; 4056067.03 north; 590337.80 east 2021-02-12T23:02:11Z- AHRS_M2 msg: 1613170931.98 epoch sec; seqNo:169 2021-02-12T23:02:11Z- AHRS_M2 msg: 5.45 phi; -0.00 theta; 0.01 psi 2021-02-12T23:02:11Z- handleDepth()-Depth_Keller msg: 1613170931993 epoch sec; seqNo:170; depth 0.04 2021-02-12T23:02:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170932.01 epoch sec; seqNo:170; 4056067.39 north; 590336.58 east 2021-02-12T23:02:12Z- AHRS_M2 msg: 1613170932.39 epoch sec; seqNo:170 2021-02-12T23:02:12Z- AHRS_M2 msg: 5.42 phi; -0.09 theta; 0.08 psi 2021-02-12T23:02:12Z- handleDepth()-Depth_Keller msg: 1613170932404 epoch sec; seqNo:171; depth 0.04 2021-02-12T23:02:12Z- handleDvl() - RDI_Pathfinder msg: 1613170932.45 epoch sec; seqNo:78 2021-02-12T23:02:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.19 , 97.25 , 98.60 , 90.48 2021-02-12T23:02:12Z- handleDvl() - RDI_Pathfinder msg: velocities -0.46 , 0.05 , 0.96 2021-02-12T23:02:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170932.46 epoch sec; seqNo:171; 4056067.03 north; 590337.80 east 2021-02-12T23:02:12Z- AHRS_M2 msg: 1613170932.79 epoch sec; seqNo:171 2021-02-12T23:02:12Z- AHRS_M2 msg: 5.39 phi; -0.18 theta; 0.02 psi 2021-02-12T23:02:12Z- handleDepth()-Depth_Keller msg: 1613170932797 epoch sec; seqNo:172; depth 0.02 2021-02-12T23:02:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170932.82 epoch sec; seqNo:172; 4056067.03 north; 590337.80 east 2021-02-12T23:02:13Z- AHRS_M2 msg: 1613170933.20 epoch sec; seqNo:172 2021-02-12T23:02:13Z- AHRS_M2 msg: 5.43 phi; -0.16 theta; -0.11 psi 2021-02-12T23:02:13Z- handleDepth()-Depth_Keller msg: 1613170933201 epoch sec; seqNo:173; depth 0.01 2021-02-12T23:02:13Z- handleDvl() - RDI_Pathfinder msg: 1613170933.21 epoch sec; seqNo:79 2021-02-12T23:02:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 102.93 , 92.11 , 97.52 , 102.93 2021-02-12T23:02:13Z- handleDvl() - RDI_Pathfinder msg: velocities -1.08 , 0.00 , -0.60 2021-02-12T23:02:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170933.26 epoch sec; seqNo:173; 4056066.65 north; 590337.81 east 2021-02-12T23:02:13Z- AHRS_M2 msg: 1613170933.60 epoch sec; seqNo:173 2021-02-12T23:02:13Z- AHRS_M2 msg: 5.46 phi; -0.12 theta; -0.14 psi 2021-02-12T23:02:13Z- handleDepth()-Depth_Keller msg: 1613170933609 epoch sec; seqNo:174; depth -0.01 2021-02-12T23:02:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170933.62 epoch sec; seqNo:174; 4056066.27 north; 590337.81 east 2021-02-12T23:02:14Z- AHRS_M2 msg: 1613170934.00 epoch sec; seqNo:174 2021-02-12T23:02:14Z- AHRS_M2 msg: 5.50 phi; -0.10 theta; -0.10 psi 2021-02-12T23:02:14Z- handleDepth()-Depth_Keller msg: 1613170934009 epoch sec; seqNo:175; depth 0.02 2021-02-12T23:02:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170934.04 epoch sec; seqNo:175; 4056065.89 north; 590337.81 east 2021-02-12T23:02:14Z- LcmTrn::handleMessages() - handling 44 messages took 0.91 ms and 126 calls... 2021-02-12T23:02:14Z- waiting for 1613170931.77, time is 1613170934.04 2021-02-12T23:02:14Z- LcmTrn::updateTrn() >>>> pose time 1613170934.04, meas time 1613170933.21 <<<< 2021-02-12T23:02:14Z- TerrainNav::Measurement type 1 from time = 1613170933.21 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:02:14Z- TerrainNav::motionUpdate() - currEstimate = (4056066, 590337.81, 0.02) 2021-02-12T23:02:14Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:02:14Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:14Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:02:14Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:02:14Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:02:14Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:02:14Z- AHRS_M2 msg: 1613170934.41 epoch sec; seqNo:175 2021-02-12T23:02:14Z- AHRS_M2 msg: 5.52 phi; -0.03 theta; -0.00 psi 2021-02-12T23:02:14Z- handleDepth()-Depth_Keller msg: 1613170934413 epoch sec; seqNo:176; depth 0.09 2021-02-12T23:02:14Z- handleDvl() - RDI_Pathfinder msg: 1613170934.42 epoch sec; seqNo:80 2021-02-12T23:02:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.00 , 105.24 , 86.29 , 105.24 2021-02-12T23:02:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.19 , -0.09 , -1.16 2021-02-12T23:02:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170934.45 epoch sec; seqNo:176; 4056066.27 north; 590337.81 east 2021-02-12T23:02:14Z- AHRS_M2 msg: 1613170934.81 epoch sec; seqNo:176 2021-02-12T23:02:14Z- AHRS_M2 msg: 5.48 phi; 0.07 theta; 0.02 psi 2021-02-12T23:02:14Z- handleDepth()-Depth_Keller msg: 1613170934821 epoch sec; seqNo:177; depth 0.17 2021-02-12T23:02:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170934.83 epoch sec; seqNo:177; 4056066.27 north; 590337.81 east 2021-02-12T23:02:15Z- AHRS_M2 msg: 1613170935.21 epoch sec; seqNo:177 2021-02-12T23:02:15Z- AHRS_M2 msg: 5.47 phi; 0.08 theta; -0.03 psi 2021-02-12T23:02:15Z- handleDepth()-Depth_Keller msg: 1613170935223 epoch sec; seqNo:178; depth 0.19 2021-02-12T23:02:15Z- handleDvl() - RDI_Pathfinder msg: 1613170935.23 epoch sec; seqNo:81 2021-02-12T23:02:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.68 , 91.97 , 97.39 , 97.39 2021-02-12T23:02:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.32 , -0.17 2021-02-12T23:02:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170935.25 epoch sec; seqNo:178; 4056066.27 north; 590337.81 east 2021-02-12T23:02:15Z- AHRS_M2 msg: 1613170935.62 epoch sec; seqNo:178 2021-02-12T23:02:15Z- AHRS_M2 msg: 5.50 phi; 0.07 theta; -0.04 psi 2021-02-12T23:02:15Z- handleDepth()-Depth_Keller msg: 1613170935629 epoch sec; seqNo:179; depth 0.20 2021-02-12T23:02:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170935.65 epoch sec; seqNo:179; 4056066.65 north; 590337.81 east 2021-02-12T23:02:16Z- AHRS_M2 msg: 1613170936.03 epoch sec; seqNo:179 2021-02-12T23:02:16Z- AHRS_M2 msg: 5.54 phi; 0.06 theta; -0.07 psi 2021-02-12T23:02:16Z- handleDepth()-Depth_Keller msg: 1613170936035 epoch sec; seqNo:180; depth 0.21 2021-02-12T23:02:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170936.07 epoch sec; seqNo:180; 4056066.63 north; 590336.59 east 2021-02-12T23:02:16Z- AHRS_M2 msg: 1613170936.43 epoch sec; seqNo:180 2021-02-12T23:02:16Z- AHRS_M2 msg: 5.56 phi; 0.01 theta; -0.07 psi 2021-02-12T23:02:16Z- handleDepth()-Depth_Keller msg: 1613170936437 epoch sec; seqNo:181; depth 0.22 2021-02-12T23:02:16Z- handleDvl() - RDI_Pathfinder msg: 1613170936.45 epoch sec; seqNo:82 2021-02-12T23:02:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.39 , 96.10 , 96.10 , 90.69 2021-02-12T23:02:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.36 , -0.14 , 0.53 2021-02-12T23:02:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170936.46 epoch sec; seqNo:181; 4056066.63 north; 590336.59 east 2021-02-12T23:02:16Z- AHRS_M2 msg: 1613170936.83 epoch sec; seqNo:181 2021-02-12T23:02:16Z- AHRS_M2 msg: 5.57 phi; -0.06 theta; -0.03 psi 2021-02-12T23:02:16Z- handleDepth()-Depth_Keller msg: 1613170936845 epoch sec; seqNo:182; depth 0.22 2021-02-12T23:02:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170936.86 epoch sec; seqNo:182; 4056066.63 north; 590336.59 east 2021-02-12T23:02:17Z- AHRS_M2 msg: 1613170937.24 epoch sec; seqNo:182 2021-02-12T23:02:17Z- AHRS_M2 msg: 5.55 phi; -0.09 theta; 0.01 psi 2021-02-12T23:02:17Z- handleDepth()-Depth_Keller msg: 1613170937240 epoch sec; seqNo:183; depth 0.22 2021-02-12T23:02:17Z- handleDvl() - RDI_Pathfinder msg: 1613170937.28 epoch sec; seqNo:83 2021-02-12T23:02:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.18 , 96.24 , 97.59 , 94.88 2021-02-12T23:02:17Z- handleDvl() - RDI_Pathfinder msg: velocities -0.38 , -0.12 , 0.43 2021-02-12T23:02:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170937.29 epoch sec; seqNo:183; 4056066.63 north; 590336.59 east 2021-02-12T23:02:17Z- AHRS_M2 msg: 1613170937.64 epoch sec; seqNo:183 2021-02-12T23:02:17Z- AHRS_M2 msg: 5.59 phi; -0.07 theta; 0.03 psi 2021-02-12T23:02:17Z- handleDepth()-Depth_Keller msg: 1613170937645 epoch sec; seqNo:184; depth 0.21 2021-02-12T23:02:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170937.68 epoch sec; seqNo:184; 4056066.63 north; 590336.59 east 2021-02-12T23:02:18Z- AHRS_M2 msg: 1613170938.08 epoch sec; seqNo:184 2021-02-12T23:02:18Z- AHRS_M2 msg: 5.63 phi; -0.06 theta; 0.08 psi 2021-02-12T23:02:18Z- handleDepth()-Depth_Keller msg: 1613170938100 epoch sec; seqNo:185; depth 0.21 2021-02-12T23:02:18Z- handleDvl() - RDI_Pathfinder msg: 1613170938.18 epoch sec; seqNo:84 2021-02-12T23:02:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.81 , 97.45 , 89.33 , 100.16 2021-02-12T23:02:18Z- handleDvl() - RDI_Pathfinder msg: velocities -0.31 , 0.27 , 0.42 2021-02-12T23:02:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170938.25 epoch sec; seqNo:185; 4056066.26 north; 590336.59 east 2021-02-12T23:02:18Z- AHRS_M2 msg: 1613170938.45 epoch sec; seqNo:185 2021-02-12T23:02:18Z- AHRS_M2 msg: 5.58 phi; -0.12 theta; 0.06 psi 2021-02-12T23:02:18Z- handleDepth()-Depth_Keller msg: 1613170938454 epoch sec; seqNo:186; depth 0.17 2021-02-12T23:02:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170938.51 epoch sec; seqNo:186; 4056066.27 north; 590337.81 east 2021-02-12T23:02:18Z- AHRS_M2 msg: 1613170938.85 epoch sec; seqNo:186 2021-02-12T23:02:18Z- AHRS_M2 msg: 5.56 phi; -0.26 theta; -0.08 psi 2021-02-12T23:02:18Z- handleDepth()-Depth_Keller msg: 1613170938857 epoch sec; seqNo:187; depth 0.15 2021-02-12T23:02:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170938.90 epoch sec; seqNo:187; 4056066.27 north; 590337.81 east 2021-02-12T23:02:19Z- LcmTrn::handleMessages() - handling 41 messages took 0.85 ms and 124 calls... 2021-02-12T23:02:19Z- waiting for 1613170937.04, time is 1613170938.90 2021-02-12T23:02:19Z- LcmTrn::updateTrn() >>>> pose time 1613170938.90, meas time 1613170938.18 <<<< 2021-02-12T23:02:19Z- TerrainNav::Measurement type 1 from time = 1613170938.18 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:02:19Z- TerrainNav::motionUpdate() - currEstimate = (4056066, 590337.81, 0.15) 2021-02-12T23:02:19Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:02:19Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:19Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:02:19Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:02:19Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:02:19Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:02:19Z- AHRS_M2 msg: 1613170939.26 epoch sec; seqNo:187 2021-02-12T23:02:19Z- AHRS_M2 msg: 5.60 phi; -0.33 theta; -0.23 psi 2021-02-12T23:02:19Z- handleDepth()-Depth_Keller msg: 1613170939267 epoch sec; seqNo:188; depth 0.13 2021-02-12T23:02:19Z- handleDvl() - RDI_Pathfinder msg: 1613170939.29 epoch sec; seqNo:85 2021-02-12T23:02:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.74 , 97.39 , 97.39 , 100.09 2021-02-12T23:02:19Z- handleDvl() - RDI_Pathfinder msg: velocities -0.80 , -0.03 , -0.64 2021-02-12T23:02:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170939.30 epoch sec; seqNo:188; 4056066.27 north; 590337.81 east 2021-02-12T23:02:19Z- AHRS_M2 msg: 1613170939.66 epoch sec; seqNo:188 2021-02-12T23:02:19Z- AHRS_M2 msg: 5.63 phi; -0.29 theta; -0.20 psi 2021-02-12T23:02:19Z- handleDepth()-Depth_Keller msg: 1613170939664 epoch sec; seqNo:189; depth 0.09 2021-02-12T23:02:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170939.69 epoch sec; seqNo:189; 4056065.89 north; 590337.81 east 2021-02-12T23:02:20Z- AHRS_M2 msg: 1613170940.07 epoch sec; seqNo:189 2021-02-12T23:02:20Z- AHRS_M2 msg: 5.66 phi; -0.26 theta; -0.06 psi 2021-02-12T23:02:20Z- handleDepth()-Depth_Keller msg: 1613170940085 epoch sec; seqNo:190; depth 0.09 2021-02-12T23:02:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170940.16 epoch sec; seqNo:190; 4056065.51 north; 590337.82 east 2021-02-12T23:02:20Z- AHRS_M2 msg: 1613170940.47 epoch sec; seqNo:190 2021-02-12T23:02:20Z- AHRS_M2 msg: 5.72 phi; -0.23 theta; 0.09 psi 2021-02-12T23:02:20Z- handleDepth()-Depth_Keller msg: 1613170940498 epoch sec; seqNo:191; depth 0.09 2021-02-12T23:02:20Z- handleDvl() - RDI_Pathfinder msg: 1613170940.53 epoch sec; seqNo:86 2021-02-12T23:02:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.18 , 114.78 , 85.00 , 120.19 2021-02-12T23:02:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.34 , -0.14 , -0.86 2021-02-12T23:02:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170940.58 epoch sec; seqNo:191; 4056065.89 north; 590337.81 east 2021-02-12T23:02:20Z- AHRS_M2 msg: 1613170940.88 epoch sec; seqNo:191 2021-02-12T23:02:20Z- AHRS_M2 msg: 5.70 phi; -0.14 theta; 0.18 psi 2021-02-12T23:02:20Z- handleDepth()-Depth_Keller msg: 1613170940893 epoch sec; seqNo:192; depth 0.13 2021-02-12T23:02:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170940.92 epoch sec; seqNo:192; 4056065.89 north; 590337.81 east 2021-02-12T23:02:21Z- AHRS_M2 msg: 1613170941.29 epoch sec; seqNo:192 2021-02-12T23:02:21Z- AHRS_M2 msg: 5.69 phi; -0.01 theta; 0.09 psi 2021-02-12T23:02:21Z- handleDepth()-Depth_Keller msg: 1613170941301 epoch sec; seqNo:193; depth 0.08 2021-02-12T23:02:21Z- handleDvl() - RDI_Pathfinder msg: 1613170941.35 epoch sec; seqNo:87 2021-02-12T23:02:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 108.21 , 88.86 , 0.00 , 112.68 2021-02-12T23:02:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.14 , 0.03 2021-02-12T23:02:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170941.39 epoch sec; seqNo:193; 4056066.27 north; 590337.81 east 2021-02-12T23:02:21Z- AHRS_M2 msg: 1613170941.70 epoch sec; seqNo:193 2021-02-12T23:02:21Z- AHRS_M2 msg: 5.72 phi; -0.01 theta; -0.04 psi 2021-02-12T23:02:21Z- handleDepth()-Depth_Keller msg: 1613170941709 epoch sec; seqNo:194; depth 0.04 2021-02-12T23:02:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170941.74 epoch sec; seqNo:194; 4056066.63 north; 590336.59 east 2021-02-12T23:02:22Z- AHRS_M2 msg: 1613170942.12 epoch sec; seqNo:194 2021-02-12T23:02:22Z- AHRS_M2 msg: 5.73 phi; -0.09 theta; -0.12 psi 2021-02-12T23:02:22Z- handleDepth()-Depth_Keller msg: 1613170942142 epoch sec; seqNo:195; depth 0.05 2021-02-12T23:02:22Z- handleDvl() - RDI_Pathfinder msg: 1613170942.17 epoch sec; seqNo:88 2021-02-12T23:02:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.39 , 96.37 , 97.86 , 94.88 2021-02-12T23:02:22Z- handleDvl() - RDI_Pathfinder msg: velocities -0.08 , -0.07 , 0.75 2021-02-12T23:02:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170942.21 epoch sec; seqNo:195; 4056066.63 north; 590336.59 east 2021-02-12T23:02:22Z- AHRS_M2 msg: 1613170942.51 epoch sec; seqNo:195 2021-02-12T23:02:22Z- AHRS_M2 msg: 5.75 phi; -0.14 theta; -0.12 psi 2021-02-12T23:02:22Z- handleDepth()-Depth_Keller msg: 1613170942525 epoch sec; seqNo:196; depth 0.08 2021-02-12T23:02:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170942.56 epoch sec; seqNo:196; 4056066.26 north; 590336.59 east 2021-02-12T23:02:22Z- AHRS_M2 msg: 1613170942.92 epoch sec; seqNo:196 2021-02-12T23:02:22Z- AHRS_M2 msg: 5.76 phi; -0.10 theta; -0.04 psi 2021-02-12T23:02:22Z- handleDepth()-Depth_Keller msg: 1613170942933 epoch sec; seqNo:197; depth 0.06 2021-02-12T23:02:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170943.01 epoch sec; seqNo:197; 4056066.26 north; 590336.59 east 2021-02-12T23:02:23Z- AHRS_M2 msg: 1613170943.35 epoch sec; seqNo:197 2021-02-12T23:02:23Z- AHRS_M2 msg: 5.76 phi; 0.03 theta; 0.10 psi 2021-02-12T23:02:23Z- handleDepth()-Depth_Keller msg: 1613170943377 epoch sec; seqNo:198; depth 0.03 2021-02-12T23:02:23Z- handleDvl() - RDI_Pathfinder msg: 1613170943.42 epoch sec; seqNo:89 2021-02-12T23:02:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.04 , 101.51 , 0.00 , 102.87 2021-02-12T23:02:23Z- handleDvl() - RDI_Pathfinder msg: velocities -0.36 , -0.46 , 0.11 2021-02-12T23:02:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170943.45 epoch sec; seqNo:198; 4056066.26 north; 590336.59 east 2021-02-12T23:02:23Z- AHRS_M2 msg: 1613170943.73 epoch sec; seqNo:198 2021-02-12T23:02:23Z- AHRS_M2 msg: 5.74 phi; 0.04 theta; 0.11 psi 2021-02-12T23:02:23Z- handleDepth()-Depth_Keller msg: 1613170943745 epoch sec; seqNo:199; depth 0.03 2021-02-12T23:02:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170943.78 epoch sec; seqNo:199; 4056065.88 north; 590336.59 east 2021-02-12T23:02:24Z- LcmTrn::handleMessages() - handling 41 messages took 0.93 ms and 123 calls... 2021-02-12T23:02:24Z- waiting for 1613170941.90, time is 1613170943.78 2021-02-12T23:02:24Z- LcmTrn::updateTrn() >>>> pose time 1613170943.78, meas time 1613170943.42 <<<< 2021-02-12T23:02:24Z- TerrainNav::Measurement type 1 from time = 1613170943.42 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:02:24Z- TerrainNav::motionUpdate() - currEstimate = (4056066, 590336.59, 0.03) 2021-02-12T23:02:24Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:02:24Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:24Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:02:24Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:02:24Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:02:24Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:02:24Z- AHRS_M2 msg: 1613170944.15 epoch sec; seqNo:199 2021-02-12T23:02:24Z- AHRS_M2 msg: 5.77 phi; -0.04 theta; 0.04 psi 2021-02-12T23:02:24Z- handleDepth()-Depth_Keller msg: 1613170944169 epoch sec; seqNo:200; depth 0.01 2021-02-12T23:02:24Z- handleDvl() - RDI_Pathfinder msg: 1613170944.21 epoch sec; seqNo:90 2021-02-12T23:02:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.95 , 89.06 , 95.83 , 94.48 2021-02-12T23:02:24Z- handleDvl() - RDI_Pathfinder msg: velocities -0.30 , -0.26 , 0.46 2021-02-12T23:02:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170944.26 epoch sec; seqNo:200; 4056065.88 north; 590336.59 east 2021-02-12T23:02:24Z- AHRS_M2 msg: 1613170944.55 epoch sec; seqNo:200 2021-02-12T23:02:24Z- AHRS_M2 msg: 5.78 phi; -0.08 theta; 0.03 psi 2021-02-12T23:02:24Z- handleDepth()-Depth_Keller msg: 1613170944561 epoch sec; seqNo:201; depth 0.02 2021-02-12T23:02:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170944.60 epoch sec; seqNo:201; 4056065.50 north; 590336.60 east 2021-02-12T23:02:24Z- AHRS_M2 msg: 1613170944.96 epoch sec; seqNo:201 2021-02-12T23:02:24Z- AHRS_M2 msg: 5.79 phi; -0.07 theta; 0.02 psi 2021-02-12T23:02:24Z- handleDepth()-Depth_Keller msg: 1613170944981 epoch sec; seqNo:202; depth 0.04 2021-02-12T23:02:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170945.02 epoch sec; seqNo:202; 4056065.50 north; 590336.60 east 2021-02-12T23:02:25Z- AHRS_M2 msg: 1613170945.37 epoch sec; seqNo:202 2021-02-12T23:02:25Z- AHRS_M2 msg: 5.79 phi; -0.06 theta; -0.02 psi 2021-02-12T23:02:25Z- handleDepth()-Depth_Keller msg: 1613170945389 epoch sec; seqNo:203; depth 0.03 2021-02-12T23:02:25Z- handleDvl() - RDI_Pathfinder msg: 1613170945.42 epoch sec; seqNo:91 2021-02-12T23:02:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.39 , 93.33 , 89.27 , 100.09 2021-02-12T23:02:25Z- handleDvl() - RDI_Pathfinder msg: velocities -0.35 , 0.10 , -0.31 2021-02-12T23:02:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170945.46 epoch sec; seqNo:203; 4056065.50 north; 590336.60 east 2021-02-12T23:02:25Z- AHRS_M2 msg: 1613170945.78 epoch sec; seqNo:203 2021-02-12T23:02:25Z- AHRS_M2 msg: 5.79 phi; -0.05 theta; -0.03 psi 2021-02-12T23:02:25Z- handleDepth()-Depth_Keller msg: 1613170945789 epoch sec; seqNo:204; depth 0.05 2021-02-12T23:02:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170945.83 epoch sec; seqNo:204; 4056065.12 north; 590336.60 east 2021-02-12T23:02:26Z- AHRS_M2 msg: 1613170946.18 epoch sec; seqNo:204 2021-02-12T23:02:26Z- AHRS_M2 msg: 5.78 phi; -0.07 theta; -0.01 psi 2021-02-12T23:02:26Z- handleDepth()-Depth_Keller msg: 1613170946197 epoch sec; seqNo:205; depth 0.08 2021-02-12T23:02:26Z- handleDvl() - RDI_Pathfinder msg: 1613170946.23 epoch sec; seqNo:92 2021-02-12T23:02:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.10 , 94.75 , 87.98 , 96.10 2021-02-12T23:02:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.00 , 0.02 , -0.11 2021-02-12T23:02:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170946.29 epoch sec; seqNo:205; 4056065.12 north; 590336.60 east 2021-02-12T23:02:26Z- AHRS_M2 msg: 1613170946.59 epoch sec; seqNo:205 2021-02-12T23:02:26Z- AHRS_M2 msg: 5.79 phi; -0.16 theta; 0.04 psi 2021-02-12T23:02:26Z- handleDepth()-Depth_Keller msg: 1613170946601 epoch sec; seqNo:206; depth 0.07 2021-02-12T23:02:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170946.64 epoch sec; seqNo:206; 4056065.12 north; 590336.60 east 2021-02-12T23:02:27Z- AHRS_M2 msg: 1613170947.00 epoch sec; seqNo:206 2021-02-12T23:02:27Z- AHRS_M2 msg: 5.76 phi; -0.22 theta; 0.06 psi 2021-02-12T23:02:27Z- handleDepth()-Depth_Keller msg: 1613170947021 epoch sec; seqNo:207; depth 0.09 2021-02-12T23:02:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170947.08 epoch sec; seqNo:207; 4056065.12 north; 590336.60 east 2021-02-12T23:02:27Z- AHRS_M2 msg: 1613170947.41 epoch sec; seqNo:207 2021-02-12T23:02:27Z- AHRS_M2 msg: 5.74 phi; -0.18 theta; 0.01 psi 2021-02-12T23:02:27Z- handleDepth()-Depth_Keller msg: 1613170947466 epoch sec; seqNo:208; depth 0.12 2021-02-12T23:02:27Z- handleDvl() - RDI_Pathfinder msg: 1613170947.51 epoch sec; seqNo:93 2021-02-12T23:02:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.59 , 93.53 , 86.76 , 104.36 2021-02-12T23:02:27Z- handleDvl() - RDI_Pathfinder msg: velocities -0.29 , 0.08 , -0.23 2021-02-12T23:02:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170947.55 epoch sec; seqNo:208; 4056065.12 north; 590336.60 east 2021-02-12T23:02:27Z- AHRS_M2 msg: 1613170947.81 epoch sec; seqNo:208 2021-02-12T23:02:27Z- AHRS_M2 msg: 5.72 phi; -0.12 theta; -0.08 psi 2021-02-12T23:02:27Z- handleDepth()-Depth_Keller msg: 1613170947814 epoch sec; seqNo:209; depth 0.06 2021-02-12T23:02:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170947.83 epoch sec; seqNo:209; 4056065.13 north; 590337.82 east 2021-02-12T23:02:28Z- AHRS_M2 msg: 1613170948.21 epoch sec; seqNo:209 2021-02-12T23:02:28Z- AHRS_M2 msg: 5.72 phi; -0.14 theta; -0.12 psi 2021-02-12T23:02:28Z- handleDepth()-Depth_Keller msg: 1613170948217 epoch sec; seqNo:210; depth 0.05 2021-02-12T23:02:28Z- handleDvl() - RDI_Pathfinder msg: 1613170948.24 epoch sec; seqNo:94 2021-02-12T23:02:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.45 , 97.45 , 0.00 , 108.28 2021-02-12T23:02:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.37 , -0.33 , -0.01 2021-02-12T23:02:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170948.26 epoch sec; seqNo:210; 4056065.12 north; 590336.60 east 2021-02-12T23:02:28Z- AHRS_M2 msg: 1613170948.62 epoch sec; seqNo:210 2021-02-12T23:02:28Z- AHRS_M2 msg: 5.77 phi; -0.18 theta; -0.05 psi 2021-02-12T23:02:28Z- handleDepth()-Depth_Keller msg: 1613170948621 epoch sec; seqNo:211; depth 0.07 2021-02-12T23:02:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170948.64 epoch sec; seqNo:211; 4056065.12 north; 590336.60 east 2021-02-12T23:02:29Z- AHRS_M2 msg: 1613170949.02 epoch sec; seqNo:211 2021-02-12T23:02:29Z- AHRS_M2 msg: 5.78 phi; -0.14 theta; 0.07 psi 2021-02-12T23:02:29Z- handleDepth()-Depth_Keller msg: 1613170949029 epoch sec; seqNo:212; depth 0.13 2021-02-12T23:02:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170949.04 epoch sec; seqNo:212; 4056065.50 north; 590336.60 east 2021-02-12T23:02:29Z- LcmTrn::handleMessages() - handling 44 messages took 0.89 ms and 126 calls... 2021-02-12T23:02:29Z- waiting for 1613170946.78, time is 1613170949.04 2021-02-12T23:02:29Z- LcmTrn::updateTrn() >>>> pose time 1613170949.04, meas time 1613170948.24 <<<< 2021-02-12T23:02:29Z- TerrainNav::Measurement type 1 from time = 1613170948.24 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:02:29Z- TerrainNav::motionUpdate() - currEstimate = (4056065, 590336.60, 0.13) 2021-02-12T23:02:29Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:02:29Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:29Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:02:29Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:02:29Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:02:29Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:02:29Z- AHRS_M2 msg: 1613170949.42 epoch sec; seqNo:212 2021-02-12T23:02:29Z- AHRS_M2 msg: 5.74 phi; -0.04 theta; 0.10 psi 2021-02-12T23:02:29Z- handleDepth()-Depth_Keller msg: 1613170949428 epoch sec; seqNo:213; depth 0.15 2021-02-12T23:02:29Z- handleDvl() - RDI_Pathfinder msg: 1613170949.46 epoch sec; seqNo:95 2021-02-12T23:02:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.57 , 95.63 , 84.80 , 107.81 2021-02-12T23:02:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.00 , 0.16 , -0.06 2021-02-12T23:02:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170949.47 epoch sec; seqNo:213; 4056065.50 north; 590336.60 east 2021-02-12T23:02:29Z- AHRS_M2 msg: 1613170949.83 epoch sec; seqNo:213 2021-02-12T23:02:29Z- AHRS_M2 msg: 5.69 phi; 0.01 theta; -0.03 psi 2021-02-12T23:02:29Z- handleDepth()-Depth_Keller msg: 1613170949833 epoch sec; seqNo:214; depth 0.11 2021-02-12T23:02:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170949.85 epoch sec; seqNo:214; 4056065.50 north; 590336.60 east 2021-02-12T23:02:30Z- AHRS_M2 msg: 1613170950.23 epoch sec; seqNo:214 2021-02-12T23:02:30Z- AHRS_M2 msg: 5.70 phi; -0.04 theta; -0.14 psi 2021-02-12T23:02:30Z- handleDepth()-Depth_Keller msg: 1613170950238 epoch sec; seqNo:215; depth 0.06 2021-02-12T23:02:30Z- handleDvl() - RDI_Pathfinder msg: 1613170950.25 epoch sec; seqNo:96 2021-02-12T23:02:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.81 , 90.69 , 97.45 , 97.45 2021-02-12T23:02:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.08 , -0.47 , 0.32 2021-02-12T23:02:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170950.27 epoch sec; seqNo:215; 4056065.50 north; 590336.60 east 2021-02-12T23:02:30Z- AHRS_M2 msg: 1613170950.64 epoch sec; seqNo:215 2021-02-12T23:02:30Z- AHRS_M2 msg: 5.77 phi; -0.12 theta; -0.08 psi 2021-02-12T23:02:30Z- handleDepth()-Depth_Keller msg: 1613170950640 epoch sec; seqNo:216; depth 0.06 2021-02-12T23:02:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170950.66 epoch sec; seqNo:216; 4056065.12 north; 590336.60 east 2021-02-12T23:02:31Z- AHRS_M2 msg: 1613170951.04 epoch sec; seqNo:216 2021-02-12T23:02:31Z- AHRS_M2 msg: 5.78 phi; -0.11 theta; 0.08 psi 2021-02-12T23:02:31Z- handleDepth()-Depth_Keller msg: 1613170951044 epoch sec; seqNo:217; depth 0.06 2021-02-12T23:02:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170951.05 epoch sec; seqNo:217; 4056065.12 north; 590336.60 east 2021-02-12T23:02:31Z- AHRS_M2 msg: 1613170951.44 epoch sec; seqNo:217 2021-02-12T23:02:31Z- AHRS_M2 msg: 5.73 phi; -0.15 theta; 0.16 psi 2021-02-12T23:02:31Z- handleDepth()-Depth_Keller msg: 1613170951450 epoch sec; seqNo:218; depth 0.03 2021-02-12T23:02:31Z- handleDvl() - RDI_Pathfinder msg: 1613170951.47 epoch sec; seqNo:97 2021-02-12T23:02:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.62 , 102.80 , 87.91 , 104.15 2021-02-12T23:02:31Z- handleDvl() - RDI_Pathfinder msg: velocities -0.75 , 0.12 , 0.33 2021-02-12T23:02:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170951.48 epoch sec; seqNo:218; 4056064.74 north; 590336.61 east 2021-02-12T23:02:31Z- AHRS_M2 msg: 1613170951.85 epoch sec; seqNo:218 2021-02-12T23:02:31Z- AHRS_M2 msg: 5.66 phi; -0.20 theta; 0.05 psi 2021-02-12T23:02:31Z- handleDepth()-Depth_Keller msg: 1613170951852 epoch sec; seqNo:219; depth 0.03 2021-02-12T23:02:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170951.87 epoch sec; seqNo:219; 4056064.74 north; 590336.61 east 2021-02-12T23:02:32Z- AHRS_M2 msg: 1613170952.27 epoch sec; seqNo:219 2021-02-12T23:02:32Z- AHRS_M2 msg: 5.65 phi; -0.12 theta; -0.18 psi 2021-02-12T23:02:32Z- handleDepth()-Depth_Keller msg: 1613170952282 epoch sec; seqNo:220; depth 0.07 2021-02-12T23:02:32Z- handleDvl() - RDI_Pathfinder msg: 1613170952.32 epoch sec; seqNo:98 2021-02-12T23:02:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 106.86 , 89.27 , 86.56 , 109.57 2021-02-12T23:02:32Z- handleDvl() - RDI_Pathfinder msg: velocities -0.59 , 0.21 , -0.76 2021-02-12T23:02:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170952.35 epoch sec; seqNo:220; 4056064.36 north; 590336.61 east 2021-02-12T23:02:32Z- AHRS_M2 msg: 1613170952.66 epoch sec; seqNo:220 2021-02-12T23:02:32Z- AHRS_M2 msg: 5.68 phi; -0.02 theta; -0.26 psi 2021-02-12T23:02:32Z- handleDepth()-Depth_Keller msg: 1613170952664 epoch sec; seqNo:221; depth 0.03 2021-02-12T23:02:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170952.68 epoch sec; seqNo:221; 4056064.36 north; 590336.61 east 2021-02-12T23:02:33Z- AHRS_M2 msg: 1613170953.08 epoch sec; seqNo:221 2021-02-12T23:02:33Z- AHRS_M2 msg: 5.68 phi; 0.06 theta; -0.12 psi 2021-02-12T23:02:33Z- handleDepth()-Depth_Keller msg: 1613170953098 epoch sec; seqNo:222; depth 0.07 2021-02-12T23:02:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170953.15 epoch sec; seqNo:222; 4056063.98 north; 590336.61 east 2021-02-12T23:02:33Z- AHRS_M2 msg: 1613170953.47 epoch sec; seqNo:222 2021-02-12T23:02:33Z- AHRS_M2 msg: 5.68 phi; 0.10 theta; 0.12 psi 2021-02-12T23:02:33Z- handleDepth()-Depth_Keller msg: 1613170953493 epoch sec; seqNo:223; depth 0.10 2021-02-12T23:02:33Z- handleDvl() - RDI_Pathfinder msg: 1613170953.52 epoch sec; seqNo:99 2021-02-12T23:02:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 113.42 , 97.18 , 99.89 2021-02-12T23:02:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.44 , -0.45 2021-02-12T23:02:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170953.56 epoch sec; seqNo:223; 4056064.36 north; 590336.61 east 2021-02-12T23:02:33Z- AHRS_M2 msg: 1613170953.88 epoch sec; seqNo:223 2021-02-12T23:02:33Z- AHRS_M2 msg: 5.68 phi; 0.03 theta; 0.23 psi 2021-02-12T23:02:33Z- handleDepth()-Depth_Keller msg: 1613170953893 epoch sec; seqNo:224; depth 0.13 2021-02-12T23:02:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170953.93 epoch sec; seqNo:224; 4056064.36 north; 590336.61 east 2021-02-12T23:02:34Z- LcmTrn::handleMessages() - handling 41 messages took 0.68 ms and 128 calls... 2021-02-12T23:02:34Z- waiting for 1613170952.04, time is 1613170953.93 2021-02-12T23:02:34Z- LcmTrn::updateTrn() >>>> pose time 1613170953.93, meas time 1613170953.52 <<<< 2021-02-12T23:02:34Z- TerrainNav::Measurement type 1 from time = 1613170953.52 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:02:34Z- TerrainNav::motionUpdate() - currEstimate = (4056064, 590336.61, 0.13) 2021-02-12T23:02:34Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:02:34Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:34Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:02:34Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:02:34Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:02:34Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:02:34Z- AHRS_M2 msg: 1613170954.29 epoch sec; seqNo:224 2021-02-12T23:02:34Z- AHRS_M2 msg: 5.65 phi; -0.06 theta; 0.17 psi 2021-02-12T23:02:34Z- handleDepth()-Depth_Keller msg: 1613170954301 epoch sec; seqNo:225; depth 0.11 2021-02-12T23:02:34Z- handleDvl() - RDI_Pathfinder msg: 1613170954.34 epoch sec; seqNo:100 2021-02-12T23:02:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 109.29 , 0.00 , 98.47 , 97.11 2021-02-12T23:02:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.23 , -0.14 , 0.43 2021-02-12T23:02:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170954.38 epoch sec; seqNo:225; 4056064.36 north; 590336.61 east 2021-02-12T23:02:34Z- AHRS_M2 msg: 1613170954.70 epoch sec; seqNo:225 2021-02-12T23:02:34Z- AHRS_M2 msg: 5.67 phi; -0.13 theta; 0.04 psi 2021-02-12T23:02:34Z- handleDepth()-Depth_Keller msg: 1613170954705 epoch sec; seqNo:226; depth 0.09 2021-02-12T23:02:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170954.78 epoch sec; seqNo:226; 4056064.36 north; 590336.61 east 2021-02-12T23:02:35Z- AHRS_M2 msg: 1613170955.12 epoch sec; seqNo:226 2021-02-12T23:02:35Z- AHRS_M2 msg: 5.66 phi; -0.18 theta; -0.08 psi 2021-02-12T23:02:35Z- handleDepth()-Depth_Keller msg: 1613170955129 epoch sec; seqNo:227; depth 0.08 2021-02-12T23:02:35Z- handleDvl() - RDI_Pathfinder msg: 1613170955.18 epoch sec; seqNo:101 2021-02-12T23:02:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.48 , 98.00 , 98.00 , 100.97 2021-02-12T23:02:35Z- handleDvl() - RDI_Pathfinder msg: velocities -0.40 , 0.25 , -0.02 2021-02-12T23:02:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170955.23 epoch sec; seqNo:227; 4056064.36 north; 590336.61 east 2021-02-12T23:02:35Z- AHRS_M2 msg: 1613170955.51 epoch sec; seqNo:227 2021-02-12T23:02:35Z- AHRS_M2 msg: 5.60 phi; -0.18 theta; -0.16 psi 2021-02-12T23:02:35Z- handleDepth()-Depth_Keller msg: 1613170955521 epoch sec; seqNo:228; depth 0.08 2021-02-12T23:02:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170955.56 epoch sec; seqNo:228; 4056064.36 north; 590336.61 east 2021-02-12T23:02:35Z- AHRS_M2 msg: 1613170955.92 epoch sec; seqNo:228 2021-02-12T23:02:35Z- AHRS_M2 msg: 5.61 phi; -0.15 theta; -0.18 psi 2021-02-12T23:02:35Z- handleDepth()-Depth_Keller msg: 1613170955941 epoch sec; seqNo:229; depth 0.05 2021-02-12T23:02:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170955.97 epoch sec; seqNo:229; 4056064.36 north; 590336.61 east 2021-02-12T23:02:36Z- AHRS_M2 msg: 1613170956.33 epoch sec; seqNo:229 2021-02-12T23:02:36Z- AHRS_M2 msg: 5.62 phi; -0.18 theta; -0.06 psi 2021-02-12T23:02:36Z- handleDepth()-Depth_Keller msg: 1613170956341 epoch sec; seqNo:230; depth 0.06 2021-02-12T23:02:36Z- handleDvl() - RDI_Pathfinder msg: 1613170956.39 epoch sec; seqNo:102 2021-02-12T23:02:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 109.29 , 86.29 , 110.65 2021-02-12T23:02:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.06 , -0.51 , -0.43 2021-02-12T23:02:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170956.44 epoch sec; seqNo:230; 4056063.98 north; 590336.61 east 2021-02-12T23:02:36Z- AHRS_M2 msg: 1613170956.74 epoch sec; seqNo:230 2021-02-12T23:02:36Z- AHRS_M2 msg: 5.60 phi; -0.17 theta; 0.09 psi 2021-02-12T23:02:36Z- handleDepth()-Depth_Keller msg: 1613170956749 epoch sec; seqNo:231; depth 0.08 2021-02-12T23:02:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170956.79 epoch sec; seqNo:231; 4056063.98 north; 590336.61 east 2021-02-12T23:02:37Z- AHRS_M2 msg: 1613170957.15 epoch sec; seqNo:231 2021-02-12T23:02:37Z- AHRS_M2 msg: 5.61 phi; -0.06 theta; 0.12 psi 2021-02-12T23:02:37Z- handleDepth()-Depth_Keller msg: 1613170957177 epoch sec; seqNo:232; depth 0.07 2021-02-12T23:02:37Z- handleDvl() - RDI_Pathfinder msg: 1613170957.21 epoch sec; seqNo:103 2021-02-12T23:02:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.83 , 91.77 , 0.00 , 108.01 2021-02-12T23:02:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.09 , -0.42 , 0.06 2021-02-12T23:02:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170957.25 epoch sec; seqNo:232; 4056063.98 north; 590336.61 east 2021-02-12T23:02:37Z- AHRS_M2 msg: 1613170957.56 epoch sec; seqNo:232 2021-02-12T23:02:37Z- AHRS_M2 msg: 5.57 phi; -0.08 theta; 0.09 psi 2021-02-12T23:02:37Z- handleDepth()-Depth_Keller msg: 1613170957565 epoch sec; seqNo:233; depth 0.09 2021-02-12T23:02:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170957.60 epoch sec; seqNo:233; 4056063.98 north; 590336.61 east 2021-02-12T23:02:37Z- AHRS_M2 msg: 1613170957.96 epoch sec; seqNo:233 2021-02-12T23:02:37Z- AHRS_M2 msg: 5.51 phi; -0.10 theta; 0.02 psi 2021-02-12T23:02:38Z- handleDepth()-Depth_Keller msg: 1613170958005 epoch sec; seqNo:234; depth 0.11 2021-02-12T23:02:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170958.04 epoch sec; seqNo:234; 4056063.98 north; 590336.61 east 2021-02-12T23:02:38Z- AHRS_M2 msg: 1613170958.37 epoch sec; seqNo:234 2021-02-12T23:02:38Z- AHRS_M2 msg: 5.51 phi; -0.17 theta; -0.13 psi 2021-02-12T23:02:38Z- handleDepth()-Depth_Keller msg: 1613170958381 epoch sec; seqNo:235; depth 0.10 2021-02-12T23:02:38Z- handleDvl() - RDI_Pathfinder msg: 1613170958.44 epoch sec; seqNo:104 2021-02-12T23:02:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.95 , 89.06 , 97.18 , 95.83 2021-02-12T23:02:38Z- handleDvl() - RDI_Pathfinder msg: velocities -0.37 , -0.14 , 0.46 2021-02-12T23:02:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170958.46 epoch sec; seqNo:235; 4056063.60 north; 590336.62 east 2021-02-12T23:02:38Z- AHRS_M2 msg: 1613170958.78 epoch sec; seqNo:235 2021-02-12T23:02:38Z- AHRS_M2 msg: 5.52 phi; -0.29 theta; -0.19 psi 2021-02-12T23:02:38Z- handleDepth()-Depth_Keller msg: 1613170958789 epoch sec; seqNo:236; depth 0.05 2021-02-12T23:02:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170958.83 epoch sec; seqNo:236; 4056063.60 north; 590336.62 east 2021-02-12T23:02:39Z- AHRS_M2 msg: 1613170959.18 epoch sec; seqNo:236 2021-02-12T23:02:39Z- AHRS_M2 msg: 5.54 phi; -0.30 theta; -0.10 psi 2021-02-12T23:02:39Z- LcmTrn::handleMessages() - handling 42 messages took 0.83 ms and 126 calls... 2021-02-12T23:02:39Z- waiting for 1613170956.93, time is 1613170959.18 2021-02-12T23:02:39Z- LcmTrn::updateTrn() >>>> pose time 1613170959.18, meas time 1613170958.44 <<<< 2021-02-12T23:02:39Z- TerrainNav::Measurement type 1 from time = 1613170958.44 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:02:39Z- TerrainNav::motionUpdate() - currEstimate = (4056064, 590336.62, 0.05) 2021-02-12T23:02:39Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:02:39Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:39Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:02:39Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:02:39Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:02:39Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:02:39Z- handleDepth()-Depth_Keller msg: 1613170959197 epoch sec; seqNo:237; depth 0.08 2021-02-12T23:02:39Z- handleDvl() - RDI_Pathfinder msg: 1613170959.22 epoch sec; seqNo:105 2021-02-12T23:02:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.32 , 104.08 , 0.00 , 106.79 2021-02-12T23:02:39Z- handleDvl() - RDI_Pathfinder msg: velocities -0.75 , -0.49 , -0.24 2021-02-12T23:02:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170959.27 epoch sec; seqNo:237; 4056063.22 north; 590336.62 east 2021-02-12T23:02:39Z- AHRS_M2 msg: 1613170959.59 epoch sec; seqNo:237 2021-02-12T23:02:39Z- AHRS_M2 msg: 5.58 phi; -0.16 theta; 0.06 psi 2021-02-12T23:02:39Z- handleDepth()-Depth_Keller msg: 1613170959614 epoch sec; seqNo:238; depth 0.08 2021-02-12T23:02:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170959.64 epoch sec; seqNo:238; 4056062.85 north; 590336.63 east 2021-02-12T23:02:40Z- AHRS_M2 msg: 1613170960.04 epoch sec; seqNo:238 2021-02-12T23:02:40Z- AHRS_M2 msg: 5.55 phi; -0.02 theta; 0.17 psi 2021-02-12T23:02:40Z- handleDepth()-Depth_Keller msg: 1613170960048 epoch sec; seqNo:239; depth 0.06 2021-02-12T23:02:40Z- handleDvl() - RDI_Pathfinder msg: 1613170960.10 epoch sec; seqNo:106 2021-02-12T23:02:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.93 , 97.93 , 0.00 , 108.35 2021-02-12T23:02:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , 0.17 , -0.15 2021-02-12T23:02:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170960.14 epoch sec; seqNo:239; 4056063.22 north; 590336.62 east 2021-02-12T23:02:40Z- AHRS_M2 msg: 1613170960.41 epoch sec; seqNo:239 2021-02-12T23:02:40Z- AHRS_M2 msg: 5.50 phi; 0.03 theta; 0.06 psi 2021-02-12T23:02:40Z- handleDepth()-Depth_Keller msg: 1613170960435 epoch sec; seqNo:240; depth 0.06 2021-02-12T23:02:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170960.47 epoch sec; seqNo:240; 4056063.22 north; 590336.62 east 2021-02-12T23:02:40Z- AHRS_M2 msg: 1613170960.82 epoch sec; seqNo:240 2021-02-12T23:02:40Z- AHRS_M2 msg: 5.50 phi; 0.01 theta; -0.14 psi 2021-02-12T23:02:40Z- handleDepth()-Depth_Keller msg: 1613170960829 epoch sec; seqNo:241; depth 0.03 2021-02-12T23:02:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170960.88 epoch sec; seqNo:241; 4056063.59 north; 590335.40 east 2021-02-12T23:02:41Z- AHRS_M2 msg: 1613170961.22 epoch sec; seqNo:241 2021-02-12T23:02:41Z- AHRS_M2 msg: 5.51 phi; -0.03 theta; -0.25 psi 2021-02-12T23:02:41Z- handleDepth()-Depth_Keller msg: 1613170961221 epoch sec; seqNo:242; depth 0.03 2021-02-12T23:02:41Z- handleDvl() - RDI_Pathfinder msg: 1613170961.25 epoch sec; seqNo:107 2021-02-12T23:02:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.00 , 98.00 , 98.00 , 98.00 2021-02-12T23:02:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.10 , -0.07 , 0.64 2021-02-12T23:02:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170961.26 epoch sec; seqNo:242; 4056063.59 north; 590335.40 east 2021-02-12T23:02:41Z- AHRS_M2 msg: 1613170961.62 epoch sec; seqNo:242 2021-02-12T23:02:41Z- AHRS_M2 msg: 5.53 phi; -0.04 theta; -0.21 psi 2021-02-12T23:02:41Z- handleDepth()-Depth_Keller msg: 1613170961624 epoch sec; seqNo:243; depth 0.02 2021-02-12T23:02:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170961.64 epoch sec; seqNo:243; 4056063.59 north; 590335.40 east 2021-02-12T23:02:42Z- AHRS_M2 msg: 1613170962.03 epoch sec; seqNo:243 2021-02-12T23:02:42Z- AHRS_M2 msg: 5.54 phi; -0.03 theta; -0.03 psi 2021-02-12T23:02:42Z- handleDepth()-Depth_Keller msg: 1613170962030 epoch sec; seqNo:244; depth 0.02 2021-02-12T23:02:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170962.04 epoch sec; seqNo:244; 4056063.59 north; 590335.40 east 2021-02-12T23:02:42Z- AHRS_M2 msg: 1613170962.43 epoch sec; seqNo:244 2021-02-12T23:02:42Z- AHRS_M2 msg: 5.56 phi; 0.01 theta; 0.13 psi 2021-02-12T23:02:42Z- handleDepth()-Depth_Keller msg: 1613170962475 epoch sec; seqNo:245; depth 0.06 2021-02-12T23:02:42Z- handleDvl() - RDI_Pathfinder msg: 1613170962.51 epoch sec; seqNo:108 2021-02-12T23:02:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.42 , 112.14 , 97.25 , 101.31 2021-02-12T23:02:42Z- handleDvl() - RDI_Pathfinder msg: velocities -0.37 , -0.21 , 0.21 2021-02-12T23:02:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170962.55 epoch sec; seqNo:245; 4056063.59 north; 590335.40 east 2021-02-12T23:02:42Z- AHRS_M2 msg: 1613170962.83 epoch sec; seqNo:245 2021-02-12T23:02:42Z- AHRS_M2 msg: 5.62 phi; 0.06 theta; 0.13 psi 2021-02-12T23:02:42Z- handleDepth()-Depth_Keller msg: 1613170962876 epoch sec; seqNo:246; depth 0.03 2021-02-12T23:02:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170962.99 epoch sec; seqNo:246; 4056063.21 north; 590335.40 east 2021-02-12T23:02:43Z- AHRS_M2 msg: 1613170963.39 epoch sec; seqNo:246 2021-02-12T23:02:43Z- AHRS_M2 msg: 5.63 phi; 0.02 theta; 0.05 psi 2021-02-12T23:02:43Z- handleDepth()-Depth_Keller msg: 1613170963417 epoch sec; seqNo:247; depth 0.04 2021-02-12T23:02:43Z- handleDvl() - RDI_Pathfinder msg: 1613170963.53 epoch sec; seqNo:109 2021-02-12T23:02:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 105.24 , 89.00 , 97.11 , 94.41 2021-02-12T23:02:43Z- handleDvl() - RDI_Pathfinder msg: velocities -0.11 , 0.13 , -0.03 2021-02-12T23:02:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170963.68 epoch sec; seqNo:247; 4056063.22 north; 590336.62 east 2021-02-12T23:02:43Z- AHRS_M2 msg: 1613170963.82 epoch sec; seqNo:247 2021-02-12T23:02:43Z- AHRS_M2 msg: 5.55 phi; -0.05 theta; -0.08 psi 2021-02-12T23:02:43Z- handleDepth()-Depth_Keller msg: 1613170963829 epoch sec; seqNo:248; depth 0.02 2021-02-12T23:02:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170963.88 epoch sec; seqNo:248; 4056063.22 north; 590336.62 east 2021-02-12T23:02:44Z- LcmTrn::handleMessages() - handling 40 messages took 0.88 ms and 123 calls... 2021-02-12T23:02:44Z- waiting for 1613170962.18, time is 1613170963.88 2021-02-12T23:02:44Z- LcmTrn::updateTrn() >>>> pose time 1613170963.88, meas time 1613170963.53 <<<< 2021-02-12T23:02:44Z- TerrainNav::Measurement type 1 from time = 1613170963.53 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:02:44Z- TerrainNav::motionUpdate() - currEstimate = (4056063, 590336.62, 0.02) 2021-02-12T23:02:44Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:02:44Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:44Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:02:44Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:02:44Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:02:44Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:02:44Z- AHRS_M2 msg: 1613170964.23 epoch sec; seqNo:248 2021-02-12T23:02:44Z- AHRS_M2 msg: 5.51 phi; -0.07 theta; -0.16 psi 2021-02-12T23:02:44Z- handleDepth()-Depth_Keller msg: 1613170964246 epoch sec; seqNo:249; depth 0.02 2021-02-12T23:02:44Z- handleDvl() - RDI_Pathfinder msg: 1613170964.30 epoch sec; seqNo:110 2021-02-12T23:02:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.39 , 97.39 , 97.39 , 97.39 2021-02-12T23:02:44Z- handleDvl() - RDI_Pathfinder msg: velocities -0.36 , -0.42 , -0.52 2021-02-12T23:02:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170964.34 epoch sec; seqNo:249; 4056062.85 north; 590336.63 east 2021-02-12T23:02:44Z- AHRS_M2 msg: 1613170964.63 epoch sec; seqNo:249 2021-02-12T23:02:44Z- AHRS_M2 msg: 5.55 phi; -0.06 theta; -0.15 psi 2021-02-12T23:02:44Z- handleDepth()-Depth_Keller msg: 1613170964657 epoch sec; seqNo:250; depth 0.01 2021-02-12T23:02:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170964.68 epoch sec; seqNo:250; 4056062.83 north; 590335.40 east 2021-02-12T23:02:45Z- AHRS_M2 msg: 1613170965.04 epoch sec; seqNo:250 2021-02-12T23:02:45Z- AHRS_M2 msg: 5.61 phi; -0.03 theta; 0.09 psi 2021-02-12T23:02:45Z- handleDepth()-Depth_Keller msg: 1613170965065 epoch sec; seqNo:251; depth 0.04 2021-02-12T23:02:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170965.13 epoch sec; seqNo:251; 4056062.45 north; 590335.41 east 2021-02-12T23:02:45Z- AHRS_M2 msg: 1613170965.44 epoch sec; seqNo:251 2021-02-12T23:02:45Z- AHRS_M2 msg: 5.62 phi; 0.01 theta; 0.15 psi 2021-02-12T23:02:45Z- handleDepth()-Depth_Keller msg: 1613170965449 epoch sec; seqNo:252; depth 0.05 2021-02-12T23:02:45Z- handleDvl() - RDI_Pathfinder msg: 1613170965.51 epoch sec; seqNo:111 2021-02-12T23:02:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.13 , 101.31 , 89.13 , 99.96 2021-02-12T23:02:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.25 , -0.20 , -0.31 2021-02-12T23:02:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170965.54 epoch sec; seqNo:252; 4056062.45 north; 590335.41 east 2021-02-12T23:02:45Z- AHRS_M2 msg: 1613170965.84 epoch sec; seqNo:252 2021-02-12T23:02:45Z- AHRS_M2 msg: 5.62 phi; 0.06 theta; 0.14 psi 2021-02-12T23:02:45Z- handleDepth()-Depth_Keller msg: 1613170965849 epoch sec; seqNo:253; depth 0.06 2021-02-12T23:02:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170965.87 epoch sec; seqNo:253; 4056062.45 north; 590335.41 east 2021-02-12T23:02:46Z- AHRS_M2 msg: 1613170966.24 epoch sec; seqNo:253 2021-02-12T23:02:46Z- AHRS_M2 msg: 5.61 phi; 0.03 theta; -0.03 psi 2021-02-12T23:02:46Z- handleDepth()-Depth_Keller msg: 1613170966249 epoch sec; seqNo:254; depth 0.08 2021-02-12T23:02:46Z- handleDvl() - RDI_Pathfinder msg: 1613170966.28 epoch sec; seqNo:112 2021-02-12T23:02:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 105.64 , 88.05 , 93.46 , 97.52 2021-02-12T23:02:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.42 , 0.08 , 0.50 2021-02-12T23:02:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170966.32 epoch sec; seqNo:254; 4056062.83 north; 590335.40 east 2021-02-12T23:02:46Z- AHRS_M2 msg: 1613170966.65 epoch sec; seqNo:254 2021-02-12T23:02:46Z- AHRS_M2 msg: 5.62 phi; -0.04 theta; -0.20 psi 2021-02-12T23:02:46Z- handleDepth()-Depth_Keller msg: 1613170966653 epoch sec; seqNo:255; depth 0.02 2021-02-12T23:02:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170966.67 epoch sec; seqNo:255; 4056062.83 north; 590335.40 east 2021-02-12T23:02:47Z- AHRS_M2 msg: 1613170967.06 epoch sec; seqNo:255 2021-02-12T23:02:47Z- AHRS_M2 msg: 5.64 phi; -0.11 theta; -0.25 psi 2021-02-12T23:02:47Z- handleDepth()-Depth_Keller msg: 1613170967065 epoch sec; seqNo:256; depth 0.01 2021-02-12T23:02:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170967.08 epoch sec; seqNo:256; 4056063.21 north; 590335.40 east 2021-02-12T23:02:47Z- AHRS_M2 msg: 1613170967.46 epoch sec; seqNo:256 2021-02-12T23:02:47Z- AHRS_M2 msg: 5.67 phi; -0.13 theta; -0.20 psi 2021-02-12T23:02:47Z- handleDepth()-Depth_Keller msg: 1613170967502 epoch sec; seqNo:257; depth 0.00 2021-02-12T23:02:47Z- handleDvl() - RDI_Pathfinder msg: 1613170967.54 epoch sec; seqNo:113 2021-02-12T23:02:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.39 , 100.09 , 97.39 , 97.39 2021-02-12T23:02:47Z- handleDvl() - RDI_Pathfinder msg: velocities -0.68 , -0.02 , 0.34 2021-02-12T23:02:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170967.57 epoch sec; seqNo:257; 4056062.83 north; 590335.40 east 2021-02-12T23:02:47Z- AHRS_M2 msg: 1613170967.86 epoch sec; seqNo:257 2021-02-12T23:02:47Z- AHRS_M2 msg: 5.66 phi; -0.09 theta; -0.06 psi 2021-02-12T23:02:47Z- handleDepth()-Depth_Keller msg: 1613170967865 epoch sec; seqNo:258; depth 0.00 2021-02-12T23:02:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170967.90 epoch sec; seqNo:258; 4056062.83 north; 590335.40 east 2021-02-12T23:02:48Z- AHRS_M2 msg: 1613170968.26 epoch sec; seqNo:258 2021-02-12T23:02:48Z- AHRS_M2 msg: 5.67 phi; -0.04 theta; 0.11 psi 2021-02-12T23:02:48Z- handleDepth()-Depth_Keller msg: 1613170968270 epoch sec; seqNo:259; depth 0.02 2021-02-12T23:02:48Z- handleDvl() - RDI_Pathfinder msg: 1613170968.29 epoch sec; seqNo:114 2021-02-12T23:02:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.71 , 110.72 , 87.71 , 108.01 2021-02-12T23:02:48Z- handleDvl() - RDI_Pathfinder msg: velocities -0.36 , -0.54 , -0.57 2021-02-12T23:02:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170968.30 epoch sec; seqNo:259; 4056054.97 north; 590344.04 east 2021-02-12T23:02:48Z- AHRS_M2 msg: 1613170968.67 epoch sec; seqNo:259 2021-02-12T23:02:48Z- AHRS_M2 msg: 5.70 phi; -0.05 theta; 0.19 psi 2021-02-12T23:02:48Z- handleDepth()-Depth_Keller msg: 1613170968674 epoch sec; seqNo:260; depth 0.06 2021-02-12T23:02:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170968.70 epoch sec; seqNo:260; 4056054.97 north; 590344.04 east 2021-02-12T23:02:49Z- AHRS_M2 msg: 1613170969.07 epoch sec; seqNo:260 2021-02-12T23:02:49Z- AHRS_M2 msg: 5.65 phi; 0.02 theta; 0.15 psi 2021-02-12T23:02:49Z- handleDepth()-Depth_Keller msg: 1613170969081 epoch sec; seqNo:261; depth 0.10 2021-02-12T23:02:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170969.15 epoch sec; seqNo:261; 4056054.59 north; 590344.04 east 2021-02-12T23:02:49Z- LcmTrn::handleMessages() - handling 44 messages took 0.91 ms and 126 calls... 2021-02-12T23:02:49Z- waiting for 1613170966.88, time is 1613170969.15 2021-02-12T23:02:49Z- LcmTrn::updateTrn() >>>> pose time 1613170969.15, meas time 1613170968.29 <<<< 2021-02-12T23:02:49Z- TerrainNav::Measurement type 1 from time = 1613170968.29 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:02:49Z- TerrainNav::motionUpdate() - currEstimate = (4056055, 590344.04, 0.10) 2021-02-12T23:02:49Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:02:49Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:49Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:02:49Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:02:49Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:02:49Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:02:49Z- AHRS_M2 msg: 1613170969.48 epoch sec; seqNo:261 2021-02-12T23:02:49Z- AHRS_M2 msg: 5.62 phi; 0.08 theta; -0.00 psi 2021-02-12T23:02:49Z- handleDepth()-Depth_Keller msg: 1613170969481 epoch sec; seqNo:262; depth 0.12 2021-02-12T23:02:49Z- handleDvl() - RDI_Pathfinder msg: 1613170969.49 epoch sec; seqNo:115 2021-02-12T23:02:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 106.66 , 87.71 , 97.18 , 99.89 2021-02-12T23:02:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.16 , 0.09 , -0.44 2021-02-12T23:02:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170969.50 epoch sec; seqNo:262; 4056054.59 north; 590344.04 east 2021-02-12T23:02:49Z- AHRS_M2 msg: 1613170969.88 epoch sec; seqNo:262 2021-02-12T23:02:49Z- AHRS_M2 msg: 5.64 phi; 0.07 theta; -0.14 psi 2021-02-12T23:02:49Z- handleDepth()-Depth_Keller msg: 1613170969889 epoch sec; seqNo:263; depth 0.08 2021-02-12T23:02:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170969.91 epoch sec; seqNo:263; 4056054.59 north; 590344.04 east 2021-02-12T23:02:50Z- AHRS_M2 msg: 1613170970.28 epoch sec; seqNo:263 2021-02-12T23:02:50Z- AHRS_M2 msg: 5.66 phi; 0.04 theta; -0.20 psi 2021-02-12T23:02:50Z- handleDepth()-Depth_Keller msg: 1613170970289 epoch sec; seqNo:264; depth 0.06 2021-02-12T23:02:50Z- handleDvl() - RDI_Pathfinder msg: 1613170970.33 epoch sec; seqNo:116 2021-02-12T23:02:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.90 , 97.25 , 97.25 , 97.25 2021-02-12T23:02:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.40 , -0.28 , 0.13 2021-02-12T23:02:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170970.34 epoch sec; seqNo:264; 4056054.59 north; 590344.04 east 2021-02-12T23:02:50Z- AHRS_M2 msg: 1613170970.69 epoch sec; seqNo:264 2021-02-12T23:02:50Z- AHRS_M2 msg: 5.62 phi; 0.04 theta; -0.11 psi 2021-02-12T23:02:50Z- handleDepth()-Depth_Keller msg: 1613170970693 epoch sec; seqNo:265; depth 0.05 2021-02-12T23:02:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170970.73 epoch sec; seqNo:265; 4056054.97 north; 590344.04 east 2021-02-12T23:02:51Z- AHRS_M2 msg: 1613170971.12 epoch sec; seqNo:265 2021-02-12T23:02:51Z- AHRS_M2 msg: 5.59 phi; 0.04 theta; 0.05 psi 2021-02-12T23:02:51Z- handleDepth()-Depth_Keller msg: 1613170971145 epoch sec; seqNo:266; depth 0.06 2021-02-12T23:02:51Z- handleDvl() - RDI_Pathfinder msg: 1613170971.17 epoch sec; seqNo:117 2021-02-12T23:02:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.85 , 108.15 , 97.32 , 97.32 2021-02-12T23:02:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.12 , -0.14 , 0.64 2021-02-12T23:02:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170971.19 epoch sec; seqNo:266; 4056054.95 north; 590342.81 east 2021-02-12T23:02:51Z- AHRS_M2 msg: 1613170971.50 epoch sec; seqNo:266 2021-02-12T23:02:51Z- AHRS_M2 msg: 5.63 phi; 0.00 theta; 0.13 psi 2021-02-12T23:02:51Z- handleDepth()-Depth_Keller msg: 1613170971517 epoch sec; seqNo:267; depth 0.05 2021-02-12T23:02:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170971.54 epoch sec; seqNo:267; 4056054.95 north; 590342.81 east 2021-02-12T23:02:51Z- AHRS_M2 msg: 1613170971.90 epoch sec; seqNo:267 2021-02-12T23:02:51Z- AHRS_M2 msg: 5.65 phi; 0.01 theta; 0.10 psi 2021-02-12T23:02:51Z- handleDepth()-Depth_Keller msg: 1613170971905 epoch sec; seqNo:268; depth 0.05 2021-02-12T23:02:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170971.93 epoch sec; seqNo:268; 4056054.95 north; 590342.81 east 2021-02-12T23:02:52Z- AHRS_M2 msg: 1613170972.30 epoch sec; seqNo:268 2021-02-12T23:02:52Z- AHRS_M2 msg: 5.70 phi; -0.01 theta; 0.07 psi 2021-02-12T23:02:52Z- handleDepth()-Depth_Keller msg: 1613170972313 epoch sec; seqNo:269; depth 0.02 2021-02-12T23:02:52Z- handleDvl() - RDI_Pathfinder msg: 1613170972.34 epoch sec; seqNo:118 2021-02-12T23:02:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 105.37 , 87.78 , 93.19 , 94.54 2021-02-12T23:02:52Z- handleDvl() - RDI_Pathfinder msg: velocities -0.12 , 0.04 , 0.18 2021-02-12T23:02:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170972.35 epoch sec; seqNo:269; 4056054.58 north; 590342.82 east 2021-02-12T23:02:52Z- AHRS_M2 msg: 1613170972.71 epoch sec; seqNo:269 2021-02-12T23:02:52Z- AHRS_M2 msg: 5.69 phi; -0.10 theta; 0.07 psi 2021-02-12T23:02:52Z- handleDepth()-Depth_Keller msg: 1613170972717 epoch sec; seqNo:270; depth 0.04 2021-02-12T23:02:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170972.76 epoch sec; seqNo:270; 4056054.58 north; 590342.82 east 2021-02-12T23:02:53Z- AHRS_M2 msg: 1613170973.12 epoch sec; seqNo:270 2021-02-12T23:02:53Z- AHRS_M2 msg: 5.61 phi; -0.07 theta; 0.03 psi 2021-02-12T23:02:53Z- handleDepth()-Depth_Keller msg: 1613170973125 epoch sec; seqNo:271; depth 0.09 2021-02-12T23:02:53Z- handleDvl() - RDI_Pathfinder msg: 1613170973.15 epoch sec; seqNo:119 2021-02-12T23:02:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 100.16 , 92.04 , 97.45 , 96.10 2021-02-12T23:02:53Z- handleDvl() - RDI_Pathfinder msg: velocities -0.51 , 0.46 , -0.04 2021-02-12T23:02:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170973.18 epoch sec; seqNo:271; 4056054.59 north; 590344.04 east 2021-02-12T23:02:53Z- AHRS_M2 msg: 1613170973.52 epoch sec; seqNo:271 2021-02-12T23:02:53Z- AHRS_M2 msg: 5.56 phi; 0.01 theta; -0.14 psi 2021-02-12T23:02:53Z- handleDepth()-Depth_Keller msg: 1613170973530 epoch sec; seqNo:272; depth 0.06 2021-02-12T23:02:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170973.54 epoch sec; seqNo:272; 4056054.59 north; 590344.04 east 2021-02-12T23:02:53Z- AHRS_M2 msg: 1613170973.92 epoch sec; seqNo:272 2021-02-12T23:02:53Z- AHRS_M2 msg: 5.56 phi; -0.04 theta; -0.28 psi 2021-02-12T23:02:53Z- handleDepth()-Depth_Keller msg: 1613170973924 epoch sec; seqNo:273; depth 0.00 2021-02-12T23:02:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170973.93 epoch sec; seqNo:273; 4056054.59 north; 590344.04 east 2021-02-12T23:02:54Z- LcmTrn::handleMessages() - handling 41 messages took 0.72 ms and 127 calls... 2021-02-12T23:02:54Z- waiting for 1613170972.15, time is 1613170973.93 2021-02-12T23:02:54Z- LcmTrn::updateTrn() >>>> pose time 1613170973.93, meas time 1613170973.15 <<<< 2021-02-12T23:02:54Z- TerrainNav::Measurement type 1 from time = 1613170973.15 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:02:54Z- TerrainNav::motionUpdate() - currEstimate = (4056055, 590344.04, 0.00) 2021-02-12T23:02:54Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:02:54Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:54Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:02:54Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:02:54Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:02:54Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:02:54Z- AHRS_M2 msg: 1613170974.32 epoch sec; seqNo:273 2021-02-12T23:02:54Z- AHRS_M2 msg: 5.58 phi; -0.14 theta; -0.18 psi 2021-02-12T23:02:54Z- handleDepth()-Depth_Keller msg: 1613170974329 epoch sec; seqNo:274; depth 0.01 2021-02-12T23:02:54Z- handleDvl() - RDI_Pathfinder msg: 1613170974.34 epoch sec; seqNo:120 2021-02-12T23:02:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.39 , 105.51 , 97.39 , 97.39 2021-02-12T23:02:54Z- handleDvl() - RDI_Pathfinder msg: velocities -0.38 , -0.46 , 0.04 2021-02-12T23:02:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170974.35 epoch sec; seqNo:274; 4056054.21 north; 590344.04 east 2021-02-12T23:02:54Z- AHRS_M2 msg: 1613170974.73 epoch sec; seqNo:274 2021-02-12T23:02:54Z- AHRS_M2 msg: 5.60 phi; -0.20 theta; -0.02 psi 2021-02-12T23:02:54Z- handleDepth()-Depth_Keller msg: 1613170974734 epoch sec; seqNo:275; depth 0.04 2021-02-12T23:02:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170974.74 epoch sec; seqNo:275; 4056054.21 north; 590344.04 east 2021-02-12T23:02:55Z- AHRS_M2 msg: 1613170975.13 epoch sec; seqNo:275 2021-02-12T23:02:55Z- AHRS_M2 msg: 5.59 phi; -0.18 theta; 0.04 psi 2021-02-12T23:02:55Z- handleDepth()-Depth_Keller msg: 1613170975137 epoch sec; seqNo:276; depth 0.03 2021-02-12T23:02:55Z- handleDvl() - RDI_Pathfinder msg: 1613170975.16 epoch sec; seqNo:121 2021-02-12T23:02:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 101.18 , 0.00 , 106.59 2021-02-12T23:02:55Z- handleDvl() - RDI_Pathfinder msg: velocities -0.10 , -0.06 , -0.93 2021-02-12T23:02:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170975.17 epoch sec; seqNo:276; 4056054.21 north; 590344.04 east 2021-02-12T23:02:55Z- AHRS_M2 msg: 1613170975.54 epoch sec; seqNo:276 2021-02-12T23:02:55Z- AHRS_M2 msg: 5.59 phi; -0.07 theta; 0.00 psi 2021-02-12T23:02:55Z- handleDepth()-Depth_Keller msg: 1613170975540 epoch sec; seqNo:277; depth 0.04 2021-02-12T23:02:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170975.58 epoch sec; seqNo:277; 4056054.21 north; 590344.04 east 2021-02-12T23:02:55Z- AHRS_M2 msg: 1613170975.94 epoch sec; seqNo:277 2021-02-12T23:02:55Z- AHRS_M2 msg: 5.56 phi; 0.00 theta; -0.07 psi 2021-02-12T23:02:55Z- handleDepth()-Depth_Keller msg: 1613170975944 epoch sec; seqNo:278; depth 0.08 2021-02-12T23:02:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170975.95 epoch sec; seqNo:278; 4056054.21 north; 590344.04 east 2021-02-12T23:02:56Z- handleDepth()-Depth_Keller msg: 1613170976365 epoch sec; seqNo:279; depth 0.10 2021-02-12T23:02:56Z- handleDvl() - RDI_Pathfinder msg: 1613170976.38 epoch sec; seqNo:122 2021-02-12T23:02:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.00 , 98.00 , 90.55 , 98.00 2021-02-12T23:02:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , -0.30 , 0.02 2021-02-12T23:02:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170976.39 epoch sec; seqNo:279; 4056054.21 north; 590344.04 east 2021-02-12T23:02:56Z- AHRS_M2 msg: 1613170976.75 epoch sec; seqNo:278 2021-02-12T23:02:56Z- AHRS_M2 msg: 5.56 phi; 0.08 theta; -0.14 psi 2021-02-12T23:02:56Z- handleDepth()-Depth_Keller msg: 1613170976766 epoch sec; seqNo:280; depth 0.12 2021-02-12T23:02:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170976.78 epoch sec; seqNo:280; 4056054.20 north; 590342.82 east 2021-02-12T23:02:57Z- AHRS_M2 msg: 1613170977.15 epoch sec; seqNo:279 2021-02-12T23:02:57Z- AHRS_M2 msg: 5.53 phi; 0.02 theta; -0.12 psi 2021-02-12T23:02:57Z- handleDepth()-Depth_Keller msg: 1613170977157 epoch sec; seqNo:281; depth 0.14 2021-02-12T23:02:57Z- handleDvl() - RDI_Pathfinder msg: 1613170977.17 epoch sec; seqNo:123 2021-02-12T23:02:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.27 , 102.80 , 98.74 , 90.62 2021-02-12T23:02:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.22 , -0.33 , 0.53 2021-02-12T23:02:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170977.19 epoch sec; seqNo:281; 4056054.20 north; 590342.82 east 2021-02-12T23:02:57Z- AHRS_M2 msg: 1613170977.56 epoch sec; seqNo:280 2021-02-12T23:02:57Z- AHRS_M2 msg: 5.53 phi; -0.00 theta; -0.11 psi 2021-02-12T23:02:57Z- handleDepth()-Depth_Keller msg: 1613170977565 epoch sec; seqNo:282; depth 0.14 2021-02-12T23:02:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170977.58 epoch sec; seqNo:282; 4056054.20 north; 590342.82 east 2021-02-12T23:02:57Z- AHRS_M2 msg: 1613170977.96 epoch sec; seqNo:281 2021-02-12T23:02:57Z- AHRS_M2 msg: 5.55 phi; 0.01 theta; -0.09 psi 2021-02-12T23:02:57Z- handleDepth()-Depth_Keller msg: 1613170977965 epoch sec; seqNo:283; depth 0.12 2021-02-12T23:02:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170977.97 epoch sec; seqNo:283; 4056054.20 north; 590342.82 east 2021-02-12T23:02:58Z- AHRS_M2 msg: 1613170978.36 epoch sec; seqNo:282 2021-02-12T23:02:58Z- AHRS_M2 msg: 5.58 phi; -0.03 theta; -0.07 psi 2021-02-12T23:02:58Z- handleDepth()-Depth_Keller msg: 1613170978368 epoch sec; seqNo:284; depth 0.09 2021-02-12T23:02:58Z- handleDvl() - RDI_Pathfinder msg: 1613170978.39 epoch sec; seqNo:124 2021-02-12T23:02:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.33 , 96.10 , 94.75 , 94.75 2021-02-12T23:02:58Z- handleDvl() - RDI_Pathfinder msg: velocities -0.08 , -0.09 , 0.31 2021-02-12T23:02:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170978.40 epoch sec; seqNo:284; 4056054.20 north; 590342.82 east 2021-02-12T23:02:58Z- AHRS_M2 msg: 1613170978.77 epoch sec; seqNo:283 2021-02-12T23:02:58Z- AHRS_M2 msg: 5.59 phi; -0.08 theta; -0.06 psi 2021-02-12T23:02:58Z- handleDepth()-Depth_Keller msg: 1613170978777 epoch sec; seqNo:285; depth 0.08 2021-02-12T23:02:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170978.80 epoch sec; seqNo:285; 4056054.20 north; 590342.82 east 2021-02-12T23:02:59Z- AHRS_M2 msg: 1613170979.17 epoch sec; seqNo:284 2021-02-12T23:02:59Z- AHRS_M2 msg: 5.57 phi; -0.11 theta; -0.05 psi 2021-02-12T23:02:59Z- handleDepth()-Depth_Keller msg: 1613170979177 epoch sec; seqNo:286; depth 0.04 2021-02-12T23:02:59Z- handleDvl() - RDI_Pathfinder msg: 1613170979.19 epoch sec; seqNo:125 2021-02-12T23:02:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.88 , 96.24 , 90.82 , 96.24 2021-02-12T23:02:59Z- handleDvl() - RDI_Pathfinder msg: velocities -0.21 , 0.18 , -0.02 2021-02-12T23:02:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170979.21 epoch sec; seqNo:286; 4056054.20 north; 590342.82 east 2021-02-12T23:02:59Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 131 calls... 2021-02-12T23:02:59Z- waiting for 1613170976.93, time is 1613170979.21 2021-02-12T23:02:59Z- LcmTrn::updateTrn() equating pose 1613170979.21 and meas times 1613170979.21 2021-02-12T23:02:59Z- LcmTrn::updateTrn() >>>> pose time 1613170979.21, meas time 1613170979.21 <<<< 2021-02-12T23:02:59Z- TerrainNav::Measurement type 1 from time = 1613170979.21 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:02:59Z- TerrainNav::motionUpdate() - currEstimate = (4056054, 590342.82, 0.04) 2021-02-12T23:02:59Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:02:59Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:59Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:02:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:02:59Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:02:59Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:02:59Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:02:59Z- AHRS_M2 msg: 1613170979.58 epoch sec; seqNo:285 2021-02-12T23:02:59Z- AHRS_M2 msg: 5.53 phi; -0.20 theta; -0.14 psi 2021-02-12T23:02:59Z- handleDepth()-Depth_Keller msg: 1613170979585 epoch sec; seqNo:287; depth 0.04 2021-02-12T23:02:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170979.60 epoch sec; seqNo:287; 4056054.20 north; 590342.82 east 2021-02-12T23:03:00Z- AHRS_M2 msg: 1613170980.01 epoch sec; seqNo:286 2021-02-12T23:03:00Z- AHRS_M2 msg: 5.60 phi; -0.20 theta; -0.24 psi 2021-02-12T23:03:00Z- handleDepth()-Depth_Keller msg: 1613170980018 epoch sec; seqNo:288; depth -0.03 2021-02-12T23:03:00Z- handleDvl() - RDI_Pathfinder msg: 1613170980.05 epoch sec; seqNo:126 2021-02-12T23:03:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.40 , 105.64 , 0.00 , 109.70 2021-02-12T23:03:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.51 , -0.34 , -0.58 2021-02-12T23:03:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170980.07 epoch sec; seqNo:288; 4056054.20 north; 590342.82 east 2021-02-12T23:03:00Z- AHRS_M2 msg: 1613170980.38 epoch sec; seqNo:287 2021-02-12T23:03:00Z- AHRS_M2 msg: 5.65 phi; -0.08 theta; -0.10 psi 2021-02-12T23:03:00Z- handleDepth()-Depth_Keller msg: 1613170980389 epoch sec; seqNo:289; depth 0.04 2021-02-12T23:03:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170980.41 epoch sec; seqNo:289; 4056054.20 north; 590342.82 east 2021-02-12T23:03:00Z- AHRS_M2 msg: 1613170980.79 epoch sec; seqNo:288 2021-02-12T23:03:00Z- AHRS_M2 msg: 5.64 phi; 0.00 theta; 0.01 psi 2021-02-12T23:03:00Z- handleDepth()-Depth_Keller msg: 1613170980793 epoch sec; seqNo:290; depth 0.09 2021-02-12T23:03:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170980.81 epoch sec; seqNo:290; 4056054.20 north; 590342.82 east 2021-02-12T23:03:01Z- AHRS_M2 msg: 1613170981.19 epoch sec; seqNo:289 2021-02-12T23:03:01Z- AHRS_M2 msg: 5.65 phi; 0.01 theta; -0.02 psi 2021-02-12T23:03:01Z- handleDepth()-Depth_Keller msg: 1613170981209 epoch sec; seqNo:291; depth 0.10 2021-02-12T23:03:01Z- handleDvl() - RDI_Pathfinder msg: 1613170981.24 epoch sec; seqNo:127 2021-02-12T23:03:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.00 , 93.53 , 98.00 , 93.53 2021-02-12T23:03:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.36 , 0.01 2021-02-12T23:03:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170981.28 epoch sec; seqNo:291; 4056054.56 north; 590341.60 east 2021-02-12T23:03:01Z- AHRS_M2 msg: 1613170981.60 epoch sec; seqNo:290 2021-02-12T23:03:01Z- AHRS_M2 msg: 5.67 phi; 0.02 theta; -0.01 psi 2021-02-12T23:03:01Z- handleDepth()-Depth_Keller msg: 1613170981605 epoch sec; seqNo:292; depth 0.12 2021-02-12T23:03:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170981.63 epoch sec; seqNo:292; 4056054.56 north; 590341.60 east 2021-02-12T23:03:02Z- AHRS_M2 msg: 1613170982.00 epoch sec; seqNo:291 2021-02-12T23:03:02Z- AHRS_M2 msg: 5.71 phi; 0.05 theta; 0.02 psi 2021-02-12T23:03:02Z- handleDepth()-Depth_Keller msg: 1613170982009 epoch sec; seqNo:293; depth 0.12 2021-02-12T23:03:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170982.03 epoch sec; seqNo:293; 4056054.94 north; 590341.59 east 2021-02-12T23:03:02Z- AHRS_M2 msg: 1613170982.41 epoch sec; seqNo:292 2021-02-12T23:03:02Z- AHRS_M2 msg: 5.72 phi; 0.05 theta; -0.12 psi 2021-02-12T23:03:02Z- handleDepth()-Depth_Keller msg: 1613170982417 epoch sec; seqNo:294; depth 0.11 2021-02-12T23:03:02Z- handleDvl() - RDI_Pathfinder msg: 1613170982.44 epoch sec; seqNo:128 2021-02-12T23:03:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.04 , 93.33 , 96.04 , 93.33 2021-02-12T23:03:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.51 , 0.07 , 0.59 2021-02-12T23:03:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170982.49 epoch sec; seqNo:294; 4056055.32 north; 590341.59 east 2021-02-12T23:03:02Z- AHRS_M2 msg: 1613170982.81 epoch sec; seqNo:293 2021-02-12T23:03:02Z- AHRS_M2 msg: 5.76 phi; -0.11 theta; -0.11 psi 2021-02-12T23:03:02Z- handleDepth()-Depth_Keller msg: 1613170982817 epoch sec; seqNo:295; depth 0.07 2021-02-12T23:03:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170982.83 epoch sec; seqNo:295; 4056055.32 north; 590341.59 east 2021-02-12T23:03:03Z- AHRS_M2 msg: 1613170983.21 epoch sec; seqNo:294 2021-02-12T23:03:03Z- AHRS_M2 msg: 5.80 phi; -0.18 theta; -0.05 psi 2021-02-12T23:03:03Z- handleDepth()-Depth_Keller msg: 1613170983216 epoch sec; seqNo:296; depth 0.08 2021-02-12T23:03:03Z- handleDvl() - RDI_Pathfinder msg: 1613170983.23 epoch sec; seqNo:129 2021-02-12T23:03:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.11 , 101.58 , 97.52 , 92.11 2021-02-12T23:03:03Z- handleDvl() - RDI_Pathfinder msg: velocities -0.28 , -0.21 , 0.24 2021-02-12T23:03:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170983.24 epoch sec; seqNo:296; 4056055.32 north; 590341.59 east 2021-02-12T23:03:03Z- AHRS_M2 msg: 1613170983.62 epoch sec; seqNo:295 2021-02-12T23:03:03Z- AHRS_M2 msg: 5.79 phi; -0.10 theta; 0.01 psi 2021-02-12T23:03:03Z- handleDepth()-Depth_Keller msg: 1613170983620 epoch sec; seqNo:297; depth 0.03 2021-02-12T23:03:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170983.63 epoch sec; seqNo:297; 4056054.94 north; 590341.59 east 2021-02-12T23:03:04Z- AHRS_M2 msg: 1613170984.02 epoch sec; seqNo:296 2021-02-12T23:03:04Z- AHRS_M2 msg: 5.78 phi; -0.03 theta; 0.02 psi 2021-02-12T23:03:04Z- handleDepth()-Depth_Keller msg: 1613170984024 epoch sec; seqNo:298; depth 0.04 2021-02-12T23:03:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170984.03 epoch sec; seqNo:298; 4056054.94 north; 590341.59 east 2021-02-12T23:03:04Z- LcmTrn::handleMessages() - handling 40 messages took 0.74 ms and 125 calls... 2021-02-12T23:03:04Z- waiting for 1613170982.21, time is 1613170984.03 2021-02-12T23:03:04Z- LcmTrn::updateTrn() >>>> pose time 1613170984.03, meas time 1613170983.23 <<<< 2021-02-12T23:03:04Z- TerrainNav::Measurement type 1 from time = 1613170983.23 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:03:04Z- TerrainNav::motionUpdate() - currEstimate = (4056055, 590341.59, 0.04) 2021-02-12T23:03:04Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:03:04Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:03:04Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:03:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:03:04Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:03:04Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:03:04Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:03:04Z- AHRS_M2 msg: 1613170984.42 epoch sec; seqNo:297 2021-02-12T23:03:04Z- AHRS_M2 msg: 5.83 phi; -0.01 theta; 0.06 psi 2021-02-12T23:03:04Z- handleDepth()-Depth_Keller msg: 1613170984429 epoch sec; seqNo:299; depth 0.12 2021-02-12T23:03:04Z- handleDvl() - RDI_Pathfinder msg: 1613170984.44 epoch sec; seqNo:130 2021-02-12T23:03:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.03 , 94.68 , 86.56 , 106.86 2021-02-12T23:03:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.28 , -0.70 2021-02-12T23:03:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170984.45 epoch sec; seqNo:299; 4056054.94 north; 590341.59 east 2021-02-12T23:03:04Z- AHRS_M2 msg: 1613170984.83 epoch sec; seqNo:298 2021-02-12T23:03:04Z- AHRS_M2 msg: 5.88 phi; 0.04 theta; 0.08 psi 2021-02-12T23:03:04Z- handleDepth()-Depth_Keller msg: 1613170984832 epoch sec; seqNo:300; depth 0.19 2021-02-12T23:03:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170984.85 epoch sec; seqNo:300; 4056055.31 north; 590340.37 east 2021-02-12T23:03:05Z- AHRS_M2 msg: 1613170985.23 epoch sec; seqNo:299 2021-02-12T23:03:05Z- AHRS_M2 msg: 5.92 phi; 0.08 theta; 0.06 psi 2021-02-12T23:03:05Z- handleDepth()-Depth_Keller msg: 1613170985237 epoch sec; seqNo:301; depth 0.23 2021-02-12T23:03:05Z- handleDvl() - RDI_Pathfinder msg: 1613170985.25 epoch sec; seqNo:131 2021-02-12T23:03:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.04 , 90.62 , 93.33 , 97.39 2021-02-12T23:03:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.11 , 0.16 2021-02-12T23:03:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170985.27 epoch sec; seqNo:301; 4056055.69 north; 590340.36 east 2021-02-12T23:03:05Z- AHRS_M2 msg: 1613170985.64 epoch sec; seqNo:300 2021-02-12T23:03:05Z- AHRS_M2 msg: 5.95 phi; 0.14 theta; -0.01 psi 2021-02-12T23:03:05Z- handleDepth()-Depth_Keller msg: 1613170985640 epoch sec; seqNo:302; depth 0.26 2021-02-12T23:03:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170985.65 epoch sec; seqNo:302; 4056056.07 north; 590340.36 east 2021-02-12T23:03:06Z- AHRS_M2 msg: 1613170986.07 epoch sec; seqNo:301 2021-02-12T23:03:06Z- AHRS_M2 msg: 5.98 phi; 0.13 theta; -0.09 psi 2021-02-12T23:03:06Z- handleDepth()-Depth_Keller msg: 1613170986074 epoch sec; seqNo:303; depth 0.27 2021-02-12T23:03:06Z- handleDvl() - RDI_Pathfinder msg: 1613170986.09 epoch sec; seqNo:132 2021-02-12T23:03:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.88 , 92.11 , 98.88 , 89.40 2021-02-12T23:03:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , 0.00 , 0.32 2021-02-12T23:03:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170986.11 epoch sec; seqNo:303; 4056056.44 north; 590340.36 east 2021-02-12T23:03:06Z- AHRS_M2 msg: 1613170986.44 epoch sec; seqNo:302 2021-02-12T23:03:06Z- AHRS_M2 msg: 6.01 phi; 0.06 theta; -0.09 psi 2021-02-12T23:03:06Z- handleDepth()-Depth_Keller msg: 1613170986448 epoch sec; seqNo:304; depth 0.29 2021-02-12T23:03:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170986.47 epoch sec; seqNo:304; 4056056.44 north; 590340.36 east 2021-02-12T23:03:06Z- AHRS_M2 msg: 1613170986.85 epoch sec; seqNo:303 2021-02-12T23:03:06Z- AHRS_M2 msg: 6.00 phi; -0.01 theta; 0.02 psi 2021-02-12T23:03:06Z- handleDepth()-Depth_Keller msg: 1613170986852 epoch sec; seqNo:305; depth 0.33 2021-02-12T23:03:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170986.86 epoch sec; seqNo:305; 4056056.82 north; 590340.35 east 2021-02-12T23:03:07Z- AHRS_M2 msg: 1613170987.25 epoch sec; seqNo:304 2021-02-12T23:03:07Z- AHRS_M2 msg: 5.99 phi; -0.06 theta; 0.10 psi 2021-02-12T23:03:07Z- handleDepth()-Depth_Keller msg: 1613170987256 epoch sec; seqNo:306; depth 0.30 2021-02-12T23:03:07Z- handleDvl() - RDI_Pathfinder msg: 1613170987.27 epoch sec; seqNo:133 2021-02-12T23:03:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.46 , 98.87 , 97.52 , 90.75 2021-02-12T23:03:07Z- handleDvl() - RDI_Pathfinder msg: velocities -0.26 , -0.04 , -0.04 2021-02-12T23:03:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170987.28 epoch sec; seqNo:306; 4056056.82 north; 590340.35 east 2021-02-12T23:03:07Z- AHRS_M2 msg: 1613170987.66 epoch sec; seqNo:305 2021-02-12T23:03:07Z- AHRS_M2 msg: 6.03 phi; -0.04 theta; 0.09 psi 2021-02-12T23:03:07Z- handleDepth()-Depth_Keller msg: 1613170987661 epoch sec; seqNo:307; depth 0.24 2021-02-12T23:03:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170987.67 epoch sec; seqNo:307; 4056056.44 north; 590340.36 east 2021-02-12T23:03:08Z- AHRS_M2 msg: 1613170988.08 epoch sec; seqNo:306 2021-02-12T23:03:08Z- AHRS_M2 msg: 6.07 phi; -0.04 theta; 0.06 psi 2021-02-12T23:03:08Z- handleDepth()-Depth_Keller msg: 1613170988089 epoch sec; seqNo:308; depth 0.16 2021-02-12T23:03:08Z- handleDvl() - RDI_Pathfinder msg: 1613170988.10 epoch sec; seqNo:134 2021-02-12T23:03:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.51 , 89.33 , 89.33 , 98.81 2021-02-12T23:03:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.46 , -0.25 , -0.25 2021-02-12T23:03:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170988.11 epoch sec; seqNo:308; 4056056.82 north; 590340.35 east 2021-02-12T23:03:08Z- AHRS_M2 msg: 1613170988.46 epoch sec; seqNo:307 2021-02-12T23:03:08Z- AHRS_M2 msg: 6.11 phi; -0.04 theta; 0.06 psi 2021-02-12T23:03:08Z- handleDepth()-Depth_Keller msg: 1613170988468 epoch sec; seqNo:309; depth 0.11 2021-02-12T23:03:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170988.48 epoch sec; seqNo:309; 4056056.82 north; 590340.35 east 2021-02-12T23:03:08Z- AHRS_M2 msg: 1613170988.87 epoch sec; seqNo:308 2021-02-12T23:03:08Z- AHRS_M2 msg: 6.15 phi; -0.06 theta; 0.05 psi 2021-02-12T23:03:08Z- handleDepth()-Depth_Keller msg: 1613170988872 epoch sec; seqNo:310; depth 0.06 2021-02-12T23:03:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170988.88 epoch sec; seqNo:310; 4056057.20 north; 590340.35 east 2021-02-12T23:03:09Z- AHRS_M2 msg: 1613170989.27 epoch sec; seqNo:309 2021-02-12T23:03:09Z- AHRS_M2 msg: 6.11 phi; -0.13 theta; 0.02 psi 2021-02-12T23:03:09Z- handleDepth()-Depth_Keller msg: 1613170989278 epoch sec; seqNo:311; depth 0.07 2021-02-12T23:03:09Z- handleDvl() - RDI_Pathfinder msg: 1613170989.29 epoch sec; seqNo:135 2021-02-12T23:03:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.17 , 90.75 , 90.75 , 98.87 2021-02-12T23:03:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , 0.05 , 0.69 2021-02-12T23:03:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170989.30 epoch sec; seqNo:311; 4056057.20 north; 590340.35 east 2021-02-12T23:03:09Z- LcmTrn::handleMessages() - handling 45 messages took 0.71 ms and 131 calls... 2021-02-12T23:03:09Z- waiting for 1613170987.03, time is 1613170989.30 2021-02-12T23:03:09Z- LcmTrn::updateTrn() equating pose 1613170989.30 and meas times 1613170989.30 2021-02-12T23:03:09Z- LcmTrn::updateTrn() >>>> pose time 1613170989.30, meas time 1613170989.30 <<<< 2021-02-12T23:03:09Z- TerrainNav::Measurement type 1 from time = 1613170989.30 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:03:09Z- TerrainNav::motionUpdate() - currEstimate = (4056057, 590340.35, 0.07) 2021-02-12T23:03:09Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:03:09Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:03:09Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:03:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:03:09Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:03:09Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:03:09Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:03:09Z- AHRS_M2 msg: 1613170989.68 epoch sec; seqNo:310 2021-02-12T23:03:09Z- AHRS_M2 msg: 6.11 phi; -0.12 theta; -0.01 psi 2021-02-12T23:03:09Z- handleDepth()-Depth_Keller msg: 1613170989680 epoch sec; seqNo:312; depth 0.08 2021-02-12T23:03:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170989.71 epoch sec; seqNo:312; 4056057.58 north; 590340.34 east 2021-02-12T23:03:10Z- AHRS_M2 msg: 1613170990.08 epoch sec; seqNo:311 2021-02-12T23:03:10Z- AHRS_M2 msg: 6.15 phi; -0.10 theta; -0.06 psi 2021-02-12T23:03:10Z- handleDepth()-Depth_Keller msg: 1613170990087 epoch sec; seqNo:313; depth 0.04 2021-02-12T23:03:10Z- handleDvl() - RDI_Pathfinder msg: 1613170990.10 epoch sec; seqNo:136 2021-02-12T23:03:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.24 , 93.53 , 86.76 , 104.36 2021-02-12T23:03:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.45 , 0.33 , -0.16 2021-02-12T23:03:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170990.13 epoch sec; seqNo:313; 4056057.58 north; 590340.34 east 2021-02-12T23:03:10Z- AHRS_M2 msg: 1613170990.48 epoch sec; seqNo:312 2021-02-12T23:03:10Z- AHRS_M2 msg: 6.17 phi; -0.13 theta; -0.12 psi 2021-02-12T23:03:10Z- handleDepth()-Depth_Keller msg: 1613170990489 epoch sec; seqNo:314; depth 0.03 2021-02-12T23:03:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170990.50 epoch sec; seqNo:314; 4056057.96 north; 590340.34 east 2021-02-12T23:03:10Z- AHRS_M2 msg: 1613170990.89 epoch sec; seqNo:313 2021-02-12T23:03:10Z- AHRS_M2 msg: 6.20 phi; -0.16 theta; -0.08 psi 2021-02-12T23:03:10Z- handleDepth()-Depth_Keller msg: 1613170990894 epoch sec; seqNo:315; depth 0.05 2021-02-12T23:03:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170990.90 epoch sec; seqNo:315; 4056057.96 north; 590340.34 east 2021-02-12T23:03:11Z- AHRS_M2 msg: 1613170991.29 epoch sec; seqNo:314 2021-02-12T23:03:11Z- AHRS_M2 msg: 6.21 phi; -0.09 theta; 0.07 psi 2021-02-12T23:03:11Z- handleDepth()-Depth_Keller msg: 1613170991301 epoch sec; seqNo:316; depth 0.07 2021-02-12T23:03:11Z- handleDvl() - RDI_Pathfinder msg: 1613170991.32 epoch sec; seqNo:137 2021-02-12T23:03:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.04 , 102.87 , 87.98 , 105.57 2021-02-12T23:03:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.32 , -0.31 , -0.02 2021-02-12T23:03:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170991.33 epoch sec; seqNo:316; 4056058.34 north; 590340.34 east 2021-02-12T23:03:11Z- AHRS_M2 msg: 1613170991.70 epoch sec; seqNo:315 2021-02-12T23:03:11Z- AHRS_M2 msg: 6.22 phi; -0.04 theta; 0.12 psi 2021-02-12T23:03:11Z- handleDepth()-Depth_Keller msg: 1613170991700 epoch sec; seqNo:317; depth 0.05 2021-02-12T23:03:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170991.71 epoch sec; seqNo:317; 4056058.34 north; 590340.34 east 2021-02-12T23:03:12Z- AHRS_M2 msg: 1613170992.12 epoch sec; seqNo:316 2021-02-12T23:03:12Z- AHRS_M2 msg: 6.23 phi; -0.12 theta; 0.03 psi 2021-02-12T23:03:12Z- handleDepth()-Depth_Keller msg: 1613170992135 epoch sec; seqNo:318; depth 0.06 2021-02-12T23:03:12Z- handleDvl() - RDI_Pathfinder msg: 1613170992.16 epoch sec; seqNo:138 2021-02-12T23:03:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 106.79 , 87.85 , 97.32 , 98.67 2021-02-12T23:03:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.52 , -0.09 , 0.06 2021-02-12T23:03:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170992.17 epoch sec; seqNo:318; 4056058.72 north; 590340.33 east 2021-02-12T23:03:12Z- AHRS_M2 msg: 1613170992.51 epoch sec; seqNo:317 2021-02-12T23:03:12Z- AHRS_M2 msg: 6.26 phi; -0.14 theta; -0.05 psi 2021-02-12T23:03:12Z- handleDepth()-Depth_Keller msg: 1613170992513 epoch sec; seqNo:319; depth 0.08 2021-02-12T23:03:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170992.53 epoch sec; seqNo:319; 4056058.72 north; 590340.33 east 2021-02-12T23:03:12Z- AHRS_M2 msg: 1613170992.91 epoch sec; seqNo:318 2021-02-12T23:03:12Z- AHRS_M2 msg: 0.02 phi; -0.07 theta; -0.06 psi 2021-02-12T23:03:12Z- handleDepth()-Depth_Keller msg: 1613170992912 epoch sec; seqNo:320; depth 0.01 2021-02-12T23:03:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170992.92 epoch sec; seqNo:320; 4056059.10 north; 590340.33 east 2021-02-12T23:03:13Z- AHRS_M2 msg: 1613170993.31 epoch sec; seqNo:319 2021-02-12T23:03:13Z- AHRS_M2 msg: 0.06 phi; -0.08 theta; -0.03 psi 2021-02-12T23:03:13Z- handleDepth()-Depth_Keller msg: 1613170993316 epoch sec; seqNo:321; depth 0.01 2021-02-12T23:03:13Z- handleDvl() - RDI_Pathfinder msg: 1613170993.33 epoch sec; seqNo:139 2021-02-12T23:03:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.19 , 95.90 , 86.42 , 104.02 2021-02-12T23:03:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.56 , -0.42 , -0.09 2021-02-12T23:03:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170993.34 epoch sec; seqNo:321; 4056059.10 north; 590340.33 east 2021-02-12T23:03:13Z- AHRS_M2 msg: 1613170993.72 epoch sec; seqNo:320 2021-02-12T23:03:13Z- AHRS_M2 msg: 0.04 phi; -0.11 theta; 0.01 psi 2021-02-12T23:03:13Z- handleDepth()-Depth_Keller msg: 1613170993721 epoch sec; seqNo:322; depth 0.04 2021-02-12T23:03:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170993.73 epoch sec; seqNo:322; 4056059.46 north; 590339.10 east 2021-02-12T23:03:14Z- AHRS_M2 msg: 1613170994.12 epoch sec; seqNo:321 2021-02-12T23:03:14Z- AHRS_M2 msg: 6.26 phi; -0.04 theta; 0.03 psi 2021-02-12T23:03:14Z- handleDepth()-Depth_Keller msg: 1613170994129 epoch sec; seqNo:323; depth 0.04 2021-02-12T23:03:14Z- handleDvl() - RDI_Pathfinder msg: 1613170994.15 epoch sec; seqNo:140 2021-02-12T23:03:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.17 , 94.82 , 86.69 , 102.93 2021-02-12T23:03:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.15 , -0.05 2021-02-12T23:03:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170994.16 epoch sec; seqNo:323; 4056059.84 north; 590339.10 east 2021-02-12T23:03:14Z- LcmTrn::handleMessages() - handling 41 messages took 0.65 ms and 128 calls... 2021-02-12T23:03:14Z- waiting for 1613170992.30, time is 1613170994.16 2021-02-12T23:03:14Z- LcmTrn::updateTrn() equating pose 1613170994.16 and meas times 1613170994.16 2021-02-12T23:03:14Z- LcmTrn::updateTrn() >>>> pose time 1613170994.16, meas time 1613170994.16 <<<< 2021-02-12T23:03:14Z- TerrainNav::Measurement type 1 from time = 1613170994.16 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:03:14Z- TerrainNav::motionUpdate() - currEstimate = (4056060, 590339.10, 0.04) 2021-02-12T23:03:14Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:03:14Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:03:14Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:03:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:03:14Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:03:14Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:03:14Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:03:14Z- AHRS_M2 msg: 1613170994.52 epoch sec; seqNo:322 2021-02-12T23:03:14Z- AHRS_M2 msg: 6.28 phi; 0.04 theta; 0.05 psi 2021-02-12T23:03:14Z- handleDepth()-Depth_Keller msg: 1613170994528 epoch sec; seqNo:324; depth 0.05 2021-02-12T23:03:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170994.54 epoch sec; seqNo:324; 4056060.22 north; 590339.10 east 2021-02-12T23:03:14Z- AHRS_M2 msg: 1613170994.93 epoch sec; seqNo:323 2021-02-12T23:03:14Z- AHRS_M2 msg: 0.07 phi; 0.05 theta; 0.11 psi 2021-02-12T23:03:14Z- handleDepth()-Depth_Keller msg: 1613170994933 epoch sec; seqNo:325; depth 0.04 2021-02-12T23:03:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170994.94 epoch sec; seqNo:325; 4056060.22 north; 590339.10 east 2021-02-12T23:03:15Z- AHRS_M2 msg: 1613170995.33 epoch sec; seqNo:324 2021-02-12T23:03:15Z- AHRS_M2 msg: 0.11 phi; -0.03 theta; 0.11 psi 2021-02-12T23:03:15Z- handleDepth()-Depth_Keller msg: 1613170995336 epoch sec; seqNo:326; depth 0.07 2021-02-12T23:03:15Z- handleDvl() - RDI_Pathfinder msg: 1613170995.35 epoch sec; seqNo:141 2021-02-12T23:03:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.45 , 90.69 , 97.45 , 97.45 2021-02-12T23:03:15Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , -0.30 , 0.33 2021-02-12T23:03:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170995.36 epoch sec; seqNo:326; 4056060.98 north; 590339.09 east 2021-02-12T23:03:15Z- handleDepth()-Depth_Keller msg: 1613170995757 epoch sec; seqNo:327; depth 0.09 2021-02-12T23:03:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170995.77 epoch sec; seqNo:327; 4056061.36 north; 590339.08 east 2021-02-12T23:03:16Z- AHRS_M2 msg: 1613170996.15 epoch sec; seqNo:325 2021-02-12T23:03:16Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 0.07 psi 2021-02-12T23:03:16Z- handleDepth()-Depth_Keller msg: 1613170996151 epoch sec; seqNo:328; depth 0.08 2021-02-12T23:03:16Z- handleDvl() - RDI_Pathfinder msg: 1613170996.16 epoch sec; seqNo:142 2021-02-12T23:03:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 100.09 , 90.62 , 86.56 , 96.03 2021-02-12T23:03:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.29 , 0.15 , 0.74 2021-02-12T23:03:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170996.17 epoch sec; seqNo:328; 4056061.36 north; 590339.08 east 2021-02-12T23:03:16Z- AHRS_M2 msg: 1613170996.55 epoch sec; seqNo:326 2021-02-12T23:03:16Z- AHRS_M2 msg: 0.21 phi; -0.12 theta; 0.05 psi 2021-02-12T23:03:16Z- handleDepth()-Depth_Keller msg: 1613170996549 epoch sec; seqNo:329; depth 0.06 2021-02-12T23:03:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170996.56 epoch sec; seqNo:329; 4056061.36 north; 590339.08 east 2021-02-12T23:03:16Z- AHRS_M2 msg: 1613170996.95 epoch sec; seqNo:327 2021-02-12T23:03:16Z- AHRS_M2 msg: 0.17 phi; -0.21 theta; 0.05 psi 2021-02-12T23:03:16Z- handleDepth()-Depth_Keller msg: 1613170996952 epoch sec; seqNo:330; depth 0.08 2021-02-12T23:03:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170996.97 epoch sec; seqNo:330; 4056061.36 north; 590339.08 east 2021-02-12T23:03:17Z- AHRS_M2 msg: 1613170997.35 epoch sec; seqNo:328 2021-02-12T23:03:17Z- AHRS_M2 msg: 0.15 phi; -0.15 theta; -0.02 psi 2021-02-12T23:03:17Z- handleDepth()-Depth_Keller msg: 1613170997356 epoch sec; seqNo:331; depth 0.02 2021-02-12T23:03:17Z- handleDvl() - RDI_Pathfinder msg: 1613170997.37 epoch sec; seqNo:143 2021-02-12T23:03:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.01 , 97.66 , 86.83 , 105.78 2021-02-12T23:03:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.04 , 0.37 , -0.28 2021-02-12T23:03:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170997.39 epoch sec; seqNo:331; 4056061.36 north; 590339.08 east 2021-02-12T23:03:17Z- AHRS_M2 msg: 1613170997.81 epoch sec; seqNo:329 2021-02-12T23:03:17Z- AHRS_M2 msg: 0.25 phi; -0.16 theta; -0.02 psi 2021-02-12T23:03:17Z- handleDepth()-Depth_Keller msg: 1613170997861 epoch sec; seqNo:332; depth 0.07 2021-02-12T23:03:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170997.91 epoch sec; seqNo:332; 4056061.37 north; 590340.30 east 2021-02-12T23:03:18Z- AHRS_M2 msg: 1613170998.24 epoch sec; seqNo:330 2021-02-12T23:03:18Z- AHRS_M2 msg: 0.19 phi; -0.18 theta; -0.04 psi 2021-02-12T23:03:18Z- handleDepth()-Depth_Keller msg: 1613170998249 epoch sec; seqNo:333; depth 0.15 2021-02-12T23:03:18Z- handleDvl() - RDI_Pathfinder msg: 1613170998.26 epoch sec; seqNo:144 2021-02-12T23:03:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.25 , 95.90 , 87.78 , 102.66 2021-02-12T23:03:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.25 , 0.76 , -0.97 2021-02-12T23:03:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170998.29 epoch sec; seqNo:333; 4056061.75 north; 590340.30 east 2021-02-12T23:03:18Z- AHRS_M2 msg: 1613170998.65 epoch sec; seqNo:331 2021-02-12T23:03:18Z- AHRS_M2 msg: 0.12 phi; -0.09 theta; -0.13 psi 2021-02-12T23:03:18Z- handleDepth()-Depth_Keller msg: 1613170998658 epoch sec; seqNo:334; depth 0.11 2021-02-12T23:03:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170998.67 epoch sec; seqNo:334; 4056061.75 north; 590340.30 east 2021-02-12T23:03:19Z- AHRS_M2 msg: 1613170999.05 epoch sec; seqNo:332 2021-02-12T23:03:19Z- AHRS_M2 msg: 0.18 phi; -0.03 theta; -0.11 psi 2021-02-12T23:03:19Z- handleDepth()-Depth_Keller msg: 1613170999058 epoch sec; seqNo:335; depth 0.03 2021-02-12T23:03:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170999.07 epoch sec; seqNo:335; 4056061.75 north; 590340.30 east 2021-02-12T23:03:19Z- LcmTrn::handleMessages() - handling 39 messages took 0.64 ms and 126 calls... 2021-02-12T23:03:19Z- waiting for 1613170997.16, time is 1613170999.07 2021-02-12T23:03:19Z- LcmTrn::updateTrn() >>>> pose time 1613170999.07, meas time 1613170998.26 <<<< 2021-02-12T23:03:19Z- TerrainNav::Measurement type 1 from time = 1613170998.26 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:03:19Z- TerrainNav::motionUpdate() - currEstimate = (4056062, 590340.30, 0.03) 2021-02-12T23:03:19Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:03:19Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:03:19Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:03:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:03:19Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:03:19Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:03:19Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:03:19Z- AHRS_M2 msg: 1613170999.46 epoch sec; seqNo:333 2021-02-12T23:03:19Z- AHRS_M2 msg: 0.25 phi; -0.06 theta; 0.08 psi 2021-02-12T23:03:19Z- handleDepth()-Depth_Keller msg: 1613170999460 epoch sec; seqNo:336; depth 0.06 2021-02-12T23:03:19Z- handleDvl() - RDI_Pathfinder msg: 1613170999.48 epoch sec; seqNo:145 2021-02-12T23:03:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.39 , 97.39 , 86.56 , 106.86 2021-02-12T23:03:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.96 , -0.14 2021-02-12T23:03:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170999.49 epoch sec; seqNo:336; 4056062.13 north; 590340.30 east 2021-02-12T23:03:19Z- AHRS_M2 msg: 1613170999.86 epoch sec; seqNo:334 2021-02-12T23:03:19Z- AHRS_M2 msg: 0.23 phi; -0.12 theta; 0.23 psi 2021-02-12T23:03:19Z- handleDepth()-Depth_Keller msg: 1613170999864 epoch sec; seqNo:337; depth 0.09 2021-02-12T23:03:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613170999.88 epoch sec; seqNo:337; 4056062.51 north; 590340.29 east 2021-02-12T23:03:20Z- AHRS_M2 msg: 1613171000.26 epoch sec; seqNo:335 2021-02-12T23:03:20Z- AHRS_M2 msg: 0.16 phi; -0.14 theta; 0.22 psi 2021-02-12T23:03:20Z- handleDepth()-Depth_Keller msg: 1613171000268 epoch sec; seqNo:338; depth 0.08 2021-02-12T23:03:20Z- handleDvl() - RDI_Pathfinder msg: 1613171000.28 epoch sec; seqNo:146 2021-02-12T23:03:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.67 , 97.32 , 97.32 , 100.03 2021-02-12T23:03:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.60 , -0.56 , 0.69 2021-02-12T23:03:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171000.29 epoch sec; seqNo:338; 4056062.88 north; 590340.29 east 2021-02-12T23:03:20Z- AHRS_M2 msg: 1613171000.67 epoch sec; seqNo:336 2021-02-12T23:03:20Z- AHRS_M2 msg: 0.15 phi; -0.10 theta; 0.13 psi 2021-02-12T23:03:20Z- handleDepth()-Depth_Keller msg: 1613171000673 epoch sec; seqNo:339; depth 0.07 2021-02-12T23:03:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171000.68 epoch sec; seqNo:339; 4056063.25 north; 590339.06 east 2021-02-12T23:03:21Z- AHRS_M2 msg: 1613171001.07 epoch sec; seqNo:337 2021-02-12T23:03:21Z- AHRS_M2 msg: 0.21 phi; -0.10 theta; 0.09 psi 2021-02-12T23:03:21Z- handleDepth()-Depth_Keller msg: 1613171001079 epoch sec; seqNo:340; depth 0.06 2021-02-12T23:03:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171001.10 epoch sec; seqNo:340; 4056063.25 north; 590339.06 east 2021-02-12T23:03:21Z- AHRS_M2 msg: 1613171001.48 epoch sec; seqNo:338 2021-02-12T23:03:21Z- AHRS_M2 msg: 0.24 phi; -0.17 theta; 0.07 psi 2021-02-12T23:03:21Z- handleDepth()-Depth_Keller msg: 1613171001480 epoch sec; seqNo:341; depth 0.09 2021-02-12T23:03:21Z- handleDvl() - RDI_Pathfinder msg: 1613171001.49 epoch sec; seqNo:147 2021-02-12T23:03:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 110.72 , 87.71 , 87.71 , 109.36 2021-02-12T23:03:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.11 , 0.60 2021-02-12T23:03:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171001.51 epoch sec; seqNo:341; 4056063.63 north; 590339.06 east 2021-02-12T23:03:21Z- AHRS_M2 msg: 1613171001.88 epoch sec; seqNo:339 2021-02-12T23:03:21Z- AHRS_M2 msg: 0.21 phi; -0.20 theta; 0.05 psi 2021-02-12T23:03:21Z- handleDepth()-Depth_Keller msg: 1613171001885 epoch sec; seqNo:342; depth 0.13 2021-02-12T23:03:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171001.90 epoch sec; seqNo:342; 4056064.01 north; 590339.06 east 2021-02-12T23:03:22Z- AHRS_M2 msg: 1613171002.28 epoch sec; seqNo:340 2021-02-12T23:03:22Z- AHRS_M2 msg: 0.20 phi; -0.18 theta; -0.00 psi 2021-02-12T23:03:22Z- handleDepth()-Depth_Keller msg: 1613171002288 epoch sec; seqNo:343; depth 0.16 2021-02-12T23:03:22Z- handleDvl() - RDI_Pathfinder msg: 1613171002.31 epoch sec; seqNo:148 2021-02-12T23:03:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 102.53 , 90.35 , 86.29 , 110.65 2021-02-12T23:03:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.41 , 0.64 , -0.04 2021-02-12T23:03:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171002.32 epoch sec; seqNo:343; 4056064.02 north; 590340.28 east 2021-02-12T23:03:22Z- AHRS_M2 msg: 1613171002.69 epoch sec; seqNo:341 2021-02-12T23:03:22Z- AHRS_M2 msg: 0.24 phi; -0.16 theta; -0.06 psi 2021-02-12T23:03:22Z- handleDepth()-Depth_Keller msg: 1613171002702 epoch sec; seqNo:344; depth 0.15 2021-02-12T23:03:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171002.72 epoch sec; seqNo:344; 4056064.02 north; 590340.28 east 2021-02-12T23:03:23Z- AHRS_M2 msg: 1613171003.09 epoch sec; seqNo:342 2021-02-12T23:03:23Z- AHRS_M2 msg: 0.30 phi; -0.25 theta; -0.08 psi 2021-02-12T23:03:23Z- handleDepth()-Depth_Keller msg: 1613171003157 epoch sec; seqNo:345; depth 0.16 2021-02-12T23:03:23Z- handleDvl() - RDI_Pathfinder msg: 1613171003.33 epoch sec; seqNo:149 2021-02-12T23:03:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.25 , 94.54 , 0.00 , 106.72 2021-02-12T23:03:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.42 , 0.10 , -0.57 2021-02-12T23:03:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171003.39 epoch sec; seqNo:345; 4056064.40 north; 590340.27 east 2021-02-12T23:03:23Z- AHRS_M2 msg: 1613171003.65 epoch sec; seqNo:343 2021-02-12T23:03:23Z- AHRS_M2 msg: 0.33 phi; -0.31 theta; -0.02 psi 2021-02-12T23:03:23Z- handleDepth()-Depth_Keller msg: 1613171003655 epoch sec; seqNo:346; depth 0.22 2021-02-12T23:03:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171003.68 epoch sec; seqNo:346; 4056064.40 north; 590340.27 east 2021-02-12T23:03:24Z- AHRS_M2 msg: 1613171004.05 epoch sec; seqNo:344 2021-02-12T23:03:24Z- AHRS_M2 msg: 0.34 phi; -0.36 theta; 0.04 psi 2021-02-12T23:03:24Z- handleDepth()-Depth_Keller msg: 1613171004061 epoch sec; seqNo:347; depth 0.20 2021-02-12T23:03:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171004.08 epoch sec; seqNo:347; 4056064.78 north; 590340.27 east 2021-02-12T23:03:24Z- LcmTrn::handleMessages() - handling 41 messages took 0.70 ms and 127 calls... 2021-02-12T23:03:24Z- waiting for 1613171002.07, time is 1613171004.08 2021-02-12T23:03:24Z- LcmTrn::updateTrn() >>>> pose time 1613171004.08, meas time 1613171003.33 <<<< 2021-02-12T23:03:24Z- TerrainNav::Measurement type 1 from time = 1613171003.33 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:03:24Z- TerrainNav::motionUpdate() - currEstimate = (4056065, 590340.27, 0.20) 2021-02-12T23:03:24Z- TerrainNav::Filter not initialized - vehicle is currently on the surface 2021-02-12T23:03:24Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:03:24Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2021-02-12T23:03:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:03:24Z- LcmTrn::publishEstimates() - MLE : 0.00 0.00 0.00 2021-02-12T23:03:24Z- LcmTrn::publishEstimates() - MMSE : 0.00 0.00 0.00 2021-02-12T23:03:24Z- LcmTrn::publishEstimates() - COVAR: 0.00 0.00 0.00 0.00 2021-02-12T23:03:24Z- AHRS_M2 msg: 1613171004.46 epoch sec; seqNo:345 2021-02-12T23:03:24Z- AHRS_M2 msg: 0.32 phi; -0.42 theta; 0.07 psi 2021-02-12T23:03:24Z- handleDepth()-Depth_Keller msg: 1613171004462 epoch sec; seqNo:348; depth 0.23 2021-02-12T23:03:24Z- handleDvl() - RDI_Pathfinder msg: 1613171004.47 epoch sec; seqNo:150 2021-02-12T23:03:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.18 , 98.47 , 83.58 , 122.83 2021-02-12T23:03:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.08 , -0.48 , -0.44 2021-02-12T23:03:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171004.48 epoch sec; seqNo:348; 4056064.78 north; 590340.27 east 2021-02-12T23:03:24Z- AHRS_M2 msg: 1613171004.86 epoch sec; seqNo:346 2021-02-12T23:03:24Z- AHRS_M2 msg: 0.30 phi; -0.42 theta; 0.07 psi 2021-02-12T23:03:24Z- handleDepth()-Depth_Keller msg: 1613171004864 epoch sec; seqNo:349; depth 0.24 2021-02-12T23:03:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171004.88 epoch sec; seqNo:349; 4056065.16 north; 590340.26 east 2021-02-12T23:03:25Z- AHRS_M2 msg: 1613171005.26 epoch sec; seqNo:347 2021-02-12T23:03:25Z- AHRS_M2 msg: 0.28 phi; -0.38 theta; 0.04 psi 2021-02-12T23:03:25Z- handleDepth()-Depth_Keller msg: 1613171005268 epoch sec; seqNo:350; depth 0.26 2021-02-12T23:03:25Z- handleDvl() - RDI_Pathfinder msg: 1613171005.28 epoch sec; seqNo:151 2021-02-12T23:03:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 111.39 , 96.51 , 79.74 , 139.68 2021-02-12T23:03:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , -0.57 , 0.30 2021-02-12T23:03:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171005.31 epoch sec; seqNo:350; 4056065.54 north; 590340.26 east 2021-02-12T23:03:25Z- AHRS_M2 msg: 1613171005.67 epoch sec; seqNo:348 2021-02-12T23:03:25Z- AHRS_M2 msg: 0.32 phi; -0.40 theta; -0.03 psi 2021-02-12T23:03:25Z- handleDepth()-Depth_Keller msg: 1613171005673 epoch sec; seqNo:351; depth 0.30 2021-02-12T23:03:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171005.69 epoch sec; seqNo:351; 4056065.54 north; 590340.26 east 2021-02-12T23:03:26Z- AHRS_M2 msg: 1613171006.09 epoch sec; seqNo:349 2021-02-12T23:03:26Z- AHRS_M2 msg: 0.35 phi; -0.42 theta; 0.06 psi 2021-02-12T23:03:26Z- handleDepth()-Depth_Keller msg: 1613171006094 epoch sec; seqNo:352; depth 0.42 2021-02-12T23:03:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171006.10 epoch sec; seqNo:352; 4056065.92 north; 590340.26 east 2021-02-12T23:03:26Z- AHRS_M2 msg: 1613171006.49 epoch sec; seqNo:350 2021-02-12T23:03:26Z- AHRS_M2 msg: 0.37 phi; -0.42 theta; 0.05 psi 2021-02-12T23:03:26Z- handleDepth()-Depth_Keller msg: 1613171006502 epoch sec; seqNo:353; depth 0.51 2021-02-12T23:03:26Z- handleDvl() - RDI_Pathfinder msg: 1613171006.53 epoch sec; seqNo:152 2021-02-12T23:03:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 104.96 , 101.99 , 82.64 , 134.74 2021-02-12T23:03:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.43 , -0.14 , 0.56 2021-02-12T23:03:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171006.54 epoch sec; seqNo:353; 4056065.92 north; 590340.26 east 2021-02-12T23:03:26Z- AHRS_M2 msg: 1613171006.88 epoch sec; seqNo:351 2021-02-12T23:03:26Z- AHRS_M2 msg: 0.39 phi; -0.45 theta; 0.02 psi 2021-02-12T23:03:26Z- handleDepth()-Depth_Keller msg: 1613171006888 epoch sec; seqNo:354; depth 0.56 2021-02-12T23:03:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171006.90 epoch sec; seqNo:354; 4056065.92 north; 590340.26 east 2021-02-12T23:03:27Z- AHRS_M2 msg: 1613171007.28 epoch sec; seqNo:352 2021-02-12T23:03:27Z- AHRS_M2 msg: 0.39 phi; -0.48 theta; 0.07 psi 2021-02-12T23:03:27Z- handleDepth()-Depth_Keller msg: 1613171007289 epoch sec; seqNo:355; depth 0.63 2021-02-12T23:03:27Z- handleDvl() - RDI_Pathfinder msg: 1613171007.30 epoch sec; seqNo:153 2021-02-12T23:03:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 112.48 , 96.11 , 84.19 , 140.77 2021-02-12T23:03:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.44 , 0.12 , 0.29 2021-02-12T23:03:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171007.31 epoch sec; seqNo:355; 4056066.29 north; 590340.25 east 2021-02-12T23:03:27Z- AHRS_M2 msg: 1613171007.69 epoch sec; seqNo:353 2021-02-12T23:03:27Z- AHRS_M2 msg: 0.37 phi; -0.46 theta; 0.12 psi 2021-02-12T23:03:27Z- handleDepth()-Depth_Keller msg: 1613171007692 epoch sec; seqNo:356; depth 0.73 2021-02-12T23:03:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171007.71 epoch sec; seqNo:356; 4056066.29 north; 590340.25 east 2021-02-12T23:03:28Z- AHRS_M2 msg: 1613171008.09 epoch sec; seqNo:354 2021-02-12T23:03:28Z- AHRS_M2 msg: 0.36 phi; -0.43 theta; 0.08 psi 2021-02-12T23:03:28Z- handleDepth()-Depth_Keller msg: 1613171008096 epoch sec; seqNo:357; depth 0.80 2021-02-12T23:03:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171008.11 epoch sec; seqNo:357; 4056066.29 north; 590340.25 east 2021-02-12T23:03:28Z- AHRS_M2 msg: 1613171008.50 epoch sec; seqNo:355 2021-02-12T23:03:28Z- AHRS_M2 msg: 0.39 phi; -0.39 theta; -0.03 psi 2021-02-12T23:03:28Z- handleDepth()-Depth_Keller msg: 1613171008500 epoch sec; seqNo:358; depth 0.86 2021-02-12T23:03:28Z- handleDvl() - RDI_Pathfinder msg: 1613171008.51 epoch sec; seqNo:154 2021-02-12T23:03:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 119.72 , 95.90 , 79.75 , 155.46 2021-02-12T23:03:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.22 , -0.19 2021-02-12T23:03:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171008.52 epoch sec; seqNo:358; 4056066.67 north; 590340.25 east 2021-02-12T23:03:28Z- AHRS_M2 msg: 1613171008.90 epoch sec; seqNo:356 2021-02-12T23:03:28Z- AHRS_M2 msg: 0.44 phi; -0.39 theta; -0.11 psi 2021-02-12T23:03:28Z- handleDepth()-Depth_Keller msg: 1613171008906 epoch sec; seqNo:359; depth 0.94 2021-02-12T23:03:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171008.92 epoch sec; seqNo:359; 4056066.67 north; 590340.25 east 2021-02-12T23:03:29Z- AHRS_M2 msg: 1613171009.30 epoch sec; seqNo:357 2021-02-12T23:03:29Z- AHRS_M2 msg: 0.49 phi; -0.42 theta; -0.06 psi 2021-02-12T23:03:29Z- handleDepth()-Depth_Keller msg: 1613171009308 epoch sec; seqNo:360; depth 1.03 2021-02-12T23:03:29Z- handleDvl() - RDI_Pathfinder msg: 1613171009.33 epoch sec; seqNo:155 2021-02-12T23:03:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 104.70 , 95.77 , 82.37 , 134.48 2021-02-12T23:03:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.19 , 0.09 2021-02-12T23:03:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171009.34 epoch sec; seqNo:360; 4056067.05 north; 590340.24 east 2021-02-12T23:03:29Z- LcmTrn::handleMessages() - handling 45 messages took 0.74 ms and 130 calls... 2021-02-12T23:03:29Z- waiting for 1613171007.08, time is 1613171009.34 2021-02-12T23:03:29Z- LcmTrn::updateTrn() equating pose 1613171009.34 and meas times 1613171009.34 2021-02-12T23:03:29Z- LcmTrn::updateTrn() >>>> pose time 1613171009.34, meas time 1613171009.34 <<<< 2021-02-12T23:03:29Z- TerrainNav::Measurement type 1 from time = 1613171009.34 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2021-02-12T23:03:29Z- TerrainNav::motionUpdate() - currEstimate = (4056067, 590340.24, 1.03) 2021-02-12T23:03:29Z- TerrainNav::attemptInitFilter is increasing Init Search Window by 0.000000 m 2021-02-12T23:03:29Z- TNavBF: Successfully initialized particles 2021-02-12T23:03:29Z- TerrainNav:: vehicle motion has been initialized 2021-02-12T23:03:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:03:29Z- TNavBF::mmse selected from filter -1 2021-02-12T23:03:29Z- variances([0], [2], and psi_berg) = 833.250000 833.250000 0.000000 2021-02-12T23:03:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:03:29Z- LcmTrn::publishEstimates() - MLE : 4056017.55 590290.74 1.03 2021-02-12T23:03:29Z- LcmTrn::publishEstimates() - MMSE : 4056067.05 590340.24 1.03 2021-02-12T23:03:29Z- LcmTrn::publishEstimates() - COVAR: 833.25 833.25 25.00 0.00 2021-02-12T23:03:29Z- AHRS_M2 msg: 1613171009.71 epoch sec; seqNo:358 2021-02-12T23:03:29Z- AHRS_M2 msg: 0.50 phi; -0.45 theta; 0.06 psi 2021-02-12T23:03:29Z- handleDepth()-Depth_Keller msg: 1613171009713 epoch sec; seqNo:361; depth 1.18 2021-02-12T23:03:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171009.72 epoch sec; seqNo:361; 4056067.43 north; 590340.24 east 2021-02-12T23:03:30Z- AHRS_M2 msg: 1613171010.11 epoch sec; seqNo:359 2021-02-12T23:03:30Z- AHRS_M2 msg: 0.50 phi; -0.47 theta; 0.14 psi 2021-02-12T23:03:30Z- handleDepth()-Depth_Keller msg: 1613171010119 epoch sec; seqNo:362; depth 1.31 2021-02-12T23:03:30Z- handleDvl() - RDI_Pathfinder msg: 1613171010.13 epoch sec; seqNo:156 2021-02-12T23:03:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:03:30Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:03:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171010.15 epoch sec; seqNo:362; 4056067.81 north; 590340.24 east 2021-02-12T23:03:30Z- AHRS_M2 msg: 1613171010.52 epoch sec; seqNo:360 2021-02-12T23:03:30Z- AHRS_M2 msg: 0.50 phi; -0.48 theta; 0.12 psi 2021-02-12T23:03:30Z- handleDepth()-Depth_Keller msg: 1613171010521 epoch sec; seqNo:363; depth 1.40 2021-02-12T23:03:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171010.53 epoch sec; seqNo:363; 4056068.19 north; 590340.23 east 2021-02-12T23:03:30Z- AHRS_M2 msg: 1613171010.92 epoch sec; seqNo:361 2021-02-12T23:03:30Z- AHRS_M2 msg: 0.50 phi; -0.49 theta; 0.03 psi 2021-02-12T23:03:30Z- handleDepth()-Depth_Keller msg: 1613171010924 epoch sec; seqNo:364; depth 1.45 2021-02-12T23:03:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171010.94 epoch sec; seqNo:364; 4056068.19 north; 590340.23 east 2021-02-12T23:03:31Z- AHRS_M2 msg: 1613171011.32 epoch sec; seqNo:362 2021-02-12T23:03:31Z- AHRS_M2 msg: 0.51 phi; -0.50 theta; -0.03 psi 2021-02-12T23:03:31Z- handleDepth()-Depth_Keller msg: 1613171011328 epoch sec; seqNo:365; depth 1.52 2021-02-12T23:03:31Z- handleDvl() - RDI_Pathfinder msg: 1613171011.34 epoch sec; seqNo:157 2021-02-12T23:03:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 114.10 , 100.70 , 81.35 , 0.00 2021-02-12T23:03:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.52 , -0.10 , -0.24 2021-02-12T23:03:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171011.37 epoch sec; seqNo:365; 4056068.57 north; 590340.23 east 2021-02-12T23:03:31Z- AHRS_M2 msg: 1613171011.73 epoch sec; seqNo:363 2021-02-12T23:03:31Z- AHRS_M2 msg: 0.51 phi; -0.50 theta; -0.07 psi 2021-02-12T23:03:31Z- handleDepth()-Depth_Keller msg: 1613171011737 epoch sec; seqNo:366; depth 1.64 2021-02-12T23:03:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171011.75 epoch sec; seqNo:366; 4056068.95 north; 590340.23 east 2021-02-12T23:03:32Z- AHRS_M2 msg: 1613171012.14 epoch sec; seqNo:364 2021-02-12T23:03:32Z- AHRS_M2 msg: 0.52 phi; -0.50 theta; -0.08 psi 2021-02-12T23:03:32Z- handleDepth()-Depth_Keller msg: 1613171012158 epoch sec; seqNo:367; depth 1.72 2021-02-12T23:03:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171012.19 epoch sec; seqNo:367; 4056068.96 north; 590341.45 east 2021-02-12T23:03:32Z- AHRS_M2 msg: 1613171012.54 epoch sec; seqNo:365 2021-02-12T23:03:32Z- AHRS_M2 msg: 0.54 phi; -0.50 theta; -0.07 psi 2021-02-12T23:03:32Z- handleDepth()-Depth_Keller msg: 1613171012540 epoch sec; seqNo:368; depth 1.86 2021-02-12T23:03:32Z- handleDvl() - RDI_Pathfinder msg: 1613171012.56 epoch sec; seqNo:158 2021-02-12T23:03:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 108.02 , 109.37 , 78.11 , 156.74 2021-02-12T23:03:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.09 , 0.10 2021-02-12T23:03:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171012.57 epoch sec; seqNo:368; 4056069.34 north; 590341.44 east 2021-02-12T23:03:32Z- AHRS_M2 msg: 1613171012.94 epoch sec; seqNo:366 2021-02-12T23:03:32Z- AHRS_M2 msg: 0.56 phi; -0.50 theta; -0.04 psi 2021-02-12T23:03:32Z- handleDepth()-Depth_Keller msg: 1613171012944 epoch sec; seqNo:369; depth 2.00 2021-02-12T23:03:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171012.95 epoch sec; seqNo:369; 4056069.34 north; 590341.44 east 2021-02-12T23:03:33Z- AHRS_M2 msg: 1613171013.34 epoch sec; seqNo:367 2021-02-12T23:03:33Z- AHRS_M2 msg: 0.56 phi; -0.49 theta; -0.01 psi 2021-02-12T23:03:33Z- handleDepth()-Depth_Keller msg: 1613171013348 epoch sec; seqNo:370; depth 2.13 2021-02-12T23:03:33Z- handleDvl() - RDI_Pathfinder msg: 1613171013.36 epoch sec; seqNo:159 2021-02-12T23:03:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 106.19 , 110.65 , 77.53 , 156.81 2021-02-12T23:03:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , 0.13 , -0.18 2021-02-12T23:03:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171013.37 epoch sec; seqNo:370; 4056069.72 north; 590341.44 east 2021-02-12T23:03:33Z- AHRS_M2 msg: 1613171013.75 epoch sec; seqNo:368 2021-02-12T23:03:33Z- AHRS_M2 msg: 0.56 phi; -0.49 theta; -0.01 psi 2021-02-12T23:03:33Z- handleDepth()-Depth_Keller msg: 1613171013753 epoch sec; seqNo:371; depth 2.26 2021-02-12T23:03:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171013.77 epoch sec; seqNo:371; 4056069.72 north; 590341.44 east 2021-02-12T23:03:34Z- AHRS_M2 msg: 1613171014.15 epoch sec; seqNo:369 2021-02-12T23:03:34Z- AHRS_M2 msg: 0.57 phi; -0.48 theta; -0.05 psi 2021-02-12T23:03:34Z- handleDepth()-Depth_Keller msg: 1613171014157 epoch sec; seqNo:372; depth 2.38 2021-02-12T23:03:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171014.18 epoch sec; seqNo:372; 4056070.10 north; 590341.43 east 2021-02-12T23:03:34Z- LcmTrn::handleMessages() - handling 40 messages took 0.67 ms and 127 calls... 2021-02-12T23:03:34Z- waiting for 1613171012.34, time is 1613171014.18 2021-02-12T23:03:34Z- LcmTrn::updateTrn() >>>> pose time 1613171014.18, meas time 1613171013.36 <<<< 2021-02-12T23:03:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171013.36 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:03:34Z- TerrainNav::motionUpdate() - currEstimate = (4056070, 590341.43, 2.38) 2021-02-12T23:03:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:03:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:03:34Z- TNavBF::Projecting Measurements 2021-02-12T23:03:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:03:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:03:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:03:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:03:34Z- TNavFilter::4 good beams. 2021-02-12T23:03:34Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:03:34Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:03:34Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:03:34Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:03:34Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:03:34Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:03:34Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:03:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:03:34Z- TerrainMapOctree: (vehN, vehE ) = (4056069.58, 590341.23). 2021-02-12T23:03:34Z- TerrainMapOctree: Min Distance = 6489.62. 2021-02-12T23:03:34Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:03:34Z- There is no specific good beam that all particles have in common. 2021-02-12T23:03:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:03:34Z- Old Depth Bias: 0.00 DepthBiasCov: 25.14 K: 0.560 ExpectedMeasDiff: nan currMeas.covariance: 18.04 2021-02-12T23:03:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:03:34Z- Old Depth Bias: 0.00 DepthBiasCov: 11.07 K: 0.342 ExpectedMeasDiff: nan currMeas.covariance: 19.59 2021-02-12T23:03:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:03:34Z- Old Depth Bias: 0.00 DepthBiasCov: 7.29 K: 0.391 ExpectedMeasDiff: nan currMeas.covariance: 9.62 2021-02-12T23:03:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:03:34Z- Old Depth Bias: 0.00 DepthBiasCov: 4.44 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 39.34 2021-02-12T23:03:34Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:03:34Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:03:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:03:34Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:03:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:03:34Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:03:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:03:34Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:03:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:03:34Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:03:34Z- Filter 0 No beams used in subcloud update 2021-02-12T23:03:34Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:03:34Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:03:34Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 832.8 EastVariance: 833.2 DepthVariance: 4.0 this->DepthBiasCov: 4.0 2021-02-12T23:03:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171013.36 sec. 2021-02-12T23:03:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:03:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:03:34Z- variances([0], [2], and psi_berg) = 832.559842 833.243753 0.000000 2021-02-12T23:03:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:03:34Z- LcmTrn::publishEstimates() - MLE : 4056022.60 590293.93 2.38 2021-02-12T23:03:34Z- LcmTrn::publishEstimates() - MMSE : 4056070.10 590341.43 2.38 2021-02-12T23:03:34Z- LcmTrn::publishEstimates() - COVAR: 832.56 833.24 4.03 0.00 2021-02-12T23:03:34Z- AHRS_M2 msg: 1613171014.56 epoch sec; seqNo:370 2021-02-12T23:03:34Z- AHRS_M2 msg: 0.58 phi; -0.47 theta; -0.08 psi 2021-02-12T23:03:34Z- handleDepth()-Depth_Keller msg: 1613171014560 epoch sec; seqNo:373; depth 2.51 2021-02-12T23:03:34Z- handleDvl() - RDI_Pathfinder msg: 1613171014.57 epoch sec; seqNo:160 2021-02-12T23:03:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 107.68 , 104.70 , 77.97 , 149.36 2021-02-12T23:03:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.03 , -0.34 2021-02-12T23:03:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171014.59 epoch sec; seqNo:373; 4056070.47 north; 590341.43 east 2021-02-12T23:03:34Z- AHRS_M2 msg: 1613171014.96 epoch sec; seqNo:371 2021-02-12T23:03:34Z- AHRS_M2 msg: 0.60 phi; -0.47 theta; -0.09 psi 2021-02-12T23:03:34Z- handleDepth()-Depth_Keller msg: 1613171014964 epoch sec; seqNo:374; depth 2.63 2021-02-12T23:03:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171014.97 epoch sec; seqNo:374; 4056070.47 north; 590341.43 east 2021-02-12T23:03:35Z- AHRS_M2 msg: 1613171015.37 epoch sec; seqNo:372 2021-02-12T23:03:35Z- AHRS_M2 msg: 0.62 phi; -0.48 theta; -0.09 psi 2021-02-12T23:03:35Z- handleDepth()-Depth_Keller msg: 1613171015369 epoch sec; seqNo:375; depth 2.76 2021-02-12T23:03:35Z- handleDvl() - RDI_Pathfinder msg: 1613171015.38 epoch sec; seqNo:161 2021-02-12T23:03:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 100.23 , 109.17 , 79.30 , 149.36 2021-02-12T23:03:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.60 , -0.25 , 0.13 2021-02-12T23:03:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171015.39 epoch sec; seqNo:375; 4056070.85 north; 590341.43 east 2021-02-12T23:03:35Z- AHRS_M2 msg: 1613171015.77 epoch sec; seqNo:373 2021-02-12T23:03:35Z- AHRS_M2 msg: 0.64 phi; -0.49 theta; -0.07 psi 2021-02-12T23:03:35Z- handleDepth()-Depth_Keller msg: 1613171015772 epoch sec; seqNo:376; depth 2.92 2021-02-12T23:03:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171015.79 epoch sec; seqNo:376; 4056070.85 north; 590341.43 east 2021-02-12T23:03:36Z- AHRS_M2 msg: 1613171016.18 epoch sec; seqNo:374 2021-02-12T23:03:36Z- AHRS_M2 msg: 0.65 phi; -0.50 theta; -0.04 psi 2021-02-12T23:03:36Z- handleDepth()-Depth_Keller msg: 1613171016180 epoch sec; seqNo:377; depth 3.03 2021-02-12T23:03:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171016.23 epoch sec; seqNo:377; 4056071.23 north; 590341.42 east 2021-02-12T23:03:36Z- AHRS_M2 msg: 1613171016.58 epoch sec; seqNo:375 2021-02-12T23:03:36Z- AHRS_M2 msg: 0.65 phi; -0.50 theta; -0.03 psi 2021-02-12T23:03:36Z- handleDepth()-Depth_Keller msg: 1613171016580 epoch sec; seqNo:378; depth 3.17 2021-02-12T23:03:36Z- handleDvl() - RDI_Pathfinder msg: 1613171016.59 epoch sec; seqNo:162 2021-02-12T23:03:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.78 , 107.74 , 77.08 , 147.93 2021-02-12T23:03:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.41 , -0.12 , 0.23 2021-02-12T23:03:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171016.60 epoch sec; seqNo:378; 4056071.23 north; 590341.42 east 2021-02-12T23:03:36Z- AHRS_M2 msg: 1613171016.98 epoch sec; seqNo:376 2021-02-12T23:03:36Z- AHRS_M2 msg: 0.65 phi; -0.50 theta; -0.03 psi 2021-02-12T23:03:36Z- handleDepth()-Depth_Keller msg: 1613171016986 epoch sec; seqNo:379; depth 3.31 2021-02-12T23:03:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171017.00 epoch sec; seqNo:379; 4056071.61 north; 590341.42 east 2021-02-12T23:03:37Z- AHRS_M2 msg: 1613171017.38 epoch sec; seqNo:377 2021-02-12T23:03:37Z- AHRS_M2 msg: 0.65 phi; -0.50 theta; -0.04 psi 2021-02-12T23:03:37Z- handleDepth()-Depth_Keller msg: 1613171017389 epoch sec; seqNo:380; depth 3.41 2021-02-12T23:03:37Z- handleDvl() - RDI_Pathfinder msg: 1613171017.41 epoch sec; seqNo:163 2021-02-12T23:03:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.34 , 104.83 , 76.88 , 149.49 2021-02-12T23:03:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.58 , -0.03 , 0.11 2021-02-12T23:03:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171017.42 epoch sec; seqNo:380; 4056071.62 north; 590342.64 east 2021-02-12T23:03:37Z- AHRS_M2 msg: 1613171017.79 epoch sec; seqNo:378 2021-02-12T23:03:37Z- AHRS_M2 msg: 0.64 phi; -0.50 theta; -0.06 psi 2021-02-12T23:03:37Z- handleDepth()-Depth_Keller msg: 1613171017793 epoch sec; seqNo:381; depth 3.55 2021-02-12T23:03:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171017.80 epoch sec; seqNo:381; 4056071.62 north; 590342.64 east 2021-02-12T23:03:38Z- AHRS_M2 msg: 1613171018.21 epoch sec; seqNo:379 2021-02-12T23:03:38Z- AHRS_M2 msg: 0.65 phi; -0.50 theta; -0.07 psi 2021-02-12T23:03:38Z- handleDepth()-Depth_Keller msg: 1613171018223 epoch sec; seqNo:382; depth 3.68 2021-02-12T23:03:38Z- handleDvl() - RDI_Pathfinder msg: 1613171018.23 epoch sec; seqNo:164 2021-02-12T23:03:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.35 , 107.81 , 78.11 , 152.48 2021-02-12T23:03:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.05 , 0.12 2021-02-12T23:03:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171018.26 epoch sec; seqNo:382; 4056072.00 north; 590342.64 east 2021-02-12T23:03:38Z- AHRS_M2 msg: 1613171018.60 epoch sec; seqNo:380 2021-02-12T23:03:38Z- AHRS_M2 msg: 0.65 phi; -0.51 theta; -0.08 psi 2021-02-12T23:03:38Z- handleDepth()-Depth_Keller msg: 1613171018600 epoch sec; seqNo:383; depth 3.83 2021-02-12T23:03:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171018.62 epoch sec; seqNo:383; 4056072.00 north; 590342.64 east 2021-02-12T23:03:39Z- AHRS_M2 msg: 1613171019.00 epoch sec; seqNo:381 2021-02-12T23:03:39Z- AHRS_M2 msg: 0.65 phi; -0.51 theta; -0.09 psi 2021-02-12T23:03:39Z- handleDepth()-Depth_Keller msg: 1613171019004 epoch sec; seqNo:384; depth 3.99 2021-02-12T23:03:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171019.01 epoch sec; seqNo:384; 4056072.38 north; 590342.63 east 2021-02-12T23:03:39Z- AHRS_M2 msg: 1613171019.40 epoch sec; seqNo:382 2021-02-12T23:03:39Z- AHRS_M2 msg: 0.66 phi; -0.51 theta; -0.09 psi 2021-02-12T23:03:39Z- handleDepth()-Depth_Keller msg: 1613171019408 epoch sec; seqNo:385; depth 4.14 2021-02-12T23:03:39Z- handleDvl() - RDI_Pathfinder msg: 1613171019.42 epoch sec; seqNo:165 2021-02-12T23:03:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.21 , 109.17 , 76.65 , 155.32 2021-02-12T23:03:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.07 , -0.09 2021-02-12T23:03:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171019.43 epoch sec; seqNo:385; 4056072.38 north; 590342.63 east 2021-02-12T23:03:39Z- LcmTrn::handleMessages() - handling 45 messages took 0.67 ms and 132 calls... 2021-02-12T23:03:39Z- waiting for 1613171017.18, time is 1613171019.43 2021-02-12T23:03:39Z- LcmTrn::updateTrn() equating pose 1613171019.43 and meas times 1613171019.43 2021-02-12T23:03:39Z- LcmTrn::updateTrn() >>>> pose time 1613171019.43, meas time 1613171019.43 <<<< 2021-02-12T23:03:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171019.43 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:03:39Z- TerrainNav::motionUpdate() - currEstimate = (4056072, 590342.63, 4.14) 2021-02-12T23:03:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:03:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:03:39Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171019.43 sec.; No relevant navigation data available 2021-02-12T23:03:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:03:39Z- TNavBF::mmse selected from filter -1 2021-02-12T23:03:39Z- variances([0], [2], and psi_berg) = 832.104102 833.239754 0.000000 2021-02-12T23:03:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:03:39Z- LcmTrn::publishEstimates() - MLE : 4056025.88 590296.13 4.14 2021-02-12T23:03:39Z- LcmTrn::publishEstimates() - MMSE : 4056072.38 590342.63 4.14 2021-02-12T23:03:39Z- LcmTrn::publishEstimates() - COVAR: 832.10 833.24 4.16 0.00 2021-02-12T23:03:39Z- AHRS_M2 msg: 1613171019.81 epoch sec; seqNo:383 2021-02-12T23:03:39Z- AHRS_M2 msg: 0.66 phi; -0.50 theta; -0.10 psi 2021-02-12T23:03:39Z- handleDepth()-Depth_Keller msg: 1613171019814 epoch sec; seqNo:386; depth 4.34 2021-02-12T23:03:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171019.83 epoch sec; seqNo:386; 4056072.76 north; 590342.63 east 2021-02-12T23:03:40Z- AHRS_M2 msg: 1613171020.24 epoch sec; seqNo:384 2021-02-12T23:03:40Z- AHRS_M2 msg: 0.67 phi; -0.49 theta; -0.11 psi 2021-02-12T23:03:40Z- handleDepth()-Depth_Keller msg: 1613171020248 epoch sec; seqNo:387; depth 4.49 2021-02-12T23:03:40Z- handleDvl() - RDI_Pathfinder msg: 1613171020.30 epoch sec; seqNo:166 2021-02-12T23:03:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 100.23 , 109.16 , 77.08 , 152.33 2021-02-12T23:03:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.18 , -0.22 2021-02-12T23:03:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171020.32 epoch sec; seqNo:387; 4056073.15 north; 590343.84 east 2021-02-12T23:03:40Z- AHRS_M2 msg: 1613171020.62 epoch sec; seqNo:385 2021-02-12T23:03:40Z- AHRS_M2 msg: 0.68 phi; -0.49 theta; -0.13 psi 2021-02-12T23:03:40Z- handleDepth()-Depth_Keller msg: 1613171020620 epoch sec; seqNo:388; depth 4.67 2021-02-12T23:03:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171020.63 epoch sec; seqNo:388; 4056073.53 north; 590343.84 east 2021-02-12T23:03:41Z- AHRS_M2 msg: 1613171021.02 epoch sec; seqNo:386 2021-02-12T23:03:41Z- AHRS_M2 msg: 0.69 phi; -0.49 theta; -0.13 psi 2021-02-12T23:03:41Z- handleDepth()-Depth_Keller msg: 1613171021024 epoch sec; seqNo:389; depth 4.79 2021-02-12T23:03:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171021.03 epoch sec; seqNo:389; 4056073.91 north; 590343.84 east 2021-02-12T23:03:41Z- AHRS_M2 msg: 1613171021.43 epoch sec; seqNo:387 2021-02-12T23:03:41Z- AHRS_M2 msg: 0.69 phi; -0.50 theta; -0.12 psi 2021-02-12T23:03:41Z- handleDepth()-Depth_Keller msg: 1613171021430 epoch sec; seqNo:390; depth 4.97 2021-02-12T23:03:41Z- handleDvl() - RDI_Pathfinder msg: 1613171021.44 epoch sec; seqNo:167 2021-02-12T23:03:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.83 , 109.23 , 77.97 , 144.95 2021-02-12T23:03:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.26 , 0.08 2021-02-12T23:03:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171021.45 epoch sec; seqNo:390; 4056073.91 north; 590343.84 east 2021-02-12T23:03:41Z- AHRS_M2 msg: 1613171021.83 epoch sec; seqNo:388 2021-02-12T23:03:41Z- AHRS_M2 msg: 0.69 phi; -0.51 theta; -0.12 psi 2021-02-12T23:03:41Z- handleDepth()-Depth_Keller msg: 1613171021832 epoch sec; seqNo:391; depth 5.14 2021-02-12T23:03:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171021.85 epoch sec; seqNo:391; 4056074.29 north; 590343.83 east 2021-02-12T23:03:42Z- AHRS_M2 msg: 1613171022.23 epoch sec; seqNo:389 2021-02-12T23:03:42Z- AHRS_M2 msg: 0.68 phi; -0.53 theta; -0.11 psi 2021-02-12T23:03:42Z- handleDepth()-Depth_Keller msg: 1613171022237 epoch sec; seqNo:392; depth 5.29 2021-02-12T23:03:42Z- handleDvl() - RDI_Pathfinder msg: 1613171022.25 epoch sec; seqNo:168 2021-02-12T23:03:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.03 , 110.92 , 76.88 , 155.58 2021-02-12T23:03:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.60 , 0.04 , 0.30 2021-02-12T23:03:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171022.26 epoch sec; seqNo:392; 4056074.29 north; 590343.83 east 2021-02-12T23:03:42Z- AHRS_M2 msg: 1613171022.64 epoch sec; seqNo:390 2021-02-12T23:03:42Z- AHRS_M2 msg: 0.68 phi; -0.53 theta; -0.12 psi 2021-02-12T23:03:42Z- handleDepth()-Depth_Keller msg: 1613171022652 epoch sec; seqNo:393; depth 5.46 2021-02-12T23:03:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171022.68 epoch sec; seqNo:393; 4056074.29 north; 590343.83 east 2021-02-12T23:03:43Z- AHRS_M2 msg: 1613171023.04 epoch sec; seqNo:391 2021-02-12T23:03:43Z- AHRS_M2 msg: 0.68 phi; -0.54 theta; -0.13 psi 2021-02-12T23:03:43Z- handleDepth()-Depth_Keller msg: 1613171023064 epoch sec; seqNo:394; depth 5.62 2021-02-12T23:03:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171023.07 epoch sec; seqNo:394; 4056074.67 north; 590343.83 east 2021-02-12T23:03:43Z- AHRS_M2 msg: 1613171023.44 epoch sec; seqNo:392 2021-02-12T23:03:43Z- AHRS_M2 msg: 0.68 phi; -0.54 theta; -0.14 psi 2021-02-12T23:03:43Z- handleDepth()-Depth_Keller msg: 1613171023453 epoch sec; seqNo:395; depth 5.80 2021-02-12T23:03:43Z- handleDvl() - RDI_Pathfinder msg: 1613171023.47 epoch sec; seqNo:169 2021-02-12T23:03:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.76 , 112.14 , 77.97 , 150.84 2021-02-12T23:03:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , 0.28 , 0.15 2021-02-12T23:03:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171023.48 epoch sec; seqNo:395; 4056074.67 north; 590343.83 east 2021-02-12T23:03:43Z- AHRS_M2 msg: 1613171023.85 epoch sec; seqNo:393 2021-02-12T23:03:43Z- AHRS_M2 msg: 0.68 phi; -0.53 theta; -0.15 psi 2021-02-12T23:03:43Z- handleDepth()-Depth_Keller msg: 1613171023858 epoch sec; seqNo:396; depth 5.98 2021-02-12T23:03:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171023.87 epoch sec; seqNo:396; 4056074.68 north; 590345.05 east 2021-02-12T23:03:44Z- AHRS_M2 msg: 1613171024.25 epoch sec; seqNo:394 2021-02-12T23:03:44Z- AHRS_M2 msg: 0.68 phi; -0.52 theta; -0.15 psi 2021-02-12T23:03:44Z- handleDepth()-Depth_Keller msg: 1613171024258 epoch sec; seqNo:397; depth 6.15 2021-02-12T23:03:44Z- handleDvl() - RDI_Pathfinder msg: 1613171024.27 epoch sec; seqNo:170 2021-02-12T23:03:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.83 , 112.21 , 77.09 , 161.34 2021-02-12T23:03:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.28 , -0.16 2021-02-12T23:03:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171024.28 epoch sec; seqNo:397; 4056075.06 north; 590345.05 east 2021-02-12T23:03:44Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-12T23:03:44Z- waiting for 1613171022.43, time is 1613171024.28 2021-02-12T23:03:44Z- LcmTrn::updateTrn() equating pose 1613171024.28 and meas times 1613171024.28 2021-02-12T23:03:44Z- LcmTrn::updateTrn() >>>> pose time 1613171024.28, meas time 1613171024.28 <<<< 2021-02-12T23:03:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171024.28 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:03:44Z- TerrainNav::motionUpdate() - currEstimate = (4056075, 590345.05, 6.15) 2021-02-12T23:03:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:03:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:03:44Z- TNavBF::Projecting Measurements 2021-02-12T23:03:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:03:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:03:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:03:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:03:44Z- TNavFilter::4 good beams. 2021-02-12T23:03:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:03:44Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:03:44Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:03:44Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:03:44Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:03:44Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:03:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:03:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:03:44Z- TerrainMapOctree: (vehN, vehE ) = (4056075.06, 590345.05). 2021-02-12T23:03:44Z- TerrainMapOctree: Min Distance = 6482.95. 2021-02-12T23:03:44Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:03:44Z- There is no specific good beam that all particles have in common. 2021-02-12T23:03:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:03:44Z- Old Depth Bias: 0.00 DepthBiasCov: 4.35 K: 0.209 ExpectedMeasDiff: nan currMeas.covariance: 14.69 2021-02-12T23:03:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:03:44Z- Old Depth Bias: 0.00 DepthBiasCov: 3.44 K: 0.136 ExpectedMeasDiff: nan currMeas.covariance: 20.15 2021-02-12T23:03:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:03:44Z- Old Depth Bias: 0.00 DepthBiasCov: 2.97 K: 0.209 ExpectedMeasDiff: nan currMeas.covariance: 9.51 2021-02-12T23:03:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:03:44Z- Old Depth Bias: 0.00 DepthBiasCov: 2.35 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 41.65 2021-02-12T23:03:44Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:03:44Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:03:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:03:44Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:03:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:03:44Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:03:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:03:44Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:03:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:03:44Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:03:44Z- Filter 0 No beams used in subcloud update 2021-02-12T23:03:44Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:03:44Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:03:44Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 831.6 EastVariance: 833.2 DepthVariance: 2.2 this->DepthBiasCov: 2.2 2021-02-12T23:03:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171024.28 sec. 2021-02-12T23:03:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:03:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:03:44Z- variances([0], [2], and psi_berg) = 831.573196 833.235216 0.000000 2021-02-12T23:03:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:03:44Z- LcmTrn::publishEstimates() - MLE : 4056029.56 590299.55 6.15 2021-02-12T23:03:44Z- LcmTrn::publishEstimates() - MMSE : 4056075.06 590345.05 6.15 2021-02-12T23:03:44Z- LcmTrn::publishEstimates() - COVAR: 831.57 833.24 2.23 0.00 2021-02-12T23:03:44Z- AHRS_M2 msg: 1613171024.66 epoch sec; seqNo:395 2021-02-12T23:03:44Z- AHRS_M2 msg: 0.68 phi; -0.51 theta; -0.17 psi 2021-02-12T23:03:44Z- handleDepth()-Depth_Keller msg: 1613171024660 epoch sec; seqNo:398; depth 6.34 2021-02-12T23:03:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171024.68 epoch sec; seqNo:398; 4056075.06 north; 590345.05 east 2021-02-12T23:03:45Z- AHRS_M2 msg: 1613171025.06 epoch sec; seqNo:396 2021-02-12T23:03:45Z- AHRS_M2 msg: 0.69 phi; -0.49 theta; -0.18 psi 2021-02-12T23:03:45Z- handleDepth()-Depth_Keller msg: 1613171025064 epoch sec; seqNo:399; depth 6.53 2021-02-12T23:03:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171025.07 epoch sec; seqNo:399; 4056075.44 north; 590345.04 east 2021-02-12T23:03:45Z- AHRS_M2 msg: 1613171025.46 epoch sec; seqNo:397 2021-02-12T23:03:45Z- AHRS_M2 msg: 0.69 phi; -0.48 theta; -0.19 psi 2021-02-12T23:03:45Z- handleDepth()-Depth_Keller msg: 1613171025468 epoch sec; seqNo:400; depth 6.71 2021-02-12T23:03:45Z- handleDvl() - RDI_Pathfinder msg: 1613171025.48 epoch sec; seqNo:171 2021-02-12T23:03:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.77 , 112.14 , 74.87 , 149.36 2021-02-12T23:03:45Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.05 , -0.35 2021-02-12T23:03:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171025.49 epoch sec; seqNo:400; 4056075.83 north; 590346.26 east 2021-02-12T23:03:45Z- AHRS_M2 msg: 1613171025.87 epoch sec; seqNo:398 2021-02-12T23:03:45Z- AHRS_M2 msg: 0.71 phi; -0.48 theta; -0.19 psi 2021-02-12T23:03:45Z- handleDepth()-Depth_Keller msg: 1613171025874 epoch sec; seqNo:401; depth 6.92 2021-02-12T23:03:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171025.88 epoch sec; seqNo:401; 4056076.21 north; 590346.25 east 2021-02-12T23:03:46Z- AHRS_M2 msg: 1613171026.27 epoch sec; seqNo:399 2021-02-12T23:03:46Z- AHRS_M2 msg: 0.72 phi; -0.48 theta; -0.18 psi 2021-02-12T23:03:46Z- handleDepth()-Depth_Keller msg: 1613171026276 epoch sec; seqNo:402; depth 7.12 2021-02-12T23:03:46Z- handleDvl() - RDI_Pathfinder msg: 1613171026.29 epoch sec; seqNo:172 2021-02-12T23:03:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.50 , 113.83 , 76.47 , 145.09 2021-02-12T23:03:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.18 , -0.20 2021-02-12T23:03:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171026.30 epoch sec; seqNo:402; 4056076.59 north; 590346.25 east 2021-02-12T23:03:46Z- AHRS_M2 msg: 1613171026.68 epoch sec; seqNo:400 2021-02-12T23:03:46Z- AHRS_M2 msg: 0.72 phi; -0.48 theta; -0.16 psi 2021-02-12T23:03:46Z- handleDepth()-Depth_Keller msg: 1613171026680 epoch sec; seqNo:403; depth 7.31 2021-02-12T23:03:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171026.69 epoch sec; seqNo:403; 4056076.59 north; 590346.25 east 2021-02-12T23:03:47Z- AHRS_M2 msg: 1613171027.08 epoch sec; seqNo:401 2021-02-12T23:03:47Z- AHRS_M2 msg: 0.72 phi; -0.48 theta; -0.14 psi 2021-02-12T23:03:47Z- handleDepth()-Depth_Keller msg: 1613171027084 epoch sec; seqNo:404; depth 7.49 2021-02-12T23:03:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171027.09 epoch sec; seqNo:404; 4056076.97 north; 590346.25 east 2021-02-12T23:03:47Z- AHRS_M2 msg: 1613171027.49 epoch sec; seqNo:402 2021-02-12T23:03:47Z- AHRS_M2 msg: 0.72 phi; -0.48 theta; -0.12 psi 2021-02-12T23:03:47Z- handleDepth()-Depth_Keller msg: 1613171027519 epoch sec; seqNo:405; depth 7.68 2021-02-12T23:03:47Z- handleDvl() - RDI_Pathfinder msg: 1613171027.55 epoch sec; seqNo:173 2021-02-12T23:03:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.57 , 109.43 , 75.65 , 145.16 2021-02-12T23:03:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.09 , -0.03 2021-02-12T23:03:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171027.56 epoch sec; seqNo:405; 4056077.34 north; 590346.24 east 2021-02-12T23:03:47Z- AHRS_M2 msg: 1613171027.89 epoch sec; seqNo:403 2021-02-12T23:03:47Z- AHRS_M2 msg: 0.71 phi; -0.48 theta; -0.11 psi 2021-02-12T23:03:47Z- handleDepth()-Depth_Keller msg: 1613171027892 epoch sec; seqNo:406; depth 7.88 2021-02-12T23:03:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171027.91 epoch sec; seqNo:406; 4056077.72 north; 590346.24 east 2021-02-12T23:03:48Z- AHRS_M2 msg: 1613171028.29 epoch sec; seqNo:404 2021-02-12T23:03:48Z- AHRS_M2 msg: 0.71 phi; -0.48 theta; -0.12 psi 2021-02-12T23:03:48Z- handleDepth()-Depth_Keller msg: 1613171028296 epoch sec; seqNo:407; depth 8.04 2021-02-12T23:03:48Z- handleDvl() - RDI_Pathfinder msg: 1613171028.31 epoch sec; seqNo:174 2021-02-12T23:03:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.19 , 103.62 , 74.02 , 134.88 2021-02-12T23:03:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.11 , -0.10 2021-02-12T23:03:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171028.32 epoch sec; seqNo:407; 4056077.74 north; 590347.46 east 2021-02-12T23:03:48Z- AHRS_M2 msg: 1613171028.70 epoch sec; seqNo:405 2021-02-12T23:03:48Z- AHRS_M2 msg: 0.71 phi; -0.46 theta; -0.14 psi 2021-02-12T23:03:48Z- handleDepth()-Depth_Keller msg: 1613171028702 epoch sec; seqNo:408; depth 8.20 2021-02-12T23:03:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171028.72 epoch sec; seqNo:408; 4056078.12 north; 590347.46 east 2021-02-12T23:03:49Z- AHRS_M2 msg: 1613171029.10 epoch sec; seqNo:406 2021-02-12T23:03:49Z- AHRS_M2 msg: 0.70 phi; -0.45 theta; -0.17 psi 2021-02-12T23:03:49Z- handleDepth()-Depth_Keller msg: 1613171029126 epoch sec; seqNo:409; depth 8.36 2021-02-12T23:03:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171029.16 epoch sec; seqNo:409; 4056078.12 north; 590347.46 east 2021-02-12T23:03:49Z- AHRS_M2 msg: 1613171029.50 epoch sec; seqNo:407 2021-02-12T23:03:49Z- AHRS_M2 msg: 0.70 phi; -0.44 theta; -0.19 psi 2021-02-12T23:03:49Z- handleDepth()-Depth_Keller msg: 1613171029508 epoch sec; seqNo:410; depth 8.58 2021-02-12T23:03:49Z- handleDvl() - RDI_Pathfinder msg: 1613171029.52 epoch sec; seqNo:175 2021-02-12T23:03:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.56 , 105.44 , 73.61 , 138.20 2021-02-12T23:03:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , -0.27 2021-02-12T23:03:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171029.54 epoch sec; seqNo:410; 4056078.49 north; 590347.45 east 2021-02-12T23:03:49Z- LcmTrn::handleMessages() - handling 44 messages took 0.73 ms and 130 calls... 2021-02-12T23:03:49Z- waiting for 1613171027.28, time is 1613171029.54 2021-02-12T23:03:49Z- LcmTrn::updateTrn() equating pose 1613171029.54 and meas times 1613171029.54 2021-02-12T23:03:49Z- LcmTrn::updateTrn() >>>> pose time 1613171029.54, meas time 1613171029.54 <<<< 2021-02-12T23:03:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171029.54 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:03:49Z- TerrainNav::motionUpdate() - currEstimate = (4056078, 590347.45, 8.58) 2021-02-12T23:03:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:03:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:03:49Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171029.54 sec.; No relevant navigation data available 2021-02-12T23:03:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:03:49Z- TNavBF::mmse selected from filter -1 2021-02-12T23:03:49Z- variances([0], [2], and psi_berg) = 831.010241 833.230549 0.000000 2021-02-12T23:03:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:03:49Z- LcmTrn::publishEstimates() - MLE : 4056033.99 590302.95 8.58 2021-02-12T23:03:49Z- LcmTrn::publishEstimates() - MMSE : 4056078.49 590347.45 8.58 2021-02-12T23:03:49Z- LcmTrn::publishEstimates() - COVAR: 831.01 833.23 2.44 0.00 2021-02-12T23:03:49Z- AHRS_M2 msg: 1613171029.91 epoch sec; seqNo:408 2021-02-12T23:03:49Z- AHRS_M2 msg: 0.70 phi; -0.43 theta; -0.21 psi 2021-02-12T23:03:49Z- handleDepth()-Depth_Keller msg: 1613171029912 epoch sec; seqNo:411; depth 8.74 2021-02-12T23:03:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171029.92 epoch sec; seqNo:411; 4056078.87 north; 590347.45 east 2021-02-12T23:03:50Z- AHRS_M2 msg: 1613171030.31 epoch sec; seqNo:409 2021-02-12T23:03:50Z- AHRS_M2 msg: 0.70 phi; -0.42 theta; -0.21 psi 2021-02-12T23:03:50Z- handleDepth()-Depth_Keller msg: 1613171030317 epoch sec; seqNo:412; depth 8.90 2021-02-12T23:03:50Z- handleDvl() - RDI_Pathfinder msg: 1613171030.33 epoch sec; seqNo:176 2021-02-12T23:03:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 108.42 , 75.25 , 130.75 2021-02-12T23:03:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.29 , -0.18 2021-02-12T23:03:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171030.34 epoch sec; seqNo:412; 4056079.26 north; 590348.67 east 2021-02-12T23:03:50Z- AHRS_M2 msg: 1613171030.72 epoch sec; seqNo:410 2021-02-12T23:03:50Z- AHRS_M2 msg: 0.70 phi; -0.41 theta; -0.21 psi 2021-02-12T23:03:50Z- handleDepth()-Depth_Keller msg: 1613171030720 epoch sec; seqNo:413; depth 9.09 2021-02-12T23:03:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171030.74 epoch sec; seqNo:413; 4056079.26 north; 590348.67 east 2021-02-12T23:03:51Z- AHRS_M2 msg: 1613171031.15 epoch sec; seqNo:411 2021-02-12T23:03:51Z- AHRS_M2 msg: 0.69 phi; -0.41 theta; -0.21 psi 2021-02-12T23:03:51Z- handleDepth()-Depth_Keller msg: 1613171031155 epoch sec; seqNo:414; depth 9.26 2021-02-12T23:03:51Z- handleDvl() - RDI_Pathfinder msg: 1613171031.17 epoch sec; seqNo:177 2021-02-12T23:03:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.33 , 108.15 , 75.25 , 124.53 2021-02-12T23:03:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.29 , 0.10 2021-02-12T23:03:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171031.18 epoch sec; seqNo:414; 4056079.64 north; 590348.66 east 2021-02-12T23:03:51Z- AHRS_M2 msg: 1613171031.52 epoch sec; seqNo:412 2021-02-12T23:03:51Z- AHRS_M2 msg: 0.69 phi; -0.40 theta; -0.20 psi 2021-02-12T23:03:51Z- handleDepth()-Depth_Keller msg: 1613171031528 epoch sec; seqNo:415; depth 9.43 2021-02-12T23:03:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171031.54 epoch sec; seqNo:415; 4056080.02 north; 590348.66 east 2021-02-12T23:03:51Z- AHRS_M2 msg: 1613171031.93 epoch sec; seqNo:413 2021-02-12T23:03:51Z- AHRS_M2 msg: 0.68 phi; -0.40 theta; -0.20 psi 2021-02-12T23:03:51Z- handleDepth()-Depth_Keller msg: 1613171031934 epoch sec; seqNo:416; depth 9.56 2021-02-12T23:03:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171031.94 epoch sec; seqNo:416; 4056080.02 north; 590348.66 east 2021-02-12T23:03:52Z- AHRS_M2 msg: 1613171032.33 epoch sec; seqNo:414 2021-02-12T23:03:52Z- AHRS_M2 msg: 0.66 phi; -0.39 theta; -0.19 psi 2021-02-12T23:03:52Z- handleDepth()-Depth_Keller msg: 1613171032336 epoch sec; seqNo:417; depth 9.72 2021-02-12T23:03:52Z- handleDvl() - RDI_Pathfinder msg: 1613171032.35 epoch sec; seqNo:178 2021-02-12T23:03:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.07 , 106.73 , 74.84 , 124.32 2021-02-12T23:03:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.11 , 0.27 2021-02-12T23:03:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171032.36 epoch sec; seqNo:417; 4056080.40 north; 590348.65 east 2021-02-12T23:03:52Z- AHRS_M2 msg: 1613171032.74 epoch sec; seqNo:415 2021-02-12T23:03:52Z- AHRS_M2 msg: 0.66 phi; -0.39 theta; -0.18 psi 2021-02-12T23:03:52Z- handleDepth()-Depth_Keller msg: 1613171032745 epoch sec; seqNo:418; depth 9.86 2021-02-12T23:03:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171032.76 epoch sec; seqNo:418; 4056080.40 north; 590348.65 east 2021-02-12T23:03:53Z- AHRS_M2 msg: 1613171033.16 epoch sec; seqNo:416 2021-02-12T23:03:53Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.18 psi 2021-02-12T23:03:53Z- handleDepth()-Depth_Keller msg: 1613171033170 epoch sec; seqNo:419; depth 9.99 2021-02-12T23:03:53Z- handleDvl() - RDI_Pathfinder msg: 1613171033.19 epoch sec; seqNo:179 2021-02-12T23:03:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.07 , 95.90 , 73.61 , 122.97 2021-02-12T23:03:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , 0.10 , 0.25 2021-02-12T23:03:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171033.20 epoch sec; seqNo:419; 4056080.78 north; 590348.65 east 2021-02-12T23:03:53Z- AHRS_M2 msg: 1613171033.54 epoch sec; seqNo:417 2021-02-12T23:03:53Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.18 psi 2021-02-12T23:03:53Z- handleDepth()-Depth_Keller msg: 1613171033550 epoch sec; seqNo:420; depth 10.15 2021-02-12T23:03:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171033.56 epoch sec; seqNo:420; 4056080.78 north; 590348.65 east 2021-02-12T23:03:53Z- AHRS_M2 msg: 1613171033.95 epoch sec; seqNo:418 2021-02-12T23:03:53Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.18 psi 2021-02-12T23:03:53Z- handleDepth()-Depth_Keller msg: 1613171033952 epoch sec; seqNo:421; depth 10.26 2021-02-12T23:03:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171033.97 epoch sec; seqNo:421; 4056081.16 north; 590348.65 east 2021-02-12T23:03:54Z- AHRS_M2 msg: 1613171034.35 epoch sec; seqNo:419 2021-02-12T23:03:54Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.18 psi 2021-02-12T23:03:54Z- handleDepth()-Depth_Keller msg: 1613171034356 epoch sec; seqNo:422; depth 10.40 2021-02-12T23:03:54Z- handleDvl() - RDI_Pathfinder msg: 1613171034.37 epoch sec; seqNo:180 2021-02-12T23:03:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.35 , 102.94 , 74.02 , 119.18 2021-02-12T23:03:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.22 , 0.08 2021-02-12T23:03:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171034.39 epoch sec; seqNo:422; 4056081.17 north; 590349.87 east 2021-02-12T23:03:54Z- LcmTrn::handleMessages() - handling 41 messages took 0.69 ms and 128 calls... 2021-02-12T23:03:54Z- waiting for 1613171032.54, time is 1613171034.39 2021-02-12T23:03:54Z- LcmTrn::updateTrn() equating pose 1613171034.39 and meas times 1613171034.39 2021-02-12T23:03:54Z- LcmTrn::updateTrn() >>>> pose time 1613171034.39, meas time 1613171034.39 <<<< 2021-02-12T23:03:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171034.39 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:03:54Z- TerrainNav::motionUpdate() - currEstimate = (4056081, 590349.87, 10.40) 2021-02-12T23:03:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:03:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:03:54Z- TNavBF::Projecting Measurements 2021-02-12T23:03:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:03:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:03:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:03:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:03:54Z- TNavFilter::4 good beams. 2021-02-12T23:03:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:03:54Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:03:54Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:03:54Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:03:54Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:03:54Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:03:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:03:54Z- TNavBF::mmse selected from filter -1 2021-02-12T23:03:54Z- TerrainMapOctree: (vehN, vehE ) = (4056081.17, 590349.87). 2021-02-12T23:03:54Z- TerrainMapOctree: Min Distance = 6475.17. 2021-02-12T23:03:54Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:03:54Z- There is no specific good beam that all particles have in common. 2021-02-12T23:03:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:03:54Z- Old Depth Bias: 0.00 DepthBiasCov: 2.63 K: 0.164 ExpectedMeasDiff: nan currMeas.covariance: 11.65 2021-02-12T23:03:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:03:54Z- Old Depth Bias: 0.00 DepthBiasCov: 2.20 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 16.95 2021-02-12T23:03:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:03:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.96 K: 0.158 ExpectedMeasDiff: nan currMeas.covariance: 8.77 2021-02-12T23:03:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:03:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.66 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 22.73 2021-02-12T23:03:54Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:03:54Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:03:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:03:54Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:03:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:03:54Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:03:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:03:54Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:03:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:03:54Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:03:54Z- Filter 0 No beams used in subcloud update 2021-02-12T23:03:54Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:03:54Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:03:54Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 830.5 EastVariance: 833.2 DepthVariance: 1.6 this->DepthBiasCov: 1.6 2021-02-12T23:03:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171034.39 sec. 2021-02-12T23:03:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:03:54Z- TNavBF::mmse selected from filter -1 2021-02-12T23:03:54Z- variances([0], [2], and psi_berg) = 830.497678 833.226436 0.000000 2021-02-12T23:03:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:03:54Z- LcmTrn::publishEstimates() - MLE : 4056037.67 590304.37 10.40 2021-02-12T23:03:54Z- LcmTrn::publishEstimates() - MMSE : 4056081.17 590349.87 10.40 2021-02-12T23:03:54Z- LcmTrn::publishEstimates() - COVAR: 830.50 833.23 1.55 0.00 2021-02-12T23:03:54Z- AHRS_M2 msg: 1613171034.76 epoch sec; seqNo:420 2021-02-12T23:03:54Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-12T23:03:54Z- handleDepth()-Depth_Keller msg: 1613171034761 epoch sec; seqNo:423; depth 10.54 2021-02-12T23:03:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171034.77 epoch sec; seqNo:423; 4056081.55 north; 590349.86 east 2021-02-12T23:03:55Z- AHRS_M2 msg: 1613171035.16 epoch sec; seqNo:421 2021-02-12T23:03:55Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.19 psi 2021-02-12T23:03:55Z- handleDepth()-Depth_Keller msg: 1613171035166 epoch sec; seqNo:424; depth 10.67 2021-02-12T23:03:55Z- handleDvl() - RDI_Pathfinder msg: 1613171035.18 epoch sec; seqNo:181 2021-02-12T23:03:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 102.87 , 74.43 , 117.76 2021-02-12T23:03:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.20 , -0.08 2021-02-12T23:03:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171035.19 epoch sec; seqNo:424; 4056081.55 north; 590349.86 east 2021-02-12T23:03:55Z- AHRS_M2 msg: 1613171035.56 epoch sec; seqNo:422 2021-02-12T23:03:55Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.21 psi 2021-02-12T23:03:55Z- handleDepth()-Depth_Keller msg: 1613171035568 epoch sec; seqNo:425; depth 10.79 2021-02-12T23:03:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171035.59 epoch sec; seqNo:425; 4056081.93 north; 590349.86 east 2021-02-12T23:03:55Z- AHRS_M2 msg: 1613171035.97 epoch sec; seqNo:423 2021-02-12T23:03:55Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.23 psi 2021-02-12T23:03:55Z- handleDepth()-Depth_Keller msg: 1613171035972 epoch sec; seqNo:426; depth 10.93 2021-02-12T23:03:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171035.98 epoch sec; seqNo:426; 4056082.32 north; 590351.08 east 2021-02-12T23:03:56Z- AHRS_M2 msg: 1613171036.38 epoch sec; seqNo:424 2021-02-12T23:03:56Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.24 psi 2021-02-12T23:03:56Z- handleDepth()-Depth_Keller msg: 1613171036384 epoch sec; seqNo:427; depth 11.04 2021-02-12T23:03:56Z- handleDvl() - RDI_Pathfinder msg: 1613171036.42 epoch sec; seqNo:182 2021-02-12T23:03:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.64 , 104.29 , 74.84 , 116.47 2021-02-12T23:03:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.09 , -0.15 2021-02-12T23:03:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171036.43 epoch sec; seqNo:427; 4056082.32 north; 590351.08 east 2021-02-12T23:03:56Z- AHRS_M2 msg: 1613171036.78 epoch sec; seqNo:425 2021-02-12T23:03:56Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.25 psi 2021-02-12T23:03:56Z- handleDepth()-Depth_Keller msg: 1613171036786 epoch sec; seqNo:428; depth 11.19 2021-02-12T23:03:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171036.80 epoch sec; seqNo:428; 4056082.70 north; 590351.07 east 2021-02-12T23:03:57Z- AHRS_M2 msg: 1613171037.18 epoch sec; seqNo:426 2021-02-12T23:03:57Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.25 psi 2021-02-12T23:03:57Z- handleDepth()-Depth_Keller msg: 1613171037185 epoch sec; seqNo:429; depth 11.34 2021-02-12T23:03:57Z- handleDvl() - RDI_Pathfinder msg: 1613171037.20 epoch sec; seqNo:183 2021-02-12T23:03:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.11 , 108.35 , 74.02 , 116.47 2021-02-12T23:03:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , -0.12 2021-02-12T23:03:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171037.21 epoch sec; seqNo:429; 4056083.08 north; 590351.07 east 2021-02-12T23:03:57Z- AHRS_M2 msg: 1613171037.58 epoch sec; seqNo:427 2021-02-12T23:03:57Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.24 psi 2021-02-12T23:03:57Z- handleDepth()-Depth_Keller msg: 1613171037588 epoch sec; seqNo:430; depth 11.47 2021-02-12T23:03:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171037.60 epoch sec; seqNo:430; 4056083.46 north; 590351.06 east 2021-02-12T23:03:57Z- AHRS_M2 msg: 1613171037.99 epoch sec; seqNo:428 2021-02-12T23:03:57Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.22 psi 2021-02-12T23:03:57Z- handleDepth()-Depth_Keller msg: 1613171037996 epoch sec; seqNo:431; depth 11.60 2021-02-12T23:03:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171038.01 epoch sec; seqNo:431; 4056083.84 north; 590351.06 east 2021-02-12T23:03:58Z- AHRS_M2 msg: 1613171038.39 epoch sec; seqNo:429 2021-02-12T23:03:58Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.22 psi 2021-02-12T23:03:58Z- handleDepth()-Depth_Keller msg: 1613171038397 epoch sec; seqNo:432; depth 11.74 2021-02-12T23:03:58Z- handleDvl() - RDI_Pathfinder msg: 1613171038.41 epoch sec; seqNo:184 2021-02-12T23:03:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.34 , 104.29 , 71.57 , 117.83 2021-02-12T23:03:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , -0.04 2021-02-12T23:03:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171038.42 epoch sec; seqNo:432; 4056084.22 north; 590351.06 east 2021-02-12T23:03:58Z- AHRS_M2 msg: 1613171038.80 epoch sec; seqNo:430 2021-02-12T23:03:58Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.21 psi 2021-02-12T23:03:58Z- handleDepth()-Depth_Keller msg: 1613171038820 epoch sec; seqNo:433; depth 11.88 2021-02-12T23:03:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171038.85 epoch sec; seqNo:433; 4056084.23 north; 590352.28 east 2021-02-12T23:03:59Z- AHRS_M2 msg: 1613171039.20 epoch sec; seqNo:431 2021-02-12T23:03:59Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.20 psi 2021-02-12T23:03:59Z- handleDepth()-Depth_Keller msg: 1613171039204 epoch sec; seqNo:434; depth 12.02 2021-02-12T23:03:59Z- handleDvl() - RDI_Pathfinder msg: 1613171039.22 epoch sec; seqNo:185 2021-02-12T23:03:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.34 , 103.07 , 73.61 , 116.61 2021-02-12T23:03:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.07 , -0.03 2021-02-12T23:03:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171039.23 epoch sec; seqNo:434; 4056084.61 north; 590352.27 east 2021-02-12T23:03:59Z- AHRS_M2 msg: 1613171039.60 epoch sec; seqNo:432 2021-02-12T23:03:59Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.18 psi 2021-02-12T23:03:59Z- handleDepth()-Depth_Keller msg: 1613171039610 epoch sec; seqNo:435; depth 12.16 2021-02-12T23:03:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171039.62 epoch sec; seqNo:435; 4056084.99 north; 590352.27 east 2021-02-12T23:03:59Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 130 calls... 2021-02-12T23:03:59Z- waiting for 1613171037.39, time is 1613171039.62 2021-02-12T23:03:59Z- LcmTrn::updateTrn() >>>> pose time 1613171039.62, meas time 1613171039.22 <<<< 2021-02-12T23:03:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171039.22 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:03:59Z- TerrainNav::motionUpdate() - currEstimate = (4056085, 590352.27, 12.16) 2021-02-12T23:03:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:03:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:03:59Z- TNavBF::Projecting Measurements 2021-02-12T23:03:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:03:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:03:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:03:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:03:59Z- TNavFilter::4 good beams. 2021-02-12T23:03:59Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:03:59Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:03:59Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:03:59Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:03:59Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:03:59Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:03:59Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:03:59Z- TNavBF::mmse selected from filter -1 2021-02-12T23:03:59Z- TerrainMapOctree: (vehN, vehE ) = (4056084.69, 590352.08). 2021-02-12T23:03:59Z- TerrainMapOctree: Min Distance = 6471.03. 2021-02-12T23:03:59Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:03:59Z- There is no specific good beam that all particles have in common. 2021-02-12T23:03:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:03:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.76 K: 0.130 ExpectedMeasDiff: nan currMeas.covariance: 10.07 2021-02-12T23:03:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:03:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.53 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 17.00 2021-02-12T23:03:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:03:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.42 K: 0.120 ExpectedMeasDiff: nan currMeas.covariance: 8.67 2021-02-12T23:03:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:03:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 21.76 2021-02-12T23:03:59Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:03:59Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:03:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:03:59Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:03:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:03:59Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:03:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:03:59Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:03:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:03:59Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:03:59Z- Filter 0 No beams used in subcloud update 2021-02-12T23:03:59Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:03:59Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:03:59Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 830.0 EastVariance: 833.2 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:03:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171039.22 sec. 2021-02-12T23:03:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:03:59Z- TNavBF::mmse selected from filter -1 2021-02-12T23:03:59Z- variances([0], [2], and psi_berg) = 829.801998 833.221053 0.000000 2021-02-12T23:03:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:03:59Z- LcmTrn::publishEstimates() - MLE : 4056043.49 590302.77 12.16 2021-02-12T23:03:59Z- LcmTrn::publishEstimates() - MMSE : 4056084.99 590352.27 12.16 2021-02-12T23:03:59Z- LcmTrn::publishEstimates() - COVAR: 829.80 833.22 1.20 0.00 2021-02-12T23:04:00Z- AHRS_M2 msg: 1613171040.02 epoch sec; seqNo:433 2021-02-12T23:04:00Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.17 psi 2021-02-12T23:04:00Z- handleDepth()-Depth_Keller msg: 1613171040039 epoch sec; seqNo:436; depth 12.28 2021-02-12T23:04:00Z- handleDvl() - RDI_Pathfinder msg: 1613171040.05 epoch sec; seqNo:186 2021-02-12T23:04:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.75 , 96.31 , 72.79 , 112.55 2021-02-12T23:04:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , -0.09 2021-02-12T23:04:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171040.06 epoch sec; seqNo:436; 4056085.36 north; 590352.26 east 2021-02-12T23:04:00Z- AHRS_M2 msg: 1613171040.41 epoch sec; seqNo:434 2021-02-12T23:04:00Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.15 psi 2021-02-12T23:04:00Z- handleDepth()-Depth_Keller msg: 1613171040422 epoch sec; seqNo:437; depth 12.44 2021-02-12T23:04:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171040.44 epoch sec; seqNo:437; 4056085.36 north; 590352.26 east 2021-02-12T23:04:00Z- AHRS_M2 msg: 1613171040.82 epoch sec; seqNo:435 2021-02-12T23:04:00Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.14 psi 2021-02-12T23:04:00Z- handleDepth()-Depth_Keller msg: 1613171040828 epoch sec; seqNo:438; depth 12.55 2021-02-12T23:04:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171040.84 epoch sec; seqNo:438; 4056085.74 north; 590352.26 east 2021-02-12T23:04:01Z- AHRS_M2 msg: 1613171041.23 epoch sec; seqNo:436 2021-02-12T23:04:01Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.14 psi 2021-02-12T23:04:01Z- handleDepth()-Depth_Keller msg: 1613171041231 epoch sec; seqNo:439; depth 12.69 2021-02-12T23:04:01Z- handleDvl() - RDI_Pathfinder msg: 1613171041.24 epoch sec; seqNo:187 2021-02-12T23:04:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.33 , 96.51 , 71.16 , 114.78 2021-02-12T23:04:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.10 , -0.17 2021-02-12T23:04:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171041.25 epoch sec; seqNo:439; 4056086.12 north; 590352.26 east 2021-02-12T23:04:01Z- AHRS_M2 msg: 1613171041.62 epoch sec; seqNo:437 2021-02-12T23:04:01Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.15 psi 2021-02-12T23:04:01Z- handleDepth()-Depth_Keller msg: 1613171041628 epoch sec; seqNo:440; depth 12.83 2021-02-12T23:04:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171041.64 epoch sec; seqNo:440; 4056086.51 north; 590353.47 east 2021-02-12T23:04:02Z- AHRS_M2 msg: 1613171042.03 epoch sec; seqNo:438 2021-02-12T23:04:02Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.15 psi 2021-02-12T23:04:02Z- handleDepth()-Depth_Keller msg: 1613171042040 epoch sec; seqNo:441; depth 12.95 2021-02-12T23:04:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171042.06 epoch sec; seqNo:441; 4056086.89 north; 590353.47 east 2021-02-12T23:04:02Z- AHRS_M2 msg: 1613171042.43 epoch sec; seqNo:439 2021-02-12T23:04:02Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.16 psi 2021-02-12T23:04:02Z- handleDepth()-Depth_Keller msg: 1613171042445 epoch sec; seqNo:442; depth 13.10 2021-02-12T23:04:02Z- handleDvl() - RDI_Pathfinder msg: 1613171042.46 epoch sec; seqNo:188 2021-02-12T23:04:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.34 , 96.44 , 71.98 , 112.28 2021-02-12T23:04:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.22 , -0.16 2021-02-12T23:04:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171042.47 epoch sec; seqNo:442; 4056087.27 north; 590353.47 east 2021-02-12T23:04:02Z- AHRS_M2 msg: 1613171042.84 epoch sec; seqNo:440 2021-02-12T23:04:02Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.15 psi 2021-02-12T23:04:02Z- handleDepth()-Depth_Keller msg: 1613171042845 epoch sec; seqNo:443; depth 13.23 2021-02-12T23:04:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171042.87 epoch sec; seqNo:443; 4056087.27 north; 590353.47 east 2021-02-12T23:04:03Z- AHRS_M2 msg: 1613171043.24 epoch sec; seqNo:441 2021-02-12T23:04:03Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.15 psi 2021-02-12T23:04:03Z- handleDepth()-Depth_Keller msg: 1613171043244 epoch sec; seqNo:444; depth 13.36 2021-02-12T23:04:03Z- handleDvl() - RDI_Pathfinder msg: 1613171043.26 epoch sec; seqNo:189 2021-02-12T23:04:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.70 , 96.58 , 70.75 , 111.20 2021-02-12T23:04:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.28 , -0.02 2021-02-12T23:04:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171043.27 epoch sec; seqNo:444; 4056087.65 north; 590353.46 east 2021-02-12T23:04:03Z- AHRS_M2 msg: 1613171043.64 epoch sec; seqNo:442 2021-02-12T23:04:03Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.14 psi 2021-02-12T23:04:03Z- handleDepth()-Depth_Keller msg: 1613171043653 epoch sec; seqNo:445; depth 13.49 2021-02-12T23:04:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171043.66 epoch sec; seqNo:445; 4056088.03 north; 590353.46 east 2021-02-12T23:04:04Z- AHRS_M2 msg: 1613171044.08 epoch sec; seqNo:443 2021-02-12T23:04:04Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.14 psi 2021-02-12T23:04:04Z- handleDepth()-Depth_Keller msg: 1613171044081 epoch sec; seqNo:446; depth 13.62 2021-02-12T23:04:04Z- handleDvl() - RDI_Pathfinder msg: 1613171044.09 epoch sec; seqNo:190 2021-02-12T23:04:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.29 , 95.43 , 69.93 , 110.05 2021-02-12T23:04:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.23 , 0.12 2021-02-12T23:04:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171044.12 epoch sec; seqNo:446; 4056088.41 north; 590353.45 east 2021-02-12T23:04:04Z- AHRS_M2 msg: 1613171044.45 epoch sec; seqNo:444 2021-02-12T23:04:04Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.13 psi 2021-02-12T23:04:04Z- handleDepth()-Depth_Keller msg: 1613171044459 epoch sec; seqNo:447; depth 13.76 2021-02-12T23:04:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171044.48 epoch sec; seqNo:447; 4056088.79 north; 590353.45 east 2021-02-12T23:04:04Z- AHRS_M2 msg: 1613171044.86 epoch sec; seqNo:445 2021-02-12T23:04:04Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.13 psi 2021-02-12T23:04:04Z- handleDepth()-Depth_Keller msg: 1613171044860 epoch sec; seqNo:448; depth 13.88 2021-02-12T23:04:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171044.87 epoch sec; seqNo:448; 4056088.79 north; 590353.45 east 2021-02-12T23:04:04Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-12T23:04:04Z- waiting for 1613171042.62, time is 1613171044.87 2021-02-12T23:04:04Z- LcmTrn::updateTrn() >>>> pose time 1613171044.87, meas time 1613171044.09 <<<< 2021-02-12T23:04:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171044.09 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:04:04Z- TerrainNav::motionUpdate() - currEstimate = (4056089, 590353.45, 13.88) 2021-02-12T23:04:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:04:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:04:04Z- TNavBF::Projecting Measurements 2021-02-12T23:04:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:04:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:04:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:04:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:04:04Z- TNavFilter::4 good beams. 2021-02-12T23:04:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:04:04Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:04:04Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:04:04Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:04:04Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:04:04Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:04:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:04:04Z- TNavBF::mmse selected from filter -1 2021-02-12T23:04:04Z- TerrainMapOctree: (vehN, vehE ) = (4056088.22, 590353.27). 2021-02-12T23:04:04Z- TerrainMapOctree: Min Distance = 6467.49. 2021-02-12T23:04:04Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:04:04Z- There is no specific good beam that all particles have in common. 2021-02-12T23:04:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:04:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 9.56 2021-02-12T23:04:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:04:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 14.57 2021-02-12T23:04:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:04:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 7.82 2021-02-12T23:04:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:04:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 19.38 2021-02-12T23:04:04Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:04:04Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:04:04Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:04:04Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:04:04Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:04:04Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:04:04Z- Filter 0 No beams used in subcloud update 2021-02-12T23:04:04Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:04:04Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:04:04Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 829.3 EastVariance: 833.2 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:04:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171044.09 sec. 2021-02-12T23:04:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:04:04Z- TNavBF::mmse selected from filter -1 2021-02-12T23:04:04Z- variances([0], [2], and psi_berg) = 829.127530 833.216044 0.000000 2021-02-12T23:04:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:04:04Z- LcmTrn::publishEstimates() - MLE : 4056048.29 590303.95 13.88 2021-02-12T23:04:04Z- LcmTrn::publishEstimates() - MMSE : 4056088.79 590353.45 13.88 2021-02-12T23:04:04Z- LcmTrn::publishEstimates() - COVAR: 829.13 833.22 1.00 0.00 2021-02-12T23:04:05Z- AHRS_M2 msg: 1613171045.27 epoch sec; seqNo:446 2021-02-12T23:04:05Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.12 psi 2021-02-12T23:04:05Z- handleDepth()-Depth_Keller msg: 1613171045271 epoch sec; seqNo:449; depth 14.00 2021-02-12T23:04:05Z- handleDvl() - RDI_Pathfinder msg: 1613171045.28 epoch sec; seqNo:191 2021-02-12T23:04:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.11 , 94.28 , 70.75 , 108.89 2021-02-12T23:04:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.10 , 0.14 2021-02-12T23:04:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171045.29 epoch sec; seqNo:449; 4056089.17 north; 590353.45 east 2021-02-12T23:04:05Z- AHRS_M2 msg: 1613171045.66 epoch sec; seqNo:447 2021-02-12T23:04:05Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.12 psi 2021-02-12T23:04:05Z- handleDepth()-Depth_Keller msg: 1613171045668 epoch sec; seqNo:450; depth 14.12 2021-02-12T23:04:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171045.68 epoch sec; seqNo:450; 4056089.56 north; 590354.66 east 2021-02-12T23:04:06Z- AHRS_M2 msg: 1613171046.08 epoch sec; seqNo:448 2021-02-12T23:04:06Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.13 psi 2021-02-12T23:04:06Z- handleDepth()-Depth_Keller msg: 1613171046083 epoch sec; seqNo:451; depth 14.24 2021-02-12T23:04:06Z- handleDvl() - RDI_Pathfinder msg: 1613171046.10 epoch sec; seqNo:192 2021-02-12T23:04:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.52 , 94.28 , 70.34 , 111.33 2021-02-12T23:04:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.07 , 0.11 2021-02-12T23:04:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171046.11 epoch sec; seqNo:451; 4056089.56 north; 590354.66 east 2021-02-12T23:04:06Z- AHRS_M2 msg: 1613171046.47 epoch sec; seqNo:449 2021-02-12T23:04:06Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.13 psi 2021-02-12T23:04:06Z- handleDepth()-Depth_Keller msg: 1613171046478 epoch sec; seqNo:452; depth 14.37 2021-02-12T23:04:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171046.49 epoch sec; seqNo:452; 4056089.94 north; 590354.66 east 2021-02-12T23:04:06Z- AHRS_M2 msg: 1613171046.88 epoch sec; seqNo:450 2021-02-12T23:04:06Z- AHRS_M2 msg: 0.69 phi; -0.36 theta; -0.13 psi 2021-02-12T23:04:06Z- handleDepth()-Depth_Keller msg: 1613171046894 epoch sec; seqNo:453; depth 14.48 2021-02-12T23:04:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171046.90 epoch sec; seqNo:453; 4056089.94 north; 590354.66 east 2021-02-12T23:04:07Z- AHRS_M2 msg: 1613171047.28 epoch sec; seqNo:451 2021-02-12T23:04:07Z- AHRS_M2 msg: 0.70 phi; -0.36 theta; -0.13 psi 2021-02-12T23:04:07Z- handleDepth()-Depth_Keller msg: 1613171047285 epoch sec; seqNo:454; depth 14.62 2021-02-12T23:04:07Z- handleDvl() - RDI_Pathfinder msg: 1613171047.30 epoch sec; seqNo:193 2021-02-12T23:04:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.11 , 94.28 , 69.52 , 110.11 2021-02-12T23:04:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.01 , 0.11 2021-02-12T23:04:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171047.32 epoch sec; seqNo:454; 4056090.32 north; 590354.66 east 2021-02-12T23:04:07Z- AHRS_M2 msg: 1613171047.68 epoch sec; seqNo:452 2021-02-12T23:04:07Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.13 psi 2021-02-12T23:04:07Z- handleDepth()-Depth_Keller msg: 1613171047688 epoch sec; seqNo:455; depth 14.73 2021-02-12T23:04:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171047.70 epoch sec; seqNo:455; 4056090.32 north; 590354.66 east 2021-02-12T23:04:08Z- AHRS_M2 msg: 1613171048.09 epoch sec; seqNo:453 2021-02-12T23:04:08Z- AHRS_M2 msg: 0.71 phi; -0.37 theta; -0.12 psi 2021-02-12T23:04:08Z- handleDepth()-Depth_Keller msg: 1613171048097 epoch sec; seqNo:456; depth 14.85 2021-02-12T23:04:08Z- handleDvl() - RDI_Pathfinder msg: 1613171048.11 epoch sec; seqNo:194 2021-02-12T23:04:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.52 , 95.50 , 69.93 , 112.55 2021-02-12T23:04:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.06 , 0.12 2021-02-12T23:04:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171048.12 epoch sec; seqNo:456; 4056090.69 north; 590354.65 east 2021-02-12T23:04:08Z- AHRS_M2 msg: 1613171048.49 epoch sec; seqNo:454 2021-02-12T23:04:08Z- AHRS_M2 msg: 0.71 phi; -0.38 theta; -0.12 psi 2021-02-12T23:04:08Z- handleDepth()-Depth_Keller msg: 1613171048496 epoch sec; seqNo:457; depth 14.99 2021-02-12T23:04:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171048.51 epoch sec; seqNo:457; 4056091.09 north; 590355.87 east 2021-02-12T23:04:08Z- AHRS_M2 msg: 1613171048.90 epoch sec; seqNo:455 2021-02-12T23:04:08Z- AHRS_M2 msg: 0.72 phi; -0.38 theta; -0.12 psi 2021-02-12T23:04:08Z- handleDepth()-Depth_Keller msg: 1613171048900 epoch sec; seqNo:458; depth 15.11 2021-02-12T23:04:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171048.91 epoch sec; seqNo:458; 4056091.09 north; 590355.87 east 2021-02-12T23:04:09Z- AHRS_M2 msg: 1613171049.30 epoch sec; seqNo:456 2021-02-12T23:04:09Z- AHRS_M2 msg: 0.72 phi; -0.38 theta; -0.12 psi 2021-02-12T23:04:09Z- handleDepth()-Depth_Keller msg: 1613171049304 epoch sec; seqNo:459; depth 15.24 2021-02-12T23:04:09Z- handleDvl() - RDI_Pathfinder msg: 1613171049.32 epoch sec; seqNo:195 2021-02-12T23:04:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.81 , 94.21 , 69.11 , 113.70 2021-02-12T23:04:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.14 , 0.06 2021-02-12T23:04:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171049.33 epoch sec; seqNo:459; 4056091.46 north; 590355.86 east 2021-02-12T23:04:09Z- AHRS_M2 msg: 1613171049.70 epoch sec; seqNo:457 2021-02-12T23:04:09Z- AHRS_M2 msg: 0.73 phi; -0.38 theta; -0.13 psi 2021-02-12T23:04:09Z- handleDepth()-Depth_Keller msg: 1613171049710 epoch sec; seqNo:460; depth 15.39 2021-02-12T23:04:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171049.72 epoch sec; seqNo:460; 4056091.46 north; 590355.86 east 2021-02-12T23:04:10Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-12T23:04:10Z- waiting for 1613171047.87, time is 1613171049.72 2021-02-12T23:04:10Z- LcmTrn::updateTrn() >>>> pose time 1613171049.72, meas time 1613171049.32 <<<< 2021-02-12T23:04:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171049.32 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:04:10Z- TerrainNav::motionUpdate() - currEstimate = (4056091, 590355.86, 15.39) 2021-02-12T23:04:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:04:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:04:10Z- TNavBF::Projecting Measurements 2021-02-12T23:04:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:04:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:04:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:04:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:04:10Z- TNavFilter::4 good beams. 2021-02-12T23:04:10Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:04:10Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:04:10Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:04:10Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:04:10Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:04:10Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:04:10Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:04:10Z- TNavBF::mmse selected from filter -1 2021-02-12T23:04:10Z- TerrainMapOctree: (vehN, vehE ) = (4056091.24, 590355.66). 2021-02-12T23:04:10Z- TerrainMapOctree: Min Distance = 6463.64. 2021-02-12T23:04:10Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:04:10Z- There is no specific good beam that all particles have in common. 2021-02-12T23:04:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:04:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 10.45 2021-02-12T23:04:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:04:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 14.20 2021-02-12T23:04:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:04:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 7.64 2021-02-12T23:04:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:04:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 20.68 2021-02-12T23:04:10Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:04:10Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:04:10Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:04:10Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:04:10Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:04:10Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:04:10Z- Filter 0 No beams used in subcloud update 2021-02-12T23:04:10Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:04:10Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:04:10Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 828.7 EastVariance: 833.2 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:04:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171049.32 sec. 2021-02-12T23:04:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:04:10Z- TNavBF::mmse selected from filter -1 2021-02-12T23:04:10Z- variances([0], [2], and psi_berg) = 828.528262 833.211752 0.000000 2021-02-12T23:04:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:04:10Z- LcmTrn::publishEstimates() - MLE : 4056050.96 590307.36 15.39 2021-02-12T23:04:10Z- LcmTrn::publishEstimates() - MMSE : 4056091.46 590355.86 15.39 2021-02-12T23:04:10Z- LcmTrn::publishEstimates() - COVAR: 828.53 833.21 0.88 0.00 2021-02-12T23:04:10Z- AHRS_M2 msg: 1613171050.11 epoch sec; seqNo:458 2021-02-12T23:04:10Z- AHRS_M2 msg: 0.74 phi; -0.38 theta; -0.13 psi 2021-02-12T23:04:10Z- handleDepth()-Depth_Keller msg: 1613171050115 epoch sec; seqNo:461; depth 15.51 2021-02-12T23:04:10Z- handleDvl() - RDI_Pathfinder msg: 1613171050.14 epoch sec; seqNo:196 2021-02-12T23:04:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 94.21 , 69.52 , 113.70 2021-02-12T23:04:10Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.18 , -0.05 2021-02-12T23:04:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171050.15 epoch sec; seqNo:461; 4056091.86 north; 590357.08 east 2021-02-12T23:04:10Z- AHRS_M2 msg: 1613171050.51 epoch sec; seqNo:459 2021-02-12T23:04:10Z- AHRS_M2 msg: 0.75 phi; -0.38 theta; -0.14 psi 2021-02-12T23:04:10Z- handleDepth()-Depth_Keller msg: 1613171050516 epoch sec; seqNo:462; depth 15.65 2021-02-12T23:04:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171050.53 epoch sec; seqNo:462; 4056092.24 north; 590357.08 east 2021-02-12T23:04:10Z- AHRS_M2 msg: 1613171050.92 epoch sec; seqNo:460 2021-02-12T23:04:10Z- AHRS_M2 msg: 0.76 phi; -0.38 theta; -0.15 psi 2021-02-12T23:04:10Z- handleDepth()-Depth_Keller msg: 1613171050925 epoch sec; seqNo:463; depth 15.76 2021-02-12T23:04:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171050.93 epoch sec; seqNo:463; 4056092.24 north; 590357.08 east 2021-02-12T23:04:11Z- AHRS_M2 msg: 1613171051.32 epoch sec; seqNo:461 2021-02-12T23:04:11Z- AHRS_M2 msg: 0.77 phi; -0.38 theta; -0.15 psi 2021-02-12T23:04:11Z- handleDepth()-Depth_Keller msg: 1613171051326 epoch sec; seqNo:464; depth 15.92 2021-02-12T23:04:11Z- handleDvl() - RDI_Pathfinder msg: 1613171051.34 epoch sec; seqNo:197 2021-02-12T23:04:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.89 , 95.02 , 69.11 , 112.62 2021-02-12T23:04:11Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.06 , -0.16 2021-02-12T23:04:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171051.35 epoch sec; seqNo:464; 4056092.61 north; 590357.07 east 2021-02-12T23:04:11Z- AHRS_M2 msg: 1613171051.72 epoch sec; seqNo:462 2021-02-12T23:04:11Z- AHRS_M2 msg: 0.78 phi; -0.39 theta; -0.15 psi 2021-02-12T23:04:11Z- handleDepth()-Depth_Keller msg: 1613171051728 epoch sec; seqNo:465; depth 16.05 2021-02-12T23:04:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171051.75 epoch sec; seqNo:465; 4056093.01 north; 590358.29 east 2021-02-12T23:04:12Z- AHRS_M2 msg: 1613171052.13 epoch sec; seqNo:463 2021-02-12T23:04:12Z- AHRS_M2 msg: 0.79 phi; -0.39 theta; -0.15 psi 2021-02-12T23:04:12Z- handleDepth()-Depth_Keller msg: 1613171052136 epoch sec; seqNo:466; depth 16.18 2021-02-12T23:04:12Z- handleDvl() - RDI_Pathfinder msg: 1613171052.15 epoch sec; seqNo:198 2021-02-12T23:04:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.07 , 95.50 , 68.71 , 112.55 2021-02-12T23:04:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.18 2021-02-12T23:04:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171052.17 epoch sec; seqNo:466; 4056093.01 north; 590358.29 east 2021-02-12T23:04:12Z- AHRS_M2 msg: 1613171052.55 epoch sec; seqNo:464 2021-02-12T23:04:12Z- AHRS_M2 msg: 0.79 phi; -0.40 theta; -0.15 psi 2021-02-12T23:04:12Z- handleDepth()-Depth_Keller msg: 1613171052553 epoch sec; seqNo:467; depth 16.34 2021-02-12T23:04:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171052.59 epoch sec; seqNo:467; 4056093.38 north; 590358.29 east 2021-02-12T23:04:12Z- AHRS_M2 msg: 1613171052.94 epoch sec; seqNo:465 2021-02-12T23:04:12Z- AHRS_M2 msg: 0.79 phi; -0.40 theta; -0.16 psi 2021-02-12T23:04:12Z- handleDepth()-Depth_Keller msg: 1613171052941 epoch sec; seqNo:468; depth 16.46 2021-02-12T23:04:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171052.95 epoch sec; seqNo:468; 4056093.76 north; 590358.28 east 2021-02-12T23:04:13Z- AHRS_M2 msg: 1613171053.34 epoch sec; seqNo:466 2021-02-12T23:04:13Z- AHRS_M2 msg: 0.78 phi; -0.41 theta; -0.16 psi 2021-02-12T23:04:13Z- handleDepth()-Depth_Keller msg: 1613171053344 epoch sec; seqNo:469; depth 16.59 2021-02-12T23:04:13Z- handleDvl() - RDI_Pathfinder msg: 1613171053.36 epoch sec; seqNo:199 2021-02-12T23:04:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.66 , 94.28 , 68.71 , 113.77 2021-02-12T23:04:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.10 , -0.13 2021-02-12T23:04:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171053.37 epoch sec; seqNo:469; 4056094.14 north; 590358.28 east 2021-02-12T23:04:13Z- AHRS_M2 msg: 1613171053.74 epoch sec; seqNo:467 2021-02-12T23:04:13Z- AHRS_M2 msg: 0.78 phi; -0.41 theta; -0.17 psi 2021-02-12T23:04:13Z- handleDepth()-Depth_Keller msg: 1613171053750 epoch sec; seqNo:470; depth 16.73 2021-02-12T23:04:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171053.76 epoch sec; seqNo:470; 4056094.14 north; 590358.28 east 2021-02-12T23:04:14Z- AHRS_M2 msg: 1613171054.15 epoch sec; seqNo:468 2021-02-12T23:04:14Z- AHRS_M2 msg: 0.77 phi; -0.42 theta; -0.18 psi 2021-02-12T23:04:14Z- handleDepth()-Depth_Keller msg: 1613171054154 epoch sec; seqNo:471; depth 16.88 2021-02-12T23:04:14Z- handleDvl() - RDI_Pathfinder msg: 1613171054.17 epoch sec; seqNo:200 2021-02-12T23:04:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.25 , 96.71 , 68.30 , 113.77 2021-02-12T23:04:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.10 , -0.06 2021-02-12T23:04:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171054.18 epoch sec; seqNo:471; 4056094.53 north; 590359.50 east 2021-02-12T23:04:14Z- AHRS_M2 msg: 1613171054.55 epoch sec; seqNo:469 2021-02-12T23:04:14Z- AHRS_M2 msg: 0.76 phi; -0.42 theta; -0.19 psi 2021-02-12T23:04:14Z- handleDepth()-Depth_Keller msg: 1613171054558 epoch sec; seqNo:472; depth 17.01 2021-02-12T23:04:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171054.57 epoch sec; seqNo:472; 4056094.91 north; 590359.49 east 2021-02-12T23:04:14Z- AHRS_M2 msg: 1613171054.96 epoch sec; seqNo:470 2021-02-12T23:04:14Z- AHRS_M2 msg: 0.75 phi; -0.42 theta; -0.21 psi 2021-02-12T23:04:14Z- handleDepth()-Depth_Keller msg: 1613171054968 epoch sec; seqNo:473; depth 17.16 2021-02-12T23:04:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171054.99 epoch sec; seqNo:473; 4056094.91 north; 590359.49 east 2021-02-12T23:04:15Z- LcmTrn::handleMessages() - handling 44 messages took 0.74 ms and 130 calls... 2021-02-12T23:04:15Z- waiting for 1613171052.72, time is 1613171054.99 2021-02-12T23:04:15Z- LcmTrn::updateTrn() >>>> pose time 1613171054.99, meas time 1613171054.17 <<<< 2021-02-12T23:04:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171054.17 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:04:15Z- TerrainNav::motionUpdate() - currEstimate = (4056095, 590359.49, 17.16) 2021-02-12T23:04:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:04:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:04:15Z- TNavBF::Projecting Measurements 2021-02-12T23:04:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:04:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:04:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:04:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:04:15Z- TNavFilter::4 good beams. 2021-02-12T23:04:15Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:04:15Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:04:15Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:04:15Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:04:15Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:04:15Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:04:15Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:04:15Z- TNavBF::mmse selected from filter -1 2021-02-12T23:04:15Z- TerrainMapOctree: (vehN, vehE ) = (4056094.37, 590358.92). 2021-02-12T23:04:15Z- TerrainMapOctree: Min Distance = 6459.18. 2021-02-12T23:04:15Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:04:15Z- There is no specific good beam that all particles have in common. 2021-02-12T23:04:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:04:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 9.06 2021-02-12T23:04:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:04:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 14.97 2021-02-12T23:04:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:04:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 7.46 2021-02-12T23:04:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:04:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 20.71 2021-02-12T23:04:15Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:04:15Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:04:15Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:04:15Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:04:15Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:04:15Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:04:15Z- Filter 0 No beams used in subcloud update 2021-02-12T23:04:15Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:04:15Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:04:15Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 828.0 EastVariance: 833.2 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:04:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171054.17 sec. 2021-02-12T23:04:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:04:15Z- TNavBF::mmse selected from filter -1 2021-02-12T23:04:15Z- variances([0], [2], and psi_berg) = 827.798881 833.206718 0.000000 2021-02-12T23:04:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:04:15Z- LcmTrn::publishEstimates() - MLE : 4056054.41 590408.99 17.16 2021-02-12T23:04:15Z- LcmTrn::publishEstimates() - MMSE : 4056094.91 590359.49 17.16 2021-02-12T23:04:15Z- LcmTrn::publishEstimates() - COVAR: 827.80 833.21 0.86 0.00 2021-02-12T23:04:15Z- AHRS_M2 msg: 1613171055.36 epoch sec; seqNo:471 2021-02-12T23:04:15Z- AHRS_M2 msg: 0.74 phi; -0.43 theta; -0.22 psi 2021-02-12T23:04:15Z- handleDepth()-Depth_Keller msg: 1613171055364 epoch sec; seqNo:474; depth 17.30 2021-02-12T23:04:15Z- handleDvl() - RDI_Pathfinder msg: 1613171055.38 epoch sec; seqNo:201 2021-02-12T23:04:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.25 , 96.71 , 67.07 , 116.20 2021-02-12T23:04:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.09 , 0.03 2021-02-12T23:04:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171055.40 epoch sec; seqNo:474; 4056095.29 north; 590359.49 east 2021-02-12T23:04:15Z- AHRS_M2 msg: 1613171055.77 epoch sec; seqNo:472 2021-02-12T23:04:15Z- AHRS_M2 msg: 0.73 phi; -0.43 theta; -0.23 psi 2021-02-12T23:04:15Z- handleDepth()-Depth_Keller msg: 1613171055770 epoch sec; seqNo:475; depth 17.46 2021-02-12T23:04:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171055.78 epoch sec; seqNo:475; 4056095.67 north; 590359.48 east 2021-02-12T23:04:16Z- AHRS_M2 msg: 1613171056.17 epoch sec; seqNo:473 2021-02-12T23:04:16Z- AHRS_M2 msg: 0.72 phi; -0.43 theta; -0.24 psi 2021-02-12T23:04:16Z- handleDepth()-Depth_Keller msg: 1613171056174 epoch sec; seqNo:476; depth 17.61 2021-02-12T23:04:16Z- handleDvl() - RDI_Pathfinder msg: 1613171056.18 epoch sec; seqNo:202 2021-02-12T23:04:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.02 , 96.65 , 67.07 , 117.36 2021-02-12T23:04:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.07 2021-02-12T23:04:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171056.19 epoch sec; seqNo:476; 4056095.67 north; 590359.48 east 2021-02-12T23:04:16Z- AHRS_M2 msg: 1613171056.57 epoch sec; seqNo:474 2021-02-12T23:04:16Z- AHRS_M2 msg: 0.71 phi; -0.43 theta; -0.25 psi 2021-02-12T23:04:16Z- handleDepth()-Depth_Keller msg: 1613171056576 epoch sec; seqNo:477; depth 17.76 2021-02-12T23:04:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171056.60 epoch sec; seqNo:477; 4056096.05 north; 590359.48 east 2021-02-12T23:04:16Z- AHRS_M2 msg: 1613171056.98 epoch sec; seqNo:475 2021-02-12T23:04:16Z- AHRS_M2 msg: 0.71 phi; -0.43 theta; -0.25 psi 2021-02-12T23:04:16Z- handleDepth()-Depth_Keller msg: 1613171056980 epoch sec; seqNo:478; depth 17.90 2021-02-12T23:04:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171056.99 epoch sec; seqNo:478; 4056096.06 north; 590360.70 east 2021-02-12T23:04:17Z- AHRS_M2 msg: 1613171057.38 epoch sec; seqNo:476 2021-02-12T23:04:17Z- AHRS_M2 msg: 0.70 phi; -0.44 theta; -0.26 psi 2021-02-12T23:04:17Z- handleDepth()-Depth_Keller msg: 1613171057389 epoch sec; seqNo:479; depth 18.06 2021-02-12T23:04:17Z- handleDvl() - RDI_Pathfinder msg: 1613171057.40 epoch sec; seqNo:203 2021-02-12T23:04:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 96.65 , 67.07 , 119.79 2021-02-12T23:04:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , 0.07 2021-02-12T23:04:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171057.41 epoch sec; seqNo:479; 4056096.44 north; 590360.70 east 2021-02-12T23:04:17Z- AHRS_M2 msg: 1613171057.78 epoch sec; seqNo:477 2021-02-12T23:04:17Z- AHRS_M2 msg: 0.69 phi; -0.44 theta; -0.26 psi 2021-02-12T23:04:17Z- handleDepth()-Depth_Keller msg: 1613171057805 epoch sec; seqNo:480; depth 18.24 2021-02-12T23:04:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171057.82 epoch sec; seqNo:480; 4056096.82 north; 590360.69 east 2021-02-12T23:04:18Z- AHRS_M2 msg: 1613171058.19 epoch sec; seqNo:478 2021-02-12T23:04:18Z- AHRS_M2 msg: 0.68 phi; -0.44 theta; -0.25 psi 2021-02-12T23:04:18Z- handleDepth()-Depth_Keller msg: 1613171058198 epoch sec; seqNo:481; depth 18.40 2021-02-12T23:04:18Z- handleDvl() - RDI_Pathfinder msg: 1613171058.22 epoch sec; seqNo:204 2021-02-12T23:04:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.02 , 96.51 , 67.48 , 118.44 2021-02-12T23:04:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , 0.05 2021-02-12T23:04:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171058.23 epoch sec; seqNo:481; 4056096.82 north; 590360.69 east 2021-02-12T23:04:18Z- AHRS_M2 msg: 1613171058.59 epoch sec; seqNo:479 2021-02-12T23:04:18Z- AHRS_M2 msg: 0.67 phi; -0.43 theta; -0.25 psi 2021-02-12T23:04:18Z- handleDepth()-Depth_Keller msg: 1613171058598 epoch sec; seqNo:482; depth 18.55 2021-02-12T23:04:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171058.61 epoch sec; seqNo:482; 4056097.20 north; 590360.69 east 2021-02-12T23:04:19Z- AHRS_M2 msg: 1613171059.00 epoch sec; seqNo:480 2021-02-12T23:04:19Z- AHRS_M2 msg: 0.66 phi; -0.43 theta; -0.25 psi 2021-02-12T23:04:19Z- handleDepth()-Depth_Keller msg: 1613171059001 epoch sec; seqNo:483; depth 18.71 2021-02-12T23:04:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171059.01 epoch sec; seqNo:483; 4056097.58 north; 590360.68 east 2021-02-12T23:04:19Z- AHRS_M2 msg: 1613171059.40 epoch sec; seqNo:481 2021-02-12T23:04:19Z- AHRS_M2 msg: 0.66 phi; -0.43 theta; -0.25 psi 2021-02-12T23:04:19Z- handleDepth()-Depth_Keller msg: 1613171059415 epoch sec; seqNo:484; depth 18.86 2021-02-12T23:04:19Z- handleDvl() - RDI_Pathfinder msg: 1613171059.43 epoch sec; seqNo:205 2021-02-12T23:04:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.61 , 96.65 , 67.48 , 117.36 2021-02-12T23:04:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.01 2021-02-12T23:04:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171059.44 epoch sec; seqNo:484; 4056097.59 north; 590361.91 east 2021-02-12T23:04:19Z- AHRS_M2 msg: 1613171059.80 epoch sec; seqNo:482 2021-02-12T23:04:19Z- AHRS_M2 msg: 0.65 phi; -0.43 theta; -0.25 psi 2021-02-12T23:04:19Z- handleDepth()-Depth_Keller msg: 1613171059809 epoch sec; seqNo:485; depth 19.05 2021-02-12T23:04:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171059.82 epoch sec; seqNo:485; 4056097.97 north; 590361.90 east 2021-02-12T23:04:20Z- AHRS_M2 msg: 1613171060.21 epoch sec; seqNo:483 2021-02-12T23:04:20Z- AHRS_M2 msg: 0.65 phi; -0.42 theta; -0.24 psi 2021-02-12T23:04:20Z- handleDepth()-Depth_Keller msg: 1613171060217 epoch sec; seqNo:486; depth 19.20 2021-02-12T23:04:20Z- handleDvl() - RDI_Pathfinder msg: 1613171060.23 epoch sec; seqNo:206 2021-02-12T23:04:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.02 , 96.65 , 66.25 , 118.57 2021-02-12T23:04:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , -0.01 2021-02-12T23:04:20Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-12T23:04:20Z- waiting for 1613171057.99, time is 1613171060.23 2021-02-12T23:04:20Z- LcmTrn::updateTrn() equating pose 1613171060.23 and meas times 1613171060.23 2021-02-12T23:04:20Z- LcmTrn::updateTrn() >>>> pose time 1613171060.23, meas time 1613171060.23 <<<< 2021-02-12T23:04:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171060.23 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:04:20Z- TerrainNav::motionUpdate() - currEstimate = (4056098, 590361.90, 19.20) 2021-02-12T23:04:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:04:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:04:20Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171060.23 sec.; No relevant navigation data available 2021-02-12T23:04:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:04:20Z- TNavBF::mmse selected from filter -1 2021-02-12T23:04:20Z- variances([0], [2], and psi_berg) = 827.318360 833.203507 0.000000 2021-02-12T23:04:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:04:20Z- LcmTrn::publishEstimates() - MLE : 4056058.47 590312.40 19.20 2021-02-12T23:04:20Z- LcmTrn::publishEstimates() - MMSE : 4056097.97 590361.90 19.20 2021-02-12T23:04:20Z- LcmTrn::publishEstimates() - COVAR: 827.32 833.20 1.06 0.00 2021-02-12T23:04:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171060.26 epoch sec; seqNo:486; 4056098.35 north; 590361.90 east 2021-02-12T23:04:20Z- AHRS_M2 msg: 1613171060.61 epoch sec; seqNo:484 2021-02-12T23:04:20Z- AHRS_M2 msg: 0.65 phi; -0.42 theta; -0.23 psi 2021-02-12T23:04:20Z- handleDepth()-Depth_Keller msg: 1613171060618 epoch sec; seqNo:487; depth 19.39 2021-02-12T23:04:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171060.63 epoch sec; seqNo:487; 4056098.35 north; 590361.90 east 2021-02-12T23:04:21Z- AHRS_M2 msg: 1613171061.02 epoch sec; seqNo:485 2021-02-12T23:04:21Z- AHRS_M2 msg: 0.64 phi; -0.42 theta; -0.23 psi 2021-02-12T23:04:21Z- handleDepth()-Depth_Keller msg: 1613171061024 epoch sec; seqNo:488; depth 19.54 2021-02-12T23:04:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171061.05 epoch sec; seqNo:488; 4056098.73 north; 590361.89 east 2021-02-12T23:04:21Z- AHRS_M2 msg: 1613171061.42 epoch sec; seqNo:486 2021-02-12T23:04:21Z- AHRS_M2 msg: 0.64 phi; -0.42 theta; -0.22 psi 2021-02-12T23:04:21Z- handleDepth()-Depth_Keller msg: 1613171061424 epoch sec; seqNo:489; depth 19.71 2021-02-12T23:04:21Z- handleDvl() - RDI_Pathfinder msg: 1613171061.44 epoch sec; seqNo:207 2021-02-12T23:04:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.57 , 96.65 , 65.43 , 116.14 2021-02-12T23:04:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , 0.00 2021-02-12T23:04:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171061.46 epoch sec; seqNo:489; 4056099.11 north; 590361.89 east 2021-02-12T23:04:21Z- AHRS_M2 msg: 1613171061.82 epoch sec; seqNo:487 2021-02-12T23:04:21Z- AHRS_M2 msg: 0.64 phi; -0.42 theta; -0.21 psi 2021-02-12T23:04:21Z- handleDepth()-Depth_Keller msg: 1613171061828 epoch sec; seqNo:490; depth 19.88 2021-02-12T23:04:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171061.84 epoch sec; seqNo:490; 4056099.50 north; 590363.11 east 2021-02-12T23:04:22Z- AHRS_M2 msg: 1613171062.23 epoch sec; seqNo:488 2021-02-12T23:04:22Z- AHRS_M2 msg: 0.64 phi; -0.42 theta; -0.20 psi 2021-02-12T23:04:22Z- handleDepth()-Depth_Keller msg: 1613171062242 epoch sec; seqNo:491; depth 20.05 2021-02-12T23:04:22Z- handleDvl() - RDI_Pathfinder msg: 1613171062.25 epoch sec; seqNo:208 2021-02-12T23:04:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.98 , 98.00 , 65.84 , 113.84 2021-02-12T23:04:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.15 , 0.03 2021-02-12T23:04:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171062.26 epoch sec; seqNo:491; 4056099.50 north; 590363.11 east 2021-02-12T23:04:22Z- AHRS_M2 msg: 1613171062.63 epoch sec; seqNo:489 2021-02-12T23:04:22Z- AHRS_M2 msg: 0.63 phi; -0.41 theta; -0.20 psi 2021-02-12T23:04:22Z- handleDepth()-Depth_Keller msg: 1613171062636 epoch sec; seqNo:492; depth 20.20 2021-02-12T23:04:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171062.65 epoch sec; seqNo:492; 4056099.88 north; 590363.10 east 2021-02-12T23:04:23Z- AHRS_M2 msg: 1613171063.04 epoch sec; seqNo:490 2021-02-12T23:04:23Z- AHRS_M2 msg: 0.63 phi; -0.41 theta; -0.19 psi 2021-02-12T23:04:23Z- handleDepth()-Depth_Keller msg: 1613171063057 epoch sec; seqNo:493; depth 20.35 2021-02-12T23:04:23Z- handleDvl() - RDI_Pathfinder msg: 1613171063.09 epoch sec; seqNo:209 2021-02-12T23:04:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.98 , 94.35 , 64.62 , 112.62 2021-02-12T23:04:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.17 , -0.00 2021-02-12T23:04:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171063.10 epoch sec; seqNo:493; 4056099.88 north; 590363.10 east 2021-02-12T23:04:23Z- AHRS_M2 msg: 1613171063.44 epoch sec; seqNo:491 2021-02-12T23:04:23Z- AHRS_M2 msg: 0.63 phi; -0.41 theta; -0.19 psi 2021-02-12T23:04:23Z- handleDepth()-Depth_Keller msg: 1613171063446 epoch sec; seqNo:494; depth 20.52 2021-02-12T23:04:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171063.46 epoch sec; seqNo:494; 4056100.26 north; 590363.10 east 2021-02-12T23:04:23Z- AHRS_M2 msg: 1613171063.84 epoch sec; seqNo:492 2021-02-12T23:04:23Z- AHRS_M2 msg: 0.62 phi; -0.41 theta; -0.19 psi 2021-02-12T23:04:23Z- handleDepth()-Depth_Keller msg: 1613171063868 epoch sec; seqNo:495; depth 20.64 2021-02-12T23:04:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171063.88 epoch sec; seqNo:495; 4056100.27 north; 590364.32 east 2021-02-12T23:04:24Z- AHRS_M2 msg: 1613171064.25 epoch sec; seqNo:493 2021-02-12T23:04:24Z- AHRS_M2 msg: 0.62 phi; -0.41 theta; -0.19 psi 2021-02-12T23:04:24Z- handleDepth()-Depth_Keller msg: 1613171064252 epoch sec; seqNo:496; depth 20.81 2021-02-12T23:04:24Z- handleDvl() - RDI_Pathfinder msg: 1613171064.26 epoch sec; seqNo:210 2021-02-12T23:04:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.57 , 94.48 , 65.03 , 112.75 2021-02-12T23:04:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.16 , -0.06 2021-02-12T23:04:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171064.29 epoch sec; seqNo:496; 4056100.65 north; 590364.32 east 2021-02-12T23:04:24Z- AHRS_M2 msg: 1613171064.65 epoch sec; seqNo:494 2021-02-12T23:04:24Z- AHRS_M2 msg: 0.62 phi; -0.41 theta; -0.19 psi 2021-02-12T23:04:24Z- handleDepth()-Depth_Keller msg: 1613171064656 epoch sec; seqNo:497; depth 20.96 2021-02-12T23:04:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171064.67 epoch sec; seqNo:497; 4056101.03 north; 590364.31 east 2021-02-12T23:04:25Z- AHRS_M2 msg: 1613171065.06 epoch sec; seqNo:495 2021-02-12T23:04:25Z- AHRS_M2 msg: 0.63 phi; -0.40 theta; -0.19 psi 2021-02-12T23:04:25Z- handleDepth()-Depth_Keller msg: 1613171065066 epoch sec; seqNo:498; depth 21.10 2021-02-12T23:04:25Z- handleDvl() - RDI_Pathfinder msg: 1613171065.08 epoch sec; seqNo:211 2021-02-12T23:04:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.16 , 93.26 , 64.62 , 111.54 2021-02-12T23:04:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.08 , -0.12 2021-02-12T23:04:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171065.09 epoch sec; seqNo:498; 4056101.40 north; 590364.31 east 2021-02-12T23:04:25Z- LcmTrn::handleMessages() - handling 42 messages took 0.69 ms and 129 calls... 2021-02-12T23:04:25Z- waiting for 1613171063.23, time is 1613171065.09 2021-02-12T23:04:25Z- LcmTrn::updateTrn() equating pose 1613171065.09 and meas times 1613171065.09 2021-02-12T23:04:25Z- LcmTrn::updateTrn() >>>> pose time 1613171065.09, meas time 1613171065.09 <<<< 2021-02-12T23:04:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171065.09 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:04:25Z- TerrainNav::motionUpdate() - currEstimate = (4056101, 590364.31, 21.10) 2021-02-12T23:04:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:04:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:04:25Z- TNavBF::Projecting Measurements 2021-02-12T23:04:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:04:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:04:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:04:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:04:25Z- TNavFilter::4 good beams. 2021-02-12T23:04:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:04:25Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:04:25Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:04:25Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:04:25Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:04:25Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:04:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:04:25Z- TNavBF::mmse selected from filter -1 2021-02-12T23:04:25Z- TerrainMapOctree: (vehN, vehE ) = (4056101.40, 590364.31). 2021-02-12T23:04:25Z- TerrainMapOctree: Min Distance = 6450.32. 2021-02-12T23:04:25Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:04:25Z- There is no specific good beam that all particles have in common. 2021-02-12T23:04:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:04:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.115 ExpectedMeasDiff: nan currMeas.covariance: 8.10 2021-02-12T23:04:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:04:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 13.92 2021-02-12T23:04:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:04:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 6.68 2021-02-12T23:04:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:04:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 19.90 2021-02-12T23:04:25Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:04:25Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:04:25Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:04:25Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:04:25Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:04:25Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:04:25Z- Filter 0 No beams used in subcloud update 2021-02-12T23:04:25Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:04:25Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:04:25Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 826.8 EastVariance: 833.2 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:04:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171065.09 sec. 2021-02-12T23:04:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:04:25Z- TNavBF::mmse selected from filter -1 2021-02-12T23:04:25Z- variances([0], [2], and psi_berg) = 826.827153 833.200314 0.000000 2021-02-12T23:04:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:04:25Z- LcmTrn::publishEstimates() - MLE : 4056061.90 590315.81 21.10 2021-02-12T23:04:25Z- LcmTrn::publishEstimates() - MMSE : 4056101.40 590364.31 21.10 2021-02-12T23:04:25Z- LcmTrn::publishEstimates() - COVAR: 826.83 833.20 0.90 0.00 2021-02-12T23:04:25Z- AHRS_M2 msg: 1613171065.46 epoch sec; seqNo:496 2021-02-12T23:04:25Z- AHRS_M2 msg: 0.63 phi; -0.40 theta; -0.20 psi 2021-02-12T23:04:25Z- handleDepth()-Depth_Keller msg: 1613171065464 epoch sec; seqNo:499; depth 21.27 2021-02-12T23:04:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171065.47 epoch sec; seqNo:499; 4056101.40 north; 590364.31 east 2021-02-12T23:04:25Z- AHRS_M2 msg: 1613171065.87 epoch sec; seqNo:497 2021-02-12T23:04:25Z- AHRS_M2 msg: 0.63 phi; -0.40 theta; -0.20 psi 2021-02-12T23:04:25Z- handleDepth()-Depth_Keller msg: 1613171065879 epoch sec; seqNo:500; depth 21.42 2021-02-12T23:04:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171065.89 epoch sec; seqNo:500; 4056101.80 north; 590365.53 east 2021-02-12T23:04:26Z- AHRS_M2 msg: 1613171066.27 epoch sec; seqNo:498 2021-02-12T23:04:26Z- AHRS_M2 msg: 0.63 phi; -0.40 theta; -0.20 psi 2021-02-12T23:04:26Z- handleDepth()-Depth_Keller msg: 1613171066281 epoch sec; seqNo:501; depth 21.58 2021-02-12T23:04:26Z- handleDvl() - RDI_Pathfinder msg: 1613171066.29 epoch sec; seqNo:212 2021-02-12T23:04:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.52 , 93.33 , 64.62 , 109.17 2021-02-12T23:04:26Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.03 , -0.13 2021-02-12T23:04:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171066.30 epoch sec; seqNo:501; 4056102.18 north; 590365.52 east 2021-02-12T23:04:26Z- AHRS_M2 msg: 1613171066.67 epoch sec; seqNo:499 2021-02-12T23:04:26Z- AHRS_M2 msg: 0.63 phi; -0.40 theta; -0.19 psi 2021-02-12T23:04:26Z- handleDepth()-Depth_Keller msg: 1613171066676 epoch sec; seqNo:502; depth 21.74 2021-02-12T23:04:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171066.69 epoch sec; seqNo:502; 4056102.55 north; 590365.52 east 2021-02-12T23:04:27Z- AHRS_M2 msg: 1613171067.08 epoch sec; seqNo:500 2021-02-12T23:04:27Z- AHRS_M2 msg: 0.63 phi; -0.39 theta; -0.19 psi 2021-02-12T23:04:27Z- handleDepth()-Depth_Keller msg: 1613171067080 epoch sec; seqNo:503; depth 21.87 2021-02-12T23:04:27Z- handleDvl() - RDI_Pathfinder msg: 1613171067.09 epoch sec; seqNo:213 2021-02-12T23:04:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.71 , 92.18 , 63.80 , 106.80 2021-02-12T23:04:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.16 , -0.10 2021-02-12T23:04:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171067.12 epoch sec; seqNo:503; 4056102.93 north; 590365.51 east 2021-02-12T23:04:27Z- AHRS_M2 msg: 1613171067.48 epoch sec; seqNo:501 2021-02-12T23:04:27Z- AHRS_M2 msg: 0.63 phi; -0.39 theta; -0.18 psi 2021-02-12T23:04:27Z- handleDepth()-Depth_Keller msg: 1613171067484 epoch sec; seqNo:504; depth 22.06 2021-02-12T23:04:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171067.49 epoch sec; seqNo:504; 4056103.31 north; 590365.51 east 2021-02-12T23:04:27Z- AHRS_M2 msg: 1613171067.89 epoch sec; seqNo:502 2021-02-12T23:04:27Z- AHRS_M2 msg: 0.63 phi; -0.39 theta; -0.18 psi 2021-02-12T23:04:27Z- handleDepth()-Depth_Keller msg: 1613171067901 epoch sec; seqNo:505; depth 22.21 2021-02-12T23:04:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171067.91 epoch sec; seqNo:505; 4056103.69 north; 590365.51 east 2021-02-12T23:04:28Z- AHRS_M2 msg: 1613171068.29 epoch sec; seqNo:503 2021-02-12T23:04:28Z- AHRS_M2 msg: 0.63 phi; -0.39 theta; -0.18 psi 2021-02-12T23:04:28Z- handleDepth()-Depth_Keller msg: 1613171068292 epoch sec; seqNo:506; depth 22.36 2021-02-12T23:04:28Z- handleDvl() - RDI_Pathfinder msg: 1613171068.31 epoch sec; seqNo:214 2021-02-12T23:04:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.89 , 89.88 , 63.39 , 104.50 2021-02-12T23:04:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.19 , -0.05 2021-02-12T23:04:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171068.32 epoch sec; seqNo:506; 4056103.69 north; 590365.51 east 2021-02-12T23:04:28Z- AHRS_M2 msg: 1613171068.69 epoch sec; seqNo:504 2021-02-12T23:04:28Z- AHRS_M2 msg: 0.63 phi; -0.39 theta; -0.18 psi 2021-02-12T23:04:28Z- handleDepth()-Depth_Keller msg: 1613171068696 epoch sec; seqNo:507; depth 22.52 2021-02-12T23:04:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171068.71 epoch sec; seqNo:507; 4056104.08 north; 590366.72 east 2021-02-12T23:04:29Z- AHRS_M2 msg: 1613171069.10 epoch sec; seqNo:505 2021-02-12T23:04:29Z- AHRS_M2 msg: 0.62 phi; -0.39 theta; -0.17 psi 2021-02-12T23:04:29Z- handleDepth()-Depth_Keller msg: 1613171069102 epoch sec; seqNo:508; depth 22.67 2021-02-12T23:04:29Z- handleDvl() - RDI_Pathfinder msg: 1613171069.12 epoch sec; seqNo:215 2021-02-12T23:04:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.89 , 90.29 , 62.16 , 105.44 2021-02-12T23:04:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.15 , 0.02 2021-02-12T23:04:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171069.13 epoch sec; seqNo:508; 4056104.46 north; 590366.72 east 2021-02-12T23:04:29Z- AHRS_M2 msg: 1613171069.50 epoch sec; seqNo:506 2021-02-12T23:04:29Z- AHRS_M2 msg: 0.62 phi; -0.39 theta; -0.17 psi 2021-02-12T23:04:29Z- handleDepth()-Depth_Keller msg: 1613171069504 epoch sec; seqNo:509; depth 22.83 2021-02-12T23:04:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171069.52 epoch sec; seqNo:509; 4056104.84 north; 590366.71 east 2021-02-12T23:04:29Z- AHRS_M2 msg: 1613171069.90 epoch sec; seqNo:507 2021-02-12T23:04:29Z- AHRS_M2 msg: 0.62 phi; -0.39 theta; -0.17 psi 2021-02-12T23:04:29Z- handleDepth()-Depth_Keller msg: 1613171069908 epoch sec; seqNo:510; depth 22.97 2021-02-12T23:04:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171069.92 epoch sec; seqNo:510; 4056104.84 north; 590366.71 east 2021-02-12T23:04:30Z- AHRS_M2 msg: 1613171070.31 epoch sec; seqNo:508 2021-02-12T23:04:30Z- AHRS_M2 msg: 0.61 phi; -0.39 theta; -0.17 psi 2021-02-12T23:04:30Z- handleDepth()-Depth_Keller msg: 1613171070312 epoch sec; seqNo:511; depth 23.12 2021-02-12T23:04:30Z- handleDvl() - RDI_Pathfinder msg: 1613171070.33 epoch sec; seqNo:216 2021-02-12T23:04:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.89 , 89.20 , 62.57 , 96.78 2021-02-12T23:04:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.07 , 0.03 2021-02-12T23:04:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171070.34 epoch sec; seqNo:511; 4056105.22 north; 590366.71 east 2021-02-12T23:04:30Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-12T23:04:30Z- waiting for 1613171068.09, time is 1613171070.34 2021-02-12T23:04:30Z- LcmTrn::updateTrn() equating pose 1613171070.34 and meas times 1613171070.34 2021-02-12T23:04:30Z- LcmTrn::updateTrn() >>>> pose time 1613171070.34, meas time 1613171070.34 <<<< 2021-02-12T23:04:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171070.34 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:04:30Z- TerrainNav::motionUpdate() - currEstimate = (4056105, 590366.71, 23.12) 2021-02-12T23:04:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:04:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:04:30Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171070.34 sec.; No relevant navigation data available 2021-02-12T23:04:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:04:30Z- TNavBF::mmse selected from filter -1 2021-02-12T23:04:30Z- variances([0], [2], and psi_berg) = 826.325832 833.197143 0.000000 2021-02-12T23:04:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:04:30Z- LcmTrn::publishEstimates() - MLE : 4056065.72 590319.21 23.12 2021-02-12T23:04:30Z- LcmTrn::publishEstimates() - MMSE : 4056105.22 590366.71 23.12 2021-02-12T23:04:30Z- LcmTrn::publishEstimates() - COVAR: 826.33 833.20 1.13 0.00 2021-02-12T23:04:30Z- AHRS_M2 msg: 1613171070.71 epoch sec; seqNo:509 2021-02-12T23:04:30Z- AHRS_M2 msg: 0.61 phi; -0.39 theta; -0.17 psi 2021-02-12T23:04:30Z- handleDepth()-Depth_Keller msg: 1613171070725 epoch sec; seqNo:512; depth 23.27 2021-02-12T23:04:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171070.74 epoch sec; seqNo:512; 4056105.60 north; 590366.71 east 2021-02-12T23:04:31Z- AHRS_M2 msg: 1613171071.12 epoch sec; seqNo:510 2021-02-12T23:04:31Z- AHRS_M2 msg: 0.61 phi; -0.39 theta; -0.18 psi 2021-02-12T23:04:31Z- handleDepth()-Depth_Keller msg: 1613171071154 epoch sec; seqNo:513; depth 23.43 2021-02-12T23:04:31Z- handleDvl() - RDI_Pathfinder msg: 1613171071.17 epoch sec; seqNo:217 2021-02-12T23:04:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.48 , 88.25 , 62.16 , 96.92 2021-02-12T23:04:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , 0.04 2021-02-12T23:04:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171071.19 epoch sec; seqNo:513; 4056105.98 north; 590366.70 east 2021-02-12T23:04:31Z- AHRS_M2 msg: 1613171071.53 epoch sec; seqNo:511 2021-02-12T23:04:31Z- AHRS_M2 msg: 0.61 phi; -0.39 theta; -0.18 psi 2021-02-12T23:04:31Z- handleDepth()-Depth_Keller msg: 1613171071531 epoch sec; seqNo:514; depth 23.60 2021-02-12T23:04:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171071.54 epoch sec; seqNo:514; 4056105.98 north; 590366.70 east 2021-02-12T23:04:31Z- AHRS_M2 msg: 1613171071.92 epoch sec; seqNo:512 2021-02-12T23:04:31Z- AHRS_M2 msg: 0.62 phi; -0.39 theta; -0.18 psi 2021-02-12T23:04:31Z- handleDepth()-Depth_Keller msg: 1613171071928 epoch sec; seqNo:515; depth 23.74 2021-02-12T23:04:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171071.95 epoch sec; seqNo:515; 4056106.37 north; 590367.92 east 2021-02-12T23:04:32Z- AHRS_M2 msg: 1613171072.33 epoch sec; seqNo:513 2021-02-12T23:04:32Z- AHRS_M2 msg: 0.62 phi; -0.39 theta; -0.18 psi 2021-02-12T23:04:32Z- handleDepth()-Depth_Keller msg: 1613171072333 epoch sec; seqNo:516; depth 23.89 2021-02-12T23:04:32Z- handleDvl() - RDI_Pathfinder msg: 1613171072.34 epoch sec; seqNo:218 2021-02-12T23:04:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.66 , 89.34 , 61.34 , 96.92 2021-02-12T23:04:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , 0.01 2021-02-12T23:04:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171072.37 epoch sec; seqNo:516; 4056106.75 north; 590367.92 east 2021-02-12T23:04:32Z- AHRS_M2 msg: 1613171072.73 epoch sec; seqNo:514 2021-02-12T23:04:32Z- AHRS_M2 msg: 0.62 phi; -0.39 theta; -0.18 psi 2021-02-12T23:04:32Z- handleDepth()-Depth_Keller msg: 1613171072738 epoch sec; seqNo:517; depth 24.04 2021-02-12T23:04:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171072.75 epoch sec; seqNo:517; 4056106.75 north; 590367.92 east 2021-02-12T23:04:33Z- AHRS_M2 msg: 1613171073.14 epoch sec; seqNo:515 2021-02-12T23:04:33Z- AHRS_M2 msg: 0.63 phi; -0.39 theta; -0.18 psi 2021-02-12T23:04:33Z- handleDepth()-Depth_Keller msg: 1613171073149 epoch sec; seqNo:518; depth 24.20 2021-02-12T23:04:33Z- handleDvl() - RDI_Pathfinder msg: 1613171073.16 epoch sec; seqNo:219 2021-02-12T23:04:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.25 , 86.09 , 61.75 , 96.92 2021-02-12T23:04:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , -0.02 2021-02-12T23:04:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171073.17 epoch sec; seqNo:518; 4056107.13 north; 590367.91 east 2021-02-12T23:04:33Z- AHRS_M2 msg: 1613171073.54 epoch sec; seqNo:516 2021-02-12T23:04:33Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.17 psi 2021-02-12T23:04:33Z- handleDepth()-Depth_Keller msg: 1613171073544 epoch sec; seqNo:519; depth 24.35 2021-02-12T23:04:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171073.56 epoch sec; seqNo:519; 4056107.50 north; 590367.91 east 2021-02-12T23:04:33Z- AHRS_M2 msg: 1613171073.94 epoch sec; seqNo:517 2021-02-12T23:04:33Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.16 psi 2021-02-12T23:04:33Z- handleDepth()-Depth_Keller msg: 1613171073955 epoch sec; seqNo:520; depth 24.49 2021-02-12T23:04:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171073.96 epoch sec; seqNo:520; 4056107.50 north; 590367.91 east 2021-02-12T23:04:34Z- AHRS_M2 msg: 1613171074.35 epoch sec; seqNo:518 2021-02-12T23:04:34Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.16 psi 2021-02-12T23:04:34Z- handleDepth()-Depth_Keller msg: 1613171074352 epoch sec; seqNo:521; depth 24.67 2021-02-12T23:04:34Z- handleDvl() - RDI_Pathfinder msg: 1613171074.38 epoch sec; seqNo:220 2021-02-12T23:04:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.43 , 87.17 , 61.34 , 104.50 2021-02-12T23:04:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , -0.03 2021-02-12T23:04:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171074.39 epoch sec; seqNo:521; 4056107.90 north; 590369.12 east 2021-02-12T23:04:34Z- AHRS_M2 msg: 1613171074.75 epoch sec; seqNo:519 2021-02-12T23:04:34Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.15 psi 2021-02-12T23:04:34Z- handleDepth()-Depth_Keller msg: 1613171074762 epoch sec; seqNo:522; depth 24.82 2021-02-12T23:04:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171074.78 epoch sec; seqNo:522; 4056108.28 north; 590369.12 east 2021-02-12T23:04:35Z- AHRS_M2 msg: 1613171075.16 epoch sec; seqNo:520 2021-02-12T23:04:35Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.14 psi 2021-02-12T23:04:35Z- handleDepth()-Depth_Keller msg: 1613171075166 epoch sec; seqNo:523; depth 24.97 2021-02-12T23:04:35Z- handleDvl() - RDI_Pathfinder msg: 1613171075.18 epoch sec; seqNo:221 2021-02-12T23:04:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.03 , 86.09 , 60.53 , 96.92 2021-02-12T23:04:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , -0.02 2021-02-12T23:04:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171075.19 epoch sec; seqNo:523; 4056108.65 north; 590369.12 east 2021-02-12T23:04:35Z- LcmTrn::handleMessages() - handling 41 messages took 0.69 ms and 128 calls... 2021-02-12T23:04:35Z- waiting for 1613171073.34, time is 1613171075.19 2021-02-12T23:04:35Z- LcmTrn::updateTrn() equating pose 1613171075.19 and meas times 1613171075.19 2021-02-12T23:04:35Z- LcmTrn::updateTrn() >>>> pose time 1613171075.19, meas time 1613171075.19 <<<< 2021-02-12T23:04:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171075.19 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:04:35Z- TerrainNav::motionUpdate() - currEstimate = (4056109, 590369.12, 24.97) 2021-02-12T23:04:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:04:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:04:35Z- TNavBF::Projecting Measurements 2021-02-12T23:04:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:04:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:04:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:04:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:04:35Z- TNavFilter::4 good beams. 2021-02-12T23:04:35Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:04:35Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:04:35Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:04:35Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:04:35Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:04:35Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:04:35Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:04:35Z- TNavBF::mmse selected from filter -1 2021-02-12T23:04:35Z- TerrainMapOctree: (vehN, vehE ) = (4056108.65, 590369.12). 2021-02-12T23:04:35Z- TerrainMapOctree: Min Distance = 6441.64. 2021-02-12T23:04:35Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:04:35Z- There is no specific good beam that all particles have in common. 2021-02-12T23:04:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:04:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.136 ExpectedMeasDiff: nan currMeas.covariance: 6.77 2021-02-12T23:04:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:04:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 11.86 2021-02-12T23:04:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:04:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.123 ExpectedMeasDiff: nan currMeas.covariance: 5.86 2021-02-12T23:04:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:04:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 15.03 2021-02-12T23:04:35Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:04:35Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:04:35Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:04:35Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:04:35Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:04:35Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:04:35Z- Filter 0 No beams used in subcloud update 2021-02-12T23:04:35Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:04:35Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:04:35Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 825.8 EastVariance: 833.2 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:04:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171075.19 sec. 2021-02-12T23:04:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:04:35Z- TNavBF::mmse selected from filter -1 2021-02-12T23:04:35Z- variances([0], [2], and psi_berg) = 825.849913 833.194217 0.000000 2021-02-12T23:04:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:04:35Z- LcmTrn::publishEstimates() - MLE : 4056069.15 590321.62 24.97 2021-02-12T23:04:35Z- LcmTrn::publishEstimates() - MMSE : 4056108.65 590369.12 24.97 2021-02-12T23:04:35Z- LcmTrn::publishEstimates() - COVAR: 825.85 833.19 0.89 0.00 2021-02-12T23:04:35Z- AHRS_M2 msg: 1613171075.57 epoch sec; seqNo:521 2021-02-12T23:04:35Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.13 psi 2021-02-12T23:04:35Z- handleDepth()-Depth_Keller msg: 1613171075570 epoch sec; seqNo:524; depth 25.13 2021-02-12T23:04:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171075.58 epoch sec; seqNo:524; 4056108.65 north; 590369.12 east 2021-02-12T23:04:35Z- handleDepth()-Depth_Keller msg: 1613171075993 epoch sec; seqNo:525; depth 25.28 2021-02-12T23:04:36Z- handleDvl() - RDI_Pathfinder msg: 1613171076.01 epoch sec; seqNo:222 2021-02-12T23:04:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.80 , 78.52 , 59.71 , 96.92 2021-02-12T23:04:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.07 , -0.01 2021-02-12T23:04:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171076.02 epoch sec; seqNo:525; 4056109.03 north; 590369.11 east 2021-02-12T23:04:36Z- AHRS_M2 msg: 1613171076.37 epoch sec; seqNo:522 2021-02-12T23:04:36Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.13 psi 2021-02-12T23:04:36Z- handleDepth()-Depth_Keller msg: 1613171076377 epoch sec; seqNo:526; depth 25.43 2021-02-12T23:04:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171076.39 epoch sec; seqNo:526; 4056109.41 north; 590369.11 east 2021-02-12T23:04:36Z- AHRS_M2 msg: 1613171076.77 epoch sec; seqNo:523 2021-02-12T23:04:36Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.13 psi 2021-02-12T23:04:36Z- handleDepth()-Depth_Keller msg: 1613171076776 epoch sec; seqNo:527; depth 25.55 2021-02-12T23:04:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171076.79 epoch sec; seqNo:527; 4056109.42 north; 590370.33 east 2021-02-12T23:04:37Z- AHRS_M2 msg: 1613171077.18 epoch sec; seqNo:524 2021-02-12T23:04:37Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.13 psi 2021-02-12T23:04:37Z- handleDepth()-Depth_Keller msg: 1613171077190 epoch sec; seqNo:528; depth 25.70 2021-02-12T23:04:37Z- handleDvl() - RDI_Pathfinder msg: 1613171077.20 epoch sec; seqNo:223 2021-02-12T23:04:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.84 , 77.70 , 60.53 , 96.92 2021-02-12T23:04:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.08 , -0.03 2021-02-12T23:04:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171077.21 epoch sec; seqNo:528; 4056109.80 north; 590370.33 east 2021-02-12T23:04:37Z- AHRS_M2 msg: 1613171077.58 epoch sec; seqNo:525 2021-02-12T23:04:37Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.14 psi 2021-02-12T23:04:37Z- handleDepth()-Depth_Keller msg: 1613171077589 epoch sec; seqNo:529; depth 25.85 2021-02-12T23:04:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171077.60 epoch sec; seqNo:529; 4056109.80 north; 590370.33 east 2021-02-12T23:04:37Z- AHRS_M2 msg: 1613171077.98 epoch sec; seqNo:526 2021-02-12T23:04:37Z- AHRS_M2 msg: 0.70 phi; -0.36 theta; -0.14 psi 2021-02-12T23:04:37Z- handleDepth()-Depth_Keller msg: 1613171077993 epoch sec; seqNo:530; depth 26.00 2021-02-12T23:04:38Z- handleDvl() - RDI_Pathfinder msg: 1613171078.01 epoch sec; seqNo:224 2021-02-12T23:04:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.62 , 78.11 , 59.71 , 96.92 2021-02-12T23:04:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.06 , -0.03 2021-02-12T23:04:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171078.02 epoch sec; seqNo:530; 4056110.18 north; 590370.32 east 2021-02-12T23:04:38Z- AHRS_M2 msg: 1613171078.39 epoch sec; seqNo:527 2021-02-12T23:04:38Z- AHRS_M2 msg: 0.71 phi; -0.36 theta; -0.14 psi 2021-02-12T23:04:38Z- handleDepth()-Depth_Keller msg: 1613171078405 epoch sec; seqNo:531; depth 26.13 2021-02-12T23:04:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171078.42 epoch sec; seqNo:531; 4056110.56 north; 590370.32 east 2021-02-12T23:04:38Z- AHRS_M2 msg: 1613171078.79 epoch sec; seqNo:528 2021-02-12T23:04:38Z- AHRS_M2 msg: 0.72 phi; -0.36 theta; -0.14 psi 2021-02-12T23:04:38Z- handleDepth()-Depth_Keller msg: 1613171078796 epoch sec; seqNo:532; depth 26.28 2021-02-12T23:04:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171078.82 epoch sec; seqNo:532; 4056110.57 north; 590371.54 east 2021-02-12T23:04:39Z- AHRS_M2 msg: 1613171079.19 epoch sec; seqNo:529 2021-02-12T23:04:39Z- AHRS_M2 msg: 0.72 phi; -0.36 theta; -0.15 psi 2021-02-12T23:04:39Z- handleDepth()-Depth_Keller msg: 1613171079200 epoch sec; seqNo:533; depth 26.40 2021-02-12T23:04:39Z- handleDvl() - RDI_Pathfinder msg: 1613171079.21 epoch sec; seqNo:225 2021-02-12T23:04:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.80 , 78.11 , 58.48 , 96.92 2021-02-12T23:04:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.06 , -0.04 2021-02-12T23:04:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171079.24 epoch sec; seqNo:533; 4056110.95 north; 590371.54 east 2021-02-12T23:04:39Z- AHRS_M2 msg: 1613171079.60 epoch sec; seqNo:530 2021-02-12T23:04:39Z- AHRS_M2 msg: 0.72 phi; -0.36 theta; -0.15 psi 2021-02-12T23:04:39Z- handleDepth()-Depth_Keller msg: 1613171079604 epoch sec; seqNo:534; depth 26.55 2021-02-12T23:04:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171079.62 epoch sec; seqNo:534; 4056111.33 north; 590371.53 east 2021-02-12T23:04:40Z- AHRS_M2 msg: 1613171080.00 epoch sec; seqNo:531 2021-02-12T23:04:40Z- AHRS_M2 msg: 0.72 phi; -0.36 theta; -0.15 psi 2021-02-12T23:04:40Z- handleDepth()-Depth_Keller msg: 1613171080017 epoch sec; seqNo:535; depth 26.68 2021-02-12T23:04:40Z- handleDvl() - RDI_Pathfinder msg: 1613171080.03 epoch sec; seqNo:226 2021-02-12T23:04:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.39 , 78.52 , 59.30 , 96.92 2021-02-12T23:04:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.07 , -0.04 2021-02-12T23:04:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171080.04 epoch sec; seqNo:535; 4056111.33 north; 590371.53 east 2021-02-12T23:04:40Z- AHRS_M2 msg: 1613171080.41 epoch sec; seqNo:532 2021-02-12T23:04:40Z- AHRS_M2 msg: 0.72 phi; -0.36 theta; -0.16 psi 2021-02-12T23:04:40Z- handleDepth()-Depth_Keller msg: 1613171080412 epoch sec; seqNo:536; depth 26.84 2021-02-12T23:04:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171080.43 epoch sec; seqNo:536; 4056111.72 north; 590372.75 east 2021-02-12T23:04:40Z- LcmTrn::handleMessages() - handling 43 messages took 0.66 ms and 130 calls... 2021-02-12T23:04:40Z- waiting for 1613171078.19, time is 1613171080.43 2021-02-12T23:04:40Z- LcmTrn::updateTrn() >>>> pose time 1613171080.43, meas time 1613171080.03 <<<< 2021-02-12T23:04:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171080.03 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:04:40Z- TerrainNav::motionUpdate() - currEstimate = (4056112, 590372.75, 26.84) 2021-02-12T23:04:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:04:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:04:40Z- TNavBF::Projecting Measurements 2021-02-12T23:04:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:04:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:04:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:04:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:04:40Z- TNavFilter::4 good beams. 2021-02-12T23:04:40Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:04:40Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:04:40Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:04:40Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:04:40Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:04:40Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:04:40Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:04:40Z- TNavBF::mmse selected from filter -1 2021-02-12T23:04:40Z- TerrainMapOctree: (vehN, vehE ) = (4056111.49, 590372.47). 2021-02-12T23:04:40Z- TerrainMapOctree: Min Distance = 6437.36. 2021-02-12T23:04:40Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:04:40Z- There is no specific good beam that all particles have in common. 2021-02-12T23:04:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:04:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.120 ExpectedMeasDiff: nan currMeas.covariance: 6.43 2021-02-12T23:04:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:04:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-12T23:04:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:04:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 5.63 2021-02-12T23:04:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:04:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 15.03 2021-02-12T23:04:40Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:04:40Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:04:40Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:04:40Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:04:40Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:04:40Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:04:40Z- Filter 0 No beams used in subcloud update 2021-02-12T23:04:40Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:04:40Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:04:40Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 825.4 EastVariance: 833.2 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:04:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171080.03 sec. 2021-02-12T23:04:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:04:40Z- TNavBF::mmse selected from filter -1 2021-02-12T23:04:40Z- variances([0], [2], and psi_berg) = 825.234101 833.190551 0.000000 2021-02-12T23:04:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:04:40Z- LcmTrn::publishEstimates() - MLE : 4056073.22 590323.25 26.84 2021-02-12T23:04:40Z- LcmTrn::publishEstimates() - MMSE : 4056111.72 590372.75 26.84 2021-02-12T23:04:40Z- LcmTrn::publishEstimates() - COVAR: 825.23 833.19 0.78 0.00 2021-02-12T23:04:40Z- AHRS_M2 msg: 1613171080.81 epoch sec; seqNo:533 2021-02-12T23:04:40Z- AHRS_M2 msg: 0.73 phi; -0.36 theta; -0.16 psi 2021-02-12T23:04:40Z- handleDepth()-Depth_Keller msg: 1613171080816 epoch sec; seqNo:537; depth 26.95 2021-02-12T23:04:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171080.83 epoch sec; seqNo:537; 4056112.10 north; 590372.74 east 2021-02-12T23:04:41Z- AHRS_M2 msg: 1613171081.21 epoch sec; seqNo:534 2021-02-12T23:04:41Z- AHRS_M2 msg: 0.73 phi; -0.36 theta; -0.17 psi 2021-02-12T23:04:41Z- handleDepth()-Depth_Keller msg: 1613171081219 epoch sec; seqNo:538; depth 27.10 2021-02-12T23:04:41Z- handleDvl() - RDI_Pathfinder msg: 1613171081.23 epoch sec; seqNo:227 2021-02-12T23:04:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.98 , 78.52 , 58.48 , 96.92 2021-02-12T23:04:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , -0.06 2021-02-12T23:04:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171081.24 epoch sec; seqNo:538; 4056112.10 north; 590372.74 east 2021-02-12T23:04:41Z- AHRS_M2 msg: 1613171081.62 epoch sec; seqNo:535 2021-02-12T23:04:41Z- AHRS_M2 msg: 0.72 phi; -0.36 theta; -0.17 psi 2021-02-12T23:04:41Z- handleDepth()-Depth_Keller msg: 1613171081626 epoch sec; seqNo:539; depth 27.23 2021-02-12T23:04:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171081.64 epoch sec; seqNo:539; 4056112.48 north; 590372.74 east 2021-02-12T23:04:42Z- AHRS_M2 msg: 1613171082.02 epoch sec; seqNo:536 2021-02-12T23:04:42Z- AHRS_M2 msg: 0.72 phi; -0.36 theta; -0.17 psi 2021-02-12T23:04:42Z- handleDepth()-Depth_Keller msg: 1613171082029 epoch sec; seqNo:540; depth 27.37 2021-02-12T23:04:42Z- handleDvl() - RDI_Pathfinder msg: 1613171082.05 epoch sec; seqNo:228 2021-02-12T23:04:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.75 , 78.52 , 58.48 , 95.83 2021-02-12T23:04:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , -0.06 2021-02-12T23:04:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171082.06 epoch sec; seqNo:540; 4056112.86 north; 590372.74 east 2021-02-12T23:04:42Z- AHRS_M2 msg: 1613171082.43 epoch sec; seqNo:537 2021-02-12T23:04:42Z- AHRS_M2 msg: 0.72 phi; -0.36 theta; -0.18 psi 2021-02-12T23:04:42Z- handleDepth()-Depth_Keller msg: 1613171082433 epoch sec; seqNo:541; depth 27.51 2021-02-12T23:04:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171082.44 epoch sec; seqNo:541; 4056112.87 north; 590373.96 east 2021-02-12T23:04:42Z- AHRS_M2 msg: 1613171082.83 epoch sec; seqNo:538 2021-02-12T23:04:42Z- AHRS_M2 msg: 0.71 phi; -0.36 theta; -0.19 psi 2021-02-12T23:04:42Z- handleDepth()-Depth_Keller msg: 1613171082837 epoch sec; seqNo:542; depth 27.65 2021-02-12T23:04:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171082.85 epoch sec; seqNo:542; 4056113.25 north; 590373.95 east 2021-02-12T23:04:43Z- AHRS_M2 msg: 1613171083.24 epoch sec; seqNo:539 2021-02-12T23:04:43Z- AHRS_M2 msg: 0.71 phi; -0.37 theta; -0.19 psi 2021-02-12T23:04:43Z- handleDepth()-Depth_Keller msg: 1613171083241 epoch sec; seqNo:543; depth 27.79 2021-02-12T23:04:43Z- handleDvl() - RDI_Pathfinder msg: 1613171083.25 epoch sec; seqNo:229 2021-02-12T23:04:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.94 , 78.93 , 58.48 , 95.83 2021-02-12T23:04:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , -0.07 2021-02-12T23:04:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171083.28 epoch sec; seqNo:543; 4056113.63 north; 590373.95 east 2021-02-12T23:04:43Z- AHRS_M2 msg: 1613171083.64 epoch sec; seqNo:540 2021-02-12T23:04:43Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.20 psi 2021-02-12T23:04:43Z- handleDepth()-Depth_Keller msg: 1613171083644 epoch sec; seqNo:544; depth 27.94 2021-02-12T23:04:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171083.65 epoch sec; seqNo:544; 4056114.01 north; 590373.95 east 2021-02-12T23:04:44Z- AHRS_M2 msg: 1613171084.05 epoch sec; seqNo:541 2021-02-12T23:04:44Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.21 psi 2021-02-12T23:04:44Z- handleDepth()-Depth_Keller msg: 1613171084061 epoch sec; seqNo:545; depth 28.07 2021-02-12T23:04:44Z- handleDvl() - RDI_Pathfinder msg: 1613171084.08 epoch sec; seqNo:230 2021-02-12T23:04:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.12 , 85.01 , 58.48 , 95.83 2021-02-12T23:04:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.08 2021-02-12T23:04:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171084.09 epoch sec; seqNo:545; 4056114.01 north; 590373.95 east 2021-02-12T23:04:44Z- AHRS_M2 msg: 1613171084.45 epoch sec; seqNo:542 2021-02-12T23:04:44Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.21 psi 2021-02-12T23:04:44Z- handleDepth()-Depth_Keller msg: 1613171084452 epoch sec; seqNo:546; depth 28.23 2021-02-12T23:04:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171084.47 epoch sec; seqNo:546; 4056114.39 north; 590373.94 east 2021-02-12T23:04:44Z- AHRS_M2 msg: 1613171084.85 epoch sec; seqNo:543 2021-02-12T23:04:44Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.21 psi 2021-02-12T23:04:44Z- handleDepth()-Depth_Keller msg: 1613171084860 epoch sec; seqNo:547; depth 28.36 2021-02-12T23:04:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171084.88 epoch sec; seqNo:547; 4056114.78 north; 590375.16 east 2021-02-12T23:04:45Z- AHRS_M2 msg: 1613171085.26 epoch sec; seqNo:544 2021-02-12T23:04:45Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.22 psi 2021-02-12T23:04:45Z- handleDepth()-Depth_Keller msg: 1613171085262 epoch sec; seqNo:548; depth 28.51 2021-02-12T23:04:45Z- handleDvl() - RDI_Pathfinder msg: 1613171085.27 epoch sec; seqNo:231 2021-02-12T23:04:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.53 , 85.01 , 58.07 , 96.92 2021-02-12T23:04:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , -0.05 2021-02-12T23:04:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171085.28 epoch sec; seqNo:548; 4056115.16 north; 590375.15 east 2021-02-12T23:04:45Z- LcmTrn::handleMessages() - handling 41 messages took 0.81 ms and 125 calls... 2021-02-12T23:04:45Z- waiting for 1613171083.43, time is 1613171085.28 2021-02-12T23:04:45Z- LcmTrn::updateTrn() equating pose 1613171085.28 and meas times 1613171085.28 2021-02-12T23:04:45Z- LcmTrn::updateTrn() >>>> pose time 1613171085.28, meas time 1613171085.28 <<<< 2021-02-12T23:04:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171085.28 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:04:45Z- TerrainNav::motionUpdate() - currEstimate = (4056115, 590375.15, 28.51) 2021-02-12T23:04:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:04:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:04:45Z- TNavBF::Projecting Measurements 2021-02-12T23:04:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:04:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:04:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:04:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:04:45Z- TNavFilter::4 good beams. 2021-02-12T23:04:45Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:04:45Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:04:45Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:04:45Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:04:45Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:04:45Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:04:45Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:04:45Z- TNavBF::mmse selected from filter -1 2021-02-12T23:04:45Z- TerrainMapOctree: (vehN, vehE ) = (4056115.16, 590375.15). 2021-02-12T23:04:45Z- TerrainMapOctree: Min Distance = 6432.81. 2021-02-12T23:04:45Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:04:45Z- There is no specific good beam that all particles have in common. 2021-02-12T23:04:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:04:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.116 ExpectedMeasDiff: nan currMeas.covariance: 5.86 2021-02-12T23:04:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:04:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 11.56 2021-02-12T23:04:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:04:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 5.40 2021-02-12T23:04:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:04:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.74 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 15.03 2021-02-12T23:04:45Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:04:45Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:04:45Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:04:45Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:04:45Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:04:45Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:04:45Z- Filter 0 No beams used in subcloud update 2021-02-12T23:04:45Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:04:45Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:04:45Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 824.8 EastVariance: 833.2 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:04:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171085.28 sec. 2021-02-12T23:04:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:04:45Z- TNavBF::mmse selected from filter -1 2021-02-12T23:04:45Z- variances([0], [2], and psi_berg) = 824.774354 833.187890 0.000000 2021-02-12T23:04:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:04:45Z- LcmTrn::publishEstimates() - MLE : 4056076.66 590326.65 28.51 2021-02-12T23:04:45Z- LcmTrn::publishEstimates() - MMSE : 4056115.16 590375.15 28.51 2021-02-12T23:04:45Z- LcmTrn::publishEstimates() - COVAR: 824.77 833.19 0.71 0.00 2021-02-12T23:04:45Z- AHRS_M2 msg: 1613171085.66 epoch sec; seqNo:545 2021-02-12T23:04:45Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.22 psi 2021-02-12T23:04:45Z- handleDepth()-Depth_Keller msg: 1613171085664 epoch sec; seqNo:549; depth 28.66 2021-02-12T23:04:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171085.68 epoch sec; seqNo:549; 4056115.16 north; 590375.15 east 2021-02-12T23:04:46Z- AHRS_M2 msg: 1613171086.06 epoch sec; seqNo:546 2021-02-12T23:04:46Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.22 psi 2021-02-12T23:04:46Z- handleDepth()-Depth_Keller msg: 1613171086068 epoch sec; seqNo:550; depth 28.80 2021-02-12T23:04:46Z- handleDvl() - RDI_Pathfinder msg: 1613171086.08 epoch sec; seqNo:232 2021-02-12T23:04:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 59.30 , 87.17 , 58.07 , 96.92 2021-02-12T23:04:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , -0.06 2021-02-12T23:04:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171086.09 epoch sec; seqNo:550; 4056115.54 north; 590375.15 east 2021-02-12T23:04:46Z- AHRS_M2 msg: 1613171086.47 epoch sec; seqNo:547 2021-02-12T23:04:46Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.22 psi 2021-02-12T23:04:46Z- handleDepth()-Depth_Keller msg: 1613171086474 epoch sec; seqNo:551; depth 28.94 2021-02-12T23:04:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171086.48 epoch sec; seqNo:551; 4056115.92 north; 590375.15 east 2021-02-12T23:04:46Z- AHRS_M2 msg: 1613171086.87 epoch sec; seqNo:548 2021-02-12T23:04:46Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.22 psi 2021-02-12T23:04:46Z- handleDepth()-Depth_Keller msg: 1613171086876 epoch sec; seqNo:552; depth 29.10 2021-02-12T23:04:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171086.90 epoch sec; seqNo:552; 4056116.30 north; 590375.14 east 2021-02-12T23:04:47Z- AHRS_M2 msg: 1613171087.28 epoch sec; seqNo:549 2021-02-12T23:04:47Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.22 psi 2021-02-12T23:04:47Z- handleDepth()-Depth_Keller msg: 1613171087284 epoch sec; seqNo:553; depth 29.25 2021-02-12T23:04:47Z- handleDvl() - RDI_Pathfinder msg: 1613171087.33 epoch sec; seqNo:233 2021-02-12T23:04:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 58.89 , 79.75 , 57.66 , 96.92 2021-02-12T23:04:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , -0.01 2021-02-12T23:04:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171087.34 epoch sec; seqNo:553; 4056116.31 north; 590376.36 east 2021-02-12T23:04:47Z- AHRS_M2 msg: 1613171087.68 epoch sec; seqNo:550 2021-02-12T23:04:47Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.22 psi 2021-02-12T23:04:47Z- handleDepth()-Depth_Keller msg: 1613171087686 epoch sec; seqNo:554; depth 29.43 2021-02-12T23:04:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171087.70 epoch sec; seqNo:554; 4056116.69 north; 590376.36 east 2021-02-12T23:04:48Z- AHRS_M2 msg: 1613171088.08 epoch sec; seqNo:551 2021-02-12T23:04:48Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.21 psi 2021-02-12T23:04:48Z- handleDepth()-Depth_Keller msg: 1613171088088 epoch sec; seqNo:555; depth 29.58 2021-02-12T23:04:48Z- handleDvl() - RDI_Pathfinder msg: 1613171088.11 epoch sec; seqNo:234 2021-02-12T23:04:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 58.89 , 78.93 , 57.26 , 92.59 2021-02-12T23:04:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , 0.03 2021-02-12T23:04:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171088.12 epoch sec; seqNo:555; 4056117.07 north; 590376.36 east 2021-02-12T23:04:48Z- AHRS_M2 msg: 1613171088.49 epoch sec; seqNo:552 2021-02-12T23:04:48Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.21 psi 2021-02-12T23:04:48Z- handleDepth()-Depth_Keller msg: 1613171088497 epoch sec; seqNo:556; depth 29.75 2021-02-12T23:04:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171088.51 epoch sec; seqNo:556; 4056117.07 north; 590376.36 east 2021-02-12T23:04:48Z- AHRS_M2 msg: 1613171088.89 epoch sec; seqNo:553 2021-02-12T23:04:48Z- AHRS_M2 msg: 0.62 phi; -0.26 theta; -0.20 psi 2021-02-12T23:04:48Z- handleDepth()-Depth_Keller msg: 1613171088896 epoch sec; seqNo:557; depth 29.91 2021-02-12T23:04:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171088.90 epoch sec; seqNo:557; 4056117.44 north; 590376.35 east 2021-02-12T23:04:49Z- AHRS_M2 msg: 1613171089.30 epoch sec; seqNo:554 2021-02-12T23:04:49Z- AHRS_M2 msg: 0.61 phi; -0.22 theta; -0.18 psi 2021-02-12T23:04:49Z- handleDepth()-Depth_Keller msg: 1613171089302 epoch sec; seqNo:558; depth 30.07 2021-02-12T23:04:49Z- handleDvl() - RDI_Pathfinder msg: 1613171089.31 epoch sec; seqNo:235 2021-02-12T23:04:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 57.66 , 74.84 , 57.66 , 83.92 2021-02-12T23:04:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , 0.05 2021-02-12T23:04:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171089.32 epoch sec; seqNo:558; 4056117.82 north; 590376.35 east 2021-02-12T23:04:49Z- AHRS_M2 msg: 1613171089.70 epoch sec; seqNo:555 2021-02-12T23:04:49Z- AHRS_M2 msg: 0.60 phi; -0.17 theta; -0.17 psi 2021-02-12T23:04:49Z- handleDepth()-Depth_Keller msg: 1613171089704 epoch sec; seqNo:559; depth 30.21 2021-02-12T23:04:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171089.72 epoch sec; seqNo:559; 4056118.22 north; 590377.56 east 2021-02-12T23:04:50Z- AHRS_M2 msg: 1613171090.10 epoch sec; seqNo:556 2021-02-12T23:04:50Z- AHRS_M2 msg: 0.59 phi; -0.13 theta; -0.15 psi 2021-02-12T23:04:50Z- handleDepth()-Depth_Keller msg: 1613171090109 epoch sec; seqNo:560; depth 30.34 2021-02-12T23:04:50Z- handleDvl() - RDI_Pathfinder msg: 1613171090.12 epoch sec; seqNo:236 2021-02-12T23:04:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 58.07 , 70.75 , 59.71 , 72.39 2021-02-12T23:04:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.08 , 0.03 2021-02-12T23:04:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171090.15 epoch sec; seqNo:560; 4056118.22 north; 590377.56 east 2021-02-12T23:04:50Z- AHRS_M2 msg: 1613171090.51 epoch sec; seqNo:557 2021-02-12T23:04:50Z- AHRS_M2 msg: 0.58 phi; -0.08 theta; -0.14 psi 2021-02-12T23:04:50Z- handleDepth()-Depth_Keller msg: 1613171090517 epoch sec; seqNo:561; depth 30.47 2021-02-12T23:04:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171090.53 epoch sec; seqNo:561; 4056118.59 north; 590377.56 east 2021-02-12T23:04:50Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-12T23:04:50Z- waiting for 1613171088.28, time is 1613171090.53 2021-02-12T23:04:50Z- LcmTrn::updateTrn() >>>> pose time 1613171090.53, meas time 1613171090.12 <<<< 2021-02-12T23:04:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171090.12 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:04:50Z- TerrainNav::motionUpdate() - currEstimate = (4056119, 590377.56, 30.47) 2021-02-12T23:04:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:04:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:04:50Z- TNavBF::Projecting Measurements 2021-02-12T23:04:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:04:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:04:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:04:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:04:50Z- TNavFilter::4 good beams. 2021-02-12T23:04:50Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:04:50Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:04:50Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:04:50Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:04:50Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:04:50Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:04:50Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:04:50Z- TNavBF::mmse selected from filter -1 2021-02-12T23:04:50Z- TerrainMapOctree: (vehN, vehE ) = (4056118.33, 590377.38). 2021-02-12T23:04:50Z- TerrainMapOctree: Min Distance = 6428.94. 2021-02-12T23:04:50Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:04:50Z- There is no specific good beam that all particles have in common. 2021-02-12T23:04:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:04:50Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 5.40 2021-02-12T23:04:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:04:50Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 8.01 2021-02-12T23:04:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:04:50Z- Old Depth Bias: 0.00 DepthBiasCov: 0.74 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 5.70 2021-02-12T23:04:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:04:50Z- Old Depth Bias: 0.00 DepthBiasCov: 0.68 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 8.38 2021-02-12T23:04:50Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:04:50Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:04:50Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:04:50Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:04:50Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:04:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:04:50Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:04:50Z- Filter 0 No beams used in subcloud update 2021-02-12T23:04:50Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:04:50Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:04:50Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 824.3 EastVariance: 833.2 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-12T23:04:50Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171090.12 sec. 2021-02-12T23:04:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:04:50Z- TNavBF::mmse selected from filter -1 2021-02-12T23:04:50Z- variances([0], [2], and psi_berg) = 824.215009 833.184749 0.000000 2021-02-12T23:04:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:04:50Z- LcmTrn::publishEstimates() - MLE : 4056080.09 590330.06 30.47 2021-02-12T23:04:50Z- LcmTrn::publishEstimates() - MMSE : 4056118.59 590377.56 30.47 2021-02-12T23:04:50Z- LcmTrn::publishEstimates() - COVAR: 824.22 833.18 0.65 0.00 2021-02-12T23:04:50Z- AHRS_M2 msg: 1613171090.91 epoch sec; seqNo:558 2021-02-12T23:04:50Z- AHRS_M2 msg: 0.58 phi; -0.04 theta; -0.13 psi 2021-02-12T23:04:50Z- handleDepth()-Depth_Keller msg: 1613171090917 epoch sec; seqNo:562; depth 30.57 2021-02-12T23:04:50Z- handleDvl() - RDI_Pathfinder msg: 1613171090.93 epoch sec; seqNo:237 2021-02-12T23:04:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 59.53 , 68.12 , 61.98 , 66.28 2021-02-12T23:04:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.13 , -0.02 2021-02-12T23:04:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171090.94 epoch sec; seqNo:562; 4056118.97 north; 590377.56 east 2021-02-12T23:04:51Z- AHRS_M2 msg: 1613171091.32 epoch sec; seqNo:559 2021-02-12T23:04:51Z- AHRS_M2 msg: 0.57 phi; 0.00 theta; -0.12 psi 2021-02-12T23:04:51Z- handleDepth()-Depth_Keller msg: 1613171091330 epoch sec; seqNo:563; depth 30.65 2021-02-12T23:04:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171091.35 epoch sec; seqNo:563; 4056119.35 north; 590377.55 east 2021-02-12T23:04:51Z- AHRS_M2 msg: 1613171091.72 epoch sec; seqNo:560 2021-02-12T23:04:51Z- AHRS_M2 msg: 0.57 phi; 0.04 theta; -0.11 psi 2021-02-12T23:04:51Z- handleDepth()-Depth_Keller msg: 1613171091729 epoch sec; seqNo:564; depth 30.71 2021-02-12T23:04:51Z- handleDvl() - RDI_Pathfinder msg: 1613171091.74 epoch sec; seqNo:238 2021-02-12T23:04:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.14 , 67.20 , 65.36 , 61.98 2021-02-12T23:04:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.12 , -0.09 2021-02-12T23:04:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171091.77 epoch sec; seqNo:564; 4056119.36 north; 590378.77 east 2021-02-12T23:04:52Z- AHRS_M2 msg: 1613171092.14 epoch sec; seqNo:561 2021-02-12T23:04:52Z- AHRS_M2 msg: 0.57 phi; 0.12 theta; -0.11 psi 2021-02-12T23:04:52Z- handleDepth()-Depth_Keller msg: 1613171092148 epoch sec; seqNo:565; depth 30.77 2021-02-12T23:04:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171092.16 epoch sec; seqNo:565; 4056119.74 north; 590378.77 east 2021-02-12T23:04:52Z- AHRS_M2 msg: 1613171092.53 epoch sec; seqNo:562 2021-02-12T23:04:52Z- AHRS_M2 msg: 0.57 phi; 0.15 theta; -0.10 psi 2021-02-12T23:04:52Z- handleDepth()-Depth_Keller msg: 1613171092547 epoch sec; seqNo:566; depth 30.80 2021-02-12T23:04:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171092.57 epoch sec; seqNo:566; 4056120.12 north; 590378.77 east 2021-02-12T23:04:52Z- AHRS_M2 msg: 1613171092.94 epoch sec; seqNo:563 2021-02-12T23:04:52Z- AHRS_M2 msg: 0.57 phi; 0.18 theta; -0.10 psi 2021-02-12T23:04:52Z- handleDepth()-Depth_Keller msg: 1613171092945 epoch sec; seqNo:567; depth 30.82 2021-02-12T23:04:52Z- handleDvl() - RDI_Pathfinder msg: 1613171092.97 epoch sec; seqNo:239 2021-02-12T23:04:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.06 , 67.81 , 69.96 , 59.84 2021-02-12T23:04:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.08 , -0.14 2021-02-12T23:04:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171092.98 epoch sec; seqNo:567; 4056120.12 north; 590378.77 east 2021-02-12T23:04:53Z- AHRS_M2 msg: 1613171093.34 epoch sec; seqNo:564 2021-02-12T23:04:53Z- AHRS_M2 msg: 0.57 phi; 0.20 theta; -0.09 psi 2021-02-12T23:04:53Z- handleDepth()-Depth_Keller msg: 1613171093340 epoch sec; seqNo:568; depth 30.83 2021-02-12T23:04:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171093.35 epoch sec; seqNo:568; 4056120.50 north; 590378.76 east 2021-02-12T23:04:53Z- AHRS_M2 msg: 1613171093.74 epoch sec; seqNo:565 2021-02-12T23:04:53Z- AHRS_M2 msg: 0.57 phi; 0.23 theta; -0.09 psi 2021-02-12T23:04:53Z- handleDepth()-Depth_Keller msg: 1613171093744 epoch sec; seqNo:569; depth 30.83 2021-02-12T23:04:53Z- handleDvl() - RDI_Pathfinder msg: 1613171093.76 epoch sec; seqNo:240 2021-02-12T23:04:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.68 , 67.81 , 73.34 , 57.99 2021-02-12T23:04:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.02 , -0.15 2021-02-12T23:04:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171093.77 epoch sec; seqNo:569; 4056120.88 north; 590378.76 east 2021-02-12T23:04:54Z- AHRS_M2 msg: 1613171094.15 epoch sec; seqNo:566 2021-02-12T23:04:54Z- AHRS_M2 msg: 0.58 phi; 0.25 theta; -0.09 psi 2021-02-12T23:04:54Z- handleDepth()-Depth_Keller msg: 1613171094158 epoch sec; seqNo:570; depth 30.80 2021-02-12T23:04:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171094.17 epoch sec; seqNo:570; 4056120.88 north; 590378.76 east 2021-02-12T23:04:54Z- AHRS_M2 msg: 1613171094.55 epoch sec; seqNo:567 2021-02-12T23:04:54Z- AHRS_M2 msg: 0.58 phi; 0.26 theta; -0.09 psi 2021-02-12T23:04:54Z- handleDepth()-Depth_Keller msg: 1613171094557 epoch sec; seqNo:571; depth 30.77 2021-02-12T23:04:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171094.57 epoch sec; seqNo:571; 4056121.27 north; 590379.98 east 2021-02-12T23:04:54Z- AHRS_M2 msg: 1613171094.95 epoch sec; seqNo:568 2021-02-12T23:04:54Z- AHRS_M2 msg: 0.58 phi; 0.28 theta; -0.08 psi 2021-02-12T23:04:54Z- handleDepth()-Depth_Keller msg: 1613171094956 epoch sec; seqNo:572; depth 30.74 2021-02-12T23:04:54Z- handleDvl() - RDI_Pathfinder msg: 1613171094.97 epoch sec; seqNo:241 2021-02-12T23:04:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.90 , 68.74 , 76.71 , 57.38 2021-02-12T23:04:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.01 , -0.14 2021-02-12T23:04:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171094.99 epoch sec; seqNo:572; 4056121.65 north; 590379.97 east 2021-02-12T23:04:55Z- AHRS_M2 msg: 1613171095.36 epoch sec; seqNo:569 2021-02-12T23:04:55Z- AHRS_M2 msg: 0.58 phi; 0.30 theta; -0.08 psi 2021-02-12T23:04:55Z- handleDepth()-Depth_Keller msg: 1613171095360 epoch sec; seqNo:573; depth 30.69 2021-02-12T23:04:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171095.37 epoch sec; seqNo:573; 4056121.65 north; 590379.97 east 2021-02-12T23:04:55Z- AHRS_M2 msg: 1613171095.76 epoch sec; seqNo:570 2021-02-12T23:04:55Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.08 psi 2021-02-12T23:04:55Z- handleDepth()-Depth_Keller msg: 1613171095765 epoch sec; seqNo:574; depth 30.64 2021-02-12T23:04:55Z- handleDvl() - RDI_Pathfinder msg: 1613171095.78 epoch sec; seqNo:242 2021-02-12T23:04:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.13 , 68.74 , 0.00 , 56.77 2021-02-12T23:04:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.04 , -0.10 2021-02-12T23:04:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171095.79 epoch sec; seqNo:574; 4056122.03 north; 590379.97 east 2021-02-12T23:04:55Z- LcmTrn::handleMessages() - handling 45 messages took 0.67 ms and 132 calls... 2021-02-12T23:04:55Z- waiting for 1613171093.53, time is 1613171095.79 2021-02-12T23:04:55Z- LcmTrn::updateTrn() equating pose 1613171095.79 and meas times 1613171095.79 2021-02-12T23:04:55Z- LcmTrn::updateTrn() >>>> pose time 1613171095.79, meas time 1613171095.79 <<<< 2021-02-12T23:04:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171095.79 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:04:55Z- TerrainNav::motionUpdate() - currEstimate = (4056122, 590379.97, 30.64) 2021-02-12T23:04:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:04:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:04:55Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171095.79 sec.; No relevant navigation data available 2021-02-12T23:04:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:04:55Z- TNavBF::mmse selected from filter -1 2021-02-12T23:04:55Z- variances([0], [2], and psi_berg) = 823.769769 833.182311 0.000000 2021-02-12T23:04:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:04:55Z- LcmTrn::publishEstimates() - MLE : 4056083.53 590332.47 30.64 2021-02-12T23:04:55Z- LcmTrn::publishEstimates() - MMSE : 4056122.03 590379.97 30.64 2021-02-12T23:04:55Z- LcmTrn::publishEstimates() - COVAR: 823.77 833.18 0.86 0.00 2021-02-12T23:04:56Z- AHRS_M2 msg: 1613171096.16 epoch sec; seqNo:571 2021-02-12T23:04:56Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.08 psi 2021-02-12T23:04:56Z- handleDepth()-Depth_Keller msg: 1613171096168 epoch sec; seqNo:575; depth 30.56 2021-02-12T23:04:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171096.18 epoch sec; seqNo:575; 4056122.41 north; 590379.96 east 2021-02-12T23:04:56Z- AHRS_M2 msg: 1613171096.57 epoch sec; seqNo:572 2021-02-12T23:04:56Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.07 psi 2021-02-12T23:04:56Z- handleDepth()-Depth_Keller msg: 1613171096576 epoch sec; seqNo:576; depth 30.50 2021-02-12T23:04:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171096.59 epoch sec; seqNo:576; 4056122.41 north; 590379.96 east 2021-02-12T23:04:56Z- AHRS_M2 msg: 1613171096.97 epoch sec; seqNo:573 2021-02-12T23:04:56Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.07 psi 2021-02-12T23:04:56Z- handleDepth()-Depth_Keller msg: 1613171096976 epoch sec; seqNo:577; depth 30.41 2021-02-12T23:04:57Z- handleDvl() - RDI_Pathfinder msg: 1613171096.99 epoch sec; seqNo:243 2021-02-12T23:04:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.92 , 68.97 , 0.00 , 56.22 2021-02-12T23:04:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.05 , -0.09 2021-02-12T23:04:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171097.02 epoch sec; seqNo:577; 4056122.79 north; 590379.96 east 2021-02-12T23:04:57Z- AHRS_M2 msg: 1613171097.38 epoch sec; seqNo:574 2021-02-12T23:04:57Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.06 psi 2021-02-12T23:04:57Z- handleDepth()-Depth_Keller msg: 1613171097382 epoch sec; seqNo:578; depth 30.33 2021-02-12T23:04:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171097.39 epoch sec; seqNo:578; 4056122.79 north; 590379.96 east 2021-02-12T23:04:57Z- AHRS_M2 msg: 1613171097.78 epoch sec; seqNo:575 2021-02-12T23:04:57Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.06 psi 2021-02-12T23:04:57Z- handleDepth()-Depth_Keller msg: 1613171097785 epoch sec; seqNo:579; depth 30.23 2021-02-12T23:04:57Z- handleDvl() - RDI_Pathfinder msg: 1613171097.80 epoch sec; seqNo:244 2021-02-12T23:04:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.07 , 69.55 , 0.00 , 56.80 2021-02-12T23:04:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.05 , -0.07 2021-02-12T23:04:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171097.82 epoch sec; seqNo:579; 4056123.18 north; 590381.18 east 2021-02-12T23:04:58Z- AHRS_M2 msg: 1613171098.18 epoch sec; seqNo:576 2021-02-12T23:04:58Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.06 psi 2021-02-12T23:04:58Z- handleDepth()-Depth_Keller msg: 1613171098188 epoch sec; seqNo:580; depth 30.13 2021-02-12T23:04:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171098.20 epoch sec; seqNo:580; 4056123.56 north; 590381.17 east 2021-02-12T23:04:58Z- AHRS_M2 msg: 1613171098.60 epoch sec; seqNo:577 2021-02-12T23:04:58Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.05 psi 2021-02-12T23:04:58Z- handleDepth()-Depth_Keller msg: 1613171098625 epoch sec; seqNo:581; depth 30.06 2021-02-12T23:04:58Z- handleDvl() - RDI_Pathfinder msg: 1613171098.65 epoch sec; seqNo:245 2021-02-12T23:04:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.59 , 69.65 , 0.00 , 56.15 2021-02-12T23:04:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.07 , -0.04 2021-02-12T23:04:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171098.66 epoch sec; seqNo:581; 4056123.56 north; 590381.17 east 2021-02-12T23:04:58Z- AHRS_M2 msg: 1613171098.99 epoch sec; seqNo:578 2021-02-12T23:04:58Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.05 psi 2021-02-12T23:04:58Z- handleDepth()-Depth_Keller msg: 1613171098995 epoch sec; seqNo:582; depth 29.95 2021-02-12T23:04:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171099.03 epoch sec; seqNo:582; 4056123.94 north; 590381.17 east 2021-02-12T23:04:59Z- AHRS_M2 msg: 1613171099.40 epoch sec; seqNo:579 2021-02-12T23:04:59Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.05 psi 2021-02-12T23:04:59Z- handleDepth()-Depth_Keller msg: 1613171099405 epoch sec; seqNo:583; depth 29.85 2021-02-12T23:04:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171099.41 epoch sec; seqNo:583; 4056124.32 north; 590381.16 east 2021-02-12T23:04:59Z- AHRS_M2 msg: 1613171099.80 epoch sec; seqNo:580 2021-02-12T23:04:59Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.05 psi 2021-02-12T23:04:59Z- handleDepth()-Depth_Keller msg: 1613171099804 epoch sec; seqNo:584; depth 29.76 2021-02-12T23:04:59Z- handleDvl() - RDI_Pathfinder msg: 1613171099.82 epoch sec; seqNo:246 2021-02-12T23:04:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.59 , 69.65 , 0.00 , 57.07 2021-02-12T23:04:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.05 , 0.00 2021-02-12T23:04:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171099.83 epoch sec; seqNo:584; 4056124.32 north; 590381.16 east 2021-02-12T23:05:00Z- AHRS_M2 msg: 1613171100.20 epoch sec; seqNo:581 2021-02-12T23:05:00Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.06 psi 2021-02-12T23:05:00Z- handleDepth()-Depth_Keller msg: 1613171100213 epoch sec; seqNo:585; depth 29.65 2021-02-12T23:05:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171100.22 epoch sec; seqNo:585; 4056124.69 north; 590381.16 east 2021-02-12T23:05:00Z- AHRS_M2 msg: 1613171100.61 epoch sec; seqNo:582 2021-02-12T23:05:00Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.07 psi 2021-02-12T23:05:00Z- handleDepth()-Depth_Keller msg: 1613171100637 epoch sec; seqNo:586; depth 29.54 2021-02-12T23:05:00Z- handleDvl() - RDI_Pathfinder msg: 1613171100.66 epoch sec; seqNo:247 2021-02-12T23:05:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.59 , 70.58 , 84.69 , 57.38 2021-02-12T23:05:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , 0.05 2021-02-12T23:05:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171100.67 epoch sec; seqNo:586; 4056125.07 north; 590381.16 east 2021-02-12T23:05:00Z- LcmTrn::handleMessages() - handling 41 messages took 0.66 ms and 128 calls... 2021-02-12T23:05:00Z- waiting for 1613171098.79, time is 1613171100.67 2021-02-12T23:05:00Z- LcmTrn::updateTrn() equating pose 1613171100.67 and meas times 1613171100.67 2021-02-12T23:05:00Z- LcmTrn::updateTrn() >>>> pose time 1613171100.67, meas time 1613171100.67 <<<< 2021-02-12T23:05:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171100.67 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:05:00Z- TerrainNav::motionUpdate() - currEstimate = (4056125, 590381.16, 29.54) 2021-02-12T23:05:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:05:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:05:00Z- TNavBF::Projecting Measurements 2021-02-12T23:05:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:05:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:05:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:05:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:05:00Z- TNavFilter::4 good beams. 2021-02-12T23:05:00Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:05:00Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:05:00Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:05:00Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:05:00Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:05:00Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:05:00Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:05:00Z- TNavBF::mmse selected from filter -1 2021-02-12T23:05:00Z- TerrainMapOctree: (vehN, vehE ) = (4056125.07, 590381.16). 2021-02-12T23:05:00Z- TerrainMapOctree: Min Distance = 6421.28. 2021-02-12T23:05:00Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:05:00Z- There is no specific good beam that all particles have in common. 2021-02-12T23:05:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:05:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 7.09 2021-02-12T23:05:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:05:00Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 7.97 2021-02-12T23:05:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:05:00Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 11.48 2021-02-12T23:05:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:05:00Z- Old Depth Bias: 0.00 DepthBiasCov: 0.79 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 5.27 2021-02-12T23:05:00Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:05:00Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:05:00Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:05:00Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:05:00Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:05:00Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:05:00Z- Filter 0 No beams used in subcloud update 2021-02-12T23:05:00Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:05:00Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:05:00Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 823.4 EastVariance: 833.2 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:05:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171100.67 sec. 2021-02-12T23:05:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:05:00Z- TNavBF::mmse selected from filter -1 2021-02-12T23:05:00Z- variances([0], [2], and psi_berg) = 823.382231 833.180238 0.000000 2021-02-12T23:05:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:05:00Z- LcmTrn::publishEstimates() - MLE : 4056087.57 590331.66 29.54 2021-02-12T23:05:00Z- LcmTrn::publishEstimates() - MMSE : 4056125.07 590381.16 29.54 2021-02-12T23:05:00Z- LcmTrn::publishEstimates() - COVAR: 823.38 833.18 0.71 0.00 2021-02-12T23:05:01Z- AHRS_M2 msg: 1613171101.02 epoch sec; seqNo:583 2021-02-12T23:05:01Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.07 psi 2021-02-12T23:05:01Z- handleDepth()-Depth_Keller msg: 1613171101033 epoch sec; seqNo:587; depth 29.42 2021-02-12T23:05:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171101.06 epoch sec; seqNo:587; 4056125.09 north; 590382.38 east 2021-02-12T23:05:01Z- AHRS_M2 msg: 1613171101.41 epoch sec; seqNo:584 2021-02-12T23:05:01Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.08 psi 2021-02-12T23:05:01Z- handleDepth()-Depth_Keller msg: 1613171101430 epoch sec; seqNo:588; depth 29.33 2021-02-12T23:05:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171101.44 epoch sec; seqNo:588; 4056125.46 north; 590382.37 east 2021-02-12T23:05:01Z- AHRS_M2 msg: 1613171101.82 epoch sec; seqNo:585 2021-02-12T23:05:01Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.09 psi 2021-02-12T23:05:01Z- handleDepth()-Depth_Keller msg: 1613171101824 epoch sec; seqNo:589; depth 29.22 2021-02-12T23:05:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171101.83 epoch sec; seqNo:589; 4056125.84 north; 590382.37 east 2021-02-12T23:05:02Z- AHRS_M2 msg: 1613171102.22 epoch sec; seqNo:586 2021-02-12T23:05:02Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.09 psi 2021-02-12T23:05:02Z- handleDepth()-Depth_Keller msg: 1613171102229 epoch sec; seqNo:590; depth 29.12 2021-02-12T23:05:02Z- handleDvl() - RDI_Pathfinder msg: 1613171102.24 epoch sec; seqNo:248 2021-02-12T23:05:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.25 , 71.16 , 0.00 , 57.66 2021-02-12T23:05:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.00 , 0.10 2021-02-12T23:05:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171102.25 epoch sec; seqNo:590; 4056125.84 north; 590382.37 east 2021-02-12T23:05:02Z- AHRS_M2 msg: 1613171102.64 epoch sec; seqNo:587 2021-02-12T23:05:02Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.10 psi 2021-02-12T23:05:02Z- handleDepth()-Depth_Keller msg: 1613171102649 epoch sec; seqNo:591; depth 29.01 2021-02-12T23:05:02Z- handleDvl() - RDI_Pathfinder msg: 1613171102.66 epoch sec; seqNo:249 2021-02-12T23:05:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.59 , 72.11 , 0.00 , 58.30 2021-02-12T23:05:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , 0.11 2021-02-12T23:05:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171102.67 epoch sec; seqNo:591; 4056126.22 north; 590382.37 east 2021-02-12T23:05:03Z- AHRS_M2 msg: 1613171103.03 epoch sec; seqNo:588 2021-02-12T23:05:03Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.10 psi 2021-02-12T23:05:03Z- handleDepth()-Depth_Keller msg: 1613171103054 epoch sec; seqNo:592; depth 28.90 2021-02-12T23:05:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171103.07 epoch sec; seqNo:592; 4056126.60 north; 590382.36 east 2021-02-12T23:05:03Z- AHRS_M2 msg: 1613171103.44 epoch sec; seqNo:589 2021-02-12T23:05:03Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.11 psi 2021-02-12T23:05:03Z- handleDepth()-Depth_Keller msg: 1613171103453 epoch sec; seqNo:593; depth 28.79 2021-02-12T23:05:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171103.47 epoch sec; seqNo:593; 4056126.61 north; 590383.58 east 2021-02-12T23:05:03Z- AHRS_M2 msg: 1613171103.84 epoch sec; seqNo:590 2021-02-12T23:05:03Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.11 psi 2021-02-12T23:05:03Z- handleDepth()-Depth_Keller msg: 1613171103846 epoch sec; seqNo:594; depth 28.69 2021-02-12T23:05:03Z- handleDvl() - RDI_Pathfinder msg: 1613171103.86 epoch sec; seqNo:250 2021-02-12T23:05:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.59 , 73.03 , 0.00 , 58.61 2021-02-12T23:05:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.12 , 0.08 2021-02-12T23:05:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171103.87 epoch sec; seqNo:594; 4056126.99 north; 590383.58 east 2021-02-12T23:05:04Z- AHRS_M2 msg: 1613171104.25 epoch sec; seqNo:591 2021-02-12T23:05:04Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.12 psi 2021-02-12T23:05:04Z- handleDepth()-Depth_Keller msg: 1613171104251 epoch sec; seqNo:595; depth 28.59 2021-02-12T23:05:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171104.27 epoch sec; seqNo:595; 4056127.37 north; 590383.57 east 2021-02-12T23:05:04Z- AHRS_M2 msg: 1613171104.65 epoch sec; seqNo:592 2021-02-12T23:05:04Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.12 psi 2021-02-12T23:05:04Z- handleDepth()-Depth_Keller msg: 1613171104658 epoch sec; seqNo:596; depth 28.47 2021-02-12T23:05:04Z- handleDvl() - RDI_Pathfinder msg: 1613171104.67 epoch sec; seqNo:251 2021-02-12T23:05:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.59 , 74.26 , 0.00 , 59.22 2021-02-12T23:05:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.15 , 0.02 2021-02-12T23:05:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171104.70 epoch sec; seqNo:596; 4056127.37 north; 590383.57 east 2021-02-12T23:05:05Z- AHRS_M2 msg: 1613171105.06 epoch sec; seqNo:593 2021-02-12T23:05:05Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.12 psi 2021-02-12T23:05:05Z- handleDepth()-Depth_Keller msg: 1613171105063 epoch sec; seqNo:597; depth 28.37 2021-02-12T23:05:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171105.07 epoch sec; seqNo:597; 4056127.75 north; 590383.57 east 2021-02-12T23:05:05Z- AHRS_M2 msg: 1613171105.45 epoch sec; seqNo:594 2021-02-12T23:05:05Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.13 psi 2021-02-12T23:05:05Z- handleDepth()-Depth_Keller msg: 1613171105470 epoch sec; seqNo:598; depth 28.28 2021-02-12T23:05:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171105.48 epoch sec; seqNo:598; 4056127.76 north; 590384.79 east 2021-02-12T23:05:05Z- AHRS_M2 msg: 1613171105.86 epoch sec; seqNo:595 2021-02-12T23:05:05Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.13 psi 2021-02-12T23:05:05Z- handleDepth()-Depth_Keller msg: 1613171105864 epoch sec; seqNo:599; depth 28.15 2021-02-12T23:05:05Z- handleDvl() - RDI_Pathfinder msg: 1613171105.88 epoch sec; seqNo:252 2021-02-12T23:05:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.28 , 75.18 , 88.07 , 59.53 2021-02-12T23:05:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.16 , -0.06 2021-02-12T23:05:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171105.90 epoch sec; seqNo:599; 4056128.14 north; 590384.79 east 2021-02-12T23:05:06Z- LcmTrn::handleMessages() - handling 44 messages took 0.78 ms and 129 calls... 2021-02-12T23:05:06Z- waiting for 1613171103.67, time is 1613171105.90 2021-02-12T23:05:06Z- LcmTrn::updateTrn() equating pose 1613171105.90 and meas times 1613171105.90 2021-02-12T23:05:06Z- LcmTrn::updateTrn() >>>> pose time 1613171105.90, meas time 1613171105.90 <<<< 2021-02-12T23:05:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171105.90 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:05:06Z- TerrainNav::motionUpdate() - currEstimate = (4056128, 590384.79, 28.15) 2021-02-12T23:05:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:05:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:05:06Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171105.90 sec.; No relevant navigation data available 2021-02-12T23:05:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:05:06Z- TNavBF::mmse selected from filter -1 2021-02-12T23:05:06Z- variances([0], [2], and psi_berg) = 822.920352 833.177819 0.000000 2021-02-12T23:05:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:05:06Z- LcmTrn::publishEstimates() - MLE : 4056090.64 590336.29 28.15 2021-02-12T23:05:06Z- LcmTrn::publishEstimates() - MMSE : 4056128.14 590384.79 28.15 2021-02-12T23:05:06Z- LcmTrn::publishEstimates() - COVAR: 822.92 833.18 0.95 0.00 2021-02-12T23:05:06Z- AHRS_M2 msg: 1613171106.26 epoch sec; seqNo:596 2021-02-12T23:05:06Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.13 psi 2021-02-12T23:05:06Z- handleDepth()-Depth_Keller msg: 1613171106268 epoch sec; seqNo:600; depth 28.05 2021-02-12T23:05:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171106.28 epoch sec; seqNo:600; 4056128.52 north; 590384.78 east 2021-02-12T23:05:06Z- AHRS_M2 msg: 1613171106.67 epoch sec; seqNo:597 2021-02-12T23:05:06Z- AHRS_M2 msg: 0.68 phi; 0.34 theta; -0.14 psi 2021-02-12T23:05:06Z- handleDepth()-Depth_Keller msg: 1613171106674 epoch sec; seqNo:601; depth 27.94 2021-02-12T23:05:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171106.68 epoch sec; seqNo:601; 4056128.52 north; 590384.78 east 2021-02-12T23:05:07Z- AHRS_M2 msg: 1613171107.07 epoch sec; seqNo:598 2021-02-12T23:05:07Z- AHRS_M2 msg: 0.68 phi; 0.34 theta; -0.14 psi 2021-02-12T23:05:07Z- handleDepth()-Depth_Keller msg: 1613171107076 epoch sec; seqNo:602; depth 27.84 2021-02-12T23:05:07Z- handleDvl() - RDI_Pathfinder msg: 1613171107.10 epoch sec; seqNo:253 2021-02-12T23:05:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.84 , 76.07 , 91.50 , 60.12 2021-02-12T23:05:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.10 , -0.12 2021-02-12T23:05:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171107.11 epoch sec; seqNo:602; 4056128.90 north; 590384.78 east 2021-02-12T23:05:07Z- AHRS_M2 msg: 1613171107.48 epoch sec; seqNo:599 2021-02-12T23:05:07Z- AHRS_M2 msg: 0.69 phi; 0.33 theta; -0.13 psi 2021-02-12T23:05:07Z- handleDepth()-Depth_Keller msg: 1613171107481 epoch sec; seqNo:603; depth 27.72 2021-02-12T23:05:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171107.49 epoch sec; seqNo:603; 4056128.91 north; 590386.00 east 2021-02-12T23:05:07Z- AHRS_M2 msg: 1613171107.88 epoch sec; seqNo:600 2021-02-12T23:05:07Z- AHRS_M2 msg: 0.69 phi; 0.33 theta; -0.14 psi 2021-02-12T23:05:07Z- handleDepth()-Depth_Keller msg: 1613171107884 epoch sec; seqNo:604; depth 27.62 2021-02-12T23:05:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171107.89 epoch sec; seqNo:604; 4056129.29 north; 590386.00 east 2021-02-12T23:05:08Z- AHRS_M2 msg: 1613171108.28 epoch sec; seqNo:601 2021-02-12T23:05:08Z- AHRS_M2 msg: 0.70 phi; 0.33 theta; -0.14 psi 2021-02-12T23:05:08Z- handleDepth()-Depth_Keller msg: 1613171108291 epoch sec; seqNo:605; depth 27.51 2021-02-12T23:05:08Z- handleDvl() - RDI_Pathfinder msg: 1613171108.30 epoch sec; seqNo:254 2021-02-12T23:05:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.84 , 76.07 , 91.50 , 59.71 2021-02-12T23:05:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.07 , -0.17 2021-02-12T23:05:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171108.31 epoch sec; seqNo:605; 4056129.67 north; 590385.99 east 2021-02-12T23:05:08Z- AHRS_M2 msg: 1613171108.69 epoch sec; seqNo:602 2021-02-12T23:05:08Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.14 psi 2021-02-12T23:05:08Z- handleDepth()-Depth_Keller msg: 1613171108697 epoch sec; seqNo:606; depth 27.40 2021-02-12T23:05:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171108.71 epoch sec; seqNo:606; 4056129.67 north; 590385.99 east 2021-02-12T23:05:09Z- AHRS_M2 msg: 1613171109.10 epoch sec; seqNo:603 2021-02-12T23:05:09Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.14 psi 2021-02-12T23:05:09Z- handleDepth()-Depth_Keller msg: 1613171109112 epoch sec; seqNo:607; depth 27.30 2021-02-12T23:05:09Z- handleDvl() - RDI_Pathfinder msg: 1613171109.13 epoch sec; seqNo:255 2021-02-12T23:05:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.43 , 76.07 , 91.50 , 58.89 2021-02-12T23:05:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.02 , -0.17 2021-02-12T23:05:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171109.15 epoch sec; seqNo:607; 4056130.05 north; 590385.99 east 2021-02-12T23:05:09Z- AHRS_M2 msg: 1613171109.50 epoch sec; seqNo:604 2021-02-12T23:05:09Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.14 psi 2021-02-12T23:05:09Z- handleDepth()-Depth_Keller msg: 1613171109511 epoch sec; seqNo:608; depth 27.18 2021-02-12T23:05:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171109.52 epoch sec; seqNo:608; 4056130.05 north; 590385.99 east 2021-02-12T23:05:09Z- AHRS_M2 msg: 1613171109.91 epoch sec; seqNo:605 2021-02-12T23:05:09Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.15 psi 2021-02-12T23:05:09Z- handleDepth()-Depth_Keller msg: 1613171109919 epoch sec; seqNo:609; depth 27.09 2021-02-12T23:05:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171109.93 epoch sec; seqNo:609; 4056130.44 north; 590387.21 east 2021-02-12T23:05:10Z- AHRS_M2 msg: 1613171110.30 epoch sec; seqNo:606 2021-02-12T23:05:10Z- AHRS_M2 msg: 0.70 phi; 0.31 theta; -0.16 psi 2021-02-12T23:05:10Z- handleDepth()-Depth_Keller msg: 1613171110308 epoch sec; seqNo:610; depth 26.98 2021-02-12T23:05:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171110.32 epoch sec; seqNo:610; 4056130.82 north; 590387.20 east 2021-02-12T23:05:10Z- AHRS_M2 msg: 1613171110.71 epoch sec; seqNo:607 2021-02-12T23:05:10Z- AHRS_M2 msg: 0.69 phi; 0.31 theta; -0.16 psi 2021-02-12T23:05:10Z- handleDepth()-Depth_Keller msg: 1613171110717 epoch sec; seqNo:611; depth 26.87 2021-02-12T23:05:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171110.73 epoch sec; seqNo:611; 4056130.82 north; 590387.20 east 2021-02-12T23:05:11Z- LcmTrn::handleMessages() - handling 39 messages took 0.61 ms and 127 calls... 2021-02-12T23:05:11Z- waiting for 1613171108.90, time is 1613171110.73 2021-02-12T23:05:11Z- LcmTrn::updateTrn() >>>> pose time 1613171110.73, meas time 1613171109.13 <<<< 2021-02-12T23:05:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171109.13 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:05:11Z- TerrainNav::motionUpdate() - currEstimate = (4056131, 590387.20, 26.87) 2021-02-12T23:05:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:05:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:05:11Z- TNavBF::Projecting Measurements 2021-02-12T23:05:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:05:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:05:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:05:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:05:11Z- TNavFilter::4 good beams. 2021-02-12T23:05:11Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:05:11Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:05:11Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:05:11Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:05:11Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:05:11Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:05:11Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:05:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:05:11Z- TerrainMapOctree: (vehN, vehE ) = (4056129.93, 590386.40). 2021-02-12T23:05:11Z- TerrainMapOctree: Min Distance = 6414.24. 2021-02-12T23:05:11Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:05:11Z- There is no specific good beam that all particles have in common. 2021-02-12T23:05:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:05:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 6.85 2021-02-12T23:05:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:05:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 9.26 2021-02-12T23:05:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:05:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 13.40 2021-02-12T23:05:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:05:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 5.55 2021-02-12T23:05:11Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:05:11Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:05:11Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:05:11Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:05:11Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:05:11Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:05:11Z- Filter 0 No beams used in subcloud update 2021-02-12T23:05:11Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:05:11Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:05:11Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 822.6 EastVariance: 833.2 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:05:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171109.13 sec. 2021-02-12T23:05:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:05:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:05:11Z- variances([0], [2], and psi_berg) = 822.370325 833.175027 0.000000 2021-02-12T23:05:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:05:11Z- LcmTrn::publishEstimates() - MLE : 4056093.32 590338.70 26.87 2021-02-12T23:05:11Z- LcmTrn::publishEstimates() - MMSE : 4056130.82 590387.20 26.87 2021-02-12T23:05:11Z- LcmTrn::publishEstimates() - COVAR: 822.37 833.18 0.81 0.00 2021-02-12T23:05:11Z- AHRS_M2 msg: 1613171111.11 epoch sec; seqNo:608 2021-02-12T23:05:11Z- AHRS_M2 msg: 0.69 phi; 0.31 theta; -0.17 psi 2021-02-12T23:05:11Z- handleDepth()-Depth_Keller msg: 1613171111118 epoch sec; seqNo:612; depth 26.78 2021-02-12T23:05:11Z- handleDvl() - RDI_Pathfinder msg: 1613171111.13 epoch sec; seqNo:256 2021-02-12T23:05:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.84 , 77.29 , 91.50 , 59.30 2021-02-12T23:05:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.07 , -0.08 2021-02-12T23:05:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171111.14 epoch sec; seqNo:612; 4056131.20 north; 590387.20 east 2021-02-12T23:05:11Z- AHRS_M2 msg: 1613171111.52 epoch sec; seqNo:609 2021-02-12T23:05:11Z- AHRS_M2 msg: 0.69 phi; 0.31 theta; -0.17 psi 2021-02-12T23:05:11Z- handleDepth()-Depth_Keller msg: 1613171111525 epoch sec; seqNo:613; depth 26.68 2021-02-12T23:05:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171111.54 epoch sec; seqNo:613; 4056131.20 north; 590387.20 east 2021-02-12T23:05:11Z- AHRS_M2 msg: 1613171111.92 epoch sec; seqNo:610 2021-02-12T23:05:11Z- AHRS_M2 msg: 0.68 phi; 0.31 theta; -0.17 psi 2021-02-12T23:05:11Z- handleDepth()-Depth_Keller msg: 1613171111931 epoch sec; seqNo:614; depth 26.59 2021-02-12T23:05:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171111.97 epoch sec; seqNo:614; 4056131.58 north; 590387.19 east 2021-02-12T23:05:12Z- AHRS_M2 msg: 1613171112.32 epoch sec; seqNo:611 2021-02-12T23:05:12Z- AHRS_M2 msg: 0.68 phi; 0.31 theta; -0.17 psi 2021-02-12T23:05:12Z- handleDepth()-Depth_Keller msg: 1613171112328 epoch sec; seqNo:615; depth 26.48 2021-02-12T23:05:12Z- handleDvl() - RDI_Pathfinder msg: 1613171112.34 epoch sec; seqNo:257 2021-02-12T23:05:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.03 , 77.70 , 91.50 , 60.12 2021-02-12T23:05:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.07 , -0.02 2021-02-12T23:05:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171112.35 epoch sec; seqNo:615; 4056131.96 north; 590387.19 east 2021-02-12T23:05:12Z- AHRS_M2 msg: 1613171112.73 epoch sec; seqNo:612 2021-02-12T23:05:12Z- AHRS_M2 msg: 0.68 phi; 0.31 theta; -0.17 psi 2021-02-12T23:05:12Z- handleDepth()-Depth_Keller msg: 1613171112737 epoch sec; seqNo:616; depth 26.38 2021-02-12T23:05:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171112.75 epoch sec; seqNo:616; 4056131.97 north; 590388.41 east 2021-02-12T23:05:13Z- AHRS_M2 msg: 1613171113.13 epoch sec; seqNo:613 2021-02-12T23:05:13Z- AHRS_M2 msg: 0.68 phi; 0.31 theta; -0.17 psi 2021-02-12T23:05:13Z- handleDepth()-Depth_Keller msg: 1613171113140 epoch sec; seqNo:617; depth 26.29 2021-02-12T23:05:13Z- handleDvl() - RDI_Pathfinder msg: 1613171113.16 epoch sec; seqNo:258 2021-02-12T23:05:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.84 , 78.52 , 90.42 , 60.53 2021-02-12T23:05:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.03 , 0.04 2021-02-12T23:05:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171113.17 epoch sec; seqNo:617; 4056132.35 north; 590388.41 east 2021-02-12T23:05:13Z- AHRS_M2 msg: 1613171113.54 epoch sec; seqNo:614 2021-02-12T23:05:13Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.17 psi 2021-02-12T23:05:13Z- handleDepth()-Depth_Keller msg: 1613171113540 epoch sec; seqNo:618; depth 26.20 2021-02-12T23:05:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171113.55 epoch sec; seqNo:618; 4056132.73 north; 590388.40 east 2021-02-12T23:05:13Z- AHRS_M2 msg: 1613171113.94 epoch sec; seqNo:615 2021-02-12T23:05:13Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.17 psi 2021-02-12T23:05:13Z- handleDepth()-Depth_Keller msg: 1613171113946 epoch sec; seqNo:619; depth 26.09 2021-02-12T23:05:13Z- handleDvl() - RDI_Pathfinder msg: 1613171113.96 epoch sec; seqNo:259 2021-02-12T23:05:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.84 , 78.11 , 0.00 , 60.53 2021-02-12T23:05:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , 0.09 2021-02-12T23:05:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171113.97 epoch sec; seqNo:619; 4056132.73 north; 590388.40 east 2021-02-12T23:05:14Z- AHRS_M2 msg: 1613171114.34 epoch sec; seqNo:616 2021-02-12T23:05:14Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.17 psi 2021-02-12T23:05:14Z- handleDepth()-Depth_Keller msg: 1613171114349 epoch sec; seqNo:620; depth 25.99 2021-02-12T23:05:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171114.36 epoch sec; seqNo:620; 4056133.11 north; 590388.40 east 2021-02-12T23:05:14Z- AHRS_M2 msg: 1613171114.75 epoch sec; seqNo:617 2021-02-12T23:05:14Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.17 psi 2021-02-12T23:05:14Z- handleDepth()-Depth_Keller msg: 1613171114755 epoch sec; seqNo:621; depth 25.90 2021-02-12T23:05:14Z- handleDvl() - RDI_Pathfinder msg: 1613171114.77 epoch sec; seqNo:260 2021-02-12T23:05:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.07 , 78.71 , 86.81 , 61.54 2021-02-12T23:05:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , 0.12 2021-02-12T23:05:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171114.78 epoch sec; seqNo:621; 4056133.48 north; 590388.40 east 2021-02-12T23:05:15Z- AHRS_M2 msg: 1613171115.15 epoch sec; seqNo:618 2021-02-12T23:05:15Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.17 psi 2021-02-12T23:05:15Z- handleDepth()-Depth_Keller msg: 1613171115165 epoch sec; seqNo:622; depth 25.80 2021-02-12T23:05:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171115.17 epoch sec; seqNo:622; 4056133.50 north; 590389.62 east 2021-02-12T23:05:15Z- AHRS_M2 msg: 1613171115.56 epoch sec; seqNo:619 2021-02-12T23:05:15Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.18 psi 2021-02-12T23:05:15Z- handleDepth()-Depth_Keller msg: 1613171115560 epoch sec; seqNo:623; depth 25.72 2021-02-12T23:05:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171115.58 epoch sec; seqNo:623; 4056133.88 north; 590389.61 east 2021-02-12T23:05:15Z- AHRS_M2 msg: 1613171115.97 epoch sec; seqNo:620 2021-02-12T23:05:15Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.18 psi 2021-02-12T23:05:15Z- handleDepth()-Depth_Keller msg: 1613171115978 epoch sec; seqNo:624; depth 25.62 2021-02-12T23:05:16Z- handleDvl() - RDI_Pathfinder msg: 1613171115.99 epoch sec; seqNo:261 2021-02-12T23:05:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.66 , 78.93 , 92.59 , 61.75 2021-02-12T23:05:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , 0.12 2021-02-12T23:05:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171116.01 epoch sec; seqNo:624; 4056134.26 north; 590389.61 east 2021-02-12T23:05:16Z- LcmTrn::handleMessages() - handling 45 messages took 0.74 ms and 130 calls... 2021-02-12T23:05:16Z- waiting for 1613171113.73, time is 1613171116.01 2021-02-12T23:05:16Z- LcmTrn::updateTrn() equating pose 1613171116.01 and meas times 1613171116.01 2021-02-12T23:05:16Z- LcmTrn::updateTrn() >>>> pose time 1613171116.01, meas time 1613171116.01 <<<< 2021-02-12T23:05:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171116.01 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:05:16Z- TerrainNav::motionUpdate() - currEstimate = (4056134, 590389.61, 25.62) 2021-02-12T23:05:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:05:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:05:16Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171116.01 sec.; No relevant navigation data available 2021-02-12T23:05:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:05:16Z- TNavBF::mmse selected from filter -1 2021-02-12T23:05:16Z- variances([0], [2], and psi_berg) = 821.949940 833.172938 0.000000 2021-02-12T23:05:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:05:16Z- LcmTrn::publishEstimates() - MLE : 4056096.76 590341.11 25.62 2021-02-12T23:05:16Z- LcmTrn::publishEstimates() - MMSE : 4056134.26 590389.61 25.62 2021-02-12T23:05:16Z- LcmTrn::publishEstimates() - COVAR: 821.95 833.17 1.02 0.00 2021-02-12T23:05:16Z- AHRS_M2 msg: 1613171116.37 epoch sec; seqNo:621 2021-02-12T23:05:16Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.18 psi 2021-02-12T23:05:16Z- handleDepth()-Depth_Keller msg: 1613171116370 epoch sec; seqNo:625; depth 25.52 2021-02-12T23:05:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171116.38 epoch sec; seqNo:625; 4056134.26 north; 590389.61 east 2021-02-12T23:05:16Z- AHRS_M2 msg: 1613171116.77 epoch sec; seqNo:622 2021-02-12T23:05:16Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.18 psi 2021-02-12T23:05:16Z- handleDepth()-Depth_Keller msg: 1613171116774 epoch sec; seqNo:626; depth 25.43 2021-02-12T23:05:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171116.79 epoch sec; seqNo:626; 4056134.63 north; 590389.60 east 2021-02-12T23:05:17Z- AHRS_M2 msg: 1613171117.17 epoch sec; seqNo:623 2021-02-12T23:05:17Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.18 psi 2021-02-12T23:05:17Z- handleDepth()-Depth_Keller msg: 1613171117176 epoch sec; seqNo:627; depth 25.33 2021-02-12T23:05:17Z- handleDvl() - RDI_Pathfinder msg: 1613171117.20 epoch sec; seqNo:262 2021-02-12T23:05:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.66 , 79.34 , 98.00 , 61.75 2021-02-12T23:05:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.09 , 0.10 2021-02-12T23:05:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171117.23 epoch sec; seqNo:627; 4056135.03 north; 590390.82 east 2021-02-12T23:05:17Z- AHRS_M2 msg: 1613171117.59 epoch sec; seqNo:624 2021-02-12T23:05:17Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.18 psi 2021-02-12T23:05:17Z- handleDepth()-Depth_Keller msg: 1613171117599 epoch sec; seqNo:628; depth 25.24 2021-02-12T23:05:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171117.61 epoch sec; seqNo:628; 4056135.40 north; 590390.82 east 2021-02-12T23:05:17Z- AHRS_M2 msg: 1613171117.98 epoch sec; seqNo:625 2021-02-12T23:05:17Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.18 psi 2021-02-12T23:05:17Z- handleDepth()-Depth_Keller msg: 1613171117991 epoch sec; seqNo:629; depth 25.13 2021-02-12T23:05:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171118.01 epoch sec; seqNo:629; 4056135.40 north; 590390.82 east 2021-02-12T23:05:18Z- AHRS_M2 msg: 1613171118.38 epoch sec; seqNo:626 2021-02-12T23:05:18Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.17 psi 2021-02-12T23:05:18Z- handleDepth()-Depth_Keller msg: 1613171118393 epoch sec; seqNo:630; depth 25.05 2021-02-12T23:05:18Z- handleDvl() - RDI_Pathfinder msg: 1613171118.41 epoch sec; seqNo:263 2021-02-12T23:05:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.48 , 86.09 , 93.67 , 61.75 2021-02-12T23:05:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.15 , 0.01 2021-02-12T23:05:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171118.42 epoch sec; seqNo:630; 4056135.78 north; 590390.81 east 2021-02-12T23:05:18Z- AHRS_M2 msg: 1613171118.79 epoch sec; seqNo:627 2021-02-12T23:05:18Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.17 psi 2021-02-12T23:05:18Z- handleDepth()-Depth_Keller msg: 1613171118797 epoch sec; seqNo:631; depth 24.93 2021-02-12T23:05:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171118.81 epoch sec; seqNo:631; 4056136.16 north; 590390.81 east 2021-02-12T23:05:19Z- AHRS_M2 msg: 1613171119.19 epoch sec; seqNo:628 2021-02-12T23:05:19Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.17 psi 2021-02-12T23:05:19Z- handleDepth()-Depth_Keller msg: 1613171119196 epoch sec; seqNo:632; depth 24.86 2021-02-12T23:05:19Z- handleDvl() - RDI_Pathfinder msg: 1613171119.21 epoch sec; seqNo:264 2021-02-12T23:05:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.48 , 86.09 , 94.75 , 62.57 2021-02-12T23:05:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.13 , -0.08 2021-02-12T23:05:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171119.22 epoch sec; seqNo:632; 4056136.18 north; 590392.03 east 2021-02-12T23:05:19Z- AHRS_M2 msg: 1613171119.60 epoch sec; seqNo:629 2021-02-12T23:05:19Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.18 psi 2021-02-12T23:05:19Z- handleDepth()-Depth_Keller msg: 1613171119617 epoch sec; seqNo:633; depth 24.75 2021-02-12T23:05:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171119.64 epoch sec; seqNo:633; 4056136.55 north; 590392.03 east 2021-02-12T23:05:20Z- AHRS_M2 msg: 1613171120.01 epoch sec; seqNo:630 2021-02-12T23:05:20Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.18 psi 2021-02-12T23:05:20Z- handleDepth()-Depth_Keller msg: 1613171120033 epoch sec; seqNo:634; depth 24.65 2021-02-12T23:05:20Z- handleDvl() - RDI_Pathfinder msg: 1613171120.05 epoch sec; seqNo:265 2021-02-12T23:05:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.48 , 85.01 , 98.00 , 62.98 2021-02-12T23:05:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.10 , -0.12 2021-02-12T23:05:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171120.06 epoch sec; seqNo:634; 4056136.93 north; 590392.02 east 2021-02-12T23:05:20Z- AHRS_M2 msg: 1613171120.40 epoch sec; seqNo:631 2021-02-12T23:05:20Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.18 psi 2021-02-12T23:05:20Z- handleDepth()-Depth_Keller msg: 1613171120413 epoch sec; seqNo:635; depth 24.55 2021-02-12T23:05:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171120.42 epoch sec; seqNo:635; 4056136.93 north; 590392.02 east 2021-02-12T23:05:20Z- AHRS_M2 msg: 1613171120.81 epoch sec; seqNo:632 2021-02-12T23:05:20Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.19 psi 2021-02-12T23:05:20Z- handleDepth()-Depth_Keller msg: 1613171120817 epoch sec; seqNo:636; depth 24.46 2021-02-12T23:05:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171120.83 epoch sec; seqNo:636; 4056137.31 north; 590392.02 east 2021-02-12T23:05:21Z- LcmTrn::handleMessages() - handling 40 messages took 0.70 ms and 126 calls... 2021-02-12T23:05:21Z- waiting for 1613171119.01, time is 1613171120.83 2021-02-12T23:05:21Z- LcmTrn::updateTrn() >>>> pose time 1613171120.83, meas time 1613171120.05 <<<< 2021-02-12T23:05:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171120.05 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:05:21Z- TerrainNav::motionUpdate() - currEstimate = (4056137, 590392.02, 24.46) 2021-02-12T23:05:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:05:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:05:21Z- TNavBF::Projecting Measurements 2021-02-12T23:05:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:05:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:05:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:05:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:05:21Z- TNavFilter::4 good beams. 2021-02-12T23:05:21Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:05:21Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:05:21Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:05:21Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:05:21Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:05:21Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:05:21Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:05:21Z- TNavBF::mmse selected from filter -1 2021-02-12T23:05:21Z- TerrainMapOctree: (vehN, vehE ) = (4056136.82, 590391.63). 2021-02-12T23:05:21Z- TerrainMapOctree: Min Distance = 6405.60. 2021-02-12T23:05:21Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:05:21Z- There is no specific good beam that all particles have in common. 2021-02-12T23:05:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:05:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.116 ExpectedMeasDiff: nan currMeas.covariance: 7.29 2021-02-12T23:05:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:05:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 11.56 2021-02-12T23:05:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:05:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.37 2021-02-12T23:05:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:05:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 6.35 2021-02-12T23:05:21Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:05:21Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:05:21Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:05:21Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:05:21Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:05:21Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:05:21Z- Filter 0 No beams used in subcloud update 2021-02-12T23:05:21Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:05:21Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:05:21Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 821.6 EastVariance: 833.2 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:05:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171120.05 sec. 2021-02-12T23:05:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:05:21Z- TNavBF::mmse selected from filter -1 2021-02-12T23:05:21Z- variances([0], [2], and psi_berg) = 821.425054 833.170401 0.000000 2021-02-12T23:05:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:05:21Z- LcmTrn::publishEstimates() - MLE : 4056099.81 590441.52 24.46 2021-02-12T23:05:21Z- LcmTrn::publishEstimates() - MMSE : 4056137.31 590392.02 24.46 2021-02-12T23:05:21Z- LcmTrn::publishEstimates() - COVAR: 821.43 833.17 0.86 0.00 2021-02-12T23:05:21Z- AHRS_M2 msg: 1613171121.21 epoch sec; seqNo:633 2021-02-12T23:05:21Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.19 psi 2021-02-12T23:05:21Z- handleDepth()-Depth_Keller msg: 1613171121223 epoch sec; seqNo:637; depth 24.36 2021-02-12T23:05:21Z- handleDvl() - RDI_Pathfinder msg: 1613171121.23 epoch sec; seqNo:266 2021-02-12T23:05:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.48 , 89.34 , 95.83 , 62.98 2021-02-12T23:05:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , -0.12 2021-02-12T23:05:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171121.24 epoch sec; seqNo:637; 4056137.70 north; 590393.24 east 2021-02-12T23:05:21Z- AHRS_M2 msg: 1613171121.62 epoch sec; seqNo:634 2021-02-12T23:05:21Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.20 psi 2021-02-12T23:05:21Z- handleDepth()-Depth_Keller msg: 1613171121620 epoch sec; seqNo:638; depth 24.24 2021-02-12T23:05:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171121.63 epoch sec; seqNo:638; 4056137.70 north; 590393.24 east 2021-02-12T23:05:22Z- AHRS_M2 msg: 1613171122.04 epoch sec; seqNo:635 2021-02-12T23:05:22Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.20 psi 2021-02-12T23:05:22Z- handleDepth()-Depth_Keller msg: 1613171122046 epoch sec; seqNo:639; depth 24.16 2021-02-12T23:05:22Z- handleDvl() - RDI_Pathfinder msg: 1613171122.06 epoch sec; seqNo:267 2021-02-12T23:05:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.48 , 88.25 , 94.75 , 63.39 2021-02-12T23:05:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.03 , -0.11 2021-02-12T23:05:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171122.07 epoch sec; seqNo:639; 4056138.08 north; 590393.23 east 2021-02-12T23:05:22Z- AHRS_M2 msg: 1613171122.42 epoch sec; seqNo:636 2021-02-12T23:05:22Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.20 psi 2021-02-12T23:05:22Z- handleDepth()-Depth_Keller msg: 1613171122433 epoch sec; seqNo:640; depth 24.03 2021-02-12T23:05:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171122.44 epoch sec; seqNo:640; 4056138.46 north; 590393.23 east 2021-02-12T23:05:22Z- AHRS_M2 msg: 1613171122.83 epoch sec; seqNo:637 2021-02-12T23:05:22Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.21 psi 2021-02-12T23:05:22Z- handleDepth()-Depth_Keller msg: 1613171122842 epoch sec; seqNo:641; depth 23.94 2021-02-12T23:05:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171122.85 epoch sec; seqNo:641; 4056138.46 north; 590393.23 east 2021-02-12T23:05:23Z- AHRS_M2 msg: 1613171123.23 epoch sec; seqNo:638 2021-02-12T23:05:23Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.21 psi 2021-02-12T23:05:23Z- handleDepth()-Depth_Keller msg: 1613171123236 epoch sec; seqNo:642; depth 23.85 2021-02-12T23:05:23Z- handleDvl() - RDI_Pathfinder msg: 1613171123.25 epoch sec; seqNo:268 2021-02-12T23:05:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.48 , 91.50 , 96.92 , 63.39 2021-02-12T23:05:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.04 , -0.10 2021-02-12T23:05:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171123.26 epoch sec; seqNo:642; 4056138.84 north; 590393.22 east 2021-02-12T23:05:23Z- AHRS_M2 msg: 1613171123.63 epoch sec; seqNo:639 2021-02-12T23:05:23Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.21 psi 2021-02-12T23:05:23Z- handleDepth()-Depth_Keller msg: 1613171123642 epoch sec; seqNo:643; depth 23.74 2021-02-12T23:05:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171123.65 epoch sec; seqNo:643; 4056138.85 north; 590394.44 east 2021-02-12T23:05:24Z- AHRS_M2 msg: 1613171124.05 epoch sec; seqNo:640 2021-02-12T23:05:24Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.21 psi 2021-02-12T23:05:24Z- handleDepth()-Depth_Keller msg: 1613171124058 epoch sec; seqNo:644; depth 23.66 2021-02-12T23:05:24Z- handleDvl() - RDI_Pathfinder msg: 1613171124.07 epoch sec; seqNo:269 2021-02-12T23:05:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.48 , 90.42 , 95.83 , 63.39 2021-02-12T23:05:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.01 , -0.10 2021-02-12T23:05:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171124.08 epoch sec; seqNo:644; 4056139.23 north; 590394.44 east 2021-02-12T23:05:24Z- AHRS_M2 msg: 1613171124.44 epoch sec; seqNo:641 2021-02-12T23:05:24Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.21 psi 2021-02-12T23:05:24Z- handleDepth()-Depth_Keller msg: 1613171124448 epoch sec; seqNo:645; depth 23.54 2021-02-12T23:05:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171124.46 epoch sec; seqNo:645; 4056139.61 north; 590394.44 east 2021-02-12T23:05:24Z- AHRS_M2 msg: 1613171124.85 epoch sec; seqNo:642 2021-02-12T23:05:24Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.20 psi 2021-02-12T23:05:24Z- handleDepth()-Depth_Keller msg: 1613171124855 epoch sec; seqNo:646; depth 23.45 2021-02-12T23:05:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171124.86 epoch sec; seqNo:646; 4056139.61 north; 590394.44 east 2021-02-12T23:05:25Z- AHRS_M2 msg: 1613171125.25 epoch sec; seqNo:643 2021-02-12T23:05:25Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.20 psi 2021-02-12T23:05:25Z- handleDepth()-Depth_Keller msg: 1613171125258 epoch sec; seqNo:647; depth 23.34 2021-02-12T23:05:25Z- handleDvl() - RDI_Pathfinder msg: 1613171125.27 epoch sec; seqNo:270 2021-02-12T23:05:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.89 , 90.42 , 96.92 , 63.39 2021-02-12T23:05:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.10 2021-02-12T23:05:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171125.28 epoch sec; seqNo:647; 4056139.99 north; 590394.43 east 2021-02-12T23:05:25Z- AHRS_M2 msg: 1613171125.66 epoch sec; seqNo:644 2021-02-12T23:05:25Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.19 psi 2021-02-12T23:05:25Z- handleDepth()-Depth_Keller msg: 1613171125665 epoch sec; seqNo:648; depth 23.24 2021-02-12T23:05:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171125.68 epoch sec; seqNo:648; 4056140.37 north; 590394.43 east 2021-02-12T23:05:26Z- AHRS_M2 msg: 1613171126.06 epoch sec; seqNo:645 2021-02-12T23:05:26Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.19 psi 2021-02-12T23:05:26Z- handleDepth()-Depth_Keller msg: 1613171126069 epoch sec; seqNo:649; depth 23.14 2021-02-12T23:05:26Z- handleDvl() - RDI_Pathfinder msg: 1613171126.09 epoch sec; seqNo:271 2021-02-12T23:05:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.29 , 87.17 , 96.91 , 62.97 2021-02-12T23:05:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.07 , -0.07 2021-02-12T23:05:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171126.09 epoch sec; seqNo:649; 4056140.75 north; 590394.42 east 2021-02-12T23:05:26Z- LcmTrn::handleMessages() - handling 45 messages took 0.75 ms and 130 calls... 2021-02-12T23:05:26Z- waiting for 1613171123.83, time is 1613171126.09 2021-02-12T23:05:26Z- LcmTrn::updateTrn() equating pose 1613171126.09 and meas times 1613171126.09 2021-02-12T23:05:26Z- LcmTrn::updateTrn() >>>> pose time 1613171126.09, meas time 1613171126.09 <<<< 2021-02-12T23:05:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171126.09 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:05:26Z- TerrainNav::motionUpdate() - currEstimate = (4056141, 590394.42, 23.14) 2021-02-12T23:05:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:05:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:05:26Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171126.09 sec.; No relevant navigation data available 2021-02-12T23:05:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:05:26Z- TNavBF::mmse selected from filter -1 2021-02-12T23:05:26Z- variances([0], [2], and psi_berg) = 821.016718 833.168469 0.000000 2021-02-12T23:05:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:05:26Z- LcmTrn::publishEstimates() - MLE : 4056103.25 590443.92 23.14 2021-02-12T23:05:26Z- LcmTrn::publishEstimates() - MMSE : 4056140.75 590394.42 23.14 2021-02-12T23:05:26Z- LcmTrn::publishEstimates() - COVAR: 821.02 833.17 1.07 0.00 2021-02-12T23:05:26Z- AHRS_M2 msg: 1613171126.46 epoch sec; seqNo:646 2021-02-12T23:05:26Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.18 psi 2021-02-12T23:05:26Z- handleDepth()-Depth_Keller msg: 1613171126472 epoch sec; seqNo:650; depth 23.06 2021-02-12T23:05:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171126.49 epoch sec; seqNo:650; 4056140.75 north; 590394.42 east 2021-02-12T23:05:26Z- AHRS_M2 msg: 1613171126.88 epoch sec; seqNo:647 2021-02-12T23:05:26Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.18 psi 2021-02-12T23:05:26Z- handleDepth()-Depth_Keller msg: 1613171126880 epoch sec; seqNo:651; depth 22.97 2021-02-12T23:05:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171126.90 epoch sec; seqNo:651; 4056141.14 north; 590395.64 east 2021-02-12T23:05:27Z- AHRS_M2 msg: 1613171127.28 epoch sec; seqNo:648 2021-02-12T23:05:27Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:05:27Z- handleDepth()-Depth_Keller msg: 1613171127285 epoch sec; seqNo:652; depth 22.86 2021-02-12T23:05:27Z- handleDvl() - RDI_Pathfinder msg: 1613171127.30 epoch sec; seqNo:272 2021-02-12T23:05:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.70 , 87.17 , 95.83 , 63.79 2021-02-12T23:05:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.11 , -0.03 2021-02-12T23:05:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171127.31 epoch sec; seqNo:652; 4056141.52 north; 590395.64 east 2021-02-12T23:05:27Z- AHRS_M2 msg: 1613171127.68 epoch sec; seqNo:649 2021-02-12T23:05:27Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-12T23:05:27Z- handleDepth()-Depth_Keller msg: 1613171127682 epoch sec; seqNo:653; depth 22.74 2021-02-12T23:05:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171127.69 epoch sec; seqNo:653; 4056141.52 north; 590395.64 east 2021-02-12T23:05:28Z- AHRS_M2 msg: 1613171128.08 epoch sec; seqNo:650 2021-02-12T23:05:28Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-12T23:05:28Z- handleDepth()-Depth_Keller msg: 1613171128084 epoch sec; seqNo:654; depth 22.67 2021-02-12T23:05:28Z- handleDvl() - RDI_Pathfinder msg: 1613171128.10 epoch sec; seqNo:273 2021-02-12T23:05:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.11 , 88.25 , 96.91 , 63.38 2021-02-12T23:05:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.14 , 0.06 2021-02-12T23:05:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171128.11 epoch sec; seqNo:654; 4056141.90 north; 590395.63 east 2021-02-12T23:05:28Z- AHRS_M2 msg: 1613171128.48 epoch sec; seqNo:651 2021-02-12T23:05:28Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-12T23:05:28Z- handleDepth()-Depth_Keller msg: 1613171128488 epoch sec; seqNo:655; depth 22.56 2021-02-12T23:05:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171128.50 epoch sec; seqNo:655; 4056142.28 north; 590395.63 east 2021-02-12T23:05:28Z- AHRS_M2 msg: 1613171128.89 epoch sec; seqNo:652 2021-02-12T23:05:28Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-12T23:05:28Z- handleDepth()-Depth_Keller msg: 1613171128892 epoch sec; seqNo:656; depth 22.47 2021-02-12T23:05:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171128.90 epoch sec; seqNo:656; 4056142.28 north; 590395.63 east 2021-02-12T23:05:29Z- AHRS_M2 msg: 1613171129.29 epoch sec; seqNo:653 2021-02-12T23:05:29Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-12T23:05:29Z- handleDepth()-Depth_Keller msg: 1613171129298 epoch sec; seqNo:657; depth 22.37 2021-02-12T23:05:29Z- handleDvl() - RDI_Pathfinder msg: 1613171129.31 epoch sec; seqNo:274 2021-02-12T23:05:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.11 , 87.17 , 96.91 , 64.20 2021-02-12T23:05:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.11 , 0.14 2021-02-12T23:05:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171129.32 epoch sec; seqNo:657; 4056142.65 north; 590395.63 east 2021-02-12T23:05:29Z- AHRS_M2 msg: 1613171129.69 epoch sec; seqNo:654 2021-02-12T23:05:29Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-12T23:05:29Z- handleDepth()-Depth_Keller msg: 1613171129701 epoch sec; seqNo:658; depth 22.27 2021-02-12T23:05:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171129.71 epoch sec; seqNo:658; 4056143.03 north; 590395.62 east 2021-02-12T23:05:30Z- AHRS_M2 msg: 1613171130.10 epoch sec; seqNo:655 2021-02-12T23:05:30Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-12T23:05:30Z- handleDepth()-Depth_Keller msg: 1613171130104 epoch sec; seqNo:659; depth 22.18 2021-02-12T23:05:30Z- handleDvl() - RDI_Pathfinder msg: 1613171130.12 epoch sec; seqNo:275 2021-02-12T23:05:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.52 , 88.25 , 96.91 , 64.61 2021-02-12T23:05:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , 0.17 2021-02-12T23:05:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171130.15 epoch sec; seqNo:659; 4056143.42 north; 590396.84 east 2021-02-12T23:05:30Z- AHRS_M2 msg: 1613171130.51 epoch sec; seqNo:656 2021-02-12T23:05:30Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-12T23:05:30Z- handleDepth()-Depth_Keller msg: 1613171130510 epoch sec; seqNo:660; depth 22.08 2021-02-12T23:05:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171130.52 epoch sec; seqNo:660; 4056143.80 north; 590396.84 east 2021-02-12T23:05:30Z- AHRS_M2 msg: 1613171130.91 epoch sec; seqNo:657 2021-02-12T23:05:30Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-12T23:05:30Z- handleDepth()-Depth_Keller msg: 1613171130912 epoch sec; seqNo:661; depth 21.99 2021-02-12T23:05:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171130.93 epoch sec; seqNo:661; 4056143.80 north; 590396.84 east 2021-02-12T23:05:31Z- LcmTrn::handleMessages() - handling 40 messages took 0.64 ms and 127 calls... 2021-02-12T23:05:31Z- waiting for 1613171129.09, time is 1613171130.93 2021-02-12T23:05:31Z- LcmTrn::updateTrn() >>>> pose time 1613171130.93, meas time 1613171130.12 <<<< 2021-02-12T23:05:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171130.12 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:05:31Z- TerrainNav::motionUpdate() - currEstimate = (4056144, 590396.84, 21.99) 2021-02-12T23:05:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:05:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:05:31Z- TNavBF::Projecting Measurements 2021-02-12T23:05:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:05:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:05:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:05:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:05:31Z- TNavFilter::4 good beams. 2021-02-12T23:05:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:05:31Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:05:31Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:05:31Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:05:31Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:05:31Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:05:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:05:31Z- TNavBF::mmse selected from filter -1 2021-02-12T23:05:31Z- TerrainMapOctree: (vehN, vehE ) = (4056143.29, 590396.43). 2021-02-12T23:05:31Z- TerrainMapOctree: Min Distance = 6397.54. 2021-02-12T23:05:31Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:05:31Z- There is no specific good beam that all particles have in common. 2021-02-12T23:05:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:05:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.116 ExpectedMeasDiff: nan currMeas.covariance: 7.73 2021-02-12T23:05:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:05:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 12.46 2021-02-12T23:05:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:05:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 15.03 2021-02-12T23:05:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:05:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 6.68 2021-02-12T23:05:31Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:05:31Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:05:31Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:05:31Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:05:31Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:05:31Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:05:31Z- Filter 0 No beams used in subcloud update 2021-02-12T23:05:31Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:05:31Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:05:31Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 820.7 EastVariance: 833.2 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:05:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171130.12 sec. 2021-02-12T23:05:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:05:31Z- TNavBF::mmse selected from filter -1 2021-02-12T23:05:31Z- variances([0], [2], and psi_berg) = 820.504990 833.166109 0.000000 2021-02-12T23:05:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:05:31Z- LcmTrn::publishEstimates() - MLE : 4056106.30 590445.34 21.99 2021-02-12T23:05:31Z- LcmTrn::publishEstimates() - MMSE : 4056143.80 590396.84 21.99 2021-02-12T23:05:31Z- LcmTrn::publishEstimates() - COVAR: 820.50 833.17 0.89 0.00 2021-02-12T23:05:31Z- AHRS_M2 msg: 1613171131.32 epoch sec; seqNo:658 2021-02-12T23:05:31Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-12T23:05:31Z- handleDepth()-Depth_Keller msg: 1613171131320 epoch sec; seqNo:662; depth 21.88 2021-02-12T23:05:31Z- handleDvl() - RDI_Pathfinder msg: 1613171131.33 epoch sec; seqNo:276 2021-02-12T23:05:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.93 , 89.33 , 96.91 , 64.20 2021-02-12T23:05:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.04 , 0.14 2021-02-12T23:05:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171131.35 epoch sec; seqNo:662; 4056144.18 north; 590396.83 east 2021-02-12T23:05:31Z- AHRS_M2 msg: 1613171131.72 epoch sec; seqNo:659 2021-02-12T23:05:31Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-12T23:05:31Z- handleDepth()-Depth_Keller msg: 1613171131721 epoch sec; seqNo:663; depth 21.79 2021-02-12T23:05:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171131.74 epoch sec; seqNo:663; 4056144.56 north; 590396.83 east 2021-02-12T23:05:32Z- AHRS_M2 msg: 1613171132.12 epoch sec; seqNo:660 2021-02-12T23:05:32Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-12T23:05:32Z- handleDepth()-Depth_Keller msg: 1613171132129 epoch sec; seqNo:664; depth 21.70 2021-02-12T23:05:32Z- handleDvl() - RDI_Pathfinder msg: 1613171132.15 epoch sec; seqNo:277 2021-02-12T23:05:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.15 , 89.27 , 96.85 , 65.02 2021-02-12T23:05:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.05 , 0.08 2021-02-12T23:05:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171132.16 epoch sec; seqNo:664; 4056144.94 north; 590396.82 east 2021-02-12T23:05:32Z- AHRS_M2 msg: 1613171132.52 epoch sec; seqNo:661 2021-02-12T23:05:32Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-12T23:05:32Z- handleDepth()-Depth_Keller msg: 1613171132528 epoch sec; seqNo:665; depth 21.58 2021-02-12T23:05:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171132.55 epoch sec; seqNo:665; 4056145.33 north; 590398.04 east 2021-02-12T23:05:32Z- AHRS_M2 msg: 1613171132.93 epoch sec; seqNo:662 2021-02-12T23:05:32Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-12T23:05:32Z- handleDepth()-Depth_Keller msg: 1613171132932 epoch sec; seqNo:666; depth 21.49 2021-02-12T23:05:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171132.94 epoch sec; seqNo:666; 4056145.33 north; 590398.04 east 2021-02-12T23:05:33Z- AHRS_M2 msg: 1613171133.33 epoch sec; seqNo:663 2021-02-12T23:05:33Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-12T23:05:33Z- handleDepth()-Depth_Keller msg: 1613171133353 epoch sec; seqNo:667; depth 21.41 2021-02-12T23:05:33Z- handleDvl() - RDI_Pathfinder msg: 1613171133.36 epoch sec; seqNo:278 2021-02-12T23:05:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.56 , 87.10 , 96.85 , 65.43 2021-02-12T23:05:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.11 , 0.00 2021-02-12T23:05:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171133.39 epoch sec; seqNo:667; 4056145.71 north; 590398.04 east 2021-02-12T23:05:33Z- AHRS_M2 msg: 1613171133.73 epoch sec; seqNo:664 2021-02-12T23:05:33Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.13 psi 2021-02-12T23:05:33Z- handleDepth()-Depth_Keller msg: 1613171133740 epoch sec; seqNo:668; depth 21.29 2021-02-12T23:05:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:05:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:05:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:05:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:05:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:05:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:05:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:05:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:05:33Z- handleDvl() - RDI_Pathfinder msg: 1613171133.74 epoch sec; seqNo:279 2021-02-12T23:05:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:05:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.11 , 0.00 2021-02-12T23:05:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171133.76 epoch sec; seqNo:668; 4056146.09 north; 590398.03 east 2021-02-12T23:05:34Z- AHRS_M2 msg: 1613171134.15 epoch sec; seqNo:665 2021-02-12T23:05:34Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.13 psi 2021-02-12T23:05:34Z- handleDepth()-Depth_Keller msg: 1613171134151 epoch sec; seqNo:669; depth 21.19 2021-02-12T23:05:34Z- handleDvl() - RDI_Pathfinder msg: 1613171134.16 epoch sec; seqNo:280 2021-02-12T23:05:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.97 , 86.02 , 96.85 , 65.84 2021-02-12T23:05:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.12 , -0.07 2021-02-12T23:05:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171134.19 epoch sec; seqNo:669; 4056146.09 north; 590398.03 east 2021-02-12T23:05:34Z- AHRS_M2 msg: 1613171134.55 epoch sec; seqNo:666 2021-02-12T23:05:34Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.13 psi 2021-02-12T23:05:34Z- handleDepth()-Depth_Keller msg: 1613171134552 epoch sec; seqNo:670; depth 21.10 2021-02-12T23:05:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171134.56 epoch sec; seqNo:670; 4056146.48 north; 590399.25 east 2021-02-12T23:05:34Z- AHRS_M2 msg: 1613171134.95 epoch sec; seqNo:667 2021-02-12T23:05:34Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.13 psi 2021-02-12T23:05:34Z- handleDepth()-Depth_Keller msg: 1613171134956 epoch sec; seqNo:671; depth 20.97 2021-02-12T23:05:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:05:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:05:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:05:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:05:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:05:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:05:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:05:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:05:34Z- handleDvl() - RDI_Pathfinder msg: 1613171134.96 epoch sec; seqNo:281 2021-02-12T23:05:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:05:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.12 , -0.07 2021-02-12T23:05:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171134.97 epoch sec; seqNo:671; 4056146.86 north; 590399.25 east 2021-02-12T23:05:35Z- AHRS_M2 msg: 1613171135.35 epoch sec; seqNo:668 2021-02-12T23:05:35Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.12 psi 2021-02-12T23:05:35Z- handleDepth()-Depth_Keller msg: 1613171135357 epoch sec; seqNo:672; depth 20.88 2021-02-12T23:05:35Z- handleDvl() - RDI_Pathfinder msg: 1613171135.38 epoch sec; seqNo:282 2021-02-12T23:05:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.97 , 88.18 , 96.85 , 65.84 2021-02-12T23:05:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.11 , -0.12 2021-02-12T23:05:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171135.39 epoch sec; seqNo:672; 4056146.86 north; 590399.25 east 2021-02-12T23:05:35Z- AHRS_M2 msg: 1613171135.76 epoch sec; seqNo:669 2021-02-12T23:05:35Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.12 psi 2021-02-12T23:05:35Z- handleDepth()-Depth_Keller msg: 1613171135773 epoch sec; seqNo:673; depth 20.79 2021-02-12T23:05:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171135.78 epoch sec; seqNo:673; 4056147.24 north; 590399.24 east 2021-02-12T23:05:36Z- AHRS_M2 msg: 1613171136.17 epoch sec; seqNo:670 2021-02-12T23:05:36Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.12 psi 2021-02-12T23:05:36Z- handleDepth()-Depth_Keller msg: 1613171136173 epoch sec; seqNo:674; depth 20.68 2021-02-12T23:05:36Z- handleDvl() - RDI_Pathfinder msg: 1613171136.18 epoch sec; seqNo:283 2021-02-12T23:05:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.38 , 86.02 , 96.85 , 65.84 2021-02-12T23:05:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.03 , -0.18 2021-02-12T23:05:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171136.19 epoch sec; seqNo:674; 4056147.62 north; 590399.24 east 2021-02-12T23:05:36Z- LcmTrn::handleMessages() - handling 47 messages took 0.73 ms and 133 calls... 2021-02-12T23:05:36Z- waiting for 1613171133.93, time is 1613171136.19 2021-02-12T23:05:36Z- LcmTrn::updateTrn() equating pose 1613171136.19 and meas times 1613171136.19 2021-02-12T23:05:36Z- LcmTrn::updateTrn() >>>> pose time 1613171136.19, meas time 1613171136.19 <<<< 2021-02-12T23:05:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171136.19 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:05:36Z- TerrainNav::motionUpdate() - currEstimate = (4056148, 590399.24, 20.68) 2021-02-12T23:05:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:05:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:05:36Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171136.19 sec.; No relevant navigation data available 2021-02-12T23:05:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:05:36Z- TNavBF::mmse selected from filter -1 2021-02-12T23:05:36Z- variances([0], [2], and psi_berg) = 820.093383 833.164248 0.000000 2021-02-12T23:05:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:05:36Z- LcmTrn::publishEstimates() - MLE : 4056111.12 590350.74 20.68 2021-02-12T23:05:36Z- LcmTrn::publishEstimates() - MMSE : 4056147.62 590399.24 20.68 2021-02-12T23:05:36Z- LcmTrn::publishEstimates() - COVAR: 820.09 833.16 1.12 0.00 2021-02-12T23:05:36Z- AHRS_M2 msg: 1613171136.56 epoch sec; seqNo:671 2021-02-12T23:05:36Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.12 psi 2021-02-12T23:05:36Z- handleDepth()-Depth_Keller msg: 1613171136568 epoch sec; seqNo:675; depth 20.57 2021-02-12T23:05:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171136.59 epoch sec; seqNo:675; 4056147.63 north; 590400.46 east 2021-02-12T23:05:36Z- AHRS_M2 msg: 1613171136.98 epoch sec; seqNo:672 2021-02-12T23:05:36Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.12 psi 2021-02-12T23:05:36Z- handleDepth()-Depth_Keller msg: 1613171136983 epoch sec; seqNo:676; depth 20.49 2021-02-12T23:05:37Z- handleDvl() - RDI_Pathfinder msg: 1613171136.99 epoch sec; seqNo:284 2021-02-12T23:05:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.38 , 0.00 , 96.85 , 66.25 2021-02-12T23:05:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.00 , -0.19 2021-02-12T23:05:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171137.02 epoch sec; seqNo:676; 4056148.01 north; 590400.45 east 2021-02-12T23:05:37Z- AHRS_M2 msg: 1613171137.37 epoch sec; seqNo:673 2021-02-12T23:05:37Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.12 psi 2021-02-12T23:05:37Z- handleDepth()-Depth_Keller msg: 1613171137385 epoch sec; seqNo:677; depth 20.38 2021-02-12T23:05:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:05:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:05:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:05:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:05:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:05:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:05:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:05:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:05:37Z- handleDvl() - RDI_Pathfinder msg: 1613171137.39 epoch sec; seqNo:285 2021-02-12T23:05:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:05:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.00 , -0.19 2021-02-12T23:05:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171137.40 epoch sec; seqNo:677; 4056148.01 north; 590400.45 east 2021-02-12T23:05:37Z- AHRS_M2 msg: 1613171137.78 epoch sec; seqNo:674 2021-02-12T23:05:37Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.12 psi 2021-02-12T23:05:37Z- handleDepth()-Depth_Keller msg: 1613171137790 epoch sec; seqNo:678; depth 20.28 2021-02-12T23:05:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171137.80 epoch sec; seqNo:678; 4056148.39 north; 590400.45 east 2021-02-12T23:05:38Z- AHRS_M2 msg: 1613171138.18 epoch sec; seqNo:675 2021-02-12T23:05:38Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.13 psi 2021-02-12T23:05:38Z- handleDepth()-Depth_Keller msg: 1613171138189 epoch sec; seqNo:679; depth 20.16 2021-02-12T23:05:38Z- handleDvl() - RDI_Pathfinder msg: 1613171138.21 epoch sec; seqNo:286 2021-02-12T23:05:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.97 , 87.17 , 96.91 , 65.02 2021-02-12T23:05:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.09 , -0.18 2021-02-12T23:05:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171138.22 epoch sec; seqNo:679; 4056148.77 north; 590400.45 east 2021-02-12T23:05:38Z- AHRS_M2 msg: 1613171138.58 epoch sec; seqNo:676 2021-02-12T23:05:38Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.13 psi 2021-02-12T23:05:38Z- handleDepth()-Depth_Keller msg: 1613171138588 epoch sec; seqNo:680; depth 20.07 2021-02-12T23:05:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:05:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:05:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:05:38Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:05:38Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:05:38Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:05:38Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:05:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:05:38Z- handleDvl() - RDI_Pathfinder msg: 1613171138.59 epoch sec; seqNo:287 2021-02-12T23:05:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:05:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.09 , -0.18 2021-02-12T23:05:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171138.60 epoch sec; seqNo:680; 4056148.77 north; 590400.45 east 2021-02-12T23:05:38Z- AHRS_M2 msg: 1613171138.99 epoch sec; seqNo:677 2021-02-12T23:05:38Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.14 psi 2021-02-12T23:05:39Z- handleDepth()-Depth_Keller msg: 1613171139000 epoch sec; seqNo:681; depth 19.97 2021-02-12T23:05:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171139.03 epoch sec; seqNo:681; 4056149.15 north; 590400.44 east 2021-02-12T23:05:39Z- AHRS_M2 msg: 1613171139.39 epoch sec; seqNo:678 2021-02-12T23:05:39Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.14 psi 2021-02-12T23:05:39Z- handleDepth()-Depth_Keller msg: 1613171139397 epoch sec; seqNo:682; depth 19.84 2021-02-12T23:05:39Z- handleDvl() - RDI_Pathfinder msg: 1613171139.41 epoch sec; seqNo:288 2021-02-12T23:05:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.97 , 88.18 , 96.85 , 65.43 2021-02-12T23:05:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.09 , -0.09 2021-02-12T23:05:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171139.42 epoch sec; seqNo:682; 4056149.52 north; 590400.44 east 2021-02-12T23:05:39Z- AHRS_M2 msg: 1613171139.80 epoch sec; seqNo:679 2021-02-12T23:05:39Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.14 psi 2021-02-12T23:05:39Z- handleDepth()-Depth_Keller msg: 1613171139800 epoch sec; seqNo:683; depth 19.73 2021-02-12T23:05:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:05:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:05:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:05:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:05:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:05:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:05:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:05:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:05:39Z- handleDvl() - RDI_Pathfinder msg: 1613171139.80 epoch sec; seqNo:289 2021-02-12T23:05:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:05:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.09 , -0.09 2021-02-12T23:05:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171139.81 epoch sec; seqNo:683; 4056149.54 north; 590401.66 east 2021-02-12T23:05:40Z- AHRS_M2 msg: 1613171140.20 epoch sec; seqNo:680 2021-02-12T23:05:40Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.14 psi 2021-02-12T23:05:40Z- handleDepth()-Depth_Keller msg: 1613171140205 epoch sec; seqNo:684; depth 19.65 2021-02-12T23:05:40Z- handleDvl() - RDI_Pathfinder msg: 1613171140.22 epoch sec; seqNo:290 2021-02-12T23:05:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.38 , 88.18 , 100.09 , 66.25 2021-02-12T23:05:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.11 , 0.02 2021-02-12T23:05:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171140.23 epoch sec; seqNo:684; 4056149.92 north; 590401.66 east 2021-02-12T23:05:40Z- AHRS_M2 msg: 1613171140.61 epoch sec; seqNo:681 2021-02-12T23:05:40Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.14 psi 2021-02-12T23:05:40Z- handleDepth()-Depth_Keller msg: 1613171140610 epoch sec; seqNo:685; depth 19.54 2021-02-12T23:05:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171140.62 epoch sec; seqNo:685; 4056150.30 north; 590401.65 east 2021-02-12T23:05:41Z- AHRS_M2 msg: 1613171141.03 epoch sec; seqNo:682 2021-02-12T23:05:41Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.14 psi 2021-02-12T23:05:41Z- handleDepth()-Depth_Keller msg: 1613171141037 epoch sec; seqNo:686; depth 19.44 2021-02-12T23:05:41Z- handleDvl() - RDI_Pathfinder msg: 1613171141.05 epoch sec; seqNo:291 2021-02-12T23:05:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.97 , 91.30 , 96.71 , 66.65 2021-02-12T23:05:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.07 , 0.08 2021-02-12T23:05:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171141.06 epoch sec; seqNo:686; 4056150.30 north; 590401.65 east 2021-02-12T23:05:41Z- LcmTrn::handleMessages() - handling 44 messages took 0.60 ms and 132 calls... 2021-02-12T23:05:41Z- waiting for 1613171139.19, time is 1613171141.06 2021-02-12T23:05:41Z- LcmTrn::updateTrn() equating pose 1613171141.06 and meas times 1613171141.06 2021-02-12T23:05:41Z- LcmTrn::updateTrn() >>>> pose time 1613171141.06, meas time 1613171141.06 <<<< 2021-02-12T23:05:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171141.06 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:05:41Z- TerrainNav::motionUpdate() - currEstimate = (4056150, 590401.65, 19.44) 2021-02-12T23:05:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:05:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:05:41Z- TNavBF::Projecting Measurements 2021-02-12T23:05:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:05:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:05:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:05:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:05:41Z- TNavFilter::4 good beams. 2021-02-12T23:05:41Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:05:41Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:05:41Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:05:41Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:05:41Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:05:41Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:05:41Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:05:41Z- TNavBF::mmse selected from filter -1 2021-02-12T23:05:41Z- TerrainMapOctree: (vehN, vehE ) = (4056150.30, 590401.65). 2021-02-12T23:05:41Z- TerrainMapOctree: Min Distance = 6388.80. 2021-02-12T23:05:41Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:05:41Z- There is no specific good beam that all particles have in common. 2021-02-12T23:05:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:05:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.115 ExpectedMeasDiff: nan currMeas.covariance: 8.29 2021-02-12T23:05:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:05:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 13.34 2021-02-12T23:05:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:05:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 14.96 2021-02-12T23:05:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:05:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 7.11 2021-02-12T23:05:41Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:05:41Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:05:41Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:05:41Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:05:41Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:05:41Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:05:41Z- Filter 0 No beams used in subcloud update 2021-02-12T23:05:41Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:05:41Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:05:41Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 819.7 EastVariance: 833.2 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:05:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171141.06 sec. 2021-02-12T23:05:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:05:41Z- TNavBF::mmse selected from filter -1 2021-02-12T23:05:41Z- variances([0], [2], and psi_berg) = 819.729848 833.162638 0.000000 2021-02-12T23:05:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:05:41Z- LcmTrn::publishEstimates() - MLE : 4056113.80 590353.15 19.44 2021-02-12T23:05:41Z- LcmTrn::publishEstimates() - MMSE : 4056150.30 590401.65 19.44 2021-02-12T23:05:41Z- LcmTrn::publishEstimates() - COVAR: 819.73 833.16 0.91 0.00 2021-02-12T23:05:41Z- AHRS_M2 msg: 1613171141.41 epoch sec; seqNo:683 2021-02-12T23:05:41Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.14 psi 2021-02-12T23:05:41Z- handleDepth()-Depth_Keller msg: 1613171141416 epoch sec; seqNo:687; depth 19.32 2021-02-12T23:05:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171141.43 epoch sec; seqNo:687; 4056150.67 north; 590401.65 east 2021-02-12T23:05:41Z- AHRS_M2 msg: 1613171141.82 epoch sec; seqNo:684 2021-02-12T23:05:41Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.14 psi 2021-02-12T23:05:41Z- handleDepth()-Depth_Keller msg: 1613171141825 epoch sec; seqNo:688; depth 19.21 2021-02-12T23:05:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171141.83 epoch sec; seqNo:688; 4056151.05 north; 590401.64 east 2021-02-12T23:05:42Z- AHRS_M2 msg: 1613171142.22 epoch sec; seqNo:685 2021-02-12T23:05:42Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.15 psi 2021-02-12T23:05:42Z- handleDepth()-Depth_Keller msg: 1613171142230 epoch sec; seqNo:689; depth 19.13 2021-02-12T23:05:42Z- handleDvl() - RDI_Pathfinder msg: 1613171142.24 epoch sec; seqNo:292 2021-02-12T23:05:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.38 , 91.30 , 99.96 , 66.25 2021-02-12T23:05:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.09 , 0.12 2021-02-12T23:05:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171142.26 epoch sec; seqNo:689; 4056151.05 north; 590401.64 east 2021-02-12T23:05:42Z- AHRS_M2 msg: 1613171142.62 epoch sec; seqNo:686 2021-02-12T23:05:42Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.15 psi 2021-02-12T23:05:42Z- handleDepth()-Depth_Keller msg: 1613171142629 epoch sec; seqNo:690; depth 19.01 2021-02-12T23:05:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171142.65 epoch sec; seqNo:690; 4056151.43 north; 590401.64 east 2021-02-12T23:05:43Z- AHRS_M2 msg: 1613171143.03 epoch sec; seqNo:687 2021-02-12T23:05:43Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.15 psi 2021-02-12T23:05:43Z- handleDepth()-Depth_Keller msg: 1613171143032 epoch sec; seqNo:691; depth 18.91 2021-02-12T23:05:43Z- handleDvl() - RDI_Pathfinder msg: 1613171143.04 epoch sec; seqNo:293 2021-02-12T23:05:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.38 , 88.66 , 97.18 , 66.65 2021-02-12T23:05:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.08 , 0.15 2021-02-12T23:05:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171143.07 epoch sec; seqNo:691; 4056151.82 north; 590402.86 east 2021-02-12T23:05:43Z- AHRS_M2 msg: 1613171143.43 epoch sec; seqNo:688 2021-02-12T23:05:43Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.15 psi 2021-02-12T23:05:43Z- handleDepth()-Depth_Keller msg: 1613171143446 epoch sec; seqNo:692; depth 18.79 2021-02-12T23:05:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171143.46 epoch sec; seqNo:692; 4056152.20 north; 590402.85 east 2021-02-12T23:05:43Z- AHRS_M2 msg: 1613171143.84 epoch sec; seqNo:689 2021-02-12T23:05:43Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.16 psi 2021-02-12T23:05:43Z- handleDepth()-Depth_Keller msg: 1613171143849 epoch sec; seqNo:693; depth 18.70 2021-02-12T23:05:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171143.86 epoch sec; seqNo:693; 4056152.20 north; 590402.85 east 2021-02-12T23:05:44Z- AHRS_M2 msg: 1613171144.25 epoch sec; seqNo:690 2021-02-12T23:05:44Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-12T23:05:44Z- handleDepth()-Depth_Keller msg: 1613171144250 epoch sec; seqNo:694; depth 18.61 2021-02-12T23:05:44Z- handleDvl() - RDI_Pathfinder msg: 1613171144.26 epoch sec; seqNo:294 2021-02-12T23:05:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.20 , 91.36 , 96.78 , 67.88 2021-02-12T23:05:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.17 2021-02-12T23:05:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171144.28 epoch sec; seqNo:694; 4056152.58 north; 590402.85 east 2021-02-12T23:05:44Z- AHRS_M2 msg: 1613171144.64 epoch sec; seqNo:691 2021-02-12T23:05:44Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-12T23:05:44Z- handleDepth()-Depth_Keller msg: 1613171144649 epoch sec; seqNo:695; depth 18.49 2021-02-12T23:05:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171144.66 epoch sec; seqNo:695; 4056152.96 north; 590402.84 east 2021-02-12T23:05:45Z- AHRS_M2 msg: 1613171145.05 epoch sec; seqNo:692 2021-02-12T23:05:45Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-12T23:05:45Z- handleDepth()-Depth_Keller msg: 1613171145056 epoch sec; seqNo:696; depth 18.41 2021-02-12T23:05:45Z- handleDvl() - RDI_Pathfinder msg: 1613171145.08 epoch sec; seqNo:295 2021-02-12T23:05:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.79 , 90.21 , 96.71 , 68.29 2021-02-12T23:05:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.06 , 0.14 2021-02-12T23:05:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171145.09 epoch sec; seqNo:696; 4056153.35 north; 590404.06 east 2021-02-12T23:05:45Z- AHRS_M2 msg: 1613171145.45 epoch sec; seqNo:693 2021-02-12T23:05:45Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-12T23:05:45Z- handleDepth()-Depth_Keller msg: 1613171145461 epoch sec; seqNo:697; depth 18.28 2021-02-12T23:05:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171145.47 epoch sec; seqNo:697; 4056153.35 north; 590404.06 east 2021-02-12T23:05:45Z- AHRS_M2 msg: 1613171145.86 epoch sec; seqNo:694 2021-02-12T23:05:45Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-12T23:05:45Z- handleDepth()-Depth_Keller msg: 1613171145860 epoch sec; seqNo:698; depth 18.19 2021-02-12T23:05:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171145.87 epoch sec; seqNo:698; 4056153.73 north; 590404.06 east 2021-02-12T23:05:46Z- AHRS_M2 msg: 1613171146.26 epoch sec; seqNo:695 2021-02-12T23:05:46Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.15 psi 2021-02-12T23:05:46Z- handleDepth()-Depth_Keller msg: 1613171146266 epoch sec; seqNo:699; depth 18.10 2021-02-12T23:05:46Z- handleDvl() - RDI_Pathfinder msg: 1613171146.28 epoch sec; seqNo:296 2021-02-12T23:05:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.61 , 91.30 , 96.71 , 68.29 2021-02-12T23:05:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.13 , 0.05 2021-02-12T23:05:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171146.29 epoch sec; seqNo:699; 4056154.11 north; 590404.05 east 2021-02-12T23:05:46Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-12T23:05:46Z- waiting for 1613171144.06, time is 1613171146.29 2021-02-12T23:05:46Z- LcmTrn::updateTrn() equating pose 1613171146.29 and meas times 1613171146.29 2021-02-12T23:05:46Z- LcmTrn::updateTrn() >>>> pose time 1613171146.29, meas time 1613171146.29 <<<< 2021-02-12T23:05:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171146.29 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:05:46Z- TerrainNav::motionUpdate() - currEstimate = (4056154, 590404.05, 18.10) 2021-02-12T23:05:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:05:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:05:46Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171146.29 sec.; No relevant navigation data available 2021-02-12T23:05:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:05:46Z- TNavBF::mmse selected from filter -1 2021-02-12T23:05:46Z- variances([0], [2], and psi_berg) = 819.327785 833.160889 0.000000 2021-02-12T23:05:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:05:46Z- LcmTrn::publishEstimates() - MLE : 4056117.61 590355.55 18.10 2021-02-12T23:05:46Z- LcmTrn::publishEstimates() - MMSE : 4056154.11 590404.05 18.10 2021-02-12T23:05:46Z- LcmTrn::publishEstimates() - COVAR: 819.33 833.16 1.13 0.00 2021-02-12T23:05:46Z- AHRS_M2 msg: 1613171146.66 epoch sec; seqNo:696 2021-02-12T23:05:46Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.15 psi 2021-02-12T23:05:46Z- handleDepth()-Depth_Keller msg: 1613171146668 epoch sec; seqNo:700; depth 17.98 2021-02-12T23:05:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171146.69 epoch sec; seqNo:700; 4056154.49 north; 590404.05 east 2021-02-12T23:05:47Z- AHRS_M2 msg: 1613171147.07 epoch sec; seqNo:697 2021-02-12T23:05:47Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.14 psi 2021-02-12T23:05:47Z- handleDepth()-Depth_Keller msg: 1613171147072 epoch sec; seqNo:701; depth 17.89 2021-02-12T23:05:47Z- handleDvl() - RDI_Pathfinder msg: 1613171147.09 epoch sec; seqNo:297 2021-02-12T23:05:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.61 , 89.88 , 97.18 , 68.70 2021-02-12T23:05:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.16 , -0.06 2021-02-12T23:05:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171147.12 epoch sec; seqNo:701; 4056154.50 north; 590405.27 east 2021-02-12T23:05:47Z- AHRS_M2 msg: 1613171147.47 epoch sec; seqNo:698 2021-02-12T23:05:47Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.14 psi 2021-02-12T23:05:47Z- handleDepth()-Depth_Keller msg: 1613171147476 epoch sec; seqNo:702; depth 17.78 2021-02-12T23:05:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171147.49 epoch sec; seqNo:702; 4056154.88 north; 590405.27 east 2021-02-12T23:05:47Z- AHRS_M2 msg: 1613171147.88 epoch sec; seqNo:699 2021-02-12T23:05:47Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.14 psi 2021-02-12T23:05:47Z- handleDepth()-Depth_Keller msg: 1613171147888 epoch sec; seqNo:703; depth 17.69 2021-02-12T23:05:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171147.90 epoch sec; seqNo:703; 4056154.88 north; 590405.27 east 2021-02-12T23:05:48Z- AHRS_M2 msg: 1613171148.28 epoch sec; seqNo:700 2021-02-12T23:05:48Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.14 psi 2021-02-12T23:05:48Z- handleDepth()-Depth_Keller msg: 1613171148284 epoch sec; seqNo:704; depth 17.58 2021-02-12T23:05:48Z- handleDvl() - RDI_Pathfinder msg: 1613171148.30 epoch sec; seqNo:298 2021-02-12T23:05:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.42 , 92.38 , 96.71 , 68.29 2021-02-12T23:05:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.12 , -0.12 2021-02-12T23:05:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171148.32 epoch sec; seqNo:704; 4056155.26 north; 590405.26 east 2021-02-12T23:05:48Z- AHRS_M2 msg: 1613171148.68 epoch sec; seqNo:701 2021-02-12T23:05:48Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.13 psi 2021-02-12T23:05:48Z- handleDepth()-Depth_Keller msg: 1613171148688 epoch sec; seqNo:705; depth 17.49 2021-02-12T23:05:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171148.70 epoch sec; seqNo:705; 4056155.64 north; 590405.26 east 2021-02-12T23:05:49Z- AHRS_M2 msg: 1613171149.09 epoch sec; seqNo:702 2021-02-12T23:05:49Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.13 psi 2021-02-12T23:05:49Z- handleDepth()-Depth_Keller msg: 1613171149094 epoch sec; seqNo:706; depth 17.38 2021-02-12T23:05:49Z- handleDvl() - RDI_Pathfinder msg: 1613171149.11 epoch sec; seqNo:299 2021-02-12T23:05:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.42 , 89.88 , 103.27 , 68.29 2021-02-12T23:05:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.07 , -0.11 2021-02-12T23:05:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171149.12 epoch sec; seqNo:706; 4056155.65 north; 590406.48 east 2021-02-12T23:05:49Z- AHRS_M2 msg: 1613171149.49 epoch sec; seqNo:703 2021-02-12T23:05:49Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.13 psi 2021-02-12T23:05:49Z- handleDepth()-Depth_Keller msg: 1613171149509 epoch sec; seqNo:707; depth 17.26 2021-02-12T23:05:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171149.53 epoch sec; seqNo:707; 4056156.03 north; 590406.48 east 2021-02-12T23:05:49Z- AHRS_M2 msg: 1613171149.89 epoch sec; seqNo:704 2021-02-12T23:05:49Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.13 psi 2021-02-12T23:05:49Z- handleDepth()-Depth_Keller msg: 1613171149905 epoch sec; seqNo:708; depth 17.17 2021-02-12T23:05:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171149.91 epoch sec; seqNo:708; 4056156.41 north; 590406.47 east 2021-02-12T23:05:50Z- AHRS_M2 msg: 1613171150.30 epoch sec; seqNo:705 2021-02-12T23:05:50Z- AHRS_M2 msg: 0.69 phi; 0.32 theta; -0.13 psi 2021-02-12T23:05:50Z- handleDepth()-Depth_Keller msg: 1613171150304 epoch sec; seqNo:709; depth 17.06 2021-02-12T23:05:50Z- handleDvl() - RDI_Pathfinder msg: 1613171150.32 epoch sec; seqNo:300 2021-02-12T23:05:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.83 , 91.03 , 105.64 , 67.88 2021-02-12T23:05:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.07 , -0.11 2021-02-12T23:05:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171150.33 epoch sec; seqNo:709; 4056156.41 north; 590406.47 east 2021-02-12T23:05:50Z- AHRS_M2 msg: 1613171150.70 epoch sec; seqNo:706 2021-02-12T23:05:50Z- AHRS_M2 msg: 0.69 phi; 0.32 theta; -0.13 psi 2021-02-12T23:05:50Z- handleDepth()-Depth_Keller msg: 1613171150710 epoch sec; seqNo:710; depth 16.95 2021-02-12T23:05:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171150.72 epoch sec; seqNo:710; 4056156.79 north; 590406.47 east 2021-02-12T23:05:51Z- AHRS_M2 msg: 1613171151.11 epoch sec; seqNo:707 2021-02-12T23:05:51Z- AHRS_M2 msg: 0.69 phi; 0.32 theta; -0.13 psi 2021-02-12T23:05:51Z- handleDepth()-Depth_Keller msg: 1613171151114 epoch sec; seqNo:711; depth 16.84 2021-02-12T23:05:51Z- handleDvl() - RDI_Pathfinder msg: 1613171151.14 epoch sec; seqNo:301 2021-02-12T23:05:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.24 , 90.89 , 104.29 , 69.11 2021-02-12T23:05:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.05 , -0.13 2021-02-12T23:05:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171151.15 epoch sec; seqNo:711; 4056156.80 north; 590407.69 east 2021-02-12T23:05:51Z- LcmTrn::handleMessages() - handling 41 messages took 0.70 ms and 127 calls... 2021-02-12T23:05:51Z- waiting for 1613171149.29, time is 1613171151.15 2021-02-12T23:05:51Z- LcmTrn::updateTrn() equating pose 1613171151.15 and meas times 1613171151.15 2021-02-12T23:05:51Z- LcmTrn::updateTrn() >>>> pose time 1613171151.15, meas time 1613171151.15 <<<< 2021-02-12T23:05:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171151.15 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:05:51Z- TerrainNav::motionUpdate() - currEstimate = (4056157, 590407.69, 16.84) 2021-02-12T23:05:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:05:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:05:51Z- TNavBF::Projecting Measurements 2021-02-12T23:05:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:05:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:05:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:05:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:05:51Z- TNavFilter::4 good beams. 2021-02-12T23:05:51Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:05:51Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:05:51Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:05:51Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:05:51Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:05:51Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:05:51Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:05:51Z- TNavBF::mmse selected from filter -1 2021-02-12T23:05:51Z- TerrainMapOctree: (vehN, vehE ) = (4056156.80, 590407.69). 2021-02-12T23:05:51Z- TerrainMapOctree: Min Distance = 6379.97. 2021-02-12T23:05:51Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:05:51Z- There is no specific good beam that all particles have in common. 2021-02-12T23:05:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:05:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 9.06 2021-02-12T23:05:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:05:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 13.22 2021-02-12T23:05:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:05:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 17.40 2021-02-12T23:05:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:05:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 7.64 2021-02-12T23:05:51Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:05:51Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:05:51Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:05:51Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:05:51Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:05:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:05:51Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:05:51Z- Filter 0 No beams used in subcloud update 2021-02-12T23:05:51Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:05:51Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:05:51Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 818.9 EastVariance: 833.2 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:05:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171151.15 sec. 2021-02-12T23:05:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:05:51Z- TNavBF::mmse selected from filter -1 2021-02-12T23:05:51Z- variances([0], [2], and psi_berg) = 818.929894 833.159190 0.000000 2021-02-12T23:05:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:05:51Z- LcmTrn::publishEstimates() - MLE : 4056120.30 590361.19 16.84 2021-02-12T23:05:51Z- LcmTrn::publishEstimates() - MMSE : 4056156.80 590407.69 16.84 2021-02-12T23:05:51Z- LcmTrn::publishEstimates() - COVAR: 818.93 833.16 0.95 0.00 2021-02-12T23:05:51Z- AHRS_M2 msg: 1613171151.51 epoch sec; seqNo:708 2021-02-12T23:05:51Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.13 psi 2021-02-12T23:05:51Z- handleDepth()-Depth_Keller msg: 1613171151516 epoch sec; seqNo:712; depth 16.73 2021-02-12T23:05:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171151.53 epoch sec; seqNo:712; 4056157.18 north; 590407.69 east 2021-02-12T23:05:51Z- AHRS_M2 msg: 1613171151.92 epoch sec; seqNo:709 2021-02-12T23:05:51Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.13 psi 2021-02-12T23:05:51Z- handleDepth()-Depth_Keller msg: 1613171151929 epoch sec; seqNo:713; depth 16.64 2021-02-12T23:05:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171151.95 epoch sec; seqNo:713; 4056157.56 north; 590407.68 east 2021-02-12T23:05:52Z- AHRS_M2 msg: 1613171152.32 epoch sec; seqNo:710 2021-02-12T23:05:52Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.14 psi 2021-02-12T23:05:52Z- handleDepth()-Depth_Keller msg: 1613171152324 epoch sec; seqNo:714; depth 16.51 2021-02-12T23:05:52Z- handleDvl() - RDI_Pathfinder msg: 1613171152.34 epoch sec; seqNo:302 2021-02-12T23:05:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.83 , 90.89 , 96.98 , 69.51 2021-02-12T23:05:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.01 , -0.17 2021-02-12T23:05:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171152.35 epoch sec; seqNo:714; 4056157.56 north; 590407.68 east 2021-02-12T23:05:52Z- AHRS_M2 msg: 1613171152.73 epoch sec; seqNo:711 2021-02-12T23:05:52Z- AHRS_M2 msg: 0.71 phi; 0.31 theta; -0.14 psi 2021-02-12T23:05:52Z- handleDepth()-Depth_Keller msg: 1613171152754 epoch sec; seqNo:715; depth 16.41 2021-02-12T23:05:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171152.87 epoch sec; seqNo:715; 4056157.94 north; 590407.68 east 2021-02-12T23:05:53Z- AHRS_M2 msg: 1613171153.13 epoch sec; seqNo:712 2021-02-12T23:05:53Z- AHRS_M2 msg: 0.71 phi; 0.31 theta; -0.15 psi 2021-02-12T23:05:53Z- handleDepth()-Depth_Keller msg: 1613171153136 epoch sec; seqNo:716; depth 16.29 2021-02-12T23:05:53Z- handleDvl() - RDI_Pathfinder msg: 1613171153.15 epoch sec; seqNo:303 2021-02-12T23:05:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.01 , 92.32 , 97.19 , 68.69 2021-02-12T23:05:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.10 , -0.14 2021-02-12T23:05:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171153.17 epoch sec; seqNo:716; 4056157.94 north; 590407.68 east 2021-02-12T23:05:53Z- AHRS_M2 msg: 1613171153.53 epoch sec; seqNo:713 2021-02-12T23:05:53Z- AHRS_M2 msg: 0.71 phi; 0.31 theta; -0.16 psi 2021-02-12T23:05:53Z- handleDepth()-Depth_Keller msg: 1613171153538 epoch sec; seqNo:717; depth 16.21 2021-02-12T23:05:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171153.55 epoch sec; seqNo:717; 4056158.32 north; 590407.67 east 2021-02-12T23:05:53Z- AHRS_M2 msg: 1613171153.93 epoch sec; seqNo:714 2021-02-12T23:05:53Z- AHRS_M2 msg: 0.70 phi; 0.31 theta; -0.16 psi 2021-02-12T23:05:53Z- handleDepth()-Depth_Keller msg: 1613171153945 epoch sec; seqNo:718; depth 16.10 2021-02-12T23:05:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171153.96 epoch sec; seqNo:718; 4056158.32 north; 590407.67 east 2021-02-12T23:05:54Z- AHRS_M2 msg: 1613171154.34 epoch sec; seqNo:715 2021-02-12T23:05:54Z- AHRS_M2 msg: 0.69 phi; 0.31 theta; -0.17 psi 2021-02-12T23:05:54Z- handleDepth()-Depth_Keller msg: 1613171154344 epoch sec; seqNo:719; depth 15.98 2021-02-12T23:05:54Z- handleDvl() - RDI_Pathfinder msg: 1613171154.36 epoch sec; seqNo:304 2021-02-12T23:05:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.61 , 94.75 , 103.27 , 69.52 2021-02-12T23:05:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.20 , -0.03 2021-02-12T23:05:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171154.37 epoch sec; seqNo:719; 4056158.71 north; 590408.89 east 2021-02-12T23:05:54Z- AHRS_M2 msg: 1613171154.74 epoch sec; seqNo:716 2021-02-12T23:05:54Z- AHRS_M2 msg: 0.69 phi; 0.31 theta; -0.18 psi 2021-02-12T23:05:54Z- handleDepth()-Depth_Keller msg: 1613171154750 epoch sec; seqNo:720; depth 15.90 2021-02-12T23:05:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171154.76 epoch sec; seqNo:720; 4056159.09 north; 590408.89 east 2021-02-12T23:05:55Z- AHRS_M2 msg: 1613171155.15 epoch sec; seqNo:717 2021-02-12T23:05:55Z- AHRS_M2 msg: 0.68 phi; 0.31 theta; -0.18 psi 2021-02-12T23:05:55Z- handleDepth()-Depth_Keller msg: 1613171155154 epoch sec; seqNo:721; depth 15.79 2021-02-12T23:05:55Z- handleDvl() - RDI_Pathfinder msg: 1613171155.17 epoch sec; seqNo:305 2021-02-12T23:05:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.01 , 94.54 , 96.98 , 69.93 2021-02-12T23:05:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.21 , 0.12 2021-02-12T23:05:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171155.18 epoch sec; seqNo:721; 4056159.46 north; 590408.88 east 2021-02-12T23:05:55Z- AHRS_M2 msg: 1613171155.55 epoch sec; seqNo:718 2021-02-12T23:05:55Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.19 psi 2021-02-12T23:05:55Z- handleDepth()-Depth_Keller msg: 1613171155556 epoch sec; seqNo:722; depth 15.67 2021-02-12T23:05:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171155.57 epoch sec; seqNo:722; 4056159.46 north; 590408.88 east 2021-02-12T23:05:55Z- AHRS_M2 msg: 1613171155.96 epoch sec; seqNo:719 2021-02-12T23:05:55Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.19 psi 2021-02-12T23:05:55Z- handleDepth()-Depth_Keller msg: 1613171155969 epoch sec; seqNo:723; depth 15.57 2021-02-12T23:05:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171155.99 epoch sec; seqNo:723; 4056159.84 north; 590408.88 east 2021-02-12T23:05:56Z- AHRS_M2 msg: 1613171156.36 epoch sec; seqNo:720 2021-02-12T23:05:56Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.20 psi 2021-02-12T23:05:56Z- handleDepth()-Depth_Keller msg: 1613171156366 epoch sec; seqNo:724; depth 15.46 2021-02-12T23:05:56Z- handleDvl() - RDI_Pathfinder msg: 1613171156.38 epoch sec; seqNo:306 2021-02-12T23:05:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.60 , 94.68 , 105.64 , 70.33 2021-02-12T23:05:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.10 , 0.23 2021-02-12T23:05:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171156.39 epoch sec; seqNo:724; 4056160.22 north; 590408.87 east 2021-02-12T23:05:56Z- LcmTrn::handleMessages() - handling 44 messages took 0.75 ms and 129 calls... 2021-02-12T23:05:56Z- waiting for 1613171154.15, time is 1613171156.39 2021-02-12T23:05:56Z- LcmTrn::updateTrn() equating pose 1613171156.39 and meas times 1613171156.39 2021-02-12T23:05:56Z- LcmTrn::updateTrn() >>>> pose time 1613171156.39, meas time 1613171156.39 <<<< 2021-02-12T23:05:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171156.39 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:05:56Z- TerrainNav::motionUpdate() - currEstimate = (4056160, 590408.87, 15.46) 2021-02-12T23:05:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:05:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:05:56Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171156.39 sec.; No relevant navigation data available 2021-02-12T23:05:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:05:56Z- TNavBF::mmse selected from filter -1 2021-02-12T23:05:56Z- variances([0], [2], and psi_berg) = 818.578055 833.157717 0.000000 2021-02-12T23:05:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:05:56Z- LcmTrn::publishEstimates() - MLE : 4056123.72 590456.37 15.46 2021-02-12T23:05:56Z- LcmTrn::publishEstimates() - MMSE : 4056160.22 590408.87 15.46 2021-02-12T23:05:56Z- LcmTrn::publishEstimates() - COVAR: 818.58 833.16 1.14 0.00 2021-02-12T23:05:56Z- AHRS_M2 msg: 1613171156.76 epoch sec; seqNo:721 2021-02-12T23:05:56Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.20 psi 2021-02-12T23:05:56Z- handleDepth()-Depth_Keller msg: 1613171156768 epoch sec; seqNo:725; depth 15.36 2021-02-12T23:05:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171156.78 epoch sec; seqNo:725; 4056160.60 north; 590408.87 east 2021-02-12T23:05:57Z- AHRS_M2 msg: 1613171157.17 epoch sec; seqNo:722 2021-02-12T23:05:57Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.21 psi 2021-02-12T23:05:57Z- handleDepth()-Depth_Keller msg: 1613171157172 epoch sec; seqNo:726; depth 15.26 2021-02-12T23:05:57Z- handleDvl() - RDI_Pathfinder msg: 1613171157.18 epoch sec; seqNo:307 2021-02-12T23:05:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.01 , 96.92 , 105.44 , 70.33 2021-02-12T23:05:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.03 , 0.23 2021-02-12T23:05:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171157.19 epoch sec; seqNo:726; 4056160.61 north; 590410.09 east 2021-02-12T23:05:57Z- AHRS_M2 msg: 1613171157.57 epoch sec; seqNo:723 2021-02-12T23:05:57Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.21 psi 2021-02-12T23:05:57Z- handleDepth()-Depth_Keller msg: 1613171157578 epoch sec; seqNo:727; depth 15.15 2021-02-12T23:05:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171157.60 epoch sec; seqNo:727; 4056160.99 north; 590410.09 east 2021-02-12T23:05:57Z- AHRS_M2 msg: 1613171157.98 epoch sec; seqNo:724 2021-02-12T23:05:57Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.21 psi 2021-02-12T23:05:57Z- handleDepth()-Depth_Keller msg: 1613171157980 epoch sec; seqNo:728; depth 15.05 2021-02-12T23:05:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171158.00 epoch sec; seqNo:728; 4056161.37 north; 590410.08 east 2021-02-12T23:05:58Z- AHRS_M2 msg: 1613171158.38 epoch sec; seqNo:725 2021-02-12T23:05:58Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.22 psi 2021-02-12T23:05:58Z- handleDepth()-Depth_Keller msg: 1613171158390 epoch sec; seqNo:729; depth 14.97 2021-02-12T23:05:58Z- handleDvl() - RDI_Pathfinder msg: 1613171158.40 epoch sec; seqNo:308 2021-02-12T23:05:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.42 , 96.85 , 106.59 , 70.74 2021-02-12T23:05:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.13 , 0.14 2021-02-12T23:05:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171158.42 epoch sec; seqNo:729; 4056161.75 north; 590410.08 east 2021-02-12T23:05:58Z- AHRS_M2 msg: 1613171158.78 epoch sec; seqNo:726 2021-02-12T23:05:58Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.22 psi 2021-02-12T23:05:58Z- handleDepth()-Depth_Keller msg: 1613171158787 epoch sec; seqNo:730; depth 14.84 2021-02-12T23:05:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171158.80 epoch sec; seqNo:730; 4056161.75 north; 590410.08 east 2021-02-12T23:05:59Z- AHRS_M2 msg: 1613171159.19 epoch sec; seqNo:727 2021-02-12T23:05:59Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.22 psi 2021-02-12T23:05:59Z- handleDepth()-Depth_Keller msg: 1613171159193 epoch sec; seqNo:731; depth 14.75 2021-02-12T23:05:59Z- handleDvl() - RDI_Pathfinder msg: 1613171159.20 epoch sec; seqNo:309 2021-02-12T23:05:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.42 , 96.78 , 106.52 , 71.96 2021-02-12T23:05:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.15 , 0.03 2021-02-12T23:05:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171159.21 epoch sec; seqNo:731; 4056162.14 north; 590411.30 east 2021-02-12T23:05:59Z- AHRS_M2 msg: 1613171159.59 epoch sec; seqNo:728 2021-02-12T23:05:59Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.22 psi 2021-02-12T23:05:59Z- handleDepth()-Depth_Keller msg: 1613171159597 epoch sec; seqNo:732; depth 14.64 2021-02-12T23:05:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171159.62 epoch sec; seqNo:732; 4056162.52 north; 590411.29 east 2021-02-12T23:05:59Z- AHRS_M2 msg: 1613171159.99 epoch sec; seqNo:729 2021-02-12T23:05:59Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.21 psi 2021-02-12T23:06:00Z- handleDepth()-Depth_Keller msg: 1613171160005 epoch sec; seqNo:733; depth 14.53 2021-02-12T23:06:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171160.01 epoch sec; seqNo:733; 4056162.52 north; 590411.29 east 2021-02-12T23:06:00Z- AHRS_M2 msg: 1613171160.40 epoch sec; seqNo:730 2021-02-12T23:06:00Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.21 psi 2021-02-12T23:06:00Z- handleDepth()-Depth_Keller msg: 1613171160404 epoch sec; seqNo:734; depth 14.41 2021-02-12T23:06:00Z- handleDvl() - RDI_Pathfinder msg: 1613171160.42 epoch sec; seqNo:310 2021-02-12T23:06:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.24 , 96.78 , 107.74 , 71.15 2021-02-12T23:06:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.09 , -0.04 2021-02-12T23:06:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171160.43 epoch sec; seqNo:734; 4056162.90 north; 590411.29 east 2021-02-12T23:06:00Z- AHRS_M2 msg: 1613171160.81 epoch sec; seqNo:731 2021-02-12T23:06:00Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.21 psi 2021-02-12T23:06:00Z- handleDepth()-Depth_Keller msg: 1613171160810 epoch sec; seqNo:735; depth 14.30 2021-02-12T23:06:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171160.83 epoch sec; seqNo:735; 4056163.28 north; 590411.28 east 2021-02-12T23:06:01Z- AHRS_M2 msg: 1613171161.21 epoch sec; seqNo:732 2021-02-12T23:06:01Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.20 psi 2021-02-12T23:06:01Z- handleDepth()-Depth_Keller msg: 1613171161218 epoch sec; seqNo:736; depth 14.19 2021-02-12T23:06:01Z- handleDvl() - RDI_Pathfinder msg: 1613171161.24 epoch sec; seqNo:311 2021-02-12T23:06:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.05 , 96.78 , 110.18 , 71.96 2021-02-12T23:06:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.07 , -0.06 2021-02-12T23:06:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171161.25 epoch sec; seqNo:736; 4056163.28 north; 590411.28 east 2021-02-12T23:06:01Z- LcmTrn::handleMessages() - handling 41 messages took 0.65 ms and 128 calls... 2021-02-12T23:06:01Z- waiting for 1613171159.39, time is 1613171161.25 2021-02-12T23:06:01Z- LcmTrn::updateTrn() equating pose 1613171161.25 and meas times 1613171161.25 2021-02-12T23:06:01Z- LcmTrn::updateTrn() >>>> pose time 1613171161.25, meas time 1613171161.25 <<<< 2021-02-12T23:06:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171161.25 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:06:01Z- TerrainNav::motionUpdate() - currEstimate = (4056163, 590411.28, 14.19) 2021-02-12T23:06:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:06:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:06:01Z- TNavBF::Projecting Measurements 2021-02-12T23:06:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:06:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:06:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:06:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:06:01Z- TNavFilter::4 good beams. 2021-02-12T23:06:01Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:06:01Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:06:01Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:06:01Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:06:01Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:06:01Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:06:01Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:06:01Z- TNavBF::mmse selected from filter -1 2021-02-12T23:06:01Z- TerrainMapOctree: (vehN, vehE ) = (4056163.28, 590411.28). 2021-02-12T23:06:01Z- TerrainMapOctree: Min Distance = 6372.63. 2021-02-12T23:06:01Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:06:01Z- There is no specific good beam that all particles have in common. 2021-02-12T23:06:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:06:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.108 ExpectedMeasDiff: nan currMeas.covariance: 9.25 2021-02-12T23:06:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:06:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 14.99 2021-02-12T23:06:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:06:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 19.42 2021-02-12T23:06:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:06:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 8.29 2021-02-12T23:06:01Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:06:01Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:06:01Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:06:01Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:06:01Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:06:01Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:06:01Z- Filter 0 No beams used in subcloud update 2021-02-12T23:06:01Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:06:01Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:06:01Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 818.2 EastVariance: 833.2 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:06:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171161.25 sec. 2021-02-12T23:06:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:06:01Z- TNavBF::mmse selected from filter -1 2021-02-12T23:06:01Z- variances([0], [2], and psi_berg) = 818.217108 833.156231 0.000000 2021-02-12T23:06:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:06:01Z- LcmTrn::publishEstimates() - MLE : 4056126.78 590458.78 14.19 2021-02-12T23:06:01Z- LcmTrn::publishEstimates() - MMSE : 4056163.28 590411.28 14.19 2021-02-12T23:06:01Z- LcmTrn::publishEstimates() - COVAR: 818.22 833.16 0.95 0.00 2021-02-12T23:06:01Z- AHRS_M2 msg: 1613171161.62 epoch sec; seqNo:733 2021-02-12T23:06:01Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.20 psi 2021-02-12T23:06:01Z- handleDepth()-Depth_Keller msg: 1613171161621 epoch sec; seqNo:737; depth 14.08 2021-02-12T23:06:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171161.63 epoch sec; seqNo:737; 4056163.67 north; 590412.50 east 2021-02-12T23:06:02Z- AHRS_M2 msg: 1613171162.02 epoch sec; seqNo:734 2021-02-12T23:06:02Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.19 psi 2021-02-12T23:06:02Z- handleDepth()-Depth_Keller msg: 1613171162025 epoch sec; seqNo:738; depth 13.97 2021-02-12T23:06:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171162.04 epoch sec; seqNo:738; 4056164.05 north; 590412.50 east 2021-02-12T23:06:02Z- AHRS_M2 msg: 1613171162.42 epoch sec; seqNo:735 2021-02-12T23:06:02Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.18 psi 2021-02-12T23:06:02Z- handleDepth()-Depth_Keller msg: 1613171162429 epoch sec; seqNo:739; depth 13.86 2021-02-12T23:06:02Z- handleDvl() - RDI_Pathfinder msg: 1613171162.45 epoch sec; seqNo:312 2021-02-12T23:06:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.05 , 96.64 , 111.26 , 70.33 2021-02-12T23:06:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.06 , -0.10 2021-02-12T23:06:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171162.46 epoch sec; seqNo:739; 4056164.05 north; 590412.50 east 2021-02-12T23:06:02Z- AHRS_M2 msg: 1613171162.82 epoch sec; seqNo:736 2021-02-12T23:06:02Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.18 psi 2021-02-12T23:06:02Z- handleDepth()-Depth_Keller msg: 1613171162828 epoch sec; seqNo:740; depth 13.74 2021-02-12T23:06:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171162.84 epoch sec; seqNo:740; 4056164.43 north; 590412.49 east 2021-02-12T23:06:03Z- AHRS_M2 msg: 1613171163.23 epoch sec; seqNo:737 2021-02-12T23:06:03Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.17 psi 2021-02-12T23:06:03Z- handleDepth()-Depth_Keller msg: 1613171163236 epoch sec; seqNo:741; depth 13.63 2021-02-12T23:06:03Z- handleDvl() - RDI_Pathfinder msg: 1613171163.25 epoch sec; seqNo:313 2021-02-12T23:06:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.69 , 96.71 , 111.33 , 72.37 2021-02-12T23:06:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.12 2021-02-12T23:06:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171163.26 epoch sec; seqNo:741; 4056164.81 north; 590412.49 east 2021-02-12T23:06:03Z- AHRS_M2 msg: 1613171163.63 epoch sec; seqNo:738 2021-02-12T23:06:03Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.17 psi 2021-02-12T23:06:03Z- handleDepth()-Depth_Keller msg: 1613171163642 epoch sec; seqNo:742; depth 13.53 2021-02-12T23:06:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171163.65 epoch sec; seqNo:742; 4056164.81 north; 590412.49 east 2021-02-12T23:06:04Z- AHRS_M2 msg: 1613171164.05 epoch sec; seqNo:739 2021-02-12T23:06:04Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.16 psi 2021-02-12T23:06:04Z- handleDepth()-Depth_Keller msg: 1613171164058 epoch sec; seqNo:743; depth 13.41 2021-02-12T23:06:04Z- handleDvl() - RDI_Pathfinder msg: 1613171164.09 epoch sec; seqNo:314 2021-02-12T23:06:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.28 , 96.58 , 111.19 , 71.15 2021-02-12T23:06:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.13 2021-02-12T23:06:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171164.10 epoch sec; seqNo:743; 4056165.20 north; 590413.71 east 2021-02-12T23:06:04Z- AHRS_M2 msg: 1613171164.44 epoch sec; seqNo:740 2021-02-12T23:06:04Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.15 psi 2021-02-12T23:06:04Z- handleDepth()-Depth_Keller msg: 1613171164449 epoch sec; seqNo:744; depth 13.28 2021-02-12T23:06:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171164.46 epoch sec; seqNo:744; 4056165.58 north; 590413.70 east 2021-02-12T23:06:04Z- AHRS_M2 msg: 1613171164.84 epoch sec; seqNo:741 2021-02-12T23:06:04Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.14 psi 2021-02-12T23:06:04Z- handleDepth()-Depth_Keller msg: 1613171164848 epoch sec; seqNo:745; depth 13.17 2021-02-12T23:06:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171164.86 epoch sec; seqNo:745; 4056165.96 north; 590413.70 east 2021-02-12T23:06:05Z- AHRS_M2 msg: 1613171165.25 epoch sec; seqNo:742 2021-02-12T23:06:05Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.13 psi 2021-02-12T23:06:05Z- handleDepth()-Depth_Keller msg: 1613171165252 epoch sec; seqNo:746; depth 13.05 2021-02-12T23:06:05Z- handleDvl() - RDI_Pathfinder msg: 1613171165.26 epoch sec; seqNo:315 2021-02-12T23:06:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.32 , 96.64 , 112.48 , 71.56 2021-02-12T23:06:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.09 , -0.09 2021-02-12T23:06:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171165.29 epoch sec; seqNo:746; 4056165.96 north; 590413.70 east 2021-02-12T23:06:05Z- AHRS_M2 msg: 1613171165.66 epoch sec; seqNo:743 2021-02-12T23:06:05Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.13 psi 2021-02-12T23:06:05Z- handleDepth()-Depth_Keller msg: 1613171165664 epoch sec; seqNo:747; depth 12.93 2021-02-12T23:06:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171165.69 epoch sec; seqNo:747; 4056166.34 north; 590413.70 east 2021-02-12T23:06:06Z- AHRS_M2 msg: 1613171166.06 epoch sec; seqNo:744 2021-02-12T23:06:06Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.13 psi 2021-02-12T23:06:06Z- handleDepth()-Depth_Keller msg: 1613171166063 epoch sec; seqNo:748; depth 12.81 2021-02-12T23:06:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171166.07 epoch sec; seqNo:748; 4056166.34 north; 590413.70 east 2021-02-12T23:06:06Z- AHRS_M2 msg: 1613171166.46 epoch sec; seqNo:745 2021-02-12T23:06:06Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.13 psi 2021-02-12T23:06:06Z- handleDepth()-Depth_Keller msg: 1613171166464 epoch sec; seqNo:749; depth 12.70 2021-02-12T23:06:06Z- handleDvl() - RDI_Pathfinder msg: 1613171166.48 epoch sec; seqNo:316 2021-02-12T23:06:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 96.58 , 113.63 , 71.96 2021-02-12T23:06:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.16 , -0.05 2021-02-12T23:06:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171166.49 epoch sec; seqNo:749; 4056166.71 north; 590413.69 east 2021-02-12T23:06:06Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-12T23:06:06Z- waiting for 1613171164.25, time is 1613171166.49 2021-02-12T23:06:06Z- LcmTrn::updateTrn() equating pose 1613171166.49 and meas times 1613171166.49 2021-02-12T23:06:06Z- LcmTrn::updateTrn() >>>> pose time 1613171166.49, meas time 1613171166.49 <<<< 2021-02-12T23:06:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171166.49 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:06:06Z- TerrainNav::motionUpdate() - currEstimate = (4056167, 590413.69, 12.70) 2021-02-12T23:06:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:06:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:06:06Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171166.49 sec.; No relevant navigation data available 2021-02-12T23:06:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:06:06Z- TNavBF::mmse selected from filter -1 2021-02-12T23:06:06Z- variances([0], [2], and psi_berg) = 817.846421 833.154729 0.000000 2021-02-12T23:06:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:06:06Z- LcmTrn::publishEstimates() - MLE : 4056131.21 590364.19 12.70 2021-02-12T23:06:06Z- LcmTrn::publishEstimates() - MMSE : 4056166.71 590413.69 12.70 2021-02-12T23:06:06Z- LcmTrn::publishEstimates() - COVAR: 817.85 833.15 1.17 0.00 2021-02-12T23:06:06Z- AHRS_M2 msg: 1613171166.86 epoch sec; seqNo:746 2021-02-12T23:06:06Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.14 psi 2021-02-12T23:06:06Z- handleDepth()-Depth_Keller msg: 1613171166873 epoch sec; seqNo:750; depth 12.58 2021-02-12T23:06:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171166.88 epoch sec; seqNo:750; 4056167.09 north; 590413.69 east 2021-02-12T23:06:07Z- AHRS_M2 msg: 1613171167.27 epoch sec; seqNo:747 2021-02-12T23:06:07Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.15 psi 2021-02-12T23:06:07Z- handleDepth()-Depth_Keller msg: 1613171167277 epoch sec; seqNo:751; depth 12.46 2021-02-12T23:06:07Z- handleDvl() - RDI_Pathfinder msg: 1613171167.29 epoch sec; seqNo:317 2021-02-12T23:06:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.05 , 96.44 , 112.27 , 72.78 2021-02-12T23:06:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.21 , 0.00 2021-02-12T23:06:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171167.30 epoch sec; seqNo:751; 4056167.47 north; 590413.68 east 2021-02-12T23:06:07Z- AHRS_M2 msg: 1613171167.67 epoch sec; seqNo:748 2021-02-12T23:06:07Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.17 psi 2021-02-12T23:06:07Z- handleDepth()-Depth_Keller msg: 1613171167679 epoch sec; seqNo:752; depth 12.34 2021-02-12T23:06:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171167.69 epoch sec; seqNo:752; 4056167.48 north; 590414.90 east 2021-02-12T23:06:08Z- AHRS_M2 msg: 1613171168.10 epoch sec; seqNo:749 2021-02-12T23:06:08Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.18 psi 2021-02-12T23:06:08Z- handleDepth()-Depth_Keller msg: 1613171168125 epoch sec; seqNo:753; depth 12.21 2021-02-12T23:06:08Z- handleDvl() - RDI_Pathfinder msg: 1613171168.15 epoch sec; seqNo:318 2021-02-12T23:06:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.32 , 96.44 , 113.48 , 71.96 2021-02-12T23:06:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.24 , 0.08 2021-02-12T23:06:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171168.17 epoch sec; seqNo:753; 4056167.86 north; 590414.90 east 2021-02-12T23:06:08Z- AHRS_M2 msg: 1613171168.48 epoch sec; seqNo:750 2021-02-12T23:06:08Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.19 psi 2021-02-12T23:06:08Z- handleDepth()-Depth_Keller msg: 1613171168489 epoch sec; seqNo:754; depth 12.06 2021-02-12T23:06:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171168.50 epoch sec; seqNo:754; 4056168.24 north; 590414.90 east 2021-02-12T23:06:08Z- AHRS_M2 msg: 1613171168.88 epoch sec; seqNo:751 2021-02-12T23:06:08Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.19 psi 2021-02-12T23:06:08Z- handleDepth()-Depth_Keller msg: 1613171168890 epoch sec; seqNo:755; depth 11.98 2021-02-12T23:06:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171168.90 epoch sec; seqNo:755; 4056168.62 north; 590414.89 east 2021-02-12T23:06:09Z- AHRS_M2 msg: 1613171169.29 epoch sec; seqNo:752 2021-02-12T23:06:09Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.19 psi 2021-02-12T23:06:09Z- handleDepth()-Depth_Keller msg: 1613171169309 epoch sec; seqNo:756; depth 11.86 2021-02-12T23:06:09Z- handleDvl() - RDI_Pathfinder msg: 1613171169.32 epoch sec; seqNo:319 2021-02-12T23:06:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.32 , 96.50 , 114.77 , 72.77 2021-02-12T23:06:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.16 , 0.18 2021-02-12T23:06:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171169.33 epoch sec; seqNo:756; 4056169.00 north; 590414.89 east 2021-02-12T23:06:09Z- AHRS_M2 msg: 1613171169.69 epoch sec; seqNo:753 2021-02-12T23:06:09Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.19 psi 2021-02-12T23:06:09Z- handleDepth()-Depth_Keller msg: 1613171169696 epoch sec; seqNo:757; depth 11.74 2021-02-12T23:06:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171169.71 epoch sec; seqNo:757; 4056169.00 north; 590414.89 east 2021-02-12T23:06:10Z- AHRS_M2 msg: 1613171170.10 epoch sec; seqNo:754 2021-02-12T23:06:10Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.18 psi 2021-02-12T23:06:10Z- handleDepth()-Depth_Keller msg: 1613171170100 epoch sec; seqNo:758; depth 11.62 2021-02-12T23:06:10Z- handleDvl() - RDI_Pathfinder msg: 1613171170.11 epoch sec; seqNo:320 2021-02-12T23:06:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.32 , 96.50 , 113.55 , 73.18 2021-02-12T23:06:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.19 2021-02-12T23:06:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171170.13 epoch sec; seqNo:758; 4056169.38 north; 590414.88 east 2021-02-12T23:06:10Z- AHRS_M2 msg: 1613171170.50 epoch sec; seqNo:755 2021-02-12T23:06:10Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.17 psi 2021-02-12T23:06:10Z- handleDepth()-Depth_Keller msg: 1613171170506 epoch sec; seqNo:759; depth 11.50 2021-02-12T23:06:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171170.52 epoch sec; seqNo:759; 4056169.76 north; 590414.88 east 2021-02-12T23:06:10Z- AHRS_M2 msg: 1613171170.90 epoch sec; seqNo:756 2021-02-12T23:06:10Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.16 psi 2021-02-12T23:06:10Z- handleDepth()-Depth_Keller msg: 1613171170918 epoch sec; seqNo:760; depth 11.39 2021-02-12T23:06:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171170.93 epoch sec; seqNo:760; 4056170.15 north; 590416.10 east 2021-02-12T23:06:11Z- AHRS_M2 msg: 1613171171.31 epoch sec; seqNo:757 2021-02-12T23:06:11Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.14 psi 2021-02-12T23:06:11Z- handleDepth()-Depth_Keller msg: 1613171171312 epoch sec; seqNo:761; depth 11.27 2021-02-12T23:06:11Z- handleDvl() - RDI_Pathfinder msg: 1613171171.32 epoch sec; seqNo:321 2021-02-12T23:06:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 96.50 , 114.77 , 74.00 2021-02-12T23:06:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.10 , 0.10 2021-02-12T23:06:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171171.35 epoch sec; seqNo:761; 4056170.15 north; 590416.10 east 2021-02-12T23:06:11Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-12T23:06:11Z- waiting for 1613171169.49, time is 1613171171.35 2021-02-12T23:06:11Z- LcmTrn::updateTrn() equating pose 1613171171.35 and meas times 1613171171.35 2021-02-12T23:06:11Z- LcmTrn::updateTrn() >>>> pose time 1613171171.35, meas time 1613171171.35 <<<< 2021-02-12T23:06:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171171.35 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:06:11Z- TerrainNav::motionUpdate() - currEstimate = (4056170, 590416.10, 11.27) 2021-02-12T23:06:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:06:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:06:11Z- TNavBF::Projecting Measurements 2021-02-12T23:06:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:06:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:06:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:06:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:06:11Z- TNavFilter::4 good beams. 2021-02-12T23:06:11Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:06:11Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:06:11Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:06:11Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:06:11Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:06:11Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:06:11Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:06:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:06:11Z- TerrainMapOctree: (vehN, vehE ) = (4056170.15, 590416.10). 2021-02-12T23:06:11Z- TerrainMapOctree: Min Distance = 6364.25. 2021-02-12T23:06:11Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:06:11Z- There is no specific good beam that all particles have in common. 2021-02-12T23:06:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:06:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 10.73 2021-02-12T23:06:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:06:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 14.90 2021-02-12T23:06:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:06:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 21.08 2021-02-12T23:06:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:06:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 8.76 2021-02-12T23:06:11Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:06:11Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:06:11Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:06:11Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:06:11Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:06:11Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:06:11Z- Filter 0 No beams used in subcloud update 2021-02-12T23:06:11Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:06:11Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:06:11Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 817.5 EastVariance: 833.2 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:06:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171171.35 sec. 2021-02-12T23:06:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:06:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:06:11Z- variances([0], [2], and psi_berg) = 817.479772 833.153270 0.000000 2021-02-12T23:06:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:06:11Z- LcmTrn::publishEstimates() - MLE : 4056134.65 590367.60 11.27 2021-02-12T23:06:11Z- LcmTrn::publishEstimates() - MMSE : 4056170.15 590416.10 11.27 2021-02-12T23:06:11Z- LcmTrn::publishEstimates() - COVAR: 817.48 833.15 1.00 0.00 2021-02-12T23:06:11Z- AHRS_M2 msg: 1613171171.71 epoch sec; seqNo:758 2021-02-12T23:06:11Z- AHRS_M2 msg: 0.62 phi; 0.38 theta; -0.13 psi 2021-02-12T23:06:11Z- handleDepth()-Depth_Keller msg: 1613171171716 epoch sec; seqNo:762; depth 11.15 2021-02-12T23:06:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171171.73 epoch sec; seqNo:762; 4056170.53 north; 590416.09 east 2021-02-12T23:06:12Z- AHRS_M2 msg: 1613171172.14 epoch sec; seqNo:759 2021-02-12T23:06:12Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.12 psi 2021-02-12T23:06:12Z- handleDepth()-Depth_Keller msg: 1613171172149 epoch sec; seqNo:763; depth 11.03 2021-02-12T23:06:12Z- handleDvl() - RDI_Pathfinder msg: 1613171172.16 epoch sec; seqNo:322 2021-02-12T23:06:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.70 , 96.44 , 114.71 , 72.78 2021-02-12T23:06:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.10 , -0.04 2021-02-12T23:06:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171172.18 epoch sec; seqNo:763; 4056170.91 north; 590416.09 east 2021-02-12T23:06:12Z- AHRS_M2 msg: 1613171172.52 epoch sec; seqNo:760 2021-02-12T23:06:12Z- AHRS_M2 msg: 0.65 phi; 0.37 theta; -0.12 psi 2021-02-12T23:06:12Z- handleDepth()-Depth_Keller msg: 1613171172529 epoch sec; seqNo:764; depth 10.91 2021-02-12T23:06:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171172.55 epoch sec; seqNo:764; 4056170.91 north; 590416.09 east 2021-02-12T23:06:12Z- AHRS_M2 msg: 1613171172.92 epoch sec; seqNo:761 2021-02-12T23:06:12Z- AHRS_M2 msg: 0.67 phi; 0.37 theta; -0.11 psi 2021-02-12T23:06:12Z- handleDepth()-Depth_Keller msg: 1613171172928 epoch sec; seqNo:765; depth 10.81 2021-02-12T23:06:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171172.94 epoch sec; seqNo:765; 4056171.29 north; 590416.09 east 2021-02-12T23:06:13Z- AHRS_M2 msg: 1613171173.33 epoch sec; seqNo:762 2021-02-12T23:06:13Z- AHRS_M2 msg: 0.67 phi; 0.36 theta; -0.10 psi 2021-02-12T23:06:13Z- handleDepth()-Depth_Keller msg: 1613171173339 epoch sec; seqNo:766; depth 10.69 2021-02-12T23:06:13Z- handleDvl() - RDI_Pathfinder msg: 1613171173.35 epoch sec; seqNo:323 2021-02-12T23:06:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.78 , 96.31 , 115.25 , 73.60 2021-02-12T23:06:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.03 , -0.10 2021-02-12T23:06:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171173.36 epoch sec; seqNo:766; 4056171.30 north; 590417.31 east 2021-02-12T23:06:13Z- AHRS_M2 msg: 1613171173.74 epoch sec; seqNo:763 2021-02-12T23:06:13Z- AHRS_M2 msg: 0.67 phi; 0.36 theta; -0.10 psi 2021-02-12T23:06:13Z- handleDepth()-Depth_Keller msg: 1613171173743 epoch sec; seqNo:767; depth 10.56 2021-02-12T23:06:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171173.75 epoch sec; seqNo:767; 4056171.68 north; 590417.30 east 2021-02-12T23:06:14Z- AHRS_M2 msg: 1613171174.14 epoch sec; seqNo:764 2021-02-12T23:06:14Z- AHRS_M2 msg: 0.67 phi; 0.36 theta; -0.10 psi 2021-02-12T23:06:14Z- handleDepth()-Depth_Keller msg: 1613171174158 epoch sec; seqNo:768; depth 10.43 2021-02-12T23:06:14Z- handleDvl() - RDI_Pathfinder msg: 1613171174.20 epoch sec; seqNo:324 2021-02-12T23:06:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 96.37 , 115.31 , 78.78 2021-02-12T23:06:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.12 , -0.03 2021-02-12T23:06:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171174.21 epoch sec; seqNo:768; 4056172.06 north; 590417.30 east 2021-02-12T23:06:14Z- AHRS_M2 msg: 1613171174.54 epoch sec; seqNo:765 2021-02-12T23:06:14Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.11 psi 2021-02-12T23:06:14Z- handleDepth()-Depth_Keller msg: 1613171174548 epoch sec; seqNo:769; depth 10.28 2021-02-12T23:06:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171174.56 epoch sec; seqNo:769; 4056172.06 north; 590417.30 east 2021-02-12T23:06:14Z- AHRS_M2 msg: 1613171174.95 epoch sec; seqNo:766 2021-02-12T23:06:14Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.12 psi 2021-02-12T23:06:14Z- handleDepth()-Depth_Keller msg: 1613171174954 epoch sec; seqNo:770; depth 10.17 2021-02-12T23:06:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171174.96 epoch sec; seqNo:770; 4056172.44 north; 590417.29 east 2021-02-12T23:06:15Z- AHRS_M2 msg: 1613171175.35 epoch sec; seqNo:767 2021-02-12T23:06:15Z- AHRS_M2 msg: 0.65 phi; 0.38 theta; -0.13 psi 2021-02-12T23:06:15Z- handleDepth()-Depth_Keller msg: 1613171175356 epoch sec; seqNo:771; depth 10.05 2021-02-12T23:06:15Z- handleDvl() - RDI_Pathfinder msg: 1613171175.38 epoch sec; seqNo:325 2021-02-12T23:06:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 96.23 , 117.88 , 74.41 2021-02-12T23:06:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.09 , 0.07 2021-02-12T23:06:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171175.39 epoch sec; seqNo:771; 4056172.81 north; 590417.29 east 2021-02-12T23:06:15Z- AHRS_M2 msg: 1613171175.76 epoch sec; seqNo:768 2021-02-12T23:06:15Z- AHRS_M2 msg: 0.65 phi; 0.38 theta; -0.14 psi 2021-02-12T23:06:15Z- handleDepth()-Depth_Keller msg: 1613171175765 epoch sec; seqNo:772; depth 9.91 2021-02-12T23:06:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171175.78 epoch sec; seqNo:772; 4056173.19 north; 590417.29 east 2021-02-12T23:06:16Z- AHRS_M2 msg: 1613171176.16 epoch sec; seqNo:769 2021-02-12T23:06:16Z- AHRS_M2 msg: 0.65 phi; 0.39 theta; -0.14 psi 2021-02-12T23:06:16Z- handleDepth()-Depth_Keller msg: 1613171176166 epoch sec; seqNo:773; depth 9.79 2021-02-12T23:06:16Z- handleDvl() - RDI_Pathfinder msg: 1613171176.18 epoch sec; seqNo:326 2021-02-12T23:06:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.13 , 96.30 , 117.95 , 74.41 2021-02-12T23:06:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , 0.07 2021-02-12T23:06:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171176.19 epoch sec; seqNo:773; 4056173.21 north; 590418.51 east 2021-02-12T23:06:16Z- AHRS_M2 msg: 1613171176.56 epoch sec; seqNo:770 2021-02-12T23:06:16Z- AHRS_M2 msg: 0.66 phi; 0.39 theta; -0.12 psi 2021-02-12T23:06:16Z- handleDepth()-Depth_Keller msg: 1613171176568 epoch sec; seqNo:774; depth 9.66 2021-02-12T23:06:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171176.58 epoch sec; seqNo:774; 4056173.58 north; 590418.50 east 2021-02-12T23:06:16Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 130 calls... 2021-02-12T23:06:16Z- waiting for 1613171174.35, time is 1613171176.58 2021-02-12T23:06:16Z- LcmTrn::updateTrn() >>>> pose time 1613171176.58, meas time 1613171176.18 <<<< 2021-02-12T23:06:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171176.18 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:06:16Z- TerrainNav::motionUpdate() - currEstimate = (4056174, 590418.50, 9.66) 2021-02-12T23:06:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:06:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:06:16Z- TNavBF::Projecting Measurements 2021-02-12T23:06:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:06:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:06:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:06:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:06:16Z- TNavFilter::4 good beams. 2021-02-12T23:06:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:06:16Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:06:16Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:06:16Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:06:16Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:06:16Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:06:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:06:16Z- TNavBF::mmse selected from filter -1 2021-02-12T23:06:16Z- TerrainMapOctree: (vehN, vehE ) = (4056173.32, 590418.32). 2021-02-12T23:06:16Z- TerrainMapOctree: Min Distance = 6360.38. 2021-02-12T23:06:16Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:06:16Z- There is no specific good beam that all particles have in common. 2021-02-12T23:06:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:06:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 11.32 2021-02-12T23:06:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:06:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 14.84 2021-02-12T23:06:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:06:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 22.26 2021-02-12T23:06:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:06:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 8.86 2021-02-12T23:06:16Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:06:16Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:06:16Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:06:16Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:06:16Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:06:16Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:06:16Z- Filter 0 No beams used in subcloud update 2021-02-12T23:06:16Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:06:16Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:06:16Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 817.1 EastVariance: 833.2 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:06:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171176.18 sec. 2021-02-12T23:06:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:06:16Z- TNavBF::mmse selected from filter -1 2021-02-12T23:06:16Z- variances([0], [2], and psi_berg) = 817.031837 833.151522 0.000000 2021-02-12T23:06:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:06:16Z- LcmTrn::publishEstimates() - MLE : 4056138.08 590370.00 9.66 2021-02-12T23:06:16Z- LcmTrn::publishEstimates() - MMSE : 4056173.58 590418.50 9.66 2021-02-12T23:06:16Z- LcmTrn::publishEstimates() - COVAR: 817.03 833.15 0.92 0.00 2021-02-12T23:06:16Z- AHRS_M2 msg: 1613171176.97 epoch sec; seqNo:771 2021-02-12T23:06:16Z- AHRS_M2 msg: 0.68 phi; 0.39 theta; -0.11 psi 2021-02-12T23:06:16Z- handleDepth()-Depth_Keller msg: 1613171176972 epoch sec; seqNo:775; depth 9.53 2021-02-12T23:06:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171176.98 epoch sec; seqNo:775; 4056173.96 north; 590418.50 east 2021-02-12T23:06:17Z- AHRS_M2 msg: 1613171177.38 epoch sec; seqNo:772 2021-02-12T23:06:17Z- AHRS_M2 msg: 0.69 phi; 0.39 theta; -0.09 psi 2021-02-12T23:06:17Z- handleDepth()-Depth_Keller msg: 1613171177382 epoch sec; seqNo:776; depth 9.41 2021-02-12T23:06:17Z- handleDvl() - RDI_Pathfinder msg: 1613171177.40 epoch sec; seqNo:327 2021-02-12T23:06:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.18 , 96.30 , 120.65 , 75.23 2021-02-12T23:06:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.00 2021-02-12T23:06:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171177.41 epoch sec; seqNo:776; 4056174.34 north; 590418.50 east 2021-02-12T23:06:17Z- AHRS_M2 msg: 1613171177.78 epoch sec; seqNo:773 2021-02-12T23:06:17Z- AHRS_M2 msg: 0.70 phi; 0.39 theta; -0.09 psi 2021-02-12T23:06:17Z- handleDepth()-Depth_Keller msg: 1613171177780 epoch sec; seqNo:777; depth 9.26 2021-02-12T23:06:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171177.81 epoch sec; seqNo:777; 4056174.34 north; 590418.50 east 2021-02-12T23:06:18Z- AHRS_M2 msg: 1613171178.19 epoch sec; seqNo:774 2021-02-12T23:06:18Z- AHRS_M2 msg: 0.72 phi; 0.37 theta; -0.09 psi 2021-02-12T23:06:18Z- handleDepth()-Depth_Keller msg: 1613171178190 epoch sec; seqNo:778; depth 9.13 2021-02-12T23:06:18Z- handleDvl() - RDI_Pathfinder msg: 1613171178.21 epoch sec; seqNo:328 2021-02-12T23:06:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.33 , 96.10 , 119.10 , 74.82 2021-02-12T23:06:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , -0.02 2021-02-12T23:06:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171178.22 epoch sec; seqNo:778; 4056174.72 north; 590418.49 east 2021-02-12T23:06:18Z- AHRS_M2 msg: 1613171178.59 epoch sec; seqNo:775 2021-02-12T23:06:18Z- AHRS_M2 msg: 0.72 phi; 0.36 theta; -0.10 psi 2021-02-12T23:06:18Z- handleDepth()-Depth_Keller msg: 1613171178593 epoch sec; seqNo:779; depth 9.01 2021-02-12T23:06:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171178.60 epoch sec; seqNo:779; 4056175.11 north; 590419.71 east 2021-02-12T23:06:18Z- AHRS_M2 msg: 1613171178.99 epoch sec; seqNo:776 2021-02-12T23:06:18Z- AHRS_M2 msg: 0.72 phi; 0.36 theta; -0.11 psi 2021-02-12T23:06:18Z- handleDepth()-Depth_Keller msg: 1613171178993 epoch sec; seqNo:780; depth 8.88 2021-02-12T23:06:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171179.00 epoch sec; seqNo:780; 4056175.11 north; 590419.71 east 2021-02-12T23:06:19Z- AHRS_M2 msg: 1613171179.39 epoch sec; seqNo:777 2021-02-12T23:06:19Z- AHRS_M2 msg: 0.72 phi; 0.35 theta; -0.13 psi 2021-02-12T23:06:19Z- handleDepth()-Depth_Keller msg: 1613171179400 epoch sec; seqNo:781; depth 8.75 2021-02-12T23:06:19Z- handleDvl() - RDI_Pathfinder msg: 1613171179.42 epoch sec; seqNo:329 2021-02-12T23:06:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.40 , 96.17 , 117.81 , 75.23 2021-02-12T23:06:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.06 , -0.03 2021-02-12T23:06:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171179.43 epoch sec; seqNo:781; 4056175.49 north; 590419.70 east 2021-02-12T23:06:19Z- AHRS_M2 msg: 1613171179.80 epoch sec; seqNo:778 2021-02-12T23:06:19Z- AHRS_M2 msg: 0.71 phi; 0.34 theta; -0.14 psi 2021-02-12T23:06:19Z- handleDepth()-Depth_Keller msg: 1613171179804 epoch sec; seqNo:782; depth 8.62 2021-02-12T23:06:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171179.84 epoch sec; seqNo:782; 4056175.49 north; 590419.70 east 2021-02-12T23:06:20Z- AHRS_M2 msg: 1613171180.20 epoch sec; seqNo:779 2021-02-12T23:06:20Z- AHRS_M2 msg: 0.71 phi; 0.34 theta; -0.16 psi 2021-02-12T23:06:20Z- handleDepth()-Depth_Keller msg: 1613171180209 epoch sec; seqNo:783; depth 8.50 2021-02-12T23:06:20Z- handleDvl() - RDI_Pathfinder msg: 1613171180.22 epoch sec; seqNo:330 2021-02-12T23:06:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 96.17 , 116.46 , 76.45 2021-02-12T23:06:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.10 , -0.06 2021-02-12T23:06:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171180.23 epoch sec; seqNo:783; 4056175.87 north; 590419.70 east 2021-02-12T23:06:20Z- AHRS_M2 msg: 1613171180.61 epoch sec; seqNo:780 2021-02-12T23:06:20Z- AHRS_M2 msg: 0.70 phi; 0.34 theta; -0.18 psi 2021-02-12T23:06:20Z- handleDepth()-Depth_Keller msg: 1613171180610 epoch sec; seqNo:784; depth 8.38 2021-02-12T23:06:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171180.62 epoch sec; seqNo:784; 4056176.25 north; 590419.70 east 2021-02-12T23:06:21Z- AHRS_M2 msg: 1613171181.01 epoch sec; seqNo:781 2021-02-12T23:06:21Z- AHRS_M2 msg: 0.70 phi; 0.33 theta; -0.19 psi 2021-02-12T23:06:21Z- handleDepth()-Depth_Keller msg: 1613171181012 epoch sec; seqNo:785; depth 8.26 2021-02-12T23:06:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171181.03 epoch sec; seqNo:785; 4056176.25 north; 590419.70 east 2021-02-12T23:06:21Z- AHRS_M2 msg: 1613171181.41 epoch sec; seqNo:782 2021-02-12T23:06:21Z- AHRS_M2 msg: 0.69 phi; 0.33 theta; -0.21 psi 2021-02-12T23:06:21Z- handleDepth()-Depth_Keller msg: 1613171181421 epoch sec; seqNo:786; depth 8.14 2021-02-12T23:06:21Z- handleDvl() - RDI_Pathfinder msg: 1613171181.44 epoch sec; seqNo:331 2021-02-12T23:06:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.71 , 96.23 , 113.82 , 76.45 2021-02-12T23:06:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.15 , -0.03 2021-02-12T23:06:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171181.45 epoch sec; seqNo:786; 4056176.63 north; 590419.69 east 2021-02-12T23:06:21Z- AHRS_M2 msg: 1613171181.82 epoch sec; seqNo:783 2021-02-12T23:06:21Z- AHRS_M2 msg: 0.69 phi; 0.34 theta; -0.21 psi 2021-02-12T23:06:21Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-12T23:06:21Z- waiting for 1613171179.58, time is 1613171181.82 2021-02-12T23:06:21Z- LcmTrn::updateTrn() >>>> pose time 1613171181.82, meas time 1613171181.44 <<<< 2021-02-12T23:06:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171181.44 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:06:21Z- TerrainNav::motionUpdate() - currEstimate = (4056177, 590419.69, 8.14) 2021-02-12T23:06:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:06:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:06:21Z- TNavBF::Projecting Measurements 2021-02-12T23:06:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:06:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:06:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:06:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:06:21Z- TNavFilter::4 good beams. 2021-02-12T23:06:21Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:06:21Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:06:21Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:06:21Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:06:21Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:06:21Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:06:21Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:06:21Z- TNavBF::mmse selected from filter -1 2021-02-12T23:06:21Z- TerrainMapOctree: (vehN, vehE ) = (4056176.41, 590419.61). 2021-02-12T23:06:21Z- TerrainMapOctree: Min Distance = 6357.14. 2021-02-12T23:06:21Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:06:21Z- There is no specific good beam that all particles have in common. 2021-02-12T23:06:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:06:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 10.94 2021-02-12T23:06:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:06:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 14.82 2021-02-12T23:06:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:06:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 20.73 2021-02-12T23:06:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:06:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 9.35 2021-02-12T23:06:21Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:06:21Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:06:21Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:06:21Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:06:21Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:06:21Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:06:21Z- Filter 0 No beams used in subcloud update 2021-02-12T23:06:21Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:06:21Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:06:21Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 816.7 EastVariance: 833.2 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:06:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171181.44 sec. 2021-02-12T23:06:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:06:21Z- TNavBF::mmse selected from filter -1 2021-02-12T23:06:21Z- variances([0], [2], and psi_berg) = 816.643424 833.150036 0.000000 2021-02-12T23:06:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:06:21Z- LcmTrn::publishEstimates() - MLE : 4056141.13 590372.19 8.14 2021-02-12T23:06:21Z- LcmTrn::publishEstimates() - MMSE : 4056176.63 590419.69 8.14 2021-02-12T23:06:21Z- LcmTrn::publishEstimates() - COVAR: 816.64 833.15 0.84 0.00 2021-02-12T23:06:21Z- handleDepth()-Depth_Keller msg: 1613171181829 epoch sec; seqNo:787; depth 8.03 2021-02-12T23:06:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171181.85 epoch sec; seqNo:787; 4056177.02 north; 590420.91 east 2021-02-12T23:06:22Z- AHRS_M2 msg: 1613171182.22 epoch sec; seqNo:784 2021-02-12T23:06:22Z- AHRS_M2 msg: 0.68 phi; 0.34 theta; -0.20 psi 2021-02-12T23:06:22Z- handleDepth()-Depth_Keller msg: 1613171182226 epoch sec; seqNo:788; depth 7.91 2021-02-12T23:06:22Z- handleDvl() - RDI_Pathfinder msg: 1613171182.24 epoch sec; seqNo:332 2021-02-12T23:06:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 105.84 , 115.31 , 77.27 2021-02-12T23:06:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.09 , 0.11 2021-02-12T23:06:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171182.25 epoch sec; seqNo:788; 4056177.02 north; 590420.91 east 2021-02-12T23:06:22Z- AHRS_M2 msg: 1613171182.64 epoch sec; seqNo:785 2021-02-12T23:06:22Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.19 psi 2021-02-12T23:06:22Z- handleDepth()-Depth_Keller msg: 1613171182649 epoch sec; seqNo:789; depth 7.81 2021-02-12T23:06:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171182.66 epoch sec; seqNo:789; 4056177.40 north; 590420.91 east 2021-02-12T23:06:23Z- AHRS_M2 msg: 1613171183.03 epoch sec; seqNo:786 2021-02-12T23:06:23Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.19 psi 2021-02-12T23:06:23Z- handleDepth()-Depth_Keller msg: 1613171183032 epoch sec; seqNo:790; depth 7.72 2021-02-12T23:06:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171183.04 epoch sec; seqNo:790; 4056177.78 north; 590420.90 east 2021-02-12T23:06:23Z- AHRS_M2 msg: 1613171183.43 epoch sec; seqNo:787 2021-02-12T23:06:23Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.19 psi 2021-02-12T23:06:23Z- handleDepth()-Depth_Keller msg: 1613171183436 epoch sec; seqNo:791; depth 7.62 2021-02-12T23:06:23Z- handleDvl() - RDI_Pathfinder msg: 1613171183.45 epoch sec; seqNo:333 2021-02-12T23:06:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.57 , 105.57 , 116.39 , 76.86 2021-02-12T23:06:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.09 , 0.23 2021-02-12T23:06:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171183.46 epoch sec; seqNo:791; 4056177.78 north; 590420.90 east 2021-02-12T23:06:23Z- AHRS_M2 msg: 1613171183.84 epoch sec; seqNo:788 2021-02-12T23:06:23Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.20 psi 2021-02-12T23:06:23Z- handleDepth()-Depth_Keller msg: 1613171183842 epoch sec; seqNo:792; depth 7.51 2021-02-12T23:06:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171183.85 epoch sec; seqNo:792; 4056178.16 north; 590420.90 east 2021-02-12T23:06:24Z- AHRS_M2 msg: 1613171184.24 epoch sec; seqNo:789 2021-02-12T23:06:24Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.19 psi 2021-02-12T23:06:24Z- handleDepth()-Depth_Keller msg: 1613171184258 epoch sec; seqNo:793; depth 7.39 2021-02-12T23:06:24Z- handleDvl() - RDI_Pathfinder msg: 1613171184.28 epoch sec; seqNo:334 2021-02-12T23:06:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 105.57 , 119.10 , 77.27 2021-02-12T23:06:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.30 , 0.14 2021-02-12T23:06:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171184.32 epoch sec; seqNo:793; 4056178.55 north; 590422.11 east 2021-02-12T23:06:24Z- AHRS_M2 msg: 1613171184.64 epoch sec; seqNo:790 2021-02-12T23:06:24Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.19 psi 2021-02-12T23:06:24Z- handleDepth()-Depth_Keller msg: 1613171184648 epoch sec; seqNo:794; depth 7.26 2021-02-12T23:06:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171184.66 epoch sec; seqNo:794; 4056178.55 north; 590422.11 east 2021-02-12T23:06:25Z- AHRS_M2 msg: 1613171185.05 epoch sec; seqNo:791 2021-02-12T23:06:25Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.19 psi 2021-02-12T23:06:25Z- handleDepth()-Depth_Keller msg: 1613171185053 epoch sec; seqNo:795; depth 7.18 2021-02-12T23:06:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171185.06 epoch sec; seqNo:795; 4056178.93 north; 590422.11 east 2021-02-12T23:06:25Z- AHRS_M2 msg: 1613171185.46 epoch sec; seqNo:792 2021-02-12T23:06:25Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.19 psi 2021-02-12T23:06:25Z- handleDepth()-Depth_Keller msg: 1613171185464 epoch sec; seqNo:796; depth 7.07 2021-02-12T23:06:25Z- handleDvl() - RDI_Pathfinder msg: 1613171185.48 epoch sec; seqNo:335 2021-02-12T23:06:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.21 , 106.86 , 119.03 , 77.27 2021-02-12T23:06:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , 0.25 , -0.18 2021-02-12T23:06:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171185.49 epoch sec; seqNo:796; 4056178.93 north; 590422.11 east 2021-02-12T23:06:25Z- AHRS_M2 msg: 1613171185.86 epoch sec; seqNo:793 2021-02-12T23:06:25Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.21 psi 2021-02-12T23:06:25Z- handleDepth()-Depth_Keller msg: 1613171185860 epoch sec; seqNo:797; depth 6.95 2021-02-12T23:06:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171185.89 epoch sec; seqNo:797; 4056178.94 north; 590423.33 east 2021-02-12T23:06:26Z- AHRS_M2 msg: 1613171186.26 epoch sec; seqNo:794 2021-02-12T23:06:26Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.23 psi 2021-02-12T23:06:26Z- handleDepth()-Depth_Keller msg: 1613171186269 epoch sec; seqNo:798; depth 6.82 2021-02-12T23:06:26Z- handleDvl() - RDI_Pathfinder msg: 1613171186.28 epoch sec; seqNo:336 2021-02-12T23:06:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 106.78 , 117.60 , 77.26 2021-02-12T23:06:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , -0.19 , -0.32 2021-02-12T23:06:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171186.30 epoch sec; seqNo:798; 4056179.32 north; 590423.33 east 2021-02-12T23:06:26Z- AHRS_M2 msg: 1613171186.66 epoch sec; seqNo:795 2021-02-12T23:06:26Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.24 psi 2021-02-12T23:06:26Z- handleDepth()-Depth_Keller msg: 1613171186677 epoch sec; seqNo:799; depth 6.69 2021-02-12T23:06:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171186.69 epoch sec; seqNo:799; 4056179.70 north; 590423.32 east 2021-02-12T23:06:26Z- LcmTrn::handleMessages() - handling 43 messages took 0.68 ms and 130 calls... 2021-02-12T23:06:26Z- waiting for 1613171184.82, time is 1613171186.69 2021-02-12T23:06:26Z- LcmTrn::updateTrn() >>>> pose time 1613171186.69, meas time 1613171186.28 <<<< 2021-02-12T23:06:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171186.28 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:06:26Z- TerrainNav::motionUpdate() - currEstimate = (4056180, 590423.32, 6.69) 2021-02-12T23:06:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:06:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:06:26Z- TNavBF::Projecting Measurements 2021-02-12T23:06:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:06:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:06:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:06:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:06:26Z- TNavFilter::4 good beams. 2021-02-12T23:06:26Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:06:26Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:06:26Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:06:26Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:06:26Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:06:26Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:06:26Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:06:26Z- TNavBF::mmse selected from filter -1 2021-02-12T23:06:26Z- TerrainMapOctree: (vehN, vehE ) = (4056179.44, 590423.02). 2021-02-12T23:06:26Z- TerrainMapOctree: Min Distance = 6352.66. 2021-02-12T23:06:26Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:06:26Z- There is no specific good beam that all particles have in common. 2021-02-12T23:06:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:06:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 11.60 2021-02-12T23:06:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:06:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 18.24 2021-02-12T23:06:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:06:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 22.13 2021-02-12T23:06:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:06:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 9.55 2021-02-12T23:06:26Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:06:26Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:06:26Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:06:27Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:06:27Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:06:27Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:06:27Z- Filter 0 No beams used in subcloud update 2021-02-12T23:06:27Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:06:27Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:06:27Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 816.3 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:06:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171186.28 sec. 2021-02-12T23:06:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:06:27Z- TNavBF::mmse selected from filter -1 2021-02-12T23:06:27Z- variances([0], [2], and psi_berg) = 816.174831 833.148274 0.000000 2021-02-12T23:06:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:06:27Z- LcmTrn::publishEstimates() - MLE : 4056144.20 590472.82 6.69 2021-02-12T23:06:27Z- LcmTrn::publishEstimates() - MMSE : 4056179.70 590423.32 6.69 2021-02-12T23:06:27Z- LcmTrn::publishEstimates() - COVAR: 816.17 833.15 0.86 0.00 2021-02-12T23:06:27Z- AHRS_M2 msg: 1613171187.07 epoch sec; seqNo:796 2021-02-12T23:06:27Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.25 psi 2021-02-12T23:06:27Z- handleDepth()-Depth_Keller msg: 1613171187072 epoch sec; seqNo:800; depth 6.56 2021-02-12T23:06:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171187.09 epoch sec; seqNo:800; 4056179.70 north; 590423.32 east 2021-02-12T23:06:27Z- AHRS_M2 msg: 1613171187.47 epoch sec; seqNo:797 2021-02-12T23:06:27Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.25 psi 2021-02-12T23:06:27Z- handleDepth()-Depth_Keller msg: 1613171187476 epoch sec; seqNo:801; depth 6.41 2021-02-12T23:06:27Z- handleDvl() - RDI_Pathfinder msg: 1613171187.49 epoch sec; seqNo:337 2021-02-12T23:06:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.43 , 109.49 , 123.01 , 78.08 2021-02-12T23:06:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.48 , 0.00 2021-02-12T23:06:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171187.51 epoch sec; seqNo:801; 4056180.08 north; 590423.32 east 2021-02-12T23:06:27Z- AHRS_M2 msg: 1613171187.88 epoch sec; seqNo:798 2021-02-12T23:06:27Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.24 psi 2021-02-12T23:06:27Z- handleDepth()-Depth_Keller msg: 1613171187883 epoch sec; seqNo:802; depth 6.28 2021-02-12T23:06:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171187.89 epoch sec; seqNo:802; 4056180.46 north; 590423.32 east 2021-02-12T23:06:28Z- AHRS_M2 msg: 1613171188.28 epoch sec; seqNo:799 2021-02-12T23:06:28Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.22 psi 2021-02-12T23:06:28Z- handleDepth()-Depth_Keller msg: 1613171188286 epoch sec; seqNo:803; depth 6.17 2021-02-12T23:06:28Z- handleDvl() - RDI_Pathfinder msg: 1613171188.30 epoch sec; seqNo:338 2021-02-12T23:06:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.65 , 112.06 , 122.88 , 77.26 2021-02-12T23:06:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.24 , 0.35 2021-02-12T23:06:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171188.31 epoch sec; seqNo:803; 4056180.83 north; 590423.31 east 2021-02-12T23:06:28Z- AHRS_M2 msg: 1613171188.68 epoch sec; seqNo:800 2021-02-12T23:06:28Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.19 psi 2021-02-12T23:06:28Z- handleDepth()-Depth_Keller msg: 1613171188688 epoch sec; seqNo:804; depth 6.07 2021-02-12T23:06:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171188.71 epoch sec; seqNo:804; 4056181.21 north; 590423.31 east 2021-02-12T23:06:29Z- AHRS_M2 msg: 1613171189.09 epoch sec; seqNo:801 2021-02-12T23:06:29Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.16 psi 2021-02-12T23:06:29Z- handleDepth()-Depth_Keller msg: 1613171189092 epoch sec; seqNo:805; depth 5.95 2021-02-12T23:06:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171189.10 epoch sec; seqNo:805; 4056181.59 north; 590423.30 east 2021-02-12T23:06:29Z- AHRS_M2 msg: 1613171189.49 epoch sec; seqNo:802 2021-02-12T23:06:29Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.14 psi 2021-02-12T23:06:29Z- handleDepth()-Depth_Keller msg: 1613171189501 epoch sec; seqNo:806; depth 5.83 2021-02-12T23:06:29Z- handleDvl() - RDI_Pathfinder msg: 1613171189.51 epoch sec; seqNo:339 2021-02-12T23:06:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 107.93 , 122.81 , 76.86 2021-02-12T23:06:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.20 , 0.31 2021-02-12T23:06:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171189.52 epoch sec; seqNo:806; 4056181.97 north; 590423.30 east 2021-02-12T23:06:29Z- AHRS_M2 msg: 1613171189.90 epoch sec; seqNo:803 2021-02-12T23:06:29Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.12 psi 2021-02-12T23:06:29Z- handleDepth()-Depth_Keller msg: 1613171189903 epoch sec; seqNo:807; depth 5.70 2021-02-12T23:06:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171189.91 epoch sec; seqNo:807; 4056181.98 north; 590424.52 east 2021-02-12T23:06:30Z- AHRS_M2 msg: 1613171190.30 epoch sec; seqNo:804 2021-02-12T23:06:30Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.11 psi 2021-02-12T23:06:30Z- handleDepth()-Depth_Keller msg: 1613171190304 epoch sec; seqNo:808; depth 5.56 2021-02-12T23:06:30Z- handleDvl() - RDI_Pathfinder msg: 1613171190.32 epoch sec; seqNo:340 2021-02-12T23:06:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.35 , 103.87 , 118.75 , 78.08 2021-02-12T23:06:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , 0.32 , 0.01 2021-02-12T23:06:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171190.33 epoch sec; seqNo:808; 4056182.36 north; 590424.52 east 2021-02-12T23:06:30Z- AHRS_M2 msg: 1613171190.70 epoch sec; seqNo:805 2021-02-12T23:06:30Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.11 psi 2021-02-12T23:06:30Z- handleDepth()-Depth_Keller msg: 1613171190708 epoch sec; seqNo:809; depth 5.42 2021-02-12T23:06:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171190.72 epoch sec; seqNo:809; 4056182.36 north; 590424.52 east 2021-02-12T23:06:31Z- AHRS_M2 msg: 1613171191.11 epoch sec; seqNo:806 2021-02-12T23:06:31Z- AHRS_M2 msg: 0.68 phi; 0.27 theta; -0.10 psi 2021-02-12T23:06:31Z- handleDepth()-Depth_Keller msg: 1613171191118 epoch sec; seqNo:810; depth 5.30 2021-02-12T23:06:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171191.13 epoch sec; seqNo:810; 4056182.36 north; 590424.52 east 2021-02-12T23:06:31Z- AHRS_M2 msg: 1613171191.51 epoch sec; seqNo:807 2021-02-12T23:06:31Z- AHRS_M2 msg: 0.69 phi; 0.23 theta; -0.12 psi 2021-02-12T23:06:31Z- handleDepth()-Depth_Keller msg: 1613171191525 epoch sec; seqNo:811; depth 5.12 2021-02-12T23:06:31Z- handleDvl() - RDI_Pathfinder msg: 1613171191.54 epoch sec; seqNo:341 2021-02-12T23:06:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.78 , 95.89 , 113.48 , 79.31 2021-02-12T23:06:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.63 , -0.06 , -0.22 2021-02-12T23:06:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171191.55 epoch sec; seqNo:811; 4056182.74 north; 590424.51 east 2021-02-12T23:06:31Z- AHRS_M2 msg: 1613171191.92 epoch sec; seqNo:808 2021-02-12T23:06:31Z- AHRS_M2 msg: 0.69 phi; 0.19 theta; -0.14 psi 2021-02-12T23:06:31Z- handleDepth()-Depth_Keller msg: 1613171191931 epoch sec; seqNo:812; depth 4.97 2021-02-12T23:06:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171191.94 epoch sec; seqNo:812; 4056183.13 north; 590425.73 east 2021-02-12T23:06:32Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-12T23:06:32Z- waiting for 1613171189.69, time is 1613171191.94 2021-02-12T23:06:32Z- LcmTrn::updateTrn() >>>> pose time 1613171191.94, meas time 1613171191.54 <<<< 2021-02-12T23:06:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171191.54 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:06:32Z- TerrainNav::motionUpdate() - currEstimate = (4056183, 590425.73, 4.97) 2021-02-12T23:06:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:06:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:06:32Z- TNavBF::Projecting Measurements 2021-02-12T23:06:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:06:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:06:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:06:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:06:32Z- TNavFilter::4 good beams. 2021-02-12T23:06:32Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:06:32Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:06:32Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:06:32Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:06:32Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:06:32Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:06:32Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:06:32Z- TNavBF::mmse selected from filter -1 2021-02-12T23:06:32Z- TerrainMapOctree: (vehN, vehE ) = (4056182.87, 590425.54). 2021-02-12T23:06:32Z- TerrainMapOctree: Min Distance = 6348.41. 2021-02-12T23:06:32Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:06:32Z- There is no specific good beam that all particles have in common. 2021-02-12T23:06:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:06:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 12.33 2021-02-12T23:06:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:06:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 14.71 2021-02-12T23:06:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:06:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 20.60 2021-02-12T23:06:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:06:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 10.06 2021-02-12T23:06:32Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:06:32Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:06:32Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:06:32Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:06:32Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:06:32Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:06:32Z- Filter 0 No beams used in subcloud update 2021-02-12T23:06:32Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:06:32Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:06:32Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 815.8 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:06:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171191.54 sec. 2021-02-12T23:06:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:06:32Z- TNavBF::mmse selected from filter -1 2021-02-12T23:06:32Z- variances([0], [2], and psi_berg) = 815.743336 833.146684 0.000000 2021-02-12T23:06:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:06:32Z- LcmTrn::publishEstimates() - MLE : 4056147.63 590475.23 4.97 2021-02-12T23:06:32Z- LcmTrn::publishEstimates() - MMSE : 4056183.13 590425.73 4.97 2021-02-12T23:06:32Z- LcmTrn::publishEstimates() - COVAR: 815.74 833.15 0.84 0.00 2021-02-12T23:06:32Z- AHRS_M2 msg: 1613171192.32 epoch sec; seqNo:809 2021-02-12T23:06:32Z- AHRS_M2 msg: 0.69 phi; 0.15 theta; -0.15 psi 2021-02-12T23:06:32Z- handleDepth()-Depth_Keller msg: 1613171192327 epoch sec; seqNo:813; depth 4.85 2021-02-12T23:06:32Z- handleDvl() - RDI_Pathfinder msg: 1613171192.34 epoch sec; seqNo:342 2021-02-12T23:06:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 101.57 , 105.63 , 82.63 2021-02-12T23:06:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.41 , 0.07 2021-02-12T23:06:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171192.36 epoch sec; seqNo:813; 4056183.13 north; 590425.73 east 2021-02-12T23:06:32Z- AHRS_M2 msg: 1613171192.72 epoch sec; seqNo:810 2021-02-12T23:06:32Z- AHRS_M2 msg: 0.68 phi; 0.12 theta; -0.15 psi 2021-02-12T23:06:32Z- handleDepth()-Depth_Keller msg: 1613171192728 epoch sec; seqNo:814; depth 4.71 2021-02-12T23:06:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171192.75 epoch sec; seqNo:814; 4056183.51 north; 590425.73 east 2021-02-12T23:06:33Z- AHRS_M2 msg: 1613171193.13 epoch sec; seqNo:811 2021-02-12T23:06:33Z- AHRS_M2 msg: 0.68 phi; 0.08 theta; -0.14 psi 2021-02-12T23:06:33Z- handleDepth()-Depth_Keller msg: 1613171193142 epoch sec; seqNo:815; depth 4.61 2021-02-12T23:06:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171193.15 epoch sec; seqNo:815; 4056183.89 north; 590425.72 east 2021-02-12T23:06:33Z- AHRS_M2 msg: 1613171193.53 epoch sec; seqNo:812 2021-02-12T23:06:33Z- AHRS_M2 msg: 0.69 phi; 0.04 theta; -0.12 psi 2021-02-12T23:06:33Z- handleDepth()-Depth_Keller msg: 1613171193547 epoch sec; seqNo:816; depth 4.52 2021-02-12T23:06:33Z- handleDvl() - RDI_Pathfinder msg: 1613171193.56 epoch sec; seqNo:343 2021-02-12T23:06:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.70 , 96.23 , 96.23 , 86.76 2021-02-12T23:06:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.16 , 0.38 2021-02-12T23:06:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171193.57 epoch sec; seqNo:816; 4056184.27 north; 590425.72 east 2021-02-12T23:06:33Z- AHRS_M2 msg: 1613171193.94 epoch sec; seqNo:813 2021-02-12T23:06:33Z- AHRS_M2 msg: 0.69 phi; -0.00 theta; -0.12 psi 2021-02-12T23:06:33Z- handleDepth()-Depth_Keller msg: 1613171193941 epoch sec; seqNo:817; depth 4.44 2021-02-12T23:06:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171193.95 epoch sec; seqNo:817; 4056184.65 north; 590425.71 east 2021-02-12T23:06:34Z- AHRS_M2 msg: 1613171194.34 epoch sec; seqNo:814 2021-02-12T23:06:34Z- AHRS_M2 msg: 0.70 phi; -0.05 theta; -0.13 psi 2021-02-12T23:06:34Z- handleDepth()-Depth_Keller msg: 1613171194344 epoch sec; seqNo:818; depth 4.40 2021-02-12T23:06:34Z- handleDvl() - RDI_Pathfinder msg: 1613171194.36 epoch sec; seqNo:344 2021-02-12T23:06:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 96.50 , 91.63 , 91.63 2021-02-12T23:06:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.24 , 0.21 2021-02-12T23:06:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171194.37 epoch sec; seqNo:818; 4056184.65 north; 590425.71 east 2021-02-12T23:06:34Z- AHRS_M2 msg: 1613171194.74 epoch sec; seqNo:815 2021-02-12T23:06:34Z- AHRS_M2 msg: 0.71 phi; -0.09 theta; -0.14 psi 2021-02-12T23:06:34Z- handleDepth()-Depth_Keller msg: 1613171194750 epoch sec; seqNo:819; depth 4.35 2021-02-12T23:06:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171194.77 epoch sec; seqNo:819; 4056185.04 north; 590426.93 east 2021-02-12T23:06:35Z- AHRS_M2 msg: 1613171195.15 epoch sec; seqNo:816 2021-02-12T23:06:35Z- AHRS_M2 msg: 0.72 phi; -0.13 theta; -0.15 psi 2021-02-12T23:06:35Z- handleDepth()-Depth_Keller msg: 1613171195168 epoch sec; seqNo:820; depth 4.32 2021-02-12T23:06:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171195.18 epoch sec; seqNo:820; 4056185.04 north; 590426.93 east 2021-02-12T23:06:35Z- AHRS_M2 msg: 1613171195.56 epoch sec; seqNo:817 2021-02-12T23:06:35Z- AHRS_M2 msg: 0.75 phi; -0.20 theta; -0.16 psi 2021-02-12T23:06:35Z- handleDepth()-Depth_Keller msg: 1613171195562 epoch sec; seqNo:821; depth 4.32 2021-02-12T23:06:35Z- handleDvl() - RDI_Pathfinder msg: 1613171195.57 epoch sec; seqNo:345 2021-02-12T23:06:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 96.50 , 87.98 , 96.50 2021-02-12T23:06:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.23 , -0.18 2021-02-12T23:06:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171195.60 epoch sec; seqNo:821; 4056185.42 north; 590426.93 east 2021-02-12T23:06:35Z- AHRS_M2 msg: 1613171195.96 epoch sec; seqNo:818 2021-02-12T23:06:35Z- AHRS_M2 msg: 0.76 phi; -0.22 theta; -0.17 psi 2021-02-12T23:06:35Z- handleDepth()-Depth_Keller msg: 1613171195967 epoch sec; seqNo:822; depth 4.33 2021-02-12T23:06:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171195.98 epoch sec; seqNo:822; 4056185.42 north; 590426.93 east 2021-02-12T23:06:36Z- AHRS_M2 msg: 1613171196.36 epoch sec; seqNo:819 2021-02-12T23:06:36Z- AHRS_M2 msg: 0.78 phi; -0.24 theta; -0.18 psi 2021-02-12T23:06:36Z- handleDepth()-Depth_Keller msg: 1613171196370 epoch sec; seqNo:823; depth 4.36 2021-02-12T23:06:36Z- handleDvl() - RDI_Pathfinder msg: 1613171196.38 epoch sec; seqNo:346 2021-02-12T23:06:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.26 , 96.43 , 85.47 , 104.95 2021-02-12T23:06:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.07 , -0.35 2021-02-12T23:06:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171196.39 epoch sec; seqNo:823; 4056185.80 north; 590426.92 east 2021-02-12T23:06:36Z- AHRS_M2 msg: 1613171196.76 epoch sec; seqNo:820 2021-02-12T23:06:36Z- AHRS_M2 msg: 0.79 phi; -0.27 theta; -0.19 psi 2021-02-12T23:06:36Z- handleDepth()-Depth_Keller msg: 1613171196768 epoch sec; seqNo:824; depth 4.40 2021-02-12T23:06:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:06:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:06:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:06:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:06:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:06:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:06:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:06:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:06:36Z- handleDvl() - RDI_Pathfinder msg: 1613171196.77 epoch sec; seqNo:347 2021-02-12T23:06:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:06:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.07 , -0.35 2021-02-12T23:06:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171196.78 epoch sec; seqNo:824; 4056186.19 north; 590428.14 east 2021-02-12T23:06:37Z- LcmTrn::handleMessages() - handling 42 messages took 0.70 ms and 128 calls... 2021-02-12T23:06:37Z- waiting for 1613171194.94, time is 1613171196.78 2021-02-12T23:06:37Z- LcmTrn::updateTrn() equating pose 1613171196.78 and meas times 1613171196.78 2021-02-12T23:06:37Z- LcmTrn::updateTrn() >>>> pose time 1613171196.78, meas time 1613171196.78 <<<< 2021-02-12T23:06:37Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613171196.78, ping # 0. 2021-02-12T23:06:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171196.78 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:06:37Z- TerrainNav::motionUpdate() - currEstimate = (4056186, 590428.14, 4.40) 2021-02-12T23:06:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:06:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:06:37Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:06:37Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:06:37Z- TNavBF::Projecting Measurements 2021-02-12T23:06:37Z- TNavFilter::Measurement from time = 1613171196.78 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:06:37Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:06:37Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:06:37Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:06:37Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:06:37Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:06:37Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:06:37Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:06:37Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 815.4 EastVariance: 833.1 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:06:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:06:37Z- TNavBF::mmse selected from filter -1 2021-02-12T23:06:37Z- variances([0], [2], and psi_berg) = 815.411063 833.145479 0.000000 2021-02-12T23:06:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:06:37Z- LcmTrn::publishEstimates() - MLE : 4056150.69 590477.64 4.40 2021-02-12T23:06:37Z- LcmTrn::publishEstimates() - MMSE : 4056186.19 590428.14 4.40 2021-02-12T23:06:37Z- LcmTrn::publishEstimates() - COVAR: 815.41 833.15 1.04 0.00 2021-02-12T23:06:37Z- AHRS_M2 msg: 1613171197.19 epoch sec; seqNo:821 2021-02-12T23:06:37Z- AHRS_M2 msg: 0.80 phi; -0.30 theta; -0.20 psi 2021-02-12T23:06:37Z- handleDepth()-Depth_Keller msg: 1613171197195 epoch sec; seqNo:825; depth 4.44 2021-02-12T23:06:37Z- handleDvl() - RDI_Pathfinder msg: 1613171197.21 epoch sec; seqNo:348 2021-02-12T23:06:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 107.12 , 81.42 , 109.82 2021-02-12T23:06:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.34 , -0.12 2021-02-12T23:06:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171197.23 epoch sec; seqNo:825; 4056186.19 north; 590428.14 east 2021-02-12T23:06:37Z- AHRS_M2 msg: 1613171197.57 epoch sec; seqNo:822 2021-02-12T23:06:37Z- AHRS_M2 msg: 0.79 phi; -0.34 theta; -0.21 psi 2021-02-12T23:06:37Z- handleDepth()-Depth_Keller msg: 1613171197578 epoch sec; seqNo:826; depth 4.53 2021-02-12T23:06:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171197.59 epoch sec; seqNo:826; 4056186.57 north; 590428.14 east 2021-02-12T23:06:37Z- AHRS_M2 msg: 1613171197.98 epoch sec; seqNo:823 2021-02-12T23:06:37Z- AHRS_M2 msg: 0.78 phi; -0.37 theta; -0.21 psi 2021-02-12T23:06:37Z- handleDepth()-Depth_Keller msg: 1613171197982 epoch sec; seqNo:827; depth 4.58 2021-02-12T23:06:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:06:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:06:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:06:38Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:06:38Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:06:38Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:06:38Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:06:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:06:38Z- handleDvl() - RDI_Pathfinder msg: 1613171198.00 epoch sec; seqNo:349 2021-02-12T23:06:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:06:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.34 , -0.12 2021-02-12T23:06:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171198.01 epoch sec; seqNo:827; 4056186.95 north; 590428.13 east 2021-02-12T23:06:38Z- AHRS_M2 msg: 1613171198.38 epoch sec; seqNo:824 2021-02-12T23:06:38Z- AHRS_M2 msg: 0.77 phi; -0.40 theta; -0.21 psi 2021-02-12T23:06:38Z- handleDepth()-Depth_Keller msg: 1613171198383 epoch sec; seqNo:828; depth 4.65 2021-02-12T23:06:38Z- handleDvl() - RDI_Pathfinder msg: 1613171198.39 epoch sec; seqNo:350 2021-02-12T23:06:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.27 , 109.62 , 78.08 , 119.09 2021-02-12T23:06:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.43 , -0.20 , 0.17 2021-02-12T23:06:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171198.43 epoch sec; seqNo:828; 4056186.95 north; 590428.13 east 2021-02-12T23:06:38Z- AHRS_M2 msg: 1613171198.78 epoch sec; seqNo:825 2021-02-12T23:06:38Z- AHRS_M2 msg: 0.74 phi; -0.42 theta; -0.22 psi 2021-02-12T23:06:38Z- handleDepth()-Depth_Keller msg: 1613171198788 epoch sec; seqNo:829; depth 4.73 2021-02-12T23:06:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171198.80 epoch sec; seqNo:829; 4056187.33 north; 590428.13 east 2021-02-12T23:06:39Z- AHRS_M2 msg: 1613171199.19 epoch sec; seqNo:826 2021-02-12T23:06:39Z- AHRS_M2 msg: 0.72 phi; -0.43 theta; -0.23 psi 2021-02-12T23:06:39Z- handleDepth()-Depth_Keller msg: 1613171199205 epoch sec; seqNo:830; depth 4.82 2021-02-12T23:06:39Z- handleDvl() - RDI_Pathfinder msg: 1613171199.22 epoch sec; seqNo:351 2021-02-12T23:06:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.71 , 114.76 , 76.86 , 133.70 2021-02-12T23:06:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.55 , 0.06 , 0.28 2021-02-12T23:06:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171199.23 epoch sec; seqNo:830; 4056187.33 north; 590428.13 east 2021-02-12T23:06:39Z- AHRS_M2 msg: 1613171199.59 epoch sec; seqNo:827 2021-02-12T23:06:39Z- AHRS_M2 msg: 0.71 phi; -0.44 theta; -0.24 psi 2021-02-12T23:06:39Z- handleDepth()-Depth_Keller msg: 1613171199601 epoch sec; seqNo:831; depth 4.96 2021-02-12T23:06:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171199.62 epoch sec; seqNo:831; 4056187.33 north; 590428.13 east 2021-02-12T23:06:40Z- AHRS_M2 msg: 1613171200.00 epoch sec; seqNo:828 2021-02-12T23:06:40Z- AHRS_M2 msg: 0.69 phi; -0.44 theta; -0.25 psi 2021-02-12T23:06:40Z- handleDepth()-Depth_Keller msg: 1613171200000 epoch sec; seqNo:832; depth 5.07 2021-02-12T23:06:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171200.01 epoch sec; seqNo:832; 4056187.33 north; 590428.13 east 2021-02-12T23:06:40Z- AHRS_M2 msg: 1613171200.40 epoch sec; seqNo:829 2021-02-12T23:06:40Z- AHRS_M2 msg: 0.69 phi; -0.45 theta; -0.26 psi 2021-02-12T23:06:40Z- handleDepth()-Depth_Keller msg: 1613171200415 epoch sec; seqNo:833; depth 5.20 2021-02-12T23:06:40Z- handleDvl() - RDI_Pathfinder msg: 1613171200.43 epoch sec; seqNo:352 2021-02-12T23:06:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 117.20 , 77.67 , 138.03 2021-02-12T23:06:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.25 , 0.12 2021-02-12T23:06:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171200.47 epoch sec; seqNo:833; 4056187.70 north; 590428.12 east 2021-02-12T23:06:40Z- AHRS_M2 msg: 1613171200.81 epoch sec; seqNo:830 2021-02-12T23:06:40Z- AHRS_M2 msg: 0.69 phi; -0.45 theta; -0.27 psi 2021-02-12T23:06:40Z- handleDepth()-Depth_Keller msg: 1613171200810 epoch sec; seqNo:834; depth 5.37 2021-02-12T23:06:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171200.82 epoch sec; seqNo:834; 4056187.72 north; 590429.35 east 2021-02-12T23:06:41Z- AHRS_M2 msg: 1613171201.22 epoch sec; seqNo:831 2021-02-12T23:06:41Z- AHRS_M2 msg: 0.69 phi; -0.45 theta; -0.27 psi 2021-02-12T23:06:41Z- handleDepth()-Depth_Keller msg: 1613171201230 epoch sec; seqNo:835; depth 5.51 2021-02-12T23:06:41Z- handleDvl() - RDI_Pathfinder msg: 1613171201.25 epoch sec; seqNo:353 2021-02-12T23:06:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.72 , 118.48 , 77.26 , 140.80 2021-02-12T23:06:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.26 , -0.15 2021-02-12T23:06:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171201.26 epoch sec; seqNo:835; 4056188.10 north; 590429.34 east 2021-02-12T23:06:41Z- AHRS_M2 msg: 1613171201.61 epoch sec; seqNo:832 2021-02-12T23:06:41Z- AHRS_M2 msg: 0.68 phi; -0.44 theta; -0.26 psi 2021-02-12T23:06:41Z- handleDepth()-Depth_Keller msg: 1613171201621 epoch sec; seqNo:836; depth 5.69 2021-02-12T23:06:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:06:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:06:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:06:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:06:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:06:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:06:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:06:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:06:41Z- handleDvl() - RDI_Pathfinder msg: 1613171201.62 epoch sec; seqNo:354 2021-02-12T23:06:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:06:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.26 , -0.15 2021-02-12T23:06:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171201.65 epoch sec; seqNo:836; 4056188.10 north; 590429.34 east 2021-02-12T23:06:42Z- AHRS_M2 msg: 1613171202.02 epoch sec; seqNo:833 2021-02-12T23:06:42Z- AHRS_M2 msg: 0.68 phi; -0.43 theta; -0.25 psi 2021-02-12T23:06:42Z- handleDepth()-Depth_Keller msg: 1613171202025 epoch sec; seqNo:837; depth 5.85 2021-02-12T23:06:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171202.03 epoch sec; seqNo:837; 4056188.48 north; 590429.34 east 2021-02-12T23:06:42Z- LcmTrn::handleMessages() - handling 46 messages took 0.73 ms and 132 calls... 2021-02-12T23:06:42Z- waiting for 1613171199.78, time is 1613171202.03 2021-02-12T23:06:42Z- LcmTrn::updateTrn() >>>> pose time 1613171202.03, meas time 1613171201.62 <<<< 2021-02-12T23:06:42Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613171201.62, ping # 0. 2021-02-12T23:06:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171201.62 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:06:42Z- TerrainNav::motionUpdate() - currEstimate = (4056188, 590429.34, 5.85) 2021-02-12T23:06:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:06:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:06:42Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:06:42Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:06:42Z- TNavBF::Projecting Measurements 2021-02-12T23:06:42Z- TNavFilter::Measurement from time = 1613171201.62 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:06:42Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:06:42Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:06:42Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:06:42Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:06:42Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:06:42Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:06:42Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:06:42Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 815.2 EastVariance: 833.1 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:06:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:06:42Z- TNavBF::mmse selected from filter -1 2021-02-12T23:06:42Z- variances([0], [2], and psi_berg) = 815.079395 833.144295 0.000000 2021-02-12T23:06:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:06:42Z- LcmTrn::publishEstimates() - MLE : 4056153.98 590379.84 5.85 2021-02-12T23:06:42Z- LcmTrn::publishEstimates() - MMSE : 4056188.48 590429.34 5.85 2021-02-12T23:06:42Z- LcmTrn::publishEstimates() - COVAR: 815.08 833.14 1.17 0.00 2021-02-12T23:06:42Z- AHRS_M2 msg: 1613171202.42 epoch sec; seqNo:834 2021-02-12T23:06:42Z- AHRS_M2 msg: 0.68 phi; -0.42 theta; -0.24 psi 2021-02-12T23:06:42Z- handleDepth()-Depth_Keller msg: 1613171202432 epoch sec; seqNo:838; depth 6.02 2021-02-12T23:06:42Z- handleDvl() - RDI_Pathfinder msg: 1613171202.45 epoch sec; seqNo:355 2021-02-12T23:06:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.65 , 119.90 , 76.04 , 139.25 2021-02-12T23:06:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.32 2021-02-12T23:06:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171202.46 epoch sec; seqNo:838; 4056188.87 north; 590430.55 east 2021-02-12T23:06:42Z- AHRS_M2 msg: 1613171202.82 epoch sec; seqNo:835 2021-02-12T23:06:42Z- AHRS_M2 msg: 0.68 phi; -0.40 theta; -0.22 psi 2021-02-12T23:06:42Z- handleDepth()-Depth_Keller msg: 1613171202828 epoch sec; seqNo:839; depth 6.23 2021-02-12T23:06:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:06:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:06:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:06:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:06:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:06:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:06:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:06:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:06:42Z- handleDvl() - RDI_Pathfinder msg: 1613171202.83 epoch sec; seqNo:356 2021-02-12T23:06:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:06:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.32 2021-02-12T23:06:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171202.85 epoch sec; seqNo:839; 4056189.25 north; 590430.55 east 2021-02-12T23:06:43Z- AHRS_M2 msg: 1613171203.23 epoch sec; seqNo:836 2021-02-12T23:06:43Z- AHRS_M2 msg: 0.68 phi; -0.38 theta; -0.20 psi 2021-02-12T23:06:43Z- handleDepth()-Depth_Keller msg: 1613171203235 epoch sec; seqNo:840; depth 6.40 2021-02-12T23:06:43Z- handleDvl() - RDI_Pathfinder msg: 1613171203.25 epoch sec; seqNo:357 2021-02-12T23:06:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.68 , 113.95 , 76.45 , 130.32 2021-02-12T23:06:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.27 , -0.34 2021-02-12T23:06:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171203.26 epoch sec; seqNo:840; 4056189.25 north; 590430.55 east 2021-02-12T23:06:43Z- AHRS_M2 msg: 1613171203.63 epoch sec; seqNo:837 2021-02-12T23:06:43Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.20 psi 2021-02-12T23:06:43Z- handleDepth()-Depth_Keller msg: 1613171203645 epoch sec; seqNo:841; depth 6.57 2021-02-12T23:06:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171203.66 epoch sec; seqNo:841; 4056189.62 north; 590430.55 east 2021-02-12T23:06:44Z- AHRS_M2 msg: 1613171204.04 epoch sec; seqNo:838 2021-02-12T23:06:44Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.19 psi 2021-02-12T23:06:44Z- handleDepth()-Depth_Keller msg: 1613171204040 epoch sec; seqNo:842; depth 6.73 2021-02-12T23:06:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:06:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:06:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:06:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:06:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:06:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:06:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:06:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:06:44Z- handleDvl() - RDI_Pathfinder msg: 1613171204.04 epoch sec; seqNo:358 2021-02-12T23:06:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:06:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.27 , -0.34 2021-02-12T23:06:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171204.06 epoch sec; seqNo:842; 4056190.00 north; 590430.54 east 2021-02-12T23:06:44Z- AHRS_M2 msg: 1613171204.44 epoch sec; seqNo:839 2021-02-12T23:06:44Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.19 psi 2021-02-12T23:06:44Z- handleDepth()-Depth_Keller msg: 1613171204444 epoch sec; seqNo:843; depth 6.88 2021-02-12T23:06:44Z- handleDvl() - RDI_Pathfinder msg: 1613171204.45 epoch sec; seqNo:359 2021-02-12T23:06:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.60 , 108.40 , 74.40 , 121.80 2021-02-12T23:06:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.37 , -0.16 2021-02-12T23:06:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171204.49 epoch sec; seqNo:843; 4056190.38 north; 590430.54 east 2021-02-12T23:06:44Z- AHRS_M2 msg: 1613171204.84 epoch sec; seqNo:840 2021-02-12T23:06:44Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.18 psi 2021-02-12T23:06:44Z- handleDepth()-Depth_Keller msg: 1613171204848 epoch sec; seqNo:844; depth 7.06 2021-02-12T23:06:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171204.86 epoch sec; seqNo:844; 4056190.76 north; 590430.53 east 2021-02-12T23:06:45Z- AHRS_M2 msg: 1613171205.25 epoch sec; seqNo:841 2021-02-12T23:06:45Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.18 psi 2021-02-12T23:06:45Z- handleDepth()-Depth_Keller msg: 1613171205254 epoch sec; seqNo:845; depth 7.18 2021-02-12T23:06:45Z- handleDvl() - RDI_Pathfinder msg: 1613171205.27 epoch sec; seqNo:360 2021-02-12T23:06:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.72 , 105.36 , 75.63 , 114.83 2021-02-12T23:06:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.61 , -0.27 , 0.07 2021-02-12T23:06:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171205.28 epoch sec; seqNo:845; 4056191.14 north; 590430.53 east 2021-02-12T23:06:45Z- AHRS_M2 msg: 1613171205.65 epoch sec; seqNo:842 2021-02-12T23:06:45Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.17 psi 2021-02-12T23:06:45Z- handleDepth()-Depth_Keller msg: 1613171205656 epoch sec; seqNo:846; depth 7.32 2021-02-12T23:06:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171205.67 epoch sec; seqNo:846; 4056191.14 north; 590430.53 east 2021-02-12T23:06:46Z- AHRS_M2 msg: 1613171206.06 epoch sec; seqNo:843 2021-02-12T23:06:46Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.16 psi 2021-02-12T23:06:46Z- handleDepth()-Depth_Keller msg: 1613171206062 epoch sec; seqNo:847; depth 7.43 2021-02-12T23:06:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171206.07 epoch sec; seqNo:847; 4056191.52 north; 590430.53 east 2021-02-12T23:06:46Z- AHRS_M2 msg: 1613171206.46 epoch sec; seqNo:844 2021-02-12T23:06:46Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.16 psi 2021-02-12T23:06:46Z- handleDepth()-Depth_Keller msg: 1613171206466 epoch sec; seqNo:848; depth 7.56 2021-02-12T23:06:46Z- handleDvl() - RDI_Pathfinder msg: 1613171206.48 epoch sec; seqNo:361 2021-02-12T23:06:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.63 , 102.93 , 75.22 , 111.04 2021-02-12T23:06:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.62 , -0.10 , 0.28 2021-02-12T23:06:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171206.49 epoch sec; seqNo:848; 4056191.52 north; 590430.53 east 2021-02-12T23:06:46Z- AHRS_M2 msg: 1613171206.86 epoch sec; seqNo:845 2021-02-12T23:06:46Z- AHRS_M2 msg: 0.59 phi; -0.27 theta; -0.16 psi 2021-02-12T23:06:46Z- handleDepth()-Depth_Keller msg: 1613171206870 epoch sec; seqNo:849; depth 7.67 2021-02-12T23:06:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171206.89 epoch sec; seqNo:849; 4056191.90 north; 590430.52 east 2021-02-12T23:06:47Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 131 calls... 2021-02-12T23:06:47Z- waiting for 1613171205.03, time is 1613171206.89 2021-02-12T23:06:47Z- LcmTrn::updateTrn() >>>> pose time 1613171206.89, meas time 1613171206.48 <<<< 2021-02-12T23:06:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171206.48 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:06:47Z- TerrainNav::motionUpdate() - currEstimate = (4056192, 590430.52, 7.67) 2021-02-12T23:06:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:06:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:06:47Z- TNavBF::Projecting Measurements 2021-02-12T23:06:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:06:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:06:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:06:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:06:47Z- TNavFilter::4 good beams. 2021-02-12T23:06:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:06:47Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:06:47Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:06:47Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:06:47Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:06:47Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:06:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:06:47Z- TNavBF::mmse selected from filter -1 2021-02-12T23:06:47Z- TerrainMapOctree: (vehN, vehE ) = (4056191.61, 590430.42). 2021-02-12T23:06:47Z- TerrainMapOctree: Min Distance = 6338.49. 2021-02-12T23:06:47Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:06:47Z- There is no specific good beam that all particles have in common. 2021-02-12T23:06:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:06:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 10.93 2021-02-12T23:06:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:06:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 16.95 2021-02-12T23:06:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:06:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 9.05 2021-02-12T23:06:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:06:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 19.73 2021-02-12T23:06:47Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:06:47Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:06:47Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:06:47Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:06:47Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:06:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:06:47Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:06:47Z- Filter 0 No beams used in subcloud update 2021-02-12T23:06:47Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:06:47Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:06:47Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 814.8 EastVariance: 833.1 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:06:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171206.48 sec. 2021-02-12T23:06:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:06:47Z- TNavBF::mmse selected from filter -1 2021-02-12T23:06:47Z- variances([0], [2], and psi_berg) = 814.687788 833.142917 0.000000 2021-02-12T23:06:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:06:47Z- LcmTrn::publishEstimates() - MLE : 4056157.40 590381.02 7.67 2021-02-12T23:06:47Z- LcmTrn::publishEstimates() - MMSE : 4056191.90 590430.52 7.67 2021-02-12T23:06:47Z- LcmTrn::publishEstimates() - COVAR: 814.69 833.14 1.00 0.00 2021-02-12T23:06:47Z- AHRS_M2 msg: 1613171207.27 epoch sec; seqNo:846 2021-02-12T23:06:47Z- AHRS_M2 msg: 0.57 phi; -0.26 theta; -0.17 psi 2021-02-12T23:06:47Z- handleDepth()-Depth_Keller msg: 1613171207272 epoch sec; seqNo:850; depth 7.77 2021-02-12T23:06:47Z- handleDvl() - RDI_Pathfinder msg: 1613171207.29 epoch sec; seqNo:362 2021-02-12T23:06:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 96.30 , 76.04 , 111.18 2021-02-12T23:06:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.16 , 0.34 2021-02-12T23:06:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171207.30 epoch sec; seqNo:850; 4056191.91 north; 590431.74 east 2021-02-12T23:06:47Z- handleDepth()-Depth_Keller msg: 1613171207693 epoch sec; seqNo:851; depth 7.88 2021-02-12T23:06:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171207.71 epoch sec; seqNo:851; 4056192.29 north; 590431.74 east 2021-02-12T23:06:48Z- AHRS_M2 msg: 1613171208.08 epoch sec; seqNo:847 2021-02-12T23:06:48Z- AHRS_M2 msg: 0.56 phi; -0.24 theta; -0.18 psi 2021-02-12T23:06:48Z- handleDepth()-Depth_Keller msg: 1613171208081 epoch sec; seqNo:852; depth 7.99 2021-02-12T23:06:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171208.09 epoch sec; seqNo:852; 4056192.29 north; 590431.74 east 2021-02-12T23:06:48Z- AHRS_M2 msg: 1613171208.48 epoch sec; seqNo:848 2021-02-12T23:06:48Z- AHRS_M2 msg: 0.56 phi; -0.23 theta; -0.18 psi 2021-02-12T23:06:48Z- handleDepth()-Depth_Keller msg: 1613171208490 epoch sec; seqNo:853; depth 8.08 2021-02-12T23:06:48Z- handleDvl() - RDI_Pathfinder msg: 1613171208.50 epoch sec; seqNo:363 2021-02-12T23:06:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 96.43 , 76.45 , 106.17 2021-02-12T23:06:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.34 , 0.05 2021-02-12T23:06:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171208.51 epoch sec; seqNo:853; 4056192.67 north; 590431.74 east 2021-02-12T23:06:48Z- AHRS_M2 msg: 1613171208.88 epoch sec; seqNo:849 2021-02-12T23:06:48Z- AHRS_M2 msg: 0.56 phi; -0.23 theta; -0.19 psi 2021-02-12T23:06:48Z- handleDepth()-Depth_Keller msg: 1613171208889 epoch sec; seqNo:854; depth 8.18 2021-02-12T23:06:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171208.91 epoch sec; seqNo:854; 4056192.67 north; 590431.74 east 2021-02-12T23:06:49Z- AHRS_M2 msg: 1613171209.29 epoch sec; seqNo:850 2021-02-12T23:06:49Z- AHRS_M2 msg: 0.57 phi; -0.22 theta; -0.19 psi 2021-02-12T23:06:49Z- handleDepth()-Depth_Keller msg: 1613171209292 epoch sec; seqNo:855; depth 8.28 2021-02-12T23:06:49Z- handleDvl() - RDI_Pathfinder msg: 1613171209.30 epoch sec; seqNo:364 2021-02-12T23:06:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.72 , 96.50 , 76.86 , 105.02 2021-02-12T23:06:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.23 , -0.39 2021-02-12T23:06:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171209.33 epoch sec; seqNo:855; 4056193.06 north; 590432.95 east 2021-02-12T23:06:49Z- AHRS_M2 msg: 1613171209.69 epoch sec; seqNo:851 2021-02-12T23:06:49Z- AHRS_M2 msg: 0.58 phi; -0.22 theta; -0.19 psi 2021-02-12T23:06:49Z- handleDepth()-Depth_Keller msg: 1613171209701 epoch sec; seqNo:856; depth 8.37 2021-02-12T23:06:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171209.71 epoch sec; seqNo:856; 4056193.44 north; 590432.95 east 2021-02-12T23:06:50Z- AHRS_M2 msg: 1613171210.10 epoch sec; seqNo:852 2021-02-12T23:06:50Z- AHRS_M2 msg: 0.59 phi; -0.22 theta; -0.19 psi 2021-02-12T23:06:50Z- handleDepth()-Depth_Keller msg: 1613171210102 epoch sec; seqNo:857; depth 8.46 2021-02-12T23:06:50Z- handleDvl() - RDI_Pathfinder msg: 1613171210.12 epoch sec; seqNo:365 2021-02-12T23:06:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.57 , 76.04 , 103.87 2021-02-12T23:06:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.14 , -0.51 2021-02-12T23:06:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171210.13 epoch sec; seqNo:857; 4056193.82 north; 590432.95 east 2021-02-12T23:06:50Z- AHRS_M2 msg: 1613171210.50 epoch sec; seqNo:853 2021-02-12T23:06:50Z- AHRS_M2 msg: 0.60 phi; -0.22 theta; -0.19 psi 2021-02-12T23:06:50Z- handleDepth()-Depth_Keller msg: 1613171210504 epoch sec; seqNo:858; depth 8.53 2021-02-12T23:06:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171210.52 epoch sec; seqNo:858; 4056194.20 north; 590432.94 east 2021-02-12T23:06:50Z- AHRS_M2 msg: 1613171210.90 epoch sec; seqNo:854 2021-02-12T23:06:50Z- AHRS_M2 msg: 0.61 phi; -0.22 theta; -0.18 psi 2021-02-12T23:06:50Z- handleDepth()-Depth_Keller msg: 1613171210908 epoch sec; seqNo:859; depth 8.62 2021-02-12T23:06:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171210.92 epoch sec; seqNo:859; 4056194.58 north; 590432.94 east 2021-02-12T23:06:51Z- AHRS_M2 msg: 1613171211.31 epoch sec; seqNo:855 2021-02-12T23:06:51Z- AHRS_M2 msg: 0.62 phi; -0.23 theta; -0.15 psi 2021-02-12T23:06:51Z- handleDepth()-Depth_Keller msg: 1613171211312 epoch sec; seqNo:860; depth 8.71 2021-02-12T23:06:51Z- handleDvl() - RDI_Pathfinder msg: 1613171211.32 epoch sec; seqNo:366 2021-02-12T23:06:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.08 , 96.36 , 80.13 , 103.13 2021-02-12T23:06:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.38 , -0.16 2021-02-12T23:06:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171211.33 epoch sec; seqNo:860; 4056194.95 north; 590432.93 east 2021-02-12T23:06:51Z- AHRS_M2 msg: 1613171211.71 epoch sec; seqNo:856 2021-02-12T23:06:51Z- AHRS_M2 msg: 0.62 phi; -0.23 theta; -0.14 psi 2021-02-12T23:06:51Z- handleDepth()-Depth_Keller msg: 1613171211725 epoch sec; seqNo:861; depth 8.81 2021-02-12T23:06:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171211.74 epoch sec; seqNo:861; 4056195.33 north; 590432.93 east 2021-02-12T23:06:52Z- AHRS_M2 msg: 1613171212.12 epoch sec; seqNo:857 2021-02-12T23:06:52Z- AHRS_M2 msg: 0.63 phi; -0.24 theta; -0.12 psi 2021-02-12T23:06:52Z- handleDepth()-Depth_Keller msg: 1613171212142 epoch sec; seqNo:862; depth 8.91 2021-02-12T23:06:52Z- handleDvl() - RDI_Pathfinder msg: 1613171212.15 epoch sec; seqNo:367 2021-02-12T23:06:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.31 , 96.57 , 74.81 , 103.87 2021-02-12T23:06:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.27 , 0.25 2021-02-12T23:06:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171212.16 epoch sec; seqNo:862; 4056195.33 north; 590432.93 east 2021-02-12T23:06:52Z- LcmTrn::handleMessages() - handling 44 messages took 0.63 ms and 132 calls... 2021-02-12T23:06:52Z- waiting for 1613171209.89, time is 1613171212.16 2021-02-12T23:06:52Z- LcmTrn::updateTrn() equating pose 1613171212.16 and meas times 1613171212.16 2021-02-12T23:06:52Z- LcmTrn::updateTrn() >>>> pose time 1613171212.16, meas time 1613171212.16 <<<< 2021-02-12T23:06:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171212.16 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:06:52Z- TerrainNav::motionUpdate() - currEstimate = (4056195, 590432.93, 8.91) 2021-02-12T23:06:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:06:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:06:52Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171212.16 sec.; No relevant navigation data available 2021-02-12T23:06:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:06:52Z- TNavBF::mmse selected from filter -1 2021-02-12T23:06:52Z- variances([0], [2], and psi_berg) = 814.353072 833.141755 0.000000 2021-02-12T23:06:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:06:52Z- LcmTrn::publishEstimates() - MLE : 4056160.83 590383.43 8.91 2021-02-12T23:06:52Z- LcmTrn::publishEstimates() - MMSE : 4056195.33 590432.93 8.91 2021-02-12T23:06:52Z- LcmTrn::publishEstimates() - COVAR: 814.35 833.14 1.21 0.00 2021-02-12T23:06:52Z- AHRS_M2 msg: 1613171212.53 epoch sec; seqNo:858 2021-02-12T23:06:52Z- AHRS_M2 msg: 0.63 phi; -0.26 theta; -0.10 psi 2021-02-12T23:06:52Z- handleDepth()-Depth_Keller msg: 1613171212535 epoch sec; seqNo:863; depth 8.97 2021-02-12T23:06:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171212.56 epoch sec; seqNo:863; 4056195.71 north; 590432.93 east 2021-02-12T23:06:52Z- AHRS_M2 msg: 1613171212.93 epoch sec; seqNo:859 2021-02-12T23:06:52Z- AHRS_M2 msg: 0.64 phi; -0.27 theta; -0.09 psi 2021-02-12T23:06:52Z- handleDepth()-Depth_Keller msg: 1613171212934 epoch sec; seqNo:864; depth 9.05 2021-02-12T23:06:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171212.94 epoch sec; seqNo:864; 4056196.09 north; 590432.92 east 2021-02-12T23:06:53Z- AHRS_M2 msg: 1613171213.33 epoch sec; seqNo:860 2021-02-12T23:06:53Z- AHRS_M2 msg: 0.64 phi; -0.27 theta; -0.09 psi 2021-02-12T23:06:53Z- handleDepth()-Depth_Keller msg: 1613171213342 epoch sec; seqNo:865; depth 9.13 2021-02-12T23:06:53Z- handleDvl() - RDI_Pathfinder msg: 1613171213.35 epoch sec; seqNo:368 2021-02-12T23:06:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.09 , 95.49 , 74.40 , 104.01 2021-02-12T23:06:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.03 , 0.29 2021-02-12T23:06:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171213.36 epoch sec; seqNo:865; 4056196.10 north; 590434.14 east 2021-02-12T23:06:53Z- AHRS_M2 msg: 1613171213.73 epoch sec; seqNo:861 2021-02-12T23:06:53Z- AHRS_M2 msg: 0.64 phi; -0.28 theta; -0.09 psi 2021-02-12T23:06:53Z- handleDepth()-Depth_Keller msg: 1613171213736 epoch sec; seqNo:866; depth 9.23 2021-02-12T23:06:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171213.75 epoch sec; seqNo:866; 4056196.48 north; 590434.14 east 2021-02-12T23:06:54Z- AHRS_M2 msg: 1613171214.17 epoch sec; seqNo:862 2021-02-12T23:06:54Z- AHRS_M2 msg: 0.65 phi; -0.29 theta; -0.09 psi 2021-02-12T23:06:54Z- handleDepth()-Depth_Keller msg: 1613171214183 epoch sec; seqNo:867; depth 9.31 2021-02-12T23:06:54Z- handleDvl() - RDI_Pathfinder msg: 1613171214.20 epoch sec; seqNo:369 2021-02-12T23:06:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.47 , 96.64 , 72.76 , 107.59 2021-02-12T23:06:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.09 , 0.05 2021-02-12T23:06:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171214.24 epoch sec; seqNo:867; 4056196.86 north; 590434.13 east 2021-02-12T23:06:54Z- AHRS_M2 msg: 1613171214.54 epoch sec; seqNo:863 2021-02-12T23:06:54Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.11 psi 2021-02-12T23:06:54Z- handleDepth()-Depth_Keller msg: 1613171214545 epoch sec; seqNo:868; depth 9.42 2021-02-12T23:06:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171214.56 epoch sec; seqNo:868; 4056196.86 north; 590434.13 east 2021-02-12T23:06:54Z- AHRS_M2 msg: 1613171214.94 epoch sec; seqNo:864 2021-02-12T23:06:54Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.13 psi 2021-02-12T23:06:54Z- handleDepth()-Depth_Keller msg: 1613171214948 epoch sec; seqNo:869; depth 9.49 2021-02-12T23:06:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171214.96 epoch sec; seqNo:869; 4056197.24 north; 590434.13 east 2021-02-12T23:06:55Z- AHRS_M2 msg: 1613171215.35 epoch sec; seqNo:865 2021-02-12T23:06:55Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.16 psi 2021-02-12T23:06:55Z- handleDepth()-Depth_Keller msg: 1613171215352 epoch sec; seqNo:870; depth 9.60 2021-02-12T23:06:55Z- handleDvl() - RDI_Pathfinder msg: 1613171215.36 epoch sec; seqNo:370 2021-02-12T23:06:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.90 , 96.50 , 78.24 , 109.89 2021-02-12T23:06:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.03 , -0.16 2021-02-12T23:06:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171215.37 epoch sec; seqNo:870; 4056197.24 north; 590434.13 east 2021-02-12T23:06:55Z- AHRS_M2 msg: 1613171215.75 epoch sec; seqNo:866 2021-02-12T23:06:55Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.19 psi 2021-02-12T23:06:55Z- handleDepth()-Depth_Keller msg: 1613171215757 epoch sec; seqNo:871; depth 9.69 2021-02-12T23:06:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171215.77 epoch sec; seqNo:871; 4056197.63 north; 590435.35 east 2021-02-12T23:06:56Z- AHRS_M2 msg: 1613171216.16 epoch sec; seqNo:867 2021-02-12T23:06:56Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.20 psi 2021-02-12T23:06:56Z- handleDepth()-Depth_Keller msg: 1613171216160 epoch sec; seqNo:872; depth 9.81 2021-02-12T23:06:56Z- handleDvl() - RDI_Pathfinder msg: 1613171216.17 epoch sec; seqNo:371 2021-02-12T23:06:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.30 , 96.44 , 75.21 , 112.26 2021-02-12T23:06:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.09 , -0.17 2021-02-12T23:06:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171216.19 epoch sec; seqNo:872; 4056198.01 north; 590435.34 east 2021-02-12T23:06:56Z- AHRS_M2 msg: 1613171216.56 epoch sec; seqNo:868 2021-02-12T23:06:56Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.21 psi 2021-02-12T23:06:56Z- handleDepth()-Depth_Keller msg: 1613171216564 epoch sec; seqNo:873; depth 9.94 2021-02-12T23:06:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171216.59 epoch sec; seqNo:873; 4056198.39 north; 590435.34 east 2021-02-12T23:06:56Z- AHRS_M2 msg: 1613171216.96 epoch sec; seqNo:869 2021-02-12T23:06:56Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.21 psi 2021-02-12T23:06:56Z- handleDepth()-Depth_Keller msg: 1613171216970 epoch sec; seqNo:874; depth 10.05 2021-02-12T23:06:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171216.98 epoch sec; seqNo:874; 4056198.39 north; 590435.34 east 2021-02-12T23:06:57Z- AHRS_M2 msg: 1613171217.37 epoch sec; seqNo:870 2021-02-12T23:06:57Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.20 psi 2021-02-12T23:06:57Z- handleDepth()-Depth_Keller msg: 1613171217372 epoch sec; seqNo:875; depth 10.16 2021-02-12T23:06:57Z- handleDvl() - RDI_Pathfinder msg: 1613171217.39 epoch sec; seqNo:372 2021-02-12T23:06:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.89 , 96.50 , 73.17 , 113.54 2021-02-12T23:06:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.18 , -0.09 2021-02-12T23:06:57Z- LcmTrn::handleMessages() - handling 43 messages took 0.66 ms and 130 calls... 2021-02-12T23:06:57Z- waiting for 1613171215.16, time is 1613171217.39 2021-02-12T23:06:57Z- LcmTrn::updateTrn() equating pose 1613171217.39 and meas times 1613171217.39 2021-02-12T23:06:57Z- LcmTrn::updateTrn() >>>> pose time 1613171217.39, meas time 1613171217.39 <<<< 2021-02-12T23:06:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171217.39 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:06:57Z- TerrainNav::motionUpdate() - currEstimate = (4056198, 590435.34, 10.16) 2021-02-12T23:06:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:06:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:06:57Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171217.39 sec.; No relevant navigation data available 2021-02-12T23:06:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:06:57Z- TNavBF::mmse selected from filter -1 2021-02-12T23:06:57Z- variances([0], [2], and psi_berg) = 814.033653 833.140661 0.000000 2021-02-12T23:06:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:06:57Z- LcmTrn::publishEstimates() - MLE : 4056163.89 590385.84 10.16 2021-02-12T23:06:57Z- LcmTrn::publishEstimates() - MMSE : 4056198.39 590435.34 10.16 2021-02-12T23:06:57Z- LcmTrn::publishEstimates() - COVAR: 814.03 833.14 1.41 0.00 2021-02-12T23:06:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171217.40 epoch sec; seqNo:875; 4056198.77 north; 590435.34 east 2021-02-12T23:06:57Z- handleDepth()-Depth_Keller msg: 1613171217793 epoch sec; seqNo:876; depth 10.28 2021-02-12T23:06:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171217.81 epoch sec; seqNo:876; 4056199.15 north; 590435.33 east 2021-02-12T23:06:58Z- AHRS_M2 msg: 1613171218.18 epoch sec; seqNo:871 2021-02-12T23:06:58Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.19 psi 2021-02-12T23:06:58Z- handleDepth()-Depth_Keller msg: 1613171218186 epoch sec; seqNo:877; depth 10.42 2021-02-12T23:06:58Z- handleDvl() - RDI_Pathfinder msg: 1613171218.20 epoch sec; seqNo:373 2021-02-12T23:06:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.72 , 96.50 , 71.95 , 114.76 2021-02-12T23:06:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.22 , 0.01 2021-02-12T23:06:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171218.21 epoch sec; seqNo:877; 4056199.53 north; 590435.33 east 2021-02-12T23:06:58Z- AHRS_M2 msg: 1613171218.58 epoch sec; seqNo:872 2021-02-12T23:06:58Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.19 psi 2021-02-12T23:06:58Z- handleDepth()-Depth_Keller msg: 1613171218586 epoch sec; seqNo:878; depth 10.53 2021-02-12T23:06:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171218.60 epoch sec; seqNo:878; 4056199.90 north; 590435.32 east 2021-02-12T23:06:58Z- AHRS_M2 msg: 1613171218.98 epoch sec; seqNo:873 2021-02-12T23:06:58Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.19 psi 2021-02-12T23:06:58Z- handleDepth()-Depth_Keller msg: 1613171218988 epoch sec; seqNo:879; depth 10.67 2021-02-12T23:06:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171219.00 epoch sec; seqNo:879; 4056199.90 north; 590435.32 east 2021-02-12T23:06:59Z- AHRS_M2 msg: 1613171219.39 epoch sec; seqNo:874 2021-02-12T23:06:59Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.18 psi 2021-02-12T23:06:59Z- handleDepth()-Depth_Keller msg: 1613171219397 epoch sec; seqNo:880; depth 10.80 2021-02-12T23:06:59Z- handleDvl() - RDI_Pathfinder msg: 1613171219.41 epoch sec; seqNo:374 2021-02-12T23:06:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.67 , 96.50 , 71.54 , 115.98 2021-02-12T23:06:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.15 , 0.12 2021-02-12T23:06:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171219.42 epoch sec; seqNo:880; 4056200.28 north; 590435.32 east 2021-02-12T23:06:59Z- AHRS_M2 msg: 1613171219.79 epoch sec; seqNo:875 2021-02-12T23:06:59Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.17 psi 2021-02-12T23:06:59Z- handleDepth()-Depth_Keller msg: 1613171219796 epoch sec; seqNo:881; depth 10.94 2021-02-12T23:06:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171219.81 epoch sec; seqNo:881; 4056200.68 north; 590436.54 east 2021-02-12T23:07:00Z- AHRS_M2 msg: 1613171220.20 epoch sec; seqNo:876 2021-02-12T23:07:00Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.17 psi 2021-02-12T23:07:00Z- handleDepth()-Depth_Keller msg: 1613171220202 epoch sec; seqNo:882; depth 11.05 2021-02-12T23:07:00Z- handleDvl() - RDI_Pathfinder msg: 1613171220.21 epoch sec; seqNo:375 2021-02-12T23:07:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.44 , 70.72 , 115.91 2021-02-12T23:07:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.10 2021-02-12T23:07:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171220.22 epoch sec; seqNo:882; 4056200.68 north; 590436.54 east 2021-02-12T23:07:00Z- AHRS_M2 msg: 1613171220.60 epoch sec; seqNo:877 2021-02-12T23:07:00Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.16 psi 2021-02-12T23:07:00Z- handleDepth()-Depth_Keller msg: 1613171220604 epoch sec; seqNo:883; depth 11.19 2021-02-12T23:07:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171220.62 epoch sec; seqNo:883; 4056201.05 north; 590436.53 east 2021-02-12T23:07:01Z- AHRS_M2 msg: 1613171221.00 epoch sec; seqNo:878 2021-02-12T23:07:01Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.16 psi 2021-02-12T23:07:01Z- handleDepth()-Depth_Keller msg: 1613171221008 epoch sec; seqNo:884; depth 11.35 2021-02-12T23:07:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171221.02 epoch sec; seqNo:884; 4056201.43 north; 590436.53 east 2021-02-12T23:07:01Z- AHRS_M2 msg: 1613171221.41 epoch sec; seqNo:879 2021-02-12T23:07:01Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.15 psi 2021-02-12T23:07:01Z- handleDepth()-Depth_Keller msg: 1613171221417 epoch sec; seqNo:885; depth 11.47 2021-02-12T23:07:01Z- handleDvl() - RDI_Pathfinder msg: 1613171221.43 epoch sec; seqNo:376 2021-02-12T23:07:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.29 , 96.17 , 70.31 , 113.75 2021-02-12T23:07:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.03 2021-02-12T23:07:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171221.44 epoch sec; seqNo:885; 4056201.43 north; 590436.53 east 2021-02-12T23:07:01Z- AHRS_M2 msg: 1613171221.81 epoch sec; seqNo:880 2021-02-12T23:07:01Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.15 psi 2021-02-12T23:07:01Z- handleDepth()-Depth_Keller msg: 1613171221818 epoch sec; seqNo:886; depth 11.62 2021-02-12T23:07:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171221.83 epoch sec; seqNo:886; 4056201.81 north; 590436.52 east 2021-02-12T23:07:02Z- AHRS_M2 msg: 1613171222.22 epoch sec; seqNo:881 2021-02-12T23:07:02Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.15 psi 2021-02-12T23:07:02Z- handleDepth()-Depth_Keller msg: 1613171222220 epoch sec; seqNo:887; depth 11.76 2021-02-12T23:07:02Z- handleDvl() - RDI_Pathfinder msg: 1613171222.23 epoch sec; seqNo:377 2021-02-12T23:07:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.46 , 96.50 , 69.90 , 114.76 2021-02-12T23:07:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.17 2021-02-12T23:07:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171222.26 epoch sec; seqNo:887; 4056202.20 north; 590437.74 east 2021-02-12T23:07:02Z- LcmTrn::handleMessages() - handling 41 messages took 0.65 ms and 128 calls... 2021-02-12T23:07:02Z- waiting for 1613171220.39, time is 1613171222.26 2021-02-12T23:07:02Z- LcmTrn::updateTrn() equating pose 1613171222.26 and meas times 1613171222.26 2021-02-12T23:07:02Z- LcmTrn::updateTrn() >>>> pose time 1613171222.26, meas time 1613171222.26 <<<< 2021-02-12T23:07:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171222.26 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:07:02Z- TerrainNav::motionUpdate() - currEstimate = (4056202, 590437.74, 11.76) 2021-02-12T23:07:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:07:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:07:02Z- TNavBF::Projecting Measurements 2021-02-12T23:07:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:07:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:07:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:07:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:07:02Z- TNavFilter::4 good beams. 2021-02-12T23:07:02Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:07:02Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:07:02Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:07:02Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:07:02Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:07:02Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:07:02Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:07:02Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:02Z- TerrainMapOctree: (vehN, vehE ) = (4056202.20, 590437.74). 2021-02-12T23:07:02Z- TerrainMapOctree: Min Distance = 6325.63. 2021-02-12T23:07:02Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:07:02Z- There is no specific good beam that all particles have in common. 2021-02-12T23:07:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:07:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.64 K: 0.122 ExpectedMeasDiff: nan currMeas.covariance: 10.10 2021-02-12T23:07:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:07:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.44 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 14.90 2021-02-12T23:07:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:07:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.122 ExpectedMeasDiff: nan currMeas.covariance: 7.82 2021-02-12T23:07:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:07:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 21.07 2021-02-12T23:07:02Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:07:02Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:07:02Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:07:02Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:07:02Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:07:02Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:07:02Z- Filter 0 No beams used in subcloud update 2021-02-12T23:07:02Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:07:02Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:07:02Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 813.7 EastVariance: 833.1 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:07:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171222.26 sec. 2021-02-12T23:07:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:07:02Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:02Z- variances([0], [2], and psi_berg) = 813.692982 833.139509 0.000000 2021-02-12T23:07:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:07:02Z- LcmTrn::publishEstimates() - MLE : 4056167.70 590486.24 11.76 2021-02-12T23:07:02Z- LcmTrn::publishEstimates() - MMSE : 4056202.20 590437.74 11.76 2021-02-12T23:07:02Z- LcmTrn::publishEstimates() - COVAR: 813.69 833.14 1.11 0.00 2021-02-12T23:07:02Z- AHRS_M2 msg: 1613171222.62 epoch sec; seqNo:882 2021-02-12T23:07:02Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.15 psi 2021-02-12T23:07:02Z- handleDepth()-Depth_Keller msg: 1613171222624 epoch sec; seqNo:888; depth 11.90 2021-02-12T23:07:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171222.63 epoch sec; seqNo:888; 4056202.58 north; 590437.74 east 2021-02-12T23:07:03Z- AHRS_M2 msg: 1613171223.03 epoch sec; seqNo:883 2021-02-12T23:07:03Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.16 psi 2021-02-12T23:07:03Z- handleDepth()-Depth_Keller msg: 1613171223030 epoch sec; seqNo:889; depth 12.03 2021-02-12T23:07:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171223.04 epoch sec; seqNo:889; 4056202.58 north; 590437.74 east 2021-02-12T23:07:03Z- AHRS_M2 msg: 1613171223.43 epoch sec; seqNo:884 2021-02-12T23:07:03Z- AHRS_M2 msg: 0.70 phi; -0.36 theta; -0.16 psi 2021-02-12T23:07:03Z- handleDepth()-Depth_Keller msg: 1613171223432 epoch sec; seqNo:890; depth 12.16 2021-02-12T23:07:03Z- handleDvl() - RDI_Pathfinder msg: 1613171223.45 epoch sec; seqNo:378 2021-02-12T23:07:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.75 , 96.57 , 69.49 , 113.61 2021-02-12T23:07:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.12 , -0.19 2021-02-12T23:07:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171223.46 epoch sec; seqNo:890; 4056202.96 north; 590437.73 east 2021-02-12T23:07:03Z- AHRS_M2 msg: 1613171223.83 epoch sec; seqNo:885 2021-02-12T23:07:03Z- AHRS_M2 msg: 0.70 phi; -0.36 theta; -0.16 psi 2021-02-12T23:07:03Z- handleDepth()-Depth_Keller msg: 1613171223841 epoch sec; seqNo:891; depth 12.31 2021-02-12T23:07:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171223.85 epoch sec; seqNo:891; 4056203.34 north; 590437.73 east 2021-02-12T23:07:04Z- AHRS_M2 msg: 1613171224.23 epoch sec; seqNo:886 2021-02-12T23:07:04Z- AHRS_M2 msg: 0.70 phi; -0.36 theta; -0.17 psi 2021-02-12T23:07:04Z- handleDepth()-Depth_Keller msg: 1613171224246 epoch sec; seqNo:892; depth 12.45 2021-02-12T23:07:04Z- handleDvl() - RDI_Pathfinder msg: 1613171224.26 epoch sec; seqNo:379 2021-02-12T23:07:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.30 , 96.57 , 69.08 , 113.61 2021-02-12T23:07:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.23 , -0.08 2021-02-12T23:07:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171224.27 epoch sec; seqNo:892; 4056203.72 north; 590437.73 east 2021-02-12T23:07:04Z- AHRS_M2 msg: 1613171224.64 epoch sec; seqNo:887 2021-02-12T23:07:04Z- AHRS_M2 msg: 0.70 phi; -0.36 theta; -0.17 psi 2021-02-12T23:07:04Z- handleDepth()-Depth_Keller msg: 1613171224646 epoch sec; seqNo:893; depth 12.59 2021-02-12T23:07:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171224.66 epoch sec; seqNo:893; 4056204.10 north; 590437.72 east 2021-02-12T23:07:05Z- AHRS_M2 msg: 1613171225.06 epoch sec; seqNo:888 2021-02-12T23:07:05Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.18 psi 2021-02-12T23:07:05Z- handleDepth()-Depth_Keller msg: 1613171225075 epoch sec; seqNo:894; depth 12.75 2021-02-12T23:07:05Z- handleDvl() - RDI_Pathfinder msg: 1613171225.09 epoch sec; seqNo:380 2021-02-12T23:07:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.07 , 96.57 , 69.08 , 113.61 2021-02-12T23:07:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.22 , 0.08 2021-02-12T23:07:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171225.10 epoch sec; seqNo:894; 4056204.11 north; 590438.94 east 2021-02-12T23:07:05Z- AHRS_M2 msg: 1613171225.45 epoch sec; seqNo:889 2021-02-12T23:07:05Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.18 psi 2021-02-12T23:07:05Z- handleDepth()-Depth_Keller msg: 1613171225453 epoch sec; seqNo:895; depth 12.88 2021-02-12T23:07:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171225.46 epoch sec; seqNo:895; 4056204.49 north; 590438.94 east 2021-02-12T23:07:05Z- AHRS_M2 msg: 1613171225.85 epoch sec; seqNo:890 2021-02-12T23:07:05Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.18 psi 2021-02-12T23:07:05Z- handleDepth()-Depth_Keller msg: 1613171225857 epoch sec; seqNo:896; depth 13.02 2021-02-12T23:07:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171225.87 epoch sec; seqNo:896; 4056204.87 north; 590438.93 east 2021-02-12T23:07:06Z- AHRS_M2 msg: 1613171226.26 epoch sec; seqNo:891 2021-02-12T23:07:06Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.18 psi 2021-02-12T23:07:06Z- handleDepth()-Depth_Keller msg: 1613171226262 epoch sec; seqNo:897; depth 13.16 2021-02-12T23:07:06Z- handleDvl() - RDI_Pathfinder msg: 1613171226.27 epoch sec; seqNo:381 2021-02-12T23:07:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.26 , 96.64 , 68.67 , 113.68 2021-02-12T23:07:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.10 , 0.13 2021-02-12T23:07:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171226.28 epoch sec; seqNo:897; 4056205.25 north; 590438.93 east 2021-02-12T23:07:06Z- AHRS_M2 msg: 1613171226.66 epoch sec; seqNo:892 2021-02-12T23:07:06Z- AHRS_M2 msg: 0.69 phi; -0.38 theta; -0.18 psi 2021-02-12T23:07:06Z- handleDepth()-Depth_Keller msg: 1613171226669 epoch sec; seqNo:898; depth 13.31 2021-02-12T23:07:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171226.68 epoch sec; seqNo:898; 4056205.25 north; 590438.93 east 2021-02-12T23:07:07Z- AHRS_M2 msg: 1613171227.08 epoch sec; seqNo:893 2021-02-12T23:07:07Z- AHRS_M2 msg: 0.69 phi; -0.38 theta; -0.18 psi 2021-02-12T23:07:07Z- handleDepth()-Depth_Keller msg: 1613171227084 epoch sec; seqNo:899; depth 13.45 2021-02-12T23:07:07Z- handleDvl() - RDI_Pathfinder msg: 1613171227.10 epoch sec; seqNo:382 2021-02-12T23:07:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.67 , 96.64 , 69.49 , 113.68 2021-02-12T23:07:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , 0.03 2021-02-12T23:07:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171227.12 epoch sec; seqNo:899; 4056205.63 north; 590438.93 east 2021-02-12T23:07:07Z- AHRS_M2 msg: 1613171227.47 epoch sec; seqNo:894 2021-02-12T23:07:07Z- AHRS_M2 msg: 0.69 phi; -0.38 theta; -0.18 psi 2021-02-12T23:07:07Z- handleDepth()-Depth_Keller msg: 1613171227478 epoch sec; seqNo:900; depth 13.60 2021-02-12T23:07:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171227.49 epoch sec; seqNo:900; 4056205.63 north; 590438.93 east 2021-02-12T23:07:07Z- LcmTrn::handleMessages() - handling 44 messages took 0.75 ms and 129 calls... 2021-02-12T23:07:07Z- waiting for 1613171225.26, time is 1613171227.49 2021-02-12T23:07:07Z- LcmTrn::updateTrn() >>>> pose time 1613171227.49, meas time 1613171227.10 <<<< 2021-02-12T23:07:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171227.10 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:07:07Z- TerrainNav::motionUpdate() - currEstimate = (4056206, 590438.93, 13.60) 2021-02-12T23:07:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:07:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:07:07Z- TNavBF::Projecting Measurements 2021-02-12T23:07:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:07:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:07:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:07:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:07:07Z- TNavFilter::4 good beams. 2021-02-12T23:07:07Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:07:07Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:07:07Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:07:07Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:07:07Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:07:07Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:07:07Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:07:07Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:07Z- TerrainMapOctree: (vehN, vehE ) = (4056205.37, 590438.84). 2021-02-12T23:07:07Z- TerrainMapOctree: Min Distance = 6322.44. 2021-02-12T23:07:07Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:07:07Z- There is no specific good beam that all particles have in common. 2021-02-12T23:07:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:07:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 9.65 2021-02-12T23:07:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:07:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 14.94 2021-02-12T23:07:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:07:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 7.73 2021-02-12T23:07:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:07:07Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 20.68 2021-02-12T23:07:07Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:07:07Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:07:07Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:07:07Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:07:07Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:07:07Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:07:07Z- Filter 0 No beams used in subcloud update 2021-02-12T23:07:07Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:07:07Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:07:07Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 813.4 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:07:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171227.10 sec. 2021-02-12T23:07:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:07:07Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:07Z- variances([0], [2], and psi_berg) = 813.319801 833.138267 0.000000 2021-02-12T23:07:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:07:07Z- LcmTrn::publishEstimates() - MLE : 4056171.13 590487.43 13.60 2021-02-12T23:07:07Z- LcmTrn::publishEstimates() - MMSE : 4056205.63 590438.93 13.60 2021-02-12T23:07:07Z- LcmTrn::publishEstimates() - COVAR: 813.32 833.14 0.94 0.00 2021-02-12T23:07:07Z- AHRS_M2 msg: 1613171227.87 epoch sec; seqNo:895 2021-02-12T23:07:07Z- AHRS_M2 msg: 0.70 phi; -0.38 theta; -0.18 psi 2021-02-12T23:07:07Z- handleDepth()-Depth_Keller msg: 1613171227881 epoch sec; seqNo:901; depth 13.73 2021-02-12T23:07:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171227.89 epoch sec; seqNo:901; 4056206.01 north; 590438.92 east 2021-02-12T23:07:08Z- AHRS_M2 msg: 1613171228.28 epoch sec; seqNo:896 2021-02-12T23:07:08Z- AHRS_M2 msg: 0.70 phi; -0.38 theta; -0.18 psi 2021-02-12T23:07:08Z- handleDepth()-Depth_Keller msg: 1613171228282 epoch sec; seqNo:902; depth 13.87 2021-02-12T23:07:08Z- handleDvl() - RDI_Pathfinder msg: 1613171228.29 epoch sec; seqNo:383 2021-02-12T23:07:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.67 , 96.64 , 69.08 , 114.90 2021-02-12T23:07:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.02 , -0.07 2021-02-12T23:07:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171228.30 epoch sec; seqNo:902; 4056206.40 north; 590440.14 east 2021-02-12T23:07:08Z- AHRS_M2 msg: 1613171228.68 epoch sec; seqNo:897 2021-02-12T23:07:08Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.19 psi 2021-02-12T23:07:08Z- handleDepth()-Depth_Keller msg: 1613171228686 epoch sec; seqNo:903; depth 14.02 2021-02-12T23:07:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171228.70 epoch sec; seqNo:903; 4056206.40 north; 590440.14 east 2021-02-12T23:07:09Z- AHRS_M2 msg: 1613171229.09 epoch sec; seqNo:898 2021-02-12T23:07:09Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.19 psi 2021-02-12T23:07:09Z- handleDepth()-Depth_Keller msg: 1613171229091 epoch sec; seqNo:904; depth 14.16 2021-02-12T23:07:09Z- handleDvl() - RDI_Pathfinder msg: 1613171229.11 epoch sec; seqNo:384 2021-02-12T23:07:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.85 , 96.57 , 67.86 , 113.61 2021-02-12T23:07:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , -0.08 2021-02-12T23:07:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171229.13 epoch sec; seqNo:904; 4056206.78 north; 590440.14 east 2021-02-12T23:07:09Z- AHRS_M2 msg: 1613171229.49 epoch sec; seqNo:899 2021-02-12T23:07:09Z- AHRS_M2 msg: 0.71 phi; -0.37 theta; -0.19 psi 2021-02-12T23:07:09Z- handleDepth()-Depth_Keller msg: 1613171229495 epoch sec; seqNo:905; depth 14.31 2021-02-12T23:07:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171229.51 epoch sec; seqNo:905; 4056207.15 north; 590440.13 east 2021-02-12T23:07:09Z- AHRS_M2 msg: 1613171229.90 epoch sec; seqNo:900 2021-02-12T23:07:09Z- AHRS_M2 msg: 0.71 phi; -0.37 theta; -0.19 psi 2021-02-12T23:07:09Z- handleDepth()-Depth_Keller msg: 1613171229903 epoch sec; seqNo:906; depth 14.45 2021-02-12T23:07:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171229.91 epoch sec; seqNo:906; 4056207.15 north; 590440.13 east 2021-02-12T23:07:10Z- AHRS_M2 msg: 1613171230.30 epoch sec; seqNo:901 2021-02-12T23:07:10Z- AHRS_M2 msg: 0.71 phi; -0.37 theta; -0.18 psi 2021-02-12T23:07:10Z- handleDepth()-Depth_Keller msg: 1613171230317 epoch sec; seqNo:907; depth 14.59 2021-02-12T23:07:10Z- handleDvl() - RDI_Pathfinder msg: 1613171230.33 epoch sec; seqNo:385 2021-02-12T23:07:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.03 , 96.64 , 67.86 , 113.68 2021-02-12T23:07:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.00 , -0.01 2021-02-12T23:07:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171230.35 epoch sec; seqNo:907; 4056207.53 north; 590440.13 east 2021-02-12T23:07:10Z- AHRS_M2 msg: 1613171230.70 epoch sec; seqNo:902 2021-02-12T23:07:10Z- AHRS_M2 msg: 0.71 phi; -0.36 theta; -0.18 psi 2021-02-12T23:07:10Z- handleDepth()-Depth_Keller msg: 1613171230706 epoch sec; seqNo:908; depth 14.75 2021-02-12T23:07:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171230.72 epoch sec; seqNo:908; 4056207.93 north; 590441.35 east 2021-02-12T23:07:11Z- AHRS_M2 msg: 1613171231.10 epoch sec; seqNo:903 2021-02-12T23:07:11Z- AHRS_M2 msg: 0.72 phi; -0.36 theta; -0.17 psi 2021-02-12T23:07:11Z- handleDepth()-Depth_Keller msg: 1613171231108 epoch sec; seqNo:909; depth 14.87 2021-02-12T23:07:11Z- handleDvl() - RDI_Pathfinder msg: 1613171231.13 epoch sec; seqNo:386 2021-02-12T23:07:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.26 , 96.71 , 67.86 , 111.31 2021-02-12T23:07:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , 0.01 2021-02-12T23:07:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171231.14 epoch sec; seqNo:909; 4056207.93 north; 590441.35 east 2021-02-12T23:07:11Z- AHRS_M2 msg: 1613171231.51 epoch sec; seqNo:904 2021-02-12T23:07:11Z- AHRS_M2 msg: 0.72 phi; -0.36 theta; -0.17 psi 2021-02-12T23:07:11Z- handleDepth()-Depth_Keller msg: 1613171231512 epoch sec; seqNo:910; depth 15.02 2021-02-12T23:07:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171231.52 epoch sec; seqNo:910; 4056208.30 north; 590441.34 east 2021-02-12T23:07:11Z- AHRS_M2 msg: 1613171231.91 epoch sec; seqNo:905 2021-02-12T23:07:11Z- AHRS_M2 msg: 0.72 phi; -0.36 theta; -0.17 psi 2021-02-12T23:07:11Z- handleDepth()-Depth_Keller msg: 1613171231916 epoch sec; seqNo:911; depth 15.14 2021-02-12T23:07:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171231.92 epoch sec; seqNo:911; 4056208.68 north; 590441.34 east 2021-02-12T23:07:12Z- AHRS_M2 msg: 1613171232.32 epoch sec; seqNo:906 2021-02-12T23:07:12Z- AHRS_M2 msg: 0.73 phi; -0.36 theta; -0.16 psi 2021-02-12T23:07:12Z- handleDepth()-Depth_Keller msg: 1613171232321 epoch sec; seqNo:912; depth 15.28 2021-02-12T23:07:12Z- handleDvl() - RDI_Pathfinder msg: 1613171232.33 epoch sec; seqNo:387 2021-02-12T23:07:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.85 , 96.71 , 67.86 , 108.88 2021-02-12T23:07:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.01 , -0.03 2021-02-12T23:07:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171232.34 epoch sec; seqNo:912; 4056208.68 north; 590441.34 east 2021-02-12T23:07:12Z- LcmTrn::handleMessages() - handling 41 messages took 0.67 ms and 128 calls... 2021-02-12T23:07:12Z- waiting for 1613171230.49, time is 1613171232.34 2021-02-12T23:07:12Z- LcmTrn::updateTrn() equating pose 1613171232.34 and meas times 1613171232.34 2021-02-12T23:07:12Z- LcmTrn::updateTrn() >>>> pose time 1613171232.34, meas time 1613171232.34 <<<< 2021-02-12T23:07:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171232.34 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:07:12Z- TerrainNav::motionUpdate() - currEstimate = (4056209, 590441.34, 15.28) 2021-02-12T23:07:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:07:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:07:12Z- TNavBF::Projecting Measurements 2021-02-12T23:07:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:07:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:07:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:07:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:07:12Z- TNavFilter::4 good beams. 2021-02-12T23:07:12Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:07:12Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:07:12Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:07:12Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:07:12Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:07:12Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:07:12Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:07:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:12Z- TerrainMapOctree: (vehN, vehE ) = (4056208.68, 590441.34). 2021-02-12T23:07:12Z- TerrainMapOctree: Min Distance = 6318.29. 2021-02-12T23:07:12Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:07:12Z- There is no specific good beam that all particles have in common. 2021-02-12T23:07:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:07:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 9.45 2021-02-12T23:07:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:07:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 14.96 2021-02-12T23:07:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:07:12Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 7.37 2021-02-12T23:07:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:07:12Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 18.97 2021-02-12T23:07:12Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:07:12Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:07:12Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:07:12Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:07:12Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:07:12Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:07:12Z- Filter 0 No beams used in subcloud update 2021-02-12T23:07:12Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:07:12Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:07:12Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 813.0 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:07:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171232.34 sec. 2021-02-12T23:07:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:07:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:12Z- variances([0], [2], and psi_berg) = 813.009461 833.137247 0.000000 2021-02-12T23:07:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:07:12Z- LcmTrn::publishEstimates() - MLE : 4056174.18 590489.84 15.28 2021-02-12T23:07:12Z- LcmTrn::publishEstimates() - MMSE : 4056208.68 590441.34 15.28 2021-02-12T23:07:12Z- LcmTrn::publishEstimates() - COVAR: 813.01 833.14 0.84 0.00 2021-02-12T23:07:12Z- handleDepth()-Depth_Keller msg: 1613171232737 epoch sec; seqNo:913; depth 15.43 2021-02-12T23:07:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171232.76 epoch sec; seqNo:913; 4056209.07 north; 590442.55 east 2021-02-12T23:07:13Z- AHRS_M2 msg: 1613171233.13 epoch sec; seqNo:907 2021-02-12T23:07:13Z- AHRS_M2 msg: 0.73 phi; -0.35 theta; -0.17 psi 2021-02-12T23:07:13Z- handleDepth()-Depth_Keller msg: 1613171233133 epoch sec; seqNo:914; depth 15.56 2021-02-12T23:07:13Z- handleDvl() - RDI_Pathfinder msg: 1613171233.15 epoch sec; seqNo:388 2021-02-12T23:07:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.03 , 95.56 , 67.04 , 107.73 2021-02-12T23:07:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.02 , -0.10 2021-02-12T23:07:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171233.16 epoch sec; seqNo:914; 4056209.45 north; 590442.55 east 2021-02-12T23:07:13Z- AHRS_M2 msg: 1613171233.53 epoch sec; seqNo:908 2021-02-12T23:07:13Z- AHRS_M2 msg: 0.73 phi; -0.36 theta; -0.18 psi 2021-02-12T23:07:13Z- handleDepth()-Depth_Keller msg: 1613171233537 epoch sec; seqNo:915; depth 15.71 2021-02-12T23:07:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171233.55 epoch sec; seqNo:915; 4056209.83 north; 590442.55 east 2021-02-12T23:07:13Z- AHRS_M2 msg: 1613171233.93 epoch sec; seqNo:909 2021-02-12T23:07:13Z- AHRS_M2 msg: 0.73 phi; -0.36 theta; -0.18 psi 2021-02-12T23:07:13Z- handleDepth()-Depth_Keller msg: 1613171233938 epoch sec; seqNo:916; depth 15.83 2021-02-12T23:07:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171233.95 epoch sec; seqNo:916; 4056210.21 north; 590442.54 east 2021-02-12T23:07:14Z- AHRS_M2 msg: 1613171234.34 epoch sec; seqNo:910 2021-02-12T23:07:14Z- AHRS_M2 msg: 0.73 phi; -0.36 theta; -0.19 psi 2021-02-12T23:07:14Z- handleDepth()-Depth_Keller msg: 1613171234340 epoch sec; seqNo:917; depth 15.97 2021-02-12T23:07:14Z- handleDvl() - RDI_Pathfinder msg: 1613171234.36 epoch sec; seqNo:389 2021-02-12T23:07:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 95.62 , 67.45 , 105.36 2021-02-12T23:07:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.07 , -0.13 2021-02-12T23:07:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171234.37 epoch sec; seqNo:917; 4056210.21 north; 590442.54 east 2021-02-12T23:07:14Z- AHRS_M2 msg: 1613171234.74 epoch sec; seqNo:911 2021-02-12T23:07:14Z- AHRS_M2 msg: 0.73 phi; -0.36 theta; -0.19 psi 2021-02-12T23:07:14Z- handleDepth()-Depth_Keller msg: 1613171234744 epoch sec; seqNo:918; depth 16.12 2021-02-12T23:07:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171234.76 epoch sec; seqNo:918; 4056210.60 north; 590443.76 east 2021-02-12T23:07:15Z- AHRS_M2 msg: 1613171235.15 epoch sec; seqNo:912 2021-02-12T23:07:15Z- AHRS_M2 msg: 0.73 phi; -0.36 theta; -0.19 psi 2021-02-12T23:07:15Z- handleDepth()-Depth_Keller msg: 1613171235150 epoch sec; seqNo:919; depth 16.24 2021-02-12T23:07:15Z- handleDvl() - RDI_Pathfinder msg: 1613171235.16 epoch sec; seqNo:390 2021-02-12T23:07:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.40 , 96.91 , 67.04 , 109.08 2021-02-12T23:07:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.09 , -0.12 2021-02-12T23:07:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171235.17 epoch sec; seqNo:919; 4056210.98 north; 590443.76 east 2021-02-12T23:07:15Z- AHRS_M2 msg: 1613171235.55 epoch sec; seqNo:913 2021-02-12T23:07:15Z- AHRS_M2 msg: 0.72 phi; -0.36 theta; -0.20 psi 2021-02-12T23:07:15Z- handleDepth()-Depth_Keller msg: 1613171235555 epoch sec; seqNo:920; depth 16.39 2021-02-12T23:07:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171235.57 epoch sec; seqNo:920; 4056210.98 north; 590443.76 east 2021-02-12T23:07:15Z- AHRS_M2 msg: 1613171235.96 epoch sec; seqNo:914 2021-02-12T23:07:15Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.20 psi 2021-02-12T23:07:15Z- handleDepth()-Depth_Keller msg: 1613171235962 epoch sec; seqNo:921; depth 16.52 2021-02-12T23:07:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171235.98 epoch sec; seqNo:921; 4056211.36 north; 590443.75 east 2021-02-12T23:07:16Z- AHRS_M2 msg: 1613171236.36 epoch sec; seqNo:915 2021-02-12T23:07:16Z- AHRS_M2 msg: 0.71 phi; -0.37 theta; -0.20 psi 2021-02-12T23:07:16Z- handleDepth()-Depth_Keller msg: 1613171236365 epoch sec; seqNo:922; depth 16.66 2021-02-12T23:07:16Z- handleDvl() - RDI_Pathfinder msg: 1613171236.38 epoch sec; seqNo:391 2021-02-12T23:07:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.40 , 96.84 , 66.22 , 109.01 2021-02-12T23:07:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , -0.06 2021-02-12T23:07:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171236.40 epoch sec; seqNo:922; 4056211.74 north; 590443.75 east 2021-02-12T23:07:16Z- AHRS_M2 msg: 1613171236.76 epoch sec; seqNo:916 2021-02-12T23:07:16Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.21 psi 2021-02-12T23:07:16Z- handleDepth()-Depth_Keller msg: 1613171236764 epoch sec; seqNo:923; depth 16.80 2021-02-12T23:07:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171236.78 epoch sec; seqNo:923; 4056212.12 north; 590443.74 east 2021-02-12T23:07:17Z- AHRS_M2 msg: 1613171237.16 epoch sec; seqNo:917 2021-02-12T23:07:17Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.22 psi 2021-02-12T23:07:17Z- handleDepth()-Depth_Keller msg: 1613171237170 epoch sec; seqNo:924; depth 16.91 2021-02-12T23:07:17Z- handleDvl() - RDI_Pathfinder msg: 1613171237.18 epoch sec; seqNo:392 2021-02-12T23:07:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.17 , 96.84 , 66.63 , 109.01 2021-02-12T23:07:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.07 , 0.02 2021-02-12T23:07:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171237.19 epoch sec; seqNo:924; 4056212.12 north; 590443.74 east 2021-02-12T23:07:17Z- AHRS_M2 msg: 1613171237.57 epoch sec; seqNo:918 2021-02-12T23:07:17Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.22 psi 2021-02-12T23:07:17Z- handleDepth()-Depth_Keller msg: 1613171237591 epoch sec; seqNo:925; depth 17.08 2021-02-12T23:07:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171237.60 epoch sec; seqNo:925; 4056212.51 north; 590444.96 east 2021-02-12T23:07:17Z- LcmTrn::handleMessages() - handling 43 messages took 0.67 ms and 130 calls... 2021-02-12T23:07:17Z- waiting for 1613171235.34, time is 1613171237.60 2021-02-12T23:07:17Z- LcmTrn::updateTrn() >>>> pose time 1613171237.60, meas time 1613171237.18 <<<< 2021-02-12T23:07:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171237.18 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:07:17Z- TerrainNav::motionUpdate() - currEstimate = (4056213, 590444.96, 17.08) 2021-02-12T23:07:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:07:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:07:17Z- TNavBF::Projecting Measurements 2021-02-12T23:07:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:07:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:07:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:07:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:07:17Z- TNavFilter::4 good beams. 2021-02-12T23:07:17Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:07:17Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:07:17Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:07:17Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:07:17Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:07:17Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:07:17Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:07:17Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:17Z- TerrainMapOctree: (vehN, vehE ) = (4056212.21, 590444.67). 2021-02-12T23:07:17Z- TerrainMapOctree: Min Distance = 6313.47. 2021-02-12T23:07:17Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:07:17Z- There is no specific good beam that all particles have in common. 2021-02-12T23:07:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:07:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 8.57 2021-02-12T23:07:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:07:17Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 15.01 2021-02-12T23:07:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:07:17Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 7.10 2021-02-12T23:07:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:07:17Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 19.01 2021-02-12T23:07:17Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:07:17Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:07:17Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:07:17Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:07:17Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:07:17Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:07:17Z- Filter 0 No beams used in subcloud update 2021-02-12T23:07:17Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:07:17Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:07:17Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 812.7 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:07:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171237.18 sec. 2021-02-12T23:07:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:07:17Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:17Z- variances([0], [2], and psi_berg) = 812.565972 833.135812 0.000000 2021-02-12T23:07:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:07:17Z- LcmTrn::publishEstimates() - MLE : 4056178.01 590493.46 17.08 2021-02-12T23:07:17Z- LcmTrn::publishEstimates() - MMSE : 4056212.51 590444.96 17.08 2021-02-12T23:07:17Z- LcmTrn::publishEstimates() - COVAR: 812.57 833.14 0.83 0.00 2021-02-12T23:07:17Z- AHRS_M2 msg: 1613171237.97 epoch sec; seqNo:919 2021-02-12T23:07:17Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.23 psi 2021-02-12T23:07:17Z- handleDepth()-Depth_Keller msg: 1613171237976 epoch sec; seqNo:926; depth 17.20 2021-02-12T23:07:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171237.98 epoch sec; seqNo:926; 4056212.51 north; 590444.96 east 2021-02-12T23:07:18Z- AHRS_M2 msg: 1613171238.38 epoch sec; seqNo:920 2021-02-12T23:07:18Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.24 psi 2021-02-12T23:07:18Z- handleDepth()-Depth_Keller msg: 1613171238380 epoch sec; seqNo:927; depth 17.34 2021-02-12T23:07:18Z- handleDvl() - RDI_Pathfinder msg: 1613171238.39 epoch sec; seqNo:393 2021-02-12T23:07:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.76 , 96.84 , 65.81 , 109.01 2021-02-12T23:07:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , 0.09 2021-02-12T23:07:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171238.40 epoch sec; seqNo:927; 4056212.89 north; 590444.96 east 2021-02-12T23:07:18Z- AHRS_M2 msg: 1613171238.78 epoch sec; seqNo:921 2021-02-12T23:07:18Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.24 psi 2021-02-12T23:07:18Z- handleDepth()-Depth_Keller msg: 1613171238792 epoch sec; seqNo:928; depth 17.48 2021-02-12T23:07:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171238.80 epoch sec; seqNo:928; 4056213.27 north; 590444.95 east 2021-02-12T23:07:19Z- AHRS_M2 msg: 1613171239.18 epoch sec; seqNo:922 2021-02-12T23:07:19Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.24 psi 2021-02-12T23:07:19Z- handleDepth()-Depth_Keller msg: 1613171239188 epoch sec; seqNo:929; depth 17.64 2021-02-12T23:07:19Z- handleDvl() - RDI_Pathfinder msg: 1613171239.20 epoch sec; seqNo:394 2021-02-12T23:07:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.76 , 96.91 , 67.86 , 110.30 2021-02-12T23:07:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.10 2021-02-12T23:07:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171239.22 epoch sec; seqNo:929; 4056213.27 north; 590444.95 east 2021-02-12T23:07:19Z- AHRS_M2 msg: 1613171239.60 epoch sec; seqNo:923 2021-02-12T23:07:19Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.24 psi 2021-02-12T23:07:19Z- handleDepth()-Depth_Keller msg: 1613171239605 epoch sec; seqNo:930; depth 17.79 2021-02-12T23:07:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171239.64 epoch sec; seqNo:930; 4056213.65 north; 590444.95 east 2021-02-12T23:07:19Z- AHRS_M2 msg: 1613171239.99 epoch sec; seqNo:924 2021-02-12T23:07:19Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.24 psi 2021-02-12T23:07:20Z- handleDepth()-Depth_Keller msg: 1613171239998 epoch sec; seqNo:931; depth 17.92 2021-02-12T23:07:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171240.01 epoch sec; seqNo:931; 4056214.03 north; 590444.94 east 2021-02-12T23:07:20Z- AHRS_M2 msg: 1613171240.40 epoch sec; seqNo:925 2021-02-12T23:07:20Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.24 psi 2021-02-12T23:07:20Z- handleDepth()-Depth_Keller msg: 1613171240410 epoch sec; seqNo:932; depth 18.07 2021-02-12T23:07:20Z- handleDvl() - RDI_Pathfinder msg: 1613171240.42 epoch sec; seqNo:395 2021-02-12T23:07:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.94 , 96.84 , 67.45 , 110.23 2021-02-12T23:07:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , 0.05 2021-02-12T23:07:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171240.43 epoch sec; seqNo:932; 4056214.04 north; 590446.17 east 2021-02-12T23:07:20Z- AHRS_M2 msg: 1613171240.80 epoch sec; seqNo:926 2021-02-12T23:07:20Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.23 psi 2021-02-12T23:07:20Z- handleDepth()-Depth_Keller msg: 1613171240809 epoch sec; seqNo:933; depth 18.22 2021-02-12T23:07:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171240.82 epoch sec; seqNo:933; 4056214.42 north; 590446.16 east 2021-02-12T23:07:21Z- AHRS_M2 msg: 1613171241.20 epoch sec; seqNo:927 2021-02-12T23:07:21Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.23 psi 2021-02-12T23:07:21Z- handleDepth()-Depth_Keller msg: 1613171241208 epoch sec; seqNo:934; depth 18.35 2021-02-12T23:07:21Z- handleDvl() - RDI_Pathfinder msg: 1613171241.22 epoch sec; seqNo:396 2021-02-12T23:07:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.53 , 96.84 , 66.63 , 110.23 2021-02-12T23:07:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , -0.03 2021-02-12T23:07:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171241.23 epoch sec; seqNo:934; 4056214.80 north; 590446.16 east 2021-02-12T23:07:21Z- AHRS_M2 msg: 1613171241.61 epoch sec; seqNo:928 2021-02-12T23:07:21Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.23 psi 2021-02-12T23:07:21Z- handleDepth()-Depth_Keller msg: 1613171241619 epoch sec; seqNo:935; depth 18.50 2021-02-12T23:07:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171241.63 epoch sec; seqNo:935; 4056214.80 north; 590446.16 east 2021-02-12T23:07:22Z- AHRS_M2 msg: 1613171242.01 epoch sec; seqNo:929 2021-02-12T23:07:22Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.23 psi 2021-02-12T23:07:22Z- handleDepth()-Depth_Keller msg: 1613171242020 epoch sec; seqNo:936; depth 18.64 2021-02-12T23:07:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171242.05 epoch sec; seqNo:936; 4056215.17 north; 590446.15 east 2021-02-12T23:07:22Z- AHRS_M2 msg: 1613171242.42 epoch sec; seqNo:930 2021-02-12T23:07:22Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.23 psi 2021-02-12T23:07:22Z- handleDepth()-Depth_Keller msg: 1613171242420 epoch sec; seqNo:937; depth 18.78 2021-02-12T23:07:22Z- handleDvl() - RDI_Pathfinder msg: 1613171242.43 epoch sec; seqNo:397 2021-02-12T23:07:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 96.84 , 65.81 , 109.01 2021-02-12T23:07:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , -0.08 2021-02-12T23:07:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171242.46 epoch sec; seqNo:937; 4056215.55 north; 590446.15 east 2021-02-12T23:07:22Z- LcmTrn::handleMessages() - handling 41 messages took 0.67 ms and 128 calls... 2021-02-12T23:07:22Z- waiting for 1613171240.60, time is 1613171242.46 2021-02-12T23:07:22Z- LcmTrn::updateTrn() equating pose 1613171242.46 and meas times 1613171242.46 2021-02-12T23:07:22Z- LcmTrn::updateTrn() >>>> pose time 1613171242.46, meas time 1613171242.46 <<<< 2021-02-12T23:07:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171242.46 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:07:22Z- TerrainNav::motionUpdate() - currEstimate = (4056216, 590446.15, 18.78) 2021-02-12T23:07:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:07:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:07:22Z- TNavBF::Projecting Measurements 2021-02-12T23:07:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:07:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:07:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:07:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:07:22Z- TNavFilter::4 good beams. 2021-02-12T23:07:22Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:07:22Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:07:22Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:07:22Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:07:22Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:07:22Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:07:22Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:07:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:22Z- TerrainMapOctree: (vehN, vehE ) = (4056215.55, 590446.15). 2021-02-12T23:07:22Z- TerrainMapOctree: Min Distance = 6309.91. 2021-02-12T23:07:22Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:07:22Z- There is no specific good beam that all particles have in common. 2021-02-12T23:07:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:07:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-12T23:07:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:07:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 15.01 2021-02-12T23:07:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:07:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 6.93 2021-02-12T23:07:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:07:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 19.01 2021-02-12T23:07:22Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:07:22Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:07:22Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:07:22Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:07:22Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:07:22Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:07:22Z- Filter 0 No beams used in subcloud update 2021-02-12T23:07:22Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:07:22Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:07:22Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 812.3 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:07:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171242.46 sec. 2021-02-12T23:07:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:07:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:22Z- variances([0], [2], and psi_berg) = 812.287475 833.134922 0.000000 2021-02-12T23:07:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:07:22Z- LcmTrn::publishEstimates() - MLE : 4056182.05 590396.65 18.78 2021-02-12T23:07:22Z- LcmTrn::publishEstimates() - MMSE : 4056215.55 590446.15 18.78 2021-02-12T23:07:22Z- LcmTrn::publishEstimates() - COVAR: 812.29 833.13 0.75 0.00 2021-02-12T23:07:22Z- AHRS_M2 msg: 1613171242.82 epoch sec; seqNo:931 2021-02-12T23:07:22Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.23 psi 2021-02-12T23:07:22Z- handleDepth()-Depth_Keller msg: 1613171242824 epoch sec; seqNo:938; depth 18.93 2021-02-12T23:07:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171242.84 epoch sec; seqNo:938; 4056215.95 north; 590447.37 east 2021-02-12T23:07:23Z- AHRS_M2 msg: 1613171243.23 epoch sec; seqNo:932 2021-02-12T23:07:23Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.22 psi 2021-02-12T23:07:23Z- handleDepth()-Depth_Keller msg: 1613171243230 epoch sec; seqNo:939; depth 19.07 2021-02-12T23:07:23Z- handleDvl() - RDI_Pathfinder msg: 1613171243.24 epoch sec; seqNo:398 2021-02-12T23:07:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 95.69 , 65.81 , 107.86 2021-02-12T23:07:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.06 2021-02-12T23:07:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171243.25 epoch sec; seqNo:939; 4056215.95 north; 590447.37 east 2021-02-12T23:07:23Z- AHRS_M2 msg: 1613171243.63 epoch sec; seqNo:933 2021-02-12T23:07:23Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.22 psi 2021-02-12T23:07:23Z- handleDepth()-Depth_Keller msg: 1613171243632 epoch sec; seqNo:940; depth 19.22 2021-02-12T23:07:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171243.65 epoch sec; seqNo:940; 4056216.32 north; 590447.36 east 2021-02-12T23:07:24Z- AHRS_M2 msg: 1613171244.03 epoch sec; seqNo:934 2021-02-12T23:07:24Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.22 psi 2021-02-12T23:07:24Z- handleDepth()-Depth_Keller msg: 1613171244041 epoch sec; seqNo:941; depth 19.35 2021-02-12T23:07:24Z- handleDvl() - RDI_Pathfinder msg: 1613171244.05 epoch sec; seqNo:399 2021-02-12T23:07:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.53 , 94.54 , 66.22 , 107.93 2021-02-12T23:07:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , -0.03 2021-02-12T23:07:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171244.07 epoch sec; seqNo:941; 4056216.70 north; 590447.36 east 2021-02-12T23:07:24Z- AHRS_M2 msg: 1613171244.44 epoch sec; seqNo:935 2021-02-12T23:07:24Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.21 psi 2021-02-12T23:07:24Z- handleDepth()-Depth_Keller msg: 1613171244440 epoch sec; seqNo:942; depth 19.51 2021-02-12T23:07:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171244.45 epoch sec; seqNo:942; 4056217.08 north; 590447.35 east 2021-02-12T23:07:24Z- AHRS_M2 msg: 1613171244.84 epoch sec; seqNo:936 2021-02-12T23:07:24Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.21 psi 2021-02-12T23:07:24Z- handleDepth()-Depth_Keller msg: 1613171244846 epoch sec; seqNo:943; depth 19.65 2021-02-12T23:07:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171244.85 epoch sec; seqNo:943; 4056217.46 north; 590447.35 east 2021-02-12T23:07:25Z- AHRS_M2 msg: 1613171245.25 epoch sec; seqNo:937 2021-02-12T23:07:25Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.21 psi 2021-02-12T23:07:25Z- handleDepth()-Depth_Keller msg: 1613171245254 epoch sec; seqNo:944; depth 19.81 2021-02-12T23:07:25Z- handleDvl() - RDI_Pathfinder msg: 1613171245.27 epoch sec; seqNo:400 2021-02-12T23:07:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.53 , 92.11 , 65.81 , 105.50 2021-02-12T23:07:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , -0.01 2021-02-12T23:07:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171245.29 epoch sec; seqNo:944; 4056217.46 north; 590447.35 east 2021-02-12T23:07:25Z- AHRS_M2 msg: 1613171245.65 epoch sec; seqNo:938 2021-02-12T23:07:25Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.20 psi 2021-02-12T23:07:25Z- handleDepth()-Depth_Keller msg: 1613171245651 epoch sec; seqNo:945; depth 19.95 2021-02-12T23:07:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171245.66 epoch sec; seqNo:945; 4056217.85 north; 590448.57 east 2021-02-12T23:07:26Z- AHRS_M2 msg: 1613171246.05 epoch sec; seqNo:939 2021-02-12T23:07:26Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.19 psi 2021-02-12T23:07:26Z- handleDepth()-Depth_Keller msg: 1613171246057 epoch sec; seqNo:946; depth 20.10 2021-02-12T23:07:26Z- handleDvl() - RDI_Pathfinder msg: 1613171246.07 epoch sec; seqNo:401 2021-02-12T23:07:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 91.03 , 64.58 , 97.11 2021-02-12T23:07:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , -0.07 2021-02-12T23:07:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171246.08 epoch sec; seqNo:946; 4056218.23 north; 590448.56 east 2021-02-12T23:07:26Z- AHRS_M2 msg: 1613171246.45 epoch sec; seqNo:940 2021-02-12T23:07:26Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.19 psi 2021-02-12T23:07:26Z- handleDepth()-Depth_Keller msg: 1613171246475 epoch sec; seqNo:947; depth 20.26 2021-02-12T23:07:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171246.49 epoch sec; seqNo:947; 4056218.61 north; 590448.56 east 2021-02-12T23:07:26Z- AHRS_M2 msg: 1613171246.86 epoch sec; seqNo:941 2021-02-12T23:07:26Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.18 psi 2021-02-12T23:07:26Z- handleDepth()-Depth_Keller msg: 1613171246868 epoch sec; seqNo:948; depth 20.40 2021-02-12T23:07:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171246.88 epoch sec; seqNo:948; 4056218.99 north; 590448.56 east 2021-02-12T23:07:27Z- AHRS_M2 msg: 1613171247.27 epoch sec; seqNo:942 2021-02-12T23:07:27Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.16 psi 2021-02-12T23:07:27Z- handleDepth()-Depth_Keller msg: 1613171247274 epoch sec; seqNo:949; depth 20.54 2021-02-12T23:07:27Z- handleDvl() - RDI_Pathfinder msg: 1613171247.29 epoch sec; seqNo:402 2021-02-12T23:07:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 90.28 , 64.58 , 104.35 2021-02-12T23:07:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , -0.12 2021-02-12T23:07:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171247.30 epoch sec; seqNo:949; 4056218.99 north; 590448.56 east 2021-02-12T23:07:27Z- AHRS_M2 msg: 1613171247.70 epoch sec; seqNo:943 2021-02-12T23:07:27Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.15 psi 2021-02-12T23:07:27Z- handleDepth()-Depth_Keller msg: 1613171247701 epoch sec; seqNo:950; depth 20.70 2021-02-12T23:07:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171247.71 epoch sec; seqNo:950; 4056219.37 north; 590448.55 east 2021-02-12T23:07:27Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-12T23:07:27Z- waiting for 1613171245.46, time is 1613171247.71 2021-02-12T23:07:27Z- LcmTrn::updateTrn() >>>> pose time 1613171247.71, meas time 1613171247.29 <<<< 2021-02-12T23:07:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171247.29 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:07:27Z- TerrainNav::motionUpdate() - currEstimate = (4056219, 590448.55, 20.70) 2021-02-12T23:07:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:07:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:07:27Z- TNavBF::Projecting Measurements 2021-02-12T23:07:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:07:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:07:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:07:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:07:27Z- TNavFilter::4 good beams. 2021-02-12T23:07:27Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:07:27Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:07:27Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:07:27Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:07:27Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:07:27Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:07:27Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:07:27Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:27Z- TerrainMapOctree: (vehN, vehE ) = (4056219.06, 590448.36). 2021-02-12T23:07:27Z- TerrainMapOctree: Min Distance = 6305.78. 2021-02-12T23:07:27Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:07:27Z- There is no specific good beam that all particles have in common. 2021-02-12T23:07:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:07:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-12T23:07:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:07:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 13.04 2021-02-12T23:07:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:07:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 6.67 2021-02-12T23:07:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:07:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.75 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 17.42 2021-02-12T23:07:27Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:07:27Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:07:27Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:07:27Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:07:27Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:07:27Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:07:27Z- Filter 0 No beams used in subcloud update 2021-02-12T23:07:27Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:07:27Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:07:27Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 812.0 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:07:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171247.29 sec. 2021-02-12T23:07:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:07:27Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:27Z- variances([0], [2], and psi_berg) = 811.884659 833.133653 0.000000 2021-02-12T23:07:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:07:27Z- LcmTrn::publishEstimates() - MLE : 4056185.87 590497.05 20.70 2021-02-12T23:07:27Z- LcmTrn::publishEstimates() - MMSE : 4056219.37 590448.55 20.70 2021-02-12T23:07:27Z- LcmTrn::publishEstimates() - COVAR: 811.88 833.13 0.74 0.00 2021-02-12T23:07:28Z- AHRS_M2 msg: 1613171248.07 epoch sec; seqNo:944 2021-02-12T23:07:28Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.14 psi 2021-02-12T23:07:28Z- handleDepth()-Depth_Keller msg: 1613171248077 epoch sec; seqNo:951; depth 20.86 2021-02-12T23:07:28Z- handleDvl() - RDI_Pathfinder msg: 1613171248.09 epoch sec; seqNo:403 2021-02-12T23:07:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 88.66 , 64.18 , 97.18 2021-02-12T23:07:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , -0.16 2021-02-12T23:07:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171248.11 epoch sec; seqNo:951; 4056219.76 north; 590449.77 east 2021-02-12T23:07:28Z- AHRS_M2 msg: 1613171248.47 epoch sec; seqNo:945 2021-02-12T23:07:28Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.14 psi 2021-02-12T23:07:28Z- handleDepth()-Depth_Keller msg: 1613171248485 epoch sec; seqNo:952; depth 21.00 2021-02-12T23:07:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171248.49 epoch sec; seqNo:952; 4056220.14 north; 590449.76 east 2021-02-12T23:07:28Z- AHRS_M2 msg: 1613171248.88 epoch sec; seqNo:946 2021-02-12T23:07:28Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.14 psi 2021-02-12T23:07:28Z- handleDepth()-Depth_Keller msg: 1613171248888 epoch sec; seqNo:953; depth 21.14 2021-02-12T23:07:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171248.91 epoch sec; seqNo:953; 4056220.14 north; 590449.76 east 2021-02-12T23:07:29Z- AHRS_M2 msg: 1613171249.29 epoch sec; seqNo:947 2021-02-12T23:07:29Z- AHRS_M2 msg: 0.69 phi; -0.36 theta; -0.14 psi 2021-02-12T23:07:29Z- handleDepth()-Depth_Keller msg: 1613171249289 epoch sec; seqNo:954; depth 21.29 2021-02-12T23:07:29Z- handleDvl() - RDI_Pathfinder msg: 1613171249.30 epoch sec; seqNo:404 2021-02-12T23:07:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 86.02 , 63.77 , 101.17 2021-02-12T23:07:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.14 , -0.12 2021-02-12T23:07:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171249.31 epoch sec; seqNo:954; 4056220.52 north; 590449.76 east 2021-02-12T23:07:29Z- AHRS_M2 msg: 1613171249.69 epoch sec; seqNo:948 2021-02-12T23:07:29Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.14 psi 2021-02-12T23:07:29Z- handleDepth()-Depth_Keller msg: 1613171249697 epoch sec; seqNo:955; depth 21.45 2021-02-12T23:07:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171249.72 epoch sec; seqNo:955; 4056220.90 north; 590449.76 east 2021-02-12T23:07:30Z- AHRS_M2 msg: 1613171250.09 epoch sec; seqNo:949 2021-02-12T23:07:30Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.14 psi 2021-02-12T23:07:30Z- handleDepth()-Depth_Keller msg: 1613171250096 epoch sec; seqNo:956; depth 21.56 2021-02-12T23:07:30Z- handleDvl() - RDI_Pathfinder msg: 1613171250.11 epoch sec; seqNo:405 2021-02-12T23:07:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.72 , 87.10 , 63.36 , 96.84 2021-02-12T23:07:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.14 , -0.03 2021-02-12T23:07:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171250.14 epoch sec; seqNo:956; 4056221.27 north; 590449.75 east 2021-02-12T23:07:30Z- AHRS_M2 msg: 1613171250.49 epoch sec; seqNo:950 2021-02-12T23:07:30Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.15 psi 2021-02-12T23:07:30Z- handleDepth()-Depth_Keller msg: 1613171250505 epoch sec; seqNo:957; depth 21.71 2021-02-12T23:07:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171250.52 epoch sec; seqNo:957; 4056221.27 north; 590449.75 east 2021-02-12T23:07:30Z- AHRS_M2 msg: 1613171250.90 epoch sec; seqNo:951 2021-02-12T23:07:30Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.15 psi 2021-02-12T23:07:30Z- handleDepth()-Depth_Keller msg: 1613171250917 epoch sec; seqNo:958; depth 21.84 2021-02-12T23:07:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171250.92 epoch sec; seqNo:958; 4056221.65 north; 590449.75 east 2021-02-12T23:07:31Z- AHRS_M2 msg: 1613171251.30 epoch sec; seqNo:952 2021-02-12T23:07:31Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.16 psi 2021-02-12T23:07:31Z- handleDepth()-Depth_Keller msg: 1613171251308 epoch sec; seqNo:959; depth 21.98 2021-02-12T23:07:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171251.32 epoch sec; seqNo:959; 4056222.05 north; 590450.97 east 2021-02-12T23:07:31Z- AHRS_M2 msg: 1613171251.71 epoch sec; seqNo:953 2021-02-12T23:07:31Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.17 psi 2021-02-12T23:07:31Z- handleDepth()-Depth_Keller msg: 1613171251713 epoch sec; seqNo:960; depth 22.10 2021-02-12T23:07:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171251.72 epoch sec; seqNo:960; 4056222.05 north; 590450.97 east 2021-02-12T23:07:32Z- AHRS_M2 msg: 1613171252.12 epoch sec; seqNo:954 2021-02-12T23:07:32Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.18 psi 2021-02-12T23:07:32Z- handleDepth()-Depth_Keller msg: 1613171252120 epoch sec; seqNo:961; depth 22.24 2021-02-12T23:07:32Z- handleDvl() - RDI_Pathfinder msg: 1613171252.13 epoch sec; seqNo:406 2021-02-12T23:07:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.49 , 87.17 , 62.54 , 101.24 2021-02-12T23:07:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , 0.12 2021-02-12T23:07:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171252.15 epoch sec; seqNo:961; 4056222.42 north; 590450.96 east 2021-02-12T23:07:32Z- AHRS_M2 msg: 1613171252.52 epoch sec; seqNo:955 2021-02-12T23:07:32Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.18 psi 2021-02-12T23:07:32Z- handleDepth()-Depth_Keller msg: 1613171252520 epoch sec; seqNo:962; depth 22.38 2021-02-12T23:07:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171252.54 epoch sec; seqNo:962; 4056222.80 north; 590450.96 east 2021-02-12T23:07:32Z- AHRS_M2 msg: 1613171252.92 epoch sec; seqNo:956 2021-02-12T23:07:32Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.19 psi 2021-02-12T23:07:32Z- handleDepth()-Depth_Keller msg: 1613171252924 epoch sec; seqNo:963; depth 22.52 2021-02-12T23:07:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171252.93 epoch sec; seqNo:963; 4056222.80 north; 590450.96 east 2021-02-12T23:07:33Z- LcmTrn::handleMessages() - handling 43 messages took 0.69 ms and 130 calls... 2021-02-12T23:07:33Z- waiting for 1613171250.71, time is 1613171252.93 2021-02-12T23:07:33Z- LcmTrn::updateTrn() >>>> pose time 1613171252.93, meas time 1613171252.13 <<<< 2021-02-12T23:07:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171252.13 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:07:33Z- TerrainNav::motionUpdate() - currEstimate = (4056223, 590450.96, 22.52) 2021-02-12T23:07:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:07:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:07:33Z- TNavBF::Projecting Measurements 2021-02-12T23:07:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:07:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:07:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:07:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:07:33Z- TNavFilter::4 good beams. 2021-02-12T23:07:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:07:33Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:07:33Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:07:33Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:07:33Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:07:33Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:07:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:07:33Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:33Z- TerrainMapOctree: (vehN, vehE ) = (4056222.28, 590450.59). 2021-02-12T23:07:33Z- TerrainMapOctree: Min Distance = 6301.87. 2021-02-12T23:07:33Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:07:33Z- There is no specific good beam that all particles have in common. 2021-02-12T23:07:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:07:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 7.73 2021-02-12T23:07:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:07:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 12.16 2021-02-12T23:07:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:07:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.79 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 6.26 2021-02-12T23:07:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:07:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.72 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 16.40 2021-02-12T23:07:33Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:07:33Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:07:33Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:07:33Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:07:33Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:07:33Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:07:33Z- Filter 0 No beams used in subcloud update 2021-02-12T23:07:33Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:07:33Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:07:33Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 811.6 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:07:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171252.13 sec. 2021-02-12T23:07:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:07:33Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:33Z- variances([0], [2], and psi_berg) = 811.479443 833.132396 0.000000 2021-02-12T23:07:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:07:33Z- LcmTrn::publishEstimates() - MLE : 4056190.30 590499.46 22.52 2021-02-12T23:07:33Z- LcmTrn::publishEstimates() - MMSE : 4056222.80 590450.96 22.52 2021-02-12T23:07:33Z- LcmTrn::publishEstimates() - COVAR: 811.48 833.13 0.73 0.00 2021-02-12T23:07:33Z- AHRS_M2 msg: 1613171253.32 epoch sec; seqNo:957 2021-02-12T23:07:33Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.19 psi 2021-02-12T23:07:33Z- handleDepth()-Depth_Keller msg: 1613171253328 epoch sec; seqNo:964; depth 22.65 2021-02-12T23:07:33Z- handleDvl() - RDI_Pathfinder msg: 1613171253.34 epoch sec; seqNo:407 2021-02-12T23:07:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.67 , 91.43 , 63.36 , 96.84 2021-02-12T23:07:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , 0.08 2021-02-12T23:07:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171253.35 epoch sec; seqNo:964; 4056223.18 north; 590450.95 east 2021-02-12T23:07:33Z- AHRS_M2 msg: 1613171253.73 epoch sec; seqNo:958 2021-02-12T23:07:33Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.19 psi 2021-02-12T23:07:33Z- handleDepth()-Depth_Keller msg: 1613171253733 epoch sec; seqNo:965; depth 22.77 2021-02-12T23:07:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171253.74 epoch sec; seqNo:965; 4056223.56 north; 590450.95 east 2021-02-12T23:07:34Z- AHRS_M2 msg: 1613171254.13 epoch sec; seqNo:959 2021-02-12T23:07:34Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.20 psi 2021-02-12T23:07:34Z- handleDepth()-Depth_Keller msg: 1613171254140 epoch sec; seqNo:966; depth 22.90 2021-02-12T23:07:34Z- handleDvl() - RDI_Pathfinder msg: 1613171254.16 epoch sec; seqNo:408 2021-02-12T23:07:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.86 , 88.18 , 62.95 , 96.84 2021-02-12T23:07:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.06 , -0.00 2021-02-12T23:07:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171254.17 epoch sec; seqNo:966; 4056223.57 north; 590452.17 east 2021-02-12T23:07:34Z- AHRS_M2 msg: 1613171254.54 epoch sec; seqNo:960 2021-02-12T23:07:34Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.20 psi 2021-02-12T23:07:34Z- handleDepth()-Depth_Keller msg: 1613171254545 epoch sec; seqNo:967; depth 23.07 2021-02-12T23:07:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171254.56 epoch sec; seqNo:967; 4056223.95 north; 590452.17 east 2021-02-12T23:07:34Z- AHRS_M2 msg: 1613171254.94 epoch sec; seqNo:961 2021-02-12T23:07:34Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.20 psi 2021-02-12T23:07:34Z- handleDepth()-Depth_Keller msg: 1613171254945 epoch sec; seqNo:968; depth 23.19 2021-02-12T23:07:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:07:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:07:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:07:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:07:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:07:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:07:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:07:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:07:34Z- handleDvl() - RDI_Pathfinder msg: 1613171254.95 epoch sec; seqNo:409 2021-02-12T23:07:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:07:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.06 , -0.00 2021-02-12T23:07:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171254.95 epoch sec; seqNo:968; 4056224.33 north; 590452.16 east 2021-02-12T23:07:35Z- AHRS_M2 msg: 1613171255.35 epoch sec; seqNo:962 2021-02-12T23:07:35Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.20 psi 2021-02-12T23:07:35Z- handleDepth()-Depth_Keller msg: 1613171255350 epoch sec; seqNo:969; depth 23.32 2021-02-12T23:07:35Z- handleDvl() - RDI_Pathfinder msg: 1613171255.36 epoch sec; seqNo:410 2021-02-12T23:07:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.04 , 89.33 , 62.13 , 96.91 2021-02-12T23:07:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.05 , -0.07 2021-02-12T23:07:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171255.37 epoch sec; seqNo:969; 4056224.71 north; 590452.16 east 2021-02-12T23:07:35Z- AHRS_M2 msg: 1613171255.75 epoch sec; seqNo:963 2021-02-12T23:07:35Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.20 psi 2021-02-12T23:07:35Z- handleDepth()-Depth_Keller msg: 1613171255752 epoch sec; seqNo:970; depth 23.45 2021-02-12T23:07:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171255.77 epoch sec; seqNo:970; 4056224.71 north; 590452.16 east 2021-02-12T23:07:36Z- AHRS_M2 msg: 1613171256.15 epoch sec; seqNo:964 2021-02-12T23:07:36Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.20 psi 2021-02-12T23:07:36Z- handleDepth()-Depth_Keller msg: 1613171256161 epoch sec; seqNo:971; depth 23.59 2021-02-12T23:07:36Z- handleDvl() - RDI_Pathfinder msg: 1613171256.18 epoch sec; seqNo:411 2021-02-12T23:07:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.81 , 89.33 , 62.54 , 97.99 2021-02-12T23:07:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , -0.10 2021-02-12T23:07:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171256.19 epoch sec; seqNo:971; 4056225.10 north; 590453.38 east 2021-02-12T23:07:36Z- AHRS_M2 msg: 1613171256.56 epoch sec; seqNo:965 2021-02-12T23:07:36Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.20 psi 2021-02-12T23:07:36Z- handleDepth()-Depth_Keller msg: 1613171256565 epoch sec; seqNo:972; depth 23.73 2021-02-12T23:07:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171256.58 epoch sec; seqNo:972; 4056225.48 north; 590453.37 east 2021-02-12T23:07:36Z- AHRS_M2 msg: 1613171256.96 epoch sec; seqNo:966 2021-02-12T23:07:36Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.20 psi 2021-02-12T23:07:36Z- handleDepth()-Depth_Keller msg: 1613171256972 epoch sec; seqNo:973; depth 23.83 2021-02-12T23:07:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171256.99 epoch sec; seqNo:973; 4056225.86 north; 590453.37 east 2021-02-12T23:07:37Z- AHRS_M2 msg: 1613171257.36 epoch sec; seqNo:967 2021-02-12T23:07:37Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.20 psi 2021-02-12T23:07:37Z- handleDepth()-Depth_Keller msg: 1613171257368 epoch sec; seqNo:974; depth 23.97 2021-02-12T23:07:37Z- handleDvl() - RDI_Pathfinder msg: 1613171257.39 epoch sec; seqNo:412 2021-02-12T23:07:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.22 , 89.33 , 61.72 , 96.91 2021-02-12T23:07:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , -0.09 2021-02-12T23:07:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171257.40 epoch sec; seqNo:974; 4056225.86 north; 590453.37 east 2021-02-12T23:07:37Z- AHRS_M2 msg: 1613171257.77 epoch sec; seqNo:968 2021-02-12T23:07:37Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.20 psi 2021-02-12T23:07:37Z- handleDepth()-Depth_Keller msg: 1613171257777 epoch sec; seqNo:975; depth 24.09 2021-02-12T23:07:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171257.79 epoch sec; seqNo:975; 4056226.24 north; 590453.36 east 2021-02-12T23:07:38Z- LcmTrn::handleMessages() - handling 42 messages took 0.62 ms and 130 calls... 2021-02-12T23:07:38Z- waiting for 1613171255.93, time is 1613171257.79 2021-02-12T23:07:38Z- LcmTrn::updateTrn() >>>> pose time 1613171257.79, meas time 1613171257.39 <<<< 2021-02-12T23:07:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171257.39 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:07:38Z- TerrainNav::motionUpdate() - currEstimate = (4056226, 590453.36, 24.09) 2021-02-12T23:07:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:07:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:07:38Z- TNavBF::Projecting Measurements 2021-02-12T23:07:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:07:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:07:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:07:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:07:38Z- TNavFilter::4 good beams. 2021-02-12T23:07:38Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:07:38Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:07:38Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:07:38Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:07:38Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:07:38Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:07:38Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:07:38Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:38Z- TerrainMapOctree: (vehN, vehE ) = (4056225.95, 590453.17). 2021-02-12T23:07:38Z- TerrainMapOctree: Min Distance = 6297.38. 2021-02-12T23:07:38Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:07:38Z- There is no specific good beam that all particles have in common. 2021-02-12T23:07:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:07:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 7.02 2021-02-12T23:07:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:07:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 12.77 2021-02-12T23:07:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:07:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.79 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 6.10 2021-02-12T23:07:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:07:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.72 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 15.03 2021-02-12T23:07:38Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:07:38Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:07:38Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:07:38Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:07:38Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:07:38Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:07:38Z- Filter 0 No beams used in subcloud update 2021-02-12T23:07:38Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:07:38Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:07:38Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 811.2 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:07:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171257.39 sec. 2021-02-12T23:07:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:07:38Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:38Z- variances([0], [2], and psi_berg) = 811.098600 833.131231 0.000000 2021-02-12T23:07:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:07:38Z- LcmTrn::publishEstimates() - MLE : 4056193.74 590501.86 24.09 2021-02-12T23:07:38Z- LcmTrn::publishEstimates() - MMSE : 4056226.24 590453.36 24.09 2021-02-12T23:07:38Z- LcmTrn::publishEstimates() - COVAR: 811.10 833.13 0.71 0.00 2021-02-12T23:07:38Z- AHRS_M2 msg: 1613171258.17 epoch sec; seqNo:969 2021-02-12T23:07:38Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.20 psi 2021-02-12T23:07:38Z- handleDepth()-Depth_Keller msg: 1613171258177 epoch sec; seqNo:976; depth 24.22 2021-02-12T23:07:38Z- handleDvl() - RDI_Pathfinder msg: 1613171258.19 epoch sec; seqNo:413 2021-02-12T23:07:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.81 , 89.33 , 61.31 , 94.75 2021-02-12T23:07:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , -0.06 2021-02-12T23:07:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171258.20 epoch sec; seqNo:976; 4056226.62 north; 590453.36 east 2021-02-12T23:07:38Z- AHRS_M2 msg: 1613171258.58 epoch sec; seqNo:970 2021-02-12T23:07:38Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.20 psi 2021-02-12T23:07:38Z- handleDepth()-Depth_Keller msg: 1613171258582 epoch sec; seqNo:977; depth 24.33 2021-02-12T23:07:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:07:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:07:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:07:38Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:07:38Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:07:38Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:07:38Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:07:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:07:38Z- handleDvl() - RDI_Pathfinder msg: 1613171258.58 epoch sec; seqNo:414 2021-02-12T23:07:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:07:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , -0.06 2021-02-12T23:07:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171258.60 epoch sec; seqNo:977; 4056226.63 north; 590454.58 east 2021-02-12T23:07:38Z- AHRS_M2 msg: 1613171258.99 epoch sec; seqNo:971 2021-02-12T23:07:38Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.20 psi 2021-02-12T23:07:38Z- handleDepth()-Depth_Keller msg: 1613171258990 epoch sec; seqNo:978; depth 24.46 2021-02-12T23:07:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171259.03 epoch sec; seqNo:978; 4056227.01 north; 590454.58 east 2021-02-12T23:07:39Z- AHRS_M2 msg: 1613171259.38 epoch sec; seqNo:972 2021-02-12T23:07:39Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.20 psi 2021-02-12T23:07:39Z- handleDepth()-Depth_Keller msg: 1613171259393 epoch sec; seqNo:979; depth 24.59 2021-02-12T23:07:39Z- handleDvl() - RDI_Pathfinder msg: 1613171259.40 epoch sec; seqNo:415 2021-02-12T23:07:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.40 , 86.09 , 61.31 , 97.99 2021-02-12T23:07:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.00 , -0.01 2021-02-12T23:07:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171259.41 epoch sec; seqNo:979; 4056227.39 north; 590454.57 east 2021-02-12T23:07:39Z- AHRS_M2 msg: 1613171259.79 epoch sec; seqNo:973 2021-02-12T23:07:39Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.20 psi 2021-02-12T23:07:39Z- handleDepth()-Depth_Keller msg: 1613171259801 epoch sec; seqNo:980; depth 24.72 2021-02-12T23:07:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:07:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:07:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:07:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:07:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:07:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:07:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:07:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:07:39Z- handleDvl() - RDI_Pathfinder msg: 1613171259.80 epoch sec; seqNo:416 2021-02-12T23:07:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:07:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.00 , -0.01 2021-02-12T23:07:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171259.81 epoch sec; seqNo:980; 4056227.77 north; 590454.57 east 2021-02-12T23:07:40Z- AHRS_M2 msg: 1613171260.19 epoch sec; seqNo:974 2021-02-12T23:07:40Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.20 psi 2021-02-12T23:07:40Z- handleDepth()-Depth_Keller msg: 1613171260206 epoch sec; seqNo:981; depth 24.85 2021-02-12T23:07:40Z- handleDvl() - RDI_Pathfinder msg: 1613171260.22 epoch sec; seqNo:417 2021-02-12T23:07:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.99 , 88.25 , 60.91 , 93.66 2021-02-12T23:07:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , -0.04 2021-02-12T23:07:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171260.23 epoch sec; seqNo:981; 4056228.15 north; 590454.57 east 2021-02-12T23:07:40Z- AHRS_M2 msg: 1613171260.60 epoch sec; seqNo:975 2021-02-12T23:07:40Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.20 psi 2021-02-12T23:07:40Z- handleDepth()-Depth_Keller msg: 1613171260605 epoch sec; seqNo:982; depth 24.96 2021-02-12T23:07:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171260.62 epoch sec; seqNo:982; 4056228.15 north; 590454.57 east 2021-02-12T23:07:41Z- AHRS_M2 msg: 1613171261.01 epoch sec; seqNo:976 2021-02-12T23:07:41Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.20 psi 2021-02-12T23:07:41Z- handleDepth()-Depth_Keller msg: 1613171261019 epoch sec; seqNo:983; depth 25.07 2021-02-12T23:07:41Z- handleDvl() - RDI_Pathfinder msg: 1613171261.06 epoch sec; seqNo:418 2021-02-12T23:07:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.58 , 86.09 , 61.31 , 92.58 2021-02-12T23:07:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , -0.04 2021-02-12T23:07:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171261.07 epoch sec; seqNo:983; 4056228.54 north; 590455.78 east 2021-02-12T23:07:41Z- AHRS_M2 msg: 1613171261.40 epoch sec; seqNo:977 2021-02-12T23:07:41Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.20 psi 2021-02-12T23:07:41Z- handleDepth()-Depth_Keller msg: 1613171261409 epoch sec; seqNo:984; depth 25.21 2021-02-12T23:07:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171261.42 epoch sec; seqNo:984; 4056228.92 north; 590455.78 east 2021-02-12T23:07:41Z- AHRS_M2 msg: 1613171261.81 epoch sec; seqNo:978 2021-02-12T23:07:41Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.20 psi 2021-02-12T23:07:41Z- handleDepth()-Depth_Keller msg: 1613171261822 epoch sec; seqNo:985; depth 25.32 2021-02-12T23:07:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171261.83 epoch sec; seqNo:985; 4056229.29 north; 590455.77 east 2021-02-12T23:07:42Z- AHRS_M2 msg: 1613171262.21 epoch sec; seqNo:979 2021-02-12T23:07:42Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.20 psi 2021-02-12T23:07:42Z- handleDepth()-Depth_Keller msg: 1613171262219 epoch sec; seqNo:986; depth 25.45 2021-02-12T23:07:42Z- handleDvl() - RDI_Pathfinder msg: 1613171262.23 epoch sec; seqNo:419 2021-02-12T23:07:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.58 , 86.09 , 60.91 , 91.50 2021-02-12T23:07:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , -0.04 2021-02-12T23:07:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171262.25 epoch sec; seqNo:986; 4056229.29 north; 590455.77 east 2021-02-12T23:07:42Z- AHRS_M2 msg: 1613171262.62 epoch sec; seqNo:980 2021-02-12T23:07:42Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.20 psi 2021-02-12T23:07:42Z- handleDepth()-Depth_Keller msg: 1613171262628 epoch sec; seqNo:987; depth 25.57 2021-02-12T23:07:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171262.67 epoch sec; seqNo:987; 4056229.67 north; 590455.77 east 2021-02-12T23:07:43Z- AHRS_M2 msg: 1613171263.02 epoch sec; seqNo:981 2021-02-12T23:07:43Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.19 psi 2021-02-12T23:07:43Z- handleDepth()-Depth_Keller msg: 1613171263025 epoch sec; seqNo:988; depth 25.68 2021-02-12T23:07:43Z- handleDvl() - RDI_Pathfinder msg: 1613171263.04 epoch sec; seqNo:420 2021-02-12T23:07:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.77 , 79.71 , 60.50 , 91.50 2021-02-12T23:07:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , -0.03 2021-02-12T23:07:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171263.05 epoch sec; seqNo:988; 4056230.05 north; 590455.77 east 2021-02-12T23:07:43Z- LcmTrn::handleMessages() - handling 47 messages took 0.68 ms and 134 calls... 2021-02-12T23:07:43Z- waiting for 1613171260.79, time is 1613171263.05 2021-02-12T23:07:43Z- LcmTrn::updateTrn() equating pose 1613171263.05 and meas times 1613171263.05 2021-02-12T23:07:43Z- LcmTrn::updateTrn() >>>> pose time 1613171263.05, meas time 1613171263.05 <<<< 2021-02-12T23:07:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171263.05 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:07:43Z- TerrainNav::motionUpdate() - currEstimate = (4056230, 590455.77, 25.68) 2021-02-12T23:07:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:07:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:07:43Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171263.05 sec.; No relevant navigation data available 2021-02-12T23:07:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:07:43Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:43Z- variances([0], [2], and psi_berg) = 810.784228 833.130281 0.000000 2021-02-12T23:07:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:07:43Z- LcmTrn::publishEstimates() - MLE : 4056197.55 590504.27 25.68 2021-02-12T23:07:43Z- LcmTrn::publishEstimates() - MMSE : 4056230.05 590455.77 25.68 2021-02-12T23:07:43Z- LcmTrn::publishEstimates() - COVAR: 810.78 833.13 0.94 0.00 2021-02-12T23:07:43Z- AHRS_M2 msg: 1613171263.42 epoch sec; seqNo:982 2021-02-12T23:07:43Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.19 psi 2021-02-12T23:07:43Z- handleDepth()-Depth_Keller msg: 1613171263428 epoch sec; seqNo:989; depth 25.81 2021-02-12T23:07:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171263.45 epoch sec; seqNo:989; 4056230.43 north; 590455.76 east 2021-02-12T23:07:43Z- AHRS_M2 msg: 1613171263.83 epoch sec; seqNo:983 2021-02-12T23:07:43Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.19 psi 2021-02-12T23:07:43Z- handleDepth()-Depth_Keller msg: 1613171263832 epoch sec; seqNo:990; depth 25.91 2021-02-12T23:07:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171263.85 epoch sec; seqNo:990; 4056230.44 north; 590456.98 east 2021-02-12T23:07:44Z- AHRS_M2 msg: 1613171264.24 epoch sec; seqNo:984 2021-02-12T23:07:44Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.19 psi 2021-02-12T23:07:44Z- handleDepth()-Depth_Keller msg: 1613171264243 epoch sec; seqNo:991; depth 26.03 2021-02-12T23:07:44Z- handleDvl() - RDI_Pathfinder msg: 1613171264.25 epoch sec; seqNo:421 2021-02-12T23:07:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.77 , 79.30 , 60.50 , 89.33 2021-02-12T23:07:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.10 , -0.01 2021-02-12T23:07:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171264.27 epoch sec; seqNo:991; 4056230.82 north; 590456.98 east 2021-02-12T23:07:44Z- AHRS_M2 msg: 1613171264.64 epoch sec; seqNo:985 2021-02-12T23:07:44Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.18 psi 2021-02-12T23:07:44Z- handleDepth()-Depth_Keller msg: 1613171264646 epoch sec; seqNo:992; depth 26.15 2021-02-12T23:07:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171264.66 epoch sec; seqNo:992; 4056231.20 north; 590456.98 east 2021-02-12T23:07:45Z- AHRS_M2 msg: 1613171265.04 epoch sec; seqNo:986 2021-02-12T23:07:45Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.18 psi 2021-02-12T23:07:45Z- handleDepth()-Depth_Keller msg: 1613171265049 epoch sec; seqNo:993; depth 26.26 2021-02-12T23:07:45Z- handleDvl() - RDI_Pathfinder msg: 1613171265.08 epoch sec; seqNo:422 2021-02-12T23:07:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.18 , 78.07 , 60.09 , 89.33 2021-02-12T23:07:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , 0.01 2021-02-12T23:07:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171265.10 epoch sec; seqNo:993; 4056231.58 north; 590456.97 east 2021-02-12T23:07:45Z- AHRS_M2 msg: 1613171265.44 epoch sec; seqNo:987 2021-02-12T23:07:45Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.18 psi 2021-02-12T23:07:45Z- handleDepth()-Depth_Keller msg: 1613171265453 epoch sec; seqNo:994; depth 26.37 2021-02-12T23:07:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171265.46 epoch sec; seqNo:994; 4056231.58 north; 590456.97 east 2021-02-12T23:07:45Z- AHRS_M2 msg: 1613171265.85 epoch sec; seqNo:988 2021-02-12T23:07:45Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.18 psi 2021-02-12T23:07:45Z- handleDepth()-Depth_Keller msg: 1613171265862 epoch sec; seqNo:995; depth 26.47 2021-02-12T23:07:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:07:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:07:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:07:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:07:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:07:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:07:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:07:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:07:45Z- handleDvl() - RDI_Pathfinder msg: 1613171265.87 epoch sec; seqNo:423 2021-02-12T23:07:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:07:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , 0.01 2021-02-12T23:07:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171265.89 epoch sec; seqNo:995; 4056231.96 north; 590456.97 east 2021-02-12T23:07:46Z- AHRS_M2 msg: 1613171266.25 epoch sec; seqNo:989 2021-02-12T23:07:46Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.17 psi 2021-02-12T23:07:46Z- handleDepth()-Depth_Keller msg: 1613171266266 epoch sec; seqNo:996; depth 26.60 2021-02-12T23:07:46Z- handleDvl() - RDI_Pathfinder msg: 1613171266.28 epoch sec; seqNo:424 2021-02-12T23:07:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.77 , 77.67 , 60.09 , 0.00 2021-02-12T23:07:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , 0.03 2021-02-12T23:07:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171266.29 epoch sec; seqNo:996; 4056232.34 north; 590456.96 east 2021-02-12T23:07:46Z- AHRS_M2 msg: 1613171266.66 epoch sec; seqNo:990 2021-02-12T23:07:46Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.17 psi 2021-02-12T23:07:46Z- handleDepth()-Depth_Keller msg: 1613171266677 epoch sec; seqNo:997; depth 26.70 2021-02-12T23:07:46Z- handleDvl() - RDI_Pathfinder msg: 1613171266.70 epoch sec; seqNo:425 2021-02-12T23:07:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.10 , 76.37 , 59.50 , 84.34 2021-02-12T23:07:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , 0.02 2021-02-12T23:07:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171266.71 epoch sec; seqNo:997; 4056232.72 north; 590456.96 east 2021-02-12T23:07:47Z- AHRS_M2 msg: 1613171267.06 epoch sec; seqNo:991 2021-02-12T23:07:47Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.17 psi 2021-02-12T23:07:47Z- handleDepth()-Depth_Keller msg: 1613171267076 epoch sec; seqNo:998; depth 26.82 2021-02-12T23:07:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171267.09 epoch sec; seqNo:998; 4056232.73 north; 590458.18 east 2021-02-12T23:07:47Z- AHRS_M2 msg: 1613171267.46 epoch sec; seqNo:992 2021-02-12T23:07:47Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.17 psi 2021-02-12T23:07:47Z- handleDepth()-Depth_Keller msg: 1613171267468 epoch sec; seqNo:999; depth 26.93 2021-02-12T23:07:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171267.48 epoch sec; seqNo:999; 4056233.11 north; 590458.18 east 2021-02-12T23:07:47Z- AHRS_M2 msg: 1613171267.87 epoch sec; seqNo:993 2021-02-12T23:07:47Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.17 psi 2021-02-12T23:07:47Z- handleDepth()-Depth_Keller msg: 1613171267872 epoch sec; seqNo:1000; depth 27.04 2021-02-12T23:07:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171267.88 epoch sec; seqNo:1000; 4056233.49 north; 590458.17 east 2021-02-12T23:07:48Z- AHRS_M2 msg: 1613171268.27 epoch sec; seqNo:994 2021-02-12T23:07:48Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.17 psi 2021-02-12T23:07:48Z- LcmTrn::handleMessages() - handling 42 messages took 0.75 ms and 127 calls... 2021-02-12T23:07:48Z- waiting for 1613171266.05, time is 1613171268.27 2021-02-12T23:07:48Z- LcmTrn::updateTrn() >>>> pose time 1613171268.27, meas time 1613171266.70 <<<< 2021-02-12T23:07:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171266.70 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:07:48Z- TerrainNav::motionUpdate() - currEstimate = (4056233, 590458.17, 27.04) 2021-02-12T23:07:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:07:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:07:48Z- TNavBF::Projecting Measurements 2021-02-12T23:07:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:07:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:07:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:07:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:07:48Z- TNavFilter::4 good beams. 2021-02-12T23:07:48Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:07:48Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:07:48Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:07:48Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:07:48Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:07:48Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:07:48Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:07:48Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:48Z- TerrainMapOctree: (vehN, vehE ) = (4056232.45, 590457.45). 2021-02-12T23:07:48Z- TerrainMapOctree: Min Distance = 6289.61. 2021-02-12T23:07:48Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:07:48Z- There is no specific good beam that all particles have in common. 2021-02-12T23:07:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:07:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.116 ExpectedMeasDiff: nan currMeas.covariance: 6.57 2021-02-12T23:07:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:07:48Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 9.33 2021-02-12T23:07:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:07:48Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 5.66 2021-02-12T23:07:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:07:48Z- Old Depth Bias: 0.00 DepthBiasCov: 0.79 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 11.38 2021-02-12T23:07:48Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:07:48Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:07:48Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:07:48Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:07:48Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:07:48Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:07:48Z- Filter 0 No beams used in subcloud update 2021-02-12T23:07:48Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:07:48Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:07:48Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 810.5 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:07:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171266.70 sec. 2021-02-12T23:07:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:07:48Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:48Z- variances([0], [2], and psi_berg) = 810.368931 833.129043 0.000000 2021-02-12T23:07:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:07:48Z- LcmTrn::publishEstimates() - MLE : 4056200.99 590506.67 27.04 2021-02-12T23:07:48Z- LcmTrn::publishEstimates() - MMSE : 4056233.49 590458.17 27.04 2021-02-12T23:07:48Z- LcmTrn::publishEstimates() - COVAR: 810.37 833.13 0.81 0.00 2021-02-12T23:07:48Z- handleDepth()-Depth_Keller msg: 1613171268276 epoch sec; seqNo:1001; depth 27.15 2021-02-12T23:07:48Z- handleDvl() - RDI_Pathfinder msg: 1613171268.29 epoch sec; seqNo:426 2021-02-12T23:07:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.36 , 75.62 , 58.86 , 89.33 2021-02-12T23:07:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.00 2021-02-12T23:07:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171268.30 epoch sec; seqNo:1001; 4056233.87 north; 590458.17 east 2021-02-12T23:07:48Z- AHRS_M2 msg: 1613171268.67 epoch sec; seqNo:995 2021-02-12T23:07:48Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.16 psi 2021-02-12T23:07:48Z- handleDepth()-Depth_Keller msg: 1613171268683 epoch sec; seqNo:1002; depth 27.27 2021-02-12T23:07:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171268.69 epoch sec; seqNo:1002; 4056233.87 north; 590458.17 east 2021-02-12T23:07:49Z- AHRS_M2 msg: 1613171269.08 epoch sec; seqNo:996 2021-02-12T23:07:49Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.16 psi 2021-02-12T23:07:49Z- handleDepth()-Depth_Keller msg: 1613171269084 epoch sec; seqNo:1003; depth 27.38 2021-02-12T23:07:49Z- handleDvl() - RDI_Pathfinder msg: 1613171269.10 epoch sec; seqNo:427 2021-02-12T23:07:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.36 , 75.62 , 58.86 , 87.17 2021-02-12T23:07:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , -0.03 2021-02-12T23:07:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171269.13 epoch sec; seqNo:1003; 4056234.25 north; 590458.16 east 2021-02-12T23:07:49Z- AHRS_M2 msg: 1613171269.48 epoch sec; seqNo:997 2021-02-12T23:07:49Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.16 psi 2021-02-12T23:07:49Z- handleDepth()-Depth_Keller msg: 1613171269488 epoch sec; seqNo:1004; depth 27.50 2021-02-12T23:07:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171269.50 epoch sec; seqNo:1004; 4056234.64 north; 590459.38 east 2021-02-12T23:07:49Z- AHRS_M2 msg: 1613171269.90 epoch sec; seqNo:998 2021-02-12T23:07:49Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.16 psi 2021-02-12T23:07:49Z- handleDepth()-Depth_Keller msg: 1613171269906 epoch sec; seqNo:1005; depth 27.60 2021-02-12T23:07:49Z- handleDvl() - RDI_Pathfinder msg: 1613171269.92 epoch sec; seqNo:428 2021-02-12T23:07:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.95 , 75.21 , 58.86 , 0.00 2021-02-12T23:07:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , -0.04 2021-02-12T23:07:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171269.93 epoch sec; seqNo:1005; 4056235.02 north; 590459.38 east 2021-02-12T23:07:50Z- AHRS_M2 msg: 1613171270.29 epoch sec; seqNo:999 2021-02-12T23:07:50Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.17 psi 2021-02-12T23:07:50Z- handleDepth()-Depth_Keller msg: 1613171270301 epoch sec; seqNo:1006; depth 27.72 2021-02-12T23:07:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171270.32 epoch sec; seqNo:1006; 4056235.39 north; 590459.37 east 2021-02-12T23:07:50Z- AHRS_M2 msg: 1613171270.70 epoch sec; seqNo:1000 2021-02-12T23:07:50Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.17 psi 2021-02-12T23:07:50Z- handleDepth()-Depth_Keller msg: 1613171270700 epoch sec; seqNo:1007; depth 27.83 2021-02-12T23:07:50Z- handleDvl() - RDI_Pathfinder msg: 1613171270.71 epoch sec; seqNo:429 2021-02-12T23:07:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.57 , 75.45 , 58.58 , 84.34 2021-02-12T23:07:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , -0.02 2021-02-12T23:07:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171270.74 epoch sec; seqNo:1007; 4056235.39 north; 590459.37 east 2021-02-12T23:07:51Z- AHRS_M2 msg: 1613171271.10 epoch sec; seqNo:1001 2021-02-12T23:07:51Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.17 psi 2021-02-12T23:07:51Z- handleDepth()-Depth_Keller msg: 1613171271104 epoch sec; seqNo:1008; depth 27.95 2021-02-12T23:07:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171271.12 epoch sec; seqNo:1008; 4056235.77 north; 590459.37 east 2021-02-12T23:07:51Z- AHRS_M2 msg: 1613171271.50 epoch sec; seqNo:1002 2021-02-12T23:07:51Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.17 psi 2021-02-12T23:07:51Z- handleDepth()-Depth_Keller msg: 1613171271510 epoch sec; seqNo:1009; depth 28.07 2021-02-12T23:07:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171271.52 epoch sec; seqNo:1009; 4056236.15 north; 590459.37 east 2021-02-12T23:07:51Z- AHRS_M2 msg: 1613171271.91 epoch sec; seqNo:1003 2021-02-12T23:07:51Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.16 psi 2021-02-12T23:07:51Z- handleDepth()-Depth_Keller msg: 1613171271916 epoch sec; seqNo:1010; depth 28.17 2021-02-12T23:07:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171271.94 epoch sec; seqNo:1010; 4056236.54 north; 590460.58 east 2021-02-12T23:07:52Z- AHRS_M2 msg: 1613171272.32 epoch sec; seqNo:1004 2021-02-12T23:07:52Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.16 psi 2021-02-12T23:07:52Z- handleDepth()-Depth_Keller msg: 1613171272322 epoch sec; seqNo:1011; depth 28.29 2021-02-12T23:07:52Z- handleDvl() - RDI_Pathfinder msg: 1613171272.34 epoch sec; seqNo:430 2021-02-12T23:07:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.72 , 74.80 , 58.04 , 87.17 2021-02-12T23:07:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , 0.00 2021-02-12T23:07:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171272.35 epoch sec; seqNo:1011; 4056236.54 north; 590460.58 east 2021-02-12T23:07:52Z- AHRS_M2 msg: 1613171272.72 epoch sec; seqNo:1005 2021-02-12T23:07:52Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.16 psi 2021-02-12T23:07:52Z- handleDepth()-Depth_Keller msg: 1613171272729 epoch sec; seqNo:1012; depth 28.41 2021-02-12T23:07:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171272.74 epoch sec; seqNo:1012; 4056236.92 north; 590460.58 east 2021-02-12T23:07:53Z- AHRS_M2 msg: 1613171273.12 epoch sec; seqNo:1006 2021-02-12T23:07:53Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.16 psi 2021-02-12T23:07:53Z- handleDepth()-Depth_Keller msg: 1613171273128 epoch sec; seqNo:1013; depth 28.51 2021-02-12T23:07:53Z- handleDvl() - RDI_Pathfinder msg: 1613171273.14 epoch sec; seqNo:431 2021-02-12T23:07:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.13 , 74.40 , 58.04 , 87.17 2021-02-12T23:07:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.04 2021-02-12T23:07:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171273.16 epoch sec; seqNo:1013; 4056237.30 north; 590460.57 east 2021-02-12T23:07:53Z- LcmTrn::handleMessages() - handling 44 messages took 0.61 ms and 132 calls... 2021-02-12T23:07:53Z- waiting for 1613171271.27, time is 1613171273.16 2021-02-12T23:07:53Z- LcmTrn::updateTrn() equating pose 1613171273.16 and meas times 1613171273.16 2021-02-12T23:07:53Z- LcmTrn::updateTrn() >>>> pose time 1613171273.16, meas time 1613171273.16 <<<< 2021-02-12T23:07:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171273.16 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:07:53Z- TerrainNav::motionUpdate() - currEstimate = (4056237, 590460.57, 28.51) 2021-02-12T23:07:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:07:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:07:53Z- TNavBF::Projecting Measurements 2021-02-12T23:07:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:07:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:07:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:07:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:07:53Z- TNavFilter::4 good beams. 2021-02-12T23:07:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:07:53Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:07:53Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:07:53Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:07:53Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:07:53Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:07:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:07:53Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:53Z- TerrainMapOctree: (vehN, vehE ) = (4056237.30, 590460.57). 2021-02-12T23:07:53Z- TerrainMapOctree: Min Distance = 6283.86. 2021-02-12T23:07:53Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:07:53Z- There is no specific good beam that all particles have in common. 2021-02-12T23:07:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:07:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.116 ExpectedMeasDiff: nan currMeas.covariance: 6.18 2021-02-12T23:07:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:07:53Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 8.86 2021-02-12T23:07:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:07:53Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 5.39 2021-02-12T23:07:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:07:53Z- Old Depth Bias: 0.00 DepthBiasCov: 0.75 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 12.16 2021-02-12T23:07:53Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:07:53Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:07:53Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:07:53Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:07:53Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:07:53Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:07:53Z- Filter 0 No beams used in subcloud update 2021-02-12T23:07:53Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:07:53Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:07:53Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 810.1 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:07:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171273.16 sec. 2021-02-12T23:07:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:07:53Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:53Z- variances([0], [2], and psi_berg) = 810.060605 833.128135 0.000000 2021-02-12T23:07:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:07:53Z- LcmTrn::publishEstimates() - MLE : 4056204.80 590509.07 28.51 2021-02-12T23:07:53Z- LcmTrn::publishEstimates() - MMSE : 4056237.30 590460.57 28.51 2021-02-12T23:07:53Z- LcmTrn::publishEstimates() - COVAR: 810.06 833.13 0.72 0.00 2021-02-12T23:07:53Z- AHRS_M2 msg: 1613171273.52 epoch sec; seqNo:1007 2021-02-12T23:07:53Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.16 psi 2021-02-12T23:07:53Z- handleDepth()-Depth_Keller msg: 1613171273533 epoch sec; seqNo:1014; depth 28.64 2021-02-12T23:07:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171273.54 epoch sec; seqNo:1014; 4056237.68 north; 590460.57 east 2021-02-12T23:07:53Z- AHRS_M2 msg: 1613171273.94 epoch sec; seqNo:1008 2021-02-12T23:07:53Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.16 psi 2021-02-12T23:07:53Z- handleDepth()-Depth_Keller msg: 1613171273948 epoch sec; seqNo:1015; depth 28.74 2021-02-12T23:07:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171273.97 epoch sec; seqNo:1015; 4056237.68 north; 590460.57 east 2021-02-12T23:07:54Z- AHRS_M2 msg: 1613171274.33 epoch sec; seqNo:1009 2021-02-12T23:07:54Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.15 psi 2021-02-12T23:07:54Z- handleDepth()-Depth_Keller msg: 1613171274336 epoch sec; seqNo:1016; depth 28.86 2021-02-12T23:07:54Z- handleDvl() - RDI_Pathfinder msg: 1613171274.35 epoch sec; seqNo:432 2021-02-12T23:07:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.91 , 73.58 , 57.64 , 87.17 2021-02-12T23:07:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.02 , 0.03 2021-02-12T23:07:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171274.36 epoch sec; seqNo:1016; 4056238.07 north; 590461.79 east 2021-02-12T23:07:54Z- AHRS_M2 msg: 1613171274.74 epoch sec; seqNo:1010 2021-02-12T23:07:54Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.15 psi 2021-02-12T23:07:54Z- handleDepth()-Depth_Keller msg: 1613171274740 epoch sec; seqNo:1017; depth 28.97 2021-02-12T23:07:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171274.76 epoch sec; seqNo:1017; 4056238.45 north; 590461.78 east 2021-02-12T23:07:55Z- AHRS_M2 msg: 1613171275.14 epoch sec; seqNo:1011 2021-02-12T23:07:55Z- AHRS_M2 msg: 0.66 phi; -0.30 theta; -0.15 psi 2021-02-12T23:07:55Z- handleDepth()-Depth_Keller msg: 1613171275147 epoch sec; seqNo:1018; depth 29.06 2021-02-12T23:07:55Z- handleDvl() - RDI_Pathfinder msg: 1613171275.16 epoch sec; seqNo:433 2021-02-12T23:07:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.32 , 73.18 , 57.23 , 87.17 2021-02-12T23:07:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.05 , -0.02 2021-02-12T23:07:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171275.17 epoch sec; seqNo:1018; 4056238.45 north; 590461.78 east 2021-02-12T23:07:55Z- AHRS_M2 msg: 1613171275.54 epoch sec; seqNo:1012 2021-02-12T23:07:55Z- AHRS_M2 msg: 0.66 phi; -0.30 theta; -0.15 psi 2021-02-12T23:07:55Z- handleDepth()-Depth_Keller msg: 1613171275550 epoch sec; seqNo:1019; depth 29.19 2021-02-12T23:07:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171275.56 epoch sec; seqNo:1019; 4056238.83 north; 590461.78 east 2021-02-12T23:07:55Z- AHRS_M2 msg: 1613171275.97 epoch sec; seqNo:1013 2021-02-12T23:07:55Z- AHRS_M2 msg: 0.67 phi; -0.30 theta; -0.15 psi 2021-02-12T23:07:55Z- handleDepth()-Depth_Keller msg: 1613171275976 epoch sec; seqNo:1020; depth 29.30 2021-02-12T23:07:55Z- handleDvl() - RDI_Pathfinder msg: 1613171275.99 epoch sec; seqNo:434 2021-02-12T23:07:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:07:55Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:07:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171276.00 epoch sec; seqNo:1020; 4056239.21 north; 590461.78 east 2021-02-12T23:07:56Z- AHRS_M2 msg: 1613171276.35 epoch sec; seqNo:1014 2021-02-12T23:07:56Z- AHRS_M2 msg: 0.67 phi; -0.29 theta; -0.15 psi 2021-02-12T23:07:56Z- handleDepth()-Depth_Keller msg: 1613171276361 epoch sec; seqNo:1021; depth 29.43 2021-02-12T23:07:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171276.37 epoch sec; seqNo:1021; 4056239.59 north; 590461.77 east 2021-02-12T23:07:56Z- AHRS_M2 msg: 1613171276.76 epoch sec; seqNo:1015 2021-02-12T23:07:56Z- AHRS_M2 msg: 0.67 phi; -0.28 theta; -0.16 psi 2021-02-12T23:07:56Z- handleDepth()-Depth_Keller msg: 1613171276765 epoch sec; seqNo:1022; depth 29.53 2021-02-12T23:07:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171276.77 epoch sec; seqNo:1022; 4056239.60 north; 590462.99 east 2021-02-12T23:07:57Z- AHRS_M2 msg: 1613171277.16 epoch sec; seqNo:1016 2021-02-12T23:07:57Z- AHRS_M2 msg: 0.67 phi; -0.26 theta; -0.16 psi 2021-02-12T23:07:57Z- handleDepth()-Depth_Keller msg: 1613171277170 epoch sec; seqNo:1023; depth 29.66 2021-02-12T23:07:57Z- handleDvl() - RDI_Pathfinder msg: 1613171277.18 epoch sec; seqNo:435 2021-02-12T23:07:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.50 , 72.35 , 56.82 , 78.89 2021-02-12T23:07:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , -0.05 2021-02-12T23:07:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171277.19 epoch sec; seqNo:1023; 4056239.98 north; 590462.99 east 2021-02-12T23:07:57Z- AHRS_M2 msg: 1613171277.56 epoch sec; seqNo:1017 2021-02-12T23:07:57Z- AHRS_M2 msg: 0.67 phi; -0.23 theta; -0.16 psi 2021-02-12T23:07:57Z- handleDepth()-Depth_Keller msg: 1613171277568 epoch sec; seqNo:1024; depth 29.78 2021-02-12T23:07:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171277.59 epoch sec; seqNo:1024; 4056240.36 north; 590462.99 east 2021-02-12T23:07:57Z- AHRS_M2 msg: 1613171277.97 epoch sec; seqNo:1018 2021-02-12T23:07:57Z- AHRS_M2 msg: 0.67 phi; -0.19 theta; -0.16 psi 2021-02-12T23:07:57Z- handleDepth()-Depth_Keller msg: 1613171277977 epoch sec; seqNo:1025; depth 29.91 2021-02-12T23:07:57Z- handleDvl() - RDI_Pathfinder msg: 1613171277.99 epoch sec; seqNo:436 2021-02-12T23:07:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.11 , 71.46 , 57.66 , 0.00 2021-02-12T23:07:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.10 , -0.03 2021-02-12T23:07:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171278.01 epoch sec; seqNo:1025; 4056240.74 north; 590462.98 east 2021-02-12T23:07:58Z- AHRS_M2 msg: 1613171278.37 epoch sec; seqNo:1019 2021-02-12T23:07:58Z- AHRS_M2 msg: 0.66 phi; -0.15 theta; -0.15 psi 2021-02-12T23:07:58Z- handleDepth()-Depth_Keller msg: 1613171278377 epoch sec; seqNo:1026; depth 30.03 2021-02-12T23:07:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171278.39 epoch sec; seqNo:1026; 4056241.12 north; 590462.98 east 2021-02-12T23:07:58Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 132 calls... 2021-02-12T23:07:58Z- waiting for 1613171276.16, time is 1613171278.39 2021-02-12T23:07:58Z- LcmTrn::updateTrn() >>>> pose time 1613171278.39, meas time 1613171277.99 <<<< 2021-02-12T23:07:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171277.99 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:07:58Z- TerrainNav::motionUpdate() - currEstimate = (4056241, 590462.98, 30.03) 2021-02-12T23:07:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:07:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:07:58Z- TNavBF::Projecting Measurements 2021-02-12T23:07:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:07:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:07:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:07:58Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:07:58Z- TNavFilter::3 good beams. 2021-02-12T23:07:58Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:07:58Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:07:58Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:07:58Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:07:58Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:07:58Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:07:58Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:07:58Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:58Z- TerrainMapOctree: (vehN, vehE ) = (4056240.82, 590462.79). 2021-02-12T23:07:58Z- TerrainMapOctree: Min Distance = 6279.71. 2021-02-12T23:07:58Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:07:58Z- There is no specific good beam that all particles have in common. 2021-02-12T23:07:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:07:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.110 ExpectedMeasDiff: nan currMeas.covariance: 5.78 2021-02-12T23:07:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:07:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 8.17 2021-02-12T23:07:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:07:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.76 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 5.32 2021-02-12T23:07:58Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:07:58Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:07:58Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:07:58Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:07:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:07:58Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:07:58Z- Filter 0 No beams used in subcloud update 2021-02-12T23:07:58Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:07:58Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:07:58Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 809.8 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:07:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171277.99 sec. 2021-02-12T23:07:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:07:58Z- TNavBF::mmse selected from filter -1 2021-02-12T23:07:58Z- variances([0], [2], and psi_berg) = 809.682843 833.127036 0.000000 2021-02-12T23:07:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:07:58Z- LcmTrn::publishEstimates() - MLE : 4056208.62 590511.48 30.03 2021-02-12T23:07:58Z- LcmTrn::publishEstimates() - MMSE : 4056241.12 590462.98 30.03 2021-02-12T23:07:58Z- LcmTrn::publishEstimates() - COVAR: 809.68 833.13 0.71 0.00 2021-02-12T23:07:58Z- AHRS_M2 msg: 1613171278.79 epoch sec; seqNo:1020 2021-02-12T23:07:58Z- AHRS_M2 msg: 0.65 phi; -0.11 theta; -0.15 psi 2021-02-12T23:07:58Z- handleDepth()-Depth_Keller msg: 1613171278797 epoch sec; seqNo:1027; depth 30.14 2021-02-12T23:07:58Z- handleDvl() - RDI_Pathfinder msg: 1613171278.82 epoch sec; seqNo:437 2021-02-12T23:07:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 59.38 , 68.94 , 59.96 , 67.49 2021-02-12T23:07:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , -0.01 2021-02-12T23:07:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171278.85 epoch sec; seqNo:1027; 4056241.12 north; 590462.98 east 2021-02-12T23:07:59Z- AHRS_M2 msg: 1613171279.18 epoch sec; seqNo:1021 2021-02-12T23:07:59Z- AHRS_M2 msg: 0.64 phi; -0.07 theta; -0.15 psi 2021-02-12T23:07:59Z- handleDepth()-Depth_Keller msg: 1613171279184 epoch sec; seqNo:1028; depth 30.24 2021-02-12T23:07:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171279.20 epoch sec; seqNo:1028; 4056241.51 north; 590464.19 east 2021-02-12T23:07:59Z- AHRS_M2 msg: 1613171279.58 epoch sec; seqNo:1022 2021-02-12T23:07:59Z- AHRS_M2 msg: 0.64 phi; -0.03 theta; -0.15 psi 2021-02-12T23:07:59Z- handleDepth()-Depth_Keller msg: 1613171279588 epoch sec; seqNo:1029; depth 30.29 2021-02-12T23:07:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171279.60 epoch sec; seqNo:1029; 4056241.89 north; 590464.19 east 2021-02-12T23:07:59Z- AHRS_M2 msg: 1613171279.99 epoch sec; seqNo:1023 2021-02-12T23:07:59Z- AHRS_M2 msg: 0.63 phi; 0.01 theta; -0.15 psi 2021-02-12T23:08:00Z- handleDepth()-Depth_Keller msg: 1613171279997 epoch sec; seqNo:1030; depth 30.39 2021-02-12T23:08:00Z- handleDvl() - RDI_Pathfinder msg: 1613171280.01 epoch sec; seqNo:438 2021-02-12T23:08:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 58.58 , 68.70 , 62.88 , 63.18 2021-02-12T23:08:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.07 , 0.00 2021-02-12T23:08:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171280.02 epoch sec; seqNo:1030; 4056242.27 north; 590464.19 east 2021-02-12T23:08:00Z- AHRS_M2 msg: 1613171280.39 epoch sec; seqNo:1024 2021-02-12T23:08:00Z- AHRS_M2 msg: 0.62 phi; 0.05 theta; -0.16 psi 2021-02-12T23:08:00Z- handleDepth()-Depth_Keller msg: 1613171280397 epoch sec; seqNo:1031; depth 30.43 2021-02-12T23:08:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171280.41 epoch sec; seqNo:1031; 4056242.64 north; 590464.18 east 2021-02-12T23:08:00Z- AHRS_M2 msg: 1613171280.80 epoch sec; seqNo:1025 2021-02-12T23:08:00Z- AHRS_M2 msg: 0.61 phi; 0.09 theta; -0.16 psi 2021-02-12T23:08:00Z- handleDepth()-Depth_Keller msg: 1613171280800 epoch sec; seqNo:1032; depth 30.48 2021-02-12T23:08:00Z- handleDvl() - RDI_Pathfinder msg: 1613171280.83 epoch sec; seqNo:439 2021-02-12T23:08:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 58.80 , 68.94 , 66.91 , 60.54 2021-02-12T23:08:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , 0.01 2021-02-12T23:08:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171280.84 epoch sec; seqNo:1032; 4056242.64 north; 590464.18 east 2021-02-12T23:08:01Z- AHRS_M2 msg: 1613171281.21 epoch sec; seqNo:1026 2021-02-12T23:08:01Z- AHRS_M2 msg: 0.59 phi; 0.15 theta; -0.17 psi 2021-02-12T23:08:01Z- handleDepth()-Depth_Keller msg: 1613171281221 epoch sec; seqNo:1033; depth 30.50 2021-02-12T23:08:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171281.25 epoch sec; seqNo:1033; 4056243.02 north; 590464.18 east 2021-02-12T23:08:01Z- AHRS_M2 msg: 1613171281.62 epoch sec; seqNo:1027 2021-02-12T23:08:01Z- AHRS_M2 msg: 0.58 phi; 0.18 theta; -0.17 psi 2021-02-12T23:08:01Z- handleDepth()-Depth_Keller msg: 1613171281630 epoch sec; seqNo:1034; depth 30.51 2021-02-12T23:08:01Z- handleDvl() - RDI_Pathfinder msg: 1613171281.67 epoch sec; seqNo:440 2021-02-12T23:08:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 59.50 , 70.85 , 71.46 , 58.89 2021-02-12T23:08:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.03 , -0.01 2021-02-12T23:08:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171281.68 epoch sec; seqNo:1034; 4056243.41 north; 590465.40 east 2021-02-12T23:08:02Z- AHRS_M2 msg: 1613171282.01 epoch sec; seqNo:1028 2021-02-12T23:08:02Z- AHRS_M2 msg: 0.58 phi; 0.21 theta; -0.17 psi 2021-02-12T23:08:02Z- handleDepth()-Depth_Keller msg: 1613171282014 epoch sec; seqNo:1035; depth 30.50 2021-02-12T23:08:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171282.03 epoch sec; seqNo:1035; 4056243.79 north; 590465.39 east 2021-02-12T23:08:02Z- AHRS_M2 msg: 1613171282.41 epoch sec; seqNo:1029 2021-02-12T23:08:02Z- AHRS_M2 msg: 0.57 phi; 0.23 theta; -0.17 psi 2021-02-12T23:08:02Z- handleDepth()-Depth_Keller msg: 1613171282416 epoch sec; seqNo:1036; depth 30.48 2021-02-12T23:08:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171282.43 epoch sec; seqNo:1036; 4056243.79 north; 590465.39 east 2021-02-12T23:08:02Z- AHRS_M2 msg: 1613171282.82 epoch sec; seqNo:1030 2021-02-12T23:08:02Z- AHRS_M2 msg: 0.57 phi; 0.25 theta; -0.17 psi 2021-02-12T23:08:02Z- handleDepth()-Depth_Keller msg: 1613171282820 epoch sec; seqNo:1037; depth 30.46 2021-02-12T23:08:02Z- handleDvl() - RDI_Pathfinder msg: 1613171282.83 epoch sec; seqNo:441 2021-02-12T23:08:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.42 , 71.46 , 75.76 , 57.97 2021-02-12T23:08:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.05 , -0.03 2021-02-12T23:08:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171282.85 epoch sec; seqNo:1037; 4056244.17 north; 590465.39 east 2021-02-12T23:08:03Z- AHRS_M2 msg: 1613171283.22 epoch sec; seqNo:1031 2021-02-12T23:08:03Z- AHRS_M2 msg: 0.57 phi; 0.27 theta; -0.16 psi 2021-02-12T23:08:03Z- handleDepth()-Depth_Keller msg: 1613171283229 epoch sec; seqNo:1038; depth 30.44 2021-02-12T23:08:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171283.24 epoch sec; seqNo:1038; 4056244.55 north; 590465.38 east 2021-02-12T23:08:03Z- LcmTrn::handleMessages() - handling 41 messages took 0.73 ms and 127 calls... 2021-02-12T23:08:03Z- waiting for 1613171281.39, time is 1613171283.24 2021-02-12T23:08:03Z- LcmTrn::updateTrn() >>>> pose time 1613171283.24, meas time 1613171282.83 <<<< 2021-02-12T23:08:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171282.83 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:08:03Z- TerrainNav::motionUpdate() - currEstimate = (4056245, 590465.38, 30.44) 2021-02-12T23:08:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:08:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:08:03Z- TNavBF::Projecting Measurements 2021-02-12T23:08:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:08:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:08:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:08:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:08:03Z- TNavFilter::4 good beams. 2021-02-12T23:08:03Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:08:03Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:08:03Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:08:03Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:08:03Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:08:03Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:08:03Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:08:03Z- TNavBF::mmse selected from filter -1 2021-02-12T23:08:03Z- TerrainMapOctree: (vehN, vehE ) = (4056244.26, 590465.18). 2021-02-12T23:08:03Z- TerrainMapOctree: Min Distance = 6275.53. 2021-02-12T23:08:03Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:08:03Z- There is no specific good beam that all particles have in common. 2021-02-12T23:08:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:08:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 5.84 2021-02-12T23:08:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:08:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 8.17 2021-02-12T23:08:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:08:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.74 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 9.18 2021-02-12T23:08:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:08:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.70 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 5.38 2021-02-12T23:08:03Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:08:03Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:08:03Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:08:03Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:08:03Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:08:03Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:08:03Z- Filter 0 No beams used in subcloud update 2021-02-12T23:08:03Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:08:03Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:08:03Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 809.4 EastVariance: 833.1 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-12T23:08:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171282.83 sec. 2021-02-12T23:08:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:08:03Z- TNavBF::mmse selected from filter -1 2021-02-12T23:08:03Z- variances([0], [2], and psi_berg) = 809.319241 833.125992 0.000000 2021-02-12T23:08:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:08:03Z- LcmTrn::publishEstimates() - MLE : 4056212.05 590513.88 30.44 2021-02-12T23:08:03Z- LcmTrn::publishEstimates() - MMSE : 4056244.55 590465.38 30.44 2021-02-12T23:08:03Z- LcmTrn::publishEstimates() - COVAR: 809.32 833.13 0.65 0.00 2021-02-12T23:08:03Z- AHRS_M2 msg: 1613171283.64 epoch sec; seqNo:1032 2021-02-12T23:08:03Z- AHRS_M2 msg: 0.57 phi; 0.29 theta; -0.16 psi 2021-02-12T23:08:03Z- handleDepth()-Depth_Keller msg: 1613171283656 epoch sec; seqNo:1039; depth 30.37 2021-02-12T23:08:03Z- handleDvl() - RDI_Pathfinder msg: 1613171283.67 epoch sec; seqNo:442 2021-02-12T23:08:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.73 , 72.38 , 79.44 , 57.05 2021-02-12T23:08:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.06 , -0.03 2021-02-12T23:08:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171283.70 epoch sec; seqNo:1039; 4056244.55 north; 590465.38 east 2021-02-12T23:08:04Z- AHRS_M2 msg: 1613171284.03 epoch sec; seqNo:1033 2021-02-12T23:08:04Z- AHRS_M2 msg: 0.57 phi; 0.31 theta; -0.16 psi 2021-02-12T23:08:04Z- handleDepth()-Depth_Keller msg: 1613171284032 epoch sec; seqNo:1040; depth 30.31 2021-02-12T23:08:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171284.04 epoch sec; seqNo:1040; 4056244.93 north; 590465.38 east 2021-02-12T23:08:04Z- AHRS_M2 msg: 1613171284.43 epoch sec; seqNo:1034 2021-02-12T23:08:04Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.16 psi 2021-02-12T23:08:04Z- handleDepth()-Depth_Keller msg: 1613171284441 epoch sec; seqNo:1041; depth 30.26 2021-02-12T23:08:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171284.45 epoch sec; seqNo:1041; 4056245.32 north; 590466.60 east 2021-02-12T23:08:04Z- AHRS_M2 msg: 1613171284.85 epoch sec; seqNo:1035 2021-02-12T23:08:04Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.16 psi 2021-02-12T23:08:04Z- handleDepth()-Depth_Keller msg: 1613171284849 epoch sec; seqNo:1042; depth 30.17 2021-02-12T23:08:04Z- handleDvl() - RDI_Pathfinder msg: 1613171284.86 epoch sec; seqNo:443 2021-02-12T23:08:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.83 , 73.17 , 83.20 , 56.98 2021-02-12T23:08:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.04 , -0.05 2021-02-12T23:08:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171284.87 epoch sec; seqNo:1042; 4056245.32 north; 590466.60 east 2021-02-12T23:08:05Z- AHRS_M2 msg: 1613171285.24 epoch sec; seqNo:1036 2021-02-12T23:08:05Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.16 psi 2021-02-12T23:08:05Z- handleDepth()-Depth_Keller msg: 1613171285245 epoch sec; seqNo:1043; depth 30.08 2021-02-12T23:08:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171285.26 epoch sec; seqNo:1043; 4056245.70 north; 590466.59 east 2021-02-12T23:08:05Z- AHRS_M2 msg: 1613171285.64 epoch sec; seqNo:1037 2021-02-12T23:08:05Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.16 psi 2021-02-12T23:08:05Z- handleDepth()-Depth_Keller msg: 1613171285653 epoch sec; seqNo:1044; depth 30.02 2021-02-12T23:08:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171285.66 epoch sec; seqNo:1044; 4056246.08 north; 590466.59 east 2021-02-12T23:08:06Z- AHRS_M2 msg: 1613171286.05 epoch sec; seqNo:1038 2021-02-12T23:08:06Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.16 psi 2021-02-12T23:08:06Z- handleDepth()-Depth_Keller msg: 1613171286057 epoch sec; seqNo:1045; depth 29.91 2021-02-12T23:08:06Z- handleDvl() - RDI_Pathfinder msg: 1613171286.07 epoch sec; seqNo:444 2021-02-12T23:08:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.54 , 73.99 , 91.50 , 56.82 2021-02-12T23:08:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , -0.05 2021-02-12T23:08:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171286.08 epoch sec; seqNo:1045; 4056246.08 north; 590466.59 east 2021-02-12T23:08:06Z- AHRS_M2 msg: 1613171286.45 epoch sec; seqNo:1039 2021-02-12T23:08:06Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.16 psi 2021-02-12T23:08:06Z- handleDepth()-Depth_Keller msg: 1613171286458 epoch sec; seqNo:1046; depth 29.80 2021-02-12T23:08:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171286.47 epoch sec; seqNo:1046; 4056246.46 north; 590466.58 east 2021-02-12T23:08:06Z- AHRS_M2 msg: 1613171286.86 epoch sec; seqNo:1040 2021-02-12T23:08:06Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.16 psi 2021-02-12T23:08:06Z- handleDepth()-Depth_Keller msg: 1613171286865 epoch sec; seqNo:1047; depth 29.72 2021-02-12T23:08:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171286.88 epoch sec; seqNo:1047; 4056246.84 north; 590466.58 east 2021-02-12T23:08:07Z- AHRS_M2 msg: 1613171287.26 epoch sec; seqNo:1041 2021-02-12T23:08:07Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.15 psi 2021-02-12T23:08:07Z- handleDepth()-Depth_Keller msg: 1613171287264 epoch sec; seqNo:1048; depth 29.59 2021-02-12T23:08:07Z- handleDvl() - RDI_Pathfinder msg: 1613171287.28 epoch sec; seqNo:445 2021-02-12T23:08:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.54 , 74.80 , 93.66 , 56.82 2021-02-12T23:08:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , -0.07 2021-02-12T23:08:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171287.30 epoch sec; seqNo:1048; 4056247.23 north; 590467.80 east 2021-02-12T23:08:07Z- AHRS_M2 msg: 1613171287.66 epoch sec; seqNo:1042 2021-02-12T23:08:07Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.15 psi 2021-02-12T23:08:07Z- handleDepth()-Depth_Keller msg: 1613171287668 epoch sec; seqNo:1049; depth 29.49 2021-02-12T23:08:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171287.68 epoch sec; seqNo:1049; 4056247.23 north; 590467.80 east 2021-02-12T23:08:08Z- AHRS_M2 msg: 1613171288.07 epoch sec; seqNo:1043 2021-02-12T23:08:08Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.15 psi 2021-02-12T23:08:08Z- handleDepth()-Depth_Keller msg: 1613171288074 epoch sec; seqNo:1050; depth 29.38 2021-02-12T23:08:08Z- handleDvl() - RDI_Pathfinder msg: 1613171288.09 epoch sec; seqNo:446 2021-02-12T23:08:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.77 , 75.21 , 97.99 , 57.23 2021-02-12T23:08:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , -0.06 2021-02-12T23:08:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171288.10 epoch sec; seqNo:1050; 4056247.61 north; 590467.79 east 2021-02-12T23:08:08Z- AHRS_M2 msg: 1613171288.47 epoch sec; seqNo:1044 2021-02-12T23:08:08Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.15 psi 2021-02-12T23:08:08Z- handleDepth()-Depth_Keller msg: 1613171288476 epoch sec; seqNo:1051; depth 29.25 2021-02-12T23:08:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171288.49 epoch sec; seqNo:1051; 4056247.99 north; 590467.79 east 2021-02-12T23:08:08Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-12T23:08:08Z- waiting for 1613171286.24, time is 1613171288.49 2021-02-12T23:08:08Z- LcmTrn::updateTrn() >>>> pose time 1613171288.49, meas time 1613171288.09 <<<< 2021-02-12T23:08:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171288.09 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:08:08Z- TerrainNav::motionUpdate() - currEstimate = (4056248, 590467.79, 29.25) 2021-02-12T23:08:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:08:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:08:08Z- TNavBF::Projecting Measurements 2021-02-12T23:08:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:08:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:08:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:08:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:08:08Z- TNavFilter::4 good beams. 2021-02-12T23:08:08Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:08:08Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:08:08Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:08:08Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:08:08Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:08:08Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:08:08Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:08:08Z- TNavBF::mmse selected from filter -1 2021-02-12T23:08:08Z- TerrainMapOctree: (vehN, vehE ) = (4056247.72, 590467.60). 2021-02-12T23:08:08Z- TerrainMapOctree: Min Distance = 6271.31. 2021-02-12T23:08:08Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:08:08Z- There is no specific good beam that all particles have in common. 2021-02-12T23:08:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:08:08Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 6.51 2021-02-12T23:08:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:08:08Z- Old Depth Bias: 0.00 DepthBiasCov: 0.77 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 9.05 2021-02-12T23:08:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:08:08Z- Old Depth Bias: 0.00 DepthBiasCov: 0.72 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-12T23:08:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:08:08Z- Old Depth Bias: 0.00 DepthBiasCov: 0.69 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 5.24 2021-02-12T23:08:08Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:08:08Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:08:08Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:08:08Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:08:08Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:08:08Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:08:08Z- Filter 0 No beams used in subcloud update 2021-02-12T23:08:08Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:08:08Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:08:08Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 809.0 EastVariance: 833.1 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-12T23:08:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171288.09 sec. 2021-02-12T23:08:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:08:08Z- TNavBF::mmse selected from filter -1 2021-02-12T23:08:08Z- variances([0], [2], and psi_berg) = 808.961268 833.124977 0.000000 2021-02-12T23:08:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:08:08Z- LcmTrn::publishEstimates() - MLE : 4056215.49 590516.29 29.25 2021-02-12T23:08:08Z- LcmTrn::publishEstimates() - MMSE : 4056247.99 590467.79 29.25 2021-02-12T23:08:08Z- LcmTrn::publishEstimates() - COVAR: 808.96 833.12 0.64 0.00 2021-02-12T23:08:08Z- AHRS_M2 msg: 1613171288.88 epoch sec; seqNo:1045 2021-02-12T23:08:08Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.15 psi 2021-02-12T23:08:08Z- handleDepth()-Depth_Keller msg: 1613171288880 epoch sec; seqNo:1052; depth 29.16 2021-02-12T23:08:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171288.89 epoch sec; seqNo:1052; 4056247.99 north; 590467.79 east 2021-02-12T23:08:09Z- AHRS_M2 msg: 1613171289.28 epoch sec; seqNo:1046 2021-02-12T23:08:09Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.15 psi 2021-02-12T23:08:09Z- handleDepth()-Depth_Keller msg: 1613171289284 epoch sec; seqNo:1053; depth 29.03 2021-02-12T23:08:09Z- handleDvl() - RDI_Pathfinder msg: 1613171289.30 epoch sec; seqNo:447 2021-02-12T23:08:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.77 , 75.21 , 94.75 , 57.64 2021-02-12T23:08:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.07 , -0.06 2021-02-12T23:08:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171289.31 epoch sec; seqNo:1053; 4056248.37 north; 590467.79 east 2021-02-12T23:08:09Z- AHRS_M2 msg: 1613171289.68 epoch sec; seqNo:1047 2021-02-12T23:08:09Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.15 psi 2021-02-12T23:08:09Z- handleDepth()-Depth_Keller msg: 1613171289697 epoch sec; seqNo:1054; depth 28.90 2021-02-12T23:08:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171289.71 epoch sec; seqNo:1054; 4056248.74 north; 590467.78 east 2021-02-12T23:08:10Z- AHRS_M2 msg: 1613171290.09 epoch sec; seqNo:1048 2021-02-12T23:08:10Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.15 psi 2021-02-12T23:08:10Z- handleDepth()-Depth_Keller msg: 1613171290102 epoch sec; seqNo:1055; depth 28.78 2021-02-12T23:08:10Z- handleDvl() - RDI_Pathfinder msg: 1613171290.12 epoch sec; seqNo:448 2021-02-12T23:08:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.77 , 75.21 , 94.75 , 56.82 2021-02-12T23:08:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.09 , -0.02 2021-02-12T23:08:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171290.13 epoch sec; seqNo:1055; 4056249.12 north; 590467.78 east 2021-02-12T23:08:10Z- AHRS_M2 msg: 1613171290.49 epoch sec; seqNo:1049 2021-02-12T23:08:10Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.15 psi 2021-02-12T23:08:10Z- handleDepth()-Depth_Keller msg: 1613171290501 epoch sec; seqNo:1056; depth 28.67 2021-02-12T23:08:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171290.51 epoch sec; seqNo:1056; 4056249.14 north; 590469.00 east 2021-02-12T23:08:10Z- AHRS_M2 msg: 1613171290.90 epoch sec; seqNo:1050 2021-02-12T23:08:10Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.14 psi 2021-02-12T23:08:10Z- handleDepth()-Depth_Keller msg: 1613171290905 epoch sec; seqNo:1057; depth 28.55 2021-02-12T23:08:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171290.92 epoch sec; seqNo:1057; 4056249.51 north; 590468.99 east 2021-02-12T23:08:11Z- AHRS_M2 msg: 1613171291.30 epoch sec; seqNo:1051 2021-02-12T23:08:11Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.14 psi 2021-02-12T23:08:11Z- handleDepth()-Depth_Keller msg: 1613171291306 epoch sec; seqNo:1058; depth 28.43 2021-02-12T23:08:11Z- handleDvl() - RDI_Pathfinder msg: 1613171291.32 epoch sec; seqNo:449 2021-02-12T23:08:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.99 , 75.21 , 97.99 , 58.04 2021-02-12T23:08:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.10 , -0.02 2021-02-12T23:08:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171291.33 epoch sec; seqNo:1058; 4056249.89 north; 590468.99 east 2021-02-12T23:08:11Z- AHRS_M2 msg: 1613171291.70 epoch sec; seqNo:1052 2021-02-12T23:08:11Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.14 psi 2021-02-12T23:08:11Z- handleDepth()-Depth_Keller msg: 1613171291713 epoch sec; seqNo:1059; depth 28.30 2021-02-12T23:08:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171291.72 epoch sec; seqNo:1059; 4056250.27 north; 590468.99 east 2021-02-12T23:08:12Z- AHRS_M2 msg: 1613171292.11 epoch sec; seqNo:1053 2021-02-12T23:08:12Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.14 psi 2021-02-12T23:08:12Z- handleDepth()-Depth_Keller msg: 1613171292121 epoch sec; seqNo:1060; depth 28.18 2021-02-12T23:08:12Z- handleDvl() - RDI_Pathfinder msg: 1613171292.13 epoch sec; seqNo:450 2021-02-12T23:08:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.58 , 74.80 , 92.58 , 57.23 2021-02-12T23:08:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.07 , 0.04 2021-02-12T23:08:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171292.15 epoch sec; seqNo:1060; 4056250.27 north; 590468.99 east 2021-02-12T23:08:12Z- AHRS_M2 msg: 1613171292.51 epoch sec; seqNo:1054 2021-02-12T23:08:12Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.13 psi 2021-02-12T23:08:12Z- handleDepth()-Depth_Keller msg: 1613171292518 epoch sec; seqNo:1061; depth 28.07 2021-02-12T23:08:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171292.53 epoch sec; seqNo:1061; 4056250.65 north; 590468.98 east 2021-02-12T23:08:12Z- AHRS_M2 msg: 1613171292.92 epoch sec; seqNo:1055 2021-02-12T23:08:12Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.13 psi 2021-02-12T23:08:12Z- handleDepth()-Depth_Keller msg: 1613171292924 epoch sec; seqNo:1062; depth 27.96 2021-02-12T23:08:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171292.97 epoch sec; seqNo:1062; 4056251.03 north; 590468.98 east 2021-02-12T23:08:13Z- AHRS_M2 msg: 1613171293.32 epoch sec; seqNo:1056 2021-02-12T23:08:13Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.13 psi 2021-02-12T23:08:13Z- handleDepth()-Depth_Keller msg: 1613171293324 epoch sec; seqNo:1063; depth 27.82 2021-02-12T23:08:13Z- handleDvl() - RDI_Pathfinder msg: 1613171293.34 epoch sec; seqNo:451 2021-02-12T23:08:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.99 , 74.40 , 93.66 , 58.04 2021-02-12T23:08:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , 0.06 2021-02-12T23:08:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171293.36 epoch sec; seqNo:1063; 4056251.42 north; 590470.20 east 2021-02-12T23:08:13Z- AHRS_M2 msg: 1613171293.72 epoch sec; seqNo:1057 2021-02-12T23:08:13Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.12 psi 2021-02-12T23:08:13Z- handleDepth()-Depth_Keller msg: 1613171293728 epoch sec; seqNo:1064; depth 27.70 2021-02-12T23:08:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171293.74 epoch sec; seqNo:1064; 4056251.42 north; 590470.20 east 2021-02-12T23:08:13Z- LcmTrn::handleMessages() - handling 44 messages took 0.76 ms and 129 calls... 2021-02-12T23:08:13Z- waiting for 1613171291.49, time is 1613171293.74 2021-02-12T23:08:13Z- LcmTrn::updateTrn() >>>> pose time 1613171293.74, meas time 1613171293.34 <<<< 2021-02-12T23:08:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171293.34 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:08:13Z- TerrainNav::motionUpdate() - currEstimate = (4056251, 590470.20, 27.70) 2021-02-12T23:08:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:08:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:08:13Z- TNavBF::Projecting Measurements 2021-02-12T23:08:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:08:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:08:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:08:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:08:13Z- TNavFilter::4 good beams. 2021-02-12T23:08:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:08:13Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:08:13Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:08:13Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:08:13Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:08:13Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:08:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:08:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:08:13Z- TerrainMapOctree: (vehN, vehE ) = (4056251.16, 590470.01). 2021-02-12T23:08:13Z- TerrainMapOctree: Min Distance = 6267.12. 2021-02-12T23:08:13Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:08:13Z- There is no specific good beam that all particles have in common. 2021-02-12T23:08:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:08:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 6.76 2021-02-12T23:08:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:08:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.77 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 8.86 2021-02-12T23:08:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:08:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.71 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 14.04 2021-02-12T23:08:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:08:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.68 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 5.39 2021-02-12T23:08:13Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:08:13Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:08:13Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:08:13Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:08:13Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:08:13Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:08:13Z- Filter 0 No beams used in subcloud update 2021-02-12T23:08:13Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:08:13Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:08:13Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 808.7 EastVariance: 833.1 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-12T23:08:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171293.34 sec. 2021-02-12T23:08:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:08:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:08:13Z- variances([0], [2], and psi_berg) = 808.606643 833.123983 0.000000 2021-02-12T23:08:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:08:13Z- LcmTrn::publishEstimates() - MLE : 4056218.92 590518.70 27.70 2021-02-12T23:08:13Z- LcmTrn::publishEstimates() - MMSE : 4056251.42 590470.20 27.70 2021-02-12T23:08:13Z- LcmTrn::publishEstimates() - COVAR: 808.61 833.12 0.64 0.00 2021-02-12T23:08:14Z- AHRS_M2 msg: 1613171294.13 epoch sec; seqNo:1058 2021-02-12T23:08:14Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.12 psi 2021-02-12T23:08:14Z- handleDepth()-Depth_Keller msg: 1613171294138 epoch sec; seqNo:1065; depth 27.56 2021-02-12T23:08:14Z- handleDvl() - RDI_Pathfinder msg: 1613171294.15 epoch sec; seqNo:452 2021-02-12T23:08:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.81 , 74.40 , 93.66 , 58.86 2021-02-12T23:08:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , 0.06 2021-02-12T23:08:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171294.16 epoch sec; seqNo:1065; 4056251.80 north; 590470.19 east 2021-02-12T23:08:14Z- AHRS_M2 msg: 1613171294.53 epoch sec; seqNo:1059 2021-02-12T23:08:14Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.12 psi 2021-02-12T23:08:14Z- handleDepth()-Depth_Keller msg: 1613171294536 epoch sec; seqNo:1066; depth 27.44 2021-02-12T23:08:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171294.55 epoch sec; seqNo:1066; 4056252.18 north; 590470.19 east 2021-02-12T23:08:14Z- AHRS_M2 msg: 1613171294.97 epoch sec; seqNo:1060 2021-02-12T23:08:14Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.11 psi 2021-02-12T23:08:14Z- handleDepth()-Depth_Keller msg: 1613171294982 epoch sec; seqNo:1067; depth 27.34 2021-02-12T23:08:15Z- handleDvl() - RDI_Pathfinder msg: 1613171295.01 epoch sec; seqNo:453 2021-02-12T23:08:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.22 , 74.40 , 93.66 , 58.86 2021-02-12T23:08:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.05 2021-02-12T23:08:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171295.03 epoch sec; seqNo:1067; 4056252.56 north; 590470.18 east 2021-02-12T23:08:15Z- AHRS_M2 msg: 1613171295.34 epoch sec; seqNo:1061 2021-02-12T23:08:15Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.11 psi 2021-02-12T23:08:15Z- handleDepth()-Depth_Keller msg: 1613171295344 epoch sec; seqNo:1068; depth 27.19 2021-02-12T23:08:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171295.36 epoch sec; seqNo:1068; 4056252.56 north; 590470.18 east 2021-02-12T23:08:15Z- AHRS_M2 msg: 1613171295.74 epoch sec; seqNo:1062 2021-02-12T23:08:15Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.11 psi 2021-02-12T23:08:15Z- handleDepth()-Depth_Keller msg: 1613171295757 epoch sec; seqNo:1069; depth 27.10 2021-02-12T23:08:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171295.77 epoch sec; seqNo:1069; 4056252.94 north; 590470.18 east 2021-02-12T23:08:16Z- AHRS_M2 msg: 1613171296.15 epoch sec; seqNo:1063 2021-02-12T23:08:16Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.12 psi 2021-02-12T23:08:16Z- handleDepth()-Depth_Keller msg: 1613171296154 epoch sec; seqNo:1070; depth 26.98 2021-02-12T23:08:16Z- handleDvl() - RDI_Pathfinder msg: 1613171296.17 epoch sec; seqNo:454 2021-02-12T23:08:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.63 , 73.99 , 97.99 , 59.27 2021-02-12T23:08:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , 0.02 2021-02-12T23:08:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171296.18 epoch sec; seqNo:1070; 4056253.33 north; 590471.40 east 2021-02-12T23:08:16Z- AHRS_M2 msg: 1613171296.55 epoch sec; seqNo:1064 2021-02-12T23:08:16Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.12 psi 2021-02-12T23:08:16Z- handleDepth()-Depth_Keller msg: 1613171296561 epoch sec; seqNo:1071; depth 26.85 2021-02-12T23:08:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171296.57 epoch sec; seqNo:1071; 4056253.71 north; 590471.39 east 2021-02-12T23:08:16Z- AHRS_M2 msg: 1613171296.97 epoch sec; seqNo:1065 2021-02-12T23:08:16Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.12 psi 2021-02-12T23:08:16Z- handleDepth()-Depth_Keller msg: 1613171296975 epoch sec; seqNo:1072; depth 26.74 2021-02-12T23:08:16Z- handleDvl() - RDI_Pathfinder msg: 1613171296.99 epoch sec; seqNo:455 2021-02-12T23:08:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.63 , 75.62 , 92.58 , 60.09 2021-02-12T23:08:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , 0.01 2021-02-12T23:08:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171297.01 epoch sec; seqNo:1072; 4056253.71 north; 590471.39 east 2021-02-12T23:08:17Z- AHRS_M2 msg: 1613171297.36 epoch sec; seqNo:1066 2021-02-12T23:08:17Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.13 psi 2021-02-12T23:08:17Z- handleDepth()-Depth_Keller msg: 1613171297366 epoch sec; seqNo:1073; depth 26.62 2021-02-12T23:08:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171297.39 epoch sec; seqNo:1073; 4056254.09 north; 590471.39 east 2021-02-12T23:08:17Z- AHRS_M2 msg: 1613171297.76 epoch sec; seqNo:1067 2021-02-12T23:08:17Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.12 psi 2021-02-12T23:08:17Z- handleDepth()-Depth_Keller msg: 1613171297768 epoch sec; seqNo:1074; depth 26.50 2021-02-12T23:08:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171297.79 epoch sec; seqNo:1074; 4056254.47 north; 590471.39 east 2021-02-12T23:08:18Z- AHRS_M2 msg: 1613171298.17 epoch sec; seqNo:1068 2021-02-12T23:08:18Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.12 psi 2021-02-12T23:08:18Z- handleDepth()-Depth_Keller msg: 1613171298173 epoch sec; seqNo:1075; depth 26.39 2021-02-12T23:08:18Z- handleDvl() - RDI_Pathfinder msg: 1613171298.19 epoch sec; seqNo:456 2021-02-12T23:08:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.04 , 75.62 , 93.66 , 60.50 2021-02-12T23:08:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , -0.02 2021-02-12T23:08:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171298.21 epoch sec; seqNo:1075; 4056254.86 north; 590472.60 east 2021-02-12T23:08:18Z- AHRS_M2 msg: 1613171298.58 epoch sec; seqNo:1069 2021-02-12T23:08:18Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.10 psi 2021-02-12T23:08:18Z- handleDepth()-Depth_Keller msg: 1613171298583 epoch sec; seqNo:1076; depth 26.27 2021-02-12T23:08:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171298.59 epoch sec; seqNo:1076; 4056254.86 north; 590472.60 east 2021-02-12T23:08:18Z- LcmTrn::handleMessages() - handling 41 messages took 0.67 ms and 128 calls... 2021-02-12T23:08:18Z- waiting for 1613171296.74, time is 1613171298.59 2021-02-12T23:08:18Z- LcmTrn::updateTrn() >>>> pose time 1613171298.59, meas time 1613171298.19 <<<< 2021-02-12T23:08:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171298.19 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:08:18Z- TerrainNav::motionUpdate() - currEstimate = (4056255, 590472.60, 26.27) 2021-02-12T23:08:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:08:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:08:18Z- TNavBF::Projecting Measurements 2021-02-12T23:08:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:08:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:08:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:08:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:08:18Z- TNavFilter::4 good beams. 2021-02-12T23:08:18Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:08:18Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:08:18Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:08:18Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:08:18Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:08:18Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:08:18Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:08:18Z- TNavBF::mmse selected from filter -1 2021-02-12T23:08:18Z- TerrainMapOctree: (vehN, vehE ) = (4056254.57, 590472.40). 2021-02-12T23:08:18Z- TerrainMapOctree: Min Distance = 6262.95. 2021-02-12T23:08:18Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:08:18Z- There is no specific good beam that all particles have in common. 2021-02-12T23:08:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:08:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 7.19 2021-02-12T23:08:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:08:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.76 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 9.15 2021-02-12T23:08:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:08:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.71 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 14.04 2021-02-12T23:08:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:08:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.68 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 5.86 2021-02-12T23:08:18Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:08:18Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:08:18Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:08:18Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:08:18Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:08:18Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:08:18Z- Filter 0 No beams used in subcloud update 2021-02-12T23:08:18Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:08:18Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:08:18Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 808.3 EastVariance: 833.1 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-12T23:08:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171298.19 sec. 2021-02-12T23:08:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:08:18Z- TNavBF::mmse selected from filter -1 2021-02-12T23:08:18Z- variances([0], [2], and psi_berg) = 808.253148 833.123004 0.000000 2021-02-12T23:08:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:08:18Z- LcmTrn::publishEstimates() - MLE : 4056222.36 590521.10 26.27 2021-02-12T23:08:18Z- LcmTrn::publishEstimates() - MMSE : 4056254.86 590472.60 26.27 2021-02-12T23:08:18Z- LcmTrn::publishEstimates() - COVAR: 808.25 833.12 0.64 0.00 2021-02-12T23:08:18Z- AHRS_M2 msg: 1613171298.98 epoch sec; seqNo:1070 2021-02-12T23:08:18Z- AHRS_M2 msg: 0.68 phi; 0.34 theta; -0.10 psi 2021-02-12T23:08:18Z- handleDepth()-Depth_Keller msg: 1613171298986 epoch sec; seqNo:1077; depth 26.14 2021-02-12T23:08:19Z- handleDvl() - RDI_Pathfinder msg: 1613171299.00 epoch sec; seqNo:457 2021-02-12T23:08:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.86 , 75.21 , 93.66 , 60.09 2021-02-12T23:08:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , -0.05 2021-02-12T23:08:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171299.01 epoch sec; seqNo:1077; 4056255.24 north; 590472.60 east 2021-02-12T23:08:19Z- AHRS_M2 msg: 1613171299.38 epoch sec; seqNo:1071 2021-02-12T23:08:19Z- AHRS_M2 msg: 0.69 phi; 0.34 theta; -0.09 psi 2021-02-12T23:08:19Z- handleDepth()-Depth_Keller msg: 1613171299390 epoch sec; seqNo:1078; depth 26.03 2021-02-12T23:08:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171299.41 epoch sec; seqNo:1078; 4056255.62 north; 590472.59 east 2021-02-12T23:08:19Z- AHRS_M2 msg: 1613171299.78 epoch sec; seqNo:1072 2021-02-12T23:08:19Z- AHRS_M2 msg: 0.69 phi; 0.33 theta; -0.09 psi 2021-02-12T23:08:19Z- handleDepth()-Depth_Keller msg: 1613171299788 epoch sec; seqNo:1079; depth 25.91 2021-02-12T23:08:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171299.80 epoch sec; seqNo:1079; 4056255.62 north; 590472.59 east 2021-02-12T23:08:20Z- AHRS_M2 msg: 1613171300.19 epoch sec; seqNo:1073 2021-02-12T23:08:20Z- AHRS_M2 msg: 0.70 phi; 0.33 theta; -0.09 psi 2021-02-12T23:08:20Z- handleDepth()-Depth_Keller msg: 1613171300201 epoch sec; seqNo:1080; depth 25.80 2021-02-12T23:08:20Z- handleDvl() - RDI_Pathfinder msg: 1613171300.21 epoch sec; seqNo:458 2021-02-12T23:08:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.26 , 74.80 , 93.66 , 60.50 2021-02-12T23:08:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , -0.06 2021-02-12T23:08:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171300.22 epoch sec; seqNo:1080; 4056255.99 north; 590472.59 east 2021-02-12T23:08:20Z- AHRS_M2 msg: 1613171300.59 epoch sec; seqNo:1074 2021-02-12T23:08:20Z- AHRS_M2 msg: 0.70 phi; 0.33 theta; -0.10 psi 2021-02-12T23:08:20Z- handleDepth()-Depth_Keller msg: 1613171300599 epoch sec; seqNo:1081; depth 25.65 2021-02-12T23:08:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171300.61 epoch sec; seqNo:1081; 4056256.39 north; 590473.81 east 2021-02-12T23:08:21Z- AHRS_M2 msg: 1613171301.00 epoch sec; seqNo:1075 2021-02-12T23:08:21Z- AHRS_M2 msg: 0.70 phi; 0.33 theta; -0.11 psi 2021-02-12T23:08:21Z- handleDepth()-Depth_Keller msg: 1613171301005 epoch sec; seqNo:1082; depth 25.55 2021-02-12T23:08:21Z- handleDvl() - RDI_Pathfinder msg: 1613171301.02 epoch sec; seqNo:459 2021-02-12T23:08:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.26 , 75.21 , 93.66 , 59.27 2021-02-12T23:08:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.04 2021-02-12T23:08:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171301.03 epoch sec; seqNo:1082; 4056256.39 north; 590473.81 east 2021-02-12T23:08:21Z- AHRS_M2 msg: 1613171301.40 epoch sec; seqNo:1076 2021-02-12T23:08:21Z- AHRS_M2 msg: 0.70 phi; 0.33 theta; -0.13 psi 2021-02-12T23:08:21Z- handleDepth()-Depth_Keller msg: 1613171301404 epoch sec; seqNo:1083; depth 25.42 2021-02-12T23:08:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171301.41 epoch sec; seqNo:1083; 4056256.76 north; 590473.80 east 2021-02-12T23:08:21Z- AHRS_M2 msg: 1613171301.80 epoch sec; seqNo:1077 2021-02-12T23:08:21Z- AHRS_M2 msg: 0.70 phi; 0.33 theta; -0.14 psi 2021-02-12T23:08:21Z- handleDepth()-Depth_Keller msg: 1613171301813 epoch sec; seqNo:1084; depth 25.30 2021-02-12T23:08:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171301.82 epoch sec; seqNo:1084; 4056257.14 north; 590473.80 east 2021-02-12T23:08:22Z- AHRS_M2 msg: 1613171302.21 epoch sec; seqNo:1078 2021-02-12T23:08:22Z- AHRS_M2 msg: 0.69 phi; 0.32 theta; -0.15 psi 2021-02-12T23:08:22Z- handleDepth()-Depth_Keller msg: 1613171302212 epoch sec; seqNo:1085; depth 25.18 2021-02-12T23:08:22Z- handleDvl() - RDI_Pathfinder msg: 1613171302.23 epoch sec; seqNo:460 2021-02-12T23:08:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.04 , 77.26 , 94.75 , 60.91 2021-02-12T23:08:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.04 , -0.01 2021-02-12T23:08:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171302.24 epoch sec; seqNo:1085; 4056257.14 north; 590473.80 east 2021-02-12T23:08:22Z- AHRS_M2 msg: 1613171302.61 epoch sec; seqNo:1079 2021-02-12T23:08:22Z- AHRS_M2 msg: 0.69 phi; 0.32 theta; -0.16 psi 2021-02-12T23:08:22Z- handleDepth()-Depth_Keller msg: 1613171302616 epoch sec; seqNo:1086; depth 25.04 2021-02-12T23:08:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171302.63 epoch sec; seqNo:1086; 4056257.52 north; 590473.80 east 2021-02-12T23:08:23Z- AHRS_M2 msg: 1613171303.02 epoch sec; seqNo:1080 2021-02-12T23:08:23Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.17 psi 2021-02-12T23:08:23Z- handleDepth()-Depth_Keller msg: 1613171303029 epoch sec; seqNo:1087; depth 24.93 2021-02-12T23:08:23Z- handleDvl() - RDI_Pathfinder msg: 1613171303.04 epoch sec; seqNo:461 2021-02-12T23:08:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.04 , 79.30 , 94.75 , 61.72 2021-02-12T23:08:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , 0.04 2021-02-12T23:08:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171303.05 epoch sec; seqNo:1087; 4056257.91 north; 590475.01 east 2021-02-12T23:08:23Z- AHRS_M2 msg: 1613171303.42 epoch sec; seqNo:1081 2021-02-12T23:08:23Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.18 psi 2021-02-12T23:08:23Z- handleDepth()-Depth_Keller msg: 1613171303424 epoch sec; seqNo:1088; depth 24.80 2021-02-12T23:08:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171303.44 epoch sec; seqNo:1088; 4056257.91 north; 590475.01 east 2021-02-12T23:08:23Z- AHRS_M2 msg: 1613171303.82 epoch sec; seqNo:1082 2021-02-12T23:08:23Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.18 psi 2021-02-12T23:08:23Z- handleDepth()-Depth_Keller msg: 1613171303828 epoch sec; seqNo:1089; depth 24.68 2021-02-12T23:08:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171303.84 epoch sec; seqNo:1089; 4056258.29 north; 590475.01 east 2021-02-12T23:08:23Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-12T23:08:23Z- waiting for 1613171301.59, time is 1613171303.84 2021-02-12T23:08:23Z- LcmTrn::updateTrn() >>>> pose time 1613171303.84, meas time 1613171303.04 <<<< 2021-02-12T23:08:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171303.04 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:08:23Z- TerrainNav::motionUpdate() - currEstimate = (4056258, 590475.01, 24.68) 2021-02-12T23:08:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:08:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:08:23Z- TNavBF::Projecting Measurements 2021-02-12T23:08:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:08:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:08:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:08:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:08:23Z- TNavFilter::4 good beams. 2021-02-12T23:08:23Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:08:23Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:08:23Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:08:23Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:08:23Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:08:23Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:08:23Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:08:23Z- TNavBF::mmse selected from filter -1 2021-02-12T23:08:23Z- TerrainMapOctree: (vehN, vehE ) = (4056257.77, 590474.64). 2021-02-12T23:08:23Z- TerrainMapOctree: Min Distance = 6259.05. 2021-02-12T23:08:23Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:08:24Z- There is no specific good beam that all particles have in common. 2021-02-12T23:08:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:08:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 7.19 2021-02-12T23:08:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:08:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.76 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 10.06 2021-02-12T23:08:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:08:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.71 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 14.36 2021-02-12T23:08:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:08:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.68 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 6.10 2021-02-12T23:08:24Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:08:24Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:08:24Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:08:24Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:08:24Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:08:24Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:08:24Z- Filter 0 No beams used in subcloud update 2021-02-12T23:08:24Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:08:24Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:08:24Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 808.0 EastVariance: 833.1 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-12T23:08:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171303.04 sec. 2021-02-12T23:08:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:08:24Z- TNavBF::mmse selected from filter -1 2021-02-12T23:08:24Z- variances([0], [2], and psi_berg) = 807.884941 833.121996 0.000000 2021-02-12T23:08:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:08:24Z- LcmTrn::publishEstimates() - MLE : 4056225.79 590523.51 24.68 2021-02-12T23:08:24Z- LcmTrn::publishEstimates() - MMSE : 4056258.29 590475.01 24.68 2021-02-12T23:08:24Z- LcmTrn::publishEstimates() - COVAR: 807.88 833.12 0.66 0.00 2021-02-12T23:08:24Z- AHRS_M2 msg: 1613171304.23 epoch sec; seqNo:1083 2021-02-12T23:08:24Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.19 psi 2021-02-12T23:08:24Z- handleDepth()-Depth_Keller msg: 1613171304233 epoch sec; seqNo:1090; depth 24.56 2021-02-12T23:08:24Z- handleDvl() - RDI_Pathfinder msg: 1613171304.24 epoch sec; seqNo:462 2021-02-12T23:08:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.63 , 85.01 , 94.75 , 61.72 2021-02-12T23:08:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , 0.11 2021-02-12T23:08:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171304.25 epoch sec; seqNo:1090; 4056258.67 north; 590475.00 east 2021-02-12T23:08:24Z- AHRS_M2 msg: 1613171304.63 epoch sec; seqNo:1084 2021-02-12T23:08:24Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.20 psi 2021-02-12T23:08:24Z- handleDepth()-Depth_Keller msg: 1613171304638 epoch sec; seqNo:1091; depth 24.43 2021-02-12T23:08:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171304.65 epoch sec; seqNo:1091; 4056258.67 north; 590475.00 east 2021-02-12T23:08:25Z- AHRS_M2 msg: 1613171305.05 epoch sec; seqNo:1085 2021-02-12T23:08:25Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.20 psi 2021-02-12T23:08:25Z- handleDepth()-Depth_Keller msg: 1613171305051 epoch sec; seqNo:1092; depth 24.31 2021-02-12T23:08:25Z- handleDvl() - RDI_Pathfinder msg: 1613171305.07 epoch sec; seqNo:463 2021-02-12T23:08:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.22 , 88.25 , 94.75 , 62.13 2021-02-12T23:08:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , 0.14 2021-02-12T23:08:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171305.08 epoch sec; seqNo:1092; 4056259.05 north; 590475.00 east 2021-02-12T23:08:25Z- AHRS_M2 msg: 1613171305.44 epoch sec; seqNo:1086 2021-02-12T23:08:25Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.20 psi 2021-02-12T23:08:25Z- handleDepth()-Depth_Keller msg: 1613171305444 epoch sec; seqNo:1093; depth 24.18 2021-02-12T23:08:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171305.45 epoch sec; seqNo:1093; 4056259.44 north; 590476.22 east 2021-02-12T23:08:25Z- AHRS_M2 msg: 1613171305.84 epoch sec; seqNo:1087 2021-02-12T23:08:25Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.20 psi 2021-02-12T23:08:25Z- handleDepth()-Depth_Keller msg: 1613171305851 epoch sec; seqNo:1094; depth 24.08 2021-02-12T23:08:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171305.86 epoch sec; seqNo:1094; 4056259.44 north; 590476.22 east 2021-02-12T23:08:26Z- AHRS_M2 msg: 1613171306.25 epoch sec; seqNo:1088 2021-02-12T23:08:26Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.20 psi 2021-02-12T23:08:26Z- handleDepth()-Depth_Keller msg: 1613171306251 epoch sec; seqNo:1095; depth 23.95 2021-02-12T23:08:26Z- handleDvl() - RDI_Pathfinder msg: 1613171306.27 epoch sec; seqNo:464 2021-02-12T23:08:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.63 , 87.17 , 96.91 , 62.13 2021-02-12T23:08:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.07 , 0.13 2021-02-12T23:08:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171306.28 epoch sec; seqNo:1095; 4056259.82 north; 590476.21 east 2021-02-12T23:08:26Z- AHRS_M2 msg: 1613171306.65 epoch sec; seqNo:1089 2021-02-12T23:08:26Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.19 psi 2021-02-12T23:08:26Z- handleDepth()-Depth_Keller msg: 1613171306656 epoch sec; seqNo:1096; depth 23.85 2021-02-12T23:08:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171306.67 epoch sec; seqNo:1096; 4056260.20 north; 590476.21 east 2021-02-12T23:08:27Z- AHRS_M2 msg: 1613171307.06 epoch sec; seqNo:1090 2021-02-12T23:08:27Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.19 psi 2021-02-12T23:08:27Z- handleDepth()-Depth_Keller msg: 1613171307060 epoch sec; seqNo:1097; depth 23.73 2021-02-12T23:08:27Z- handleDvl() - RDI_Pathfinder msg: 1613171307.07 epoch sec; seqNo:465 2021-02-12T23:08:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.45 , 88.25 , 95.83 , 61.72 2021-02-12T23:08:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , 0.06 2021-02-12T23:08:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171307.08 epoch sec; seqNo:1097; 4056260.58 north; 590476.21 east 2021-02-12T23:08:27Z- AHRS_M2 msg: 1613171307.46 epoch sec; seqNo:1091 2021-02-12T23:08:27Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.19 psi 2021-02-12T23:08:27Z- handleDepth()-Depth_Keller msg: 1613171307468 epoch sec; seqNo:1098; depth 23.62 2021-02-12T23:08:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171307.48 epoch sec; seqNo:1098; 4056260.59 north; 590477.43 east 2021-02-12T23:08:27Z- AHRS_M2 msg: 1613171307.86 epoch sec; seqNo:1092 2021-02-12T23:08:27Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.18 psi 2021-02-12T23:08:27Z- handleDepth()-Depth_Keller msg: 1613171307869 epoch sec; seqNo:1099; depth 23.50 2021-02-12T23:08:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171307.89 epoch sec; seqNo:1099; 4056260.97 north; 590477.42 east 2021-02-12T23:08:28Z- AHRS_M2 msg: 1613171308.27 epoch sec; seqNo:1093 2021-02-12T23:08:28Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.18 psi 2021-02-12T23:08:28Z- handleDepth()-Depth_Keller msg: 1613171308272 epoch sec; seqNo:1100; depth 23.37 2021-02-12T23:08:28Z- handleDvl() - RDI_Pathfinder msg: 1613171308.28 epoch sec; seqNo:466 2021-02-12T23:08:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.86 , 88.25 , 95.83 , 63.36 2021-02-12T23:08:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.08 , -0.02 2021-02-12T23:08:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171308.31 epoch sec; seqNo:1100; 4056261.35 north; 590477.42 east 2021-02-12T23:08:28Z- AHRS_M2 msg: 1613171308.67 epoch sec; seqNo:1094 2021-02-12T23:08:28Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.18 psi 2021-02-12T23:08:28Z- handleDepth()-Depth_Keller msg: 1613171308681 epoch sec; seqNo:1101; depth 23.27 2021-02-12T23:08:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171308.69 epoch sec; seqNo:1101; 4056261.35 north; 590477.42 east 2021-02-12T23:08:29Z- AHRS_M2 msg: 1613171309.08 epoch sec; seqNo:1095 2021-02-12T23:08:29Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.18 psi 2021-02-12T23:08:29Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-12T23:08:29Z- waiting for 1613171306.84, time is 1613171309.08 2021-02-12T23:08:29Z- LcmTrn::updateTrn() >>>> pose time 1613171309.08, meas time 1613171308.28 <<<< 2021-02-12T23:08:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171308.28 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:08:29Z- TerrainNav::motionUpdate() - currEstimate = (4056261, 590477.42, 23.27) 2021-02-12T23:08:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:08:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:08:29Z- TNavBF::Projecting Measurements 2021-02-12T23:08:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:08:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:08:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:08:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:08:29Z- TNavFilter::4 good beams. 2021-02-12T23:08:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:08:29Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:08:29Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:08:29Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:08:29Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:08:29Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:08:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:08:29Z- TNavBF::mmse selected from filter -1 2021-02-12T23:08:29Z- TerrainMapOctree: (vehN, vehE ) = (4056260.89, 590477.05). 2021-02-12T23:08:29Z- TerrainMapOctree: Min Distance = 6255.11. 2021-02-12T23:08:29Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:08:29Z- There is no specific good beam that all particles have in common. 2021-02-12T23:08:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:08:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 7.37 2021-02-12T23:08:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:08:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.76 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 12.46 2021-02-12T23:08:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:08:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.72 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 14.69 2021-02-12T23:08:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:08:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.69 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 6.42 2021-02-12T23:08:29Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:08:29Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:08:29Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:08:29Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:08:29Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:08:29Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:08:29Z- Filter 0 No beams used in subcloud update 2021-02-12T23:08:29Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:08:29Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:08:29Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 807.6 EastVariance: 833.1 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-12T23:08:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171308.28 sec. 2021-02-12T23:08:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:08:29Z- TNavBF::mmse selected from filter -1 2021-02-12T23:08:29Z- variances([0], [2], and psi_berg) = 807.533655 833.121046 0.000000 2021-02-12T23:08:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:08:29Z- LcmTrn::publishEstimates() - MLE : 4056228.85 590525.92 23.27 2021-02-12T23:08:29Z- LcmTrn::publishEstimates() - MMSE : 4056261.35 590477.42 23.27 2021-02-12T23:08:29Z- LcmTrn::publishEstimates() - COVAR: 807.53 833.12 0.67 0.00 2021-02-12T23:08:29Z- handleDepth()-Depth_Keller msg: 1613171309088 epoch sec; seqNo:1102; depth 23.16 2021-02-12T23:08:29Z- handleDvl() - RDI_Pathfinder msg: 1613171309.10 epoch sec; seqNo:467 2021-02-12T23:08:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.26 , 89.33 , 95.83 , 62.54 2021-02-12T23:08:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.06 , -0.08 2021-02-12T23:08:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171309.11 epoch sec; seqNo:1102; 4056261.73 north; 590477.41 east 2021-02-12T23:08:29Z- AHRS_M2 msg: 1613171309.48 epoch sec; seqNo:1096 2021-02-12T23:08:29Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.18 psi 2021-02-12T23:08:29Z- handleDepth()-Depth_Keller msg: 1613171309484 epoch sec; seqNo:1103; depth 23.04 2021-02-12T23:08:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171309.49 epoch sec; seqNo:1103; 4056262.11 north; 590477.41 east 2021-02-12T23:08:29Z- AHRS_M2 msg: 1613171309.89 epoch sec; seqNo:1097 2021-02-12T23:08:29Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.18 psi 2021-02-12T23:08:29Z- handleDepth()-Depth_Keller msg: 1613171309895 epoch sec; seqNo:1104; depth 22.94 2021-02-12T23:08:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171309.90 epoch sec; seqNo:1104; 4056262.12 north; 590478.63 east 2021-02-12T23:08:30Z- AHRS_M2 msg: 1613171310.29 epoch sec; seqNo:1098 2021-02-12T23:08:30Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.19 psi 2021-02-12T23:08:30Z- handleDepth()-Depth_Keller msg: 1613171310301 epoch sec; seqNo:1105; depth 22.81 2021-02-12T23:08:30Z- handleDvl() - RDI_Pathfinder msg: 1613171310.31 epoch sec; seqNo:468 2021-02-12T23:08:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.26 , 88.25 , 96.91 , 63.77 2021-02-12T23:08:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.09 2021-02-12T23:08:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171310.32 epoch sec; seqNo:1105; 4056262.50 north; 590478.63 east 2021-02-12T23:08:30Z- AHRS_M2 msg: 1613171310.69 epoch sec; seqNo:1099 2021-02-12T23:08:30Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.19 psi 2021-02-12T23:08:30Z- handleDepth()-Depth_Keller msg: 1613171310697 epoch sec; seqNo:1106; depth 22.70 2021-02-12T23:08:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171310.71 epoch sec; seqNo:1106; 4056262.88 north; 590478.62 east 2021-02-12T23:08:31Z- AHRS_M2 msg: 1613171311.10 epoch sec; seqNo:1100 2021-02-12T23:08:31Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.19 psi 2021-02-12T23:08:31Z- handleDepth()-Depth_Keller msg: 1613171311100 epoch sec; seqNo:1107; depth 22.58 2021-02-12T23:08:31Z- handleDvl() - RDI_Pathfinder msg: 1613171311.15 epoch sec; seqNo:469 2021-02-12T23:08:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.86 , 88.25 , 96.91 , 64.18 2021-02-12T23:08:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , -0.06 2021-02-12T23:08:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171311.16 epoch sec; seqNo:1107; 4056262.88 north; 590478.62 east 2021-02-12T23:08:31Z- AHRS_M2 msg: 1613171311.50 epoch sec; seqNo:1101 2021-02-12T23:08:31Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.19 psi 2021-02-12T23:08:31Z- handleDepth()-Depth_Keller msg: 1613171311505 epoch sec; seqNo:1108; depth 22.46 2021-02-12T23:08:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171311.52 epoch sec; seqNo:1108; 4056263.26 north; 590478.62 east 2021-02-12T23:08:31Z- AHRS_M2 msg: 1613171311.91 epoch sec; seqNo:1102 2021-02-12T23:08:31Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.18 psi 2021-02-12T23:08:31Z- handleDepth()-Depth_Keller msg: 1613171311930 epoch sec; seqNo:1109; depth 22.35 2021-02-12T23:08:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171311.94 epoch sec; seqNo:1109; 4056263.64 north; 590478.62 east 2021-02-12T23:08:32Z- AHRS_M2 msg: 1613171312.31 epoch sec; seqNo:1103 2021-02-12T23:08:32Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.18 psi 2021-02-12T23:08:32Z- handleDepth()-Depth_Keller msg: 1613171312317 epoch sec; seqNo:1110; depth 22.23 2021-02-12T23:08:32Z- handleDvl() - RDI_Pathfinder msg: 1613171312.33 epoch sec; seqNo:470 2021-02-12T23:08:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.26 , 90.42 , 95.83 , 64.18 2021-02-12T23:08:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , -0.01 2021-02-12T23:08:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171312.35 epoch sec; seqNo:1110; 4056263.64 north; 590478.62 east 2021-02-12T23:08:32Z- AHRS_M2 msg: 1613171312.71 epoch sec; seqNo:1104 2021-02-12T23:08:32Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.18 psi 2021-02-12T23:08:32Z- handleDepth()-Depth_Keller msg: 1613171312739 epoch sec; seqNo:1111; depth 22.13 2021-02-12T23:08:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171312.76 epoch sec; seqNo:1111; 4056264.03 north; 590479.83 east 2021-02-12T23:08:33Z- AHRS_M2 msg: 1613171313.12 epoch sec; seqNo:1105 2021-02-12T23:08:33Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.19 psi 2021-02-12T23:08:33Z- handleDepth()-Depth_Keller msg: 1613171313125 epoch sec; seqNo:1112; depth 22.01 2021-02-12T23:08:33Z- handleDvl() - RDI_Pathfinder msg: 1613171313.14 epoch sec; seqNo:471 2021-02-12T23:08:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.67 , 88.25 , 96.91 , 64.18 2021-02-12T23:08:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.02 , 0.04 2021-02-12T23:08:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171313.15 epoch sec; seqNo:1112; 4056264.41 north; 590479.83 east 2021-02-12T23:08:33Z- AHRS_M2 msg: 1613171313.52 epoch sec; seqNo:1106 2021-02-12T23:08:33Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.19 psi 2021-02-12T23:08:33Z- handleDepth()-Depth_Keller msg: 1613171313532 epoch sec; seqNo:1113; depth 21.90 2021-02-12T23:08:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171313.56 epoch sec; seqNo:1113; 4056264.41 north; 590479.83 east 2021-02-12T23:08:33Z- AHRS_M2 msg: 1613171313.92 epoch sec; seqNo:1107 2021-02-12T23:08:33Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.19 psi 2021-02-12T23:08:33Z- handleDepth()-Depth_Keller msg: 1613171313929 epoch sec; seqNo:1114; depth 21.80 2021-02-12T23:08:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171313.94 epoch sec; seqNo:1114; 4056264.78 north; 590479.82 east 2021-02-12T23:08:34Z- LcmTrn::handleMessages() - handling 43 messages took 0.58 ms and 132 calls... 2021-02-12T23:08:34Z- waiting for 1613171312.08, time is 1613171313.94 2021-02-12T23:08:34Z- LcmTrn::updateTrn() >>>> pose time 1613171313.94, meas time 1613171313.14 <<<< 2021-02-12T23:08:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171313.14 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:08:34Z- TerrainNav::motionUpdate() - currEstimate = (4056265, 590479.82, 21.80) 2021-02-12T23:08:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:08:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:08:34Z- TNavBF::Projecting Measurements 2021-02-12T23:08:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:08:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:08:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:08:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:08:34Z- TNavFilter::4 good beams. 2021-02-12T23:08:34Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:08:34Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:08:34Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:08:34Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:08:34Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:08:34Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:08:34Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:08:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:08:34Z- TerrainMapOctree: (vehN, vehE ) = (4056264.22, 590479.43). 2021-02-12T23:08:34Z- TerrainMapOctree: Min Distance = 6251.01. 2021-02-12T23:08:34Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:08:34Z- There is no specific good beam that all particles have in common. 2021-02-12T23:08:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:08:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 7.55 2021-02-12T23:08:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:08:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.77 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 12.46 2021-02-12T23:08:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:08:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.73 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 15.03 2021-02-12T23:08:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:08:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.70 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 6.59 2021-02-12T23:08:34Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:08:34Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:08:34Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:08:34Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:08:34Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:08:34Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:08:34Z- Filter 0 No beams used in subcloud update 2021-02-12T23:08:34Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:08:34Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:08:34Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 807.3 EastVariance: 833.1 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-12T23:08:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171313.14 sec. 2021-02-12T23:08:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:08:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:08:34Z- variances([0], [2], and psi_berg) = 807.169768 833.120073 0.000000 2021-02-12T23:08:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:08:34Z- LcmTrn::publishEstimates() - MLE : 4056232.28 590529.32 21.80 2021-02-12T23:08:34Z- LcmTrn::publishEstimates() - MMSE : 4056264.78 590479.82 21.80 2021-02-12T23:08:34Z- LcmTrn::publishEstimates() - COVAR: 807.17 833.12 0.68 0.00 2021-02-12T23:08:34Z- AHRS_M2 msg: 1613171314.33 epoch sec; seqNo:1108 2021-02-12T23:08:34Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.19 psi 2021-02-12T23:08:34Z- handleDepth()-Depth_Keller msg: 1613171314332 epoch sec; seqNo:1115; depth 21.69 2021-02-12T23:08:34Z- handleDvl() - RDI_Pathfinder msg: 1613171314.35 epoch sec; seqNo:472 2021-02-12T23:08:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.08 , 89.33 , 96.91 , 64.18 2021-02-12T23:08:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.00 , 0.07 2021-02-12T23:08:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171314.36 epoch sec; seqNo:1115; 4056265.16 north; 590479.82 east 2021-02-12T23:08:34Z- AHRS_M2 msg: 1613171314.73 epoch sec; seqNo:1109 2021-02-12T23:08:34Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.19 psi 2021-02-12T23:08:34Z- handleDepth()-Depth_Keller msg: 1613171314736 epoch sec; seqNo:1116; depth 21.58 2021-02-12T23:08:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171314.75 epoch sec; seqNo:1116; 4056265.16 north; 590479.82 east 2021-02-12T23:08:35Z- AHRS_M2 msg: 1613171315.14 epoch sec; seqNo:1110 2021-02-12T23:08:35Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.19 psi 2021-02-12T23:08:35Z- handleDepth()-Depth_Keller msg: 1613171315144 epoch sec; seqNo:1117; depth 21.47 2021-02-12T23:08:35Z- handleDvl() - RDI_Pathfinder msg: 1613171315.17 epoch sec; seqNo:473 2021-02-12T23:08:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.67 , 88.25 , 94.75 , 64.58 2021-02-12T23:08:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.02 , 0.08 2021-02-12T23:08:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171315.18 epoch sec; seqNo:1117; 4056265.56 north; 590481.04 east 2021-02-12T23:08:35Z- AHRS_M2 msg: 1613171315.54 epoch sec; seqNo:1111 2021-02-12T23:08:35Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.19 psi 2021-02-12T23:08:35Z- handleDepth()-Depth_Keller msg: 1613171315553 epoch sec; seqNo:1118; depth 21.35 2021-02-12T23:08:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171315.56 epoch sec; seqNo:1118; 4056265.93 north; 590481.03 east 2021-02-12T23:08:35Z- AHRS_M2 msg: 1613171315.96 epoch sec; seqNo:1112 2021-02-12T23:08:35Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.20 psi 2021-02-12T23:08:35Z- handleDepth()-Depth_Keller msg: 1613171315970 epoch sec; seqNo:1119; depth 21.24 2021-02-12T23:08:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171315.98 epoch sec; seqNo:1119; 4056265.93 north; 590481.03 east 2021-02-12T23:08:36Z- AHRS_M2 msg: 1613171316.35 epoch sec; seqNo:1113 2021-02-12T23:08:36Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.20 psi 2021-02-12T23:08:36Z- handleDepth()-Depth_Keller msg: 1613171316359 epoch sec; seqNo:1120; depth 21.13 2021-02-12T23:08:36Z- handleDvl() - RDI_Pathfinder msg: 1613171316.37 epoch sec; seqNo:474 2021-02-12T23:08:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.08 , 91.50 , 95.83 , 65.40 2021-02-12T23:08:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , 0.09 2021-02-12T23:08:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171316.38 epoch sec; seqNo:1120; 4056266.31 north; 590481.03 east 2021-02-12T23:08:36Z- AHRS_M2 msg: 1613171316.75 epoch sec; seqNo:1114 2021-02-12T23:08:36Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.20 psi 2021-02-12T23:08:36Z- handleDepth()-Depth_Keller msg: 1613171316756 epoch sec; seqNo:1121; depth 21.02 2021-02-12T23:08:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171316.77 epoch sec; seqNo:1121; 4056266.69 north; 590481.03 east 2021-02-12T23:08:37Z- AHRS_M2 msg: 1613171317.15 epoch sec; seqNo:1115 2021-02-12T23:08:37Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.21 psi 2021-02-12T23:08:37Z- handleDepth()-Depth_Keller msg: 1613171317161 epoch sec; seqNo:1122; depth 20.94 2021-02-12T23:08:37Z- handleDvl() - RDI_Pathfinder msg: 1613171317.17 epoch sec; seqNo:475 2021-02-12T23:08:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.08 , 92.51 , 96.84 , 66.63 2021-02-12T23:08:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , 0.08 2021-02-12T23:08:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171317.18 epoch sec; seqNo:1122; 4056266.69 north; 590481.03 east 2021-02-12T23:08:37Z- AHRS_M2 msg: 1613171317.56 epoch sec; seqNo:1116 2021-02-12T23:08:37Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.21 psi 2021-02-12T23:08:37Z- handleDepth()-Depth_Keller msg: 1613171317566 epoch sec; seqNo:1123; depth 20.81 2021-02-12T23:08:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171317.58 epoch sec; seqNo:1123; 4056267.07 north; 590481.02 east 2021-02-12T23:08:37Z- AHRS_M2 msg: 1613171317.96 epoch sec; seqNo:1117 2021-02-12T23:08:37Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.21 psi 2021-02-12T23:08:37Z- handleDepth()-Depth_Keller msg: 1613171317968 epoch sec; seqNo:1124; depth 20.69 2021-02-12T23:08:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171317.99 epoch sec; seqNo:1124; 4056267.46 north; 590482.24 east 2021-02-12T23:08:38Z- AHRS_M2 msg: 1613171318.37 epoch sec; seqNo:1118 2021-02-12T23:08:38Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.20 psi 2021-02-12T23:08:38Z- handleDepth()-Depth_Keller msg: 1613171318377 epoch sec; seqNo:1125; depth 20.61 2021-02-12T23:08:38Z- handleDvl() - RDI_Pathfinder msg: 1613171318.39 epoch sec; seqNo:476 2021-02-12T23:08:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.90 , 91.36 , 100.02 , 65.81 2021-02-12T23:08:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.08 2021-02-12T23:08:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171318.40 epoch sec; seqNo:1125; 4056267.84 north; 590482.23 east 2021-02-12T23:08:38Z- AHRS_M2 msg: 1613171318.77 epoch sec; seqNo:1119 2021-02-12T23:08:38Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.20 psi 2021-02-12T23:08:38Z- handleDepth()-Depth_Keller msg: 1613171318776 epoch sec; seqNo:1126; depth 20.49 2021-02-12T23:08:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171318.79 epoch sec; seqNo:1126; 4056267.84 north; 590482.23 east 2021-02-12T23:08:39Z- AHRS_M2 msg: 1613171319.18 epoch sec; seqNo:1120 2021-02-12T23:08:39Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.19 psi 2021-02-12T23:08:39Z- handleDepth()-Depth_Keller msg: 1613171319188 epoch sec; seqNo:1127; depth 20.38 2021-02-12T23:08:39Z- handleDvl() - RDI_Pathfinder msg: 1613171319.20 epoch sec; seqNo:477 2021-02-12T23:08:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.90 , 92.38 , 96.71 , 65.40 2021-02-12T23:08:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , 0.06 2021-02-12T23:08:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171319.21 epoch sec; seqNo:1127; 4056268.22 north; 590482.23 east 2021-02-12T23:08:39Z- LcmTrn::handleMessages() - handling 45 messages took 0.69 ms and 132 calls... 2021-02-12T23:08:39Z- waiting for 1613171316.94, time is 1613171319.21 2021-02-12T23:08:39Z- LcmTrn::updateTrn() equating pose 1613171319.21 and meas times 1613171319.21 2021-02-12T23:08:39Z- LcmTrn::updateTrn() >>>> pose time 1613171319.21, meas time 1613171319.21 <<<< 2021-02-12T23:08:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171319.21 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:08:39Z- TerrainNav::motionUpdate() - currEstimate = (4056268, 590482.23, 20.38) 2021-02-12T23:08:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:08:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:08:39Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171319.21 sec.; No relevant navigation data available 2021-02-12T23:08:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:08:39Z- TNavBF::mmse selected from filter -1 2021-02-12T23:08:39Z- variances([0], [2], and psi_berg) = 806.895910 833.119347 0.000000 2021-02-12T23:08:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:08:39Z- LcmTrn::publishEstimates() - MLE : 4056235.72 590531.73 20.38 2021-02-12T23:08:39Z- LcmTrn::publishEstimates() - MMSE : 4056268.22 590482.23 20.38 2021-02-12T23:08:39Z- LcmTrn::publishEstimates() - COVAR: 806.90 833.12 0.90 0.00 2021-02-12T23:08:39Z- AHRS_M2 msg: 1613171319.58 epoch sec; seqNo:1121 2021-02-12T23:08:39Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.18 psi 2021-02-12T23:08:39Z- handleDepth()-Depth_Keller msg: 1613171319584 epoch sec; seqNo:1128; depth 20.27 2021-02-12T23:08:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171319.60 epoch sec; seqNo:1128; 4056268.60 north; 590482.23 east 2021-02-12T23:08:39Z- AHRS_M2 msg: 1613171319.99 epoch sec; seqNo:1122 2021-02-12T23:08:39Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.16 psi 2021-02-12T23:08:40Z- handleDepth()-Depth_Keller msg: 1613171319997 epoch sec; seqNo:1129; depth 20.18 2021-02-12T23:08:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171320.04 epoch sec; seqNo:1129; 4056268.98 north; 590482.22 east 2021-02-12T23:08:40Z- AHRS_M2 msg: 1613171320.39 epoch sec; seqNo:1123 2021-02-12T23:08:40Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-12T23:08:40Z- handleDepth()-Depth_Keller msg: 1613171320394 epoch sec; seqNo:1130; depth 20.06 2021-02-12T23:08:40Z- handleDvl() - RDI_Pathfinder msg: 1613171320.41 epoch sec; seqNo:478 2021-02-12T23:08:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.72 , 90.28 , 96.77 , 66.22 2021-02-12T23:08:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , 0.03 2021-02-12T23:08:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171320.42 epoch sec; seqNo:1130; 4056269.37 north; 590483.44 east 2021-02-12T23:08:40Z- AHRS_M2 msg: 1613171320.79 epoch sec; seqNo:1124 2021-02-12T23:08:40Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.14 psi 2021-02-12T23:08:40Z- handleDepth()-Depth_Keller msg: 1613171320802 epoch sec; seqNo:1131; depth 19.97 2021-02-12T23:08:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171320.82 epoch sec; seqNo:1131; 4056269.37 north; 590483.44 east 2021-02-12T23:08:41Z- AHRS_M2 msg: 1613171321.20 epoch sec; seqNo:1125 2021-02-12T23:08:41Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.13 psi 2021-02-12T23:08:41Z- handleDepth()-Depth_Keller msg: 1613171321201 epoch sec; seqNo:1132; depth 19.84 2021-02-12T23:08:41Z- handleDvl() - RDI_Pathfinder msg: 1613171321.22 epoch sec; seqNo:479 2021-02-12T23:08:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.35 , 88.12 , 96.77 , 66.22 2021-02-12T23:08:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.06 , -0.01 2021-02-12T23:08:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171321.23 epoch sec; seqNo:1132; 4056269.75 north; 590483.44 east 2021-02-12T23:08:41Z- AHRS_M2 msg: 1613171321.60 epoch sec; seqNo:1126 2021-02-12T23:08:41Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.12 psi 2021-02-12T23:08:41Z- handleDepth()-Depth_Keller msg: 1613171321604 epoch sec; seqNo:1133; depth 19.74 2021-02-12T23:08:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171321.62 epoch sec; seqNo:1133; 4056270.13 north; 590483.43 east 2021-02-12T23:08:42Z- AHRS_M2 msg: 1613171322.03 epoch sec; seqNo:1127 2021-02-12T23:08:42Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.11 psi 2021-02-12T23:08:42Z- handleDepth()-Depth_Keller msg: 1613171322041 epoch sec; seqNo:1134; depth 19.65 2021-02-12T23:08:42Z- handleDvl() - RDI_Pathfinder msg: 1613171322.06 epoch sec; seqNo:480 2021-02-12T23:08:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.17 , 87.04 , 96.77 , 65.81 2021-02-12T23:08:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.05 , -0.06 2021-02-12T23:08:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171322.07 epoch sec; seqNo:1134; 4056270.13 north; 590483.43 east 2021-02-12T23:08:42Z- AHRS_M2 msg: 1613171322.41 epoch sec; seqNo:1128 2021-02-12T23:08:42Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.10 psi 2021-02-12T23:08:42Z- handleDepth()-Depth_Keller msg: 1613171322414 epoch sec; seqNo:1135; depth 19.52 2021-02-12T23:08:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171322.42 epoch sec; seqNo:1135; 4056270.51 north; 590483.43 east 2021-02-12T23:08:42Z- AHRS_M2 msg: 1613171322.81 epoch sec; seqNo:1129 2021-02-12T23:08:42Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.10 psi 2021-02-12T23:08:42Z- handleDepth()-Depth_Keller msg: 1613171322816 epoch sec; seqNo:1136; depth 19.42 2021-02-12T23:08:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171322.84 epoch sec; seqNo:1136; 4056270.52 north; 590484.65 east 2021-02-12T23:08:43Z- AHRS_M2 msg: 1613171323.22 epoch sec; seqNo:1130 2021-02-12T23:08:43Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.10 psi 2021-02-12T23:08:43Z- handleDepth()-Depth_Keller msg: 1613171323221 epoch sec; seqNo:1137; depth 19.29 2021-02-12T23:08:43Z- handleDvl() - RDI_Pathfinder msg: 1613171323.23 epoch sec; seqNo:481 2021-02-12T23:08:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.58 , 87.10 , 97.92 , 66.63 2021-02-12T23:08:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.01 , -0.10 2021-02-12T23:08:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171323.26 epoch sec; seqNo:1137; 4056270.90 north; 590484.65 east 2021-02-12T23:08:43Z- AHRS_M2 msg: 1613171323.62 epoch sec; seqNo:1131 2021-02-12T23:08:43Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.10 psi 2021-02-12T23:08:43Z- handleDepth()-Depth_Keller msg: 1613171323624 epoch sec; seqNo:1138; depth 19.20 2021-02-12T23:08:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171323.63 epoch sec; seqNo:1138; 4056271.28 north; 590484.64 east 2021-02-12T23:08:44Z- AHRS_M2 msg: 1613171324.04 epoch sec; seqNo:1132 2021-02-12T23:08:44Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.10 psi 2021-02-12T23:08:44Z- handleDepth()-Depth_Keller msg: 1613171324047 epoch sec; seqNo:1139; depth 19.08 2021-02-12T23:08:44Z- handleDvl() - RDI_Pathfinder msg: 1613171324.06 epoch sec; seqNo:482 2021-02-12T23:08:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.58 , 87.04 , 96.77 , 66.22 2021-02-12T23:08:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.11 , -0.10 2021-02-12T23:08:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171324.07 epoch sec; seqNo:1139; 4056271.28 north; 590484.64 east 2021-02-12T23:08:44Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-12T23:08:44Z- waiting for 1613171322.21, time is 1613171324.07 2021-02-12T23:08:44Z- LcmTrn::updateTrn() equating pose 1613171324.07 and meas times 1613171324.07 2021-02-12T23:08:44Z- LcmTrn::updateTrn() >>>> pose time 1613171324.07, meas time 1613171324.07 <<<< 2021-02-12T23:08:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171324.07 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:08:44Z- TerrainNav::motionUpdate() - currEstimate = (4056271, 590484.64, 19.08) 2021-02-12T23:08:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:08:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:08:44Z- TNavBF::Projecting Measurements 2021-02-12T23:08:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:08:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:08:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:08:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:08:44Z- TNavFilter::4 good beams. 2021-02-12T23:08:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:08:44Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:08:44Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:08:44Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:08:44Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:08:44Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:08:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:08:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:08:44Z- TerrainMapOctree: (vehN, vehE ) = (4056271.28, 590484.64). 2021-02-12T23:08:44Z- TerrainMapOctree: Min Distance = 6242.24. 2021-02-12T23:08:44Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:08:44Z- There is no specific good beam that all particles have in common. 2021-02-12T23:08:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:08:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 8.66 2021-02-12T23:08:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:08:44Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 12.12 2021-02-12T23:08:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:08:44Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 14.98 2021-02-12T23:08:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:08:44Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 7.02 2021-02-12T23:08:44Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:08:44Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:08:44Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:08:44Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:08:44Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:08:44Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:08:44Z- Filter 0 No beams used in subcloud update 2021-02-12T23:08:44Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:08:44Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:08:44Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 806.6 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:08:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171324.07 sec. 2021-02-12T23:08:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:08:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:08:44Z- variances([0], [2], and psi_berg) = 806.633897 833.118658 0.000000 2021-02-12T23:08:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:08:44Z- LcmTrn::publishEstimates() - MLE : 4056238.78 590534.14 19.08 2021-02-12T23:08:44Z- LcmTrn::publishEstimates() - MMSE : 4056271.28 590484.64 19.08 2021-02-12T23:08:44Z- LcmTrn::publishEstimates() - COVAR: 806.63 833.12 0.80 0.00 2021-02-12T23:08:44Z- AHRS_M2 msg: 1613171324.43 epoch sec; seqNo:1133 2021-02-12T23:08:44Z- AHRS_M2 msg: 0.67 phi; 0.30 theta; -0.11 psi 2021-02-12T23:08:44Z- handleDepth()-Depth_Keller msg: 1613171324431 epoch sec; seqNo:1140; depth 18.97 2021-02-12T23:08:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171324.45 epoch sec; seqNo:1140; 4056271.66 north; 590484.64 east 2021-02-12T23:08:44Z- AHRS_M2 msg: 1613171324.83 epoch sec; seqNo:1134 2021-02-12T23:08:44Z- AHRS_M2 msg: 0.67 phi; 0.30 theta; -0.12 psi 2021-02-12T23:08:44Z- handleDepth()-Depth_Keller msg: 1613171324836 epoch sec; seqNo:1141; depth 18.85 2021-02-12T23:08:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171324.85 epoch sec; seqNo:1141; 4056272.03 north; 590484.63 east 2021-02-12T23:08:45Z- AHRS_M2 msg: 1613171325.24 epoch sec; seqNo:1135 2021-02-12T23:08:45Z- AHRS_M2 msg: 0.66 phi; 0.30 theta; -0.13 psi 2021-02-12T23:08:45Z- handleDepth()-Depth_Keller msg: 1613171325242 epoch sec; seqNo:1142; depth 18.74 2021-02-12T23:08:45Z- handleDvl() - RDI_Pathfinder msg: 1613171325.25 epoch sec; seqNo:483 2021-02-12T23:08:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.17 , 88.12 , 96.77 , 66.63 2021-02-12T23:08:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.19 , -0.01 2021-02-12T23:08:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171325.26 epoch sec; seqNo:1142; 4056272.03 north; 590484.63 east 2021-02-12T23:08:45Z- AHRS_M2 msg: 1613171325.64 epoch sec; seqNo:1136 2021-02-12T23:08:45Z- AHRS_M2 msg: 0.66 phi; 0.30 theta; -0.14 psi 2021-02-12T23:08:45Z- handleDepth()-Depth_Keller msg: 1613171325644 epoch sec; seqNo:1143; depth 18.63 2021-02-12T23:08:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171325.66 epoch sec; seqNo:1143; 4056272.41 north; 590484.63 east 2021-02-12T23:08:46Z- AHRS_M2 msg: 1613171326.05 epoch sec; seqNo:1137 2021-02-12T23:08:46Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.15 psi 2021-02-12T23:08:46Z- handleDepth()-Depth_Keller msg: 1613171326053 epoch sec; seqNo:1144; depth 18.51 2021-02-12T23:08:46Z- handleDvl() - RDI_Pathfinder msg: 1613171326.07 epoch sec; seqNo:484 2021-02-12T23:08:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.17 , 89.20 , 98.94 , 67.04 2021-02-12T23:08:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.20 , 0.13 2021-02-12T23:08:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171326.09 epoch sec; seqNo:1144; 4056272.80 north; 590485.85 east 2021-02-12T23:08:46Z- AHRS_M2 msg: 1613171326.45 epoch sec; seqNo:1138 2021-02-12T23:08:46Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-12T23:08:46Z- handleDepth()-Depth_Keller msg: 1613171326456 epoch sec; seqNo:1145; depth 18.39 2021-02-12T23:08:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171326.47 epoch sec; seqNo:1145; 4056273.18 north; 590485.84 east 2021-02-12T23:08:46Z- AHRS_M2 msg: 1613171326.85 epoch sec; seqNo:1139 2021-02-12T23:08:46Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-12T23:08:46Z- handleDepth()-Depth_Keller msg: 1613171326865 epoch sec; seqNo:1146; depth 18.28 2021-02-12T23:08:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171326.87 epoch sec; seqNo:1146; 4056273.56 north; 590485.84 east 2021-02-12T23:08:47Z- AHRS_M2 msg: 1613171327.26 epoch sec; seqNo:1140 2021-02-12T23:08:47Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:08:47Z- handleDepth()-Depth_Keller msg: 1613171327268 epoch sec; seqNo:1147; depth 18.17 2021-02-12T23:08:47Z- handleDvl() - RDI_Pathfinder msg: 1613171327.30 epoch sec; seqNo:485 2021-02-12T23:08:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.17 , 92.38 , 98.87 , 67.45 2021-02-12T23:08:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.08 , 0.21 2021-02-12T23:08:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171327.31 epoch sec; seqNo:1147; 4056273.94 north; 590485.83 east 2021-02-12T23:08:47Z- AHRS_M2 msg: 1613171327.66 epoch sec; seqNo:1141 2021-02-12T23:08:47Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:08:47Z- handleDepth()-Depth_Keller msg: 1613171327664 epoch sec; seqNo:1148; depth 18.06 2021-02-12T23:08:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171327.67 epoch sec; seqNo:1148; 4056273.94 north; 590485.83 east 2021-02-12T23:08:48Z- AHRS_M2 msg: 1613171328.06 epoch sec; seqNo:1142 2021-02-12T23:08:48Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:08:48Z- handleDepth()-Depth_Keller msg: 1613171328075 epoch sec; seqNo:1149; depth 17.96 2021-02-12T23:08:48Z- handleDvl() - RDI_Pathfinder msg: 1613171328.09 epoch sec; seqNo:486 2021-02-12T23:08:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.99 , 88.66 , 97.18 , 68.26 2021-02-12T23:08:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.06 , 0.20 2021-02-12T23:08:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171328.10 epoch sec; seqNo:1149; 4056274.32 north; 590485.83 east 2021-02-12T23:08:48Z- AHRS_M2 msg: 1613171328.47 epoch sec; seqNo:1143 2021-02-12T23:08:48Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:08:48Z- handleDepth()-Depth_Keller msg: 1613171328474 epoch sec; seqNo:1150; depth 17.84 2021-02-12T23:08:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171328.48 epoch sec; seqNo:1150; 4056274.71 north; 590487.05 east 2021-02-12T23:08:48Z- AHRS_M2 msg: 1613171328.87 epoch sec; seqNo:1144 2021-02-12T23:08:48Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:08:48Z- handleDepth()-Depth_Keller msg: 1613171328876 epoch sec; seqNo:1151; depth 17.74 2021-02-12T23:08:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171328.90 epoch sec; seqNo:1151; 4056275.09 north; 590487.04 east 2021-02-12T23:08:49Z- AHRS_M2 msg: 1613171329.28 epoch sec; seqNo:1145 2021-02-12T23:08:49Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-12T23:08:49Z- handleDepth()-Depth_Keller msg: 1613171329280 epoch sec; seqNo:1152; depth 17.63 2021-02-12T23:08:49Z- handleDvl() - RDI_Pathfinder msg: 1613171329.29 epoch sec; seqNo:487 2021-02-12T23:08:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.40 , 92.38 , 96.71 , 66.63 2021-02-12T23:08:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.15 , 0.07 2021-02-12T23:08:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171329.30 epoch sec; seqNo:1152; 4056275.09 north; 590487.04 east 2021-02-12T23:08:49Z- LcmTrn::handleMessages() - handling 44 messages took 0.76 ms and 129 calls... 2021-02-12T23:08:49Z- waiting for 1613171327.07, time is 1613171329.30 2021-02-12T23:08:49Z- LcmTrn::updateTrn() equating pose 1613171329.30 and meas times 1613171329.30 2021-02-12T23:08:49Z- LcmTrn::updateTrn() >>>> pose time 1613171329.30, meas time 1613171329.30 <<<< 2021-02-12T23:08:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171329.30 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:08:49Z- TerrainNav::motionUpdate() - currEstimate = (4056275, 590487.04, 17.63) 2021-02-12T23:08:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:08:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:08:49Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171329.30 sec.; No relevant navigation data available 2021-02-12T23:08:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:08:49Z- TNavBF::mmse selected from filter -1 2021-02-12T23:08:49Z- variances([0], [2], and psi_berg) = 806.353775 833.117928 0.000000 2021-02-12T23:08:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:08:49Z- LcmTrn::publishEstimates() - MLE : 4056242.59 590536.54 17.63 2021-02-12T23:08:49Z- LcmTrn::publishEstimates() - MMSE : 4056275.09 590487.04 17.63 2021-02-12T23:08:49Z- LcmTrn::publishEstimates() - COVAR: 806.35 833.12 1.03 0.00 2021-02-12T23:08:49Z- AHRS_M2 msg: 1613171329.68 epoch sec; seqNo:1146 2021-02-12T23:08:49Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-12T23:08:49Z- handleDepth()-Depth_Keller msg: 1613171329692 epoch sec; seqNo:1153; depth 17.51 2021-02-12T23:08:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171329.71 epoch sec; seqNo:1153; 4056275.47 north; 590487.04 east 2021-02-12T23:08:50Z- AHRS_M2 msg: 1613171330.08 epoch sec; seqNo:1147 2021-02-12T23:08:50Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-12T23:08:50Z- handleDepth()-Depth_Keller msg: 1613171330088 epoch sec; seqNo:1154; depth 17.38 2021-02-12T23:08:50Z- handleDvl() - RDI_Pathfinder msg: 1613171330.11 epoch sec; seqNo:488 2021-02-12T23:08:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 91.09 , 97.17 , 67.85 2021-02-12T23:08:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.14 , -0.05 2021-02-12T23:08:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171330.12 epoch sec; seqNo:1154; 4056275.85 north; 590487.04 east 2021-02-12T23:08:50Z- AHRS_M2 msg: 1613171330.49 epoch sec; seqNo:1148 2021-02-12T23:08:50Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-12T23:08:50Z- handleDepth()-Depth_Keller msg: 1613171330494 epoch sec; seqNo:1155; depth 17.26 2021-02-12T23:08:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171330.51 epoch sec; seqNo:1155; 4056275.86 north; 590488.26 east 2021-02-12T23:08:50Z- AHRS_M2 msg: 1613171330.89 epoch sec; seqNo:1149 2021-02-12T23:08:50Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.14 psi 2021-02-12T23:08:50Z- handleDepth()-Depth_Keller msg: 1613171330896 epoch sec; seqNo:1156; depth 17.15 2021-02-12T23:08:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171330.90 epoch sec; seqNo:1156; 4056276.24 north; 590488.25 east 2021-02-12T23:08:51Z- AHRS_M2 msg: 1613171331.30 epoch sec; seqNo:1150 2021-02-12T23:08:51Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.15 psi 2021-02-12T23:08:51Z- handleDepth()-Depth_Keller msg: 1613171331303 epoch sec; seqNo:1157; depth 17.06 2021-02-12T23:08:51Z- handleDvl() - RDI_Pathfinder msg: 1613171331.31 epoch sec; seqNo:489 2021-02-12T23:08:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.21 , 91.09 , 100.83 , 68.67 2021-02-12T23:08:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.04 , -0.11 2021-02-12T23:08:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171331.33 epoch sec; seqNo:1157; 4056276.62 north; 590488.25 east 2021-02-12T23:08:51Z- AHRS_M2 msg: 1613171331.70 epoch sec; seqNo:1151 2021-02-12T23:08:51Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.15 psi 2021-02-12T23:08:51Z- handleDepth()-Depth_Keller msg: 1613171331704 epoch sec; seqNo:1158; depth 16.93 2021-02-12T23:08:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171331.72 epoch sec; seqNo:1158; 4056276.62 north; 590488.25 east 2021-02-12T23:08:52Z- AHRS_M2 msg: 1613171332.11 epoch sec; seqNo:1152 2021-02-12T23:08:52Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.15 psi 2021-02-12T23:08:52Z- handleDepth()-Depth_Keller msg: 1613171332114 epoch sec; seqNo:1159; depth 16.80 2021-02-12T23:08:52Z- handleDvl() - RDI_Pathfinder msg: 1613171332.14 epoch sec; seqNo:490 2021-02-12T23:08:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 92.17 , 103.12 , 68.67 2021-02-12T23:08:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.05 , -0.05 2021-02-12T23:08:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171332.15 epoch sec; seqNo:1159; 4056277.00 north; 590488.24 east 2021-02-12T23:08:52Z- AHRS_M2 msg: 1613171332.51 epoch sec; seqNo:1153 2021-02-12T23:08:52Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.16 psi 2021-02-12T23:08:52Z- handleDepth()-Depth_Keller msg: 1613171332515 epoch sec; seqNo:1160; depth 16.69 2021-02-12T23:08:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171332.53 epoch sec; seqNo:1160; 4056277.00 north; 590488.24 east 2021-02-12T23:08:52Z- AHRS_M2 msg: 1613171332.92 epoch sec; seqNo:1154 2021-02-12T23:08:52Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.16 psi 2021-02-12T23:08:52Z- handleDepth()-Depth_Keller msg: 1613171332923 epoch sec; seqNo:1161; depth 16.57 2021-02-12T23:08:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171332.93 epoch sec; seqNo:1161; 4056277.39 north; 590489.46 east 2021-02-12T23:08:53Z- AHRS_M2 msg: 1613171333.32 epoch sec; seqNo:1155 2021-02-12T23:08:53Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.17 psi 2021-02-12T23:08:53Z- handleDepth()-Depth_Keller msg: 1613171333324 epoch sec; seqNo:1162; depth 16.45 2021-02-12T23:08:53Z- handleDvl() - RDI_Pathfinder msg: 1613171333.34 epoch sec; seqNo:491 2021-02-12T23:08:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.21 , 93.39 , 103.12 , 69.07 2021-02-12T23:08:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.04 , 0.03 2021-02-12T23:08:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171333.35 epoch sec; seqNo:1162; 4056277.77 north; 590489.46 east 2021-02-12T23:08:53Z- AHRS_M2 msg: 1613171333.72 epoch sec; seqNo:1156 2021-02-12T23:08:53Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.17 psi 2021-02-12T23:08:53Z- handleDepth()-Depth_Keller msg: 1613171333729 epoch sec; seqNo:1163; depth 16.34 2021-02-12T23:08:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171333.74 epoch sec; seqNo:1163; 4056277.77 north; 590489.46 east 2021-02-12T23:08:54Z- AHRS_M2 msg: 1613171334.13 epoch sec; seqNo:1157 2021-02-12T23:08:54Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:08:54Z- handleDepth()-Depth_Keller msg: 1613171334137 epoch sec; seqNo:1164; depth 16.22 2021-02-12T23:08:54Z- handleDvl() - RDI_Pathfinder msg: 1613171334.15 epoch sec; seqNo:492 2021-02-12T23:08:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 93.32 , 96.97 , 69.07 2021-02-12T23:08:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.01 , 0.05 2021-02-12T23:08:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171334.17 epoch sec; seqNo:1164; 4056278.15 north; 590489.45 east 2021-02-12T23:08:54Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 129 calls... 2021-02-12T23:08:54Z- waiting for 1613171332.30, time is 1613171334.17 2021-02-12T23:08:54Z- LcmTrn::updateTrn() equating pose 1613171334.17 and meas times 1613171334.17 2021-02-12T23:08:54Z- LcmTrn::updateTrn() >>>> pose time 1613171334.17, meas time 1613171334.17 <<<< 2021-02-12T23:08:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171334.17 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:08:54Z- TerrainNav::motionUpdate() - currEstimate = (4056278, 590489.45, 16.22) 2021-02-12T23:08:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:08:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:08:54Z- TNavBF::Projecting Measurements 2021-02-12T23:08:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:08:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:08:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:08:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:08:54Z- TNavFilter::4 good beams. 2021-02-12T23:08:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:08:54Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:08:54Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:08:54Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:08:54Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:08:54Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:08:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:08:54Z- TNavBF::mmse selected from filter -1 2021-02-12T23:08:54Z- TerrainMapOctree: (vehN, vehE ) = (4056278.15, 590489.45). 2021-02-12T23:08:54Z- TerrainMapOctree: Min Distance = 6233.86. 2021-02-12T23:08:54Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:08:54Z- There is no specific good beam that all particles have in common. 2021-02-12T23:08:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:08:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 8.95 2021-02-12T23:08:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:08:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 13.93 2021-02-12T23:08:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:08:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 15.05 2021-02-12T23:08:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:08:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-12T23:08:54Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:08:54Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:08:54Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:08:54Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:08:54Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:08:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:08:54Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:08:54Z- Filter 0 No beams used in subcloud update 2021-02-12T23:08:54Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:08:54Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:08:54Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 806.1 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:08:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171334.17 sec. 2021-02-12T23:08:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:08:54Z- TNavBF::mmse selected from filter -1 2021-02-12T23:08:54Z- variances([0], [2], and psi_berg) = 806.095281 833.117260 0.000000 2021-02-12T23:08:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:08:54Z- LcmTrn::publishEstimates() - MLE : 4056245.65 590538.95 16.22 2021-02-12T23:08:54Z- LcmTrn::publishEstimates() - MMSE : 4056278.15 590489.45 16.22 2021-02-12T23:08:54Z- LcmTrn::publishEstimates() - COVAR: 806.10 833.12 0.88 0.00 2021-02-12T23:08:54Z- AHRS_M2 msg: 1613171334.53 epoch sec; seqNo:1158 2021-02-12T23:08:54Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.17 psi 2021-02-12T23:08:54Z- handleDepth()-Depth_Keller msg: 1613171334535 epoch sec; seqNo:1165; depth 16.10 2021-02-12T23:08:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171334.55 epoch sec; seqNo:1165; 4056278.53 north; 590489.45 east 2021-02-12T23:08:54Z- AHRS_M2 msg: 1613171334.93 epoch sec; seqNo:1159 2021-02-12T23:08:54Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.17 psi 2021-02-12T23:08:54Z- handleDepth()-Depth_Keller msg: 1613171334936 epoch sec; seqNo:1166; depth 16.00 2021-02-12T23:08:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171334.96 epoch sec; seqNo:1166; 4056278.53 north; 590489.45 east 2021-02-12T23:08:55Z- AHRS_M2 msg: 1613171335.34 epoch sec; seqNo:1160 2021-02-12T23:08:55Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.17 psi 2021-02-12T23:08:55Z- handleDepth()-Depth_Keller msg: 1613171335341 epoch sec; seqNo:1167; depth 15.88 2021-02-12T23:08:55Z- handleDvl() - RDI_Pathfinder msg: 1613171335.35 epoch sec; seqNo:493 2021-02-12T23:08:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.21 , 93.46 , 103.19 , 69.07 2021-02-12T23:08:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.04 , 0.01 2021-02-12T23:08:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171335.36 epoch sec; seqNo:1167; 4056278.90 north; 590489.45 east 2021-02-12T23:08:55Z- AHRS_M2 msg: 1613171335.74 epoch sec; seqNo:1161 2021-02-12T23:08:55Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.17 psi 2021-02-12T23:08:55Z- handleDepth()-Depth_Keller msg: 1613171335749 epoch sec; seqNo:1168; depth 15.75 2021-02-12T23:08:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171335.77 epoch sec; seqNo:1168; 4056279.30 north; 590490.66 east 2021-02-12T23:08:56Z- AHRS_M2 msg: 1613171336.15 epoch sec; seqNo:1162 2021-02-12T23:08:56Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.17 psi 2021-02-12T23:08:56Z- handleDepth()-Depth_Keller msg: 1613171336150 epoch sec; seqNo:1169; depth 15.63 2021-02-12T23:08:56Z- handleDvl() - RDI_Pathfinder msg: 1613171336.16 epoch sec; seqNo:494 2021-02-12T23:08:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.21 , 95.75 , 96.97 , 69.48 2021-02-12T23:08:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.04 , -0.02 2021-02-12T23:08:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171336.17 epoch sec; seqNo:1169; 4056279.30 north; 590490.66 east 2021-02-12T23:08:56Z- AHRS_M2 msg: 1613171336.55 epoch sec; seqNo:1163 2021-02-12T23:08:56Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.18 psi 2021-02-12T23:08:56Z- handleDepth()-Depth_Keller msg: 1613171336557 epoch sec; seqNo:1170; depth 15.49 2021-02-12T23:08:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171336.57 epoch sec; seqNo:1170; 4056279.68 north; 590490.66 east 2021-02-12T23:08:56Z- AHRS_M2 msg: 1613171336.95 epoch sec; seqNo:1164 2021-02-12T23:08:56Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.18 psi 2021-02-12T23:08:56Z- handleDepth()-Depth_Keller msg: 1613171336969 epoch sec; seqNo:1171; depth 15.38 2021-02-12T23:08:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171336.99 epoch sec; seqNo:1171; 4056279.68 north; 590490.66 east 2021-02-12T23:08:57Z- AHRS_M2 msg: 1613171337.36 epoch sec; seqNo:1165 2021-02-12T23:08:57Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.18 psi 2021-02-12T23:08:57Z- handleDepth()-Depth_Keller msg: 1613171337365 epoch sec; seqNo:1172; depth 15.26 2021-02-12T23:08:57Z- handleDvl() - RDI_Pathfinder msg: 1613171337.38 epoch sec; seqNo:495 2021-02-12T23:08:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 94.61 , 104.34 , 69.89 2021-02-12T23:08:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.14 , 0.00 2021-02-12T23:08:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171337.39 epoch sec; seqNo:1172; 4056280.05 north; 590490.65 east 2021-02-12T23:08:57Z- AHRS_M2 msg: 1613171337.82 epoch sec; seqNo:1166 2021-02-12T23:08:57Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.18 psi 2021-02-12T23:08:57Z- handleDepth()-Depth_Keller msg: 1613171337821 epoch sec; seqNo:1173; depth 15.13 2021-02-12T23:08:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171337.83 epoch sec; seqNo:1173; 4056280.43 north; 590490.65 east 2021-02-12T23:08:58Z- AHRS_M2 msg: 1613171338.16 epoch sec; seqNo:1167 2021-02-12T23:08:58Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.18 psi 2021-02-12T23:08:58Z- handleDepth()-Depth_Keller msg: 1613171338178 epoch sec; seqNo:1174; depth 15.02 2021-02-12T23:08:58Z- handleDvl() - RDI_Pathfinder msg: 1613171338.19 epoch sec; seqNo:496 2021-02-12T23:08:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 94.47 , 104.21 , 69.89 2021-02-12T23:08:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.18 , 0.10 2021-02-12T23:08:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171338.20 epoch sec; seqNo:1174; 4056280.81 north; 590490.65 east 2021-02-12T23:08:58Z- AHRS_M2 msg: 1613171338.57 epoch sec; seqNo:1168 2021-02-12T23:08:58Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:08:58Z- handleDepth()-Depth_Keller msg: 1613171338572 epoch sec; seqNo:1175; depth 14.90 2021-02-12T23:08:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171338.58 epoch sec; seqNo:1175; 4056280.81 north; 590490.65 east 2021-02-12T23:08:58Z- AHRS_M2 msg: 1613171338.97 epoch sec; seqNo:1169 2021-02-12T23:08:58Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:08:58Z- handleDepth()-Depth_Keller msg: 1613171338985 epoch sec; seqNo:1176; depth 14.79 2021-02-12T23:08:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171339.03 epoch sec; seqNo:1176; 4056281.20 north; 590491.86 east 2021-02-12T23:08:59Z- AHRS_M2 msg: 1613171339.38 epoch sec; seqNo:1170 2021-02-12T23:08:59Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-12T23:08:59Z- handleDepth()-Depth_Keller msg: 1613171339389 epoch sec; seqNo:1177; depth 14.66 2021-02-12T23:08:59Z- handleDvl() - RDI_Pathfinder msg: 1613171339.40 epoch sec; seqNo:497 2021-02-12T23:08:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.02 , 95.69 , 96.90 , 71.12 2021-02-12T23:08:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.10 , 0.21 2021-02-12T23:08:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171339.41 epoch sec; seqNo:1177; 4056281.58 north; 590491.86 east 2021-02-12T23:08:59Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-12T23:08:59Z- waiting for 1613171337.17, time is 1613171339.41 2021-02-12T23:08:59Z- LcmTrn::updateTrn() equating pose 1613171339.41 and meas times 1613171339.41 2021-02-12T23:08:59Z- LcmTrn::updateTrn() >>>> pose time 1613171339.41, meas time 1613171339.41 <<<< 2021-02-12T23:08:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171339.41 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:08:59Z- TerrainNav::motionUpdate() - currEstimate = (4056282, 590491.86, 14.66) 2021-02-12T23:08:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:08:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:08:59Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171339.41 sec.; No relevant navigation data available 2021-02-12T23:08:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:08:59Z- TNavBF::mmse selected from filter -1 2021-02-12T23:08:59Z- variances([0], [2], and psi_berg) = 805.828665 833.116577 0.000000 2021-02-12T23:08:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:08:59Z- LcmTrn::publishEstimates() - MLE : 4056250.08 590444.36 14.66 2021-02-12T23:08:59Z- LcmTrn::publishEstimates() - MMSE : 4056281.58 590491.86 14.66 2021-02-12T23:08:59Z- LcmTrn::publishEstimates() - COVAR: 805.83 833.12 1.09 0.00 2021-02-12T23:08:59Z- AHRS_M2 msg: 1613171339.78 epoch sec; seqNo:1171 2021-02-12T23:08:59Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-12T23:08:59Z- handleDepth()-Depth_Keller msg: 1613171339784 epoch sec; seqNo:1178; depth 14.55 2021-02-12T23:08:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171339.80 epoch sec; seqNo:1178; 4056281.96 north; 590491.86 east 2021-02-12T23:09:00Z- AHRS_M2 msg: 1613171340.18 epoch sec; seqNo:1172 2021-02-12T23:09:00Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-12T23:09:00Z- handleDepth()-Depth_Keller msg: 1613171340193 epoch sec; seqNo:1179; depth 14.44 2021-02-12T23:09:00Z- handleDvl() - RDI_Pathfinder msg: 1613171340.20 epoch sec; seqNo:498 2021-02-12T23:09:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.84 , 94.54 , 105.49 , 70.71 2021-02-12T23:09:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.01 , 0.20 2021-02-12T23:09:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171340.23 epoch sec; seqNo:1179; 4056282.34 north; 590491.85 east 2021-02-12T23:09:00Z- AHRS_M2 msg: 1613171340.59 epoch sec; seqNo:1173 2021-02-12T23:09:00Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-12T23:09:00Z- handleDepth()-Depth_Keller msg: 1613171340597 epoch sec; seqNo:1180; depth 14.30 2021-02-12T23:09:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171340.61 epoch sec; seqNo:1180; 4056282.72 north; 590491.85 east 2021-02-12T23:09:00Z- AHRS_M2 msg: 1613171340.99 epoch sec; seqNo:1174 2021-02-12T23:09:00Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.18 psi 2021-02-12T23:09:01Z- handleDepth()-Depth_Keller msg: 1613171341005 epoch sec; seqNo:1181; depth 14.19 2021-02-12T23:09:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171341.02 epoch sec; seqNo:1181; 4056282.72 north; 590491.85 east 2021-02-12T23:09:01Z- AHRS_M2 msg: 1613171341.40 epoch sec; seqNo:1175 2021-02-12T23:09:01Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.19 psi 2021-02-12T23:09:01Z- handleDepth()-Depth_Keller msg: 1613171341400 epoch sec; seqNo:1182; depth 14.07 2021-02-12T23:09:01Z- handleDvl() - RDI_Pathfinder msg: 1613171341.42 epoch sec; seqNo:499 2021-02-12T23:09:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.25 , 96.84 , 105.36 , 71.53 2021-02-12T23:09:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.12 , 0.11 2021-02-12T23:09:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171341.43 epoch sec; seqNo:1182; 4056283.11 north; 590493.06 east 2021-02-12T23:09:01Z- AHRS_M2 msg: 1613171341.80 epoch sec; seqNo:1176 2021-02-12T23:09:01Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.20 psi 2021-02-12T23:09:01Z- handleDepth()-Depth_Keller msg: 1613171341804 epoch sec; seqNo:1183; depth 13.96 2021-02-12T23:09:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171341.82 epoch sec; seqNo:1183; 4056283.49 north; 590493.06 east 2021-02-12T23:09:02Z- AHRS_M2 msg: 1613171342.20 epoch sec; seqNo:1177 2021-02-12T23:09:02Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.21 psi 2021-02-12T23:09:02Z- handleDepth()-Depth_Keller msg: 1613171342209 epoch sec; seqNo:1184; depth 13.84 2021-02-12T23:09:02Z- handleDvl() - RDI_Pathfinder msg: 1613171342.22 epoch sec; seqNo:500 2021-02-12T23:09:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.25 , 96.77 , 106.51 , 72.35 2021-02-12T23:09:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.11 , -0.02 2021-02-12T23:09:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171342.23 epoch sec; seqNo:1184; 4056283.87 north; 590493.06 east 2021-02-12T23:09:02Z- AHRS_M2 msg: 1613171342.61 epoch sec; seqNo:1178 2021-02-12T23:09:02Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.21 psi 2021-02-12T23:09:02Z- handleDepth()-Depth_Keller msg: 1613171342614 epoch sec; seqNo:1185; depth 13.73 2021-02-12T23:09:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171342.62 epoch sec; seqNo:1185; 4056283.87 north; 590493.06 east 2021-02-12T23:09:03Z- AHRS_M2 msg: 1613171343.01 epoch sec; seqNo:1179 2021-02-12T23:09:03Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.21 psi 2021-02-12T23:09:03Z- handleDepth()-Depth_Keller msg: 1613171343023 epoch sec; seqNo:1186; depth 13.61 2021-02-12T23:09:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171343.03 epoch sec; seqNo:1186; 4056284.26 north; 590494.27 east 2021-02-12T23:09:03Z- AHRS_M2 msg: 1613171343.42 epoch sec; seqNo:1180 2021-02-12T23:09:03Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.22 psi 2021-02-12T23:09:03Z- handleDepth()-Depth_Keller msg: 1613171343437 epoch sec; seqNo:1187; depth 13.50 2021-02-12T23:09:03Z- handleDvl() - RDI_Pathfinder msg: 1613171343.46 epoch sec; seqNo:501 2021-02-12T23:09:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 101.57 , 110.09 , 72.35 2021-02-12T23:09:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.06 , -0.09 2021-02-12T23:09:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171343.48 epoch sec; seqNo:1187; 4056284.64 north; 590494.27 east 2021-02-12T23:09:03Z- AHRS_M2 msg: 1613171343.82 epoch sec; seqNo:1181 2021-02-12T23:09:03Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.22 psi 2021-02-12T23:09:03Z- handleDepth()-Depth_Keller msg: 1613171343824 epoch sec; seqNo:1188; depth 13.36 2021-02-12T23:09:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171343.84 epoch sec; seqNo:1188; 4056284.64 north; 590494.27 east 2021-02-12T23:09:04Z- AHRS_M2 msg: 1613171344.22 epoch sec; seqNo:1182 2021-02-12T23:09:04Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.22 psi 2021-02-12T23:09:04Z- handleDepth()-Depth_Keller msg: 1613171344233 epoch sec; seqNo:1189; depth 13.25 2021-02-12T23:09:04Z- handleDvl() - RDI_Pathfinder msg: 1613171344.24 epoch sec; seqNo:502 2021-02-12T23:09:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 96.57 , 109.95 , 73.16 2021-02-12T23:09:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.00 , -0.12 2021-02-12T23:09:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171344.26 epoch sec; seqNo:1189; 4056285.02 north; 590494.27 east 2021-02-12T23:09:04Z- LcmTrn::handleMessages() - handling 41 messages took 0.69 ms and 128 calls... 2021-02-12T23:09:04Z- waiting for 1613171342.41, time is 1613171344.26 2021-02-12T23:09:04Z- LcmTrn::updateTrn() equating pose 1613171344.26 and meas times 1613171344.26 2021-02-12T23:09:04Z- LcmTrn::updateTrn() >>>> pose time 1613171344.26, meas time 1613171344.26 <<<< 2021-02-12T23:09:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171344.26 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:09:04Z- TerrainNav::motionUpdate() - currEstimate = (4056285, 590494.27, 13.25) 2021-02-12T23:09:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:09:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:09:04Z- TNavBF::Projecting Measurements 2021-02-12T23:09:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:09:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:09:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:09:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:09:04Z- TNavFilter::4 good beams. 2021-02-12T23:09:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:09:04Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:09:04Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:09:04Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:09:04Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:09:04Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:09:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:09:04Z- TNavBF::mmse selected from filter -1 2021-02-12T23:09:04Z- TerrainMapOctree: (vehN, vehE ) = (4056285.02, 590494.27). 2021-02-12T23:09:04Z- TerrainMapOctree: Min Distance = 6225.48. 2021-02-12T23:09:04Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:09:04Z- There is no specific good beam that all particles have in common. 2021-02-12T23:09:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:09:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 9.35 2021-02-12T23:09:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:09:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 14.92 2021-02-12T23:09:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:09:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 19.34 2021-02-12T23:09:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:09:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-12T23:09:04Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:09:04Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:09:04Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:09:04Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:09:04Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:09:04Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:09:04Z- Filter 0 No beams used in subcloud update 2021-02-12T23:09:04Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:09:04Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:09:04Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 805.6 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:09:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171344.26 sec. 2021-02-12T23:09:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:09:04Z- TNavBF::mmse selected from filter -1 2021-02-12T23:09:04Z- variances([0], [2], and psi_berg) = 805.563797 833.115903 0.000000 2021-02-12T23:09:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:09:04Z- LcmTrn::publishEstimates() - MLE : 4056253.52 590446.77 13.25 2021-02-12T23:09:04Z- LcmTrn::publishEstimates() - MMSE : 4056285.02 590494.27 13.25 2021-02-12T23:09:04Z- LcmTrn::publishEstimates() - COVAR: 805.56 833.12 0.94 0.00 2021-02-12T23:09:04Z- AHRS_M2 msg: 1613171344.63 epoch sec; seqNo:1183 2021-02-12T23:09:04Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.21 psi 2021-02-12T23:09:04Z- handleDepth()-Depth_Keller msg: 1613171344632 epoch sec; seqNo:1190; depth 13.13 2021-02-12T23:09:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171344.64 epoch sec; seqNo:1190; 4056285.40 north; 590494.26 east 2021-02-12T23:09:05Z- AHRS_M2 msg: 1613171345.07 epoch sec; seqNo:1184 2021-02-12T23:09:05Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.21 psi 2021-02-12T23:09:05Z- handleDepth()-Depth_Keller msg: 1613171345076 epoch sec; seqNo:1191; depth 13.03 2021-02-12T23:09:05Z- handleDvl() - RDI_Pathfinder msg: 1613171345.09 epoch sec; seqNo:503 2021-02-12T23:09:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.84 , 96.63 , 111.24 , 72.35 2021-02-12T23:09:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.05 , -0.08 2021-02-12T23:09:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171345.10 epoch sec; seqNo:1191; 4056285.40 north; 590494.26 east 2021-02-12T23:09:05Z- AHRS_M2 msg: 1613171345.44 epoch sec; seqNo:1185 2021-02-12T23:09:05Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.21 psi 2021-02-12T23:09:05Z- handleDepth()-Depth_Keller msg: 1613171345440 epoch sec; seqNo:1192; depth 12.89 2021-02-12T23:09:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171345.46 epoch sec; seqNo:1192; 4056285.78 north; 590494.26 east 2021-02-12T23:09:05Z- AHRS_M2 msg: 1613171345.84 epoch sec; seqNo:1186 2021-02-12T23:09:05Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.20 psi 2021-02-12T23:09:05Z- handleDepth()-Depth_Keller msg: 1613171345844 epoch sec; seqNo:1193; depth 12.78 2021-02-12T23:09:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171345.87 epoch sec; seqNo:1193; 4056286.15 north; 590494.25 east 2021-02-12T23:09:06Z- AHRS_M2 msg: 1613171346.24 epoch sec; seqNo:1187 2021-02-12T23:09:06Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.20 psi 2021-02-12T23:09:06Z- handleDepth()-Depth_Keller msg: 1613171346250 epoch sec; seqNo:1194; depth 12.67 2021-02-12T23:09:06Z- handleDvl() - RDI_Pathfinder msg: 1613171346.26 epoch sec; seqNo:504 2021-02-12T23:09:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.25 , 96.63 , 110.02 , 72.35 2021-02-12T23:09:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.11 , -0.01 2021-02-12T23:09:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171346.27 epoch sec; seqNo:1194; 4056286.17 north; 590495.48 east 2021-02-12T23:09:06Z- AHRS_M2 msg: 1613171346.65 epoch sec; seqNo:1188 2021-02-12T23:09:06Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.20 psi 2021-02-12T23:09:06Z- handleDepth()-Depth_Keller msg: 1613171346657 epoch sec; seqNo:1195; depth 12.55 2021-02-12T23:09:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171346.67 epoch sec; seqNo:1195; 4056286.55 north; 590495.47 east 2021-02-12T23:09:07Z- AHRS_M2 msg: 1613171347.07 epoch sec; seqNo:1189 2021-02-12T23:09:07Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.20 psi 2021-02-12T23:09:07Z- handleDepth()-Depth_Keller msg: 1613171347079 epoch sec; seqNo:1196; depth 12.41 2021-02-12T23:09:07Z- handleDvl() - RDI_Pathfinder msg: 1613171347.10 epoch sec; seqNo:505 2021-02-12T23:09:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.84 , 96.63 , 108.81 , 71.53 2021-02-12T23:09:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.16 , 0.06 2021-02-12T23:09:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171347.11 epoch sec; seqNo:1196; 4056286.92 north; 590495.47 east 2021-02-12T23:09:07Z- AHRS_M2 msg: 1613171347.46 epoch sec; seqNo:1190 2021-02-12T23:09:07Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.20 psi 2021-02-12T23:09:07Z- handleDepth()-Depth_Keller msg: 1613171347461 epoch sec; seqNo:1197; depth 12.28 2021-02-12T23:09:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171347.47 epoch sec; seqNo:1197; 4056287.30 north; 590495.46 east 2021-02-12T23:09:07Z- AHRS_M2 msg: 1613171347.86 epoch sec; seqNo:1191 2021-02-12T23:09:07Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.20 psi 2021-02-12T23:09:07Z- handleDepth()-Depth_Keller msg: 1613171347866 epoch sec; seqNo:1198; depth 12.17 2021-02-12T23:09:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171347.88 epoch sec; seqNo:1198; 4056287.30 north; 590495.46 east 2021-02-12T23:09:08Z- AHRS_M2 msg: 1613171348.26 epoch sec; seqNo:1192 2021-02-12T23:09:08Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.19 psi 2021-02-12T23:09:08Z- handleDepth()-Depth_Keller msg: 1613171348268 epoch sec; seqNo:1199; depth 12.03 2021-02-12T23:09:08Z- handleDvl() - RDI_Pathfinder msg: 1613171348.29 epoch sec; seqNo:506 2021-02-12T23:09:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.48 , 100.35 , 110.09 , 72.35 2021-02-12T23:09:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.18 , 0.14 2021-02-12T23:09:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171348.30 epoch sec; seqNo:1199; 4056287.68 north; 590495.46 east 2021-02-12T23:09:08Z- AHRS_M2 msg: 1613171348.67 epoch sec; seqNo:1193 2021-02-12T23:09:08Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.18 psi 2021-02-12T23:09:08Z- handleDepth()-Depth_Keller msg: 1613171348672 epoch sec; seqNo:1200; depth 11.92 2021-02-12T23:09:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171348.68 epoch sec; seqNo:1200; 4056288.06 north; 590495.46 east 2021-02-12T23:09:09Z- AHRS_M2 msg: 1613171349.09 epoch sec; seqNo:1194 2021-02-12T23:09:09Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.17 psi 2021-02-12T23:09:09Z- handleDepth()-Depth_Keller msg: 1613171349091 epoch sec; seqNo:1201; depth 11.81 2021-02-12T23:09:09Z- handleDvl() - RDI_Pathfinder msg: 1613171349.11 epoch sec; seqNo:507 2021-02-12T23:09:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.88 , 96.50 , 112.32 , 71.94 2021-02-12T23:09:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.12 , 0.14 2021-02-12T23:09:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171349.13 epoch sec; seqNo:1201; 4056288.44 north; 590495.45 east 2021-02-12T23:09:09Z- AHRS_M2 msg: 1613171349.47 epoch sec; seqNo:1195 2021-02-12T23:09:09Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.16 psi 2021-02-12T23:09:09Z- handleDepth()-Depth_Keller msg: 1613171349483 epoch sec; seqNo:1202; depth 11.66 2021-02-12T23:09:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171349.50 epoch sec; seqNo:1202; 4056288.44 north; 590495.45 east 2021-02-12T23:09:09Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-12T23:09:09Z- waiting for 1613171347.26, time is 1613171349.50 2021-02-12T23:09:09Z- LcmTrn::updateTrn() >>>> pose time 1613171349.50, meas time 1613171349.11 <<<< 2021-02-12T23:09:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171349.11 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:09:09Z- TerrainNav::motionUpdate() - currEstimate = (4056288, 590495.45, 11.66) 2021-02-12T23:09:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:09:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:09:09Z- TNavBF::Projecting Measurements 2021-02-12T23:09:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:09:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:09:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:09:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:09:09Z- TNavFilter::4 good beams. 2021-02-12T23:09:09Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:09:09Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:09:09Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:09:09Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:09:09Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:09:09Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:09:09Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:09:09Z- TNavBF::mmse selected from filter -1 2021-02-12T23:09:09Z- TerrainMapOctree: (vehN, vehE ) = (4056288.18, 590495.36). 2021-02-12T23:09:09Z- TerrainMapOctree: Min Distance = 6222.29. 2021-02-12T23:09:09Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:09:09Z- There is no specific good beam that all particles have in common. 2021-02-12T23:09:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:09:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 9.96 2021-02-12T23:09:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:09:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 14.90 2021-02-12T23:09:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:09:09Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 20.19 2021-02-12T23:09:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:09:09Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 8.28 2021-02-12T23:09:09Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:09:09Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:09:09Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:09:09Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:09:09Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:09:09Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:09:09Z- Filter 0 No beams used in subcloud update 2021-02-12T23:09:09Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:09:09Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:09:09Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 805.3 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:09:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171349.11 sec. 2021-02-12T23:09:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:09:09Z- TNavBF::mmse selected from filter -1 2021-02-12T23:09:09Z- variances([0], [2], and psi_berg) = 805.262137 833.115143 0.000000 2021-02-12T23:09:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:09:09Z- LcmTrn::publishEstimates() - MLE : 4056256.94 590448.95 11.66 2021-02-12T23:09:09Z- LcmTrn::publishEstimates() - MMSE : 4056288.44 590495.45 11.66 2021-02-12T23:09:09Z- LcmTrn::publishEstimates() - COVAR: 805.26 833.12 0.85 0.00 2021-02-12T23:09:09Z- AHRS_M2 msg: 1613171349.88 epoch sec; seqNo:1196 2021-02-12T23:09:09Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.15 psi 2021-02-12T23:09:09Z- handleDepth()-Depth_Keller msg: 1613171349884 epoch sec; seqNo:1203; depth 11.55 2021-02-12T23:09:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171349.90 epoch sec; seqNo:1203; 4056288.83 north; 590496.67 east 2021-02-12T23:09:10Z- AHRS_M2 msg: 1613171350.29 epoch sec; seqNo:1197 2021-02-12T23:09:10Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.14 psi 2021-02-12T23:09:10Z- handleDepth()-Depth_Keller msg: 1613171350301 epoch sec; seqNo:1204; depth 11.43 2021-02-12T23:09:10Z- handleDvl() - RDI_Pathfinder msg: 1613171350.31 epoch sec; seqNo:508 2021-02-12T23:09:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.18 , 97.78 , 109.95 , 73.15 2021-02-12T23:09:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.11 , 0.11 2021-02-12T23:09:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171350.33 epoch sec; seqNo:1204; 4056289.21 north; 590496.66 east 2021-02-12T23:09:10Z- AHRS_M2 msg: 1613171350.69 epoch sec; seqNo:1198 2021-02-12T23:09:10Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.13 psi 2021-02-12T23:09:10Z- handleDepth()-Depth_Keller msg: 1613171350695 epoch sec; seqNo:1205; depth 11.30 2021-02-12T23:09:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171350.71 epoch sec; seqNo:1205; 4056289.59 north; 590496.66 east 2021-02-12T23:09:11Z- AHRS_M2 msg: 1613171351.10 epoch sec; seqNo:1199 2021-02-12T23:09:11Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.12 psi 2021-02-12T23:09:11Z- handleDepth()-Depth_Keller msg: 1613171351105 epoch sec; seqNo:1206; depth 11.19 2021-02-12T23:09:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171351.13 epoch sec; seqNo:1206; 4056289.97 north; 590496.66 east 2021-02-12T23:09:11Z- AHRS_M2 msg: 1613171351.50 epoch sec; seqNo:1200 2021-02-12T23:09:11Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.11 psi 2021-02-12T23:09:11Z- handleDepth()-Depth_Keller msg: 1613171351500 epoch sec; seqNo:1207; depth 11.06 2021-02-12T23:09:11Z- handleDvl() - RDI_Pathfinder msg: 1613171351.52 epoch sec; seqNo:509 2021-02-12T23:09:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 98.86 , 112.25 , 71.93 2021-02-12T23:09:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.10 , 0.05 2021-02-12T23:09:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171351.53 epoch sec; seqNo:1207; 4056289.97 north; 590496.66 east 2021-02-12T23:09:11Z- AHRS_M2 msg: 1613171351.90 epoch sec; seqNo:1201 2021-02-12T23:09:11Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.11 psi 2021-02-12T23:09:11Z- handleDepth()-Depth_Keller msg: 1613171351904 epoch sec; seqNo:1208; depth 10.93 2021-02-12T23:09:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171351.91 epoch sec; seqNo:1208; 4056290.35 north; 590496.65 east 2021-02-12T23:09:12Z- AHRS_M2 msg: 1613171352.31 epoch sec; seqNo:1202 2021-02-12T23:09:12Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.12 psi 2021-02-12T23:09:12Z- handleDepth()-Depth_Keller msg: 1613171352313 epoch sec; seqNo:1209; depth 10.83 2021-02-12T23:09:12Z- handleDvl() - RDI_Pathfinder msg: 1613171352.33 epoch sec; seqNo:510 2021-02-12T23:09:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.54 , 96.36 , 109.88 , 73.97 2021-02-12T23:09:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.14 , 0.04 2021-02-12T23:09:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171352.34 epoch sec; seqNo:1209; 4056290.73 north; 590496.65 east 2021-02-12T23:09:12Z- AHRS_M2 msg: 1613171352.71 epoch sec; seqNo:1203 2021-02-12T23:09:12Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.14 psi 2021-02-12T23:09:12Z- handleDepth()-Depth_Keller msg: 1613171352714 epoch sec; seqNo:1210; depth 10.69 2021-02-12T23:09:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171352.72 epoch sec; seqNo:1210; 4056291.10 north; 590496.64 east 2021-02-12T23:09:13Z- AHRS_M2 msg: 1613171353.12 epoch sec; seqNo:1204 2021-02-12T23:09:13Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-12T23:09:13Z- handleDepth()-Depth_Keller msg: 1613171353143 epoch sec; seqNo:1211; depth 10.57 2021-02-12T23:09:13Z- handleDvl() - RDI_Pathfinder msg: 1613171353.16 epoch sec; seqNo:511 2021-02-12T23:09:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 96.43 , 111.03 , 73.97 2021-02-12T23:09:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.14 , 0.09 2021-02-12T23:09:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171353.17 epoch sec; seqNo:1211; 4056291.50 north; 590497.86 east 2021-02-12T23:09:13Z- AHRS_M2 msg: 1613171353.52 epoch sec; seqNo:1205 2021-02-12T23:09:13Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-12T23:09:13Z- handleDepth()-Depth_Keller msg: 1613171353520 epoch sec; seqNo:1212; depth 10.43 2021-02-12T23:09:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171353.54 epoch sec; seqNo:1212; 4056291.50 north; 590497.86 east 2021-02-12T23:09:13Z- AHRS_M2 msg: 1613171353.92 epoch sec; seqNo:1206 2021-02-12T23:09:13Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-12T23:09:13Z- handleDepth()-Depth_Keller msg: 1613171353929 epoch sec; seqNo:1213; depth 10.33 2021-02-12T23:09:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171353.94 epoch sec; seqNo:1213; 4056291.88 north; 590497.86 east 2021-02-12T23:09:14Z- AHRS_M2 msg: 1613171354.32 epoch sec; seqNo:1207 2021-02-12T23:09:14Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.14 psi 2021-02-12T23:09:14Z- handleDepth()-Depth_Keller msg: 1613171354331 epoch sec; seqNo:1214; depth 10.22 2021-02-12T23:09:14Z- handleDvl() - RDI_Pathfinder msg: 1613171354.34 epoch sec; seqNo:512 2021-02-12T23:09:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 96.43 , 111.03 , 73.97 2021-02-12T23:09:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , 0.13 2021-02-12T23:09:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171354.35 epoch sec; seqNo:1214; 4056292.25 north; 590497.85 east 2021-02-12T23:09:14Z- AHRS_M2 msg: 1613171354.73 epoch sec; seqNo:1208 2021-02-12T23:09:14Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.14 psi 2021-02-12T23:09:14Z- handleDepth()-Depth_Keller msg: 1613171354737 epoch sec; seqNo:1215; depth 10.10 2021-02-12T23:09:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171354.75 epoch sec; seqNo:1215; 4056292.63 north; 590497.85 east 2021-02-12T23:09:14Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-12T23:09:14Z- waiting for 1613171352.50, time is 1613171354.75 2021-02-12T23:09:14Z- LcmTrn::updateTrn() >>>> pose time 1613171354.75, meas time 1613171354.34 <<<< 2021-02-12T23:09:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171354.34 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:09:14Z- TerrainNav::motionUpdate() - currEstimate = (4056293, 590497.85, 10.10) 2021-02-12T23:09:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:09:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:09:14Z- TNavBF::Projecting Measurements 2021-02-12T23:09:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:09:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:09:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:09:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:09:14Z- TNavFilter::4 good beams. 2021-02-12T23:09:14Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:09:14Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:09:14Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:09:14Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:09:14Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:09:14Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:09:14Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:09:14Z- TNavBF::mmse selected from filter -1 2021-02-12T23:09:14Z- TerrainMapOctree: (vehN, vehE ) = (4056292.31, 590497.66). 2021-02-12T23:09:14Z- TerrainMapOctree: Min Distance = 6217.61. 2021-02-12T23:09:14Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:09:14Z- There is no specific good beam that all particles have in common. 2021-02-12T23:09:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:09:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 11.69 2021-02-12T23:09:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:09:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 14.88 2021-02-12T23:09:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:09:14Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 19.72 2021-02-12T23:09:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:09:14Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 8.76 2021-02-12T23:09:14Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:09:14Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:09:14Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:09:14Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:09:14Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:09:14Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:09:14Z- Filter 0 No beams used in subcloud update 2021-02-12T23:09:14Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:09:14Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:09:14Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 805.0 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:09:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171354.34 sec. 2021-02-12T23:09:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:09:14Z- TNavBF::mmse selected from filter -1 2021-02-12T23:09:14Z- variances([0], [2], and psi_berg) = 804.915487 833.114277 0.000000 2021-02-12T23:09:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:09:14Z- LcmTrn::publishEstimates() - MLE : 4056261.13 590451.35 10.10 2021-02-12T23:09:14Z- LcmTrn::publishEstimates() - MMSE : 4056292.63 590497.85 10.10 2021-02-12T23:09:14Z- LcmTrn::publishEstimates() - COVAR: 804.92 833.11 0.85 0.00 2021-02-12T23:09:15Z- AHRS_M2 msg: 1613171355.16 epoch sec; seqNo:1209 2021-02-12T23:09:15Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.13 psi 2021-02-12T23:09:15Z- handleDepth()-Depth_Keller msg: 1613171355161 epoch sec; seqNo:1216; depth 9.98 2021-02-12T23:09:15Z- handleDvl() - RDI_Pathfinder msg: 1613171355.18 epoch sec; seqNo:513 2021-02-12T23:09:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.91 , 96.43 , 111.03 , 74.79 2021-02-12T23:09:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , 0.09 2021-02-12T23:09:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171355.19 epoch sec; seqNo:1216; 4056293.01 north; 590497.85 east 2021-02-12T23:09:15Z- AHRS_M2 msg: 1613171355.54 epoch sec; seqNo:1210 2021-02-12T23:09:15Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.12 psi 2021-02-12T23:09:15Z- handleDepth()-Depth_Keller msg: 1613171355542 epoch sec; seqNo:1217; depth 9.86 2021-02-12T23:09:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171355.55 epoch sec; seqNo:1217; 4056293.02 north; 590499.07 east 2021-02-12T23:09:15Z- AHRS_M2 msg: 1613171355.94 epoch sec; seqNo:1211 2021-02-12T23:09:15Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.12 psi 2021-02-12T23:09:15Z- handleDepth()-Depth_Keller msg: 1613171355944 epoch sec; seqNo:1218; depth 9.76 2021-02-12T23:09:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171355.96 epoch sec; seqNo:1218; 4056293.40 north; 590499.06 east 2021-02-12T23:09:16Z- AHRS_M2 msg: 1613171356.34 epoch sec; seqNo:1212 2021-02-12T23:09:16Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.12 psi 2021-02-12T23:09:16Z- handleDepth()-Depth_Keller msg: 1613171356353 epoch sec; seqNo:1219; depth 9.62 2021-02-12T23:09:16Z- handleDvl() - RDI_Pathfinder msg: 1613171356.36 epoch sec; seqNo:514 2021-02-12T23:09:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 96.29 , 113.87 , 74.38 2021-02-12T23:09:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.08 , -0.03 2021-02-12T23:09:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171356.38 epoch sec; seqNo:1219; 4056293.78 north; 590499.06 east 2021-02-12T23:09:16Z- AHRS_M2 msg: 1613171356.75 epoch sec; seqNo:1213 2021-02-12T23:09:16Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.13 psi 2021-02-12T23:09:16Z- handleDepth()-Depth_Keller msg: 1613171356752 epoch sec; seqNo:1220; depth 9.50 2021-02-12T23:09:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171356.76 epoch sec; seqNo:1220; 4056293.78 north; 590499.06 east 2021-02-12T23:09:17Z- AHRS_M2 msg: 1613171357.15 epoch sec; seqNo:1214 2021-02-12T23:09:17Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.13 psi 2021-02-12T23:09:17Z- handleDepth()-Depth_Keller msg: 1613171357162 epoch sec; seqNo:1221; depth 9.38 2021-02-12T23:09:17Z- handleDvl() - RDI_Pathfinder msg: 1613171357.17 epoch sec; seqNo:515 2021-02-12T23:09:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 96.36 , 112.58 , 75.20 2021-02-12T23:09:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , -0.05 2021-02-12T23:09:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171357.18 epoch sec; seqNo:1221; 4056294.16 north; 590499.05 east 2021-02-12T23:09:17Z- AHRS_M2 msg: 1613171357.56 epoch sec; seqNo:1215 2021-02-12T23:09:17Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.13 psi 2021-02-12T23:09:17Z- handleDepth()-Depth_Keller msg: 1613171357560 epoch sec; seqNo:1222; depth 9.25 2021-02-12T23:09:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171357.57 epoch sec; seqNo:1222; 4056294.54 north; 590499.05 east 2021-02-12T23:09:17Z- AHRS_M2 msg: 1613171357.96 epoch sec; seqNo:1216 2021-02-12T23:09:17Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.13 psi 2021-02-12T23:09:17Z- handleDepth()-Depth_Keller msg: 1613171357964 epoch sec; seqNo:1223; depth 9.14 2021-02-12T23:09:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171357.97 epoch sec; seqNo:1223; 4056294.54 north; 590499.05 east 2021-02-12T23:09:18Z- AHRS_M2 msg: 1613171358.37 epoch sec; seqNo:1217 2021-02-12T23:09:18Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.12 psi 2021-02-12T23:09:18Z- handleDepth()-Depth_Keller msg: 1613171358374 epoch sec; seqNo:1224; depth 9.03 2021-02-12T23:09:18Z- handleDvl() - RDI_Pathfinder msg: 1613171358.39 epoch sec; seqNo:516 2021-02-12T23:09:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 96.36 , 109.89 , 75.61 2021-02-12T23:09:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , 0.06 2021-02-12T23:09:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171358.40 epoch sec; seqNo:1224; 4056294.93 north; 590500.27 east 2021-02-12T23:09:18Z- AHRS_M2 msg: 1613171358.77 epoch sec; seqNo:1218 2021-02-12T23:09:18Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.12 psi 2021-02-12T23:09:18Z- handleDepth()-Depth_Keller msg: 1613171358781 epoch sec; seqNo:1225; depth 8.90 2021-02-12T23:09:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171358.79 epoch sec; seqNo:1225; 4056295.31 north; 590500.26 east 2021-02-12T23:09:19Z- AHRS_M2 msg: 1613171359.17 epoch sec; seqNo:1219 2021-02-12T23:09:19Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.11 psi 2021-02-12T23:09:19Z- handleDepth()-Depth_Keller msg: 1613171359182 epoch sec; seqNo:1226; depth 8.77 2021-02-12T23:09:19Z- handleDvl() - RDI_Pathfinder msg: 1613171359.20 epoch sec; seqNo:517 2021-02-12T23:09:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.86 , 114.69 , 74.39 2021-02-12T23:09:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , 0.06 2021-02-12T23:09:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171359.21 epoch sec; seqNo:1226; 4056295.69 north; 590500.26 east 2021-02-12T23:09:19Z- AHRS_M2 msg: 1613171359.58 epoch sec; seqNo:1220 2021-02-12T23:09:19Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.11 psi 2021-02-12T23:09:19Z- handleDepth()-Depth_Keller msg: 1613171359580 epoch sec; seqNo:1227; depth 8.64 2021-02-12T23:09:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171359.60 epoch sec; seqNo:1227; 4056295.69 north; 590500.26 east 2021-02-12T23:09:19Z- LcmTrn::handleMessages() - handling 41 messages took 0.67 ms and 128 calls... 2021-02-12T23:09:19Z- waiting for 1613171357.75, time is 1613171359.60 2021-02-12T23:09:19Z- LcmTrn::updateTrn() >>>> pose time 1613171359.60, meas time 1613171359.20 <<<< 2021-02-12T23:09:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171359.20 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:09:19Z- TerrainNav::motionUpdate() - currEstimate = (4056296, 590500.26, 8.64) 2021-02-12T23:09:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:09:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:09:19Z- TNavBF::Projecting Measurements 2021-02-12T23:09:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:09:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:09:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:09:19Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:09:19Z- TNavFilter::3 good beams. 2021-02-12T23:09:19Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:09:19Z- TNavBF:: beamIndex[0] = 1 2021-02-12T23:09:19Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:09:19Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:09:19Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:09:19Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:09:19Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:09:19Z- TNavBF::mmse selected from filter -1 2021-02-12T23:09:19Z- TerrainMapOctree: (vehN, vehE ) = (4056295.44, 590500.06). 2021-02-12T23:09:19Z- TerrainMapOctree: Min Distance = 6213.67. 2021-02-12T23:09:19Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:09:19Z- There is no specific good beam that all particles have in common. 2021-02-12T23:09:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:09:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 15.64 2021-02-12T23:09:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:09:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 21.04 2021-02-12T23:09:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:09:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-12T23:09:19Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:09:19Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:09:19Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:09:19Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:09:19Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:09:19Z- Filter 0 No beams used in subcloud update 2021-02-12T23:09:19Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:09:19Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:09:19Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 804.7 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:09:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171359.20 sec. 2021-02-12T23:09:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:09:19Z- TNavBF::mmse selected from filter -1 2021-02-12T23:09:19Z- variances([0], [2], and psi_berg) = 804.605253 833.113510 0.000000 2021-02-12T23:09:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:09:19Z- LcmTrn::publishEstimates() - MLE : 4056264.19 590453.76 8.64 2021-02-12T23:09:19Z- LcmTrn::publishEstimates() - MMSE : 4056295.69 590500.26 8.64 2021-02-12T23:09:19Z- LcmTrn::publishEstimates() - COVAR: 804.61 833.11 0.88 0.00 2021-02-12T23:09:19Z- AHRS_M2 msg: 1613171359.98 epoch sec; seqNo:1221 2021-02-12T23:09:19Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.11 psi 2021-02-12T23:09:19Z- handleDepth()-Depth_Keller msg: 1613171359984 epoch sec; seqNo:1228; depth 8.52 2021-02-12T23:09:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171359.99 epoch sec; seqNo:1228; 4056296.07 north; 590500.26 east 2021-02-12T23:09:20Z- AHRS_M2 msg: 1613171360.38 epoch sec; seqNo:1222 2021-02-12T23:09:20Z- AHRS_M2 msg: 0.68 phi; 0.35 theta; -0.10 psi 2021-02-12T23:09:20Z- handleDepth()-Depth_Keller msg: 1613171360397 epoch sec; seqNo:1229; depth 8.40 2021-02-12T23:09:20Z- handleDvl() - RDI_Pathfinder msg: 1613171360.41 epoch sec; seqNo:518 2021-02-12T23:09:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.26 , 96.02 , 114.95 , 76.02 2021-02-12T23:09:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.07 , -0.09 2021-02-12T23:09:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171360.42 epoch sec; seqNo:1229; 4056296.46 north; 590501.47 east 2021-02-12T23:09:20Z- AHRS_M2 msg: 1613171360.79 epoch sec; seqNo:1223 2021-02-12T23:09:20Z- AHRS_M2 msg: 0.69 phi; 0.34 theta; -0.11 psi 2021-02-12T23:09:20Z- handleDepth()-Depth_Keller msg: 1613171360797 epoch sec; seqNo:1230; depth 8.28 2021-02-12T23:09:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171360.82 epoch sec; seqNo:1230; 4056296.46 north; 590501.47 east 2021-02-12T23:09:21Z- AHRS_M2 msg: 1613171361.19 epoch sec; seqNo:1224 2021-02-12T23:09:21Z- AHRS_M2 msg: 0.69 phi; 0.34 theta; -0.11 psi 2021-02-12T23:09:21Z- handleDepth()-Depth_Keller msg: 1613171361196 epoch sec; seqNo:1231; depth 8.15 2021-02-12T23:09:21Z- handleDvl() - RDI_Pathfinder msg: 1613171361.21 epoch sec; seqNo:519 2021-02-12T23:09:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.46 , 96.22 , 115.15 , 73.56 2021-02-12T23:09:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.06 , -0.18 2021-02-12T23:09:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171361.23 epoch sec; seqNo:1231; 4056296.84 north; 590501.47 east 2021-02-12T23:09:21Z- AHRS_M2 msg: 1613171361.60 epoch sec; seqNo:1225 2021-02-12T23:09:21Z- AHRS_M2 msg: 0.70 phi; 0.33 theta; -0.12 psi 2021-02-12T23:09:21Z- handleDepth()-Depth_Keller msg: 1613171361600 epoch sec; seqNo:1232; depth 8.01 2021-02-12T23:09:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171361.61 epoch sec; seqNo:1232; 4056296.84 north; 590501.47 east 2021-02-12T23:09:22Z- AHRS_M2 msg: 1613171362.00 epoch sec; seqNo:1226 2021-02-12T23:09:22Z- AHRS_M2 msg: 0.70 phi; 0.33 theta; -0.13 psi 2021-02-12T23:09:22Z- handleDepth()-Depth_Keller msg: 1613171362012 epoch sec; seqNo:1233; depth 7.89 2021-02-12T23:09:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171362.03 epoch sec; seqNo:1233; 4056297.22 north; 590501.46 east 2021-02-12T23:09:22Z- AHRS_M2 msg: 1613171362.40 epoch sec; seqNo:1227 2021-02-12T23:09:22Z- AHRS_M2 msg: 0.70 phi; 0.33 theta; -0.14 psi 2021-02-12T23:09:22Z- handleDepth()-Depth_Keller msg: 1613171362408 epoch sec; seqNo:1234; depth 7.74 2021-02-12T23:09:22Z- handleDvl() - RDI_Pathfinder msg: 1613171362.42 epoch sec; seqNo:520 2021-02-12T23:09:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.11 , 96.22 , 116.50 , 75.20 2021-02-12T23:09:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.61 , -0.23 , -0.06 2021-02-12T23:09:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171362.44 epoch sec; seqNo:1234; 4056297.22 north; 590501.46 east 2021-02-12T23:09:22Z- AHRS_M2 msg: 1613171362.81 epoch sec; seqNo:1228 2021-02-12T23:09:22Z- AHRS_M2 msg: 0.69 phi; 0.32 theta; -0.16 psi 2021-02-12T23:09:22Z- handleDepth()-Depth_Keller msg: 1613171362812 epoch sec; seqNo:1235; depth 7.61 2021-02-12T23:09:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171362.82 epoch sec; seqNo:1235; 4056297.60 north; 590501.46 east 2021-02-12T23:09:23Z- AHRS_M2 msg: 1613171363.21 epoch sec; seqNo:1229 2021-02-12T23:09:23Z- AHRS_M2 msg: 0.68 phi; 0.31 theta; -0.17 psi 2021-02-12T23:09:23Z- handleDepth()-Depth_Keller msg: 1613171363217 epoch sec; seqNo:1236; depth 7.49 2021-02-12T23:09:23Z- handleDvl() - RDI_Pathfinder msg: 1613171363.23 epoch sec; seqNo:521 2021-02-12T23:09:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.05 , 102.98 , 115.15 , 75.20 2021-02-12T23:09:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.61 , -0.26 , 0.18 2021-02-12T23:09:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171363.24 epoch sec; seqNo:1236; 4056297.98 north; 590501.46 east 2021-02-12T23:09:23Z- AHRS_M2 msg: 1613171363.62 epoch sec; seqNo:1230 2021-02-12T23:09:23Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.18 psi 2021-02-12T23:09:23Z- handleDepth()-Depth_Keller msg: 1613171363630 epoch sec; seqNo:1237; depth 7.34 2021-02-12T23:09:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171363.64 epoch sec; seqNo:1237; 4056297.98 north; 590501.46 east 2021-02-12T23:09:24Z- AHRS_M2 msg: 1613171364.02 epoch sec; seqNo:1231 2021-02-12T23:09:24Z- AHRS_M2 msg: 0.66 phi; 0.30 theta; -0.19 psi 2021-02-12T23:09:24Z- handleDepth()-Depth_Keller msg: 1613171364024 epoch sec; seqNo:1238; depth 7.22 2021-02-12T23:09:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171364.03 epoch sec; seqNo:1238; 4056298.35 north; 590501.45 east 2021-02-12T23:09:24Z- AHRS_M2 msg: 1613171364.42 epoch sec; seqNo:1232 2021-02-12T23:09:24Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.21 psi 2021-02-12T23:09:24Z- handleDepth()-Depth_Keller msg: 1613171364429 epoch sec; seqNo:1239; depth 7.09 2021-02-12T23:09:24Z- handleDvl() - RDI_Pathfinder msg: 1613171364.44 epoch sec; seqNo:522 2021-02-12T23:09:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.63 , 104.27 , 111.03 , 77.24 2021-02-12T23:09:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.13 , 0.38 2021-02-12T23:09:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171364.45 epoch sec; seqNo:1239; 4056298.73 north; 590501.45 east 2021-02-12T23:09:24Z- AHRS_M2 msg: 1613171364.84 epoch sec; seqNo:1233 2021-02-12T23:09:24Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.21 psi 2021-02-12T23:09:24Z- handleDepth()-Depth_Keller msg: 1613171364840 epoch sec; seqNo:1240; depth 6.97 2021-02-12T23:09:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171364.86 epoch sec; seqNo:1240; 4056298.75 north; 590502.67 east 2021-02-12T23:09:25Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-12T23:09:25Z- waiting for 1613171362.60, time is 1613171364.86 2021-02-12T23:09:25Z- LcmTrn::updateTrn() >>>> pose time 1613171364.86, meas time 1613171364.44 <<<< 2021-02-12T23:09:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171364.44 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:09:25Z- TerrainNav::motionUpdate() - currEstimate = (4056299, 590502.67, 6.97) 2021-02-12T23:09:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:09:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:09:25Z- TNavBF::Projecting Measurements 2021-02-12T23:09:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:09:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:09:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:09:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:09:25Z- TNavFilter::4 good beams. 2021-02-12T23:09:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:09:25Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:09:25Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:09:25Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:09:25Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:09:25Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:09:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:09:25Z- TNavBF::mmse selected from filter -1 2021-02-12T23:09:25Z- TerrainMapOctree: (vehN, vehE ) = (4056298.50, 590502.48). 2021-02-12T23:09:25Z- TerrainMapOctree: Min Distance = 6209.77. 2021-02-12T23:09:25Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:09:25Z- There is no specific good beam that all particles have in common. 2021-02-12T23:09:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:09:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 10.93 2021-02-12T23:09:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:09:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 17.39 2021-02-12T23:09:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:09:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 19.72 2021-02-12T23:09:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:09:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 9.55 2021-02-12T23:09:25Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:09:25Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:09:25Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:09:25Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:09:25Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:09:25Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:09:25Z- Filter 0 No beams used in subcloud update 2021-02-12T23:09:25Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:09:25Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:09:25Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 804.4 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:09:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171364.44 sec. 2021-02-12T23:09:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:09:25Z- TNavBF::mmse selected from filter -1 2021-02-12T23:09:25Z- variances([0], [2], and psi_berg) = 804.298308 833.112758 0.000000 2021-02-12T23:09:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:09:25Z- LcmTrn::publishEstimates() - MLE : 4056267.25 590456.17 6.97 2021-02-12T23:09:25Z- LcmTrn::publishEstimates() - MMSE : 4056298.75 590502.67 6.97 2021-02-12T23:09:25Z- LcmTrn::publishEstimates() - COVAR: 804.30 833.11 0.84 0.00 2021-02-12T23:09:25Z- AHRS_M2 msg: 1613171365.23 epoch sec; seqNo:1234 2021-02-12T23:09:25Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.21 psi 2021-02-12T23:09:25Z- handleDepth()-Depth_Keller msg: 1613171365246 epoch sec; seqNo:1241; depth 6.85 2021-02-12T23:09:25Z- handleDvl() - RDI_Pathfinder msg: 1613171365.26 epoch sec; seqNo:523 2021-02-12T23:09:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.05 , 105.69 , 115.15 , 78.88 2021-02-12T23:09:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.10 , 0.44 2021-02-12T23:09:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171365.27 epoch sec; seqNo:1241; 4056299.13 north; 590502.67 east 2021-02-12T23:09:25Z- AHRS_M2 msg: 1613171365.64 epoch sec; seqNo:1235 2021-02-12T23:09:25Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.21 psi 2021-02-12T23:09:25Z- handleDepth()-Depth_Keller msg: 1613171365640 epoch sec; seqNo:1242; depth 6.72 2021-02-12T23:09:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171365.65 epoch sec; seqNo:1242; 4056299.50 north; 590502.66 east 2021-02-12T23:09:26Z- AHRS_M2 msg: 1613171366.04 epoch sec; seqNo:1236 2021-02-12T23:09:26Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.21 psi 2021-02-12T23:09:26Z- handleDepth()-Depth_Keller msg: 1613171366045 epoch sec; seqNo:1243; depth 6.62 2021-02-12T23:09:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171366.05 epoch sec; seqNo:1243; 4056299.88 north; 590502.66 east 2021-02-12T23:09:26Z- AHRS_M2 msg: 1613171366.44 epoch sec; seqNo:1237 2021-02-12T23:09:26Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.21 psi 2021-02-12T23:09:26Z- handleDepth()-Depth_Keller msg: 1613171366457 epoch sec; seqNo:1244; depth 6.52 2021-02-12T23:09:26Z- handleDvl() - RDI_Pathfinder msg: 1613171366.47 epoch sec; seqNo:524 2021-02-12T23:09:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.85 , 106.84 , 113.60 , 79.29 2021-02-12T23:09:26Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , 0.31 , 0.15 2021-02-12T23:09:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171366.48 epoch sec; seqNo:1244; 4056300.27 north; 590503.87 east 2021-02-12T23:09:26Z- AHRS_M2 msg: 1613171366.85 epoch sec; seqNo:1238 2021-02-12T23:09:26Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.21 psi 2021-02-12T23:09:26Z- handleDepth()-Depth_Keller msg: 1613171366852 epoch sec; seqNo:1245; depth 6.40 2021-02-12T23:09:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171366.88 epoch sec; seqNo:1245; 4056300.27 north; 590503.87 east 2021-02-12T23:09:27Z- AHRS_M2 msg: 1613171367.25 epoch sec; seqNo:1239 2021-02-12T23:09:27Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.21 psi 2021-02-12T23:09:27Z- handleDepth()-Depth_Keller msg: 1613171367261 epoch sec; seqNo:1246; depth 6.30 2021-02-12T23:09:27Z- handleDvl() - RDI_Pathfinder msg: 1613171367.27 epoch sec; seqNo:525 2021-02-12T23:09:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 108.19 , 117.65 , 78.47 2021-02-12T23:09:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.15 , -0.25 2021-02-12T23:09:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171367.30 epoch sec; seqNo:1246; 4056300.65 north; 590503.87 east 2021-02-12T23:09:27Z- AHRS_M2 msg: 1613171367.66 epoch sec; seqNo:1240 2021-02-12T23:09:27Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.21 psi 2021-02-12T23:09:27Z- handleDepth()-Depth_Keller msg: 1613171367667 epoch sec; seqNo:1247; depth 6.18 2021-02-12T23:09:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171367.68 epoch sec; seqNo:1247; 4056301.03 north; 590503.87 east 2021-02-12T23:09:28Z- AHRS_M2 msg: 1613171368.06 epoch sec; seqNo:1241 2021-02-12T23:09:28Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.20 psi 2021-02-12T23:09:28Z- handleDepth()-Depth_Keller msg: 1613171368065 epoch sec; seqNo:1248; depth 6.07 2021-02-12T23:09:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171368.08 epoch sec; seqNo:1248; 4056301.04 north; 590505.09 east 2021-02-12T23:09:28Z- AHRS_M2 msg: 1613171368.46 epoch sec; seqNo:1242 2021-02-12T23:09:28Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.18 psi 2021-02-12T23:09:28Z- handleDepth()-Depth_Keller msg: 1613171368473 epoch sec; seqNo:1249; depth 5.97 2021-02-12T23:09:28Z- handleDvl() - RDI_Pathfinder msg: 1613171368.48 epoch sec; seqNo:526 2021-02-12T23:09:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.07 , 108.05 , 120.22 , 78.88 2021-02-12T23:09:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.13 , -0.32 2021-02-12T23:09:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171368.51 epoch sec; seqNo:1249; 4056301.42 north; 590505.08 east 2021-02-12T23:09:28Z- AHRS_M2 msg: 1613171368.87 epoch sec; seqNo:1243 2021-02-12T23:09:28Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.17 psi 2021-02-12T23:09:28Z- handleDepth()-Depth_Keller msg: 1613171368872 epoch sec; seqNo:1250; depth 5.83 2021-02-12T23:09:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171368.88 epoch sec; seqNo:1250; 4056301.80 north; 590505.08 east 2021-02-12T23:09:29Z- AHRS_M2 msg: 1613171369.27 epoch sec; seqNo:1244 2021-02-12T23:09:29Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.17 psi 2021-02-12T23:09:29Z- handleDepth()-Depth_Keller msg: 1613171369279 epoch sec; seqNo:1251; depth 5.73 2021-02-12T23:09:29Z- handleDvl() - RDI_Pathfinder msg: 1613171369.29 epoch sec; seqNo:527 2021-02-12T23:09:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.35 , 106.63 , 120.16 , 77.24 2021-02-12T23:09:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.27 , -0.15 2021-02-12T23:09:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171369.30 epoch sec; seqNo:1251; 4056301.80 north; 590505.08 east 2021-02-12T23:09:29Z- AHRS_M2 msg: 1613171369.68 epoch sec; seqNo:1245 2021-02-12T23:09:29Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.17 psi 2021-02-12T23:09:29Z- handleDepth()-Depth_Keller msg: 1613171369680 epoch sec; seqNo:1252; depth 5.61 2021-02-12T23:09:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171369.70 epoch sec; seqNo:1252; 4056302.18 north; 590505.08 east 2021-02-12T23:09:30Z- AHRS_M2 msg: 1613171370.08 epoch sec; seqNo:1246 2021-02-12T23:09:30Z- AHRS_M2 msg: 0.66 phi; 0.29 theta; -0.18 psi 2021-02-12T23:09:30Z- handleDepth()-Depth_Keller msg: 1613171370084 epoch sec; seqNo:1253; depth 5.47 2021-02-12T23:09:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171370.09 epoch sec; seqNo:1253; 4056302.56 north; 590505.07 east 2021-02-12T23:09:30Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-12T23:09:30Z- waiting for 1613171367.86, time is 1613171370.09 2021-02-12T23:09:30Z- LcmTrn::updateTrn() >>>> pose time 1613171370.09, meas time 1613171369.29 <<<< 2021-02-12T23:09:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171369.29 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:09:30Z- TerrainNav::motionUpdate() - currEstimate = (4056303, 590505.07, 5.47) 2021-02-12T23:09:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:09:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:09:30Z- TNavBF::Projecting Measurements 2021-02-12T23:09:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:09:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:09:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:09:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:09:30Z- TNavFilter::4 good beams. 2021-02-12T23:09:30Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:09:30Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:09:30Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:09:30Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:09:30Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:09:30Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:09:30Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:09:30Z- TNavBF::mmse selected from filter -1 2021-02-12T23:09:30Z- TerrainMapOctree: (vehN, vehE ) = (4056301.98, 590504.70). 2021-02-12T23:09:30Z- TerrainMapOctree: Min Distance = 6205.66. 2021-02-12T23:09:30Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:09:30Z- There is no specific good beam that all particles have in common. 2021-02-12T23:09:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:09:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 11.93 2021-02-12T23:09:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:09:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 18.19 2021-02-12T23:09:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:09:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 23.10 2021-02-12T23:09:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:09:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 9.55 2021-02-12T23:09:30Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:09:30Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:09:30Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:09:30Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:09:30Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:09:30Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:09:30Z- Filter 0 No beams used in subcloud update 2021-02-12T23:09:30Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:09:30Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:09:30Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 804.1 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:09:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171369.29 sec. 2021-02-12T23:09:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:09:30Z- TNavBF::mmse selected from filter -1 2021-02-12T23:09:30Z- variances([0], [2], and psi_berg) = 803.951885 833.111917 0.000000 2021-02-12T23:09:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:09:30Z- LcmTrn::publishEstimates() - MLE : 4056271.06 590458.57 5.47 2021-02-12T23:09:30Z- LcmTrn::publishEstimates() - MMSE : 4056302.56 590505.07 5.47 2021-02-12T23:09:30Z- LcmTrn::publishEstimates() - COVAR: 803.95 833.11 0.86 0.00 2021-02-12T23:09:30Z- AHRS_M2 msg: 1613171370.49 epoch sec; seqNo:1247 2021-02-12T23:09:30Z- AHRS_M2 msg: 0.66 phi; 0.27 theta; -0.18 psi 2021-02-12T23:09:30Z- handleDepth()-Depth_Keller msg: 1613171370490 epoch sec; seqNo:1254; depth 5.33 2021-02-12T23:09:30Z- handleDvl() - RDI_Pathfinder msg: 1613171370.50 epoch sec; seqNo:528 2021-02-12T23:09:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.77 , 104.00 , 116.17 , 78.47 2021-02-12T23:09:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.33 , 0.14 2021-02-12T23:09:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171370.51 epoch sec; seqNo:1254; 4056302.94 north; 590505.07 east 2021-02-12T23:09:30Z- AHRS_M2 msg: 1613171370.89 epoch sec; seqNo:1248 2021-02-12T23:09:30Z- AHRS_M2 msg: 0.66 phi; 0.25 theta; -0.17 psi 2021-02-12T23:09:30Z- handleDepth()-Depth_Keller msg: 1613171370903 epoch sec; seqNo:1255; depth 5.19 2021-02-12T23:09:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171370.91 epoch sec; seqNo:1255; 4056302.94 north; 590505.07 east 2021-02-12T23:09:31Z- AHRS_M2 msg: 1613171371.29 epoch sec; seqNo:1249 2021-02-12T23:09:31Z- AHRS_M2 msg: 0.66 phi; 0.22 theta; -0.13 psi 2021-02-12T23:09:31Z- handleDepth()-Depth_Keller msg: 1613171371301 epoch sec; seqNo:1256; depth 5.06 2021-02-12T23:09:31Z- handleDvl() - RDI_Pathfinder msg: 1613171371.32 epoch sec; seqNo:529 2021-02-12T23:09:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 105.42 , 112.18 , 81.08 2021-02-12T23:09:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.41 2021-02-12T23:09:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171371.33 epoch sec; seqNo:1256; 4056303.32 north; 590505.06 east 2021-02-12T23:09:31Z- AHRS_M2 msg: 1613171371.70 epoch sec; seqNo:1250 2021-02-12T23:09:31Z- AHRS_M2 msg: 0.67 phi; 0.19 theta; -0.10 psi 2021-02-12T23:09:31Z- handleDepth()-Depth_Keller msg: 1613171371700 epoch sec; seqNo:1257; depth 4.95 2021-02-12T23:09:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171371.71 epoch sec; seqNo:1257; 4056303.70 north; 590505.06 east 2021-02-12T23:09:32Z- AHRS_M2 msg: 1613171372.10 epoch sec; seqNo:1251 2021-02-12T23:09:32Z- AHRS_M2 msg: 0.68 phi; 0.16 theta; -0.07 psi 2021-02-12T23:09:32Z- handleDepth()-Depth_Keller msg: 1613171372107 epoch sec; seqNo:1258; depth 4.85 2021-02-12T23:09:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171372.12 epoch sec; seqNo:1258; 4056304.09 north; 590506.28 east 2021-02-12T23:09:32Z- AHRS_M2 msg: 1613171372.50 epoch sec; seqNo:1252 2021-02-12T23:09:32Z- AHRS_M2 msg: 0.70 phi; 0.11 theta; -0.06 psi 2021-02-12T23:09:32Z- handleDepth()-Depth_Keller msg: 1613171372508 epoch sec; seqNo:1259; depth 4.76 2021-02-12T23:09:32Z- handleDvl() - RDI_Pathfinder msg: 1613171372.53 epoch sec; seqNo:530 2021-02-12T23:09:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 96.02 , 105.49 , 81.15 2021-02-12T23:09:32Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.43 , 0.18 2021-02-12T23:09:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171372.54 epoch sec; seqNo:1259; 4056304.09 north; 590506.28 east 2021-02-12T23:09:32Z- handleDepth()-Depth_Keller msg: 1613171372929 epoch sec; seqNo:1260; depth 4.65 2021-02-12T23:09:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171372.95 epoch sec; seqNo:1260; 4056304.47 north; 590506.27 east 2021-02-12T23:09:33Z- AHRS_M2 msg: 1613171373.31 epoch sec; seqNo:1253 2021-02-12T23:09:33Z- AHRS_M2 msg: 0.73 phi; 0.02 theta; -0.11 psi 2021-02-12T23:09:33Z- handleDepth()-Depth_Keller msg: 1613171373317 epoch sec; seqNo:1261; depth 4.58 2021-02-12T23:09:33Z- handleDvl() - RDI_Pathfinder msg: 1613171373.33 epoch sec; seqNo:531 2021-02-12T23:09:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 92.30 , 96.36 , 85.54 2021-02-12T23:09:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.35 , -0.40 2021-02-12T23:09:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171373.34 epoch sec; seqNo:1261; 4056304.48 north; 590507.49 east 2021-02-12T23:09:33Z- AHRS_M2 msg: 1613171373.72 epoch sec; seqNo:1254 2021-02-12T23:09:33Z- AHRS_M2 msg: 0.74 phi; -0.02 theta; -0.15 psi 2021-02-12T23:09:33Z- handleDepth()-Depth_Keller msg: 1613171373721 epoch sec; seqNo:1262; depth 4.50 2021-02-12T23:09:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171373.73 epoch sec; seqNo:1262; 4056304.86 north; 590507.49 east 2021-02-12T23:09:34Z- AHRS_M2 msg: 1613171374.12 epoch sec; seqNo:1255 2021-02-12T23:09:34Z- AHRS_M2 msg: 0.75 phi; -0.06 theta; -0.19 psi 2021-02-12T23:09:34Z- handleDepth()-Depth_Keller msg: 1613171374139 epoch sec; seqNo:1263; depth 4.45 2021-02-12T23:09:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171374.16 epoch sec; seqNo:1263; 4056304.86 north; 590507.49 east 2021-02-12T23:09:34Z- AHRS_M2 msg: 1613171374.52 epoch sec; seqNo:1256 2021-02-12T23:09:34Z- AHRS_M2 msg: 0.76 phi; -0.11 theta; -0.22 psi 2021-02-12T23:09:34Z- handleDepth()-Depth_Keller msg: 1613171374533 epoch sec; seqNo:1264; depth 4.39 2021-02-12T23:09:34Z- handleDvl() - RDI_Pathfinder msg: 1613171374.54 epoch sec; seqNo:532 2021-02-12T23:09:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 96.29 , 89.53 , 90.89 2021-02-12T23:09:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.31 , -0.44 2021-02-12T23:09:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171374.57 epoch sec; seqNo:1264; 4056305.24 north; 590507.49 east 2021-02-12T23:09:34Z- AHRS_M2 msg: 1613171374.93 epoch sec; seqNo:1257 2021-02-12T23:09:34Z- AHRS_M2 msg: 0.76 phi; -0.15 theta; -0.22 psi 2021-02-12T23:09:34Z- handleDepth()-Depth_Keller msg: 1613171374932 epoch sec; seqNo:1265; depth 4.38 2021-02-12T23:09:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171374.94 epoch sec; seqNo:1265; 4056305.62 north; 590507.48 east 2021-02-12T23:09:35Z- LcmTrn::handleMessages() - handling 40 messages took 0.64 ms and 128 calls... 2021-02-12T23:09:35Z- waiting for 1613171373.09, time is 1613171374.94 2021-02-12T23:09:35Z- LcmTrn::updateTrn() >>>> pose time 1613171374.94, meas time 1613171374.54 <<<< 2021-02-12T23:09:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171374.54 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:09:35Z- TerrainNav::motionUpdate() - currEstimate = (4056306, 590507.48, 4.38) 2021-02-12T23:09:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:09:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:09:35Z- TNavBF::Projecting Measurements 2021-02-12T23:09:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:09:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:09:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:09:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:09:35Z- TNavFilter::4 good beams. 2021-02-12T23:09:35Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:09:35Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:09:35Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:09:35Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:09:35Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:09:35Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:09:35Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:09:35Z- TNavBF::mmse selected from filter -1 2021-02-12T23:09:35Z- TerrainMapOctree: (vehN, vehE ) = (4056305.37, 590507.28). 2021-02-12T23:09:35Z- TerrainMapOctree: Min Distance = 6201.40. 2021-02-12T23:09:35Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:09:35Z- There is no specific good beam that all particles have in common. 2021-02-12T23:09:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:09:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 10.96 2021-02-12T23:09:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:09:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 14.84 2021-02-12T23:09:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:09:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 12.83 2021-02-12T23:09:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:09:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 13.22 2021-02-12T23:09:35Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:09:35Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:09:35Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:09:35Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:09:35Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:09:35Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:09:35Z- Filter 0 No beams used in subcloud update 2021-02-12T23:09:35Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:09:35Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:09:35Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 803.7 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:09:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171374.54 sec. 2021-02-12T23:09:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:09:35Z- TNavBF::mmse selected from filter -1 2021-02-12T23:09:35Z- variances([0], [2], and psi_berg) = 803.648776 833.111188 0.000000 2021-02-12T23:09:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:09:35Z- LcmTrn::publishEstimates() - MLE : 4056274.12 590460.98 4.38 2021-02-12T23:09:35Z- LcmTrn::publishEstimates() - MMSE : 4056305.62 590507.48 4.38 2021-02-12T23:09:35Z- LcmTrn::publishEstimates() - COVAR: 803.65 833.11 0.82 0.00 2021-02-12T23:09:35Z- AHRS_M2 msg: 1613171375.33 epoch sec; seqNo:1258 2021-02-12T23:09:35Z- AHRS_M2 msg: 0.75 phi; -0.19 theta; -0.22 psi 2021-02-12T23:09:35Z- handleDepth()-Depth_Keller msg: 1613171375345 epoch sec; seqNo:1266; depth 4.37 2021-02-12T23:09:35Z- handleDvl() - RDI_Pathfinder msg: 1613171375.36 epoch sec; seqNo:533 2021-02-12T23:09:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 106.30 , 85.61 , 96.56 2021-02-12T23:09:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.51 , -0.53 , 0.12 2021-02-12T23:09:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171375.37 epoch sec; seqNo:1266; 4056306.00 north; 590507.48 east 2021-02-12T23:09:35Z- AHRS_M2 msg: 1613171375.74 epoch sec; seqNo:1259 2021-02-12T23:09:35Z- AHRS_M2 msg: 0.73 phi; -0.22 theta; -0.23 psi 2021-02-12T23:09:35Z- handleDepth()-Depth_Keller msg: 1613171375758 epoch sec; seqNo:1267; depth 4.37 2021-02-12T23:09:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171375.78 epoch sec; seqNo:1267; 4056306.00 north; 590507.48 east 2021-02-12T23:09:36Z- AHRS_M2 msg: 1613171376.15 epoch sec; seqNo:1260 2021-02-12T23:09:36Z- AHRS_M2 msg: 0.72 phi; -0.25 theta; -0.23 psi 2021-02-12T23:09:36Z- handleDepth()-Depth_Keller msg: 1613171376165 epoch sec; seqNo:1268; depth 4.35 2021-02-12T23:09:36Z- handleDvl() - RDI_Pathfinder msg: 1613171376.21 epoch sec; seqNo:534 2021-02-12T23:09:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 107.11 , 82.77 , 107.11 2021-02-12T23:09:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.49 , -0.16 , 0.37 2021-02-12T23:09:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171376.22 epoch sec; seqNo:1268; 4056306.37 north; 590507.47 east 2021-02-12T23:09:36Z- AHRS_M2 msg: 1613171376.55 epoch sec; seqNo:1261 2021-02-12T23:09:36Z- AHRS_M2 msg: 0.70 phi; -0.27 theta; -0.23 psi 2021-02-12T23:09:36Z- handleDepth()-Depth_Keller msg: 1613171376550 epoch sec; seqNo:1269; depth 4.41 2021-02-12T23:09:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171376.56 epoch sec; seqNo:1269; 4056306.37 north; 590507.47 east 2021-02-12T23:09:36Z- AHRS_M2 msg: 1613171376.95 epoch sec; seqNo:1262 2021-02-12T23:09:36Z- AHRS_M2 msg: 0.70 phi; -0.28 theta; -0.23 psi 2021-02-12T23:09:36Z- handleDepth()-Depth_Keller msg: 1613171376957 epoch sec; seqNo:1270; depth 4.44 2021-02-12T23:09:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171376.97 epoch sec; seqNo:1270; 4056306.37 north; 590507.47 east 2021-02-12T23:09:37Z- handleDepth()-Depth_Keller msg: 1613171377373 epoch sec; seqNo:1271; depth 4.52 2021-02-12T23:09:37Z- handleDvl() - RDI_Pathfinder msg: 1613171377.39 epoch sec; seqNo:535 2021-02-12T23:09:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.28 , 108.26 , 79.69 , 112.32 2021-02-12T23:09:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , 0.16 , 0.30 2021-02-12T23:09:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171377.40 epoch sec; seqNo:1271; 4056306.77 north; 590508.69 east 2021-02-12T23:09:37Z- AHRS_M2 msg: 1613171377.76 epoch sec; seqNo:1263 2021-02-12T23:09:37Z- AHRS_M2 msg: 0.69 phi; -0.30 theta; -0.24 psi 2021-02-12T23:09:37Z- handleDepth()-Depth_Keller msg: 1613171377761 epoch sec; seqNo:1272; depth 4.58 2021-02-12T23:09:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171377.81 epoch sec; seqNo:1272; 4056306.77 north; 590508.69 east 2021-02-12T23:09:38Z- AHRS_M2 msg: 1613171378.20 epoch sec; seqNo:1264 2021-02-12T23:09:38Z- AHRS_M2 msg: 0.69 phi; -0.31 theta; -0.25 psi 2021-02-12T23:09:38Z- handleDepth()-Depth_Keller msg: 1613171378209 epoch sec; seqNo:1273; depth 4.65 2021-02-12T23:09:38Z- handleDvl() - RDI_Pathfinder msg: 1613171378.24 epoch sec; seqNo:536 2021-02-12T23:09:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.20 , 110.89 , 79.69 , 117.65 2021-02-12T23:09:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , 0.14 , 0.14 2021-02-12T23:09:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171378.25 epoch sec; seqNo:1273; 4056307.15 north; 590508.69 east 2021-02-12T23:09:38Z- AHRS_M2 msg: 1613171378.56 epoch sec; seqNo:1265 2021-02-12T23:09:38Z- AHRS_M2 msg: 0.68 phi; -0.31 theta; -0.26 psi 2021-02-12T23:09:38Z- handleDepth()-Depth_Keller msg: 1613171378568 epoch sec; seqNo:1274; depth 4.73 2021-02-12T23:09:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171378.58 epoch sec; seqNo:1274; 4056307.15 north; 590508.69 east 2021-02-12T23:09:38Z- AHRS_M2 msg: 1613171378.97 epoch sec; seqNo:1266 2021-02-12T23:09:38Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.27 psi 2021-02-12T23:09:38Z- handleDepth()-Depth_Keller msg: 1613171378977 epoch sec; seqNo:1275; depth 4.82 2021-02-12T23:09:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171378.99 epoch sec; seqNo:1275; 4056307.15 north; 590508.69 east 2021-02-12T23:09:39Z- AHRS_M2 msg: 1613171379.38 epoch sec; seqNo:1267 2021-02-12T23:09:39Z- AHRS_M2 msg: 0.66 phi; -0.31 theta; -0.28 psi 2021-02-12T23:09:39Z- handleDepth()-Depth_Keller msg: 1613171379379 epoch sec; seqNo:1276; depth 4.92 2021-02-12T23:09:39Z- handleDvl() - RDI_Pathfinder msg: 1613171379.39 epoch sec; seqNo:537 2021-02-12T23:09:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.65 , 114.75 , 82.30 , 118.80 2021-02-12T23:09:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.03 , 0.09 2021-02-12T23:09:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171379.40 epoch sec; seqNo:1276; 4056307.52 north; 590508.68 east 2021-02-12T23:09:39Z- AHRS_M2 msg: 1613171379.78 epoch sec; seqNo:1268 2021-02-12T23:09:39Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.27 psi 2021-02-12T23:09:39Z- handleDepth()-Depth_Keller msg: 1613171379780 epoch sec; seqNo:1277; depth 5.01 2021-02-12T23:09:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171379.80 epoch sec; seqNo:1277; 4056307.92 north; 590509.90 east 2021-02-12T23:09:40Z- AHRS_M2 msg: 1613171380.19 epoch sec; seqNo:1269 2021-02-12T23:09:40Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.26 psi 2021-02-12T23:09:40Z- handleDepth()-Depth_Keller msg: 1613171380191 epoch sec; seqNo:1278; depth 5.11 2021-02-12T23:09:40Z- handleDvl() - RDI_Pathfinder msg: 1613171380.20 epoch sec; seqNo:538 2021-02-12T23:09:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 113.26 , 82.17 , 120.02 2021-02-12T23:09:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.09 , 0.07 2021-02-12T23:09:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171380.23 epoch sec; seqNo:1278; 4056307.92 north; 590509.90 east 2021-02-12T23:09:40Z- LcmTrn::handleMessages() - handling 44 messages took 0.83 ms and 128 calls... 2021-02-12T23:09:40Z- waiting for 1613171377.94, time is 1613171380.23 2021-02-12T23:09:40Z- LcmTrn::updateTrn() equating pose 1613171380.23 and meas times 1613171380.23 2021-02-12T23:09:40Z- LcmTrn::updateTrn() >>>> pose time 1613171380.23, meas time 1613171380.23 <<<< 2021-02-12T23:09:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171380.23 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:09:40Z- TerrainNav::motionUpdate() - currEstimate = (4056308, 590509.90, 5.11) 2021-02-12T23:09:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:09:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:09:40Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171380.23 sec.; No relevant navigation data available 2021-02-12T23:09:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:09:40Z- TNavBF::mmse selected from filter -1 2021-02-12T23:09:40Z- variances([0], [2], and psi_berg) = 803.424667 833.110652 0.000000 2021-02-12T23:09:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:09:40Z- LcmTrn::publishEstimates() - MLE : 4056276.42 590464.40 5.11 2021-02-12T23:09:40Z- LcmTrn::publishEstimates() - MMSE : 4056307.92 590509.90 5.11 2021-02-12T23:09:40Z- LcmTrn::publishEstimates() - COVAR: 803.42 833.11 0.99 0.00 2021-02-12T23:09:40Z- AHRS_M2 msg: 1613171380.58 epoch sec; seqNo:1270 2021-02-12T23:09:40Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.25 psi 2021-02-12T23:09:40Z- handleDepth()-Depth_Keller msg: 1613171380593 epoch sec; seqNo:1279; depth 5.24 2021-02-12T23:09:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171380.61 epoch sec; seqNo:1279; 4056308.29 north; 590509.90 east 2021-02-12T23:09:41Z- AHRS_M2 msg: 1613171380.99 epoch sec; seqNo:1271 2021-02-12T23:09:41Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.23 psi 2021-02-12T23:09:41Z- handleDepth()-Depth_Keller msg: 1613171380999 epoch sec; seqNo:1280; depth 5.35 2021-02-12T23:09:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171381.01 epoch sec; seqNo:1280; 4056308.67 north; 590509.89 east 2021-02-12T23:09:41Z- AHRS_M2 msg: 1613171381.39 epoch sec; seqNo:1272 2021-02-12T23:09:41Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.22 psi 2021-02-12T23:09:41Z- handleDepth()-Depth_Keller msg: 1613171381397 epoch sec; seqNo:1281; depth 5.47 2021-02-12T23:09:41Z- handleDvl() - RDI_Pathfinder msg: 1613171381.41 epoch sec; seqNo:539 2021-02-12T23:09:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.26 , 111.03 , 79.69 , 118.47 2021-02-12T23:09:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.25 , -0.21 2021-02-12T23:09:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171381.42 epoch sec; seqNo:1281; 4056308.67 north; 590509.89 east 2021-02-12T23:09:41Z- AHRS_M2 msg: 1613171381.80 epoch sec; seqNo:1273 2021-02-12T23:09:41Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.22 psi 2021-02-12T23:09:41Z- handleDepth()-Depth_Keller msg: 1613171381801 epoch sec; seqNo:1282; depth 5.59 2021-02-12T23:09:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171381.81 epoch sec; seqNo:1282; 4056309.07 north; 590511.11 east 2021-02-12T23:09:42Z- AHRS_M2 msg: 1613171382.21 epoch sec; seqNo:1274 2021-02-12T23:09:42Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.24 psi 2021-02-12T23:09:42Z- handleDepth()-Depth_Keller msg: 1613171382211 epoch sec; seqNo:1283; depth 5.68 2021-02-12T23:09:42Z- handleDvl() - RDI_Pathfinder msg: 1613171382.22 epoch sec; seqNo:540 2021-02-12T23:09:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.65 , 107.99 , 78.87 , 113.40 2021-02-12T23:09:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.04 , -0.50 2021-02-12T23:09:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171382.23 epoch sec; seqNo:1283; 4056309.44 north; 590511.11 east 2021-02-12T23:09:42Z- AHRS_M2 msg: 1613171382.60 epoch sec; seqNo:1275 2021-02-12T23:09:42Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.25 psi 2021-02-12T23:09:42Z- handleDepth()-Depth_Keller msg: 1613171382611 epoch sec; seqNo:1284; depth 5.77 2021-02-12T23:09:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171382.62 epoch sec; seqNo:1284; 4056309.82 north; 590511.10 east 2021-02-12T23:09:43Z- AHRS_M2 msg: 1613171383.01 epoch sec; seqNo:1276 2021-02-12T23:09:43Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.26 psi 2021-02-12T23:09:43Z- handleDepth()-Depth_Keller msg: 1613171383014 epoch sec; seqNo:1285; depth 5.86 2021-02-12T23:09:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171383.04 epoch sec; seqNo:1285; 4056310.20 north; 590511.10 east 2021-02-12T23:09:43Z- AHRS_M2 msg: 1613171383.41 epoch sec; seqNo:1277 2021-02-12T23:09:43Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.26 psi 2021-02-12T23:09:43Z- handleDepth()-Depth_Keller msg: 1613171383416 epoch sec; seqNo:1286; depth 5.98 2021-02-12T23:09:43Z- handleDvl() - RDI_Pathfinder msg: 1613171383.43 epoch sec; seqNo:541 2021-02-12T23:09:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.50 , 110.89 , 79.28 , 116.30 2021-02-12T23:09:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.38 , -0.23 2021-02-12T23:09:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171383.46 epoch sec; seqNo:1286; 4056310.20 north; 590511.10 east 2021-02-12T23:09:43Z- AHRS_M2 msg: 1613171383.82 epoch sec; seqNo:1278 2021-02-12T23:09:43Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.24 psi 2021-02-12T23:09:43Z- handleDepth()-Depth_Keller msg: 1613171383821 epoch sec; seqNo:1287; depth 6.07 2021-02-12T23:09:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171383.84 epoch sec; seqNo:1287; 4056310.58 north; 590511.09 east 2021-02-12T23:09:44Z- AHRS_M2 msg: 1613171384.22 epoch sec; seqNo:1279 2021-02-12T23:09:44Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.22 psi 2021-02-12T23:09:44Z- handleDepth()-Depth_Keller msg: 1613171384225 epoch sec; seqNo:1288; depth 6.17 2021-02-12T23:09:44Z- handleDvl() - RDI_Pathfinder msg: 1613171384.25 epoch sec; seqNo:542 2021-02-12T23:09:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.37 , 109.41 , 77.64 , 118.87 2021-02-12T23:09:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.41 , 0.28 2021-02-12T23:09:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171384.26 epoch sec; seqNo:1288; 4056310.96 north; 590511.09 east 2021-02-12T23:09:44Z- AHRS_M2 msg: 1613171384.63 epoch sec; seqNo:1280 2021-02-12T23:09:44Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.17 psi 2021-02-12T23:09:44Z- handleDepth()-Depth_Keller msg: 1613171384634 epoch sec; seqNo:1289; depth 6.28 2021-02-12T23:09:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171384.64 epoch sec; seqNo:1289; 4056311.34 north; 590511.09 east 2021-02-12T23:09:45Z- AHRS_M2 msg: 1613171385.03 epoch sec; seqNo:1281 2021-02-12T23:09:45Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.15 psi 2021-02-12T23:09:45Z- handleDepth()-Depth_Keller msg: 1613171385036 epoch sec; seqNo:1290; depth 6.35 2021-02-12T23:09:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171385.05 epoch sec; seqNo:1290; 4056311.72 north; 590511.08 east 2021-02-12T23:09:45Z- LcmTrn::handleMessages() - handling 40 messages took 0.65 ms and 127 calls... 2021-02-12T23:09:45Z- waiting for 1613171383.23, time is 1613171385.05 2021-02-12T23:09:45Z- LcmTrn::updateTrn() >>>> pose time 1613171385.05, meas time 1613171384.25 <<<< 2021-02-12T23:09:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171384.25 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:09:45Z- TerrainNav::motionUpdate() - currEstimate = (4056312, 590511.08, 6.35) 2021-02-12T23:09:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:09:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:09:45Z- TNavBF::Projecting Measurements 2021-02-12T23:09:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:09:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:09:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:09:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:09:45Z- TNavFilter::4 good beams. 2021-02-12T23:09:45Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:09:45Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:09:45Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:09:45Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:09:45Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:09:45Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:09:45Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:09:45Z- TNavBF::mmse selected from filter -1 2021-02-12T23:09:45Z- TerrainMapOctree: (vehN, vehE ) = (4056311.09, 590510.89). 2021-02-12T23:09:45Z- TerrainMapOctree: Min Distance = 6194.66. 2021-02-12T23:09:45Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:09:45Z- There is no specific good beam that all particles have in common. 2021-02-12T23:09:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:09:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 10.86 2021-02-12T23:09:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:09:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 19.15 2021-02-12T23:09:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:09:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 9.65 2021-02-12T23:09:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:09:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 22.61 2021-02-12T23:09:45Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:09:45Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:09:45Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:09:45Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:09:45Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:09:45Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:09:45Z- Filter 0 No beams used in subcloud update 2021-02-12T23:09:45Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:09:45Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:09:45Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 803.2 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:09:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171384.25 sec. 2021-02-12T23:09:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:09:45Z- TNavBF::mmse selected from filter -1 2021-02-12T23:09:45Z- variances([0], [2], and psi_berg) = 803.103842 833.109892 0.000000 2021-02-12T23:09:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:09:45Z- LcmTrn::publishEstimates() - MLE : 4056281.22 590465.58 6.35 2021-02-12T23:09:45Z- LcmTrn::publishEstimates() - MMSE : 4056311.72 590511.08 6.35 2021-02-12T23:09:45Z- LcmTrn::publishEstimates() - COVAR: 803.10 833.11 0.93 0.00 2021-02-12T23:09:45Z- AHRS_M2 msg: 1613171385.43 epoch sec; seqNo:1282 2021-02-12T23:09:45Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.13 psi 2021-02-12T23:09:45Z- handleDepth()-Depth_Keller msg: 1613171385438 epoch sec; seqNo:1291; depth 6.48 2021-02-12T23:09:45Z- handleDvl() - RDI_Pathfinder msg: 1613171385.45 epoch sec; seqNo:543 2021-02-12T23:09:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 104.07 , 77.64 , 116.24 2021-02-12T23:09:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.12 , 0.43 2021-02-12T23:09:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171385.46 epoch sec; seqNo:1291; 4056311.72 north; 590511.08 east 2021-02-12T23:09:45Z- AHRS_M2 msg: 1613171385.84 epoch sec; seqNo:1283 2021-02-12T23:09:45Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.13 psi 2021-02-12T23:09:45Z- handleDepth()-Depth_Keller msg: 1613171385840 epoch sec; seqNo:1292; depth 6.60 2021-02-12T23:09:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171385.86 epoch sec; seqNo:1292; 4056312.10 north; 590511.08 east 2021-02-12T23:09:46Z- AHRS_M2 msg: 1613171386.24 epoch sec; seqNo:1284 2021-02-12T23:09:46Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.13 psi 2021-02-12T23:09:46Z- handleDepth()-Depth_Keller msg: 1613171386247 epoch sec; seqNo:1293; depth 6.72 2021-02-12T23:09:46Z- handleDvl() - RDI_Pathfinder msg: 1613171386.27 epoch sec; seqNo:544 2021-02-12T23:09:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.91 , 96.02 , 75.61 , 116.30 2021-02-12T23:09:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.16 , 0.17 2021-02-12T23:09:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171386.28 epoch sec; seqNo:1293; 4056312.11 north; 590512.30 east 2021-02-12T23:09:46Z- AHRS_M2 msg: 1613171386.64 epoch sec; seqNo:1285 2021-02-12T23:09:46Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.14 psi 2021-02-12T23:09:46Z- handleDepth()-Depth_Keller msg: 1613171386653 epoch sec; seqNo:1294; depth 6.84 2021-02-12T23:09:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171386.66 epoch sec; seqNo:1294; 4056312.49 north; 590512.29 east 2021-02-12T23:09:47Z- AHRS_M2 msg: 1613171387.05 epoch sec; seqNo:1286 2021-02-12T23:09:47Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.16 psi 2021-02-12T23:09:47Z- handleDepth()-Depth_Keller msg: 1613171387054 epoch sec; seqNo:1295; depth 6.93 2021-02-12T23:09:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171387.07 epoch sec; seqNo:1295; 4056312.49 north; 590512.29 east 2021-02-12T23:09:47Z- AHRS_M2 msg: 1613171387.45 epoch sec; seqNo:1287 2021-02-12T23:09:47Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-12T23:09:47Z- handleDepth()-Depth_Keller msg: 1613171387463 epoch sec; seqNo:1296; depth 7.06 2021-02-12T23:09:47Z- handleDvl() - RDI_Pathfinder msg: 1613171387.47 epoch sec; seqNo:545 2021-02-12T23:09:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.76 , 96.22 , 76.43 , 113.80 2021-02-12T23:09:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.15 , -0.17 2021-02-12T23:09:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171387.48 epoch sec; seqNo:1296; 4056312.87 north; 590512.29 east 2021-02-12T23:09:47Z- handleDepth()-Depth_Keller msg: 1613171387875 epoch sec; seqNo:1297; depth 7.17 2021-02-12T23:09:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171387.89 epoch sec; seqNo:1297; 4056313.26 north; 590513.51 east 2021-02-12T23:09:48Z- AHRS_M2 msg: 1613171388.26 epoch sec; seqNo:1288 2021-02-12T23:09:48Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.20 psi 2021-02-12T23:09:48Z- handleDepth()-Depth_Keller msg: 1613171388265 epoch sec; seqNo:1298; depth 7.29 2021-02-12T23:09:48Z- handleDvl() - RDI_Pathfinder msg: 1613171388.28 epoch sec; seqNo:546 2021-02-12T23:09:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 105.69 , 78.65 , 116.51 2021-02-12T23:09:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.09 , -0.25 2021-02-12T23:09:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171388.29 epoch sec; seqNo:1298; 4056313.64 north; 590513.50 east 2021-02-12T23:09:48Z- AHRS_M2 msg: 1613171388.66 epoch sec; seqNo:1289 2021-02-12T23:09:48Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.19 psi 2021-02-12T23:09:48Z- handleDepth()-Depth_Keller msg: 1613171388677 epoch sec; seqNo:1299; depth 7.42 2021-02-12T23:09:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171388.69 epoch sec; seqNo:1299; 4056313.64 north; 590513.50 east 2021-02-12T23:09:49Z- AHRS_M2 msg: 1613171389.07 epoch sec; seqNo:1290 2021-02-12T23:09:49Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.19 psi 2021-02-12T23:09:49Z- handleDepth()-Depth_Keller msg: 1613171389072 epoch sec; seqNo:1300; depth 7.53 2021-02-12T23:09:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171389.09 epoch sec; seqNo:1300; 4056314.02 north; 590513.50 east 2021-02-12T23:09:49Z- AHRS_M2 msg: 1613171389.47 epoch sec; seqNo:1291 2021-02-12T23:09:49Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.17 psi 2021-02-12T23:09:49Z- handleDepth()-Depth_Keller msg: 1613171389476 epoch sec; seqNo:1301; depth 7.65 2021-02-12T23:09:49Z- handleDvl() - RDI_Pathfinder msg: 1613171389.49 epoch sec; seqNo:547 2021-02-12T23:09:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.15 , 105.49 , 76.01 , 114.95 2021-02-12T23:09:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.28 , -0.01 2021-02-12T23:09:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171389.51 epoch sec; seqNo:1301; 4056314.39 north; 590513.50 east 2021-02-12T23:09:49Z- AHRS_M2 msg: 1613171389.88 epoch sec; seqNo:1292 2021-02-12T23:09:49Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.16 psi 2021-02-12T23:09:49Z- handleDepth()-Depth_Keller msg: 1613171389889 epoch sec; seqNo:1302; depth 7.75 2021-02-12T23:09:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171389.90 epoch sec; seqNo:1302; 4056314.77 north; 590513.49 east 2021-02-12T23:09:50Z- AHRS_M2 msg: 1613171390.28 epoch sec; seqNo:1293 2021-02-12T23:09:50Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.14 psi 2021-02-12T23:09:50Z- handleDepth()-Depth_Keller msg: 1613171390285 epoch sec; seqNo:1303; depth 7.86 2021-02-12T23:09:50Z- handleDvl() - RDI_Pathfinder msg: 1613171390.30 epoch sec; seqNo:548 2021-02-12T23:09:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 102.92 , 76.01 , 116.44 2021-02-12T23:09:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.14 , 0.24 2021-02-12T23:09:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171390.31 epoch sec; seqNo:1303; 4056314.77 north; 590513.49 east 2021-02-12T23:09:50Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-12T23:09:50Z- waiting for 1613171388.05, time is 1613171390.31 2021-02-12T23:09:50Z- LcmTrn::updateTrn() equating pose 1613171390.31 and meas times 1613171390.31 2021-02-12T23:09:50Z- LcmTrn::updateTrn() >>>> pose time 1613171390.31, meas time 1613171390.31 <<<< 2021-02-12T23:09:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171390.31 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:09:50Z- TerrainNav::motionUpdate() - currEstimate = (4056315, 590513.49, 7.86) 2021-02-12T23:09:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:09:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:09:50Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171390.31 sec.; No relevant navigation data available 2021-02-12T23:09:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:09:50Z- TNavBF::mmse selected from filter -1 2021-02-12T23:09:50Z- variances([0], [2], and psi_berg) = 802.864876 833.109330 0.000000 2021-02-12T23:09:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:09:50Z- LcmTrn::publishEstimates() - MLE : 4056284.27 590467.99 7.86 2021-02-12T23:09:50Z- LcmTrn::publishEstimates() - MMSE : 4056314.77 590513.49 7.86 2021-02-12T23:09:50Z- LcmTrn::publishEstimates() - COVAR: 802.86 833.11 1.13 0.00 2021-02-12T23:09:50Z- AHRS_M2 msg: 1613171390.68 epoch sec; seqNo:1294 2021-02-12T23:09:50Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.12 psi 2021-02-12T23:09:50Z- handleDepth()-Depth_Keller msg: 1613171390688 epoch sec; seqNo:1304; depth 7.97 2021-02-12T23:09:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171390.70 epoch sec; seqNo:1304; 4056315.15 north; 590513.49 east 2021-02-12T23:09:51Z- AHRS_M2 msg: 1613171391.10 epoch sec; seqNo:1295 2021-02-12T23:09:51Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.10 psi 2021-02-12T23:09:51Z- handleDepth()-Depth_Keller msg: 1613171391128 epoch sec; seqNo:1305; depth 8.10 2021-02-12T23:09:51Z- handleDvl() - RDI_Pathfinder msg: 1613171391.14 epoch sec; seqNo:549 2021-02-12T23:09:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 96.09 , 74.78 , 116.37 2021-02-12T23:09:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.13 , 0.22 2021-02-12T23:09:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171391.15 epoch sec; seqNo:1305; 4056315.15 north; 590513.49 east 2021-02-12T23:09:51Z- AHRS_M2 msg: 1613171391.49 epoch sec; seqNo:1296 2021-02-12T23:09:51Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.10 psi 2021-02-12T23:09:51Z- handleDepth()-Depth_Keller msg: 1613171391496 epoch sec; seqNo:1306; depth 8.23 2021-02-12T23:09:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171391.51 epoch sec; seqNo:1306; 4056315.54 north; 590514.70 east 2021-02-12T23:09:51Z- AHRS_M2 msg: 1613171391.91 epoch sec; seqNo:1297 2021-02-12T23:09:51Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.11 psi 2021-02-12T23:09:51Z- handleDepth()-Depth_Keller msg: 1613171391915 epoch sec; seqNo:1307; depth 8.34 2021-02-12T23:09:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171391.93 epoch sec; seqNo:1307; 4056315.54 north; 590514.70 east 2021-02-12T23:09:52Z- AHRS_M2 msg: 1613171392.30 epoch sec; seqNo:1298 2021-02-12T23:09:52Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.12 psi 2021-02-12T23:09:52Z- handleDepth()-Depth_Keller msg: 1613171392304 epoch sec; seqNo:1308; depth 8.49 2021-02-12T23:09:52Z- handleDvl() - RDI_Pathfinder msg: 1613171392.32 epoch sec; seqNo:550 2021-02-12T23:09:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.76 , 96.23 , 75.19 , 117.86 2021-02-12T23:09:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.23 , -0.03 2021-02-12T23:09:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171392.33 epoch sec; seqNo:1308; 4056315.92 north; 590514.70 east 2021-02-12T23:09:52Z- AHRS_M2 msg: 1613171392.70 epoch sec; seqNo:1299 2021-02-12T23:09:52Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.15 psi 2021-02-12T23:09:52Z- handleDepth()-Depth_Keller msg: 1613171392708 epoch sec; seqNo:1309; depth 8.58 2021-02-12T23:09:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171392.72 epoch sec; seqNo:1309; 4056315.92 north; 590514.70 east 2021-02-12T23:09:53Z- AHRS_M2 msg: 1613171393.13 epoch sec; seqNo:1300 2021-02-12T23:09:53Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.19 psi 2021-02-12T23:09:53Z- handleDepth()-Depth_Keller msg: 1613171393138 epoch sec; seqNo:1310; depth 8.73 2021-02-12T23:09:53Z- handleDvl() - RDI_Pathfinder msg: 1613171393.15 epoch sec; seqNo:551 2021-02-12T23:09:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.76 , 102.99 , 75.19 , 120.56 2021-02-12T23:09:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.07 , -0.24 2021-02-12T23:09:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171393.16 epoch sec; seqNo:1310; 4056316.30 north; 590514.70 east 2021-02-12T23:09:53Z- AHRS_M2 msg: 1613171393.51 epoch sec; seqNo:1301 2021-02-12T23:09:53Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.21 psi 2021-02-12T23:09:53Z- handleDepth()-Depth_Keller msg: 1613171393516 epoch sec; seqNo:1311; depth 8.88 2021-02-12T23:09:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171393.53 epoch sec; seqNo:1311; 4056316.69 north; 590515.91 east 2021-02-12T23:09:53Z- AHRS_M2 msg: 1613171393.92 epoch sec; seqNo:1302 2021-02-12T23:09:53Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.22 psi 2021-02-12T23:09:53Z- handleDepth()-Depth_Keller msg: 1613171393920 epoch sec; seqNo:1312; depth 9.00 2021-02-12T23:09:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171393.93 epoch sec; seqNo:1312; 4056317.07 north; 590515.91 east 2021-02-12T23:09:54Z- AHRS_M2 msg: 1613171394.33 epoch sec; seqNo:1303 2021-02-12T23:09:54Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.22 psi 2021-02-12T23:09:54Z- handleDepth()-Depth_Keller msg: 1613171394337 epoch sec; seqNo:1313; depth 9.16 2021-02-12T23:09:54Z- handleDvl() - RDI_Pathfinder msg: 1613171394.36 epoch sec; seqNo:552 2021-02-12T23:09:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.50 , 106.84 , 74.78 , 119.01 2021-02-12T23:09:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.16 , -0.19 2021-02-12T23:09:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171394.39 epoch sec; seqNo:1313; 4056317.45 north; 590515.91 east 2021-02-12T23:09:54Z- AHRS_M2 msg: 1613171394.72 epoch sec; seqNo:1304 2021-02-12T23:09:54Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.22 psi 2021-02-12T23:09:54Z- handleDepth()-Depth_Keller msg: 1613171394733 epoch sec; seqNo:1314; depth 9.33 2021-02-12T23:09:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171394.75 epoch sec; seqNo:1314; 4056317.45 north; 590515.91 east 2021-02-12T23:09:55Z- AHRS_M2 msg: 1613171395.13 epoch sec; seqNo:1305 2021-02-12T23:09:55Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.20 psi 2021-02-12T23:09:55Z- handleDepth()-Depth_Keller msg: 1613171395140 epoch sec; seqNo:1315; depth 9.46 2021-02-12T23:09:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171395.18 epoch sec; seqNo:1315; 4056317.83 north; 590515.90 east 2021-02-12T23:09:55Z- LcmTrn::handleMessages() - handling 40 messages took 0.71 ms and 126 calls... 2021-02-12T23:09:55Z- waiting for 1613171393.31, time is 1613171395.18 2021-02-12T23:09:55Z- LcmTrn::updateTrn() >>>> pose time 1613171395.18, meas time 1613171394.36 <<<< 2021-02-12T23:09:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171394.36 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:09:55Z- TerrainNav::motionUpdate() - currEstimate = (4056318, 590515.90, 9.46) 2021-02-12T23:09:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:09:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:09:55Z- TNavBF::Projecting Measurements 2021-02-12T23:09:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:09:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:09:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:09:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:09:55Z- TNavFilter::4 good beams. 2021-02-12T23:09:55Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:09:55Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:09:55Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:09:55Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:09:55Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:09:55Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:09:55Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:09:55Z- TNavBF::mmse selected from filter -1 2021-02-12T23:09:55Z- TerrainMapOctree: (vehN, vehE ) = (4056317.31, 590515.49). 2021-02-12T23:09:55Z- TerrainMapOctree: Min Distance = 6186.92. 2021-02-12T23:09:55Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:09:55Z- There is no specific good beam that all particles have in common. 2021-02-12T23:09:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:09:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 10.89 2021-02-12T23:09:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:09:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 18.26 2021-02-12T23:09:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:09:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 8.95 2021-02-12T23:09:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:09:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 22.66 2021-02-12T23:09:55Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:09:55Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:09:55Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:09:55Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:09:55Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:09:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:09:55Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:09:55Z- Filter 0 No beams used in subcloud update 2021-02-12T23:09:55Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:09:55Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:09:55Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 802.6 EastVariance: 833.1 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:09:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171394.36 sec. 2021-02-12T23:09:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:09:55Z- TNavBF::mmse selected from filter -1 2021-02-12T23:09:55Z- variances([0], [2], and psi_berg) = 802.551093 833.108598 0.000000 2021-02-12T23:09:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:09:55Z- LcmTrn::publishEstimates() - MLE : 4056287.33 590470.40 9.46 2021-02-12T23:09:55Z- LcmTrn::publishEstimates() - MMSE : 4056317.83 590515.90 9.46 2021-02-12T23:09:55Z- LcmTrn::publishEstimates() - COVAR: 802.55 833.11 1.00 0.00 2021-02-12T23:09:55Z- AHRS_M2 msg: 1613171395.53 epoch sec; seqNo:1306 2021-02-12T23:09:55Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-12T23:09:55Z- handleDepth()-Depth_Keller msg: 1613171395536 epoch sec; seqNo:1316; depth 9.59 2021-02-12T23:09:55Z- handleDvl() - RDI_Pathfinder msg: 1613171395.55 epoch sec; seqNo:553 2021-02-12T23:09:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.15 , 106.84 , 75.19 , 119.01 2021-02-12T23:09:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.22 , -0.03 2021-02-12T23:09:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171395.56 epoch sec; seqNo:1316; 4056318.21 north; 590515.90 east 2021-02-12T23:09:55Z- AHRS_M2 msg: 1613171395.93 epoch sec; seqNo:1307 2021-02-12T23:09:55Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.18 psi 2021-02-12T23:09:55Z- handleDepth()-Depth_Keller msg: 1613171395949 epoch sec; seqNo:1317; depth 9.72 2021-02-12T23:09:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171395.96 epoch sec; seqNo:1317; 4056318.59 north; 590515.89 east 2021-02-12T23:09:56Z- AHRS_M2 msg: 1613171396.34 epoch sec; seqNo:1308 2021-02-12T23:09:56Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-12T23:09:56Z- handleDepth()-Depth_Keller msg: 1613171396344 epoch sec; seqNo:1318; depth 9.85 2021-02-12T23:09:56Z- handleDvl() - RDI_Pathfinder msg: 1613171396.36 epoch sec; seqNo:554 2021-02-12T23:09:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.22 , 96.09 , 73.96 , 115.02 2021-02-12T23:09:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.13 , 0.07 2021-02-12T23:09:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171396.38 epoch sec; seqNo:1318; 4056318.98 north; 590517.11 east 2021-02-12T23:09:56Z- AHRS_M2 msg: 1613171396.74 epoch sec; seqNo:1309 2021-02-12T23:09:56Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-12T23:09:56Z- handleDepth()-Depth_Keller msg: 1613171396753 epoch sec; seqNo:1319; depth 9.99 2021-02-12T23:09:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171396.76 epoch sec; seqNo:1319; 4056318.98 north; 590517.11 east 2021-02-12T23:09:57Z- AHRS_M2 msg: 1613171397.16 epoch sec; seqNo:1310 2021-02-12T23:09:57Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.19 psi 2021-02-12T23:09:57Z- handleDepth()-Depth_Keller msg: 1613171397167 epoch sec; seqNo:1320; depth 10.11 2021-02-12T23:09:57Z- handleDvl() - RDI_Pathfinder msg: 1613171397.18 epoch sec; seqNo:555 2021-02-12T23:09:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 96.43 , 73.56 , 111.03 2021-02-12T23:09:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , 0.05 2021-02-12T23:09:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171397.19 epoch sec; seqNo:1320; 4056319.36 north; 590517.11 east 2021-02-12T23:09:57Z- AHRS_M2 msg: 1613171397.56 epoch sec; seqNo:1311 2021-02-12T23:09:57Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.20 psi 2021-02-12T23:09:57Z- handleDepth()-Depth_Keller msg: 1613171397561 epoch sec; seqNo:1321; depth 10.23 2021-02-12T23:09:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171397.58 epoch sec; seqNo:1321; 4056319.74 north; 590517.10 east 2021-02-12T23:09:57Z- AHRS_M2 msg: 1613171397.96 epoch sec; seqNo:1312 2021-02-12T23:09:57Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.21 psi 2021-02-12T23:09:57Z- handleDepth()-Depth_Keller msg: 1613171397965 epoch sec; seqNo:1322; depth 10.34 2021-02-12T23:09:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171397.97 epoch sec; seqNo:1322; 4056319.74 north; 590517.10 east 2021-02-12T23:09:58Z- AHRS_M2 msg: 1613171398.36 epoch sec; seqNo:1313 2021-02-12T23:09:58Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.22 psi 2021-02-12T23:09:58Z- handleDepth()-Depth_Keller msg: 1613171398365 epoch sec; seqNo:1323; depth 10.45 2021-02-12T23:09:58Z- handleDvl() - RDI_Pathfinder msg: 1613171398.38 epoch sec; seqNo:556 2021-02-12T23:09:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.28 , 101.36 , 74.37 , 108.66 2021-02-12T23:09:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , -0.07 2021-02-12T23:09:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171398.39 epoch sec; seqNo:1323; 4056320.12 north; 590517.10 east 2021-02-12T23:09:58Z- AHRS_M2 msg: 1613171398.76 epoch sec; seqNo:1314 2021-02-12T23:09:58Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.23 psi 2021-02-12T23:09:58Z- handleDepth()-Depth_Keller msg: 1613171398768 epoch sec; seqNo:1324; depth 10.58 2021-02-12T23:09:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171398.79 epoch sec; seqNo:1324; 4056320.49 north; 590517.09 east 2021-02-12T23:09:59Z- AHRS_M2 msg: 1613171399.17 epoch sec; seqNo:1315 2021-02-12T23:09:59Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.23 psi 2021-02-12T23:09:59Z- handleDepth()-Depth_Keller msg: 1613171399177 epoch sec; seqNo:1325; depth 10.69 2021-02-12T23:09:59Z- handleDvl() - RDI_Pathfinder msg: 1613171399.19 epoch sec; seqNo:557 2021-02-12T23:09:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 96.50 , 74.78 , 107.45 2021-02-12T23:09:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.11 , -0.09 2021-02-12T23:09:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171399.20 epoch sec; seqNo:1325; 4056320.89 north; 590518.31 east 2021-02-12T23:09:59Z- AHRS_M2 msg: 1613171399.57 epoch sec; seqNo:1316 2021-02-12T23:09:59Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.22 psi 2021-02-12T23:09:59Z- handleDepth()-Depth_Keller msg: 1613171399576 epoch sec; seqNo:1326; depth 10.81 2021-02-12T23:09:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171399.59 epoch sec; seqNo:1326; 4056321.27 north; 590518.31 east 2021-02-12T23:09:59Z- AHRS_M2 msg: 1613171399.98 epoch sec; seqNo:1317 2021-02-12T23:09:59Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.21 psi 2021-02-12T23:09:59Z- handleDepth()-Depth_Keller msg: 1613171399982 epoch sec; seqNo:1327; depth 10.90 2021-02-12T23:09:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171399.99 epoch sec; seqNo:1327; 4056321.64 north; 590518.30 east 2021-02-12T23:10:00Z- AHRS_M2 msg: 1613171400.38 epoch sec; seqNo:1318 2021-02-12T23:10:00Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.20 psi 2021-02-12T23:10:00Z- handleDepth()-Depth_Keller msg: 1613171400384 epoch sec; seqNo:1328; depth 11.02 2021-02-12T23:10:00Z- handleDvl() - RDI_Pathfinder msg: 1613171400.40 epoch sec; seqNo:558 2021-02-12T23:10:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 96.63 , 73.96 , 108.80 2021-02-12T23:10:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.16 , -0.00 2021-02-12T23:10:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171400.42 epoch sec; seqNo:1328; 4056321.64 north; 590518.30 east 2021-02-12T23:10:00Z- LcmTrn::handleMessages() - handling 45 messages took 0.64 ms and 132 calls... 2021-02-12T23:10:00Z- waiting for 1613171398.18, time is 1613171400.42 2021-02-12T23:10:00Z- LcmTrn::updateTrn() equating pose 1613171400.42 and meas times 1613171400.42 2021-02-12T23:10:00Z- LcmTrn::updateTrn() >>>> pose time 1613171400.42, meas time 1613171400.42 <<<< 2021-02-12T23:10:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171400.42 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:10:00Z- TerrainNav::motionUpdate() - currEstimate = (4056322, 590518.30, 11.02) 2021-02-12T23:10:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:10:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:10:00Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171400.42 sec.; No relevant navigation data available 2021-02-12T23:10:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:10:00Z- TNavBF::mmse selected from filter -1 2021-02-12T23:10:00Z- variances([0], [2], and psi_berg) = 802.297243 833.108010 0.000000 2021-02-12T23:10:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:10:00Z- LcmTrn::publishEstimates() - MLE : 4056291.14 590472.80 11.02 2021-02-12T23:10:00Z- LcmTrn::publishEstimates() - MMSE : 4056321.64 590518.30 11.02 2021-02-12T23:10:00Z- LcmTrn::publishEstimates() - COVAR: 802.30 833.11 1.23 0.00 2021-02-12T23:10:00Z- AHRS_M2 msg: 1613171400.78 epoch sec; seqNo:1319 2021-02-12T23:10:00Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.19 psi 2021-02-12T23:10:00Z- handleDepth()-Depth_Keller msg: 1613171400788 epoch sec; seqNo:1329; depth 11.15 2021-02-12T23:10:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171400.80 epoch sec; seqNo:1329; 4056322.02 north; 590518.30 east 2021-02-12T23:10:01Z- AHRS_M2 msg: 1613171401.19 epoch sec; seqNo:1320 2021-02-12T23:10:01Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.18 psi 2021-02-12T23:10:01Z- handleDepth()-Depth_Keller msg: 1613171401196 epoch sec; seqNo:1330; depth 11.25 2021-02-12T23:10:01Z- handleDvl() - RDI_Pathfinder msg: 1613171401.21 epoch sec; seqNo:559 2021-02-12T23:10:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.05 , 100.28 , 72.74 , 105.15 2021-02-12T23:10:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.07 , 0.08 2021-02-12T23:10:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171401.22 epoch sec; seqNo:1330; 4056322.40 north; 590518.30 east 2021-02-12T23:10:01Z- AHRS_M2 msg: 1613171401.59 epoch sec; seqNo:1321 2021-02-12T23:10:01Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.17 psi 2021-02-12T23:10:01Z- handleDepth()-Depth_Keller msg: 1613171401606 epoch sec; seqNo:1331; depth 11.35 2021-02-12T23:10:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171401.62 epoch sec; seqNo:1331; 4056322.78 north; 590518.29 east 2021-02-12T23:10:02Z- AHRS_M2 msg: 1613171402.00 epoch sec; seqNo:1322 2021-02-12T23:10:02Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.16 psi 2021-02-12T23:10:02Z- handleDepth()-Depth_Keller msg: 1613171402020 epoch sec; seqNo:1332; depth 11.47 2021-02-12T23:10:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171402.03 epoch sec; seqNo:1332; 4056322.78 north; 590518.29 east 2021-02-12T23:10:02Z- AHRS_M2 msg: 1613171402.40 epoch sec; seqNo:1323 2021-02-12T23:10:02Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.15 psi 2021-02-12T23:10:02Z- handleDepth()-Depth_Keller msg: 1613171402408 epoch sec; seqNo:1333; depth 11.57 2021-02-12T23:10:02Z- handleDvl() - RDI_Pathfinder msg: 1613171402.42 epoch sec; seqNo:560 2021-02-12T23:10:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.05 , 96.63 , 73.56 , 105.15 2021-02-12T23:10:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , 0.08 2021-02-12T23:10:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171402.43 epoch sec; seqNo:1333; 4056323.17 north; 590519.51 east 2021-02-12T23:10:02Z- AHRS_M2 msg: 1613171402.82 epoch sec; seqNo:1324 2021-02-12T23:10:02Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.15 psi 2021-02-12T23:10:02Z- handleDepth()-Depth_Keller msg: 1613171402826 epoch sec; seqNo:1334; depth 11.67 2021-02-12T23:10:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171402.84 epoch sec; seqNo:1334; 4056323.55 north; 590519.51 east 2021-02-12T23:10:03Z- AHRS_M2 msg: 1613171403.21 epoch sec; seqNo:1325 2021-02-12T23:10:03Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.15 psi 2021-02-12T23:10:03Z- handleDepth()-Depth_Keller msg: 1613171403214 epoch sec; seqNo:1335; depth 11.76 2021-02-12T23:10:03Z- handleDvl() - RDI_Pathfinder msg: 1613171403.23 epoch sec; seqNo:561 2021-02-12T23:10:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.87 , 95.48 , 73.15 , 104.00 2021-02-12T23:10:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , 0.05 2021-02-12T23:10:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171403.25 epoch sec; seqNo:1335; 4056323.55 north; 590519.51 east 2021-02-12T23:10:03Z- AHRS_M2 msg: 1613171403.61 epoch sec; seqNo:1326 2021-02-12T23:10:03Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.17 psi 2021-02-12T23:10:03Z- handleDepth()-Depth_Keller msg: 1613171403616 epoch sec; seqNo:1336; depth 11.88 2021-02-12T23:10:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171403.63 epoch sec; seqNo:1336; 4056323.93 north; 590519.50 east 2021-02-12T23:10:04Z- AHRS_M2 msg: 1613171404.02 epoch sec; seqNo:1327 2021-02-12T23:10:04Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.17 psi 2021-02-12T23:10:04Z- handleDepth()-Depth_Keller msg: 1613171404025 epoch sec; seqNo:1337; depth 11.98 2021-02-12T23:10:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171404.04 epoch sec; seqNo:1337; 4056324.31 north; 590519.50 east 2021-02-12T23:10:04Z- AHRS_M2 msg: 1613171404.42 epoch sec; seqNo:1328 2021-02-12T23:10:04Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.18 psi 2021-02-12T23:10:04Z- handleDepth()-Depth_Keller msg: 1613171404424 epoch sec; seqNo:1338; depth 12.10 2021-02-12T23:10:04Z- handleDvl() - RDI_Pathfinder msg: 1613171404.44 epoch sec; seqNo:562 2021-02-12T23:10:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.46 , 97.92 , 72.74 , 106.43 2021-02-12T23:10:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , 0.03 2021-02-12T23:10:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171404.45 epoch sec; seqNo:1338; 4056324.31 north; 590519.50 east 2021-02-12T23:10:04Z- AHRS_M2 msg: 1613171404.82 epoch sec; seqNo:1329 2021-02-12T23:10:04Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.19 psi 2021-02-12T23:10:04Z- handleDepth()-Depth_Keller msg: 1613171404833 epoch sec; seqNo:1339; depth 12.22 2021-02-12T23:10:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171404.84 epoch sec; seqNo:1339; 4056324.70 north; 590520.71 east 2021-02-12T23:10:05Z- AHRS_M2 msg: 1613171405.23 epoch sec; seqNo:1330 2021-02-12T23:10:05Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.19 psi 2021-02-12T23:10:05Z- handleDepth()-Depth_Keller msg: 1613171405233 epoch sec; seqNo:1340; depth 12.33 2021-02-12T23:10:05Z- handleDvl() - RDI_Pathfinder msg: 1613171405.24 epoch sec; seqNo:563 2021-02-12T23:10:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.64 , 96.63 , 71.51 , 107.58 2021-02-12T23:10:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.00 , -0.05 2021-02-12T23:10:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171405.25 epoch sec; seqNo:1340; 4056325.08 north; 590520.71 east 2021-02-12T23:10:05Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-12T23:10:05Z- waiting for 1613171403.42, time is 1613171405.25 2021-02-12T23:10:05Z- LcmTrn::updateTrn() equating pose 1613171405.25 and meas times 1613171405.25 2021-02-12T23:10:05Z- LcmTrn::updateTrn() >>>> pose time 1613171405.25, meas time 1613171405.25 <<<< 2021-02-12T23:10:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171405.25 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:10:05Z- TerrainNav::motionUpdate() - currEstimate = (4056325, 590520.71, 12.33) 2021-02-12T23:10:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:10:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:10:05Z- TNavBF::Projecting Measurements 2021-02-12T23:10:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:10:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:10:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:10:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:10:05Z- TNavFilter::4 good beams. 2021-02-12T23:10:05Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:10:05Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:10:05Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:10:05Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:10:05Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:10:05Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:10:05Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:10:05Z- TNavBF::mmse selected from filter -1 2021-02-12T23:10:05Z- TerrainMapOctree: (vehN, vehE ) = (4056325.08, 590520.71). 2021-02-12T23:10:05Z- TerrainMapOctree: Min Distance = 6177.58. 2021-02-12T23:10:05Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:10:05Z- There is no specific good beam that all particles have in common. 2021-02-12T23:10:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:10:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.44 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 9.65 2021-02-12T23:10:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:10:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 14.94 2021-02-12T23:10:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:10:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 8.18 2021-02-12T23:10:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:10:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 18.52 2021-02-12T23:10:05Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:10:05Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:10:05Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:10:05Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:10:05Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:10:05Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:10:05Z- Filter 0 No beams used in subcloud update 2021-02-12T23:10:05Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:10:05Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:10:05Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 802.1 EastVariance: 833.1 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:10:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171405.25 sec. 2021-02-12T23:10:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:10:05Z- TNavBF::mmse selected from filter -1 2021-02-12T23:10:05Z- variances([0], [2], and psi_berg) = 802.053824 833.107449 0.000000 2021-02-12T23:10:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:10:05Z- LcmTrn::publishEstimates() - MLE : 4056294.58 590475.21 12.33 2021-02-12T23:10:05Z- LcmTrn::publishEstimates() - MMSE : 4056325.08 590520.71 12.33 2021-02-12T23:10:05Z- LcmTrn::publishEstimates() - COVAR: 802.05 833.11 1.01 0.00 2021-02-12T23:10:05Z- AHRS_M2 msg: 1613171405.63 epoch sec; seqNo:1331 2021-02-12T23:10:05Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.19 psi 2021-02-12T23:10:05Z- handleDepth()-Depth_Keller msg: 1613171405638 epoch sec; seqNo:1341; depth 12.44 2021-02-12T23:10:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171405.65 epoch sec; seqNo:1341; 4056325.46 north; 590520.71 east 2021-02-12T23:10:06Z- AHRS_M2 msg: 1613171406.04 epoch sec; seqNo:1332 2021-02-12T23:10:06Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.19 psi 2021-02-12T23:10:06Z- handleDepth()-Depth_Keller msg: 1613171406060 epoch sec; seqNo:1342; depth 12.57 2021-02-12T23:10:06Z- handleDvl() - RDI_Pathfinder msg: 1613171406.08 epoch sec; seqNo:564 2021-02-12T23:10:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 96.70 , 72.74 , 107.65 2021-02-12T23:10:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.14 2021-02-12T23:10:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171406.09 epoch sec; seqNo:1342; 4056325.46 north; 590520.71 east 2021-02-12T23:10:06Z- AHRS_M2 msg: 1613171406.44 epoch sec; seqNo:1333 2021-02-12T23:10:06Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.19 psi 2021-02-12T23:10:06Z- handleDepth()-Depth_Keller msg: 1613171406449 epoch sec; seqNo:1343; depth 12.68 2021-02-12T23:10:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171406.46 epoch sec; seqNo:1343; 4056325.84 north; 590520.70 east 2021-02-12T23:10:06Z- AHRS_M2 msg: 1613171406.85 epoch sec; seqNo:1334 2021-02-12T23:10:06Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.19 psi 2021-02-12T23:10:06Z- handleDepth()-Depth_Keller msg: 1613171406857 epoch sec; seqNo:1344; depth 12.79 2021-02-12T23:10:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171406.87 epoch sec; seqNo:1344; 4056326.22 north; 590520.70 east 2021-02-12T23:10:07Z- AHRS_M2 msg: 1613171407.25 epoch sec; seqNo:1335 2021-02-12T23:10:07Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.19 psi 2021-02-12T23:10:07Z- handleDepth()-Depth_Keller msg: 1613171407252 epoch sec; seqNo:1345; depth 12.90 2021-02-12T23:10:07Z- handleDvl() - RDI_Pathfinder msg: 1613171407.26 epoch sec; seqNo:565 2021-02-12T23:10:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.42 , 96.70 , 72.74 , 104.00 2021-02-12T23:10:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.13 , -0.17 2021-02-12T23:10:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171407.29 epoch sec; seqNo:1345; 4056326.59 north; 590520.69 east 2021-02-12T23:10:07Z- AHRS_M2 msg: 1613171407.65 epoch sec; seqNo:1336 2021-02-12T23:10:07Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.19 psi 2021-02-12T23:10:07Z- handleDepth()-Depth_Keller msg: 1613171407657 epoch sec; seqNo:1346; depth 13.04 2021-02-12T23:10:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171407.67 epoch sec; seqNo:1346; 4056326.99 north; 590521.91 east 2021-02-12T23:10:08Z- AHRS_M2 msg: 1613171408.08 epoch sec; seqNo:1337 2021-02-12T23:10:08Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.19 psi 2021-02-12T23:10:08Z- handleDepth()-Depth_Keller msg: 1613171408093 epoch sec; seqNo:1347; depth 13.14 2021-02-12T23:10:08Z- handleDvl() - RDI_Pathfinder msg: 1613171408.12 epoch sec; seqNo:566 2021-02-12T23:10:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.00 , 96.76 , 71.92 , 106.50 2021-02-12T23:10:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.23 , -0.07 2021-02-12T23:10:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171408.15 epoch sec; seqNo:1347; 4056327.37 north; 590521.91 east 2021-02-12T23:10:08Z- AHRS_M2 msg: 1613171408.46 epoch sec; seqNo:1338 2021-02-12T23:10:08Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.19 psi 2021-02-12T23:10:08Z- handleDepth()-Depth_Keller msg: 1613171408464 epoch sec; seqNo:1348; depth 13.28 2021-02-12T23:10:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171408.47 epoch sec; seqNo:1348; 4056327.37 north; 590521.91 east 2021-02-12T23:10:08Z- AHRS_M2 msg: 1613171408.86 epoch sec; seqNo:1339 2021-02-12T23:10:08Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.18 psi 2021-02-12T23:10:08Z- handleDepth()-Depth_Keller msg: 1613171408873 epoch sec; seqNo:1349; depth 13.37 2021-02-12T23:10:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171408.88 epoch sec; seqNo:1349; 4056327.74 north; 590521.90 east 2021-02-12T23:10:09Z- AHRS_M2 msg: 1613171409.27 epoch sec; seqNo:1340 2021-02-12T23:10:09Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.18 psi 2021-02-12T23:10:09Z- handleDepth()-Depth_Keller msg: 1613171409277 epoch sec; seqNo:1350; depth 13.47 2021-02-12T23:10:09Z- handleDvl() - RDI_Pathfinder msg: 1613171409.29 epoch sec; seqNo:567 2021-02-12T23:10:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.41 , 96.76 , 71.51 , 96.76 2021-02-12T23:10:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.22 , 0.06 2021-02-12T23:10:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171409.30 epoch sec; seqNo:1350; 4056328.12 north; 590521.90 east 2021-02-12T23:10:09Z- AHRS_M2 msg: 1613171409.67 epoch sec; seqNo:1341 2021-02-12T23:10:09Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.17 psi 2021-02-12T23:10:09Z- handleDepth()-Depth_Keller msg: 1613171409681 epoch sec; seqNo:1351; depth 13.60 2021-02-12T23:10:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171409.70 epoch sec; seqNo:1351; 4056328.50 north; 590521.90 east 2021-02-12T23:10:10Z- AHRS_M2 msg: 1613171410.08 epoch sec; seqNo:1342 2021-02-12T23:10:10Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.16 psi 2021-02-12T23:10:10Z- handleDepth()-Depth_Keller msg: 1613171410080 epoch sec; seqNo:1352; depth 13.70 2021-02-12T23:10:10Z- handleDvl() - RDI_Pathfinder msg: 1613171410.09 epoch sec; seqNo:568 2021-02-12T23:10:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 95.75 , 70.70 , 96.97 2021-02-12T23:10:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.11 , 0.14 2021-02-12T23:10:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171410.10 epoch sec; seqNo:1352; 4056328.88 north; 590521.89 east 2021-02-12T23:10:10Z- AHRS_M2 msg: 1613171410.48 epoch sec; seqNo:1343 2021-02-12T23:10:10Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.16 psi 2021-02-12T23:10:10Z- handleDepth()-Depth_Keller msg: 1613171410493 epoch sec; seqNo:1353; depth 13.83 2021-02-12T23:10:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171410.52 epoch sec; seqNo:1353; 4056328.88 north; 590521.89 east 2021-02-12T23:10:10Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-12T23:10:10Z- waiting for 1613171408.25, time is 1613171410.52 2021-02-12T23:10:10Z- LcmTrn::updateTrn() >>>> pose time 1613171410.52, meas time 1613171410.09 <<<< 2021-02-12T23:10:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171410.09 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:10:10Z- TerrainNav::motionUpdate() - currEstimate = (4056329, 590521.89, 13.83) 2021-02-12T23:10:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:10:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:10:10Z- TNavBF::Projecting Measurements 2021-02-12T23:10:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:10:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:10:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:10:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:10:10Z- TNavFilter::4 good beams. 2021-02-12T23:10:10Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:10:10Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:10:10Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:10:10Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:10:10Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:10:10Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:10:10Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:10:10Z- TNavBF::mmse selected from filter -1 2021-02-12T23:10:10Z- TerrainMapOctree: (vehN, vehE ) = (4056328.57, 590521.80). 2021-02-12T23:10:10Z- TerrainMapOctree: Min Distance = 6174.14. 2021-02-12T23:10:10Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:10:10Z- There is no specific good beam that all particles have in common. 2021-02-12T23:10:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:10:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 9.44 2021-02-12T23:10:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:10:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 14.67 2021-02-12T23:10:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:10:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-12T23:10:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:10:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 15.05 2021-02-12T23:10:10Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:10:10Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:10:10Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:10:10Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:10:10Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:10:10Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:10:10Z- Filter 0 No beams used in subcloud update 2021-02-12T23:10:10Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:10:10Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:10:10Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 801.8 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:10:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171410.09 sec. 2021-02-12T23:10:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:10:10Z- TNavBF::mmse selected from filter -1 2021-02-12T23:10:10Z- variances([0], [2], and psi_berg) = 801.760888 833.106780 0.000000 2021-02-12T23:10:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:10:10Z- LcmTrn::publishEstimates() - MLE : 4056298.38 590476.39 13.83 2021-02-12T23:10:10Z- LcmTrn::publishEstimates() - MMSE : 4056328.88 590521.89 13.83 2021-02-12T23:10:10Z- LcmTrn::publishEstimates() - COVAR: 801.76 833.11 0.89 0.00 2021-02-12T23:10:10Z- AHRS_M2 msg: 1613171410.88 epoch sec; seqNo:1344 2021-02-12T23:10:10Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.15 psi 2021-02-12T23:10:10Z- handleDepth()-Depth_Keller msg: 1613171410894 epoch sec; seqNo:1354; depth 13.94 2021-02-12T23:10:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171410.90 epoch sec; seqNo:1354; 4056329.26 north; 590521.89 east 2021-02-12T23:10:11Z- AHRS_M2 msg: 1613171411.29 epoch sec; seqNo:1345 2021-02-12T23:10:11Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.14 psi 2021-02-12T23:10:11Z- handleDepth()-Depth_Keller msg: 1613171411297 epoch sec; seqNo:1355; depth 14.04 2021-02-12T23:10:11Z- handleDvl() - RDI_Pathfinder msg: 1613171411.33 epoch sec; seqNo:569 2021-02-12T23:10:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 94.54 , 71.11 , 96.97 2021-02-12T23:10:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , 0.11 2021-02-12T23:10:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171411.34 epoch sec; seqNo:1355; 4056329.65 north; 590523.10 east 2021-02-12T23:10:11Z- AHRS_M2 msg: 1613171411.69 epoch sec; seqNo:1346 2021-02-12T23:10:11Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.14 psi 2021-02-12T23:10:11Z- handleDepth()-Depth_Keller msg: 1613171411696 epoch sec; seqNo:1356; depth 14.15 2021-02-12T23:10:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171411.71 epoch sec; seqNo:1356; 4056329.65 north; 590523.10 east 2021-02-12T23:10:12Z- AHRS_M2 msg: 1613171412.10 epoch sec; seqNo:1347 2021-02-12T23:10:12Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.14 psi 2021-02-12T23:10:12Z- handleDepth()-Depth_Keller msg: 1613171412100 epoch sec; seqNo:1357; depth 14.26 2021-02-12T23:10:12Z- handleDvl() - RDI_Pathfinder msg: 1613171412.11 epoch sec; seqNo:570 2021-02-12T23:10:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 93.32 , 70.29 , 104.27 2021-02-12T23:10:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.05 , 0.07 2021-02-12T23:10:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171412.13 epoch sec; seqNo:1357; 4056330.03 north; 590523.10 east 2021-02-12T23:10:12Z- AHRS_M2 msg: 1613171412.50 epoch sec; seqNo:1348 2021-02-12T23:10:12Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.14 psi 2021-02-12T23:10:12Z- handleDepth()-Depth_Keller msg: 1613171412506 epoch sec; seqNo:1358; depth 14.36 2021-02-12T23:10:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171412.52 epoch sec; seqNo:1358; 4056330.41 north; 590523.10 east 2021-02-12T23:10:12Z- AHRS_M2 msg: 1613171412.90 epoch sec; seqNo:1349 2021-02-12T23:10:12Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.15 psi 2021-02-12T23:10:12Z- handleDepth()-Depth_Keller msg: 1613171412908 epoch sec; seqNo:1359; depth 14.46 2021-02-12T23:10:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171412.93 epoch sec; seqNo:1359; 4056330.41 north; 590523.10 east 2021-02-12T23:10:13Z- AHRS_M2 msg: 1613171413.31 epoch sec; seqNo:1350 2021-02-12T23:10:13Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.16 psi 2021-02-12T23:10:13Z- handleDepth()-Depth_Keller msg: 1613171413317 epoch sec; seqNo:1360; depth 14.57 2021-02-12T23:10:13Z- handleDvl() - RDI_Pathfinder msg: 1613171413.33 epoch sec; seqNo:571 2021-02-12T23:10:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 91.97 , 70.29 , 105.35 2021-02-12T23:10:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , 0.03 2021-02-12T23:10:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171413.37 epoch sec; seqNo:1360; 4056330.79 north; 590523.09 east 2021-02-12T23:10:13Z- AHRS_M2 msg: 1613171413.71 epoch sec; seqNo:1351 2021-02-12T23:10:13Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.16 psi 2021-02-12T23:10:13Z- handleDepth()-Depth_Keller msg: 1613171413717 epoch sec; seqNo:1361; depth 14.69 2021-02-12T23:10:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171413.73 epoch sec; seqNo:1361; 4056331.18 north; 590524.31 east 2021-02-12T23:10:14Z- AHRS_M2 msg: 1613171414.12 epoch sec; seqNo:1352 2021-02-12T23:10:14Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.17 psi 2021-02-12T23:10:14Z- handleDepth()-Depth_Keller msg: 1613171414130 epoch sec; seqNo:1362; depth 14.80 2021-02-12T23:10:14Z- handleDvl() - RDI_Pathfinder msg: 1613171414.14 epoch sec; seqNo:572 2021-02-12T23:10:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.01 , 94.47 , 69.88 , 96.90 2021-02-12T23:10:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , -0.02 2021-02-12T23:10:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171414.15 epoch sec; seqNo:1362; 4056331.18 north; 590524.31 east 2021-02-12T23:10:14Z- AHRS_M2 msg: 1613171414.52 epoch sec; seqNo:1353 2021-02-12T23:10:14Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.17 psi 2021-02-12T23:10:14Z- handleDepth()-Depth_Keller msg: 1613171414524 epoch sec; seqNo:1363; depth 14.92 2021-02-12T23:10:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171414.54 epoch sec; seqNo:1363; 4056331.56 north; 590524.31 east 2021-02-12T23:10:14Z- AHRS_M2 msg: 1613171414.93 epoch sec; seqNo:1354 2021-02-12T23:10:14Z- AHRS_M2 msg: 0.66 phi; -0.30 theta; -0.17 psi 2021-02-12T23:10:14Z- handleDepth()-Depth_Keller msg: 1613171414930 epoch sec; seqNo:1364; depth 15.02 2021-02-12T23:10:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171414.94 epoch sec; seqNo:1364; 4056331.94 north; 590524.30 east 2021-02-12T23:10:15Z- AHRS_M2 msg: 1613171415.34 epoch sec; seqNo:1355 2021-02-12T23:10:15Z- AHRS_M2 msg: 0.66 phi; -0.30 theta; -0.17 psi 2021-02-12T23:10:15Z- handleDepth()-Depth_Keller msg: 1613171415341 epoch sec; seqNo:1365; depth 15.13 2021-02-12T23:10:15Z- handleDvl() - RDI_Pathfinder msg: 1613171415.35 epoch sec; seqNo:573 2021-02-12T23:10:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.19 , 94.60 , 70.29 , 97.04 2021-02-12T23:10:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.00 , -0.10 2021-02-12T23:10:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171415.36 epoch sec; seqNo:1365; 4056332.32 north; 590524.30 east 2021-02-12T23:10:15Z- AHRS_M2 msg: 1613171415.73 epoch sec; seqNo:1356 2021-02-12T23:10:15Z- AHRS_M2 msg: 0.67 phi; -0.30 theta; -0.18 psi 2021-02-12T23:10:15Z- handleDepth()-Depth_Keller msg: 1613171415736 epoch sec; seqNo:1366; depth 15.26 2021-02-12T23:10:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171415.75 epoch sec; seqNo:1366; 4056332.33 north; 590525.52 east 2021-02-12T23:10:15Z- LcmTrn::handleMessages() - handling 44 messages took 0.75 ms and 129 calls... 2021-02-12T23:10:15Z- waiting for 1613171413.52, time is 1613171415.75 2021-02-12T23:10:15Z- LcmTrn::updateTrn() >>>> pose time 1613171415.75, meas time 1613171415.35 <<<< 2021-02-12T23:10:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171415.35 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:10:15Z- TerrainNav::motionUpdate() - currEstimate = (4056332, 590525.52, 15.26) 2021-02-12T23:10:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:10:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:10:15Z- TNavBF::Projecting Measurements 2021-02-12T23:10:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:10:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:10:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:10:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:10:15Z- TNavFilter::4 good beams. 2021-02-12T23:10:15Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:10:15Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:10:15Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:10:15Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:10:15Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:10:15Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:10:15Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:10:15Z- TNavBF::mmse selected from filter -1 2021-02-12T23:10:15Z- TerrainMapOctree: (vehN, vehE ) = (4056332.07, 590525.24). 2021-02-12T23:10:15Z- TerrainMapOctree: Min Distance = 6169.27. 2021-02-12T23:10:15Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:10:15Z- There is no specific good beam that all particles have in common. 2021-02-12T23:10:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:10:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 9.05 2021-02-12T23:10:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:10:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 14.32 2021-02-12T23:10:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:10:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 7.90 2021-02-12T23:10:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:10:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 15.07 2021-02-12T23:10:15Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:10:15Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:10:15Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:10:15Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:10:15Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:10:15Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:10:15Z- Filter 0 No beams used in subcloud update 2021-02-12T23:10:15Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:10:15Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:10:15Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 801.5 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:10:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171415.35 sec. 2021-02-12T23:10:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:10:15Z- TNavBF::mmse selected from filter -1 2021-02-12T23:10:15Z- variances([0], [2], and psi_berg) = 801.436324 833.106045 0.000000 2021-02-12T23:10:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:10:15Z- LcmTrn::publishEstimates() - MLE : 4056301.83 590480.02 15.26 2021-02-12T23:10:15Z- LcmTrn::publishEstimates() - MMSE : 4056332.33 590525.52 15.26 2021-02-12T23:10:15Z- LcmTrn::publishEstimates() - COVAR: 801.44 833.11 0.85 0.00 2021-02-12T23:10:16Z- AHRS_M2 msg: 1613171416.14 epoch sec; seqNo:1357 2021-02-12T23:10:16Z- AHRS_M2 msg: 0.67 phi; -0.30 theta; -0.18 psi 2021-02-12T23:10:16Z- handleDepth()-Depth_Keller msg: 1613171416144 epoch sec; seqNo:1367; depth 15.37 2021-02-12T23:10:16Z- handleDvl() - RDI_Pathfinder msg: 1613171416.15 epoch sec; seqNo:574 2021-02-12T23:10:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 94.60 , 69.88 , 97.04 2021-02-12T23:10:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.07 , -0.15 2021-02-12T23:10:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171416.17 epoch sec; seqNo:1367; 4056332.71 north; 590525.51 east 2021-02-12T23:10:16Z- AHRS_M2 msg: 1613171416.54 epoch sec; seqNo:1358 2021-02-12T23:10:16Z- AHRS_M2 msg: 0.68 phi; -0.30 theta; -0.18 psi 2021-02-12T23:10:16Z- handleDepth()-Depth_Keller msg: 1613171416549 epoch sec; seqNo:1368; depth 15.48 2021-02-12T23:10:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171416.56 epoch sec; seqNo:1368; 4056333.09 north; 590525.51 east 2021-02-12T23:10:16Z- AHRS_M2 msg: 1613171416.94 epoch sec; seqNo:1359 2021-02-12T23:10:16Z- AHRS_M2 msg: 0.68 phi; -0.30 theta; -0.19 psi 2021-02-12T23:10:16Z- handleDepth()-Depth_Keller msg: 1613171416949 epoch sec; seqNo:1369; depth 15.59 2021-02-12T23:10:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171416.96 epoch sec; seqNo:1369; 4056333.47 north; 590525.51 east 2021-02-12T23:10:17Z- AHRS_M2 msg: 1613171417.35 epoch sec; seqNo:1360 2021-02-12T23:10:17Z- AHRS_M2 msg: 0.68 phi; -0.30 theta; -0.19 psi 2021-02-12T23:10:17Z- handleDepth()-Depth_Keller msg: 1613171417352 epoch sec; seqNo:1370; depth 15.69 2021-02-12T23:10:17Z- handleDvl() - RDI_Pathfinder msg: 1613171417.37 epoch sec; seqNo:575 2021-02-12T23:10:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 94.60 , 69.47 , 97.04 2021-02-12T23:10:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.13 , -0.13 2021-02-12T23:10:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171417.39 epoch sec; seqNo:1370; 4056333.84 north; 590525.50 east 2021-02-12T23:10:17Z- AHRS_M2 msg: 1613171417.75 epoch sec; seqNo:1361 2021-02-12T23:10:17Z- AHRS_M2 msg: 0.69 phi; -0.30 theta; -0.18 psi 2021-02-12T23:10:17Z- handleDepth()-Depth_Keller msg: 1613171417756 epoch sec; seqNo:1371; depth 15.80 2021-02-12T23:10:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171417.77 epoch sec; seqNo:1371; 4056334.22 north; 590525.50 east 2021-02-12T23:10:18Z- AHRS_M2 msg: 1613171418.16 epoch sec; seqNo:1362 2021-02-12T23:10:18Z- AHRS_M2 msg: 0.69 phi; -0.30 theta; -0.18 psi 2021-02-12T23:10:18Z- handleDepth()-Depth_Keller msg: 1613171418166 epoch sec; seqNo:1372; depth 15.91 2021-02-12T23:10:18Z- handleDvl() - RDI_Pathfinder msg: 1613171418.18 epoch sec; seqNo:576 2021-02-12T23:10:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.55 , 94.67 , 69.06 , 100.75 2021-02-12T23:10:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.12 , -0.03 2021-02-12T23:10:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171418.19 epoch sec; seqNo:1372; 4056334.22 north; 590525.50 east 2021-02-12T23:10:18Z- AHRS_M2 msg: 1613171418.56 epoch sec; seqNo:1363 2021-02-12T23:10:18Z- AHRS_M2 msg: 0.69 phi; -0.30 theta; -0.17 psi 2021-02-12T23:10:18Z- handleDepth()-Depth_Keller msg: 1613171418564 epoch sec; seqNo:1373; depth 16.04 2021-02-12T23:10:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171418.59 epoch sec; seqNo:1373; 4056334.61 north; 590526.72 east 2021-02-12T23:10:19Z- AHRS_M2 msg: 1613171418.99 epoch sec; seqNo:1364 2021-02-12T23:10:19Z- AHRS_M2 msg: 0.70 phi; -0.30 theta; -0.16 psi 2021-02-12T23:10:19Z- handleDepth()-Depth_Keller msg: 1613171419002 epoch sec; seqNo:1374; depth 16.14 2021-02-12T23:10:19Z- handleDvl() - RDI_Pathfinder msg: 1613171419.02 epoch sec; seqNo:577 2021-02-12T23:10:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.55 , 93.38 , 68.65 , 99.46 2021-02-12T23:10:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , 0.02 2021-02-12T23:10:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171419.03 epoch sec; seqNo:1374; 4056334.99 north; 590526.71 east 2021-02-12T23:10:19Z- AHRS_M2 msg: 1613171419.38 epoch sec; seqNo:1365 2021-02-12T23:10:19Z- AHRS_M2 msg: 0.70 phi; -0.29 theta; -0.16 psi 2021-02-12T23:10:19Z- handleDepth()-Depth_Keller msg: 1613171419383 epoch sec; seqNo:1375; depth 16.25 2021-02-12T23:10:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171419.39 epoch sec; seqNo:1375; 4056334.99 north; 590526.71 east 2021-02-12T23:10:19Z- AHRS_M2 msg: 1613171419.77 epoch sec; seqNo:1366 2021-02-12T23:10:19Z- AHRS_M2 msg: 0.70 phi; -0.29 theta; -0.16 psi 2021-02-12T23:10:19Z- handleDepth()-Depth_Keller msg: 1613171419778 epoch sec; seqNo:1376; depth 16.35 2021-02-12T23:10:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171419.79 epoch sec; seqNo:1376; 4056335.37 north; 590526.71 east 2021-02-12T23:10:20Z- AHRS_M2 msg: 1613171420.18 epoch sec; seqNo:1367 2021-02-12T23:10:20Z- AHRS_M2 msg: 0.71 phi; -0.29 theta; -0.15 psi 2021-02-12T23:10:20Z- handleDepth()-Depth_Keller msg: 1613171420180 epoch sec; seqNo:1377; depth 16.45 2021-02-12T23:10:20Z- handleDvl() - RDI_Pathfinder msg: 1613171420.20 epoch sec; seqNo:578 2021-02-12T23:10:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 91.02 , 68.24 , 97.11 2021-02-12T23:10:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.04 , 0.01 2021-02-12T23:10:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171420.21 epoch sec; seqNo:1377; 4056335.75 north; 590526.70 east 2021-02-12T23:10:20Z- AHRS_M2 msg: 1613171420.58 epoch sec; seqNo:1368 2021-02-12T23:10:20Z- AHRS_M2 msg: 0.71 phi; -0.29 theta; -0.15 psi 2021-02-12T23:10:20Z- handleDepth()-Depth_Keller msg: 1613171420584 epoch sec; seqNo:1378; depth 16.56 2021-02-12T23:10:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171420.60 epoch sec; seqNo:1378; 4056335.75 north; 590526.70 east 2021-02-12T23:10:20Z- LcmTrn::handleMessages() - handling 41 messages took 0.61 ms and 129 calls... 2021-02-12T23:10:20Z- waiting for 1613171418.75, time is 1613171420.60 2021-02-12T23:10:20Z- LcmTrn::updateTrn() >>>> pose time 1613171420.60, meas time 1613171420.20 <<<< 2021-02-12T23:10:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171420.20 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:10:20Z- TerrainNav::motionUpdate() - currEstimate = (4056336, 590526.70, 16.56) 2021-02-12T23:10:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:10:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:10:20Z- TNavBF::Projecting Measurements 2021-02-12T23:10:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:10:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:10:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:10:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:10:20Z- TNavFilter::4 good beams. 2021-02-12T23:10:20Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:10:20Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:10:20Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:10:20Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:10:20Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:10:20Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:10:20Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:10:20Z- TNavBF::mmse selected from filter -1 2021-02-12T23:10:20Z- TerrainMapOctree: (vehN, vehE ) = (4056335.47, 590526.61). 2021-02-12T23:10:20Z- TerrainMapOctree: Min Distance = 6165.73. 2021-02-12T23:10:20Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:10:20Z- There is no specific good beam that all particles have in common. 2021-02-12T23:10:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:10:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 8.75 2021-02-12T23:10:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:10:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 13.26 2021-02-12T23:10:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:10:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-12T23:10:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:10:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-12T23:10:20Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:10:20Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:10:20Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:10:20Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:10:20Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:10:20Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:10:20Z- Filter 0 No beams used in subcloud update 2021-02-12T23:10:20Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:10:20Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:10:20Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 801.2 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:10:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171420.20 sec. 2021-02-12T23:10:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:10:20Z- TNavBF::mmse selected from filter -1 2021-02-12T23:10:20Z- variances([0], [2], and psi_berg) = 801.160684 833.105425 0.000000 2021-02-12T23:10:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:10:20Z- LcmTrn::publishEstimates() - MLE : 4056306.25 590478.20 16.56 2021-02-12T23:10:20Z- LcmTrn::publishEstimates() - MMSE : 4056335.75 590526.70 16.56 2021-02-12T23:10:20Z- LcmTrn::publishEstimates() - COVAR: 801.16 833.11 0.78 0.00 2021-02-12T23:10:21Z- AHRS_M2 msg: 1613171421.00 epoch sec; seqNo:1369 2021-02-12T23:10:21Z- AHRS_M2 msg: 0.71 phi; -0.29 theta; -0.15 psi 2021-02-12T23:10:21Z- handleDepth()-Depth_Keller msg: 1613171421015 epoch sec; seqNo:1379; depth 16.66 2021-02-12T23:10:21Z- handleDvl() - RDI_Pathfinder msg: 1613171421.03 epoch sec; seqNo:579 2021-02-12T23:10:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 98.39 , 67.84 , 97.17 2021-02-12T23:10:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.11 , -0.01 2021-02-12T23:10:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171421.04 epoch sec; seqNo:1379; 4056336.14 north; 590527.92 east 2021-02-12T23:10:21Z- AHRS_M2 msg: 1613171421.39 epoch sec; seqNo:1370 2021-02-12T23:10:21Z- AHRS_M2 msg: 0.71 phi; -0.29 theta; -0.16 psi 2021-02-12T23:10:21Z- handleDepth()-Depth_Keller msg: 1613171421406 epoch sec; seqNo:1380; depth 16.78 2021-02-12T23:10:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171421.42 epoch sec; seqNo:1380; 4056336.52 north; 590527.92 east 2021-02-12T23:10:21Z- AHRS_M2 msg: 1613171421.79 epoch sec; seqNo:1371 2021-02-12T23:10:21Z- AHRS_M2 msg: 0.71 phi; -0.30 theta; -0.17 psi 2021-02-12T23:10:21Z- handleDepth()-Depth_Keller msg: 1613171421796 epoch sec; seqNo:1381; depth 16.88 2021-02-12T23:10:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171421.82 epoch sec; seqNo:1381; 4056336.52 north; 590527.92 east 2021-02-12T23:10:22Z- AHRS_M2 msg: 1613171422.20 epoch sec; seqNo:1372 2021-02-12T23:10:22Z- AHRS_M2 msg: 0.71 phi; -0.30 theta; -0.17 psi 2021-02-12T23:10:22Z- handleDepth()-Depth_Keller msg: 1613171422200 epoch sec; seqNo:1382; depth 16.99 2021-02-12T23:10:22Z- handleDvl() - RDI_Pathfinder msg: 1613171422.21 epoch sec; seqNo:580 2021-02-12T23:10:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 90.95 , 68.65 , 97.04 2021-02-12T23:10:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.09 , 0.01 2021-02-12T23:10:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171422.24 epoch sec; seqNo:1382; 4056336.90 north; 590527.91 east 2021-02-12T23:10:22Z- AHRS_M2 msg: 1613171422.60 epoch sec; seqNo:1373 2021-02-12T23:10:22Z- AHRS_M2 msg: 0.70 phi; -0.30 theta; -0.18 psi 2021-02-12T23:10:22Z- handleDepth()-Depth_Keller msg: 1613171422604 epoch sec; seqNo:1383; depth 17.08 2021-02-12T23:10:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171422.62 epoch sec; seqNo:1383; 4056337.28 north; 590527.91 east 2021-02-12T23:10:23Z- AHRS_M2 msg: 1613171423.02 epoch sec; seqNo:1374 2021-02-12T23:10:23Z- AHRS_M2 msg: 0.69 phi; -0.31 theta; -0.19 psi 2021-02-12T23:10:23Z- handleDepth()-Depth_Keller msg: 1613171423023 epoch sec; seqNo:1384; depth 17.18 2021-02-12T23:10:23Z- handleDvl() - RDI_Pathfinder msg: 1613171423.03 epoch sec; seqNo:581 2021-02-12T23:10:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.74 , 93.52 , 67.84 , 97.17 2021-02-12T23:10:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , 0.10 2021-02-12T23:10:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171423.04 epoch sec; seqNo:1384; 4056337.28 north; 590527.91 east 2021-02-12T23:10:23Z- AHRS_M2 msg: 1613171423.41 epoch sec; seqNo:1375 2021-02-12T23:10:23Z- AHRS_M2 msg: 0.69 phi; -0.31 theta; -0.19 psi 2021-02-12T23:10:23Z- handleDepth()-Depth_Keller msg: 1613171423412 epoch sec; seqNo:1385; depth 17.29 2021-02-12T23:10:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171423.42 epoch sec; seqNo:1385; 4056337.66 north; 590527.90 east 2021-02-12T23:10:23Z- AHRS_M2 msg: 1613171423.81 epoch sec; seqNo:1376 2021-02-12T23:10:23Z- AHRS_M2 msg: 0.68 phi; -0.31 theta; -0.20 psi 2021-02-12T23:10:23Z- handleDepth()-Depth_Keller msg: 1613171423817 epoch sec; seqNo:1386; depth 17.41 2021-02-12T23:10:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171423.83 epoch sec; seqNo:1386; 4056338.04 north; 590527.90 east 2021-02-12T23:10:24Z- AHRS_M2 msg: 1613171424.22 epoch sec; seqNo:1377 2021-02-12T23:10:24Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.20 psi 2021-02-12T23:10:24Z- handleDepth()-Depth_Keller msg: 1613171424249 epoch sec; seqNo:1387; depth 17.52 2021-02-12T23:10:24Z- handleDvl() - RDI_Pathfinder msg: 1613171424.26 epoch sec; seqNo:582 2021-02-12T23:10:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.74 , 93.52 , 67.84 , 97.17 2021-02-12T23:10:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , 0.16 2021-02-12T23:10:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171424.28 epoch sec; seqNo:1387; 4056338.05 north; 590529.12 east 2021-02-12T23:10:24Z- AHRS_M2 msg: 1613171424.62 epoch sec; seqNo:1378 2021-02-12T23:10:24Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.21 psi 2021-02-12T23:10:24Z- handleDepth()-Depth_Keller msg: 1613171424632 epoch sec; seqNo:1388; depth 17.63 2021-02-12T23:10:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171424.64 epoch sec; seqNo:1388; 4056338.43 north; 590529.12 east 2021-02-12T23:10:25Z- AHRS_M2 msg: 1613171425.03 epoch sec; seqNo:1379 2021-02-12T23:10:25Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.21 psi 2021-02-12T23:10:25Z- handleDepth()-Depth_Keller msg: 1613171425037 epoch sec; seqNo:1389; depth 17.74 2021-02-12T23:10:25Z- handleDvl() - RDI_Pathfinder msg: 1613171425.05 epoch sec; seqNo:583 2021-02-12T23:10:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.74 , 94.74 , 67.43 , 97.17 2021-02-12T23:10:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.08 , 0.13 2021-02-12T23:10:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171425.07 epoch sec; seqNo:1389; 4056338.43 north; 590529.12 east 2021-02-12T23:10:25Z- AHRS_M2 msg: 1613171425.43 epoch sec; seqNo:1380 2021-02-12T23:10:25Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.21 psi 2021-02-12T23:10:25Z- handleDepth()-Depth_Keller msg: 1613171425432 epoch sec; seqNo:1390; depth 17.86 2021-02-12T23:10:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171425.44 epoch sec; seqNo:1390; 4056338.81 north; 590529.11 east 2021-02-12T23:10:25Z- AHRS_M2 msg: 1613171425.83 epoch sec; seqNo:1381 2021-02-12T23:10:25Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.22 psi 2021-02-12T23:10:25Z- handleDepth()-Depth_Keller msg: 1613171425838 epoch sec; seqNo:1391; depth 17.97 2021-02-12T23:10:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171425.85 epoch sec; seqNo:1391; 4056339.19 north; 590529.11 east 2021-02-12T23:10:26Z- LcmTrn::handleMessages() - handling 44 messages took 0.61 ms and 132 calls... 2021-02-12T23:10:26Z- waiting for 1613171423.60, time is 1613171425.85 2021-02-12T23:10:26Z- LcmTrn::updateTrn() >>>> pose time 1613171425.85, meas time 1613171425.05 <<<< 2021-02-12T23:10:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171425.05 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:10:26Z- TerrainNav::motionUpdate() - currEstimate = (4056339, 590529.11, 17.97) 2021-02-12T23:10:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:10:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:10:26Z- TNavBF::Projecting Measurements 2021-02-12T23:10:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:10:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:10:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:10:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:10:26Z- TNavFilter::4 good beams. 2021-02-12T23:10:26Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:10:26Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:10:26Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:10:26Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:10:26Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:10:26Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:10:26Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:10:26Z- TNavBF::mmse selected from filter -1 2021-02-12T23:10:26Z- TerrainMapOctree: (vehN, vehE ) = (4056338.67, 590528.75). 2021-02-12T23:10:26Z- TerrainMapOctree: Min Distance = 6161.89. 2021-02-12T23:10:26Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:10:26Z- There is no specific good beam that all particles have in common. 2021-02-12T23:10:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:10:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 8.47 2021-02-12T23:10:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:10:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 14.36 2021-02-12T23:10:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:10:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 7.27 2021-02-12T23:10:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:10:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.76 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 15.11 2021-02-12T23:10:26Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:10:26Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:10:26Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:10:26Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:10:26Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:10:26Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:10:26Z- Filter 0 No beams used in subcloud update 2021-02-12T23:10:26Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:10:26Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:10:26Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 800.9 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:10:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171425.05 sec. 2021-02-12T23:10:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:10:26Z- TNavBF::mmse selected from filter -1 2021-02-12T23:10:26Z- variances([0], [2], and psi_berg) = 800.850546 833.104733 0.000000 2021-02-12T23:10:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:10:26Z- LcmTrn::publishEstimates() - MLE : 4056309.69 590480.61 17.97 2021-02-12T23:10:26Z- LcmTrn::publishEstimates() - MMSE : 4056339.19 590529.11 17.97 2021-02-12T23:10:26Z- LcmTrn::publishEstimates() - COVAR: 800.85 833.10 0.76 0.00 2021-02-12T23:10:26Z- AHRS_M2 msg: 1613171426.24 epoch sec; seqNo:1382 2021-02-12T23:10:26Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.22 psi 2021-02-12T23:10:26Z- handleDepth()-Depth_Keller msg: 1613171426245 epoch sec; seqNo:1392; depth 18.09 2021-02-12T23:10:26Z- handleDvl() - RDI_Pathfinder msg: 1613171426.26 epoch sec; seqNo:584 2021-02-12T23:10:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 95.96 , 67.84 , 97.17 2021-02-12T23:10:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.13 , 0.01 2021-02-12T23:10:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171426.27 epoch sec; seqNo:1392; 4056339.20 north; 590530.33 east 2021-02-12T23:10:26Z- AHRS_M2 msg: 1613171426.64 epoch sec; seqNo:1383 2021-02-12T23:10:26Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.22 psi 2021-02-12T23:10:26Z- handleDepth()-Depth_Keller msg: 1613171426657 epoch sec; seqNo:1393; depth 18.21 2021-02-12T23:10:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171426.71 epoch sec; seqNo:1393; 4056339.58 north; 590530.33 east 2021-02-12T23:10:27Z- AHRS_M2 msg: 1613171427.04 epoch sec; seqNo:1384 2021-02-12T23:10:27Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.22 psi 2021-02-12T23:10:27Z- handleDepth()-Depth_Keller msg: 1613171427050 epoch sec; seqNo:1394; depth 18.32 2021-02-12T23:10:27Z- handleDvl() - RDI_Pathfinder msg: 1613171427.06 epoch sec; seqNo:585 2021-02-12T23:10:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 94.67 , 66.61 , 103.19 2021-02-12T23:10:27Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.09 , -0.10 2021-02-12T23:10:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171427.07 epoch sec; seqNo:1394; 4056339.96 north; 590530.32 east 2021-02-12T23:10:27Z- AHRS_M2 msg: 1613171427.45 epoch sec; seqNo:1385 2021-02-12T23:10:27Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.22 psi 2021-02-12T23:10:27Z- handleDepth()-Depth_Keller msg: 1613171427457 epoch sec; seqNo:1395; depth 18.44 2021-02-12T23:10:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171427.47 epoch sec; seqNo:1395; 4056340.34 north; 590530.32 east 2021-02-12T23:10:27Z- AHRS_M2 msg: 1613171427.85 epoch sec; seqNo:1386 2021-02-12T23:10:27Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.22 psi 2021-02-12T23:10:27Z- handleDepth()-Depth_Keller msg: 1613171427856 epoch sec; seqNo:1396; depth 18.55 2021-02-12T23:10:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171427.87 epoch sec; seqNo:1396; 4056340.35 north; 590531.54 east 2021-02-12T23:10:28Z- AHRS_M2 msg: 1613171428.26 epoch sec; seqNo:1387 2021-02-12T23:10:28Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.22 psi 2021-02-12T23:10:28Z- handleDepth()-Depth_Keller msg: 1613171428261 epoch sec; seqNo:1397; depth 18.67 2021-02-12T23:10:28Z- handleDvl() - RDI_Pathfinder msg: 1613171428.27 epoch sec; seqNo:586 2021-02-12T23:10:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 98.25 , 66.61 , 97.04 2021-02-12T23:10:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.05 , -0.19 2021-02-12T23:10:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171428.28 epoch sec; seqNo:1397; 4056340.73 north; 590531.54 east 2021-02-12T23:10:28Z- AHRS_M2 msg: 1613171428.66 epoch sec; seqNo:1388 2021-02-12T23:10:28Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.22 psi 2021-02-12T23:10:28Z- handleDepth()-Depth_Keller msg: 1613171428673 epoch sec; seqNo:1398; depth 18.80 2021-02-12T23:10:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171428.68 epoch sec; seqNo:1398; 4056341.11 north; 590531.53 east 2021-02-12T23:10:29Z- AHRS_M2 msg: 1613171429.06 epoch sec; seqNo:1389 2021-02-12T23:10:29Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.22 psi 2021-02-12T23:10:29Z- handleDepth()-Depth_Keller msg: 1613171429073 epoch sec; seqNo:1399; depth 18.93 2021-02-12T23:10:29Z- handleDvl() - RDI_Pathfinder msg: 1613171429.09 epoch sec; seqNo:587 2021-02-12T23:10:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 95.96 , 66.61 , 103.26 2021-02-12T23:10:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.08 , -0.17 2021-02-12T23:10:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171429.10 epoch sec; seqNo:1399; 4056341.49 north; 590531.53 east 2021-02-12T23:10:29Z- AHRS_M2 msg: 1613171429.47 epoch sec; seqNo:1390 2021-02-12T23:10:29Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.22 psi 2021-02-12T23:10:29Z- handleDepth()-Depth_Keller msg: 1613171429477 epoch sec; seqNo:1400; depth 19.05 2021-02-12T23:10:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171429.49 epoch sec; seqNo:1400; 4056341.86 north; 590531.52 east 2021-02-12T23:10:29Z- AHRS_M2 msg: 1613171429.87 epoch sec; seqNo:1391 2021-02-12T23:10:29Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.22 psi 2021-02-12T23:10:29Z- handleDepth()-Depth_Keller msg: 1613171429876 epoch sec; seqNo:1401; depth 19.18 2021-02-12T23:10:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171429.88 epoch sec; seqNo:1401; 4056342.24 north; 590531.52 east 2021-02-12T23:10:30Z- AHRS_M2 msg: 1613171430.28 epoch sec; seqNo:1392 2021-02-12T23:10:30Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.22 psi 2021-02-12T23:10:30Z- handleDepth()-Depth_Keller msg: 1613171430289 epoch sec; seqNo:1402; depth 19.30 2021-02-12T23:10:30Z- handleDvl() - RDI_Pathfinder msg: 1613171430.30 epoch sec; seqNo:588 2021-02-12T23:10:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 93.39 , 65.79 , 101.90 2021-02-12T23:10:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.13 , -0.10 2021-02-12T23:10:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171430.31 epoch sec; seqNo:1402; 4056342.26 north; 590532.74 east 2021-02-12T23:10:30Z- AHRS_M2 msg: 1613171430.68 epoch sec; seqNo:1393 2021-02-12T23:10:30Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.22 psi 2021-02-12T23:10:30Z- handleDepth()-Depth_Keller msg: 1613171430689 epoch sec; seqNo:1403; depth 19.44 2021-02-12T23:10:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171430.71 epoch sec; seqNo:1403; 4056342.64 north; 590532.74 east 2021-02-12T23:10:31Z- AHRS_M2 msg: 1613171431.08 epoch sec; seqNo:1394 2021-02-12T23:10:31Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.22 psi 2021-02-12T23:10:31Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-12T23:10:31Z- waiting for 1613171428.85, time is 1613171431.08 2021-02-12T23:10:31Z- LcmTrn::updateTrn() >>>> pose time 1613171431.08, meas time 1613171430.30 <<<< 2021-02-12T23:10:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171430.30 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:10:31Z- TerrainNav::motionUpdate() - currEstimate = (4056343, 590532.74, 19.44) 2021-02-12T23:10:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:10:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:10:31Z- TNavBF::Projecting Measurements 2021-02-12T23:10:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:10:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:10:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:10:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:10:31Z- TNavFilter::4 good beams. 2021-02-12T23:10:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:10:31Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:10:31Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:10:31Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:10:31Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:10:31Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:10:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:10:31Z- TNavBF::mmse selected from filter -1 2021-02-12T23:10:31Z- TerrainMapOctree: (vehN, vehE ) = (4056342.12, 590532.19). 2021-02-12T23:10:31Z- TerrainMapOctree: Min Distance = 6157.06. 2021-02-12T23:10:31Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:10:31Z- There is no specific good beam that all particles have in common. 2021-02-12T23:10:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:10:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 7.90 2021-02-12T23:10:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:10:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 13.95 2021-02-12T23:10:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:10:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 6.93 2021-02-12T23:10:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:10:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.76 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 16.62 2021-02-12T23:10:31Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:10:31Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:10:31Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:10:31Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:10:31Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:10:31Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:10:31Z- Filter 0 No beams used in subcloud update 2021-02-12T23:10:31Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:10:31Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:10:31Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 800.6 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:10:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171430.30 sec. 2021-02-12T23:10:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:10:31Z- TNavBF::mmse selected from filter -1 2021-02-12T23:10:31Z- variances([0], [2], and psi_berg) = 800.514434 833.103990 0.000000 2021-02-12T23:10:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:10:31Z- LcmTrn::publishEstimates() - MLE : 4056313.14 590486.24 19.44 2021-02-12T23:10:31Z- LcmTrn::publishEstimates() - MMSE : 4056342.64 590532.74 19.44 2021-02-12T23:10:31Z- LcmTrn::publishEstimates() - COVAR: 800.51 833.10 0.77 0.00 2021-02-12T23:10:31Z- handleDepth()-Depth_Keller msg: 1613171431093 epoch sec; seqNo:1404; depth 19.57 2021-02-12T23:10:31Z- handleDvl() - RDI_Pathfinder msg: 1613171431.11 epoch sec; seqNo:589 2021-02-12T23:10:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 94.74 , 65.79 , 97.17 2021-02-12T23:10:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.12 , -0.01 2021-02-12T23:10:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171431.13 epoch sec; seqNo:1404; 4056343.01 north; 590532.73 east 2021-02-12T23:10:31Z- AHRS_M2 msg: 1613171431.49 epoch sec; seqNo:1395 2021-02-12T23:10:31Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.22 psi 2021-02-12T23:10:31Z- handleDepth()-Depth_Keller msg: 1613171431497 epoch sec; seqNo:1405; depth 19.69 2021-02-12T23:10:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171431.51 epoch sec; seqNo:1405; 4056343.39 north; 590532.73 east 2021-02-12T23:10:31Z- AHRS_M2 msg: 1613171431.89 epoch sec; seqNo:1396 2021-02-12T23:10:31Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.22 psi 2021-02-12T23:10:31Z- handleDepth()-Depth_Keller msg: 1613171431896 epoch sec; seqNo:1406; depth 19.82 2021-02-12T23:10:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171431.91 epoch sec; seqNo:1406; 4056343.77 north; 590532.73 east 2021-02-12T23:10:32Z- AHRS_M2 msg: 1613171432.30 epoch sec; seqNo:1397 2021-02-12T23:10:32Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.21 psi 2021-02-12T23:10:32Z- handleDepth()-Depth_Keller msg: 1613171432305 epoch sec; seqNo:1407; depth 19.94 2021-02-12T23:10:32Z- handleDvl() - RDI_Pathfinder msg: 1613171432.32 epoch sec; seqNo:590 2021-02-12T23:10:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 94.54 , 64.97 , 96.70 2021-02-12T23:10:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.10 , 0.05 2021-02-12T23:10:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171432.33 epoch sec; seqNo:1407; 4056343.77 north; 590532.73 east 2021-02-12T23:10:32Z- AHRS_M2 msg: 1613171432.70 epoch sec; seqNo:1398 2021-02-12T23:10:32Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.21 psi 2021-02-12T23:10:32Z- handleDepth()-Depth_Keller msg: 1613171432704 epoch sec; seqNo:1408; depth 20.08 2021-02-12T23:10:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171432.71 epoch sec; seqNo:1408; 4056344.15 north; 590532.72 east 2021-02-12T23:10:33Z- AHRS_M2 msg: 1613171433.11 epoch sec; seqNo:1399 2021-02-12T23:10:33Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.21 psi 2021-02-12T23:10:33Z- handleDepth()-Depth_Keller msg: 1613171433111 epoch sec; seqNo:1409; depth 20.21 2021-02-12T23:10:33Z- handleDvl() - RDI_Pathfinder msg: 1613171433.12 epoch sec; seqNo:591 2021-02-12T23:10:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 94.60 , 64.97 , 104.34 2021-02-12T23:10:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.07 , 0.08 2021-02-12T23:10:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171433.14 epoch sec; seqNo:1409; 4056344.53 north; 590532.72 east 2021-02-12T23:10:33Z- AHRS_M2 msg: 1613171433.51 epoch sec; seqNo:1400 2021-02-12T23:10:33Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-12T23:10:33Z- handleDepth()-Depth_Keller msg: 1613171433512 epoch sec; seqNo:1410; depth 20.33 2021-02-12T23:10:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171433.53 epoch sec; seqNo:1410; 4056344.53 north; 590532.72 east 2021-02-12T23:10:33Z- AHRS_M2 msg: 1613171433.91 epoch sec; seqNo:1401 2021-02-12T23:10:33Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.20 psi 2021-02-12T23:10:33Z- handleDepth()-Depth_Keller msg: 1613171433916 epoch sec; seqNo:1411; depth 20.46 2021-02-12T23:10:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171433.92 epoch sec; seqNo:1411; 4056344.92 north; 590533.93 east 2021-02-12T23:10:34Z- AHRS_M2 msg: 1613171434.32 epoch sec; seqNo:1402 2021-02-12T23:10:34Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-12T23:10:34Z- handleDepth()-Depth_Keller msg: 1613171434320 epoch sec; seqNo:1412; depth 20.58 2021-02-12T23:10:34Z- handleDvl() - RDI_Pathfinder msg: 1613171434.33 epoch sec; seqNo:592 2021-02-12T23:10:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 92.24 , 64.97 , 97.11 2021-02-12T23:10:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.05 , 0.08 2021-02-12T23:10:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171434.34 epoch sec; seqNo:1412; 4056345.30 north; 590533.93 east 2021-02-12T23:10:34Z- AHRS_M2 msg: 1613171434.72 epoch sec; seqNo:1403 2021-02-12T23:10:34Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-12T23:10:34Z- handleDepth()-Depth_Keller msg: 1613171434727 epoch sec; seqNo:1413; depth 20.72 2021-02-12T23:10:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171434.74 epoch sec; seqNo:1413; 4056345.30 north; 590533.93 east 2021-02-12T23:10:35Z- AHRS_M2 msg: 1613171435.12 epoch sec; seqNo:1404 2021-02-12T23:10:35Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-12T23:10:35Z- handleDepth()-Depth_Keller msg: 1613171435133 epoch sec; seqNo:1414; depth 20.85 2021-02-12T23:10:35Z- handleDvl() - RDI_Pathfinder msg: 1613171435.16 epoch sec; seqNo:593 2021-02-12T23:10:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 90.28 , 64.97 , 96.77 2021-02-12T23:10:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.02 , 0.07 2021-02-12T23:10:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171435.17 epoch sec; seqNo:1414; 4056345.68 north; 590533.93 east 2021-02-12T23:10:35Z- AHRS_M2 msg: 1613171435.53 epoch sec; seqNo:1405 2021-02-12T23:10:35Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-12T23:10:35Z- handleDepth()-Depth_Keller msg: 1613171435532 epoch sec; seqNo:1415; depth 20.99 2021-02-12T23:10:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171435.55 epoch sec; seqNo:1415; 4056345.68 north; 590533.93 east 2021-02-12T23:10:35Z- AHRS_M2 msg: 1613171435.93 epoch sec; seqNo:1406 2021-02-12T23:10:35Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.20 psi 2021-02-12T23:10:35Z- handleDepth()-Depth_Keller msg: 1613171435941 epoch sec; seqNo:1416; depth 21.12 2021-02-12T23:10:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171435.95 epoch sec; seqNo:1416; 4056346.06 north; 590533.92 east 2021-02-12T23:10:36Z- LcmTrn::handleMessages() - handling 43 messages took 0.63 ms and 131 calls... 2021-02-12T23:10:36Z- waiting for 1613171434.08, time is 1613171435.95 2021-02-12T23:10:36Z- LcmTrn::updateTrn() >>>> pose time 1613171435.95, meas time 1613171435.16 <<<< 2021-02-12T23:10:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171435.16 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:10:36Z- TerrainNav::motionUpdate() - currEstimate = (4056346, 590533.92, 21.12) 2021-02-12T23:10:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:10:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:10:36Z- TNavBF::Projecting Measurements 2021-02-12T23:10:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:10:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:10:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:10:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:10:36Z- TNavFilter::4 good beams. 2021-02-12T23:10:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:10:36Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:10:36Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:10:36Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:10:36Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:10:36Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:10:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:10:36Z- TNavBF::mmse selected from filter -1 2021-02-12T23:10:36Z- TerrainMapOctree: (vehN, vehE ) = (4056345.50, 590533.73). 2021-02-12T23:10:36Z- TerrainMapOctree: Min Distance = 6153.44. 2021-02-12T23:10:36Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:10:36Z- There is no specific good beam that all particles have in common. 2021-02-12T23:10:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:10:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 7.90 2021-02-12T23:10:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:10:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 13.04 2021-02-12T23:10:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:10:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 6.75 2021-02-12T23:10:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:10:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.73 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 14.98 2021-02-12T23:10:36Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:10:36Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:10:36Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:10:36Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:10:36Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:10:36Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:10:36Z- Filter 0 No beams used in subcloud update 2021-02-12T23:10:36Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:10:36Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:10:36Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 800.3 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:10:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171435.16 sec. 2021-02-12T23:10:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:10:36Z- TNavBF::mmse selected from filter -1 2021-02-12T23:10:36Z- variances([0], [2], and psi_berg) = 800.228581 833.103363 0.000000 2021-02-12T23:10:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:10:36Z- LcmTrn::publishEstimates() - MLE : 4056316.56 590487.42 21.12 2021-02-12T23:10:36Z- LcmTrn::publishEstimates() - MMSE : 4056346.06 590533.92 21.12 2021-02-12T23:10:36Z- LcmTrn::publishEstimates() - COVAR: 800.23 833.10 0.73 0.00 2021-02-12T23:10:36Z- AHRS_M2 msg: 1613171436.34 epoch sec; seqNo:1407 2021-02-12T23:10:36Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-12T23:10:36Z- handleDepth()-Depth_Keller msg: 1613171436353 epoch sec; seqNo:1417; depth 21.24 2021-02-12T23:10:36Z- handleDvl() - RDI_Pathfinder msg: 1613171436.36 epoch sec; seqNo:594 2021-02-12T23:10:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 92.51 , 64.57 , 96.83 2021-02-12T23:10:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , 0.02 2021-02-12T23:10:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171436.37 epoch sec; seqNo:1417; 4056346.44 north; 590533.92 east 2021-02-12T23:10:36Z- AHRS_M2 msg: 1613171436.74 epoch sec; seqNo:1408 2021-02-12T23:10:36Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-12T23:10:36Z- handleDepth()-Depth_Keller msg: 1613171436745 epoch sec; seqNo:1418; depth 21.39 2021-02-12T23:10:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171436.76 epoch sec; seqNo:1418; 4056346.45 north; 590535.14 east 2021-02-12T23:10:37Z- AHRS_M2 msg: 1613171437.15 epoch sec; seqNo:1409 2021-02-12T23:10:37Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-12T23:10:37Z- handleDepth()-Depth_Keller msg: 1613171437153 epoch sec; seqNo:1419; depth 21.53 2021-02-12T23:10:37Z- handleDvl() - RDI_Pathfinder msg: 1613171437.17 epoch sec; seqNo:595 2021-02-12T23:10:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 92.44 , 63.75 , 103.26 2021-02-12T23:10:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , -0.01 2021-02-12T23:10:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171437.18 epoch sec; seqNo:1419; 4056346.83 north; 590535.14 east 2021-02-12T23:10:37Z- AHRS_M2 msg: 1613171437.55 epoch sec; seqNo:1410 2021-02-12T23:10:37Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.19 psi 2021-02-12T23:10:37Z- handleDepth()-Depth_Keller msg: 1613171437552 epoch sec; seqNo:1420; depth 21.68 2021-02-12T23:10:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171437.56 epoch sec; seqNo:1420; 4056347.21 north; 590535.13 east 2021-02-12T23:10:37Z- AHRS_M2 msg: 1613171437.95 epoch sec; seqNo:1411 2021-02-12T23:10:37Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.19 psi 2021-02-12T23:10:37Z- handleDepth()-Depth_Keller msg: 1613171437962 epoch sec; seqNo:1421; depth 21.81 2021-02-12T23:10:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171437.97 epoch sec; seqNo:1421; 4056347.59 north; 590535.13 east 2021-02-12T23:10:38Z- AHRS_M2 msg: 1613171438.36 epoch sec; seqNo:1412 2021-02-12T23:10:38Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.19 psi 2021-02-12T23:10:38Z- handleDepth()-Depth_Keller msg: 1613171438360 epoch sec; seqNo:1422; depth 21.94 2021-02-12T23:10:38Z- handleDvl() - RDI_Pathfinder msg: 1613171438.37 epoch sec; seqNo:596 2021-02-12T23:10:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 91.29 , 62.93 , 96.70 2021-02-12T23:10:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , -0.07 2021-02-12T23:10:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171438.39 epoch sec; seqNo:1422; 4056347.59 north; 590535.13 east 2021-02-12T23:10:38Z- AHRS_M2 msg: 1613171438.76 epoch sec; seqNo:1413 2021-02-12T23:10:38Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.19 psi 2021-02-12T23:10:38Z- handleDepth()-Depth_Keller msg: 1613171438763 epoch sec; seqNo:1423; depth 22.08 2021-02-12T23:10:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171438.77 epoch sec; seqNo:1423; 4056347.96 north; 590535.12 east 2021-02-12T23:10:39Z- AHRS_M2 msg: 1613171439.16 epoch sec; seqNo:1414 2021-02-12T23:10:39Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.19 psi 2021-02-12T23:10:39Z- handleDepth()-Depth_Keller msg: 1613171439170 epoch sec; seqNo:1424; depth 22.20 2021-02-12T23:10:39Z- handleDvl() - RDI_Pathfinder msg: 1613171439.18 epoch sec; seqNo:597 2021-02-12T23:10:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 89.33 , 63.34 , 96.90 2021-02-12T23:10:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.03 , -0.09 2021-02-12T23:10:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171439.19 epoch sec; seqNo:1424; 4056348.36 north; 590536.34 east 2021-02-12T23:10:39Z- AHRS_M2 msg: 1613171439.57 epoch sec; seqNo:1415 2021-02-12T23:10:39Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.19 psi 2021-02-12T23:10:39Z- handleDepth()-Depth_Keller msg: 1613171439572 epoch sec; seqNo:1425; depth 22.34 2021-02-12T23:10:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171439.59 epoch sec; seqNo:1425; 4056348.74 north; 590536.34 east 2021-02-12T23:10:39Z- AHRS_M2 msg: 1613171439.97 epoch sec; seqNo:1416 2021-02-12T23:10:39Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.19 psi 2021-02-12T23:10:39Z- handleDepth()-Depth_Keller msg: 1613171439976 epoch sec; seqNo:1426; depth 22.48 2021-02-12T23:10:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171439.99 epoch sec; seqNo:1426; 4056349.11 north; 590536.33 east 2021-02-12T23:10:40Z- AHRS_M2 msg: 1613171440.38 epoch sec; seqNo:1417 2021-02-12T23:10:40Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.19 psi 2021-02-12T23:10:40Z- handleDepth()-Depth_Keller msg: 1613171440398 epoch sec; seqNo:1427; depth 22.60 2021-02-12T23:10:40Z- handleDvl() - RDI_Pathfinder msg: 1613171440.44 epoch sec; seqNo:598 2021-02-12T23:10:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 90.41 , 62.52 , 96.90 2021-02-12T23:10:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.05 , -0.09 2021-02-12T23:10:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171440.45 epoch sec; seqNo:1427; 4056349.49 north; 590536.33 east 2021-02-12T23:10:40Z- AHRS_M2 msg: 1613171440.78 epoch sec; seqNo:1418 2021-02-12T23:10:40Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-12T23:10:40Z- handleDepth()-Depth_Keller msg: 1613171440794 epoch sec; seqNo:1428; depth 22.77 2021-02-12T23:10:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171440.81 epoch sec; seqNo:1428; 4056349.49 north; 590536.33 east 2021-02-12T23:10:41Z- AHRS_M2 msg: 1613171441.18 epoch sec; seqNo:1419 2021-02-12T23:10:41Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-12T23:10:41Z- handleDepth()-Depth_Keller msg: 1613171441193 epoch sec; seqNo:1429; depth 22.89 2021-02-12T23:10:41Z- handleDvl() - RDI_Pathfinder msg: 1613171441.20 epoch sec; seqNo:599 2021-02-12T23:10:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 89.33 , 62.52 , 96.90 2021-02-12T23:10:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.08 , -0.05 2021-02-12T23:10:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171441.22 epoch sec; seqNo:1429; 4056349.87 north; 590536.32 east 2021-02-12T23:10:41Z- LcmTrn::handleMessages() - handling 45 messages took 0.74 ms and 131 calls... 2021-02-12T23:10:41Z- waiting for 1613171438.95, time is 1613171441.22 2021-02-12T23:10:41Z- LcmTrn::updateTrn() equating pose 1613171441.22 and meas times 1613171441.22 2021-02-12T23:10:41Z- LcmTrn::updateTrn() >>>> pose time 1613171441.22, meas time 1613171441.22 <<<< 2021-02-12T23:10:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171441.22 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:10:41Z- TerrainNav::motionUpdate() - currEstimate = (4056350, 590536.32, 22.89) 2021-02-12T23:10:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:10:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:10:41Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171441.22 sec.; No relevant navigation data available 2021-02-12T23:10:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:10:41Z- TNavBF::mmse selected from filter -1 2021-02-12T23:10:41Z- variances([0], [2], and psi_berg) = 799.987890 833.102838 0.000000 2021-02-12T23:10:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:10:41Z- LcmTrn::publishEstimates() - MLE : 4056321.37 590487.82 22.89 2021-02-12T23:10:41Z- LcmTrn::publishEstimates() - MMSE : 4056349.87 590536.32 22.89 2021-02-12T23:10:41Z- LcmTrn::publishEstimates() - COVAR: 799.99 833.10 0.96 0.00 2021-02-12T23:10:41Z- AHRS_M2 msg: 1613171441.59 epoch sec; seqNo:1420 2021-02-12T23:10:41Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.18 psi 2021-02-12T23:10:41Z- handleDepth()-Depth_Keller msg: 1613171441597 epoch sec; seqNo:1430; depth 23.03 2021-02-12T23:10:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171441.61 epoch sec; seqNo:1430; 4056350.26 north; 590537.54 east 2021-02-12T23:10:42Z- AHRS_M2 msg: 1613171442.00 epoch sec; seqNo:1421 2021-02-12T23:10:42Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.18 psi 2021-02-12T23:10:42Z- handleDepth()-Depth_Keller msg: 1613171442006 epoch sec; seqNo:1431; depth 23.15 2021-02-12T23:10:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171442.04 epoch sec; seqNo:1431; 4056350.64 north; 590537.54 east 2021-02-12T23:10:42Z- AHRS_M2 msg: 1613171442.39 epoch sec; seqNo:1422 2021-02-12T23:10:42Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.17 psi 2021-02-12T23:10:42Z- handleDepth()-Depth_Keller msg: 1613171442410 epoch sec; seqNo:1432; depth 23.30 2021-02-12T23:10:42Z- handleDvl() - RDI_Pathfinder msg: 1613171442.42 epoch sec; seqNo:600 2021-02-12T23:10:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 88.25 , 61.71 , 96.90 2021-02-12T23:10:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , -0.01 2021-02-12T23:10:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171442.43 epoch sec; seqNo:1432; 4056351.02 north; 590537.53 east 2021-02-12T23:10:42Z- AHRS_M2 msg: 1613171442.80 epoch sec; seqNo:1423 2021-02-12T23:10:42Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.17 psi 2021-02-12T23:10:42Z- handleDepth()-Depth_Keller msg: 1613171442812 epoch sec; seqNo:1433; depth 23.44 2021-02-12T23:10:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171442.86 epoch sec; seqNo:1433; 4056351.02 north; 590537.53 east 2021-02-12T23:10:43Z- AHRS_M2 msg: 1613171443.20 epoch sec; seqNo:1424 2021-02-12T23:10:43Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.16 psi 2021-02-12T23:10:43Z- handleDepth()-Depth_Keller msg: 1613171443208 epoch sec; seqNo:1434; depth 23.59 2021-02-12T23:10:43Z- handleDvl() - RDI_Pathfinder msg: 1613171443.22 epoch sec; seqNo:601 2021-02-12T23:10:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 87.17 , 61.71 , 96.90 2021-02-12T23:10:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , 0.01 2021-02-12T23:10:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171443.23 epoch sec; seqNo:1434; 4056351.40 north; 590537.53 east 2021-02-12T23:10:43Z- AHRS_M2 msg: 1613171443.61 epoch sec; seqNo:1425 2021-02-12T23:10:43Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.15 psi 2021-02-12T23:10:43Z- handleDepth()-Depth_Keller msg: 1613171443613 epoch sec; seqNo:1435; depth 23.72 2021-02-12T23:10:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171443.62 epoch sec; seqNo:1435; 4056351.78 north; 590537.53 east 2021-02-12T23:10:44Z- AHRS_M2 msg: 1613171444.04 epoch sec; seqNo:1426 2021-02-12T23:10:44Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.15 psi 2021-02-12T23:10:44Z- handleDepth()-Depth_Keller msg: 1613171444047 epoch sec; seqNo:1436; depth 23.85 2021-02-12T23:10:44Z- handleDvl() - RDI_Pathfinder msg: 1613171444.06 epoch sec; seqNo:602 2021-02-12T23:10:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 85.00 , 61.30 , 96.90 2021-02-12T23:10:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.03 2021-02-12T23:10:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171444.07 epoch sec; seqNo:1436; 4056352.16 north; 590537.52 east 2021-02-12T23:10:44Z- AHRS_M2 msg: 1613171444.41 epoch sec; seqNo:1427 2021-02-12T23:10:44Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.15 psi 2021-02-12T23:10:44Z- handleDepth()-Depth_Keller msg: 1613171444425 epoch sec; seqNo:1437; depth 23.99 2021-02-12T23:10:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171444.44 epoch sec; seqNo:1437; 4056352.17 north; 590538.74 east 2021-02-12T23:10:44Z- AHRS_M2 msg: 1613171444.82 epoch sec; seqNo:1428 2021-02-12T23:10:44Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.15 psi 2021-02-12T23:10:44Z- handleDepth()-Depth_Keller msg: 1613171444824 epoch sec; seqNo:1438; depth 24.13 2021-02-12T23:10:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171444.83 epoch sec; seqNo:1438; 4056352.55 north; 590538.74 east 2021-02-12T23:10:45Z- AHRS_M2 msg: 1613171445.22 epoch sec; seqNo:1429 2021-02-12T23:10:45Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.15 psi 2021-02-12T23:10:45Z- handleDepth()-Depth_Keller msg: 1613171445237 epoch sec; seqNo:1439; depth 24.25 2021-02-12T23:10:45Z- handleDvl() - RDI_Pathfinder msg: 1613171445.25 epoch sec; seqNo:603 2021-02-12T23:10:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 85.00 , 60.48 , 101.23 2021-02-12T23:10:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.04 2021-02-12T23:10:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171445.26 epoch sec; seqNo:1439; 4056352.55 north; 590538.74 east 2021-02-12T23:10:45Z- AHRS_M2 msg: 1613171445.63 epoch sec; seqNo:1430 2021-02-12T23:10:45Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.15 psi 2021-02-12T23:10:45Z- handleDepth()-Depth_Keller msg: 1613171445632 epoch sec; seqNo:1440; depth 24.41 2021-02-12T23:10:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171445.65 epoch sec; seqNo:1440; 4056352.93 north; 590538.73 east 2021-02-12T23:10:46Z- AHRS_M2 msg: 1613171446.05 epoch sec; seqNo:1431 2021-02-12T23:10:46Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.16 psi 2021-02-12T23:10:46Z- handleDepth()-Depth_Keller msg: 1613171446055 epoch sec; seqNo:1441; depth 24.56 2021-02-12T23:10:46Z- handleDvl() - RDI_Pathfinder msg: 1613171446.08 epoch sec; seqNo:604 2021-02-12T23:10:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 85.00 , 60.89 , 96.90 2021-02-12T23:10:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.05 , 0.04 2021-02-12T23:10:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171446.09 epoch sec; seqNo:1441; 4056353.31 north; 590538.73 east 2021-02-12T23:10:46Z- LcmTrn::handleMessages() - handling 41 messages took 0.70 ms and 127 calls... 2021-02-12T23:10:46Z- waiting for 1613171444.22, time is 1613171446.09 2021-02-12T23:10:46Z- LcmTrn::updateTrn() equating pose 1613171446.09 and meas times 1613171446.09 2021-02-12T23:10:46Z- LcmTrn::updateTrn() >>>> pose time 1613171446.09, meas time 1613171446.09 <<<< 2021-02-12T23:10:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171446.09 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:10:46Z- TerrainNav::motionUpdate() - currEstimate = (4056353, 590538.73, 24.56) 2021-02-12T23:10:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:10:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:10:46Z- TNavBF::Projecting Measurements 2021-02-12T23:10:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:10:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:10:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:10:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:10:46Z- TNavFilter::4 good beams. 2021-02-12T23:10:46Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:10:46Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:10:46Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:10:46Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:10:46Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:10:46Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:10:46Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:10:46Z- TNavBF::mmse selected from filter -1 2021-02-12T23:10:46Z- TerrainMapOctree: (vehN, vehE ) = (4056353.31, 590538.73). 2021-02-12T23:10:46Z- TerrainMapOctree: Min Distance = 6144.19. 2021-02-12T23:10:46Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:10:46Z- There is no specific good beam that all particles have in common. 2021-02-12T23:10:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:10:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 7.54 2021-02-12T23:10:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:10:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 11.56 2021-02-12T23:10:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:10:46Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 5.93 2021-02-12T23:10:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:10:46Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 15.02 2021-02-12T23:10:46Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:10:46Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:10:46Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:10:46Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:10:46Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:10:46Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:10:46Z- Filter 0 No beams used in subcloud update 2021-02-12T23:10:46Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:10:46Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:10:46Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 799.8 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:10:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171446.09 sec. 2021-02-12T23:10:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:10:46Z- TNavBF::mmse selected from filter -1 2021-02-12T23:10:46Z- variances([0], [2], and psi_berg) = 799.756970 833.102337 0.000000 2021-02-12T23:10:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:10:46Z- LcmTrn::publishEstimates() - MLE : 4056324.81 590490.23 24.56 2021-02-12T23:10:46Z- LcmTrn::publishEstimates() - MMSE : 4056353.31 590538.73 24.56 2021-02-12T23:10:46Z- LcmTrn::publishEstimates() - COVAR: 799.76 833.10 0.82 0.00 2021-02-12T23:10:46Z- AHRS_M2 msg: 1613171446.43 epoch sec; seqNo:1432 2021-02-12T23:10:46Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.16 psi 2021-02-12T23:10:46Z- handleDepth()-Depth_Keller msg: 1613171446440 epoch sec; seqNo:1442; depth 24.70 2021-02-12T23:10:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171446.45 epoch sec; seqNo:1442; 4056353.31 north; 590538.73 east 2021-02-12T23:10:46Z- AHRS_M2 msg: 1613171446.84 epoch sec; seqNo:1433 2021-02-12T23:10:46Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.16 psi 2021-02-12T23:10:46Z- handleDepth()-Depth_Keller msg: 1613171446845 epoch sec; seqNo:1443; depth 24.81 2021-02-12T23:10:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171446.85 epoch sec; seqNo:1443; 4056353.70 north; 590539.95 east 2021-02-12T23:10:47Z- AHRS_M2 msg: 1613171447.24 epoch sec; seqNo:1434 2021-02-12T23:10:47Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.16 psi 2021-02-12T23:10:47Z- handleDepth()-Depth_Keller msg: 1613171447248 epoch sec; seqNo:1444; depth 24.95 2021-02-12T23:10:47Z- handleDvl() - RDI_Pathfinder msg: 1613171447.27 epoch sec; seqNo:605 2021-02-12T23:10:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 85.00 , 60.48 , 100.15 2021-02-12T23:10:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , 0.01 2021-02-12T23:10:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171447.28 epoch sec; seqNo:1444; 4056354.08 north; 590539.94 east 2021-02-12T23:10:47Z- AHRS_M2 msg: 1613171447.65 epoch sec; seqNo:1435 2021-02-12T23:10:47Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.17 psi 2021-02-12T23:10:47Z- handleDepth()-Depth_Keller msg: 1613171447656 epoch sec; seqNo:1445; depth 25.09 2021-02-12T23:10:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171447.67 epoch sec; seqNo:1445; 4056354.08 north; 590539.94 east 2021-02-12T23:10:48Z- AHRS_M2 msg: 1613171448.05 epoch sec; seqNo:1436 2021-02-12T23:10:48Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.17 psi 2021-02-12T23:10:48Z- handleDepth()-Depth_Keller msg: 1613171448061 epoch sec; seqNo:1446; depth 25.23 2021-02-12T23:10:48Z- handleDvl() - RDI_Pathfinder msg: 1613171448.07 epoch sec; seqNo:606 2021-02-12T23:10:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 85.00 , 60.48 , 96.90 2021-02-12T23:10:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , -0.03 2021-02-12T23:10:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171448.08 epoch sec; seqNo:1446; 4056354.46 north; 590539.94 east 2021-02-12T23:10:48Z- AHRS_M2 msg: 1613171448.46 epoch sec; seqNo:1437 2021-02-12T23:10:48Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.18 psi 2021-02-12T23:10:48Z- handleDepth()-Depth_Keller msg: 1613171448462 epoch sec; seqNo:1447; depth 25.37 2021-02-12T23:10:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171448.48 epoch sec; seqNo:1447; 4056354.84 north; 590539.94 east 2021-02-12T23:10:48Z- AHRS_M2 msg: 1613171448.86 epoch sec; seqNo:1438 2021-02-12T23:10:48Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.18 psi 2021-02-12T23:10:48Z- handleDepth()-Depth_Keller msg: 1613171448869 epoch sec; seqNo:1448; depth 25.50 2021-02-12T23:10:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:10:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:10:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:10:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:10:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:10:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:10:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:10:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:10:48Z- handleDvl() - RDI_Pathfinder msg: 1613171448.87 epoch sec; seqNo:607 2021-02-12T23:10:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:10:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , -0.03 2021-02-12T23:10:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171448.88 epoch sec; seqNo:1448; 4056354.85 north; 590541.16 east 2021-02-12T23:10:49Z- AHRS_M2 msg: 1613171449.26 epoch sec; seqNo:1439 2021-02-12T23:10:49Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.19 psi 2021-02-12T23:10:49Z- handleDepth()-Depth_Keller msg: 1613171449273 epoch sec; seqNo:1449; depth 25.64 2021-02-12T23:10:49Z- handleDvl() - RDI_Pathfinder msg: 1613171449.28 epoch sec; seqNo:608 2021-02-12T23:10:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.02 , 86.09 , 60.07 , 96.90 2021-02-12T23:10:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , -0.07 2021-02-12T23:10:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171449.30 epoch sec; seqNo:1449; 4056355.23 north; 590541.15 east 2021-02-12T23:10:49Z- AHRS_M2 msg: 1613171449.67 epoch sec; seqNo:1440 2021-02-12T23:10:49Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.19 psi 2021-02-12T23:10:49Z- handleDepth()-Depth_Keller msg: 1613171449674 epoch sec; seqNo:1450; depth 25.80 2021-02-12T23:10:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171449.68 epoch sec; seqNo:1450; 4056355.61 north; 590541.15 east 2021-02-12T23:10:50Z- AHRS_M2 msg: 1613171450.07 epoch sec; seqNo:1441 2021-02-12T23:10:50Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.20 psi 2021-02-12T23:10:50Z- handleDepth()-Depth_Keller msg: 1613171450081 epoch sec; seqNo:1451; depth 25.92 2021-02-12T23:10:50Z- handleDvl() - RDI_Pathfinder msg: 1613171450.10 epoch sec; seqNo:609 2021-02-12T23:10:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.79 , 87.17 , 58.85 , 96.90 2021-02-12T23:10:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , -0.11 2021-02-12T23:10:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171450.11 epoch sec; seqNo:1451; 4056355.61 north; 590541.15 east 2021-02-12T23:10:50Z- AHRS_M2 msg: 1613171450.48 epoch sec; seqNo:1442 2021-02-12T23:10:50Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.20 psi 2021-02-12T23:10:50Z- handleDepth()-Depth_Keller msg: 1613171450489 epoch sec; seqNo:1452; depth 26.06 2021-02-12T23:10:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171450.50 epoch sec; seqNo:1452; 4056355.98 north; 590541.14 east 2021-02-12T23:10:50Z- AHRS_M2 msg: 1613171450.88 epoch sec; seqNo:1443 2021-02-12T23:10:50Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.20 psi 2021-02-12T23:10:50Z- handleDepth()-Depth_Keller msg: 1613171450886 epoch sec; seqNo:1453; depth 26.20 2021-02-12T23:10:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171450.89 epoch sec; seqNo:1453; 4056356.36 north; 590541.14 east 2021-02-12T23:10:51Z- AHRS_M2 msg: 1613171451.28 epoch sec; seqNo:1444 2021-02-12T23:10:51Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.21 psi 2021-02-12T23:10:51Z- handleDepth()-Depth_Keller msg: 1613171451288 epoch sec; seqNo:1454; depth 26.32 2021-02-12T23:10:51Z- handleDvl() - RDI_Pathfinder msg: 1613171451.30 epoch sec; seqNo:610 2021-02-12T23:10:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.38 , 87.17 , 59.25 , 94.74 2021-02-12T23:10:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , -0.10 2021-02-12T23:10:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171451.32 epoch sec; seqNo:1454; 4056356.76 north; 590542.36 east 2021-02-12T23:10:51Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-12T23:10:51Z- waiting for 1613171449.09, time is 1613171451.32 2021-02-12T23:10:51Z- LcmTrn::updateTrn() equating pose 1613171451.32 and meas times 1613171451.32 2021-02-12T23:10:51Z- LcmTrn::updateTrn() >>>> pose time 1613171451.32, meas time 1613171451.32 <<<< 2021-02-12T23:10:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171451.32 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:10:51Z- TerrainNav::motionUpdate() - currEstimate = (4056357, 590542.36, 26.32) 2021-02-12T23:10:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:10:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:10:51Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171451.32 sec.; No relevant navigation data available 2021-02-12T23:10:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:10:51Z- TNavBF::mmse selected from filter -1 2021-02-12T23:10:51Z- variances([0], [2], and psi_berg) = 799.506021 833.101796 0.000000 2021-02-12T23:10:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:10:51Z- LcmTrn::publishEstimates() - MLE : 4056328.26 590493.86 26.32 2021-02-12T23:10:51Z- LcmTrn::publishEstimates() - MMSE : 4056356.76 590542.36 26.32 2021-02-12T23:10:51Z- LcmTrn::publishEstimates() - COVAR: 799.51 833.10 1.07 0.00 2021-02-12T23:10:51Z- AHRS_M2 msg: 1613171451.69 epoch sec; seqNo:1445 2021-02-12T23:10:51Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.21 psi 2021-02-12T23:10:51Z- handleDepth()-Depth_Keller msg: 1613171451697 epoch sec; seqNo:1455; depth 26.47 2021-02-12T23:10:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171451.71 epoch sec; seqNo:1455; 4056356.76 north; 590542.36 east 2021-02-12T23:10:52Z- AHRS_M2 msg: 1613171452.09 epoch sec; seqNo:1446 2021-02-12T23:10:52Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.21 psi 2021-02-12T23:10:52Z- handleDepth()-Depth_Keller msg: 1613171452097 epoch sec; seqNo:1456; depth 26.59 2021-02-12T23:10:52Z- handleDvl() - RDI_Pathfinder msg: 1613171452.11 epoch sec; seqNo:611 2021-02-12T23:10:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.97 , 85.00 , 58.85 , 95.82 2021-02-12T23:10:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , -0.06 2021-02-12T23:10:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171452.13 epoch sec; seqNo:1456; 4056357.13 north; 590542.35 east 2021-02-12T23:10:52Z- AHRS_M2 msg: 1613171452.50 epoch sec; seqNo:1447 2021-02-12T23:10:52Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.21 psi 2021-02-12T23:10:52Z- handleDepth()-Depth_Keller msg: 1613171452520 epoch sec; seqNo:1457; depth 26.74 2021-02-12T23:10:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:10:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:10:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:10:52Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:10:52Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:10:52Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:10:52Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:10:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:10:52Z- handleDvl() - RDI_Pathfinder msg: 1613171452.52 epoch sec; seqNo:612 2021-02-12T23:10:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:10:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , -0.06 2021-02-12T23:10:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171452.53 epoch sec; seqNo:1457; 4056357.51 north; 590542.35 east 2021-02-12T23:10:52Z- AHRS_M2 msg: 1613171452.90 epoch sec; seqNo:1448 2021-02-12T23:10:52Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.21 psi 2021-02-12T23:10:52Z- handleDepth()-Depth_Keller msg: 1613171452904 epoch sec; seqNo:1458; depth 26.88 2021-02-12T23:10:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171452.92 epoch sec; seqNo:1458; 4056357.51 north; 590542.35 east 2021-02-12T23:10:53Z- AHRS_M2 msg: 1613171453.31 epoch sec; seqNo:1449 2021-02-12T23:10:53Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.20 psi 2021-02-12T23:10:53Z- handleDepth()-Depth_Keller msg: 1613171453314 epoch sec; seqNo:1459; depth 27.00 2021-02-12T23:10:53Z- handleDvl() - RDI_Pathfinder msg: 1613171453.33 epoch sec; seqNo:613 2021-02-12T23:10:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.57 , 85.00 , 58.85 , 96.90 2021-02-12T23:10:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , -0.01 2021-02-12T23:10:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171453.34 epoch sec; seqNo:1459; 4056357.89 north; 590542.35 east 2021-02-12T23:10:53Z- AHRS_M2 msg: 1613171453.71 epoch sec; seqNo:1450 2021-02-12T23:10:53Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.20 psi 2021-02-12T23:10:53Z- handleDepth()-Depth_Keller msg: 1613171453718 epoch sec; seqNo:1460; depth 27.14 2021-02-12T23:10:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171453.73 epoch sec; seqNo:1460; 4056358.28 north; 590543.56 east 2021-02-12T23:10:54Z- AHRS_M2 msg: 1613171454.12 epoch sec; seqNo:1451 2021-02-12T23:10:54Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.20 psi 2021-02-12T23:10:54Z- handleDepth()-Depth_Keller msg: 1613171454128 epoch sec; seqNo:1461; depth 27.29 2021-02-12T23:10:54Z- handleDvl() - RDI_Pathfinder msg: 1613171454.15 epoch sec; seqNo:614 2021-02-12T23:10:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.16 , 85.00 , 58.44 , 97.98 2021-02-12T23:10:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.03 2021-02-12T23:10:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171454.16 epoch sec; seqNo:1461; 4056358.66 north; 590543.56 east 2021-02-12T23:10:54Z- AHRS_M2 msg: 1613171454.52 epoch sec; seqNo:1452 2021-02-12T23:10:54Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.20 psi 2021-02-12T23:10:54Z- handleDepth()-Depth_Keller msg: 1613171454520 epoch sec; seqNo:1462; depth 27.42 2021-02-12T23:10:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171454.53 epoch sec; seqNo:1462; 4056358.66 north; 590543.56 east 2021-02-12T23:10:54Z- AHRS_M2 msg: 1613171454.92 epoch sec; seqNo:1453 2021-02-12T23:10:54Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.20 psi 2021-02-12T23:10:54Z- handleDepth()-Depth_Keller msg: 1613171454934 epoch sec; seqNo:1463; depth 27.56 2021-02-12T23:10:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171454.98 epoch sec; seqNo:1463; 4056359.04 north; 590543.55 east 2021-02-12T23:10:55Z- AHRS_M2 msg: 1613171455.32 epoch sec; seqNo:1454 2021-02-12T23:10:55Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.20 psi 2021-02-12T23:10:55Z- handleDepth()-Depth_Keller msg: 1613171455328 epoch sec; seqNo:1464; depth 27.70 2021-02-12T23:10:55Z- handleDvl() - RDI_Pathfinder msg: 1613171455.35 epoch sec; seqNo:615 2021-02-12T23:10:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.34 , 79.28 , 58.03 , 97.98 2021-02-12T23:10:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , 0.05 2021-02-12T23:10:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171455.36 epoch sec; seqNo:1464; 4056359.42 north; 590543.55 east 2021-02-12T23:10:55Z- AHRS_M2 msg: 1613171455.73 epoch sec; seqNo:1455 2021-02-12T23:10:55Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.21 psi 2021-02-12T23:10:55Z- handleDepth()-Depth_Keller msg: 1613171455737 epoch sec; seqNo:1465; depth 27.83 2021-02-12T23:10:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171455.75 epoch sec; seqNo:1465; 4056359.42 north; 590543.55 east 2021-02-12T23:10:56Z- AHRS_M2 msg: 1613171456.14 epoch sec; seqNo:1456 2021-02-12T23:10:56Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.21 psi 2021-02-12T23:10:56Z- handleDepth()-Depth_Keller msg: 1613171456143 epoch sec; seqNo:1466; depth 27.98 2021-02-12T23:10:56Z- handleDvl() - RDI_Pathfinder msg: 1613171456.15 epoch sec; seqNo:616 2021-02-12T23:10:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.93 , 78.87 , 58.03 , 93.66 2021-02-12T23:10:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.06 , 0.04 2021-02-12T23:10:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171456.16 epoch sec; seqNo:1466; 4056359.81 north; 590544.77 east 2021-02-12T23:10:56Z- LcmTrn::handleMessages() - handling 42 messages took 0.68 ms and 129 calls... 2021-02-12T23:10:56Z- waiting for 1613171454.32, time is 1613171456.16 2021-02-12T23:10:56Z- LcmTrn::updateTrn() equating pose 1613171456.16 and meas times 1613171456.16 2021-02-12T23:10:56Z- LcmTrn::updateTrn() >>>> pose time 1613171456.16, meas time 1613171456.16 <<<< 2021-02-12T23:10:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171456.16 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:10:56Z- TerrainNav::motionUpdate() - currEstimate = (4056360, 590544.77, 27.98) 2021-02-12T23:10:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:10:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:10:56Z- TNavBF::Projecting Measurements 2021-02-12T23:10:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:10:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:10:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:10:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:10:56Z- TNavFilter::4 good beams. 2021-02-12T23:10:56Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:10:56Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:10:56Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:10:56Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:10:56Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:10:56Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:10:56Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:10:56Z- TNavBF::mmse selected from filter -1 2021-02-12T23:10:56Z- TerrainMapOctree: (vehN, vehE ) = (4056359.81, 590544.77). 2021-02-12T23:10:56Z- TerrainMapOctree: Min Distance = 6135.36. 2021-02-12T23:10:56Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:10:56Z- There is no specific good beam that all particles have in common. 2021-02-12T23:10:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:10:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.136 ExpectedMeasDiff: nan currMeas.covariance: 6.34 2021-02-12T23:10:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:10:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 9.95 2021-02-12T23:10:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:10:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.123 ExpectedMeasDiff: nan currMeas.covariance: 5.39 2021-02-12T23:10:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:10:56Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 14.03 2021-02-12T23:10:56Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:10:56Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:10:56Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:10:56Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:10:56Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:10:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:10:56Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:10:56Z- Filter 0 No beams used in subcloud update 2021-02-12T23:10:56Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:10:56Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:10:56Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 799.3 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:10:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171456.16 sec. 2021-02-12T23:10:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:10:56Z- TNavBF::mmse selected from filter -1 2021-02-12T23:10:56Z- variances([0], [2], and psi_berg) = 799.285948 833.101325 0.000000 2021-02-12T23:10:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:10:56Z- LcmTrn::publishEstimates() - MLE : 4056331.31 590496.27 27.98 2021-02-12T23:10:56Z- LcmTrn::publishEstimates() - MMSE : 4056359.81 590544.77 27.98 2021-02-12T23:10:56Z- LcmTrn::publishEstimates() - COVAR: 799.29 833.10 0.83 0.00 2021-02-12T23:10:56Z- AHRS_M2 msg: 1613171456.54 epoch sec; seqNo:1457 2021-02-12T23:10:56Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.21 psi 2021-02-12T23:10:56Z- handleDepth()-Depth_Keller msg: 1613171456576 epoch sec; seqNo:1467; depth 28.10 2021-02-12T23:10:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171456.59 epoch sec; seqNo:1467; 4056360.19 north; 590544.76 east 2021-02-12T23:10:56Z- AHRS_M2 msg: 1613171456.96 epoch sec; seqNo:1458 2021-02-12T23:10:56Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.21 psi 2021-02-12T23:10:56Z- handleDepth()-Depth_Keller msg: 1613171456971 epoch sec; seqNo:1468; depth 28.26 2021-02-12T23:10:56Z- handleDvl() - RDI_Pathfinder msg: 1613171456.98 epoch sec; seqNo:617 2021-02-12T23:10:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.52 , 79.28 , 58.03 , 93.66 2021-02-12T23:10:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.01 2021-02-12T23:10:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171456.99 epoch sec; seqNo:1468; 4056360.19 north; 590544.76 east 2021-02-12T23:10:57Z- AHRS_M2 msg: 1613171457.35 epoch sec; seqNo:1459 2021-02-12T23:10:57Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.21 psi 2021-02-12T23:10:57Z- handleDepth()-Depth_Keller msg: 1613171457351 epoch sec; seqNo:1469; depth 28.40 2021-02-12T23:10:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171457.36 epoch sec; seqNo:1469; 4056360.57 north; 590544.76 east 2021-02-12T23:10:57Z- AHRS_M2 msg: 1613171457.75 epoch sec; seqNo:1460 2021-02-12T23:10:57Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.21 psi 2021-02-12T23:10:57Z- handleDepth()-Depth_Keller msg: 1613171457753 epoch sec; seqNo:1470; depth 28.53 2021-02-12T23:10:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171457.76 epoch sec; seqNo:1470; 4056360.95 north; 590544.76 east 2021-02-12T23:10:58Z- AHRS_M2 msg: 1613171458.15 epoch sec; seqNo:1461 2021-02-12T23:10:58Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.21 psi 2021-02-12T23:10:58Z- handleDepth()-Depth_Keller msg: 1613171458158 epoch sec; seqNo:1471; depth 28.65 2021-02-12T23:10:58Z- handleDvl() - RDI_Pathfinder msg: 1613171458.17 epoch sec; seqNo:618 2021-02-12T23:10:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.71 , 79.28 , 57.62 , 93.66 2021-02-12T23:10:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , -0.03 2021-02-12T23:10:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171458.18 epoch sec; seqNo:1471; 4056361.33 north; 590544.75 east 2021-02-12T23:10:58Z- AHRS_M2 msg: 1613171458.56 epoch sec; seqNo:1462 2021-02-12T23:10:58Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.21 psi 2021-02-12T23:10:58Z- handleDepth()-Depth_Keller msg: 1613171458573 epoch sec; seqNo:1472; depth 28.78 2021-02-12T23:10:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171458.58 epoch sec; seqNo:1472; 4056361.34 north; 590545.97 east 2021-02-12T23:10:58Z- AHRS_M2 msg: 1613171458.99 epoch sec; seqNo:1463 2021-02-12T23:10:58Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.21 psi 2021-02-12T23:10:58Z- handleDepth()-Depth_Keller msg: 1613171458995 epoch sec; seqNo:1473; depth 28.93 2021-02-12T23:10:59Z- handleDvl() - RDI_Pathfinder msg: 1613171459.02 epoch sec; seqNo:619 2021-02-12T23:10:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.30 , 78.05 , 57.62 , 90.41 2021-02-12T23:10:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , -0.05 2021-02-12T23:10:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171459.04 epoch sec; seqNo:1473; 4056361.72 north; 590545.97 east 2021-02-12T23:10:59Z- AHRS_M2 msg: 1613171459.36 epoch sec; seqNo:1464 2021-02-12T23:10:59Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.21 psi 2021-02-12T23:10:59Z- handleDepth()-Depth_Keller msg: 1613171459368 epoch sec; seqNo:1474; depth 29.06 2021-02-12T23:10:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171459.39 epoch sec; seqNo:1474; 4056362.10 north; 590545.96 east 2021-02-12T23:10:59Z- AHRS_M2 msg: 1613171459.77 epoch sec; seqNo:1465 2021-02-12T23:10:59Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-12T23:10:59Z- handleDepth()-Depth_Keller msg: 1613171459772 epoch sec; seqNo:1475; depth 29.17 2021-02-12T23:10:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171459.78 epoch sec; seqNo:1475; 4056362.10 north; 590545.96 east 2021-02-12T23:11:00Z- AHRS_M2 msg: 1613171460.17 epoch sec; seqNo:1466 2021-02-12T23:11:00Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.20 psi 2021-02-12T23:11:00Z- handleDepth()-Depth_Keller msg: 1613171460177 epoch sec; seqNo:1476; depth 29.31 2021-02-12T23:11:00Z- handleDvl() - RDI_Pathfinder msg: 1613171460.19 epoch sec; seqNo:620 2021-02-12T23:11:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.48 , 77.24 , 57.21 , 89.33 2021-02-12T23:11:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , -0.04 2021-02-12T23:11:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171460.20 epoch sec; seqNo:1476; 4056362.48 north; 590545.96 east 2021-02-12T23:11:00Z- AHRS_M2 msg: 1613171460.58 epoch sec; seqNo:1467 2021-02-12T23:11:00Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.20 psi 2021-02-12T23:11:00Z- handleDepth()-Depth_Keller msg: 1613171460582 epoch sec; seqNo:1477; depth 29.45 2021-02-12T23:11:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171460.59 epoch sec; seqNo:1477; 4056362.86 north; 590545.96 east 2021-02-12T23:11:00Z- AHRS_M2 msg: 1613171460.98 epoch sec; seqNo:1468 2021-02-12T23:11:00Z- AHRS_M2 msg: 0.59 phi; -0.24 theta; -0.19 psi 2021-02-12T23:11:00Z- handleDepth()-Depth_Keller msg: 1613171460989 epoch sec; seqNo:1478; depth 29.59 2021-02-12T23:11:01Z- handleDvl() - RDI_Pathfinder msg: 1613171461.01 epoch sec; seqNo:621 2021-02-12T23:11:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 58.85 , 74.78 , 57.21 , 83.92 2021-02-12T23:11:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.02 2021-02-12T23:11:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171461.02 epoch sec; seqNo:1478; 4056363.25 north; 590547.17 east 2021-02-12T23:11:01Z- AHRS_M2 msg: 1613171461.38 epoch sec; seqNo:1469 2021-02-12T23:11:01Z- AHRS_M2 msg: 0.58 phi; -0.20 theta; -0.17 psi 2021-02-12T23:11:01Z- handleDepth()-Depth_Keller msg: 1613171461388 epoch sec; seqNo:1479; depth 29.74 2021-02-12T23:11:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171461.40 epoch sec; seqNo:1479; 4056363.25 north; 590547.17 east 2021-02-12T23:11:01Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-12T23:11:01Z- waiting for 1613171459.16, time is 1613171461.40 2021-02-12T23:11:01Z- LcmTrn::updateTrn() >>>> pose time 1613171461.40, meas time 1613171461.01 <<<< 2021-02-12T23:11:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171461.01 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:11:01Z- TerrainNav::motionUpdate() - currEstimate = (4056363, 590547.17, 29.74) 2021-02-12T23:11:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:11:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:11:01Z- TNavBF::Projecting Measurements 2021-02-12T23:11:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:11:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:11:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:11:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:11:01Z- TNavFilter::4 good beams. 2021-02-12T23:11:01Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:11:01Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:11:01Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:11:01Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:11:01Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:11:01Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:11:01Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:11:01Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:01Z- TerrainMapOctree: (vehN, vehE ) = (4056362.99, 590546.99). 2021-02-12T23:11:01Z- TerrainMapOctree: Min Distance = 6131.49. 2021-02-12T23:11:01Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:11:01Z- There is no specific good beam that all particles have in common. 2021-02-12T23:11:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:11:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.124 ExpectedMeasDiff: nan currMeas.covariance: 5.54 2021-02-12T23:11:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:11:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 8.95 2021-02-12T23:11:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:11:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 5.24 2021-02-12T23:11:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:11:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.74 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 11.27 2021-02-12T23:11:01Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:11:01Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:11:01Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:11:01Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:11:01Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:11:01Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:11:01Z- Filter 0 No beams used in subcloud update 2021-02-12T23:11:01Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:11:01Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:11:01Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 799.1 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:11:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171461.01 sec. 2021-02-12T23:11:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:11:01Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:01Z- variances([0], [2], and psi_berg) = 799.005391 833.100728 0.000000 2021-02-12T23:11:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:11:01Z- LcmTrn::publishEstimates() - MLE : 4056334.75 590498.67 29.74 2021-02-12T23:11:01Z- LcmTrn::publishEstimates() - MMSE : 4056363.25 590547.17 29.74 2021-02-12T23:11:01Z- LcmTrn::publishEstimates() - COVAR: 799.01 833.10 0.72 0.00 2021-02-12T23:11:01Z- AHRS_M2 msg: 1613171461.79 epoch sec; seqNo:1470 2021-02-12T23:11:01Z- AHRS_M2 msg: 0.57 phi; -0.16 theta; -0.16 psi 2021-02-12T23:11:01Z- handleDepth()-Depth_Keller msg: 1613171461793 epoch sec; seqNo:1480; depth 29.85 2021-02-12T23:11:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171461.80 epoch sec; seqNo:1480; 4056363.63 north; 590547.17 east 2021-02-12T23:11:02Z- AHRS_M2 msg: 1613171462.20 epoch sec; seqNo:1471 2021-02-12T23:11:02Z- AHRS_M2 msg: 0.56 phi; -0.11 theta; -0.15 psi 2021-02-12T23:11:02Z- handleDepth()-Depth_Keller msg: 1613171462203 epoch sec; seqNo:1481; depth 29.97 2021-02-12T23:11:02Z- handleDvl() - RDI_Pathfinder msg: 1613171462.21 epoch sec; seqNo:622 2021-02-12T23:11:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 58.85 , 71.11 , 58.44 , 71.92 2021-02-12T23:11:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , -0.03 2021-02-12T23:11:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171462.22 epoch sec; seqNo:1481; 4056364.00 north; 590547.17 east 2021-02-12T23:11:02Z- AHRS_M2 msg: 1613171462.60 epoch sec; seqNo:1472 2021-02-12T23:11:02Z- AHRS_M2 msg: 0.56 phi; -0.07 theta; -0.14 psi 2021-02-12T23:11:02Z- handleDepth()-Depth_Keller msg: 1613171462604 epoch sec; seqNo:1482; depth 30.07 2021-02-12T23:11:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171462.64 epoch sec; seqNo:1482; 4056364.38 north; 590547.16 east 2021-02-12T23:11:03Z- AHRS_M2 msg: 1613171463.00 epoch sec; seqNo:1473 2021-02-12T23:11:03Z- AHRS_M2 msg: 0.55 phi; -0.03 theta; -0.14 psi 2021-02-12T23:11:03Z- handleDepth()-Depth_Keller msg: 1613171463004 epoch sec; seqNo:1483; depth 30.16 2021-02-12T23:11:03Z- handleDvl() - RDI_Pathfinder msg: 1613171463.02 epoch sec; seqNo:623 2021-02-12T23:11:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 58.87 , 68.99 , 61.32 , 66.23 2021-02-12T23:11:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.04 2021-02-12T23:11:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171463.04 epoch sec; seqNo:1483; 4056364.76 north; 590547.16 east 2021-02-12T23:11:03Z- AHRS_M2 msg: 1613171463.40 epoch sec; seqNo:1474 2021-02-12T23:11:03Z- AHRS_M2 msg: 0.54 phi; 0.01 theta; -0.13 psi 2021-02-12T23:11:03Z- handleDepth()-Depth_Keller msg: 1613171463408 epoch sec; seqNo:1484; depth 30.24 2021-02-12T23:11:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171463.42 epoch sec; seqNo:1484; 4056364.76 north; 590547.16 east 2021-02-12T23:11:03Z- AHRS_M2 msg: 1613171463.81 epoch sec; seqNo:1475 2021-02-12T23:11:03Z- AHRS_M2 msg: 0.54 phi; 0.06 theta; -0.12 psi 2021-02-12T23:11:03Z- handleDepth()-Depth_Keller msg: 1613171463813 epoch sec; seqNo:1485; depth 30.30 2021-02-12T23:11:03Z- handleDvl() - RDI_Pathfinder msg: 1613171463.83 epoch sec; seqNo:624 2021-02-12T23:11:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 59.18 , 67.46 , 64.70 , 61.94 2021-02-12T23:11:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.04 2021-02-12T23:11:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171463.84 epoch sec; seqNo:1485; 4056365.15 north; 590548.38 east 2021-02-12T23:11:04Z- AHRS_M2 msg: 1613171464.21 epoch sec; seqNo:1476 2021-02-12T23:11:04Z- AHRS_M2 msg: 0.54 phi; 0.10 theta; -0.11 psi 2021-02-12T23:11:04Z- handleDepth()-Depth_Keller msg: 1613171464218 epoch sec; seqNo:1486; depth 30.34 2021-02-12T23:11:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171464.24 epoch sec; seqNo:1486; 4056365.53 north; 590548.37 east 2021-02-12T23:11:04Z- AHRS_M2 msg: 1613171464.64 epoch sec; seqNo:1477 2021-02-12T23:11:04Z- AHRS_M2 msg: 0.54 phi; 0.13 theta; -0.10 psi 2021-02-12T23:11:04Z- handleDepth()-Depth_Keller msg: 1613171464647 epoch sec; seqNo:1487; depth 30.37 2021-02-12T23:11:04Z- handleDvl() - RDI_Pathfinder msg: 1613171464.66 epoch sec; seqNo:625 2021-02-12T23:11:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.81 , 67.18 , 69.79 , 59.36 2021-02-12T23:11:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , -0.03 2021-02-12T23:11:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171464.67 epoch sec; seqNo:1487; 4056365.91 north; 590548.37 east 2021-02-12T23:11:05Z- AHRS_M2 msg: 1613171465.02 epoch sec; seqNo:1478 2021-02-12T23:11:05Z- AHRS_M2 msg: 0.54 phi; 0.17 theta; -0.10 psi 2021-02-12T23:11:05Z- handleDepth()-Depth_Keller msg: 1613171465024 epoch sec; seqNo:1488; depth 30.39 2021-02-12T23:11:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171465.04 epoch sec; seqNo:1488; 4056366.29 north; 590548.36 east 2021-02-12T23:11:05Z- AHRS_M2 msg: 1613171465.42 epoch sec; seqNo:1479 2021-02-12T23:11:05Z- AHRS_M2 msg: 0.54 phi; 0.20 theta; -0.09 psi 2021-02-12T23:11:05Z- handleDepth()-Depth_Keller msg: 1613171465429 epoch sec; seqNo:1489; depth 30.38 2021-02-12T23:11:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171465.44 epoch sec; seqNo:1489; 4056366.29 north; 590548.36 east 2021-02-12T23:11:05Z- AHRS_M2 msg: 1613171465.83 epoch sec; seqNo:1480 2021-02-12T23:11:05Z- AHRS_M2 msg: 0.54 phi; 0.22 theta; -0.09 psi 2021-02-12T23:11:05Z- handleDepth()-Depth_Keller msg: 1613171465832 epoch sec; seqNo:1490; depth 30.37 2021-02-12T23:11:05Z- handleDvl() - RDI_Pathfinder msg: 1613171465.85 epoch sec; seqNo:626 2021-02-12T23:11:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.55 , 67.46 , 74.20 , 57.95 2021-02-12T23:11:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , -0.01 2021-02-12T23:11:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171465.86 epoch sec; seqNo:1490; 4056366.67 north; 590548.36 east 2021-02-12T23:11:06Z- AHRS_M2 msg: 1613171466.23 epoch sec; seqNo:1481 2021-02-12T23:11:06Z- AHRS_M2 msg: 0.54 phi; 0.25 theta; -0.09 psi 2021-02-12T23:11:06Z- handleDepth()-Depth_Keller msg: 1613171466237 epoch sec; seqNo:1491; depth 30.34 2021-02-12T23:11:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171466.25 epoch sec; seqNo:1491; 4056367.05 north; 590548.36 east 2021-02-12T23:11:06Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-12T23:11:06Z- waiting for 1613171464.40, time is 1613171466.25 2021-02-12T23:11:06Z- LcmTrn::updateTrn() >>>> pose time 1613171466.25, meas time 1613171465.85 <<<< 2021-02-12T23:11:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171465.85 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:11:06Z- TerrainNav::motionUpdate() - currEstimate = (4056367, 590548.36, 30.34) 2021-02-12T23:11:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:11:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:11:06Z- TNavBF::Projecting Measurements 2021-02-12T23:11:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:11:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:11:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:11:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:11:06Z- TNavFilter::4 good beams. 2021-02-12T23:11:06Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:11:06Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:11:06Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:11:06Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:11:06Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:11:06Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:11:06Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:11:06Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:06Z- TerrainMapOctree: (vehN, vehE ) = (4056366.74, 590548.26). 2021-02-12T23:11:06Z- TerrainMapOctree: Min Distance = 6127.74. 2021-02-12T23:11:06Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:11:06Z- There is no specific good beam that all particles have in common. 2021-02-12T23:11:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:11:06Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 6.26 2021-02-12T23:11:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:11:06Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 7.28 2021-02-12T23:11:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:11:06Z- Old Depth Bias: 0.00 DepthBiasCov: 0.75 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 8.81 2021-02-12T23:11:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:11:06Z- Old Depth Bias: 0.00 DepthBiasCov: 0.70 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 5.37 2021-02-12T23:11:06Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:11:06Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:11:06Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:11:06Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:11:06Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:11:06Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:11:06Z- Filter 0 No beams used in subcloud update 2021-02-12T23:11:06Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:11:06Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:11:06Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 798.8 EastVariance: 833.1 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-12T23:11:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171465.85 sec. 2021-02-12T23:11:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:11:06Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:06Z- variances([0], [2], and psi_berg) = 798.731767 833.100149 0.000000 2021-02-12T23:11:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:11:06Z- LcmTrn::publishEstimates() - MLE : 4056338.55 590500.86 30.34 2021-02-12T23:11:06Z- LcmTrn::publishEstimates() - MMSE : 4056367.05 590548.36 30.34 2021-02-12T23:11:06Z- LcmTrn::publishEstimates() - COVAR: 798.73 833.10 0.65 0.00 2021-02-12T23:11:06Z- AHRS_M2 msg: 1613171466.65 epoch sec; seqNo:1482 2021-02-12T23:11:06Z- AHRS_M2 msg: 0.54 phi; 0.27 theta; -0.08 psi 2021-02-12T23:11:06Z- handleDepth()-Depth_Keller msg: 1613171466660 epoch sec; seqNo:1492; depth 30.29 2021-02-12T23:11:06Z- handleDvl() - RDI_Pathfinder msg: 1613171466.68 epoch sec; seqNo:627 2021-02-12T23:11:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.39 , 68.68 , 78.19 , 57.03 2021-02-12T23:11:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.00 2021-02-12T23:11:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171466.69 epoch sec; seqNo:1492; 4056367.43 north; 590548.35 east 2021-02-12T23:11:07Z- AHRS_M2 msg: 1613171467.04 epoch sec; seqNo:1483 2021-02-12T23:11:07Z- AHRS_M2 msg: 0.54 phi; 0.29 theta; -0.08 psi 2021-02-12T23:11:07Z- handleDepth()-Depth_Keller msg: 1613171467046 epoch sec; seqNo:1493; depth 30.24 2021-02-12T23:11:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171467.06 epoch sec; seqNo:1493; 4056367.44 north; 590549.57 east 2021-02-12T23:11:07Z- AHRS_M2 msg: 1613171467.44 epoch sec; seqNo:1484 2021-02-12T23:11:07Z- AHRS_M2 msg: 0.55 phi; 0.30 theta; -0.08 psi 2021-02-12T23:11:07Z- handleDepth()-Depth_Keller msg: 1613171467453 epoch sec; seqNo:1494; depth 30.18 2021-02-12T23:11:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171467.48 epoch sec; seqNo:1494; 4056367.82 north; 590549.57 east 2021-02-12T23:11:07Z- AHRS_M2 msg: 1613171467.87 epoch sec; seqNo:1485 2021-02-12T23:11:07Z- AHRS_M2 msg: 0.55 phi; 0.32 theta; -0.07 psi 2021-02-12T23:11:07Z- handleDepth()-Depth_Keller msg: 1613171467883 epoch sec; seqNo:1495; depth 30.10 2021-02-12T23:11:07Z- handleDvl() - RDI_Pathfinder msg: 1613171467.89 epoch sec; seqNo:628 2021-02-12T23:11:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.23 , 68.99 , 0.00 , 56.72 2021-02-12T23:11:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.01 , -0.01 2021-02-12T23:11:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171467.90 epoch sec; seqNo:1495; 4056368.20 north; 590549.56 east 2021-02-12T23:11:08Z- AHRS_M2 msg: 1613171468.25 epoch sec; seqNo:1486 2021-02-12T23:11:08Z- AHRS_M2 msg: 0.56 phi; 0.33 theta; -0.07 psi 2021-02-12T23:11:08Z- handleDepth()-Depth_Keller msg: 1613171468257 epoch sec; seqNo:1496; depth 30.02 2021-02-12T23:11:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171468.27 epoch sec; seqNo:1496; 4056368.20 north; 590549.56 east 2021-02-12T23:11:08Z- AHRS_M2 msg: 1613171468.65 epoch sec; seqNo:1487 2021-02-12T23:11:08Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.07 psi 2021-02-12T23:11:08Z- handleDepth()-Depth_Keller msg: 1613171468669 epoch sec; seqNo:1497; depth 29.95 2021-02-12T23:11:08Z- handleDvl() - RDI_Pathfinder msg: 1613171468.68 epoch sec; seqNo:629 2021-02-12T23:11:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.76 , 69.60 , 0.00 , 56.72 2021-02-12T23:11:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.04 , -0.03 2021-02-12T23:11:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171468.69 epoch sec; seqNo:1497; 4056368.58 north; 590549.56 east 2021-02-12T23:11:09Z- AHRS_M2 msg: 1613171469.06 epoch sec; seqNo:1488 2021-02-12T23:11:09Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.06 psi 2021-02-12T23:11:09Z- handleDepth()-Depth_Keller msg: 1613171469064 epoch sec; seqNo:1498; depth 29.86 2021-02-12T23:11:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171469.08 epoch sec; seqNo:1498; 4056368.96 north; 590549.56 east 2021-02-12T23:11:09Z- AHRS_M2 msg: 1613171469.46 epoch sec; seqNo:1489 2021-02-12T23:11:09Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.06 psi 2021-02-12T23:11:09Z- handleDepth()-Depth_Keller msg: 1613171469468 epoch sec; seqNo:1499; depth 29.78 2021-02-12T23:11:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171469.48 epoch sec; seqNo:1499; 4056368.96 north; 590549.56 east 2021-02-12T23:11:09Z- AHRS_M2 msg: 1613171469.87 epoch sec; seqNo:1490 2021-02-12T23:11:09Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.06 psi 2021-02-12T23:11:09Z- handleDepth()-Depth_Keller msg: 1613171469872 epoch sec; seqNo:1500; depth 29.69 2021-02-12T23:11:09Z- handleDvl() - RDI_Pathfinder msg: 1613171469.89 epoch sec; seqNo:630 2021-02-12T23:11:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.76 , 70.52 , 0.00 , 57.03 2021-02-12T23:11:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.03 , -0.06 2021-02-12T23:11:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171469.90 epoch sec; seqNo:1500; 4056369.35 north; 590550.77 east 2021-02-12T23:11:10Z- AHRS_M2 msg: 1613171470.27 epoch sec; seqNo:1491 2021-02-12T23:11:10Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.06 psi 2021-02-12T23:11:10Z- handleDepth()-Depth_Keller msg: 1613171470282 epoch sec; seqNo:1501; depth 29.56 2021-02-12T23:11:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171470.29 epoch sec; seqNo:1501; 4056369.73 north; 590550.77 east 2021-02-12T23:11:10Z- AHRS_M2 msg: 1613171470.68 epoch sec; seqNo:1492 2021-02-12T23:11:10Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.07 psi 2021-02-12T23:11:10Z- handleDepth()-Depth_Keller msg: 1613171470698 epoch sec; seqNo:1502; depth 29.44 2021-02-12T23:11:10Z- handleDvl() - RDI_Pathfinder msg: 1613171470.71 epoch sec; seqNo:631 2021-02-12T23:11:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.38 , 71.44 , 0.00 , 56.72 2021-02-12T23:11:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.00 , -0.06 2021-02-12T23:11:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171470.72 epoch sec; seqNo:1502; 4056369.73 north; 590550.77 east 2021-02-12T23:11:11Z- AHRS_M2 msg: 1613171471.08 epoch sec; seqNo:1493 2021-02-12T23:11:11Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.07 psi 2021-02-12T23:11:11Z- handleDepth()-Depth_Keller msg: 1613171471085 epoch sec; seqNo:1503; depth 29.33 2021-02-12T23:11:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171471.10 epoch sec; seqNo:1503; 4056370.11 north; 590550.77 east 2021-02-12T23:11:11Z- AHRS_M2 msg: 1613171471.48 epoch sec; seqNo:1494 2021-02-12T23:11:11Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.07 psi 2021-02-12T23:11:11Z- handleDepth()-Depth_Keller msg: 1613171471493 epoch sec; seqNo:1504; depth 29.22 2021-02-12T23:11:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171471.50 epoch sec; seqNo:1504; 4056370.48 north; 590550.76 east 2021-02-12T23:11:11Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-12T23:11:11Z- waiting for 1613171469.25, time is 1613171471.50 2021-02-12T23:11:11Z- LcmTrn::updateTrn() >>>> pose time 1613171471.50, meas time 1613171470.71 <<<< 2021-02-12T23:11:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171470.71 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:11:11Z- TerrainNav::motionUpdate() - currEstimate = (4056370, 590550.76, 29.22) 2021-02-12T23:11:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:11:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:11:11Z- TNavBF::Projecting Measurements 2021-02-12T23:11:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:11:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:11:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:11:11Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:11:11Z- TNavFilter::3 good beams. 2021-02-12T23:11:11Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:11:11Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:11:11Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:11:11Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:11:11Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:11:11Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:11:11Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:11:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:11Z- TerrainMapOctree: (vehN, vehE ) = (4056369.97, 590550.40). 2021-02-12T23:11:11Z- TerrainMapOctree: Min Distance = 6123.87. 2021-02-12T23:11:11Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:11:11Z- There is no specific good beam that all particles have in common. 2021-02-12T23:11:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:11:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 7.48 2021-02-12T23:11:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:11:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.76 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 8.17 2021-02-12T23:11:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:11:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.71 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 5.15 2021-02-12T23:11:11Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:11:11Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:11:11Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:11:11Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:11:11Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:11:11Z- Filter 0 No beams used in subcloud update 2021-02-12T23:11:11Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:11:11Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:11:11Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 798.5 EastVariance: 833.1 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-12T23:11:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171470.71 sec. 2021-02-12T23:11:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:11:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:11Z- variances([0], [2], and psi_berg) = 798.438030 833.099533 0.000000 2021-02-12T23:11:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:11:11Z- LcmTrn::publishEstimates() - MLE : 4056341.98 590503.26 29.22 2021-02-12T23:11:11Z- LcmTrn::publishEstimates() - MMSE : 4056370.48 590550.76 29.22 2021-02-12T23:11:11Z- LcmTrn::publishEstimates() - COVAR: 798.44 833.10 0.67 0.00 2021-02-12T23:11:11Z- AHRS_M2 msg: 1613171471.89 epoch sec; seqNo:1495 2021-02-12T23:11:11Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.08 psi 2021-02-12T23:11:11Z- handleDepth()-Depth_Keller msg: 1613171471892 epoch sec; seqNo:1505; depth 29.10 2021-02-12T23:11:11Z- handleDvl() - RDI_Pathfinder msg: 1613171471.91 epoch sec; seqNo:632 2021-02-12T23:11:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 71.75 , 0.00 , 57.34 2021-02-12T23:11:11Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:11:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171471.92 epoch sec; seqNo:1505; 4056370.48 north; 590550.76 east 2021-02-12T23:11:12Z- AHRS_M2 msg: 1613171472.29 epoch sec; seqNo:1496 2021-02-12T23:11:12Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.08 psi 2021-02-12T23:11:12Z- handleDepth()-Depth_Keller msg: 1613171472301 epoch sec; seqNo:1506; depth 28.97 2021-02-12T23:11:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171472.31 epoch sec; seqNo:1506; 4056370.86 north; 590550.76 east 2021-02-12T23:11:12Z- AHRS_M2 msg: 1613171472.74 epoch sec; seqNo:1497 2021-02-12T23:11:12Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.08 psi 2021-02-12T23:11:12Z- handleDepth()-Depth_Keller msg: 1613171472742 epoch sec; seqNo:1507; depth 28.85 2021-02-12T23:11:12Z- handleDvl() - RDI_Pathfinder msg: 1613171472.77 epoch sec; seqNo:633 2021-02-12T23:11:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.27 , 72.36 , 0.00 , 57.24 2021-02-12T23:11:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , 0.01 2021-02-12T23:11:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171472.79 epoch sec; seqNo:1507; 4056371.25 north; 590551.97 east 2021-02-12T23:11:13Z- AHRS_M2 msg: 1613171473.11 epoch sec; seqNo:1498 2021-02-12T23:11:13Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.08 psi 2021-02-12T23:11:13Z- handleDepth()-Depth_Keller msg: 1613171473113 epoch sec; seqNo:1508; depth 28.69 2021-02-12T23:11:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171473.14 epoch sec; seqNo:1508; 4056371.25 north; 590551.97 east 2021-02-12T23:11:13Z- AHRS_M2 msg: 1613171473.51 epoch sec; seqNo:1499 2021-02-12T23:11:13Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.09 psi 2021-02-12T23:11:13Z- handleDepth()-Depth_Keller msg: 1613171473517 epoch sec; seqNo:1509; depth 28.60 2021-02-12T23:11:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171473.53 epoch sec; seqNo:1509; 4056371.63 north; 590551.97 east 2021-02-12T23:11:13Z- AHRS_M2 msg: 1613171473.91 epoch sec; seqNo:1500 2021-02-12T23:11:13Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.09 psi 2021-02-12T23:11:13Z- handleDepth()-Depth_Keller msg: 1613171473916 epoch sec; seqNo:1510; depth 28.49 2021-02-12T23:11:13Z- handleDvl() - RDI_Pathfinder msg: 1613171473.93 epoch sec; seqNo:634 2021-02-12T23:11:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.38 , 73.28 , 0.00 , 57.64 2021-02-12T23:11:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.00 , 0.03 2021-02-12T23:11:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171473.94 epoch sec; seqNo:1510; 4056372.01 north; 590551.97 east 2021-02-12T23:11:14Z- AHRS_M2 msg: 1613171474.32 epoch sec; seqNo:1501 2021-02-12T23:11:14Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.10 psi 2021-02-12T23:11:14Z- handleDepth()-Depth_Keller msg: 1613171474327 epoch sec; seqNo:1511; depth 28.35 2021-02-12T23:11:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171474.34 epoch sec; seqNo:1511; 4056372.01 north; 590551.97 east 2021-02-12T23:11:14Z- AHRS_M2 msg: 1613171474.72 epoch sec; seqNo:1502 2021-02-12T23:11:14Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.11 psi 2021-02-12T23:11:14Z- handleDepth()-Depth_Keller msg: 1613171474729 epoch sec; seqNo:1512; depth 28.23 2021-02-12T23:11:14Z- handleDvl() - RDI_Pathfinder msg: 1613171474.75 epoch sec; seqNo:635 2021-02-12T23:11:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.38 , 73.89 , 0.00 , 58.26 2021-02-12T23:11:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , 0.03 2021-02-12T23:11:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171474.76 epoch sec; seqNo:1512; 4056372.39 north; 590551.96 east 2021-02-12T23:11:15Z- AHRS_M2 msg: 1613171475.13 epoch sec; seqNo:1503 2021-02-12T23:11:15Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.11 psi 2021-02-12T23:11:15Z- handleDepth()-Depth_Keller msg: 1613171475145 epoch sec; seqNo:1513; depth 28.09 2021-02-12T23:11:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171475.17 epoch sec; seqNo:1513; 4056372.78 north; 590553.18 east 2021-02-12T23:11:15Z- AHRS_M2 msg: 1613171475.53 epoch sec; seqNo:1504 2021-02-12T23:11:15Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.11 psi 2021-02-12T23:11:15Z- handleDepth()-Depth_Keller msg: 1613171475532 epoch sec; seqNo:1514; depth 27.97 2021-02-12T23:11:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171475.54 epoch sec; seqNo:1514; 4056372.78 north; 590553.18 east 2021-02-12T23:11:15Z- AHRS_M2 msg: 1613171475.93 epoch sec; seqNo:1505 2021-02-12T23:11:15Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.11 psi 2021-02-12T23:11:15Z- handleDepth()-Depth_Keller msg: 1613171475941 epoch sec; seqNo:1515; depth 27.84 2021-02-12T23:11:15Z- handleDvl() - RDI_Pathfinder msg: 1613171475.95 epoch sec; seqNo:636 2021-02-12T23:11:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.38 , 74.51 , 0.00 , 58.56 2021-02-12T23:11:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.03 , 0.01 2021-02-12T23:11:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171475.97 epoch sec; seqNo:1515; 4056373.16 north; 590553.18 east 2021-02-12T23:11:16Z- AHRS_M2 msg: 1613171476.34 epoch sec; seqNo:1506 2021-02-12T23:11:16Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.12 psi 2021-02-12T23:11:16Z- handleDepth()-Depth_Keller msg: 1613171476340 epoch sec; seqNo:1516; depth 27.70 2021-02-12T23:11:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171476.35 epoch sec; seqNo:1516; 4056373.54 north; 590553.17 east 2021-02-12T23:11:16Z- AHRS_M2 msg: 1613171476.74 epoch sec; seqNo:1507 2021-02-12T23:11:16Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.12 psi 2021-02-12T23:11:16Z- handleDepth()-Depth_Keller msg: 1613171476744 epoch sec; seqNo:1517; depth 27.58 2021-02-12T23:11:16Z- handleDvl() - RDI_Pathfinder msg: 1613171476.76 epoch sec; seqNo:637 2021-02-12T23:11:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.07 , 75.43 , 0.00 , 58.56 2021-02-12T23:11:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.00 , 0.00 2021-02-12T23:11:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171476.77 epoch sec; seqNo:1517; 4056373.54 north; 590553.17 east 2021-02-12T23:11:16Z- LcmTrn::handleMessages() - handling 45 messages took 0.71 ms and 131 calls... 2021-02-12T23:11:16Z- waiting for 1613171474.50, time is 1613171476.77 2021-02-12T23:11:16Z- LcmTrn::updateTrn() equating pose 1613171476.77 and meas times 1613171476.77 2021-02-12T23:11:16Z- LcmTrn::updateTrn() >>>> pose time 1613171476.77, meas time 1613171476.77 <<<< 2021-02-12T23:11:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171476.77 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:11:16Z- TerrainNav::motionUpdate() - currEstimate = (4056374, 590553.17, 27.58) 2021-02-12T23:11:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:11:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:11:16Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171476.77 sec.; No relevant navigation data available 2021-02-12T23:11:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:11:16Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:16Z- variances([0], [2], and psi_berg) = 798.223083 833.099084 0.000000 2021-02-12T23:11:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:11:16Z- LcmTrn::publishEstimates() - MLE : 4056345.04 590504.67 27.58 2021-02-12T23:11:16Z- LcmTrn::publishEstimates() - MMSE : 4056373.54 590553.17 27.58 2021-02-12T23:11:16Z- LcmTrn::publishEstimates() - COVAR: 798.22 833.10 0.87 0.00 2021-02-12T23:11:17Z- AHRS_M2 msg: 1613171477.15 epoch sec; seqNo:1508 2021-02-12T23:11:17Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.12 psi 2021-02-12T23:11:17Z- handleDepth()-Depth_Keller msg: 1613171477157 epoch sec; seqNo:1518; depth 27.45 2021-02-12T23:11:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171477.17 epoch sec; seqNo:1518; 4056373.92 north; 590553.17 east 2021-02-12T23:11:17Z- AHRS_M2 msg: 1613171477.55 epoch sec; seqNo:1509 2021-02-12T23:11:17Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.13 psi 2021-02-12T23:11:17Z- handleDepth()-Depth_Keller msg: 1613171477554 epoch sec; seqNo:1519; depth 27.32 2021-02-12T23:11:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171477.56 epoch sec; seqNo:1519; 4056374.30 north; 590553.16 east 2021-02-12T23:11:17Z- AHRS_M2 msg: 1613171477.95 epoch sec; seqNo:1510 2021-02-12T23:11:17Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.14 psi 2021-02-12T23:11:17Z- handleDepth()-Depth_Keller msg: 1613171477956 epoch sec; seqNo:1520; depth 27.20 2021-02-12T23:11:17Z- handleDvl() - RDI_Pathfinder msg: 1613171477.97 epoch sec; seqNo:638 2021-02-12T23:11:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.29 , 76.38 , 0.00 , 59.23 2021-02-12T23:11:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , 0.03 2021-02-12T23:11:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171477.98 epoch sec; seqNo:1520; 4056374.31 north; 590554.38 east 2021-02-12T23:11:18Z- AHRS_M2 msg: 1613171478.36 epoch sec; seqNo:1511 2021-02-12T23:11:18Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.14 psi 2021-02-12T23:11:18Z- handleDepth()-Depth_Keller msg: 1613171478360 epoch sec; seqNo:1521; depth 27.08 2021-02-12T23:11:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171478.37 epoch sec; seqNo:1521; 4056374.69 north; 590554.38 east 2021-02-12T23:11:18Z- AHRS_M2 msg: 1613171478.77 epoch sec; seqNo:1512 2021-02-12T23:11:18Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.15 psi 2021-02-12T23:11:18Z- handleDepth()-Depth_Keller msg: 1613171478771 epoch sec; seqNo:1522; depth 26.96 2021-02-12T23:11:18Z- handleDvl() - RDI_Pathfinder msg: 1613171478.78 epoch sec; seqNo:639 2021-02-12T23:11:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.97 , 77.35 , 0.00 , 59.55 2021-02-12T23:11:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.01 , 0.06 2021-02-12T23:11:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171478.79 epoch sec; seqNo:1522; 4056375.07 north; 590554.38 east 2021-02-12T23:11:19Z- AHRS_M2 msg: 1613171479.16 epoch sec; seqNo:1513 2021-02-12T23:11:19Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.15 psi 2021-02-12T23:11:19Z- handleDepth()-Depth_Keller msg: 1613171479170 epoch sec; seqNo:1523; depth 26.83 2021-02-12T23:11:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171479.18 epoch sec; seqNo:1523; 4056375.07 north; 590554.38 east 2021-02-12T23:11:19Z- AHRS_M2 msg: 1613171479.57 epoch sec; seqNo:1514 2021-02-12T23:11:19Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.16 psi 2021-02-12T23:11:19Z- handleDepth()-Depth_Keller msg: 1613171479577 epoch sec; seqNo:1524; depth 26.71 2021-02-12T23:11:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171479.60 epoch sec; seqNo:1524; 4056375.45 north; 590554.37 east 2021-02-12T23:11:19Z- AHRS_M2 msg: 1613171479.97 epoch sec; seqNo:1515 2021-02-12T23:11:19Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.16 psi 2021-02-12T23:11:19Z- handleDepth()-Depth_Keller msg: 1613171479981 epoch sec; seqNo:1525; depth 26.57 2021-02-12T23:11:20Z- handleDvl() - RDI_Pathfinder msg: 1613171480.00 epoch sec; seqNo:640 2021-02-12T23:11:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.97 , 78.32 , 0.00 , 59.88 2021-02-12T23:11:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.06 , 0.07 2021-02-12T23:11:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171480.01 epoch sec; seqNo:1525; 4056375.83 north; 590554.37 east 2021-02-12T23:11:20Z- AHRS_M2 msg: 1613171480.38 epoch sec; seqNo:1516 2021-02-12T23:11:20Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.17 psi 2021-02-12T23:11:20Z- handleDepth()-Depth_Keller msg: 1613171480382 epoch sec; seqNo:1526; depth 26.45 2021-02-12T23:11:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171480.39 epoch sec; seqNo:1526; 4056375.84 north; 590555.59 east 2021-02-12T23:11:20Z- AHRS_M2 msg: 1613171480.78 epoch sec; seqNo:1517 2021-02-12T23:11:20Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.17 psi 2021-02-12T23:11:20Z- handleDepth()-Depth_Keller msg: 1613171480790 epoch sec; seqNo:1527; depth 26.33 2021-02-12T23:11:20Z- handleDvl() - RDI_Pathfinder msg: 1613171480.80 epoch sec; seqNo:641 2021-02-12T23:11:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.97 , 79.29 , 0.00 , 60.20 2021-02-12T23:11:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.10 , 0.03 2021-02-12T23:11:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171480.81 epoch sec; seqNo:1527; 4056376.22 north; 590555.59 east 2021-02-12T23:11:21Z- AHRS_M2 msg: 1613171481.18 epoch sec; seqNo:1518 2021-02-12T23:11:21Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.18 psi 2021-02-12T23:11:21Z- handleDepth()-Depth_Keller msg: 1613171481189 epoch sec; seqNo:1528; depth 26.20 2021-02-12T23:11:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171481.21 epoch sec; seqNo:1528; 4056376.60 north; 590555.58 east 2021-02-12T23:11:21Z- AHRS_M2 msg: 1613171481.59 epoch sec; seqNo:1519 2021-02-12T23:11:21Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.18 psi 2021-02-12T23:11:21Z- handleDepth()-Depth_Keller msg: 1613171481592 epoch sec; seqNo:1529; depth 26.08 2021-02-12T23:11:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171481.61 epoch sec; seqNo:1529; 4056376.60 north; 590555.58 east 2021-02-12T23:11:21Z- LcmTrn::handleMessages() - handling 40 messages took 0.61 ms and 128 calls... 2021-02-12T23:11:21Z- waiting for 1613171479.77, time is 1613171481.61 2021-02-12T23:11:21Z- LcmTrn::updateTrn() >>>> pose time 1613171481.61, meas time 1613171480.80 <<<< 2021-02-12T23:11:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171480.80 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:11:21Z- TerrainNav::motionUpdate() - currEstimate = (4056377, 590555.58, 26.08) 2021-02-12T23:11:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:11:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:11:21Z- TNavBF::Projecting Measurements 2021-02-12T23:11:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:11:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:11:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:11:21Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:11:21Z- TNavFilter::3 good beams. 2021-02-12T23:11:21Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:11:21Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:11:21Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:11:21Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:11:21Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:11:21Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:11:21Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:11:21Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:21Z- TerrainMapOctree: (vehN, vehE ) = (4056376.09, 590555.18). 2021-02-12T23:11:21Z- TerrainMapOctree: Min Distance = 6116.10. 2021-02-12T23:11:21Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:11:21Z- There is no specific good beam that all particles have in common. 2021-02-12T23:11:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:11:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 7.39 2021-02-12T23:11:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:11:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 10.06 2021-02-12T23:11:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:11:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 5.80 2021-02-12T23:11:21Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:11:21Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:11:21Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:11:21Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:11:21Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:11:21Z- Filter 0 No beams used in subcloud update 2021-02-12T23:11:21Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:11:21Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:11:21Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 798.0 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:11:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171480.80 sec. 2021-02-12T23:11:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:11:21Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:21Z- variances([0], [2], and psi_berg) = 797.940800 833.098499 0.000000 2021-02-12T23:11:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:11:21Z- LcmTrn::publishEstimates() - MLE : 4056348.10 590507.08 26.08 2021-02-12T23:11:21Z- LcmTrn::publishEstimates() - MMSE : 4056376.60 590555.58 26.08 2021-02-12T23:11:21Z- LcmTrn::publishEstimates() - COVAR: 797.94 833.10 0.81 0.00 2021-02-12T23:11:21Z- AHRS_M2 msg: 1613171481.99 epoch sec; seqNo:1520 2021-02-12T23:11:21Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.18 psi 2021-02-12T23:11:22Z- handleDepth()-Depth_Keller msg: 1613171482005 epoch sec; seqNo:1530; depth 25.97 2021-02-12T23:11:22Z- handleDvl() - RDI_Pathfinder msg: 1613171482.02 epoch sec; seqNo:642 2021-02-12T23:11:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.64 , 80.59 , 0.00 , 60.85 2021-02-12T23:11:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.11 , -0.02 2021-02-12T23:11:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171482.03 epoch sec; seqNo:1530; 4056376.98 north; 590555.58 east 2021-02-12T23:11:22Z- AHRS_M2 msg: 1613171482.39 epoch sec; seqNo:1521 2021-02-12T23:11:22Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.19 psi 2021-02-12T23:11:22Z- handleDepth()-Depth_Keller msg: 1613171482405 epoch sec; seqNo:1531; depth 25.84 2021-02-12T23:11:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171482.41 epoch sec; seqNo:1531; 4056376.99 north; 590556.80 east 2021-02-12T23:11:22Z- AHRS_M2 msg: 1613171482.80 epoch sec; seqNo:1522 2021-02-12T23:11:22Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.19 psi 2021-02-12T23:11:22Z- handleDepth()-Depth_Keller msg: 1613171482809 epoch sec; seqNo:1532; depth 25.72 2021-02-12T23:11:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171482.82 epoch sec; seqNo:1532; 4056377.37 north; 590556.79 east 2021-02-12T23:11:23Z- AHRS_M2 msg: 1613171483.20 epoch sec; seqNo:1523 2021-02-12T23:11:23Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.19 psi 2021-02-12T23:11:23Z- handleDepth()-Depth_Keller msg: 1613171483210 epoch sec; seqNo:1533; depth 25.61 2021-02-12T23:11:23Z- handleDvl() - RDI_Pathfinder msg: 1613171483.22 epoch sec; seqNo:643 2021-02-12T23:11:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 85.00 , 94.74 , 60.48 2021-02-12T23:11:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.08 , -0.07 2021-02-12T23:11:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171483.23 epoch sec; seqNo:1533; 4056377.75 north; 590556.79 east 2021-02-12T23:11:23Z- AHRS_M2 msg: 1613171483.61 epoch sec; seqNo:1524 2021-02-12T23:11:23Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.19 psi 2021-02-12T23:11:23Z- handleDepth()-Depth_Keller msg: 1613171483615 epoch sec; seqNo:1534; depth 25.48 2021-02-12T23:11:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171483.64 epoch sec; seqNo:1534; 4056377.75 north; 590556.79 east 2021-02-12T23:11:24Z- AHRS_M2 msg: 1613171484.03 epoch sec; seqNo:1525 2021-02-12T23:11:24Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.19 psi 2021-02-12T23:11:24Z- handleDepth()-Depth_Keller msg: 1613171484046 epoch sec; seqNo:1535; depth 25.36 2021-02-12T23:11:24Z- handleDvl() - RDI_Pathfinder msg: 1613171484.06 epoch sec; seqNo:644 2021-02-12T23:11:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 87.17 , 93.66 , 61.30 2021-02-12T23:11:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , -0.08 2021-02-12T23:11:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171484.07 epoch sec; seqNo:1535; 4056378.13 north; 590556.79 east 2021-02-12T23:11:24Z- AHRS_M2 msg: 1613171484.42 epoch sec; seqNo:1526 2021-02-12T23:11:24Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.20 psi 2021-02-12T23:11:24Z- handleDepth()-Depth_Keller msg: 1613171484424 epoch sec; seqNo:1536; depth 25.23 2021-02-12T23:11:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171484.43 epoch sec; seqNo:1536; 4056378.50 north; 590556.78 east 2021-02-12T23:11:24Z- AHRS_M2 msg: 1613171484.82 epoch sec; seqNo:1527 2021-02-12T23:11:24Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.20 psi 2021-02-12T23:11:24Z- handleDepth()-Depth_Keller msg: 1613171484830 epoch sec; seqNo:1537; depth 25.13 2021-02-12T23:11:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171484.84 epoch sec; seqNo:1537; 4056378.52 north; 590558.00 east 2021-02-12T23:11:25Z- AHRS_M2 msg: 1613171485.22 epoch sec; seqNo:1528 2021-02-12T23:11:25Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.19 psi 2021-02-12T23:11:25Z- handleDepth()-Depth_Keller msg: 1613171485233 epoch sec; seqNo:1538; depth 25.00 2021-02-12T23:11:25Z- handleDvl() - RDI_Pathfinder msg: 1613171485.24 epoch sec; seqNo:645 2021-02-12T23:11:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.02 , 87.17 , 93.66 , 61.71 2021-02-12T23:11:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , -0.04 2021-02-12T23:11:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171485.27 epoch sec; seqNo:1538; 4056378.90 north; 590558.00 east 2021-02-12T23:11:25Z- AHRS_M2 msg: 1613171485.63 epoch sec; seqNo:1529 2021-02-12T23:11:25Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.19 psi 2021-02-12T23:11:25Z- handleDepth()-Depth_Keller msg: 1613171485632 epoch sec; seqNo:1539; depth 24.88 2021-02-12T23:11:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171485.64 epoch sec; seqNo:1539; 4056379.27 north; 590558.00 east 2021-02-12T23:11:26Z- AHRS_M2 msg: 1613171486.05 epoch sec; seqNo:1530 2021-02-12T23:11:26Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.19 psi 2021-02-12T23:11:26Z- handleDepth()-Depth_Keller msg: 1613171486050 epoch sec; seqNo:1540; depth 24.78 2021-02-12T23:11:26Z- handleDvl() - RDI_Pathfinder msg: 1613171486.06 epoch sec; seqNo:646 2021-02-12T23:11:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 86.09 , 95.82 , 61.30 2021-02-12T23:11:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , 0.00 2021-02-12T23:11:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171486.08 epoch sec; seqNo:1540; 4056379.27 north; 590558.00 east 2021-02-12T23:11:26Z- AHRS_M2 msg: 1613171486.43 epoch sec; seqNo:1531 2021-02-12T23:11:26Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.19 psi 2021-02-12T23:11:26Z- handleDepth()-Depth_Keller msg: 1613171486440 epoch sec; seqNo:1541; depth 24.65 2021-02-12T23:11:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171486.46 epoch sec; seqNo:1541; 4056379.65 north; 590557.99 east 2021-02-12T23:11:26Z- AHRS_M2 msg: 1613171486.84 epoch sec; seqNo:1532 2021-02-12T23:11:26Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.19 psi 2021-02-12T23:11:26Z- handleDepth()-Depth_Keller msg: 1613171486849 epoch sec; seqNo:1542; depth 24.55 2021-02-12T23:11:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171486.87 epoch sec; seqNo:1542; 4056380.03 north; 590557.99 east 2021-02-12T23:11:26Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 130 calls... 2021-02-12T23:11:26Z- waiting for 1613171484.61, time is 1613171486.87 2021-02-12T23:11:26Z- LcmTrn::updateTrn() >>>> pose time 1613171486.87, meas time 1613171486.06 <<<< 2021-02-12T23:11:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171486.06 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:11:26Z- TerrainNav::motionUpdate() - currEstimate = (4056380, 590557.99, 24.55) 2021-02-12T23:11:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:11:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:11:26Z- TNavBF::Projecting Measurements 2021-02-12T23:11:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:11:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:11:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:11:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:11:26Z- TNavFilter::4 good beams. 2021-02-12T23:11:26Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:11:26Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:11:26Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:11:26Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:11:26Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:11:26Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:11:26Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:11:26Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:26Z- TerrainMapOctree: (vehN, vehE ) = (4056379.50, 590557.62). 2021-02-12T23:11:26Z- TerrainMapOctree: Min Distance = 6111.91. 2021-02-12T23:11:26Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:11:26Z- There is no specific good beam that all particles have in common. 2021-02-12T23:11:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:11:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-12T23:11:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:11:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 11.86 2021-02-12T23:11:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:11:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 14.69 2021-02-12T23:11:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:11:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 6.01 2021-02-12T23:11:26Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:11:26Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:11:26Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:11:26Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:11:26Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:11:26Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:11:26Z- Filter 0 No beams used in subcloud update 2021-02-12T23:11:26Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:11:26Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:11:26Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 797.7 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:11:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171486.06 sec. 2021-02-12T23:11:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:11:27Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:27Z- variances([0], [2], and psi_berg) = 797.651552 833.097903 0.000000 2021-02-12T23:11:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:11:27Z- LcmTrn::publishEstimates() - MLE : 4056351.53 590509.49 24.55 2021-02-12T23:11:27Z- LcmTrn::publishEstimates() - MMSE : 4056380.03 590557.99 24.55 2021-02-12T23:11:27Z- LcmTrn::publishEstimates() - COVAR: 797.65 833.10 0.76 0.00 2021-02-12T23:11:27Z- AHRS_M2 msg: 1613171487.24 epoch sec; seqNo:1533 2021-02-12T23:11:27Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.19 psi 2021-02-12T23:11:27Z- handleDepth()-Depth_Keller msg: 1613171487254 epoch sec; seqNo:1543; depth 24.43 2021-02-12T23:11:27Z- handleDvl() - RDI_Pathfinder msg: 1613171487.27 epoch sec; seqNo:647 2021-02-12T23:11:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 86.09 , 92.58 , 62.12 2021-02-12T23:11:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , 0.03 2021-02-12T23:11:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171487.28 epoch sec; seqNo:1543; 4056380.41 north; 590557.98 east 2021-02-12T23:11:27Z- AHRS_M2 msg: 1613171487.66 epoch sec; seqNo:1534 2021-02-12T23:11:27Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-12T23:11:27Z- handleDepth()-Depth_Keller msg: 1613171487663 epoch sec; seqNo:1544; depth 24.31 2021-02-12T23:11:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171487.67 epoch sec; seqNo:1544; 4056380.42 north; 590559.20 east 2021-02-12T23:11:28Z- AHRS_M2 msg: 1613171488.05 epoch sec; seqNo:1535 2021-02-12T23:11:28Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-12T23:11:28Z- handleDepth()-Depth_Keller msg: 1613171488058 epoch sec; seqNo:1545; depth 24.20 2021-02-12T23:11:28Z- handleDvl() - RDI_Pathfinder msg: 1613171488.08 epoch sec; seqNo:648 2021-02-12T23:11:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 0.00 , 94.74 , 61.71 2021-02-12T23:11:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.04 , 0.03 2021-02-12T23:11:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171488.09 epoch sec; seqNo:1545; 4056380.80 north; 590559.20 east 2021-02-12T23:11:28Z- AHRS_M2 msg: 1613171488.46 epoch sec; seqNo:1536 2021-02-12T23:11:28Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-12T23:11:28Z- handleDepth()-Depth_Keller msg: 1613171488460 epoch sec; seqNo:1546; depth 24.08 2021-02-12T23:11:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171488.47 epoch sec; seqNo:1546; 4056381.18 north; 590559.20 east 2021-02-12T23:11:28Z- AHRS_M2 msg: 1613171488.87 epoch sec; seqNo:1537 2021-02-12T23:11:28Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-12T23:11:28Z- handleDepth()-Depth_Keller msg: 1613171488874 epoch sec; seqNo:1547; depth 23.97 2021-02-12T23:11:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171488.89 epoch sec; seqNo:1547; 4056381.18 north; 590559.20 east 2021-02-12T23:11:29Z- AHRS_M2 msg: 1613171489.26 epoch sec; seqNo:1538 2021-02-12T23:11:29Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-12T23:11:29Z- handleDepth()-Depth_Keller msg: 1613171489274 epoch sec; seqNo:1548; depth 23.87 2021-02-12T23:11:29Z- handleDvl() - RDI_Pathfinder msg: 1613171489.29 epoch sec; seqNo:649 2021-02-12T23:11:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 86.09 , 94.74 , 62.12 2021-02-12T23:11:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , 0.05 2021-02-12T23:11:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171489.30 epoch sec; seqNo:1548; 4056381.56 north; 590559.19 east 2021-02-12T23:11:29Z- AHRS_M2 msg: 1613171489.67 epoch sec; seqNo:1539 2021-02-12T23:11:29Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:29Z- handleDepth()-Depth_Keller msg: 1613171489672 epoch sec; seqNo:1549; depth 23.75 2021-02-12T23:11:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171489.68 epoch sec; seqNo:1549; 4056381.94 north; 590559.19 east 2021-02-12T23:11:30Z- AHRS_M2 msg: 1613171490.07 epoch sec; seqNo:1540 2021-02-12T23:11:30Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:30Z- handleDepth()-Depth_Keller msg: 1613171490076 epoch sec; seqNo:1550; depth 23.62 2021-02-12T23:11:30Z- handleDvl() - RDI_Pathfinder msg: 1613171490.09 epoch sec; seqNo:650 2021-02-12T23:11:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 86.09 , 95.82 , 62.12 2021-02-12T23:11:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.08 2021-02-12T23:11:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171490.10 epoch sec; seqNo:1550; 4056381.95 north; 590560.41 east 2021-02-12T23:11:30Z- AHRS_M2 msg: 1613171490.48 epoch sec; seqNo:1541 2021-02-12T23:11:30Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:30Z- handleDepth()-Depth_Keller msg: 1613171490485 epoch sec; seqNo:1551; depth 23.50 2021-02-12T23:11:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171490.50 epoch sec; seqNo:1551; 4056382.33 north; 590560.41 east 2021-02-12T23:11:30Z- AHRS_M2 msg: 1613171490.88 epoch sec; seqNo:1542 2021-02-12T23:11:30Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:30Z- handleDepth()-Depth_Keller msg: 1613171490884 epoch sec; seqNo:1552; depth 23.40 2021-02-12T23:11:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171490.90 epoch sec; seqNo:1552; 4056382.71 north; 590560.40 east 2021-02-12T23:11:31Z- AHRS_M2 msg: 1613171491.28 epoch sec; seqNo:1543 2021-02-12T23:11:31Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:31Z- handleDepth()-Depth_Keller msg: 1613171491296 epoch sec; seqNo:1553; depth 23.29 2021-02-12T23:11:31Z- handleDvl() - RDI_Pathfinder msg: 1613171491.33 epoch sec; seqNo:651 2021-02-12T23:11:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 87.17 , 94.74 , 62.93 2021-02-12T23:11:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , 0.12 2021-02-12T23:11:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171491.35 epoch sec; seqNo:1553; 4056383.09 north; 590560.40 east 2021-02-12T23:11:31Z- AHRS_M2 msg: 1613171491.69 epoch sec; seqNo:1544 2021-02-12T23:11:31Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:31Z- handleDepth()-Depth_Keller msg: 1613171491696 epoch sec; seqNo:1554; depth 23.17 2021-02-12T23:11:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171491.72 epoch sec; seqNo:1554; 4056383.09 north; 590560.40 east 2021-02-12T23:11:32Z- LcmTrn::handleMessages() - handling 41 messages took 0.71 ms and 127 calls... 2021-02-12T23:11:32Z- waiting for 1613171489.87, time is 1613171491.72 2021-02-12T23:11:32Z- LcmTrn::updateTrn() >>>> pose time 1613171491.72, meas time 1613171491.33 <<<< 2021-02-12T23:11:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171491.33 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:11:32Z- TerrainNav::motionUpdate() - currEstimate = (4056383, 590560.40, 23.17) 2021-02-12T23:11:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:11:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:11:32Z- TNavBF::Projecting Measurements 2021-02-12T23:11:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:11:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:11:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:11:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:11:32Z- TNavFilter::4 good beams. 2021-02-12T23:11:32Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:11:32Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:11:32Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:11:32Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:11:32Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:11:32Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:11:32Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:11:32Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:32Z- TerrainMapOctree: (vehN, vehE ) = (4056382.84, 590560.20). 2021-02-12T23:11:32Z- TerrainMapOctree: Min Distance = 6107.68. 2021-02-12T23:11:32Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:11:32Z- There is no specific good beam that all particles have in common. 2021-02-12T23:11:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:11:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 7.72 2021-02-12T23:11:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:11:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 12.16 2021-02-12T23:11:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:11:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 14.36 2021-02-12T23:11:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:11:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.77 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 6.34 2021-02-12T23:11:32Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:11:32Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:11:32Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:11:32Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:11:32Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:11:32Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:11:32Z- Filter 0 No beams used in subcloud update 2021-02-12T23:11:32Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:11:32Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:11:32Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 797.4 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:11:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171491.33 sec. 2021-02-12T23:11:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:11:32Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:32Z- variances([0], [2], and psi_berg) = 797.389313 833.097367 0.000000 2021-02-12T23:11:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:11:32Z- LcmTrn::publishEstimates() - MLE : 4056354.59 590511.90 23.17 2021-02-12T23:11:32Z- LcmTrn::publishEstimates() - MMSE : 4056383.09 590560.40 23.17 2021-02-12T23:11:32Z- LcmTrn::publishEstimates() - COVAR: 797.39 833.10 0.72 0.00 2021-02-12T23:11:32Z- AHRS_M2 msg: 1613171492.09 epoch sec; seqNo:1545 2021-02-12T23:11:32Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:32Z- handleDepth()-Depth_Keller msg: 1613171492097 epoch sec; seqNo:1555; depth 23.06 2021-02-12T23:11:32Z- handleDvl() - RDI_Pathfinder msg: 1613171492.12 epoch sec; seqNo:652 2021-02-12T23:11:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 87.16 , 97.98 , 63.35 2021-02-12T23:11:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.08 , 0.12 2021-02-12T23:11:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171492.13 epoch sec; seqNo:1555; 4056383.47 north; 590560.39 east 2021-02-12T23:11:32Z- AHRS_M2 msg: 1613171492.49 epoch sec; seqNo:1546 2021-02-12T23:11:32Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:32Z- handleDepth()-Depth_Keller msg: 1613171492500 epoch sec; seqNo:1556; depth 22.94 2021-02-12T23:11:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171492.52 epoch sec; seqNo:1556; 4056383.86 north; 590561.61 east 2021-02-12T23:11:32Z- AHRS_M2 msg: 1613171492.90 epoch sec; seqNo:1547 2021-02-12T23:11:32Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-12T23:11:32Z- handleDepth()-Depth_Keller msg: 1613171492907 epoch sec; seqNo:1557; depth 22.84 2021-02-12T23:11:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171492.92 epoch sec; seqNo:1557; 4056383.86 north; 590561.61 east 2021-02-12T23:11:33Z- AHRS_M2 msg: 1613171493.30 epoch sec; seqNo:1548 2021-02-12T23:11:33Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-12T23:11:33Z- handleDepth()-Depth_Keller msg: 1613171493310 epoch sec; seqNo:1558; depth 22.73 2021-02-12T23:11:33Z- handleDvl() - RDI_Pathfinder msg: 1613171493.32 epoch sec; seqNo:653 2021-02-12T23:11:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 86.08 , 95.82 , 62.53 2021-02-12T23:11:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.14 , 0.06 2021-02-12T23:11:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171493.33 epoch sec; seqNo:1558; 4056384.24 north; 590561.61 east 2021-02-12T23:11:33Z- AHRS_M2 msg: 1613171493.71 epoch sec; seqNo:1549 2021-02-12T23:11:33Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-12T23:11:33Z- handleDepth()-Depth_Keller msg: 1613171493712 epoch sec; seqNo:1559; depth 22.63 2021-02-12T23:11:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171493.72 epoch sec; seqNo:1559; 4056384.62 north; 590561.60 east 2021-02-12T23:11:34Z- AHRS_M2 msg: 1613171494.11 epoch sec; seqNo:1550 2021-02-12T23:11:34Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-12T23:11:34Z- handleDepth()-Depth_Keller msg: 1613171494118 epoch sec; seqNo:1560; depth 22.50 2021-02-12T23:11:34Z- handleDvl() - RDI_Pathfinder msg: 1613171494.13 epoch sec; seqNo:654 2021-02-12T23:11:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 86.09 , 94.74 , 63.34 2021-02-12T23:11:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.16 , -0.05 2021-02-12T23:11:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171494.14 epoch sec; seqNo:1560; 4056384.62 north; 590561.60 east 2021-02-12T23:11:34Z- AHRS_M2 msg: 1613171494.52 epoch sec; seqNo:1551 2021-02-12T23:11:34Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:34Z- handleDepth()-Depth_Keller msg: 1613171494521 epoch sec; seqNo:1561; depth 22.39 2021-02-12T23:11:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171494.53 epoch sec; seqNo:1561; 4056385.01 north; 590562.82 east 2021-02-12T23:11:34Z- AHRS_M2 msg: 1613171494.92 epoch sec; seqNo:1552 2021-02-12T23:11:34Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-12T23:11:34Z- handleDepth()-Depth_Keller msg: 1613171494924 epoch sec; seqNo:1562; depth 22.29 2021-02-12T23:11:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171494.94 epoch sec; seqNo:1562; 4056385.01 north; 590562.82 east 2021-02-12T23:11:35Z- AHRS_M2 msg: 1613171495.32 epoch sec; seqNo:1553 2021-02-12T23:11:35Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-12T23:11:35Z- handleDepth()-Depth_Keller msg: 1613171495328 epoch sec; seqNo:1563; depth 22.16 2021-02-12T23:11:35Z- handleDvl() - RDI_Pathfinder msg: 1613171495.34 epoch sec; seqNo:655 2021-02-12T23:11:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 89.33 , 97.98 , 63.75 2021-02-12T23:11:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.07 , -0.13 2021-02-12T23:11:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171495.36 epoch sec; seqNo:1563; 4056385.39 north; 590562.82 east 2021-02-12T23:11:35Z- AHRS_M2 msg: 1613171495.73 epoch sec; seqNo:1554 2021-02-12T23:11:35Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-12T23:11:35Z- handleDepth()-Depth_Keller msg: 1613171495732 epoch sec; seqNo:1564; depth 22.05 2021-02-12T23:11:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171495.74 epoch sec; seqNo:1564; 4056385.77 north; 590562.81 east 2021-02-12T23:11:36Z- AHRS_M2 msg: 1613171496.14 epoch sec; seqNo:1555 2021-02-12T23:11:36Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-12T23:11:36Z- handleDepth()-Depth_Keller msg: 1613171496141 epoch sec; seqNo:1565; depth 21.94 2021-02-12T23:11:36Z- handleDvl() - RDI_Pathfinder msg: 1613171496.15 epoch sec; seqNo:656 2021-02-12T23:11:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 89.26 , 95.75 , 64.57 2021-02-12T23:11:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , -0.12 2021-02-12T23:11:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171496.16 epoch sec; seqNo:1565; 4056385.77 north; 590562.81 east 2021-02-12T23:11:36Z- AHRS_M2 msg: 1613171496.54 epoch sec; seqNo:1556 2021-02-12T23:11:36Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-12T23:11:36Z- handleDepth()-Depth_Keller msg: 1613171496540 epoch sec; seqNo:1566; depth 21.83 2021-02-12T23:11:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171496.56 epoch sec; seqNo:1566; 4056386.15 north; 590562.81 east 2021-02-12T23:11:36Z- AHRS_M2 msg: 1613171496.94 epoch sec; seqNo:1557 2021-02-12T23:11:36Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.19 psi 2021-02-12T23:11:36Z- handleDepth()-Depth_Keller msg: 1613171496945 epoch sec; seqNo:1567; depth 21.71 2021-02-12T23:11:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171496.96 epoch sec; seqNo:1567; 4056386.52 north; 590562.80 east 2021-02-12T23:11:37Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-12T23:11:37Z- waiting for 1613171494.72, time is 1613171496.96 2021-02-12T23:11:37Z- LcmTrn::updateTrn() >>>> pose time 1613171496.96, meas time 1613171496.15 <<<< 2021-02-12T23:11:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171496.15 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:11:37Z- TerrainNav::motionUpdate() - currEstimate = (4056387, 590562.80, 21.71) 2021-02-12T23:11:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:11:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:11:37Z- TNavBF::Projecting Measurements 2021-02-12T23:11:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:11:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:11:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:11:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:11:37Z- TNavFilter::4 good beams. 2021-02-12T23:11:37Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:11:37Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:11:37Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:11:37Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:11:37Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:11:37Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:11:37Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:11:37Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:37Z- TerrainMapOctree: (vehN, vehE ) = (4056386.00, 590562.43). 2021-02-12T23:11:37Z- TerrainMapOctree: Min Distance = 6103.82. 2021-02-12T23:11:37Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:11:37Z- There is no specific good beam that all particles have in common. 2021-02-12T23:11:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:11:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 7.90 2021-02-12T23:11:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:11:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 12.75 2021-02-12T23:11:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:11:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 14.67 2021-02-12T23:11:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:11:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.74 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 6.67 2021-02-12T23:11:37Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:11:37Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:11:37Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:11:37Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:11:37Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:11:37Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:11:37Z- Filter 0 No beams used in subcloud update 2021-02-12T23:11:37Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:11:37Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:11:37Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 797.2 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:11:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171496.15 sec. 2021-02-12T23:11:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:11:37Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:37Z- variances([0], [2], and psi_berg) = 797.103553 833.096786 0.000000 2021-02-12T23:11:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:11:37Z- LcmTrn::publishEstimates() - MLE : 4056358.02 590514.30 21.71 2021-02-12T23:11:37Z- LcmTrn::publishEstimates() - MMSE : 4056386.52 590562.80 21.71 2021-02-12T23:11:37Z- LcmTrn::publishEstimates() - COVAR: 797.10 833.10 0.71 0.00 2021-02-12T23:11:37Z- AHRS_M2 msg: 1613171497.34 epoch sec; seqNo:1558 2021-02-12T23:11:37Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.19 psi 2021-02-12T23:11:37Z- handleDepth()-Depth_Keller msg: 1613171497348 epoch sec; seqNo:1568; depth 21.60 2021-02-12T23:11:37Z- handleDvl() - RDI_Pathfinder msg: 1613171497.37 epoch sec; seqNo:657 2021-02-12T23:11:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 89.33 , 97.98 , 64.57 2021-02-12T23:11:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , -0.07 2021-02-12T23:11:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171497.38 epoch sec; seqNo:1568; 4056386.54 north; 590564.03 east 2021-02-12T23:11:37Z- AHRS_M2 msg: 1613171497.75 epoch sec; seqNo:1559 2021-02-12T23:11:37Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.19 psi 2021-02-12T23:11:37Z- handleDepth()-Depth_Keller msg: 1613171497757 epoch sec; seqNo:1569; depth 21.49 2021-02-12T23:11:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171497.77 epoch sec; seqNo:1569; 4056386.92 north; 590564.02 east 2021-02-12T23:11:38Z- AHRS_M2 msg: 1613171498.15 epoch sec; seqNo:1560 2021-02-12T23:11:38Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.19 psi 2021-02-12T23:11:38Z- handleDepth()-Depth_Keller msg: 1613171498161 epoch sec; seqNo:1570; depth 21.36 2021-02-12T23:11:38Z- handleDvl() - RDI_Pathfinder msg: 1613171498.17 epoch sec; seqNo:658 2021-02-12T23:11:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 90.34 , 95.75 , 64.57 2021-02-12T23:11:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.10 , 0.00 2021-02-12T23:11:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171498.18 epoch sec; seqNo:1570; 4056387.29 north; 590564.02 east 2021-02-12T23:11:38Z- AHRS_M2 msg: 1613171498.56 epoch sec; seqNo:1561 2021-02-12T23:11:38Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.19 psi 2021-02-12T23:11:38Z- handleDepth()-Depth_Keller msg: 1613171498562 epoch sec; seqNo:1571; depth 21.26 2021-02-12T23:11:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171498.57 epoch sec; seqNo:1571; 4056387.67 north; 590564.01 east 2021-02-12T23:11:38Z- AHRS_M2 msg: 1613171498.96 epoch sec; seqNo:1562 2021-02-12T23:11:38Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:38Z- handleDepth()-Depth_Keller msg: 1613171498965 epoch sec; seqNo:1572; depth 21.14 2021-02-12T23:11:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171498.99 epoch sec; seqNo:1572; 4056387.67 north; 590564.01 east 2021-02-12T23:11:39Z- AHRS_M2 msg: 1613171499.36 epoch sec; seqNo:1563 2021-02-12T23:11:39Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:39Z- handleDepth()-Depth_Keller msg: 1613171499373 epoch sec; seqNo:1573; depth 21.04 2021-02-12T23:11:39Z- handleDvl() - RDI_Pathfinder msg: 1613171499.38 epoch sec; seqNo:659 2021-02-12T23:11:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.52 , 89.26 , 96.83 , 64.98 2021-02-12T23:11:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.09 , 0.05 2021-02-12T23:11:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171499.40 epoch sec; seqNo:1573; 4056388.05 north; 590564.01 east 2021-02-12T23:11:39Z- AHRS_M2 msg: 1613171499.77 epoch sec; seqNo:1564 2021-02-12T23:11:39Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:39Z- handleDepth()-Depth_Keller msg: 1613171499772 epoch sec; seqNo:1574; depth 20.91 2021-02-12T23:11:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171499.78 epoch sec; seqNo:1574; 4056388.43 north; 590564.01 east 2021-02-12T23:11:40Z- AHRS_M2 msg: 1613171500.17 epoch sec; seqNo:1565 2021-02-12T23:11:40Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:40Z- handleDepth()-Depth_Keller msg: 1613171500180 epoch sec; seqNo:1575; depth 20.81 2021-02-12T23:11:40Z- handleDvl() - RDI_Pathfinder msg: 1613171500.19 epoch sec; seqNo:660 2021-02-12T23:11:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.93 , 89.26 , 96.83 , 64.98 2021-02-12T23:11:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.07 , 0.07 2021-02-12T23:11:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171500.21 epoch sec; seqNo:1575; 4056388.81 north; 590564.00 east 2021-02-12T23:11:40Z- AHRS_M2 msg: 1613171500.58 epoch sec; seqNo:1566 2021-02-12T23:11:40Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:40Z- handleDepth()-Depth_Keller msg: 1613171500580 epoch sec; seqNo:1576; depth 20.70 2021-02-12T23:11:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171500.60 epoch sec; seqNo:1576; 4056388.82 north; 590565.22 east 2021-02-12T23:11:41Z- AHRS_M2 msg: 1613171501.01 epoch sec; seqNo:1567 2021-02-12T23:11:41Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:41Z- handleDepth()-Depth_Keller msg: 1613171501016 epoch sec; seqNo:1577; depth 20.59 2021-02-12T23:11:41Z- handleDvl() - RDI_Pathfinder msg: 1613171501.03 epoch sec; seqNo:661 2021-02-12T23:11:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 0.00 , 97.91 , 65.39 2021-02-12T23:11:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.02 , 0.07 2021-02-12T23:11:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171501.04 epoch sec; seqNo:1577; 4056389.20 north; 590565.22 east 2021-02-12T23:11:41Z- AHRS_M2 msg: 1613171501.38 epoch sec; seqNo:1568 2021-02-12T23:11:41Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:41Z- handleDepth()-Depth_Keller msg: 1613171501388 epoch sec; seqNo:1578; depth 20.47 2021-02-12T23:11:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171501.40 epoch sec; seqNo:1578; 4056389.58 north; 590565.21 east 2021-02-12T23:11:41Z- AHRS_M2 msg: 1613171501.79 epoch sec; seqNo:1569 2021-02-12T23:11:41Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:41Z- handleDepth()-Depth_Keller msg: 1613171501792 epoch sec; seqNo:1579; depth 20.37 2021-02-12T23:11:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171501.80 epoch sec; seqNo:1579; 4056389.96 north; 590565.21 east 2021-02-12T23:11:42Z- AHRS_M2 msg: 1613171502.19 epoch sec; seqNo:1570 2021-02-12T23:11:42Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:42Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-12T23:11:42Z- waiting for 1613171499.96, time is 1613171502.19 2021-02-12T23:11:42Z- LcmTrn::updateTrn() >>>> pose time 1613171502.19, meas time 1613171501.03 <<<< 2021-02-12T23:11:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171501.03 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:11:42Z- TerrainNav::motionUpdate() - currEstimate = (4056390, 590565.21, 20.37) 2021-02-12T23:11:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:11:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:11:42Z- TNavBF::Projecting Measurements 2021-02-12T23:11:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:11:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:11:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:11:42Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:11:42Z- TNavFilter::3 good beams. 2021-02-12T23:11:42Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:11:42Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:11:42Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:11:42Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:11:42Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:11:42Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:11:42Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:11:42Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:42Z- TerrainMapOctree: (vehN, vehE ) = (4056389.20, 590564.68). 2021-02-12T23:11:42Z- TerrainMapOctree: Min Distance = 6099.92. 2021-02-12T23:11:42Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:11:42Z- There is no specific good beam that all particles have in common. 2021-02-12T23:11:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:11:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-12T23:11:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:11:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-12T23:11:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:11:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.77 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-12T23:11:42Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:11:42Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:11:42Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:11:42Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:11:42Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:11:42Z- Filter 0 No beams used in subcloud update 2021-02-12T23:11:42Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:11:42Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:11:42Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 796.9 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:11:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171501.03 sec. 2021-02-12T23:11:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:11:42Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:42Z- variances([0], [2], and psi_berg) = 796.810611 833.096194 0.000000 2021-02-12T23:11:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:11:42Z- LcmTrn::publishEstimates() - MLE : 4056361.46 590516.71 20.37 2021-02-12T23:11:42Z- LcmTrn::publishEstimates() - MMSE : 4056389.96 590565.21 20.37 2021-02-12T23:11:42Z- LcmTrn::publishEstimates() - COVAR: 796.81 833.10 0.76 0.00 2021-02-12T23:11:42Z- handleDepth()-Depth_Keller msg: 1613171502196 epoch sec; seqNo:1580; depth 20.26 2021-02-12T23:11:42Z- handleDvl() - RDI_Pathfinder msg: 1613171502.21 epoch sec; seqNo:662 2021-02-12T23:11:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 89.33 , 96.90 , 65.79 2021-02-12T23:11:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , 0.08 2021-02-12T23:11:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171502.22 epoch sec; seqNo:1580; 4056389.96 north; 590565.21 east 2021-02-12T23:11:42Z- AHRS_M2 msg: 1613171502.60 epoch sec; seqNo:1571 2021-02-12T23:11:42Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:42Z- handleDepth()-Depth_Keller msg: 1613171502609 epoch sec; seqNo:1581; depth 20.16 2021-02-12T23:11:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171502.62 epoch sec; seqNo:1581; 4056390.34 north; 590565.21 east 2021-02-12T23:11:43Z- AHRS_M2 msg: 1613171503.00 epoch sec; seqNo:1572 2021-02-12T23:11:43Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:43Z- handleDepth()-Depth_Keller msg: 1613171503032 epoch sec; seqNo:1582; depth 20.05 2021-02-12T23:11:43Z- handleDvl() - RDI_Pathfinder msg: 1613171503.05 epoch sec; seqNo:663 2021-02-12T23:11:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.74 , 90.28 , 98.93 , 66.20 2021-02-12T23:11:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.07 2021-02-12T23:11:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171503.07 epoch sec; seqNo:1582; 4056390.72 north; 590565.20 east 2021-02-12T23:11:43Z- handleDepth()-Depth_Keller msg: 1613171503426 epoch sec; seqNo:1583; depth 19.92 2021-02-12T23:11:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171503.44 epoch sec; seqNo:1583; 4056391.11 north; 590566.42 east 2021-02-12T23:11:43Z- AHRS_M2 msg: 1613171503.81 epoch sec; seqNo:1573 2021-02-12T23:11:43Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:43Z- handleDepth()-Depth_Keller msg: 1613171503813 epoch sec; seqNo:1584; depth 19.81 2021-02-12T23:11:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171503.82 epoch sec; seqNo:1584; 4056391.11 north; 590566.42 east 2021-02-12T23:11:44Z- AHRS_M2 msg: 1613171504.21 epoch sec; seqNo:1574 2021-02-12T23:11:44Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-12T23:11:44Z- handleDepth()-Depth_Keller msg: 1613171504218 epoch sec; seqNo:1585; depth 19.69 2021-02-12T23:11:44Z- handleDvl() - RDI_Pathfinder msg: 1613171504.23 epoch sec; seqNo:664 2021-02-12T23:11:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.74 , 90.21 , 97.78 , 66.20 2021-02-12T23:11:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , 0.06 2021-02-12T23:11:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171504.24 epoch sec; seqNo:1585; 4056391.49 north; 590566.42 east 2021-02-12T23:11:44Z- AHRS_M2 msg: 1613171504.62 epoch sec; seqNo:1575 2021-02-12T23:11:44Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.17 psi 2021-02-12T23:11:44Z- handleDepth()-Depth_Keller msg: 1613171504624 epoch sec; seqNo:1586; depth 19.59 2021-02-12T23:11:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171504.64 epoch sec; seqNo:1586; 4056391.87 north; 590566.41 east 2021-02-12T23:11:45Z- AHRS_M2 msg: 1613171505.07 epoch sec; seqNo:1576 2021-02-12T23:11:45Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.16 psi 2021-02-12T23:11:45Z- handleDepth()-Depth_Keller msg: 1613171505070 epoch sec; seqNo:1587; depth 19.48 2021-02-12T23:11:45Z- handleDvl() - RDI_Pathfinder msg: 1613171505.09 epoch sec; seqNo:665 2021-02-12T23:11:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 91.29 , 96.70 , 66.61 2021-02-12T23:11:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , 0.07 2021-02-12T23:11:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171505.13 epoch sec; seqNo:1587; 4056392.25 north; 590566.41 east 2021-02-12T23:11:45Z- AHRS_M2 msg: 1613171505.42 epoch sec; seqNo:1577 2021-02-12T23:11:45Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.16 psi 2021-02-12T23:11:45Z- handleDepth()-Depth_Keller msg: 1613171505430 epoch sec; seqNo:1588; depth 19.34 2021-02-12T23:11:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171505.44 epoch sec; seqNo:1588; 4056392.25 north; 590566.41 east 2021-02-12T23:11:45Z- AHRS_M2 msg: 1613171505.83 epoch sec; seqNo:1578 2021-02-12T23:11:45Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-12T23:11:45Z- handleDepth()-Depth_Keller msg: 1613171505838 epoch sec; seqNo:1589; depth 19.26 2021-02-12T23:11:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171505.85 epoch sec; seqNo:1589; 4056392.62 north; 590566.40 east 2021-02-12T23:11:46Z- AHRS_M2 msg: 1613171506.23 epoch sec; seqNo:1579 2021-02-12T23:11:46Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.14 psi 2021-02-12T23:11:46Z- handleDepth()-Depth_Keller msg: 1613171506237 epoch sec; seqNo:1590; depth 19.13 2021-02-12T23:11:46Z- handleDvl() - RDI_Pathfinder msg: 1613171506.25 epoch sec; seqNo:666 2021-02-12T23:11:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.38 , 90.21 , 101.02 , 66.21 2021-02-12T23:11:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , 0.05 2021-02-12T23:11:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171506.26 epoch sec; seqNo:1590; 4056393.02 north; 590567.62 east 2021-02-12T23:11:46Z- AHRS_M2 msg: 1613171506.63 epoch sec; seqNo:1580 2021-02-12T23:11:46Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.14 psi 2021-02-12T23:11:46Z- handleDepth()-Depth_Keller msg: 1613171506645 epoch sec; seqNo:1591; depth 19.02 2021-02-12T23:11:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171506.66 epoch sec; seqNo:1591; 4056393.39 north; 590567.62 east 2021-02-12T23:11:47Z- AHRS_M2 msg: 1613171507.04 epoch sec; seqNo:1581 2021-02-12T23:11:47Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.13 psi 2021-02-12T23:11:47Z- handleDepth()-Depth_Keller msg: 1613171507044 epoch sec; seqNo:1592; depth 18.92 2021-02-12T23:11:47Z- handleDvl() - RDI_Pathfinder msg: 1613171507.06 epoch sec; seqNo:667 2021-02-12T23:11:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.20 , 88.65 , 97.17 , 66.62 2021-02-12T23:11:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.07 , 0.01 2021-02-12T23:11:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171507.09 epoch sec; seqNo:1592; 4056393.39 north; 590567.62 east 2021-02-12T23:11:47Z- LcmTrn::handleMessages() - handling 43 messages took 0.72 ms and 129 calls... 2021-02-12T23:11:47Z- waiting for 1613171505.19, time is 1613171507.09 2021-02-12T23:11:47Z- LcmTrn::updateTrn() equating pose 1613171507.09 and meas times 1613171507.09 2021-02-12T23:11:47Z- LcmTrn::updateTrn() >>>> pose time 1613171507.09, meas time 1613171507.09 <<<< 2021-02-12T23:11:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171507.09 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:11:47Z- TerrainNav::motionUpdate() - currEstimate = (4056393, 590567.62, 18.92) 2021-02-12T23:11:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:11:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:11:47Z- TNavBF::Projecting Measurements 2021-02-12T23:11:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:11:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:11:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:11:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:11:47Z- TNavFilter::4 good beams. 2021-02-12T23:11:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:11:47Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:11:47Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:11:47Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:11:47Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:11:47Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:11:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:11:47Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:47Z- TerrainMapOctree: (vehN, vehE ) = (4056393.39, 590567.62). 2021-02-12T23:11:47Z- TerrainMapOctree: Min Distance = 6094.80. 2021-02-12T23:11:47Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:11:47Z- There is no specific good beam that all particles have in common. 2021-02-12T23:11:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:11:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 9.05 2021-02-12T23:11:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:11:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 12.57 2021-02-12T23:11:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:11:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 15.11 2021-02-12T23:11:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:11:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 7.10 2021-02-12T23:11:47Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:11:47Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:11:47Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:11:47Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:11:47Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:11:47Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:11:47Z- Filter 0 No beams used in subcloud update 2021-02-12T23:11:47Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:11:47Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:11:47Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 796.6 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:11:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171507.09 sec. 2021-02-12T23:11:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:11:47Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:47Z- variances([0], [2], and psi_berg) = 796.595201 833.095762 0.000000 2021-02-12T23:11:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:11:47Z- LcmTrn::publishEstimates() - MLE : 4056364.89 590519.12 18.92 2021-02-12T23:11:47Z- LcmTrn::publishEstimates() - MMSE : 4056393.39 590567.62 18.92 2021-02-12T23:11:47Z- LcmTrn::publishEstimates() - COVAR: 796.60 833.10 0.73 0.00 2021-02-12T23:11:47Z- AHRS_M2 msg: 1613171507.45 epoch sec; seqNo:1582 2021-02-12T23:11:47Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.13 psi 2021-02-12T23:11:47Z- handleDepth()-Depth_Keller msg: 1613171507457 epoch sec; seqNo:1593; depth 18.83 2021-02-12T23:11:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171507.48 epoch sec; seqNo:1593; 4056393.77 north; 590567.61 east 2021-02-12T23:11:47Z- AHRS_M2 msg: 1613171507.85 epoch sec; seqNo:1583 2021-02-12T23:11:47Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.12 psi 2021-02-12T23:11:47Z- handleDepth()-Depth_Keller msg: 1613171507852 epoch sec; seqNo:1594; depth 18.70 2021-02-12T23:11:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171507.87 epoch sec; seqNo:1594; 4056394.15 north; 590567.61 east 2021-02-12T23:11:48Z- AHRS_M2 msg: 1613171508.25 epoch sec; seqNo:1584 2021-02-12T23:11:48Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.12 psi 2021-02-12T23:11:48Z- handleDepth()-Depth_Keller msg: 1613171508261 epoch sec; seqNo:1595; depth 18.59 2021-02-12T23:11:48Z- handleDvl() - RDI_Pathfinder msg: 1613171508.28 epoch sec; seqNo:668 2021-02-12T23:11:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.01 , 88.05 , 101.03 , 66.61 2021-02-12T23:11:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.09 , -0.02 2021-02-12T23:11:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171508.29 epoch sec; seqNo:1595; 4056394.17 north; 590568.83 east 2021-02-12T23:11:48Z- AHRS_M2 msg: 1613171508.66 epoch sec; seqNo:1585 2021-02-12T23:11:48Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.12 psi 2021-02-12T23:11:48Z- handleDepth()-Depth_Keller msg: 1613171508660 epoch sec; seqNo:1596; depth 18.46 2021-02-12T23:11:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171508.67 epoch sec; seqNo:1596; 4056394.54 north; 590568.83 east 2021-02-12T23:11:49Z- AHRS_M2 msg: 1613171509.06 epoch sec; seqNo:1586 2021-02-12T23:11:49Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.12 psi 2021-02-12T23:11:49Z- handleDepth()-Depth_Keller msg: 1613171509069 epoch sec; seqNo:1597; depth 18.35 2021-02-12T23:11:49Z- handleDvl() - RDI_Pathfinder msg: 1613171509.08 epoch sec; seqNo:669 2021-02-12T23:11:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.42 , 88.65 , 98.39 , 65.79 2021-02-12T23:11:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.06 , -0.06 2021-02-12T23:11:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171509.09 epoch sec; seqNo:1597; 4056394.54 north; 590568.83 east 2021-02-12T23:11:49Z- AHRS_M2 msg: 1613171509.46 epoch sec; seqNo:1587 2021-02-12T23:11:49Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.12 psi 2021-02-12T23:11:49Z- handleDepth()-Depth_Keller msg: 1613171509468 epoch sec; seqNo:1598; depth 18.25 2021-02-12T23:11:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171509.49 epoch sec; seqNo:1598; 4056394.92 north; 590568.82 east 2021-02-12T23:11:49Z- AHRS_M2 msg: 1613171509.87 epoch sec; seqNo:1588 2021-02-12T23:11:49Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.12 psi 2021-02-12T23:11:49Z- handleDepth()-Depth_Keller msg: 1613171509877 epoch sec; seqNo:1599; depth 18.14 2021-02-12T23:11:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171509.90 epoch sec; seqNo:1599; 4056395.30 north; 590568.82 east 2021-02-12T23:11:50Z- AHRS_M2 msg: 1613171510.27 epoch sec; seqNo:1589 2021-02-12T23:11:50Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.12 psi 2021-02-12T23:11:50Z- handleDepth()-Depth_Keller msg: 1613171510276 epoch sec; seqNo:1600; depth 18.02 2021-02-12T23:11:50Z- handleDvl() - RDI_Pathfinder msg: 1613171510.29 epoch sec; seqNo:670 2021-02-12T23:11:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 89.12 , 96.70 , 67.43 2021-02-12T23:11:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.01 , -0.07 2021-02-12T23:11:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171510.30 epoch sec; seqNo:1600; 4056395.30 north; 590568.82 east 2021-02-12T23:11:50Z- AHRS_M2 msg: 1613171510.67 epoch sec; seqNo:1590 2021-02-12T23:11:50Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.13 psi 2021-02-12T23:11:50Z- handleDepth()-Depth_Keller msg: 1613171510682 epoch sec; seqNo:1601; depth 17.92 2021-02-12T23:11:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171510.69 epoch sec; seqNo:1601; 4056395.68 north; 590568.81 east 2021-02-12T23:11:51Z- AHRS_M2 msg: 1613171511.08 epoch sec; seqNo:1591 2021-02-12T23:11:51Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.13 psi 2021-02-12T23:11:51Z- handleDepth()-Depth_Keller msg: 1613171511089 epoch sec; seqNo:1602; depth 17.80 2021-02-12T23:11:51Z- handleDvl() - RDI_Pathfinder msg: 1613171511.11 epoch sec; seqNo:671 2021-02-12T23:11:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.02 , 89.87 , 99.60 , 67.84 2021-02-12T23:11:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , -0.07 2021-02-12T23:11:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171511.12 epoch sec; seqNo:1602; 4056396.07 north; 590570.03 east 2021-02-12T23:11:51Z- AHRS_M2 msg: 1613171511.49 epoch sec; seqNo:1592 2021-02-12T23:11:51Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.14 psi 2021-02-12T23:11:51Z- handleDepth()-Depth_Keller msg: 1613171511497 epoch sec; seqNo:1603; depth 17.68 2021-02-12T23:11:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171511.51 epoch sec; seqNo:1603; 4056396.07 north; 590570.03 east 2021-02-12T23:11:51Z- AHRS_M2 msg: 1613171511.89 epoch sec; seqNo:1593 2021-02-12T23:11:51Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-12T23:11:51Z- handleDepth()-Depth_Keller msg: 1613171511894 epoch sec; seqNo:1604; depth 17.57 2021-02-12T23:11:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:11:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:11:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:11:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:11:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:11:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:11:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:11:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:11:51Z- handleDvl() - RDI_Pathfinder msg: 1613171511.90 epoch sec; seqNo:672 2021-02-12T23:11:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:11:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , -0.07 2021-02-12T23:11:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171511.90 epoch sec; seqNo:1604; 4056396.45 north; 590570.03 east 2021-02-12T23:11:52Z- AHRS_M2 msg: 1613171512.29 epoch sec; seqNo:1594 2021-02-12T23:11:52Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.15 psi 2021-02-12T23:11:52Z- handleDepth()-Depth_Keller msg: 1613171512296 epoch sec; seqNo:1605; depth 17.48 2021-02-12T23:11:52Z- handleDvl() - RDI_Pathfinder msg: 1613171512.31 epoch sec; seqNo:673 2021-02-12T23:11:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.01 , 91.02 , 97.11 , 68.24 2021-02-12T23:11:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.14 , -0.07 2021-02-12T23:11:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171512.33 epoch sec; seqNo:1605; 4056396.83 north; 590570.02 east 2021-02-12T23:11:52Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-12T23:11:52Z- waiting for 1613171510.09, time is 1613171512.33 2021-02-12T23:11:52Z- LcmTrn::updateTrn() equating pose 1613171512.33 and meas times 1613171512.33 2021-02-12T23:11:52Z- LcmTrn::updateTrn() >>>> pose time 1613171512.33, meas time 1613171512.33 <<<< 2021-02-12T23:11:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171512.33 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:11:52Z- TerrainNav::motionUpdate() - currEstimate = (4056397, 590570.02, 17.48) 2021-02-12T23:11:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:11:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:11:52Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171512.33 sec.; No relevant navigation data available 2021-02-12T23:11:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:11:52Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:52Z- variances([0], [2], and psi_berg) = 796.380780 833.095333 0.000000 2021-02-12T23:11:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:11:52Z- LcmTrn::publishEstimates() - MLE : 4056368.33 590521.52 17.48 2021-02-12T23:11:52Z- LcmTrn::publishEstimates() - MMSE : 4056396.83 590570.02 17.48 2021-02-12T23:11:52Z- LcmTrn::publishEstimates() - COVAR: 796.38 833.10 0.95 0.00 2021-02-12T23:11:52Z- AHRS_M2 msg: 1613171512.70 epoch sec; seqNo:1595 2021-02-12T23:11:52Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.15 psi 2021-02-12T23:11:52Z- handleDepth()-Depth_Keller msg: 1613171512700 epoch sec; seqNo:1606; depth 17.36 2021-02-12T23:11:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171512.71 epoch sec; seqNo:1606; 4056397.21 north; 590570.02 east 2021-02-12T23:11:53Z- AHRS_M2 msg: 1613171513.10 epoch sec; seqNo:1596 2021-02-12T23:11:53Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-12T23:11:53Z- handleDepth()-Depth_Keller msg: 1613171513109 epoch sec; seqNo:1607; depth 17.25 2021-02-12T23:11:53Z- handleDvl() - RDI_Pathfinder msg: 1613171513.12 epoch sec; seqNo:674 2021-02-12T23:11:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.01 , 89.87 , 99.61 , 68.24 2021-02-12T23:11:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.17 , -0.02 2021-02-12T23:11:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171513.14 epoch sec; seqNo:1607; 4056397.21 north; 590570.02 east 2021-02-12T23:11:53Z- AHRS_M2 msg: 1613171513.50 epoch sec; seqNo:1597 2021-02-12T23:11:53Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-12T23:11:53Z- handleDepth()-Depth_Keller msg: 1613171513510 epoch sec; seqNo:1608; depth 17.15 2021-02-12T23:11:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171513.52 epoch sec; seqNo:1608; 4056397.59 north; 590570.01 east 2021-02-12T23:11:53Z- AHRS_M2 msg: 1613171513.91 epoch sec; seqNo:1598 2021-02-12T23:11:53Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-12T23:11:53Z- handleDepth()-Depth_Keller msg: 1613171513917 epoch sec; seqNo:1609; depth 17.03 2021-02-12T23:11:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171513.92 epoch sec; seqNo:1609; 4056397.97 north; 590570.01 east 2021-02-12T23:11:54Z- AHRS_M2 msg: 1613171514.31 epoch sec; seqNo:1599 2021-02-12T23:11:54Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-12T23:11:54Z- handleDepth()-Depth_Keller msg: 1613171514316 epoch sec; seqNo:1610; depth 16.90 2021-02-12T23:11:54Z- handleDvl() - RDI_Pathfinder msg: 1613171514.33 epoch sec; seqNo:675 2021-02-12T23:11:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.42 , 92.24 , 101.97 , 67.02 2021-02-12T23:11:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.15 , 0.07 2021-02-12T23:11:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171514.34 epoch sec; seqNo:1610; 4056398.36 north; 590571.23 east 2021-02-12T23:11:54Z- AHRS_M2 msg: 1613171514.72 epoch sec; seqNo:1600 2021-02-12T23:11:54Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:11:54Z- handleDepth()-Depth_Keller msg: 1613171514721 epoch sec; seqNo:1611; depth 16.80 2021-02-12T23:11:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171514.74 epoch sec; seqNo:1611; 4056398.74 north; 590571.22 east 2021-02-12T23:11:55Z- AHRS_M2 msg: 1613171515.12 epoch sec; seqNo:1601 2021-02-12T23:11:55Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:11:55Z- handleDepth()-Depth_Keller msg: 1613171515127 epoch sec; seqNo:1612; depth 16.69 2021-02-12T23:11:55Z- handleDvl() - RDI_Pathfinder msg: 1613171515.15 epoch sec; seqNo:676 2021-02-12T23:11:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.42 , 92.17 , 97.04 , 68.65 2021-02-12T23:11:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , 0.15 2021-02-12T23:11:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171515.16 epoch sec; seqNo:1612; 4056398.74 north; 590571.22 east 2021-02-12T23:11:55Z- AHRS_M2 msg: 1613171515.52 epoch sec; seqNo:1602 2021-02-12T23:11:55Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:11:55Z- handleDepth()-Depth_Keller msg: 1613171515528 epoch sec; seqNo:1613; depth 16.58 2021-02-12T23:11:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:11:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:11:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:11:55Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:11:55Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:11:55Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:11:55Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:11:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:11:55Z- handleDvl() - RDI_Pathfinder msg: 1613171515.53 epoch sec; seqNo:677 2021-02-12T23:11:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:11:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , 0.15 2021-02-12T23:11:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171515.54 epoch sec; seqNo:1613; 4056399.12 north; 590571.22 east 2021-02-12T23:11:55Z- AHRS_M2 msg: 1613171515.93 epoch sec; seqNo:1603 2021-02-12T23:11:55Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:11:55Z- handleDepth()-Depth_Keller msg: 1613171515934 epoch sec; seqNo:1614; depth 16.47 2021-02-12T23:11:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171515.94 epoch sec; seqNo:1614; 4056399.50 north; 590571.22 east 2021-02-12T23:11:56Z- AHRS_M2 msg: 1613171516.33 epoch sec; seqNo:1604 2021-02-12T23:11:56Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:11:56Z- handleDepth()-Depth_Keller msg: 1613171516341 epoch sec; seqNo:1615; depth 16.37 2021-02-12T23:11:56Z- handleDvl() - RDI_Pathfinder msg: 1613171516.36 epoch sec; seqNo:678 2021-02-12T23:11:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 92.24 , 100.75 , 68.65 2021-02-12T23:11:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , 0.13 2021-02-12T23:11:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171516.37 epoch sec; seqNo:1615; 4056399.87 north; 590571.21 east 2021-02-12T23:11:56Z- AHRS_M2 msg: 1613171516.73 epoch sec; seqNo:1605 2021-02-12T23:11:56Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:11:56Z- handleDepth()-Depth_Keller msg: 1613171516744 epoch sec; seqNo:1616; depth 16.26 2021-02-12T23:11:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:11:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:11:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:11:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:11:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:11:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:11:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:11:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:11:56Z- handleDvl() - RDI_Pathfinder msg: 1613171516.75 epoch sec; seqNo:679 2021-02-12T23:11:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:11:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , 0.13 2021-02-12T23:11:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171516.76 epoch sec; seqNo:1616; 4056399.87 north; 590571.21 east 2021-02-12T23:11:57Z- AHRS_M2 msg: 1613171517.14 epoch sec; seqNo:1606 2021-02-12T23:11:57Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:11:57Z- handleDepth()-Depth_Keller msg: 1613171517148 epoch sec; seqNo:1617; depth 16.15 2021-02-12T23:11:57Z- handleDvl() - RDI_Pathfinder msg: 1613171517.16 epoch sec; seqNo:680 2021-02-12T23:11:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 90.95 , 101.90 , 69.06 2021-02-12T23:11:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , 0.06 2021-02-12T23:11:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171517.17 epoch sec; seqNo:1617; 4056400.27 north; 590572.43 east 2021-02-12T23:11:57Z- LcmTrn::handleMessages() - handling 43 messages took 0.68 ms and 130 calls... 2021-02-12T23:11:57Z- waiting for 1613171515.33, time is 1613171517.17 2021-02-12T23:11:57Z- LcmTrn::updateTrn() equating pose 1613171517.17 and meas times 1613171517.17 2021-02-12T23:11:57Z- LcmTrn::updateTrn() >>>> pose time 1613171517.17, meas time 1613171517.17 <<<< 2021-02-12T23:11:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171517.17 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:11:57Z- TerrainNav::motionUpdate() - currEstimate = (4056400, 590572.43, 16.15) 2021-02-12T23:11:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:11:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:11:57Z- TNavBF::Projecting Measurements 2021-02-12T23:11:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:11:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:11:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:11:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:11:57Z- TNavFilter::4 good beams. 2021-02-12T23:11:57Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:11:57Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:11:57Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:11:57Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:11:57Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:11:57Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:11:57Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:11:57Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:57Z- TerrainMapOctree: (vehN, vehE ) = (4056400.27, 590572.43). 2021-02-12T23:11:57Z- TerrainMapOctree: Min Distance = 6086.42. 2021-02-12T23:11:57Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:11:57Z- There is no specific good beam that all particles have in common. 2021-02-12T23:11:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:11:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 9.44 2021-02-12T23:11:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:11:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 13.24 2021-02-12T23:11:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:11:57Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 16.62 2021-02-12T23:11:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:11:57Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-12T23:11:57Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:11:57Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:11:57Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:11:57Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:11:57Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:11:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:11:57Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:11:57Z- Filter 0 No beams used in subcloud update 2021-02-12T23:11:57Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:11:57Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:11:57Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 796.2 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:11:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171517.17 sec. 2021-02-12T23:11:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:11:57Z- TNavBF::mmse selected from filter -1 2021-02-12T23:11:57Z- variances([0], [2], and psi_berg) = 796.167334 833.094909 0.000000 2021-02-12T23:11:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:11:57Z- LcmTrn::publishEstimates() - MLE : 4056371.77 590523.93 16.15 2021-02-12T23:11:57Z- LcmTrn::publishEstimates() - MMSE : 4056400.27 590572.43 16.15 2021-02-12T23:11:57Z- LcmTrn::publishEstimates() - COVAR: 796.17 833.09 0.85 0.00 2021-02-12T23:11:57Z- AHRS_M2 msg: 1613171517.55 epoch sec; seqNo:1607 2021-02-12T23:11:57Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-12T23:11:57Z- handleDepth()-Depth_Keller msg: 1613171517557 epoch sec; seqNo:1618; depth 16.05 2021-02-12T23:11:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171517.57 epoch sec; seqNo:1618; 4056400.64 north; 590572.42 east 2021-02-12T23:11:57Z- AHRS_M2 msg: 1613171517.95 epoch sec; seqNo:1608 2021-02-12T23:11:57Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-12T23:11:57Z- handleDepth()-Depth_Keller msg: 1613171517958 epoch sec; seqNo:1619; depth 15.92 2021-02-12T23:11:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:11:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:11:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:11:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:11:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:11:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:11:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:11:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:11:57Z- handleDvl() - RDI_Pathfinder msg: 1613171517.97 epoch sec; seqNo:681 2021-02-12T23:11:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:11:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , 0.06 2021-02-12T23:11:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171517.98 epoch sec; seqNo:1619; 4056400.64 north; 590572.42 east 2021-02-12T23:11:58Z- AHRS_M2 msg: 1613171518.35 epoch sec; seqNo:1609 2021-02-12T23:11:58Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-12T23:11:58Z- handleDepth()-Depth_Keller msg: 1613171518356 epoch sec; seqNo:1620; depth 15.81 2021-02-12T23:11:58Z- handleDvl() - RDI_Pathfinder msg: 1613171518.37 epoch sec; seqNo:682 2021-02-12T23:11:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.64 , 98.25 , 104.34 , 69.88 2021-02-12T23:11:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.05 , -0.01 2021-02-12T23:11:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171518.39 epoch sec; seqNo:1620; 4056401.02 north; 590572.42 east 2021-02-12T23:11:58Z- AHRS_M2 msg: 1613171518.76 epoch sec; seqNo:1610 2021-02-12T23:11:58Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-12T23:11:58Z- handleDepth()-Depth_Keller msg: 1613171518769 epoch sec; seqNo:1621; depth 15.70 2021-02-12T23:11:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171518.78 epoch sec; seqNo:1621; 4056401.40 north; 590572.42 east 2021-02-12T23:11:59Z- AHRS_M2 msg: 1613171519.16 epoch sec; seqNo:1611 2021-02-12T23:11:59Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-12T23:11:59Z- handleDepth()-Depth_Keller msg: 1613171519166 epoch sec; seqNo:1622; depth 15.60 2021-02-12T23:11:59Z- handleDvl() - RDI_Pathfinder msg: 1613171519.18 epoch sec; seqNo:683 2021-02-12T23:11:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.05 , 94.33 , 102.85 , 69.47 2021-02-12T23:11:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.01 2021-02-12T23:11:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171519.19 epoch sec; seqNo:1622; 4056401.79 north; 590573.63 east 2021-02-12T23:11:59Z- AHRS_M2 msg: 1613171519.56 epoch sec; seqNo:1612 2021-02-12T23:11:59Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:11:59Z- handleDepth()-Depth_Keller msg: 1613171519569 epoch sec; seqNo:1623; depth 15.48 2021-02-12T23:11:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171519.59 epoch sec; seqNo:1623; 4056401.79 north; 590573.63 east 2021-02-12T23:11:59Z- AHRS_M2 msg: 1613171519.97 epoch sec; seqNo:1613 2021-02-12T23:11:59Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:11:59Z- handleDepth()-Depth_Keller msg: 1613171519978 epoch sec; seqNo:1624; depth 15.36 2021-02-12T23:12:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171520.02 epoch sec; seqNo:1624; 4056402.17 north; 590573.63 east 2021-02-12T23:12:00Z- AHRS_M2 msg: 1613171520.37 epoch sec; seqNo:1614 2021-02-12T23:12:00Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:12:00Z- handleDepth()-Depth_Keller msg: 1613171520376 epoch sec; seqNo:1625; depth 15.25 2021-02-12T23:12:00Z- handleDvl() - RDI_Pathfinder msg: 1613171520.39 epoch sec; seqNo:684 2021-02-12T23:12:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.46 , 94.47 , 96.90 , 69.88 2021-02-12T23:12:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.02 , 0.04 2021-02-12T23:12:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171520.40 epoch sec; seqNo:1625; 4056402.55 north; 590573.63 east 2021-02-12T23:12:00Z- AHRS_M2 msg: 1613171520.78 epoch sec; seqNo:1615 2021-02-12T23:12:00Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:12:00Z- handleDepth()-Depth_Keller msg: 1613171520793 epoch sec; seqNo:1626; depth 15.14 2021-02-12T23:12:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171520.81 epoch sec; seqNo:1626; 4056402.93 north; 590573.62 east 2021-02-12T23:12:01Z- AHRS_M2 msg: 1613171521.20 epoch sec; seqNo:1616 2021-02-12T23:12:01Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:12:01Z- handleDepth()-Depth_Keller msg: 1613171521211 epoch sec; seqNo:1627; depth 15.01 2021-02-12T23:12:01Z- handleDvl() - RDI_Pathfinder msg: 1613171521.23 epoch sec; seqNo:685 2021-02-12T23:12:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.46 , 94.54 , 105.49 , 70.29 2021-02-12T23:12:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.04 , 0.03 2021-02-12T23:12:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171521.24 epoch sec; seqNo:1627; 4056402.93 north; 590573.62 east 2021-02-12T23:12:01Z- handleDepth()-Depth_Keller msg: 1613171521605 epoch sec; seqNo:1628; depth 14.90 2021-02-12T23:12:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:12:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:12:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:12:01Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:12:01Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:12:01Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:12:01Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:12:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:12:01Z- handleDvl() - RDI_Pathfinder msg: 1613171521.61 epoch sec; seqNo:686 2021-02-12T23:12:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:12:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.04 , 0.03 2021-02-12T23:12:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171521.63 epoch sec; seqNo:1628; 4056403.32 north; 590574.84 east 2021-02-12T23:12:02Z- AHRS_M2 msg: 1613171521.99 epoch sec; seqNo:1617 2021-02-12T23:12:02Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.15 psi 2021-02-12T23:12:02Z- handleDepth()-Depth_Keller msg: 1613171522005 epoch sec; seqNo:1629; depth 14.78 2021-02-12T23:12:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171522.01 epoch sec; seqNo:1629; 4056403.32 north; 590574.84 east 2021-02-12T23:12:02Z- AHRS_M2 msg: 1613171522.39 epoch sec; seqNo:1618 2021-02-12T23:12:02Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.15 psi 2021-02-12T23:12:02Z- handleDepth()-Depth_Keller msg: 1613171522399 epoch sec; seqNo:1630; depth 14.68 2021-02-12T23:12:02Z- handleDvl() - RDI_Pathfinder msg: 1613171522.42 epoch sec; seqNo:687 2021-02-12T23:12:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.28 , 94.33 , 105.28 , 71.11 2021-02-12T23:12:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.04 , -0.01 2021-02-12T23:12:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171522.43 epoch sec; seqNo:1630; 4056403.70 north; 590574.83 east 2021-02-12T23:12:02Z- LcmTrn::handleMessages() - handling 45 messages took 0.72 ms and 131 calls... 2021-02-12T23:12:02Z- waiting for 1613171520.17, time is 1613171522.43 2021-02-12T23:12:02Z- LcmTrn::updateTrn() equating pose 1613171522.43 and meas times 1613171522.43 2021-02-12T23:12:02Z- LcmTrn::updateTrn() >>>> pose time 1613171522.43, meas time 1613171522.43 <<<< 2021-02-12T23:12:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171522.43 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:12:02Z- TerrainNav::motionUpdate() - currEstimate = (4056404, 590574.83, 14.68) 2021-02-12T23:12:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:12:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:12:02Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171522.43 sec.; No relevant navigation data available 2021-02-12T23:12:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:12:02Z- TNavBF::mmse selected from filter -1 2021-02-12T23:12:02Z- variances([0], [2], and psi_berg) = 795.954852 833.094488 0.000000 2021-02-12T23:12:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:12:02Z- LcmTrn::publishEstimates() - MLE : 4056375.20 590528.33 14.68 2021-02-12T23:12:02Z- LcmTrn::publishEstimates() - MMSE : 4056403.70 590574.83 14.68 2021-02-12T23:12:02Z- LcmTrn::publishEstimates() - COVAR: 795.95 833.09 1.06 0.00 2021-02-12T23:12:02Z- AHRS_M2 msg: 1613171522.80 epoch sec; seqNo:1619 2021-02-12T23:12:02Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:12:02Z- handleDepth()-Depth_Keller msg: 1613171522800 epoch sec; seqNo:1631; depth 14.55 2021-02-12T23:12:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:12:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:12:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:12:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:12:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:12:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:12:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:12:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:12:02Z- handleDvl() - RDI_Pathfinder msg: 1613171522.80 epoch sec; seqNo:688 2021-02-12T23:12:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:12:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.04 , -0.01 2021-02-12T23:12:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171522.81 epoch sec; seqNo:1631; 4056404.08 north; 590574.83 east 2021-02-12T23:12:03Z- AHRS_M2 msg: 1613171523.20 epoch sec; seqNo:1620 2021-02-12T23:12:03Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-12T23:12:03Z- handleDepth()-Depth_Keller msg: 1613171523207 epoch sec; seqNo:1632; depth 14.43 2021-02-12T23:12:03Z- handleDvl() - RDI_Pathfinder msg: 1613171523.22 epoch sec; seqNo:689 2021-02-12T23:12:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.69 , 94.33 , 106.50 , 71.11 2021-02-12T23:12:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , -0.02 2021-02-12T23:12:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171523.23 epoch sec; seqNo:1632; 4056404.08 north; 590574.83 east 2021-02-12T23:12:03Z- AHRS_M2 msg: 1613171523.61 epoch sec; seqNo:1621 2021-02-12T23:12:03Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.18 psi 2021-02-12T23:12:03Z- handleDepth()-Depth_Keller msg: 1613171523625 epoch sec; seqNo:1633; depth 14.30 2021-02-12T23:12:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171523.67 epoch sec; seqNo:1633; 4056404.46 north; 590574.83 east 2021-02-12T23:12:04Z- AHRS_M2 msg: 1613171524.01 epoch sec; seqNo:1622 2021-02-12T23:12:04Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.19 psi 2021-02-12T23:12:04Z- handleDepth()-Depth_Keller msg: 1613171524012 epoch sec; seqNo:1634; depth 14.18 2021-02-12T23:12:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171524.03 epoch sec; seqNo:1634; 4056404.84 north; 590574.82 east 2021-02-12T23:12:04Z- AHRS_M2 msg: 1613171524.41 epoch sec; seqNo:1623 2021-02-12T23:12:04Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.20 psi 2021-02-12T23:12:04Z- handleDepth()-Depth_Keller msg: 1613171524416 epoch sec; seqNo:1635; depth 14.07 2021-02-12T23:12:04Z- handleDvl() - RDI_Pathfinder msg: 1613171524.43 epoch sec; seqNo:690 2021-02-12T23:12:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.28 , 96.70 , 107.65 , 71.51 2021-02-12T23:12:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.01 2021-02-12T23:12:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171524.45 epoch sec; seqNo:1635; 4056405.23 north; 590576.04 east 2021-02-12T23:12:04Z- AHRS_M2 msg: 1613171524.82 epoch sec; seqNo:1624 2021-02-12T23:12:04Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.20 psi 2021-02-12T23:12:04Z- handleDepth()-Depth_Keller msg: 1613171524829 epoch sec; seqNo:1636; depth 13.94 2021-02-12T23:12:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171524.84 epoch sec; seqNo:1636; 4056405.23 north; 590576.04 east 2021-02-12T23:12:05Z- AHRS_M2 msg: 1613171525.23 epoch sec; seqNo:1625 2021-02-12T23:12:05Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.20 psi 2021-02-12T23:12:05Z- handleDepth()-Depth_Keller msg: 1613171525233 epoch sec; seqNo:1637; depth 13.82 2021-02-12T23:12:05Z- handleDvl() - RDI_Pathfinder msg: 1613171525.24 epoch sec; seqNo:691 2021-02-12T23:12:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.87 , 97.85 , 107.58 , 71.11 2021-02-12T23:12:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , -0.03 2021-02-12T23:12:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171525.26 epoch sec; seqNo:1637; 4056405.61 north; 590576.04 east 2021-02-12T23:12:05Z- AHRS_M2 msg: 1613171525.62 epoch sec; seqNo:1626 2021-02-12T23:12:05Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.20 psi 2021-02-12T23:12:05Z- handleDepth()-Depth_Keller msg: 1613171525633 epoch sec; seqNo:1638; depth 13.69 2021-02-12T23:12:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171525.64 epoch sec; seqNo:1638; 4056405.99 north; 590576.03 east 2021-02-12T23:12:06Z- AHRS_M2 msg: 1613171526.03 epoch sec; seqNo:1627 2021-02-12T23:12:06Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.20 psi 2021-02-12T23:12:06Z- handleDepth()-Depth_Keller msg: 1613171526042 epoch sec; seqNo:1639; depth 13.59 2021-02-12T23:12:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171526.07 epoch sec; seqNo:1639; 4056405.99 north; 590576.03 east 2021-02-12T23:12:06Z- AHRS_M2 msg: 1613171526.43 epoch sec; seqNo:1628 2021-02-12T23:12:06Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.20 psi 2021-02-12T23:12:06Z- handleDepth()-Depth_Keller msg: 1613171526438 epoch sec; seqNo:1640; depth 13.47 2021-02-12T23:12:06Z- handleDvl() - RDI_Pathfinder msg: 1613171526.45 epoch sec; seqNo:692 2021-02-12T23:12:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.46 , 99.07 , 111.23 , 71.11 2021-02-12T23:12:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.07 , -0.06 2021-02-12T23:12:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171526.46 epoch sec; seqNo:1640; 4056406.37 north; 590576.03 east 2021-02-12T23:12:06Z- AHRS_M2 msg: 1613171526.84 epoch sec; seqNo:1629 2021-02-12T23:12:06Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.19 psi 2021-02-12T23:12:06Z- handleDepth()-Depth_Keller msg: 1613171526845 epoch sec; seqNo:1641; depth 13.34 2021-02-12T23:12:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171526.86 epoch sec; seqNo:1641; 4056406.74 north; 590576.02 east 2021-02-12T23:12:07Z- AHRS_M2 msg: 1613171527.24 epoch sec; seqNo:1630 2021-02-12T23:12:07Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.19 psi 2021-02-12T23:12:07Z- handleDepth()-Depth_Keller msg: 1613171527245 epoch sec; seqNo:1642; depth 13.22 2021-02-12T23:12:07Z- handleDvl() - RDI_Pathfinder msg: 1613171527.26 epoch sec; seqNo:693 2021-02-12T23:12:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.87 , 96.56 , 108.73 , 72.33 2021-02-12T23:12:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.15 , -0.01 2021-02-12T23:12:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171527.29 epoch sec; seqNo:1642; 4056407.14 north; 590577.24 east 2021-02-12T23:12:07Z- LcmTrn::handleMessages() - handling 42 messages took 0.74 ms and 127 calls... 2021-02-12T23:12:07Z- waiting for 1613171525.43, time is 1613171527.29 2021-02-12T23:12:07Z- LcmTrn::updateTrn() equating pose 1613171527.29 and meas times 1613171527.29 2021-02-12T23:12:07Z- LcmTrn::updateTrn() >>>> pose time 1613171527.29, meas time 1613171527.29 <<<< 2021-02-12T23:12:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171527.29 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:12:07Z- TerrainNav::motionUpdate() - currEstimate = (4056407, 590577.24, 13.22) 2021-02-12T23:12:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:12:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:12:07Z- TNavBF::Projecting Measurements 2021-02-12T23:12:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:12:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:12:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:12:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:12:07Z- TNavFilter::4 good beams. 2021-02-12T23:12:07Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:12:07Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:12:07Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:12:07Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:12:07Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:12:07Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:12:07Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:12:07Z- TNavBF::mmse selected from filter -1 2021-02-12T23:12:07Z- TerrainMapOctree: (vehN, vehE ) = (4056407.14, 590577.24). 2021-02-12T23:12:07Z- TerrainMapOctree: Min Distance = 6078.03. 2021-02-12T23:12:07Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:12:07Z- There is no specific good beam that all particles have in common. 2021-02-12T23:12:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:12:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 9.95 2021-02-12T23:12:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:12:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 14.92 2021-02-12T23:12:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:12:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 18.92 2021-02-12T23:12:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:12:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-12T23:12:07Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:12:07Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:12:07Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:12:07Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:12:07Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:12:07Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:12:07Z- Filter 0 No beams used in subcloud update 2021-02-12T23:12:07Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:12:07Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:12:07Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 795.7 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:12:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171527.29 sec. 2021-02-12T23:12:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:12:07Z- TNavBF::mmse selected from filter -1 2021-02-12T23:12:07Z- variances([0], [2], and psi_berg) = 795.743322 833.094072 0.000000 2021-02-12T23:12:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:12:07Z- LcmTrn::publishEstimates() - MLE : 4056378.64 590530.74 13.22 2021-02-12T23:12:07Z- LcmTrn::publishEstimates() - MMSE : 4056407.14 590577.24 13.22 2021-02-12T23:12:07Z- LcmTrn::publishEstimates() - COVAR: 795.74 833.09 0.93 0.00 2021-02-12T23:12:07Z- AHRS_M2 msg: 1613171527.64 epoch sec; seqNo:1631 2021-02-12T23:12:07Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.19 psi 2021-02-12T23:12:07Z- handleDepth()-Depth_Keller msg: 1613171527648 epoch sec; seqNo:1643; depth 13.09 2021-02-12T23:12:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171527.66 epoch sec; seqNo:1643; 4056407.14 north; 590577.24 east 2021-02-12T23:12:08Z- handleDepth()-Depth_Keller msg: 1613171528089 epoch sec; seqNo:1644; depth 12.98 2021-02-12T23:12:08Z- handleDvl() - RDI_Pathfinder msg: 1613171528.10 epoch sec; seqNo:694 2021-02-12T23:12:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.87 , 96.63 , 108.80 , 71.51 2021-02-12T23:12:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.20 , 0.09 2021-02-12T23:12:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171528.12 epoch sec; seqNo:1644; 4056407.52 north; 590577.24 east 2021-02-12T23:12:08Z- AHRS_M2 msg: 1613171528.45 epoch sec; seqNo:1632 2021-02-12T23:12:08Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-12T23:12:08Z- handleDepth()-Depth_Keller msg: 1613171528457 epoch sec; seqNo:1645; depth 12.82 2021-02-12T23:12:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171528.47 epoch sec; seqNo:1645; 4056407.89 north; 590577.23 east 2021-02-12T23:12:08Z- AHRS_M2 msg: 1613171528.86 epoch sec; seqNo:1633 2021-02-12T23:12:08Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.19 psi 2021-02-12T23:12:08Z- handleDepth()-Depth_Keller msg: 1613171528860 epoch sec; seqNo:1646; depth 12.72 2021-02-12T23:12:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171528.87 epoch sec; seqNo:1646; 4056408.27 north; 590577.23 east 2021-02-12T23:12:09Z- AHRS_M2 msg: 1613171529.26 epoch sec; seqNo:1634 2021-02-12T23:12:09Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.19 psi 2021-02-12T23:12:09Z- handleDepth()-Depth_Keller msg: 1613171529265 epoch sec; seqNo:1647; depth 12.60 2021-02-12T23:12:09Z- handleDvl() - RDI_Pathfinder msg: 1613171529.28 epoch sec; seqNo:695 2021-02-12T23:12:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.81 , 96.63 , 110.02 , 71.51 2021-02-12T23:12:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.15 , 0.18 2021-02-12T23:12:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171529.29 epoch sec; seqNo:1647; 4056408.65 north; 590577.22 east 2021-02-12T23:12:09Z- AHRS_M2 msg: 1613171529.66 epoch sec; seqNo:1635 2021-02-12T23:12:09Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.19 psi 2021-02-12T23:12:09Z- handleDepth()-Depth_Keller msg: 1613171529677 epoch sec; seqNo:1648; depth 12.48 2021-02-12T23:12:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171529.69 epoch sec; seqNo:1648; 4056409.03 north; 590577.22 east 2021-02-12T23:12:10Z- AHRS_M2 msg: 1613171530.07 epoch sec; seqNo:1636 2021-02-12T23:12:10Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.18 psi 2021-02-12T23:12:10Z- handleDepth()-Depth_Keller msg: 1613171530076 epoch sec; seqNo:1649; depth 12.35 2021-02-12T23:12:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171530.10 epoch sec; seqNo:1649; 4056409.03 north; 590577.22 east 2021-02-12T23:12:10Z- AHRS_M2 msg: 1613171530.47 epoch sec; seqNo:1637 2021-02-12T23:12:10Z- AHRS_M2 msg: 0.57 phi; 0.37 theta; -0.18 psi 2021-02-12T23:12:10Z- handleDepth()-Depth_Keller msg: 1613171530476 epoch sec; seqNo:1650; depth 12.21 2021-02-12T23:12:10Z- handleDvl() - RDI_Pathfinder msg: 1613171530.49 epoch sec; seqNo:696 2021-02-12T23:12:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 96.56 , 112.38 , 71.92 2021-02-12T23:12:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.02 , 0.16 2021-02-12T23:12:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171530.51 epoch sec; seqNo:1650; 4056409.42 north; 590578.44 east 2021-02-12T23:12:10Z- AHRS_M2 msg: 1613171530.88 epoch sec; seqNo:1638 2021-02-12T23:12:10Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.18 psi 2021-02-12T23:12:10Z- handleDepth()-Depth_Keller msg: 1613171530885 epoch sec; seqNo:1651; depth 12.10 2021-02-12T23:12:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171530.90 epoch sec; seqNo:1651; 4056409.80 north; 590578.43 east 2021-02-12T23:12:11Z- AHRS_M2 msg: 1613171531.28 epoch sec; seqNo:1639 2021-02-12T23:12:11Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.17 psi 2021-02-12T23:12:11Z- handleDepth()-Depth_Keller msg: 1613171531286 epoch sec; seqNo:1652; depth 12.00 2021-02-12T23:12:11Z- handleDvl() - RDI_Pathfinder msg: 1613171531.30 epoch sec; seqNo:697 2021-02-12T23:12:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.55 , 98.93 , 113.53 , 73.15 2021-02-12T23:12:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.06 , -0.01 2021-02-12T23:12:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171531.31 epoch sec; seqNo:1652; 4056410.18 north; 590578.43 east 2021-02-12T23:12:11Z- AHRS_M2 msg: 1613171531.68 epoch sec; seqNo:1640 2021-02-12T23:12:11Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.16 psi 2021-02-12T23:12:11Z- handleDepth()-Depth_Keller msg: 1613171531688 epoch sec; seqNo:1653; depth 11.87 2021-02-12T23:12:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171531.70 epoch sec; seqNo:1653; 4056410.56 north; 590578.43 east 2021-02-12T23:12:12Z- AHRS_M2 msg: 1613171532.11 epoch sec; seqNo:1641 2021-02-12T23:12:12Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.16 psi 2021-02-12T23:12:12Z- handleDepth()-Depth_Keller msg: 1613171532122 epoch sec; seqNo:1654; depth 11.76 2021-02-12T23:12:12Z- handleDvl() - RDI_Pathfinder msg: 1613171532.13 epoch sec; seqNo:698 2021-02-12T23:12:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.48 , 96.29 , 113.87 , 73.15 2021-02-12T23:12:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.01 , -0.17 2021-02-12T23:12:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171532.15 epoch sec; seqNo:1654; 4056410.56 north; 590578.43 east 2021-02-12T23:12:12Z- AHRS_M2 msg: 1613171532.49 epoch sec; seqNo:1642 2021-02-12T23:12:12Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-12T23:12:12Z- handleDepth()-Depth_Keller msg: 1613171532497 epoch sec; seqNo:1655; depth 11.62 2021-02-12T23:12:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171532.51 epoch sec; seqNo:1655; 4056410.94 north; 590578.42 east 2021-02-12T23:12:12Z- LcmTrn::handleMessages() - handling 43 messages took 0.68 ms and 130 calls... 2021-02-12T23:12:12Z- waiting for 1613171530.29, time is 1613171532.51 2021-02-12T23:12:12Z- LcmTrn::updateTrn() >>>> pose time 1613171532.51, meas time 1613171532.13 <<<< 2021-02-12T23:12:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171532.13 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:12:12Z- TerrainNav::motionUpdate() - currEstimate = (4056411, 590578.42, 11.62) 2021-02-12T23:12:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:12:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:12:12Z- TNavBF::Projecting Measurements 2021-02-12T23:12:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:12:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:12:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:12:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:12:12Z- TNavFilter::4 good beams. 2021-02-12T23:12:12Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:12:12Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:12:12Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:12:12Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:12:12Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:12:12Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:12:12Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:12:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:12:12Z- TerrainMapOctree: (vehN, vehE ) = (4056410.67, 590578.34). 2021-02-12T23:12:12Z- TerrainMapOctree: Min Distance = 6074.56. 2021-02-12T23:12:12Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:12:12Z- There is no specific good beam that all particles have in common. 2021-02-12T23:12:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:12:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 11.69 2021-02-12T23:12:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:12:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 14.84 2021-02-12T23:12:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:12:12Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 20.75 2021-02-12T23:12:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:12:12Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-12T23:12:12Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:12:12Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:12:12Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:12:12Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:12:12Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:12:12Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:12:12Z- Filter 0 No beams used in subcloud update 2021-02-12T23:12:12Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:12:12Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:12:12Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 795.5 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:12:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171532.13 sec. 2021-02-12T23:12:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:12:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:12:12Z- variances([0], [2], and psi_berg) = 795.491030 833.093578 0.000000 2021-02-12T23:12:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:12:12Z- LcmTrn::publishEstimates() - MLE : 4056382.44 590531.92 11.62 2021-02-12T23:12:12Z- LcmTrn::publishEstimates() - MMSE : 4056410.94 590578.42 11.62 2021-02-12T23:12:12Z- LcmTrn::publishEstimates() - COVAR: 795.49 833.09 0.87 0.00 2021-02-12T23:12:12Z- AHRS_M2 msg: 1613171532.93 epoch sec; seqNo:1643 2021-02-12T23:12:12Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-12T23:12:12Z- handleDepth()-Depth_Keller msg: 1613171532934 epoch sec; seqNo:1656; depth 11.51 2021-02-12T23:12:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171532.96 epoch sec; seqNo:1656; 4056410.95 north; 590579.64 east 2021-02-12T23:12:13Z- AHRS_M2 msg: 1613171533.30 epoch sec; seqNo:1644 2021-02-12T23:12:13Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.15 psi 2021-02-12T23:12:13Z- handleDepth()-Depth_Keller msg: 1613171533304 epoch sec; seqNo:1657; depth 11.36 2021-02-12T23:12:13Z- handleDvl() - RDI_Pathfinder msg: 1613171533.33 epoch sec; seqNo:699 2021-02-12T23:12:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.55 , 96.36 , 112.58 , 72.33 2021-02-12T23:12:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.15 , -0.10 2021-02-12T23:12:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171533.34 epoch sec; seqNo:1657; 4056411.33 north; 590579.64 east 2021-02-12T23:12:13Z- AHRS_M2 msg: 1613171533.70 epoch sec; seqNo:1645 2021-02-12T23:12:13Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.15 psi 2021-02-12T23:12:13Z- handleDepth()-Depth_Keller msg: 1613171533708 epoch sec; seqNo:1658; depth 11.25 2021-02-12T23:12:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171533.72 epoch sec; seqNo:1658; 4056411.71 north; 590579.64 east 2021-02-12T23:12:14Z- AHRS_M2 msg: 1613171534.11 epoch sec; seqNo:1646 2021-02-12T23:12:14Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.15 psi 2021-02-12T23:12:14Z- handleDepth()-Depth_Keller msg: 1613171534122 epoch sec; seqNo:1659; depth 11.13 2021-02-12T23:12:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171534.16 epoch sec; seqNo:1659; 4056411.71 north; 590579.64 east 2021-02-12T23:12:14Z- AHRS_M2 msg: 1613171534.51 epoch sec; seqNo:1647 2021-02-12T23:12:14Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-12T23:12:14Z- handleDepth()-Depth_Keller msg: 1613171534517 epoch sec; seqNo:1660; depth 11.00 2021-02-12T23:12:14Z- handleDvl() - RDI_Pathfinder msg: 1613171534.53 epoch sec; seqNo:700 2021-02-12T23:12:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 96.43 , 113.46 , 72.33 2021-02-12T23:12:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.18 , 0.15 2021-02-12T23:12:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171534.54 epoch sec; seqNo:1660; 4056412.09 north; 590579.63 east 2021-02-12T23:12:14Z- AHRS_M2 msg: 1613171534.92 epoch sec; seqNo:1648 2021-02-12T23:12:14Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.13 psi 2021-02-12T23:12:14Z- handleDepth()-Depth_Keller msg: 1613171534922 epoch sec; seqNo:1661; depth 10.89 2021-02-12T23:12:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171534.93 epoch sec; seqNo:1661; 4056412.47 north; 590579.63 east 2021-02-12T23:12:15Z- AHRS_M2 msg: 1613171535.32 epoch sec; seqNo:1649 2021-02-12T23:12:15Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.13 psi 2021-02-12T23:12:15Z- handleDepth()-Depth_Keller msg: 1613171535324 epoch sec; seqNo:1662; depth 10.73 2021-02-12T23:12:15Z- handleDvl() - RDI_Pathfinder msg: 1613171535.35 epoch sec; seqNo:701 2021-02-12T23:12:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.55 , 96.36 , 112.58 , 73.56 2021-02-12T23:12:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , 0.31 2021-02-12T23:12:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171535.36 epoch sec; seqNo:1662; 4056412.84 north; 590579.62 east 2021-02-12T23:12:15Z- AHRS_M2 msg: 1613171535.72 epoch sec; seqNo:1650 2021-02-12T23:12:15Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.13 psi 2021-02-12T23:12:15Z- handleDepth()-Depth_Keller msg: 1613171535733 epoch sec; seqNo:1663; depth 10.61 2021-02-12T23:12:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171535.74 epoch sec; seqNo:1663; 4056413.22 north; 590579.62 east 2021-02-12T23:12:16Z- AHRS_M2 msg: 1613171536.13 epoch sec; seqNo:1651 2021-02-12T23:12:16Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.14 psi 2021-02-12T23:12:16Z- handleDepth()-Depth_Keller msg: 1613171536137 epoch sec; seqNo:1664; depth 10.47 2021-02-12T23:12:16Z- handleDvl() - RDI_Pathfinder msg: 1613171536.15 epoch sec; seqNo:702 2021-02-12T23:12:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.90 , 97.71 , 113.94 , 74.78 2021-02-12T23:12:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , 0.21 2021-02-12T23:12:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171536.16 epoch sec; seqNo:1664; 4056413.24 north; 590580.84 east 2021-02-12T23:12:16Z- AHRS_M2 msg: 1613171536.53 epoch sec; seqNo:1652 2021-02-12T23:12:16Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.15 psi 2021-02-12T23:12:16Z- handleDepth()-Depth_Keller msg: 1613171536544 epoch sec; seqNo:1665; depth 10.35 2021-02-12T23:12:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171536.56 epoch sec; seqNo:1665; 4056413.62 north; 590580.84 east 2021-02-12T23:12:16Z- AHRS_M2 msg: 1613171536.93 epoch sec; seqNo:1653 2021-02-12T23:12:16Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.16 psi 2021-02-12T23:12:16Z- handleDepth()-Depth_Keller msg: 1613171536945 epoch sec; seqNo:1666; depth 10.21 2021-02-12T23:12:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171536.96 epoch sec; seqNo:1666; 4056413.99 north; 590580.83 east 2021-02-12T23:12:17Z- AHRS_M2 msg: 1613171537.34 epoch sec; seqNo:1654 2021-02-12T23:12:17Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.16 psi 2021-02-12T23:12:17Z- handleDepth()-Depth_Keller msg: 1613171537348 epoch sec; seqNo:1667; depth 10.07 2021-02-12T23:12:17Z- handleDvl() - RDI_Pathfinder msg: 1613171537.37 epoch sec; seqNo:703 2021-02-12T23:12:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.11 , 98.93 , 116.51 , 74.37 2021-02-12T23:12:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.07 , -0.01 2021-02-12T23:12:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171537.40 epoch sec; seqNo:1667; 4056414.37 north; 590580.83 east 2021-02-12T23:12:17Z- AHRS_M2 msg: 1613171537.74 epoch sec; seqNo:1655 2021-02-12T23:12:17Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.16 psi 2021-02-12T23:12:17Z- LcmTrn::handleMessages() - handling 42 messages took 0.80 ms and 126 calls... 2021-02-12T23:12:17Z- waiting for 1613171535.51, time is 1613171537.74 2021-02-12T23:12:17Z- LcmTrn::updateTrn() >>>> pose time 1613171537.74, meas time 1613171537.37 <<<< 2021-02-12T23:12:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171537.37 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:12:17Z- TerrainNav::motionUpdate() - currEstimate = (4056414, 590580.83, 10.07) 2021-02-12T23:12:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:12:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:12:17Z- TNavBF::Projecting Measurements 2021-02-12T23:12:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:12:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:12:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:12:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:12:17Z- TNavFilter::4 good beams. 2021-02-12T23:12:17Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:12:17Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:12:17Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:12:17Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:12:17Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:12:17Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:12:17Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:12:17Z- TNavBF::mmse selected from filter -1 2021-02-12T23:12:17Z- TerrainMapOctree: (vehN, vehE ) = (4056414.13, 590580.66). 2021-02-12T23:12:17Z- TerrainMapOctree: Min Distance = 6070.40. 2021-02-12T23:12:17Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:12:17Z- There is no specific good beam that all particles have in common. 2021-02-12T23:12:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:12:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 12.42 2021-02-12T23:12:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:12:17Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.66 2021-02-12T23:12:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:12:17Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 21.72 2021-02-12T23:12:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:12:17Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-12T23:12:17Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:12:17Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:12:17Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:12:17Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:12:17Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:12:17Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:12:17Z- Filter 0 No beams used in subcloud update 2021-02-12T23:12:17Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:12:17Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:12:17Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 795.3 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:12:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171537.37 sec. 2021-02-12T23:12:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:12:17Z- TNavBF::mmse selected from filter -1 2021-02-12T23:12:17Z- variances([0], [2], and psi_berg) = 795.233489 833.093076 0.000000 2021-02-12T23:12:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:12:17Z- LcmTrn::publishEstimates() - MLE : 4056385.87 590532.33 10.07 2021-02-12T23:12:17Z- LcmTrn::publishEstimates() - MMSE : 4056414.37 590580.83 10.07 2021-02-12T23:12:17Z- LcmTrn::publishEstimates() - COVAR: 795.23 833.09 0.85 0.00 2021-02-12T23:12:17Z- handleDepth()-Depth_Keller msg: 1613171537750 epoch sec; seqNo:1668; depth 9.94 2021-02-12T23:12:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171537.76 epoch sec; seqNo:1668; 4056414.37 north; 590580.83 east 2021-02-12T23:12:18Z- AHRS_M2 msg: 1613171538.15 epoch sec; seqNo:1656 2021-02-12T23:12:18Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.16 psi 2021-02-12T23:12:18Z- handleDepth()-Depth_Keller msg: 1613171538156 epoch sec; seqNo:1669; depth 9.82 2021-02-12T23:12:18Z- handleDvl() - RDI_Pathfinder msg: 1613171538.18 epoch sec; seqNo:704 2021-02-12T23:12:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 100.21 , 116.44 , 74.37 2021-02-12T23:12:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , -0.16 2021-02-12T23:12:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171538.19 epoch sec; seqNo:1669; 4056414.75 north; 590580.82 east 2021-02-12T23:12:18Z- AHRS_M2 msg: 1613171538.56 epoch sec; seqNo:1657 2021-02-12T23:12:18Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.15 psi 2021-02-12T23:12:18Z- handleDepth()-Depth_Keller msg: 1613171538562 epoch sec; seqNo:1670; depth 9.69 2021-02-12T23:12:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171538.58 epoch sec; seqNo:1670; 4056414.76 north; 590582.05 east 2021-02-12T23:12:18Z- AHRS_M2 msg: 1613171538.97 epoch sec; seqNo:1658 2021-02-12T23:12:18Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.14 psi 2021-02-12T23:12:18Z- handleDepth()-Depth_Keller msg: 1613171538971 epoch sec; seqNo:1671; depth 9.57 2021-02-12T23:12:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171538.98 epoch sec; seqNo:1671; 4056415.14 north; 590582.04 east 2021-02-12T23:12:19Z- AHRS_M2 msg: 1613171539.37 epoch sec; seqNo:1659 2021-02-12T23:12:19Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.14 psi 2021-02-12T23:12:19Z- handleDepth()-Depth_Keller msg: 1613171539370 epoch sec; seqNo:1672; depth 9.45 2021-02-12T23:12:19Z- handleDvl() - RDI_Pathfinder msg: 1613171539.38 epoch sec; seqNo:705 2021-02-12T23:12:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.05 , 96.16 , 116.44 , 75.60 2021-02-12T23:12:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.09 , -0.16 2021-02-12T23:12:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171539.39 epoch sec; seqNo:1672; 4056415.52 north; 590582.04 east 2021-02-12T23:12:19Z- AHRS_M2 msg: 1613171539.76 epoch sec; seqNo:1660 2021-02-12T23:12:19Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.14 psi 2021-02-12T23:12:19Z- handleDepth()-Depth_Keller msg: 1613171539777 epoch sec; seqNo:1673; depth 9.32 2021-02-12T23:12:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171539.82 epoch sec; seqNo:1673; 4056415.52 north; 590582.04 east 2021-02-12T23:12:20Z- AHRS_M2 msg: 1613171540.17 epoch sec; seqNo:1661 2021-02-12T23:12:20Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.15 psi 2021-02-12T23:12:20Z- handleDepth()-Depth_Keller msg: 1613171540181 epoch sec; seqNo:1674; depth 9.20 2021-02-12T23:12:20Z- handleDvl() - RDI_Pathfinder msg: 1613171540.19 epoch sec; seqNo:706 2021-02-12T23:12:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 96.16 , 115.09 , 73.96 2021-02-12T23:12:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.13 , -0.05 2021-02-12T23:12:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171540.20 epoch sec; seqNo:1674; 4056415.90 north; 590582.03 east 2021-02-12T23:12:20Z- AHRS_M2 msg: 1613171540.57 epoch sec; seqNo:1662 2021-02-12T23:12:20Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.16 psi 2021-02-12T23:12:20Z- handleDepth()-Depth_Keller msg: 1613171540576 epoch sec; seqNo:1675; depth 9.06 2021-02-12T23:12:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171540.60 epoch sec; seqNo:1675; 4056416.28 north; 590582.03 east 2021-02-12T23:12:20Z- AHRS_M2 msg: 1613171540.97 epoch sec; seqNo:1663 2021-02-12T23:12:20Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.15 psi 2021-02-12T23:12:20Z- handleDepth()-Depth_Keller msg: 1613171540985 epoch sec; seqNo:1676; depth 8.95 2021-02-12T23:12:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171540.99 epoch sec; seqNo:1676; 4056416.66 north; 590582.03 east 2021-02-12T23:12:21Z- AHRS_M2 msg: 1613171541.38 epoch sec; seqNo:1664 2021-02-12T23:12:21Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.13 psi 2021-02-12T23:12:21Z- handleDepth()-Depth_Keller msg: 1613171541384 epoch sec; seqNo:1677; depth 8.84 2021-02-12T23:12:21Z- handleDvl() - RDI_Pathfinder msg: 1613171541.39 epoch sec; seqNo:707 2021-02-12T23:12:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.83 , 96.29 , 113.87 , 75.60 2021-02-12T23:12:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.10 , 0.04 2021-02-12T23:12:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171541.41 epoch sec; seqNo:1677; 4056416.66 north; 590582.03 east 2021-02-12T23:12:21Z- AHRS_M2 msg: 1613171541.78 epoch sec; seqNo:1665 2021-02-12T23:12:21Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.12 psi 2021-02-12T23:12:21Z- handleDepth()-Depth_Keller msg: 1613171541797 epoch sec; seqNo:1678; depth 8.73 2021-02-12T23:12:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171541.81 epoch sec; seqNo:1678; 4056417.05 north; 590583.24 east 2021-02-12T23:12:22Z- AHRS_M2 msg: 1613171542.19 epoch sec; seqNo:1666 2021-02-12T23:12:22Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.11 psi 2021-02-12T23:12:22Z- handleDepth()-Depth_Keller msg: 1613171542192 epoch sec; seqNo:1679; depth 8.61 2021-02-12T23:12:22Z- handleDvl() - RDI_Pathfinder msg: 1613171542.21 epoch sec; seqNo:708 2021-02-12T23:12:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.53 , 96.29 , 115.22 , 76.42 2021-02-12T23:12:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.07 , -0.00 2021-02-12T23:12:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171542.22 epoch sec; seqNo:1679; 4056417.43 north; 590583.24 east 2021-02-12T23:12:22Z- AHRS_M2 msg: 1613171542.59 epoch sec; seqNo:1667 2021-02-12T23:12:22Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.12 psi 2021-02-12T23:12:22Z- handleDepth()-Depth_Keller msg: 1613171542596 epoch sec; seqNo:1680; depth 8.48 2021-02-12T23:12:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171542.61 epoch sec; seqNo:1680; 4056417.43 north; 590583.24 east 2021-02-12T23:12:22Z- LcmTrn::handleMessages() - handling 43 messages took 0.62 ms and 131 calls... 2021-02-12T23:12:22Z- waiting for 1613171540.74, time is 1613171542.61 2021-02-12T23:12:22Z- LcmTrn::updateTrn() >>>> pose time 1613171542.61, meas time 1613171542.21 <<<< 2021-02-12T23:12:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171542.21 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:12:22Z- TerrainNav::motionUpdate() - currEstimate = (4056417, 590583.24, 8.48) 2021-02-12T23:12:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:12:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:12:22Z- TNavBF::Projecting Measurements 2021-02-12T23:12:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:12:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:12:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:12:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:12:22Z- TNavFilter::4 good beams. 2021-02-12T23:12:22Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:12:22Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:12:22Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:12:22Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:12:22Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:12:22Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:12:22Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:12:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:12:22Z- TerrainMapOctree: (vehN, vehE ) = (4056417.18, 590583.04). 2021-02-12T23:12:22Z- TerrainMapOctree: Min Distance = 6066.53. 2021-02-12T23:12:22Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:12:22Z- There is no specific good beam that all particles have in common. 2021-02-12T23:12:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:12:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 12.83 2021-02-12T23:12:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:12:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 14.84 2021-02-12T23:12:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:12:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 21.24 2021-02-12T23:12:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:12:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 9.34 2021-02-12T23:12:22Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:12:22Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:12:22Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:12:22Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:12:22Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:12:22Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:12:22Z- Filter 0 No beams used in subcloud update 2021-02-12T23:12:22Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:12:22Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:12:22Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 795.0 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:12:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171542.21 sec. 2021-02-12T23:12:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:12:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:12:22Z- variances([0], [2], and psi_berg) = 794.984024 833.092594 0.000000 2021-02-12T23:12:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:12:22Z- LcmTrn::publishEstimates() - MLE : 4056388.93 590534.74 8.48 2021-02-12T23:12:22Z- LcmTrn::publishEstimates() - MMSE : 4056417.43 590583.24 8.48 2021-02-12T23:12:22Z- LcmTrn::publishEstimates() - COVAR: 794.98 833.09 0.83 0.00 2021-02-12T23:12:23Z- handleDepth()-Depth_Keller msg: 1613171543017 epoch sec; seqNo:1681; depth 8.37 2021-02-12T23:12:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171543.03 epoch sec; seqNo:1681; 4056417.81 north; 590583.23 east 2021-02-12T23:12:23Z- AHRS_M2 msg: 1613171543.40 epoch sec; seqNo:1668 2021-02-12T23:12:23Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.14 psi 2021-02-12T23:12:23Z- handleDepth()-Depth_Keller msg: 1613171543409 epoch sec; seqNo:1682; depth 8.24 2021-02-12T23:12:23Z- handleDvl() - RDI_Pathfinder msg: 1613171543.42 epoch sec; seqNo:709 2021-02-12T23:12:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.40 , 96.16 , 115.09 , 75.19 2021-02-12T23:12:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.17 , -0.05 2021-02-12T23:12:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171543.43 epoch sec; seqNo:1682; 4056418.19 north; 590583.23 east 2021-02-12T23:12:23Z- AHRS_M2 msg: 1613171543.80 epoch sec; seqNo:1669 2021-02-12T23:12:23Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.15 psi 2021-02-12T23:12:23Z- handleDepth()-Depth_Keller msg: 1613171543813 epoch sec; seqNo:1683; depth 8.12 2021-02-12T23:12:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171543.83 epoch sec; seqNo:1683; 4056418.19 north; 590583.23 east 2021-02-12T23:12:24Z- AHRS_M2 msg: 1613171544.21 epoch sec; seqNo:1670 2021-02-12T23:12:24Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.17 psi 2021-02-12T23:12:24Z- handleDepth()-Depth_Keller msg: 1613171544214 epoch sec; seqNo:1684; depth 7.98 2021-02-12T23:12:24Z- handleDvl() - RDI_Pathfinder msg: 1613171544.23 epoch sec; seqNo:710 2021-02-12T23:12:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.11 , 96.23 , 115.15 , 75.60 2021-02-12T23:12:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.26 , 0.11 2021-02-12T23:12:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171544.24 epoch sec; seqNo:1684; 4056418.57 north; 590583.23 east 2021-02-12T23:12:24Z- AHRS_M2 msg: 1613171544.61 epoch sec; seqNo:1671 2021-02-12T23:12:24Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.18 psi 2021-02-12T23:12:24Z- handleDepth()-Depth_Keller msg: 1613171544618 epoch sec; seqNo:1685; depth 7.84 2021-02-12T23:12:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171544.63 epoch sec; seqNo:1685; 4056418.94 north; 590583.22 east 2021-02-12T23:12:25Z- AHRS_M2 msg: 1613171545.02 epoch sec; seqNo:1672 2021-02-12T23:12:25Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-12T23:12:25Z- handleDepth()-Depth_Keller msg: 1613171545033 epoch sec; seqNo:1686; depth 7.74 2021-02-12T23:12:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171545.04 epoch sec; seqNo:1686; 4056419.32 north; 590583.22 east 2021-02-12T23:12:25Z- AHRS_M2 msg: 1613171545.42 epoch sec; seqNo:1673 2021-02-12T23:12:25Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.17 psi 2021-02-12T23:12:25Z- handleDepth()-Depth_Keller msg: 1613171545429 epoch sec; seqNo:1687; depth 7.60 2021-02-12T23:12:25Z- handleDvl() - RDI_Pathfinder msg: 1613171545.44 epoch sec; seqNo:711 2021-02-12T23:12:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.76 , 102.99 , 116.51 , 76.83 2021-02-12T23:12:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.12 , 0.29 2021-02-12T23:12:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171545.46 epoch sec; seqNo:1687; 4056419.72 north; 590584.44 east 2021-02-12T23:12:25Z- AHRS_M2 msg: 1613171545.82 epoch sec; seqNo:1674 2021-02-12T23:12:25Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-12T23:12:25Z- handleDepth()-Depth_Keller msg: 1613171545828 epoch sec; seqNo:1688; depth 7.47 2021-02-12T23:12:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171545.84 epoch sec; seqNo:1688; 4056419.72 north; 590584.44 east 2021-02-12T23:12:26Z- AHRS_M2 msg: 1613171546.23 epoch sec; seqNo:1675 2021-02-12T23:12:26Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.15 psi 2021-02-12T23:12:26Z- handleDepth()-Depth_Keller msg: 1613171546241 epoch sec; seqNo:1689; depth 7.37 2021-02-12T23:12:26Z- handleDvl() - RDI_Pathfinder msg: 1613171546.25 epoch sec; seqNo:712 2021-02-12T23:12:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.91 , 101.43 , 119.01 , 77.24 2021-02-12T23:12:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.14 , 0.25 2021-02-12T23:12:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171546.26 epoch sec; seqNo:1689; 4056420.09 north; 590584.43 east 2021-02-12T23:12:26Z- AHRS_M2 msg: 1613171546.63 epoch sec; seqNo:1676 2021-02-12T23:12:26Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.13 psi 2021-02-12T23:12:26Z- handleDepth()-Depth_Keller msg: 1613171546641 epoch sec; seqNo:1690; depth 7.24 2021-02-12T23:12:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171546.66 epoch sec; seqNo:1690; 4056420.47 north; 590584.43 east 2021-02-12T23:12:27Z- AHRS_M2 msg: 1613171547.04 epoch sec; seqNo:1677 2021-02-12T23:12:27Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.13 psi 2021-02-12T23:12:27Z- handleDepth()-Depth_Keller msg: 1613171547040 epoch sec; seqNo:1691; depth 7.13 2021-02-12T23:12:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171547.05 epoch sec; seqNo:1691; 4056420.85 north; 590584.42 east 2021-02-12T23:12:27Z- AHRS_M2 msg: 1613171547.44 epoch sec; seqNo:1678 2021-02-12T23:12:27Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.13 psi 2021-02-12T23:12:27Z- handleDepth()-Depth_Keller msg: 1613171547444 epoch sec; seqNo:1692; depth 7.00 2021-02-12T23:12:27Z- handleDvl() - RDI_Pathfinder msg: 1613171547.46 epoch sec; seqNo:713 2021-02-12T23:12:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.97 , 96.02 , 120.36 , 76.42 2021-02-12T23:12:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.25 , 0.05 2021-02-12T23:12:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171547.47 epoch sec; seqNo:1692; 4056420.86 north; 590585.64 east 2021-02-12T23:12:27Z- AHRS_M2 msg: 1613171547.84 epoch sec; seqNo:1679 2021-02-12T23:12:27Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.14 psi 2021-02-12T23:12:27Z- handleDepth()-Depth_Keller msg: 1613171547855 epoch sec; seqNo:1693; depth 6.88 2021-02-12T23:12:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171547.87 epoch sec; seqNo:1693; 4056421.24 north; 590585.64 east 2021-02-12T23:12:27Z- LcmTrn::handleMessages() - handling 43 messages took 0.68 ms and 130 calls... 2021-02-12T23:12:27Z- waiting for 1613171545.61, time is 1613171547.87 2021-02-12T23:12:27Z- LcmTrn::updateTrn() >>>> pose time 1613171547.87, meas time 1613171547.46 <<<< 2021-02-12T23:12:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171547.46 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:12:27Z- TerrainNav::motionUpdate() - currEstimate = (4056421, 590585.64, 6.88) 2021-02-12T23:12:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:12:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:12:27Z- TNavBF::Projecting Measurements 2021-02-12T23:12:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:12:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:12:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:12:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:12:27Z- TNavFilter::4 good beams. 2021-02-12T23:12:27Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:12:27Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:12:27Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:12:27Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:12:27Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:12:27Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:12:27Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:12:27Z- TNavBF::mmse selected from filter -1 2021-02-12T23:12:27Z- TerrainMapOctree: (vehN, vehE ) = (4056420.95, 590585.45). 2021-02-12T23:12:27Z- TerrainMapOctree: Min Distance = 6062.07. 2021-02-12T23:12:27Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:12:27Z- There is no specific good beam that all particles have in common. 2021-02-12T23:12:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:12:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 13.53 2021-02-12T23:12:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:12:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 14.75 2021-02-12T23:12:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:12:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 23.18 2021-02-12T23:12:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:12:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 9.34 2021-02-12T23:12:28Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:12:28Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:12:28Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:12:28Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:12:28Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:12:28Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:12:28Z- Filter 0 No beams used in subcloud update 2021-02-12T23:12:28Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:12:28Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:12:28Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 794.8 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:12:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171547.46 sec. 2021-02-12T23:12:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:12:28Z- TNavBF::mmse selected from filter -1 2021-02-12T23:12:28Z- variances([0], [2], and psi_berg) = 794.717960 833.092082 0.000000 2021-02-12T23:12:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:12:28Z- LcmTrn::publishEstimates() - MLE : 4056392.74 590539.14 6.88 2021-02-12T23:12:28Z- LcmTrn::publishEstimates() - MMSE : 4056421.24 590585.64 6.88 2021-02-12T23:12:28Z- LcmTrn::publishEstimates() - COVAR: 794.72 833.09 0.84 0.00 2021-02-12T23:12:28Z- AHRS_M2 msg: 1613171548.25 epoch sec; seqNo:1680 2021-02-12T23:12:28Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.14 psi 2021-02-12T23:12:28Z- handleDepth()-Depth_Keller msg: 1613171548257 epoch sec; seqNo:1694; depth 6.75 2021-02-12T23:12:28Z- handleDvl() - RDI_Pathfinder msg: 1613171548.28 epoch sec; seqNo:714 2021-02-12T23:12:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.62 , 101.43 , 119.01 , 76.83 2021-02-12T23:12:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.12 , -0.04 2021-02-12T23:12:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171548.29 epoch sec; seqNo:1694; 4056421.24 north; 590585.64 east 2021-02-12T23:12:28Z- AHRS_M2 msg: 1613171548.65 epoch sec; seqNo:1681 2021-02-12T23:12:28Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.15 psi 2021-02-12T23:12:28Z- handleDepth()-Depth_Keller msg: 1613171548656 epoch sec; seqNo:1695; depth 6.62 2021-02-12T23:12:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171548.67 epoch sec; seqNo:1695; 4056421.62 north; 590585.64 east 2021-02-12T23:12:29Z- AHRS_M2 msg: 1613171549.06 epoch sec; seqNo:1682 2021-02-12T23:12:29Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.16 psi 2021-02-12T23:12:29Z- handleDepth()-Depth_Keller msg: 1613171549063 epoch sec; seqNo:1696; depth 6.49 2021-02-12T23:12:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171549.07 epoch sec; seqNo:1696; 4056421.62 north; 590585.64 east 2021-02-12T23:12:29Z- AHRS_M2 msg: 1613171549.46 epoch sec; seqNo:1683 2021-02-12T23:12:29Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.18 psi 2021-02-12T23:12:29Z- handleDepth()-Depth_Keller msg: 1613171549464 epoch sec; seqNo:1697; depth 6.36 2021-02-12T23:12:29Z- handleDvl() - RDI_Pathfinder msg: 1613171549.48 epoch sec; seqNo:715 2021-02-12T23:12:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.20 , 95.96 , 117.59 , 77.64 2021-02-12T23:12:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.10 , -0.05 2021-02-12T23:12:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171549.49 epoch sec; seqNo:1697; 4056422.01 north; 590586.85 east 2021-02-12T23:12:29Z- AHRS_M2 msg: 1613171549.86 epoch sec; seqNo:1684 2021-02-12T23:12:29Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.20 psi 2021-02-12T23:12:29Z- handleDepth()-Depth_Keller msg: 1613171549868 epoch sec; seqNo:1698; depth 6.24 2021-02-12T23:12:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171549.88 epoch sec; seqNo:1698; 4056422.01 north; 590586.85 east 2021-02-12T23:12:30Z- AHRS_M2 msg: 1613171550.27 epoch sec; seqNo:1685 2021-02-12T23:12:30Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.20 psi 2021-02-12T23:12:30Z- handleDepth()-Depth_Keller msg: 1613171550272 epoch sec; seqNo:1699; depth 6.12 2021-02-12T23:12:30Z- handleDvl() - RDI_Pathfinder msg: 1613171550.28 epoch sec; seqNo:716 2021-02-12T23:12:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 105.56 , 116.37 , 78.46 2021-02-12T23:12:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.24 , 0.08 2021-02-12T23:12:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171550.29 epoch sec; seqNo:1699; 4056422.39 north; 590586.85 east 2021-02-12T23:12:30Z- AHRS_M2 msg: 1613171550.67 epoch sec; seqNo:1686 2021-02-12T23:12:30Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.20 psi 2021-02-12T23:12:30Z- handleDepth()-Depth_Keller msg: 1613171550678 epoch sec; seqNo:1700; depth 5.98 2021-02-12T23:12:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171550.69 epoch sec; seqNo:1700; 4056422.77 north; 590586.85 east 2021-02-12T23:12:31Z- AHRS_M2 msg: 1613171551.08 epoch sec; seqNo:1687 2021-02-12T23:12:31Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.19 psi 2021-02-12T23:12:31Z- handleDepth()-Depth_Keller msg: 1613171551090 epoch sec; seqNo:1701; depth 5.87 2021-02-12T23:12:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171551.10 epoch sec; seqNo:1701; 4056423.15 north; 590586.84 east 2021-02-12T23:12:31Z- AHRS_M2 msg: 1613171551.48 epoch sec; seqNo:1688 2021-02-12T23:12:31Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-12T23:12:31Z- handleDepth()-Depth_Keller msg: 1613171551484 epoch sec; seqNo:1702; depth 5.74 2021-02-12T23:12:31Z- handleDvl() - RDI_Pathfinder msg: 1613171551.50 epoch sec; seqNo:717 2021-02-12T23:12:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 105.42 , 120.29 , 79.28 2021-02-12T23:12:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.11 , 0.17 2021-02-12T23:12:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171551.52 epoch sec; seqNo:1702; 4056423.53 north; 590586.84 east 2021-02-12T23:12:31Z- AHRS_M2 msg: 1613171551.88 epoch sec; seqNo:1689 2021-02-12T23:12:31Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-12T23:12:31Z- handleDepth()-Depth_Keller msg: 1613171551888 epoch sec; seqNo:1703; depth 5.64 2021-02-12T23:12:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171551.90 epoch sec; seqNo:1703; 4056423.91 north; 590586.83 east 2021-02-12T23:12:32Z- AHRS_M2 msg: 1613171552.29 epoch sec; seqNo:1690 2021-02-12T23:12:32Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.16 psi 2021-02-12T23:12:32Z- handleDepth()-Depth_Keller msg: 1613171552294 epoch sec; seqNo:1704; depth 5.51 2021-02-12T23:12:32Z- handleDvl() - RDI_Pathfinder msg: 1613171552.31 epoch sec; seqNo:718 2021-02-12T23:12:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.13 , 104.00 , 118.87 , 78.46 2021-02-12T23:12:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.01 2021-02-12T23:12:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171552.32 epoch sec; seqNo:1704; 4056424.30 north; 590588.05 east 2021-02-12T23:12:32Z- AHRS_M2 msg: 1613171552.69 epoch sec; seqNo:1691 2021-02-12T23:12:32Z- AHRS_M2 msg: 0.62 phi; 0.28 theta; -0.17 psi 2021-02-12T23:12:32Z- handleDepth()-Depth_Keller msg: 1613171552701 epoch sec; seqNo:1705; depth 5.38 2021-02-12T23:12:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171552.72 epoch sec; seqNo:1705; 4056424.30 north; 590588.05 east 2021-02-12T23:12:33Z- LcmTrn::handleMessages() - handling 41 messages took 0.61 ms and 129 calls... 2021-02-12T23:12:33Z- waiting for 1613171550.87, time is 1613171552.72 2021-02-12T23:12:33Z- LcmTrn::updateTrn() >>>> pose time 1613171552.72, meas time 1613171552.31 <<<< 2021-02-12T23:12:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171552.31 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:12:33Z- TerrainNav::motionUpdate() - currEstimate = (4056424, 590588.05, 5.38) 2021-02-12T23:12:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:12:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:12:33Z- TNavBF::Projecting Measurements 2021-02-12T23:12:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:12:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:12:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:12:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:12:33Z- TNavFilter::4 good beams. 2021-02-12T23:12:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:12:33Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:12:33Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:12:33Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:12:33Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:12:33Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:12:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:12:33Z- TNavBF::mmse selected from filter -1 2021-02-12T23:12:33Z- TerrainMapOctree: (vehN, vehE ) = (4056424.04, 590587.85). 2021-02-12T23:12:33Z- TerrainMapOctree: Min Distance = 6058.16. 2021-02-12T23:12:33Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:12:33Z- There is no specific good beam that all particles have in common. 2021-02-12T23:12:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:12:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 12.71 2021-02-12T23:12:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:12:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 17.31 2021-02-12T23:12:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:12:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 22.61 2021-02-12T23:12:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:12:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-12T23:12:33Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:12:33Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:12:33Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:12:33Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:12:33Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:12:33Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:12:33Z- Filter 0 No beams used in subcloud update 2021-02-12T23:12:33Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:12:33Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:12:33Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 794.5 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:12:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171552.31 sec. 2021-02-12T23:12:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:12:33Z- TNavBF::mmse selected from filter -1 2021-02-12T23:12:33Z- variances([0], [2], and psi_berg) = 794.470371 833.091608 0.000000 2021-02-12T23:12:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:12:33Z- LcmTrn::publishEstimates() - MLE : 4056395.80 590541.55 5.38 2021-02-12T23:12:33Z- LcmTrn::publishEstimates() - MMSE : 4056424.30 590588.05 5.38 2021-02-12T23:12:33Z- LcmTrn::publishEstimates() - COVAR: 794.47 833.09 0.83 0.00 2021-02-12T23:12:33Z- AHRS_M2 msg: 1613171553.10 epoch sec; seqNo:1692 2021-02-12T23:12:33Z- AHRS_M2 msg: 0.63 phi; 0.25 theta; -0.18 psi 2021-02-12T23:12:33Z- handleDepth()-Depth_Keller msg: 1613171553100 epoch sec; seqNo:1706; depth 5.24 2021-02-12T23:12:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171553.11 epoch sec; seqNo:1706; 4056424.68 north; 590588.05 east 2021-02-12T23:12:33Z- AHRS_M2 msg: 1613171553.51 epoch sec; seqNo:1693 2021-02-12T23:12:33Z- AHRS_M2 msg: 0.64 phi; 0.21 theta; -0.20 psi 2021-02-12T23:12:33Z- handleDepth()-Depth_Keller msg: 1613171553516 epoch sec; seqNo:1707; depth 5.11 2021-02-12T23:12:33Z- handleDvl() - RDI_Pathfinder msg: 1613171553.55 epoch sec; seqNo:719 2021-02-12T23:12:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.42 , 102.65 , 110.76 , 82.37 2021-02-12T23:12:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , -0.19 2021-02-12T23:12:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171553.56 epoch sec; seqNo:1707; 4056425.06 north; 590588.04 east 2021-02-12T23:12:33Z- AHRS_M2 msg: 1613171553.90 epoch sec; seqNo:1694 2021-02-12T23:12:33Z- AHRS_M2 msg: 0.65 phi; 0.17 theta; -0.21 psi 2021-02-12T23:12:33Z- handleDepth()-Depth_Keller msg: 1613171553914 epoch sec; seqNo:1708; depth 4.97 2021-02-12T23:12:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171553.92 epoch sec; seqNo:1708; 4056425.44 north; 590588.04 east 2021-02-12T23:12:34Z- AHRS_M2 msg: 1613171554.31 epoch sec; seqNo:1695 2021-02-12T23:12:34Z- AHRS_M2 msg: 0.66 phi; 0.13 theta; -0.20 psi 2021-02-12T23:12:34Z- handleDepth()-Depth_Keller msg: 1613171554317 epoch sec; seqNo:1709; depth 4.89 2021-02-12T23:12:34Z- handleDvl() - RDI_Pathfinder msg: 1613171554.33 epoch sec; seqNo:720 2021-02-12T23:12:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.20 , 105.56 , 83.92 2021-02-12T23:12:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.13 , -0.14 2021-02-12T23:12:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171554.36 epoch sec; seqNo:1709; 4056425.44 north; 590588.04 east 2021-02-12T23:12:34Z- AHRS_M2 msg: 1613171554.71 epoch sec; seqNo:1696 2021-02-12T23:12:34Z- AHRS_M2 msg: 0.68 phi; 0.08 theta; -0.19 psi 2021-02-12T23:12:34Z- handleDepth()-Depth_Keller msg: 1613171554716 epoch sec; seqNo:1710; depth 4.78 2021-02-12T23:12:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171554.73 epoch sec; seqNo:1710; 4056425.82 north; 590588.03 east 2021-02-12T23:12:35Z- AHRS_M2 msg: 1613171555.12 epoch sec; seqNo:1697 2021-02-12T23:12:35Z- AHRS_M2 msg: 0.70 phi; 0.03 theta; -0.17 psi 2021-02-12T23:12:35Z- handleDepth()-Depth_Keller msg: 1613171555129 epoch sec; seqNo:1711; depth 4.72 2021-02-12T23:12:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171555.14 epoch sec; seqNo:1711; 4056426.21 north; 590589.25 east 2021-02-12T23:12:35Z- AHRS_M2 msg: 1613171555.52 epoch sec; seqNo:1698 2021-02-12T23:12:35Z- AHRS_M2 msg: 0.71 phi; -0.02 theta; -0.15 psi 2021-02-12T23:12:35Z- handleDepth()-Depth_Keller msg: 1613171555524 epoch sec; seqNo:1712; depth 4.66 2021-02-12T23:12:35Z- handleDvl() - RDI_Pathfinder msg: 1613171555.54 epoch sec; seqNo:721 2021-02-12T23:12:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.07 , 95.89 , 94.54 , 86.42 2021-02-12T23:12:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.09 , -0.06 2021-02-12T23:12:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171555.56 epoch sec; seqNo:1712; 4056426.59 north; 590589.25 east 2021-02-12T23:12:35Z- AHRS_M2 msg: 1613171555.94 epoch sec; seqNo:1699 2021-02-12T23:12:35Z- AHRS_M2 msg: 0.74 phi; -0.12 theta; -0.12 psi 2021-02-12T23:12:35Z- handleDepth()-Depth_Keller msg: 1613171555946 epoch sec; seqNo:1713; depth 4.61 2021-02-12T23:12:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171555.98 epoch sec; seqNo:1713; 4056426.59 north; 590589.25 east 2021-02-12T23:12:36Z- AHRS_M2 msg: 1613171556.33 epoch sec; seqNo:1700 2021-02-12T23:12:36Z- AHRS_M2 msg: 0.74 phi; -0.16 theta; -0.12 psi 2021-02-12T23:12:36Z- handleDepth()-Depth_Keller msg: 1613171556338 epoch sec; seqNo:1714; depth 4.58 2021-02-12T23:12:36Z- handleDvl() - RDI_Pathfinder msg: 1613171556.35 epoch sec; seqNo:722 2021-02-12T23:12:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.33 , 96.50 , 86.76 , 94.06 2021-02-12T23:12:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.58 , -0.04 , -0.06 2021-02-12T23:12:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171556.36 epoch sec; seqNo:1714; 4056426.97 north; 590589.24 east 2021-02-12T23:12:36Z- AHRS_M2 msg: 1613171556.74 epoch sec; seqNo:1701 2021-02-12T23:12:36Z- AHRS_M2 msg: 0.74 phi; -0.20 theta; -0.12 psi 2021-02-12T23:12:36Z- handleDepth()-Depth_Keller msg: 1613171556740 epoch sec; seqNo:1715; depth 4.54 2021-02-12T23:12:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171556.76 epoch sec; seqNo:1715; 4056426.97 north; 590589.24 east 2021-02-12T23:12:37Z- AHRS_M2 msg: 1613171557.14 epoch sec; seqNo:1702 2021-02-12T23:12:37Z- AHRS_M2 msg: 0.74 phi; -0.23 theta; -0.15 psi 2021-02-12T23:12:37Z- handleDepth()-Depth_Keller msg: 1613171557145 epoch sec; seqNo:1716; depth 4.55 2021-02-12T23:12:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171557.15 epoch sec; seqNo:1716; 4056427.34 north; 590589.24 east 2021-02-12T23:12:37Z- AHRS_M2 msg: 1613171557.54 epoch sec; seqNo:1703 2021-02-12T23:12:37Z- AHRS_M2 msg: 0.74 phi; -0.25 theta; -0.20 psi 2021-02-12T23:12:37Z- handleDepth()-Depth_Keller msg: 1613171557544 epoch sec; seqNo:1717; depth 4.56 2021-02-12T23:12:37Z- handleDvl() - RDI_Pathfinder msg: 1613171557.56 epoch sec; seqNo:723 2021-02-12T23:12:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.98 , 97.71 , 80.68 , 102.58 2021-02-12T23:12:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.56 , -0.18 , -0.09 2021-02-12T23:12:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171557.57 epoch sec; seqNo:1717; 4056427.34 north; 590589.24 east 2021-02-12T23:12:37Z- AHRS_M2 msg: 1613171557.96 epoch sec; seqNo:1704 2021-02-12T23:12:37Z- AHRS_M2 msg: 0.74 phi; -0.28 theta; -0.24 psi 2021-02-12T23:12:37Z- handleDepth()-Depth_Keller msg: 1613171557966 epoch sec; seqNo:1718; depth 4.57 2021-02-12T23:12:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171557.98 epoch sec; seqNo:1718; 4056427.72 north; 590589.24 east 2021-02-12T23:12:38Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-12T23:12:38Z- waiting for 1613171555.72, time is 1613171557.98 2021-02-12T23:12:38Z- LcmTrn::updateTrn() >>>> pose time 1613171557.98, meas time 1613171557.56 <<<< 2021-02-12T23:12:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171557.56 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:12:38Z- TerrainNav::motionUpdate() - currEstimate = (4056428, 590589.24, 4.57) 2021-02-12T23:12:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:12:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:12:38Z- TNavBF::Projecting Measurements 2021-02-12T23:12:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:12:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:12:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:12:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:12:38Z- TNavFilter::4 good beams. 2021-02-12T23:12:38Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:12:38Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:12:38Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:12:38Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:12:38Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:12:38Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:12:38Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:12:38Z- TNavBF::mmse selected from filter -1 2021-02-12T23:12:38Z- TerrainMapOctree: (vehN, vehE ) = (4056427.45, 590589.14). 2021-02-12T23:12:38Z- TerrainMapOctree: Min Distance = 6054.66. 2021-02-12T23:12:38Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:12:38Z- There is no specific good beam that all particles have in common. 2021-02-12T23:12:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:12:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 12.38 2021-02-12T23:12:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:12:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-12T23:12:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:12:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 10.41 2021-02-12T23:12:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:12:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 16.84 2021-02-12T23:12:38Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:12:38Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:12:38Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:12:38Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:12:38Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:12:38Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:12:38Z- Filter 0 No beams used in subcloud update 2021-02-12T23:12:38Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:12:38Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:12:38Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 794.3 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:12:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171557.56 sec. 2021-02-12T23:12:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:12:38Z- TNavBF::mmse selected from filter -1 2021-02-12T23:12:38Z- variances([0], [2], and psi_berg) = 794.233878 833.091158 0.000000 2021-02-12T23:12:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:12:38Z- LcmTrn::publishEstimates() - MLE : 4056399.22 590544.74 4.57 2021-02-12T23:12:38Z- LcmTrn::publishEstimates() - MMSE : 4056427.72 590589.24 4.57 2021-02-12T23:12:38Z- LcmTrn::publishEstimates() - COVAR: 794.23 833.09 0.80 0.00 2021-02-12T23:12:38Z- AHRS_M2 msg: 1613171558.35 epoch sec; seqNo:1705 2021-02-12T23:12:38Z- AHRS_M2 msg: 0.74 phi; -0.30 theta; -0.27 psi 2021-02-12T23:12:38Z- handleDepth()-Depth_Keller msg: 1613171558352 epoch sec; seqNo:1719; depth 4.64 2021-02-12T23:12:38Z- handleDvl() - RDI_Pathfinder msg: 1613171558.37 epoch sec; seqNo:724 2021-02-12T23:12:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.55 , 104.47 , 78.87 , 108.53 2021-02-12T23:12:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.62 , -0.35 , 0.16 2021-02-12T23:12:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171558.38 epoch sec; seqNo:1719; 4056428.10 north; 590589.23 east 2021-02-12T23:12:38Z- AHRS_M2 msg: 1613171558.75 epoch sec; seqNo:1706 2021-02-12T23:12:38Z- AHRS_M2 msg: 0.74 phi; -0.32 theta; -0.27 psi 2021-02-12T23:12:38Z- handleDepth()-Depth_Keller msg: 1613171558756 epoch sec; seqNo:1720; depth 4.67 2021-02-12T23:12:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171558.77 epoch sec; seqNo:1720; 4056428.10 north; 590589.23 east 2021-02-12T23:12:39Z- handleDepth()-Depth_Keller msg: 1613171559177 epoch sec; seqNo:1721; depth 4.74 2021-02-12T23:12:39Z- handleDvl() - RDI_Pathfinder msg: 1613171559.19 epoch sec; seqNo:725 2021-02-12T23:12:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 112.38 , 79.28 , 117.79 2021-02-12T23:12:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.59 , -0.07 , 0.52 2021-02-12T23:12:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171559.21 epoch sec; seqNo:1721; 4056428.11 north; 590590.45 east 2021-02-12T23:12:39Z- AHRS_M2 msg: 1613171559.56 epoch sec; seqNo:1707 2021-02-12T23:12:39Z- AHRS_M2 msg: 0.73 phi; -0.36 theta; -0.23 psi 2021-02-12T23:12:39Z- handleDepth()-Depth_Keller msg: 1613171559566 epoch sec; seqNo:1722; depth 4.83 2021-02-12T23:12:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171559.58 epoch sec; seqNo:1722; 4056428.49 north; 590590.45 east 2021-02-12T23:12:39Z- AHRS_M2 msg: 1613171559.96 epoch sec; seqNo:1708 2021-02-12T23:12:39Z- AHRS_M2 msg: 0.71 phi; -0.38 theta; -0.21 psi 2021-02-12T23:12:39Z- handleDepth()-Depth_Keller msg: 1613171559969 epoch sec; seqNo:1723; depth 4.92 2021-02-12T23:12:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171560.00 epoch sec; seqNo:1723; 4056428.49 north; 590590.45 east 2021-02-12T23:12:40Z- AHRS_M2 msg: 1613171560.37 epoch sec; seqNo:1709 2021-02-12T23:12:40Z- AHRS_M2 msg: 0.69 phi; -0.39 theta; -0.20 psi 2021-02-12T23:12:40Z- handleDepth()-Depth_Keller msg: 1613171560372 epoch sec; seqNo:1724; depth 5.00 2021-02-12T23:12:40Z- handleDvl() - RDI_Pathfinder msg: 1613171560.38 epoch sec; seqNo:726 2021-02-12T23:12:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.71 , 110.69 , 77.24 , 124.21 2021-02-12T23:12:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , 0.33 , 0.37 2021-02-12T23:12:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171560.40 epoch sec; seqNo:1724; 4056428.49 north; 590590.45 east 2021-02-12T23:12:40Z- AHRS_M2 msg: 1613171560.77 epoch sec; seqNo:1710 2021-02-12T23:12:40Z- AHRS_M2 msg: 0.67 phi; -0.38 theta; -0.21 psi 2021-02-12T23:12:40Z- handleDepth()-Depth_Keller msg: 1613171560781 epoch sec; seqNo:1725; depth 5.09 2021-02-12T23:12:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171560.79 epoch sec; seqNo:1725; 4056428.49 north; 590590.45 east 2021-02-12T23:12:41Z- AHRS_M2 msg: 1613171561.19 epoch sec; seqNo:1711 2021-02-12T23:12:41Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.24 psi 2021-02-12T23:12:41Z- handleDepth()-Depth_Keller msg: 1613171561199 epoch sec; seqNo:1726; depth 5.20 2021-02-12T23:12:41Z- handleDvl() - RDI_Pathfinder msg: 1613171561.21 epoch sec; seqNo:727 2021-02-12T23:12:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 109.21 , 75.60 , 126.78 2021-02-12T23:12:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.33 , -0.10 2021-02-12T23:12:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171561.22 epoch sec; seqNo:1726; 4056428.87 north; 590590.44 east 2021-02-12T23:12:41Z- AHRS_M2 msg: 1613171561.58 epoch sec; seqNo:1712 2021-02-12T23:12:41Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.26 psi 2021-02-12T23:12:41Z- handleDepth()-Depth_Keller msg: 1613171561584 epoch sec; seqNo:1727; depth 5.30 2021-02-12T23:12:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171561.60 epoch sec; seqNo:1727; 4056428.89 north; 590591.67 east 2021-02-12T23:12:41Z- AHRS_M2 msg: 1613171561.98 epoch sec; seqNo:1713 2021-02-12T23:12:41Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.28 psi 2021-02-12T23:12:41Z- handleDepth()-Depth_Keller msg: 1613171561989 epoch sec; seqNo:1728; depth 5.42 2021-02-12T23:12:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171562.00 epoch sec; seqNo:1728; 4056429.26 north; 590591.66 east 2021-02-12T23:12:42Z- handleDepth()-Depth_Keller msg: 1613171562409 epoch sec; seqNo:1729; depth 5.53 2021-02-12T23:12:42Z- handleDvl() - RDI_Pathfinder msg: 1613171562.42 epoch sec; seqNo:728 2021-02-12T23:12:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 113.26 , 80.81 , 125.43 2021-02-12T23:12:42Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , -0.01 , -0.32 2021-02-12T23:12:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171562.44 epoch sec; seqNo:1729; 4056429.64 north; 590591.66 east 2021-02-12T23:12:42Z- AHRS_M2 msg: 1613171562.79 epoch sec; seqNo:1714 2021-02-12T23:12:42Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.27 psi 2021-02-12T23:12:42Z- handleDepth()-Depth_Keller msg: 1613171562799 epoch sec; seqNo:1730; depth 5.67 2021-02-12T23:12:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171562.81 epoch sec; seqNo:1730; 4056430.02 north; 590591.65 east 2021-02-12T23:12:43Z- AHRS_M2 msg: 1613171563.20 epoch sec; seqNo:1715 2021-02-12T23:12:43Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.25 psi 2021-02-12T23:12:43Z- handleDepth()-Depth_Keller msg: 1613171563200 epoch sec; seqNo:1731; depth 5.81 2021-02-12T23:12:43Z- handleDvl() - RDI_Pathfinder msg: 1613171563.22 epoch sec; seqNo:729 2021-02-12T23:12:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.09 , 112.86 , 78.87 , 120.29 2021-02-12T23:12:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.20 , -0.12 2021-02-12T23:12:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171563.23 epoch sec; seqNo:1731; 4056430.41 north; 590592.87 east 2021-02-12T23:12:43Z- LcmTrn::handleMessages() - handling 43 messages took 0.57 ms and 132 calls... 2021-02-12T23:12:43Z- waiting for 1613171560.98, time is 1613171563.23 2021-02-12T23:12:43Z- LcmTrn::updateTrn() equating pose 1613171563.23 and meas times 1613171563.23 2021-02-12T23:12:43Z- LcmTrn::updateTrn() >>>> pose time 1613171563.23, meas time 1613171563.23 <<<< 2021-02-12T23:12:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171563.23 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:12:43Z- TerrainNav::motionUpdate() - currEstimate = (4056430, 590592.87, 5.81) 2021-02-12T23:12:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:12:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:12:43Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171563.23 sec.; No relevant navigation data available 2021-02-12T23:12:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:12:43Z- TNavBF::mmse selected from filter -1 2021-02-12T23:12:43Z- variances([0], [2], and psi_berg) = 794.021958 833.090756 0.000000 2021-02-12T23:12:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:12:43Z- LcmTrn::publishEstimates() - MLE : 4056401.91 590549.37 5.81 2021-02-12T23:12:43Z- LcmTrn::publishEstimates() - MMSE : 4056430.41 590592.87 5.81 2021-02-12T23:12:43Z- LcmTrn::publishEstimates() - COVAR: 794.02 833.09 1.03 0.00 2021-02-12T23:12:43Z- AHRS_M2 msg: 1613171563.60 epoch sec; seqNo:1716 2021-02-12T23:12:43Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.22 psi 2021-02-12T23:12:43Z- handleDepth()-Depth_Keller msg: 1613171563604 epoch sec; seqNo:1732; depth 5.96 2021-02-12T23:12:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171563.62 epoch sec; seqNo:1732; 4056430.79 north; 590592.87 east 2021-02-12T23:12:44Z- AHRS_M2 msg: 1613171564.00 epoch sec; seqNo:1717 2021-02-12T23:12:44Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.21 psi 2021-02-12T23:12:44Z- handleDepth()-Depth_Keller msg: 1613171564010 epoch sec; seqNo:1733; depth 6.07 2021-02-12T23:12:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171564.02 epoch sec; seqNo:1733; 4056430.79 north; 590592.87 east 2021-02-12T23:12:44Z- AHRS_M2 msg: 1613171564.41 epoch sec; seqNo:1718 2021-02-12T23:12:44Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.19 psi 2021-02-12T23:12:44Z- handleDepth()-Depth_Keller msg: 1613171564417 epoch sec; seqNo:1734; depth 6.21 2021-02-12T23:12:44Z- handleDvl() - RDI_Pathfinder msg: 1613171564.43 epoch sec; seqNo:730 2021-02-12T23:12:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 107.85 , 76.83 , 120.02 2021-02-12T23:12:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.63 , -0.06 , 0.01 2021-02-12T23:12:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171564.44 epoch sec; seqNo:1734; 4056431.17 north; 590592.86 east 2021-02-12T23:12:44Z- AHRS_M2 msg: 1613171564.81 epoch sec; seqNo:1719 2021-02-12T23:12:44Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.19 psi 2021-02-12T23:12:44Z- handleDepth()-Depth_Keller msg: 1613171564821 epoch sec; seqNo:1735; depth 6.30 2021-02-12T23:12:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171564.83 epoch sec; seqNo:1735; 4056431.55 north; 590592.86 east 2021-02-12T23:12:45Z- AHRS_M2 msg: 1613171565.21 epoch sec; seqNo:1720 2021-02-12T23:12:45Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.20 psi 2021-02-12T23:12:45Z- handleDepth()-Depth_Keller msg: 1613171565220 epoch sec; seqNo:1736; depth 6.41 2021-02-12T23:12:45Z- handleDvl() - RDI_Pathfinder msg: 1613171565.23 epoch sec; seqNo:731 2021-02-12T23:12:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.71 , 105.28 , 74.37 , 120.16 2021-02-12T23:12:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.01 , -0.05 2021-02-12T23:12:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171565.24 epoch sec; seqNo:1736; 4056431.55 north; 590592.86 east 2021-02-12T23:12:45Z- AHRS_M2 msg: 1613171565.63 epoch sec; seqNo:1721 2021-02-12T23:12:45Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.21 psi 2021-02-12T23:12:45Z- handleDepth()-Depth_Keller msg: 1613171565632 epoch sec; seqNo:1737; depth 6.54 2021-02-12T23:12:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171565.64 epoch sec; seqNo:1737; 4056431.93 north; 590592.85 east 2021-02-12T23:12:46Z- AHRS_M2 msg: 1613171566.02 epoch sec; seqNo:1722 2021-02-12T23:12:46Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.22 psi 2021-02-12T23:12:46Z- handleDepth()-Depth_Keller msg: 1613171566028 epoch sec; seqNo:1738; depth 6.65 2021-02-12T23:12:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171566.04 epoch sec; seqNo:1738; 4056431.93 north; 590592.85 east 2021-02-12T23:12:46Z- AHRS_M2 msg: 1613171566.43 epoch sec; seqNo:1723 2021-02-12T23:12:46Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.23 psi 2021-02-12T23:12:46Z- handleDepth()-Depth_Keller msg: 1613171566432 epoch sec; seqNo:1739; depth 6.75 2021-02-12T23:12:46Z- handleDvl() - RDI_Pathfinder msg: 1613171566.44 epoch sec; seqNo:732 2021-02-12T23:12:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 108.12 , 78.38 , 121.64 2021-02-12T23:12:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.12 , -0.04 2021-02-12T23:12:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171566.46 epoch sec; seqNo:1739; 4056432.31 north; 590592.85 east 2021-02-12T23:12:46Z- AHRS_M2 msg: 1613171566.83 epoch sec; seqNo:1724 2021-02-12T23:12:46Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.23 psi 2021-02-12T23:12:46Z- handleDepth()-Depth_Keller msg: 1613171566836 epoch sec; seqNo:1740; depth 6.89 2021-02-12T23:12:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171566.85 epoch sec; seqNo:1740; 4056432.70 north; 590594.07 east 2021-02-12T23:12:47Z- AHRS_M2 msg: 1613171567.24 epoch sec; seqNo:1725 2021-02-12T23:12:47Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.23 psi 2021-02-12T23:12:47Z- handleDepth()-Depth_Keller msg: 1613171567246 epoch sec; seqNo:1741; depth 7.01 2021-02-12T23:12:47Z- handleDvl() - RDI_Pathfinder msg: 1613171567.26 epoch sec; seqNo:733 2021-02-12T23:12:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 106.63 , 75.59 , 117.44 2021-02-12T23:12:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.14 , -0.03 2021-02-12T23:12:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171567.27 epoch sec; seqNo:1741; 4056433.08 north; 590594.06 east 2021-02-12T23:12:47Z- AHRS_M2 msg: 1613171567.64 epoch sec; seqNo:1726 2021-02-12T23:12:47Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.22 psi 2021-02-12T23:12:47Z- handleDepth()-Depth_Keller msg: 1613171567644 epoch sec; seqNo:1742; depth 7.15 2021-02-12T23:12:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171567.65 epoch sec; seqNo:1742; 4056433.46 north; 590594.06 east 2021-02-12T23:12:48Z- AHRS_M2 msg: 1613171568.04 epoch sec; seqNo:1727 2021-02-12T23:12:48Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.21 psi 2021-02-12T23:12:48Z- handleDepth()-Depth_Keller msg: 1613171568048 epoch sec; seqNo:1743; depth 7.26 2021-02-12T23:12:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171568.06 epoch sec; seqNo:1743; 4056433.84 north; 590594.06 east 2021-02-12T23:12:48Z- LcmTrn::handleMessages() - handling 40 messages took 0.61 ms and 128 calls... 2021-02-12T23:12:48Z- waiting for 1613171566.23, time is 1613171568.06 2021-02-12T23:12:48Z- LcmTrn::updateTrn() >>>> pose time 1613171568.06, meas time 1613171567.26 <<<< 2021-02-12T23:12:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171567.26 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:12:48Z- TerrainNav::motionUpdate() - currEstimate = (4056434, 590594.06, 7.26) 2021-02-12T23:12:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:12:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:12:48Z- TNavBF::Projecting Measurements 2021-02-12T23:12:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:12:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:12:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:12:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:12:48Z- TNavFilter::4 good beams. 2021-02-12T23:12:48Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:12:48Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:12:48Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:12:48Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:12:48Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:12:48Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:12:48Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:12:48Z- TNavBF::mmse selected from filter -1 2021-02-12T23:12:48Z- TerrainMapOctree: (vehN, vehE ) = (4056433.27, 590593.86). 2021-02-12T23:12:48Z- TerrainMapOctree: Min Distance = 6047.17. 2021-02-12T23:12:48Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:12:48Z- There is no specific good beam that all particles have in common. 2021-02-12T23:12:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:12:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 11.56 2021-02-12T23:12:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:12:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 18.19 2021-02-12T23:12:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:12:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 9.14 2021-02-12T23:12:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:12:48Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 22.07 2021-02-12T23:12:48Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:12:48Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:12:48Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:12:48Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:12:48Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:12:48Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:12:48Z- Filter 0 No beams used in subcloud update 2021-02-12T23:12:48Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:12:48Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:12:48Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 793.8 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:12:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171567.26 sec. 2021-02-12T23:12:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:12:48Z- TNavBF::mmse selected from filter -1 2021-02-12T23:12:48Z- variances([0], [2], and psi_berg) = 793.772931 833.090287 0.000000 2021-02-12T23:12:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:12:48Z- LcmTrn::publishEstimates() - MLE : 4056405.34 590549.56 7.26 2021-02-12T23:12:48Z- LcmTrn::publishEstimates() - MMSE : 4056433.84 590594.06 7.26 2021-02-12T23:12:48Z- LcmTrn::publishEstimates() - COVAR: 793.77 833.09 0.94 0.00 2021-02-12T23:12:48Z- AHRS_M2 msg: 1613171568.45 epoch sec; seqNo:1728 2021-02-12T23:12:48Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-12T23:12:48Z- handleDepth()-Depth_Keller msg: 1613171568457 epoch sec; seqNo:1744; depth 7.38 2021-02-12T23:12:48Z- handleDvl() - RDI_Pathfinder msg: 1613171568.47 epoch sec; seqNo:734 2021-02-12T23:12:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.20 , 106.83 , 76.00 , 116.29 2021-02-12T23:12:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.15 , -0.10 2021-02-12T23:12:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171568.48 epoch sec; seqNo:1744; 4056433.84 north; 590594.06 east 2021-02-12T23:12:48Z- AHRS_M2 msg: 1613171568.85 epoch sec; seqNo:1729 2021-02-12T23:12:48Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.19 psi 2021-02-12T23:12:48Z- handleDepth()-Depth_Keller msg: 1613171568858 epoch sec; seqNo:1745; depth 7.52 2021-02-12T23:12:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171568.87 epoch sec; seqNo:1745; 4056434.21 north; 590594.05 east 2021-02-12T23:12:49Z- AHRS_M2 msg: 1613171569.26 epoch sec; seqNo:1730 2021-02-12T23:12:49Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.19 psi 2021-02-12T23:12:49Z- handleDepth()-Depth_Keller msg: 1613171569273 epoch sec; seqNo:1746; depth 7.63 2021-02-12T23:12:49Z- handleDvl() - RDI_Pathfinder msg: 1613171569.29 epoch sec; seqNo:735 2021-02-12T23:12:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.21 , 104.14 , 78.45 , 113.60 2021-02-12T23:12:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.22 , -0.04 2021-02-12T23:12:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171569.30 epoch sec; seqNo:1746; 4056434.59 north; 590594.05 east 2021-02-12T23:12:49Z- AHRS_M2 msg: 1613171569.66 epoch sec; seqNo:1731 2021-02-12T23:12:49Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.19 psi 2021-02-12T23:12:49Z- handleDepth()-Depth_Keller msg: 1613171569664 epoch sec; seqNo:1747; depth 7.78 2021-02-12T23:12:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171569.67 epoch sec; seqNo:1747; 4056434.97 north; 590594.04 east 2021-02-12T23:12:50Z- AHRS_M2 msg: 1613171570.07 epoch sec; seqNo:1732 2021-02-12T23:12:50Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.19 psi 2021-02-12T23:12:50Z- handleDepth()-Depth_Keller msg: 1613171570083 epoch sec; seqNo:1748; depth 7.88 2021-02-12T23:12:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171570.14 epoch sec; seqNo:1748; 4056435.35 north; 590594.04 east 2021-02-12T23:12:50Z- AHRS_M2 msg: 1613171570.47 epoch sec; seqNo:1733 2021-02-12T23:12:50Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.18 psi 2021-02-12T23:12:50Z- handleDepth()-Depth_Keller msg: 1613171570477 epoch sec; seqNo:1749; depth 8.01 2021-02-12T23:12:50Z- handleDvl() - RDI_Pathfinder msg: 1613171570.49 epoch sec; seqNo:736 2021-02-12T23:12:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.27 , 101.49 , 75.18 , 110.96 2021-02-12T23:12:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.14 , 0.10 2021-02-12T23:12:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171570.51 epoch sec; seqNo:1749; 4056435.36 north; 590595.26 east 2021-02-12T23:12:50Z- AHRS_M2 msg: 1613171570.87 epoch sec; seqNo:1734 2021-02-12T23:12:50Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.18 psi 2021-02-12T23:12:50Z- handleDepth()-Depth_Keller msg: 1613171570881 epoch sec; seqNo:1750; depth 8.13 2021-02-12T23:12:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171570.89 epoch sec; seqNo:1750; 4056435.74 north; 590595.26 east 2021-02-12T23:12:51Z- AHRS_M2 msg: 1613171571.28 epoch sec; seqNo:1735 2021-02-12T23:12:51Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.17 psi 2021-02-12T23:12:51Z- handleDepth()-Depth_Keller msg: 1613171571286 epoch sec; seqNo:1751; depth 8.24 2021-02-12T23:12:51Z- handleDvl() - RDI_Pathfinder msg: 1613171571.30 epoch sec; seqNo:737 2021-02-12T23:12:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 96.29 , 75.18 , 113.86 2021-02-12T23:12:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , 0.02 , 0.14 2021-02-12T23:12:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171571.31 epoch sec; seqNo:1751; 4056435.74 north; 590595.26 east 2021-02-12T23:12:51Z- AHRS_M2 msg: 1613171571.68 epoch sec; seqNo:1736 2021-02-12T23:12:51Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.17 psi 2021-02-12T23:12:51Z- handleDepth()-Depth_Keller msg: 1613171571684 epoch sec; seqNo:1752; depth 8.36 2021-02-12T23:12:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171571.70 epoch sec; seqNo:1752; 4056436.12 north; 590595.25 east 2021-02-12T23:12:52Z- AHRS_M2 msg: 1613171572.10 epoch sec; seqNo:1737 2021-02-12T23:12:52Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.18 psi 2021-02-12T23:12:52Z- handleDepth()-Depth_Keller msg: 1613171572107 epoch sec; seqNo:1753; depth 8.47 2021-02-12T23:12:52Z- handleDvl() - RDI_Pathfinder msg: 1613171572.14 epoch sec; seqNo:738 2021-02-12T23:12:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.13 , 101.70 , 76.01 , 112.52 2021-02-12T23:12:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , 0.06 , 0.07 2021-02-12T23:12:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171572.15 epoch sec; seqNo:1753; 4056436.12 north; 590595.25 east 2021-02-12T23:12:52Z- AHRS_M2 msg: 1613171572.50 epoch sec; seqNo:1738 2021-02-12T23:12:52Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.18 psi 2021-02-12T23:12:52Z- handleDepth()-Depth_Keller msg: 1613171572505 epoch sec; seqNo:1754; depth 8.60 2021-02-12T23:12:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:12:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:12:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:12:52Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:12:52Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:12:52Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:12:52Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:12:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:12:52Z- handleDvl() - RDI_Pathfinder msg: 1613171572.51 epoch sec; seqNo:739 2021-02-12T23:12:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:12:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , 0.06 , 0.07 2021-02-12T23:12:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171572.54 epoch sec; seqNo:1754; 4056436.50 north; 590595.25 east 2021-02-12T23:12:52Z- AHRS_M2 msg: 1613171572.89 epoch sec; seqNo:1739 2021-02-12T23:12:52Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.19 psi 2021-02-12T23:12:52Z- handleDepth()-Depth_Keller msg: 1613171572896 epoch sec; seqNo:1755; depth 8.68 2021-02-12T23:12:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171572.91 epoch sec; seqNo:1755; 4056436.50 north; 590595.25 east 2021-02-12T23:12:53Z- AHRS_M2 msg: 1613171573.30 epoch sec; seqNo:1740 2021-02-12T23:12:53Z- AHRS_M2 msg: 0.58 phi; -0.29 theta; -0.19 psi 2021-02-12T23:12:53Z- handleDepth()-Depth_Keller msg: 1613171573305 epoch sec; seqNo:1756; depth 8.81 2021-02-12T23:12:53Z- handleDvl() - RDI_Pathfinder msg: 1613171573.32 epoch sec; seqNo:740 2021-02-12T23:12:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 96.23 , 74.78 , 111.10 2021-02-12T23:12:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.04 , 0.01 2021-02-12T23:12:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171573.33 epoch sec; seqNo:1756; 4056436.88 north; 590595.24 east 2021-02-12T23:12:53Z- LcmTrn::handleMessages() - handling 46 messages took 0.79 ms and 130 calls... 2021-02-12T23:12:53Z- waiting for 1613171571.06, time is 1613171573.33 2021-02-12T23:12:53Z- LcmTrn::updateTrn() equating pose 1613171573.33 and meas times 1613171573.33 2021-02-12T23:12:53Z- LcmTrn::updateTrn() >>>> pose time 1613171573.33, meas time 1613171573.33 <<<< 2021-02-12T23:12:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171573.33 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:12:53Z- TerrainNav::motionUpdate() - currEstimate = (4056437, 590595.24, 8.81) 2021-02-12T23:12:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:12:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:12:53Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171573.33 sec.; No relevant navigation data available 2021-02-12T23:12:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:12:53Z- TNavBF::mmse selected from filter -1 2021-02-12T23:12:53Z- variances([0], [2], and psi_berg) = 793.594495 833.089952 0.000000 2021-02-12T23:12:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:12:53Z- LcmTrn::publishEstimates() - MLE : 4056409.38 590545.74 8.81 2021-02-12T23:12:53Z- LcmTrn::publishEstimates() - MMSE : 4056436.88 590595.24 8.81 2021-02-12T23:12:53Z- LcmTrn::publishEstimates() - COVAR: 793.59 833.09 1.11 0.00 2021-02-12T23:12:53Z- AHRS_M2 msg: 1613171573.71 epoch sec; seqNo:1741 2021-02-12T23:12:53Z- AHRS_M2 msg: 0.57 phi; -0.28 theta; -0.20 psi 2021-02-12T23:12:53Z- handleDepth()-Depth_Keller msg: 1613171573710 epoch sec; seqNo:1757; depth 8.93 2021-02-12T23:12:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:12:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:12:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:12:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:12:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:12:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:12:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:12:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:12:53Z- handleDvl() - RDI_Pathfinder msg: 1613171573.71 epoch sec; seqNo:741 2021-02-12T23:12:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:12:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.04 , 0.01 2021-02-12T23:12:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171573.72 epoch sec; seqNo:1757; 4056437.27 north; 590596.46 east 2021-02-12T23:12:54Z- handleDepth()-Depth_Keller msg: 1613171574146 epoch sec; seqNo:1758; depth 9.03 2021-02-12T23:12:54Z- handleDvl() - RDI_Pathfinder msg: 1613171574.16 epoch sec; seqNo:742 2021-02-12T23:12:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 96.43 , 74.37 , 108.60 2021-02-12T23:12:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , -0.06 2021-02-12T23:12:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171574.17 epoch sec; seqNo:1758; 4056437.65 north; 590596.46 east 2021-02-12T23:12:54Z- AHRS_M2 msg: 1613171574.51 epoch sec; seqNo:1742 2021-02-12T23:12:54Z- AHRS_M2 msg: 0.56 phi; -0.28 theta; -0.20 psi 2021-02-12T23:12:54Z- handleDepth()-Depth_Keller msg: 1613171574513 epoch sec; seqNo:1759; depth 9.15 2021-02-12T23:12:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171574.53 epoch sec; seqNo:1759; 4056438.03 north; 590596.45 east 2021-02-12T23:12:54Z- AHRS_M2 msg: 1613171574.91 epoch sec; seqNo:1743 2021-02-12T23:12:54Z- AHRS_M2 msg: 0.57 phi; -0.28 theta; -0.20 psi 2021-02-12T23:12:54Z- handleDepth()-Depth_Keller msg: 1613171574916 epoch sec; seqNo:1760; depth 9.25 2021-02-12T23:12:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:12:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:12:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:12:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:12:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:12:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:12:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:12:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:12:54Z- handleDvl() - RDI_Pathfinder msg: 1613171574.92 epoch sec; seqNo:743 2021-02-12T23:12:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:12:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , -0.06 2021-02-12T23:12:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171574.93 epoch sec; seqNo:1760; 4056438.03 north; 590596.45 east 2021-02-12T23:12:55Z- AHRS_M2 msg: 1613171575.32 epoch sec; seqNo:1744 2021-02-12T23:12:55Z- AHRS_M2 msg: 0.57 phi; -0.28 theta; -0.19 psi 2021-02-12T23:12:55Z- handleDepth()-Depth_Keller msg: 1613171575320 epoch sec; seqNo:1761; depth 9.37 2021-02-12T23:12:55Z- handleDvl() - RDI_Pathfinder msg: 1613171575.33 epoch sec; seqNo:744 2021-02-12T23:12:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 96.50 , 75.60 , 106.23 2021-02-12T23:12:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , -0.13 2021-02-12T23:12:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171575.34 epoch sec; seqNo:1761; 4056438.41 north; 590596.45 east 2021-02-12T23:12:55Z- AHRS_M2 msg: 1613171575.72 epoch sec; seqNo:1745 2021-02-12T23:12:55Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.18 psi 2021-02-12T23:12:55Z- handleDepth()-Depth_Keller msg: 1613171575726 epoch sec; seqNo:1762; depth 9.48 2021-02-12T23:12:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171575.74 epoch sec; seqNo:1762; 4056438.79 north; 590596.45 east 2021-02-12T23:12:56Z- AHRS_M2 msg: 1613171576.15 epoch sec; seqNo:1746 2021-02-12T23:12:56Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.18 psi 2021-02-12T23:12:56Z- handleDepth()-Depth_Keller msg: 1613171576158 epoch sec; seqNo:1763; depth 9.60 2021-02-12T23:12:56Z- handleDvl() - RDI_Pathfinder msg: 1613171576.17 epoch sec; seqNo:745 2021-02-12T23:12:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 96.56 , 74.78 , 106.29 2021-02-12T23:12:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , -0.16 2021-02-12T23:12:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171576.18 epoch sec; seqNo:1763; 4056439.18 north; 590597.66 east 2021-02-12T23:12:56Z- AHRS_M2 msg: 1613171576.53 epoch sec; seqNo:1747 2021-02-12T23:12:56Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.17 psi 2021-02-12T23:12:56Z- handleDepth()-Depth_Keller msg: 1613171576537 epoch sec; seqNo:1764; depth 9.72 2021-02-12T23:12:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171576.55 epoch sec; seqNo:1764; 4056439.56 north; 590597.66 east 2021-02-12T23:12:56Z- AHRS_M2 msg: 1613171576.93 epoch sec; seqNo:1748 2021-02-12T23:12:56Z- AHRS_M2 msg: 0.59 phi; -0.27 theta; -0.17 psi 2021-02-12T23:12:56Z- handleDepth()-Depth_Keller msg: 1613171576939 epoch sec; seqNo:1765; depth 9.79 2021-02-12T23:12:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171576.95 epoch sec; seqNo:1765; 4056439.94 north; 590597.65 east 2021-02-12T23:12:57Z- AHRS_M2 msg: 1613171577.34 epoch sec; seqNo:1749 2021-02-12T23:12:57Z- AHRS_M2 msg: 0.59 phi; -0.27 theta; -0.16 psi 2021-02-12T23:12:57Z- handleDepth()-Depth_Keller msg: 1613171577340 epoch sec; seqNo:1766; depth 9.91 2021-02-12T23:12:57Z- handleDvl() - RDI_Pathfinder msg: 1613171577.36 epoch sec; seqNo:746 2021-02-12T23:12:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 96.56 , 74.37 , 106.29 2021-02-12T23:12:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.17 , -0.06 2021-02-12T23:12:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171577.37 epoch sec; seqNo:1766; 4056439.94 north; 590597.65 east 2021-02-12T23:12:57Z- AHRS_M2 msg: 1613171577.74 epoch sec; seqNo:1750 2021-02-12T23:12:57Z- AHRS_M2 msg: 0.59 phi; -0.27 theta; -0.15 psi 2021-02-12T23:12:57Z- handleDepth()-Depth_Keller msg: 1613171577746 epoch sec; seqNo:1767; depth 10.03 2021-02-12T23:12:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171577.77 epoch sec; seqNo:1767; 4056440.31 north; 590597.65 east 2021-02-12T23:12:58Z- AHRS_M2 msg: 1613171578.16 epoch sec; seqNo:1751 2021-02-12T23:12:58Z- AHRS_M2 msg: 0.59 phi; -0.27 theta; -0.14 psi 2021-02-12T23:12:58Z- handleDepth()-Depth_Keller msg: 1613171578166 epoch sec; seqNo:1768; depth 10.13 2021-02-12T23:12:58Z- handleDvl() - RDI_Pathfinder msg: 1613171578.19 epoch sec; seqNo:747 2021-02-12T23:12:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.18 , 97.78 , 73.55 , 105.07 2021-02-12T23:12:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.16 , 0.08 2021-02-12T23:12:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171578.20 epoch sec; seqNo:1768; 4056440.69 north; 590597.65 east 2021-02-12T23:12:58Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-12T23:12:58Z- waiting for 1613171576.33, time is 1613171578.20 2021-02-12T23:12:58Z- LcmTrn::updateTrn() equating pose 1613171578.20 and meas times 1613171578.20 2021-02-12T23:12:58Z- LcmTrn::updateTrn() >>>> pose time 1613171578.20, meas time 1613171578.20 <<<< 2021-02-12T23:12:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171578.20 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:12:58Z- TerrainNav::motionUpdate() - currEstimate = (4056441, 590597.65, 10.13) 2021-02-12T23:12:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:12:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:12:58Z- TNavBF::Projecting Measurements 2021-02-12T23:12:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:12:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:12:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:12:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:12:58Z- TNavFilter::4 good beams. 2021-02-12T23:12:58Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:12:58Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:12:58Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:12:58Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:12:58Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:12:58Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:12:58Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:12:58Z- TNavBF::mmse selected from filter -1 2021-02-12T23:12:58Z- TerrainMapOctree: (vehN, vehE ) = (4056440.69, 590597.65). 2021-02-12T23:12:58Z- TerrainMapOctree: Min Distance = 6038.97. 2021-02-12T23:12:58Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:12:58Z- There is no specific good beam that all particles have in common. 2021-02-12T23:12:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:12:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 11.07 2021-02-12T23:12:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:12:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-12T23:12:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:12:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 8.66 2021-02-12T23:12:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:12:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 17.67 2021-02-12T23:12:58Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:12:58Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:12:58Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:12:58Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:12:58Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:12:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:12:58Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:12:58Z- Filter 0 No beams used in subcloud update 2021-02-12T23:12:58Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:12:58Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:12:58Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 793.4 EastVariance: 833.1 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:12:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171578.20 sec. 2021-02-12T23:12:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:12:58Z- TNavBF::mmse selected from filter -1 2021-02-12T23:12:58Z- variances([0], [2], and psi_berg) = 793.385679 833.089562 0.000000 2021-02-12T23:12:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:12:58Z- LcmTrn::publishEstimates() - MLE : 4056413.19 590548.15 10.13 2021-02-12T23:12:58Z- LcmTrn::publishEstimates() - MMSE : 4056440.69 590597.65 10.13 2021-02-12T23:12:58Z- LcmTrn::publishEstimates() - COVAR: 793.39 833.09 0.97 0.00 2021-02-12T23:12:58Z- AHRS_M2 msg: 1613171578.55 epoch sec; seqNo:1752 2021-02-12T23:12:58Z- AHRS_M2 msg: 0.60 phi; -0.27 theta; -0.13 psi 2021-02-12T23:12:58Z- handleDepth()-Depth_Keller msg: 1613171578554 epoch sec; seqNo:1769; depth 10.25 2021-02-12T23:12:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:12:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:12:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:12:58Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:12:58Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:12:58Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:12:58Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:12:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:12:58Z- handleDvl() - RDI_Pathfinder msg: 1613171578.56 epoch sec; seqNo:748 2021-02-12T23:12:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:12:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.16 , 0.08 2021-02-12T23:12:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171578.57 epoch sec; seqNo:1769; 4056441.07 north; 590597.64 east 2021-02-12T23:12:58Z- AHRS_M2 msg: 1613171578.95 epoch sec; seqNo:1753 2021-02-12T23:12:58Z- AHRS_M2 msg: 0.60 phi; -0.27 theta; -0.12 psi 2021-02-12T23:12:58Z- handleDepth()-Depth_Keller msg: 1613171578956 epoch sec; seqNo:1770; depth 10.33 2021-02-12T23:12:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171578.97 epoch sec; seqNo:1770; 4056441.45 north; 590597.64 east 2021-02-12T23:12:59Z- AHRS_M2 msg: 1613171579.36 epoch sec; seqNo:1754 2021-02-12T23:12:59Z- AHRS_M2 msg: 0.61 phi; -0.27 theta; -0.12 psi 2021-02-12T23:12:59Z- handleDepth()-Depth_Keller msg: 1613171579360 epoch sec; seqNo:1771; depth 10.44 2021-02-12T23:12:59Z- handleDvl() - RDI_Pathfinder msg: 1613171579.37 epoch sec; seqNo:749 2021-02-12T23:12:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.18 , 94.13 , 73.96 , 105.07 2021-02-12T23:12:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.10 , 0.06 2021-02-12T23:12:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171579.40 epoch sec; seqNo:1771; 4056441.45 north; 590597.64 east 2021-02-12T23:12:59Z- AHRS_M2 msg: 1613171579.76 epoch sec; seqNo:1755 2021-02-12T23:12:59Z- AHRS_M2 msg: 0.61 phi; -0.27 theta; -0.12 psi 2021-02-12T23:12:59Z- handleDepth()-Depth_Keller msg: 1613171579764 epoch sec; seqNo:1772; depth 10.54 2021-02-12T23:12:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:12:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:12:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:12:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:12:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:12:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:12:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:12:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:12:59Z- handleDvl() - RDI_Pathfinder msg: 1613171579.77 epoch sec; seqNo:750 2021-02-12T23:12:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:12:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.10 , 0.06 2021-02-12T23:12:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171579.78 epoch sec; seqNo:1772; 4056441.84 north; 590598.86 east 2021-02-12T23:13:00Z- AHRS_M2 msg: 1613171580.17 epoch sec; seqNo:1756 2021-02-12T23:13:00Z- AHRS_M2 msg: 0.62 phi; -0.27 theta; -0.12 psi 2021-02-12T23:13:00Z- handleDepth()-Depth_Keller msg: 1613171580171 epoch sec; seqNo:1773; depth 10.65 2021-02-12T23:13:00Z- handleDvl() - RDI_Pathfinder msg: 1613171580.18 epoch sec; seqNo:751 2021-02-12T23:13:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.46 , 94.19 , 74.37 , 105.14 2021-02-12T23:13:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.10 , -0.06 2021-02-12T23:13:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171580.19 epoch sec; seqNo:1773; 4056442.22 north; 590598.85 east 2021-02-12T23:13:00Z- AHRS_M2 msg: 1613171580.57 epoch sec; seqNo:1757 2021-02-12T23:13:00Z- AHRS_M2 msg: 0.63 phi; -0.27 theta; -0.13 psi 2021-02-12T23:13:00Z- handleDepth()-Depth_Keller msg: 1613171580582 epoch sec; seqNo:1774; depth 10.74 2021-02-12T23:13:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171580.59 epoch sec; seqNo:1774; 4056442.60 north; 590598.85 east 2021-02-12T23:13:00Z- AHRS_M2 msg: 1613171580.97 epoch sec; seqNo:1758 2021-02-12T23:13:00Z- AHRS_M2 msg: 0.63 phi; -0.27 theta; -0.14 psi 2021-02-12T23:13:00Z- handleDepth()-Depth_Keller msg: 1613171580976 epoch sec; seqNo:1775; depth 10.84 2021-02-12T23:13:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:13:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:13:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:13:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:13:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:13:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:13:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:13:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:13:00Z- handleDvl() - RDI_Pathfinder msg: 1613171580.98 epoch sec; seqNo:752 2021-02-12T23:13:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:13:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.10 , -0.06 2021-02-12T23:13:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171580.99 epoch sec; seqNo:1775; 4056442.98 north; 590598.84 east 2021-02-12T23:13:01Z- AHRS_M2 msg: 1613171581.38 epoch sec; seqNo:1759 2021-02-12T23:13:01Z- AHRS_M2 msg: 0.63 phi; -0.28 theta; -0.15 psi 2021-02-12T23:13:01Z- handleDepth()-Depth_Keller msg: 1613171581380 epoch sec; seqNo:1776; depth 10.93 2021-02-12T23:13:01Z- handleDvl() - RDI_Pathfinder msg: 1613171581.39 epoch sec; seqNo:753 2021-02-12T23:13:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 97.78 , 73.14 , 96.56 2021-02-12T23:13:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.21 , -0.11 2021-02-12T23:13:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171581.40 epoch sec; seqNo:1776; 4056442.98 north; 590598.84 east 2021-02-12T23:13:01Z- AHRS_M2 msg: 1613171581.78 epoch sec; seqNo:1760 2021-02-12T23:13:01Z- AHRS_M2 msg: 0.63 phi; -0.28 theta; -0.16 psi 2021-02-12T23:13:01Z- handleDepth()-Depth_Keller msg: 1613171581786 epoch sec; seqNo:1777; depth 11.03 2021-02-12T23:13:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171581.80 epoch sec; seqNo:1777; 4056443.36 north; 590598.84 east 2021-02-12T23:13:02Z- AHRS_M2 msg: 1613171582.18 epoch sec; seqNo:1761 2021-02-12T23:13:02Z- AHRS_M2 msg: 0.64 phi; -0.28 theta; -0.16 psi 2021-02-12T23:13:02Z- handleDepth()-Depth_Keller msg: 1613171582200 epoch sec; seqNo:1778; depth 11.15 2021-02-12T23:13:02Z- handleDvl() - RDI_Pathfinder msg: 1613171582.21 epoch sec; seqNo:754 2021-02-12T23:13:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.16 , 95.54 , 72.32 , 102.85 2021-02-12T23:13:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.26 , -0.05 2021-02-12T23:13:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171582.22 epoch sec; seqNo:1778; 4056443.74 north; 590598.84 east 2021-02-12T23:13:02Z- AHRS_M2 msg: 1613171582.59 epoch sec; seqNo:1762 2021-02-12T23:13:02Z- AHRS_M2 msg: 0.64 phi; -0.28 theta; -0.17 psi 2021-02-12T23:13:02Z- handleDepth()-Depth_Keller msg: 1613171582597 epoch sec; seqNo:1779; depth 11.24 2021-02-12T23:13:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171582.61 epoch sec; seqNo:1779; 4056444.12 north; 590598.83 east 2021-02-12T23:13:02Z- AHRS_M2 msg: 1613171582.99 epoch sec; seqNo:1763 2021-02-12T23:13:02Z- AHRS_M2 msg: 0.64 phi; -0.28 theta; -0.17 psi 2021-02-12T23:13:03Z- handleDepth()-Depth_Keller msg: 1613171583001 epoch sec; seqNo:1780; depth 11.36 2021-02-12T23:13:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171583.01 epoch sec; seqNo:1780; 4056444.51 north; 590600.05 east 2021-02-12T23:13:03Z- AHRS_M2 msg: 1613171583.40 epoch sec; seqNo:1764 2021-02-12T23:13:03Z- AHRS_M2 msg: 0.65 phi; -0.28 theta; -0.17 psi 2021-02-12T23:13:03Z- handleDepth()-Depth_Keller msg: 1613171583402 epoch sec; seqNo:1781; depth 11.46 2021-02-12T23:13:03Z- handleDvl() - RDI_Pathfinder msg: 1613171583.41 epoch sec; seqNo:755 2021-02-12T23:13:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.66 , 96.69 , 74.37 , 103.99 2021-02-12T23:13:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.21 , 0.03 2021-02-12T23:13:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171583.42 epoch sec; seqNo:1781; 4056444.89 north; 590600.05 east 2021-02-12T23:13:03Z- LcmTrn::handleMessages() - handling 47 messages took 0.69 ms and 133 calls... 2021-02-12T23:13:03Z- waiting for 1613171581.20, time is 1613171583.42 2021-02-12T23:13:03Z- LcmTrn::updateTrn() equating pose 1613171583.42 and meas times 1613171583.42 2021-02-12T23:13:03Z- LcmTrn::updateTrn() >>>> pose time 1613171583.42, meas time 1613171583.42 <<<< 2021-02-12T23:13:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171583.42 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:13:03Z- TerrainNav::motionUpdate() - currEstimate = (4056445, 590600.05, 11.46) 2021-02-12T23:13:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:13:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:13:03Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171583.42 sec.; No relevant navigation data available 2021-02-12T23:13:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:13:03Z- TNavBF::mmse selected from filter -1 2021-02-12T23:13:03Z- variances([0], [2], and psi_berg) = 793.170433 833.089161 0.000000 2021-02-12T23:13:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:13:03Z- LcmTrn::publishEstimates() - MLE : 4056417.39 590550.55 11.46 2021-02-12T23:13:03Z- LcmTrn::publishEstimates() - MMSE : 4056444.89 590600.05 11.46 2021-02-12T23:13:03Z- LcmTrn::publishEstimates() - COVAR: 793.17 833.09 1.21 0.00 2021-02-12T23:13:03Z- AHRS_M2 msg: 1613171583.80 epoch sec; seqNo:1765 2021-02-12T23:13:03Z- AHRS_M2 msg: 0.65 phi; -0.29 theta; -0.16 psi 2021-02-12T23:13:03Z- handleDepth()-Depth_Keller msg: 1613171583814 epoch sec; seqNo:1782; depth 11.57 2021-02-12T23:13:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171583.83 epoch sec; seqNo:1782; 4056444.89 north; 590600.05 east 2021-02-12T23:13:04Z- AHRS_M2 msg: 1613171584.20 epoch sec; seqNo:1766 2021-02-12T23:13:04Z- AHRS_M2 msg: 0.66 phi; -0.29 theta; -0.15 psi 2021-02-12T23:13:04Z- handleDepth()-Depth_Keller msg: 1613171584208 epoch sec; seqNo:1783; depth 11.69 2021-02-12T23:13:04Z- handleDvl() - RDI_Pathfinder msg: 1613171584.22 epoch sec; seqNo:756 2021-02-12T23:13:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.03 , 97.84 , 71.92 , 96.63 2021-02-12T23:13:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.10 , 0.09 2021-02-12T23:13:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171584.24 epoch sec; seqNo:1783; 4056445.27 north; 590600.04 east 2021-02-12T23:13:04Z- AHRS_M2 msg: 1613171584.61 epoch sec; seqNo:1767 2021-02-12T23:13:04Z- AHRS_M2 msg: 0.66 phi; -0.29 theta; -0.14 psi 2021-02-12T23:13:04Z- handleDepth()-Depth_Keller msg: 1613171584612 epoch sec; seqNo:1784; depth 11.79 2021-02-12T23:13:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171584.62 epoch sec; seqNo:1784; 4056445.64 north; 590600.04 east 2021-02-12T23:13:05Z- AHRS_M2 msg: 1613171585.01 epoch sec; seqNo:1768 2021-02-12T23:13:05Z- AHRS_M2 msg: 0.67 phi; -0.29 theta; -0.13 psi 2021-02-12T23:13:05Z- handleDepth()-Depth_Keller msg: 1613171585018 epoch sec; seqNo:1785; depth 11.90 2021-02-12T23:13:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171585.03 epoch sec; seqNo:1785; 4056445.64 north; 590600.04 east 2021-02-12T23:13:05Z- AHRS_M2 msg: 1613171585.41 epoch sec; seqNo:1769 2021-02-12T23:13:05Z- AHRS_M2 msg: 0.67 phi; -0.30 theta; -0.12 psi 2021-02-12T23:13:05Z- handleDepth()-Depth_Keller msg: 1613171585425 epoch sec; seqNo:1786; depth 12.00 2021-02-12T23:13:05Z- handleDvl() - RDI_Pathfinder msg: 1613171585.44 epoch sec; seqNo:757 2021-02-12T23:13:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.38 , 93.11 , 71.92 , 96.76 2021-02-12T23:13:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.03 , 0.09 2021-02-12T23:13:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171585.46 epoch sec; seqNo:1786; 4056446.02 north; 590600.03 east 2021-02-12T23:13:05Z- AHRS_M2 msg: 1613171585.82 epoch sec; seqNo:1770 2021-02-12T23:13:05Z- AHRS_M2 msg: 0.68 phi; -0.30 theta; -0.12 psi 2021-02-12T23:13:05Z- handleDepth()-Depth_Keller msg: 1613171585824 epoch sec; seqNo:1787; depth 12.11 2021-02-12T23:13:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171585.84 epoch sec; seqNo:1787; 4056446.40 north; 590600.03 east 2021-02-12T23:13:06Z- AHRS_M2 msg: 1613171586.23 epoch sec; seqNo:1771 2021-02-12T23:13:06Z- AHRS_M2 msg: 0.68 phi; -0.30 theta; -0.12 psi 2021-02-12T23:13:06Z- handleDepth()-Depth_Keller msg: 1613171586236 epoch sec; seqNo:1788; depth 12.21 2021-02-12T23:13:06Z- handleDvl() - RDI_Pathfinder msg: 1613171586.26 epoch sec; seqNo:758 2021-02-12T23:13:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.45 , 91.96 , 72.73 , 106.56 2021-02-12T23:13:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.09 , 0.05 2021-02-12T23:13:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171586.29 epoch sec; seqNo:1788; 4056446.41 north; 590601.25 east 2021-02-12T23:13:06Z- AHRS_M2 msg: 1613171586.63 epoch sec; seqNo:1772 2021-02-12T23:13:06Z- AHRS_M2 msg: 0.68 phi; -0.31 theta; -0.13 psi 2021-02-12T23:13:06Z- handleDepth()-Depth_Keller msg: 1613171586642 epoch sec; seqNo:1789; depth 12.33 2021-02-12T23:13:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171586.65 epoch sec; seqNo:1789; 4056446.79 north; 590601.25 east 2021-02-12T23:13:07Z- AHRS_M2 msg: 1613171587.03 epoch sec; seqNo:1773 2021-02-12T23:13:07Z- AHRS_M2 msg: 0.69 phi; -0.31 theta; -0.15 psi 2021-02-12T23:13:07Z- handleDepth()-Depth_Keller msg: 1613171587036 epoch sec; seqNo:1790; depth 12.44 2021-02-12T23:13:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171587.05 epoch sec; seqNo:1790; 4056446.79 north; 590601.25 east 2021-02-12T23:13:07Z- AHRS_M2 msg: 1613171587.44 epoch sec; seqNo:1774 2021-02-12T23:13:07Z- AHRS_M2 msg: 0.69 phi; -0.31 theta; -0.16 psi 2021-02-12T23:13:07Z- handleDepth()-Depth_Keller msg: 1613171587441 epoch sec; seqNo:1791; depth 12.53 2021-02-12T23:13:07Z- handleDvl() - RDI_Pathfinder msg: 1613171587.45 epoch sec; seqNo:759 2021-02-12T23:13:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.24 , 94.19 , 72.32 , 107.58 2021-02-12T23:13:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.09 , -0.03 2021-02-12T23:13:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171587.46 epoch sec; seqNo:1791; 4056447.17 north; 590601.24 east 2021-02-12T23:13:07Z- AHRS_M2 msg: 1613171587.84 epoch sec; seqNo:1775 2021-02-12T23:13:07Z- AHRS_M2 msg: 0.70 phi; -0.31 theta; -0.17 psi 2021-02-12T23:13:07Z- handleDepth()-Depth_Keller msg: 1613171587853 epoch sec; seqNo:1792; depth 12.66 2021-02-12T23:13:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171587.86 epoch sec; seqNo:1792; 4056447.55 north; 590601.24 east 2021-02-12T23:13:08Z- AHRS_M2 msg: 1613171588.24 epoch sec; seqNo:1776 2021-02-12T23:13:08Z- AHRS_M2 msg: 0.70 phi; -0.31 theta; -0.19 psi 2021-02-12T23:13:08Z- handleDepth()-Depth_Keller msg: 1613171588251 epoch sec; seqNo:1793; depth 12.77 2021-02-12T23:13:08Z- handleDvl() - RDI_Pathfinder msg: 1613171588.27 epoch sec; seqNo:760 2021-02-12T23:13:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.27 , 96.63 , 73.14 , 105.14 2021-02-12T23:13:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , -0.08 2021-02-12T23:13:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171588.28 epoch sec; seqNo:1793; 4056447.56 north; 590602.46 east 2021-02-12T23:13:08Z- LcmTrn::handleMessages() - handling 41 messages took 0.69 ms and 127 calls... 2021-02-12T23:13:08Z- waiting for 1613171586.42, time is 1613171588.28 2021-02-12T23:13:08Z- LcmTrn::updateTrn() equating pose 1613171588.28 and meas times 1613171588.28 2021-02-12T23:13:08Z- LcmTrn::updateTrn() >>>> pose time 1613171588.28, meas time 1613171588.28 <<<< 2021-02-12T23:13:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171588.28 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:13:08Z- TerrainNav::motionUpdate() - currEstimate = (4056448, 590602.46, 12.77) 2021-02-12T23:13:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:13:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:13:08Z- TNavBF::Projecting Measurements 2021-02-12T23:13:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:13:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:13:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:13:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:13:08Z- TNavFilter::4 good beams. 2021-02-12T23:13:08Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:13:08Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:13:08Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:13:08Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:13:08Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:13:08Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:13:08Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:13:08Z- TNavBF::mmse selected from filter -1 2021-02-12T23:13:08Z- TerrainMapOctree: (vehN, vehE ) = (4056447.56, 590602.46). 2021-02-12T23:13:08Z- TerrainMapOctree: Min Distance = 6030.58. 2021-02-12T23:13:08Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:13:08Z- There is no specific good beam that all particles have in common. 2021-02-12T23:13:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:13:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 10.05 2021-02-12T23:13:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:13:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 14.94 2021-02-12T23:13:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:13:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-12T23:13:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:13:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 17.69 2021-02-12T23:13:08Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:13:08Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:13:08Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:13:08Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:13:08Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:13:08Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:13:08Z- Filter 0 No beams used in subcloud update 2021-02-12T23:13:08Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:13:08Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:13:08Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 793.0 EastVariance: 833.1 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:13:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171588.28 sec. 2021-02-12T23:13:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:13:08Z- TNavBF::mmse selected from filter -1 2021-02-12T23:13:08Z- variances([0], [2], and psi_berg) = 792.985290 833.088819 0.000000 2021-02-12T23:13:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:13:08Z- LcmTrn::publishEstimates() - MLE : 4056420.06 590552.96 12.77 2021-02-12T23:13:08Z- LcmTrn::publishEstimates() - MMSE : 4056447.56 590602.46 12.77 2021-02-12T23:13:08Z- LcmTrn::publishEstimates() - COVAR: 792.99 833.09 0.99 0.00 2021-02-12T23:13:08Z- AHRS_M2 msg: 1613171588.65 epoch sec; seqNo:1777 2021-02-12T23:13:08Z- AHRS_M2 msg: 0.71 phi; -0.31 theta; -0.19 psi 2021-02-12T23:13:08Z- handleDepth()-Depth_Keller msg: 1613171588660 epoch sec; seqNo:1794; depth 12.90 2021-02-12T23:13:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171588.68 epoch sec; seqNo:1794; 4056447.94 north; 590602.46 east 2021-02-12T23:13:09Z- AHRS_M2 msg: 1613171589.08 epoch sec; seqNo:1778 2021-02-12T23:13:09Z- AHRS_M2 msg: 0.71 phi; -0.31 theta; -0.20 psi 2021-02-12T23:13:09Z- handleDepth()-Depth_Keller msg: 1613171589083 epoch sec; seqNo:1795; depth 13.01 2021-02-12T23:13:09Z- handleDvl() - RDI_Pathfinder msg: 1613171589.10 epoch sec; seqNo:761 2021-02-12T23:13:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.86 , 96.69 , 70.69 , 107.64 2021-02-12T23:13:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , -0.04 2021-02-12T23:13:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171589.12 epoch sec; seqNo:1795; 4056448.32 north; 590602.45 east 2021-02-12T23:13:09Z- AHRS_M2 msg: 1613171589.45 epoch sec; seqNo:1779 2021-02-12T23:13:09Z- AHRS_M2 msg: 0.71 phi; -0.32 theta; -0.20 psi 2021-02-12T23:13:09Z- handleDepth()-Depth_Keller msg: 1613171589460 epoch sec; seqNo:1796; depth 13.14 2021-02-12T23:13:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171589.48 epoch sec; seqNo:1796; 4056448.70 north; 590602.45 east 2021-02-12T23:13:09Z- AHRS_M2 msg: 1613171589.86 epoch sec; seqNo:1780 2021-02-12T23:13:09Z- AHRS_M2 msg: 0.71 phi; -0.32 theta; -0.20 psi 2021-02-12T23:13:09Z- handleDepth()-Depth_Keller msg: 1613171589864 epoch sec; seqNo:1797; depth 13.24 2021-02-12T23:13:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171589.87 epoch sec; seqNo:1797; 4056448.70 north; 590602.45 east 2021-02-12T23:13:10Z- AHRS_M2 msg: 1613171590.26 epoch sec; seqNo:1781 2021-02-12T23:13:10Z- AHRS_M2 msg: 0.71 phi; -0.32 theta; -0.19 psi 2021-02-12T23:13:10Z- handleDepth()-Depth_Keller msg: 1613171590269 epoch sec; seqNo:1798; depth 13.37 2021-02-12T23:13:10Z- handleDvl() - RDI_Pathfinder msg: 1613171590.29 epoch sec; seqNo:762 2021-02-12T23:13:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.86 , 99.13 , 70.69 , 106.43 2021-02-12T23:13:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , -0.01 2021-02-12T23:13:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171590.30 epoch sec; seqNo:1798; 4056449.08 north; 590602.44 east 2021-02-12T23:13:10Z- AHRS_M2 msg: 1613171590.67 epoch sec; seqNo:1782 2021-02-12T23:13:10Z- AHRS_M2 msg: 0.71 phi; -0.32 theta; -0.18 psi 2021-02-12T23:13:10Z- handleDepth()-Depth_Keller msg: 1613171590679 epoch sec; seqNo:1799; depth 13.50 2021-02-12T23:13:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171590.69 epoch sec; seqNo:1799; 4056449.47 north; 590603.66 east 2021-02-12T23:13:11Z- AHRS_M2 msg: 1613171591.09 epoch sec; seqNo:1783 2021-02-12T23:13:11Z- AHRS_M2 msg: 0.71 phi; -0.32 theta; -0.18 psi 2021-02-12T23:13:11Z- handleDepth()-Depth_Keller msg: 1613171591098 epoch sec; seqNo:1800; depth 13.60 2021-02-12T23:13:11Z- handleDvl() - RDI_Pathfinder msg: 1613171591.11 epoch sec; seqNo:763 2021-02-12T23:13:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.86 , 97.84 , 70.28 , 107.58 2021-02-12T23:13:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , -0.05 2021-02-12T23:13:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171591.12 epoch sec; seqNo:1800; 4056449.47 north; 590603.66 east 2021-02-12T23:13:11Z- AHRS_M2 msg: 1613171591.47 epoch sec; seqNo:1784 2021-02-12T23:13:11Z- AHRS_M2 msg: 0.71 phi; -0.32 theta; -0.18 psi 2021-02-12T23:13:11Z- handleDepth()-Depth_Keller msg: 1613171591485 epoch sec; seqNo:1801; depth 13.73 2021-02-12T23:13:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171591.49 epoch sec; seqNo:1801; 4056449.85 north; 590603.66 east 2021-02-12T23:13:11Z- AHRS_M2 msg: 1613171591.88 epoch sec; seqNo:1785 2021-02-12T23:13:11Z- AHRS_M2 msg: 0.71 phi; -0.32 theta; -0.19 psi 2021-02-12T23:13:11Z- handleDepth()-Depth_Keller msg: 1613171591884 epoch sec; seqNo:1802; depth 13.85 2021-02-12T23:13:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171591.89 epoch sec; seqNo:1802; 4056450.23 north; 590603.65 east 2021-02-12T23:13:12Z- AHRS_M2 msg: 1613171592.28 epoch sec; seqNo:1786 2021-02-12T23:13:12Z- AHRS_M2 msg: 0.70 phi; -0.32 theta; -0.20 psi 2021-02-12T23:13:12Z- handleDepth()-Depth_Keller msg: 1613171592299 epoch sec; seqNo:1803; depth 13.97 2021-02-12T23:13:12Z- handleDvl() - RDI_Pathfinder msg: 1613171592.31 epoch sec; seqNo:764 2021-02-12T23:13:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 96.63 , 69.06 , 105.14 2021-02-12T23:13:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.02 , -0.09 2021-02-12T23:13:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171592.32 epoch sec; seqNo:1803; 4056450.61 north; 590603.65 east 2021-02-12T23:13:12Z- handleDepth()-Depth_Keller msg: 1613171592711 epoch sec; seqNo:1804; depth 14.10 2021-02-12T23:13:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171592.72 epoch sec; seqNo:1804; 4056450.62 north; 590604.87 east 2021-02-12T23:13:13Z- AHRS_M2 msg: 1613171593.09 epoch sec; seqNo:1787 2021-02-12T23:13:13Z- AHRS_M2 msg: 0.70 phi; -0.32 theta; -0.21 psi 2021-02-12T23:13:13Z- handleDepth()-Depth_Keller msg: 1613171593106 epoch sec; seqNo:1805; depth 14.21 2021-02-12T23:13:13Z- handleDvl() - RDI_Pathfinder msg: 1613171593.12 epoch sec; seqNo:765 2021-02-12T23:13:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.64 , 96.76 , 69.87 , 108.93 2021-02-12T23:13:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.12 , -0.13 2021-02-12T23:13:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171593.15 epoch sec; seqNo:1805; 4056451.00 north; 590604.87 east 2021-02-12T23:13:13Z- AHRS_M2 msg: 1613171593.49 epoch sec; seqNo:1788 2021-02-12T23:13:13Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.22 psi 2021-02-12T23:13:13Z- handleDepth()-Depth_Keller msg: 1613171593503 epoch sec; seqNo:1806; depth 14.36 2021-02-12T23:13:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171593.51 epoch sec; seqNo:1806; 4056451.38 north; 590604.86 east 2021-02-12T23:13:13Z- LcmTrn::handleMessages() - handling 43 messages took 0.79 ms and 127 calls... 2021-02-12T23:13:13Z- waiting for 1613171591.28, time is 1613171593.51 2021-02-12T23:13:13Z- LcmTrn::updateTrn() >>>> pose time 1613171593.51, meas time 1613171593.12 <<<< 2021-02-12T23:13:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171593.12 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:13:13Z- TerrainNav::motionUpdate() - currEstimate = (4056451, 590604.86, 14.36) 2021-02-12T23:13:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:13:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:13:13Z- TNavBF::Projecting Measurements 2021-02-12T23:13:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:13:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:13:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:13:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:13:13Z- TNavFilter::4 good beams. 2021-02-12T23:13:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:13:13Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:13:13Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:13:13Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:13:13Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:13:13Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:13:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:13:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:13:13Z- TerrainMapOctree: (vehN, vehE ) = (4056451.09, 590604.68). 2021-02-12T23:13:13Z- TerrainMapOctree: Min Distance = 6026.43. 2021-02-12T23:13:13Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:13:13Z- There is no specific good beam that all particles have in common. 2021-02-12T23:13:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:13:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 9.64 2021-02-12T23:13:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:13:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 14.98 2021-02-12T23:13:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:13:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 7.81 2021-02-12T23:13:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:13:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 18.98 2021-02-12T23:13:13Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:13:13Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:13:13Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:13:13Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:13:13Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:13:13Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:13:13Z- Filter 0 No beams used in subcloud update 2021-02-12T23:13:13Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:13:13Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:13:13Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 792.8 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:13:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171593.12 sec. 2021-02-12T23:13:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:13:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:13:13Z- variances([0], [2], and psi_berg) = 792.730588 833.088349 0.000000 2021-02-12T23:13:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:13:13Z- LcmTrn::publishEstimates() - MLE : 4056423.88 590555.36 14.36 2021-02-12T23:13:13Z- LcmTrn::publishEstimates() - MMSE : 4056451.38 590604.86 14.36 2021-02-12T23:13:13Z- LcmTrn::publishEstimates() - COVAR: 792.73 833.09 0.90 0.00 2021-02-12T23:13:13Z- AHRS_M2 msg: 1613171593.90 epoch sec; seqNo:1789 2021-02-12T23:13:13Z- AHRS_M2 msg: 0.69 phi; -0.33 theta; -0.22 psi 2021-02-12T23:13:13Z- handleDepth()-Depth_Keller msg: 1613171593904 epoch sec; seqNo:1807; depth 14.47 2021-02-12T23:13:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171593.91 epoch sec; seqNo:1807; 4056451.76 north; 590604.86 east 2021-02-12T23:13:14Z- AHRS_M2 msg: 1613171594.31 epoch sec; seqNo:1790 2021-02-12T23:13:14Z- AHRS_M2 msg: 0.69 phi; -0.33 theta; -0.23 psi 2021-02-12T23:13:14Z- handleDepth()-Depth_Keller msg: 1613171594317 epoch sec; seqNo:1808; depth 14.60 2021-02-12T23:13:14Z- handleDvl() - RDI_Pathfinder msg: 1613171594.34 epoch sec; seqNo:766 2021-02-12T23:13:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.82 , 96.69 , 69.06 , 105.21 2021-02-12T23:13:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.20 , -0.05 2021-02-12T23:13:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171594.35 epoch sec; seqNo:1808; 4056452.14 north; 590604.85 east 2021-02-12T23:13:14Z- AHRS_M2 msg: 1613171594.71 epoch sec; seqNo:1791 2021-02-12T23:13:14Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.23 psi 2021-02-12T23:13:14Z- handleDepth()-Depth_Keller msg: 1613171594717 epoch sec; seqNo:1809; depth 14.73 2021-02-12T23:13:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171594.73 epoch sec; seqNo:1809; 4056452.52 north; 590604.85 east 2021-02-12T23:13:15Z- AHRS_M2 msg: 1613171595.12 epoch sec; seqNo:1792 2021-02-12T23:13:15Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.23 psi 2021-02-12T23:13:15Z- handleDepth()-Depth_Keller msg: 1613171595133 epoch sec; seqNo:1810; depth 14.84 2021-02-12T23:13:15Z- handleDvl() - RDI_Pathfinder msg: 1613171595.15 epoch sec; seqNo:767 2021-02-12T23:13:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.00 , 96.83 , 69.46 , 105.35 2021-02-12T23:13:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.20 , 0.06 2021-02-12T23:13:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171595.16 epoch sec; seqNo:1810; 4056452.52 north; 590604.85 east 2021-02-12T23:13:15Z- AHRS_M2 msg: 1613171595.52 epoch sec; seqNo:1793 2021-02-12T23:13:15Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.23 psi 2021-02-12T23:13:15Z- handleDepth()-Depth_Keller msg: 1613171595520 epoch sec; seqNo:1811; depth 14.98 2021-02-12T23:13:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171595.54 epoch sec; seqNo:1811; 4056452.89 north; 590604.85 east 2021-02-12T23:13:15Z- AHRS_M2 msg: 1613171595.92 epoch sec; seqNo:1794 2021-02-12T23:13:15Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.23 psi 2021-02-12T23:13:15Z- handleDepth()-Depth_Keller msg: 1613171595924 epoch sec; seqNo:1812; depth 15.09 2021-02-12T23:13:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171595.93 epoch sec; seqNo:1812; 4056453.29 north; 590606.06 east 2021-02-12T23:13:16Z- AHRS_M2 msg: 1613171596.33 epoch sec; seqNo:1795 2021-02-12T23:13:16Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.23 psi 2021-02-12T23:13:16Z- handleDepth()-Depth_Keller msg: 1613171596331 epoch sec; seqNo:1813; depth 15.21 2021-02-12T23:13:16Z- handleDvl() - RDI_Pathfinder msg: 1613171596.34 epoch sec; seqNo:768 2021-02-12T23:13:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.41 , 96.83 , 69.87 , 106.56 2021-02-12T23:13:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.10 , 0.14 2021-02-12T23:13:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171596.35 epoch sec; seqNo:1813; 4056453.29 north; 590606.06 east 2021-02-12T23:13:16Z- AHRS_M2 msg: 1613171596.73 epoch sec; seqNo:1796 2021-02-12T23:13:16Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.23 psi 2021-02-12T23:13:16Z- handleDepth()-Depth_Keller msg: 1613171596734 epoch sec; seqNo:1814; depth 15.33 2021-02-12T23:13:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171596.74 epoch sec; seqNo:1814; 4056453.66 north; 590606.06 east 2021-02-12T23:13:17Z- AHRS_M2 msg: 1613171597.13 epoch sec; seqNo:1797 2021-02-12T23:13:17Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.23 psi 2021-02-12T23:13:17Z- handleDepth()-Depth_Keller msg: 1613171597140 epoch sec; seqNo:1815; depth 15.46 2021-02-12T23:13:17Z- handleDvl() - RDI_Pathfinder msg: 1613171597.17 epoch sec; seqNo:769 2021-02-12T23:13:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.82 , 97.98 , 69.46 , 106.50 2021-02-12T23:13:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.01 , 0.10 2021-02-12T23:13:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171597.18 epoch sec; seqNo:1815; 4056454.04 north; 590606.05 east 2021-02-12T23:13:17Z- AHRS_M2 msg: 1613171597.54 epoch sec; seqNo:1798 2021-02-12T23:13:17Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.23 psi 2021-02-12T23:13:17Z- handleDepth()-Depth_Keller msg: 1613171597545 epoch sec; seqNo:1816; depth 15.58 2021-02-12T23:13:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171597.56 epoch sec; seqNo:1816; 4056454.04 north; 590606.05 east 2021-02-12T23:13:17Z- AHRS_M2 msg: 1613171597.94 epoch sec; seqNo:1799 2021-02-12T23:13:17Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.23 psi 2021-02-12T23:13:17Z- handleDepth()-Depth_Keller msg: 1613171597944 epoch sec; seqNo:1817; depth 15.71 2021-02-12T23:13:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171597.99 epoch sec; seqNo:1817; 4056454.42 north; 590606.05 east 2021-02-12T23:13:18Z- AHRS_M2 msg: 1613171598.35 epoch sec; seqNo:1800 2021-02-12T23:13:18Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.22 psi 2021-02-12T23:13:18Z- handleDepth()-Depth_Keller msg: 1613171598358 epoch sec; seqNo:1818; depth 15.83 2021-02-12T23:13:18Z- handleDvl() - RDI_Pathfinder msg: 1613171598.37 epoch sec; seqNo:770 2021-02-12T23:13:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.00 , 96.76 , 69.06 , 107.71 2021-02-12T23:13:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.11 , 0.03 2021-02-12T23:13:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171598.38 epoch sec; seqNo:1818; 4056454.80 north; 590606.05 east 2021-02-12T23:13:18Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-12T23:13:18Z- waiting for 1613171596.51, time is 1613171598.38 2021-02-12T23:13:18Z- LcmTrn::updateTrn() equating pose 1613171598.38 and meas times 1613171598.38 2021-02-12T23:13:18Z- LcmTrn::updateTrn() >>>> pose time 1613171598.38, meas time 1613171598.38 <<<< 2021-02-12T23:13:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171598.38 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:13:18Z- TerrainNav::motionUpdate() - currEstimate = (4056455, 590606.05, 15.83) 2021-02-12T23:13:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:13:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:13:18Z- TNavBF::Projecting Measurements 2021-02-12T23:13:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:13:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:13:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:13:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:13:18Z- TNavFilter::4 good beams. 2021-02-12T23:13:18Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:13:18Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:13:18Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:13:18Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:13:18Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:13:18Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:13:18Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:13:18Z- TNavBF::mmse selected from filter -1 2021-02-12T23:13:18Z- TerrainMapOctree: (vehN, vehE ) = (4056454.80, 590606.05). 2021-02-12T23:13:18Z- TerrainMapOctree: Min Distance = 6022.65. 2021-02-12T23:13:18Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:13:18Z- There is no specific good beam that all particles have in common. 2021-02-12T23:13:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:13:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 9.24 2021-02-12T23:13:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:13:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 14.98 2021-02-12T23:13:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:13:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-12T23:13:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:13:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 18.56 2021-02-12T23:13:18Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:13:18Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:13:18Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:13:18Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:13:18Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:13:18Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:13:18Z- Filter 0 No beams used in subcloud update 2021-02-12T23:13:18Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:13:18Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:13:18Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 792.5 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:13:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171598.38 sec. 2021-02-12T23:13:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:13:18Z- TNavBF::mmse selected from filter -1 2021-02-12T23:13:18Z- variances([0], [2], and psi_berg) = 792.546663 833.088012 0.000000 2021-02-12T23:13:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:13:18Z- LcmTrn::publishEstimates() - MLE : 4056427.30 590556.55 15.83 2021-02-12T23:13:18Z- LcmTrn::publishEstimates() - MMSE : 4056454.80 590606.05 15.83 2021-02-12T23:13:18Z- LcmTrn::publishEstimates() - COVAR: 792.55 833.09 0.81 0.00 2021-02-12T23:13:18Z- AHRS_M2 msg: 1613171598.75 epoch sec; seqNo:1801 2021-02-12T23:13:18Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.22 psi 2021-02-12T23:13:18Z- handleDepth()-Depth_Keller msg: 1613171598762 epoch sec; seqNo:1819; depth 15.96 2021-02-12T23:13:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171598.77 epoch sec; seqNo:1819; 4056454.81 north; 590607.27 east 2021-02-12T23:13:19Z- AHRS_M2 msg: 1613171599.15 epoch sec; seqNo:1802 2021-02-12T23:13:19Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.22 psi 2021-02-12T23:13:19Z- handleDepth()-Depth_Keller msg: 1613171599157 epoch sec; seqNo:1820; depth 16.08 2021-02-12T23:13:19Z- handleDvl() - RDI_Pathfinder msg: 1613171599.17 epoch sec; seqNo:771 2021-02-12T23:13:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.00 , 96.76 , 68.65 , 107.71 2021-02-12T23:13:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.04 2021-02-12T23:13:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171599.19 epoch sec; seqNo:1820; 4056455.19 north; 590607.26 east 2021-02-12T23:13:19Z- AHRS_M2 msg: 1613171599.56 epoch sec; seqNo:1803 2021-02-12T23:13:19Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.21 psi 2021-02-12T23:13:19Z- handleDepth()-Depth_Keller msg: 1613171599565 epoch sec; seqNo:1821; depth 16.22 2021-02-12T23:13:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171599.58 epoch sec; seqNo:1821; 4056455.57 north; 590607.26 east 2021-02-12T23:13:19Z- AHRS_M2 msg: 1613171599.96 epoch sec; seqNo:1804 2021-02-12T23:13:19Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.21 psi 2021-02-12T23:13:19Z- handleDepth()-Depth_Keller msg: 1613171599973 epoch sec; seqNo:1822; depth 16.35 2021-02-12T23:13:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171599.98 epoch sec; seqNo:1822; 4056455.57 north; 590607.26 east 2021-02-12T23:13:20Z- AHRS_M2 msg: 1613171600.36 epoch sec; seqNo:1805 2021-02-12T23:13:20Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.20 psi 2021-02-12T23:13:20Z- handleDepth()-Depth_Keller msg: 1613171600379 epoch sec; seqNo:1823; depth 16.48 2021-02-12T23:13:20Z- handleDvl() - RDI_Pathfinder msg: 1613171600.39 epoch sec; seqNo:772 2021-02-12T23:13:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.00 , 97.98 , 69.06 , 96.76 2021-02-12T23:13:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.04 , -0.06 2021-02-12T23:13:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171600.40 epoch sec; seqNo:1823; 4056455.95 north; 590607.26 east 2021-02-12T23:13:20Z- AHRS_M2 msg: 1613171600.77 epoch sec; seqNo:1806 2021-02-12T23:13:20Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.20 psi 2021-02-12T23:13:20Z- handleDepth()-Depth_Keller msg: 1613171600777 epoch sec; seqNo:1824; depth 16.60 2021-02-12T23:13:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171600.79 epoch sec; seqNo:1824; 4056456.33 north; 590607.25 east 2021-02-12T23:13:21Z- AHRS_M2 msg: 1613171601.18 epoch sec; seqNo:1807 2021-02-12T23:13:21Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.20 psi 2021-02-12T23:13:21Z- handleDepth()-Depth_Keller msg: 1613171601189 epoch sec; seqNo:1825; depth 16.73 2021-02-12T23:13:21Z- handleDvl() - RDI_Pathfinder msg: 1613171601.20 epoch sec; seqNo:773 2021-02-12T23:13:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.00 , 94.53 , 67.83 , 96.96 2021-02-12T23:13:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , -0.02 2021-02-12T23:13:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171601.21 epoch sec; seqNo:1825; 4056456.72 north; 590608.47 east 2021-02-12T23:13:21Z- AHRS_M2 msg: 1613171601.58 epoch sec; seqNo:1808 2021-02-12T23:13:21Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.20 psi 2021-02-12T23:13:21Z- handleDepth()-Depth_Keller msg: 1613171601580 epoch sec; seqNo:1826; depth 16.87 2021-02-12T23:13:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171601.60 epoch sec; seqNo:1826; 4056456.72 north; 590608.47 east 2021-02-12T23:13:22Z- AHRS_M2 msg: 1613171601.99 epoch sec; seqNo:1809 2021-02-12T23:13:22Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.20 psi 2021-02-12T23:13:22Z- handleDepth()-Depth_Keller msg: 1613171602016 epoch sec; seqNo:1827; depth 16.98 2021-02-12T23:13:22Z- handleDvl() - RDI_Pathfinder msg: 1613171602.03 epoch sec; seqNo:774 2021-02-12T23:13:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.59 , 93.45 , 67.42 , 105.62 2021-02-12T23:13:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.00 2021-02-12T23:13:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171602.04 epoch sec; seqNo:1827; 4056457.10 north; 590608.46 east 2021-02-12T23:13:22Z- AHRS_M2 msg: 1613171602.39 epoch sec; seqNo:1810 2021-02-12T23:13:22Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-12T23:13:22Z- handleDepth()-Depth_Keller msg: 1613171602401 epoch sec; seqNo:1828; depth 17.13 2021-02-12T23:13:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171602.42 epoch sec; seqNo:1828; 4056457.48 north; 590608.46 east 2021-02-12T23:13:22Z- AHRS_M2 msg: 1613171602.79 epoch sec; seqNo:1811 2021-02-12T23:13:22Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-12T23:13:22Z- handleDepth()-Depth_Keller msg: 1613171602792 epoch sec; seqNo:1829; depth 17.25 2021-02-12T23:13:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171602.81 epoch sec; seqNo:1829; 4056457.48 north; 590608.46 east 2021-02-12T23:13:23Z- AHRS_M2 msg: 1613171603.19 epoch sec; seqNo:1812 2021-02-12T23:13:23Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-12T23:13:23Z- handleDepth()-Depth_Keller msg: 1613171603197 epoch sec; seqNo:1830; depth 17.37 2021-02-12T23:13:23Z- handleDvl() - RDI_Pathfinder msg: 1613171603.21 epoch sec; seqNo:775 2021-02-12T23:13:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.00 , 94.46 , 67.83 , 104.20 2021-02-12T23:13:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.02 2021-02-12T23:13:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171603.23 epoch sec; seqNo:1830; 4056457.86 north; 590608.46 east 2021-02-12T23:13:23Z- AHRS_M2 msg: 1613171603.60 epoch sec; seqNo:1813 2021-02-12T23:13:23Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-12T23:13:23Z- handleDepth()-Depth_Keller msg: 1613171603605 epoch sec; seqNo:1831; depth 17.51 2021-02-12T23:13:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171603.62 epoch sec; seqNo:1831; 4056458.25 north; 590609.67 east 2021-02-12T23:13:23Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 130 calls... 2021-02-12T23:13:23Z- waiting for 1613171601.38, time is 1613171603.62 2021-02-12T23:13:23Z- LcmTrn::updateTrn() >>>> pose time 1613171603.62, meas time 1613171603.21 <<<< 2021-02-12T23:13:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171603.21 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:13:23Z- TerrainNav::motionUpdate() - currEstimate = (4056458, 590609.67, 17.51) 2021-02-12T23:13:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:13:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:13:23Z- TNavBF::Projecting Measurements 2021-02-12T23:13:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:13:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:13:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:13:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:13:23Z- TNavFilter::4 good beams. 2021-02-12T23:13:23Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:13:23Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:13:23Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:13:23Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:13:23Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:13:23Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:13:23Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:13:23Z- TNavBF::mmse selected from filter -1 2021-02-12T23:13:23Z- TerrainMapOctree: (vehN, vehE ) = (4056457.98, 590609.39). 2021-02-12T23:13:23Z- TerrainMapOctree: Min Distance = 6018.10. 2021-02-12T23:13:23Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:13:23Z- There is no specific good beam that all particles have in common. 2021-02-12T23:13:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:13:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 9.24 2021-02-12T23:13:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:13:23Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 14.28 2021-02-12T23:13:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:13:23Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 7.36 2021-02-12T23:13:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:13:23Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 17.37 2021-02-12T23:13:23Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:13:23Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:13:23Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:13:23Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:13:23Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:13:23Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:13:23Z- Filter 0 No beams used in subcloud update 2021-02-12T23:13:23Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:13:23Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:13:23Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 792.3 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:13:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171603.21 sec. 2021-02-12T23:13:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:13:23Z- TNavBF::mmse selected from filter -1 2021-02-12T23:13:23Z- variances([0], [2], and psi_berg) = 792.279981 833.087525 0.000000 2021-02-12T23:13:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:13:23Z- LcmTrn::publishEstimates() - MLE : 4056430.75 590565.17 17.51 2021-02-12T23:13:23Z- LcmTrn::publishEstimates() - MMSE : 4056458.25 590609.67 17.51 2021-02-12T23:13:23Z- LcmTrn::publishEstimates() - COVAR: 792.28 833.09 0.81 0.00 2021-02-12T23:13:24Z- AHRS_M2 msg: 1613171604.02 epoch sec; seqNo:1814 2021-02-12T23:13:24Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-12T23:13:24Z- handleDepth()-Depth_Keller msg: 1613171604023 epoch sec; seqNo:1832; depth 17.64 2021-02-12T23:13:24Z- handleDvl() - RDI_Pathfinder msg: 1613171604.04 epoch sec; seqNo:776 2021-02-12T23:13:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.59 , 94.46 , 67.83 , 96.90 2021-02-12T23:13:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , -0.03 2021-02-12T23:13:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171604.05 epoch sec; seqNo:1832; 4056458.63 north; 590609.67 east 2021-02-12T23:13:24Z- AHRS_M2 msg: 1613171604.40 epoch sec; seqNo:1815 2021-02-12T23:13:24Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-12T23:13:24Z- handleDepth()-Depth_Keller msg: 1613171604408 epoch sec; seqNo:1833; depth 17.78 2021-02-12T23:13:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171604.43 epoch sec; seqNo:1833; 4056458.63 north; 590609.67 east 2021-02-12T23:13:24Z- AHRS_M2 msg: 1613171604.81 epoch sec; seqNo:1816 2021-02-12T23:13:24Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-12T23:13:24Z- handleDepth()-Depth_Keller msg: 1613171604837 epoch sec; seqNo:1834; depth 17.90 2021-02-12T23:13:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171604.87 epoch sec; seqNo:1834; 4056459.01 north; 590609.67 east 2021-02-12T23:13:25Z- AHRS_M2 msg: 1613171605.22 epoch sec; seqNo:1817 2021-02-12T23:13:25Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.20 psi 2021-02-12T23:13:25Z- handleDepth()-Depth_Keller msg: 1613171605223 epoch sec; seqNo:1835; depth 18.05 2021-02-12T23:13:25Z- handleDvl() - RDI_Pathfinder msg: 1613171605.23 epoch sec; seqNo:777 2021-02-12T23:13:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.78 , 93.45 , 67.01 , 105.62 2021-02-12T23:13:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.03 , -0.06 2021-02-12T23:13:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171605.24 epoch sec; seqNo:1835; 4056459.39 north; 590609.66 east 2021-02-12T23:13:25Z- AHRS_M2 msg: 1613171605.62 epoch sec; seqNo:1818 2021-02-12T23:13:25Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.20 psi 2021-02-12T23:13:25Z- handleDepth()-Depth_Keller msg: 1613171605619 epoch sec; seqNo:1836; depth 18.17 2021-02-12T23:13:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171605.64 epoch sec; seqNo:1836; 4056459.76 north; 590609.66 east 2021-02-12T23:13:26Z- AHRS_M2 msg: 1613171606.02 epoch sec; seqNo:1819 2021-02-12T23:13:26Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.19 psi 2021-02-12T23:13:26Z- handleDepth()-Depth_Keller msg: 1613171606029 epoch sec; seqNo:1837; depth 18.30 2021-02-12T23:13:26Z- handleDvl() - RDI_Pathfinder msg: 1613171606.04 epoch sec; seqNo:778 2021-02-12T23:13:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 92.10 , 66.60 , 105.48 2021-02-12T23:13:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.05 , -0.11 2021-02-12T23:13:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171606.05 epoch sec; seqNo:1837; 4056460.14 north; 590609.65 east 2021-02-12T23:13:26Z- AHRS_M2 msg: 1613171606.42 epoch sec; seqNo:1820 2021-02-12T23:13:26Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-12T23:13:26Z- handleDepth()-Depth_Keller msg: 1613171606428 epoch sec; seqNo:1838; depth 18.46 2021-02-12T23:13:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171606.45 epoch sec; seqNo:1838; 4056460.16 north; 590610.87 east 2021-02-12T23:13:26Z- AHRS_M2 msg: 1613171606.83 epoch sec; seqNo:1821 2021-02-12T23:13:26Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.18 psi 2021-02-12T23:13:26Z- handleDepth()-Depth_Keller msg: 1613171606837 epoch sec; seqNo:1839; depth 18.57 2021-02-12T23:13:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171606.86 epoch sec; seqNo:1839; 4056460.54 north; 590610.87 east 2021-02-12T23:13:27Z- AHRS_M2 msg: 1613171607.23 epoch sec; seqNo:1822 2021-02-12T23:13:27Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.18 psi 2021-02-12T23:13:27Z- handleDepth()-Depth_Keller msg: 1613171607241 epoch sec; seqNo:1840; depth 18.72 2021-02-12T23:13:27Z- handleDvl() - RDI_Pathfinder msg: 1613171607.26 epoch sec; seqNo:779 2021-02-12T23:13:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 90.95 , 66.19 , 103.11 2021-02-12T23:13:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.10 , -0.13 2021-02-12T23:13:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171607.28 epoch sec; seqNo:1840; 4056460.91 north; 590610.87 east 2021-02-12T23:13:27Z- AHRS_M2 msg: 1613171607.64 epoch sec; seqNo:1823 2021-02-12T23:13:27Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.18 psi 2021-02-12T23:13:27Z- handleDepth()-Depth_Keller msg: 1613171607644 epoch sec; seqNo:1841; depth 18.85 2021-02-12T23:13:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171607.66 epoch sec; seqNo:1841; 4056461.29 north; 590610.86 east 2021-02-12T23:13:28Z- AHRS_M2 msg: 1613171608.05 epoch sec; seqNo:1824 2021-02-12T23:13:28Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-12T23:13:28Z- handleDepth()-Depth_Keller msg: 1613171608053 epoch sec; seqNo:1842; depth 18.98 2021-02-12T23:13:28Z- handleDvl() - RDI_Pathfinder msg: 1613171608.07 epoch sec; seqNo:780 2021-02-12T23:13:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 92.23 , 64.56 , 97.10 2021-02-12T23:13:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.14 , -0.09 2021-02-12T23:13:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171608.08 epoch sec; seqNo:1842; 4056461.67 north; 590610.86 east 2021-02-12T23:13:28Z- AHRS_M2 msg: 1613171608.45 epoch sec; seqNo:1825 2021-02-12T23:13:28Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-12T23:13:28Z- handleDepth()-Depth_Keller msg: 1613171608463 epoch sec; seqNo:1843; depth 19.13 2021-02-12T23:13:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171608.47 epoch sec; seqNo:1843; 4056461.67 north; 590610.86 east 2021-02-12T23:13:28Z- AHRS_M2 msg: 1613171608.85 epoch sec; seqNo:1826 2021-02-12T23:13:28Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-12T23:13:28Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-12T23:13:28Z- waiting for 1613171606.62, time is 1613171608.85 2021-02-12T23:13:28Z- LcmTrn::updateTrn() >>>> pose time 1613171608.85, meas time 1613171608.07 <<<< 2021-02-12T23:13:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171608.07 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:13:28Z- TerrainNav::motionUpdate() - currEstimate = (4056462, 590610.86, 19.13) 2021-02-12T23:13:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:13:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:13:28Z- TNavBF::Projecting Measurements 2021-02-12T23:13:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:13:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:13:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:13:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:13:28Z- TNavFilter::4 good beams. 2021-02-12T23:13:28Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:13:28Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:13:28Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:13:28Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:13:28Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:13:28Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:13:28Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:13:28Z- TNavBF::mmse selected from filter -1 2021-02-12T23:13:28Z- TerrainMapOctree: (vehN, vehE ) = (4056461.16, 590610.68). 2021-02-12T23:13:28Z- TerrainMapOctree: Min Distance = 6014.78. 2021-02-12T23:13:28Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:13:28Z- There is no specific good beam that all particles have in common. 2021-02-12T23:13:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:13:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-12T23:13:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:13:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 13.61 2021-02-12T23:13:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:13:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 6.67 2021-02-12T23:13:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:13:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.76 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-12T23:13:28Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:13:28Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:13:28Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:13:28Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:13:28Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:13:28Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:13:28Z- Filter 0 No beams used in subcloud update 2021-02-12T23:13:28Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:13:28Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:13:28Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 792.1 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:13:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171608.07 sec. 2021-02-12T23:13:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:13:28Z- TNavBF::mmse selected from filter -1 2021-02-12T23:13:28Z- variances([0], [2], and psi_berg) = 792.041641 833.087092 0.000000 2021-02-12T23:13:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:13:28Z- LcmTrn::publishEstimates() - MLE : 4056434.17 590562.36 19.13 2021-02-12T23:13:28Z- LcmTrn::publishEstimates() - MMSE : 4056461.67 590610.86 19.13 2021-02-12T23:13:28Z- LcmTrn::publishEstimates() - COVAR: 792.04 833.09 0.76 0.00 2021-02-12T23:13:28Z- handleDepth()-Depth_Keller msg: 1613171608852 epoch sec; seqNo:1844; depth 19.26 2021-02-12T23:13:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171608.86 epoch sec; seqNo:1844; 4056462.05 north; 590610.85 east 2021-02-12T23:13:29Z- AHRS_M2 msg: 1613171609.25 epoch sec; seqNo:1827 2021-02-12T23:13:29Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-12T23:13:29Z- handleDepth()-Depth_Keller msg: 1613171609261 epoch sec; seqNo:1845; depth 19.40 2021-02-12T23:13:29Z- handleDvl() - RDI_Pathfinder msg: 1613171609.27 epoch sec; seqNo:781 2021-02-12T23:13:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 91.29 , 65.79 , 96.69 2021-02-12T23:13:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.16 , -0.01 2021-02-12T23:13:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171609.28 epoch sec; seqNo:1845; 4056462.43 north; 590610.85 east 2021-02-12T23:13:29Z- AHRS_M2 msg: 1613171609.65 epoch sec; seqNo:1828 2021-02-12T23:13:29Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-12T23:13:29Z- handleDepth()-Depth_Keller msg: 1613171609662 epoch sec; seqNo:1846; depth 19.53 2021-02-12T23:13:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171609.67 epoch sec; seqNo:1846; 4056462.82 north; 590612.07 east 2021-02-12T23:13:30Z- AHRS_M2 msg: 1613171610.06 epoch sec; seqNo:1829 2021-02-12T23:13:30Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.18 psi 2021-02-12T23:13:30Z- handleDepth()-Depth_Keller msg: 1613171610065 epoch sec; seqNo:1847; depth 19.67 2021-02-12T23:13:30Z- handleDvl() - RDI_Pathfinder msg: 1613171610.08 epoch sec; seqNo:782 2021-02-12T23:13:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.55 , 90.21 , 65.38 , 96.69 2021-02-12T23:13:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.11 , 0.10 2021-02-12T23:13:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171610.09 epoch sec; seqNo:1847; 4056462.82 north; 590612.07 east 2021-02-12T23:13:30Z- AHRS_M2 msg: 1613171610.46 epoch sec; seqNo:1830 2021-02-12T23:13:30Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.18 psi 2021-02-12T23:13:30Z- handleDepth()-Depth_Keller msg: 1613171610473 epoch sec; seqNo:1848; depth 19.81 2021-02-12T23:13:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171610.48 epoch sec; seqNo:1848; 4056463.20 north; 590612.06 east 2021-02-12T23:13:30Z- AHRS_M2 msg: 1613171610.87 epoch sec; seqNo:1831 2021-02-12T23:13:30Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.18 psi 2021-02-12T23:13:30Z- handleDepth()-Depth_Keller msg: 1613171610881 epoch sec; seqNo:1849; depth 19.94 2021-02-12T23:13:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171610.89 epoch sec; seqNo:1849; 4056463.58 north; 590612.06 east 2021-02-12T23:13:31Z- AHRS_M2 msg: 1613171611.27 epoch sec; seqNo:1832 2021-02-12T23:13:31Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.18 psi 2021-02-12T23:13:31Z- handleDepth()-Depth_Keller msg: 1613171611282 epoch sec; seqNo:1850; depth 20.08 2021-02-12T23:13:31Z- handleDvl() - RDI_Pathfinder msg: 1613171611.29 epoch sec; seqNo:783 2021-02-12T23:13:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.55 , 89.19 , 64.97 , 96.76 2021-02-12T23:13:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.12 2021-02-12T23:13:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171611.30 epoch sec; seqNo:1850; 4056463.58 north; 590612.06 east 2021-02-12T23:13:31Z- AHRS_M2 msg: 1613171611.67 epoch sec; seqNo:1833 2021-02-12T23:13:31Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.18 psi 2021-02-12T23:13:31Z- handleDepth()-Depth_Keller msg: 1613171611685 epoch sec; seqNo:1851; depth 20.22 2021-02-12T23:13:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171611.70 epoch sec; seqNo:1851; 4056463.96 north; 590612.06 east 2021-02-12T23:13:32Z- AHRS_M2 msg: 1613171612.08 epoch sec; seqNo:1834 2021-02-12T23:13:32Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.18 psi 2021-02-12T23:13:32Z- handleDepth()-Depth_Keller msg: 1613171612084 epoch sec; seqNo:1852; depth 20.35 2021-02-12T23:13:32Z- handleDvl() - RDI_Pathfinder msg: 1613171612.10 epoch sec; seqNo:784 2021-02-12T23:13:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 89.19 , 65.38 , 104.33 2021-02-12T23:13:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , 0.07 2021-02-12T23:13:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171612.11 epoch sec; seqNo:1852; 4056464.34 north; 590612.05 east 2021-02-12T23:13:32Z- AHRS_M2 msg: 1613171612.48 epoch sec; seqNo:1835 2021-02-12T23:13:32Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.18 psi 2021-02-12T23:13:32Z- handleDepth()-Depth_Keller msg: 1613171612490 epoch sec; seqNo:1853; depth 20.50 2021-02-12T23:13:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171612.50 epoch sec; seqNo:1853; 4056464.35 north; 590613.27 east 2021-02-12T23:13:32Z- AHRS_M2 msg: 1613171612.89 epoch sec; seqNo:1836 2021-02-12T23:13:32Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.18 psi 2021-02-12T23:13:32Z- handleDepth()-Depth_Keller msg: 1613171612892 epoch sec; seqNo:1854; depth 20.63 2021-02-12T23:13:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171612.91 epoch sec; seqNo:1854; 4056464.73 north; 590613.27 east 2021-02-12T23:13:33Z- AHRS_M2 msg: 1613171613.29 epoch sec; seqNo:1837 2021-02-12T23:13:33Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.18 psi 2021-02-12T23:13:33Z- handleDepth()-Depth_Keller msg: 1613171613301 epoch sec; seqNo:1855; depth 20.77 2021-02-12T23:13:33Z- handleDvl() - RDI_Pathfinder msg: 1613171613.31 epoch sec; seqNo:785 2021-02-12T23:13:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.14 , 89.80 , 63.74 , 97.10 2021-02-12T23:13:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.07 , -0.01 2021-02-12T23:13:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171613.33 epoch sec; seqNo:1855; 4056465.11 north; 590613.26 east 2021-02-12T23:13:33Z- AHRS_M2 msg: 1613171613.69 epoch sec; seqNo:1838 2021-02-12T23:13:33Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.18 psi 2021-02-12T23:13:33Z- handleDepth()-Depth_Keller msg: 1613171613700 epoch sec; seqNo:1856; depth 20.91 2021-02-12T23:13:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171613.71 epoch sec; seqNo:1856; 4056465.11 north; 590613.26 east 2021-02-12T23:13:33Z- LcmTrn::handleMessages() - handling 43 messages took 0.63 ms and 131 calls... 2021-02-12T23:13:33Z- waiting for 1613171611.85, time is 1613171613.71 2021-02-12T23:13:33Z- LcmTrn::updateTrn() >>>> pose time 1613171613.71, meas time 1613171613.31 <<<< 2021-02-12T23:13:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171613.31 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:13:33Z- TerrainNav::motionUpdate() - currEstimate = (4056465, 590613.26, 20.91) 2021-02-12T23:13:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:13:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:13:33Z- TNavBF::Projecting Measurements 2021-02-12T23:13:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:13:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:13:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:13:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:13:33Z- TNavFilter::4 good beams. 2021-02-12T23:13:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:13:33Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:13:33Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:13:33Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:13:33Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:13:33Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:13:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:13:33Z- TNavBF::mmse selected from filter -1 2021-02-12T23:13:33Z- TerrainMapOctree: (vehN, vehE ) = (4056464.83, 590613.07). 2021-02-12T23:13:33Z- TerrainMapOctree: Min Distance = 6010.42. 2021-02-12T23:13:33Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:13:33Z- There is no specific good beam that all particles have in common. 2021-02-12T23:13:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:13:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-12T23:13:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:13:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 12.90 2021-02-12T23:13:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:13:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 6.50 2021-02-12T23:13:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:13:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.75 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-12T23:13:34Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:13:34Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:13:34Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:13:34Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:13:34Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:13:34Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:13:34Z- Filter 0 No beams used in subcloud update 2021-02-12T23:13:34Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:13:34Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:13:34Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 791.9 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:13:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171613.31 sec. 2021-02-12T23:13:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:13:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:13:34Z- variances([0], [2], and psi_berg) = 791.798908 833.086654 0.000000 2021-02-12T23:13:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:13:34Z- LcmTrn::publishEstimates() - MLE : 4056437.61 590564.76 20.91 2021-02-12T23:13:34Z- LcmTrn::publishEstimates() - MMSE : 4056465.11 590613.26 20.91 2021-02-12T23:13:34Z- LcmTrn::publishEstimates() - COVAR: 791.80 833.09 0.73 0.00 2021-02-12T23:13:34Z- AHRS_M2 msg: 1613171614.10 epoch sec; seqNo:1839 2021-02-12T23:13:34Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.19 psi 2021-02-12T23:13:34Z- handleDepth()-Depth_Keller msg: 1613171614107 epoch sec; seqNo:1857; depth 21.03 2021-02-12T23:13:34Z- handleDvl() - RDI_Pathfinder msg: 1613171614.12 epoch sec; seqNo:786 2021-02-12T23:13:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.73 , 90.27 , 64.97 , 96.76 2021-02-12T23:13:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , -0.06 2021-02-12T23:13:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171614.14 epoch sec; seqNo:1857; 4056465.49 north; 590613.26 east 2021-02-12T23:13:34Z- AHRS_M2 msg: 1613171614.50 epoch sec; seqNo:1840 2021-02-12T23:13:34Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.19 psi 2021-02-12T23:13:34Z- handleDepth()-Depth_Keller msg: 1613171614508 epoch sec; seqNo:1858; depth 21.18 2021-02-12T23:13:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171614.53 epoch sec; seqNo:1858; 4056465.86 north; 590613.26 east 2021-02-12T23:13:34Z- AHRS_M2 msg: 1613171614.91 epoch sec; seqNo:1841 2021-02-12T23:13:34Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.18 psi 2021-02-12T23:13:34Z- handleDepth()-Depth_Keller msg: 1613171614912 epoch sec; seqNo:1859; depth 21.34 2021-02-12T23:13:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171614.92 epoch sec; seqNo:1859; 4056466.26 north; 590614.47 east 2021-02-12T23:13:35Z- AHRS_M2 msg: 1613171615.31 epoch sec; seqNo:1842 2021-02-12T23:13:35Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.18 psi 2021-02-12T23:13:35Z- handleDepth()-Depth_Keller msg: 1613171615317 epoch sec; seqNo:1860; depth 21.47 2021-02-12T23:13:35Z- handleDvl() - RDI_Pathfinder msg: 1613171615.33 epoch sec; seqNo:787 2021-02-12T23:13:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.73 , 89.19 , 64.56 , 105.41 2021-02-12T23:13:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , -0.07 2021-02-12T23:13:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171615.34 epoch sec; seqNo:1860; 4056466.26 north; 590614.47 east 2021-02-12T23:13:35Z- AHRS_M2 msg: 1613171615.71 epoch sec; seqNo:1843 2021-02-12T23:13:35Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.17 psi 2021-02-12T23:13:35Z- handleDepth()-Depth_Keller msg: 1613171615729 epoch sec; seqNo:1861; depth 21.60 2021-02-12T23:13:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171615.74 epoch sec; seqNo:1861; 4056466.64 north; 590614.47 east 2021-02-12T23:13:36Z- AHRS_M2 msg: 1613171616.12 epoch sec; seqNo:1844 2021-02-12T23:13:36Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.17 psi 2021-02-12T23:13:36Z- handleDepth()-Depth_Keller msg: 1613171616128 epoch sec; seqNo:1862; depth 21.75 2021-02-12T23:13:36Z- handleDvl() - RDI_Pathfinder msg: 1613171616.15 epoch sec; seqNo:788 2021-02-12T23:13:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 87.43 , 62.52 , 97.17 2021-02-12T23:13:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , -0.05 2021-02-12T23:13:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171616.16 epoch sec; seqNo:1862; 4056467.01 north; 590614.47 east 2021-02-12T23:13:36Z- AHRS_M2 msg: 1613171616.52 epoch sec; seqNo:1845 2021-02-12T23:13:36Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.17 psi 2021-02-12T23:13:36Z- handleDepth()-Depth_Keller msg: 1613171616528 epoch sec; seqNo:1863; depth 21.90 2021-02-12T23:13:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171616.54 epoch sec; seqNo:1863; 4056467.39 north; 590614.46 east 2021-02-12T23:13:36Z- AHRS_M2 msg: 1613171616.93 epoch sec; seqNo:1846 2021-02-12T23:13:36Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.16 psi 2021-02-12T23:13:36Z- handleDepth()-Depth_Keller msg: 1613171616932 epoch sec; seqNo:1864; depth 22.04 2021-02-12T23:13:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171616.94 epoch sec; seqNo:1864; 4056467.41 north; 590615.68 east 2021-02-12T23:13:37Z- AHRS_M2 msg: 1613171617.33 epoch sec; seqNo:1847 2021-02-12T23:13:37Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.16 psi 2021-02-12T23:13:37Z- handleDepth()-Depth_Keller msg: 1613171617338 epoch sec; seqNo:1865; depth 22.17 2021-02-12T23:13:37Z- handleDvl() - RDI_Pathfinder msg: 1613171617.35 epoch sec; seqNo:789 2021-02-12T23:13:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.32 , 86.01 , 62.93 , 96.83 2021-02-12T23:13:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.05 2021-02-12T23:13:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171617.36 epoch sec; seqNo:1865; 4056467.78 north; 590615.68 east 2021-02-12T23:13:37Z- AHRS_M2 msg: 1613171617.74 epoch sec; seqNo:1848 2021-02-12T23:13:37Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.16 psi 2021-02-12T23:13:37Z- handleDepth()-Depth_Keller msg: 1613171617745 epoch sec; seqNo:1866; depth 22.31 2021-02-12T23:13:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171617.76 epoch sec; seqNo:1866; 4056468.16 north; 590615.67 east 2021-02-12T23:13:38Z- AHRS_M2 msg: 1613171618.14 epoch sec; seqNo:1849 2021-02-12T23:13:38Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.16 psi 2021-02-12T23:13:38Z- handleDepth()-Depth_Keller msg: 1613171618153 epoch sec; seqNo:1867; depth 22.47 2021-02-12T23:13:38Z- handleDvl() - RDI_Pathfinder msg: 1613171618.16 epoch sec; seqNo:790 2021-02-12T23:13:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 86.01 , 63.33 , 96.83 2021-02-12T23:13:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , -0.05 2021-02-12T23:13:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171618.20 epoch sec; seqNo:1867; 4056468.54 north; 590615.67 east 2021-02-12T23:13:38Z- AHRS_M2 msg: 1613171618.55 epoch sec; seqNo:1850 2021-02-12T23:13:38Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.17 psi 2021-02-12T23:13:38Z- handleDepth()-Depth_Keller msg: 1613171618556 epoch sec; seqNo:1868; depth 22.59 2021-02-12T23:13:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171618.58 epoch sec; seqNo:1868; 4056468.54 north; 590615.67 east 2021-02-12T23:13:38Z- AHRS_M2 msg: 1613171618.95 epoch sec; seqNo:1851 2021-02-12T23:13:38Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.17 psi 2021-02-12T23:13:38Z- handleDepth()-Depth_Keller msg: 1613171618962 epoch sec; seqNo:1869; depth 22.73 2021-02-12T23:13:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171618.97 epoch sec; seqNo:1869; 4056468.92 north; 590615.67 east 2021-02-12T23:13:39Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-12T23:13:39Z- waiting for 1613171616.71, time is 1613171618.97 2021-02-12T23:13:39Z- LcmTrn::updateTrn() >>>> pose time 1613171618.97, meas time 1613171618.16 <<<< 2021-02-12T23:13:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171618.16 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:13:39Z- TerrainNav::motionUpdate() - currEstimate = (4056469, 590615.67, 22.73) 2021-02-12T23:13:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:13:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:13:39Z- TNavBF::Projecting Measurements 2021-02-12T23:13:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:13:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:13:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:13:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:13:39Z- TNavFilter::4 good beams. 2021-02-12T23:13:39Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:13:39Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:13:39Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:13:39Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:13:39Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:13:39Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:13:39Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:13:39Z- TNavBF::mmse selected from filter -1 2021-02-12T23:13:39Z- TerrainMapOctree: (vehN, vehE ) = (4056468.34, 590615.30). 2021-02-12T23:13:39Z- TerrainMapOctree: Min Distance = 6006.28. 2021-02-12T23:13:39Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:13:39Z- There is no specific good beam that all particles have in common. 2021-02-12T23:13:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:13:39Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 8.27 2021-02-12T23:13:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:13:39Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 11.84 2021-02-12T23:13:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:13:39Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 6.42 2021-02-12T23:13:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:13:39Z- Old Depth Bias: 0.00 DepthBiasCov: 0.73 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 15.00 2021-02-12T23:13:39Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:13:39Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:13:39Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:13:39Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:13:39Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:13:39Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:13:39Z- Filter 0 No beams used in subcloud update 2021-02-12T23:13:39Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:13:39Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:13:39Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 791.6 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:13:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171618.16 sec. 2021-02-12T23:13:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:13:39Z- TNavBF::mmse selected from filter -1 2021-02-12T23:13:39Z- variances([0], [2], and psi_berg) = 791.535038 833.086180 0.000000 2021-02-12T23:13:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:13:39Z- LcmTrn::publishEstimates() - MLE : 4056441.42 590664.17 22.73 2021-02-12T23:13:39Z- LcmTrn::publishEstimates() - MMSE : 4056468.92 590615.67 22.73 2021-02-12T23:13:39Z- LcmTrn::publishEstimates() - COVAR: 791.54 833.09 0.73 0.00 2021-02-12T23:13:39Z- AHRS_M2 msg: 1613171619.35 epoch sec; seqNo:1852 2021-02-12T23:13:39Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.18 psi 2021-02-12T23:13:39Z- handleDepth()-Depth_Keller msg: 1613171619357 epoch sec; seqNo:1870; depth 22.87 2021-02-12T23:13:39Z- handleDvl() - RDI_Pathfinder msg: 1613171619.37 epoch sec; seqNo:791 2021-02-12T23:13:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 87.10 , 62.52 , 101.15 2021-02-12T23:13:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , -0.06 2021-02-12T23:13:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171619.39 epoch sec; seqNo:1870; 4056469.31 north; 590616.88 east 2021-02-12T23:13:39Z- AHRS_M2 msg: 1613171619.76 epoch sec; seqNo:1853 2021-02-12T23:13:39Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.19 psi 2021-02-12T23:13:39Z- handleDepth()-Depth_Keller msg: 1613171619765 epoch sec; seqNo:1871; depth 23.01 2021-02-12T23:13:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171619.78 epoch sec; seqNo:1871; 4056469.31 north; 590616.88 east 2021-02-12T23:13:40Z- AHRS_M2 msg: 1613171620.16 epoch sec; seqNo:1854 2021-02-12T23:13:40Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.19 psi 2021-02-12T23:13:40Z- handleDepth()-Depth_Keller msg: 1613171620165 epoch sec; seqNo:1872; depth 23.15 2021-02-12T23:13:40Z- handleDvl() - RDI_Pathfinder msg: 1613171620.18 epoch sec; seqNo:792 2021-02-12T23:13:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.28 , 84.93 , 62.52 , 96.83 2021-02-12T23:13:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , -0.09 2021-02-12T23:13:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171620.19 epoch sec; seqNo:1872; 4056469.69 north; 590616.88 east 2021-02-12T23:13:40Z- AHRS_M2 msg: 1613171620.56 epoch sec; seqNo:1855 2021-02-12T23:13:40Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.20 psi 2021-02-12T23:13:40Z- handleDepth()-Depth_Keller msg: 1613171620578 epoch sec; seqNo:1873; depth 23.29 2021-02-12T23:13:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171620.59 epoch sec; seqNo:1873; 4056470.07 north; 590616.88 east 2021-02-12T23:13:40Z- AHRS_M2 msg: 1613171620.97 epoch sec; seqNo:1856 2021-02-12T23:13:40Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.20 psi 2021-02-12T23:13:40Z- handleDepth()-Depth_Keller msg: 1613171620980 epoch sec; seqNo:1874; depth 23.43 2021-02-12T23:13:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171621.04 epoch sec; seqNo:1874; 4056470.07 north; 590616.88 east 2021-02-12T23:13:41Z- AHRS_M2 msg: 1613171621.37 epoch sec; seqNo:1857 2021-02-12T23:13:41Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.21 psi 2021-02-12T23:13:41Z- handleDepth()-Depth_Keller msg: 1613171621377 epoch sec; seqNo:1875; depth 23.55 2021-02-12T23:13:41Z- handleDvl() - RDI_Pathfinder msg: 1613171621.39 epoch sec; seqNo:793 2021-02-12T23:13:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 88.18 , 62.11 , 96.83 2021-02-12T23:13:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.08 2021-02-12T23:13:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171621.40 epoch sec; seqNo:1875; 4056470.45 north; 590616.87 east 2021-02-12T23:13:41Z- AHRS_M2 msg: 1613171621.78 epoch sec; seqNo:1858 2021-02-12T23:13:41Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.21 psi 2021-02-12T23:13:41Z- handleDepth()-Depth_Keller msg: 1613171621790 epoch sec; seqNo:1876; depth 23.69 2021-02-12T23:13:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171621.80 epoch sec; seqNo:1876; 4056470.84 north; 590618.09 east 2021-02-12T23:13:42Z- AHRS_M2 msg: 1613171622.18 epoch sec; seqNo:1859 2021-02-12T23:13:42Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.21 psi 2021-02-12T23:13:42Z- handleDepth()-Depth_Keller msg: 1613171622184 epoch sec; seqNo:1877; depth 23.84 2021-02-12T23:13:42Z- handleDvl() - RDI_Pathfinder msg: 1613171622.20 epoch sec; seqNo:794 2021-02-12T23:13:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 87.10 , 62.52 , 96.83 2021-02-12T23:13:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.04 2021-02-12T23:13:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171622.23 epoch sec; seqNo:1877; 4056471.22 north; 590618.08 east 2021-02-12T23:13:42Z- AHRS_M2 msg: 1613171622.58 epoch sec; seqNo:1860 2021-02-12T23:13:42Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.22 psi 2021-02-12T23:13:42Z- handleDepth()-Depth_Keller msg: 1613171622593 epoch sec; seqNo:1878; depth 23.98 2021-02-12T23:13:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171622.60 epoch sec; seqNo:1878; 4056471.22 north; 590618.08 east 2021-02-12T23:13:43Z- AHRS_M2 msg: 1613171623.02 epoch sec; seqNo:1861 2021-02-12T23:13:43Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.22 psi 2021-02-12T23:13:43Z- handleDepth()-Depth_Keller msg: 1613171623036 epoch sec; seqNo:1879; depth 24.12 2021-02-12T23:13:43Z- handleDvl() - RDI_Pathfinder msg: 1613171623.06 epoch sec; seqNo:795 2021-02-12T23:13:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 87.10 , 60.88 , 96.83 2021-02-12T23:13:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.00 2021-02-12T23:13:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171623.07 epoch sec; seqNo:1879; 4056471.60 north; 590618.08 east 2021-02-12T23:13:43Z- AHRS_M2 msg: 1613171623.39 epoch sec; seqNo:1862 2021-02-12T23:13:43Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.22 psi 2021-02-12T23:13:43Z- handleDepth()-Depth_Keller msg: 1613171623396 epoch sec; seqNo:1880; depth 24.26 2021-02-12T23:13:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171623.42 epoch sec; seqNo:1880; 4056471.98 north; 590618.08 east 2021-02-12T23:13:43Z- AHRS_M2 msg: 1613171623.80 epoch sec; seqNo:1863 2021-02-12T23:13:43Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.22 psi 2021-02-12T23:13:43Z- handleDepth()-Depth_Keller msg: 1613171623805 epoch sec; seqNo:1881; depth 24.38 2021-02-12T23:13:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171623.81 epoch sec; seqNo:1881; 4056471.98 north; 590618.08 east 2021-02-12T23:13:44Z- LcmTrn::handleMessages() - handling 41 messages took 0.61 ms and 129 calls... 2021-02-12T23:13:44Z- waiting for 1613171621.97, time is 1613171623.81 2021-02-12T23:13:44Z- LcmTrn::updateTrn() >>>> pose time 1613171623.81, meas time 1613171623.06 <<<< 2021-02-12T23:13:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171623.06 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:13:44Z- TerrainNav::motionUpdate() - currEstimate = (4056472, 590618.08, 24.38) 2021-02-12T23:13:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:13:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:13:44Z- TNavBF::Projecting Measurements 2021-02-12T23:13:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:13:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:13:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:13:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:13:44Z- TNavFilter::4 good beams. 2021-02-12T23:13:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:13:44Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:13:44Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:13:44Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:13:44Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:13:44Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:13:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:13:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:13:44Z- TerrainMapOctree: (vehN, vehE ) = (4056471.50, 590617.70). 2021-02-12T23:13:44Z- TerrainMapOctree: Min Distance = 6002.30. 2021-02-12T23:13:44Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:13:44Z- There is no specific good beam that all particles have in common. 2021-02-12T23:13:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:13:44Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-12T23:13:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:13:44Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 12.14 2021-02-12T23:13:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:13:44Z- Old Depth Bias: 0.00 DepthBiasCov: 0.77 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 5.93 2021-02-12T23:13:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:13:44Z- Old Depth Bias: 0.00 DepthBiasCov: 0.70 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 15.00 2021-02-12T23:13:44Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:13:44Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:13:44Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:13:44Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:13:44Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:13:44Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:13:44Z- Filter 0 No beams used in subcloud update 2021-02-12T23:13:44Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:13:44Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:13:44Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 791.4 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:13:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171623.06 sec. 2021-02-12T23:13:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:13:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:13:44Z- variances([0], [2], and psi_berg) = 791.290768 833.085743 0.000000 2021-02-12T23:13:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:13:44Z- LcmTrn::publishEstimates() - MLE : 4056444.48 590666.58 24.38 2021-02-12T23:13:44Z- LcmTrn::publishEstimates() - MMSE : 4056471.98 590618.08 24.38 2021-02-12T23:13:44Z- LcmTrn::publishEstimates() - COVAR: 791.29 833.09 0.71 0.00 2021-02-12T23:13:44Z- AHRS_M2 msg: 1613171624.20 epoch sec; seqNo:1864 2021-02-12T23:13:44Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.21 psi 2021-02-12T23:13:44Z- handleDepth()-Depth_Keller msg: 1613171624208 epoch sec; seqNo:1882; depth 24.51 2021-02-12T23:13:44Z- handleDvl() - RDI_Pathfinder msg: 1613171624.22 epoch sec; seqNo:796 2021-02-12T23:13:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.83 , 88.18 , 61.29 , 98.99 2021-02-12T23:13:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.01 , 0.02 2021-02-12T23:13:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171624.23 epoch sec; seqNo:1882; 4056472.37 north; 590619.29 east 2021-02-12T23:13:44Z- AHRS_M2 msg: 1613171624.61 epoch sec; seqNo:1865 2021-02-12T23:13:44Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.21 psi 2021-02-12T23:13:44Z- handleDepth()-Depth_Keller msg: 1613171624610 epoch sec; seqNo:1883; depth 24.66 2021-02-12T23:13:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171624.62 epoch sec; seqNo:1883; 4056472.75 north; 590619.29 east 2021-02-12T23:13:45Z- AHRS_M2 msg: 1613171625.03 epoch sec; seqNo:1866 2021-02-12T23:13:45Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.21 psi 2021-02-12T23:13:45Z- handleDepth()-Depth_Keller msg: 1613171625042 epoch sec; seqNo:1884; depth 24.79 2021-02-12T23:13:45Z- handleDvl() - RDI_Pathfinder msg: 1613171625.05 epoch sec; seqNo:797 2021-02-12T23:13:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 88.18 , 61.70 , 96.83 2021-02-12T23:13:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , -0.00 2021-02-12T23:13:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171625.07 epoch sec; seqNo:1884; 4056472.75 north; 590619.29 east 2021-02-12T23:13:45Z- AHRS_M2 msg: 1613171625.42 epoch sec; seqNo:1867 2021-02-12T23:13:45Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.21 psi 2021-02-12T23:13:45Z- handleDepth()-Depth_Keller msg: 1613171625426 epoch sec; seqNo:1885; depth 24.96 2021-02-12T23:13:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171625.44 epoch sec; seqNo:1885; 4056473.13 north; 590619.29 east 2021-02-12T23:13:45Z- AHRS_M2 msg: 1613171625.82 epoch sec; seqNo:1868 2021-02-12T23:13:45Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.21 psi 2021-02-12T23:13:45Z- handleDepth()-Depth_Keller msg: 1613171625829 epoch sec; seqNo:1886; depth 25.09 2021-02-12T23:13:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171625.84 epoch sec; seqNo:1886; 4056473.51 north; 590619.28 east 2021-02-12T23:13:46Z- AHRS_M2 msg: 1613171626.22 epoch sec; seqNo:1869 2021-02-12T23:13:46Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.21 psi 2021-02-12T23:13:46Z- handleDepth()-Depth_Keller msg: 1613171626228 epoch sec; seqNo:1887; depth 25.21 2021-02-12T23:13:46Z- handleDvl() - RDI_Pathfinder msg: 1613171626.25 epoch sec; seqNo:798 2021-02-12T23:13:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 85.88 , 60.88 , 96.69 2021-02-12T23:13:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , -0.01 2021-02-12T23:13:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171626.26 epoch sec; seqNo:1887; 4056473.51 north; 590619.28 east 2021-02-12T23:13:46Z- AHRS_M2 msg: 1613171626.62 epoch sec; seqNo:1870 2021-02-12T23:13:46Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.21 psi 2021-02-12T23:13:46Z- handleDepth()-Depth_Keller msg: 1613171626633 epoch sec; seqNo:1888; depth 25.37 2021-02-12T23:13:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171626.64 epoch sec; seqNo:1888; 4056473.89 north; 590619.28 east 2021-02-12T23:13:47Z- AHRS_M2 msg: 1613171627.04 epoch sec; seqNo:1871 2021-02-12T23:13:47Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.21 psi 2021-02-12T23:13:47Z- handleDepth()-Depth_Keller msg: 1613171627048 epoch sec; seqNo:1889; depth 25.50 2021-02-12T23:13:47Z- handleDvl() - RDI_Pathfinder msg: 1613171627.06 epoch sec; seqNo:799 2021-02-12T23:13:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.01 , 87.03 , 60.07 , 95.68 2021-02-12T23:13:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.01 2021-02-12T23:13:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171627.09 epoch sec; seqNo:1889; 4056474.28 north; 590620.49 east 2021-02-12T23:13:47Z- AHRS_M2 msg: 1613171627.43 epoch sec; seqNo:1872 2021-02-12T23:13:47Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.21 psi 2021-02-12T23:13:47Z- handleDepth()-Depth_Keller msg: 1613171627438 epoch sec; seqNo:1890; depth 25.66 2021-02-12T23:13:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171627.45 epoch sec; seqNo:1890; 4056474.66 north; 590620.49 east 2021-02-12T23:13:47Z- AHRS_M2 msg: 1613171627.84 epoch sec; seqNo:1873 2021-02-12T23:13:47Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.21 psi 2021-02-12T23:13:47Z- handleDepth()-Depth_Keller msg: 1613171627840 epoch sec; seqNo:1891; depth 25.78 2021-02-12T23:13:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171627.86 epoch sec; seqNo:1891; 4056474.66 north; 590620.49 east 2021-02-12T23:13:48Z- AHRS_M2 msg: 1613171628.24 epoch sec; seqNo:1874 2021-02-12T23:13:48Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-12T23:13:48Z- handleDepth()-Depth_Keller msg: 1613171628249 epoch sec; seqNo:1892; depth 25.93 2021-02-12T23:13:48Z- handleDvl() - RDI_Pathfinder msg: 1613171628.26 epoch sec; seqNo:800 2021-02-12T23:13:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.19 , 84.93 , 60.47 , 96.83 2021-02-12T23:13:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.01 2021-02-12T23:13:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171628.29 epoch sec; seqNo:1892; 4056475.03 north; 590620.49 east 2021-02-12T23:13:48Z- AHRS_M2 msg: 1613171628.64 epoch sec; seqNo:1875 2021-02-12T23:13:48Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-12T23:13:48Z- handleDepth()-Depth_Keller msg: 1613171628648 epoch sec; seqNo:1893; depth 26.07 2021-02-12T23:13:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171628.66 epoch sec; seqNo:1893; 4056475.41 north; 590620.48 east 2021-02-12T23:13:49Z- AHRS_M2 msg: 1613171629.05 epoch sec; seqNo:1876 2021-02-12T23:13:49Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.20 psi 2021-02-12T23:13:49Z- handleDepth()-Depth_Keller msg: 1613171629065 epoch sec; seqNo:1894; depth 26.20 2021-02-12T23:13:49Z- handleDvl() - RDI_Pathfinder msg: 1613171629.08 epoch sec; seqNo:801 2021-02-12T23:13:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.60 , 84.93 , 60.47 , 95.75 2021-02-12T23:13:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , 0.03 2021-02-12T23:13:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171629.09 epoch sec; seqNo:1894; 4056475.79 north; 590620.48 east 2021-02-12T23:13:49Z- LcmTrn::handleMessages() - handling 45 messages took 0.64 ms and 132 calls... 2021-02-12T23:13:49Z- waiting for 1613171626.81, time is 1613171629.09 2021-02-12T23:13:49Z- LcmTrn::updateTrn() equating pose 1613171629.09 and meas times 1613171629.09 2021-02-12T23:13:49Z- LcmTrn::updateTrn() >>>> pose time 1613171629.09, meas time 1613171629.09 <<<< 2021-02-12T23:13:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171629.09 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:13:49Z- TerrainNav::motionUpdate() - currEstimate = (4056476, 590620.48, 26.20) 2021-02-12T23:13:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:13:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:13:49Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171629.09 sec.; No relevant navigation data available 2021-02-12T23:13:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:13:49Z- TNavBF::mmse selected from filter -1 2021-02-12T23:13:49Z- variances([0], [2], and psi_berg) = 791.091387 833.085388 0.000000 2021-02-12T23:13:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:13:49Z- LcmTrn::publishEstimates() - MLE : 4056448.29 590668.98 26.20 2021-02-12T23:13:49Z- LcmTrn::publishEstimates() - MMSE : 4056475.79 590620.48 26.20 2021-02-12T23:13:49Z- LcmTrn::publishEstimates() - COVAR: 791.09 833.09 0.94 0.00 2021-02-12T23:13:49Z- AHRS_M2 msg: 1613171629.45 epoch sec; seqNo:1877 2021-02-12T23:13:49Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.19 psi 2021-02-12T23:13:49Z- handleDepth()-Depth_Keller msg: 1613171629461 epoch sec; seqNo:1895; depth 26.32 2021-02-12T23:13:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171629.48 epoch sec; seqNo:1895; 4056475.81 north; 590621.70 east 2021-02-12T23:13:49Z- AHRS_M2 msg: 1613171629.86 epoch sec; seqNo:1878 2021-02-12T23:13:49Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.19 psi 2021-02-12T23:13:49Z- handleDepth()-Depth_Keller msg: 1613171629861 epoch sec; seqNo:1896; depth 26.46 2021-02-12T23:13:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171629.87 epoch sec; seqNo:1896; 4056476.18 north; 590621.70 east 2021-02-12T23:13:50Z- AHRS_M2 msg: 1613171630.27 epoch sec; seqNo:1879 2021-02-12T23:13:50Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.19 psi 2021-02-12T23:13:50Z- handleDepth()-Depth_Keller msg: 1613171630270 epoch sec; seqNo:1897; depth 26.60 2021-02-12T23:13:50Z- handleDvl() - RDI_Pathfinder msg: 1613171630.28 epoch sec; seqNo:802 2021-02-12T23:13:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.19 , 79.68 , 59.66 , 96.83 2021-02-12T23:13:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.01 2021-02-12T23:13:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171630.29 epoch sec; seqNo:1897; 4056476.56 north; 590621.69 east 2021-02-12T23:13:50Z- AHRS_M2 msg: 1613171630.66 epoch sec; seqNo:1880 2021-02-12T23:13:50Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.19 psi 2021-02-12T23:13:50Z- handleDepth()-Depth_Keller msg: 1613171630668 epoch sec; seqNo:1898; depth 26.73 2021-02-12T23:13:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171630.69 epoch sec; seqNo:1898; 4056476.56 north; 590621.69 east 2021-02-12T23:13:51Z- AHRS_M2 msg: 1613171631.07 epoch sec; seqNo:1881 2021-02-12T23:13:51Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-12T23:13:51Z- handleDepth()-Depth_Keller msg: 1613171631081 epoch sec; seqNo:1899; depth 26.87 2021-02-12T23:13:51Z- handleDvl() - RDI_Pathfinder msg: 1613171631.10 epoch sec; seqNo:803 2021-02-12T23:13:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.79 , 79.68 , 58.84 , 96.83 2021-02-12T23:13:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , -0.02 2021-02-12T23:13:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171631.11 epoch sec; seqNo:1899; 4056476.94 north; 590621.69 east 2021-02-12T23:13:51Z- AHRS_M2 msg: 1613171631.47 epoch sec; seqNo:1882 2021-02-12T23:13:51Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-12T23:13:51Z- handleDepth()-Depth_Keller msg: 1613171631476 epoch sec; seqNo:1900; depth 27.01 2021-02-12T23:13:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171631.49 epoch sec; seqNo:1900; 4056477.32 north; 590621.68 east 2021-02-12T23:13:51Z- AHRS_M2 msg: 1613171631.88 epoch sec; seqNo:1883 2021-02-12T23:13:51Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-12T23:13:51Z- handleDepth()-Depth_Keller msg: 1613171631882 epoch sec; seqNo:1901; depth 27.12 2021-02-12T23:13:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171631.89 epoch sec; seqNo:1901; 4056477.71 north; 590622.90 east 2021-02-12T23:13:52Z- AHRS_M2 msg: 1613171632.28 epoch sec; seqNo:1884 2021-02-12T23:13:52Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-12T23:13:52Z- handleDepth()-Depth_Keller msg: 1613171632284 epoch sec; seqNo:1902; depth 27.25 2021-02-12T23:13:52Z- handleDvl() - RDI_Pathfinder msg: 1613171632.30 epoch sec; seqNo:804 2021-02-12T23:13:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.79 , 78.45 , 58.43 , 93.58 2021-02-12T23:13:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.04 , -0.08 2021-02-12T23:13:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171632.31 epoch sec; seqNo:1902; 4056477.71 north; 590622.90 east 2021-02-12T23:13:52Z- AHRS_M2 msg: 1613171632.68 epoch sec; seqNo:1885 2021-02-12T23:13:52Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-12T23:13:52Z- handleDepth()-Depth_Keller msg: 1613171632688 epoch sec; seqNo:1903; depth 27.40 2021-02-12T23:13:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171632.70 epoch sec; seqNo:1903; 4056478.09 north; 590622.90 east 2021-02-12T23:13:53Z- AHRS_M2 msg: 1613171633.09 epoch sec; seqNo:1886 2021-02-12T23:13:53Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.19 psi 2021-02-12T23:13:53Z- handleDepth()-Depth_Keller msg: 1613171633092 epoch sec; seqNo:1904; depth 27.53 2021-02-12T23:13:53Z- handleDvl() - RDI_Pathfinder msg: 1613171633.10 epoch sec; seqNo:805 2021-02-12T23:13:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.97 , 77.64 , 59.25 , 93.58 2021-02-12T23:13:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , -0.08 2021-02-12T23:13:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171633.12 epoch sec; seqNo:1904; 4056478.47 north; 590622.89 east 2021-02-12T23:13:53Z- AHRS_M2 msg: 1613171633.49 epoch sec; seqNo:1887 2021-02-12T23:13:53Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.19 psi 2021-02-12T23:13:53Z- handleDepth()-Depth_Keller msg: 1613171633497 epoch sec; seqNo:1905; depth 27.66 2021-02-12T23:13:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171633.51 epoch sec; seqNo:1905; 4056478.85 north; 590622.89 east 2021-02-12T23:13:53Z- AHRS_M2 msg: 1613171633.89 epoch sec; seqNo:1888 2021-02-12T23:13:53Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.19 psi 2021-02-12T23:13:53Z- handleDepth()-Depth_Keller msg: 1613171633900 epoch sec; seqNo:1906; depth 27.79 2021-02-12T23:13:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171633.92 epoch sec; seqNo:1906; 4056479.23 north; 590622.89 east 2021-02-12T23:13:54Z- LcmTrn::handleMessages() - handling 40 messages took 0.61 ms and 128 calls... 2021-02-12T23:13:54Z- waiting for 1613171632.09, time is 1613171633.92 2021-02-12T23:13:54Z- LcmTrn::updateTrn() >>>> pose time 1613171633.92, meas time 1613171633.10 <<<< 2021-02-12T23:13:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171633.10 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:13:54Z- TerrainNav::motionUpdate() - currEstimate = (4056479, 590622.89, 27.79) 2021-02-12T23:13:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:13:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:13:54Z- TNavBF::Projecting Measurements 2021-02-12T23:13:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:13:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:13:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:13:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:13:54Z- TNavFilter::4 good beams. 2021-02-12T23:13:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:13:54Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:13:54Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:13:54Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:13:54Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:13:54Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:13:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:13:54Z- TNavBF::mmse selected from filter -1 2021-02-12T23:13:54Z- TerrainMapOctree: (vehN, vehE ) = (4056478.65, 590622.48). 2021-02-12T23:13:54Z- TerrainMapOctree: Min Distance = 5993.72. 2021-02-12T23:13:54Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:13:54Z- There is no specific good beam that all particles have in common. 2021-02-12T23:13:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:13:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.116 ExpectedMeasDiff: nan currMeas.covariance: 6.75 2021-02-12T23:13:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:13:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 9.64 2021-02-12T23:13:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:13:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.110 ExpectedMeasDiff: nan currMeas.covariance: 5.62 2021-02-12T23:13:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:13:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 14.01 2021-02-12T23:13:54Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:13:54Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:13:54Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:13:54Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:13:54Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:13:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:13:54Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:13:54Z- Filter 0 No beams used in subcloud update 2021-02-12T23:13:54Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:13:54Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:13:54Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 790.9 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:13:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171633.10 sec. 2021-02-12T23:13:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:13:54Z- TNavBF::mmse selected from filter -1 2021-02-12T23:13:54Z- variances([0], [2], and psi_berg) = 790.838292 833.084940 0.000000 2021-02-12T23:13:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:13:54Z- LcmTrn::publishEstimates() - MLE : 4056451.73 590671.39 27.79 2021-02-12T23:13:54Z- LcmTrn::publishEstimates() - MMSE : 4056479.23 590622.89 27.79 2021-02-12T23:13:54Z- LcmTrn::publishEstimates() - COVAR: 790.84 833.08 0.80 0.00 2021-02-12T23:13:54Z- AHRS_M2 msg: 1613171634.30 epoch sec; seqNo:1889 2021-02-12T23:13:54Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.19 psi 2021-02-12T23:13:54Z- handleDepth()-Depth_Keller msg: 1613171634304 epoch sec; seqNo:1907; depth 27.92 2021-02-12T23:13:54Z- handleDvl() - RDI_Pathfinder msg: 1613171634.32 epoch sec; seqNo:806 2021-02-12T23:13:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.97 , 76.82 , 58.84 , 93.58 2021-02-12T23:13:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , -0.07 2021-02-12T23:13:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171634.34 epoch sec; seqNo:1907; 4056479.24 north; 590624.11 east 2021-02-12T23:13:54Z- AHRS_M2 msg: 1613171634.71 epoch sec; seqNo:1890 2021-02-12T23:13:54Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.19 psi 2021-02-12T23:13:54Z- handleDepth()-Depth_Keller msg: 1613171634716 epoch sec; seqNo:1908; depth 28.06 2021-02-12T23:13:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171634.75 epoch sec; seqNo:1908; 4056479.62 north; 590624.10 east 2021-02-12T23:13:55Z- AHRS_M2 msg: 1613171635.11 epoch sec; seqNo:1891 2021-02-12T23:13:55Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.18 psi 2021-02-12T23:13:55Z- handleDepth()-Depth_Keller msg: 1613171635115 epoch sec; seqNo:1909; depth 28.17 2021-02-12T23:13:55Z- handleDvl() - RDI_Pathfinder msg: 1613171635.13 epoch sec; seqNo:807 2021-02-12T23:13:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.15 , 76.82 , 58.43 , 92.50 2021-02-12T23:13:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , -0.06 2021-02-12T23:13:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171635.14 epoch sec; seqNo:1909; 4056480.00 north; 590624.10 east 2021-02-12T23:13:55Z- AHRS_M2 msg: 1613171635.51 epoch sec; seqNo:1892 2021-02-12T23:13:55Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.18 psi 2021-02-12T23:13:55Z- handleDepth()-Depth_Keller msg: 1613171635517 epoch sec; seqNo:1910; depth 28.30 2021-02-12T23:13:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171635.54 epoch sec; seqNo:1910; 4056480.38 north; 590624.09 east 2021-02-12T23:13:55Z- AHRS_M2 msg: 1613171635.92 epoch sec; seqNo:1893 2021-02-12T23:13:55Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.18 psi 2021-02-12T23:13:55Z- handleDepth()-Depth_Keller msg: 1613171635930 epoch sec; seqNo:1911; depth 28.42 2021-02-12T23:13:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171636.03 epoch sec; seqNo:1911; 4056480.76 north; 590624.09 east 2021-02-12T23:13:56Z- AHRS_M2 msg: 1613171636.32 epoch sec; seqNo:1894 2021-02-12T23:13:56Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-12T23:13:56Z- handleDepth()-Depth_Keller msg: 1613171636324 epoch sec; seqNo:1912; depth 28.56 2021-02-12T23:13:56Z- handleDvl() - RDI_Pathfinder msg: 1613171636.34 epoch sec; seqNo:808 2021-02-12T23:13:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.15 , 76.00 , 58.84 , 91.42 2021-02-12T23:13:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , -0.03 2021-02-12T23:13:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171636.35 epoch sec; seqNo:1912; 4056480.76 north; 590624.09 east 2021-02-12T23:13:56Z- AHRS_M2 msg: 1613171636.72 epoch sec; seqNo:1895 2021-02-12T23:13:56Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-12T23:13:56Z- handleDepth()-Depth_Keller msg: 1613171636730 epoch sec; seqNo:1913; depth 28.68 2021-02-12T23:13:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171636.74 epoch sec; seqNo:1913; 4056481.13 north; 590624.09 east 2021-02-12T23:13:57Z- AHRS_M2 msg: 1613171637.13 epoch sec; seqNo:1896 2021-02-12T23:13:57Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-12T23:13:57Z- handleDepth()-Depth_Keller msg: 1613171637163 epoch sec; seqNo:1914; depth 28.80 2021-02-12T23:13:57Z- handleDvl() - RDI_Pathfinder msg: 1613171637.19 epoch sec; seqNo:809 2021-02-12T23:13:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.74 , 75.18 , 58.43 , 90.34 2021-02-12T23:13:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , -0.01 2021-02-12T23:13:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171637.22 epoch sec; seqNo:1914; 4056481.53 north; 590625.30 east 2021-02-12T23:13:57Z- AHRS_M2 msg: 1613171637.53 epoch sec; seqNo:1897 2021-02-12T23:13:57Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-12T23:13:57Z- handleDepth()-Depth_Keller msg: 1613171637541 epoch sec; seqNo:1915; depth 28.93 2021-02-12T23:13:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171637.55 epoch sec; seqNo:1915; 4056481.53 north; 590625.30 east 2021-02-12T23:13:57Z- AHRS_M2 msg: 1613171637.94 epoch sec; seqNo:1898 2021-02-12T23:13:57Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-12T23:13:57Z- handleDepth()-Depth_Keller msg: 1613171637940 epoch sec; seqNo:1916; depth 29.03 2021-02-12T23:13:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171637.95 epoch sec; seqNo:1916; 4056481.91 north; 590625.30 east 2021-02-12T23:13:58Z- AHRS_M2 msg: 1613171638.34 epoch sec; seqNo:1899 2021-02-12T23:13:58Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-12T23:13:58Z- handleDepth()-Depth_Keller msg: 1613171638344 epoch sec; seqNo:1917; depth 29.16 2021-02-12T23:13:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171638.37 epoch sec; seqNo:1917; 4056482.28 north; 590625.30 east 2021-02-12T23:13:58Z- AHRS_M2 msg: 1613171638.74 epoch sec; seqNo:1900 2021-02-12T23:13:58Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.19 psi 2021-02-12T23:13:58Z- handleDepth()-Depth_Keller msg: 1613171638748 epoch sec; seqNo:1918; depth 29.29 2021-02-12T23:13:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171638.76 epoch sec; seqNo:1918; 4056482.66 north; 590625.29 east 2021-02-12T23:13:59Z- AHRS_M2 msg: 1613171639.15 epoch sec; seqNo:1901 2021-02-12T23:13:59Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.18 psi 2021-02-12T23:13:59Z- handleDepth()-Depth_Keller msg: 1613171639154 epoch sec; seqNo:1919; depth 29.41 2021-02-12T23:13:59Z- handleDvl() - RDI_Pathfinder msg: 1613171639.17 epoch sec; seqNo:810 2021-02-12T23:13:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.52 , 74.78 , 58.02 , 88.18 2021-02-12T23:13:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , 0.01 2021-02-12T23:13:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171639.18 epoch sec; seqNo:1919; 4056483.04 north; 590625.29 east 2021-02-12T23:13:59Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-12T23:13:59Z- waiting for 1613171636.92, time is 1613171639.18 2021-02-12T23:13:59Z- LcmTrn::updateTrn() equating pose 1613171639.18 and meas times 1613171639.18 2021-02-12T23:13:59Z- LcmTrn::updateTrn() >>>> pose time 1613171639.18, meas time 1613171639.18 <<<< 2021-02-12T23:13:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171639.18 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:13:59Z- TerrainNav::motionUpdate() - currEstimate = (4056483, 590625.29, 29.41) 2021-02-12T23:13:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:13:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:13:59Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171639.18 sec.; No relevant navigation data available 2021-02-12T23:13:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:13:59Z- TNavBF::mmse selected from filter -1 2021-02-12T23:13:59Z- variances([0], [2], and psi_berg) = 790.640681 833.084591 0.000000 2021-02-12T23:13:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:13:59Z- LcmTrn::publishEstimates() - MLE : 4056456.54 590580.79 29.41 2021-02-12T23:13:59Z- LcmTrn::publishEstimates() - MMSE : 4056483.04 590625.29 29.41 2021-02-12T23:13:59Z- LcmTrn::publishEstimates() - COVAR: 790.64 833.08 1.03 0.00 2021-02-12T23:13:59Z- AHRS_M2 msg: 1613171639.55 epoch sec; seqNo:1902 2021-02-12T23:13:59Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.18 psi 2021-02-12T23:13:59Z- handleDepth()-Depth_Keller msg: 1613171639558 epoch sec; seqNo:1920; depth 29.54 2021-02-12T23:13:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171639.57 epoch sec; seqNo:1920; 4056483.04 north; 590625.29 east 2021-02-12T23:13:59Z- AHRS_M2 msg: 1613171639.97 epoch sec; seqNo:1903 2021-02-12T23:13:59Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.18 psi 2021-02-12T23:13:59Z- handleDepth()-Depth_Keller msg: 1613171639978 epoch sec; seqNo:1921; depth 29.67 2021-02-12T23:13:59Z- handleDvl() - RDI_Pathfinder msg: 1613171639.99 epoch sec; seqNo:811 2021-02-12T23:13:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.70 , 73.14 , 57.61 , 84.93 2021-02-12T23:13:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , 0.02 2021-02-12T23:14:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171640.00 epoch sec; seqNo:1921; 4056483.42 north; 590625.28 east 2021-02-12T23:14:00Z- AHRS_M2 msg: 1613171640.36 epoch sec; seqNo:1904 2021-02-12T23:14:00Z- AHRS_M2 msg: 0.58 phi; -0.25 theta; -0.17 psi 2021-02-12T23:14:00Z- handleDepth()-Depth_Keller msg: 1613171640364 epoch sec; seqNo:1922; depth 29.81 2021-02-12T23:14:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171640.38 epoch sec; seqNo:1922; 4056483.81 north; 590626.50 east 2021-02-12T23:14:00Z- AHRS_M2 msg: 1613171640.77 epoch sec; seqNo:1905 2021-02-12T23:14:00Z- AHRS_M2 msg: 0.58 phi; -0.22 theta; -0.16 psi 2021-02-12T23:14:00Z- handleDepth()-Depth_Keller msg: 1613171640769 epoch sec; seqNo:1923; depth 29.94 2021-02-12T23:14:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171640.78 epoch sec; seqNo:1923; 4056484.19 north; 590626.50 east 2021-02-12T23:14:01Z- AHRS_M2 msg: 1613171641.17 epoch sec; seqNo:1906 2021-02-12T23:14:01Z- AHRS_M2 msg: 0.57 phi; -0.18 theta; -0.15 psi 2021-02-12T23:14:01Z- handleDepth()-Depth_Keller msg: 1613171641172 epoch sec; seqNo:1924; depth 30.07 2021-02-12T23:14:01Z- handleDvl() - RDI_Pathfinder msg: 1613171641.19 epoch sec; seqNo:812 2021-02-12T23:14:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.29 , 70.69 , 58.84 , 74.37 2021-02-12T23:14:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , 0.00 2021-02-12T23:14:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171641.21 epoch sec; seqNo:1924; 4056484.57 north; 590626.49 east 2021-02-12T23:14:01Z- AHRS_M2 msg: 1613171641.57 epoch sec; seqNo:1907 2021-02-12T23:14:01Z- AHRS_M2 msg: 0.57 phi; -0.13 theta; -0.14 psi 2021-02-12T23:14:01Z- handleDepth()-Depth_Keller msg: 1613171641576 epoch sec; seqNo:1925; depth 30.22 2021-02-12T23:14:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171641.61 epoch sec; seqNo:1925; 4056484.57 north; 590626.49 east 2021-02-12T23:14:01Z- AHRS_M2 msg: 1613171641.98 epoch sec; seqNo:1908 2021-02-12T23:14:01Z- AHRS_M2 msg: 0.56 phi; -0.09 theta; -0.14 psi 2021-02-12T23:14:01Z- handleDepth()-Depth_Keller msg: 1613171641980 epoch sec; seqNo:1926; depth 30.30 2021-02-12T23:14:02Z- handleDvl() - RDI_Pathfinder msg: 1613171642.00 epoch sec; seqNo:813 2021-02-12T23:14:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.40 , 68.68 , 61.63 , 67.15 2021-02-12T23:14:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.08 , 0.00 2021-02-12T23:14:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171642.01 epoch sec; seqNo:1926; 4056484.95 north; 590626.49 east 2021-02-12T23:14:02Z- AHRS_M2 msg: 1613171642.39 epoch sec; seqNo:1909 2021-02-12T23:14:02Z- AHRS_M2 msg: 0.55 phi; -0.01 theta; -0.14 psi 2021-02-12T23:14:02Z- handleDepth()-Depth_Keller msg: 1613171642391 epoch sec; seqNo:1927; depth 30.39 2021-02-12T23:14:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171642.40 epoch sec; seqNo:1927; 4056485.33 north; 590626.48 east 2021-02-12T23:14:02Z- AHRS_M2 msg: 1613171642.79 epoch sec; seqNo:1910 2021-02-12T23:14:02Z- AHRS_M2 msg: 0.55 phi; 0.03 theta; -0.14 psi 2021-02-12T23:14:02Z- handleDepth()-Depth_Keller msg: 1613171642790 epoch sec; seqNo:1928; depth 30.48 2021-02-12T23:14:02Z- handleDvl() - RDI_Pathfinder msg: 1613171642.81 epoch sec; seqNo:814 2021-02-12T23:14:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.40 , 67.76 , 65.00 , 63.16 2021-02-12T23:14:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , 0.01 2021-02-12T23:14:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171642.82 epoch sec; seqNo:1928; 4056485.71 north; 590626.48 east 2021-02-12T23:14:03Z- AHRS_M2 msg: 1613171643.19 epoch sec; seqNo:1911 2021-02-12T23:14:03Z- AHRS_M2 msg: 0.54 phi; 0.07 theta; -0.14 psi 2021-02-12T23:14:03Z- handleDepth()-Depth_Keller msg: 1613171643193 epoch sec; seqNo:1929; depth 30.53 2021-02-12T23:14:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171643.20 epoch sec; seqNo:1929; 4056486.10 north; 590627.70 east 2021-02-12T23:14:03Z- AHRS_M2 msg: 1613171643.59 epoch sec; seqNo:1912 2021-02-12T23:14:03Z- AHRS_M2 msg: 0.53 phi; 0.11 theta; -0.14 psi 2021-02-12T23:14:03Z- handleDepth()-Depth_Keller msg: 1613171643600 epoch sec; seqNo:1930; depth 30.57 2021-02-12T23:14:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171643.61 epoch sec; seqNo:1930; 4056486.48 north; 590627.69 east 2021-02-12T23:14:04Z- AHRS_M2 msg: 1613171644.00 epoch sec; seqNo:1913 2021-02-12T23:14:04Z- AHRS_M2 msg: 0.53 phi; 0.14 theta; -0.13 psi 2021-02-12T23:14:04Z- handleDepth()-Depth_Keller msg: 1613171644002 epoch sec; seqNo:1931; depth 30.60 2021-02-12T23:14:04Z- handleDvl() - RDI_Pathfinder msg: 1613171644.01 epoch sec; seqNo:815 2021-02-12T23:14:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.70 , 68.37 , 68.98 , 60.40 2021-02-12T23:14:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , 0.03 2021-02-12T23:14:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171644.02 epoch sec; seqNo:1931; 4056486.48 north; 590627.69 east 2021-02-12T23:14:04Z- AHRS_M2 msg: 1613171644.40 epoch sec; seqNo:1914 2021-02-12T23:14:04Z- AHRS_M2 msg: 0.52 phi; 0.17 theta; -0.13 psi 2021-02-12T23:14:04Z- LcmTrn::handleMessages() - handling 42 messages took 0.60 ms and 130 calls... 2021-02-12T23:14:04Z- waiting for 1613171642.18, time is 1613171644.40 2021-02-12T23:14:04Z- LcmTrn::updateTrn() >>>> pose time 1613171644.40, meas time 1613171644.01 <<<< 2021-02-12T23:14:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171644.01 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:14:04Z- TerrainNav::motionUpdate() - currEstimate = (4056486, 590627.69, 30.60) 2021-02-12T23:14:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:14:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:14:04Z- TNavBF::Projecting Measurements 2021-02-12T23:14:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:14:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:14:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:14:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:14:04Z- TNavFilter::4 good beams. 2021-02-12T23:14:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:14:04Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:14:04Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:14:04Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:14:04Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:14:04Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:14:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:14:04Z- TNavBF::mmse selected from filter -1 2021-02-12T23:14:04Z- TerrainMapOctree: (vehN, vehE ) = (4056486.22, 590627.51). 2021-02-12T23:14:04Z- TerrainMapOctree: Min Distance = 5984.64. 2021-02-12T23:14:04Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:14:04Z- There is no specific good beam that all particles have in common. 2021-02-12T23:14:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:14:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.139 ExpectedMeasDiff: nan currMeas.covariance: 5.90 2021-02-12T23:14:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:14:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 7.48 2021-02-12T23:14:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:14:04Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 7.61 2021-02-12T23:14:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:14:04Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 5.84 2021-02-12T23:14:04Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:14:04Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:14:04Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:14:04Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:14:04Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:14:04Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:14:04Z- Filter 0 No beams used in subcloud update 2021-02-12T23:14:04Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:14:04Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:14:04Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 790.5 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:14:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171644.01 sec. 2021-02-12T23:14:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:14:04Z- TNavBF::mmse selected from filter -1 2021-02-12T23:14:04Z- variances([0], [2], and psi_berg) = 790.406472 833.084180 0.000000 2021-02-12T23:14:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:14:04Z- LcmTrn::publishEstimates() - MLE : 4056459.98 590583.19 30.60 2021-02-12T23:14:04Z- LcmTrn::publishEstimates() - MMSE : 4056486.48 590627.69 30.60 2021-02-12T23:14:04Z- LcmTrn::publishEstimates() - COVAR: 790.41 833.08 0.79 0.00 2021-02-12T23:14:04Z- handleDepth()-Depth_Keller msg: 1613171644404 epoch sec; seqNo:1932; depth 30.63 2021-02-12T23:14:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171644.42 epoch sec; seqNo:1932; 4056486.86 north; 590627.69 east 2021-02-12T23:14:04Z- AHRS_M2 msg: 1613171644.80 epoch sec; seqNo:1915 2021-02-12T23:14:04Z- AHRS_M2 msg: 0.52 phi; 0.19 theta; -0.13 psi 2021-02-12T23:14:04Z- handleDepth()-Depth_Keller msg: 1613171644808 epoch sec; seqNo:1933; depth 30.63 2021-02-12T23:14:04Z- handleDvl() - RDI_Pathfinder msg: 1613171644.82 epoch sec; seqNo:816 2021-02-12T23:14:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.93 , 69.29 , 73.58 , 58.87 2021-02-12T23:14:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.02 2021-02-12T23:14:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171644.84 epoch sec; seqNo:1933; 4056487.23 north; 590627.69 east 2021-02-12T23:14:05Z- AHRS_M2 msg: 1613171645.21 epoch sec; seqNo:1916 2021-02-12T23:14:05Z- AHRS_M2 msg: 0.52 phi; 0.22 theta; -0.13 psi 2021-02-12T23:14:05Z- handleDepth()-Depth_Keller msg: 1613171645213 epoch sec; seqNo:1934; depth 30.63 2021-02-12T23:14:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171645.23 epoch sec; seqNo:1934; 4056487.61 north; 590627.68 east 2021-02-12T23:14:05Z- AHRS_M2 msg: 1613171645.62 epoch sec; seqNo:1917 2021-02-12T23:14:05Z- AHRS_M2 msg: 0.52 phi; 0.24 theta; -0.13 psi 2021-02-12T23:14:05Z- handleDepth()-Depth_Keller msg: 1613171645629 epoch sec; seqNo:1935; depth 30.59 2021-02-12T23:14:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171645.66 epoch sec; seqNo:1935; 4056487.99 north; 590627.68 east 2021-02-12T23:14:06Z- AHRS_M2 msg: 1613171646.02 epoch sec; seqNo:1918 2021-02-12T23:14:06Z- AHRS_M2 msg: 0.52 phi; 0.26 theta; -0.12 psi 2021-02-12T23:14:06Z- handleDepth()-Depth_Keller msg: 1613171646030 epoch sec; seqNo:1936; depth 30.55 2021-02-12T23:14:06Z- handleDvl() - RDI_Pathfinder msg: 1613171646.04 epoch sec; seqNo:817 2021-02-12T23:14:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.86 , 69.60 , 77.27 , 57.95 2021-02-12T23:14:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.01 2021-02-12T23:14:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171646.05 epoch sec; seqNo:1936; 4056488.01 north; 590628.90 east 2021-02-12T23:14:06Z- AHRS_M2 msg: 1613171646.42 epoch sec; seqNo:1919 2021-02-12T23:14:06Z- AHRS_M2 msg: 0.52 phi; 0.27 theta; -0.12 psi 2021-02-12T23:14:06Z- handleDepth()-Depth_Keller msg: 1613171646424 epoch sec; seqNo:1937; depth 30.50 2021-02-12T23:14:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171646.43 epoch sec; seqNo:1937; 4056488.38 north; 590628.89 east 2021-02-12T23:14:06Z- AHRS_M2 msg: 1613171646.82 epoch sec; seqNo:1920 2021-02-12T23:14:06Z- AHRS_M2 msg: 0.53 phi; 0.29 theta; -0.12 psi 2021-02-12T23:14:06Z- handleDepth()-Depth_Keller msg: 1613171646828 epoch sec; seqNo:1938; depth 30.44 2021-02-12T23:14:06Z- handleDvl() - RDI_Pathfinder msg: 1613171646.84 epoch sec; seqNo:818 2021-02-12T23:14:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.08 , 70.21 , 79.41 , 58.26 2021-02-12T23:14:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.07 , -0.06 2021-02-12T23:14:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171646.88 epoch sec; seqNo:1938; 4056488.76 north; 590628.89 east 2021-02-12T23:14:07Z- AHRS_M2 msg: 1613171647.23 epoch sec; seqNo:1921 2021-02-12T23:14:07Z- AHRS_M2 msg: 0.53 phi; 0.30 theta; -0.12 psi 2021-02-12T23:14:07Z- handleDepth()-Depth_Keller msg: 1613171647236 epoch sec; seqNo:1939; depth 30.39 2021-02-12T23:14:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171647.25 epoch sec; seqNo:1939; 4056489.14 north; 590628.89 east 2021-02-12T23:14:07Z- AHRS_M2 msg: 1613171647.65 epoch sec; seqNo:1922 2021-02-12T23:14:07Z- AHRS_M2 msg: 0.54 phi; 0.31 theta; -0.11 psi 2021-02-12T23:14:07Z- handleDepth()-Depth_Keller msg: 1613171647658 epoch sec; seqNo:1940; depth 30.34 2021-02-12T23:14:07Z- handleDvl() - RDI_Pathfinder msg: 1613171647.68 epoch sec; seqNo:819 2021-02-12T23:14:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.37 , 70.87 , 83.17 , 57.60 2021-02-12T23:14:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.07 , -0.13 2021-02-12T23:14:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171647.69 epoch sec; seqNo:1940; 4056489.14 north; 590628.89 east 2021-02-12T23:14:08Z- AHRS_M2 msg: 1613171648.04 epoch sec; seqNo:1923 2021-02-12T23:14:08Z- AHRS_M2 msg: 0.55 phi; 0.32 theta; -0.11 psi 2021-02-12T23:14:08Z- handleDepth()-Depth_Keller msg: 1613171648039 epoch sec; seqNo:1941; depth 30.25 2021-02-12T23:14:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171648.05 epoch sec; seqNo:1941; 4056489.52 north; 590628.88 east 2021-02-12T23:14:08Z- AHRS_M2 msg: 1613171648.44 epoch sec; seqNo:1924 2021-02-12T23:14:08Z- AHRS_M2 msg: 0.55 phi; 0.33 theta; -0.11 psi 2021-02-12T23:14:08Z- handleDepth()-Depth_Keller msg: 1613171648449 epoch sec; seqNo:1942; depth 30.18 2021-02-12T23:14:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171648.46 epoch sec; seqNo:1942; 4056489.90 north; 590628.88 east 2021-02-12T23:14:08Z- AHRS_M2 msg: 1613171648.85 epoch sec; seqNo:1925 2021-02-12T23:14:08Z- AHRS_M2 msg: 0.56 phi; 0.33 theta; -0.11 psi 2021-02-12T23:14:08Z- handleDepth()-Depth_Keller msg: 1613171648853 epoch sec; seqNo:1943; depth 30.09 2021-02-12T23:14:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171648.87 epoch sec; seqNo:1943; 4056489.91 north; 590630.10 east 2021-02-12T23:14:09Z- AHRS_M2 msg: 1613171649.25 epoch sec; seqNo:1926 2021-02-12T23:14:09Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.11 psi 2021-02-12T23:14:09Z- handleDepth()-Depth_Keller msg: 1613171649254 epoch sec; seqNo:1944; depth 29.99 2021-02-12T23:14:09Z- handleDvl() - RDI_Pathfinder msg: 1613171649.28 epoch sec; seqNo:820 2021-02-12T23:14:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.38 , 71.51 , 0.00 , 58.02 2021-02-12T23:14:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.05 , -0.16 2021-02-12T23:14:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171649.29 epoch sec; seqNo:1944; 4056490.29 north; 590630.10 east 2021-02-12T23:14:09Z- LcmTrn::handleMessages() - handling 43 messages took 0.70 ms and 129 calls... 2021-02-12T23:14:09Z- waiting for 1613171647.40, time is 1613171649.29 2021-02-12T23:14:09Z- LcmTrn::updateTrn() equating pose 1613171649.29 and meas times 1613171649.29 2021-02-12T23:14:09Z- LcmTrn::updateTrn() >>>> pose time 1613171649.29, meas time 1613171649.29 <<<< 2021-02-12T23:14:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171649.29 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:14:09Z- TerrainNav::motionUpdate() - currEstimate = (4056490, 590630.10, 29.99) 2021-02-12T23:14:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:14:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:14:09Z- TNavBF::Projecting Measurements 2021-02-12T23:14:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:14:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:14:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:14:09Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:14:09Z- TNavFilter::3 good beams. 2021-02-12T23:14:09Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:14:09Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:14:09Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:14:09Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:14:09Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:14:09Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:14:09Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:14:09Z- TNavBF::mmse selected from filter -1 2021-02-12T23:14:09Z- TerrainMapOctree: (vehN, vehE ) = (4056490.29, 590630.10). 2021-02-12T23:14:09Z- TerrainMapOctree: Min Distance = 5979.84. 2021-02-12T23:14:09Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:14:09Z- There is no specific good beam that all particles have in common. 2021-02-12T23:14:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:14:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-12T23:14:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:14:09Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 8.18 2021-02-12T23:14:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:14:09Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 5.39 2021-02-12T23:14:09Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:14:09Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:14:09Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:14:09Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:14:09Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:14:09Z- Filter 0 No beams used in subcloud update 2021-02-12T23:14:09Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:14:09Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:14:09Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 790.2 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:14:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171649.29 sec. 2021-02-12T23:14:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:14:09Z- TNavBF::mmse selected from filter -1 2021-02-12T23:14:09Z- variances([0], [2], and psi_berg) = 790.210524 833.083838 0.000000 2021-02-12T23:14:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:14:09Z- LcmTrn::publishEstimates() - MLE : 4056463.79 590585.60 29.99 2021-02-12T23:14:09Z- LcmTrn::publishEstimates() - MMSE : 4056490.29 590630.10 29.99 2021-02-12T23:14:09Z- LcmTrn::publishEstimates() - COVAR: 790.21 833.08 0.75 0.00 2021-02-12T23:14:09Z- AHRS_M2 msg: 1613171649.65 epoch sec; seqNo:1927 2021-02-12T23:14:09Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.11 psi 2021-02-12T23:14:09Z- handleDepth()-Depth_Keller msg: 1613171649659 epoch sec; seqNo:1945; depth 29.90 2021-02-12T23:14:09Z- handleDvl() - RDI_Pathfinder msg: 1613171649.67 epoch sec; seqNo:821 2021-02-12T23:14:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.23 , 72.67 , 0.00 , 57.03 2021-02-12T23:14:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.05 , -0.14 2021-02-12T23:14:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171649.68 epoch sec; seqNo:1945; 4056490.67 north; 590630.09 east 2021-02-12T23:14:10Z- AHRS_M2 msg: 1613171650.06 epoch sec; seqNo:1928 2021-02-12T23:14:10Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.11 psi 2021-02-12T23:14:10Z- handleDepth()-Depth_Keller msg: 1613171650061 epoch sec; seqNo:1946; depth 29.79 2021-02-12T23:14:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171650.07 epoch sec; seqNo:1946; 4056490.67 north; 590630.09 east 2021-02-12T23:14:10Z- AHRS_M2 msg: 1613171650.46 epoch sec; seqNo:1929 2021-02-12T23:14:10Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.11 psi 2021-02-12T23:14:10Z- handleDepth()-Depth_Keller msg: 1613171650464 epoch sec; seqNo:1947; depth 29.70 2021-02-12T23:14:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171650.48 epoch sec; seqNo:1947; 4056491.05 north; 590630.09 east 2021-02-12T23:14:10Z- AHRS_M2 msg: 1613171650.86 epoch sec; seqNo:1930 2021-02-12T23:14:10Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.12 psi 2021-02-12T23:14:10Z- handleDepth()-Depth_Keller msg: 1613171650868 epoch sec; seqNo:1948; depth 29.58 2021-02-12T23:14:10Z- handleDvl() - RDI_Pathfinder msg: 1613171650.89 epoch sec; seqNo:822 2021-02-12T23:14:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.54 , 72.97 , 0.00 , 57.95 2021-02-12T23:14:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.09 , -0.09 2021-02-12T23:14:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171650.92 epoch sec; seqNo:1948; 4056491.43 north; 590630.08 east 2021-02-12T23:14:11Z- AHRS_M2 msg: 1613171651.27 epoch sec; seqNo:1931 2021-02-12T23:14:11Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.12 psi 2021-02-12T23:14:11Z- handleDepth()-Depth_Keller msg: 1613171651274 epoch sec; seqNo:1949; depth 29.47 2021-02-12T23:14:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171651.29 epoch sec; seqNo:1949; 4056491.43 north; 590630.08 east 2021-02-12T23:14:11Z- AHRS_M2 msg: 1613171651.67 epoch sec; seqNo:1932 2021-02-12T23:14:11Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.12 psi 2021-02-12T23:14:11Z- handleDepth()-Depth_Keller msg: 1613171651686 epoch sec; seqNo:1950; depth 29.36 2021-02-12T23:14:11Z- handleDvl() - RDI_Pathfinder msg: 1613171651.70 epoch sec; seqNo:823 2021-02-12T23:14:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.54 , 73.28 , 0.00 , 57.95 2021-02-12T23:14:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.11 , -0.03 2021-02-12T23:14:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171651.71 epoch sec; seqNo:1950; 4056491.81 north; 590630.08 east 2021-02-12T23:14:12Z- AHRS_M2 msg: 1613171652.08 epoch sec; seqNo:1933 2021-02-12T23:14:12Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.12 psi 2021-02-12T23:14:12Z- handleDepth()-Depth_Keller msg: 1613171652085 epoch sec; seqNo:1951; depth 29.24 2021-02-12T23:14:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171652.10 epoch sec; seqNo:1951; 4056492.20 north; 590631.30 east 2021-02-12T23:14:12Z- AHRS_M2 msg: 1613171652.48 epoch sec; seqNo:1934 2021-02-12T23:14:12Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.12 psi 2021-02-12T23:14:12Z- handleDepth()-Depth_Keller msg: 1613171652492 epoch sec; seqNo:1952; depth 29.12 2021-02-12T23:14:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171652.50 epoch sec; seqNo:1952; 4056492.58 north; 590631.29 east 2021-02-12T23:14:12Z- AHRS_M2 msg: 1613171652.88 epoch sec; seqNo:1935 2021-02-12T23:14:12Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.12 psi 2021-02-12T23:14:12Z- handleDepth()-Depth_Keller msg: 1613171652890 epoch sec; seqNo:1953; depth 29.02 2021-02-12T23:14:12Z- handleDvl() - RDI_Pathfinder msg: 1613171652.90 epoch sec; seqNo:824 2021-02-12T23:14:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.15 , 73.28 , 0.00 , 58.56 2021-02-12T23:14:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.11 , 0.01 2021-02-12T23:14:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171652.91 epoch sec; seqNo:1953; 4056492.58 north; 590631.29 east 2021-02-12T23:14:13Z- AHRS_M2 msg: 1613171653.29 epoch sec; seqNo:1936 2021-02-12T23:14:13Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.12 psi 2021-02-12T23:14:13Z- handleDepth()-Depth_Keller msg: 1613171653300 epoch sec; seqNo:1954; depth 28.91 2021-02-12T23:14:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171653.34 epoch sec; seqNo:1954; 4056492.96 north; 590631.29 east 2021-02-12T23:14:13Z- AHRS_M2 msg: 1613171653.69 epoch sec; seqNo:1937 2021-02-12T23:14:13Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.12 psi 2021-02-12T23:14:13Z- handleDepth()-Depth_Keller msg: 1613171653696 epoch sec; seqNo:1955; depth 28.78 2021-02-12T23:14:13Z- handleDvl() - RDI_Pathfinder msg: 1613171653.71 epoch sec; seqNo:825 2021-02-12T23:14:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.45 , 73.58 , 0.00 , 58.25 2021-02-12T23:14:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.10 , 0.04 2021-02-12T23:14:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171653.72 epoch sec; seqNo:1955; 4056493.34 north; 590631.28 east 2021-02-12T23:14:14Z- AHRS_M2 msg: 1613171654.10 epoch sec; seqNo:1938 2021-02-12T23:14:14Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.12 psi 2021-02-12T23:14:14Z- handleDepth()-Depth_Keller msg: 1613171654109 epoch sec; seqNo:1956; depth 28.67 2021-02-12T23:14:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171654.12 epoch sec; seqNo:1956; 4056493.71 north; 590631.28 east 2021-02-12T23:14:14Z- AHRS_M2 msg: 1613171654.50 epoch sec; seqNo:1939 2021-02-12T23:14:14Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.11 psi 2021-02-12T23:14:14Z- handleDepth()-Depth_Keller msg: 1613171654504 epoch sec; seqNo:1957; depth 28.55 2021-02-12T23:14:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171654.51 epoch sec; seqNo:1957; 4056493.71 north; 590631.28 east 2021-02-12T23:14:14Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-12T23:14:14Z- waiting for 1613171652.29, time is 1613171654.51 2021-02-12T23:14:14Z- LcmTrn::updateTrn() >>>> pose time 1613171654.51, meas time 1613171653.71 <<<< 2021-02-12T23:14:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171653.71 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:14:14Z- TerrainNav::motionUpdate() - currEstimate = (4056494, 590631.28, 28.55) 2021-02-12T23:14:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:14:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:14:14Z- TNavBF::Projecting Measurements 2021-02-12T23:14:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:14:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:14:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:14:14Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:14:14Z- TNavFilter::3 good beams. 2021-02-12T23:14:14Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:14:14Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:14:14Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:14:14Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:14:14Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:14:14Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:14:14Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:14:14Z- TNavBF::mmse selected from filter -1 2021-02-12T23:14:14Z- TerrainMapOctree: (vehN, vehE ) = (4056493.19, 590631.10). 2021-02-12T23:14:14Z- TerrainMapOctree: Min Distance = 5976.93. 2021-02-12T23:14:14Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:14:14Z- There is no specific good beam that all particles have in common. 2021-02-12T23:14:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:14:14Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 7.28 2021-02-12T23:14:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:14:14Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 8.66 2021-02-12T23:14:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:14:14Z- Old Depth Bias: 0.00 DepthBiasCov: 0.76 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 5.43 2021-02-12T23:14:14Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:14:14Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:14:14Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:14:14Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:14:14Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:14:14Z- Filter 0 No beams used in subcloud update 2021-02-12T23:14:14Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:14:14Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:14:14Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 790.0 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:14:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171653.71 sec. 2021-02-12T23:14:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:14:14Z- TNavBF::mmse selected from filter -1 2021-02-12T23:14:14Z- variances([0], [2], and psi_berg) = 789.981169 833.083439 0.000000 2021-02-12T23:14:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:14:14Z- LcmTrn::publishEstimates() - MLE : 4056467.21 590586.78 28.55 2021-02-12T23:14:14Z- LcmTrn::publishEstimates() - MMSE : 4056493.71 590631.28 28.55 2021-02-12T23:14:14Z- LcmTrn::publishEstimates() - COVAR: 789.98 833.08 0.72 0.00 2021-02-12T23:14:14Z- AHRS_M2 msg: 1613171654.90 epoch sec; seqNo:1940 2021-02-12T23:14:14Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.10 psi 2021-02-12T23:14:14Z- handleDepth()-Depth_Keller msg: 1613171654913 epoch sec; seqNo:1958; depth 28.45 2021-02-12T23:14:14Z- handleDvl() - RDI_Pathfinder msg: 1613171654.92 epoch sec; seqNo:826 2021-02-12T23:14:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.06 , 73.58 , 0.00 , 58.87 2021-02-12T23:14:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , 0.06 2021-02-12T23:14:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171654.93 epoch sec; seqNo:1958; 4056494.09 north; 590631.28 east 2021-02-12T23:14:15Z- AHRS_M2 msg: 1613171655.31 epoch sec; seqNo:1941 2021-02-12T23:14:15Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.10 psi 2021-02-12T23:14:15Z- handleDepth()-Depth_Keller msg: 1613171655313 epoch sec; seqNo:1959; depth 28.33 2021-02-12T23:14:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171655.32 epoch sec; seqNo:1959; 4056494.48 north; 590632.49 east 2021-02-12T23:14:15Z- AHRS_M2 msg: 1613171655.71 epoch sec; seqNo:1942 2021-02-12T23:14:15Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.09 psi 2021-02-12T23:14:15Z- handleDepth()-Depth_Keller msg: 1613171655726 epoch sec; seqNo:1960; depth 28.21 2021-02-12T23:14:15Z- handleDvl() - RDI_Pathfinder msg: 1613171655.74 epoch sec; seqNo:827 2021-02-12T23:14:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.29 , 73.28 , 0.00 , 59.18 2021-02-12T23:14:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , 0.07 2021-02-12T23:14:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171655.75 epoch sec; seqNo:1960; 4056494.86 north; 590632.49 east 2021-02-12T23:14:16Z- AHRS_M2 msg: 1613171656.12 epoch sec; seqNo:1943 2021-02-12T23:14:16Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.08 psi 2021-02-12T23:14:16Z- handleDepth()-Depth_Keller msg: 1613171656123 epoch sec; seqNo:1961; depth 28.09 2021-02-12T23:14:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171656.14 epoch sec; seqNo:1961; 4056495.24 north; 590632.49 east 2021-02-12T23:14:16Z- AHRS_M2 msg: 1613171656.52 epoch sec; seqNo:1944 2021-02-12T23:14:16Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.07 psi 2021-02-12T23:14:16Z- handleDepth()-Depth_Keller msg: 1613171656529 epoch sec; seqNo:1962; depth 27.97 2021-02-12T23:14:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171656.54 epoch sec; seqNo:1962; 4056495.24 north; 590632.49 east 2021-02-12T23:14:16Z- AHRS_M2 msg: 1613171656.92 epoch sec; seqNo:1945 2021-02-12T23:14:16Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.07 psi 2021-02-12T23:14:16Z- handleDepth()-Depth_Keller msg: 1613171656930 epoch sec; seqNo:1963; depth 27.85 2021-02-12T23:14:16Z- handleDvl() - RDI_Pathfinder msg: 1613171656.94 epoch sec; seqNo:828 2021-02-12T23:14:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.23 , 73.14 , 0.00 , 59.55 2021-02-12T23:14:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , 0.08 2021-02-12T23:14:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171656.95 epoch sec; seqNo:1963; 4056495.62 north; 590632.48 east 2021-02-12T23:14:17Z- AHRS_M2 msg: 1613171657.33 epoch sec; seqNo:1946 2021-02-12T23:14:17Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.07 psi 2021-02-12T23:14:17Z- handleDepth()-Depth_Keller msg: 1613171657332 epoch sec; seqNo:1964; depth 27.72 2021-02-12T23:14:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171657.36 epoch sec; seqNo:1964; 4056496.00 north; 590632.48 east 2021-02-12T23:14:17Z- AHRS_M2 msg: 1613171657.73 epoch sec; seqNo:1947 2021-02-12T23:14:17Z- AHRS_M2 msg: 0.65 phi; 0.37 theta; -0.07 psi 2021-02-12T23:14:17Z- handleDepth()-Depth_Keller msg: 1613171657739 epoch sec; seqNo:1965; depth 27.61 2021-02-12T23:14:17Z- handleDvl() - RDI_Pathfinder msg: 1613171657.75 epoch sec; seqNo:829 2021-02-12T23:14:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.56 , 73.14 , 0.00 , 59.55 2021-02-12T23:14:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , 0.07 2021-02-12T23:14:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171657.77 epoch sec; seqNo:1965; 4056496.38 north; 590632.47 east 2021-02-12T23:14:18Z- AHRS_M2 msg: 1613171658.14 epoch sec; seqNo:1948 2021-02-12T23:14:18Z- AHRS_M2 msg: 0.65 phi; 0.37 theta; -0.08 psi 2021-02-12T23:14:18Z- handleDepth()-Depth_Keller msg: 1613171658153 epoch sec; seqNo:1966; depth 27.47 2021-02-12T23:14:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171658.16 epoch sec; seqNo:1966; 4056496.39 north; 590633.69 east 2021-02-12T23:14:18Z- AHRS_M2 msg: 1613171658.54 epoch sec; seqNo:1949 2021-02-12T23:14:18Z- AHRS_M2 msg: 0.65 phi; 0.37 theta; -0.09 psi 2021-02-12T23:14:18Z- handleDepth()-Depth_Keller msg: 1613171658553 epoch sec; seqNo:1967; depth 27.33 2021-02-12T23:14:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171658.57 epoch sec; seqNo:1967; 4056496.77 north; 590633.69 east 2021-02-12T23:14:18Z- AHRS_M2 msg: 1613171658.94 epoch sec; seqNo:1950 2021-02-12T23:14:18Z- AHRS_M2 msg: 0.65 phi; 0.38 theta; -0.10 psi 2021-02-12T23:14:18Z- handleDepth()-Depth_Keller msg: 1613171658956 epoch sec; seqNo:1968; depth 27.21 2021-02-12T23:14:18Z- handleDvl() - RDI_Pathfinder msg: 1613171658.98 epoch sec; seqNo:830 2021-02-12T23:14:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.23 , 75.09 , 0.00 , 59.23 2021-02-12T23:14:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , 0.04 2021-02-12T23:14:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171658.99 epoch sec; seqNo:1968; 4056497.15 north; 590633.69 east 2021-02-12T23:14:19Z- AHRS_M2 msg: 1613171659.36 epoch sec; seqNo:1951 2021-02-12T23:14:19Z- AHRS_M2 msg: 0.65 phi; 0.38 theta; -0.12 psi 2021-02-12T23:14:19Z- handleDepth()-Depth_Keller msg: 1613171659362 epoch sec; seqNo:1969; depth 27.07 2021-02-12T23:14:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171659.39 epoch sec; seqNo:1969; 4056497.53 north; 590633.68 east 2021-02-12T23:14:19Z- LcmTrn::handleMessages() - handling 41 messages took 0.65 ms and 128 calls... 2021-02-12T23:14:19Z- waiting for 1613171657.51, time is 1613171659.39 2021-02-12T23:14:19Z- LcmTrn::updateTrn() >>>> pose time 1613171659.39, meas time 1613171658.98 <<<< 2021-02-12T23:14:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171658.98 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:14:19Z- TerrainNav::motionUpdate() - currEstimate = (4056498, 590633.68, 27.07) 2021-02-12T23:14:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:14:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:14:19Z- TNavBF::Projecting Measurements 2021-02-12T23:14:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:14:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:14:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:14:19Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:14:19Z- TNavFilter::3 good beams. 2021-02-12T23:14:19Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:14:19Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:14:19Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:14:19Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:14:19Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:14:19Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:14:19Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:14:19Z- TNavBF::mmse selected from filter -1 2021-02-12T23:14:19Z- TerrainMapOctree: (vehN, vehE ) = (4056497.21, 590633.48). 2021-02-12T23:14:19Z- TerrainMapOctree: Min Distance = 5972.29. 2021-02-12T23:14:19Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:14:19Z- There is no specific good beam that all particles have in common. 2021-02-12T23:14:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:14:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 7.89 2021-02-12T23:14:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:14:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 9.02 2021-02-12T23:14:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:14:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 5.61 2021-02-12T23:14:19Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:14:19Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:14:19Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:14:19Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:14:19Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:14:19Z- Filter 0 No beams used in subcloud update 2021-02-12T23:14:19Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:14:19Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:14:19Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 789.8 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:14:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171658.98 sec. 2021-02-12T23:14:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:14:19Z- TNavBF::mmse selected from filter -1 2021-02-12T23:14:19Z- variances([0], [2], and psi_berg) = 789.738953 833.083019 0.000000 2021-02-12T23:14:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:14:19Z- LcmTrn::publishEstimates() - MLE : 4056472.03 590589.18 27.07 2021-02-12T23:14:19Z- LcmTrn::publishEstimates() - MMSE : 4056497.53 590633.68 27.07 2021-02-12T23:14:19Z- LcmTrn::publishEstimates() - COVAR: 789.74 833.08 0.73 0.00 2021-02-12T23:14:19Z- AHRS_M2 msg: 1613171659.76 epoch sec; seqNo:1952 2021-02-12T23:14:19Z- AHRS_M2 msg: 0.65 phi; 0.38 theta; -0.13 psi 2021-02-12T23:14:19Z- handleDepth()-Depth_Keller msg: 1613171659760 epoch sec; seqNo:1970; depth 26.95 2021-02-12T23:14:19Z- handleDvl() - RDI_Pathfinder msg: 1613171659.77 epoch sec; seqNo:831 2021-02-12T23:14:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.91 , 77.03 , 0.00 , 60.20 2021-02-12T23:14:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.07 , -0.01 2021-02-12T23:14:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171659.78 epoch sec; seqNo:1970; 4056497.53 north; 590633.68 east 2021-02-12T23:14:20Z- AHRS_M2 msg: 1613171660.15 epoch sec; seqNo:1953 2021-02-12T23:14:20Z- AHRS_M2 msg: 0.65 phi; 0.38 theta; -0.14 psi 2021-02-12T23:14:20Z- handleDepth()-Depth_Keller msg: 1613171660169 epoch sec; seqNo:1971; depth 26.83 2021-02-12T23:14:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171660.18 epoch sec; seqNo:1971; 4056497.92 north; 590634.90 east 2021-02-12T23:14:20Z- AHRS_M2 msg: 1613171660.56 epoch sec; seqNo:1954 2021-02-12T23:14:20Z- AHRS_M2 msg: 0.65 phi; 0.38 theta; -0.14 psi 2021-02-12T23:14:20Z- handleDepth()-Depth_Keller msg: 1613171660570 epoch sec; seqNo:1972; depth 26.69 2021-02-12T23:14:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171660.59 epoch sec; seqNo:1972; 4056498.30 north; 590634.90 east 2021-02-12T23:14:20Z- AHRS_M2 msg: 1613171660.96 epoch sec; seqNo:1955 2021-02-12T23:14:20Z- AHRS_M2 msg: 0.65 phi; 0.38 theta; -0.15 psi 2021-02-12T23:14:20Z- handleDepth()-Depth_Keller msg: 1613171660968 epoch sec; seqNo:1973; depth 26.55 2021-02-12T23:14:20Z- handleDvl() - RDI_Pathfinder msg: 1613171660.98 epoch sec; seqNo:832 2021-02-12T23:14:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 69.91 , 0.00 , 0.00 , 60.85 2021-02-12T23:14:20Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:14:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171661.00 epoch sec; seqNo:1973; 4056498.30 north; 590634.90 east 2021-02-12T23:14:21Z- AHRS_M2 msg: 1613171661.37 epoch sec; seqNo:1956 2021-02-12T23:14:21Z- AHRS_M2 msg: 0.66 phi; 0.38 theta; -0.15 psi 2021-02-12T23:14:21Z- handleDepth()-Depth_Keller msg: 1613171661378 epoch sec; seqNo:1974; depth 26.43 2021-02-12T23:14:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171661.39 epoch sec; seqNo:1974; 4056498.68 north; 590634.89 east 2021-02-12T23:14:21Z- AHRS_M2 msg: 1613171661.77 epoch sec; seqNo:1957 2021-02-12T23:14:21Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.15 psi 2021-02-12T23:14:21Z- handleDepth()-Depth_Keller msg: 1613171661781 epoch sec; seqNo:1975; depth 26.30 2021-02-12T23:14:21Z- handleDvl() - RDI_Pathfinder msg: 1613171661.80 epoch sec; seqNo:833 2021-02-12T23:14:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 69.84 , 0.00 , 0.00 , 60.90 2021-02-12T23:14:21Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:14:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171661.81 epoch sec; seqNo:1975; 4056499.06 north; 590634.89 east 2021-02-12T23:14:22Z- AHRS_M2 msg: 1613171662.17 epoch sec; seqNo:1958 2021-02-12T23:14:22Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.15 psi 2021-02-12T23:14:22Z- handleDepth()-Depth_Keller msg: 1613171662180 epoch sec; seqNo:1976; depth 26.17 2021-02-12T23:14:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171662.19 epoch sec; seqNo:1976; 4056499.07 north; 590636.11 east 2021-02-12T23:14:22Z- AHRS_M2 msg: 1613171662.58 epoch sec; seqNo:1959 2021-02-12T23:14:22Z- AHRS_M2 msg: 0.67 phi; 0.37 theta; -0.15 psi 2021-02-12T23:14:22Z- handleDepth()-Depth_Keller msg: 1613171662584 epoch sec; seqNo:1977; depth 26.03 2021-02-12T23:14:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171662.59 epoch sec; seqNo:1977; 4056499.45 north; 590636.10 east 2021-02-12T23:14:22Z- AHRS_M2 msg: 1613171662.98 epoch sec; seqNo:1960 2021-02-12T23:14:22Z- AHRS_M2 msg: 0.67 phi; 0.36 theta; -0.15 psi 2021-02-12T23:14:23Z- handleDepth()-Depth_Keller msg: 1613171662997 epoch sec; seqNo:1978; depth 25.90 2021-02-12T23:14:23Z- handleDvl() - RDI_Pathfinder msg: 1613171663.03 epoch sec; seqNo:834 2021-02-12T23:14:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.42 , 79.64 , 0.00 , 61.32 2021-02-12T23:14:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.07 , -0.11 2021-02-12T23:14:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171663.06 epoch sec; seqNo:1978; 4056499.83 north; 590636.10 east 2021-02-12T23:14:23Z- AHRS_M2 msg: 1613171663.39 epoch sec; seqNo:1961 2021-02-12T23:14:23Z- AHRS_M2 msg: 0.68 phi; 0.36 theta; -0.15 psi 2021-02-12T23:14:23Z- handleDepth()-Depth_Keller msg: 1613171663397 epoch sec; seqNo:1979; depth 25.75 2021-02-12T23:14:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171663.42 epoch sec; seqNo:1979; 4056499.83 north; 590636.10 east 2021-02-12T23:14:23Z- AHRS_M2 msg: 1613171663.79 epoch sec; seqNo:1962 2021-02-12T23:14:23Z- AHRS_M2 msg: 0.68 phi; 0.36 theta; -0.15 psi 2021-02-12T23:14:23Z- handleDepth()-Depth_Keller msg: 1613171663797 epoch sec; seqNo:1980; depth 25.63 2021-02-12T23:14:23Z- handleDvl() - RDI_Pathfinder msg: 1613171663.81 epoch sec; seqNo:835 2021-02-12T23:14:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.58 , 79.29 , 0.00 , 61.17 2021-02-12T23:14:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.03 , -0.09 2021-02-12T23:14:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171663.83 epoch sec; seqNo:1980; 4056500.21 north; 590636.10 east 2021-02-12T23:14:24Z- AHRS_M2 msg: 1613171664.19 epoch sec; seqNo:1963 2021-02-12T23:14:24Z- AHRS_M2 msg: 0.68 phi; 0.36 theta; -0.15 psi 2021-02-12T23:14:24Z- handleDepth()-Depth_Keller msg: 1613171664205 epoch sec; seqNo:1981; depth 25.49 2021-02-12T23:14:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171664.21 epoch sec; seqNo:1981; 4056500.21 north; 590636.10 east 2021-02-12T23:14:24Z- AHRS_M2 msg: 1613171664.60 epoch sec; seqNo:1964 2021-02-12T23:14:24Z- AHRS_M2 msg: 0.68 phi; 0.35 theta; -0.15 psi 2021-02-12T23:14:24Z- handleDepth()-Depth_Keller msg: 1613171664617 epoch sec; seqNo:1982; depth 25.37 2021-02-12T23:14:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171664.63 epoch sec; seqNo:1982; 4056500.60 north; 590637.31 east 2021-02-12T23:14:24Z- LcmTrn::handleMessages() - handling 44 messages took 0.73 ms and 130 calls... 2021-02-12T23:14:24Z- waiting for 1613171662.39, time is 1613171664.63 2021-02-12T23:14:24Z- LcmTrn::updateTrn() >>>> pose time 1613171664.63, meas time 1613171663.81 <<<< 2021-02-12T23:14:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171663.81 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:14:24Z- TerrainNav::motionUpdate() - currEstimate = (4056501, 590637.31, 25.37) 2021-02-12T23:14:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:14:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:14:24Z- TNavBF::Projecting Measurements 2021-02-12T23:14:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:14:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:14:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:14:24Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:14:24Z- TNavFilter::3 good beams. 2021-02-12T23:14:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:14:24Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:14:24Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:14:24Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:14:24Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:14:24Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:14:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:14:24Z- TNavBF::mmse selected from filter -1 2021-02-12T23:14:24Z- TerrainMapOctree: (vehN, vehE ) = (4056500.11, 590636.74). 2021-02-12T23:14:24Z- TerrainMapOctree: Min Distance = 5968.00. 2021-02-12T23:14:24Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:14:24Z- There is no specific good beam that all particles have in common. 2021-02-12T23:14:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:14:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 7.75 2021-02-12T23:14:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:14:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 10.06 2021-02-12T23:14:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:14:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.79 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 5.99 2021-02-12T23:14:24Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:14:24Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:14:24Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:14:24Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:14:24Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:14:24Z- Filter 0 No beams used in subcloud update 2021-02-12T23:14:24Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:14:24Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:14:24Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 789.6 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:14:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171663.81 sec. 2021-02-12T23:14:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:14:24Z- TNavBF::mmse selected from filter -1 2021-02-12T23:14:24Z- variances([0], [2], and psi_berg) = 789.478068 833.082570 0.000000 2021-02-12T23:14:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:14:24Z- LcmTrn::publishEstimates() - MLE : 4056475.10 590592.81 25.37 2021-02-12T23:14:24Z- LcmTrn::publishEstimates() - MMSE : 4056500.60 590637.31 25.37 2021-02-12T23:14:24Z- LcmTrn::publishEstimates() - COVAR: 789.48 833.08 0.76 0.00 2021-02-12T23:14:25Z- AHRS_M2 msg: 1613171665.00 epoch sec; seqNo:1965 2021-02-12T23:14:25Z- AHRS_M2 msg: 0.68 phi; 0.35 theta; -0.16 psi 2021-02-12T23:14:25Z- handleDepth()-Depth_Keller msg: 1613171665008 epoch sec; seqNo:1983; depth 25.23 2021-02-12T23:14:25Z- handleDvl() - RDI_Pathfinder msg: 1613171665.03 epoch sec; seqNo:836 2021-02-12T23:14:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.58 , 80.27 , 0.00 , 61.82 2021-02-12T23:14:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.02 , -0.05 2021-02-12T23:14:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171665.04 epoch sec; seqNo:1983; 4056500.98 north; 590637.31 east 2021-02-12T23:14:25Z- AHRS_M2 msg: 1613171665.41 epoch sec; seqNo:1966 2021-02-12T23:14:25Z- AHRS_M2 msg: 0.68 phi; 0.35 theta; -0.16 psi 2021-02-12T23:14:25Z- handleDepth()-Depth_Keller msg: 1613171665413 epoch sec; seqNo:1984; depth 25.09 2021-02-12T23:14:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171665.42 epoch sec; seqNo:1984; 4056500.98 north; 590637.31 east 2021-02-12T23:14:25Z- AHRS_M2 msg: 1613171665.81 epoch sec; seqNo:1967 2021-02-12T23:14:25Z- AHRS_M2 msg: 0.68 phi; 0.35 theta; -0.17 psi 2021-02-12T23:14:25Z- handleDepth()-Depth_Keller msg: 1613171665816 epoch sec; seqNo:1985; depth 24.98 2021-02-12T23:14:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171665.83 epoch sec; seqNo:1985; 4056501.36 north; 590637.31 east 2021-02-12T23:14:26Z- AHRS_M2 msg: 1613171666.22 epoch sec; seqNo:1968 2021-02-12T23:14:26Z- AHRS_M2 msg: 0.68 phi; 0.35 theta; -0.17 psi 2021-02-12T23:14:26Z- handleDepth()-Depth_Keller msg: 1613171666226 epoch sec; seqNo:1986; depth 24.83 2021-02-12T23:14:26Z- handleDvl() - RDI_Pathfinder msg: 1613171666.24 epoch sec; seqNo:837 2021-02-12T23:14:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 83.92 , 97.98 , 62.12 2021-02-12T23:14:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.06 , 0.00 2021-02-12T23:14:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171666.25 epoch sec; seqNo:1986; 4056501.73 north; 590637.30 east 2021-02-12T23:14:26Z- AHRS_M2 msg: 1613171666.62 epoch sec; seqNo:1969 2021-02-12T23:14:26Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.18 psi 2021-02-12T23:14:26Z- handleDepth()-Depth_Keller msg: 1613171666629 epoch sec; seqNo:1987; depth 24.71 2021-02-12T23:14:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171666.65 epoch sec; seqNo:1987; 4056501.73 north; 590637.30 east 2021-02-12T23:14:27Z- AHRS_M2 msg: 1613171667.06 epoch sec; seqNo:1970 2021-02-12T23:14:27Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.18 psi 2021-02-12T23:14:27Z- handleDepth()-Depth_Keller msg: 1613171667070 epoch sec; seqNo:1988; depth 24.58 2021-02-12T23:14:27Z- handleDvl() - RDI_Pathfinder msg: 1613171667.09 epoch sec; seqNo:838 2021-02-12T23:14:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 87.17 , 97.98 , 61.71 2021-02-12T23:14:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.09 , 0.05 2021-02-12T23:14:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171667.10 epoch sec; seqNo:1988; 4056502.11 north; 590637.30 east 2021-02-12T23:14:27Z- AHRS_M2 msg: 1613171667.43 epoch sec; seqNo:1971 2021-02-12T23:14:27Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.19 psi 2021-02-12T23:14:27Z- handleDepth()-Depth_Keller msg: 1613171667443 epoch sec; seqNo:1989; depth 24.44 2021-02-12T23:14:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171667.45 epoch sec; seqNo:1989; 4056502.50 north; 590638.51 east 2021-02-12T23:14:27Z- AHRS_M2 msg: 1613171667.83 epoch sec; seqNo:1972 2021-02-12T23:14:27Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.19 psi 2021-02-12T23:14:27Z- handleDepth()-Depth_Keller msg: 1613171667841 epoch sec; seqNo:1990; depth 24.32 2021-02-12T23:14:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171667.86 epoch sec; seqNo:1990; 4056502.50 north; 590638.51 east 2021-02-12T23:14:28Z- handleDepth()-Depth_Keller msg: 1613171668257 epoch sec; seqNo:1991; depth 24.21 2021-02-12T23:14:28Z- handleDvl() - RDI_Pathfinder msg: 1613171668.28 epoch sec; seqNo:839 2021-02-12T23:14:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 87.17 , 97.98 , 62.52 2021-02-12T23:14:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.10 , 0.10 2021-02-12T23:14:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171668.29 epoch sec; seqNo:1991; 4056502.88 north; 590638.51 east 2021-02-12T23:14:28Z- AHRS_M2 msg: 1613171668.64 epoch sec; seqNo:1973 2021-02-12T23:14:28Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.20 psi 2021-02-12T23:14:28Z- handleDepth()-Depth_Keller msg: 1613171668646 epoch sec; seqNo:1992; depth 24.07 2021-02-12T23:14:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171668.66 epoch sec; seqNo:1992; 4056503.26 north; 590638.51 east 2021-02-12T23:14:29Z- AHRS_M2 msg: 1613171669.04 epoch sec; seqNo:1974 2021-02-12T23:14:29Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.20 psi 2021-02-12T23:14:29Z- handleDepth()-Depth_Keller msg: 1613171669053 epoch sec; seqNo:1993; depth 23.95 2021-02-12T23:14:29Z- handleDvl() - RDI_Pathfinder msg: 1613171669.07 epoch sec; seqNo:840 2021-02-12T23:14:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 0.00 , 97.98 , 62.12 2021-02-12T23:14:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , 0.13 2021-02-12T23:14:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171669.08 epoch sec; seqNo:1993; 4056503.64 north; 590638.50 east 2021-02-12T23:14:29Z- AHRS_M2 msg: 1613171669.45 epoch sec; seqNo:1975 2021-02-12T23:14:29Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.20 psi 2021-02-12T23:14:29Z- handleDepth()-Depth_Keller msg: 1613171669460 epoch sec; seqNo:1994; depth 23.82 2021-02-12T23:14:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171669.49 epoch sec; seqNo:1994; 4056503.64 north; 590638.50 east 2021-02-12T23:14:29Z- LcmTrn::handleMessages() - handling 40 messages took 0.73 ms and 126 calls... 2021-02-12T23:14:29Z- waiting for 1613171667.63, time is 1613171669.49 2021-02-12T23:14:29Z- LcmTrn::updateTrn() >>>> pose time 1613171669.49, meas time 1613171669.07 <<<< 2021-02-12T23:14:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171669.07 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:14:29Z- TerrainNav::motionUpdate() - currEstimate = (4056504, 590638.50, 23.82) 2021-02-12T23:14:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:14:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:14:29Z- TNavBF::Projecting Measurements 2021-02-12T23:14:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:14:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:14:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:14:29Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:14:29Z- TNavFilter::3 good beams. 2021-02-12T23:14:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:14:29Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:14:29Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:14:29Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:14:29Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:14:29Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:14:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:14:29Z- TNavBF::mmse selected from filter -1 2021-02-12T23:14:29Z- TerrainMapOctree: (vehN, vehE ) = (4056503.38, 590638.40). 2021-02-12T23:14:29Z- TerrainMapOctree: Min Distance = 5964.40. 2021-02-12T23:14:29Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:14:29Z- There is no specific good beam that all particles have in common. 2021-02-12T23:14:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:14:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-12T23:14:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:14:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-12T23:14:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:14:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.79 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 6.17 2021-02-12T23:14:29Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:14:29Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:14:29Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:14:29Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:14:29Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:14:29Z- Filter 0 No beams used in subcloud update 2021-02-12T23:14:29Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:14:29Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:14:29Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 789.3 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:14:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171669.07 sec. 2021-02-12T23:14:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:14:29Z- TNavBF::mmse selected from filter -1 2021-02-12T23:14:29Z- variances([0], [2], and psi_berg) = 789.273527 833.082219 0.000000 2021-02-12T23:14:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:14:29Z- LcmTrn::publishEstimates() - MLE : 4056478.14 590594.00 23.82 2021-02-12T23:14:29Z- LcmTrn::publishEstimates() - MMSE : 4056503.64 590638.50 23.82 2021-02-12T23:14:29Z- LcmTrn::publishEstimates() - COVAR: 789.27 833.08 0.73 0.00 2021-02-12T23:14:29Z- AHRS_M2 msg: 1613171669.85 epoch sec; seqNo:1976 2021-02-12T23:14:29Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.20 psi 2021-02-12T23:14:29Z- handleDepth()-Depth_Keller msg: 1613171669865 epoch sec; seqNo:1995; depth 23.72 2021-02-12T23:14:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171669.87 epoch sec; seqNo:1995; 4056504.02 north; 590638.50 east 2021-02-12T23:14:30Z- AHRS_M2 msg: 1613171670.26 epoch sec; seqNo:1977 2021-02-12T23:14:30Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.20 psi 2021-02-12T23:14:30Z- handleDepth()-Depth_Keller msg: 1613171670273 epoch sec; seqNo:1996; depth 23.60 2021-02-12T23:14:30Z- handleDvl() - RDI_Pathfinder msg: 1613171670.28 epoch sec; seqNo:841 2021-02-12T23:14:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 87.17 , 97.98 , 63.34 2021-02-12T23:14:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.14 2021-02-12T23:14:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171670.29 epoch sec; seqNo:1996; 4056504.41 north; 590639.72 east 2021-02-12T23:14:30Z- AHRS_M2 msg: 1613171670.66 epoch sec; seqNo:1978 2021-02-12T23:14:30Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.19 psi 2021-02-12T23:14:30Z- handleDepth()-Depth_Keller msg: 1613171670664 epoch sec; seqNo:1997; depth 23.47 2021-02-12T23:14:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171670.68 epoch sec; seqNo:1997; 4056504.79 north; 590639.71 east 2021-02-12T23:14:31Z- AHRS_M2 msg: 1613171671.06 epoch sec; seqNo:1979 2021-02-12T23:14:31Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.19 psi 2021-02-12T23:14:31Z- handleDepth()-Depth_Keller msg: 1613171671068 epoch sec; seqNo:1998; depth 23.39 2021-02-12T23:14:31Z- handleDvl() - RDI_Pathfinder msg: 1613171671.08 epoch sec; seqNo:842 2021-02-12T23:14:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 88.25 , 97.98 , 63.34 2021-02-12T23:14:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.06 , 0.12 2021-02-12T23:14:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171671.11 epoch sec; seqNo:1998; 4056505.17 north; 590639.71 east 2021-02-12T23:14:31Z- AHRS_M2 msg: 1613171671.47 epoch sec; seqNo:1980 2021-02-12T23:14:31Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-12T23:14:31Z- handleDepth()-Depth_Keller msg: 1613171671472 epoch sec; seqNo:1999; depth 23.25 2021-02-12T23:14:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171671.48 epoch sec; seqNo:1999; 4056505.17 north; 590639.71 east 2021-02-12T23:14:31Z- AHRS_M2 msg: 1613171671.87 epoch sec; seqNo:1981 2021-02-12T23:14:31Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-12T23:14:31Z- handleDepth()-Depth_Keller msg: 1613171671879 epoch sec; seqNo:2000; depth 23.14 2021-02-12T23:14:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171671.89 epoch sec; seqNo:2000; 4056505.55 north; 590639.70 east 2021-02-12T23:14:32Z- AHRS_M2 msg: 1613171672.28 epoch sec; seqNo:1982 2021-02-12T23:14:32Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-12T23:14:32Z- handleDepth()-Depth_Keller msg: 1613171672285 epoch sec; seqNo:2001; depth 23.03 2021-02-12T23:14:32Z- handleDvl() - RDI_Pathfinder msg: 1613171672.30 epoch sec; seqNo:843 2021-02-12T23:14:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 86.09 , 97.98 , 63.34 2021-02-12T23:14:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.12 , 0.06 2021-02-12T23:14:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171672.31 epoch sec; seqNo:2001; 4056505.94 north; 590640.92 east 2021-02-12T23:14:32Z- AHRS_M2 msg: 1613171672.68 epoch sec; seqNo:1983 2021-02-12T23:14:32Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-12T23:14:32Z- handleDepth()-Depth_Keller msg: 1613171672684 epoch sec; seqNo:2002; depth 22.90 2021-02-12T23:14:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171672.69 epoch sec; seqNo:2002; 4056505.94 north; 590640.92 east 2021-02-12T23:14:33Z- AHRS_M2 msg: 1613171673.08 epoch sec; seqNo:1984 2021-02-12T23:14:33Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.18 psi 2021-02-12T23:14:33Z- handleDepth()-Depth_Keller msg: 1613171673093 epoch sec; seqNo:2003; depth 22.82 2021-02-12T23:14:33Z- handleDvl() - RDI_Pathfinder msg: 1613171673.10 epoch sec; seqNo:844 2021-02-12T23:14:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 87.17 , 97.98 , 64.97 2021-02-12T23:14:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.15 , -0.01 2021-02-12T23:14:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171673.12 epoch sec; seqNo:2003; 4056506.32 north; 590640.92 east 2021-02-12T23:14:33Z- AHRS_M2 msg: 1613171673.49 epoch sec; seqNo:1985 2021-02-12T23:14:33Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.18 psi 2021-02-12T23:14:33Z- handleDepth()-Depth_Keller msg: 1613171673492 epoch sec; seqNo:2004; depth 22.69 2021-02-12T23:14:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171673.51 epoch sec; seqNo:2004; 4056506.32 north; 590640.92 east 2021-02-12T23:14:33Z- AHRS_M2 msg: 1613171673.89 epoch sec; seqNo:1986 2021-02-12T23:14:33Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.18 psi 2021-02-12T23:14:33Z- handleDepth()-Depth_Keller msg: 1613171673901 epoch sec; seqNo:2005; depth 22.58 2021-02-12T23:14:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171673.91 epoch sec; seqNo:2005; 4056506.70 north; 590640.91 east 2021-02-12T23:14:34Z- AHRS_M2 msg: 1613171674.30 epoch sec; seqNo:1987 2021-02-12T23:14:34Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:14:34Z- handleDepth()-Depth_Keller msg: 1613171674301 epoch sec; seqNo:2006; depth 22.46 2021-02-12T23:14:34Z- handleDvl() - RDI_Pathfinder msg: 1613171674.31 epoch sec; seqNo:845 2021-02-12T23:14:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 87.10 , 96.83 , 64.16 2021-02-12T23:14:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.12 , -0.06 2021-02-12T23:14:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171674.32 epoch sec; seqNo:2006; 4056507.09 north; 590642.13 east 2021-02-12T23:14:34Z- AHRS_M2 msg: 1613171674.70 epoch sec; seqNo:1988 2021-02-12T23:14:34Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:14:34Z- handleDepth()-Depth_Keller msg: 1613171674706 epoch sec; seqNo:2007; depth 22.35 2021-02-12T23:14:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171674.72 epoch sec; seqNo:2007; 4056507.09 north; 590642.13 east 2021-02-12T23:14:34Z- LcmTrn::handleMessages() - handling 44 messages took 0.60 ms and 132 calls... 2021-02-12T23:14:34Z- waiting for 1613171672.49, time is 1613171674.72 2021-02-12T23:14:34Z- LcmTrn::updateTrn() >>>> pose time 1613171674.72, meas time 1613171674.31 <<<< 2021-02-12T23:14:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171674.31 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:14:34Z- TerrainNav::motionUpdate() - currEstimate = (4056507, 590642.13, 22.35) 2021-02-12T23:14:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:14:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:14:34Z- TNavBF::Projecting Measurements 2021-02-12T23:14:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:14:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:14:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:14:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:14:34Z- TNavFilter::4 good beams. 2021-02-12T23:14:34Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:14:34Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:14:34Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:14:34Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:14:34Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:14:34Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:14:34Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:14:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:14:34Z- TerrainMapOctree: (vehN, vehE ) = (4056506.82, 590641.85). 2021-02-12T23:14:34Z- TerrainMapOctree: Min Distance = 5959.57. 2021-02-12T23:14:34Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:14:34Z- There is no specific good beam that all particles have in common. 2021-02-12T23:14:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:14:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-12T23:14:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:14:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 12.14 2021-02-12T23:14:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:14:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 15.00 2021-02-12T23:14:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:14:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.79 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 6.59 2021-02-12T23:14:34Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:14:34Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:14:34Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:14:34Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:14:34Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:14:34Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:14:34Z- Filter 0 No beams used in subcloud update 2021-02-12T23:14:34Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:14:34Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:14:34Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 789.1 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:14:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171674.31 sec. 2021-02-12T23:14:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:14:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:14:34Z- variances([0], [2], and psi_berg) = 789.023572 833.081792 0.000000 2021-02-12T23:14:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:14:34Z- LcmTrn::publishEstimates() - MLE : 4056481.59 590597.63 22.35 2021-02-12T23:14:34Z- LcmTrn::publishEstimates() - MMSE : 4056507.09 590642.13 22.35 2021-02-12T23:14:34Z- LcmTrn::publishEstimates() - COVAR: 789.02 833.08 0.74 0.00 2021-02-12T23:14:35Z- AHRS_M2 msg: 1613171675.10 epoch sec; seqNo:1989 2021-02-12T23:14:35Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.18 psi 2021-02-12T23:14:35Z- handleDepth()-Depth_Keller msg: 1613171675113 epoch sec; seqNo:2008; depth 22.22 2021-02-12T23:14:35Z- handleDvl() - RDI_Pathfinder msg: 1613171675.13 epoch sec; seqNo:846 2021-02-12T23:14:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 87.17 , 97.98 , 64.97 2021-02-12T23:14:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.09 , -0.07 2021-02-12T23:14:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171675.15 epoch sec; seqNo:2008; 4056507.47 north; 590642.13 east 2021-02-12T23:14:35Z- AHRS_M2 msg: 1613171675.51 epoch sec; seqNo:1990 2021-02-12T23:14:35Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:14:35Z- handleDepth()-Depth_Keller msg: 1613171675512 epoch sec; seqNo:2009; depth 22.11 2021-02-12T23:14:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171675.52 epoch sec; seqNo:2009; 4056507.85 north; 590642.12 east 2021-02-12T23:14:35Z- AHRS_M2 msg: 1613171675.91 epoch sec; seqNo:1991 2021-02-12T23:14:35Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.18 psi 2021-02-12T23:14:35Z- handleDepth()-Depth_Keller msg: 1613171675921 epoch sec; seqNo:2010; depth 22.00 2021-02-12T23:14:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171675.93 epoch sec; seqNo:2010; 4056507.85 north; 590642.12 east 2021-02-12T23:14:36Z- AHRS_M2 msg: 1613171676.32 epoch sec; seqNo:1992 2021-02-12T23:14:36Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.18 psi 2021-02-12T23:14:36Z- handleDepth()-Depth_Keller msg: 1613171676327 epoch sec; seqNo:2011; depth 21.89 2021-02-12T23:14:36Z- handleDvl() - RDI_Pathfinder msg: 1613171676.34 epoch sec; seqNo:847 2021-02-12T23:14:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 88.18 , 96.83 , 64.97 2021-02-12T23:14:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , -0.08 2021-02-12T23:14:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171676.35 epoch sec; seqNo:2011; 4056508.23 north; 590642.12 east 2021-02-12T23:14:36Z- AHRS_M2 msg: 1613171676.73 epoch sec; seqNo:1993 2021-02-12T23:14:36Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.18 psi 2021-02-12T23:14:36Z- handleDepth()-Depth_Keller msg: 1613171676730 epoch sec; seqNo:2012; depth 21.78 2021-02-12T23:14:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171676.75 epoch sec; seqNo:2012; 4056508.62 north; 590643.33 east 2021-02-12T23:14:37Z- AHRS_M2 msg: 1613171677.13 epoch sec; seqNo:1994 2021-02-12T23:14:37Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.18 psi 2021-02-12T23:14:37Z- handleDepth()-Depth_Keller msg: 1613171677135 epoch sec; seqNo:2013; depth 21.68 2021-02-12T23:14:37Z- handleDvl() - RDI_Pathfinder msg: 1613171677.15 epoch sec; seqNo:848 2021-02-12T23:14:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 88.18 , 97.92 , 64.97 2021-02-12T23:14:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.07 2021-02-12T23:14:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171677.16 epoch sec; seqNo:2013; 4056508.62 north; 590643.33 east 2021-02-12T23:14:37Z- AHRS_M2 msg: 1613171677.53 epoch sec; seqNo:1995 2021-02-12T23:14:37Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.19 psi 2021-02-12T23:14:37Z- handleDepth()-Depth_Keller msg: 1613171677535 epoch sec; seqNo:2014; depth 21.55 2021-02-12T23:14:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171677.55 epoch sec; seqNo:2014; 4056509.00 north; 590643.33 east 2021-02-12T23:14:37Z- AHRS_M2 msg: 1613171677.93 epoch sec; seqNo:1996 2021-02-12T23:14:37Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.19 psi 2021-02-12T23:14:37Z- handleDepth()-Depth_Keller msg: 1613171677938 epoch sec; seqNo:2015; depth 21.45 2021-02-12T23:14:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171677.95 epoch sec; seqNo:2015; 4056509.38 north; 590643.33 east 2021-02-12T23:14:38Z- AHRS_M2 msg: 1613171678.34 epoch sec; seqNo:1997 2021-02-12T23:14:38Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.19 psi 2021-02-12T23:14:38Z- handleDepth()-Depth_Keller msg: 1613171678345 epoch sec; seqNo:2016; depth 21.33 2021-02-12T23:14:38Z- handleDvl() - RDI_Pathfinder msg: 1613171678.36 epoch sec; seqNo:849 2021-02-12T23:14:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 89.26 , 97.92 , 64.57 2021-02-12T23:14:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , -0.06 2021-02-12T23:14:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171678.38 epoch sec; seqNo:2016; 4056509.38 north; 590643.33 east 2021-02-12T23:14:38Z- AHRS_M2 msg: 1613171678.74 epoch sec; seqNo:1998 2021-02-12T23:14:38Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.20 psi 2021-02-12T23:14:38Z- handleDepth()-Depth_Keller msg: 1613171678749 epoch sec; seqNo:2017; depth 21.22 2021-02-12T23:14:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171678.76 epoch sec; seqNo:2017; 4056509.75 north; 590643.32 east 2021-02-12T23:14:39Z- AHRS_M2 msg: 1613171679.15 epoch sec; seqNo:1999 2021-02-12T23:14:39Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.20 psi 2021-02-12T23:14:39Z- handleDepth()-Depth_Keller msg: 1613171679155 epoch sec; seqNo:2018; depth 21.13 2021-02-12T23:14:39Z- handleDvl() - RDI_Pathfinder msg: 1613171679.17 epoch sec; seqNo:850 2021-02-12T23:14:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 91.36 , 97.85 , 66.20 2021-02-12T23:14:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , -0.04 2021-02-12T23:14:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171679.18 epoch sec; seqNo:2018; 4056510.13 north; 590643.32 east 2021-02-12T23:14:39Z- AHRS_M2 msg: 1613171679.55 epoch sec; seqNo:2000 2021-02-12T23:14:39Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.20 psi 2021-02-12T23:14:39Z- handleDepth()-Depth_Keller msg: 1613171679559 epoch sec; seqNo:2019; depth 21.01 2021-02-12T23:14:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171679.57 epoch sec; seqNo:2019; 4056510.15 north; 590644.54 east 2021-02-12T23:14:39Z- AHRS_M2 msg: 1613171679.95 epoch sec; seqNo:2001 2021-02-12T23:14:39Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.21 psi 2021-02-12T23:14:39Z- handleDepth()-Depth_Keller msg: 1613171679961 epoch sec; seqNo:2020; depth 20.89 2021-02-12T23:14:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171679.97 epoch sec; seqNo:2020; 4056510.52 north; 590644.54 east 2021-02-12T23:14:40Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-12T23:14:40Z- waiting for 1613171677.72, time is 1613171679.97 2021-02-12T23:14:40Z- LcmTrn::updateTrn() >>>> pose time 1613171679.97, meas time 1613171679.17 <<<< 2021-02-12T23:14:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171679.17 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:14:40Z- TerrainNav::motionUpdate() - currEstimate = (4056511, 590644.54, 20.89) 2021-02-12T23:14:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:14:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:14:40Z- TNavBF::Projecting Measurements 2021-02-12T23:14:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:14:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:14:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:14:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:14:40Z- TNavFilter::4 good beams. 2021-02-12T23:14:40Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:14:40Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:14:40Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:14:40Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:14:40Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:14:40Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:14:40Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:14:40Z- TNavBF::mmse selected from filter -1 2021-02-12T23:14:40Z- TerrainMapOctree: (vehN, vehE ) = (4056510.00, 590644.17). 2021-02-12T23:14:40Z- TerrainMapOctree: Min Distance = 5955.64. 2021-02-12T23:14:40Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:14:40Z- There is no specific good beam that all particles have in common. 2021-02-12T23:14:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:14:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-12T23:14:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:14:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 13.35 2021-02-12T23:14:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:14:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-12T23:14:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:14:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.76 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 7.01 2021-02-12T23:14:40Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:14:40Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:14:40Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:14:40Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:14:40Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:14:40Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:14:40Z- Filter 0 No beams used in subcloud update 2021-02-12T23:14:40Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:14:40Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:14:40Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 788.9 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:14:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171679.17 sec. 2021-02-12T23:14:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:14:40Z- TNavBF::mmse selected from filter -1 2021-02-12T23:14:40Z- variances([0], [2], and psi_berg) = 788.782509 833.081383 0.000000 2021-02-12T23:14:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:14:40Z- LcmTrn::publishEstimates() - MLE : 4056485.02 590600.04 20.89 2021-02-12T23:14:40Z- LcmTrn::publishEstimates() - MMSE : 4056510.52 590644.54 20.89 2021-02-12T23:14:40Z- LcmTrn::publishEstimates() - COVAR: 788.78 833.08 0.73 0.00 2021-02-12T23:14:40Z- AHRS_M2 msg: 1613171680.36 epoch sec; seqNo:2002 2021-02-12T23:14:40Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.21 psi 2021-02-12T23:14:40Z- handleDepth()-Depth_Keller msg: 1613171680360 epoch sec; seqNo:2021; depth 20.79 2021-02-12T23:14:40Z- handleDvl() - RDI_Pathfinder msg: 1613171680.38 epoch sec; seqNo:851 2021-02-12T23:14:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 92.37 , 96.70 , 66.20 2021-02-12T23:14:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.07 , 0.01 2021-02-12T23:14:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171680.39 epoch sec; seqNo:2021; 4056510.90 north; 590644.53 east 2021-02-12T23:14:40Z- AHRS_M2 msg: 1613171680.76 epoch sec; seqNo:2003 2021-02-12T23:14:40Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.21 psi 2021-02-12T23:14:40Z- handleDepth()-Depth_Keller msg: 1613171680764 epoch sec; seqNo:2022; depth 20.70 2021-02-12T23:14:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171680.78 epoch sec; seqNo:2022; 4056511.28 north; 590644.53 east 2021-02-12T23:14:41Z- AHRS_M2 msg: 1613171681.16 epoch sec; seqNo:2004 2021-02-12T23:14:41Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.21 psi 2021-02-12T23:14:41Z- handleDepth()-Depth_Keller msg: 1613171681169 epoch sec; seqNo:2023; depth 20.58 2021-02-12T23:14:41Z- handleDvl() - RDI_Pathfinder msg: 1613171681.18 epoch sec; seqNo:852 2021-02-12T23:14:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 91.29 , 98.86 , 67.43 2021-02-12T23:14:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.10 , 0.07 2021-02-12T23:14:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171681.19 epoch sec; seqNo:2023; 4056511.28 north; 590644.53 east 2021-02-12T23:14:41Z- AHRS_M2 msg: 1613171681.57 epoch sec; seqNo:2005 2021-02-12T23:14:41Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.20 psi 2021-02-12T23:14:41Z- handleDepth()-Depth_Keller msg: 1613171681572 epoch sec; seqNo:2024; depth 20.48 2021-02-12T23:14:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171681.59 epoch sec; seqNo:2024; 4056511.66 north; 590644.52 east 2021-02-12T23:14:41Z- AHRS_M2 msg: 1613171681.97 epoch sec; seqNo:2006 2021-02-12T23:14:41Z- AHRS_M2 msg: 0.58 phi; 0.30 theta; -0.20 psi 2021-02-12T23:14:41Z- handleDepth()-Depth_Keller msg: 1613171681981 epoch sec; seqNo:2025; depth 20.37 2021-02-12T23:14:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171681.99 epoch sec; seqNo:2025; 4056512.04 north; 590644.52 east 2021-02-12T23:14:42Z- AHRS_M2 msg: 1613171682.38 epoch sec; seqNo:2007 2021-02-12T23:14:42Z- AHRS_M2 msg: 0.58 phi; 0.30 theta; -0.20 psi 2021-02-12T23:14:42Z- handleDepth()-Depth_Keller msg: 1613171682380 epoch sec; seqNo:2026; depth 20.26 2021-02-12T23:14:42Z- handleDvl() - RDI_Pathfinder msg: 1613171682.39 epoch sec; seqNo:853 2021-02-12T23:14:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 91.29 , 98.86 , 66.20 2021-02-12T23:14:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.06 , 0.14 2021-02-12T23:14:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171682.40 epoch sec; seqNo:2026; 4056512.04 north; 590644.52 east 2021-02-12T23:14:42Z- handleDepth()-Depth_Keller msg: 1613171682801 epoch sec; seqNo:2027; depth 20.16 2021-02-12T23:14:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171682.81 epoch sec; seqNo:2027; 4056512.43 north; 590645.74 east 2021-02-12T23:14:43Z- AHRS_M2 msg: 1613171683.18 epoch sec; seqNo:2008 2021-02-12T23:14:43Z- AHRS_M2 msg: 0.57 phi; 0.31 theta; -0.20 psi 2021-02-12T23:14:43Z- handleDepth()-Depth_Keller msg: 1613171683189 epoch sec; seqNo:2028; depth 20.04 2021-02-12T23:14:43Z- handleDvl() - RDI_Pathfinder msg: 1613171683.21 epoch sec; seqNo:854 2021-02-12T23:14:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 91.29 , 98.86 , 66.61 2021-02-12T23:14:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.16 2021-02-12T23:14:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171683.24 epoch sec; seqNo:2028; 4056512.81 north; 590645.73 east 2021-02-12T23:14:43Z- AHRS_M2 msg: 1613171683.59 epoch sec; seqNo:2009 2021-02-12T23:14:43Z- AHRS_M2 msg: 0.57 phi; 0.31 theta; -0.19 psi 2021-02-12T23:14:43Z- handleDepth()-Depth_Keller msg: 1613171683594 epoch sec; seqNo:2029; depth 19.93 2021-02-12T23:14:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171683.61 epoch sec; seqNo:2029; 4056513.19 north; 590645.73 east 2021-02-12T23:14:44Z- AHRS_M2 msg: 1613171684.00 epoch sec; seqNo:2010 2021-02-12T23:14:44Z- AHRS_M2 msg: 0.57 phi; 0.31 theta; -0.19 psi 2021-02-12T23:14:44Z- handleDepth()-Depth_Keller msg: 1613171684000 epoch sec; seqNo:2030; depth 19.84 2021-02-12T23:14:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171684.04 epoch sec; seqNo:2030; 4056513.57 north; 590645.72 east 2021-02-12T23:14:44Z- AHRS_M2 msg: 1613171684.39 epoch sec; seqNo:2011 2021-02-12T23:14:44Z- AHRS_M2 msg: 0.57 phi; 0.31 theta; -0.18 psi 2021-02-12T23:14:44Z- handleDepth()-Depth_Keller msg: 1613171684405 epoch sec; seqNo:2031; depth 19.74 2021-02-12T23:14:44Z- handleDvl() - RDI_Pathfinder msg: 1613171684.42 epoch sec; seqNo:855 2021-02-12T23:14:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.32 , 90.21 , 98.86 , 66.19 2021-02-12T23:14:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , 0.12 2021-02-12T23:14:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171684.44 epoch sec; seqNo:2031; 4056513.57 north; 590645.72 east 2021-02-12T23:14:44Z- AHRS_M2 msg: 1613171684.80 epoch sec; seqNo:2012 2021-02-12T23:14:44Z- AHRS_M2 msg: 0.57 phi; 0.31 theta; -0.17 psi 2021-02-12T23:14:44Z- handleDepth()-Depth_Keller msg: 1613171684804 epoch sec; seqNo:2032; depth 19.63 2021-02-12T23:14:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171684.82 epoch sec; seqNo:2032; 4056513.95 north; 590645.72 east 2021-02-12T23:14:45Z- LcmTrn::handleMessages() - handling 40 messages took 0.71 ms and 126 calls... 2021-02-12T23:14:45Z- waiting for 1613171682.97, time is 1613171684.82 2021-02-12T23:14:45Z- LcmTrn::updateTrn() >>>> pose time 1613171684.82, meas time 1613171684.42 <<<< 2021-02-12T23:14:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171684.42 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:14:45Z- TerrainNav::motionUpdate() - currEstimate = (4056514, 590645.72, 19.63) 2021-02-12T23:14:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:14:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:14:45Z- TNavBF::Projecting Measurements 2021-02-12T23:14:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:14:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:14:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:14:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:14:45Z- TNavFilter::4 good beams. 2021-02-12T23:14:45Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:14:45Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:14:45Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:14:45Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:14:45Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:14:45Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:14:45Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:14:45Z- TNavBF::mmse selected from filter -1 2021-02-12T23:14:45Z- TerrainMapOctree: (vehN, vehE ) = (4056513.67, 590645.62). 2021-02-12T23:14:45Z- TerrainMapOctree: Min Distance = 5951.84. 2021-02-12T23:14:45Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:14:45Z- There is no specific good beam that all particles have in common. 2021-02-12T23:14:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:14:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-12T23:14:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:14:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 13.02 2021-02-12T23:14:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:14:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 15.64 2021-02-12T23:14:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:14:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.75 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 7.01 2021-02-12T23:14:45Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:14:45Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:14:45Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:14:45Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:14:45Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:14:45Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:14:45Z- Filter 0 No beams used in subcloud update 2021-02-12T23:14:45Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:14:45Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:14:45Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 788.6 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:14:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171684.42 sec. 2021-02-12T23:14:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:14:45Z- TNavBF::mmse selected from filter -1 2021-02-12T23:14:45Z- variances([0], [2], and psi_berg) = 788.570761 833.081025 0.000000 2021-02-12T23:14:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:14:45Z- LcmTrn::publishEstimates() - MLE : 4056488.45 590601.22 19.63 2021-02-12T23:14:45Z- LcmTrn::publishEstimates() - MMSE : 4056513.95 590645.72 19.63 2021-02-12T23:14:45Z- LcmTrn::publishEstimates() - COVAR: 788.57 833.08 0.71 0.00 2021-02-12T23:14:45Z- handleDepth()-Depth_Keller msg: 1613171685233 epoch sec; seqNo:2033; depth 19.53 2021-02-12T23:14:45Z- handleDvl() - RDI_Pathfinder msg: 1613171685.24 epoch sec; seqNo:856 2021-02-12T23:14:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.55 , 90.21 , 99.94 , 67.42 2021-02-12T23:14:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , 0.07 2021-02-12T23:14:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171685.26 epoch sec; seqNo:2033; 4056514.34 north; 590646.94 east 2021-02-12T23:14:45Z- AHRS_M2 msg: 1613171685.61 epoch sec; seqNo:2013 2021-02-12T23:14:45Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.16 psi 2021-02-12T23:14:45Z- handleDepth()-Depth_Keller msg: 1613171685625 epoch sec; seqNo:2034; depth 19.42 2021-02-12T23:14:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171685.64 epoch sec; seqNo:2034; 4056514.72 north; 590646.93 east 2021-02-12T23:14:46Z- AHRS_M2 msg: 1613171686.02 epoch sec; seqNo:2014 2021-02-12T23:14:46Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.16 psi 2021-02-12T23:14:46Z- handleDepth()-Depth_Keller msg: 1613171686026 epoch sec; seqNo:2035; depth 19.31 2021-02-12T23:14:46Z- handleDvl() - RDI_Pathfinder msg: 1613171686.04 epoch sec; seqNo:857 2021-02-12T23:14:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 89.87 , 98.39 , 68.24 2021-02-12T23:14:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , 0.02 2021-02-12T23:14:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171686.05 epoch sec; seqNo:2035; 4056514.72 north; 590646.93 east 2021-02-12T23:14:46Z- AHRS_M2 msg: 1613171686.42 epoch sec; seqNo:2015 2021-02-12T23:14:46Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.15 psi 2021-02-12T23:14:46Z- handleDepth()-Depth_Keller msg: 1613171686420 epoch sec; seqNo:2036; depth 19.21 2021-02-12T23:14:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171686.43 epoch sec; seqNo:2036; 4056515.10 north; 590646.93 east 2021-02-12T23:14:46Z- AHRS_M2 msg: 1613171686.82 epoch sec; seqNo:2016 2021-02-12T23:14:46Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.15 psi 2021-02-12T23:14:46Z- handleDepth()-Depth_Keller msg: 1613171686834 epoch sec; seqNo:2037; depth 19.11 2021-02-12T23:14:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171686.84 epoch sec; seqNo:2037; 4056515.48 north; 590646.93 east 2021-02-12T23:14:47Z- AHRS_M2 msg: 1613171687.22 epoch sec; seqNo:2017 2021-02-12T23:14:47Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.15 psi 2021-02-12T23:14:47Z- handleDepth()-Depth_Keller msg: 1613171687229 epoch sec; seqNo:2038; depth 19.01 2021-02-12T23:14:47Z- handleDvl() - RDI_Pathfinder msg: 1613171687.24 epoch sec; seqNo:858 2021-02-12T23:14:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.78 , 89.87 , 99.61 , 67.43 2021-02-12T23:14:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , -0.01 2021-02-12T23:14:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171687.26 epoch sec; seqNo:2038; 4056515.48 north; 590646.93 east 2021-02-12T23:14:47Z- AHRS_M2 msg: 1613171687.63 epoch sec; seqNo:2018 2021-02-12T23:14:47Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.14 psi 2021-02-12T23:14:47Z- handleDepth()-Depth_Keller msg: 1613171687632 epoch sec; seqNo:2039; depth 18.91 2021-02-12T23:14:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171687.64 epoch sec; seqNo:2039; 4056515.87 north; 590648.14 east 2021-02-12T23:14:48Z- AHRS_M2 msg: 1613171688.06 epoch sec; seqNo:2019 2021-02-12T23:14:48Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.14 psi 2021-02-12T23:14:48Z- handleDepth()-Depth_Keller msg: 1613171688064 epoch sec; seqNo:2040; depth 18.81 2021-02-12T23:14:48Z- handleDvl() - RDI_Pathfinder msg: 1613171688.08 epoch sec; seqNo:859 2021-02-12T23:14:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.78 , 89.87 , 99.61 , 67.84 2021-02-12T23:14:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , -0.05 2021-02-12T23:14:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171688.09 epoch sec; seqNo:2040; 4056516.25 north; 590648.14 east 2021-02-12T23:14:48Z- AHRS_M2 msg: 1613171688.44 epoch sec; seqNo:2020 2021-02-12T23:14:48Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.14 psi 2021-02-12T23:14:48Z- handleDepth()-Depth_Keller msg: 1613171688440 epoch sec; seqNo:2041; depth 18.69 2021-02-12T23:14:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171688.45 epoch sec; seqNo:2041; 4056516.63 north; 590648.13 east 2021-02-12T23:14:48Z- AHRS_M2 msg: 1613171688.84 epoch sec; seqNo:2021 2021-02-12T23:14:48Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.13 psi 2021-02-12T23:14:48Z- handleDepth()-Depth_Keller msg: 1613171688844 epoch sec; seqNo:2042; depth 18.61 2021-02-12T23:14:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171688.85 epoch sec; seqNo:2042; 4056516.63 north; 590648.13 east 2021-02-12T23:14:49Z- AHRS_M2 msg: 1613171689.24 epoch sec; seqNo:2022 2021-02-12T23:14:49Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.12 psi 2021-02-12T23:14:49Z- handleDepth()-Depth_Keller msg: 1613171689250 epoch sec; seqNo:2043; depth 18.50 2021-02-12T23:14:49Z- handleDvl() - RDI_Pathfinder msg: 1613171689.26 epoch sec; seqNo:860 2021-02-12T23:14:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.01 , 89.87 , 99.61 , 67.02 2021-02-12T23:14:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , -0.10 2021-02-12T23:14:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171689.27 epoch sec; seqNo:2043; 4056517.00 north; 590648.13 east 2021-02-12T23:14:49Z- AHRS_M2 msg: 1613171689.65 epoch sec; seqNo:2023 2021-02-12T23:14:49Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.12 psi 2021-02-12T23:14:49Z- handleDepth()-Depth_Keller msg: 1613171689652 epoch sec; seqNo:2044; depth 18.39 2021-02-12T23:14:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171689.67 epoch sec; seqNo:2044; 4056517.38 north; 590648.13 east 2021-02-12T23:14:50Z- AHRS_M2 msg: 1613171690.06 epoch sec; seqNo:2024 2021-02-12T23:14:50Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.11 psi 2021-02-12T23:14:50Z- handleDepth()-Depth_Keller msg: 1613171690088 epoch sec; seqNo:2045; depth 18.29 2021-02-12T23:14:50Z- handleDvl() - RDI_Pathfinder msg: 1613171690.10 epoch sec; seqNo:861 2021-02-12T23:14:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.41 , 88.58 , 99.53 , 78.85 2021-02-12T23:14:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , 0.01 , -0.18 2021-02-12T23:14:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171690.12 epoch sec; seqNo:2045; 4056517.40 north; 590649.35 east 2021-02-12T23:14:50Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-12T23:14:50Z- waiting for 1613171687.82, time is 1613171690.12 2021-02-12T23:14:50Z- LcmTrn::updateTrn() equating pose 1613171690.12 and meas times 1613171690.12 2021-02-12T23:14:50Z- LcmTrn::updateTrn() >>>> pose time 1613171690.12, meas time 1613171690.12 <<<< 2021-02-12T23:14:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171690.12 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:14:50Z- TerrainNav::motionUpdate() - currEstimate = (4056517, 590649.35, 18.29) 2021-02-12T23:14:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:14:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:14:50Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171690.12 sec.; No relevant navigation data available 2021-02-12T23:14:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:14:50Z- TNavBF::mmse selected from filter -1 2021-02-12T23:14:50Z- variances([0], [2], and psi_berg) = 788.371471 833.080689 0.000000 2021-02-12T23:14:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:14:50Z- LcmTrn::publishEstimates() - MLE : 4056491.90 590604.85 18.29 2021-02-12T23:14:50Z- LcmTrn::publishEstimates() - MMSE : 4056517.40 590649.35 18.29 2021-02-12T23:14:50Z- LcmTrn::publishEstimates() - COVAR: 788.37 833.08 0.96 0.00 2021-02-12T23:14:50Z- AHRS_M2 msg: 1613171690.45 epoch sec; seqNo:2025 2021-02-12T23:14:50Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.11 psi 2021-02-12T23:14:50Z- handleDepth()-Depth_Keller msg: 1613171690459 epoch sec; seqNo:2046; depth 18.18 2021-02-12T23:14:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171690.47 epoch sec; seqNo:2046; 4056517.77 north; 590649.34 east 2021-02-12T23:14:50Z- AHRS_M2 msg: 1613171690.86 epoch sec; seqNo:2026 2021-02-12T23:14:50Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.11 psi 2021-02-12T23:14:50Z- handleDepth()-Depth_Keller msg: 1613171690866 epoch sec; seqNo:2047; depth 18.06 2021-02-12T23:14:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171690.87 epoch sec; seqNo:2047; 4056517.77 north; 590649.34 east 2021-02-12T23:14:51Z- AHRS_M2 msg: 1613171691.26 epoch sec; seqNo:2027 2021-02-12T23:14:51Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.11 psi 2021-02-12T23:14:51Z- handleDepth()-Depth_Keller msg: 1613171691271 epoch sec; seqNo:2048; depth 17.97 2021-02-12T23:14:51Z- handleDvl() - RDI_Pathfinder msg: 1613171691.28 epoch sec; seqNo:862 2021-02-12T23:14:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.41 , 88.38 , 100.55 , 67.83 2021-02-12T23:14:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.09 , -0.13 2021-02-12T23:14:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171691.29 epoch sec; seqNo:2048; 4056518.15 north; 590649.34 east 2021-02-12T23:14:51Z- AHRS_M2 msg: 1613171691.67 epoch sec; seqNo:2028 2021-02-12T23:14:51Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.12 psi 2021-02-12T23:14:51Z- handleDepth()-Depth_Keller msg: 1613171691677 epoch sec; seqNo:2049; depth 17.85 2021-02-12T23:14:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171691.69 epoch sec; seqNo:2049; 4056518.15 north; 590649.34 east 2021-02-12T23:14:52Z- AHRS_M2 msg: 1613171692.07 epoch sec; seqNo:2029 2021-02-12T23:14:52Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.12 psi 2021-02-12T23:14:52Z- handleDepth()-Depth_Keller msg: 1613171692076 epoch sec; seqNo:2050; depth 17.76 2021-02-12T23:14:52Z- handleDvl() - RDI_Pathfinder msg: 1613171692.09 epoch sec; seqNo:863 2021-02-12T23:14:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:14:52Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:14:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171692.10 epoch sec; seqNo:2050; 4056518.53 north; 590649.34 east 2021-02-12T23:14:52Z- AHRS_M2 msg: 1613171692.47 epoch sec; seqNo:2030 2021-02-12T23:14:52Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.13 psi 2021-02-12T23:14:52Z- handleDepth()-Depth_Keller msg: 1613171692491 epoch sec; seqNo:2051; depth 17.64 2021-02-12T23:14:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171692.50 epoch sec; seqNo:2051; 4056518.91 north; 590649.33 east 2021-02-12T23:14:52Z- AHRS_M2 msg: 1613171692.88 epoch sec; seqNo:2031 2021-02-12T23:14:52Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.13 psi 2021-02-12T23:14:52Z- handleDepth()-Depth_Keller msg: 1613171692884 epoch sec; seqNo:2052; depth 17.52 2021-02-12T23:14:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171692.89 epoch sec; seqNo:2052; 4056518.91 north; 590649.33 east 2021-02-12T23:14:53Z- AHRS_M2 msg: 1613171693.28 epoch sec; seqNo:2032 2021-02-12T23:14:53Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.14 psi 2021-02-12T23:14:53Z- handleDepth()-Depth_Keller msg: 1613171693288 epoch sec; seqNo:2053; depth 17.42 2021-02-12T23:14:53Z- handleDvl() - RDI_Pathfinder msg: 1613171693.31 epoch sec; seqNo:864 2021-02-12T23:14:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.00 , 90.41 , 101.22 , 67.42 2021-02-12T23:14:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.23 , 0.09 2021-02-12T23:14:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171693.32 epoch sec; seqNo:2053; 4056519.29 north; 590649.33 east 2021-02-12T23:14:53Z- AHRS_M2 msg: 1613171693.69 epoch sec; seqNo:2033 2021-02-12T23:14:53Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-12T23:14:53Z- handleDepth()-Depth_Keller msg: 1613171693699 epoch sec; seqNo:2054; depth 17.29 2021-02-12T23:14:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171693.71 epoch sec; seqNo:2054; 4056519.68 north; 590650.55 east 2021-02-12T23:14:54Z- AHRS_M2 msg: 1613171694.10 epoch sec; seqNo:2034 2021-02-12T23:14:54Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-12T23:14:54Z- handleDepth()-Depth_Keller msg: 1613171694105 epoch sec; seqNo:2055; depth 17.18 2021-02-12T23:14:54Z- handleDvl() - RDI_Pathfinder msg: 1613171694.13 epoch sec; seqNo:865 2021-02-12T23:14:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.41 , 91.02 , 101.97 , 69.06 2021-02-12T23:14:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.12 , 0.14 2021-02-12T23:14:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171694.14 epoch sec; seqNo:2055; 4056520.06 north; 590650.54 east 2021-02-12T23:14:54Z- AHRS_M2 msg: 1613171694.50 epoch sec; seqNo:2035 2021-02-12T23:14:54Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-12T23:14:54Z- handleDepth()-Depth_Keller msg: 1613171694504 epoch sec; seqNo:2056; depth 17.08 2021-02-12T23:14:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171694.52 epoch sec; seqNo:2056; 4056520.44 north; 590650.54 east 2021-02-12T23:14:54Z- AHRS_M2 msg: 1613171694.90 epoch sec; seqNo:2036 2021-02-12T23:14:54Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-12T23:14:54Z- handleDepth()-Depth_Keller msg: 1613171694906 epoch sec; seqNo:2057; depth 16.98 2021-02-12T23:14:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171694.91 epoch sec; seqNo:2057; 4056520.44 north; 590650.54 east 2021-02-12T23:14:55Z- LcmTrn::handleMessages() - handling 40 messages took 0.63 ms and 128 calls... 2021-02-12T23:14:55Z- waiting for 1613171693.12, time is 1613171694.91 2021-02-12T23:14:55Z- LcmTrn::updateTrn() >>>> pose time 1613171694.91, meas time 1613171694.13 <<<< 2021-02-12T23:14:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171694.13 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:14:55Z- TerrainNav::motionUpdate() - currEstimate = (4056520, 590650.54, 16.98) 2021-02-12T23:14:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:14:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:14:55Z- TNavBF::Projecting Measurements 2021-02-12T23:14:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:14:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:14:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:14:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:14:55Z- TNavFilter::4 good beams. 2021-02-12T23:14:55Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:14:55Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:14:55Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:14:55Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:14:55Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:14:55Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:14:55Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:14:55Z- TNavBF::mmse selected from filter -1 2021-02-12T23:14:55Z- TerrainMapOctree: (vehN, vehE ) = (4056519.94, 590650.34). 2021-02-12T23:14:55Z- TerrainMapOctree: Min Distance = 5943.99. 2021-02-12T23:14:55Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:14:55Z- There is no specific good beam that all particles have in common. 2021-02-12T23:14:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:14:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 9.34 2021-02-12T23:14:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:14:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 13.25 2021-02-12T23:14:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:14:55Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 16.64 2021-02-12T23:14:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:14:55Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-12T23:14:55Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:14:55Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:14:55Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:14:55Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:14:55Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:14:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:14:55Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:14:55Z- Filter 0 No beams used in subcloud update 2021-02-12T23:14:55Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:14:55Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:14:55Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 788.2 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:14:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171694.13 sec. 2021-02-12T23:14:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:14:55Z- TNavBF::mmse selected from filter -1 2021-02-12T23:14:55Z- variances([0], [2], and psi_berg) = 788.160017 833.080334 0.000000 2021-02-12T23:14:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:14:55Z- LcmTrn::publishEstimates() - MLE : 4056494.94 590606.04 16.98 2021-02-12T23:14:55Z- LcmTrn::publishEstimates() - MMSE : 4056520.44 590650.54 16.98 2021-02-12T23:14:55Z- LcmTrn::publishEstimates() - COVAR: 788.16 833.08 0.84 0.00 2021-02-12T23:14:55Z- AHRS_M2 msg: 1613171695.30 epoch sec; seqNo:2037 2021-02-12T23:14:55Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:14:55Z- handleDepth()-Depth_Keller msg: 1613171695315 epoch sec; seqNo:2058; depth 16.84 2021-02-12T23:14:55Z- handleDvl() - RDI_Pathfinder msg: 1613171695.33 epoch sec; seqNo:866 2021-02-12T23:14:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.82 , 90.95 , 101.90 , 69.06 2021-02-12T23:14:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , 0.13 2021-02-12T23:14:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171695.34 epoch sec; seqNo:2058; 4056520.82 north; 590650.53 east 2021-02-12T23:14:55Z- AHRS_M2 msg: 1613171695.71 epoch sec; seqNo:2038 2021-02-12T23:14:55Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:14:55Z- handleDepth()-Depth_Keller msg: 1613171695717 epoch sec; seqNo:2059; depth 16.74 2021-02-12T23:14:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171695.73 epoch sec; seqNo:2059; 4056521.20 north; 590650.53 east 2021-02-12T23:14:56Z- AHRS_M2 msg: 1613171696.11 epoch sec; seqNo:2039 2021-02-12T23:14:56Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:14:56Z- handleDepth()-Depth_Keller msg: 1613171696118 epoch sec; seqNo:2060; depth 16.63 2021-02-12T23:14:56Z- handleDvl() - RDI_Pathfinder msg: 1613171696.13 epoch sec; seqNo:867 2021-02-12T23:14:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.23 , 92.10 , 103.05 , 69.46 2021-02-12T23:14:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , 0.10 2021-02-12T23:14:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171696.16 epoch sec; seqNo:2060; 4056521.58 north; 590650.53 east 2021-02-12T23:14:56Z- AHRS_M2 msg: 1613171696.52 epoch sec; seqNo:2040 2021-02-12T23:14:56Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:14:56Z- handleDepth()-Depth_Keller msg: 1613171696525 epoch sec; seqNo:2061; depth 16.51 2021-02-12T23:14:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171696.54 epoch sec; seqNo:2061; 4056521.59 north; 590651.75 east 2021-02-12T23:14:56Z- AHRS_M2 msg: 1613171696.92 epoch sec; seqNo:2041 2021-02-12T23:14:56Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-12T23:14:56Z- handleDepth()-Depth_Keller msg: 1613171696933 epoch sec; seqNo:2062; depth 16.42 2021-02-12T23:14:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171696.94 epoch sec; seqNo:2062; 4056521.97 north; 590651.74 east 2021-02-12T23:14:57Z- AHRS_M2 msg: 1613171697.32 epoch sec; seqNo:2042 2021-02-12T23:14:57Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-12T23:14:57Z- handleDepth()-Depth_Keller msg: 1613171697333 epoch sec; seqNo:2063; depth 16.30 2021-02-12T23:14:57Z- handleDvl() - RDI_Pathfinder msg: 1613171697.35 epoch sec; seqNo:868 2021-02-12T23:14:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.82 , 93.32 , 103.05 , 71.10 2021-02-12T23:14:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , 0.10 2021-02-12T23:14:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171697.36 epoch sec; seqNo:2063; 4056522.35 north; 590651.74 east 2021-02-12T23:14:57Z- AHRS_M2 msg: 1613171697.73 epoch sec; seqNo:2043 2021-02-12T23:14:57Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-12T23:14:57Z- handleDepth()-Depth_Keller msg: 1613171697732 epoch sec; seqNo:2064; depth 16.17 2021-02-12T23:14:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171697.74 epoch sec; seqNo:2064; 4056522.35 north; 590651.74 east 2021-02-12T23:14:58Z- handleDepth()-Depth_Keller msg: 1613171698157 epoch sec; seqNo:2065; depth 16.08 2021-02-12T23:14:58Z- handleDvl() - RDI_Pathfinder msg: 1613171698.17 epoch sec; seqNo:869 2021-02-12T23:14:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.23 , 94.46 , 104.20 , 70.69 2021-02-12T23:14:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , 0.09 2021-02-12T23:14:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171698.18 epoch sec; seqNo:2065; 4056522.73 north; 590651.73 east 2021-02-12T23:14:58Z- AHRS_M2 msg: 1613171698.54 epoch sec; seqNo:2044 2021-02-12T23:14:58Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:14:58Z- handleDepth()-Depth_Keller msg: 1613171698549 epoch sec; seqNo:2066; depth 15.95 2021-02-12T23:14:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171698.56 epoch sec; seqNo:2066; 4056523.10 north; 590651.73 east 2021-02-12T23:14:58Z- AHRS_M2 msg: 1613171698.94 epoch sec; seqNo:2045 2021-02-12T23:14:58Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:14:58Z- handleDepth()-Depth_Keller msg: 1613171698944 epoch sec; seqNo:2067; depth 15.84 2021-02-12T23:14:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171698.96 epoch sec; seqNo:2067; 4056523.50 north; 590652.95 east 2021-02-12T23:14:59Z- AHRS_M2 msg: 1613171699.34 epoch sec; seqNo:2046 2021-02-12T23:14:59Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:14:59Z- handleDepth()-Depth_Keller msg: 1613171699352 epoch sec; seqNo:2068; depth 15.74 2021-02-12T23:14:59Z- handleDvl() - RDI_Pathfinder msg: 1613171699.39 epoch sec; seqNo:870 2021-02-12T23:14:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.64 , 94.40 , 106.56 , 70.28 2021-02-12T23:14:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.05 , 0.04 2021-02-12T23:14:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171699.40 epoch sec; seqNo:2068; 4056523.50 north; 590652.95 east 2021-02-12T23:14:59Z- AHRS_M2 msg: 1613171699.75 epoch sec; seqNo:2047 2021-02-12T23:14:59Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-12T23:14:59Z- handleDepth()-Depth_Keller msg: 1613171699752 epoch sec; seqNo:2069; depth 15.63 2021-02-12T23:14:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171699.76 epoch sec; seqNo:2069; 4056523.87 north; 590652.94 east 2021-02-12T23:15:00Z- AHRS_M2 msg: 1613171700.15 epoch sec; seqNo:2048 2021-02-12T23:15:00Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-12T23:15:00Z- handleDepth()-Depth_Keller msg: 1613171700165 epoch sec; seqNo:2070; depth 15.51 2021-02-12T23:15:00Z- handleDvl() - RDI_Pathfinder msg: 1613171700.18 epoch sec; seqNo:871 2021-02-12T23:15:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 94.33 , 105.28 , 70.28 2021-02-12T23:15:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.06 , -0.01 2021-02-12T23:15:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171700.19 epoch sec; seqNo:2070; 4056524.25 north; 590652.94 east 2021-02-12T23:15:00Z- LcmTrn::handleMessages() - handling 44 messages took 0.63 ms and 132 calls... 2021-02-12T23:15:00Z- waiting for 1613171697.91, time is 1613171700.19 2021-02-12T23:15:00Z- LcmTrn::updateTrn() equating pose 1613171700.19 and meas times 1613171700.19 2021-02-12T23:15:00Z- LcmTrn::updateTrn() >>>> pose time 1613171700.19, meas time 1613171700.19 <<<< 2021-02-12T23:15:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171700.19 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:15:00Z- TerrainNav::motionUpdate() - currEstimate = (4056524, 590652.94, 15.51) 2021-02-12T23:15:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:15:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:15:00Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171700.19 sec.; No relevant navigation data available 2021-02-12T23:15:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:15:00Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:00Z- variances([0], [2], and psi_berg) = 787.972355 833.080020 0.000000 2021-02-12T23:15:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:15:00Z- LcmTrn::publishEstimates() - MLE : 4056498.75 590608.44 15.51 2021-02-12T23:15:00Z- LcmTrn::publishEstimates() - MMSE : 4056524.25 590652.94 15.51 2021-02-12T23:15:00Z- LcmTrn::publishEstimates() - COVAR: 787.97 833.08 1.07 0.00 2021-02-12T23:15:00Z- AHRS_M2 msg: 1613171700.56 epoch sec; seqNo:2049 2021-02-12T23:15:00Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-12T23:15:00Z- handleDepth()-Depth_Keller msg: 1613171700570 epoch sec; seqNo:2071; depth 15.39 2021-02-12T23:15:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171700.58 epoch sec; seqNo:2071; 4056524.25 north; 590652.94 east 2021-02-12T23:15:00Z- AHRS_M2 msg: 1613171700.96 epoch sec; seqNo:2050 2021-02-12T23:15:00Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-12T23:15:00Z- handleDepth()-Depth_Keller msg: 1613171700964 epoch sec; seqNo:2072; depth 15.30 2021-02-12T23:15:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171700.97 epoch sec; seqNo:2072; 4056524.65 north; 590654.16 east 2021-02-12T23:15:01Z- AHRS_M2 msg: 1613171701.36 epoch sec; seqNo:2051 2021-02-12T23:15:01Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-12T23:15:01Z- handleDepth()-Depth_Keller msg: 1613171701367 epoch sec; seqNo:2073; depth 15.19 2021-02-12T23:15:01Z- handleDvl() - RDI_Pathfinder msg: 1613171701.38 epoch sec; seqNo:872 2021-02-12T23:15:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 95.61 , 106.56 , 71.10 2021-02-12T23:15:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , -0.03 2021-02-12T23:15:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171701.39 epoch sec; seqNo:2073; 4056525.02 north; 590654.15 east 2021-02-12T23:15:01Z- AHRS_M2 msg: 1613171701.78 epoch sec; seqNo:2052 2021-02-12T23:15:01Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-12T23:15:01Z- handleDepth()-Depth_Keller msg: 1613171701785 epoch sec; seqNo:2074; depth 15.06 2021-02-12T23:15:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171701.80 epoch sec; seqNo:2074; 4056525.40 north; 590654.15 east 2021-02-12T23:15:02Z- AHRS_M2 msg: 1613171702.17 epoch sec; seqNo:2053 2021-02-12T23:15:02Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:15:02Z- handleDepth()-Depth_Keller msg: 1613171702176 epoch sec; seqNo:2075; depth 14.96 2021-02-12T23:15:02Z- handleDvl() - RDI_Pathfinder msg: 1613171702.19 epoch sec; seqNo:873 2021-02-12T23:15:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 95.48 , 106.43 , 69.87 2021-02-12T23:15:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , -0.02 2021-02-12T23:15:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171702.21 epoch sec; seqNo:2075; 4056525.40 north; 590654.15 east 2021-02-12T23:15:02Z- AHRS_M2 msg: 1613171702.58 epoch sec; seqNo:2054 2021-02-12T23:15:02Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:15:02Z- handleDepth()-Depth_Keller msg: 1613171702585 epoch sec; seqNo:2076; depth 14.85 2021-02-12T23:15:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171702.60 epoch sec; seqNo:2076; 4056525.78 north; 590654.14 east 2021-02-12T23:15:02Z- AHRS_M2 msg: 1613171702.98 epoch sec; seqNo:2055 2021-02-12T23:15:02Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-12T23:15:02Z- handleDepth()-Depth_Keller msg: 1613171702986 epoch sec; seqNo:2077; depth 14.74 2021-02-12T23:15:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171703.00 epoch sec; seqNo:2077; 4056526.16 north; 590654.14 east 2021-02-12T23:15:03Z- AHRS_M2 msg: 1613171703.38 epoch sec; seqNo:2056 2021-02-12T23:15:03Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-12T23:15:03Z- handleDepth()-Depth_Keller msg: 1613171703388 epoch sec; seqNo:2078; depth 14.62 2021-02-12T23:15:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171703.41 epoch sec; seqNo:2078; 4056526.16 north; 590654.14 east 2021-02-12T23:15:03Z- AHRS_M2 msg: 1613171703.79 epoch sec; seqNo:2057 2021-02-12T23:15:03Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.15 psi 2021-02-12T23:15:03Z- handleDepth()-Depth_Keller msg: 1613171703793 epoch sec; seqNo:2079; depth 14.50 2021-02-12T23:15:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171703.81 epoch sec; seqNo:2079; 4056526.55 north; 590655.36 east 2021-02-12T23:15:04Z- AHRS_M2 msg: 1613171704.19 epoch sec; seqNo:2058 2021-02-12T23:15:04Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.15 psi 2021-02-12T23:15:04Z- handleDepth()-Depth_Keller msg: 1613171704197 epoch sec; seqNo:2080; depth 14.39 2021-02-12T23:15:04Z- handleDvl() - RDI_Pathfinder msg: 1613171704.21 epoch sec; seqNo:874 2021-02-12T23:15:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.68 , 94.26 , 106.43 , 72.32 2021-02-12T23:15:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , -0.09 2021-02-12T23:15:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171704.22 epoch sec; seqNo:2080; 4056526.93 north; 590655.35 east 2021-02-12T23:15:04Z- AHRS_M2 msg: 1613171704.60 epoch sec; seqNo:2059 2021-02-12T23:15:04Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.14 psi 2021-02-12T23:15:04Z- handleDepth()-Depth_Keller msg: 1613171704602 epoch sec; seqNo:2081; depth 14.29 2021-02-12T23:15:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171704.61 epoch sec; seqNo:2081; 4056526.93 north; 590655.35 east 2021-02-12T23:15:05Z- AHRS_M2 msg: 1613171705.00 epoch sec; seqNo:2060 2021-02-12T23:15:05Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-12T23:15:05Z- handleDepth()-Depth_Keller msg: 1613171705004 epoch sec; seqNo:2082; depth 14.18 2021-02-12T23:15:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171705.01 epoch sec; seqNo:2082; 4056527.31 north; 590655.35 east 2021-02-12T23:15:05Z- AHRS_M2 msg: 1613171705.40 epoch sec; seqNo:2061 2021-02-12T23:15:05Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-12T23:15:05Z- LcmTrn::handleMessages() - handling 40 messages took 0.65 ms and 127 calls... 2021-02-12T23:15:05Z- waiting for 1613171703.19, time is 1613171705.40 2021-02-12T23:15:05Z- LcmTrn::updateTrn() >>>> pose time 1613171705.40, meas time 1613171704.21 <<<< 2021-02-12T23:15:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171704.21 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:15:05Z- TerrainNav::motionUpdate() - currEstimate = (4056527, 590655.35, 14.18) 2021-02-12T23:15:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:15:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:15:05Z- TNavBF::Projecting Measurements 2021-02-12T23:15:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:15:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:15:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:15:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:15:05Z- TNavFilter::4 good beams. 2021-02-12T23:15:05Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:15:05Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:15:05Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:15:05Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:15:05Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:15:05Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:15:05Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:15:05Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:05Z- TerrainMapOctree: (vehN, vehE ) = (4056526.61, 590654.80). 2021-02-12T23:15:05Z- TerrainMapOctree: Min Distance = 5935.98. 2021-02-12T23:15:05Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:15:05Z- There is no specific good beam that all particles have in common. 2021-02-12T23:15:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:15:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 10.16 2021-02-12T23:15:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:15:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 14.22 2021-02-12T23:15:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:15:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 18.12 2021-02-12T23:15:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:15:05Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-12T23:15:05Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:15:05Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:15:05Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:15:05Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:15:05Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:15:05Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:15:05Z- Filter 0 No beams used in subcloud update 2021-02-12T23:15:05Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:15:05Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:15:05Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 787.8 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:15:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171704.21 sec. 2021-02-12T23:15:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:15:05Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:05Z- variances([0], [2], and psi_berg) = 787.736852 833.079628 0.000000 2021-02-12T23:15:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:15:05Z- LcmTrn::publishEstimates() - MLE : 4056501.81 590610.85 14.18 2021-02-12T23:15:05Z- LcmTrn::publishEstimates() - MMSE : 4056527.31 590655.35 14.18 2021-02-12T23:15:05Z- LcmTrn::publishEstimates() - COVAR: 787.74 833.08 0.94 0.00 2021-02-12T23:15:05Z- handleDepth()-Depth_Keller msg: 1613171705408 epoch sec; seqNo:2083; depth 14.07 2021-02-12T23:15:05Z- handleDvl() - RDI_Pathfinder msg: 1613171705.42 epoch sec; seqNo:875 2021-02-12T23:15:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.66 , 95.48 , 106.43 , 71.51 2021-02-12T23:15:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , -0.15 2021-02-12T23:15:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171705.43 epoch sec; seqNo:2083; 4056527.69 north; 590655.35 east 2021-02-12T23:15:05Z- AHRS_M2 msg: 1613171705.81 epoch sec; seqNo:2062 2021-02-12T23:15:05Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.14 psi 2021-02-12T23:15:05Z- handleDepth()-Depth_Keller msg: 1613171705814 epoch sec; seqNo:2084; depth 13.95 2021-02-12T23:15:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171705.82 epoch sec; seqNo:2084; 4056527.69 north; 590655.35 east 2021-02-12T23:15:06Z- AHRS_M2 msg: 1613171706.21 epoch sec; seqNo:2063 2021-02-12T23:15:06Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.14 psi 2021-02-12T23:15:06Z- handleDepth()-Depth_Keller msg: 1613171706216 epoch sec; seqNo:2085; depth 13.85 2021-02-12T23:15:06Z- handleDvl() - RDI_Pathfinder msg: 1613171706.24 epoch sec; seqNo:876 2021-02-12T23:15:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.66 , 94.26 , 107.64 , 72.32 2021-02-12T23:15:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.13 , -0.15 2021-02-12T23:15:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171706.25 epoch sec; seqNo:2085; 4056528.07 north; 590655.34 east 2021-02-12T23:15:06Z- AHRS_M2 msg: 1613171706.62 epoch sec; seqNo:2064 2021-02-12T23:15:06Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.15 psi 2021-02-12T23:15:06Z- handleDepth()-Depth_Keller msg: 1613171706629 epoch sec; seqNo:2086; depth 13.73 2021-02-12T23:15:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171706.65 epoch sec; seqNo:2086; 4056528.46 north; 590656.56 east 2021-02-12T23:15:07Z- AHRS_M2 msg: 1613171707.02 epoch sec; seqNo:2065 2021-02-12T23:15:07Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.16 psi 2021-02-12T23:15:07Z- handleDepth()-Depth_Keller msg: 1613171707028 epoch sec; seqNo:2087; depth 13.61 2021-02-12T23:15:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171707.04 epoch sec; seqNo:2087; 4056528.46 north; 590656.56 east 2021-02-12T23:15:07Z- AHRS_M2 msg: 1613171707.42 epoch sec; seqNo:2066 2021-02-12T23:15:07Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.17 psi 2021-02-12T23:15:07Z- handleDepth()-Depth_Keller msg: 1613171707431 epoch sec; seqNo:2088; depth 13.51 2021-02-12T23:15:07Z- handleDvl() - RDI_Pathfinder msg: 1613171707.44 epoch sec; seqNo:877 2021-02-12T23:15:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.68 , 94.26 , 106.43 , 71.92 2021-02-12T23:15:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.25 , -0.04 2021-02-12T23:15:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171707.45 epoch sec; seqNo:2088; 4056528.84 north; 590656.55 east 2021-02-12T23:15:07Z- AHRS_M2 msg: 1613171707.83 epoch sec; seqNo:2067 2021-02-12T23:15:07Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.19 psi 2021-02-12T23:15:07Z- handleDepth()-Depth_Keller msg: 1613171707832 epoch sec; seqNo:2089; depth 13.39 2021-02-12T23:15:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171707.84 epoch sec; seqNo:2089; 4056529.22 north; 590656.55 east 2021-02-12T23:15:08Z- AHRS_M2 msg: 1613171708.23 epoch sec; seqNo:2068 2021-02-12T23:15:08Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.21 psi 2021-02-12T23:15:08Z- handleDepth()-Depth_Keller msg: 1613171708236 epoch sec; seqNo:2090; depth 13.27 2021-02-12T23:15:08Z- handleDvl() - RDI_Pathfinder msg: 1613171708.26 epoch sec; seqNo:878 2021-02-12T23:15:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 96.69 , 106.43 , 72.32 2021-02-12T23:15:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.27 , 0.18 2021-02-12T23:15:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171708.27 epoch sec; seqNo:2090; 4056529.60 north; 590656.55 east 2021-02-12T23:15:08Z- AHRS_M2 msg: 1613171708.64 epoch sec; seqNo:2069 2021-02-12T23:15:08Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.23 psi 2021-02-12T23:15:08Z- handleDepth()-Depth_Keller msg: 1613171708640 epoch sec; seqNo:2091; depth 13.16 2021-02-12T23:15:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171708.65 epoch sec; seqNo:2091; 4056529.60 north; 590656.55 east 2021-02-12T23:15:09Z- AHRS_M2 msg: 1613171709.05 epoch sec; seqNo:2070 2021-02-12T23:15:09Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.24 psi 2021-02-12T23:15:09Z- handleDepth()-Depth_Keller msg: 1613171709063 epoch sec; seqNo:2092; depth 13.05 2021-02-12T23:15:09Z- handleDvl() - RDI_Pathfinder msg: 1613171709.09 epoch sec; seqNo:879 2021-02-12T23:15:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 96.69 , 112.51 , 74.37 2021-02-12T23:15:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.17 , 0.35 2021-02-12T23:15:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171709.10 epoch sec; seqNo:2092; 4056529.97 north; 590656.54 east 2021-02-12T23:15:09Z- AHRS_M2 msg: 1613171709.44 epoch sec; seqNo:2071 2021-02-12T23:15:09Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.25 psi 2021-02-12T23:15:09Z- handleDepth()-Depth_Keller msg: 1613171709449 epoch sec; seqNo:2093; depth 12.91 2021-02-12T23:15:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171709.46 epoch sec; seqNo:2093; 4056530.35 north; 590656.54 east 2021-02-12T23:15:09Z- AHRS_M2 msg: 1613171709.85 epoch sec; seqNo:2072 2021-02-12T23:15:09Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.25 psi 2021-02-12T23:15:09Z- handleDepth()-Depth_Keller msg: 1613171709854 epoch sec; seqNo:2094; depth 12.80 2021-02-12T23:15:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171709.86 epoch sec; seqNo:2094; 4056530.73 north; 590656.53 east 2021-02-12T23:15:10Z- AHRS_M2 msg: 1613171710.25 epoch sec; seqNo:2073 2021-02-12T23:15:10Z- AHRS_M2 msg: 0.56 phi; 0.33 theta; -0.25 psi 2021-02-12T23:15:10Z- handleDepth()-Depth_Keller msg: 1613171710256 epoch sec; seqNo:2095; depth 12.69 2021-02-12T23:15:10Z- handleDvl() - RDI_Pathfinder msg: 1613171710.28 epoch sec; seqNo:880 2021-02-12T23:15:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 103.79 , 109.87 , 75.59 2021-02-12T23:15:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.02 , 0.37 2021-02-12T23:15:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171710.29 epoch sec; seqNo:2095; 4056531.12 north; 590657.75 east 2021-02-12T23:15:10Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-12T23:15:10Z- waiting for 1613171708.40, time is 1613171710.29 2021-02-12T23:15:10Z- LcmTrn::updateTrn() equating pose 1613171710.29 and meas times 1613171710.29 2021-02-12T23:15:10Z- LcmTrn::updateTrn() >>>> pose time 1613171710.29, meas time 1613171710.29 <<<< 2021-02-12T23:15:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171710.29 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:15:10Z- TerrainNav::motionUpdate() - currEstimate = (4056531, 590657.75, 12.69) 2021-02-12T23:15:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:15:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:15:10Z- TNavBF::Projecting Measurements 2021-02-12T23:15:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:15:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:15:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:15:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:15:10Z- TNavFilter::4 good beams. 2021-02-12T23:15:10Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:15:10Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:15:10Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:15:10Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:15:10Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:15:10Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:15:10Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:15:10Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:10Z- TerrainMapOctree: (vehN, vehE ) = (4056531.12, 590657.75). 2021-02-12T23:15:10Z- TerrainMapOctree: Min Distance = 5930.60. 2021-02-12T23:15:10Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:15:10Z- There is no specific good beam that all particles have in common. 2021-02-12T23:15:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:15:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 9.75 2021-02-12T23:15:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:15:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 17.24 2021-02-12T23:15:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:15:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 19.32 2021-02-12T23:15:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:15:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 9.14 2021-02-12T23:15:10Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:15:10Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:15:10Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:15:10Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:15:10Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:15:10Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:15:10Z- Filter 0 No beams used in subcloud update 2021-02-12T23:15:10Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:15:10Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:15:10Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 787.6 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:15:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171710.29 sec. 2021-02-12T23:15:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:15:10Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:10Z- variances([0], [2], and psi_berg) = 787.550685 833.079320 0.000000 2021-02-12T23:15:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:15:10Z- LcmTrn::publishEstimates() - MLE : 4056505.62 590613.25 12.69 2021-02-12T23:15:10Z- LcmTrn::publishEstimates() - MMSE : 4056531.12 590657.75 12.69 2021-02-12T23:15:10Z- LcmTrn::publishEstimates() - COVAR: 787.55 833.08 0.88 0.00 2021-02-12T23:15:10Z- AHRS_M2 msg: 1613171710.66 epoch sec; seqNo:2074 2021-02-12T23:15:10Z- AHRS_M2 msg: 0.56 phi; 0.33 theta; -0.24 psi 2021-02-12T23:15:10Z- handleDepth()-Depth_Keller msg: 1613171710660 epoch sec; seqNo:2096; depth 12.58 2021-02-12T23:15:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171710.68 epoch sec; seqNo:2096; 4056531.50 north; 590657.75 east 2021-02-12T23:15:11Z- AHRS_M2 msg: 1613171711.07 epoch sec; seqNo:2075 2021-02-12T23:15:11Z- AHRS_M2 msg: 0.55 phi; 0.35 theta; -0.21 psi 2021-02-12T23:15:11Z- handleDepth()-Depth_Keller msg: 1613171711071 epoch sec; seqNo:2097; depth 12.47 2021-02-12T23:15:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171711.09 epoch sec; seqNo:2097; 4056531.50 north; 590657.75 east 2021-02-12T23:15:11Z- AHRS_M2 msg: 1613171711.47 epoch sec; seqNo:2076 2021-02-12T23:15:11Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.19 psi 2021-02-12T23:15:11Z- handleDepth()-Depth_Keller msg: 1613171711481 epoch sec; seqNo:2098; depth 12.34 2021-02-12T23:15:11Z- handleDvl() - RDI_Pathfinder msg: 1613171711.49 epoch sec; seqNo:881 2021-02-12T23:15:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.14 , 96.36 , 111.23 , 75.59 2021-02-12T23:15:11Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.17 , 0.23 2021-02-12T23:15:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171711.50 epoch sec; seqNo:2098; 4056531.88 north; 590657.74 east 2021-02-12T23:15:11Z- AHRS_M2 msg: 1613171711.87 epoch sec; seqNo:2077 2021-02-12T23:15:11Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.17 psi 2021-02-12T23:15:11Z- handleDepth()-Depth_Keller msg: 1613171711872 epoch sec; seqNo:2099; depth 12.24 2021-02-12T23:15:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171711.89 epoch sec; seqNo:2099; 4056532.27 north; 590658.96 east 2021-02-12T23:15:12Z- AHRS_M2 msg: 1613171712.28 epoch sec; seqNo:2078 2021-02-12T23:15:12Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.15 psi 2021-02-12T23:15:12Z- handleDepth()-Depth_Keller msg: 1613171712285 epoch sec; seqNo:2100; depth 12.13 2021-02-12T23:15:12Z- handleDvl() - RDI_Pathfinder msg: 1613171712.31 epoch sec; seqNo:882 2021-02-12T23:15:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 96.49 , 108.66 , 74.37 2021-02-12T23:15:12Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.26 , -0.00 2021-02-12T23:15:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171712.32 epoch sec; seqNo:2100; 4056532.65 north; 590658.96 east 2021-02-12T23:15:12Z- AHRS_M2 msg: 1613171712.68 epoch sec; seqNo:2079 2021-02-12T23:15:12Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.13 psi 2021-02-12T23:15:12Z- handleDepth()-Depth_Keller msg: 1613171712680 epoch sec; seqNo:2101; depth 12.03 2021-02-12T23:15:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171712.69 epoch sec; seqNo:2101; 4056532.65 north; 590658.96 east 2021-02-12T23:15:13Z- AHRS_M2 msg: 1613171713.08 epoch sec; seqNo:2080 2021-02-12T23:15:13Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.12 psi 2021-02-12T23:15:13Z- handleDepth()-Depth_Keller msg: 1613171713104 epoch sec; seqNo:2102; depth 11.91 2021-02-12T23:15:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171713.12 epoch sec; seqNo:2102; 4056533.03 north; 590658.95 east 2021-02-12T23:15:13Z- AHRS_M2 msg: 1613171713.48 epoch sec; seqNo:2081 2021-02-12T23:15:13Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.11 psi 2021-02-12T23:15:13Z- handleDepth()-Depth_Keller msg: 1613171713493 epoch sec; seqNo:2103; depth 11.79 2021-02-12T23:15:13Z- handleDvl() - RDI_Pathfinder msg: 1613171713.51 epoch sec; seqNo:883 2021-02-12T23:15:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.76 , 96.49 , 112.31 , 74.37 2021-02-12T23:15:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.20 , -0.24 2021-02-12T23:15:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171713.52 epoch sec; seqNo:2103; 4056533.41 north; 590658.95 east 2021-02-12T23:15:13Z- AHRS_M2 msg: 1613171713.89 epoch sec; seqNo:2082 2021-02-12T23:15:13Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.11 psi 2021-02-12T23:15:13Z- handleDepth()-Depth_Keller msg: 1613171713892 epoch sec; seqNo:2104; depth 11.67 2021-02-12T23:15:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171713.90 epoch sec; seqNo:2104; 4056533.42 north; 590660.17 east 2021-02-12T23:15:14Z- AHRS_M2 msg: 1613171714.29 epoch sec; seqNo:2083 2021-02-12T23:15:14Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.11 psi 2021-02-12T23:15:14Z- handleDepth()-Depth_Keller msg: 1613171714298 epoch sec; seqNo:2105; depth 11.56 2021-02-12T23:15:14Z- handleDvl() - RDI_Pathfinder msg: 1613171714.31 epoch sec; seqNo:884 2021-02-12T23:15:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 93.65 , 109.87 , 73.96 2021-02-12T23:15:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.04 , -0.34 2021-02-12T23:15:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171714.32 epoch sec; seqNo:2105; 4056533.80 north; 590660.17 east 2021-02-12T23:15:14Z- AHRS_M2 msg: 1613171714.70 epoch sec; seqNo:2084 2021-02-12T23:15:14Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.12 psi 2021-02-12T23:15:14Z- handleDepth()-Depth_Keller msg: 1613171714702 epoch sec; seqNo:2106; depth 11.44 2021-02-12T23:15:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171714.71 epoch sec; seqNo:2106; 4056533.80 north; 590660.17 east 2021-02-12T23:15:15Z- AHRS_M2 msg: 1613171715.10 epoch sec; seqNo:2085 2021-02-12T23:15:15Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.13 psi 2021-02-12T23:15:15Z- handleDepth()-Depth_Keller msg: 1613171715109 epoch sec; seqNo:2107; depth 11.32 2021-02-12T23:15:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171715.15 epoch sec; seqNo:2107; 4056534.18 north; 590660.16 east 2021-02-12T23:15:15Z- AHRS_M2 msg: 1613171715.51 epoch sec; seqNo:2086 2021-02-12T23:15:15Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.14 psi 2021-02-12T23:15:15Z- handleDepth()-Depth_Keller msg: 1613171715516 epoch sec; seqNo:2108; depth 11.16 2021-02-12T23:15:15Z- handleDvl() - RDI_Pathfinder msg: 1613171715.54 epoch sec; seqNo:885 2021-02-12T23:15:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 95.28 , 111.09 , 73.96 2021-02-12T23:15:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.27 , -0.19 2021-02-12T23:15:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171715.56 epoch sec; seqNo:2108; 4056534.56 north; 590660.16 east 2021-02-12T23:15:15Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 131 calls... 2021-02-12T23:15:15Z- waiting for 1613171713.29, time is 1613171715.56 2021-02-12T23:15:15Z- LcmTrn::updateTrn() equating pose 1613171715.56 and meas times 1613171715.56 2021-02-12T23:15:15Z- LcmTrn::updateTrn() >>>> pose time 1613171715.56, meas time 1613171715.56 <<<< 2021-02-12T23:15:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171715.56 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:15:15Z- TerrainNav::motionUpdate() - currEstimate = (4056535, 590660.16, 11.16) 2021-02-12T23:15:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:15:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:15:15Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171715.56 sec.; No relevant navigation data available 2021-02-12T23:15:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:15:15Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:15Z- variances([0], [2], and psi_berg) = 787.371736 833.079024 0.000000 2021-02-12T23:15:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:15:15Z- LcmTrn::publishEstimates() - MLE : 4056509.06 590615.66 11.16 2021-02-12T23:15:15Z- LcmTrn::publishEstimates() - MMSE : 4056534.56 590660.16 11.16 2021-02-12T23:15:15Z- LcmTrn::publishEstimates() - COVAR: 787.37 833.08 1.10 0.00 2021-02-12T23:15:15Z- AHRS_M2 msg: 1613171715.91 epoch sec; seqNo:2087 2021-02-12T23:15:15Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-12T23:15:15Z- handleDepth()-Depth_Keller msg: 1613171715912 epoch sec; seqNo:2109; depth 11.04 2021-02-12T23:15:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171715.93 epoch sec; seqNo:2109; 4056534.56 north; 590660.16 east 2021-02-12T23:15:16Z- AHRS_M2 msg: 1613171716.31 epoch sec; seqNo:2088 2021-02-12T23:15:16Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:15:16Z- handleDepth()-Depth_Keller msg: 1613171716321 epoch sec; seqNo:2110; depth 10.94 2021-02-12T23:15:16Z- handleDvl() - RDI_Pathfinder msg: 1613171716.34 epoch sec; seqNo:886 2021-02-12T23:15:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.54 , 96.49 , 111.09 , 73.96 2021-02-12T23:15:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.29 , 0.06 2021-02-12T23:15:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171716.35 epoch sec; seqNo:2110; 4056534.94 north; 590660.15 east 2021-02-12T23:15:16Z- AHRS_M2 msg: 1613171716.72 epoch sec; seqNo:2089 2021-02-12T23:15:16Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-12T23:15:16Z- handleDepth()-Depth_Keller msg: 1613171716725 epoch sec; seqNo:2111; depth 10.81 2021-02-12T23:15:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171716.74 epoch sec; seqNo:2111; 4056535.32 north; 590660.15 east 2021-02-12T23:15:17Z- AHRS_M2 msg: 1613171717.13 epoch sec; seqNo:2090 2021-02-12T23:15:17Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-12T23:15:17Z- handleDepth()-Depth_Keller msg: 1613171717134 epoch sec; seqNo:2112; depth 10.67 2021-02-12T23:15:17Z- handleDvl() - RDI_Pathfinder msg: 1613171717.15 epoch sec; seqNo:887 2021-02-12T23:15:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 96.36 , 113.93 , 76.00 2021-02-12T23:15:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.16 , 0.17 2021-02-12T23:15:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171717.16 epoch sec; seqNo:2112; 4056535.70 north; 590660.15 east 2021-02-12T23:15:17Z- AHRS_M2 msg: 1613171717.52 epoch sec; seqNo:2091 2021-02-12T23:15:17Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-12T23:15:17Z- handleDepth()-Depth_Keller msg: 1613171717537 epoch sec; seqNo:2113; depth 10.57 2021-02-12T23:15:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171717.55 epoch sec; seqNo:2113; 4056536.08 north; 590660.14 east 2021-02-12T23:15:17Z- AHRS_M2 msg: 1613171717.94 epoch sec; seqNo:2092 2021-02-12T23:15:17Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-12T23:15:17Z- handleDepth()-Depth_Keller msg: 1613171717948 epoch sec; seqNo:2114; depth 10.47 2021-02-12T23:15:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171717.96 epoch sec; seqNo:2114; 4056536.09 north; 590661.36 east 2021-02-12T23:15:18Z- AHRS_M2 msg: 1613171718.33 epoch sec; seqNo:2093 2021-02-12T23:15:18Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-12T23:15:18Z- handleDepth()-Depth_Keller msg: 1613171718338 epoch sec; seqNo:2115; depth 10.32 2021-02-12T23:15:18Z- handleDvl() - RDI_Pathfinder msg: 1613171718.35 epoch sec; seqNo:888 2021-02-12T23:15:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 96.29 , 111.16 , 75.59 2021-02-12T23:15:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.03 , 0.11 2021-02-12T23:15:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171718.38 epoch sec; seqNo:2115; 4056536.47 north; 590661.36 east 2021-02-12T23:15:18Z- AHRS_M2 msg: 1613171718.74 epoch sec; seqNo:2094 2021-02-12T23:15:18Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-12T23:15:18Z- handleDepth()-Depth_Keller msg: 1613171718740 epoch sec; seqNo:2116; depth 10.22 2021-02-12T23:15:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171718.75 epoch sec; seqNo:2116; 4056536.85 north; 590661.35 east 2021-02-12T23:15:19Z- AHRS_M2 msg: 1613171719.14 epoch sec; seqNo:2095 2021-02-12T23:15:19Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.14 psi 2021-02-12T23:15:19Z- handleDepth()-Depth_Keller msg: 1613171719149 epoch sec; seqNo:2117; depth 10.12 2021-02-12T23:15:19Z- handleDvl() - RDI_Pathfinder msg: 1613171719.16 epoch sec; seqNo:889 2021-02-12T23:15:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 96.36 , 113.93 , 74.37 2021-02-12T23:15:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.04 , -0.02 2021-02-12T23:15:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171719.17 epoch sec; seqNo:2117; 4056536.85 north; 590661.35 east 2021-02-12T23:15:19Z- AHRS_M2 msg: 1613171719.54 epoch sec; seqNo:2096 2021-02-12T23:15:19Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.14 psi 2021-02-12T23:15:19Z- handleDepth()-Depth_Keller msg: 1613171719548 epoch sec; seqNo:2118; depth 9.99 2021-02-12T23:15:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171719.57 epoch sec; seqNo:2118; 4056537.22 north; 590661.35 east 2021-02-12T23:15:19Z- AHRS_M2 msg: 1613171719.95 epoch sec; seqNo:2097 2021-02-12T23:15:19Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.15 psi 2021-02-12T23:15:19Z- handleDepth()-Depth_Keller msg: 1613171719952 epoch sec; seqNo:2119; depth 9.87 2021-02-12T23:15:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171719.96 epoch sec; seqNo:2119; 4056537.60 north; 590661.35 east 2021-02-12T23:15:20Z- AHRS_M2 msg: 1613171720.35 epoch sec; seqNo:2098 2021-02-12T23:15:20Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:15:20Z- handleDepth()-Depth_Keller msg: 1613171720356 epoch sec; seqNo:2120; depth 9.76 2021-02-12T23:15:20Z- handleDvl() - RDI_Pathfinder msg: 1613171720.37 epoch sec; seqNo:890 2021-02-12T23:15:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 96.36 , 112.58 , 76.41 2021-02-12T23:15:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.19 , -0.06 2021-02-12T23:15:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171720.38 epoch sec; seqNo:2120; 4056537.60 north; 590661.35 east 2021-02-12T23:15:20Z- LcmTrn::handleMessages() - handling 41 messages took 0.65 ms and 128 calls... 2021-02-12T23:15:20Z- waiting for 1613171718.56, time is 1613171720.38 2021-02-12T23:15:20Z- LcmTrn::updateTrn() equating pose 1613171720.38 and meas times 1613171720.38 2021-02-12T23:15:20Z- LcmTrn::updateTrn() >>>> pose time 1613171720.38, meas time 1613171720.38 <<<< 2021-02-12T23:15:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171720.38 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:15:20Z- TerrainNav::motionUpdate() - currEstimate = (4056538, 590661.35, 9.76) 2021-02-12T23:15:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:15:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:15:20Z- TNavBF::Projecting Measurements 2021-02-12T23:15:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:15:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:15:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:15:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:15:20Z- TNavFilter::4 good beams. 2021-02-12T23:15:20Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:15:20Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:15:20Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:15:20Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:15:20Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:15:20Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:15:20Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:15:20Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:20Z- TerrainMapOctree: (vehN, vehE ) = (4056537.60, 590661.35). 2021-02-12T23:15:20Z- TerrainMapOctree: Min Distance = 5923.27. 2021-02-12T23:15:20Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:15:20Z- There is no specific good beam that all particles have in common. 2021-02-12T23:15:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:15:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 11.34 2021-02-12T23:15:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:15:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 14.86 2021-02-12T23:15:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:15:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 20.28 2021-02-12T23:15:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:15:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 9.34 2021-02-12T23:15:20Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:15:20Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:15:20Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:15:20Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:15:20Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:15:20Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:15:20Z- Filter 0 No beams used in subcloud update 2021-02-12T23:15:20Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:15:20Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:15:20Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 787.2 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:15:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171720.38 sec. 2021-02-12T23:15:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:15:20Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:20Z- variances([0], [2], and psi_berg) = 787.214317 833.078765 0.000000 2021-02-12T23:15:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:15:20Z- LcmTrn::publishEstimates() - MLE : 4056512.10 590616.85 9.76 2021-02-12T23:15:20Z- LcmTrn::publishEstimates() - MMSE : 4056537.60 590661.35 9.76 2021-02-12T23:15:20Z- LcmTrn::publishEstimates() - COVAR: 787.21 833.08 0.94 0.00 2021-02-12T23:15:20Z- AHRS_M2 msg: 1613171720.76 epoch sec; seqNo:2099 2021-02-12T23:15:20Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:15:20Z- handleDepth()-Depth_Keller msg: 1613171720761 epoch sec; seqNo:2121; depth 9.62 2021-02-12T23:15:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171720.77 epoch sec; seqNo:2121; 4056537.98 north; 590661.34 east 2021-02-12T23:15:21Z- AHRS_M2 msg: 1613171721.16 epoch sec; seqNo:2100 2021-02-12T23:15:21Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:15:21Z- handleDepth()-Depth_Keller msg: 1613171721164 epoch sec; seqNo:2122; depth 9.50 2021-02-12T23:15:21Z- handleDvl() - RDI_Pathfinder msg: 1613171721.18 epoch sec; seqNo:891 2021-02-12T23:15:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 96.36 , 113.93 , 76.00 2021-02-12T23:15:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.28 , 0.10 2021-02-12T23:15:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171721.19 epoch sec; seqNo:2122; 4056538.36 north; 590661.34 east 2021-02-12T23:15:21Z- AHRS_M2 msg: 1613171721.56 epoch sec; seqNo:2101 2021-02-12T23:15:21Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-12T23:15:21Z- handleDepth()-Depth_Keller msg: 1613171721568 epoch sec; seqNo:2123; depth 9.39 2021-02-12T23:15:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171721.58 epoch sec; seqNo:2123; 4056538.75 north; 590662.56 east 2021-02-12T23:15:21Z- AHRS_M2 msg: 1613171721.97 epoch sec; seqNo:2102 2021-02-12T23:15:21Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-12T23:15:21Z- handleDepth()-Depth_Keller msg: 1613171721972 epoch sec; seqNo:2124; depth 9.29 2021-02-12T23:15:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171721.98 epoch sec; seqNo:2124; 4056539.13 north; 590662.55 east 2021-02-12T23:15:22Z- handleDepth()-Depth_Keller msg: 1613171722393 epoch sec; seqNo:2125; depth 9.17 2021-02-12T23:15:22Z- handleDvl() - RDI_Pathfinder msg: 1613171722.40 epoch sec; seqNo:892 2021-02-12T23:15:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.83 , 96.29 , 115.21 , 76.00 2021-02-12T23:15:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.16 , 0.26 2021-02-12T23:15:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171722.42 epoch sec; seqNo:2125; 4056539.51 north; 590662.55 east 2021-02-12T23:15:22Z- AHRS_M2 msg: 1613171722.78 epoch sec; seqNo:2103 2021-02-12T23:15:22Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.12 psi 2021-02-12T23:15:22Z- handleDepth()-Depth_Keller msg: 1613171722781 epoch sec; seqNo:2126; depth 9.02 2021-02-12T23:15:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171722.80 epoch sec; seqNo:2126; 4056539.51 north; 590662.55 east 2021-02-12T23:15:23Z- AHRS_M2 msg: 1613171723.18 epoch sec; seqNo:2104 2021-02-12T23:15:23Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.12 psi 2021-02-12T23:15:23Z- handleDepth()-Depth_Keller msg: 1613171723184 epoch sec; seqNo:2127; depth 8.93 2021-02-12T23:15:23Z- handleDvl() - RDI_Pathfinder msg: 1613171723.20 epoch sec; seqNo:893 2021-02-12T23:15:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.11 , 96.22 , 115.15 , 75.59 2021-02-12T23:15:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , 0.24 2021-02-12T23:15:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171723.21 epoch sec; seqNo:2127; 4056539.89 north; 590662.54 east 2021-02-12T23:15:23Z- AHRS_M2 msg: 1613171723.59 epoch sec; seqNo:2105 2021-02-12T23:15:23Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-12T23:15:23Z- handleDepth()-Depth_Keller msg: 1613171723589 epoch sec; seqNo:2128; depth 8.79 2021-02-12T23:15:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171723.60 epoch sec; seqNo:2128; 4056540.27 north; 590662.54 east 2021-02-12T23:15:23Z- AHRS_M2 msg: 1613171723.99 epoch sec; seqNo:2106 2021-02-12T23:15:23Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.14 psi 2021-02-12T23:15:23Z- handleDepth()-Depth_Keller msg: 1613171723992 epoch sec; seqNo:2129; depth 8.68 2021-02-12T23:15:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171724.01 epoch sec; seqNo:2129; 4056540.65 north; 590662.54 east 2021-02-12T23:15:24Z- AHRS_M2 msg: 1613171724.39 epoch sec; seqNo:2107 2021-02-12T23:15:24Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.14 psi 2021-02-12T23:15:24Z- handleDepth()-Depth_Keller msg: 1613171724396 epoch sec; seqNo:2130; depth 8.55 2021-02-12T23:15:24Z- handleDvl() - RDI_Pathfinder msg: 1613171724.41 epoch sec; seqNo:894 2021-02-12T23:15:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.11 , 96.22 , 117.85 , 78.04 2021-02-12T23:15:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , 0.19 2021-02-12T23:15:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171724.42 epoch sec; seqNo:2130; 4056540.66 north; 590663.76 east 2021-02-12T23:15:24Z- AHRS_M2 msg: 1613171724.80 epoch sec; seqNo:2108 2021-02-12T23:15:24Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-12T23:15:24Z- handleDepth()-Depth_Keller msg: 1613171724802 epoch sec; seqNo:2131; depth 8.43 2021-02-12T23:15:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171724.81 epoch sec; seqNo:2131; 4056541.04 north; 590663.75 east 2021-02-12T23:15:25Z- AHRS_M2 msg: 1613171725.20 epoch sec; seqNo:2109 2021-02-12T23:15:25Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-12T23:15:25Z- handleDepth()-Depth_Keller msg: 1613171725215 epoch sec; seqNo:2132; depth 8.34 2021-02-12T23:15:25Z- handleDvl() - RDI_Pathfinder msg: 1613171725.23 epoch sec; seqNo:895 2021-02-12T23:15:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.62 , 96.09 , 116.36 , 79.86 2021-02-12T23:15:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.14 , 0.12 2021-02-12T23:15:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171725.24 epoch sec; seqNo:2132; 4056541.42 north; 590663.75 east 2021-02-12T23:15:25Z- AHRS_M2 msg: 1613171725.60 epoch sec; seqNo:2110 2021-02-12T23:15:25Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-12T23:15:25Z- handleDepth()-Depth_Keller msg: 1613171725613 epoch sec; seqNo:2133; depth 8.21 2021-02-12T23:15:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171725.62 epoch sec; seqNo:2133; 4056541.42 north; 590663.75 east 2021-02-12T23:15:25Z- LcmTrn::handleMessages() - handling 43 messages took 0.63 ms and 131 calls... 2021-02-12T23:15:25Z- waiting for 1613171723.38, time is 1613171725.62 2021-02-12T23:15:25Z- LcmTrn::updateTrn() >>>> pose time 1613171725.62, meas time 1613171725.23 <<<< 2021-02-12T23:15:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171725.23 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:15:25Z- TerrainNav::motionUpdate() - currEstimate = (4056541, 590663.75, 8.21) 2021-02-12T23:15:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:15:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:15:25Z- TNavBF::Projecting Measurements 2021-02-12T23:15:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:15:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:15:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:15:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:15:25Z- TNavFilter::4 good beams. 2021-02-12T23:15:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:15:25Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:15:25Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:15:25Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:15:25Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:15:25Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:15:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:15:25Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:25Z- TerrainMapOctree: (vehN, vehE ) = (4056541.13, 590663.57). 2021-02-12T23:15:25Z- TerrainMapOctree: Min Distance = 5919.12. 2021-02-12T23:15:25Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:15:25Z- There is no specific good beam that all particles have in common. 2021-02-12T23:15:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:15:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 12.01 2021-02-12T23:15:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:15:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 14.77 2021-02-12T23:15:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:15:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 21.66 2021-02-12T23:15:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:15:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 10.21 2021-02-12T23:15:25Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:15:25Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:15:25Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:15:25Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:15:25Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:15:25Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:15:25Z- Filter 0 No beams used in subcloud update 2021-02-12T23:15:25Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:15:25Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:15:25Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 787.0 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:15:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171725.23 sec. 2021-02-12T23:15:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:15:25Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:25Z- variances([0], [2], and psi_berg) = 786.986543 833.078391 0.000000 2021-02-12T23:15:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:15:25Z- LcmTrn::publishEstimates() - MLE : 4056515.92 590619.25 8.21 2021-02-12T23:15:25Z- LcmTrn::publishEstimates() - MMSE : 4056541.42 590663.75 8.21 2021-02-12T23:15:25Z- LcmTrn::publishEstimates() - COVAR: 786.99 833.08 0.90 0.00 2021-02-12T23:15:26Z- AHRS_M2 msg: 1613171726.01 epoch sec; seqNo:2111 2021-02-12T23:15:26Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-12T23:15:26Z- handleDepth()-Depth_Keller msg: 1613171726012 epoch sec; seqNo:2134; depth 8.10 2021-02-12T23:15:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171726.02 epoch sec; seqNo:2134; 4056541.81 north; 590664.97 east 2021-02-12T23:15:26Z- AHRS_M2 msg: 1613171726.41 epoch sec; seqNo:2112 2021-02-12T23:15:26Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-12T23:15:26Z- handleDepth()-Depth_Keller msg: 1613171726418 epoch sec; seqNo:2135; depth 7.99 2021-02-12T23:15:26Z- handleDvl() - RDI_Pathfinder msg: 1613171726.43 epoch sec; seqNo:896 2021-02-12T23:15:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 96.15 , 115.07 , 77.22 2021-02-12T23:15:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.16 , -0.03 2021-02-12T23:15:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171726.44 epoch sec; seqNo:2135; 4056542.19 north; 590664.96 east 2021-02-12T23:15:26Z- AHRS_M2 msg: 1613171726.82 epoch sec; seqNo:2113 2021-02-12T23:15:26Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-12T23:15:26Z- handleDepth()-Depth_Keller msg: 1613171726825 epoch sec; seqNo:2136; depth 7.86 2021-02-12T23:15:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171726.84 epoch sec; seqNo:2136; 4056542.57 north; 590664.96 east 2021-02-12T23:15:27Z- AHRS_M2 msg: 1613171727.22 epoch sec; seqNo:2114 2021-02-12T23:15:27Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.18 psi 2021-02-12T23:15:27Z- handleDepth()-Depth_Keller msg: 1613171727225 epoch sec; seqNo:2137; depth 7.75 2021-02-12T23:15:27Z- handleDvl() - RDI_Pathfinder msg: 1613171727.24 epoch sec; seqNo:897 2021-02-12T23:15:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.75 , 96.22 , 120.54 , 78.85 2021-02-12T23:15:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , -0.19 2021-02-12T23:15:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171727.26 epoch sec; seqNo:2137; 4056542.57 north; 590664.96 east 2021-02-12T23:15:27Z- AHRS_M2 msg: 1613171727.62 epoch sec; seqNo:2115 2021-02-12T23:15:27Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.18 psi 2021-02-12T23:15:27Z- handleDepth()-Depth_Keller msg: 1613171727629 epoch sec; seqNo:2138; depth 7.65 2021-02-12T23:15:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171727.64 epoch sec; seqNo:2138; 4056542.95 north; 590664.95 east 2021-02-12T23:15:28Z- AHRS_M2 msg: 1613171728.03 epoch sec; seqNo:2116 2021-02-12T23:15:28Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.17 psi 2021-02-12T23:15:28Z- handleDepth()-Depth_Keller msg: 1613171728047 epoch sec; seqNo:2139; depth 7.53 2021-02-12T23:15:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171728.09 epoch sec; seqNo:2139; 4056543.32 north; 590664.95 east 2021-02-12T23:15:28Z- AHRS_M2 msg: 1613171728.44 epoch sec; seqNo:2117 2021-02-12T23:15:28Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.14 psi 2021-02-12T23:15:28Z- handleDepth()-Depth_Keller msg: 1613171728442 epoch sec; seqNo:2140; depth 7.40 2021-02-12T23:15:28Z- handleDvl() - RDI_Pathfinder msg: 1613171728.45 epoch sec; seqNo:898 2021-02-12T23:15:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.90 , 104.12 , 118.99 , 78.04 2021-02-12T23:15:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.08 , -0.23 2021-02-12T23:15:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171728.47 epoch sec; seqNo:2140; 4056543.34 north; 590666.17 east 2021-02-12T23:15:28Z- AHRS_M2 msg: 1613171728.84 epoch sec; seqNo:2118 2021-02-12T23:15:28Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.12 psi 2021-02-12T23:15:28Z- handleDepth()-Depth_Keller msg: 1613171728841 epoch sec; seqNo:2141; depth 7.29 2021-02-12T23:15:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171728.85 epoch sec; seqNo:2141; 4056543.72 north; 590666.17 east 2021-02-12T23:15:29Z- AHRS_M2 msg: 1613171729.24 epoch sec; seqNo:2119 2021-02-12T23:15:29Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.13 psi 2021-02-12T23:15:29Z- handleDepth()-Depth_Keller msg: 1613171729251 epoch sec; seqNo:2142; depth 7.17 2021-02-12T23:15:29Z- handleDvl() - RDI_Pathfinder msg: 1613171729.26 epoch sec; seqNo:899 2021-02-12T23:15:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.77 , 95.88 , 117.50 , 77.63 2021-02-12T23:15:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.23 , -0.16 2021-02-12T23:15:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171729.27 epoch sec; seqNo:2142; 4056543.72 north; 590666.17 east 2021-02-12T23:15:29Z- AHRS_M2 msg: 1613171729.64 epoch sec; seqNo:2120 2021-02-12T23:15:29Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.14 psi 2021-02-12T23:15:29Z- handleDepth()-Depth_Keller msg: 1613171729648 epoch sec; seqNo:2143; depth 7.04 2021-02-12T23:15:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171729.67 epoch sec; seqNo:2143; 4056544.10 north; 590666.16 east 2021-02-12T23:15:30Z- AHRS_M2 msg: 1613171730.05 epoch sec; seqNo:2121 2021-02-12T23:15:30Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.16 psi 2021-02-12T23:15:30Z- handleDepth()-Depth_Keller msg: 1613171730052 epoch sec; seqNo:2144; depth 6.90 2021-02-12T23:15:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171730.06 epoch sec; seqNo:2144; 4056544.47 north; 590666.16 east 2021-02-12T23:15:30Z- AHRS_M2 msg: 1613171730.45 epoch sec; seqNo:2122 2021-02-12T23:15:30Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.18 psi 2021-02-12T23:15:30Z- handleDepth()-Depth_Keller msg: 1613171730459 epoch sec; seqNo:2145; depth 6.78 2021-02-12T23:15:30Z- handleDvl() - RDI_Pathfinder msg: 1613171730.47 epoch sec; seqNo:900 2021-02-12T23:15:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.04 , 96.15 , 116.42 , 77.22 2021-02-12T23:15:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.29 , -0.03 2021-02-12T23:15:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171730.48 epoch sec; seqNo:2145; 4056544.47 north; 590666.16 east 2021-02-12T23:15:30Z- LcmTrn::handleMessages() - handling 41 messages took 0.70 ms and 127 calls... 2021-02-12T23:15:30Z- waiting for 1613171728.62, time is 1613171730.48 2021-02-12T23:15:30Z- LcmTrn::updateTrn() equating pose 1613171730.48 and meas times 1613171730.48 2021-02-12T23:15:30Z- LcmTrn::updateTrn() >>>> pose time 1613171730.48, meas time 1613171730.48 <<<< 2021-02-12T23:15:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171730.48 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:15:30Z- TerrainNav::motionUpdate() - currEstimate = (4056544, 590666.16, 6.78) 2021-02-12T23:15:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:15:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:15:30Z- TNavBF::Projecting Measurements 2021-02-12T23:15:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:15:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:15:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:15:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:15:30Z- TNavFilter::4 good beams. 2021-02-12T23:15:30Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:15:30Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:15:30Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:15:30Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:15:30Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:15:30Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:15:30Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:15:30Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:30Z- TerrainMapOctree: (vehN, vehE ) = (4056544.47, 590666.16). 2021-02-12T23:15:30Z- TerrainMapOctree: Min Distance = 5914.89. 2021-02-12T23:15:30Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:15:30Z- There is no specific good beam that all particles have in common. 2021-02-12T23:15:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:15:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 12.40 2021-02-12T23:15:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:15:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 14.79 2021-02-12T23:15:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:15:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 21.69 2021-02-12T23:15:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:15:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 9.54 2021-02-12T23:15:30Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:15:30Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:15:30Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:15:30Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:15:30Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:15:30Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:15:30Z- Filter 0 No beams used in subcloud update 2021-02-12T23:15:30Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:15:30Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:15:30Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 786.8 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:15:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171730.48 sec. 2021-02-12T23:15:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:15:30Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:30Z- variances([0], [2], and psi_berg) = 786.815966 833.078112 0.000000 2021-02-12T23:15:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:15:30Z- LcmTrn::publishEstimates() - MLE : 4056518.97 590621.66 6.78 2021-02-12T23:15:30Z- LcmTrn::publishEstimates() - MMSE : 4056544.47 590666.16 6.78 2021-02-12T23:15:30Z- LcmTrn::publishEstimates() - COVAR: 786.82 833.08 0.86 0.00 2021-02-12T23:15:30Z- AHRS_M2 msg: 1613171730.86 epoch sec; seqNo:2123 2021-02-12T23:15:30Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.20 psi 2021-02-12T23:15:30Z- handleDepth()-Depth_Keller msg: 1613171730862 epoch sec; seqNo:2146; depth 6.64 2021-02-12T23:15:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171730.87 epoch sec; seqNo:2146; 4056544.85 north; 590666.15 east 2021-02-12T23:15:31Z- AHRS_M2 msg: 1613171731.26 epoch sec; seqNo:2124 2021-02-12T23:15:31Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.20 psi 2021-02-12T23:15:31Z- handleDepth()-Depth_Keller msg: 1613171731264 epoch sec; seqNo:2147; depth 6.53 2021-02-12T23:15:31Z- handleDvl() - RDI_Pathfinder msg: 1613171731.28 epoch sec; seqNo:901 2021-02-12T23:15:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.04 , 105.61 , 119.13 , 79.67 2021-02-12T23:15:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.19 , 0.13 2021-02-12T23:15:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171731.29 epoch sec; seqNo:2147; 4056545.23 north; 590666.15 east 2021-02-12T23:15:31Z- AHRS_M2 msg: 1613171731.67 epoch sec; seqNo:2125 2021-02-12T23:15:31Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.18 psi 2021-02-12T23:15:31Z- handleDepth()-Depth_Keller msg: 1613171731693 epoch sec; seqNo:2148; depth 6.40 2021-02-12T23:15:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171731.72 epoch sec; seqNo:2148; 4056545.61 north; 590666.15 east 2021-02-12T23:15:32Z- AHRS_M2 msg: 1613171732.07 epoch sec; seqNo:2126 2021-02-12T23:15:32Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.16 psi 2021-02-12T23:15:32Z- handleDepth()-Depth_Keller msg: 1613171732073 epoch sec; seqNo:2149; depth 6.28 2021-02-12T23:15:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171732.08 epoch sec; seqNo:2149; 4056545.99 north; 590666.14 east 2021-02-12T23:15:32Z- AHRS_M2 msg: 1613171732.47 epoch sec; seqNo:2127 2021-02-12T23:15:32Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.15 psi 2021-02-12T23:15:32Z- handleDepth()-Depth_Keller msg: 1613171732476 epoch sec; seqNo:2150; depth 6.16 2021-02-12T23:15:32Z- handleDvl() - RDI_Pathfinder msg: 1613171732.49 epoch sec; seqNo:902 2021-02-12T23:15:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.05 , 105.27 , 121.49 , 79.26 2021-02-12T23:15:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.01 , 0.12 2021-02-12T23:15:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171732.51 epoch sec; seqNo:2150; 4056545.99 north; 590666.14 east 2021-02-12T23:15:32Z- AHRS_M2 msg: 1613171732.88 epoch sec; seqNo:2128 2021-02-12T23:15:32Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-12T23:15:32Z- handleDepth()-Depth_Keller msg: 1613171732880 epoch sec; seqNo:2151; depth 6.03 2021-02-12T23:15:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171732.89 epoch sec; seqNo:2151; 4056546.38 north; 590667.36 east 2021-02-12T23:15:33Z- AHRS_M2 msg: 1613171733.28 epoch sec; seqNo:2129 2021-02-12T23:15:33Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.15 psi 2021-02-12T23:15:33Z- handleDepth()-Depth_Keller msg: 1613171733289 epoch sec; seqNo:2152; depth 5.90 2021-02-12T23:15:33Z- handleDvl() - RDI_Pathfinder msg: 1613171733.30 epoch sec; seqNo:903 2021-02-12T23:15:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 95.74 , 118.72 , 79.26 2021-02-12T23:15:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , -0.10 , -0.01 2021-02-12T23:15:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171733.31 epoch sec; seqNo:2152; 4056546.76 north; 590667.36 east 2021-02-12T23:15:33Z- AHRS_M2 msg: 1613171733.68 epoch sec; seqNo:2130 2021-02-12T23:15:33Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.17 psi 2021-02-12T23:15:33Z- handleDepth()-Depth_Keller msg: 1613171733702 epoch sec; seqNo:2153; depth 5.76 2021-02-12T23:15:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171733.72 epoch sec; seqNo:2153; 4056546.76 north; 590667.36 east 2021-02-12T23:15:34Z- AHRS_M2 msg: 1613171734.09 epoch sec; seqNo:2131 2021-02-12T23:15:34Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.19 psi 2021-02-12T23:15:34Z- handleDepth()-Depth_Keller msg: 1613171734101 epoch sec; seqNo:2154; depth 5.60 2021-02-12T23:15:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171734.13 epoch sec; seqNo:2154; 4056547.14 north; 590667.35 east 2021-02-12T23:15:34Z- AHRS_M2 msg: 1613171734.49 epoch sec; seqNo:2132 2021-02-12T23:15:34Z- AHRS_M2 msg: 0.63 phi; 0.29 theta; -0.20 psi 2021-02-12T23:15:34Z- handleDepth()-Depth_Keller msg: 1613171734501 epoch sec; seqNo:2155; depth 5.45 2021-02-12T23:15:34Z- handleDvl() - RDI_Pathfinder msg: 1613171734.51 epoch sec; seqNo:904 2021-02-12T23:15:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.90 , 104.12 , 118.99 , 78.85 2021-02-12T23:15:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.32 , 0.09 2021-02-12T23:15:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171734.54 epoch sec; seqNo:2155; 4056547.52 north; 590667.35 east 2021-02-12T23:15:34Z- AHRS_M2 msg: 1613171734.90 epoch sec; seqNo:2133 2021-02-12T23:15:34Z- AHRS_M2 msg: 0.63 phi; 0.26 theta; -0.20 psi 2021-02-12T23:15:34Z- handleDepth()-Depth_Keller msg: 1613171734901 epoch sec; seqNo:2156; depth 5.30 2021-02-12T23:15:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171734.91 epoch sec; seqNo:2156; 4056547.52 north; 590667.35 east 2021-02-12T23:15:35Z- AHRS_M2 msg: 1613171735.30 epoch sec; seqNo:2134 2021-02-12T23:15:35Z- AHRS_M2 msg: 0.64 phi; 0.23 theta; -0.19 psi 2021-02-12T23:15:35Z- handleDepth()-Depth_Keller msg: 1613171735306 epoch sec; seqNo:2157; depth 5.16 2021-02-12T23:15:35Z- handleDvl() - RDI_Pathfinder msg: 1613171735.32 epoch sec; seqNo:905 2021-02-12T23:15:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.07 , 106.69 , 113.45 , 82.36 2021-02-12T23:15:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.25 , 0.44 2021-02-12T23:15:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171735.33 epoch sec; seqNo:2157; 4056547.90 north; 590667.34 east 2021-02-12T23:15:35Z- AHRS_M2 msg: 1613171735.70 epoch sec; seqNo:2135 2021-02-12T23:15:35Z- AHRS_M2 msg: 0.65 phi; 0.19 theta; -0.15 psi 2021-02-12T23:15:35Z- handleDepth()-Depth_Keller msg: 1613171735713 epoch sec; seqNo:2158; depth 5.01 2021-02-12T23:15:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171735.73 epoch sec; seqNo:2158; 4056548.28 north; 590667.34 east 2021-02-12T23:15:35Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-12T23:15:35Z- waiting for 1613171733.48, time is 1613171735.73 2021-02-12T23:15:35Z- LcmTrn::updateTrn() >>>> pose time 1613171735.73, meas time 1613171735.32 <<<< 2021-02-12T23:15:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171735.32 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:15:35Z- TerrainNav::motionUpdate() - currEstimate = (4056548, 590667.34, 5.01) 2021-02-12T23:15:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:15:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:15:35Z- TNavBF::Projecting Measurements 2021-02-12T23:15:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:15:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:15:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:15:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:15:35Z- TNavFilter::4 good beams. 2021-02-12T23:15:35Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:15:35Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:15:35Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:15:35Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:15:35Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:15:35Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:15:35Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:15:35Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:35Z- TerrainMapOctree: (vehN, vehE ) = (4056547.98, 590667.25). 2021-02-12T23:15:35Z- TerrainMapOctree: Min Distance = 5911.44. 2021-02-12T23:15:35Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:15:35Z- There is no specific good beam that all particles have in common. 2021-02-12T23:15:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:15:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 11.58 2021-02-12T23:15:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:15:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 18.21 2021-02-12T23:15:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:15:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 20.59 2021-02-12T23:15:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:15:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 10.85 2021-02-12T23:15:35Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:15:35Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:15:35Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:15:35Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:15:36Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:15:36Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:15:36Z- Filter 0 No beams used in subcloud update 2021-02-12T23:15:36Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:15:36Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:15:36Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 786.7 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:15:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171735.32 sec. 2021-02-12T23:15:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:15:36Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:36Z- variances([0], [2], and psi_berg) = 786.602767 833.077765 0.000000 2021-02-12T23:15:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:15:36Z- LcmTrn::publishEstimates() - MLE : 4056522.78 590622.84 5.01 2021-02-12T23:15:36Z- LcmTrn::publishEstimates() - MMSE : 4056548.28 590667.34 5.01 2021-02-12T23:15:36Z- LcmTrn::publishEstimates() - COVAR: 786.60 833.08 0.84 0.00 2021-02-12T23:15:36Z- AHRS_M2 msg: 1613171736.11 epoch sec; seqNo:2136 2021-02-12T23:15:36Z- AHRS_M2 msg: 0.66 phi; 0.14 theta; -0.12 psi 2021-02-12T23:15:36Z- handleDepth()-Depth_Keller msg: 1613171736114 epoch sec; seqNo:2159; depth 4.90 2021-02-12T23:15:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171736.13 epoch sec; seqNo:2159; 4056548.65 north; 590667.34 east 2021-02-12T23:15:36Z- AHRS_M2 msg: 1613171736.51 epoch sec; seqNo:2137 2021-02-12T23:15:36Z- AHRS_M2 msg: 0.66 phi; 0.09 theta; -0.09 psi 2021-02-12T23:15:36Z- handleDepth()-Depth_Keller msg: 1613171736518 epoch sec; seqNo:2160; depth 4.80 2021-02-12T23:15:36Z- handleDvl() - RDI_Pathfinder msg: 1613171736.53 epoch sec; seqNo:906 2021-02-12T23:15:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 101.35 , 106.76 , 85.13 2021-02-12T23:15:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.14 , 0.51 2021-02-12T23:15:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171736.54 epoch sec; seqNo:2160; 4056549.05 north; 590668.55 east 2021-02-12T23:15:36Z- AHRS_M2 msg: 1613171736.92 epoch sec; seqNo:2138 2021-02-12T23:15:36Z- AHRS_M2 msg: 0.67 phi; 0.04 theta; -0.08 psi 2021-02-12T23:15:36Z- handleDepth()-Depth_Keller msg: 1613171736922 epoch sec; seqNo:2161; depth 4.71 2021-02-12T23:15:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171736.93 epoch sec; seqNo:2161; 4056549.05 north; 590668.55 east 2021-02-12T23:15:37Z- AHRS_M2 msg: 1613171737.32 epoch sec; seqNo:2139 2021-02-12T23:15:37Z- AHRS_M2 msg: 0.67 phi; -0.00 theta; -0.08 psi 2021-02-12T23:15:37Z- handleDepth()-Depth_Keller msg: 1613171737324 epoch sec; seqNo:2162; depth 4.63 2021-02-12T23:15:37Z- handleDvl() - RDI_Pathfinder msg: 1613171737.34 epoch sec; seqNo:907 2021-02-12T23:15:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.62 , 93.38 , 97.43 , 89.32 2021-02-12T23:15:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.42 , 0.21 2021-02-12T23:15:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171737.35 epoch sec; seqNo:2162; 4056549.42 north; 590668.55 east 2021-02-12T23:15:37Z- AHRS_M2 msg: 1613171737.72 epoch sec; seqNo:2140 2021-02-12T23:15:37Z- AHRS_M2 msg: 0.66 phi; -0.04 theta; -0.11 psi 2021-02-12T23:15:37Z- handleDepth()-Depth_Keller msg: 1613171737728 epoch sec; seqNo:2163; depth 4.55 2021-02-12T23:15:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171737.74 epoch sec; seqNo:2163; 4056549.42 north; 590668.55 east 2021-02-12T23:15:38Z- AHRS_M2 msg: 1613171738.14 epoch sec; seqNo:2141 2021-02-12T23:15:38Z- AHRS_M2 msg: 0.67 phi; -0.07 theta; -0.14 psi 2021-02-12T23:15:38Z- handleDepth()-Depth_Keller msg: 1613171738143 epoch sec; seqNo:2164; depth 4.48 2021-02-12T23:15:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171738.15 epoch sec; seqNo:2164; 4056549.82 north; 590669.77 east 2021-02-12T23:15:38Z- AHRS_M2 msg: 1613171738.53 epoch sec; seqNo:2142 2021-02-12T23:15:38Z- AHRS_M2 msg: 0.68 phi; -0.09 theta; -0.18 psi 2021-02-12T23:15:38Z- handleDepth()-Depth_Keller msg: 1613171738546 epoch sec; seqNo:2165; depth 4.46 2021-02-12T23:15:38Z- handleDvl() - RDI_Pathfinder msg: 1613171738.56 epoch sec; seqNo:908 2021-02-12T23:15:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 96.42 , 89.12 , 97.64 2021-02-12T23:15:38Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.27 , -0.16 2021-02-12T23:15:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171738.57 epoch sec; seqNo:2165; 4056550.20 north; 590669.76 east 2021-02-12T23:15:38Z- AHRS_M2 msg: 1613171738.94 epoch sec; seqNo:2143 2021-02-12T23:15:38Z- AHRS_M2 msg: 0.70 phi; -0.11 theta; -0.21 psi 2021-02-12T23:15:38Z- handleDepth()-Depth_Keller msg: 1613171738940 epoch sec; seqNo:2166; depth 4.43 2021-02-12T23:15:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171738.97 epoch sec; seqNo:2166; 4056550.20 north; 590669.76 east 2021-02-12T23:15:39Z- AHRS_M2 msg: 1613171739.35 epoch sec; seqNo:2144 2021-02-12T23:15:39Z- AHRS_M2 msg: 0.72 phi; -0.13 theta; -0.23 psi 2021-02-12T23:15:39Z- handleDepth()-Depth_Keller msg: 1613171739351 epoch sec; seqNo:2167; depth 4.42 2021-02-12T23:15:39Z- handleDvl() - RDI_Pathfinder msg: 1613171739.37 epoch sec; seqNo:909 2021-02-12T23:15:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 105.81 , 0.00 , 99.05 2021-02-12T23:15:39Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , -0.12 , -0.21 2021-02-12T23:15:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171739.39 epoch sec; seqNo:2167; 4056550.57 north; 590669.76 east 2021-02-12T23:15:39Z- AHRS_M2 msg: 1613171739.75 epoch sec; seqNo:2145 2021-02-12T23:15:39Z- AHRS_M2 msg: 0.73 phi; -0.16 theta; -0.22 psi 2021-02-12T23:15:39Z- handleDepth()-Depth_Keller msg: 1613171739754 epoch sec; seqNo:2168; depth 4.46 2021-02-12T23:15:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171739.77 epoch sec; seqNo:2168; 4056550.97 north; 590670.97 east 2021-02-12T23:15:40Z- AHRS_M2 msg: 1613171740.15 epoch sec; seqNo:2146 2021-02-12T23:15:40Z- AHRS_M2 msg: 0.75 phi; -0.19 theta; -0.20 psi 2021-02-12T23:15:40Z- handleDepth()-Depth_Keller msg: 1613171740154 epoch sec; seqNo:2169; depth 4.49 2021-02-12T23:15:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171740.17 epoch sec; seqNo:2169; 4056551.34 north; 590670.97 east 2021-02-12T23:15:40Z- AHRS_M2 msg: 1613171740.55 epoch sec; seqNo:2147 2021-02-12T23:15:40Z- AHRS_M2 msg: 0.75 phi; -0.22 theta; -0.17 psi 2021-02-12T23:15:40Z- handleDepth()-Depth_Keller msg: 1613171740556 epoch sec; seqNo:2170; depth 4.53 2021-02-12T23:15:40Z- handleDvl() - RDI_Pathfinder msg: 1613171740.57 epoch sec; seqNo:910 2021-02-12T23:15:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.85 , 109.53 , 86.55 , 102.77 2021-02-12T23:15:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.28 , -0.14 2021-02-12T23:15:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171740.59 epoch sec; seqNo:2170; 4056551.72 north; 590670.97 east 2021-02-12T23:15:40Z- AHRS_M2 msg: 1613171740.96 epoch sec; seqNo:2148 2021-02-12T23:15:40Z- AHRS_M2 msg: 0.75 phi; -0.26 theta; -0.15 psi 2021-02-12T23:15:40Z- handleDepth()-Depth_Keller msg: 1613171740960 epoch sec; seqNo:2171; depth 4.59 2021-02-12T23:15:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171740.97 epoch sec; seqNo:2171; 4056552.10 north; 590670.96 east 2021-02-12T23:15:41Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-12T23:15:41Z- waiting for 1613171738.73, time is 1613171740.97 2021-02-12T23:15:41Z- LcmTrn::updateTrn() >>>> pose time 1613171740.97, meas time 1613171740.57 <<<< 2021-02-12T23:15:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171740.57 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:15:41Z- TerrainNav::motionUpdate() - currEstimate = (4056552, 590670.96, 4.59) 2021-02-12T23:15:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:15:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:15:41Z- TNavBF::Projecting Measurements 2021-02-12T23:15:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:15:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:15:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:15:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:15:41Z- TNavFilter::4 good beams. 2021-02-12T23:15:41Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:15:41Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:15:41Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:15:41Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:15:41Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:15:41Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:15:41Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:15:41Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:41Z- TerrainMapOctree: (vehN, vehE ) = (4056551.81, 590670.68). 2021-02-12T23:15:41Z- TerrainMapOctree: Min Distance = 5906.31. 2021-02-12T23:15:41Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:15:41Z- There is no specific good beam that all particles have in common. 2021-02-12T23:15:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:15:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 11.25 2021-02-12T23:15:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:15:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 19.19 2021-02-12T23:15:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:15:41Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 11.99 2021-02-12T23:15:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:15:41Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 16.90 2021-02-12T23:15:41Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:15:41Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:15:41Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:15:41Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:15:41Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:15:41Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:15:41Z- Filter 0 No beams used in subcloud update 2021-02-12T23:15:41Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:15:41Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:15:41Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 786.4 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:15:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171740.57 sec. 2021-02-12T23:15:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:15:41Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:41Z- variances([0], [2], and psi_berg) = 786.358963 833.077370 0.000000 2021-02-12T23:15:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:15:41Z- LcmTrn::publishEstimates() - MLE : 4056527.60 590624.46 4.59 2021-02-12T23:15:41Z- LcmTrn::publishEstimates() - MMSE : 4056552.10 590670.96 4.59 2021-02-12T23:15:41Z- LcmTrn::publishEstimates() - COVAR: 786.36 833.08 0.88 0.00 2021-02-12T23:15:41Z- AHRS_M2 msg: 1613171741.36 epoch sec; seqNo:2149 2021-02-12T23:15:41Z- AHRS_M2 msg: 0.74 phi; -0.29 theta; -0.15 psi 2021-02-12T23:15:41Z- handleDepth()-Depth_Keller msg: 1613171741366 epoch sec; seqNo:2172; depth 4.61 2021-02-12T23:15:41Z- handleDvl() - RDI_Pathfinder msg: 1613171741.38 epoch sec; seqNo:911 2021-02-12T23:15:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.55 , 104.12 , 83.85 , 108.18 2021-02-12T23:15:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.55 , -0.20 , -0.16 2021-02-12T23:15:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171741.39 epoch sec; seqNo:2172; 4056552.10 north; 590670.96 east 2021-02-12T23:15:41Z- AHRS_M2 msg: 1613171741.76 epoch sec; seqNo:2150 2021-02-12T23:15:41Z- AHRS_M2 msg: 0.72 phi; -0.31 theta; -0.16 psi 2021-02-12T23:15:41Z- handleDepth()-Depth_Keller msg: 1613171741768 epoch sec; seqNo:2173; depth 4.67 2021-02-12T23:15:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171741.79 epoch sec; seqNo:2173; 4056552.48 north; 590670.96 east 2021-02-12T23:15:42Z- AHRS_M2 msg: 1613171742.17 epoch sec; seqNo:2151 2021-02-12T23:15:42Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.18 psi 2021-02-12T23:15:42Z- handleDepth()-Depth_Keller msg: 1613171742172 epoch sec; seqNo:2174; depth 4.71 2021-02-12T23:15:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171742.18 epoch sec; seqNo:2174; 4056552.86 north; 590670.95 east 2021-02-12T23:15:42Z- AHRS_M2 msg: 1613171742.57 epoch sec; seqNo:2152 2021-02-12T23:15:42Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.21 psi 2021-02-12T23:15:42Z- handleDepth()-Depth_Keller msg: 1613171742578 epoch sec; seqNo:2175; depth 4.78 2021-02-12T23:15:42Z- handleDvl() - RDI_Pathfinder msg: 1613171742.59 epoch sec; seqNo:912 2021-02-12T23:15:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.90 , 104.12 , 81.15 , 116.29 2021-02-12T23:15:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , -0.16 , -0.03 2021-02-12T23:15:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171742.60 epoch sec; seqNo:2175; 4056552.86 north; 590670.95 east 2021-02-12T23:15:42Z- AHRS_M2 msg: 1613171742.98 epoch sec; seqNo:2153 2021-02-12T23:15:42Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.23 psi 2021-02-12T23:15:42Z- handleDepth()-Depth_Keller msg: 1613171742982 epoch sec; seqNo:2176; depth 4.85 2021-02-12T23:15:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171742.99 epoch sec; seqNo:2176; 4056553.24 north; 590670.95 east 2021-02-12T23:15:43Z- AHRS_M2 msg: 1613171743.38 epoch sec; seqNo:2154 2021-02-12T23:15:43Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.26 psi 2021-02-12T23:15:43Z- handleDepth()-Depth_Keller msg: 1613171743384 epoch sec; seqNo:2177; depth 4.93 2021-02-12T23:15:43Z- handleDvl() - RDI_Pathfinder msg: 1613171743.39 epoch sec; seqNo:913 2021-02-12T23:15:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.77 , 110.75 , 78.85 , 118.86 2021-02-12T23:15:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.09 , 0.07 2021-02-12T23:15:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171743.42 epoch sec; seqNo:2177; 4056553.25 north; 590672.17 east 2021-02-12T23:15:43Z- AHRS_M2 msg: 1613171743.78 epoch sec; seqNo:2155 2021-02-12T23:15:43Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.27 psi 2021-02-12T23:15:43Z- handleDepth()-Depth_Keller msg: 1613171743793 epoch sec; seqNo:2178; depth 5.05 2021-02-12T23:15:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171743.80 epoch sec; seqNo:2178; 4056553.63 north; 590672.17 east 2021-02-12T23:15:44Z- AHRS_M2 msg: 1613171744.19 epoch sec; seqNo:2156 2021-02-12T23:15:44Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.28 psi 2021-02-12T23:15:44Z- handleDepth()-Depth_Keller msg: 1613171744193 epoch sec; seqNo:2179; depth 5.16 2021-02-12T23:15:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171744.21 epoch sec; seqNo:2179; 4056554.01 north; 590672.16 east 2021-02-12T23:15:44Z- AHRS_M2 msg: 1613171744.59 epoch sec; seqNo:2157 2021-02-12T23:15:44Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.27 psi 2021-02-12T23:15:44Z- handleDepth()-Depth_Keller msg: 1613171744596 epoch sec; seqNo:2180; depth 5.26 2021-02-12T23:15:44Z- handleDvl() - RDI_Pathfinder msg: 1613171744.62 epoch sec; seqNo:914 2021-02-12T23:15:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.28 , 116.02 , 82.23 , 118.72 2021-02-12T23:15:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.13 , 0.02 2021-02-12T23:15:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171744.63 epoch sec; seqNo:2180; 4056554.39 north; 590672.16 east 2021-02-12T23:15:45Z- AHRS_M2 msg: 1613171745.00 epoch sec; seqNo:2158 2021-02-12T23:15:45Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.25 psi 2021-02-12T23:15:45Z- handleDepth()-Depth_Keller msg: 1613171745009 epoch sec; seqNo:2181; depth 5.39 2021-02-12T23:15:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171745.03 epoch sec; seqNo:2181; 4056554.39 north; 590672.16 east 2021-02-12T23:15:45Z- AHRS_M2 msg: 1613171745.41 epoch sec; seqNo:2159 2021-02-12T23:15:45Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.22 psi 2021-02-12T23:15:45Z- handleDepth()-Depth_Keller msg: 1613171745411 epoch sec; seqNo:2182; depth 5.50 2021-02-12T23:15:45Z- handleDvl() - RDI_Pathfinder msg: 1613171745.42 epoch sec; seqNo:915 2021-02-12T23:15:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.15 , 115.88 , 80.74 , 118.58 2021-02-12T23:15:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.18 , -0.01 2021-02-12T23:15:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171745.43 epoch sec; seqNo:2182; 4056554.77 north; 590672.16 east 2021-02-12T23:15:45Z- AHRS_M2 msg: 1613171745.81 epoch sec; seqNo:2160 2021-02-12T23:15:45Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.20 psi 2021-02-12T23:15:45Z- handleDepth()-Depth_Keller msg: 1613171745814 epoch sec; seqNo:2183; depth 5.62 2021-02-12T23:15:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171745.83 epoch sec; seqNo:2183; 4056555.15 north; 590672.15 east 2021-02-12T23:15:46Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-12T23:15:46Z- waiting for 1613171743.97, time is 1613171745.83 2021-02-12T23:15:46Z- LcmTrn::updateTrn() >>>> pose time 1613171745.83, meas time 1613171745.42 <<<< 2021-02-12T23:15:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171745.42 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:15:46Z- TerrainNav::motionUpdate() - currEstimate = (4056555, 590672.15, 5.62) 2021-02-12T23:15:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:15:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:15:46Z- TNavBF::Projecting Measurements 2021-02-12T23:15:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:15:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:15:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:15:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:15:46Z- TNavFilter::4 good beams. 2021-02-12T23:15:46Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:15:46Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:15:46Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:15:46Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:15:46Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:15:46Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:15:46Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:15:46Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:46Z- TerrainMapOctree: (vehN, vehE ) = (4056554.89, 590672.05). 2021-02-12T23:15:46Z- TerrainMapOctree: Min Distance = 5903.03. 2021-02-12T23:15:46Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:15:46Z- There is no specific good beam that all particles have in common. 2021-02-12T23:15:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:15:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 11.87 2021-02-12T23:15:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:15:46Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 21.49 2021-02-12T23:15:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:15:46Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 10.43 2021-02-12T23:15:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:15:46Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.034 ExpectedMeasDiff: nan currMeas.covariance: 22.50 2021-02-12T23:15:46Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:15:46Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:15:46Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:15:46Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:15:46Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:15:46Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:15:46Z- Filter 0 No beams used in subcloud update 2021-02-12T23:15:46Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:15:46Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:15:46Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 786.2 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:15:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171745.42 sec. 2021-02-12T23:15:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:15:46Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:46Z- variances([0], [2], and psi_berg) = 786.166695 833.077060 0.000000 2021-02-12T23:15:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:15:46Z- LcmTrn::publishEstimates() - MLE : 4056530.65 590625.65 5.62 2021-02-12T23:15:46Z- LcmTrn::publishEstimates() - MMSE : 4056555.15 590672.15 5.62 2021-02-12T23:15:46Z- LcmTrn::publishEstimates() - COVAR: 786.17 833.08 0.84 0.00 2021-02-12T23:15:46Z- AHRS_M2 msg: 1613171746.21 epoch sec; seqNo:2161 2021-02-12T23:15:46Z- AHRS_M2 msg: 0.66 phi; -0.38 theta; -0.20 psi 2021-02-12T23:15:46Z- handleDepth()-Depth_Keller msg: 1613171746216 epoch sec; seqNo:2184; depth 5.73 2021-02-12T23:15:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171746.25 epoch sec; seqNo:2184; 4056555.15 north; 590672.15 east 2021-02-12T23:15:46Z- AHRS_M2 msg: 1613171746.61 epoch sec; seqNo:2162 2021-02-12T23:15:46Z- AHRS_M2 msg: 0.66 phi; -0.38 theta; -0.20 psi 2021-02-12T23:15:46Z- handleDepth()-Depth_Keller msg: 1613171746616 epoch sec; seqNo:2185; depth 5.87 2021-02-12T23:15:46Z- handleDvl() - RDI_Pathfinder msg: 1613171746.63 epoch sec; seqNo:916 2021-02-12T23:15:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.50 , 110.47 , 80.74 , 121.29 2021-02-12T23:15:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.63 , -0.17 , 0.08 2021-02-12T23:15:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171746.65 epoch sec; seqNo:2185; 4056555.53 north; 590672.15 east 2021-02-12T23:15:47Z- AHRS_M2 msg: 1613171747.02 epoch sec; seqNo:2163 2021-02-12T23:15:47Z- AHRS_M2 msg: 0.65 phi; -0.39 theta; -0.20 psi 2021-02-12T23:15:47Z- handleDepth()-Depth_Keller msg: 1613171747020 epoch sec; seqNo:2186; depth 6.00 2021-02-12T23:15:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171747.03 epoch sec; seqNo:2186; 4056555.54 north; 590673.37 east 2021-02-12T23:15:47Z- AHRS_M2 msg: 1613171747.42 epoch sec; seqNo:2164 2021-02-12T23:15:47Z- AHRS_M2 msg: 0.64 phi; -0.39 theta; -0.19 psi 2021-02-12T23:15:47Z- handleDepth()-Depth_Keller msg: 1613171747425 epoch sec; seqNo:2187; depth 6.12 2021-02-12T23:15:47Z- handleDvl() - RDI_Pathfinder msg: 1613171747.44 epoch sec; seqNo:917 2021-02-12T23:15:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.92 , 109.19 , 79.46 , 124.06 2021-02-12T23:15:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.03 , 0.18 2021-02-12T23:15:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171747.45 epoch sec; seqNo:2187; 4056555.92 north; 590673.36 east 2021-02-12T23:15:47Z- AHRS_M2 msg: 1613171747.82 epoch sec; seqNo:2165 2021-02-12T23:15:47Z- AHRS_M2 msg: 0.64 phi; -0.40 theta; -0.19 psi 2021-02-12T23:15:47Z- handleDepth()-Depth_Keller msg: 1613171747845 epoch sec; seqNo:2188; depth 6.26 2021-02-12T23:15:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171747.87 epoch sec; seqNo:2188; 4056555.92 north; 590673.36 east 2021-02-12T23:15:48Z- AHRS_M2 msg: 1613171748.24 epoch sec; seqNo:2166 2021-02-12T23:15:48Z- AHRS_M2 msg: 0.63 phi; -0.40 theta; -0.19 psi 2021-02-12T23:15:48Z- handleDepth()-Depth_Keller msg: 1613171748248 epoch sec; seqNo:2189; depth 6.40 2021-02-12T23:15:48Z- handleDvl() - RDI_Pathfinder msg: 1613171748.28 epoch sec; seqNo:918 2021-02-12T23:15:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.20 , 107.77 , 79.39 , 125.34 2021-02-12T23:15:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , 0.12 , 0.17 2021-02-12T23:15:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171748.29 epoch sec; seqNo:2189; 4056556.30 north; 590673.36 east 2021-02-12T23:15:48Z- AHRS_M2 msg: 1613171748.63 epoch sec; seqNo:2167 2021-02-12T23:15:48Z- AHRS_M2 msg: 0.62 phi; -0.39 theta; -0.20 psi 2021-02-12T23:15:48Z- handleDepth()-Depth_Keller msg: 1613171748636 epoch sec; seqNo:2190; depth 6.57 2021-02-12T23:15:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171748.65 epoch sec; seqNo:2190; 4056556.30 north; 590673.36 east 2021-02-12T23:15:49Z- AHRS_M2 msg: 1613171749.04 epoch sec; seqNo:2168 2021-02-12T23:15:49Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.21 psi 2021-02-12T23:15:49Z- handleDepth()-Depth_Keller msg: 1613171749049 epoch sec; seqNo:2191; depth 6.66 2021-02-12T23:15:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171749.06 epoch sec; seqNo:2191; 4056556.30 north; 590673.36 east 2021-02-12T23:15:49Z- AHRS_M2 msg: 1613171749.44 epoch sec; seqNo:2169 2021-02-12T23:15:49Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.22 psi 2021-02-12T23:15:49Z- handleDepth()-Depth_Keller msg: 1613171749444 epoch sec; seqNo:2192; depth 6.81 2021-02-12T23:15:49Z- handleDvl() - RDI_Pathfinder msg: 1613171749.46 epoch sec; seqNo:919 2021-02-12T23:15:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.20 , 109.12 , 79.39 , 126.69 2021-02-12T23:15:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.15 , 0.08 2021-02-12T23:15:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171749.47 epoch sec; seqNo:2192; 4056556.67 north; 590673.36 east 2021-02-12T23:15:49Z- AHRS_M2 msg: 1613171749.84 epoch sec; seqNo:2170 2021-02-12T23:15:49Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.22 psi 2021-02-12T23:15:49Z- handleDepth()-Depth_Keller msg: 1613171749853 epoch sec; seqNo:2193; depth 6.95 2021-02-12T23:15:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171749.86 epoch sec; seqNo:2193; 4056557.07 north; 590674.57 east 2021-02-12T23:15:50Z- AHRS_M2 msg: 1613171750.26 epoch sec; seqNo:2171 2021-02-12T23:15:50Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.22 psi 2021-02-12T23:15:50Z- handleDepth()-Depth_Keller msg: 1613171750264 epoch sec; seqNo:2194; depth 7.10 2021-02-12T23:15:50Z- handleDvl() - RDI_Pathfinder msg: 1613171750.29 epoch sec; seqNo:920 2021-02-12T23:15:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 109.87 , 78.44 , 123.25 2021-02-12T23:15:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.13 , -0.01 2021-02-12T23:15:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171750.32 epoch sec; seqNo:2194; 4056557.07 north; 590674.57 east 2021-02-12T23:15:50Z- AHRS_M2 msg: 1613171750.65 epoch sec; seqNo:2172 2021-02-12T23:15:50Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.22 psi 2021-02-12T23:15:50Z- handleDepth()-Depth_Keller msg: 1613171750663 epoch sec; seqNo:2195; depth 7.26 2021-02-12T23:15:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171750.67 epoch sec; seqNo:2195; 4056557.45 north; 590674.57 east 2021-02-12T23:15:51Z- AHRS_M2 msg: 1613171751.06 epoch sec; seqNo:2173 2021-02-12T23:15:51Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-12T23:15:51Z- handleDepth()-Depth_Keller msg: 1613171751060 epoch sec; seqNo:2196; depth 7.39 2021-02-12T23:15:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171751.07 epoch sec; seqNo:2196; 4056557.82 north; 590674.57 east 2021-02-12T23:15:51Z- LcmTrn::handleMessages() - handling 44 messages took 0.76 ms and 129 calls... 2021-02-12T23:15:51Z- waiting for 1613171748.83, time is 1613171751.07 2021-02-12T23:15:51Z- LcmTrn::updateTrn() >>>> pose time 1613171751.07, meas time 1613171750.29 <<<< 2021-02-12T23:15:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171750.29 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:15:51Z- TerrainNav::motionUpdate() - currEstimate = (4056558, 590674.57, 7.39) 2021-02-12T23:15:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:15:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:15:51Z- TNavBF::Projecting Measurements 2021-02-12T23:15:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:15:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:15:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:15:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:15:51Z- TNavFilter::4 good beams. 2021-02-12T23:15:51Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:15:51Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:15:51Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:15:51Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:15:51Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:15:51Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:15:51Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:15:51Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:51Z- TerrainMapOctree: (vehN, vehE ) = (4056557.43, 590674.21). 2021-02-12T23:15:51Z- TerrainMapOctree: Min Distance = 5899.71. 2021-02-12T23:15:51Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:15:51Z- There is no specific good beam that all particles have in common. 2021-02-12T23:15:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:15:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 12.27 2021-02-12T23:15:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:15:51Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 19.31 2021-02-12T23:15:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:15:51Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-12T23:15:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:15:51Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.031 ExpectedMeasDiff: nan currMeas.covariance: 24.30 2021-02-12T23:15:51Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:15:51Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:15:51Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:15:51Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:15:51Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:15:51Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:15:51Z- Filter 0 No beams used in subcloud update 2021-02-12T23:15:51Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:15:51Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:15:51Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 786.0 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:15:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171750.29 sec. 2021-02-12T23:15:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:15:51Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:51Z- variances([0], [2], and psi_berg) = 785.955319 833.076720 0.000000 2021-02-12T23:15:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:15:51Z- LcmTrn::publishEstimates() - MLE : 4056533.32 590628.07 7.39 2021-02-12T23:15:51Z- LcmTrn::publishEstimates() - MMSE : 4056557.82 590674.57 7.39 2021-02-12T23:15:51Z- LcmTrn::publishEstimates() - COVAR: 785.96 833.08 0.83 0.00 2021-02-12T23:15:51Z- AHRS_M2 msg: 1613171751.46 epoch sec; seqNo:2174 2021-02-12T23:15:51Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.20 psi 2021-02-12T23:15:51Z- handleDepth()-Depth_Keller msg: 1613171751464 epoch sec; seqNo:2197; depth 7.51 2021-02-12T23:15:51Z- handleDvl() - RDI_Pathfinder msg: 1613171751.48 epoch sec; seqNo:921 2021-02-12T23:15:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.86 , 110.54 , 79.46 , 118.65 2021-02-12T23:15:51Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.11 , -0.14 2021-02-12T23:15:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171751.49 epoch sec; seqNo:2197; 4056558.20 north; 590674.56 east 2021-02-12T23:15:51Z- AHRS_M2 msg: 1613171751.86 epoch sec; seqNo:2175 2021-02-12T23:15:51Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.19 psi 2021-02-12T23:15:51Z- handleDepth()-Depth_Keller msg: 1613171751870 epoch sec; seqNo:2198; depth 7.64 2021-02-12T23:15:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171751.88 epoch sec; seqNo:2198; 4056558.22 north; 590675.78 east 2021-02-12T23:15:52Z- AHRS_M2 msg: 1613171752.27 epoch sec; seqNo:2176 2021-02-12T23:15:52Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.20 psi 2021-02-12T23:15:52Z- handleDepth()-Depth_Keller msg: 1613171752272 epoch sec; seqNo:2199; depth 7.77 2021-02-12T23:15:52Z- handleDvl() - RDI_Pathfinder msg: 1613171752.29 epoch sec; seqNo:922 2021-02-12T23:15:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 105.27 , 76.40 , 113.38 2021-02-12T23:15:52Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.03 , -0.29 2021-02-12T23:15:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171752.30 epoch sec; seqNo:2199; 4056558.59 north; 590675.78 east 2021-02-12T23:15:52Z- AHRS_M2 msg: 1613171752.67 epoch sec; seqNo:2177 2021-02-12T23:15:52Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.20 psi 2021-02-12T23:15:52Z- handleDepth()-Depth_Keller msg: 1613171752676 epoch sec; seqNo:2200; depth 7.91 2021-02-12T23:15:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171752.69 epoch sec; seqNo:2200; 4056558.97 north; 590675.77 east 2021-02-12T23:15:53Z- AHRS_M2 msg: 1613171753.14 epoch sec; seqNo:2178 2021-02-12T23:15:53Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.19 psi 2021-02-12T23:15:53Z- handleDepth()-Depth_Keller msg: 1613171753145 epoch sec; seqNo:2201; depth 8.04 2021-02-12T23:15:53Z- handleDvl() - RDI_Pathfinder msg: 1613171753.16 epoch sec; seqNo:923 2021-02-12T23:15:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.57 , 105.54 , 77.63 , 110.95 2021-02-12T23:15:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.25 , -0.32 2021-02-12T23:15:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171753.17 epoch sec; seqNo:2201; 4056559.73 north; 590675.77 east 2021-02-12T23:15:53Z- AHRS_M2 msg: 1613171753.48 epoch sec; seqNo:2179 2021-02-12T23:15:53Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.19 psi 2021-02-12T23:15:53Z- handleDepth()-Depth_Keller msg: 1613171753488 epoch sec; seqNo:2202; depth 8.19 2021-02-12T23:15:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171753.51 epoch sec; seqNo:2202; 4056560.11 north; 590675.76 east 2021-02-12T23:15:53Z- AHRS_M2 msg: 1613171753.89 epoch sec; seqNo:2180 2021-02-12T23:15:53Z- AHRS_M2 msg: 0.64 phi; -0.28 theta; -0.17 psi 2021-02-12T23:15:53Z- handleDepth()-Depth_Keller msg: 1613171753897 epoch sec; seqNo:2203; depth 8.30 2021-02-12T23:15:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171753.92 epoch sec; seqNo:2203; 4056560.49 north; 590675.76 east 2021-02-12T23:15:54Z- AHRS_M2 msg: 1613171754.29 epoch sec; seqNo:2181 2021-02-12T23:15:54Z- AHRS_M2 msg: 0.64 phi; -0.29 theta; -0.16 psi 2021-02-12T23:15:54Z- handleDepth()-Depth_Keller msg: 1613171754296 epoch sec; seqNo:2204; depth 8.40 2021-02-12T23:15:54Z- handleDvl() - RDI_Pathfinder msg: 1613171754.31 epoch sec; seqNo:924 2021-02-12T23:15:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.63 , 101.56 , 77.63 , 108.31 2021-02-12T23:15:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.38 , -0.19 2021-02-12T23:15:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171754.32 epoch sec; seqNo:2204; 4056560.87 north; 590675.75 east 2021-02-12T23:15:54Z- AHRS_M2 msg: 1613171754.70 epoch sec; seqNo:2182 2021-02-12T23:15:54Z- AHRS_M2 msg: 0.64 phi; -0.29 theta; -0.16 psi 2021-02-12T23:15:54Z- handleDepth()-Depth_Keller msg: 1613171754701 epoch sec; seqNo:2205; depth 8.53 2021-02-12T23:15:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171754.71 epoch sec; seqNo:2205; 4056560.87 north; 590675.75 east 2021-02-12T23:15:55Z- AHRS_M2 msg: 1613171755.10 epoch sec; seqNo:2183 2021-02-12T23:15:55Z- AHRS_M2 msg: 0.64 phi; -0.29 theta; -0.15 psi 2021-02-12T23:15:55Z- handleDepth()-Depth_Keller msg: 1613171755104 epoch sec; seqNo:2206; depth 8.65 2021-02-12T23:15:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171755.12 epoch sec; seqNo:2206; 4056561.25 north; 590675.75 east 2021-02-12T23:15:55Z- AHRS_M2 msg: 1613171755.50 epoch sec; seqNo:2184 2021-02-12T23:15:55Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.15 psi 2021-02-12T23:15:55Z- handleDepth()-Depth_Keller msg: 1613171755508 epoch sec; seqNo:2207; depth 8.76 2021-02-12T23:15:55Z- handleDvl() - RDI_Pathfinder msg: 1613171755.52 epoch sec; seqNo:925 2021-02-12T23:15:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 100.34 , 78.71 , 108.45 2021-02-12T23:15:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.34 , 0.05 2021-02-12T23:15:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171755.53 epoch sec; seqNo:2207; 4056561.64 north; 590676.97 east 2021-02-12T23:15:55Z- AHRS_M2 msg: 1613171755.91 epoch sec; seqNo:2185 2021-02-12T23:15:55Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.15 psi 2021-02-12T23:15:55Z- handleDepth()-Depth_Keller msg: 1613171755914 epoch sec; seqNo:2208; depth 8.86 2021-02-12T23:15:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171755.92 epoch sec; seqNo:2208; 4056562.02 north; 590676.96 east 2021-02-12T23:15:56Z- AHRS_M2 msg: 1613171756.31 epoch sec; seqNo:2186 2021-02-12T23:15:56Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.15 psi 2021-02-12T23:15:56Z- LcmTrn::handleMessages() - handling 42 messages took 0.61 ms and 130 calls... 2021-02-12T23:15:56Z- waiting for 1613171754.07, time is 1613171756.31 2021-02-12T23:15:56Z- LcmTrn::updateTrn() >>>> pose time 1613171756.31, meas time 1613171755.52 <<<< 2021-02-12T23:15:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171755.52 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:15:56Z- TerrainNav::motionUpdate() - currEstimate = (4056562, 590676.96, 8.86) 2021-02-12T23:15:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:15:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:15:56Z- TNavBF::Projecting Measurements 2021-02-12T23:15:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:15:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:15:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:15:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:15:56Z- TNavFilter::4 good beams. 2021-02-12T23:15:56Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:15:56Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:15:56Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:15:56Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:15:56Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:15:56Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:15:56Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:15:56Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:56Z- TerrainMapOctree: (vehN, vehE ) = (4056561.38, 590676.60). 2021-02-12T23:15:56Z- TerrainMapOctree: Min Distance = 5895.11. 2021-02-12T23:15:56Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:15:56Z- There is no specific good beam that all particles have in common. 2021-02-12T23:15:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:15:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 10.96 2021-02-12T23:15:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:15:56Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 16.11 2021-02-12T23:15:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:15:56Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 9.91 2021-02-12T23:15:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:15:56Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 18.82 2021-02-12T23:15:56Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:15:56Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:15:56Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:15:56Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:15:56Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:15:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:15:56Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:15:56Z- Filter 0 No beams used in subcloud update 2021-02-12T23:15:56Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:15:56Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:15:56Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 785.8 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:15:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171755.52 sec. 2021-02-12T23:15:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:15:56Z- TNavBF::mmse selected from filter -1 2021-02-12T23:15:56Z- variances([0], [2], and psi_berg) = 785.711307 833.076329 0.000000 2021-02-12T23:15:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:15:56Z- LcmTrn::publishEstimates() - MLE : 4056537.52 590630.46 8.86 2021-02-12T23:15:56Z- LcmTrn::publishEstimates() - MMSE : 4056562.02 590676.96 8.86 2021-02-12T23:15:56Z- LcmTrn::publishEstimates() - COVAR: 785.71 833.08 0.85 0.00 2021-02-12T23:15:56Z- handleDepth()-Depth_Keller msg: 1613171756321 epoch sec; seqNo:2209; depth 8.97 2021-02-12T23:15:56Z- handleDvl() - RDI_Pathfinder msg: 1613171756.34 epoch sec; seqNo:926 2021-02-12T23:15:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 96.28 , 78.71 , 108.45 2021-02-12T23:15:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.63 , -0.14 , 0.23 2021-02-12T23:15:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171756.35 epoch sec; seqNo:2209; 4056562.02 north; 590676.96 east 2021-02-12T23:15:56Z- AHRS_M2 msg: 1613171756.71 epoch sec; seqNo:2187 2021-02-12T23:15:56Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.15 psi 2021-02-12T23:15:56Z- handleDepth()-Depth_Keller msg: 1613171756720 epoch sec; seqNo:2210; depth 9.09 2021-02-12T23:15:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171756.73 epoch sec; seqNo:2210; 4056562.40 north; 590676.96 east 2021-02-12T23:15:57Z- AHRS_M2 msg: 1613171757.13 epoch sec; seqNo:2188 2021-02-12T23:15:57Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.16 psi 2021-02-12T23:15:57Z- handleDepth()-Depth_Keller msg: 1613171757135 epoch sec; seqNo:2211; depth 9.19 2021-02-12T23:15:57Z- handleDvl() - RDI_Pathfinder msg: 1613171757.15 epoch sec; seqNo:927 2021-02-12T23:15:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.54 , 99.05 , 74.77 , 111.22 2021-02-12T23:15:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , 0.04 , 0.17 2021-02-12T23:15:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171757.17 epoch sec; seqNo:2211; 4056562.40 north; 590676.96 east 2021-02-12T23:15:57Z- AHRS_M2 msg: 1613171757.53 epoch sec; seqNo:2189 2021-02-12T23:15:57Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.17 psi 2021-02-12T23:15:57Z- handleDepth()-Depth_Keller msg: 1613171757530 epoch sec; seqNo:2212; depth 9.29 2021-02-12T23:15:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171757.54 epoch sec; seqNo:2212; 4056562.77 north; 590676.96 east 2021-02-12T23:15:57Z- AHRS_M2 msg: 1613171757.93 epoch sec; seqNo:2190 2021-02-12T23:15:57Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.18 psi 2021-02-12T23:15:57Z- handleDepth()-Depth_Keller msg: 1613171757932 epoch sec; seqNo:2213; depth 9.39 2021-02-12T23:15:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171757.95 epoch sec; seqNo:2213; 4056562.77 north; 590676.96 east 2021-02-12T23:15:58Z- AHRS_M2 msg: 1613171758.33 epoch sec; seqNo:2191 2021-02-12T23:15:58Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.19 psi 2021-02-12T23:15:58Z- handleDepth()-Depth_Keller msg: 1613171758336 epoch sec; seqNo:2214; depth 9.51 2021-02-12T23:15:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171758.35 epoch sec; seqNo:2214; 4056563.15 north; 590676.95 east 2021-02-12T23:15:58Z- AHRS_M2 msg: 1613171758.74 epoch sec; seqNo:2192 2021-02-12T23:15:58Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.20 psi 2021-02-12T23:15:58Z- handleDepth()-Depth_Keller msg: 1613171758740 epoch sec; seqNo:2215; depth 9.61 2021-02-12T23:15:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171758.75 epoch sec; seqNo:2215; 4056563.53 north; 590676.95 east 2021-02-12T23:15:59Z- AHRS_M2 msg: 1613171759.15 epoch sec; seqNo:2193 2021-02-12T23:15:59Z- AHRS_M2 msg: 0.58 phi; -0.27 theta; -0.22 psi 2021-02-12T23:15:59Z- handleDepth()-Depth_Keller msg: 1613171759151 epoch sec; seqNo:2216; depth 9.70 2021-02-12T23:15:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171759.18 epoch sec; seqNo:2216; 4056563.55 north; 590678.17 east 2021-02-12T23:15:59Z- AHRS_M2 msg: 1613171759.54 epoch sec; seqNo:2194 2021-02-12T23:15:59Z- AHRS_M2 msg: 0.57 phi; -0.27 theta; -0.23 psi 2021-02-12T23:15:59Z- handleDepth()-Depth_Keller msg: 1613171759548 epoch sec; seqNo:2217; depth 9.81 2021-02-12T23:15:59Z- handleDvl() - RDI_Pathfinder msg: 1613171759.57 epoch sec; seqNo:928 2021-02-12T23:15:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.83 , 96.35 , 78.78 , 108.52 2021-02-12T23:15:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.07 , 0.01 2021-02-12T23:15:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171759.58 epoch sec; seqNo:2217; 4056563.92 north; 590678.16 east 2021-02-12T23:15:59Z- AHRS_M2 msg: 1613171759.95 epoch sec; seqNo:2195 2021-02-12T23:15:59Z- AHRS_M2 msg: 0.57 phi; -0.27 theta; -0.22 psi 2021-02-12T23:15:59Z- handleDepth()-Depth_Keller msg: 1613171759957 epoch sec; seqNo:2218; depth 9.92 2021-02-12T23:15:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171759.97 epoch sec; seqNo:2218; 4056564.30 north; 590678.16 east 2021-02-12T23:16:00Z- AHRS_M2 msg: 1613171760.35 epoch sec; seqNo:2196 2021-02-12T23:16:00Z- AHRS_M2 msg: 0.57 phi; -0.27 theta; -0.22 psi 2021-02-12T23:16:00Z- handleDepth()-Depth_Keller msg: 1613171760356 epoch sec; seqNo:2219; depth 10.02 2021-02-12T23:16:00Z- handleDvl() - RDI_Pathfinder msg: 1613171760.37 epoch sec; seqNo:929 2021-02-12T23:16:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.67 , 96.42 , 78.17 , 107.37 2021-02-12T23:16:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , 0.14 2021-02-12T23:16:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171760.38 epoch sec; seqNo:2219; 4056564.68 north; 590678.16 east 2021-02-12T23:16:00Z- AHRS_M2 msg: 1613171760.76 epoch sec; seqNo:2197 2021-02-12T23:16:00Z- AHRS_M2 msg: 0.57 phi; -0.28 theta; -0.21 psi 2021-02-12T23:16:00Z- handleDepth()-Depth_Keller msg: 1613171760762 epoch sec; seqNo:2220; depth 10.12 2021-02-12T23:16:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171760.78 epoch sec; seqNo:2220; 4056564.68 north; 590678.16 east 2021-02-12T23:16:01Z- AHRS_M2 msg: 1613171761.16 epoch sec; seqNo:2198 2021-02-12T23:16:01Z- AHRS_M2 msg: 0.57 phi; -0.28 theta; -0.20 psi 2021-02-12T23:16:01Z- handleDepth()-Depth_Keller msg: 1613171761169 epoch sec; seqNo:2221; depth 10.24 2021-02-12T23:16:01Z- handleDvl() - RDI_Pathfinder msg: 1613171761.20 epoch sec; seqNo:930 2021-02-12T23:16:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.49 , 78.24 , 106.22 2021-02-12T23:16:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , 0.12 2021-02-12T23:16:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171761.21 epoch sec; seqNo:2221; 4056565.06 north; 590678.15 east 2021-02-12T23:16:01Z- LcmTrn::handleMessages() - handling 43 messages took 0.66 ms and 130 calls... 2021-02-12T23:16:01Z- waiting for 1613171759.31, time is 1613171761.21 2021-02-12T23:16:01Z- LcmTrn::updateTrn() equating pose 1613171761.21 and meas times 1613171761.21 2021-02-12T23:16:01Z- LcmTrn::updateTrn() >>>> pose time 1613171761.21, meas time 1613171761.21 <<<< 2021-02-12T23:16:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171761.21 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:16:01Z- TerrainNav::motionUpdate() - currEstimate = (4056565, 590678.15, 10.24) 2021-02-12T23:16:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:16:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:16:01Z- TNavBF::Projecting Measurements 2021-02-12T23:16:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:16:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:16:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:16:01Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:16:01Z- TNavFilter::3 good beams. 2021-02-12T23:16:01Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:16:01Z- TNavBF:: beamIndex[0] = 1 2021-02-12T23:16:01Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:16:01Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:16:01Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:16:01Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:16:01Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:16:01Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:01Z- TerrainMapOctree: (vehN, vehE ) = (4056565.06, 590678.15). 2021-02-12T23:16:01Z- TerrainMapOctree: Min Distance = 5891.24. 2021-02-12T23:16:01Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:16:01Z- There is no specific good beam that all particles have in common. 2021-02-12T23:16:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:16:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 14.90 2021-02-12T23:16:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:16:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 9.79 2021-02-12T23:16:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:16:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 18.05 2021-02-12T23:16:01Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:16:01Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:16:01Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:16:01Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:16:01Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:16:01Z- Filter 0 No beams used in subcloud update 2021-02-12T23:16:01Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:16:01Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:16:01Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 785.6 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:16:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171761.21 sec. 2021-02-12T23:16:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:16:01Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:01Z- variances([0], [2], and psi_berg) = 785.558668 833.076085 0.000000 2021-02-12T23:16:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:16:01Z- LcmTrn::publishEstimates() - MLE : 4056540.56 590631.65 10.24 2021-02-12T23:16:01Z- LcmTrn::publishEstimates() - MMSE : 4056565.06 590678.15 10.24 2021-02-12T23:16:01Z- LcmTrn::publishEstimates() - COVAR: 785.56 833.08 0.85 0.00 2021-02-12T23:16:01Z- AHRS_M2 msg: 1613171761.56 epoch sec; seqNo:2199 2021-02-12T23:16:01Z- AHRS_M2 msg: 0.57 phi; -0.28 theta; -0.18 psi 2021-02-12T23:16:01Z- handleDepth()-Depth_Keller msg: 1613171761569 epoch sec; seqNo:2222; depth 10.34 2021-02-12T23:16:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171761.58 epoch sec; seqNo:2222; 4056565.44 north; 590678.15 east 2021-02-12T23:16:01Z- AHRS_M2 msg: 1613171761.97 epoch sec; seqNo:2200 2021-02-12T23:16:01Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.17 psi 2021-02-12T23:16:01Z- handleDepth()-Depth_Keller msg: 1613171761973 epoch sec; seqNo:2223; depth 10.44 2021-02-12T23:16:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171761.99 epoch sec; seqNo:2223; 4056565.45 north; 590679.37 east 2021-02-12T23:16:02Z- AHRS_M2 msg: 1613171762.38 epoch sec; seqNo:2201 2021-02-12T23:16:02Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.16 psi 2021-02-12T23:16:02Z- handleDepth()-Depth_Keller msg: 1613171762382 epoch sec; seqNo:2224; depth 10.55 2021-02-12T23:16:02Z- handleDvl() - RDI_Pathfinder msg: 1613171762.40 epoch sec; seqNo:931 2021-02-12T23:16:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.70 , 96.22 , 78.64 , 107.03 2021-02-12T23:16:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , -0.02 2021-02-12T23:16:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171762.41 epoch sec; seqNo:2224; 4056565.83 north; 590679.37 east 2021-02-12T23:16:02Z- AHRS_M2 msg: 1613171762.78 epoch sec; seqNo:2202 2021-02-12T23:16:02Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.17 psi 2021-02-12T23:16:02Z- handleDepth()-Depth_Keller msg: 1613171762788 epoch sec; seqNo:2225; depth 10.65 2021-02-12T23:16:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171762.80 epoch sec; seqNo:2225; 4056566.21 north; 590679.36 east 2021-02-12T23:16:03Z- AHRS_M2 msg: 1613171763.18 epoch sec; seqNo:2203 2021-02-12T23:16:03Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.17 psi 2021-02-12T23:16:03Z- handleDepth()-Depth_Keller msg: 1613171763191 epoch sec; seqNo:2226; depth 10.74 2021-02-12T23:16:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171763.23 epoch sec; seqNo:2226; 4056566.59 north; 590679.36 east 2021-02-12T23:16:03Z- AHRS_M2 msg: 1613171763.58 epoch sec; seqNo:2204 2021-02-12T23:16:03Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.17 psi 2021-02-12T23:16:03Z- handleDepth()-Depth_Keller msg: 1613171763590 epoch sec; seqNo:2227; depth 10.84 2021-02-12T23:16:03Z- handleDvl() - RDI_Pathfinder msg: 1613171763.60 epoch sec; seqNo:932 2021-02-12T23:16:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 96.42 , 78.17 , 106.15 2021-02-12T23:16:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , -0.13 2021-02-12T23:16:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171763.61 epoch sec; seqNo:2227; 4056566.97 north; 590679.35 east 2021-02-12T23:16:03Z- AHRS_M2 msg: 1613171763.99 epoch sec; seqNo:2205 2021-02-12T23:16:03Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.17 psi 2021-02-12T23:16:04Z- handleDepth()-Depth_Keller msg: 1613171764006 epoch sec; seqNo:2228; depth 10.94 2021-02-12T23:16:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171764.02 epoch sec; seqNo:2228; 4056566.97 north; 590679.35 east 2021-02-12T23:16:04Z- AHRS_M2 msg: 1613171764.39 epoch sec; seqNo:2206 2021-02-12T23:16:04Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.16 psi 2021-02-12T23:16:04Z- handleDepth()-Depth_Keller msg: 1613171764396 epoch sec; seqNo:2229; depth 11.04 2021-02-12T23:16:04Z- handleDvl() - RDI_Pathfinder msg: 1613171764.41 epoch sec; seqNo:933 2021-02-12T23:16:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.67 , 96.49 , 78.24 , 107.43 2021-02-12T23:16:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.14 , -0.10 2021-02-12T23:16:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171764.43 epoch sec; seqNo:2229; 4056567.36 north; 590680.57 east 2021-02-12T23:16:04Z- AHRS_M2 msg: 1613171764.80 epoch sec; seqNo:2207 2021-02-12T23:16:04Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.15 psi 2021-02-12T23:16:04Z- handleDepth()-Depth_Keller msg: 1613171764803 epoch sec; seqNo:2230; depth 11.15 2021-02-12T23:16:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171764.81 epoch sec; seqNo:2230; 4056567.74 north; 590680.57 east 2021-02-12T23:16:05Z- AHRS_M2 msg: 1613171765.22 epoch sec; seqNo:2208 2021-02-12T23:16:05Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.15 psi 2021-02-12T23:16:05Z- handleDepth()-Depth_Keller msg: 1613171765230 epoch sec; seqNo:2231; depth 11.24 2021-02-12T23:16:05Z- handleDvl() - RDI_Pathfinder msg: 1613171765.24 epoch sec; seqNo:934 2021-02-12T23:16:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 96.49 , 78.24 , 106.22 2021-02-12T23:16:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.14 , -0.04 2021-02-12T23:16:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171765.26 epoch sec; seqNo:2231; 4056568.12 north; 590680.56 east 2021-02-12T23:16:05Z- AHRS_M2 msg: 1613171765.60 epoch sec; seqNo:2209 2021-02-12T23:16:05Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.15 psi 2021-02-12T23:16:05Z- handleDepth()-Depth_Keller msg: 1613171765608 epoch sec; seqNo:2232; depth 11.38 2021-02-12T23:16:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171765.62 epoch sec; seqNo:2232; 4056568.50 north; 590680.56 east 2021-02-12T23:16:06Z- AHRS_M2 msg: 1613171766.01 epoch sec; seqNo:2210 2021-02-12T23:16:06Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.15 psi 2021-02-12T23:16:06Z- handleDepth()-Depth_Keller msg: 1613171766017 epoch sec; seqNo:2233; depth 11.47 2021-02-12T23:16:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171766.03 epoch sec; seqNo:2233; 4056568.87 north; 590680.55 east 2021-02-12T23:16:06Z- AHRS_M2 msg: 1613171766.42 epoch sec; seqNo:2211 2021-02-12T23:16:06Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.14 psi 2021-02-12T23:16:06Z- handleDepth()-Depth_Keller msg: 1613171766438 epoch sec; seqNo:2234; depth 11.58 2021-02-12T23:16:06Z- handleDvl() - RDI_Pathfinder msg: 1613171766.47 epoch sec; seqNo:935 2021-02-12T23:16:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 95.21 , 78.18 , 106.15 2021-02-12T23:16:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.13 , -0.06 2021-02-12T23:16:06Z- LcmTrn::handleMessages() - handling 43 messages took 0.74 ms and 128 calls... 2021-02-12T23:16:06Z- waiting for 1613171764.21, time is 1613171766.47 2021-02-12T23:16:06Z- LcmTrn::updateTrn() equating pose 1613171766.47 and meas times 1613171766.47 2021-02-12T23:16:06Z- LcmTrn::updateTrn() >>>> pose time 1613171766.47, meas time 1613171766.47 <<<< 2021-02-12T23:16:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171766.47 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:16:06Z- TerrainNav::motionUpdate() - currEstimate = (4056569, 590680.55, 11.58) 2021-02-12T23:16:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:16:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:16:06Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171766.47 sec.; No relevant navigation data available 2021-02-12T23:16:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:16:06Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:06Z- variances([0], [2], and psi_berg) = 785.379891 833.075801 0.000000 2021-02-12T23:16:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:16:06Z- LcmTrn::publishEstimates() - MLE : 4056544.37 590634.05 11.58 2021-02-12T23:16:06Z- LcmTrn::publishEstimates() - MMSE : 4056568.87 590680.55 11.58 2021-02-12T23:16:06Z- LcmTrn::publishEstimates() - COVAR: 785.38 833.08 1.07 0.00 2021-02-12T23:16:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171766.49 epoch sec; seqNo:2234; 4056569.25 north; 590680.55 east 2021-02-12T23:16:06Z- AHRS_M2 msg: 1613171766.82 epoch sec; seqNo:2212 2021-02-12T23:16:06Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.14 psi 2021-02-12T23:16:06Z- handleDepth()-Depth_Keller msg: 1613171766830 epoch sec; seqNo:2235; depth 11.69 2021-02-12T23:16:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171766.84 epoch sec; seqNo:2235; 4056569.25 north; 590680.55 east 2021-02-12T23:16:07Z- AHRS_M2 msg: 1613171767.23 epoch sec; seqNo:2213 2021-02-12T23:16:07Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.14 psi 2021-02-12T23:16:07Z- handleDepth()-Depth_Keller msg: 1613171767235 epoch sec; seqNo:2236; depth 11.79 2021-02-12T23:16:07Z- handleDvl() - RDI_Pathfinder msg: 1613171767.25 epoch sec; seqNo:936 2021-02-12T23:16:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.75 , 92.91 , 78.31 , 107.50 2021-02-12T23:16:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.17 , -0.10 2021-02-12T23:16:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171767.26 epoch sec; seqNo:2236; 4056569.63 north; 590680.55 east 2021-02-12T23:16:07Z- AHRS_M2 msg: 1613171767.62 epoch sec; seqNo:2214 2021-02-12T23:16:07Z- AHRS_M2 msg: 0.66 phi; -0.30 theta; -0.14 psi 2021-02-12T23:16:07Z- handleDepth()-Depth_Keller msg: 1613171767628 epoch sec; seqNo:2237; depth 11.89 2021-02-12T23:16:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:16:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:16:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:16:07Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:16:07Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:16:07Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:16:07Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:16:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:16:07Z- handleDvl() - RDI_Pathfinder msg: 1613171767.63 epoch sec; seqNo:937 2021-02-12T23:16:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:16:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.17 , -0.10 2021-02-12T23:16:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171767.64 epoch sec; seqNo:2237; 4056570.02 north; 590681.76 east 2021-02-12T23:16:08Z- AHRS_M2 msg: 1613171768.03 epoch sec; seqNo:2215 2021-02-12T23:16:08Z- AHRS_M2 msg: 0.67 phi; -0.30 theta; -0.13 psi 2021-02-12T23:16:08Z- handleDepth()-Depth_Keller msg: 1613171768037 epoch sec; seqNo:2238; depth 12.00 2021-02-12T23:16:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171768.05 epoch sec; seqNo:2238; 4056570.40 north; 590681.76 east 2021-02-12T23:16:08Z- AHRS_M2 msg: 1613171768.43 epoch sec; seqNo:2216 2021-02-12T23:16:08Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.13 psi 2021-02-12T23:16:08Z- handleDepth()-Depth_Keller msg: 1613171768438 epoch sec; seqNo:2239; depth 12.11 2021-02-12T23:16:08Z- handleDvl() - RDI_Pathfinder msg: 1613171768.45 epoch sec; seqNo:938 2021-02-12T23:16:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 94.12 , 72.32 , 105.07 2021-02-12T23:16:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.22 , -0.09 2021-02-12T23:16:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171768.46 epoch sec; seqNo:2239; 4056570.78 north; 590681.76 east 2021-02-12T23:16:08Z- AHRS_M2 msg: 1613171768.84 epoch sec; seqNo:2217 2021-02-12T23:16:08Z- AHRS_M2 msg: 0.68 phi; -0.31 theta; -0.14 psi 2021-02-12T23:16:08Z- handleDepth()-Depth_Keller msg: 1613171768840 epoch sec; seqNo:2240; depth 12.24 2021-02-12T23:16:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:16:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:16:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:16:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:16:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:16:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:16:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:16:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:16:08Z- handleDvl() - RDI_Pathfinder msg: 1613171768.84 epoch sec; seqNo:939 2021-02-12T23:16:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:16:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.22 , -0.09 2021-02-12T23:16:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171768.87 epoch sec; seqNo:2240; 4056571.16 north; 590681.75 east 2021-02-12T23:16:09Z- AHRS_M2 msg: 1613171769.24 epoch sec; seqNo:2218 2021-02-12T23:16:09Z- AHRS_M2 msg: 0.68 phi; -0.31 theta; -0.14 psi 2021-02-12T23:16:09Z- handleDepth()-Depth_Keller msg: 1613171769245 epoch sec; seqNo:2241; depth 12.34 2021-02-12T23:16:09Z- handleDvl() - RDI_Pathfinder msg: 1613171769.26 epoch sec; seqNo:940 2021-02-12T23:16:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.66 , 93.04 , 73.13 , 106.42 2021-02-12T23:16:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.25 , -0.05 2021-02-12T23:16:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171769.28 epoch sec; seqNo:2241; 4056571.54 north; 590681.75 east 2021-02-12T23:16:09Z- AHRS_M2 msg: 1613171769.64 epoch sec; seqNo:2219 2021-02-12T23:16:09Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.16 psi 2021-02-12T23:16:09Z- handleDepth()-Depth_Keller msg: 1613171769648 epoch sec; seqNo:2242; depth 12.46 2021-02-12T23:16:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171769.66 epoch sec; seqNo:2242; 4056571.54 north; 590681.75 east 2021-02-12T23:16:10Z- AHRS_M2 msg: 1613171770.05 epoch sec; seqNo:2220 2021-02-12T23:16:10Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.17 psi 2021-02-12T23:16:10Z- handleDepth()-Depth_Keller msg: 1613171770054 epoch sec; seqNo:2243; depth 12.54 2021-02-12T23:16:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:16:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:16:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:16:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:16:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:16:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:16:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:16:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:16:10Z- handleDvl() - RDI_Pathfinder msg: 1613171770.06 epoch sec; seqNo:941 2021-02-12T23:16:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:16:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.25 , -0.05 2021-02-12T23:16:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171770.06 epoch sec; seqNo:2243; 4056571.92 north; 590681.74 east 2021-02-12T23:16:10Z- AHRS_M2 msg: 1613171770.45 epoch sec; seqNo:2221 2021-02-12T23:16:10Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.17 psi 2021-02-12T23:16:10Z- handleDepth()-Depth_Keller msg: 1613171770457 epoch sec; seqNo:2244; depth 12.65 2021-02-12T23:16:10Z- handleDvl() - RDI_Pathfinder msg: 1613171770.47 epoch sec; seqNo:942 2021-02-12T23:16:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.88 , 93.04 , 71.91 , 107.64 2021-02-12T23:16:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.24 , 0.05 2021-02-12T23:16:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171770.49 epoch sec; seqNo:2244; 4056572.30 north; 590681.74 east 2021-02-12T23:16:10Z- AHRS_M2 msg: 1613171770.86 epoch sec; seqNo:2222 2021-02-12T23:16:10Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.17 psi 2021-02-12T23:16:10Z- handleDepth()-Depth_Keller msg: 1613171770865 epoch sec; seqNo:2245; depth 12.78 2021-02-12T23:16:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171770.88 epoch sec; seqNo:2245; 4056572.30 north; 590681.74 east 2021-02-12T23:16:11Z- AHRS_M2 msg: 1613171771.26 epoch sec; seqNo:2223 2021-02-12T23:16:11Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-12T23:16:11Z- handleDepth()-Depth_Keller msg: 1613171771268 epoch sec; seqNo:2246; depth 12.88 2021-02-12T23:16:11Z- handleDvl() - RDI_Pathfinder msg: 1613171771.28 epoch sec; seqNo:943 2021-02-12T23:16:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.88 , 95.47 , 71.91 , 108.85 2021-02-12T23:16:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.12 , 0.17 2021-02-12T23:16:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171771.29 epoch sec; seqNo:2246; 4056572.68 north; 590681.74 east 2021-02-12T23:16:11Z- LcmTrn::handleMessages() - handling 45 messages took 0.65 ms and 132 calls... 2021-02-12T23:16:11Z- waiting for 1613171769.47, time is 1613171771.29 2021-02-12T23:16:11Z- LcmTrn::updateTrn() equating pose 1613171771.29 and meas times 1613171771.29 2021-02-12T23:16:11Z- LcmTrn::updateTrn() >>>> pose time 1613171771.29, meas time 1613171771.29 <<<< 2021-02-12T23:16:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171771.29 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:16:11Z- TerrainNav::motionUpdate() - currEstimate = (4056573, 590681.74, 12.88) 2021-02-12T23:16:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:16:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:16:11Z- TNavBF::Projecting Measurements 2021-02-12T23:16:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:16:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:16:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:16:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:16:11Z- TNavFilter::4 good beams. 2021-02-12T23:16:11Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:16:11Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:16:11Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:16:11Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:16:11Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:16:11Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:16:11Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:16:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:11Z- TerrainMapOctree: (vehN, vehE ) = (4056572.68, 590681.74). 2021-02-12T23:16:11Z- TerrainMapOctree: Min Distance = 5883.02. 2021-02-12T23:16:11Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:16:11Z- There is no specific good beam that all particles have in common. 2021-02-12T23:16:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:16:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 10.47 2021-02-12T23:16:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:16:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 14.58 2021-02-12T23:16:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:16:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 8.27 2021-02-12T23:16:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:16:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 18.96 2021-02-12T23:16:11Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:16:11Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:16:11Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:16:11Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:16:11Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:16:11Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:16:11Z- Filter 0 No beams used in subcloud update 2021-02-12T23:16:11Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:16:11Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:16:11Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 785.2 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:16:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171771.29 sec. 2021-02-12T23:16:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:16:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:11Z- variances([0], [2], and psi_berg) = 785.212442 833.075536 0.000000 2021-02-12T23:16:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:16:11Z- LcmTrn::publishEstimates() - MLE : 4056548.18 590635.24 12.88 2021-02-12T23:16:11Z- LcmTrn::publishEstimates() - MMSE : 4056572.68 590681.74 12.88 2021-02-12T23:16:11Z- LcmTrn::publishEstimates() - COVAR: 785.21 833.08 0.93 0.00 2021-02-12T23:16:11Z- AHRS_M2 msg: 1613171771.66 epoch sec; seqNo:2224 2021-02-12T23:16:11Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-12T23:16:11Z- handleDepth()-Depth_Keller msg: 1613171771677 epoch sec; seqNo:2247; depth 13.00 2021-02-12T23:16:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171771.69 epoch sec; seqNo:2247; 4056573.07 north; 590682.95 east 2021-02-12T23:16:12Z- AHRS_M2 msg: 1613171772.07 epoch sec; seqNo:2225 2021-02-12T23:16:12Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.19 psi 2021-02-12T23:16:12Z- handleDepth()-Depth_Keller msg: 1613171772078 epoch sec; seqNo:2248; depth 13.11 2021-02-12T23:16:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171772.09 epoch sec; seqNo:2248; 4056573.07 north; 590682.95 east 2021-02-12T23:16:12Z- AHRS_M2 msg: 1613171772.47 epoch sec; seqNo:2226 2021-02-12T23:16:12Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.18 psi 2021-02-12T23:16:12Z- handleDepth()-Depth_Keller msg: 1613171772476 epoch sec; seqNo:2249; depth 13.23 2021-02-12T23:16:12Z- handleDvl() - RDI_Pathfinder msg: 1613171772.49 epoch sec; seqNo:944 2021-02-12T23:16:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.81 , 96.62 , 73.13 , 110.00 2021-02-12T23:16:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.06 , 0.23 2021-02-12T23:16:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171772.50 epoch sec; seqNo:2249; 4056573.45 north; 590682.95 east 2021-02-12T23:16:12Z- AHRS_M2 msg: 1613171772.88 epoch sec; seqNo:2227 2021-02-12T23:16:12Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.18 psi 2021-02-12T23:16:12Z- handleDepth()-Depth_Keller msg: 1613171772885 epoch sec; seqNo:2250; depth 13.36 2021-02-12T23:16:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171772.90 epoch sec; seqNo:2250; 4056573.45 north; 590682.95 east 2021-02-12T23:16:13Z- AHRS_M2 msg: 1613171773.28 epoch sec; seqNo:2228 2021-02-12T23:16:13Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.18 psi 2021-02-12T23:16:13Z- handleDepth()-Depth_Keller msg: 1613171773287 epoch sec; seqNo:2251; depth 13.48 2021-02-12T23:16:13Z- handleDvl() - RDI_Pathfinder msg: 1613171773.31 epoch sec; seqNo:945 2021-02-12T23:16:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.67 , 96.49 , 78.24 , 111.08 2021-02-12T23:16:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.21 , 0.16 2021-02-12T23:16:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171773.32 epoch sec; seqNo:2251; 4056573.83 north; 590682.94 east 2021-02-12T23:16:13Z- AHRS_M2 msg: 1613171773.68 epoch sec; seqNo:2229 2021-02-12T23:16:13Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.18 psi 2021-02-12T23:16:13Z- handleDepth()-Depth_Keller msg: 1613171773688 epoch sec; seqNo:2252; depth 13.59 2021-02-12T23:16:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171773.70 epoch sec; seqNo:2252; 4056573.83 north; 590682.94 east 2021-02-12T23:16:14Z- AHRS_M2 msg: 1613171774.09 epoch sec; seqNo:2230 2021-02-12T23:16:14Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.19 psi 2021-02-12T23:16:14Z- handleDepth()-Depth_Keller msg: 1613171774094 epoch sec; seqNo:2253; depth 13.70 2021-02-12T23:16:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171774.11 epoch sec; seqNo:2253; 4056574.22 north; 590684.16 east 2021-02-12T23:16:14Z- AHRS_M2 msg: 1613171774.49 epoch sec; seqNo:2231 2021-02-12T23:16:14Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-12T23:16:14Z- handleDepth()-Depth_Keller msg: 1613171774497 epoch sec; seqNo:2254; depth 13.83 2021-02-12T23:16:14Z- handleDvl() - RDI_Pathfinder msg: 1613171774.51 epoch sec; seqNo:946 2021-02-12T23:16:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 96.42 , 71.09 , 109.80 2021-02-12T23:16:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.23 , -0.01 2021-02-12T23:16:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171774.52 epoch sec; seqNo:2254; 4056574.22 north; 590684.16 east 2021-02-12T23:16:14Z- AHRS_M2 msg: 1613171774.90 epoch sec; seqNo:2232 2021-02-12T23:16:14Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.20 psi 2021-02-12T23:16:14Z- handleDepth()-Depth_Keller msg: 1613171774901 epoch sec; seqNo:2255; depth 13.94 2021-02-12T23:16:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171774.91 epoch sec; seqNo:2255; 4056574.60 north; 590684.16 east 2021-02-12T23:16:15Z- AHRS_M2 msg: 1613171775.30 epoch sec; seqNo:2233 2021-02-12T23:16:15Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.21 psi 2021-02-12T23:16:15Z- handleDepth()-Depth_Keller msg: 1613171775304 epoch sec; seqNo:2256; depth 14.06 2021-02-12T23:16:15Z- handleDvl() - RDI_Pathfinder msg: 1613171775.32 epoch sec; seqNo:947 2021-02-12T23:16:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.26 , 96.55 , 71.91 , 111.15 2021-02-12T23:16:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.08 , -0.13 2021-02-12T23:16:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171775.33 epoch sec; seqNo:2256; 4056574.98 north; 590684.15 east 2021-02-12T23:16:15Z- AHRS_M2 msg: 1613171775.70 epoch sec; seqNo:2234 2021-02-12T23:16:15Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.21 psi 2021-02-12T23:16:15Z- handleDepth()-Depth_Keller msg: 1613171775710 epoch sec; seqNo:2257; depth 14.20 2021-02-12T23:16:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171775.72 epoch sec; seqNo:2257; 4056574.99 north; 590685.37 east 2021-02-12T23:16:16Z- AHRS_M2 msg: 1613171776.11 epoch sec; seqNo:2235 2021-02-12T23:16:16Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.21 psi 2021-02-12T23:16:16Z- handleDepth()-Depth_Keller msg: 1613171776137 epoch sec; seqNo:2258; depth 14.31 2021-02-12T23:16:16Z- handleDvl() - RDI_Pathfinder msg: 1613171776.16 epoch sec; seqNo:948 2021-02-12T23:16:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.63 , 96.55 , 78.31 , 109.93 2021-02-12T23:16:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.11 , -0.10 2021-02-12T23:16:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171776.18 epoch sec; seqNo:2258; 4056575.37 north; 590685.37 east 2021-02-12T23:16:16Z- AHRS_M2 msg: 1613171776.51 epoch sec; seqNo:2236 2021-02-12T23:16:16Z- AHRS_M2 msg: 0.70 phi; -0.34 theta; -0.21 psi 2021-02-12T23:16:16Z- handleDepth()-Depth_Keller msg: 1613171776516 epoch sec; seqNo:2259; depth 14.45 2021-02-12T23:16:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171776.53 epoch sec; seqNo:2259; 4056575.75 north; 590685.37 east 2021-02-12T23:16:16Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-12T23:16:16Z- waiting for 1613171774.29, time is 1613171776.53 2021-02-12T23:16:16Z- LcmTrn::updateTrn() >>>> pose time 1613171776.53, meas time 1613171776.16 <<<< 2021-02-12T23:16:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171776.16 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:16:16Z- TerrainNav::motionUpdate() - currEstimate = (4056576, 590685.37, 14.45) 2021-02-12T23:16:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:16:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:16:16Z- TNavBF::Projecting Measurements 2021-02-12T23:16:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:16:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:16:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:16:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:16:16Z- TNavFilter::4 good beams. 2021-02-12T23:16:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:16:16Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:16:16Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:16:16Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:16:16Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:16:16Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:16:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:16:16Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:16Z- TerrainMapOctree: (vehN, vehE ) = (4056575.53, 590685.11). 2021-02-12T23:16:16Z- TerrainMapOctree: Min Distance = 5878.70. 2021-02-12T23:16:16Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:16:16Z- There is no specific good beam that all particles have in common. 2021-02-12T23:16:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:16:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 9.64 2021-02-12T23:16:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:16:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 14.92 2021-02-12T23:16:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:16:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 9.81 2021-02-12T23:16:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:16:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 19.34 2021-02-12T23:16:16Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:16:16Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:16:16Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:16:16Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:16:16Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:16:16Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:16:16Z- Filter 0 No beams used in subcloud update 2021-02-12T23:16:16Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:16:16Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:16:16Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 785.0 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:16:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171776.16 sec. 2021-02-12T23:16:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:16:16Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:16Z- variances([0], [2], and psi_berg) = 784.988314 833.075182 0.000000 2021-02-12T23:16:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:16:16Z- LcmTrn::publishEstimates() - MLE : 4056550.25 590639.87 14.45 2021-02-12T23:16:16Z- LcmTrn::publishEstimates() - MMSE : 4056575.75 590685.37 14.45 2021-02-12T23:16:16Z- LcmTrn::publishEstimates() - COVAR: 784.99 833.08 0.89 0.00 2021-02-12T23:16:16Z- AHRS_M2 msg: 1613171776.92 epoch sec; seqNo:2237 2021-02-12T23:16:16Z- AHRS_M2 msg: 0.70 phi; -0.34 theta; -0.21 psi 2021-02-12T23:16:16Z- handleDepth()-Depth_Keller msg: 1613171776920 epoch sec; seqNo:2260; depth 14.53 2021-02-12T23:16:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171776.93 epoch sec; seqNo:2260; 4056576.12 north; 590685.36 east 2021-02-12T23:16:17Z- AHRS_M2 msg: 1613171777.32 epoch sec; seqNo:2238 2021-02-12T23:16:17Z- AHRS_M2 msg: 0.70 phi; -0.34 theta; -0.20 psi 2021-02-12T23:16:17Z- handleDepth()-Depth_Keller msg: 1613171777326 epoch sec; seqNo:2261; depth 14.67 2021-02-12T23:16:17Z- handleDvl() - RDI_Pathfinder msg: 1613171777.34 epoch sec; seqNo:949 2021-02-12T23:16:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.63 , 96.49 , 71.91 , 111.08 2021-02-12T23:16:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.17 , 0.01 2021-02-12T23:16:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171777.35 epoch sec; seqNo:2261; 4056576.50 north; 590685.36 east 2021-02-12T23:16:17Z- AHRS_M2 msg: 1613171777.73 epoch sec; seqNo:2239 2021-02-12T23:16:17Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.19 psi 2021-02-12T23:16:17Z- handleDepth()-Depth_Keller msg: 1613171777731 epoch sec; seqNo:2262; depth 14.79 2021-02-12T23:16:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171777.75 epoch sec; seqNo:2262; 4056576.88 north; 590685.35 east 2021-02-12T23:16:18Z- AHRS_M2 msg: 1613171778.14 epoch sec; seqNo:2240 2021-02-12T23:16:18Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.18 psi 2021-02-12T23:16:18Z- handleDepth()-Depth_Keller msg: 1613171778145 epoch sec; seqNo:2263; depth 14.92 2021-02-12T23:16:18Z- handleDvl() - RDI_Pathfinder msg: 1613171778.16 epoch sec; seqNo:950 2021-02-12T23:16:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.63 , 95.40 , 71.09 , 110.00 2021-02-12T23:16:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.10 , 0.07 2021-02-12T23:16:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171778.18 epoch sec; seqNo:2263; 4056577.27 north; 590686.57 east 2021-02-12T23:16:18Z- AHRS_M2 msg: 1613171778.53 epoch sec; seqNo:2241 2021-02-12T23:16:18Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.18 psi 2021-02-12T23:16:18Z- handleDepth()-Depth_Keller msg: 1613171778538 epoch sec; seqNo:2264; depth 15.04 2021-02-12T23:16:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171778.55 epoch sec; seqNo:2264; 4056577.27 north; 590686.57 east 2021-02-12T23:16:18Z- AHRS_M2 msg: 1613171778.94 epoch sec; seqNo:2242 2021-02-12T23:16:18Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.18 psi 2021-02-12T23:16:18Z- handleDepth()-Depth_Keller msg: 1613171778940 epoch sec; seqNo:2265; depth 15.17 2021-02-12T23:16:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171778.96 epoch sec; seqNo:2265; 4056577.65 north; 590686.57 east 2021-02-12T23:16:19Z- handleDepth()-Depth_Keller msg: 1613171779361 epoch sec; seqNo:2266; depth 15.29 2021-02-12T23:16:19Z- handleDvl() - RDI_Pathfinder msg: 1613171779.38 epoch sec; seqNo:951 2021-02-12T23:16:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 94.26 , 68.64 , 108.85 2021-02-12T23:16:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.03 , -0.00 2021-02-12T23:16:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171779.39 epoch sec; seqNo:2266; 4056578.03 north; 590686.56 east 2021-02-12T23:16:19Z- AHRS_M2 msg: 1613171779.75 epoch sec; seqNo:2243 2021-02-12T23:16:19Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.19 psi 2021-02-12T23:16:19Z- handleDepth()-Depth_Keller msg: 1613171779756 epoch sec; seqNo:2267; depth 15.42 2021-02-12T23:16:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171779.77 epoch sec; seqNo:2267; 4056578.41 north; 590686.56 east 2021-02-12T23:16:20Z- AHRS_M2 msg: 1613171780.16 epoch sec; seqNo:2244 2021-02-12T23:16:20Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.20 psi 2021-02-12T23:16:20Z- handleDepth()-Depth_Keller msg: 1613171780189 epoch sec; seqNo:2268; depth 15.54 2021-02-12T23:16:20Z- handleDvl() - RDI_Pathfinder msg: 1613171780.21 epoch sec; seqNo:952 2021-02-12T23:16:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.22 , 96.76 , 78.51 , 108.92 2021-02-12T23:16:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , -0.12 2021-02-12T23:16:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171780.23 epoch sec; seqNo:2268; 4056578.41 north; 590686.56 east 2021-02-12T23:16:20Z- AHRS_M2 msg: 1613171780.55 epoch sec; seqNo:2245 2021-02-12T23:16:20Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.21 psi 2021-02-12T23:16:20Z- handleDepth()-Depth_Keller msg: 1613171780556 epoch sec; seqNo:2269; depth 15.68 2021-02-12T23:16:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171780.57 epoch sec; seqNo:2269; 4056578.80 north; 590687.78 east 2021-02-12T23:16:20Z- AHRS_M2 msg: 1613171780.96 epoch sec; seqNo:2246 2021-02-12T23:16:20Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.22 psi 2021-02-12T23:16:20Z- handleDepth()-Depth_Keller msg: 1613171780960 epoch sec; seqNo:2270; depth 15.79 2021-02-12T23:16:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171780.97 epoch sec; seqNo:2270; 4056579.18 north; 590687.77 east 2021-02-12T23:16:21Z- AHRS_M2 msg: 1613171781.36 epoch sec; seqNo:2247 2021-02-12T23:16:21Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.22 psi 2021-02-12T23:16:21Z- handleDepth()-Depth_Keller msg: 1613171781364 epoch sec; seqNo:2271; depth 15.91 2021-02-12T23:16:21Z- handleDvl() - RDI_Pathfinder msg: 1613171781.38 epoch sec; seqNo:953 2021-02-12T23:16:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.99 , 96.49 , 78.24 , 108.65 2021-02-12T23:16:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.09 , -0.15 2021-02-12T23:16:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171781.39 epoch sec; seqNo:2271; 4056579.56 north; 590687.77 east 2021-02-12T23:16:21Z- LcmTrn::handleMessages() - handling 40 messages took 0.64 ms and 128 calls... 2021-02-12T23:16:21Z- waiting for 1613171779.53, time is 1613171781.39 2021-02-12T23:16:21Z- LcmTrn::updateTrn() equating pose 1613171781.39 and meas times 1613171781.39 2021-02-12T23:16:21Z- LcmTrn::updateTrn() >>>> pose time 1613171781.39, meas time 1613171781.39 <<<< 2021-02-12T23:16:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171781.39 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:16:21Z- TerrainNav::motionUpdate() - currEstimate = (4056580, 590687.77, 15.91) 2021-02-12T23:16:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:16:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:16:21Z- TNavBF::Projecting Measurements 2021-02-12T23:16:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:16:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:16:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:16:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:16:21Z- TNavFilter::4 good beams. 2021-02-12T23:16:21Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:16:21Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:16:21Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:16:21Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:16:21Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:16:21Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:16:21Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:16:21Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:21Z- TerrainMapOctree: (vehN, vehE ) = (4056579.56, 590687.77). 2021-02-12T23:16:21Z- TerrainMapOctree: Min Distance = 5873.88. 2021-02-12T23:16:21Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:16:21Z- There is no specific good beam that all particles have in common. 2021-02-12T23:16:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:16:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 9.24 2021-02-12T23:16:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:16:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 14.90 2021-02-12T23:16:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:16:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 9.79 2021-02-12T23:16:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:16:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 18.89 2021-02-12T23:16:21Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:16:21Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:16:21Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:16:21Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:16:21Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:16:21Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:16:21Z- Filter 0 No beams used in subcloud update 2021-02-12T23:16:21Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:16:21Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:16:21Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 784.8 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:16:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171781.39 sec. 2021-02-12T23:16:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:16:21Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:21Z- variances([0], [2], and psi_berg) = 784.811393 833.074903 0.000000 2021-02-12T23:16:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:16:21Z- LcmTrn::publishEstimates() - MLE : 4056554.06 590642.27 15.91 2021-02-12T23:16:21Z- LcmTrn::publishEstimates() - MMSE : 4056579.56 590687.77 15.91 2021-02-12T23:16:21Z- LcmTrn::publishEstimates() - COVAR: 784.81 833.07 0.85 0.00 2021-02-12T23:16:21Z- AHRS_M2 msg: 1613171781.76 epoch sec; seqNo:2248 2021-02-12T23:16:21Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.23 psi 2021-02-12T23:16:21Z- handleDepth()-Depth_Keller msg: 1613171781768 epoch sec; seqNo:2272; depth 16.05 2021-02-12T23:16:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171781.78 epoch sec; seqNo:2272; 4056579.56 north; 590687.77 east 2021-02-12T23:16:22Z- AHRS_M2 msg: 1613171782.17 epoch sec; seqNo:2249 2021-02-12T23:16:22Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.23 psi 2021-02-12T23:16:22Z- handleDepth()-Depth_Keller msg: 1613171782172 epoch sec; seqNo:2273; depth 16.17 2021-02-12T23:16:22Z- handleDvl() - RDI_Pathfinder msg: 1613171782.18 epoch sec; seqNo:954 2021-02-12T23:16:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.17 , 96.56 , 69.04 , 108.72 2021-02-12T23:16:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.18 , -0.08 2021-02-12T23:16:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171782.19 epoch sec; seqNo:2273; 4056579.94 north; 590687.76 east 2021-02-12T23:16:22Z- AHRS_M2 msg: 1613171782.58 epoch sec; seqNo:2250 2021-02-12T23:16:22Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.23 psi 2021-02-12T23:16:22Z- handleDepth()-Depth_Keller msg: 1613171782585 epoch sec; seqNo:2274; depth 16.29 2021-02-12T23:16:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171782.60 epoch sec; seqNo:2274; 4056580.32 north; 590687.76 east 2021-02-12T23:16:22Z- AHRS_M2 msg: 1613171782.98 epoch sec; seqNo:2251 2021-02-12T23:16:22Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.22 psi 2021-02-12T23:16:22Z- handleDepth()-Depth_Keller msg: 1613171782980 epoch sec; seqNo:2275; depth 16.42 2021-02-12T23:16:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171782.99 epoch sec; seqNo:2275; 4056580.70 north; 590687.76 east 2021-02-12T23:16:23Z- AHRS_M2 msg: 1613171783.38 epoch sec; seqNo:2252 2021-02-12T23:16:23Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.22 psi 2021-02-12T23:16:23Z- handleDepth()-Depth_Keller msg: 1613171783386 epoch sec; seqNo:2276; depth 16.54 2021-02-12T23:16:23Z- handleDvl() - RDI_Pathfinder msg: 1613171783.40 epoch sec; seqNo:955 2021-02-12T23:16:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.17 , 96.76 , 69.86 , 108.92 2021-02-12T23:16:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.18 , 0.06 2021-02-12T23:16:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171783.42 epoch sec; seqNo:2276; 4056580.70 north; 590687.76 east 2021-02-12T23:16:23Z- AHRS_M2 msg: 1613171783.78 epoch sec; seqNo:2253 2021-02-12T23:16:23Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.22 psi 2021-02-12T23:16:23Z- handleDepth()-Depth_Keller msg: 1613171783788 epoch sec; seqNo:2277; depth 16.68 2021-02-12T23:16:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171783.80 epoch sec; seqNo:2277; 4056581.09 north; 590688.97 east 2021-02-12T23:16:24Z- AHRS_M2 msg: 1613171784.19 epoch sec; seqNo:2254 2021-02-12T23:16:24Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.22 psi 2021-02-12T23:16:24Z- handleDepth()-Depth_Keller msg: 1613171784197 epoch sec; seqNo:2278; depth 16.79 2021-02-12T23:16:24Z- handleDvl() - RDI_Pathfinder msg: 1613171784.21 epoch sec; seqNo:956 2021-02-12T23:16:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:16:24Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:16:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171784.22 epoch sec; seqNo:2278; 4056581.47 north; 590688.97 east 2021-02-12T23:16:24Z- AHRS_M2 msg: 1613171784.59 epoch sec; seqNo:2255 2021-02-12T23:16:24Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.22 psi 2021-02-12T23:16:24Z- handleDepth()-Depth_Keller msg: 1613171784596 epoch sec; seqNo:2279; depth 16.93 2021-02-12T23:16:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171784.61 epoch sec; seqNo:2279; 4056581.85 north; 590688.97 east 2021-02-12T23:16:25Z- AHRS_M2 msg: 1613171785.00 epoch sec; seqNo:2256 2021-02-12T23:16:25Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.22 psi 2021-02-12T23:16:25Z- handleDepth()-Depth_Keller msg: 1613171785005 epoch sec; seqNo:2280; depth 17.06 2021-02-12T23:16:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171785.01 epoch sec; seqNo:2280; 4056581.85 north; 590688.97 east 2021-02-12T23:16:25Z- AHRS_M2 msg: 1613171785.40 epoch sec; seqNo:2257 2021-02-12T23:16:25Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.22 psi 2021-02-12T23:16:25Z- handleDepth()-Depth_Keller msg: 1613171785404 epoch sec; seqNo:2281; depth 17.20 2021-02-12T23:16:25Z- handleDvl() - RDI_Pathfinder msg: 1613171785.42 epoch sec; seqNo:957 2021-02-12T23:16:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.25 , 95.74 , 68.16 , 110.61 2021-02-12T23:16:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.01 , 0.16 2021-02-12T23:16:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171785.43 epoch sec; seqNo:2281; 4056582.22 north; 590688.96 east 2021-02-12T23:16:25Z- AHRS_M2 msg: 1613171785.80 epoch sec; seqNo:2258 2021-02-12T23:16:25Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.22 psi 2021-02-12T23:16:25Z- handleDepth()-Depth_Keller msg: 1613171785811 epoch sec; seqNo:2282; depth 17.33 2021-02-12T23:16:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171785.82 epoch sec; seqNo:2282; 4056582.22 north; 590688.96 east 2021-02-12T23:16:26Z- AHRS_M2 msg: 1613171786.21 epoch sec; seqNo:2259 2021-02-12T23:16:26Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.22 psi 2021-02-12T23:16:26Z- handleDepth()-Depth_Keller msg: 1613171786212 epoch sec; seqNo:2283; depth 17.46 2021-02-12T23:16:26Z- handleDvl() - RDI_Pathfinder msg: 1613171786.22 epoch sec; seqNo:958 2021-02-12T23:16:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.99 , 95.54 , 68.64 , 108.92 2021-02-12T23:16:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.11 , 0.13 2021-02-12T23:16:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171786.24 epoch sec; seqNo:2283; 4056582.60 north; 590688.96 east 2021-02-12T23:16:26Z- AHRS_M2 msg: 1613171786.61 epoch sec; seqNo:2260 2021-02-12T23:16:26Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.22 psi 2021-02-12T23:16:26Z- handleDepth()-Depth_Keller msg: 1613171786616 epoch sec; seqNo:2284; depth 17.59 2021-02-12T23:16:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171786.63 epoch sec; seqNo:2284; 4056582.98 north; 590688.95 east 2021-02-12T23:16:26Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-12T23:16:26Z- waiting for 1613171784.39, time is 1613171786.63 2021-02-12T23:16:26Z- LcmTrn::updateTrn() >>>> pose time 1613171786.63, meas time 1613171786.22 <<<< 2021-02-12T23:16:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171786.22 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:16:26Z- TerrainNav::motionUpdate() - currEstimate = (4056583, 590688.95, 17.59) 2021-02-12T23:16:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:16:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:16:26Z- TNavBF::Projecting Measurements 2021-02-12T23:16:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:16:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:16:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:16:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:16:26Z- TNavFilter::4 good beams. 2021-02-12T23:16:26Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:16:26Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:16:26Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:16:26Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:16:26Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:16:26Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:16:26Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:16:26Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:26Z- TerrainMapOctree: (vehN, vehE ) = (4056582.72, 590688.86). 2021-02-12T23:16:26Z- TerrainMapOctree: Min Distance = 5870.71. 2021-02-12T23:16:26Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:16:26Z- There is no specific good beam that all particles have in common. 2021-02-12T23:16:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:16:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 9.24 2021-02-12T23:16:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:16:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 14.60 2021-02-12T23:16:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:16:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 7.54 2021-02-12T23:16:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:16:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 18.98 2021-02-12T23:16:26Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:16:26Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:16:26Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:16:26Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:16:26Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:16:26Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:16:26Z- Filter 0 No beams used in subcloud update 2021-02-12T23:16:26Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:16:26Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:16:26Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 784.7 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:16:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171786.22 sec. 2021-02-12T23:16:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:16:26Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:26Z- variances([0], [2], and psi_berg) = 784.615779 833.074596 0.000000 2021-02-12T23:16:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:16:26Z- LcmTrn::publishEstimates() - MLE : 4056557.48 590646.45 17.59 2021-02-12T23:16:26Z- LcmTrn::publishEstimates() - MMSE : 4056582.98 590688.95 17.59 2021-02-12T23:16:26Z- LcmTrn::publishEstimates() - COVAR: 784.62 833.07 0.79 0.00 2021-02-12T23:16:27Z- AHRS_M2 msg: 1613171787.02 epoch sec; seqNo:2261 2021-02-12T23:16:27Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.21 psi 2021-02-12T23:16:27Z- handleDepth()-Depth_Keller msg: 1613171787024 epoch sec; seqNo:2285; depth 17.73 2021-02-12T23:16:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171787.03 epoch sec; seqNo:2285; 4056583.00 north; 590690.17 east 2021-02-12T23:16:27Z- AHRS_M2 msg: 1613171787.42 epoch sec; seqNo:2262 2021-02-12T23:16:27Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.21 psi 2021-02-12T23:16:27Z- handleDepth()-Depth_Keller msg: 1613171787426 epoch sec; seqNo:2286; depth 17.86 2021-02-12T23:16:27Z- handleDvl() - RDI_Pathfinder msg: 1613171787.44 epoch sec; seqNo:959 2021-02-12T23:16:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.58 , 96.76 , 68.64 , 108.92 2021-02-12T23:16:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.13 , 0.05 2021-02-12T23:16:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171787.45 epoch sec; seqNo:2286; 4056583.37 north; 590690.17 east 2021-02-12T23:16:27Z- AHRS_M2 msg: 1613171787.82 epoch sec; seqNo:2263 2021-02-12T23:16:27Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.21 psi 2021-02-12T23:16:27Z- handleDepth()-Depth_Keller msg: 1613171787833 epoch sec; seqNo:2287; depth 18.00 2021-02-12T23:16:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171787.84 epoch sec; seqNo:2287; 4056583.37 north; 590690.17 east 2021-02-12T23:16:28Z- AHRS_M2 msg: 1613171788.23 epoch sec; seqNo:2264 2021-02-12T23:16:28Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.21 psi 2021-02-12T23:16:28Z- handleDepth()-Depth_Keller msg: 1613171788233 epoch sec; seqNo:2288; depth 18.13 2021-02-12T23:16:28Z- handleDvl() - RDI_Pathfinder msg: 1613171788.24 epoch sec; seqNo:960 2021-02-12T23:16:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.18 , 94.32 , 68.64 , 108.92 2021-02-12T23:16:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.11 , -0.04 2021-02-12T23:16:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171788.26 epoch sec; seqNo:2288; 4056583.75 north; 590690.17 east 2021-02-12T23:16:28Z- AHRS_M2 msg: 1613171788.63 epoch sec; seqNo:2265 2021-02-12T23:16:28Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.21 psi 2021-02-12T23:16:28Z- handleDepth()-Depth_Keller msg: 1613171788638 epoch sec; seqNo:2289; depth 18.26 2021-02-12T23:16:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171788.65 epoch sec; seqNo:2289; 4056584.13 north; 590690.16 east 2021-02-12T23:16:29Z- AHRS_M2 msg: 1613171789.04 epoch sec; seqNo:2266 2021-02-12T23:16:29Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.21 psi 2021-02-12T23:16:29Z- handleDepth()-Depth_Keller msg: 1613171789045 epoch sec; seqNo:2290; depth 18.41 2021-02-12T23:16:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171789.07 epoch sec; seqNo:2290; 4056584.52 north; 590691.38 east 2021-02-12T23:16:29Z- AHRS_M2 msg: 1613171789.44 epoch sec; seqNo:2267 2021-02-12T23:16:29Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.21 psi 2021-02-12T23:16:29Z- handleDepth()-Depth_Keller msg: 1613171789444 epoch sec; seqNo:2291; depth 18.54 2021-02-12T23:16:29Z- handleDvl() - RDI_Pathfinder msg: 1613171789.46 epoch sec; seqNo:961 2021-02-12T23:16:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.36 , 93.18 , 78.58 , 108.99 2021-02-12T23:16:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , -0.08 2021-02-12T23:16:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171789.47 epoch sec; seqNo:2291; 4056584.90 north; 590691.38 east 2021-02-12T23:16:29Z- AHRS_M2 msg: 1613171789.85 epoch sec; seqNo:2268 2021-02-12T23:16:29Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.21 psi 2021-02-12T23:16:29Z- handleDepth()-Depth_Keller msg: 1613171789850 epoch sec; seqNo:2292; depth 18.69 2021-02-12T23:16:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171789.86 epoch sec; seqNo:2292; 4056584.90 north; 590691.38 east 2021-02-12T23:16:30Z- AHRS_M2 msg: 1613171790.25 epoch sec; seqNo:2269 2021-02-12T23:16:30Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.21 psi 2021-02-12T23:16:30Z- handleDepth()-Depth_Keller msg: 1613171790262 epoch sec; seqNo:2293; depth 18.83 2021-02-12T23:16:30Z- handleDvl() - RDI_Pathfinder msg: 1613171790.27 epoch sec; seqNo:962 2021-02-12T23:16:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.13 , 94.19 , 65.78 , 107.57 2021-02-12T23:16:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.10 , -0.11 2021-02-12T23:16:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171790.28 epoch sec; seqNo:2293; 4056585.28 north; 590691.37 east 2021-02-12T23:16:30Z- AHRS_M2 msg: 1613171790.65 epoch sec; seqNo:2270 2021-02-12T23:16:30Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.21 psi 2021-02-12T23:16:30Z- handleDepth()-Depth_Keller msg: 1613171790656 epoch sec; seqNo:2294; depth 18.97 2021-02-12T23:16:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171790.67 epoch sec; seqNo:2294; 4056585.66 north; 590691.37 east 2021-02-12T23:16:31Z- AHRS_M2 msg: 1613171791.06 epoch sec; seqNo:2271 2021-02-12T23:16:31Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.21 psi 2021-02-12T23:16:31Z- handleDepth()-Depth_Keller msg: 1613171791060 epoch sec; seqNo:2295; depth 19.09 2021-02-12T23:16:31Z- handleDvl() - RDI_Pathfinder msg: 1613171791.07 epoch sec; seqNo:963 2021-02-12T23:16:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.54 , 93.31 , 66.19 , 106.69 2021-02-12T23:16:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.15 , -0.07 2021-02-12T23:16:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171791.08 epoch sec; seqNo:2295; 4056586.04 north; 590691.36 east 2021-02-12T23:16:31Z- AHRS_M2 msg: 1613171791.46 epoch sec; seqNo:2272 2021-02-12T23:16:31Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.20 psi 2021-02-12T23:16:31Z- handleDepth()-Depth_Keller msg: 1613171791468 epoch sec; seqNo:2296; depth 19.24 2021-02-12T23:16:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171791.48 epoch sec; seqNo:2296; 4056586.43 north; 590692.58 east 2021-02-12T23:16:31Z- AHRS_M2 msg: 1613171791.86 epoch sec; seqNo:2273 2021-02-12T23:16:31Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-12T23:16:31Z- LcmTrn::handleMessages() - handling 42 messages took 0.64 ms and 129 calls... 2021-02-12T23:16:31Z- waiting for 1613171789.63, time is 1613171791.86 2021-02-12T23:16:31Z- LcmTrn::updateTrn() >>>> pose time 1613171791.86, meas time 1613171791.07 <<<< 2021-02-12T23:16:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171791.07 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:16:31Z- TerrainNav::motionUpdate() - currEstimate = (4056586, 590692.58, 19.24) 2021-02-12T23:16:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:16:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:16:31Z- TNavBF::Projecting Measurements 2021-02-12T23:16:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:16:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:16:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:16:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:16:31Z- TNavFilter::4 good beams. 2021-02-12T23:16:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:16:31Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:16:31Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:16:31Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:16:31Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:16:31Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:16:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:16:31Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:31Z- TerrainMapOctree: (vehN, vehE ) = (4056585.91, 590692.03). 2021-02-12T23:16:31Z- TerrainMapOctree: Min Distance = 5866.25. 2021-02-12T23:16:31Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:16:31Z- There is no specific good beam that all particles have in common. 2021-02-12T23:16:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:16:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 8.65 2021-02-12T23:16:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:16:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 13.93 2021-02-12T23:16:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:16:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 7.01 2021-02-12T23:16:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:16:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.79 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 18.21 2021-02-12T23:16:31Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:16:31Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:16:31Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:16:31Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:16:31Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:16:31Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:16:31Z- Filter 0 No beams used in subcloud update 2021-02-12T23:16:31Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:16:31Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:16:31Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 784.4 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:16:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171791.07 sec. 2021-02-12T23:16:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:16:31Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:31Z- variances([0], [2], and psi_berg) = 784.373434 833.074218 0.000000 2021-02-12T23:16:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:16:31Z- LcmTrn::publishEstimates() - MLE : 4056560.93 590650.08 19.24 2021-02-12T23:16:31Z- LcmTrn::publishEstimates() - MMSE : 4056586.43 590692.58 19.24 2021-02-12T23:16:31Z- LcmTrn::publishEstimates() - COVAR: 784.37 833.07 0.80 0.00 2021-02-12T23:16:31Z- handleDepth()-Depth_Keller msg: 1613171791868 epoch sec; seqNo:2297; depth 19.36 2021-02-12T23:16:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171791.88 epoch sec; seqNo:2297; 4056586.81 north; 590692.58 east 2021-02-12T23:16:32Z- AHRS_M2 msg: 1613171792.27 epoch sec; seqNo:2274 2021-02-12T23:16:32Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.19 psi 2021-02-12T23:16:32Z- handleDepth()-Depth_Keller msg: 1613171792273 epoch sec; seqNo:2298; depth 19.51 2021-02-12T23:16:32Z- handleDvl() - RDI_Pathfinder msg: 1613171792.29 epoch sec; seqNo:964 2021-02-12T23:16:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.54 , 92.09 , 65.37 , 105.47 2021-02-12T23:16:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.10 , -0.04 2021-02-12T23:16:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171792.30 epoch sec; seqNo:2298; 4056586.81 north; 590692.58 east 2021-02-12T23:16:32Z- AHRS_M2 msg: 1613171792.67 epoch sec; seqNo:2275 2021-02-12T23:16:32Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.19 psi 2021-02-12T23:16:32Z- handleDepth()-Depth_Keller msg: 1613171792685 epoch sec; seqNo:2299; depth 19.66 2021-02-12T23:16:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171792.69 epoch sec; seqNo:2299; 4056587.19 north; 590692.57 east 2021-02-12T23:16:33Z- AHRS_M2 msg: 1613171793.08 epoch sec; seqNo:2276 2021-02-12T23:16:33Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.19 psi 2021-02-12T23:16:33Z- handleDepth()-Depth_Keller msg: 1613171793089 epoch sec; seqNo:2300; depth 19.79 2021-02-12T23:16:33Z- handleDvl() - RDI_Pathfinder msg: 1613171793.10 epoch sec; seqNo:965 2021-02-12T23:16:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.54 , 89.73 , 65.37 , 104.33 2021-02-12T23:16:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , -0.06 2021-02-12T23:16:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171793.15 epoch sec; seqNo:2300; 4056587.57 north; 590692.57 east 2021-02-12T23:16:33Z- AHRS_M2 msg: 1613171793.48 epoch sec; seqNo:2277 2021-02-12T23:16:33Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.19 psi 2021-02-12T23:16:33Z- handleDepth()-Depth_Keller msg: 1613171793484 epoch sec; seqNo:2301; depth 19.94 2021-02-12T23:16:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171793.51 epoch sec; seqNo:2301; 4056587.95 north; 590692.56 east 2021-02-12T23:16:33Z- AHRS_M2 msg: 1613171793.88 epoch sec; seqNo:2278 2021-02-12T23:16:33Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.18 psi 2021-02-12T23:16:33Z- handleDepth()-Depth_Keller msg: 1613171793888 epoch sec; seqNo:2302; depth 20.06 2021-02-12T23:16:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171793.90 epoch sec; seqNo:2302; 4056587.95 north; 590692.56 east 2021-02-12T23:16:34Z- AHRS_M2 msg: 1613171794.29 epoch sec; seqNo:2279 2021-02-12T23:16:34Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.18 psi 2021-02-12T23:16:34Z- handleDepth()-Depth_Keller msg: 1613171794293 epoch sec; seqNo:2303; depth 20.20 2021-02-12T23:16:34Z- handleDvl() - RDI_Pathfinder msg: 1613171794.30 epoch sec; seqNo:966 2021-02-12T23:16:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.13 , 89.80 , 65.78 , 105.61 2021-02-12T23:16:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , -0.08 2021-02-12T23:16:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171794.31 epoch sec; seqNo:2303; 4056588.32 north; 590692.56 east 2021-02-12T23:16:34Z- AHRS_M2 msg: 1613171794.69 epoch sec; seqNo:2280 2021-02-12T23:16:34Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-12T23:16:34Z- handleDepth()-Depth_Keller msg: 1613171794698 epoch sec; seqNo:2304; depth 20.34 2021-02-12T23:16:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171794.71 epoch sec; seqNo:2304; 4056588.72 north; 590693.78 east 2021-02-12T23:16:35Z- AHRS_M2 msg: 1613171795.10 epoch sec; seqNo:2281 2021-02-12T23:16:35Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-12T23:16:35Z- handleDepth()-Depth_Keller msg: 1613171795101 epoch sec; seqNo:2305; depth 20.47 2021-02-12T23:16:35Z- handleDvl() - RDI_Pathfinder msg: 1613171795.12 epoch sec; seqNo:967 2021-02-12T23:16:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.32 , 91.01 , 64.55 , 104.39 2021-02-12T23:16:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.13 , -0.07 2021-02-12T23:16:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171795.14 epoch sec; seqNo:2305; 4056589.10 north; 590693.77 east 2021-02-12T23:16:35Z- AHRS_M2 msg: 1613171795.50 epoch sec; seqNo:2282 2021-02-12T23:16:35Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-12T23:16:35Z- handleDepth()-Depth_Keller msg: 1613171795504 epoch sec; seqNo:2306; depth 20.62 2021-02-12T23:16:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171795.52 epoch sec; seqNo:2306; 4056589.10 north; 590693.77 east 2021-02-12T23:16:35Z- AHRS_M2 msg: 1613171795.90 epoch sec; seqNo:2283 2021-02-12T23:16:35Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-12T23:16:35Z- handleDepth()-Depth_Keller msg: 1613171795908 epoch sec; seqNo:2307; depth 20.76 2021-02-12T23:16:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171795.93 epoch sec; seqNo:2307; 4056589.47 north; 590693.77 east 2021-02-12T23:16:36Z- AHRS_M2 msg: 1613171796.31 epoch sec; seqNo:2284 2021-02-12T23:16:36Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-12T23:16:36Z- handleDepth()-Depth_Keller msg: 1613171796325 epoch sec; seqNo:2308; depth 20.88 2021-02-12T23:16:36Z- handleDvl() - RDI_Pathfinder msg: 1613171796.34 epoch sec; seqNo:968 2021-02-12T23:16:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.32 , 88.65 , 65.37 , 103.24 2021-02-12T23:16:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.15 , 0.00 2021-02-12T23:16:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171796.37 epoch sec; seqNo:2308; 4056589.85 north; 590693.77 east 2021-02-12T23:16:36Z- AHRS_M2 msg: 1613171796.72 epoch sec; seqNo:2285 2021-02-12T23:16:36Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.18 psi 2021-02-12T23:16:36Z- handleDepth()-Depth_Keller msg: 1613171796723 epoch sec; seqNo:2309; depth 21.04 2021-02-12T23:16:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171796.73 epoch sec; seqNo:2309; 4056590.23 north; 590693.76 east 2021-02-12T23:16:36Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 131 calls... 2021-02-12T23:16:36Z- waiting for 1613171794.86, time is 1613171796.73 2021-02-12T23:16:36Z- LcmTrn::updateTrn() >>>> pose time 1613171796.73, meas time 1613171796.34 <<<< 2021-02-12T23:16:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171796.34 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:16:36Z- TerrainNav::motionUpdate() - currEstimate = (4056590, 590693.76, 21.04) 2021-02-12T23:16:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:16:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:16:36Z- TNavBF::Projecting Measurements 2021-02-12T23:16:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:16:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:16:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:16:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:16:36Z- TNavFilter::4 good beams. 2021-02-12T23:16:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:16:36Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:16:36Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:16:36Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:16:36Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:16:36Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:16:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:16:36Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:36Z- TerrainMapOctree: (vehN, vehE ) = (4056589.92, 590693.67). 2021-02-12T23:16:36Z- TerrainMapOctree: Min Distance = 5862.07. 2021-02-12T23:16:36Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:16:37Z- There is no specific good beam that all particles have in common. 2021-02-12T23:16:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:16:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-12T23:16:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:16:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 12.57 2021-02-12T23:16:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:16:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-12T23:16:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:16:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.77 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 17.05 2021-02-12T23:16:37Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:16:37Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:16:37Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:16:37Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:16:37Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:16:37Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:16:37Z- Filter 0 No beams used in subcloud update 2021-02-12T23:16:37Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:16:37Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:16:37Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 784.2 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:16:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171796.34 sec. 2021-02-12T23:16:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:16:37Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:37Z- variances([0], [2], and psi_berg) = 784.169025 833.073900 0.000000 2021-02-12T23:16:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:16:37Z- LcmTrn::publishEstimates() - MLE : 4056564.73 590651.26 21.04 2021-02-12T23:16:37Z- LcmTrn::publishEstimates() - MMSE : 4056590.23 590693.76 21.04 2021-02-12T23:16:37Z- LcmTrn::publishEstimates() - COVAR: 784.17 833.07 0.75 0.00 2021-02-12T23:16:37Z- AHRS_M2 msg: 1613171797.12 epoch sec; seqNo:2286 2021-02-12T23:16:37Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.18 psi 2021-02-12T23:16:37Z- handleDepth()-Depth_Keller msg: 1613171797128 epoch sec; seqNo:2310; depth 21.17 2021-02-12T23:16:37Z- handleDvl() - RDI_Pathfinder msg: 1613171797.15 epoch sec; seqNo:969 2021-02-12T23:16:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:16:37Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:16:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171797.16 epoch sec; seqNo:2310; 4056590.61 north; 590693.76 east 2021-02-12T23:16:37Z- AHRS_M2 msg: 1613171797.52 epoch sec; seqNo:2287 2021-02-12T23:16:37Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.18 psi 2021-02-12T23:16:37Z- handleDepth()-Depth_Keller msg: 1613171797524 epoch sec; seqNo:2311; depth 21.31 2021-02-12T23:16:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171797.54 epoch sec; seqNo:2311; 4056590.61 north; 590693.76 east 2021-02-12T23:16:37Z- AHRS_M2 msg: 1613171797.92 epoch sec; seqNo:2288 2021-02-12T23:16:37Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.17 psi 2021-02-12T23:16:37Z- handleDepth()-Depth_Keller msg: 1613171797934 epoch sec; seqNo:2312; depth 21.45 2021-02-12T23:16:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171797.94 epoch sec; seqNo:2312; 4056590.99 north; 590693.75 east 2021-02-12T23:16:38Z- AHRS_M2 msg: 1613171798.33 epoch sec; seqNo:2289 2021-02-12T23:16:38Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.17 psi 2021-02-12T23:16:38Z- handleDepth()-Depth_Keller msg: 1613171798333 epoch sec; seqNo:2313; depth 21.58 2021-02-12T23:16:38Z- handleDvl() - RDI_Pathfinder msg: 1613171798.35 epoch sec; seqNo:970 2021-02-12T23:16:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.72 , 87.30 , 64.96 , 103.11 2021-02-12T23:16:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , 0.14 2021-02-12T23:16:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171798.36 epoch sec; seqNo:2313; 4056591.38 north; 590694.97 east 2021-02-12T23:16:38Z- AHRS_M2 msg: 1613171798.73 epoch sec; seqNo:2290 2021-02-12T23:16:38Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.17 psi 2021-02-12T23:16:38Z- handleDepth()-Depth_Keller msg: 1613171798749 epoch sec; seqNo:2314; depth 21.74 2021-02-12T23:16:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171798.78 epoch sec; seqNo:2314; 4056591.38 north; 590694.97 east 2021-02-12T23:16:39Z- AHRS_M2 msg: 1613171799.14 epoch sec; seqNo:2291 2021-02-12T23:16:39Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.17 psi 2021-02-12T23:16:39Z- handleDepth()-Depth_Keller msg: 1613171799149 epoch sec; seqNo:2315; depth 21.86 2021-02-12T23:16:39Z- handleDvl() - RDI_Pathfinder msg: 1613171799.16 epoch sec; seqNo:971 2021-02-12T23:16:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.72 , 86.96 , 64.96 , 104.26 2021-02-12T23:16:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.06 , 0.11 2021-02-12T23:16:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171799.17 epoch sec; seqNo:2315; 4056591.76 north; 590694.97 east 2021-02-12T23:16:39Z- AHRS_M2 msg: 1613171799.54 epoch sec; seqNo:2292 2021-02-12T23:16:39Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.17 psi 2021-02-12T23:16:39Z- handleDepth()-Depth_Keller msg: 1613171799544 epoch sec; seqNo:2316; depth 22.00 2021-02-12T23:16:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171799.56 epoch sec; seqNo:2316; 4056592.14 north; 590694.96 east 2021-02-12T23:16:39Z- AHRS_M2 msg: 1613171799.94 epoch sec; seqNo:2293 2021-02-12T23:16:39Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.17 psi 2021-02-12T23:16:39Z- handleDepth()-Depth_Keller msg: 1613171799956 epoch sec; seqNo:2317; depth 22.13 2021-02-12T23:16:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171799.96 epoch sec; seqNo:2317; 4056592.14 north; 590694.96 east 2021-02-12T23:16:40Z- AHRS_M2 msg: 1613171800.35 epoch sec; seqNo:2294 2021-02-12T23:16:40Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.17 psi 2021-02-12T23:16:40Z- handleDepth()-Depth_Keller msg: 1613171800352 epoch sec; seqNo:2318; depth 22.28 2021-02-12T23:16:40Z- handleDvl() - RDI_Pathfinder msg: 1613171800.36 epoch sec; seqNo:972 2021-02-12T23:16:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.32 , 86.96 , 64.96 , 99.93 2021-02-12T23:16:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.14 , 0.03 2021-02-12T23:16:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171800.37 epoch sec; seqNo:2318; 4056592.53 north; 590696.18 east 2021-02-12T23:16:40Z- AHRS_M2 msg: 1613171800.75 epoch sec; seqNo:2295 2021-02-12T23:16:40Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.18 psi 2021-02-12T23:16:40Z- handleDepth()-Depth_Keller msg: 1613171800757 epoch sec; seqNo:2319; depth 22.42 2021-02-12T23:16:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171800.77 epoch sec; seqNo:2319; 4056592.91 north; 590696.18 east 2021-02-12T23:16:41Z- AHRS_M2 msg: 1613171801.16 epoch sec; seqNo:2296 2021-02-12T23:16:41Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.18 psi 2021-02-12T23:16:41Z- handleDepth()-Depth_Keller msg: 1613171801160 epoch sec; seqNo:2320; depth 22.56 2021-02-12T23:16:41Z- handleDvl() - RDI_Pathfinder msg: 1613171801.18 epoch sec; seqNo:973 2021-02-12T23:16:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.91 , 88.11 , 64.55 , 104.33 2021-02-12T23:16:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.11 , -0.06 2021-02-12T23:16:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171801.19 epoch sec; seqNo:2320; 4056592.91 north; 590696.18 east 2021-02-12T23:16:41Z- AHRS_M2 msg: 1613171801.56 epoch sec; seqNo:2297 2021-02-12T23:16:41Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.19 psi 2021-02-12T23:16:41Z- handleDepth()-Depth_Keller msg: 1613171801564 epoch sec; seqNo:2321; depth 22.70 2021-02-12T23:16:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171801.58 epoch sec; seqNo:2321; 4056593.29 north; 590696.17 east 2021-02-12T23:16:41Z- AHRS_M2 msg: 1613171801.97 epoch sec; seqNo:2298 2021-02-12T23:16:41Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.19 psi 2021-02-12T23:16:41Z- handleDepth()-Depth_Keller msg: 1613171801972 epoch sec; seqNo:2322; depth 22.84 2021-02-12T23:16:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171801.98 epoch sec; seqNo:2322; 4056593.67 north; 590696.17 east 2021-02-12T23:16:42Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-12T23:16:42Z- waiting for 1613171799.73, time is 1613171801.98 2021-02-12T23:16:42Z- LcmTrn::updateTrn() >>>> pose time 1613171801.98, meas time 1613171801.18 <<<< 2021-02-12T23:16:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171801.18 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:16:42Z- TerrainNav::motionUpdate() - currEstimate = (4056594, 590696.17, 22.84) 2021-02-12T23:16:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:16:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:16:42Z- TNavBF::Projecting Measurements 2021-02-12T23:16:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:16:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:16:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:16:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:16:42Z- TNavFilter::4 good beams. 2021-02-12T23:16:42Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:16:42Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:16:42Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:16:42Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:16:42Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:16:42Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:16:42Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:16:42Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:42Z- TerrainMapOctree: (vehN, vehE ) = (4056593.14, 590695.80). 2021-02-12T23:16:42Z- TerrainMapOctree: Min Distance = 5858.21. 2021-02-12T23:16:42Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:16:42Z- There is no specific good beam that all particles have in common. 2021-02-12T23:16:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:16:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 8.27 2021-02-12T23:16:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:16:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 12.42 2021-02-12T23:16:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:16:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 6.67 2021-02-12T23:16:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:16:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.74 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 17.41 2021-02-12T23:16:42Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:16:42Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:16:42Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:16:42Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:16:42Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:16:42Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:16:42Z- Filter 0 No beams used in subcloud update 2021-02-12T23:16:42Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:16:42Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:16:42Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 784.0 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:16:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171801.18 sec. 2021-02-12T23:16:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:16:42Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:42Z- variances([0], [2], and psi_berg) = 783.948694 833.073558 0.000000 2021-02-12T23:16:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:16:42Z- LcmTrn::publishEstimates() - MLE : 4056568.17 590654.67 22.84 2021-02-12T23:16:42Z- LcmTrn::publishEstimates() - MMSE : 4056593.67 590696.17 22.84 2021-02-12T23:16:42Z- LcmTrn::publishEstimates() - COVAR: 783.95 833.07 0.74 0.00 2021-02-12T23:16:42Z- AHRS_M2 msg: 1613171802.37 epoch sec; seqNo:2299 2021-02-12T23:16:42Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.19 psi 2021-02-12T23:16:42Z- handleDepth()-Depth_Keller msg: 1613171802381 epoch sec; seqNo:2323; depth 22.98 2021-02-12T23:16:42Z- handleDvl() - RDI_Pathfinder msg: 1613171802.39 epoch sec; seqNo:974 2021-02-12T23:16:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.68 , 88.04 , 62.10 , 96.69 2021-02-12T23:16:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.05 , -0.10 2021-02-12T23:16:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171802.40 epoch sec; seqNo:2323; 4056594.06 north; 590697.39 east 2021-02-12T23:16:42Z- AHRS_M2 msg: 1613171802.77 epoch sec; seqNo:2300 2021-02-12T23:16:42Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.20 psi 2021-02-12T23:16:42Z- handleDepth()-Depth_Keller msg: 1613171802776 epoch sec; seqNo:2324; depth 23.12 2021-02-12T23:16:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171802.80 epoch sec; seqNo:2324; 4056594.06 north; 590697.39 east 2021-02-12T23:16:43Z- AHRS_M2 msg: 1613171803.18 epoch sec; seqNo:2301 2021-02-12T23:16:43Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.20 psi 2021-02-12T23:16:43Z- handleDepth()-Depth_Keller msg: 1613171803180 epoch sec; seqNo:2325; depth 23.23 2021-02-12T23:16:43Z- handleDvl() - RDI_Pathfinder msg: 1613171803.19 epoch sec; seqNo:975 2021-02-12T23:16:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.27 , 87.09 , 63.33 , 96.82 2021-02-12T23:16:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.09 2021-02-12T23:16:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171803.22 epoch sec; seqNo:2325; 4056594.44 north; 590697.38 east 2021-02-12T23:16:43Z- AHRS_M2 msg: 1613171803.58 epoch sec; seqNo:2302 2021-02-12T23:16:43Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.20 psi 2021-02-12T23:16:43Z- handleDepth()-Depth_Keller msg: 1613171803584 epoch sec; seqNo:2326; depth 23.37 2021-02-12T23:16:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171803.60 epoch sec; seqNo:2326; 4056594.82 north; 590697.38 east 2021-02-12T23:16:44Z- AHRS_M2 msg: 1613171804.00 epoch sec; seqNo:2303 2021-02-12T23:16:44Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.20 psi 2021-02-12T23:16:44Z- handleDepth()-Depth_Keller msg: 1613171804007 epoch sec; seqNo:2327; depth 23.51 2021-02-12T23:16:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171804.03 epoch sec; seqNo:2327; 4056595.20 north; 590697.37 east 2021-02-12T23:16:44Z- AHRS_M2 msg: 1613171804.39 epoch sec; seqNo:2304 2021-02-12T23:16:44Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.20 psi 2021-02-12T23:16:44Z- handleDepth()-Depth_Keller msg: 1613171804392 epoch sec; seqNo:2328; depth 23.65 2021-02-12T23:16:44Z- handleDvl() - RDI_Pathfinder msg: 1613171804.40 epoch sec; seqNo:976 2021-02-12T23:16:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.46 , 88.17 , 62.51 , 96.82 2021-02-12T23:16:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.06 2021-02-12T23:16:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171804.43 epoch sec; seqNo:2328; 4056595.20 north; 590697.37 east 2021-02-12T23:16:44Z- AHRS_M2 msg: 1613171804.80 epoch sec; seqNo:2305 2021-02-12T23:16:44Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.20 psi 2021-02-12T23:16:44Z- handleDepth()-Depth_Keller msg: 1613171804805 epoch sec; seqNo:2329; depth 23.77 2021-02-12T23:16:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171804.82 epoch sec; seqNo:2329; 4056595.59 north; 590698.59 east 2021-02-12T23:16:45Z- AHRS_M2 msg: 1613171805.20 epoch sec; seqNo:2306 2021-02-12T23:16:45Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.20 psi 2021-02-12T23:16:45Z- handleDepth()-Depth_Keller msg: 1613171805202 epoch sec; seqNo:2330; depth 23.90 2021-02-12T23:16:45Z- handleDvl() - RDI_Pathfinder msg: 1613171805.21 epoch sec; seqNo:977 2021-02-12T23:16:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.46 , 86.01 , 63.33 , 96.82 2021-02-12T23:16:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , -0.06 2021-02-12T23:16:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171805.22 epoch sec; seqNo:2330; 4056595.97 north; 590698.59 east 2021-02-12T23:16:45Z- AHRS_M2 msg: 1613171805.60 epoch sec; seqNo:2307 2021-02-12T23:16:45Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.20 psi 2021-02-12T23:16:45Z- handleDepth()-Depth_Keller msg: 1613171805609 epoch sec; seqNo:2331; depth 24.03 2021-02-12T23:16:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171805.63 epoch sec; seqNo:2331; 4056595.97 north; 590698.59 east 2021-02-12T23:16:46Z- AHRS_M2 msg: 1613171806.01 epoch sec; seqNo:2308 2021-02-12T23:16:46Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.20 psi 2021-02-12T23:16:46Z- handleDepth()-Depth_Keller msg: 1613171806024 epoch sec; seqNo:2332; depth 24.17 2021-02-12T23:16:46Z- handleDvl() - RDI_Pathfinder msg: 1613171806.06 epoch sec; seqNo:978 2021-02-12T23:16:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.64 , 87.09 , 63.33 , 100.07 2021-02-12T23:16:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , -0.09 2021-02-12T23:16:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171806.07 epoch sec; seqNo:2332; 4056596.35 north; 590698.58 east 2021-02-12T23:16:46Z- AHRS_M2 msg: 1613171806.41 epoch sec; seqNo:2309 2021-02-12T23:16:46Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.20 psi 2021-02-12T23:16:46Z- handleDepth()-Depth_Keller msg: 1613171806417 epoch sec; seqNo:2333; depth 24.32 2021-02-12T23:16:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171806.43 epoch sec; seqNo:2333; 4056596.72 north; 590698.58 east 2021-02-12T23:16:46Z- AHRS_M2 msg: 1613171806.81 epoch sec; seqNo:2310 2021-02-12T23:16:46Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.21 psi 2021-02-12T23:16:46Z- handleDepth()-Depth_Keller msg: 1613171806822 epoch sec; seqNo:2334; depth 24.43 2021-02-12T23:16:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171806.83 epoch sec; seqNo:2334; 4056597.10 north; 590698.57 east 2021-02-12T23:16:47Z- LcmTrn::handleMessages() - handling 41 messages took 0.66 ms and 128 calls... 2021-02-12T23:16:47Z- waiting for 1613171804.98, time is 1613171806.83 2021-02-12T23:16:47Z- LcmTrn::updateTrn() >>>> pose time 1613171806.83, meas time 1613171806.06 <<<< 2021-02-12T23:16:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171806.06 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:16:47Z- TerrainNav::motionUpdate() - currEstimate = (4056597, 590698.57, 24.43) 2021-02-12T23:16:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:16:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:16:47Z- TNavBF::Projecting Measurements 2021-02-12T23:16:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:16:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:16:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:16:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:16:47Z- TNavFilter::4 good beams. 2021-02-12T23:16:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:16:47Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:16:47Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:16:47Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:16:47Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:16:47Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:16:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:16:47Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:47Z- TerrainMapOctree: (vehN, vehE ) = (4056596.56, 590698.19). 2021-02-12T23:16:47Z- TerrainMapOctree: Min Distance = 5854.05. 2021-02-12T23:16:47Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:16:47Z- There is no specific good beam that all particles have in common. 2021-02-12T23:16:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:16:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 7.54 2021-02-12T23:16:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:16:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 12.14 2021-02-12T23:16:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:16:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.79 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 6.42 2021-02-12T23:16:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:16:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.72 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 16.02 2021-02-12T23:16:47Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:16:47Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:16:47Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:16:47Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:16:47Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:16:47Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:16:47Z- Filter 0 No beams used in subcloud update 2021-02-12T23:16:47Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:16:47Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:16:47Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 783.8 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:16:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171806.06 sec. 2021-02-12T23:16:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:16:47Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:47Z- variances([0], [2], and psi_berg) = 783.728541 833.073219 0.000000 2021-02-12T23:16:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:16:47Z- LcmTrn::publishEstimates() - MLE : 4056571.60 590657.07 24.43 2021-02-12T23:16:47Z- LcmTrn::publishEstimates() - MMSE : 4056597.10 590698.57 24.43 2021-02-12T23:16:47Z- LcmTrn::publishEstimates() - COVAR: 783.73 833.07 0.73 0.00 2021-02-12T23:16:47Z- AHRS_M2 msg: 1613171807.21 epoch sec; seqNo:2311 2021-02-12T23:16:47Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.21 psi 2021-02-12T23:16:47Z- handleDepth()-Depth_Keller msg: 1613171807221 epoch sec; seqNo:2335; depth 24.55 2021-02-12T23:16:47Z- handleDvl() - RDI_Pathfinder msg: 1613171807.24 epoch sec; seqNo:979 2021-02-12T23:16:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.64 , 87.09 , 62.92 , 96.82 2021-02-12T23:16:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , -0.11 2021-02-12T23:16:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171807.25 epoch sec; seqNo:2335; 4056597.12 north; 590699.80 east 2021-02-12T23:16:47Z- AHRS_M2 msg: 1613171807.62 epoch sec; seqNo:2312 2021-02-12T23:16:47Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.21 psi 2021-02-12T23:16:47Z- handleDepth()-Depth_Keller msg: 1613171807624 epoch sec; seqNo:2336; depth 24.69 2021-02-12T23:16:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171807.63 epoch sec; seqNo:2336; 4056597.49 north; 590699.79 east 2021-02-12T23:16:48Z- AHRS_M2 msg: 1613171808.05 epoch sec; seqNo:2313 2021-02-12T23:16:48Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.21 psi 2021-02-12T23:16:48Z- handleDepth()-Depth_Keller msg: 1613171808053 epoch sec; seqNo:2337; depth 24.82 2021-02-12T23:16:48Z- handleDvl() - RDI_Pathfinder msg: 1613171808.07 epoch sec; seqNo:980 2021-02-12T23:16:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.82 , 86.01 , 62.10 , 95.74 2021-02-12T23:16:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.10 , -0.10 2021-02-12T23:16:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171808.08 epoch sec; seqNo:2337; 4056597.87 north; 590699.79 east 2021-02-12T23:16:48Z- AHRS_M2 msg: 1613171808.43 epoch sec; seqNo:2314 2021-02-12T23:16:48Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.21 psi 2021-02-12T23:16:48Z- handleDepth()-Depth_Keller msg: 1613171808441 epoch sec; seqNo:2338; depth 24.97 2021-02-12T23:16:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171808.45 epoch sec; seqNo:2338; 4056598.25 north; 590699.78 east 2021-02-12T23:16:48Z- AHRS_M2 msg: 1613171808.83 epoch sec; seqNo:2315 2021-02-12T23:16:48Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.21 psi 2021-02-12T23:16:48Z- handleDepth()-Depth_Keller msg: 1613171808836 epoch sec; seqNo:2339; depth 25.10 2021-02-12T23:16:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171808.86 epoch sec; seqNo:2339; 4056598.63 north; 590699.78 east 2021-02-12T23:16:49Z- AHRS_M2 msg: 1613171809.24 epoch sec; seqNo:2316 2021-02-12T23:16:49Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.21 psi 2021-02-12T23:16:49Z- handleDepth()-Depth_Keller msg: 1613171809241 epoch sec; seqNo:2340; depth 25.23 2021-02-12T23:16:49Z- handleDvl() - RDI_Pathfinder msg: 1613171809.25 epoch sec; seqNo:981 2021-02-12T23:16:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.41 , 0.00 , 60.88 , 96.82 2021-02-12T23:16:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.13 , -0.07 2021-02-12T23:16:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171809.28 epoch sec; seqNo:2340; 4056598.63 north; 590699.78 east 2021-02-12T23:16:49Z- AHRS_M2 msg: 1613171809.64 epoch sec; seqNo:2317 2021-02-12T23:16:49Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.21 psi 2021-02-12T23:16:49Z- handleDepth()-Depth_Keller msg: 1613171809644 epoch sec; seqNo:2341; depth 25.37 2021-02-12T23:16:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171809.65 epoch sec; seqNo:2341; 4056599.01 north; 590699.78 east 2021-02-12T23:16:50Z- AHRS_M2 msg: 1613171810.07 epoch sec; seqNo:2318 2021-02-12T23:16:50Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.21 psi 2021-02-12T23:16:50Z- handleDepth()-Depth_Keller msg: 1613171810070 epoch sec; seqNo:2342; depth 25.50 2021-02-12T23:16:50Z- handleDvl() - RDI_Pathfinder msg: 1613171810.08 epoch sec; seqNo:982 2021-02-12T23:16:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.41 , 86.01 , 61.69 , 97.91 2021-02-12T23:16:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.14 , -0.01 2021-02-12T23:16:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171810.09 epoch sec; seqNo:2342; 4056599.39 north; 590699.77 east 2021-02-12T23:16:50Z- AHRS_M2 msg: 1613171810.45 epoch sec; seqNo:2319 2021-02-12T23:16:50Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.21 psi 2021-02-12T23:16:50Z- handleDepth()-Depth_Keller msg: 1613171810463 epoch sec; seqNo:2343; depth 25.65 2021-02-12T23:16:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171810.47 epoch sec; seqNo:2343; 4056599.78 north; 590700.99 east 2021-02-12T23:16:50Z- AHRS_M2 msg: 1613171810.85 epoch sec; seqNo:2320 2021-02-12T23:16:50Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.21 psi 2021-02-12T23:16:50Z- handleDepth()-Depth_Keller msg: 1613171810856 epoch sec; seqNo:2344; depth 25.77 2021-02-12T23:16:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171810.86 epoch sec; seqNo:2344; 4056599.78 north; 590700.99 east 2021-02-12T23:16:51Z- AHRS_M2 msg: 1613171811.26 epoch sec; seqNo:2321 2021-02-12T23:16:51Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-12T23:16:51Z- handleDepth()-Depth_Keller msg: 1613171811265 epoch sec; seqNo:2345; depth 25.91 2021-02-12T23:16:51Z- handleDvl() - RDI_Pathfinder msg: 1613171811.28 epoch sec; seqNo:983 2021-02-12T23:16:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.00 , 86.01 , 0.00 , 96.82 2021-02-12T23:16:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.12 , 0.05 2021-02-12T23:16:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171811.29 epoch sec; seqNo:2345; 4056600.16 north; 590700.98 east 2021-02-12T23:16:51Z- AHRS_M2 msg: 1613171811.66 epoch sec; seqNo:2322 2021-02-12T23:16:51Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-12T23:16:51Z- handleDepth()-Depth_Keller msg: 1613171811666 epoch sec; seqNo:2346; depth 26.05 2021-02-12T23:16:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171811.68 epoch sec; seqNo:2346; 4056600.54 north; 590700.98 east 2021-02-12T23:16:52Z- AHRS_M2 msg: 1613171812.06 epoch sec; seqNo:2323 2021-02-12T23:16:52Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.20 psi 2021-02-12T23:16:52Z- handleDepth()-Depth_Keller msg: 1613171812073 epoch sec; seqNo:2347; depth 26.17 2021-02-12T23:16:52Z- handleDvl() - RDI_Pathfinder msg: 1613171812.09 epoch sec; seqNo:984 2021-02-12T23:16:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.60 , 83.71 , 61.28 , 98.31 2021-02-12T23:16:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.07 , 0.10 2021-02-12T23:16:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171812.10 epoch sec; seqNo:2347; 4056600.54 north; 590700.98 east 2021-02-12T23:16:52Z- LcmTrn::handleMessages() - handling 45 messages took 0.63 ms and 133 calls... 2021-02-12T23:16:52Z- waiting for 1613171809.83, time is 1613171812.10 2021-02-12T23:16:52Z- LcmTrn::updateTrn() equating pose 1613171812.10 and meas times 1613171812.10 2021-02-12T23:16:52Z- LcmTrn::updateTrn() >>>> pose time 1613171812.10, meas time 1613171812.10 <<<< 2021-02-12T23:16:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171812.10 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:16:52Z- TerrainNav::motionUpdate() - currEstimate = (4056601, 590700.98, 26.17) 2021-02-12T23:16:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:16:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:16:52Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171812.10 sec.; No relevant navigation data available 2021-02-12T23:16:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:16:52Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:52Z- variances([0], [2], and psi_berg) = 783.561725 833.072962 0.000000 2021-02-12T23:16:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:16:52Z- LcmTrn::publishEstimates() - MLE : 4056576.04 590657.48 26.17 2021-02-12T23:16:52Z- LcmTrn::publishEstimates() - MMSE : 4056600.54 590700.98 26.17 2021-02-12T23:16:52Z- LcmTrn::publishEstimates() - COVAR: 783.56 833.07 0.94 0.00 2021-02-12T23:16:52Z- AHRS_M2 msg: 1613171812.47 epoch sec; seqNo:2324 2021-02-12T23:16:52Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.20 psi 2021-02-12T23:16:52Z- handleDepth()-Depth_Keller msg: 1613171812489 epoch sec; seqNo:2348; depth 26.32 2021-02-12T23:16:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171812.52 epoch sec; seqNo:2348; 4056600.92 north; 590700.98 east 2021-02-12T23:16:52Z- AHRS_M2 msg: 1613171812.87 epoch sec; seqNo:2325 2021-02-12T23:16:52Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.20 psi 2021-02-12T23:16:52Z- handleDepth()-Depth_Keller msg: 1613171812878 epoch sec; seqNo:2349; depth 26.45 2021-02-12T23:16:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171812.89 epoch sec; seqNo:2349; 4056601.30 north; 590700.97 east 2021-02-12T23:16:53Z- AHRS_M2 msg: 1613171813.28 epoch sec; seqNo:2326 2021-02-12T23:16:53Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.20 psi 2021-02-12T23:16:53Z- handleDepth()-Depth_Keller msg: 1613171813281 epoch sec; seqNo:2350; depth 26.57 2021-02-12T23:16:53Z- handleDvl() - RDI_Pathfinder msg: 1613171813.30 epoch sec; seqNo:985 2021-02-12T23:16:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:16:53Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:16:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171813.32 epoch sec; seqNo:2350; 4056601.67 north; 590700.97 east 2021-02-12T23:16:53Z- AHRS_M2 msg: 1613171813.68 epoch sec; seqNo:2327 2021-02-12T23:16:53Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.20 psi 2021-02-12T23:16:53Z- handleDepth()-Depth_Keller msg: 1613171813684 epoch sec; seqNo:2351; depth 26.71 2021-02-12T23:16:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171813.70 epoch sec; seqNo:2351; 4056601.69 north; 590702.19 east 2021-02-12T23:16:54Z- AHRS_M2 msg: 1613171814.09 epoch sec; seqNo:2328 2021-02-12T23:16:54Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.20 psi 2021-02-12T23:16:54Z- handleDepth()-Depth_Keller msg: 1613171814099 epoch sec; seqNo:2352; depth 26.84 2021-02-12T23:16:54Z- handleDvl() - RDI_Pathfinder msg: 1613171814.11 epoch sec; seqNo:986 2021-02-12T23:16:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.78 , 79.67 , 60.88 , 95.61 2021-02-12T23:16:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.07 , 0.10 2021-02-12T23:16:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171814.13 epoch sec; seqNo:2352; 4056602.07 north; 590702.19 east 2021-02-12T23:16:54Z- AHRS_M2 msg: 1613171814.49 epoch sec; seqNo:2329 2021-02-12T23:16:54Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.20 psi 2021-02-12T23:16:54Z- handleDepth()-Depth_Keller msg: 1613171814497 epoch sec; seqNo:2353; depth 26.98 2021-02-12T23:16:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171814.52 epoch sec; seqNo:2353; 4056602.07 north; 590702.19 east 2021-02-12T23:16:54Z- AHRS_M2 msg: 1613171814.89 epoch sec; seqNo:2330 2021-02-12T23:16:54Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.20 psi 2021-02-12T23:16:54Z- handleDepth()-Depth_Keller msg: 1613171814900 epoch sec; seqNo:2354; depth 27.10 2021-02-12T23:16:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171814.92 epoch sec; seqNo:2354; 4056602.45 north; 590702.18 east 2021-02-12T23:16:55Z- AHRS_M2 msg: 1613171815.30 epoch sec; seqNo:2331 2021-02-12T23:16:55Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.19 psi 2021-02-12T23:16:55Z- handleDepth()-Depth_Keller msg: 1613171815309 epoch sec; seqNo:2355; depth 27.24 2021-02-12T23:16:55Z- handleDvl() - RDI_Pathfinder msg: 1613171815.32 epoch sec; seqNo:987 2021-02-12T23:16:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.19 , 86.01 , 60.88 , 95.74 2021-02-12T23:16:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.13 , 0.03 2021-02-12T23:16:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171815.33 epoch sec; seqNo:2355; 4056602.82 north; 590702.18 east 2021-02-12T23:16:55Z- AHRS_M2 msg: 1613171815.70 epoch sec; seqNo:2332 2021-02-12T23:16:55Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:16:55Z- handleDepth()-Depth_Keller msg: 1613171815704 epoch sec; seqNo:2356; depth 27.36 2021-02-12T23:16:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171815.72 epoch sec; seqNo:2356; 4056602.82 north; 590702.18 east 2021-02-12T23:16:56Z- AHRS_M2 msg: 1613171816.10 epoch sec; seqNo:2333 2021-02-12T23:16:56Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:16:56Z- handleDepth()-Depth_Keller msg: 1613171816109 epoch sec; seqNo:2357; depth 27.50 2021-02-12T23:16:56Z- handleDvl() - RDI_Pathfinder msg: 1613171816.12 epoch sec; seqNo:988 2021-02-12T23:16:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.78 , 79.26 , 60.47 , 91.42 2021-02-12T23:16:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.15 , -0.05 2021-02-12T23:16:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171816.16 epoch sec; seqNo:2357; 4056603.22 north; 590703.39 east 2021-02-12T23:16:56Z- AHRS_M2 msg: 1613171816.51 epoch sec; seqNo:2334 2021-02-12T23:16:56Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:16:56Z- handleDepth()-Depth_Keller msg: 1613171816517 epoch sec; seqNo:2358; depth 27.64 2021-02-12T23:16:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171816.53 epoch sec; seqNo:2358; 4056603.59 north; 590703.39 east 2021-02-12T23:16:56Z- AHRS_M2 msg: 1613171816.91 epoch sec; seqNo:2335 2021-02-12T23:16:56Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:16:56Z- handleDepth()-Depth_Keller msg: 1613171816930 epoch sec; seqNo:2359; depth 27.75 2021-02-12T23:16:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171816.95 epoch sec; seqNo:2359; 4056603.97 north; 590703.39 east 2021-02-12T23:16:57Z- LcmTrn::handleMessages() - handling 40 messages took 0.74 ms and 125 calls... 2021-02-12T23:16:57Z- waiting for 1613171815.10, time is 1613171816.95 2021-02-12T23:16:57Z- LcmTrn::updateTrn() >>>> pose time 1613171816.95, meas time 1613171816.12 <<<< 2021-02-12T23:16:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171816.12 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:16:57Z- TerrainNav::motionUpdate() - currEstimate = (4056604, 590703.39, 27.75) 2021-02-12T23:16:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:16:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:16:57Z- TNavBF::Projecting Measurements 2021-02-12T23:16:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:16:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:16:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:16:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:16:57Z- TNavFilter::4 good beams. 2021-02-12T23:16:57Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:16:57Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:16:57Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:16:57Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:16:57Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:16:57Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:16:57Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:16:57Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:57Z- TerrainMapOctree: (vehN, vehE ) = (4056603.39, 590702.97). 2021-02-12T23:16:57Z- TerrainMapOctree: Min Distance = 5845.72. 2021-02-12T23:16:57Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:16:57Z- There is no specific good beam that all particles have in common. 2021-02-12T23:16:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:16:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.114 ExpectedMeasDiff: nan currMeas.covariance: 6.92 2021-02-12T23:16:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:16:57Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 10.05 2021-02-12T23:16:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:16:57Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 5.85 2021-02-12T23:16:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:16:57Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 13.37 2021-02-12T23:16:57Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:16:57Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:16:57Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:16:57Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:16:57Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:16:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:16:57Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:16:57Z- Filter 0 No beams used in subcloud update 2021-02-12T23:16:57Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:16:57Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:16:57Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 783.4 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:16:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171816.12 sec. 2021-02-12T23:16:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:16:57Z- TNavBF::mmse selected from filter -1 2021-02-12T23:16:57Z- variances([0], [2], and psi_berg) = 783.341708 833.072625 0.000000 2021-02-12T23:16:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:16:57Z- LcmTrn::publishEstimates() - MLE : 4056579.47 590659.89 27.75 2021-02-12T23:16:57Z- LcmTrn::publishEstimates() - MMSE : 4056603.97 590703.39 27.75 2021-02-12T23:16:57Z- LcmTrn::publishEstimates() - COVAR: 783.34 833.07 0.81 0.00 2021-02-12T23:16:57Z- AHRS_M2 msg: 1613171817.32 epoch sec; seqNo:2336 2021-02-12T23:16:57Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.19 psi 2021-02-12T23:16:57Z- handleDepth()-Depth_Keller msg: 1613171817320 epoch sec; seqNo:2360; depth 27.88 2021-02-12T23:16:57Z- handleDvl() - RDI_Pathfinder msg: 1613171817.33 epoch sec; seqNo:989 2021-02-12T23:16:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.37 , 78.44 , 59.65 , 92.50 2021-02-12T23:16:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.09 , -0.11 2021-02-12T23:16:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171817.35 epoch sec; seqNo:2360; 4056603.97 north; 590703.39 east 2021-02-12T23:16:57Z- AHRS_M2 msg: 1613171817.72 epoch sec; seqNo:2337 2021-02-12T23:16:57Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.19 psi 2021-02-12T23:16:57Z- handleDepth()-Depth_Keller msg: 1613171817724 epoch sec; seqNo:2361; depth 28.00 2021-02-12T23:16:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171817.74 epoch sec; seqNo:2361; 4056604.35 north; 590703.38 east 2021-02-12T23:16:58Z- AHRS_M2 msg: 1613171818.13 epoch sec; seqNo:2338 2021-02-12T23:16:58Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.19 psi 2021-02-12T23:16:58Z- handleDepth()-Depth_Keller msg: 1613171818135 epoch sec; seqNo:2362; depth 28.13 2021-02-12T23:16:58Z- handleDvl() - RDI_Pathfinder msg: 1613171818.19 epoch sec; seqNo:990 2021-02-12T23:16:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.55 , 78.04 , 60.06 , 90.34 2021-02-12T23:16:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , -0.14 2021-02-12T23:16:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171818.20 epoch sec; seqNo:2362; 4056604.74 north; 590704.60 east 2021-02-12T23:16:58Z- AHRS_M2 msg: 1613171818.53 epoch sec; seqNo:2339 2021-02-12T23:16:58Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.19 psi 2021-02-12T23:16:58Z- handleDepth()-Depth_Keller msg: 1613171818532 epoch sec; seqNo:2363; depth 28.28 2021-02-12T23:16:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171818.55 epoch sec; seqNo:2363; 4056605.12 north; 590704.60 east 2021-02-12T23:16:58Z- AHRS_M2 msg: 1613171818.93 epoch sec; seqNo:2340 2021-02-12T23:16:58Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.18 psi 2021-02-12T23:16:58Z- handleDepth()-Depth_Keller msg: 1613171818941 epoch sec; seqNo:2364; depth 28.37 2021-02-12T23:16:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171818.95 epoch sec; seqNo:2364; 4056605.12 north; 590704.60 east 2021-02-12T23:16:59Z- AHRS_M2 msg: 1613171819.34 epoch sec; seqNo:2341 2021-02-12T23:16:59Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.19 psi 2021-02-12T23:16:59Z- handleDepth()-Depth_Keller msg: 1613171819340 epoch sec; seqNo:2365; depth 28.49 2021-02-12T23:16:59Z- handleDvl() - RDI_Pathfinder msg: 1613171819.35 epoch sec; seqNo:991 2021-02-12T23:16:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.74 , 80.61 , 59.65 , 169.27 2021-02-12T23:16:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.05 , -0.13 2021-02-12T23:16:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171819.36 epoch sec; seqNo:2365; 4056605.50 north; 590704.59 east 2021-02-12T23:16:59Z- AHRS_M2 msg: 1613171819.74 epoch sec; seqNo:2342 2021-02-12T23:16:59Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.18 psi 2021-02-12T23:16:59Z- handleDepth()-Depth_Keller msg: 1613171819746 epoch sec; seqNo:2366; depth 28.62 2021-02-12T23:16:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171819.76 epoch sec; seqNo:2366; 4056605.88 north; 590704.59 east 2021-02-12T23:17:00Z- AHRS_M2 msg: 1613171820.15 epoch sec; seqNo:2343 2021-02-12T23:17:00Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.18 psi 2021-02-12T23:17:00Z- handleDepth()-Depth_Keller msg: 1613171820158 epoch sec; seqNo:2367; depth 28.74 2021-02-12T23:17:00Z- handleDvl() - RDI_Pathfinder msg: 1613171820.17 epoch sec; seqNo:992 2021-02-12T23:17:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.92 , 75.99 , 58.83 , 0.00 2021-02-12T23:17:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.11 , -0.10 2021-02-12T23:17:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171820.18 epoch sec; seqNo:2367; 4056606.26 north; 590704.58 east 2021-02-12T23:17:00Z- AHRS_M2 msg: 1613171820.55 epoch sec; seqNo:2344 2021-02-12T23:17:00Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.18 psi 2021-02-12T23:17:00Z- handleDepth()-Depth_Keller msg: 1613171820557 epoch sec; seqNo:2368; depth 28.87 2021-02-12T23:17:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171820.59 epoch sec; seqNo:2368; 4056606.64 north; 590704.58 east 2021-02-12T23:17:00Z- AHRS_M2 msg: 1613171820.95 epoch sec; seqNo:2345 2021-02-12T23:17:00Z- AHRS_M2 msg: 0.59 phi; -0.25 theta; -0.18 psi 2021-02-12T23:17:00Z- handleDepth()-Depth_Keller msg: 1613171820956 epoch sec; seqNo:2369; depth 28.99 2021-02-12T23:17:00Z- handleDvl() - RDI_Pathfinder msg: 1613171820.97 epoch sec; seqNo:993 2021-02-12T23:17:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.84 , 75.41 , 59.17 , 0.00 2021-02-12T23:17:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.17 , -0.05 2021-02-12T23:17:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171820.98 epoch sec; seqNo:2369; 4056607.03 north; 590705.80 east 2021-02-12T23:17:01Z- AHRS_M2 msg: 1613171821.36 epoch sec; seqNo:2346 2021-02-12T23:17:01Z- AHRS_M2 msg: 0.58 phi; -0.22 theta; -0.17 psi 2021-02-12T23:17:01Z- handleDepth()-Depth_Keller msg: 1613171821362 epoch sec; seqNo:2370; depth 29.12 2021-02-12T23:17:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171821.37 epoch sec; seqNo:2370; 4056607.41 north; 590705.79 east 2021-02-12T23:17:01Z- AHRS_M2 msg: 1613171821.76 epoch sec; seqNo:2347 2021-02-12T23:17:01Z- AHRS_M2 msg: 0.57 phi; -0.18 theta; -0.16 psi 2021-02-12T23:17:01Z- handleDepth()-Depth_Keller msg: 1613171821764 epoch sec; seqNo:2371; depth 29.24 2021-02-12T23:17:01Z- handleDvl() - RDI_Pathfinder msg: 1613171821.78 epoch sec; seqNo:994 2021-02-12T23:17:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.23 , 72.96 , 60.39 , 0.00 2021-02-12T23:17:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.17 , 0.01 2021-02-12T23:17:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171821.79 epoch sec; seqNo:2371; 4056607.41 north; 590705.79 east 2021-02-12T23:17:02Z- AHRS_M2 msg: 1613171822.16 epoch sec; seqNo:2348 2021-02-12T23:17:02Z- AHRS_M2 msg: 0.56 phi; -0.14 theta; -0.15 psi 2021-02-12T23:17:02Z- handleDepth()-Depth_Keller msg: 1613171822168 epoch sec; seqNo:2372; depth 29.35 2021-02-12T23:17:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171822.18 epoch sec; seqNo:2372; 4056607.79 north; 590705.79 east 2021-02-12T23:17:02Z- LcmTrn::handleMessages() - handling 45 messages took 0.64 ms and 132 calls... 2021-02-12T23:17:02Z- waiting for 1613171819.95, time is 1613171822.18 2021-02-12T23:17:02Z- LcmTrn::updateTrn() >>>> pose time 1613171822.18, meas time 1613171821.78 <<<< 2021-02-12T23:17:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171821.78 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:17:02Z- TerrainNav::motionUpdate() - currEstimate = (4056608, 590705.79, 29.35) 2021-02-12T23:17:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:17:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:17:02Z- TNavBF::Projecting Measurements 2021-02-12T23:17:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:17:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:17:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:17:02Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:17:02Z- TNavFilter::3 good beams. 2021-02-12T23:17:02Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:17:02Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:17:02Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:17:02Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:17:02Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:17:02Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:17:02Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:17:02Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:02Z- TerrainMapOctree: (vehN, vehE ) = (4056607.49, 590705.60). 2021-02-12T23:17:02Z- TerrainMapOctree: Min Distance = 5840.86. 2021-02-12T23:17:02Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:17:02Z- There is no specific good beam that all particles have in common. 2021-02-12T23:17:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:17:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.114 ExpectedMeasDiff: nan currMeas.covariance: 6.20 2021-02-12T23:17:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:17:02Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 8.52 2021-02-12T23:17:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:17:02Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 5.84 2021-02-12T23:17:02Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:17:02Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:17:02Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:17:02Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:17:02Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:17:02Z- Filter 0 No beams used in subcloud update 2021-02-12T23:17:02Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:17:02Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:17:02Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 783.2 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:17:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171821.78 sec. 2021-02-12T23:17:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:17:02Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:02Z- variances([0], [2], and psi_berg) = 783.129130 833.072300 0.000000 2021-02-12T23:17:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:17:02Z- LcmTrn::publishEstimates() - MLE : 4056583.29 590662.29 29.35 2021-02-12T23:17:02Z- LcmTrn::publishEstimates() - MMSE : 4056607.79 590705.79 29.35 2021-02-12T23:17:02Z- LcmTrn::publishEstimates() - COVAR: 783.13 833.07 0.77 0.00 2021-02-12T23:17:02Z- AHRS_M2 msg: 1613171822.57 epoch sec; seqNo:2349 2021-02-12T23:17:02Z- AHRS_M2 msg: 0.55 phi; -0.10 theta; -0.14 psi 2021-02-12T23:17:02Z- handleDepth()-Depth_Keller msg: 1613171822577 epoch sec; seqNo:2373; depth 29.46 2021-02-12T23:17:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171822.59 epoch sec; seqNo:2373; 4056608.17 north; 590705.78 east 2021-02-12T23:17:02Z- AHRS_M2 msg: 1613171822.97 epoch sec; seqNo:2350 2021-02-12T23:17:02Z- AHRS_M2 msg: 0.54 phi; -0.05 theta; -0.14 psi 2021-02-12T23:17:02Z- handleDepth()-Depth_Keller msg: 1613171822979 epoch sec; seqNo:2374; depth 29.55 2021-02-12T23:17:03Z- handleDvl() - RDI_Pathfinder msg: 1613171823.00 epoch sec; seqNo:995 2021-02-12T23:17:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.62 , 70.82 , 62.84 , 0.00 2021-02-12T23:17:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.15 , 0.04 2021-02-12T23:17:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171823.01 epoch sec; seqNo:2374; 4056608.55 north; 590705.78 east 2021-02-12T23:17:03Z- AHRS_M2 msg: 1613171823.38 epoch sec; seqNo:2351 2021-02-12T23:17:03Z- AHRS_M2 msg: 0.54 phi; -0.01 theta; -0.14 psi 2021-02-12T23:17:03Z- handleDepth()-Depth_Keller msg: 1613171823380 epoch sec; seqNo:2375; depth 29.65 2021-02-12T23:17:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171823.39 epoch sec; seqNo:2375; 4056608.92 north; 590705.78 east 2021-02-12T23:17:03Z- AHRS_M2 msg: 1613171823.78 epoch sec; seqNo:2352 2021-02-12T23:17:03Z- AHRS_M2 msg: 0.53 phi; 0.03 theta; -0.13 psi 2021-02-12T23:17:03Z- handleDepth()-Depth_Keller msg: 1613171823784 epoch sec; seqNo:2376; depth 29.71 2021-02-12T23:17:03Z- handleDvl() - RDI_Pathfinder msg: 1613171823.80 epoch sec; seqNo:996 2021-02-12T23:17:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.62 , 69.59 , 65.91 , 64.38 2021-02-12T23:17:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.10 , 0.05 2021-02-12T23:17:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171823.81 epoch sec; seqNo:2376; 4056609.30 north; 590705.77 east 2021-02-12T23:17:04Z- AHRS_M2 msg: 1613171824.18 epoch sec; seqNo:2353 2021-02-12T23:17:04Z- AHRS_M2 msg: 0.52 phi; 0.06 theta; -0.13 psi 2021-02-12T23:17:04Z- handleDepth()-Depth_Keller msg: 1613171824197 epoch sec; seqNo:2377; depth 29.75 2021-02-12T23:17:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171824.21 epoch sec; seqNo:2377; 4056609.30 north; 590705.77 east 2021-02-12T23:17:04Z- AHRS_M2 msg: 1613171824.59 epoch sec; seqNo:2354 2021-02-12T23:17:04Z- AHRS_M2 msg: 0.52 phi; 0.10 theta; -0.13 psi 2021-02-12T23:17:04Z- handleDepth()-Depth_Keller msg: 1613171824592 epoch sec; seqNo:2378; depth 29.80 2021-02-12T23:17:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171824.61 epoch sec; seqNo:2378; 4056609.70 north; 590706.99 east 2021-02-12T23:17:04Z- AHRS_M2 msg: 1613171824.99 epoch sec; seqNo:2355 2021-02-12T23:17:04Z- AHRS_M2 msg: 0.52 phi; 0.13 theta; -0.12 psi 2021-02-12T23:17:04Z- handleDepth()-Depth_Keller msg: 1613171824996 epoch sec; seqNo:2379; depth 29.84 2021-02-12T23:17:05Z- handleDvl() - RDI_Pathfinder msg: 1613171825.01 epoch sec; seqNo:997 2021-02-12T23:17:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.23 , 69.59 , 69.59 , 61.92 2021-02-12T23:17:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.07 , 0.04 2021-02-12T23:17:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171825.03 epoch sec; seqNo:2379; 4056610.07 north; 590706.99 east 2021-02-12T23:17:05Z- AHRS_M2 msg: 1613171825.40 epoch sec; seqNo:2356 2021-02-12T23:17:05Z- AHRS_M2 msg: 0.52 phi; 0.16 theta; -0.12 psi 2021-02-12T23:17:05Z- handleDepth()-Depth_Keller msg: 1613171825401 epoch sec; seqNo:2380; depth 29.86 2021-02-12T23:17:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171825.41 epoch sec; seqNo:2380; 4056610.45 north; 590706.98 east 2021-02-12T23:17:05Z- AHRS_M2 msg: 1613171825.80 epoch sec; seqNo:2357 2021-02-12T23:17:05Z- AHRS_M2 msg: 0.52 phi; 0.19 theta; -0.12 psi 2021-02-12T23:17:05Z- handleDepth()-Depth_Keller msg: 1613171825806 epoch sec; seqNo:2381; depth 29.83 2021-02-12T23:17:05Z- handleDvl() - RDI_Pathfinder msg: 1613171825.82 epoch sec; seqNo:998 2021-02-12T23:17:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.84 , 70.20 , 73.57 , 59.78 2021-02-12T23:17:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.01 2021-02-12T23:17:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171825.83 epoch sec; seqNo:2381; 4056610.83 north; 590706.98 east 2021-02-12T23:17:06Z- AHRS_M2 msg: 1613171826.21 epoch sec; seqNo:2358 2021-02-12T23:17:06Z- AHRS_M2 msg: 0.52 phi; 0.21 theta; -0.12 psi 2021-02-12T23:17:06Z- handleDepth()-Depth_Keller msg: 1613171826227 epoch sec; seqNo:2382; depth 29.85 2021-02-12T23:17:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171826.24 epoch sec; seqNo:2382; 4056611.21 north; 590706.97 east 2021-02-12T23:17:06Z- AHRS_M2 msg: 1613171826.61 epoch sec; seqNo:2359 2021-02-12T23:17:06Z- AHRS_M2 msg: 0.52 phi; 0.23 theta; -0.12 psi 2021-02-12T23:17:06Z- handleDepth()-Depth_Keller msg: 1613171826622 epoch sec; seqNo:2383; depth 29.80 2021-02-12T23:17:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171826.66 epoch sec; seqNo:2383; 4056611.21 north; 590706.97 east 2021-02-12T23:17:07Z- AHRS_M2 msg: 1613171827.01 epoch sec; seqNo:2360 2021-02-12T23:17:07Z- AHRS_M2 msg: 0.53 phi; 0.25 theta; -0.12 psi 2021-02-12T23:17:07Z- handleDepth()-Depth_Keller msg: 1613171827017 epoch sec; seqNo:2384; depth 29.77 2021-02-12T23:17:07Z- handleDvl() - RDI_Pathfinder msg: 1613171827.03 epoch sec; seqNo:999 2021-02-12T23:17:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.07 , 71.43 , 77.87 , 59.17 2021-02-12T23:17:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , -0.02 2021-02-12T23:17:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171827.04 epoch sec; seqNo:2384; 4056611.59 north; 590706.97 east 2021-02-12T23:17:07Z- AHRS_M2 msg: 1613171827.42 epoch sec; seqNo:2361 2021-02-12T23:17:07Z- AHRS_M2 msg: 0.53 phi; 0.27 theta; -0.12 psi 2021-02-12T23:17:07Z- handleDepth()-Depth_Keller msg: 1613171827429 epoch sec; seqNo:2385; depth 29.74 2021-02-12T23:17:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171827.44 epoch sec; seqNo:2385; 4056611.97 north; 590706.97 east 2021-02-12T23:17:07Z- LcmTrn::handleMessages() - handling 44 messages took 0.74 ms and 130 calls... 2021-02-12T23:17:07Z- waiting for 1613171825.18, time is 1613171827.44 2021-02-12T23:17:07Z- LcmTrn::updateTrn() >>>> pose time 1613171827.44, meas time 1613171827.03 <<<< 2021-02-12T23:17:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171827.03 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:17:07Z- TerrainNav::motionUpdate() - currEstimate = (4056612, 590706.97, 29.74) 2021-02-12T23:17:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:17:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:17:07Z- TNavBF::Projecting Measurements 2021-02-12T23:17:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:17:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:17:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:17:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:17:07Z- TNavFilter::4 good beams. 2021-02-12T23:17:07Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:17:07Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:17:07Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:17:07Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:17:07Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:17:07Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:17:07Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:17:07Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:07Z- TerrainMapOctree: (vehN, vehE ) = (4056611.64, 590706.87). 2021-02-12T23:17:07Z- TerrainMapOctree: Min Distance = 5836.79. 2021-02-12T23:17:07Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:17:07Z- There is no specific good beam that all particles have in common. 2021-02-12T23:17:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:17:07Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 6.57 2021-02-12T23:17:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:17:07Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 8.16 2021-02-12T23:17:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:17:07Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 9.70 2021-02-12T23:17:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:17:07Z- Old Depth Bias: 0.00 DepthBiasCov: 0.75 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 5.60 2021-02-12T23:17:07Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:17:07Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:17:07Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:17:07Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:17:07Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:17:07Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:17:07Z- Filter 0 No beams used in subcloud update 2021-02-12T23:17:07Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:17:07Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:17:07Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 783.0 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:17:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171827.03 sec. 2021-02-12T23:17:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:17:07Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:07Z- variances([0], [2], and psi_berg) = 782.921052 833.071984 0.000000 2021-02-12T23:17:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:17:07Z- LcmTrn::publishEstimates() - MLE : 4056587.47 590663.47 29.74 2021-02-12T23:17:07Z- LcmTrn::publishEstimates() - MMSE : 4056611.97 590706.97 29.74 2021-02-12T23:17:07Z- LcmTrn::publishEstimates() - COVAR: 782.92 833.07 0.70 0.00 2021-02-12T23:17:07Z- AHRS_M2 msg: 1613171827.82 epoch sec; seqNo:2362 2021-02-12T23:17:07Z- AHRS_M2 msg: 0.53 phi; 0.28 theta; -0.12 psi 2021-02-12T23:17:07Z- handleDepth()-Depth_Keller msg: 1613171827824 epoch sec; seqNo:2386; depth 29.67 2021-02-12T23:17:07Z- handleDvl() - RDI_Pathfinder msg: 1613171827.84 epoch sec; seqNo:1000 2021-02-12T23:17:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.72 , 72.16 , 81.22 , 59.22 2021-02-12T23:17:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , -0.03 2021-02-12T23:17:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171827.85 epoch sec; seqNo:2386; 4056612.36 north; 590708.18 east 2021-02-12T23:17:08Z- AHRS_M2 msg: 1613171828.22 epoch sec; seqNo:2363 2021-02-12T23:17:08Z- AHRS_M2 msg: 0.54 phi; 0.30 theta; -0.11 psi 2021-02-12T23:17:08Z- handleDepth()-Depth_Keller msg: 1613171828234 epoch sec; seqNo:2387; depth 29.63 2021-02-12T23:17:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:17:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:17:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:17:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:17:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:17:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:17:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:17:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:17:08Z- handleDvl() - RDI_Pathfinder msg: 1613171828.24 epoch sec; seqNo:1001 2021-02-12T23:17:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:17:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , -0.03 2021-02-12T23:17:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171828.25 epoch sec; seqNo:2387; 4056612.36 north; 590708.18 east 2021-02-12T23:17:08Z- AHRS_M2 msg: 1613171828.63 epoch sec; seqNo:2364 2021-02-12T23:17:08Z- AHRS_M2 msg: 0.54 phi; 0.31 theta; -0.11 psi 2021-02-12T23:17:08Z- handleDepth()-Depth_Keller msg: 1613171828640 epoch sec; seqNo:2388; depth 29.54 2021-02-12T23:17:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171828.66 epoch sec; seqNo:2388; 4056612.74 north; 590708.18 east 2021-02-12T23:17:09Z- AHRS_M2 msg: 1613171829.03 epoch sec; seqNo:2365 2021-02-12T23:17:09Z- AHRS_M2 msg: 0.54 phi; 0.32 theta; -0.11 psi 2021-02-12T23:17:09Z- handleDepth()-Depth_Keller msg: 1613171829036 epoch sec; seqNo:2389; depth 29.47 2021-02-12T23:17:09Z- handleDvl() - RDI_Pathfinder msg: 1613171829.05 epoch sec; seqNo:1002 2021-02-12T23:17:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.78 , 72.32 , 88.17 , 57.20 2021-02-12T23:17:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , -0.03 2021-02-12T23:17:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171829.06 epoch sec; seqNo:2389; 4056613.12 north; 590708.17 east 2021-02-12T23:17:09Z- AHRS_M2 msg: 1613171829.44 epoch sec; seqNo:2366 2021-02-12T23:17:09Z- AHRS_M2 msg: 0.54 phi; 0.33 theta; -0.11 psi 2021-02-12T23:17:09Z- handleDepth()-Depth_Keller msg: 1613171829440 epoch sec; seqNo:2390; depth 29.40 2021-02-12T23:17:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:17:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:17:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:17:09Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:17:09Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:17:09Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:17:09Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:17:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:17:09Z- handleDvl() - RDI_Pathfinder msg: 1613171829.44 epoch sec; seqNo:1003 2021-02-12T23:17:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:17:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , -0.03 2021-02-12T23:17:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171829.45 epoch sec; seqNo:2390; 4056613.50 north; 590708.17 east 2021-02-12T23:17:09Z- AHRS_M2 msg: 1613171829.84 epoch sec; seqNo:2367 2021-02-12T23:17:09Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.10 psi 2021-02-12T23:17:09Z- handleDepth()-Depth_Keller msg: 1613171829844 epoch sec; seqNo:2391; depth 29.31 2021-02-12T23:17:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171829.85 epoch sec; seqNo:2391; 4056613.50 north; 590708.17 east 2021-02-12T23:17:10Z- AHRS_M2 msg: 1613171830.24 epoch sec; seqNo:2368 2021-02-12T23:17:10Z- AHRS_M2 msg: 0.55 phi; 0.35 theta; -0.10 psi 2021-02-12T23:17:10Z- handleDepth()-Depth_Keller msg: 1613171830257 epoch sec; seqNo:2392; depth 29.21 2021-02-12T23:17:10Z- handleDvl() - RDI_Pathfinder msg: 1613171830.27 epoch sec; seqNo:1004 2021-02-12T23:17:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.60 , 73.13 , 91.42 , 57.20 2021-02-12T23:17:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.01 , -0.03 2021-02-12T23:17:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171830.28 epoch sec; seqNo:2392; 4056613.88 north; 590708.17 east 2021-02-12T23:17:10Z- AHRS_M2 msg: 1613171830.65 epoch sec; seqNo:2369 2021-02-12T23:17:10Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.09 psi 2021-02-12T23:17:10Z- handleDepth()-Depth_Keller msg: 1613171830652 epoch sec; seqNo:2393; depth 29.10 2021-02-12T23:17:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:17:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:17:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:17:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:17:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:17:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:17:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:17:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:17:10Z- handleDvl() - RDI_Pathfinder msg: 1613171830.65 epoch sec; seqNo:1005 2021-02-12T23:17:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:17:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.01 , -0.03 2021-02-12T23:17:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171830.68 epoch sec; seqNo:2393; 4056614.27 north; 590709.38 east 2021-02-12T23:17:11Z- AHRS_M2 msg: 1613171831.06 epoch sec; seqNo:2370 2021-02-12T23:17:11Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.08 psi 2021-02-12T23:17:11Z- handleDepth()-Depth_Keller msg: 1613171831063 epoch sec; seqNo:2394; depth 29.02 2021-02-12T23:17:11Z- handleDvl() - RDI_Pathfinder msg: 1613171831.10 epoch sec; seqNo:1006 2021-02-12T23:17:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.41 , 78.44 , 93.58 , 57.61 2021-02-12T23:17:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.03 , -0.03 2021-02-12T23:17:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171831.12 epoch sec; seqNo:2394; 4056614.27 north; 590709.38 east 2021-02-12T23:17:11Z- AHRS_M2 msg: 1613171831.46 epoch sec; seqNo:2371 2021-02-12T23:17:11Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.08 psi 2021-02-12T23:17:11Z- handleDepth()-Depth_Keller msg: 1613171831466 epoch sec; seqNo:2395; depth 28.91 2021-02-12T23:17:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171831.48 epoch sec; seqNo:2395; 4056614.65 north; 590709.38 east 2021-02-12T23:17:11Z- AHRS_M2 msg: 1613171831.87 epoch sec; seqNo:2372 2021-02-12T23:17:11Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.07 psi 2021-02-12T23:17:11Z- handleDepth()-Depth_Keller msg: 1613171831873 epoch sec; seqNo:2396; depth 28.80 2021-02-12T23:17:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:17:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:17:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:17:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:17:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:17:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:17:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:17:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:17:11Z- handleDvl() - RDI_Pathfinder msg: 1613171831.88 epoch sec; seqNo:1007 2021-02-12T23:17:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:17:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.03 , -0.03 2021-02-12T23:17:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171831.88 epoch sec; seqNo:2396; 4056615.02 north; 590709.38 east 2021-02-12T23:17:12Z- AHRS_M2 msg: 1613171832.26 epoch sec; seqNo:2373 2021-02-12T23:17:12Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.07 psi 2021-02-12T23:17:12Z- handleDepth()-Depth_Keller msg: 1613171832271 epoch sec; seqNo:2397; depth 28.68 2021-02-12T23:17:12Z- handleDvl() - RDI_Pathfinder msg: 1613171832.28 epoch sec; seqNo:1008 2021-02-12T23:17:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.23 , 73.13 , 94.66 , 58.43 2021-02-12T23:17:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.05 , -0.04 2021-02-12T23:17:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171832.31 epoch sec; seqNo:2397; 4056615.02 north; 590709.38 east 2021-02-12T23:17:12Z- LcmTrn::handleMessages() - handling 45 messages took 0.68 ms and 132 calls... 2021-02-12T23:17:12Z- waiting for 1613171830.44, time is 1613171832.31 2021-02-12T23:17:12Z- LcmTrn::updateTrn() equating pose 1613171832.31 and meas times 1613171832.31 2021-02-12T23:17:12Z- LcmTrn::updateTrn() >>>> pose time 1613171832.31, meas time 1613171832.31 <<<< 2021-02-12T23:17:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171832.31 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:17:12Z- TerrainNav::motionUpdate() - currEstimate = (4056615, 590709.38, 28.68) 2021-02-12T23:17:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:17:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:17:12Z- TNavBF::Projecting Measurements 2021-02-12T23:17:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:17:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:17:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:17:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:17:12Z- TNavFilter::4 good beams. 2021-02-12T23:17:12Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:17:12Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:17:12Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:17:12Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:17:12Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:17:12Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:17:12Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:17:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:12Z- TerrainMapOctree: (vehN, vehE ) = (4056615.02, 590709.38). 2021-02-12T23:17:12Z- TerrainMapOctree: Min Distance = 5832.58. 2021-02-12T23:17:12Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:17:12Z- There is no specific good beam that all particles have in common. 2021-02-12T23:17:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:17:12Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-12T23:17:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:17:12Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-12T23:17:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:17:12Z- Old Depth Bias: 0.00 DepthBiasCov: 0.75 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 14.34 2021-02-12T23:17:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:17:12Z- Old Depth Bias: 0.00 DepthBiasCov: 0.72 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 5.46 2021-02-12T23:17:12Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:17:12Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:17:12Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:17:12Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:17:12Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:17:12Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:17:12Z- Filter 0 No beams used in subcloud update 2021-02-12T23:17:12Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:17:12Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:17:12Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 782.8 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:17:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171832.31 sec. 2021-02-12T23:17:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:17:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:12Z- variances([0], [2], and psi_berg) = 782.762651 833.071744 0.000000 2021-02-12T23:17:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:17:12Z- LcmTrn::publishEstimates() - MLE : 4056590.52 590665.88 28.68 2021-02-12T23:17:12Z- LcmTrn::publishEstimates() - MMSE : 4056615.02 590709.38 28.68 2021-02-12T23:17:12Z- LcmTrn::publishEstimates() - COVAR: 782.76 833.07 0.66 0.00 2021-02-12T23:17:12Z- AHRS_M2 msg: 1613171832.67 epoch sec; seqNo:2374 2021-02-12T23:17:12Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.07 psi 2021-02-12T23:17:12Z- handleDepth()-Depth_Keller msg: 1613171832672 epoch sec; seqNo:2398; depth 28.57 2021-02-12T23:17:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171832.68 epoch sec; seqNo:2398; 4056615.40 north; 590709.37 east 2021-02-12T23:17:13Z- AHRS_M2 msg: 1613171833.07 epoch sec; seqNo:2375 2021-02-12T23:17:13Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.07 psi 2021-02-12T23:17:13Z- handleDepth()-Depth_Keller msg: 1613171833078 epoch sec; seqNo:2399; depth 28.46 2021-02-12T23:17:13Z- handleDvl() - RDI_Pathfinder msg: 1613171833.09 epoch sec; seqNo:1009 2021-02-12T23:17:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.05 , 73.13 , 97.91 , 58.43 2021-02-12T23:17:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.00 , -0.05 2021-02-12T23:17:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171833.10 epoch sec; seqNo:2399; 4056615.78 north; 590709.37 east 2021-02-12T23:17:13Z- AHRS_M2 msg: 1613171833.48 epoch sec; seqNo:2376 2021-02-12T23:17:13Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.08 psi 2021-02-12T23:17:13Z- handleDepth()-Depth_Keller msg: 1613171833480 epoch sec; seqNo:2400; depth 28.33 2021-02-12T23:17:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171833.49 epoch sec; seqNo:2400; 4056615.80 north; 590710.59 east 2021-02-12T23:17:13Z- AHRS_M2 msg: 1613171833.88 epoch sec; seqNo:2377 2021-02-12T23:17:13Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.09 psi 2021-02-12T23:17:13Z- handleDepth()-Depth_Keller msg: 1613171833884 epoch sec; seqNo:2401; depth 28.21 2021-02-12T23:17:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171833.89 epoch sec; seqNo:2401; 4056616.17 north; 590710.58 east 2021-02-12T23:17:14Z- AHRS_M2 msg: 1613171834.29 epoch sec; seqNo:2378 2021-02-12T23:17:14Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.09 psi 2021-02-12T23:17:14Z- handleDepth()-Depth_Keller msg: 1613171834297 epoch sec; seqNo:2402; depth 28.09 2021-02-12T23:17:14Z- handleDvl() - RDI_Pathfinder msg: 1613171834.31 epoch sec; seqNo:1010 2021-02-12T23:17:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 74.37 , 95.75 , 59.66 2021-02-12T23:17:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , -0.05 2021-02-12T23:17:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171834.32 epoch sec; seqNo:2402; 4056616.55 north; 590710.58 east 2021-02-12T23:17:14Z- AHRS_M2 msg: 1613171834.69 epoch sec; seqNo:2379 2021-02-12T23:17:14Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.10 psi 2021-02-12T23:17:14Z- handleDepth()-Depth_Keller msg: 1613171834692 epoch sec; seqNo:2403; depth 27.96 2021-02-12T23:17:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171834.71 epoch sec; seqNo:2403; 4056616.55 north; 590710.58 east 2021-02-12T23:17:15Z- AHRS_M2 msg: 1613171835.09 epoch sec; seqNo:2380 2021-02-12T23:17:15Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.11 psi 2021-02-12T23:17:15Z- handleDepth()-Depth_Keller msg: 1613171835104 epoch sec; seqNo:2404; depth 27.83 2021-02-12T23:17:15Z- handleDvl() - RDI_Pathfinder msg: 1613171835.13 epoch sec; seqNo:1011 2021-02-12T23:17:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 78.44 , 94.67 , 58.43 2021-02-12T23:17:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.07 , -0.04 2021-02-12T23:17:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171835.14 epoch sec; seqNo:2404; 4056616.93 north; 590710.58 east 2021-02-12T23:17:15Z- AHRS_M2 msg: 1613171835.50 epoch sec; seqNo:2381 2021-02-12T23:17:15Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.11 psi 2021-02-12T23:17:15Z- handleDepth()-Depth_Keller msg: 1613171835500 epoch sec; seqNo:2405; depth 27.71 2021-02-12T23:17:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:17:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:17:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:17:15Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:17:15Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:17:15Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:17:15Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:17:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:17:15Z- handleDvl() - RDI_Pathfinder msg: 1613171835.50 epoch sec; seqNo:1012 2021-02-12T23:17:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:17:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.07 , -0.04 2021-02-12T23:17:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171835.51 epoch sec; seqNo:2405; 4056617.31 north; 590710.57 east 2021-02-12T23:17:15Z- AHRS_M2 msg: 1613171835.90 epoch sec; seqNo:2382 2021-02-12T23:17:15Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.11 psi 2021-02-12T23:17:15Z- handleDepth()-Depth_Keller msg: 1613171835904 epoch sec; seqNo:2406; depth 27.57 2021-02-12T23:17:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171835.92 epoch sec; seqNo:2406; 4056617.31 north; 590710.57 east 2021-02-12T23:17:16Z- AHRS_M2 msg: 1613171836.30 epoch sec; seqNo:2383 2021-02-12T23:17:16Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.12 psi 2021-02-12T23:17:16Z- handleDepth()-Depth_Keller msg: 1613171836317 epoch sec; seqNo:2407; depth 27.45 2021-02-12T23:17:16Z- handleDvl() - RDI_Pathfinder msg: 1613171836.33 epoch sec; seqNo:1013 2021-02-12T23:17:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.82 , 75.99 , 93.58 , 60.06 2021-02-12T23:17:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.11 , -0.02 2021-02-12T23:17:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171836.34 epoch sec; seqNo:2407; 4056617.69 north; 590710.57 east 2021-02-12T23:17:16Z- AHRS_M2 msg: 1613171836.71 epoch sec; seqNo:2384 2021-02-12T23:17:16Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.12 psi 2021-02-12T23:17:16Z- handleDepth()-Depth_Keller msg: 1613171836712 epoch sec; seqNo:2408; depth 27.33 2021-02-12T23:17:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:17:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:17:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:17:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:17:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:17:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:17:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:17:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:17:16Z- handleDvl() - RDI_Pathfinder msg: 1613171836.71 epoch sec; seqNo:1014 2021-02-12T23:17:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:17:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.11 , -0.02 2021-02-12T23:17:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171836.74 epoch sec; seqNo:2408; 4056618.08 north; 590711.79 east 2021-02-12T23:17:17Z- AHRS_M2 msg: 1613171837.12 epoch sec; seqNo:2385 2021-02-12T23:17:17Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.12 psi 2021-02-12T23:17:17Z- handleDepth()-Depth_Keller msg: 1613171837121 epoch sec; seqNo:2409; depth 27.21 2021-02-12T23:17:17Z- handleDvl() - RDI_Pathfinder msg: 1613171837.14 epoch sec; seqNo:1015 2021-02-12T23:17:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.23 , 76.40 , 95.74 , 60.06 2021-02-12T23:17:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.09 , -0.01 2021-02-12T23:17:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171837.17 epoch sec; seqNo:2409; 4056618.46 north; 590711.78 east 2021-02-12T23:17:17Z- AHRS_M2 msg: 1613171837.52 epoch sec; seqNo:2386 2021-02-12T23:17:17Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.12 psi 2021-02-12T23:17:17Z- handleDepth()-Depth_Keller msg: 1613171837522 epoch sec; seqNo:2410; depth 27.08 2021-02-12T23:17:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171837.53 epoch sec; seqNo:2410; 4056618.46 north; 590711.78 east 2021-02-12T23:17:17Z- LcmTrn::handleMessages() - handling 46 messages took 0.73 ms and 132 calls... 2021-02-12T23:17:17Z- waiting for 1613171835.31, time is 1613171837.53 2021-02-12T23:17:17Z- LcmTrn::updateTrn() >>>> pose time 1613171837.53, meas time 1613171837.14 <<<< 2021-02-12T23:17:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171837.14 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:17:17Z- TerrainNav::motionUpdate() - currEstimate = (4056618, 590711.78, 27.08) 2021-02-12T23:17:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:17:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:17:17Z- TNavBF::Projecting Measurements 2021-02-12T23:17:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:17:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:17:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:17:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:17:17Z- TNavFilter::4 good beams. 2021-02-12T23:17:17Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:17:17Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:17:17Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:17:17Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:17:17Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:17:17Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:17:17Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:17:17Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:17Z- TerrainMapOctree: (vehN, vehE ) = (4056618.20, 590711.60). 2021-02-12T23:17:17Z- TerrainMapOctree: Min Distance = 5828.71. 2021-02-12T23:17:17Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:17:17Z- There is no specific good beam that all particles have in common. 2021-02-12T23:17:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:17:17Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-12T23:17:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:17:17Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 9.34 2021-02-12T23:17:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:17:17Z- Old Depth Bias: 0.00 DepthBiasCov: 0.73 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 14.67 2021-02-12T23:17:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:17:17Z- Old Depth Bias: 0.00 DepthBiasCov: 0.70 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 5.77 2021-02-12T23:17:17Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:17:17Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:17:17Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:17:17Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:17:17Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:17:17Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:17:17Z- Filter 0 No beams used in subcloud update 2021-02-12T23:17:17Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:17:17Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:17:17Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 782.6 EastVariance: 833.1 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-12T23:17:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171837.14 sec. 2021-02-12T23:17:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:17:17Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:17Z- variances([0], [2], and psi_berg) = 782.559967 833.071438 0.000000 2021-02-12T23:17:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:17:17Z- LcmTrn::publishEstimates() - MLE : 4056593.96 590667.28 27.08 2021-02-12T23:17:17Z- LcmTrn::publishEstimates() - MMSE : 4056618.46 590711.78 27.08 2021-02-12T23:17:17Z- LcmTrn::publishEstimates() - COVAR: 782.56 833.07 0.65 0.00 2021-02-12T23:17:17Z- AHRS_M2 msg: 1613171837.92 epoch sec; seqNo:2387 2021-02-12T23:17:17Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.12 psi 2021-02-12T23:17:17Z- handleDepth()-Depth_Keller msg: 1613171837933 epoch sec; seqNo:2411; depth 26.96 2021-02-12T23:17:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:17:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:17:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:17:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:17:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:17:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:17:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:17:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:17:17Z- handleDvl() - RDI_Pathfinder msg: 1613171837.94 epoch sec; seqNo:1016 2021-02-12T23:17:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:17:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.09 , -0.01 2021-02-12T23:17:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171837.96 epoch sec; seqNo:2411; 4056618.84 north; 590711.78 east 2021-02-12T23:17:18Z- AHRS_M2 msg: 1613171838.33 epoch sec; seqNo:2388 2021-02-12T23:17:18Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.12 psi 2021-02-12T23:17:18Z- handleDepth()-Depth_Keller msg: 1613171838333 epoch sec; seqNo:2412; depth 26.85 2021-02-12T23:17:18Z- handleDvl() - RDI_Pathfinder msg: 1613171838.35 epoch sec; seqNo:1017 2021-02-12T23:17:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.23 , 75.99 , 93.58 , 60.06 2021-02-12T23:17:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , 0.01 2021-02-12T23:17:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171838.36 epoch sec; seqNo:2412; 4056619.22 north; 590711.77 east 2021-02-12T23:17:18Z- AHRS_M2 msg: 1613171838.73 epoch sec; seqNo:2389 2021-02-12T23:17:18Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.13 psi 2021-02-12T23:17:18Z- handleDepth()-Depth_Keller msg: 1613171838737 epoch sec; seqNo:2413; depth 26.72 2021-02-12T23:17:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171838.75 epoch sec; seqNo:2413; 4056619.22 north; 590711.77 east 2021-02-12T23:17:19Z- AHRS_M2 msg: 1613171839.14 epoch sec; seqNo:2390 2021-02-12T23:17:19Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.13 psi 2021-02-12T23:17:19Z- handleDepth()-Depth_Keller msg: 1613171839142 epoch sec; seqNo:2414; depth 26.63 2021-02-12T23:17:19Z- handleDvl() - RDI_Pathfinder msg: 1613171839.16 epoch sec; seqNo:1018 2021-02-12T23:17:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.82 , 77.22 , 94.66 , 60.88 2021-02-12T23:17:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , 0.03 2021-02-12T23:17:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171839.17 epoch sec; seqNo:2414; 4056619.60 north; 590711.77 east 2021-02-12T23:17:19Z- AHRS_M2 msg: 1613171839.54 epoch sec; seqNo:2391 2021-02-12T23:17:19Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.13 psi 2021-02-12T23:17:19Z- handleDepth()-Depth_Keller msg: 1613171839540 epoch sec; seqNo:2415; depth 26.48 2021-02-12T23:17:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171839.56 epoch sec; seqNo:2415; 4056619.99 north; 590712.99 east 2021-02-12T23:17:19Z- AHRS_M2 msg: 1613171839.96 epoch sec; seqNo:2392 2021-02-12T23:17:19Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.13 psi 2021-02-12T23:17:19Z- handleDepth()-Depth_Keller msg: 1613171839972 epoch sec; seqNo:2416; depth 26.37 2021-02-12T23:17:19Z- handleDvl() - RDI_Pathfinder msg: 1613171839.98 epoch sec; seqNo:1019 2021-02-12T23:17:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.82 , 77.63 , 93.58 , 61.28 2021-02-12T23:17:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.06 , 0.07 2021-02-12T23:17:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171839.99 epoch sec; seqNo:2416; 4056619.99 north; 590712.99 east 2021-02-12T23:17:20Z- AHRS_M2 msg: 1613171840.34 epoch sec; seqNo:2393 2021-02-12T23:17:20Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-12T23:17:20Z- handleDepth()-Depth_Keller msg: 1613171840349 epoch sec; seqNo:2417; depth 26.26 2021-02-12T23:17:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171840.36 epoch sec; seqNo:2417; 4056620.37 north; 590712.98 east 2021-02-12T23:17:20Z- AHRS_M2 msg: 1613171840.75 epoch sec; seqNo:2394 2021-02-12T23:17:20Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-12T23:17:20Z- handleDepth()-Depth_Keller msg: 1613171840757 epoch sec; seqNo:2418; depth 26.16 2021-02-12T23:17:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171840.77 epoch sec; seqNo:2418; 4056620.75 north; 590712.98 east 2021-02-12T23:17:21Z- AHRS_M2 msg: 1613171841.16 epoch sec; seqNo:2395 2021-02-12T23:17:21Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-12T23:17:21Z- handleDepth()-Depth_Keller msg: 1613171841173 epoch sec; seqNo:2419; depth 26.04 2021-02-12T23:17:21Z- handleDvl() - RDI_Pathfinder msg: 1613171841.18 epoch sec; seqNo:1020 2021-02-12T23:17:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.23 , 77.63 , 93.58 , 62.10 2021-02-12T23:17:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , 0.10 2021-02-12T23:17:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171841.19 epoch sec; seqNo:2419; 4056621.12 north; 590712.97 east 2021-02-12T23:17:21Z- AHRS_M2 msg: 1613171841.56 epoch sec; seqNo:2396 2021-02-12T23:17:21Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-12T23:17:21Z- handleDepth()-Depth_Keller msg: 1613171841561 epoch sec; seqNo:2420; depth 25.91 2021-02-12T23:17:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171841.59 epoch sec; seqNo:2420; 4056621.12 north; 590712.97 east 2021-02-12T23:17:21Z- AHRS_M2 msg: 1613171841.97 epoch sec; seqNo:2397 2021-02-12T23:17:21Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-12T23:17:21Z- handleDepth()-Depth_Keller msg: 1613171841978 epoch sec; seqNo:2421; depth 25.80 2021-02-12T23:17:22Z- handleDvl() - RDI_Pathfinder msg: 1613171842.00 epoch sec; seqNo:1021 2021-02-12T23:17:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.23 , 78.44 , 92.50 , 62.10 2021-02-12T23:17:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , 0.10 2021-02-12T23:17:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171842.01 epoch sec; seqNo:2421; 4056621.52 north; 590714.19 east 2021-02-12T23:17:22Z- AHRS_M2 msg: 1613171842.37 epoch sec; seqNo:2398 2021-02-12T23:17:22Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.14 psi 2021-02-12T23:17:22Z- handleDepth()-Depth_Keller msg: 1613171842376 epoch sec; seqNo:2422; depth 25.69 2021-02-12T23:17:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171842.39 epoch sec; seqNo:2422; 4056621.90 north; 590714.19 east 2021-02-12T23:17:22Z- AHRS_M2 msg: 1613171842.77 epoch sec; seqNo:2399 2021-02-12T23:17:22Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.15 psi 2021-02-12T23:17:22Z- handleDepth()-Depth_Keller msg: 1613171842772 epoch sec; seqNo:2423; depth 25.60 2021-02-12T23:17:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171842.79 epoch sec; seqNo:2423; 4056622.27 north; 590714.18 east 2021-02-12T23:17:22Z- LcmTrn::handleMessages() - handling 45 messages took 0.68 ms and 132 calls... 2021-02-12T23:17:22Z- waiting for 1613171840.53, time is 1613171842.79 2021-02-12T23:17:22Z- LcmTrn::updateTrn() >>>> pose time 1613171842.79, meas time 1613171842.00 <<<< 2021-02-12T23:17:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171842.00 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:17:22Z- TerrainNav::motionUpdate() - currEstimate = (4056622, 590714.18, 25.60) 2021-02-12T23:17:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:17:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:17:22Z- TNavBF::Projecting Measurements 2021-02-12T23:17:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:17:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:17:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:17:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:17:22Z- TNavFilter::4 good beams. 2021-02-12T23:17:22Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:17:22Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:17:22Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:17:22Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:17:22Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:17:22Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:17:22Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:17:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:22Z- TerrainMapOctree: (vehN, vehE ) = (4056621.70, 590713.82). 2021-02-12T23:17:22Z- TerrainMapOctree: Min Distance = 5824.58. 2021-02-12T23:17:22Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:17:22Z- There is no specific good beam that all particles have in common. 2021-02-12T23:17:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:17:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-12T23:17:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:17:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-12T23:17:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:17:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.73 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 13.69 2021-02-12T23:17:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:17:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.70 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 6.17 2021-02-12T23:17:22Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:17:22Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:17:22Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:17:22Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:17:22Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:17:22Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:17:22Z- Filter 0 No beams used in subcloud update 2021-02-12T23:17:22Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:17:22Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:17:22Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 782.4 EastVariance: 833.1 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-12T23:17:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171842.00 sec. 2021-02-12T23:17:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:17:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:22Z- variances([0], [2], and psi_berg) = 782.338303 833.071104 0.000000 2021-02-12T23:17:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:17:22Z- LcmTrn::publishEstimates() - MLE : 4056597.77 590669.68 25.60 2021-02-12T23:17:22Z- LcmTrn::publishEstimates() - MMSE : 4056622.27 590714.18 25.60 2021-02-12T23:17:22Z- LcmTrn::publishEstimates() - COVAR: 782.34 833.07 0.67 0.00 2021-02-12T23:17:23Z- AHRS_M2 msg: 1613171843.19 epoch sec; seqNo:2400 2021-02-12T23:17:23Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.15 psi 2021-02-12T23:17:23Z- handleDepth()-Depth_Keller msg: 1613171843211 epoch sec; seqNo:2424; depth 25.48 2021-02-12T23:17:23Z- handleDvl() - RDI_Pathfinder msg: 1613171843.22 epoch sec; seqNo:1022 2021-02-12T23:17:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.64 , 0.00 , 92.50 , 62.10 2021-02-12T23:17:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , 0.09 2021-02-12T23:17:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171843.23 epoch sec; seqNo:2424; 4056622.27 north; 590714.18 east 2021-02-12T23:17:23Z- AHRS_M2 msg: 1613171843.58 epoch sec; seqNo:2401 2021-02-12T23:17:23Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.15 psi 2021-02-12T23:17:23Z- handleDepth()-Depth_Keller msg: 1613171843582 epoch sec; seqNo:2425; depth 25.35 2021-02-12T23:17:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171843.59 epoch sec; seqNo:2425; 4056622.65 north; 590714.18 east 2021-02-12T23:17:23Z- AHRS_M2 msg: 1613171843.98 epoch sec; seqNo:2402 2021-02-12T23:17:23Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.15 psi 2021-02-12T23:17:23Z- handleDepth()-Depth_Keller msg: 1613171843988 epoch sec; seqNo:2426; depth 25.27 2021-02-12T23:17:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171844.01 epoch sec; seqNo:2426; 4056623.03 north; 590714.18 east 2021-02-12T23:17:24Z- AHRS_M2 msg: 1613171844.38 epoch sec; seqNo:2403 2021-02-12T23:17:24Z- AHRS_M2 msg: 0.66 phi; 0.30 theta; -0.15 psi 2021-02-12T23:17:24Z- handleDepth()-Depth_Keller msg: 1613171844388 epoch sec; seqNo:2427; depth 25.15 2021-02-12T23:17:24Z- handleDvl() - RDI_Pathfinder msg: 1613171844.40 epoch sec; seqNo:1023 2021-02-12T23:17:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 82.77 , 94.67 , 61.70 2021-02-12T23:17:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , 0.06 2021-02-12T23:17:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171844.43 epoch sec; seqNo:2427; 4056623.42 north; 590715.39 east 2021-02-12T23:17:24Z- AHRS_M2 msg: 1613171844.79 epoch sec; seqNo:2404 2021-02-12T23:17:24Z- AHRS_M2 msg: 0.66 phi; 0.30 theta; -0.15 psi 2021-02-12T23:17:24Z- handleDepth()-Depth_Keller msg: 1613171844817 epoch sec; seqNo:2428; depth 25.04 2021-02-12T23:17:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171844.83 epoch sec; seqNo:2428; 4056623.42 north; 590715.39 east 2021-02-12T23:17:25Z- AHRS_M2 msg: 1613171845.19 epoch sec; seqNo:2405 2021-02-12T23:17:25Z- AHRS_M2 msg: 0.66 phi; 0.30 theta; -0.15 psi 2021-02-12T23:17:25Z- handleDepth()-Depth_Keller msg: 1613171845196 epoch sec; seqNo:2429; depth 24.93 2021-02-12T23:17:25Z- handleDvl() - RDI_Pathfinder msg: 1613171845.22 epoch sec; seqNo:1024 2021-02-12T23:17:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 79.27 , 94.67 , 63.74 2021-02-12T23:17:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.13 , 0.01 2021-02-12T23:17:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171845.23 epoch sec; seqNo:2429; 4056623.80 north; 590715.39 east 2021-02-12T23:17:25Z- AHRS_M2 msg: 1613171845.60 epoch sec; seqNo:2406 2021-02-12T23:17:25Z- AHRS_M2 msg: 0.66 phi; 0.30 theta; -0.15 psi 2021-02-12T23:17:25Z- handleDepth()-Depth_Keller msg: 1613171845600 epoch sec; seqNo:2430; depth 24.84 2021-02-12T23:17:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171845.61 epoch sec; seqNo:2430; 4056623.80 north; 590715.39 east 2021-02-12T23:17:26Z- AHRS_M2 msg: 1613171846.00 epoch sec; seqNo:2407 2021-02-12T23:17:26Z- AHRS_M2 msg: 0.67 phi; 0.30 theta; -0.15 psi 2021-02-12T23:17:26Z- handleDepth()-Depth_Keller msg: 1613171846004 epoch sec; seqNo:2431; depth 24.74 2021-02-12T23:17:26Z- handleDvl() - RDI_Pathfinder msg: 1613171846.02 epoch sec; seqNo:1025 2021-02-12T23:17:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.05 , 79.67 , 93.58 , 62.10 2021-02-12T23:17:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.12 , -0.03 2021-02-12T23:17:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171846.03 epoch sec; seqNo:2431; 4056624.18 north; 590715.38 east 2021-02-12T23:17:26Z- AHRS_M2 msg: 1613171846.40 epoch sec; seqNo:2408 2021-02-12T23:17:26Z- AHRS_M2 msg: 0.67 phi; 0.29 theta; -0.16 psi 2021-02-12T23:17:26Z- handleDepth()-Depth_Keller msg: 1613171846409 epoch sec; seqNo:2432; depth 24.61 2021-02-12T23:17:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171846.42 epoch sec; seqNo:2432; 4056624.57 north; 590716.60 east 2021-02-12T23:17:26Z- AHRS_M2 msg: 1613171846.81 epoch sec; seqNo:2409 2021-02-12T23:17:26Z- AHRS_M2 msg: 0.67 phi; 0.29 theta; -0.16 psi 2021-02-12T23:17:26Z- handleDepth()-Depth_Keller msg: 1613171846821 epoch sec; seqNo:2433; depth 24.52 2021-02-12T23:17:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171846.83 epoch sec; seqNo:2433; 4056624.57 north; 590716.60 east 2021-02-12T23:17:27Z- AHRS_M2 msg: 1613171847.23 epoch sec; seqNo:2410 2021-02-12T23:17:27Z- AHRS_M2 msg: 0.67 phi; 0.29 theta; -0.16 psi 2021-02-12T23:17:27Z- handleDepth()-Depth_Keller msg: 1613171847230 epoch sec; seqNo:2434; depth 24.41 2021-02-12T23:17:27Z- handleDvl() - RDI_Pathfinder msg: 1613171847.26 epoch sec; seqNo:1026 2021-02-12T23:17:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.64 , 84.93 , 97.91 , 63.33 2021-02-12T23:17:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.09 , -0.07 2021-02-12T23:17:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171847.28 epoch sec; seqNo:2434; 4056624.95 north; 590716.60 east 2021-02-12T23:17:27Z- AHRS_M2 msg: 1613171847.62 epoch sec; seqNo:2411 2021-02-12T23:17:27Z- AHRS_M2 msg: 0.67 phi; 0.29 theta; -0.17 psi 2021-02-12T23:17:27Z- handleDepth()-Depth_Keller msg: 1613171847625 epoch sec; seqNo:2435; depth 24.29 2021-02-12T23:17:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171847.66 epoch sec; seqNo:2435; 4056625.33 north; 590716.59 east 2021-02-12T23:17:27Z- LcmTrn::handleMessages() - handling 41 messages took 0.73 ms and 127 calls... 2021-02-12T23:17:27Z- waiting for 1613171845.79, time is 1613171847.66 2021-02-12T23:17:27Z- LcmTrn::updateTrn() >>>> pose time 1613171847.66, meas time 1613171847.26 <<<< 2021-02-12T23:17:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171847.26 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:17:27Z- TerrainNav::motionUpdate() - currEstimate = (4056625, 590716.59, 24.29) 2021-02-12T23:17:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:17:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:17:27Z- TNavBF::Projecting Measurements 2021-02-12T23:17:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:17:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:17:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:17:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:17:27Z- TNavFilter::4 good beams. 2021-02-12T23:17:27Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:17:27Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:17:27Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:17:27Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:17:27Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:17:27Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:17:27Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:17:27Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:27Z- TerrainMapOctree: (vehN, vehE ) = (4056625.08, 590716.40). 2021-02-12T23:17:27Z- TerrainMapOctree: Min Distance = 5820.33. 2021-02-12T23:17:27Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:17:27Z- There is no specific good beam that all particles have in common. 2021-02-12T23:17:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:17:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 7.54 2021-02-12T23:17:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:17:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 11.54 2021-02-12T23:17:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:17:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.74 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-12T23:17:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:17:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.71 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 6.42 2021-02-12T23:17:28Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:17:28Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:17:28Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:17:28Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:17:28Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:17:28Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:17:28Z- Filter 0 No beams used in subcloud update 2021-02-12T23:17:28Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:17:28Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:17:28Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 782.2 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:17:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171847.26 sec. 2021-02-12T23:17:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:17:28Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:28Z- variances([0], [2], and psi_berg) = 782.143440 833.070812 0.000000 2021-02-12T23:17:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:17:28Z- LcmTrn::publishEstimates() - MLE : 4056600.83 590672.09 24.29 2021-02-12T23:17:28Z- LcmTrn::publishEstimates() - MMSE : 4056625.33 590716.59 24.29 2021-02-12T23:17:28Z- LcmTrn::publishEstimates() - COVAR: 782.14 833.07 0.67 0.00 2021-02-12T23:17:28Z- AHRS_M2 msg: 1613171848.04 epoch sec; seqNo:2412 2021-02-12T23:17:28Z- AHRS_M2 msg: 0.68 phi; 0.29 theta; -0.17 psi 2021-02-12T23:17:28Z- handleDepth()-Depth_Keller msg: 1613171848047 epoch sec; seqNo:2436; depth 24.19 2021-02-12T23:17:28Z- handleDvl() - RDI_Pathfinder msg: 1613171848.07 epoch sec; seqNo:1027 2021-02-12T23:17:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.64 , 83.85 , 92.50 , 62.92 2021-02-12T23:17:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.04 , -0.08 2021-02-12T23:17:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171848.08 epoch sec; seqNo:2436; 4056625.33 north; 590716.59 east 2021-02-12T23:17:28Z- AHRS_M2 msg: 1613171848.43 epoch sec; seqNo:2413 2021-02-12T23:17:28Z- AHRS_M2 msg: 0.68 phi; 0.29 theta; -0.17 psi 2021-02-12T23:17:28Z- handleDepth()-Depth_Keller msg: 1613171848435 epoch sec; seqNo:2437; depth 24.09 2021-02-12T23:17:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171848.45 epoch sec; seqNo:2437; 4056625.72 north; 590717.81 east 2021-02-12T23:17:28Z- AHRS_M2 msg: 1613171848.83 epoch sec; seqNo:2414 2021-02-12T23:17:28Z- AHRS_M2 msg: 0.68 phi; 0.28 theta; -0.17 psi 2021-02-12T23:17:28Z- handleDepth()-Depth_Keller msg: 1613171848835 epoch sec; seqNo:2438; depth 23.99 2021-02-12T23:17:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171848.85 epoch sec; seqNo:2438; 4056625.72 north; 590717.81 east 2021-02-12T23:17:29Z- AHRS_M2 msg: 1613171849.23 epoch sec; seqNo:2415 2021-02-12T23:17:29Z- AHRS_M2 msg: 0.67 phi; 0.28 theta; -0.17 psi 2021-02-12T23:17:29Z- handleDepth()-Depth_Keller msg: 1613171849237 epoch sec; seqNo:2439; depth 23.89 2021-02-12T23:17:29Z- handleDvl() - RDI_Pathfinder msg: 1613171849.25 epoch sec; seqNo:1028 2021-02-12T23:17:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.23 , 87.09 , 94.66 , 62.92 2021-02-12T23:17:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.02 , -0.09 2021-02-12T23:17:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171849.27 epoch sec; seqNo:2439; 4056626.10 north; 590717.81 east 2021-02-12T23:17:29Z- AHRS_M2 msg: 1613171849.64 epoch sec; seqNo:2416 2021-02-12T23:17:29Z- AHRS_M2 msg: 0.67 phi; 0.28 theta; -0.17 psi 2021-02-12T23:17:29Z- handleDepth()-Depth_Keller msg: 1613171849640 epoch sec; seqNo:2440; depth 23.78 2021-02-12T23:17:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171849.65 epoch sec; seqNo:2440; 4056626.48 north; 590717.80 east 2021-02-12T23:17:30Z- AHRS_M2 msg: 1613171850.05 epoch sec; seqNo:2417 2021-02-12T23:17:30Z- AHRS_M2 msg: 0.67 phi; 0.28 theta; -0.17 psi 2021-02-12T23:17:30Z- handleDepth()-Depth_Keller msg: 1613171850054 epoch sec; seqNo:2441; depth 23.67 2021-02-12T23:17:30Z- handleDvl() - RDI_Pathfinder msg: 1613171850.07 epoch sec; seqNo:1029 2021-02-12T23:17:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 84.93 , 92.50 , 64.15 2021-02-12T23:17:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , -0.05 2021-02-12T23:17:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171850.08 epoch sec; seqNo:2441; 4056626.48 north; 590717.80 east 2021-02-12T23:17:30Z- AHRS_M2 msg: 1613171850.44 epoch sec; seqNo:2418 2021-02-12T23:17:30Z- AHRS_M2 msg: 0.67 phi; 0.28 theta; -0.17 psi 2021-02-12T23:17:30Z- handleDepth()-Depth_Keller msg: 1613171850448 epoch sec; seqNo:2442; depth 23.57 2021-02-12T23:17:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171850.47 epoch sec; seqNo:2442; 4056626.86 north; 590717.80 east 2021-02-12T23:17:30Z- AHRS_M2 msg: 1613171850.85 epoch sec; seqNo:2419 2021-02-12T23:17:30Z- AHRS_M2 msg: 0.66 phi; 0.28 theta; -0.17 psi 2021-02-12T23:17:30Z- handleDepth()-Depth_Keller msg: 1613171850853 epoch sec; seqNo:2443; depth 23.48 2021-02-12T23:17:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171850.86 epoch sec; seqNo:2443; 4056627.24 north; 590717.79 east 2021-02-12T23:17:31Z- AHRS_M2 msg: 1613171851.25 epoch sec; seqNo:2420 2021-02-12T23:17:31Z- AHRS_M2 msg: 0.66 phi; 0.28 theta; -0.17 psi 2021-02-12T23:17:31Z- handleDepth()-Depth_Keller msg: 1613171851257 epoch sec; seqNo:2444; depth 23.37 2021-02-12T23:17:31Z- handleDvl() - RDI_Pathfinder msg: 1613171851.27 epoch sec; seqNo:1030 2021-02-12T23:17:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 84.93 , 92.50 , 64.15 2021-02-12T23:17:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.08 , -0.03 2021-02-12T23:17:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171851.28 epoch sec; seqNo:2444; 4056627.25 north; 590719.02 east 2021-02-12T23:17:31Z- AHRS_M2 msg: 1613171851.65 epoch sec; seqNo:2421 2021-02-12T23:17:31Z- AHRS_M2 msg: 0.66 phi; 0.28 theta; -0.17 psi 2021-02-12T23:17:31Z- handleDepth()-Depth_Keller msg: 1613171851665 epoch sec; seqNo:2445; depth 23.27 2021-02-12T23:17:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171851.68 epoch sec; seqNo:2445; 4056627.63 north; 590719.01 east 2021-02-12T23:17:32Z- AHRS_M2 msg: 1613171852.06 epoch sec; seqNo:2422 2021-02-12T23:17:32Z- AHRS_M2 msg: 0.66 phi; 0.28 theta; -0.17 psi 2021-02-12T23:17:32Z- handleDepth()-Depth_Keller msg: 1613171852064 epoch sec; seqNo:2446; depth 23.17 2021-02-12T23:17:32Z- handleDvl() - RDI_Pathfinder msg: 1613171852.08 epoch sec; seqNo:1031 2021-02-12T23:17:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 86.01 , 94.67 , 63.74 2021-02-12T23:17:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.08 , -0.01 2021-02-12T23:17:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171852.10 epoch sec; seqNo:2446; 4056628.01 north; 590719.01 east 2021-02-12T23:17:32Z- AHRS_M2 msg: 1613171852.46 epoch sec; seqNo:2423 2021-02-12T23:17:32Z- AHRS_M2 msg: 0.66 phi; 0.28 theta; -0.17 psi 2021-02-12T23:17:32Z- handleDepth()-Depth_Keller msg: 1613171852470 epoch sec; seqNo:2447; depth 23.07 2021-02-12T23:17:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171852.48 epoch sec; seqNo:2447; 4056628.01 north; 590719.01 east 2021-02-12T23:17:32Z- AHRS_M2 msg: 1613171852.87 epoch sec; seqNo:2424 2021-02-12T23:17:32Z- AHRS_M2 msg: 0.65 phi; 0.28 theta; -0.18 psi 2021-02-12T23:17:32Z- handleDepth()-Depth_Keller msg: 1613171852881 epoch sec; seqNo:2448; depth 22.98 2021-02-12T23:17:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171852.89 epoch sec; seqNo:2448; 4056628.39 north; 590719.00 east 2021-02-12T23:17:33Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-12T23:17:33Z- waiting for 1613171850.66, time is 1613171852.89 2021-02-12T23:17:33Z- LcmTrn::updateTrn() >>>> pose time 1613171852.89, meas time 1613171852.08 <<<< 2021-02-12T23:17:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171852.08 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:17:33Z- TerrainNav::motionUpdate() - currEstimate = (4056628, 590719.00, 22.98) 2021-02-12T23:17:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:17:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:17:33Z- TNavBF::Projecting Measurements 2021-02-12T23:17:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:17:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:17:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:17:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:17:33Z- TNavFilter::4 good beams. 2021-02-12T23:17:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:17:33Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:17:33Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:17:33Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:17:33Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:17:33Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:17:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:17:33Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:33Z- TerrainMapOctree: (vehN, vehE ) = (4056627.91, 590718.63). 2021-02-12T23:17:33Z- TerrainMapOctree: Min Distance = 5816.72. 2021-02-12T23:17:33Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:17:33Z- There is no specific good beam that all particles have in common. 2021-02-12T23:17:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:17:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-12T23:17:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:17:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.77 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 11.84 2021-02-12T23:17:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:17:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.73 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 14.34 2021-02-12T23:17:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:17:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.70 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 6.50 2021-02-12T23:17:33Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:17:33Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:17:33Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:17:33Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:17:33Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:17:33Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:17:33Z- Filter 0 No beams used in subcloud update 2021-02-12T23:17:33Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:17:33Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:17:33Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 782.0 EastVariance: 833.1 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-12T23:17:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171852.08 sec. 2021-02-12T23:17:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:17:33Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:33Z- variances([0], [2], and psi_berg) = 781.938410 833.070505 0.000000 2021-02-12T23:17:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:17:33Z- LcmTrn::publishEstimates() - MLE : 4056603.89 590674.50 22.98 2021-02-12T23:17:33Z- LcmTrn::publishEstimates() - MMSE : 4056628.39 590719.00 22.98 2021-02-12T23:17:33Z- LcmTrn::publishEstimates() - COVAR: 781.94 833.07 0.67 0.00 2021-02-12T23:17:33Z- AHRS_M2 msg: 1613171853.27 epoch sec; seqNo:2425 2021-02-12T23:17:33Z- AHRS_M2 msg: 0.65 phi; 0.28 theta; -0.18 psi 2021-02-12T23:17:33Z- handleDepth()-Depth_Keller msg: 1613171853280 epoch sec; seqNo:2449; depth 22.87 2021-02-12T23:17:33Z- handleDvl() - RDI_Pathfinder msg: 1613171853.30 epoch sec; seqNo:1032 2021-02-12T23:17:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 0.00 , 94.67 , 63.74 2021-02-12T23:17:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.07 , 0.02 2021-02-12T23:17:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171853.31 epoch sec; seqNo:2449; 4056628.77 north; 590719.00 east 2021-02-12T23:17:33Z- AHRS_M2 msg: 1613171853.68 epoch sec; seqNo:2426 2021-02-12T23:17:33Z- AHRS_M2 msg: 0.64 phi; 0.28 theta; -0.18 psi 2021-02-12T23:17:33Z- handleDepth()-Depth_Keller msg: 1613171853680 epoch sec; seqNo:2450; depth 22.78 2021-02-12T23:17:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171853.69 epoch sec; seqNo:2450; 4056629.15 north; 590719.00 east 2021-02-12T23:17:34Z- AHRS_M2 msg: 1613171854.08 epoch sec; seqNo:2427 2021-02-12T23:17:34Z- AHRS_M2 msg: 0.64 phi; 0.28 theta; -0.18 psi 2021-02-12T23:17:34Z- handleDepth()-Depth_Keller msg: 1613171854084 epoch sec; seqNo:2451; depth 22.67 2021-02-12T23:17:34Z- handleDvl() - RDI_Pathfinder msg: 1613171854.10 epoch sec; seqNo:1033 2021-02-12T23:17:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.46 , 86.01 , 94.66 , 64.14 2021-02-12T23:17:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , 0.07 2021-02-12T23:17:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171854.11 epoch sec; seqNo:2451; 4056629.15 north; 590719.00 east 2021-02-12T23:17:34Z- AHRS_M2 msg: 1613171854.48 epoch sec; seqNo:2428 2021-02-12T23:17:34Z- AHRS_M2 msg: 0.63 phi; 0.28 theta; -0.19 psi 2021-02-12T23:17:34Z- handleDepth()-Depth_Keller msg: 1613171854501 epoch sec; seqNo:2452; depth 22.58 2021-02-12T23:17:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171854.51 epoch sec; seqNo:2452; 4056629.54 north; 590720.21 east 2021-02-12T23:17:34Z- AHRS_M2 msg: 1613171854.89 epoch sec; seqNo:2429 2021-02-12T23:17:34Z- AHRS_M2 msg: 0.63 phi; 0.28 theta; -0.19 psi 2021-02-12T23:17:34Z- handleDepth()-Depth_Keller msg: 1613171854892 epoch sec; seqNo:2453; depth 22.48 2021-02-12T23:17:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171854.91 epoch sec; seqNo:2453; 4056629.92 north; 590720.21 east 2021-02-12T23:17:35Z- AHRS_M2 msg: 1613171855.29 epoch sec; seqNo:2430 2021-02-12T23:17:35Z- AHRS_M2 msg: 0.62 phi; 0.28 theta; -0.19 psi 2021-02-12T23:17:35Z- handleDepth()-Depth_Keller msg: 1613171855296 epoch sec; seqNo:2454; depth 22.39 2021-02-12T23:17:35Z- handleDvl() - RDI_Pathfinder msg: 1613171855.31 epoch sec; seqNo:1034 2021-02-12T23:17:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.46 , 87.09 , 94.66 , 65.37 2021-02-12T23:17:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.06 , 0.12 2021-02-12T23:17:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171855.33 epoch sec; seqNo:2454; 4056630.29 north; 590720.20 east 2021-02-12T23:17:35Z- AHRS_M2 msg: 1613171855.70 epoch sec; seqNo:2431 2021-02-12T23:17:35Z- AHRS_M2 msg: 0.62 phi; 0.28 theta; -0.19 psi 2021-02-12T23:17:35Z- handleDepth()-Depth_Keller msg: 1613171855705 epoch sec; seqNo:2455; depth 22.27 2021-02-12T23:17:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171855.72 epoch sec; seqNo:2455; 4056630.29 north; 590720.20 east 2021-02-12T23:17:36Z- AHRS_M2 msg: 1613171856.10 epoch sec; seqNo:2432 2021-02-12T23:17:36Z- AHRS_M2 msg: 0.61 phi; 0.28 theta; -0.20 psi 2021-02-12T23:17:36Z- handleDepth()-Depth_Keller msg: 1613171856106 epoch sec; seqNo:2456; depth 22.18 2021-02-12T23:17:36Z- handleDvl() - RDI_Pathfinder msg: 1613171856.12 epoch sec; seqNo:1035 2021-02-12T23:17:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.86 , 88.17 , 96.82 , 0.00 2021-02-12T23:17:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , 0.16 2021-02-12T23:17:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171856.13 epoch sec; seqNo:2456; 4056630.67 north; 590720.20 east 2021-02-12T23:17:36Z- AHRS_M2 msg: 1613171856.50 epoch sec; seqNo:2433 2021-02-12T23:17:36Z- AHRS_M2 msg: 0.61 phi; 0.29 theta; -0.19 psi 2021-02-12T23:17:36Z- handleDepth()-Depth_Keller msg: 1613171856508 epoch sec; seqNo:2457; depth 22.10 2021-02-12T23:17:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171856.53 epoch sec; seqNo:2457; 4056631.05 north; 590720.20 east 2021-02-12T23:17:36Z- AHRS_M2 msg: 1613171856.91 epoch sec; seqNo:2434 2021-02-12T23:17:36Z- AHRS_M2 msg: 0.61 phi; 0.29 theta; -0.19 psi 2021-02-12T23:17:36Z- handleDepth()-Depth_Keller msg: 1613171856917 epoch sec; seqNo:2458; depth 22.00 2021-02-12T23:17:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171856.92 epoch sec; seqNo:2458; 4056631.44 north; 590721.41 east 2021-02-12T23:17:37Z- AHRS_M2 msg: 1613171857.31 epoch sec; seqNo:2435 2021-02-12T23:17:37Z- AHRS_M2 msg: 0.61 phi; 0.29 theta; -0.18 psi 2021-02-12T23:17:37Z- handleDepth()-Depth_Keller msg: 1613171857316 epoch sec; seqNo:2459; depth 21.91 2021-02-12T23:17:37Z- handleDvl() - RDI_Pathfinder msg: 1613171857.33 epoch sec; seqNo:1036 2021-02-12T23:17:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.27 , 88.44 , 95.74 , 66.60 2021-02-12T23:17:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.09 , 0.13 2021-02-12T23:17:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171857.34 epoch sec; seqNo:2459; 4056631.44 north; 590721.41 east 2021-02-12T23:17:37Z- AHRS_M2 msg: 1613171857.72 epoch sec; seqNo:2436 2021-02-12T23:17:37Z- AHRS_M2 msg: 0.61 phi; 0.29 theta; -0.18 psi 2021-02-12T23:17:37Z- handleDepth()-Depth_Keller msg: 1613171857721 epoch sec; seqNo:2460; depth 21.81 2021-02-12T23:17:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171857.73 epoch sec; seqNo:2460; 4056631.82 north; 590721.41 east 2021-02-12T23:17:38Z- AHRS_M2 msg: 1613171858.12 epoch sec; seqNo:2437 2021-02-12T23:17:38Z- AHRS_M2 msg: 0.61 phi; 0.29 theta; -0.18 psi 2021-02-12T23:17:38Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-12T23:17:38Z- waiting for 1613171855.89, time is 1613171858.12 2021-02-12T23:17:38Z- LcmTrn::updateTrn() >>>> pose time 1613171858.12, meas time 1613171857.33 <<<< 2021-02-12T23:17:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171857.33 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:17:38Z- TerrainNav::motionUpdate() - currEstimate = (4056632, 590721.41, 21.81) 2021-02-12T23:17:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:17:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:17:38Z- TNavBF::Projecting Measurements 2021-02-12T23:17:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:17:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:17:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:17:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:17:38Z- TNavFilter::4 good beams. 2021-02-12T23:17:38Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:17:38Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:17:38Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:17:38Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:17:38Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:17:38Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:17:38Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:17:38Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:38Z- TerrainMapOctree: (vehN, vehE ) = (4056631.30, 590721.04). 2021-02-12T23:17:38Z- TerrainMapOctree: Min Distance = 5812.56. 2021-02-12T23:17:38Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:17:38Z- There is no specific good beam that all particles have in common. 2021-02-12T23:17:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:17:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 7.90 2021-02-12T23:17:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:17:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 12.52 2021-02-12T23:17:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:17:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.74 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 14.67 2021-02-12T23:17:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:17:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.71 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 7.10 2021-02-12T23:17:38Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:17:38Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:17:38Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:17:38Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:17:38Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:17:38Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:17:38Z- Filter 0 No beams used in subcloud update 2021-02-12T23:17:38Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:17:38Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:17:38Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 781.8 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:17:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171857.33 sec. 2021-02-12T23:17:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:17:38Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:38Z- variances([0], [2], and psi_berg) = 781.726798 833.070190 0.000000 2021-02-12T23:17:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:17:38Z- LcmTrn::publishEstimates() - MLE : 4056607.32 590770.91 21.81 2021-02-12T23:17:38Z- LcmTrn::publishEstimates() - MMSE : 4056631.82 590721.41 21.81 2021-02-12T23:17:38Z- LcmTrn::publishEstimates() - COVAR: 781.73 833.07 0.69 0.00 2021-02-12T23:17:38Z- handleDepth()-Depth_Keller msg: 1613171858128 epoch sec; seqNo:2461; depth 21.72 2021-02-12T23:17:38Z- handleDvl() - RDI_Pathfinder msg: 1613171858.15 epoch sec; seqNo:1037 2021-02-12T23:17:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.10 , 88.18 , 97.91 , 66.19 2021-02-12T23:17:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.14 , 0.08 2021-02-12T23:17:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171858.16 epoch sec; seqNo:2461; 4056632.20 north; 590721.41 east 2021-02-12T23:17:38Z- AHRS_M2 msg: 1613171858.52 epoch sec; seqNo:2438 2021-02-12T23:17:38Z- AHRS_M2 msg: 0.61 phi; 0.29 theta; -0.18 psi 2021-02-12T23:17:38Z- handleDepth()-Depth_Keller msg: 1613171858529 epoch sec; seqNo:2462; depth 21.61 2021-02-12T23:17:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171858.54 epoch sec; seqNo:2462; 4056632.20 north; 590721.41 east 2021-02-12T23:17:38Z- AHRS_M2 msg: 1613171858.93 epoch sec; seqNo:2439 2021-02-12T23:17:38Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.18 psi 2021-02-12T23:17:38Z- handleDepth()-Depth_Keller msg: 1613171858940 epoch sec; seqNo:2463; depth 21.52 2021-02-12T23:17:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171858.95 epoch sec; seqNo:2463; 4056632.59 north; 590722.62 east 2021-02-12T23:17:39Z- AHRS_M2 msg: 1613171859.33 epoch sec; seqNo:2440 2021-02-12T23:17:39Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.18 psi 2021-02-12T23:17:39Z- handleDepth()-Depth_Keller msg: 1613171859341 epoch sec; seqNo:2464; depth 21.42 2021-02-12T23:17:39Z- handleDvl() - RDI_Pathfinder msg: 1613171859.36 epoch sec; seqNo:1038 2021-02-12T23:17:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 89.19 , 96.76 , 65.79 2021-02-12T23:17:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.17 , 0.01 2021-02-12T23:17:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171859.37 epoch sec; seqNo:2464; 4056632.97 north; 590722.62 east 2021-02-12T23:17:39Z- AHRS_M2 msg: 1613171859.74 epoch sec; seqNo:2441 2021-02-12T23:17:39Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.18 psi 2021-02-12T23:17:39Z- handleDepth()-Depth_Keller msg: 1613171859740 epoch sec; seqNo:2465; depth 21.31 2021-02-12T23:17:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171859.75 epoch sec; seqNo:2465; 4056632.97 north; 590722.62 east 2021-02-12T23:17:40Z- AHRS_M2 msg: 1613171860.14 epoch sec; seqNo:2442 2021-02-12T23:17:40Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.18 psi 2021-02-12T23:17:40Z- handleDepth()-Depth_Keller msg: 1613171860147 epoch sec; seqNo:2466; depth 21.22 2021-02-12T23:17:40Z- handleDvl() - RDI_Pathfinder msg: 1613171860.16 epoch sec; seqNo:1039 2021-02-12T23:17:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 90.27 , 96.76 , 66.19 2021-02-12T23:17:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.12 , -0.06 2021-02-12T23:17:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171860.17 epoch sec; seqNo:2466; 4056633.35 north; 590722.61 east 2021-02-12T23:17:40Z- AHRS_M2 msg: 1613171860.54 epoch sec; seqNo:2443 2021-02-12T23:17:40Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.19 psi 2021-02-12T23:17:40Z- handleDepth()-Depth_Keller msg: 1613171860557 epoch sec; seqNo:2467; depth 21.12 2021-02-12T23:17:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171860.57 epoch sec; seqNo:2467; 4056633.73 north; 590722.61 east 2021-02-12T23:17:40Z- AHRS_M2 msg: 1613171860.96 epoch sec; seqNo:2444 2021-02-12T23:17:40Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.19 psi 2021-02-12T23:17:40Z- handleDepth()-Depth_Keller msg: 1613171860969 epoch sec; seqNo:2468; depth 21.02 2021-02-12T23:17:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171860.99 epoch sec; seqNo:2468; 4056633.74 north; 590723.83 east 2021-02-12T23:17:41Z- AHRS_M2 msg: 1613171861.35 epoch sec; seqNo:2445 2021-02-12T23:17:41Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.19 psi 2021-02-12T23:17:41Z- handleDepth()-Depth_Keller msg: 1613171861361 epoch sec; seqNo:2469; depth 20.90 2021-02-12T23:17:41Z- handleDvl() - RDI_Pathfinder msg: 1613171861.37 epoch sec; seqNo:1040 2021-02-12T23:17:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 92.44 , 96.76 , 67.42 2021-02-12T23:17:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.07 , -0.10 2021-02-12T23:17:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171861.39 epoch sec; seqNo:2469; 4056634.12 north; 590723.83 east 2021-02-12T23:17:41Z- AHRS_M2 msg: 1613171861.76 epoch sec; seqNo:2446 2021-02-12T23:17:41Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.19 psi 2021-02-12T23:17:41Z- handleDepth()-Depth_Keller msg: 1613171861760 epoch sec; seqNo:2470; depth 20.81 2021-02-12T23:17:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171861.77 epoch sec; seqNo:2470; 4056634.12 north; 590723.83 east 2021-02-12T23:17:42Z- AHRS_M2 msg: 1613171862.16 epoch sec; seqNo:2447 2021-02-12T23:17:42Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.19 psi 2021-02-12T23:17:42Z- handleDepth()-Depth_Keller msg: 1613171862170 epoch sec; seqNo:2471; depth 20.70 2021-02-12T23:17:42Z- handleDvl() - RDI_Pathfinder msg: 1613171862.18 epoch sec; seqNo:1041 2021-02-12T23:17:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.68 , 92.36 , 96.69 , 67.41 2021-02-12T23:17:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.00 , -0.11 2021-02-12T23:17:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171862.19 epoch sec; seqNo:2471; 4056634.50 north; 590723.82 east 2021-02-12T23:17:42Z- AHRS_M2 msg: 1613171862.56 epoch sec; seqNo:2448 2021-02-12T23:17:42Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.20 psi 2021-02-12T23:17:42Z- handleDepth()-Depth_Keller msg: 1613171862582 epoch sec; seqNo:2472; depth 20.61 2021-02-12T23:17:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171862.59 epoch sec; seqNo:2472; 4056634.88 north; 590723.82 east 2021-02-12T23:17:42Z- AHRS_M2 msg: 1613171862.97 epoch sec; seqNo:2449 2021-02-12T23:17:42Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.20 psi 2021-02-12T23:17:42Z- handleDepth()-Depth_Keller msg: 1613171862977 epoch sec; seqNo:2473; depth 20.49 2021-02-12T23:17:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171862.98 epoch sec; seqNo:2473; 4056634.88 north; 590723.82 east 2021-02-12T23:17:43Z- LcmTrn::handleMessages() - handling 43 messages took 0.62 ms and 131 calls... 2021-02-12T23:17:43Z- waiting for 1613171861.12, time is 1613171862.98 2021-02-12T23:17:43Z- LcmTrn::updateTrn() >>>> pose time 1613171862.98, meas time 1613171862.18 <<<< 2021-02-12T23:17:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171862.18 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:17:43Z- TerrainNav::motionUpdate() - currEstimate = (4056635, 590723.82, 20.49) 2021-02-12T23:17:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:17:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:17:43Z- TNavBF::Projecting Measurements 2021-02-12T23:17:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:17:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:17:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:17:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:17:43Z- TNavFilter::4 good beams. 2021-02-12T23:17:43Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:17:43Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:17:43Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:17:43Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:17:43Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:17:43Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:17:43Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:17:43Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:43Z- TerrainMapOctree: (vehN, vehE ) = (4056634.37, 590723.42). 2021-02-12T23:17:43Z- TerrainMapOctree: Min Distance = 5808.68. 2021-02-12T23:17:43Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:17:43Z- There is no specific good beam that all particles have in common. 2021-02-12T23:17:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:17:43Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 7.99 2021-02-12T23:17:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:17:43Z- Old Depth Bias: 0.00 DepthBiasCov: 0.79 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 13.65 2021-02-12T23:17:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:17:43Z- Old Depth Bias: 0.00 DepthBiasCov: 0.75 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 14.96 2021-02-12T23:17:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:17:43Z- Old Depth Bias: 0.00 DepthBiasCov: 0.72 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 7.27 2021-02-12T23:17:43Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:17:43Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:17:43Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:17:43Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:17:43Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:17:43Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:17:43Z- Filter 0 No beams used in subcloud update 2021-02-12T23:17:43Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:17:43Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:17:43Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 781.6 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:17:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171862.18 sec. 2021-02-12T23:17:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:17:43Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:43Z- variances([0], [2], and psi_berg) = 781.522221 833.069887 0.000000 2021-02-12T23:17:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:17:43Z- LcmTrn::publishEstimates() - MLE : 4056610.38 590773.32 20.49 2021-02-12T23:17:43Z- LcmTrn::publishEstimates() - MMSE : 4056634.88 590723.82 20.49 2021-02-12T23:17:43Z- LcmTrn::publishEstimates() - COVAR: 781.52 833.07 0.70 0.00 2021-02-12T23:17:43Z- AHRS_M2 msg: 1613171863.38 epoch sec; seqNo:2450 2021-02-12T23:17:43Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.20 psi 2021-02-12T23:17:43Z- handleDepth()-Depth_Keller msg: 1613171863386 epoch sec; seqNo:2474; depth 20.39 2021-02-12T23:17:43Z- handleDvl() - RDI_Pathfinder msg: 1613171863.41 epoch sec; seqNo:1042 2021-02-12T23:17:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:17:43Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:17:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171863.45 epoch sec; seqNo:2474; 4056635.27 north; 590725.04 east 2021-02-12T23:17:43Z- AHRS_M2 msg: 1613171863.78 epoch sec; seqNo:2451 2021-02-12T23:17:43Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.20 psi 2021-02-12T23:17:43Z- handleDepth()-Depth_Keller msg: 1613171863780 epoch sec; seqNo:2475; depth 20.27 2021-02-12T23:17:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171863.80 epoch sec; seqNo:2475; 4056635.65 north; 590725.03 east 2021-02-12T23:17:44Z- AHRS_M2 msg: 1613171864.18 epoch sec; seqNo:2452 2021-02-12T23:17:44Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.20 psi 2021-02-12T23:17:44Z- handleDepth()-Depth_Keller msg: 1613171864184 epoch sec; seqNo:2476; depth 20.15 2021-02-12T23:17:44Z- handleDvl() - RDI_Pathfinder msg: 1613171864.20 epoch sec; seqNo:1043 2021-02-12T23:17:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.68 , 91.76 , 102.57 , 66.60 2021-02-12T23:17:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.10 , -0.06 2021-02-12T23:17:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171864.22 epoch sec; seqNo:2476; 4056636.03 north; 590725.03 east 2021-02-12T23:17:44Z- AHRS_M2 msg: 1613171864.59 epoch sec; seqNo:2453 2021-02-12T23:17:44Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.20 psi 2021-02-12T23:17:44Z- handleDepth()-Depth_Keller msg: 1613171864591 epoch sec; seqNo:2477; depth 20.07 2021-02-12T23:17:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171864.60 epoch sec; seqNo:2477; 4056636.03 north; 590725.03 east 2021-02-12T23:17:45Z- AHRS_M2 msg: 1613171865.02 epoch sec; seqNo:2454 2021-02-12T23:17:45Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.19 psi 2021-02-12T23:17:45Z- handleDepth()-Depth_Keller msg: 1613171865021 epoch sec; seqNo:2478; depth 19.96 2021-02-12T23:17:45Z- handleDvl() - RDI_Pathfinder msg: 1613171865.04 epoch sec; seqNo:1044 2021-02-12T23:17:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.50 , 89.86 , 97.16 , 67.00 2021-02-12T23:17:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.11 , -0.01 2021-02-12T23:17:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171865.05 epoch sec; seqNo:2478; 4056636.41 north; 590725.02 east 2021-02-12T23:17:45Z- handleDepth()-Depth_Keller msg: 1613171865413 epoch sec; seqNo:2479; depth 19.84 2021-02-12T23:17:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171865.42 epoch sec; seqNo:2479; 4056636.79 north; 590725.02 east 2021-02-12T23:17:45Z- AHRS_M2 msg: 1613171865.80 epoch sec; seqNo:2455 2021-02-12T23:17:45Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.19 psi 2021-02-12T23:17:45Z- handleDepth()-Depth_Keller msg: 1613171865807 epoch sec; seqNo:2480; depth 19.74 2021-02-12T23:17:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171865.82 epoch sec; seqNo:2480; 4056636.79 north; 590725.02 east 2021-02-12T23:17:46Z- AHRS_M2 msg: 1613171866.20 epoch sec; seqNo:2456 2021-02-12T23:17:46Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.18 psi 2021-02-12T23:17:46Z- handleDepth()-Depth_Keller msg: 1613171866209 epoch sec; seqNo:2481; depth 19.63 2021-02-12T23:17:46Z- handleDvl() - RDI_Pathfinder msg: 1613171866.22 epoch sec; seqNo:1045 2021-02-12T23:17:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.32 , 90.27 , 96.76 , 66.60 2021-02-12T23:17:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.12 , 0.03 2021-02-12T23:17:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171866.25 epoch sec; seqNo:2481; 4056637.17 north; 590725.02 east 2021-02-12T23:17:46Z- AHRS_M2 msg: 1613171866.60 epoch sec; seqNo:2457 2021-02-12T23:17:46Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.18 psi 2021-02-12T23:17:46Z- handleDepth()-Depth_Keller msg: 1613171866611 epoch sec; seqNo:2482; depth 19.52 2021-02-12T23:17:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171866.62 epoch sec; seqNo:2482; 4056637.54 north; 590725.01 east 2021-02-12T23:17:47Z- AHRS_M2 msg: 1613171867.01 epoch sec; seqNo:2458 2021-02-12T23:17:47Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.18 psi 2021-02-12T23:17:47Z- handleDepth()-Depth_Keller msg: 1613171867012 epoch sec; seqNo:2483; depth 19.43 2021-02-12T23:17:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171867.03 epoch sec; seqNo:2483; 4056637.92 north; 590725.01 east 2021-02-12T23:17:47Z- AHRS_M2 msg: 1613171867.41 epoch sec; seqNo:2459 2021-02-12T23:17:47Z- AHRS_M2 msg: 0.56 phi; 0.32 theta; -0.18 psi 2021-02-12T23:17:47Z- handleDepth()-Depth_Keller msg: 1613171867421 epoch sec; seqNo:2484; depth 19.31 2021-02-12T23:17:47Z- handleDvl() - RDI_Pathfinder msg: 1613171867.43 epoch sec; seqNo:1046 2021-02-12T23:17:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 97.84 , 103.24 , 67.00 2021-02-12T23:17:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.09 , 0.06 2021-02-12T23:17:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171867.45 epoch sec; seqNo:2484; 4056637.94 north; 590726.23 east 2021-02-12T23:17:47Z- AHRS_M2 msg: 1613171867.82 epoch sec; seqNo:2460 2021-02-12T23:17:47Z- AHRS_M2 msg: 0.56 phi; 0.32 theta; -0.18 psi 2021-02-12T23:17:47Z- handleDepth()-Depth_Keller msg: 1613171867820 epoch sec; seqNo:2485; depth 19.19 2021-02-12T23:17:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171867.83 epoch sec; seqNo:2485; 4056638.31 north; 590726.23 east 2021-02-12T23:17:48Z- AHRS_M2 msg: 1613171868.22 epoch sec; seqNo:2461 2021-02-12T23:17:48Z- AHRS_M2 msg: 0.56 phi; 0.33 theta; -0.18 psi 2021-02-12T23:17:48Z- handleDepth()-Depth_Keller msg: 1613171868241 epoch sec; seqNo:2486; depth 19.10 2021-02-12T23:17:48Z- handleDvl() - RDI_Pathfinder msg: 1613171868.26 epoch sec; seqNo:1047 2021-02-12T23:17:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.14 , 98.05 , 96.83 , 66.60 2021-02-12T23:17:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.10 , 0.10 2021-02-12T23:17:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171868.27 epoch sec; seqNo:2486; 4056638.69 north; 590726.22 east 2021-02-12T23:17:48Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-12T23:17:48Z- waiting for 1613171865.98, time is 1613171868.27 2021-02-12T23:17:48Z- LcmTrn::updateTrn() equating pose 1613171868.27 and meas times 1613171868.27 2021-02-12T23:17:48Z- LcmTrn::updateTrn() >>>> pose time 1613171868.27, meas time 1613171868.27 <<<< 2021-02-12T23:17:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171868.27 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:17:48Z- TerrainNav::motionUpdate() - currEstimate = (4056639, 590726.22, 19.10) 2021-02-12T23:17:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:17:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:17:48Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171868.27 sec.; No relevant navigation data available 2021-02-12T23:17:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:17:48Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:48Z- variances([0], [2], and psi_berg) = 781.355914 833.069641 0.000000 2021-02-12T23:17:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:17:48Z- LcmTrn::publishEstimates() - MLE : 4056614.19 590775.72 19.10 2021-02-12T23:17:48Z- LcmTrn::publishEstimates() - MMSE : 4056638.69 590726.22 19.10 2021-02-12T23:17:48Z- LcmTrn::publishEstimates() - COVAR: 781.36 833.07 0.93 0.00 2021-02-12T23:17:48Z- AHRS_M2 msg: 1613171868.62 epoch sec; seqNo:2462 2021-02-12T23:17:48Z- AHRS_M2 msg: 0.56 phi; 0.33 theta; -0.17 psi 2021-02-12T23:17:48Z- handleDepth()-Depth_Keller msg: 1613171868628 epoch sec; seqNo:2487; depth 18.97 2021-02-12T23:17:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171868.64 epoch sec; seqNo:2487; 4056639.07 north; 590726.22 east 2021-02-12T23:17:49Z- AHRS_M2 msg: 1613171869.03 epoch sec; seqNo:2463 2021-02-12T23:17:49Z- AHRS_M2 msg: 0.56 phi; 0.33 theta; -0.17 psi 2021-02-12T23:17:49Z- handleDepth()-Depth_Keller msg: 1613171869038 epoch sec; seqNo:2488; depth 18.86 2021-02-12T23:17:49Z- handleDvl() - RDI_Pathfinder msg: 1613171869.05 epoch sec; seqNo:1048 2021-02-12T23:17:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:17:49Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:17:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171869.06 epoch sec; seqNo:2488; 4056639.07 north; 590726.22 east 2021-02-12T23:17:49Z- AHRS_M2 msg: 1613171869.43 epoch sec; seqNo:2464 2021-02-12T23:17:49Z- AHRS_M2 msg: 0.56 phi; 0.33 theta; -0.16 psi 2021-02-12T23:17:49Z- handleDepth()-Depth_Keller msg: 1613171869441 epoch sec; seqNo:2489; depth 18.76 2021-02-12T23:17:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171869.46 epoch sec; seqNo:2489; 4056639.45 north; 590726.21 east 2021-02-12T23:17:49Z- AHRS_M2 msg: 1613171869.84 epoch sec; seqNo:2465 2021-02-12T23:17:49Z- AHRS_M2 msg: 0.56 phi; 0.33 theta; -0.15 psi 2021-02-12T23:17:49Z- handleDepth()-Depth_Keller msg: 1613171869840 epoch sec; seqNo:2490; depth 18.66 2021-02-12T23:17:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171869.85 epoch sec; seqNo:2490; 4056639.83 north; 590726.21 east 2021-02-12T23:17:50Z- AHRS_M2 msg: 1613171870.24 epoch sec; seqNo:2466 2021-02-12T23:17:50Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.15 psi 2021-02-12T23:17:50Z- handleDepth()-Depth_Keller msg: 1613171870245 epoch sec; seqNo:2491; depth 18.56 2021-02-12T23:17:50Z- handleDvl() - RDI_Pathfinder msg: 1613171870.26 epoch sec; seqNo:1049 2021-02-12T23:17:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.18 , 89.39 , 96.15 , 67.42 2021-02-12T23:17:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , 0.12 2021-02-12T23:17:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171870.27 epoch sec; seqNo:2491; 4056640.21 north; 590726.21 east 2021-02-12T23:17:50Z- AHRS_M2 msg: 1613171870.64 epoch sec; seqNo:2467 2021-02-12T23:17:50Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.14 psi 2021-02-12T23:17:50Z- handleDepth()-Depth_Keller msg: 1613171870650 epoch sec; seqNo:2492; depth 18.45 2021-02-12T23:17:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171870.66 epoch sec; seqNo:2492; 4056640.22 north; 590727.43 east 2021-02-12T23:17:51Z- AHRS_M2 msg: 1613171871.05 epoch sec; seqNo:2468 2021-02-12T23:17:51Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.13 psi 2021-02-12T23:17:51Z- handleDepth()-Depth_Keller msg: 1613171871057 epoch sec; seqNo:2493; depth 18.35 2021-02-12T23:17:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171871.08 epoch sec; seqNo:2493; 4056640.60 north; 590727.42 east 2021-02-12T23:17:51Z- AHRS_M2 msg: 1613171871.45 epoch sec; seqNo:2469 2021-02-12T23:17:51Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.13 psi 2021-02-12T23:17:51Z- handleDepth()-Depth_Keller msg: 1613171871456 epoch sec; seqNo:2494; depth 18.22 2021-02-12T23:17:51Z- handleDvl() - RDI_Pathfinder msg: 1613171871.47 epoch sec; seqNo:1050 2021-02-12T23:17:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 91.29 , 105.35 , 67.83 2021-02-12T23:17:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.05 , 0.06 2021-02-12T23:17:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171871.49 epoch sec; seqNo:2494; 4056640.98 north; 590727.42 east 2021-02-12T23:17:51Z- AHRS_M2 msg: 1613171871.86 epoch sec; seqNo:2470 2021-02-12T23:17:51Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.12 psi 2021-02-12T23:17:51Z- handleDepth()-Depth_Keller msg: 1613171871860 epoch sec; seqNo:2495; depth 18.12 2021-02-12T23:17:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171871.87 epoch sec; seqNo:2495; 4056641.36 north; 590727.41 east 2021-02-12T23:17:52Z- AHRS_M2 msg: 1613171872.26 epoch sec; seqNo:2471 2021-02-12T23:17:52Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.12 psi 2021-02-12T23:17:52Z- handleDepth()-Depth_Keller msg: 1613171872264 epoch sec; seqNo:2496; depth 18.01 2021-02-12T23:17:52Z- handleDvl() - RDI_Pathfinder msg: 1613171872.29 epoch sec; seqNo:1051 2021-02-12T23:17:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.41 , 89.60 , 96.90 , 69.46 2021-02-12T23:17:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , 0.03 2021-02-12T23:17:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171872.30 epoch sec; seqNo:2496; 4056641.36 north; 590727.41 east 2021-02-12T23:17:52Z- AHRS_M2 msg: 1613171872.66 epoch sec; seqNo:2472 2021-02-12T23:17:52Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.12 psi 2021-02-12T23:17:52Z- handleDepth()-Depth_Keller msg: 1613171872673 epoch sec; seqNo:2497; depth 17.91 2021-02-12T23:17:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171872.68 epoch sec; seqNo:2497; 4056641.74 north; 590727.41 east 2021-02-12T23:17:53Z- AHRS_M2 msg: 1613171873.07 epoch sec; seqNo:2473 2021-02-12T23:17:53Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.12 psi 2021-02-12T23:17:53Z- handleDepth()-Depth_Keller msg: 1613171873072 epoch sec; seqNo:2498; depth 17.79 2021-02-12T23:17:53Z- handleDvl() - RDI_Pathfinder msg: 1613171873.08 epoch sec; seqNo:1052 2021-02-12T23:17:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.82 , 89.87 , 97.17 , 69.46 2021-02-12T23:17:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , 0.01 2021-02-12T23:17:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171873.09 epoch sec; seqNo:2498; 4056642.13 north; 590728.63 east 2021-02-12T23:17:53Z- LcmTrn::handleMessages() - handling 41 messages took 0.66 ms and 128 calls... 2021-02-12T23:17:53Z- waiting for 1613171871.27, time is 1613171873.09 2021-02-12T23:17:53Z- LcmTrn::updateTrn() equating pose 1613171873.09 and meas times 1613171873.09 2021-02-12T23:17:53Z- LcmTrn::updateTrn() >>>> pose time 1613171873.09, meas time 1613171873.09 <<<< 2021-02-12T23:17:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171873.09 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:17:53Z- TerrainNav::motionUpdate() - currEstimate = (4056642, 590728.63, 17.79) 2021-02-12T23:17:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:17:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:17:53Z- TNavBF::Projecting Measurements 2021-02-12T23:17:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:17:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:17:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:17:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:17:53Z- TNavFilter::4 good beams. 2021-02-12T23:17:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:17:53Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:17:53Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:17:53Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:17:53Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:17:53Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:17:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:17:53Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:53Z- TerrainMapOctree: (vehN, vehE ) = (4056642.13, 590728.63). 2021-02-12T23:17:53Z- TerrainMapOctree: Min Distance = 5799.36. 2021-02-12T23:17:53Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:17:53Z- There is no specific good beam that all particles have in common. 2021-02-12T23:17:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:17:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 9.44 2021-02-12T23:17:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:17:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 12.92 2021-02-12T23:17:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:17:53Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.11 2021-02-12T23:17:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:17:53Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 7.72 2021-02-12T23:17:53Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:17:53Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:17:53Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:17:53Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:17:53Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:17:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:17:53Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:17:53Z- Filter 0 No beams used in subcloud update 2021-02-12T23:17:53Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:17:53Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:17:53Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 781.2 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:17:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171873.09 sec. 2021-02-12T23:17:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:17:53Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:53Z- variances([0], [2], and psi_berg) = 781.195958 833.069405 0.000000 2021-02-12T23:17:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:17:53Z- LcmTrn::publishEstimates() - MLE : 4056617.63 590778.13 17.79 2021-02-12T23:17:53Z- LcmTrn::publishEstimates() - MMSE : 4056642.13 590728.63 17.79 2021-02-12T23:17:53Z- LcmTrn::publishEstimates() - COVAR: 781.20 833.07 0.83 0.00 2021-02-12T23:17:53Z- AHRS_M2 msg: 1613171873.47 epoch sec; seqNo:2474 2021-02-12T23:17:53Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.12 psi 2021-02-12T23:17:53Z- handleDepth()-Depth_Keller msg: 1613171873478 epoch sec; seqNo:2499; depth 17.69 2021-02-12T23:17:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171873.49 epoch sec; seqNo:2499; 4056642.13 north; 590728.63 east 2021-02-12T23:17:53Z- AHRS_M2 msg: 1613171873.88 epoch sec; seqNo:2475 2021-02-12T23:17:53Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.12 psi 2021-02-12T23:17:53Z- handleDepth()-Depth_Keller msg: 1613171873880 epoch sec; seqNo:2500; depth 17.57 2021-02-12T23:17:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171873.90 epoch sec; seqNo:2500; 4056642.51 north; 590728.62 east 2021-02-12T23:17:54Z- AHRS_M2 msg: 1613171874.28 epoch sec; seqNo:2476 2021-02-12T23:17:54Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.12 psi 2021-02-12T23:17:54Z- handleDepth()-Depth_Keller msg: 1613171874289 epoch sec; seqNo:2501; depth 17.46 2021-02-12T23:17:54Z- handleDvl() - RDI_Pathfinder msg: 1613171874.30 epoch sec; seqNo:1053 2021-02-12T23:17:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.64 , 91.02 , 103.18 , 69.06 2021-02-12T23:17:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , 0.02 2021-02-12T23:17:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171874.33 epoch sec; seqNo:2501; 4056642.89 north; 590728.62 east 2021-02-12T23:17:54Z- AHRS_M2 msg: 1613171874.68 epoch sec; seqNo:2477 2021-02-12T23:17:54Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.12 psi 2021-02-12T23:17:54Z- handleDepth()-Depth_Keller msg: 1613171874688 epoch sec; seqNo:2502; depth 17.35 2021-02-12T23:17:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171874.70 epoch sec; seqNo:2502; 4056643.27 north; 590728.62 east 2021-02-12T23:17:55Z- AHRS_M2 msg: 1613171875.09 epoch sec; seqNo:2478 2021-02-12T23:17:55Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.12 psi 2021-02-12T23:17:55Z- handleDepth()-Depth_Keller msg: 1613171875102 epoch sec; seqNo:2503; depth 17.22 2021-02-12T23:17:55Z- handleDvl() - RDI_Pathfinder msg: 1613171875.11 epoch sec; seqNo:1054 2021-02-12T23:17:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.64 , 89.67 , 105.48 , 69.87 2021-02-12T23:17:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , 0.02 2021-02-12T23:17:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171875.13 epoch sec; seqNo:2503; 4056643.27 north; 590728.62 east 2021-02-12T23:17:55Z- AHRS_M2 msg: 1613171875.49 epoch sec; seqNo:2479 2021-02-12T23:17:55Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-12T23:17:55Z- handleDepth()-Depth_Keller msg: 1613171875496 epoch sec; seqNo:2504; depth 17.11 2021-02-12T23:17:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171875.51 epoch sec; seqNo:2504; 4056643.66 north; 590729.83 east 2021-02-12T23:17:55Z- AHRS_M2 msg: 1613171875.90 epoch sec; seqNo:2480 2021-02-12T23:17:55Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-12T23:17:55Z- handleDepth()-Depth_Keller msg: 1613171875900 epoch sec; seqNo:2505; depth 16.99 2021-02-12T23:17:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171875.91 epoch sec; seqNo:2505; 4056644.04 north; 590729.83 east 2021-02-12T23:17:56Z- AHRS_M2 msg: 1613171876.31 epoch sec; seqNo:2481 2021-02-12T23:17:56Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.13 psi 2021-02-12T23:17:56Z- handleDepth()-Depth_Keller msg: 1613171876310 epoch sec; seqNo:2506; depth 16.88 2021-02-12T23:17:56Z- handleDvl() - RDI_Pathfinder msg: 1613171876.32 epoch sec; seqNo:1055 2021-02-12T23:17:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 90.81 , 96.89 , 69.86 2021-02-12T23:17:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.05 , -0.04 2021-02-12T23:17:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171876.33 epoch sec; seqNo:2506; 4056644.04 north; 590729.83 east 2021-02-12T23:17:56Z- AHRS_M2 msg: 1613171876.70 epoch sec; seqNo:2482 2021-02-12T23:17:56Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.13 psi 2021-02-12T23:17:56Z- handleDepth()-Depth_Keller msg: 1613171876718 epoch sec; seqNo:2507; depth 16.77 2021-02-12T23:17:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171876.73 epoch sec; seqNo:2507; 4056644.42 north; 590729.82 east 2021-02-12T23:17:57Z- AHRS_M2 msg: 1613171877.12 epoch sec; seqNo:2483 2021-02-12T23:17:57Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.13 psi 2021-02-12T23:17:57Z- handleDepth()-Depth_Keller msg: 1613171877126 epoch sec; seqNo:2508; depth 16.65 2021-02-12T23:17:57Z- handleDvl() - RDI_Pathfinder msg: 1613171877.16 epoch sec; seqNo:1056 2021-02-12T23:17:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 98.24 , 104.33 , 68.23 2021-02-12T23:17:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.08 , -0.12 2021-02-12T23:17:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171877.17 epoch sec; seqNo:2508; 4056644.79 north; 590729.82 east 2021-02-12T23:17:57Z- AHRS_M2 msg: 1613171877.51 epoch sec; seqNo:2484 2021-02-12T23:17:57Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.12 psi 2021-02-12T23:17:57Z- handleDepth()-Depth_Keller msg: 1613171877518 epoch sec; seqNo:2509; depth 16.52 2021-02-12T23:17:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171877.53 epoch sec; seqNo:2509; 4056644.79 north; 590729.82 east 2021-02-12T23:17:57Z- AHRS_M2 msg: 1613171877.92 epoch sec; seqNo:2485 2021-02-12T23:17:57Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.12 psi 2021-02-12T23:17:57Z- handleDepth()-Depth_Keller msg: 1613171877920 epoch sec; seqNo:2510; depth 16.43 2021-02-12T23:17:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171877.94 epoch sec; seqNo:2510; 4056645.19 north; 590731.04 east 2021-02-12T23:17:58Z- AHRS_M2 msg: 1613171878.32 epoch sec; seqNo:2486 2021-02-12T23:17:58Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.12 psi 2021-02-12T23:17:58Z- handleDepth()-Depth_Keller msg: 1613171878324 epoch sec; seqNo:2511; depth 16.31 2021-02-12T23:17:58Z- handleDvl() - RDI_Pathfinder msg: 1613171878.34 epoch sec; seqNo:1057 2021-02-12T23:17:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 91.89 , 105.27 , 69.05 2021-02-12T23:17:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.01 , -0.17 2021-02-12T23:17:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171878.35 epoch sec; seqNo:2511; 4056645.19 north; 590731.04 east 2021-02-12T23:17:58Z- LcmTrn::handleMessages() - handling 44 messages took 0.78 ms and 129 calls... 2021-02-12T23:17:58Z- waiting for 1613171876.09, time is 1613171878.35 2021-02-12T23:17:58Z- LcmTrn::updateTrn() equating pose 1613171878.35 and meas times 1613171878.35 2021-02-12T23:17:58Z- LcmTrn::updateTrn() >>>> pose time 1613171878.35, meas time 1613171878.35 <<<< 2021-02-12T23:17:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171878.35 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:17:58Z- TerrainNav::motionUpdate() - currEstimate = (4056645, 590731.04, 16.31) 2021-02-12T23:17:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:17:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:17:58Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171878.35 sec.; No relevant navigation data available 2021-02-12T23:17:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:17:58Z- TNavBF::mmse selected from filter -1 2021-02-12T23:17:58Z- variances([0], [2], and psi_berg) = 781.042295 833.069179 0.000000 2021-02-12T23:17:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:17:58Z- LcmTrn::publishEstimates() - MLE : 4056620.69 590780.54 16.31 2021-02-12T23:17:58Z- LcmTrn::publishEstimates() - MMSE : 4056645.19 590731.04 16.31 2021-02-12T23:17:58Z- LcmTrn::publishEstimates() - COVAR: 781.04 833.07 1.03 0.00 2021-02-12T23:17:58Z- AHRS_M2 msg: 1613171878.72 epoch sec; seqNo:2487 2021-02-12T23:17:58Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.13 psi 2021-02-12T23:17:58Z- handleDepth()-Depth_Keller msg: 1613171878729 epoch sec; seqNo:2512; depth 16.21 2021-02-12T23:17:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171878.74 epoch sec; seqNo:2512; 4056645.56 north; 590731.03 east 2021-02-12T23:17:59Z- AHRS_M2 msg: 1613171879.13 epoch sec; seqNo:2488 2021-02-12T23:17:59Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.13 psi 2021-02-12T23:17:59Z- handleDepth()-Depth_Keller msg: 1613171879132 epoch sec; seqNo:2513; depth 16.10 2021-02-12T23:17:59Z- handleDvl() - RDI_Pathfinder msg: 1613171879.15 epoch sec; seqNo:1058 2021-02-12T23:17:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 92.03 , 106.62 , 69.46 2021-02-12T23:17:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.12 , -0.15 2021-02-12T23:17:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171879.17 epoch sec; seqNo:2513; 4056645.94 north; 590731.03 east 2021-02-12T23:17:59Z- AHRS_M2 msg: 1613171879.53 epoch sec; seqNo:2489 2021-02-12T23:17:59Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.14 psi 2021-02-12T23:17:59Z- handleDepth()-Depth_Keller msg: 1613171879536 epoch sec; seqNo:2514; depth 15.96 2021-02-12T23:17:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171879.56 epoch sec; seqNo:2514; 4056645.94 north; 590731.03 east 2021-02-12T23:17:59Z- AHRS_M2 msg: 1613171879.94 epoch sec; seqNo:2490 2021-02-12T23:17:59Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.16 psi 2021-02-12T23:17:59Z- handleDepth()-Depth_Keller msg: 1613171879946 epoch sec; seqNo:2515; depth 15.85 2021-02-12T23:17:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171879.95 epoch sec; seqNo:2515; 4056646.32 north; 590731.03 east 2021-02-12T23:18:00Z- AHRS_M2 msg: 1613171880.34 epoch sec; seqNo:2491 2021-02-12T23:18:00Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.17 psi 2021-02-12T23:18:00Z- handleDepth()-Depth_Keller msg: 1613171880346 epoch sec; seqNo:2516; depth 15.74 2021-02-12T23:18:00Z- handleDvl() - RDI_Pathfinder msg: 1613171880.37 epoch sec; seqNo:1059 2021-02-12T23:18:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.81 , 93.18 , 107.77 , 70.68 2021-02-12T23:18:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.22 , -0.03 2021-02-12T23:18:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171880.38 epoch sec; seqNo:2516; 4056646.70 north; 590731.02 east 2021-02-12T23:18:00Z- AHRS_M2 msg: 1613171880.74 epoch sec; seqNo:2492 2021-02-12T23:18:00Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.18 psi 2021-02-12T23:18:00Z- handleDepth()-Depth_Keller msg: 1613171880753 epoch sec; seqNo:2517; depth 15.61 2021-02-12T23:18:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171880.76 epoch sec; seqNo:2517; 4056646.70 north; 590731.02 east 2021-02-12T23:18:01Z- AHRS_M2 msg: 1613171881.15 epoch sec; seqNo:2493 2021-02-12T23:18:01Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.18 psi 2021-02-12T23:18:01Z- handleDepth()-Depth_Keller msg: 1613171881155 epoch sec; seqNo:2518; depth 15.50 2021-02-12T23:18:01Z- handleDvl() - RDI_Pathfinder msg: 1613171881.17 epoch sec; seqNo:1060 2021-02-12T23:18:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.40 , 95.54 , 106.49 , 71.09 2021-02-12T23:18:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.21 , 0.11 2021-02-12T23:18:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171881.18 epoch sec; seqNo:2518; 4056647.08 north; 590731.02 east 2021-02-12T23:18:01Z- AHRS_M2 msg: 1613171881.55 epoch sec; seqNo:2494 2021-02-12T23:18:01Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.19 psi 2021-02-12T23:18:01Z- handleDepth()-Depth_Keller msg: 1613171881561 epoch sec; seqNo:2519; depth 15.39 2021-02-12T23:18:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171881.57 epoch sec; seqNo:2519; 4056647.47 north; 590732.23 east 2021-02-12T23:18:01Z- AHRS_M2 msg: 1613171881.96 epoch sec; seqNo:2495 2021-02-12T23:18:01Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.19 psi 2021-02-12T23:18:01Z- handleDepth()-Depth_Keller msg: 1613171881961 epoch sec; seqNo:2520; depth 15.28 2021-02-12T23:18:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171881.97 epoch sec; seqNo:2520; 4056647.85 north; 590732.23 east 2021-02-12T23:18:02Z- AHRS_M2 msg: 1613171882.36 epoch sec; seqNo:2496 2021-02-12T23:18:02Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.19 psi 2021-02-12T23:18:02Z- handleDepth()-Depth_Keller msg: 1613171882366 epoch sec; seqNo:2521; depth 15.17 2021-02-12T23:18:02Z- handleDvl() - RDI_Pathfinder msg: 1613171882.38 epoch sec; seqNo:1061 2021-02-12T23:18:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.81 , 96.76 , 107.71 , 70.27 2021-02-12T23:18:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.12 , 0.18 2021-02-12T23:18:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171882.39 epoch sec; seqNo:2521; 4056648.23 north; 590732.23 east 2021-02-12T23:18:02Z- AHRS_M2 msg: 1613171882.77 epoch sec; seqNo:2497 2021-02-12T23:18:02Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-12T23:18:02Z- handleDepth()-Depth_Keller msg: 1613171882774 epoch sec; seqNo:2522; depth 15.04 2021-02-12T23:18:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171882.79 epoch sec; seqNo:2522; 4056648.23 north; 590732.23 east 2021-02-12T23:18:03Z- AHRS_M2 msg: 1613171883.17 epoch sec; seqNo:2498 2021-02-12T23:18:03Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-12T23:18:03Z- handleDepth()-Depth_Keller msg: 1613171883181 epoch sec; seqNo:2523; depth 14.93 2021-02-12T23:18:03Z- handleDvl() - RDI_Pathfinder msg: 1613171883.22 epoch sec; seqNo:1062 2021-02-12T23:18:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.63 , 97.91 , 107.64 , 72.32 2021-02-12T23:18:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.16 2021-02-12T23:18:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171883.23 epoch sec; seqNo:2523; 4056648.61 north; 590732.22 east 2021-02-12T23:18:03Z- LcmTrn::handleMessages() - handling 41 messages took 0.65 ms and 128 calls... 2021-02-12T23:18:03Z- waiting for 1613171881.35, time is 1613171883.23 2021-02-12T23:18:03Z- LcmTrn::updateTrn() equating pose 1613171883.23 and meas times 1613171883.23 2021-02-12T23:18:03Z- LcmTrn::updateTrn() >>>> pose time 1613171883.23, meas time 1613171883.23 <<<< 2021-02-12T23:18:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171883.23 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:18:03Z- TerrainNav::motionUpdate() - currEstimate = (4056649, 590732.22, 14.93) 2021-02-12T23:18:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:18:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:18:03Z- TNavBF::Projecting Measurements 2021-02-12T23:18:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:18:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:18:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:18:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:18:03Z- TNavFilter::4 good beams. 2021-02-12T23:18:03Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:18:03Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:18:03Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:18:03Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:18:03Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:18:03Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:18:03Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:18:03Z- TNavBF::mmse selected from filter -1 2021-02-12T23:18:03Z- TerrainMapOctree: (vehN, vehE ) = (4056648.61, 590732.22). 2021-02-12T23:18:03Z- TerrainMapOctree: Min Distance = 5792.03. 2021-02-12T23:18:03Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:18:03Z- There is no specific good beam that all particles have in common. 2021-02-12T23:18:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:18:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 9.64 2021-02-12T23:18:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:18:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-12T23:18:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:18:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 18.54 2021-02-12T23:18:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:18:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-12T23:18:03Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:18:03Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:18:03Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:18:03Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:18:03Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:18:03Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:18:03Z- Filter 0 No beams used in subcloud update 2021-02-12T23:18:03Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:18:03Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:18:03Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 780.9 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:18:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171883.23 sec. 2021-02-12T23:18:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:18:03Z- TNavBF::mmse selected from filter -1 2021-02-12T23:18:03Z- variances([0], [2], and psi_berg) = 780.894465 833.068962 0.000000 2021-02-12T23:18:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:18:03Z- LcmTrn::publishEstimates() - MLE : 4056624.11 590781.72 14.93 2021-02-12T23:18:03Z- LcmTrn::publishEstimates() - MMSE : 4056648.61 590732.22 14.93 2021-02-12T23:18:03Z- LcmTrn::publishEstimates() - COVAR: 780.89 833.07 0.90 0.00 2021-02-12T23:18:03Z- AHRS_M2 msg: 1613171883.57 epoch sec; seqNo:2499 2021-02-12T23:18:03Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-12T23:18:03Z- handleDepth()-Depth_Keller msg: 1613171883581 epoch sec; seqNo:2524; depth 14.81 2021-02-12T23:18:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171883.59 epoch sec; seqNo:2524; 4056648.99 north; 590732.22 east 2021-02-12T23:18:03Z- AHRS_M2 msg: 1613171883.98 epoch sec; seqNo:2500 2021-02-12T23:18:03Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-12T23:18:03Z- handleDepth()-Depth_Keller msg: 1613171883989 epoch sec; seqNo:2525; depth 14.70 2021-02-12T23:18:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171884.00 epoch sec; seqNo:2525; 4056649.37 north; 590732.21 east 2021-02-12T23:18:04Z- AHRS_M2 msg: 1613171884.38 epoch sec; seqNo:2501 2021-02-12T23:18:04Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-12T23:18:04Z- handleDepth()-Depth_Keller msg: 1613171884385 epoch sec; seqNo:2526; depth 14.60 2021-02-12T23:18:04Z- handleDvl() - RDI_Pathfinder msg: 1613171884.40 epoch sec; seqNo:1063 2021-02-12T23:18:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 96.62 , 107.57 , 72.32 2021-02-12T23:18:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , 0.07 2021-02-12T23:18:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171884.41 epoch sec; seqNo:2526; 4056649.38 north; 590733.44 east 2021-02-12T23:18:04Z- AHRS_M2 msg: 1613171884.78 epoch sec; seqNo:2502 2021-02-12T23:18:04Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.14 psi 2021-02-12T23:18:04Z- handleDepth()-Depth_Keller msg: 1613171884789 epoch sec; seqNo:2527; depth 14.48 2021-02-12T23:18:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171884.80 epoch sec; seqNo:2527; 4056649.76 north; 590733.43 east 2021-02-12T23:18:05Z- AHRS_M2 msg: 1613171885.20 epoch sec; seqNo:2503 2021-02-12T23:18:05Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-12T23:18:05Z- handleDepth()-Depth_Keller msg: 1613171885214 epoch sec; seqNo:2528; depth 14.39 2021-02-12T23:18:05Z- handleDvl() - RDI_Pathfinder msg: 1613171885.23 epoch sec; seqNo:1064 2021-02-12T23:18:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.81 , 97.84 , 107.57 , 78.38 2021-02-12T23:18:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.08 , -0.00 2021-02-12T23:18:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171885.26 epoch sec; seqNo:2528; 4056650.14 north; 590733.43 east 2021-02-12T23:18:05Z- AHRS_M2 msg: 1613171885.59 epoch sec; seqNo:2504 2021-02-12T23:18:05Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.15 psi 2021-02-12T23:18:05Z- handleDepth()-Depth_Keller msg: 1613171885597 epoch sec; seqNo:2529; depth 14.26 2021-02-12T23:18:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171885.61 epoch sec; seqNo:2529; 4056650.52 north; 590733.42 east 2021-02-12T23:18:06Z- AHRS_M2 msg: 1613171886.00 epoch sec; seqNo:2505 2021-02-12T23:18:06Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.15 psi 2021-02-12T23:18:06Z- handleDepth()-Depth_Keller msg: 1613171886013 epoch sec; seqNo:2530; depth 14.16 2021-02-12T23:18:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171886.03 epoch sec; seqNo:2530; 4056650.52 north; 590733.42 east 2021-02-12T23:18:06Z- AHRS_M2 msg: 1613171886.40 epoch sec; seqNo:2506 2021-02-12T23:18:06Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.16 psi 2021-02-12T23:18:06Z- handleDepth()-Depth_Keller msg: 1613171886413 epoch sec; seqNo:2531; depth 14.05 2021-02-12T23:18:06Z- handleDvl() - RDI_Pathfinder msg: 1613171886.42 epoch sec; seqNo:1065 2021-02-12T23:18:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.67 , 96.42 , 109.80 , 71.50 2021-02-12T23:18:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.03 , -0.04 2021-02-12T23:18:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171886.43 epoch sec; seqNo:2531; 4056650.91 north; 590734.64 east 2021-02-12T23:18:06Z- AHRS_M2 msg: 1613171886.80 epoch sec; seqNo:2507 2021-02-12T23:18:06Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.17 psi 2021-02-12T23:18:06Z- handleDepth()-Depth_Keller msg: 1613171886808 epoch sec; seqNo:2532; depth 13.93 2021-02-12T23:18:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171886.83 epoch sec; seqNo:2532; 4056651.29 north; 590734.64 east 2021-02-12T23:18:07Z- AHRS_M2 msg: 1613171887.21 epoch sec; seqNo:2508 2021-02-12T23:18:07Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.19 psi 2021-02-12T23:18:07Z- handleDepth()-Depth_Keller msg: 1613171887211 epoch sec; seqNo:2533; depth 13.82 2021-02-12T23:18:07Z- handleDvl() - RDI_Pathfinder msg: 1613171887.22 epoch sec; seqNo:1066 2021-02-12T23:18:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 96.62 , 108.79 , 71.91 2021-02-12T23:18:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , 0.00 2021-02-12T23:18:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171887.25 epoch sec; seqNo:2533; 4056651.29 north; 590734.64 east 2021-02-12T23:18:07Z- AHRS_M2 msg: 1613171887.61 epoch sec; seqNo:2509 2021-02-12T23:18:07Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.20 psi 2021-02-12T23:18:07Z- handleDepth()-Depth_Keller msg: 1613171887616 epoch sec; seqNo:2534; depth 13.69 2021-02-12T23:18:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171887.63 epoch sec; seqNo:2534; 4056651.66 north; 590734.63 east 2021-02-12T23:18:08Z- AHRS_M2 msg: 1613171888.02 epoch sec; seqNo:2510 2021-02-12T23:18:08Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.21 psi 2021-02-12T23:18:08Z- handleDepth()-Depth_Keller msg: 1613171888025 epoch sec; seqNo:2535; depth 13.58 2021-02-12T23:18:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171888.03 epoch sec; seqNo:2535; 4056652.04 north; 590734.63 east 2021-02-12T23:18:08Z- AHRS_M2 msg: 1613171888.42 epoch sec; seqNo:2511 2021-02-12T23:18:08Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.21 psi 2021-02-12T23:18:08Z- handleDepth()-Depth_Keller msg: 1613171888425 epoch sec; seqNo:2536; depth 13.46 2021-02-12T23:18:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171888.45 epoch sec; seqNo:2536; 4056652.42 north; 590734.62 east 2021-02-12T23:18:08Z- LcmTrn::handleMessages() - handling 43 messages took 0.67 ms and 130 calls... 2021-02-12T23:18:08Z- waiting for 1613171886.23, time is 1613171888.45 2021-02-12T23:18:08Z- LcmTrn::updateTrn() >>>> pose time 1613171888.45, meas time 1613171887.22 <<<< 2021-02-12T23:18:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171887.22 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:18:08Z- TerrainNav::motionUpdate() - currEstimate = (4056652, 590734.62, 13.46) 2021-02-12T23:18:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:18:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:18:08Z- TNavBF::Projecting Measurements 2021-02-12T23:18:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:18:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:18:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:18:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:18:08Z- TNavFilter::4 good beams. 2021-02-12T23:18:08Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:18:08Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:18:08Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:18:08Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:18:08Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:18:08Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:18:08Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:18:08Z- TNavBF::mmse selected from filter -1 2021-02-12T23:18:08Z- TerrainMapOctree: (vehN, vehE ) = (4056651.53, 590734.06). 2021-02-12T23:18:08Z- TerrainMapOctree: Min Distance = 5788.59. 2021-02-12T23:18:08Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:18:08Z- There is no specific good beam that all particles have in common. 2021-02-12T23:18:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:18:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-12T23:18:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:18:08Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 14.94 2021-02-12T23:18:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:18:08Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 18.94 2021-02-12T23:18:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:18:08Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 8.27 2021-02-12T23:18:08Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:18:08Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:18:08Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:18:08Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:18:08Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:18:08Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:18:08Z- Filter 0 No beams used in subcloud update 2021-02-12T23:18:08Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:18:08Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:18:08Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 780.8 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:18:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171887.22 sec. 2021-02-12T23:18:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:18:08Z- TNavBF::mmse selected from filter -1 2021-02-12T23:18:08Z- variances([0], [2], and psi_berg) = 780.671037 833.068636 0.000000 2021-02-12T23:18:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:18:08Z- LcmTrn::publishEstimates() - MLE : 4056627.92 590784.12 13.46 2021-02-12T23:18:08Z- LcmTrn::publishEstimates() - MMSE : 4056652.42 590734.62 13.46 2021-02-12T23:18:08Z- LcmTrn::publishEstimates() - COVAR: 780.67 833.07 0.87 0.00 2021-02-12T23:18:08Z- AHRS_M2 msg: 1613171888.82 epoch sec; seqNo:2512 2021-02-12T23:18:08Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.21 psi 2021-02-12T23:18:08Z- handleDepth()-Depth_Keller msg: 1613171888828 epoch sec; seqNo:2537; depth 13.33 2021-02-12T23:18:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171888.84 epoch sec; seqNo:2537; 4056652.42 north; 590734.62 east 2021-02-12T23:18:09Z- AHRS_M2 msg: 1613171889.23 epoch sec; seqNo:2513 2021-02-12T23:18:09Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.21 psi 2021-02-12T23:18:09Z- handleDepth()-Depth_Keller msg: 1613171889237 epoch sec; seqNo:2538; depth 13.22 2021-02-12T23:18:09Z- handleDvl() - RDI_Pathfinder msg: 1613171889.25 epoch sec; seqNo:1067 2021-02-12T23:18:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 98.92 , 114.73 , 73.54 2021-02-12T23:18:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.12 2021-02-12T23:18:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171889.26 epoch sec; seqNo:2538; 4056652.81 north; 590735.84 east 2021-02-12T23:18:09Z- AHRS_M2 msg: 1613171889.63 epoch sec; seqNo:2514 2021-02-12T23:18:09Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.20 psi 2021-02-12T23:18:09Z- handleDepth()-Depth_Keller msg: 1613171889645 epoch sec; seqNo:2539; depth 13.10 2021-02-12T23:18:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171889.66 epoch sec; seqNo:2539; 4056653.19 north; 590735.84 east 2021-02-12T23:18:10Z- AHRS_M2 msg: 1613171890.04 epoch sec; seqNo:2515 2021-02-12T23:18:10Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.20 psi 2021-02-12T23:18:10Z- handleDepth()-Depth_Keller msg: 1613171890040 epoch sec; seqNo:2540; depth 12.99 2021-02-12T23:18:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:18:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:18:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:18:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:18:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:18:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:18:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:18:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:18:10Z- handleDvl() - RDI_Pathfinder msg: 1613171890.04 epoch sec; seqNo:1068 2021-02-12T23:18:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:18:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.12 2021-02-12T23:18:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171890.06 epoch sec; seqNo:2540; 4056653.57 north; 590735.83 east 2021-02-12T23:18:10Z- AHRS_M2 msg: 1613171890.44 epoch sec; seqNo:2516 2021-02-12T23:18:10Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.19 psi 2021-02-12T23:18:10Z- handleDepth()-Depth_Keller msg: 1613171890444 epoch sec; seqNo:2541; depth 12.87 2021-02-12T23:18:10Z- handleDvl() - RDI_Pathfinder msg: 1613171890.45 epoch sec; seqNo:1069 2021-02-12T23:18:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 100.07 , 113.45 , 73.54 2021-02-12T23:18:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.07 , 0.05 2021-02-12T23:18:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171890.48 epoch sec; seqNo:2541; 4056653.57 north; 590735.83 east 2021-02-12T23:18:10Z- AHRS_M2 msg: 1613171890.84 epoch sec; seqNo:2517 2021-02-12T23:18:10Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.19 psi 2021-02-12T23:18:10Z- handleDepth()-Depth_Keller msg: 1613171890848 epoch sec; seqNo:2542; depth 12.76 2021-02-12T23:18:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171890.86 epoch sec; seqNo:2542; 4056653.95 north; 590735.83 east 2021-02-12T23:18:11Z- AHRS_M2 msg: 1613171891.25 epoch sec; seqNo:2518 2021-02-12T23:18:11Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.18 psi 2021-02-12T23:18:11Z- handleDepth()-Depth_Keller msg: 1613171891254 epoch sec; seqNo:2543; depth 12.65 2021-02-12T23:18:11Z- handleDvl() - RDI_Pathfinder msg: 1613171891.27 epoch sec; seqNo:1070 2021-02-12T23:18:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.48 , 100.41 , 113.92 , 74.36 2021-02-12T23:18:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.09 , -0.07 2021-02-12T23:18:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171891.28 epoch sec; seqNo:2543; 4056654.33 north; 590735.83 east 2021-02-12T23:18:11Z- AHRS_M2 msg: 1613171891.65 epoch sec; seqNo:2519 2021-02-12T23:18:11Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.18 psi 2021-02-12T23:18:11Z- handleDepth()-Depth_Keller msg: 1613171891660 epoch sec; seqNo:2544; depth 12.53 2021-02-12T23:18:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171891.68 epoch sec; seqNo:2544; 4056654.34 north; 590737.05 east 2021-02-12T23:18:12Z- AHRS_M2 msg: 1613171892.06 epoch sec; seqNo:2520 2021-02-12T23:18:12Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.18 psi 2021-02-12T23:18:12Z- handleDepth()-Depth_Keller msg: 1613171892060 epoch sec; seqNo:2545; depth 12.41 2021-02-12T23:18:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171892.07 epoch sec; seqNo:2545; 4056654.72 north; 590737.04 east 2021-02-12T23:18:12Z- AHRS_M2 msg: 1613171892.46 epoch sec; seqNo:2521 2021-02-12T23:18:12Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-12T23:18:12Z- handleDepth()-Depth_Keller msg: 1613171892469 epoch sec; seqNo:2546; depth 12.28 2021-02-12T23:18:12Z- handleDvl() - RDI_Pathfinder msg: 1613171892.48 epoch sec; seqNo:1071 2021-02-12T23:18:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 98.99 , 113.85 , 74.77 2021-02-12T23:18:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.01 , -0.13 2021-02-12T23:18:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171892.49 epoch sec; seqNo:2546; 4056655.10 north; 590737.04 east 2021-02-12T23:18:12Z- AHRS_M2 msg: 1613171892.86 epoch sec; seqNo:2522 2021-02-12T23:18:12Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-12T23:18:12Z- handleDepth()-Depth_Keller msg: 1613171892877 epoch sec; seqNo:2547; depth 12.17 2021-02-12T23:18:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171892.89 epoch sec; seqNo:2547; 4056655.10 north; 590737.04 east 2021-02-12T23:18:13Z- AHRS_M2 msg: 1613171893.27 epoch sec; seqNo:2523 2021-02-12T23:18:13Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-12T23:18:13Z- handleDepth()-Depth_Keller msg: 1613171893285 epoch sec; seqNo:2548; depth 12.04 2021-02-12T23:18:13Z- handleDvl() - RDI_Pathfinder msg: 1613171893.31 epoch sec; seqNo:1072 2021-02-12T23:18:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 101.69 , 112.50 , 74.36 2021-02-12T23:18:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , -0.11 2021-02-12T23:18:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171893.33 epoch sec; seqNo:2548; 4056655.48 north; 590737.03 east 2021-02-12T23:18:13Z- AHRS_M2 msg: 1613171893.67 epoch sec; seqNo:2524 2021-02-12T23:18:13Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-12T23:18:13Z- LcmTrn::handleMessages() - handling 43 messages took 0.69 ms and 129 calls... 2021-02-12T23:18:13Z- waiting for 1613171891.45, time is 1613171893.67 2021-02-12T23:18:13Z- LcmTrn::updateTrn() >>>> pose time 1613171893.67, meas time 1613171893.31 <<<< 2021-02-12T23:18:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171893.31 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:18:13Z- TerrainNav::motionUpdate() - currEstimate = (4056655, 590737.03, 12.04) 2021-02-12T23:18:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:18:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:18:13Z- TNavBF::Projecting Measurements 2021-02-12T23:18:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:18:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:18:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:18:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:18:13Z- TNavFilter::4 good beams. 2021-02-12T23:18:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:18:13Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:18:13Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:18:13Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:18:13Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:18:13Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:18:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:18:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:18:13Z- TerrainMapOctree: (vehN, vehE ) = (4056655.27, 590736.87). 2021-02-12T23:18:13Z- TerrainMapOctree: Min Distance = 5783.91. 2021-02-12T23:18:13Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:18:13Z- There is no specific good beam that all particles have in common. 2021-02-12T23:18:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:18:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 10.96 2021-02-12T23:18:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:18:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 16.55 2021-02-12T23:18:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:18:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 20.25 2021-02-12T23:18:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:18:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-12T23:18:13Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:18:13Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:18:13Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:18:13Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:18:13Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:18:13Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:18:13Z- Filter 0 No beams used in subcloud update 2021-02-12T23:18:13Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:18:13Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:18:13Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 780.5 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:18:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171893.31 sec. 2021-02-12T23:18:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:18:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:18:13Z- variances([0], [2], and psi_berg) = 780.484239 833.068364 0.000000 2021-02-12T23:18:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:18:13Z- LcmTrn::publishEstimates() - MLE : 4056630.98 590786.53 12.04 2021-02-12T23:18:13Z- LcmTrn::publishEstimates() - MMSE : 4056655.48 590737.03 12.04 2021-02-12T23:18:13Z- LcmTrn::publishEstimates() - COVAR: 780.48 833.07 0.83 0.00 2021-02-12T23:18:13Z- handleDepth()-Depth_Keller msg: 1613171893677 epoch sec; seqNo:2549; depth 11.93 2021-02-12T23:18:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:18:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:18:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:18:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:18:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:18:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:18:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:18:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:18:13Z- handleDvl() - RDI_Pathfinder msg: 1613171893.68 epoch sec; seqNo:1073 2021-02-12T23:18:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:18:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , -0.11 2021-02-12T23:18:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171893.69 epoch sec; seqNo:2549; 4056655.86 north; 590737.03 east 2021-02-12T23:18:14Z- AHRS_M2 msg: 1613171894.08 epoch sec; seqNo:2525 2021-02-12T23:18:14Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-12T23:18:14Z- handleDepth()-Depth_Keller msg: 1613171894106 epoch sec; seqNo:2550; depth 11.83 2021-02-12T23:18:14Z- handleDvl() - RDI_Pathfinder msg: 1613171894.13 epoch sec; seqNo:1074 2021-02-12T23:18:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 98.99 , 113.85 , 73.95 2021-02-12T23:18:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.17 , -0.08 2021-02-12T23:18:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171894.15 epoch sec; seqNo:2550; 4056656.24 north; 590737.03 east 2021-02-12T23:18:14Z- AHRS_M2 msg: 1613171894.48 epoch sec; seqNo:2526 2021-02-12T23:18:14Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-12T23:18:14Z- handleDepth()-Depth_Keller msg: 1613171894484 epoch sec; seqNo:2551; depth 11.68 2021-02-12T23:18:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171894.49 epoch sec; seqNo:2551; 4056656.24 north; 590737.03 east 2021-02-12T23:18:14Z- AHRS_M2 msg: 1613171894.88 epoch sec; seqNo:2527 2021-02-12T23:18:14Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-12T23:18:14Z- handleDepth()-Depth_Keller msg: 1613171894888 epoch sec; seqNo:2552; depth 11.58 2021-02-12T23:18:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:18:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:18:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:18:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:18:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:18:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:18:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:18:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:18:14Z- handleDvl() - RDI_Pathfinder msg: 1613171894.89 epoch sec; seqNo:1075 2021-02-12T23:18:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:18:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.17 , -0.08 2021-02-12T23:18:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171894.90 epoch sec; seqNo:2552; 4056656.63 north; 590738.24 east 2021-02-12T23:18:15Z- AHRS_M2 msg: 1613171895.29 epoch sec; seqNo:2528 2021-02-12T23:18:15Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-12T23:18:15Z- handleDepth()-Depth_Keller msg: 1613171895294 epoch sec; seqNo:2553; depth 11.46 2021-02-12T23:18:15Z- handleDvl() - RDI_Pathfinder msg: 1613171895.31 epoch sec; seqNo:1076 2021-02-12T23:18:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.83 , 99.05 , 112.57 , 75.18 2021-02-12T23:18:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.24 , -0.05 2021-02-12T23:18:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171895.32 epoch sec; seqNo:2553; 4056657.01 north; 590738.24 east 2021-02-12T23:18:15Z- AHRS_M2 msg: 1613171895.69 epoch sec; seqNo:2529 2021-02-12T23:18:15Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-12T23:18:15Z- handleDepth()-Depth_Keller msg: 1613171895710 epoch sec; seqNo:2554; depth 11.35 2021-02-12T23:18:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171895.73 epoch sec; seqNo:2554; 4056657.39 north; 590738.24 east 2021-02-12T23:18:16Z- AHRS_M2 msg: 1613171896.12 epoch sec; seqNo:2530 2021-02-12T23:18:16Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-12T23:18:16Z- handleDepth()-Depth_Keller msg: 1613171896129 epoch sec; seqNo:2555; depth 11.22 2021-02-12T23:18:16Z- handleDvl() - RDI_Pathfinder msg: 1613171896.14 epoch sec; seqNo:1077 2021-02-12T23:18:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.83 , 99.05 , 113.92 , 74.77 2021-02-12T23:18:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.27 , -0.03 2021-02-12T23:18:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171896.16 epoch sec; seqNo:2555; 4056657.39 north; 590738.24 east 2021-02-12T23:18:16Z- AHRS_M2 msg: 1613171896.50 epoch sec; seqNo:2531 2021-02-12T23:18:16Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-12T23:18:16Z- handleDepth()-Depth_Keller msg: 1613171896504 epoch sec; seqNo:2556; depth 11.10 2021-02-12T23:18:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171896.52 epoch sec; seqNo:2556; 4056657.76 north; 590738.23 east 2021-02-12T23:18:16Z- AHRS_M2 msg: 1613171896.90 epoch sec; seqNo:2532 2021-02-12T23:18:16Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-12T23:18:16Z- handleDepth()-Depth_Keller msg: 1613171896910 epoch sec; seqNo:2557; depth 10.98 2021-02-12T23:18:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171896.92 epoch sec; seqNo:2557; 4056658.14 north; 590738.23 east 2021-02-12T23:18:17Z- AHRS_M2 msg: 1613171897.31 epoch sec; seqNo:2533 2021-02-12T23:18:17Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-12T23:18:17Z- handleDepth()-Depth_Keller msg: 1613171897313 epoch sec; seqNo:2558; depth 10.85 2021-02-12T23:18:17Z- handleDvl() - RDI_Pathfinder msg: 1613171897.34 epoch sec; seqNo:1078 2021-02-12T23:18:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 98.99 , 113.85 , 73.95 2021-02-12T23:18:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.27 , 0.07 2021-02-12T23:18:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171897.35 epoch sec; seqNo:2558; 4056658.52 north; 590738.22 east 2021-02-12T23:18:17Z- AHRS_M2 msg: 1613171897.71 epoch sec; seqNo:2534 2021-02-12T23:18:17Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-12T23:18:17Z- handleDepth()-Depth_Keller msg: 1613171897716 epoch sec; seqNo:2559; depth 10.72 2021-02-12T23:18:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171897.73 epoch sec; seqNo:2559; 4056658.90 north; 590738.22 east 2021-02-12T23:18:18Z- AHRS_M2 msg: 1613171898.12 epoch sec; seqNo:2535 2021-02-12T23:18:18Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-12T23:18:18Z- handleDepth()-Depth_Keller msg: 1613171898123 epoch sec; seqNo:2560; depth 10.60 2021-02-12T23:18:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171898.13 epoch sec; seqNo:2560; 4056658.90 north; 590738.22 east 2021-02-12T23:18:18Z- AHRS_M2 msg: 1613171898.52 epoch sec; seqNo:2536 2021-02-12T23:18:18Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-12T23:18:18Z- handleDepth()-Depth_Keller msg: 1613171898534 epoch sec; seqNo:2561; depth 10.48 2021-02-12T23:18:18Z- handleDvl() - RDI_Pathfinder msg: 1613171898.55 epoch sec; seqNo:1079 2021-02-12T23:18:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 97.64 , 113.85 , 78.71 2021-02-12T23:18:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.19 , 0.21 2021-02-12T23:18:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171898.56 epoch sec; seqNo:2561; 4056659.28 north; 590738.22 east 2021-02-12T23:18:18Z- LcmTrn::handleMessages() - handling 45 messages took 0.76 ms and 130 calls... 2021-02-12T23:18:18Z- waiting for 1613171896.67, time is 1613171898.56 2021-02-12T23:18:18Z- LcmTrn::updateTrn() equating pose 1613171898.56 and meas times 1613171898.56 2021-02-12T23:18:18Z- LcmTrn::updateTrn() >>>> pose time 1613171898.56, meas time 1613171898.56 <<<< 2021-02-12T23:18:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171898.56 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:18:18Z- TerrainNav::motionUpdate() - currEstimate = (4056659, 590738.22, 10.48) 2021-02-12T23:18:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:18:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:18:18Z- TNavBF::Projecting Measurements 2021-02-12T23:18:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:18:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:18:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:18:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:18:18Z- TNavFilter::4 good beams. 2021-02-12T23:18:18Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:18:18Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:18:18Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:18:18Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:18:18Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:18:18Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:18:18Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:18:18Z- TNavBF::mmse selected from filter -1 2021-02-12T23:18:18Z- TerrainMapOctree: (vehN, vehE ) = (4056659.28, 590738.22). 2021-02-12T23:18:18Z- TerrainMapOctree: Min Distance = 5779.91. 2021-02-12T23:18:18Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:18:18Z- There is no specific good beam that all particles have in common. 2021-02-12T23:18:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:18:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 10.96 2021-02-12T23:18:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:18:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 15.25 2021-02-12T23:18:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:18:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 20.74 2021-02-12T23:18:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:18:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 9.91 2021-02-12T23:18:18Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:18:18Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:18:18Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:18:18Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:18:18Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:18:18Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:18:18Z- Filter 0 No beams used in subcloud update 2021-02-12T23:18:18Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:18:18Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:18:18Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 780.3 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:18:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171898.56 sec. 2021-02-12T23:18:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:18:18Z- TNavBF::mmse selected from filter -1 2021-02-12T23:18:18Z- variances([0], [2], and psi_berg) = 780.330752 833.068141 0.000000 2021-02-12T23:18:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:18:18Z- LcmTrn::publishEstimates() - MLE : 4056634.78 590787.72 10.48 2021-02-12T23:18:18Z- LcmTrn::publishEstimates() - MMSE : 4056659.28 590738.22 10.48 2021-02-12T23:18:18Z- LcmTrn::publishEstimates() - COVAR: 780.33 833.07 0.81 0.00 2021-02-12T23:18:18Z- AHRS_M2 msg: 1613171898.92 epoch sec; seqNo:2537 2021-02-12T23:18:18Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.14 psi 2021-02-12T23:18:18Z- handleDepth()-Depth_Keller msg: 1613171898927 epoch sec; seqNo:2562; depth 10.35 2021-02-12T23:18:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171898.94 epoch sec; seqNo:2562; 4056659.66 north; 590738.21 east 2021-02-12T23:18:19Z- AHRS_M2 msg: 1613171899.33 epoch sec; seqNo:2538 2021-02-12T23:18:19Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.13 psi 2021-02-12T23:18:19Z- handleDepth()-Depth_Keller msg: 1613171899334 epoch sec; seqNo:2563; depth 10.24 2021-02-12T23:18:19Z- handleDvl() - RDI_Pathfinder msg: 1613171899.35 epoch sec; seqNo:1080 2021-02-12T23:18:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.82 , 98.99 , 113.85 , 75.18 2021-02-12T23:18:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.32 2021-02-12T23:18:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171899.36 epoch sec; seqNo:2563; 4056660.04 north; 590738.21 east 2021-02-12T23:18:19Z- AHRS_M2 msg: 1613171899.73 epoch sec; seqNo:2539 2021-02-12T23:18:19Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.12 psi 2021-02-12T23:18:19Z- handleDepth()-Depth_Keller msg: 1613171899746 epoch sec; seqNo:2564; depth 10.12 2021-02-12T23:18:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171899.76 epoch sec; seqNo:2564; 4056660.43 north; 590739.42 east 2021-02-12T23:18:20Z- AHRS_M2 msg: 1613171900.15 epoch sec; seqNo:2540 2021-02-12T23:18:20Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.11 psi 2021-02-12T23:18:20Z- handleDepth()-Depth_Keller msg: 1613171900152 epoch sec; seqNo:2565; depth 10.00 2021-02-12T23:18:20Z- handleDvl() - RDI_Pathfinder msg: 1613171900.16 epoch sec; seqNo:1081 2021-02-12T23:18:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.11 , 97.57 , 115.14 , 75.58 2021-02-12T23:18:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.18 , 0.25 2021-02-12T23:18:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171900.18 epoch sec; seqNo:2565; 4056660.43 north; 590739.42 east 2021-02-12T23:18:20Z- AHRS_M2 msg: 1613171900.54 epoch sec; seqNo:2541 2021-02-12T23:18:20Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.12 psi 2021-02-12T23:18:20Z- handleDepth()-Depth_Keller msg: 1613171900546 epoch sec; seqNo:2566; depth 9.87 2021-02-12T23:18:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171900.56 epoch sec; seqNo:2566; 4056660.81 north; 590739.42 east 2021-02-12T23:18:20Z- AHRS_M2 msg: 1613171900.94 epoch sec; seqNo:2542 2021-02-12T23:18:20Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.12 psi 2021-02-12T23:18:20Z- handleDepth()-Depth_Keller msg: 1613171900954 epoch sec; seqNo:2567; depth 9.75 2021-02-12T23:18:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171900.96 epoch sec; seqNo:2567; 4056661.19 north; 590739.42 east 2021-02-12T23:18:21Z- AHRS_M2 msg: 1613171901.35 epoch sec; seqNo:2543 2021-02-12T23:18:21Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.12 psi 2021-02-12T23:18:21Z- handleDepth()-Depth_Keller msg: 1613171901353 epoch sec; seqNo:2568; depth 9.63 2021-02-12T23:18:21Z- handleDvl() - RDI_Pathfinder msg: 1613171901.37 epoch sec; seqNo:1082 2021-02-12T23:18:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.04 , 97.50 , 115.07 , 74.36 2021-02-12T23:18:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.22 , -0.01 2021-02-12T23:18:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171901.38 epoch sec; seqNo:2568; 4056661.20 north; 590740.64 east 2021-02-12T23:18:21Z- AHRS_M2 msg: 1613171901.75 epoch sec; seqNo:2544 2021-02-12T23:18:21Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.13 psi 2021-02-12T23:18:21Z- handleDepth()-Depth_Keller msg: 1613171901756 epoch sec; seqNo:2569; depth 9.49 2021-02-12T23:18:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171901.77 epoch sec; seqNo:2569; 4056661.58 north; 590740.63 east 2021-02-12T23:18:22Z- AHRS_M2 msg: 1613171902.16 epoch sec; seqNo:2545 2021-02-12T23:18:22Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.13 psi 2021-02-12T23:18:22Z- handleDepth()-Depth_Keller msg: 1613171902160 epoch sec; seqNo:2570; depth 9.37 2021-02-12T23:18:22Z- handleDvl() - RDI_Pathfinder msg: 1613171902.17 epoch sec; seqNo:1083 2021-02-12T23:18:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.11 , 98.92 , 116.49 , 76.40 2021-02-12T23:18:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.01 , -0.19 2021-02-12T23:18:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171902.18 epoch sec; seqNo:2570; 4056661.58 north; 590740.63 east 2021-02-12T23:18:22Z- AHRS_M2 msg: 1613171902.56 epoch sec; seqNo:2546 2021-02-12T23:18:22Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.14 psi 2021-02-12T23:18:22Z- handleDepth()-Depth_Keller msg: 1613171902573 epoch sec; seqNo:2571; depth 9.25 2021-02-12T23:18:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171902.58 epoch sec; seqNo:2571; 4056661.96 north; 590740.63 east 2021-02-12T23:18:22Z- AHRS_M2 msg: 1613171902.96 epoch sec; seqNo:2547 2021-02-12T23:18:22Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.14 psi 2021-02-12T23:18:22Z- handleDepth()-Depth_Keller msg: 1613171902973 epoch sec; seqNo:2572; depth 9.10 2021-02-12T23:18:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171902.99 epoch sec; seqNo:2572; 4056662.34 north; 590740.63 east 2021-02-12T23:18:23Z- AHRS_M2 msg: 1613171903.37 epoch sec; seqNo:2548 2021-02-12T23:18:23Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.15 psi 2021-02-12T23:18:23Z- handleDepth()-Depth_Keller msg: 1613171903375 epoch sec; seqNo:2573; depth 8.96 2021-02-12T23:18:23Z- handleDvl() - RDI_Pathfinder msg: 1613171903.39 epoch sec; seqNo:1084 2021-02-12T23:18:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.62 , 98.78 , 116.36 , 75.18 2021-02-12T23:18:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.21 , -0.11 2021-02-12T23:18:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171903.42 epoch sec; seqNo:2573; 4056662.34 north; 590740.63 east 2021-02-12T23:18:23Z- AHRS_M2 msg: 1613171903.77 epoch sec; seqNo:2549 2021-02-12T23:18:23Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.16 psi 2021-02-12T23:18:23Z- handleDepth()-Depth_Keller msg: 1613171903776 epoch sec; seqNo:2574; depth 8.83 2021-02-12T23:18:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171903.79 epoch sec; seqNo:2574; 4056662.72 north; 590740.62 east 2021-02-12T23:18:23Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 130 calls... 2021-02-12T23:18:23Z- waiting for 1613171901.56, time is 1613171903.79 2021-02-12T23:18:23Z- LcmTrn::updateTrn() >>>> pose time 1613171903.79, meas time 1613171903.39 <<<< 2021-02-12T23:18:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171903.39 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:18:23Z- TerrainNav::motionUpdate() - currEstimate = (4056663, 590740.62, 8.83) 2021-02-12T23:18:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:18:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:18:23Z- TNavBF::Projecting Measurements 2021-02-12T23:18:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:18:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:18:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:18:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:18:23Z- TNavFilter::4 good beams. 2021-02-12T23:18:23Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:18:23Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:18:23Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:18:23Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:18:23Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:18:23Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:18:23Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:18:23Z- TNavBF::mmse selected from filter -1 2021-02-12T23:18:23Z- TerrainMapOctree: (vehN, vehE ) = (4056662.45, 590740.44). 2021-02-12T23:18:23Z- TerrainMapOctree: Min Distance = 5776.04. 2021-02-12T23:18:23Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:18:23Z- There is no specific good beam that all particles have in common. 2021-02-12T23:18:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:18:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 12.00 2021-02-12T23:18:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:18:23Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 15.61 2021-02-12T23:18:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:18:23Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 21.66 2021-02-12T23:18:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:18:23Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 9.04 2021-02-12T23:18:23Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:18:23Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:18:23Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:18:23Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:18:23Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:18:23Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:18:23Z- Filter 0 No beams used in subcloud update 2021-02-12T23:18:23Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:18:23Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:18:23Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 780.2 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:18:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171903.39 sec. 2021-02-12T23:18:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:18:23Z- TNavBF::mmse selected from filter -1 2021-02-12T23:18:23Z- variances([0], [2], and psi_berg) = 780.136397 833.067860 0.000000 2021-02-12T23:18:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:18:23Z- LcmTrn::publishEstimates() - MLE : 4056639.22 590691.12 8.83 2021-02-12T23:18:23Z- LcmTrn::publishEstimates() - MMSE : 4056662.72 590740.62 8.83 2021-02-12T23:18:23Z- LcmTrn::publishEstimates() - COVAR: 780.14 833.07 0.81 0.00 2021-02-12T23:18:24Z- AHRS_M2 msg: 1613171904.18 epoch sec; seqNo:2550 2021-02-12T23:18:24Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.16 psi 2021-02-12T23:18:24Z- handleDepth()-Depth_Keller msg: 1613171904182 epoch sec; seqNo:2575; depth 8.70 2021-02-12T23:18:24Z- handleDvl() - RDI_Pathfinder msg: 1613171904.19 epoch sec; seqNo:1085 2021-02-12T23:18:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 101.56 , 119.13 , 75.58 2021-02-12T23:18:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.19 , 0.11 2021-02-12T23:18:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171904.20 epoch sec; seqNo:2575; 4056663.09 north; 590740.62 east 2021-02-12T23:18:24Z- AHRS_M2 msg: 1613171904.58 epoch sec; seqNo:2551 2021-02-12T23:18:24Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.16 psi 2021-02-12T23:18:24Z- handleDepth()-Depth_Keller msg: 1613171904602 epoch sec; seqNo:2576; depth 8.57 2021-02-12T23:18:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171904.62 epoch sec; seqNo:2576; 4056663.49 north; 590741.83 east 2021-02-12T23:18:24Z- AHRS_M2 msg: 1613171904.98 epoch sec; seqNo:2552 2021-02-12T23:18:24Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.16 psi 2021-02-12T23:18:24Z- handleDepth()-Depth_Keller msg: 1613171904993 epoch sec; seqNo:2577; depth 8.44 2021-02-12T23:18:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171905.00 epoch sec; seqNo:2577; 4056663.87 north; 590741.83 east 2021-02-12T23:18:25Z- AHRS_M2 msg: 1613171905.39 epoch sec; seqNo:2553 2021-02-12T23:18:25Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.16 psi 2021-02-12T23:18:25Z- handleDepth()-Depth_Keller msg: 1613171905394 epoch sec; seqNo:2578; depth 8.31 2021-02-12T23:18:25Z- handleDvl() - RDI_Pathfinder msg: 1613171905.41 epoch sec; seqNo:1086 2021-02-12T23:18:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.25 , 101.42 , 118.99 , 75.58 2021-02-12T23:18:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , 0.18 2021-02-12T23:18:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171905.42 epoch sec; seqNo:2578; 4056663.87 north; 590741.83 east 2021-02-12T23:18:25Z- AHRS_M2 msg: 1613171905.79 epoch sec; seqNo:2554 2021-02-12T23:18:25Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.16 psi 2021-02-12T23:18:25Z- handleDepth()-Depth_Keller msg: 1613171905798 epoch sec; seqNo:2579; depth 8.17 2021-02-12T23:18:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171905.81 epoch sec; seqNo:2579; 4056664.24 north; 590741.83 east 2021-02-12T23:18:26Z- AHRS_M2 msg: 1613171906.19 epoch sec; seqNo:2555 2021-02-12T23:18:26Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.17 psi 2021-02-12T23:18:26Z- handleDepth()-Depth_Keller msg: 1613171906201 epoch sec; seqNo:2580; depth 8.04 2021-02-12T23:18:26Z- handleDvl() - RDI_Pathfinder msg: 1613171906.22 epoch sec; seqNo:1087 2021-02-12T23:18:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.36 , 102.77 , 117.64 , 77.22 2021-02-12T23:18:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.02 , 0.03 2021-02-12T23:18:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171906.23 epoch sec; seqNo:2580; 4056664.62 north; 590741.82 east 2021-02-12T23:18:26Z- AHRS_M2 msg: 1613171906.60 epoch sec; seqNo:2556 2021-02-12T23:18:26Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.18 psi 2021-02-12T23:18:26Z- handleDepth()-Depth_Keller msg: 1613171906604 epoch sec; seqNo:2581; depth 7.90 2021-02-12T23:18:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171906.62 epoch sec; seqNo:2581; 4056664.62 north; 590741.82 east 2021-02-12T23:18:27Z- AHRS_M2 msg: 1613171907.00 epoch sec; seqNo:2557 2021-02-12T23:18:27Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.19 psi 2021-02-12T23:18:27Z- handleDepth()-Depth_Keller msg: 1613171907010 epoch sec; seqNo:2582; depth 7.77 2021-02-12T23:18:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171907.02 epoch sec; seqNo:2582; 4056665.01 north; 590743.04 east 2021-02-12T23:18:27Z- AHRS_M2 msg: 1613171907.41 epoch sec; seqNo:2558 2021-02-12T23:18:27Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.20 psi 2021-02-12T23:18:27Z- handleDepth()-Depth_Keller msg: 1613171907413 epoch sec; seqNo:2583; depth 7.65 2021-02-12T23:18:27Z- handleDvl() - RDI_Pathfinder msg: 1613171907.45 epoch sec; seqNo:1088 2021-02-12T23:18:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.63 , 103.85 , 118.72 , 78.17 2021-02-12T23:18:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.04 , -0.05 2021-02-12T23:18:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171907.46 epoch sec; seqNo:2583; 4056665.39 north; 590743.04 east 2021-02-12T23:18:27Z- AHRS_M2 msg: 1613171907.81 epoch sec; seqNo:2559 2021-02-12T23:18:27Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.20 psi 2021-02-12T23:18:27Z- handleDepth()-Depth_Keller msg: 1613171907821 epoch sec; seqNo:2584; depth 7.50 2021-02-12T23:18:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171907.83 epoch sec; seqNo:2584; 4056665.39 north; 590743.04 east 2021-02-12T23:18:28Z- AHRS_M2 msg: 1613171908.26 epoch sec; seqNo:2560 2021-02-12T23:18:28Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.20 psi 2021-02-12T23:18:28Z- handleDepth()-Depth_Keller msg: 1613171908273 epoch sec; seqNo:2585; depth 7.39 2021-02-12T23:18:28Z- handleDvl() - RDI_Pathfinder msg: 1613171908.29 epoch sec; seqNo:1089 2021-02-12T23:18:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 105.41 , 118.92 , 78.38 2021-02-12T23:18:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.09 , -0.08 2021-02-12T23:18:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171908.30 epoch sec; seqNo:2585; 4056665.77 north; 590743.03 east 2021-02-12T23:18:28Z- AHRS_M2 msg: 1613171908.62 epoch sec; seqNo:2561 2021-02-12T23:18:28Z- AHRS_M2 msg: 0.68 phi; 0.34 theta; -0.18 psi 2021-02-12T23:18:28Z- handleDepth()-Depth_Keller msg: 1613171908629 epoch sec; seqNo:2586; depth 7.25 2021-02-12T23:18:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171908.64 epoch sec; seqNo:2586; 4056666.15 north; 590743.03 east 2021-02-12T23:18:28Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-12T23:18:28Z- waiting for 1613171906.79, time is 1613171908.64 2021-02-12T23:18:28Z- LcmTrn::updateTrn() >>>> pose time 1613171908.64, meas time 1613171908.29 <<<< 2021-02-12T23:18:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171908.29 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:18:28Z- TerrainNav::motionUpdate() - currEstimate = (4056666, 590743.03, 7.25) 2021-02-12T23:18:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:18:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:18:28Z- TNavBF::Projecting Measurements 2021-02-12T23:18:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:18:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:18:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:18:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:18:28Z- TNavFilter::4 good beams. 2021-02-12T23:18:28Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:18:28Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:18:28Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:18:28Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:18:28Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:18:28Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:18:28Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:18:28Z- TNavBF::mmse selected from filter -1 2021-02-12T23:18:28Z- TerrainMapOctree: (vehN, vehE ) = (4056665.90, 590742.85). 2021-02-12T23:18:28Z- TerrainMapOctree: Min Distance = 5771.83. 2021-02-12T23:18:28Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:18:28Z- There is no specific good beam that all particles have in common. 2021-02-12T23:18:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:18:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 11.97 2021-02-12T23:18:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:18:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 17.78 2021-02-12T23:18:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:18:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.035 ExpectedMeasDiff: nan currMeas.covariance: 22.63 2021-02-12T23:18:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:18:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 9.83 2021-02-12T23:18:29Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:18:29Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:18:29Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:18:29Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:18:29Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:18:29Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:18:29Z- Filter 0 No beams used in subcloud update 2021-02-12T23:18:29Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:18:29Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:18:29Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 780.0 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:18:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171908.29 sec. 2021-02-12T23:18:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:18:29Z- TNavBF::mmse selected from filter -1 2021-02-12T23:18:29Z- variances([0], [2], and psi_berg) = 779.943511 833.067581 0.000000 2021-02-12T23:18:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:18:29Z- LcmTrn::publishEstimates() - MLE : 4056642.65 590693.53 7.25 2021-02-12T23:18:29Z- LcmTrn::publishEstimates() - MMSE : 4056666.15 590743.03 7.25 2021-02-12T23:18:29Z- LcmTrn::publishEstimates() - COVAR: 779.94 833.07 0.82 0.00 2021-02-12T23:18:29Z- AHRS_M2 msg: 1613171909.02 epoch sec; seqNo:2562 2021-02-12T23:18:29Z- AHRS_M2 msg: 0.69 phi; 0.33 theta; -0.17 psi 2021-02-12T23:18:29Z- handleDepth()-Depth_Keller msg: 1613171909028 epoch sec; seqNo:2587; depth 7.16 2021-02-12T23:18:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171909.04 epoch sec; seqNo:2587; 4056666.15 north; 590743.03 east 2021-02-12T23:18:29Z- AHRS_M2 msg: 1613171909.44 epoch sec; seqNo:2563 2021-02-12T23:18:29Z- AHRS_M2 msg: 0.70 phi; 0.33 theta; -0.16 psi 2021-02-12T23:18:29Z- handleDepth()-Depth_Keller msg: 1613171909448 epoch sec; seqNo:2588; depth 7.04 2021-02-12T23:18:29Z- handleDvl() - RDI_Pathfinder msg: 1613171909.47 epoch sec; seqNo:1090 2021-02-12T23:18:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.42 , 106.69 , 117.50 , 77.63 2021-02-12T23:18:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.14 , -0.05 2021-02-12T23:18:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171909.50 epoch sec; seqNo:2588; 4056666.53 north; 590743.02 east 2021-02-12T23:18:29Z- AHRS_M2 msg: 1613171909.83 epoch sec; seqNo:2564 2021-02-12T23:18:29Z- AHRS_M2 msg: 0.69 phi; 0.32 theta; -0.15 psi 2021-02-12T23:18:29Z- handleDepth()-Depth_Keller msg: 1613171909836 epoch sec; seqNo:2589; depth 6.92 2021-02-12T23:18:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171909.85 epoch sec; seqNo:2589; 4056666.92 north; 590744.24 east 2021-02-12T23:18:30Z- AHRS_M2 msg: 1613171910.25 epoch sec; seqNo:2565 2021-02-12T23:18:30Z- AHRS_M2 msg: 0.69 phi; 0.31 theta; -0.15 psi 2021-02-12T23:18:30Z- handleDepth()-Depth_Keller msg: 1613171910266 epoch sec; seqNo:2590; depth 6.83 2021-02-12T23:18:30Z- handleDvl() - RDI_Pathfinder msg: 1613171910.28 epoch sec; seqNo:1091 2021-02-12T23:18:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.84 , 102.70 , 116.22 , 78.38 2021-02-12T23:18:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.21 , 0.01 2021-02-12T23:18:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171910.29 epoch sec; seqNo:2590; 4056666.92 north; 590744.24 east 2021-02-12T23:18:30Z- AHRS_M2 msg: 1613171910.64 epoch sec; seqNo:2566 2021-02-12T23:18:30Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.16 psi 2021-02-12T23:18:30Z- handleDepth()-Depth_Keller msg: 1613171910644 epoch sec; seqNo:2591; depth 6.69 2021-02-12T23:18:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171910.65 epoch sec; seqNo:2591; 4056667.30 north; 590744.24 east 2021-02-12T23:18:31Z- AHRS_M2 msg: 1613171911.05 epoch sec; seqNo:2567 2021-02-12T23:18:31Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.18 psi 2021-02-12T23:18:31Z- handleDepth()-Depth_Keller msg: 1613171911049 epoch sec; seqNo:2592; depth 6.60 2021-02-12T23:18:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171911.06 epoch sec; seqNo:2592; 4056667.68 north; 590744.23 east 2021-02-12T23:18:31Z- AHRS_M2 msg: 1613171911.45 epoch sec; seqNo:2568 2021-02-12T23:18:31Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.19 psi 2021-02-12T23:18:31Z- handleDepth()-Depth_Keller msg: 1613171911456 epoch sec; seqNo:2593; depth 6.49 2021-02-12T23:18:31Z- handleDvl() - RDI_Pathfinder msg: 1613171911.48 epoch sec; seqNo:1092 2021-02-12T23:18:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.55 , 104.12 , 114.94 , 78.85 2021-02-12T23:18:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.15 , 0.11 2021-02-12T23:18:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171911.49 epoch sec; seqNo:2593; 4056668.06 north; 590744.23 east 2021-02-12T23:18:31Z- AHRS_M2 msg: 1613171911.85 epoch sec; seqNo:2569 2021-02-12T23:18:31Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.20 psi 2021-02-12T23:18:31Z- handleDepth()-Depth_Keller msg: 1613171911865 epoch sec; seqNo:2594; depth 6.39 2021-02-12T23:18:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171911.88 epoch sec; seqNo:2594; 4056668.44 north; 590744.22 east 2021-02-12T23:18:32Z- AHRS_M2 msg: 1613171912.27 epoch sec; seqNo:2570 2021-02-12T23:18:32Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.20 psi 2021-02-12T23:18:32Z- handleDepth()-Depth_Keller msg: 1613171912269 epoch sec; seqNo:2595; depth 6.29 2021-02-12T23:18:32Z- handleDvl() - RDI_Pathfinder msg: 1613171912.28 epoch sec; seqNo:1093 2021-02-12T23:18:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.55 , 106.83 , 117.64 , 81.15 2021-02-12T23:18:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , 0.15 2021-02-12T23:18:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171912.29 epoch sec; seqNo:2595; 4056668.44 north; 590744.22 east 2021-02-12T23:18:32Z- AHRS_M2 msg: 1613171912.66 epoch sec; seqNo:2571 2021-02-12T23:18:32Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.19 psi 2021-02-12T23:18:32Z- handleDepth()-Depth_Keller msg: 1613171912666 epoch sec; seqNo:2596; depth 6.19 2021-02-12T23:18:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171912.68 epoch sec; seqNo:2596; 4056668.82 north; 590744.22 east 2021-02-12T23:18:33Z- AHRS_M2 msg: 1613171913.07 epoch sec; seqNo:2572 2021-02-12T23:18:33Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.19 psi 2021-02-12T23:18:33Z- handleDepth()-Depth_Keller msg: 1613171913074 epoch sec; seqNo:2597; depth 6.09 2021-02-12T23:18:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171913.10 epoch sec; seqNo:2597; 4056669.21 north; 590745.44 east 2021-02-12T23:18:33Z- AHRS_M2 msg: 1613171913.47 epoch sec; seqNo:2573 2021-02-12T23:18:33Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.20 psi 2021-02-12T23:18:33Z- handleDepth()-Depth_Keller msg: 1613171913475 epoch sec; seqNo:2598; depth 5.97 2021-02-12T23:18:33Z- handleDvl() - RDI_Pathfinder msg: 1613171913.49 epoch sec; seqNo:1094 2021-02-12T23:18:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.70 , 105.27 , 120.14 , 79.59 2021-02-12T23:18:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.14 , 0.01 2021-02-12T23:18:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171913.52 epoch sec; seqNo:2598; 4056669.21 north; 590745.44 east 2021-02-12T23:18:33Z- AHRS_M2 msg: 1613171913.87 epoch sec; seqNo:2574 2021-02-12T23:18:33Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.21 psi 2021-02-12T23:18:33Z- handleDepth()-Depth_Keller msg: 1613171913876 epoch sec; seqNo:2599; depth 5.87 2021-02-12T23:18:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171913.89 epoch sec; seqNo:2599; 4056669.59 north; 590745.43 east 2021-02-12T23:18:34Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-12T23:18:34Z- waiting for 1613171911.64, time is 1613171913.89 2021-02-12T23:18:34Z- LcmTrn::updateTrn() >>>> pose time 1613171913.89, meas time 1613171913.49 <<<< 2021-02-12T23:18:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171913.49 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:18:34Z- TerrainNav::motionUpdate() - currEstimate = (4056670, 590745.43, 5.87) 2021-02-12T23:18:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:18:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:18:34Z- TNavBF::Projecting Measurements 2021-02-12T23:18:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:18:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:18:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:18:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:18:34Z- TNavFilter::4 good beams. 2021-02-12T23:18:34Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:18:34Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:18:34Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:18:34Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:18:34Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:18:34Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:18:34Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:18:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:18:34Z- TerrainMapOctree: (vehN, vehE ) = (4056669.32, 590745.25). 2021-02-12T23:18:34Z- TerrainMapOctree: Min Distance = 5767.66. 2021-02-12T23:18:34Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:18:34Z- There is no specific good beam that all particles have in common. 2021-02-12T23:18:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:18:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 12.31 2021-02-12T23:18:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:18:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 17.73 2021-02-12T23:18:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:18:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.035 ExpectedMeasDiff: nan currMeas.covariance: 23.09 2021-02-12T23:18:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:18:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 10.14 2021-02-12T23:18:34Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:18:34Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:18:34Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:18:34Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:18:34Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:18:34Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:18:34Z- Filter 0 No beams used in subcloud update 2021-02-12T23:18:34Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:18:34Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:18:34Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 779.8 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:18:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171913.49 sec. 2021-02-12T23:18:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:18:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:18:34Z- variances([0], [2], and psi_berg) = 779.750469 833.067304 0.000000 2021-02-12T23:18:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:18:34Z- LcmTrn::publishEstimates() - MLE : 4056646.09 590695.93 5.87 2021-02-12T23:18:34Z- LcmTrn::publishEstimates() - MMSE : 4056669.59 590745.43 5.87 2021-02-12T23:18:34Z- LcmTrn::publishEstimates() - COVAR: 779.75 833.07 0.83 0.00 2021-02-12T23:18:34Z- AHRS_M2 msg: 1613171914.28 epoch sec; seqNo:2575 2021-02-12T23:18:34Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.21 psi 2021-02-12T23:18:34Z- handleDepth()-Depth_Keller msg: 1613171914282 epoch sec; seqNo:2600; depth 5.73 2021-02-12T23:18:34Z- handleDvl() - RDI_Pathfinder msg: 1613171914.29 epoch sec; seqNo:1095 2021-02-12T23:18:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.70 , 109.33 , 117.44 , 78.04 2021-02-12T23:18:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , 0.06 , -0.03 2021-02-12T23:18:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171914.30 epoch sec; seqNo:2600; 4056669.59 north; 590745.43 east 2021-02-12T23:18:34Z- AHRS_M2 msg: 1613171914.68 epoch sec; seqNo:2576 2021-02-12T23:18:34Z- AHRS_M2 msg: 0.62 phi; 0.28 theta; -0.22 psi 2021-02-12T23:18:34Z- handleDepth()-Depth_Keller msg: 1613171914685 epoch sec; seqNo:2601; depth 5.60 2021-02-12T23:18:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171914.70 epoch sec; seqNo:2601; 4056669.97 north; 590745.43 east 2021-02-12T23:18:35Z- AHRS_M2 msg: 1613171915.08 epoch sec; seqNo:2577 2021-02-12T23:18:35Z- AHRS_M2 msg: 0.62 phi; 0.25 theta; -0.22 psi 2021-02-12T23:18:35Z- handleDepth()-Depth_Keller msg: 1613171915088 epoch sec; seqNo:2602; depth 5.48 2021-02-12T23:18:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171915.10 epoch sec; seqNo:2602; 4056669.97 north; 590745.43 east 2021-02-12T23:18:35Z- AHRS_M2 msg: 1613171915.49 epoch sec; seqNo:2578 2021-02-12T23:18:35Z- AHRS_M2 msg: 0.62 phi; 0.22 theta; -0.22 psi 2021-02-12T23:18:35Z- handleDepth()-Depth_Keller msg: 1613171915499 epoch sec; seqNo:2603; depth 5.35 2021-02-12T23:18:35Z- handleDvl() - RDI_Pathfinder msg: 1613171915.51 epoch sec; seqNo:1096 2021-02-12T23:18:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.35 , 109.33 , 113.38 , 82.29 2021-02-12T23:18:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , 0.03 , 0.03 2021-02-12T23:18:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171915.52 epoch sec; seqNo:2603; 4056670.36 north; 590746.65 east 2021-02-12T23:18:35Z- AHRS_M2 msg: 1613171915.89 epoch sec; seqNo:2579 2021-02-12T23:18:35Z- AHRS_M2 msg: 0.63 phi; 0.18 theta; -0.22 psi 2021-02-12T23:18:35Z- handleDepth()-Depth_Keller msg: 1613171915898 epoch sec; seqNo:2604; depth 5.22 2021-02-12T23:18:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171915.91 epoch sec; seqNo:2604; 4056670.36 north; 590746.65 east 2021-02-12T23:18:36Z- AHRS_M2 msg: 1613171916.30 epoch sec; seqNo:2580 2021-02-12T23:18:36Z- AHRS_M2 msg: 0.64 phi; 0.14 theta; -0.23 psi 2021-02-12T23:18:36Z- handleDepth()-Depth_Keller msg: 1613171916305 epoch sec; seqNo:2605; depth 5.11 2021-02-12T23:18:36Z- handleDvl() - RDI_Pathfinder msg: 1613171916.33 epoch sec; seqNo:1097 2021-02-12T23:18:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.71 , 108.04 , 105.34 , 0.00 2021-02-12T23:18:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.05 , -0.03 2021-02-12T23:18:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171916.34 epoch sec; seqNo:2605; 4056670.74 north; 590746.64 east 2021-02-12T23:18:36Z- AHRS_M2 msg: 1613171916.70 epoch sec; seqNo:2581 2021-02-12T23:18:36Z- AHRS_M2 msg: 0.65 phi; 0.10 theta; -0.23 psi 2021-02-12T23:18:36Z- handleDepth()-Depth_Keller msg: 1613171916711 epoch sec; seqNo:2606; depth 4.99 2021-02-12T23:18:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171916.72 epoch sec; seqNo:2606; 4056671.11 north; 590746.64 east 2021-02-12T23:18:37Z- AHRS_M2 msg: 1613171917.11 epoch sec; seqNo:2582 2021-02-12T23:18:37Z- AHRS_M2 msg: 0.67 phi; 0.00 theta; -0.22 psi 2021-02-12T23:18:37Z- handleDepth()-Depth_Keller msg: 1613171917116 epoch sec; seqNo:2607; depth 4.93 2021-02-12T23:18:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171917.13 epoch sec; seqNo:2607; 4056671.11 north; 590746.64 east 2021-02-12T23:18:37Z- AHRS_M2 msg: 1613171917.52 epoch sec; seqNo:2583 2021-02-12T23:18:37Z- AHRS_M2 msg: 0.69 phi; -0.05 theta; -0.21 psi 2021-02-12T23:18:37Z- handleDepth()-Depth_Keller msg: 1613171917523 epoch sec; seqNo:2608; depth 4.85 2021-02-12T23:18:37Z- handleDvl() - RDI_Pathfinder msg: 1613171917.54 epoch sec; seqNo:1098 2021-02-12T23:18:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.23 , 107.91 , 95.74 , 88.99 2021-02-12T23:18:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.15 , 0.03 2021-02-12T23:18:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171917.55 epoch sec; seqNo:2608; 4056671.49 north; 590746.63 east 2021-02-12T23:18:37Z- AHRS_M2 msg: 1613171917.91 epoch sec; seqNo:2584 2021-02-12T23:18:37Z- AHRS_M2 msg: 0.71 phi; -0.10 theta; -0.19 psi 2021-02-12T23:18:37Z- handleDepth()-Depth_Keller msg: 1613171917916 epoch sec; seqNo:2609; depth 4.82 2021-02-12T23:18:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171917.93 epoch sec; seqNo:2609; 4056671.87 north; 590746.63 east 2021-02-12T23:18:38Z- AHRS_M2 msg: 1613171918.32 epoch sec; seqNo:2585 2021-02-12T23:18:38Z- AHRS_M2 msg: 0.72 phi; -0.15 theta; -0.18 psi 2021-02-12T23:18:38Z- handleDepth()-Depth_Keller msg: 1613171918325 epoch sec; seqNo:2610; depth 4.77 2021-02-12T23:18:38Z- handleDvl() - RDI_Pathfinder msg: 1613171918.34 epoch sec; seqNo:1099 2021-02-12T23:18:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.70 , 102.97 , 89.46 , 97.57 2021-02-12T23:18:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.14 , 0.03 2021-02-12T23:18:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171918.36 epoch sec; seqNo:2610; 4056672.25 north; 590746.63 east 2021-02-12T23:18:38Z- AHRS_M2 msg: 1613171918.72 epoch sec; seqNo:2586 2021-02-12T23:18:38Z- AHRS_M2 msg: 0.73 phi; -0.19 theta; -0.17 psi 2021-02-12T23:18:38Z- handleDepth()-Depth_Keller msg: 1613171918732 epoch sec; seqNo:2611; depth 4.75 2021-02-12T23:18:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171918.74 epoch sec; seqNo:2611; 4056672.26 north; 590747.85 east 2021-02-12T23:18:39Z- AHRS_M2 msg: 1613171919.14 epoch sec; seqNo:2587 2021-02-12T23:18:39Z- AHRS_M2 msg: 0.74 phi; -0.22 theta; -0.17 psi 2021-02-12T23:18:39Z- LcmTrn::handleMessages() - handling 42 messages took 0.67 ms and 129 calls... 2021-02-12T23:18:39Z- waiting for 1613171916.89, time is 1613171919.14 2021-02-12T23:18:39Z- LcmTrn::updateTrn() >>>> pose time 1613171919.14, meas time 1613171918.34 <<<< 2021-02-12T23:18:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171918.34 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:18:39Z- TerrainNav::motionUpdate() - currEstimate = (4056672, 590747.85, 4.75) 2021-02-12T23:18:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:18:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:18:39Z- TNavBF::Projecting Measurements 2021-02-12T23:18:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:18:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:18:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:18:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:18:39Z- TNavFilter::4 good beams. 2021-02-12T23:18:39Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:18:39Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:18:39Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:18:39Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:18:39Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:18:39Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:18:39Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:18:39Z- TNavBF::mmse selected from filter -1 2021-02-12T23:18:39Z- TerrainMapOctree: (vehN, vehE ) = (4056671.86, 590747.48). 2021-02-12T23:18:39Z- TerrainMapOctree: Min Distance = 5764.29. 2021-02-12T23:18:39Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:18:39Z- There is no specific good beam that all particles have in common. 2021-02-12T23:18:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:18:39Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 10.94 2021-02-12T23:18:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:18:39Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 16.97 2021-02-12T23:18:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:18:39Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 12.80 2021-02-12T23:18:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:18:39Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 15.23 2021-02-12T23:18:39Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:18:39Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:18:39Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:18:39Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:18:39Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:18:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:18:39Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:18:39Z- Filter 0 No beams used in subcloud update 2021-02-12T23:18:39Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:18:39Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:18:39Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 779.6 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:18:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171918.34 sec. 2021-02-12T23:18:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:18:39Z- TNavBF::mmse selected from filter -1 2021-02-12T23:18:39Z- variances([0], [2], and psi_berg) = 779.561487 833.067033 0.000000 2021-02-12T23:18:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:18:39Z- LcmTrn::publishEstimates() - MLE : 4056648.76 590698.35 4.75 2021-02-12T23:18:39Z- LcmTrn::publishEstimates() - MMSE : 4056672.26 590747.85 4.75 2021-02-12T23:18:39Z- LcmTrn::publishEstimates() - COVAR: 779.56 833.07 0.81 0.00 2021-02-12T23:18:39Z- handleDepth()-Depth_Keller msg: 1613171919144 epoch sec; seqNo:2612; depth 4.76 2021-02-12T23:18:39Z- handleDvl() - RDI_Pathfinder msg: 1613171919.16 epoch sec; seqNo:1100 2021-02-12T23:18:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 103.04 , 0.00 , 104.39 2021-02-12T23:18:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.20 , 0.11 2021-02-12T23:18:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171919.17 epoch sec; seqNo:2612; 4056672.64 north; 590747.84 east 2021-02-12T23:18:39Z- AHRS_M2 msg: 1613171919.53 epoch sec; seqNo:2588 2021-02-12T23:18:39Z- AHRS_M2 msg: 0.74 phi; -0.25 theta; -0.17 psi 2021-02-12T23:18:39Z- handleDepth()-Depth_Keller msg: 1613171919532 epoch sec; seqNo:2613; depth 4.77 2021-02-12T23:18:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171919.55 epoch sec; seqNo:2613; 4056673.02 north; 590747.84 east 2021-02-12T23:18:39Z- AHRS_M2 msg: 1613171919.93 epoch sec; seqNo:2589 2021-02-12T23:18:39Z- AHRS_M2 msg: 0.73 phi; -0.27 theta; -0.16 psi 2021-02-12T23:18:39Z- handleDepth()-Depth_Keller msg: 1613171919936 epoch sec; seqNo:2614; depth 4.81 2021-02-12T23:18:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171919.94 epoch sec; seqNo:2614; 4056673.02 north; 590747.84 east 2021-02-12T23:18:40Z- AHRS_M2 msg: 1613171920.34 epoch sec; seqNo:2590 2021-02-12T23:18:40Z- AHRS_M2 msg: 0.72 phi; -0.28 theta; -0.16 psi 2021-02-12T23:18:40Z- handleDepth()-Depth_Keller msg: 1613171920340 epoch sec; seqNo:2615; depth 4.86 2021-02-12T23:18:40Z- handleDvl() - RDI_Pathfinder msg: 1613171920.35 epoch sec; seqNo:1101 2021-02-12T23:18:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.07 , 103.99 , 82.36 , 110.75 2021-02-12T23:18:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.06 , 0.22 2021-02-12T23:18:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171920.36 epoch sec; seqNo:2615; 4056673.40 north; 590747.84 east 2021-02-12T23:18:40Z- AHRS_M2 msg: 1613171920.74 epoch sec; seqNo:2591 2021-02-12T23:18:40Z- AHRS_M2 msg: 0.70 phi; -0.29 theta; -0.16 psi 2021-02-12T23:18:40Z- handleDepth()-Depth_Keller msg: 1613171920753 epoch sec; seqNo:2616; depth 4.93 2021-02-12T23:18:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171920.76 epoch sec; seqNo:2616; 4056673.40 north; 590747.84 east 2021-02-12T23:18:41Z- AHRS_M2 msg: 1613171921.15 epoch sec; seqNo:2592 2021-02-12T23:18:41Z- AHRS_M2 msg: 0.69 phi; -0.29 theta; -0.19 psi 2021-02-12T23:18:41Z- handleDepth()-Depth_Keller msg: 1613171921163 epoch sec; seqNo:2617; depth 4.99 2021-02-12T23:18:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171921.20 epoch sec; seqNo:2617; 4056673.78 north; 590747.83 east 2021-02-12T23:18:41Z- AHRS_M2 msg: 1613171921.55 epoch sec; seqNo:2593 2021-02-12T23:18:41Z- AHRS_M2 msg: 0.68 phi; -0.30 theta; -0.22 psi 2021-02-12T23:18:41Z- handleDepth()-Depth_Keller msg: 1613171921557 epoch sec; seqNo:2618; depth 5.05 2021-02-12T23:18:41Z- handleDvl() - RDI_Pathfinder msg: 1613171921.58 epoch sec; seqNo:1102 2021-02-12T23:18:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.97 , 104.19 , 81.21 , 115.00 2021-02-12T23:18:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.08 , 0.14 2021-02-12T23:18:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171921.59 epoch sec; seqNo:2618; 4056673.78 north; 590747.83 east 2021-02-12T23:18:41Z- AHRS_M2 msg: 1613171921.95 epoch sec; seqNo:2594 2021-02-12T23:18:41Z- AHRS_M2 msg: 0.67 phi; -0.30 theta; -0.25 psi 2021-02-12T23:18:41Z- handleDepth()-Depth_Keller msg: 1613171921956 epoch sec; seqNo:2619; depth 5.14 2021-02-12T23:18:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171921.97 epoch sec; seqNo:2619; 4056674.17 north; 590749.05 east 2021-02-12T23:18:42Z- AHRS_M2 msg: 1613171922.36 epoch sec; seqNo:2595 2021-02-12T23:18:42Z- AHRS_M2 msg: 0.66 phi; -0.31 theta; -0.27 psi 2021-02-12T23:18:42Z- handleDepth()-Depth_Keller msg: 1613171922362 epoch sec; seqNo:2620; depth 5.21 2021-02-12T23:18:42Z- handleDvl() - RDI_Pathfinder msg: 1613171922.37 epoch sec; seqNo:1103 2021-02-12T23:18:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 108.11 , 82.43 , 116.22 2021-02-12T23:18:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.12 , 0.02 2021-02-12T23:18:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171922.38 epoch sec; seqNo:2620; 4056674.17 north; 590749.05 east 2021-02-12T23:18:42Z- AHRS_M2 msg: 1613171922.76 epoch sec; seqNo:2596 2021-02-12T23:18:42Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.28 psi 2021-02-12T23:18:42Z- handleDepth()-Depth_Keller msg: 1613171922774 epoch sec; seqNo:2621; depth 5.30 2021-02-12T23:18:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171922.79 epoch sec; seqNo:2621; 4056674.55 north; 590749.04 east 2021-02-12T23:18:43Z- AHRS_M2 msg: 1613171923.16 epoch sec; seqNo:2597 2021-02-12T23:18:43Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.29 psi 2021-02-12T23:18:43Z- handleDepth()-Depth_Keller msg: 1613171923168 epoch sec; seqNo:2622; depth 5.42 2021-02-12T23:18:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171923.18 epoch sec; seqNo:2622; 4056674.93 north; 590749.04 east 2021-02-12T23:18:43Z- AHRS_M2 msg: 1613171923.57 epoch sec; seqNo:2598 2021-02-12T23:18:43Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.30 psi 2021-02-12T23:18:43Z- handleDepth()-Depth_Keller msg: 1613171923577 epoch sec; seqNo:2623; depth 5.50 2021-02-12T23:18:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171923.65 epoch sec; seqNo:2623; 4056674.93 north; 590749.04 east 2021-02-12T23:18:43Z- AHRS_M2 msg: 1613171923.98 epoch sec; seqNo:2599 2021-02-12T23:18:43Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.30 psi 2021-02-12T23:18:43Z- handleDepth()-Depth_Keller msg: 1613171923986 epoch sec; seqNo:2624; depth 5.63 2021-02-12T23:18:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:18:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:18:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:18:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:18:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:18:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:18:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:18:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:18:44Z- handleDvl() - RDI_Pathfinder msg: 1613171924.06 epoch sec; seqNo:1104 2021-02-12T23:18:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:18:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.12 , 0.02 2021-02-12T23:18:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171924.07 epoch sec; seqNo:2624; 4056675.32 north; 590750.26 east 2021-02-12T23:18:44Z- LcmTrn::handleMessages() - handling 43 messages took 0.65 ms and 130 calls... 2021-02-12T23:18:44Z- waiting for 1613171922.14, time is 1613171924.07 2021-02-12T23:18:44Z- LcmTrn::updateTrn() equating pose 1613171924.07 and meas times 1613171924.07 2021-02-12T23:18:44Z- LcmTrn::updateTrn() >>>> pose time 1613171924.07, meas time 1613171924.07 <<<< 2021-02-12T23:18:44Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613171924.07, ping # 0. 2021-02-12T23:18:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171924.07 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:18:44Z- TerrainNav::motionUpdate() - currEstimate = (4056675, 590750.26, 5.63) 2021-02-12T23:18:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:18:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:18:44Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:18:44Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:18:44Z- TNavBF::Projecting Measurements 2021-02-12T23:18:44Z- TNavFilter::Measurement from time = 1613171924.07 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:18:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:18:44Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:18:44Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:18:44Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:18:44Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:18:44Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:18:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:18:44Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 779.4 EastVariance: 833.1 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:18:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:18:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:18:44Z- variances([0], [2], and psi_berg) = 779.412100 833.066819 0.000000 2021-02-12T23:18:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:18:44Z- LcmTrn::publishEstimates() - MLE : 4056651.82 590700.76 5.63 2021-02-12T23:18:44Z- LcmTrn::publishEstimates() - MMSE : 4056675.32 590750.26 5.63 2021-02-12T23:18:44Z- LcmTrn::publishEstimates() - COVAR: 779.41 833.07 1.01 0.00 2021-02-12T23:18:44Z- AHRS_M2 msg: 1613171924.38 epoch sec; seqNo:2600 2021-02-12T23:18:44Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.29 psi 2021-02-12T23:18:44Z- handleDepth()-Depth_Keller msg: 1613171924382 epoch sec; seqNo:2625; depth 5.73 2021-02-12T23:18:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171924.39 epoch sec; seqNo:2625; 4056675.32 north; 590750.26 east 2021-02-12T23:18:44Z- AHRS_M2 msg: 1613171924.78 epoch sec; seqNo:2601 2021-02-12T23:18:44Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.27 psi 2021-02-12T23:18:44Z- handleDepth()-Depth_Keller msg: 1613171924789 epoch sec; seqNo:2626; depth 5.83 2021-02-12T23:18:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171924.80 epoch sec; seqNo:2626; 4056675.70 north; 590750.25 east 2021-02-12T23:18:45Z- AHRS_M2 msg: 1613171925.18 epoch sec; seqNo:2602 2021-02-12T23:18:45Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.25 psi 2021-02-12T23:18:45Z- handleDepth()-Depth_Keller msg: 1613171925193 epoch sec; seqNo:2627; depth 5.93 2021-02-12T23:18:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171925.20 epoch sec; seqNo:2627; 4056675.70 north; 590750.25 east 2021-02-12T23:18:45Z- AHRS_M2 msg: 1613171925.59 epoch sec; seqNo:2603 2021-02-12T23:18:45Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.23 psi 2021-02-12T23:18:45Z- handleDepth()-Depth_Keller msg: 1613171925600 epoch sec; seqNo:2628; depth 6.05 2021-02-12T23:18:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171925.62 epoch sec; seqNo:2628; 4056676.08 north; 590750.25 east 2021-02-12T23:18:45Z- AHRS_M2 msg: 1613171925.99 epoch sec; seqNo:2604 2021-02-12T23:18:45Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.20 psi 2021-02-12T23:18:46Z- handleDepth()-Depth_Keller msg: 1613171926005 epoch sec; seqNo:2629; depth 6.18 2021-02-12T23:18:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171926.01 epoch sec; seqNo:2629; 4056676.47 north; 590751.47 east 2021-02-12T23:18:46Z- AHRS_M2 msg: 1613171926.40 epoch sec; seqNo:2605 2021-02-12T23:18:46Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.18 psi 2021-02-12T23:18:46Z- handleDepth()-Depth_Keller msg: 1613171926400 epoch sec; seqNo:2630; depth 6.29 2021-02-12T23:18:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171926.41 epoch sec; seqNo:2630; 4056676.47 north; 590751.47 east 2021-02-12T23:18:46Z- AHRS_M2 msg: 1613171926.80 epoch sec; seqNo:2606 2021-02-12T23:18:46Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.17 psi 2021-02-12T23:18:46Z- handleDepth()-Depth_Keller msg: 1613171926809 epoch sec; seqNo:2631; depth 6.39 2021-02-12T23:18:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171926.82 epoch sec; seqNo:2631; 4056676.85 north; 590751.46 east 2021-02-12T23:18:47Z- AHRS_M2 msg: 1613171927.20 epoch sec; seqNo:2607 2021-02-12T23:18:47Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.17 psi 2021-02-12T23:18:47Z- handleDepth()-Depth_Keller msg: 1613171927208 epoch sec; seqNo:2632; depth 6.52 2021-02-12T23:18:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171927.22 epoch sec; seqNo:2632; 4056676.85 north; 590751.46 east 2021-02-12T23:18:47Z- AHRS_M2 msg: 1613171927.61 epoch sec; seqNo:2608 2021-02-12T23:18:47Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-12T23:18:47Z- handleDepth()-Depth_Keller msg: 1613171927614 epoch sec; seqNo:2633; depth 6.63 2021-02-12T23:18:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171927.62 epoch sec; seqNo:2633; 4056677.23 north; 590751.46 east 2021-02-12T23:18:48Z- AHRS_M2 msg: 1613171928.01 epoch sec; seqNo:2609 2021-02-12T23:18:48Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.19 psi 2021-02-12T23:18:48Z- handleDepth()-Depth_Keller msg: 1613171928053 epoch sec; seqNo:2634; depth 6.73 2021-02-12T23:18:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171928.07 epoch sec; seqNo:2634; 4056677.61 north; 590751.45 east 2021-02-12T23:18:48Z- AHRS_M2 msg: 1613171928.42 epoch sec; seqNo:2610 2021-02-12T23:18:48Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.21 psi 2021-02-12T23:18:48Z- handleDepth()-Depth_Keller msg: 1613171928420 epoch sec; seqNo:2635; depth 6.86 2021-02-12T23:18:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171928.43 epoch sec; seqNo:2635; 4056677.62 north; 590752.68 east 2021-02-12T23:18:48Z- AHRS_M2 msg: 1613171928.82 epoch sec; seqNo:2611 2021-02-12T23:18:48Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.22 psi 2021-02-12T23:18:48Z- handleDepth()-Depth_Keller msg: 1613171928824 epoch sec; seqNo:2636; depth 6.97 2021-02-12T23:18:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171928.83 epoch sec; seqNo:2636; 4056678.00 north; 590752.67 east 2021-02-12T23:18:49Z- AHRS_M2 msg: 1613171929.23 epoch sec; seqNo:2612 2021-02-12T23:18:49Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.22 psi 2021-02-12T23:18:49Z- handleDepth()-Depth_Keller msg: 1613171929230 epoch sec; seqNo:2637; depth 7.08 2021-02-12T23:18:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171929.24 epoch sec; seqNo:2637; 4056678.00 north; 590752.67 east 2021-02-12T23:18:49Z- LcmTrn::handleMessages() - handling 39 messages took 0.74 ms and 124 calls... 2021-02-12T23:18:49Z- waiting for 1613171927.07, time is 1613171929.24 2021-02-12T23:18:49Z- LcmTrn::updateTrn() >>>> pose time 1613171929.24, meas time 1613171924.07 <<<< 2021-02-12T23:18:49Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613171924.07, ping # 0. 2021-02-12T23:18:49Z- TNavBF::Projecting Measurements 2021-02-12T23:18:49Z- TNavFilter::Measurement from time = 1613171924.07 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:18:49Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:18:49Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:18:49Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:18:49Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:18:49Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:18:49Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:18:49Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:18:49Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 779.4 EastVariance: 833.1 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:18:49Z- TerrainNav::motionUpdate() - currEstimate = (4056678, 590752.67, 7.08) 2021-02-12T23:18:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:18:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:18:49Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:18:49Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:18:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:18:49Z- TNavBF::mmse selected from filter -1 2021-02-12T23:18:49Z- variances([0], [2], and psi_berg) = 779.268697 833.066615 0.000000 2021-02-12T23:18:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:18:49Z- LcmTrn::publishEstimates() - MLE : 4056654.50 590705.17 7.08 2021-02-12T23:18:49Z- LcmTrn::publishEstimates() - MMSE : 4056678.00 590752.67 7.08 2021-02-12T23:18:49Z- LcmTrn::publishEstimates() - COVAR: 779.27 833.07 1.19 0.00 2021-02-12T23:18:49Z- AHRS_M2 msg: 1613171929.63 epoch sec; seqNo:2613 2021-02-12T23:18:49Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.22 psi 2021-02-12T23:18:49Z- handleDepth()-Depth_Keller msg: 1613171929632 epoch sec; seqNo:2638; depth 7.21 2021-02-12T23:18:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171929.65 epoch sec; seqNo:2638; 4056678.38 north; 590752.67 east 2021-02-12T23:18:50Z- AHRS_M2 msg: 1613171930.03 epoch sec; seqNo:2614 2021-02-12T23:18:50Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.22 psi 2021-02-12T23:18:50Z- handleDepth()-Depth_Keller msg: 1613171930036 epoch sec; seqNo:2639; depth 7.33 2021-02-12T23:18:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171930.04 epoch sec; seqNo:2639; 4056678.76 north; 590752.66 east 2021-02-12T23:18:50Z- AHRS_M2 msg: 1613171930.43 epoch sec; seqNo:2615 2021-02-12T23:18:50Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.21 psi 2021-02-12T23:18:50Z- handleDepth()-Depth_Keller msg: 1613171930445 epoch sec; seqNo:2640; depth 7.45 2021-02-12T23:18:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171930.45 epoch sec; seqNo:2640; 4056678.77 north; 590753.88 east 2021-02-12T23:18:50Z- AHRS_M2 msg: 1613171930.84 epoch sec; seqNo:2616 2021-02-12T23:18:50Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.20 psi 2021-02-12T23:18:50Z- handleDepth()-Depth_Keller msg: 1613171930853 epoch sec; seqNo:2641; depth 7.57 2021-02-12T23:18:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171930.86 epoch sec; seqNo:2641; 4056679.15 north; 590753.88 east 2021-02-12T23:18:51Z- AHRS_M2 msg: 1613171931.24 epoch sec; seqNo:2617 2021-02-12T23:18:51Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.20 psi 2021-02-12T23:18:51Z- handleDepth()-Depth_Keller msg: 1613171931249 epoch sec; seqNo:2642; depth 7.70 2021-02-12T23:18:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171931.27 epoch sec; seqNo:2642; 4056679.15 north; 590753.88 east 2021-02-12T23:18:51Z- AHRS_M2 msg: 1613171931.65 epoch sec; seqNo:2618 2021-02-12T23:18:51Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.19 psi 2021-02-12T23:18:51Z- handleDepth()-Depth_Keller msg: 1613171931657 epoch sec; seqNo:2643; depth 7.81 2021-02-12T23:18:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171931.66 epoch sec; seqNo:2643; 4056679.53 north; 590753.88 east 2021-02-12T23:18:52Z- AHRS_M2 msg: 1613171932.05 epoch sec; seqNo:2619 2021-02-12T23:18:52Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-12T23:18:52Z- handleDepth()-Depth_Keller msg: 1613171932061 epoch sec; seqNo:2644; depth 7.92 2021-02-12T23:18:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171932.07 epoch sec; seqNo:2644; 4056679.91 north; 590753.87 east 2021-02-12T23:18:52Z- AHRS_M2 msg: 1613171932.46 epoch sec; seqNo:2620 2021-02-12T23:18:52Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-12T23:18:52Z- handleDepth()-Depth_Keller msg: 1613171932462 epoch sec; seqNo:2645; depth 8.04 2021-02-12T23:18:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171932.47 epoch sec; seqNo:2645; 4056679.92 north; 590755.09 east 2021-02-12T23:18:52Z- AHRS_M2 msg: 1613171932.86 epoch sec; seqNo:2621 2021-02-12T23:18:52Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.18 psi 2021-02-12T23:18:52Z- handleDepth()-Depth_Keller msg: 1613171932869 epoch sec; seqNo:2646; depth 8.16 2021-02-12T23:18:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171932.88 epoch sec; seqNo:2646; 4056680.30 north; 590755.09 east 2021-02-12T23:18:53Z- AHRS_M2 msg: 1613171933.28 epoch sec; seqNo:2622 2021-02-12T23:18:53Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.18 psi 2021-02-12T23:18:53Z- handleDepth()-Depth_Keller msg: 1613171933283 epoch sec; seqNo:2647; depth 8.28 2021-02-12T23:18:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171933.30 epoch sec; seqNo:2647; 4056680.30 north; 590755.09 east 2021-02-12T23:18:53Z- AHRS_M2 msg: 1613171933.67 epoch sec; seqNo:2623 2021-02-12T23:18:53Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-12T23:18:53Z- handleDepth()-Depth_Keller msg: 1613171933683 epoch sec; seqNo:2648; depth 8.40 2021-02-12T23:18:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171933.70 epoch sec; seqNo:2648; 4056680.68 north; 590755.09 east 2021-02-12T23:18:54Z- AHRS_M2 msg: 1613171934.07 epoch sec; seqNo:2624 2021-02-12T23:18:54Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-12T23:18:54Z- handleDepth()-Depth_Keller msg: 1613171934081 epoch sec; seqNo:2649; depth 8.52 2021-02-12T23:18:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171934.10 epoch sec; seqNo:2649; 4056681.06 north; 590755.08 east 2021-02-12T23:18:54Z- LcmTrn::handleMessages() - handling 36 messages took 0.64 ms and 123 calls... 2021-02-12T23:18:54Z- waiting for 1613171932.24, time is 1613171934.10 2021-02-12T23:18:54Z- LcmTrn::updateTrn() >>>> pose time 1613171934.10, meas time 1613171924.07 <<<< 2021-02-12T23:18:54Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613171924.07, ping # 0. 2021-02-12T23:18:54Z- TerrainNav::Did not incorporate measurement type 1 from time= 1613171924.07 sec, ping # 0; no INS pose data available. 2021-02-12T23:18:54Z- TerrainNav::motionUpdate() - currEstimate = (4056681, 590755.08, 8.52) 2021-02-12T23:18:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:18:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:18:54Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:18:54Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:18:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:18:54Z- TNavBF::mmse selected from filter -1 2021-02-12T23:18:54Z- variances([0], [2], and psi_berg) = 779.120056 833.066403 0.000000 2021-02-12T23:18:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:18:54Z- LcmTrn::publishEstimates() - MLE : 4056657.56 590707.58 8.52 2021-02-12T23:18:54Z- LcmTrn::publishEstimates() - MMSE : 4056681.06 590755.08 8.52 2021-02-12T23:18:54Z- LcmTrn::publishEstimates() - COVAR: 779.12 833.07 1.39 0.00 2021-02-12T23:18:54Z- AHRS_M2 msg: 1613171934.48 epoch sec; seqNo:2625 2021-02-12T23:18:54Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-12T23:18:54Z- handleDepth()-Depth_Keller msg: 1613171934486 epoch sec; seqNo:2650; depth 8.63 2021-02-12T23:18:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171934.49 epoch sec; seqNo:2650; 4056681.06 north; 590755.08 east 2021-02-12T23:18:54Z- AHRS_M2 msg: 1613171934.88 epoch sec; seqNo:2626 2021-02-12T23:18:54Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.21 psi 2021-02-12T23:18:54Z- handleDepth()-Depth_Keller msg: 1613171934887 epoch sec; seqNo:2651; depth 8.75 2021-02-12T23:18:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171934.89 epoch sec; seqNo:2651; 4056681.45 north; 590756.30 east 2021-02-12T23:18:55Z- AHRS_M2 msg: 1613171935.28 epoch sec; seqNo:2627 2021-02-12T23:18:55Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.22 psi 2021-02-12T23:18:55Z- handleDepth()-Depth_Keller msg: 1613171935289 epoch sec; seqNo:2652; depth 8.87 2021-02-12T23:18:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171935.30 epoch sec; seqNo:2652; 4056681.45 north; 590756.30 east 2021-02-12T23:18:55Z- AHRS_M2 msg: 1613171935.69 epoch sec; seqNo:2628 2021-02-12T23:18:55Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.22 psi 2021-02-12T23:18:55Z- handleDepth()-Depth_Keller msg: 1613171935692 epoch sec; seqNo:2653; depth 9.00 2021-02-12T23:18:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171935.70 epoch sec; seqNo:2653; 4056681.83 north; 590756.29 east 2021-02-12T23:18:56Z- AHRS_M2 msg: 1613171936.09 epoch sec; seqNo:2629 2021-02-12T23:18:56Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.22 psi 2021-02-12T23:18:56Z- handleDepth()-Depth_Keller msg: 1613171936101 epoch sec; seqNo:2654; depth 9.15 2021-02-12T23:18:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171936.11 epoch sec; seqNo:2654; 4056682.20 north; 590756.29 east 2021-02-12T23:18:56Z- AHRS_M2 msg: 1613171936.50 epoch sec; seqNo:2630 2021-02-12T23:18:56Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.21 psi 2021-02-12T23:18:56Z- handleDepth()-Depth_Keller msg: 1613171936509 epoch sec; seqNo:2655; depth 9.29 2021-02-12T23:18:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171936.53 epoch sec; seqNo:2655; 4056682.20 north; 590756.29 east 2021-02-12T23:18:56Z- AHRS_M2 msg: 1613171936.90 epoch sec; seqNo:2631 2021-02-12T23:18:56Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.21 psi 2021-02-12T23:18:56Z- handleDepth()-Depth_Keller msg: 1613171936904 epoch sec; seqNo:2656; depth 9.42 2021-02-12T23:18:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171936.91 epoch sec; seqNo:2656; 4056682.60 north; 590757.51 east 2021-02-12T23:18:57Z- AHRS_M2 msg: 1613171937.30 epoch sec; seqNo:2632 2021-02-12T23:18:57Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.21 psi 2021-02-12T23:18:57Z- handleDepth()-Depth_Keller msg: 1613171937308 epoch sec; seqNo:2657; depth 9.56 2021-02-12T23:18:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171937.32 epoch sec; seqNo:2657; 4056682.60 north; 590757.51 east 2021-02-12T23:18:57Z- AHRS_M2 msg: 1613171937.71 epoch sec; seqNo:2633 2021-02-12T23:18:57Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.21 psi 2021-02-12T23:18:57Z- handleDepth()-Depth_Keller msg: 1613171937717 epoch sec; seqNo:2658; depth 9.69 2021-02-12T23:18:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171937.73 epoch sec; seqNo:2658; 4056682.98 north; 590757.50 east 2021-02-12T23:18:58Z- AHRS_M2 msg: 1613171938.11 epoch sec; seqNo:2634 2021-02-12T23:18:58Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.20 psi 2021-02-12T23:18:58Z- handleDepth()-Depth_Keller msg: 1613171938123 epoch sec; seqNo:2659; depth 9.85 2021-02-12T23:18:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171938.13 epoch sec; seqNo:2659; 4056683.35 north; 590757.50 east 2021-02-12T23:18:58Z- AHRS_M2 msg: 1613171938.52 epoch sec; seqNo:2635 2021-02-12T23:18:58Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.19 psi 2021-02-12T23:18:58Z- handleDepth()-Depth_Keller msg: 1613171938521 epoch sec; seqNo:2660; depth 9.98 2021-02-12T23:18:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171938.53 epoch sec; seqNo:2660; 4056683.35 north; 590757.50 east 2021-02-12T23:18:58Z- AHRS_M2 msg: 1613171938.92 epoch sec; seqNo:2636 2021-02-12T23:18:58Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.19 psi 2021-02-12T23:18:58Z- handleDepth()-Depth_Keller msg: 1613171938929 epoch sec; seqNo:2661; depth 10.14 2021-02-12T23:18:58Z- handleDvl() - RDI_Pathfinder msg: 1613171938.94 epoch sec; seqNo:1105 2021-02-12T23:18:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:18:58Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:18:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171938.95 epoch sec; seqNo:2661; 4056683.73 north; 590757.50 east 2021-02-12T23:18:59Z- LcmTrn::handleMessages() - handling 37 messages took 0.63 ms and 125 calls... 2021-02-12T23:18:59Z- waiting for 1613171937.10, time is 1613171938.95 2021-02-12T23:18:59Z- LcmTrn::updateTrn() equating pose 1613171938.95 and meas times 1613171938.95 2021-02-12T23:18:59Z- LcmTrn::updateTrn() >>>> pose time 1613171938.95, meas time 1613171938.95 <<<< 2021-02-12T23:18:59Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613171938.95, ping # 0. 2021-02-12T23:18:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171938.95 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:18:59Z- TerrainNav::motionUpdate() - currEstimate = (4056684, 590757.50, 10.14) 2021-02-12T23:18:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:18:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:18:59Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:18:59Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:18:59Z- TNavBF::Projecting Measurements 2021-02-12T23:18:59Z- TNavFilter::Measurement from time = 1613171938.95 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:18:59Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:18:59Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:18:59Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:18:59Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:18:59Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:18:59Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:18:59Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:18:59Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 779.0 EastVariance: 833.1 DepthVariance: 1.6 this->DepthBiasCov: 1.6 2021-02-12T23:18:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:18:59Z- TNavBF::mmse selected from filter -1 2021-02-12T23:18:59Z- variances([0], [2], and psi_berg) = 778.977365 833.066201 0.000000 2021-02-12T23:18:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:18:59Z- LcmTrn::publishEstimates() - MLE : 4056660.23 590710.00 10.14 2021-02-12T23:18:59Z- LcmTrn::publishEstimates() - MMSE : 4056683.73 590757.50 10.14 2021-02-12T23:18:59Z- LcmTrn::publishEstimates() - COVAR: 778.98 833.07 1.58 0.00 2021-02-12T23:18:59Z- AHRS_M2 msg: 1613171939.33 epoch sec; seqNo:2637 2021-02-12T23:18:59Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.18 psi 2021-02-12T23:18:59Z- handleDepth()-Depth_Keller msg: 1613171939337 epoch sec; seqNo:2662; depth 10.29 2021-02-12T23:18:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171939.35 epoch sec; seqNo:2662; 4056683.75 north; 590758.72 east 2021-02-12T23:18:59Z- AHRS_M2 msg: 1613171939.74 epoch sec; seqNo:2638 2021-02-12T23:18:59Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.17 psi 2021-02-12T23:18:59Z- handleDepth()-Depth_Keller msg: 1613171939744 epoch sec; seqNo:2663; depth 10.44 2021-02-12T23:18:59Z- handleDvl() - RDI_Pathfinder msg: 1613171939.76 epoch sec; seqNo:1106 2021-02-12T23:18:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:18:59Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:18:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171939.77 epoch sec; seqNo:2663; 4056684.12 north; 590758.71 east 2021-02-12T23:19:00Z- AHRS_M2 msg: 1613171940.13 epoch sec; seqNo:2639 2021-02-12T23:19:00Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.16 psi 2021-02-12T23:19:00Z- handleDepth()-Depth_Keller msg: 1613171940141 epoch sec; seqNo:2664; depth 10.58 2021-02-12T23:19:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:00Z- handleDvl() - RDI_Pathfinder msg: 1613171940.14 epoch sec; seqNo:1107 2021-02-12T23:19:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:00Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:19:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171940.17 epoch sec; seqNo:2664; 4056684.50 north; 590758.71 east 2021-02-12T23:19:00Z- AHRS_M2 msg: 1613171940.54 epoch sec; seqNo:2640 2021-02-12T23:19:00Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.18 psi 2021-02-12T23:19:00Z- handleDepth()-Depth_Keller msg: 1613171940540 epoch sec; seqNo:2665; depth 10.72 2021-02-12T23:19:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171940.55 epoch sec; seqNo:2665; 4056684.50 north; 590758.71 east 2021-02-12T23:19:00Z- AHRS_M2 msg: 1613171940.94 epoch sec; seqNo:2641 2021-02-12T23:19:00Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.19 psi 2021-02-12T23:19:00Z- handleDepth()-Depth_Keller msg: 1613171940947 epoch sec; seqNo:2666; depth 10.84 2021-02-12T23:19:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171940.95 epoch sec; seqNo:2666; 4056684.88 north; 590758.70 east 2021-02-12T23:19:01Z- AHRS_M2 msg: 1613171941.34 epoch sec; seqNo:2642 2021-02-12T23:19:01Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.21 psi 2021-02-12T23:19:01Z- handleDepth()-Depth_Keller msg: 1613171941348 epoch sec; seqNo:2667; depth 10.98 2021-02-12T23:19:01Z- handleDvl() - RDI_Pathfinder msg: 1613171941.37 epoch sec; seqNo:1108 2021-02-12T23:19:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.85 , 101.15 , 74.35 , 116.29 2021-02-12T23:19:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.11 , -0.13 2021-02-12T23:19:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171941.38 epoch sec; seqNo:2667; 4056684.90 north; 590759.93 east 2021-02-12T23:19:01Z- AHRS_M2 msg: 1613171941.75 epoch sec; seqNo:2643 2021-02-12T23:19:01Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.21 psi 2021-02-12T23:19:01Z- handleDepth()-Depth_Keller msg: 1613171941763 epoch sec; seqNo:2668; depth 11.13 2021-02-12T23:19:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171941.79 epoch sec; seqNo:2668; 4056685.27 north; 590759.92 east 2021-02-12T23:19:02Z- AHRS_M2 msg: 1613171942.15 epoch sec; seqNo:2644 2021-02-12T23:19:02Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.21 psi 2021-02-12T23:19:02Z- handleDepth()-Depth_Keller msg: 1613171942157 epoch sec; seqNo:2669; depth 11.26 2021-02-12T23:19:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171942.17 epoch sec; seqNo:2669; 4056685.65 north; 590759.92 east 2021-02-12T23:19:02Z- AHRS_M2 msg: 1613171942.56 epoch sec; seqNo:2645 2021-02-12T23:19:02Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.21 psi 2021-02-12T23:19:02Z- handleDepth()-Depth_Keller msg: 1613171942562 epoch sec; seqNo:2670; depth 11.40 2021-02-12T23:19:02Z- handleDvl() - RDI_Pathfinder msg: 1613171942.58 epoch sec; seqNo:1109 2021-02-12T23:19:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.25 , 96.22 , 74.76 , 109.73 2021-02-12T23:19:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.05 , -0.26 2021-02-12T23:19:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171942.59 epoch sec; seqNo:2670; 4056686.03 north; 590759.91 east 2021-02-12T23:19:02Z- AHRS_M2 msg: 1613171942.96 epoch sec; seqNo:2646 2021-02-12T23:19:02Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.21 psi 2021-02-12T23:19:02Z- handleDepth()-Depth_Keller msg: 1613171942965 epoch sec; seqNo:2671; depth 11.54 2021-02-12T23:19:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171942.98 epoch sec; seqNo:2671; 4056686.41 north; 590759.91 east 2021-02-12T23:19:03Z- AHRS_M2 msg: 1613171943.36 epoch sec; seqNo:2647 2021-02-12T23:19:03Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.20 psi 2021-02-12T23:19:03Z- handleDepth()-Depth_Keller msg: 1613171943368 epoch sec; seqNo:2672; depth 11.65 2021-02-12T23:19:03Z- handleDvl() - RDI_Pathfinder msg: 1613171943.38 epoch sec; seqNo:1110 2021-02-12T23:19:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 102.64 , 78.31 , 108.72 2021-02-12T23:19:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.28 , -0.19 2021-02-12T23:19:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171943.40 epoch sec; seqNo:2672; 4056686.79 north; 590759.91 east 2021-02-12T23:19:03Z- AHRS_M2 msg: 1613171943.77 epoch sec; seqNo:2648 2021-02-12T23:19:03Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.19 psi 2021-02-12T23:19:03Z- handleDepth()-Depth_Keller msg: 1613171943772 epoch sec; seqNo:2673; depth 11.78 2021-02-12T23:19:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:03Z- handleDvl() - RDI_Pathfinder msg: 1613171943.77 epoch sec; seqNo:1111 2021-02-12T23:19:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.28 , -0.19 2021-02-12T23:19:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171943.79 epoch sec; seqNo:2673; 4056687.17 north; 590759.90 east 2021-02-12T23:19:04Z- AHRS_M2 msg: 1613171944.19 epoch sec; seqNo:2649 2021-02-12T23:19:04Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.19 psi 2021-02-12T23:19:04Z- handleDepth()-Depth_Keller msg: 1613171944217 epoch sec; seqNo:2674; depth 11.91 2021-02-12T23:19:04Z- handleDvl() - RDI_Pathfinder msg: 1613171944.24 epoch sec; seqNo:1112 2021-02-12T23:19:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 100.41 , 73.94 , 109.87 2021-02-12T23:19:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.32 , 0.02 2021-02-12T23:19:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171944.27 epoch sec; seqNo:2674; 4056687.55 north; 590759.90 east 2021-02-12T23:19:04Z- LcmTrn::handleMessages() - handling 46 messages took 0.70 ms and 132 calls... 2021-02-12T23:19:04Z- waiting for 1613171941.95, time is 1613171944.27 2021-02-12T23:19:04Z- LcmTrn::updateTrn() equating pose 1613171944.27 and meas times 1613171944.27 2021-02-12T23:19:04Z- LcmTrn::updateTrn() >>>> pose time 1613171944.27, meas time 1613171944.27 <<<< 2021-02-12T23:19:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171944.27 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:19:04Z- TerrainNav::motionUpdate() - currEstimate = (4056688, 590759.90, 11.91) 2021-02-12T23:19:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:19:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:19:04Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171944.27 sec.; No relevant navigation data available 2021-02-12T23:19:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:19:04Z- TNavBF::mmse selected from filter -1 2021-02-12T23:19:04Z- variances([0], [2], and psi_berg) = 778.818199 833.065976 0.000000 2021-02-12T23:19:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:19:04Z- LcmTrn::publishEstimates() - MLE : 4056664.05 590712.40 11.91 2021-02-12T23:19:04Z- LcmTrn::publishEstimates() - MMSE : 4056687.55 590759.90 11.91 2021-02-12T23:19:04Z- LcmTrn::publishEstimates() - COVAR: 778.82 833.07 1.81 0.00 2021-02-12T23:19:04Z- AHRS_M2 msg: 1613171944.58 epoch sec; seqNo:2650 2021-02-12T23:19:04Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.19 psi 2021-02-12T23:19:04Z- handleDepth()-Depth_Keller msg: 1613171944602 epoch sec; seqNo:2675; depth 12.06 2021-02-12T23:19:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171944.61 epoch sec; seqNo:2675; 4056687.56 north; 590761.12 east 2021-02-12T23:19:04Z- AHRS_M2 msg: 1613171944.98 epoch sec; seqNo:2651 2021-02-12T23:19:04Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.19 psi 2021-02-12T23:19:04Z- handleDepth()-Depth_Keller msg: 1613171944984 epoch sec; seqNo:2676; depth 12.17 2021-02-12T23:19:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:04Z- handleDvl() - RDI_Pathfinder msg: 1613171944.99 epoch sec; seqNo:1113 2021-02-12T23:19:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.32 , 0.02 2021-02-12T23:19:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171944.99 epoch sec; seqNo:2676; 4056687.94 north; 590761.11 east 2021-02-12T23:19:05Z- AHRS_M2 msg: 1613171945.38 epoch sec; seqNo:2652 2021-02-12T23:19:05Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.18 psi 2021-02-12T23:19:05Z- handleDepth()-Depth_Keller msg: 1613171945388 epoch sec; seqNo:2677; depth 12.26 2021-02-12T23:19:05Z- handleDvl() - RDI_Pathfinder msg: 1613171945.40 epoch sec; seqNo:1114 2021-02-12T23:19:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 98.92 , 73.54 , 106.22 2021-02-12T23:19:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.18 , 0.20 2021-02-12T23:19:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171945.41 epoch sec; seqNo:2677; 4056688.32 north; 590761.11 east 2021-02-12T23:19:05Z- AHRS_M2 msg: 1613171945.79 epoch sec; seqNo:2653 2021-02-12T23:19:05Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.18 psi 2021-02-12T23:19:05Z- handleDepth()-Depth_Keller msg: 1613171945800 epoch sec; seqNo:2678; depth 12.40 2021-02-12T23:19:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171945.81 epoch sec; seqNo:2678; 4056688.32 north; 590761.11 east 2021-02-12T23:19:06Z- AHRS_M2 msg: 1613171946.19 epoch sec; seqNo:2654 2021-02-12T23:19:06Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.18 psi 2021-02-12T23:19:06Z- handleDepth()-Depth_Keller msg: 1613171946205 epoch sec; seqNo:2679; depth 12.50 2021-02-12T23:19:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:06Z- handleDvl() - RDI_Pathfinder msg: 1613171946.21 epoch sec; seqNo:1115 2021-02-12T23:19:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.18 , 0.20 2021-02-12T23:19:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171946.26 epoch sec; seqNo:2679; 4056688.70 north; 590761.11 east 2021-02-12T23:19:06Z- AHRS_M2 msg: 1613171946.60 epoch sec; seqNo:2655 2021-02-12T23:19:06Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.19 psi 2021-02-12T23:19:06Z- handleDepth()-Depth_Keller msg: 1613171946605 epoch sec; seqNo:2680; depth 12.59 2021-02-12T23:19:06Z- handleDvl() - RDI_Pathfinder msg: 1613171946.62 epoch sec; seqNo:1116 2021-02-12T23:19:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 96.56 , 72.72 , 108.72 2021-02-12T23:19:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.03 , 0.21 2021-02-12T23:19:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171946.64 epoch sec; seqNo:2680; 4056689.08 north; 590761.10 east 2021-02-12T23:19:07Z- AHRS_M2 msg: 1613171947.00 epoch sec; seqNo:2656 2021-02-12T23:19:07Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.19 psi 2021-02-12T23:19:07Z- handleDepth()-Depth_Keller msg: 1613171947004 epoch sec; seqNo:2681; depth 12.71 2021-02-12T23:19:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171947.02 epoch sec; seqNo:2681; 4056689.08 north; 590761.10 east 2021-02-12T23:19:07Z- AHRS_M2 msg: 1613171947.40 epoch sec; seqNo:2657 2021-02-12T23:19:07Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.19 psi 2021-02-12T23:19:07Z- handleDepth()-Depth_Keller msg: 1613171947410 epoch sec; seqNo:2682; depth 12.81 2021-02-12T23:19:07Z- handleDvl() - RDI_Pathfinder msg: 1613171947.42 epoch sec; seqNo:1117 2021-02-12T23:19:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 97.84 , 73.94 , 107.57 2021-02-12T23:19:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.10 , 0.06 2021-02-12T23:19:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171947.43 epoch sec; seqNo:2682; 4056689.45 north; 590761.10 east 2021-02-12T23:19:07Z- AHRS_M2 msg: 1613171947.81 epoch sec; seqNo:2658 2021-02-12T23:19:07Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.19 psi 2021-02-12T23:19:07Z- handleDepth()-Depth_Keller msg: 1613171947812 epoch sec; seqNo:2683; depth 12.92 2021-02-12T23:19:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171947.83 epoch sec; seqNo:2683; 4056689.45 north; 590761.10 east 2021-02-12T23:19:08Z- AHRS_M2 msg: 1613171948.22 epoch sec; seqNo:2659 2021-02-12T23:19:08Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.19 psi 2021-02-12T23:19:08Z- handleDepth()-Depth_Keller msg: 1613171948231 epoch sec; seqNo:2684; depth 13.03 2021-02-12T23:19:08Z- handleDvl() - RDI_Pathfinder msg: 1613171948.30 epoch sec; seqNo:1118 2021-02-12T23:19:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.03 , 98.99 , 73.13 , 105.07 2021-02-12T23:19:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , -0.08 2021-02-12T23:19:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171948.31 epoch sec; seqNo:2684; 4056689.85 north; 590762.32 east 2021-02-12T23:19:08Z- AHRS_M2 msg: 1613171948.62 epoch sec; seqNo:2660 2021-02-12T23:19:08Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.18 psi 2021-02-12T23:19:08Z- handleDepth()-Depth_Keller msg: 1613171948622 epoch sec; seqNo:2685; depth 13.14 2021-02-12T23:19:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:08Z- handleDvl() - RDI_Pathfinder msg: 1613171948.62 epoch sec; seqNo:1119 2021-02-12T23:19:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , -0.08 2021-02-12T23:19:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171948.63 epoch sec; seqNo:2685; 4056690.22 north; 590762.31 east 2021-02-12T23:19:09Z- AHRS_M2 msg: 1613171949.02 epoch sec; seqNo:2661 2021-02-12T23:19:09Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.17 psi 2021-02-12T23:19:09Z- handleDepth()-Depth_Keller msg: 1613171949026 epoch sec; seqNo:2686; depth 13.24 2021-02-12T23:19:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171949.04 epoch sec; seqNo:2686; 4056690.60 north; 590762.31 east 2021-02-12T23:19:09Z- LcmTrn::handleMessages() - handling 43 messages took 0.65 ms and 130 calls... 2021-02-12T23:19:09Z- waiting for 1613171947.27, time is 1613171949.04 2021-02-12T23:19:09Z- LcmTrn::updateTrn() >>>> pose time 1613171949.04, meas time 1613171948.62 <<<< 2021-02-12T23:19:09Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613171948.62, ping # 0. 2021-02-12T23:19:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171948.62 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:19:09Z- TerrainNav::motionUpdate() - currEstimate = (4056691, 590762.31, 13.24) 2021-02-12T23:19:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:19:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:19:09Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:19:09Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:19:09Z- TNavBF::Projecting Measurements 2021-02-12T23:19:09Z- TNavFilter::Measurement from time = 1613171948.62 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:19:09Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:19:09Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:19:09Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:19:09Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:19:09Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:19:09Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:19:09Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:19:09Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 778.7 EastVariance: 833.1 DepthVariance: 2.0 this->DepthBiasCov: 2.0 2021-02-12T23:19:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:19:09Z- TNavBF::mmse selected from filter -1 2021-02-12T23:19:09Z- variances([0], [2], and psi_berg) = 778.633955 833.065716 0.000000 2021-02-12T23:19:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:19:09Z- LcmTrn::publishEstimates() - MLE : 4056667.10 590714.81 13.24 2021-02-12T23:19:09Z- LcmTrn::publishEstimates() - MMSE : 4056690.60 590762.31 13.24 2021-02-12T23:19:09Z- LcmTrn::publishEstimates() - COVAR: 778.63 833.07 2.00 0.00 2021-02-12T23:19:09Z- AHRS_M2 msg: 1613171949.42 epoch sec; seqNo:2662 2021-02-12T23:19:09Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.17 psi 2021-02-12T23:19:09Z- handleDepth()-Depth_Keller msg: 1613171949429 epoch sec; seqNo:2687; depth 13.35 2021-02-12T23:19:09Z- handleDvl() - RDI_Pathfinder msg: 1613171949.45 epoch sec; seqNo:1120 2021-02-12T23:19:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 96.62 , 73.54 , 105.14 2021-02-12T23:19:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.09 , -0.11 2021-02-12T23:19:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171949.46 epoch sec; seqNo:2687; 4056690.98 north; 590762.30 east 2021-02-12T23:19:09Z- AHRS_M2 msg: 1613171949.83 epoch sec; seqNo:2663 2021-02-12T23:19:09Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.16 psi 2021-02-12T23:19:09Z- handleDepth()-Depth_Keller msg: 1613171949837 epoch sec; seqNo:2688; depth 13.47 2021-02-12T23:19:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:09Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:09Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:09Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:09Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:09Z- handleDvl() - RDI_Pathfinder msg: 1613171949.84 epoch sec; seqNo:1121 2021-02-12T23:19:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.09 , -0.11 2021-02-12T23:19:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171949.85 epoch sec; seqNo:2688; 4056691.36 north; 590762.30 east 2021-02-12T23:19:10Z- AHRS_M2 msg: 1613171950.25 epoch sec; seqNo:2664 2021-02-12T23:19:10Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.15 psi 2021-02-12T23:19:10Z- handleDepth()-Depth_Keller msg: 1613171950254 epoch sec; seqNo:2689; depth 13.58 2021-02-12T23:19:10Z- handleDvl() - RDI_Pathfinder msg: 1613171950.28 epoch sec; seqNo:1122 2021-02-12T23:19:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 72.72 , 105.21 2021-02-12T23:19:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , -0.08 2021-02-12T23:19:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171950.29 epoch sec; seqNo:2689; 4056691.36 north; 590762.30 east 2021-02-12T23:19:10Z- AHRS_M2 msg: 1613171950.65 epoch sec; seqNo:2665 2021-02-12T23:19:10Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.15 psi 2021-02-12T23:19:10Z- handleDepth()-Depth_Keller msg: 1613171950652 epoch sec; seqNo:2690; depth 13.71 2021-02-12T23:19:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171950.67 epoch sec; seqNo:2690; 4056691.75 north; 590763.52 east 2021-02-12T23:19:11Z- AHRS_M2 msg: 1613171951.04 epoch sec; seqNo:2666 2021-02-12T23:19:11Z- AHRS_M2 msg: 0.66 phi; -0.30 theta; -0.15 psi 2021-02-12T23:19:11Z- handleDepth()-Depth_Keller msg: 1613171951058 epoch sec; seqNo:2691; depth 13.80 2021-02-12T23:19:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:11Z- handleDvl() - RDI_Pathfinder msg: 1613171951.06 epoch sec; seqNo:1123 2021-02-12T23:19:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , -0.08 2021-02-12T23:19:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171951.07 epoch sec; seqNo:2691; 4056692.13 north; 590763.51 east 2021-02-12T23:19:11Z- AHRS_M2 msg: 1613171951.45 epoch sec; seqNo:2667 2021-02-12T23:19:11Z- AHRS_M2 msg: 0.66 phi; -0.31 theta; -0.15 psi 2021-02-12T23:19:11Z- handleDepth()-Depth_Keller msg: 1613171951457 epoch sec; seqNo:2692; depth 13.91 2021-02-12T23:19:11Z- handleDvl() - RDI_Pathfinder msg: 1613171951.47 epoch sec; seqNo:1124 2021-02-12T23:19:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.62 , 94.19 , 71.90 , 106.36 2021-02-12T23:19:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , -0.06 2021-02-12T23:19:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171951.49 epoch sec; seqNo:2692; 4056692.51 north; 590763.51 east 2021-02-12T23:19:11Z- AHRS_M2 msg: 1613171951.85 epoch sec; seqNo:2668 2021-02-12T23:19:11Z- AHRS_M2 msg: 0.66 phi; -0.31 theta; -0.16 psi 2021-02-12T23:19:11Z- handleDepth()-Depth_Keller msg: 1613171951858 epoch sec; seqNo:2693; depth 14.02 2021-02-12T23:19:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171951.87 epoch sec; seqNo:2693; 4056692.51 north; 590763.51 east 2021-02-12T23:19:12Z- AHRS_M2 msg: 1613171952.26 epoch sec; seqNo:2669 2021-02-12T23:19:12Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.16 psi 2021-02-12T23:19:12Z- handleDepth()-Depth_Keller msg: 1613171952266 epoch sec; seqNo:2694; depth 14.13 2021-02-12T23:19:12Z- handleDvl() - RDI_Pathfinder msg: 1613171952.28 epoch sec; seqNo:1125 2021-02-12T23:19:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.03 , 95.54 , 72.31 , 104.06 2021-02-12T23:19:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.11 , -0.03 2021-02-12T23:19:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171952.29 epoch sec; seqNo:2694; 4056692.89 north; 590763.50 east 2021-02-12T23:19:12Z- AHRS_M2 msg: 1613171952.65 epoch sec; seqNo:2670 2021-02-12T23:19:12Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.16 psi 2021-02-12T23:19:12Z- handleDepth()-Depth_Keller msg: 1613171952665 epoch sec; seqNo:2695; depth 14.26 2021-02-12T23:19:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171952.69 epoch sec; seqNo:2695; 4056693.27 north; 590763.50 east 2021-02-12T23:19:13Z- AHRS_M2 msg: 1613171953.06 epoch sec; seqNo:2671 2021-02-12T23:19:13Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.17 psi 2021-02-12T23:19:13Z- handleDepth()-Depth_Keller msg: 1613171953064 epoch sec; seqNo:2696; depth 14.35 2021-02-12T23:19:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171953.07 epoch sec; seqNo:2696; 4056693.65 north; 590763.50 east 2021-02-12T23:19:13Z- AHRS_M2 msg: 1613171953.46 epoch sec; seqNo:2672 2021-02-12T23:19:13Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.17 psi 2021-02-12T23:19:13Z- handleDepth()-Depth_Keller msg: 1613171953469 epoch sec; seqNo:2697; depth 14.48 2021-02-12T23:19:13Z- handleDvl() - RDI_Pathfinder msg: 1613171953.48 epoch sec; seqNo:1126 2021-02-12T23:19:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.21 , 95.54 , 71.08 , 106.49 2021-02-12T23:19:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , 0.00 2021-02-12T23:19:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171953.49 epoch sec; seqNo:2697; 4056693.66 north; 590764.72 east 2021-02-12T23:19:13Z- AHRS_M2 msg: 1613171953.87 epoch sec; seqNo:2673 2021-02-12T23:19:13Z- AHRS_M2 msg: 0.68 phi; -0.31 theta; -0.17 psi 2021-02-12T23:19:13Z- handleDepth()-Depth_Keller msg: 1613171953872 epoch sec; seqNo:2698; depth 14.59 2021-02-12T23:19:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171953.89 epoch sec; seqNo:2698; 4056694.04 north; 590764.71 east 2021-02-12T23:19:14Z- AHRS_M2 msg: 1613171954.27 epoch sec; seqNo:2674 2021-02-12T23:19:14Z- AHRS_M2 msg: 0.68 phi; -0.31 theta; -0.17 psi 2021-02-12T23:19:14Z- handleDepth()-Depth_Keller msg: 1613171954276 epoch sec; seqNo:2699; depth 14.69 2021-02-12T23:19:14Z- handleDvl() - RDI_Pathfinder msg: 1613171954.29 epoch sec; seqNo:1127 2021-02-12T23:19:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.63 , 95.54 , 71.50 , 105.27 2021-02-12T23:19:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.02 , 0.01 2021-02-12T23:19:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171954.31 epoch sec; seqNo:2699; 4056694.42 north; 590764.71 east 2021-02-12T23:19:14Z- LcmTrn::handleMessages() - handling 47 messages took 0.70 ms and 133 calls... 2021-02-12T23:19:14Z- waiting for 1613171952.04, time is 1613171954.31 2021-02-12T23:19:14Z- LcmTrn::updateTrn() equating pose 1613171954.31 and meas times 1613171954.31 2021-02-12T23:19:14Z- LcmTrn::updateTrn() >>>> pose time 1613171954.31, meas time 1613171954.31 <<<< 2021-02-12T23:19:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171954.31 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:19:14Z- TerrainNav::motionUpdate() - currEstimate = (4056694, 590764.71, 14.69) 2021-02-12T23:19:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:19:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:19:14Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171954.31 sec.; No relevant navigation data available 2021-02-12T23:19:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:19:14Z- TNavBF::mmse selected from filter -1 2021-02-12T23:19:14Z- variances([0], [2], and psi_berg) = 778.475714 833.065493 0.000000 2021-02-12T23:19:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:19:14Z- LcmTrn::publishEstimates() - MLE : 4056670.92 590718.21 14.69 2021-02-12T23:19:14Z- LcmTrn::publishEstimates() - MMSE : 4056694.42 590764.71 14.69 2021-02-12T23:19:14Z- LcmTrn::publishEstimates() - COVAR: 778.48 833.07 2.23 0.00 2021-02-12T23:19:14Z- AHRS_M2 msg: 1613171954.68 epoch sec; seqNo:2675 2021-02-12T23:19:14Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.17 psi 2021-02-12T23:19:14Z- handleDepth()-Depth_Keller msg: 1613171954680 epoch sec; seqNo:2700; depth 14.83 2021-02-12T23:19:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:14Z- handleDvl() - RDI_Pathfinder msg: 1613171954.68 epoch sec; seqNo:1128 2021-02-12T23:19:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.02 , 0.01 2021-02-12T23:19:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171954.69 epoch sec; seqNo:2700; 4056694.42 north; 590764.71 east 2021-02-12T23:19:15Z- AHRS_M2 msg: 1613171955.08 epoch sec; seqNo:2676 2021-02-12T23:19:15Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.17 psi 2021-02-12T23:19:15Z- handleDepth()-Depth_Keller msg: 1613171955086 epoch sec; seqNo:2701; depth 14.93 2021-02-12T23:19:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171955.09 epoch sec; seqNo:2701; 4056694.80 north; 590764.71 east 2021-02-12T23:19:15Z- AHRS_M2 msg: 1613171955.48 epoch sec; seqNo:2677 2021-02-12T23:19:15Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.17 psi 2021-02-12T23:19:15Z- handleDepth()-Depth_Keller msg: 1613171955493 epoch sec; seqNo:2702; depth 15.03 2021-02-12T23:19:15Z- handleDvl() - RDI_Pathfinder msg: 1613171955.50 epoch sec; seqNo:1129 2021-02-12T23:19:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.22 , 95.54 , 71.09 , 96.76 2021-02-12T23:19:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.01 , 0.00 2021-02-12T23:19:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171955.51 epoch sec; seqNo:2702; 4056694.80 north; 590764.71 east 2021-02-12T23:19:15Z- AHRS_M2 msg: 1613171955.89 epoch sec; seqNo:2678 2021-02-12T23:19:15Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.17 psi 2021-02-12T23:19:15Z- handleDepth()-Depth_Keller msg: 1613171955894 epoch sec; seqNo:2703; depth 15.15 2021-02-12T23:19:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:15Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:15Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:15Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:15Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:15Z- handleDvl() - RDI_Pathfinder msg: 1613171955.90 epoch sec; seqNo:1130 2021-02-12T23:19:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.01 , 0.00 2021-02-12T23:19:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171955.91 epoch sec; seqNo:2703; 4056695.18 north; 590764.70 east 2021-02-12T23:19:16Z- AHRS_M2 msg: 1613171956.29 epoch sec; seqNo:2679 2021-02-12T23:19:16Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.18 psi 2021-02-12T23:19:16Z- handleDepth()-Depth_Keller msg: 1613171956306 epoch sec; seqNo:2704; depth 15.26 2021-02-12T23:19:16Z- handleDvl() - RDI_Pathfinder msg: 1613171956.32 epoch sec; seqNo:1131 2021-02-12T23:19:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.81 , 94.53 , 71.09 , 104.26 2021-02-12T23:19:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , 0.00 2021-02-12T23:19:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171956.33 epoch sec; seqNo:2704; 4056695.57 north; 590765.92 east 2021-02-12T23:19:16Z- AHRS_M2 msg: 1613171956.70 epoch sec; seqNo:2680 2021-02-12T23:19:16Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.18 psi 2021-02-12T23:19:16Z- handleDepth()-Depth_Keller msg: 1613171956700 epoch sec; seqNo:2705; depth 15.37 2021-02-12T23:19:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171956.71 epoch sec; seqNo:2705; 4056695.57 north; 590765.92 east 2021-02-12T23:19:17Z- AHRS_M2 msg: 1613171957.10 epoch sec; seqNo:2681 2021-02-12T23:19:17Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.19 psi 2021-02-12T23:19:17Z- handleDepth()-Depth_Keller msg: 1613171957107 epoch sec; seqNo:2706; depth 15.47 2021-02-12T23:19:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:17Z- handleDvl() - RDI_Pathfinder msg: 1613171957.11 epoch sec; seqNo:1132 2021-02-12T23:19:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , 0.00 2021-02-12T23:19:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171957.12 epoch sec; seqNo:2706; 4056695.95 north; 590765.91 east 2021-02-12T23:19:17Z- AHRS_M2 msg: 1613171957.50 epoch sec; seqNo:2682 2021-02-12T23:19:17Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.20 psi 2021-02-12T23:19:17Z- handleDepth()-Depth_Keller msg: 1613171957508 epoch sec; seqNo:2707; depth 15.60 2021-02-12T23:19:17Z- handleDvl() - RDI_Pathfinder msg: 1613171957.53 epoch sec; seqNo:1133 2021-02-12T23:19:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.99 , 95.61 , 70.27 , 106.56 2021-02-12T23:19:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.05 2021-02-12T23:19:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171957.54 epoch sec; seqNo:2707; 4056696.33 north; 590765.91 east 2021-02-12T23:19:17Z- AHRS_M2 msg: 1613171957.91 epoch sec; seqNo:2683 2021-02-12T23:19:17Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.20 psi 2021-02-12T23:19:17Z- handleDepth()-Depth_Keller msg: 1613171957921 epoch sec; seqNo:2708; depth 15.73 2021-02-12T23:19:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171957.94 epoch sec; seqNo:2708; 4056696.33 north; 590765.91 east 2021-02-12T23:19:18Z- AHRS_M2 msg: 1613171958.32 epoch sec; seqNo:2684 2021-02-12T23:19:18Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.20 psi 2021-02-12T23:19:18Z- handleDepth()-Depth_Keller msg: 1613171958321 epoch sec; seqNo:2709; depth 15.84 2021-02-12T23:19:18Z- handleDvl() - RDI_Pathfinder msg: 1613171958.34 epoch sec; seqNo:1134 2021-02-12T23:19:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.99 , 97.98 , 70.27 , 105.27 2021-02-12T23:19:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , 0.05 2021-02-12T23:19:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171958.35 epoch sec; seqNo:2709; 4056696.70 north; 590765.91 east 2021-02-12T23:19:18Z- AHRS_M2 msg: 1613171958.71 epoch sec; seqNo:2685 2021-02-12T23:19:18Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.20 psi 2021-02-12T23:19:18Z- handleDepth()-Depth_Keller msg: 1613171958721 epoch sec; seqNo:2710; depth 15.95 2021-02-12T23:19:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171958.74 epoch sec; seqNo:2710; 4056697.10 north; 590767.12 east 2021-02-12T23:19:19Z- AHRS_M2 msg: 1613171959.12 epoch sec; seqNo:2686 2021-02-12T23:19:19Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.20 psi 2021-02-12T23:19:19Z- handleDepth()-Depth_Keller msg: 1613171959128 epoch sec; seqNo:2711; depth 16.07 2021-02-12T23:19:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171959.14 epoch sec; seqNo:2711; 4056697.47 north; 590767.12 east 2021-02-12T23:19:19Z- LcmTrn::handleMessages() - handling 43 messages took 0.61 ms and 131 calls... 2021-02-12T23:19:19Z- waiting for 1613171957.31, time is 1613171959.14 2021-02-12T23:19:19Z- LcmTrn::updateTrn() >>>> pose time 1613171959.14, meas time 1613171958.34 <<<< 2021-02-12T23:19:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171958.34 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:19:19Z- TerrainNav::motionUpdate() - currEstimate = (4056697, 590767.12, 16.07) 2021-02-12T23:19:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:19:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:19:19Z- TNavBF::Projecting Measurements 2021-02-12T23:19:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:19:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:19:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:19:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:19:19Z- TNavFilter::4 good beams. 2021-02-12T23:19:19Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:19:19Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:19:19Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:19:19Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:19:19Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:19:19Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:19:19Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:19:19Z- TNavBF::mmse selected from filter -1 2021-02-12T23:19:19Z- TerrainMapOctree: (vehN, vehE ) = (4056696.97, 590766.72). 2021-02-12T23:19:19Z- TerrainMapOctree: Min Distance = 5732.66. 2021-02-12T23:19:19Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:19:19Z- There is no specific good beam that all particles have in common. 2021-02-12T23:19:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:19:19Z- Old Depth Bias: 0.00 DepthBiasCov: 2.40 K: 0.179 ExpectedMeasDiff: nan currMeas.covariance: 9.24 2021-02-12T23:19:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:19:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.97 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-12T23:19:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:19:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.77 K: 0.155 ExpectedMeasDiff: nan currMeas.covariance: 7.90 2021-02-12T23:19:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:19:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.49 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 17.73 2021-02-12T23:19:19Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:19:19Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:19:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:19:19Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:19:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:19:19Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:19:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:19:19Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:19:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:19:19Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:19:19Z- Filter 0 No beams used in subcloud update 2021-02-12T23:19:19Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:19:19Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:19:19Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 778.3 EastVariance: 833.1 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-12T23:19:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171958.34 sec. 2021-02-12T23:19:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:19:19Z- TNavBF::mmse selected from filter -1 2021-02-12T23:19:19Z- variances([0], [2], and psi_berg) = 778.282167 833.065222 0.000000 2021-02-12T23:19:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:19:19Z- LcmTrn::publishEstimates() - MLE : 4056673.97 590720.62 16.07 2021-02-12T23:19:19Z- LcmTrn::publishEstimates() - MMSE : 4056697.47 590767.12 16.07 2021-02-12T23:19:19Z- LcmTrn::publishEstimates() - COVAR: 778.28 833.07 1.42 0.00 2021-02-12T23:19:19Z- AHRS_M2 msg: 1613171959.52 epoch sec; seqNo:2687 2021-02-12T23:19:19Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.21 psi 2021-02-12T23:19:19Z- handleDepth()-Depth_Keller msg: 1613171959528 epoch sec; seqNo:2712; depth 16.18 2021-02-12T23:19:19Z- handleDvl() - RDI_Pathfinder msg: 1613171959.54 epoch sec; seqNo:1135 2021-02-12T23:19:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.99 , 97.98 , 69.86 , 105.27 2021-02-12T23:19:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , 0.00 2021-02-12T23:19:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171959.55 epoch sec; seqNo:2712; 4056697.47 north; 590767.12 east 2021-02-12T23:19:19Z- AHRS_M2 msg: 1613171959.93 epoch sec; seqNo:2688 2021-02-12T23:19:19Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.21 psi 2021-02-12T23:19:19Z- handleDepth()-Depth_Keller msg: 1613171959938 epoch sec; seqNo:2713; depth 16.31 2021-02-12T23:19:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171959.96 epoch sec; seqNo:2713; 4056697.85 north; 590767.12 east 2021-02-12T23:19:20Z- AHRS_M2 msg: 1613171960.33 epoch sec; seqNo:2689 2021-02-12T23:19:20Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.21 psi 2021-02-12T23:19:20Z- handleDepth()-Depth_Keller msg: 1613171960344 epoch sec; seqNo:2714; depth 16.41 2021-02-12T23:19:20Z- handleDvl() - RDI_Pathfinder msg: 1613171960.38 epoch sec; seqNo:1136 2021-02-12T23:19:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.76 , 96.76 , 69.45 , 106.49 2021-02-12T23:19:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , -0.08 2021-02-12T23:19:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171960.39 epoch sec; seqNo:2714; 4056698.23 north; 590767.11 east 2021-02-12T23:19:20Z- AHRS_M2 msg: 1613171960.73 epoch sec; seqNo:2690 2021-02-12T23:19:20Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.21 psi 2021-02-12T23:19:20Z- handleDepth()-Depth_Keller msg: 1613171960745 epoch sec; seqNo:2715; depth 16.55 2021-02-12T23:19:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:20Z- handleDvl() - RDI_Pathfinder msg: 1613171960.75 epoch sec; seqNo:1137 2021-02-12T23:19:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , -0.08 2021-02-12T23:19:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171960.76 epoch sec; seqNo:2715; 4056698.61 north; 590767.11 east 2021-02-12T23:19:21Z- AHRS_M2 msg: 1613171961.14 epoch sec; seqNo:2691 2021-02-12T23:19:21Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.21 psi 2021-02-12T23:19:21Z- handleDepth()-Depth_Keller msg: 1613171961148 epoch sec; seqNo:2716; depth 16.67 2021-02-12T23:19:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171961.16 epoch sec; seqNo:2716; 4056698.62 north; 590768.33 east 2021-02-12T23:19:21Z- AHRS_M2 msg: 1613171961.54 epoch sec; seqNo:2692 2021-02-12T23:19:21Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.21 psi 2021-02-12T23:19:21Z- handleDepth()-Depth_Keller msg: 1613171961554 epoch sec; seqNo:2717; depth 16.78 2021-02-12T23:19:21Z- handleDvl() - RDI_Pathfinder msg: 1613171961.57 epoch sec; seqNo:1138 2021-02-12T23:19:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.94 , 95.61 , 69.45 , 102.91 2021-02-12T23:19:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , -0.10 2021-02-12T23:19:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171961.58 epoch sec; seqNo:2717; 4056699.00 north; 590768.32 east 2021-02-12T23:19:21Z- AHRS_M2 msg: 1613171961.95 epoch sec; seqNo:2693 2021-02-12T23:19:21Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.21 psi 2021-02-12T23:19:21Z- handleDepth()-Depth_Keller msg: 1613171961957 epoch sec; seqNo:2718; depth 16.89 2021-02-12T23:19:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:21Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:21Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:21Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:21Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:21Z- handleDvl() - RDI_Pathfinder msg: 1613171961.96 epoch sec; seqNo:1139 2021-02-12T23:19:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , -0.10 2021-02-12T23:19:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171961.98 epoch sec; seqNo:2718; 4056699.38 north; 590768.32 east 2021-02-12T23:19:22Z- AHRS_M2 msg: 1613171962.35 epoch sec; seqNo:2694 2021-02-12T23:19:22Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.21 psi 2021-02-12T23:19:22Z- handleDepth()-Depth_Keller msg: 1613171962361 epoch sec; seqNo:2719; depth 17.03 2021-02-12T23:19:22Z- handleDvl() - RDI_Pathfinder msg: 1613171962.37 epoch sec; seqNo:1140 2021-02-12T23:19:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.36 , 98.11 , 69.86 , 102.97 2021-02-12T23:19:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , -0.07 2021-02-12T23:19:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171962.40 epoch sec; seqNo:2719; 4056699.76 north; 590768.32 east 2021-02-12T23:19:22Z- AHRS_M2 msg: 1613171962.76 epoch sec; seqNo:2695 2021-02-12T23:19:22Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.21 psi 2021-02-12T23:19:22Z- handleDepth()-Depth_Keller msg: 1613171962761 epoch sec; seqNo:2720; depth 17.16 2021-02-12T23:19:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171962.77 epoch sec; seqNo:2720; 4056699.76 north; 590768.32 east 2021-02-12T23:19:23Z- AHRS_M2 msg: 1613171963.16 epoch sec; seqNo:2696 2021-02-12T23:19:23Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:23Z- handleDepth()-Depth_Keller msg: 1613171963175 epoch sec; seqNo:2721; depth 17.27 2021-02-12T23:19:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:23Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:23Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:23Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:23Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:23Z- handleDvl() - RDI_Pathfinder msg: 1613171963.18 epoch sec; seqNo:1141 2021-02-12T23:19:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , -0.07 2021-02-12T23:19:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171963.18 epoch sec; seqNo:2721; 4056700.14 north; 590768.31 east 2021-02-12T23:19:23Z- AHRS_M2 msg: 1613171963.56 epoch sec; seqNo:2697 2021-02-12T23:19:23Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:23Z- handleDepth()-Depth_Keller msg: 1613171963573 epoch sec; seqNo:2722; depth 17.41 2021-02-12T23:19:23Z- handleDvl() - RDI_Pathfinder msg: 1613171963.58 epoch sec; seqNo:1142 2021-02-12T23:19:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.54 , 94.39 , 69.05 , 104.12 2021-02-12T23:19:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.09 , 0.00 2021-02-12T23:19:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171963.61 epoch sec; seqNo:2722; 4056700.53 north; 590769.53 east 2021-02-12T23:19:23Z- AHRS_M2 msg: 1613171963.97 epoch sec; seqNo:2698 2021-02-12T23:19:23Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:23Z- handleDepth()-Depth_Keller msg: 1613171963972 epoch sec; seqNo:2723; depth 17.53 2021-02-12T23:19:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171963.98 epoch sec; seqNo:2723; 4056700.91 north; 590769.53 east 2021-02-12T23:19:24Z- AHRS_M2 msg: 1613171964.37 epoch sec; seqNo:2699 2021-02-12T23:19:24Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.21 psi 2021-02-12T23:19:24Z- handleDepth()-Depth_Keller msg: 1613171964381 epoch sec; seqNo:2724; depth 17.63 2021-02-12T23:19:24Z- handleDvl() - RDI_Pathfinder msg: 1613171964.39 epoch sec; seqNo:1143 2021-02-12T23:19:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.35 , 98.18 , 68.63 , 103.04 2021-02-12T23:19:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.07 , 0.04 2021-02-12T23:19:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171964.40 epoch sec; seqNo:2724; 4056700.91 north; 590769.53 east 2021-02-12T23:19:24Z- LcmTrn::handleMessages() - handling 48 messages took 0.73 ms and 134 calls... 2021-02-12T23:19:24Z- waiting for 1613171962.14, time is 1613171964.40 2021-02-12T23:19:24Z- LcmTrn::updateTrn() equating pose 1613171964.40 and meas times 1613171964.40 2021-02-12T23:19:24Z- LcmTrn::updateTrn() >>>> pose time 1613171964.40, meas time 1613171964.40 <<<< 2021-02-12T23:19:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171964.40 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:19:24Z- TerrainNav::motionUpdate() - currEstimate = (4056701, 590769.53, 17.63) 2021-02-12T23:19:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:19:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:19:24Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171964.40 sec.; No relevant navigation data available 2021-02-12T23:19:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:19:24Z- TNavBF::mmse selected from filter -1 2021-02-12T23:19:24Z- variances([0], [2], and psi_berg) = 778.130433 833.065009 0.000000 2021-02-12T23:19:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:19:24Z- LcmTrn::publishEstimates() - MLE : 4056677.41 590723.03 17.63 2021-02-12T23:19:24Z- LcmTrn::publishEstimates() - MMSE : 4056700.91 590769.53 17.63 2021-02-12T23:19:24Z- LcmTrn::publishEstimates() - COVAR: 778.13 833.07 1.63 0.00 2021-02-12T23:19:24Z- AHRS_M2 msg: 1613171964.78 epoch sec; seqNo:2700 2021-02-12T23:19:24Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.21 psi 2021-02-12T23:19:24Z- handleDepth()-Depth_Keller msg: 1613171964785 epoch sec; seqNo:2725; depth 17.76 2021-02-12T23:19:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171964.80 epoch sec; seqNo:2725; 4056701.29 north; 590769.52 east 2021-02-12T23:19:25Z- AHRS_M2 msg: 1613171965.19 epoch sec; seqNo:2701 2021-02-12T23:19:25Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.21 psi 2021-02-12T23:19:25Z- handleDepth()-Depth_Keller msg: 1613171965197 epoch sec; seqNo:2726; depth 17.87 2021-02-12T23:19:25Z- handleDvl() - RDI_Pathfinder msg: 1613171965.21 epoch sec; seqNo:1144 2021-02-12T23:19:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.94 , 98.11 , 69.04 , 102.98 2021-02-12T23:19:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.05 2021-02-12T23:19:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171965.23 epoch sec; seqNo:2726; 4056701.67 north; 590769.52 east 2021-02-12T23:19:25Z- AHRS_M2 msg: 1613171965.58 epoch sec; seqNo:2702 2021-02-12T23:19:25Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.21 psi 2021-02-12T23:19:25Z- handleDepth()-Depth_Keller msg: 1613171965588 epoch sec; seqNo:2727; depth 17.99 2021-02-12T23:19:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:25Z- handleDvl() - RDI_Pathfinder msg: 1613171965.59 epoch sec; seqNo:1145 2021-02-12T23:19:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.05 2021-02-12T23:19:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171965.60 epoch sec; seqNo:2727; 4056701.67 north; 590769.52 east 2021-02-12T23:19:25Z- AHRS_M2 msg: 1613171965.99 epoch sec; seqNo:2703 2021-02-12T23:19:25Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.21 psi 2021-02-12T23:19:26Z- handleDepth()-Depth_Keller msg: 1613171966001 epoch sec; seqNo:2728; depth 18.11 2021-02-12T23:19:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171966.01 epoch sec; seqNo:2728; 4056702.05 north; 590769.51 east 2021-02-12T23:19:26Z- AHRS_M2 msg: 1613171966.39 epoch sec; seqNo:2704 2021-02-12T23:19:26Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.21 psi 2021-02-12T23:19:26Z- handleDepth()-Depth_Keller msg: 1613171966396 epoch sec; seqNo:2729; depth 18.23 2021-02-12T23:19:26Z- handleDvl() - RDI_Pathfinder msg: 1613171966.42 epoch sec; seqNo:1146 2021-02-12T23:19:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.54 , 98.11 , 68.22 , 102.98 2021-02-12T23:19:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , 0.03 2021-02-12T23:19:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171966.43 epoch sec; seqNo:2729; 4056702.43 north; 590769.51 east 2021-02-12T23:19:26Z- AHRS_M2 msg: 1613171966.80 epoch sec; seqNo:2705 2021-02-12T23:19:26Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.21 psi 2021-02-12T23:19:26Z- handleDepth()-Depth_Keller msg: 1613171966801 epoch sec; seqNo:2730; depth 18.35 2021-02-12T23:19:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:26Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:26Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:26Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:26Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:26Z- handleDvl() - RDI_Pathfinder msg: 1613171966.80 epoch sec; seqNo:1147 2021-02-12T23:19:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , 0.03 2021-02-12T23:19:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171966.81 epoch sec; seqNo:2730; 4056702.44 north; 590770.73 east 2021-02-12T23:19:27Z- AHRS_M2 msg: 1613171967.20 epoch sec; seqNo:2706 2021-02-12T23:19:27Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.21 psi 2021-02-12T23:19:27Z- handleDepth()-Depth_Keller msg: 1613171967205 epoch sec; seqNo:2731; depth 18.47 2021-02-12T23:19:27Z- handleDvl() - RDI_Pathfinder msg: 1613171967.22 epoch sec; seqNo:1148 2021-02-12T23:19:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.54 , 98.11 , 68.22 , 101.76 2021-02-12T23:19:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.01 , 0.00 2021-02-12T23:19:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171967.24 epoch sec; seqNo:2731; 4056702.82 north; 590770.73 east 2021-02-12T23:19:27Z- AHRS_M2 msg: 1613171967.60 epoch sec; seqNo:2707 2021-02-12T23:19:27Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:27Z- handleDepth()-Depth_Keller msg: 1613171967611 epoch sec; seqNo:2732; depth 18.60 2021-02-12T23:19:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171967.62 epoch sec; seqNo:2732; 4056703.20 north; 590770.72 east 2021-02-12T23:19:28Z- AHRS_M2 msg: 1613171968.01 epoch sec; seqNo:2708 2021-02-12T23:19:28Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:28Z- handleDepth()-Depth_Keller msg: 1613171968012 epoch sec; seqNo:2733; depth 18.72 2021-02-12T23:19:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:28Z- handleDvl() - RDI_Pathfinder msg: 1613171968.01 epoch sec; seqNo:1149 2021-02-12T23:19:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.01 , 0.00 2021-02-12T23:19:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171968.04 epoch sec; seqNo:2733; 4056703.57 north; 590770.72 east 2021-02-12T23:19:28Z- AHRS_M2 msg: 1613171968.41 epoch sec; seqNo:2709 2021-02-12T23:19:28Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:28Z- handleDepth()-Depth_Keller msg: 1613171968425 epoch sec; seqNo:2734; depth 18.84 2021-02-12T23:19:28Z- handleDvl() - RDI_Pathfinder msg: 1613171968.44 epoch sec; seqNo:1150 2021-02-12T23:19:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.13 , 0.00 , 68.23 , 101.82 2021-02-12T23:19:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.03 2021-02-12T23:19:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171968.46 epoch sec; seqNo:2734; 4056703.57 north; 590770.72 east 2021-02-12T23:19:28Z- AHRS_M2 msg: 1613171968.82 epoch sec; seqNo:2710 2021-02-12T23:19:28Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:28Z- handleDepth()-Depth_Keller msg: 1613171968827 epoch sec; seqNo:2735; depth 18.98 2021-02-12T23:19:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171968.84 epoch sec; seqNo:2735; 4056703.95 north; 590770.71 east 2021-02-12T23:19:29Z- AHRS_M2 msg: 1613171969.23 epoch sec; seqNo:2711 2021-02-12T23:19:29Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:29Z- handleDepth()-Depth_Keller msg: 1613171969230 epoch sec; seqNo:2736; depth 19.09 2021-02-12T23:19:29Z- handleDvl() - RDI_Pathfinder msg: 1613171969.24 epoch sec; seqNo:1151 2021-02-12T23:19:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.13 , 97.84 , 67.82 , 101.08 2021-02-12T23:19:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.04 2021-02-12T23:19:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171969.25 epoch sec; seqNo:2736; 4056704.35 north; 590771.93 east 2021-02-12T23:19:29Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-12T23:19:29Z- waiting for 1613171967.40, time is 1613171969.25 2021-02-12T23:19:29Z- LcmTrn::updateTrn() equating pose 1613171969.25 and meas times 1613171969.25 2021-02-12T23:19:29Z- LcmTrn::updateTrn() >>>> pose time 1613171969.25, meas time 1613171969.25 <<<< 2021-02-12T23:19:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171969.25 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:19:29Z- TerrainNav::motionUpdate() - currEstimate = (4056704, 590771.93, 19.09) 2021-02-12T23:19:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:19:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:19:29Z- TNavBF::Projecting Measurements 2021-02-12T23:19:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:19:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:19:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:19:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:19:29Z- TNavFilter::4 good beams. 2021-02-12T23:19:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:19:29Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:19:29Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:19:29Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:19:29Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:19:29Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:19:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:19:29Z- TNavBF::mmse selected from filter -1 2021-02-12T23:19:29Z- TerrainMapOctree: (vehN, vehE ) = (4056704.35, 590771.93). 2021-02-12T23:19:29Z- TerrainMapOctree: Min Distance = 5723.63. 2021-02-12T23:19:29Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:19:29Z- There is no specific good beam that all particles have in common. 2021-02-12T23:19:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:19:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.85 K: 0.152 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-12T23:19:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:19:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.57 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-12T23:19:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:19:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.136 ExpectedMeasDiff: nan currMeas.covariance: 7.36 2021-02-12T23:19:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:19:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 16.35 2021-02-12T23:19:29Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:19:29Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:19:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:19:29Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:19:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:19:29Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:19:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:19:29Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:19:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:19:29Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:19:29Z- Filter 0 No beams used in subcloud update 2021-02-12T23:19:29Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:19:29Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:19:29Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 778.0 EastVariance: 833.1 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:19:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171969.25 sec. 2021-02-12T23:19:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:19:29Z- TNavBF::mmse selected from filter -1 2021-02-12T23:19:29Z- variances([0], [2], and psi_berg) = 777.979086 833.064798 0.000000 2021-02-12T23:19:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:19:29Z- LcmTrn::publishEstimates() - MLE : 4056680.85 590725.43 19.09 2021-02-12T23:19:29Z- LcmTrn::publishEstimates() - MMSE : 4056704.35 590771.93 19.09 2021-02-12T23:19:29Z- LcmTrn::publishEstimates() - COVAR: 777.98 833.06 1.16 0.00 2021-02-12T23:19:29Z- AHRS_M2 msg: 1613171969.62 epoch sec; seqNo:2712 2021-02-12T23:19:29Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:29Z- handleDepth()-Depth_Keller msg: 1613171969633 epoch sec; seqNo:2737; depth 19.23 2021-02-12T23:19:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171969.65 epoch sec; seqNo:2737; 4056704.72 north; 590771.93 east 2021-02-12T23:19:30Z- AHRS_M2 msg: 1613171970.03 epoch sec; seqNo:2713 2021-02-12T23:19:30Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:30Z- handleDepth()-Depth_Keller msg: 1613171970032 epoch sec; seqNo:2738; depth 19.33 2021-02-12T23:19:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171970.04 epoch sec; seqNo:2738; 4056704.72 north; 590771.93 east 2021-02-12T23:19:30Z- AHRS_M2 msg: 1613171970.43 epoch sec; seqNo:2714 2021-02-12T23:19:30Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:30Z- handleDepth()-Depth_Keller msg: 1613171970437 epoch sec; seqNo:2739; depth 19.46 2021-02-12T23:19:30Z- handleDvl() - RDI_Pathfinder msg: 1613171970.45 epoch sec; seqNo:1152 2021-02-12T23:19:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.31 , 92.10 , 67.41 , 100.61 2021-02-12T23:19:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.03 2021-02-12T23:19:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171970.46 epoch sec; seqNo:2739; 4056705.10 north; 590771.92 east 2021-02-12T23:19:30Z- AHRS_M2 msg: 1613171970.84 epoch sec; seqNo:2715 2021-02-12T23:19:30Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:30Z- handleDepth()-Depth_Keller msg: 1613171970849 epoch sec; seqNo:2740; depth 19.59 2021-02-12T23:19:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171970.86 epoch sec; seqNo:2740; 4056705.48 north; 590771.92 east 2021-02-12T23:19:31Z- AHRS_M2 msg: 1613171971.24 epoch sec; seqNo:2716 2021-02-12T23:19:31Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:31Z- handleDepth()-Depth_Keller msg: 1613171971249 epoch sec; seqNo:2741; depth 19.71 2021-02-12T23:19:31Z- handleDvl() - RDI_Pathfinder msg: 1613171971.27 epoch sec; seqNo:1153 2021-02-12T23:19:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.31 , 98.31 , 67.41 , 100.75 2021-02-12T23:19:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , 0.00 2021-02-12T23:19:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171971.28 epoch sec; seqNo:2741; 4056705.86 north; 590771.91 east 2021-02-12T23:19:31Z- AHRS_M2 msg: 1613171971.64 epoch sec; seqNo:2717 2021-02-12T23:19:31Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:31Z- handleDepth()-Depth_Keller msg: 1613171971647 epoch sec; seqNo:2742; depth 19.84 2021-02-12T23:19:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:31Z- handleDvl() - RDI_Pathfinder msg: 1613171971.65 epoch sec; seqNo:1154 2021-02-12T23:19:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , 0.00 2021-02-12T23:19:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171971.66 epoch sec; seqNo:2742; 4056705.86 north; 590771.91 east 2021-02-12T23:19:32Z- AHRS_M2 msg: 1613171972.05 epoch sec; seqNo:2718 2021-02-12T23:19:32Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:32Z- handleDepth()-Depth_Keller msg: 1613171972057 epoch sec; seqNo:2743; depth 19.96 2021-02-12T23:19:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171972.07 epoch sec; seqNo:2743; 4056706.25 north; 590773.13 east 2021-02-12T23:19:32Z- AHRS_M2 msg: 1613171972.45 epoch sec; seqNo:2719 2021-02-12T23:19:32Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:32Z- handleDepth()-Depth_Keller msg: 1613171972458 epoch sec; seqNo:2744; depth 20.06 2021-02-12T23:19:32Z- handleDvl() - RDI_Pathfinder msg: 1613171972.47 epoch sec; seqNo:1155 2021-02-12T23:19:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.90 , 0.00 , 66.59 , 101.83 2021-02-12T23:19:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.01 2021-02-12T23:19:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171972.48 epoch sec; seqNo:2744; 4056706.63 north; 590773.13 east 2021-02-12T23:19:32Z- AHRS_M2 msg: 1613171972.86 epoch sec; seqNo:2720 2021-02-12T23:19:32Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:32Z- handleDepth()-Depth_Keller msg: 1613171972864 epoch sec; seqNo:2745; depth 20.20 2021-02-12T23:19:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:32Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:32Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:32Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:32Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:32Z- handleDvl() - RDI_Pathfinder msg: 1613171972.87 epoch sec; seqNo:1156 2021-02-12T23:19:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.01 2021-02-12T23:19:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171972.89 epoch sec; seqNo:2745; 4056707.01 north; 590773.12 east 2021-02-12T23:19:33Z- AHRS_M2 msg: 1613171973.26 epoch sec; seqNo:2721 2021-02-12T23:19:33Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:33Z- handleDepth()-Depth_Keller msg: 1613171973295 epoch sec; seqNo:2746; depth 20.32 2021-02-12T23:19:33Z- handleDvl() - RDI_Pathfinder msg: 1613171973.31 epoch sec; seqNo:1157 2021-02-12T23:19:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.90 , 0.00 , 66.18 , 100.07 2021-02-12T23:19:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.00 , -0.04 2021-02-12T23:19:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171973.32 epoch sec; seqNo:2746; 4056707.01 north; 590773.12 east 2021-02-12T23:19:33Z- AHRS_M2 msg: 1613171973.66 epoch sec; seqNo:2722 2021-02-12T23:19:33Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-12T23:19:33Z- handleDepth()-Depth_Keller msg: 1613171973670 epoch sec; seqNo:2747; depth 20.44 2021-02-12T23:19:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171973.68 epoch sec; seqNo:2747; 4056707.39 north; 590773.12 east 2021-02-12T23:19:34Z- AHRS_M2 msg: 1613171974.07 epoch sec; seqNo:2723 2021-02-12T23:19:34Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-12T23:19:34Z- handleDepth()-Depth_Keller msg: 1613171974076 epoch sec; seqNo:2748; depth 20.57 2021-02-12T23:19:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:34Z- handleDvl() - RDI_Pathfinder msg: 1613171974.08 epoch sec; seqNo:1158 2021-02-12T23:19:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.00 , -0.04 2021-02-12T23:19:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171974.12 epoch sec; seqNo:2748; 4056707.77 north; 590773.12 east 2021-02-12T23:19:34Z- AHRS_M2 msg: 1613171974.48 epoch sec; seqNo:2724 2021-02-12T23:19:34Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-12T23:19:34Z- handleDepth()-Depth_Keller msg: 1613171974483 epoch sec; seqNo:2749; depth 20.67 2021-02-12T23:19:34Z- handleDvl() - RDI_Pathfinder msg: 1613171974.49 epoch sec; seqNo:1159 2021-02-12T23:19:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.50 , 90.34 , 66.19 , 101.15 2021-02-12T23:19:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , -0.08 2021-02-12T23:19:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171974.51 epoch sec; seqNo:2749; 4056708.16 north; 590774.33 east 2021-02-12T23:19:34Z- LcmTrn::handleMessages() - handling 47 messages took 0.72 ms and 133 calls... 2021-02-12T23:19:34Z- waiting for 1613171972.25, time is 1613171974.51 2021-02-12T23:19:34Z- LcmTrn::updateTrn() equating pose 1613171974.51 and meas times 1613171974.51 2021-02-12T23:19:34Z- LcmTrn::updateTrn() >>>> pose time 1613171974.51, meas time 1613171974.51 <<<< 2021-02-12T23:19:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171974.51 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:19:34Z- TerrainNav::motionUpdate() - currEstimate = (4056708, 590774.33, 20.67) 2021-02-12T23:19:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:19:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:19:34Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171974.51 sec.; No relevant navigation data available 2021-02-12T23:19:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:19:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:19:34Z- variances([0], [2], and psi_berg) = 777.822586 833.064580 0.000000 2021-02-12T23:19:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:19:34Z- LcmTrn::publishEstimates() - MLE : 4056685.66 590727.83 20.67 2021-02-12T23:19:34Z- LcmTrn::publishEstimates() - MMSE : 4056708.16 590774.33 20.67 2021-02-12T23:19:34Z- LcmTrn::publishEstimates() - COVAR: 777.82 833.06 1.39 0.00 2021-02-12T23:19:34Z- AHRS_M2 msg: 1613171974.88 epoch sec; seqNo:2725 2021-02-12T23:19:34Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-12T23:19:34Z- handleDepth()-Depth_Keller msg: 1613171974890 epoch sec; seqNo:2750; depth 20.82 2021-02-12T23:19:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171974.90 epoch sec; seqNo:2750; 4056708.16 north; 590774.33 east 2021-02-12T23:19:35Z- AHRS_M2 msg: 1613171975.28 epoch sec; seqNo:2726 2021-02-12T23:19:35Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-12T23:19:35Z- handleDepth()-Depth_Keller msg: 1613171975289 epoch sec; seqNo:2751; depth 20.92 2021-02-12T23:19:35Z- handleDvl() - RDI_Pathfinder msg: 1613171975.30 epoch sec; seqNo:1160 2021-02-12T23:19:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.09 , 0.00 , 65.78 , 101.01 2021-02-12T23:19:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , -0.08 2021-02-12T23:19:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171975.31 epoch sec; seqNo:2751; 4056708.54 north; 590774.33 east 2021-02-12T23:19:35Z- AHRS_M2 msg: 1613171975.68 epoch sec; seqNo:2727 2021-02-12T23:19:35Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-12T23:19:35Z- handleDepth()-Depth_Keller msg: 1613171975691 epoch sec; seqNo:2752; depth 21.07 2021-02-12T23:19:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171975.71 epoch sec; seqNo:2752; 4056708.92 north; 590774.32 east 2021-02-12T23:19:36Z- AHRS_M2 msg: 1613171976.09 epoch sec; seqNo:2728 2021-02-12T23:19:36Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-12T23:19:36Z- handleDepth()-Depth_Keller msg: 1613171976092 epoch sec; seqNo:2753; depth 21.18 2021-02-12T23:19:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171976.11 epoch sec; seqNo:2753; 4056709.30 north; 590774.32 east 2021-02-12T23:19:36Z- AHRS_M2 msg: 1613171976.50 epoch sec; seqNo:2729 2021-02-12T23:19:36Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-12T23:19:36Z- handleDepth()-Depth_Keller msg: 1613171976510 epoch sec; seqNo:2754; depth 21.31 2021-02-12T23:19:36Z- handleDvl() - RDI_Pathfinder msg: 1613171976.55 epoch sec; seqNo:1161 2021-02-12T23:19:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.49 , 89.26 , 65.36 , 100.07 2021-02-12T23:19:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , -0.06 2021-02-12T23:19:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171976.56 epoch sec; seqNo:2754; 4056709.68 north; 590774.32 east 2021-02-12T23:19:36Z- AHRS_M2 msg: 1613171976.89 epoch sec; seqNo:2730 2021-02-12T23:19:36Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-12T23:19:36Z- handleDepth()-Depth_Keller msg: 1613171976909 epoch sec; seqNo:2755; depth 21.45 2021-02-12T23:19:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171976.92 epoch sec; seqNo:2755; 4056709.69 north; 590775.54 east 2021-02-12T23:19:37Z- AHRS_M2 msg: 1613171977.30 epoch sec; seqNo:2731 2021-02-12T23:19:37Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.17 psi 2021-02-12T23:19:37Z- handleDepth()-Depth_Keller msg: 1613171977304 epoch sec; seqNo:2756; depth 21.56 2021-02-12T23:19:37Z- handleDvl() - RDI_Pathfinder msg: 1613171977.32 epoch sec; seqNo:1162 2021-02-12T23:19:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.68 , 0.00 , 64.14 , 102.10 2021-02-12T23:19:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , -0.01 2021-02-12T23:19:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171977.33 epoch sec; seqNo:2756; 4056710.07 north; 590775.53 east 2021-02-12T23:19:37Z- AHRS_M2 msg: 1613171977.70 epoch sec; seqNo:2732 2021-02-12T23:19:37Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.17 psi 2021-02-12T23:19:37Z- handleDepth()-Depth_Keller msg: 1613171977708 epoch sec; seqNo:2757; depth 21.68 2021-02-12T23:19:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:37Z- handleDvl() - RDI_Pathfinder msg: 1613171977.71 epoch sec; seqNo:1163 2021-02-12T23:19:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , -0.01 2021-02-12T23:19:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171977.72 epoch sec; seqNo:2757; 4056710.45 north; 590775.53 east 2021-02-12T23:19:38Z- AHRS_M2 msg: 1613171978.12 epoch sec; seqNo:2733 2021-02-12T23:19:38Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.17 psi 2021-02-12T23:19:38Z- handleDepth()-Depth_Keller msg: 1613171978134 epoch sec; seqNo:2758; depth 21.81 2021-02-12T23:19:38Z- handleDvl() - RDI_Pathfinder msg: 1613171978.16 epoch sec; seqNo:1164 2021-02-12T23:19:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.09 , 0.00 , 64.55 , 98.99 2021-02-12T23:19:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , 0.03 2021-02-12T23:19:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171978.17 epoch sec; seqNo:2758; 4056710.45 north; 590775.53 east 2021-02-12T23:19:38Z- handleDepth()-Depth_Keller msg: 1613171978537 epoch sec; seqNo:2759; depth 21.94 2021-02-12T23:19:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171978.55 epoch sec; seqNo:2759; 4056710.82 north; 590775.53 east 2021-02-12T23:19:38Z- AHRS_M2 msg: 1613171978.92 epoch sec; seqNo:2734 2021-02-12T23:19:38Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.17 psi 2021-02-12T23:19:38Z- handleDepth()-Depth_Keller msg: 1613171978921 epoch sec; seqNo:2760; depth 22.06 2021-02-12T23:19:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:38Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:38Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:38Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:38Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:38Z- handleDvl() - RDI_Pathfinder msg: 1613171978.92 epoch sec; seqNo:1165 2021-02-12T23:19:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , 0.03 2021-02-12T23:19:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171978.93 epoch sec; seqNo:2760; 4056711.20 north; 590775.52 east 2021-02-12T23:19:39Z- AHRS_M2 msg: 1613171979.32 epoch sec; seqNo:2735 2021-02-12T23:19:39Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.17 psi 2021-02-12T23:19:39Z- handleDepth()-Depth_Keller msg: 1613171979324 epoch sec; seqNo:2761; depth 22.19 2021-02-12T23:19:39Z- handleDvl() - RDI_Pathfinder msg: 1613171979.34 epoch sec; seqNo:1166 2021-02-12T23:19:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.09 , 87.09 , 64.14 , 100.07 2021-02-12T23:19:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , 0.06 2021-02-12T23:19:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171979.35 epoch sec; seqNo:2761; 4056711.20 north; 590775.52 east 2021-02-12T23:19:39Z- LcmTrn::handleMessages() - handling 42 messages took 0.62 ms and 130 calls... 2021-02-12T23:19:39Z- waiting for 1613171977.51, time is 1613171979.35 2021-02-12T23:19:39Z- LcmTrn::updateTrn() equating pose 1613171979.35 and meas times 1613171979.35 2021-02-12T23:19:39Z- LcmTrn::updateTrn() >>>> pose time 1613171979.35, meas time 1613171979.35 <<<< 2021-02-12T23:19:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171979.35 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:19:39Z- TerrainNav::motionUpdate() - currEstimate = (4056711, 590775.52, 22.19) 2021-02-12T23:19:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:19:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:19:39Z- TNavBF::Projecting Measurements 2021-02-12T23:19:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:19:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:19:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:19:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:19:39Z- TNavFilter::4 good beams. 2021-02-12T23:19:39Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:19:39Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:19:39Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:19:39Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:19:39Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:19:39Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:19:39Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:19:39Z- TNavBF::mmse selected from filter -1 2021-02-12T23:19:39Z- TerrainMapOctree: (vehN, vehE ) = (4056711.20, 590775.52). 2021-02-12T23:19:39Z- TerrainMapOctree: Min Distance = 5716.00. 2021-02-12T23:19:39Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:19:39Z- There is no specific good beam that all particles have in common. 2021-02-12T23:19:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:19:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.56 K: 0.137 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-12T23:19:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:19:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 12.14 2021-02-12T23:19:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:19:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.128 ExpectedMeasDiff: nan currMeas.covariance: 6.58 2021-02-12T23:19:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:19:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 16.02 2021-02-12T23:19:39Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:19:39Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:19:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:19:39Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:19:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:19:39Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:19:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:19:39Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:19:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:19:39Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:19:39Z- Filter 0 No beams used in subcloud update 2021-02-12T23:19:39Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:19:39Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:19:39Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 777.7 EastVariance: 833.1 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:19:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171979.35 sec. 2021-02-12T23:19:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:19:39Z- TNavBF::mmse selected from filter -1 2021-02-12T23:19:39Z- variances([0], [2], and psi_berg) = 777.689689 833.064396 0.000000 2021-02-12T23:19:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:19:39Z- LcmTrn::publishEstimates() - MLE : 4056688.70 590729.02 22.19 2021-02-12T23:19:39Z- LcmTrn::publishEstimates() - MMSE : 4056711.20 590775.52 22.19 2021-02-12T23:19:39Z- LcmTrn::publishEstimates() - COVAR: 777.69 833.06 1.01 0.00 2021-02-12T23:19:39Z- AHRS_M2 msg: 1613171979.74 epoch sec; seqNo:2736 2021-02-12T23:19:39Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-12T23:19:39Z- handleDepth()-Depth_Keller msg: 1613171979741 epoch sec; seqNo:2762; depth 22.31 2021-02-12T23:19:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171979.75 epoch sec; seqNo:2762; 4056711.58 north; 590775.52 east 2021-02-12T23:19:40Z- AHRS_M2 msg: 1613171980.13 epoch sec; seqNo:2737 2021-02-12T23:19:40Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-12T23:19:40Z- handleDepth()-Depth_Keller msg: 1613171980135 epoch sec; seqNo:2763; depth 22.44 2021-02-12T23:19:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:40Z- handleDvl() - RDI_Pathfinder msg: 1613171980.15 epoch sec; seqNo:1167 2021-02-12T23:19:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , 0.06 2021-02-12T23:19:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171980.16 epoch sec; seqNo:2763; 4056711.97 north; 590776.73 east 2021-02-12T23:19:40Z- AHRS_M2 msg: 1613171980.53 epoch sec; seqNo:2738 2021-02-12T23:19:40Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-12T23:19:40Z- handleDepth()-Depth_Keller msg: 1613171980536 epoch sec; seqNo:2764; depth 22.57 2021-02-12T23:19:40Z- handleDvl() - RDI_Pathfinder msg: 1613171980.55 epoch sec; seqNo:1168 2021-02-12T23:19:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.09 , 87.09 , 63.74 , 100.07 2021-02-12T23:19:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , 0.07 2021-02-12T23:19:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171980.57 epoch sec; seqNo:2764; 4056711.97 north; 590776.73 east 2021-02-12T23:19:40Z- AHRS_M2 msg: 1613171980.94 epoch sec; seqNo:2739 2021-02-12T23:19:40Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-12T23:19:40Z- handleDepth()-Depth_Keller msg: 1613171980940 epoch sec; seqNo:2765; depth 22.68 2021-02-12T23:19:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171980.95 epoch sec; seqNo:2765; 4056712.35 north; 590776.73 east 2021-02-12T23:19:41Z- AHRS_M2 msg: 1613171981.34 epoch sec; seqNo:2740 2021-02-12T23:19:41Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-12T23:19:41Z- handleDepth()-Depth_Keller msg: 1613171981345 epoch sec; seqNo:2766; depth 22.81 2021-02-12T23:19:41Z- handleDvl() - RDI_Pathfinder msg: 1613171981.36 epoch sec; seqNo:1169 2021-02-12T23:19:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.68 , 87.09 , 64.14 , 96.82 2021-02-12T23:19:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.03 , 0.07 2021-02-12T23:19:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171981.37 epoch sec; seqNo:2766; 4056712.73 north; 590776.73 east 2021-02-12T23:19:41Z- AHRS_M2 msg: 1613171981.74 epoch sec; seqNo:2741 2021-02-12T23:19:41Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-12T23:19:41Z- handleDepth()-Depth_Keller msg: 1613171981748 epoch sec; seqNo:2767; depth 22.95 2021-02-12T23:19:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171981.77 epoch sec; seqNo:2767; 4056712.73 north; 590776.73 east 2021-02-12T23:19:42Z- AHRS_M2 msg: 1613171982.15 epoch sec; seqNo:2742 2021-02-12T23:19:42Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-12T23:19:42Z- handleDepth()-Depth_Keller msg: 1613171982158 epoch sec; seqNo:2768; depth 23.06 2021-02-12T23:19:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171982.17 epoch sec; seqNo:2768; 4056713.11 north; 590776.72 east 2021-02-12T23:19:42Z- AHRS_M2 msg: 1613171982.55 epoch sec; seqNo:2743 2021-02-12T23:19:42Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.18 psi 2021-02-12T23:19:42Z- handleDepth()-Depth_Keller msg: 1613171982557 epoch sec; seqNo:2769; depth 23.18 2021-02-12T23:19:42Z- handleDvl() - RDI_Pathfinder msg: 1613171982.57 epoch sec; seqNo:1170 2021-02-12T23:19:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.27 , 158.45 , 64.14 , 96.83 2021-02-12T23:19:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.11 , 0.09 2021-02-12T23:19:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171982.58 epoch sec; seqNo:2769; 4056713.50 north; 590777.94 east 2021-02-12T23:19:42Z- AHRS_M2 msg: 1613171982.95 epoch sec; seqNo:2744 2021-02-12T23:19:42Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.18 psi 2021-02-12T23:19:42Z- handleDepth()-Depth_Keller msg: 1613171982962 epoch sec; seqNo:2770; depth 23.31 2021-02-12T23:19:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171982.97 epoch sec; seqNo:2770; 4056713.50 north; 590777.94 east 2021-02-12T23:19:43Z- AHRS_M2 msg: 1613171983.36 epoch sec; seqNo:2745 2021-02-12T23:19:43Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.18 psi 2021-02-12T23:19:43Z- handleDepth()-Depth_Keller msg: 1613171983369 epoch sec; seqNo:2771; depth 23.44 2021-02-12T23:19:43Z- handleDvl() - RDI_Pathfinder msg: 1613171983.39 epoch sec; seqNo:1171 2021-02-12T23:19:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.86 , 87.77 , 63.32 , 95.88 2021-02-12T23:19:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.10 , 0.02 2021-02-12T23:19:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171983.40 epoch sec; seqNo:2771; 4056713.88 north; 590777.94 east 2021-02-12T23:19:43Z- AHRS_M2 msg: 1613171983.77 epoch sec; seqNo:2746 2021-02-12T23:19:43Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-12T23:19:43Z- handleDepth()-Depth_Keller msg: 1613171983769 epoch sec; seqNo:2772; depth 23.56 2021-02-12T23:19:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:43Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:43Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:43Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:43Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:43Z- handleDvl() - RDI_Pathfinder msg: 1613171983.77 epoch sec; seqNo:1172 2021-02-12T23:19:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.10 , 0.02 2021-02-12T23:19:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171983.78 epoch sec; seqNo:2772; 4056714.26 north; 590777.93 east 2021-02-12T23:19:44Z- AHRS_M2 msg: 1613171984.17 epoch sec; seqNo:2747 2021-02-12T23:19:44Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.19 psi 2021-02-12T23:19:44Z- handleDepth()-Depth_Keller msg: 1613171984181 epoch sec; seqNo:2773; depth 23.68 2021-02-12T23:19:44Z- handleDvl() - RDI_Pathfinder msg: 1613171984.19 epoch sec; seqNo:1173 2021-02-12T23:19:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.86 , 0.00 , 63.33 , 96.82 2021-02-12T23:19:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.09 , -0.04 2021-02-12T23:19:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171984.20 epoch sec; seqNo:2773; 4056714.26 north; 590777.93 east 2021-02-12T23:19:44Z- AHRS_M2 msg: 1613171984.57 epoch sec; seqNo:2748 2021-02-12T23:19:44Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.19 psi 2021-02-12T23:19:44Z- handleDepth()-Depth_Keller msg: 1613171984576 epoch sec; seqNo:2774; depth 23.81 2021-02-12T23:19:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171984.60 epoch sec; seqNo:2774; 4056714.65 north; 590779.15 east 2021-02-12T23:19:44Z- LcmTrn::handleMessages() - handling 46 messages took 0.61 ms and 134 calls... 2021-02-12T23:19:44Z- waiting for 1613171982.35, time is 1613171984.60 2021-02-12T23:19:44Z- LcmTrn::updateTrn() >>>> pose time 1613171984.60, meas time 1613171984.19 <<<< 2021-02-12T23:19:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171984.19 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:19:44Z- TerrainNav::motionUpdate() - currEstimate = (4056715, 590779.15, 23.81) 2021-02-12T23:19:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:19:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:19:44Z- TNavBF::Projecting Measurements 2021-02-12T23:19:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:19:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:19:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:19:44Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:19:44Z- TNavFilter::3 good beams. 2021-02-12T23:19:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:19:44Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:19:44Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:19:44Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:19:44Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:19:44Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:19:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:19:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:19:44Z- TerrainMapOctree: (vehN, vehE ) = (4056714.39, 590778.87). 2021-02-12T23:19:44Z- TerrainMapOctree: Min Distance = 5711.44. 2021-02-12T23:19:44Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:19:44Z- There is no specific good beam that all particles have in common. 2021-02-12T23:19:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:19:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.115 ExpectedMeasDiff: nan currMeas.covariance: 7.81 2021-02-12T23:19:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:19:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.119 ExpectedMeasDiff: nan currMeas.covariance: 6.42 2021-02-12T23:19:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:19:44Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.00 2021-02-12T23:19:44Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:19:44Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:19:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:19:44Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:19:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:19:44Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:19:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:19:44Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:19:44Z- Filter 0 No beams used in subcloud update 2021-02-12T23:19:44Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:19:44Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:19:44Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 777.5 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:19:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613171984.19 sec. 2021-02-12T23:19:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:19:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:19:44Z- variances([0], [2], and psi_berg) = 777.486590 833.064115 0.000000 2021-02-12T23:19:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:19:44Z- LcmTrn::publishEstimates() - MLE : 4056692.15 590732.65 23.81 2021-02-12T23:19:44Z- LcmTrn::publishEstimates() - MMSE : 4056714.65 590779.15 23.81 2021-02-12T23:19:44Z- LcmTrn::publishEstimates() - COVAR: 777.49 833.06 0.93 0.00 2021-02-12T23:19:44Z- AHRS_M2 msg: 1613171984.97 epoch sec; seqNo:2749 2021-02-12T23:19:44Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.20 psi 2021-02-12T23:19:44Z- handleDepth()-Depth_Keller msg: 1613171984985 epoch sec; seqNo:2775; depth 23.94 2021-02-12T23:19:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:44Z- handleDvl() - RDI_Pathfinder msg: 1613171984.99 epoch sec; seqNo:1174 2021-02-12T23:19:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.09 , -0.04 2021-02-12T23:19:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171984.99 epoch sec; seqNo:2775; 4056715.03 north; 590779.14 east 2021-02-12T23:19:45Z- AHRS_M2 msg: 1613171985.38 epoch sec; seqNo:2750 2021-02-12T23:19:45Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.20 psi 2021-02-12T23:19:45Z- handleDepth()-Depth_Keller msg: 1613171985383 epoch sec; seqNo:2776; depth 24.06 2021-02-12T23:19:45Z- handleDvl() - RDI_Pathfinder msg: 1613171985.40 epoch sec; seqNo:1175 2021-02-12T23:19:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.64 , 88.17 , 63.33 , 100.07 2021-02-12T23:19:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.07 , -0.10 2021-02-12T23:19:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171985.41 epoch sec; seqNo:2776; 4056715.41 north; 590779.14 east 2021-02-12T23:19:45Z- AHRS_M2 msg: 1613171985.79 epoch sec; seqNo:2751 2021-02-12T23:19:45Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.20 psi 2021-02-12T23:19:45Z- handleDepth()-Depth_Keller msg: 1613171985798 epoch sec; seqNo:2777; depth 24.20 2021-02-12T23:19:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171985.82 epoch sec; seqNo:2777; 4056715.41 north; 590779.14 east 2021-02-12T23:19:46Z- AHRS_M2 msg: 1613171986.21 epoch sec; seqNo:2752 2021-02-12T23:19:46Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.21 psi 2021-02-12T23:19:46Z- handleDepth()-Depth_Keller msg: 1613171986218 epoch sec; seqNo:2778; depth 24.30 2021-02-12T23:19:46Z- handleDvl() - RDI_Pathfinder msg: 1613171986.23 epoch sec; seqNo:1176 2021-02-12T23:19:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.23 , 87.09 , 62.51 , 97.91 2021-02-12T23:19:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , -0.14 2021-02-12T23:19:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171986.24 epoch sec; seqNo:2778; 4056715.79 north; 590779.14 east 2021-02-12T23:19:46Z- AHRS_M2 msg: 1613171986.59 epoch sec; seqNo:2753 2021-02-12T23:19:46Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.21 psi 2021-02-12T23:19:46Z- handleDepth()-Depth_Keller msg: 1613171986596 epoch sec; seqNo:2779; depth 24.44 2021-02-12T23:19:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171986.61 epoch sec; seqNo:2779; 4056716.18 north; 590780.35 east 2021-02-12T23:19:46Z- AHRS_M2 msg: 1613171987.00 epoch sec; seqNo:2754 2021-02-12T23:19:46Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.21 psi 2021-02-12T23:19:47Z- handleDepth()-Depth_Keller msg: 1613171987000 epoch sec; seqNo:2780; depth 24.56 2021-02-12T23:19:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171987.01 epoch sec; seqNo:2780; 4056716.56 north; 590780.35 east 2021-02-12T23:19:47Z- AHRS_M2 msg: 1613171987.40 epoch sec; seqNo:2755 2021-02-12T23:19:47Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.21 psi 2021-02-12T23:19:47Z- handleDepth()-Depth_Keller msg: 1613171987407 epoch sec; seqNo:2781; depth 24.67 2021-02-12T23:19:47Z- handleDvl() - RDI_Pathfinder msg: 1613171987.42 epoch sec; seqNo:1177 2021-02-12T23:19:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.41 , 0.00 , 62.10 , 98.99 2021-02-12T23:19:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , -0.15 2021-02-12T23:19:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171987.43 epoch sec; seqNo:2781; 4056716.56 north; 590780.35 east 2021-02-12T23:19:47Z- AHRS_M2 msg: 1613171987.80 epoch sec; seqNo:2756 2021-02-12T23:19:47Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.21 psi 2021-02-12T23:19:47Z- handleDepth()-Depth_Keller msg: 1613171987808 epoch sec; seqNo:2782; depth 24.80 2021-02-12T23:19:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171987.83 epoch sec; seqNo:2782; 4056716.94 north; 590780.35 east 2021-02-12T23:19:48Z- AHRS_M2 msg: 1613171988.22 epoch sec; seqNo:2757 2021-02-12T23:19:48Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.21 psi 2021-02-12T23:19:48Z- handleDepth()-Depth_Keller msg: 1613171988223 epoch sec; seqNo:2783; depth 24.91 2021-02-12T23:19:48Z- handleDvl() - RDI_Pathfinder msg: 1613171988.24 epoch sec; seqNo:1178 2021-02-12T23:19:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.00 , 88.17 , 61.69 , 96.82 2021-02-12T23:19:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , -0.11 2021-02-12T23:19:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171988.25 epoch sec; seqNo:2783; 4056717.32 north; 590780.34 east 2021-02-12T23:19:48Z- AHRS_M2 msg: 1613171988.61 epoch sec; seqNo:2758 2021-02-12T23:19:48Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.21 psi 2021-02-12T23:19:48Z- handleDepth()-Depth_Keller msg: 1613171988616 epoch sec; seqNo:2784; depth 25.06 2021-02-12T23:19:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:48Z- handleDvl() - RDI_Pathfinder msg: 1613171988.62 epoch sec; seqNo:1179 2021-02-12T23:19:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , -0.11 2021-02-12T23:19:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171988.63 epoch sec; seqNo:2784; 4056717.70 north; 590780.34 east 2021-02-12T23:19:49Z- AHRS_M2 msg: 1613171989.02 epoch sec; seqNo:2759 2021-02-12T23:19:49Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.21 psi 2021-02-12T23:19:49Z- handleDepth()-Depth_Keller msg: 1613171989023 epoch sec; seqNo:2785; depth 25.19 2021-02-12T23:19:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171989.03 epoch sec; seqNo:2785; 4056717.70 north; 590780.34 east 2021-02-12T23:19:49Z- AHRS_M2 msg: 1613171989.42 epoch sec; seqNo:2760 2021-02-12T23:19:49Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.21 psi 2021-02-12T23:19:49Z- handleDepth()-Depth_Keller msg: 1613171989424 epoch sec; seqNo:2786; depth 25.31 2021-02-12T23:19:49Z- handleDvl() - RDI_Pathfinder msg: 1613171989.44 epoch sec; seqNo:1180 2021-02-12T23:19:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.00 , 88.17 , 61.28 , 97.91 2021-02-12T23:19:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , -0.04 2021-02-12T23:19:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171989.45 epoch sec; seqNo:2786; 4056718.09 north; 590781.55 east 2021-02-12T23:19:49Z- AHRS_M2 msg: 1613171989.82 epoch sec; seqNo:2761 2021-02-12T23:19:49Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.21 psi 2021-02-12T23:19:49Z- handleDepth()-Depth_Keller msg: 1613171989833 epoch sec; seqNo:2787; depth 25.42 2021-02-12T23:19:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:49Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:49Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:49Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:49Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:49Z- handleDvl() - RDI_Pathfinder msg: 1613171989.84 epoch sec; seqNo:1181 2021-02-12T23:19:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , -0.04 2021-02-12T23:19:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171989.85 epoch sec; seqNo:2787; 4056718.47 north; 590781.55 east 2021-02-12T23:19:49Z- LcmTrn::handleMessages() - handling 47 messages took 0.74 ms and 132 calls... 2021-02-12T23:19:49Z- waiting for 1613171987.60, time is 1613171989.85 2021-02-12T23:19:49Z- LcmTrn::updateTrn() equating pose 1613171989.85 and meas times 1613171989.85 2021-02-12T23:19:49Z- LcmTrn::updateTrn() >>>> pose time 1613171989.85, meas time 1613171989.85 <<<< 2021-02-12T23:19:49Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613171989.85, ping # 0. 2021-02-12T23:19:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171989.85 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:19:49Z- TerrainNav::motionUpdate() - currEstimate = (4056718, 590781.55, 25.42) 2021-02-12T23:19:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:19:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:19:49Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:19:49Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:19:49Z- TerrainNav::Measurement type 1 not incorporated from time = 1613171989.85 sec.; No relevant navigation data available 2021-02-12T23:19:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:19:49Z- TNavBF::mmse selected from filter -1 2021-02-12T23:19:49Z- variances([0], [2], and psi_berg) = 777.331377 833.063901 0.000000 2021-02-12T23:19:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:19:49Z- LcmTrn::publishEstimates() - MLE : 4056695.97 590735.05 25.42 2021-02-12T23:19:49Z- LcmTrn::publishEstimates() - MMSE : 4056718.47 590781.55 25.42 2021-02-12T23:19:49Z- LcmTrn::publishEstimates() - COVAR: 777.33 833.06 1.16 0.00 2021-02-12T23:19:50Z- AHRS_M2 msg: 1613171990.24 epoch sec; seqNo:2762 2021-02-12T23:19:50Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.21 psi 2021-02-12T23:19:50Z- handleDepth()-Depth_Keller msg: 1613171990242 epoch sec; seqNo:2788; depth 25.55 2021-02-12T23:19:50Z- handleDvl() - RDI_Pathfinder msg: 1613171990.27 epoch sec; seqNo:1182 2021-02-12T23:19:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.60 , 0.00 , 61.28 , 97.91 2021-02-12T23:19:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.08 , 0.01 2021-02-12T23:19:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171990.29 epoch sec; seqNo:2788; 4056718.84 north; 590781.55 east 2021-02-12T23:19:50Z- AHRS_M2 msg: 1613171990.63 epoch sec; seqNo:2763 2021-02-12T23:19:50Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.21 psi 2021-02-12T23:19:50Z- handleDepth()-Depth_Keller msg: 1613171990642 epoch sec; seqNo:2789; depth 25.70 2021-02-12T23:19:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171990.65 epoch sec; seqNo:2789; 4056718.84 north; 590781.55 east 2021-02-12T23:19:51Z- AHRS_M2 msg: 1613171991.04 epoch sec; seqNo:2764 2021-02-12T23:19:51Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.21 psi 2021-02-12T23:19:51Z- handleDepth()-Depth_Keller msg: 1613171991041 epoch sec; seqNo:2790; depth 25.81 2021-02-12T23:19:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:51Z- handleDvl() - RDI_Pathfinder msg: 1613171991.04 epoch sec; seqNo:1183 2021-02-12T23:19:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.08 , 0.01 2021-02-12T23:19:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171991.05 epoch sec; seqNo:2790; 4056719.22 north; 590781.54 east 2021-02-12T23:19:51Z- AHRS_M2 msg: 1613171991.44 epoch sec; seqNo:2765 2021-02-12T23:19:51Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.21 psi 2021-02-12T23:19:51Z- handleDepth()-Depth_Keller msg: 1613171991449 epoch sec; seqNo:2791; depth 25.93 2021-02-12T23:19:51Z- handleDvl() - RDI_Pathfinder msg: 1613171991.46 epoch sec; seqNo:1184 2021-02-12T23:19:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.60 , 87.09 , 61.28 , 96.82 2021-02-12T23:19:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.09 , 0.06 2021-02-12T23:19:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171991.48 epoch sec; seqNo:2791; 4056719.60 north; 590781.54 east 2021-02-12T23:19:51Z- AHRS_M2 msg: 1613171991.85 epoch sec; seqNo:2766 2021-02-12T23:19:51Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.21 psi 2021-02-12T23:19:51Z- handleDepth()-Depth_Keller msg: 1613171991853 epoch sec; seqNo:2792; depth 26.06 2021-02-12T23:19:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171991.86 epoch sec; seqNo:2792; 4056719.60 north; 590781.54 east 2021-02-12T23:19:52Z- AHRS_M2 msg: 1613171992.25 epoch sec; seqNo:2767 2021-02-12T23:19:52Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.21 psi 2021-02-12T23:19:52Z- handleDepth()-Depth_Keller msg: 1613171992262 epoch sec; seqNo:2793; depth 26.19 2021-02-12T23:19:52Z- handleDvl() - RDI_Pathfinder msg: 1613171992.27 epoch sec; seqNo:1185 2021-02-12T23:19:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.19 , 86.01 , 60.88 , 97.91 2021-02-12T23:19:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , 0.09 2021-02-12T23:19:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171992.28 epoch sec; seqNo:2793; 4056719.99 north; 590782.76 east 2021-02-12T23:19:52Z- AHRS_M2 msg: 1613171992.66 epoch sec; seqNo:2768 2021-02-12T23:19:52Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.21 psi 2021-02-12T23:19:52Z- handleDepth()-Depth_Keller msg: 1613171992664 epoch sec; seqNo:2794; depth 26.32 2021-02-12T23:19:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171992.70 epoch sec; seqNo:2794; 4056720.37 north; 590782.75 east 2021-02-12T23:19:53Z- AHRS_M2 msg: 1613171993.06 epoch sec; seqNo:2769 2021-02-12T23:19:53Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.21 psi 2021-02-12T23:19:53Z- handleDepth()-Depth_Keller msg: 1613171993062 epoch sec; seqNo:2795; depth 26.43 2021-02-12T23:19:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171993.07 epoch sec; seqNo:2795; 4056720.37 north; 590782.75 east 2021-02-12T23:19:53Z- handleDepth()-Depth_Keller msg: 1613171993478 epoch sec; seqNo:2796; depth 26.55 2021-02-12T23:19:53Z- handleDvl() - RDI_Pathfinder msg: 1613171993.49 epoch sec; seqNo:1186 2021-02-12T23:19:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.60 , 0.00 , 60.88 , 97.91 2021-02-12T23:19:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , 0.10 2021-02-12T23:19:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171993.50 epoch sec; seqNo:2796; 4056720.75 north; 590782.75 east 2021-02-12T23:19:53Z- AHRS_M2 msg: 1613171993.86 epoch sec; seqNo:2770 2021-02-12T23:19:53Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.20 psi 2021-02-12T23:19:53Z- handleDepth()-Depth_Keller msg: 1613171993869 epoch sec; seqNo:2797; depth 26.70 2021-02-12T23:19:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171993.88 epoch sec; seqNo:2797; 4056721.13 north; 590782.74 east 2021-02-12T23:19:54Z- AHRS_M2 msg: 1613171994.27 epoch sec; seqNo:2771 2021-02-12T23:19:54Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.20 psi 2021-02-12T23:19:54Z- handleDepth()-Depth_Keller msg: 1613171994274 epoch sec; seqNo:2798; depth 26.82 2021-02-12T23:19:54Z- handleDvl() - RDI_Pathfinder msg: 1613171994.29 epoch sec; seqNo:1187 2021-02-12T23:19:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.19 , 86.01 , 60.47 , 97.91 2021-02-12T23:19:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.07 , 0.08 2021-02-12T23:19:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171994.30 epoch sec; seqNo:2798; 4056721.13 north; 590782.74 east 2021-02-12T23:19:54Z- AHRS_M2 msg: 1613171994.67 epoch sec; seqNo:2772 2021-02-12T23:19:54Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.20 psi 2021-02-12T23:19:54Z- handleDepth()-Depth_Keller msg: 1613171994676 epoch sec; seqNo:2799; depth 26.93 2021-02-12T23:19:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:54Z- handleDvl() - RDI_Pathfinder msg: 1613171994.68 epoch sec; seqNo:1188 2021-02-12T23:19:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.07 , 0.08 2021-02-12T23:19:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171994.70 epoch sec; seqNo:2799; 4056721.51 north; 590782.74 east 2021-02-12T23:19:54Z- LcmTrn::handleMessages() - handling 42 messages took 0.62 ms and 130 calls... 2021-02-12T23:19:54Z- waiting for 1613171992.85, time is 1613171994.70 2021-02-12T23:19:54Z- LcmTrn::updateTrn() equating pose 1613171994.70 and meas times 1613171994.70 2021-02-12T23:19:54Z- LcmTrn::updateTrn() >>>> pose time 1613171994.70, meas time 1613171994.70 <<<< 2021-02-12T23:19:54Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613171994.70, ping # 0. 2021-02-12T23:19:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171994.70 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:19:54Z- TerrainNav::motionUpdate() - currEstimate = (4056722, 590782.74, 26.93) 2021-02-12T23:19:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:19:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:19:54Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:19:54Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:19:54Z- TNavBF::Projecting Measurements 2021-02-12T23:19:54Z- TNavFilter::Measurement from time = 1613171994.70 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:19:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:19:54Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:19:54Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:19:54Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:19:54Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:19:54Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:19:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:19:54Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 777.2 EastVariance: 833.1 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-12T23:19:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:19:54Z- TNavBF::mmse selected from filter -1 2021-02-12T23:19:54Z- variances([0], [2], and psi_berg) = 777.199570 833.063719 0.000000 2021-02-12T23:19:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:19:54Z- LcmTrn::publishEstimates() - MLE : 4056699.01 590736.24 26.93 2021-02-12T23:19:54Z- LcmTrn::publishEstimates() - MMSE : 4056721.51 590782.74 26.93 2021-02-12T23:19:54Z- LcmTrn::publishEstimates() - COVAR: 777.20 833.06 1.33 0.00 2021-02-12T23:19:55Z- AHRS_M2 msg: 1613171995.08 epoch sec; seqNo:2773 2021-02-12T23:19:55Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.20 psi 2021-02-12T23:19:55Z- handleDepth()-Depth_Keller msg: 1613171995085 epoch sec; seqNo:2800; depth 27.06 2021-02-12T23:19:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171995.09 epoch sec; seqNo:2800; 4056721.52 north; 590783.96 east 2021-02-12T23:19:55Z- AHRS_M2 msg: 1613171995.48 epoch sec; seqNo:2774 2021-02-12T23:19:55Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.20 psi 2021-02-12T23:19:55Z- handleDepth()-Depth_Keller msg: 1613171995484 epoch sec; seqNo:2801; depth 27.18 2021-02-12T23:19:55Z- handleDvl() - RDI_Pathfinder msg: 1613171995.50 epoch sec; seqNo:1189 2021-02-12T23:19:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.78 , 86.01 , 60.06 , 94.66 2021-02-12T23:19:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.09 , 0.03 2021-02-12T23:19:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171995.51 epoch sec; seqNo:2801; 4056721.90 north; 590783.96 east 2021-02-12T23:19:55Z- AHRS_M2 msg: 1613171995.88 epoch sec; seqNo:2775 2021-02-12T23:19:55Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.20 psi 2021-02-12T23:19:55Z- handleDepth()-Depth_Keller msg: 1613171995890 epoch sec; seqNo:2802; depth 27.31 2021-02-12T23:19:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:55Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:55Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:55Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:55Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:55Z- handleDvl() - RDI_Pathfinder msg: 1613171995.89 epoch sec; seqNo:1190 2021-02-12T23:19:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.09 , 0.03 2021-02-12T23:19:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171995.90 epoch sec; seqNo:2802; 4056722.28 north; 590783.95 east 2021-02-12T23:19:56Z- AHRS_M2 msg: 1613171996.29 epoch sec; seqNo:2776 2021-02-12T23:19:56Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.20 psi 2021-02-12T23:19:56Z- handleDepth()-Depth_Keller msg: 1613171996302 epoch sec; seqNo:2803; depth 27.44 2021-02-12T23:19:56Z- handleDvl() - RDI_Pathfinder msg: 1613171996.31 epoch sec; seqNo:1191 2021-02-12T23:19:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.37 , 84.93 , 60.06 , 93.58 2021-02-12T23:19:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.10 , -0.03 2021-02-12T23:19:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171996.33 epoch sec; seqNo:2803; 4056722.66 north; 590783.95 east 2021-02-12T23:19:56Z- AHRS_M2 msg: 1613171996.69 epoch sec; seqNo:2777 2021-02-12T23:19:56Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.20 psi 2021-02-12T23:19:56Z- handleDepth()-Depth_Keller msg: 1613171996696 epoch sec; seqNo:2804; depth 27.56 2021-02-12T23:19:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171996.71 epoch sec; seqNo:2804; 4056722.66 north; 590783.95 east 2021-02-12T23:19:57Z- AHRS_M2 msg: 1613171997.10 epoch sec; seqNo:2778 2021-02-12T23:19:57Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:57Z- handleDepth()-Depth_Keller msg: 1613171997105 epoch sec; seqNo:2805; depth 27.69 2021-02-12T23:19:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:57Z- handleDvl() - RDI_Pathfinder msg: 1613171997.11 epoch sec; seqNo:1192 2021-02-12T23:19:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.10 , -0.03 2021-02-12T23:19:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171997.12 epoch sec; seqNo:2805; 4056723.04 north; 590783.94 east 2021-02-12T23:19:57Z- AHRS_M2 msg: 1613171997.50 epoch sec; seqNo:2779 2021-02-12T23:19:57Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.21 psi 2021-02-12T23:19:57Z- handleDepth()-Depth_Keller msg: 1613171997513 epoch sec; seqNo:2806; depth 27.80 2021-02-12T23:19:57Z- handleDvl() - RDI_Pathfinder msg: 1613171997.52 epoch sec; seqNo:1193 2021-02-12T23:19:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.96 , 84.93 , 59.24 , 97.91 2021-02-12T23:19:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.01 , -0.07 2021-02-12T23:19:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171997.54 epoch sec; seqNo:2806; 4056723.43 north; 590785.16 east 2021-02-12T23:19:57Z- AHRS_M2 msg: 1613171997.90 epoch sec; seqNo:2780 2021-02-12T23:19:57Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:57Z- handleDepth()-Depth_Keller msg: 1613171997908 epoch sec; seqNo:2807; depth 27.95 2021-02-12T23:19:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171997.93 epoch sec; seqNo:2807; 4056723.81 north; 590785.16 east 2021-02-12T23:19:58Z- AHRS_M2 msg: 1613171998.33 epoch sec; seqNo:2781 2021-02-12T23:19:58Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:58Z- handleDepth()-Depth_Keller msg: 1613171998342 epoch sec; seqNo:2808; depth 28.06 2021-02-12T23:19:58Z- handleDvl() - RDI_Pathfinder msg: 1613171998.35 epoch sec; seqNo:1194 2021-02-12T23:19:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.55 , 84.93 , 59.65 , 92.50 2021-02-12T23:19:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , -0.07 2021-02-12T23:19:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171998.37 epoch sec; seqNo:2808; 4056724.19 north; 590785.15 east 2021-02-12T23:19:58Z- AHRS_M2 msg: 1613171998.71 epoch sec; seqNo:2782 2021-02-12T23:19:58Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.20 psi 2021-02-12T23:19:58Z- handleDepth()-Depth_Keller msg: 1613171998717 epoch sec; seqNo:2809; depth 28.18 2021-02-12T23:19:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171998.73 epoch sec; seqNo:2809; 4056724.19 north; 590785.15 east 2021-02-12T23:19:59Z- AHRS_M2 msg: 1613171999.14 epoch sec; seqNo:2783 2021-02-12T23:19:59Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:19:59Z- handleDepth()-Depth_Keller msg: 1613171999145 epoch sec; seqNo:2810; depth 28.31 2021-02-12T23:19:59Z- handleDvl() - RDI_Pathfinder msg: 1613171999.16 epoch sec; seqNo:1195 2021-02-12T23:19:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:59Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:19:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171999.17 epoch sec; seqNo:2810; 4056724.57 north; 590785.15 east 2021-02-12T23:19:59Z- AHRS_M2 msg: 1613171999.52 epoch sec; seqNo:2784 2021-02-12T23:19:59Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.19 psi 2021-02-12T23:19:59Z- handleDepth()-Depth_Keller msg: 1613171999529 epoch sec; seqNo:2811; depth 28.42 2021-02-12T23:19:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:19:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:19:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:19:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:19:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:19:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:19:59Z- handleDvl() - RDI_Pathfinder msg: 1613171999.53 epoch sec; seqNo:1196 2021-02-12T23:19:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:19:59Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:19:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171999.54 epoch sec; seqNo:2811; 4056724.95 north; 590785.15 east 2021-02-12T23:19:59Z- AHRS_M2 msg: 1613171999.93 epoch sec; seqNo:2785 2021-02-12T23:19:59Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.18 psi 2021-02-12T23:19:59Z- LcmTrn::handleMessages() - handling 45 messages took 0.66 ms and 132 calls... 2021-02-12T23:19:59Z- waiting for 1613171997.70, time is 1613171999.93 2021-02-12T23:19:59Z- LcmTrn::updateTrn() >>>> pose time 1613171999.93, meas time 1613171999.53 <<<< 2021-02-12T23:19:59Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613171999.53, ping # 0. 2021-02-12T23:19:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613171999.53 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:19:59Z- TerrainNav::motionUpdate() - currEstimate = (4056725, 590785.15, 28.42) 2021-02-12T23:19:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:19:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:19:59Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:19:59Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:19:59Z- TNavBF::Projecting Measurements 2021-02-12T23:19:59Z- TNavFilter::Measurement from time = 1613171999.53 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:19:59Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:19:59Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:19:59Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:19:59Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:19:59Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:19:59Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:19:59Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:19:59Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 777.1 EastVariance: 833.1 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-12T23:19:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:19:59Z- TNavBF::mmse selected from filter -1 2021-02-12T23:19:59Z- variances([0], [2], and psi_berg) = 777.015455 833.063466 0.000000 2021-02-12T23:19:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:19:59Z- LcmTrn::publishEstimates() - MLE : 4056702.45 590737.65 28.42 2021-02-12T23:19:59Z- LcmTrn::publishEstimates() - MMSE : 4056724.95 590785.15 28.42 2021-02-12T23:19:59Z- LcmTrn::publishEstimates() - COVAR: 777.02 833.06 1.54 0.00 2021-02-12T23:19:59Z- handleDepth()-Depth_Keller msg: 1613171999932 epoch sec; seqNo:2812; depth 28.55 2021-02-12T23:19:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613171999.94 epoch sec; seqNo:2812; 4056725.34 north; 590786.36 east 2021-02-12T23:20:00Z- AHRS_M2 msg: 1613172000.33 epoch sec; seqNo:2786 2021-02-12T23:20:00Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.18 psi 2021-02-12T23:20:00Z- handleDepth()-Depth_Keller msg: 1613172000332 epoch sec; seqNo:2813; depth 28.65 2021-02-12T23:20:00Z- handleDvl() - RDI_Pathfinder msg: 1613172000.35 epoch sec; seqNo:1197 2021-02-12T23:20:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:00Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:20:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172000.36 epoch sec; seqNo:2813; 4056725.72 north; 590786.36 east 2021-02-12T23:20:00Z- AHRS_M2 msg: 1613172000.73 epoch sec; seqNo:2787 2021-02-12T23:20:00Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-12T23:20:00Z- handleDepth()-Depth_Keller msg: 1613172000736 epoch sec; seqNo:2814; depth 28.77 2021-02-12T23:20:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:00Z- handleDvl() - RDI_Pathfinder msg: 1613172000.74 epoch sec; seqNo:1198 2021-02-12T23:20:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:00Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:20:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172000.75 epoch sec; seqNo:2814; 4056726.09 north; 590786.35 east 2021-02-12T23:20:01Z- AHRS_M2 msg: 1613172001.14 epoch sec; seqNo:2788 2021-02-12T23:20:01Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-12T23:20:01Z- handleDepth()-Depth_Keller msg: 1613172001162 epoch sec; seqNo:2815; depth 28.88 2021-02-12T23:20:01Z- handleDvl() - RDI_Pathfinder msg: 1613172001.20 epoch sec; seqNo:1199 2021-02-12T23:20:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.33 , 75.99 , 58.43 , 90.47 2021-02-12T23:20:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , -0.04 2021-02-12T23:20:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172001.21 epoch sec; seqNo:2815; 4056726.09 north; 590786.35 east 2021-02-12T23:20:01Z- AHRS_M2 msg: 1613172001.54 epoch sec; seqNo:2789 2021-02-12T23:20:01Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-12T23:20:01Z- handleDepth()-Depth_Keller msg: 1613172001546 epoch sec; seqNo:2816; depth 29.00 2021-02-12T23:20:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172001.56 epoch sec; seqNo:2816; 4056726.47 north; 590786.35 east 2021-02-12T23:20:01Z- AHRS_M2 msg: 1613172001.94 epoch sec; seqNo:2790 2021-02-12T23:20:01Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-12T23:20:01Z- handleDepth()-Depth_Keller msg: 1613172001948 epoch sec; seqNo:2817; depth 29.10 2021-02-12T23:20:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:01Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:01Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:01Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:01Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:01Z- handleDvl() - RDI_Pathfinder msg: 1613172001.95 epoch sec; seqNo:1200 2021-02-12T23:20:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , -0.04 2021-02-12T23:20:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172001.96 epoch sec; seqNo:2817; 4056726.85 north; 590786.35 east 2021-02-12T23:20:02Z- AHRS_M2 msg: 1613172002.35 epoch sec; seqNo:2791 2021-02-12T23:20:02Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-12T23:20:02Z- handleDepth()-Depth_Keller msg: 1613172002357 epoch sec; seqNo:2818; depth 29.23 2021-02-12T23:20:02Z- handleDvl() - RDI_Pathfinder msg: 1613172002.38 epoch sec; seqNo:1201 2021-02-12T23:20:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.74 , 75.99 , 58.02 , 90.34 2021-02-12T23:20:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , -0.03 2021-02-12T23:20:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172002.39 epoch sec; seqNo:2818; 4056727.23 north; 590786.34 east 2021-02-12T23:20:02Z- AHRS_M2 msg: 1613172002.75 epoch sec; seqNo:2792 2021-02-12T23:20:02Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.16 psi 2021-02-12T23:20:02Z- handleDepth()-Depth_Keller msg: 1613172002756 epoch sec; seqNo:2819; depth 29.35 2021-02-12T23:20:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172002.77 epoch sec; seqNo:2819; 4056727.24 north; 590787.56 east 2021-02-12T23:20:03Z- AHRS_M2 msg: 1613172003.16 epoch sec; seqNo:2793 2021-02-12T23:20:03Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.17 psi 2021-02-12T23:20:03Z- handleDepth()-Depth_Keller msg: 1613172003167 epoch sec; seqNo:2820; depth 29.46 2021-02-12T23:20:03Z- handleDvl() - RDI_Pathfinder msg: 1613172003.18 epoch sec; seqNo:1202 2021-02-12T23:20:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.92 , 74.77 , 58.43 , 0.00 2021-02-12T23:20:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.13 , 0.02 2021-02-12T23:20:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172003.19 epoch sec; seqNo:2820; 4056727.62 north; 590787.56 east 2021-02-12T23:20:03Z- AHRS_M2 msg: 1613172003.56 epoch sec; seqNo:2794 2021-02-12T23:20:03Z- AHRS_M2 msg: 0.59 phi; -0.25 theta; -0.16 psi 2021-02-12T23:20:03Z- handleDepth()-Depth_Keller msg: 1613172003574 epoch sec; seqNo:2821; depth 29.58 2021-02-12T23:20:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172003.59 epoch sec; seqNo:2821; 4056728.00 north; 590787.56 east 2021-02-12T23:20:03Z- AHRS_M2 msg: 1613172003.96 epoch sec; seqNo:2795 2021-02-12T23:20:03Z- AHRS_M2 msg: 0.59 phi; -0.22 theta; -0.16 psi 2021-02-12T23:20:03Z- handleDepth()-Depth_Keller msg: 1613172003969 epoch sec; seqNo:2822; depth 29.71 2021-02-12T23:20:03Z- handleDvl() - RDI_Pathfinder msg: 1613172003.98 epoch sec; seqNo:1203 2021-02-12T23:20:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.23 , 73.57 , 58.86 , 79.09 2021-02-12T23:20:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.11 , 0.05 2021-02-12T23:20:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172003.99 epoch sec; seqNo:2822; 4056728.38 north; 590787.55 east 2021-02-12T23:20:04Z- AHRS_M2 msg: 1613172004.37 epoch sec; seqNo:2796 2021-02-12T23:20:04Z- AHRS_M2 msg: 0.58 phi; -0.18 theta; -0.15 psi 2021-02-12T23:20:04Z- handleDepth()-Depth_Keller msg: 1613172004374 epoch sec; seqNo:2823; depth 29.83 2021-02-12T23:20:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:04Z- handleDvl() - RDI_Pathfinder msg: 1613172004.38 epoch sec; seqNo:1204 2021-02-12T23:20:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.11 , 0.05 2021-02-12T23:20:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172004.39 epoch sec; seqNo:2823; 4056728.76 north; 590787.55 east 2021-02-12T23:20:04Z- AHRS_M2 msg: 1613172004.77 epoch sec; seqNo:2797 2021-02-12T23:20:04Z- AHRS_M2 msg: 0.58 phi; -0.14 theta; -0.14 psi 2021-02-12T23:20:04Z- handleDepth()-Depth_Keller msg: 1613172004776 epoch sec; seqNo:2824; depth 29.96 2021-02-12T23:20:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172004.80 epoch sec; seqNo:2824; 4056728.76 north; 590787.55 east 2021-02-12T23:20:04Z- LcmTrn::handleMessages() - handling 46 messages took 0.70 ms and 132 calls... 2021-02-12T23:20:04Z- waiting for 1613172002.93, time is 1613172004.80 2021-02-12T23:20:04Z- LcmTrn::updateTrn() >>>> pose time 1613172004.80, meas time 1613172004.38 <<<< 2021-02-12T23:20:04Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172004.38, ping # 0. 2021-02-12T23:20:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172004.38 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:20:04Z- TerrainNav::motionUpdate() - currEstimate = (4056729, 590787.55, 29.96) 2021-02-12T23:20:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:20:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:20:04Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:20:04Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:20:04Z- TNavBF::Projecting Measurements 2021-02-12T23:20:04Z- TNavFilter::Measurement from time = 1613172004.38 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:20:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:20:04Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:20:04Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:20:04Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:20:04Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:20:04Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:20:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:20:04Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 776.9 EastVariance: 833.1 DepthVariance: 1.8 this->DepthBiasCov: 1.8 2021-02-12T23:20:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:20:04Z- TNavBF::mmse selected from filter -1 2021-02-12T23:20:04Z- variances([0], [2], and psi_berg) = 776.823174 833.063203 0.000000 2021-02-12T23:20:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:20:04Z- LcmTrn::publishEstimates() - MLE : 4056706.26 590740.05 29.96 2021-02-12T23:20:04Z- LcmTrn::publishEstimates() - MMSE : 4056728.76 590787.55 29.96 2021-02-12T23:20:04Z- LcmTrn::publishEstimates() - COVAR: 776.82 833.06 1.77 0.00 2021-02-12T23:20:05Z- AHRS_M2 msg: 1613172005.18 epoch sec; seqNo:2798 2021-02-12T23:20:05Z- AHRS_M2 msg: 0.57 phi; -0.10 theta; -0.14 psi 2021-02-12T23:20:05Z- handleDepth()-Depth_Keller msg: 1613172005185 epoch sec; seqNo:2825; depth 30.06 2021-02-12T23:20:05Z- handleDvl() - RDI_Pathfinder msg: 1613172005.20 epoch sec; seqNo:1205 2021-02-12T23:20:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.16 , 70.54 , 60.19 , 72.16 2021-02-12T23:20:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , 0.07 2021-02-12T23:20:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172005.21 epoch sec; seqNo:2825; 4056729.14 north; 590787.54 east 2021-02-12T23:20:05Z- AHRS_M2 msg: 1613172005.58 epoch sec; seqNo:2799 2021-02-12T23:20:05Z- AHRS_M2 msg: 0.57 phi; -0.06 theta; -0.13 psi 2021-02-12T23:20:05Z- handleDepth()-Depth_Keller msg: 1613172005597 epoch sec; seqNo:2826; depth 30.16 2021-02-12T23:20:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:05Z- handleDvl() - RDI_Pathfinder msg: 1613172005.60 epoch sec; seqNo:1206 2021-02-12T23:20:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , 0.07 2021-02-12T23:20:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172005.62 epoch sec; seqNo:2826; 4056729.52 north; 590787.54 east 2021-02-12T23:20:05Z- AHRS_M2 msg: 1613172005.98 epoch sec; seqNo:2800 2021-02-12T23:20:05Z- AHRS_M2 msg: 0.56 phi; -0.02 theta; -0.13 psi 2021-02-12T23:20:05Z- handleDepth()-Depth_Keller msg: 1613172005993 epoch sec; seqNo:2827; depth 30.23 2021-02-12T23:20:06Z- handleDvl() - RDI_Pathfinder msg: 1613172006.01 epoch sec; seqNo:1207 2021-02-12T23:20:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.70 , 68.98 , 62.54 , 66.83 2021-02-12T23:20:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , 0.06 2021-02-12T23:20:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172006.02 epoch sec; seqNo:2827; 4056729.91 north; 590788.76 east 2021-02-12T23:20:06Z- AHRS_M2 msg: 1613172006.39 epoch sec; seqNo:2801 2021-02-12T23:20:06Z- AHRS_M2 msg: 0.55 phi; 0.02 theta; -0.13 psi 2021-02-12T23:20:06Z- handleDepth()-Depth_Keller msg: 1613172006402 epoch sec; seqNo:2828; depth 30.30 2021-02-12T23:20:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172006.42 epoch sec; seqNo:2828; 4056730.29 north; 590788.75 east 2021-02-12T23:20:06Z- AHRS_M2 msg: 1613172006.81 epoch sec; seqNo:2802 2021-02-12T23:20:06Z- AHRS_M2 msg: 0.55 phi; 0.06 theta; -0.13 psi 2021-02-12T23:20:06Z- handleDepth()-Depth_Keller msg: 1613172006834 epoch sec; seqNo:2829; depth 30.35 2021-02-12T23:20:06Z- handleDvl() - RDI_Pathfinder msg: 1613172006.85 epoch sec; seqNo:1208 2021-02-12T23:20:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.01 , 68.98 , 66.22 , 63.46 2021-02-12T23:20:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , 0.05 2021-02-12T23:20:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172006.86 epoch sec; seqNo:2829; 4056730.29 north; 590788.75 east 2021-02-12T23:20:07Z- AHRS_M2 msg: 1613172007.19 epoch sec; seqNo:2803 2021-02-12T23:20:07Z- AHRS_M2 msg: 0.55 phi; 0.10 theta; -0.14 psi 2021-02-12T23:20:07Z- handleDepth()-Depth_Keller msg: 1613172007205 epoch sec; seqNo:2830; depth 30.40 2021-02-12T23:20:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172007.22 epoch sec; seqNo:2830; 4056730.67 north; 590788.75 east 2021-02-12T23:20:07Z- AHRS_M2 msg: 1613172007.60 epoch sec; seqNo:2804 2021-02-12T23:20:07Z- AHRS_M2 msg: 0.54 phi; 0.13 theta; -0.14 psi 2021-02-12T23:20:07Z- handleDepth()-Depth_Keller msg: 1613172007606 epoch sec; seqNo:2831; depth 30.40 2021-02-12T23:20:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172007.62 epoch sec; seqNo:2831; 4056731.05 north; 590788.74 east 2021-02-12T23:20:08Z- AHRS_M2 msg: 1613172008.00 epoch sec; seqNo:2805 2021-02-12T23:20:08Z- AHRS_M2 msg: 0.54 phi; 0.16 theta; -0.14 psi 2021-02-12T23:20:08Z- handleDepth()-Depth_Keller msg: 1613172008008 epoch sec; seqNo:2832; depth 30.43 2021-02-12T23:20:08Z- handleDvl() - RDI_Pathfinder msg: 1613172008.03 epoch sec; seqNo:1209 2021-02-12T23:20:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.31 , 71.12 , 71.43 , 61.01 2021-02-12T23:20:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.01 2021-02-12T23:20:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172008.04 epoch sec; seqNo:2832; 4056731.42 north; 590788.74 east 2021-02-12T23:20:08Z- AHRS_M2 msg: 1613172008.41 epoch sec; seqNo:2806 2021-02-12T23:20:08Z- AHRS_M2 msg: 0.53 phi; 0.19 theta; -0.14 psi 2021-02-12T23:20:08Z- handleDepth()-Depth_Keller msg: 1613172008420 epoch sec; seqNo:2833; depth 30.43 2021-02-12T23:20:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172008.43 epoch sec; seqNo:2833; 4056731.80 north; 590788.74 east 2021-02-12T23:20:08Z- AHRS_M2 msg: 1613172008.84 epoch sec; seqNo:2807 2021-02-12T23:20:08Z- AHRS_M2 msg: 0.53 phi; 0.22 theta; -0.14 psi 2021-02-12T23:20:08Z- handleDepth()-Depth_Keller msg: 1613172008866 epoch sec; seqNo:2834; depth 30.42 2021-02-12T23:20:08Z- handleDvl() - RDI_Pathfinder msg: 1613172008.89 epoch sec; seqNo:1210 2021-02-12T23:20:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.23 , 71.12 , 75.41 , 58.86 2021-02-12T23:20:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.02 , -0.03 2021-02-12T23:20:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172008.91 epoch sec; seqNo:2834; 4056732.19 north; 590789.95 east 2021-02-12T23:20:09Z- AHRS_M2 msg: 1613172009.22 epoch sec; seqNo:2808 2021-02-12T23:20:09Z- AHRS_M2 msg: 0.53 phi; 0.24 theta; -0.14 psi 2021-02-12T23:20:09Z- handleDepth()-Depth_Keller msg: 1613172009223 epoch sec; seqNo:2835; depth 30.37 2021-02-12T23:20:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:09Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:09Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:09Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:09Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:09Z- handleDvl() - RDI_Pathfinder msg: 1613172009.22 epoch sec; seqNo:1211 2021-02-12T23:20:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.02 , -0.03 2021-02-12T23:20:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172009.24 epoch sec; seqNo:2835; 4056732.19 north; 590789.95 east 2021-02-12T23:20:09Z- AHRS_M2 msg: 1613172009.62 epoch sec; seqNo:2809 2021-02-12T23:20:09Z- AHRS_M2 msg: 0.53 phi; 0.26 theta; -0.14 psi 2021-02-12T23:20:09Z- handleDepth()-Depth_Keller msg: 1613172009629 epoch sec; seqNo:2836; depth 30.34 2021-02-12T23:20:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172009.64 epoch sec; seqNo:2836; 4056732.57 north; 590789.95 east 2021-02-12T23:20:09Z- LcmTrn::handleMessages() - handling 43 messages took 0.68 ms and 130 calls... 2021-02-12T23:20:09Z- waiting for 1613172007.80, time is 1613172009.64 2021-02-12T23:20:09Z- LcmTrn::updateTrn() >>>> pose time 1613172009.64, meas time 1613172009.22 <<<< 2021-02-12T23:20:09Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172009.22, ping # 0. 2021-02-12T23:20:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172009.22 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:20:09Z- TerrainNav::motionUpdate() - currEstimate = (4056733, 590789.95, 30.34) 2021-02-12T23:20:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:20:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:20:09Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:20:09Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:20:10Z- TNavBF::Projecting Measurements 2021-02-12T23:20:10Z- TNavFilter::Measurement from time = 1613172009.22 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:20:10Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:20:10Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:20:10Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:20:10Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:20:10Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:20:10Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:20:10Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:20:10Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 776.7 EastVariance: 833.1 DepthVariance: 2.0 this->DepthBiasCov: 2.0 2021-02-12T23:20:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:20:10Z- TNavBF::mmse selected from filter -1 2021-02-12T23:20:10Z- variances([0], [2], and psi_berg) = 776.631442 833.062941 0.000000 2021-02-12T23:20:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:20:10Z- LcmTrn::publishEstimates() - MLE : 4056710.07 590743.45 30.34 2021-02-12T23:20:10Z- LcmTrn::publishEstimates() - MMSE : 4056732.57 590789.95 30.34 2021-02-12T23:20:10Z- LcmTrn::publishEstimates() - COVAR: 776.63 833.06 2.00 0.00 2021-02-12T23:20:10Z- AHRS_M2 msg: 1613172010.02 epoch sec; seqNo:2810 2021-02-12T23:20:10Z- AHRS_M2 msg: 0.53 phi; 0.28 theta; -0.13 psi 2021-02-12T23:20:10Z- handleDepth()-Depth_Keller msg: 1613172010033 epoch sec; seqNo:2837; depth 30.28 2021-02-12T23:20:10Z- handleDvl() - RDI_Pathfinder msg: 1613172010.04 epoch sec; seqNo:1212 2021-02-12T23:20:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.46 , 72.35 , 80.01 , 57.94 2021-02-12T23:20:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , -0.07 2021-02-12T23:20:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172010.07 epoch sec; seqNo:2837; 4056732.95 north; 590789.95 east 2021-02-12T23:20:10Z- AHRS_M2 msg: 1613172010.43 epoch sec; seqNo:2811 2021-02-12T23:20:10Z- AHRS_M2 msg: 0.53 phi; 0.29 theta; -0.13 psi 2021-02-12T23:20:10Z- handleDepth()-Depth_Keller msg: 1613172010433 epoch sec; seqNo:2838; depth 30.24 2021-02-12T23:20:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:10Z- handleDvl() - RDI_Pathfinder msg: 1613172010.43 epoch sec; seqNo:1213 2021-02-12T23:20:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , -0.07 2021-02-12T23:20:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172010.45 epoch sec; seqNo:2838; 4056732.95 north; 590789.95 east 2021-02-12T23:20:10Z- AHRS_M2 msg: 1613172010.83 epoch sec; seqNo:2812 2021-02-12T23:20:10Z- AHRS_M2 msg: 0.54 phi; 0.31 theta; -0.13 psi 2021-02-12T23:20:10Z- handleDepth()-Depth_Keller msg: 1613172010838 epoch sec; seqNo:2839; depth 30.16 2021-02-12T23:20:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172010.85 epoch sec; seqNo:2839; 4056733.33 north; 590789.94 east 2021-02-12T23:20:11Z- AHRS_M2 msg: 1613172011.24 epoch sec; seqNo:2813 2021-02-12T23:20:11Z- AHRS_M2 msg: 0.54 phi; 0.32 theta; -0.12 psi 2021-02-12T23:20:11Z- handleDepth()-Depth_Keller msg: 1613172011249 epoch sec; seqNo:2840; depth 30.09 2021-02-12T23:20:11Z- handleDvl() - RDI_Pathfinder msg: 1613172011.27 epoch sec; seqNo:1214 2021-02-12T23:20:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.14 , 72.72 , 0.00 , 57.20 2021-02-12T23:20:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.11 2021-02-12T23:20:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172011.28 epoch sec; seqNo:2840; 4056733.72 north; 590791.16 east 2021-02-12T23:20:11Z- AHRS_M2 msg: 1613172011.64 epoch sec; seqNo:2814 2021-02-12T23:20:11Z- AHRS_M2 msg: 0.55 phi; 0.33 theta; -0.12 psi 2021-02-12T23:20:11Z- handleDepth()-Depth_Keller msg: 1613172011649 epoch sec; seqNo:2841; depth 29.99 2021-02-12T23:20:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:11Z- handleDvl() - RDI_Pathfinder msg: 1613172011.65 epoch sec; seqNo:1215 2021-02-12T23:20:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.11 2021-02-12T23:20:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172011.66 epoch sec; seqNo:2841; 4056734.10 north; 590791.15 east 2021-02-12T23:20:12Z- AHRS_M2 msg: 1613172012.05 epoch sec; seqNo:2815 2021-02-12T23:20:12Z- AHRS_M2 msg: 0.56 phi; 0.33 theta; -0.12 psi 2021-02-12T23:20:12Z- handleDepth()-Depth_Keller msg: 1613172012049 epoch sec; seqNo:2842; depth 29.91 2021-02-12T23:20:12Z- handleDvl() - RDI_Pathfinder msg: 1613172012.06 epoch sec; seqNo:1216 2021-02-12T23:20:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.60 , 73.57 , 84.92 , 58.55 2021-02-12T23:20:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , -0.12 2021-02-12T23:20:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172012.07 epoch sec; seqNo:2842; 4056734.10 north; 590791.15 east 2021-02-12T23:20:12Z- AHRS_M2 msg: 1613172012.45 epoch sec; seqNo:2816 2021-02-12T23:20:12Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.11 psi 2021-02-12T23:20:12Z- handleDepth()-Depth_Keller msg: 1613172012452 epoch sec; seqNo:2843; depth 29.82 2021-02-12T23:20:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172012.47 epoch sec; seqNo:2843; 4056734.48 north; 590791.15 east 2021-02-12T23:20:12Z- AHRS_M2 msg: 1613172012.85 epoch sec; seqNo:2817 2021-02-12T23:20:12Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.11 psi 2021-02-12T23:20:12Z- handleDepth()-Depth_Keller msg: 1613172012860 epoch sec; seqNo:2844; depth 29.71 2021-02-12T23:20:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:12Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:12Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:12Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:12Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:12Z- handleDvl() - RDI_Pathfinder msg: 1613172012.86 epoch sec; seqNo:1217 2021-02-12T23:20:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , -0.12 2021-02-12T23:20:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172012.87 epoch sec; seqNo:2844; 4056734.86 north; 590791.15 east 2021-02-12T23:20:13Z- AHRS_M2 msg: 1613172013.26 epoch sec; seqNo:2818 2021-02-12T23:20:13Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.10 psi 2021-02-12T23:20:13Z- handleDepth()-Depth_Keller msg: 1613172013260 epoch sec; seqNo:2845; depth 29.62 2021-02-12T23:20:13Z- handleDvl() - RDI_Pathfinder msg: 1613172013.27 epoch sec; seqNo:1218 2021-02-12T23:20:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.19 , 73.13 , 93.58 , 57.20 2021-02-12T23:20:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.10 , -0.11 2021-02-12T23:20:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172013.28 epoch sec; seqNo:2845; 4056735.24 north; 590791.14 east 2021-02-12T23:20:13Z- AHRS_M2 msg: 1613172013.66 epoch sec; seqNo:2819 2021-02-12T23:20:13Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.10 psi 2021-02-12T23:20:13Z- handleDepth()-Depth_Keller msg: 1613172013666 epoch sec; seqNo:2846; depth 29.49 2021-02-12T23:20:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172013.68 epoch sec; seqNo:2846; 4056735.24 north; 590791.14 east 2021-02-12T23:20:14Z- AHRS_M2 msg: 1613172014.06 epoch sec; seqNo:2820 2021-02-12T23:20:14Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.09 psi 2021-02-12T23:20:14Z- handleDepth()-Depth_Keller msg: 1613172014068 epoch sec; seqNo:2847; depth 29.39 2021-02-12T23:20:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:14Z- handleDvl() - RDI_Pathfinder msg: 1613172014.07 epoch sec; seqNo:1219 2021-02-12T23:20:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.10 , -0.11 2021-02-12T23:20:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172014.09 epoch sec; seqNo:2847; 4056735.62 north; 590791.14 east 2021-02-12T23:20:14Z- AHRS_M2 msg: 1613172014.47 epoch sec; seqNo:2821 2021-02-12T23:20:14Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.09 psi 2021-02-12T23:20:14Z- handleDepth()-Depth_Keller msg: 1613172014472 epoch sec; seqNo:2848; depth 29.29 2021-02-12T23:20:14Z- handleDvl() - RDI_Pathfinder msg: 1613172014.48 epoch sec; seqNo:1220 2021-02-12T23:20:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.60 , 72.72 , 92.50 , 58.43 2021-02-12T23:20:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.11 , -0.06 2021-02-12T23:20:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172014.49 epoch sec; seqNo:2848; 4056736.00 north; 590791.13 east 2021-02-12T23:20:14Z- AHRS_M2 msg: 1613172014.88 epoch sec; seqNo:2822 2021-02-12T23:20:14Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.08 psi 2021-02-12T23:20:14Z- handleDepth()-Depth_Keller msg: 1613172014882 epoch sec; seqNo:2849; depth 29.15 2021-02-12T23:20:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172014.89 epoch sec; seqNo:2849; 4056736.01 north; 590792.36 east 2021-02-12T23:20:15Z- LcmTrn::handleMessages() - handling 48 messages took 0.68 ms and 135 calls... 2021-02-12T23:20:15Z- waiting for 1613172012.64, time is 1613172014.89 2021-02-12T23:20:15Z- LcmTrn::updateTrn() >>>> pose time 1613172014.89, meas time 1613172014.48 <<<< 2021-02-12T23:20:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172014.48 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:20:15Z- TerrainNav::motionUpdate() - currEstimate = (4056736, 590792.36, 29.15) 2021-02-12T23:20:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:20:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:20:15Z- TNavBF::Projecting Measurements 2021-02-12T23:20:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:20:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:20:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:20:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:20:15Z- TNavFilter::4 good beams. 2021-02-12T23:20:15Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:20:15Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:20:15Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:20:15Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:20:15Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:20:15Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:20:15Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:20:15Z- TNavBF::mmse selected from filter -1 2021-02-12T23:20:15Z- TerrainMapOctree: (vehN, vehE ) = (4056735.74, 590792.17). 2021-02-12T23:20:15Z- TerrainMapOctree: Min Distance = 5686.40. 2021-02-12T23:20:15Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:20:15Z- There is no specific good beam that all particles have in common. 2021-02-12T23:20:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:20:15Z- Old Depth Bias: 0.00 DepthBiasCov: 2.20 K: 0.200 ExpectedMeasDiff: nan currMeas.covariance: 7.10 2021-02-12T23:20:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:20:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.76 K: 0.147 ExpectedMeasDiff: nan currMeas.covariance: 8.46 2021-02-12T23:20:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:20:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.50 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 13.69 2021-02-12T23:20:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:20:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.160 ExpectedMeasDiff: nan currMeas.covariance: 5.46 2021-02-12T23:20:15Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:20:15Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:20:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:20:15Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:20:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:20:15Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:20:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:20:15Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:20:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:20:15Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:20:15Z- Filter 0 No beams used in subcloud update 2021-02-12T23:20:15Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:20:15Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:20:15Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 776.5 EastVariance: 833.1 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:20:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172014.48 sec. 2021-02-12T23:20:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:20:15Z- TNavBF::mmse selected from filter -1 2021-02-12T23:20:15Z- variances([0], [2], and psi_berg) = 776.448571 833.062693 0.000000 2021-02-12T23:20:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:20:15Z- LcmTrn::publishEstimates() - MLE : 4056713.51 590745.86 29.15 2021-02-12T23:20:15Z- LcmTrn::publishEstimates() - MMSE : 4056736.01 590792.36 29.15 2021-02-12T23:20:15Z- LcmTrn::publishEstimates() - COVAR: 776.45 833.06 1.17 0.00 2021-02-12T23:20:15Z- AHRS_M2 msg: 1613172015.28 epoch sec; seqNo:2823 2021-02-12T23:20:15Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.08 psi 2021-02-12T23:20:15Z- handleDepth()-Depth_Keller msg: 1613172015280 epoch sec; seqNo:2850; depth 29.03 2021-02-12T23:20:15Z- handleDvl() - RDI_Pathfinder msg: 1613172015.30 epoch sec; seqNo:1221 2021-02-12T23:20:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.41 , 72.72 , 93.58 , 57.61 2021-02-12T23:20:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.12 , 0.02 2021-02-12T23:20:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172015.31 epoch sec; seqNo:2850; 4056736.39 north; 590792.35 east 2021-02-12T23:20:15Z- AHRS_M2 msg: 1613172015.68 epoch sec; seqNo:2824 2021-02-12T23:20:15Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.08 psi 2021-02-12T23:20:15Z- handleDepth()-Depth_Keller msg: 1613172015689 epoch sec; seqNo:2851; depth 28.91 2021-02-12T23:20:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172015.71 epoch sec; seqNo:2851; 4056736.77 north; 590792.35 east 2021-02-12T23:20:16Z- AHRS_M2 msg: 1613172016.09 epoch sec; seqNo:2825 2021-02-12T23:20:16Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.09 psi 2021-02-12T23:20:16Z- handleDepth()-Depth_Keller msg: 1613172016092 epoch sec; seqNo:2852; depth 28.79 2021-02-12T23:20:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172016.11 epoch sec; seqNo:2852; 4056737.15 north; 590792.34 east 2021-02-12T23:20:16Z- AHRS_M2 msg: 1613172016.49 epoch sec; seqNo:2826 2021-02-12T23:20:16Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.09 psi 2021-02-12T23:20:16Z- handleDepth()-Depth_Keller msg: 1613172016492 epoch sec; seqNo:2853; depth 28.68 2021-02-12T23:20:16Z- handleDvl() - RDI_Pathfinder msg: 1613172016.51 epoch sec; seqNo:1222 2021-02-12T23:20:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.41 , 73.13 , 93.58 , 58.83 2021-02-12T23:20:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , 0.08 2021-02-12T23:20:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172016.52 epoch sec; seqNo:2853; 4056737.15 north; 590792.34 east 2021-02-12T23:20:16Z- AHRS_M2 msg: 1613172016.89 epoch sec; seqNo:2827 2021-02-12T23:20:16Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.09 psi 2021-02-12T23:20:16Z- handleDepth()-Depth_Keller msg: 1613172016903 epoch sec; seqNo:2854; depth 28.53 2021-02-12T23:20:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172016.91 epoch sec; seqNo:2854; 4056737.52 north; 590792.34 east 2021-02-12T23:20:17Z- AHRS_M2 msg: 1613172017.30 epoch sec; seqNo:2828 2021-02-12T23:20:17Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.09 psi 2021-02-12T23:20:17Z- handleDepth()-Depth_Keller msg: 1613172017300 epoch sec; seqNo:2855; depth 28.42 2021-02-12T23:20:17Z- handleDvl() - RDI_Pathfinder msg: 1613172017.33 epoch sec; seqNo:1223 2021-02-12T23:20:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.23 , 73.13 , 97.91 , 58.83 2021-02-12T23:20:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.12 2021-02-12T23:20:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172017.34 epoch sec; seqNo:2855; 4056737.90 north; 590792.34 east 2021-02-12T23:20:17Z- AHRS_M2 msg: 1613172017.70 epoch sec; seqNo:2829 2021-02-12T23:20:17Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.09 psi 2021-02-12T23:20:17Z- handleDepth()-Depth_Keller msg: 1613172017709 epoch sec; seqNo:2856; depth 28.26 2021-02-12T23:20:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:17Z- handleDvl() - RDI_Pathfinder msg: 1613172017.71 epoch sec; seqNo:1224 2021-02-12T23:20:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.12 2021-02-12T23:20:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172017.72 epoch sec; seqNo:2856; 4056738.28 north; 590792.33 east 2021-02-12T23:20:18Z- AHRS_M2 msg: 1613172018.12 epoch sec; seqNo:2830 2021-02-12T23:20:18Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.09 psi 2021-02-12T23:20:18Z- handleDepth()-Depth_Keller msg: 1613172018130 epoch sec; seqNo:2857; depth 28.15 2021-02-12T23:20:18Z- handleDvl() - RDI_Pathfinder msg: 1613172018.15 epoch sec; seqNo:1225 2021-02-12T23:20:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.64 , 73.95 , 97.91 , 58.43 2021-02-12T23:20:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , 0.11 2021-02-12T23:20:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172018.17 epoch sec; seqNo:2857; 4056738.67 north; 590793.55 east 2021-02-12T23:20:18Z- AHRS_M2 msg: 1613172018.51 epoch sec; seqNo:2831 2021-02-12T23:20:18Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.10 psi 2021-02-12T23:20:18Z- handleDepth()-Depth_Keller msg: 1613172018517 epoch sec; seqNo:2858; depth 28.00 2021-02-12T23:20:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172018.54 epoch sec; seqNo:2858; 4056738.67 north; 590793.55 east 2021-02-12T23:20:18Z- AHRS_M2 msg: 1613172018.91 epoch sec; seqNo:2832 2021-02-12T23:20:18Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.10 psi 2021-02-12T23:20:18Z- handleDepth()-Depth_Keller msg: 1613172018916 epoch sec; seqNo:2859; depth 27.89 2021-02-12T23:20:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:18Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:18Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:18Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:18Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:18Z- handleDvl() - RDI_Pathfinder msg: 1613172018.92 epoch sec; seqNo:1226 2021-02-12T23:20:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , 0.11 2021-02-12T23:20:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172018.93 epoch sec; seqNo:2859; 4056739.05 north; 590793.54 east 2021-02-12T23:20:19Z- AHRS_M2 msg: 1613172019.32 epoch sec; seqNo:2833 2021-02-12T23:20:19Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.10 psi 2021-02-12T23:20:19Z- handleDepth()-Depth_Keller msg: 1613172019324 epoch sec; seqNo:2860; depth 27.76 2021-02-12T23:20:19Z- handleDvl() - RDI_Pathfinder msg: 1613172019.34 epoch sec; seqNo:1227 2021-02-12T23:20:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.64 , 74.77 , 97.91 , 59.24 2021-02-12T23:20:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , 0.08 2021-02-12T23:20:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172019.35 epoch sec; seqNo:2860; 4056739.43 north; 590793.54 east 2021-02-12T23:20:19Z- AHRS_M2 msg: 1613172019.72 epoch sec; seqNo:2834 2021-02-12T23:20:19Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.10 psi 2021-02-12T23:20:19Z- handleDepth()-Depth_Keller msg: 1613172019731 epoch sec; seqNo:2861; depth 27.62 2021-02-12T23:20:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172019.75 epoch sec; seqNo:2861; 4056739.43 north; 590793.54 east 2021-02-12T23:20:20Z- LcmTrn::handleMessages() - handling 43 messages took 0.67 ms and 130 calls... 2021-02-12T23:20:20Z- waiting for 1613172017.89, time is 1613172019.75 2021-02-12T23:20:20Z- LcmTrn::updateTrn() >>>> pose time 1613172019.75, meas time 1613172019.34 <<<< 2021-02-12T23:20:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172019.34 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:20:20Z- TerrainNav::motionUpdate() - currEstimate = (4056739, 590793.54, 27.62) 2021-02-12T23:20:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:20:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:20:20Z- TNavBF::Projecting Measurements 2021-02-12T23:20:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:20:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:20:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:20:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:20:20Z- TNavFilter::4 good beams. 2021-02-12T23:20:20Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:20:20Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:20:20Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:20:20Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:20:20Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:20:20Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:20:20Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:20:20Z- TNavBF::mmse selected from filter -1 2021-02-12T23:20:20Z- TerrainMapOctree: (vehN, vehE ) = (4056739.14, 590793.44). 2021-02-12T23:20:20Z- TerrainMapOctree: Min Distance = 5682.92. 2021-02-12T23:20:20Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:20:20Z- There is no specific good beam that all particles have in common. 2021-02-12T23:20:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:20:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.126 ExpectedMeasDiff: nan currMeas.covariance: 7.54 2021-02-12T23:20:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:20:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 8.94 2021-02-12T23:20:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:20:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-12T23:20:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:20:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.119 ExpectedMeasDiff: nan currMeas.covariance: 5.62 2021-02-12T23:20:20Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:20:20Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:20:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:20:20Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:20:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:20:20Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:20:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:20:20Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:20:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:20:20Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:20:20Z- Filter 0 No beams used in subcloud update 2021-02-12T23:20:20Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:20:20Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:20:20Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 776.3 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:20:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172019.34 sec. 2021-02-12T23:20:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:20:20Z- TNavBF::mmse selected from filter -1 2021-02-12T23:20:20Z- variances([0], [2], and psi_berg) = 776.278140 833.062462 0.000000 2021-02-12T23:20:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:20:20Z- LcmTrn::publishEstimates() - MLE : 4056716.93 590747.04 27.62 2021-02-12T23:20:20Z- LcmTrn::publishEstimates() - MMSE : 4056739.43 590793.54 27.62 2021-02-12T23:20:20Z- LcmTrn::publishEstimates() - COVAR: 776.28 833.06 0.89 0.00 2021-02-12T23:20:20Z- AHRS_M2 msg: 1613172020.15 epoch sec; seqNo:2835 2021-02-12T23:20:20Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.09 psi 2021-02-12T23:20:20Z- handleDepth()-Depth_Keller msg: 1613172020155 epoch sec; seqNo:2862; depth 27.48 2021-02-12T23:20:20Z- handleDvl() - RDI_Pathfinder msg: 1613172020.17 epoch sec; seqNo:1228 2021-02-12T23:20:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.05 , 74.77 , 96.82 , 60.06 2021-02-12T23:20:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.09 , 0.04 2021-02-12T23:20:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172020.18 epoch sec; seqNo:2862; 4056739.82 north; 590794.76 east 2021-02-12T23:20:20Z- AHRS_M2 msg: 1613172020.53 epoch sec; seqNo:2836 2021-02-12T23:20:20Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.09 psi 2021-02-12T23:20:20Z- handleDepth()-Depth_Keller msg: 1613172020541 epoch sec; seqNo:2863; depth 27.33 2021-02-12T23:20:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172020.55 epoch sec; seqNo:2863; 4056740.20 north; 590794.75 east 2021-02-12T23:20:20Z- AHRS_M2 msg: 1613172020.94 epoch sec; seqNo:2837 2021-02-12T23:20:20Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.09 psi 2021-02-12T23:20:20Z- handleDepth()-Depth_Keller msg: 1613172020943 epoch sec; seqNo:2864; depth 27.22 2021-02-12T23:20:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172020.96 epoch sec; seqNo:2864; 4056740.20 north; 590794.75 east 2021-02-12T23:20:21Z- AHRS_M2 msg: 1613172021.34 epoch sec; seqNo:2838 2021-02-12T23:20:21Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.09 psi 2021-02-12T23:20:21Z- handleDepth()-Depth_Keller msg: 1613172021344 epoch sec; seqNo:2865; depth 27.09 2021-02-12T23:20:21Z- handleDvl() - RDI_Pathfinder msg: 1613172021.36 epoch sec; seqNo:1229 2021-02-12T23:20:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.46 , 75.18 , 95.74 , 60.47 2021-02-12T23:20:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.01 2021-02-12T23:20:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172021.37 epoch sec; seqNo:2865; 4056740.58 north; 590794.75 east 2021-02-12T23:20:21Z- AHRS_M2 msg: 1613172021.74 epoch sec; seqNo:2839 2021-02-12T23:20:21Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.11 psi 2021-02-12T23:20:21Z- handleDepth()-Depth_Keller msg: 1613172021749 epoch sec; seqNo:2866; depth 26.95 2021-02-12T23:20:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172021.77 epoch sec; seqNo:2866; 4056740.96 north; 590794.75 east 2021-02-12T23:20:22Z- AHRS_M2 msg: 1613172022.15 epoch sec; seqNo:2840 2021-02-12T23:20:22Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.12 psi 2021-02-12T23:20:22Z- handleDepth()-Depth_Keller msg: 1613172022154 epoch sec; seqNo:2867; depth 26.82 2021-02-12T23:20:22Z- handleDvl() - RDI_Pathfinder msg: 1613172022.17 epoch sec; seqNo:1230 2021-02-12T23:20:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.05 , 76.40 , 95.74 , 60.88 2021-02-12T23:20:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , 0.01 2021-02-12T23:20:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172022.19 epoch sec; seqNo:2867; 4056740.96 north; 590794.75 east 2021-02-12T23:20:22Z- AHRS_M2 msg: 1613172022.55 epoch sec; seqNo:2841 2021-02-12T23:20:22Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.13 psi 2021-02-12T23:20:22Z- handleDepth()-Depth_Keller msg: 1613172022552 epoch sec; seqNo:2868; depth 26.69 2021-02-12T23:20:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:22Z- handleDvl() - RDI_Pathfinder msg: 1613172022.55 epoch sec; seqNo:1231 2021-02-12T23:20:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , 0.01 2021-02-12T23:20:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172022.57 epoch sec; seqNo:2868; 4056741.35 north; 590795.96 east 2021-02-12T23:20:22Z- AHRS_M2 msg: 1613172022.95 epoch sec; seqNo:2842 2021-02-12T23:20:22Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.14 psi 2021-02-12T23:20:22Z- handleDepth()-Depth_Keller msg: 1613172022967 epoch sec; seqNo:2869; depth 26.57 2021-02-12T23:20:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172022.98 epoch sec; seqNo:2869; 4056741.73 north; 590795.96 east 2021-02-12T23:20:23Z- AHRS_M2 msg: 1613172023.36 epoch sec; seqNo:2843 2021-02-12T23:20:23Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.15 psi 2021-02-12T23:20:23Z- handleDepth()-Depth_Keller msg: 1613172023361 epoch sec; seqNo:2870; depth 26.44 2021-02-12T23:20:23Z- handleDvl() - RDI_Pathfinder msg: 1613172023.38 epoch sec; seqNo:1232 2021-02-12T23:20:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.23 , 78.04 , 94.66 , 60.47 2021-02-12T23:20:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , -0.01 2021-02-12T23:20:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172023.39 epoch sec; seqNo:2870; 4056742.11 north; 590795.95 east 2021-02-12T23:20:23Z- AHRS_M2 msg: 1613172023.76 epoch sec; seqNo:2844 2021-02-12T23:20:23Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.16 psi 2021-02-12T23:20:23Z- handleDepth()-Depth_Keller msg: 1613172023764 epoch sec; seqNo:2871; depth 26.30 2021-02-12T23:20:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:23Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:23Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:23Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:23Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:23Z- handleDvl() - RDI_Pathfinder msg: 1613172023.77 epoch sec; seqNo:1233 2021-02-12T23:20:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , -0.01 2021-02-12T23:20:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172023.78 epoch sec; seqNo:2871; 4056742.11 north; 590795.95 east 2021-02-12T23:20:24Z- AHRS_M2 msg: 1613172024.16 epoch sec; seqNo:2845 2021-02-12T23:20:24Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.16 psi 2021-02-12T23:20:24Z- handleDepth()-Depth_Keller msg: 1613172024170 epoch sec; seqNo:2872; depth 26.18 2021-02-12T23:20:24Z- handleDvl() - RDI_Pathfinder msg: 1613172024.18 epoch sec; seqNo:1234 2021-02-12T23:20:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.23 , 79.67 , 96.82 , 61.69 2021-02-12T23:20:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.02 2021-02-12T23:20:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172024.19 epoch sec; seqNo:2872; 4056742.49 north; 590795.95 east 2021-02-12T23:20:24Z- AHRS_M2 msg: 1613172024.57 epoch sec; seqNo:2846 2021-02-12T23:20:24Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.17 psi 2021-02-12T23:20:24Z- handleDepth()-Depth_Keller msg: 1613172024572 epoch sec; seqNo:2873; depth 26.06 2021-02-12T23:20:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172024.59 epoch sec; seqNo:2873; 4056742.87 north; 590795.95 east 2021-02-12T23:20:24Z- AHRS_M2 msg: 1613172024.97 epoch sec; seqNo:2847 2021-02-12T23:20:24Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.17 psi 2021-02-12T23:20:24Z- handleDepth()-Depth_Keller msg: 1613172024981 epoch sec; seqNo:2874; depth 25.92 2021-02-12T23:20:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:24Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:24Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:24Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:24Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:24Z- handleDvl() - RDI_Pathfinder msg: 1613172024.98 epoch sec; seqNo:1235 2021-02-12T23:20:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.02 2021-02-12T23:20:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172024.99 epoch sec; seqNo:2874; 4056742.88 north; 590797.17 east 2021-02-12T23:20:25Z- LcmTrn::handleMessages() - handling 47 messages took 0.60 ms and 135 calls... 2021-02-12T23:20:25Z- waiting for 1613172022.75, time is 1613172024.99 2021-02-12T23:20:25Z- LcmTrn::updateTrn() equating pose 1613172024.99 and meas times 1613172024.99 2021-02-12T23:20:25Z- LcmTrn::updateTrn() >>>> pose time 1613172024.99, meas time 1613172024.99 <<<< 2021-02-12T23:20:25Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172024.99, ping # 0. 2021-02-12T23:20:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172024.99 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:20:25Z- TerrainNav::motionUpdate() - currEstimate = (4056743, 590797.17, 25.92) 2021-02-12T23:20:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:20:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:20:25Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:20:25Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:20:25Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172024.99 sec.; No relevant navigation data available 2021-02-12T23:20:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:20:25Z- TNavBF::mmse selected from filter -1 2021-02-12T23:20:25Z- variances([0], [2], and psi_berg) = 776.117847 833.062245 0.000000 2021-02-12T23:20:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:20:25Z- LcmTrn::publishEstimates() - MLE : 4056720.38 590750.67 25.92 2021-02-12T23:20:25Z- LcmTrn::publishEstimates() - MMSE : 4056742.88 590797.17 25.92 2021-02-12T23:20:25Z- LcmTrn::publishEstimates() - COVAR: 776.12 833.06 1.14 0.00 2021-02-12T23:20:25Z- AHRS_M2 msg: 1613172025.38 epoch sec; seqNo:2848 2021-02-12T23:20:25Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.17 psi 2021-02-12T23:20:25Z- handleDepth()-Depth_Keller msg: 1613172025393 epoch sec; seqNo:2875; depth 25.80 2021-02-12T23:20:25Z- handleDvl() - RDI_Pathfinder msg: 1613172025.42 epoch sec; seqNo:1236 2021-02-12T23:20:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.23 , 84.93 , 97.91 , 61.69 2021-02-12T23:20:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , -0.04 2021-02-12T23:20:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172025.44 epoch sec; seqNo:2875; 4056743.26 north; 590797.16 east 2021-02-12T23:20:25Z- AHRS_M2 msg: 1613172025.78 epoch sec; seqNo:2849 2021-02-12T23:20:25Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.17 psi 2021-02-12T23:20:25Z- handleDepth()-Depth_Keller msg: 1613172025788 epoch sec; seqNo:2876; depth 25.67 2021-02-12T23:20:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172025.80 epoch sec; seqNo:2876; 4056743.64 north; 590797.16 east 2021-02-12T23:20:26Z- AHRS_M2 msg: 1613172026.20 epoch sec; seqNo:2850 2021-02-12T23:20:26Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.17 psi 2021-02-12T23:20:26Z- handleDepth()-Depth_Keller msg: 1613172026214 epoch sec; seqNo:2877; depth 25.55 2021-02-12T23:20:26Z- handleDvl() - RDI_Pathfinder msg: 1613172026.23 epoch sec; seqNo:1237 2021-02-12T23:20:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.23 , 86.01 , 97.91 , 62.10 2021-02-12T23:20:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.02 , -0.02 2021-02-12T23:20:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172026.24 epoch sec; seqNo:2877; 4056743.64 north; 590797.16 east 2021-02-12T23:20:26Z- AHRS_M2 msg: 1613172026.59 epoch sec; seqNo:2851 2021-02-12T23:20:26Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.17 psi 2021-02-12T23:20:26Z- handleDepth()-Depth_Keller msg: 1613172026597 epoch sec; seqNo:2878; depth 25.43 2021-02-12T23:20:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172026.61 epoch sec; seqNo:2878; 4056744.02 north; 590797.16 east 2021-02-12T23:20:26Z- AHRS_M2 msg: 1613172026.99 epoch sec; seqNo:2852 2021-02-12T23:20:26Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.17 psi 2021-02-12T23:20:26Z- handleDepth()-Depth_Keller msg: 1613172026997 epoch sec; seqNo:2879; depth 25.31 2021-02-12T23:20:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172027.01 epoch sec; seqNo:2879; 4056744.40 north; 590797.15 east 2021-02-12T23:20:27Z- AHRS_M2 msg: 1613172027.40 epoch sec; seqNo:2853 2021-02-12T23:20:27Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.17 psi 2021-02-12T23:20:27Z- handleDepth()-Depth_Keller msg: 1613172027414 epoch sec; seqNo:2880; depth 25.17 2021-02-12T23:20:27Z- handleDvl() - RDI_Pathfinder msg: 1613172027.43 epoch sec; seqNo:1238 2021-02-12T23:20:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.23 , 88.17 , 96.82 , 62.10 2021-02-12T23:20:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.00 , -0.00 2021-02-12T23:20:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172027.44 epoch sec; seqNo:2880; 4056744.41 north; 590798.37 east 2021-02-12T23:20:27Z- AHRS_M2 msg: 1613172027.80 epoch sec; seqNo:2854 2021-02-12T23:20:27Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.17 psi 2021-02-12T23:20:27Z- handleDepth()-Depth_Keller msg: 1613172027809 epoch sec; seqNo:2881; depth 25.05 2021-02-12T23:20:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172027.83 epoch sec; seqNo:2881; 4056744.79 north; 590798.37 east 2021-02-12T23:20:28Z- AHRS_M2 msg: 1613172028.22 epoch sec; seqNo:2855 2021-02-12T23:20:28Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.17 psi 2021-02-12T23:20:28Z- handleDepth()-Depth_Keller msg: 1613172028221 epoch sec; seqNo:2882; depth 24.92 2021-02-12T23:20:28Z- handleDvl() - RDI_Pathfinder msg: 1613172028.23 epoch sec; seqNo:1239 2021-02-12T23:20:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.23 , 86.01 , 95.74 , 62.51 2021-02-12T23:20:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , 0.04 2021-02-12T23:20:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172028.26 epoch sec; seqNo:2882; 4056745.17 north; 590798.36 east 2021-02-12T23:20:28Z- AHRS_M2 msg: 1613172028.61 epoch sec; seqNo:2856 2021-02-12T23:20:28Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.17 psi 2021-02-12T23:20:28Z- handleDepth()-Depth_Keller msg: 1613172028613 epoch sec; seqNo:2883; depth 24.81 2021-02-12T23:20:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:28Z- handleDvl() - RDI_Pathfinder msg: 1613172028.62 epoch sec; seqNo:1240 2021-02-12T23:20:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , 0.04 2021-02-12T23:20:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172028.63 epoch sec; seqNo:2883; 4056745.17 north; 590798.36 east 2021-02-12T23:20:29Z- AHRS_M2 msg: 1613172029.01 epoch sec; seqNo:2857 2021-02-12T23:20:29Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.17 psi 2021-02-12T23:20:29Z- handleDepth()-Depth_Keller msg: 1613172029016 epoch sec; seqNo:2884; depth 24.71 2021-02-12T23:20:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172029.03 epoch sec; seqNo:2884; 4056745.54 north; 590798.36 east 2021-02-12T23:20:29Z- AHRS_M2 msg: 1613172029.41 epoch sec; seqNo:2858 2021-02-12T23:20:29Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:20:29Z- handleDepth()-Depth_Keller msg: 1613172029425 epoch sec; seqNo:2885; depth 24.57 2021-02-12T23:20:29Z- handleDvl() - RDI_Pathfinder msg: 1613172029.45 epoch sec; seqNo:1241 2021-02-12T23:20:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.82 , 88.17 , 97.91 , 62.92 2021-02-12T23:20:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.00 , 0.09 2021-02-12T23:20:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172029.46 epoch sec; seqNo:2885; 4056745.92 north; 590798.36 east 2021-02-12T23:20:29Z- AHRS_M2 msg: 1613172029.82 epoch sec; seqNo:2859 2021-02-12T23:20:29Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:20:29Z- handleDepth()-Depth_Keller msg: 1613172029829 epoch sec; seqNo:2886; depth 24.44 2021-02-12T23:20:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:29Z- handleDvl() - RDI_Pathfinder msg: 1613172029.83 epoch sec; seqNo:1242 2021-02-12T23:20:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.00 , 0.09 2021-02-12T23:20:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172029.84 epoch sec; seqNo:2886; 4056745.94 north; 590799.58 east 2021-02-12T23:20:30Z- AHRS_M2 msg: 1613172030.23 epoch sec; seqNo:2860 2021-02-12T23:20:30Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:20:30Z- handleDepth()-Depth_Keller msg: 1613172030231 epoch sec; seqNo:2887; depth 24.33 2021-02-12T23:20:30Z- handleDvl() - RDI_Pathfinder msg: 1613172030.24 epoch sec; seqNo:1243 2021-02-12T23:20:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.64 , 87.09 , 97.91 , 63.74 2021-02-12T23:20:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , 0.09 2021-02-12T23:20:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172030.25 epoch sec; seqNo:2887; 4056746.32 north; 590799.57 east 2021-02-12T23:20:30Z- LcmTrn::handleMessages() - handling 47 messages took 0.74 ms and 132 calls... 2021-02-12T23:20:30Z- waiting for 1613172027.99, time is 1613172030.25 2021-02-12T23:20:30Z- LcmTrn::updateTrn() equating pose 1613172030.25 and meas times 1613172030.25 2021-02-12T23:20:30Z- LcmTrn::updateTrn() >>>> pose time 1613172030.25, meas time 1613172030.25 <<<< 2021-02-12T23:20:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172030.25 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:20:30Z- TerrainNav::motionUpdate() - currEstimate = (4056746, 590799.57, 24.33) 2021-02-12T23:20:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:20:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:20:30Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172030.25 sec.; No relevant navigation data available 2021-02-12T23:20:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:20:30Z- TNavBF::mmse selected from filter -1 2021-02-12T23:20:30Z- variances([0], [2], and psi_berg) = 775.971495 833.062047 0.000000 2021-02-12T23:20:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:20:30Z- LcmTrn::publishEstimates() - MLE : 4056723.82 590753.07 24.33 2021-02-12T23:20:30Z- LcmTrn::publishEstimates() - MMSE : 4056746.32 590799.57 24.33 2021-02-12T23:20:30Z- LcmTrn::publishEstimates() - COVAR: 775.97 833.06 1.36 0.00 2021-02-12T23:20:30Z- AHRS_M2 msg: 1613172030.63 epoch sec; seqNo:2861 2021-02-12T23:20:30Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:20:30Z- handleDepth()-Depth_Keller msg: 1613172030632 epoch sec; seqNo:2888; depth 24.19 2021-02-12T23:20:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172030.64 epoch sec; seqNo:2888; 4056746.69 north; 590799.57 east 2021-02-12T23:20:31Z- AHRS_M2 msg: 1613172031.03 epoch sec; seqNo:2862 2021-02-12T23:20:31Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:20:31Z- handleDepth()-Depth_Keller msg: 1613172031036 epoch sec; seqNo:2889; depth 24.09 2021-02-12T23:20:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:31Z- handleDvl() - RDI_Pathfinder msg: 1613172031.04 epoch sec; seqNo:1244 2021-02-12T23:20:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , 0.09 2021-02-12T23:20:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172031.05 epoch sec; seqNo:2889; 4056746.69 north; 590799.57 east 2021-02-12T23:20:31Z- AHRS_M2 msg: 1613172031.43 epoch sec; seqNo:2863 2021-02-12T23:20:31Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:20:31Z- handleDepth()-Depth_Keller msg: 1613172031447 epoch sec; seqNo:2890; depth 23.98 2021-02-12T23:20:31Z- handleDvl() - RDI_Pathfinder msg: 1613172031.46 epoch sec; seqNo:1245 2021-02-12T23:20:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.05 , 87.09 , 96.82 , 63.74 2021-02-12T23:20:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , 0.06 2021-02-12T23:20:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172031.47 epoch sec; seqNo:2890; 4056747.07 north; 590799.57 east 2021-02-12T23:20:31Z- AHRS_M2 msg: 1613172031.84 epoch sec; seqNo:2864 2021-02-12T23:20:31Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:20:31Z- handleDepth()-Depth_Keller msg: 1613172031844 epoch sec; seqNo:2891; depth 23.86 2021-02-12T23:20:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172031.86 epoch sec; seqNo:2891; 4056747.45 north; 590799.56 east 2021-02-12T23:20:32Z- AHRS_M2 msg: 1613172032.24 epoch sec; seqNo:2865 2021-02-12T23:20:32Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.18 psi 2021-02-12T23:20:32Z- handleDepth()-Depth_Keller msg: 1613172032248 epoch sec; seqNo:2892; depth 23.75 2021-02-12T23:20:32Z- handleDvl() - RDI_Pathfinder msg: 1613172032.26 epoch sec; seqNo:1246 2021-02-12T23:20:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.86 , 88.17 , 96.82 , 64.14 2021-02-12T23:20:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.06 , 0.02 2021-02-12T23:20:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172032.27 epoch sec; seqNo:2892; 4056747.84 north; 590800.78 east 2021-02-12T23:20:32Z- AHRS_M2 msg: 1613172032.65 epoch sec; seqNo:2866 2021-02-12T23:20:32Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.18 psi 2021-02-12T23:20:32Z- handleDepth()-Depth_Keller msg: 1613172032659 epoch sec; seqNo:2893; depth 23.62 2021-02-12T23:20:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172032.67 epoch sec; seqNo:2893; 4056747.84 north; 590800.78 east 2021-02-12T23:20:33Z- AHRS_M2 msg: 1613172033.05 epoch sec; seqNo:2867 2021-02-12T23:20:33Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.18 psi 2021-02-12T23:20:33Z- handleDepth()-Depth_Keller msg: 1613172033061 epoch sec; seqNo:2894; depth 23.51 2021-02-12T23:20:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172033.08 epoch sec; seqNo:2894; 4056748.22 north; 590800.77 east 2021-02-12T23:20:33Z- AHRS_M2 msg: 1613172033.46 epoch sec; seqNo:2868 2021-02-12T23:20:33Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.19 psi 2021-02-12T23:20:33Z- handleDepth()-Depth_Keller msg: 1613172033463 epoch sec; seqNo:2895; depth 23.39 2021-02-12T23:20:33Z- handleDvl() - RDI_Pathfinder msg: 1613172033.47 epoch sec; seqNo:1247 2021-02-12T23:20:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.46 , 88.17 , 96.82 , 64.14 2021-02-12T23:20:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , -0.01 2021-02-12T23:20:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172033.50 epoch sec; seqNo:2895; 4056748.60 north; 590800.77 east 2021-02-12T23:20:33Z- AHRS_M2 msg: 1613172033.86 epoch sec; seqNo:2869 2021-02-12T23:20:33Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.19 psi 2021-02-12T23:20:33Z- handleDepth()-Depth_Keller msg: 1613172033869 epoch sec; seqNo:2896; depth 23.27 2021-02-12T23:20:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172033.88 epoch sec; seqNo:2896; 4056748.60 north; 590800.77 east 2021-02-12T23:20:34Z- AHRS_M2 msg: 1613172034.26 epoch sec; seqNo:2870 2021-02-12T23:20:34Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.20 psi 2021-02-12T23:20:34Z- handleDepth()-Depth_Keller msg: 1613172034277 epoch sec; seqNo:2897; depth 23.15 2021-02-12T23:20:34Z- handleDvl() - RDI_Pathfinder msg: 1613172034.29 epoch sec; seqNo:1248 2021-02-12T23:20:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.46 , 90.34 , 96.82 , 64.55 2021-02-12T23:20:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.01 2021-02-12T23:20:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172034.30 epoch sec; seqNo:2897; 4056748.98 north; 590800.77 east 2021-02-12T23:20:34Z- AHRS_M2 msg: 1613172034.67 epoch sec; seqNo:2871 2021-02-12T23:20:34Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.20 psi 2021-02-12T23:20:34Z- handleDepth()-Depth_Keller msg: 1613172034672 epoch sec; seqNo:2898; depth 23.05 2021-02-12T23:20:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:34Z- handleDvl() - RDI_Pathfinder msg: 1613172034.67 epoch sec; seqNo:1249 2021-02-12T23:20:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.01 2021-02-12T23:20:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172034.69 epoch sec; seqNo:2898; 4056749.37 north; 590801.98 east 2021-02-12T23:20:35Z- AHRS_M2 msg: 1613172035.07 epoch sec; seqNo:2872 2021-02-12T23:20:35Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.21 psi 2021-02-12T23:20:35Z- handleDepth()-Depth_Keller msg: 1613172035081 epoch sec; seqNo:2899; depth 22.93 2021-02-12T23:20:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172035.09 epoch sec; seqNo:2899; 4056749.37 north; 590801.98 east 2021-02-12T23:20:35Z- LcmTrn::handleMessages() - handling 42 messages took 0.62 ms and 130 calls... 2021-02-12T23:20:35Z- waiting for 1613172033.25, time is 1613172035.09 2021-02-12T23:20:35Z- LcmTrn::updateTrn() >>>> pose time 1613172035.09, meas time 1613172034.67 <<<< 2021-02-12T23:20:35Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172034.67, ping # 0. 2021-02-12T23:20:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172034.67 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:20:35Z- TerrainNav::motionUpdate() - currEstimate = (4056749, 590801.98, 22.93) 2021-02-12T23:20:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:20:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:20:35Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:20:35Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:20:35Z- TNavBF::Projecting Measurements 2021-02-12T23:20:35Z- TNavFilter::Measurement from time = 1613172034.67 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:20:35Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:20:35Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:20:35Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:20:35Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:20:35Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:20:35Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:20:35Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:20:35Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 775.8 EastVariance: 833.1 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-12T23:20:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:20:35Z- TNavBF::mmse selected from filter -1 2021-02-12T23:20:35Z- variances([0], [2], and psi_berg) = 775.795875 833.061811 0.000000 2021-02-12T23:20:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:20:35Z- LcmTrn::publishEstimates() - MLE : 4056726.87 590755.48 22.93 2021-02-12T23:20:35Z- LcmTrn::publishEstimates() - MMSE : 4056749.37 590801.98 22.93 2021-02-12T23:20:35Z- LcmTrn::publishEstimates() - COVAR: 775.80 833.06 1.56 0.00 2021-02-12T23:20:35Z- AHRS_M2 msg: 1613172035.48 epoch sec; seqNo:2873 2021-02-12T23:20:35Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.21 psi 2021-02-12T23:20:35Z- handleDepth()-Depth_Keller msg: 1613172035480 epoch sec; seqNo:2900; depth 22.82 2021-02-12T23:20:35Z- handleDvl() - RDI_Pathfinder msg: 1613172035.49 epoch sec; seqNo:1250 2021-02-12T23:20:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.46 , 91.42 , 97.91 , 64.55 2021-02-12T23:20:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , 0.05 2021-02-12T23:20:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172035.50 epoch sec; seqNo:2900; 4056749.75 north; 590801.98 east 2021-02-12T23:20:35Z- AHRS_M2 msg: 1613172035.88 epoch sec; seqNo:2874 2021-02-12T23:20:35Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.21 psi 2021-02-12T23:20:35Z- handleDepth()-Depth_Keller msg: 1613172035886 epoch sec; seqNo:2901; depth 22.68 2021-02-12T23:20:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:35Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:35Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:35Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:35Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:35Z- handleDvl() - RDI_Pathfinder msg: 1613172035.89 epoch sec; seqNo:1251 2021-02-12T23:20:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , 0.05 2021-02-12T23:20:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172035.90 epoch sec; seqNo:2901; 4056750.13 north; 590801.98 east 2021-02-12T23:20:36Z- AHRS_M2 msg: 1613172036.28 epoch sec; seqNo:2875 2021-02-12T23:20:36Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.22 psi 2021-02-12T23:20:36Z- handleDepth()-Depth_Keller msg: 1613172036288 epoch sec; seqNo:2902; depth 22.57 2021-02-12T23:20:36Z- handleDvl() - RDI_Pathfinder msg: 1613172036.31 epoch sec; seqNo:1252 2021-02-12T23:20:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.86 , 91.35 , 96.76 , 65.37 2021-02-12T23:20:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , 0.08 2021-02-12T23:20:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172036.32 epoch sec; seqNo:2902; 4056750.51 north; 590801.97 east 2021-02-12T23:20:36Z- AHRS_M2 msg: 1613172036.69 epoch sec; seqNo:2876 2021-02-12T23:20:36Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.22 psi 2021-02-12T23:20:36Z- handleDepth()-Depth_Keller msg: 1613172036697 epoch sec; seqNo:2903; depth 22.45 2021-02-12T23:20:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172036.71 epoch sec; seqNo:2903; 4056750.51 north; 590801.97 east 2021-02-12T23:20:37Z- AHRS_M2 msg: 1613172037.09 epoch sec; seqNo:2877 2021-02-12T23:20:37Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.21 psi 2021-02-12T23:20:37Z- handleDepth()-Depth_Keller msg: 1613172037099 epoch sec; seqNo:2904; depth 22.34 2021-02-12T23:20:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:37Z- handleDvl() - RDI_Pathfinder msg: 1613172037.11 epoch sec; seqNo:1253 2021-02-12T23:20:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , 0.08 2021-02-12T23:20:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172037.11 epoch sec; seqNo:2904; 4056750.90 north; 590803.19 east 2021-02-12T23:20:37Z- AHRS_M2 msg: 1613172037.50 epoch sec; seqNo:2878 2021-02-12T23:20:37Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.20 psi 2021-02-12T23:20:37Z- handleDepth()-Depth_Keller msg: 1613172037514 epoch sec; seqNo:2905; depth 22.23 2021-02-12T23:20:37Z- handleDvl() - RDI_Pathfinder msg: 1613172037.53 epoch sec; seqNo:1254 2021-02-12T23:20:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.27 , 92.43 , 96.76 , 65.37 2021-02-12T23:20:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.08 , 0.07 2021-02-12T23:20:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172037.54 epoch sec; seqNo:2905; 4056751.28 north; 590803.18 east 2021-02-12T23:20:37Z- AHRS_M2 msg: 1613172037.90 epoch sec; seqNo:2879 2021-02-12T23:20:37Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.19 psi 2021-02-12T23:20:37Z- handleDepth()-Depth_Keller msg: 1613172037907 epoch sec; seqNo:2906; depth 22.11 2021-02-12T23:20:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172037.92 epoch sec; seqNo:2906; 4056751.28 north; 590803.18 east 2021-02-12T23:20:38Z- AHRS_M2 msg: 1613172038.31 epoch sec; seqNo:2880 2021-02-12T23:20:38Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-12T23:20:38Z- handleDepth()-Depth_Keller msg: 1613172038312 epoch sec; seqNo:2907; depth 22.00 2021-02-12T23:20:38Z- handleDvl() - RDI_Pathfinder msg: 1613172038.36 epoch sec; seqNo:1255 2021-02-12T23:20:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.68 , 91.35 , 96.76 , 65.78 2021-02-12T23:20:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.08 , -0.01 2021-02-12T23:20:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172038.37 epoch sec; seqNo:2907; 4056751.66 north; 590803.18 east 2021-02-12T23:20:38Z- AHRS_M2 msg: 1613172038.71 epoch sec; seqNo:2881 2021-02-12T23:20:38Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-12T23:20:38Z- handleDepth()-Depth_Keller msg: 1613172038721 epoch sec; seqNo:2908; depth 21.87 2021-02-12T23:20:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172038.73 epoch sec; seqNo:2908; 4056752.04 north; 590803.18 east 2021-02-12T23:20:39Z- AHRS_M2 msg: 1613172039.12 epoch sec; seqNo:2882 2021-02-12T23:20:39Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-12T23:20:39Z- handleDepth()-Depth_Keller msg: 1613172039137 epoch sec; seqNo:2909; depth 21.77 2021-02-12T23:20:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172039.15 epoch sec; seqNo:2909; 4056752.04 north; 590803.18 east 2021-02-12T23:20:39Z- AHRS_M2 msg: 1613172039.52 epoch sec; seqNo:2883 2021-02-12T23:20:39Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-12T23:20:39Z- handleDepth()-Depth_Keller msg: 1613172039520 epoch sec; seqNo:2910; depth 21.63 2021-02-12T23:20:39Z- handleDvl() - RDI_Pathfinder msg: 1613172039.53 epoch sec; seqNo:1256 2021-02-12T23:20:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.09 , 90.27 , 100.00 , 65.37 2021-02-12T23:20:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.06 , -0.07 2021-02-12T23:20:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172039.56 epoch sec; seqNo:2910; 4056752.43 north; 590804.39 east 2021-02-12T23:20:39Z- AHRS_M2 msg: 1613172039.92 epoch sec; seqNo:2884 2021-02-12T23:20:39Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.16 psi 2021-02-12T23:20:39Z- handleDepth()-Depth_Keller msg: 1613172039929 epoch sec; seqNo:2911; depth 21.53 2021-02-12T23:20:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172039.94 epoch sec; seqNo:2911; 4056752.81 north; 590804.39 east 2021-02-12T23:20:40Z- AHRS_M2 msg: 1613172040.33 epoch sec; seqNo:2885 2021-02-12T23:20:40Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:20:40Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-12T23:20:40Z- waiting for 1613172038.09, time is 1613172040.33 2021-02-12T23:20:40Z- LcmTrn::updateTrn() >>>> pose time 1613172040.33, meas time 1613172039.53 <<<< 2021-02-12T23:20:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172039.53 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:20:40Z- TerrainNav::motionUpdate() - currEstimate = (4056753, 590804.39, 21.53) 2021-02-12T23:20:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:20:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:20:40Z- TNavBF::Projecting Measurements 2021-02-12T23:20:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:20:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:20:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:20:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:20:40Z- TNavFilter::4 good beams. 2021-02-12T23:20:40Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:20:40Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:20:40Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:20:40Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:20:40Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:20:40Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:20:40Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:20:40Z- TNavBF::mmse selected from filter -1 2021-02-12T23:20:40Z- TerrainMapOctree: (vehN, vehE ) = (4056752.29, 590804.02). 2021-02-12T23:20:40Z- TerrainMapOctree: Min Distance = 5666.05. 2021-02-12T23:20:40Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:20:40Z- There is no specific good beam that all particles have in common. 2021-02-12T23:20:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:20:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.74 K: 0.150 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-12T23:20:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:20:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.48 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 13.04 2021-02-12T23:20:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:20:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 16.00 2021-02-12T23:20:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:20:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.127 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-12T23:20:40Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:20:40Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:20:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:20:40Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:20:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:20:40Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:20:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:20:40Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:20:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:20:40Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:20:40Z- Filter 0 No beams used in subcloud update 2021-02-12T23:20:40Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:20:40Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:20:40Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 775.7 EastVariance: 833.1 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:20:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172039.53 sec. 2021-02-12T23:20:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:20:40Z- TNavBF::mmse selected from filter -1 2021-02-12T23:20:40Z- variances([0], [2], and psi_berg) = 775.605245 833.061555 0.000000 2021-02-12T23:20:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:20:40Z- LcmTrn::publishEstimates() - MLE : 4056730.31 590757.89 21.53 2021-02-12T23:20:40Z- LcmTrn::publishEstimates() - MMSE : 4056752.81 590804.39 21.53 2021-02-12T23:20:40Z- LcmTrn::publishEstimates() - COVAR: 775.61 833.06 1.12 0.00 2021-02-12T23:20:40Z- handleDepth()-Depth_Keller msg: 1613172040330 epoch sec; seqNo:2912; depth 21.41 2021-02-12T23:20:40Z- handleDvl() - RDI_Pathfinder msg: 1613172040.34 epoch sec; seqNo:1257 2021-02-12T23:20:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.50 , 90.20 , 96.69 , 65.78 2021-02-12T23:20:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.08 2021-02-12T23:20:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172040.35 epoch sec; seqNo:2912; 4056752.81 north; 590804.39 east 2021-02-12T23:20:40Z- AHRS_M2 msg: 1613172040.73 epoch sec; seqNo:2886 2021-02-12T23:20:40Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:20:40Z- handleDepth()-Depth_Keller msg: 1613172040732 epoch sec; seqNo:2913; depth 21.29 2021-02-12T23:20:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:40Z- handleDvl() - RDI_Pathfinder msg: 1613172040.73 epoch sec; seqNo:1258 2021-02-12T23:20:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.08 2021-02-12T23:20:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172040.76 epoch sec; seqNo:2913; 4056753.19 north; 590804.39 east 2021-02-12T23:20:41Z- AHRS_M2 msg: 1613172041.13 epoch sec; seqNo:2887 2021-02-12T23:20:41Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-12T23:20:41Z- handleDepth()-Depth_Keller msg: 1613172041139 epoch sec; seqNo:2914; depth 21.16 2021-02-12T23:20:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172041.15 epoch sec; seqNo:2914; 4056753.57 north; 590804.38 east 2021-02-12T23:20:41Z- AHRS_M2 msg: 1613172041.54 epoch sec; seqNo:2888 2021-02-12T23:20:41Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-12T23:20:41Z- handleDepth()-Depth_Keller msg: 1613172041556 epoch sec; seqNo:2915; depth 21.06 2021-02-12T23:20:41Z- handleDvl() - RDI_Pathfinder msg: 1613172041.58 epoch sec; seqNo:1259 2021-02-12T23:20:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.91 , 90.27 , 96.76 , 65.37 2021-02-12T23:20:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.13 , -0.01 2021-02-12T23:20:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172041.61 epoch sec; seqNo:2915; 4056753.94 north; 590804.38 east 2021-02-12T23:20:41Z- AHRS_M2 msg: 1613172041.94 epoch sec; seqNo:2889 2021-02-12T23:20:41Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-12T23:20:41Z- handleDepth()-Depth_Keller msg: 1613172041949 epoch sec; seqNo:2916; depth 20.93 2021-02-12T23:20:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:41Z- handleDvl() - RDI_Pathfinder msg: 1613172041.95 epoch sec; seqNo:1260 2021-02-12T23:20:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.13 , -0.01 2021-02-12T23:20:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172041.97 epoch sec; seqNo:2916; 4056753.94 north; 590804.38 east 2021-02-12T23:20:42Z- AHRS_M2 msg: 1613172042.34 epoch sec; seqNo:2890 2021-02-12T23:20:42Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-12T23:20:42Z- handleDepth()-Depth_Keller msg: 1613172042348 epoch sec; seqNo:2917; depth 20.85 2021-02-12T23:20:42Z- handleDvl() - RDI_Pathfinder msg: 1613172042.36 epoch sec; seqNo:1261 2021-02-12T23:20:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.91 , 90.27 , 100.00 , 66.19 2021-02-12T23:20:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.11 , 0.08 2021-02-12T23:20:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172042.37 epoch sec; seqNo:2917; 4056754.34 north; 590805.59 east 2021-02-12T23:20:42Z- AHRS_M2 msg: 1613172042.75 epoch sec; seqNo:2891 2021-02-12T23:20:42Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.18 psi 2021-02-12T23:20:42Z- handleDepth()-Depth_Keller msg: 1613172042754 epoch sec; seqNo:2918; depth 20.72 2021-02-12T23:20:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172042.77 epoch sec; seqNo:2918; 4056754.71 north; 590805.59 east 2021-02-12T23:20:43Z- AHRS_M2 msg: 1613172043.15 epoch sec; seqNo:2892 2021-02-12T23:20:43Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.18 psi 2021-02-12T23:20:43Z- handleDepth()-Depth_Keller msg: 1613172043162 epoch sec; seqNo:2919; depth 20.61 2021-02-12T23:20:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:43Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:43Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:43Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:43Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:43Z- handleDvl() - RDI_Pathfinder msg: 1613172043.16 epoch sec; seqNo:1262 2021-02-12T23:20:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.11 , 0.08 2021-02-12T23:20:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172043.17 epoch sec; seqNo:2919; 4056755.09 north; 590805.59 east 2021-02-12T23:20:43Z- AHRS_M2 msg: 1613172043.56 epoch sec; seqNo:2893 2021-02-12T23:20:43Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.18 psi 2021-02-12T23:20:43Z- handleDepth()-Depth_Keller msg: 1613172043565 epoch sec; seqNo:2920; depth 20.50 2021-02-12T23:20:43Z- handleDvl() - RDI_Pathfinder msg: 1613172043.58 epoch sec; seqNo:1263 2021-02-12T23:20:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.32 , 90.20 , 96.69 , 67.00 2021-02-12T23:20:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , 0.13 2021-02-12T23:20:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172043.59 epoch sec; seqNo:2920; 4056755.47 north; 590805.58 east 2021-02-12T23:20:43Z- AHRS_M2 msg: 1613172043.96 epoch sec; seqNo:2894 2021-02-12T23:20:43Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.17 psi 2021-02-12T23:20:43Z- handleDepth()-Depth_Keller msg: 1613172043966 epoch sec; seqNo:2921; depth 20.38 2021-02-12T23:20:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172043.98 epoch sec; seqNo:2921; 4056755.47 north; 590805.58 east 2021-02-12T23:20:44Z- AHRS_M2 msg: 1613172044.36 epoch sec; seqNo:2895 2021-02-12T23:20:44Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.17 psi 2021-02-12T23:20:44Z- handleDepth()-Depth_Keller msg: 1613172044368 epoch sec; seqNo:2922; depth 20.29 2021-02-12T23:20:44Z- handleDvl() - RDI_Pathfinder msg: 1613172044.39 epoch sec; seqNo:1264 2021-02-12T23:20:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.72 , 92.36 , 96.69 , 66.60 2021-02-12T23:20:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.05 , 0.09 2021-02-12T23:20:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172044.40 epoch sec; seqNo:2922; 4056755.85 north; 590805.58 east 2021-02-12T23:20:44Z- AHRS_M2 msg: 1613172044.77 epoch sec; seqNo:2896 2021-02-12T23:20:44Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.17 psi 2021-02-12T23:20:44Z- handleDepth()-Depth_Keller msg: 1613172044777 epoch sec; seqNo:2923; depth 20.18 2021-02-12T23:20:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172044.79 epoch sec; seqNo:2923; 4056756.24 north; 590806.80 east 2021-02-12T23:20:45Z- AHRS_M2 msg: 1613172045.17 epoch sec; seqNo:2897 2021-02-12T23:20:45Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.16 psi 2021-02-12T23:20:45Z- handleDepth()-Depth_Keller msg: 1613172045183 epoch sec; seqNo:2924; depth 20.08 2021-02-12T23:20:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172045.21 epoch sec; seqNo:2924; 4056756.62 north; 590806.79 east 2021-02-12T23:20:45Z- LcmTrn::handleMessages() - handling 46 messages took 0.67 ms and 133 calls... 2021-02-12T23:20:45Z- waiting for 1613172043.33, time is 1613172045.21 2021-02-12T23:20:45Z- LcmTrn::updateTrn() >>>> pose time 1613172045.21, meas time 1613172044.39 <<<< 2021-02-12T23:20:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172044.39 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:20:45Z- TerrainNav::motionUpdate() - currEstimate = (4056757, 590806.79, 20.08) 2021-02-12T23:20:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:20:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:20:45Z- TNavBF::Projecting Measurements 2021-02-12T23:20:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:20:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:20:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:20:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:20:45Z- TNavFilter::4 good beams. 2021-02-12T23:20:45Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:20:45Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:20:45Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:20:45Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:20:45Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:20:45Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:20:45Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:20:45Z- TNavBF::mmse selected from filter -1 2021-02-12T23:20:45Z- TerrainMapOctree: (vehN, vehE ) = (4056755.98, 590806.39). 2021-02-12T23:20:45Z- TerrainMapOctree: Min Distance = 5661.69. 2021-02-12T23:20:45Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:20:45Z- There is no specific good beam that all particles have in common. 2021-02-12T23:20:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:20:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.114 ExpectedMeasDiff: nan currMeas.covariance: 8.46 2021-02-12T23:20:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:20:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 13.65 2021-02-12T23:20:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:20:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 14.96 2021-02-12T23:20:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:20:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 7.10 2021-02-12T23:20:45Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:20:45Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:20:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:20:45Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:20:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:20:45Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:20:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:20:45Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:20:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:20:45Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:20:45Z- Filter 0 No beams used in subcloud update 2021-02-12T23:20:45Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:20:45Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:20:45Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 775.5 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:20:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172044.39 sec. 2021-02-12T23:20:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:20:45Z- TNavBF::mmse selected from filter -1 2021-02-12T23:20:45Z- variances([0], [2], and psi_berg) = 775.406481 833.061289 0.000000 2021-02-12T23:20:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:20:45Z- LcmTrn::publishEstimates() - MLE : 4056734.12 590758.29 20.08 2021-02-12T23:20:45Z- LcmTrn::publishEstimates() - MMSE : 4056756.62 590806.79 20.08 2021-02-12T23:20:45Z- LcmTrn::publishEstimates() - COVAR: 775.41 833.06 0.95 0.00 2021-02-12T23:20:45Z- AHRS_M2 msg: 1613172045.58 epoch sec; seqNo:2898 2021-02-12T23:20:45Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.16 psi 2021-02-12T23:20:45Z- handleDepth()-Depth_Keller msg: 1613172045585 epoch sec; seqNo:2925; depth 19.95 2021-02-12T23:20:45Z- handleDvl() - RDI_Pathfinder msg: 1613172045.60 epoch sec; seqNo:1265 2021-02-12T23:20:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.13 , 90.20 , 96.69 , 67.00 2021-02-12T23:20:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.07 , -0.02 2021-02-12T23:20:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172045.61 epoch sec; seqNo:2925; 4056756.62 north; 590806.79 east 2021-02-12T23:20:45Z- AHRS_M2 msg: 1613172045.98 epoch sec; seqNo:2899 2021-02-12T23:20:45Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.16 psi 2021-02-12T23:20:45Z- handleDepth()-Depth_Keller msg: 1613172045983 epoch sec; seqNo:2926; depth 19.85 2021-02-12T23:20:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172046.01 epoch sec; seqNo:2926; 4056757.00 north; 590806.79 east 2021-02-12T23:20:46Z- AHRS_M2 msg: 1613172046.38 epoch sec; seqNo:2900 2021-02-12T23:20:46Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.16 psi 2021-02-12T23:20:46Z- handleDepth()-Depth_Keller msg: 1613172046388 epoch sec; seqNo:2927; depth 19.76 2021-02-12T23:20:46Z- handleDvl() - RDI_Pathfinder msg: 1613172046.40 epoch sec; seqNo:1266 2021-02-12T23:20:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.32 , 91.28 , 96.69 , 67.00 2021-02-12T23:20:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.00 , -0.10 2021-02-12T23:20:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172046.42 epoch sec; seqNo:2927; 4056757.38 north; 590806.78 east 2021-02-12T23:20:46Z- AHRS_M2 msg: 1613172046.79 epoch sec; seqNo:2901 2021-02-12T23:20:46Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.15 psi 2021-02-12T23:20:46Z- handleDepth()-Depth_Keller msg: 1613172046792 epoch sec; seqNo:2928; depth 19.64 2021-02-12T23:20:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:46Z- handleDvl() - RDI_Pathfinder msg: 1613172046.79 epoch sec; seqNo:1267 2021-02-12T23:20:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.00 , -0.10 2021-02-12T23:20:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172046.81 epoch sec; seqNo:2928; 4056757.38 north; 590806.78 east 2021-02-12T23:20:47Z- AHRS_M2 msg: 1613172047.21 epoch sec; seqNo:2902 2021-02-12T23:20:47Z- AHRS_M2 msg: 0.63 phi; 0.29 theta; -0.15 psi 2021-02-12T23:20:47Z- handleDepth()-Depth_Keller msg: 1613172047221 epoch sec; seqNo:2929; depth 19.56 2021-02-12T23:20:47Z- handleDvl() - RDI_Pathfinder msg: 1613172047.23 epoch sec; seqNo:1268 2021-02-12T23:20:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.72 , 90.20 , 96.69 , 67.82 2021-02-12T23:20:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.12 , -0.07 2021-02-12T23:20:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172047.26 epoch sec; seqNo:2929; 4056757.76 north; 590806.78 east 2021-02-12T23:20:47Z- AHRS_M2 msg: 1613172047.60 epoch sec; seqNo:2903 2021-02-12T23:20:47Z- AHRS_M2 msg: 0.63 phi; 0.29 theta; -0.15 psi 2021-02-12T23:20:47Z- handleDepth()-Depth_Keller msg: 1613172047600 epoch sec; seqNo:2930; depth 19.43 2021-02-12T23:20:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172047.62 epoch sec; seqNo:2930; 4056758.15 north; 590808.00 east 2021-02-12T23:20:48Z- AHRS_M2 msg: 1613172048.00 epoch sec; seqNo:2904 2021-02-12T23:20:48Z- AHRS_M2 msg: 0.63 phi; 0.29 theta; -0.15 psi 2021-02-12T23:20:48Z- handleDepth()-Depth_Keller msg: 1613172048008 epoch sec; seqNo:2931; depth 19.32 2021-02-12T23:20:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:48Z- handleDvl() - RDI_Pathfinder msg: 1613172048.01 epoch sec; seqNo:1269 2021-02-12T23:20:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.12 , -0.07 2021-02-12T23:20:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172048.02 epoch sec; seqNo:2931; 4056758.53 north; 590807.99 east 2021-02-12T23:20:48Z- AHRS_M2 msg: 1613172048.40 epoch sec; seqNo:2905 2021-02-12T23:20:48Z- AHRS_M2 msg: 0.63 phi; 0.29 theta; -0.15 psi 2021-02-12T23:20:48Z- handleDepth()-Depth_Keller msg: 1613172048408 epoch sec; seqNo:2932; depth 19.22 2021-02-12T23:20:48Z- handleDvl() - RDI_Pathfinder msg: 1613172048.42 epoch sec; seqNo:1270 2021-02-12T23:20:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.13 , 91.28 , 94.53 , 67.82 2021-02-12T23:20:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.19 , 0.08 2021-02-12T23:20:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172048.43 epoch sec; seqNo:2932; 4056758.53 north; 590807.99 east 2021-02-12T23:20:48Z- AHRS_M2 msg: 1613172048.81 epoch sec; seqNo:2906 2021-02-12T23:20:48Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.15 psi 2021-02-12T23:20:48Z- handleDepth()-Depth_Keller msg: 1613172048814 epoch sec; seqNo:2933; depth 19.10 2021-02-12T23:20:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172048.83 epoch sec; seqNo:2933; 4056758.91 north; 590807.99 east 2021-02-12T23:20:49Z- AHRS_M2 msg: 1613172049.22 epoch sec; seqNo:2907 2021-02-12T23:20:49Z- AHRS_M2 msg: 0.61 phi; 0.29 theta; -0.15 psi 2021-02-12T23:20:49Z- handleDepth()-Depth_Keller msg: 1613172049232 epoch sec; seqNo:2934; depth 19.00 2021-02-12T23:20:49Z- handleDvl() - RDI_Pathfinder msg: 1613172049.24 epoch sec; seqNo:1271 2021-02-12T23:20:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.95 , 91.28 , 96.69 , 67.00 2021-02-12T23:20:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.09 , 0.23 2021-02-12T23:20:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172049.25 epoch sec; seqNo:2934; 4056759.29 north; 590807.98 east 2021-02-12T23:20:49Z- AHRS_M2 msg: 1613172049.62 epoch sec; seqNo:2908 2021-02-12T23:20:49Z- AHRS_M2 msg: 0.60 phi; 0.29 theta; -0.15 psi 2021-02-12T23:20:49Z- handleDepth()-Depth_Keller msg: 1613172049625 epoch sec; seqNo:2935; depth 18.89 2021-02-12T23:20:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172049.64 epoch sec; seqNo:2935; 4056759.67 north; 590807.98 east 2021-02-12T23:20:50Z- AHRS_M2 msg: 1613172050.03 epoch sec; seqNo:2909 2021-02-12T23:20:50Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.15 psi 2021-02-12T23:20:50Z- handleDepth()-Depth_Keller msg: 1613172050033 epoch sec; seqNo:2936; depth 18.79 2021-02-12T23:20:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172050.04 epoch sec; seqNo:2936; 4056760.04 north; 590807.98 east 2021-02-12T23:20:50Z- AHRS_M2 msg: 1613172050.42 epoch sec; seqNo:2910 2021-02-12T23:20:50Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.15 psi 2021-02-12T23:20:50Z- handleDepth()-Depth_Keller msg: 1613172050428 epoch sec; seqNo:2937; depth 18.68 2021-02-12T23:20:50Z- handleDvl() - RDI_Pathfinder msg: 1613172050.45 epoch sec; seqNo:1272 2021-02-12T23:20:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.36 , 90.20 , 96.69 , 68.64 2021-02-12T23:20:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , 0.24 2021-02-12T23:20:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172050.46 epoch sec; seqNo:2937; 4056760.06 north; 590809.20 east 2021-02-12T23:20:50Z- LcmTrn::handleMessages() - handling 47 messages took 0.64 ms and 134 calls... 2021-02-12T23:20:50Z- waiting for 1613172048.21, time is 1613172050.46 2021-02-12T23:20:50Z- LcmTrn::updateTrn() equating pose 1613172050.46 and meas times 1613172050.46 2021-02-12T23:20:50Z- LcmTrn::updateTrn() >>>> pose time 1613172050.46, meas time 1613172050.46 <<<< 2021-02-12T23:20:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172050.46 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:20:50Z- TerrainNav::motionUpdate() - currEstimate = (4056760, 590809.20, 18.68) 2021-02-12T23:20:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:20:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:20:50Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172050.46 sec.; No relevant navigation data available 2021-02-12T23:20:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:20:50Z- TNavBF::mmse selected from filter -1 2021-02-12T23:20:50Z- variances([0], [2], and psi_berg) = 775.261830 833.061096 0.000000 2021-02-12T23:20:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:20:50Z- LcmTrn::publishEstimates() - MLE : 4056737.56 590858.70 18.68 2021-02-12T23:20:50Z- LcmTrn::publishEstimates() - MMSE : 4056760.06 590809.20 18.68 2021-02-12T23:20:50Z- LcmTrn::publishEstimates() - COVAR: 775.26 833.06 1.16 0.00 2021-02-12T23:20:50Z- AHRS_M2 msg: 1613172050.83 epoch sec; seqNo:2911 2021-02-12T23:20:50Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.15 psi 2021-02-12T23:20:50Z- handleDepth()-Depth_Keller msg: 1613172050837 epoch sec; seqNo:2938; depth 18.57 2021-02-12T23:20:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172050.86 epoch sec; seqNo:2938; 4056760.44 north; 590809.19 east 2021-02-12T23:20:51Z- AHRS_M2 msg: 1613172051.23 epoch sec; seqNo:2912 2021-02-12T23:20:51Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.15 psi 2021-02-12T23:20:51Z- handleDepth()-Depth_Keller msg: 1613172051241 epoch sec; seqNo:2939; depth 18.45 2021-02-12T23:20:51Z- handleDvl() - RDI_Pathfinder msg: 1613172051.25 epoch sec; seqNo:1273 2021-02-12T23:20:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.36 , 89.86 , 97.16 , 68.64 2021-02-12T23:20:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.19 , 0.08 2021-02-12T23:20:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172051.28 epoch sec; seqNo:2939; 4056760.81 north; 590809.19 east 2021-02-12T23:20:51Z- AHRS_M2 msg: 1613172051.64 epoch sec; seqNo:2913 2021-02-12T23:20:51Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-12T23:20:51Z- handleDepth()-Depth_Keller msg: 1613172051641 epoch sec; seqNo:2940; depth 18.37 2021-02-12T23:20:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:51Z- handleDvl() - RDI_Pathfinder msg: 1613172051.64 epoch sec; seqNo:1274 2021-02-12T23:20:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.19 , 0.08 2021-02-12T23:20:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172051.65 epoch sec; seqNo:2940; 4056761.19 north; 590809.19 east 2021-02-12T23:20:52Z- AHRS_M2 msg: 1613172052.04 epoch sec; seqNo:2914 2021-02-12T23:20:52Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-12T23:20:52Z- handleDepth()-Depth_Keller msg: 1613172052049 epoch sec; seqNo:2941; depth 18.25 2021-02-12T23:20:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172052.07 epoch sec; seqNo:2941; 4056761.19 north; 590809.19 east 2021-02-12T23:20:52Z- AHRS_M2 msg: 1613172052.44 epoch sec; seqNo:2915 2021-02-12T23:20:52Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.15 psi 2021-02-12T23:20:52Z- handleDepth()-Depth_Keller msg: 1613172052448 epoch sec; seqNo:2942; depth 18.17 2021-02-12T23:20:52Z- handleDvl() - RDI_Pathfinder msg: 1613172052.46 epoch sec; seqNo:1275 2021-02-12T23:20:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.58 , 89.86 , 97.16 , 68.64 2021-02-12T23:20:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.17 , -0.14 2021-02-12T23:20:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172052.48 epoch sec; seqNo:2942; 4056761.59 north; 590810.40 east 2021-02-12T23:20:52Z- AHRS_M2 msg: 1613172052.85 epoch sec; seqNo:2916 2021-02-12T23:20:52Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-12T23:20:52Z- handleDepth()-Depth_Keller msg: 1613172052857 epoch sec; seqNo:2943; depth 18.05 2021-02-12T23:20:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:52Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:52Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:52Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:52Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:52Z- handleDvl() - RDI_Pathfinder msg: 1613172052.86 epoch sec; seqNo:1276 2021-02-12T23:20:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.17 , -0.14 2021-02-12T23:20:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172052.87 epoch sec; seqNo:2943; 4056761.96 north; 590810.40 east 2021-02-12T23:20:53Z- AHRS_M2 msg: 1613172053.25 epoch sec; seqNo:2917 2021-02-12T23:20:53Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-12T23:20:53Z- handleDepth()-Depth_Keller msg: 1613172053258 epoch sec; seqNo:2944; depth 17.93 2021-02-12T23:20:53Z- handleDvl() - RDI_Pathfinder msg: 1613172053.27 epoch sec; seqNo:1277 2021-02-12T23:20:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.18 , 93.51 , 97.16 , 69.05 2021-02-12T23:20:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , -0.25 2021-02-12T23:20:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172053.29 epoch sec; seqNo:2944; 4056761.96 north; 590810.40 east 2021-02-12T23:20:53Z- AHRS_M2 msg: 1613172053.66 epoch sec; seqNo:2918 2021-02-12T23:20:53Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-12T23:20:53Z- handleDepth()-Depth_Keller msg: 1613172053660 epoch sec; seqNo:2945; depth 17.82 2021-02-12T23:20:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172053.67 epoch sec; seqNo:2945; 4056762.34 north; 590810.39 east 2021-02-12T23:20:54Z- AHRS_M2 msg: 1613172054.06 epoch sec; seqNo:2919 2021-02-12T23:20:54Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-12T23:20:54Z- handleDepth()-Depth_Keller msg: 1613172054064 epoch sec; seqNo:2946; depth 17.71 2021-02-12T23:20:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:54Z- handleDvl() - RDI_Pathfinder msg: 1613172054.07 epoch sec; seqNo:1278 2021-02-12T23:20:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , -0.25 2021-02-12T23:20:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172054.08 epoch sec; seqNo:2946; 4056762.34 north; 590810.39 east 2021-02-12T23:20:54Z- AHRS_M2 msg: 1613172054.46 epoch sec; seqNo:2920 2021-02-12T23:20:54Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.17 psi 2021-02-12T23:20:54Z- handleDepth()-Depth_Keller msg: 1613172054473 epoch sec; seqNo:2947; depth 17.62 2021-02-12T23:20:54Z- handleDvl() - RDI_Pathfinder msg: 1613172054.48 epoch sec; seqNo:1279 2021-02-12T23:20:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.18 , 92.23 , 97.10 , 68.64 2021-02-12T23:20:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.14 , -0.19 2021-02-12T23:20:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172054.50 epoch sec; seqNo:2947; 4056762.72 north; 590810.39 east 2021-02-12T23:20:54Z- AHRS_M2 msg: 1613172054.88 epoch sec; seqNo:2921 2021-02-12T23:20:54Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.17 psi 2021-02-12T23:20:54Z- handleDepth()-Depth_Keller msg: 1613172054879 epoch sec; seqNo:2948; depth 17.49 2021-02-12T23:20:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172054.89 epoch sec; seqNo:2948; 4056763.10 north; 590810.39 east 2021-02-12T23:20:55Z- AHRS_M2 msg: 1613172055.27 epoch sec; seqNo:2922 2021-02-12T23:20:55Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.17 psi 2021-02-12T23:20:55Z- handleDepth()-Depth_Keller msg: 1613172055280 epoch sec; seqNo:2949; depth 17.37 2021-02-12T23:20:55Z- handleDvl() - RDI_Pathfinder msg: 1613172055.31 epoch sec; seqNo:1280 2021-02-12T23:20:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.18 , 93.44 , 97.10 , 69.86 2021-02-12T23:20:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.20 , -0.03 2021-02-12T23:20:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172055.33 epoch sec; seqNo:2949; 4056763.49 north; 590811.60 east 2021-02-12T23:20:55Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-12T23:20:55Z- waiting for 1613172053.46, time is 1613172055.33 2021-02-12T23:20:55Z- LcmTrn::updateTrn() equating pose 1613172055.33 and meas times 1613172055.33 2021-02-12T23:20:55Z- LcmTrn::updateTrn() >>>> pose time 1613172055.33, meas time 1613172055.33 <<<< 2021-02-12T23:20:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172055.33 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:20:55Z- TerrainNav::motionUpdate() - currEstimate = (4056763, 590811.60, 17.37) 2021-02-12T23:20:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:20:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:20:55Z- TNavBF::Projecting Measurements 2021-02-12T23:20:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:20:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:20:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:20:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:20:55Z- TNavFilter::4 good beams. 2021-02-12T23:20:55Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:20:55Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:20:55Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:20:55Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:20:55Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:20:55Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:20:55Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:20:55Z- TNavBF::mmse selected from filter -1 2021-02-12T23:20:55Z- TerrainMapOctree: (vehN, vehE ) = (4056763.49, 590811.60). 2021-02-12T23:20:55Z- TerrainMapOctree: Min Distance = 5652.55. 2021-02-12T23:20:55Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:20:55Z- There is no specific good beam that all particles have in common. 2021-02-12T23:20:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:20:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 9.04 2021-02-12T23:20:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:20:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 13.97 2021-02-12T23:20:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:20:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 15.08 2021-02-12T23:20:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:20:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 7.81 2021-02-12T23:20:55Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:20:55Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:20:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:20:55Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:20:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:20:55Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:20:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:20:55Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:20:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:20:55Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:20:55Z- Filter 0 No beams used in subcloud update 2021-02-12T23:20:55Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:20:55Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:20:55Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 775.1 EastVariance: 833.1 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:20:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172055.33 sec. 2021-02-12T23:20:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:20:55Z- TNavBF::mmse selected from filter -1 2021-02-12T23:20:55Z- variances([0], [2], and psi_berg) = 775.117521 833.060904 0.000000 2021-02-12T23:20:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:20:55Z- LcmTrn::publishEstimates() - MLE : 4056740.99 590861.10 17.37 2021-02-12T23:20:55Z- LcmTrn::publishEstimates() - MMSE : 4056763.49 590811.60 17.37 2021-02-12T23:20:55Z- LcmTrn::publishEstimates() - COVAR: 775.12 833.06 0.95 0.00 2021-02-12T23:20:55Z- AHRS_M2 msg: 1613172055.67 epoch sec; seqNo:2923 2021-02-12T23:20:55Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.17 psi 2021-02-12T23:20:55Z- handleDepth()-Depth_Keller msg: 1613172055685 epoch sec; seqNo:2950; depth 17.26 2021-02-12T23:20:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172055.70 epoch sec; seqNo:2950; 4056763.87 north; 590811.60 east 2021-02-12T23:20:56Z- AHRS_M2 msg: 1613172056.09 epoch sec; seqNo:2924 2021-02-12T23:20:56Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:20:56Z- handleDepth()-Depth_Keller msg: 1613172056089 epoch sec; seqNo:2951; depth 17.16 2021-02-12T23:20:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172056.10 epoch sec; seqNo:2951; 4056763.87 north; 590811.60 east 2021-02-12T23:20:56Z- AHRS_M2 msg: 1613172056.48 epoch sec; seqNo:2925 2021-02-12T23:20:56Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:20:56Z- handleDepth()-Depth_Keller msg: 1613172056494 epoch sec; seqNo:2952; depth 17.04 2021-02-12T23:20:56Z- handleDvl() - RDI_Pathfinder msg: 1613172056.51 epoch sec; seqNo:1281 2021-02-12T23:20:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.58 , 94.59 , 97.03 , 69.05 2021-02-12T23:20:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.17 , 0.12 2021-02-12T23:20:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172056.52 epoch sec; seqNo:2952; 4056764.25 north; 590811.60 east 2021-02-12T23:20:56Z- AHRS_M2 msg: 1613172056.89 epoch sec; seqNo:2926 2021-02-12T23:20:56Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.15 psi 2021-02-12T23:20:56Z- handleDepth()-Depth_Keller msg: 1613172056891 epoch sec; seqNo:2953; depth 16.94 2021-02-12T23:20:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172056.90 epoch sec; seqNo:2953; 4056764.63 north; 590811.59 east 2021-02-12T23:20:57Z- AHRS_M2 msg: 1613172057.29 epoch sec; seqNo:2927 2021-02-12T23:20:57Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-12T23:20:57Z- handleDepth()-Depth_Keller msg: 1613172057309 epoch sec; seqNo:2954; depth 16.82 2021-02-12T23:20:57Z- handleDvl() - RDI_Pathfinder msg: 1613172057.32 epoch sec; seqNo:1282 2021-02-12T23:20:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.40 , 93.38 , 103.11 , 69.86 2021-02-12T23:20:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , 0.18 2021-02-12T23:20:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172057.33 epoch sec; seqNo:2954; 4056765.01 north; 590811.59 east 2021-02-12T23:20:57Z- AHRS_M2 msg: 1613172057.70 epoch sec; seqNo:2928 2021-02-12T23:20:57Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.14 psi 2021-02-12T23:20:57Z- handleDepth()-Depth_Keller msg: 1613172057708 epoch sec; seqNo:2955; depth 16.70 2021-02-12T23:20:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:57Z- handleDvl() - RDI_Pathfinder msg: 1613172057.72 epoch sec; seqNo:1283 2021-02-12T23:20:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , 0.18 2021-02-12T23:20:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172057.74 epoch sec; seqNo:2955; 4056765.39 north; 590811.58 east 2021-02-12T23:20:58Z- AHRS_M2 msg: 1613172058.10 epoch sec; seqNo:2929 2021-02-12T23:20:58Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.14 psi 2021-02-12T23:20:58Z- handleDepth()-Depth_Keller msg: 1613172058104 epoch sec; seqNo:2956; depth 16.58 2021-02-12T23:20:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172058.12 epoch sec; seqNo:2956; 4056765.39 north; 590811.58 east 2021-02-12T23:20:58Z- AHRS_M2 msg: 1613172058.51 epoch sec; seqNo:2930 2021-02-12T23:20:58Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.14 psi 2021-02-12T23:20:58Z- handleDepth()-Depth_Keller msg: 1613172058517 epoch sec; seqNo:2957; depth 16.49 2021-02-12T23:20:58Z- handleDvl() - RDI_Pathfinder msg: 1613172058.53 epoch sec; seqNo:1284 2021-02-12T23:20:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.63 , 90.81 , 104.19 , 69.46 2021-02-12T23:20:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , 0.17 2021-02-12T23:20:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172058.54 epoch sec; seqNo:2957; 4056765.78 north; 590812.80 east 2021-02-12T23:20:58Z- AHRS_M2 msg: 1613172058.91 epoch sec; seqNo:2931 2021-02-12T23:20:58Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-12T23:20:58Z- handleDepth()-Depth_Keller msg: 1613172058912 epoch sec; seqNo:2958; depth 16.38 2021-02-12T23:20:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:20:58Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:20:58Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:20:58Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:20:58Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:20:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:20:58Z- handleDvl() - RDI_Pathfinder msg: 1613172058.91 epoch sec; seqNo:1285 2021-02-12T23:20:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:20:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , 0.17 2021-02-12T23:20:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172058.93 epoch sec; seqNo:2958; 4056766.16 north; 590812.80 east 2021-02-12T23:20:59Z- AHRS_M2 msg: 1613172059.31 epoch sec; seqNo:2932 2021-02-12T23:20:59Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-12T23:20:59Z- handleDepth()-Depth_Keller msg: 1613172059316 epoch sec; seqNo:2959; depth 16.28 2021-02-12T23:20:59Z- handleDvl() - RDI_Pathfinder msg: 1613172059.33 epoch sec; seqNo:1286 2021-02-12T23:20:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 93.24 , 105.41 , 70.27 2021-02-12T23:20:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.07 , 0.07 2021-02-12T23:20:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172059.35 epoch sec; seqNo:2959; 4056766.54 north; 590812.79 east 2021-02-12T23:20:59Z- AHRS_M2 msg: 1613172059.72 epoch sec; seqNo:2933 2021-02-12T23:20:59Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-12T23:20:59Z- handleDepth()-Depth_Keller msg: 1613172059725 epoch sec; seqNo:2960; depth 16.15 2021-02-12T23:20:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172059.74 epoch sec; seqNo:2960; 4056766.54 north; 590812.79 east 2021-02-12T23:21:00Z- AHRS_M2 msg: 1613172060.12 epoch sec; seqNo:2934 2021-02-12T23:21:00Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-12T23:21:00Z- handleDepth()-Depth_Keller msg: 1613172060130 epoch sec; seqNo:2961; depth 16.04 2021-02-12T23:21:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:00Z- handleDvl() - RDI_Pathfinder msg: 1613172060.13 epoch sec; seqNo:1287 2021-02-12T23:21:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.07 , 0.07 2021-02-12T23:21:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172060.14 epoch sec; seqNo:2961; 4056766.92 north; 590812.79 east 2021-02-12T23:21:00Z- AHRS_M2 msg: 1613172060.52 epoch sec; seqNo:2935 2021-02-12T23:21:00Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-12T23:21:00Z- handleDepth()-Depth_Keller msg: 1613172060534 epoch sec; seqNo:2962; depth 15.95 2021-02-12T23:21:00Z- handleDvl() - RDI_Pathfinder msg: 1613172060.55 epoch sec; seqNo:1288 2021-02-12T23:21:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 93.18 , 105.34 , 70.27 2021-02-12T23:21:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.08 , -0.03 2021-02-12T23:21:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172060.56 epoch sec; seqNo:2962; 4056766.93 north; 590814.01 east 2021-02-12T23:21:00Z- LcmTrn::handleMessages() - handling 47 messages took 0.74 ms and 132 calls... 2021-02-12T23:21:00Z- waiting for 1613172058.33, time is 1613172060.56 2021-02-12T23:21:00Z- LcmTrn::updateTrn() equating pose 1613172060.56 and meas times 1613172060.56 2021-02-12T23:21:00Z- LcmTrn::updateTrn() >>>> pose time 1613172060.56, meas time 1613172060.56 <<<< 2021-02-12T23:21:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172060.56 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:21:00Z- TerrainNav::motionUpdate() - currEstimate = (4056767, 590814.01, 15.95) 2021-02-12T23:21:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:21:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:21:00Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172060.56 sec.; No relevant navigation data available 2021-02-12T23:21:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:21:00Z- TNavBF::mmse selected from filter -1 2021-02-12T23:21:00Z- variances([0], [2], and psi_berg) = 774.973552 833.060713 0.000000 2021-02-12T23:21:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:21:00Z- LcmTrn::publishEstimates() - MLE : 4056744.43 590863.51 15.95 2021-02-12T23:21:00Z- LcmTrn::publishEstimates() - MMSE : 4056766.93 590814.01 15.95 2021-02-12T23:21:00Z- LcmTrn::publishEstimates() - COVAR: 774.97 833.06 1.17 0.00 2021-02-12T23:21:00Z- handleDepth()-Depth_Keller msg: 1613172060950 epoch sec; seqNo:2963; depth 15.83 2021-02-12T23:21:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172060.96 epoch sec; seqNo:2963; 4056767.31 north; 590814.01 east 2021-02-12T23:21:01Z- AHRS_M2 msg: 1613172061.33 epoch sec; seqNo:2936 2021-02-12T23:21:01Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-12T23:21:01Z- handleDepth()-Depth_Keller msg: 1613172061337 epoch sec; seqNo:2964; depth 15.71 2021-02-12T23:21:01Z- handleDvl() - RDI_Pathfinder msg: 1613172061.35 epoch sec; seqNo:1289 2021-02-12T23:21:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.63 , 94.39 , 105.34 , 71.09 2021-02-12T23:21:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , -0.07 2021-02-12T23:21:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172061.36 epoch sec; seqNo:2964; 4056767.69 north; 590814.00 east 2021-02-12T23:21:01Z- AHRS_M2 msg: 1613172061.73 epoch sec; seqNo:2937 2021-02-12T23:21:01Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:21:01Z- handleDepth()-Depth_Keller msg: 1613172061743 epoch sec; seqNo:2965; depth 15.61 2021-02-12T23:21:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172061.76 epoch sec; seqNo:2965; 4056767.69 north; 590814.00 east 2021-02-12T23:21:02Z- AHRS_M2 msg: 1613172062.14 epoch sec; seqNo:2938 2021-02-12T23:21:02Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.18 psi 2021-02-12T23:21:02Z- handleDepth()-Depth_Keller msg: 1613172062147 epoch sec; seqNo:2966; depth 15.48 2021-02-12T23:21:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172062.16 epoch sec; seqNo:2966; 4056768.06 north; 590814.00 east 2021-02-12T23:21:02Z- AHRS_M2 msg: 1613172062.54 epoch sec; seqNo:2939 2021-02-12T23:21:02Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-12T23:21:02Z- handleDepth()-Depth_Keller msg: 1613172062549 epoch sec; seqNo:2967; depth 15.38 2021-02-12T23:21:02Z- handleDvl() - RDI_Pathfinder msg: 1613172062.56 epoch sec; seqNo:1290 2021-02-12T23:21:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.81 , 96.76 , 105.27 , 71.50 2021-02-12T23:21:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.14 , 0.03 2021-02-12T23:21:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172062.59 epoch sec; seqNo:2967; 4056768.44 north; 590813.99 east 2021-02-12T23:21:02Z- AHRS_M2 msg: 1613172062.95 epoch sec; seqNo:2940 2021-02-12T23:21:02Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.20 psi 2021-02-12T23:21:02Z- handleDepth()-Depth_Keller msg: 1613172062952 epoch sec; seqNo:2968; depth 15.26 2021-02-12T23:21:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172062.96 epoch sec; seqNo:2968; 4056768.82 north; 590813.99 east 2021-02-12T23:21:03Z- AHRS_M2 msg: 1613172063.38 epoch sec; seqNo:2941 2021-02-12T23:21:03Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.22 psi 2021-02-12T23:21:03Z- handleDepth()-Depth_Keller msg: 1613172063383 epoch sec; seqNo:2969; depth 15.14 2021-02-12T23:21:03Z- handleDvl() - RDI_Pathfinder msg: 1613172063.39 epoch sec; seqNo:1291 2021-02-12T23:21:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.22 , 96.76 , 107.71 , 71.91 2021-02-12T23:21:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.10 , 0.19 2021-02-12T23:21:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172063.40 epoch sec; seqNo:2969; 4056769.21 north; 590815.21 east 2021-02-12T23:21:03Z- AHRS_M2 msg: 1613172063.76 epoch sec; seqNo:2942 2021-02-12T23:21:03Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.22 psi 2021-02-12T23:21:03Z- handleDepth()-Depth_Keller msg: 1613172063760 epoch sec; seqNo:2970; depth 15.03 2021-02-12T23:21:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:03Z- handleDvl() - RDI_Pathfinder msg: 1613172063.76 epoch sec; seqNo:1292 2021-02-12T23:21:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.10 , 0.19 2021-02-12T23:21:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172063.79 epoch sec; seqNo:2970; 4056769.21 north; 590815.21 east 2021-02-12T23:21:04Z- AHRS_M2 msg: 1613172064.16 epoch sec; seqNo:2943 2021-02-12T23:21:04Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.22 psi 2021-02-12T23:21:04Z- handleDepth()-Depth_Keller msg: 1613172064164 epoch sec; seqNo:2971; depth 14.91 2021-02-12T23:21:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172064.17 epoch sec; seqNo:2971; 4056769.59 north; 590815.20 east 2021-02-12T23:21:04Z- AHRS_M2 msg: 1613172064.57 epoch sec; seqNo:2944 2021-02-12T23:21:04Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.22 psi 2021-02-12T23:21:04Z- handleDepth()-Depth_Keller msg: 1613172064569 epoch sec; seqNo:2972; depth 14.81 2021-02-12T23:21:04Z- handleDvl() - RDI_Pathfinder msg: 1613172064.58 epoch sec; seqNo:1293 2021-02-12T23:21:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.63 , 101.56 , 108.85 , 72.72 2021-02-12T23:21:04Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.09 , 0.16 2021-02-12T23:21:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172064.59 epoch sec; seqNo:2972; 4056769.97 north; 590815.20 east 2021-02-12T23:21:04Z- AHRS_M2 msg: 1613172064.97 epoch sec; seqNo:2945 2021-02-12T23:21:04Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.21 psi 2021-02-12T23:21:04Z- handleDepth()-Depth_Keller msg: 1613172064974 epoch sec; seqNo:2973; depth 14.70 2021-02-12T23:21:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:04Z- handleDvl() - RDI_Pathfinder msg: 1613172064.98 epoch sec; seqNo:1294 2021-02-12T23:21:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:04Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.09 , 0.16 2021-02-12T23:21:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172064.99 epoch sec; seqNo:2973; 4056770.35 north; 590815.19 east 2021-02-12T23:21:05Z- AHRS_M2 msg: 1613172065.38 epoch sec; seqNo:2946 2021-02-12T23:21:05Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.20 psi 2021-02-12T23:21:05Z- handleDepth()-Depth_Keller msg: 1613172065382 epoch sec; seqNo:2974; depth 14.58 2021-02-12T23:21:05Z- handleDvl() - RDI_Pathfinder msg: 1613172065.40 epoch sec; seqNo:1295 2021-02-12T23:21:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 96.49 , 111.08 , 71.91 2021-02-12T23:21:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.23 , -0.05 2021-02-12T23:21:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172065.41 epoch sec; seqNo:2974; 4056770.73 north; 590815.19 east 2021-02-12T23:21:05Z- LcmTrn::handleMessages() - handling 42 messages took 0.64 ms and 130 calls... 2021-02-12T23:21:05Z- waiting for 1613172063.56, time is 1613172065.41 2021-02-12T23:21:05Z- LcmTrn::updateTrn() equating pose 1613172065.41 and meas times 1613172065.41 2021-02-12T23:21:05Z- LcmTrn::updateTrn() >>>> pose time 1613172065.41, meas time 1613172065.41 <<<< 2021-02-12T23:21:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172065.41 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:21:05Z- TerrainNav::motionUpdate() - currEstimate = (4056771, 590815.19, 14.58) 2021-02-12T23:21:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:21:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:21:05Z- TNavBF::Projecting Measurements 2021-02-12T23:21:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:21:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:21:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:21:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:21:05Z- TNavFilter::4 good beams. 2021-02-12T23:21:05Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:21:05Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:21:05Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:21:05Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:21:05Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:21:05Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:21:05Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:21:05Z- TNavBF::mmse selected from filter -1 2021-02-12T23:21:05Z- TerrainMapOctree: (vehN, vehE ) = (4056770.73, 590815.19). 2021-02-12T23:21:05Z- TerrainMapOctree: Min Distance = 5644.62. 2021-02-12T23:21:05Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:21:05Z- There is no specific good beam that all particles have in common. 2021-02-12T23:21:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:21:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-12T23:21:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:21:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 14.90 2021-02-12T23:21:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:21:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 19.74 2021-02-12T23:21:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:21:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 8.27 2021-02-12T23:21:05Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:21:05Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:21:05Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:21:05Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:21:05Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:21:05Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:21:05Z- Filter 0 No beams used in subcloud update 2021-02-12T23:21:05Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:21:05Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:21:05Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 774.8 EastVariance: 833.1 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:21:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172065.41 sec. 2021-02-12T23:21:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:21:05Z- TNavBF::mmse selected from filter -1 2021-02-12T23:21:05Z- variances([0], [2], and psi_berg) = 774.833627 833.060528 0.000000 2021-02-12T23:21:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:21:05Z- LcmTrn::publishEstimates() - MLE : 4056748.23 590864.69 14.58 2021-02-12T23:21:05Z- LcmTrn::publishEstimates() - MMSE : 4056770.73 590815.19 14.58 2021-02-12T23:21:05Z- LcmTrn::publishEstimates() - COVAR: 774.83 833.06 0.98 0.00 2021-02-12T23:21:05Z- AHRS_M2 msg: 1613172065.78 epoch sec; seqNo:2947 2021-02-12T23:21:05Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.19 psi 2021-02-12T23:21:05Z- handleDepth()-Depth_Keller msg: 1613172065780 epoch sec; seqNo:2975; depth 14.47 2021-02-12T23:21:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172065.80 epoch sec; seqNo:2975; 4056770.74 north; 590816.41 east 2021-02-12T23:21:06Z- AHRS_M2 msg: 1613172066.18 epoch sec; seqNo:2948 2021-02-12T23:21:06Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.18 psi 2021-02-12T23:21:06Z- handleDepth()-Depth_Keller msg: 1613172066184 epoch sec; seqNo:2976; depth 14.34 2021-02-12T23:21:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:06Z- handleDvl() - RDI_Pathfinder msg: 1613172066.19 epoch sec; seqNo:1296 2021-02-12T23:21:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.23 , -0.05 2021-02-12T23:21:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172066.20 epoch sec; seqNo:2976; 4056771.12 north; 590816.41 east 2021-02-12T23:21:06Z- AHRS_M2 msg: 1613172066.59 epoch sec; seqNo:2949 2021-02-12T23:21:06Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.18 psi 2021-02-12T23:21:06Z- handleDepth()-Depth_Keller msg: 1613172066590 epoch sec; seqNo:2977; depth 14.22 2021-02-12T23:21:06Z- handleDvl() - RDI_Pathfinder msg: 1613172066.60 epoch sec; seqNo:1297 2021-02-12T23:21:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.26 , 96.55 , 112.37 , 72.32 2021-02-12T23:21:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.11 , -0.27 2021-02-12T23:21:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172066.61 epoch sec; seqNo:2977; 4056771.12 north; 590816.41 east 2021-02-12T23:21:06Z- AHRS_M2 msg: 1613172066.99 epoch sec; seqNo:2950 2021-02-12T23:21:06Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-12T23:21:06Z- handleDepth()-Depth_Keller msg: 1613172066992 epoch sec; seqNo:2978; depth 14.08 2021-02-12T23:21:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172067.01 epoch sec; seqNo:2978; 4056771.50 north; 590816.40 east 2021-02-12T23:21:07Z- AHRS_M2 msg: 1613172067.39 epoch sec; seqNo:2951 2021-02-12T23:21:07Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-12T23:21:07Z- handleDepth()-Depth_Keller msg: 1613172067396 epoch sec; seqNo:2979; depth 13.96 2021-02-12T23:21:07Z- handleDvl() - RDI_Pathfinder msg: 1613172067.41 epoch sec; seqNo:1298 2021-02-12T23:21:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.26 , 96.55 , 109.93 , 72.32 2021-02-12T23:21:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.16 , -0.25 2021-02-12T23:21:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172067.43 epoch sec; seqNo:2979; 4056771.50 north; 590816.40 east 2021-02-12T23:21:07Z- AHRS_M2 msg: 1613172067.80 epoch sec; seqNo:2952 2021-02-12T23:21:07Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-12T23:21:07Z- handleDepth()-Depth_Keller msg: 1613172067805 epoch sec; seqNo:2980; depth 13.83 2021-02-12T23:21:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172067.82 epoch sec; seqNo:2980; 4056771.88 north; 590816.40 east 2021-02-12T23:21:08Z- AHRS_M2 msg: 1613172068.20 epoch sec; seqNo:2953 2021-02-12T23:21:08Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-12T23:21:08Z- handleDepth()-Depth_Keller msg: 1613172068213 epoch sec; seqNo:2981; depth 13.70 2021-02-12T23:21:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172068.24 epoch sec; seqNo:2981; 4056772.26 north; 590816.39 east 2021-02-12T23:21:08Z- AHRS_M2 msg: 1613172068.60 epoch sec; seqNo:2954 2021-02-12T23:21:08Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-12T23:21:08Z- handleDepth()-Depth_Keller msg: 1613172068608 epoch sec; seqNo:2982; depth 13.56 2021-02-12T23:21:08Z- handleDvl() - RDI_Pathfinder msg: 1613172068.63 epoch sec; seqNo:1299 2021-02-12T23:21:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 96.55 , 107.50 , 72.32 2021-02-12T23:21:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.34 , -0.05 2021-02-12T23:21:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172068.64 epoch sec; seqNo:2982; 4056772.64 north; 590816.39 east 2021-02-12T23:21:09Z- AHRS_M2 msg: 1613172069.01 epoch sec; seqNo:2955 2021-02-12T23:21:09Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-12T23:21:09Z- handleDepth()-Depth_Keller msg: 1613172069017 epoch sec; seqNo:2983; depth 13.43 2021-02-12T23:21:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172069.03 epoch sec; seqNo:2983; 4056772.64 north; 590816.39 east 2021-02-12T23:21:09Z- AHRS_M2 msg: 1613172069.41 epoch sec; seqNo:2956 2021-02-12T23:21:09Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-12T23:21:09Z- handleDepth()-Depth_Keller msg: 1613172069417 epoch sec; seqNo:2984; depth 13.31 2021-02-12T23:21:09Z- handleDvl() - RDI_Pathfinder msg: 1613172069.43 epoch sec; seqNo:1300 2021-02-12T23:21:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.26 , 96.62 , 111.22 , 72.32 2021-02-12T23:21:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.30 , 0.16 2021-02-12T23:21:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172069.44 epoch sec; seqNo:2984; 4056773.03 north; 590817.61 east 2021-02-12T23:21:09Z- AHRS_M2 msg: 1613172069.82 epoch sec; seqNo:2957 2021-02-12T23:21:09Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-12T23:21:09Z- handleDepth()-Depth_Keller msg: 1613172069830 epoch sec; seqNo:2985; depth 13.18 2021-02-12T23:21:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:09Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:09Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:09Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:09Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:09Z- handleDvl() - RDI_Pathfinder msg: 1613172069.83 epoch sec; seqNo:1301 2021-02-12T23:21:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.30 , 0.16 2021-02-12T23:21:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172069.84 epoch sec; seqNo:2985; 4056773.41 north; 590817.60 east 2021-02-12T23:21:10Z- AHRS_M2 msg: 1613172070.22 epoch sec; seqNo:2958 2021-02-12T23:21:10Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.15 psi 2021-02-12T23:21:10Z- handleDepth()-Depth_Keller msg: 1613172070229 epoch sec; seqNo:2986; depth 13.07 2021-02-12T23:21:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172070.24 epoch sec; seqNo:2986; 4056773.79 north; 590817.60 east 2021-02-12T23:21:10Z- AHRS_M2 msg: 1613172070.62 epoch sec; seqNo:2959 2021-02-12T23:21:10Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.15 psi 2021-02-12T23:21:10Z- handleDepth()-Depth_Keller msg: 1613172070628 epoch sec; seqNo:2987; depth 12.93 2021-02-12T23:21:10Z- handleDvl() - RDI_Pathfinder msg: 1613172070.64 epoch sec; seqNo:1302 2021-02-12T23:21:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 96.55 , 111.15 , 78.31 2021-02-12T23:21:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.16 , 0.22 2021-02-12T23:21:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172070.65 epoch sec; seqNo:2987; 4056774.16 north; 590817.60 east 2021-02-12T23:21:10Z- LcmTrn::handleMessages() - handling 46 messages took 0.76 ms and 131 calls... 2021-02-12T23:21:10Z- waiting for 1613172068.41, time is 1613172070.65 2021-02-12T23:21:10Z- LcmTrn::updateTrn() equating pose 1613172070.65 and meas times 1613172070.65 2021-02-12T23:21:10Z- LcmTrn::updateTrn() >>>> pose time 1613172070.65, meas time 1613172070.65 <<<< 2021-02-12T23:21:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172070.65 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:21:10Z- TerrainNav::motionUpdate() - currEstimate = (4056774, 590817.60, 12.93) 2021-02-12T23:21:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:21:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:21:10Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172070.65 sec.; No relevant navigation data available 2021-02-12T23:21:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:21:10Z- TNavBF::mmse selected from filter -1 2021-02-12T23:21:10Z- variances([0], [2], and psi_berg) = 774.690323 833.060338 0.000000 2021-02-12T23:21:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:21:10Z- LcmTrn::publishEstimates() - MLE : 4056751.66 590867.10 12.93 2021-02-12T23:21:10Z- LcmTrn::publishEstimates() - MMSE : 4056774.16 590817.60 12.93 2021-02-12T23:21:10Z- LcmTrn::publishEstimates() - COVAR: 774.69 833.06 1.19 0.00 2021-02-12T23:21:11Z- AHRS_M2 msg: 1613172071.03 epoch sec; seqNo:2960 2021-02-12T23:21:11Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.14 psi 2021-02-12T23:21:11Z- handleDepth()-Depth_Keller msg: 1613172071041 epoch sec; seqNo:2988; depth 12.83 2021-02-12T23:21:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:11Z- handleDvl() - RDI_Pathfinder msg: 1613172071.04 epoch sec; seqNo:1303 2021-02-12T23:21:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.16 , 0.22 2021-02-12T23:21:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172071.06 epoch sec; seqNo:2988; 4056774.54 north; 590817.59 east 2021-02-12T23:21:11Z- AHRS_M2 msg: 1613172071.44 epoch sec; seqNo:2961 2021-02-12T23:21:11Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.13 psi 2021-02-12T23:21:11Z- handleDepth()-Depth_Keller msg: 1613172071454 epoch sec; seqNo:2989; depth 12.70 2021-02-12T23:21:11Z- handleDvl() - RDI_Pathfinder msg: 1613172071.48 epoch sec; seqNo:1304 2021-02-12T23:21:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 95.00 , 111.22 , 73.13 2021-02-12T23:21:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.14 2021-02-12T23:21:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172071.49 epoch sec; seqNo:2989; 4056774.92 north; 590817.59 east 2021-02-12T23:21:11Z- AHRS_M2 msg: 1613172071.84 epoch sec; seqNo:2962 2021-02-12T23:21:11Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.12 psi 2021-02-12T23:21:11Z- handleDepth()-Depth_Keller msg: 1613172071846 epoch sec; seqNo:2990; depth 12.57 2021-02-12T23:21:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172071.86 epoch sec; seqNo:2990; 4056774.92 north; 590817.59 east 2021-02-12T23:21:12Z- AHRS_M2 msg: 1613172072.24 epoch sec; seqNo:2963 2021-02-12T23:21:12Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.12 psi 2021-02-12T23:21:12Z- handleDepth()-Depth_Keller msg: 1613172072253 epoch sec; seqNo:2991; depth 12.46 2021-02-12T23:21:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:12Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:12Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:12Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:12Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:12Z- handleDvl() - RDI_Pathfinder msg: 1613172072.26 epoch sec; seqNo:1305 2021-02-12T23:21:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.14 2021-02-12T23:21:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172072.26 epoch sec; seqNo:2991; 4056775.30 north; 590817.58 east 2021-02-12T23:21:12Z- AHRS_M2 msg: 1613172072.64 epoch sec; seqNo:2964 2021-02-12T23:21:12Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.12 psi 2021-02-12T23:21:12Z- handleDepth()-Depth_Keller msg: 1613172072654 epoch sec; seqNo:2992; depth 12.35 2021-02-12T23:21:12Z- handleDvl() - RDI_Pathfinder msg: 1613172072.67 epoch sec; seqNo:1306 2021-02-12T23:21:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 96.49 , 111.08 , 72.72 2021-02-12T23:21:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , 0.06 2021-02-12T23:21:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172072.68 epoch sec; seqNo:2992; 4056775.69 north; 590818.80 east 2021-02-12T23:21:13Z- AHRS_M2 msg: 1613172073.05 epoch sec; seqNo:2965 2021-02-12T23:21:13Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.13 psi 2021-02-12T23:21:13Z- handleDepth()-Depth_Keller msg: 1613172073052 epoch sec; seqNo:2993; depth 12.22 2021-02-12T23:21:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172073.06 epoch sec; seqNo:2993; 4056776.07 north; 590818.80 east 2021-02-12T23:21:13Z- AHRS_M2 msg: 1613172073.45 epoch sec; seqNo:2966 2021-02-12T23:21:13Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.13 psi 2021-02-12T23:21:13Z- handleDepth()-Depth_Keller msg: 1613172073461 epoch sec; seqNo:2994; depth 12.10 2021-02-12T23:21:13Z- handleDvl() - RDI_Pathfinder msg: 1613172073.47 epoch sec; seqNo:1307 2021-02-12T23:21:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 96.49 , 112.30 , 73.13 2021-02-12T23:21:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , 0.08 2021-02-12T23:21:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172073.50 epoch sec; seqNo:2994; 4056776.07 north; 590818.80 east 2021-02-12T23:21:13Z- AHRS_M2 msg: 1613172073.87 epoch sec; seqNo:2967 2021-02-12T23:21:13Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.14 psi 2021-02-12T23:21:13Z- handleDepth()-Depth_Keller msg: 1613172073873 epoch sec; seqNo:2995; depth 11.96 2021-02-12T23:21:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172073.89 epoch sec; seqNo:2995; 4056776.45 north; 590818.79 east 2021-02-12T23:21:14Z- AHRS_M2 msg: 1613172074.26 epoch sec; seqNo:2968 2021-02-12T23:21:14Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.15 psi 2021-02-12T23:21:14Z- handleDepth()-Depth_Keller msg: 1613172074266 epoch sec; seqNo:2996; depth 11.86 2021-02-12T23:21:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172074.29 epoch sec; seqNo:2996; 4056776.83 north; 590818.79 east 2021-02-12T23:21:14Z- AHRS_M2 msg: 1613172074.66 epoch sec; seqNo:2969 2021-02-12T23:21:14Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-12T23:21:14Z- handleDepth()-Depth_Keller msg: 1613172074668 epoch sec; seqNo:2997; depth 11.72 2021-02-12T23:21:14Z- handleDvl() - RDI_Pathfinder msg: 1613172074.68 epoch sec; seqNo:1308 2021-02-12T23:21:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 96.42 , 111.02 , 72.72 2021-02-12T23:21:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , 0.17 2021-02-12T23:21:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172074.70 epoch sec; seqNo:2997; 4056777.21 north; 590818.78 east 2021-02-12T23:21:15Z- AHRS_M2 msg: 1613172075.07 epoch sec; seqNo:2970 2021-02-12T23:21:15Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.16 psi 2021-02-12T23:21:15Z- handleDepth()-Depth_Keller msg: 1613172075077 epoch sec; seqNo:2998; depth 11.60 2021-02-12T23:21:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172075.09 epoch sec; seqNo:2998; 4056777.22 north; 590820.01 east 2021-02-12T23:21:15Z- AHRS_M2 msg: 1613172075.47 epoch sec; seqNo:2971 2021-02-12T23:21:15Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-12T23:21:15Z- handleDepth()-Depth_Keller msg: 1613172075478 epoch sec; seqNo:2999; depth 11.47 2021-02-12T23:21:15Z- handleDvl() - RDI_Pathfinder msg: 1613172075.49 epoch sec; seqNo:1309 2021-02-12T23:21:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.32 , 96.49 , 112.30 , 72.72 2021-02-12T23:21:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , 0.24 2021-02-12T23:21:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172075.50 epoch sec; seqNo:2999; 4056777.60 north; 590820.00 east 2021-02-12T23:21:15Z- LcmTrn::handleMessages() - handling 43 messages took 0.62 ms and 131 calls... 2021-02-12T23:21:15Z- waiting for 1613172073.65, time is 1613172075.50 2021-02-12T23:21:15Z- LcmTrn::updateTrn() equating pose 1613172075.50 and meas times 1613172075.50 2021-02-12T23:21:15Z- LcmTrn::updateTrn() >>>> pose time 1613172075.50, meas time 1613172075.50 <<<< 2021-02-12T23:21:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172075.50 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:21:15Z- TerrainNav::motionUpdate() - currEstimate = (4056778, 590820.00, 11.47) 2021-02-12T23:21:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:21:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:21:15Z- TNavBF::Projecting Measurements 2021-02-12T23:21:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:21:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:21:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:21:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:21:15Z- TNavFilter::4 good beams. 2021-02-12T23:21:15Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:21:15Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:21:15Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:21:15Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:21:15Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:21:15Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:21:15Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:21:15Z- TNavBF::mmse selected from filter -1 2021-02-12T23:21:15Z- TerrainMapOctree: (vehN, vehE ) = (4056777.60, 590820.00). 2021-02-12T23:21:15Z- TerrainMapOctree: Min Distance = 5636.24. 2021-02-12T23:21:15Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:21:15Z- There is no specific good beam that all particles have in common. 2021-02-12T23:21:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:21:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.41 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 11.38 2021-02-12T23:21:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:21:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 14.90 2021-02-12T23:21:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:21:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 20.18 2021-02-12T23:21:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:21:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 8.46 2021-02-12T23:21:15Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:21:15Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:21:15Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:21:15Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:21:15Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:21:15Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:21:15Z- Filter 0 No beams used in subcloud update 2021-02-12T23:21:15Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:21:15Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:21:15Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 774.5 EastVariance: 833.1 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:21:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172075.50 sec. 2021-02-12T23:21:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:21:15Z- TNavBF::mmse selected from filter -1 2021-02-12T23:21:15Z- variances([0], [2], and psi_berg) = 774.547354 833.060150 0.000000 2021-02-12T23:21:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:21:15Z- LcmTrn::publishEstimates() - MLE : 4056755.10 590869.50 11.47 2021-02-12T23:21:15Z- LcmTrn::publishEstimates() - MMSE : 4056777.60 590820.00 11.47 2021-02-12T23:21:15Z- LcmTrn::publishEstimates() - COVAR: 774.55 833.06 1.01 0.00 2021-02-12T23:21:15Z- AHRS_M2 msg: 1613172075.88 epoch sec; seqNo:2972 2021-02-12T23:21:15Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-12T23:21:15Z- handleDepth()-Depth_Keller msg: 1613172075881 epoch sec; seqNo:3000; depth 11.34 2021-02-12T23:21:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:15Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:15Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:15Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:15Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:15Z- handleDvl() - RDI_Pathfinder msg: 1613172075.88 epoch sec; seqNo:1310 2021-02-12T23:21:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , 0.24 2021-02-12T23:21:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172075.90 epoch sec; seqNo:3000; 4056777.98 north; 590820.00 east 2021-02-12T23:21:16Z- AHRS_M2 msg: 1613172076.28 epoch sec; seqNo:2973 2021-02-12T23:21:16Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.15 psi 2021-02-12T23:21:16Z- handleDepth()-Depth_Keller msg: 1613172076291 epoch sec; seqNo:3001; depth 11.22 2021-02-12T23:21:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172076.33 epoch sec; seqNo:3001; 4056778.36 north; 590819.99 east 2021-02-12T23:21:16Z- AHRS_M2 msg: 1613172076.68 epoch sec; seqNo:2974 2021-02-12T23:21:16Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.16 psi 2021-02-12T23:21:16Z- handleDepth()-Depth_Keller msg: 1613172076688 epoch sec; seqNo:3002; depth 11.09 2021-02-12T23:21:16Z- handleDvl() - RDI_Pathfinder msg: 1613172076.70 epoch sec; seqNo:1311 2021-02-12T23:21:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 96.42 , 114.67 , 75.58 2021-02-12T23:21:16Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.09 , 0.17 2021-02-12T23:21:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172076.71 epoch sec; seqNo:3002; 4056778.74 north; 590819.99 east 2021-02-12T23:21:17Z- AHRS_M2 msg: 1613172077.09 epoch sec; seqNo:2975 2021-02-12T23:21:17Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.17 psi 2021-02-12T23:21:17Z- handleDepth()-Depth_Keller msg: 1613172077097 epoch sec; seqNo:3003; depth 10.96 2021-02-12T23:21:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:17Z- handleDvl() - RDI_Pathfinder msg: 1613172077.10 epoch sec; seqNo:1312 2021-02-12T23:21:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:17Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.09 , 0.17 2021-02-12T23:21:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172077.11 epoch sec; seqNo:3003; 4056778.75 north; 590821.21 east 2021-02-12T23:21:17Z- AHRS_M2 msg: 1613172077.49 epoch sec; seqNo:2976 2021-02-12T23:21:17Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.17 psi 2021-02-12T23:21:17Z- handleDepth()-Depth_Keller msg: 1613172077496 epoch sec; seqNo:3004; depth 10.83 2021-02-12T23:21:17Z- handleDvl() - RDI_Pathfinder msg: 1613172077.51 epoch sec; seqNo:1313 2021-02-12T23:21:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 96.28 , 116.56 , 74.36 2021-02-12T23:21:17Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.24 , -0.07 2021-02-12T23:21:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172077.53 epoch sec; seqNo:3004; 4056779.13 north; 590821.21 east 2021-02-12T23:21:17Z- AHRS_M2 msg: 1613172077.89 epoch sec; seqNo:2977 2021-02-12T23:21:17Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.16 psi 2021-02-12T23:21:17Z- handleDepth()-Depth_Keller msg: 1613172077900 epoch sec; seqNo:3005; depth 10.73 2021-02-12T23:21:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172077.91 epoch sec; seqNo:3005; 4056779.51 north; 590821.20 east 2021-02-12T23:21:18Z- AHRS_M2 msg: 1613172078.31 epoch sec; seqNo:2978 2021-02-12T23:21:18Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-12T23:21:18Z- handleDepth()-Depth_Keller msg: 1613172078316 epoch sec; seqNo:3006; depth 10.61 2021-02-12T23:21:18Z- handleDvl() - RDI_Pathfinder msg: 1613172078.33 epoch sec; seqNo:1314 2021-02-12T23:21:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 96.28 , 115.21 , 73.95 2021-02-12T23:21:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.18 , -0.34 2021-02-12T23:21:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172078.35 epoch sec; seqNo:3006; 4056779.51 north; 590821.20 east 2021-02-12T23:21:18Z- AHRS_M2 msg: 1613172078.70 epoch sec; seqNo:2979 2021-02-12T23:21:18Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.16 psi 2021-02-12T23:21:18Z- handleDepth()-Depth_Keller msg: 1613172078709 epoch sec; seqNo:3007; depth 10.48 2021-02-12T23:21:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172078.72 epoch sec; seqNo:3007; 4056779.89 north; 590821.20 east 2021-02-12T23:21:19Z- AHRS_M2 msg: 1613172079.11 epoch sec; seqNo:2980 2021-02-12T23:21:19Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.17 psi 2021-02-12T23:21:19Z- handleDepth()-Depth_Keller msg: 1613172079113 epoch sec; seqNo:3008; depth 10.35 2021-02-12T23:21:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172079.14 epoch sec; seqNo:3008; 4056779.90 north; 590822.42 east 2021-02-12T23:21:19Z- AHRS_M2 msg: 1613172079.51 epoch sec; seqNo:2981 2021-02-12T23:21:19Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.17 psi 2021-02-12T23:21:19Z- handleDepth()-Depth_Keller msg: 1613172079516 epoch sec; seqNo:3009; depth 10.23 2021-02-12T23:21:19Z- handleDvl() - RDI_Pathfinder msg: 1613172079.53 epoch sec; seqNo:1315 2021-02-12T23:21:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 96.28 , 115.21 , 74.77 2021-02-12T23:21:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.07 , -0.42 2021-02-12T23:21:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172079.54 epoch sec; seqNo:3009; 4056780.28 north; 590822.42 east 2021-02-12T23:21:19Z- AHRS_M2 msg: 1613172079.92 epoch sec; seqNo:2982 2021-02-12T23:21:19Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.18 psi 2021-02-12T23:21:19Z- handleDepth()-Depth_Keller msg: 1613172079925 epoch sec; seqNo:3010; depth 10.11 2021-02-12T23:21:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172079.94 epoch sec; seqNo:3010; 4056780.66 north; 590822.41 east 2021-02-12T23:21:20Z- AHRS_M2 msg: 1613172080.32 epoch sec; seqNo:2983 2021-02-12T23:21:20Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.17 psi 2021-02-12T23:21:20Z- handleDepth()-Depth_Keller msg: 1613172080326 epoch sec; seqNo:3011; depth 10.01 2021-02-12T23:21:20Z- handleDvl() - RDI_Pathfinder msg: 1613172080.34 epoch sec; seqNo:1316 2021-02-12T23:21:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 96.28 , 112.50 , 75.18 2021-02-12T23:21:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.28 , -0.20 2021-02-12T23:21:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172080.35 epoch sec; seqNo:3011; 4056780.66 north; 590822.41 east 2021-02-12T23:21:20Z- AHRS_M2 msg: 1613172080.72 epoch sec; seqNo:2984 2021-02-12T23:21:20Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.16 psi 2021-02-12T23:21:20Z- handleDepth()-Depth_Keller msg: 1613172080728 epoch sec; seqNo:3012; depth 9.88 2021-02-12T23:21:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:20Z- handleDvl() - RDI_Pathfinder msg: 1613172080.73 epoch sec; seqNo:1317 2021-02-12T23:21:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.28 , -0.20 2021-02-12T23:21:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172080.75 epoch sec; seqNo:3012; 4056781.04 north; 590822.41 east 2021-02-12T23:21:20Z- LcmTrn::handleMessages() - handling 47 messages took 0.72 ms and 133 calls... 2021-02-12T23:21:20Z- waiting for 1613172078.50, time is 1613172080.75 2021-02-12T23:21:20Z- LcmTrn::updateTrn() equating pose 1613172080.75 and meas times 1613172080.75 2021-02-12T23:21:20Z- LcmTrn::updateTrn() >>>> pose time 1613172080.75, meas time 1613172080.75 <<<< 2021-02-12T23:21:20Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172080.75, ping # 0. 2021-02-12T23:21:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172080.75 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:21:20Z- TerrainNav::motionUpdate() - currEstimate = (4056781, 590822.41, 9.88) 2021-02-12T23:21:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:21:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:21:20Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:21:20Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:21:20Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172080.75 sec.; No relevant navigation data available 2021-02-12T23:21:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:21:20Z- TNavBF::mmse selected from filter -1 2021-02-12T23:21:20Z- variances([0], [2], and psi_berg) = 774.404716 833.059962 0.000000 2021-02-12T23:21:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:21:20Z- LcmTrn::publishEstimates() - MLE : 4056758.54 590871.91 9.88 2021-02-12T23:21:20Z- LcmTrn::publishEstimates() - MMSE : 4056781.04 590822.41 9.88 2021-02-12T23:21:20Z- LcmTrn::publishEstimates() - COVAR: 774.40 833.06 1.22 0.00 2021-02-12T23:21:21Z- AHRS_M2 msg: 1613172081.13 epoch sec; seqNo:2985 2021-02-12T23:21:21Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.16 psi 2021-02-12T23:21:21Z- handleDepth()-Depth_Keller msg: 1613172081138 epoch sec; seqNo:3013; depth 9.76 2021-02-12T23:21:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172081.15 epoch sec; seqNo:3013; 4056781.41 north; 590822.40 east 2021-02-12T23:21:21Z- AHRS_M2 msg: 1613172081.53 epoch sec; seqNo:2986 2021-02-12T23:21:21Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.15 psi 2021-02-12T23:21:21Z- handleDepth()-Depth_Keller msg: 1613172081537 epoch sec; seqNo:3014; depth 9.64 2021-02-12T23:21:21Z- handleDvl() - RDI_Pathfinder msg: 1613172081.55 epoch sec; seqNo:1318 2021-02-12T23:21:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 96.42 , 112.23 , 78.17 2021-02-12T23:21:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.30 , 0.05 2021-02-12T23:21:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172081.56 epoch sec; seqNo:3014; 4056781.41 north; 590822.40 east 2021-02-12T23:21:21Z- AHRS_M2 msg: 1613172081.93 epoch sec; seqNo:2987 2021-02-12T23:21:21Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.15 psi 2021-02-12T23:21:21Z- handleDepth()-Depth_Keller msg: 1613172081949 epoch sec; seqNo:3015; depth 9.53 2021-02-12T23:21:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:21Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:21Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:21Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:21Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:21Z- handleDvl() - RDI_Pathfinder msg: 1613172081.95 epoch sec; seqNo:1319 2021-02-12T23:21:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.30 , 0.05 2021-02-12T23:21:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172081.96 epoch sec; seqNo:3015; 4056781.79 north; 590822.40 east 2021-02-12T23:21:22Z- AHRS_M2 msg: 1613172082.37 epoch sec; seqNo:2988 2021-02-12T23:21:22Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-12T23:21:22Z- handleDepth()-Depth_Keller msg: 1613172082372 epoch sec; seqNo:3016; depth 9.41 2021-02-12T23:21:22Z- handleDvl() - RDI_Pathfinder msg: 1613172082.38 epoch sec; seqNo:1320 2021-02-12T23:21:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.54 , 96.35 , 111.22 , 78.78 2021-02-12T23:21:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.19 , 0.17 2021-02-12T23:21:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172082.39 epoch sec; seqNo:3016; 4056782.17 north; 590822.40 east 2021-02-12T23:21:22Z- AHRS_M2 msg: 1613172082.74 epoch sec; seqNo:2989 2021-02-12T23:21:22Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-12T23:21:22Z- handleDepth()-Depth_Keller msg: 1613172082748 epoch sec; seqNo:3017; depth 9.30 2021-02-12T23:21:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172082.76 epoch sec; seqNo:3017; 4056782.55 north; 590822.39 east 2021-02-12T23:21:23Z- AHRS_M2 msg: 1613172083.15 epoch sec; seqNo:2990 2021-02-12T23:21:23Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.13 psi 2021-02-12T23:21:23Z- handleDepth()-Depth_Keller msg: 1613172083158 epoch sec; seqNo:3018; depth 9.19 2021-02-12T23:21:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:23Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:23Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:23Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:23Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:23Z- handleDvl() - RDI_Pathfinder msg: 1613172083.16 epoch sec; seqNo:1321 2021-02-12T23:21:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.19 , 0.17 2021-02-12T23:21:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172083.17 epoch sec; seqNo:3018; 4056782.55 north; 590822.39 east 2021-02-12T23:21:23Z- AHRS_M2 msg: 1613172083.55 epoch sec; seqNo:2991 2021-02-12T23:21:23Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.13 psi 2021-02-12T23:21:23Z- handleDepth()-Depth_Keller msg: 1613172083556 epoch sec; seqNo:3019; depth 9.08 2021-02-12T23:21:23Z- handleDvl() - RDI_Pathfinder msg: 1613172083.57 epoch sec; seqNo:1322 2021-02-12T23:21:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 96.28 , 112.50 , 76.40 2021-02-12T23:21:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.07 , 0.17 2021-02-12T23:21:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172083.59 epoch sec; seqNo:3019; 4056782.93 north; 590822.39 east 2021-02-12T23:21:23Z- AHRS_M2 msg: 1613172083.96 epoch sec; seqNo:2992 2021-02-12T23:21:23Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.13 psi 2021-02-12T23:21:23Z- handleDepth()-Depth_Keller msg: 1613172083965 epoch sec; seqNo:3020; depth 8.95 2021-02-12T23:21:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172083.98 epoch sec; seqNo:3020; 4056783.32 north; 590823.60 east 2021-02-12T23:21:24Z- AHRS_M2 msg: 1613172084.37 epoch sec; seqNo:2993 2021-02-12T23:21:24Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.14 psi 2021-02-12T23:21:24Z- handleDepth()-Depth_Keller msg: 1613172084375 epoch sec; seqNo:3021; depth 8.83 2021-02-12T23:21:24Z- handleDvl() - RDI_Pathfinder msg: 1613172084.39 epoch sec; seqNo:1323 2021-02-12T23:21:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.82 , 98.99 , 113.85 , 78.71 2021-02-12T23:21:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.12 2021-02-12T23:21:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172084.40 epoch sec; seqNo:3021; 4056783.32 north; 590823.60 east 2021-02-12T23:21:24Z- AHRS_M2 msg: 1613172084.76 epoch sec; seqNo:2994 2021-02-12T23:21:24Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.14 psi 2021-02-12T23:21:24Z- handleDepth()-Depth_Keller msg: 1613172084768 epoch sec; seqNo:3022; depth 8.72 2021-02-12T23:21:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172084.79 epoch sec; seqNo:3022; 4056783.70 north; 590823.60 east 2021-02-12T23:21:25Z- AHRS_M2 msg: 1613172085.18 epoch sec; seqNo:2995 2021-02-12T23:21:25Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.15 psi 2021-02-12T23:21:25Z- handleDepth()-Depth_Keller msg: 1613172085189 epoch sec; seqNo:3023; depth 8.60 2021-02-12T23:21:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172085.20 epoch sec; seqNo:3023; 4056784.08 north; 590823.60 east 2021-02-12T23:21:25Z- AHRS_M2 msg: 1613172085.57 epoch sec; seqNo:2996 2021-02-12T23:21:25Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.17 psi 2021-02-12T23:21:25Z- handleDepth()-Depth_Keller msg: 1613172085578 epoch sec; seqNo:3024; depth 8.47 2021-02-12T23:21:25Z- handleDvl() - RDI_Pathfinder msg: 1613172085.59 epoch sec; seqNo:1324 2021-02-12T23:21:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 96.15 , 115.07 , 77.63 2021-02-12T23:21:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.09 , 0.14 2021-02-12T23:21:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172085.60 epoch sec; seqNo:3024; 4056784.46 north; 590823.59 east 2021-02-12T23:21:25Z- LcmTrn::handleMessages() - handling 43 messages took 0.68 ms and 130 calls... 2021-02-12T23:21:25Z- waiting for 1613172083.75, time is 1613172085.60 2021-02-12T23:21:25Z- LcmTrn::updateTrn() equating pose 1613172085.60 and meas times 1613172085.60 2021-02-12T23:21:25Z- LcmTrn::updateTrn() >>>> pose time 1613172085.60, meas time 1613172085.60 <<<< 2021-02-12T23:21:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172085.60 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:21:25Z- TerrainNav::motionUpdate() - currEstimate = (4056784, 590823.59, 8.47) 2021-02-12T23:21:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:21:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:21:25Z- TNavBF::Projecting Measurements 2021-02-12T23:21:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:21:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:21:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:21:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:21:25Z- TNavFilter::4 good beams. 2021-02-12T23:21:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:21:25Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:21:25Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:21:25Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:21:25Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:21:25Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:21:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:21:25Z- TNavBF::mmse selected from filter -1 2021-02-12T23:21:25Z- TerrainMapOctree: (vehN, vehE ) = (4056784.46, 590823.59). 2021-02-12T23:21:25Z- TerrainMapOctree: Min Distance = 5628.62. 2021-02-12T23:21:25Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:21:25Z- There is no specific good beam that all particles have in common. 2021-02-12T23:21:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:21:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.41 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 12.02 2021-02-12T23:21:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:21:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 14.79 2021-02-12T23:21:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:21:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 21.19 2021-02-12T23:21:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:21:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 9.64 2021-02-12T23:21:25Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:21:26Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:21:26Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:21:26Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:21:26Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:21:26Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:21:26Z- Filter 0 No beams used in subcloud update 2021-02-12T23:21:26Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:21:26Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:21:26Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 774.3 EastVariance: 833.1 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:21:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172085.60 sec. 2021-02-12T23:21:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:21:26Z- TNavBF::mmse selected from filter -1 2021-02-12T23:21:26Z- variances([0], [2], and psi_berg) = 774.272474 833.059788 0.000000 2021-02-12T23:21:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:21:26Z- LcmTrn::publishEstimates() - MLE : 4056761.96 590873.09 8.47 2021-02-12T23:21:26Z- LcmTrn::publishEstimates() - MMSE : 4056784.46 590823.59 8.47 2021-02-12T23:21:26Z- LcmTrn::publishEstimates() - COVAR: 774.27 833.06 1.03 0.00 2021-02-12T23:21:26Z- AHRS_M2 msg: 1613172085.97 epoch sec; seqNo:2997 2021-02-12T23:21:26Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.18 psi 2021-02-12T23:21:26Z- handleDepth()-Depth_Keller msg: 1613172085990 epoch sec; seqNo:3025; depth 8.37 2021-02-12T23:21:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172086.00 epoch sec; seqNo:3025; 4056784.46 north; 590823.59 east 2021-02-12T23:21:26Z- AHRS_M2 msg: 1613172086.38 epoch sec; seqNo:2998 2021-02-12T23:21:26Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.18 psi 2021-02-12T23:21:26Z- handleDepth()-Depth_Keller msg: 1613172086384 epoch sec; seqNo:3026; depth 8.23 2021-02-12T23:21:26Z- handleDvl() - RDI_Pathfinder msg: 1613172086.40 epoch sec; seqNo:1325 2021-02-12T23:21:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.40 , 104.33 , 116.49 , 76.81 2021-02-12T23:21:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , 0.27 2021-02-12T23:21:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172086.42 epoch sec; seqNo:3026; 4056784.85 north; 590824.81 east 2021-02-12T23:21:26Z- AHRS_M2 msg: 1613172086.78 epoch sec; seqNo:2999 2021-02-12T23:21:26Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.18 psi 2021-02-12T23:21:26Z- handleDepth()-Depth_Keller msg: 1613172086793 epoch sec; seqNo:3027; depth 8.11 2021-02-12T23:21:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:26Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:26Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:26Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:26Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:26Z- handleDvl() - RDI_Pathfinder msg: 1613172086.80 epoch sec; seqNo:1326 2021-02-12T23:21:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , 0.27 2021-02-12T23:21:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172086.81 epoch sec; seqNo:3027; 4056785.23 north; 590824.81 east 2021-02-12T23:21:27Z- AHRS_M2 msg: 1613172087.19 epoch sec; seqNo:3000 2021-02-12T23:21:27Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.18 psi 2021-02-12T23:21:27Z- handleDepth()-Depth_Keller msg: 1613172087201 epoch sec; seqNo:3028; depth 8.00 2021-02-12T23:21:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172087.21 epoch sec; seqNo:3028; 4056785.61 north; 590824.80 east 2021-02-12T23:21:27Z- AHRS_M2 msg: 1613172087.60 epoch sec; seqNo:3001 2021-02-12T23:21:27Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-12T23:21:27Z- handleDepth()-Depth_Keller msg: 1613172087604 epoch sec; seqNo:3029; depth 7.88 2021-02-12T23:21:27Z- handleDvl() - RDI_Pathfinder msg: 1613172087.64 epoch sec; seqNo:1327 2021-02-12T23:21:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.55 , 105.47 , 118.99 , 75.99 2021-02-12T23:21:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.12 , 0.32 2021-02-12T23:21:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172087.65 epoch sec; seqNo:3029; 4056785.99 north; 590824.80 east 2021-02-12T23:21:27Z- AHRS_M2 msg: 1613172087.99 epoch sec; seqNo:3002 2021-02-12T23:21:27Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-12T23:21:28Z- handleDepth()-Depth_Keller msg: 1613172088005 epoch sec; seqNo:3030; depth 7.75 2021-02-12T23:21:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:28Z- handleDvl() - RDI_Pathfinder msg: 1613172088.01 epoch sec; seqNo:1328 2021-02-12T23:21:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.12 , 0.32 2021-02-12T23:21:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172088.02 epoch sec; seqNo:3030; 4056785.99 north; 590824.80 east 2021-02-12T23:21:28Z- AHRS_M2 msg: 1613172088.42 epoch sec; seqNo:3003 2021-02-12T23:21:28Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-12T23:21:28Z- handleDepth()-Depth_Keller msg: 1613172088426 epoch sec; seqNo:3031; depth 7.64 2021-02-12T23:21:28Z- handleDvl() - RDI_Pathfinder msg: 1613172088.45 epoch sec; seqNo:1329 2021-02-12T23:21:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 104.06 , 118.92 , 77.22 2021-02-12T23:21:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.23 , 0.17 2021-02-12T23:21:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172088.46 epoch sec; seqNo:3031; 4056786.38 north; 590826.01 east 2021-02-12T23:21:28Z- handleDepth()-Depth_Keller msg: 1613172088825 epoch sec; seqNo:3032; depth 7.50 2021-02-12T23:21:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172088.84 epoch sec; seqNo:3032; 4056786.76 north; 590826.01 east 2021-02-12T23:21:29Z- AHRS_M2 msg: 1613172089.21 epoch sec; seqNo:3004 2021-02-12T23:21:29Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.20 psi 2021-02-12T23:21:29Z- handleDepth()-Depth_Keller msg: 1613172089219 epoch sec; seqNo:3033; depth 7.37 2021-02-12T23:21:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:29Z- handleDvl() - RDI_Pathfinder msg: 1613172089.22 epoch sec; seqNo:1330 2021-02-12T23:21:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.23 , 0.17 2021-02-12T23:21:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172089.24 epoch sec; seqNo:3033; 4056786.76 north; 590826.01 east 2021-02-12T23:21:29Z- AHRS_M2 msg: 1613172089.61 epoch sec; seqNo:3005 2021-02-12T23:21:29Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.21 psi 2021-02-12T23:21:29Z- handleDepth()-Depth_Keller msg: 1613172089617 epoch sec; seqNo:3034; depth 7.27 2021-02-12T23:21:29Z- handleDvl() - RDI_Pathfinder msg: 1613172089.63 epoch sec; seqNo:1331 2021-02-12T23:21:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.19 , 105.41 , 120.27 , 78.85 2021-02-12T23:21:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.22 , -0.08 2021-02-12T23:21:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172089.64 epoch sec; seqNo:3034; 4056787.14 north; 590826.01 east 2021-02-12T23:21:30Z- AHRS_M2 msg: 1613172090.03 epoch sec; seqNo:3006 2021-02-12T23:21:30Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.22 psi 2021-02-12T23:21:30Z- handleDepth()-Depth_Keller msg: 1613172090033 epoch sec; seqNo:3035; depth 7.14 2021-02-12T23:21:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172090.05 epoch sec; seqNo:3035; 4056787.53 north; 590827.22 east 2021-02-12T23:21:30Z- AHRS_M2 msg: 1613172090.42 epoch sec; seqNo:3007 2021-02-12T23:21:30Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.21 psi 2021-02-12T23:21:30Z- handleDepth()-Depth_Keller msg: 1613172090424 epoch sec; seqNo:3036; depth 7.00 2021-02-12T23:21:30Z- handleDvl() - RDI_Pathfinder msg: 1613172090.45 epoch sec; seqNo:1332 2021-02-12T23:21:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.70 , 109.33 , 121.49 , 78.04 2021-02-12T23:21:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , -0.27 2021-02-12T23:21:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172090.46 epoch sec; seqNo:3036; 4056787.53 north; 590827.22 east 2021-02-12T23:21:30Z- AHRS_M2 msg: 1613172090.82 epoch sec; seqNo:3008 2021-02-12T23:21:30Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.21 psi 2021-02-12T23:21:30Z- handleDepth()-Depth_Keller msg: 1613172090832 epoch sec; seqNo:3037; depth 6.89 2021-02-12T23:21:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172090.84 epoch sec; seqNo:3037; 4056787.91 north; 590827.22 east 2021-02-12T23:21:31Z- LcmTrn::handleMessages() - handling 46 messages took 0.66 ms and 133 calls... 2021-02-12T23:21:31Z- waiting for 1613172088.60, time is 1613172090.84 2021-02-12T23:21:31Z- LcmTrn::updateTrn() >>>> pose time 1613172090.84, meas time 1613172090.45 <<<< 2021-02-12T23:21:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172090.45 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:21:31Z- TerrainNav::motionUpdate() - currEstimate = (4056788, 590827.22, 6.89) 2021-02-12T23:21:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:21:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:21:31Z- TNavBF::Projecting Measurements 2021-02-12T23:21:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:21:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:21:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:21:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:21:31Z- TNavFilter::4 good beams. 2021-02-12T23:21:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:21:31Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:21:31Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:21:31Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:21:31Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:21:31Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:21:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:21:31Z- TNavBF::mmse selected from filter -1 2021-02-12T23:21:31Z- TerrainMapOctree: (vehN, vehE ) = (4056787.65, 590826.95). 2021-02-12T23:21:31Z- TerrainMapOctree: Min Distance = 5624.04. 2021-02-12T23:21:31Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:21:31Z- There is no specific good beam that all particles have in common. 2021-02-12T23:21:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:21:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 12.31 2021-02-12T23:21:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:21:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 19.12 2021-02-12T23:21:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:21:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 23.62 2021-02-12T23:21:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:21:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 9.74 2021-02-12T23:21:31Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:21:31Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:21:31Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:21:31Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:21:31Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:21:31Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:21:31Z- Filter 0 No beams used in subcloud update 2021-02-12T23:21:31Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:21:31Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:21:31Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 774.1 EastVariance: 833.1 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:21:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172090.45 sec. 2021-02-12T23:21:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:21:31Z- TNavBF::mmse selected from filter -1 2021-02-12T23:21:31Z- variances([0], [2], and psi_berg) = 774.080886 833.059538 0.000000 2021-02-12T23:21:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:21:31Z- LcmTrn::publishEstimates() - MLE : 4056766.41 590784.72 6.89 2021-02-12T23:21:31Z- LcmTrn::publishEstimates() - MMSE : 4056787.91 590827.22 6.89 2021-02-12T23:21:31Z- LcmTrn::publishEstimates() - COVAR: 774.08 833.06 0.98 0.00 2021-02-12T23:21:31Z- AHRS_M2 msg: 1613172091.23 epoch sec; seqNo:3009 2021-02-12T23:21:31Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.21 psi 2021-02-12T23:21:31Z- handleDepth()-Depth_Keller msg: 1613172091238 epoch sec; seqNo:3038; depth 6.77 2021-02-12T23:21:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172091.25 epoch sec; seqNo:3038; 4056788.29 north; 590827.22 east 2021-02-12T23:21:31Z- AHRS_M2 msg: 1613172091.63 epoch sec; seqNo:3010 2021-02-12T23:21:31Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.19 psi 2021-02-12T23:21:31Z- handleDepth()-Depth_Keller msg: 1613172091636 epoch sec; seqNo:3039; depth 6.64 2021-02-12T23:21:31Z- handleDvl() - RDI_Pathfinder msg: 1613172091.65 epoch sec; seqNo:1333 2021-02-12T23:21:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.63 , 110.61 , 122.78 , 78.85 2021-02-12T23:21:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , -0.23 2021-02-12T23:21:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172091.67 epoch sec; seqNo:3039; 4056788.29 north; 590827.22 east 2021-02-12T23:21:32Z- AHRS_M2 msg: 1613172092.04 epoch sec; seqNo:3011 2021-02-12T23:21:32Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.18 psi 2021-02-12T23:21:32Z- handleDepth()-Depth_Keller msg: 1613172092045 epoch sec; seqNo:3040; depth 6.51 2021-02-12T23:21:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172092.06 epoch sec; seqNo:3040; 4056788.66 north; 590827.21 east 2021-02-12T23:21:32Z- AHRS_M2 msg: 1613172092.45 epoch sec; seqNo:3012 2021-02-12T23:21:32Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.18 psi 2021-02-12T23:21:32Z- handleDepth()-Depth_Keller msg: 1613172092454 epoch sec; seqNo:3041; depth 6.39 2021-02-12T23:21:32Z- handleDvl() - RDI_Pathfinder msg: 1613172092.47 epoch sec; seqNo:1334 2021-02-12T23:21:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 109.19 , 122.71 , 79.67 2021-02-12T23:21:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.07 , -0.08 2021-02-12T23:21:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172092.48 epoch sec; seqNo:3041; 4056789.04 north; 590827.21 east 2021-02-12T23:21:32Z- AHRS_M2 msg: 1613172092.84 epoch sec; seqNo:3013 2021-02-12T23:21:32Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.16 psi 2021-02-12T23:21:32Z- handleDepth()-Depth_Keller msg: 1613172092854 epoch sec; seqNo:3042; depth 6.26 2021-02-12T23:21:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:32Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:32Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:32Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:32Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:32Z- handleDvl() - RDI_Pathfinder msg: 1613172092.86 epoch sec; seqNo:1335 2021-02-12T23:21:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.07 , -0.08 2021-02-12T23:21:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172092.87 epoch sec; seqNo:3042; 4056789.06 north; 590828.43 east 2021-02-12T23:21:33Z- AHRS_M2 msg: 1613172093.25 epoch sec; seqNo:3014 2021-02-12T23:21:33Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.15 psi 2021-02-12T23:21:33Z- handleDepth()-Depth_Keller msg: 1613172093252 epoch sec; seqNo:3043; depth 6.16 2021-02-12T23:21:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172093.26 epoch sec; seqNo:3043; 4056789.43 north; 590828.42 east 2021-02-12T23:21:33Z- AHRS_M2 msg: 1613172093.65 epoch sec; seqNo:3015 2021-02-12T23:21:33Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.14 psi 2021-02-12T23:21:33Z- handleDepth()-Depth_Keller msg: 1613172093656 epoch sec; seqNo:3044; depth 6.03 2021-02-12T23:21:33Z- handleDvl() - RDI_Pathfinder msg: 1613172093.67 epoch sec; seqNo:1336 2021-02-12T23:21:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 106.49 , 120.00 , 78.44 2021-02-12T23:21:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , 0.00 , 0.00 2021-02-12T23:21:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172093.68 epoch sec; seqNo:3044; 4056789.81 north; 590828.42 east 2021-02-12T23:21:34Z- AHRS_M2 msg: 1613172094.06 epoch sec; seqNo:3016 2021-02-12T23:21:34Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.14 psi 2021-02-12T23:21:34Z- handleDepth()-Depth_Keller msg: 1613172094062 epoch sec; seqNo:3045; depth 5.88 2021-02-12T23:21:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:34Z- handleDvl() - RDI_Pathfinder msg: 1613172094.06 epoch sec; seqNo:1337 2021-02-12T23:21:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , 0.00 , 0.00 2021-02-12T23:21:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172094.08 epoch sec; seqNo:3045; 4056789.81 north; 590828.42 east 2021-02-12T23:21:34Z- handleDepth()-Depth_Keller msg: 1613172094485 epoch sec; seqNo:3046; depth 5.76 2021-02-12T23:21:34Z- handleDvl() - RDI_Pathfinder msg: 1613172094.50 epoch sec; seqNo:1338 2021-02-12T23:21:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.40 , 102.57 , 118.79 , 77.63 2021-02-12T23:21:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.63 , -0.01 , -0.06 2021-02-12T23:21:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172094.51 epoch sec; seqNo:3046; 4056790.19 north; 590828.42 east 2021-02-12T23:21:34Z- AHRS_M2 msg: 1613172094.86 epoch sec; seqNo:3017 2021-02-12T23:21:34Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-12T23:21:34Z- handleDepth()-Depth_Keller msg: 1613172094869 epoch sec; seqNo:3047; depth 5.61 2021-02-12T23:21:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172094.88 epoch sec; seqNo:3047; 4056790.19 north; 590828.42 east 2021-02-12T23:21:35Z- AHRS_M2 msg: 1613172095.27 epoch sec; seqNo:3018 2021-02-12T23:21:35Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.18 psi 2021-02-12T23:21:35Z- handleDepth()-Depth_Keller msg: 1613172095274 epoch sec; seqNo:3048; depth 5.46 2021-02-12T23:21:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:35Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:35Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:35Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:35Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:35Z- handleDvl() - RDI_Pathfinder msg: 1613172095.28 epoch sec; seqNo:1339 2021-02-12T23:21:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.63 , -0.01 , -0.06 2021-02-12T23:21:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172095.28 epoch sec; seqNo:3048; 4056790.57 north; 590828.41 east 2021-02-12T23:21:35Z- AHRS_M2 msg: 1613172095.67 epoch sec; seqNo:3019 2021-02-12T23:21:35Z- AHRS_M2 msg: 0.64 phi; 0.28 theta; -0.19 psi 2021-02-12T23:21:35Z- handleDepth()-Depth_Keller msg: 1613172095676 epoch sec; seqNo:3049; depth 5.32 2021-02-12T23:21:35Z- handleDvl() - RDI_Pathfinder msg: 1613172095.69 epoch sec; seqNo:1340 2021-02-12T23:21:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.12 , 105.34 , 117.50 , 79.26 2021-02-12T23:21:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.18 , -0.11 2021-02-12T23:21:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172095.71 epoch sec; seqNo:3049; 4056790.57 north; 590828.41 east 2021-02-12T23:21:36Z- AHRS_M2 msg: 1613172096.08 epoch sec; seqNo:3020 2021-02-12T23:21:36Z- AHRS_M2 msg: 0.65 phi; 0.25 theta; -0.20 psi 2021-02-12T23:21:36Z- handleDepth()-Depth_Keller msg: 1613172096080 epoch sec; seqNo:3050; depth 5.16 2021-02-12T23:21:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172096.09 epoch sec; seqNo:3050; 4056790.95 north; 590828.41 east 2021-02-12T23:21:36Z- LcmTrn::handleMessages() - handling 46 messages took 0.61 ms and 134 calls... 2021-02-12T23:21:36Z- waiting for 1613172093.84, time is 1613172096.09 2021-02-12T23:21:36Z- LcmTrn::updateTrn() >>>> pose time 1613172096.09, meas time 1613172095.69 <<<< 2021-02-12T23:21:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172095.69 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:21:36Z- TerrainNav::motionUpdate() - currEstimate = (4056791, 590828.41, 5.16) 2021-02-12T23:21:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:21:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:21:36Z- TNavBF::Projecting Measurements 2021-02-12T23:21:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:21:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:21:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:21:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:21:36Z- TNavFilter::4 good beams. 2021-02-12T23:21:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:21:36Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:21:36Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:21:36Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:21:36Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:21:36Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:21:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:21:36Z- TNavBF::mmse selected from filter -1 2021-02-12T23:21:36Z- TerrainMapOctree: (vehN, vehE ) = (4056790.72, 590828.32). 2021-02-12T23:21:36Z- TerrainMapOctree: Min Distance = 5620.77. 2021-02-12T23:21:36Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:21:36Z- There is no specific good beam that all particles have in common. 2021-02-12T23:21:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:21:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 12.71 2021-02-12T23:21:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:21:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 17.75 2021-02-12T23:21:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:21:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 22.09 2021-02-12T23:21:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:21:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 10.05 2021-02-12T23:21:36Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:21:36Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:21:36Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:21:36Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:21:36Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:21:36Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:21:36Z- Filter 0 No beams used in subcloud update 2021-02-12T23:21:36Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:21:36Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:21:36Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 774.0 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:21:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172095.69 sec. 2021-02-12T23:21:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:21:36Z- TNavBF::mmse selected from filter -1 2021-02-12T23:21:36Z- variances([0], [2], and psi_berg) = 773.926483 833.059336 0.000000 2021-02-12T23:21:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:21:36Z- LcmTrn::publishEstimates() - MLE : 4056769.45 590785.91 5.16 2021-02-12T23:21:36Z- LcmTrn::publishEstimates() - MMSE : 4056790.95 590828.41 5.16 2021-02-12T23:21:36Z- LcmTrn::publishEstimates() - COVAR: 773.93 833.06 0.90 0.00 2021-02-12T23:21:36Z- AHRS_M2 msg: 1613172096.48 epoch sec; seqNo:3021 2021-02-12T23:21:36Z- AHRS_M2 msg: 0.65 phi; 0.21 theta; -0.21 psi 2021-02-12T23:21:36Z- handleDepth()-Depth_Keller msg: 1613172096485 epoch sec; seqNo:3051; depth 5.03 2021-02-12T23:21:36Z- handleDvl() - RDI_Pathfinder msg: 1613172096.50 epoch sec; seqNo:1341 2021-02-12T23:21:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.07 , 108.04 , 112.10 , 81.01 2021-02-12T23:21:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.29 , 0.03 2021-02-12T23:21:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172096.51 epoch sec; seqNo:3051; 4056791.33 north; 590828.40 east 2021-02-12T23:21:36Z- AHRS_M2 msg: 1613172096.88 epoch sec; seqNo:3022 2021-02-12T23:21:36Z- AHRS_M2 msg: 0.65 phi; 0.18 theta; -0.21 psi 2021-02-12T23:21:36Z- handleDepth()-Depth_Keller msg: 1613172096890 epoch sec; seqNo:3052; depth 4.89 2021-02-12T23:21:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172096.90 epoch sec; seqNo:3052; 4056791.72 north; 590829.62 east 2021-02-12T23:21:37Z- AHRS_M2 msg: 1613172097.29 epoch sec; seqNo:3023 2021-02-12T23:21:37Z- AHRS_M2 msg: 0.65 phi; 0.14 theta; -0.21 psi 2021-02-12T23:21:37Z- handleDepth()-Depth_Keller msg: 1613172097292 epoch sec; seqNo:3053; depth 4.77 2021-02-12T23:21:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172097.30 epoch sec; seqNo:3053; 4056792.10 north; 590829.62 east 2021-02-12T23:21:37Z- AHRS_M2 msg: 1613172097.69 epoch sec; seqNo:3024 2021-02-12T23:21:37Z- AHRS_M2 msg: 0.66 phi; 0.09 theta; -0.19 psi 2021-02-12T23:21:37Z- handleDepth()-Depth_Keller msg: 1613172097697 epoch sec; seqNo:3054; depth 4.68 2021-02-12T23:21:37Z- handleDvl() - RDI_Pathfinder msg: 1613172097.71 epoch sec; seqNo:1342 2021-02-12T23:21:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.78 , 105.41 , 104.06 , 83.78 2021-02-12T23:21:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.23 , 0.20 2021-02-12T23:21:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172097.73 epoch sec; seqNo:3054; 4056792.48 north; 590829.61 east 2021-02-12T23:21:38Z- AHRS_M2 msg: 1613172098.10 epoch sec; seqNo:3025 2021-02-12T23:21:38Z- AHRS_M2 msg: 0.67 phi; 0.05 theta; -0.18 psi 2021-02-12T23:21:38Z- handleDepth()-Depth_Keller msg: 1613172098105 epoch sec; seqNo:3055; depth 4.58 2021-02-12T23:21:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172098.12 epoch sec; seqNo:3055; 4056792.86 north; 590829.61 east 2021-02-12T23:21:38Z- AHRS_M2 msg: 1613172098.51 epoch sec; seqNo:3026 2021-02-12T23:21:38Z- AHRS_M2 msg: 0.69 phi; -0.00 theta; -0.17 psi 2021-02-12T23:21:38Z- handleDepth()-Depth_Keller msg: 1613172098517 epoch sec; seqNo:3056; depth 4.51 2021-02-12T23:21:38Z- handleDvl() - RDI_Pathfinder msg: 1613172098.53 epoch sec; seqNo:1343 2021-02-12T23:21:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.98 , 102.91 , 96.15 , 88.04 2021-02-12T23:21:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , 0.21 2021-02-12T23:21:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172098.54 epoch sec; seqNo:3056; 4056792.86 north; 590829.61 east 2021-02-12T23:21:38Z- AHRS_M2 msg: 1613172098.90 epoch sec; seqNo:3027 2021-02-12T23:21:38Z- AHRS_M2 msg: 0.70 phi; -0.05 theta; -0.16 psi 2021-02-12T23:21:38Z- handleDepth()-Depth_Keller msg: 1613172098908 epoch sec; seqNo:3057; depth 4.46 2021-02-12T23:21:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:38Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:38Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:38Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:38Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:38Z- handleDvl() - RDI_Pathfinder msg: 1613172098.91 epoch sec; seqNo:1344 2021-02-12T23:21:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , 0.21 2021-02-12T23:21:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172098.93 epoch sec; seqNo:3057; 4056793.24 north; 590829.61 east 2021-02-12T23:21:39Z- AHRS_M2 msg: 1613172099.31 epoch sec; seqNo:3028 2021-02-12T23:21:39Z- AHRS_M2 msg: 0.71 phi; -0.09 theta; -0.15 psi 2021-02-12T23:21:39Z- handleDepth()-Depth_Keller msg: 1613172099316 epoch sec; seqNo:3058; depth 4.43 2021-02-12T23:21:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172099.33 epoch sec; seqNo:3058; 4056793.63 north; 590830.82 east 2021-02-12T23:21:39Z- AHRS_M2 msg: 1613172099.71 epoch sec; seqNo:3029 2021-02-12T23:21:39Z- AHRS_M2 msg: 0.72 phi; -0.13 theta; -0.14 psi 2021-02-12T23:21:39Z- handleDepth()-Depth_Keller msg: 1613172099716 epoch sec; seqNo:3059; depth 4.40 2021-02-12T23:21:39Z- handleDvl() - RDI_Pathfinder msg: 1613172099.73 epoch sec; seqNo:1345 2021-02-12T23:21:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 98.99 , 89.53 , 94.93 2021-02-12T23:21:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , 0.09 , 0.20 2021-02-12T23:21:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172099.74 epoch sec; seqNo:3059; 4056793.63 north; 590830.82 east 2021-02-12T23:21:40Z- AHRS_M2 msg: 1613172100.12 epoch sec; seqNo:3030 2021-02-12T23:21:40Z- AHRS_M2 msg: 0.73 phi; -0.17 theta; -0.14 psi 2021-02-12T23:21:40Z- handleDepth()-Depth_Keller msg: 1613172100126 epoch sec; seqNo:3060; depth 4.39 2021-02-12T23:21:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:40Z- handleDvl() - RDI_Pathfinder msg: 1613172100.13 epoch sec; seqNo:1346 2021-02-12T23:21:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , 0.09 , 0.20 2021-02-12T23:21:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172100.14 epoch sec; seqNo:3060; 4056794.01 north; 590830.82 east 2021-02-12T23:21:40Z- AHRS_M2 msg: 1613172100.52 epoch sec; seqNo:3031 2021-02-12T23:21:40Z- AHRS_M2 msg: 0.73 phi; -0.20 theta; -0.16 psi 2021-02-12T23:21:40Z- handleDepth()-Depth_Keller msg: 1613172100527 epoch sec; seqNo:3061; depth 4.39 2021-02-12T23:21:40Z- handleDvl() - RDI_Pathfinder msg: 1613172100.55 epoch sec; seqNo:1347 2021-02-12T23:21:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.89 , 96.35 , 85.54 , 96.35 2021-02-12T23:21:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.56 , 0.07 , 0.04 2021-02-12T23:21:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172100.56 epoch sec; seqNo:3061; 4056794.01 north; 590830.82 east 2021-02-12T23:21:40Z- AHRS_M2 msg: 1613172100.92 epoch sec; seqNo:3032 2021-02-12T23:21:40Z- AHRS_M2 msg: 0.73 phi; -0.22 theta; -0.19 psi 2021-02-12T23:21:40Z- handleDepth()-Depth_Keller msg: 1613172100928 epoch sec; seqNo:3062; depth 4.41 2021-02-12T23:21:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172100.94 epoch sec; seqNo:3062; 4056794.01 north; 590830.82 east 2021-02-12T23:21:41Z- LcmTrn::handleMessages() - handling 43 messages took 0.61 ms and 131 calls... 2021-02-12T23:21:41Z- waiting for 1613172099.09, time is 1613172100.94 2021-02-12T23:21:41Z- LcmTrn::updateTrn() >>>> pose time 1613172100.94, meas time 1613172100.55 <<<< 2021-02-12T23:21:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172100.55 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:21:41Z- TerrainNav::motionUpdate() - currEstimate = (4056794, 590830.82, 4.41) 2021-02-12T23:21:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:21:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:21:41Z- TNavBF::Projecting Measurements 2021-02-12T23:21:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:21:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:21:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:21:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:21:41Z- TNavFilter::4 good beams. 2021-02-12T23:21:41Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:21:41Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:21:41Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:21:41Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:21:41Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:21:41Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:21:41Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:21:41Z- TNavBF::mmse selected from filter -1 2021-02-12T23:21:41Z- TerrainMapOctree: (vehN, vehE ) = (4056793.76, 590830.62). 2021-02-12T23:21:41Z- TerrainMapOctree: Min Distance = 5616.96. 2021-02-12T23:21:41Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:21:41Z- There is no specific good beam that all particles have in common. 2021-02-12T23:21:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:21:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 12.08 2021-02-12T23:21:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:21:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 14.85 2021-02-12T23:21:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:21:41Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 11.71 2021-02-12T23:21:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:21:41Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 14.85 2021-02-12T23:21:41Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:21:41Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:21:41Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:21:41Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:21:41Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:21:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:21:41Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:21:41Z- Filter 0 No beams used in subcloud update 2021-02-12T23:21:41Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:21:41Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:21:41Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 773.8 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:21:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172100.55 sec. 2021-02-12T23:21:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:21:41Z- TNavBF::mmse selected from filter -1 2021-02-12T23:21:41Z- variances([0], [2], and psi_berg) = 773.757499 833.059116 0.000000 2021-02-12T23:21:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:21:41Z- LcmTrn::publishEstimates() - MLE : 4056772.51 590788.32 4.41 2021-02-12T23:21:41Z- LcmTrn::publishEstimates() - MMSE : 4056794.01 590830.82 4.41 2021-02-12T23:21:41Z- LcmTrn::publishEstimates() - COVAR: 773.76 833.06 0.85 0.00 2021-02-12T23:21:41Z- AHRS_M2 msg: 1613172101.33 epoch sec; seqNo:3033 2021-02-12T23:21:41Z- AHRS_M2 msg: 0.73 phi; -0.24 theta; -0.24 psi 2021-02-12T23:21:41Z- handleDepth()-Depth_Keller msg: 1613172101332 epoch sec; seqNo:3063; depth 4.40 2021-02-12T23:21:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:41Z- handleDvl() - RDI_Pathfinder msg: 1613172101.33 epoch sec; seqNo:1348 2021-02-12T23:21:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.56 , 0.07 , 0.04 2021-02-12T23:21:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172101.35 epoch sec; seqNo:3063; 4056794.39 north; 590830.81 east 2021-02-12T23:21:41Z- AHRS_M2 msg: 1613172101.73 epoch sec; seqNo:3034 2021-02-12T23:21:41Z- AHRS_M2 msg: 0.73 phi; -0.26 theta; -0.26 psi 2021-02-12T23:21:41Z- handleDepth()-Depth_Keller msg: 1613172101738 epoch sec; seqNo:3064; depth 4.42 2021-02-12T23:21:41Z- handleDvl() - RDI_Pathfinder msg: 1613172101.75 epoch sec; seqNo:1349 2021-02-12T23:21:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 104.93 , 84.25 , 111.02 2021-02-12T23:21:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , -0.12 , 0.10 2021-02-12T23:21:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172101.76 epoch sec; seqNo:3064; 4056794.39 north; 590830.81 east 2021-02-12T23:21:42Z- AHRS_M2 msg: 1613172102.14 epoch sec; seqNo:3035 2021-02-12T23:21:42Z- AHRS_M2 msg: 0.73 phi; -0.28 theta; -0.27 psi 2021-02-12T23:21:42Z- handleDepth()-Depth_Keller msg: 1613172102151 epoch sec; seqNo:3065; depth 4.46 2021-02-12T23:21:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172102.16 epoch sec; seqNo:3065; 4056794.76 north; 590830.81 east 2021-02-12T23:21:42Z- AHRS_M2 msg: 1613172102.54 epoch sec; seqNo:3036 2021-02-12T23:21:42Z- AHRS_M2 msg: 0.72 phi; -0.31 theta; -0.26 psi 2021-02-12T23:21:42Z- handleDepth()-Depth_Keller msg: 1613172102545 epoch sec; seqNo:3066; depth 4.53 2021-02-12T23:21:42Z- handleDvl() - RDI_Pathfinder msg: 1613172102.56 epoch sec; seqNo:1350 2021-02-12T23:21:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.13 , 114.87 , 83.78 , 117.57 2021-02-12T23:21:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , 0.02 , 0.44 2021-02-12T23:21:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172102.57 epoch sec; seqNo:3066; 4056794.78 north; 590832.03 east 2021-02-12T23:21:42Z- AHRS_M2 msg: 1613172102.94 epoch sec; seqNo:3037 2021-02-12T23:21:42Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.24 psi 2021-02-12T23:21:42Z- handleDepth()-Depth_Keller msg: 1613172102949 epoch sec; seqNo:3067; depth 4.60 2021-02-12T23:21:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172102.96 epoch sec; seqNo:3067; 4056794.78 north; 590832.03 east 2021-02-12T23:21:43Z- AHRS_M2 msg: 1613172103.35 epoch sec; seqNo:3038 2021-02-12T23:21:43Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.23 psi 2021-02-12T23:21:43Z- handleDepth()-Depth_Keller msg: 1613172103357 epoch sec; seqNo:3068; depth 4.67 2021-02-12T23:21:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172103.37 epoch sec; seqNo:3068; 4056795.16 north; 590832.03 east 2021-02-12T23:21:43Z- AHRS_M2 msg: 1613172103.76 epoch sec; seqNo:3039 2021-02-12T23:21:43Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.23 psi 2021-02-12T23:21:43Z- handleDepth()-Depth_Keller msg: 1613172103762 epoch sec; seqNo:3069; depth 4.75 2021-02-12T23:21:43Z- handleDvl() - RDI_Pathfinder msg: 1613172103.79 epoch sec; seqNo:1351 2021-02-12T23:21:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.50 , 111.83 , 82.09 , 123.99 2021-02-12T23:21:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.40 , 0.32 2021-02-12T23:21:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172103.80 epoch sec; seqNo:3069; 4056795.16 north; 590832.03 east 2021-02-12T23:21:44Z- AHRS_M2 msg: 1613172104.16 epoch sec; seqNo:3040 2021-02-12T23:21:44Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.25 psi 2021-02-12T23:21:44Z- handleDepth()-Depth_Keller msg: 1613172104160 epoch sec; seqNo:3070; depth 4.83 2021-02-12T23:21:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172104.17 epoch sec; seqNo:3070; 4056795.16 north; 590832.03 east 2021-02-12T23:21:44Z- AHRS_M2 msg: 1613172104.56 epoch sec; seqNo:3041 2021-02-12T23:21:44Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.26 psi 2021-02-12T23:21:44Z- handleDepth()-Depth_Keller msg: 1613172104566 epoch sec; seqNo:3071; depth 4.92 2021-02-12T23:21:44Z- handleDvl() - RDI_Pathfinder msg: 1613172104.58 epoch sec; seqNo:1352 2021-02-12T23:21:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.97 , 112.84 , 79.26 , 124.74 2021-02-12T23:21:44Z- handleDvl() - RDI_Pathfinder msg: velocities 1.10 , 0.43 , -0.04 2021-02-12T23:21:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172104.59 epoch sec; seqNo:3071; 4056795.55 north; 590833.24 east 2021-02-12T23:21:44Z- AHRS_M2 msg: 1613172104.96 epoch sec; seqNo:3042 2021-02-12T23:21:44Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.27 psi 2021-02-12T23:21:44Z- handleDepth()-Depth_Keller msg: 1613172104970 epoch sec; seqNo:3072; depth 5.03 2021-02-12T23:21:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:44Z- handleDvl() - RDI_Pathfinder msg: 1613172104.97 epoch sec; seqNo:1353 2021-02-12T23:21:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:44Z- handleDvl() - RDI_Pathfinder msg: velocities 1.10 , 0.43 , -0.04 2021-02-12T23:21:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172104.98 epoch sec; seqNo:3072; 4056795.55 north; 590833.24 east 2021-02-12T23:21:45Z- AHRS_M2 msg: 1613172105.40 epoch sec; seqNo:3043 2021-02-12T23:21:45Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.28 psi 2021-02-12T23:21:45Z- handleDepth()-Depth_Keller msg: 1613172105402 epoch sec; seqNo:3073; depth 5.12 2021-02-12T23:21:45Z- handleDvl() - RDI_Pathfinder msg: 1613172105.41 epoch sec; seqNo:1354 2021-02-12T23:21:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.88 , 115.61 , 82.91 , 123.04 2021-02-12T23:21:45Z- handleDvl() - RDI_Pathfinder msg: velocities 1.18 , 0.21 , -0.24 2021-02-12T23:21:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172105.42 epoch sec; seqNo:3073; 4056795.93 north; 590833.24 east 2021-02-12T23:21:45Z- handleDepth()-Depth_Keller msg: 1613172105793 epoch sec; seqNo:3074; depth 5.26 2021-02-12T23:21:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172105.81 epoch sec; seqNo:3074; 4056796.32 north; 590834.46 east 2021-02-12T23:21:46Z- AHRS_M2 msg: 1613172106.19 epoch sec; seqNo:3044 2021-02-12T23:21:46Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.27 psi 2021-02-12T23:21:46Z- handleDepth()-Depth_Keller msg: 1613172106225 epoch sec; seqNo:3075; depth 5.37 2021-02-12T23:21:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:46Z- handleDvl() - RDI_Pathfinder msg: 1613172106.23 epoch sec; seqNo:1355 2021-02-12T23:21:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:46Z- handleDvl() - RDI_Pathfinder msg: velocities 1.18 , 0.21 , -0.24 2021-02-12T23:21:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172106.24 epoch sec; seqNo:3075; 4056796.70 north; 590834.45 east 2021-02-12T23:21:46Z- LcmTrn::handleMessages() - handling 46 messages took 0.72 ms and 132 calls... 2021-02-12T23:21:46Z- waiting for 1613172103.94, time is 1613172106.24 2021-02-12T23:21:46Z- LcmTrn::updateTrn() equating pose 1613172106.24 and meas times 1613172106.24 2021-02-12T23:21:46Z- LcmTrn::updateTrn() >>>> pose time 1613172106.24, meas time 1613172106.24 <<<< 2021-02-12T23:21:46Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172106.24, ping # 0. 2021-02-12T23:21:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172106.24 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:21:46Z- TerrainNav::motionUpdate() - currEstimate = (4056797, 590834.45, 5.37) 2021-02-12T23:21:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:21:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:21:46Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:21:46Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:21:46Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172106.24 sec.; No relevant navigation data available 2021-02-12T23:21:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:21:46Z- TNavBF::mmse selected from filter -1 2021-02-12T23:21:46Z- variances([0], [2], and psi_berg) = 773.611262 833.058926 0.000000 2021-02-12T23:21:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:21:46Z- LcmTrn::publishEstimates() - MLE : 4056776.20 590785.95 5.37 2021-02-12T23:21:46Z- LcmTrn::publishEstimates() - MMSE : 4056796.70 590834.45 5.37 2021-02-12T23:21:46Z- LcmTrn::publishEstimates() - COVAR: 773.61 833.06 1.09 0.00 2021-02-12T23:21:46Z- AHRS_M2 msg: 1613172106.58 epoch sec; seqNo:3045 2021-02-12T23:21:46Z- AHRS_M2 msg: 0.69 phi; -0.33 theta; -0.27 psi 2021-02-12T23:21:46Z- handleDepth()-Depth_Keller msg: 1613172106590 epoch sec; seqNo:3076; depth 5.52 2021-02-12T23:21:46Z- handleDvl() - RDI_Pathfinder msg: 1613172106.60 epoch sec; seqNo:1356 2021-02-12T23:21:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.02 , 117.23 , 83.04 , 121.69 2021-02-12T23:21:46Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , -0.00 , -0.35 2021-02-12T23:21:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172106.61 epoch sec; seqNo:3076; 4056797.08 north; 590834.45 east 2021-02-12T23:21:46Z- AHRS_M2 msg: 1613172106.98 epoch sec; seqNo:3046 2021-02-12T23:21:46Z- AHRS_M2 msg: 0.70 phi; -0.34 theta; -0.26 psi 2021-02-12T23:21:46Z- handleDepth()-Depth_Keller msg: 1613172106988 epoch sec; seqNo:3077; depth 5.63 2021-02-12T23:21:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172107.00 epoch sec; seqNo:3077; 4056797.46 north; 590834.45 east 2021-02-12T23:21:47Z- AHRS_M2 msg: 1613172107.39 epoch sec; seqNo:3047 2021-02-12T23:21:47Z- AHRS_M2 msg: 0.70 phi; -0.35 theta; -0.25 psi 2021-02-12T23:21:47Z- handleDepth()-Depth_Keller msg: 1613172107397 epoch sec; seqNo:3078; depth 5.75 2021-02-12T23:21:47Z- handleDvl() - RDI_Pathfinder msg: 1613172107.41 epoch sec; seqNo:1357 2021-02-12T23:21:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.53 , 114.26 , 79.25 , 121.69 2021-02-12T23:21:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.19 , -0.48 2021-02-12T23:21:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172107.42 epoch sec; seqNo:3078; 4056797.83 north; 590834.44 east 2021-02-12T23:21:47Z- AHRS_M2 msg: 1613172107.79 epoch sec; seqNo:3048 2021-02-12T23:21:47Z- AHRS_M2 msg: 0.70 phi; -0.36 theta; -0.25 psi 2021-02-12T23:21:47Z- handleDepth()-Depth_Keller msg: 1613172107797 epoch sec; seqNo:3079; depth 5.88 2021-02-12T23:21:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172107.81 epoch sec; seqNo:3079; 4056798.23 north; 590835.66 east 2021-02-12T23:21:48Z- AHRS_M2 msg: 1613172108.19 epoch sec; seqNo:3049 2021-02-12T23:21:48Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.25 psi 2021-02-12T23:21:48Z- handleDepth()-Depth_Keller msg: 1613172108200 epoch sec; seqNo:3080; depth 6.00 2021-02-12T23:21:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172108.21 epoch sec; seqNo:3080; 4056798.60 north; 590835.65 east 2021-02-12T23:21:48Z- AHRS_M2 msg: 1613172108.60 epoch sec; seqNo:3050 2021-02-12T23:21:48Z- AHRS_M2 msg: 0.70 phi; -0.39 theta; -0.25 psi 2021-02-12T23:21:48Z- handleDepth()-Depth_Keller msg: 1613172108613 epoch sec; seqNo:3081; depth 6.12 2021-02-12T23:21:48Z- handleDvl() - RDI_Pathfinder msg: 1613172108.62 epoch sec; seqNo:1358 2021-02-12T23:21:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.52 , 115.95 , 79.66 , 122.71 2021-02-12T23:21:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.60 , -0.44 , -0.40 2021-02-12T23:21:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172108.63 epoch sec; seqNo:3081; 4056798.98 north; 590835.65 east 2021-02-12T23:21:49Z- AHRS_M2 msg: 1613172109.01 epoch sec; seqNo:3051 2021-02-12T23:21:49Z- AHRS_M2 msg: 0.69 phi; -0.40 theta; -0.23 psi 2021-02-12T23:21:49Z- handleDepth()-Depth_Keller msg: 1613172109010 epoch sec; seqNo:3082; depth 6.23 2021-02-12T23:21:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172109.02 epoch sec; seqNo:3082; 4056799.36 north; 590835.65 east 2021-02-12T23:21:49Z- AHRS_M2 msg: 1613172109.43 epoch sec; seqNo:3052 2021-02-12T23:21:49Z- AHRS_M2 msg: 0.68 phi; -0.40 theta; -0.22 psi 2021-02-12T23:21:49Z- handleDepth()-Depth_Keller msg: 1613172109439 epoch sec; seqNo:3083; depth 6.35 2021-02-12T23:21:49Z- handleDvl() - RDI_Pathfinder msg: 1613172109.45 epoch sec; seqNo:1359 2021-02-12T23:21:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.15 , 114.53 , 79.39 , 126.69 2021-02-12T23:21:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.60 , -0.49 , 0.07 2021-02-12T23:21:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172109.46 epoch sec; seqNo:3083; 4056799.36 north; 590835.65 east 2021-02-12T23:21:49Z- AHRS_M2 msg: 1613172109.81 epoch sec; seqNo:3053 2021-02-12T23:21:49Z- AHRS_M2 msg: 0.67 phi; -0.40 theta; -0.20 psi 2021-02-12T23:21:49Z- handleDepth()-Depth_Keller msg: 1613172109821 epoch sec; seqNo:3084; depth 6.49 2021-02-12T23:21:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:49Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:49Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:49Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:49Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:49Z- handleDvl() - RDI_Pathfinder msg: 1613172109.82 epoch sec; seqNo:1360 2021-02-12T23:21:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.60 , -0.49 , 0.07 2021-02-12T23:21:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172109.85 epoch sec; seqNo:3084; 4056799.74 north; 590835.64 east 2021-02-12T23:21:50Z- AHRS_M2 msg: 1613172110.22 epoch sec; seqNo:3054 2021-02-12T23:21:50Z- AHRS_M2 msg: 0.66 phi; -0.40 theta; -0.19 psi 2021-02-12T23:21:50Z- handleDepth()-Depth_Keller msg: 1613172110220 epoch sec; seqNo:3085; depth 6.63 2021-02-12T23:21:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172110.23 epoch sec; seqNo:3085; 4056800.12 north; 590835.64 east 2021-02-12T23:21:50Z- AHRS_M2 msg: 1613172110.62 epoch sec; seqNo:3055 2021-02-12T23:21:50Z- AHRS_M2 msg: 0.65 phi; -0.40 theta; -0.17 psi 2021-02-12T23:21:50Z- handleDepth()-Depth_Keller msg: 1613172110629 epoch sec; seqNo:3086; depth 6.77 2021-02-12T23:21:50Z- handleDvl() - RDI_Pathfinder msg: 1613172110.64 epoch sec; seqNo:1361 2021-02-12T23:21:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 111.35 , 80.13 , 129.20 2021-02-12T23:21:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.20 , 0.44 2021-02-12T23:21:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172110.65 epoch sec; seqNo:3086; 4056800.12 north; 590835.64 east 2021-02-12T23:21:51Z- AHRS_M2 msg: 1613172111.02 epoch sec; seqNo:3056 2021-02-12T23:21:51Z- AHRS_M2 msg: 0.64 phi; -0.39 theta; -0.17 psi 2021-02-12T23:21:51Z- handleDepth()-Depth_Keller msg: 1613172111028 epoch sec; seqNo:3087; depth 6.92 2021-02-12T23:21:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:51Z- handleDvl() - RDI_Pathfinder msg: 1613172111.03 epoch sec; seqNo:1362 2021-02-12T23:21:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.20 , 0.44 2021-02-12T23:21:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172111.05 epoch sec; seqNo:3087; 4056800.50 north; 590835.63 east 2021-02-12T23:21:51Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-12T23:21:51Z- waiting for 1613172109.24, time is 1613172111.05 2021-02-12T23:21:51Z- LcmTrn::updateTrn() equating pose 1613172111.05 and meas times 1613172111.05 2021-02-12T23:21:51Z- LcmTrn::updateTrn() >>>> pose time 1613172111.05, meas time 1613172111.05 <<<< 2021-02-12T23:21:51Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172111.05, ping # 0. 2021-02-12T23:21:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172111.05 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:21:51Z- TerrainNav::motionUpdate() - currEstimate = (4056800, 590835.63, 6.92) 2021-02-12T23:21:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:21:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:21:51Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:21:51Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:21:51Z- TNavBF::Projecting Measurements 2021-02-12T23:21:51Z- TNavFilter::Measurement from time = 1613172111.05 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:21:51Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:21:51Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:21:51Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:21:51Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:21:51Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:21:51Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:21:51Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:21:51Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 773.5 EastVariance: 833.1 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-12T23:21:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:21:51Z- TNavBF::mmse selected from filter -1 2021-02-12T23:21:51Z- variances([0], [2], and psi_berg) = 773.474390 833.058749 0.000000 2021-02-12T23:21:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:21:51Z- LcmTrn::publishEstimates() - MLE : 4056780.00 590787.13 6.92 2021-02-12T23:21:51Z- LcmTrn::publishEstimates() - MMSE : 4056800.50 590835.63 6.92 2021-02-12T23:21:51Z- LcmTrn::publishEstimates() - COVAR: 773.47 833.06 1.29 0.00 2021-02-12T23:21:51Z- AHRS_M2 msg: 1613172111.43 epoch sec; seqNo:3057 2021-02-12T23:21:51Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.17 psi 2021-02-12T23:21:51Z- handleDepth()-Depth_Keller msg: 1613172111433 epoch sec; seqNo:3088; depth 7.04 2021-02-12T23:21:51Z- handleDvl() - RDI_Pathfinder msg: 1613172111.44 epoch sec; seqNo:1363 2021-02-12T23:21:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 106.83 , 75.99 , 127.64 2021-02-12T23:21:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.12 , 0.32 2021-02-12T23:21:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172111.47 epoch sec; seqNo:3088; 4056800.50 north; 590835.63 east 2021-02-12T23:21:51Z- AHRS_M2 msg: 1613172111.83 epoch sec; seqNo:3058 2021-02-12T23:21:51Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.19 psi 2021-02-12T23:21:51Z- handleDepth()-Depth_Keller msg: 1613172111836 epoch sec; seqNo:3089; depth 7.20 2021-02-12T23:21:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172111.85 epoch sec; seqNo:3089; 4056800.88 north; 590835.63 east 2021-02-12T23:21:52Z- AHRS_M2 msg: 1613172112.24 epoch sec; seqNo:3059 2021-02-12T23:21:52Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.20 psi 2021-02-12T23:21:52Z- handleDepth()-Depth_Keller msg: 1613172112245 epoch sec; seqNo:3090; depth 7.35 2021-02-12T23:21:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:52Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:52Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:52Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:52Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:52Z- handleDvl() - RDI_Pathfinder msg: 1613172112.25 epoch sec; seqNo:1364 2021-02-12T23:21:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.12 , 0.32 2021-02-12T23:21:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172112.26 epoch sec; seqNo:3090; 4056800.88 north; 590835.63 east 2021-02-12T23:21:52Z- AHRS_M2 msg: 1613172112.64 epoch sec; seqNo:3060 2021-02-12T23:21:52Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.21 psi 2021-02-12T23:21:52Z- handleDepth()-Depth_Keller msg: 1613172112644 epoch sec; seqNo:3091; depth 7.49 2021-02-12T23:21:52Z- handleDvl() - RDI_Pathfinder msg: 1613172112.66 epoch sec; seqNo:1365 2021-02-12T23:21:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 109.19 , 77.63 , 122.71 2021-02-12T23:21:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.20 , -0.04 2021-02-12T23:21:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172112.67 epoch sec; seqNo:3091; 4056801.27 north; 590836.85 east 2021-02-12T23:21:53Z- AHRS_M2 msg: 1613172113.04 epoch sec; seqNo:3061 2021-02-12T23:21:53Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.21 psi 2021-02-12T23:21:53Z- handleDepth()-Depth_Keller msg: 1613172113048 epoch sec; seqNo:3092; depth 7.63 2021-02-12T23:21:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172113.06 epoch sec; seqNo:3092; 4056801.27 north; 590836.85 east 2021-02-12T23:21:53Z- AHRS_M2 msg: 1613172113.45 epoch sec; seqNo:3062 2021-02-12T23:21:53Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.21 psi 2021-02-12T23:21:53Z- handleDepth()-Depth_Keller msg: 1613172113457 epoch sec; seqNo:3093; depth 7.77 2021-02-12T23:21:53Z- handleDvl() - RDI_Pathfinder msg: 1613172113.47 epoch sec; seqNo:1366 2021-02-12T23:21:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.93 , 110.61 , 77.63 , 118.72 2021-02-12T23:21:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , -0.17 2021-02-12T23:21:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172113.48 epoch sec; seqNo:3093; 4056801.65 north; 590836.84 east 2021-02-12T23:21:53Z- AHRS_M2 msg: 1613172113.85 epoch sec; seqNo:3063 2021-02-12T23:21:53Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.22 psi 2021-02-12T23:21:53Z- handleDepth()-Depth_Keller msg: 1613172113858 epoch sec; seqNo:3094; depth 7.91 2021-02-12T23:21:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172113.87 epoch sec; seqNo:3094; 4056802.03 north; 590836.84 east 2021-02-12T23:21:54Z- AHRS_M2 msg: 1613172114.26 epoch sec; seqNo:3064 2021-02-12T23:21:54Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.21 psi 2021-02-12T23:21:54Z- handleDepth()-Depth_Keller msg: 1613172114260 epoch sec; seqNo:3095; depth 8.05 2021-02-12T23:21:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172114.28 epoch sec; seqNo:3095; 4056802.41 north; 590836.84 east 2021-02-12T23:21:54Z- AHRS_M2 msg: 1613172114.66 epoch sec; seqNo:3065 2021-02-12T23:21:54Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.20 psi 2021-02-12T23:21:54Z- handleDepth()-Depth_Keller msg: 1613172114664 epoch sec; seqNo:3096; depth 8.18 2021-02-12T23:21:54Z- handleDvl() - RDI_Pathfinder msg: 1613172114.68 epoch sec; seqNo:1367 2021-02-12T23:21:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.29 , 106.62 , 77.63 , 114.73 2021-02-12T23:21:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.12 , 0.00 2021-02-12T23:21:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172114.70 epoch sec; seqNo:3096; 4056802.78 north; 590836.83 east 2021-02-12T23:21:55Z- AHRS_M2 msg: 1613172115.06 epoch sec; seqNo:3066 2021-02-12T23:21:55Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.20 psi 2021-02-12T23:21:55Z- handleDepth()-Depth_Keller msg: 1613172115068 epoch sec; seqNo:3097; depth 8.31 2021-02-12T23:21:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172115.08 epoch sec; seqNo:3097; 4056802.78 north; 590836.83 east 2021-02-12T23:21:55Z- AHRS_M2 msg: 1613172115.47 epoch sec; seqNo:3067 2021-02-12T23:21:55Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.20 psi 2021-02-12T23:21:55Z- handleDepth()-Depth_Keller msg: 1613172115481 epoch sec; seqNo:3098; depth 8.43 2021-02-12T23:21:55Z- handleDvl() - RDI_Pathfinder msg: 1613172115.49 epoch sec; seqNo:1368 2021-02-12T23:21:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 104.19 , 77.63 , 113.65 2021-02-12T23:21:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.00 , 0.18 2021-02-12T23:21:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172115.50 epoch sec; seqNo:3098; 4056803.18 north; 590838.05 east 2021-02-12T23:21:55Z- AHRS_M2 msg: 1613172115.87 epoch sec; seqNo:3068 2021-02-12T23:21:55Z- AHRS_M2 msg: 0.57 phi; -0.29 theta; -0.20 psi 2021-02-12T23:21:55Z- handleDepth()-Depth_Keller msg: 1613172115878 epoch sec; seqNo:3099; depth 8.56 2021-02-12T23:21:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:55Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:55Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:55Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:55Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:55Z- handleDvl() - RDI_Pathfinder msg: 1613172115.88 epoch sec; seqNo:1369 2021-02-12T23:21:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.00 , 0.18 2021-02-12T23:21:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172115.90 epoch sec; seqNo:3099; 4056803.56 north; 590838.04 east 2021-02-12T23:21:56Z- AHRS_M2 msg: 1613172116.30 epoch sec; seqNo:3069 2021-02-12T23:21:56Z- AHRS_M2 msg: 0.56 phi; -0.29 theta; -0.20 psi 2021-02-12T23:21:56Z- handleDepth()-Depth_Keller msg: 1613172116312 epoch sec; seqNo:3100; depth 8.67 2021-02-12T23:21:56Z- handleDvl() - RDI_Pathfinder msg: 1613172116.32 epoch sec; seqNo:1370 2021-02-12T23:21:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 104.19 , 77.22 , 113.65 2021-02-12T23:21:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.16 , 0.17 2021-02-12T23:21:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172116.34 epoch sec; seqNo:3100; 4056803.56 north; 590838.04 east 2021-02-12T23:21:56Z- LcmTrn::handleMessages() - handling 47 messages took 0.66 ms and 134 calls... 2021-02-12T23:21:56Z- waiting for 1613172114.05, time is 1613172116.34 2021-02-12T23:21:56Z- LcmTrn::updateTrn() equating pose 1613172116.34 and meas times 1613172116.34 2021-02-12T23:21:56Z- LcmTrn::updateTrn() >>>> pose time 1613172116.34, meas time 1613172116.34 <<<< 2021-02-12T23:21:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172116.34 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:21:56Z- TerrainNav::motionUpdate() - currEstimate = (4056804, 590838.04, 8.67) 2021-02-12T23:21:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:21:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:21:56Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172116.34 sec.; No relevant navigation data available 2021-02-12T23:21:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:21:56Z- TNavBF::mmse selected from filter -1 2021-02-12T23:21:56Z- variances([0], [2], and psi_berg) = 773.339340 833.058574 0.000000 2021-02-12T23:21:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:21:56Z- LcmTrn::publishEstimates() - MLE : 4056783.06 590789.54 8.67 2021-02-12T23:21:56Z- LcmTrn::publishEstimates() - MMSE : 4056803.56 590838.04 8.67 2021-02-12T23:21:56Z- LcmTrn::publishEstimates() - COVAR: 773.34 833.06 1.49 0.00 2021-02-12T23:21:56Z- AHRS_M2 msg: 1613172116.68 epoch sec; seqNo:3070 2021-02-12T23:21:56Z- AHRS_M2 msg: 0.56 phi; -0.28 theta; -0.20 psi 2021-02-12T23:21:56Z- handleDepth()-Depth_Keller msg: 1613172116684 epoch sec; seqNo:3101; depth 8.79 2021-02-12T23:21:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172116.69 epoch sec; seqNo:3101; 4056803.93 north; 590838.04 east 2021-02-12T23:21:57Z- AHRS_M2 msg: 1613172117.09 epoch sec; seqNo:3071 2021-02-12T23:21:57Z- AHRS_M2 msg: 0.56 phi; -0.27 theta; -0.19 psi 2021-02-12T23:21:57Z- handleDepth()-Depth_Keller msg: 1613172117089 epoch sec; seqNo:3102; depth 8.88 2021-02-12T23:21:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:21:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:21:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:21:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:21:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:21:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:21:57Z- handleDvl() - RDI_Pathfinder msg: 1613172117.09 epoch sec; seqNo:1371 2021-02-12T23:21:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:21:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.16 , 0.17 2021-02-12T23:21:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172117.10 epoch sec; seqNo:3102; 4056803.93 north; 590838.04 east 2021-02-12T23:21:57Z- AHRS_M2 msg: 1613172117.49 epoch sec; seqNo:3072 2021-02-12T23:21:57Z- AHRS_M2 msg: 0.56 phi; -0.26 theta; -0.18 psi 2021-02-12T23:21:57Z- handleDepth()-Depth_Keller msg: 1613172117492 epoch sec; seqNo:3103; depth 8.99 2021-02-12T23:21:57Z- handleDvl() - RDI_Pathfinder msg: 1613172117.51 epoch sec; seqNo:1372 2021-02-12T23:21:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.63 , 104.26 , 77.63 , 111.02 2021-02-12T23:21:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.22 , 0.02 2021-02-12T23:21:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172117.52 epoch sec; seqNo:3103; 4056804.31 north; 590838.04 east 2021-02-12T23:21:57Z- AHRS_M2 msg: 1613172117.89 epoch sec; seqNo:3073 2021-02-12T23:21:57Z- AHRS_M2 msg: 0.57 phi; -0.25 theta; -0.17 psi 2021-02-12T23:21:57Z- handleDepth()-Depth_Keller msg: 1613172117896 epoch sec; seqNo:3104; depth 9.10 2021-02-12T23:21:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172117.91 epoch sec; seqNo:3104; 4056804.70 north; 590839.25 east 2021-02-12T23:21:58Z- AHRS_M2 msg: 1613172118.31 epoch sec; seqNo:3074 2021-02-12T23:21:58Z- AHRS_M2 msg: 0.58 phi; -0.25 theta; -0.16 psi 2021-02-12T23:21:58Z- handleDepth()-Depth_Keller msg: 1613172118310 epoch sec; seqNo:3105; depth 9.19 2021-02-12T23:21:58Z- handleDvl() - RDI_Pathfinder msg: 1613172118.32 epoch sec; seqNo:1373 2021-02-12T23:21:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.05 , 96.22 , 78.04 , 107.03 2021-02-12T23:21:58Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.16 , -0.16 2021-02-12T23:21:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172118.35 epoch sec; seqNo:3105; 4056805.08 north; 590839.25 east 2021-02-12T23:21:58Z- AHRS_M2 msg: 1613172118.70 epoch sec; seqNo:3075 2021-02-12T23:21:58Z- AHRS_M2 msg: 0.58 phi; -0.24 theta; -0.15 psi 2021-02-12T23:21:58Z- handleDepth()-Depth_Keller msg: 1613172118709 epoch sec; seqNo:3106; depth 9.30 2021-02-12T23:21:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172118.72 epoch sec; seqNo:3106; 4056805.08 north; 590839.25 east 2021-02-12T23:21:59Z- AHRS_M2 msg: 1613172119.10 epoch sec; seqNo:3076 2021-02-12T23:21:59Z- AHRS_M2 msg: 0.59 phi; -0.24 theta; -0.15 psi 2021-02-12T23:21:59Z- handleDepth()-Depth_Keller msg: 1613172119108 epoch sec; seqNo:3107; depth 9.42 2021-02-12T23:21:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172119.13 epoch sec; seqNo:3107; 4056805.46 north; 590839.25 east 2021-02-12T23:21:59Z- AHRS_M2 msg: 1613172119.51 epoch sec; seqNo:3077 2021-02-12T23:21:59Z- AHRS_M2 msg: 0.59 phi; -0.24 theta; -0.15 psi 2021-02-12T23:21:59Z- handleDepth()-Depth_Keller msg: 1613172119515 epoch sec; seqNo:3108; depth 9.51 2021-02-12T23:21:59Z- handleDvl() - RDI_Pathfinder msg: 1613172119.54 epoch sec; seqNo:1374 2021-02-12T23:21:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 96.42 , 77.22 , 104.93 2021-02-12T23:21:59Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.02 , -0.21 2021-02-12T23:21:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172119.55 epoch sec; seqNo:3108; 4056805.85 north; 590840.46 east 2021-02-12T23:21:59Z- AHRS_M2 msg: 1613172119.92 epoch sec; seqNo:3078 2021-02-12T23:21:59Z- AHRS_M2 msg: 0.60 phi; -0.24 theta; -0.15 psi 2021-02-12T23:21:59Z- handleDepth()-Depth_Keller msg: 1613172119924 epoch sec; seqNo:3109; depth 9.59 2021-02-12T23:21:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172119.94 epoch sec; seqNo:3109; 4056806.23 north; 590840.46 east 2021-02-12T23:22:00Z- AHRS_M2 msg: 1613172120.34 epoch sec; seqNo:3079 2021-02-12T23:22:00Z- AHRS_M2 msg: 0.60 phi; -0.24 theta; -0.14 psi 2021-02-12T23:22:00Z- handleDepth()-Depth_Keller msg: 1613172120346 epoch sec; seqNo:3110; depth 9.68 2021-02-12T23:22:00Z- handleDvl() - RDI_Pathfinder msg: 1613172120.36 epoch sec; seqNo:1375 2021-02-12T23:22:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 96.55 , 77.22 , 106.29 2021-02-12T23:22:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.08 , -0.14 2021-02-12T23:22:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172120.37 epoch sec; seqNo:3110; 4056806.61 north; 590840.45 east 2021-02-12T23:22:00Z- AHRS_M2 msg: 1613172120.72 epoch sec; seqNo:3080 2021-02-12T23:22:00Z- AHRS_M2 msg: 0.60 phi; -0.25 theta; -0.14 psi 2021-02-12T23:22:00Z- handleDepth()-Depth_Keller msg: 1613172120728 epoch sec; seqNo:3111; depth 9.78 2021-02-12T23:22:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:00Z- handleDvl() - RDI_Pathfinder msg: 1613172120.73 epoch sec; seqNo:1376 2021-02-12T23:22:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.08 , -0.14 2021-02-12T23:22:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172120.74 epoch sec; seqNo:3111; 4056806.99 north; 590840.45 east 2021-02-12T23:22:01Z- AHRS_M2 msg: 1613172121.13 epoch sec; seqNo:3081 2021-02-12T23:22:01Z- AHRS_M2 msg: 0.60 phi; -0.25 theta; -0.15 psi 2021-02-12T23:22:01Z- handleDepth()-Depth_Keller msg: 1613172121137 epoch sec; seqNo:3112; depth 9.87 2021-02-12T23:22:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172121.15 epoch sec; seqNo:3112; 4056807.37 north; 590840.45 east 2021-02-12T23:22:01Z- LcmTrn::handleMessages() - handling 42 messages took 0.70 ms and 128 calls... 2021-02-12T23:22:01Z- waiting for 1613172119.34, time is 1613172121.15 2021-02-12T23:22:01Z- LcmTrn::updateTrn() >>>> pose time 1613172121.15, meas time 1613172120.73 <<<< 2021-02-12T23:22:01Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172120.73, ping # 0. 2021-02-12T23:22:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172120.73 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:22:01Z- TerrainNav::motionUpdate() - currEstimate = (4056807, 590840.45, 9.87) 2021-02-12T23:22:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:22:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:22:01Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:22:01Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:22:01Z- TNavBF::Projecting Measurements 2021-02-12T23:22:01Z- TNavFilter::Measurement from time = 1613172120.73 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:22:01Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:22:01Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:22:01Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:22:01Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:22:01Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:22:01Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:22:01Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:22:01Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 773.2 EastVariance: 833.1 DepthVariance: 1.7 this->DepthBiasCov: 1.7 2021-02-12T23:22:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:22:01Z- TNavBF::mmse selected from filter -1 2021-02-12T23:22:01Z- variances([0], [2], and psi_berg) = 773.158126 833.058340 0.000000 2021-02-12T23:22:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:22:01Z- LcmTrn::publishEstimates() - MLE : 4056786.87 590791.95 9.87 2021-02-12T23:22:01Z- LcmTrn::publishEstimates() - MMSE : 4056807.37 590840.45 9.87 2021-02-12T23:22:01Z- LcmTrn::publishEstimates() - COVAR: 773.16 833.06 1.72 0.00 2021-02-12T23:22:01Z- AHRS_M2 msg: 1613172121.53 epoch sec; seqNo:3082 2021-02-12T23:22:01Z- AHRS_M2 msg: 0.60 phi; -0.26 theta; -0.15 psi 2021-02-12T23:22:01Z- handleDepth()-Depth_Keller msg: 1613172121537 epoch sec; seqNo:3113; depth 9.96 2021-02-12T23:22:01Z- handleDvl() - RDI_Pathfinder msg: 1613172121.55 epoch sec; seqNo:1377 2021-02-12T23:22:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 96.49 , 75.99 , 103.79 2021-02-12T23:22:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , -0.08 2021-02-12T23:22:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172121.56 epoch sec; seqNo:3113; 4056807.75 north; 590840.44 east 2021-02-12T23:22:01Z- AHRS_M2 msg: 1613172121.94 epoch sec; seqNo:3083 2021-02-12T23:22:01Z- AHRS_M2 msg: 0.60 phi; -0.26 theta; -0.15 psi 2021-02-12T23:22:01Z- handleDepth()-Depth_Keller msg: 1613172121940 epoch sec; seqNo:3114; depth 10.05 2021-02-12T23:22:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:01Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:01Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:01Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:01Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:01Z- handleDvl() - RDI_Pathfinder msg: 1613172121.94 epoch sec; seqNo:1378 2021-02-12T23:22:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , -0.08 2021-02-12T23:22:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172121.96 epoch sec; seqNo:3114; 4056807.75 north; 590840.44 east 2021-02-12T23:22:02Z- AHRS_M2 msg: 1613172122.35 epoch sec; seqNo:3084 2021-02-12T23:22:02Z- AHRS_M2 msg: 0.60 phi; -0.27 theta; -0.15 psi 2021-02-12T23:22:02Z- handleDepth()-Depth_Keller msg: 1613172122352 epoch sec; seqNo:3115; depth 10.14 2021-02-12T23:22:02Z- handleDvl() - RDI_Pathfinder msg: 1613172122.39 epoch sec; seqNo:1379 2021-02-12T23:22:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 96.56 , 75.17 , 103.86 2021-02-12T23:22:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.02 , -0.12 2021-02-12T23:22:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172122.42 epoch sec; seqNo:3115; 4056808.14 north; 590841.66 east 2021-02-12T23:22:02Z- AHRS_M2 msg: 1613172122.74 epoch sec; seqNo:3085 2021-02-12T23:22:02Z- AHRS_M2 msg: 0.61 phi; -0.28 theta; -0.14 psi 2021-02-12T23:22:02Z- handleDepth()-Depth_Keller msg: 1613172122748 epoch sec; seqNo:3116; depth 10.23 2021-02-12T23:22:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172122.76 epoch sec; seqNo:3116; 4056808.52 north; 590841.66 east 2021-02-12T23:22:03Z- AHRS_M2 msg: 1613172123.16 epoch sec; seqNo:3086 2021-02-12T23:22:03Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.15 psi 2021-02-12T23:22:03Z- handleDepth()-Depth_Keller msg: 1613172123160 epoch sec; seqNo:3117; depth 10.31 2021-02-12T23:22:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:03Z- handleDvl() - RDI_Pathfinder msg: 1613172123.16 epoch sec; seqNo:1380 2021-02-12T23:22:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.02 , -0.12 2021-02-12T23:22:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172123.17 epoch sec; seqNo:3117; 4056808.52 north; 590841.66 east 2021-02-12T23:22:03Z- AHRS_M2 msg: 1613172123.56 epoch sec; seqNo:3087 2021-02-12T23:22:03Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.15 psi 2021-02-12T23:22:03Z- handleDepth()-Depth_Keller msg: 1613172123562 epoch sec; seqNo:3118; depth 10.42 2021-02-12T23:22:03Z- handleDvl() - RDI_Pathfinder msg: 1613172123.58 epoch sec; seqNo:1381 2021-02-12T23:22:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.18 , 96.56 , 74.35 , 106.29 2021-02-12T23:22:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.11 , -0.17 2021-02-12T23:22:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172123.59 epoch sec; seqNo:3118; 4056808.90 north; 590841.65 east 2021-02-12T23:22:03Z- AHRS_M2 msg: 1613172123.96 epoch sec; seqNo:3088 2021-02-12T23:22:03Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.15 psi 2021-02-12T23:22:03Z- handleDepth()-Depth_Keller msg: 1613172123961 epoch sec; seqNo:3119; depth 10.51 2021-02-12T23:22:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172123.97 epoch sec; seqNo:3119; 4056809.28 north; 590841.65 east 2021-02-12T23:22:04Z- AHRS_M2 msg: 1613172124.36 epoch sec; seqNo:3089 2021-02-12T23:22:04Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.15 psi 2021-02-12T23:22:04Z- handleDepth()-Depth_Keller msg: 1613172124364 epoch sec; seqNo:3120; depth 10.61 2021-02-12T23:22:04Z- handleDvl() - RDI_Pathfinder msg: 1613172124.38 epoch sec; seqNo:1382 2021-02-12T23:22:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.75 , 96.56 , 75.58 , 108.72 2021-02-12T23:22:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.24 , -0.11 2021-02-12T23:22:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172124.39 epoch sec; seqNo:3120; 4056809.66 north; 590841.64 east 2021-02-12T23:22:04Z- AHRS_M2 msg: 1613172124.76 epoch sec; seqNo:3090 2021-02-12T23:22:04Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.15 psi 2021-02-12T23:22:04Z- handleDepth()-Depth_Keller msg: 1613172124769 epoch sec; seqNo:3121; depth 10.71 2021-02-12T23:22:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172124.78 epoch sec; seqNo:3121; 4056810.03 north; 590841.64 east 2021-02-12T23:22:05Z- AHRS_M2 msg: 1613172125.17 epoch sec; seqNo:3091 2021-02-12T23:22:05Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.15 psi 2021-02-12T23:22:05Z- handleDepth()-Depth_Keller msg: 1613172125181 epoch sec; seqNo:3122; depth 10.82 2021-02-12T23:22:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172125.19 epoch sec; seqNo:3122; 4056810.03 north; 590841.64 east 2021-02-12T23:22:05Z- AHRS_M2 msg: 1613172125.57 epoch sec; seqNo:3092 2021-02-12T23:22:05Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.14 psi 2021-02-12T23:22:05Z- handleDepth()-Depth_Keller msg: 1613172125576 epoch sec; seqNo:3123; depth 10.93 2021-02-12T23:22:05Z- handleDvl() - RDI_Pathfinder msg: 1613172125.60 epoch sec; seqNo:1383 2021-02-12T23:22:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 96.49 , 74.35 , 108.65 2021-02-12T23:22:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.31 , 0.04 2021-02-12T23:22:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172125.61 epoch sec; seqNo:3123; 4056810.41 north; 590841.64 east 2021-02-12T23:22:05Z- AHRS_M2 msg: 1613172125.98 epoch sec; seqNo:3093 2021-02-12T23:22:05Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.14 psi 2021-02-12T23:22:05Z- handleDepth()-Depth_Keller msg: 1613172125980 epoch sec; seqNo:3124; depth 11.05 2021-02-12T23:22:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172125.99 epoch sec; seqNo:3124; 4056810.81 north; 590842.85 east 2021-02-12T23:22:06Z- AHRS_M2 msg: 1613172126.38 epoch sec; seqNo:3094 2021-02-12T23:22:06Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.14 psi 2021-02-12T23:22:06Z- handleDepth()-Depth_Keller msg: 1613172126390 epoch sec; seqNo:3125; depth 11.15 2021-02-12T23:22:06Z- handleDvl() - RDI_Pathfinder msg: 1613172126.40 epoch sec; seqNo:1384 2021-02-12T23:22:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.18 , 96.56 , 73.13 , 109.94 2021-02-12T23:22:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.26 , 0.24 2021-02-12T23:22:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172126.41 epoch sec; seqNo:3125; 4056811.18 north; 590842.85 east 2021-02-12T23:22:06Z- LcmTrn::handleMessages() - handling 47 messages took 0.74 ms and 132 calls... 2021-02-12T23:22:06Z- waiting for 1613172124.15, time is 1613172126.41 2021-02-12T23:22:06Z- LcmTrn::updateTrn() equating pose 1613172126.41 and meas times 1613172126.41 2021-02-12T23:22:06Z- LcmTrn::updateTrn() >>>> pose time 1613172126.41, meas time 1613172126.41 <<<< 2021-02-12T23:22:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172126.41 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:22:06Z- TerrainNav::motionUpdate() - currEstimate = (4056811, 590842.85, 11.15) 2021-02-12T23:22:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:22:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:22:06Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172126.41 sec.; No relevant navigation data available 2021-02-12T23:22:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:22:06Z- TNavBF::mmse selected from filter -1 2021-02-12T23:22:06Z- variances([0], [2], and psi_berg) = 773.013529 833.058154 0.000000 2021-02-12T23:22:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:22:06Z- LcmTrn::publishEstimates() - MLE : 4056790.68 590794.35 11.15 2021-02-12T23:22:06Z- LcmTrn::publishEstimates() - MMSE : 4056811.18 590842.85 11.15 2021-02-12T23:22:06Z- LcmTrn::publishEstimates() - COVAR: 773.01 833.06 1.95 0.00 2021-02-12T23:22:06Z- AHRS_M2 msg: 1613172126.78 epoch sec; seqNo:3095 2021-02-12T23:22:06Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.14 psi 2021-02-12T23:22:06Z- handleDepth()-Depth_Keller msg: 1613172126799 epoch sec; seqNo:3126; depth 11.29 2021-02-12T23:22:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:06Z- handleDvl() - RDI_Pathfinder msg: 1613172126.80 epoch sec; seqNo:1385 2021-02-12T23:22:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.26 , 0.24 2021-02-12T23:22:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172126.81 epoch sec; seqNo:3126; 4056811.56 north; 590842.84 east 2021-02-12T23:22:07Z- AHRS_M2 msg: 1613172127.19 epoch sec; seqNo:3096 2021-02-12T23:22:07Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.14 psi 2021-02-12T23:22:07Z- handleDepth()-Depth_Keller msg: 1613172127192 epoch sec; seqNo:3127; depth 11.40 2021-02-12T23:22:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172127.20 epoch sec; seqNo:3127; 4056811.56 north; 590842.84 east 2021-02-12T23:22:07Z- AHRS_M2 msg: 1613172127.59 epoch sec; seqNo:3097 2021-02-12T23:22:07Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.14 psi 2021-02-12T23:22:07Z- handleDepth()-Depth_Keller msg: 1613172127596 epoch sec; seqNo:3128; depth 11.51 2021-02-12T23:22:07Z- handleDvl() - RDI_Pathfinder msg: 1613172127.61 epoch sec; seqNo:1386 2021-02-12T23:22:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 96.49 , 73.94 , 109.87 2021-02-12T23:22:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.05 , 0.37 2021-02-12T23:22:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172127.62 epoch sec; seqNo:3128; 4056811.94 north; 590842.84 east 2021-02-12T23:22:08Z- AHRS_M2 msg: 1613172128.00 epoch sec; seqNo:3098 2021-02-12T23:22:08Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.15 psi 2021-02-12T23:22:08Z- handleDepth()-Depth_Keller msg: 1613172128001 epoch sec; seqNo:3129; depth 11.63 2021-02-12T23:22:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:08Z- handleDvl() - RDI_Pathfinder msg: 1613172128.00 epoch sec; seqNo:1387 2021-02-12T23:22:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.05 , 0.37 2021-02-12T23:22:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172128.01 epoch sec; seqNo:3129; 4056811.94 north; 590842.84 east 2021-02-12T23:22:08Z- AHRS_M2 msg: 1613172128.43 epoch sec; seqNo:3099 2021-02-12T23:22:08Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.16 psi 2021-02-12T23:22:08Z- handleDepth()-Depth_Keller msg: 1613172128430 epoch sec; seqNo:3130; depth 11.73 2021-02-12T23:22:08Z- handleDvl() - RDI_Pathfinder msg: 1613172128.44 epoch sec; seqNo:1388 2021-02-12T23:22:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.54 , 96.49 , 74.35 , 113.52 2021-02-12T23:22:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.23 , 0.28 2021-02-12T23:22:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172128.45 epoch sec; seqNo:3130; 4056812.32 north; 590842.84 east 2021-02-12T23:22:08Z- AHRS_M2 msg: 1613172128.80 epoch sec; seqNo:3100 2021-02-12T23:22:08Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.18 psi 2021-02-12T23:22:08Z- handleDepth()-Depth_Keller msg: 1613172128808 epoch sec; seqNo:3131; depth 11.87 2021-02-12T23:22:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172128.82 epoch sec; seqNo:3131; 4056812.32 north; 590842.84 east 2021-02-12T23:22:09Z- AHRS_M2 msg: 1613172129.21 epoch sec; seqNo:3101 2021-02-12T23:22:09Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.20 psi 2021-02-12T23:22:09Z- handleDepth()-Depth_Keller msg: 1613172129212 epoch sec; seqNo:3132; depth 11.97 2021-02-12T23:22:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:09Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:09Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:09Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:09Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:09Z- handleDvl() - RDI_Pathfinder msg: 1613172129.21 epoch sec; seqNo:1389 2021-02-12T23:22:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.23 , 0.28 2021-02-12T23:22:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172129.22 epoch sec; seqNo:3132; 4056812.71 north; 590844.05 east 2021-02-12T23:22:09Z- AHRS_M2 msg: 1613172129.61 epoch sec; seqNo:3102 2021-02-12T23:22:09Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.21 psi 2021-02-12T23:22:09Z- handleDepth()-Depth_Keller msg: 1613172129617 epoch sec; seqNo:3133; depth 12.08 2021-02-12T23:22:09Z- handleDvl() - RDI_Pathfinder msg: 1613172129.63 epoch sec; seqNo:1390 2021-02-12T23:22:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 96.29 , 73.54 , 116.56 2021-02-12T23:22:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.29 , 0.03 2021-02-12T23:22:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172129.64 epoch sec; seqNo:3133; 4056812.71 north; 590844.05 east 2021-02-12T23:22:10Z- AHRS_M2 msg: 1613172130.02 epoch sec; seqNo:3103 2021-02-12T23:22:10Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.23 psi 2021-02-12T23:22:10Z- handleDepth()-Depth_Keller msg: 1613172130020 epoch sec; seqNo:3134; depth 12.21 2021-02-12T23:22:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172130.04 epoch sec; seqNo:3134; 4056813.09 north; 590844.05 east 2021-02-12T23:22:10Z- AHRS_M2 msg: 1613172130.42 epoch sec; seqNo:3104 2021-02-12T23:22:10Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.24 psi 2021-02-12T23:22:10Z- handleDepth()-Depth_Keller msg: 1613172130424 epoch sec; seqNo:3135; depth 12.32 2021-02-12T23:22:10Z- handleDvl() - RDI_Pathfinder msg: 1613172130.44 epoch sec; seqNo:1391 2021-02-12T23:22:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.42 , 104.39 , 73.94 , 117.91 2021-02-12T23:22:10Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.20 , -0.18 2021-02-12T23:22:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172130.46 epoch sec; seqNo:3135; 4056813.47 north; 590844.05 east 2021-02-12T23:22:10Z- AHRS_M2 msg: 1613172130.83 epoch sec; seqNo:3105 2021-02-12T23:22:10Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.25 psi 2021-02-12T23:22:10Z- handleDepth()-Depth_Keller msg: 1613172130837 epoch sec; seqNo:3136; depth 12.48 2021-02-12T23:22:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172130.85 epoch sec; seqNo:3136; 4056813.48 north; 590845.27 east 2021-02-12T23:22:11Z- AHRS_M2 msg: 1613172131.23 epoch sec; seqNo:3106 2021-02-12T23:22:11Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.25 psi 2021-02-12T23:22:11Z- handleDepth()-Depth_Keller msg: 1613172131238 epoch sec; seqNo:3137; depth 12.60 2021-02-12T23:22:11Z- handleDvl() - RDI_Pathfinder msg: 1613172131.25 epoch sec; seqNo:1392 2021-02-12T23:22:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 106.96 , 73.94 , 117.77 2021-02-12T23:22:11Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , -0.03 , -0.19 2021-02-12T23:22:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172131.26 epoch sec; seqNo:3137; 4056813.86 north; 590845.26 east 2021-02-12T23:22:11Z- LcmTrn::handleMessages() - handling 44 messages took 0.63 ms and 132 calls... 2021-02-12T23:22:11Z- waiting for 1613172129.41, time is 1613172131.26 2021-02-12T23:22:11Z- LcmTrn::updateTrn() equating pose 1613172131.26 and meas times 1613172131.26 2021-02-12T23:22:11Z- LcmTrn::updateTrn() >>>> pose time 1613172131.26, meas time 1613172131.26 <<<< 2021-02-12T23:22:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172131.26 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:22:11Z- TerrainNav::motionUpdate() - currEstimate = (4056814, 590845.26, 12.60) 2021-02-12T23:22:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:22:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:22:11Z- TNavBF::Projecting Measurements 2021-02-12T23:22:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:22:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:22:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:22:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:22:11Z- TNavFilter::4 good beams. 2021-02-12T23:22:11Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:22:11Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:22:11Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:22:11Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:22:11Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:22:11Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:22:11Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:22:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:22:11Z- TerrainMapOctree: (vehN, vehE ) = (4056813.86, 590845.26). 2021-02-12T23:22:11Z- TerrainMapOctree: Min Distance = 5592.10. 2021-02-12T23:22:11Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:22:11Z- There is no specific good beam that all particles have in common. 2021-02-12T23:22:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:22:11Z- Old Depth Bias: 0.00 DepthBiasCov: 2.13 K: 0.154 ExpectedMeasDiff: nan currMeas.covariance: 9.95 2021-02-12T23:22:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:22:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.80 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 18.31 2021-02-12T23:22:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:22:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.65 K: 0.136 ExpectedMeasDiff: nan currMeas.covariance: 8.75 2021-02-12T23:22:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:22:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 22.19 2021-02-12T23:22:11Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:22:11Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:22:11Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:22:11Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:22:11Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:22:11Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:22:11Z- Filter 0 No beams used in subcloud update 2021-02-12T23:22:11Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:22:11Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:22:11Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 772.9 EastVariance: 833.1 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-12T23:22:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172131.26 sec. 2021-02-12T23:22:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:22:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:22:11Z- variances([0], [2], and psi_berg) = 772.884514 833.057988 0.000000 2021-02-12T23:22:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:22:11Z- LcmTrn::publishEstimates() - MLE : 4056793.36 590796.76 12.60 2021-02-12T23:22:11Z- LcmTrn::publishEstimates() - MMSE : 4056813.86 590845.26 12.60 2021-02-12T23:22:11Z- LcmTrn::publishEstimates() - COVAR: 772.88 833.06 1.35 0.00 2021-02-12T23:22:11Z- AHRS_M2 msg: 1613172131.63 epoch sec; seqNo:3107 2021-02-12T23:22:11Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.25 psi 2021-02-12T23:22:11Z- handleDepth()-Depth_Keller msg: 1613172131635 epoch sec; seqNo:3138; depth 12.74 2021-02-12T23:22:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:11Z- handleDvl() - RDI_Pathfinder msg: 1613172131.64 epoch sec; seqNo:1393 2021-02-12T23:22:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:11Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , -0.03 , -0.19 2021-02-12T23:22:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172131.65 epoch sec; seqNo:3138; 4056814.24 north; 590845.26 east 2021-02-12T23:22:12Z- AHRS_M2 msg: 1613172132.04 epoch sec; seqNo:3108 2021-02-12T23:22:12Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.24 psi 2021-02-12T23:22:12Z- handleDepth()-Depth_Keller msg: 1613172132045 epoch sec; seqNo:3139; depth 12.87 2021-02-12T23:22:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172132.07 epoch sec; seqNo:3139; 4056814.62 north; 590845.25 east 2021-02-12T23:22:12Z- AHRS_M2 msg: 1613172132.44 epoch sec; seqNo:3109 2021-02-12T23:22:12Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.23 psi 2021-02-12T23:22:12Z- handleDepth()-Depth_Keller msg: 1613172132445 epoch sec; seqNo:3140; depth 13.01 2021-02-12T23:22:12Z- handleDvl() - RDI_Pathfinder msg: 1613172132.46 epoch sec; seqNo:1394 2021-02-12T23:22:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.03 , 105.61 , 71.49 , 115.07 2021-02-12T23:22:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.18 , -0.13 2021-02-12T23:22:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172132.47 epoch sec; seqNo:3140; 4056815.00 north; 590845.25 east 2021-02-12T23:22:12Z- AHRS_M2 msg: 1613172132.85 epoch sec; seqNo:3110 2021-02-12T23:22:12Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.23 psi 2021-02-12T23:22:12Z- handleDepth()-Depth_Keller msg: 1613172132849 epoch sec; seqNo:3141; depth 13.16 2021-02-12T23:22:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:12Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:12Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:12Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:12Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:12Z- handleDvl() - RDI_Pathfinder msg: 1613172132.85 epoch sec; seqNo:1395 2021-02-12T23:22:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.18 , -0.13 2021-02-12T23:22:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172132.86 epoch sec; seqNo:3141; 4056815.38 north; 590845.25 east 2021-02-12T23:22:13Z- AHRS_M2 msg: 1613172133.25 epoch sec; seqNo:3111 2021-02-12T23:22:13Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.22 psi 2021-02-12T23:22:13Z- handleDepth()-Depth_Keller msg: 1613172133262 epoch sec; seqNo:3142; depth 13.27 2021-02-12T23:22:13Z- handleDvl() - RDI_Pathfinder msg: 1613172133.27 epoch sec; seqNo:1396 2021-02-12T23:22:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 103.04 , 72.31 , 112.50 2021-02-12T23:22:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.18 , -0.06 2021-02-12T23:22:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172133.29 epoch sec; seqNo:3142; 4056815.77 north; 590846.46 east 2021-02-12T23:22:13Z- handleDepth()-Depth_Keller msg: 1613172133673 epoch sec; seqNo:3143; depth 13.43 2021-02-12T23:22:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172133.68 epoch sec; seqNo:3143; 4056816.15 north; 590846.46 east 2021-02-12T23:22:14Z- AHRS_M2 msg: 1613172134.06 epoch sec; seqNo:3112 2021-02-12T23:22:14Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.20 psi 2021-02-12T23:22:14Z- handleDepth()-Depth_Keller msg: 1613172134062 epoch sec; seqNo:3144; depth 13.55 2021-02-12T23:22:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:14Z- handleDvl() - RDI_Pathfinder msg: 1613172134.07 epoch sec; seqNo:1397 2021-02-12T23:22:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.18 , -0.06 2021-02-12T23:22:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172134.07 epoch sec; seqNo:3144; 4056816.53 north; 590846.45 east 2021-02-12T23:22:14Z- AHRS_M2 msg: 1613172134.46 epoch sec; seqNo:3113 2021-02-12T23:22:14Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.20 psi 2021-02-12T23:22:14Z- handleDepth()-Depth_Keller msg: 1613172134474 epoch sec; seqNo:3145; depth 13.66 2021-02-12T23:22:14Z- handleDvl() - RDI_Pathfinder msg: 1613172134.49 epoch sec; seqNo:1398 2021-02-12T23:22:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.25 , 96.42 , 71.49 , 113.45 2021-02-12T23:22:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.11 , -0.01 2021-02-12T23:22:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172134.50 epoch sec; seqNo:3145; 4056816.53 north; 590846.45 east 2021-02-12T23:22:14Z- AHRS_M2 msg: 1613172134.86 epoch sec; seqNo:3114 2021-02-12T23:22:14Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.19 psi 2021-02-12T23:22:14Z- handleDepth()-Depth_Keller msg: 1613172134868 epoch sec; seqNo:3146; depth 13.80 2021-02-12T23:22:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172134.88 epoch sec; seqNo:3146; 4056816.91 north; 590846.45 east 2021-02-12T23:22:15Z- AHRS_M2 msg: 1613172135.27 epoch sec; seqNo:3115 2021-02-12T23:22:15Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.18 psi 2021-02-12T23:22:15Z- handleDepth()-Depth_Keller msg: 1613172135272 epoch sec; seqNo:3147; depth 13.92 2021-02-12T23:22:15Z- handleDvl() - RDI_Pathfinder msg: 1613172135.29 epoch sec; seqNo:1399 2021-02-12T23:22:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.25 , 96.49 , 70.68 , 113.52 2021-02-12T23:22:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.04 , 0.01 2021-02-12T23:22:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172135.30 epoch sec; seqNo:3147; 4056817.28 north; 590846.45 east 2021-02-12T23:22:15Z- AHRS_M2 msg: 1613172135.67 epoch sec; seqNo:3116 2021-02-12T23:22:15Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.18 psi 2021-02-12T23:22:15Z- handleDepth()-Depth_Keller msg: 1613172135679 epoch sec; seqNo:3148; depth 14.07 2021-02-12T23:22:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172135.69 epoch sec; seqNo:3148; 4056817.28 north; 590846.45 east 2021-02-12T23:22:16Z- AHRS_M2 msg: 1613172136.08 epoch sec; seqNo:3117 2021-02-12T23:22:16Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.18 psi 2021-02-12T23:22:16Z- handleDepth()-Depth_Keller msg: 1613172136088 epoch sec; seqNo:3149; depth 14.19 2021-02-12T23:22:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172136.10 epoch sec; seqNo:3149; 4056817.66 north; 590846.44 east 2021-02-12T23:22:16Z- AHRS_M2 msg: 1613172136.48 epoch sec; seqNo:3118 2021-02-12T23:22:16Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.18 psi 2021-02-12T23:22:16Z- handleDepth()-Depth_Keller msg: 1613172136484 epoch sec; seqNo:3150; depth 14.32 2021-02-12T23:22:16Z- handleDvl() - RDI_Pathfinder msg: 1613172136.50 epoch sec; seqNo:1400 2021-02-12T23:22:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.67 , 96.55 , 71.91 , 113.58 2021-02-12T23:22:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.03 , -0.03 2021-02-12T23:22:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172136.52 epoch sec; seqNo:3150; 4056818.05 north; 590847.66 east 2021-02-12T23:22:16Z- LcmTrn::handleMessages() - handling 46 messages took 0.72 ms and 132 calls... 2021-02-12T23:22:16Z- waiting for 1613172134.26, time is 1613172136.52 2021-02-12T23:22:16Z- LcmTrn::updateTrn() equating pose 1613172136.52 and meas times 1613172136.52 2021-02-12T23:22:16Z- LcmTrn::updateTrn() >>>> pose time 1613172136.52, meas time 1613172136.52 <<<< 2021-02-12T23:22:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172136.52 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:22:16Z- TerrainNav::motionUpdate() - currEstimate = (4056818, 590847.66, 14.32) 2021-02-12T23:22:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:22:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:22:16Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172136.52 sec.; No relevant navigation data available 2021-02-12T23:22:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:22:16Z- TNavBF::mmse selected from filter -1 2021-02-12T23:22:16Z- variances([0], [2], and psi_berg) = 772.735483 833.057797 0.000000 2021-02-12T23:22:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:22:16Z- LcmTrn::publishEstimates() - MLE : 4056797.55 590799.16 14.32 2021-02-12T23:22:16Z- LcmTrn::publishEstimates() - MMSE : 4056818.05 590847.66 14.32 2021-02-12T23:22:16Z- LcmTrn::publishEstimates() - COVAR: 772.74 833.06 1.59 0.00 2021-02-12T23:22:16Z- AHRS_M2 msg: 1613172136.89 epoch sec; seqNo:3119 2021-02-12T23:22:16Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.19 psi 2021-02-12T23:22:16Z- handleDepth()-Depth_Keller msg: 1613172136895 epoch sec; seqNo:3151; depth 14.46 2021-02-12T23:22:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172136.91 epoch sec; seqNo:3151; 4056818.05 north; 590847.66 east 2021-02-12T23:22:17Z- AHRS_M2 msg: 1613172137.29 epoch sec; seqNo:3120 2021-02-12T23:22:17Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.19 psi 2021-02-12T23:22:17Z- handleDepth()-Depth_Keller msg: 1613172137301 epoch sec; seqNo:3152; depth 14.59 2021-02-12T23:22:17Z- handleDvl() - RDI_Pathfinder msg: 1613172137.31 epoch sec; seqNo:1401 2021-02-12T23:22:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 96.49 , 69.86 , 114.73 2021-02-12T23:22:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , -0.11 2021-02-12T23:22:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172137.32 epoch sec; seqNo:3152; 4056818.43 north; 590847.66 east 2021-02-12T23:22:17Z- AHRS_M2 msg: 1613172137.69 epoch sec; seqNo:3121 2021-02-12T23:22:17Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.20 psi 2021-02-12T23:22:17Z- handleDepth()-Depth_Keller msg: 1613172137696 epoch sec; seqNo:3153; depth 14.72 2021-02-12T23:22:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:17Z- handleDvl() - RDI_Pathfinder msg: 1613172137.70 epoch sec; seqNo:1402 2021-02-12T23:22:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , -0.11 2021-02-12T23:22:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172137.72 epoch sec; seqNo:3153; 4056818.81 north; 590847.65 east 2021-02-12T23:22:18Z- AHRS_M2 msg: 1613172138.10 epoch sec; seqNo:3122 2021-02-12T23:22:18Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.20 psi 2021-02-12T23:22:18Z- handleDepth()-Depth_Keller msg: 1613172138105 epoch sec; seqNo:3154; depth 14.86 2021-02-12T23:22:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172138.12 epoch sec; seqNo:3154; 4056819.19 north; 590847.65 east 2021-02-12T23:22:18Z- AHRS_M2 msg: 1613172138.50 epoch sec; seqNo:3123 2021-02-12T23:22:18Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.20 psi 2021-02-12T23:22:18Z- handleDepth()-Depth_Keller msg: 1613172138505 epoch sec; seqNo:3155; depth 15.01 2021-02-12T23:22:18Z- handleDvl() - RDI_Pathfinder msg: 1613172138.53 epoch sec; seqNo:1403 2021-02-12T23:22:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.22 , 96.55 , 69.46 , 112.37 2021-02-12T23:22:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.16 , -0.12 2021-02-12T23:22:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172138.56 epoch sec; seqNo:3155; 4056819.57 north; 590847.64 east 2021-02-12T23:22:18Z- AHRS_M2 msg: 1613172138.90 epoch sec; seqNo:3124 2021-02-12T23:22:18Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.19 psi 2021-02-12T23:22:18Z- handleDepth()-Depth_Keller msg: 1613172138909 epoch sec; seqNo:3156; depth 15.16 2021-02-12T23:22:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:18Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:18Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:18Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:18Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:18Z- handleDvl() - RDI_Pathfinder msg: 1613172138.91 epoch sec; seqNo:1404 2021-02-12T23:22:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.16 , -0.12 2021-02-12T23:22:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172138.92 epoch sec; seqNo:3156; 4056819.57 north; 590847.64 east 2021-02-12T23:22:19Z- AHRS_M2 msg: 1613172139.31 epoch sec; seqNo:3125 2021-02-12T23:22:19Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.19 psi 2021-02-12T23:22:19Z- handleDepth()-Depth_Keller msg: 1613172139322 epoch sec; seqNo:3157; depth 15.30 2021-02-12T23:22:19Z- handleDvl() - RDI_Pathfinder msg: 1613172139.34 epoch sec; seqNo:1405 2021-02-12T23:22:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.81 , 100.27 , 69.86 , 111.22 2021-02-12T23:22:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.28 , -0.01 2021-02-12T23:22:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172139.35 epoch sec; seqNo:3157; 4056819.96 north; 590848.86 east 2021-02-12T23:22:19Z- AHRS_M2 msg: 1613172139.71 epoch sec; seqNo:3126 2021-02-12T23:22:19Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.18 psi 2021-02-12T23:22:19Z- handleDepth()-Depth_Keller msg: 1613172139716 epoch sec; seqNo:3158; depth 15.43 2021-02-12T23:22:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172139.73 epoch sec; seqNo:3158; 4056820.34 north; 590848.86 east 2021-02-12T23:22:20Z- AHRS_M2 msg: 1613172140.12 epoch sec; seqNo:3127 2021-02-12T23:22:20Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.18 psi 2021-02-12T23:22:20Z- handleDepth()-Depth_Keller msg: 1613172140128 epoch sec; seqNo:3159; depth 15.58 2021-02-12T23:22:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:20Z- handleDvl() - RDI_Pathfinder msg: 1613172140.13 epoch sec; seqNo:1406 2021-02-12T23:22:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.28 , -0.01 2021-02-12T23:22:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172140.14 epoch sec; seqNo:3159; 4056820.72 north; 590848.85 east 2021-02-12T23:22:20Z- AHRS_M2 msg: 1613172140.53 epoch sec; seqNo:3128 2021-02-12T23:22:20Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.17 psi 2021-02-12T23:22:20Z- handleDepth()-Depth_Keller msg: 1613172140531 epoch sec; seqNo:3160; depth 15.71 2021-02-12T23:22:20Z- handleDvl() - RDI_Pathfinder msg: 1613172140.54 epoch sec; seqNo:1407 2021-02-12T23:22:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.21 , 96.56 , 70.27 , 111.15 2021-02-12T23:22:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.27 , 0.15 2021-02-12T23:22:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172140.55 epoch sec; seqNo:3160; 4056821.10 north; 590848.85 east 2021-02-12T23:22:20Z- AHRS_M2 msg: 1613172140.93 epoch sec; seqNo:3129 2021-02-12T23:22:20Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.17 psi 2021-02-12T23:22:20Z- handleDepth()-Depth_Keller msg: 1613172140930 epoch sec; seqNo:3161; depth 15.84 2021-02-12T23:22:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172140.96 epoch sec; seqNo:3161; 4056821.48 north; 590848.84 east 2021-02-12T23:22:21Z- AHRS_M2 msg: 1613172141.33 epoch sec; seqNo:3130 2021-02-12T23:22:21Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.17 psi 2021-02-12T23:22:21Z- handleDepth()-Depth_Keller msg: 1613172141332 epoch sec; seqNo:3162; depth 15.99 2021-02-12T23:22:21Z- handleDvl() - RDI_Pathfinder msg: 1613172141.34 epoch sec; seqNo:1408 2021-02-12T23:22:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.03 , 95.41 , 68.63 , 110.00 2021-02-12T23:22:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.10 , 0.28 2021-02-12T23:22:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172141.38 epoch sec; seqNo:3162; 4056821.48 north; 590848.84 east 2021-02-12T23:22:21Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 130 calls... 2021-02-12T23:22:21Z- waiting for 1613172139.52, time is 1613172141.38 2021-02-12T23:22:21Z- LcmTrn::updateTrn() equating pose 1613172141.38 and meas times 1613172141.38 2021-02-12T23:22:21Z- LcmTrn::updateTrn() >>>> pose time 1613172141.38, meas time 1613172141.38 <<<< 2021-02-12T23:22:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172141.38 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:22:21Z- TerrainNav::motionUpdate() - currEstimate = (4056821, 590848.84, 15.99) 2021-02-12T23:22:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:22:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:22:21Z- TNavBF::Projecting Measurements 2021-02-12T23:22:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:22:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:22:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:22:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:22:21Z- TNavFilter::4 good beams. 2021-02-12T23:22:21Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:22:21Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:22:21Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:22:21Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:22:21Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:22:21Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:22:21Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:22:21Z- TNavBF::mmse selected from filter -1 2021-02-12T23:22:21Z- TerrainMapOctree: (vehN, vehE ) = (4056821.48, 590848.84). 2021-02-12T23:22:21Z- TerrainMapOctree: Min Distance = 5583.88. 2021-02-12T23:22:21Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:22:21Z- There is no specific good beam that all particles have in common. 2021-02-12T23:22:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:22:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.78 K: 0.134 ExpectedMeasDiff: nan currMeas.covariance: 9.74 2021-02-12T23:22:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:22:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.54 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 14.56 2021-02-12T23:22:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:22:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.41 K: 0.132 ExpectedMeasDiff: nan currMeas.covariance: 7.54 2021-02-12T23:22:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:22:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 19.36 2021-02-12T23:22:21Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:22:21Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:22:21Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:22:21Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:22:21Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:22:21Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:22:21Z- Filter 0 No beams used in subcloud update 2021-02-12T23:22:21Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:22:21Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:22:21Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 772.6 EastVariance: 833.1 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:22:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172141.38 sec. 2021-02-12T23:22:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:22:21Z- TNavBF::mmse selected from filter -1 2021-02-12T23:22:21Z- variances([0], [2], and psi_berg) = 772.606743 833.057633 0.000000 2021-02-12T23:22:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:22:21Z- LcmTrn::publishEstimates() - MLE : 4056800.98 590800.34 15.99 2021-02-12T23:22:21Z- LcmTrn::publishEstimates() - MMSE : 4056821.48 590848.84 15.99 2021-02-12T23:22:21Z- LcmTrn::publishEstimates() - COVAR: 772.61 833.06 1.15 0.00 2021-02-12T23:22:21Z- AHRS_M2 msg: 1613172141.73 epoch sec; seqNo:3131 2021-02-12T23:22:21Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.16 psi 2021-02-12T23:22:21Z- handleDepth()-Depth_Keller msg: 1613172141735 epoch sec; seqNo:3163; depth 16.12 2021-02-12T23:22:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172141.75 epoch sec; seqNo:3163; 4056821.86 north; 590848.84 east 2021-02-12T23:22:22Z- AHRS_M2 msg: 1613172142.14 epoch sec; seqNo:3132 2021-02-12T23:22:22Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.17 psi 2021-02-12T23:22:22Z- handleDepth()-Depth_Keller msg: 1613172142144 epoch sec; seqNo:3164; depth 16.24 2021-02-12T23:22:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172142.16 epoch sec; seqNo:3164; 4056821.86 north; 590848.84 east 2021-02-12T23:22:22Z- AHRS_M2 msg: 1613172142.54 epoch sec; seqNo:3133 2021-02-12T23:22:22Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.17 psi 2021-02-12T23:22:22Z- handleDepth()-Depth_Keller msg: 1613172142544 epoch sec; seqNo:3165; depth 16.39 2021-02-12T23:22:22Z- handleDvl() - RDI_Pathfinder msg: 1613172142.56 epoch sec; seqNo:1409 2021-02-12T23:22:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 95.47 , 68.64 , 111.29 2021-02-12T23:22:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.12 , 0.23 2021-02-12T23:22:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172142.58 epoch sec; seqNo:3165; 4056822.24 north; 590848.84 east 2021-02-12T23:22:22Z- AHRS_M2 msg: 1613172142.94 epoch sec; seqNo:3134 2021-02-12T23:22:22Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.18 psi 2021-02-12T23:22:22Z- handleDepth()-Depth_Keller msg: 1613172142948 epoch sec; seqNo:3166; depth 16.50 2021-02-12T23:22:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172142.96 epoch sec; seqNo:3166; 4056822.25 north; 590850.06 east 2021-02-12T23:22:23Z- AHRS_M2 msg: 1613172143.35 epoch sec; seqNo:3135 2021-02-12T23:22:23Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.18 psi 2021-02-12T23:22:23Z- handleDepth()-Depth_Keller msg: 1613172143354 epoch sec; seqNo:3167; depth 16.64 2021-02-12T23:22:23Z- handleDvl() - RDI_Pathfinder msg: 1613172143.37 epoch sec; seqNo:1410 2021-02-12T23:22:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 95.47 , 69.05 , 110.07 2021-02-12T23:22:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.22 , 0.04 2021-02-12T23:22:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172143.38 epoch sec; seqNo:3167; 4056822.63 north; 590850.05 east 2021-02-12T23:22:23Z- AHRS_M2 msg: 1613172143.75 epoch sec; seqNo:3136 2021-02-12T23:22:23Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.19 psi 2021-02-12T23:22:23Z- handleDepth()-Depth_Keller msg: 1613172143756 epoch sec; seqNo:3168; depth 16.76 2021-02-12T23:22:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:23Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:23Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:23Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:23Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:23Z- handleDvl() - RDI_Pathfinder msg: 1613172143.76 epoch sec; seqNo:1411 2021-02-12T23:22:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.22 , 0.04 2021-02-12T23:22:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172143.78 epoch sec; seqNo:3168; 4056823.01 north; 590850.05 east 2021-02-12T23:22:24Z- AHRS_M2 msg: 1613172144.16 epoch sec; seqNo:3137 2021-02-12T23:22:24Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.20 psi 2021-02-12T23:22:24Z- handleDepth()-Depth_Keller msg: 1613172144165 epoch sec; seqNo:3169; depth 16.91 2021-02-12T23:22:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172144.17 epoch sec; seqNo:3169; 4056823.01 north; 590850.05 east 2021-02-12T23:22:24Z- AHRS_M2 msg: 1613172144.56 epoch sec; seqNo:3138 2021-02-12T23:22:24Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.20 psi 2021-02-12T23:22:24Z- handleDepth()-Depth_Keller msg: 1613172144564 epoch sec; seqNo:3170; depth 17.03 2021-02-12T23:22:24Z- handleDvl() - RDI_Pathfinder msg: 1613172144.58 epoch sec; seqNo:1412 2021-02-12T23:22:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.81 , 96.69 , 69.86 , 107.64 2021-02-12T23:22:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.12 , -0.13 2021-02-12T23:22:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172144.59 epoch sec; seqNo:3170; 4056823.38 north; 590850.05 east 2021-02-12T23:22:24Z- AHRS_M2 msg: 1613172144.96 epoch sec; seqNo:3139 2021-02-12T23:22:24Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.20 psi 2021-02-12T23:22:24Z- handleDepth()-Depth_Keller msg: 1613172144968 epoch sec; seqNo:3171; depth 17.18 2021-02-12T23:22:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:24Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:24Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:24Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:24Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:24Z- handleDvl() - RDI_Pathfinder msg: 1613172144.97 epoch sec; seqNo:1413 2021-02-12T23:22:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.12 , -0.13 2021-02-12T23:22:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172144.99 epoch sec; seqNo:3171; 4056823.78 north; 590851.26 east 2021-02-12T23:22:25Z- AHRS_M2 msg: 1613172145.37 epoch sec; seqNo:3140 2021-02-12T23:22:25Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.20 psi 2021-02-12T23:22:25Z- handleDepth()-Depth_Keller msg: 1613172145377 epoch sec; seqNo:3172; depth 17.31 2021-02-12T23:22:25Z- handleDvl() - RDI_Pathfinder msg: 1613172145.39 epoch sec; seqNo:1414 2021-02-12T23:22:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.58 , 96.76 , 69.46 , 108.92 2021-02-12T23:22:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.04 , -0.16 2021-02-12T23:22:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172145.42 epoch sec; seqNo:3172; 4056824.16 north; 590851.26 east 2021-02-12T23:22:25Z- AHRS_M2 msg: 1613172145.77 epoch sec; seqNo:3141 2021-02-12T23:22:25Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.20 psi 2021-02-12T23:22:25Z- handleDepth()-Depth_Keller msg: 1613172145776 epoch sec; seqNo:3173; depth 17.46 2021-02-12T23:22:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172145.79 epoch sec; seqNo:3173; 4056824.16 north; 590851.26 east 2021-02-12T23:22:26Z- AHRS_M2 msg: 1613172146.18 epoch sec; seqNo:3142 2021-02-12T23:22:26Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.19 psi 2021-02-12T23:22:26Z- handleDepth()-Depth_Keller msg: 1613172146181 epoch sec; seqNo:3174; depth 17.59 2021-02-12T23:22:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:26Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:26Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:26Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:26Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:26Z- handleDvl() - RDI_Pathfinder msg: 1613172146.18 epoch sec; seqNo:1415 2021-02-12T23:22:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.04 , -0.16 2021-02-12T23:22:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172146.19 epoch sec; seqNo:3174; 4056824.53 north; 590851.25 east 2021-02-12T23:22:26Z- AHRS_M2 msg: 1613172146.58 epoch sec; seqNo:3143 2021-02-12T23:22:26Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.18 psi 2021-02-12T23:22:26Z- handleDepth()-Depth_Keller msg: 1613172146597 epoch sec; seqNo:3175; depth 17.72 2021-02-12T23:22:26Z- handleDvl() - RDI_Pathfinder msg: 1613172146.61 epoch sec; seqNo:1416 2021-02-12T23:22:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.18 , 95.54 , 68.64 , 106.49 2021-02-12T23:22:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.11 , -0.11 2021-02-12T23:22:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172146.62 epoch sec; seqNo:3175; 4056824.91 north; 590851.25 east 2021-02-12T23:22:26Z- LcmTrn::handleMessages() - handling 47 messages took 0.71 ms and 133 calls... 2021-02-12T23:22:26Z- waiting for 1613172144.38, time is 1613172146.62 2021-02-12T23:22:26Z- LcmTrn::updateTrn() equating pose 1613172146.62 and meas times 1613172146.62 2021-02-12T23:22:26Z- LcmTrn::updateTrn() >>>> pose time 1613172146.62, meas time 1613172146.62 <<<< 2021-02-12T23:22:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172146.62 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:22:26Z- TerrainNav::motionUpdate() - currEstimate = (4056825, 590851.25, 17.72) 2021-02-12T23:22:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:22:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:22:26Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172146.62 sec.; No relevant navigation data available 2021-02-12T23:22:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:22:26Z- TNavBF::mmse selected from filter -1 2021-02-12T23:22:26Z- variances([0], [2], and psi_berg) = 772.468488 833.057456 0.000000 2021-02-12T23:22:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:22:26Z- LcmTrn::publishEstimates() - MLE : 4056804.41 590802.75 17.72 2021-02-12T23:22:26Z- LcmTrn::publishEstimates() - MMSE : 4056824.91 590851.25 17.72 2021-02-12T23:22:26Z- LcmTrn::publishEstimates() - COVAR: 772.47 833.06 1.37 0.00 2021-02-12T23:22:26Z- AHRS_M2 msg: 1613172146.98 epoch sec; seqNo:3144 2021-02-12T23:22:26Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.18 psi 2021-02-12T23:22:26Z- handleDepth()-Depth_Keller msg: 1613172146988 epoch sec; seqNo:3176; depth 17.85 2021-02-12T23:22:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172147.00 epoch sec; seqNo:3176; 4056825.30 north; 590852.47 east 2021-02-12T23:22:27Z- AHRS_M2 msg: 1613172147.39 epoch sec; seqNo:3145 2021-02-12T23:22:27Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.17 psi 2021-02-12T23:22:27Z- handleDepth()-Depth_Keller msg: 1613172147392 epoch sec; seqNo:3177; depth 17.99 2021-02-12T23:22:27Z- handleDvl() - RDI_Pathfinder msg: 1613172147.40 epoch sec; seqNo:1417 2021-02-12T23:22:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.18 , 94.39 , 68.64 , 106.56 2021-02-12T23:22:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , -0.03 2021-02-12T23:22:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172147.41 epoch sec; seqNo:3177; 4056825.68 north; 590852.46 east 2021-02-12T23:22:27Z- AHRS_M2 msg: 1613172147.79 epoch sec; seqNo:3146 2021-02-12T23:22:27Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.17 psi 2021-02-12T23:22:27Z- handleDepth()-Depth_Keller msg: 1613172147799 epoch sec; seqNo:3178; depth 18.11 2021-02-12T23:22:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172147.81 epoch sec; seqNo:3178; 4056825.68 north; 590852.46 east 2021-02-12T23:22:28Z- AHRS_M2 msg: 1613172148.20 epoch sec; seqNo:3147 2021-02-12T23:22:28Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.16 psi 2021-02-12T23:22:28Z- handleDepth()-Depth_Keller msg: 1613172148204 epoch sec; seqNo:3179; depth 18.24 2021-02-12T23:22:28Z- handleDvl() - RDI_Pathfinder msg: 1613172148.22 epoch sec; seqNo:1418 2021-02-12T23:22:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.58 , 94.46 , 67.82 , 106.62 2021-02-12T23:22:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , -0.02 2021-02-12T23:22:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172148.23 epoch sec; seqNo:3179; 4056826.06 north; 590852.46 east 2021-02-12T23:22:28Z- AHRS_M2 msg: 1613172148.60 epoch sec; seqNo:3148 2021-02-12T23:22:28Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.17 psi 2021-02-12T23:22:28Z- handleDepth()-Depth_Keller msg: 1613172148608 epoch sec; seqNo:3180; depth 18.37 2021-02-12T23:22:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:28Z- handleDvl() - RDI_Pathfinder msg: 1613172148.61 epoch sec; seqNo:1419 2021-02-12T23:22:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , -0.02 2021-02-12T23:22:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172148.62 epoch sec; seqNo:3180; 4056826.44 north; 590852.46 east 2021-02-12T23:22:29Z- AHRS_M2 msg: 1613172149.01 epoch sec; seqNo:3149 2021-02-12T23:22:29Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.17 psi 2021-02-12T23:22:29Z- handleDepth()-Depth_Keller msg: 1613172149010 epoch sec; seqNo:3181; depth 18.50 2021-02-12T23:22:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172149.02 epoch sec; seqNo:3181; 4056826.82 north; 590852.45 east 2021-02-12T23:22:29Z- AHRS_M2 msg: 1613172149.41 epoch sec; seqNo:3150 2021-02-12T23:22:29Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.18 psi 2021-02-12T23:22:29Z- handleDepth()-Depth_Keller msg: 1613172149412 epoch sec; seqNo:3182; depth 18.63 2021-02-12T23:22:29Z- handleDvl() - RDI_Pathfinder msg: 1613172149.43 epoch sec; seqNo:1420 2021-02-12T23:22:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.17 , 93.25 , 67.41 , 105.41 2021-02-12T23:22:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , -0.04 2021-02-12T23:22:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172149.44 epoch sec; seqNo:3182; 4056826.83 north; 590853.67 east 2021-02-12T23:22:29Z- AHRS_M2 msg: 1613172149.81 epoch sec; seqNo:3151 2021-02-12T23:22:29Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.18 psi 2021-02-12T23:22:29Z- handleDepth()-Depth_Keller msg: 1613172149816 epoch sec; seqNo:3183; depth 18.75 2021-02-12T23:22:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:29Z- handleDvl() - RDI_Pathfinder msg: 1613172149.82 epoch sec; seqNo:1421 2021-02-12T23:22:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , -0.04 2021-02-12T23:22:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172149.83 epoch sec; seqNo:3183; 4056827.21 north; 590853.67 east 2021-02-12T23:22:30Z- AHRS_M2 msg: 1613172150.22 epoch sec; seqNo:3152 2021-02-12T23:22:30Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-12T23:22:30Z- handleDepth()-Depth_Keller msg: 1613172150222 epoch sec; seqNo:3184; depth 18.86 2021-02-12T23:22:30Z- handleDvl() - RDI_Pathfinder msg: 1613172150.23 epoch sec; seqNo:1422 2021-02-12T23:22:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.36 , 94.53 , 67.41 , 105.47 2021-02-12T23:22:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.05 2021-02-12T23:22:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172150.24 epoch sec; seqNo:3184; 4056827.59 north; 590853.66 east 2021-02-12T23:22:30Z- AHRS_M2 msg: 1613172150.62 epoch sec; seqNo:3153 2021-02-12T23:22:30Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.20 psi 2021-02-12T23:22:30Z- handleDepth()-Depth_Keller msg: 1613172150625 epoch sec; seqNo:3185; depth 19.00 2021-02-12T23:22:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172150.64 epoch sec; seqNo:3185; 4056827.59 north; 590853.66 east 2021-02-12T23:22:31Z- AHRS_M2 msg: 1613172151.02 epoch sec; seqNo:3154 2021-02-12T23:22:31Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.20 psi 2021-02-12T23:22:31Z- handleDepth()-Depth_Keller msg: 1613172151028 epoch sec; seqNo:3186; depth 19.13 2021-02-12T23:22:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:31Z- handleDvl() - RDI_Pathfinder msg: 1613172151.03 epoch sec; seqNo:1423 2021-02-12T23:22:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.05 2021-02-12T23:22:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172151.04 epoch sec; seqNo:3186; 4056827.97 north; 590853.66 east 2021-02-12T23:22:31Z- AHRS_M2 msg: 1613172151.43 epoch sec; seqNo:3155 2021-02-12T23:22:31Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.20 psi 2021-02-12T23:22:31Z- handleDepth()-Depth_Keller msg: 1613172151432 epoch sec; seqNo:3187; depth 19.26 2021-02-12T23:22:31Z- handleDvl() - RDI_Pathfinder msg: 1613172151.46 epoch sec; seqNo:1424 2021-02-12T23:22:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.72 , 93.31 , 67.82 , 105.47 2021-02-12T23:22:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.03 , -0.04 2021-02-12T23:22:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172151.47 epoch sec; seqNo:3187; 4056828.35 north; 590853.66 east 2021-02-12T23:22:31Z- LcmTrn::handleMessages() - handling 44 messages took 0.63 ms and 132 calls... 2021-02-12T23:22:31Z- waiting for 1613172149.62, time is 1613172151.47 2021-02-12T23:22:31Z- LcmTrn::updateTrn() equating pose 1613172151.47 and meas times 1613172151.47 2021-02-12T23:22:31Z- LcmTrn::updateTrn() >>>> pose time 1613172151.47, meas time 1613172151.47 <<<< 2021-02-12T23:22:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172151.47 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:22:31Z- TerrainNav::motionUpdate() - currEstimate = (4056828, 590853.66, 19.26) 2021-02-12T23:22:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:22:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:22:31Z- TNavBF::Projecting Measurements 2021-02-12T23:22:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:22:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:22:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:22:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:22:31Z- TNavFilter::4 good beams. 2021-02-12T23:22:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:22:31Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:22:31Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:22:31Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:22:31Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:22:31Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:22:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:22:31Z- TNavBF::mmse selected from filter -1 2021-02-12T23:22:31Z- TerrainMapOctree: (vehN, vehE ) = (4056828.35, 590853.66). 2021-02-12T23:22:31Z- TerrainMapOctree: Min Distance = 5575.50. 2021-02-12T23:22:31Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:22:31Z- There is no specific good beam that all particles have in common. 2021-02-12T23:22:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:22:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.58 K: 0.134 ExpectedMeasDiff: nan currMeas.covariance: 8.46 2021-02-12T23:22:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:22:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 13.93 2021-02-12T23:22:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:22:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.122 ExpectedMeasDiff: nan currMeas.covariance: 7.36 2021-02-12T23:22:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:22:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 17.80 2021-02-12T23:22:31Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:22:31Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:22:31Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:22:31Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:22:31Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:22:31Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:22:31Z- Filter 0 No beams used in subcloud update 2021-02-12T23:22:31Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:22:31Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:22:31Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 772.3 EastVariance: 833.1 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:22:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172151.47 sec. 2021-02-12T23:22:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:22:31Z- TNavBF::mmse selected from filter -1 2021-02-12T23:22:31Z- variances([0], [2], and psi_berg) = 772.330537 833.057281 0.000000 2021-02-12T23:22:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:22:31Z- LcmTrn::publishEstimates() - MLE : 4056807.85 590805.16 19.26 2021-02-12T23:22:31Z- LcmTrn::publishEstimates() - MMSE : 4056828.35 590853.66 19.26 2021-02-12T23:22:31Z- LcmTrn::publishEstimates() - COVAR: 772.33 833.06 1.05 0.00 2021-02-12T23:22:31Z- AHRS_M2 msg: 1613172151.83 epoch sec; seqNo:3156 2021-02-12T23:22:31Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.20 psi 2021-02-12T23:22:31Z- handleDepth()-Depth_Keller msg: 1613172151836 epoch sec; seqNo:3188; depth 19.41 2021-02-12T23:22:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172151.85 epoch sec; seqNo:3188; 4056828.74 north; 590854.87 east 2021-02-12T23:22:32Z- AHRS_M2 msg: 1613172152.24 epoch sec; seqNo:3157 2021-02-12T23:22:32Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.20 psi 2021-02-12T23:22:32Z- handleDepth()-Depth_Keller msg: 1613172152274 epoch sec; seqNo:3189; depth 19.54 2021-02-12T23:22:32Z- handleDvl() - RDI_Pathfinder msg: 1613172152.29 epoch sec; seqNo:1425 2021-02-12T23:22:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.32 , 92.09 , 67.00 , 101.82 2021-02-12T23:22:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.10 , -0.01 2021-02-12T23:22:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172152.30 epoch sec; seqNo:3189; 4056829.12 north; 590854.87 east 2021-02-12T23:22:32Z- AHRS_M2 msg: 1613172152.64 epoch sec; seqNo:3158 2021-02-12T23:22:32Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.20 psi 2021-02-12T23:22:32Z- handleDepth()-Depth_Keller msg: 1613172152653 epoch sec; seqNo:3190; depth 19.69 2021-02-12T23:22:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172152.66 epoch sec; seqNo:3190; 4056829.12 north; 590854.87 east 2021-02-12T23:22:33Z- AHRS_M2 msg: 1613172153.04 epoch sec; seqNo:3159 2021-02-12T23:22:33Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.20 psi 2021-02-12T23:22:33Z- handleDepth()-Depth_Keller msg: 1613172153048 epoch sec; seqNo:3191; depth 19.80 2021-02-12T23:22:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172153.07 epoch sec; seqNo:3191; 4056829.50 north; 590854.87 east 2021-02-12T23:22:33Z- AHRS_M2 msg: 1613172153.47 epoch sec; seqNo:3160 2021-02-12T23:22:33Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.20 psi 2021-02-12T23:22:33Z- handleDepth()-Depth_Keller msg: 1613172153478 epoch sec; seqNo:3192; depth 19.92 2021-02-12T23:22:33Z- handleDvl() - RDI_Pathfinder msg: 1613172153.50 epoch sec; seqNo:1426 2021-02-12T23:22:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.32 , 92.23 , 66.60 , 101.96 2021-02-12T23:22:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.12 , 0.02 2021-02-12T23:22:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172153.51 epoch sec; seqNo:3192; 4056829.88 north; 590854.86 east 2021-02-12T23:22:33Z- AHRS_M2 msg: 1613172153.85 epoch sec; seqNo:3161 2021-02-12T23:22:33Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.20 psi 2021-02-12T23:22:33Z- handleDepth()-Depth_Keller msg: 1613172153866 epoch sec; seqNo:3193; depth 20.07 2021-02-12T23:22:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172153.88 epoch sec; seqNo:3193; 4056830.26 north; 590854.86 east 2021-02-12T23:22:34Z- AHRS_M2 msg: 1613172154.26 epoch sec; seqNo:3162 2021-02-12T23:22:34Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.21 psi 2021-02-12T23:22:34Z- handleDepth()-Depth_Keller msg: 1613172154260 epoch sec; seqNo:3194; depth 20.19 2021-02-12T23:22:34Z- handleDvl() - RDI_Pathfinder msg: 1613172154.27 epoch sec; seqNo:1427 2021-02-12T23:22:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.90 , 98.31 , 65.77 , 97.10 2021-02-12T23:22:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.09 , 0.07 2021-02-12T23:22:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172154.30 epoch sec; seqNo:3194; 4056830.63 north; 590854.85 east 2021-02-12T23:22:34Z- AHRS_M2 msg: 1613172154.66 epoch sec; seqNo:3163 2021-02-12T23:22:34Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.21 psi 2021-02-12T23:22:34Z- handleDepth()-Depth_Keller msg: 1613172154672 epoch sec; seqNo:3195; depth 20.32 2021-02-12T23:22:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:34Z- handleDvl() - RDI_Pathfinder msg: 1613172154.68 epoch sec; seqNo:1428 2021-02-12T23:22:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.09 , 0.07 2021-02-12T23:22:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172154.71 epoch sec; seqNo:3195; 4056830.65 north; 590856.07 east 2021-02-12T23:22:35Z- AHRS_M2 msg: 1613172155.06 epoch sec; seqNo:3164 2021-02-12T23:22:35Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.21 psi 2021-02-12T23:22:35Z- handleDepth()-Depth_Keller msg: 1613172155068 epoch sec; seqNo:3196; depth 20.45 2021-02-12T23:22:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172155.08 epoch sec; seqNo:3196; 4056831.03 north; 590856.07 east 2021-02-12T23:22:35Z- AHRS_M2 msg: 1613172155.47 epoch sec; seqNo:3165 2021-02-12T23:22:35Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.21 psi 2021-02-12T23:22:35Z- handleDepth()-Depth_Keller msg: 1613172155477 epoch sec; seqNo:3197; depth 20.58 2021-02-12T23:22:35Z- handleDvl() - RDI_Pathfinder msg: 1613172155.49 epoch sec; seqNo:1429 2021-02-12T23:22:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.90 , 92.23 , 66.59 , 104.39 2021-02-12T23:22:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.10 2021-02-12T23:22:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172155.52 epoch sec; seqNo:3197; 4056831.40 north; 590856.07 east 2021-02-12T23:22:35Z- AHRS_M2 msg: 1613172155.87 epoch sec; seqNo:3166 2021-02-12T23:22:35Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.21 psi 2021-02-12T23:22:35Z- handleDepth()-Depth_Keller msg: 1613172155881 epoch sec; seqNo:3198; depth 20.70 2021-02-12T23:22:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:35Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:35Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:35Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:35Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:35Z- handleDvl() - RDI_Pathfinder msg: 1613172155.88 epoch sec; seqNo:1430 2021-02-12T23:22:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.10 2021-02-12T23:22:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172155.89 epoch sec; seqNo:3198; 4056831.40 north; 590856.07 east 2021-02-12T23:22:36Z- AHRS_M2 msg: 1613172156.28 epoch sec; seqNo:3167 2021-02-12T23:22:36Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.21 psi 2021-02-12T23:22:36Z- handleDepth()-Depth_Keller msg: 1613172156281 epoch sec; seqNo:3199; depth 20.82 2021-02-12T23:22:36Z- handleDvl() - RDI_Pathfinder msg: 1613172156.29 epoch sec; seqNo:1431 2021-02-12T23:22:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.91 , 93.44 , 64.96 , 97.10 2021-02-12T23:22:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.05 , 0.09 2021-02-12T23:22:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172156.31 epoch sec; seqNo:3199; 4056831.78 north; 590856.06 east 2021-02-12T23:22:36Z- AHRS_M2 msg: 1613172156.68 epoch sec; seqNo:3168 2021-02-12T23:22:36Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.20 psi 2021-02-12T23:22:36Z- handleDepth()-Depth_Keller msg: 1613172156684 epoch sec; seqNo:3200; depth 20.95 2021-02-12T23:22:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172156.71 epoch sec; seqNo:3200; 4056832.16 north; 590856.06 east 2021-02-12T23:22:36Z- LcmTrn::handleMessages() - handling 46 messages took 0.81 ms and 130 calls... 2021-02-12T23:22:36Z- waiting for 1613172154.47, time is 1613172156.71 2021-02-12T23:22:36Z- LcmTrn::updateTrn() >>>> pose time 1613172156.71, meas time 1613172156.29 <<<< 2021-02-12T23:22:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172156.29 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:22:36Z- TerrainNav::motionUpdate() - currEstimate = (4056832, 590856.06, 20.95) 2021-02-12T23:22:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:22:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:22:36Z- TNavBF::Projecting Measurements 2021-02-12T23:22:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:22:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:22:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:22:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:22:36Z- TNavFilter::4 good beams. 2021-02-12T23:22:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:22:36Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:22:36Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:22:36Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:22:36Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:22:36Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:22:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:22:36Z- TNavBF::mmse selected from filter -1 2021-02-12T23:22:36Z- TerrainMapOctree: (vehN, vehE ) = (4056831.86, 590855.87). 2021-02-12T23:22:36Z- TerrainMapOctree: Min Distance = 5571.36. 2021-02-12T23:22:36Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:22:36Z- There is no specific good beam that all particles have in common. 2021-02-12T23:22:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:22:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 8.27 2021-02-12T23:22:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:22:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 13.97 2021-02-12T23:22:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:22:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.110 ExpectedMeasDiff: nan currMeas.covariance: 6.75 2021-02-12T23:22:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:22:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 15.08 2021-02-12T23:22:36Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:22:36Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:22:36Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:22:36Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:22:36Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:22:36Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:22:36Z- Filter 0 No beams used in subcloud update 2021-02-12T23:22:36Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:22:36Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:22:36Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 772.2 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:22:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172156.29 sec. 2021-02-12T23:22:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:22:36Z- TNavBF::mmse selected from filter -1 2021-02-12T23:22:36Z- variances([0], [2], and psi_berg) = 772.153599 833.057056 0.000000 2021-02-12T23:22:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:22:36Z- LcmTrn::publishEstimates() - MLE : 4056811.66 590807.56 20.95 2021-02-12T23:22:36Z- LcmTrn::publishEstimates() - MMSE : 4056832.16 590856.06 20.95 2021-02-12T23:22:36Z- LcmTrn::publishEstimates() - COVAR: 772.15 833.06 0.90 0.00 2021-02-12T23:22:37Z- AHRS_M2 msg: 1613172157.08 epoch sec; seqNo:3169 2021-02-12T23:22:37Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.20 psi 2021-02-12T23:22:37Z- handleDepth()-Depth_Keller msg: 1613172157088 epoch sec; seqNo:3201; depth 21.09 2021-02-12T23:22:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:37Z- handleDvl() - RDI_Pathfinder msg: 1613172157.09 epoch sec; seqNo:1432 2021-02-12T23:22:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.05 , 0.09 2021-02-12T23:22:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172157.10 epoch sec; seqNo:3201; 4056832.16 north; 590856.06 east 2021-02-12T23:22:37Z- AHRS_M2 msg: 1613172157.50 epoch sec; seqNo:3170 2021-02-12T23:22:37Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.20 psi 2021-02-12T23:22:37Z- handleDepth()-Depth_Keller msg: 1613172157505 epoch sec; seqNo:3202; depth 21.21 2021-02-12T23:22:37Z- handleDvl() - RDI_Pathfinder msg: 1613172157.52 epoch sec; seqNo:1433 2021-02-12T23:22:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.50 , 93.44 , 64.96 , 96.69 2021-02-12T23:22:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.11 , 0.04 2021-02-12T23:22:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172157.53 epoch sec; seqNo:3202; 4056832.55 north; 590857.28 east 2021-02-12T23:22:37Z- AHRS_M2 msg: 1613172157.90 epoch sec; seqNo:3171 2021-02-12T23:22:37Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-12T23:22:37Z- handleDepth()-Depth_Keller msg: 1613172157903 epoch sec; seqNo:3203; depth 21.34 2021-02-12T23:22:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172157.92 epoch sec; seqNo:3203; 4056832.93 north; 590857.27 east 2021-02-12T23:22:38Z- AHRS_M2 msg: 1613172158.30 epoch sec; seqNo:3172 2021-02-12T23:22:38Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-12T23:22:38Z- handleDepth()-Depth_Keller msg: 1613172158305 epoch sec; seqNo:3204; depth 21.48 2021-02-12T23:22:38Z- handleDvl() - RDI_Pathfinder msg: 1613172158.32 epoch sec; seqNo:1434 2021-02-12T23:22:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.68 , 91.28 , 65.78 , 96.69 2021-02-12T23:22:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.09 , -0.04 2021-02-12T23:22:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172158.34 epoch sec; seqNo:3204; 4056832.93 north; 590857.27 east 2021-02-12T23:22:38Z- AHRS_M2 msg: 1613172158.70 epoch sec; seqNo:3173 2021-02-12T23:22:38Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-12T23:22:38Z- handleDepth()-Depth_Keller msg: 1613172158704 epoch sec; seqNo:3205; depth 21.60 2021-02-12T23:22:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172158.72 epoch sec; seqNo:3205; 4056833.31 north; 590857.27 east 2021-02-12T23:22:39Z- AHRS_M2 msg: 1613172159.10 epoch sec; seqNo:3174 2021-02-12T23:22:39Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-12T23:22:39Z- handleDepth()-Depth_Keller msg: 1613172159111 epoch sec; seqNo:3206; depth 21.73 2021-02-12T23:22:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172159.12 epoch sec; seqNo:3206; 4056833.69 north; 590857.26 east 2021-02-12T23:22:39Z- AHRS_M2 msg: 1613172159.51 epoch sec; seqNo:3175 2021-02-12T23:22:39Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-12T23:22:39Z- handleDepth()-Depth_Keller msg: 1613172159512 epoch sec; seqNo:3207; depth 21.87 2021-02-12T23:22:39Z- handleDvl() - RDI_Pathfinder msg: 1613172159.54 epoch sec; seqNo:1435 2021-02-12T23:22:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.09 , 92.43 , 64.96 , 96.76 2021-02-12T23:22:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , -0.08 2021-02-12T23:22:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172159.55 epoch sec; seqNo:3207; 4056833.70 north; 590858.48 east 2021-02-12T23:22:39Z- AHRS_M2 msg: 1613172159.91 epoch sec; seqNo:3176 2021-02-12T23:22:39Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.20 psi 2021-02-12T23:22:39Z- handleDepth()-Depth_Keller msg: 1613172159921 epoch sec; seqNo:3208; depth 22.00 2021-02-12T23:22:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172159.93 epoch sec; seqNo:3208; 4056834.08 north; 590858.48 east 2021-02-12T23:22:40Z- AHRS_M2 msg: 1613172160.32 epoch sec; seqNo:3177 2021-02-12T23:22:40Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.20 psi 2021-02-12T23:22:40Z- handleDepth()-Depth_Keller msg: 1613172160320 epoch sec; seqNo:3209; depth 22.11 2021-02-12T23:22:40Z- handleDvl() - RDI_Pathfinder msg: 1613172160.33 epoch sec; seqNo:1436 2021-02-12T23:22:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.68 , 90.27 , 65.37 , 96.76 2021-02-12T23:22:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , -0.09 2021-02-12T23:22:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172160.34 epoch sec; seqNo:3209; 4056834.46 north; 590858.48 east 2021-02-12T23:22:40Z- AHRS_M2 msg: 1613172160.72 epoch sec; seqNo:3178 2021-02-12T23:22:40Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.20 psi 2021-02-12T23:22:40Z- handleDepth()-Depth_Keller msg: 1613172160725 epoch sec; seqNo:3210; depth 22.23 2021-02-12T23:22:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:40Z- handleDvl() - RDI_Pathfinder msg: 1613172160.73 epoch sec; seqNo:1437 2021-02-12T23:22:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , -0.09 2021-02-12T23:22:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172160.75 epoch sec; seqNo:3210; 4056834.84 north; 590858.47 east 2021-02-12T23:22:41Z- AHRS_M2 msg: 1613172161.13 epoch sec; seqNo:3179 2021-02-12T23:22:41Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.20 psi 2021-02-12T23:22:41Z- handleDepth()-Depth_Keller msg: 1613172161131 epoch sec; seqNo:3211; depth 22.37 2021-02-12T23:22:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172161.14 epoch sec; seqNo:3211; 4056835.22 north; 590858.47 east 2021-02-12T23:22:41Z- AHRS_M2 msg: 1613172161.53 epoch sec; seqNo:3180 2021-02-12T23:22:41Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-12T23:22:41Z- handleDepth()-Depth_Keller msg: 1613172161537 epoch sec; seqNo:3212; depth 22.49 2021-02-12T23:22:41Z- handleDvl() - RDI_Pathfinder msg: 1613172161.55 epoch sec; seqNo:1438 2021-02-12T23:22:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.86 , 92.43 , 64.96 , 96.76 2021-02-12T23:22:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , -0.06 2021-02-12T23:22:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172161.56 epoch sec; seqNo:3212; 4056835.22 north; 590858.47 east 2021-02-12T23:22:41Z- LcmTrn::handleMessages() - handling 43 messages took 0.62 ms and 131 calls... 2021-02-12T23:22:41Z- waiting for 1613172159.71, time is 1613172161.56 2021-02-12T23:22:41Z- LcmTrn::updateTrn() equating pose 1613172161.56 and meas times 1613172161.56 2021-02-12T23:22:41Z- LcmTrn::updateTrn() >>>> pose time 1613172161.56, meas time 1613172161.56 <<<< 2021-02-12T23:22:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172161.56 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:22:41Z- TerrainNav::motionUpdate() - currEstimate = (4056835, 590858.47, 22.49) 2021-02-12T23:22:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:22:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:22:41Z- TNavBF::Projecting Measurements 2021-02-12T23:22:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:22:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:22:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:22:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:22:41Z- TNavFilter::4 good beams. 2021-02-12T23:22:41Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:22:41Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:22:41Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:22:41Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:22:41Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:22:41Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:22:41Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:22:41Z- TNavBF::mmse selected from filter -1 2021-02-12T23:22:41Z- TerrainMapOctree: (vehN, vehE ) = (4056835.22, 590858.47). 2021-02-12T23:22:41Z- TerrainMapOctree: Min Distance = 5567.12. 2021-02-12T23:22:41Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:22:41Z- There is no specific good beam that all particles have in common. 2021-02-12T23:22:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:22:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 7.81 2021-02-12T23:22:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:22:41Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 13.67 2021-02-12T23:22:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:22:41Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 6.75 2021-02-12T23:22:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:22:41Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 14.98 2021-02-12T23:22:41Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:22:41Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:22:41Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:22:41Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:22:41Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:22:41Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:22:41Z- Filter 0 No beams used in subcloud update 2021-02-12T23:22:41Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:22:41Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:22:41Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 772.0 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:22:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172161.56 sec. 2021-02-12T23:22:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:22:41Z- TNavBF::mmse selected from filter -1 2021-02-12T23:22:41Z- variances([0], [2], and psi_berg) = 772.021366 833.056889 0.000000 2021-02-12T23:22:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:22:41Z- LcmTrn::publishEstimates() - MLE : 4056814.72 590809.97 22.49 2021-02-12T23:22:41Z- LcmTrn::publishEstimates() - MMSE : 4056835.22 590858.47 22.49 2021-02-12T23:22:41Z- LcmTrn::publishEstimates() - COVAR: 772.02 833.06 0.79 0.00 2021-02-12T23:22:41Z- AHRS_M2 msg: 1613172161.93 epoch sec; seqNo:3181 2021-02-12T23:22:41Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-12T23:22:41Z- handleDepth()-Depth_Keller msg: 1613172161937 epoch sec; seqNo:3213; depth 22.63 2021-02-12T23:22:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:41Z- handleDvl() - RDI_Pathfinder msg: 1613172161.94 epoch sec; seqNo:1439 2021-02-12T23:22:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , -0.06 2021-02-12T23:22:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172161.95 epoch sec; seqNo:3213; 4056835.61 north; 590859.69 east 2021-02-12T23:22:42Z- AHRS_M2 msg: 1613172162.34 epoch sec; seqNo:3182 2021-02-12T23:22:42Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-12T23:22:42Z- handleDepth()-Depth_Keller msg: 1613172162350 epoch sec; seqNo:3214; depth 22.74 2021-02-12T23:22:42Z- handleDvl() - RDI_Pathfinder msg: 1613172162.36 epoch sec; seqNo:1440 2021-02-12T23:22:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.86 , 90.27 , 64.55 , 96.76 2021-02-12T23:22:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.03 2021-02-12T23:22:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172162.37 epoch sec; seqNo:3214; 4056835.99 north; 590859.68 east 2021-02-12T23:22:42Z- AHRS_M2 msg: 1613172162.74 epoch sec; seqNo:3183 2021-02-12T23:22:42Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-12T23:22:42Z- handleDepth()-Depth_Keller msg: 1613172162744 epoch sec; seqNo:3215; depth 22.88 2021-02-12T23:22:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172162.76 epoch sec; seqNo:3215; 4056836.37 north; 590859.68 east 2021-02-12T23:22:43Z- AHRS_M2 msg: 1613172163.15 epoch sec; seqNo:3184 2021-02-12T23:22:43Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-12T23:22:43Z- handleDepth()-Depth_Keller msg: 1613172163150 epoch sec; seqNo:3216; depth 23.00 2021-02-12T23:22:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:43Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:43Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:43Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:43Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:43Z- handleDvl() - RDI_Pathfinder msg: 1613172163.15 epoch sec; seqNo:1441 2021-02-12T23:22:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.03 2021-02-12T23:22:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172163.16 epoch sec; seqNo:3216; 4056836.75 north; 590859.67 east 2021-02-12T23:22:43Z- AHRS_M2 msg: 1613172163.55 epoch sec; seqNo:3185 2021-02-12T23:22:43Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-12T23:22:43Z- handleDepth()-Depth_Keller msg: 1613172163557 epoch sec; seqNo:3217; depth 23.13 2021-02-12T23:22:43Z- handleDvl() - RDI_Pathfinder msg: 1613172163.57 epoch sec; seqNo:1442 2021-02-12T23:22:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.05 , 89.25 , 64.14 , 96.82 2021-02-12T23:22:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.03 2021-02-12T23:22:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172163.58 epoch sec; seqNo:3217; 4056836.75 north; 590859.67 east 2021-02-12T23:22:43Z- AHRS_M2 msg: 1613172163.95 epoch sec; seqNo:3186 2021-02-12T23:22:43Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-12T23:22:43Z- handleDepth()-Depth_Keller msg: 1613172163956 epoch sec; seqNo:3218; depth 23.26 2021-02-12T23:22:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172163.98 epoch sec; seqNo:3218; 4056837.13 north; 590859.67 east 2021-02-12T23:22:44Z- AHRS_M2 msg: 1613172164.36 epoch sec; seqNo:3187 2021-02-12T23:22:44Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-12T23:22:44Z- handleDepth()-Depth_Keller msg: 1613172164360 epoch sec; seqNo:3219; depth 23.38 2021-02-12T23:22:44Z- handleDvl() - RDI_Pathfinder msg: 1613172164.37 epoch sec; seqNo:1443 2021-02-12T23:22:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.64 , 88.17 , 64.14 , 95.74 2021-02-12T23:22:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , -0.03 2021-02-12T23:22:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172164.39 epoch sec; seqNo:3219; 4056837.52 north; 590860.89 east 2021-02-12T23:22:44Z- AHRS_M2 msg: 1613172164.76 epoch sec; seqNo:3188 2021-02-12T23:22:44Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-12T23:22:44Z- handleDepth()-Depth_Keller msg: 1613172164763 epoch sec; seqNo:3220; depth 23.52 2021-02-12T23:22:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172164.78 epoch sec; seqNo:3220; 4056837.90 north; 590860.88 east 2021-02-12T23:22:45Z- AHRS_M2 msg: 1613172165.17 epoch sec; seqNo:3189 2021-02-12T23:22:45Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-12T23:22:45Z- handleDepth()-Depth_Keller msg: 1613172165178 epoch sec; seqNo:3221; depth 23.62 2021-02-12T23:22:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172165.20 epoch sec; seqNo:3221; 4056838.28 north; 590860.88 east 2021-02-12T23:22:45Z- AHRS_M2 msg: 1613172165.57 epoch sec; seqNo:3190 2021-02-12T23:22:45Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-12T23:22:45Z- handleDepth()-Depth_Keller msg: 1613172165573 epoch sec; seqNo:3222; depth 23.76 2021-02-12T23:22:45Z- handleDvl() - RDI_Pathfinder msg: 1613172165.59 epoch sec; seqNo:1444 2021-02-12T23:22:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.64 , 88.17 , 63.74 , 96.82 2021-02-12T23:22:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , -0.04 2021-02-12T23:22:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172165.60 epoch sec; seqNo:3222; 4056838.28 north; 590860.88 east 2021-02-12T23:22:45Z- AHRS_M2 msg: 1613172165.97 epoch sec; seqNo:3191 2021-02-12T23:22:45Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-12T23:22:45Z- handleDepth()-Depth_Keller msg: 1613172165976 epoch sec; seqNo:3223; depth 23.89 2021-02-12T23:22:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172165.99 epoch sec; seqNo:3223; 4056838.65 north; 590860.87 east 2021-02-12T23:22:46Z- AHRS_M2 msg: 1613172166.38 epoch sec; seqNo:3192 2021-02-12T23:22:46Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-12T23:22:46Z- handleDepth()-Depth_Keller msg: 1613172166385 epoch sec; seqNo:3224; depth 24.01 2021-02-12T23:22:46Z- handleDvl() - RDI_Pathfinder msg: 1613172166.40 epoch sec; seqNo:1445 2021-02-12T23:22:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.64 , 0.00 , 63.33 , 97.91 2021-02-12T23:22:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , -0.02 2021-02-12T23:22:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172166.41 epoch sec; seqNo:3224; 4056839.03 north; 590860.87 east 2021-02-12T23:22:46Z- AHRS_M2 msg: 1613172166.78 epoch sec; seqNo:3193 2021-02-12T23:22:46Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-12T23:22:46Z- handleDepth()-Depth_Keller msg: 1613172166786 epoch sec; seqNo:3225; depth 24.15 2021-02-12T23:22:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:46Z- handleDvl() - RDI_Pathfinder msg: 1613172166.79 epoch sec; seqNo:1446 2021-02-12T23:22:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , -0.02 2021-02-12T23:22:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172166.80 epoch sec; seqNo:3225; 4056839.41 north; 590860.87 east 2021-02-12T23:22:47Z- LcmTrn::handleMessages() - handling 47 messages took 0.66 ms and 134 calls... 2021-02-12T23:22:47Z- waiting for 1613172164.56, time is 1613172166.80 2021-02-12T23:22:47Z- LcmTrn::updateTrn() equating pose 1613172166.80 and meas times 1613172166.80 2021-02-12T23:22:47Z- LcmTrn::updateTrn() >>>> pose time 1613172166.80, meas time 1613172166.80 <<<< 2021-02-12T23:22:47Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172166.80, ping # 0. 2021-02-12T23:22:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172166.80 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:22:47Z- TerrainNav::motionUpdate() - currEstimate = (4056839, 590860.87, 24.15) 2021-02-12T23:22:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:22:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:22:47Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:22:47Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:22:47Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172166.80 sec.; No relevant navigation data available 2021-02-12T23:22:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:22:47Z- TNavBF::mmse selected from filter -1 2021-02-12T23:22:47Z- variances([0], [2], and psi_berg) = 771.874367 833.056703 0.000000 2021-02-12T23:22:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:22:47Z- LcmTrn::publishEstimates() - MLE : 4056818.91 590812.37 24.15 2021-02-12T23:22:47Z- LcmTrn::publishEstimates() - MMSE : 4056839.41 590860.87 24.15 2021-02-12T23:22:47Z- LcmTrn::publishEstimates() - COVAR: 771.87 833.06 1.04 0.00 2021-02-12T23:22:47Z- AHRS_M2 msg: 1613172167.18 epoch sec; seqNo:3194 2021-02-12T23:22:47Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-12T23:22:47Z- handleDepth()-Depth_Keller msg: 1613172167188 epoch sec; seqNo:3226; depth 24.27 2021-02-12T23:22:47Z- handleDvl() - RDI_Pathfinder msg: 1613172167.21 epoch sec; seqNo:1447 2021-02-12T23:22:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.64 , 0.00 , 62.92 , 96.82 2021-02-12T23:22:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , 0.02 2021-02-12T23:22:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172167.22 epoch sec; seqNo:3226; 4056839.43 north; 590862.09 east 2021-02-12T23:22:47Z- AHRS_M2 msg: 1613172167.59 epoch sec; seqNo:3195 2021-02-12T23:22:47Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-12T23:22:47Z- handleDepth()-Depth_Keller msg: 1613172167593 epoch sec; seqNo:3227; depth 24.41 2021-02-12T23:22:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172167.60 epoch sec; seqNo:3227; 4056839.80 north; 590862.08 east 2021-02-12T23:22:47Z- AHRS_M2 msg: 1613172167.99 epoch sec; seqNo:3196 2021-02-12T23:22:47Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-12T23:22:48Z- handleDepth()-Depth_Keller msg: 1613172168001 epoch sec; seqNo:3228; depth 24.54 2021-02-12T23:22:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:48Z- handleDvl() - RDI_Pathfinder msg: 1613172168.00 epoch sec; seqNo:1448 2021-02-12T23:22:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , 0.02 2021-02-12T23:22:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172168.01 epoch sec; seqNo:3228; 4056840.18 north; 590862.08 east 2021-02-12T23:22:48Z- AHRS_M2 msg: 1613172168.39 epoch sec; seqNo:3197 2021-02-12T23:22:48Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-12T23:22:48Z- handleDepth()-Depth_Keller msg: 1613172168425 epoch sec; seqNo:3229; depth 24.66 2021-02-12T23:22:48Z- handleDvl() - RDI_Pathfinder msg: 1613172168.47 epoch sec; seqNo:1449 2021-02-12T23:22:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.82 , 88.17 , 62.92 , 96.82 2021-02-12T23:22:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.04 2021-02-12T23:22:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172168.48 epoch sec; seqNo:3229; 4056840.56 north; 590862.08 east 2021-02-12T23:22:48Z- AHRS_M2 msg: 1613172168.80 epoch sec; seqNo:3198 2021-02-12T23:22:48Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-12T23:22:48Z- handleDepth()-Depth_Keller msg: 1613172168809 epoch sec; seqNo:3230; depth 24.82 2021-02-12T23:22:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172168.83 epoch sec; seqNo:3230; 4056840.56 north; 590862.08 east 2021-02-12T23:22:49Z- AHRS_M2 msg: 1613172169.22 epoch sec; seqNo:3199 2021-02-12T23:22:49Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-12T23:22:49Z- handleDepth()-Depth_Keller msg: 1613172169222 epoch sec; seqNo:3231; depth 24.91 2021-02-12T23:22:49Z- handleDvl() - RDI_Pathfinder msg: 1613172169.24 epoch sec; seqNo:1450 2021-02-12T23:22:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.82 , 88.17 , 62.10 , 97.91 2021-02-12T23:22:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , 0.05 2021-02-12T23:22:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172169.25 epoch sec; seqNo:3231; 4056840.94 north; 590862.07 east 2021-02-12T23:22:49Z- AHRS_M2 msg: 1613172169.61 epoch sec; seqNo:3200 2021-02-12T23:22:49Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-12T23:22:49Z- handleDepth()-Depth_Keller msg: 1613172169625 epoch sec; seqNo:3232; depth 25.05 2021-02-12T23:22:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172169.64 epoch sec; seqNo:3232; 4056841.32 north; 590862.07 east 2021-02-12T23:22:50Z- AHRS_M2 msg: 1613172170.01 epoch sec; seqNo:3201 2021-02-12T23:22:50Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-12T23:22:50Z- handleDepth()-Depth_Keller msg: 1613172170016 epoch sec; seqNo:3233; depth 25.18 2021-02-12T23:22:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172170.03 epoch sec; seqNo:3233; 4056841.33 north; 590863.29 east 2021-02-12T23:22:50Z- AHRS_M2 msg: 1613172170.41 epoch sec; seqNo:3202 2021-02-12T23:22:50Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-12T23:22:50Z- handleDepth()-Depth_Keller msg: 1613172170425 epoch sec; seqNo:3234; depth 25.32 2021-02-12T23:22:50Z- handleDvl() - RDI_Pathfinder msg: 1613172170.44 epoch sec; seqNo:1451 2021-02-12T23:22:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.82 , 0.00 , 62.10 , 96.82 2021-02-12T23:22:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , 0.01 2021-02-12T23:22:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172170.45 epoch sec; seqNo:3234; 4056841.71 north; 590863.28 east 2021-02-12T23:22:50Z- AHRS_M2 msg: 1613172170.82 epoch sec; seqNo:3203 2021-02-12T23:22:50Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-12T23:22:50Z- handleDepth()-Depth_Keller msg: 1613172170837 epoch sec; seqNo:3235; depth 25.43 2021-02-12T23:22:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172170.85 epoch sec; seqNo:3235; 4056842.09 north; 590863.28 east 2021-02-12T23:22:51Z- AHRS_M2 msg: 1613172171.22 epoch sec; seqNo:3204 2021-02-12T23:22:51Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-12T23:22:51Z- handleDepth()-Depth_Keller msg: 1613172171237 epoch sec; seqNo:3236; depth 25.57 2021-02-12T23:22:51Z- handleDvl() - RDI_Pathfinder msg: 1613172171.26 epoch sec; seqNo:1452 2021-02-12T23:22:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.82 , 89.25 , 62.51 , 97.91 2021-02-12T23:22:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , -0.02 2021-02-12T23:22:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172171.27 epoch sec; seqNo:3236; 4056842.09 north; 590863.28 east 2021-02-12T23:22:51Z- AHRS_M2 msg: 1613172171.63 epoch sec; seqNo:3205 2021-02-12T23:22:51Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-12T23:22:51Z- handleDepth()-Depth_Keller msg: 1613172171632 epoch sec; seqNo:3237; depth 25.70 2021-02-12T23:22:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:51Z- handleDvl() - RDI_Pathfinder msg: 1613172171.63 epoch sec; seqNo:1453 2021-02-12T23:22:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , -0.02 2021-02-12T23:22:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172171.64 epoch sec; seqNo:3237; 4056842.47 north; 590863.28 east 2021-02-12T23:22:52Z- AHRS_M2 msg: 1613172172.03 epoch sec; seqNo:3206 2021-02-12T23:22:52Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-12T23:22:52Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-12T23:22:52Z- waiting for 1613172169.80, time is 1613172172.03 2021-02-12T23:22:52Z- LcmTrn::updateTrn() >>>> pose time 1613172172.03, meas time 1613172171.63 <<<< 2021-02-12T23:22:52Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172171.63, ping # 0. 2021-02-12T23:22:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172171.63 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:22:52Z- TerrainNav::motionUpdate() - currEstimate = (4056842, 590863.28, 25.70) 2021-02-12T23:22:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:22:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:22:52Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:22:52Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:22:52Z- TNavBF::Projecting Measurements 2021-02-12T23:22:52Z- TNavFilter::Measurement from time = 1613172171.63 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:22:52Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:22:52Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:22:52Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:22:52Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:22:52Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:22:52Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:22:52Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:22:52Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 771.7 EastVariance: 833.1 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:22:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:22:52Z- TNavBF::mmse selected from filter -1 2021-02-12T23:22:52Z- variances([0], [2], and psi_berg) = 771.711593 833.056498 0.000000 2021-02-12T23:22:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:22:52Z- LcmTrn::publishEstimates() - MLE : 4056821.97 590814.78 25.70 2021-02-12T23:22:52Z- LcmTrn::publishEstimates() - MMSE : 4056842.47 590863.28 25.70 2021-02-12T23:22:52Z- LcmTrn::publishEstimates() - COVAR: 771.71 833.06 1.24 0.00 2021-02-12T23:22:52Z- handleDepth()-Depth_Keller msg: 1613172172041 epoch sec; seqNo:3238; depth 25.83 2021-02-12T23:22:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172172.05 epoch sec; seqNo:3238; 4056842.86 north; 590864.49 east 2021-02-12T23:22:52Z- AHRS_M2 msg: 1613172172.44 epoch sec; seqNo:3207 2021-02-12T23:22:52Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-12T23:22:52Z- handleDepth()-Depth_Keller msg: 1613172172442 epoch sec; seqNo:3239; depth 25.97 2021-02-12T23:22:52Z- handleDvl() - RDI_Pathfinder msg: 1613172172.45 epoch sec; seqNo:1454 2021-02-12T23:22:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.60 , 88.17 , 61.69 , 96.82 2021-02-12T23:22:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.06 2021-02-12T23:22:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172172.46 epoch sec; seqNo:3239; 4056843.24 north; 590864.49 east 2021-02-12T23:22:52Z- AHRS_M2 msg: 1613172172.84 epoch sec; seqNo:3208 2021-02-12T23:22:52Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.20 psi 2021-02-12T23:22:52Z- handleDepth()-Depth_Keller msg: 1613172172849 epoch sec; seqNo:3240; depth 26.11 2021-02-12T23:22:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:52Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:52Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:52Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:52Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:52Z- handleDvl() - RDI_Pathfinder msg: 1613172172.85 epoch sec; seqNo:1455 2021-02-12T23:22:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.06 2021-02-12T23:22:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172172.87 epoch sec; seqNo:3240; 4056843.24 north; 590864.49 east 2021-02-12T23:22:53Z- AHRS_M2 msg: 1613172173.24 epoch sec; seqNo:3209 2021-02-12T23:22:53Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.20 psi 2021-02-12T23:22:53Z- handleDepth()-Depth_Keller msg: 1613172173253 epoch sec; seqNo:3241; depth 26.24 2021-02-12T23:22:53Z- handleDvl() - RDI_Pathfinder msg: 1613172173.26 epoch sec; seqNo:1456 2021-02-12T23:22:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.60 , 87.09 , 61.69 , 96.82 2021-02-12T23:22:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , -0.08 2021-02-12T23:22:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172173.29 epoch sec; seqNo:3241; 4056843.62 north; 590864.49 east 2021-02-12T23:22:53Z- AHRS_M2 msg: 1613172173.65 epoch sec; seqNo:3210 2021-02-12T23:22:53Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.20 psi 2021-02-12T23:22:53Z- handleDepth()-Depth_Keller msg: 1613172173655 epoch sec; seqNo:3242; depth 26.37 2021-02-12T23:22:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172173.67 epoch sec; seqNo:3242; 4056844.00 north; 590864.48 east 2021-02-12T23:22:54Z- AHRS_M2 msg: 1613172174.05 epoch sec; seqNo:3211 2021-02-12T23:22:54Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.20 psi 2021-02-12T23:22:54Z- handleDepth()-Depth_Keller msg: 1613172174057 epoch sec; seqNo:3243; depth 26.48 2021-02-12T23:22:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:54Z- handleDvl() - RDI_Pathfinder msg: 1613172174.06 epoch sec; seqNo:1457 2021-02-12T23:22:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , -0.08 2021-02-12T23:22:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172174.07 epoch sec; seqNo:3243; 4056844.38 north; 590864.48 east 2021-02-12T23:22:54Z- AHRS_M2 msg: 1613172174.45 epoch sec; seqNo:3212 2021-02-12T23:22:54Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.20 psi 2021-02-12T23:22:54Z- handleDepth()-Depth_Keller msg: 1613172174471 epoch sec; seqNo:3244; depth 26.62 2021-02-12T23:22:54Z- handleDvl() - RDI_Pathfinder msg: 1613172174.48 epoch sec; seqNo:1458 2021-02-12T23:22:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.78 , 87.09 , 61.28 , 94.66 2021-02-12T23:22:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , -0.08 2021-02-12T23:22:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172174.49 epoch sec; seqNo:3244; 4056844.77 north; 590865.69 east 2021-02-12T23:22:54Z- AHRS_M2 msg: 1613172174.87 epoch sec; seqNo:3213 2021-02-12T23:22:54Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.20 psi 2021-02-12T23:22:54Z- handleDepth()-Depth_Keller msg: 1613172174870 epoch sec; seqNo:3245; depth 26.75 2021-02-12T23:22:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172174.89 epoch sec; seqNo:3245; 4056844.77 north; 590865.69 east 2021-02-12T23:22:55Z- AHRS_M2 msg: 1613172175.27 epoch sec; seqNo:3214 2021-02-12T23:22:55Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-12T23:22:55Z- handleDepth()-Depth_Keller msg: 1613172175271 epoch sec; seqNo:3246; depth 26.89 2021-02-12T23:22:55Z- handleDvl() - RDI_Pathfinder msg: 1613172175.28 epoch sec; seqNo:1459 2021-02-12T23:22:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.19 , 86.01 , 60.47 , 95.74 2021-02-12T23:22:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , -0.05 2021-02-12T23:22:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172175.29 epoch sec; seqNo:3246; 4056845.15 north; 590865.69 east 2021-02-12T23:22:55Z- AHRS_M2 msg: 1613172175.67 epoch sec; seqNo:3215 2021-02-12T23:22:55Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-12T23:22:55Z- handleDepth()-Depth_Keller msg: 1613172175674 epoch sec; seqNo:3247; depth 27.02 2021-02-12T23:22:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172175.69 epoch sec; seqNo:3247; 4056845.53 north; 590865.69 east 2021-02-12T23:22:56Z- AHRS_M2 msg: 1613172176.07 epoch sec; seqNo:3216 2021-02-12T23:22:56Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-12T23:22:56Z- handleDepth()-Depth_Keller msg: 1613172176076 epoch sec; seqNo:3248; depth 27.15 2021-02-12T23:22:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172176.09 epoch sec; seqNo:3248; 4056845.90 north; 590865.68 east 2021-02-12T23:22:56Z- AHRS_M2 msg: 1613172176.47 epoch sec; seqNo:3217 2021-02-12T23:22:56Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-12T23:22:56Z- handleDepth()-Depth_Keller msg: 1613172176481 epoch sec; seqNo:3249; depth 27.26 2021-02-12T23:22:56Z- handleDvl() - RDI_Pathfinder msg: 1613172176.49 epoch sec; seqNo:1460 2021-02-12T23:22:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.78 , 87.09 , 59.65 , 95.74 2021-02-12T23:22:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.08 , -0.02 2021-02-12T23:22:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172176.50 epoch sec; seqNo:3249; 4056846.28 north; 590865.68 east 2021-02-12T23:22:56Z- AHRS_M2 msg: 1613172176.88 epoch sec; seqNo:3218 2021-02-12T23:22:56Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-12T23:22:56Z- handleDepth()-Depth_Keller msg: 1613172176889 epoch sec; seqNo:3250; depth 27.42 2021-02-12T23:22:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172176.90 epoch sec; seqNo:3250; 4056846.28 north; 590865.68 east 2021-02-12T23:22:57Z- LcmTrn::handleMessages() - handling 45 messages took 0.65 ms and 132 calls... 2021-02-12T23:22:57Z- waiting for 1613172175.03, time is 1613172176.90 2021-02-12T23:22:57Z- LcmTrn::updateTrn() >>>> pose time 1613172176.90, meas time 1613172176.49 <<<< 2021-02-12T23:22:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172176.49 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:22:57Z- TerrainNav::motionUpdate() - currEstimate = (4056846, 590865.68, 27.42) 2021-02-12T23:22:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:22:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:22:57Z- TNavBF::Projecting Measurements 2021-02-12T23:22:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:22:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:22:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:22:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:22:57Z- TNavFilter::4 good beams. 2021-02-12T23:22:57Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:22:57Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:22:57Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:22:57Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:22:57Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:22:57Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:22:57Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:22:57Z- TNavBF::mmse selected from filter -1 2021-02-12T23:22:57Z- TerrainMapOctree: (vehN, vehE ) = (4056845.96, 590865.48). 2021-02-12T23:22:57Z- TerrainMapOctree: Min Distance = 5554.33. 2021-02-12T23:22:57Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:22:57Z- There is no specific good beam that all particles have in common. 2021-02-12T23:22:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:22:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.45 K: 0.143 ExpectedMeasDiff: nan currMeas.covariance: 6.92 2021-02-12T23:22:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:22:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 12.14 2021-02-12T23:22:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:22:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.133 ExpectedMeasDiff: nan currMeas.covariance: 5.69 2021-02-12T23:22:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:22:57Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 14.67 2021-02-12T23:22:57Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:22:57Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:22:57Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:22:57Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:22:57Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:22:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:22:57Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:22:57Z- Filter 0 No beams used in subcloud update 2021-02-12T23:22:57Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:22:57Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:22:57Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 771.6 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:22:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172176.49 sec. 2021-02-12T23:22:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:22:57Z- TNavBF::mmse selected from filter -1 2021-02-12T23:22:57Z- variances([0], [2], and psi_berg) = 771.535521 833.056276 0.000000 2021-02-12T23:22:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:22:57Z- LcmTrn::publishEstimates() - MLE : 4056825.78 590817.18 27.42 2021-02-12T23:22:57Z- LcmTrn::publishEstimates() - MMSE : 4056846.28 590865.68 27.42 2021-02-12T23:22:57Z- LcmTrn::publishEstimates() - COVAR: 771.54 833.06 0.95 0.00 2021-02-12T23:22:57Z- AHRS_M2 msg: 1613172177.28 epoch sec; seqNo:3219 2021-02-12T23:22:57Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-12T23:22:57Z- handleDepth()-Depth_Keller msg: 1613172177298 epoch sec; seqNo:3251; depth 27.55 2021-02-12T23:22:57Z- handleDvl() - RDI_Pathfinder msg: 1613172177.31 epoch sec; seqNo:1461 2021-02-12T23:22:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.37 , 84.93 , 60.06 , 94.66 2021-02-12T23:22:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.02 2021-02-12T23:22:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172177.32 epoch sec; seqNo:3251; 4056846.68 north; 590866.90 east 2021-02-12T23:22:57Z- AHRS_M2 msg: 1613172177.69 epoch sec; seqNo:3220 2021-02-12T23:22:57Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-12T23:22:57Z- handleDepth()-Depth_Keller msg: 1613172177697 epoch sec; seqNo:3252; depth 27.68 2021-02-12T23:22:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:57Z- handleDvl() - RDI_Pathfinder msg: 1613172177.70 epoch sec; seqNo:1462 2021-02-12T23:22:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.02 2021-02-12T23:22:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172177.72 epoch sec; seqNo:3252; 4056847.05 north; 590866.89 east 2021-02-12T23:22:58Z- AHRS_M2 msg: 1613172178.10 epoch sec; seqNo:3221 2021-02-12T23:22:58Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-12T23:22:58Z- handleDepth()-Depth_Keller msg: 1613172178100 epoch sec; seqNo:3253; depth 27.82 2021-02-12T23:22:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172178.14 epoch sec; seqNo:3253; 4056847.43 north; 590866.89 east 2021-02-12T23:22:58Z- AHRS_M2 msg: 1613172178.50 epoch sec; seqNo:3222 2021-02-12T23:22:58Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-12T23:22:58Z- handleDepth()-Depth_Keller msg: 1613172178507 epoch sec; seqNo:3254; depth 27.95 2021-02-12T23:22:58Z- handleDvl() - RDI_Pathfinder msg: 1613172178.52 epoch sec; seqNo:1463 2021-02-12T23:22:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.96 , 78.85 , 59.65 , 93.58 2021-02-12T23:22:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , 0.03 2021-02-12T23:22:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172178.53 epoch sec; seqNo:3254; 4056847.43 north; 590866.89 east 2021-02-12T23:22:58Z- AHRS_M2 msg: 1613172178.90 epoch sec; seqNo:3223 2021-02-12T23:22:58Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-12T23:22:58Z- handleDepth()-Depth_Keller msg: 1613172178914 epoch sec; seqNo:3255; depth 28.08 2021-02-12T23:22:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:22:58Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:22:58Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:22:58Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:22:58Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:22:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:22:58Z- handleDvl() - RDI_Pathfinder msg: 1613172178.92 epoch sec; seqNo:1464 2021-02-12T23:22:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:22:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , 0.03 2021-02-12T23:22:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172178.93 epoch sec; seqNo:3255; 4056847.81 north; 590866.88 east 2021-02-12T23:22:59Z- AHRS_M2 msg: 1613172179.31 epoch sec; seqNo:3224 2021-02-12T23:22:59Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-12T23:22:59Z- handleDepth()-Depth_Keller msg: 1613172179313 epoch sec; seqNo:3256; depth 28.21 2021-02-12T23:22:59Z- handleDvl() - RDI_Pathfinder msg: 1613172179.34 epoch sec; seqNo:1465 2021-02-12T23:22:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.96 , 78.04 , 59.24 , 97.91 2021-02-12T23:22:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , 0.01 2021-02-12T23:22:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172179.35 epoch sec; seqNo:3256; 4056848.19 north; 590866.88 east 2021-02-12T23:22:59Z- AHRS_M2 msg: 1613172179.71 epoch sec; seqNo:3225 2021-02-12T23:22:59Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-12T23:22:59Z- handleDepth()-Depth_Keller msg: 1613172179712 epoch sec; seqNo:3257; depth 28.36 2021-02-12T23:22:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172179.72 epoch sec; seqNo:3257; 4056848.20 north; 590868.10 east 2021-02-12T23:23:00Z- AHRS_M2 msg: 1613172180.14 epoch sec; seqNo:3226 2021-02-12T23:23:00Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.17 psi 2021-02-12T23:23:00Z- handleDepth()-Depth_Keller msg: 1613172180145 epoch sec; seqNo:3258; depth 28.48 2021-02-12T23:23:00Z- handleDvl() - RDI_Pathfinder msg: 1613172180.16 epoch sec; seqNo:1466 2021-02-12T23:23:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.55 , 78.04 , 58.83 , 97.91 2021-02-12T23:23:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , -0.02 2021-02-12T23:23:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172180.18 epoch sec; seqNo:3258; 4056848.58 north; 590868.10 east 2021-02-12T23:23:00Z- AHRS_M2 msg: 1613172180.52 epoch sec; seqNo:3227 2021-02-12T23:23:00Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.17 psi 2021-02-12T23:23:00Z- handleDepth()-Depth_Keller msg: 1613172180525 epoch sec; seqNo:3259; depth 28.63 2021-02-12T23:23:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172180.55 epoch sec; seqNo:3259; 4056848.96 north; 590868.09 east 2021-02-12T23:23:00Z- AHRS_M2 msg: 1613172180.92 epoch sec; seqNo:3228 2021-02-12T23:23:00Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.17 psi 2021-02-12T23:23:00Z- handleDepth()-Depth_Keller msg: 1613172180924 epoch sec; seqNo:3260; depth 28.76 2021-02-12T23:23:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172180.93 epoch sec; seqNo:3260; 4056849.34 north; 590868.09 east 2021-02-12T23:23:01Z- AHRS_M2 msg: 1613172181.33 epoch sec; seqNo:3229 2021-02-12T23:23:01Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.18 psi 2021-02-12T23:23:01Z- handleDepth()-Depth_Keller msg: 1613172181330 epoch sec; seqNo:3261; depth 28.86 2021-02-12T23:23:01Z- handleDvl() - RDI_Pathfinder msg: 1613172181.34 epoch sec; seqNo:1467 2021-02-12T23:23:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.55 , 77.22 , 58.83 , 93.58 2021-02-12T23:23:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.03 , -0.05 2021-02-12T23:23:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172181.35 epoch sec; seqNo:3261; 4056849.34 north; 590868.09 east 2021-02-12T23:23:01Z- AHRS_M2 msg: 1613172181.73 epoch sec; seqNo:3230 2021-02-12T23:23:01Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.18 psi 2021-02-12T23:23:01Z- handleDepth()-Depth_Keller msg: 1613172181732 epoch sec; seqNo:3262; depth 29.02 2021-02-12T23:23:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172181.76 epoch sec; seqNo:3262; 4056849.72 north; 590868.08 east 2021-02-12T23:23:02Z- AHRS_M2 msg: 1613172182.15 epoch sec; seqNo:3231 2021-02-12T23:23:02Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-12T23:23:02Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-12T23:23:02Z- waiting for 1613172179.90, time is 1613172182.15 2021-02-12T23:23:02Z- LcmTrn::updateTrn() >>>> pose time 1613172182.15, meas time 1613172181.34 <<<< 2021-02-12T23:23:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172181.34 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:23:02Z- TerrainNav::motionUpdate() - currEstimate = (4056850, 590868.08, 29.02) 2021-02-12T23:23:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:23:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:23:02Z- TNavBF::Projecting Measurements 2021-02-12T23:23:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:23:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:23:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:23:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:23:02Z- TNavFilter::4 good beams. 2021-02-12T23:23:02Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:23:02Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:23:02Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:23:02Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:23:02Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:23:02Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:23:02Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:23:02Z- TNavBF::mmse selected from filter -1 2021-02-12T23:23:02Z- TerrainMapOctree: (vehN, vehE ) = (4056849.19, 590867.71). 2021-02-12T23:23:02Z- TerrainMapOctree: Min Distance = 5550.41. 2021-02-12T23:23:02Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:23:02Z- There is no specific good beam that all particles have in common. 2021-02-12T23:23:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:23:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.119 ExpectedMeasDiff: nan currMeas.covariance: 6.67 2021-02-12T23:23:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:23:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 9.54 2021-02-12T23:23:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:23:02Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 5.54 2021-02-12T23:23:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:23:02Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 14.01 2021-02-12T23:23:02Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:23:02Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:23:02Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:23:02Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:23:02Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:23:02Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:23:02Z- Filter 0 No beams used in subcloud update 2021-02-12T23:23:02Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:23:02Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:23:02Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 771.4 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:23:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172181.34 sec. 2021-02-12T23:23:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:23:02Z- TNavBF::mmse selected from filter -1 2021-02-12T23:23:02Z- variances([0], [2], and psi_berg) = 771.357206 833.056053 0.000000 2021-02-12T23:23:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:23:02Z- LcmTrn::publishEstimates() - MLE : 4056829.22 590819.58 29.02 2021-02-12T23:23:02Z- LcmTrn::publishEstimates() - MMSE : 4056849.72 590868.08 29.02 2021-02-12T23:23:02Z- LcmTrn::publishEstimates() - COVAR: 771.36 833.06 0.81 0.00 2021-02-12T23:23:02Z- handleDepth()-Depth_Keller msg: 1613172182158 epoch sec; seqNo:3263; depth 29.15 2021-02-12T23:23:02Z- handleDvl() - RDI_Pathfinder msg: 1613172182.18 epoch sec; seqNo:1468 2021-02-12T23:23:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.14 , 77.22 , 58.43 , 92.50 2021-02-12T23:23:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.06 2021-02-12T23:23:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172182.19 epoch sec; seqNo:3263; 4056850.11 north; 590869.30 east 2021-02-12T23:23:02Z- AHRS_M2 msg: 1613172182.54 epoch sec; seqNo:3232 2021-02-12T23:23:02Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-12T23:23:02Z- handleDepth()-Depth_Keller msg: 1613172182542 epoch sec; seqNo:3264; depth 29.30 2021-02-12T23:23:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:02Z- handleDvl() - RDI_Pathfinder msg: 1613172182.55 epoch sec; seqNo:1469 2021-02-12T23:23:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.06 2021-02-12T23:23:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172182.56 epoch sec; seqNo:3264; 4056850.49 north; 590869.30 east 2021-02-12T23:23:02Z- AHRS_M2 msg: 1613172182.94 epoch sec; seqNo:3233 2021-02-12T23:23:02Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.19 psi 2021-02-12T23:23:02Z- handleDepth()-Depth_Keller msg: 1613172182944 epoch sec; seqNo:3265; depth 29.43 2021-02-12T23:23:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172182.96 epoch sec; seqNo:3265; 4056850.49 north; 590869.30 east 2021-02-12T23:23:03Z- AHRS_M2 msg: 1613172183.34 epoch sec; seqNo:3234 2021-02-12T23:23:03Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.19 psi 2021-02-12T23:23:03Z- handleDepth()-Depth_Keller msg: 1613172183353 epoch sec; seqNo:3266; depth 29.55 2021-02-12T23:23:03Z- handleDvl() - RDI_Pathfinder msg: 1613172183.37 epoch sec; seqNo:1470 2021-02-12T23:23:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.92 , 77.22 , 58.43 , 89.25 2021-02-12T23:23:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , -0.04 2021-02-12T23:23:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172183.39 epoch sec; seqNo:3266; 4056850.87 north; 590869.29 east 2021-02-12T23:23:03Z- AHRS_M2 msg: 1613172183.75 epoch sec; seqNo:3235 2021-02-12T23:23:03Z- AHRS_M2 msg: 0.61 phi; -0.26 theta; -0.18 psi 2021-02-12T23:23:03Z- handleDepth()-Depth_Keller msg: 1613172183754 epoch sec; seqNo:3267; depth 29.72 2021-02-12T23:23:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:03Z- handleDvl() - RDI_Pathfinder msg: 1613172183.76 epoch sec; seqNo:1471 2021-02-12T23:23:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , -0.04 2021-02-12T23:23:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172183.77 epoch sec; seqNo:3267; 4056851.25 north; 590869.29 east 2021-02-12T23:23:04Z- AHRS_M2 msg: 1613172184.15 epoch sec; seqNo:3236 2021-02-12T23:23:04Z- AHRS_M2 msg: 0.60 phi; -0.22 theta; -0.17 psi 2021-02-12T23:23:04Z- handleDepth()-Depth_Keller msg: 1613172184161 epoch sec; seqNo:3268; depth 29.85 2021-02-12T23:23:04Z- handleDvl() - RDI_Pathfinder msg: 1613172184.18 epoch sec; seqNo:1472 2021-02-12T23:23:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.69 , 74.36 , 58.83 , 78.85 2021-02-12T23:23:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.04 , -0.05 2021-02-12T23:23:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172184.19 epoch sec; seqNo:3268; 4056851.63 north; 590869.28 east 2021-02-12T23:23:04Z- AHRS_M2 msg: 1613172184.56 epoch sec; seqNo:3237 2021-02-12T23:23:04Z- AHRS_M2 msg: 0.60 phi; -0.17 theta; -0.16 psi 2021-02-12T23:23:04Z- handleDepth()-Depth_Keller msg: 1613172184565 epoch sec; seqNo:3269; depth 30.01 2021-02-12T23:23:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172184.58 epoch sec; seqNo:3269; 4056851.64 north; 590870.51 east 2021-02-12T23:23:04Z- AHRS_M2 msg: 1613172184.98 epoch sec; seqNo:3238 2021-02-12T23:23:04Z- AHRS_M2 msg: 0.59 phi; -0.13 theta; -0.16 psi 2021-02-12T23:23:04Z- handleDepth()-Depth_Keller msg: 1613172184985 epoch sec; seqNo:3270; depth 30.13 2021-02-12T23:23:05Z- handleDvl() - RDI_Pathfinder msg: 1613172185.01 epoch sec; seqNo:1473 2021-02-12T23:23:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.83 , 71.51 , 60.83 , 71.19 2021-02-12T23:23:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , -0.04 2021-02-12T23:23:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172185.03 epoch sec; seqNo:3270; 4056852.02 north; 590870.50 east 2021-02-12T23:23:05Z- AHRS_M2 msg: 1613172185.37 epoch sec; seqNo:3239 2021-02-12T23:23:05Z- AHRS_M2 msg: 0.58 phi; -0.08 theta; -0.15 psi 2021-02-12T23:23:05Z- handleDepth()-Depth_Keller msg: 1613172185370 epoch sec; seqNo:3271; depth 30.25 2021-02-12T23:23:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172185.38 epoch sec; seqNo:3271; 4056852.40 north; 590870.50 east 2021-02-12T23:23:05Z- AHRS_M2 msg: 1613172185.77 epoch sec; seqNo:3240 2021-02-12T23:23:05Z- AHRS_M2 msg: 0.57 phi; -0.04 theta; -0.15 psi 2021-02-12T23:23:05Z- handleDepth()-Depth_Keller msg: 1613172185776 epoch sec; seqNo:3272; depth 30.34 2021-02-12T23:23:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172185.80 epoch sec; seqNo:3272; 4056852.40 north; 590870.50 east 2021-02-12T23:23:06Z- AHRS_M2 msg: 1613172186.17 epoch sec; seqNo:3241 2021-02-12T23:23:06Z- AHRS_M2 msg: 0.56 phi; 0.01 theta; -0.15 psi 2021-02-12T23:23:06Z- handleDepth()-Depth_Keller msg: 1613172186179 epoch sec; seqNo:3273; depth 30.42 2021-02-12T23:23:06Z- handleDvl() - RDI_Pathfinder msg: 1613172186.19 epoch sec; seqNo:1474 2021-02-12T23:23:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.39 , 70.20 , 63.76 , 65.60 2021-02-12T23:23:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.02 , -0.06 2021-02-12T23:23:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172186.22 epoch sec; seqNo:3273; 4056852.78 north; 590870.49 east 2021-02-12T23:23:06Z- AHRS_M2 msg: 1613172186.58 epoch sec; seqNo:3242 2021-02-12T23:23:06Z- AHRS_M2 msg: 0.56 phi; 0.05 theta; -0.15 psi 2021-02-12T23:23:06Z- handleDepth()-Depth_Keller msg: 1613172186580 epoch sec; seqNo:3274; depth 30.48 2021-02-12T23:23:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172186.60 epoch sec; seqNo:3274; 4056853.15 north; 590870.49 east 2021-02-12T23:23:06Z- AHRS_M2 msg: 1613172186.98 epoch sec; seqNo:3243 2021-02-12T23:23:06Z- AHRS_M2 msg: 0.55 phi; 0.09 theta; -0.14 psi 2021-02-12T23:23:06Z- handleDepth()-Depth_Keller msg: 1613172186986 epoch sec; seqNo:3275; depth 30.53 2021-02-12T23:23:07Z- handleDvl() - RDI_Pathfinder msg: 1613172187.00 epoch sec; seqNo:1475 2021-02-12T23:23:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.39 , 69.89 , 67.75 , 62.54 2021-02-12T23:23:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.00 , -0.06 2021-02-12T23:23:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172187.01 epoch sec; seqNo:3275; 4056853.55 north; 590871.71 east 2021-02-12T23:23:07Z- LcmTrn::handleMessages() - handling 46 messages took 0.67 ms and 133 calls... 2021-02-12T23:23:07Z- waiting for 1613172185.15, time is 1613172187.01 2021-02-12T23:23:07Z- LcmTrn::updateTrn() equating pose 1613172187.01 and meas times 1613172187.01 2021-02-12T23:23:07Z- LcmTrn::updateTrn() >>>> pose time 1613172187.01, meas time 1613172187.01 <<<< 2021-02-12T23:23:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172187.01 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:23:07Z- TerrainNav::motionUpdate() - currEstimate = (4056854, 590871.71, 30.53) 2021-02-12T23:23:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:23:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:23:07Z- TNavBF::Projecting Measurements 2021-02-12T23:23:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:23:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:23:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:23:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:23:07Z- TNavFilter::4 good beams. 2021-02-12T23:23:07Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:23:07Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:23:07Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:23:07Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:23:07Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:23:07Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:23:07Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:23:07Z- TNavBF::mmse selected from filter -1 2021-02-12T23:23:07Z- TerrainMapOctree: (vehN, vehE ) = (4056853.55, 590871.71). 2021-02-12T23:23:07Z- TerrainMapOctree: Min Distance = 5544.52. 2021-02-12T23:23:07Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:23:07Z- There is no specific good beam that all particles have in common. 2021-02-12T23:23:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:23:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.124 ExpectedMeasDiff: nan currMeas.covariance: 5.84 2021-02-12T23:23:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:23:07Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 7.82 2021-02-12T23:23:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:23:07Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 7.34 2021-02-12T23:23:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:23:07Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 6.26 2021-02-12T23:23:07Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:23:07Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:23:07Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:23:07Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:23:07Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:23:07Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:23:07Z- Filter 0 No beams used in subcloud update 2021-02-12T23:23:07Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:23:07Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:23:07Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 771.2 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:23:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172187.01 sec. 2021-02-12T23:23:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:23:07Z- TNavBF::mmse selected from filter -1 2021-02-12T23:23:07Z- variances([0], [2], and psi_berg) = 771.205264 833.055862 0.000000 2021-02-12T23:23:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:23:07Z- LcmTrn::publishEstimates() - MLE : 4056833.05 590823.21 30.53 2021-02-12T23:23:07Z- LcmTrn::publishEstimates() - MMSE : 4056853.55 590871.71 30.53 2021-02-12T23:23:07Z- LcmTrn::publishEstimates() - COVAR: 771.21 833.06 0.71 0.00 2021-02-12T23:23:07Z- AHRS_M2 msg: 1613172187.39 epoch sec; seqNo:3244 2021-02-12T23:23:07Z- AHRS_M2 msg: 0.55 phi; 0.12 theta; -0.14 psi 2021-02-12T23:23:07Z- handleDepth()-Depth_Keller msg: 1613172187396 epoch sec; seqNo:3276; depth 30.56 2021-02-12T23:23:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:07Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:07Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:07Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:07Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:07Z- handleDvl() - RDI_Pathfinder msg: 1613172187.41 epoch sec; seqNo:1476 2021-02-12T23:23:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.00 , -0.06 2021-02-12T23:23:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172187.43 epoch sec; seqNo:3276; 4056853.55 north; 590871.71 east 2021-02-12T23:23:07Z- AHRS_M2 msg: 1613172187.79 epoch sec; seqNo:3245 2021-02-12T23:23:07Z- AHRS_M2 msg: 0.54 phi; 0.15 theta; -0.14 psi 2021-02-12T23:23:07Z- handleDepth()-Depth_Keller msg: 1613172187805 epoch sec; seqNo:3277; depth 30.58 2021-02-12T23:23:07Z- handleDvl() - RDI_Pathfinder msg: 1613172187.84 epoch sec; seqNo:1477 2021-02-12T23:23:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.31 , 70.51 , 72.04 , 60.39 2021-02-12T23:23:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , -0.06 2021-02-12T23:23:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172187.85 epoch sec; seqNo:3277; 4056853.92 north; 590871.70 east 2021-02-12T23:23:08Z- AHRS_M2 msg: 1613172188.19 epoch sec; seqNo:3246 2021-02-12T23:23:08Z- AHRS_M2 msg: 0.54 phi; 0.19 theta; -0.13 psi 2021-02-12T23:23:08Z- handleDepth()-Depth_Keller msg: 1613172188196 epoch sec; seqNo:3278; depth 30.59 2021-02-12T23:23:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172188.21 epoch sec; seqNo:3278; 4056854.30 north; 590871.70 east 2021-02-12T23:23:08Z- AHRS_M2 msg: 1613172188.60 epoch sec; seqNo:3247 2021-02-12T23:23:08Z- AHRS_M2 msg: 0.53 phi; 0.21 theta; -0.13 psi 2021-02-12T23:23:08Z- handleDepth()-Depth_Keller msg: 1613172188606 epoch sec; seqNo:3279; depth 30.58 2021-02-12T23:23:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:08Z- handleDvl() - RDI_Pathfinder msg: 1613172188.61 epoch sec; seqNo:1478 2021-02-12T23:23:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , -0.06 2021-02-12T23:23:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172188.62 epoch sec; seqNo:3279; 4056854.68 north; 590871.69 east 2021-02-12T23:23:09Z- AHRS_M2 msg: 1613172189.00 epoch sec; seqNo:3248 2021-02-12T23:23:09Z- AHRS_M2 msg: 0.53 phi; 0.24 theta; -0.14 psi 2021-02-12T23:23:09Z- handleDepth()-Depth_Keller msg: 1613172189014 epoch sec; seqNo:3280; depth 30.55 2021-02-12T23:23:09Z- handleDvl() - RDI_Pathfinder msg: 1613172189.03 epoch sec; seqNo:1479 2021-02-12T23:23:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.23 , 71.43 , 76.64 , 58.86 2021-02-12T23:23:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , -0.04 2021-02-12T23:23:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172189.04 epoch sec; seqNo:3280; 4056854.68 north; 590871.69 east 2021-02-12T23:23:09Z- AHRS_M2 msg: 1613172189.40 epoch sec; seqNo:3249 2021-02-12T23:23:09Z- AHRS_M2 msg: 0.53 phi; 0.26 theta; -0.14 psi 2021-02-12T23:23:09Z- handleDepth()-Depth_Keller msg: 1613172189408 epoch sec; seqNo:3281; depth 30.52 2021-02-12T23:23:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172189.42 epoch sec; seqNo:3281; 4056855.06 north; 590871.69 east 2021-02-12T23:23:09Z- AHRS_M2 msg: 1613172189.84 epoch sec; seqNo:3250 2021-02-12T23:23:09Z- AHRS_M2 msg: 0.52 phi; 0.28 theta; -0.14 psi 2021-02-12T23:23:09Z- handleDepth()-Depth_Keller msg: 1613172189843 epoch sec; seqNo:3282; depth 30.48 2021-02-12T23:23:09Z- handleDvl() - RDI_Pathfinder msg: 1613172189.87 epoch sec; seqNo:1480 2021-02-12T23:23:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.75 , 72.48 , 81.87 , 58.25 2021-02-12T23:23:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.01 2021-02-12T23:23:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172189.88 epoch sec; seqNo:3282; 4056855.44 north; 590871.69 east 2021-02-12T23:23:10Z- AHRS_M2 msg: 1613172190.21 epoch sec; seqNo:3251 2021-02-12T23:23:10Z- AHRS_M2 msg: 0.52 phi; 0.30 theta; -0.13 psi 2021-02-12T23:23:10Z- handleDepth()-Depth_Keller msg: 1613172190218 epoch sec; seqNo:3283; depth 30.43 2021-02-12T23:23:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172190.23 epoch sec; seqNo:3283; 4056855.83 north; 590872.90 east 2021-02-12T23:23:10Z- AHRS_M2 msg: 1613172190.62 epoch sec; seqNo:3252 2021-02-12T23:23:10Z- AHRS_M2 msg: 0.52 phi; 0.32 theta; -0.13 psi 2021-02-12T23:23:10Z- handleDepth()-Depth_Keller msg: 1613172190626 epoch sec; seqNo:3284; depth 30.36 2021-02-12T23:23:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172190.65 epoch sec; seqNo:3284; 4056856.21 north; 590872.90 east 2021-02-12T23:23:11Z- AHRS_M2 msg: 1613172191.02 epoch sec; seqNo:3253 2021-02-12T23:23:11Z- AHRS_M2 msg: 0.52 phi; 0.33 theta; -0.12 psi 2021-02-12T23:23:11Z- handleDepth()-Depth_Keller msg: 1613172191028 epoch sec; seqNo:3285; depth 30.30 2021-02-12T23:23:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:11Z- handleDvl() - RDI_Pathfinder msg: 1613172191.03 epoch sec; seqNo:1481 2021-02-12T23:23:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.01 2021-02-12T23:23:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172191.08 epoch sec; seqNo:3285; 4056856.21 north; 590872.90 east 2021-02-12T23:23:11Z- AHRS_M2 msg: 1613172191.42 epoch sec; seqNo:3254 2021-02-12T23:23:11Z- AHRS_M2 msg: 0.53 phi; 0.35 theta; -0.12 psi 2021-02-12T23:23:11Z- handleDepth()-Depth_Keller msg: 1613172191428 epoch sec; seqNo:3286; depth 30.21 2021-02-12T23:23:11Z- handleDvl() - RDI_Pathfinder msg: 1613172191.44 epoch sec; seqNo:1482 2021-02-12T23:23:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.96 , 72.72 , 0.00 , 58.02 2021-02-12T23:23:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.01 , -0.00 2021-02-12T23:23:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172191.46 epoch sec; seqNo:3286; 4056856.59 north; 590872.90 east 2021-02-12T23:23:11Z- AHRS_M2 msg: 1613172191.85 epoch sec; seqNo:3255 2021-02-12T23:23:11Z- AHRS_M2 msg: 0.54 phi; 0.37 theta; -0.10 psi 2021-02-12T23:23:11Z- handleDepth()-Depth_Keller msg: 1613172191853 epoch sec; seqNo:3287; depth 30.15 2021-02-12T23:23:11Z- handleDvl() - RDI_Pathfinder msg: 1613172191.87 epoch sec; seqNo:1483 2021-02-12T23:23:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.52 , 73.27 , 0.00 , 57.94 2021-02-12T23:23:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.05 , -0.02 2021-02-12T23:23:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172191.88 epoch sec; seqNo:3287; 4056856.97 north; 590872.89 east 2021-02-12T23:23:12Z- AHRS_M2 msg: 1613172192.24 epoch sec; seqNo:3256 2021-02-12T23:23:12Z- AHRS_M2 msg: 0.54 phi; 0.37 theta; -0.10 psi 2021-02-12T23:23:12Z- handleDepth()-Depth_Keller msg: 1613172192241 epoch sec; seqNo:3288; depth 30.04 2021-02-12T23:23:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:12Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:12Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:12Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:12Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:12Z- handleDvl() - RDI_Pathfinder msg: 1613172192.24 epoch sec; seqNo:1484 2021-02-12T23:23:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.05 , -0.02 2021-02-12T23:23:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172192.26 epoch sec; seqNo:3288; 4056856.97 north; 590872.89 east 2021-02-12T23:23:12Z- LcmTrn::handleMessages() - handling 48 messages took 0.77 ms and 133 calls... 2021-02-12T23:23:12Z- waiting for 1613172190.01, time is 1613172192.26 2021-02-12T23:23:12Z- LcmTrn::updateTrn() equating pose 1613172192.26 and meas times 1613172192.26 2021-02-12T23:23:12Z- LcmTrn::updateTrn() >>>> pose time 1613172192.26, meas time 1613172192.26 <<<< 2021-02-12T23:23:12Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172192.26, ping # 0. 2021-02-12T23:23:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172192.26 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:23:12Z- TerrainNav::motionUpdate() - currEstimate = (4056857, 590872.89, 30.04) 2021-02-12T23:23:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:23:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:23:12Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:23:12Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:23:12Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172192.26 sec.; No relevant navigation data available 2021-02-12T23:23:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:23:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:23:12Z- variances([0], [2], and psi_berg) = 771.079605 833.055706 0.000000 2021-02-12T23:23:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:23:12Z- LcmTrn::publishEstimates() - MLE : 4056836.47 590824.39 30.04 2021-02-12T23:23:12Z- LcmTrn::publishEstimates() - MMSE : 4056856.97 590872.89 30.04 2021-02-12T23:23:12Z- LcmTrn::publishEstimates() - COVAR: 771.08 833.06 0.90 0.00 2021-02-12T23:23:12Z- AHRS_M2 msg: 1613172192.64 epoch sec; seqNo:3257 2021-02-12T23:23:12Z- AHRS_M2 msg: 0.55 phi; 0.38 theta; -0.09 psi 2021-02-12T23:23:12Z- handleDepth()-Depth_Keller msg: 1613172192640 epoch sec; seqNo:3289; depth 29.94 2021-02-12T23:23:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172192.65 epoch sec; seqNo:3289; 4056857.35 north; 590872.89 east 2021-02-12T23:23:13Z- AHRS_M2 msg: 1613172193.04 epoch sec; seqNo:3258 2021-02-12T23:23:13Z- AHRS_M2 msg: 0.56 phi; 0.38 theta; -0.09 psi 2021-02-12T23:23:13Z- handleDepth()-Depth_Keller msg: 1613172193045 epoch sec; seqNo:3290; depth 29.86 2021-02-12T23:23:13Z- handleDvl() - RDI_Pathfinder msg: 1613172193.06 epoch sec; seqNo:1485 2021-02-12T23:23:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.44 , 73.57 , 0.00 , 57.94 2021-02-12T23:23:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.07 , -0.04 2021-02-12T23:23:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172193.07 epoch sec; seqNo:3290; 4056857.74 north; 590874.10 east 2021-02-12T23:23:13Z- AHRS_M2 msg: 1613172193.44 epoch sec; seqNo:3259 2021-02-12T23:23:13Z- AHRS_M2 msg: 0.57 phi; 0.38 theta; -0.09 psi 2021-02-12T23:23:13Z- handleDepth()-Depth_Keller msg: 1613172193464 epoch sec; seqNo:3291; depth 29.76 2021-02-12T23:23:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:13Z- handleDvl() - RDI_Pathfinder msg: 1613172193.48 epoch sec; seqNo:1486 2021-02-12T23:23:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.07 , -0.04 2021-02-12T23:23:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172193.49 epoch sec; seqNo:3291; 4056857.74 north; 590874.10 east 2021-02-12T23:23:13Z- AHRS_M2 msg: 1613172193.85 epoch sec; seqNo:3260 2021-02-12T23:23:13Z- AHRS_M2 msg: 0.58 phi; 0.38 theta; -0.08 psi 2021-02-12T23:23:13Z- handleDepth()-Depth_Keller msg: 1613172193857 epoch sec; seqNo:3292; depth 29.62 2021-02-12T23:23:13Z- handleDvl() - RDI_Pathfinder msg: 1613172193.88 epoch sec; seqNo:1487 2021-02-12T23:23:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.75 , 73.27 , 0.00 , 58.55 2021-02-12T23:23:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.06 , -0.06 2021-02-12T23:23:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172193.89 epoch sec; seqNo:3292; 4056858.12 north; 590874.10 east 2021-02-12T23:23:14Z- AHRS_M2 msg: 1613172194.25 epoch sec; seqNo:3261 2021-02-12T23:23:14Z- AHRS_M2 msg: 0.58 phi; 0.38 theta; -0.08 psi 2021-02-12T23:23:14Z- handleDepth()-Depth_Keller msg: 1613172194256 epoch sec; seqNo:3293; depth 29.51 2021-02-12T23:23:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172194.27 epoch sec; seqNo:3293; 4056858.12 north; 590874.10 east 2021-02-12T23:23:14Z- AHRS_M2 msg: 1613172194.66 epoch sec; seqNo:3262 2021-02-12T23:23:14Z- AHRS_M2 msg: 0.59 phi; 0.38 theta; -0.08 psi 2021-02-12T23:23:14Z- handleDepth()-Depth_Keller msg: 1613172194670 epoch sec; seqNo:3294; depth 29.41 2021-02-12T23:23:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:14Z- handleDvl() - RDI_Pathfinder msg: 1613172194.67 epoch sec; seqNo:1488 2021-02-12T23:23:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.06 , -0.06 2021-02-12T23:23:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172194.68 epoch sec; seqNo:3294; 4056858.50 north; 590874.10 east 2021-02-12T23:23:15Z- AHRS_M2 msg: 1613172195.06 epoch sec; seqNo:3263 2021-02-12T23:23:15Z- AHRS_M2 msg: 0.60 phi; 0.38 theta; -0.08 psi 2021-02-12T23:23:15Z- handleDepth()-Depth_Keller msg: 1613172195066 epoch sec; seqNo:3295; depth 29.29 2021-02-12T23:23:15Z- handleDvl() - RDI_Pathfinder msg: 1613172195.08 epoch sec; seqNo:1489 2021-02-12T23:23:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.36 , 73.57 , 0.00 , 58.55 2021-02-12T23:23:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.04 , -0.06 2021-02-12T23:23:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172195.09 epoch sec; seqNo:3295; 4056858.88 north; 590874.09 east 2021-02-12T23:23:15Z- AHRS_M2 msg: 1613172195.46 epoch sec; seqNo:3264 2021-02-12T23:23:15Z- AHRS_M2 msg: 0.60 phi; 0.38 theta; -0.08 psi 2021-02-12T23:23:15Z- handleDepth()-Depth_Keller msg: 1613172195468 epoch sec; seqNo:3296; depth 29.18 2021-02-12T23:23:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172195.49 epoch sec; seqNo:3296; 4056858.88 north; 590874.09 east 2021-02-12T23:23:15Z- AHRS_M2 msg: 1613172195.87 epoch sec; seqNo:3265 2021-02-12T23:23:15Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.08 psi 2021-02-12T23:23:15Z- handleDepth()-Depth_Keller msg: 1613172195877 epoch sec; seqNo:3297; depth 29.05 2021-02-12T23:23:15Z- handleDvl() - RDI_Pathfinder msg: 1613172195.89 epoch sec; seqNo:1490 2021-02-12T23:23:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.06 , 73.57 , 0.00 , 58.55 2021-02-12T23:23:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.01 , -0.05 2021-02-12T23:23:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172195.91 epoch sec; seqNo:3297; 4056859.27 north; 590875.31 east 2021-02-12T23:23:16Z- AHRS_M2 msg: 1613172196.27 epoch sec; seqNo:3266 2021-02-12T23:23:16Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.09 psi 2021-02-12T23:23:16Z- handleDepth()-Depth_Keller msg: 1613172196276 epoch sec; seqNo:3298; depth 28.94 2021-02-12T23:23:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172196.29 epoch sec; seqNo:3298; 4056859.27 north; 590875.31 east 2021-02-12T23:23:16Z- AHRS_M2 msg: 1613172196.68 epoch sec; seqNo:3267 2021-02-12T23:23:16Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.09 psi 2021-02-12T23:23:16Z- handleDepth()-Depth_Keller msg: 1613172196686 epoch sec; seqNo:3299; depth 28.81 2021-02-12T23:23:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172196.69 epoch sec; seqNo:3299; 4056859.65 north; 590875.31 east 2021-02-12T23:23:17Z- AHRS_M2 msg: 1613172197.08 epoch sec; seqNo:3268 2021-02-12T23:23:17Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.10 psi 2021-02-12T23:23:17Z- handleDepth()-Depth_Keller msg: 1613172197089 epoch sec; seqNo:3300; depth 28.70 2021-02-12T23:23:17Z- handleDvl() - RDI_Pathfinder msg: 1613172197.11 epoch sec; seqNo:1491 2021-02-12T23:23:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.75 , 74.19 , 0.00 , 59.17 2021-02-12T23:23:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.01 , -0.02 2021-02-12T23:23:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172197.12 epoch sec; seqNo:3300; 4056860.03 north; 590875.30 east 2021-02-12T23:23:17Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-12T23:23:17Z- waiting for 1613172195.26, time is 1613172197.12 2021-02-12T23:23:17Z- LcmTrn::updateTrn() equating pose 1613172197.12 and meas times 1613172197.12 2021-02-12T23:23:17Z- LcmTrn::updateTrn() >>>> pose time 1613172197.12, meas time 1613172197.12 <<<< 2021-02-12T23:23:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172197.12 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:23:17Z- TerrainNav::motionUpdate() - currEstimate = (4056860, 590875.30, 28.70) 2021-02-12T23:23:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:23:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:23:17Z- TNavBF::Projecting Measurements 2021-02-12T23:23:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:23:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:23:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:23:17Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:23:17Z- TNavFilter::3 good beams. 2021-02-12T23:23:17Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:23:17Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:23:17Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:23:17Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:23:17Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:23:17Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:23:17Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:23:17Z- TNavBF::mmse selected from filter -1 2021-02-12T23:23:17Z- TerrainMapOctree: (vehN, vehE ) = (4056860.03, 590875.30). 2021-02-12T23:23:17Z- TerrainMapOctree: Min Distance = 5537.19. 2021-02-12T23:23:17Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:23:17Z- There is no specific good beam that all particles have in common. 2021-02-12T23:23:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:23:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.108 ExpectedMeasDiff: nan currMeas.covariance: 7.34 2021-02-12T23:23:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:23:17Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 8.81 2021-02-12T23:23:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:23:17Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 5.60 2021-02-12T23:23:17Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:23:17Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:23:17Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:23:17Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:23:17Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:23:17Z- Filter 0 No beams used in subcloud update 2021-02-12T23:23:17Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:23:17Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:23:17Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 770.9 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:23:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172197.12 sec. 2021-02-12T23:23:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:23:17Z- TNavBF::mmse selected from filter -1 2021-02-12T23:23:17Z- variances([0], [2], and psi_berg) = 770.949595 833.055544 0.000000 2021-02-12T23:23:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:23:17Z- LcmTrn::publishEstimates() - MLE : 4056839.53 590826.80 28.70 2021-02-12T23:23:17Z- LcmTrn::publishEstimates() - MMSE : 4056860.03 590875.30 28.70 2021-02-12T23:23:17Z- LcmTrn::publishEstimates() - COVAR: 770.95 833.06 0.80 0.00 2021-02-12T23:23:17Z- AHRS_M2 msg: 1613172197.48 epoch sec; seqNo:3269 2021-02-12T23:23:17Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.11 psi 2021-02-12T23:23:17Z- handleDepth()-Depth_Keller msg: 1613172197488 epoch sec; seqNo:3301; depth 28.57 2021-02-12T23:23:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172197.50 epoch sec; seqNo:3301; 4056860.03 north; 590875.30 east 2021-02-12T23:23:17Z- AHRS_M2 msg: 1613172197.89 epoch sec; seqNo:3270 2021-02-12T23:23:17Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.12 psi 2021-02-12T23:23:17Z- handleDepth()-Depth_Keller msg: 1613172197928 epoch sec; seqNo:3302; depth 28.47 2021-02-12T23:23:17Z- handleDvl() - RDI_Pathfinder msg: 1613172197.98 epoch sec; seqNo:1492 2021-02-12T23:23:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.14 , 75.72 , 0.00 , 59.17 2021-02-12T23:23:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.01 , 0.02 2021-02-12T23:23:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172198.00 epoch sec; seqNo:3302; 4056860.40 north; 590875.30 east 2021-02-12T23:23:18Z- AHRS_M2 msg: 1613172198.29 epoch sec; seqNo:3271 2021-02-12T23:23:18Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.13 psi 2021-02-12T23:23:18Z- handleDepth()-Depth_Keller msg: 1613172198295 epoch sec; seqNo:3303; depth 28.31 2021-02-12T23:23:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:18Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:18Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:18Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:18Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:18Z- handleDvl() - RDI_Pathfinder msg: 1613172198.30 epoch sec; seqNo:1493 2021-02-12T23:23:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.01 , 0.02 2021-02-12T23:23:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172198.32 epoch sec; seqNo:3303; 4056860.40 north; 590875.30 east 2021-02-12T23:23:18Z- AHRS_M2 msg: 1613172198.70 epoch sec; seqNo:3272 2021-02-12T23:23:18Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-12T23:23:18Z- handleDepth()-Depth_Keller msg: 1613172198703 epoch sec; seqNo:3304; depth 28.22 2021-02-12T23:23:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172198.71 epoch sec; seqNo:3304; 4056860.78 north; 590875.29 east 2021-02-12T23:23:19Z- AHRS_M2 msg: 1613172199.10 epoch sec; seqNo:3273 2021-02-12T23:23:19Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-12T23:23:19Z- handleDepth()-Depth_Keller msg: 1613172199104 epoch sec; seqNo:3305; depth 28.10 2021-02-12T23:23:19Z- handleDvl() - RDI_Pathfinder msg: 1613172199.12 epoch sec; seqNo:1494 2021-02-12T23:23:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.98 , 77.34 , 0.00 , 60.19 2021-02-12T23:23:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.00 , 0.05 2021-02-12T23:23:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172199.13 epoch sec; seqNo:3305; 4056861.17 north; 590876.51 east 2021-02-12T23:23:19Z- AHRS_M2 msg: 1613172199.50 epoch sec; seqNo:3274 2021-02-12T23:23:19Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-12T23:23:19Z- handleDepth()-Depth_Keller msg: 1613172199510 epoch sec; seqNo:3306; depth 27.97 2021-02-12T23:23:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:19Z- handleDvl() - RDI_Pathfinder msg: 1613172199.51 epoch sec; seqNo:1495 2021-02-12T23:23:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.00 , 0.05 2021-02-12T23:23:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172199.52 epoch sec; seqNo:3306; 4056861.55 north; 590876.51 east 2021-02-12T23:23:19Z- AHRS_M2 msg: 1613172199.91 epoch sec; seqNo:3275 2021-02-12T23:23:19Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-12T23:23:19Z- handleDepth()-Depth_Keller msg: 1613172199917 epoch sec; seqNo:3307; depth 27.84 2021-02-12T23:23:19Z- handleDvl() - RDI_Pathfinder msg: 1613172199.93 epoch sec; seqNo:1496 2021-02-12T23:23:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.66 , 78.31 , 0.00 , 60.19 2021-02-12T23:23:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , 0.06 2021-02-12T23:23:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172199.95 epoch sec; seqNo:3307; 4056861.55 north; 590876.51 east 2021-02-12T23:23:20Z- AHRS_M2 msg: 1613172200.31 epoch sec; seqNo:3276 2021-02-12T23:23:20Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-12T23:23:20Z- handleDepth()-Depth_Keller msg: 1613172200316 epoch sec; seqNo:3308; depth 27.73 2021-02-12T23:23:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172200.33 epoch sec; seqNo:3308; 4056861.93 north; 590876.50 east 2021-02-12T23:23:20Z- AHRS_M2 msg: 1613172200.72 epoch sec; seqNo:3277 2021-02-12T23:23:20Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-12T23:23:20Z- handleDepth()-Depth_Keller msg: 1613172200722 epoch sec; seqNo:3309; depth 27.63 2021-02-12T23:23:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:20Z- handleDvl() - RDI_Pathfinder msg: 1613172200.72 epoch sec; seqNo:1497 2021-02-12T23:23:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , 0.06 2021-02-12T23:23:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172200.73 epoch sec; seqNo:3309; 4056862.31 north; 590876.50 east 2021-02-12T23:23:21Z- AHRS_M2 msg: 1613172201.12 epoch sec; seqNo:3278 2021-02-12T23:23:21Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.16 psi 2021-02-12T23:23:21Z- handleDepth()-Depth_Keller msg: 1613172201129 epoch sec; seqNo:3310; depth 27.50 2021-02-12T23:23:21Z- handleDvl() - RDI_Pathfinder msg: 1613172201.16 epoch sec; seqNo:1498 2021-02-12T23:23:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.98 , 78.63 , 0.00 , 60.51 2021-02-12T23:23:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , 0.04 2021-02-12T23:23:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172201.18 epoch sec; seqNo:3310; 4056862.31 north; 590876.50 east 2021-02-12T23:23:21Z- AHRS_M2 msg: 1613172201.52 epoch sec; seqNo:3279 2021-02-12T23:23:21Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.16 psi 2021-02-12T23:23:21Z- handleDepth()-Depth_Keller msg: 1613172201530 epoch sec; seqNo:3311; depth 27.37 2021-02-12T23:23:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172201.54 epoch sec; seqNo:3311; 4056862.70 north; 590877.72 east 2021-02-12T23:23:21Z- AHRS_M2 msg: 1613172201.93 epoch sec; seqNo:3280 2021-02-12T23:23:21Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-12T23:23:21Z- handleDepth()-Depth_Keller msg: 1613172201937 epoch sec; seqNo:3312; depth 27.27 2021-02-12T23:23:21Z- handleDvl() - RDI_Pathfinder msg: 1613172201.95 epoch sec; seqNo:1499 2021-02-12T23:23:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.31 , 78.63 , 0.00 , 60.83 2021-02-12T23:23:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , 0.01 2021-02-12T23:23:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172201.97 epoch sec; seqNo:3312; 4056863.08 north; 590877.71 east 2021-02-12T23:23:22Z- AHRS_M2 msg: 1613172202.33 epoch sec; seqNo:3281 2021-02-12T23:23:22Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-12T23:23:22Z- handleDepth()-Depth_Keller msg: 1613172202341 epoch sec; seqNo:3313; depth 27.13 2021-02-12T23:23:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172202.35 epoch sec; seqNo:3313; 4056863.08 north; 590877.71 east 2021-02-12T23:23:22Z- LcmTrn::handleMessages() - handling 47 messages took 0.76 ms and 132 calls... 2021-02-12T23:23:22Z- waiting for 1613172200.12, time is 1613172202.35 2021-02-12T23:23:22Z- LcmTrn::updateTrn() >>>> pose time 1613172202.35, meas time 1613172201.95 <<<< 2021-02-12T23:23:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172201.95 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:23:22Z- TerrainNav::motionUpdate() - currEstimate = (4056863, 590877.71, 27.13) 2021-02-12T23:23:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:23:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:23:22Z- TNavBF::Projecting Measurements 2021-02-12T23:23:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:23:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:23:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:23:22Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:23:22Z- TNavFilter::3 good beams. 2021-02-12T23:23:22Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:23:22Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:23:22Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:23:22Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:23:22Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:23:22Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:23:22Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:23:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:23:22Z- TerrainMapOctree: (vehN, vehE ) = (4056862.85, 590877.53). 2021-02-12T23:23:22Z- TerrainMapOctree: Min Distance = 5533.60. 2021-02-12T23:23:22Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:23:22Z- There is no specific good beam that all particles have in common. 2021-02-12T23:23:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:23:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 7.25 2021-02-12T23:23:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:23:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 9.89 2021-02-12T23:23:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:23:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 5.92 2021-02-12T23:23:22Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:23:22Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:23:22Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:23:22Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:23:22Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:23:22Z- Filter 0 No beams used in subcloud update 2021-02-12T23:23:22Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:23:22Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:23:22Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 770.8 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:23:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172201.95 sec. 2021-02-12T23:23:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:23:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:23:22Z- variances([0], [2], and psi_berg) = 770.788978 833.055344 0.000000 2021-02-12T23:23:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:23:22Z- LcmTrn::publishEstimates() - MLE : 4056842.58 590829.21 27.13 2021-02-12T23:23:22Z- LcmTrn::publishEstimates() - MMSE : 4056863.08 590877.71 27.13 2021-02-12T23:23:22Z- LcmTrn::publishEstimates() - COVAR: 770.79 833.06 0.76 0.00 2021-02-12T23:23:22Z- AHRS_M2 msg: 1613172202.74 epoch sec; seqNo:3282 2021-02-12T23:23:22Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-12T23:23:22Z- handleDepth()-Depth_Keller msg: 1613172202747 epoch sec; seqNo:3314; depth 27.04 2021-02-12T23:23:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172202.76 epoch sec; seqNo:3314; 4056863.46 north; 590877.71 east 2021-02-12T23:23:23Z- AHRS_M2 msg: 1613172203.14 epoch sec; seqNo:3283 2021-02-12T23:23:23Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-12T23:23:23Z- handleDepth()-Depth_Keller msg: 1613172203148 epoch sec; seqNo:3315; depth 26.92 2021-02-12T23:23:23Z- handleDvl() - RDI_Pathfinder msg: 1613172203.17 epoch sec; seqNo:1500 2021-02-12T23:23:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.63 , 78.63 , 0.00 , 61.16 2021-02-12T23:23:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , -0.02 2021-02-12T23:23:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172203.18 epoch sec; seqNo:3315; 4056863.84 north; 590877.70 east 2021-02-12T23:23:23Z- AHRS_M2 msg: 1613172203.54 epoch sec; seqNo:3284 2021-02-12T23:23:23Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:23:23Z- handleDepth()-Depth_Keller msg: 1613172203548 epoch sec; seqNo:3316; depth 26.80 2021-02-12T23:23:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172203.57 epoch sec; seqNo:3316; 4056864.23 north; 590878.92 east 2021-02-12T23:23:23Z- AHRS_M2 msg: 1613172203.95 epoch sec; seqNo:3285 2021-02-12T23:23:23Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-12T23:23:23Z- handleDepth()-Depth_Keller msg: 1613172203954 epoch sec; seqNo:3317; depth 26.70 2021-02-12T23:23:23Z- handleDvl() - RDI_Pathfinder msg: 1613172203.97 epoch sec; seqNo:1501 2021-02-12T23:23:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.63 , 79.28 , 0.00 , 61.48 2021-02-12T23:23:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , -0.05 2021-02-12T23:23:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172203.98 epoch sec; seqNo:3317; 4056864.23 north; 590878.92 east 2021-02-12T23:23:24Z- AHRS_M2 msg: 1613172204.35 epoch sec; seqNo:3286 2021-02-12T23:23:24Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-12T23:23:24Z- handleDepth()-Depth_Keller msg: 1613172204357 epoch sec; seqNo:3318; depth 26.58 2021-02-12T23:23:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:24Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:24Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:24Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:24Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:24Z- handleDvl() - RDI_Pathfinder msg: 1613172204.36 epoch sec; seqNo:1502 2021-02-12T23:23:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , -0.05 2021-02-12T23:23:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172204.38 epoch sec; seqNo:3318; 4056864.61 north; 590878.92 east 2021-02-12T23:23:24Z- AHRS_M2 msg: 1613172204.76 epoch sec; seqNo:3287 2021-02-12T23:23:24Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.17 psi 2021-02-12T23:23:24Z- handleDepth()-Depth_Keller msg: 1613172204760 epoch sec; seqNo:3319; depth 26.47 2021-02-12T23:23:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172204.77 epoch sec; seqNo:3319; 4056864.99 north; 590878.91 east 2021-02-12T23:23:25Z- AHRS_M2 msg: 1613172205.16 epoch sec; seqNo:3288 2021-02-12T23:23:25Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.17 psi 2021-02-12T23:23:25Z- handleDepth()-Depth_Keller msg: 1613172205169 epoch sec; seqNo:3320; depth 26.37 2021-02-12T23:23:25Z- handleDvl() - RDI_Pathfinder msg: 1613172205.18 epoch sec; seqNo:1503 2021-02-12T23:23:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.98 , 80.25 , 0.00 , 61.81 2021-02-12T23:23:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , -0.05 2021-02-12T23:23:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172205.19 epoch sec; seqNo:3320; 4056864.99 north; 590878.91 east 2021-02-12T23:23:25Z- AHRS_M2 msg: 1613172205.57 epoch sec; seqNo:3289 2021-02-12T23:23:25Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:23:25Z- handleDepth()-Depth_Keller msg: 1613172205570 epoch sec; seqNo:3321; depth 26.26 2021-02-12T23:23:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:25Z- handleDvl() - RDI_Pathfinder msg: 1613172205.57 epoch sec; seqNo:1504 2021-02-12T23:23:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , -0.05 2021-02-12T23:23:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172205.58 epoch sec; seqNo:3321; 4056865.37 north; 590878.91 east 2021-02-12T23:23:25Z- AHRS_M2 msg: 1613172205.97 epoch sec; seqNo:3290 2021-02-12T23:23:25Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:23:25Z- handleDepth()-Depth_Keller msg: 1613172205987 epoch sec; seqNo:3322; depth 26.15 2021-02-12T23:23:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172205.99 epoch sec; seqNo:3322; 4056865.75 north; 590878.90 east 2021-02-12T23:23:26Z- AHRS_M2 msg: 1613172206.37 epoch sec; seqNo:3291 2021-02-12T23:23:26Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:23:26Z- handleDepth()-Depth_Keller msg: 1613172206380 epoch sec; seqNo:3323; depth 26.04 2021-02-12T23:23:26Z- handleDvl() - RDI_Pathfinder msg: 1613172206.40 epoch sec; seqNo:1505 2021-02-12T23:23:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.01 , 86.01 , 93.58 , 62.11 2021-02-12T23:23:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , -0.05 2021-02-12T23:23:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172206.42 epoch sec; seqNo:3323; 4056865.76 north; 590880.13 east 2021-02-12T23:23:26Z- AHRS_M2 msg: 1613172206.78 epoch sec; seqNo:3292 2021-02-12T23:23:26Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:23:26Z- handleDepth()-Depth_Keller msg: 1613172206780 epoch sec; seqNo:3324; depth 25.92 2021-02-12T23:23:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:26Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:26Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:26Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:26Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:26Z- handleDvl() - RDI_Pathfinder msg: 1613172206.78 epoch sec; seqNo:1506 2021-02-12T23:23:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , -0.05 2021-02-12T23:23:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172206.79 epoch sec; seqNo:3324; 4056866.14 north; 590880.12 east 2021-02-12T23:23:27Z- AHRS_M2 msg: 1613172207.20 epoch sec; seqNo:3293 2021-02-12T23:23:27Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:23:27Z- handleDepth()-Depth_Keller msg: 1613172207208 epoch sec; seqNo:3325; depth 25.80 2021-02-12T23:23:27Z- handleDvl() - RDI_Pathfinder msg: 1613172207.23 epoch sec; seqNo:1507 2021-02-12T23:23:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.01 , 86.01 , 93.58 , 62.11 2021-02-12T23:23:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , -0.05 2021-02-12T23:23:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172207.24 epoch sec; seqNo:3325; 4056866.52 north; 590880.12 east 2021-02-12T23:23:27Z- LcmTrn::handleMessages() - handling 44 messages took 0.62 ms and 132 calls... 2021-02-12T23:23:27Z- waiting for 1613172205.35, time is 1613172207.24 2021-02-12T23:23:27Z- LcmTrn::updateTrn() equating pose 1613172207.24 and meas times 1613172207.24 2021-02-12T23:23:27Z- LcmTrn::updateTrn() >>>> pose time 1613172207.24, meas time 1613172207.24 <<<< 2021-02-12T23:23:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172207.24 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:23:27Z- TerrainNav::motionUpdate() - currEstimate = (4056867, 590880.12, 25.80) 2021-02-12T23:23:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:23:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:23:27Z- TNavBF::Projecting Measurements 2021-02-12T23:23:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:23:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:23:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:23:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:23:27Z- TNavFilter::4 good beams. 2021-02-12T23:23:27Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:23:27Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:23:27Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:23:27Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:23:27Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:23:27Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:23:27Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:23:27Z- TNavBF::mmse selected from filter -1 2021-02-12T23:23:27Z- TerrainMapOctree: (vehN, vehE ) = (4056866.52, 590880.12). 2021-02-12T23:23:27Z- TerrainMapOctree: Min Distance = 5529.11. 2021-02-12T23:23:27Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:23:27Z- There is no specific good beam that all particles have in common. 2021-02-12T23:23:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:23:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 7.18 2021-02-12T23:23:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:23:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 11.84 2021-02-12T23:23:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:23:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 14.01 2021-02-12T23:23:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:23:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 6.17 2021-02-12T23:23:27Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:23:27Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:23:27Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:23:27Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:23:27Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:23:27Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:23:27Z- Filter 0 No beams used in subcloud update 2021-02-12T23:23:27Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:23:27Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:23:27Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 770.7 EastVariance: 833.1 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:23:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172207.24 sec. 2021-02-12T23:23:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:23:27Z- TNavBF::mmse selected from filter -1 2021-02-12T23:23:27Z- variances([0], [2], and psi_berg) = 770.654635 833.055177 0.000000 2021-02-12T23:23:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:23:27Z- LcmTrn::publishEstimates() - MLE : 4056846.02 590831.62 25.80 2021-02-12T23:23:27Z- LcmTrn::publishEstimates() - MMSE : 4056866.52 590880.12 25.80 2021-02-12T23:23:27Z- LcmTrn::publishEstimates() - COVAR: 770.65 833.06 0.71 0.00 2021-02-12T23:23:27Z- AHRS_M2 msg: 1613172207.58 epoch sec; seqNo:3294 2021-02-12T23:23:27Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:23:27Z- handleDepth()-Depth_Keller msg: 1613172207588 epoch sec; seqNo:3326; depth 25.69 2021-02-12T23:23:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172207.61 epoch sec; seqNo:3326; 4056866.52 north; 590880.12 east 2021-02-12T23:23:27Z- AHRS_M2 msg: 1613172207.99 epoch sec; seqNo:3295 2021-02-12T23:23:27Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:23:27Z- handleDepth()-Depth_Keller msg: 1613172207993 epoch sec; seqNo:3327; depth 25.60 2021-02-12T23:23:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:27Z- handleDvl() - RDI_Pathfinder msg: 1613172207.99 epoch sec; seqNo:1508 2021-02-12T23:23:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , -0.05 2021-02-12T23:23:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172208.00 epoch sec; seqNo:3327; 4056866.90 north; 590880.11 east 2021-02-12T23:23:28Z- AHRS_M2 msg: 1613172208.39 epoch sec; seqNo:3296 2021-02-12T23:23:28Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:23:28Z- handleDepth()-Depth_Keller msg: 1613172208401 epoch sec; seqNo:3328; depth 25.49 2021-02-12T23:23:28Z- handleDvl() - RDI_Pathfinder msg: 1613172208.42 epoch sec; seqNo:1509 2021-02-12T23:23:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.41 , 87.09 , 95.74 , 62.10 2021-02-12T23:23:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.07 , -0.02 2021-02-12T23:23:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172208.43 epoch sec; seqNo:3328; 4056867.27 north; 590880.11 east 2021-02-12T23:23:28Z- AHRS_M2 msg: 1613172208.80 epoch sec; seqNo:3297 2021-02-12T23:23:28Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:28Z- handleDepth()-Depth_Keller msg: 1613172208800 epoch sec; seqNo:3329; depth 25.39 2021-02-12T23:23:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172208.82 epoch sec; seqNo:3329; 4056867.27 north; 590880.11 east 2021-02-12T23:23:29Z- AHRS_M2 msg: 1613172209.22 epoch sec; seqNo:3298 2021-02-12T23:23:29Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.18 psi 2021-02-12T23:23:29Z- handleDepth()-Depth_Keller msg: 1613172209224 epoch sec; seqNo:3330; depth 25.28 2021-02-12T23:23:29Z- handleDvl() - RDI_Pathfinder msg: 1613172209.24 epoch sec; seqNo:1510 2021-02-12T23:23:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.41 , 86.01 , 94.66 , 62.92 2021-02-12T23:23:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.05 , -0.01 2021-02-12T23:23:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172209.25 epoch sec; seqNo:3330; 4056867.67 north; 590881.33 east 2021-02-12T23:23:29Z- AHRS_M2 msg: 1613172209.61 epoch sec; seqNo:3299 2021-02-12T23:23:29Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:29Z- handleDepth()-Depth_Keller msg: 1613172209611 epoch sec; seqNo:3331; depth 25.17 2021-02-12T23:23:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172209.62 epoch sec; seqNo:3331; 4056868.05 north; 590881.32 east 2021-02-12T23:23:30Z- AHRS_M2 msg: 1613172210.01 epoch sec; seqNo:3300 2021-02-12T23:23:30Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:30Z- handleDepth()-Depth_Keller msg: 1613172210014 epoch sec; seqNo:3332; depth 25.08 2021-02-12T23:23:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172210.02 epoch sec; seqNo:3332; 4056868.05 north; 590881.32 east 2021-02-12T23:23:30Z- AHRS_M2 msg: 1613172210.41 epoch sec; seqNo:3301 2021-02-12T23:23:30Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:30Z- handleDepth()-Depth_Keller msg: 1613172210421 epoch sec; seqNo:3333; depth 24.97 2021-02-12T23:23:30Z- handleDvl() - RDI_Pathfinder msg: 1613172210.44 epoch sec; seqNo:1511 2021-02-12T23:23:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.42 , 84.93 , 97.91 , 63.33 2021-02-12T23:23:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.05 , 0.03 2021-02-12T23:23:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172210.45 epoch sec; seqNo:3333; 4056868.42 north; 590881.32 east 2021-02-12T23:23:30Z- AHRS_M2 msg: 1613172210.82 epoch sec; seqNo:3302 2021-02-12T23:23:30Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:30Z- handleDepth()-Depth_Keller msg: 1613172210820 epoch sec; seqNo:3334; depth 24.86 2021-02-12T23:23:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172210.83 epoch sec; seqNo:3334; 4056868.80 north; 590881.31 east 2021-02-12T23:23:31Z- AHRS_M2 msg: 1613172211.22 epoch sec; seqNo:3303 2021-02-12T23:23:31Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:31Z- handleDepth()-Depth_Keller msg: 1613172211228 epoch sec; seqNo:3335; depth 24.76 2021-02-12T23:23:31Z- handleDvl() - RDI_Pathfinder msg: 1613172211.24 epoch sec; seqNo:1512 2021-02-12T23:23:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 86.01 , 93.58 , 62.93 2021-02-12T23:23:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , 0.05 2021-02-12T23:23:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172211.25 epoch sec; seqNo:3335; 4056868.80 north; 590881.31 east 2021-02-12T23:23:31Z- AHRS_M2 msg: 1613172211.62 epoch sec; seqNo:3304 2021-02-12T23:23:31Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:31Z- handleDepth()-Depth_Keller msg: 1613172211629 epoch sec; seqNo:3336; depth 24.65 2021-02-12T23:23:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:31Z- handleDvl() - RDI_Pathfinder msg: 1613172211.63 epoch sec; seqNo:1513 2021-02-12T23:23:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , 0.05 2021-02-12T23:23:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172211.64 epoch sec; seqNo:3336; 4056869.18 north; 590881.31 east 2021-02-12T23:23:32Z- AHRS_M2 msg: 1613172212.03 epoch sec; seqNo:3305 2021-02-12T23:23:32Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:32Z- handleDepth()-Depth_Keller msg: 1613172212045 epoch sec; seqNo:3337; depth 24.55 2021-02-12T23:23:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172212.05 epoch sec; seqNo:3337; 4056869.57 north; 590882.53 east 2021-02-12T23:23:32Z- AHRS_M2 msg: 1613172212.43 epoch sec; seqNo:3306 2021-02-12T23:23:32Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:32Z- handleDepth()-Depth_Keller msg: 1613172212436 epoch sec; seqNo:3338; depth 24.45 2021-02-12T23:23:32Z- handleDvl() - RDI_Pathfinder msg: 1613172212.45 epoch sec; seqNo:1514 2021-02-12T23:23:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 88.18 , 93.58 , 63.74 2021-02-12T23:23:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , 0.06 2021-02-12T23:23:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172212.48 epoch sec; seqNo:3338; 4056869.95 north; 590882.52 east 2021-02-12T23:23:32Z- LcmTrn::handleMessages() - handling 46 messages took 0.65 ms and 133 calls... 2021-02-12T23:23:32Z- waiting for 1613172210.24, time is 1613172212.48 2021-02-12T23:23:32Z- LcmTrn::updateTrn() equating pose 1613172212.48 and meas times 1613172212.48 2021-02-12T23:23:32Z- LcmTrn::updateTrn() >>>> pose time 1613172212.48, meas time 1613172212.48 <<<< 2021-02-12T23:23:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172212.48 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:23:32Z- TerrainNav::motionUpdate() - currEstimate = (4056870, 590882.52, 24.45) 2021-02-12T23:23:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:23:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:23:32Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172212.48 sec.; No relevant navigation data available 2021-02-12T23:23:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:23:32Z- TNavBF::mmse selected from filter -1 2021-02-12T23:23:32Z- variances([0], [2], and psi_berg) = 770.520574 833.055011 0.000000 2021-02-12T23:23:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:23:32Z- LcmTrn::publishEstimates() - MLE : 4056849.45 590836.02 24.45 2021-02-12T23:23:32Z- LcmTrn::publishEstimates() - MMSE : 4056869.95 590882.52 24.45 2021-02-12T23:23:32Z- LcmTrn::publishEstimates() - COVAR: 770.52 833.06 0.93 0.00 2021-02-12T23:23:32Z- AHRS_M2 msg: 1613172212.84 epoch sec; seqNo:3307 2021-02-12T23:23:32Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:32Z- handleDepth()-Depth_Keller msg: 1613172212845 epoch sec; seqNo:3339; depth 24.34 2021-02-12T23:23:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:32Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:32Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:32Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:32Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:32Z- handleDvl() - RDI_Pathfinder msg: 1613172212.85 epoch sec; seqNo:1515 2021-02-12T23:23:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , 0.06 2021-02-12T23:23:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172212.86 epoch sec; seqNo:3339; 4056869.95 north; 590882.52 east 2021-02-12T23:23:33Z- AHRS_M2 msg: 1613172213.24 epoch sec; seqNo:3308 2021-02-12T23:23:33Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:33Z- handleDepth()-Depth_Keller msg: 1613172213246 epoch sec; seqNo:3340; depth 24.24 2021-02-12T23:23:33Z- handleDvl() - RDI_Pathfinder msg: 1613172213.26 epoch sec; seqNo:1516 2021-02-12T23:23:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 86.01 , 93.58 , 62.93 2021-02-12T23:23:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , 0.06 2021-02-12T23:23:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172213.27 epoch sec; seqNo:3340; 4056870.33 north; 590882.52 east 2021-02-12T23:23:33Z- AHRS_M2 msg: 1613172213.64 epoch sec; seqNo:3309 2021-02-12T23:23:33Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:33Z- handleDepth()-Depth_Keller msg: 1613172213653 epoch sec; seqNo:3341; depth 24.14 2021-02-12T23:23:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172213.66 epoch sec; seqNo:3341; 4056870.71 north; 590882.52 east 2021-02-12T23:23:34Z- AHRS_M2 msg: 1613172214.05 epoch sec; seqNo:3310 2021-02-12T23:23:34Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:34Z- handleDepth()-Depth_Keller msg: 1613172214057 epoch sec; seqNo:3342; depth 24.03 2021-02-12T23:23:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:34Z- handleDvl() - RDI_Pathfinder msg: 1613172214.06 epoch sec; seqNo:1517 2021-02-12T23:23:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , 0.06 2021-02-12T23:23:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172214.07 epoch sec; seqNo:3342; 4056870.71 north; 590882.52 east 2021-02-12T23:23:34Z- AHRS_M2 msg: 1613172214.45 epoch sec; seqNo:3311 2021-02-12T23:23:34Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:34Z- handleDepth()-Depth_Keller msg: 1613172214465 epoch sec; seqNo:3343; depth 23.93 2021-02-12T23:23:34Z- handleDvl() - RDI_Pathfinder msg: 1613172214.48 epoch sec; seqNo:1518 2021-02-12T23:23:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 87.10 , 96.83 , 64.15 2021-02-12T23:23:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , 0.05 2021-02-12T23:23:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172214.49 epoch sec; seqNo:3343; 4056871.10 north; 590883.73 east 2021-02-12T23:23:34Z- AHRS_M2 msg: 1613172214.86 epoch sec; seqNo:3312 2021-02-12T23:23:34Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:34Z- handleDepth()-Depth_Keller msg: 1613172214865 epoch sec; seqNo:3344; depth 23.83 2021-02-12T23:23:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172214.88 epoch sec; seqNo:3344; 4056871.48 north; 590883.73 east 2021-02-12T23:23:35Z- AHRS_M2 msg: 1613172215.26 epoch sec; seqNo:3313 2021-02-12T23:23:35Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:35Z- handleDepth()-Depth_Keller msg: 1613172215269 epoch sec; seqNo:3345; depth 23.72 2021-02-12T23:23:35Z- handleDvl() - RDI_Pathfinder msg: 1613172215.28 epoch sec; seqNo:1519 2021-02-12T23:23:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 87.10 , 95.75 , 63.74 2021-02-12T23:23:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , 0.03 2021-02-12T23:23:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172215.30 epoch sec; seqNo:3345; 4056871.86 north; 590883.72 east 2021-02-12T23:23:35Z- AHRS_M2 msg: 1613172215.66 epoch sec; seqNo:3314 2021-02-12T23:23:35Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:35Z- handleDepth()-Depth_Keller msg: 1613172215668 epoch sec; seqNo:3346; depth 23.62 2021-02-12T23:23:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172215.68 epoch sec; seqNo:3346; 4056871.86 north; 590883.72 east 2021-02-12T23:23:36Z- AHRS_M2 msg: 1613172216.07 epoch sec; seqNo:3315 2021-02-12T23:23:36Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:36Z- handleDepth()-Depth_Keller msg: 1613172216075 epoch sec; seqNo:3347; depth 23.54 2021-02-12T23:23:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172216.08 epoch sec; seqNo:3347; 4056872.24 north; 590883.72 east 2021-02-12T23:23:36Z- AHRS_M2 msg: 1613172216.47 epoch sec; seqNo:3316 2021-02-12T23:23:36Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:36Z- handleDepth()-Depth_Keller msg: 1613172216481 epoch sec; seqNo:3348; depth 23.43 2021-02-12T23:23:36Z- handleDvl() - RDI_Pathfinder msg: 1613172216.49 epoch sec; seqNo:1520 2021-02-12T23:23:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 88.18 , 95.75 , 64.56 2021-02-12T23:23:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.06 , 0.02 2021-02-12T23:23:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172216.53 epoch sec; seqNo:3348; 4056872.63 north; 590884.94 east 2021-02-12T23:23:36Z- AHRS_M2 msg: 1613172216.88 epoch sec; seqNo:3317 2021-02-12T23:23:36Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:36Z- handleDepth()-Depth_Keller msg: 1613172216885 epoch sec; seqNo:3349; depth 23.33 2021-02-12T23:23:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172216.91 epoch sec; seqNo:3349; 4056873.01 north; 590884.93 east 2021-02-12T23:23:37Z- AHRS_M2 msg: 1613172217.28 epoch sec; seqNo:3318 2021-02-12T23:23:37Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:37Z- handleDepth()-Depth_Keller msg: 1613172217293 epoch sec; seqNo:3350; depth 23.21 2021-02-12T23:23:37Z- handleDvl() - RDI_Pathfinder msg: 1613172217.32 epoch sec; seqNo:1521 2021-02-12T23:23:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.87 , 88.18 , 95.75 , 64.15 2021-02-12T23:23:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.03 2021-02-12T23:23:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172217.35 epoch sec; seqNo:3350; 4056873.01 north; 590884.93 east 2021-02-12T23:23:37Z- LcmTrn::handleMessages() - handling 43 messages took 0.76 ms and 128 calls... 2021-02-12T23:23:37Z- waiting for 1613172215.48, time is 1613172217.35 2021-02-12T23:23:37Z- LcmTrn::updateTrn() equating pose 1613172217.35 and meas times 1613172217.35 2021-02-12T23:23:37Z- LcmTrn::updateTrn() >>>> pose time 1613172217.35, meas time 1613172217.35 <<<< 2021-02-12T23:23:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172217.35 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:23:37Z- TerrainNav::motionUpdate() - currEstimate = (4056873, 590884.93, 23.21) 2021-02-12T23:23:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:23:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:23:37Z- TNavBF::Projecting Measurements 2021-02-12T23:23:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:23:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:23:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:23:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:23:37Z- TNavFilter::4 good beams. 2021-02-12T23:23:37Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:23:37Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:23:37Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:23:37Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:23:37Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:23:37Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:23:37Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:23:37Z- TNavBF::mmse selected from filter -1 2021-02-12T23:23:37Z- TerrainMapOctree: (vehN, vehE ) = (4056873.01, 590884.93). 2021-02-12T23:23:37Z- TerrainMapOctree: Min Distance = 5521.03. 2021-02-12T23:23:37Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:23:37Z- There is no specific good beam that all particles have in common. 2021-02-12T23:23:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:23:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 7.81 2021-02-12T23:23:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:23:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 12.44 2021-02-12T23:23:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:23:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 14.67 2021-02-12T23:23:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:23:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 6.58 2021-02-12T23:23:37Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:23:37Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:23:37Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:23:37Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:23:37Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:23:37Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:23:37Z- Filter 0 No beams used in subcloud update 2021-02-12T23:23:37Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:23:37Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:23:37Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 770.4 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:23:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172217.35 sec. 2021-02-12T23:23:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:23:37Z- TNavBF::mmse selected from filter -1 2021-02-12T23:23:37Z- variances([0], [2], and psi_berg) = 770.391696 833.054852 0.000000 2021-02-12T23:23:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:23:37Z- LcmTrn::publishEstimates() - MLE : 4056852.51 590934.43 23.21 2021-02-12T23:23:37Z- LcmTrn::publishEstimates() - MMSE : 4056873.01 590884.93 23.21 2021-02-12T23:23:37Z- LcmTrn::publishEstimates() - COVAR: 770.39 833.05 0.80 0.00 2021-02-12T23:23:37Z- AHRS_M2 msg: 1613172217.69 epoch sec; seqNo:3319 2021-02-12T23:23:37Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:37Z- handleDepth()-Depth_Keller msg: 1613172217693 epoch sec; seqNo:3351; depth 23.11 2021-02-12T23:23:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:37Z- handleDvl() - RDI_Pathfinder msg: 1613172217.69 epoch sec; seqNo:1522 2021-02-12T23:23:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.03 2021-02-12T23:23:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172217.75 epoch sec; seqNo:3351; 4056873.39 north; 590884.93 east 2021-02-12T23:23:38Z- AHRS_M2 msg: 1613172218.09 epoch sec; seqNo:3320 2021-02-12T23:23:38Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-12T23:23:38Z- handleDepth()-Depth_Keller msg: 1613172218093 epoch sec; seqNo:3352; depth 23.02 2021-02-12T23:23:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172218.11 epoch sec; seqNo:3352; 4056873.77 north; 590884.93 east 2021-02-12T23:23:38Z- AHRS_M2 msg: 1613172218.49 epoch sec; seqNo:3321 2021-02-12T23:23:38Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-12T23:23:38Z- handleDepth()-Depth_Keller msg: 1613172218501 epoch sec; seqNo:3353; depth 22.92 2021-02-12T23:23:38Z- handleDvl() - RDI_Pathfinder msg: 1613172218.51 epoch sec; seqNo:1523 2021-02-12T23:23:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.87 , 88.18 , 96.83 , 64.56 2021-02-12T23:23:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.07 , -0.08 2021-02-12T23:23:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172218.55 epoch sec; seqNo:3353; 4056873.77 north; 590884.93 east 2021-02-12T23:23:38Z- AHRS_M2 msg: 1613172218.90 epoch sec; seqNo:3322 2021-02-12T23:23:38Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-12T23:23:38Z- handleDepth()-Depth_Keller msg: 1613172218905 epoch sec; seqNo:3354; depth 22.82 2021-02-12T23:23:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:38Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:38Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:38Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:38Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:38Z- handleDvl() - RDI_Pathfinder msg: 1613172218.91 epoch sec; seqNo:1524 2021-02-12T23:23:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.07 , -0.08 2021-02-12T23:23:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172218.92 epoch sec; seqNo:3354; 4056874.16 north; 590886.14 east 2021-02-12T23:23:39Z- AHRS_M2 msg: 1613172219.30 epoch sec; seqNo:3323 2021-02-12T23:23:39Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-12T23:23:39Z- handleDepth()-Depth_Keller msg: 1613172219305 epoch sec; seqNo:3355; depth 22.71 2021-02-12T23:23:39Z- handleDvl() - RDI_Pathfinder msg: 1613172219.32 epoch sec; seqNo:1525 2021-02-12T23:23:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.87 , 89.26 , 96.83 , 65.38 2021-02-12T23:23:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , -0.13 2021-02-12T23:23:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172219.34 epoch sec; seqNo:3355; 4056874.54 north; 590886.14 east 2021-02-12T23:23:39Z- AHRS_M2 msg: 1613172219.70 epoch sec; seqNo:3324 2021-02-12T23:23:39Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-12T23:23:39Z- handleDepth()-Depth_Keller msg: 1613172219709 epoch sec; seqNo:3356; depth 22.60 2021-02-12T23:23:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172219.74 epoch sec; seqNo:3356; 4056874.54 north; 590886.14 east 2021-02-12T23:23:40Z- AHRS_M2 msg: 1613172220.11 epoch sec; seqNo:3325 2021-02-12T23:23:40Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-12T23:23:40Z- handleDepth()-Depth_Keller msg: 1613172220113 epoch sec; seqNo:3357; depth 22.50 2021-02-12T23:23:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:40Z- handleDvl() - RDI_Pathfinder msg: 1613172220.12 epoch sec; seqNo:1526 2021-02-12T23:23:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , -0.13 2021-02-12T23:23:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172220.17 epoch sec; seqNo:3357; 4056874.92 north; 590886.13 east 2021-02-12T23:23:40Z- AHRS_M2 msg: 1613172220.51 epoch sec; seqNo:3326 2021-02-12T23:23:40Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:40Z- handleDepth()-Depth_Keller msg: 1613172220517 epoch sec; seqNo:3358; depth 22.40 2021-02-12T23:23:40Z- handleDvl() - RDI_Pathfinder msg: 1613172220.54 epoch sec; seqNo:1527 2021-02-12T23:23:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.28 , 89.26 , 95.75 , 65.38 2021-02-12T23:23:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.05 , -0.12 2021-02-12T23:23:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172220.56 epoch sec; seqNo:3358; 4056875.30 north; 590886.13 east 2021-02-12T23:23:40Z- AHRS_M2 msg: 1613172220.92 epoch sec; seqNo:3327 2021-02-12T23:23:40Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:40Z- handleDepth()-Depth_Keller msg: 1613172220921 epoch sec; seqNo:3359; depth 22.29 2021-02-12T23:23:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172220.94 epoch sec; seqNo:3359; 4056875.30 north; 590886.13 east 2021-02-12T23:23:41Z- AHRS_M2 msg: 1613172221.32 epoch sec; seqNo:3328 2021-02-12T23:23:41Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:41Z- handleDepth()-Depth_Keller msg: 1613172221325 epoch sec; seqNo:3360; depth 22.20 2021-02-12T23:23:41Z- handleDvl() - RDI_Pathfinder msg: 1613172221.35 epoch sec; seqNo:1528 2021-02-12T23:23:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.28 , 89.26 , 96.83 , 65.38 2021-02-12T23:23:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.10 , -0.06 2021-02-12T23:23:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172221.39 epoch sec; seqNo:3360; 4056875.67 north; 590886.13 east 2021-02-12T23:23:41Z- AHRS_M2 msg: 1613172221.72 epoch sec; seqNo:3329 2021-02-12T23:23:41Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:41Z- handleDepth()-Depth_Keller msg: 1613172221729 epoch sec; seqNo:3361; depth 22.10 2021-02-12T23:23:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172221.75 epoch sec; seqNo:3361; 4056876.05 north; 590886.12 east 2021-02-12T23:23:42Z- AHRS_M2 msg: 1613172222.13 epoch sec; seqNo:3330 2021-02-12T23:23:42Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:42Z- handleDepth()-Depth_Keller msg: 1613172222135 epoch sec; seqNo:3362; depth 21.99 2021-02-12T23:23:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172222.16 epoch sec; seqNo:3362; 4056876.07 north; 590887.34 east 2021-02-12T23:23:42Z- AHRS_M2 msg: 1613172222.53 epoch sec; seqNo:3331 2021-02-12T23:23:42Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:42Z- handleDepth()-Depth_Keller msg: 1613172222543 epoch sec; seqNo:3363; depth 21.88 2021-02-12T23:23:42Z- handleDvl() - RDI_Pathfinder msg: 1613172222.56 epoch sec; seqNo:1529 2021-02-12T23:23:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.28 , 89.26 , 96.83 , 65.38 2021-02-12T23:23:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.12 , 0.01 2021-02-12T23:23:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172222.57 epoch sec; seqNo:3363; 4056876.44 north; 590887.34 east 2021-02-12T23:23:42Z- LcmTrn::handleMessages() - handling 47 messages took 0.77 ms and 132 calls... 2021-02-12T23:23:42Z- waiting for 1613172220.35, time is 1613172222.57 2021-02-12T23:23:42Z- LcmTrn::updateTrn() equating pose 1613172222.57 and meas times 1613172222.57 2021-02-12T23:23:42Z- LcmTrn::updateTrn() >>>> pose time 1613172222.57, meas time 1613172222.57 <<<< 2021-02-12T23:23:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172222.57 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:23:42Z- TerrainNav::motionUpdate() - currEstimate = (4056876, 590887.34, 21.88) 2021-02-12T23:23:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:23:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:23:42Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172222.57 sec.; No relevant navigation data available 2021-02-12T23:23:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:23:42Z- TNavBF::mmse selected from filter -1 2021-02-12T23:23:42Z- variances([0], [2], and psi_berg) = 770.258184 833.054688 0.000000 2021-02-12T23:23:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:23:42Z- LcmTrn::publishEstimates() - MLE : 4056855.94 590936.84 21.88 2021-02-12T23:23:42Z- LcmTrn::publishEstimates() - MMSE : 4056876.44 590887.34 21.88 2021-02-12T23:23:42Z- LcmTrn::publishEstimates() - COVAR: 770.26 833.05 1.02 0.00 2021-02-12T23:23:42Z- AHRS_M2 msg: 1613172222.93 epoch sec; seqNo:3332 2021-02-12T23:23:42Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:42Z- handleDepth()-Depth_Keller msg: 1613172222941 epoch sec; seqNo:3364; depth 21.79 2021-02-12T23:23:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172222.96 epoch sec; seqNo:3364; 4056876.82 north; 590887.34 east 2021-02-12T23:23:43Z- AHRS_M2 msg: 1613172223.34 epoch sec; seqNo:3333 2021-02-12T23:23:43Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:43Z- handleDepth()-Depth_Keller msg: 1613172223344 epoch sec; seqNo:3365; depth 21.68 2021-02-12T23:23:43Z- handleDvl() - RDI_Pathfinder msg: 1613172223.36 epoch sec; seqNo:1530 2021-02-12T23:23:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.28 , 89.26 , 96.83 , 64.97 2021-02-12T23:23:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.11 , 0.09 2021-02-12T23:23:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172223.37 epoch sec; seqNo:3365; 4056877.20 north; 590887.33 east 2021-02-12T23:23:43Z- AHRS_M2 msg: 1613172223.75 epoch sec; seqNo:3334 2021-02-12T23:23:43Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:43Z- handleDepth()-Depth_Keller msg: 1613172223765 epoch sec; seqNo:3366; depth 21.56 2021-02-12T23:23:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:43Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:43Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:43Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:43Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:43Z- handleDvl() - RDI_Pathfinder msg: 1613172223.77 epoch sec; seqNo:1531 2021-02-12T23:23:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.11 , 0.09 2021-02-12T23:23:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172223.78 epoch sec; seqNo:3366; 4056877.20 north; 590887.33 east 2021-02-12T23:23:44Z- AHRS_M2 msg: 1613172224.15 epoch sec; seqNo:3335 2021-02-12T23:23:44Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:44Z- handleDepth()-Depth_Keller msg: 1613172224166 epoch sec; seqNo:3367; depth 21.46 2021-02-12T23:23:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172224.18 epoch sec; seqNo:3367; 4056877.58 north; 590887.33 east 2021-02-12T23:23:44Z- AHRS_M2 msg: 1613172224.55 epoch sec; seqNo:3336 2021-02-12T23:23:44Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:44Z- handleDepth()-Depth_Keller msg: 1613172224556 epoch sec; seqNo:3368; depth 21.36 2021-02-12T23:23:44Z- handleDvl() - RDI_Pathfinder msg: 1613172224.57 epoch sec; seqNo:1532 2021-02-12T23:23:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.10 , 88.18 , 96.83 , 65.38 2021-02-12T23:23:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , 0.11 2021-02-12T23:23:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172224.60 epoch sec; seqNo:3368; 4056877.96 north; 590887.32 east 2021-02-12T23:23:44Z- AHRS_M2 msg: 1613172224.97 epoch sec; seqNo:3337 2021-02-12T23:23:44Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:44Z- handleDepth()-Depth_Keller msg: 1613172224974 epoch sec; seqNo:3369; depth 21.27 2021-02-12T23:23:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:44Z- handleDvl() - RDI_Pathfinder msg: 1613172224.98 epoch sec; seqNo:1533 2021-02-12T23:23:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , 0.11 2021-02-12T23:23:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172225.01 epoch sec; seqNo:3369; 4056877.96 north; 590887.32 east 2021-02-12T23:23:45Z- AHRS_M2 msg: 1613172225.36 epoch sec; seqNo:3338 2021-02-12T23:23:45Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.18 psi 2021-02-12T23:23:45Z- handleDepth()-Depth_Keller msg: 1613172225365 epoch sec; seqNo:3370; depth 21.17 2021-02-12T23:23:45Z- handleDvl() - RDI_Pathfinder msg: 1613172225.38 epoch sec; seqNo:1534 2021-02-12T23:23:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 89.26 , 97.91 , 66.19 2021-02-12T23:23:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , 0.10 2021-02-12T23:23:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172225.39 epoch sec; seqNo:3370; 4056878.35 north; 590888.54 east 2021-02-12T23:23:45Z- AHRS_M2 msg: 1613172225.76 epoch sec; seqNo:3339 2021-02-12T23:23:45Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:45Z- handleDepth()-Depth_Keller msg: 1613172225789 epoch sec; seqNo:3371; depth 21.06 2021-02-12T23:23:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172225.80 epoch sec; seqNo:3371; 4056878.73 north; 590888.54 east 2021-02-12T23:23:46Z- AHRS_M2 msg: 1613172226.17 epoch sec; seqNo:3340 2021-02-12T23:23:46Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:46Z- handleDepth()-Depth_Keller msg: 1613172226183 epoch sec; seqNo:3372; depth 20.95 2021-02-12T23:23:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:46Z- handleDvl() - RDI_Pathfinder msg: 1613172226.19 epoch sec; seqNo:1535 2021-02-12T23:23:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , 0.10 2021-02-12T23:23:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172226.22 epoch sec; seqNo:3372; 4056879.11 north; 590888.53 east 2021-02-12T23:23:46Z- AHRS_M2 msg: 1613172226.58 epoch sec; seqNo:3341 2021-02-12T23:23:46Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.16 psi 2021-02-12T23:23:46Z- handleDepth()-Depth_Keller msg: 1613172226589 epoch sec; seqNo:3373; depth 20.85 2021-02-12T23:23:46Z- handleDvl() - RDI_Pathfinder msg: 1613172226.60 epoch sec; seqNo:1536 2021-02-12T23:23:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 91.36 , 96.76 , 66.60 2021-02-12T23:23:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.07 , 0.04 2021-02-12T23:23:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172226.62 epoch sec; seqNo:3373; 4056879.11 north; 590888.53 east 2021-02-12T23:23:46Z- AHRS_M2 msg: 1613172226.98 epoch sec; seqNo:3342 2021-02-12T23:23:46Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.16 psi 2021-02-12T23:23:46Z- handleDepth()-Depth_Keller msg: 1613172226989 epoch sec; seqNo:3374; depth 20.76 2021-02-12T23:23:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172227.00 epoch sec; seqNo:3374; 4056879.49 north; 590888.53 east 2021-02-12T23:23:47Z- AHRS_M2 msg: 1613172227.38 epoch sec; seqNo:3343 2021-02-12T23:23:47Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:23:47Z- handleDepth()-Depth_Keller msg: 1613172227394 epoch sec; seqNo:3375; depth 20.64 2021-02-12T23:23:47Z- handleDvl() - RDI_Pathfinder msg: 1613172227.41 epoch sec; seqNo:1537 2021-02-12T23:23:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 90.21 , 96.69 , 67.01 2021-02-12T23:23:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.07 , 0.00 2021-02-12T23:23:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172227.41 epoch sec; seqNo:3375; 4056879.88 north; 590889.74 east 2021-02-12T23:23:47Z- AHRS_M2 msg: 1613172227.78 epoch sec; seqNo:3344 2021-02-12T23:23:47Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:23:47Z- LcmTrn::handleMessages() - handling 45 messages took 0.65 ms and 132 calls... 2021-02-12T23:23:47Z- waiting for 1613172225.57, time is 1613172227.78 2021-02-12T23:23:47Z- LcmTrn::updateTrn() >>>> pose time 1613172227.78, meas time 1613172227.41 <<<< 2021-02-12T23:23:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172227.41 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:23:47Z- TerrainNav::motionUpdate() - currEstimate = (4056880, 590889.74, 20.64) 2021-02-12T23:23:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:23:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:23:47Z- TNavBF::Projecting Measurements 2021-02-12T23:23:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:23:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:23:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:23:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:23:47Z- TNavFilter::4 good beams. 2021-02-12T23:23:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:23:47Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:23:47Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:23:47Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:23:47Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:23:47Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:23:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:23:47Z- TNavBF::mmse selected from filter -1 2021-02-12T23:23:47Z- TerrainMapOctree: (vehN, vehE ) = (4056879.63, 590889.57). 2021-02-12T23:23:47Z- TerrainMapOctree: Min Distance = 5512.95. 2021-02-12T23:23:47Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:23:47Z- There is no specific good beam that all particles have in common. 2021-02-12T23:23:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:23:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.108 ExpectedMeasDiff: nan currMeas.covariance: 8.27 2021-02-12T23:23:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:23:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 13.02 2021-02-12T23:23:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:23:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 14.96 2021-02-12T23:23:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:23:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 7.18 2021-02-12T23:23:47Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:23:47Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:23:47Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:23:47Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:23:47Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:23:47Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:23:47Z- Filter 0 No beams used in subcloud update 2021-02-12T23:23:47Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:23:47Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:23:47Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 770.1 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:23:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172227.41 sec. 2021-02-12T23:23:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:23:47Z- TNavBF::mmse selected from filter -1 2021-02-12T23:23:47Z- variances([0], [2], and psi_berg) = 770.094081 833.054486 0.000000 2021-02-12T23:23:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:23:47Z- LcmTrn::publishEstimates() - MLE : 4056859.38 590939.24 20.64 2021-02-12T23:23:47Z- LcmTrn::publishEstimates() - MMSE : 4056879.88 590889.74 20.64 2021-02-12T23:23:47Z- LcmTrn::publishEstimates() - COVAR: 770.09 833.05 0.88 0.00 2021-02-12T23:23:47Z- handleDepth()-Depth_Keller msg: 1613172227788 epoch sec; seqNo:3376; depth 20.54 2021-02-12T23:23:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172227.80 epoch sec; seqNo:3376; 4056879.88 north; 590889.74 east 2021-02-12T23:23:48Z- AHRS_M2 msg: 1613172228.21 epoch sec; seqNo:3345 2021-02-12T23:23:48Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:23:48Z- handleDepth()-Depth_Keller msg: 1613172228215 epoch sec; seqNo:3377; depth 20.45 2021-02-12T23:23:48Z- handleDvl() - RDI_Pathfinder msg: 1613172228.23 epoch sec; seqNo:1538 2021-02-12T23:23:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.32 , 90.27 , 96.76 , 66.60 2021-02-12T23:23:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.01 2021-02-12T23:23:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172228.24 epoch sec; seqNo:3377; 4056880.26 north; 590889.74 east 2021-02-12T23:23:48Z- AHRS_M2 msg: 1613172228.59 epoch sec; seqNo:3346 2021-02-12T23:23:48Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:23:48Z- handleDepth()-Depth_Keller msg: 1613172228598 epoch sec; seqNo:3378; depth 20.34 2021-02-12T23:23:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:48Z- handleDvl() - RDI_Pathfinder msg: 1613172228.60 epoch sec; seqNo:1539 2021-02-12T23:23:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.01 2021-02-12T23:23:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172228.61 epoch sec; seqNo:3378; 4056880.64 north; 590889.74 east 2021-02-12T23:23:48Z- AHRS_M2 msg: 1613172229.00 epoch sec; seqNo:3347 2021-02-12T23:23:48Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:23:49Z- handleDepth()-Depth_Keller msg: 1613172229000 epoch sec; seqNo:3379; depth 20.25 2021-02-12T23:23:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172229.02 epoch sec; seqNo:3379; 4056881.02 north; 590889.73 east 2021-02-12T23:23:49Z- AHRS_M2 msg: 1613172229.40 epoch sec; seqNo:3348 2021-02-12T23:23:49Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:23:49Z- handleDepth()-Depth_Keller msg: 1613172229404 epoch sec; seqNo:3380; depth 20.14 2021-02-12T23:23:49Z- handleDvl() - RDI_Pathfinder msg: 1613172229.42 epoch sec; seqNo:1540 2021-02-12T23:23:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.14 , 90.27 , 101.09 , 67.42 2021-02-12T23:23:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , -0.01 2021-02-12T23:23:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172229.44 epoch sec; seqNo:3380; 4056881.02 north; 590889.73 east 2021-02-12T23:23:49Z- AHRS_M2 msg: 1613172229.80 epoch sec; seqNo:3349 2021-02-12T23:23:49Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:23:49Z- handleDepth()-Depth_Keller msg: 1613172229808 epoch sec; seqNo:3381; depth 20.05 2021-02-12T23:23:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:49Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:49Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:49Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:49Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:49Z- handleDvl() - RDI_Pathfinder msg: 1613172229.81 epoch sec; seqNo:1541 2021-02-12T23:23:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , -0.01 2021-02-12T23:23:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172229.82 epoch sec; seqNo:3381; 4056881.41 north; 590890.95 east 2021-02-12T23:23:50Z- AHRS_M2 msg: 1613172230.21 epoch sec; seqNo:3350 2021-02-12T23:23:50Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.16 psi 2021-02-12T23:23:50Z- handleDepth()-Depth_Keller msg: 1613172230214 epoch sec; seqNo:3382; depth 19.95 2021-02-12T23:23:50Z- handleDvl() - RDI_Pathfinder msg: 1613172230.23 epoch sec; seqNo:1542 2021-02-12T23:23:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 88.65 , 97.17 , 67.83 2021-02-12T23:23:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , -0.01 2021-02-12T23:23:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172230.24 epoch sec; seqNo:3382; 4056881.79 north; 590890.95 east 2021-02-12T23:23:50Z- AHRS_M2 msg: 1613172230.61 epoch sec; seqNo:3351 2021-02-12T23:23:50Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-12T23:23:50Z- handleDepth()-Depth_Keller msg: 1613172230621 epoch sec; seqNo:3383; depth 19.83 2021-02-12T23:23:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172230.63 epoch sec; seqNo:3383; 4056881.79 north; 590890.95 east 2021-02-12T23:23:51Z- AHRS_M2 msg: 1613172231.02 epoch sec; seqNo:3352 2021-02-12T23:23:51Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-12T23:23:51Z- handleDepth()-Depth_Keller msg: 1613172231029 epoch sec; seqNo:3384; depth 19.73 2021-02-12T23:23:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:51Z- handleDvl() - RDI_Pathfinder msg: 1613172231.03 epoch sec; seqNo:1543 2021-02-12T23:23:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , -0.01 2021-02-12T23:23:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172231.04 epoch sec; seqNo:3384; 4056882.17 north; 590890.94 east 2021-02-12T23:23:51Z- AHRS_M2 msg: 1613172231.42 epoch sec; seqNo:3353 2021-02-12T23:23:51Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-12T23:23:51Z- handleDepth()-Depth_Keller msg: 1613172231426 epoch sec; seqNo:3385; depth 19.62 2021-02-12T23:23:51Z- handleDvl() - RDI_Pathfinder msg: 1613172231.44 epoch sec; seqNo:1544 2021-02-12T23:23:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.55 , 92.44 , 104.33 , 67.42 2021-02-12T23:23:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , -0.04 2021-02-12T23:23:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172231.45 epoch sec; seqNo:3385; 4056882.54 north; 590890.94 east 2021-02-12T23:23:51Z- AHRS_M2 msg: 1613172231.82 epoch sec; seqNo:3354 2021-02-12T23:23:51Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-12T23:23:51Z- handleDepth()-Depth_Keller msg: 1613172231833 epoch sec; seqNo:3386; depth 19.52 2021-02-12T23:23:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172231.85 epoch sec; seqNo:3386; 4056882.92 north; 590890.93 east 2021-02-12T23:23:52Z- AHRS_M2 msg: 1613172232.23 epoch sec; seqNo:3355 2021-02-12T23:23:52Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-12T23:23:52Z- handleDepth()-Depth_Keller msg: 1613172232237 epoch sec; seqNo:3387; depth 19.41 2021-02-12T23:23:52Z- handleDvl() - RDI_Pathfinder msg: 1613172232.25 epoch sec; seqNo:1545 2021-02-12T23:23:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.14 , 92.17 , 97.03 , 68.24 2021-02-12T23:23:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , -0.07 2021-02-12T23:23:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172232.27 epoch sec; seqNo:3387; 4056882.94 north; 590892.15 east 2021-02-12T23:23:52Z- AHRS_M2 msg: 1613172232.63 epoch sec; seqNo:3356 2021-02-12T23:23:52Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-12T23:23:52Z- handleDepth()-Depth_Keller msg: 1613172232638 epoch sec; seqNo:3388; depth 19.29 2021-02-12T23:23:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172232.65 epoch sec; seqNo:3388; 4056883.32 north; 590892.15 east 2021-02-12T23:23:52Z- LcmTrn::handleMessages() - handling 46 messages took 0.64 ms and 134 calls... 2021-02-12T23:23:52Z- waiting for 1613172230.78, time is 1613172232.65 2021-02-12T23:23:52Z- LcmTrn::updateTrn() >>>> pose time 1613172232.65, meas time 1613172232.25 <<<< 2021-02-12T23:23:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172232.25 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:23:52Z- TerrainNav::motionUpdate() - currEstimate = (4056883, 590892.15, 19.29) 2021-02-12T23:23:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:23:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:23:52Z- TNavBF::Projecting Measurements 2021-02-12T23:23:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:23:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:23:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:23:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:23:52Z- TNavFilter::4 good beams. 2021-02-12T23:23:52Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:23:52Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:23:52Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:23:52Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:23:52Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:23:52Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:23:52Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:23:52Z- TNavBF::mmse selected from filter -1 2021-02-12T23:23:52Z- TerrainMapOctree: (vehN, vehE ) = (4056883.03, 590891.95). 2021-02-12T23:23:52Z- TerrainMapOctree: Min Distance = 5508.80. 2021-02-12T23:23:52Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:23:52Z- There is no specific good beam that all particles have in common. 2021-02-12T23:23:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:23:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-12T23:23:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:23:52Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 13.59 2021-02-12T23:23:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:23:52Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 15.06 2021-02-12T23:23:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:23:52Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-12T23:23:52Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:23:52Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:23:52Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:23:52Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:23:52Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:23:52Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:23:52Z- Filter 0 No beams used in subcloud update 2021-02-12T23:23:52Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:23:52Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:23:52Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 770.0 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:23:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172232.25 sec. 2021-02-12T23:23:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:23:52Z- TNavBF::mmse selected from filter -1 2021-02-12T23:23:52Z- variances([0], [2], and psi_berg) = 769.928676 833.054283 0.000000 2021-02-12T23:23:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:23:52Z- LcmTrn::publishEstimates() - MLE : 4056862.82 590941.65 19.29 2021-02-12T23:23:52Z- LcmTrn::publishEstimates() - MMSE : 4056883.32 590892.15 19.29 2021-02-12T23:23:52Z- LcmTrn::publishEstimates() - COVAR: 769.93 833.05 0.81 0.00 2021-02-12T23:23:53Z- AHRS_M2 msg: 1613172233.04 epoch sec; seqNo:3357 2021-02-12T23:23:53Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-12T23:23:53Z- handleDepth()-Depth_Keller msg: 1613172233042 epoch sec; seqNo:3389; depth 19.19 2021-02-12T23:23:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172233.05 epoch sec; seqNo:3389; 4056883.69 north; 590892.15 east 2021-02-12T23:23:53Z- AHRS_M2 msg: 1613172233.44 epoch sec; seqNo:3358 2021-02-12T23:23:53Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:53Z- handleDepth()-Depth_Keller msg: 1613172233457 epoch sec; seqNo:3390; depth 19.10 2021-02-12T23:23:53Z- handleDvl() - RDI_Pathfinder msg: 1613172233.52 epoch sec; seqNo:1546 2021-02-12T23:23:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.73 , 92.30 , 97.17 , 68.65 2021-02-12T23:23:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , -0.11 2021-02-12T23:23:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172233.54 epoch sec; seqNo:3390; 4056883.69 north; 590892.15 east 2021-02-12T23:23:53Z- AHRS_M2 msg: 1613172233.85 epoch sec; seqNo:3359 2021-02-12T23:23:53Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:53Z- handleDepth()-Depth_Keller msg: 1613172233849 epoch sec; seqNo:3391; depth 18.97 2021-02-12T23:23:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172233.86 epoch sec; seqNo:3391; 4056884.07 north; 590892.14 east 2021-02-12T23:23:54Z- AHRS_M2 msg: 1613172234.25 epoch sec; seqNo:3360 2021-02-12T23:23:54Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:23:54Z- handleDepth()-Depth_Keller msg: 1613172234252 epoch sec; seqNo:3392; depth 18.88 2021-02-12T23:23:54Z- handleDvl() - RDI_Pathfinder msg: 1613172234.27 epoch sec; seqNo:1547 2021-02-12T23:23:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.55 , 92.30 , 97.17 , 67.42 2021-02-12T23:23:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.08 , -0.08 2021-02-12T23:23:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172234.28 epoch sec; seqNo:3392; 4056884.45 north; 590892.14 east 2021-02-12T23:23:54Z- AHRS_M2 msg: 1613172234.65 epoch sec; seqNo:3361 2021-02-12T23:23:54Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-12T23:23:54Z- handleDepth()-Depth_Keller msg: 1613172234665 epoch sec; seqNo:3393; depth 18.77 2021-02-12T23:23:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:54Z- handleDvl() - RDI_Pathfinder msg: 1613172234.67 epoch sec; seqNo:1548 2021-02-12T23:23:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.08 , -0.08 2021-02-12T23:23:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172234.69 epoch sec; seqNo:3393; 4056884.45 north; 590892.14 east 2021-02-12T23:23:55Z- AHRS_M2 msg: 1613172235.06 epoch sec; seqNo:3362 2021-02-12T23:23:55Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:23:55Z- handleDepth()-Depth_Keller msg: 1613172235060 epoch sec; seqNo:3394; depth 18.66 2021-02-12T23:23:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172235.07 epoch sec; seqNo:3394; 4056884.83 north; 590892.13 east 2021-02-12T23:23:55Z- AHRS_M2 msg: 1613172235.46 epoch sec; seqNo:3363 2021-02-12T23:23:55Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:23:55Z- handleDepth()-Depth_Keller msg: 1613172235466 epoch sec; seqNo:3395; depth 18.57 2021-02-12T23:23:55Z- handleDvl() - RDI_Pathfinder msg: 1613172235.48 epoch sec; seqNo:1549 2021-02-12T23:23:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.55 , 92.30 , 97.17 , 68.24 2021-02-12T23:23:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.11 , -0.01 2021-02-12T23:23:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172235.49 epoch sec; seqNo:3395; 4056885.22 north; 590893.35 east 2021-02-12T23:23:55Z- AHRS_M2 msg: 1613172235.86 epoch sec; seqNo:3364 2021-02-12T23:23:55Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:23:55Z- handleDepth()-Depth_Keller msg: 1613172235893 epoch sec; seqNo:3396; depth 18.44 2021-02-12T23:23:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:55Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:55Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:55Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:55Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:55Z- handleDvl() - RDI_Pathfinder msg: 1613172235.89 epoch sec; seqNo:1550 2021-02-12T23:23:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.11 , -0.01 2021-02-12T23:23:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172235.93 epoch sec; seqNo:3396; 4056885.60 north; 590893.35 east 2021-02-12T23:23:56Z- AHRS_M2 msg: 1613172236.27 epoch sec; seqNo:3365 2021-02-12T23:23:56Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-12T23:23:56Z- handleDepth()-Depth_Keller msg: 1613172236273 epoch sec; seqNo:3397; depth 18.33 2021-02-12T23:23:56Z- handleDvl() - RDI_Pathfinder msg: 1613172236.29 epoch sec; seqNo:1551 2021-02-12T23:23:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 91.08 , 97.17 , 68.65 2021-02-12T23:23:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.12 , 0.05 2021-02-12T23:23:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172236.31 epoch sec; seqNo:3397; 4056885.60 north; 590893.35 east 2021-02-12T23:23:56Z- AHRS_M2 msg: 1613172236.67 epoch sec; seqNo:3366 2021-02-12T23:23:56Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-12T23:23:56Z- handleDepth()-Depth_Keller msg: 1613172236676 epoch sec; seqNo:3398; depth 18.23 2021-02-12T23:23:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172236.69 epoch sec; seqNo:3398; 4056885.98 north; 590893.34 east 2021-02-12T23:23:57Z- AHRS_M2 msg: 1613172237.08 epoch sec; seqNo:3367 2021-02-12T23:23:57Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-12T23:23:57Z- handleDepth()-Depth_Keller msg: 1613172237082 epoch sec; seqNo:3399; depth 18.15 2021-02-12T23:23:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:23:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:23:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:23:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:23:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:23:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:23:57Z- handleDvl() - RDI_Pathfinder msg: 1613172237.08 epoch sec; seqNo:1552 2021-02-12T23:23:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:23:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.12 , 0.05 2021-02-12T23:23:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172237.09 epoch sec; seqNo:3399; 4056886.36 north; 590893.34 east 2021-02-12T23:23:57Z- AHRS_M2 msg: 1613172237.48 epoch sec; seqNo:3368 2021-02-12T23:23:57Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-12T23:23:57Z- handleDepth()-Depth_Keller msg: 1613172237489 epoch sec; seqNo:3400; depth 18.02 2021-02-12T23:23:57Z- handleDvl() - RDI_Pathfinder msg: 1613172237.51 epoch sec; seqNo:1553 2021-02-12T23:23:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 98.38 , 97.17 , 68.24 2021-02-12T23:23:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.11 , 0.10 2021-02-12T23:23:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172237.52 epoch sec; seqNo:3400; 4056886.74 north; 590893.34 east 2021-02-12T23:23:57Z- AHRS_M2 msg: 1613172237.88 epoch sec; seqNo:3369 2021-02-12T23:23:57Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.14 psi 2021-02-12T23:23:57Z- handleDepth()-Depth_Keller msg: 1613172237907 epoch sec; seqNo:3401; depth 17.91 2021-02-12T23:23:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172237.93 epoch sec; seqNo:3401; 4056887.12 north; 590893.33 east 2021-02-12T23:23:57Z- LcmTrn::handleMessages() - handling 47 messages took 0.74 ms and 132 calls... 2021-02-12T23:23:57Z- waiting for 1613172235.65, time is 1613172237.93 2021-02-12T23:23:57Z- LcmTrn::updateTrn() >>>> pose time 1613172237.93, meas time 1613172237.51 <<<< 2021-02-12T23:23:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172237.51 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:23:57Z- TerrainNav::motionUpdate() - currEstimate = (4056887, 590893.33, 17.91) 2021-02-12T23:23:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:23:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:23:57Z- TNavBF::Projecting Measurements 2021-02-12T23:23:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:23:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:23:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:23:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:23:57Z- TNavFilter::4 good beams. 2021-02-12T23:23:57Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:23:57Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:23:57Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:23:57Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:23:57Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:23:57Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:23:57Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:23:57Z- TNavBF::mmse selected from filter -1 2021-02-12T23:23:57Z- TerrainMapOctree: (vehN, vehE ) = (4056886.81, 590893.24). 2021-02-12T23:23:57Z- TerrainMapOctree: Min Distance = 5505.02. 2021-02-12T23:23:57Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:23:58Z- There is no specific good beam that all particles have in common. 2021-02-12T23:23:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:23:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-12T23:23:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:23:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 15.49 2021-02-12T23:23:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:23:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 15.11 2021-02-12T23:23:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:23:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-12T23:23:58Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:23:58Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:23:58Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:23:58Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:23:58Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:23:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:23:58Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:23:58Z- Filter 0 No beams used in subcloud update 2021-02-12T23:23:58Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:23:58Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:23:58Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 769.8 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:23:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172237.51 sec. 2021-02-12T23:23:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:23:58Z- TNavBF::mmse selected from filter -1 2021-02-12T23:23:58Z- variances([0], [2], and psi_berg) = 769.768372 833.054087 0.000000 2021-02-12T23:23:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:23:58Z- LcmTrn::publishEstimates() - MLE : 4056866.62 590942.83 17.91 2021-02-12T23:23:58Z- LcmTrn::publishEstimates() - MMSE : 4056887.12 590893.33 17.91 2021-02-12T23:23:58Z- LcmTrn::publishEstimates() - COVAR: 769.77 833.05 0.77 0.00 2021-02-12T23:23:58Z- AHRS_M2 msg: 1613172238.29 epoch sec; seqNo:3370 2021-02-12T23:23:58Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.14 psi 2021-02-12T23:23:58Z- handleDepth()-Depth_Keller msg: 1613172238301 epoch sec; seqNo:3402; depth 17.81 2021-02-12T23:23:58Z- handleDvl() - RDI_Pathfinder msg: 1613172238.31 epoch sec; seqNo:1554 2021-02-12T23:23:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.59 , 90.95 , 103.11 , 69.06 2021-02-12T23:23:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.08 2021-02-12T23:23:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172238.32 epoch sec; seqNo:3402; 4056887.12 north; 590893.33 east 2021-02-12T23:23:58Z- AHRS_M2 msg: 1613172238.69 epoch sec; seqNo:3371 2021-02-12T23:23:58Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.14 psi 2021-02-12T23:23:58Z- handleDepth()-Depth_Keller msg: 1613172238702 epoch sec; seqNo:3403; depth 17.70 2021-02-12T23:23:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172238.71 epoch sec; seqNo:3403; 4056887.51 north; 590894.55 east 2021-02-12T23:23:59Z- AHRS_M2 msg: 1613172239.10 epoch sec; seqNo:3372 2021-02-12T23:23:59Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.14 psi 2021-02-12T23:23:59Z- handleDepth()-Depth_Keller msg: 1613172239105 epoch sec; seqNo:3404; depth 17.60 2021-02-12T23:23:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172239.13 epoch sec; seqNo:3404; 4056887.89 north; 590894.54 east 2021-02-12T23:23:59Z- AHRS_M2 msg: 1613172239.50 epoch sec; seqNo:3373 2021-02-12T23:23:59Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.14 psi 2021-02-12T23:23:59Z- handleDepth()-Depth_Keller msg: 1613172239509 epoch sec; seqNo:3405; depth 17.49 2021-02-12T23:23:59Z- handleDvl() - RDI_Pathfinder msg: 1613172239.52 epoch sec; seqNo:1555 2021-02-12T23:23:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.41 , 93.38 , 101.90 , 69.46 2021-02-12T23:23:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , 0.05 2021-02-12T23:23:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172239.55 epoch sec; seqNo:3405; 4056888.27 north; 590894.54 east 2021-02-12T23:23:59Z- AHRS_M2 msg: 1613172239.90 epoch sec; seqNo:3374 2021-02-12T23:23:59Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-12T23:23:59Z- handleDepth()-Depth_Keller msg: 1613172239909 epoch sec; seqNo:3406; depth 17.37 2021-02-12T23:23:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172239.92 epoch sec; seqNo:3406; 4056888.27 north; 590894.54 east 2021-02-12T23:24:00Z- AHRS_M2 msg: 1613172240.31 epoch sec; seqNo:3375 2021-02-12T23:24:00Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-12T23:24:00Z- handleDepth()-Depth_Keller msg: 1613172240312 epoch sec; seqNo:3407; depth 17.26 2021-02-12T23:24:00Z- handleDvl() - RDI_Pathfinder msg: 1613172240.33 epoch sec; seqNo:1556 2021-02-12T23:24:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.41 , 92.10 , 105.48 , 69.46 2021-02-12T23:24:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.09 , 0.03 2021-02-12T23:24:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172240.34 epoch sec; seqNo:3407; 4056888.65 north; 590894.54 east 2021-02-12T23:24:00Z- AHRS_M2 msg: 1613172240.71 epoch sec; seqNo:3376 2021-02-12T23:24:00Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-12T23:24:00Z- handleDepth()-Depth_Keller msg: 1613172240717 epoch sec; seqNo:3408; depth 17.16 2021-02-12T23:24:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:00Z- handleDvl() - RDI_Pathfinder msg: 1613172240.72 epoch sec; seqNo:1557 2021-02-12T23:24:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.09 , 0.03 2021-02-12T23:24:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172240.73 epoch sec; seqNo:3408; 4056889.04 north; 590895.75 east 2021-02-12T23:24:01Z- AHRS_M2 msg: 1613172241.12 epoch sec; seqNo:3377 2021-02-12T23:24:01Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-12T23:24:01Z- handleDepth()-Depth_Keller msg: 1613172241125 epoch sec; seqNo:3409; depth 17.05 2021-02-12T23:24:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172241.14 epoch sec; seqNo:3409; 4056889.04 north; 590895.75 east 2021-02-12T23:24:01Z- AHRS_M2 msg: 1613172241.52 epoch sec; seqNo:3378 2021-02-12T23:24:01Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-12T23:24:01Z- handleDepth()-Depth_Keller msg: 1613172241524 epoch sec; seqNo:3410; depth 16.94 2021-02-12T23:24:01Z- handleDvl() - RDI_Pathfinder msg: 1613172241.54 epoch sec; seqNo:1558 2021-02-12T23:24:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.82 , 92.03 , 105.41 , 69.87 2021-02-12T23:24:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.11 , 0.00 2021-02-12T23:24:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172241.55 epoch sec; seqNo:3410; 4056889.42 north; 590895.75 east 2021-02-12T23:24:01Z- AHRS_M2 msg: 1613172241.92 epoch sec; seqNo:3379 2021-02-12T23:24:01Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:24:01Z- handleDepth()-Depth_Keller msg: 1613172241933 epoch sec; seqNo:3411; depth 16.84 2021-02-12T23:24:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:01Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:01Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:01Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:01Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:01Z- handleDvl() - RDI_Pathfinder msg: 1613172241.93 epoch sec; seqNo:1559 2021-02-12T23:24:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.11 , 0.00 2021-02-12T23:24:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172241.95 epoch sec; seqNo:3411; 4056889.42 north; 590895.75 east 2021-02-12T23:24:02Z- AHRS_M2 msg: 1613172242.33 epoch sec; seqNo:3380 2021-02-12T23:24:02Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:24:02Z- handleDepth()-Depth_Keller msg: 1613172242337 epoch sec; seqNo:3412; depth 16.70 2021-02-12T23:24:02Z- handleDvl() - RDI_Pathfinder msg: 1613172242.35 epoch sec; seqNo:1560 2021-02-12T23:24:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.82 , 94.46 , 105.41 , 70.28 2021-02-12T23:24:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.07 , -0.04 2021-02-12T23:24:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172242.36 epoch sec; seqNo:3412; 4056889.79 north; 590895.75 east 2021-02-12T23:24:02Z- AHRS_M2 msg: 1613172242.73 epoch sec; seqNo:3381 2021-02-12T23:24:02Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:24:02Z- handleDepth()-Depth_Keller msg: 1613172242816 epoch sec; seqNo:3413; depth 16.61 2021-02-12T23:24:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:02Z- handleDvl() - RDI_Pathfinder msg: 1613172242.85 epoch sec; seqNo:1561 2021-02-12T23:24:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.07 , -0.04 2021-02-12T23:24:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172242.94 epoch sec; seqNo:3413; 4056890.19 north; 590896.96 east 2021-02-12T23:24:03Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-12T23:24:03Z- waiting for 1613172240.93, time is 1613172242.94 2021-02-12T23:24:03Z- LcmTrn::updateTrn() equating pose 1613172242.94 and meas times 1613172242.94 2021-02-12T23:24:03Z- LcmTrn::updateTrn() >>>> pose time 1613172242.94, meas time 1613172242.94 <<<< 2021-02-12T23:24:03Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172242.94, ping # 0. 2021-02-12T23:24:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172242.94 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:24:03Z- TerrainNav::motionUpdate() - currEstimate = (4056890, 590896.96, 16.61) 2021-02-12T23:24:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:24:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:24:03Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:24:03Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:24:03Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172242.94 sec.; No relevant navigation data available 2021-02-12T23:24:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:24:03Z- TNavBF::mmse selected from filter -1 2021-02-12T23:24:03Z- variances([0], [2], and psi_berg) = 769.627591 833.053915 0.000000 2021-02-12T23:24:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:24:03Z- LcmTrn::publishEstimates() - MLE : 4056869.69 590946.46 16.61 2021-02-12T23:24:03Z- LcmTrn::publishEstimates() - MMSE : 4056890.19 590896.96 16.61 2021-02-12T23:24:03Z- LcmTrn::publishEstimates() - COVAR: 769.63 833.05 1.01 0.00 2021-02-12T23:24:03Z- AHRS_M2 msg: 1613172243.14 epoch sec; seqNo:3382 2021-02-12T23:24:03Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.18 psi 2021-02-12T23:24:03Z- handleDepth()-Depth_Keller msg: 1613172243147 epoch sec; seqNo:3414; depth 16.45 2021-02-12T23:24:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172243.16 epoch sec; seqNo:3414; 4056890.19 north; 590896.96 east 2021-02-12T23:24:03Z- AHRS_M2 msg: 1613172243.55 epoch sec; seqNo:3383 2021-02-12T23:24:03Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-12T23:24:03Z- handleDepth()-Depth_Keller msg: 1613172243556 epoch sec; seqNo:3415; depth 16.39 2021-02-12T23:24:03Z- handleDvl() - RDI_Pathfinder msg: 1613172243.57 epoch sec; seqNo:1562 2021-02-12T23:24:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.00 , 98.05 , 106.56 , 70.28 2021-02-12T23:24:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.04 2021-02-12T23:24:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172243.58 epoch sec; seqNo:3415; 4056890.57 north; 590896.96 east 2021-02-12T23:24:03Z- AHRS_M2 msg: 1613172243.95 epoch sec; seqNo:3384 2021-02-12T23:24:03Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.20 psi 2021-02-12T23:24:03Z- handleDepth()-Depth_Keller msg: 1613172243958 epoch sec; seqNo:3416; depth 16.26 2021-02-12T23:24:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:03Z- handleDvl() - RDI_Pathfinder msg: 1613172243.96 epoch sec; seqNo:1563 2021-02-12T23:24:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.04 2021-02-12T23:24:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172243.97 epoch sec; seqNo:3416; 4056890.94 north; 590896.95 east 2021-02-12T23:24:04Z- AHRS_M2 msg: 1613172244.35 epoch sec; seqNo:3385 2021-02-12T23:24:04Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.20 psi 2021-02-12T23:24:04Z- handleDepth()-Depth_Keller msg: 1613172244357 epoch sec; seqNo:3417; depth 16.15 2021-02-12T23:24:04Z- handleDvl() - RDI_Pathfinder msg: 1613172244.38 epoch sec; seqNo:1564 2021-02-12T23:24:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.00 , 96.76 , 105.28 , 71.10 2021-02-12T23:24:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.08 , 0.02 2021-02-12T23:24:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172244.39 epoch sec; seqNo:3417; 4056891.32 north; 590896.95 east 2021-02-12T23:24:04Z- AHRS_M2 msg: 1613172244.76 epoch sec; seqNo:3386 2021-02-12T23:24:04Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.21 psi 2021-02-12T23:24:04Z- handleDepth()-Depth_Keller msg: 1613172244766 epoch sec; seqNo:3418; depth 16.05 2021-02-12T23:24:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172244.78 epoch sec; seqNo:3418; 4056891.32 north; 590896.95 east 2021-02-12T23:24:05Z- AHRS_M2 msg: 1613172245.16 epoch sec; seqNo:3387 2021-02-12T23:24:05Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.22 psi 2021-02-12T23:24:05Z- handleDepth()-Depth_Keller msg: 1613172245162 epoch sec; seqNo:3419; depth 15.94 2021-02-12T23:24:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:05Z- handleDvl() - RDI_Pathfinder msg: 1613172245.16 epoch sec; seqNo:1565 2021-02-12T23:24:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.08 , 0.02 2021-02-12T23:24:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172245.17 epoch sec; seqNo:3419; 4056891.70 north; 590896.95 east 2021-02-12T23:24:05Z- AHRS_M2 msg: 1613172245.56 epoch sec; seqNo:3388 2021-02-12T23:24:05Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.22 psi 2021-02-12T23:24:05Z- handleDepth()-Depth_Keller msg: 1613172245570 epoch sec; seqNo:3420; depth 15.83 2021-02-12T23:24:05Z- handleDvl() - RDI_Pathfinder msg: 1613172245.58 epoch sec; seqNo:1566 2021-02-12T23:24:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.00 , 96.76 , 108.93 , 71.10 2021-02-12T23:24:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.06 , 0.06 2021-02-12T23:24:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172245.59 epoch sec; seqNo:3420; 4056892.08 north; 590896.94 east 2021-02-12T23:24:05Z- AHRS_M2 msg: 1613172245.96 epoch sec; seqNo:3389 2021-02-12T23:24:05Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.23 psi 2021-02-12T23:24:05Z- handleDepth()-Depth_Keller msg: 1613172245973 epoch sec; seqNo:3421; depth 15.71 2021-02-12T23:24:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172245.98 epoch sec; seqNo:3421; 4056892.47 north; 590898.16 east 2021-02-12T23:24:06Z- AHRS_M2 msg: 1613172246.37 epoch sec; seqNo:3390 2021-02-12T23:24:06Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.23 psi 2021-02-12T23:24:06Z- handleDepth()-Depth_Keller msg: 1613172246372 epoch sec; seqNo:3422; depth 15.59 2021-02-12T23:24:06Z- handleDvl() - RDI_Pathfinder msg: 1613172246.38 epoch sec; seqNo:1567 2021-02-12T23:24:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.82 , 96.69 , 108.86 , 70.69 2021-02-12T23:24:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.03 2021-02-12T23:24:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172246.39 epoch sec; seqNo:3422; 4056892.47 north; 590898.16 east 2021-02-12T23:24:06Z- AHRS_M2 msg: 1613172246.77 epoch sec; seqNo:3391 2021-02-12T23:24:06Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.22 psi 2021-02-12T23:24:06Z- handleDepth()-Depth_Keller msg: 1613172246777 epoch sec; seqNo:3423; depth 15.48 2021-02-12T23:24:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172246.79 epoch sec; seqNo:3423; 4056892.85 north; 590898.16 east 2021-02-12T23:24:07Z- AHRS_M2 msg: 1613172247.18 epoch sec; seqNo:3392 2021-02-12T23:24:07Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.21 psi 2021-02-12T23:24:07Z- handleDepth()-Depth_Keller msg: 1613172247193 epoch sec; seqNo:3424; depth 15.35 2021-02-12T23:24:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172247.20 epoch sec; seqNo:3424; 4056893.23 north; 590898.15 east 2021-02-12T23:24:07Z- AHRS_M2 msg: 1613172247.58 epoch sec; seqNo:3393 2021-02-12T23:24:07Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.20 psi 2021-02-12T23:24:07Z- handleDepth()-Depth_Keller msg: 1613172247589 epoch sec; seqNo:3425; depth 15.24 2021-02-12T23:24:07Z- handleDvl() - RDI_Pathfinder msg: 1613172247.61 epoch sec; seqNo:1568 2021-02-12T23:24:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.41 , 96.69 , 108.86 , 71.92 2021-02-12T23:24:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , -0.01 2021-02-12T23:24:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172247.62 epoch sec; seqNo:3425; 4056893.61 north; 590898.15 east 2021-02-12T23:24:07Z- AHRS_M2 msg: 1613172247.98 epoch sec; seqNo:3394 2021-02-12T23:24:07Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.19 psi 2021-02-12T23:24:07Z- handleDepth()-Depth_Keller msg: 1613172247994 epoch sec; seqNo:3426; depth 15.13 2021-02-12T23:24:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172248.01 epoch sec; seqNo:3426; 4056893.61 north; 590898.15 east 2021-02-12T23:24:08Z- LcmTrn::handleMessages() - handling 46 messages took 0.71 ms and 132 calls... 2021-02-12T23:24:08Z- waiting for 1613172245.94, time is 1613172248.01 2021-02-12T23:24:08Z- LcmTrn::updateTrn() >>>> pose time 1613172248.01, meas time 1613172247.61 <<<< 2021-02-12T23:24:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172247.61 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:24:08Z- TerrainNav::motionUpdate() - currEstimate = (4056894, 590898.15, 15.13) 2021-02-12T23:24:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:24:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:24:08Z- TNavBF::Projecting Measurements 2021-02-12T23:24:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:24:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:24:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:24:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:24:08Z- TNavFilter::4 good beams. 2021-02-12T23:24:08Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:24:08Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:24:08Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:24:08Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:24:08Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:24:08Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:24:08Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:24:08Z- TNavBF::mmse selected from filter -1 2021-02-12T23:24:08Z- TerrainMapOctree: (vehN, vehE ) = (4056893.34, 590898.05). 2021-02-12T23:24:08Z- TerrainMapOctree: Min Distance = 5496.91. 2021-02-12T23:24:08Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:24:08Z- There is no specific good beam that all particles have in common. 2021-02-12T23:24:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:24:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 9.34 2021-02-12T23:24:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:24:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 14.96 2021-02-12T23:24:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:24:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 18.96 2021-02-12T23:24:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:24:08Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 8.27 2021-02-12T23:24:08Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:24:08Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:24:08Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:24:08Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:24:08Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:24:08Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:24:08Z- Filter 0 No beams used in subcloud update 2021-02-12T23:24:08Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:24:08Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:24:08Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 769.5 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:24:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172247.61 sec. 2021-02-12T23:24:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:24:08Z- TNavBF::mmse selected from filter -1 2021-02-12T23:24:08Z- variances([0], [2], and psi_berg) = 769.475528 833.053730 0.000000 2021-02-12T23:24:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:24:08Z- LcmTrn::publishEstimates() - MLE : 4056873.11 590947.65 15.13 2021-02-12T23:24:08Z- LcmTrn::publishEstimates() - MMSE : 4056893.61 590898.15 15.13 2021-02-12T23:24:08Z- LcmTrn::publishEstimates() - COVAR: 769.48 833.05 0.89 0.00 2021-02-12T23:24:08Z- AHRS_M2 msg: 1613172248.39 epoch sec; seqNo:3395 2021-02-12T23:24:08Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-12T23:24:08Z- handleDepth()-Depth_Keller msg: 1613172248397 epoch sec; seqNo:3427; depth 15.01 2021-02-12T23:24:08Z- handleDvl() - RDI_Pathfinder msg: 1613172248.41 epoch sec; seqNo:1569 2021-02-12T23:24:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.64 , 96.69 , 110.08 , 70.69 2021-02-12T23:24:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , -0.03 2021-02-12T23:24:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172248.43 epoch sec; seqNo:3427; 4056893.99 north; 590898.14 east 2021-02-12T23:24:08Z- AHRS_M2 msg: 1613172248.79 epoch sec; seqNo:3396 2021-02-12T23:24:08Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.17 psi 2021-02-12T23:24:08Z- handleDepth()-Depth_Keller msg: 1613172248798 epoch sec; seqNo:3428; depth 14.89 2021-02-12T23:24:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:08Z- handleDvl() - RDI_Pathfinder msg: 1613172248.80 epoch sec; seqNo:1570 2021-02-12T23:24:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , -0.03 2021-02-12T23:24:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172248.81 epoch sec; seqNo:3428; 4056894.38 north; 590899.36 east 2021-02-12T23:24:09Z- AHRS_M2 msg: 1613172249.20 epoch sec; seqNo:3397 2021-02-12T23:24:09Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.16 psi 2021-02-12T23:24:09Z- handleDepth()-Depth_Keller msg: 1613172249204 epoch sec; seqNo:3429; depth 14.79 2021-02-12T23:24:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172249.24 epoch sec; seqNo:3429; 4056894.76 north; 590899.36 east 2021-02-12T23:24:09Z- AHRS_M2 msg: 1613172249.60 epoch sec; seqNo:3398 2021-02-12T23:24:09Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-12T23:24:09Z- handleDepth()-Depth_Keller msg: 1613172249612 epoch sec; seqNo:3430; depth 14.66 2021-02-12T23:24:09Z- handleDvl() - RDI_Pathfinder msg: 1613172249.64 epoch sec; seqNo:1571 2021-02-12T23:24:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 96.63 , 110.01 , 71.51 2021-02-12T23:24:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.12 , -0.03 2021-02-12T23:24:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172249.65 epoch sec; seqNo:3430; 4056894.76 north; 590899.36 east 2021-02-12T23:24:10Z- AHRS_M2 msg: 1613172250.00 epoch sec; seqNo:3399 2021-02-12T23:24:10Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-12T23:24:10Z- handleDepth()-Depth_Keller msg: 1613172250010 epoch sec; seqNo:3431; depth 14.55 2021-02-12T23:24:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:10Z- handleDvl() - RDI_Pathfinder msg: 1613172250.01 epoch sec; seqNo:1572 2021-02-12T23:24:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.12 , -0.03 2021-02-12T23:24:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172250.02 epoch sec; seqNo:3431; 4056895.14 north; 590899.35 east 2021-02-12T23:24:10Z- AHRS_M2 msg: 1613172250.41 epoch sec; seqNo:3400 2021-02-12T23:24:10Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-12T23:24:10Z- handleDepth()-Depth_Keller msg: 1613172250413 epoch sec; seqNo:3432; depth 14.44 2021-02-12T23:24:10Z- handleDvl() - RDI_Pathfinder msg: 1613172250.43 epoch sec; seqNo:1573 2021-02-12T23:24:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.86 , 96.63 , 108.79 , 72.32 2021-02-12T23:24:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.13 , -0.05 2021-02-12T23:24:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172250.44 epoch sec; seqNo:3432; 4056895.52 north; 590899.35 east 2021-02-12T23:24:10Z- AHRS_M2 msg: 1613172250.81 epoch sec; seqNo:3401 2021-02-12T23:24:10Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-12T23:24:10Z- handleDepth()-Depth_Keller msg: 1613172250816 epoch sec; seqNo:3433; depth 14.33 2021-02-12T23:24:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172250.83 epoch sec; seqNo:3433; 4056895.90 north; 590899.34 east 2021-02-12T23:24:11Z- AHRS_M2 msg: 1613172251.23 epoch sec; seqNo:3402 2021-02-12T23:24:11Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-12T23:24:11Z- handleDepth()-Depth_Keller msg: 1613172251241 epoch sec; seqNo:3434; depth 14.23 2021-02-12T23:24:11Z- handleDvl() - RDI_Pathfinder msg: 1613172251.27 epoch sec; seqNo:1574 2021-02-12T23:24:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.86 , 96.63 , 108.79 , 70.28 2021-02-12T23:24:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.16 , -0.05 2021-02-12T23:24:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172251.28 epoch sec; seqNo:3434; 4056896.27 north; 590899.34 east 2021-02-12T23:24:11Z- AHRS_M2 msg: 1613172251.62 epoch sec; seqNo:3403 2021-02-12T23:24:11Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-12T23:24:11Z- handleDepth()-Depth_Keller msg: 1613172251634 epoch sec; seqNo:3435; depth 14.11 2021-02-12T23:24:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172251.65 epoch sec; seqNo:3435; 4056896.27 north; 590899.34 east 2021-02-12T23:24:12Z- AHRS_M2 msg: 1613172252.02 epoch sec; seqNo:3404 2021-02-12T23:24:12Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.16 psi 2021-02-12T23:24:12Z- handleDepth()-Depth_Keller msg: 1613172252028 epoch sec; seqNo:3436; depth 13.99 2021-02-12T23:24:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172252.05 epoch sec; seqNo:3436; 4056896.65 north; 590899.34 east 2021-02-12T23:24:12Z- AHRS_M2 msg: 1613172252.43 epoch sec; seqNo:3405 2021-02-12T23:24:12Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-12T23:24:12Z- handleDepth()-Depth_Keller msg: 1613172252432 epoch sec; seqNo:3437; depth 13.88 2021-02-12T23:24:12Z- handleDvl() - RDI_Pathfinder msg: 1613172252.45 epoch sec; seqNo:1575 2021-02-12T23:24:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 96.62 , 110.00 , 72.32 2021-02-12T23:24:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.14 , -0.00 2021-02-12T23:24:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172252.46 epoch sec; seqNo:3437; 4056897.04 north; 590900.55 east 2021-02-12T23:24:12Z- AHRS_M2 msg: 1613172252.83 epoch sec; seqNo:3406 2021-02-12T23:24:12Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-12T23:24:12Z- handleDepth()-Depth_Keller msg: 1613172252836 epoch sec; seqNo:3438; depth 13.76 2021-02-12T23:24:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172252.85 epoch sec; seqNo:3438; 4056897.04 north; 590900.55 east 2021-02-12T23:24:13Z- LcmTrn::handleMessages() - handling 43 messages took 0.60 ms and 131 calls... 2021-02-12T23:24:13Z- waiting for 1613172251.01, time is 1613172252.85 2021-02-12T23:24:13Z- LcmTrn::updateTrn() >>>> pose time 1613172252.85, meas time 1613172252.45 <<<< 2021-02-12T23:24:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172252.45 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:24:13Z- TerrainNav::motionUpdate() - currEstimate = (4056897, 590900.55, 13.76) 2021-02-12T23:24:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:24:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:24:13Z- TNavBF::Projecting Measurements 2021-02-12T23:24:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:24:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:24:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:24:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:24:13Z- TNavFilter::4 good beams. 2021-02-12T23:24:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:24:13Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:24:13Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:24:13Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:24:13Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:24:13Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:24:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:24:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:24:13Z- TerrainMapOctree: (vehN, vehE ) = (4056896.76, 590900.36). 2021-02-12T23:24:13Z- TerrainMapOctree: Min Distance = 5492.79. 2021-02-12T23:24:13Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:24:13Z- There is no specific good beam that all particles have in common. 2021-02-12T23:24:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:24:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 9.95 2021-02-12T23:24:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:24:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 14.94 2021-02-12T23:24:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:24:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 19.36 2021-02-12T23:24:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:24:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-12T23:24:13Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:24:13Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:24:13Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:24:13Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:24:13Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:24:13Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:24:13Z- Filter 0 No beams used in subcloud update 2021-02-12T23:24:13Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:24:13Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:24:13Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 769.3 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:24:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172252.45 sec. 2021-02-12T23:24:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:24:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:24:13Z- variances([0], [2], and psi_berg) = 769.311789 833.053531 0.000000 2021-02-12T23:24:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:24:13Z- LcmTrn::publishEstimates() - MLE : 4056876.54 590950.05 13.76 2021-02-12T23:24:13Z- LcmTrn::publishEstimates() - MMSE : 4056897.04 590900.55 13.76 2021-02-12T23:24:13Z- LcmTrn::publishEstimates() - COVAR: 769.31 833.05 0.84 0.00 2021-02-12T23:24:13Z- AHRS_M2 msg: 1613172253.25 epoch sec; seqNo:3407 2021-02-12T23:24:13Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.17 psi 2021-02-12T23:24:13Z- handleDepth()-Depth_Keller msg: 1613172253258 epoch sec; seqNo:3439; depth 13.66 2021-02-12T23:24:13Z- handleDvl() - RDI_Pathfinder msg: 1613172253.27 epoch sec; seqNo:1576 2021-02-12T23:24:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 96.55 , 111.15 , 72.72 2021-02-12T23:24:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.07 , 0.09 2021-02-12T23:24:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172253.29 epoch sec; seqNo:3439; 4056897.42 north; 590900.55 east 2021-02-12T23:24:13Z- AHRS_M2 msg: 1613172253.64 epoch sec; seqNo:3408 2021-02-12T23:24:13Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-12T23:24:13Z- handleDepth()-Depth_Keller msg: 1613172253645 epoch sec; seqNo:3440; depth 13.55 2021-02-12T23:24:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:13Z- handleDvl() - RDI_Pathfinder msg: 1613172253.65 epoch sec; seqNo:1577 2021-02-12T23:24:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.07 , 0.09 2021-02-12T23:24:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172253.66 epoch sec; seqNo:3440; 4056897.80 north; 590900.55 east 2021-02-12T23:24:14Z- AHRS_M2 msg: 1613172254.04 epoch sec; seqNo:3409 2021-02-12T23:24:14Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.16 psi 2021-02-12T23:24:14Z- handleDepth()-Depth_Keller msg: 1613172254048 epoch sec; seqNo:3441; depth 13.46 2021-02-12T23:24:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172254.06 epoch sec; seqNo:3441; 4056898.18 north; 590900.54 east 2021-02-12T23:24:14Z- AHRS_M2 msg: 1613172254.45 epoch sec; seqNo:3410 2021-02-12T23:24:14Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.15 psi 2021-02-12T23:24:14Z- handleDepth()-Depth_Keller msg: 1613172254452 epoch sec; seqNo:3442; depth 13.33 2021-02-12T23:24:14Z- handleDvl() - RDI_Pathfinder msg: 1613172254.46 epoch sec; seqNo:1578 2021-02-12T23:24:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.86 , 96.56 , 108.72 , 71.10 2021-02-12T23:24:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , 0.14 2021-02-12T23:24:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172254.47 epoch sec; seqNo:3442; 4056898.56 north; 590900.54 east 2021-02-12T23:24:14Z- AHRS_M2 msg: 1613172254.85 epoch sec; seqNo:3411 2021-02-12T23:24:14Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.14 psi 2021-02-12T23:24:14Z- handleDepth()-Depth_Keller msg: 1613172254858 epoch sec; seqNo:3443; depth 13.22 2021-02-12T23:24:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:14Z- handleDvl() - RDI_Pathfinder msg: 1613172254.86 epoch sec; seqNo:1579 2021-02-12T23:24:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , 0.14 2021-02-12T23:24:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172254.88 epoch sec; seqNo:3443; 4056898.56 north; 590900.54 east 2021-02-12T23:24:15Z- AHRS_M2 msg: 1613172255.26 epoch sec; seqNo:3412 2021-02-12T23:24:15Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.13 psi 2021-02-12T23:24:15Z- handleDepth()-Depth_Keller msg: 1613172255266 epoch sec; seqNo:3444; depth 13.11 2021-02-12T23:24:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172255.29 epoch sec; seqNo:3444; 4056898.95 north; 590901.75 east 2021-02-12T23:24:15Z- AHRS_M2 msg: 1613172255.66 epoch sec; seqNo:3413 2021-02-12T23:24:15Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-12T23:24:15Z- handleDepth()-Depth_Keller msg: 1613172255664 epoch sec; seqNo:3445; depth 13.01 2021-02-12T23:24:15Z- handleDvl() - RDI_Pathfinder msg: 1613172255.68 epoch sec; seqNo:1580 2021-02-12T23:24:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.24 , 96.63 , 111.23 , 72.32 2021-02-12T23:24:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.03 , 0.10 2021-02-12T23:24:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172255.69 epoch sec; seqNo:3445; 4056899.33 north; 590901.75 east 2021-02-12T23:24:16Z- AHRS_M2 msg: 1613172256.07 epoch sec; seqNo:3414 2021-02-12T23:24:16Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.14 psi 2021-02-12T23:24:16Z- handleDepth()-Depth_Keller msg: 1613172256071 epoch sec; seqNo:3446; depth 12.88 2021-02-12T23:24:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:16Z- handleDvl() - RDI_Pathfinder msg: 1613172256.07 epoch sec; seqNo:1581 2021-02-12T23:24:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.03 , 0.10 2021-02-12T23:24:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172256.08 epoch sec; seqNo:3446; 4056899.33 north; 590901.75 east 2021-02-12T23:24:16Z- AHRS_M2 msg: 1613172256.47 epoch sec; seqNo:3415 2021-02-12T23:24:16Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.14 psi 2021-02-12T23:24:16Z- handleDepth()-Depth_Keller msg: 1613172256482 epoch sec; seqNo:3447; depth 12.77 2021-02-12T23:24:16Z- handleDvl() - RDI_Pathfinder msg: 1613172256.49 epoch sec; seqNo:1582 2021-02-12T23:24:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 96.49 , 109.87 , 73.96 2021-02-12T23:24:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , 0.04 2021-02-12T23:24:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172256.51 epoch sec; seqNo:3447; 4056899.71 north; 590901.75 east 2021-02-12T23:24:16Z- AHRS_M2 msg: 1613172256.87 epoch sec; seqNo:3416 2021-02-12T23:24:16Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.14 psi 2021-02-12T23:24:16Z- handleDepth()-Depth_Keller msg: 1613172256876 epoch sec; seqNo:3448; depth 12.66 2021-02-12T23:24:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172256.89 epoch sec; seqNo:3448; 4056900.09 north; 590901.74 east 2021-02-12T23:24:17Z- AHRS_M2 msg: 1613172257.28 epoch sec; seqNo:3417 2021-02-12T23:24:17Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.14 psi 2021-02-12T23:24:17Z- handleDepth()-Depth_Keller msg: 1613172257292 epoch sec; seqNo:3449; depth 12.55 2021-02-12T23:24:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:17Z- handleDvl() - RDI_Pathfinder msg: 1613172257.30 epoch sec; seqNo:1583 2021-02-12T23:24:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , 0.04 2021-02-12T23:24:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172257.32 epoch sec; seqNo:3449; 4056900.09 north; 590901.74 east 2021-02-12T23:24:17Z- AHRS_M2 msg: 1613172257.68 epoch sec; seqNo:3418 2021-02-12T23:24:17Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.14 psi 2021-02-12T23:24:17Z- handleDepth()-Depth_Keller msg: 1613172257694 epoch sec; seqNo:3450; depth 12.43 2021-02-12T23:24:17Z- handleDvl() - RDI_Pathfinder msg: 1613172257.70 epoch sec; seqNo:1584 2021-02-12T23:24:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 96.49 , 111.09 , 72.32 2021-02-12T23:24:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , -0.01 2021-02-12T23:24:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172257.72 epoch sec; seqNo:3450; 4056900.47 north; 590901.74 east 2021-02-12T23:24:18Z- AHRS_M2 msg: 1613172258.08 epoch sec; seqNo:3419 2021-02-12T23:24:18Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.14 psi 2021-02-12T23:24:18Z- handleDepth()-Depth_Keller msg: 1613172258088 epoch sec; seqNo:3451; depth 12.30 2021-02-12T23:24:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172258.10 epoch sec; seqNo:3451; 4056900.86 north; 590902.96 east 2021-02-12T23:24:18Z- LcmTrn::handleMessages() - handling 48 messages took 0.71 ms and 134 calls... 2021-02-12T23:24:18Z- waiting for 1613172255.85, time is 1613172258.10 2021-02-12T23:24:18Z- LcmTrn::updateTrn() >>>> pose time 1613172258.10, meas time 1613172257.70 <<<< 2021-02-12T23:24:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172257.70 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:24:18Z- TerrainNav::motionUpdate() - currEstimate = (4056901, 590902.96, 12.30) 2021-02-12T23:24:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:24:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:24:18Z- TNavBF::Projecting Measurements 2021-02-12T23:24:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:24:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:24:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:24:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:24:18Z- TNavFilter::4 good beams. 2021-02-12T23:24:18Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:24:18Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:24:18Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:24:18Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:24:18Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:24:18Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:24:18Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:24:18Z- TNavBF::mmse selected from filter -1 2021-02-12T23:24:18Z- TerrainMapOctree: (vehN, vehE ) = (4056900.57, 590902.77). 2021-02-12T23:24:18Z- TerrainMapOctree: Min Distance = 5488.30. 2021-02-12T23:24:18Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:24:18Z- There is no specific good beam that all particles have in common. 2021-02-12T23:24:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:24:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 10.41 2021-02-12T23:24:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:24:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 14.90 2021-02-12T23:24:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:24:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 19.75 2021-02-12T23:24:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:24:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-12T23:24:18Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:24:18Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:24:18Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:24:18Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:24:18Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:24:18Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:24:18Z- Filter 0 No beams used in subcloud update 2021-02-12T23:24:18Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:24:18Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:24:18Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 769.2 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:24:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172257.70 sec. 2021-02-12T23:24:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:24:18Z- TNavBF::mmse selected from filter -1 2021-02-12T23:24:18Z- variances([0], [2], and psi_berg) = 769.143678 833.053327 0.000000 2021-02-12T23:24:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:24:18Z- LcmTrn::publishEstimates() - MLE : 4056880.36 590952.46 12.30 2021-02-12T23:24:18Z- LcmTrn::publishEstimates() - MMSE : 4056900.86 590902.96 12.30 2021-02-12T23:24:18Z- LcmTrn::publishEstimates() - COVAR: 769.14 833.05 0.83 0.00 2021-02-12T23:24:18Z- AHRS_M2 msg: 1613172258.49 epoch sec; seqNo:3420 2021-02-12T23:24:18Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.14 psi 2021-02-12T23:24:18Z- handleDepth()-Depth_Keller msg: 1613172258492 epoch sec; seqNo:3452; depth 12.20 2021-02-12T23:24:18Z- handleDvl() - RDI_Pathfinder msg: 1613172258.54 epoch sec; seqNo:1585 2021-02-12T23:24:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 96.49 , 109.87 , 73.96 2021-02-12T23:24:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , 0.00 2021-02-12T23:24:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172258.56 epoch sec; seqNo:3452; 4056901.24 north; 590902.95 east 2021-02-12T23:24:18Z- AHRS_M2 msg: 1613172258.90 epoch sec; seqNo:3421 2021-02-12T23:24:18Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-12T23:24:18Z- handleDepth()-Depth_Keller msg: 1613172258905 epoch sec; seqNo:3453; depth 12.07 2021-02-12T23:24:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172258.92 epoch sec; seqNo:3453; 4056901.24 north; 590902.95 east 2021-02-12T23:24:19Z- AHRS_M2 msg: 1613172259.31 epoch sec; seqNo:3422 2021-02-12T23:24:19Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-12T23:24:19Z- handleDepth()-Depth_Keller msg: 1613172259333 epoch sec; seqNo:3454; depth 11.97 2021-02-12T23:24:19Z- handleDvl() - RDI_Pathfinder msg: 1613172259.34 epoch sec; seqNo:1586 2021-02-12T23:24:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 96.49 , 111.09 , 73.96 2021-02-12T23:24:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.08 , 0.05 2021-02-12T23:24:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172259.36 epoch sec; seqNo:3454; 4056901.62 north; 590902.95 east 2021-02-12T23:24:19Z- AHRS_M2 msg: 1613172259.70 epoch sec; seqNo:3423 2021-02-12T23:24:19Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-12T23:24:19Z- handleDepth()-Depth_Keller msg: 1613172259704 epoch sec; seqNo:3455; depth 11.85 2021-02-12T23:24:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:19Z- handleDvl() - RDI_Pathfinder msg: 1613172259.71 epoch sec; seqNo:1587 2021-02-12T23:24:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.08 , 0.05 2021-02-12T23:24:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172259.72 epoch sec; seqNo:3455; 4056902.00 north; 590902.94 east 2021-02-12T23:24:20Z- AHRS_M2 msg: 1613172260.11 epoch sec; seqNo:3424 2021-02-12T23:24:20Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-12T23:24:20Z- handleDepth()-Depth_Keller msg: 1613172260119 epoch sec; seqNo:3456; depth 11.76 2021-02-12T23:24:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172260.13 epoch sec; seqNo:3456; 4056902.37 north; 590902.94 east 2021-02-12T23:24:20Z- AHRS_M2 msg: 1613172260.51 epoch sec; seqNo:3425 2021-02-12T23:24:20Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.14 psi 2021-02-12T23:24:20Z- handleDepth()-Depth_Keller msg: 1613172260522 epoch sec; seqNo:3457; depth 11.65 2021-02-12T23:24:20Z- handleDvl() - RDI_Pathfinder msg: 1613172260.54 epoch sec; seqNo:1588 2021-02-12T23:24:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.26 , 96.42 , 113.46 , 73.96 2021-02-12T23:24:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , 0.06 2021-02-12T23:24:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172260.55 epoch sec; seqNo:3457; 4056902.75 north; 590902.94 east 2021-02-12T23:24:20Z- AHRS_M2 msg: 1613172260.92 epoch sec; seqNo:3426 2021-02-12T23:24:20Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.13 psi 2021-02-12T23:24:20Z- handleDepth()-Depth_Keller msg: 1613172260922 epoch sec; seqNo:3458; depth 11.52 2021-02-12T23:24:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:20Z- handleDvl() - RDI_Pathfinder msg: 1613172260.92 epoch sec; seqNo:1589 2021-02-12T23:24:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , 0.06 2021-02-12T23:24:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172260.95 epoch sec; seqNo:3458; 4056902.77 north; 590904.16 east 2021-02-12T23:24:21Z- AHRS_M2 msg: 1613172261.32 epoch sec; seqNo:3427 2021-02-12T23:24:21Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.13 psi 2021-02-12T23:24:21Z- handleDepth()-Depth_Keller msg: 1613172261321 epoch sec; seqNo:3459; depth 11.44 2021-02-12T23:24:21Z- handleDvl() - RDI_Pathfinder msg: 1613172261.33 epoch sec; seqNo:1590 2021-02-12T23:24:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 96.36 , 109.87 , 73.14 2021-02-12T23:24:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , -0.05 2021-02-12T23:24:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172261.34 epoch sec; seqNo:3459; 4056903.14 north; 590904.15 east 2021-02-12T23:24:21Z- AHRS_M2 msg: 1613172261.72 epoch sec; seqNo:3428 2021-02-12T23:24:21Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.14 psi 2021-02-12T23:24:21Z- handleDepth()-Depth_Keller msg: 1613172261726 epoch sec; seqNo:3460; depth 11.30 2021-02-12T23:24:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172261.74 epoch sec; seqNo:3460; 4056903.52 north; 590904.15 east 2021-02-12T23:24:22Z- AHRS_M2 msg: 1613172262.13 epoch sec; seqNo:3429 2021-02-12T23:24:22Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.14 psi 2021-02-12T23:24:22Z- handleDepth()-Depth_Keller msg: 1613172262131 epoch sec; seqNo:3461; depth 11.21 2021-02-12T23:24:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:22Z- handleDvl() - RDI_Pathfinder msg: 1613172262.14 epoch sec; seqNo:1591 2021-02-12T23:24:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , -0.05 2021-02-12T23:24:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172262.15 epoch sec; seqNo:3461; 4056903.52 north; 590904.15 east 2021-02-12T23:24:22Z- AHRS_M2 msg: 1613172262.53 epoch sec; seqNo:3430 2021-02-12T23:24:22Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.15 psi 2021-02-12T23:24:22Z- handleDepth()-Depth_Keller msg: 1613172262537 epoch sec; seqNo:3462; depth 11.07 2021-02-12T23:24:22Z- handleDvl() - RDI_Pathfinder msg: 1613172262.55 epoch sec; seqNo:1592 2021-02-12T23:24:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.54 , 96.49 , 113.52 , 73.55 2021-02-12T23:24:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.04 , -0.17 2021-02-12T23:24:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172262.57 epoch sec; seqNo:3462; 4056903.90 north; 590904.14 east 2021-02-12T23:24:22Z- AHRS_M2 msg: 1613172262.94 epoch sec; seqNo:3431 2021-02-12T23:24:22Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.16 psi 2021-02-12T23:24:22Z- handleDepth()-Depth_Keller msg: 1613172262944 epoch sec; seqNo:3463; depth 10.96 2021-02-12T23:24:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172262.95 epoch sec; seqNo:3463; 4056904.28 north; 590904.14 east 2021-02-12T23:24:23Z- AHRS_M2 msg: 1613172263.34 epoch sec; seqNo:3432 2021-02-12T23:24:23Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.16 psi 2021-02-12T23:24:23Z- handleDepth()-Depth_Keller msg: 1613172263349 epoch sec; seqNo:3464; depth 10.84 2021-02-12T23:24:23Z- handleDvl() - RDI_Pathfinder msg: 1613172263.36 epoch sec; seqNo:1593 2021-02-12T23:24:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 96.36 , 115.28 , 74.78 2021-02-12T23:24:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.17 , -0.15 2021-02-12T23:24:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172263.37 epoch sec; seqNo:3464; 4056904.28 north; 590904.14 east 2021-02-12T23:24:23Z- LcmTrn::handleMessages() - handling 48 messages took 0.68 ms and 135 calls... 2021-02-12T23:24:23Z- waiting for 1613172261.10, time is 1613172263.37 2021-02-12T23:24:23Z- LcmTrn::updateTrn() equating pose 1613172263.37 and meas times 1613172263.37 2021-02-12T23:24:23Z- LcmTrn::updateTrn() >>>> pose time 1613172263.37, meas time 1613172263.37 <<<< 2021-02-12T23:24:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172263.37 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:24:23Z- TerrainNav::motionUpdate() - currEstimate = (4056904, 590904.14, 10.84) 2021-02-12T23:24:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:24:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:24:23Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172263.37 sec.; No relevant navigation data available 2021-02-12T23:24:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:24:23Z- TNavBF::mmse selected from filter -1 2021-02-12T23:24:23Z- variances([0], [2], and psi_berg) = 769.021988 833.053180 0.000000 2021-02-12T23:24:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:24:23Z- LcmTrn::publishEstimates() - MLE : 4056883.78 590953.64 10.84 2021-02-12T23:24:23Z- LcmTrn::publishEstimates() - MMSE : 4056904.28 590904.14 10.84 2021-02-12T23:24:23Z- LcmTrn::publishEstimates() - COVAR: 769.02 833.05 1.01 0.00 2021-02-12T23:24:23Z- AHRS_M2 msg: 1613172263.74 epoch sec; seqNo:3433 2021-02-12T23:24:23Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.16 psi 2021-02-12T23:24:23Z- handleDepth()-Depth_Keller msg: 1613172263744 epoch sec; seqNo:3465; depth 10.74 2021-02-12T23:24:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172263.77 epoch sec; seqNo:3465; 4056904.66 north; 590904.14 east 2021-02-12T23:24:24Z- AHRS_M2 msg: 1613172264.15 epoch sec; seqNo:3434 2021-02-12T23:24:24Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.16 psi 2021-02-12T23:24:24Z- handleDepth()-Depth_Keller msg: 1613172264151 epoch sec; seqNo:3466; depth 10.61 2021-02-12T23:24:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172264.16 epoch sec; seqNo:3466; 4056905.05 north; 590905.35 east 2021-02-12T23:24:24Z- AHRS_M2 msg: 1613172264.55 epoch sec; seqNo:3435 2021-02-12T23:24:24Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.16 psi 2021-02-12T23:24:24Z- handleDepth()-Depth_Keller msg: 1613172264552 epoch sec; seqNo:3467; depth 10.49 2021-02-12T23:24:24Z- handleDvl() - RDI_Pathfinder msg: 1613172264.56 epoch sec; seqNo:1594 2021-02-12T23:24:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.83 , 96.35 , 111.22 , 74.36 2021-02-12T23:24:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.23 , 0.03 2021-02-12T23:24:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172264.57 epoch sec; seqNo:3467; 4056905.05 north; 590905.35 east 2021-02-12T23:24:24Z- AHRS_M2 msg: 1613172264.95 epoch sec; seqNo:3436 2021-02-12T23:24:24Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-12T23:24:24Z- handleDepth()-Depth_Keller msg: 1613172264962 epoch sec; seqNo:3468; depth 10.39 2021-02-12T23:24:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172264.97 epoch sec; seqNo:3468; 4056905.43 north; 590905.35 east 2021-02-12T23:24:25Z- AHRS_M2 msg: 1613172265.36 epoch sec; seqNo:3437 2021-02-12T23:24:25Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:24:25Z- handleDepth()-Depth_Keller msg: 1613172265360 epoch sec; seqNo:3469; depth 10.26 2021-02-12T23:24:25Z- handleDvl() - RDI_Pathfinder msg: 1613172265.38 epoch sec; seqNo:1595 2021-02-12T23:24:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 96.42 , 112.23 , 74.77 2021-02-12T23:24:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.17 , 0.18 2021-02-12T23:24:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172265.39 epoch sec; seqNo:3469; 4056905.81 north; 590905.34 east 2021-02-12T23:24:25Z- AHRS_M2 msg: 1613172265.76 epoch sec; seqNo:3438 2021-02-12T23:24:25Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-12T23:24:25Z- handleDepth()-Depth_Keller msg: 1613172265781 epoch sec; seqNo:3470; depth 10.15 2021-02-12T23:24:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:25Z- handleDvl() - RDI_Pathfinder msg: 1613172265.78 epoch sec; seqNo:1596 2021-02-12T23:24:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.17 , 0.18 2021-02-12T23:24:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172265.81 epoch sec; seqNo:3470; 4056906.19 north; 590905.34 east 2021-02-12T23:24:26Z- AHRS_M2 msg: 1613172266.17 epoch sec; seqNo:3439 2021-02-12T23:24:26Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-12T23:24:26Z- handleDepth()-Depth_Keller msg: 1613172266174 epoch sec; seqNo:3471; depth 10.04 2021-02-12T23:24:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172266.19 epoch sec; seqNo:3471; 4056906.57 north; 590905.34 east 2021-02-12T23:24:26Z- AHRS_M2 msg: 1613172266.57 epoch sec; seqNo:3440 2021-02-12T23:24:26Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-12T23:24:26Z- handleDepth()-Depth_Keller msg: 1613172266572 epoch sec; seqNo:3472; depth 9.94 2021-02-12T23:24:26Z- handleDvl() - RDI_Pathfinder msg: 1613172266.59 epoch sec; seqNo:1597 2021-02-12T23:24:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 96.36 , 111.23 , 76.00 2021-02-12T23:24:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , 0.12 2021-02-12T23:24:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172266.60 epoch sec; seqNo:3472; 4056906.57 north; 590905.34 east 2021-02-12T23:24:26Z- AHRS_M2 msg: 1613172266.97 epoch sec; seqNo:3441 2021-02-12T23:24:26Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.14 psi 2021-02-12T23:24:26Z- handleDepth()-Depth_Keller msg: 1613172266977 epoch sec; seqNo:3473; depth 9.82 2021-02-12T23:24:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:26Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:26Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:26Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:26Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:26Z- handleDvl() - RDI_Pathfinder msg: 1613172266.98 epoch sec; seqNo:1598 2021-02-12T23:24:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , 0.12 2021-02-12T23:24:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172266.99 epoch sec; seqNo:3473; 4056906.95 north; 590905.33 east 2021-02-12T23:24:27Z- AHRS_M2 msg: 1613172267.38 epoch sec; seqNo:3442 2021-02-12T23:24:27Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.13 psi 2021-02-12T23:24:27Z- handleDepth()-Depth_Keller msg: 1613172267385 epoch sec; seqNo:3474; depth 9.73 2021-02-12T23:24:27Z- handleDvl() - RDI_Pathfinder msg: 1613172267.40 epoch sec; seqNo:1599 2021-02-12T23:24:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.89 , 96.36 , 113.93 , 75.59 2021-02-12T23:24:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , -0.02 2021-02-12T23:24:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172267.41 epoch sec; seqNo:3474; 4056907.34 north; 590906.55 east 2021-02-12T23:24:27Z- AHRS_M2 msg: 1613172267.78 epoch sec; seqNo:3443 2021-02-12T23:24:27Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.12 psi 2021-02-12T23:24:27Z- handleDepth()-Depth_Keller msg: 1613172267784 epoch sec; seqNo:3475; depth 9.60 2021-02-12T23:24:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172267.80 epoch sec; seqNo:3475; 4056907.72 north; 590906.55 east 2021-02-12T23:24:28Z- AHRS_M2 msg: 1613172268.18 epoch sec; seqNo:3444 2021-02-12T23:24:28Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.12 psi 2021-02-12T23:24:28Z- handleDepth()-Depth_Keller msg: 1613172268192 epoch sec; seqNo:3476; depth 9.48 2021-02-12T23:24:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:28Z- handleDvl() - RDI_Pathfinder msg: 1613172268.20 epoch sec; seqNo:1600 2021-02-12T23:24:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , -0.02 2021-02-12T23:24:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172268.21 epoch sec; seqNo:3476; 4056907.72 north; 590906.55 east 2021-02-12T23:24:28Z- LcmTrn::handleMessages() - handling 43 messages took 0.65 ms and 130 calls... 2021-02-12T23:24:28Z- waiting for 1613172266.37, time is 1613172268.21 2021-02-12T23:24:28Z- LcmTrn::updateTrn() equating pose 1613172268.21 and meas times 1613172268.21 2021-02-12T23:24:28Z- LcmTrn::updateTrn() >>>> pose time 1613172268.21, meas time 1613172268.21 <<<< 2021-02-12T23:24:28Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172268.21, ping # 0. 2021-02-12T23:24:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172268.21 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:24:28Z- TerrainNav::motionUpdate() - currEstimate = (4056908, 590906.55, 9.48) 2021-02-12T23:24:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:24:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:24:28Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:24:28Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:24:28Z- TNavBF::Projecting Measurements 2021-02-12T23:24:28Z- TNavFilter::Measurement from time = 1613172268.21 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:24:28Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:24:28Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:24:28Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:24:28Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:24:28Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:24:28Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:24:28Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:24:28Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 768.9 EastVariance: 833.1 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:24:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:24:28Z- TNavBF::mmse selected from filter -1 2021-02-12T23:24:28Z- variances([0], [2], and psi_berg) = 768.891280 833.053022 0.000000 2021-02-12T23:24:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:24:28Z- LcmTrn::publishEstimates() - MLE : 4056887.22 590956.05 9.48 2021-02-12T23:24:28Z- LcmTrn::publishEstimates() - MMSE : 4056907.72 590906.55 9.48 2021-02-12T23:24:28Z- LcmTrn::publishEstimates() - COVAR: 768.89 833.05 1.22 0.00 2021-02-12T23:24:28Z- AHRS_M2 msg: 1613172268.59 epoch sec; seqNo:3445 2021-02-12T23:24:28Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.13 psi 2021-02-12T23:24:28Z- handleDepth()-Depth_Keller msg: 1613172268595 epoch sec; seqNo:3477; depth 9.38 2021-02-12T23:24:28Z- handleDvl() - RDI_Pathfinder msg: 1613172268.61 epoch sec; seqNo:1601 2021-02-12T23:24:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.83 , 96.29 , 113.86 , 75.59 2021-02-12T23:24:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.14 , -0.04 2021-02-12T23:24:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172268.62 epoch sec; seqNo:3477; 4056908.10 north; 590906.54 east 2021-02-12T23:24:28Z- AHRS_M2 msg: 1613172268.99 epoch sec; seqNo:3446 2021-02-12T23:24:28Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.14 psi 2021-02-12T23:24:28Z- handleDepth()-Depth_Keller msg: 1613172268996 epoch sec; seqNo:3478; depth 9.24 2021-02-12T23:24:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172269.01 epoch sec; seqNo:3478; 4056908.47 north; 590906.54 east 2021-02-12T23:24:29Z- AHRS_M2 msg: 1613172269.40 epoch sec; seqNo:3447 2021-02-12T23:24:29Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.15 psi 2021-02-12T23:24:29Z- handleDepth()-Depth_Keller msg: 1613172269400 epoch sec; seqNo:3479; depth 9.13 2021-02-12T23:24:29Z- handleDvl() - RDI_Pathfinder msg: 1613172269.41 epoch sec; seqNo:1602 2021-02-12T23:24:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.83 , 96.29 , 116.57 , 75.18 2021-02-12T23:24:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.18 , 0.14 2021-02-12T23:24:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172269.44 epoch sec; seqNo:3479; 4056908.85 north; 590906.53 east 2021-02-12T23:24:29Z- AHRS_M2 msg: 1613172269.80 epoch sec; seqNo:3448 2021-02-12T23:24:29Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-12T23:24:29Z- handleDepth()-Depth_Keller msg: 1613172269809 epoch sec; seqNo:3480; depth 9.01 2021-02-12T23:24:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172269.82 epoch sec; seqNo:3480; 4056908.85 north; 590906.53 east 2021-02-12T23:24:30Z- AHRS_M2 msg: 1613172270.20 epoch sec; seqNo:3449 2021-02-12T23:24:30Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-12T23:24:30Z- handleDepth()-Depth_Keller msg: 1613172270213 epoch sec; seqNo:3481; depth 8.87 2021-02-12T23:24:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172270.22 epoch sec; seqNo:3481; 4056909.23 north; 590906.53 east 2021-02-12T23:24:30Z- AHRS_M2 msg: 1613172270.61 epoch sec; seqNo:3450 2021-02-12T23:24:30Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-12T23:24:30Z- handleDepth()-Depth_Keller msg: 1613172270612 epoch sec; seqNo:3482; depth 8.75 2021-02-12T23:24:30Z- handleDvl() - RDI_Pathfinder msg: 1613172270.63 epoch sec; seqNo:1603 2021-02-12T23:24:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.76 , 101.63 , 117.85 , 76.41 2021-02-12T23:24:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.04 , 0.32 2021-02-12T23:24:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172270.64 epoch sec; seqNo:3482; 4056909.62 north; 590907.75 east 2021-02-12T23:24:31Z- AHRS_M2 msg: 1613172271.01 epoch sec; seqNo:3451 2021-02-12T23:24:31Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.14 psi 2021-02-12T23:24:31Z- handleDepth()-Depth_Keller msg: 1613172271016 epoch sec; seqNo:3483; depth 8.62 2021-02-12T23:24:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172271.03 epoch sec; seqNo:3483; 4056910.00 north; 590907.74 east 2021-02-12T23:24:31Z- AHRS_M2 msg: 1613172271.42 epoch sec; seqNo:3452 2021-02-12T23:24:31Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.13 psi 2021-02-12T23:24:31Z- handleDepth()-Depth_Keller msg: 1613172271422 epoch sec; seqNo:3484; depth 8.50 2021-02-12T23:24:31Z- handleDvl() - RDI_Pathfinder msg: 1613172271.43 epoch sec; seqNo:1604 2021-02-12T23:24:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.26 , 102.78 , 120.35 , 76.00 2021-02-12T23:24:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.26 , 0.22 2021-02-12T23:24:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172271.44 epoch sec; seqNo:3484; 4056910.00 north; 590907.74 east 2021-02-12T23:24:31Z- AHRS_M2 msg: 1613172271.83 epoch sec; seqNo:3453 2021-02-12T23:24:31Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.13 psi 2021-02-12T23:24:31Z- handleDepth()-Depth_Keller msg: 1613172271839 epoch sec; seqNo:3485; depth 8.38 2021-02-12T23:24:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:31Z- handleDvl() - RDI_Pathfinder msg: 1613172271.84 epoch sec; seqNo:1605 2021-02-12T23:24:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.26 , 0.22 2021-02-12T23:24:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172271.85 epoch sec; seqNo:3485; 4056910.38 north; 590907.74 east 2021-02-12T23:24:32Z- AHRS_M2 msg: 1613172272.22 epoch sec; seqNo:3454 2021-02-12T23:24:32Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.12 psi 2021-02-12T23:24:32Z- handleDepth()-Depth_Keller msg: 1613172272233 epoch sec; seqNo:3486; depth 8.26 2021-02-12T23:24:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172272.24 epoch sec; seqNo:3486; 4056910.39 north; 590908.96 east 2021-02-12T23:24:32Z- AHRS_M2 msg: 1613172272.63 epoch sec; seqNo:3455 2021-02-12T23:24:32Z- AHRS_M2 msg: 0.65 phi; 0.37 theta; -0.12 psi 2021-02-12T23:24:32Z- handleDepth()-Depth_Keller msg: 1613172272632 epoch sec; seqNo:3487; depth 8.13 2021-02-12T23:24:32Z- handleDvl() - RDI_Pathfinder msg: 1613172272.64 epoch sec; seqNo:1606 2021-02-12T23:24:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.54 , 95.95 , 120.28 , 77.64 2021-02-12T23:24:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.36 , -0.09 2021-02-12T23:24:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172272.65 epoch sec; seqNo:3487; 4056910.77 north; 590908.96 east 2021-02-12T23:24:33Z- AHRS_M2 msg: 1613172273.04 epoch sec; seqNo:3456 2021-02-12T23:24:33Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.12 psi 2021-02-12T23:24:33Z- handleDepth()-Depth_Keller msg: 1613172273041 epoch sec; seqNo:3488; depth 8.00 2021-02-12T23:24:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:33Z- handleDvl() - RDI_Pathfinder msg: 1613172273.04 epoch sec; seqNo:1607 2021-02-12T23:24:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.36 , -0.09 2021-02-12T23:24:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172273.05 epoch sec; seqNo:3488; 4056910.77 north; 590908.96 east 2021-02-12T23:24:33Z- AHRS_M2 msg: 1613172273.44 epoch sec; seqNo:3457 2021-02-12T23:24:33Z- AHRS_M2 msg: 0.67 phi; 0.36 theta; -0.13 psi 2021-02-12T23:24:33Z- handleDepth()-Depth_Keller msg: 1613172273440 epoch sec; seqNo:3489; depth 7.89 2021-02-12T23:24:33Z- handleDvl() - RDI_Pathfinder msg: 1613172273.46 epoch sec; seqNo:1608 2021-02-12T23:24:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.37 , 100.07 , 119.00 , 77.23 2021-02-12T23:24:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.18 , -0.33 2021-02-12T23:24:33Z- LcmTrn::handleMessages() - handling 46 messages took 0.70 ms and 132 calls... 2021-02-12T23:24:33Z- waiting for 1613172271.21, time is 1613172273.46 2021-02-12T23:24:33Z- LcmTrn::updateTrn() equating pose 1613172273.46 and meas times 1613172273.46 2021-02-12T23:24:33Z- LcmTrn::updateTrn() >>>> pose time 1613172273.46, meas time 1613172273.46 <<<< 2021-02-12T23:24:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172273.46 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:24:33Z- TerrainNav::motionUpdate() - currEstimate = (4056911, 590908.96, 7.89) 2021-02-12T23:24:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:24:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:24:33Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172273.46 sec.; No relevant navigation data available 2021-02-12T23:24:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:24:33Z- TNavBF::mmse selected from filter -1 2021-02-12T23:24:33Z- variances([0], [2], and psi_berg) = 768.765615 833.052870 0.000000 2021-02-12T23:24:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:24:33Z- LcmTrn::publishEstimates() - MLE : 4056890.27 590958.46 7.89 2021-02-12T23:24:33Z- LcmTrn::publishEstimates() - MMSE : 4056910.77 590908.96 7.89 2021-02-12T23:24:33Z- LcmTrn::publishEstimates() - COVAR: 768.77 833.05 1.42 0.00 2021-02-12T23:24:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172273.47 epoch sec; seqNo:3489; 4056911.15 north; 590908.95 east 2021-02-12T23:24:33Z- handleDepth()-Depth_Keller msg: 1613172273861 epoch sec; seqNo:3490; depth 7.76 2021-02-12T23:24:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172273.88 epoch sec; seqNo:3490; 4056911.17 north; 590910.17 east 2021-02-12T23:24:34Z- AHRS_M2 msg: 1613172274.24 epoch sec; seqNo:3458 2021-02-12T23:24:34Z- AHRS_M2 msg: 0.68 phi; 0.35 theta; -0.16 psi 2021-02-12T23:24:34Z- handleDepth()-Depth_Keller msg: 1613172274249 epoch sec; seqNo:3491; depth 7.65 2021-02-12T23:24:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:34Z- handleDvl() - RDI_Pathfinder msg: 1613172274.25 epoch sec; seqNo:1609 2021-02-12T23:24:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.18 , -0.33 2021-02-12T23:24:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172274.26 epoch sec; seqNo:3491; 4056911.54 north; 590910.17 east 2021-02-12T23:24:34Z- AHRS_M2 msg: 1613172274.65 epoch sec; seqNo:3459 2021-02-12T23:24:34Z- AHRS_M2 msg: 0.68 phi; 0.35 theta; -0.17 psi 2021-02-12T23:24:34Z- handleDepth()-Depth_Keller msg: 1613172274657 epoch sec; seqNo:3492; depth 7.54 2021-02-12T23:24:34Z- handleDvl() - RDI_Pathfinder msg: 1613172274.67 epoch sec; seqNo:1610 2021-02-12T23:24:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 103.93 , 121.50 , 78.04 2021-02-12T23:24:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.09 , -0.35 2021-02-12T23:24:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172274.68 epoch sec; seqNo:3492; 4056911.54 north; 590910.17 east 2021-02-12T23:24:35Z- AHRS_M2 msg: 1613172275.05 epoch sec; seqNo:3460 2021-02-12T23:24:35Z- AHRS_M2 msg: 0.68 phi; 0.34 theta; -0.17 psi 2021-02-12T23:24:35Z- handleDepth()-Depth_Keller msg: 1613172275056 epoch sec; seqNo:3493; depth 7.40 2021-02-12T23:24:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172275.08 epoch sec; seqNo:3493; 4056911.92 north; 590910.16 east 2021-02-12T23:24:35Z- AHRS_M2 msg: 1613172275.46 epoch sec; seqNo:3461 2021-02-12T23:24:35Z- AHRS_M2 msg: 0.68 phi; 0.34 theta; -0.17 psi 2021-02-12T23:24:35Z- handleDepth()-Depth_Keller msg: 1613172275460 epoch sec; seqNo:3494; depth 7.27 2021-02-12T23:24:35Z- handleDvl() - RDI_Pathfinder msg: 1613172275.47 epoch sec; seqNo:1611 2021-02-12T23:24:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.35 , 105.28 , 118.80 , 78.24 2021-02-12T23:24:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.21 , -0.20 2021-02-12T23:24:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172275.50 epoch sec; seqNo:3494; 4056911.92 north; 590910.16 east 2021-02-12T23:24:35Z- AHRS_M2 msg: 1613172275.86 epoch sec; seqNo:3462 2021-02-12T23:24:35Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.18 psi 2021-02-12T23:24:35Z- handleDepth()-Depth_Keller msg: 1613172275869 epoch sec; seqNo:3495; depth 7.17 2021-02-12T23:24:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172275.88 epoch sec; seqNo:3495; 4056912.30 north; 590910.16 east 2021-02-12T23:24:36Z- AHRS_M2 msg: 1613172276.27 epoch sec; seqNo:3463 2021-02-12T23:24:36Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.18 psi 2021-02-12T23:24:36Z- handleDepth()-Depth_Keller msg: 1613172276270 epoch sec; seqNo:3496; depth 7.06 2021-02-12T23:24:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172276.28 epoch sec; seqNo:3496; 4056912.68 north; 590910.16 east 2021-02-12T23:24:36Z- AHRS_M2 msg: 1613172276.67 epoch sec; seqNo:3464 2021-02-12T23:24:36Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.18 psi 2021-02-12T23:24:36Z- handleDepth()-Depth_Keller msg: 1613172276672 epoch sec; seqNo:3497; depth 6.95 2021-02-12T23:24:36Z- handleDvl() - RDI_Pathfinder msg: 1613172276.69 epoch sec; seqNo:1612 2021-02-12T23:24:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.07 , 106.69 , 118.86 , 78.04 2021-02-12T23:24:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.25 , -0.01 2021-02-12T23:24:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172276.71 epoch sec; seqNo:3497; 4056912.68 north; 590910.16 east 2021-02-12T23:24:37Z- AHRS_M2 msg: 1613172277.07 epoch sec; seqNo:3465 2021-02-12T23:24:37Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.18 psi 2021-02-12T23:24:37Z- handleDepth()-Depth_Keller msg: 1613172277076 epoch sec; seqNo:3498; depth 6.80 2021-02-12T23:24:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172277.09 epoch sec; seqNo:3498; 4056913.06 north; 590910.15 east 2021-02-12T23:24:37Z- AHRS_M2 msg: 1613172277.48 epoch sec; seqNo:3466 2021-02-12T23:24:37Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.19 psi 2021-02-12T23:24:37Z- handleDepth()-Depth_Keller msg: 1613172277491 epoch sec; seqNo:3499; depth 6.71 2021-02-12T23:24:37Z- handleDvl() - RDI_Pathfinder msg: 1613172277.50 epoch sec; seqNo:1613 2021-02-12T23:24:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.77 , 105.34 , 120.21 , 78.04 2021-02-12T23:24:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.19 , 0.24 2021-02-12T23:24:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172277.51 epoch sec; seqNo:3499; 4056913.44 north; 590910.15 east 2021-02-12T23:24:37Z- AHRS_M2 msg: 1613172277.89 epoch sec; seqNo:3467 2021-02-12T23:24:37Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.19 psi 2021-02-12T23:24:37Z- handleDepth()-Depth_Keller msg: 1613172277891 epoch sec; seqNo:3500; depth 6.58 2021-02-12T23:24:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:37Z- handleDvl() - RDI_Pathfinder msg: 1613172277.89 epoch sec; seqNo:1614 2021-02-12T23:24:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.19 , 0.24 2021-02-12T23:24:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172277.90 epoch sec; seqNo:3500; 4056913.82 north; 590910.14 east 2021-02-12T23:24:38Z- AHRS_M2 msg: 1613172278.29 epoch sec; seqNo:3468 2021-02-12T23:24:38Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.20 psi 2021-02-12T23:24:38Z- handleDepth()-Depth_Keller msg: 1613172278292 epoch sec; seqNo:3501; depth 6.47 2021-02-12T23:24:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172278.31 epoch sec; seqNo:3501; 4056913.82 north; 590910.14 east 2021-02-12T23:24:38Z- LcmTrn::handleMessages() - handling 42 messages took 0.64 ms and 129 calls... 2021-02-12T23:24:38Z- waiting for 1613172276.46, time is 1613172278.31 2021-02-12T23:24:38Z- LcmTrn::updateTrn() >>>> pose time 1613172278.31, meas time 1613172277.89 <<<< 2021-02-12T23:24:38Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172277.89, ping # 0. 2021-02-12T23:24:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172277.89 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:24:38Z- TerrainNav::motionUpdate() - currEstimate = (4056914, 590910.14, 6.47) 2021-02-12T23:24:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:24:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:24:38Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:24:38Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:24:38Z- TNavBF::Projecting Measurements 2021-02-12T23:24:38Z- TNavFilter::Measurement from time = 1613172277.89 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:24:38Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:24:38Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:24:38Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:24:38Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:24:38Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:24:38Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:24:38Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:24:38Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 768.7 EastVariance: 833.1 DepthVariance: 1.6 this->DepthBiasCov: 1.6 2021-02-12T23:24:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:24:38Z- TNavBF::mmse selected from filter -1 2021-02-12T23:24:38Z- variances([0], [2], and psi_berg) = 768.622092 833.052698 0.000000 2021-02-12T23:24:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:24:38Z- LcmTrn::publishEstimates() - MLE : 4056894.32 590861.64 6.47 2021-02-12T23:24:38Z- LcmTrn::publishEstimates() - MMSE : 4056913.82 590910.14 6.47 2021-02-12T23:24:38Z- LcmTrn::publishEstimates() - COVAR: 768.62 833.05 1.59 0.00 2021-02-12T23:24:38Z- AHRS_M2 msg: 1613172278.69 epoch sec; seqNo:3469 2021-02-12T23:24:38Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.21 psi 2021-02-12T23:24:38Z- handleDepth()-Depth_Keller msg: 1613172278692 epoch sec; seqNo:3502; depth 6.34 2021-02-12T23:24:38Z- handleDvl() - RDI_Pathfinder msg: 1613172278.70 epoch sec; seqNo:1615 2021-02-12T23:24:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.77 , 108.05 , 118.86 , 77.64 2021-02-12T23:24:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.08 , 0.33 2021-02-12T23:24:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172278.73 epoch sec; seqNo:3502; 4056914.21 north; 590911.36 east 2021-02-12T23:24:39Z- AHRS_M2 msg: 1613172279.09 epoch sec; seqNo:3470 2021-02-12T23:24:39Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.21 psi 2021-02-12T23:24:39Z- handleDepth()-Depth_Keller msg: 1613172279099 epoch sec; seqNo:3503; depth 6.22 2021-02-12T23:24:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:39Z- handleDvl() - RDI_Pathfinder msg: 1613172279.10 epoch sec; seqNo:1616 2021-02-12T23:24:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.08 , 0.33 2021-02-12T23:24:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172279.12 epoch sec; seqNo:3503; 4056914.59 north; 590911.36 east 2021-02-12T23:24:39Z- AHRS_M2 msg: 1613172279.50 epoch sec; seqNo:3471 2021-02-12T23:24:39Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.22 psi 2021-02-12T23:24:39Z- handleDepth()-Depth_Keller msg: 1613172279502 epoch sec; seqNo:3504; depth 6.09 2021-02-12T23:24:39Z- handleDvl() - RDI_Pathfinder msg: 1613172279.51 epoch sec; seqNo:1617 2021-02-12T23:24:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.71 , 109.33 , 121.50 , 78.86 2021-02-12T23:24:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , 0.22 2021-02-12T23:24:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172279.53 epoch sec; seqNo:3504; 4056914.97 north; 590911.35 east 2021-02-12T23:24:39Z- AHRS_M2 msg: 1613172279.90 epoch sec; seqNo:3472 2021-02-12T23:24:39Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.23 psi 2021-02-12T23:24:39Z- handleDepth()-Depth_Keller msg: 1613172279904 epoch sec; seqNo:3505; depth 5.95 2021-02-12T23:24:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172279.92 epoch sec; seqNo:3505; 4056915.35 north; 590911.35 east 2021-02-12T23:24:40Z- AHRS_M2 msg: 1613172280.30 epoch sec; seqNo:3473 2021-02-12T23:24:40Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.24 psi 2021-02-12T23:24:40Z- handleDepth()-Depth_Keller msg: 1613172280308 epoch sec; seqNo:3506; depth 5.84 2021-02-12T23:24:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172280.32 epoch sec; seqNo:3506; 4056915.35 north; 590911.35 east 2021-02-12T23:24:40Z- AHRS_M2 msg: 1613172280.71 epoch sec; seqNo:3474 2021-02-12T23:24:40Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.24 psi 2021-02-12T23:24:40Z- handleDepth()-Depth_Keller msg: 1613172280717 epoch sec; seqNo:3507; depth 5.70 2021-02-12T23:24:40Z- handleDvl() - RDI_Pathfinder msg: 1613172280.73 epoch sec; seqNo:1618 2021-02-12T23:24:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 111.97 , 124.14 , 80.88 2021-02-12T23:24:40Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.01 , 0.09 2021-02-12T23:24:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172280.74 epoch sec; seqNo:3507; 4056915.74 north; 590912.57 east 2021-02-12T23:24:41Z- AHRS_M2 msg: 1613172281.12 epoch sec; seqNo:3475 2021-02-12T23:24:41Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.23 psi 2021-02-12T23:24:41Z- handleDepth()-Depth_Keller msg: 1613172281129 epoch sec; seqNo:3508; depth 5.57 2021-02-12T23:24:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172281.14 epoch sec; seqNo:3508; 4056916.12 north; 590912.56 east 2021-02-12T23:24:41Z- AHRS_M2 msg: 1613172281.52 epoch sec; seqNo:3476 2021-02-12T23:24:41Z- AHRS_M2 msg: 0.59 phi; 0.28 theta; -0.21 psi 2021-02-12T23:24:41Z- handleDepth()-Depth_Keller msg: 1613172281520 epoch sec; seqNo:3509; depth 5.45 2021-02-12T23:24:41Z- handleDvl() - RDI_Pathfinder msg: 1613172281.54 epoch sec; seqNo:1619 2021-02-12T23:24:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 112.85 , 121.77 , 83.11 2021-02-12T23:24:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.07 , -0.06 2021-02-12T23:24:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172281.55 epoch sec; seqNo:3509; 4056916.49 north; 590912.56 east 2021-02-12T23:24:41Z- AHRS_M2 msg: 1613172281.92 epoch sec; seqNo:3477 2021-02-12T23:24:41Z- AHRS_M2 msg: 0.61 phi; 0.23 theta; -0.21 psi 2021-02-12T23:24:41Z- handleDepth()-Depth_Keller msg: 1613172281924 epoch sec; seqNo:3510; depth 5.33 2021-02-12T23:24:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172281.95 epoch sec; seqNo:3510; 4056916.87 north; 590912.55 east 2021-02-12T23:24:42Z- AHRS_M2 msg: 1613172282.32 epoch sec; seqNo:3478 2021-02-12T23:24:42Z- AHRS_M2 msg: 0.63 phi; 0.18 theta; -0.22 psi 2021-02-12T23:24:42Z- handleDepth()-Depth_Keller msg: 1613172282329 epoch sec; seqNo:3511; depth 5.18 2021-02-12T23:24:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172282.34 epoch sec; seqNo:3511; 4056916.87 north; 590912.55 east 2021-02-12T23:24:42Z- AHRS_M2 msg: 1613172282.73 epoch sec; seqNo:3479 2021-02-12T23:24:42Z- AHRS_M2 msg: 0.64 phi; 0.13 theta; -0.23 psi 2021-02-12T23:24:42Z- handleDepth()-Depth_Keller msg: 1613172282734 epoch sec; seqNo:3512; depth 5.07 2021-02-12T23:24:42Z- handleDvl() - RDI_Pathfinder msg: 1613172282.75 epoch sec; seqNo:1620 2021-02-12T23:24:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.08 , 106.90 , 111.36 , 83.11 2021-02-12T23:24:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.01 , -0.21 2021-02-12T23:24:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172282.76 epoch sec; seqNo:3512; 4056917.25 north; 590912.55 east 2021-02-12T23:24:43Z- AHRS_M2 msg: 1613172283.13 epoch sec; seqNo:3480 2021-02-12T23:24:43Z- AHRS_M2 msg: 0.65 phi; 0.07 theta; -0.23 psi 2021-02-12T23:24:43Z- handleDepth()-Depth_Keller msg: 1613172283142 epoch sec; seqNo:3513; depth 4.95 2021-02-12T23:24:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172283.16 epoch sec; seqNo:3513; 4056917.64 north; 590913.77 east 2021-02-12T23:24:43Z- LcmTrn::handleMessages() - handling 42 messages took 0.62 ms and 130 calls... 2021-02-12T23:24:43Z- waiting for 1613172281.31, time is 1613172283.16 2021-02-12T23:24:43Z- LcmTrn::updateTrn() >>>> pose time 1613172283.16, meas time 1613172282.75 <<<< 2021-02-12T23:24:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172282.75 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:24:43Z- TerrainNav::motionUpdate() - currEstimate = (4056918, 590913.77, 4.95) 2021-02-12T23:24:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:24:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:24:43Z- TNavBF::Projecting Measurements 2021-02-12T23:24:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:24:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:24:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:24:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:24:43Z- TNavFilter::4 good beams. 2021-02-12T23:24:43Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:24:43Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:24:43Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:24:43Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:24:43Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:24:43Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:24:43Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:24:43Z- TNavBF::mmse selected from filter -1 2021-02-12T23:24:43Z- TerrainMapOctree: (vehN, vehE ) = (4056917.32, 590913.46). 2021-02-12T23:24:43Z- TerrainMapOctree: Min Distance = 5468.51. 2021-02-12T23:24:43Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:24:43Z- There is no specific good beam that all particles have in common. 2021-02-12T23:24:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:24:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.83 K: 0.119 ExpectedMeasDiff: nan currMeas.covariance: 11.86 2021-02-12T23:24:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:24:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.62 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 18.28 2021-02-12T23:24:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:24:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.50 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 19.84 2021-02-12T23:24:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:24:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 11.05 2021-02-12T23:24:43Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:24:43Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:24:43Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:24:43Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:24:43Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:24:43Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:24:43Z- Filter 0 No beams used in subcloud update 2021-02-12T23:24:43Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:24:43Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:24:43Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 768.5 EastVariance: 833.1 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-12T23:24:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172282.75 sec. 2021-02-12T23:24:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:24:43Z- TNavBF::mmse selected from filter -1 2021-02-12T23:24:43Z- variances([0], [2], and psi_berg) = 768.440408 833.052480 0.000000 2021-02-12T23:24:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:24:43Z- LcmTrn::publishEstimates() - MLE : 4056898.14 590865.27 4.95 2021-02-12T23:24:43Z- LcmTrn::publishEstimates() - MMSE : 4056917.64 590913.77 4.95 2021-02-12T23:24:43Z- LcmTrn::publishEstimates() - COVAR: 768.44 833.05 1.28 0.00 2021-02-12T23:24:43Z- AHRS_M2 msg: 1613172283.56 epoch sec; seqNo:3481 2021-02-12T23:24:43Z- AHRS_M2 msg: 0.66 phi; 0.02 theta; -0.22 psi 2021-02-12T23:24:43Z- handleDepth()-Depth_Keller msg: 1613172283560 epoch sec; seqNo:3514; depth 4.85 2021-02-12T23:24:43Z- handleDvl() - RDI_Pathfinder msg: 1613172283.59 epoch sec; seqNo:1621 2021-02-12T23:24:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.79 , 108.12 , 101.36 , 0.00 2021-02-12T23:24:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , -0.19 , 0.08 2021-02-12T23:24:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172283.60 epoch sec; seqNo:3514; 4056918.02 north; 590913.76 east 2021-02-12T23:24:43Z- AHRS_M2 msg: 1613172283.94 epoch sec; seqNo:3482 2021-02-12T23:24:43Z- AHRS_M2 msg: 0.66 phi; -0.02 theta; -0.20 psi 2021-02-12T23:24:43Z- handleDepth()-Depth_Keller msg: 1613172283945 epoch sec; seqNo:3515; depth 4.75 2021-02-12T23:24:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:43Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:43Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:43Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:43Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:43Z- handleDvl() - RDI_Pathfinder msg: 1613172283.95 epoch sec; seqNo:1622 2021-02-12T23:24:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , -0.19 , 0.08 2021-02-12T23:24:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172283.96 epoch sec; seqNo:3515; 4056918.02 north; 590913.76 east 2021-02-12T23:24:44Z- AHRS_M2 msg: 1613172284.35 epoch sec; seqNo:3483 2021-02-12T23:24:44Z- AHRS_M2 msg: 0.67 phi; -0.06 theta; -0.18 psi 2021-02-12T23:24:44Z- handleDepth()-Depth_Keller msg: 1613172284358 epoch sec; seqNo:3516; depth 4.69 2021-02-12T23:24:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172284.38 epoch sec; seqNo:3516; 4056918.40 north; 590913.76 east 2021-02-12T23:24:44Z- AHRS_M2 msg: 1613172284.75 epoch sec; seqNo:3484 2021-02-12T23:24:44Z- AHRS_M2 msg: 0.67 phi; -0.09 theta; -0.15 psi 2021-02-12T23:24:44Z- handleDepth()-Depth_Keller msg: 1613172284752 epoch sec; seqNo:3517; depth 4.65 2021-02-12T23:24:44Z- handleDvl() - RDI_Pathfinder msg: 1613172284.77 epoch sec; seqNo:1623 2021-02-12T23:24:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.36 , 105.35 , 97.24 , 93.18 2021-02-12T23:24:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.63 , 0.07 , 0.48 2021-02-12T23:24:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172284.78 epoch sec; seqNo:3517; 4056918.40 north; 590913.76 east 2021-02-12T23:24:45Z- AHRS_M2 msg: 1613172285.15 epoch sec; seqNo:3485 2021-02-12T23:24:45Z- AHRS_M2 msg: 0.68 phi; -0.12 theta; -0.13 psi 2021-02-12T23:24:45Z- handleDepth()-Depth_Keller msg: 1613172285157 epoch sec; seqNo:3518; depth 4.62 2021-02-12T23:24:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172285.17 epoch sec; seqNo:3518; 4056918.78 north; 590913.76 east 2021-02-12T23:24:45Z- AHRS_M2 msg: 1613172285.56 epoch sec; seqNo:3486 2021-02-12T23:24:45Z- AHRS_M2 msg: 0.68 phi; -0.15 theta; -0.12 psi 2021-02-12T23:24:45Z- handleDepth()-Depth_Keller msg: 1613172285560 epoch sec; seqNo:3519; depth 4.59 2021-02-12T23:24:45Z- handleDvl() - RDI_Pathfinder msg: 1613172285.57 epoch sec; seqNo:1624 2021-02-12T23:24:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.15 , 88.04 , 96.15 2021-02-12T23:24:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , 0.51 , 0.44 2021-02-12T23:24:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172285.58 epoch sec; seqNo:3519; 4056918.78 north; 590913.76 east 2021-02-12T23:24:45Z- AHRS_M2 msg: 1613172285.96 epoch sec; seqNo:3487 2021-02-12T23:24:45Z- AHRS_M2 msg: 0.68 phi; -0.17 theta; -0.11 psi 2021-02-12T23:24:45Z- handleDepth()-Depth_Keller msg: 1613172285966 epoch sec; seqNo:3520; depth 4.60 2021-02-12T23:24:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172285.98 epoch sec; seqNo:3520; 4056918.79 north; 590914.98 east 2021-02-12T23:24:46Z- AHRS_M2 msg: 1613172286.38 epoch sec; seqNo:3488 2021-02-12T23:24:46Z- AHRS_M2 msg: 0.67 phi; -0.20 theta; -0.12 psi 2021-02-12T23:24:46Z- handleDepth()-Depth_Keller msg: 1613172286381 epoch sec; seqNo:3521; depth 4.60 2021-02-12T23:24:46Z- handleDvl() - RDI_Pathfinder msg: 1613172286.39 epoch sec; seqNo:1625 2021-02-12T23:24:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.11 , 96.22 , 84.05 , 104.33 2021-02-12T23:24:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.65 , -0.06 2021-02-12T23:24:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172286.40 epoch sec; seqNo:3521; 4056918.79 north; 590914.98 east 2021-02-12T23:24:46Z- AHRS_M2 msg: 1613172286.77 epoch sec; seqNo:3489 2021-02-12T23:24:46Z- AHRS_M2 msg: 0.67 phi; -0.21 theta; -0.15 psi 2021-02-12T23:24:46Z- handleDepth()-Depth_Keller msg: 1613172286771 epoch sec; seqNo:3522; depth 4.61 2021-02-12T23:24:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172286.78 epoch sec; seqNo:3522; 4056919.17 north; 590914.97 east 2021-02-12T23:24:47Z- AHRS_M2 msg: 1613172287.17 epoch sec; seqNo:3490 2021-02-12T23:24:47Z- AHRS_M2 msg: 0.67 phi; -0.22 theta; -0.19 psi 2021-02-12T23:24:47Z- handleDepth()-Depth_Keller msg: 1613172287176 epoch sec; seqNo:3523; depth 4.64 2021-02-12T23:24:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172287.18 epoch sec; seqNo:3523; 4056919.19 north; 590916.19 east 2021-02-12T23:24:47Z- handleDepth()-Depth_Keller msg: 1613172287597 epoch sec; seqNo:3524; depth 4.67 2021-02-12T23:24:47Z- handleDvl() - RDI_Pathfinder msg: 1613172287.62 epoch sec; seqNo:1626 2021-02-12T23:24:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 103.04 , 84.12 , 108.45 2021-02-12T23:24:47Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.21 , -0.56 2021-02-12T23:24:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172287.64 epoch sec; seqNo:3524; 4056919.56 north; 590916.19 east 2021-02-12T23:24:47Z- AHRS_M2 msg: 1613172287.98 epoch sec; seqNo:3491 2021-02-12T23:24:47Z- AHRS_M2 msg: 0.68 phi; -0.24 theta; -0.24 psi 2021-02-12T23:24:47Z- handleDepth()-Depth_Keller msg: 1613172287994 epoch sec; seqNo:3525; depth 4.72 2021-02-12T23:24:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172288.01 epoch sec; seqNo:3525; 4056919.94 north; 590916.19 east 2021-02-12T23:24:48Z- AHRS_M2 msg: 1613172288.38 epoch sec; seqNo:3492 2021-02-12T23:24:48Z- AHRS_M2 msg: 0.69 phi; -0.25 theta; -0.24 psi 2021-02-12T23:24:48Z- handleDepth()-Depth_Keller msg: 1613172288388 epoch sec; seqNo:3526; depth 4.79 2021-02-12T23:24:48Z- handleDvl() - RDI_Pathfinder msg: 1613172288.40 epoch sec; seqNo:1627 2021-02-12T23:24:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 109.60 , 83.92 , 110.95 2021-02-12T23:24:48Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.37 , -0.53 2021-02-12T23:24:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172288.41 epoch sec; seqNo:3526; 4056920.32 north; 590916.18 east 2021-02-12T23:24:48Z- LcmTrn::handleMessages() - handling 45 messages took 0.68 ms and 132 calls... 2021-02-12T23:24:48Z- waiting for 1613172286.16, time is 1613172288.41 2021-02-12T23:24:48Z- LcmTrn::updateTrn() equating pose 1613172288.41 and meas times 1613172288.41 2021-02-12T23:24:48Z- LcmTrn::updateTrn() >>>> pose time 1613172288.41, meas time 1613172288.41 <<<< 2021-02-12T23:24:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172288.41 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:24:48Z- TerrainNav::motionUpdate() - currEstimate = (4056920, 590916.18, 4.79) 2021-02-12T23:24:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:24:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:24:48Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172288.41 sec.; No relevant navigation data available 2021-02-12T23:24:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:24:48Z- TNavBF::mmse selected from filter -1 2021-02-12T23:24:48Z- variances([0], [2], and psi_berg) = 768.320302 833.052336 0.000000 2021-02-12T23:24:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:24:48Z- LcmTrn::publishEstimates() - MLE : 4056900.82 590867.68 4.79 2021-02-12T23:24:48Z- LcmTrn::publishEstimates() - MMSE : 4056920.32 590916.18 4.79 2021-02-12T23:24:48Z- LcmTrn::publishEstimates() - COVAR: 768.32 833.05 1.47 0.00 2021-02-12T23:24:48Z- handleDepth()-Depth_Keller msg: 1613172288809 epoch sec; seqNo:3527; depth 4.87 2021-02-12T23:24:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:48Z- handleDvl() - RDI_Pathfinder msg: 1613172288.81 epoch sec; seqNo:1628 2021-02-12T23:24:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:48Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.37 , -0.53 2021-02-12T23:24:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172288.82 epoch sec; seqNo:3527; 4056920.70 north; 590916.18 east 2021-02-12T23:24:49Z- AHRS_M2 msg: 1613172289.19 epoch sec; seqNo:3493 2021-02-12T23:24:49Z- AHRS_M2 msg: 0.71 phi; -0.27 theta; -0.21 psi 2021-02-12T23:24:49Z- handleDepth()-Depth_Keller msg: 1613172289197 epoch sec; seqNo:3528; depth 4.96 2021-02-12T23:24:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172289.21 epoch sec; seqNo:3528; 4056921.09 north; 590917.39 east 2021-02-12T23:24:49Z- AHRS_M2 msg: 1613172289.60 epoch sec; seqNo:3494 2021-02-12T23:24:49Z- AHRS_M2 msg: 0.71 phi; -0.29 theta; -0.20 psi 2021-02-12T23:24:49Z- handleDepth()-Depth_Keller msg: 1613172289600 epoch sec; seqNo:3529; depth 5.02 2021-02-12T23:24:49Z- handleDvl() - RDI_Pathfinder msg: 1613172289.61 epoch sec; seqNo:1629 2021-02-12T23:24:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.79 , 108.12 , 82.43 , 112.17 2021-02-12T23:24:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.62 , -0.19 2021-02-12T23:24:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172289.62 epoch sec; seqNo:3529; 4056921.85 north; 590917.39 east 2021-02-12T23:24:50Z- AHRS_M2 msg: 1613172290.00 epoch sec; seqNo:3495 2021-02-12T23:24:50Z- AHRS_M2 msg: 0.71 phi; -0.30 theta; -0.18 psi 2021-02-12T23:24:50Z- handleDepth()-Depth_Keller msg: 1613172290005 epoch sec; seqNo:3530; depth 5.09 2021-02-12T23:24:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:50Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:50Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:50Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:50Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:50Z- handleDvl() - RDI_Pathfinder msg: 1613172290.01 epoch sec; seqNo:1630 2021-02-12T23:24:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.62 , -0.19 2021-02-12T23:24:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172290.02 epoch sec; seqNo:3530; 4056922.23 north; 590917.38 east 2021-02-12T23:24:50Z- AHRS_M2 msg: 1613172290.40 epoch sec; seqNo:3496 2021-02-12T23:24:50Z- AHRS_M2 msg: 0.71 phi; -0.31 theta; -0.17 psi 2021-02-12T23:24:50Z- handleDepth()-Depth_Keller msg: 1613172290417 epoch sec; seqNo:3531; depth 5.19 2021-02-12T23:24:50Z- handleDvl() - RDI_Pathfinder msg: 1613172290.43 epoch sec; seqNo:1631 2021-02-12T23:24:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 106.76 , 78.45 , 112.17 2021-02-12T23:24:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , -0.49 , 0.13 2021-02-12T23:24:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172290.44 epoch sec; seqNo:3531; 4056922.23 north; 590917.38 east 2021-02-12T23:24:50Z- AHRS_M2 msg: 1613172290.81 epoch sec; seqNo:3497 2021-02-12T23:24:50Z- AHRS_M2 msg: 0.70 phi; -0.32 theta; -0.16 psi 2021-02-12T23:24:50Z- handleDepth()-Depth_Keller msg: 1613172290812 epoch sec; seqNo:3532; depth 5.29 2021-02-12T23:24:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172290.83 epoch sec; seqNo:3532; 4056922.61 north; 590917.38 east 2021-02-12T23:24:51Z- AHRS_M2 msg: 1613172291.21 epoch sec; seqNo:3498 2021-02-12T23:24:51Z- AHRS_M2 msg: 0.69 phi; -0.32 theta; -0.15 psi 2021-02-12T23:24:51Z- handleDepth()-Depth_Keller msg: 1613172291216 epoch sec; seqNo:3533; depth 5.37 2021-02-12T23:24:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:51Z- handleDvl() - RDI_Pathfinder msg: 1613172291.22 epoch sec; seqNo:1632 2021-02-12T23:24:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , -0.49 , 0.13 2021-02-12T23:24:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172291.23 epoch sec; seqNo:3533; 4056922.99 north; 590917.37 east 2021-02-12T23:24:51Z- AHRS_M2 msg: 1613172291.62 epoch sec; seqNo:3499 2021-02-12T23:24:51Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.14 psi 2021-02-12T23:24:51Z- handleDepth()-Depth_Keller msg: 1613172291620 epoch sec; seqNo:3534; depth 5.47 2021-02-12T23:24:51Z- handleDvl() - RDI_Pathfinder msg: 1613172291.63 epoch sec; seqNo:1633 2021-02-12T23:24:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.91 , 104.13 , 81.15 , 116.29 2021-02-12T23:24:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.55 , -0.22 , 0.24 2021-02-12T23:24:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172291.64 epoch sec; seqNo:3534; 4056922.99 north; 590917.37 east 2021-02-12T23:24:52Z- AHRS_M2 msg: 1613172292.03 epoch sec; seqNo:3500 2021-02-12T23:24:52Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.14 psi 2021-02-12T23:24:52Z- handleDepth()-Depth_Keller msg: 1613172292034 epoch sec; seqNo:3535; depth 5.56 2021-02-12T23:24:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172292.05 epoch sec; seqNo:3535; 4056923.37 north; 590917.37 east 2021-02-12T23:24:52Z- AHRS_M2 msg: 1613172292.42 epoch sec; seqNo:3501 2021-02-12T23:24:52Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.15 psi 2021-02-12T23:24:52Z- handleDepth()-Depth_Keller msg: 1613172292428 epoch sec; seqNo:3536; depth 5.66 2021-02-12T23:24:52Z- handleDvl() - RDI_Pathfinder msg: 1613172292.45 epoch sec; seqNo:1634 2021-02-12T23:24:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.12 , 102.64 , 78.45 , 116.16 2021-02-12T23:24:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.60 , -0.03 , 0.20 2021-02-12T23:24:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172292.46 epoch sec; seqNo:3536; 4056923.37 north; 590917.37 east 2021-02-12T23:24:52Z- AHRS_M2 msg: 1613172292.83 epoch sec; seqNo:3502 2021-02-12T23:24:52Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.18 psi 2021-02-12T23:24:52Z- handleDepth()-Depth_Keller msg: 1613172292833 epoch sec; seqNo:3537; depth 5.74 2021-02-12T23:24:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172292.85 epoch sec; seqNo:3537; 4056923.74 north; 590917.37 east 2021-02-12T23:24:53Z- AHRS_M2 msg: 1613172293.23 epoch sec; seqNo:3503 2021-02-12T23:24:53Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.21 psi 2021-02-12T23:24:53Z- handleDepth()-Depth_Keller msg: 1613172293242 epoch sec; seqNo:3538; depth 5.85 2021-02-12T23:24:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172293.26 epoch sec; seqNo:3538; 4056923.74 north; 590917.37 east 2021-02-12T23:24:53Z- AHRS_M2 msg: 1613172293.64 epoch sec; seqNo:3504 2021-02-12T23:24:53Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.24 psi 2021-02-12T23:24:53Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-12T23:24:53Z- waiting for 1613172291.41, time is 1613172293.64 2021-02-12T23:24:53Z- LcmTrn::updateTrn() >>>> pose time 1613172293.64, meas time 1613172292.45 <<<< 2021-02-12T23:24:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172292.45 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:24:53Z- TerrainNav::motionUpdate() - currEstimate = (4056924, 590917.37, 5.85) 2021-02-12T23:24:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:24:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:24:53Z- TNavBF::Projecting Measurements 2021-02-12T23:24:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:24:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:24:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:24:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:24:53Z- TNavFilter::4 good beams. 2021-02-12T23:24:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:24:53Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:24:53Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:24:53Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:24:53Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:24:53Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:24:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:24:53Z- TNavBF::mmse selected from filter -1 2021-02-12T23:24:53Z- TerrainMapOctree: (vehN, vehE ) = (4056922.97, 590917.10). 2021-02-12T23:24:53Z- TerrainMapOctree: Min Distance = 5461.82. 2021-02-12T23:24:53Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:24:53Z- There is no specific good beam that all particles have in common. 2021-02-12T23:24:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:24:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.61 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 12.71 2021-02-12T23:24:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:24:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.45 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 16.86 2021-02-12T23:24:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:24:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-12T23:24:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:24:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 21.59 2021-02-12T23:24:53Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:24:53Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:24:53Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:24:53Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:24:53Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:24:53Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:24:53Z- Filter 0 No beams used in subcloud update 2021-02-12T23:24:53Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:24:53Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:24:53Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 768.2 EastVariance: 833.1 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:24:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172292.45 sec. 2021-02-12T23:24:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:24:53Z- TNavBF::mmse selected from filter -1 2021-02-12T23:24:53Z- variances([0], [2], and psi_berg) = 768.157471 833.052141 0.000000 2021-02-12T23:24:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:24:53Z- LcmTrn::publishEstimates() - MLE : 4056904.24 590964.87 5.85 2021-02-12T23:24:53Z- LcmTrn::publishEstimates() - MMSE : 4056923.74 590917.37 5.85 2021-02-12T23:24:53Z- LcmTrn::publishEstimates() - COVAR: 768.16 833.05 1.19 0.00 2021-02-12T23:24:53Z- handleDepth()-Depth_Keller msg: 1613172293640 epoch sec; seqNo:3539; depth 5.96 2021-02-12T23:24:53Z- handleDvl() - RDI_Pathfinder msg: 1613172293.66 epoch sec; seqNo:1635 2021-02-12T23:24:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.84 , 106.76 , 78.86 , 120.28 2021-02-12T23:24:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , 0.04 , 0.14 2021-02-12T23:24:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172293.68 epoch sec; seqNo:3539; 4056924.12 north; 590917.36 east 2021-02-12T23:24:54Z- AHRS_M2 msg: 1613172294.04 epoch sec; seqNo:3505 2021-02-12T23:24:54Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.26 psi 2021-02-12T23:24:54Z- handleDepth()-Depth_Keller msg: 1613172294044 epoch sec; seqNo:3540; depth 6.06 2021-02-12T23:24:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172294.06 epoch sec; seqNo:3540; 4056924.12 north; 590917.36 east 2021-02-12T23:24:54Z- AHRS_M2 msg: 1613172294.44 epoch sec; seqNo:3506 2021-02-12T23:24:54Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.27 psi 2021-02-12T23:24:54Z- handleDepth()-Depth_Keller msg: 1613172294448 epoch sec; seqNo:3541; depth 6.17 2021-02-12T23:24:54Z- handleDvl() - RDI_Pathfinder msg: 1613172294.46 epoch sec; seqNo:1636 2021-02-12T23:24:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 113.39 , 80.95 , 122.85 2021-02-12T23:24:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.00 , 0.14 2021-02-12T23:24:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172294.47 epoch sec; seqNo:3541; 4056924.50 north; 590917.36 east 2021-02-12T23:24:54Z- AHRS_M2 msg: 1613172294.85 epoch sec; seqNo:3507 2021-02-12T23:24:54Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.27 psi 2021-02-12T23:24:54Z- handleDepth()-Depth_Keller msg: 1613172294858 epoch sec; seqNo:3542; depth 6.31 2021-02-12T23:24:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:54Z- handleDvl() - RDI_Pathfinder msg: 1613172294.86 epoch sec; seqNo:1637 2021-02-12T23:24:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.00 , 0.14 2021-02-12T23:24:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172294.87 epoch sec; seqNo:3542; 4056924.52 north; 590918.58 east 2021-02-12T23:24:55Z- AHRS_M2 msg: 1613172295.26 epoch sec; seqNo:3508 2021-02-12T23:24:55Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.24 psi 2021-02-12T23:24:55Z- handleDepth()-Depth_Keller msg: 1613172295268 epoch sec; seqNo:3543; depth 6.43 2021-02-12T23:24:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172295.28 epoch sec; seqNo:3543; 4056924.89 north; 590918.58 east 2021-02-12T23:24:55Z- AHRS_M2 msg: 1613172295.66 epoch sec; seqNo:3509 2021-02-12T23:24:55Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.22 psi 2021-02-12T23:24:55Z- handleDepth()-Depth_Keller msg: 1613172295669 epoch sec; seqNo:3544; depth 6.56 2021-02-12T23:24:55Z- handleDvl() - RDI_Pathfinder msg: 1613172295.68 epoch sec; seqNo:1638 2021-02-12T23:24:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.80 , 113.19 , 78.86 , 122.65 2021-02-12T23:24:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , 0.07 , 0.12 2021-02-12T23:24:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172295.70 epoch sec; seqNo:3544; 4056924.89 north; 590918.58 east 2021-02-12T23:24:56Z- AHRS_M2 msg: 1613172296.07 epoch sec; seqNo:3510 2021-02-12T23:24:56Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.22 psi 2021-02-12T23:24:56Z- handleDepth()-Depth_Keller msg: 1613172296072 epoch sec; seqNo:3545; depth 6.69 2021-02-12T23:24:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:56Z- handleDvl() - RDI_Pathfinder msg: 1613172296.07 epoch sec; seqNo:1639 2021-02-12T23:24:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , 0.07 , 0.12 2021-02-12T23:24:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172296.09 epoch sec; seqNo:3545; 4056925.27 north; 590918.57 east 2021-02-12T23:24:56Z- AHRS_M2 msg: 1613172296.46 epoch sec; seqNo:3511 2021-02-12T23:24:56Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.22 psi 2021-02-12T23:24:56Z- handleDepth()-Depth_Keller msg: 1613172296471 epoch sec; seqNo:3546; depth 6.80 2021-02-12T23:24:56Z- handleDvl() - RDI_Pathfinder msg: 1613172296.48 epoch sec; seqNo:1640 2021-02-12T23:24:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 111.90 , 78.45 , 124.07 2021-02-12T23:24:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.13 , 0.08 2021-02-12T23:24:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172296.49 epoch sec; seqNo:3546; 4056925.27 north; 590918.57 east 2021-02-12T23:24:56Z- AHRS_M2 msg: 1613172296.87 epoch sec; seqNo:3512 2021-02-12T23:24:56Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.22 psi 2021-02-12T23:24:56Z- handleDepth()-Depth_Keller msg: 1613172296877 epoch sec; seqNo:3547; depth 6.92 2021-02-12T23:24:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172296.89 epoch sec; seqNo:3547; 4056925.65 north; 590918.57 east 2021-02-12T23:24:57Z- AHRS_M2 msg: 1613172297.27 epoch sec; seqNo:3513 2021-02-12T23:24:57Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.22 psi 2021-02-12T23:24:57Z- handleDepth()-Depth_Keller msg: 1613172297276 epoch sec; seqNo:3548; depth 7.04 2021-02-12T23:24:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:24:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:24:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:24:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:24:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:24:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:24:57Z- handleDvl() - RDI_Pathfinder msg: 1613172297.28 epoch sec; seqNo:1641 2021-02-12T23:24:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:24:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.13 , 0.08 2021-02-12T23:24:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172297.29 epoch sec; seqNo:3548; 4056925.66 north; 590919.79 east 2021-02-12T23:24:57Z- AHRS_M2 msg: 1613172297.68 epoch sec; seqNo:3514 2021-02-12T23:24:57Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.21 psi 2021-02-12T23:24:57Z- handleDepth()-Depth_Keller msg: 1613172297685 epoch sec; seqNo:3549; depth 7.17 2021-02-12T23:24:57Z- handleDvl() - RDI_Pathfinder msg: 1613172297.70 epoch sec; seqNo:1642 2021-02-12T23:24:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 110.55 , 78.86 , 125.42 2021-02-12T23:24:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.19 , 0.10 2021-02-12T23:24:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172297.71 epoch sec; seqNo:3549; 4056926.04 north; 590919.78 east 2021-02-12T23:24:58Z- AHRS_M2 msg: 1613172298.09 epoch sec; seqNo:3515 2021-02-12T23:24:58Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.21 psi 2021-02-12T23:24:58Z- handleDepth()-Depth_Keller msg: 1613172298098 epoch sec; seqNo:3550; depth 7.31 2021-02-12T23:24:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172298.11 epoch sec; seqNo:3550; 4056926.42 north; 590919.78 east 2021-02-12T23:24:58Z- AHRS_M2 msg: 1613172298.48 epoch sec; seqNo:3516 2021-02-12T23:24:58Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-12T23:24:58Z- handleDepth()-Depth_Keller msg: 1613172298488 epoch sec; seqNo:3551; depth 7.45 2021-02-12T23:24:58Z- handleDvl() - RDI_Pathfinder msg: 1613172298.51 epoch sec; seqNo:1643 2021-02-12T23:24:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 110.55 , 77.64 , 122.72 2021-02-12T23:24:58Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.22 , -0.05 2021-02-12T23:24:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172298.52 epoch sec; seqNo:3551; 4056926.42 north; 590919.78 east 2021-02-12T23:24:58Z- LcmTrn::handleMessages() - handling 47 messages took 0.65 ms and 134 calls... 2021-02-12T23:24:58Z- waiting for 1613172296.64, time is 1613172298.52 2021-02-12T23:24:58Z- LcmTrn::updateTrn() equating pose 1613172298.52 and meas times 1613172298.52 2021-02-12T23:24:58Z- LcmTrn::updateTrn() >>>> pose time 1613172298.52, meas time 1613172298.52 <<<< 2021-02-12T23:24:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172298.52 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:24:58Z- TerrainNav::motionUpdate() - currEstimate = (4056926, 590919.78, 7.45) 2021-02-12T23:24:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:24:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:24:58Z- TNavBF::Projecting Measurements 2021-02-12T23:24:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:24:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:24:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:24:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:24:58Z- TNavFilter::4 good beams. 2021-02-12T23:24:58Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:24:58Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:24:58Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:24:58Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:24:58Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:24:58Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:24:58Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:24:58Z- TNavBF::mmse selected from filter -1 2021-02-12T23:24:58Z- TerrainMapOctree: (vehN, vehE ) = (4056926.42, 590919.78). 2021-02-12T23:24:58Z- TerrainMapOctree: Min Distance = 5457.44. 2021-02-12T23:24:58Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:24:58Z- There is no specific good beam that all particles have in common. 2021-02-12T23:24:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:24:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 11.89 2021-02-12T23:24:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:24:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 19.55 2021-02-12T23:24:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:24:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 9.64 2021-02-12T23:24:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:24:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 24.09 2021-02-12T23:24:58Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:24:58Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:24:58Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:24:58Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:24:58Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:24:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:24:58Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:24:58Z- Filter 0 No beams used in subcloud update 2021-02-12T23:24:58Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:24:58Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:24:58Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 768.0 EastVariance: 833.1 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:24:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172298.52 sec. 2021-02-12T23:24:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:24:58Z- TNavBF::mmse selected from filter -1 2021-02-12T23:24:58Z- variances([0], [2], and psi_berg) = 768.037884 833.051999 0.000000 2021-02-12T23:24:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:24:58Z- LcmTrn::publishEstimates() - MLE : 4056906.92 590967.28 7.45 2021-02-12T23:24:58Z- LcmTrn::publishEstimates() - MMSE : 4056926.42 590919.78 7.45 2021-02-12T23:24:58Z- LcmTrn::publishEstimates() - COVAR: 768.04 833.05 1.02 0.00 2021-02-12T23:24:58Z- AHRS_M2 msg: 1613172298.89 epoch sec; seqNo:3517 2021-02-12T23:24:58Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-12T23:24:58Z- handleDepth()-Depth_Keller msg: 1613172298901 epoch sec; seqNo:3552; depth 7.59 2021-02-12T23:24:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172298.92 epoch sec; seqNo:3552; 4056926.80 north; 590919.78 east 2021-02-12T23:24:59Z- AHRS_M2 msg: 1613172299.29 epoch sec; seqNo:3518 2021-02-12T23:24:59Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-12T23:24:59Z- handleDepth()-Depth_Keller msg: 1613172299305 epoch sec; seqNo:3553; depth 7.72 2021-02-12T23:24:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172299.31 epoch sec; seqNo:3553; 4056927.19 north; 590920.99 east 2021-02-12T23:24:59Z- AHRS_M2 msg: 1613172299.69 epoch sec; seqNo:3519 2021-02-12T23:24:59Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-12T23:24:59Z- handleDepth()-Depth_Keller msg: 1613172299710 epoch sec; seqNo:3554; depth 7.84 2021-02-12T23:24:59Z- handleDvl() - RDI_Pathfinder msg: 1613172299.72 epoch sec; seqNo:1644 2021-02-12T23:24:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.93 , 106.56 , 76.81 , 118.72 2021-02-12T23:24:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.09 , -0.26 2021-02-12T23:24:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172299.73 epoch sec; seqNo:3554; 4056927.57 north; 590920.99 east 2021-02-12T23:25:00Z- AHRS_M2 msg: 1613172300.10 epoch sec; seqNo:3520 2021-02-12T23:25:00Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.20 psi 2021-02-12T23:25:00Z- handleDepth()-Depth_Keller msg: 1613172300109 epoch sec; seqNo:3555; depth 7.98 2021-02-12T23:25:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172300.12 epoch sec; seqNo:3555; 4056927.95 north; 590920.98 east 2021-02-12T23:25:00Z- AHRS_M2 msg: 1613172300.51 epoch sec; seqNo:3521 2021-02-12T23:25:00Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.21 psi 2021-02-12T23:25:00Z- handleDepth()-Depth_Keller msg: 1613172300517 epoch sec; seqNo:3556; depth 8.10 2021-02-12T23:25:00Z- handleDvl() - RDI_Pathfinder msg: 1613172300.54 epoch sec; seqNo:1645 2021-02-12T23:25:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.13 , 106.76 , 78.04 , 118.92 2021-02-12T23:25:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.08 , -0.24 2021-02-12T23:25:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172300.57 epoch sec; seqNo:3556; 4056928.33 north; 590920.98 east 2021-02-12T23:25:00Z- AHRS_M2 msg: 1613172300.91 epoch sec; seqNo:3522 2021-02-12T23:25:00Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-12T23:25:00Z- handleDepth()-Depth_Keller msg: 1613172300912 epoch sec; seqNo:3557; depth 8.25 2021-02-12T23:25:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:00Z- handleDvl() - RDI_Pathfinder msg: 1613172300.91 epoch sec; seqNo:1646 2021-02-12T23:25:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.08 , -0.24 2021-02-12T23:25:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172300.93 epoch sec; seqNo:3557; 4056928.33 north; 590920.98 east 2021-02-12T23:25:01Z- AHRS_M2 msg: 1613172301.31 epoch sec; seqNo:3523 2021-02-12T23:25:01Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.19 psi 2021-02-12T23:25:01Z- handleDepth()-Depth_Keller msg: 1613172301322 epoch sec; seqNo:3558; depth 8.36 2021-02-12T23:25:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172301.36 epoch sec; seqNo:3558; 4056928.72 north; 590922.20 east 2021-02-12T23:25:01Z- AHRS_M2 msg: 1613172301.72 epoch sec; seqNo:3524 2021-02-12T23:25:01Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.18 psi 2021-02-12T23:25:01Z- handleDepth()-Depth_Keller msg: 1613172301720 epoch sec; seqNo:3559; depth 8.49 2021-02-12T23:25:01Z- handleDvl() - RDI_Pathfinder msg: 1613172301.73 epoch sec; seqNo:1647 2021-02-12T23:25:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.71 , 106.69 , 76.81 , 117.50 2021-02-12T23:25:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.13 , -0.14 2021-02-12T23:25:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172301.74 epoch sec; seqNo:3559; 4056929.10 north; 590922.19 east 2021-02-12T23:25:02Z- AHRS_M2 msg: 1613172302.12 epoch sec; seqNo:3525 2021-02-12T23:25:02Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.18 psi 2021-02-12T23:25:02Z- handleDepth()-Depth_Keller msg: 1613172302129 epoch sec; seqNo:3560; depth 8.60 2021-02-12T23:25:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:02Z- handleDvl() - RDI_Pathfinder msg: 1613172302.13 epoch sec; seqNo:1648 2021-02-12T23:25:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.13 , -0.14 2021-02-12T23:25:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172302.15 epoch sec; seqNo:3560; 4056929.48 north; 590922.19 east 2021-02-12T23:25:02Z- AHRS_M2 msg: 1613172302.52 epoch sec; seqNo:3526 2021-02-12T23:25:02Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.18 psi 2021-02-12T23:25:02Z- handleDepth()-Depth_Keller msg: 1613172302538 epoch sec; seqNo:3561; depth 8.74 2021-02-12T23:25:02Z- handleDvl() - RDI_Pathfinder msg: 1613172302.55 epoch sec; seqNo:1649 2021-02-12T23:25:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.85 , 104.12 , 76.40 , 117.64 2021-02-12T23:25:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.13 , -0.14 2021-02-12T23:25:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172302.56 epoch sec; seqNo:3561; 4056929.86 north; 590922.19 east 2021-02-12T23:25:02Z- AHRS_M2 msg: 1613172302.93 epoch sec; seqNo:3527 2021-02-12T23:25:02Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.18 psi 2021-02-12T23:25:02Z- handleDepth()-Depth_Keller msg: 1613172302932 epoch sec; seqNo:3562; depth 8.87 2021-02-12T23:25:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172302.94 epoch sec; seqNo:3562; 4056930.24 north; 590922.18 east 2021-02-12T23:25:03Z- AHRS_M2 msg: 1613172303.33 epoch sec; seqNo:3528 2021-02-12T23:25:03Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-12T23:25:03Z- handleDepth()-Depth_Keller msg: 1613172303336 epoch sec; seqNo:3563; depth 8.98 2021-02-12T23:25:03Z- handleDvl() - RDI_Pathfinder msg: 1613172303.35 epoch sec; seqNo:1650 2021-02-12T23:25:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 104.19 , 76.40 , 112.30 2021-02-12T23:25:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.22 , -0.16 2021-02-12T23:25:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172303.36 epoch sec; seqNo:3563; 4056930.62 north; 590922.18 east 2021-02-12T23:25:03Z- AHRS_M2 msg: 1613172303.74 epoch sec; seqNo:3529 2021-02-12T23:25:03Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-12T23:25:03Z- handleDepth()-Depth_Keller msg: 1613172303741 epoch sec; seqNo:3564; depth 9.13 2021-02-12T23:25:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172303.76 epoch sec; seqNo:3564; 4056930.62 north; 590922.18 east 2021-02-12T23:25:03Z- LcmTrn::handleMessages() - handling 46 messages took 0.70 ms and 132 calls... 2021-02-12T23:25:03Z- waiting for 1613172301.52, time is 1613172303.76 2021-02-12T23:25:03Z- LcmTrn::updateTrn() >>>> pose time 1613172303.76, meas time 1613172303.35 <<<< 2021-02-12T23:25:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172303.35 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:25:03Z- TerrainNav::motionUpdate() - currEstimate = (4056931, 590922.18, 9.13) 2021-02-12T23:25:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:25:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:25:03Z- TNavBF::Projecting Measurements 2021-02-12T23:25:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:25:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:25:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:25:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:25:03Z- TNavFilter::4 good beams. 2021-02-12T23:25:03Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:25:03Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:25:03Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:25:03Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:25:03Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:25:03Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:25:03Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:25:03Z- TNavBF::mmse selected from filter -1 2021-02-12T23:25:03Z- TerrainMapOctree: (vehN, vehE ) = (4056930.29, 590921.99). 2021-02-12T23:25:03Z- TerrainMapOctree: Min Distance = 5453.03. 2021-02-12T23:25:03Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:25:03Z- There is no specific good beam that all particles have in common. 2021-02-12T23:25:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:25:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 11.27 2021-02-12T23:25:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:25:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 17.37 2021-02-12T23:25:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:25:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 9.34 2021-02-12T23:25:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:25:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 20.18 2021-02-12T23:25:03Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:25:03Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:25:03Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:25:03Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:25:03Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:25:03Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:25:03Z- Filter 0 No beams used in subcloud update 2021-02-12T23:25:03Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:25:03Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:25:03Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 767.9 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:25:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172303.35 sec. 2021-02-12T23:25:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:25:03Z- TNavBF::mmse selected from filter -1 2021-02-12T23:25:03Z- variances([0], [2], and psi_berg) = 767.866723 833.051795 0.000000 2021-02-12T23:25:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:25:03Z- LcmTrn::publishEstimates() - MLE : 4056912.12 590873.68 9.13 2021-02-12T23:25:03Z- LcmTrn::publishEstimates() - MMSE : 4056930.62 590922.18 9.13 2021-02-12T23:25:03Z- LcmTrn::publishEstimates() - COVAR: 767.87 833.05 0.96 0.00 2021-02-12T23:25:04Z- AHRS_M2 msg: 1613172304.14 epoch sec; seqNo:3530 2021-02-12T23:25:04Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-12T23:25:04Z- handleDepth()-Depth_Keller msg: 1613172304147 epoch sec; seqNo:3565; depth 9.25 2021-02-12T23:25:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172304.16 epoch sec; seqNo:3565; 4056930.99 north; 590922.17 east 2021-02-12T23:25:04Z- AHRS_M2 msg: 1613172304.55 epoch sec; seqNo:3531 2021-02-12T23:25:04Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.17 psi 2021-02-12T23:25:04Z- handleDepth()-Depth_Keller msg: 1613172304554 epoch sec; seqNo:3566; depth 9.39 2021-02-12T23:25:04Z- handleDvl() - RDI_Pathfinder msg: 1613172304.57 epoch sec; seqNo:1651 2021-02-12T23:25:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.63 , 104.26 , 74.36 , 115.07 2021-02-12T23:25:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.31 , -0.09 2021-02-12T23:25:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172304.59 epoch sec; seqNo:3566; 4056931.37 north; 590922.17 east 2021-02-12T23:25:04Z- AHRS_M2 msg: 1613172304.95 epoch sec; seqNo:3532 2021-02-12T23:25:04Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.16 psi 2021-02-12T23:25:04Z- handleDepth()-Depth_Keller msg: 1613172304953 epoch sec; seqNo:3567; depth 9.52 2021-02-12T23:25:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172304.96 epoch sec; seqNo:3567; 4056931.75 north; 590922.17 east 2021-02-12T23:25:05Z- AHRS_M2 msg: 1613172305.35 epoch sec; seqNo:3533 2021-02-12T23:25:05Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.16 psi 2021-02-12T23:25:05Z- handleDepth()-Depth_Keller msg: 1613172305356 epoch sec; seqNo:3568; depth 9.64 2021-02-12T23:25:05Z- handleDvl() - RDI_Pathfinder msg: 1613172305.37 epoch sec; seqNo:1652 2021-02-12T23:25:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.63 , 96.15 , 74.36 , 113.72 2021-02-12T23:25:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.31 , 0.01 2021-02-12T23:25:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172305.38 epoch sec; seqNo:3568; 4056932.14 north; 590923.38 east 2021-02-12T23:25:05Z- AHRS_M2 msg: 1613172305.76 epoch sec; seqNo:3534 2021-02-12T23:25:05Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.14 psi 2021-02-12T23:25:05Z- handleDepth()-Depth_Keller msg: 1613172305760 epoch sec; seqNo:3569; depth 9.77 2021-02-12T23:25:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:05Z- handleDvl() - RDI_Pathfinder msg: 1613172305.77 epoch sec; seqNo:1653 2021-02-12T23:25:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.31 , 0.01 2021-02-12T23:25:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172305.78 epoch sec; seqNo:3569; 4056932.52 north; 590923.38 east 2021-02-12T23:25:06Z- AHRS_M2 msg: 1613172306.17 epoch sec; seqNo:3535 2021-02-12T23:25:06Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.13 psi 2021-02-12T23:25:06Z- handleDepth()-Depth_Keller msg: 1613172306172 epoch sec; seqNo:3570; depth 9.90 2021-02-12T23:25:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172306.18 epoch sec; seqNo:3570; 4056932.52 north; 590923.38 east 2021-02-12T23:25:06Z- AHRS_M2 msg: 1613172306.56 epoch sec; seqNo:3536 2021-02-12T23:25:06Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.12 psi 2021-02-12T23:25:06Z- handleDepth()-Depth_Keller msg: 1613172306569 epoch sec; seqNo:3571; depth 10.04 2021-02-12T23:25:06Z- handleDvl() - RDI_Pathfinder msg: 1613172306.59 epoch sec; seqNo:1654 2021-02-12T23:25:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.83 , 96.35 , 73.95 , 115.27 2021-02-12T23:25:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.24 , 0.18 2021-02-12T23:25:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172306.60 epoch sec; seqNo:3571; 4056932.90 north; 590923.37 east 2021-02-12T23:25:06Z- AHRS_M2 msg: 1613172306.97 epoch sec; seqNo:3537 2021-02-12T23:25:06Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.12 psi 2021-02-12T23:25:06Z- handleDepth()-Depth_Keller msg: 1613172306972 epoch sec; seqNo:3572; depth 10.16 2021-02-12T23:25:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:06Z- handleDvl() - RDI_Pathfinder msg: 1613172306.97 epoch sec; seqNo:1655 2021-02-12T23:25:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.24 , 0.18 2021-02-12T23:25:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172306.98 epoch sec; seqNo:3572; 4056933.28 north; 590923.37 east 2021-02-12T23:25:07Z- AHRS_M2 msg: 1613172307.37 epoch sec; seqNo:3538 2021-02-12T23:25:07Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.12 psi 2021-02-12T23:25:07Z- handleDepth()-Depth_Keller msg: 1613172307376 epoch sec; seqNo:3573; depth 10.28 2021-02-12T23:25:07Z- handleDvl() - RDI_Pathfinder msg: 1613172307.39 epoch sec; seqNo:1656 2021-02-12T23:25:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.54 , 96.35 , 74.77 , 113.92 2021-02-12T23:25:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.07 , 0.31 2021-02-12T23:25:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172307.40 epoch sec; seqNo:3573; 4056933.28 north; 590923.37 east 2021-02-12T23:25:07Z- AHRS_M2 msg: 1613172307.78 epoch sec; seqNo:3539 2021-02-12T23:25:07Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.13 psi 2021-02-12T23:25:07Z- handleDepth()-Depth_Keller msg: 1613172307782 epoch sec; seqNo:3574; depth 10.43 2021-02-12T23:25:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172307.79 epoch sec; seqNo:3574; 4056933.66 north; 590923.37 east 2021-02-12T23:25:08Z- AHRS_M2 msg: 1613172308.18 epoch sec; seqNo:3540 2021-02-12T23:25:08Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.14 psi 2021-02-12T23:25:08Z- handleDepth()-Depth_Keller msg: 1613172308184 epoch sec; seqNo:3575; depth 10.55 2021-02-12T23:25:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:08Z- handleDvl() - RDI_Pathfinder msg: 1613172308.19 epoch sec; seqNo:1657 2021-02-12T23:25:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.07 , 0.31 2021-02-12T23:25:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172308.20 epoch sec; seqNo:3575; 4056933.66 north; 590923.37 east 2021-02-12T23:25:08Z- handleDepth()-Depth_Keller msg: 1613172308609 epoch sec; seqNo:3576; depth 10.69 2021-02-12T23:25:08Z- handleDvl() - RDI_Pathfinder msg: 1613172308.62 epoch sec; seqNo:1658 2021-02-12T23:25:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.82 , 96.28 , 74.36 , 117.91 2021-02-12T23:25:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , 0.13 , 0.25 2021-02-12T23:25:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172308.63 epoch sec; seqNo:3576; 4056934.04 north; 590923.36 east 2021-02-12T23:25:08Z- LcmTrn::handleMessages() - handling 43 messages took 0.59 ms and 131 calls... 2021-02-12T23:25:08Z- waiting for 1613172306.76, time is 1613172308.63 2021-02-12T23:25:08Z- LcmTrn::updateTrn() equating pose 1613172308.63 and meas times 1613172308.63 2021-02-12T23:25:08Z- LcmTrn::updateTrn() >>>> pose time 1613172308.63, meas time 1613172308.63 <<<< 2021-02-12T23:25:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172308.63 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:25:08Z- TerrainNav::motionUpdate() - currEstimate = (4056934, 590923.36, 10.69) 2021-02-12T23:25:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:25:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:25:08Z- TNavBF::Projecting Measurements 2021-02-12T23:25:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:25:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:25:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:25:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:25:08Z- TNavFilter::4 good beams. 2021-02-12T23:25:08Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:25:08Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:25:08Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:25:08Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:25:08Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:25:08Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:25:08Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:25:08Z- TNavBF::mmse selected from filter -1 2021-02-12T23:25:08Z- TerrainMapOctree: (vehN, vehE ) = (4056934.04, 590923.36). 2021-02-12T23:25:08Z- TerrainMapOctree: Min Distance = 5449.22. 2021-02-12T23:25:08Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:25:08Z- There is no specific good beam that all particles have in common. 2021-02-12T23:25:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:25:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 12.06 2021-02-12T23:25:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:25:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 14.83 2021-02-12T23:25:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:25:08Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-12T23:25:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:25:08Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 22.24 2021-02-12T23:25:08Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:25:08Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:25:08Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:25:08Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:25:08Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:25:08Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:25:08Z- Filter 0 No beams used in subcloud update 2021-02-12T23:25:08Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:25:08Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:25:08Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 767.7 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:25:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172308.63 sec. 2021-02-12T23:25:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:25:08Z- TNavBF::mmse selected from filter -1 2021-02-12T23:25:08Z- variances([0], [2], and psi_berg) = 767.747391 833.051654 0.000000 2021-02-12T23:25:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:25:08Z- LcmTrn::publishEstimates() - MLE : 4056915.54 590874.86 10.69 2021-02-12T23:25:08Z- LcmTrn::publishEstimates() - MMSE : 4056934.04 590923.36 10.69 2021-02-12T23:25:08Z- LcmTrn::publishEstimates() - COVAR: 767.75 833.05 0.87 0.00 2021-02-12T23:25:08Z- AHRS_M2 msg: 1613172308.99 epoch sec; seqNo:3541 2021-02-12T23:25:08Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.17 psi 2021-02-12T23:25:08Z- handleDepth()-Depth_Keller msg: 1613172308993 epoch sec; seqNo:3577; depth 10.82 2021-02-12T23:25:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172309.00 epoch sec; seqNo:3577; 4056934.04 north; 590923.36 east 2021-02-12T23:25:09Z- AHRS_M2 msg: 1613172309.39 epoch sec; seqNo:3542 2021-02-12T23:25:09Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.19 psi 2021-02-12T23:25:09Z- handleDepth()-Depth_Keller msg: 1613172309398 epoch sec; seqNo:3578; depth 10.95 2021-02-12T23:25:09Z- handleDvl() - RDI_Pathfinder msg: 1613172309.41 epoch sec; seqNo:1659 2021-02-12T23:25:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 96.22 , 75.18 , 117.85 2021-02-12T23:25:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.20 , 0.10 2021-02-12T23:25:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172309.42 epoch sec; seqNo:3578; 4056934.43 north; 590924.58 east 2021-02-12T23:25:09Z- AHRS_M2 msg: 1613172309.80 epoch sec; seqNo:3543 2021-02-12T23:25:09Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.20 psi 2021-02-12T23:25:09Z- handleDepth()-Depth_Keller msg: 1613172309801 epoch sec; seqNo:3579; depth 11.08 2021-02-12T23:25:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172309.82 epoch sec; seqNo:3579; 4056934.43 north; 590924.58 east 2021-02-12T23:25:10Z- AHRS_M2 msg: 1613172310.20 epoch sec; seqNo:3544 2021-02-12T23:25:10Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.21 psi 2021-02-12T23:25:10Z- handleDepth()-Depth_Keller msg: 1613172310213 epoch sec; seqNo:3580; depth 11.19 2021-02-12T23:25:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172310.22 epoch sec; seqNo:3580; 4056934.81 north; 590924.58 east 2021-02-12T23:25:10Z- AHRS_M2 msg: 1613172310.60 epoch sec; seqNo:3545 2021-02-12T23:25:10Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.22 psi 2021-02-12T23:25:10Z- handleDepth()-Depth_Keller msg: 1613172310608 epoch sec; seqNo:3581; depth 11.33 2021-02-12T23:25:10Z- handleDvl() - RDI_Pathfinder msg: 1613172310.62 epoch sec; seqNo:1660 2021-02-12T23:25:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.70 , 104.33 , 75.18 , 119.20 2021-02-12T23:25:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.18 , -0.01 2021-02-12T23:25:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172310.63 epoch sec; seqNo:3581; 4056934.81 north; 590924.58 east 2021-02-12T23:25:11Z- AHRS_M2 msg: 1613172311.01 epoch sec; seqNo:3546 2021-02-12T23:25:11Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.23 psi 2021-02-12T23:25:11Z- handleDepth()-Depth_Keller msg: 1613172311014 epoch sec; seqNo:3582; depth 11.47 2021-02-12T23:25:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172311.03 epoch sec; seqNo:3582; 4056935.19 north; 590924.57 east 2021-02-12T23:25:11Z- AHRS_M2 msg: 1613172311.41 epoch sec; seqNo:3547 2021-02-12T23:25:11Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.24 psi 2021-02-12T23:25:11Z- handleDepth()-Depth_Keller msg: 1613172311416 epoch sec; seqNo:3583; depth 11.60 2021-02-12T23:25:11Z- handleDvl() - RDI_Pathfinder msg: 1613172311.44 epoch sec; seqNo:1661 2021-02-12T23:25:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.22 , 106.90 , 73.55 , 117.71 2021-02-12T23:25:11Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.15 , -0.07 2021-02-12T23:25:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172311.45 epoch sec; seqNo:3583; 4056935.58 north; 590925.79 east 2021-02-12T23:25:11Z- AHRS_M2 msg: 1613172311.82 epoch sec; seqNo:3548 2021-02-12T23:25:11Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.24 psi 2021-02-12T23:25:11Z- handleDepth()-Depth_Keller msg: 1613172311821 epoch sec; seqNo:3584; depth 11.75 2021-02-12T23:25:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:11Z- handleDvl() - RDI_Pathfinder msg: 1613172311.82 epoch sec; seqNo:1662 2021-02-12T23:25:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:11Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.15 , -0.07 2021-02-12T23:25:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172311.84 epoch sec; seqNo:3584; 4056935.96 north; 590925.78 east 2021-02-12T23:25:12Z- AHRS_M2 msg: 1613172312.23 epoch sec; seqNo:3549 2021-02-12T23:25:12Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.23 psi 2021-02-12T23:25:12Z- handleDepth()-Depth_Keller msg: 1613172312238 epoch sec; seqNo:3585; depth 11.87 2021-02-12T23:25:12Z- handleDvl() - RDI_Pathfinder msg: 1613172312.25 epoch sec; seqNo:1663 2021-02-12T23:25:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.86 , 106.90 , 74.78 , 116.36 2021-02-12T23:25:12Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.07 , -0.19 2021-02-12T23:25:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172312.26 epoch sec; seqNo:3585; 4056935.96 north; 590925.78 east 2021-02-12T23:25:12Z- AHRS_M2 msg: 1613172312.63 epoch sec; seqNo:3550 2021-02-12T23:25:12Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.23 psi 2021-02-12T23:25:12Z- handleDepth()-Depth_Keller msg: 1613172312638 epoch sec; seqNo:3586; depth 12.02 2021-02-12T23:25:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172312.65 epoch sec; seqNo:3586; 4056936.34 north; 590925.78 east 2021-02-12T23:25:13Z- AHRS_M2 msg: 1613172313.03 epoch sec; seqNo:3551 2021-02-12T23:25:13Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.22 psi 2021-02-12T23:25:13Z- handleDepth()-Depth_Keller msg: 1613172313032 epoch sec; seqNo:3587; depth 12.16 2021-02-12T23:25:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:13Z- handleDvl() - RDI_Pathfinder msg: 1613172313.03 epoch sec; seqNo:1664 2021-02-12T23:25:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:13Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.07 , -0.19 2021-02-12T23:25:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172313.04 epoch sec; seqNo:3587; 4056936.72 north; 590925.78 east 2021-02-12T23:25:13Z- AHRS_M2 msg: 1613172313.43 epoch sec; seqNo:3552 2021-02-12T23:25:13Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.21 psi 2021-02-12T23:25:13Z- handleDepth()-Depth_Keller msg: 1613172313436 epoch sec; seqNo:3588; depth 12.29 2021-02-12T23:25:13Z- handleDvl() - RDI_Pathfinder msg: 1613172313.45 epoch sec; seqNo:1665 2021-02-12T23:25:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 104.26 , 73.54 , 115.07 2021-02-12T23:25:13Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , -0.08 , -0.31 2021-02-12T23:25:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172313.46 epoch sec; seqNo:3588; 4056937.11 north; 590926.99 east 2021-02-12T23:25:13Z- AHRS_M2 msg: 1613172313.85 epoch sec; seqNo:3553 2021-02-12T23:25:13Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-12T23:25:13Z- handleDepth()-Depth_Keller msg: 1613172313850 epoch sec; seqNo:3589; depth 12.44 2021-02-12T23:25:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172313.86 epoch sec; seqNo:3589; 4056937.49 north; 590926.99 east 2021-02-12T23:25:13Z- LcmTrn::handleMessages() - handling 46 messages took 0.69 ms and 132 calls... 2021-02-12T23:25:13Z- waiting for 1613172311.63, time is 1613172313.86 2021-02-12T23:25:13Z- LcmTrn::updateTrn() >>>> pose time 1613172313.86, meas time 1613172313.45 <<<< 2021-02-12T23:25:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172313.45 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:25:13Z- TerrainNav::motionUpdate() - currEstimate = (4056937, 590926.99, 12.44) 2021-02-12T23:25:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:25:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:25:13Z- TNavBF::Projecting Measurements 2021-02-12T23:25:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:25:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:25:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:25:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:25:13Z- TNavFilter::4 good beams. 2021-02-12T23:25:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:25:13Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:25:13Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:25:13Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:25:13Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:25:13Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:25:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:25:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:25:13Z- TerrainMapOctree: (vehN, vehE ) = (4056937.21, 590926.70). 2021-02-12T23:25:13Z- TerrainMapOctree: Min Distance = 5444.67. 2021-02-12T23:25:13Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:25:13Z- There is no specific good beam that all particles have in common. 2021-02-12T23:25:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:25:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-12T23:25:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:25:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 17.39 2021-02-12T23:25:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:25:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 8.65 2021-02-12T23:25:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:25:14Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 21.19 2021-02-12T23:25:14Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:25:14Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:25:14Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:25:14Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:25:14Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:25:14Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:25:14Z- Filter 0 No beams used in subcloud update 2021-02-12T23:25:14Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:25:14Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:25:14Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 767.6 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:25:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172313.45 sec. 2021-02-12T23:25:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:25:14Z- TNavBF::mmse selected from filter -1 2021-02-12T23:25:14Z- variances([0], [2], and psi_berg) = 767.573738 833.051448 0.000000 2021-02-12T23:25:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:25:14Z- LcmTrn::publishEstimates() - MLE : 4056918.99 590878.49 12.44 2021-02-12T23:25:14Z- LcmTrn::publishEstimates() - MMSE : 4056937.49 590926.99 12.44 2021-02-12T23:25:14Z- LcmTrn::publishEstimates() - COVAR: 767.57 833.05 0.87 0.00 2021-02-12T23:25:14Z- AHRS_M2 msg: 1613172314.25 epoch sec; seqNo:3554 2021-02-12T23:25:14Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-12T23:25:14Z- handleDepth()-Depth_Keller msg: 1613172314252 epoch sec; seqNo:3590; depth 12.57 2021-02-12T23:25:14Z- handleDvl() - RDI_Pathfinder msg: 1613172314.29 epoch sec; seqNo:1666 2021-02-12T23:25:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 101.69 , 72.72 , 111.15 2021-02-12T23:25:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.26 , -0.28 2021-02-12T23:25:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172314.30 epoch sec; seqNo:3590; 4056937.87 north; 590926.99 east 2021-02-12T23:25:14Z- AHRS_M2 msg: 1613172314.64 epoch sec; seqNo:3555 2021-02-12T23:25:14Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.18 psi 2021-02-12T23:25:14Z- handleDepth()-Depth_Keller msg: 1613172314648 epoch sec; seqNo:3591; depth 12.72 2021-02-12T23:25:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172314.66 epoch sec; seqNo:3591; 4056938.24 north; 590926.98 east 2021-02-12T23:25:15Z- AHRS_M2 msg: 1613172315.05 epoch sec; seqNo:3556 2021-02-12T23:25:15Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.17 psi 2021-02-12T23:25:15Z- handleDepth()-Depth_Keller msg: 1613172315057 epoch sec; seqNo:3592; depth 12.86 2021-02-12T23:25:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172315.07 epoch sec; seqNo:3592; 4056938.62 north; 590926.98 east 2021-02-12T23:25:15Z- AHRS_M2 msg: 1613172315.45 epoch sec; seqNo:3557 2021-02-12T23:25:15Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.15 psi 2021-02-12T23:25:15Z- handleDepth()-Depth_Keller msg: 1613172315458 epoch sec; seqNo:3593; depth 12.99 2021-02-12T23:25:15Z- handleDvl() - RDI_Pathfinder msg: 1613172315.47 epoch sec; seqNo:1667 2021-02-12T23:25:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 96.42 , 72.72 , 109.80 2021-02-12T23:25:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.34 , -0.13 2021-02-12T23:25:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172315.48 epoch sec; seqNo:3593; 4056939.00 north; 590926.97 east 2021-02-12T23:25:15Z- AHRS_M2 msg: 1613172315.86 epoch sec; seqNo:3558 2021-02-12T23:25:15Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.15 psi 2021-02-12T23:25:15Z- handleDepth()-Depth_Keller msg: 1613172315862 epoch sec; seqNo:3594; depth 13.13 2021-02-12T23:25:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172315.87 epoch sec; seqNo:3594; 4056939.38 north; 590926.97 east 2021-02-12T23:25:16Z- AHRS_M2 msg: 1613172316.26 epoch sec; seqNo:3559 2021-02-12T23:25:16Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.14 psi 2021-02-12T23:25:16Z- handleDepth()-Depth_Keller msg: 1613172316273 epoch sec; seqNo:3595; depth 13.24 2021-02-12T23:25:16Z- handleDvl() - RDI_Pathfinder msg: 1613172316.28 epoch sec; seqNo:1668 2021-02-12T23:25:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.26 , 96.62 , 71.09 , 107.57 2021-02-12T23:25:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.26 , 0.01 2021-02-12T23:25:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172316.29 epoch sec; seqNo:3595; 4056939.76 north; 590926.97 east 2021-02-12T23:25:16Z- AHRS_M2 msg: 1613172316.67 epoch sec; seqNo:3560 2021-02-12T23:25:16Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.15 psi 2021-02-12T23:25:16Z- handleDepth()-Depth_Keller msg: 1613172316676 epoch sec; seqNo:3596; depth 13.36 2021-02-12T23:25:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:16Z- handleDvl() - RDI_Pathfinder msg: 1613172316.68 epoch sec; seqNo:1669 2021-02-12T23:25:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.26 , 0.01 2021-02-12T23:25:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172316.70 epoch sec; seqNo:3596; 4056940.14 north; 590926.96 east 2021-02-12T23:25:17Z- AHRS_M2 msg: 1613172317.07 epoch sec; seqNo:3561 2021-02-12T23:25:17Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.16 psi 2021-02-12T23:25:17Z- handleDepth()-Depth_Keller msg: 1613172317072 epoch sec; seqNo:3597; depth 13.49 2021-02-12T23:25:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172317.08 epoch sec; seqNo:3597; 4056940.14 north; 590926.96 east 2021-02-12T23:25:17Z- AHRS_M2 msg: 1613172317.47 epoch sec; seqNo:3562 2021-02-12T23:25:17Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.18 psi 2021-02-12T23:25:17Z- handleDepth()-Depth_Keller msg: 1613172317481 epoch sec; seqNo:3598; depth 13.61 2021-02-12T23:25:17Z- handleDvl() - RDI_Pathfinder msg: 1613172317.49 epoch sec; seqNo:1670 2021-02-12T23:25:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 97.84 , 69.86 , 111.22 2021-02-12T23:25:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.17 , 0.06 2021-02-12T23:25:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172317.50 epoch sec; seqNo:3598; 4056940.52 north; 590926.96 east 2021-02-12T23:25:17Z- AHRS_M2 msg: 1613172317.88 epoch sec; seqNo:3563 2021-02-12T23:25:17Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.18 psi 2021-02-12T23:25:17Z- handleDepth()-Depth_Keller msg: 1613172317889 epoch sec; seqNo:3599; depth 13.73 2021-02-12T23:25:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:17Z- handleDvl() - RDI_Pathfinder msg: 1613172317.89 epoch sec; seqNo:1671 2021-02-12T23:25:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.17 , 0.06 2021-02-12T23:25:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172317.90 epoch sec; seqNo:3599; 4056940.90 north; 590926.95 east 2021-02-12T23:25:18Z- AHRS_M2 msg: 1613172318.28 epoch sec; seqNo:3564 2021-02-12T23:25:18Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:18Z- handleDepth()-Depth_Keller msg: 1613172318289 epoch sec; seqNo:3600; depth 13.84 2021-02-12T23:25:18Z- handleDvl() - RDI_Pathfinder msg: 1613172318.31 epoch sec; seqNo:1672 2021-02-12T23:25:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 96.62 , 71.91 , 110.00 2021-02-12T23:25:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.10 , 0.06 2021-02-12T23:25:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172318.32 epoch sec; seqNo:3600; 4056940.91 north; 590928.17 east 2021-02-12T23:25:18Z- AHRS_M2 msg: 1613172318.68 epoch sec; seqNo:3565 2021-02-12T23:25:18Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:25:18Z- handleDepth()-Depth_Keller msg: 1613172318693 epoch sec; seqNo:3601; depth 13.97 2021-02-12T23:25:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172318.70 epoch sec; seqNo:3601; 4056941.29 north; 590928.17 east 2021-02-12T23:25:19Z- LcmTrn::handleMessages() - handling 43 messages took 0.66 ms and 130 calls... 2021-02-12T23:25:19Z- waiting for 1613172316.86, time is 1613172318.70 2021-02-12T23:25:19Z- LcmTrn::updateTrn() >>>> pose time 1613172318.70, meas time 1613172318.31 <<<< 2021-02-12T23:25:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172318.31 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:25:19Z- TerrainNav::motionUpdate() - currEstimate = (4056941, 590928.17, 13.97) 2021-02-12T23:25:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:25:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:25:19Z- TNavBF::Projecting Measurements 2021-02-12T23:25:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:25:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:25:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:25:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:25:19Z- TNavFilter::4 good beams. 2021-02-12T23:25:19Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:25:19Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:25:19Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:25:19Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:25:19Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:25:19Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:25:19Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:25:19Z- TNavBF::mmse selected from filter -1 2021-02-12T23:25:19Z- TerrainMapOctree: (vehN, vehE ) = (4056940.98, 590928.07). 2021-02-12T23:25:19Z- TerrainMapOctree: Min Distance = 5440.85. 2021-02-12T23:25:19Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:25:19Z- There is no specific good beam that all particles have in common. 2021-02-12T23:25:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:25:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 9.95 2021-02-12T23:25:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:25:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 14.94 2021-02-12T23:25:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:25:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 8.27 2021-02-12T23:25:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:25:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 19.36 2021-02-12T23:25:19Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:25:19Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:25:19Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:25:19Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:25:19Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:25:19Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:25:19Z- Filter 0 No beams used in subcloud update 2021-02-12T23:25:19Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:25:19Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:25:19Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 767.5 EastVariance: 833.1 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:25:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172318.31 sec. 2021-02-12T23:25:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:25:19Z- TNavBF::mmse selected from filter -1 2021-02-12T23:25:19Z- variances([0], [2], and psi_berg) = 767.418814 833.051265 0.000000 2021-02-12T23:25:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:25:19Z- LcmTrn::publishEstimates() - MLE : 4056922.79 590879.67 13.97 2021-02-12T23:25:19Z- LcmTrn::publishEstimates() - MMSE : 4056941.29 590928.17 13.97 2021-02-12T23:25:19Z- LcmTrn::publishEstimates() - COVAR: 767.42 833.05 0.82 0.00 2021-02-12T23:25:19Z- AHRS_M2 msg: 1613172319.09 epoch sec; seqNo:3566 2021-02-12T23:25:19Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.19 psi 2021-02-12T23:25:19Z- handleDepth()-Depth_Keller msg: 1613172319093 epoch sec; seqNo:3602; depth 14.10 2021-02-12T23:25:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:19Z- handleDvl() - RDI_Pathfinder msg: 1613172319.10 epoch sec; seqNo:1673 2021-02-12T23:25:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.10 , 0.06 2021-02-12T23:25:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172319.10 epoch sec; seqNo:3602; 4056941.67 north; 590928.17 east 2021-02-12T23:25:19Z- AHRS_M2 msg: 1613172319.49 epoch sec; seqNo:3567 2021-02-12T23:25:19Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.19 psi 2021-02-12T23:25:19Z- handleDepth()-Depth_Keller msg: 1613172319498 epoch sec; seqNo:3603; depth 14.21 2021-02-12T23:25:19Z- handleDvl() - RDI_Pathfinder msg: 1613172319.51 epoch sec; seqNo:1674 2021-02-12T23:25:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.40 , 96.62 , 72.32 , 108.79 2021-02-12T23:25:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.12 , 0.07 2021-02-12T23:25:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172319.52 epoch sec; seqNo:3603; 4056941.67 north; 590928.17 east 2021-02-12T23:25:19Z- AHRS_M2 msg: 1613172319.90 epoch sec; seqNo:3568 2021-02-12T23:25:19Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.19 psi 2021-02-12T23:25:19Z- handleDepth()-Depth_Keller msg: 1613172319905 epoch sec; seqNo:3604; depth 14.34 2021-02-12T23:25:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172319.92 epoch sec; seqNo:3604; 4056942.05 north; 590928.16 east 2021-02-12T23:25:20Z- AHRS_M2 msg: 1613172320.30 epoch sec; seqNo:3569 2021-02-12T23:25:20Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.19 psi 2021-02-12T23:25:20Z- handleDepth()-Depth_Keller msg: 1613172320304 epoch sec; seqNo:3605; depth 14.46 2021-02-12T23:25:20Z- handleDvl() - RDI_Pathfinder msg: 1613172320.32 epoch sec; seqNo:1675 2021-02-12T23:25:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.22 , 96.69 , 71.91 , 108.85 2021-02-12T23:25:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.12 , 0.11 2021-02-12T23:25:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172320.35 epoch sec; seqNo:3605; 4056942.42 north; 590928.16 east 2021-02-12T23:25:20Z- AHRS_M2 msg: 1613172320.70 epoch sec; seqNo:3570 2021-02-12T23:25:20Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.19 psi 2021-02-12T23:25:20Z- handleDepth()-Depth_Keller msg: 1613172320713 epoch sec; seqNo:3606; depth 14.61 2021-02-12T23:25:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172320.72 epoch sec; seqNo:3606; 4056942.80 north; 590928.15 east 2021-02-12T23:25:21Z- AHRS_M2 msg: 1613172321.11 epoch sec; seqNo:3571 2021-02-12T23:25:21Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:21Z- handleDepth()-Depth_Keller msg: 1613172321121 epoch sec; seqNo:3607; depth 14.72 2021-02-12T23:25:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172321.14 epoch sec; seqNo:3607; 4056942.80 north; 590928.15 east 2021-02-12T23:25:21Z- AHRS_M2 msg: 1613172321.51 epoch sec; seqNo:3572 2021-02-12T23:25:21Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-12T23:25:21Z- handleDepth()-Depth_Keller msg: 1613172321516 epoch sec; seqNo:3608; depth 14.84 2021-02-12T23:25:21Z- handleDvl() - RDI_Pathfinder msg: 1613172321.54 epoch sec; seqNo:1676 2021-02-12T23:25:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.22 , 96.62 , 71.50 , 108.79 2021-02-12T23:25:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.05 , 0.15 2021-02-12T23:25:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172321.55 epoch sec; seqNo:3608; 4056943.18 north; 590928.15 east 2021-02-12T23:25:21Z- AHRS_M2 msg: 1613172321.92 epoch sec; seqNo:3573 2021-02-12T23:25:21Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-12T23:25:21Z- handleDepth()-Depth_Keller msg: 1613172321920 epoch sec; seqNo:3609; depth 14.97 2021-02-12T23:25:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172321.93 epoch sec; seqNo:3609; 4056943.20 north; 590929.37 east 2021-02-12T23:25:22Z- AHRS_M2 msg: 1613172322.32 epoch sec; seqNo:3574 2021-02-12T23:25:22Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.16 psi 2021-02-12T23:25:22Z- handleDepth()-Depth_Keller msg: 1613172322329 epoch sec; seqNo:3610; depth 15.07 2021-02-12T23:25:22Z- handleDvl() - RDI_Pathfinder msg: 1613172322.34 epoch sec; seqNo:1677 2021-02-12T23:25:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 96.69 , 69.86 , 101.56 2021-02-12T23:25:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.04 , 0.16 2021-02-12T23:25:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172322.35 epoch sec; seqNo:3610; 4056943.57 north; 590929.37 east 2021-02-12T23:25:22Z- AHRS_M2 msg: 1613172322.72 epoch sec; seqNo:3575 2021-02-12T23:25:22Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.16 psi 2021-02-12T23:25:22Z- handleDepth()-Depth_Keller msg: 1613172322730 epoch sec; seqNo:3611; depth 15.21 2021-02-12T23:25:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:22Z- handleDvl() - RDI_Pathfinder msg: 1613172322.73 epoch sec; seqNo:1678 2021-02-12T23:25:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.04 , 0.16 2021-02-12T23:25:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172322.74 epoch sec; seqNo:3611; 4056943.95 north; 590929.36 east 2021-02-12T23:25:23Z- AHRS_M2 msg: 1613172323.13 epoch sec; seqNo:3576 2021-02-12T23:25:23Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.16 psi 2021-02-12T23:25:23Z- handleDepth()-Depth_Keller msg: 1613172323137 epoch sec; seqNo:3612; depth 15.33 2021-02-12T23:25:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172323.16 epoch sec; seqNo:3612; 4056943.95 north; 590929.36 east 2021-02-12T23:25:23Z- AHRS_M2 msg: 1613172323.53 epoch sec; seqNo:3577 2021-02-12T23:25:23Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.16 psi 2021-02-12T23:25:23Z- handleDepth()-Depth_Keller msg: 1613172323536 epoch sec; seqNo:3613; depth 15.45 2021-02-12T23:25:23Z- handleDvl() - RDI_Pathfinder msg: 1613172323.55 epoch sec; seqNo:1679 2021-02-12T23:25:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.64 , 95.61 , 69.46 , 107.78 2021-02-12T23:25:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.10 , 0.09 2021-02-12T23:25:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172323.59 epoch sec; seqNo:3613; 4056944.33 north; 590929.36 east 2021-02-12T23:25:23Z- AHRS_M2 msg: 1613172323.94 epoch sec; seqNo:3578 2021-02-12T23:25:23Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.15 psi 2021-02-12T23:25:23Z- handleDepth()-Depth_Keller msg: 1613172323940 epoch sec; seqNo:3614; depth 15.57 2021-02-12T23:25:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:23Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:23Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:23Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:23Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:23Z- handleDvl() - RDI_Pathfinder msg: 1613172323.94 epoch sec; seqNo:1680 2021-02-12T23:25:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.10 , 0.09 2021-02-12T23:25:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172323.96 epoch sec; seqNo:3614; 4056944.71 north; 590929.36 east 2021-02-12T23:25:24Z- LcmTrn::handleMessages() - handling 47 messages took 0.72 ms and 133 calls... 2021-02-12T23:25:24Z- waiting for 1613172321.70, time is 1613172323.96 2021-02-12T23:25:24Z- LcmTrn::updateTrn() equating pose 1613172323.96 and meas times 1613172323.96 2021-02-12T23:25:24Z- LcmTrn::updateTrn() >>>> pose time 1613172323.96, meas time 1613172323.96 <<<< 2021-02-12T23:25:24Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172323.96, ping # 0. 2021-02-12T23:25:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172323.96 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:25:24Z- TerrainNav::motionUpdate() - currEstimate = (4056945, 590929.36, 15.57) 2021-02-12T23:25:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:25:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:25:24Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:25:24Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:25:24Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172323.96 sec.; No relevant navigation data available 2021-02-12T23:25:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:25:24Z- TNavBF::mmse selected from filter -1 2021-02-12T23:25:24Z- variances([0], [2], and psi_berg) = 767.300292 833.051125 0.000000 2021-02-12T23:25:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:25:24Z- LcmTrn::publishEstimates() - MLE : 4056926.21 590880.86 15.57 2021-02-12T23:25:24Z- LcmTrn::publishEstimates() - MMSE : 4056944.71 590929.36 15.57 2021-02-12T23:25:24Z- LcmTrn::publishEstimates() - COVAR: 767.30 833.05 1.01 0.00 2021-02-12T23:25:24Z- AHRS_M2 msg: 1613172324.34 epoch sec; seqNo:3579 2021-02-12T23:25:24Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.15 psi 2021-02-12T23:25:24Z- handleDepth()-Depth_Keller msg: 1613172324353 epoch sec; seqNo:3615; depth 15.70 2021-02-12T23:25:24Z- handleDvl() - RDI_Pathfinder msg: 1613172324.36 epoch sec; seqNo:1681 2021-02-12T23:25:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.73 , 95.54 , 69.87 , 108.93 2021-02-12T23:25:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.13 , -0.02 2021-02-12T23:25:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172324.38 epoch sec; seqNo:3615; 4056944.72 north; 590930.58 east 2021-02-12T23:25:24Z- AHRS_M2 msg: 1613172324.74 epoch sec; seqNo:3580 2021-02-12T23:25:24Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.15 psi 2021-02-12T23:25:24Z- handleDepth()-Depth_Keller msg: 1613172324753 epoch sec; seqNo:3616; depth 15.85 2021-02-12T23:25:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172324.77 epoch sec; seqNo:3616; 4056945.10 north; 590930.57 east 2021-02-12T23:25:25Z- AHRS_M2 msg: 1613172325.15 epoch sec; seqNo:3581 2021-02-12T23:25:25Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.14 psi 2021-02-12T23:25:25Z- handleDepth()-Depth_Keller msg: 1613172325157 epoch sec; seqNo:3617; depth 15.96 2021-02-12T23:25:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:25Z- handleDvl() - RDI_Pathfinder msg: 1613172325.16 epoch sec; seqNo:1682 2021-02-12T23:25:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.13 , -0.02 2021-02-12T23:25:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172325.17 epoch sec; seqNo:3617; 4056945.48 north; 590930.57 east 2021-02-12T23:25:25Z- handleDepth()-Depth_Keller msg: 1613172325573 epoch sec; seqNo:3618; depth 16.09 2021-02-12T23:25:25Z- handleDvl() - RDI_Pathfinder msg: 1613172325.59 epoch sec; seqNo:1683 2021-02-12T23:25:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.63 , 93.04 , 70.27 , 107.64 2021-02-12T23:25:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.07 , -0.17 2021-02-12T23:25:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172325.60 epoch sec; seqNo:3618; 4056945.48 north; 590930.57 east 2021-02-12T23:25:25Z- AHRS_M2 msg: 1613172325.96 epoch sec; seqNo:3582 2021-02-12T23:25:25Z- AHRS_M2 msg: 0.70 phi; -0.34 theta; -0.14 psi 2021-02-12T23:25:25Z- handleDepth()-Depth_Keller msg: 1613172325972 epoch sec; seqNo:3619; depth 16.24 2021-02-12T23:25:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172325.99 epoch sec; seqNo:3619; 4056945.87 north; 590931.79 east 2021-02-12T23:25:26Z- AHRS_M2 msg: 1613172326.36 epoch sec; seqNo:3583 2021-02-12T23:25:26Z- AHRS_M2 msg: 0.70 phi; -0.34 theta; -0.15 psi 2021-02-12T23:25:26Z- handleDepth()-Depth_Keller msg: 1613172326364 epoch sec; seqNo:3620; depth 16.36 2021-02-12T23:25:26Z- handleDvl() - RDI_Pathfinder msg: 1613172326.38 epoch sec; seqNo:1684 2021-02-12T23:25:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.81 , 93.11 , 69.05 , 105.27 2021-02-12T23:25:26Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.03 , -0.28 2021-02-12T23:25:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172326.40 epoch sec; seqNo:3620; 4056946.25 north; 590931.78 east 2021-02-12T23:25:26Z- AHRS_M2 msg: 1613172326.76 epoch sec; seqNo:3584 2021-02-12T23:25:26Z- AHRS_M2 msg: 0.70 phi; -0.34 theta; -0.16 psi 2021-02-12T23:25:26Z- handleDepth()-Depth_Keller msg: 1613172326773 epoch sec; seqNo:3621; depth 16.48 2021-02-12T23:25:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172326.79 epoch sec; seqNo:3621; 4056946.63 north; 590931.78 east 2021-02-12T23:25:27Z- AHRS_M2 msg: 1613172327.20 epoch sec; seqNo:3585 2021-02-12T23:25:27Z- AHRS_M2 msg: 0.71 phi; -0.34 theta; -0.17 psi 2021-02-12T23:25:27Z- handleDepth()-Depth_Keller msg: 1613172327202 epoch sec; seqNo:3622; depth 16.60 2021-02-12T23:25:27Z- handleDvl() - RDI_Pathfinder msg: 1613172327.21 epoch sec; seqNo:1685 2021-02-12T23:25:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.59 , 94.46 , 69.46 , 104.20 2021-02-12T23:25:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.17 , -0.25 2021-02-12T23:25:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172327.24 epoch sec; seqNo:3622; 4056947.01 north; 590931.77 east 2021-02-12T23:25:27Z- AHRS_M2 msg: 1613172327.57 epoch sec; seqNo:3586 2021-02-12T23:25:27Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:27Z- handleDepth()-Depth_Keller msg: 1613172327576 epoch sec; seqNo:3623; depth 16.75 2021-02-12T23:25:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:27Z- handleDvl() - RDI_Pathfinder msg: 1613172327.58 epoch sec; seqNo:1686 2021-02-12T23:25:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.17 , -0.25 2021-02-12T23:25:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172327.60 epoch sec; seqNo:3623; 4056947.39 north; 590931.77 east 2021-02-12T23:25:27Z- AHRS_M2 msg: 1613172327.98 epoch sec; seqNo:3587 2021-02-12T23:25:27Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:27Z- handleDepth()-Depth_Keller msg: 1613172327980 epoch sec; seqNo:3624; depth 16.86 2021-02-12T23:25:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172327.99 epoch sec; seqNo:3624; 4056947.77 north; 590931.77 east 2021-02-12T23:25:28Z- AHRS_M2 msg: 1613172328.38 epoch sec; seqNo:3588 2021-02-12T23:25:28Z- AHRS_M2 msg: 0.71 phi; -0.34 theta; -0.18 psi 2021-02-12T23:25:28Z- handleDepth()-Depth_Keller msg: 1613172328390 epoch sec; seqNo:3625; depth 16.99 2021-02-12T23:25:28Z- handleDvl() - RDI_Pathfinder msg: 1613172328.40 epoch sec; seqNo:1687 2021-02-12T23:25:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 94.46 , 69.46 , 104.20 2021-02-12T23:25:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.20 , -0.11 2021-02-12T23:25:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172328.42 epoch sec; seqNo:3625; 4056948.16 north; 590932.98 east 2021-02-12T23:25:28Z- AHRS_M2 msg: 1613172328.78 epoch sec; seqNo:3589 2021-02-12T23:25:28Z- AHRS_M2 msg: 0.71 phi; -0.34 theta; -0.18 psi 2021-02-12T23:25:28Z- handleDepth()-Depth_Keller msg: 1613172328788 epoch sec; seqNo:3626; depth 17.13 2021-02-12T23:25:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:28Z- handleDvl() - RDI_Pathfinder msg: 1613172328.79 epoch sec; seqNo:1688 2021-02-12T23:25:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.20 , -0.11 2021-02-12T23:25:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172328.81 epoch sec; seqNo:3626; 4056948.16 north; 590932.98 east 2021-02-12T23:25:29Z- AHRS_M2 msg: 1613172329.21 epoch sec; seqNo:3590 2021-02-12T23:25:29Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.19 psi 2021-02-12T23:25:29Z- LcmTrn::handleMessages() - handling 44 messages took 0.63 ms and 132 calls... 2021-02-12T23:25:29Z- waiting for 1613172326.96, time is 1613172329.21 2021-02-12T23:25:29Z- LcmTrn::updateTrn() >>>> pose time 1613172329.21, meas time 1613172328.79 <<<< 2021-02-12T23:25:29Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172328.79, ping # 0. 2021-02-12T23:25:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172328.79 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:25:29Z- TerrainNav::motionUpdate() - currEstimate = (4056948, 590932.98, 17.13) 2021-02-12T23:25:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:25:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:25:29Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:25:29Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:25:29Z- TNavBF::Projecting Measurements 2021-02-12T23:25:29Z- TNavFilter::Measurement from time = 1613172328.79 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:25:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:25:29Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:25:29Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:25:29Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:25:29Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:25:29Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:25:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:25:29Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 767.2 EastVariance: 833.1 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:25:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:25:29Z- TNavBF::mmse selected from filter -1 2021-02-12T23:25:29Z- variances([0], [2], and psi_berg) = 767.127649 833.050922 0.000000 2021-02-12T23:25:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:25:29Z- LcmTrn::publishEstimates() - MLE : 4056929.66 590884.48 17.13 2021-02-12T23:25:29Z- LcmTrn::publishEstimates() - MMSE : 4056948.16 590932.98 17.13 2021-02-12T23:25:29Z- LcmTrn::publishEstimates() - COVAR: 767.13 833.05 1.26 0.00 2021-02-12T23:25:29Z- handleDepth()-Depth_Keller msg: 1613172329215 epoch sec; seqNo:3627; depth 17.25 2021-02-12T23:25:29Z- handleDvl() - RDI_Pathfinder msg: 1613172329.23 epoch sec; seqNo:1689 2021-02-12T23:25:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.36 , 95.68 , 69.05 , 105.41 2021-02-12T23:25:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.12 , 0.02 2021-02-12T23:25:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172329.25 epoch sec; seqNo:3627; 4056948.54 north; 590932.98 east 2021-02-12T23:25:29Z- AHRS_M2 msg: 1613172329.60 epoch sec; seqNo:3591 2021-02-12T23:25:29Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.19 psi 2021-02-12T23:25:29Z- handleDepth()-Depth_Keller msg: 1613172329602 epoch sec; seqNo:3628; depth 17.40 2021-02-12T23:25:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172329.61 epoch sec; seqNo:3628; 4056948.92 north; 590932.97 east 2021-02-12T23:25:30Z- AHRS_M2 msg: 1613172330.00 epoch sec; seqNo:3592 2021-02-12T23:25:30Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.20 psi 2021-02-12T23:25:30Z- handleDepth()-Depth_Keller msg: 1613172330002 epoch sec; seqNo:3629; depth 17.50 2021-02-12T23:25:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:30Z- handleDvl() - RDI_Pathfinder msg: 1613172330.00 epoch sec; seqNo:1690 2021-02-12T23:25:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.12 , 0.02 2021-02-12T23:25:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172330.01 epoch sec; seqNo:3629; 4056948.92 north; 590932.97 east 2021-02-12T23:25:30Z- AHRS_M2 msg: 1613172330.40 epoch sec; seqNo:3593 2021-02-12T23:25:30Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.20 psi 2021-02-12T23:25:30Z- handleDepth()-Depth_Keller msg: 1613172330413 epoch sec; seqNo:3630; depth 17.63 2021-02-12T23:25:30Z- handleDvl() - RDI_Pathfinder msg: 1613172330.43 epoch sec; seqNo:1691 2021-02-12T23:25:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.18 , 94.46 , 68.64 , 105.41 2021-02-12T23:25:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , 0.01 , 0.09 2021-02-12T23:25:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172330.46 epoch sec; seqNo:3630; 4056949.30 north; 590932.97 east 2021-02-12T23:25:30Z- AHRS_M2 msg: 1613172330.80 epoch sec; seqNo:3594 2021-02-12T23:25:30Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.21 psi 2021-02-12T23:25:30Z- handleDepth()-Depth_Keller msg: 1613172330813 epoch sec; seqNo:3631; depth 17.75 2021-02-12T23:25:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172330.82 epoch sec; seqNo:3631; 4056949.30 north; 590932.97 east 2021-02-12T23:25:31Z- AHRS_M2 msg: 1613172331.22 epoch sec; seqNo:3595 2021-02-12T23:25:31Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.22 psi 2021-02-12T23:25:31Z- handleDepth()-Depth_Keller msg: 1613172331226 epoch sec; seqNo:3632; depth 17.85 2021-02-12T23:25:31Z- handleDvl() - RDI_Pathfinder msg: 1613172331.24 epoch sec; seqNo:1692 2021-02-12T23:25:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 96.90 , 68.24 , 106.63 2021-02-12T23:25:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.04 , 0.08 2021-02-12T23:25:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172331.25 epoch sec; seqNo:3632; 4056949.67 north; 590932.97 east 2021-02-12T23:25:31Z- AHRS_M2 msg: 1613172331.62 epoch sec; seqNo:3596 2021-02-12T23:25:31Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.22 psi 2021-02-12T23:25:31Z- handleDepth()-Depth_Keller msg: 1613172331623 epoch sec; seqNo:3633; depth 17.97 2021-02-12T23:25:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172331.65 epoch sec; seqNo:3633; 4056950.07 north; 590934.18 east 2021-02-12T23:25:32Z- AHRS_M2 msg: 1613172332.02 epoch sec; seqNo:3597 2021-02-12T23:25:32Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.23 psi 2021-02-12T23:25:32Z- handleDepth()-Depth_Keller msg: 1613172332023 epoch sec; seqNo:3634; depth 18.10 2021-02-12T23:25:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172332.03 epoch sec; seqNo:3634; 4056950.07 north; 590934.18 east 2021-02-12T23:25:32Z- AHRS_M2 msg: 1613172332.42 epoch sec; seqNo:3598 2021-02-12T23:25:32Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.23 psi 2021-02-12T23:25:32Z- handleDepth()-Depth_Keller msg: 1613172332426 epoch sec; seqNo:3635; depth 18.20 2021-02-12T23:25:32Z- handleDvl() - RDI_Pathfinder msg: 1613172332.44 epoch sec; seqNo:1693 2021-02-12T23:25:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.73 , 96.83 , 69.06 , 106.56 2021-02-12T23:25:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , 0.05 2021-02-12T23:25:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172332.45 epoch sec; seqNo:3635; 4056950.44 north; 590934.18 east 2021-02-12T23:25:32Z- AHRS_M2 msg: 1613172332.82 epoch sec; seqNo:3599 2021-02-12T23:25:32Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.23 psi 2021-02-12T23:25:32Z- handleDepth()-Depth_Keller msg: 1613172332828 epoch sec; seqNo:3636; depth 18.33 2021-02-12T23:25:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172332.85 epoch sec; seqNo:3636; 4056950.44 north; 590934.18 east 2021-02-12T23:25:33Z- AHRS_M2 msg: 1613172333.26 epoch sec; seqNo:3600 2021-02-12T23:25:33Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.23 psi 2021-02-12T23:25:33Z- handleDepth()-Depth_Keller msg: 1613172333260 epoch sec; seqNo:3637; depth 18.45 2021-02-12T23:25:33Z- handleDvl() - RDI_Pathfinder msg: 1613172333.28 epoch sec; seqNo:1694 2021-02-12T23:25:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.14 , 96.83 , 68.65 , 106.56 2021-02-12T23:25:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.04 2021-02-12T23:25:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172333.29 epoch sec; seqNo:3637; 4056950.82 north; 590934.17 east 2021-02-12T23:25:33Z- AHRS_M2 msg: 1613172333.64 epoch sec; seqNo:3601 2021-02-12T23:25:33Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.24 psi 2021-02-12T23:25:33Z- handleDepth()-Depth_Keller msg: 1613172333654 epoch sec; seqNo:3638; depth 18.59 2021-02-12T23:25:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:33Z- handleDvl() - RDI_Pathfinder msg: 1613172333.66 epoch sec; seqNo:1695 2021-02-12T23:25:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.04 2021-02-12T23:25:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172333.67 epoch sec; seqNo:3638; 4056951.20 north; 590934.17 east 2021-02-12T23:25:34Z- AHRS_M2 msg: 1613172334.04 epoch sec; seqNo:3602 2021-02-12T23:25:34Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.24 psi 2021-02-12T23:25:34Z- handleDepth()-Depth_Keller msg: 1613172334044 epoch sec; seqNo:3639; depth 18.70 2021-02-12T23:25:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172334.05 epoch sec; seqNo:3639; 4056951.59 north; 590935.39 east 2021-02-12T23:25:34Z- LcmTrn::handleMessages() - handling 45 messages took 0.72 ms and 131 calls... 2021-02-12T23:25:34Z- waiting for 1613172332.21, time is 1613172334.05 2021-02-12T23:25:34Z- LcmTrn::updateTrn() >>>> pose time 1613172334.05, meas time 1613172333.66 <<<< 2021-02-12T23:25:34Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172333.66, ping # 0. 2021-02-12T23:25:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172333.66 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:25:34Z- TerrainNav::motionUpdate() - currEstimate = (4056952, 590935.39, 18.70) 2021-02-12T23:25:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:25:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:25:34Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:25:34Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:25:34Z- TNavBF::Projecting Measurements 2021-02-12T23:25:34Z- TNavFilter::Measurement from time = 1613172333.66 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:25:34Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:25:34Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:25:34Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:25:34Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:25:34Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:25:34Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:25:34Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:25:34Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 767.0 EastVariance: 833.1 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-12T23:25:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:25:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:25:34Z- variances([0], [2], and psi_berg) = 766.969697 833.050737 0.000000 2021-02-12T23:25:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:25:34Z- LcmTrn::publishEstimates() - MLE : 4056933.09 590886.89 18.70 2021-02-12T23:25:34Z- LcmTrn::publishEstimates() - MMSE : 4056951.59 590935.39 18.70 2021-02-12T23:25:34Z- LcmTrn::publishEstimates() - COVAR: 766.97 833.05 1.48 0.00 2021-02-12T23:25:34Z- AHRS_M2 msg: 1613172334.44 epoch sec; seqNo:3603 2021-02-12T23:25:34Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.24 psi 2021-02-12T23:25:34Z- handleDepth()-Depth_Keller msg: 1613172334453 epoch sec; seqNo:3640; depth 18.82 2021-02-12T23:25:34Z- handleDvl() - RDI_Pathfinder msg: 1613172334.46 epoch sec; seqNo:1696 2021-02-12T23:25:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 96.83 , 67.42 , 104.13 2021-02-12T23:25:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.06 2021-02-12T23:25:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172334.47 epoch sec; seqNo:3640; 4056951.59 north; 590935.39 east 2021-02-12T23:25:34Z- AHRS_M2 msg: 1613172334.85 epoch sec; seqNo:3604 2021-02-12T23:25:34Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.23 psi 2021-02-12T23:25:34Z- handleDepth()-Depth_Keller msg: 1613172334854 epoch sec; seqNo:3641; depth 18.96 2021-02-12T23:25:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:34Z- handleDvl() - RDI_Pathfinder msg: 1613172334.86 epoch sec; seqNo:1697 2021-02-12T23:25:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.06 2021-02-12T23:25:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172334.87 epoch sec; seqNo:3641; 4056951.97 north; 590935.38 east 2021-02-12T23:25:35Z- AHRS_M2 msg: 1613172335.25 epoch sec; seqNo:3605 2021-02-12T23:25:35Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.23 psi 2021-02-12T23:25:35Z- handleDepth()-Depth_Keller msg: 1613172335266 epoch sec; seqNo:3642; depth 19.07 2021-02-12T23:25:35Z- handleDvl() - RDI_Pathfinder msg: 1613172335.28 epoch sec; seqNo:1698 2021-02-12T23:25:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.73 , 96.96 , 67.83 , 103.05 2021-02-12T23:25:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.06 2021-02-12T23:25:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172335.29 epoch sec; seqNo:3642; 4056952.35 north; 590935.38 east 2021-02-12T23:25:35Z- AHRS_M2 msg: 1613172335.66 epoch sec; seqNo:3606 2021-02-12T23:25:35Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.23 psi 2021-02-12T23:25:35Z- handleDepth()-Depth_Keller msg: 1613172335660 epoch sec; seqNo:3643; depth 19.21 2021-02-12T23:25:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172335.67 epoch sec; seqNo:3643; 4056952.35 north; 590935.38 east 2021-02-12T23:25:36Z- AHRS_M2 msg: 1613172336.06 epoch sec; seqNo:3607 2021-02-12T23:25:36Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.22 psi 2021-02-12T23:25:36Z- handleDepth()-Depth_Keller msg: 1613172336064 epoch sec; seqNo:3644; depth 19.32 2021-02-12T23:25:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:36Z- handleDvl() - RDI_Pathfinder msg: 1613172336.07 epoch sec; seqNo:1699 2021-02-12T23:25:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.06 2021-02-12T23:25:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172336.07 epoch sec; seqNo:3644; 4056952.73 north; 590935.38 east 2021-02-12T23:25:36Z- AHRS_M2 msg: 1613172336.46 epoch sec; seqNo:3608 2021-02-12T23:25:36Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.22 psi 2021-02-12T23:25:36Z- handleDepth()-Depth_Keller msg: 1613172336477 epoch sec; seqNo:3645; depth 19.45 2021-02-12T23:25:36Z- handleDvl() - RDI_Pathfinder msg: 1613172336.49 epoch sec; seqNo:1700 2021-02-12T23:25:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.73 , 94.53 , 67.01 , 104.26 2021-02-12T23:25:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , 0.02 2021-02-12T23:25:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172336.50 epoch sec; seqNo:3645; 4056953.12 north; 590936.59 east 2021-02-12T23:25:36Z- AHRS_M2 msg: 1613172336.87 epoch sec; seqNo:3609 2021-02-12T23:25:36Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.22 psi 2021-02-12T23:25:36Z- handleDepth()-Depth_Keller msg: 1613172336877 epoch sec; seqNo:3646; depth 19.58 2021-02-12T23:25:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172336.89 epoch sec; seqNo:3646; 4056953.50 north; 590936.59 east 2021-02-12T23:25:37Z- AHRS_M2 msg: 1613172337.28 epoch sec; seqNo:3610 2021-02-12T23:25:37Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.21 psi 2021-02-12T23:25:37Z- handleDepth()-Depth_Keller msg: 1613172337279 epoch sec; seqNo:3647; depth 19.70 2021-02-12T23:25:37Z- handleDvl() - RDI_Pathfinder msg: 1613172337.29 epoch sec; seqNo:1701 2021-02-12T23:25:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.32 , 94.60 , 67.42 , 101.90 2021-02-12T23:25:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.05 , -0.03 2021-02-12T23:25:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172337.32 epoch sec; seqNo:3647; 4056953.50 north; 590936.59 east 2021-02-12T23:25:37Z- AHRS_M2 msg: 1613172337.68 epoch sec; seqNo:3611 2021-02-12T23:25:37Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.21 psi 2021-02-12T23:25:37Z- handleDepth()-Depth_Keller msg: 1613172337680 epoch sec; seqNo:3648; depth 19.83 2021-02-12T23:25:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172337.69 epoch sec; seqNo:3648; 4056953.88 north; 590936.58 east 2021-02-12T23:25:38Z- AHRS_M2 msg: 1613172338.08 epoch sec; seqNo:3612 2021-02-12T23:25:38Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-12T23:25:38Z- handleDepth()-Depth_Keller msg: 1613172338093 epoch sec; seqNo:3649; depth 19.95 2021-02-12T23:25:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172338.10 epoch sec; seqNo:3649; 4056954.26 north; 590936.58 east 2021-02-12T23:25:38Z- AHRS_M2 msg: 1613172338.48 epoch sec; seqNo:3613 2021-02-12T23:25:38Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-12T23:25:38Z- handleDepth()-Depth_Keller msg: 1613172338488 epoch sec; seqNo:3650; depth 20.08 2021-02-12T23:25:38Z- handleDvl() - RDI_Pathfinder msg: 1613172338.50 epoch sec; seqNo:1702 2021-02-12T23:25:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.32 , 93.38 , 66.19 , 97.03 2021-02-12T23:25:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , -0.09 2021-02-12T23:25:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172338.52 epoch sec; seqNo:3650; 4056954.64 north; 590936.58 east 2021-02-12T23:25:38Z- AHRS_M2 msg: 1613172338.89 epoch sec; seqNo:3614 2021-02-12T23:25:38Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.19 psi 2021-02-12T23:25:38Z- handleDepth()-Depth_Keller msg: 1613172338892 epoch sec; seqNo:3651; depth 20.20 2021-02-12T23:25:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172338.90 epoch sec; seqNo:3651; 4056954.65 north; 590937.80 east 2021-02-12T23:25:39Z- AHRS_M2 msg: 1613172339.29 epoch sec; seqNo:3615 2021-02-12T23:25:39Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.19 psi 2021-02-12T23:25:39Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-12T23:25:39Z- waiting for 1613172337.05, time is 1613172339.29 2021-02-12T23:25:39Z- LcmTrn::updateTrn() >>>> pose time 1613172339.29, meas time 1613172338.50 <<<< 2021-02-12T23:25:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172338.50 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:25:39Z- TerrainNav::motionUpdate() - currEstimate = (4056955, 590937.80, 20.20) 2021-02-12T23:25:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:25:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:25:39Z- TNavBF::Projecting Measurements 2021-02-12T23:25:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:25:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:25:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:25:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:25:39Z- TNavFilter::4 good beams. 2021-02-12T23:25:39Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:25:39Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:25:39Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:25:39Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:25:39Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:25:39Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:25:39Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:25:39Z- TNavBF::mmse selected from filter -1 2021-02-12T23:25:39Z- TerrainMapOctree: (vehN, vehE ) = (4056954.19, 590937.43). 2021-02-12T23:25:39Z- TerrainMapOctree: Min Distance = 5424.67. 2021-02-12T23:25:39Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:25:39Z- There is no specific good beam that all particles have in common. 2021-02-12T23:25:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:25:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.64 K: 0.140 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-12T23:25:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:25:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.41 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 13.95 2021-02-12T23:25:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:25:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.129 ExpectedMeasDiff: nan currMeas.covariance: 7.01 2021-02-12T23:25:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:25:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 15.06 2021-02-12T23:25:39Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:25:39Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:25:39Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:25:39Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:25:39Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:25:39Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:25:39Z- Filter 0 No beams used in subcloud update 2021-02-12T23:25:39Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:25:39Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:25:39Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 766.9 EastVariance: 833.1 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:25:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172338.50 sec. 2021-02-12T23:25:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:25:39Z- TNavBF::mmse selected from filter -1 2021-02-12T23:25:39Z- variances([0], [2], and psi_berg) = 766.809898 833.050550 0.000000 2021-02-12T23:25:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:25:39Z- LcmTrn::publishEstimates() - MLE : 4056936.15 590889.30 20.20 2021-02-12T23:25:39Z- LcmTrn::publishEstimates() - MMSE : 4056954.65 590937.80 20.20 2021-02-12T23:25:39Z- LcmTrn::publishEstimates() - COVAR: 766.81 833.05 1.09 0.00 2021-02-12T23:25:39Z- handleDepth()-Depth_Keller msg: 1613172339298 epoch sec; seqNo:3652; depth 20.31 2021-02-12T23:25:39Z- handleDvl() - RDI_Pathfinder msg: 1613172339.31 epoch sec; seqNo:1703 2021-02-12T23:25:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 91.29 , 65.38 , 96.69 2021-02-12T23:25:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.12 2021-02-12T23:25:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172339.32 epoch sec; seqNo:3652; 4056955.03 north; 590937.79 east 2021-02-12T23:25:39Z- AHRS_M2 msg: 1613172339.70 epoch sec; seqNo:3616 2021-02-12T23:25:39Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-12T23:25:39Z- handleDepth()-Depth_Keller msg: 1613172339705 epoch sec; seqNo:3653; depth 20.46 2021-02-12T23:25:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:39Z- handleDvl() - RDI_Pathfinder msg: 1613172339.71 epoch sec; seqNo:1704 2021-02-12T23:25:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.12 2021-02-12T23:25:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172339.72 epoch sec; seqNo:3653; 4056955.41 north; 590937.79 east 2021-02-12T23:25:40Z- AHRS_M2 msg: 1613172340.10 epoch sec; seqNo:3617 2021-02-12T23:25:40Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:40Z- handleDepth()-Depth_Keller msg: 1613172340109 epoch sec; seqNo:3654; depth 20.56 2021-02-12T23:25:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172340.12 epoch sec; seqNo:3654; 4056955.79 north; 590937.79 east 2021-02-12T23:25:40Z- AHRS_M2 msg: 1613172340.50 epoch sec; seqNo:3618 2021-02-12T23:25:40Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:40Z- handleDepth()-Depth_Keller msg: 1613172340517 epoch sec; seqNo:3655; depth 20.69 2021-02-12T23:25:40Z- handleDvl() - RDI_Pathfinder msg: 1613172340.53 epoch sec; seqNo:1705 2021-02-12T23:25:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.10 , 92.44 , 65.79 , 96.76 2021-02-12T23:25:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , -0.10 2021-02-12T23:25:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172340.54 epoch sec; seqNo:3655; 4056956.17 north; 590937.78 east 2021-02-12T23:25:40Z- AHRS_M2 msg: 1613172340.91 epoch sec; seqNo:3619 2021-02-12T23:25:40Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:40Z- handleDepth()-Depth_Keller msg: 1613172340912 epoch sec; seqNo:3656; depth 20.82 2021-02-12T23:25:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:40Z- handleDvl() - RDI_Pathfinder msg: 1613172340.91 epoch sec; seqNo:1706 2021-02-12T23:25:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , -0.10 2021-02-12T23:25:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172340.93 epoch sec; seqNo:3656; 4056956.17 north; 590937.78 east 2021-02-12T23:25:41Z- AHRS_M2 msg: 1613172341.31 epoch sec; seqNo:3620 2021-02-12T23:25:41Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:41Z- handleDepth()-Depth_Keller msg: 1613172341316 epoch sec; seqNo:3657; depth 20.94 2021-02-12T23:25:41Z- handleDvl() - RDI_Pathfinder msg: 1613172341.33 epoch sec; seqNo:1707 2021-02-12T23:25:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 90.27 , 65.38 , 96.76 2021-02-12T23:25:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , -0.06 2021-02-12T23:25:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172341.35 epoch sec; seqNo:3657; 4056956.56 north; 590939.00 east 2021-02-12T23:25:41Z- AHRS_M2 msg: 1613172341.72 epoch sec; seqNo:3621 2021-02-12T23:25:41Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.17 psi 2021-02-12T23:25:41Z- handleDepth()-Depth_Keller msg: 1613172341720 epoch sec; seqNo:3658; depth 21.06 2021-02-12T23:25:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172341.73 epoch sec; seqNo:3658; 4056956.94 north; 590938.99 east 2021-02-12T23:25:42Z- AHRS_M2 msg: 1613172342.13 epoch sec; seqNo:3622 2021-02-12T23:25:42Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.17 psi 2021-02-12T23:25:42Z- handleDepth()-Depth_Keller msg: 1613172342134 epoch sec; seqNo:3659; depth 21.19 2021-02-12T23:25:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:42Z- handleDvl() - RDI_Pathfinder msg: 1613172342.14 epoch sec; seqNo:1708 2021-02-12T23:25:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , -0.06 2021-02-12T23:25:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172342.15 epoch sec; seqNo:3659; 4056957.32 north; 590938.99 east 2021-02-12T23:25:42Z- AHRS_M2 msg: 1613172342.52 epoch sec; seqNo:3623 2021-02-12T23:25:42Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.17 psi 2021-02-12T23:25:42Z- handleDepth()-Depth_Keller msg: 1613172342529 epoch sec; seqNo:3660; depth 21.30 2021-02-12T23:25:42Z- handleDvl() - RDI_Pathfinder msg: 1613172342.55 epoch sec; seqNo:1709 2021-02-12T23:25:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 90.27 , 65.38 , 96.76 2021-02-12T23:25:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.04 , -0.00 2021-02-12T23:25:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172342.56 epoch sec; seqNo:3660; 4056957.32 north; 590938.99 east 2021-02-12T23:25:42Z- AHRS_M2 msg: 1613172342.93 epoch sec; seqNo:3624 2021-02-12T23:25:42Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.17 psi 2021-02-12T23:25:42Z- handleDepth()-Depth_Keller msg: 1613172342937 epoch sec; seqNo:3661; depth 21.43 2021-02-12T23:25:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172342.95 epoch sec; seqNo:3661; 4056957.69 north; 590938.99 east 2021-02-12T23:25:43Z- AHRS_M2 msg: 1613172343.33 epoch sec; seqNo:3625 2021-02-12T23:25:43Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.17 psi 2021-02-12T23:25:43Z- handleDepth()-Depth_Keller msg: 1613172343341 epoch sec; seqNo:3662; depth 21.55 2021-02-12T23:25:43Z- handleDvl() - RDI_Pathfinder msg: 1613172343.35 epoch sec; seqNo:1710 2021-02-12T23:25:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.10 , 90.27 , 65.38 , 96.76 2021-02-12T23:25:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.04 2021-02-12T23:25:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172343.36 epoch sec; seqNo:3662; 4056958.07 north; 590938.98 east 2021-02-12T23:25:43Z- AHRS_M2 msg: 1613172343.74 epoch sec; seqNo:3626 2021-02-12T23:25:43Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:43Z- handleDepth()-Depth_Keller msg: 1613172343742 epoch sec; seqNo:3663; depth 21.66 2021-02-12T23:25:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172343.75 epoch sec; seqNo:3663; 4056958.07 north; 590938.98 east 2021-02-12T23:25:44Z- AHRS_M2 msg: 1613172344.14 epoch sec; seqNo:3627 2021-02-12T23:25:44Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:44Z- handleDepth()-Depth_Keller msg: 1613172344149 epoch sec; seqNo:3664; depth 21.77 2021-02-12T23:25:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172344.16 epoch sec; seqNo:3664; 4056958.47 north; 590940.20 east 2021-02-12T23:25:44Z- LcmTrn::handleMessages() - handling 46 messages took 0.63 ms and 134 calls... 2021-02-12T23:25:44Z- waiting for 1613172342.29, time is 1613172344.16 2021-02-12T23:25:44Z- LcmTrn::updateTrn() >>>> pose time 1613172344.16, meas time 1613172343.35 <<<< 2021-02-12T23:25:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172343.35 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:25:44Z- TerrainNav::motionUpdate() - currEstimate = (4056958, 590940.20, 21.77) 2021-02-12T23:25:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:25:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:25:44Z- TNavBF::Projecting Measurements 2021-02-12T23:25:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:25:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:25:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:25:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:25:44Z- TNavFilter::4 good beams. 2021-02-12T23:25:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:25:44Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:25:44Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:25:44Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:25:44Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:25:44Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:25:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:25:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:25:44Z- TerrainMapOctree: (vehN, vehE ) = (4056957.83, 590939.80). 2021-02-12T23:25:44Z- TerrainMapOctree: Min Distance = 5420.34. 2021-02-12T23:25:44Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:25:44Z- There is no specific good beam that all particles have in common. 2021-02-12T23:25:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:25:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.115 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-12T23:25:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:25:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 13.04 2021-02-12T23:25:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:25:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-12T23:25:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:25:44Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 14.98 2021-02-12T23:25:44Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:25:44Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:25:44Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:25:44Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:25:44Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:25:44Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:25:44Z- Filter 0 No beams used in subcloud update 2021-02-12T23:25:44Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:25:44Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:25:44Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 766.7 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:25:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172343.35 sec. 2021-02-12T23:25:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:25:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:25:44Z- variances([0], [2], and psi_berg) = 766.636981 833.050348 0.000000 2021-02-12T23:25:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:25:44Z- LcmTrn::publishEstimates() - MLE : 4056939.97 590891.70 21.77 2021-02-12T23:25:44Z- LcmTrn::publishEstimates() - MMSE : 4056958.47 590940.20 21.77 2021-02-12T23:25:44Z- LcmTrn::publishEstimates() - COVAR: 766.64 833.05 0.93 0.00 2021-02-12T23:25:44Z- AHRS_M2 msg: 1613172344.54 epoch sec; seqNo:3628 2021-02-12T23:25:44Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:44Z- handleDepth()-Depth_Keller msg: 1613172344549 epoch sec; seqNo:3665; depth 21.90 2021-02-12T23:25:44Z- handleDvl() - RDI_Pathfinder msg: 1613172344.56 epoch sec; seqNo:1711 2021-02-12T23:25:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 89.19 , 65.79 , 96.76 2021-02-12T23:25:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , 0.08 2021-02-12T23:25:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172344.57 epoch sec; seqNo:3665; 4056958.84 north; 590940.20 east 2021-02-12T23:25:44Z- AHRS_M2 msg: 1613172344.95 epoch sec; seqNo:3629 2021-02-12T23:25:44Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:44Z- handleDepth()-Depth_Keller msg: 1613172344955 epoch sec; seqNo:3666; depth 22.03 2021-02-12T23:25:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172344.97 epoch sec; seqNo:3666; 4056959.22 north; 590940.19 east 2021-02-12T23:25:45Z- AHRS_M2 msg: 1613172345.35 epoch sec; seqNo:3630 2021-02-12T23:25:45Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:45Z- handleDepth()-Depth_Keller msg: 1613172345356 epoch sec; seqNo:3667; depth 22.14 2021-02-12T23:25:45Z- handleDvl() - RDI_Pathfinder msg: 1613172345.37 epoch sec; seqNo:1712 2021-02-12T23:25:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.10 , 92.44 , 65.38 , 96.76 2021-02-12T23:25:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , 0.07 2021-02-12T23:25:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172345.39 epoch sec; seqNo:3667; 4056959.22 north; 590940.19 east 2021-02-12T23:25:45Z- AHRS_M2 msg: 1613172345.76 epoch sec; seqNo:3631 2021-02-12T23:25:45Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:45Z- handleDepth()-Depth_Keller msg: 1613172345760 epoch sec; seqNo:3668; depth 22.27 2021-02-12T23:25:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:45Z- handleDvl() - RDI_Pathfinder msg: 1613172345.76 epoch sec; seqNo:1713 2021-02-12T23:25:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , 0.07 2021-02-12T23:25:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172345.78 epoch sec; seqNo:3668; 4056959.60 north; 590940.19 east 2021-02-12T23:25:46Z- AHRS_M2 msg: 1613172346.16 epoch sec; seqNo:3632 2021-02-12T23:25:46Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:46Z- handleDepth()-Depth_Keller msg: 1613172346184 epoch sec; seqNo:3669; depth 22.40 2021-02-12T23:25:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172346.22 epoch sec; seqNo:3669; 4056959.98 north; 590940.18 east 2021-02-12T23:25:46Z- AHRS_M2 msg: 1613172346.58 epoch sec; seqNo:3633 2021-02-12T23:25:46Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.18 psi 2021-02-12T23:25:46Z- handleDepth()-Depth_Keller msg: 1613172346595 epoch sec; seqNo:3670; depth 22.52 2021-02-12T23:25:46Z- handleDvl() - RDI_Pathfinder msg: 1613172346.61 epoch sec; seqNo:1714 2021-02-12T23:25:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.10 , 89.19 , 64.97 , 100.01 2021-02-12T23:25:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.03 2021-02-12T23:25:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172346.63 epoch sec; seqNo:3670; 4056960.37 north; 590941.40 east 2021-02-12T23:25:46Z- AHRS_M2 msg: 1613172346.97 epoch sec; seqNo:3634 2021-02-12T23:25:46Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-12T23:25:46Z- handleDepth()-Depth_Keller msg: 1613172346975 epoch sec; seqNo:3671; depth 22.66 2021-02-12T23:25:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:46Z- handleDvl() - RDI_Pathfinder msg: 1613172346.98 epoch sec; seqNo:1715 2021-02-12T23:25:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.03 2021-02-12T23:25:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172346.99 epoch sec; seqNo:3671; 4056960.37 north; 590941.40 east 2021-02-12T23:25:47Z- AHRS_M2 msg: 1613172347.37 epoch sec; seqNo:3635 2021-02-12T23:25:47Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-12T23:25:47Z- handleDepth()-Depth_Keller msg: 1613172347376 epoch sec; seqNo:3672; depth 22.78 2021-02-12T23:25:47Z- handleDvl() - RDI_Pathfinder msg: 1613172347.39 epoch sec; seqNo:1716 2021-02-12T23:25:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.69 , 89.19 , 64.15 , 96.76 2021-02-12T23:25:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , -0.02 2021-02-12T23:25:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172347.42 epoch sec; seqNo:3672; 4056960.75 north; 590941.40 east 2021-02-12T23:25:47Z- AHRS_M2 msg: 1613172347.78 epoch sec; seqNo:3636 2021-02-12T23:25:47Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-12T23:25:47Z- handleDepth()-Depth_Keller msg: 1613172347785 epoch sec; seqNo:3673; depth 22.91 2021-02-12T23:25:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172347.80 epoch sec; seqNo:3673; 4056961.13 north; 590941.39 east 2021-02-12T23:25:48Z- AHRS_M2 msg: 1613172348.20 epoch sec; seqNo:3637 2021-02-12T23:25:48Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-12T23:25:48Z- handleDepth()-Depth_Keller msg: 1613172348213 epoch sec; seqNo:3674; depth 23.02 2021-02-12T23:25:48Z- handleDvl() - RDI_Pathfinder msg: 1613172348.23 epoch sec; seqNo:1717 2021-02-12T23:25:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.28 , 89.26 , 63.74 , 96.83 2021-02-12T23:25:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , -0.07 2021-02-12T23:25:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172348.24 epoch sec; seqNo:3674; 4056961.51 north; 590941.39 east 2021-02-12T23:25:48Z- AHRS_M2 msg: 1613172348.61 epoch sec; seqNo:3638 2021-02-12T23:25:48Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-12T23:25:48Z- handleDepth()-Depth_Keller msg: 1613172348613 epoch sec; seqNo:3675; depth 23.16 2021-02-12T23:25:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172348.62 epoch sec; seqNo:3675; 4056961.51 north; 590941.39 east 2021-02-12T23:25:48Z- AHRS_M2 msg: 1613172348.99 epoch sec; seqNo:3639 2021-02-12T23:25:48Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-12T23:25:48Z- handleDepth()-Depth_Keller msg: 1613172348996 epoch sec; seqNo:3676; depth 23.29 2021-02-12T23:25:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172349.01 epoch sec; seqNo:3676; 4056961.89 north; 590941.38 east 2021-02-12T23:25:49Z- AHRS_M2 msg: 1613172349.39 epoch sec; seqNo:3640 2021-02-12T23:25:49Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-12T23:25:49Z- handleDepth()-Depth_Keller msg: 1613172349398 epoch sec; seqNo:3677; depth 23.40 2021-02-12T23:25:49Z- handleDvl() - RDI_Pathfinder msg: 1613172349.41 epoch sec; seqNo:1718 2021-02-12T23:25:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.46 , 88.18 , 62.11 , 96.83 2021-02-12T23:25:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.07 2021-02-12T23:25:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172349.42 epoch sec; seqNo:3677; 4056962.28 north; 590942.60 east 2021-02-12T23:25:49Z- LcmTrn::handleMessages() - handling 47 messages took 0.68 ms and 134 calls... 2021-02-12T23:25:49Z- waiting for 1613172347.16, time is 1613172349.42 2021-02-12T23:25:49Z- LcmTrn::updateTrn() equating pose 1613172349.42 and meas times 1613172349.42 2021-02-12T23:25:49Z- LcmTrn::updateTrn() >>>> pose time 1613172349.42, meas time 1613172349.42 <<<< 2021-02-12T23:25:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172349.42 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:25:49Z- TerrainNav::motionUpdate() - currEstimate = (4056962, 590942.60, 23.40) 2021-02-12T23:25:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:25:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:25:49Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172349.42 sec.; No relevant navigation data available 2021-02-12T23:25:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:25:49Z- TNavBF::mmse selected from filter -1 2021-02-12T23:25:49Z- variances([0], [2], and psi_berg) = 766.506312 833.050196 0.000000 2021-02-12T23:25:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:25:49Z- LcmTrn::publishEstimates() - MLE : 4056943.78 590894.10 23.40 2021-02-12T23:25:49Z- LcmTrn::publishEstimates() - MMSE : 4056962.28 590942.60 23.40 2021-02-12T23:25:49Z- LcmTrn::publishEstimates() - COVAR: 766.51 833.05 1.15 0.00 2021-02-12T23:25:49Z- AHRS_M2 msg: 1613172349.80 epoch sec; seqNo:3641 2021-02-12T23:25:49Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-12T23:25:49Z- handleDepth()-Depth_Keller msg: 1613172349801 epoch sec; seqNo:3678; depth 23.54 2021-02-12T23:25:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172349.82 epoch sec; seqNo:3678; 4056962.66 north; 590942.60 east 2021-02-12T23:25:50Z- AHRS_M2 msg: 1613172350.21 epoch sec; seqNo:3642 2021-02-12T23:25:50Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-12T23:25:50Z- handleDepth()-Depth_Keller msg: 1613172350219 epoch sec; seqNo:3679; depth 23.66 2021-02-12T23:25:50Z- handleDvl() - RDI_Pathfinder msg: 1613172350.23 epoch sec; seqNo:1719 2021-02-12T23:25:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.46 , 88.18 , 62.52 , 98.99 2021-02-12T23:25:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , -0.03 2021-02-12T23:25:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172350.26 epoch sec; seqNo:3679; 4056962.66 north; 590942.60 east 2021-02-12T23:25:50Z- AHRS_M2 msg: 1613172350.60 epoch sec; seqNo:3643 2021-02-12T23:25:50Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-12T23:25:50Z- handleDepth()-Depth_Keller msg: 1613172350610 epoch sec; seqNo:3680; depth 23.78 2021-02-12T23:25:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:50Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:50Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:50Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:50Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:50Z- handleDvl() - RDI_Pathfinder msg: 1613172350.61 epoch sec; seqNo:1720 2021-02-12T23:25:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , -0.03 2021-02-12T23:25:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172350.62 epoch sec; seqNo:3680; 4056963.04 north; 590942.59 east 2021-02-12T23:25:51Z- AHRS_M2 msg: 1613172351.01 epoch sec; seqNo:3644 2021-02-12T23:25:51Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-12T23:25:51Z- handleDepth()-Depth_Keller msg: 1613172351017 epoch sec; seqNo:3681; depth 23.91 2021-02-12T23:25:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172351.03 epoch sec; seqNo:3681; 4056963.42 north; 590942.59 east 2021-02-12T23:25:51Z- AHRS_M2 msg: 1613172351.41 epoch sec; seqNo:3645 2021-02-12T23:25:51Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-12T23:25:51Z- handleDepth()-Depth_Keller msg: 1613172351417 epoch sec; seqNo:3682; depth 24.04 2021-02-12T23:25:51Z- handleDvl() - RDI_Pathfinder msg: 1613172351.43 epoch sec; seqNo:1721 2021-02-12T23:25:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.46 , 89.26 , 62.11 , 96.83 2021-02-12T23:25:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.06 , 0.01 2021-02-12T23:25:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172351.44 epoch sec; seqNo:3682; 4056963.79 north; 590942.59 east 2021-02-12T23:25:51Z- AHRS_M2 msg: 1613172351.82 epoch sec; seqNo:3646 2021-02-12T23:25:51Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-12T23:25:51Z- handleDepth()-Depth_Keller msg: 1613172351821 epoch sec; seqNo:3683; depth 24.17 2021-02-12T23:25:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:51Z- handleDvl() - RDI_Pathfinder msg: 1613172351.82 epoch sec; seqNo:1722 2021-02-12T23:25:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.06 , 0.01 2021-02-12T23:25:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172351.83 epoch sec; seqNo:3683; 4056963.79 north; 590942.59 east 2021-02-12T23:25:52Z- AHRS_M2 msg: 1613172352.22 epoch sec; seqNo:3647 2021-02-12T23:25:52Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-12T23:25:52Z- handleDepth()-Depth_Keller msg: 1613172352233 epoch sec; seqNo:3684; depth 24.28 2021-02-12T23:25:52Z- handleDvl() - RDI_Pathfinder msg: 1613172352.25 epoch sec; seqNo:1723 2021-02-12T23:25:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 87.10 , 61.70 , 96.83 2021-02-12T23:25:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , 0.04 2021-02-12T23:25:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172352.27 epoch sec; seqNo:3684; 4056964.19 north; 590943.80 east 2021-02-12T23:25:52Z- AHRS_M2 msg: 1613172352.62 epoch sec; seqNo:3648 2021-02-12T23:25:52Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-12T23:25:52Z- handleDepth()-Depth_Keller msg: 1613172352633 epoch sec; seqNo:3685; depth 24.42 2021-02-12T23:25:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172352.64 epoch sec; seqNo:3685; 4056964.57 north; 590943.80 east 2021-02-12T23:25:53Z- AHRS_M2 msg: 1613172353.03 epoch sec; seqNo:3649 2021-02-12T23:25:53Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-12T23:25:53Z- handleDepth()-Depth_Keller msg: 1613172353034 epoch sec; seqNo:3686; depth 24.54 2021-02-12T23:25:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:53Z- handleDvl() - RDI_Pathfinder msg: 1613172353.04 epoch sec; seqNo:1724 2021-02-12T23:25:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , 0.04 2021-02-12T23:25:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172353.05 epoch sec; seqNo:3686; 4056964.57 north; 590943.80 east 2021-02-12T23:25:53Z- AHRS_M2 msg: 1613172353.43 epoch sec; seqNo:3650 2021-02-12T23:25:53Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-12T23:25:53Z- handleDepth()-Depth_Keller msg: 1613172353441 epoch sec; seqNo:3687; depth 24.66 2021-02-12T23:25:53Z- handleDvl() - RDI_Pathfinder msg: 1613172353.46 epoch sec; seqNo:1725 2021-02-12T23:25:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 89.26 , 61.70 , 100.07 2021-02-12T23:25:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , 0.05 2021-02-12T23:25:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172353.47 epoch sec; seqNo:3687; 4056964.94 north; 590943.79 east 2021-02-12T23:25:53Z- AHRS_M2 msg: 1613172353.84 epoch sec; seqNo:3651 2021-02-12T23:25:53Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-12T23:25:53Z- handleDepth()-Depth_Keller msg: 1613172353840 epoch sec; seqNo:3688; depth 24.78 2021-02-12T23:25:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172353.86 epoch sec; seqNo:3688; 4056965.32 north; 590943.79 east 2021-02-12T23:25:54Z- AHRS_M2 msg: 1613172354.24 epoch sec; seqNo:3652 2021-02-12T23:25:54Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-12T23:25:54Z- handleDepth()-Depth_Keller msg: 1613172354246 epoch sec; seqNo:3689; depth 24.89 2021-02-12T23:25:54Z- handleDvl() - RDI_Pathfinder msg: 1613172354.26 epoch sec; seqNo:1726 2021-02-12T23:25:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 88.18 , 62.52 , 96.83 2021-02-12T23:25:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.07 , 0.01 2021-02-12T23:25:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172354.27 epoch sec; seqNo:3689; 4056965.32 north; 590943.79 east 2021-02-12T23:25:54Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-12T23:25:54Z- waiting for 1613172352.42, time is 1613172354.27 2021-02-12T23:25:54Z- LcmTrn::updateTrn() equating pose 1613172354.27 and meas times 1613172354.27 2021-02-12T23:25:54Z- LcmTrn::updateTrn() >>>> pose time 1613172354.27, meas time 1613172354.27 <<<< 2021-02-12T23:25:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172354.27 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:25:54Z- TerrainNav::motionUpdate() - currEstimate = (4056965, 590943.79, 24.89) 2021-02-12T23:25:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:25:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:25:54Z- TNavBF::Projecting Measurements 2021-02-12T23:25:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:25:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:25:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:25:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:25:54Z- TNavFilter::4 good beams. 2021-02-12T23:25:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:25:54Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:25:54Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:25:54Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:25:54Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:25:54Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:25:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:25:54Z- TNavBF::mmse selected from filter -1 2021-02-12T23:25:54Z- TerrainMapOctree: (vehN, vehE ) = (4056965.32, 590943.79). 2021-02-12T23:25:54Z- TerrainMapOctree: Min Distance = 5411.98. 2021-02-12T23:25:54Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:25:54Z- There is no specific good beam that all particles have in common. 2021-02-12T23:25:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:25:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.32 K: 0.124 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-12T23:25:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:25:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 12.44 2021-02-12T23:25:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:25:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.118 ExpectedMeasDiff: nan currMeas.covariance: 6.25 2021-02-12T23:25:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:25:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 15.00 2021-02-12T23:25:54Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:25:54Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:25:54Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:25:54Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:25:54Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:25:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:25:54Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:25:54Z- Filter 0 No beams used in subcloud update 2021-02-12T23:25:54Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:25:54Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:25:54Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 766.4 EastVariance: 833.1 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:25:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172354.27 sec. 2021-02-12T23:25:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:25:54Z- TNavBF::mmse selected from filter -1 2021-02-12T23:25:54Z- variances([0], [2], and psi_berg) = 766.395277 833.050067 0.000000 2021-02-12T23:25:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:25:54Z- LcmTrn::publishEstimates() - MLE : 4056946.82 590895.29 24.89 2021-02-12T23:25:54Z- LcmTrn::publishEstimates() - MMSE : 4056965.32 590943.79 24.89 2021-02-12T23:25:54Z- LcmTrn::publishEstimates() - COVAR: 766.40 833.05 0.89 0.00 2021-02-12T23:25:54Z- AHRS_M2 msg: 1613172354.64 epoch sec; seqNo:3653 2021-02-12T23:25:54Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-12T23:25:54Z- handleDepth()-Depth_Keller msg: 1613172354653 epoch sec; seqNo:3690; depth 25.03 2021-02-12T23:25:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172354.66 epoch sec; seqNo:3690; 4056965.72 north; 590945.01 east 2021-02-12T23:25:55Z- AHRS_M2 msg: 1613172355.05 epoch sec; seqNo:3654 2021-02-12T23:25:55Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:55Z- handleDepth()-Depth_Keller msg: 1613172355057 epoch sec; seqNo:3691; depth 25.16 2021-02-12T23:25:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172355.07 epoch sec; seqNo:3691; 4056966.09 north; 590945.00 east 2021-02-12T23:25:55Z- AHRS_M2 msg: 1613172355.45 epoch sec; seqNo:3655 2021-02-12T23:25:55Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:55Z- handleDepth()-Depth_Keller msg: 1613172355456 epoch sec; seqNo:3692; depth 25.29 2021-02-12T23:25:55Z- handleDvl() - RDI_Pathfinder msg: 1613172355.47 epoch sec; seqNo:1727 2021-02-12T23:25:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 87.10 , 61.70 , 95.75 2021-02-12T23:25:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.06 , -0.05 2021-02-12T23:25:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172355.49 epoch sec; seqNo:3692; 4056966.47 north; 590945.00 east 2021-02-12T23:25:55Z- AHRS_M2 msg: 1613172355.86 epoch sec; seqNo:3656 2021-02-12T23:25:55Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:55Z- handleDepth()-Depth_Keller msg: 1613172355860 epoch sec; seqNo:3693; depth 25.42 2021-02-12T23:25:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172355.87 epoch sec; seqNo:3693; 4056966.47 north; 590945.00 east 2021-02-12T23:25:56Z- AHRS_M2 msg: 1613172356.26 epoch sec; seqNo:3657 2021-02-12T23:25:56Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:56Z- handleDepth()-Depth_Keller msg: 1613172356274 epoch sec; seqNo:3694; depth 25.51 2021-02-12T23:25:56Z- handleDvl() - RDI_Pathfinder msg: 1613172356.29 epoch sec; seqNo:1728 2021-02-12T23:25:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.83 , 88.18 , 62.11 , 96.83 2021-02-12T23:25:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.08 2021-02-12T23:25:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172356.30 epoch sec; seqNo:3694; 4056966.85 north; 590945.00 east 2021-02-12T23:25:56Z- AHRS_M2 msg: 1613172356.66 epoch sec; seqNo:3658 2021-02-12T23:25:56Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:56Z- handleDepth()-Depth_Keller msg: 1613172356668 epoch sec; seqNo:3695; depth 25.66 2021-02-12T23:25:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:56Z- handleDvl() - RDI_Pathfinder msg: 1613172356.67 epoch sec; seqNo:1729 2021-02-12T23:25:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.08 2021-02-12T23:25:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172356.68 epoch sec; seqNo:3695; 4056967.24 north; 590946.21 east 2021-02-12T23:25:57Z- AHRS_M2 msg: 1613172357.07 epoch sec; seqNo:3659 2021-02-12T23:25:57Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:57Z- handleDepth()-Depth_Keller msg: 1613172357077 epoch sec; seqNo:3696; depth 25.78 2021-02-12T23:25:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172357.09 epoch sec; seqNo:3696; 4056967.62 north; 590946.21 east 2021-02-12T23:25:57Z- AHRS_M2 msg: 1613172357.47 epoch sec; seqNo:3660 2021-02-12T23:25:57Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:57Z- handleDepth()-Depth_Keller msg: 1613172357485 epoch sec; seqNo:3697; depth 25.89 2021-02-12T23:25:57Z- handleDvl() - RDI_Pathfinder msg: 1613172357.50 epoch sec; seqNo:1730 2021-02-12T23:25:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.42 , 88.18 , 0.00 , 94.67 2021-02-12T23:25:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , -0.05 2021-02-12T23:25:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172357.51 epoch sec; seqNo:3697; 4056967.62 north; 590946.21 east 2021-02-12T23:25:57Z- AHRS_M2 msg: 1613172357.88 epoch sec; seqNo:3661 2021-02-12T23:25:57Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:57Z- handleDepth()-Depth_Keller msg: 1613172357885 epoch sec; seqNo:3698; depth 26.02 2021-02-12T23:25:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:57Z- handleDvl() - RDI_Pathfinder msg: 1613172357.89 epoch sec; seqNo:1731 2021-02-12T23:25:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , -0.05 2021-02-12T23:25:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172357.91 epoch sec; seqNo:3698; 4056968.00 north; 590946.20 east 2021-02-12T23:25:58Z- AHRS_M2 msg: 1613172358.28 epoch sec; seqNo:3662 2021-02-12T23:25:58Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:58Z- handleDepth()-Depth_Keller msg: 1613172358284 epoch sec; seqNo:3699; depth 26.15 2021-02-12T23:25:58Z- handleDvl() - RDI_Pathfinder msg: 1613172358.30 epoch sec; seqNo:1732 2021-02-12T23:25:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.01 , 86.15 , 61.29 , 93.45 2021-02-12T23:25:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , -0.01 2021-02-12T23:25:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172358.33 epoch sec; seqNo:3699; 4056968.38 north; 590946.20 east 2021-02-12T23:25:58Z- AHRS_M2 msg: 1613172358.69 epoch sec; seqNo:3663 2021-02-12T23:25:58Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:58Z- handleDepth()-Depth_Keller msg: 1613172358701 epoch sec; seqNo:3700; depth 26.27 2021-02-12T23:25:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172358.71 epoch sec; seqNo:3700; 4056968.76 north; 590946.20 east 2021-02-12T23:25:59Z- AHRS_M2 msg: 1613172359.09 epoch sec; seqNo:3664 2021-02-12T23:25:59Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:59Z- handleDepth()-Depth_Keller msg: 1613172359098 epoch sec; seqNo:3701; depth 26.37 2021-02-12T23:25:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:25:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:25:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:25:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:25:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:25:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:25:59Z- handleDvl() - RDI_Pathfinder msg: 1613172359.10 epoch sec; seqNo:1733 2021-02-12T23:25:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:25:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , -0.01 2021-02-12T23:25:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172359.12 epoch sec; seqNo:3701; 4056968.77 north; 590947.42 east 2021-02-12T23:25:59Z- AHRS_M2 msg: 1613172359.49 epoch sec; seqNo:3665 2021-02-12T23:25:59Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:59Z- handleDepth()-Depth_Keller msg: 1613172359496 epoch sec; seqNo:3702; depth 26.49 2021-02-12T23:25:59Z- handleDvl() - RDI_Pathfinder msg: 1613172359.51 epoch sec; seqNo:1734 2021-02-12T23:25:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.01 , 79.68 , 60.47 , 94.67 2021-02-12T23:25:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , 0.02 2021-02-12T23:25:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172359.53 epoch sec; seqNo:3702; 4056969.15 north; 590947.41 east 2021-02-12T23:25:59Z- LcmTrn::handleMessages() - handling 47 messages took 0.70 ms and 133 calls... 2021-02-12T23:25:59Z- waiting for 1613172357.27, time is 1613172359.53 2021-02-12T23:25:59Z- LcmTrn::updateTrn() equating pose 1613172359.53 and meas times 1613172359.53 2021-02-12T23:25:59Z- LcmTrn::updateTrn() >>>> pose time 1613172359.53, meas time 1613172359.53 <<<< 2021-02-12T23:25:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172359.53 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:25:59Z- TerrainNav::motionUpdate() - currEstimate = (4056969, 590947.41, 26.49) 2021-02-12T23:25:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:25:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:25:59Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172359.53 sec.; No relevant navigation data available 2021-02-12T23:25:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:25:59Z- TNavBF::mmse selected from filter -1 2021-02-12T23:25:59Z- variances([0], [2], and psi_berg) = 766.254500 833.049903 0.000000 2021-02-12T23:25:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:25:59Z- LcmTrn::publishEstimates() - MLE : 4056950.65 590898.91 26.49 2021-02-12T23:25:59Z- LcmTrn::publishEstimates() - MMSE : 4056969.15 590947.41 26.49 2021-02-12T23:25:59Z- LcmTrn::publishEstimates() - COVAR: 766.25 833.05 1.16 0.00 2021-02-12T23:25:59Z- AHRS_M2 msg: 1613172359.90 epoch sec; seqNo:3666 2021-02-12T23:25:59Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-12T23:25:59Z- handleDepth()-Depth_Keller msg: 1613172359905 epoch sec; seqNo:3703; depth 26.61 2021-02-12T23:25:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172359.92 epoch sec; seqNo:3703; 4056969.53 north; 590947.41 east 2021-02-12T23:26:00Z- AHRS_M2 msg: 1613172360.31 epoch sec; seqNo:3667 2021-02-12T23:26:00Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-12T23:26:00Z- handleDepth()-Depth_Keller msg: 1613172360317 epoch sec; seqNo:3704; depth 26.73 2021-02-12T23:26:00Z- handleDvl() - RDI_Pathfinder msg: 1613172360.33 epoch sec; seqNo:1735 2021-02-12T23:26:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.60 , 79.68 , 60.88 , 92.50 2021-02-12T23:26:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.04 , 0.01 2021-02-12T23:26:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172360.34 epoch sec; seqNo:3704; 4056969.53 north; 590947.41 east 2021-02-12T23:26:00Z- AHRS_M2 msg: 1613172360.70 epoch sec; seqNo:3668 2021-02-12T23:26:00Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-12T23:26:00Z- handleDepth()-Depth_Keller msg: 1613172360717 epoch sec; seqNo:3705; depth 26.85 2021-02-12T23:26:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172360.73 epoch sec; seqNo:3705; 4056969.91 north; 590947.40 east 2021-02-12T23:26:01Z- AHRS_M2 msg: 1613172361.13 epoch sec; seqNo:3669 2021-02-12T23:26:01Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-12T23:26:01Z- handleDepth()-Depth_Keller msg: 1613172361138 epoch sec; seqNo:3706; depth 26.96 2021-02-12T23:26:01Z- handleDvl() - RDI_Pathfinder msg: 1613172361.15 epoch sec; seqNo:1736 2021-02-12T23:26:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.60 , 79.68 , 60.88 , 93.58 2021-02-12T23:26:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , -0.01 2021-02-12T23:26:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172361.16 epoch sec; seqNo:3706; 4056970.29 north; 590947.40 east 2021-02-12T23:26:01Z- AHRS_M2 msg: 1613172361.51 epoch sec; seqNo:3670 2021-02-12T23:26:01Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-12T23:26:01Z- handleDepth()-Depth_Keller msg: 1613172361516 epoch sec; seqNo:3707; depth 27.08 2021-02-12T23:26:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:01Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:01Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:01Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:01Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:01Z- handleDvl() - RDI_Pathfinder msg: 1613172361.52 epoch sec; seqNo:1737 2021-02-12T23:26:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , -0.01 2021-02-12T23:26:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172361.53 epoch sec; seqNo:3707; 4056970.30 north; 590948.62 east 2021-02-12T23:26:01Z- AHRS_M2 msg: 1613172361.92 epoch sec; seqNo:3671 2021-02-12T23:26:01Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-12T23:26:01Z- handleDepth()-Depth_Keller msg: 1613172361921 epoch sec; seqNo:3708; depth 27.22 2021-02-12T23:26:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172361.93 epoch sec; seqNo:3708; 4056970.68 north; 590948.62 east 2021-02-12T23:26:02Z- AHRS_M2 msg: 1613172362.32 epoch sec; seqNo:3672 2021-02-12T23:26:02Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-12T23:26:02Z- handleDepth()-Depth_Keller msg: 1613172362329 epoch sec; seqNo:3709; depth 27.33 2021-02-12T23:26:02Z- handleDvl() - RDI_Pathfinder msg: 1613172362.34 epoch sec; seqNo:1738 2021-02-12T23:26:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.79 , 79.68 , 60.07 , 92.50 2021-02-12T23:26:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , -0.03 2021-02-12T23:26:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172362.36 epoch sec; seqNo:3709; 4056971.06 north; 590948.61 east 2021-02-12T23:26:02Z- AHRS_M2 msg: 1613172362.72 epoch sec; seqNo:3673 2021-02-12T23:26:02Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.19 psi 2021-02-12T23:26:02Z- handleDepth()-Depth_Keller msg: 1613172362736 epoch sec; seqNo:3710; depth 27.45 2021-02-12T23:26:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:02Z- handleDvl() - RDI_Pathfinder msg: 1613172362.76 epoch sec; seqNo:1739 2021-02-12T23:26:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , -0.03 2021-02-12T23:26:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172362.78 epoch sec; seqNo:3710; 4056971.44 north; 590948.61 east 2021-02-12T23:26:03Z- AHRS_M2 msg: 1613172363.15 epoch sec; seqNo:3674 2021-02-12T23:26:03Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.19 psi 2021-02-12T23:26:03Z- handleDepth()-Depth_Keller msg: 1613172363151 epoch sec; seqNo:3711; depth 27.59 2021-02-12T23:26:03Z- handleDvl() - RDI_Pathfinder msg: 1613172363.17 epoch sec; seqNo:1740 2021-02-12T23:26:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.79 , 79.27 , 60.07 , 93.58 2021-02-12T23:26:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.03 2021-02-12T23:26:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172363.18 epoch sec; seqNo:3711; 4056971.44 north; 590948.61 east 2021-02-12T23:26:03Z- handleDepth()-Depth_Keller msg: 1613172363553 epoch sec; seqNo:3712; depth 27.70 2021-02-12T23:26:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172363.56 epoch sec; seqNo:3712; 4056971.82 north; 590948.61 east 2021-02-12T23:26:03Z- AHRS_M2 msg: 1613172363.94 epoch sec; seqNo:3675 2021-02-12T23:26:03Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.19 psi 2021-02-12T23:26:03Z- handleDepth()-Depth_Keller msg: 1613172363955 epoch sec; seqNo:3713; depth 27.83 2021-02-12T23:26:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:03Z- handleDvl() - RDI_Pathfinder msg: 1613172363.96 epoch sec; seqNo:1741 2021-02-12T23:26:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.03 2021-02-12T23:26:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172363.96 epoch sec; seqNo:3713; 4056972.21 north; 590949.82 east 2021-02-12T23:26:04Z- AHRS_M2 msg: 1613172364.35 epoch sec; seqNo:3676 2021-02-12T23:26:04Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.20 psi 2021-02-12T23:26:04Z- handleDepth()-Depth_Keller msg: 1613172364351 epoch sec; seqNo:3714; depth 27.93 2021-02-12T23:26:04Z- handleDvl() - RDI_Pathfinder msg: 1613172364.36 epoch sec; seqNo:1742 2021-02-12T23:26:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.56 , 79.68 , 59.66 , 91.42 2021-02-12T23:26:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.00 2021-02-12T23:26:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172364.39 epoch sec; seqNo:3714; 4056972.59 north; 590949.82 east 2021-02-12T23:26:04Z- LcmTrn::handleMessages() - handling 43 messages took 0.70 ms and 129 calls... 2021-02-12T23:26:04Z- waiting for 1613172362.53, time is 1613172364.39 2021-02-12T23:26:04Z- LcmTrn::updateTrn() equating pose 1613172364.39 and meas times 1613172364.39 2021-02-12T23:26:04Z- LcmTrn::updateTrn() >>>> pose time 1613172364.39, meas time 1613172364.39 <<<< 2021-02-12T23:26:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172364.39 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:26:04Z- TerrainNav::motionUpdate() - currEstimate = (4056973, 590949.82, 27.93) 2021-02-12T23:26:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:26:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:26:04Z- TNavBF::Projecting Measurements 2021-02-12T23:26:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:26:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:26:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:26:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:26:04Z- TNavFilter::4 good beams. 2021-02-12T23:26:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:26:04Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:26:04Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:26:04Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:26:04Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:26:04Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:26:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:26:04Z- TNavBF::mmse selected from filter -1 2021-02-12T23:26:04Z- TerrainMapOctree: (vehN, vehE ) = (4056972.59, 590949.82). 2021-02-12T23:26:04Z- TerrainMapOctree: Min Distance = 5402.54. 2021-02-12T23:26:04Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:26:04Z- There is no specific good beam that all particles have in common. 2021-02-12T23:26:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:26:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.141 ExpectedMeasDiff: nan currMeas.covariance: 6.67 2021-02-12T23:26:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:26:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 10.16 2021-02-12T23:26:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:26:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.126 ExpectedMeasDiff: nan currMeas.covariance: 5.69 2021-02-12T23:26:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:26:04Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 13.37 2021-02-12T23:26:04Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:26:04Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:26:04Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:26:04Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:26:04Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:26:04Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:26:04Z- Filter 0 No beams used in subcloud update 2021-02-12T23:26:04Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:26:04Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:26:04Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 766.1 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:26:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172364.39 sec. 2021-02-12T23:26:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:26:04Z- TNavBF::mmse selected from filter -1 2021-02-12T23:26:04Z- variances([0], [2], and psi_berg) = 766.129179 833.049758 0.000000 2021-02-12T23:26:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:26:04Z- LcmTrn::publishEstimates() - MLE : 4056954.09 590901.32 27.93 2021-02-12T23:26:04Z- LcmTrn::publishEstimates() - MMSE : 4056972.59 590949.82 27.93 2021-02-12T23:26:04Z- LcmTrn::publishEstimates() - COVAR: 766.13 833.05 0.88 0.00 2021-02-12T23:26:04Z- AHRS_M2 msg: 1613172364.74 epoch sec; seqNo:3677 2021-02-12T23:26:04Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.20 psi 2021-02-12T23:26:04Z- handleDepth()-Depth_Keller msg: 1613172364753 epoch sec; seqNo:3715; depth 28.07 2021-02-12T23:26:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172364.76 epoch sec; seqNo:3715; 4056972.59 north; 590949.82 east 2021-02-12T23:26:05Z- AHRS_M2 msg: 1613172365.17 epoch sec; seqNo:3678 2021-02-12T23:26:05Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.20 psi 2021-02-12T23:26:05Z- handleDepth()-Depth_Keller msg: 1613172365174 epoch sec; seqNo:3716; depth 28.18 2021-02-12T23:26:05Z- handleDvl() - RDI_Pathfinder msg: 1613172365.20 epoch sec; seqNo:1743 2021-02-12T23:26:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.97 , 79.27 , 59.66 , 92.50 2021-02-12T23:26:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.02 2021-02-12T23:26:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172365.23 epoch sec; seqNo:3716; 4056972.96 north; 590949.81 east 2021-02-12T23:26:05Z- AHRS_M2 msg: 1613172365.55 epoch sec; seqNo:3679 2021-02-12T23:26:05Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.20 psi 2021-02-12T23:26:05Z- handleDepth()-Depth_Keller msg: 1613172365556 epoch sec; seqNo:3717; depth 28.32 2021-02-12T23:26:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172365.58 epoch sec; seqNo:3717; 4056973.34 north; 590949.81 east 2021-02-12T23:26:05Z- AHRS_M2 msg: 1613172365.96 epoch sec; seqNo:3680 2021-02-12T23:26:05Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.20 psi 2021-02-12T23:26:05Z- handleDepth()-Depth_Keller msg: 1613172365965 epoch sec; seqNo:3718; depth 28.42 2021-02-12T23:26:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172365.97 epoch sec; seqNo:3718; 4056973.34 north; 590949.81 east 2021-02-12T23:26:06Z- AHRS_M2 msg: 1613172366.36 epoch sec; seqNo:3681 2021-02-12T23:26:06Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.20 psi 2021-02-12T23:26:06Z- handleDepth()-Depth_Keller msg: 1613172366370 epoch sec; seqNo:3719; depth 28.53 2021-02-12T23:26:06Z- handleDvl() - RDI_Pathfinder msg: 1613172366.38 epoch sec; seqNo:1744 2021-02-12T23:26:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.56 , 78.86 , 58.84 , 93.58 2021-02-12T23:26:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.04 , 0.04 2021-02-12T23:26:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172366.39 epoch sec; seqNo:3719; 4056973.74 north; 590951.03 east 2021-02-12T23:26:06Z- AHRS_M2 msg: 1613172366.76 epoch sec; seqNo:3682 2021-02-12T23:26:06Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.20 psi 2021-02-12T23:26:06Z- handleDepth()-Depth_Keller msg: 1613172366770 epoch sec; seqNo:3720; depth 28.66 2021-02-12T23:26:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172366.78 epoch sec; seqNo:3720; 4056974.11 north; 590951.02 east 2021-02-12T23:26:07Z- AHRS_M2 msg: 1613172367.17 epoch sec; seqNo:3683 2021-02-12T23:26:07Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.20 psi 2021-02-12T23:26:07Z- handleDepth()-Depth_Keller msg: 1613172367177 epoch sec; seqNo:3721; depth 28.80 2021-02-12T23:26:07Z- handleDvl() - RDI_Pathfinder msg: 1613172367.19 epoch sec; seqNo:1745 2021-02-12T23:26:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.56 , 78.45 , 58.84 , 91.42 2021-02-12T23:26:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.06 , 0.03 2021-02-12T23:26:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172367.20 epoch sec; seqNo:3721; 4056974.11 north; 590951.02 east 2021-02-12T23:26:07Z- AHRS_M2 msg: 1613172367.57 epoch sec; seqNo:3684 2021-02-12T23:26:07Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.20 psi 2021-02-12T23:26:07Z- handleDepth()-Depth_Keller msg: 1613172367576 epoch sec; seqNo:3722; depth 28.91 2021-02-12T23:26:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:07Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:07Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:07Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:07Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:07Z- handleDvl() - RDI_Pathfinder msg: 1613172367.58 epoch sec; seqNo:1746 2021-02-12T23:26:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.06 , 0.03 2021-02-12T23:26:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172367.59 epoch sec; seqNo:3722; 4056974.49 north; 590951.02 east 2021-02-12T23:26:07Z- AHRS_M2 msg: 1613172367.98 epoch sec; seqNo:3685 2021-02-12T23:26:07Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.20 psi 2021-02-12T23:26:07Z- handleDepth()-Depth_Keller msg: 1613172367980 epoch sec; seqNo:3723; depth 29.03 2021-02-12T23:26:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172367.99 epoch sec; seqNo:3723; 4056974.87 north; 590951.02 east 2021-02-12T23:26:08Z- AHRS_M2 msg: 1613172368.38 epoch sec; seqNo:3686 2021-02-12T23:26:08Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.20 psi 2021-02-12T23:26:08Z- handleDepth()-Depth_Keller msg: 1613172368389 epoch sec; seqNo:3724; depth 29.15 2021-02-12T23:26:08Z- handleDvl() - RDI_Pathfinder msg: 1613172368.41 epoch sec; seqNo:1747 2021-02-12T23:26:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.74 , 79.27 , 59.25 , 90.34 2021-02-12T23:26:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.09 , 0.02 2021-02-12T23:26:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172368.42 epoch sec; seqNo:3724; 4056974.87 north; 590951.02 east 2021-02-12T23:26:08Z- AHRS_M2 msg: 1613172368.78 epoch sec; seqNo:3687 2021-02-12T23:26:08Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.20 psi 2021-02-12T23:26:08Z- handleDepth()-Depth_Keller msg: 1613172368793 epoch sec; seqNo:3725; depth 29.29 2021-02-12T23:26:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:08Z- handleDvl() - RDI_Pathfinder msg: 1613172368.80 epoch sec; seqNo:1748 2021-02-12T23:26:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.09 , 0.02 2021-02-12T23:26:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172368.81 epoch sec; seqNo:3725; 4056975.26 north; 590952.23 east 2021-02-12T23:26:09Z- AHRS_M2 msg: 1613172369.19 epoch sec; seqNo:3688 2021-02-12T23:26:09Z- AHRS_M2 msg: 0.61 phi; -0.27 theta; -0.20 psi 2021-02-12T23:26:09Z- handleDepth()-Depth_Keller msg: 1613172369192 epoch sec; seqNo:3726; depth 29.40 2021-02-12T23:26:09Z- handleDvl() - RDI_Pathfinder msg: 1613172369.20 epoch sec; seqNo:1749 2021-02-12T23:26:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.93 , 78.04 , 58.43 , 0.00 2021-02-12T23:26:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.08 , -0.01 2021-02-12T23:26:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172369.23 epoch sec; seqNo:3726; 4056975.64 north; 590952.23 east 2021-02-12T23:26:09Z- AHRS_M2 msg: 1613172369.59 epoch sec; seqNo:3689 2021-02-12T23:26:09Z- AHRS_M2 msg: 0.60 phi; -0.23 theta; -0.19 psi 2021-02-12T23:26:09Z- handleDepth()-Depth_Keller msg: 1613172369596 epoch sec; seqNo:3727; depth 29.55 2021-02-12T23:26:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172369.61 epoch sec; seqNo:3727; 4056976.02 north; 590952.22 east 2021-02-12T23:26:09Z- LcmTrn::handleMessages() - handling 46 messages took 0.68 ms and 133 calls... 2021-02-12T23:26:09Z- waiting for 1613172367.39, time is 1613172369.61 2021-02-12T23:26:09Z- LcmTrn::updateTrn() >>>> pose time 1613172369.61, meas time 1613172369.20 <<<< 2021-02-12T23:26:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172369.20 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:26:09Z- TerrainNav::motionUpdate() - currEstimate = (4056976, 590952.22, 29.55) 2021-02-12T23:26:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:26:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:26:09Z- TNavBF::Projecting Measurements 2021-02-12T23:26:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:26:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:26:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:26:09Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:26:09Z- TNavFilter::3 good beams. 2021-02-12T23:26:09Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:26:09Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:26:09Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:26:09Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:26:09Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:26:09Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:26:09Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:26:09Z- TNavBF::mmse selected from filter -1 2021-02-12T23:26:09Z- TerrainMapOctree: (vehN, vehE ) = (4056975.76, 590952.04). 2021-02-12T23:26:09Z- TerrainMapOctree: Min Distance = 5398.68. 2021-02-12T23:26:09Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:26:09Z- There is no specific good beam that all particles have in common. 2021-02-12T23:26:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:26:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.118 ExpectedMeasDiff: nan currMeas.covariance: 6.34 2021-02-12T23:26:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:26:09Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 9.75 2021-02-12T23:26:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:26:09Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 5.46 2021-02-12T23:26:09Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:26:09Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:26:09Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:26:09Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:26:09Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:26:09Z- Filter 0 No beams used in subcloud update 2021-02-12T23:26:09Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:26:09Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:26:09Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 766.0 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:26:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172369.20 sec. 2021-02-12T23:26:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:26:09Z- TNavBF::mmse selected from filter -1 2021-02-12T23:26:09Z- variances([0], [2], and psi_berg) = 765.974301 833.049579 0.000000 2021-02-12T23:26:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:26:09Z- LcmTrn::publishEstimates() - MLE : 4056957.52 590903.72 29.55 2021-02-12T23:26:09Z- LcmTrn::publishEstimates() - MMSE : 4056976.02 590952.22 29.55 2021-02-12T23:26:09Z- LcmTrn::publishEstimates() - COVAR: 765.97 833.05 0.80 0.00 2021-02-12T23:26:10Z- AHRS_M2 msg: 1613172370.00 epoch sec; seqNo:3690 2021-02-12T23:26:10Z- AHRS_M2 msg: 0.60 phi; -0.19 theta; -0.18 psi 2021-02-12T23:26:10Z- handleDepth()-Depth_Keller msg: 1613172370002 epoch sec; seqNo:3728; depth 29.67 2021-02-12T23:26:10Z- handleDvl() - RDI_Pathfinder msg: 1613172370.02 epoch sec; seqNo:1750 2021-02-12T23:26:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.24 , 74.50 , 60.09 , 77.26 2021-02-12T23:26:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , -0.03 2021-02-12T23:26:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172370.03 epoch sec; seqNo:3728; 4056976.02 north; 590952.22 east 2021-02-12T23:26:10Z- AHRS_M2 msg: 1613172370.40 epoch sec; seqNo:3691 2021-02-12T23:26:10Z- AHRS_M2 msg: 0.59 phi; -0.15 theta; -0.16 psi 2021-02-12T23:26:10Z- handleDepth()-Depth_Keller msg: 1613172370414 epoch sec; seqNo:3729; depth 29.81 2021-02-12T23:26:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172370.43 epoch sec; seqNo:3729; 4056976.40 north; 590952.22 east 2021-02-12T23:26:10Z- AHRS_M2 msg: 1613172370.81 epoch sec; seqNo:3692 2021-02-12T23:26:10Z- AHRS_M2 msg: 0.58 phi; -0.10 theta; -0.16 psi 2021-02-12T23:26:10Z- handleDepth()-Depth_Keller msg: 1613172370824 epoch sec; seqNo:3730; depth 29.91 2021-02-12T23:26:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172370.85 epoch sec; seqNo:3730; 4056976.78 north; 590952.22 east 2021-02-12T23:26:11Z- AHRS_M2 msg: 1613172371.21 epoch sec; seqNo:3693 2021-02-12T23:26:11Z- AHRS_M2 msg: 0.57 phi; -0.06 theta; -0.15 psi 2021-02-12T23:26:11Z- handleDepth()-Depth_Keller msg: 1613172371221 epoch sec; seqNo:3731; depth 30.01 2021-02-12T23:26:11Z- handleDvl() - RDI_Pathfinder msg: 1613172371.23 epoch sec; seqNo:1751 2021-02-12T23:26:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.16 , 71.20 , 62.13 , 69.90 2021-02-12T23:26:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , -0.04 2021-02-12T23:26:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172371.24 epoch sec; seqNo:3731; 4056977.17 north; 590953.43 east 2021-02-12T23:26:11Z- AHRS_M2 msg: 1613172371.61 epoch sec; seqNo:3694 2021-02-12T23:26:11Z- AHRS_M2 msg: 0.56 phi; -0.01 theta; -0.14 psi 2021-02-12T23:26:11Z- handleDepth()-Depth_Keller msg: 1613172371616 epoch sec; seqNo:3732; depth 30.08 2021-02-12T23:26:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172371.63 epoch sec; seqNo:3732; 4056977.17 north; 590953.43 east 2021-02-12T23:26:12Z- AHRS_M2 msg: 1613172372.02 epoch sec; seqNo:3695 2021-02-12T23:26:12Z- AHRS_M2 msg: 0.56 phi; 0.03 theta; -0.14 psi 2021-02-12T23:26:12Z- handleDepth()-Depth_Keller msg: 1613172372020 epoch sec; seqNo:3733; depth 30.14 2021-02-12T23:26:12Z- handleDvl() - RDI_Pathfinder msg: 1613172372.03 epoch sec; seqNo:1752 2021-02-12T23:26:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.32 , 69.90 , 65.30 , 65.30 2021-02-12T23:26:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , -0.05 2021-02-12T23:26:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172372.05 epoch sec; seqNo:3733; 4056977.55 north; 590953.43 east 2021-02-12T23:26:12Z- AHRS_M2 msg: 1613172372.42 epoch sec; seqNo:3696 2021-02-12T23:26:12Z- AHRS_M2 msg: 0.55 phi; 0.07 theta; -0.14 psi 2021-02-12T23:26:12Z- handleDepth()-Depth_Keller msg: 1613172372424 epoch sec; seqNo:3734; depth 30.21 2021-02-12T23:26:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:12Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:12Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:12Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:12Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:12Z- handleDvl() - RDI_Pathfinder msg: 1613172372.43 epoch sec; seqNo:1753 2021-02-12T23:26:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , -0.05 2021-02-12T23:26:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172372.44 epoch sec; seqNo:3734; 4056977.93 north; 590953.43 east 2021-02-12T23:26:12Z- AHRS_M2 msg: 1613172372.84 epoch sec; seqNo:3697 2021-02-12T23:26:12Z- AHRS_M2 msg: 0.54 phi; 0.11 theta; -0.14 psi 2021-02-12T23:26:12Z- handleDepth()-Depth_Keller msg: 1613172372842 epoch sec; seqNo:3735; depth 30.24 2021-02-12T23:26:12Z- handleDvl() - RDI_Pathfinder msg: 1613172372.85 epoch sec; seqNo:1754 2021-02-12T23:26:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.93 , 69.90 , 69.90 , 61.93 2021-02-12T23:26:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , -0.06 2021-02-12T23:26:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172372.88 epoch sec; seqNo:3735; 4056978.31 north; 590953.42 east 2021-02-12T23:26:13Z- AHRS_M2 msg: 1613172373.23 epoch sec; seqNo:3698 2021-02-12T23:26:13Z- AHRS_M2 msg: 0.54 phi; 0.14 theta; -0.14 psi 2021-02-12T23:26:13Z- handleDepth()-Depth_Keller msg: 1613172373234 epoch sec; seqNo:3736; depth 30.26 2021-02-12T23:26:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172373.24 epoch sec; seqNo:3736; 4056978.31 north; 590953.42 east 2021-02-12T23:26:13Z- AHRS_M2 msg: 1613172373.63 epoch sec; seqNo:3699 2021-02-12T23:26:13Z- AHRS_M2 msg: 0.54 phi; 0.17 theta; -0.14 psi 2021-02-12T23:26:13Z- handleDepth()-Depth_Keller msg: 1613172373636 epoch sec; seqNo:3737; depth 30.26 2021-02-12T23:26:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:13Z- handleDvl() - RDI_Pathfinder msg: 1613172373.64 epoch sec; seqNo:1755 2021-02-12T23:26:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , -0.06 2021-02-12T23:26:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172373.66 epoch sec; seqNo:3737; 4056978.69 north; 590953.42 east 2021-02-12T23:26:14Z- AHRS_M2 msg: 1613172374.04 epoch sec; seqNo:3700 2021-02-12T23:26:14Z- AHRS_M2 msg: 0.53 phi; 0.20 theta; -0.13 psi 2021-02-12T23:26:14Z- handleDepth()-Depth_Keller msg: 1613172374040 epoch sec; seqNo:3738; depth 30.23 2021-02-12T23:26:14Z- handleDvl() - RDI_Pathfinder msg: 1613172374.05 epoch sec; seqNo:1756 2021-02-12T23:26:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.85 , 70.82 , 0.00 , 60.09 2021-02-12T23:26:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.05 2021-02-12T23:26:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172374.08 epoch sec; seqNo:3738; 4056979.08 north; 590954.63 east 2021-02-12T23:26:14Z- AHRS_M2 msg: 1613172374.44 epoch sec; seqNo:3701 2021-02-12T23:26:14Z- AHRS_M2 msg: 0.53 phi; 0.22 theta; -0.13 psi 2021-02-12T23:26:14Z- handleDepth()-Depth_Keller msg: 1613172374444 epoch sec; seqNo:3739; depth 30.21 2021-02-12T23:26:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172374.45 epoch sec; seqNo:3739; 4056979.46 north; 590954.63 east 2021-02-12T23:26:14Z- AHRS_M2 msg: 1613172374.85 epoch sec; seqNo:3702 2021-02-12T23:26:14Z- AHRS_M2 msg: 0.53 phi; 0.25 theta; -0.13 psi 2021-02-12T23:26:14Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-12T23:26:14Z- waiting for 1613172372.61, time is 1613172374.85 2021-02-12T23:26:14Z- LcmTrn::updateTrn() >>>> pose time 1613172374.85, meas time 1613172374.05 <<<< 2021-02-12T23:26:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172374.05 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:26:14Z- TerrainNav::motionUpdate() - currEstimate = (4056979, 590954.63, 30.21) 2021-02-12T23:26:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:26:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:26:14Z- TNavBF::Projecting Measurements 2021-02-12T23:26:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:26:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:26:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:26:14Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:26:14Z- TNavFilter::3 good beams. 2021-02-12T23:26:14Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:26:14Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:26:14Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:26:14Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:26:14Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:26:14Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:26:14Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:26:14Z- TNavBF::mmse selected from filter -1 2021-02-12T23:26:14Z- TerrainMapOctree: (vehN, vehE ) = (4056978.94, 590954.27). 2021-02-12T23:26:14Z- TerrainMapOctree: Min Distance = 5394.80. 2021-02-12T23:26:14Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:26:14Z- There is no specific good beam that all particles have in common. 2021-02-12T23:26:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:26:14Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 6.32 2021-02-12T23:26:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:26:14Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 8.03 2021-02-12T23:26:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:26:14Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 5.78 2021-02-12T23:26:14Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:26:14Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:26:14Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:26:14Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:26:14Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:26:14Z- Filter 0 No beams used in subcloud update 2021-02-12T23:26:14Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:26:14Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:26:14Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 765.9 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:26:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172374.05 sec. 2021-02-12T23:26:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:26:14Z- TNavBF::mmse selected from filter -1 2021-02-12T23:26:14Z- variances([0], [2], and psi_berg) = 765.810855 833.049390 0.000000 2021-02-12T23:26:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:26:14Z- LcmTrn::publishEstimates() - MLE : 4056960.96 590906.13 30.21 2021-02-12T23:26:14Z- LcmTrn::publishEstimates() - MMSE : 4056979.46 590954.63 30.21 2021-02-12T23:26:14Z- LcmTrn::publishEstimates() - COVAR: 765.81 833.05 0.76 0.00 2021-02-12T23:26:14Z- handleDepth()-Depth_Keller msg: 1613172374850 epoch sec; seqNo:3740; depth 30.18 2021-02-12T23:26:14Z- handleDvl() - RDI_Pathfinder msg: 1613172374.86 epoch sec; seqNo:1757 2021-02-12T23:26:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.08 , 71.43 , 0.00 , 58.87 2021-02-12T23:26:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.07 , -0.05 2021-02-12T23:26:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172374.87 epoch sec; seqNo:3740; 4056979.46 north; 590954.63 east 2021-02-12T23:26:15Z- AHRS_M2 msg: 1613172375.25 epoch sec; seqNo:3703 2021-02-12T23:26:15Z- AHRS_M2 msg: 0.53 phi; 0.27 theta; -0.12 psi 2021-02-12T23:26:15Z- handleDepth()-Depth_Keller msg: 1613172375252 epoch sec; seqNo:3741; depth 30.13 2021-02-12T23:26:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172375.27 epoch sec; seqNo:3741; 4056979.84 north; 590954.63 east 2021-02-12T23:26:15Z- AHRS_M2 msg: 1613172375.65 epoch sec; seqNo:3704 2021-02-12T23:26:15Z- AHRS_M2 msg: 0.53 phi; 0.29 theta; -0.12 psi 2021-02-12T23:26:15Z- handleDepth()-Depth_Keller msg: 1613172375656 epoch sec; seqNo:3742; depth 30.07 2021-02-12T23:26:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172375.66 epoch sec; seqNo:3742; 4056980.21 north; 590954.62 east 2021-02-12T23:26:16Z- AHRS_M2 msg: 1613172376.06 epoch sec; seqNo:3705 2021-02-12T23:26:16Z- AHRS_M2 msg: 0.53 phi; 0.31 theta; -0.12 psi 2021-02-12T23:26:16Z- handleDepth()-Depth_Keller msg: 1613172376060 epoch sec; seqNo:3743; depth 30.02 2021-02-12T23:26:16Z- handleDvl() - RDI_Pathfinder msg: 1613172376.07 epoch sec; seqNo:1758 2021-02-12T23:26:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.30 , 71.74 , 0.00 , 58.56 2021-02-12T23:26:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , -0.02 2021-02-12T23:26:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172376.08 epoch sec; seqNo:3743; 4056980.59 north; 590954.62 east 2021-02-12T23:26:16Z- AHRS_M2 msg: 1613172376.46 epoch sec; seqNo:3706 2021-02-12T23:26:16Z- AHRS_M2 msg: 0.53 phi; 0.32 theta; -0.12 psi 2021-02-12T23:26:16Z- handleDepth()-Depth_Keller msg: 1613172376466 epoch sec; seqNo:3744; depth 29.94 2021-02-12T23:26:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172376.48 epoch sec; seqNo:3744; 4056980.59 north; 590954.62 east 2021-02-12T23:26:16Z- AHRS_M2 msg: 1613172376.86 epoch sec; seqNo:3707 2021-02-12T23:26:16Z- AHRS_M2 msg: 0.53 phi; 0.33 theta; -0.12 psi 2021-02-12T23:26:16Z- handleDepth()-Depth_Keller msg: 1613172376878 epoch sec; seqNo:3745; depth 29.85 2021-02-12T23:26:16Z- handleDvl() - RDI_Pathfinder msg: 1613172376.89 epoch sec; seqNo:1759 2021-02-12T23:26:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.53 , 73.27 , 0.00 , 58.56 2021-02-12T23:26:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , 0.03 2021-02-12T23:26:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172376.90 epoch sec; seqNo:3745; 4056980.97 north; 590954.61 east 2021-02-12T23:26:17Z- AHRS_M2 msg: 1613172377.27 epoch sec; seqNo:3708 2021-02-12T23:26:17Z- AHRS_M2 msg: 0.53 phi; 0.34 theta; -0.12 psi 2021-02-12T23:26:17Z- handleDepth()-Depth_Keller msg: 1613172377277 epoch sec; seqNo:3746; depth 29.76 2021-02-12T23:26:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:17Z- handleDvl() - RDI_Pathfinder msg: 1613172377.28 epoch sec; seqNo:1760 2021-02-12T23:26:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , 0.03 2021-02-12T23:26:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172377.30 epoch sec; seqNo:3746; 4056981.35 north; 590954.61 east 2021-02-12T23:26:17Z- AHRS_M2 msg: 1613172377.67 epoch sec; seqNo:3709 2021-02-12T23:26:17Z- AHRS_M2 msg: 0.53 phi; 0.35 theta; -0.12 psi 2021-02-12T23:26:17Z- handleDepth()-Depth_Keller msg: 1613172377681 epoch sec; seqNo:3747; depth 29.66 2021-02-12T23:26:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172377.69 epoch sec; seqNo:3747; 4056981.74 north; 590955.83 east 2021-02-12T23:26:18Z- AHRS_M2 msg: 1613172378.08 epoch sec; seqNo:3710 2021-02-12T23:26:18Z- AHRS_M2 msg: 0.53 phi; 0.36 theta; -0.12 psi 2021-02-12T23:26:18Z- handleDepth()-Depth_Keller msg: 1613172378082 epoch sec; seqNo:3748; depth 29.56 2021-02-12T23:26:18Z- handleDvl() - RDI_Pathfinder msg: 1613172378.09 epoch sec; seqNo:1761 2021-02-12T23:26:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.14 , 73.58 , 0.00 , 58.87 2021-02-12T23:26:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , 0.05 2021-02-12T23:26:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172378.10 epoch sec; seqNo:3748; 4056981.74 north; 590955.83 east 2021-02-12T23:26:18Z- AHRS_M2 msg: 1613172378.48 epoch sec; seqNo:3711 2021-02-12T23:26:18Z- AHRS_M2 msg: 0.54 phi; 0.36 theta; -0.12 psi 2021-02-12T23:26:18Z- handleDepth()-Depth_Keller msg: 1613172378484 epoch sec; seqNo:3749; depth 29.45 2021-02-12T23:26:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:18Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:18Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:18Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:18Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:18Z- handleDvl() - RDI_Pathfinder msg: 1613172378.49 epoch sec; seqNo:1762 2021-02-12T23:26:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , 0.05 2021-02-12T23:26:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172378.51 epoch sec; seqNo:3749; 4056982.12 north; 590955.82 east 2021-02-12T23:26:18Z- AHRS_M2 msg: 1613172378.88 epoch sec; seqNo:3712 2021-02-12T23:26:18Z- AHRS_M2 msg: 0.54 phi; 0.37 theta; -0.11 psi 2021-02-12T23:26:18Z- handleDepth()-Depth_Keller msg: 1613172378892 epoch sec; seqNo:3750; depth 29.33 2021-02-12T23:26:18Z- handleDvl() - RDI_Pathfinder msg: 1613172378.93 epoch sec; seqNo:1763 2021-02-12T23:26:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.37 , 73.89 , 0.00 , 58.25 2021-02-12T23:26:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.03 , 0.05 2021-02-12T23:26:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172378.95 epoch sec; seqNo:3750; 4056982.50 north; 590955.82 east 2021-02-12T23:26:19Z- AHRS_M2 msg: 1613172379.29 epoch sec; seqNo:3713 2021-02-12T23:26:19Z- AHRS_M2 msg: 0.54 phi; 0.37 theta; -0.11 psi 2021-02-12T23:26:19Z- handleDepth()-Depth_Keller msg: 1613172379292 epoch sec; seqNo:3751; depth 29.22 2021-02-12T23:26:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172379.30 epoch sec; seqNo:3751; 4056982.50 north; 590955.82 east 2021-02-12T23:26:19Z- AHRS_M2 msg: 1613172379.69 epoch sec; seqNo:3714 2021-02-12T23:26:19Z- AHRS_M2 msg: 0.55 phi; 0.38 theta; -0.11 psi 2021-02-12T23:26:19Z- handleDepth()-Depth_Keller msg: 1613172379696 epoch sec; seqNo:3752; depth 29.11 2021-02-12T23:26:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:19Z- handleDvl() - RDI_Pathfinder msg: 1613172379.70 epoch sec; seqNo:1764 2021-02-12T23:26:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.03 , 0.05 2021-02-12T23:26:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172379.71 epoch sec; seqNo:3752; 4056982.88 north; 590955.82 east 2021-02-12T23:26:19Z- LcmTrn::handleMessages() - handling 46 messages took 0.69 ms and 132 calls... 2021-02-12T23:26:19Z- waiting for 1613172377.85, time is 1613172379.71 2021-02-12T23:26:19Z- LcmTrn::updateTrn() equating pose 1613172379.71 and meas times 1613172379.71 2021-02-12T23:26:19Z- LcmTrn::updateTrn() >>>> pose time 1613172379.71, meas time 1613172379.71 <<<< 2021-02-12T23:26:19Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172379.71, ping # 0. 2021-02-12T23:26:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172379.71 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:26:19Z- TerrainNav::motionUpdate() - currEstimate = (4056983, 590955.82, 29.11) 2021-02-12T23:26:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:26:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:26:19Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:26:19Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:26:19Z- TNavBF::Projecting Measurements 2021-02-12T23:26:19Z- TNavFilter::Measurement from time = 1613172379.71 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:26:19Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:26:19Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:26:19Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:26:19Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:26:19Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:26:19Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:26:19Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:26:19Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 765.7 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:26:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:26:19Z- TNavBF::mmse selected from filter -1 2021-02-12T23:26:19Z- variances([0], [2], and psi_berg) = 765.695171 833.049257 0.000000 2021-02-12T23:26:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:26:19Z- LcmTrn::publishEstimates() - MLE : 4056964.38 590907.32 29.11 2021-02-12T23:26:19Z- LcmTrn::publishEstimates() - MMSE : 4056982.88 590955.82 29.11 2021-02-12T23:26:19Z- LcmTrn::publishEstimates() - COVAR: 765.70 833.05 0.94 0.00 2021-02-12T23:26:20Z- AHRS_M2 msg: 1613172380.10 epoch sec; seqNo:3715 2021-02-12T23:26:20Z- AHRS_M2 msg: 0.55 phi; 0.38 theta; -0.11 psi 2021-02-12T23:26:20Z- handleDepth()-Depth_Keller msg: 1613172380100 epoch sec; seqNo:3753; depth 28.99 2021-02-12T23:26:20Z- handleDvl() - RDI_Pathfinder msg: 1613172380.13 epoch sec; seqNo:1765 2021-02-12T23:26:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.98 , 74.19 , 0.00 , 59.17 2021-02-12T23:26:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.07 , 0.03 2021-02-12T23:26:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172380.14 epoch sec; seqNo:3753; 4056983.26 north; 590955.81 east 2021-02-12T23:26:20Z- AHRS_M2 msg: 1613172380.50 epoch sec; seqNo:3716 2021-02-12T23:26:20Z- AHRS_M2 msg: 0.56 phi; 0.38 theta; -0.10 psi 2021-02-12T23:26:20Z- handleDepth()-Depth_Keller msg: 1613172380511 epoch sec; seqNo:3754; depth 28.86 2021-02-12T23:26:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172380.52 epoch sec; seqNo:3754; 4056983.27 north; 590957.03 east 2021-02-12T23:26:20Z- AHRS_M2 msg: 1613172380.91 epoch sec; seqNo:3717 2021-02-12T23:26:20Z- AHRS_M2 msg: 0.57 phi; 0.38 theta; -0.10 psi 2021-02-12T23:26:20Z- handleDepth()-Depth_Keller msg: 1613172380915 epoch sec; seqNo:3755; depth 28.74 2021-02-12T23:26:20Z- handleDvl() - RDI_Pathfinder msg: 1613172380.93 epoch sec; seqNo:1766 2021-02-12T23:26:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.58 , 74.43 , 0.00 , 59.55 2021-02-12T23:26:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.09 , 0.01 2021-02-12T23:26:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172380.94 epoch sec; seqNo:3755; 4056983.65 north; 590957.03 east 2021-02-12T23:26:21Z- AHRS_M2 msg: 1613172381.32 epoch sec; seqNo:3718 2021-02-12T23:26:21Z- AHRS_M2 msg: 0.58 phi; 0.38 theta; -0.10 psi 2021-02-12T23:26:21Z- handleDepth()-Depth_Keller msg: 1613172381353 epoch sec; seqNo:3756; depth 28.62 2021-02-12T23:26:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172381.38 epoch sec; seqNo:3756; 4056984.03 north; 590957.02 east 2021-02-12T23:26:21Z- AHRS_M2 msg: 1613172381.71 epoch sec; seqNo:3719 2021-02-12T23:26:21Z- AHRS_M2 msg: 0.59 phi; 0.38 theta; -0.10 psi 2021-02-12T23:26:21Z- handleDepth()-Depth_Keller msg: 1613172381716 epoch sec; seqNo:3757; depth 28.49 2021-02-12T23:26:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172381.72 epoch sec; seqNo:3757; 4056984.03 north; 590957.02 east 2021-02-12T23:26:22Z- AHRS_M2 msg: 1613172382.12 epoch sec; seqNo:3720 2021-02-12T23:26:22Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.10 psi 2021-02-12T23:26:22Z- handleDepth()-Depth_Keller msg: 1613172382123 epoch sec; seqNo:3758; depth 28.38 2021-02-12T23:26:22Z- handleDvl() - RDI_Pathfinder msg: 1613172382.14 epoch sec; seqNo:1767 2021-02-12T23:26:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.90 , 75.08 , 0.00 , 58.25 2021-02-12T23:26:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.06 , -0.01 2021-02-12T23:26:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172382.16 epoch sec; seqNo:3758; 4056984.41 north; 590957.02 east 2021-02-12T23:26:22Z- AHRS_M2 msg: 1613172382.52 epoch sec; seqNo:3721 2021-02-12T23:26:22Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.10 psi 2021-02-12T23:26:22Z- handleDepth()-Depth_Keller msg: 1613172382527 epoch sec; seqNo:3759; depth 28.23 2021-02-12T23:26:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172382.54 epoch sec; seqNo:3759; 4056984.79 north; 590957.02 east 2021-02-12T23:26:22Z- AHRS_M2 msg: 1613172382.92 epoch sec; seqNo:3722 2021-02-12T23:26:22Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.10 psi 2021-02-12T23:26:22Z- handleDepth()-Depth_Keller msg: 1613172382933 epoch sec; seqNo:3760; depth 28.12 2021-02-12T23:26:22Z- handleDvl() - RDI_Pathfinder msg: 1613172382.95 epoch sec; seqNo:1768 2021-02-12T23:26:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.90 , 75.40 , 0.00 , 59.55 2021-02-12T23:26:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.04 , -0.02 2021-02-12T23:26:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172382.96 epoch sec; seqNo:3760; 4056984.80 north; 590958.24 east 2021-02-12T23:26:23Z- AHRS_M2 msg: 1613172383.33 epoch sec; seqNo:3723 2021-02-12T23:26:23Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.10 psi 2021-02-12T23:26:23Z- handleDepth()-Depth_Keller msg: 1613172383332 epoch sec; seqNo:3761; depth 27.97 2021-02-12T23:26:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:23Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:23Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:23Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:23Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:23Z- handleDvl() - RDI_Pathfinder msg: 1613172383.33 epoch sec; seqNo:1769 2021-02-12T23:26:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.04 , -0.02 2021-02-12T23:26:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172383.35 epoch sec; seqNo:3761; 4056985.18 north; 590958.23 east 2021-02-12T23:26:23Z- AHRS_M2 msg: 1613172383.73 epoch sec; seqNo:3724 2021-02-12T23:26:23Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.11 psi 2021-02-12T23:26:23Z- handleDepth()-Depth_Keller msg: 1613172383737 epoch sec; seqNo:3762; depth 27.85 2021-02-12T23:26:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172383.75 epoch sec; seqNo:3762; 4056985.18 north; 590958.23 east 2021-02-12T23:26:24Z- AHRS_M2 msg: 1613172384.14 epoch sec; seqNo:3725 2021-02-12T23:26:24Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.11 psi 2021-02-12T23:26:24Z- handleDepth()-Depth_Keller msg: 1613172384146 epoch sec; seqNo:3763; depth 27.72 2021-02-12T23:26:24Z- handleDvl() - RDI_Pathfinder msg: 1613172384.16 epoch sec; seqNo:1770 2021-02-12T23:26:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.58 , 75.73 , 0.00 , 60.19 2021-02-12T23:26:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.01 , -0.01 2021-02-12T23:26:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172384.17 epoch sec; seqNo:3763; 4056985.56 north; 590958.23 east 2021-02-12T23:26:24Z- AHRS_M2 msg: 1613172384.54 epoch sec; seqNo:3726 2021-02-12T23:26:24Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.11 psi 2021-02-12T23:26:24Z- handleDepth()-Depth_Keller msg: 1613172384544 epoch sec; seqNo:3764; depth 27.58 2021-02-12T23:26:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:24Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:24Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:24Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:24Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:24Z- handleDvl() - RDI_Pathfinder msg: 1613172384.55 epoch sec; seqNo:1771 2021-02-12T23:26:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.01 , -0.01 2021-02-12T23:26:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172384.57 epoch sec; seqNo:3764; 4056985.94 north; 590958.23 east 2021-02-12T23:26:24Z- AHRS_M2 msg: 1613172384.94 epoch sec; seqNo:3727 2021-02-12T23:26:24Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.12 psi 2021-02-12T23:26:24Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-12T23:26:24Z- waiting for 1613172382.71, time is 1613172384.94 2021-02-12T23:26:24Z- LcmTrn::updateTrn() >>>> pose time 1613172384.94, meas time 1613172384.55 <<<< 2021-02-12T23:26:24Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172384.55, ping # 0. 2021-02-12T23:26:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172384.55 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:26:24Z- TerrainNav::motionUpdate() - currEstimate = (4056986, 590958.23, 27.58) 2021-02-12T23:26:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:26:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:26:24Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:26:24Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:26:24Z- TNavBF::Projecting Measurements 2021-02-12T23:26:24Z- TNavFilter::Measurement from time = 1613172384.55 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:26:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:26:24Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:26:24Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:26:24Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:26:24Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:26:24Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:26:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:26:24Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 765.6 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:26:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:26:24Z- TNavBF::mmse selected from filter -1 2021-02-12T23:26:24Z- variances([0], [2], and psi_berg) = 765.547159 833.049087 0.000000 2021-02-12T23:26:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:26:24Z- LcmTrn::publishEstimates() - MLE : 4056967.44 590909.73 27.58 2021-02-12T23:26:24Z- LcmTrn::publishEstimates() - MMSE : 4056985.94 590958.23 27.58 2021-02-12T23:26:24Z- LcmTrn::publishEstimates() - COVAR: 765.55 833.05 1.14 0.00 2021-02-12T23:26:24Z- handleDepth()-Depth_Keller msg: 1613172384953 epoch sec; seqNo:3765; depth 27.47 2021-02-12T23:26:24Z- handleDvl() - RDI_Pathfinder msg: 1613172384.96 epoch sec; seqNo:1772 2021-02-12T23:26:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.94 , 76.06 , 0.00 , 60.52 2021-02-12T23:26:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , 0.01 2021-02-12T23:26:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172384.99 epoch sec; seqNo:3765; 4056985.94 north; 590958.23 east 2021-02-12T23:26:25Z- AHRS_M2 msg: 1613172385.35 epoch sec; seqNo:3728 2021-02-12T23:26:25Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.13 psi 2021-02-12T23:26:25Z- handleDepth()-Depth_Keller msg: 1613172385352 epoch sec; seqNo:3766; depth 27.32 2021-02-12T23:26:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172385.36 epoch sec; seqNo:3766; 4056986.32 north; 590958.22 east 2021-02-12T23:26:25Z- AHRS_M2 msg: 1613172385.76 epoch sec; seqNo:3729 2021-02-12T23:26:25Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-12T23:26:25Z- handleDepth()-Depth_Keller msg: 1613172385764 epoch sec; seqNo:3767; depth 27.19 2021-02-12T23:26:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:25Z- handleDvl() - RDI_Pathfinder msg: 1613172385.77 epoch sec; seqNo:1773 2021-02-12T23:26:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , 0.01 2021-02-12T23:26:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172385.78 epoch sec; seqNo:3767; 4056986.71 north; 590959.44 east 2021-02-12T23:26:26Z- AHRS_M2 msg: 1613172386.16 epoch sec; seqNo:3730 2021-02-12T23:26:26Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-12T23:26:26Z- handleDepth()-Depth_Keller msg: 1613172386165 epoch sec; seqNo:3768; depth 27.07 2021-02-12T23:26:26Z- handleDvl() - RDI_Pathfinder msg: 1613172386.18 epoch sec; seqNo:1774 2021-02-12T23:26:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.61 , 77.68 , 0.00 , 60.52 2021-02-12T23:26:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.03 2021-02-12T23:26:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172386.19 epoch sec; seqNo:3768; 4056986.71 north; 590959.44 east 2021-02-12T23:26:26Z- AHRS_M2 msg: 1613172386.56 epoch sec; seqNo:3731 2021-02-12T23:26:26Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-12T23:26:26Z- handleDepth()-Depth_Keller msg: 1613172386569 epoch sec; seqNo:3769; depth 26.92 2021-02-12T23:26:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172386.58 epoch sec; seqNo:3769; 4056987.09 north; 590959.43 east 2021-02-12T23:26:26Z- AHRS_M2 msg: 1613172386.96 epoch sec; seqNo:3732 2021-02-12T23:26:26Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-12T23:26:26Z- handleDepth()-Depth_Keller msg: 1613172386969 epoch sec; seqNo:3770; depth 26.82 2021-02-12T23:26:26Z- handleDvl() - RDI_Pathfinder msg: 1613172386.98 epoch sec; seqNo:1775 2021-02-12T23:26:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.28 , 79.29 , 0.00 , 61.16 2021-02-12T23:26:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.05 2021-02-12T23:26:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172386.99 epoch sec; seqNo:3770; 4056987.46 north; 590959.43 east 2021-02-12T23:26:27Z- AHRS_M2 msg: 1613172387.37 epoch sec; seqNo:3733 2021-02-12T23:26:27Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-12T23:26:27Z- handleDepth()-Depth_Keller msg: 1613172387381 epoch sec; seqNo:3771; depth 26.68 2021-02-12T23:26:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172387.39 epoch sec; seqNo:3771; 4056987.84 north; 590959.43 east 2021-02-12T23:26:27Z- AHRS_M2 msg: 1613172387.77 epoch sec; seqNo:3734 2021-02-12T23:26:27Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-12T23:26:27Z- handleDepth()-Depth_Keller msg: 1613172387779 epoch sec; seqNo:3772; depth 26.56 2021-02-12T23:26:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172387.79 epoch sec; seqNo:3772; 4056988.22 north; 590959.42 east 2021-02-12T23:26:28Z- AHRS_M2 msg: 1613172388.18 epoch sec; seqNo:3735 2021-02-12T23:26:28Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-12T23:26:28Z- handleDepth()-Depth_Keller msg: 1613172388180 epoch sec; seqNo:3773; depth 26.44 2021-02-12T23:26:28Z- handleDvl() - RDI_Pathfinder msg: 1613172388.19 epoch sec; seqNo:1776 2021-02-12T23:26:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.28 , 80.26 , 0.00 , 61.49 2021-02-12T23:26:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , 0.06 2021-02-12T23:26:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172388.20 epoch sec; seqNo:3773; 4056988.22 north; 590959.42 east 2021-02-12T23:26:28Z- AHRS_M2 msg: 1613172388.58 epoch sec; seqNo:3736 2021-02-12T23:26:28Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-12T23:26:28Z- handleDepth()-Depth_Keller msg: 1613172388587 epoch sec; seqNo:3774; depth 26.31 2021-02-12T23:26:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172388.60 epoch sec; seqNo:3774; 4056988.61 north; 590960.64 east 2021-02-12T23:26:28Z- AHRS_M2 msg: 1613172388.98 epoch sec; seqNo:3737 2021-02-12T23:26:28Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-12T23:26:28Z- handleDepth()-Depth_Keller msg: 1613172388989 epoch sec; seqNo:3775; depth 26.21 2021-02-12T23:26:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172389.01 epoch sec; seqNo:3775; 4056988.99 north; 590960.63 east 2021-02-12T23:26:29Z- AHRS_M2 msg: 1613172389.39 epoch sec; seqNo:3738 2021-02-12T23:26:29Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-12T23:26:29Z- handleDepth()-Depth_Keller msg: 1613172389392 epoch sec; seqNo:3776; depth 26.08 2021-02-12T23:26:29Z- handleDvl() - RDI_Pathfinder msg: 1613172389.40 epoch sec; seqNo:1777 2021-02-12T23:26:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 82.77 , 96.83 , 61.29 2021-02-12T23:26:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , 0.04 2021-02-12T23:26:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172389.43 epoch sec; seqNo:3776; 4056989.37 north; 590960.63 east 2021-02-12T23:26:29Z- AHRS_M2 msg: 1613172389.79 epoch sec; seqNo:3739 2021-02-12T23:26:29Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-12T23:26:29Z- handleDepth()-Depth_Keller msg: 1613172389801 epoch sec; seqNo:3777; depth 25.95 2021-02-12T23:26:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172389.81 epoch sec; seqNo:3777; 4056989.37 north; 590960.63 east 2021-02-12T23:26:29Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-12T23:26:29Z- waiting for 1613172387.94, time is 1613172389.81 2021-02-12T23:26:29Z- LcmTrn::updateTrn() >>>> pose time 1613172389.81, meas time 1613172389.40 <<<< 2021-02-12T23:26:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172389.40 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:26:29Z- TerrainNav::motionUpdate() - currEstimate = (4056989, 590960.63, 25.95) 2021-02-12T23:26:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:26:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:26:29Z- TNavBF::Projecting Measurements 2021-02-12T23:26:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:26:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:26:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:26:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:26:29Z- TNavFilter::4 good beams. 2021-02-12T23:26:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:26:29Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:26:29Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:26:29Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:26:29Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:26:29Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:26:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:26:29Z- TNavBF::mmse selected from filter -1 2021-02-12T23:26:29Z- TerrainMapOctree: (vehN, vehE ) = (4056989.08, 590960.43). 2021-02-12T23:26:29Z- TerrainMapOctree: Min Distance = 5383.00. 2021-02-12T23:26:29Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:26:29Z- There is no specific good beam that all particles have in common. 2021-02-12T23:26:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:26:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.126 ExpectedMeasDiff: nan currMeas.covariance: 7.54 2021-02-12T23:26:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:26:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 10.96 2021-02-12T23:26:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:26:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.00 2021-02-12T23:26:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:26:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.115 ExpectedMeasDiff: nan currMeas.covariance: 6.01 2021-02-12T23:26:30Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:26:30Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:26:30Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:26:30Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:26:30Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:26:30Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:26:30Z- Filter 0 No beams used in subcloud update 2021-02-12T23:26:30Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:26:30Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:26:30Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 765.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:26:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172389.40 sec. 2021-02-12T23:26:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:26:30Z- TNavBF::mmse selected from filter -1 2021-02-12T23:26:30Z- variances([0], [2], and psi_berg) = 765.392611 833.048910 0.000000 2021-02-12T23:26:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:26:30Z- LcmTrn::publishEstimates() - MLE : 4056970.87 590912.13 25.95 2021-02-12T23:26:30Z- LcmTrn::publishEstimates() - MMSE : 4056989.37 590960.63 25.95 2021-02-12T23:26:30Z- LcmTrn::publishEstimates() - COVAR: 765.39 833.05 0.91 0.00 2021-02-12T23:26:30Z- AHRS_M2 msg: 1613172390.22 epoch sec; seqNo:3740 2021-02-12T23:26:30Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-12T23:26:30Z- handleDepth()-Depth_Keller msg: 1613172390220 epoch sec; seqNo:3778; depth 25.85 2021-02-12T23:26:30Z- handleDvl() - RDI_Pathfinder msg: 1613172390.23 epoch sec; seqNo:1778 2021-02-12T23:26:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 84.93 , 94.67 , 61.70 2021-02-12T23:26:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , 0.02 2021-02-12T23:26:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172390.24 epoch sec; seqNo:3778; 4056989.75 north; 590960.63 east 2021-02-12T23:26:30Z- AHRS_M2 msg: 1613172390.60 epoch sec; seqNo:3741 2021-02-12T23:26:30Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-12T23:26:30Z- handleDepth()-Depth_Keller msg: 1613172390604 epoch sec; seqNo:3779; depth 25.70 2021-02-12T23:26:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:30Z- handleDvl() - RDI_Pathfinder msg: 1613172390.61 epoch sec; seqNo:1779 2021-02-12T23:26:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , 0.02 2021-02-12T23:26:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172390.62 epoch sec; seqNo:3779; 4056990.13 north; 590960.62 east 2021-02-12T23:26:31Z- AHRS_M2 msg: 1613172391.00 epoch sec; seqNo:3742 2021-02-12T23:26:31Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-12T23:26:31Z- handleDepth()-Depth_Keller msg: 1613172391013 epoch sec; seqNo:3780; depth 25.60 2021-02-12T23:26:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172391.02 epoch sec; seqNo:3780; 4056990.14 north; 590961.84 east 2021-02-12T23:26:31Z- AHRS_M2 msg: 1613172391.41 epoch sec; seqNo:3743 2021-02-12T23:26:31Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-12T23:26:31Z- handleDepth()-Depth_Keller msg: 1613172391422 epoch sec; seqNo:3781; depth 25.47 2021-02-12T23:26:31Z- handleDvl() - RDI_Pathfinder msg: 1613172391.43 epoch sec; seqNo:1780 2021-02-12T23:26:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.46 , 83.85 , 93.58 , 62.93 2021-02-12T23:26:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , -0.00 2021-02-12T23:26:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172391.46 epoch sec; seqNo:3781; 4056990.52 north; 590961.84 east 2021-02-12T23:26:31Z- AHRS_M2 msg: 1613172391.81 epoch sec; seqNo:3744 2021-02-12T23:26:31Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-12T23:26:31Z- handleDepth()-Depth_Keller msg: 1613172391818 epoch sec; seqNo:3782; depth 25.34 2021-02-12T23:26:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:31Z- handleDvl() - RDI_Pathfinder msg: 1613172391.82 epoch sec; seqNo:1781 2021-02-12T23:26:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , -0.00 2021-02-12T23:26:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172391.83 epoch sec; seqNo:3782; 4056990.90 north; 590961.84 east 2021-02-12T23:26:32Z- AHRS_M2 msg: 1613172392.22 epoch sec; seqNo:3745 2021-02-12T23:26:32Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:26:32Z- handleDepth()-Depth_Keller msg: 1613172392229 epoch sec; seqNo:3783; depth 25.23 2021-02-12T23:26:32Z- handleDvl() - RDI_Pathfinder msg: 1613172392.24 epoch sec; seqNo:1782 2021-02-12T23:26:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.46 , 82.77 , 95.75 , 62.93 2021-02-12T23:26:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , 0.01 2021-02-12T23:26:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172392.25 epoch sec; seqNo:3783; 4056991.28 north; 590961.83 east 2021-02-12T23:26:32Z- AHRS_M2 msg: 1613172392.62 epoch sec; seqNo:3746 2021-02-12T23:26:32Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:26:32Z- handleDepth()-Depth_Keller msg: 1613172392624 epoch sec; seqNo:3784; depth 25.10 2021-02-12T23:26:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172392.63 epoch sec; seqNo:3784; 4056991.28 north; 590961.83 east 2021-02-12T23:26:33Z- AHRS_M2 msg: 1613172393.02 epoch sec; seqNo:3747 2021-02-12T23:26:33Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:26:33Z- handleDepth()-Depth_Keller msg: 1613172393028 epoch sec; seqNo:3785; depth 24.99 2021-02-12T23:26:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:33Z- handleDvl() - RDI_Pathfinder msg: 1613172393.03 epoch sec; seqNo:1783 2021-02-12T23:26:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , 0.01 2021-02-12T23:26:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172393.04 epoch sec; seqNo:3785; 4056991.66 north; 590961.83 east 2021-02-12T23:26:33Z- AHRS_M2 msg: 1613172393.44 epoch sec; seqNo:3748 2021-02-12T23:26:33Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:26:33Z- handleDepth()-Depth_Keller msg: 1613172393446 epoch sec; seqNo:3786; depth 24.88 2021-02-12T23:26:33Z- handleDvl() - RDI_Pathfinder msg: 1613172393.46 epoch sec; seqNo:1784 2021-02-12T23:26:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.87 , 83.85 , 95.75 , 63.33 2021-02-12T23:26:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.02 2021-02-12T23:26:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172393.47 epoch sec; seqNo:3786; 4056992.05 north; 590963.04 east 2021-02-12T23:26:33Z- AHRS_M2 msg: 1613172393.83 epoch sec; seqNo:3749 2021-02-12T23:26:33Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-12T23:26:33Z- handleDepth()-Depth_Keller msg: 1613172393841 epoch sec; seqNo:3787; depth 24.75 2021-02-12T23:26:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172393.86 epoch sec; seqNo:3787; 4056992.05 north; 590963.04 east 2021-02-12T23:26:34Z- AHRS_M2 msg: 1613172394.24 epoch sec; seqNo:3750 2021-02-12T23:26:34Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-12T23:26:34Z- handleDepth()-Depth_Keller msg: 1613172394240 epoch sec; seqNo:3788; depth 24.64 2021-02-12T23:26:34Z- handleDvl() - RDI_Pathfinder msg: 1613172394.25 epoch sec; seqNo:1785 2021-02-12T23:26:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.28 , 83.85 , 95.75 , 62.52 2021-02-12T23:26:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , 0.03 2021-02-12T23:26:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172394.28 epoch sec; seqNo:3788; 4056992.43 north; 590963.04 east 2021-02-12T23:26:34Z- AHRS_M2 msg: 1613172394.64 epoch sec; seqNo:3751 2021-02-12T23:26:34Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-12T23:26:34Z- handleDepth()-Depth_Keller msg: 1613172394643 epoch sec; seqNo:3789; depth 24.51 2021-02-12T23:26:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172394.65 epoch sec; seqNo:3789; 4056992.81 north; 590963.04 east 2021-02-12T23:26:35Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-12T23:26:35Z- waiting for 1613172392.81, time is 1613172394.65 2021-02-12T23:26:35Z- LcmTrn::updateTrn() >>>> pose time 1613172394.65, meas time 1613172394.25 <<<< 2021-02-12T23:26:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172394.25 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:26:35Z- TerrainNav::motionUpdate() - currEstimate = (4056993, 590963.04, 24.51) 2021-02-12T23:26:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:26:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:26:35Z- TNavBF::Projecting Measurements 2021-02-12T23:26:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:26:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:26:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:26:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:26:35Z- TNavFilter::4 good beams. 2021-02-12T23:26:35Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:26:35Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:26:35Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:26:35Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:26:35Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:26:35Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:26:35Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:26:35Z- TNavBF::mmse selected from filter -1 2021-02-12T23:26:35Z- TerrainMapOctree: (vehN, vehE ) = (4056992.52, 590962.84). 2021-02-12T23:26:35Z- TerrainMapOctree: Min Distance = 5378.81. 2021-02-12T23:26:35Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:26:35Z- There is no specific good beam that all particles have in common. 2021-02-12T23:26:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:26:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 7.90 2021-02-12T23:26:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:26:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 11.25 2021-02-12T23:26:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:26:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 14.67 2021-02-12T23:26:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:26:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 6.25 2021-02-12T23:26:35Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:26:35Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:26:35Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:26:35Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:26:35Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:26:35Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:26:35Z- Filter 0 No beams used in subcloud update 2021-02-12T23:26:35Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:26:35Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:26:35Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 765.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:26:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172394.25 sec. 2021-02-12T23:26:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:26:35Z- TNavBF::mmse selected from filter -1 2021-02-12T23:26:35Z- variances([0], [2], and psi_berg) = 765.238532 833.048733 0.000000 2021-02-12T23:26:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:26:35Z- LcmTrn::publishEstimates() - MLE : 4056974.31 590914.54 24.51 2021-02-12T23:26:35Z- LcmTrn::publishEstimates() - MMSE : 4056992.81 590963.04 24.51 2021-02-12T23:26:35Z- LcmTrn::publishEstimates() - COVAR: 765.24 833.05 0.80 0.00 2021-02-12T23:26:35Z- AHRS_M2 msg: 1613172395.06 epoch sec; seqNo:3752 2021-02-12T23:26:35Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-12T23:26:35Z- handleDepth()-Depth_Keller msg: 1613172395062 epoch sec; seqNo:3790; depth 24.41 2021-02-12T23:26:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172395.08 epoch sec; seqNo:3790; 4056993.19 north; 590963.03 east 2021-02-12T23:26:35Z- AHRS_M2 msg: 1613172395.45 epoch sec; seqNo:3753 2021-02-12T23:26:35Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-12T23:26:35Z- handleDepth()-Depth_Keller msg: 1613172395452 epoch sec; seqNo:3791; depth 24.28 2021-02-12T23:26:35Z- handleDvl() - RDI_Pathfinder msg: 1613172395.48 epoch sec; seqNo:1786 2021-02-12T23:26:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.28 , 79.27 , 96.83 , 63.33 2021-02-12T23:26:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , 0.02 2021-02-12T23:26:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172395.49 epoch sec; seqNo:3791; 4056993.19 north; 590963.03 east 2021-02-12T23:26:35Z- AHRS_M2 msg: 1613172395.85 epoch sec; seqNo:3754 2021-02-12T23:26:35Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.14 psi 2021-02-12T23:26:35Z- handleDepth()-Depth_Keller msg: 1613172395856 epoch sec; seqNo:3792; depth 24.17 2021-02-12T23:26:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172395.87 epoch sec; seqNo:3792; 4056993.58 north; 590964.25 east 2021-02-12T23:26:36Z- AHRS_M2 msg: 1613172396.26 epoch sec; seqNo:3755 2021-02-12T23:26:36Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.14 psi 2021-02-12T23:26:36Z- handleDepth()-Depth_Keller msg: 1613172396264 epoch sec; seqNo:3793; depth 24.06 2021-02-12T23:26:36Z- handleDvl() - RDI_Pathfinder msg: 1613172396.28 epoch sec; seqNo:1787 2021-02-12T23:26:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.69 , 83.85 , 95.75 , 63.74 2021-02-12T23:26:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.03 , -0.01 2021-02-12T23:26:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172396.29 epoch sec; seqNo:3793; 4056993.96 north; 590964.25 east 2021-02-12T23:26:36Z- AHRS_M2 msg: 1613172396.66 epoch sec; seqNo:3756 2021-02-12T23:26:36Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.14 psi 2021-02-12T23:26:36Z- handleDepth()-Depth_Keller msg: 1613172396666 epoch sec; seqNo:3794; depth 23.94 2021-02-12T23:26:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:36Z- handleDvl() - RDI_Pathfinder msg: 1613172396.67 epoch sec; seqNo:1788 2021-02-12T23:26:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.03 , -0.01 2021-02-12T23:26:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172396.68 epoch sec; seqNo:3794; 4056993.96 north; 590964.25 east 2021-02-12T23:26:37Z- AHRS_M2 msg: 1613172397.07 epoch sec; seqNo:3757 2021-02-12T23:26:37Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.14 psi 2021-02-12T23:26:37Z- handleDepth()-Depth_Keller msg: 1613172397070 epoch sec; seqNo:3795; depth 23.82 2021-02-12T23:26:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172397.09 epoch sec; seqNo:3795; 4056994.34 north; 590964.24 east 2021-02-12T23:26:37Z- AHRS_M2 msg: 1613172397.47 epoch sec; seqNo:3758 2021-02-12T23:26:37Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.15 psi 2021-02-12T23:26:37Z- handleDepth()-Depth_Keller msg: 1613172397476 epoch sec; seqNo:3796; depth 23.71 2021-02-12T23:26:37Z- handleDvl() - RDI_Pathfinder msg: 1613172397.51 epoch sec; seqNo:1789 2021-02-12T23:26:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 83.92 , 94.74 , 63.75 2021-02-12T23:26:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.00 , -0.02 2021-02-12T23:26:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172397.53 epoch sec; seqNo:3796; 4056994.71 north; 590964.24 east 2021-02-12T23:26:37Z- AHRS_M2 msg: 1613172397.87 epoch sec; seqNo:3759 2021-02-12T23:26:37Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.15 psi 2021-02-12T23:26:37Z- handleDepth()-Depth_Keller msg: 1613172397881 epoch sec; seqNo:3797; depth 23.61 2021-02-12T23:26:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:37Z- handleDvl() - RDI_Pathfinder msg: 1613172397.88 epoch sec; seqNo:1790 2021-02-12T23:26:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.00 , -0.02 2021-02-12T23:26:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172397.89 epoch sec; seqNo:3797; 4056994.71 north; 590964.24 east 2021-02-12T23:26:38Z- AHRS_M2 msg: 1613172398.28 epoch sec; seqNo:3760 2021-02-12T23:26:38Z- AHRS_M2 msg: 0.65 phi; 0.30 theta; -0.16 psi 2021-02-12T23:26:38Z- handleDepth()-Depth_Keller msg: 1613172398287 epoch sec; seqNo:3798; depth 23.49 2021-02-12T23:26:38Z- handleDvl() - RDI_Pathfinder msg: 1613172398.30 epoch sec; seqNo:1791 2021-02-12T23:26:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 85.00 , 94.74 , 63.75 2021-02-12T23:26:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , -0.01 2021-02-12T23:26:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172398.31 epoch sec; seqNo:3798; 4056995.11 north; 590965.45 east 2021-02-12T23:26:38Z- AHRS_M2 msg: 1613172398.68 epoch sec; seqNo:3761 2021-02-12T23:26:38Z- AHRS_M2 msg: 0.65 phi; 0.30 theta; -0.16 psi 2021-02-12T23:26:38Z- handleDepth()-Depth_Keller msg: 1613172398685 epoch sec; seqNo:3799; depth 23.38 2021-02-12T23:26:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172398.71 epoch sec; seqNo:3799; 4056995.48 north; 590965.45 east 2021-02-12T23:26:39Z- AHRS_M2 msg: 1613172399.08 epoch sec; seqNo:3762 2021-02-12T23:26:39Z- AHRS_M2 msg: 0.65 phi; 0.30 theta; -0.17 psi 2021-02-12T23:26:39Z- handleDepth()-Depth_Keller msg: 1613172399088 epoch sec; seqNo:3800; depth 23.27 2021-02-12T23:26:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:39Z- handleDvl() - RDI_Pathfinder msg: 1613172399.09 epoch sec; seqNo:1792 2021-02-12T23:26:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , -0.01 2021-02-12T23:26:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172399.10 epoch sec; seqNo:3800; 4056995.48 north; 590965.45 east 2021-02-12T23:26:39Z- AHRS_M2 msg: 1613172399.49 epoch sec; seqNo:3763 2021-02-12T23:26:39Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.17 psi 2021-02-12T23:26:39Z- handleDepth()-Depth_Keller msg: 1613172399496 epoch sec; seqNo:3801; depth 23.15 2021-02-12T23:26:39Z- handleDvl() - RDI_Pathfinder msg: 1613172399.51 epoch sec; seqNo:1793 2021-02-12T23:26:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 86.09 , 94.74 , 64.97 2021-02-12T23:26:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , 0.04 2021-02-12T23:26:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172399.52 epoch sec; seqNo:3801; 4056995.86 north; 590965.45 east 2021-02-12T23:26:39Z- AHRS_M2 msg: 1613172399.90 epoch sec; seqNo:3764 2021-02-12T23:26:39Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.17 psi 2021-02-12T23:26:39Z- handleDepth()-Depth_Keller msg: 1613172399902 epoch sec; seqNo:3802; depth 23.04 2021-02-12T23:26:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172399.91 epoch sec; seqNo:3802; 4056996.24 north; 590965.44 east 2021-02-12T23:26:40Z- LcmTrn::handleMessages() - handling 47 messages took 0.67 ms and 134 calls... 2021-02-12T23:26:40Z- waiting for 1613172397.65, time is 1613172399.91 2021-02-12T23:26:40Z- LcmTrn::updateTrn() >>>> pose time 1613172399.91, meas time 1613172399.51 <<<< 2021-02-12T23:26:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172399.51 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:26:40Z- TerrainNav::motionUpdate() - currEstimate = (4056996, 590965.44, 23.04) 2021-02-12T23:26:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:26:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:26:40Z- TNavBF::Projecting Measurements 2021-02-12T23:26:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:26:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:26:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:26:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:26:40Z- TNavFilter::4 good beams. 2021-02-12T23:26:40Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:26:40Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:26:40Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:26:40Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:26:40Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:26:40Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:26:40Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:26:40Z- TNavBF::mmse selected from filter -1 2021-02-12T23:26:40Z- TerrainMapOctree: (vehN, vehE ) = (4056995.98, 590965.26). 2021-02-12T23:26:40Z- TerrainMapOctree: Min Distance = 5374.59. 2021-02-12T23:26:40Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:26:40Z- There is no specific good beam that all particles have in common. 2021-02-12T23:26:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:26:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-12T23:26:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:26:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 11.86 2021-02-12T23:26:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:26:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 14.36 2021-02-12T23:26:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:26:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 6.75 2021-02-12T23:26:40Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:26:40Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:26:40Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:26:40Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:26:40Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:26:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:26:40Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:26:40Z- Filter 0 No beams used in subcloud update 2021-02-12T23:26:40Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:26:40Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:26:40Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 765.1 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:26:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172399.51 sec. 2021-02-12T23:26:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:26:40Z- TNavBF::mmse selected from filter -1 2021-02-12T23:26:40Z- variances([0], [2], and psi_berg) = 765.085756 833.048559 0.000000 2021-02-12T23:26:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:26:40Z- LcmTrn::publishEstimates() - MLE : 4056977.74 590916.94 23.04 2021-02-12T23:26:40Z- LcmTrn::publishEstimates() - MMSE : 4056996.24 590965.44 23.04 2021-02-12T23:26:40Z- LcmTrn::publishEstimates() - COVAR: 765.09 833.05 0.75 0.00 2021-02-12T23:26:40Z- AHRS_M2 msg: 1613172400.30 epoch sec; seqNo:3765 2021-02-12T23:26:40Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.18 psi 2021-02-12T23:26:40Z- handleDepth()-Depth_Keller msg: 1613172400304 epoch sec; seqNo:3803; depth 22.94 2021-02-12T23:26:40Z- handleDvl() - RDI_Pathfinder msg: 1613172400.32 epoch sec; seqNo:1794 2021-02-12T23:26:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 85.00 , 97.98 , 65.79 2021-02-12T23:26:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , 0.10 2021-02-12T23:26:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172400.34 epoch sec; seqNo:3803; 4056996.62 north; 590965.44 east 2021-02-12T23:26:40Z- AHRS_M2 msg: 1613172400.70 epoch sec; seqNo:3766 2021-02-12T23:26:40Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.18 psi 2021-02-12T23:26:40Z- handleDepth()-Depth_Keller msg: 1613172400708 epoch sec; seqNo:3804; depth 22.82 2021-02-12T23:26:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172400.72 epoch sec; seqNo:3804; 4056996.62 north; 590965.44 east 2021-02-12T23:26:41Z- AHRS_M2 msg: 1613172401.11 epoch sec; seqNo:3767 2021-02-12T23:26:41Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.18 psi 2021-02-12T23:26:41Z- handleDepth()-Depth_Keller msg: 1613172401115 epoch sec; seqNo:3805; depth 22.70 2021-02-12T23:26:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172401.13 epoch sec; seqNo:3805; 4056997.01 north; 590966.66 east 2021-02-12T23:26:41Z- AHRS_M2 msg: 1613172401.51 epoch sec; seqNo:3768 2021-02-12T23:26:41Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.18 psi 2021-02-12T23:26:41Z- handleDepth()-Depth_Keller msg: 1613172401526 epoch sec; seqNo:3806; depth 22.60 2021-02-12T23:26:41Z- handleDvl() - RDI_Pathfinder msg: 1613172401.54 epoch sec; seqNo:1795 2021-02-12T23:26:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 87.17 , 95.82 , 64.97 2021-02-12T23:26:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , 0.12 2021-02-12T23:26:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172401.56 epoch sec; seqNo:3806; 4056997.39 north; 590966.65 east 2021-02-12T23:26:41Z- AHRS_M2 msg: 1613172401.91 epoch sec; seqNo:3769 2021-02-12T23:26:41Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.17 psi 2021-02-12T23:26:41Z- handleDepth()-Depth_Keller msg: 1613172401919 epoch sec; seqNo:3807; depth 22.50 2021-02-12T23:26:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172401.93 epoch sec; seqNo:3807; 4056997.77 north; 590966.65 east 2021-02-12T23:26:42Z- AHRS_M2 msg: 1613172402.32 epoch sec; seqNo:3770 2021-02-12T23:26:42Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:26:42Z- handleDepth()-Depth_Keller msg: 1613172402324 epoch sec; seqNo:3808; depth 22.40 2021-02-12T23:26:42Z- handleDvl() - RDI_Pathfinder msg: 1613172402.34 epoch sec; seqNo:1796 2021-02-12T23:26:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 86.09 , 96.90 , 66.20 2021-02-12T23:26:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.05 , 0.07 2021-02-12T23:26:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172402.35 epoch sec; seqNo:3808; 4056997.77 north; 590966.65 east 2021-02-12T23:26:42Z- AHRS_M2 msg: 1613172402.72 epoch sec; seqNo:3771 2021-02-12T23:26:42Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:26:42Z- handleDepth()-Depth_Keller msg: 1613172402733 epoch sec; seqNo:3809; depth 22.28 2021-02-12T23:26:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:42Z- handleDvl() - RDI_Pathfinder msg: 1613172402.73 epoch sec; seqNo:1797 2021-02-12T23:26:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.05 , 0.07 2021-02-12T23:26:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172402.75 epoch sec; seqNo:3809; 4056998.15 north; 590966.64 east 2021-02-12T23:26:43Z- AHRS_M2 msg: 1613172403.13 epoch sec; seqNo:3772 2021-02-12T23:26:43Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:26:43Z- handleDepth()-Depth_Keller msg: 1613172403139 epoch sec; seqNo:3810; depth 22.17 2021-02-12T23:26:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172403.16 epoch sec; seqNo:3810; 4056998.54 north; 590967.86 east 2021-02-12T23:26:43Z- AHRS_M2 msg: 1613172403.53 epoch sec; seqNo:3773 2021-02-12T23:26:43Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:26:43Z- handleDepth()-Depth_Keller msg: 1613172403536 epoch sec; seqNo:3811; depth 22.08 2021-02-12T23:26:43Z- handleDvl() - RDI_Pathfinder msg: 1613172403.55 epoch sec; seqNo:1798 2021-02-12T23:26:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 88.18 , 97.91 , 64.56 2021-02-12T23:26:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , -0.00 2021-02-12T23:26:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172403.57 epoch sec; seqNo:3811; 4056998.92 north; 590967.86 east 2021-02-12T23:26:43Z- AHRS_M2 msg: 1613172403.94 epoch sec; seqNo:3774 2021-02-12T23:26:43Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.17 psi 2021-02-12T23:26:43Z- handleDepth()-Depth_Keller msg: 1613172403940 epoch sec; seqNo:3812; depth 21.96 2021-02-12T23:26:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:43Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:43Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:43Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:43Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:43Z- handleDvl() - RDI_Pathfinder msg: 1613172403.94 epoch sec; seqNo:1799 2021-02-12T23:26:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , -0.00 2021-02-12T23:26:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172403.95 epoch sec; seqNo:3812; 4056998.92 north; 590967.86 east 2021-02-12T23:26:44Z- AHRS_M2 msg: 1613172404.34 epoch sec; seqNo:3775 2021-02-12T23:26:44Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.17 psi 2021-02-12T23:26:44Z- handleDepth()-Depth_Keller msg: 1613172404345 epoch sec; seqNo:3813; depth 21.85 2021-02-12T23:26:44Z- handleDvl() - RDI_Pathfinder msg: 1613172404.36 epoch sec; seqNo:1800 2021-02-12T23:26:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 87.10 , 96.83 , 66.60 2021-02-12T23:26:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , -0.04 2021-02-12T23:26:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172404.37 epoch sec; seqNo:3813; 4056999.30 north; 590967.85 east 2021-02-12T23:26:44Z- AHRS_M2 msg: 1613172404.74 epoch sec; seqNo:3776 2021-02-12T23:26:44Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.18 psi 2021-02-12T23:26:44Z- handleDepth()-Depth_Keller msg: 1613172404749 epoch sec; seqNo:3814; depth 21.72 2021-02-12T23:26:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172404.76 epoch sec; seqNo:3814; 4056999.68 north; 590967.85 east 2021-02-12T23:26:45Z- AHRS_M2 msg: 1613172405.15 epoch sec; seqNo:3777 2021-02-12T23:26:45Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.18 psi 2021-02-12T23:26:45Z- handleDepth()-Depth_Keller msg: 1613172405160 epoch sec; seqNo:3815; depth 21.64 2021-02-12T23:26:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:45Z- handleDvl() - RDI_Pathfinder msg: 1613172405.16 epoch sec; seqNo:1801 2021-02-12T23:26:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , -0.04 2021-02-12T23:26:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172405.17 epoch sec; seqNo:3815; 4056999.68 north; 590967.85 east 2021-02-12T23:26:45Z- LcmTrn::handleMessages() - handling 47 messages took 0.67 ms and 134 calls... 2021-02-12T23:26:45Z- waiting for 1613172402.91, time is 1613172405.17 2021-02-12T23:26:45Z- LcmTrn::updateTrn() equating pose 1613172405.17 and meas times 1613172405.17 2021-02-12T23:26:45Z- LcmTrn::updateTrn() >>>> pose time 1613172405.17, meas time 1613172405.17 <<<< 2021-02-12T23:26:45Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172405.17, ping # 0. 2021-02-12T23:26:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172405.17 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:26:45Z- TerrainNav::motionUpdate() - currEstimate = (4057000, 590967.85, 21.64) 2021-02-12T23:26:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:26:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:26:45Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:26:45Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:26:45Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172405.17 sec.; No relevant navigation data available 2021-02-12T23:26:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:26:45Z- TNavBF::mmse selected from filter -1 2021-02-12T23:26:45Z- variances([0], [2], and psi_berg) = 764.962606 833.048419 0.000000 2021-02-12T23:26:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:26:45Z- LcmTrn::publishEstimates() - MLE : 4056981.18 590920.35 21.64 2021-02-12T23:26:45Z- LcmTrn::publishEstimates() - MMSE : 4056999.68 590967.85 21.64 2021-02-12T23:26:45Z- LcmTrn::publishEstimates() - COVAR: 764.96 833.05 0.97 0.00 2021-02-12T23:26:45Z- AHRS_M2 msg: 1613172405.55 epoch sec; seqNo:3778 2021-02-12T23:26:45Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.18 psi 2021-02-12T23:26:45Z- handleDepth()-Depth_Keller msg: 1613172405557 epoch sec; seqNo:3816; depth 21.52 2021-02-12T23:26:45Z- handleDvl() - RDI_Pathfinder msg: 1613172405.57 epoch sec; seqNo:1802 2021-02-12T23:26:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 89.26 , 97.91 , 66.60 2021-02-12T23:26:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , -0.03 2021-02-12T23:26:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172405.58 epoch sec; seqNo:3816; 4057000.07 north; 590969.07 east 2021-02-12T23:26:45Z- AHRS_M2 msg: 1613172405.96 epoch sec; seqNo:3779 2021-02-12T23:26:45Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.18 psi 2021-02-12T23:26:45Z- handleDepth()-Depth_Keller msg: 1613172405970 epoch sec; seqNo:3817; depth 21.41 2021-02-12T23:26:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172405.98 epoch sec; seqNo:3817; 4057000.45 north; 590969.06 east 2021-02-12T23:26:46Z- AHRS_M2 msg: 1613172406.36 epoch sec; seqNo:3780 2021-02-12T23:26:46Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-12T23:26:46Z- handleDepth()-Depth_Keller msg: 1613172406369 epoch sec; seqNo:3818; depth 21.31 2021-02-12T23:26:46Z- handleDvl() - RDI_Pathfinder msg: 1613172406.39 epoch sec; seqNo:1803 2021-02-12T23:26:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 89.19 , 96.76 , 66.19 2021-02-12T23:26:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.00 2021-02-12T23:26:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172406.40 epoch sec; seqNo:3818; 4057000.83 north; 590969.06 east 2021-02-12T23:26:46Z- AHRS_M2 msg: 1613172406.76 epoch sec; seqNo:3781 2021-02-12T23:26:46Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-12T23:26:46Z- handleDepth()-Depth_Keller msg: 1613172406769 epoch sec; seqNo:3819; depth 21.18 2021-02-12T23:26:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172406.79 epoch sec; seqNo:3819; 4057000.83 north; 590969.06 east 2021-02-12T23:26:47Z- AHRS_M2 msg: 1613172407.17 epoch sec; seqNo:3782 2021-02-12T23:26:47Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-12T23:26:47Z- handleDepth()-Depth_Keller msg: 1613172407183 epoch sec; seqNo:3820; depth 21.10 2021-02-12T23:26:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172407.21 epoch sec; seqNo:3820; 4057001.21 north; 590969.05 east 2021-02-12T23:26:47Z- AHRS_M2 msg: 1613172407.57 epoch sec; seqNo:3783 2021-02-12T23:26:47Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-12T23:26:47Z- handleDepth()-Depth_Keller msg: 1613172407576 epoch sec; seqNo:3821; depth 20.98 2021-02-12T23:26:47Z- handleDvl() - RDI_Pathfinder msg: 1613172407.60 epoch sec; seqNo:1804 2021-02-12T23:26:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.14 , 89.19 , 98.93 , 67.01 2021-02-12T23:26:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , 0.01 2021-02-12T23:26:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172407.61 epoch sec; seqNo:3821; 4057001.59 north; 590969.05 east 2021-02-12T23:26:47Z- AHRS_M2 msg: 1613172407.97 epoch sec; seqNo:3784 2021-02-12T23:26:47Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-12T23:26:47Z- handleDepth()-Depth_Keller msg: 1613172407985 epoch sec; seqNo:3822; depth 20.86 2021-02-12T23:26:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172408.01 epoch sec; seqNo:3822; 4057001.98 north; 590970.27 east 2021-02-12T23:26:48Z- AHRS_M2 msg: 1613172408.38 epoch sec; seqNo:3785 2021-02-12T23:26:48Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-12T23:26:48Z- handleDepth()-Depth_Keller msg: 1613172408384 epoch sec; seqNo:3823; depth 20.77 2021-02-12T23:26:48Z- handleDvl() - RDI_Pathfinder msg: 1613172408.39 epoch sec; seqNo:1805 2021-02-12T23:26:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.55 , 88.04 , 96.69 , 66.19 2021-02-12T23:26:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.05 , -0.03 2021-02-12T23:26:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172408.40 epoch sec; seqNo:3823; 4057001.98 north; 590970.27 east 2021-02-12T23:26:48Z- AHRS_M2 msg: 1613172408.78 epoch sec; seqNo:3786 2021-02-12T23:26:48Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:26:48Z- handleDepth()-Depth_Keller msg: 1613172408790 epoch sec; seqNo:3824; depth 20.65 2021-02-12T23:26:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:48Z- handleDvl() - RDI_Pathfinder msg: 1613172408.79 epoch sec; seqNo:1806 2021-02-12T23:26:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.05 , -0.03 2021-02-12T23:26:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172408.80 epoch sec; seqNo:3824; 4057002.36 north; 590970.26 east 2021-02-12T23:26:49Z- AHRS_M2 msg: 1613172409.21 epoch sec; seqNo:3787 2021-02-12T23:26:49Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:26:49Z- handleDepth()-Depth_Keller msg: 1613172409218 epoch sec; seqNo:3825; depth 20.53 2021-02-12T23:26:49Z- handleDvl() - RDI_Pathfinder msg: 1613172409.23 epoch sec; seqNo:1807 2021-02-12T23:26:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.14 , 89.19 , 100.01 , 67.01 2021-02-12T23:26:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.00 , -0.07 2021-02-12T23:26:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172409.24 epoch sec; seqNo:3825; 4057002.73 north; 590970.26 east 2021-02-12T23:26:49Z- AHRS_M2 msg: 1613172409.59 epoch sec; seqNo:3788 2021-02-12T23:26:49Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:26:49Z- handleDepth()-Depth_Keller msg: 1613172409596 epoch sec; seqNo:3826; depth 20.43 2021-02-12T23:26:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172409.61 epoch sec; seqNo:3826; 4057002.73 north; 590970.26 east 2021-02-12T23:26:49Z- AHRS_M2 msg: 1613172409.99 epoch sec; seqNo:3789 2021-02-12T23:26:49Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:26:50Z- handleDepth()-Depth_Keller msg: 1613172410000 epoch sec; seqNo:3827; depth 20.32 2021-02-12T23:26:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:50Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:50Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:50Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:50Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:50Z- handleDvl() - RDI_Pathfinder msg: 1613172410.00 epoch sec; seqNo:1808 2021-02-12T23:26:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.00 , -0.07 2021-02-12T23:26:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172410.01 epoch sec; seqNo:3827; 4057003.11 north; 590970.25 east 2021-02-12T23:26:50Z- LcmTrn::handleMessages() - handling 43 messages took 0.71 ms and 129 calls... 2021-02-12T23:26:50Z- waiting for 1613172408.17, time is 1613172410.01 2021-02-12T23:26:50Z- LcmTrn::updateTrn() equating pose 1613172410.01 and meas times 1613172410.01 2021-02-12T23:26:50Z- LcmTrn::updateTrn() >>>> pose time 1613172410.01, meas time 1613172410.01 <<<< 2021-02-12T23:26:50Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172410.01, ping # 0. 2021-02-12T23:26:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172410.01 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:26:50Z- TerrainNav::motionUpdate() - currEstimate = (4057003, 590970.25, 20.32) 2021-02-12T23:26:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:26:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:26:50Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:26:50Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:26:50Z- TNavBF::Projecting Measurements 2021-02-12T23:26:50Z- TNavFilter::Measurement from time = 1613172410.01 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:26:50Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:26:50Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:26:50Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:26:50Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:26:50Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:26:50Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:26:50Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:26:50Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 764.8 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:26:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:26:50Z- TNavBF::mmse selected from filter -1 2021-02-12T23:26:50Z- variances([0], [2], and psi_berg) = 764.839681 833.048279 0.000000 2021-02-12T23:26:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:26:50Z- LcmTrn::publishEstimates() - MLE : 4056984.61 591017.75 20.32 2021-02-12T23:26:50Z- LcmTrn::publishEstimates() - MMSE : 4057003.11 590970.25 20.32 2021-02-12T23:26:50Z- LcmTrn::publishEstimates() - COVAR: 764.84 833.05 1.18 0.00 2021-02-12T23:26:50Z- AHRS_M2 msg: 1613172410.40 epoch sec; seqNo:3790 2021-02-12T23:26:50Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:26:50Z- handleDepth()-Depth_Keller msg: 1613172410406 epoch sec; seqNo:3828; depth 20.22 2021-02-12T23:26:50Z- handleDvl() - RDI_Pathfinder msg: 1613172410.42 epoch sec; seqNo:1809 2021-02-12T23:26:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 89.87 , 98.38 , 66.60 2021-02-12T23:26:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , -0.05 2021-02-12T23:26:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172410.43 epoch sec; seqNo:3828; 4057003.49 north; 590970.25 east 2021-02-12T23:26:50Z- AHRS_M2 msg: 1613172410.80 epoch sec; seqNo:3791 2021-02-12T23:26:50Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:26:50Z- handleDepth()-Depth_Keller msg: 1613172410813 epoch sec; seqNo:3829; depth 20.10 2021-02-12T23:26:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172410.82 epoch sec; seqNo:3829; 4057003.51 north; 590971.47 east 2021-02-12T23:26:51Z- AHRS_M2 msg: 1613172411.23 epoch sec; seqNo:3792 2021-02-12T23:26:51Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-12T23:26:51Z- handleDepth()-Depth_Keller msg: 1613172411242 epoch sec; seqNo:3830; depth 19.99 2021-02-12T23:26:51Z- handleDvl() - RDI_Pathfinder msg: 1613172411.27 epoch sec; seqNo:1810 2021-02-12T23:26:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 90.21 , 99.94 , 67.84 2021-02-12T23:26:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.10 , 0.03 2021-02-12T23:26:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172411.29 epoch sec; seqNo:3830; 4057003.88 north; 590971.47 east 2021-02-12T23:26:51Z- AHRS_M2 msg: 1613172411.61 epoch sec; seqNo:3793 2021-02-12T23:26:51Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:26:51Z- handleDepth()-Depth_Keller msg: 1613172411618 epoch sec; seqNo:3831; depth 19.86 2021-02-12T23:26:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172411.63 epoch sec; seqNo:3831; 4057004.26 north; 590971.46 east 2021-02-12T23:26:52Z- AHRS_M2 msg: 1613172412.02 epoch sec; seqNo:3794 2021-02-12T23:26:52Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:26:52Z- handleDepth()-Depth_Keller msg: 1613172412021 epoch sec; seqNo:3832; depth 19.77 2021-02-12T23:26:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172412.04 epoch sec; seqNo:3832; 4057004.64 north; 590971.46 east 2021-02-12T23:26:52Z- AHRS_M2 msg: 1613172412.42 epoch sec; seqNo:3795 2021-02-12T23:26:52Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-12T23:26:52Z- handleDepth()-Depth_Keller msg: 1613172412424 epoch sec; seqNo:3833; depth 19.65 2021-02-12T23:26:52Z- handleDvl() - RDI_Pathfinder msg: 1613172412.44 epoch sec; seqNo:1811 2021-02-12T23:26:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 88.65 , 97.17 , 67.02 2021-02-12T23:26:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , 0.10 2021-02-12T23:26:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172412.46 epoch sec; seqNo:3833; 4057004.64 north; 590971.46 east 2021-02-12T23:26:52Z- AHRS_M2 msg: 1613172412.83 epoch sec; seqNo:3796 2021-02-12T23:26:52Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-12T23:26:52Z- handleDepth()-Depth_Keller msg: 1613172412832 epoch sec; seqNo:3834; depth 19.53 2021-02-12T23:26:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172412.85 epoch sec; seqNo:3834; 4057005.02 north; 590971.46 east 2021-02-12T23:26:53Z- AHRS_M2 msg: 1613172413.24 epoch sec; seqNo:3797 2021-02-12T23:26:53Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-12T23:26:53Z- handleDepth()-Depth_Keller msg: 1613172413241 epoch sec; seqNo:3835; depth 19.40 2021-02-12T23:26:53Z- handleDvl() - RDI_Pathfinder msg: 1613172413.25 epoch sec; seqNo:1812 2021-02-12T23:26:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.78 , 91.29 , 102.10 , 66.19 2021-02-12T23:26:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.10 2021-02-12T23:26:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172413.26 epoch sec; seqNo:3835; 4057005.40 north; 590971.45 east 2021-02-12T23:26:53Z- AHRS_M2 msg: 1613172413.67 epoch sec; seqNo:3798 2021-02-12T23:26:53Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-12T23:26:53Z- handleDepth()-Depth_Keller msg: 1613172413678 epoch sec; seqNo:3836; depth 19.29 2021-02-12T23:26:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:53Z- handleDvl() - RDI_Pathfinder msg: 1613172413.68 epoch sec; seqNo:1813 2021-02-12T23:26:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.10 2021-02-12T23:26:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172413.71 epoch sec; seqNo:3836; 4057005.79 north; 590972.67 east 2021-02-12T23:26:54Z- handleDepth()-Depth_Keller msg: 1613172414053 epoch sec; seqNo:3837; depth 19.15 2021-02-12T23:26:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172414.07 epoch sec; seqNo:3837; 4057005.79 north; 590972.67 east 2021-02-12T23:26:54Z- AHRS_M2 msg: 1613172414.44 epoch sec; seqNo:3799 2021-02-12T23:26:54Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.19 psi 2021-02-12T23:26:54Z- handleDepth()-Depth_Keller msg: 1613172414445 epoch sec; seqNo:3838; depth 19.05 2021-02-12T23:26:54Z- handleDvl() - RDI_Pathfinder msg: 1613172414.46 epoch sec; seqNo:1814 2021-02-12T23:26:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.78 , 92.23 , 101.97 , 67.01 2021-02-12T23:26:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.08 , 0.05 2021-02-12T23:26:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172414.47 epoch sec; seqNo:3838; 4057006.17 north; 590972.66 east 2021-02-12T23:26:54Z- AHRS_M2 msg: 1613172414.85 epoch sec; seqNo:3800 2021-02-12T23:26:54Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.19 psi 2021-02-12T23:26:54Z- handleDepth()-Depth_Keller msg: 1613172414854 epoch sec; seqNo:3839; depth 18.93 2021-02-12T23:26:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:54Z- handleDvl() - RDI_Pathfinder msg: 1613172414.86 epoch sec; seqNo:1815 2021-02-12T23:26:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.08 , 0.05 2021-02-12T23:26:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172414.87 epoch sec; seqNo:3839; 4057006.55 north; 590972.66 east 2021-02-12T23:26:55Z- AHRS_M2 msg: 1613172415.25 epoch sec; seqNo:3801 2021-02-12T23:26:55Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.20 psi 2021-02-12T23:26:55Z- handleDepth()-Depth_Keller msg: 1613172415265 epoch sec; seqNo:3840; depth 18.80 2021-02-12T23:26:55Z- handleDvl() - RDI_Pathfinder msg: 1613172415.28 epoch sec; seqNo:1816 2021-02-12T23:26:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.78 , 90.95 , 97.03 , 67.42 2021-02-12T23:26:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.06 , 0.01 2021-02-12T23:26:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172415.29 epoch sec; seqNo:3840; 4057006.55 north; 590972.66 east 2021-02-12T23:26:55Z- LcmTrn::handleMessages() - handling 46 messages took 0.77 ms and 131 calls... 2021-02-12T23:26:55Z- waiting for 1613172413.01, time is 1613172415.29 2021-02-12T23:26:55Z- LcmTrn::updateTrn() equating pose 1613172415.29 and meas times 1613172415.29 2021-02-12T23:26:55Z- LcmTrn::updateTrn() >>>> pose time 1613172415.29, meas time 1613172415.29 <<<< 2021-02-12T23:26:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172415.29 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:26:55Z- TerrainNav::motionUpdate() - currEstimate = (4057007, 590972.66, 18.80) 2021-02-12T23:26:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:26:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:26:55Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172415.29 sec.; No relevant navigation data available 2021-02-12T23:26:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:26:55Z- TNavBF::mmse selected from filter -1 2021-02-12T23:26:55Z- variances([0], [2], and psi_berg) = 764.716980 833.048140 0.000000 2021-02-12T23:26:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:26:55Z- LcmTrn::publishEstimates() - MLE : 4056988.05 591020.16 18.80 2021-02-12T23:26:55Z- LcmTrn::publishEstimates() - MMSE : 4057006.55 590972.66 18.80 2021-02-12T23:26:55Z- LcmTrn::publishEstimates() - COVAR: 764.72 833.05 1.40 0.00 2021-02-12T23:26:55Z- AHRS_M2 msg: 1613172415.65 epoch sec; seqNo:3802 2021-02-12T23:26:55Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.19 psi 2021-02-12T23:26:55Z- handleDepth()-Depth_Keller msg: 1613172415657 epoch sec; seqNo:3841; depth 18.69 2021-02-12T23:26:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172415.67 epoch sec; seqNo:3841; 4057006.93 north; 590972.66 east 2021-02-12T23:26:56Z- AHRS_M2 msg: 1613172416.06 epoch sec; seqNo:3803 2021-02-12T23:26:56Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.19 psi 2021-02-12T23:26:56Z- handleDepth()-Depth_Keller msg: 1613172416065 epoch sec; seqNo:3842; depth 18.58 2021-02-12T23:26:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:56Z- handleDvl() - RDI_Pathfinder msg: 1613172416.07 epoch sec; seqNo:1817 2021-02-12T23:26:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.06 , 0.01 2021-02-12T23:26:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172416.08 epoch sec; seqNo:3842; 4057007.32 north; 590973.87 east 2021-02-12T23:26:56Z- AHRS_M2 msg: 1613172416.46 epoch sec; seqNo:3804 2021-02-12T23:26:56Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.19 psi 2021-02-12T23:26:56Z- handleDepth()-Depth_Keller msg: 1613172416473 epoch sec; seqNo:3843; depth 18.44 2021-02-12T23:26:56Z- handleDvl() - RDI_Pathfinder msg: 1613172416.48 epoch sec; seqNo:1818 2021-02-12T23:26:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 93.52 , 104.47 , 69.46 2021-02-12T23:26:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , -0.00 2021-02-12T23:26:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172416.49 epoch sec; seqNo:3843; 4057007.32 north; 590973.87 east 2021-02-12T23:26:56Z- AHRS_M2 msg: 1613172416.86 epoch sec; seqNo:3805 2021-02-12T23:26:56Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.19 psi 2021-02-12T23:26:56Z- handleDepth()-Depth_Keller msg: 1613172416873 epoch sec; seqNo:3844; depth 18.33 2021-02-12T23:26:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172416.89 epoch sec; seqNo:3844; 4057007.70 north; 590973.87 east 2021-02-12T23:26:57Z- AHRS_M2 msg: 1613172417.27 epoch sec; seqNo:3806 2021-02-12T23:26:57Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.19 psi 2021-02-12T23:26:57Z- handleDepth()-Depth_Keller msg: 1613172417276 epoch sec; seqNo:3845; depth 18.21 2021-02-12T23:26:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:57Z- handleDvl() - RDI_Pathfinder msg: 1613172417.28 epoch sec; seqNo:1819 2021-02-12T23:26:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , -0.00 2021-02-12T23:26:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172417.32 epoch sec; seqNo:3845; 4057008.08 north; 590973.86 east 2021-02-12T23:26:57Z- AHRS_M2 msg: 1613172417.67 epoch sec; seqNo:3807 2021-02-12T23:26:57Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-12T23:26:57Z- handleDepth()-Depth_Keller msg: 1613172417676 epoch sec; seqNo:3846; depth 18.09 2021-02-12T23:26:57Z- handleDvl() - RDI_Pathfinder msg: 1613172417.69 epoch sec; seqNo:1820 2021-02-12T23:26:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 94.40 , 96.83 , 69.06 2021-02-12T23:26:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , 0.02 2021-02-12T23:26:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172417.70 epoch sec; seqNo:3846; 4057008.46 north; 590973.86 east 2021-02-12T23:26:58Z- AHRS_M2 msg: 1613172418.08 epoch sec; seqNo:3808 2021-02-12T23:26:58Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-12T23:26:58Z- handleDepth()-Depth_Keller msg: 1613172418089 epoch sec; seqNo:3847; depth 17.98 2021-02-12T23:26:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172418.10 epoch sec; seqNo:3847; 4057008.46 north; 590973.86 east 2021-02-12T23:26:58Z- AHRS_M2 msg: 1613172418.48 epoch sec; seqNo:3809 2021-02-12T23:26:58Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-12T23:26:58Z- handleDepth()-Depth_Keller msg: 1613172418489 epoch sec; seqNo:3848; depth 17.85 2021-02-12T23:26:58Z- handleDvl() - RDI_Pathfinder msg: 1613172418.51 epoch sec; seqNo:1821 2021-02-12T23:26:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 93.32 , 105.48 , 68.65 2021-02-12T23:26:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.07 , 0.06 2021-02-12T23:26:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172418.52 epoch sec; seqNo:3848; 4057008.85 north; 590975.08 east 2021-02-12T23:26:58Z- AHRS_M2 msg: 1613172418.88 epoch sec; seqNo:3810 2021-02-12T23:26:58Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-12T23:26:58Z- handleDepth()-Depth_Keller msg: 1613172418893 epoch sec; seqNo:3849; depth 17.72 2021-02-12T23:26:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172418.90 epoch sec; seqNo:3849; 4057009.23 north; 590975.07 east 2021-02-12T23:26:59Z- AHRS_M2 msg: 1613172419.30 epoch sec; seqNo:3811 2021-02-12T23:26:59Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-12T23:26:59Z- handleDepth()-Depth_Keller msg: 1613172419312 epoch sec; seqNo:3850; depth 17.60 2021-02-12T23:26:59Z- handleDvl() - RDI_Pathfinder msg: 1613172419.34 epoch sec; seqNo:1822 2021-02-12T23:26:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 94.40 , 104.13 , 68.65 2021-02-12T23:26:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , 0.07 2021-02-12T23:26:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172419.35 epoch sec; seqNo:3850; 4057009.61 north; 590975.07 east 2021-02-12T23:26:59Z- AHRS_M2 msg: 1613172419.69 epoch sec; seqNo:3812 2021-02-12T23:26:59Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-12T23:26:59Z- handleDepth()-Depth_Keller msg: 1613172419696 epoch sec; seqNo:3851; depth 17.47 2021-02-12T23:26:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:26:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:26:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:26:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:26:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:26:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:26:59Z- handleDvl() - RDI_Pathfinder msg: 1613172419.70 epoch sec; seqNo:1823 2021-02-12T23:26:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:26:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , 0.07 2021-02-12T23:26:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172419.72 epoch sec; seqNo:3851; 4057009.61 north; 590975.07 east 2021-02-12T23:27:00Z- AHRS_M2 msg: 1613172420.10 epoch sec; seqNo:3813 2021-02-12T23:27:00Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.17 psi 2021-02-12T23:27:00Z- handleDepth()-Depth_Keller msg: 1613172420105 epoch sec; seqNo:3852; depth 17.35 2021-02-12T23:27:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172420.12 epoch sec; seqNo:3852; 4057009.98 north; 590975.07 east 2021-02-12T23:27:00Z- LcmTrn::handleMessages() - handling 43 messages took 0.73 ms and 129 calls... 2021-02-12T23:27:00Z- waiting for 1613172418.29, time is 1613172420.12 2021-02-12T23:27:00Z- LcmTrn::updateTrn() >>>> pose time 1613172420.12, meas time 1613172419.70 <<<< 2021-02-12T23:27:00Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172419.70, ping # 0. 2021-02-12T23:27:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172419.70 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:27:00Z- TerrainNav::motionUpdate() - currEstimate = (4057010, 590975.07, 17.35) 2021-02-12T23:27:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:27:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:27:00Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:27:00Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:27:00Z- TNavBF::Projecting Measurements 2021-02-12T23:27:00Z- TNavFilter::Measurement from time = 1613172419.70 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:27:00Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:27:00Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:27:00Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:27:00Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:27:00Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:27:00Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:27:00Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:27:00Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 764.6 EastVariance: 833.0 DepthVariance: 1.6 this->DepthBiasCov: 1.6 2021-02-12T23:27:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:27:00Z- TNavBF::mmse selected from filter -1 2021-02-12T23:27:00Z- variances([0], [2], and psi_berg) = 764.564188 833.047967 0.000000 2021-02-12T23:27:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:27:00Z- LcmTrn::publishEstimates() - MLE : 4056991.48 591022.57 17.35 2021-02-12T23:27:00Z- LcmTrn::publishEstimates() - MMSE : 4057009.98 590975.07 17.35 2021-02-12T23:27:00Z- LcmTrn::publishEstimates() - COVAR: 764.56 833.05 1.61 0.00 2021-02-12T23:27:00Z- AHRS_M2 msg: 1613172420.50 epoch sec; seqNo:3814 2021-02-12T23:27:00Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-12T23:27:00Z- handleDepth()-Depth_Keller msg: 1613172420504 epoch sec; seqNo:3853; depth 17.23 2021-02-12T23:27:00Z- handleDvl() - RDI_Pathfinder msg: 1613172420.52 epoch sec; seqNo:1824 2021-02-12T23:27:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.82 , 93.18 , 104.13 , 69.87 2021-02-12T23:27:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , 0.05 2021-02-12T23:27:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172420.53 epoch sec; seqNo:3853; 4057010.36 north; 590975.06 east 2021-02-12T23:27:00Z- AHRS_M2 msg: 1613172420.90 epoch sec; seqNo:3815 2021-02-12T23:27:00Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-12T23:27:00Z- handleDepth()-Depth_Keller msg: 1613172420910 epoch sec; seqNo:3854; depth 17.10 2021-02-12T23:27:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:00Z- handleDvl() - RDI_Pathfinder msg: 1613172420.91 epoch sec; seqNo:1825 2021-02-12T23:27:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , 0.05 2021-02-12T23:27:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172420.92 epoch sec; seqNo:3854; 4057010.74 north; 590975.06 east 2021-02-12T23:27:01Z- AHRS_M2 msg: 1613172421.31 epoch sec; seqNo:3816 2021-02-12T23:27:01Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-12T23:27:01Z- handleDepth()-Depth_Keller msg: 1613172421312 epoch sec; seqNo:3855; depth 16.99 2021-02-12T23:27:01Z- handleDvl() - RDI_Pathfinder msg: 1613172421.33 epoch sec; seqNo:1826 2021-02-12T23:27:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 93.25 , 106.63 , 70.28 2021-02-12T23:27:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , 0.01 2021-02-12T23:27:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172421.34 epoch sec; seqNo:3855; 4057011.13 north; 590976.27 east 2021-02-12T23:27:01Z- AHRS_M2 msg: 1613172421.71 epoch sec; seqNo:3817 2021-02-12T23:27:01Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-12T23:27:01Z- handleDepth()-Depth_Keller msg: 1613172421716 epoch sec; seqNo:3856; depth 16.86 2021-02-12T23:27:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172421.73 epoch sec; seqNo:3856; 4057011.13 north; 590976.27 east 2021-02-12T23:27:02Z- AHRS_M2 msg: 1613172422.12 epoch sec; seqNo:3818 2021-02-12T23:27:02Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-12T23:27:02Z- handleDepth()-Depth_Keller msg: 1613172422123 epoch sec; seqNo:3857; depth 16.74 2021-02-12T23:27:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:02Z- handleDvl() - RDI_Pathfinder msg: 1613172422.13 epoch sec; seqNo:1827 2021-02-12T23:27:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , 0.01 2021-02-12T23:27:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172422.14 epoch sec; seqNo:3857; 4057011.51 north; 590976.27 east 2021-02-12T23:27:02Z- AHRS_M2 msg: 1613172422.52 epoch sec; seqNo:3819 2021-02-12T23:27:02Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-12T23:27:02Z- handleDepth()-Depth_Keller msg: 1613172422526 epoch sec; seqNo:3858; depth 16.61 2021-02-12T23:27:02Z- handleDvl() - RDI_Pathfinder msg: 1613172422.54 epoch sec; seqNo:1828 2021-02-12T23:27:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 93.11 , 108.93 , 78.51 2021-02-12T23:27:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , -0.02 2021-02-12T23:27:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172422.55 epoch sec; seqNo:3858; 4057011.89 north; 590976.27 east 2021-02-12T23:27:02Z- AHRS_M2 msg: 1613172422.92 epoch sec; seqNo:3820 2021-02-12T23:27:02Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-12T23:27:02Z- handleDepth()-Depth_Keller msg: 1613172422929 epoch sec; seqNo:3859; depth 16.50 2021-02-12T23:27:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172422.95 epoch sec; seqNo:3859; 4057012.27 north; 590976.26 east 2021-02-12T23:27:03Z- AHRS_M2 msg: 1613172423.33 epoch sec; seqNo:3821 2021-02-12T23:27:03Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-12T23:27:03Z- handleDepth()-Depth_Keller msg: 1613172423338 epoch sec; seqNo:3860; depth 16.39 2021-02-12T23:27:03Z- handleDvl() - RDI_Pathfinder msg: 1613172423.35 epoch sec; seqNo:1829 2021-02-12T23:27:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 95.34 , 107.51 , 69.87 2021-02-12T23:27:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.01 , -0.04 2021-02-12T23:27:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172423.37 epoch sec; seqNo:3860; 4057012.27 north; 590976.26 east 2021-02-12T23:27:03Z- AHRS_M2 msg: 1613172423.73 epoch sec; seqNo:3822 2021-02-12T23:27:03Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-12T23:27:03Z- handleDepth()-Depth_Keller msg: 1613172423736 epoch sec; seqNo:3861; depth 16.26 2021-02-12T23:27:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172423.75 epoch sec; seqNo:3861; 4057012.66 north; 590977.48 east 2021-02-12T23:27:04Z- AHRS_M2 msg: 1613172424.14 epoch sec; seqNo:3823 2021-02-12T23:27:04Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-12T23:27:04Z- handleDepth()-Depth_Keller msg: 1613172424146 epoch sec; seqNo:3862; depth 16.15 2021-02-12T23:27:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172424.15 epoch sec; seqNo:3862; 4057013.04 north; 590977.48 east 2021-02-12T23:27:04Z- AHRS_M2 msg: 1613172424.54 epoch sec; seqNo:3824 2021-02-12T23:27:04Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-12T23:27:04Z- handleDepth()-Depth_Keller msg: 1613172424544 epoch sec; seqNo:3863; depth 16.03 2021-02-12T23:27:04Z- handleDvl() - RDI_Pathfinder msg: 1613172424.56 epoch sec; seqNo:1830 2021-02-12T23:27:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.64 , 95.54 , 107.71 , 71.92 2021-02-12T23:27:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.04 , -0.04 2021-02-12T23:27:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172424.58 epoch sec; seqNo:3863; 4057013.04 north; 590977.48 east 2021-02-12T23:27:04Z- AHRS_M2 msg: 1613172424.94 epoch sec; seqNo:3825 2021-02-12T23:27:04Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.19 psi 2021-02-12T23:27:04Z- handleDepth()-Depth_Keller msg: 1613172424953 epoch sec; seqNo:3864; depth 15.90 2021-02-12T23:27:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172424.96 epoch sec; seqNo:3864; 4057013.42 north; 590977.47 east 2021-02-12T23:27:05Z- AHRS_M2 msg: 1613172425.35 epoch sec; seqNo:3826 2021-02-12T23:27:05Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.19 psi 2021-02-12T23:27:05Z- handleDepth()-Depth_Keller msg: 1613172425352 epoch sec; seqNo:3865; depth 15.79 2021-02-12T23:27:05Z- handleDvl() - RDI_Pathfinder msg: 1613172425.37 epoch sec; seqNo:1831 2021-02-12T23:27:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 96.69 , 107.64 , 70.28 2021-02-12T23:27:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.11 , 0.01 2021-02-12T23:27:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172425.38 epoch sec; seqNo:3865; 4057013.80 north; 590977.47 east 2021-02-12T23:27:05Z- LcmTrn::handleMessages() - handling 47 messages took 0.68 ms and 134 calls... 2021-02-12T23:27:05Z- waiting for 1613172423.12, time is 1613172425.38 2021-02-12T23:27:05Z- LcmTrn::updateTrn() equating pose 1613172425.38 and meas times 1613172425.38 2021-02-12T23:27:05Z- LcmTrn::updateTrn() >>>> pose time 1613172425.38, meas time 1613172425.38 <<<< 2021-02-12T23:27:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172425.38 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:27:05Z- TerrainNav::motionUpdate() - currEstimate = (4057014, 590977.47, 15.79) 2021-02-12T23:27:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:27:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:27:05Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172425.38 sec.; No relevant navigation data available 2021-02-12T23:27:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:27:05Z- TNavBF::mmse selected from filter -1 2021-02-12T23:27:05Z- variances([0], [2], and psi_berg) = 764.437503 833.047823 0.000000 2021-02-12T23:27:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:27:05Z- LcmTrn::publishEstimates() - MLE : 4056996.30 590928.97 15.79 2021-02-12T23:27:05Z- LcmTrn::publishEstimates() - MMSE : 4057013.80 590977.47 15.79 2021-02-12T23:27:05Z- LcmTrn::publishEstimates() - COVAR: 764.44 833.05 1.84 0.00 2021-02-12T23:27:05Z- AHRS_M2 msg: 1613172425.75 epoch sec; seqNo:3827 2021-02-12T23:27:05Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.19 psi 2021-02-12T23:27:05Z- handleDepth()-Depth_Keller msg: 1613172425756 epoch sec; seqNo:3866; depth 15.66 2021-02-12T23:27:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:05Z- handleDvl() - RDI_Pathfinder msg: 1613172425.76 epoch sec; seqNo:1832 2021-02-12T23:27:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.11 , 0.01 2021-02-12T23:27:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172425.78 epoch sec; seqNo:3866; 4057013.80 north; 590977.47 east 2021-02-12T23:27:06Z- AHRS_M2 msg: 1613172426.16 epoch sec; seqNo:3828 2021-02-12T23:27:06Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.20 psi 2021-02-12T23:27:06Z- handleDepth()-Depth_Keller msg: 1613172426165 epoch sec; seqNo:3867; depth 15.54 2021-02-12T23:27:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172426.17 epoch sec; seqNo:3867; 4057014.18 north; 590977.46 east 2021-02-12T23:27:06Z- AHRS_M2 msg: 1613172426.56 epoch sec; seqNo:3829 2021-02-12T23:27:06Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.19 psi 2021-02-12T23:27:06Z- handleDepth()-Depth_Keller msg: 1613172426565 epoch sec; seqNo:3868; depth 15.43 2021-02-12T23:27:06Z- handleDvl() - RDI_Pathfinder msg: 1613172426.58 epoch sec; seqNo:1833 2021-02-12T23:27:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 95.48 , 105.21 , 70.28 2021-02-12T23:27:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.11 , 0.06 2021-02-12T23:27:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172426.59 epoch sec; seqNo:3868; 4057014.56 north; 590977.46 east 2021-02-12T23:27:06Z- AHRS_M2 msg: 1613172426.96 epoch sec; seqNo:3830 2021-02-12T23:27:06Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.19 psi 2021-02-12T23:27:06Z- handleDepth()-Depth_Keller msg: 1613172426970 epoch sec; seqNo:3869; depth 15.31 2021-02-12T23:27:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:06Z- handleDvl() - RDI_Pathfinder msg: 1613172426.97 epoch sec; seqNo:1834 2021-02-12T23:27:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.11 , 0.06 2021-02-12T23:27:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172426.98 epoch sec; seqNo:3869; 4057014.95 north; 590978.68 east 2021-02-12T23:27:07Z- AHRS_M2 msg: 1613172427.37 epoch sec; seqNo:3831 2021-02-12T23:27:07Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.18 psi 2021-02-12T23:27:07Z- handleDepth()-Depth_Keller msg: 1613172427376 epoch sec; seqNo:3870; depth 15.19 2021-02-12T23:27:07Z- handleDvl() - RDI_Pathfinder msg: 1613172427.41 epoch sec; seqNo:1835 2021-02-12T23:27:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.76 , 107.71 , 71.10 2021-02-12T23:27:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , 0.10 2021-02-12T23:27:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172427.42 epoch sec; seqNo:3870; 4057015.33 north; 590978.67 east 2021-02-12T23:27:07Z- AHRS_M2 msg: 1613172427.77 epoch sec; seqNo:3832 2021-02-12T23:27:07Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.17 psi 2021-02-12T23:27:07Z- handleDepth()-Depth_Keller msg: 1613172427776 epoch sec; seqNo:3871; depth 15.06 2021-02-12T23:27:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172427.79 epoch sec; seqNo:3871; 4057015.33 north; 590978.67 east 2021-02-12T23:27:08Z- AHRS_M2 msg: 1613172428.18 epoch sec; seqNo:3833 2021-02-12T23:27:08Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.16 psi 2021-02-12T23:27:08Z- handleDepth()-Depth_Keller msg: 1613172428180 epoch sec; seqNo:3872; depth 14.94 2021-02-12T23:27:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:08Z- handleDvl() - RDI_Pathfinder msg: 1613172428.18 epoch sec; seqNo:1836 2021-02-12T23:27:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , 0.10 2021-02-12T23:27:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172428.19 epoch sec; seqNo:3872; 4057015.71 north; 590978.67 east 2021-02-12T23:27:08Z- AHRS_M2 msg: 1613172428.59 epoch sec; seqNo:3834 2021-02-12T23:27:08Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.16 psi 2021-02-12T23:27:08Z- handleDepth()-Depth_Keller msg: 1613172428589 epoch sec; seqNo:3873; depth 14.82 2021-02-12T23:27:08Z- handleDvl() - RDI_Pathfinder msg: 1613172428.60 epoch sec; seqNo:1837 2021-02-12T23:27:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.14 , 96.36 , 108.52 , 71.51 2021-02-12T23:27:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.06 , 0.07 2021-02-12T23:27:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172428.61 epoch sec; seqNo:3873; 4057016.08 north; 590978.66 east 2021-02-12T23:27:08Z- AHRS_M2 msg: 1613172428.98 epoch sec; seqNo:3835 2021-02-12T23:27:08Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.15 psi 2021-02-12T23:27:08Z- handleDepth()-Depth_Keller msg: 1613172428998 epoch sec; seqNo:3874; depth 14.69 2021-02-12T23:27:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172429.01 epoch sec; seqNo:3874; 4057016.08 north; 590978.66 east 2021-02-12T23:27:09Z- AHRS_M2 msg: 1613172429.39 epoch sec; seqNo:3836 2021-02-12T23:27:09Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.14 psi 2021-02-12T23:27:09Z- handleDepth()-Depth_Keller msg: 1613172429392 epoch sec; seqNo:3875; depth 14.58 2021-02-12T23:27:09Z- handleDvl() - RDI_Pathfinder msg: 1613172429.40 epoch sec; seqNo:1838 2021-02-12T23:27:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.03 , 95.41 , 108.79 , 71.51 2021-02-12T23:27:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.09 , 0.06 2021-02-12T23:27:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172429.41 epoch sec; seqNo:3875; 4057016.46 north; 590978.66 east 2021-02-12T23:27:09Z- AHRS_M2 msg: 1613172429.80 epoch sec; seqNo:3837 2021-02-12T23:27:09Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.13 psi 2021-02-12T23:27:09Z- handleDepth()-Depth_Keller msg: 1613172429806 epoch sec; seqNo:3876; depth 14.46 2021-02-12T23:27:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172429.83 epoch sec; seqNo:3876; 4057016.84 north; 590978.66 east 2021-02-12T23:27:10Z- AHRS_M2 msg: 1613172430.19 epoch sec; seqNo:3838 2021-02-12T23:27:10Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.13 psi 2021-02-12T23:27:10Z- handleDepth()-Depth_Keller msg: 1613172430201 epoch sec; seqNo:3877; depth 14.32 2021-02-12T23:27:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172430.22 epoch sec; seqNo:3877; 4057017.23 north; 590979.87 east 2021-02-12T23:27:10Z- LcmTrn::handleMessages() - handling 43 messages took 0.62 ms and 131 calls... 2021-02-12T23:27:10Z- waiting for 1613172428.38, time is 1613172430.22 2021-02-12T23:27:10Z- LcmTrn::updateTrn() >>>> pose time 1613172430.22, meas time 1613172429.40 <<<< 2021-02-12T23:27:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172429.40 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:27:10Z- TerrainNav::motionUpdate() - currEstimate = (4057017, 590979.87, 14.32) 2021-02-12T23:27:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:27:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:27:10Z- TNavBF::Projecting Measurements 2021-02-12T23:27:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:27:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:27:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:27:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:27:10Z- TNavFilter::4 good beams. 2021-02-12T23:27:10Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:27:10Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:27:10Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:27:10Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:27:10Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:27:10Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:27:10Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:27:10Z- TNavBF::mmse selected from filter -1 2021-02-12T23:27:10Z- TerrainMapOctree: (vehN, vehE ) = (4057016.66, 590979.47). 2021-02-12T23:27:10Z- TerrainMapOctree: Min Distance = 5349.54. 2021-02-12T23:27:10Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:27:10Z- There is no specific good beam that all particles have in common. 2021-02-12T23:27:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:27:10Z- Old Depth Bias: 0.00 DepthBiasCov: 2.02 K: 0.139 ExpectedMeasDiff: nan currMeas.covariance: 10.77 2021-02-12T23:27:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:27:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.74 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 14.56 2021-02-12T23:27:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:27:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.57 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 18.94 2021-02-12T23:27:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:27:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.46 K: 0.128 ExpectedMeasDiff: nan currMeas.covariance: 8.18 2021-02-12T23:27:10Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:27:10Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:27:10Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:27:10Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:27:10Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:27:10Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:27:10Z- Filter 0 No beams used in subcloud update 2021-02-12T23:27:10Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:27:10Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:27:10Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 764.3 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-12T23:27:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172429.40 sec. 2021-02-12T23:27:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:27:10Z- TNavBF::mmse selected from filter -1 2021-02-12T23:27:10Z- variances([0], [2], and psi_berg) = 764.276339 833.047642 0.000000 2021-02-12T23:27:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:27:10Z- LcmTrn::publishEstimates() - MLE : 4056999.73 590931.37 14.32 2021-02-12T23:27:10Z- LcmTrn::publishEstimates() - MMSE : 4057017.23 590979.87 14.32 2021-02-12T23:27:10Z- LcmTrn::publishEstimates() - COVAR: 764.28 833.05 1.31 0.00 2021-02-12T23:27:10Z- AHRS_M2 msg: 1613172430.60 epoch sec; seqNo:3839 2021-02-12T23:27:10Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.14 psi 2021-02-12T23:27:10Z- handleDepth()-Depth_Keller msg: 1613172430607 epoch sec; seqNo:3878; depth 14.20 2021-02-12T23:27:10Z- handleDvl() - RDI_Pathfinder msg: 1613172430.62 epoch sec; seqNo:1839 2021-02-12T23:27:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.39 , 95.34 , 108.72 , 71.92 2021-02-12T23:27:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , 0.07 2021-02-12T23:27:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172430.65 epoch sec; seqNo:3878; 4057017.61 north; 590979.87 east 2021-02-12T23:27:11Z- AHRS_M2 msg: 1613172431.00 epoch sec; seqNo:3840 2021-02-12T23:27:11Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.14 psi 2021-02-12T23:27:11Z- handleDepth()-Depth_Keller msg: 1613172431013 epoch sec; seqNo:3879; depth 14.08 2021-02-12T23:27:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172431.03 epoch sec; seqNo:3879; 4057017.61 north; 590979.87 east 2021-02-12T23:27:11Z- AHRS_M2 msg: 1613172431.41 epoch sec; seqNo:3841 2021-02-12T23:27:11Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.14 psi 2021-02-12T23:27:11Z- handleDepth()-Depth_Keller msg: 1613172431417 epoch sec; seqNo:3880; depth 13.96 2021-02-12T23:27:11Z- handleDvl() - RDI_Pathfinder msg: 1613172431.43 epoch sec; seqNo:1840 2021-02-12T23:27:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 96.56 , 109.94 , 72.32 2021-02-12T23:27:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , 0.08 2021-02-12T23:27:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172431.44 epoch sec; seqNo:3880; 4057017.99 north; 590979.87 east 2021-02-12T23:27:11Z- AHRS_M2 msg: 1613172431.81 epoch sec; seqNo:3842 2021-02-12T23:27:11Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.14 psi 2021-02-12T23:27:11Z- handleDepth()-Depth_Keller msg: 1613172431817 epoch sec; seqNo:3881; depth 13.83 2021-02-12T23:27:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:11Z- handleDvl() - RDI_Pathfinder msg: 1613172431.82 epoch sec; seqNo:1841 2021-02-12T23:27:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , 0.08 2021-02-12T23:27:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172431.84 epoch sec; seqNo:3881; 4057018.37 north; 590979.86 east 2021-02-12T23:27:12Z- AHRS_M2 msg: 1613172432.22 epoch sec; seqNo:3843 2021-02-12T23:27:12Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.14 psi 2021-02-12T23:27:12Z- handleDepth()-Depth_Keller msg: 1613172432233 epoch sec; seqNo:3882; depth 13.71 2021-02-12T23:27:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172432.25 epoch sec; seqNo:3882; 4057018.75 north; 590979.86 east 2021-02-12T23:27:12Z- AHRS_M2 msg: 1613172432.62 epoch sec; seqNo:3844 2021-02-12T23:27:12Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.14 psi 2021-02-12T23:27:12Z- handleDepth()-Depth_Keller msg: 1613172432629 epoch sec; seqNo:3883; depth 13.60 2021-02-12T23:27:12Z- handleDvl() - RDI_Pathfinder msg: 1613172432.64 epoch sec; seqNo:1842 2021-02-12T23:27:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.54 , 97.71 , 105.01 , 72.32 2021-02-12T23:27:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.03 , 0.06 2021-02-12T23:27:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172432.65 epoch sec; seqNo:3883; 4057019.14 north; 590981.07 east 2021-02-12T23:27:13Z- AHRS_M2 msg: 1613172433.02 epoch sec; seqNo:3845 2021-02-12T23:27:13Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-12T23:27:13Z- handleDepth()-Depth_Keller msg: 1613172433030 epoch sec; seqNo:3884; depth 13.48 2021-02-12T23:27:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:13Z- handleDvl() - RDI_Pathfinder msg: 1613172433.03 epoch sec; seqNo:1843 2021-02-12T23:27:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.03 , 0.06 2021-02-12T23:27:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172433.04 epoch sec; seqNo:3884; 4057019.14 north; 590981.07 east 2021-02-12T23:27:13Z- AHRS_M2 msg: 1613172433.43 epoch sec; seqNo:3846 2021-02-12T23:27:13Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.13 psi 2021-02-12T23:27:13Z- handleDepth()-Depth_Keller msg: 1613172433432 epoch sec; seqNo:3885; depth 13.36 2021-02-12T23:27:13Z- handleDvl() - RDI_Pathfinder msg: 1613172433.45 epoch sec; seqNo:1844 2021-02-12T23:27:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.54 , 97.71 , 112.31 , 71.51 2021-02-12T23:27:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.02 2021-02-12T23:27:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172433.46 epoch sec; seqNo:3885; 4057019.52 north; 590981.07 east 2021-02-12T23:27:13Z- AHRS_M2 msg: 1613172433.83 epoch sec; seqNo:3847 2021-02-12T23:27:13Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.13 psi 2021-02-12T23:27:13Z- handleDepth()-Depth_Keller msg: 1613172433837 epoch sec; seqNo:3886; depth 13.24 2021-02-12T23:27:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172433.85 epoch sec; seqNo:3886; 4057019.90 north; 590981.07 east 2021-02-12T23:27:14Z- AHRS_M2 msg: 1613172434.24 epoch sec; seqNo:3848 2021-02-12T23:27:14Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.12 psi 2021-02-12T23:27:14Z- handleDepth()-Depth_Keller msg: 1613172434246 epoch sec; seqNo:3887; depth 13.12 2021-02-12T23:27:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:14Z- handleDvl() - RDI_Pathfinder msg: 1613172434.25 epoch sec; seqNo:1845 2021-02-12T23:27:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.02 2021-02-12T23:27:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172434.27 epoch sec; seqNo:3887; 4057020.28 north; 590981.06 east 2021-02-12T23:27:14Z- AHRS_M2 msg: 1613172434.64 epoch sec; seqNo:3849 2021-02-12T23:27:14Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.11 psi 2021-02-12T23:27:14Z- handleDepth()-Depth_Keller msg: 1613172434644 epoch sec; seqNo:3888; depth 12.99 2021-02-12T23:27:14Z- handleDvl() - RDI_Pathfinder msg: 1613172434.66 epoch sec; seqNo:1846 2021-02-12T23:27:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.54 , 95.00 , 113.93 , 72.32 2021-02-12T23:27:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , -0.03 2021-02-12T23:27:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172434.68 epoch sec; seqNo:3888; 4057020.28 north; 590981.06 east 2021-02-12T23:27:15Z- AHRS_M2 msg: 1613172435.04 epoch sec; seqNo:3850 2021-02-12T23:27:15Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.11 psi 2021-02-12T23:27:15Z- handleDepth()-Depth_Keller msg: 1613172435050 epoch sec; seqNo:3889; depth 12.87 2021-02-12T23:27:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172435.06 epoch sec; seqNo:3889; 4057020.67 north; 590982.28 east 2021-02-12T23:27:15Z- AHRS_M2 msg: 1613172435.45 epoch sec; seqNo:3851 2021-02-12T23:27:15Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.11 psi 2021-02-12T23:27:15Z- handleDepth()-Depth_Keller msg: 1613172435455 epoch sec; seqNo:3890; depth 12.77 2021-02-12T23:27:15Z- handleDvl() - RDI_Pathfinder msg: 1613172435.47 epoch sec; seqNo:1847 2021-02-12T23:27:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 95.21 , 112.24 , 73.55 2021-02-12T23:27:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.04 , -0.07 2021-02-12T23:27:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172435.48 epoch sec; seqNo:3890; 4057021.05 north; 590982.28 east 2021-02-12T23:27:15Z- LcmTrn::handleMessages() - handling 48 messages took 0.68 ms and 135 calls... 2021-02-12T23:27:15Z- waiting for 1613172433.22, time is 1613172435.48 2021-02-12T23:27:15Z- LcmTrn::updateTrn() equating pose 1613172435.48 and meas times 1613172435.48 2021-02-12T23:27:15Z- LcmTrn::updateTrn() >>>> pose time 1613172435.48, meas time 1613172435.48 <<<< 2021-02-12T23:27:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172435.48 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:27:15Z- TerrainNav::motionUpdate() - currEstimate = (4057021, 590982.28, 12.77) 2021-02-12T23:27:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:27:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:27:15Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172435.48 sec.; No relevant navigation data available 2021-02-12T23:27:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:27:15Z- TNavBF::mmse selected from filter -1 2021-02-12T23:27:15Z- variances([0], [2], and psi_berg) = 764.150192 833.047499 0.000000 2021-02-12T23:27:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:27:15Z- LcmTrn::publishEstimates() - MLE : 4057003.55 590934.78 12.77 2021-02-12T23:27:15Z- LcmTrn::publishEstimates() - MMSE : 4057021.05 590982.28 12.77 2021-02-12T23:27:15Z- LcmTrn::publishEstimates() - COVAR: 764.15 833.05 1.54 0.00 2021-02-12T23:27:15Z- AHRS_M2 msg: 1613172435.85 epoch sec; seqNo:3852 2021-02-12T23:27:15Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.11 psi 2021-02-12T23:27:15Z- handleDepth()-Depth_Keller msg: 1613172435858 epoch sec; seqNo:3891; depth 12.64 2021-02-12T23:27:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172435.87 epoch sec; seqNo:3891; 4057021.05 north; 590982.28 east 2021-02-12T23:27:16Z- AHRS_M2 msg: 1613172436.26 epoch sec; seqNo:3853 2021-02-12T23:27:16Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.12 psi 2021-02-12T23:27:16Z- handleDepth()-Depth_Keller msg: 1613172436260 epoch sec; seqNo:3892; depth 12.50 2021-02-12T23:27:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172436.28 epoch sec; seqNo:3892; 4057021.43 north; 590982.27 east 2021-02-12T23:27:16Z- AHRS_M2 msg: 1613172436.66 epoch sec; seqNo:3854 2021-02-12T23:27:16Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.13 psi 2021-02-12T23:27:16Z- handleDepth()-Depth_Keller msg: 1613172436664 epoch sec; seqNo:3893; depth 12.37 2021-02-12T23:27:16Z- handleDvl() - RDI_Pathfinder msg: 1613172436.68 epoch sec; seqNo:1848 2021-02-12T23:27:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 93.99 , 112.24 , 72.32 2021-02-12T23:27:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.15 , -0.04 2021-02-12T23:27:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172436.70 epoch sec; seqNo:3893; 4057021.43 north; 590982.27 east 2021-02-12T23:27:17Z- AHRS_M2 msg: 1613172437.06 epoch sec; seqNo:3855 2021-02-12T23:27:17Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.14 psi 2021-02-12T23:27:17Z- handleDepth()-Depth_Keller msg: 1613172437073 epoch sec; seqNo:3894; depth 12.25 2021-02-12T23:27:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172437.09 epoch sec; seqNo:3894; 4057021.81 north; 590982.27 east 2021-02-12T23:27:17Z- AHRS_M2 msg: 1613172437.47 epoch sec; seqNo:3856 2021-02-12T23:27:17Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.14 psi 2021-02-12T23:27:17Z- handleDepth()-Depth_Keller msg: 1613172437474 epoch sec; seqNo:3895; depth 12.11 2021-02-12T23:27:17Z- handleDvl() - RDI_Pathfinder msg: 1613172437.48 epoch sec; seqNo:1849 2021-02-12T23:27:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.26 , 97.64 , 109.81 , 73.96 2021-02-12T23:27:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.17 , 0.11 2021-02-12T23:27:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172437.49 epoch sec; seqNo:3895; 4057022.19 north; 590982.26 east 2021-02-12T23:27:17Z- AHRS_M2 msg: 1613172437.87 epoch sec; seqNo:3857 2021-02-12T23:27:17Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.14 psi 2021-02-12T23:27:17Z- handleDepth()-Depth_Keller msg: 1613172437881 epoch sec; seqNo:3896; depth 11.98 2021-02-12T23:27:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:17Z- handleDvl() - RDI_Pathfinder msg: 1613172437.88 epoch sec; seqNo:1850 2021-02-12T23:27:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.17 , 0.11 2021-02-12T23:27:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172437.91 epoch sec; seqNo:3896; 4057022.56 north; 590982.26 east 2021-02-12T23:27:18Z- AHRS_M2 msg: 1613172438.28 epoch sec; seqNo:3858 2021-02-12T23:27:18Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.14 psi 2021-02-12T23:27:18Z- handleDepth()-Depth_Keller msg: 1613172438290 epoch sec; seqNo:3897; depth 11.85 2021-02-12T23:27:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172438.33 epoch sec; seqNo:3897; 4057022.56 north; 590982.26 east 2021-02-12T23:27:18Z- AHRS_M2 msg: 1613172438.68 epoch sec; seqNo:3859 2021-02-12T23:27:18Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.14 psi 2021-02-12T23:27:18Z- handleDepth()-Depth_Keller msg: 1613172438696 epoch sec; seqNo:3898; depth 11.73 2021-02-12T23:27:18Z- handleDvl() - RDI_Pathfinder msg: 1613172438.71 epoch sec; seqNo:1851 2021-02-12T23:27:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.26 , 96.42 , 115.89 , 74.78 2021-02-12T23:27:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , 0.19 2021-02-12T23:27:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172438.72 epoch sec; seqNo:3898; 4057022.96 north; 590983.48 east 2021-02-12T23:27:19Z- AHRS_M2 msg: 1613172439.08 epoch sec; seqNo:3860 2021-02-12T23:27:19Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.13 psi 2021-02-12T23:27:19Z- handleDepth()-Depth_Keller msg: 1613172439088 epoch sec; seqNo:3899; depth 11.58 2021-02-12T23:27:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:19Z- handleDvl() - RDI_Pathfinder msg: 1613172439.10 epoch sec; seqNo:1852 2021-02-12T23:27:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , 0.19 2021-02-12T23:27:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172439.11 epoch sec; seqNo:3899; 4057023.33 north; 590983.47 east 2021-02-12T23:27:19Z- AHRS_M2 msg: 1613172439.49 epoch sec; seqNo:3861 2021-02-12T23:27:19Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.13 psi 2021-02-12T23:27:19Z- handleDepth()-Depth_Keller msg: 1613172439492 epoch sec; seqNo:3900; depth 11.47 2021-02-12T23:27:19Z- handleDvl() - RDI_Pathfinder msg: 1613172439.50 epoch sec; seqNo:1853 2021-02-12T23:27:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.83 , 97.64 , 115.21 , 73.96 2021-02-12T23:27:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.09 , 0.11 2021-02-12T23:27:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172439.52 epoch sec; seqNo:3900; 4057023.71 north; 590983.47 east 2021-02-12T23:27:19Z- AHRS_M2 msg: 1613172439.89 epoch sec; seqNo:3862 2021-02-12T23:27:19Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.13 psi 2021-02-12T23:27:19Z- handleDepth()-Depth_Keller msg: 1613172439897 epoch sec; seqNo:3901; depth 11.34 2021-02-12T23:27:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172439.91 epoch sec; seqNo:3901; 4057023.71 north; 590983.47 east 2021-02-12T23:27:20Z- AHRS_M2 msg: 1613172440.31 epoch sec; seqNo:3863 2021-02-12T23:27:20Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.13 psi 2021-02-12T23:27:20Z- handleDepth()-Depth_Keller msg: 1613172440309 epoch sec; seqNo:3902; depth 11.23 2021-02-12T23:27:20Z- handleDvl() - RDI_Pathfinder msg: 1613172440.32 epoch sec; seqNo:1854 2021-02-12T23:27:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.76 , 97.57 , 113.79 , 73.14 2021-02-12T23:27:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.14 , -0.02 2021-02-12T23:27:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172440.33 epoch sec; seqNo:3902; 4057024.09 north; 590983.46 east 2021-02-12T23:27:20Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 131 calls... 2021-02-12T23:27:20Z- waiting for 1613172438.48, time is 1613172440.33 2021-02-12T23:27:20Z- LcmTrn::updateTrn() equating pose 1613172440.33 and meas times 1613172440.33 2021-02-12T23:27:20Z- LcmTrn::updateTrn() >>>> pose time 1613172440.33, meas time 1613172440.33 <<<< 2021-02-12T23:27:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172440.33 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:27:20Z- TerrainNav::motionUpdate() - currEstimate = (4057024, 590983.46, 11.23) 2021-02-12T23:27:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:27:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:27:20Z- TNavBF::Projecting Measurements 2021-02-12T23:27:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:27:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:27:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:27:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:27:20Z- TNavFilter::4 good beams. 2021-02-12T23:27:20Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:27:20Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:27:20Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:27:20Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:27:20Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:27:20Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:27:20Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:27:20Z- TNavBF::mmse selected from filter -1 2021-02-12T23:27:20Z- TerrainMapOctree: (vehN, vehE ) = (4057024.09, 590983.46). 2021-02-12T23:27:20Z- TerrainMapOctree: Min Distance = 5341.21. 2021-02-12T23:27:20Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:27:20Z- There is no specific good beam that all particles have in common. 2021-02-12T23:27:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:27:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.70 K: 0.110 ExpectedMeasDiff: nan currMeas.covariance: 12.04 2021-02-12T23:27:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:27:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.52 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 15.23 2021-02-12T23:27:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:27:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.39 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 20.72 2021-02-12T23:27:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:27:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-12T23:27:20Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:27:20Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:27:20Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:27:20Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:27:20Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:27:20Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:27:20Z- Filter 0 No beams used in subcloud update 2021-02-12T23:27:20Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:27:20Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:27:20Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 764.0 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:27:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172440.33 sec. 2021-02-12T23:27:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:27:20Z- TNavBF::mmse selected from filter -1 2021-02-12T23:27:20Z- variances([0], [2], and psi_berg) = 764.042989 833.047379 0.000000 2021-02-12T23:27:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:27:20Z- LcmTrn::publishEstimates() - MLE : 4057006.59 590935.96 11.23 2021-02-12T23:27:20Z- LcmTrn::publishEstimates() - MMSE : 4057024.09 590983.46 11.23 2021-02-12T23:27:20Z- LcmTrn::publishEstimates() - COVAR: 764.04 833.05 1.16 0.00 2021-02-12T23:27:20Z- AHRS_M2 msg: 1613172440.70 epoch sec; seqNo:3864 2021-02-12T23:27:20Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.14 psi 2021-02-12T23:27:20Z- handleDepth()-Depth_Keller msg: 1613172440706 epoch sec; seqNo:3903; depth 11.10 2021-02-12T23:27:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172440.72 epoch sec; seqNo:3903; 4057024.11 north; 590984.68 east 2021-02-12T23:27:21Z- AHRS_M2 msg: 1613172441.10 epoch sec; seqNo:3865 2021-02-12T23:27:21Z- AHRS_M2 msg: 0.68 phi; 0.34 theta; -0.14 psi 2021-02-12T23:27:21Z- handleDepth()-Depth_Keller msg: 1613172441113 epoch sec; seqNo:3904; depth 10.98 2021-02-12T23:27:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172441.13 epoch sec; seqNo:3904; 4057024.48 north; 590984.68 east 2021-02-12T23:27:21Z- AHRS_M2 msg: 1613172441.51 epoch sec; seqNo:3866 2021-02-12T23:27:21Z- AHRS_M2 msg: 0.68 phi; 0.33 theta; -0.15 psi 2021-02-12T23:27:21Z- handleDepth()-Depth_Keller msg: 1613172441512 epoch sec; seqNo:3905; depth 10.86 2021-02-12T23:27:21Z- handleDvl() - RDI_Pathfinder msg: 1613172441.52 epoch sec; seqNo:1855 2021-02-12T23:27:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 96.29 , 115.21 , 74.37 2021-02-12T23:27:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , 0.09 , -0.08 2021-02-12T23:27:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172441.55 epoch sec; seqNo:3905; 4057024.86 north; 590984.68 east 2021-02-12T23:27:21Z- AHRS_M2 msg: 1613172441.91 epoch sec; seqNo:3867 2021-02-12T23:27:21Z- AHRS_M2 msg: 0.68 phi; 0.33 theta; -0.16 psi 2021-02-12T23:27:21Z- handleDepth()-Depth_Keller msg: 1613172441921 epoch sec; seqNo:3906; depth 10.72 2021-02-12T23:27:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172441.93 epoch sec; seqNo:3906; 4057024.86 north; 590984.68 east 2021-02-12T23:27:22Z- AHRS_M2 msg: 1613172442.32 epoch sec; seqNo:3868 2021-02-12T23:27:22Z- AHRS_M2 msg: 0.68 phi; 0.33 theta; -0.17 psi 2021-02-12T23:27:22Z- handleDepth()-Depth_Keller msg: 1613172442329 epoch sec; seqNo:3907; depth 10.60 2021-02-12T23:27:22Z- handleDvl() - RDI_Pathfinder msg: 1613172442.34 epoch sec; seqNo:1856 2021-02-12T23:27:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 100.34 , 112.51 , 74.37 2021-02-12T23:27:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.00 , -0.05 2021-02-12T23:27:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172442.35 epoch sec; seqNo:3907; 4057025.24 north; 590984.67 east 2021-02-12T23:27:22Z- AHRS_M2 msg: 1613172442.72 epoch sec; seqNo:3869 2021-02-12T23:27:22Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.18 psi 2021-02-12T23:27:22Z- handleDepth()-Depth_Keller msg: 1613172442729 epoch sec; seqNo:3908; depth 10.45 2021-02-12T23:27:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:22Z- handleDvl() - RDI_Pathfinder msg: 1613172442.73 epoch sec; seqNo:1857 2021-02-12T23:27:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.00 , -0.05 2021-02-12T23:27:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172442.75 epoch sec; seqNo:3908; 4057025.24 north; 590984.67 east 2021-02-12T23:27:23Z- AHRS_M2 msg: 1613172443.13 epoch sec; seqNo:3870 2021-02-12T23:27:23Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.20 psi 2021-02-12T23:27:23Z- handleDepth()-Depth_Keller msg: 1613172443137 epoch sec; seqNo:3909; depth 10.33 2021-02-12T23:27:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172443.15 epoch sec; seqNo:3909; 4057025.63 north; 590985.89 east 2021-02-12T23:27:23Z- AHRS_M2 msg: 1613172443.53 epoch sec; seqNo:3871 2021-02-12T23:27:23Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.21 psi 2021-02-12T23:27:23Z- handleDepth()-Depth_Keller msg: 1613172443541 epoch sec; seqNo:3910; depth 10.21 2021-02-12T23:27:23Z- handleDvl() - RDI_Pathfinder msg: 1613172443.56 epoch sec; seqNo:1858 2021-02-12T23:27:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 101.70 , 113.86 , 75.18 2021-02-12T23:27:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.08 , 0.03 2021-02-12T23:27:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172443.58 epoch sec; seqNo:3910; 4057025.63 north; 590985.89 east 2021-02-12T23:27:23Z- handleDepth()-Depth_Keller msg: 1613172443958 epoch sec; seqNo:3911; depth 10.05 2021-02-12T23:27:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:23Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:23Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:23Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:23Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:23Z- handleDvl() - RDI_Pathfinder msg: 1613172443.96 epoch sec; seqNo:1859 2021-02-12T23:27:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.08 , 0.03 2021-02-12T23:27:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172443.97 epoch sec; seqNo:3911; 4057026.01 north; 590985.89 east 2021-02-12T23:27:24Z- AHRS_M2 msg: 1613172444.34 epoch sec; seqNo:3872 2021-02-12T23:27:24Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.22 psi 2021-02-12T23:27:24Z- handleDepth()-Depth_Keller msg: 1613172444345 epoch sec; seqNo:3912; depth 9.93 2021-02-12T23:27:24Z- handleDvl() - RDI_Pathfinder msg: 1613172444.36 epoch sec; seqNo:1860 2021-02-12T23:27:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.70 , 104.33 , 115.15 , 74.78 2021-02-12T23:27:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.09 , 0.12 2021-02-12T23:27:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172444.37 epoch sec; seqNo:3912; 4057026.39 north; 590985.88 east 2021-02-12T23:27:24Z- AHRS_M2 msg: 1613172444.74 epoch sec; seqNo:3873 2021-02-12T23:27:24Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.21 psi 2021-02-12T23:27:24Z- handleDepth()-Depth_Keller msg: 1613172444746 epoch sec; seqNo:3913; depth 9.82 2021-02-12T23:27:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172444.76 epoch sec; seqNo:3913; 4057026.77 north; 590985.88 east 2021-02-12T23:27:25Z- AHRS_M2 msg: 1613172445.15 epoch sec; seqNo:3874 2021-02-12T23:27:25Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.21 psi 2021-02-12T23:27:25Z- handleDepth()-Depth_Keller msg: 1613172445151 epoch sec; seqNo:3914; depth 9.71 2021-02-12T23:27:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:25Z- handleDvl() - RDI_Pathfinder msg: 1613172445.16 epoch sec; seqNo:1861 2021-02-12T23:27:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.09 , 0.12 2021-02-12T23:27:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172445.17 epoch sec; seqNo:3914; 4057026.77 north; 590985.88 east 2021-02-12T23:27:25Z- AHRS_M2 msg: 1613172445.55 epoch sec; seqNo:3875 2021-02-12T23:27:25Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.22 psi 2021-02-12T23:27:25Z- handleDepth()-Depth_Keller msg: 1613172445559 epoch sec; seqNo:3915; depth 9.57 2021-02-12T23:27:25Z- handleDvl() - RDI_Pathfinder msg: 1613172445.58 epoch sec; seqNo:1862 2021-02-12T23:27:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 102.92 , 115.09 , 75.19 2021-02-12T23:27:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.03 , 0.16 2021-02-12T23:27:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172445.59 epoch sec; seqNo:3915; 4057027.15 north; 590985.87 east 2021-02-12T23:27:25Z- LcmTrn::handleMessages() - handling 46 messages took 0.67 ms and 133 calls... 2021-02-12T23:27:25Z- waiting for 1613172443.33, time is 1613172445.59 2021-02-12T23:27:25Z- LcmTrn::updateTrn() equating pose 1613172445.59 and meas times 1613172445.59 2021-02-12T23:27:25Z- LcmTrn::updateTrn() >>>> pose time 1613172445.59, meas time 1613172445.59 <<<< 2021-02-12T23:27:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172445.59 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:27:25Z- TerrainNav::motionUpdate() - currEstimate = (4057027, 590985.87, 9.57) 2021-02-12T23:27:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:27:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:27:25Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172445.59 sec.; No relevant navigation data available 2021-02-12T23:27:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:27:25Z- TNavBF::mmse selected from filter -1 2021-02-12T23:27:25Z- variances([0], [2], and psi_berg) = 763.926169 833.047248 0.000000 2021-02-12T23:27:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:27:25Z- LcmTrn::publishEstimates() - MLE : 4057009.65 590938.37 9.57 2021-02-12T23:27:25Z- LcmTrn::publishEstimates() - MMSE : 4057027.15 590985.87 9.57 2021-02-12T23:27:25Z- LcmTrn::publishEstimates() - COVAR: 763.93 833.05 1.36 0.00 2021-02-12T23:27:25Z- AHRS_M2 msg: 1613172445.95 epoch sec; seqNo:3876 2021-02-12T23:27:25Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.22 psi 2021-02-12T23:27:25Z- handleDepth()-Depth_Keller msg: 1613172445964 epoch sec; seqNo:3916; depth 9.47 2021-02-12T23:27:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172446.01 epoch sec; seqNo:3916; 4057027.54 north; 590987.09 east 2021-02-12T23:27:26Z- AHRS_M2 msg: 1613172446.36 epoch sec; seqNo:3877 2021-02-12T23:27:26Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.22 psi 2021-02-12T23:27:26Z- handleDepth()-Depth_Keller msg: 1613172446364 epoch sec; seqNo:3917; depth 9.37 2021-02-12T23:27:26Z- handleDvl() - RDI_Pathfinder msg: 1613172446.38 epoch sec; seqNo:1863 2021-02-12T23:27:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.62 , 115.09 , 76.01 2021-02-12T23:27:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.06 , 0.10 2021-02-12T23:27:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172446.39 epoch sec; seqNo:3917; 4057027.92 north; 590987.09 east 2021-02-12T23:27:26Z- AHRS_M2 msg: 1613172446.76 epoch sec; seqNo:3878 2021-02-12T23:27:26Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.21 psi 2021-02-12T23:27:26Z- handleDepth()-Depth_Keller msg: 1613172446765 epoch sec; seqNo:3918; depth 9.26 2021-02-12T23:27:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172446.79 epoch sec; seqNo:3918; 4057028.30 north; 590987.08 east 2021-02-12T23:27:27Z- AHRS_M2 msg: 1613172447.16 epoch sec; seqNo:3879 2021-02-12T23:27:27Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.19 psi 2021-02-12T23:27:27Z- handleDepth()-Depth_Keller msg: 1613172447169 epoch sec; seqNo:3919; depth 9.17 2021-02-12T23:27:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172447.18 epoch sec; seqNo:3919; 4057028.30 north; 590987.08 east 2021-02-12T23:27:27Z- AHRS_M2 msg: 1613172447.57 epoch sec; seqNo:3880 2021-02-12T23:27:27Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-12T23:27:27Z- handleDepth()-Depth_Keller msg: 1613172447574 epoch sec; seqNo:3920; depth 9.07 2021-02-12T23:27:27Z- handleDvl() - RDI_Pathfinder msg: 1613172447.59 epoch sec; seqNo:1864 2021-02-12T23:27:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.93 , 103.86 , 113.32 , 75.59 2021-02-12T23:27:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.09 , -0.02 2021-02-12T23:27:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172447.60 epoch sec; seqNo:3920; 4057028.68 north; 590987.08 east 2021-02-12T23:27:27Z- AHRS_M2 msg: 1613172447.97 epoch sec; seqNo:3881 2021-02-12T23:27:27Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-12T23:27:27Z- handleDepth()-Depth_Keller msg: 1613172447976 epoch sec; seqNo:3921; depth 8.98 2021-02-12T23:27:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172448.00 epoch sec; seqNo:3921; 4057029.07 north; 590988.30 east 2021-02-12T23:27:28Z- AHRS_M2 msg: 1613172448.38 epoch sec; seqNo:3882 2021-02-12T23:27:28Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-12T23:27:28Z- handleDepth()-Depth_Keller msg: 1613172448380 epoch sec; seqNo:3922; depth 8.87 2021-02-12T23:27:28Z- handleDvl() - RDI_Pathfinder msg: 1613172448.39 epoch sec; seqNo:1865 2021-02-12T23:27:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 96.15 , 112.38 , 77.23 2021-02-12T23:27:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.04 , -0.11 2021-02-12T23:27:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172448.42 epoch sec; seqNo:3922; 4057029.07 north; 590988.30 east 2021-02-12T23:27:28Z- AHRS_M2 msg: 1613172448.78 epoch sec; seqNo:3883 2021-02-12T23:27:28Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.15 psi 2021-02-12T23:27:28Z- handleDepth()-Depth_Keller msg: 1613172448784 epoch sec; seqNo:3923; depth 8.77 2021-02-12T23:27:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:28Z- handleDvl() - RDI_Pathfinder msg: 1613172448.79 epoch sec; seqNo:1866 2021-02-12T23:27:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.04 , -0.11 2021-02-12T23:27:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172448.80 epoch sec; seqNo:3923; 4057029.45 north; 590988.29 east 2021-02-12T23:27:29Z- AHRS_M2 msg: 1613172449.18 epoch sec; seqNo:3884 2021-02-12T23:27:29Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.15 psi 2021-02-12T23:27:29Z- handleDepth()-Depth_Keller msg: 1613172449194 epoch sec; seqNo:3924; depth 8.66 2021-02-12T23:27:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172449.20 epoch sec; seqNo:3924; 4057029.83 north; 590988.29 east 2021-02-12T23:27:29Z- AHRS_M2 msg: 1613172449.59 epoch sec; seqNo:3885 2021-02-12T23:27:29Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.16 psi 2021-02-12T23:27:29Z- handleDepth()-Depth_Keller msg: 1613172449603 epoch sec; seqNo:3925; depth 8.57 2021-02-12T23:27:29Z- handleDvl() - RDI_Pathfinder msg: 1613172449.62 epoch sec; seqNo:1867 2021-02-12T23:27:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.83 , 96.29 , 112.51 , 77.23 2021-02-12T23:27:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.05 , -0.10 2021-02-12T23:27:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172449.62 epoch sec; seqNo:3925; 4057029.83 north; 590988.29 east 2021-02-12T23:27:29Z- AHRS_M2 msg: 1613172449.99 epoch sec; seqNo:3886 2021-02-12T23:27:29Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-12T23:27:29Z- handleDepth()-Depth_Keller msg: 1613172449997 epoch sec; seqNo:3926; depth 8.46 2021-02-12T23:27:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:30Z- handleDvl() - RDI_Pathfinder msg: 1613172450.00 epoch sec; seqNo:1868 2021-02-12T23:27:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.05 , -0.10 2021-02-12T23:27:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172450.01 epoch sec; seqNo:3926; 4057030.21 north; 590988.28 east 2021-02-12T23:27:30Z- AHRS_M2 msg: 1613172450.39 epoch sec; seqNo:3887 2021-02-12T23:27:30Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-12T23:27:30Z- handleDepth()-Depth_Keller msg: 1613172450405 epoch sec; seqNo:3927; depth 8.36 2021-02-12T23:27:30Z- handleDvl() - RDI_Pathfinder msg: 1613172450.42 epoch sec; seqNo:1869 2021-02-12T23:27:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 103.05 , 112.51 , 77.23 2021-02-12T23:27:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.09 , 0.03 2021-02-12T23:27:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172450.43 epoch sec; seqNo:3927; 4057030.21 north; 590988.28 east 2021-02-12T23:27:30Z- LcmTrn::handleMessages() - handling 43 messages took 0.74 ms and 129 calls... 2021-02-12T23:27:30Z- waiting for 1613172448.59, time is 1613172450.43 2021-02-12T23:27:30Z- LcmTrn::updateTrn() equating pose 1613172450.43 and meas times 1613172450.43 2021-02-12T23:27:30Z- LcmTrn::updateTrn() >>>> pose time 1613172450.43, meas time 1613172450.43 <<<< 2021-02-12T23:27:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172450.43 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:27:30Z- TerrainNav::motionUpdate() - currEstimate = (4057030, 590988.28, 8.36) 2021-02-12T23:27:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:27:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:27:30Z- TNavBF::Projecting Measurements 2021-02-12T23:27:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:27:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:27:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:27:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:27:30Z- TNavFilter::4 good beams. 2021-02-12T23:27:30Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:27:30Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:27:30Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:27:30Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:27:30Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:27:30Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:27:30Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:27:30Z- TNavBF::mmse selected from filter -1 2021-02-12T23:27:30Z- TerrainMapOctree: (vehN, vehE ) = (4057030.21, 590988.28). 2021-02-12T23:27:30Z- TerrainMapOctree: Min Distance = 5333.43. 2021-02-12T23:27:30Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:27:30Z- There is no specific good beam that all particles have in common. 2021-02-12T23:27:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:27:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.56 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 11.69 2021-02-12T23:27:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:27:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 16.99 2021-02-12T23:27:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:27:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 20.25 2021-02-12T23:27:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:27:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 9.54 2021-02-12T23:27:30Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:27:30Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:27:30Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:27:30Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:27:30Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:27:30Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:27:30Z- Filter 0 No beams used in subcloud update 2021-02-12T23:27:30Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:27:30Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:27:30Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 763.8 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:27:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172450.43 sec. 2021-02-12T23:27:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:27:30Z- TNavBF::mmse selected from filter -1 2021-02-12T23:27:30Z- variances([0], [2], and psi_berg) = 763.809551 833.047117 0.000000 2021-02-12T23:27:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:27:30Z- LcmTrn::publishEstimates() - MLE : 4057012.71 590940.78 8.36 2021-02-12T23:27:30Z- LcmTrn::publishEstimates() - MMSE : 4057030.21 590988.28 8.36 2021-02-12T23:27:30Z- LcmTrn::publishEstimates() - COVAR: 763.81 833.05 1.11 0.00 2021-02-12T23:27:30Z- AHRS_M2 msg: 1613172450.80 epoch sec; seqNo:3888 2021-02-12T23:27:30Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-12T23:27:30Z- handleDepth()-Depth_Keller msg: 1613172450806 epoch sec; seqNo:3928; depth 8.23 2021-02-12T23:27:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172450.82 epoch sec; seqNo:3928; 4057030.58 north; 590988.28 east 2021-02-12T23:27:31Z- AHRS_M2 msg: 1613172451.23 epoch sec; seqNo:3889 2021-02-12T23:27:31Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-12T23:27:31Z- handleDepth()-Depth_Keller msg: 1613172451233 epoch sec; seqNo:3929; depth 8.14 2021-02-12T23:27:31Z- handleDvl() - RDI_Pathfinder msg: 1613172451.25 epoch sec; seqNo:1870 2021-02-12T23:27:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.91 , 113.73 , 76.82 2021-02-12T23:27:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.03 , 0.10 2021-02-12T23:27:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172451.26 epoch sec; seqNo:3929; 4057030.98 north; 590989.50 east 2021-02-12T23:27:31Z- AHRS_M2 msg: 1613172451.61 epoch sec; seqNo:3890 2021-02-12T23:27:31Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.18 psi 2021-02-12T23:27:31Z- handleDepth()-Depth_Keller msg: 1613172451612 epoch sec; seqNo:3930; depth 8.02 2021-02-12T23:27:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172451.62 epoch sec; seqNo:3930; 4057030.98 north; 590989.50 east 2021-02-12T23:27:32Z- AHRS_M2 msg: 1613172452.01 epoch sec; seqNo:3891 2021-02-12T23:27:32Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.19 psi 2021-02-12T23:27:32Z- handleDepth()-Depth_Keller msg: 1613172452016 epoch sec; seqNo:3931; depth 7.92 2021-02-12T23:27:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172452.03 epoch sec; seqNo:3931; 4057031.36 north; 590989.49 east 2021-02-12T23:27:32Z- AHRS_M2 msg: 1613172452.43 epoch sec; seqNo:3892 2021-02-12T23:27:32Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.18 psi 2021-02-12T23:27:32Z- handleDepth()-Depth_Keller msg: 1613172452438 epoch sec; seqNo:3932; depth 7.82 2021-02-12T23:27:32Z- handleDvl() - RDI_Pathfinder msg: 1613172452.45 epoch sec; seqNo:1871 2021-02-12T23:27:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.77 , 103.99 , 117.51 , 78.31 2021-02-12T23:27:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.09 2021-02-12T23:27:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172452.46 epoch sec; seqNo:3932; 4057031.73 north; 590989.49 east 2021-02-12T23:27:32Z- AHRS_M2 msg: 1613172452.82 epoch sec; seqNo:3893 2021-02-12T23:27:32Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.18 psi 2021-02-12T23:27:32Z- handleDepth()-Depth_Keller msg: 1613172452829 epoch sec; seqNo:3933; depth 7.70 2021-02-12T23:27:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172452.85 epoch sec; seqNo:3933; 4057032.11 north; 590989.48 east 2021-02-12T23:27:33Z- AHRS_M2 msg: 1613172453.22 epoch sec; seqNo:3894 2021-02-12T23:27:33Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.17 psi 2021-02-12T23:27:33Z- handleDepth()-Depth_Keller msg: 1613172453229 epoch sec; seqNo:3934; depth 7.58 2021-02-12T23:27:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172453.24 epoch sec; seqNo:3934; 4057032.11 north; 590989.48 east 2021-02-12T23:27:33Z- AHRS_M2 msg: 1613172453.63 epoch sec; seqNo:3895 2021-02-12T23:27:33Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.16 psi 2021-02-12T23:27:33Z- handleDepth()-Depth_Keller msg: 1613172453632 epoch sec; seqNo:3935; depth 7.47 2021-02-12T23:27:33Z- handleDvl() - RDI_Pathfinder msg: 1613172453.64 epoch sec; seqNo:1872 2021-02-12T23:27:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.30 , 105.41 , 120.28 , 78.45 2021-02-12T23:27:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , 0.06 2021-02-12T23:27:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172453.65 epoch sec; seqNo:3935; 4057032.49 north; 590989.48 east 2021-02-12T23:27:34Z- AHRS_M2 msg: 1613172454.03 epoch sec; seqNo:3896 2021-02-12T23:27:34Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.15 psi 2021-02-12T23:27:34Z- handleDepth()-Depth_Keller msg: 1613172454041 epoch sec; seqNo:3936; depth 7.35 2021-02-12T23:27:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172454.05 epoch sec; seqNo:3936; 4057032.88 north; 590990.70 east 2021-02-12T23:27:34Z- AHRS_M2 msg: 1613172454.44 epoch sec; seqNo:3897 2021-02-12T23:27:34Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.15 psi 2021-02-12T23:27:34Z- handleDepth()-Depth_Keller msg: 1613172454440 epoch sec; seqNo:3937; depth 7.24 2021-02-12T23:27:34Z- handleDvl() - RDI_Pathfinder msg: 1613172454.46 epoch sec; seqNo:1873 2021-02-12T23:27:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.85 , 105.07 , 121.30 , 78.45 2021-02-12T23:27:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.11 , -0.07 2021-02-12T23:27:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172454.47 epoch sec; seqNo:3937; 4057032.88 north; 590990.70 east 2021-02-12T23:27:34Z- AHRS_M2 msg: 1613172454.84 epoch sec; seqNo:3898 2021-02-12T23:27:34Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.15 psi 2021-02-12T23:27:34Z- handleDepth()-Depth_Keller msg: 1613172454844 epoch sec; seqNo:3938; depth 7.12 2021-02-12T23:27:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:34Z- handleDvl() - RDI_Pathfinder msg: 1613172454.85 epoch sec; seqNo:1874 2021-02-12T23:27:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.11 , -0.07 2021-02-12T23:27:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172454.86 epoch sec; seqNo:3938; 4057033.26 north; 590990.69 east 2021-02-12T23:27:35Z- AHRS_M2 msg: 1613172455.24 epoch sec; seqNo:3899 2021-02-12T23:27:35Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.15 psi 2021-02-12T23:27:35Z- handleDepth()-Depth_Keller msg: 1613172455253 epoch sec; seqNo:3939; depth 7.00 2021-02-12T23:27:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172455.26 epoch sec; seqNo:3939; 4057033.64 north; 590990.69 east 2021-02-12T23:27:35Z- AHRS_M2 msg: 1613172455.65 epoch sec; seqNo:3900 2021-02-12T23:27:35Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.16 psi 2021-02-12T23:27:35Z- handleDepth()-Depth_Keller msg: 1613172455654 epoch sec; seqNo:3940; depth 6.85 2021-02-12T23:27:35Z- handleDvl() - RDI_Pathfinder msg: 1613172455.66 epoch sec; seqNo:1875 2021-02-12T23:27:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.41 , 103.93 , 122.85 , 79.27 2021-02-12T23:27:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.03 , -0.19 2021-02-12T23:27:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172455.67 epoch sec; seqNo:3940; 4057033.64 north; 590990.69 east 2021-02-12T23:27:35Z- LcmTrn::handleMessages() - handling 45 messages took 0.55 ms and 134 calls... 2021-02-12T23:27:35Z- waiting for 1613172453.43, time is 1613172455.67 2021-02-12T23:27:35Z- LcmTrn::updateTrn() equating pose 1613172455.67 and meas times 1613172455.67 2021-02-12T23:27:35Z- LcmTrn::updateTrn() >>>> pose time 1613172455.67, meas time 1613172455.67 <<<< 2021-02-12T23:27:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172455.67 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:27:35Z- TerrainNav::motionUpdate() - currEstimate = (4057034, 590990.69, 6.85) 2021-02-12T23:27:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:27:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:27:35Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172455.67 sec.; No relevant navigation data available 2021-02-12T23:27:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:27:35Z- TNavBF::mmse selected from filter -1 2021-02-12T23:27:35Z- variances([0], [2], and psi_berg) = 763.688704 833.046982 0.000000 2021-02-12T23:27:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:27:35Z- LcmTrn::publishEstimates() - MLE : 4057016.14 590943.19 6.85 2021-02-12T23:27:35Z- LcmTrn::publishEstimates() - MMSE : 4057033.64 590990.69 6.85 2021-02-12T23:27:35Z- LcmTrn::publishEstimates() - COVAR: 763.69 833.05 1.32 0.00 2021-02-12T23:27:36Z- AHRS_M2 msg: 1613172456.05 epoch sec; seqNo:3901 2021-02-12T23:27:36Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.16 psi 2021-02-12T23:27:36Z- handleDepth()-Depth_Keller msg: 1613172456059 epoch sec; seqNo:3941; depth 6.73 2021-02-12T23:27:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:36Z- handleDvl() - RDI_Pathfinder msg: 1613172456.07 epoch sec; seqNo:1876 2021-02-12T23:27:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.03 , -0.19 2021-02-12T23:27:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172456.09 epoch sec; seqNo:3941; 4057034.02 north; 590990.69 east 2021-02-12T23:27:36Z- AHRS_M2 msg: 1613172456.45 epoch sec; seqNo:3902 2021-02-12T23:27:36Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.17 psi 2021-02-12T23:27:36Z- handleDepth()-Depth_Keller msg: 1613172456465 epoch sec; seqNo:3942; depth 6.60 2021-02-12T23:27:36Z- handleDvl() - RDI_Pathfinder msg: 1613172456.48 epoch sec; seqNo:1877 2021-02-12T23:27:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.10 , 106.56 , 122.78 , 76.00 2021-02-12T23:27:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.25 , -0.09 2021-02-12T23:27:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172456.49 epoch sec; seqNo:3942; 4057034.40 north; 590990.68 east 2021-02-12T23:27:36Z- AHRS_M2 msg: 1613172456.86 epoch sec; seqNo:3903 2021-02-12T23:27:36Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.17 psi 2021-02-12T23:27:36Z- handleDepth()-Depth_Keller msg: 1613172456873 epoch sec; seqNo:3943; depth 6.46 2021-02-12T23:27:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172456.88 epoch sec; seqNo:3943; 4057034.41 north; 590991.90 east 2021-02-12T23:27:37Z- AHRS_M2 msg: 1613172457.27 epoch sec; seqNo:3904 2021-02-12T23:27:37Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.17 psi 2021-02-12T23:27:37Z- handleDepth()-Depth_Keller msg: 1613172457270 epoch sec; seqNo:3944; depth 6.33 2021-02-12T23:27:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:37Z- handleDvl() - RDI_Pathfinder msg: 1613172457.27 epoch sec; seqNo:1878 2021-02-12T23:27:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.25 , -0.09 2021-02-12T23:27:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172457.28 epoch sec; seqNo:3944; 4057034.79 north; 590991.90 east 2021-02-12T23:27:37Z- AHRS_M2 msg: 1613172457.67 epoch sec; seqNo:3905 2021-02-12T23:27:37Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.16 psi 2021-02-12T23:27:37Z- handleDepth()-Depth_Keller msg: 1613172457672 epoch sec; seqNo:3945; depth 6.20 2021-02-12T23:27:37Z- handleDvl() - RDI_Pathfinder msg: 1613172457.69 epoch sec; seqNo:1879 2021-02-12T23:27:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.96 , 106.43 , 124.00 , 76.82 2021-02-12T23:27:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.29 , 0.18 2021-02-12T23:27:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172457.70 epoch sec; seqNo:3945; 4057035.17 north; 590991.89 east 2021-02-12T23:27:38Z- AHRS_M2 msg: 1613172458.08 epoch sec; seqNo:3906 2021-02-12T23:27:38Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.16 psi 2021-02-12T23:27:38Z- handleDepth()-Depth_Keller msg: 1613172458079 epoch sec; seqNo:3946; depth 6.06 2021-02-12T23:27:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172458.09 epoch sec; seqNo:3946; 4057035.55 north; 590991.89 east 2021-02-12T23:27:38Z- AHRS_M2 msg: 1613172458.48 epoch sec; seqNo:3907 2021-02-12T23:27:38Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.15 psi 2021-02-12T23:27:38Z- handleDepth()-Depth_Keller msg: 1613172458480 epoch sec; seqNo:3947; depth 5.93 2021-02-12T23:27:38Z- handleDvl() - RDI_Pathfinder msg: 1613172458.49 epoch sec; seqNo:1880 2021-02-12T23:27:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.56 , 106.43 , 124.00 , 79.27 2021-02-12T23:27:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , 0.29 2021-02-12T23:27:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172458.50 epoch sec; seqNo:3947; 4057035.93 north; 590991.89 east 2021-02-12T23:27:38Z- AHRS_M2 msg: 1613172458.88 epoch sec; seqNo:3908 2021-02-12T23:27:38Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.15 psi 2021-02-12T23:27:38Z- handleDepth()-Depth_Keller msg: 1613172458886 epoch sec; seqNo:3948; depth 5.79 2021-02-12T23:27:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172458.90 epoch sec; seqNo:3948; 4057036.31 north; 590991.88 east 2021-02-12T23:27:39Z- AHRS_M2 msg: 1613172459.28 epoch sec; seqNo:3909 2021-02-12T23:27:39Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.15 psi 2021-02-12T23:27:39Z- handleDepth()-Depth_Keller msg: 1613172459288 epoch sec; seqNo:3949; depth 5.65 2021-02-12T23:27:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172459.31 epoch sec; seqNo:3949; 4057036.31 north; 590991.88 east 2021-02-12T23:27:39Z- AHRS_M2 msg: 1613172459.69 epoch sec; seqNo:3910 2021-02-12T23:27:39Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.14 psi 2021-02-12T23:27:39Z- handleDepth()-Depth_Keller msg: 1613172459692 epoch sec; seqNo:3950; depth 5.52 2021-02-12T23:27:39Z- handleDvl() - RDI_Pathfinder msg: 1613172459.73 epoch sec; seqNo:1881 2021-02-12T23:27:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.91 , 105.07 , 125.35 , 80.74 2021-02-12T23:27:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.10 , 0.23 2021-02-12T23:27:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172459.75 epoch sec; seqNo:3950; 4057036.68 north; 590991.88 east 2021-02-12T23:27:40Z- AHRS_M2 msg: 1613172460.10 epoch sec; seqNo:3911 2021-02-12T23:27:40Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.13 psi 2021-02-12T23:27:40Z- handleDepth()-Depth_Keller msg: 1613172460099 epoch sec; seqNo:3951; depth 5.38 2021-02-12T23:27:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172460.11 epoch sec; seqNo:3951; 4057037.08 north; 590993.10 east 2021-02-12T23:27:40Z- AHRS_M2 msg: 1613172460.50 epoch sec; seqNo:3912 2021-02-12T23:27:40Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.12 psi 2021-02-12T23:27:40Z- handleDepth()-Depth_Keller msg: 1613172460500 epoch sec; seqNo:3952; depth 5.25 2021-02-12T23:27:40Z- handleDvl() - RDI_Pathfinder msg: 1613172460.51 epoch sec; seqNo:1882 2021-02-12T23:27:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.26 , 105.07 , 124.00 , 77.64 2021-02-12T23:27:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.14 , 0.07 2021-02-12T23:27:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172460.53 epoch sec; seqNo:3952; 4057037.08 north; 590993.10 east 2021-02-12T23:27:40Z- LcmTrn::handleMessages() - handling 43 messages took 0.69 ms and 129 calls... 2021-02-12T23:27:40Z- waiting for 1613172458.67, time is 1613172460.53 2021-02-12T23:27:40Z- LcmTrn::updateTrn() equating pose 1613172460.53 and meas times 1613172460.53 2021-02-12T23:27:40Z- LcmTrn::updateTrn() >>>> pose time 1613172460.53, meas time 1613172460.53 <<<< 2021-02-12T23:27:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172460.53 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:27:40Z- TerrainNav::motionUpdate() - currEstimate = (4057037, 590993.10, 5.25) 2021-02-12T23:27:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:27:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:27:40Z- TNavBF::Projecting Measurements 2021-02-12T23:27:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:27:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:27:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:27:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:27:40Z- TNavFilter::4 good beams. 2021-02-12T23:27:40Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:27:40Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:27:40Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:27:40Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:27:40Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:27:40Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:27:40Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:27:40Z- TNavBF::mmse selected from filter -1 2021-02-12T23:27:40Z- TerrainMapOctree: (vehN, vehE ) = (4057037.08, 590993.10). 2021-02-12T23:27:40Z- TerrainMapOctree: Min Distance = 5325.05. 2021-02-12T23:27:40Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:27:40Z- There is no specific good beam that all particles have in common. 2021-02-12T23:27:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:27:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.53 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 14.22 2021-02-12T23:27:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:27:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 17.67 2021-02-12T23:27:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:27:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 24.60 2021-02-12T23:27:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:27:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 9.64 2021-02-12T23:27:40Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:27:40Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:27:40Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:27:40Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:27:40Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:27:40Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:27:40Z- Filter 0 No beams used in subcloud update 2021-02-12T23:27:40Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:27:40Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:27:40Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 763.6 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:27:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172460.53 sec. 2021-02-12T23:27:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:27:40Z- TNavBF::mmse selected from filter -1 2021-02-12T23:27:40Z- variances([0], [2], and psi_berg) = 763.568072 833.046847 0.000000 2021-02-12T23:27:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:27:40Z- LcmTrn::publishEstimates() - MLE : 4057019.58 590945.60 5.25 2021-02-12T23:27:40Z- LcmTrn::publishEstimates() - MMSE : 4057037.08 590993.10 5.25 2021-02-12T23:27:40Z- LcmTrn::publishEstimates() - COVAR: 763.57 833.05 1.12 0.00 2021-02-12T23:27:40Z- AHRS_M2 msg: 1613172460.90 epoch sec; seqNo:3913 2021-02-12T23:27:40Z- AHRS_M2 msg: 0.63 phi; 0.28 theta; -0.13 psi 2021-02-12T23:27:40Z- handleDepth()-Depth_Keller msg: 1613172460904 epoch sec; seqNo:3953; depth 5.10 2021-02-12T23:27:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:40Z- handleDvl() - RDI_Pathfinder msg: 1613172460.91 epoch sec; seqNo:1883 2021-02-12T23:27:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.14 , 0.07 2021-02-12T23:27:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172460.92 epoch sec; seqNo:3953; 4057037.46 north; 590993.09 east 2021-02-12T23:27:41Z- AHRS_M2 msg: 1613172461.31 epoch sec; seqNo:3914 2021-02-12T23:27:41Z- AHRS_M2 msg: 0.63 phi; 0.24 theta; -0.15 psi 2021-02-12T23:27:41Z- handleDepth()-Depth_Keller msg: 1613172461310 epoch sec; seqNo:3954; depth 4.97 2021-02-12T23:27:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172461.32 epoch sec; seqNo:3954; 4057037.83 north; 590993.09 east 2021-02-12T23:27:41Z- handleDepth()-Depth_Keller msg: 1613172461729 epoch sec; seqNo:3955; depth 4.85 2021-02-12T23:27:41Z- handleDvl() - RDI_Pathfinder msg: 1613172461.74 epoch sec; seqNo:1884 2021-02-12T23:27:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.27 , 102.44 , 113.25 , 82.16 2021-02-12T23:27:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.17 , -0.04 2021-02-12T23:27:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172461.75 epoch sec; seqNo:3955; 4057037.83 north; 590993.09 east 2021-02-12T23:27:42Z- AHRS_M2 msg: 1613172462.11 epoch sec; seqNo:3915 2021-02-12T23:27:42Z- AHRS_M2 msg: 0.65 phi; 0.14 theta; -0.17 psi 2021-02-12T23:27:42Z- handleDepth()-Depth_Keller msg: 1613172462126 epoch sec; seqNo:3956; depth 4.73 2021-02-12T23:27:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:42Z- handleDvl() - RDI_Pathfinder msg: 1613172462.13 epoch sec; seqNo:1885 2021-02-12T23:27:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.17 , -0.04 2021-02-12T23:27:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172462.17 epoch sec; seqNo:3956; 4057038.23 north; 590994.30 east 2021-02-12T23:27:42Z- AHRS_M2 msg: 1613172462.52 epoch sec; seqNo:3916 2021-02-12T23:27:42Z- AHRS_M2 msg: 0.67 phi; 0.10 theta; -0.17 psi 2021-02-12T23:27:42Z- handleDepth()-Depth_Keller msg: 1613172462520 epoch sec; seqNo:3957; depth 4.62 2021-02-12T23:27:42Z- handleDvl() - RDI_Pathfinder msg: 1613172462.53 epoch sec; seqNo:1886 2021-02-12T23:27:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 102.71 , 105.41 , 85.14 2021-02-12T23:27:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.16 , -0.10 2021-02-12T23:27:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172462.54 epoch sec; seqNo:3957; 4057038.60 north; 590994.30 east 2021-02-12T23:27:42Z- AHRS_M2 msg: 1613172462.92 epoch sec; seqNo:3917 2021-02-12T23:27:42Z- AHRS_M2 msg: 0.69 phi; 0.05 theta; -0.17 psi 2021-02-12T23:27:42Z- handleDepth()-Depth_Keller msg: 1613172462926 epoch sec; seqNo:3958; depth 4.55 2021-02-12T23:27:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172462.94 epoch sec; seqNo:3958; 4057038.60 north; 590994.30 east 2021-02-12T23:27:43Z- AHRS_M2 msg: 1613172463.33 epoch sec; seqNo:3918 2021-02-12T23:27:43Z- AHRS_M2 msg: 0.71 phi; 0.02 theta; -0.16 psi 2021-02-12T23:27:43Z- handleDepth()-Depth_Keller msg: 1613172463353 epoch sec; seqNo:3959; depth 4.49 2021-02-12T23:27:43Z- handleDvl() - RDI_Pathfinder msg: 1613172463.38 epoch sec; seqNo:1887 2021-02-12T23:27:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.62 , 101.49 , 94.73 , 89.33 2021-02-12T23:27:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.07 , -0.16 2021-02-12T23:27:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172463.39 epoch sec; seqNo:3959; 4057038.98 north; 590994.30 east 2021-02-12T23:27:43Z- AHRS_M2 msg: 1613172463.73 epoch sec; seqNo:3919 2021-02-12T23:27:43Z- AHRS_M2 msg: 0.72 phi; -0.02 theta; -0.16 psi 2021-02-12T23:27:43Z- handleDepth()-Depth_Keller msg: 1613172463741 epoch sec; seqNo:3960; depth 4.43 2021-02-12T23:27:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172463.76 epoch sec; seqNo:3960; 4057038.98 north; 590994.30 east 2021-02-12T23:27:44Z- AHRS_M2 msg: 1613172464.14 epoch sec; seqNo:3920 2021-02-12T23:27:44Z- AHRS_M2 msg: 0.74 phi; -0.07 theta; -0.16 psi 2021-02-12T23:27:44Z- handleDepth()-Depth_Keller msg: 1613172464141 epoch sec; seqNo:3961; depth 4.41 2021-02-12T23:27:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172464.15 epoch sec; seqNo:3961; 4057039.38 north; 590995.51 east 2021-02-12T23:27:44Z- AHRS_M2 msg: 1613172464.54 epoch sec; seqNo:3921 2021-02-12T23:27:44Z- AHRS_M2 msg: 0.75 phi; -0.11 theta; -0.16 psi 2021-02-12T23:27:44Z- handleDepth()-Depth_Keller msg: 1613172464543 epoch sec; seqNo:3962; depth 4.39 2021-02-12T23:27:44Z- handleDvl() - RDI_Pathfinder msg: 1613172464.56 epoch sec; seqNo:1888 2021-02-12T23:27:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 96.29 , 90.88 , 94.94 2021-02-12T23:27:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.10 , -0.15 2021-02-12T23:27:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172464.57 epoch sec; seqNo:3962; 4057039.75 north; 590995.51 east 2021-02-12T23:27:44Z- AHRS_M2 msg: 1613172464.94 epoch sec; seqNo:3922 2021-02-12T23:27:44Z- AHRS_M2 msg: 0.75 phi; -0.15 theta; -0.16 psi 2021-02-12T23:27:44Z- handleDepth()-Depth_Keller msg: 1613172464944 epoch sec; seqNo:3963; depth 4.39 2021-02-12T23:27:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172464.96 epoch sec; seqNo:3963; 4057039.75 north; 590995.51 east 2021-02-12T23:27:45Z- AHRS_M2 msg: 1613172465.34 epoch sec; seqNo:3923 2021-02-12T23:27:45Z- AHRS_M2 msg: 0.74 phi; -0.19 theta; -0.18 psi 2021-02-12T23:27:45Z- handleDepth()-Depth_Keller msg: 1613172465353 epoch sec; seqNo:3964; depth 4.41 2021-02-12T23:27:45Z- handleDvl() - RDI_Pathfinder msg: 1613172465.36 epoch sec; seqNo:1889 2021-02-12T23:27:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.89 , 101.76 , 0.00 , 104.47 2021-02-12T23:27:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.62 , -0.14 , -0.14 2021-02-12T23:27:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172465.39 epoch sec; seqNo:3964; 4057040.13 north; 590995.50 east 2021-02-12T23:27:45Z- AHRS_M2 msg: 1613172465.75 epoch sec; seqNo:3924 2021-02-12T23:27:45Z- AHRS_M2 msg: 0.73 phi; -0.21 theta; -0.20 psi 2021-02-12T23:27:45Z- handleDepth()-Depth_Keller msg: 1613172465758 epoch sec; seqNo:3965; depth 4.42 2021-02-12T23:27:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:45Z- handleDvl() - RDI_Pathfinder msg: 1613172465.76 epoch sec; seqNo:1890 2021-02-12T23:27:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.62 , -0.14 , -0.14 2021-02-12T23:27:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172465.78 epoch sec; seqNo:3965; 4057040.51 north; 590995.50 east 2021-02-12T23:27:45Z- LcmTrn::handleMessages() - handling 46 messages took 0.71 ms and 132 calls... 2021-02-12T23:27:45Z- waiting for 1613172463.53, time is 1613172465.78 2021-02-12T23:27:45Z- LcmTrn::updateTrn() equating pose 1613172465.78 and meas times 1613172465.78 2021-02-12T23:27:45Z- LcmTrn::updateTrn() >>>> pose time 1613172465.78, meas time 1613172465.78 <<<< 2021-02-12T23:27:45Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172465.78, ping # 0. 2021-02-12T23:27:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172465.78 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:27:45Z- TerrainNav::motionUpdate() - currEstimate = (4057041, 590995.50, 4.42) 2021-02-12T23:27:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:27:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:27:45Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:27:45Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:27:45Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172465.78 sec.; No relevant navigation data available 2021-02-12T23:27:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:27:45Z- TNavBF::mmse selected from filter -1 2021-02-12T23:27:45Z- variances([0], [2], and psi_berg) = 763.447653 833.046713 0.000000 2021-02-12T23:27:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:27:45Z- LcmTrn::publishEstimates() - MLE : 4057023.01 590948.00 4.42 2021-02-12T23:27:45Z- LcmTrn::publishEstimates() - MMSE : 4057040.51 590995.50 4.42 2021-02-12T23:27:45Z- LcmTrn::publishEstimates() - COVAR: 763.45 833.05 1.33 0.00 2021-02-12T23:27:46Z- AHRS_M2 msg: 1613172466.17 epoch sec; seqNo:3925 2021-02-12T23:27:46Z- AHRS_M2 msg: 0.72 phi; -0.25 theta; -0.25 psi 2021-02-12T23:27:46Z- handleDepth()-Depth_Keller msg: 1613172466172 epoch sec; seqNo:3966; depth 4.46 2021-02-12T23:27:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172466.18 epoch sec; seqNo:3966; 4057040.51 north; 590995.50 east 2021-02-12T23:27:46Z- AHRS_M2 msg: 1613172466.56 epoch sec; seqNo:3926 2021-02-12T23:27:46Z- AHRS_M2 msg: 0.71 phi; -0.27 theta; -0.28 psi 2021-02-12T23:27:46Z- handleDepth()-Depth_Keller msg: 1613172466565 epoch sec; seqNo:3967; depth 4.51 2021-02-12T23:27:46Z- handleDvl() - RDI_Pathfinder msg: 1613172466.58 epoch sec; seqNo:1891 2021-02-12T23:27:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.07 , 106.70 , 83.72 , 110.75 2021-02-12T23:27:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.62 , -0.13 , -0.06 2021-02-12T23:27:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172466.59 epoch sec; seqNo:3967; 4057040.89 north; 590995.50 east 2021-02-12T23:27:46Z- AHRS_M2 msg: 1613172466.96 epoch sec; seqNo:3927 2021-02-12T23:27:46Z- AHRS_M2 msg: 0.71 phi; -0.28 theta; -0.30 psi 2021-02-12T23:27:46Z- handleDepth()-Depth_Keller msg: 1613172466969 epoch sec; seqNo:3968; depth 4.58 2021-02-12T23:27:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:46Z- handleDvl() - RDI_Pathfinder msg: 1613172466.97 epoch sec; seqNo:1892 2021-02-12T23:27:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.62 , -0.13 , -0.06 2021-02-12T23:27:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172466.99 epoch sec; seqNo:3968; 4057040.89 north; 590995.50 east 2021-02-12T23:27:47Z- AHRS_M2 msg: 1613172467.36 epoch sec; seqNo:3928 2021-02-12T23:27:47Z- AHRS_M2 msg: 0.70 phi; -0.29 theta; -0.30 psi 2021-02-12T23:27:47Z- handleDepth()-Depth_Keller msg: 1613172467368 epoch sec; seqNo:3969; depth 4.63 2021-02-12T23:27:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172467.38 epoch sec; seqNo:3969; 4057041.28 north; 590996.71 east 2021-02-12T23:27:47Z- AHRS_M2 msg: 1613172467.77 epoch sec; seqNo:3929 2021-02-12T23:27:47Z- AHRS_M2 msg: 0.68 phi; -0.28 theta; -0.29 psi 2021-02-12T23:27:47Z- handleDepth()-Depth_Keller msg: 1613172467778 epoch sec; seqNo:3970; depth 4.70 2021-02-12T23:27:47Z- handleDvl() - RDI_Pathfinder msg: 1613172467.79 epoch sec; seqNo:1893 2021-02-12T23:27:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 114.88 , 83.79 , 116.23 2021-02-12T23:27:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.06 , 0.13 2021-02-12T23:27:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172467.80 epoch sec; seqNo:3970; 4057041.66 north; 590996.71 east 2021-02-12T23:27:48Z- AHRS_M2 msg: 1613172468.17 epoch sec; seqNo:3930 2021-02-12T23:27:48Z- AHRS_M2 msg: 0.68 phi; -0.28 theta; -0.27 psi 2021-02-12T23:27:48Z- handleDepth()-Depth_Keller msg: 1613172468179 epoch sec; seqNo:3971; depth 4.79 2021-02-12T23:27:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:48Z- handleDvl() - RDI_Pathfinder msg: 1613172468.18 epoch sec; seqNo:1894 2021-02-12T23:27:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.06 , 0.13 2021-02-12T23:27:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172468.20 epoch sec; seqNo:3971; 4057041.66 north; 590996.71 east 2021-02-12T23:27:48Z- AHRS_M2 msg: 1613172468.58 epoch sec; seqNo:3931 2021-02-12T23:27:48Z- AHRS_M2 msg: 0.68 phi; -0.28 theta; -0.25 psi 2021-02-12T23:27:48Z- handleDepth()-Depth_Keller msg: 1613172468585 epoch sec; seqNo:3972; depth 4.89 2021-02-12T23:27:48Z- handleDvl() - RDI_Pathfinder msg: 1613172468.60 epoch sec; seqNo:1895 2021-02-12T23:27:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 117.31 , 83.51 , 118.66 2021-02-12T23:27:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.08 , 0.29 2021-02-12T23:27:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172468.62 epoch sec; seqNo:3972; 4057042.04 north; 590996.71 east 2021-02-12T23:27:48Z- AHRS_M2 msg: 1613172468.98 epoch sec; seqNo:3932 2021-02-12T23:27:48Z- AHRS_M2 msg: 0.68 phi; -0.28 theta; -0.23 psi 2021-02-12T23:27:48Z- handleDepth()-Depth_Keller msg: 1613172468984 epoch sec; seqNo:3973; depth 4.96 2021-02-12T23:27:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172468.99 epoch sec; seqNo:3973; 4057042.04 north; 590996.71 east 2021-02-12T23:27:49Z- AHRS_M2 msg: 1613172469.40 epoch sec; seqNo:3933 2021-02-12T23:27:49Z- AHRS_M2 msg: 0.68 phi; -0.28 theta; -0.22 psi 2021-02-12T23:27:49Z- handleDepth()-Depth_Keller msg: 1613172469409 epoch sec; seqNo:3974; depth 5.04 2021-02-12T23:27:49Z- handleDvl() - RDI_Pathfinder msg: 1613172469.42 epoch sec; seqNo:1896 2021-02-12T23:27:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.08 , 111.36 , 84.60 , 115.82 2021-02-12T23:27:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.21 , 0.13 2021-02-12T23:27:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172469.43 epoch sec; seqNo:3974; 4057042.42 north; 590996.70 east 2021-02-12T23:27:49Z- AHRS_M2 msg: 1613172469.79 epoch sec; seqNo:3934 2021-02-12T23:27:49Z- AHRS_M2 msg: 0.69 phi; -0.29 theta; -0.21 psi 2021-02-12T23:27:49Z- handleDepth()-Depth_Keller msg: 1613172469792 epoch sec; seqNo:3975; depth 5.17 2021-02-12T23:27:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172469.82 epoch sec; seqNo:3975; 4057042.43 north; 590997.92 east 2021-02-12T23:27:50Z- AHRS_M2 msg: 1613172470.19 epoch sec; seqNo:3935 2021-02-12T23:27:50Z- AHRS_M2 msg: 0.69 phi; -0.30 theta; -0.21 psi 2021-02-12T23:27:50Z- handleDepth()-Depth_Keller msg: 1613172470196 epoch sec; seqNo:3976; depth 5.27 2021-02-12T23:27:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172470.20 epoch sec; seqNo:3976; 4057042.81 north; 590997.92 east 2021-02-12T23:27:50Z- AHRS_M2 msg: 1613172470.60 epoch sec; seqNo:3936 2021-02-12T23:27:50Z- AHRS_M2 msg: 0.68 phi; -0.31 theta; -0.22 psi 2021-02-12T23:27:50Z- handleDepth()-Depth_Keller msg: 1613172470600 epoch sec; seqNo:3977; depth 5.35 2021-02-12T23:27:50Z- handleDvl() - RDI_Pathfinder msg: 1613172470.61 epoch sec; seqNo:1897 2021-02-12T23:27:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 107.91 , 82.23 , 116.03 2021-02-12T23:27:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.09 , -0.22 2021-02-12T23:27:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172470.62 epoch sec; seqNo:3977; 4057042.81 north; 590997.92 east 2021-02-12T23:27:51Z- AHRS_M2 msg: 1613172471.00 epoch sec; seqNo:3937 2021-02-12T23:27:51Z- AHRS_M2 msg: 0.68 phi; -0.31 theta; -0.24 psi 2021-02-12T23:27:51Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-12T23:27:51Z- waiting for 1613172468.78, time is 1613172471.00 2021-02-12T23:27:51Z- LcmTrn::updateTrn() >>>> pose time 1613172471.00, meas time 1613172470.61 <<<< 2021-02-12T23:27:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172470.61 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:27:51Z- TerrainNav::motionUpdate() - currEstimate = (4057043, 590997.92, 5.35) 2021-02-12T23:27:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:27:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:27:51Z- TNavBF::Projecting Measurements 2021-02-12T23:27:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:27:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:27:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:27:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:27:51Z- TNavFilter::4 good beams. 2021-02-12T23:27:51Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:27:51Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:27:51Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:27:51Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:27:51Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:27:51Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:27:51Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:27:51Z- TNavBF::mmse selected from filter -1 2021-02-12T23:27:51Z- TerrainMapOctree: (vehN, vehE ) = (4057042.64, 590997.74). 2021-02-12T23:27:51Z- TerrainMapOctree: Min Distance = 5317.81. 2021-02-12T23:27:51Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:27:51Z- There is no specific good beam that all particles have in common. 2021-02-12T23:27:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:27:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.49 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 12.29 2021-02-12T23:27:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:27:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.35 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 18.63 2021-02-12T23:27:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:27:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 10.82 2021-02-12T23:27:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:27:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 21.54 2021-02-12T23:27:51Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:27:51Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:27:51Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:27:51Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:27:51Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:27:51Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:27:51Z- Filter 0 No beams used in subcloud update 2021-02-12T23:27:51Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:27:51Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:27:51Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 763.3 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:27:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172470.61 sec. 2021-02-12T23:27:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:27:51Z- TNavBF::mmse selected from filter -1 2021-02-12T23:27:51Z- variances([0], [2], and psi_berg) = 763.315305 833.046566 0.000000 2021-02-12T23:27:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:27:51Z- LcmTrn::publishEstimates() - MLE : 4057025.31 590950.42 5.35 2021-02-12T23:27:51Z- LcmTrn::publishEstimates() - MMSE : 4057042.81 590997.92 5.35 2021-02-12T23:27:51Z- LcmTrn::publishEstimates() - COVAR: 763.32 833.05 1.11 0.00 2021-02-12T23:27:51Z- handleDepth()-Depth_Keller msg: 1613172471006 epoch sec; seqNo:3978; depth 5.43 2021-02-12T23:27:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172471.02 epoch sec; seqNo:3978; 4057043.19 north; 590997.91 east 2021-02-12T23:27:51Z- AHRS_M2 msg: 1613172471.43 epoch sec; seqNo:3938 2021-02-12T23:27:51Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.26 psi 2021-02-12T23:27:51Z- handleDepth()-Depth_Keller msg: 1613172471435 epoch sec; seqNo:3979; depth 5.53 2021-02-12T23:27:51Z- handleDvl() - RDI_Pathfinder msg: 1613172471.45 epoch sec; seqNo:1898 2021-02-12T23:27:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.71 , 110.68 , 82.30 , 118.80 2021-02-12T23:27:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.21 , -0.21 2021-02-12T23:27:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172471.46 epoch sec; seqNo:3979; 4057043.57 north; 590997.91 east 2021-02-12T23:27:51Z- AHRS_M2 msg: 1613172471.81 epoch sec; seqNo:3939 2021-02-12T23:27:51Z- AHRS_M2 msg: 0.66 phi; -0.31 theta; -0.25 psi 2021-02-12T23:27:51Z- handleDepth()-Depth_Keller msg: 1613172471812 epoch sec; seqNo:3980; depth 5.62 2021-02-12T23:27:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:51Z- handleDvl() - RDI_Pathfinder msg: 1613172471.81 epoch sec; seqNo:1899 2021-02-12T23:27:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.21 , -0.21 2021-02-12T23:27:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172471.83 epoch sec; seqNo:3980; 4057043.95 north; 590997.91 east 2021-02-12T23:27:52Z- AHRS_M2 msg: 1613172472.21 epoch sec; seqNo:3940 2021-02-12T23:27:52Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.24 psi 2021-02-12T23:27:52Z- handleDepth()-Depth_Keller msg: 1613172472216 epoch sec; seqNo:3981; depth 5.72 2021-02-12T23:27:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172472.23 epoch sec; seqNo:3981; 4057043.95 north; 590997.91 east 2021-02-12T23:27:52Z- AHRS_M2 msg: 1613172472.62 epoch sec; seqNo:3941 2021-02-12T23:27:52Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.23 psi 2021-02-12T23:27:52Z- handleDepth()-Depth_Keller msg: 1613172472625 epoch sec; seqNo:3982; depth 5.82 2021-02-12T23:27:52Z- handleDvl() - RDI_Pathfinder msg: 1613172472.64 epoch sec; seqNo:1900 2021-02-12T23:27:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 111.90 , 82.16 , 117.31 2021-02-12T23:27:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.20 , 0.12 2021-02-12T23:27:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172472.65 epoch sec; seqNo:3982; 4057044.34 north; 590999.12 east 2021-02-12T23:27:53Z- AHRS_M2 msg: 1613172473.02 epoch sec; seqNo:3942 2021-02-12T23:27:53Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.21 psi 2021-02-12T23:27:53Z- handleDepth()-Depth_Keller msg: 1613172473024 epoch sec; seqNo:3983; depth 5.93 2021-02-12T23:27:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:53Z- handleDvl() - RDI_Pathfinder msg: 1613172473.03 epoch sec; seqNo:1901 2021-02-12T23:27:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.20 , 0.12 2021-02-12T23:27:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172473.04 epoch sec; seqNo:3983; 4057044.72 north; 590999.12 east 2021-02-12T23:27:53Z- AHRS_M2 msg: 1613172473.44 epoch sec; seqNo:3943 2021-02-12T23:27:53Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.21 psi 2021-02-12T23:27:53Z- handleDepth()-Depth_Keller msg: 1613172473445 epoch sec; seqNo:3984; depth 6.04 2021-02-12T23:27:53Z- handleDvl() - RDI_Pathfinder msg: 1613172473.46 epoch sec; seqNo:1902 2021-02-12T23:27:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 109.27 , 82.23 , 117.38 2021-02-12T23:27:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.03 , 0.20 2021-02-12T23:27:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172473.47 epoch sec; seqNo:3984; 4057045.10 north; 590999.12 east 2021-02-12T23:27:53Z- AHRS_M2 msg: 1613172473.83 epoch sec; seqNo:3944 2021-02-12T23:27:53Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.22 psi 2021-02-12T23:27:53Z- handleDepth()-Depth_Keller msg: 1613172473833 epoch sec; seqNo:3985; depth 6.14 2021-02-12T23:27:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172473.84 epoch sec; seqNo:3985; 4057045.10 north; 590999.12 east 2021-02-12T23:27:54Z- AHRS_M2 msg: 1613172474.24 epoch sec; seqNo:3945 2021-02-12T23:27:54Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.24 psi 2021-02-12T23:27:54Z- handleDepth()-Depth_Keller msg: 1613172474243 epoch sec; seqNo:3986; depth 6.24 2021-02-12T23:27:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:54Z- handleDvl() - RDI_Pathfinder msg: 1613172474.25 epoch sec; seqNo:1903 2021-02-12T23:27:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.03 , 0.20 2021-02-12T23:27:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172474.25 epoch sec; seqNo:3986; 4057045.48 north; 590999.11 east 2021-02-12T23:27:54Z- AHRS_M2 msg: 1613172474.64 epoch sec; seqNo:3946 2021-02-12T23:27:54Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.25 psi 2021-02-12T23:27:54Z- handleDepth()-Depth_Keller msg: 1613172474640 epoch sec; seqNo:3987; depth 6.34 2021-02-12T23:27:54Z- handleDvl() - RDI_Pathfinder msg: 1613172474.66 epoch sec; seqNo:1904 2021-02-12T23:27:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 109.27 , 82.23 , 118.73 2021-02-12T23:27:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.16 , 0.07 2021-02-12T23:27:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172474.67 epoch sec; seqNo:3987; 4057045.48 north; 590999.11 east 2021-02-12T23:27:55Z- AHRS_M2 msg: 1613172475.04 epoch sec; seqNo:3947 2021-02-12T23:27:55Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.26 psi 2021-02-12T23:27:55Z- handleDepth()-Depth_Keller msg: 1613172475044 epoch sec; seqNo:3988; depth 6.47 2021-02-12T23:27:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172475.06 epoch sec; seqNo:3988; 4057045.87 north; 591000.33 east 2021-02-12T23:27:55Z- AHRS_M2 msg: 1613172475.45 epoch sec; seqNo:3948 2021-02-12T23:27:55Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.25 psi 2021-02-12T23:27:55Z- handleDepth()-Depth_Keller msg: 1613172475453 epoch sec; seqNo:3989; depth 6.58 2021-02-12T23:27:55Z- handleDvl() - RDI_Pathfinder msg: 1613172475.46 epoch sec; seqNo:1905 2021-02-12T23:27:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.29 , 111.97 , 80.88 , 121.43 2021-02-12T23:27:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.11 , -0.02 2021-02-12T23:27:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172475.47 epoch sec; seqNo:3989; 4057045.87 north; 591000.33 east 2021-02-12T23:27:55Z- AHRS_M2 msg: 1613172475.85 epoch sec; seqNo:3949 2021-02-12T23:27:55Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.25 psi 2021-02-12T23:27:55Z- handleDepth()-Depth_Keller msg: 1613172475854 epoch sec; seqNo:3990; depth 6.68 2021-02-12T23:27:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172475.87 epoch sec; seqNo:3990; 4057046.25 north; 591000.32 east 2021-02-12T23:27:56Z- LcmTrn::handleMessages() - handling 46 messages took 0.58 ms and 135 calls... 2021-02-12T23:27:56Z- waiting for 1613172474.00, time is 1613172475.87 2021-02-12T23:27:56Z- LcmTrn::updateTrn() >>>> pose time 1613172475.87, meas time 1613172475.46 <<<< 2021-02-12T23:27:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172475.46 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:27:56Z- TerrainNav::motionUpdate() - currEstimate = (4057046, 591000.32, 6.68) 2021-02-12T23:27:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:27:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:27:56Z- TNavBF::Projecting Measurements 2021-02-12T23:27:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:27:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:27:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:27:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:27:56Z- TNavFilter::4 good beams. 2021-02-12T23:27:56Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:27:56Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:27:56Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:27:56Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:27:56Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:27:56Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:27:56Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:27:56Z- TNavBF::mmse selected from filter -1 2021-02-12T23:27:56Z- TerrainMapOctree: (vehN, vehE ) = (4057045.96, 591000.13). 2021-02-12T23:27:56Z- TerrainMapOctree: Min Distance = 5313.72. 2021-02-12T23:27:56Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:27:56Z- There is no specific good beam that all particles have in common. 2021-02-12T23:27:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:27:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 11.91 2021-02-12T23:27:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:27:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 20.06 2021-02-12T23:27:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:27:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 10.47 2021-02-12T23:27:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:27:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 23.59 2021-02-12T23:27:56Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:27:56Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:27:56Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:27:56Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:27:56Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:27:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:27:56Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:27:56Z- Filter 0 No beams used in subcloud update 2021-02-12T23:27:56Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:27:56Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:27:56Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 763.2 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:27:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172475.46 sec. 2021-02-12T23:27:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:27:56Z- TNavBF::mmse selected from filter -1 2021-02-12T23:27:56Z- variances([0], [2], and psi_berg) = 763.165999 833.046400 0.000000 2021-02-12T23:27:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:27:56Z- LcmTrn::publishEstimates() - MLE : 4057028.75 590952.82 6.68 2021-02-12T23:27:56Z- LcmTrn::publishEstimates() - MMSE : 4057046.25 591000.32 6.68 2021-02-12T23:27:56Z- LcmTrn::publishEstimates() - COVAR: 763.17 833.05 1.01 0.00 2021-02-12T23:27:56Z- AHRS_M2 msg: 1613172476.25 epoch sec; seqNo:3950 2021-02-12T23:27:56Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.24 psi 2021-02-12T23:27:56Z- handleDepth()-Depth_Keller msg: 1613172476256 epoch sec; seqNo:3991; depth 6.79 2021-02-12T23:27:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172476.29 epoch sec; seqNo:3991; 4057046.63 north; 591000.32 east 2021-02-12T23:27:56Z- AHRS_M2 msg: 1613172476.65 epoch sec; seqNo:3951 2021-02-12T23:27:56Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.23 psi 2021-02-12T23:27:56Z- handleDepth()-Depth_Keller msg: 1613172476661 epoch sec; seqNo:3992; depth 6.90 2021-02-12T23:27:56Z- handleDvl() - RDI_Pathfinder msg: 1613172476.67 epoch sec; seqNo:1906 2021-02-12T23:27:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 111.90 , 80.81 , 121.36 2021-02-12T23:27:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.09 , -0.08 2021-02-12T23:27:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172476.70 epoch sec; seqNo:3992; 4057046.63 north; 591000.32 east 2021-02-12T23:27:57Z- AHRS_M2 msg: 1613172477.06 epoch sec; seqNo:3952 2021-02-12T23:27:57Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.23 psi 2021-02-12T23:27:57Z- handleDepth()-Depth_Keller msg: 1613172477064 epoch sec; seqNo:3993; depth 7.02 2021-02-12T23:27:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172477.08 epoch sec; seqNo:3993; 4057047.00 north; 591000.32 east 2021-02-12T23:27:57Z- AHRS_M2 msg: 1613172477.46 epoch sec; seqNo:3953 2021-02-12T23:27:57Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.22 psi 2021-02-12T23:27:57Z- handleDepth()-Depth_Keller msg: 1613172477477 epoch sec; seqNo:3994; depth 7.14 2021-02-12T23:27:57Z- handleDvl() - RDI_Pathfinder msg: 1613172477.49 epoch sec; seqNo:1907 2021-02-12T23:27:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 110.55 , 80.81 , 122.72 2021-02-12T23:27:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.04 , -0.15 2021-02-12T23:27:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172477.50 epoch sec; seqNo:3994; 4057047.40 north; 591001.53 east 2021-02-12T23:27:57Z- AHRS_M2 msg: 1613172477.87 epoch sec; seqNo:3954 2021-02-12T23:27:57Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.21 psi 2021-02-12T23:27:57Z- handleDepth()-Depth_Keller msg: 1613172477872 epoch sec; seqNo:3995; depth 7.26 2021-02-12T23:27:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:57Z- handleDvl() - RDI_Pathfinder msg: 1613172477.87 epoch sec; seqNo:1908 2021-02-12T23:27:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.04 , -0.15 2021-02-12T23:27:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172477.89 epoch sec; seqNo:3995; 4057047.40 north; 591001.53 east 2021-02-12T23:27:58Z- AHRS_M2 msg: 1613172478.28 epoch sec; seqNo:3955 2021-02-12T23:27:58Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.22 psi 2021-02-12T23:27:58Z- handleDepth()-Depth_Keller msg: 1613172478281 epoch sec; seqNo:3996; depth 7.38 2021-02-12T23:27:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172478.29 epoch sec; seqNo:3996; 4057047.77 north; 591001.53 east 2021-02-12T23:27:58Z- AHRS_M2 msg: 1613172478.67 epoch sec; seqNo:3956 2021-02-12T23:27:58Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.22 psi 2021-02-12T23:27:58Z- handleDepth()-Depth_Keller msg: 1613172478723 epoch sec; seqNo:3997; depth 7.50 2021-02-12T23:27:58Z- handleDvl() - RDI_Pathfinder msg: 1613172478.73 epoch sec; seqNo:1909 2021-02-12T23:27:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 109.20 , 79.27 , 120.01 2021-02-12T23:27:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.13 , -0.17 2021-02-12T23:27:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172478.79 epoch sec; seqNo:3997; 4057048.15 north; 591001.53 east 2021-02-12T23:27:59Z- AHRS_M2 msg: 1613172479.09 epoch sec; seqNo:3957 2021-02-12T23:27:59Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.23 psi 2021-02-12T23:27:59Z- handleDepth()-Depth_Keller msg: 1613172479089 epoch sec; seqNo:3998; depth 7.64 2021-02-12T23:27:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:27:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:27:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:27:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:27:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:27:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:27:59Z- handleDvl() - RDI_Pathfinder msg: 1613172479.09 epoch sec; seqNo:1910 2021-02-12T23:27:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:27:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.13 , -0.17 2021-02-12T23:27:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172479.10 epoch sec; seqNo:3998; 4057048.53 north; 591001.52 east 2021-02-12T23:27:59Z- AHRS_M2 msg: 1613172479.48 epoch sec; seqNo:3958 2021-02-12T23:27:59Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.22 psi 2021-02-12T23:27:59Z- handleDepth()-Depth_Keller msg: 1613172479488 epoch sec; seqNo:3999; depth 7.75 2021-02-12T23:27:59Z- handleDvl() - RDI_Pathfinder msg: 1613172479.50 epoch sec; seqNo:1911 2021-02-12T23:27:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.35 , 109.33 , 78.04 , 118.80 2021-02-12T23:27:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.26 , -0.03 2021-02-12T23:27:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172479.52 epoch sec; seqNo:3999; 4057048.91 north; 591001.52 east 2021-02-12T23:27:59Z- AHRS_M2 msg: 1613172479.89 epoch sec; seqNo:3959 2021-02-12T23:27:59Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.21 psi 2021-02-12T23:27:59Z- handleDepth()-Depth_Keller msg: 1613172479892 epoch sec; seqNo:4000; depth 7.87 2021-02-12T23:27:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172479.90 epoch sec; seqNo:4000; 4057049.29 north; 591001.51 east 2021-02-12T23:28:00Z- AHRS_M2 msg: 1613172480.29 epoch sec; seqNo:3960 2021-02-12T23:28:00Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.19 psi 2021-02-12T23:28:00Z- handleDepth()-Depth_Keller msg: 1613172480303 epoch sec; seqNo:4001; depth 8.01 2021-02-12T23:28:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:00Z- handleDvl() - RDI_Pathfinder msg: 1613172480.31 epoch sec; seqNo:1912 2021-02-12T23:28:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.26 , -0.03 2021-02-12T23:28:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172480.31 epoch sec; seqNo:4001; 4057049.67 north; 591001.51 east 2021-02-12T23:28:00Z- AHRS_M2 msg: 1613172480.69 epoch sec; seqNo:3961 2021-02-12T23:28:00Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.17 psi 2021-02-12T23:28:00Z- handleDepth()-Depth_Keller msg: 1613172480705 epoch sec; seqNo:4002; depth 8.13 2021-02-12T23:28:00Z- handleDvl() - RDI_Pathfinder msg: 1613172480.72 epoch sec; seqNo:1913 2021-02-12T23:28:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 106.63 , 78.04 , 114.74 2021-02-12T23:28:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.22 , 0.12 2021-02-12T23:28:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172480.73 epoch sec; seqNo:4002; 4057050.05 north; 591001.51 east 2021-02-12T23:28:01Z- AHRS_M2 msg: 1613172481.10 epoch sec; seqNo:3962 2021-02-12T23:28:01Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.15 psi 2021-02-12T23:28:01Z- handleDepth()-Depth_Keller msg: 1613172481109 epoch sec; seqNo:4003; depth 8.25 2021-02-12T23:28:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172481.12 epoch sec; seqNo:4003; 4057050.06 north; 591002.73 east 2021-02-12T23:28:01Z- LcmTrn::handleMessages() - handling 47 messages took 0.69 ms and 133 calls... 2021-02-12T23:28:01Z- waiting for 1613172478.87, time is 1613172481.12 2021-02-12T23:28:01Z- LcmTrn::updateTrn() >>>> pose time 1613172481.12, meas time 1613172480.72 <<<< 2021-02-12T23:28:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172480.72 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:28:01Z- TerrainNav::motionUpdate() - currEstimate = (4057050, 591002.73, 8.25) 2021-02-12T23:28:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:28:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:28:01Z- TNavBF::Projecting Measurements 2021-02-12T23:28:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:28:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:28:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:28:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:28:01Z- TNavFilter::4 good beams. 2021-02-12T23:28:01Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:28:01Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:28:01Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:28:01Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:28:01Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:28:01Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:28:01Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:28:01Z- TNavBF::mmse selected from filter -1 2021-02-12T23:28:01Z- TerrainMapOctree: (vehN, vehE ) = (4057049.77, 591002.54). 2021-02-12T23:28:01Z- TerrainMapOctree: Min Distance = 5309.23. 2021-02-12T23:28:01Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:28:01Z- There is no specific good beam that all particles have in common. 2021-02-12T23:28:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:28:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 11.56 2021-02-12T23:28:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:28:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 18.19 2021-02-12T23:28:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:28:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 9.75 2021-02-12T23:28:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:28:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 21.06 2021-02-12T23:28:01Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:28:01Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:28:01Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:28:01Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:28:01Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:28:01Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:28:01Z- Filter 0 No beams used in subcloud update 2021-02-12T23:28:01Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:28:01Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:28:01Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 763.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:28:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172480.72 sec. 2021-02-12T23:28:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:28:01Z- TNavBF::mmse selected from filter -1 2021-02-12T23:28:01Z- variances([0], [2], and psi_berg) = 763.012567 833.046230 0.000000 2021-02-12T23:28:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:28:01Z- LcmTrn::publishEstimates() - MLE : 4057032.56 590955.23 8.25 2021-02-12T23:28:01Z- LcmTrn::publishEstimates() - MMSE : 4057050.06 591002.73 8.25 2021-02-12T23:28:01Z- LcmTrn::publishEstimates() - COVAR: 763.01 833.05 0.95 0.00 2021-02-12T23:28:01Z- AHRS_M2 msg: 1613172481.50 epoch sec; seqNo:3963 2021-02-12T23:28:01Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.14 psi 2021-02-12T23:28:01Z- handleDepth()-Depth_Keller msg: 1613172481508 epoch sec; seqNo:4004; depth 8.38 2021-02-12T23:28:01Z- handleDvl() - RDI_Pathfinder msg: 1613172481.52 epoch sec; seqNo:1914 2021-02-12T23:28:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 102.78 , 76.41 , 116.29 2021-02-12T23:28:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.01 , 0.11 2021-02-12T23:28:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172481.53 epoch sec; seqNo:4004; 4057050.44 north; 591002.72 east 2021-02-12T23:28:01Z- AHRS_M2 msg: 1613172481.91 epoch sec; seqNo:3964 2021-02-12T23:28:01Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.13 psi 2021-02-12T23:28:01Z- handleDepth()-Depth_Keller msg: 1613172481921 epoch sec; seqNo:4005; depth 8.51 2021-02-12T23:28:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172481.93 epoch sec; seqNo:4005; 4057050.44 north; 591002.72 east 2021-02-12T23:28:02Z- AHRS_M2 msg: 1613172482.31 epoch sec; seqNo:3965 2021-02-12T23:28:02Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.12 psi 2021-02-12T23:28:02Z- handleDepth()-Depth_Keller msg: 1613172482317 epoch sec; seqNo:4006; depth 8.63 2021-02-12T23:28:02Z- handleDvl() - RDI_Pathfinder msg: 1613172482.34 epoch sec; seqNo:1915 2021-02-12T23:28:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.97 , 100.14 , 76.41 , 119.07 2021-02-12T23:28:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.10 , 0.00 2021-02-12T23:28:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172482.35 epoch sec; seqNo:4006; 4057050.82 north; 591002.72 east 2021-02-12T23:28:02Z- AHRS_M2 msg: 1613172482.71 epoch sec; seqNo:3966 2021-02-12T23:28:02Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.12 psi 2021-02-12T23:28:02Z- handleDepth()-Depth_Keller msg: 1613172482720 epoch sec; seqNo:4007; depth 8.75 2021-02-12T23:28:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:02Z- handleDvl() - RDI_Pathfinder msg: 1613172482.72 epoch sec; seqNo:1916 2021-02-12T23:28:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.10 , 0.00 2021-02-12T23:28:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172482.73 epoch sec; seqNo:4007; 4057051.20 north; 591002.71 east 2021-02-12T23:28:03Z- AHRS_M2 msg: 1613172483.12 epoch sec; seqNo:3967 2021-02-12T23:28:03Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.13 psi 2021-02-12T23:28:03Z- handleDepth()-Depth_Keller msg: 1613172483130 epoch sec; seqNo:4008; depth 8.87 2021-02-12T23:28:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172483.14 epoch sec; seqNo:4008; 4057051.20 north; 591002.71 east 2021-02-12T23:28:03Z- AHRS_M2 msg: 1613172483.52 epoch sec; seqNo:3968 2021-02-12T23:28:03Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.14 psi 2021-02-12T23:28:03Z- handleDepth()-Depth_Keller msg: 1613172483538 epoch sec; seqNo:4009; depth 9.00 2021-02-12T23:28:03Z- handleDvl() - RDI_Pathfinder msg: 1613172483.55 epoch sec; seqNo:1917 2021-02-12T23:28:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.37 , 96.02 , 76.82 , 117.65 2021-02-12T23:28:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , -0.06 2021-02-12T23:28:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172483.56 epoch sec; seqNo:4009; 4057051.58 north; 591002.71 east 2021-02-12T23:28:03Z- AHRS_M2 msg: 1613172483.93 epoch sec; seqNo:3969 2021-02-12T23:28:03Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.17 psi 2021-02-12T23:28:03Z- handleDepth()-Depth_Keller msg: 1613172483940 epoch sec; seqNo:4010; depth 9.13 2021-02-12T23:28:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:03Z- handleDvl() - RDI_Pathfinder msg: 1613172483.94 epoch sec; seqNo:1918 2021-02-12T23:28:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , -0.06 2021-02-12T23:28:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172483.97 epoch sec; seqNo:4010; 4057051.97 north; 591003.93 east 2021-02-12T23:28:04Z- AHRS_M2 msg: 1613172484.34 epoch sec; seqNo:3970 2021-02-12T23:28:04Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.17 psi 2021-02-12T23:28:04Z- handleDepth()-Depth_Keller msg: 1613172484343 epoch sec; seqNo:4011; depth 9.26 2021-02-12T23:28:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172484.35 epoch sec; seqNo:4011; 4057051.97 north; 591003.93 east 2021-02-12T23:28:04Z- AHRS_M2 msg: 1613172484.74 epoch sec; seqNo:3971 2021-02-12T23:28:04Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.17 psi 2021-02-12T23:28:04Z- handleDepth()-Depth_Keller msg: 1613172484742 epoch sec; seqNo:4012; depth 9.37 2021-02-12T23:28:04Z- handleDvl() - RDI_Pathfinder msg: 1613172484.75 epoch sec; seqNo:1919 2021-02-12T23:28:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.84 , 102.71 , 78.38 , 117.58 2021-02-12T23:28:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.06 , 0.02 2021-02-12T23:28:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172484.76 epoch sec; seqNo:4012; 4057052.35 north; 591003.92 east 2021-02-12T23:28:05Z- AHRS_M2 msg: 1613172485.14 epoch sec; seqNo:3972 2021-02-12T23:28:05Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.16 psi 2021-02-12T23:28:05Z- handleDepth()-Depth_Keller msg: 1613172485148 epoch sec; seqNo:4013; depth 9.52 2021-02-12T23:28:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:05Z- handleDvl() - RDI_Pathfinder msg: 1613172485.15 epoch sec; seqNo:1920 2021-02-12T23:28:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.06 , 0.02 2021-02-12T23:28:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172485.17 epoch sec; seqNo:4013; 4057052.73 north; 591003.92 east 2021-02-12T23:28:05Z- AHRS_M2 msg: 1613172485.54 epoch sec; seqNo:3973 2021-02-12T23:28:05Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.15 psi 2021-02-12T23:28:05Z- handleDepth()-Depth_Keller msg: 1613172485548 epoch sec; seqNo:4014; depth 9.66 2021-02-12T23:28:05Z- handleDvl() - RDI_Pathfinder msg: 1613172485.56 epoch sec; seqNo:1921 2021-02-12T23:28:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.71 , 74.78 , 120.28 2021-02-12T23:28:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.09 2021-02-12T23:28:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172485.57 epoch sec; seqNo:4014; 4057052.73 north; 591003.92 east 2021-02-12T23:28:05Z- AHRS_M2 msg: 1613172485.95 epoch sec; seqNo:3974 2021-02-12T23:28:05Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.15 psi 2021-02-12T23:28:05Z- handleDepth()-Depth_Keller msg: 1613172485957 epoch sec; seqNo:4015; depth 9.79 2021-02-12T23:28:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172485.97 epoch sec; seqNo:4015; 4057053.10 north; 591003.92 east 2021-02-12T23:28:06Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-12T23:28:06Z- waiting for 1613172484.12, time is 1613172485.97 2021-02-12T23:28:06Z- LcmTrn::updateTrn() >>>> pose time 1613172485.97, meas time 1613172485.56 <<<< 2021-02-12T23:28:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172485.56 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:28:06Z- TerrainNav::motionUpdate() - currEstimate = (4057053, 591003.92, 9.79) 2021-02-12T23:28:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:28:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:28:06Z- TNavBF::Projecting Measurements 2021-02-12T23:28:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:28:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:28:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:28:06Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:28:06Z- TNavFilter::3 good beams. 2021-02-12T23:28:06Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:28:06Z- TNavBF:: beamIndex[0] = 1 2021-02-12T23:28:06Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:28:06Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:28:06Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:28:06Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:28:06Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:28:06Z- TNavBF::mmse selected from filter -1 2021-02-12T23:28:06Z- TerrainMapOctree: (vehN, vehE ) = (4057052.85, 591003.82). 2021-02-12T23:28:06Z- TerrainMapOctree: Min Distance = 5306.02. 2021-02-12T23:28:06Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:28:06Z- There is no specific good beam that all particles have in common. 2021-02-12T23:28:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:28:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 16.88 2021-02-12T23:28:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:28:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 8.95 2021-02-12T23:28:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:28:06Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 23.15 2021-02-12T23:28:06Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:28:06Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:28:06Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:28:06Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:28:06Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:28:06Z- Filter 0 No beams used in subcloud update 2021-02-12T23:28:06Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:28:06Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:28:06Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 762.9 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:28:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172485.56 sec. 2021-02-12T23:28:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:28:06Z- TNavBF::mmse selected from filter -1 2021-02-12T23:28:06Z- variances([0], [2], and psi_berg) = 762.881132 833.046085 0.000000 2021-02-12T23:28:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:28:06Z- LcmTrn::publishEstimates() - MLE : 4057035.60 590956.42 9.79 2021-02-12T23:28:06Z- LcmTrn::publishEstimates() - MMSE : 4057053.10 591003.92 9.79 2021-02-12T23:28:06Z- LcmTrn::publishEstimates() - COVAR: 762.88 833.05 0.93 0.00 2021-02-12T23:28:06Z- AHRS_M2 msg: 1613172486.37 epoch sec; seqNo:3975 2021-02-12T23:28:06Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.15 psi 2021-02-12T23:28:06Z- handleDepth()-Depth_Keller msg: 1613172486373 epoch sec; seqNo:4016; depth 9.92 2021-02-12T23:28:06Z- handleDvl() - RDI_Pathfinder msg: 1613172486.39 epoch sec; seqNo:1922 2021-02-12T23:28:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 101.15 , 76.41 , 121.43 2021-02-12T23:28:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , 0.04 2021-02-12T23:28:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172486.40 epoch sec; seqNo:4016; 4057053.50 north; 591005.13 east 2021-02-12T23:28:06Z- AHRS_M2 msg: 1613172486.76 epoch sec; seqNo:3976 2021-02-12T23:28:06Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.15 psi 2021-02-12T23:28:06Z- handleDepth()-Depth_Keller msg: 1613172486760 epoch sec; seqNo:4017; depth 10.09 2021-02-12T23:28:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172486.77 epoch sec; seqNo:4017; 4057053.50 north; 591005.13 east 2021-02-12T23:28:07Z- AHRS_M2 msg: 1613172487.16 epoch sec; seqNo:3977 2021-02-12T23:28:07Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.15 psi 2021-02-12T23:28:07Z- handleDepth()-Depth_Keller msg: 1613172487164 epoch sec; seqNo:4018; depth 10.21 2021-02-12T23:28:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172487.17 epoch sec; seqNo:4018; 4057053.88 north; 591005.13 east 2021-02-12T23:28:07Z- AHRS_M2 msg: 1613172487.56 epoch sec; seqNo:3978 2021-02-12T23:28:07Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.15 psi 2021-02-12T23:28:07Z- handleDepth()-Depth_Keller msg: 1613172487568 epoch sec; seqNo:4019; depth 10.35 2021-02-12T23:28:07Z- handleDvl() - RDI_Pathfinder msg: 1613172487.58 epoch sec; seqNo:1923 2021-02-12T23:28:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.84 , 95.95 , 74.37 , 120.28 2021-02-12T23:28:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.09 , -0.07 2021-02-12T23:28:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172487.59 epoch sec; seqNo:4019; 4057054.25 north; 591005.12 east 2021-02-12T23:28:07Z- AHRS_M2 msg: 1613172487.97 epoch sec; seqNo:3979 2021-02-12T23:28:07Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.15 psi 2021-02-12T23:28:07Z- handleDepth()-Depth_Keller msg: 1613172487974 epoch sec; seqNo:4020; depth 10.50 2021-02-12T23:28:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172487.99 epoch sec; seqNo:4020; 4057054.25 north; 591005.12 east 2021-02-12T23:28:08Z- AHRS_M2 msg: 1613172488.37 epoch sec; seqNo:3980 2021-02-12T23:28:08Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.16 psi 2021-02-12T23:28:08Z- handleDepth()-Depth_Keller msg: 1613172488376 epoch sec; seqNo:4021; depth 10.65 2021-02-12T23:28:08Z- handleDvl() - RDI_Pathfinder msg: 1613172488.39 epoch sec; seqNo:1924 2021-02-12T23:28:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 96.15 , 75.18 , 116.43 2021-02-12T23:28:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , -0.16 2021-02-12T23:28:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172488.40 epoch sec; seqNo:4021; 4057054.65 north; 591006.34 east 2021-02-12T23:28:08Z- AHRS_M2 msg: 1613172488.78 epoch sec; seqNo:3981 2021-02-12T23:28:08Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.16 psi 2021-02-12T23:28:08Z- handleDepth()-Depth_Keller msg: 1613172488780 epoch sec; seqNo:4022; depth 10.78 2021-02-12T23:28:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:08Z- handleDvl() - RDI_Pathfinder msg: 1613172488.78 epoch sec; seqNo:1925 2021-02-12T23:28:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , -0.16 2021-02-12T23:28:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172488.79 epoch sec; seqNo:4022; 4057055.02 north; 591006.34 east 2021-02-12T23:28:09Z- AHRS_M2 msg: 1613172489.18 epoch sec; seqNo:3982 2021-02-12T23:28:09Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.17 psi 2021-02-12T23:28:09Z- handleDepth()-Depth_Keller msg: 1613172489185 epoch sec; seqNo:4023; depth 10.93 2021-02-12T23:28:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172489.19 epoch sec; seqNo:4023; 4057055.40 north; 591006.33 east 2021-02-12T23:28:09Z- AHRS_M2 msg: 1613172489.58 epoch sec; seqNo:3983 2021-02-12T23:28:09Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.17 psi 2021-02-12T23:28:09Z- handleDepth()-Depth_Keller msg: 1613172489590 epoch sec; seqNo:4024; depth 11.05 2021-02-12T23:28:09Z- handleDvl() - RDI_Pathfinder msg: 1613172489.60 epoch sec; seqNo:1926 2021-02-12T23:28:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 100.28 , 73.55 , 115.15 2021-02-12T23:28:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.06 , -0.15 2021-02-12T23:28:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172489.61 epoch sec; seqNo:4024; 4057055.40 north; 591006.33 east 2021-02-12T23:28:09Z- AHRS_M2 msg: 1613172489.99 epoch sec; seqNo:3984 2021-02-12T23:28:09Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.18 psi 2021-02-12T23:28:09Z- handleDepth()-Depth_Keller msg: 1613172489997 epoch sec; seqNo:4025; depth 11.21 2021-02-12T23:28:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:10Z- handleDvl() - RDI_Pathfinder msg: 1613172490.00 epoch sec; seqNo:1927 2021-02-12T23:28:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.06 , -0.15 2021-02-12T23:28:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172490.02 epoch sec; seqNo:4025; 4057055.78 north; 591006.33 east 2021-02-12T23:28:10Z- AHRS_M2 msg: 1613172490.39 epoch sec; seqNo:3985 2021-02-12T23:28:10Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.18 psi 2021-02-12T23:28:10Z- handleDepth()-Depth_Keller msg: 1613172490396 epoch sec; seqNo:4026; depth 11.34 2021-02-12T23:28:10Z- handleDvl() - RDI_Pathfinder msg: 1613172490.41 epoch sec; seqNo:1928 2021-02-12T23:28:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 101.70 , 75.18 , 116.57 2021-02-12T23:28:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.12 , -0.05 2021-02-12T23:28:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172490.43 epoch sec; seqNo:4026; 4057056.16 north; 591006.32 east 2021-02-12T23:28:10Z- AHRS_M2 msg: 1613172490.80 epoch sec; seqNo:3986 2021-02-12T23:28:10Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.18 psi 2021-02-12T23:28:10Z- handleDepth()-Depth_Keller msg: 1613172490805 epoch sec; seqNo:4027; depth 11.48 2021-02-12T23:28:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172490.82 epoch sec; seqNo:4027; 4057056.16 north; 591006.32 east 2021-02-12T23:28:11Z- AHRS_M2 msg: 1613172491.20 epoch sec; seqNo:3987 2021-02-12T23:28:11Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.19 psi 2021-02-12T23:28:11Z- handleDepth()-Depth_Keller msg: 1613172491206 epoch sec; seqNo:4028; depth 11.60 2021-02-12T23:28:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:11Z- handleDvl() - RDI_Pathfinder msg: 1613172491.21 epoch sec; seqNo:1929 2021-02-12T23:28:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.12 , -0.05 2021-02-12T23:28:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172491.21 epoch sec; seqNo:4028; 4057056.55 north; 591007.54 east 2021-02-12T23:28:11Z- LcmTrn::handleMessages() - handling 47 messages took 0.68 ms and 134 calls... 2021-02-12T23:28:11Z- waiting for 1613172488.97, time is 1613172491.21 2021-02-12T23:28:11Z- LcmTrn::updateTrn() equating pose 1613172491.21 and meas times 1613172491.21 2021-02-12T23:28:11Z- LcmTrn::updateTrn() >>>> pose time 1613172491.21, meas time 1613172491.21 <<<< 2021-02-12T23:28:11Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172491.21, ping # 0. 2021-02-12T23:28:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172491.21 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:28:11Z- TerrainNav::motionUpdate() - currEstimate = (4057057, 591007.54, 11.60) 2021-02-12T23:28:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:28:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:28:11Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:28:11Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:28:11Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172491.21 sec.; No relevant navigation data available 2021-02-12T23:28:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:28:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:28:11Z- variances([0], [2], and psi_berg) = 762.750907 833.045941 0.000000 2021-02-12T23:28:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:28:11Z- LcmTrn::publishEstimates() - MLE : 4057039.05 590960.04 11.60 2021-02-12T23:28:11Z- LcmTrn::publishEstimates() - MMSE : 4057056.55 591007.54 11.60 2021-02-12T23:28:11Z- LcmTrn::publishEstimates() - COVAR: 762.75 833.05 1.18 0.00 2021-02-12T23:28:11Z- AHRS_M2 msg: 1613172491.60 epoch sec; seqNo:3988 2021-02-12T23:28:11Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.19 psi 2021-02-12T23:28:11Z- handleDepth()-Depth_Keller msg: 1613172491608 epoch sec; seqNo:4029; depth 11.72 2021-02-12T23:28:11Z- handleDvl() - RDI_Pathfinder msg: 1613172491.63 epoch sec; seqNo:1930 2021-02-12T23:28:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.64 , 96.15 , 74.78 , 113.73 2021-02-12T23:28:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.09 , 0.07 2021-02-12T23:28:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172491.64 epoch sec; seqNo:4029; 4057056.93 north; 591007.54 east 2021-02-12T23:28:12Z- AHRS_M2 msg: 1613172492.01 epoch sec; seqNo:3989 2021-02-12T23:28:12Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.19 psi 2021-02-12T23:28:12Z- handleDepth()-Depth_Keller msg: 1613172492017 epoch sec; seqNo:4030; depth 11.85 2021-02-12T23:28:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172492.03 epoch sec; seqNo:4030; 4057056.93 north; 591007.54 east 2021-02-12T23:28:12Z- AHRS_M2 msg: 1613172492.42 epoch sec; seqNo:3990 2021-02-12T23:28:12Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-12T23:28:12Z- handleDepth()-Depth_Keller msg: 1613172492424 epoch sec; seqNo:4031; depth 11.98 2021-02-12T23:28:12Z- handleDvl() - RDI_Pathfinder msg: 1613172492.45 epoch sec; seqNo:1931 2021-02-12T23:28:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 100.41 , 74.37 , 113.93 2021-02-12T23:28:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , 0.13 2021-02-12T23:28:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172492.47 epoch sec; seqNo:4031; 4057057.31 north; 591007.53 east 2021-02-12T23:28:12Z- AHRS_M2 msg: 1613172492.82 epoch sec; seqNo:3991 2021-02-12T23:28:12Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-12T23:28:12Z- handleDepth()-Depth_Keller msg: 1613172492829 epoch sec; seqNo:4032; depth 12.12 2021-02-12T23:28:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172492.85 epoch sec; seqNo:4032; 4057057.69 north; 591007.53 east 2021-02-12T23:28:13Z- AHRS_M2 msg: 1613172493.22 epoch sec; seqNo:3992 2021-02-12T23:28:13Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-12T23:28:13Z- handleDepth()-Depth_Keller msg: 1613172493224 epoch sec; seqNo:4033; depth 12.25 2021-02-12T23:28:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172493.25 epoch sec; seqNo:4033; 4057057.69 north; 591007.53 east 2021-02-12T23:28:13Z- AHRS_M2 msg: 1613172493.62 epoch sec; seqNo:3993 2021-02-12T23:28:13Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.20 psi 2021-02-12T23:28:13Z- handleDepth()-Depth_Keller msg: 1613172493628 epoch sec; seqNo:4034; depth 12.38 2021-02-12T23:28:13Z- handleDvl() - RDI_Pathfinder msg: 1613172493.64 epoch sec; seqNo:1932 2021-02-12T23:28:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 96.29 , 73.96 , 113.86 2021-02-12T23:28:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.13 , 0.06 2021-02-12T23:28:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172493.66 epoch sec; seqNo:4034; 4057058.07 north; 591007.53 east 2021-02-12T23:28:14Z- AHRS_M2 msg: 1613172494.03 epoch sec; seqNo:3994 2021-02-12T23:28:14Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.21 psi 2021-02-12T23:28:14Z- handleDepth()-Depth_Keller msg: 1613172494037 epoch sec; seqNo:4035; depth 12.52 2021-02-12T23:28:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172494.05 epoch sec; seqNo:4035; 4057058.46 north; 591008.74 east 2021-02-12T23:28:14Z- AHRS_M2 msg: 1613172494.44 epoch sec; seqNo:3995 2021-02-12T23:28:14Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.21 psi 2021-02-12T23:28:14Z- handleDepth()-Depth_Keller msg: 1613172494441 epoch sec; seqNo:4036; depth 12.64 2021-02-12T23:28:14Z- handleDvl() - RDI_Pathfinder msg: 1613172494.45 epoch sec; seqNo:1933 2021-02-12T23:28:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.49 , 96.36 , 73.96 , 111.23 2021-02-12T23:28:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.15 , -0.10 2021-02-12T23:28:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172494.46 epoch sec; seqNo:4036; 4057058.46 north; 591008.74 east 2021-02-12T23:28:14Z- AHRS_M2 msg: 1613172494.84 epoch sec; seqNo:3996 2021-02-12T23:28:14Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.22 psi 2021-02-12T23:28:14Z- handleDepth()-Depth_Keller msg: 1613172494840 epoch sec; seqNo:4037; depth 12.78 2021-02-12T23:28:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:14Z- handleDvl() - RDI_Pathfinder msg: 1613172494.86 epoch sec; seqNo:1934 2021-02-12T23:28:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.15 , -0.10 2021-02-12T23:28:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172494.88 epoch sec; seqNo:4037; 4057058.84 north; 591008.74 east 2021-02-12T23:28:15Z- AHRS_M2 msg: 1613172495.24 epoch sec; seqNo:3997 2021-02-12T23:28:15Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.22 psi 2021-02-12T23:28:15Z- handleDepth()-Depth_Keller msg: 1613172495244 epoch sec; seqNo:4038; depth 12.92 2021-02-12T23:28:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172495.25 epoch sec; seqNo:4038; 4057059.22 north; 591008.73 east 2021-02-12T23:28:15Z- AHRS_M2 msg: 1613172495.64 epoch sec; seqNo:3998 2021-02-12T23:28:15Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.22 psi 2021-02-12T23:28:15Z- handleDepth()-Depth_Keller msg: 1613172495648 epoch sec; seqNo:4039; depth 13.03 2021-02-12T23:28:15Z- handleDvl() - RDI_Pathfinder msg: 1613172495.66 epoch sec; seqNo:1935 2021-02-12T23:28:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.46 , 96.49 , 74.78 , 107.44 2021-02-12T23:28:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.02 , -0.22 2021-02-12T23:28:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172495.67 epoch sec; seqNo:4039; 4057059.60 north; 591008.73 east 2021-02-12T23:28:16Z- AHRS_M2 msg: 1613172496.05 epoch sec; seqNo:3999 2021-02-12T23:28:16Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.22 psi 2021-02-12T23:28:16Z- handleDepth()-Depth_Keller msg: 1613172496054 epoch sec; seqNo:4040; depth 13.15 2021-02-12T23:28:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:16Z- handleDvl() - RDI_Pathfinder msg: 1613172496.06 epoch sec; seqNo:1936 2021-02-12T23:28:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.02 , -0.22 2021-02-12T23:28:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172496.07 epoch sec; seqNo:4040; 4057059.99 north; 591009.95 east 2021-02-12T23:28:16Z- LcmTrn::handleMessages() - handling 43 messages took 0.63 ms and 131 calls... 2021-02-12T23:28:16Z- waiting for 1613172494.21, time is 1613172496.07 2021-02-12T23:28:16Z- LcmTrn::updateTrn() equating pose 1613172496.07 and meas times 1613172496.07 2021-02-12T23:28:16Z- LcmTrn::updateTrn() >>>> pose time 1613172496.07, meas time 1613172496.07 <<<< 2021-02-12T23:28:16Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172496.07, ping # 0. 2021-02-12T23:28:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172496.07 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:28:16Z- TerrainNav::motionUpdate() - currEstimate = (4057060, 591009.95, 13.15) 2021-02-12T23:28:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:28:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:28:16Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:28:16Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:28:16Z- TNavBF::Projecting Measurements 2021-02-12T23:28:16Z- TNavFilter::Measurement from time = 1613172496.07 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:28:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:28:16Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:28:16Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:28:16Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:28:16Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:28:16Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:28:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:28:16Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 762.6 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-12T23:28:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:28:16Z- TNavBF::mmse selected from filter -1 2021-02-12T23:28:16Z- variances([0], [2], and psi_berg) = 762.631920 833.045810 0.000000 2021-02-12T23:28:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:28:16Z- LcmTrn::publishEstimates() - MLE : 4057042.49 590961.45 13.15 2021-02-12T23:28:16Z- LcmTrn::publishEstimates() - MMSE : 4057059.99 591009.95 13.15 2021-02-12T23:28:16Z- LcmTrn::publishEstimates() - COVAR: 762.63 833.05 1.40 0.00 2021-02-12T23:28:16Z- AHRS_M2 msg: 1613172496.45 epoch sec; seqNo:4000 2021-02-12T23:28:16Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.21 psi 2021-02-12T23:28:16Z- handleDepth()-Depth_Keller msg: 1613172496456 epoch sec; seqNo:4041; depth 13.28 2021-02-12T23:28:16Z- handleDvl() - RDI_Pathfinder msg: 1613172496.48 epoch sec; seqNo:1937 2021-02-12T23:28:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.56 , 73.55 , 107.51 2021-02-12T23:28:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.15 , -0.16 2021-02-12T23:28:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172496.49 epoch sec; seqNo:4041; 4057059.99 north; 591009.95 east 2021-02-12T23:28:16Z- AHRS_M2 msg: 1613172496.86 epoch sec; seqNo:4001 2021-02-12T23:28:16Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.21 psi 2021-02-12T23:28:16Z- handleDepth()-Depth_Keller msg: 1613172496869 epoch sec; seqNo:4042; depth 13.39 2021-02-12T23:28:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172496.89 epoch sec; seqNo:4042; 4057060.37 north; 591009.94 east 2021-02-12T23:28:17Z- AHRS_M2 msg: 1613172497.26 epoch sec; seqNo:4002 2021-02-12T23:28:17Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.20 psi 2021-02-12T23:28:17Z- handleDepth()-Depth_Keller msg: 1613172497264 epoch sec; seqNo:4043; depth 13.52 2021-02-12T23:28:17Z- handleDvl() - RDI_Pathfinder msg: 1613172497.28 epoch sec; seqNo:1938 2021-02-12T23:28:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.64 , 96.29 , 73.14 , 105.75 2021-02-12T23:28:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.22 , 0.01 2021-02-12T23:28:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172497.29 epoch sec; seqNo:4043; 4057060.75 north; 591009.94 east 2021-02-12T23:28:17Z- AHRS_M2 msg: 1613172497.66 epoch sec; seqNo:4003 2021-02-12T23:28:17Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.20 psi 2021-02-12T23:28:17Z- handleDepth()-Depth_Keller msg: 1613172497670 epoch sec; seqNo:4044; depth 13.62 2021-02-12T23:28:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172497.68 epoch sec; seqNo:4044; 4057061.13 north; 591009.94 east 2021-02-12T23:28:18Z- AHRS_M2 msg: 1613172498.07 epoch sec; seqNo:4004 2021-02-12T23:28:18Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.20 psi 2021-02-12T23:28:18Z- handleDepth()-Depth_Keller msg: 1613172498072 epoch sec; seqNo:4045; depth 13.73 2021-02-12T23:28:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172498.08 epoch sec; seqNo:4045; 4057061.50 north; 591009.93 east 2021-02-12T23:28:18Z- AHRS_M2 msg: 1613172498.47 epoch sec; seqNo:4005 2021-02-12T23:28:18Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.20 psi 2021-02-12T23:28:18Z- handleDepth()-Depth_Keller msg: 1613172498476 epoch sec; seqNo:4046; depth 13.84 2021-02-12T23:28:18Z- handleDvl() - RDI_Pathfinder msg: 1613172498.49 epoch sec; seqNo:1939 2021-02-12T23:28:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 96.69 , 72.73 , 106.43 2021-02-12T23:28:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.12 , 0.13 2021-02-12T23:28:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172498.50 epoch sec; seqNo:4046; 4057061.50 north; 591009.93 east 2021-02-12T23:28:18Z- AHRS_M2 msg: 1613172498.88 epoch sec; seqNo:4006 2021-02-12T23:28:18Z- AHRS_M2 msg: 0.57 phi; -0.30 theta; -0.20 psi 2021-02-12T23:28:18Z- handleDepth()-Depth_Keller msg: 1613172498886 epoch sec; seqNo:4047; depth 13.95 2021-02-12T23:28:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172498.90 epoch sec; seqNo:4047; 4057061.88 north; 591009.93 east 2021-02-12T23:28:19Z- AHRS_M2 msg: 1613172499.28 epoch sec; seqNo:4007 2021-02-12T23:28:19Z- AHRS_M2 msg: 0.56 phi; -0.30 theta; -0.20 psi 2021-02-12T23:28:19Z- handleDepth()-Depth_Keller msg: 1613172499312 epoch sec; seqNo:4048; depth 14.04 2021-02-12T23:28:19Z- handleDvl() - RDI_Pathfinder msg: 1613172499.33 epoch sec; seqNo:1940 2021-02-12T23:28:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 96.69 , 72.73 , 106.43 2021-02-12T23:28:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , 0.13 2021-02-12T23:28:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172499.35 epoch sec; seqNo:4048; 4057062.26 north; 591009.92 east 2021-02-12T23:28:19Z- AHRS_M2 msg: 1613172499.68 epoch sec; seqNo:4008 2021-02-12T23:28:19Z- AHRS_M2 msg: 0.56 phi; -0.30 theta; -0.20 psi 2021-02-12T23:28:19Z- handleDepth()-Depth_Keller msg: 1613172499688 epoch sec; seqNo:4049; depth 14.16 2021-02-12T23:28:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:19Z- handleDvl() - RDI_Pathfinder msg: 1613172499.69 epoch sec; seqNo:1941 2021-02-12T23:28:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , 0.13 2021-02-12T23:28:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172499.72 epoch sec; seqNo:4049; 4057062.65 north; 591011.14 east 2021-02-12T23:28:20Z- AHRS_M2 msg: 1613172500.09 epoch sec; seqNo:4009 2021-02-12T23:28:20Z- AHRS_M2 msg: 0.56 phi; -0.29 theta; -0.21 psi 2021-02-12T23:28:20Z- handleDepth()-Depth_Keller msg: 1613172500092 epoch sec; seqNo:4050; depth 14.27 2021-02-12T23:28:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172500.10 epoch sec; seqNo:4050; 4057062.65 north; 591011.14 east 2021-02-12T23:28:20Z- AHRS_M2 msg: 1613172500.49 epoch sec; seqNo:4010 2021-02-12T23:28:20Z- AHRS_M2 msg: 0.56 phi; -0.29 theta; -0.21 psi 2021-02-12T23:28:20Z- handleDepth()-Depth_Keller msg: 1613172500498 epoch sec; seqNo:4051; depth 14.38 2021-02-12T23:28:20Z- handleDvl() - RDI_Pathfinder msg: 1613172500.51 epoch sec; seqNo:1942 2021-02-12T23:28:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.66 , 96.69 , 73.55 , 103.99 2021-02-12T23:28:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , 0.07 2021-02-12T23:28:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172500.52 epoch sec; seqNo:4051; 4057063.03 north; 591011.14 east 2021-02-12T23:28:20Z- AHRS_M2 msg: 1613172500.90 epoch sec; seqNo:4011 2021-02-12T23:28:20Z- AHRS_M2 msg: 0.58 phi; -0.29 theta; -0.19 psi 2021-02-12T23:28:20Z- handleDepth()-Depth_Keller msg: 1613172500916 epoch sec; seqNo:4052; depth 14.49 2021-02-12T23:28:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:20Z- handleDvl() - RDI_Pathfinder msg: 1613172500.92 epoch sec; seqNo:1943 2021-02-12T23:28:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , 0.07 2021-02-12T23:28:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172500.93 epoch sec; seqNo:4052; 4057063.41 north; 591011.13 east 2021-02-12T23:28:21Z- AHRS_M2 msg: 1613172501.32 epoch sec; seqNo:4012 2021-02-12T23:28:21Z- AHRS_M2 msg: 0.58 phi; -0.29 theta; -0.18 psi 2021-02-12T23:28:21Z- handleDepth()-Depth_Keller msg: 1613172501331 epoch sec; seqNo:4053; depth 14.60 2021-02-12T23:28:21Z- handleDvl() - RDI_Pathfinder msg: 1613172501.34 epoch sec; seqNo:1944 2021-02-12T23:28:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:21Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:28:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172501.35 epoch sec; seqNo:4053; 4057063.79 north; 591011.13 east 2021-02-12T23:28:21Z- LcmTrn::handleMessages() - handling 47 messages took 0.76 ms and 132 calls... 2021-02-12T23:28:21Z- waiting for 1613172499.07, time is 1613172501.35 2021-02-12T23:28:21Z- LcmTrn::updateTrn() equating pose 1613172501.35 and meas times 1613172501.35 2021-02-12T23:28:21Z- LcmTrn::updateTrn() >>>> pose time 1613172501.35, meas time 1613172501.35 <<<< 2021-02-12T23:28:21Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172501.35, ping # 0. 2021-02-12T23:28:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172501.35 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:28:21Z- TerrainNav::motionUpdate() - currEstimate = (4057064, 591011.13, 14.60) 2021-02-12T23:28:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:28:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:28:21Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:28:21Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:28:21Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172501.35 sec.; No relevant navigation data available 2021-02-12T23:28:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:28:21Z- TNavBF::mmse selected from filter -1 2021-02-12T23:28:21Z- variances([0], [2], and psi_berg) = 762.516202 833.045683 0.000000 2021-02-12T23:28:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:28:21Z- LcmTrn::publishEstimates() - MLE : 4057046.29 590962.63 14.60 2021-02-12T23:28:21Z- LcmTrn::publishEstimates() - MMSE : 4057063.79 591011.13 14.60 2021-02-12T23:28:21Z- LcmTrn::publishEstimates() - COVAR: 762.52 833.05 1.60 0.00 2021-02-12T23:28:21Z- AHRS_M2 msg: 1613172501.70 epoch sec; seqNo:4013 2021-02-12T23:28:21Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.16 psi 2021-02-12T23:28:21Z- handleDepth()-Depth_Keller msg: 1613172501713 epoch sec; seqNo:4054; depth 14.71 2021-02-12T23:28:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172501.72 epoch sec; seqNo:4054; 4057063.79 north; 591011.13 east 2021-02-12T23:28:22Z- AHRS_M2 msg: 1613172502.11 epoch sec; seqNo:4014 2021-02-12T23:28:22Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.15 psi 2021-02-12T23:28:22Z- handleDepth()-Depth_Keller msg: 1613172502115 epoch sec; seqNo:4055; depth 14.81 2021-02-12T23:28:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:22Z- handleDvl() - RDI_Pathfinder msg: 1613172502.12 epoch sec; seqNo:1945 2021-02-12T23:28:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:22Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:28:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172502.13 epoch sec; seqNo:4055; 4057064.18 north; 591012.35 east 2021-02-12T23:28:22Z- AHRS_M2 msg: 1613172502.51 epoch sec; seqNo:4015 2021-02-12T23:28:22Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.14 psi 2021-02-12T23:28:22Z- handleDepth()-Depth_Keller msg: 1613172502516 epoch sec; seqNo:4056; depth 14.92 2021-02-12T23:28:22Z- handleDvl() - RDI_Pathfinder msg: 1613172502.53 epoch sec; seqNo:1946 2021-02-12T23:28:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 94.73 , 71.51 , 102.85 2021-02-12T23:28:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.04 , 0.02 2021-02-12T23:28:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172502.55 epoch sec; seqNo:4056; 4057064.56 north; 591012.34 east 2021-02-12T23:28:22Z- AHRS_M2 msg: 1613172502.91 epoch sec; seqNo:4016 2021-02-12T23:28:22Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.14 psi 2021-02-12T23:28:22Z- handleDepth()-Depth_Keller msg: 1613172502925 epoch sec; seqNo:4057; depth 15.04 2021-02-12T23:28:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172502.93 epoch sec; seqNo:4057; 4057064.56 north; 591012.34 east 2021-02-12T23:28:23Z- AHRS_M2 msg: 1613172503.32 epoch sec; seqNo:4017 2021-02-12T23:28:23Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.14 psi 2021-02-12T23:28:23Z- handleDepth()-Depth_Keller msg: 1613172503325 epoch sec; seqNo:4058; depth 15.12 2021-02-12T23:28:23Z- handleDvl() - RDI_Pathfinder msg: 1613172503.34 epoch sec; seqNo:1947 2021-02-12T23:28:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.27 , 91.69 , 70.28 , 102.64 2021-02-12T23:28:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.09 , -0.02 2021-02-12T23:28:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172503.35 epoch sec; seqNo:4058; 4057064.94 north; 591012.34 east 2021-02-12T23:28:23Z- AHRS_M2 msg: 1613172503.73 epoch sec; seqNo:4018 2021-02-12T23:28:23Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.14 psi 2021-02-12T23:28:23Z- handleDepth()-Depth_Keller msg: 1613172503741 epoch sec; seqNo:4059; depth 15.23 2021-02-12T23:28:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172503.75 epoch sec; seqNo:4059; 4057065.32 north; 591012.33 east 2021-02-12T23:28:24Z- AHRS_M2 msg: 1613172504.13 epoch sec; seqNo:4019 2021-02-12T23:28:24Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.14 psi 2021-02-12T23:28:24Z- handleDepth()-Depth_Keller msg: 1613172504139 epoch sec; seqNo:4060; depth 15.33 2021-02-12T23:28:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172504.17 epoch sec; seqNo:4060; 4057065.32 north; 591012.33 east 2021-02-12T23:28:24Z- AHRS_M2 msg: 1613172504.53 epoch sec; seqNo:4020 2021-02-12T23:28:24Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.15 psi 2021-02-12T23:28:24Z- handleDepth()-Depth_Keller msg: 1613172504536 epoch sec; seqNo:4061; depth 15.41 2021-02-12T23:28:24Z- handleDvl() - RDI_Pathfinder msg: 1613172504.55 epoch sec; seqNo:1948 2021-02-12T23:28:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.86 , 90.81 , 71.10 , 101.76 2021-02-12T23:28:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.05 , -0.09 2021-02-12T23:28:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172504.56 epoch sec; seqNo:4061; 4057065.71 north; 591013.55 east 2021-02-12T23:28:24Z- AHRS_M2 msg: 1613172504.93 epoch sec; seqNo:4021 2021-02-12T23:28:24Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.16 psi 2021-02-12T23:28:24Z- handleDepth()-Depth_Keller msg: 1613172504947 epoch sec; seqNo:4062; depth 15.52 2021-02-12T23:28:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172504.96 epoch sec; seqNo:4062; 4057066.09 north; 591013.55 east 2021-02-12T23:28:25Z- AHRS_M2 msg: 1613172505.34 epoch sec; seqNo:4022 2021-02-12T23:28:25Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.17 psi 2021-02-12T23:28:25Z- handleDepth()-Depth_Keller msg: 1613172505350 epoch sec; seqNo:4063; depth 15.62 2021-02-12T23:28:25Z- handleDvl() - RDI_Pathfinder msg: 1613172505.37 epoch sec; seqNo:1949 2021-02-12T23:28:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 92.03 , 72.32 , 101.76 2021-02-12T23:28:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , -0.14 2021-02-12T23:28:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172505.38 epoch sec; seqNo:4063; 4057066.47 north; 591013.54 east 2021-02-12T23:28:25Z- AHRS_M2 msg: 1613172505.74 epoch sec; seqNo:4023 2021-02-12T23:28:25Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.18 psi 2021-02-12T23:28:25Z- handleDepth()-Depth_Keller msg: 1613172505748 epoch sec; seqNo:4064; depth 15.72 2021-02-12T23:28:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:25Z- handleDvl() - RDI_Pathfinder msg: 1613172505.75 epoch sec; seqNo:1950 2021-02-12T23:28:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , -0.14 2021-02-12T23:28:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172505.76 epoch sec; seqNo:4064; 4057066.47 north; 591013.54 east 2021-02-12T23:28:26Z- AHRS_M2 msg: 1613172506.15 epoch sec; seqNo:4024 2021-02-12T23:28:26Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.18 psi 2021-02-12T23:28:26Z- handleDepth()-Depth_Keller msg: 1613172506155 epoch sec; seqNo:4065; depth 15.83 2021-02-12T23:28:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172506.16 epoch sec; seqNo:4065; 4057066.85 north; 591013.54 east 2021-02-12T23:28:26Z- LcmTrn::handleMessages() - handling 42 messages took 0.64 ms and 129 calls... 2021-02-12T23:28:26Z- waiting for 1613172504.35, time is 1613172506.16 2021-02-12T23:28:26Z- LcmTrn::updateTrn() >>>> pose time 1613172506.16, meas time 1613172505.75 <<<< 2021-02-12T23:28:26Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172505.75, ping # 0. 2021-02-12T23:28:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172505.75 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:28:26Z- TerrainNav::motionUpdate() - currEstimate = (4057067, 591013.54, 15.83) 2021-02-12T23:28:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:28:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:28:26Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:28:26Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:28:26Z- TNavBF::Projecting Measurements 2021-02-12T23:28:26Z- TNavFilter::Measurement from time = 1613172505.75 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:28:26Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:28:26Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:28:26Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:28:26Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:28:26Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:28:26Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:28:26Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:28:26Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 762.4 EastVariance: 833.0 DepthVariance: 1.8 this->DepthBiasCov: 1.8 2021-02-12T23:28:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:28:26Z- TNavBF::mmse selected from filter -1 2021-02-12T23:28:26Z- variances([0], [2], and psi_berg) = 762.373535 833.045526 0.000000 2021-02-12T23:28:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:28:26Z- LcmTrn::publishEstimates() - MLE : 4057049.35 590967.04 15.83 2021-02-12T23:28:26Z- LcmTrn::publishEstimates() - MMSE : 4057066.85 591013.54 15.83 2021-02-12T23:28:26Z- LcmTrn::publishEstimates() - COVAR: 762.37 833.05 1.80 0.00 2021-02-12T23:28:26Z- AHRS_M2 msg: 1613172506.55 epoch sec; seqNo:4025 2021-02-12T23:28:26Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.18 psi 2021-02-12T23:28:26Z- handleDepth()-Depth_Keller msg: 1613172506569 epoch sec; seqNo:4066; depth 15.92 2021-02-12T23:28:26Z- handleDvl() - RDI_Pathfinder msg: 1613172506.58 epoch sec; seqNo:1951 2021-02-12T23:28:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 98.11 , 71.92 , 100.55 2021-02-12T23:28:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.15 , -0.12 2021-02-12T23:28:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172506.59 epoch sec; seqNo:4066; 4057067.23 north; 591013.53 east 2021-02-12T23:28:26Z- AHRS_M2 msg: 1613172506.95 epoch sec; seqNo:4026 2021-02-12T23:28:26Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.18 psi 2021-02-12T23:28:26Z- handleDepth()-Depth_Keller msg: 1613172506960 epoch sec; seqNo:4067; depth 16.03 2021-02-12T23:28:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:26Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:26Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:26Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:26Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:26Z- handleDvl() - RDI_Pathfinder msg: 1613172506.96 epoch sec; seqNo:1952 2021-02-12T23:28:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.15 , -0.12 2021-02-12T23:28:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172506.97 epoch sec; seqNo:4067; 4057067.60 north; 591013.53 east 2021-02-12T23:28:27Z- AHRS_M2 msg: 1613172507.36 epoch sec; seqNo:4027 2021-02-12T23:28:27Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.17 psi 2021-02-12T23:28:27Z- handleDepth()-Depth_Keller msg: 1613172507364 epoch sec; seqNo:4068; depth 16.14 2021-02-12T23:28:27Z- handleDvl() - RDI_Pathfinder msg: 1613172507.38 epoch sec; seqNo:1953 2021-02-12T23:28:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.59 , 94.33 , 71.92 , 100.41 2021-02-12T23:28:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.21 , -0.04 2021-02-12T23:28:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172507.39 epoch sec; seqNo:4068; 4057068.00 north; 591014.75 east 2021-02-12T23:28:27Z- AHRS_M2 msg: 1613172507.76 epoch sec; seqNo:4028 2021-02-12T23:28:27Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.16 psi 2021-02-12T23:28:27Z- handleDepth()-Depth_Keller msg: 1613172507770 epoch sec; seqNo:4069; depth 16.24 2021-02-12T23:28:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172507.78 epoch sec; seqNo:4069; 4057068.37 north; 591014.74 east 2021-02-12T23:28:28Z- AHRS_M2 msg: 1613172508.19 epoch sec; seqNo:4029 2021-02-12T23:28:28Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.16 psi 2021-02-12T23:28:28Z- handleDepth()-Depth_Keller msg: 1613172508201 epoch sec; seqNo:4070; depth 16.36 2021-02-12T23:28:28Z- handleDvl() - RDI_Pathfinder msg: 1613172508.21 epoch sec; seqNo:1954 2021-02-12T23:28:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 92.03 , 70.69 , 96.90 2021-02-12T23:28:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.18 , 0.05 2021-02-12T23:28:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172508.22 epoch sec; seqNo:4070; 4057068.75 north; 591014.74 east 2021-02-12T23:28:28Z- AHRS_M2 msg: 1613172508.57 epoch sec; seqNo:4030 2021-02-12T23:28:28Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.16 psi 2021-02-12T23:28:28Z- handleDepth()-Depth_Keller msg: 1613172508580 epoch sec; seqNo:4071; depth 16.48 2021-02-12T23:28:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172508.60 epoch sec; seqNo:4071; 4057068.75 north; 591014.74 east 2021-02-12T23:28:28Z- AHRS_M2 msg: 1613172508.98 epoch sec; seqNo:4031 2021-02-12T23:28:28Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.15 psi 2021-02-12T23:28:28Z- handleDepth()-Depth_Keller msg: 1613172508981 epoch sec; seqNo:4072; depth 16.57 2021-02-12T23:28:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172508.99 epoch sec; seqNo:4072; 4057069.13 north; 591014.74 east 2021-02-12T23:28:29Z- AHRS_M2 msg: 1613172509.38 epoch sec; seqNo:4032 2021-02-12T23:28:29Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.15 psi 2021-02-12T23:28:29Z- handleDepth()-Depth_Keller msg: 1613172509386 epoch sec; seqNo:4073; depth 16.67 2021-02-12T23:28:29Z- handleDvl() - RDI_Pathfinder msg: 1613172509.40 epoch sec; seqNo:1955 2021-02-12T23:28:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.23 , 90.75 , 70.28 , 100.48 2021-02-12T23:28:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.11 , 0.11 2021-02-12T23:28:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172509.41 epoch sec; seqNo:4073; 4057069.51 north; 591014.73 east 2021-02-12T23:28:29Z- AHRS_M2 msg: 1613172509.78 epoch sec; seqNo:4033 2021-02-12T23:28:29Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.15 psi 2021-02-12T23:28:29Z- handleDepth()-Depth_Keller msg: 1613172509789 epoch sec; seqNo:4074; depth 16.78 2021-02-12T23:28:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172509.80 epoch sec; seqNo:4074; 4057069.51 north; 591014.73 east 2021-02-12T23:28:30Z- AHRS_M2 msg: 1613172510.20 epoch sec; seqNo:4034 2021-02-12T23:28:30Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.15 psi 2021-02-12T23:28:30Z- handleDepth()-Depth_Keller msg: 1613172510201 epoch sec; seqNo:4075; depth 16.88 2021-02-12T23:28:30Z- handleDvl() - RDI_Pathfinder msg: 1613172510.21 epoch sec; seqNo:1956 2021-02-12T23:28:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.23 , 90.88 , 70.69 , 99.40 2021-02-12T23:28:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , 0.12 2021-02-12T23:28:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172510.22 epoch sec; seqNo:4075; 4057069.89 north; 591014.73 east 2021-02-12T23:28:30Z- AHRS_M2 msg: 1613172510.59 epoch sec; seqNo:4035 2021-02-12T23:28:30Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.16 psi 2021-02-12T23:28:30Z- handleDepth()-Depth_Keller msg: 1613172510599 epoch sec; seqNo:4076; depth 16.99 2021-02-12T23:28:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:30Z- handleDvl() - RDI_Pathfinder msg: 1613172510.60 epoch sec; seqNo:1957 2021-02-12T23:28:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , 0.12 2021-02-12T23:28:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172510.61 epoch sec; seqNo:4076; 4057070.28 north; 591015.94 east 2021-02-12T23:28:31Z- AHRS_M2 msg: 1613172511.00 epoch sec; seqNo:4036 2021-02-12T23:28:31Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.16 psi 2021-02-12T23:28:31Z- handleDepth()-Depth_Keller msg: 1613172511037 epoch sec; seqNo:4077; depth 17.08 2021-02-12T23:28:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172511.06 epoch sec; seqNo:4077; 4057070.28 north; 591015.94 east 2021-02-12T23:28:31Z- AHRS_M2 msg: 1613172511.40 epoch sec; seqNo:4037 2021-02-12T23:28:31Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.16 psi 2021-02-12T23:28:31Z- handleDepth()-Depth_Keller msg: 1613172511404 epoch sec; seqNo:4078; depth 17.19 2021-02-12T23:28:31Z- handleDvl() - RDI_Pathfinder msg: 1613172511.42 epoch sec; seqNo:1958 2021-02-12T23:28:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:31Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:28:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172511.45 epoch sec; seqNo:4078; 4057070.66 north; 591015.94 east 2021-02-12T23:28:31Z- LcmTrn::handleMessages() - handling 47 messages took 0.66 ms and 134 calls... 2021-02-12T23:28:31Z- waiting for 1613172509.16, time is 1613172511.45 2021-02-12T23:28:31Z- LcmTrn::updateTrn() equating pose 1613172511.45 and meas times 1613172511.45 2021-02-12T23:28:31Z- LcmTrn::updateTrn() >>>> pose time 1613172511.45, meas time 1613172511.45 <<<< 2021-02-12T23:28:31Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172511.45, ping # 0. 2021-02-12T23:28:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172511.45 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:28:31Z- TerrainNav::motionUpdate() - currEstimate = (4057071, 591015.94, 17.19) 2021-02-12T23:28:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:28:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:28:31Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:28:31Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:28:31Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172511.45 sec.; No relevant navigation data available 2021-02-12T23:28:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:28:31Z- TNavBF::mmse selected from filter -1 2021-02-12T23:28:31Z- variances([0], [2], and psi_berg) = 762.250858 833.045391 0.000000 2021-02-12T23:28:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:28:31Z- LcmTrn::publishEstimates() - MLE : 4057053.16 590969.44 17.19 2021-02-12T23:28:31Z- LcmTrn::publishEstimates() - MMSE : 4057070.66 591015.94 17.19 2021-02-12T23:28:31Z- LcmTrn::publishEstimates() - COVAR: 762.25 833.05 2.03 0.00 2021-02-12T23:28:31Z- AHRS_M2 msg: 1613172511.80 epoch sec; seqNo:4038 2021-02-12T23:28:31Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.17 psi 2021-02-12T23:28:31Z- handleDepth()-Depth_Keller msg: 1613172511809 epoch sec; seqNo:4079; depth 17.29 2021-02-12T23:28:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:31Z- handleDvl() - RDI_Pathfinder msg: 1613172511.81 epoch sec; seqNo:1959 2021-02-12T23:28:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:31Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:28:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172511.82 epoch sec; seqNo:4079; 4057071.04 north; 591015.94 east 2021-02-12T23:28:32Z- AHRS_M2 msg: 1613172512.21 epoch sec; seqNo:4039 2021-02-12T23:28:32Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.16 psi 2021-02-12T23:28:32Z- handleDepth()-Depth_Keller msg: 1613172512214 epoch sec; seqNo:4080; depth 17.39 2021-02-12T23:28:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172512.22 epoch sec; seqNo:4080; 4057071.04 north; 591015.94 east 2021-02-12T23:28:32Z- AHRS_M2 msg: 1613172512.61 epoch sec; seqNo:4040 2021-02-12T23:28:32Z- AHRS_M2 msg: 0.66 phi; -0.31 theta; -0.16 psi 2021-02-12T23:28:32Z- handleDepth()-Depth_Keller msg: 1613172512616 epoch sec; seqNo:4081; depth 17.50 2021-02-12T23:28:32Z- handleDvl() - RDI_Pathfinder msg: 1613172512.63 epoch sec; seqNo:1960 2021-02-12T23:28:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.41 , 93.18 , 69.06 , 103.59 2021-02-12T23:28:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , 0.04 2021-02-12T23:28:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172512.64 epoch sec; seqNo:4081; 4057071.42 north; 591015.93 east 2021-02-12T23:28:33Z- AHRS_M2 msg: 1613172513.02 epoch sec; seqNo:4041 2021-02-12T23:28:33Z- AHRS_M2 msg: 0.66 phi; -0.31 theta; -0.16 psi 2021-02-12T23:28:33Z- handleDepth()-Depth_Keller msg: 1613172513025 epoch sec; seqNo:4082; depth 17.62 2021-02-12T23:28:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:33Z- handleDvl() - RDI_Pathfinder msg: 1613172513.03 epoch sec; seqNo:1961 2021-02-12T23:28:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , 0.04 2021-02-12T23:28:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172513.05 epoch sec; seqNo:4082; 4057071.80 north; 591015.93 east 2021-02-12T23:28:33Z- AHRS_M2 msg: 1613172513.42 epoch sec; seqNo:4042 2021-02-12T23:28:33Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.17 psi 2021-02-12T23:28:33Z- handleDepth()-Depth_Keller msg: 1613172513429 epoch sec; seqNo:4083; depth 17.72 2021-02-12T23:28:33Z- handleDvl() - RDI_Pathfinder msg: 1613172513.44 epoch sec; seqNo:1962 2021-02-12T23:28:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 92.03 , 69.46 , 102.98 2021-02-12T23:28:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.02 , -0.01 2021-02-12T23:28:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172513.47 epoch sec; seqNo:4083; 4057072.19 north; 591017.14 east 2021-02-12T23:28:33Z- AHRS_M2 msg: 1613172513.83 epoch sec; seqNo:4043 2021-02-12T23:28:33Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.18 psi 2021-02-12T23:28:33Z- handleDepth()-Depth_Keller msg: 1613172513829 epoch sec; seqNo:4084; depth 17.83 2021-02-12T23:28:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172513.84 epoch sec; seqNo:4084; 4057072.19 north; 591017.14 east 2021-02-12T23:28:34Z- AHRS_M2 msg: 1613172514.23 epoch sec; seqNo:4044 2021-02-12T23:28:34Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.18 psi 2021-02-12T23:28:34Z- handleDepth()-Depth_Keller msg: 1613172514233 epoch sec; seqNo:4085; depth 17.95 2021-02-12T23:28:34Z- handleDvl() - RDI_Pathfinder msg: 1613172514.25 epoch sec; seqNo:1963 2021-02-12T23:28:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.59 , 93.25 , 69.06 , 100.55 2021-02-12T23:28:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , -0.01 2021-02-12T23:28:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172514.26 epoch sec; seqNo:4085; 4057072.57 north; 591017.14 east 2021-02-12T23:28:34Z- AHRS_M2 msg: 1613172514.63 epoch sec; seqNo:4045 2021-02-12T23:28:34Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.18 psi 2021-02-12T23:28:34Z- handleDepth()-Depth_Keller msg: 1613172514636 epoch sec; seqNo:4086; depth 18.07 2021-02-12T23:28:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172514.65 epoch sec; seqNo:4086; 4057072.95 north; 591017.14 east 2021-02-12T23:28:35Z- AHRS_M2 msg: 1613172515.04 epoch sec; seqNo:4046 2021-02-12T23:28:35Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.18 psi 2021-02-12T23:28:35Z- handleDepth()-Depth_Keller msg: 1613172515040 epoch sec; seqNo:4087; depth 18.18 2021-02-12T23:28:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172515.05 epoch sec; seqNo:4087; 4057073.33 north; 591017.13 east 2021-02-12T23:28:35Z- AHRS_M2 msg: 1613172515.44 epoch sec; seqNo:4047 2021-02-12T23:28:35Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.18 psi 2021-02-12T23:28:35Z- handleDepth()-Depth_Keller msg: 1613172515446 epoch sec; seqNo:4088; depth 18.28 2021-02-12T23:28:35Z- handleDvl() - RDI_Pathfinder msg: 1613172515.46 epoch sec; seqNo:1964 2021-02-12T23:28:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.78 , 92.17 , 69.06 , 97.03 2021-02-12T23:28:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , 0.00 2021-02-12T23:28:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172515.47 epoch sec; seqNo:4088; 4057073.33 north; 591017.13 east 2021-02-12T23:28:35Z- AHRS_M2 msg: 1613172515.84 epoch sec; seqNo:4048 2021-02-12T23:28:35Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.18 psi 2021-02-12T23:28:35Z- handleDepth()-Depth_Keller msg: 1613172515848 epoch sec; seqNo:4089; depth 18.41 2021-02-12T23:28:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172515.87 epoch sec; seqNo:4089; 4057073.72 north; 591018.35 east 2021-02-12T23:28:36Z- AHRS_M2 msg: 1613172516.26 epoch sec; seqNo:4049 2021-02-12T23:28:36Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.18 psi 2021-02-12T23:28:36Z- handleDepth()-Depth_Keller msg: 1613172516268 epoch sec; seqNo:4090; depth 18.52 2021-02-12T23:28:36Z- handleDvl() - RDI_Pathfinder msg: 1613172516.29 epoch sec; seqNo:1965 2021-02-12T23:28:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.78 , 98.25 , 68.24 , 99.46 2021-02-12T23:28:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.02 , 0.03 2021-02-12T23:28:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172516.30 epoch sec; seqNo:4090; 4057074.10 north; 591018.35 east 2021-02-12T23:28:36Z- LcmTrn::handleMessages() - handling 43 messages took 0.69 ms and 130 calls... 2021-02-12T23:28:36Z- waiting for 1613172514.45, time is 1613172516.30 2021-02-12T23:28:36Z- LcmTrn::updateTrn() equating pose 1613172516.30 and meas times 1613172516.30 2021-02-12T23:28:36Z- LcmTrn::updateTrn() >>>> pose time 1613172516.30, meas time 1613172516.30 <<<< 2021-02-12T23:28:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172516.30 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:28:36Z- TerrainNav::motionUpdate() - currEstimate = (4057074, 591018.35, 18.52) 2021-02-12T23:28:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:28:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:28:36Z- TNavBF::Projecting Measurements 2021-02-12T23:28:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:28:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:28:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:28:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:28:36Z- TNavFilter::4 good beams. 2021-02-12T23:28:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:28:36Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:28:36Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:28:36Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:28:36Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:28:36Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:28:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:28:36Z- TNavBF::mmse selected from filter -1 2021-02-12T23:28:36Z- TerrainMapOctree: (vehN, vehE ) = (4057074.10, 591018.35). 2021-02-12T23:28:36Z- TerrainMapOctree: Min Distance = 5280.32. 2021-02-12T23:28:36Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:28:36Z- There is no specific good beam that all particles have in common. 2021-02-12T23:28:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:28:36Z- Old Depth Bias: 0.00 DepthBiasCov: 2.24 K: 0.161 ExpectedMeasDiff: nan currMeas.covariance: 9.93 2021-02-12T23:28:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:28:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.88 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 15.44 2021-02-12T23:28:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:28:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.69 K: 0.156 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-12T23:28:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:28:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 15.83 2021-02-12T23:28:36Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:28:36Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:28:36Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:28:36Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:28:36Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:28:36Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:28:36Z- Filter 0 No beams used in subcloud update 2021-02-12T23:28:36Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:28:36Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:28:36Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 762.1 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-12T23:28:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172516.30 sec. 2021-02-12T23:28:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:28:36Z- TNavBF::mmse selected from filter -1 2021-02-12T23:28:36Z- variances([0], [2], and psi_berg) = 762.132733 833.045262 0.000000 2021-02-12T23:28:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:28:36Z- LcmTrn::publishEstimates() - MLE : 4057056.60 590971.85 18.52 2021-02-12T23:28:36Z- LcmTrn::publishEstimates() - MMSE : 4057074.10 591018.35 18.52 2021-02-12T23:28:36Z- LcmTrn::publishEstimates() - COVAR: 762.13 833.05 1.32 0.00 2021-02-12T23:28:36Z- AHRS_M2 msg: 1613172516.65 epoch sec; seqNo:4050 2021-02-12T23:28:36Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.18 psi 2021-02-12T23:28:36Z- handleDepth()-Depth_Keller msg: 1613172516659 epoch sec; seqNo:4091; depth 18.64 2021-02-12T23:28:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:36Z- handleDvl() - RDI_Pathfinder msg: 1613172516.66 epoch sec; seqNo:1966 2021-02-12T23:28:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.02 , 0.03 2021-02-12T23:28:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172516.67 epoch sec; seqNo:4091; 4057074.10 north; 591018.35 east 2021-02-12T23:28:37Z- AHRS_M2 msg: 1613172517.06 epoch sec; seqNo:4051 2021-02-12T23:28:37Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.18 psi 2021-02-12T23:28:37Z- handleDepth()-Depth_Keller msg: 1613172517069 epoch sec; seqNo:4092; depth 18.73 2021-02-12T23:28:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172517.08 epoch sec; seqNo:4092; 4057074.48 north; 591018.34 east 2021-02-12T23:28:37Z- AHRS_M2 msg: 1613172517.46 epoch sec; seqNo:4052 2021-02-12T23:28:37Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.18 psi 2021-02-12T23:28:37Z- handleDepth()-Depth_Keller msg: 1613172517464 epoch sec; seqNo:4093; depth 18.84 2021-02-12T23:28:37Z- handleDvl() - RDI_Pathfinder msg: 1613172517.49 epoch sec; seqNo:1967 2021-02-12T23:28:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.78 , 92.04 , 69.06 , 101.77 2021-02-12T23:28:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.07 , -0.02 2021-02-12T23:28:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172517.50 epoch sec; seqNo:4093; 4057074.85 north; 591018.34 east 2021-02-12T23:28:37Z- AHRS_M2 msg: 1613172517.86 epoch sec; seqNo:4053 2021-02-12T23:28:37Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.18 psi 2021-02-12T23:28:37Z- handleDepth()-Depth_Keller msg: 1613172517868 epoch sec; seqNo:4094; depth 18.95 2021-02-12T23:28:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:37Z- handleDvl() - RDI_Pathfinder msg: 1613172517.87 epoch sec; seqNo:1968 2021-02-12T23:28:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.07 , -0.02 2021-02-12T23:28:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172517.89 epoch sec; seqNo:4094; 4057074.87 north; 591019.56 east 2021-02-12T23:28:38Z- AHRS_M2 msg: 1613172518.27 epoch sec; seqNo:4054 2021-02-12T23:28:38Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.19 psi 2021-02-12T23:28:38Z- handleDepth()-Depth_Keller msg: 1613172518278 epoch sec; seqNo:4095; depth 19.06 2021-02-12T23:28:38Z- handleDvl() - RDI_Pathfinder msg: 1613172518.29 epoch sec; seqNo:1969 2021-02-12T23:28:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 92.17 , 67.84 , 103.12 2021-02-12T23:28:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.04 , -0.08 2021-02-12T23:28:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172518.30 epoch sec; seqNo:4095; 4057075.25 north; 591019.55 east 2021-02-12T23:28:38Z- AHRS_M2 msg: 1613172518.67 epoch sec; seqNo:4055 2021-02-12T23:28:38Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.19 psi 2021-02-12T23:28:38Z- handleDepth()-Depth_Keller msg: 1613172518689 epoch sec; seqNo:4096; depth 19.17 2021-02-12T23:28:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172518.70 epoch sec; seqNo:4096; 4057075.62 north; 591019.55 east 2021-02-12T23:28:39Z- AHRS_M2 msg: 1613172519.08 epoch sec; seqNo:4056 2021-02-12T23:28:39Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.20 psi 2021-02-12T23:28:39Z- handleDepth()-Depth_Keller msg: 1613172519085 epoch sec; seqNo:4097; depth 19.28 2021-02-12T23:28:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:39Z- handleDvl() - RDI_Pathfinder msg: 1613172519.09 epoch sec; seqNo:1970 2021-02-12T23:28:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.04 , -0.08 2021-02-12T23:28:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172519.11 epoch sec; seqNo:4097; 4057075.62 north; 591019.55 east 2021-02-12T23:28:39Z- AHRS_M2 msg: 1613172519.48 epoch sec; seqNo:4057 2021-02-12T23:28:39Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.20 psi 2021-02-12T23:28:39Z- handleDepth()-Depth_Keller msg: 1613172519484 epoch sec; seqNo:4098; depth 19.40 2021-02-12T23:28:39Z- handleDvl() - RDI_Pathfinder msg: 1613172519.49 epoch sec; seqNo:1971 2021-02-12T23:28:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.14 , 92.17 , 67.83 , 99.46 2021-02-12T23:28:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , -0.11 2021-02-12T23:28:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172519.51 epoch sec; seqNo:4098; 4057076.00 north; 591019.55 east 2021-02-12T23:28:39Z- AHRS_M2 msg: 1613172519.88 epoch sec; seqNo:4058 2021-02-12T23:28:39Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.20 psi 2021-02-12T23:28:39Z- handleDepth()-Depth_Keller msg: 1613172519889 epoch sec; seqNo:4099; depth 19.52 2021-02-12T23:28:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172519.91 epoch sec; seqNo:4099; 4057076.38 north; 591019.54 east 2021-02-12T23:28:40Z- AHRS_M2 msg: 1613172520.29 epoch sec; seqNo:4059 2021-02-12T23:28:40Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.20 psi 2021-02-12T23:28:40Z- handleDepth()-Depth_Keller msg: 1613172520294 epoch sec; seqNo:4100; depth 19.63 2021-02-12T23:28:40Z- handleDvl() - RDI_Pathfinder msg: 1613172520.31 epoch sec; seqNo:1972 2021-02-12T23:28:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 92.23 , 67.01 , 100.75 2021-02-12T23:28:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , -0.07 2021-02-12T23:28:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172520.32 epoch sec; seqNo:4100; 4057076.40 north; 591020.76 east 2021-02-12T23:28:40Z- AHRS_M2 msg: 1613172520.69 epoch sec; seqNo:4060 2021-02-12T23:28:40Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.19 psi 2021-02-12T23:28:40Z- handleDepth()-Depth_Keller msg: 1613172520696 epoch sec; seqNo:4101; depth 19.73 2021-02-12T23:28:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172520.71 epoch sec; seqNo:4101; 4057076.77 north; 591020.76 east 2021-02-12T23:28:41Z- AHRS_M2 msg: 1613172521.10 epoch sec; seqNo:4061 2021-02-12T23:28:41Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.19 psi 2021-02-12T23:28:41Z- handleDepth()-Depth_Keller msg: 1613172521105 epoch sec; seqNo:4102; depth 19.85 2021-02-12T23:28:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172521.12 epoch sec; seqNo:4102; 4057077.15 north; 591020.76 east 2021-02-12T23:28:41Z- AHRS_M2 msg: 1613172521.50 epoch sec; seqNo:4062 2021-02-12T23:28:41Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.18 psi 2021-02-12T23:28:41Z- handleDepth()-Depth_Keller msg: 1613172521505 epoch sec; seqNo:4103; depth 19.95 2021-02-12T23:28:41Z- handleDvl() - RDI_Pathfinder msg: 1613172521.52 epoch sec; seqNo:1973 2021-02-12T23:28:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 90.88 , 67.83 , 96.96 2021-02-12T23:28:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , -0.02 2021-02-12T23:28:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172521.53 epoch sec; seqNo:4103; 4057077.53 north; 591020.75 east 2021-02-12T23:28:41Z- LcmTrn::handleMessages() - handling 47 messages took 0.69 ms and 133 calls... 2021-02-12T23:28:41Z- waiting for 1613172519.30, time is 1613172521.53 2021-02-12T23:28:41Z- LcmTrn::updateTrn() equating pose 1613172521.53 and meas times 1613172521.53 2021-02-12T23:28:41Z- LcmTrn::updateTrn() >>>> pose time 1613172521.53, meas time 1613172521.53 <<<< 2021-02-12T23:28:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172521.53 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:28:41Z- TerrainNav::motionUpdate() - currEstimate = (4057078, 591020.75, 19.95) 2021-02-12T23:28:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:28:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:28:41Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172521.53 sec.; No relevant navigation data available 2021-02-12T23:28:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:28:41Z- TNavBF::mmse selected from filter -1 2021-02-12T23:28:41Z- variances([0], [2], and psi_berg) = 762.014812 833.045133 0.000000 2021-02-12T23:28:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:28:41Z- LcmTrn::publishEstimates() - MLE : 4057060.03 590972.25 19.95 2021-02-12T23:28:41Z- LcmTrn::publishEstimates() - MMSE : 4057077.53 591020.75 19.95 2021-02-12T23:28:41Z- LcmTrn::publishEstimates() - COVAR: 762.01 833.05 1.54 0.00 2021-02-12T23:28:41Z- AHRS_M2 msg: 1613172521.90 epoch sec; seqNo:4063 2021-02-12T23:28:41Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.18 psi 2021-02-12T23:28:41Z- handleDepth()-Depth_Keller msg: 1613172521909 epoch sec; seqNo:4104; depth 20.08 2021-02-12T23:28:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172521.94 epoch sec; seqNo:4104; 4057077.91 north; 591020.75 east 2021-02-12T23:28:42Z- AHRS_M2 msg: 1613172522.31 epoch sec; seqNo:4064 2021-02-12T23:28:42Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.18 psi 2021-02-12T23:28:42Z- handleDepth()-Depth_Keller msg: 1613172522312 epoch sec; seqNo:4105; depth 20.19 2021-02-12T23:28:42Z- handleDvl() - RDI_Pathfinder msg: 1613172522.32 epoch sec; seqNo:1974 2021-02-12T23:28:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.14 , 92.23 , 67.01 , 97.10 2021-02-12T23:28:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , 0.03 2021-02-12T23:28:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172522.33 epoch sec; seqNo:4105; 4057077.91 north; 591020.75 east 2021-02-12T23:28:42Z- AHRS_M2 msg: 1613172522.71 epoch sec; seqNo:4065 2021-02-12T23:28:42Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.18 psi 2021-02-12T23:28:42Z- handleDepth()-Depth_Keller msg: 1613172522717 epoch sec; seqNo:4106; depth 20.32 2021-02-12T23:28:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:42Z- handleDvl() - RDI_Pathfinder msg: 1613172522.72 epoch sec; seqNo:1975 2021-02-12T23:28:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , 0.03 2021-02-12T23:28:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172522.73 epoch sec; seqNo:4106; 4057078.29 north; 591020.74 east 2021-02-12T23:28:43Z- AHRS_M2 msg: 1613172523.12 epoch sec; seqNo:4066 2021-02-12T23:28:43Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.18 psi 2021-02-12T23:28:43Z- handleDepth()-Depth_Keller msg: 1613172523129 epoch sec; seqNo:4107; depth 20.42 2021-02-12T23:28:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172523.15 epoch sec; seqNo:4107; 4057078.68 north; 591021.96 east 2021-02-12T23:28:43Z- AHRS_M2 msg: 1613172523.52 epoch sec; seqNo:4067 2021-02-12T23:28:43Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.18 psi 2021-02-12T23:28:43Z- handleDepth()-Depth_Keller msg: 1613172523529 epoch sec; seqNo:4108; depth 20.52 2021-02-12T23:28:43Z- handleDvl() - RDI_Pathfinder msg: 1613172523.54 epoch sec; seqNo:1976 2021-02-12T23:28:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.32 , 89.87 , 67.01 , 98.38 2021-02-12T23:28:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.05 2021-02-12T23:28:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172523.56 epoch sec; seqNo:4108; 4057078.68 north; 591021.96 east 2021-02-12T23:28:43Z- AHRS_M2 msg: 1613172523.92 epoch sec; seqNo:4068 2021-02-12T23:28:43Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.19 psi 2021-02-12T23:28:43Z- handleDepth()-Depth_Keller msg: 1613172523941 epoch sec; seqNo:4109; depth 20.65 2021-02-12T23:28:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:43Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:43Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:43Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:43Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:43Z- handleDvl() - RDI_Pathfinder msg: 1613172523.94 epoch sec; seqNo:1977 2021-02-12T23:28:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.05 2021-02-12T23:28:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172523.95 epoch sec; seqNo:4109; 4057079.06 north; 591021.96 east 2021-02-12T23:28:44Z- AHRS_M2 msg: 1613172524.33 epoch sec; seqNo:4069 2021-02-12T23:28:44Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.20 psi 2021-02-12T23:28:44Z- handleDepth()-Depth_Keller msg: 1613172524334 epoch sec; seqNo:4110; depth 20.77 2021-02-12T23:28:44Z- handleDvl() - RDI_Pathfinder msg: 1613172524.35 epoch sec; seqNo:1978 2021-02-12T23:28:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.32 , 90.21 , 65.38 , 96.69 2021-02-12T23:28:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.05 2021-02-12T23:28:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172524.36 epoch sec; seqNo:4110; 4057079.44 north; 591021.95 east 2021-02-12T23:28:44Z- AHRS_M2 msg: 1613172524.73 epoch sec; seqNo:4070 2021-02-12T23:28:44Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.21 psi 2021-02-12T23:28:44Z- handleDepth()-Depth_Keller msg: 1613172524745 epoch sec; seqNo:4111; depth 20.87 2021-02-12T23:28:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172524.77 epoch sec; seqNo:4111; 4057079.82 north; 591021.95 east 2021-02-12T23:28:45Z- AHRS_M2 msg: 1613172525.14 epoch sec; seqNo:4071 2021-02-12T23:28:45Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.21 psi 2021-02-12T23:28:45Z- handleDepth()-Depth_Keller msg: 1613172525144 epoch sec; seqNo:4112; depth 21.00 2021-02-12T23:28:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:45Z- handleDvl() - RDI_Pathfinder msg: 1613172525.15 epoch sec; seqNo:1979 2021-02-12T23:28:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.05 2021-02-12T23:28:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172525.15 epoch sec; seqNo:4112; 4057079.82 north; 591021.95 east 2021-02-12T23:28:45Z- AHRS_M2 msg: 1613172525.54 epoch sec; seqNo:4072 2021-02-12T23:28:45Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.22 psi 2021-02-12T23:28:45Z- handleDepth()-Depth_Keller msg: 1613172525546 epoch sec; seqNo:4113; depth 21.10 2021-02-12T23:28:45Z- handleDvl() - RDI_Pathfinder msg: 1613172525.56 epoch sec; seqNo:1980 2021-02-12T23:28:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 91.36 , 66.60 , 97.84 2021-02-12T23:28:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , 0.07 2021-02-12T23:28:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172525.57 epoch sec; seqNo:4113; 4057080.21 north; 591023.17 east 2021-02-12T23:28:45Z- AHRS_M2 msg: 1613172525.94 epoch sec; seqNo:4073 2021-02-12T23:28:45Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.23 psi 2021-02-12T23:28:45Z- handleDepth()-Depth_Keller msg: 1613172525949 epoch sec; seqNo:4114; depth 21.24 2021-02-12T23:28:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172525.97 epoch sec; seqNo:4114; 4057080.59 north; 591023.16 east 2021-02-12T23:28:46Z- AHRS_M2 msg: 1613172526.35 epoch sec; seqNo:4074 2021-02-12T23:28:46Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.23 psi 2021-02-12T23:28:46Z- handleDepth()-Depth_Keller msg: 1613172526352 epoch sec; seqNo:4115; depth 21.34 2021-02-12T23:28:46Z- handleDvl() - RDI_Pathfinder msg: 1613172526.36 epoch sec; seqNo:1981 2021-02-12T23:28:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.28 , 92.37 , 66.60 , 96.69 2021-02-12T23:28:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.08 2021-02-12T23:28:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172526.39 epoch sec; seqNo:4115; 4057080.59 north; 591023.16 east 2021-02-12T23:28:46Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-12T23:28:46Z- waiting for 1613172524.53, time is 1613172526.39 2021-02-12T23:28:46Z- LcmTrn::updateTrn() equating pose 1613172526.39 and meas times 1613172526.39 2021-02-12T23:28:46Z- LcmTrn::updateTrn() >>>> pose time 1613172526.39, meas time 1613172526.39 <<<< 2021-02-12T23:28:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172526.39 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:28:46Z- TerrainNav::motionUpdate() - currEstimate = (4057081, 591023.16, 21.34) 2021-02-12T23:28:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:28:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:28:46Z- TNavBF::Projecting Measurements 2021-02-12T23:28:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:28:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:28:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:28:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:28:46Z- TNavFilter::4 good beams. 2021-02-12T23:28:46Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:28:46Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:28:46Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:28:46Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:28:46Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:28:46Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:28:46Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:28:46Z- TNavBF::mmse selected from filter -1 2021-02-12T23:28:46Z- TerrainMapOctree: (vehN, vehE ) = (4057080.59, 591023.16). 2021-02-12T23:28:46Z- TerrainMapOctree: Min Distance = 5272.24. 2021-02-12T23:28:46Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:28:46Z- There is no specific good beam that all particles have in common. 2021-02-12T23:28:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:28:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.73 K: 0.153 ExpectedMeasDiff: nan currMeas.covariance: 7.90 2021-02-12T23:28:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:28:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.47 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 13.65 2021-02-12T23:28:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:28:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.132 ExpectedMeasDiff: nan currMeas.covariance: 7.10 2021-02-12T23:28:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:28:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 14.96 2021-02-12T23:28:46Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:28:46Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:28:46Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:28:46Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:28:46Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:28:46Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:28:46Z- Filter 0 No beams used in subcloud update 2021-02-12T23:28:46Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:28:46Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:28:46Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 761.9 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:28:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172526.39 sec. 2021-02-12T23:28:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:28:46Z- TNavBF::mmse selected from filter -1 2021-02-12T23:28:46Z- variances([0], [2], and psi_berg) = 761.901405 833.045010 0.000000 2021-02-12T23:28:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:28:46Z- LcmTrn::publishEstimates() - MLE : 4057063.09 590974.66 21.34 2021-02-12T23:28:46Z- LcmTrn::publishEstimates() - MMSE : 4057080.59 591023.16 21.34 2021-02-12T23:28:46Z- LcmTrn::publishEstimates() - COVAR: 761.90 833.05 1.09 0.00 2021-02-12T23:28:46Z- AHRS_M2 msg: 1613172526.75 epoch sec; seqNo:4075 2021-02-12T23:28:46Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.23 psi 2021-02-12T23:28:46Z- handleDepth()-Depth_Keller msg: 1613172526756 epoch sec; seqNo:4116; depth 21.47 2021-02-12T23:28:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172526.77 epoch sec; seqNo:4116; 4057080.97 north; 591023.16 east 2021-02-12T23:28:47Z- AHRS_M2 msg: 1613172527.17 epoch sec; seqNo:4076 2021-02-12T23:28:47Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.23 psi 2021-02-12T23:28:47Z- handleDepth()-Depth_Keller msg: 1613172527195 epoch sec; seqNo:4117; depth 21.58 2021-02-12T23:28:47Z- handleDvl() - RDI_Pathfinder msg: 1613172527.23 epoch sec; seqNo:1982 2021-02-12T23:28:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.69 , 91.36 , 65.38 , 96.76 2021-02-12T23:28:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , 0.09 2021-02-12T23:28:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172527.27 epoch sec; seqNo:4117; 4057081.35 north; 591023.15 east 2021-02-12T23:28:47Z- AHRS_M2 msg: 1613172527.56 epoch sec; seqNo:4077 2021-02-12T23:28:47Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.23 psi 2021-02-12T23:28:47Z- handleDepth()-Depth_Keller msg: 1613172527569 epoch sec; seqNo:4118; depth 21.71 2021-02-12T23:28:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:47Z- handleDvl() - RDI_Pathfinder msg: 1613172527.57 epoch sec; seqNo:1983 2021-02-12T23:28:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , 0.09 2021-02-12T23:28:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172527.59 epoch sec; seqNo:4118; 4057081.35 north; 591023.15 east 2021-02-12T23:28:47Z- AHRS_M2 msg: 1613172527.96 epoch sec; seqNo:4078 2021-02-12T23:28:47Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.22 psi 2021-02-12T23:28:47Z- handleDepth()-Depth_Keller msg: 1613172527968 epoch sec; seqNo:4119; depth 21.83 2021-02-12T23:28:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172527.98 epoch sec; seqNo:4119; 4057081.74 north; 591024.37 east 2021-02-12T23:28:48Z- AHRS_M2 msg: 1613172528.37 epoch sec; seqNo:4079 2021-02-12T23:28:48Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.22 psi 2021-02-12T23:28:48Z- handleDepth()-Depth_Keller msg: 1613172528377 epoch sec; seqNo:4120; depth 21.96 2021-02-12T23:28:48Z- handleDvl() - RDI_Pathfinder msg: 1613172528.39 epoch sec; seqNo:1984 2021-02-12T23:28:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.69 , 92.44 , 66.19 , 97.84 2021-02-12T23:28:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.12 , 0.03 2021-02-12T23:28:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172528.40 epoch sec; seqNo:4120; 4057082.12 north; 591024.37 east 2021-02-12T23:28:48Z- AHRS_M2 msg: 1613172528.78 epoch sec; seqNo:4080 2021-02-12T23:28:48Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.21 psi 2021-02-12T23:28:48Z- handleDepth()-Depth_Keller msg: 1613172528781 epoch sec; seqNo:4121; depth 22.07 2021-02-12T23:28:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:48Z- handleDvl() - RDI_Pathfinder msg: 1613172528.78 epoch sec; seqNo:1985 2021-02-12T23:28:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.12 , 0.03 2021-02-12T23:28:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172528.79 epoch sec; seqNo:4121; 4057082.12 north; 591024.37 east 2021-02-12T23:28:49Z- AHRS_M2 msg: 1613172529.18 epoch sec; seqNo:4081 2021-02-12T23:28:49Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.21 psi 2021-02-12T23:28:49Z- handleDepth()-Depth_Keller msg: 1613172529218 epoch sec; seqNo:4122; depth 22.19 2021-02-12T23:28:49Z- handleDvl() - RDI_Pathfinder msg: 1613172529.24 epoch sec; seqNo:1986 2021-02-12T23:28:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.87 , 97.78 , 66.19 , 98.86 2021-02-12T23:28:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , -0.04 2021-02-12T23:28:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172529.25 epoch sec; seqNo:4122; 4057082.50 north; 591024.36 east 2021-02-12T23:28:49Z- AHRS_M2 msg: 1613172529.58 epoch sec; seqNo:4082 2021-02-12T23:28:49Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.21 psi 2021-02-12T23:28:49Z- handleDepth()-Depth_Keller msg: 1613172529589 epoch sec; seqNo:4123; depth 22.31 2021-02-12T23:28:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172529.60 epoch sec; seqNo:4123; 4057082.87 north; 591024.36 east 2021-02-12T23:28:49Z- AHRS_M2 msg: 1613172529.99 epoch sec; seqNo:4083 2021-02-12T23:28:49Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.21 psi 2021-02-12T23:28:49Z- handleDepth()-Depth_Keller msg: 1613172529993 epoch sec; seqNo:4124; depth 22.41 2021-02-12T23:28:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:49Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:49Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:49Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:49Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:49Z- handleDvl() - RDI_Pathfinder msg: 1613172529.99 epoch sec; seqNo:1987 2021-02-12T23:28:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , -0.04 2021-02-12T23:28:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172530.00 epoch sec; seqNo:4124; 4057083.25 north; 591024.35 east 2021-02-12T23:28:50Z- AHRS_M2 msg: 1613172530.39 epoch sec; seqNo:4084 2021-02-12T23:28:50Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.21 psi 2021-02-12T23:28:50Z- handleDepth()-Depth_Keller msg: 1613172530397 epoch sec; seqNo:4125; depth 22.55 2021-02-12T23:28:50Z- handleDvl() - RDI_Pathfinder msg: 1613172530.41 epoch sec; seqNo:1988 2021-02-12T23:28:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.28 , 91.02 , 64.56 , 98.32 2021-02-12T23:28:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.08 , -0.12 2021-02-12T23:28:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172530.43 epoch sec; seqNo:4125; 4057083.27 north; 591025.58 east 2021-02-12T23:28:50Z- AHRS_M2 msg: 1613172530.79 epoch sec; seqNo:4085 2021-02-12T23:28:50Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.21 psi 2021-02-12T23:28:50Z- handleDepth()-Depth_Keller msg: 1613172530797 epoch sec; seqNo:4126; depth 22.67 2021-02-12T23:28:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172530.81 epoch sec; seqNo:4126; 4057083.65 north; 591025.57 east 2021-02-12T23:28:51Z- AHRS_M2 msg: 1613172531.22 epoch sec; seqNo:4086 2021-02-12T23:28:51Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.21 psi 2021-02-12T23:28:51Z- handleDepth()-Depth_Keller msg: 1613172531222 epoch sec; seqNo:4127; depth 22.78 2021-02-12T23:28:51Z- handleDvl() - RDI_Pathfinder msg: 1613172531.23 epoch sec; seqNo:1989 2021-02-12T23:28:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.46 , 92.44 , 65.79 , 95.68 2021-02-12T23:28:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , -0.12 2021-02-12T23:28:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172531.24 epoch sec; seqNo:4127; 4057084.02 north; 591025.57 east 2021-02-12T23:28:51Z- AHRS_M2 msg: 1613172531.60 epoch sec; seqNo:4087 2021-02-12T23:28:51Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.21 psi 2021-02-12T23:28:51Z- handleDepth()-Depth_Keller msg: 1613172531609 epoch sec; seqNo:4128; depth 22.88 2021-02-12T23:28:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172531.62 epoch sec; seqNo:4128; 4057084.40 north; 591025.56 east 2021-02-12T23:28:51Z- LcmTrn::handleMessages() - handling 47 messages took 0.74 ms and 132 calls... 2021-02-12T23:28:51Z- waiting for 1613172529.39, time is 1613172531.62 2021-02-12T23:28:51Z- LcmTrn::updateTrn() >>>> pose time 1613172531.62, meas time 1613172531.23 <<<< 2021-02-12T23:28:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172531.23 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:28:51Z- TerrainNav::motionUpdate() - currEstimate = (4057084, 591025.56, 22.88) 2021-02-12T23:28:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:28:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:28:51Z- TNavBF::Projecting Measurements 2021-02-12T23:28:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:28:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:28:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:28:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:28:51Z- TNavFilter::4 good beams. 2021-02-12T23:28:51Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:28:51Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:28:51Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:28:51Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:28:51Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:28:51Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:28:51Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:28:51Z- TNavBF::mmse selected from filter -1 2021-02-12T23:28:51Z- TerrainMapOctree: (vehN, vehE ) = (4057084.12, 591025.39). 2021-02-12T23:28:51Z- TerrainMapOctree: Min Distance = 5268.08. 2021-02-12T23:28:51Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:28:51Z- There is no specific good beam that all particles have in common. 2021-02-12T23:28:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:28:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.121 ExpectedMeasDiff: nan currMeas.covariance: 7.72 2021-02-12T23:28:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:28:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 13.67 2021-02-12T23:28:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:28:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.110 ExpectedMeasDiff: nan currMeas.covariance: 6.92 2021-02-12T23:28:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:28:51Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 14.65 2021-02-12T23:28:51Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:28:51Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:28:51Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:28:51Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:28:51Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:28:51Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:28:51Z- Filter 0 No beams used in subcloud update 2021-02-12T23:28:51Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:28:51Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:28:51Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 761.8 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:28:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172531.23 sec. 2021-02-12T23:28:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:28:51Z- TNavBF::mmse selected from filter -1 2021-02-12T23:28:51Z- variances([0], [2], and psi_berg) = 761.751111 833.044846 0.000000 2021-02-12T23:28:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:28:51Z- LcmTrn::publishEstimates() - MLE : 4057066.90 590977.06 22.88 2021-02-12T23:28:51Z- LcmTrn::publishEstimates() - MMSE : 4057084.40 591025.56 22.88 2021-02-12T23:28:51Z- LcmTrn::publishEstimates() - COVAR: 761.75 833.04 0.91 0.00 2021-02-12T23:28:52Z- AHRS_M2 msg: 1613172532.01 epoch sec; seqNo:4088 2021-02-12T23:28:52Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.21 psi 2021-02-12T23:28:52Z- handleDepth()-Depth_Keller msg: 1613172532011 epoch sec; seqNo:4129; depth 23.01 2021-02-12T23:28:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172532.02 epoch sec; seqNo:4129; 4057084.78 north; 591025.56 east 2021-02-12T23:28:52Z- AHRS_M2 msg: 1613172532.41 epoch sec; seqNo:4089 2021-02-12T23:28:52Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.21 psi 2021-02-12T23:28:52Z- handleDepth()-Depth_Keller msg: 1613172532421 epoch sec; seqNo:4130; depth 23.12 2021-02-12T23:28:52Z- handleDvl() - RDI_Pathfinder msg: 1613172532.44 epoch sec; seqNo:1990 2021-02-12T23:28:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 93.52 , 65.38 , 97.84 2021-02-12T23:28:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.10 , -0.07 2021-02-12T23:28:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172532.45 epoch sec; seqNo:4130; 4057085.16 north; 591025.56 east 2021-02-12T23:28:52Z- AHRS_M2 msg: 1613172532.81 epoch sec; seqNo:4090 2021-02-12T23:28:52Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.21 psi 2021-02-12T23:28:52Z- handleDepth()-Depth_Keller msg: 1613172532816 epoch sec; seqNo:4131; depth 23.24 2021-02-12T23:28:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172532.84 epoch sec; seqNo:4131; 4057085.17 north; 591026.78 east 2021-02-12T23:28:53Z- AHRS_M2 msg: 1613172533.21 epoch sec; seqNo:4091 2021-02-12T23:28:53Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.21 psi 2021-02-12T23:28:53Z- handleDepth()-Depth_Keller msg: 1613172533221 epoch sec; seqNo:4132; depth 23.34 2021-02-12T23:28:53Z- handleDvl() - RDI_Pathfinder msg: 1613172533.23 epoch sec; seqNo:1991 2021-02-12T23:28:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 0.00 , 64.16 , 97.85 2021-02-12T23:28:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , -0.01 2021-02-12T23:28:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172533.26 epoch sec; seqNo:4132; 4057085.55 north; 591026.77 east 2021-02-12T23:28:53Z- AHRS_M2 msg: 1613172533.62 epoch sec; seqNo:4092 2021-02-12T23:28:53Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.21 psi 2021-02-12T23:28:53Z- handleDepth()-Depth_Keller msg: 1613172533624 epoch sec; seqNo:4133; depth 23.46 2021-02-12T23:28:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:53Z- handleDvl() - RDI_Pathfinder msg: 1613172533.63 epoch sec; seqNo:1992 2021-02-12T23:28:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , -0.01 2021-02-12T23:28:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172533.64 epoch sec; seqNo:4133; 4057085.93 north; 591026.77 east 2021-02-12T23:28:54Z- AHRS_M2 msg: 1613172534.03 epoch sec; seqNo:4093 2021-02-12T23:28:54Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.21 psi 2021-02-12T23:28:54Z- handleDepth()-Depth_Keller msg: 1613172534033 epoch sec; seqNo:4134; depth 23.60 2021-02-12T23:28:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172534.04 epoch sec; seqNo:4134; 4057086.31 north; 591026.76 east 2021-02-12T23:28:54Z- AHRS_M2 msg: 1613172534.43 epoch sec; seqNo:4094 2021-02-12T23:28:54Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.21 psi 2021-02-12T23:28:54Z- handleDepth()-Depth_Keller msg: 1613172534432 epoch sec; seqNo:4135; depth 23.72 2021-02-12T23:28:54Z- handleDvl() - RDI_Pathfinder msg: 1613172534.45 epoch sec; seqNo:1993 2021-02-12T23:28:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 90.41 , 64.16 , 96.90 2021-02-12T23:28:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , 0.01 2021-02-12T23:28:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172534.46 epoch sec; seqNo:4135; 4057086.31 north; 591026.76 east 2021-02-12T23:28:54Z- AHRS_M2 msg: 1613172534.83 epoch sec; seqNo:4095 2021-02-12T23:28:54Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.21 psi 2021-02-12T23:28:54Z- handleDepth()-Depth_Keller msg: 1613172534836 epoch sec; seqNo:4136; depth 23.83 2021-02-12T23:28:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:54Z- handleDvl() - RDI_Pathfinder msg: 1613172534.84 epoch sec; seqNo:1994 2021-02-12T23:28:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , 0.01 2021-02-12T23:28:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172534.85 epoch sec; seqNo:4136; 4057086.69 north; 591026.76 east 2021-02-12T23:28:55Z- AHRS_M2 msg: 1613172535.24 epoch sec; seqNo:4096 2021-02-12T23:28:55Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.20 psi 2021-02-12T23:28:55Z- handleDepth()-Depth_Keller msg: 1613172535247 epoch sec; seqNo:4137; depth 23.95 2021-02-12T23:28:55Z- handleDvl() - RDI_Pathfinder msg: 1613172535.26 epoch sec; seqNo:1995 2021-02-12T23:28:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 90.34 , 63.74 , 96.83 2021-02-12T23:28:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.03 2021-02-12T23:28:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172535.27 epoch sec; seqNo:4137; 4057087.07 north; 591026.76 east 2021-02-12T23:28:55Z- AHRS_M2 msg: 1613172535.64 epoch sec; seqNo:4097 2021-02-12T23:28:55Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.20 psi 2021-02-12T23:28:55Z- handleDepth()-Depth_Keller msg: 1613172535650 epoch sec; seqNo:4138; depth 24.05 2021-02-12T23:28:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172535.66 epoch sec; seqNo:4138; 4057087.45 north; 591026.75 east 2021-02-12T23:28:56Z- AHRS_M2 msg: 1613172536.04 epoch sec; seqNo:4098 2021-02-12T23:28:56Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.20 psi 2021-02-12T23:28:56Z- handleDepth()-Depth_Keller msg: 1613172536048 epoch sec; seqNo:4139; depth 24.18 2021-02-12T23:28:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:56Z- handleDvl() - RDI_Pathfinder msg: 1613172536.05 epoch sec; seqNo:1996 2021-02-12T23:28:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.03 2021-02-12T23:28:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172536.06 epoch sec; seqNo:4139; 4057087.46 north; 591027.97 east 2021-02-12T23:28:56Z- AHRS_M2 msg: 1613172536.45 epoch sec; seqNo:4099 2021-02-12T23:28:56Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.20 psi 2021-02-12T23:28:56Z- handleDepth()-Depth_Keller msg: 1613172536461 epoch sec; seqNo:4140; depth 24.29 2021-02-12T23:28:56Z- handleDvl() - RDI_Pathfinder msg: 1613172536.47 epoch sec; seqNo:1997 2021-02-12T23:28:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 90.34 , 62.52 , 95.75 2021-02-12T23:28:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , 0.02 2021-02-12T23:28:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172536.50 epoch sec; seqNo:4140; 4057087.84 north; 591027.97 east 2021-02-12T23:28:56Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-12T23:28:56Z- waiting for 1613172534.62, time is 1613172536.50 2021-02-12T23:28:56Z- LcmTrn::updateTrn() equating pose 1613172536.50 and meas times 1613172536.50 2021-02-12T23:28:56Z- LcmTrn::updateTrn() >>>> pose time 1613172536.50, meas time 1613172536.50 <<<< 2021-02-12T23:28:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172536.50 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:28:56Z- TerrainNav::motionUpdate() - currEstimate = (4057088, 591027.97, 24.29) 2021-02-12T23:28:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:28:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:28:56Z- TNavBF::Projecting Measurements 2021-02-12T23:28:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:28:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:28:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:28:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:28:56Z- TNavFilter::4 good beams. 2021-02-12T23:28:56Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:28:56Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:28:56Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:28:56Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:28:56Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:28:56Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:28:56Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:28:56Z- TNavBF::mmse selected from filter -1 2021-02-12T23:28:56Z- TerrainMapOctree: (vehN, vehE ) = (4057087.84, 591027.97). 2021-02-12T23:28:56Z- TerrainMapOctree: Min Distance = 5263.56. 2021-02-12T23:28:56Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:28:56Z- There is no specific good beam that all particles have in common. 2021-02-12T23:28:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:28:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-12T23:28:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:28:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 13.06 2021-02-12T23:28:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:28:56Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 6.25 2021-02-12T23:28:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:28:56Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 14.67 2021-02-12T23:28:56Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:28:56Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:28:56Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:28:56Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:28:56Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:28:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:28:56Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:28:56Z- Filter 0 No beams used in subcloud update 2021-02-12T23:28:56Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:28:56Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:28:56Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 761.6 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:28:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172536.50 sec. 2021-02-12T23:28:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:28:56Z- TNavBF::mmse selected from filter -1 2021-02-12T23:28:56Z- variances([0], [2], and psi_berg) = 761.633839 833.044719 0.000000 2021-02-12T23:28:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:28:56Z- LcmTrn::publishEstimates() - MLE : 4057070.34 590979.47 24.29 2021-02-12T23:28:56Z- LcmTrn::publishEstimates() - MMSE : 4057087.84 591027.97 24.29 2021-02-12T23:28:56Z- LcmTrn::publishEstimates() - COVAR: 761.63 833.04 0.80 0.00 2021-02-12T23:28:56Z- AHRS_M2 msg: 1613172536.85 epoch sec; seqNo:4100 2021-02-12T23:28:56Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.20 psi 2021-02-12T23:28:56Z- handleDepth()-Depth_Keller msg: 1613172536857 epoch sec; seqNo:4141; depth 24.41 2021-02-12T23:28:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172536.87 epoch sec; seqNo:4141; 4057088.22 north; 591027.96 east 2021-02-12T23:28:57Z- AHRS_M2 msg: 1613172537.26 epoch sec; seqNo:4101 2021-02-12T23:28:57Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:28:57Z- handleDepth()-Depth_Keller msg: 1613172537260 epoch sec; seqNo:4142; depth 24.55 2021-02-12T23:28:57Z- handleDvl() - RDI_Pathfinder msg: 1613172537.28 epoch sec; seqNo:1998 2021-02-12T23:28:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 89.33 , 62.93 , 97.98 2021-02-12T23:28:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , 0.03 2021-02-12T23:28:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172537.29 epoch sec; seqNo:4142; 4057088.60 north; 591027.96 east 2021-02-12T23:28:57Z- AHRS_M2 msg: 1613172537.66 epoch sec; seqNo:4102 2021-02-12T23:28:57Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:28:57Z- handleDepth()-Depth_Keller msg: 1613172537669 epoch sec; seqNo:4143; depth 24.67 2021-02-12T23:28:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172537.68 epoch sec; seqNo:4143; 4057088.60 north; 591027.96 east 2021-02-12T23:28:58Z- AHRS_M2 msg: 1613172538.08 epoch sec; seqNo:4103 2021-02-12T23:28:58Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:28:58Z- handleDepth()-Depth_Keller msg: 1613172538087 epoch sec; seqNo:4144; depth 24.78 2021-02-12T23:28:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172538.10 epoch sec; seqNo:4144; 4057088.98 north; 591027.96 east 2021-02-12T23:28:58Z- AHRS_M2 msg: 1613172538.47 epoch sec; seqNo:4104 2021-02-12T23:28:58Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:28:58Z- handleDepth()-Depth_Keller msg: 1613172538478 epoch sec; seqNo:4145; depth 24.92 2021-02-12T23:28:58Z- handleDvl() - RDI_Pathfinder msg: 1613172538.49 epoch sec; seqNo:1999 2021-02-12T23:28:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 87.17 , 62.52 , 95.82 2021-02-12T23:28:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , 0.03 2021-02-12T23:28:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172538.50 epoch sec; seqNo:4145; 4057089.35 north; 591027.95 east 2021-02-12T23:28:58Z- AHRS_M2 msg: 1613172538.87 epoch sec; seqNo:4105 2021-02-12T23:28:58Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:28:58Z- handleDepth()-Depth_Keller msg: 1613172538881 epoch sec; seqNo:4146; depth 25.06 2021-02-12T23:28:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172538.90 epoch sec; seqNo:4146; 4057089.75 north; 591029.17 east 2021-02-12T23:28:59Z- AHRS_M2 msg: 1613172539.28 epoch sec; seqNo:4106 2021-02-12T23:28:59Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:28:59Z- handleDepth()-Depth_Keller msg: 1613172539280 epoch sec; seqNo:4147; depth 25.16 2021-02-12T23:28:59Z- handleDvl() - RDI_Pathfinder msg: 1613172539.30 epoch sec; seqNo:2000 2021-02-12T23:28:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 90.34 , 62.52 , 94.67 2021-02-12T23:28:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , 0.04 2021-02-12T23:28:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172539.32 epoch sec; seqNo:4147; 4057090.12 north; 591029.17 east 2021-02-12T23:28:59Z- AHRS_M2 msg: 1613172539.68 epoch sec; seqNo:4107 2021-02-12T23:28:59Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:28:59Z- handleDepth()-Depth_Keller msg: 1613172539686 epoch sec; seqNo:4148; depth 25.28 2021-02-12T23:28:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:28:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:28:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:28:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:28:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:28:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:28:59Z- handleDvl() - RDI_Pathfinder msg: 1613172539.69 epoch sec; seqNo:2001 2021-02-12T23:28:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:28:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , 0.04 2021-02-12T23:28:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172539.70 epoch sec; seqNo:4148; 4057090.12 north; 591029.17 east 2021-02-12T23:29:00Z- AHRS_M2 msg: 1613172540.08 epoch sec; seqNo:4108 2021-02-12T23:29:00Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:29:00Z- handleDepth()-Depth_Keller msg: 1613172540088 epoch sec; seqNo:4149; depth 25.41 2021-02-12T23:29:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172540.11 epoch sec; seqNo:4149; 4057090.50 north; 591029.16 east 2021-02-12T23:29:00Z- AHRS_M2 msg: 1613172540.49 epoch sec; seqNo:4109 2021-02-12T23:29:00Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:29:00Z- handleDepth()-Depth_Keller msg: 1613172540492 epoch sec; seqNo:4150; depth 25.52 2021-02-12T23:29:00Z- handleDvl() - RDI_Pathfinder msg: 1613172540.50 epoch sec; seqNo:2002 2021-02-12T23:29:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 87.10 , 62.52 , 93.58 2021-02-12T23:29:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , 0.03 2021-02-12T23:29:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172540.53 epoch sec; seqNo:4150; 4057090.88 north; 591029.16 east 2021-02-12T23:29:00Z- AHRS_M2 msg: 1613172540.90 epoch sec; seqNo:4110 2021-02-12T23:29:00Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:29:00Z- handleDepth()-Depth_Keller msg: 1613172540904 epoch sec; seqNo:4151; depth 25.65 2021-02-12T23:29:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:00Z- handleDvl() - RDI_Pathfinder msg: 1613172540.91 epoch sec; seqNo:2003 2021-02-12T23:29:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , 0.03 2021-02-12T23:29:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172540.93 epoch sec; seqNo:4151; 4057091.26 north; 591029.15 east 2021-02-12T23:29:01Z- AHRS_M2 msg: 1613172541.30 epoch sec; seqNo:4111 2021-02-12T23:29:01Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:29:01Z- handleDepth()-Depth_Keller msg: 1613172541304 epoch sec; seqNo:4152; depth 25.77 2021-02-12T23:29:01Z- handleDvl() - RDI_Pathfinder msg: 1613172541.32 epoch sec; seqNo:2004 2021-02-12T23:29:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.42 , 86.01 , 62.52 , 97.91 2021-02-12T23:29:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , 0.02 2021-02-12T23:29:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172541.33 epoch sec; seqNo:4152; 4057091.26 north; 591029.15 east 2021-02-12T23:29:01Z- AHRS_M2 msg: 1613172541.70 epoch sec; seqNo:4112 2021-02-12T23:29:01Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:29:01Z- handleDepth()-Depth_Keller msg: 1613172541709 epoch sec; seqNo:4153; depth 25.89 2021-02-12T23:29:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172541.72 epoch sec; seqNo:4153; 4057091.65 north; 591030.37 east 2021-02-12T23:29:01Z- LcmTrn::handleMessages() - handling 46 messages took 0.59 ms and 135 calls... 2021-02-12T23:29:01Z- waiting for 1613172539.50, time is 1613172541.72 2021-02-12T23:29:01Z- LcmTrn::updateTrn() >>>> pose time 1613172541.72, meas time 1613172541.32 <<<< 2021-02-12T23:29:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172541.32 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:29:01Z- TerrainNav::motionUpdate() - currEstimate = (4057092, 591030.37, 25.89) 2021-02-12T23:29:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:29:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:29:01Z- TNavBF::Projecting Measurements 2021-02-12T23:29:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:29:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:29:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:29:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:29:01Z- TNavFilter::4 good beams. 2021-02-12T23:29:01Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:29:01Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:29:01Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:29:01Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:29:01Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:29:01Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:29:01Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:29:01Z- TNavBF::mmse selected from filter -1 2021-02-12T23:29:01Z- TerrainMapOctree: (vehN, vehE ) = (4057091.36, 591030.18). 2021-02-12T23:29:01Z- TerrainMapOctree: Min Distance = 5259.42. 2021-02-12T23:29:01Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:29:01Z- There is no specific good beam that all particles have in common. 2021-02-12T23:29:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:29:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 7.27 2021-02-12T23:29:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:29:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 11.84 2021-02-12T23:29:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:29:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 6.25 2021-02-12T23:29:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:29:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.77 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-12T23:29:01Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:29:01Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:29:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:29:01Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:29:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:29:01Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:29:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:29:01Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:29:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:29:01Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:29:01Z- Filter 0 No beams used in subcloud update 2021-02-12T23:29:01Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:29:01Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:29:01Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 761.5 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:29:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172541.32 sec. 2021-02-12T23:29:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:29:01Z- TNavBF::mmse selected from filter -1 2021-02-12T23:29:01Z- variances([0], [2], and psi_berg) = 761.483561 833.044556 0.000000 2021-02-12T23:29:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:29:01Z- LcmTrn::publishEstimates() - MLE : 4057074.15 590985.87 25.89 2021-02-12T23:29:01Z- LcmTrn::publishEstimates() - MMSE : 4057091.65 591030.37 25.89 2021-02-12T23:29:01Z- LcmTrn::publishEstimates() - COVAR: 761.48 833.04 0.75 0.00 2021-02-12T23:29:02Z- AHRS_M2 msg: 1613172542.10 epoch sec; seqNo:4113 2021-02-12T23:29:02Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:29:02Z- handleDepth()-Depth_Keller msg: 1613172542109 epoch sec; seqNo:4154; depth 26.02 2021-02-12T23:29:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:02Z- handleDvl() - RDI_Pathfinder msg: 1613172542.11 epoch sec; seqNo:2005 2021-02-12T23:29:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , 0.02 2021-02-12T23:29:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172542.12 epoch sec; seqNo:4154; 4057092.03 north; 591030.37 east 2021-02-12T23:29:02Z- AHRS_M2 msg: 1613172542.51 epoch sec; seqNo:4114 2021-02-12T23:29:02Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:29:02Z- handleDepth()-Depth_Keller msg: 1613172542521 epoch sec; seqNo:4155; depth 26.13 2021-02-12T23:29:02Z- handleDvl() - RDI_Pathfinder msg: 1613172542.53 epoch sec; seqNo:2006 2021-02-12T23:29:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 87.10 , 62.52 , 94.67 2021-02-12T23:29:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.06 , 0.01 2021-02-12T23:29:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172542.54 epoch sec; seqNo:4155; 4057092.03 north; 591030.37 east 2021-02-12T23:29:02Z- AHRS_M2 msg: 1613172542.91 epoch sec; seqNo:4115 2021-02-12T23:29:02Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:29:02Z- handleDepth()-Depth_Keller msg: 1613172542916 epoch sec; seqNo:4156; depth 26.26 2021-02-12T23:29:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172542.93 epoch sec; seqNo:4156; 4057092.41 north; 591030.36 east 2021-02-12T23:29:03Z- AHRS_M2 msg: 1613172543.32 epoch sec; seqNo:4116 2021-02-12T23:29:03Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:29:03Z- handleDepth()-Depth_Keller msg: 1613172543325 epoch sec; seqNo:4157; depth 26.37 2021-02-12T23:29:03Z- handleDvl() - RDI_Pathfinder msg: 1613172543.36 epoch sec; seqNo:2007 2021-02-12T23:29:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.42 , 88.18 , 61.70 , 97.91 2021-02-12T23:29:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.07 , -0.02 2021-02-12T23:29:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172543.38 epoch sec; seqNo:4157; 4057092.79 north; 591030.36 east 2021-02-12T23:29:03Z- AHRS_M2 msg: 1613172543.72 epoch sec; seqNo:4117 2021-02-12T23:29:03Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:29:03Z- handleDepth()-Depth_Keller msg: 1613172543727 epoch sec; seqNo:4158; depth 26.51 2021-02-12T23:29:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172543.77 epoch sec; seqNo:4158; 4057093.17 north; 591030.35 east 2021-02-12T23:29:04Z- AHRS_M2 msg: 1613172544.13 epoch sec; seqNo:4118 2021-02-12T23:29:04Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:29:04Z- handleDepth()-Depth_Keller msg: 1613172544134 epoch sec; seqNo:4159; depth 26.64 2021-02-12T23:29:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172544.14 epoch sec; seqNo:4159; 4057093.18 north; 591031.58 east 2021-02-12T23:29:04Z- AHRS_M2 msg: 1613172544.53 epoch sec; seqNo:4119 2021-02-12T23:29:04Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:29:04Z- handleDepth()-Depth_Keller msg: 1613172544532 epoch sec; seqNo:4160; depth 26.74 2021-02-12T23:29:04Z- handleDvl() - RDI_Pathfinder msg: 1613172544.55 epoch sec; seqNo:2008 2021-02-12T23:29:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.01 , 87.10 , 61.70 , 97.91 2021-02-12T23:29:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , -0.04 2021-02-12T23:29:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172544.56 epoch sec; seqNo:4160; 4057093.56 north; 591031.57 east 2021-02-12T23:29:04Z- AHRS_M2 msg: 1613172544.93 epoch sec; seqNo:4120 2021-02-12T23:29:04Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:29:04Z- handleDepth()-Depth_Keller msg: 1613172544941 epoch sec; seqNo:4161; depth 26.87 2021-02-12T23:29:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172544.95 epoch sec; seqNo:4161; 4057093.94 north; 591031.57 east 2021-02-12T23:29:05Z- AHRS_M2 msg: 1613172545.34 epoch sec; seqNo:4121 2021-02-12T23:29:05Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.18 psi 2021-02-12T23:29:05Z- handleDepth()-Depth_Keller msg: 1613172545341 epoch sec; seqNo:4162; depth 26.98 2021-02-12T23:29:05Z- handleDvl() - RDI_Pathfinder msg: 1613172545.35 epoch sec; seqNo:2009 2021-02-12T23:29:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.61 , 86.09 , 60.89 , 97.98 2021-02-12T23:29:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , -0.07 2021-02-12T23:29:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172545.36 epoch sec; seqNo:4162; 4057094.32 north; 591031.56 east 2021-02-12T23:29:05Z- AHRS_M2 msg: 1613172545.74 epoch sec; seqNo:4122 2021-02-12T23:29:05Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.18 psi 2021-02-12T23:29:05Z- handleDepth()-Depth_Keller msg: 1613172545746 epoch sec; seqNo:4163; depth 27.09 2021-02-12T23:29:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:05Z- handleDvl() - RDI_Pathfinder msg: 1613172545.75 epoch sec; seqNo:2010 2021-02-12T23:29:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , -0.07 2021-02-12T23:29:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172545.76 epoch sec; seqNo:4163; 4057094.70 north; 591031.56 east 2021-02-12T23:29:06Z- AHRS_M2 msg: 1613172546.15 epoch sec; seqNo:4123 2021-02-12T23:29:06Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.18 psi 2021-02-12T23:29:06Z- handleDepth()-Depth_Keller msg: 1613172546150 epoch sec; seqNo:4164; depth 27.22 2021-02-12T23:29:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172546.16 epoch sec; seqNo:4164; 4057094.70 north; 591031.56 east 2021-02-12T23:29:06Z- AHRS_M2 msg: 1613172546.55 epoch sec; seqNo:4124 2021-02-12T23:29:06Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.18 psi 2021-02-12T23:29:06Z- handleDepth()-Depth_Keller msg: 1613172546552 epoch sec; seqNo:4165; depth 27.35 2021-02-12T23:29:06Z- handleDvl() - RDI_Pathfinder msg: 1613172546.56 epoch sec; seqNo:2011 2021-02-12T23:29:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.20 , 87.17 , 60.89 , 97.98 2021-02-12T23:29:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , -0.07 2021-02-12T23:29:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172546.57 epoch sec; seqNo:4165; 4057095.09 north; 591032.78 east 2021-02-12T23:29:06Z- AHRS_M2 msg: 1613172546.95 epoch sec; seqNo:4125 2021-02-12T23:29:06Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.18 psi 2021-02-12T23:29:06Z- handleDepth()-Depth_Keller msg: 1613172546965 epoch sec; seqNo:4166; depth 27.47 2021-02-12T23:29:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:06Z- handleDvl() - RDI_Pathfinder msg: 1613172546.97 epoch sec; seqNo:2012 2021-02-12T23:29:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , -0.07 2021-02-12T23:29:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172546.98 epoch sec; seqNo:4166; 4057095.47 north; 591032.77 east 2021-02-12T23:29:07Z- LcmTrn::handleMessages() - handling 47 messages took 0.73 ms and 133 calls... 2021-02-12T23:29:07Z- waiting for 1613172544.72, time is 1613172546.98 2021-02-12T23:29:07Z- LcmTrn::updateTrn() equating pose 1613172546.98 and meas times 1613172546.98 2021-02-12T23:29:07Z- LcmTrn::updateTrn() >>>> pose time 1613172546.98, meas time 1613172546.98 <<<< 2021-02-12T23:29:07Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172546.98, ping # 0. 2021-02-12T23:29:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172546.98 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:29:07Z- TerrainNav::motionUpdate() - currEstimate = (4057095, 591032.77, 27.47) 2021-02-12T23:29:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:29:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:29:07Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:29:07Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:29:07Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172546.98 sec.; No relevant navigation data available 2021-02-12T23:29:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:29:07Z- TNavBF::mmse selected from filter -1 2021-02-12T23:29:07Z- variances([0], [2], and psi_berg) = 761.362455 833.044425 0.000000 2021-02-12T23:29:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:29:07Z- LcmTrn::publishEstimates() - MLE : 4057077.97 591079.27 27.47 2021-02-12T23:29:07Z- LcmTrn::publishEstimates() - MMSE : 4057095.47 591032.77 27.47 2021-02-12T23:29:07Z- LcmTrn::publishEstimates() - COVAR: 761.36 833.04 0.98 0.00 2021-02-12T23:29:07Z- AHRS_M2 msg: 1613172547.36 epoch sec; seqNo:4126 2021-02-12T23:29:07Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.18 psi 2021-02-12T23:29:07Z- handleDepth()-Depth_Keller msg: 1613172547364 epoch sec; seqNo:4167; depth 27.60 2021-02-12T23:29:07Z- handleDvl() - RDI_Pathfinder msg: 1613172547.38 epoch sec; seqNo:2013 2021-02-12T23:29:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.38 , 79.28 , 60.48 , 91.49 2021-02-12T23:29:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , -0.03 2021-02-12T23:29:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172547.40 epoch sec; seqNo:4167; 4057095.85 north; 591032.77 east 2021-02-12T23:29:07Z- AHRS_M2 msg: 1613172547.76 epoch sec; seqNo:4127 2021-02-12T23:29:07Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.18 psi 2021-02-12T23:29:07Z- handleDepth()-Depth_Keller msg: 1613172547764 epoch sec; seqNo:4168; depth 27.71 2021-02-12T23:29:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172547.78 epoch sec; seqNo:4168; 4057095.85 north; 591032.77 east 2021-02-12T23:29:08Z- AHRS_M2 msg: 1613172548.16 epoch sec; seqNo:4128 2021-02-12T23:29:08Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-12T23:29:08Z- handleDepth()-Depth_Keller msg: 1613172548173 epoch sec; seqNo:4169; depth 27.84 2021-02-12T23:29:08Z- handleDvl() - RDI_Pathfinder msg: 1613172548.18 epoch sec; seqNo:2014 2021-02-12T23:29:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:08Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:29:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172548.20 epoch sec; seqNo:4169; 4057096.22 north; 591032.76 east 2021-02-12T23:29:08Z- AHRS_M2 msg: 1613172548.57 epoch sec; seqNo:4129 2021-02-12T23:29:08Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.17 psi 2021-02-12T23:29:08Z- handleDepth()-Depth_Keller msg: 1613172548581 epoch sec; seqNo:4170; depth 27.98 2021-02-12T23:29:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172548.59 epoch sec; seqNo:4170; 4057096.60 north; 591032.76 east 2021-02-12T23:29:08Z- AHRS_M2 msg: 1613172548.97 epoch sec; seqNo:4130 2021-02-12T23:29:08Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.17 psi 2021-02-12T23:29:08Z- handleDepth()-Depth_Keller msg: 1613172548979 epoch sec; seqNo:4171; depth 28.09 2021-02-12T23:29:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172549.00 epoch sec; seqNo:4171; 4057096.98 north; 591032.76 east 2021-02-12T23:29:09Z- AHRS_M2 msg: 1613172549.38 epoch sec; seqNo:4131 2021-02-12T23:29:09Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.16 psi 2021-02-12T23:29:09Z- handleDepth()-Depth_Keller msg: 1613172549389 epoch sec; seqNo:4172; depth 28.20 2021-02-12T23:29:09Z- handleDvl() - RDI_Pathfinder msg: 1613172549.41 epoch sec; seqNo:2015 2021-02-12T23:29:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.97 , 77.24 , 60.07 , 98.32 2021-02-12T23:29:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.07 , 0.04 2021-02-12T23:29:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172549.42 epoch sec; seqNo:4172; 4057097.37 north; 591033.97 east 2021-02-12T23:29:09Z- AHRS_M2 msg: 1613172549.78 epoch sec; seqNo:4132 2021-02-12T23:29:09Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.16 psi 2021-02-12T23:29:09Z- handleDepth()-Depth_Keller msg: 1613172549783 epoch sec; seqNo:4173; depth 28.33 2021-02-12T23:29:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172549.80 epoch sec; seqNo:4173; 4057097.37 north; 591033.97 east 2021-02-12T23:29:10Z- AHRS_M2 msg: 1613172550.18 epoch sec; seqNo:4133 2021-02-12T23:29:10Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.16 psi 2021-02-12T23:29:10Z- handleDepth()-Depth_Keller msg: 1613172550189 epoch sec; seqNo:4174; depth 28.45 2021-02-12T23:29:10Z- handleDvl() - RDI_Pathfinder msg: 1613172550.20 epoch sec; seqNo:2016 2021-02-12T23:29:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.79 , 76.83 , 59.66 , 97.98 2021-02-12T23:29:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.01 2021-02-12T23:29:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172550.21 epoch sec; seqNo:4174; 4057097.75 north; 591033.97 east 2021-02-12T23:29:10Z- AHRS_M2 msg: 1613172550.59 epoch sec; seqNo:4134 2021-02-12T23:29:10Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.16 psi 2021-02-12T23:29:10Z- handleDepth()-Depth_Keller msg: 1613172550593 epoch sec; seqNo:4175; depth 28.56 2021-02-12T23:29:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:10Z- handleDvl() - RDI_Pathfinder msg: 1613172550.60 epoch sec; seqNo:2017 2021-02-12T23:29:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.01 2021-02-12T23:29:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172550.61 epoch sec; seqNo:4175; 4057098.13 north; 591033.97 east 2021-02-12T23:29:10Z- AHRS_M2 msg: 1613172550.99 epoch sec; seqNo:4135 2021-02-12T23:29:10Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.15 psi 2021-02-12T23:29:10Z- handleDepth()-Depth_Keller msg: 1613172550996 epoch sec; seqNo:4176; depth 28.69 2021-02-12T23:29:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172551.01 epoch sec; seqNo:4176; 4057098.13 north; 591033.97 east 2021-02-12T23:29:11Z- AHRS_M2 msg: 1613172551.40 epoch sec; seqNo:4136 2021-02-12T23:29:11Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.16 psi 2021-02-12T23:29:11Z- handleDepth()-Depth_Keller msg: 1613172551405 epoch sec; seqNo:4177; depth 28.80 2021-02-12T23:29:11Z- handleDvl() - RDI_Pathfinder msg: 1613172551.42 epoch sec; seqNo:2018 2021-02-12T23:29:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.79 , 76.83 , 59.25 , 91.49 2021-02-12T23:29:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , 0.00 2021-02-12T23:29:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172551.43 epoch sec; seqNo:4177; 4057098.51 north; 591033.96 east 2021-02-12T23:29:11Z- AHRS_M2 msg: 1613172551.80 epoch sec; seqNo:4137 2021-02-12T23:29:11Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.16 psi 2021-02-12T23:29:11Z- handleDepth()-Depth_Keller msg: 1613172551807 epoch sec; seqNo:4178; depth 28.94 2021-02-12T23:29:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:11Z- handleDvl() - RDI_Pathfinder msg: 1613172551.81 epoch sec; seqNo:2019 2021-02-12T23:29:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , 0.00 2021-02-12T23:29:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172551.83 epoch sec; seqNo:4178; 4057098.90 north; 591035.18 east 2021-02-12T23:29:12Z- LcmTrn::handleMessages() - handling 43 messages took 0.68 ms and 130 calls... 2021-02-12T23:29:12Z- waiting for 1613172549.98, time is 1613172551.83 2021-02-12T23:29:12Z- LcmTrn::updateTrn() equating pose 1613172551.83 and meas times 1613172551.83 2021-02-12T23:29:12Z- LcmTrn::updateTrn() >>>> pose time 1613172551.83, meas time 1613172551.83 <<<< 2021-02-12T23:29:12Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172551.83, ping # 0. 2021-02-12T23:29:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172551.83 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:29:12Z- TerrainNav::motionUpdate() - currEstimate = (4057099, 591035.18, 28.94) 2021-02-12T23:29:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:29:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:29:12Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:29:12Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:29:12Z- TNavBF::Projecting Measurements 2021-02-12T23:29:12Z- TNavFilter::Measurement from time = 1613172551.83 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:29:12Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:29:12Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:29:12Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:29:12Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:29:12Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:29:12Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:29:12Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:29:12Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 761.2 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:29:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:29:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:29:12Z- variances([0], [2], and psi_berg) = 761.245840 833.044299 0.000000 2021-02-12T23:29:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:29:12Z- LcmTrn::publishEstimates() - MLE : 4057081.40 590991.68 28.94 2021-02-12T23:29:12Z- LcmTrn::publishEstimates() - MMSE : 4057098.90 591035.18 28.94 2021-02-12T23:29:12Z- LcmTrn::publishEstimates() - COVAR: 761.25 833.04 1.20 0.00 2021-02-12T23:29:12Z- AHRS_M2 msg: 1613172552.20 epoch sec; seqNo:4138 2021-02-12T23:29:12Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.16 psi 2021-02-12T23:29:12Z- handleDepth()-Depth_Keller msg: 1613172552215 epoch sec; seqNo:4179; depth 29.04 2021-02-12T23:29:12Z- handleDvl() - RDI_Pathfinder msg: 1613172552.23 epoch sec; seqNo:2020 2021-02-12T23:29:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.38 , 76.42 , 58.03 , 91.49 2021-02-12T23:29:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.00 2021-02-12T23:29:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172552.24 epoch sec; seqNo:4179; 4057099.28 north; 591035.17 east 2021-02-12T23:29:12Z- AHRS_M2 msg: 1613172552.61 epoch sec; seqNo:4139 2021-02-12T23:29:12Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.17 psi 2021-02-12T23:29:12Z- handleDepth()-Depth_Keller msg: 1613172552618 epoch sec; seqNo:4180; depth 29.18 2021-02-12T23:29:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172552.63 epoch sec; seqNo:4180; 4057099.28 north; 591035.17 east 2021-02-12T23:29:13Z- AHRS_M2 msg: 1613172553.02 epoch sec; seqNo:4140 2021-02-12T23:29:13Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.17 psi 2021-02-12T23:29:13Z- handleDepth()-Depth_Keller msg: 1613172553021 epoch sec; seqNo:4181; depth 29.30 2021-02-12T23:29:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172553.03 epoch sec; seqNo:4181; 4057099.66 north; 591035.17 east 2021-02-12T23:29:13Z- AHRS_M2 msg: 1613172553.42 epoch sec; seqNo:4141 2021-02-12T23:29:13Z- AHRS_M2 msg: 0.64 phi; -0.27 theta; -0.17 psi 2021-02-12T23:29:13Z- handleDepth()-Depth_Keller msg: 1613172553423 epoch sec; seqNo:4182; depth 29.44 2021-02-12T23:29:13Z- handleDvl() - RDI_Pathfinder msg: 1613172553.43 epoch sec; seqNo:2021 2021-02-12T23:29:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.97 , 83.92 , 58.44 , 90.41 2021-02-12T23:29:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.03 2021-02-12T23:29:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172553.44 epoch sec; seqNo:4182; 4057100.04 north; 591035.17 east 2021-02-12T23:29:13Z- AHRS_M2 msg: 1613172553.82 epoch sec; seqNo:4142 2021-02-12T23:29:13Z- AHRS_M2 msg: 0.64 phi; -0.23 theta; -0.17 psi 2021-02-12T23:29:13Z- handleDepth()-Depth_Keller msg: 1613172553829 epoch sec; seqNo:4183; depth 29.57 2021-02-12T23:29:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172553.85 epoch sec; seqNo:4183; 4057100.04 north; 591035.17 east 2021-02-12T23:29:14Z- AHRS_M2 msg: 1613172554.23 epoch sec; seqNo:4143 2021-02-12T23:29:14Z- AHRS_M2 msg: 0.63 phi; -0.19 theta; -0.16 psi 2021-02-12T23:29:14Z- handleDepth()-Depth_Keller msg: 1613172554231 epoch sec; seqNo:4184; depth 29.70 2021-02-12T23:29:14Z- handleDvl() - RDI_Pathfinder msg: 1613172554.24 epoch sec; seqNo:2022 2021-02-12T23:29:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.93 , 79.60 , 59.66 , 83.92 2021-02-12T23:29:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.04 2021-02-12T23:29:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172554.27 epoch sec; seqNo:4184; 4057100.43 north; 591036.38 east 2021-02-12T23:29:14Z- AHRS_M2 msg: 1613172554.63 epoch sec; seqNo:4144 2021-02-12T23:29:14Z- AHRS_M2 msg: 0.62 phi; -0.15 theta; -0.16 psi 2021-02-12T23:29:14Z- handleDepth()-Depth_Keller msg: 1613172554634 epoch sec; seqNo:4185; depth 29.85 2021-02-12T23:29:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172554.64 epoch sec; seqNo:4185; 4057100.81 north; 591036.38 east 2021-02-12T23:29:15Z- AHRS_M2 msg: 1613172555.03 epoch sec; seqNo:4145 2021-02-12T23:29:15Z- AHRS_M2 msg: 0.61 phi; -0.11 theta; -0.15 psi 2021-02-12T23:29:15Z- handleDepth()-Depth_Keller msg: 1613172555036 epoch sec; seqNo:4186; depth 29.94 2021-02-12T23:29:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172555.06 epoch sec; seqNo:4186; 4057101.19 north; 591036.37 east 2021-02-12T23:29:15Z- AHRS_M2 msg: 1613172555.44 epoch sec; seqNo:4146 2021-02-12T23:29:15Z- AHRS_M2 msg: 0.60 phi; -0.07 theta; -0.15 psi 2021-02-12T23:29:15Z- handleDepth()-Depth_Keller msg: 1613172555443 epoch sec; seqNo:4187; depth 30.03 2021-02-12T23:29:15Z- handleDvl() - RDI_Pathfinder msg: 1613172555.45 epoch sec; seqNo:2023 2021-02-12T23:29:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.71 , 71.92 , 60.48 , 72.74 2021-02-12T23:29:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , 0.02 2021-02-12T23:29:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172555.49 epoch sec; seqNo:4187; 4057101.57 north; 591036.37 east 2021-02-12T23:29:15Z- AHRS_M2 msg: 1613172555.84 epoch sec; seqNo:4147 2021-02-12T23:29:15Z- AHRS_M2 msg: 0.60 phi; -0.02 theta; -0.15 psi 2021-02-12T23:29:15Z- handleDepth()-Depth_Keller msg: 1613172555844 epoch sec; seqNo:4188; depth 30.13 2021-02-12T23:29:15Z- handleDvl() - RDI_Pathfinder msg: 1613172555.86 epoch sec; seqNo:2024 2021-02-12T23:29:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.63 , 0.00 , 64.39 , 67.76 2021-02-12T23:29:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , -0.01 2021-02-12T23:29:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172555.87 epoch sec; seqNo:4188; 4057101.57 north; 591036.37 east 2021-02-12T23:29:16Z- AHRS_M2 msg: 1613172556.24 epoch sec; seqNo:4148 2021-02-12T23:29:16Z- AHRS_M2 msg: 0.59 phi; 0.01 theta; -0.15 psi 2021-02-12T23:29:16Z- handleDepth()-Depth_Keller msg: 1613172556257 epoch sec; seqNo:4189; depth 30.21 2021-02-12T23:29:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172556.27 epoch sec; seqNo:4189; 4057101.95 north; 591036.37 east 2021-02-12T23:29:16Z- AHRS_M2 msg: 1613172556.65 epoch sec; seqNo:4149 2021-02-12T23:29:16Z- AHRS_M2 msg: 0.58 phi; 0.05 theta; -0.15 psi 2021-02-12T23:29:16Z- handleDepth()-Depth_Keller msg: 1613172556652 epoch sec; seqNo:4190; depth 30.27 2021-02-12T23:29:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:16Z- handleDvl() - RDI_Pathfinder msg: 1613172556.65 epoch sec; seqNo:2025 2021-02-12T23:29:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , -0.01 2021-02-12T23:29:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172556.67 epoch sec; seqNo:4190; 4057102.34 north; 591037.58 east 2021-02-12T23:29:17Z- AHRS_M2 msg: 1613172557.05 epoch sec; seqNo:4150 2021-02-12T23:29:17Z- AHRS_M2 msg: 0.58 phi; 0.09 theta; -0.15 psi 2021-02-12T23:29:17Z- handleDepth()-Depth_Keller msg: 1613172557060 epoch sec; seqNo:4191; depth 30.30 2021-02-12T23:29:17Z- handleDvl() - RDI_Pathfinder msg: 1613172557.09 epoch sec; seqNo:2026 2021-02-12T23:29:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.63 , 71.44 , 68.07 , 64.08 2021-02-12T23:29:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.03 , -0.06 2021-02-12T23:29:17Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-12T23:29:17Z- waiting for 1613172554.83, time is 1613172557.09 2021-02-12T23:29:17Z- LcmTrn::updateTrn() equating pose 1613172557.09 and meas times 1613172557.09 2021-02-12T23:29:17Z- LcmTrn::updateTrn() >>>> pose time 1613172557.09, meas time 1613172557.09 <<<< 2021-02-12T23:29:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172557.09 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:29:17Z- TerrainNav::motionUpdate() - currEstimate = (4057102, 591037.58, 30.30) 2021-02-12T23:29:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:29:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:29:17Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172557.09 sec.; No relevant navigation data available 2021-02-12T23:29:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:29:17Z- TNavBF::mmse selected from filter -1 2021-02-12T23:29:17Z- variances([0], [2], and psi_berg) = 761.129421 833.044173 0.000000 2021-02-12T23:29:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:29:17Z- LcmTrn::publishEstimates() - MLE : 4057084.84 590994.08 30.30 2021-02-12T23:29:17Z- LcmTrn::publishEstimates() - MMSE : 4057102.34 591037.58 30.30 2021-02-12T23:29:17Z- LcmTrn::publishEstimates() - COVAR: 761.13 833.04 1.41 0.00 2021-02-12T23:29:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172557.11 epoch sec; seqNo:4191; 4057102.72 north; 591037.58 east 2021-02-12T23:29:17Z- AHRS_M2 msg: 1613172557.46 epoch sec; seqNo:4151 2021-02-12T23:29:17Z- AHRS_M2 msg: 0.57 phi; 0.12 theta; -0.15 psi 2021-02-12T23:29:17Z- handleDepth()-Depth_Keller msg: 1613172557460 epoch sec; seqNo:4192; depth 30.35 2021-02-12T23:29:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172557.47 epoch sec; seqNo:4192; 4057102.72 north; 591037.58 east 2021-02-12T23:29:17Z- AHRS_M2 msg: 1613172557.86 epoch sec; seqNo:4152 2021-02-12T23:29:17Z- AHRS_M2 msg: 0.57 phi; 0.15 theta; -0.15 psi 2021-02-12T23:29:17Z- handleDepth()-Depth_Keller msg: 1613172557866 epoch sec; seqNo:4193; depth 30.35 2021-02-12T23:29:17Z- handleDvl() - RDI_Pathfinder msg: 1613172557.88 epoch sec; seqNo:2027 2021-02-12T23:29:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.94 , 72.36 , 0.00 , 61.94 2021-02-12T23:29:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.00 , -0.08 2021-02-12T23:29:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172557.89 epoch sec; seqNo:4193; 4057103.10 north; 591037.58 east 2021-02-12T23:29:18Z- AHRS_M2 msg: 1613172558.26 epoch sec; seqNo:4153 2021-02-12T23:29:18Z- AHRS_M2 msg: 0.56 phi; 0.18 theta; -0.16 psi 2021-02-12T23:29:18Z- handleDepth()-Depth_Keller msg: 1613172558268 epoch sec; seqNo:4194; depth 30.35 2021-02-12T23:29:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172558.29 epoch sec; seqNo:4194; 4057103.47 north; 591037.57 east 2021-02-12T23:29:18Z- AHRS_M2 msg: 1613172558.67 epoch sec; seqNo:4154 2021-02-12T23:29:18Z- AHRS_M2 msg: 0.56 phi; 0.20 theta; -0.15 psi 2021-02-12T23:29:18Z- handleDepth()-Depth_Keller msg: 1613172558677 epoch sec; seqNo:4195; depth 30.34 2021-02-12T23:29:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172558.68 epoch sec; seqNo:4195; 4057103.47 north; 591037.57 east 2021-02-12T23:29:19Z- AHRS_M2 msg: 1613172559.07 epoch sec; seqNo:4155 2021-02-12T23:29:19Z- AHRS_M2 msg: 0.56 phi; 0.22 theta; -0.16 psi 2021-02-12T23:29:19Z- handleDepth()-Depth_Keller msg: 1613172559076 epoch sec; seqNo:4196; depth 30.32 2021-02-12T23:29:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:19Z- handleDvl() - RDI_Pathfinder msg: 1613172559.08 epoch sec; seqNo:2028 2021-02-12T23:29:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.00 , -0.08 2021-02-12T23:29:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172559.09 epoch sec; seqNo:4196; 4057103.85 north; 591037.57 east 2021-02-12T23:29:19Z- AHRS_M2 msg: 1613172559.48 epoch sec; seqNo:4156 2021-02-12T23:29:19Z- AHRS_M2 msg: 0.55 phi; 0.24 theta; -0.16 psi 2021-02-12T23:29:19Z- handleDepth()-Depth_Keller msg: 1613172559493 epoch sec; seqNo:4197; depth 30.30 2021-02-12T23:29:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172559.52 epoch sec; seqNo:4197; 4057104.25 north; 591038.78 east 2021-02-12T23:29:19Z- AHRS_M2 msg: 1613172559.90 epoch sec; seqNo:4157 2021-02-12T23:29:19Z- AHRS_M2 msg: 0.55 phi; 0.26 theta; -0.16 psi 2021-02-12T23:29:19Z- handleDepth()-Depth_Keller msg: 1613172559909 epoch sec; seqNo:4198; depth 30.24 2021-02-12T23:29:19Z- handleDvl() - RDI_Pathfinder msg: 1613172559.92 epoch sec; seqNo:2029 2021-02-12T23:29:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.96 , 74.00 , 0.00 , 59.69 2021-02-12T23:29:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.03 2021-02-12T23:29:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172559.93 epoch sec; seqNo:4198; 4057104.62 north; 591038.78 east 2021-02-12T23:29:20Z- AHRS_M2 msg: 1613172560.28 epoch sec; seqNo:4158 2021-02-12T23:29:20Z- AHRS_M2 msg: 0.54 phi; 0.28 theta; -0.16 psi 2021-02-12T23:29:20Z- handleDepth()-Depth_Keller msg: 1613172560288 epoch sec; seqNo:4199; depth 30.16 2021-02-12T23:29:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:20Z- handleDvl() - RDI_Pathfinder msg: 1613172560.29 epoch sec; seqNo:2030 2021-02-12T23:29:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.03 2021-02-12T23:29:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172560.30 epoch sec; seqNo:4199; 4057104.62 north; 591038.78 east 2021-02-12T23:29:20Z- AHRS_M2 msg: 1613172560.69 epoch sec; seqNo:4159 2021-02-12T23:29:20Z- AHRS_M2 msg: 0.54 phi; 0.29 theta; -0.16 psi 2021-02-12T23:29:20Z- handleDepth()-Depth_Keller msg: 1613172560697 epoch sec; seqNo:4200; depth 30.11 2021-02-12T23:29:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172560.71 epoch sec; seqNo:4200; 4057105.00 north; 591038.78 east 2021-02-12T23:29:21Z- AHRS_M2 msg: 1613172561.09 epoch sec; seqNo:4160 2021-02-12T23:29:21Z- AHRS_M2 msg: 0.54 phi; 0.31 theta; -0.15 psi 2021-02-12T23:29:21Z- handleDepth()-Depth_Keller msg: 1613172561098 epoch sec; seqNo:4201; depth 30.05 2021-02-12T23:29:21Z- handleDvl() - RDI_Pathfinder msg: 1613172561.11 epoch sec; seqNo:2031 2021-02-12T23:29:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.00 , 74.82 , 0.00 , 59.18 2021-02-12T23:29:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , -0.00 2021-02-12T23:29:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172561.12 epoch sec; seqNo:4201; 4057105.38 north; 591038.77 east 2021-02-12T23:29:21Z- AHRS_M2 msg: 1613172561.50 epoch sec; seqNo:4161 2021-02-12T23:29:21Z- AHRS_M2 msg: 0.54 phi; 0.32 theta; -0.15 psi 2021-02-12T23:29:21Z- handleDepth()-Depth_Keller msg: 1613172561501 epoch sec; seqNo:4202; depth 29.97 2021-02-12T23:29:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:21Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:21Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:21Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:21Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:21Z- handleDvl() - RDI_Pathfinder msg: 1613172561.50 epoch sec; seqNo:2032 2021-02-12T23:29:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , -0.00 2021-02-12T23:29:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172561.52 epoch sec; seqNo:4202; 4057105.76 north; 591038.77 east 2021-02-12T23:29:21Z- AHRS_M2 msg: 1613172561.90 epoch sec; seqNo:4162 2021-02-12T23:29:21Z- AHRS_M2 msg: 0.54 phi; 0.33 theta; -0.14 psi 2021-02-12T23:29:21Z- handleDepth()-Depth_Keller msg: 1613172561905 epoch sec; seqNo:4203; depth 29.88 2021-02-12T23:29:21Z- handleDvl() - RDI_Pathfinder msg: 1613172561.92 epoch sec; seqNo:2033 2021-02-12T23:29:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.92 , 75.12 , 0.00 , 58.87 2021-02-12T23:29:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.01 , -0.01 2021-02-12T23:29:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172561.94 epoch sec; seqNo:4203; 4057105.76 north; 591038.77 east 2021-02-12T23:29:22Z- LcmTrn::handleMessages() - handling 44 messages took 0.63 ms and 132 calls... 2021-02-12T23:29:22Z- waiting for 1613172560.09, time is 1613172561.94 2021-02-12T23:29:22Z- LcmTrn::updateTrn() equating pose 1613172561.94 and meas times 1613172561.94 2021-02-12T23:29:22Z- LcmTrn::updateTrn() >>>> pose time 1613172561.94, meas time 1613172561.94 <<<< 2021-02-12T23:29:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172561.94 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:29:22Z- TerrainNav::motionUpdate() - currEstimate = (4057106, 591038.77, 29.88) 2021-02-12T23:29:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:29:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:29:22Z- TNavBF::Projecting Measurements 2021-02-12T23:29:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:29:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:29:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:29:22Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:29:22Z- TNavFilter::3 good beams. 2021-02-12T23:29:22Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:29:22Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:29:22Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:29:22Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:29:22Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:29:22Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:29:22Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:29:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:29:22Z- TerrainMapOctree: (vehN, vehE ) = (4057105.76, 591038.77). 2021-02-12T23:29:22Z- TerrainMapOctree: Min Distance = 5242.78. 2021-02-12T23:29:22Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:29:22Z- There is no specific good beam that all particles have in common. 2021-02-12T23:29:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:29:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.60 K: 0.155 ExpectedMeasDiff: nan currMeas.covariance: 6.95 2021-02-12T23:29:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:29:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.35 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 9.03 2021-02-12T23:29:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:29:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.141 ExpectedMeasDiff: nan currMeas.covariance: 5.55 2021-02-12T23:29:22Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:29:22Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:29:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:29:22Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:29:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:29:22Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:29:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:29:22Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:29:22Z- Filter 0 No beams used in subcloud update 2021-02-12T23:29:22Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:29:22Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:29:22Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 761.0 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:29:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172561.94 sec. 2021-02-12T23:29:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:29:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:29:22Z- variances([0], [2], and psi_berg) = 761.021418 833.044057 0.000000 2021-02-12T23:29:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:29:22Z- LcmTrn::publishEstimates() - MLE : 4057088.26 590995.27 29.88 2021-02-12T23:29:22Z- LcmTrn::publishEstimates() - MMSE : 4057105.76 591038.77 29.88 2021-02-12T23:29:22Z- LcmTrn::publishEstimates() - COVAR: 761.02 833.04 1.03 0.00 2021-02-12T23:29:22Z- AHRS_M2 msg: 1613172562.30 epoch sec; seqNo:4163 2021-02-12T23:29:22Z- AHRS_M2 msg: 0.54 phi; 0.33 theta; -0.14 psi 2021-02-12T23:29:22Z- handleDepth()-Depth_Keller msg: 1613172562308 epoch sec; seqNo:4204; depth 29.79 2021-02-12T23:29:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172562.32 epoch sec; seqNo:4204; 4057106.14 north; 591038.76 east 2021-02-12T23:29:22Z- AHRS_M2 msg: 1613172562.71 epoch sec; seqNo:4164 2021-02-12T23:29:22Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.13 psi 2021-02-12T23:29:22Z- handleDepth()-Depth_Keller msg: 1613172562713 epoch sec; seqNo:4205; depth 29.69 2021-02-12T23:29:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:22Z- handleDvl() - RDI_Pathfinder msg: 1613172562.71 epoch sec; seqNo:2034 2021-02-12T23:29:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.01 , -0.01 2021-02-12T23:29:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172562.72 epoch sec; seqNo:4205; 4057106.53 north; 591039.98 east 2021-02-12T23:29:23Z- AHRS_M2 msg: 1613172563.11 epoch sec; seqNo:4165 2021-02-12T23:29:23Z- AHRS_M2 msg: 0.55 phi; 0.35 theta; -0.13 psi 2021-02-12T23:29:23Z- handleDepth()-Depth_Keller msg: 1613172563133 epoch sec; seqNo:4206; depth 29.60 2021-02-12T23:29:23Z- handleDvl() - RDI_Pathfinder msg: 1613172563.15 epoch sec; seqNo:2035 2021-02-12T23:29:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.84 , 75.43 , 0.00 , 59.48 2021-02-12T23:29:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.03 , -0.03 2021-02-12T23:29:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172563.16 epoch sec; seqNo:4206; 4057106.53 north; 591039.98 east 2021-02-12T23:29:23Z- AHRS_M2 msg: 1613172563.52 epoch sec; seqNo:4166 2021-02-12T23:29:23Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.13 psi 2021-02-12T23:29:23Z- handleDepth()-Depth_Keller msg: 1613172563520 epoch sec; seqNo:4207; depth 29.49 2021-02-12T23:29:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172563.53 epoch sec; seqNo:4207; 4057106.91 north; 591039.98 east 2021-02-12T23:29:23Z- AHRS_M2 msg: 1613172563.92 epoch sec; seqNo:4167 2021-02-12T23:29:23Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.12 psi 2021-02-12T23:29:23Z- handleDepth()-Depth_Keller msg: 1613172563924 epoch sec; seqNo:4208; depth 29.39 2021-02-12T23:29:23Z- handleDvl() - RDI_Pathfinder msg: 1613172563.94 epoch sec; seqNo:2036 2021-02-12T23:29:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.97 , 75.73 , 0.00 , 59.23 2021-02-12T23:29:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.01 , -0.05 2021-02-12T23:29:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172563.95 epoch sec; seqNo:4208; 4057107.29 north; 591039.97 east 2021-02-12T23:29:24Z- AHRS_M2 msg: 1613172564.33 epoch sec; seqNo:4168 2021-02-12T23:29:24Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.12 psi 2021-02-12T23:29:24Z- handleDepth()-Depth_Keller msg: 1613172564330 epoch sec; seqNo:4209; depth 29.27 2021-02-12T23:29:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172564.34 epoch sec; seqNo:4209; 4057107.29 north; 591039.97 east 2021-02-12T23:29:24Z- AHRS_M2 msg: 1613172564.73 epoch sec; seqNo:4169 2021-02-12T23:29:24Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.12 psi 2021-02-12T23:29:24Z- handleDepth()-Depth_Keller msg: 1613172564732 epoch sec; seqNo:4210; depth 29.16 2021-02-12T23:29:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172564.74 epoch sec; seqNo:4210; 4057107.67 north; 591039.97 east 2021-02-12T23:29:25Z- AHRS_M2 msg: 1613172565.14 epoch sec; seqNo:4170 2021-02-12T23:29:25Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.12 psi 2021-02-12T23:29:25Z- handleDepth()-Depth_Keller msg: 1613172565146 epoch sec; seqNo:4211; depth 29.05 2021-02-12T23:29:25Z- handleDvl() - RDI_Pathfinder msg: 1613172565.16 epoch sec; seqNo:2037 2021-02-12T23:29:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.97 , 75.41 , 0.00 , 59.55 2021-02-12T23:29:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.04 , -0.05 2021-02-12T23:29:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172565.19 epoch sec; seqNo:4211; 4057108.05 north; 591039.97 east 2021-02-12T23:29:25Z- AHRS_M2 msg: 1613172565.54 epoch sec; seqNo:4171 2021-02-12T23:29:25Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.12 psi 2021-02-12T23:29:25Z- handleDepth()-Depth_Keller msg: 1613172565547 epoch sec; seqNo:4212; depth 28.92 2021-02-12T23:29:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172565.56 epoch sec; seqNo:4212; 4057108.06 north; 591041.19 east 2021-02-12T23:29:25Z- AHRS_M2 msg: 1613172565.94 epoch sec; seqNo:4172 2021-02-12T23:29:25Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.12 psi 2021-02-12T23:29:25Z- handleDepth()-Depth_Keller msg: 1613172565949 epoch sec; seqNo:4213; depth 28.82 2021-02-12T23:29:25Z- handleDvl() - RDI_Pathfinder msg: 1613172565.97 epoch sec; seqNo:2038 2021-02-12T23:29:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 67.97 , 0.00 , 0.00 , 60.20 2021-02-12T23:29:25Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:29:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172565.98 epoch sec; seqNo:4213; 4057108.44 north; 591041.18 east 2021-02-12T23:29:26Z- AHRS_M2 msg: 1613172566.34 epoch sec; seqNo:4173 2021-02-12T23:29:26Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.12 psi 2021-02-12T23:29:26Z- handleDepth()-Depth_Keller msg: 1613172566348 epoch sec; seqNo:4214; depth 28.70 2021-02-12T23:29:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:26Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:26Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:26Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:26Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:26Z- handleDvl() - RDI_Pathfinder msg: 1613172566.35 epoch sec; seqNo:2039 2021-02-12T23:29:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:26Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:29:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172566.36 epoch sec; seqNo:4214; 4057108.82 north; 591041.18 east 2021-02-12T23:29:26Z- AHRS_M2 msg: 1613172566.75 epoch sec; seqNo:4174 2021-02-12T23:29:26Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.13 psi 2021-02-12T23:29:26Z- handleDepth()-Depth_Keller msg: 1613172566752 epoch sec; seqNo:4215; depth 28.59 2021-02-12T23:29:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172566.76 epoch sec; seqNo:4215; 4057108.82 north; 591041.18 east 2021-02-12T23:29:27Z- AHRS_M2 msg: 1613172567.16 epoch sec; seqNo:4175 2021-02-12T23:29:27Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.13 psi 2021-02-12T23:29:27Z- handleDepth()-Depth_Keller msg: 1613172567164 epoch sec; seqNo:4216; depth 28.48 2021-02-12T23:29:27Z- handleDvl() - RDI_Pathfinder msg: 1613172567.18 epoch sec; seqNo:2040 2021-02-12T23:29:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.14 , 77.08 , 0.00 , 60.05 2021-02-12T23:29:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.07 , 0.05 2021-02-12T23:29:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172567.19 epoch sec; seqNo:4216; 4057109.20 north; 591041.17 east 2021-02-12T23:29:27Z- LcmTrn::handleMessages() - handling 46 messages took 0.73 ms and 132 calls... 2021-02-12T23:29:27Z- waiting for 1613172564.94, time is 1613172567.19 2021-02-12T23:29:27Z- LcmTrn::updateTrn() equating pose 1613172567.19 and meas times 1613172567.19 2021-02-12T23:29:27Z- LcmTrn::updateTrn() >>>> pose time 1613172567.19, meas time 1613172567.19 <<<< 2021-02-12T23:29:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172567.19 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:29:27Z- TerrainNav::motionUpdate() - currEstimate = (4057109, 591041.17, 28.48) 2021-02-12T23:29:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:29:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:29:27Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172567.19 sec.; No relevant navigation data available 2021-02-12T23:29:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:29:27Z- TNavBF::mmse selected from filter -1 2021-02-12T23:29:27Z- variances([0], [2], and psi_berg) = 760.905374 833.043932 0.000000 2021-02-12T23:29:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:29:27Z- LcmTrn::publishEstimates() - MLE : 4057091.70 590997.67 28.48 2021-02-12T23:29:27Z- LcmTrn::publishEstimates() - MMSE : 4057109.20 591041.17 28.48 2021-02-12T23:29:27Z- LcmTrn::publishEstimates() - COVAR: 760.91 833.04 1.24 0.00 2021-02-12T23:29:27Z- AHRS_M2 msg: 1613172567.56 epoch sec; seqNo:4176 2021-02-12T23:29:27Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.14 psi 2021-02-12T23:29:27Z- handleDepth()-Depth_Keller msg: 1613172567574 epoch sec; seqNo:4217; depth 28.34 2021-02-12T23:29:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:27Z- handleDvl() - RDI_Pathfinder msg: 1613172567.58 epoch sec; seqNo:2041 2021-02-12T23:29:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.07 , 0.05 2021-02-12T23:29:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172567.59 epoch sec; seqNo:4217; 4057109.58 north; 591041.17 east 2021-02-12T23:29:27Z- AHRS_M2 msg: 1613172567.96 epoch sec; seqNo:4177 2021-02-12T23:29:27Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.14 psi 2021-02-12T23:29:27Z- handleDepth()-Depth_Keller msg: 1613172567969 epoch sec; seqNo:4218; depth 28.22 2021-02-12T23:29:27Z- handleDvl() - RDI_Pathfinder msg: 1613172567.99 epoch sec; seqNo:2042 2021-02-12T23:29:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.97 , 78.00 , 0.00 , 60.20 2021-02-12T23:29:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , 0.09 2021-02-12T23:29:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172568.00 epoch sec; seqNo:4218; 4057109.95 north; 591041.17 east 2021-02-12T23:29:28Z- AHRS_M2 msg: 1613172568.36 epoch sec; seqNo:4178 2021-02-12T23:29:28Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.15 psi 2021-02-12T23:29:28Z- handleDepth()-Depth_Keller msg: 1613172568368 epoch sec; seqNo:4219; depth 28.10 2021-02-12T23:29:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172568.38 epoch sec; seqNo:4219; 4057109.95 north; 591041.17 east 2021-02-12T23:29:28Z- AHRS_M2 msg: 1613172568.80 epoch sec; seqNo:4179 2021-02-12T23:29:28Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.15 psi 2021-02-12T23:29:28Z- handleDepth()-Depth_Keller msg: 1613172568808 epoch sec; seqNo:4220; depth 27.99 2021-02-12T23:29:28Z- handleDvl() - RDI_Pathfinder msg: 1613172568.85 epoch sec; seqNo:2043 2021-02-12T23:29:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.29 , 78.32 , 0.00 , 60.20 2021-02-12T23:29:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , 0.08 2021-02-12T23:29:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172568.86 epoch sec; seqNo:4220; 4057110.35 north; 591042.38 east 2021-02-12T23:29:29Z- handleDepth()-Depth_Keller msg: 1613172569198 epoch sec; seqNo:4221; depth 27.85 2021-02-12T23:29:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172569.21 epoch sec; seqNo:4221; 4057110.72 north; 591042.38 east 2021-02-12T23:29:29Z- AHRS_M2 msg: 1613172569.58 epoch sec; seqNo:4180 2021-02-12T23:29:29Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.15 psi 2021-02-12T23:29:29Z- handleDepth()-Depth_Keller msg: 1613172569585 epoch sec; seqNo:4222; depth 27.75 2021-02-12T23:29:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172569.61 epoch sec; seqNo:4222; 4057111.10 north; 591042.38 east 2021-02-12T23:29:29Z- AHRS_M2 msg: 1613172569.99 epoch sec; seqNo:4181 2021-02-12T23:29:29Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.14 psi 2021-02-12T23:29:29Z- handleDepth()-Depth_Keller msg: 1613172569990 epoch sec; seqNo:4223; depth 27.63 2021-02-12T23:29:30Z- handleDvl() - RDI_Pathfinder msg: 1613172570.00 epoch sec; seqNo:2044 2021-02-12T23:29:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.94 , 78.32 , 0.00 , 60.52 2021-02-12T23:29:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , 0.03 2021-02-12T23:29:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172570.03 epoch sec; seqNo:4223; 4057111.10 north; 591042.38 east 2021-02-12T23:29:30Z- AHRS_M2 msg: 1613172570.38 epoch sec; seqNo:4182 2021-02-12T23:29:30Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.14 psi 2021-02-12T23:29:30Z- handleDepth()-Depth_Keller msg: 1613172570388 epoch sec; seqNo:4224; depth 27.52 2021-02-12T23:29:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172570.40 epoch sec; seqNo:4224; 4057111.48 north; 591042.37 east 2021-02-12T23:29:30Z- AHRS_M2 msg: 1613172570.80 epoch sec; seqNo:4183 2021-02-12T23:29:30Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.13 psi 2021-02-12T23:29:30Z- handleDepth()-Depth_Keller msg: 1613172570811 epoch sec; seqNo:4225; depth 27.39 2021-02-12T23:29:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172570.83 epoch sec; seqNo:4225; 4057111.86 north; 591042.37 east 2021-02-12T23:29:31Z- AHRS_M2 msg: 1613172571.19 epoch sec; seqNo:4184 2021-02-12T23:29:31Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-12T23:29:31Z- handleDepth()-Depth_Keller msg: 1613172571196 epoch sec; seqNo:4226; depth 27.26 2021-02-12T23:29:31Z- handleDvl() - RDI_Pathfinder msg: 1613172571.21 epoch sec; seqNo:2045 2021-02-12T23:29:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.26 , 78.32 , 0.00 , 61.49 2021-02-12T23:29:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , -0.02 2021-02-12T23:29:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172571.23 epoch sec; seqNo:4226; 4057111.87 north; 591043.59 east 2021-02-12T23:29:31Z- AHRS_M2 msg: 1613172571.60 epoch sec; seqNo:4185 2021-02-12T23:29:31Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.13 psi 2021-02-12T23:29:31Z- handleDepth()-Depth_Keller msg: 1613172571605 epoch sec; seqNo:4227; depth 27.15 2021-02-12T23:29:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172571.62 epoch sec; seqNo:4227; 4057112.25 north; 591043.58 east 2021-02-12T23:29:32Z- AHRS_M2 msg: 1613172572.00 epoch sec; seqNo:4186 2021-02-12T23:29:32Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.13 psi 2021-02-12T23:29:32Z- handleDepth()-Depth_Keller msg: 1613172572013 epoch sec; seqNo:4228; depth 27.04 2021-02-12T23:29:32Z- handleDvl() - RDI_Pathfinder msg: 1613172572.02 epoch sec; seqNo:2046 2021-02-12T23:29:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.26 , 78.00 , 0.00 , 61.49 2021-02-12T23:29:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , -0.05 2021-02-12T23:29:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172572.03 epoch sec; seqNo:4228; 4057112.63 north; 591043.58 east 2021-02-12T23:29:32Z- LcmTrn::handleMessages() - handling 41 messages took 0.58 ms and 130 calls... 2021-02-12T23:29:32Z- waiting for 1613172570.19, time is 1613172572.03 2021-02-12T23:29:32Z- LcmTrn::updateTrn() equating pose 1613172572.03 and meas times 1613172572.03 2021-02-12T23:29:32Z- LcmTrn::updateTrn() >>>> pose time 1613172572.03, meas time 1613172572.03 <<<< 2021-02-12T23:29:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172572.03 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:29:32Z- TerrainNav::motionUpdate() - currEstimate = (4057113, 591043.58, 27.04) 2021-02-12T23:29:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:29:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:29:32Z- TNavBF::Projecting Measurements 2021-02-12T23:29:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:29:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:29:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:29:32Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:29:32Z- TNavFilter::3 good beams. 2021-02-12T23:29:32Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:29:32Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:29:32Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:29:32Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:29:32Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:29:32Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:29:32Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:29:32Z- TNavBF::mmse selected from filter -1 2021-02-12T23:29:32Z- TerrainMapOctree: (vehN, vehE ) = (4057112.63, 591043.58). 2021-02-12T23:29:32Z- TerrainMapOctree: Min Distance = 5234.40. 2021-02-12T23:29:32Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:29:32Z- There is no specific good beam that all particles have in common. 2021-02-12T23:29:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:29:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.46 K: 0.134 ExpectedMeasDiff: nan currMeas.covariance: 7.68 2021-02-12T23:29:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:29:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 9.73 2021-02-12T23:29:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:29:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.127 ExpectedMeasDiff: nan currMeas.covariance: 6.05 2021-02-12T23:29:32Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:29:32Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:29:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:29:32Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:29:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:29:32Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:29:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:29:32Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:29:32Z- Filter 0 No beams used in subcloud update 2021-02-12T23:29:32Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:29:32Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:29:32Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 760.8 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:29:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172572.03 sec. 2021-02-12T23:29:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:29:32Z- TNavBF::mmse selected from filter -1 2021-02-12T23:29:32Z- variances([0], [2], and psi_berg) = 760.789524 833.043808 0.000000 2021-02-12T23:29:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:29:32Z- LcmTrn::publishEstimates() - MLE : 4057095.13 591000.08 27.04 2021-02-12T23:29:32Z- LcmTrn::publishEstimates() - MMSE : 4057112.63 591043.58 27.04 2021-02-12T23:29:32Z- LcmTrn::publishEstimates() - COVAR: 760.79 833.04 0.99 0.00 2021-02-12T23:29:32Z- AHRS_M2 msg: 1613172572.40 epoch sec; seqNo:4187 2021-02-12T23:29:32Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.12 psi 2021-02-12T23:29:32Z- handleDepth()-Depth_Keller msg: 1613172572408 epoch sec; seqNo:4229; depth 26.92 2021-02-12T23:29:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:32Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:32Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:32Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:32Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:32Z- handleDvl() - RDI_Pathfinder msg: 1613172572.41 epoch sec; seqNo:2047 2021-02-12T23:29:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , -0.05 2021-02-12T23:29:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172572.43 epoch sec; seqNo:4229; 4057113.01 north; 591043.58 east 2021-02-12T23:29:32Z- AHRS_M2 msg: 1613172572.83 epoch sec; seqNo:4188 2021-02-12T23:29:32Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.13 psi 2021-02-12T23:29:32Z- handleDepth()-Depth_Keller msg: 1613172572843 epoch sec; seqNo:4230; depth 26.79 2021-02-12T23:29:32Z- handleDvl() - RDI_Pathfinder msg: 1613172572.86 epoch sec; seqNo:2048 2021-02-12T23:29:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.58 , 78.32 , 0.00 , 61.17 2021-02-12T23:29:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , -0.03 2021-02-12T23:29:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172572.87 epoch sec; seqNo:4230; 4057113.01 north; 591043.58 east 2021-02-12T23:29:33Z- AHRS_M2 msg: 1613172573.22 epoch sec; seqNo:4189 2021-02-12T23:29:33Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.13 psi 2021-02-12T23:29:33Z- handleDepth()-Depth_Keller msg: 1613172573228 epoch sec; seqNo:4231; depth 26.67 2021-02-12T23:29:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172573.26 epoch sec; seqNo:4231; 4057113.39 north; 591043.57 east 2021-02-12T23:29:33Z- AHRS_M2 msg: 1613172573.62 epoch sec; seqNo:4190 2021-02-12T23:29:33Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.13 psi 2021-02-12T23:29:33Z- handleDepth()-Depth_Keller msg: 1613172573622 epoch sec; seqNo:4232; depth 26.56 2021-02-12T23:29:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:33Z- handleDvl() - RDI_Pathfinder msg: 1613172573.62 epoch sec; seqNo:2049 2021-02-12T23:29:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , -0.03 2021-02-12T23:29:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172573.63 epoch sec; seqNo:4232; 4057113.77 north; 591043.57 east 2021-02-12T23:29:34Z- AHRS_M2 msg: 1613172574.02 epoch sec; seqNo:4191 2021-02-12T23:29:34Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.14 psi 2021-02-12T23:29:34Z- handleDepth()-Depth_Keller msg: 1613172574025 epoch sec; seqNo:4233; depth 26.44 2021-02-12T23:29:34Z- handleDvl() - RDI_Pathfinder msg: 1613172574.04 epoch sec; seqNo:2050 2021-02-12T23:29:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.58 , 78.97 , 0.00 , 62.14 2021-02-12T23:29:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , 0.01 2021-02-12T23:29:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172574.06 epoch sec; seqNo:4233; 4057114.16 north; 591044.78 east 2021-02-12T23:29:34Z- AHRS_M2 msg: 1613172574.42 epoch sec; seqNo:4192 2021-02-12T23:29:34Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.15 psi 2021-02-12T23:29:34Z- handleDepth()-Depth_Keller msg: 1613172574433 epoch sec; seqNo:4234; depth 26.32 2021-02-12T23:29:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172574.44 epoch sec; seqNo:4234; 4057114.16 north; 591044.78 east 2021-02-12T23:29:34Z- AHRS_M2 msg: 1613172574.84 epoch sec; seqNo:4193 2021-02-12T23:29:34Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.15 psi 2021-02-12T23:29:34Z- handleDepth()-Depth_Keller msg: 1613172574846 epoch sec; seqNo:4235; depth 26.21 2021-02-12T23:29:34Z- handleDvl() - RDI_Pathfinder msg: 1613172574.86 epoch sec; seqNo:2051 2021-02-12T23:29:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.58 , 80.27 , 0.00 , 62.47 2021-02-12T23:29:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.06 2021-02-12T23:29:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172574.88 epoch sec; seqNo:4235; 4057114.54 north; 591044.78 east 2021-02-12T23:29:35Z- AHRS_M2 msg: 1613172575.23 epoch sec; seqNo:4194 2021-02-12T23:29:35Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:29:35Z- handleDepth()-Depth_Keller msg: 1613172575245 epoch sec; seqNo:4236; depth 26.07 2021-02-12T23:29:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172575.26 epoch sec; seqNo:4236; 4057114.92 north; 591044.78 east 2021-02-12T23:29:35Z- AHRS_M2 msg: 1613172575.63 epoch sec; seqNo:4195 2021-02-12T23:29:35Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:29:35Z- handleDepth()-Depth_Keller msg: 1613172575655 epoch sec; seqNo:4237; depth 25.97 2021-02-12T23:29:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172575.69 epoch sec; seqNo:4237; 4057114.92 north; 591044.78 east 2021-02-12T23:29:36Z- AHRS_M2 msg: 1613172576.04 epoch sec; seqNo:4196 2021-02-12T23:29:36Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-12T23:29:36Z- handleDepth()-Depth_Keller msg: 1613172576044 epoch sec; seqNo:4238; depth 25.84 2021-02-12T23:29:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:36Z- handleDvl() - RDI_Pathfinder msg: 1613172576.05 epoch sec; seqNo:2052 2021-02-12T23:29:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.06 2021-02-12T23:29:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172576.05 epoch sec; seqNo:4238; 4057115.30 north; 591044.77 east 2021-02-12T23:29:36Z- AHRS_M2 msg: 1613172576.44 epoch sec; seqNo:4197 2021-02-12T23:29:36Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-12T23:29:36Z- handleDepth()-Depth_Keller msg: 1613172576450 epoch sec; seqNo:4239; depth 25.74 2021-02-12T23:29:36Z- handleDvl() - RDI_Pathfinder msg: 1613172576.46 epoch sec; seqNo:2053 2021-02-12T23:29:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 85.00 , 97.98 , 62.52 2021-02-12T23:29:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.04 2021-02-12T23:29:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172576.47 epoch sec; seqNo:4239; 4057115.69 north; 591045.99 east 2021-02-12T23:29:36Z- AHRS_M2 msg: 1613172576.85 epoch sec; seqNo:4198 2021-02-12T23:29:36Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:29:36Z- handleDepth()-Depth_Keller msg: 1613172576861 epoch sec; seqNo:4240; depth 25.61 2021-02-12T23:29:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172576.87 epoch sec; seqNo:4240; 4057116.07 north; 591045.99 east 2021-02-12T23:29:37Z- AHRS_M2 msg: 1613172577.25 epoch sec; seqNo:4199 2021-02-12T23:29:37Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:29:37Z- handleDepth()-Depth_Keller msg: 1613172577256 epoch sec; seqNo:4241; depth 25.50 2021-02-12T23:29:37Z- handleDvl() - RDI_Pathfinder msg: 1613172577.28 epoch sec; seqNo:2054 2021-02-12T23:29:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 85.00 , 97.98 , 61.71 2021-02-12T23:29:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , 0.01 2021-02-12T23:29:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172577.29 epoch sec; seqNo:4241; 4057116.07 north; 591045.99 east 2021-02-12T23:29:37Z- LcmTrn::handleMessages() - handling 47 messages took 0.68 ms and 134 calls... 2021-02-12T23:29:37Z- waiting for 1613172575.03, time is 1613172577.29 2021-02-12T23:29:37Z- LcmTrn::updateTrn() equating pose 1613172577.29 and meas times 1613172577.29 2021-02-12T23:29:37Z- LcmTrn::updateTrn() >>>> pose time 1613172577.29, meas time 1613172577.29 <<<< 2021-02-12T23:29:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172577.29 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:29:37Z- TerrainNav::motionUpdate() - currEstimate = (4057116, 591045.99, 25.50) 2021-02-12T23:29:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:29:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:29:37Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172577.29 sec.; No relevant navigation data available 2021-02-12T23:29:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:29:37Z- TNavBF::mmse selected from filter -1 2021-02-12T23:29:37Z- variances([0], [2], and psi_berg) = 760.673867 833.043684 0.000000 2021-02-12T23:29:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:29:37Z- LcmTrn::publishEstimates() - MLE : 4057098.57 591002.49 25.50 2021-02-12T23:29:37Z- LcmTrn::publishEstimates() - MMSE : 4057116.07 591045.99 25.50 2021-02-12T23:29:37Z- LcmTrn::publishEstimates() - COVAR: 760.67 833.04 1.21 0.00 2021-02-12T23:29:37Z- AHRS_M2 msg: 1613172577.65 epoch sec; seqNo:4200 2021-02-12T23:29:37Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:29:37Z- handleDepth()-Depth_Keller msg: 1613172577665 epoch sec; seqNo:4242; depth 25.39 2021-02-12T23:29:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172577.68 epoch sec; seqNo:4242; 4057116.45 north; 591045.98 east 2021-02-12T23:29:38Z- AHRS_M2 msg: 1613172578.06 epoch sec; seqNo:4201 2021-02-12T23:29:38Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:29:38Z- handleDepth()-Depth_Keller msg: 1613172578065 epoch sec; seqNo:4243; depth 25.28 2021-02-12T23:29:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172578.09 epoch sec; seqNo:4243; 4057116.82 north; 591045.98 east 2021-02-12T23:29:38Z- AHRS_M2 msg: 1613172578.46 epoch sec; seqNo:4202 2021-02-12T23:29:38Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-12T23:29:38Z- handleDepth()-Depth_Keller msg: 1613172578468 epoch sec; seqNo:4244; depth 25.15 2021-02-12T23:29:38Z- handleDvl() - RDI_Pathfinder msg: 1613172578.48 epoch sec; seqNo:2055 2021-02-12T23:29:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 87.17 , 96.90 , 63.75 2021-02-12T23:29:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.02 , -0.05 2021-02-12T23:29:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172578.50 epoch sec; seqNo:4244; 4057116.82 north; 591045.98 east 2021-02-12T23:29:38Z- AHRS_M2 msg: 1613172578.87 epoch sec; seqNo:4203 2021-02-12T23:29:38Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-12T23:29:38Z- handleDepth()-Depth_Keller msg: 1613172578872 epoch sec; seqNo:4245; depth 25.04 2021-02-12T23:29:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172578.88 epoch sec; seqNo:4245; 4057117.22 north; 591047.19 east 2021-02-12T23:29:39Z- AHRS_M2 msg: 1613172579.27 epoch sec; seqNo:4204 2021-02-12T23:29:39Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-12T23:29:39Z- handleDepth()-Depth_Keller msg: 1613172579285 epoch sec; seqNo:4246; depth 24.92 2021-02-12T23:29:39Z- handleDvl() - RDI_Pathfinder msg: 1613172579.30 epoch sec; seqNo:2056 2021-02-12T23:29:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 86.09 , 97.98 , 62.52 2021-02-12T23:29:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.05 2021-02-12T23:29:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172579.31 epoch sec; seqNo:4246; 4057117.60 north; 591047.19 east 2021-02-12T23:29:39Z- AHRS_M2 msg: 1613172579.68 epoch sec; seqNo:4205 2021-02-12T23:29:39Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:29:39Z- handleDepth()-Depth_Keller msg: 1613172579680 epoch sec; seqNo:4247; depth 24.80 2021-02-12T23:29:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:39Z- handleDvl() - RDI_Pathfinder msg: 1613172579.68 epoch sec; seqNo:2057 2021-02-12T23:29:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.05 2021-02-12T23:29:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172579.71 epoch sec; seqNo:4247; 4057117.60 north; 591047.19 east 2021-02-12T23:29:40Z- AHRS_M2 msg: 1613172580.08 epoch sec; seqNo:4206 2021-02-12T23:29:40Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:29:40Z- handleDepth()-Depth_Keller msg: 1613172580083 epoch sec; seqNo:4248; depth 24.69 2021-02-12T23:29:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172580.09 epoch sec; seqNo:4248; 4057117.97 north; 591047.19 east 2021-02-12T23:29:40Z- AHRS_M2 msg: 1613172580.48 epoch sec; seqNo:4207 2021-02-12T23:29:40Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:29:40Z- handleDepth()-Depth_Keller msg: 1613172580493 epoch sec; seqNo:4249; depth 24.57 2021-02-12T23:29:40Z- handleDvl() - RDI_Pathfinder msg: 1613172580.50 epoch sec; seqNo:2058 2021-02-12T23:29:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 87.17 , 97.98 , 64.16 2021-02-12T23:29:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.06 , -0.02 2021-02-12T23:29:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172580.51 epoch sec; seqNo:4249; 4057118.35 north; 591047.18 east 2021-02-12T23:29:40Z- AHRS_M2 msg: 1613172580.89 epoch sec; seqNo:4208 2021-02-12T23:29:40Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:29:40Z- handleDepth()-Depth_Keller msg: 1613172580894 epoch sec; seqNo:4250; depth 24.46 2021-02-12T23:29:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:40Z- handleDvl() - RDI_Pathfinder msg: 1613172580.90 epoch sec; seqNo:2059 2021-02-12T23:29:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.06 , -0.02 2021-02-12T23:29:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172580.91 epoch sec; seqNo:4250; 4057118.35 north; 591047.18 east 2021-02-12T23:29:41Z- AHRS_M2 msg: 1613172581.29 epoch sec; seqNo:4209 2021-02-12T23:29:41Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:29:41Z- handleDepth()-Depth_Keller msg: 1613172581296 epoch sec; seqNo:4251; depth 24.35 2021-02-12T23:29:41Z- handleDvl() - RDI_Pathfinder msg: 1613172581.31 epoch sec; seqNo:2060 2021-02-12T23:29:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 88.25 , 97.98 , 64.16 2021-02-12T23:29:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , 0.06 2021-02-12T23:29:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172581.33 epoch sec; seqNo:4251; 4057118.73 north; 591047.18 east 2021-02-12T23:29:41Z- AHRS_M2 msg: 1613172581.69 epoch sec; seqNo:4210 2021-02-12T23:29:41Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-12T23:29:41Z- handleDepth()-Depth_Keller msg: 1613172581705 epoch sec; seqNo:4252; depth 24.22 2021-02-12T23:29:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172581.72 epoch sec; seqNo:4252; 4057119.12 north; 591048.40 east 2021-02-12T23:29:42Z- AHRS_M2 msg: 1613172582.10 epoch sec; seqNo:4211 2021-02-12T23:29:42Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-12T23:29:42Z- handleDepth()-Depth_Keller msg: 1613172582115 epoch sec; seqNo:4253; depth 24.10 2021-02-12T23:29:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:42Z- handleDvl() - RDI_Pathfinder msg: 1613172582.12 epoch sec; seqNo:2061 2021-02-12T23:29:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , 0.06 2021-02-12T23:29:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172582.14 epoch sec; seqNo:4253; 4057119.50 north; 591048.39 east 2021-02-12T23:29:42Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-12T23:29:42Z- waiting for 1613172580.29, time is 1613172582.14 2021-02-12T23:29:42Z- LcmTrn::updateTrn() equating pose 1613172582.14 and meas times 1613172582.14 2021-02-12T23:29:42Z- LcmTrn::updateTrn() >>>> pose time 1613172582.14, meas time 1613172582.14 <<<< 2021-02-12T23:29:42Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172582.14, ping # 0. 2021-02-12T23:29:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172582.14 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:29:42Z- TerrainNav::motionUpdate() - currEstimate = (4057120, 591048.39, 24.10) 2021-02-12T23:29:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:29:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:29:42Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:29:42Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:29:42Z- TNavBF::Projecting Measurements 2021-02-12T23:29:42Z- TNavFilter::Measurement from time = 1613172582.14 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:29:42Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:29:42Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:29:42Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:29:42Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:29:42Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:29:42Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:29:42Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:29:42Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 760.6 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-12T23:29:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:29:42Z- TNavBF::mmse selected from filter -1 2021-02-12T23:29:42Z- variances([0], [2], and psi_berg) = 760.558401 833.043560 0.000000 2021-02-12T23:29:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:29:42Z- LcmTrn::publishEstimates() - MLE : 4057102.00 591095.89 24.10 2021-02-12T23:29:42Z- LcmTrn::publishEstimates() - MMSE : 4057119.50 591048.39 24.10 2021-02-12T23:29:42Z- LcmTrn::publishEstimates() - COVAR: 760.56 833.04 1.42 0.00 2021-02-12T23:29:42Z- AHRS_M2 msg: 1613172582.50 epoch sec; seqNo:4212 2021-02-12T23:29:42Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-12T23:29:42Z- handleDepth()-Depth_Keller msg: 1613172582508 epoch sec; seqNo:4254; depth 24.00 2021-02-12T23:29:42Z- handleDvl() - RDI_Pathfinder msg: 1613172582.53 epoch sec; seqNo:2062 2021-02-12T23:29:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 0.00 , 97.98 , 64.57 2021-02-12T23:29:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.09 2021-02-12T23:29:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172582.54 epoch sec; seqNo:4254; 4057119.88 north; 591048.39 east 2021-02-12T23:29:42Z- AHRS_M2 msg: 1613172582.91 epoch sec; seqNo:4213 2021-02-12T23:29:42Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-12T23:29:42Z- handleDepth()-Depth_Keller msg: 1613172582911 epoch sec; seqNo:4255; depth 23.90 2021-02-12T23:29:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172582.92 epoch sec; seqNo:4255; 4057119.88 north; 591048.39 east 2021-02-12T23:29:43Z- AHRS_M2 msg: 1613172583.31 epoch sec; seqNo:4214 2021-02-12T23:29:43Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-12T23:29:43Z- handleDepth()-Depth_Keller msg: 1613172583319 epoch sec; seqNo:4256; depth 23.78 2021-02-12T23:29:43Z- handleDvl() - RDI_Pathfinder msg: 1613172583.33 epoch sec; seqNo:2063 2021-02-12T23:29:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 89.33 , 97.98 , 64.57 2021-02-12T23:29:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , 0.09 2021-02-12T23:29:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172583.34 epoch sec; seqNo:4256; 4057120.26 north; 591048.38 east 2021-02-12T23:29:43Z- AHRS_M2 msg: 1613172583.71 epoch sec; seqNo:4215 2021-02-12T23:29:43Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-12T23:29:43Z- handleDepth()-Depth_Keller msg: 1613172583721 epoch sec; seqNo:4257; depth 23.66 2021-02-12T23:29:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172583.75 epoch sec; seqNo:4257; 4057120.64 north; 591048.38 east 2021-02-12T23:29:44Z- AHRS_M2 msg: 1613172584.12 epoch sec; seqNo:4216 2021-02-12T23:29:44Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-12T23:29:44Z- handleDepth()-Depth_Keller msg: 1613172584128 epoch sec; seqNo:4258; depth 23.55 2021-02-12T23:29:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172584.14 epoch sec; seqNo:4258; 4057121.03 north; 591049.60 east 2021-02-12T23:29:44Z- AHRS_M2 msg: 1613172584.52 epoch sec; seqNo:4217 2021-02-12T23:29:44Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:29:44Z- handleDepth()-Depth_Keller msg: 1613172584529 epoch sec; seqNo:4259; depth 23.44 2021-02-12T23:29:44Z- handleDvl() - RDI_Pathfinder msg: 1613172584.55 epoch sec; seqNo:2064 2021-02-12T23:29:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 88.18 , 96.83 , 64.97 2021-02-12T23:29:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.03 2021-02-12T23:29:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172584.56 epoch sec; seqNo:4259; 4057121.03 north; 591049.60 east 2021-02-12T23:29:44Z- AHRS_M2 msg: 1613172584.93 epoch sec; seqNo:4218 2021-02-12T23:29:44Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:29:44Z- handleDepth()-Depth_Keller msg: 1613172584932 epoch sec; seqNo:4260; depth 23.33 2021-02-12T23:29:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172584.94 epoch sec; seqNo:4260; 4057121.41 north; 591049.59 east 2021-02-12T23:29:45Z- AHRS_M2 msg: 1613172585.33 epoch sec; seqNo:4219 2021-02-12T23:29:45Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-12T23:29:45Z- handleDepth()-Depth_Keller msg: 1613172585338 epoch sec; seqNo:4261; depth 23.23 2021-02-12T23:29:45Z- handleDvl() - RDI_Pathfinder msg: 1613172585.35 epoch sec; seqNo:2065 2021-02-12T23:29:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 87.10 , 97.92 , 64.57 2021-02-12T23:29:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.06 , -0.04 2021-02-12T23:29:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172585.36 epoch sec; seqNo:4261; 4057121.79 north; 591049.59 east 2021-02-12T23:29:45Z- AHRS_M2 msg: 1613172585.74 epoch sec; seqNo:4220 2021-02-12T23:29:45Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-12T23:29:45Z- handleDepth()-Depth_Keller msg: 1613172585741 epoch sec; seqNo:4262; depth 23.12 2021-02-12T23:29:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:45Z- handleDvl() - RDI_Pathfinder msg: 1613172585.74 epoch sec; seqNo:2066 2021-02-12T23:29:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.06 , -0.04 2021-02-12T23:29:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172585.76 epoch sec; seqNo:4262; 4057122.17 north; 591049.58 east 2021-02-12T23:29:46Z- AHRS_M2 msg: 1613172586.14 epoch sec; seqNo:4221 2021-02-12T23:29:46Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-12T23:29:46Z- handleDepth()-Depth_Keller msg: 1613172586153 epoch sec; seqNo:4263; depth 23.00 2021-02-12T23:29:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172586.17 epoch sec; seqNo:4263; 4057122.17 north; 591049.58 east 2021-02-12T23:29:46Z- AHRS_M2 msg: 1613172586.54 epoch sec; seqNo:4222 2021-02-12T23:29:46Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-12T23:29:46Z- handleDepth()-Depth_Keller msg: 1613172586548 epoch sec; seqNo:4264; depth 22.90 2021-02-12T23:29:46Z- handleDvl() - RDI_Pathfinder msg: 1613172586.57 epoch sec; seqNo:2067 2021-02-12T23:29:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 88.18 , 96.83 , 65.38 2021-02-12T23:29:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.03 , -0.07 2021-02-12T23:29:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172586.60 epoch sec; seqNo:4264; 4057122.56 north; 591050.80 east 2021-02-12T23:29:46Z- AHRS_M2 msg: 1613172586.95 epoch sec; seqNo:4223 2021-02-12T23:29:46Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-12T23:29:46Z- handleDepth()-Depth_Keller msg: 1613172586959 epoch sec; seqNo:4265; depth 22.79 2021-02-12T23:29:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:46Z- handleDvl() - RDI_Pathfinder msg: 1613172586.96 epoch sec; seqNo:2068 2021-02-12T23:29:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.03 , -0.07 2021-02-12T23:29:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172586.97 epoch sec; seqNo:4265; 4057122.56 north; 591050.80 east 2021-02-12T23:29:47Z- AHRS_M2 msg: 1613172587.36 epoch sec; seqNo:4224 2021-02-12T23:29:47Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:29:47Z- handleDepth()-Depth_Keller msg: 1613172587359 epoch sec; seqNo:4266; depth 22.66 2021-02-12T23:29:47Z- handleDvl() - RDI_Pathfinder msg: 1613172587.38 epoch sec; seqNo:2069 2021-02-12T23:29:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 89.26 , 99.00 , 65.79 2021-02-12T23:29:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.00 , -0.07 2021-02-12T23:29:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172587.39 epoch sec; seqNo:4266; 4057122.94 north; 591050.80 east 2021-02-12T23:29:47Z- LcmTrn::handleMessages() - handling 47 messages took 0.67 ms and 134 calls... 2021-02-12T23:29:47Z- waiting for 1613172585.14, time is 1613172587.39 2021-02-12T23:29:47Z- LcmTrn::updateTrn() equating pose 1613172587.39 and meas times 1613172587.39 2021-02-12T23:29:47Z- LcmTrn::updateTrn() >>>> pose time 1613172587.39, meas time 1613172587.39 <<<< 2021-02-12T23:29:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172587.39 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:29:47Z- TerrainNav::motionUpdate() - currEstimate = (4057123, 591050.80, 22.66) 2021-02-12T23:29:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:29:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:29:47Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172587.39 sec.; No relevant navigation data available 2021-02-12T23:29:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:29:47Z- TNavBF::mmse selected from filter -1 2021-02-12T23:29:47Z- variances([0], [2], and psi_berg) = 760.443126 833.043437 0.000000 2021-02-12T23:29:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:29:47Z- LcmTrn::publishEstimates() - MLE : 4057106.44 591002.30 22.66 2021-02-12T23:29:47Z- LcmTrn::publishEstimates() - MMSE : 4057122.94 591050.80 22.66 2021-02-12T23:29:47Z- LcmTrn::publishEstimates() - COVAR: 760.44 833.04 1.63 0.00 2021-02-12T23:29:47Z- AHRS_M2 msg: 1613172587.76 epoch sec; seqNo:4225 2021-02-12T23:29:47Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:29:47Z- handleDepth()-Depth_Keller msg: 1613172587765 epoch sec; seqNo:4267; depth 22.55 2021-02-12T23:29:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172587.78 epoch sec; seqNo:4267; 4057123.32 north; 591050.79 east 2021-02-12T23:29:48Z- AHRS_M2 msg: 1613172588.16 epoch sec; seqNo:4226 2021-02-12T23:29:48Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:29:48Z- handleDepth()-Depth_Keller msg: 1613172588169 epoch sec; seqNo:4268; depth 22.44 2021-02-12T23:29:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:48Z- handleDvl() - RDI_Pathfinder msg: 1613172588.17 epoch sec; seqNo:2070 2021-02-12T23:29:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.00 , -0.07 2021-02-12T23:29:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172588.19 epoch sec; seqNo:4268; 4057123.32 north; 591050.79 east 2021-02-12T23:29:48Z- AHRS_M2 msg: 1613172588.56 epoch sec; seqNo:4227 2021-02-12T23:29:48Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:29:48Z- handleDepth()-Depth_Keller msg: 1613172588568 epoch sec; seqNo:4269; depth 22.32 2021-02-12T23:29:48Z- handleDvl() - RDI_Pathfinder msg: 1613172588.58 epoch sec; seqNo:2071 2021-02-12T23:29:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 89.20 , 101.09 , 66.20 2021-02-12T23:29:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , -0.03 2021-02-12T23:29:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172588.60 epoch sec; seqNo:4269; 4057123.70 north; 591050.79 east 2021-02-12T23:29:48Z- AHRS_M2 msg: 1613172588.97 epoch sec; seqNo:4228 2021-02-12T23:29:48Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.18 psi 2021-02-12T23:29:48Z- handleDepth()-Depth_Keller msg: 1613172588977 epoch sec; seqNo:4270; depth 22.20 2021-02-12T23:29:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172588.99 epoch sec; seqNo:4270; 4057124.07 north; 591050.79 east 2021-02-12T23:29:49Z- AHRS_M2 msg: 1613172589.37 epoch sec; seqNo:4229 2021-02-12T23:29:49Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-12T23:29:49Z- handleDepth()-Depth_Keller msg: 1613172589381 epoch sec; seqNo:4271; depth 22.10 2021-02-12T23:29:49Z- handleDvl() - RDI_Pathfinder msg: 1613172589.41 epoch sec; seqNo:2072 2021-02-12T23:29:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 90.21 , 96.70 , 66.20 2021-02-12T23:29:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , 0.03 2021-02-12T23:29:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172589.42 epoch sec; seqNo:4271; 4057124.47 north; 591052.00 east 2021-02-12T23:29:49Z- AHRS_M2 msg: 1613172589.78 epoch sec; seqNo:4230 2021-02-12T23:29:49Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-12T23:29:49Z- handleDepth()-Depth_Keller msg: 1613172589790 epoch sec; seqNo:4272; depth 21.97 2021-02-12T23:29:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172589.80 epoch sec; seqNo:4272; 4057124.47 north; 591052.00 east 2021-02-12T23:29:50Z- AHRS_M2 msg: 1613172590.19 epoch sec; seqNo:4231 2021-02-12T23:29:50Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-12T23:29:50Z- handleDepth()-Depth_Keller msg: 1613172590215 epoch sec; seqNo:4273; depth 21.86 2021-02-12T23:29:50Z- handleDvl() - RDI_Pathfinder msg: 1613172590.23 epoch sec; seqNo:2073 2021-02-12T23:29:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 90.28 , 98.93 , 66.61 2021-02-12T23:29:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , 0.08 2021-02-12T23:29:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172590.24 epoch sec; seqNo:4273; 4057124.85 north; 591052.00 east 2021-02-12T23:29:50Z- AHRS_M2 msg: 1613172590.58 epoch sec; seqNo:4232 2021-02-12T23:29:50Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-12T23:29:50Z- handleDepth()-Depth_Keller msg: 1613172590588 epoch sec; seqNo:4274; depth 21.74 2021-02-12T23:29:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:50Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:50Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:50Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:50Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:50Z- handleDvl() - RDI_Pathfinder msg: 1613172590.59 epoch sec; seqNo:2074 2021-02-12T23:29:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , 0.08 2021-02-12T23:29:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172590.60 epoch sec; seqNo:4274; 4057125.22 north; 591051.99 east 2021-02-12T23:29:50Z- AHRS_M2 msg: 1613172590.99 epoch sec; seqNo:4233 2021-02-12T23:29:50Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-12T23:29:50Z- handleDepth()-Depth_Keller msg: 1613172590994 epoch sec; seqNo:4275; depth 21.63 2021-02-12T23:29:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172591.00 epoch sec; seqNo:4275; 4057125.60 north; 591051.99 east 2021-02-12T23:29:51Z- AHRS_M2 msg: 1613172591.39 epoch sec; seqNo:4234 2021-02-12T23:29:51Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-12T23:29:51Z- handleDepth()-Depth_Keller msg: 1613172591401 epoch sec; seqNo:4276; depth 21.53 2021-02-12T23:29:51Z- handleDvl() - RDI_Pathfinder msg: 1613172591.42 epoch sec; seqNo:2075 2021-02-12T23:29:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 91.29 , 98.86 , 65.79 2021-02-12T23:29:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , 0.10 2021-02-12T23:29:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172591.43 epoch sec; seqNo:4276; 4057125.98 north; 591051.99 east 2021-02-12T23:29:51Z- AHRS_M2 msg: 1613172591.80 epoch sec; seqNo:4235 2021-02-12T23:29:51Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-12T23:29:51Z- handleDepth()-Depth_Keller msg: 1613172591808 epoch sec; seqNo:4277; depth 21.41 2021-02-12T23:29:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:51Z- handleDvl() - RDI_Pathfinder msg: 1613172591.81 epoch sec; seqNo:2076 2021-02-12T23:29:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , 0.10 2021-02-12T23:29:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172591.85 epoch sec; seqNo:4277; 4057125.98 north; 591051.99 east 2021-02-12T23:29:52Z- AHRS_M2 msg: 1613172592.22 epoch sec; seqNo:4236 2021-02-12T23:29:52Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-12T23:29:52Z- handleDepth()-Depth_Keller msg: 1613172592232 epoch sec; seqNo:4278; depth 21.29 2021-02-12T23:29:52Z- handleDvl() - RDI_Pathfinder msg: 1613172592.26 epoch sec; seqNo:2077 2021-02-12T23:29:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 89.13 , 101.03 , 66.61 2021-02-12T23:29:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.06 2021-02-12T23:29:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172592.27 epoch sec; seqNo:4278; 4057126.37 north; 591053.20 east 2021-02-12T23:29:52Z- LcmTrn::handleMessages() - handling 44 messages took 0.77 ms and 129 calls... 2021-02-12T23:29:52Z- waiting for 1613172590.39, time is 1613172592.27 2021-02-12T23:29:52Z- LcmTrn::updateTrn() equating pose 1613172592.27 and meas times 1613172592.27 2021-02-12T23:29:52Z- LcmTrn::updateTrn() >>>> pose time 1613172592.27, meas time 1613172592.27 <<<< 2021-02-12T23:29:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172592.27 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:29:52Z- TerrainNav::motionUpdate() - currEstimate = (4057126, 591053.20, 21.29) 2021-02-12T23:29:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:29:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:29:52Z- TNavBF::Projecting Measurements 2021-02-12T23:29:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:29:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:29:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:29:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:29:52Z- TNavFilter::4 good beams. 2021-02-12T23:29:52Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:29:52Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:29:52Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:29:52Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:29:52Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:29:52Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:29:52Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:29:52Z- TNavBF::mmse selected from filter -1 2021-02-12T23:29:52Z- TerrainMapOctree: (vehN, vehE ) = (4057126.37, 591053.20). 2021-02-12T23:29:52Z- TerrainMapOctree: Min Distance = 5217.64. 2021-02-12T23:29:52Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:29:52Z- There is no specific good beam that all particles have in common. 2021-02-12T23:29:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:29:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.85 K: 0.150 ExpectedMeasDiff: nan currMeas.covariance: 8.75 2021-02-12T23:29:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:29:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.57 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 12.71 2021-02-12T23:29:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:29:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.42 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 16.33 2021-02-12T23:29:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:29:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.129 ExpectedMeasDiff: nan currMeas.covariance: 7.10 2021-02-12T23:29:52Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:29:52Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:29:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:29:52Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:29:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:29:52Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:29:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:29:52Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:29:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:29:52Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:29:52Z- Filter 0 No beams used in subcloud update 2021-02-12T23:29:52Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:29:52Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:29:52Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 760.3 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:29:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172592.27 sec. 2021-02-12T23:29:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:29:52Z- TNavBF::mmse selected from filter -1 2021-02-12T23:29:52Z- variances([0], [2], and psi_berg) = 760.328042 833.043314 0.000000 2021-02-12T23:29:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:29:52Z- LcmTrn::publishEstimates() - MLE : 4057109.87 591004.70 21.29 2021-02-12T23:29:52Z- LcmTrn::publishEstimates() - MMSE : 4057126.37 591053.20 21.29 2021-02-12T23:29:52Z- LcmTrn::publishEstimates() - COVAR: 760.33 833.04 1.14 0.00 2021-02-12T23:29:52Z- AHRS_M2 msg: 1613172592.60 epoch sec; seqNo:4237 2021-02-12T23:29:52Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-12T23:29:52Z- handleDepth()-Depth_Keller msg: 1613172592618 epoch sec; seqNo:4279; depth 21.17 2021-02-12T23:29:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172592.63 epoch sec; seqNo:4279; 4057126.75 north; 591053.20 east 2021-02-12T23:29:53Z- AHRS_M2 msg: 1613172593.01 epoch sec; seqNo:4238 2021-02-12T23:29:53Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-12T23:29:53Z- handleDepth()-Depth_Keller msg: 1613172593012 epoch sec; seqNo:4280; depth 21.08 2021-02-12T23:29:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:53Z- handleDvl() - RDI_Pathfinder msg: 1613172593.01 epoch sec; seqNo:2078 2021-02-12T23:29:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.06 2021-02-12T23:29:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172593.02 epoch sec; seqNo:4280; 4057127.13 north; 591053.19 east 2021-02-12T23:29:53Z- AHRS_M2 msg: 1613172593.41 epoch sec; seqNo:4239 2021-02-12T23:29:53Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-12T23:29:53Z- handleDepth()-Depth_Keller msg: 1613172593416 epoch sec; seqNo:4281; depth 20.96 2021-02-12T23:29:53Z- handleDvl() - RDI_Pathfinder msg: 1613172593.43 epoch sec; seqNo:2079 2021-02-12T23:29:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 0.00 , 99.61 , 65.79 2021-02-12T23:29:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.10 , 0.01 2021-02-12T23:29:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172593.44 epoch sec; seqNo:4281; 4057127.13 north; 591053.19 east 2021-02-12T23:29:53Z- AHRS_M2 msg: 1613172593.82 epoch sec; seqNo:4240 2021-02-12T23:29:53Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.13 psi 2021-02-12T23:29:53Z- handleDepth()-Depth_Keller msg: 1613172593829 epoch sec; seqNo:4282; depth 20.84 2021-02-12T23:29:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172593.84 epoch sec; seqNo:4282; 4057127.51 north; 591053.19 east 2021-02-12T23:29:54Z- AHRS_M2 msg: 1613172594.23 epoch sec; seqNo:4241 2021-02-12T23:29:54Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.13 psi 2021-02-12T23:29:54Z- handleDepth()-Depth_Keller msg: 1613172594232 epoch sec; seqNo:4283; depth 20.74 2021-02-12T23:29:54Z- handleDvl() - RDI_Pathfinder msg: 1613172594.25 epoch sec; seqNo:2080 2021-02-12T23:29:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.60 , 88.25 , 96.90 , 67.02 2021-02-12T23:29:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.06 , -0.04 2021-02-12T23:29:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172594.27 epoch sec; seqNo:4283; 4057127.90 north; 591054.41 east 2021-02-12T23:29:54Z- AHRS_M2 msg: 1613172594.62 epoch sec; seqNo:4242 2021-02-12T23:29:54Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.13 psi 2021-02-12T23:29:54Z- handleDepth()-Depth_Keller msg: 1613172594628 epoch sec; seqNo:4284; depth 20.63 2021-02-12T23:29:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172594.64 epoch sec; seqNo:4284; 4057127.90 north; 591054.41 east 2021-02-12T23:29:55Z- AHRS_M2 msg: 1613172595.03 epoch sec; seqNo:4243 2021-02-12T23:29:55Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.13 psi 2021-02-12T23:29:55Z- handleDepth()-Depth_Keller msg: 1613172595033 epoch sec; seqNo:4285; depth 20.50 2021-02-12T23:29:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172595.04 epoch sec; seqNo:4285; 4057128.28 north; 591054.40 east 2021-02-12T23:29:55Z- AHRS_M2 msg: 1613172595.43 epoch sec; seqNo:4244 2021-02-12T23:29:55Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.14 psi 2021-02-12T23:29:55Z- handleDepth()-Depth_Keller msg: 1613172595438 epoch sec; seqNo:4286; depth 20.40 2021-02-12T23:29:55Z- handleDvl() - RDI_Pathfinder msg: 1613172595.45 epoch sec; seqNo:2081 2021-02-12T23:29:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.78 , 89.13 , 96.70 , 66.61 2021-02-12T23:29:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.00 , -0.05 2021-02-12T23:29:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172595.46 epoch sec; seqNo:4286; 4057128.28 north; 591054.40 east 2021-02-12T23:29:55Z- AHRS_M2 msg: 1613172595.84 epoch sec; seqNo:4245 2021-02-12T23:29:55Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-12T23:29:55Z- handleDepth()-Depth_Keller msg: 1613172595840 epoch sec; seqNo:4287; depth 20.27 2021-02-12T23:29:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172595.86 epoch sec; seqNo:4287; 4057128.66 north; 591054.40 east 2021-02-12T23:29:56Z- AHRS_M2 msg: 1613172596.24 epoch sec; seqNo:4246 2021-02-12T23:29:56Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-12T23:29:56Z- handleDepth()-Depth_Keller msg: 1613172596244 epoch sec; seqNo:4288; depth 20.16 2021-02-12T23:29:56Z- handleDvl() - RDI_Pathfinder msg: 1613172596.26 epoch sec; seqNo:2082 2021-02-12T23:29:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.78 , 90.21 , 102.11 , 66.61 2021-02-12T23:29:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.02 2021-02-12T23:29:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172596.33 epoch sec; seqNo:4288; 4057129.04 north; 591054.40 east 2021-02-12T23:29:56Z- AHRS_M2 msg: 1613172596.64 epoch sec; seqNo:4247 2021-02-12T23:29:56Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:29:56Z- handleDepth()-Depth_Keller msg: 1613172596650 epoch sec; seqNo:4289; depth 20.03 2021-02-12T23:29:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:56Z- handleDvl() - RDI_Pathfinder msg: 1613172596.65 epoch sec; seqNo:2083 2021-02-12T23:29:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.02 2021-02-12T23:29:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172596.66 epoch sec; seqNo:4289; 4057129.42 north; 591054.39 east 2021-02-12T23:29:57Z- AHRS_M2 msg: 1613172597.05 epoch sec; seqNo:4248 2021-02-12T23:29:57Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:29:57Z- handleDepth()-Depth_Keller msg: 1613172597052 epoch sec; seqNo:4290; depth 19.93 2021-02-12T23:29:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172597.07 epoch sec; seqNo:4290; 4057129.43 north; 591055.61 east 2021-02-12T23:29:57Z- AHRS_M2 msg: 1613172597.45 epoch sec; seqNo:4249 2021-02-12T23:29:57Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.18 psi 2021-02-12T23:29:57Z- handleDepth()-Depth_Keller msg: 1613172597461 epoch sec; seqNo:4291; depth 19.82 2021-02-12T23:29:57Z- handleDvl() - RDI_Pathfinder msg: 1613172597.47 epoch sec; seqNo:2084 2021-02-12T23:29:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 91.02 , 97.11 , 67.84 2021-02-12T23:29:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.01 2021-02-12T23:29:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172597.49 epoch sec; seqNo:4291; 4057129.81 north; 591055.61 east 2021-02-12T23:29:57Z- LcmTrn::handleMessages() - handling 46 messages took 0.68 ms and 133 calls... 2021-02-12T23:29:57Z- waiting for 1613172595.27, time is 1613172597.49 2021-02-12T23:29:57Z- LcmTrn::updateTrn() equating pose 1613172597.49 and meas times 1613172597.49 2021-02-12T23:29:57Z- LcmTrn::updateTrn() >>>> pose time 1613172597.49, meas time 1613172597.49 <<<< 2021-02-12T23:29:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172597.49 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:29:57Z- TerrainNav::motionUpdate() - currEstimate = (4057130, 591055.61, 19.82) 2021-02-12T23:29:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:29:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:29:57Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172597.49 sec.; No relevant navigation data available 2021-02-12T23:29:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:29:57Z- TNavBF::mmse selected from filter -1 2021-02-12T23:29:57Z- variances([0], [2], and psi_berg) = 760.213147 833.043192 0.000000 2021-02-12T23:29:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:29:57Z- LcmTrn::publishEstimates() - MLE : 4057113.31 591011.11 19.82 2021-02-12T23:29:57Z- LcmTrn::publishEstimates() - MMSE : 4057129.81 591055.61 19.82 2021-02-12T23:29:57Z- LcmTrn::publishEstimates() - COVAR: 760.21 833.04 1.36 0.00 2021-02-12T23:29:57Z- AHRS_M2 msg: 1613172597.86 epoch sec; seqNo:4250 2021-02-12T23:29:57Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.19 psi 2021-02-12T23:29:57Z- handleDepth()-Depth_Keller msg: 1613172597866 epoch sec; seqNo:4292; depth 19.72 2021-02-12T23:29:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:57Z- handleDvl() - RDI_Pathfinder msg: 1613172597.87 epoch sec; seqNo:2085 2021-02-12T23:29:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.01 2021-02-12T23:29:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172597.88 epoch sec; seqNo:4292; 4057130.19 north; 591055.60 east 2021-02-12T23:29:58Z- AHRS_M2 msg: 1613172598.26 epoch sec; seqNo:4251 2021-02-12T23:29:58Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-12T23:29:58Z- handleDepth()-Depth_Keller msg: 1613172598265 epoch sec; seqNo:4293; depth 19.60 2021-02-12T23:29:58Z- handleDvl() - RDI_Pathfinder msg: 1613172598.28 epoch sec; seqNo:2086 2021-02-12T23:29:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 92.24 , 100.75 , 67.84 2021-02-12T23:29:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.01 2021-02-12T23:29:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172598.29 epoch sec; seqNo:4293; 4057130.57 north; 591055.60 east 2021-02-12T23:29:58Z- AHRS_M2 msg: 1613172598.66 epoch sec; seqNo:4252 2021-02-12T23:29:58Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-12T23:29:58Z- handleDepth()-Depth_Keller msg: 1613172598678 epoch sec; seqNo:4294; depth 19.50 2021-02-12T23:29:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172598.69 epoch sec; seqNo:4294; 4057130.57 north; 591055.60 east 2021-02-12T23:29:59Z- handleDepth()-Depth_Keller msg: 1613172599090 epoch sec; seqNo:4295; depth 19.37 2021-02-12T23:29:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:29:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:29:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:29:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:29:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:29:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:29:59Z- handleDvl() - RDI_Pathfinder msg: 1613172599.09 epoch sec; seqNo:2087 2021-02-12T23:29:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:29:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.01 2021-02-12T23:29:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172599.10 epoch sec; seqNo:4295; 4057130.95 north; 591055.60 east 2021-02-12T23:29:59Z- AHRS_M2 msg: 1613172599.47 epoch sec; seqNo:4253 2021-02-12T23:29:59Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-12T23:29:59Z- handleDepth()-Depth_Keller msg: 1613172599477 epoch sec; seqNo:4296; depth 19.30 2021-02-12T23:29:59Z- handleDvl() - RDI_Pathfinder msg: 1613172599.49 epoch sec; seqNo:2088 2021-02-12T23:29:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 92.24 , 97.11 , 67.43 2021-02-12T23:29:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.00 2021-02-12T23:29:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172599.50 epoch sec; seqNo:4296; 4057131.34 north; 591056.81 east 2021-02-12T23:29:59Z- AHRS_M2 msg: 1613172599.88 epoch sec; seqNo:4254 2021-02-12T23:29:59Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.18 psi 2021-02-12T23:29:59Z- handleDepth()-Depth_Keller msg: 1613172599882 epoch sec; seqNo:4297; depth 19.19 2021-02-12T23:29:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172599.89 epoch sec; seqNo:4297; 4057131.34 north; 591056.81 east 2021-02-12T23:30:00Z- AHRS_M2 msg: 1613172600.28 epoch sec; seqNo:4255 2021-02-12T23:30:00Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.17 psi 2021-02-12T23:30:00Z- handleDepth()-Depth_Keller msg: 1613172600287 epoch sec; seqNo:4298; depth 19.09 2021-02-12T23:30:00Z- handleDvl() - RDI_Pathfinder msg: 1613172600.30 epoch sec; seqNo:2089 2021-02-12T23:30:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.78 , 93.46 , 97.11 , 68.65 2021-02-12T23:30:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , -0.05 2021-02-12T23:30:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172600.33 epoch sec; seqNo:4298; 4057131.72 north; 591056.81 east 2021-02-12T23:30:00Z- AHRS_M2 msg: 1613172600.68 epoch sec; seqNo:4256 2021-02-12T23:30:00Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.18 psi 2021-02-12T23:30:00Z- handleDepth()-Depth_Keller msg: 1613172600693 epoch sec; seqNo:4299; depth 18.98 2021-02-12T23:30:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172600.70 epoch sec; seqNo:4299; 4057132.10 north; 591056.81 east 2021-02-12T23:30:01Z- AHRS_M2 msg: 1613172601.09 epoch sec; seqNo:4257 2021-02-12T23:30:01Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.19 psi 2021-02-12T23:30:01Z- handleDepth()-Depth_Keller msg: 1613172601094 epoch sec; seqNo:4300; depth 18.87 2021-02-12T23:30:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172601.10 epoch sec; seqNo:4300; 4057132.47 north; 591056.80 east 2021-02-12T23:30:01Z- AHRS_M2 msg: 1613172601.49 epoch sec; seqNo:4258 2021-02-12T23:30:01Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.20 psi 2021-02-12T23:30:01Z- handleDepth()-Depth_Keller msg: 1613172601496 epoch sec; seqNo:4301; depth 18.77 2021-02-12T23:30:01Z- handleDvl() - RDI_Pathfinder msg: 1613172601.51 epoch sec; seqNo:2090 2021-02-12T23:30:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.19 , 94.67 , 97.11 , 69.47 2021-02-12T23:30:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , -0.04 2021-02-12T23:30:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172601.53 epoch sec; seqNo:4301; 4057132.47 north; 591056.80 east 2021-02-12T23:30:01Z- AHRS_M2 msg: 1613172601.90 epoch sec; seqNo:4259 2021-02-12T23:30:01Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.21 psi 2021-02-12T23:30:01Z- handleDepth()-Depth_Keller msg: 1613172601900 epoch sec; seqNo:4302; depth 18.66 2021-02-12T23:30:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172601.91 epoch sec; seqNo:4302; 4057132.85 north; 591056.80 east 2021-02-12T23:30:02Z- AHRS_M2 msg: 1613172602.31 epoch sec; seqNo:4260 2021-02-12T23:30:02Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.21 psi 2021-02-12T23:30:02Z- handleDepth()-Depth_Keller msg: 1613172602310 epoch sec; seqNo:4303; depth 18.55 2021-02-12T23:30:02Z- handleDvl() - RDI_Pathfinder msg: 1613172602.32 epoch sec; seqNo:2091 2021-02-12T23:30:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 94.60 , 97.04 , 68.24 2021-02-12T23:30:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.07 , 0.01 2021-02-12T23:30:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172602.33 epoch sec; seqNo:4303; 4057133.24 north; 591058.01 east 2021-02-12T23:30:02Z- LcmTrn::handleMessages() - handling 42 messages took 0.62 ms and 130 calls... 2021-02-12T23:30:02Z- waiting for 1613172600.49, time is 1613172602.33 2021-02-12T23:30:02Z- LcmTrn::updateTrn() equating pose 1613172602.33 and meas times 1613172602.33 2021-02-12T23:30:02Z- LcmTrn::updateTrn() >>>> pose time 1613172602.33, meas time 1613172602.33 <<<< 2021-02-12T23:30:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172602.33 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:30:02Z- TerrainNav::motionUpdate() - currEstimate = (4057133, 591058.01, 18.55) 2021-02-12T23:30:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:30:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:30:02Z- TNavBF::Projecting Measurements 2021-02-12T23:30:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:30:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:30:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:30:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:30:02Z- TNavFilter::4 good beams. 2021-02-12T23:30:02Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:30:02Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:30:02Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:30:02Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:30:02Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:30:02Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:30:02Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:30:02Z- TNavBF::mmse selected from filter -1 2021-02-12T23:30:02Z- TerrainMapOctree: (vehN, vehE ) = (4057133.24, 591058.01). 2021-02-12T23:30:02Z- TerrainMapOctree: Min Distance = 5209.27. 2021-02-12T23:30:02Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:30:02Z- There is no specific good beam that all particles have in common. 2021-02-12T23:30:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:30:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.57 K: 0.129 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-12T23:30:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:30:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 14.32 2021-02-12T23:30:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:30:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 15.07 2021-02-12T23:30:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:30:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-12T23:30:02Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:30:02Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:30:02Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:30:02Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:30:02Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:30:02Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:30:02Z- Filter 0 No beams used in subcloud update 2021-02-12T23:30:02Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:30:02Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:30:02Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 760.1 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:30:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172602.33 sec. 2021-02-12T23:30:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:30:02Z- TNavBF::mmse selected from filter -1 2021-02-12T23:30:02Z- variances([0], [2], and psi_berg) = 760.098441 833.043070 0.000000 2021-02-12T23:30:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:30:02Z- LcmTrn::publishEstimates() - MLE : 4057116.74 591013.51 18.55 2021-02-12T23:30:02Z- LcmTrn::publishEstimates() - MMSE : 4057133.24 591058.01 18.55 2021-02-12T23:30:02Z- LcmTrn::publishEstimates() - COVAR: 760.10 833.04 1.04 0.00 2021-02-12T23:30:02Z- AHRS_M2 msg: 1613172602.70 epoch sec; seqNo:4261 2021-02-12T23:30:02Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.21 psi 2021-02-12T23:30:02Z- handleDepth()-Depth_Keller msg: 1613172602708 epoch sec; seqNo:4304; depth 18.44 2021-02-12T23:30:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:02Z- handleDvl() - RDI_Pathfinder msg: 1613172602.71 epoch sec; seqNo:2092 2021-02-12T23:30:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.07 , 0.01 2021-02-12T23:30:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172602.73 epoch sec; seqNo:4304; 4057133.24 north; 591058.01 east 2021-02-12T23:30:03Z- AHRS_M2 msg: 1613172603.11 epoch sec; seqNo:4262 2021-02-12T23:30:03Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.22 psi 2021-02-12T23:30:03Z- handleDepth()-Depth_Keller msg: 1613172603121 epoch sec; seqNo:4305; depth 18.35 2021-02-12T23:30:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172603.15 epoch sec; seqNo:4305; 4057133.62 north; 591058.01 east 2021-02-12T23:30:03Z- AHRS_M2 msg: 1613172603.52 epoch sec; seqNo:4263 2021-02-12T23:30:03Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.22 psi 2021-02-12T23:30:03Z- handleDepth()-Depth_Keller msg: 1613172603527 epoch sec; seqNo:4306; depth 18.23 2021-02-12T23:30:03Z- handleDvl() - RDI_Pathfinder msg: 1613172603.54 epoch sec; seqNo:2093 2021-02-12T23:30:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 97.11 , 104.41 , 69.06 2021-02-12T23:30:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , 0.08 2021-02-12T23:30:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172603.57 epoch sec; seqNo:4306; 4057134.00 north; 591058.01 east 2021-02-12T23:30:03Z- AHRS_M2 msg: 1613172603.92 epoch sec; seqNo:4264 2021-02-12T23:30:03Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.21 psi 2021-02-12T23:30:03Z- handleDepth()-Depth_Keller msg: 1613172603927 epoch sec; seqNo:4307; depth 18.12 2021-02-12T23:30:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:03Z- handleDvl() - RDI_Pathfinder msg: 1613172603.93 epoch sec; seqNo:2094 2021-02-12T23:30:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , 0.08 2021-02-12T23:30:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172603.94 epoch sec; seqNo:4307; 4057134.00 north; 591058.01 east 2021-02-12T23:30:04Z- AHRS_M2 msg: 1613172604.32 epoch sec; seqNo:4265 2021-02-12T23:30:04Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.21 psi 2021-02-12T23:30:04Z- handleDepth()-Depth_Keller msg: 1613172604343 epoch sec; seqNo:4308; depth 18.00 2021-02-12T23:30:04Z- handleDvl() - RDI_Pathfinder msg: 1613172604.35 epoch sec; seqNo:2095 2021-02-12T23:30:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.60 , 96.90 , 105.42 , 69.88 2021-02-12T23:30:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , 0.11 2021-02-12T23:30:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172604.37 epoch sec; seqNo:4308; 4057134.38 north; 591058.00 east 2021-02-12T23:30:04Z- AHRS_M2 msg: 1613172604.72 epoch sec; seqNo:4266 2021-02-12T23:30:04Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.20 psi 2021-02-12T23:30:04Z- handleDepth()-Depth_Keller msg: 1613172604733 epoch sec; seqNo:4309; depth 17.89 2021-02-12T23:30:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172604.74 epoch sec; seqNo:4309; 4057134.76 north; 591058.00 east 2021-02-12T23:30:05Z- AHRS_M2 msg: 1613172605.13 epoch sec; seqNo:4267 2021-02-12T23:30:05Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.20 psi 2021-02-12T23:30:05Z- handleDepth()-Depth_Keller msg: 1613172605135 epoch sec; seqNo:4310; depth 17.80 2021-02-12T23:30:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:05Z- handleDvl() - RDI_Pathfinder msg: 1613172605.14 epoch sec; seqNo:2096 2021-02-12T23:30:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , 0.11 2021-02-12T23:30:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172605.15 epoch sec; seqNo:4310; 4057135.15 north; 591059.22 east 2021-02-12T23:30:05Z- AHRS_M2 msg: 1613172605.54 epoch sec; seqNo:4268 2021-02-12T23:30:05Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.19 psi 2021-02-12T23:30:05Z- handleDepth()-Depth_Keller msg: 1613172605544 epoch sec; seqNo:4311; depth 17.68 2021-02-12T23:30:05Z- handleDvl() - RDI_Pathfinder msg: 1613172605.57 epoch sec; seqNo:2097 2021-02-12T23:30:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.01 , 95.62 , 96.83 , 69.06 2021-02-12T23:30:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , 0.09 2021-02-12T23:30:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172605.60 epoch sec; seqNo:4311; 4057135.53 north; 591059.21 east 2021-02-12T23:30:05Z- AHRS_M2 msg: 1613172605.94 epoch sec; seqNo:4269 2021-02-12T23:30:05Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.18 psi 2021-02-12T23:30:05Z- handleDepth()-Depth_Keller msg: 1613172605940 epoch sec; seqNo:4312; depth 17.56 2021-02-12T23:30:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172605.96 epoch sec; seqNo:4312; 4057135.91 north; 591059.21 east 2021-02-12T23:30:06Z- AHRS_M2 msg: 1613172606.34 epoch sec; seqNo:4270 2021-02-12T23:30:06Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.18 psi 2021-02-12T23:30:06Z- handleDepth()-Depth_Keller msg: 1613172606344 epoch sec; seqNo:4313; depth 17.46 2021-02-12T23:30:06Z- handleDvl() - RDI_Pathfinder msg: 1613172606.36 epoch sec; seqNo:2098 2021-02-12T23:30:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 94.60 , 105.56 , 69.47 2021-02-12T23:30:06Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.01 , 0.07 2021-02-12T23:30:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172606.38 epoch sec; seqNo:4313; 4057135.91 north; 591059.21 east 2021-02-12T23:30:06Z- AHRS_M2 msg: 1613172606.74 epoch sec; seqNo:4271 2021-02-12T23:30:06Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.17 psi 2021-02-12T23:30:06Z- handleDepth()-Depth_Keller msg: 1613172606753 epoch sec; seqNo:4314; depth 17.37 2021-02-12T23:30:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172606.77 epoch sec; seqNo:4314; 4057136.29 north; 591059.20 east 2021-02-12T23:30:07Z- AHRS_M2 msg: 1613172607.15 epoch sec; seqNo:4272 2021-02-12T23:30:07Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.16 psi 2021-02-12T23:30:07Z- handleDepth()-Depth_Keller msg: 1613172607161 epoch sec; seqNo:4315; depth 17.24 2021-02-12T23:30:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172607.17 epoch sec; seqNo:4315; 4057136.67 north; 591059.20 east 2021-02-12T23:30:07Z- AHRS_M2 msg: 1613172607.55 epoch sec; seqNo:4273 2021-02-12T23:30:07Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.16 psi 2021-02-12T23:30:07Z- handleDepth()-Depth_Keller msg: 1613172607561 epoch sec; seqNo:4316; depth 17.13 2021-02-12T23:30:07Z- handleDvl() - RDI_Pathfinder msg: 1613172607.58 epoch sec; seqNo:2099 2021-02-12T23:30:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.64 , 94.40 , 105.35 , 69.47 2021-02-12T23:30:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , 0.01 2021-02-12T23:30:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172607.59 epoch sec; seqNo:4316; 4057137.06 north; 591060.42 east 2021-02-12T23:30:07Z- LcmTrn::handleMessages() - handling 47 messages took 0.72 ms and 133 calls... 2021-02-12T23:30:07Z- waiting for 1613172605.33, time is 1613172607.59 2021-02-12T23:30:07Z- LcmTrn::updateTrn() equating pose 1613172607.59 and meas times 1613172607.59 2021-02-12T23:30:07Z- LcmTrn::updateTrn() >>>> pose time 1613172607.59, meas time 1613172607.59 <<<< 2021-02-12T23:30:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172607.59 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:30:07Z- TerrainNav::motionUpdate() - currEstimate = (4057137, 591060.42, 17.13) 2021-02-12T23:30:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:30:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:30:07Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172607.59 sec.; No relevant navigation data available 2021-02-12T23:30:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:30:07Z- TNavBF::mmse selected from filter -1 2021-02-12T23:30:07Z- variances([0], [2], and psi_berg) = 759.979720 833.042944 0.000000 2021-02-12T23:30:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:30:07Z- LcmTrn::publishEstimates() - MLE : 4057120.56 591015.92 17.13 2021-02-12T23:30:07Z- LcmTrn::publishEstimates() - MMSE : 4057137.06 591060.42 17.13 2021-02-12T23:30:07Z- LcmTrn::publishEstimates() - COVAR: 759.98 833.04 1.27 0.00 2021-02-12T23:30:07Z- AHRS_M2 msg: 1613172607.96 epoch sec; seqNo:4274 2021-02-12T23:30:07Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-12T23:30:07Z- handleDepth()-Depth_Keller msg: 1613172607969 epoch sec; seqNo:4317; depth 17.02 2021-02-12T23:30:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172608.01 epoch sec; seqNo:4317; 4057137.44 north; 591060.41 east 2021-02-12T23:30:08Z- AHRS_M2 msg: 1613172608.36 epoch sec; seqNo:4275 2021-02-12T23:30:08Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-12T23:30:08Z- handleDepth()-Depth_Keller msg: 1613172608373 epoch sec; seqNo:4318; depth 16.92 2021-02-12T23:30:08Z- handleDvl() - RDI_Pathfinder msg: 1613172608.38 epoch sec; seqNo:2100 2021-02-12T23:30:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.87 , 94.40 , 106.57 , 69.88 2021-02-12T23:30:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , -0.04 2021-02-12T23:30:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172608.39 epoch sec; seqNo:4318; 4057137.44 north; 591060.41 east 2021-02-12T23:30:08Z- AHRS_M2 msg: 1613172608.76 epoch sec; seqNo:4276 2021-02-12T23:30:08Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-12T23:30:08Z- handleDepth()-Depth_Keller msg: 1613172608770 epoch sec; seqNo:4319; depth 16.81 2021-02-12T23:30:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:08Z- handleDvl() - RDI_Pathfinder msg: 1613172608.77 epoch sec; seqNo:2101 2021-02-12T23:30:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , -0.04 2021-02-12T23:30:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172608.81 epoch sec; seqNo:4319; 4057137.82 north; 591060.41 east 2021-02-12T23:30:09Z- AHRS_M2 msg: 1613172609.17 epoch sec; seqNo:4277 2021-02-12T23:30:09Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.14 psi 2021-02-12T23:30:09Z- handleDepth()-Depth_Keller msg: 1613172609177 epoch sec; seqNo:4320; depth 16.70 2021-02-12T23:30:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172609.19 epoch sec; seqNo:4320; 4057138.20 north; 591060.40 east 2021-02-12T23:30:09Z- AHRS_M2 msg: 1613172609.57 epoch sec; seqNo:4278 2021-02-12T23:30:09Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.14 psi 2021-02-12T23:30:09Z- handleDepth()-Depth_Keller msg: 1613172609581 epoch sec; seqNo:4321; depth 16.60 2021-02-12T23:30:09Z- handleDvl() - RDI_Pathfinder msg: 1613172609.59 epoch sec; seqNo:2102 2021-02-12T23:30:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.87 , 94.33 , 105.28 , 71.51 2021-02-12T23:30:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.02 , -0.09 2021-02-12T23:30:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172609.60 epoch sec; seqNo:4321; 4057138.20 north; 591060.40 east 2021-02-12T23:30:09Z- AHRS_M2 msg: 1613172609.98 epoch sec; seqNo:4279 2021-02-12T23:30:09Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-12T23:30:09Z- handleDepth()-Depth_Keller msg: 1613172609982 epoch sec; seqNo:4322; depth 16.48 2021-02-12T23:30:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:09Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:09Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:09Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:09Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:09Z- handleDvl() - RDI_Pathfinder msg: 1613172609.98 epoch sec; seqNo:2103 2021-02-12T23:30:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.02 , -0.09 2021-02-12T23:30:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172609.99 epoch sec; seqNo:4322; 4057138.59 north; 591061.62 east 2021-02-12T23:30:10Z- AHRS_M2 msg: 1613172610.38 epoch sec; seqNo:4280 2021-02-12T23:30:10Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-12T23:30:10Z- handleDepth()-Depth_Keller msg: 1613172610384 epoch sec; seqNo:4323; depth 16.36 2021-02-12T23:30:10Z- handleDvl() - RDI_Pathfinder msg: 1613172610.41 epoch sec; seqNo:2104 2021-02-12T23:30:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.87 , 91.90 , 101.63 , 70.29 2021-02-12T23:30:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.09 2021-02-12T23:30:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172610.42 epoch sec; seqNo:4323; 4057138.97 north; 591061.62 east 2021-02-12T23:30:10Z- AHRS_M2 msg: 1613172610.78 epoch sec; seqNo:4281 2021-02-12T23:30:10Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-12T23:30:10Z- handleDepth()-Depth_Keller msg: 1613172610788 epoch sec; seqNo:4324; depth 16.25 2021-02-12T23:30:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172610.80 epoch sec; seqNo:4324; 4057138.97 north; 591061.62 east 2021-02-12T23:30:11Z- AHRS_M2 msg: 1613172611.20 epoch sec; seqNo:4282 2021-02-12T23:30:11Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-12T23:30:11Z- handleDepth()-Depth_Keller msg: 1613172611202 epoch sec; seqNo:4325; depth 16.15 2021-02-12T23:30:11Z- handleDvl() - RDI_Pathfinder msg: 1613172611.21 epoch sec; seqNo:2105 2021-02-12T23:30:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.87 , 94.47 , 96.90 , 71.92 2021-02-12T23:30:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.09 , -0.05 2021-02-12T23:30:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172611.22 epoch sec; seqNo:4325; 4057139.35 north; 591061.61 east 2021-02-12T23:30:11Z- AHRS_M2 msg: 1613172611.59 epoch sec; seqNo:4283 2021-02-12T23:30:11Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-12T23:30:11Z- handleDepth()-Depth_Keller msg: 1613172611597 epoch sec; seqNo:4326; depth 16.04 2021-02-12T23:30:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172611.62 epoch sec; seqNo:4326; 4057139.72 north; 591061.61 east 2021-02-12T23:30:11Z- AHRS_M2 msg: 1613172612.00 epoch sec; seqNo:4284 2021-02-12T23:30:11Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:30:12Z- handleDepth()-Depth_Keller msg: 1613172612000 epoch sec; seqNo:4327; depth 15.95 2021-02-12T23:30:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172612.01 epoch sec; seqNo:4327; 4057140.10 north; 591061.60 east 2021-02-12T23:30:12Z- AHRS_M2 msg: 1613172612.40 epoch sec; seqNo:4285 2021-02-12T23:30:12Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:30:12Z- handleDepth()-Depth_Keller msg: 1613172612405 epoch sec; seqNo:4328; depth 15.84 2021-02-12T23:30:12Z- handleDvl() - RDI_Pathfinder msg: 1613172612.42 epoch sec; seqNo:2106 2021-02-12T23:30:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.69 , 94.47 , 104.20 , 71.92 2021-02-12T23:30:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.09 , 0.01 2021-02-12T23:30:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172612.43 epoch sec; seqNo:4328; 4057140.10 north; 591061.60 east 2021-02-12T23:30:12Z- LcmTrn::handleMessages() - handling 43 messages took 0.62 ms and 131 calls... 2021-02-12T23:30:12Z- waiting for 1613172610.59, time is 1613172612.43 2021-02-12T23:30:12Z- LcmTrn::updateTrn() equating pose 1613172612.43 and meas times 1613172612.43 2021-02-12T23:30:12Z- LcmTrn::updateTrn() >>>> pose time 1613172612.43, meas time 1613172612.43 <<<< 2021-02-12T23:30:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172612.43 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:30:12Z- TerrainNav::motionUpdate() - currEstimate = (4057140, 591061.60, 15.84) 2021-02-12T23:30:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:30:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:30:12Z- TNavBF::Projecting Measurements 2021-02-12T23:30:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:30:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:30:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:30:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:30:12Z- TNavFilter::4 good beams. 2021-02-12T23:30:12Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:30:12Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:30:12Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:30:12Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:30:12Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:30:12Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:30:12Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:30:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:30:12Z- TerrainMapOctree: (vehN, vehE ) = (4057140.10, 591061.60). 2021-02-12T23:30:12Z- TerrainMapOctree: Min Distance = 5201.65. 2021-02-12T23:30:12Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:30:12Z- There is no specific good beam that all particles have in common. 2021-02-12T23:30:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:30:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.44 K: 0.108 ExpectedMeasDiff: nan currMeas.covariance: 10.16 2021-02-12T23:30:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:30:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 14.28 2021-02-12T23:30:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:30:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 17.37 2021-02-12T23:30:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:30:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 8.28 2021-02-12T23:30:12Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:30:12Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:30:12Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:30:12Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:30:12Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:30:12Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:30:12Z- Filter 0 No beams used in subcloud update 2021-02-12T23:30:12Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:30:12Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:30:12Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 759.9 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:30:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172612.43 sec. 2021-02-12T23:30:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:30:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:30:12Z- variances([0], [2], and psi_berg) = 759.878810 833.042837 0.000000 2021-02-12T23:30:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:30:12Z- LcmTrn::publishEstimates() - MLE : 4057123.60 591017.10 15.84 2021-02-12T23:30:12Z- LcmTrn::publishEstimates() - MMSE : 4057140.10 591061.60 15.84 2021-02-12T23:30:12Z- LcmTrn::publishEstimates() - COVAR: 759.88 833.04 1.00 0.00 2021-02-12T23:30:12Z- AHRS_M2 msg: 1613172612.80 epoch sec; seqNo:4286 2021-02-12T23:30:12Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-12T23:30:12Z- handleDepth()-Depth_Keller msg: 1613172612811 epoch sec; seqNo:4329; depth 15.73 2021-02-12T23:30:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172612.82 epoch sec; seqNo:4329; 4057140.48 north; 591061.60 east 2021-02-12T23:30:13Z- AHRS_M2 msg: 1613172613.21 epoch sec; seqNo:4287 2021-02-12T23:30:13Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-12T23:30:13Z- handleDepth()-Depth_Keller msg: 1613172613219 epoch sec; seqNo:4330; depth 15.63 2021-02-12T23:30:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172613.26 epoch sec; seqNo:4330; 4057140.87 north; 591062.82 east 2021-02-12T23:30:13Z- AHRS_M2 msg: 1613172613.61 epoch sec; seqNo:4288 2021-02-12T23:30:13Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-12T23:30:13Z- handleDepth()-Depth_Keller msg: 1613172613616 epoch sec; seqNo:4331; depth 15.52 2021-02-12T23:30:13Z- handleDvl() - RDI_Pathfinder msg: 1613172613.63 epoch sec; seqNo:2107 2021-02-12T23:30:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.69 , 94.33 , 106.50 , 71.11 2021-02-12T23:30:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.09 , 0.01 2021-02-12T23:30:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172613.66 epoch sec; seqNo:4331; 4057141.25 north; 591062.81 east 2021-02-12T23:30:14Z- handleDepth()-Depth_Keller msg: 1613172614037 epoch sec; seqNo:4332; depth 15.41 2021-02-12T23:30:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172614.05 epoch sec; seqNo:4332; 4057141.25 north; 591062.81 east 2021-02-12T23:30:14Z- AHRS_M2 msg: 1613172614.42 epoch sec; seqNo:4289 2021-02-12T23:30:14Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.14 psi 2021-02-12T23:30:14Z- handleDepth()-Depth_Keller msg: 1613172614427 epoch sec; seqNo:4333; depth 15.31 2021-02-12T23:30:14Z- handleDvl() - RDI_Pathfinder msg: 1613172614.44 epoch sec; seqNo:2108 2021-02-12T23:30:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.69 , 95.48 , 105.22 , 71.11 2021-02-12T23:30:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.08 , 0.01 2021-02-12T23:30:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172614.45 epoch sec; seqNo:4333; 4057141.63 north; 591062.81 east 2021-02-12T23:30:14Z- AHRS_M2 msg: 1613172614.82 epoch sec; seqNo:4290 2021-02-12T23:30:14Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.14 psi 2021-02-12T23:30:14Z- handleDepth()-Depth_Keller msg: 1613172614828 epoch sec; seqNo:4334; depth 15.21 2021-02-12T23:30:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:14Z- handleDvl() - RDI_Pathfinder msg: 1613172614.83 epoch sec; seqNo:2109 2021-02-12T23:30:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.08 , 0.01 2021-02-12T23:30:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172614.84 epoch sec; seqNo:4334; 4057141.63 north; 591062.81 east 2021-02-12T23:30:15Z- AHRS_M2 msg: 1613172615.23 epoch sec; seqNo:4291 2021-02-12T23:30:15Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.14 psi 2021-02-12T23:30:15Z- handleDepth()-Depth_Keller msg: 1613172615232 epoch sec; seqNo:4335; depth 15.10 2021-02-12T23:30:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172615.24 epoch sec; seqNo:4335; 4057142.01 north; 591062.81 east 2021-02-12T23:30:15Z- AHRS_M2 msg: 1613172615.63 epoch sec; seqNo:4292 2021-02-12T23:30:15Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.14 psi 2021-02-12T23:30:15Z- handleDepth()-Depth_Keller msg: 1613172615639 epoch sec; seqNo:4336; depth 14.99 2021-02-12T23:30:15Z- handleDvl() - RDI_Pathfinder msg: 1613172615.65 epoch sec; seqNo:2110 2021-02-12T23:30:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.88 , 94.27 , 104.00 , 73.15 2021-02-12T23:30:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.11 , 0.03 2021-02-12T23:30:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172615.66 epoch sec; seqNo:4336; 4057142.39 north; 591062.80 east 2021-02-12T23:30:16Z- AHRS_M2 msg: 1613172616.04 epoch sec; seqNo:4293 2021-02-12T23:30:16Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-12T23:30:16Z- handleDepth()-Depth_Keller msg: 1613172616040 epoch sec; seqNo:4337; depth 14.87 2021-02-12T23:30:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:16Z- handleDvl() - RDI_Pathfinder msg: 1613172616.04 epoch sec; seqNo:2111 2021-02-12T23:30:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.11 , 0.03 2021-02-12T23:30:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172616.06 epoch sec; seqNo:4337; 4057142.77 north; 591062.80 east 2021-02-12T23:30:16Z- AHRS_M2 msg: 1613172616.44 epoch sec; seqNo:4294 2021-02-12T23:30:16Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-12T23:30:16Z- handleDepth()-Depth_Keller msg: 1613172616445 epoch sec; seqNo:4338; depth 14.78 2021-02-12T23:30:16Z- handleDvl() - RDI_Pathfinder msg: 1613172616.46 epoch sec; seqNo:2112 2021-02-12T23:30:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.88 , 93.05 , 106.43 , 73.15 2021-02-12T23:30:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.11 , 0.09 2021-02-12T23:30:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172616.48 epoch sec; seqNo:4338; 4057142.78 north; 591064.02 east 2021-02-12T23:30:16Z- AHRS_M2 msg: 1613172616.84 epoch sec; seqNo:4295 2021-02-12T23:30:16Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-12T23:30:16Z- handleDepth()-Depth_Keller msg: 1613172616853 epoch sec; seqNo:4339; depth 14.67 2021-02-12T23:30:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172616.86 epoch sec; seqNo:4339; 4057143.16 north; 591064.01 east 2021-02-12T23:30:17Z- AHRS_M2 msg: 1613172617.25 epoch sec; seqNo:4296 2021-02-12T23:30:17Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.17 psi 2021-02-12T23:30:17Z- handleDepth()-Depth_Keller msg: 1613172617254 epoch sec; seqNo:4340; depth 14.54 2021-02-12T23:30:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:17Z- handleDvl() - RDI_Pathfinder msg: 1613172617.26 epoch sec; seqNo:2113 2021-02-12T23:30:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.11 , 0.09 2021-02-12T23:30:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172617.27 epoch sec; seqNo:4340; 4057143.54 north; 591064.01 east 2021-02-12T23:30:17Z- AHRS_M2 msg: 1613172617.65 epoch sec; seqNo:4297 2021-02-12T23:30:17Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-12T23:30:17Z- handleDepth()-Depth_Keller msg: 1613172617657 epoch sec; seqNo:4341; depth 14.44 2021-02-12T23:30:17Z- handleDvl() - RDI_Pathfinder msg: 1613172617.67 epoch sec; seqNo:2114 2021-02-12T23:30:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.10 , 96.70 , 106.43 , 73.56 2021-02-12T23:30:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , 0.14 2021-02-12T23:30:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172617.69 epoch sec; seqNo:4341; 4057143.92 north; 591064.01 east 2021-02-12T23:30:17Z- LcmTrn::handleMessages() - handling 46 messages took 0.62 ms and 134 calls... 2021-02-12T23:30:17Z- waiting for 1613172615.43, time is 1613172617.69 2021-02-12T23:30:17Z- LcmTrn::updateTrn() equating pose 1613172617.69 and meas times 1613172617.69 2021-02-12T23:30:17Z- LcmTrn::updateTrn() >>>> pose time 1613172617.69, meas time 1613172617.69 <<<< 2021-02-12T23:30:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172617.69 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:30:17Z- TerrainNav::motionUpdate() - currEstimate = (4057144, 591064.01, 14.44) 2021-02-12T23:30:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:30:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:30:17Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172617.69 sec.; No relevant navigation data available 2021-02-12T23:30:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:30:17Z- TNavBF::mmse selected from filter -1 2021-02-12T23:30:17Z- variances([0], [2], and psi_berg) = 759.760461 833.042711 0.000000 2021-02-12T23:30:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:30:17Z- LcmTrn::publishEstimates() - MLE : 4057128.42 591019.51 14.44 2021-02-12T23:30:17Z- LcmTrn::publishEstimates() - MMSE : 4057143.92 591064.01 14.44 2021-02-12T23:30:17Z- LcmTrn::publishEstimates() - COVAR: 759.76 833.04 1.23 0.00 2021-02-12T23:30:18Z- AHRS_M2 msg: 1613172618.06 epoch sec; seqNo:4298 2021-02-12T23:30:18Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-12T23:30:18Z- handleDepth()-Depth_Keller msg: 1613172618060 epoch sec; seqNo:4342; depth 14.33 2021-02-12T23:30:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172618.07 epoch sec; seqNo:4342; 4057144.30 north; 591064.00 east 2021-02-12T23:30:18Z- AHRS_M2 msg: 1613172618.46 epoch sec; seqNo:4299 2021-02-12T23:30:18Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-12T23:30:18Z- handleDepth()-Depth_Keller msg: 1613172618467 epoch sec; seqNo:4343; depth 14.22 2021-02-12T23:30:18Z- handleDvl() - RDI_Pathfinder msg: 1613172618.48 epoch sec; seqNo:2115 2021-02-12T23:30:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 96.56 , 109.95 , 73.15 2021-02-12T23:30:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.02 , 0.12 2021-02-12T23:30:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172618.49 epoch sec; seqNo:4343; 4057144.67 north; 591064.00 east 2021-02-12T23:30:18Z- AHRS_M2 msg: 1613172618.87 epoch sec; seqNo:4300 2021-02-12T23:30:18Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-12T23:30:18Z- handleDepth()-Depth_Keller msg: 1613172618871 epoch sec; seqNo:4344; depth 14.10 2021-02-12T23:30:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172618.88 epoch sec; seqNo:4344; 4057144.67 north; 591064.00 east 2021-02-12T23:30:19Z- AHRS_M2 msg: 1613172619.27 epoch sec; seqNo:4301 2021-02-12T23:30:19Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-12T23:30:19Z- handleDepth()-Depth_Keller msg: 1613172619277 epoch sec; seqNo:4345; depth 14.00 2021-02-12T23:30:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172619.32 epoch sec; seqNo:4345; 4057145.07 north; 591065.22 east 2021-02-12T23:30:19Z- AHRS_M2 msg: 1613172619.67 epoch sec; seqNo:4302 2021-02-12T23:30:19Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.13 psi 2021-02-12T23:30:19Z- handleDepth()-Depth_Keller msg: 1613172619681 epoch sec; seqNo:4346; depth 13.87 2021-02-12T23:30:19Z- handleDvl() - RDI_Pathfinder msg: 1613172619.69 epoch sec; seqNo:2116 2021-02-12T23:30:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 95.28 , 107.45 , 71.51 2021-02-12T23:30:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , 0.06 2021-02-12T23:30:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172619.70 epoch sec; seqNo:4346; 4057145.45 north; 591065.21 east 2021-02-12T23:30:20Z- AHRS_M2 msg: 1613172620.08 epoch sec; seqNo:4303 2021-02-12T23:30:20Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.12 psi 2021-02-12T23:30:20Z- handleDepth()-Depth_Keller msg: 1613172620082 epoch sec; seqNo:4347; depth 13.76 2021-02-12T23:30:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172620.09 epoch sec; seqNo:4347; 4057145.82 north; 591065.21 east 2021-02-12T23:30:20Z- AHRS_M2 msg: 1613172620.48 epoch sec; seqNo:4304 2021-02-12T23:30:20Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.12 psi 2021-02-12T23:30:20Z- handleDepth()-Depth_Keller msg: 1613172620487 epoch sec; seqNo:4348; depth 13.65 2021-02-12T23:30:20Z- handleDvl() - RDI_Pathfinder msg: 1613172620.51 epoch sec; seqNo:2117 2021-02-12T23:30:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.46 , 95.42 , 110.02 , 71.92 2021-02-12T23:30:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , 0.00 2021-02-12T23:30:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172620.52 epoch sec; seqNo:4348; 4057145.82 north; 591065.21 east 2021-02-12T23:30:20Z- AHRS_M2 msg: 1613172620.88 epoch sec; seqNo:4305 2021-02-12T23:30:20Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.12 psi 2021-02-12T23:30:20Z- handleDepth()-Depth_Keller msg: 1613172620889 epoch sec; seqNo:4349; depth 13.53 2021-02-12T23:30:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:20Z- handleDvl() - RDI_Pathfinder msg: 1613172620.89 epoch sec; seqNo:2118 2021-02-12T23:30:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , 0.00 2021-02-12T23:30:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172620.92 epoch sec; seqNo:4349; 4057146.20 north; 591065.20 east 2021-02-12T23:30:21Z- AHRS_M2 msg: 1613172621.30 epoch sec; seqNo:4306 2021-02-12T23:30:21Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.13 psi 2021-02-12T23:30:21Z- handleDepth()-Depth_Keller msg: 1613172621314 epoch sec; seqNo:4350; depth 13.42 2021-02-12T23:30:21Z- handleDvl() - RDI_Pathfinder msg: 1613172621.34 epoch sec; seqNo:2119 2021-02-12T23:30:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.33 , 97.71 , 109.88 , 72.33 2021-02-12T23:30:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.04 , 0.00 2021-02-12T23:30:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172621.35 epoch sec; seqNo:4350; 4057146.60 north; 591066.42 east 2021-02-12T23:30:21Z- AHRS_M2 msg: 1613172621.70 epoch sec; seqNo:4307 2021-02-12T23:30:21Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.13 psi 2021-02-12T23:30:21Z- handleDepth()-Depth_Keller msg: 1613172621721 epoch sec; seqNo:4351; depth 13.27 2021-02-12T23:30:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172621.73 epoch sec; seqNo:4351; 4057146.60 north; 591066.42 east 2021-02-12T23:30:22Z- AHRS_M2 msg: 1613172622.10 epoch sec; seqNo:4308 2021-02-12T23:30:22Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.14 psi 2021-02-12T23:30:22Z- handleDepth()-Depth_Keller msg: 1613172622100 epoch sec; seqNo:4352; depth 13.16 2021-02-12T23:30:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:22Z- handleDvl() - RDI_Pathfinder msg: 1613172622.10 epoch sec; seqNo:2120 2021-02-12T23:30:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.04 , 0.00 2021-02-12T23:30:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172622.12 epoch sec; seqNo:4352; 4057146.97 north; 591066.42 east 2021-02-12T23:30:22Z- AHRS_M2 msg: 1613172622.50 epoch sec; seqNo:4309 2021-02-12T23:30:22Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-12T23:30:22Z- handleDepth()-Depth_Keller msg: 1613172622505 epoch sec; seqNo:4353; depth 13.05 2021-02-12T23:30:22Z- handleDvl() - RDI_Pathfinder msg: 1613172622.52 epoch sec; seqNo:2121 2021-02-12T23:30:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.26 , 96.43 , 112.25 , 78.18 2021-02-12T23:30:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.05 , -0.00 2021-02-12T23:30:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172622.54 epoch sec; seqNo:4353; 4057147.35 north; 591066.41 east 2021-02-12T23:30:22Z- AHRS_M2 msg: 1613172622.90 epoch sec; seqNo:4310 2021-02-12T23:30:22Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.15 psi 2021-02-12T23:30:22Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-12T23:30:22Z- waiting for 1613172620.69, time is 1613172622.90 2021-02-12T23:30:22Z- LcmTrn::updateTrn() >>>> pose time 1613172622.90, meas time 1613172622.52 <<<< 2021-02-12T23:30:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172622.52 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:30:22Z- TerrainNav::motionUpdate() - currEstimate = (4057147, 591066.41, 13.05) 2021-02-12T23:30:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:30:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:30:22Z- TNavBF::Projecting Measurements 2021-02-12T23:30:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:30:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:30:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:30:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:30:22Z- TNavFilter::4 good beams. 2021-02-12T23:30:22Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:30:22Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:30:22Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:30:22Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:30:22Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:30:22Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:30:22Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:30:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:30:22Z- TerrainMapOctree: (vehN, vehE ) = (4057147.10, 591066.23). 2021-02-12T23:30:22Z- TerrainMapOctree: Min Distance = 5193.28. 2021-02-12T23:30:22Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:30:22Z- There is no specific good beam that all particles have in common. 2021-02-12T23:30:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:30:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 11.36 2021-02-12T23:30:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:30:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 14.88 2021-02-12T23:30:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:30:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 20.16 2021-02-12T23:30:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:30:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 9.78 2021-02-12T23:30:22Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:30:22Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:30:22Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:30:22Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:30:22Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:30:22Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:30:22Z- Filter 0 No beams used in subcloud update 2021-02-12T23:30:22Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:30:22Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:30:22Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 759.7 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:30:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172622.52 sec. 2021-02-12T23:30:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:30:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:30:22Z- variances([0], [2], and psi_berg) = 759.619812 833.042563 0.000000 2021-02-12T23:30:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:30:22Z- LcmTrn::publishEstimates() - MLE : 4057130.85 591022.91 13.05 2021-02-12T23:30:22Z- LcmTrn::publishEstimates() - MMSE : 4057147.35 591066.41 13.05 2021-02-12T23:30:22Z- LcmTrn::publishEstimates() - COVAR: 759.62 833.04 1.05 0.00 2021-02-12T23:30:22Z- handleDepth()-Depth_Keller msg: 1613172622913 epoch sec; seqNo:4354; depth 12.92 2021-02-12T23:30:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172622.92 epoch sec; seqNo:4354; 4057147.35 north; 591066.41 east 2021-02-12T23:30:23Z- AHRS_M2 msg: 1613172623.31 epoch sec; seqNo:4311 2021-02-12T23:30:23Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.15 psi 2021-02-12T23:30:23Z- handleDepth()-Depth_Keller msg: 1613172623314 epoch sec; seqNo:4355; depth 12.80 2021-02-12T23:30:23Z- handleDvl() - RDI_Pathfinder msg: 1613172623.33 epoch sec; seqNo:2122 2021-02-12T23:30:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.13 , 96.29 , 111.17 , 73.96 2021-02-12T23:30:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , -0.04 2021-02-12T23:30:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172623.34 epoch sec; seqNo:4355; 4057147.73 north; 591066.41 east 2021-02-12T23:30:23Z- AHRS_M2 msg: 1613172623.71 epoch sec; seqNo:4312 2021-02-12T23:30:23Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.15 psi 2021-02-12T23:30:23Z- handleDepth()-Depth_Keller msg: 1613172623726 epoch sec; seqNo:4356; depth 12.70 2021-02-12T23:30:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172623.74 epoch sec; seqNo:4356; 4057148.12 north; 591067.63 east 2021-02-12T23:30:24Z- AHRS_M2 msg: 1613172624.12 epoch sec; seqNo:4313 2021-02-12T23:30:24Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.16 psi 2021-02-12T23:30:24Z- handleDepth()-Depth_Keller msg: 1613172624130 epoch sec; seqNo:4357; depth 12.58 2021-02-12T23:30:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172624.16 epoch sec; seqNo:4357; 4057148.12 north; 591067.63 east 2021-02-12T23:30:24Z- AHRS_M2 msg: 1613172624.52 epoch sec; seqNo:4314 2021-02-12T23:30:24Z- AHRS_M2 msg: 0.67 phi; 0.30 theta; -0.16 psi 2021-02-12T23:30:24Z- handleDepth()-Depth_Keller msg: 1613172624525 epoch sec; seqNo:4358; depth 12.48 2021-02-12T23:30:24Z- handleDvl() - RDI_Pathfinder msg: 1613172624.54 epoch sec; seqNo:2123 2021-02-12T23:30:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.26 , 96.43 , 109.81 , 78.18 2021-02-12T23:30:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , -0.10 2021-02-12T23:30:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172624.55 epoch sec; seqNo:4358; 4057148.50 north; 591067.62 east 2021-02-12T23:30:24Z- AHRS_M2 msg: 1613172624.93 epoch sec; seqNo:4315 2021-02-12T23:30:24Z- AHRS_M2 msg: 0.68 phi; 0.29 theta; -0.16 psi 2021-02-12T23:30:24Z- handleDepth()-Depth_Keller msg: 1613172624938 epoch sec; seqNo:4359; depth 12.39 2021-02-12T23:30:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172624.95 epoch sec; seqNo:4359; 4057148.88 north; 591067.62 east 2021-02-12T23:30:25Z- AHRS_M2 msg: 1613172625.33 epoch sec; seqNo:4316 2021-02-12T23:30:25Z- AHRS_M2 msg: 0.68 phi; 0.29 theta; -0.16 psi 2021-02-12T23:30:25Z- handleDepth()-Depth_Keller msg: 1613172625337 epoch sec; seqNo:4360; depth 12.28 2021-02-12T23:30:25Z- handleDvl() - RDI_Pathfinder msg: 1613172625.36 epoch sec; seqNo:2124 2021-02-12T23:30:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 96.36 , 108.53 , 73.96 2021-02-12T23:30:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.03 , -0.15 2021-02-12T23:30:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172625.37 epoch sec; seqNo:4360; 4057148.88 north; 591067.62 east 2021-02-12T23:30:25Z- AHRS_M2 msg: 1613172625.73 epoch sec; seqNo:4317 2021-02-12T23:30:25Z- AHRS_M2 msg: 0.69 phi; 0.28 theta; -0.16 psi 2021-02-12T23:30:25Z- handleDepth()-Depth_Keller msg: 1613172625736 epoch sec; seqNo:4361; depth 12.19 2021-02-12T23:30:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:25Z- handleDvl() - RDI_Pathfinder msg: 1613172625.74 epoch sec; seqNo:2125 2021-02-12T23:30:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.03 , -0.15 2021-02-12T23:30:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172625.75 epoch sec; seqNo:4361; 4057149.26 north; 591067.61 east 2021-02-12T23:30:26Z- AHRS_M2 msg: 1613172626.14 epoch sec; seqNo:4318 2021-02-12T23:30:26Z- AHRS_M2 msg: 0.69 phi; 0.28 theta; -0.16 psi 2021-02-12T23:30:26Z- handleDepth()-Depth_Keller msg: 1613172626145 epoch sec; seqNo:4362; depth 12.09 2021-02-12T23:30:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172626.15 epoch sec; seqNo:4362; 4057149.26 north; 591067.61 east 2021-02-12T23:30:26Z- AHRS_M2 msg: 1613172626.55 epoch sec; seqNo:4319 2021-02-12T23:30:26Z- AHRS_M2 msg: 0.69 phi; 0.27 theta; -0.16 psi 2021-02-12T23:30:26Z- handleDepth()-Depth_Keller msg: 1613172626553 epoch sec; seqNo:4363; depth 11.98 2021-02-12T23:30:26Z- handleDvl() - RDI_Pathfinder msg: 1613172626.57 epoch sec; seqNo:2126 2021-02-12T23:30:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 96.50 , 106.23 , 74.37 2021-02-12T23:30:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.11 , -0.12 2021-02-12T23:30:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172626.58 epoch sec; seqNo:4363; 4057149.65 north; 591068.83 east 2021-02-12T23:30:26Z- AHRS_M2 msg: 1613172626.94 epoch sec; seqNo:4320 2021-02-12T23:30:26Z- AHRS_M2 msg: 0.68 phi; 0.27 theta; -0.16 psi 2021-02-12T23:30:26Z- handleDepth()-Depth_Keller msg: 1613172626948 epoch sec; seqNo:4364; depth 11.89 2021-02-12T23:30:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:26Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:26Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:26Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:26Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:26Z- handleDvl() - RDI_Pathfinder msg: 1613172626.95 epoch sec; seqNo:2127 2021-02-12T23:30:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.11 , -0.12 2021-02-12T23:30:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172626.97 epoch sec; seqNo:4364; 4057150.03 north; 591068.83 east 2021-02-12T23:30:27Z- AHRS_M2 msg: 1613172627.35 epoch sec; seqNo:4321 2021-02-12T23:30:27Z- AHRS_M2 msg: 0.68 phi; 0.28 theta; -0.16 psi 2021-02-12T23:30:27Z- handleDepth()-Depth_Keller msg: 1613172627357 epoch sec; seqNo:4365; depth 11.80 2021-02-12T23:30:27Z- handleDvl() - RDI_Pathfinder msg: 1613172627.38 epoch sec; seqNo:2128 2021-02-12T23:30:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 96.56 , 107.51 , 75.60 2021-02-12T23:30:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.15 , -0.03 2021-02-12T23:30:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172627.39 epoch sec; seqNo:4365; 4057150.03 north; 591068.83 east 2021-02-12T23:30:27Z- AHRS_M2 msg: 1613172627.75 epoch sec; seqNo:4322 2021-02-12T23:30:27Z- AHRS_M2 msg: 0.67 phi; 0.28 theta; -0.17 psi 2021-02-12T23:30:27Z- handleDepth()-Depth_Keller msg: 1613172627757 epoch sec; seqNo:4366; depth 11.70 2021-02-12T23:30:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172627.77 epoch sec; seqNo:4366; 4057150.41 north; 591068.82 east 2021-02-12T23:30:28Z- LcmTrn::handleMessages() - handling 45 messages took 0.64 ms and 133 calls... 2021-02-12T23:30:28Z- waiting for 1613172625.90, time is 1613172627.77 2021-02-12T23:30:28Z- LcmTrn::updateTrn() >>>> pose time 1613172627.77, meas time 1613172627.38 <<<< 2021-02-12T23:30:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172627.38 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:30:28Z- TerrainNav::motionUpdate() - currEstimate = (4057150, 591068.82, 11.70) 2021-02-12T23:30:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:30:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:30:28Z- TNavBF::Projecting Measurements 2021-02-12T23:30:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:30:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:30:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:30:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:30:28Z- TNavFilter::4 good beams. 2021-02-12T23:30:28Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:30:28Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:30:28Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:30:28Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:30:28Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:30:28Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:30:28Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:30:28Z- TNavBF::mmse selected from filter -1 2021-02-12T23:30:28Z- TerrainMapOctree: (vehN, vehE ) = (4057150.16, 591068.62). 2021-02-12T23:30:28Z- TerrainMapOctree: Min Distance = 5189.40. 2021-02-12T23:30:28Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:30:28Z- There is no specific good beam that all particles have in common. 2021-02-12T23:30:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:30:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 10.75 2021-02-12T23:30:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:30:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 14.92 2021-02-12T23:30:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:30:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 18.50 2021-02-12T23:30:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:30:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 9.14 2021-02-12T23:30:28Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:30:28Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:30:28Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:30:28Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:30:28Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:30:28Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:30:28Z- Filter 0 No beams used in subcloud update 2021-02-12T23:30:28Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:30:28Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:30:28Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 759.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:30:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172627.38 sec. 2021-02-12T23:30:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:30:28Z- TNavBF::mmse selected from filter -1 2021-02-12T23:30:28Z- variances([0], [2], and psi_berg) = 759.483513 833.042419 0.000000 2021-02-12T23:30:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:30:28Z- LcmTrn::publishEstimates() - MLE : 4057133.91 591025.32 11.70 2021-02-12T23:30:28Z- LcmTrn::publishEstimates() - MMSE : 4057150.41 591068.82 11.70 2021-02-12T23:30:28Z- LcmTrn::publishEstimates() - COVAR: 759.48 833.04 0.93 0.00 2021-02-12T23:30:28Z- AHRS_M2 msg: 1613172628.16 epoch sec; seqNo:4323 2021-02-12T23:30:28Z- AHRS_M2 msg: 0.65 phi; 0.29 theta; -0.18 psi 2021-02-12T23:30:28Z- handleDepth()-Depth_Keller msg: 1613172628162 epoch sec; seqNo:4367; depth 11.62 2021-02-12T23:30:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:28Z- handleDvl() - RDI_Pathfinder msg: 1613172628.16 epoch sec; seqNo:2129 2021-02-12T23:30:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.15 , -0.03 2021-02-12T23:30:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172628.17 epoch sec; seqNo:4367; 4057150.79 north; 591068.82 east 2021-02-12T23:30:28Z- AHRS_M2 msg: 1613172628.56 epoch sec; seqNo:4324 2021-02-12T23:30:28Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.19 psi 2021-02-12T23:30:28Z- handleDepth()-Depth_Keller msg: 1613172628564 epoch sec; seqNo:4368; depth 11.51 2021-02-12T23:30:28Z- handleDvl() - RDI_Pathfinder msg: 1613172628.58 epoch sec; seqNo:2130 2021-02-12T23:30:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 96.50 , 107.45 , 74.37 2021-02-12T23:30:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.12 , 0.13 2021-02-12T23:30:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172628.60 epoch sec; seqNo:4368; 4057151.17 north; 591068.81 east 2021-02-12T23:30:28Z- AHRS_M2 msg: 1613172628.96 epoch sec; seqNo:4325 2021-02-12T23:30:28Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.20 psi 2021-02-12T23:30:28Z- handleDepth()-Depth_Keller msg: 1613172628967 epoch sec; seqNo:4369; depth 11.41 2021-02-12T23:30:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172628.98 epoch sec; seqNo:4369; 4057151.17 north; 591068.81 east 2021-02-12T23:30:29Z- AHRS_M2 msg: 1613172629.37 epoch sec; seqNo:4326 2021-02-12T23:30:29Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.21 psi 2021-02-12T23:30:29Z- handleDepth()-Depth_Keller msg: 1613172629372 epoch sec; seqNo:4370; depth 11.31 2021-02-12T23:30:29Z- handleDvl() - RDI_Pathfinder msg: 1613172629.38 epoch sec; seqNo:2131 2021-02-12T23:30:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 96.50 , 112.32 , 75.60 2021-02-12T23:30:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.22 2021-02-12T23:30:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172629.39 epoch sec; seqNo:4370; 4057151.55 north; 591068.81 east 2021-02-12T23:30:29Z- AHRS_M2 msg: 1613172629.77 epoch sec; seqNo:4327 2021-02-12T23:30:29Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.21 psi 2021-02-12T23:30:29Z- handleDepth()-Depth_Keller msg: 1613172629777 epoch sec; seqNo:4371; depth 11.21 2021-02-12T23:30:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172629.79 epoch sec; seqNo:4371; 4057151.94 north; 591070.03 east 2021-02-12T23:30:30Z- AHRS_M2 msg: 1613172630.18 epoch sec; seqNo:4328 2021-02-12T23:30:30Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.22 psi 2021-02-12T23:30:30Z- handleDepth()-Depth_Keller msg: 1613172630182 epoch sec; seqNo:4372; depth 11.12 2021-02-12T23:30:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172630.20 epoch sec; seqNo:4372; 4057152.32 north; 591070.02 east 2021-02-12T23:30:30Z- AHRS_M2 msg: 1613172630.59 epoch sec; seqNo:4329 2021-02-12T23:30:30Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.22 psi 2021-02-12T23:30:30Z- handleDepth()-Depth_Keller msg: 1613172630592 epoch sec; seqNo:4373; depth 11.01 2021-02-12T23:30:30Z- handleDvl() - RDI_Pathfinder msg: 1613172630.62 epoch sec; seqNo:2132 2021-02-12T23:30:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 104.47 , 115.29 , 78.79 2021-02-12T23:30:30Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.10 , 0.17 2021-02-12T23:30:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172630.63 epoch sec; seqNo:4373; 4057152.70 north; 591070.02 east 2021-02-12T23:30:30Z- AHRS_M2 msg: 1613172630.98 epoch sec; seqNo:4330 2021-02-12T23:30:30Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.22 psi 2021-02-12T23:30:30Z- handleDepth()-Depth_Keller msg: 1613172630988 epoch sec; seqNo:4374; depth 10.88 2021-02-12T23:30:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172631.00 epoch sec; seqNo:4374; 4057152.70 north; 591070.02 east 2021-02-12T23:30:31Z- AHRS_M2 msg: 1613172631.39 epoch sec; seqNo:4331 2021-02-12T23:30:31Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.21 psi 2021-02-12T23:30:31Z- handleDepth()-Depth_Keller msg: 1613172631394 epoch sec; seqNo:4375; depth 10.78 2021-02-12T23:30:31Z- handleDvl() - RDI_Pathfinder msg: 1613172631.41 epoch sec; seqNo:2133 2021-02-12T23:30:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.28 , 105.56 , 119.07 , 76.83 2021-02-12T23:30:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.08 , -0.01 2021-02-12T23:30:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172631.42 epoch sec; seqNo:4375; 4057153.07 north; 591070.01 east 2021-02-12T23:30:31Z- AHRS_M2 msg: 1613172631.79 epoch sec; seqNo:4332 2021-02-12T23:30:31Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.21 psi 2021-02-12T23:30:31Z- handleDepth()-Depth_Keller msg: 1613172631796 epoch sec; seqNo:4376; depth 10.68 2021-02-12T23:30:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:31Z- handleDvl() - RDI_Pathfinder msg: 1613172631.80 epoch sec; seqNo:2134 2021-02-12T23:30:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.08 , -0.01 2021-02-12T23:30:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172631.82 epoch sec; seqNo:4376; 4057153.47 north; 591071.23 east 2021-02-12T23:30:32Z- AHRS_M2 msg: 1613172632.20 epoch sec; seqNo:4333 2021-02-12T23:30:32Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.21 psi 2021-02-12T23:30:32Z- handleDepth()-Depth_Keller msg: 1613172632200 epoch sec; seqNo:4377; depth 10.55 2021-02-12T23:30:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172632.21 epoch sec; seqNo:4377; 4057153.47 north; 591071.23 east 2021-02-12T23:30:32Z- AHRS_M2 msg: 1613172632.60 epoch sec; seqNo:4334 2021-02-12T23:30:32Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.21 psi 2021-02-12T23:30:32Z- handleDepth()-Depth_Keller msg: 1613172632609 epoch sec; seqNo:4378; depth 10.43 2021-02-12T23:30:32Z- handleDvl() - RDI_Pathfinder msg: 1613172632.62 epoch sec; seqNo:2135 2021-02-12T23:30:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 106.77 , 118.94 , 78.38 2021-02-12T23:30:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , -0.10 2021-02-12T23:30:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172632.63 epoch sec; seqNo:4378; 4057153.84 north; 591071.23 east 2021-02-12T23:30:33Z- AHRS_M2 msg: 1613172633.00 epoch sec; seqNo:4335 2021-02-12T23:30:33Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.20 psi 2021-02-12T23:30:33Z- handleDepth()-Depth_Keller msg: 1613172633010 epoch sec; seqNo:4379; depth 10.32 2021-02-12T23:30:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:33Z- handleDvl() - RDI_Pathfinder msg: 1613172633.01 epoch sec; seqNo:2136 2021-02-12T23:30:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , -0.10 2021-02-12T23:30:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172633.02 epoch sec; seqNo:4379; 4057154.22 north; 591071.22 east 2021-02-12T23:30:33Z- LcmTrn::handleMessages() - handling 47 messages took 0.66 ms and 134 calls... 2021-02-12T23:30:33Z- waiting for 1613172630.77, time is 1613172633.02 2021-02-12T23:30:33Z- LcmTrn::updateTrn() equating pose 1613172633.02 and meas times 1613172633.02 2021-02-12T23:30:33Z- LcmTrn::updateTrn() >>>> pose time 1613172633.02, meas time 1613172633.02 <<<< 2021-02-12T23:30:33Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172633.02, ping # 0. 2021-02-12T23:30:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172633.02 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:30:33Z- TerrainNav::motionUpdate() - currEstimate = (4057154, 591071.22, 10.32) 2021-02-12T23:30:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:30:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:30:33Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:30:33Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:30:33Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172633.02 sec.; No relevant navigation data available 2021-02-12T23:30:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:30:33Z- TNavBF::mmse selected from filter -1 2021-02-12T23:30:33Z- variances([0], [2], and psi_berg) = 759.365827 833.042295 0.000000 2021-02-12T23:30:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:30:33Z- LcmTrn::publishEstimates() - MLE : 4057137.72 591027.72 10.32 2021-02-12T23:30:33Z- LcmTrn::publishEstimates() - MMSE : 4057154.22 591071.22 10.32 2021-02-12T23:30:33Z- LcmTrn::publishEstimates() - COVAR: 759.37 833.04 1.16 0.00 2021-02-12T23:30:33Z- AHRS_M2 msg: 1613172633.41 epoch sec; seqNo:4336 2021-02-12T23:30:33Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.20 psi 2021-02-12T23:30:33Z- handleDepth()-Depth_Keller msg: 1613172633412 epoch sec; seqNo:4380; depth 10.19 2021-02-12T23:30:33Z- handleDvl() - RDI_Pathfinder msg: 1613172633.43 epoch sec; seqNo:2137 2021-02-12T23:30:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.07 , 106.70 , 121.58 , 75.19 2021-02-12T23:30:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.16 , -0.03 2021-02-12T23:30:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172633.45 epoch sec; seqNo:4380; 4057154.60 north; 591071.22 east 2021-02-12T23:30:33Z- AHRS_M2 msg: 1613172633.83 epoch sec; seqNo:4337 2021-02-12T23:30:33Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.20 psi 2021-02-12T23:30:33Z- handleDepth()-Depth_Keller msg: 1613172633838 epoch sec; seqNo:4381; depth 10.07 2021-02-12T23:30:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172633.86 epoch sec; seqNo:4381; 4057154.98 north; 591071.22 east 2021-02-12T23:30:34Z- AHRS_M2 msg: 1613172634.22 epoch sec; seqNo:4338 2021-02-12T23:30:34Z- AHRS_M2 msg: 0.57 phi; 0.37 theta; -0.20 psi 2021-02-12T23:30:34Z- handleDepth()-Depth_Keller msg: 1613172634222 epoch sec; seqNo:4382; depth 9.95 2021-02-12T23:30:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:34Z- handleDvl() - RDI_Pathfinder msg: 1613172634.22 epoch sec; seqNo:2138 2021-02-12T23:30:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.16 , -0.03 2021-02-12T23:30:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172634.23 epoch sec; seqNo:4382; 4057155.36 north; 591071.21 east 2021-02-12T23:30:34Z- AHRS_M2 msg: 1613172634.62 epoch sec; seqNo:4339 2021-02-12T23:30:34Z- AHRS_M2 msg: 0.56 phi; 0.37 theta; -0.20 psi 2021-02-12T23:30:34Z- handleDepth()-Depth_Keller msg: 1613172634629 epoch sec; seqNo:4383; depth 9.82 2021-02-12T23:30:34Z- handleDvl() - RDI_Pathfinder msg: 1613172634.64 epoch sec; seqNo:2139 2021-02-12T23:30:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 106.77 , 121.64 , 76.83 2021-02-12T23:30:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.09 , 0.08 2021-02-12T23:30:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172634.65 epoch sec; seqNo:4383; 4057155.37 north; 591072.43 east 2021-02-12T23:30:35Z- AHRS_M2 msg: 1613172635.03 epoch sec; seqNo:4340 2021-02-12T23:30:35Z- AHRS_M2 msg: 0.55 phi; 0.37 theta; -0.20 psi 2021-02-12T23:30:35Z- handleDepth()-Depth_Keller msg: 1613172635029 epoch sec; seqNo:4384; depth 9.69 2021-02-12T23:30:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172635.04 epoch sec; seqNo:4384; 4057155.75 north; 591072.43 east 2021-02-12T23:30:35Z- AHRS_M2 msg: 1613172635.43 epoch sec; seqNo:4341 2021-02-12T23:30:35Z- AHRS_M2 msg: 0.55 phi; 0.37 theta; -0.20 psi 2021-02-12T23:30:35Z- handleDepth()-Depth_Keller msg: 1613172635436 epoch sec; seqNo:4385; depth 9.56 2021-02-12T23:30:35Z- handleDvl() - RDI_Pathfinder msg: 1613172635.46 epoch sec; seqNo:2140 2021-02-12T23:30:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.42 , 105.35 , 121.58 , 76.83 2021-02-12T23:30:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.03 , 0.15 2021-02-12T23:30:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172635.47 epoch sec; seqNo:4385; 4057156.13 north; 591072.42 east 2021-02-12T23:30:35Z- AHRS_M2 msg: 1613172635.83 epoch sec; seqNo:4342 2021-02-12T23:30:35Z- AHRS_M2 msg: 0.54 phi; 0.38 theta; -0.19 psi 2021-02-12T23:30:35Z- handleDepth()-Depth_Keller msg: 1613172635841 epoch sec; seqNo:4386; depth 9.42 2021-02-12T23:30:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172635.87 epoch sec; seqNo:4386; 4057156.51 north; 591072.42 east 2021-02-12T23:30:36Z- AHRS_M2 msg: 1613172636.24 epoch sec; seqNo:4343 2021-02-12T23:30:36Z- AHRS_M2 msg: 0.54 phi; 0.38 theta; -0.18 psi 2021-02-12T23:30:36Z- handleDepth()-Depth_Keller msg: 1613172636242 epoch sec; seqNo:4387; depth 9.30 2021-02-12T23:30:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172636.25 epoch sec; seqNo:4387; 4057156.51 north; 591072.42 east 2021-02-12T23:30:36Z- AHRS_M2 msg: 1613172636.64 epoch sec; seqNo:4344 2021-02-12T23:30:36Z- AHRS_M2 msg: 0.55 phi; 0.39 theta; -0.17 psi 2021-02-12T23:30:36Z- handleDepth()-Depth_Keller msg: 1613172636657 epoch sec; seqNo:4388; depth 9.17 2021-02-12T23:30:36Z- handleDvl() - RDI_Pathfinder msg: 1613172636.67 epoch sec; seqNo:2141 2021-02-12T23:30:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.78 , 106.70 , 122.93 , 75.60 2021-02-12T23:30:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.22 , 0.13 2021-02-12T23:30:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172636.68 epoch sec; seqNo:4388; 4057156.89 north; 591072.42 east 2021-02-12T23:30:37Z- AHRS_M2 msg: 1613172637.04 epoch sec; seqNo:4345 2021-02-12T23:30:37Z- AHRS_M2 msg: 0.56 phi; 0.39 theta; -0.15 psi 2021-02-12T23:30:37Z- handleDepth()-Depth_Keller msg: 1613172637048 epoch sec; seqNo:4389; depth 9.03 2021-02-12T23:30:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172637.07 epoch sec; seqNo:4389; 4057156.90 north; 591073.64 east 2021-02-12T23:30:37Z- AHRS_M2 msg: 1613172637.45 epoch sec; seqNo:4346 2021-02-12T23:30:37Z- AHRS_M2 msg: 0.57 phi; 0.40 theta; -0.13 psi 2021-02-12T23:30:37Z- handleDepth()-Depth_Keller msg: 1613172637452 epoch sec; seqNo:4390; depth 8.91 2021-02-12T23:30:37Z- handleDvl() - RDI_Pathfinder msg: 1613172637.46 epoch sec; seqNo:2142 2021-02-12T23:30:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.76 , 102.58 , 125.56 , 79.60 2021-02-12T23:30:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.28 , -0.07 2021-02-12T23:30:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172637.49 epoch sec; seqNo:4390; 4057157.28 north; 591073.63 east 2021-02-12T23:30:37Z- AHRS_M2 msg: 1613172637.86 epoch sec; seqNo:4347 2021-02-12T23:30:37Z- AHRS_M2 msg: 0.59 phi; 0.40 theta; -0.12 psi 2021-02-12T23:30:37Z- handleDepth()-Depth_Keller msg: 1613172637869 epoch sec; seqNo:4391; depth 8.77 2021-02-12T23:30:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:37Z- handleDvl() - RDI_Pathfinder msg: 1613172637.87 epoch sec; seqNo:2143 2021-02-12T23:30:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.28 , -0.07 2021-02-12T23:30:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172637.89 epoch sec; seqNo:4391; 4057157.28 north; 591073.63 east 2021-02-12T23:30:38Z- LcmTrn::handleMessages() - handling 43 messages took 0.67 ms and 130 calls... 2021-02-12T23:30:38Z- waiting for 1613172636.02, time is 1613172637.89 2021-02-12T23:30:38Z- LcmTrn::updateTrn() equating pose 1613172637.89 and meas times 1613172637.89 2021-02-12T23:30:38Z- LcmTrn::updateTrn() >>>> pose time 1613172637.89, meas time 1613172637.89 <<<< 2021-02-12T23:30:38Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172637.89, ping # 0. 2021-02-12T23:30:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172637.89 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:30:38Z- TerrainNav::motionUpdate() - currEstimate = (4057157, 591073.63, 8.77) 2021-02-12T23:30:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:30:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:30:38Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:30:38Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:30:38Z- TNavBF::Projecting Measurements 2021-02-12T23:30:38Z- TNavFilter::Measurement from time = 1613172637.89 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:30:38Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:30:38Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:30:38Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:30:38Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:30:38Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:30:38Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:30:38Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:30:38Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 759.3 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-12T23:30:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:30:38Z- TNavBF::mmse selected from filter -1 2021-02-12T23:30:38Z- variances([0], [2], and psi_berg) = 759.256651 833.042180 0.000000 2021-02-12T23:30:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:30:38Z- LcmTrn::publishEstimates() - MLE : 4057140.78 591030.13 8.77 2021-02-12T23:30:38Z- LcmTrn::publishEstimates() - MMSE : 4057157.28 591073.63 8.77 2021-02-12T23:30:38Z- LcmTrn::publishEstimates() - COVAR: 759.26 833.04 1.36 0.00 2021-02-12T23:30:38Z- AHRS_M2 msg: 1613172638.26 epoch sec; seqNo:4348 2021-02-12T23:30:38Z- AHRS_M2 msg: 0.60 phi; 0.40 theta; -0.10 psi 2021-02-12T23:30:38Z- handleDepth()-Depth_Keller msg: 1613172638266 epoch sec; seqNo:4392; depth 8.62 2021-02-12T23:30:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172638.28 epoch sec; seqNo:4392; 4057157.66 north; 591073.63 east 2021-02-12T23:30:38Z- AHRS_M2 msg: 1613172638.67 epoch sec; seqNo:4349 2021-02-12T23:30:38Z- AHRS_M2 msg: 0.63 phi; 0.40 theta; -0.09 psi 2021-02-12T23:30:38Z- handleDepth()-Depth_Keller msg: 1613172638678 epoch sec; seqNo:4393; depth 8.47 2021-02-12T23:30:38Z- handleDvl() - RDI_Pathfinder msg: 1613172638.69 epoch sec; seqNo:2144 2021-02-12T23:30:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.98 , 102.44 , 126.78 , 76.42 2021-02-12T23:30:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.10 , -0.30 2021-02-12T23:30:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172638.70 epoch sec; seqNo:4393; 4057157.66 north; 591073.63 east 2021-02-12T23:30:39Z- AHRS_M2 msg: 1613172639.07 epoch sec; seqNo:4350 2021-02-12T23:30:39Z- AHRS_M2 msg: 0.64 phi; 0.39 theta; -0.09 psi 2021-02-12T23:30:39Z- handleDepth()-Depth_Keller msg: 1613172639069 epoch sec; seqNo:4394; depth 8.31 2021-02-12T23:30:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:39Z- handleDvl() - RDI_Pathfinder msg: 1613172639.07 epoch sec; seqNo:2145 2021-02-12T23:30:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.10 , -0.30 2021-02-12T23:30:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172639.10 epoch sec; seqNo:4394; 4057158.04 north; 591073.63 east 2021-02-12T23:30:39Z- AHRS_M2 msg: 1613172639.47 epoch sec; seqNo:4351 2021-02-12T23:30:39Z- AHRS_M2 msg: 0.65 phi; 0.38 theta; -0.10 psi 2021-02-12T23:30:39Z- handleDepth()-Depth_Keller msg: 1613172639473 epoch sec; seqNo:4395; depth 8.16 2021-02-12T23:30:39Z- handleDvl() - RDI_Pathfinder msg: 1613172639.48 epoch sec; seqNo:2146 2021-02-12T23:30:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 99.74 , 125.43 , 74.78 2021-02-12T23:30:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.20 , -0.27 2021-02-12T23:30:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172639.50 epoch sec; seqNo:4395; 4057158.43 north; 591074.84 east 2021-02-12T23:30:39Z- AHRS_M2 msg: 1613172639.87 epoch sec; seqNo:4352 2021-02-12T23:30:39Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.11 psi 2021-02-12T23:30:39Z- handleDepth()-Depth_Keller msg: 1613172639878 epoch sec; seqNo:4396; depth 8.02 2021-02-12T23:30:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172639.89 epoch sec; seqNo:4396; 4057158.43 north; 591074.84 east 2021-02-12T23:30:40Z- AHRS_M2 msg: 1613172640.28 epoch sec; seqNo:4353 2021-02-12T23:30:40Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.11 psi 2021-02-12T23:30:40Z- handleDepth()-Depth_Keller msg: 1613172640284 epoch sec; seqNo:4397; depth 7.87 2021-02-12T23:30:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:40Z- handleDvl() - RDI_Pathfinder msg: 1613172640.29 epoch sec; seqNo:2147 2021-02-12T23:30:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.20 , -0.27 2021-02-12T23:30:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172640.32 epoch sec; seqNo:4397; 4057158.81 north; 591074.84 east 2021-02-12T23:30:40Z- AHRS_M2 msg: 1613172640.68 epoch sec; seqNo:4354 2021-02-12T23:30:40Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.12 psi 2021-02-12T23:30:40Z- handleDepth()-Depth_Keller msg: 1613172640684 epoch sec; seqNo:4398; depth 7.71 2021-02-12T23:30:40Z- handleDvl() - RDI_Pathfinder msg: 1613172640.70 epoch sec; seqNo:2148 2021-02-12T23:30:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.05 , 95.75 , 121.44 , 75.19 2021-02-12T23:30:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.37 , 0.07 2021-02-12T23:30:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172640.72 epoch sec; seqNo:4398; 4057159.19 north; 591074.83 east 2021-02-12T23:30:41Z- AHRS_M2 msg: 1613172641.08 epoch sec; seqNo:4355 2021-02-12T23:30:41Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.14 psi 2021-02-12T23:30:41Z- handleDepth()-Depth_Keller msg: 1613172641088 epoch sec; seqNo:4399; depth 7.56 2021-02-12T23:30:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172641.10 epoch sec; seqNo:4399; 4057159.57 north; 591074.83 east 2021-02-12T23:30:41Z- AHRS_M2 msg: 1613172641.49 epoch sec; seqNo:4356 2021-02-12T23:30:41Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.14 psi 2021-02-12T23:30:41Z- handleDepth()-Depth_Keller msg: 1613172641500 epoch sec; seqNo:4400; depth 7.44 2021-02-12T23:30:41Z- handleDvl() - RDI_Pathfinder msg: 1613172641.51 epoch sec; seqNo:2149 2021-02-12T23:30:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.97 , 96.02 , 121.71 , 76.83 2021-02-12T23:30:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.20 , 0.32 2021-02-12T23:30:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172641.52 epoch sec; seqNo:4400; 4057159.57 north; 591074.83 east 2021-02-12T23:30:41Z- AHRS_M2 msg: 1613172641.89 epoch sec; seqNo:4357 2021-02-12T23:30:41Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-12T23:30:41Z- handleDepth()-Depth_Keller msg: 1613172641896 epoch sec; seqNo:4401; depth 7.28 2021-02-12T23:30:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172641.91 epoch sec; seqNo:4401; 4057159.95 north; 591074.83 east 2021-02-12T23:30:42Z- AHRS_M2 msg: 1613172642.30 epoch sec; seqNo:4358 2021-02-12T23:30:42Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-12T23:30:42Z- handleDepth()-Depth_Keller msg: 1613172642304 epoch sec; seqNo:4402; depth 7.15 2021-02-12T23:30:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172642.31 epoch sec; seqNo:4402; 4057160.32 north; 591074.82 east 2021-02-12T23:30:42Z- AHRS_M2 msg: 1613172642.70 epoch sec; seqNo:4359 2021-02-12T23:30:42Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.17 psi 2021-02-12T23:30:42Z- handleDepth()-Depth_Keller msg: 1613172642705 epoch sec; seqNo:4403; depth 7.02 2021-02-12T23:30:42Z- handleDvl() - RDI_Pathfinder msg: 1613172642.72 epoch sec; seqNo:2150 2021-02-12T23:30:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.55 , 104.07 , 121.64 , 77.24 2021-02-12T23:30:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , 0.24 2021-02-12T23:30:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172642.73 epoch sec; seqNo:4403; 4057160.70 north; 591074.82 east 2021-02-12T23:30:43Z- AHRS_M2 msg: 1613172643.10 epoch sec; seqNo:4360 2021-02-12T23:30:43Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.17 psi 2021-02-12T23:30:43Z- handleDepth()-Depth_Keller msg: 1613172643111 epoch sec; seqNo:4404; depth 6.88 2021-02-12T23:30:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172643.13 epoch sec; seqNo:4404; 4057161.08 north; 591074.81 east 2021-02-12T23:30:43Z- LcmTrn::handleMessages() - handling 46 messages took 0.70 ms and 132 calls... 2021-02-12T23:30:43Z- waiting for 1613172640.89, time is 1613172643.13 2021-02-12T23:30:43Z- LcmTrn::updateTrn() >>>> pose time 1613172643.13, meas time 1613172642.72 <<<< 2021-02-12T23:30:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172642.72 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:30:43Z- TerrainNav::motionUpdate() - currEstimate = (4057161, 591074.81, 6.88) 2021-02-12T23:30:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:30:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:30:43Z- TNavBF::Projecting Measurements 2021-02-12T23:30:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:30:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:30:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:30:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:30:43Z- TNavFilter::4 good beams. 2021-02-12T23:30:43Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:30:43Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:30:43Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:30:43Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:30:43Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:30:43Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:30:43Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:30:43Z- TNavBF::mmse selected from filter -1 2021-02-12T23:30:43Z- TerrainMapOctree: (vehN, vehE ) = (4057160.79, 591074.72). 2021-02-12T23:30:43Z- TerrainMapOctree: Min Distance = 5177.26. 2021-02-12T23:30:43Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:30:43Z- There is no specific good beam that all particles have in common. 2021-02-12T23:30:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:30:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.55 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 13.12 2021-02-12T23:30:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:30:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 17.33 2021-02-12T23:30:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:30:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 23.68 2021-02-12T23:30:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:30:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 9.54 2021-02-12T23:30:43Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:30:43Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:30:43Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:30:43Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:30:43Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:30:43Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:30:43Z- Filter 0 No beams used in subcloud update 2021-02-12T23:30:43Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:30:43Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:30:43Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 759.2 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:30:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172642.72 sec. 2021-02-12T23:30:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:30:43Z- TNavBF::mmse selected from filter -1 2021-02-12T23:30:43Z- variances([0], [2], and psi_berg) = 759.120054 833.042037 0.000000 2021-02-12T23:30:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:30:43Z- LcmTrn::publishEstimates() - MLE : 4057144.58 591031.31 6.88 2021-02-12T23:30:43Z- LcmTrn::publishEstimates() - MMSE : 4057161.08 591074.81 6.88 2021-02-12T23:30:43Z- LcmTrn::publishEstimates() - COVAR: 759.12 833.04 1.13 0.00 2021-02-12T23:30:43Z- AHRS_M2 msg: 1613172643.51 epoch sec; seqNo:4361 2021-02-12T23:30:43Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-12T23:30:43Z- handleDepth()-Depth_Keller msg: 1613172643517 epoch sec; seqNo:4405; depth 6.76 2021-02-12T23:30:43Z- handleDvl() - RDI_Pathfinder msg: 1613172643.54 epoch sec; seqNo:2151 2021-02-12T23:30:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.76 , 106.64 , 122.86 , 78.05 2021-02-12T23:30:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.12 , 0.01 2021-02-12T23:30:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172643.55 epoch sec; seqNo:4405; 4057161.09 north; 591076.04 east 2021-02-12T23:30:43Z- AHRS_M2 msg: 1613172643.91 epoch sec; seqNo:4362 2021-02-12T23:30:43Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.17 psi 2021-02-12T23:30:43Z- handleDepth()-Depth_Keller msg: 1613172643916 epoch sec; seqNo:4406; depth 6.65 2021-02-12T23:30:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:43Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:43Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:43Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:43Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:43Z- handleDvl() - RDI_Pathfinder msg: 1613172643.92 epoch sec; seqNo:2152 2021-02-12T23:30:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.12 , 0.01 2021-02-12T23:30:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172643.93 epoch sec; seqNo:4406; 4057161.47 north; 591076.03 east 2021-02-12T23:30:44Z- AHRS_M2 msg: 1613172644.32 epoch sec; seqNo:4363 2021-02-12T23:30:44Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.17 psi 2021-02-12T23:30:44Z- handleDepth()-Depth_Keller msg: 1613172644327 epoch sec; seqNo:4407; depth 6.51 2021-02-12T23:30:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172644.35 epoch sec; seqNo:4407; 4057161.85 north; 591076.03 east 2021-02-12T23:30:44Z- AHRS_M2 msg: 1613172644.72 epoch sec; seqNo:4364 2021-02-12T23:30:44Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.17 psi 2021-02-12T23:30:44Z- handleDepth()-Depth_Keller msg: 1613172644726 epoch sec; seqNo:4408; depth 6.40 2021-02-12T23:30:44Z- handleDvl() - RDI_Pathfinder msg: 1613172644.74 epoch sec; seqNo:2153 2021-02-12T23:30:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.28 , 106.50 , 121.37 , 78.46 2021-02-12T23:30:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , -0.10 2021-02-12T23:30:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172644.75 epoch sec; seqNo:4408; 4057161.85 north; 591076.03 east 2021-02-12T23:30:45Z- AHRS_M2 msg: 1613172645.13 epoch sec; seqNo:4365 2021-02-12T23:30:45Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.17 psi 2021-02-12T23:30:45Z- handleDepth()-Depth_Keller msg: 1613172645140 epoch sec; seqNo:4409; depth 6.29 2021-02-12T23:30:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:45Z- handleDvl() - RDI_Pathfinder msg: 1613172645.14 epoch sec; seqNo:2154 2021-02-12T23:30:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , -0.10 2021-02-12T23:30:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172645.15 epoch sec; seqNo:4409; 4057162.23 north; 591076.02 east 2021-02-12T23:30:45Z- AHRS_M2 msg: 1613172645.53 epoch sec; seqNo:4366 2021-02-12T23:30:45Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.18 psi 2021-02-12T23:30:45Z- handleDepth()-Depth_Keller msg: 1613172645537 epoch sec; seqNo:4410; depth 6.17 2021-02-12T23:30:45Z- handleDvl() - RDI_Pathfinder msg: 1613172645.56 epoch sec; seqNo:2155 2021-02-12T23:30:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 107.92 , 124.14 , 80.88 2021-02-12T23:30:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.09 , -0.05 2021-02-12T23:30:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172645.57 epoch sec; seqNo:4410; 4057162.62 north; 591077.24 east 2021-02-12T23:30:45Z- AHRS_M2 msg: 1613172645.93 epoch sec; seqNo:4367 2021-02-12T23:30:45Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.19 psi 2021-02-12T23:30:45Z- handleDepth()-Depth_Keller msg: 1613172645935 epoch sec; seqNo:4411; depth 6.05 2021-02-12T23:30:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172645.95 epoch sec; seqNo:4411; 4057162.62 north; 591077.24 east 2021-02-12T23:30:46Z- AHRS_M2 msg: 1613172646.36 epoch sec; seqNo:4368 2021-02-12T23:30:46Z- AHRS_M2 msg: 0.65 phi; 0.30 theta; -0.19 psi 2021-02-12T23:30:46Z- handleDepth()-Depth_Keller msg: 1613172646373 epoch sec; seqNo:4412; depth 5.95 2021-02-12T23:30:46Z- handleDvl() - RDI_Pathfinder msg: 1613172646.40 epoch sec; seqNo:2156 2021-02-12T23:30:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.86 , 109.14 , 118.60 , 78.87 2021-02-12T23:30:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.18 , 0.12 2021-02-12T23:30:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172646.41 epoch sec; seqNo:4412; 4057163.00 north; 591077.24 east 2021-02-12T23:30:46Z- AHRS_M2 msg: 1613172646.74 epoch sec; seqNo:4369 2021-02-12T23:30:46Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.20 psi 2021-02-12T23:30:46Z- handleDepth()-Depth_Keller msg: 1613172646744 epoch sec; seqNo:4413; depth 5.84 2021-02-12T23:30:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172646.76 epoch sec; seqNo:4413; 4057163.38 north; 591077.23 east 2021-02-12T23:30:47Z- AHRS_M2 msg: 1613172647.15 epoch sec; seqNo:4370 2021-02-12T23:30:47Z- AHRS_M2 msg: 0.64 phi; 0.28 theta; -0.20 psi 2021-02-12T23:30:47Z- handleDepth()-Depth_Keller msg: 1613172647152 epoch sec; seqNo:4414; depth 5.72 2021-02-12T23:30:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172647.16 epoch sec; seqNo:4414; 4057163.76 north; 591077.23 east 2021-02-12T23:30:47Z- AHRS_M2 msg: 1613172647.55 epoch sec; seqNo:4371 2021-02-12T23:30:47Z- AHRS_M2 msg: 0.65 phi; 0.25 theta; -0.18 psi 2021-02-12T23:30:47Z- handleDepth()-Depth_Keller msg: 1613172647557 epoch sec; seqNo:4415; depth 5.61 2021-02-12T23:30:47Z- handleDvl() - RDI_Pathfinder msg: 1613172647.57 epoch sec; seqNo:2157 2021-02-12T23:30:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 107.92 , 116.03 , 82.23 2021-02-12T23:30:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.26 2021-02-12T23:30:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172647.58 epoch sec; seqNo:4415; 4057164.14 north; 591077.22 east 2021-02-12T23:30:47Z- AHRS_M2 msg: 1613172647.95 epoch sec; seqNo:4372 2021-02-12T23:30:47Z- AHRS_M2 msg: 0.65 phi; 0.21 theta; -0.17 psi 2021-02-12T23:30:47Z- handleDepth()-Depth_Keller msg: 1613172647958 epoch sec; seqNo:4416; depth 5.51 2021-02-12T23:30:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172647.97 epoch sec; seqNo:4416; 4057164.14 north; 591077.22 east 2021-02-12T23:30:48Z- LcmTrn::handleMessages() - handling 43 messages took 0.61 ms and 131 calls... 2021-02-12T23:30:48Z- waiting for 1613172646.13, time is 1613172647.97 2021-02-12T23:30:48Z- LcmTrn::updateTrn() >>>> pose time 1613172647.97, meas time 1613172647.57 <<<< 2021-02-12T23:30:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172647.57 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:30:48Z- TerrainNav::motionUpdate() - currEstimate = (4057164, 591077.22, 5.51) 2021-02-12T23:30:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:30:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:30:48Z- TNavBF::Projecting Measurements 2021-02-12T23:30:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:30:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:30:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:30:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:30:48Z- TNavFilter::4 good beams. 2021-02-12T23:30:48Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:30:48Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:30:48Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:30:48Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:30:48Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:30:48Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:30:48Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:30:48Z- TNavBF::mmse selected from filter -1 2021-02-12T23:30:48Z- TerrainMapOctree: (vehN, vehE ) = (4057163.89, 591077.02). 2021-02-12T23:30:48Z- TerrainMapOctree: Min Distance = 5173.40. 2021-02-12T23:30:48Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:30:48Z- There is no specific good beam that all particles have in common. 2021-02-12T23:30:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:30:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.32 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-12T23:30:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:30:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 18.64 2021-02-12T23:30:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:30:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 21.54 2021-02-12T23:30:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:30:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 10.82 2021-02-12T23:30:48Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:30:48Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:30:48Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:30:48Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:30:48Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:30:48Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:30:48Z- Filter 0 No beams used in subcloud update 2021-02-12T23:30:48Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:30:48Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:30:48Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 759.0 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:30:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172647.57 sec. 2021-02-12T23:30:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:30:48Z- TNavBF::mmse selected from filter -1 2021-02-12T23:30:48Z- variances([0], [2], and psi_berg) = 758.984601 833.041895 0.000000 2021-02-12T23:30:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:30:48Z- LcmTrn::publishEstimates() - MLE : 4057147.64 591033.72 5.51 2021-02-12T23:30:48Z- LcmTrn::publishEstimates() - MMSE : 4057164.14 591077.22 5.51 2021-02-12T23:30:48Z- LcmTrn::publishEstimates() - COVAR: 758.98 833.04 1.03 0.00 2021-02-12T23:30:48Z- AHRS_M2 msg: 1613172648.36 epoch sec; seqNo:4373 2021-02-12T23:30:48Z- AHRS_M2 msg: 0.66 phi; 0.17 theta; -0.16 psi 2021-02-12T23:30:48Z- handleDepth()-Depth_Keller msg: 1613172648360 epoch sec; seqNo:4417; depth 5.41 2021-02-12T23:30:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172648.38 epoch sec; seqNo:4417; 4057164.53 north; 591078.44 east 2021-02-12T23:30:48Z- AHRS_M2 msg: 1613172648.76 epoch sec; seqNo:4374 2021-02-12T23:30:48Z- AHRS_M2 msg: 0.66 phi; 0.13 theta; -0.15 psi 2021-02-12T23:30:48Z- handleDepth()-Depth_Keller msg: 1613172648764 epoch sec; seqNo:4418; depth 5.30 2021-02-12T23:30:48Z- handleDvl() - RDI_Pathfinder msg: 1613172648.78 epoch sec; seqNo:2158 2021-02-12T23:30:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.86 , 103.73 , 109.14 , 82.10 2021-02-12T23:30:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.18 , 0.18 2021-02-12T23:30:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172648.79 epoch sec; seqNo:4418; 4057164.91 north; 591078.44 east 2021-02-12T23:30:49Z- AHRS_M2 msg: 1613172649.16 epoch sec; seqNo:4375 2021-02-12T23:30:49Z- AHRS_M2 msg: 0.66 phi; 0.08 theta; -0.15 psi 2021-02-12T23:30:49Z- handleDepth()-Depth_Keller msg: 1613172649168 epoch sec; seqNo:4419; depth 5.21 2021-02-12T23:30:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172649.18 epoch sec; seqNo:4419; 4057164.91 north; 591078.44 east 2021-02-12T23:30:49Z- AHRS_M2 msg: 1613172649.57 epoch sec; seqNo:4376 2021-02-12T23:30:49Z- AHRS_M2 msg: 0.65 phi; 0.05 theta; -0.17 psi 2021-02-12T23:30:49Z- handleDepth()-Depth_Keller msg: 1613172649573 epoch sec; seqNo:4420; depth 5.14 2021-02-12T23:30:49Z- handleDvl() - RDI_Pathfinder msg: 1613172649.59 epoch sec; seqNo:2159 2021-02-12T23:30:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 100.08 , 101.43 , 87.91 2021-02-12T23:30:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.30 , -0.02 2021-02-12T23:30:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172649.60 epoch sec; seqNo:4420; 4057165.29 north; 591078.43 east 2021-02-12T23:30:49Z- AHRS_M2 msg: 1613172649.97 epoch sec; seqNo:4377 2021-02-12T23:30:49Z- AHRS_M2 msg: 0.66 phi; 0.02 theta; -0.20 psi 2021-02-12T23:30:49Z- handleDepth()-Depth_Keller msg: 1613172649976 epoch sec; seqNo:4421; depth 5.07 2021-02-12T23:30:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:49Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:49Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:49Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:49Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:49Z- handleDvl() - RDI_Pathfinder msg: 1613172649.98 epoch sec; seqNo:2160 2021-02-12T23:30:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.30 , -0.02 2021-02-12T23:30:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172650.00 epoch sec; seqNo:4421; 4057165.30 north; 591079.65 east 2021-02-12T23:30:50Z- AHRS_M2 msg: 1613172650.38 epoch sec; seqNo:4378 2021-02-12T23:30:50Z- AHRS_M2 msg: 0.67 phi; -0.01 theta; -0.22 psi 2021-02-12T23:30:50Z- handleDepth()-Depth_Keller msg: 1613172650384 epoch sec; seqNo:4422; depth 5.02 2021-02-12T23:30:50Z- handleDvl() - RDI_Pathfinder msg: 1613172650.39 epoch sec; seqNo:2161 2021-02-12T23:30:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 104.34 , 97.58 , 97.58 2021-02-12T23:30:50Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.31 , -0.13 2021-02-12T23:30:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172650.42 epoch sec; seqNo:4422; 4057165.68 north; 591079.65 east 2021-02-12T23:30:50Z- AHRS_M2 msg: 1613172650.78 epoch sec; seqNo:4379 2021-02-12T23:30:50Z- AHRS_M2 msg: 0.68 phi; -0.03 theta; -0.22 psi 2021-02-12T23:30:50Z- handleDepth()-Depth_Keller msg: 1613172650784 epoch sec; seqNo:4423; depth 5.00 2021-02-12T23:30:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172650.80 epoch sec; seqNo:4423; 4057166.06 north; 591079.65 east 2021-02-12T23:30:51Z- AHRS_M2 msg: 1613172651.18 epoch sec; seqNo:4380 2021-02-12T23:30:51Z- AHRS_M2 msg: 0.70 phi; -0.05 theta; -0.20 psi 2021-02-12T23:30:51Z- handleDepth()-Depth_Keller msg: 1613172651190 epoch sec; seqNo:4424; depth 5.00 2021-02-12T23:30:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:51Z- handleDvl() - RDI_Pathfinder msg: 1613172651.19 epoch sec; seqNo:2162 2021-02-12T23:30:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:51Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.31 , -0.13 2021-02-12T23:30:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172651.20 epoch sec; seqNo:4424; 4057166.44 north; 591079.64 east 2021-02-12T23:30:51Z- AHRS_M2 msg: 1613172651.59 epoch sec; seqNo:4381 2021-02-12T23:30:51Z- AHRS_M2 msg: 0.71 phi; -0.08 theta; -0.19 psi 2021-02-12T23:30:51Z- handleDepth()-Depth_Keller msg: 1613172651608 epoch sec; seqNo:4425; depth 5.00 2021-02-12T23:30:51Z- handleDvl() - RDI_Pathfinder msg: 1613172651.62 epoch sec; seqNo:2163 2021-02-12T23:30:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.21 , 105.49 , 91.97 , 96.02 2021-02-12T23:30:51Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.20 , -0.31 2021-02-12T23:30:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172651.63 epoch sec; seqNo:4425; 4057166.45 north; 591080.86 east 2021-02-12T23:30:51Z- AHRS_M2 msg: 1613172651.99 epoch sec; seqNo:4382 2021-02-12T23:30:51Z- AHRS_M2 msg: 0.72 phi; -0.10 theta; -0.19 psi 2021-02-12T23:30:52Z- handleDepth()-Depth_Keller msg: 1613172651999 epoch sec; seqNo:4426; depth 5.00 2021-02-12T23:30:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172652.03 epoch sec; seqNo:4426; 4057166.83 north; 591080.86 east 2021-02-12T23:30:52Z- AHRS_M2 msg: 1613172652.40 epoch sec; seqNo:4383 2021-02-12T23:30:52Z- AHRS_M2 msg: 0.74 phi; -0.13 theta; -0.20 psi 2021-02-12T23:30:52Z- handleDepth()-Depth_Keller msg: 1613172652400 epoch sec; seqNo:4427; depth 5.04 2021-02-12T23:30:52Z- handleDvl() - RDI_Pathfinder msg: 1613172652.41 epoch sec; seqNo:2164 2021-02-12T23:30:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 104.27 , 88.05 , 98.86 2021-02-12T23:30:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.16 , -0.45 2021-02-12T23:30:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172652.42 epoch sec; seqNo:4427; 4057167.21 north; 591080.85 east 2021-02-12T23:30:52Z- AHRS_M2 msg: 1613172652.80 epoch sec; seqNo:4384 2021-02-12T23:30:52Z- AHRS_M2 msg: 0.75 phi; -0.16 theta; -0.20 psi 2021-02-12T23:30:52Z- handleDepth()-Depth_Keller msg: 1613172652805 epoch sec; seqNo:4428; depth 5.04 2021-02-12T23:30:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172652.82 epoch sec; seqNo:4428; 4057167.59 north; 591080.85 east 2021-02-12T23:30:53Z- AHRS_M2 msg: 1613172653.20 epoch sec; seqNo:4385 2021-02-12T23:30:53Z- AHRS_M2 msg: 0.76 phi; -0.19 theta; -0.20 psi 2021-02-12T23:30:53Z- handleDepth()-Depth_Keller msg: 1613172653208 epoch sec; seqNo:4429; depth 5.08 2021-02-12T23:30:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172653.22 epoch sec; seqNo:4429; 4057167.59 north; 591080.85 east 2021-02-12T23:30:53Z- LcmTrn::handleMessages() - handling 46 messages took 0.67 ms and 133 calls... 2021-02-12T23:30:53Z- waiting for 1613172650.97, time is 1613172653.22 2021-02-12T23:30:53Z- LcmTrn::updateTrn() >>>> pose time 1613172653.22, meas time 1613172652.41 <<<< 2021-02-12T23:30:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172652.41 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:30:53Z- TerrainNav::motionUpdate() - currEstimate = (4057168, 591080.85, 5.08) 2021-02-12T23:30:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:30:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:30:53Z- TNavBF::Projecting Measurements 2021-02-12T23:30:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:30:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:30:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:30:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:30:53Z- TNavFilter::4 good beams. 2021-02-12T23:30:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:30:53Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:30:53Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:30:53Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:30:53Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:30:53Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:30:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:30:53Z- TNavBF::mmse selected from filter -1 2021-02-12T23:30:53Z- TerrainMapOctree: (vehN, vehE ) = (4057167.05, 591080.29). 2021-02-12T23:30:53Z- TerrainMapOctree: Min Distance = 5168.90. 2021-02-12T23:30:53Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:30:53Z- There is no specific good beam that all particles have in common. 2021-02-12T23:30:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:30:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 11.65 2021-02-12T23:30:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:30:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 17.40 2021-02-12T23:30:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:30:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 12.40 2021-02-12T23:30:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:30:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.64 2021-02-12T23:30:53Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:30:53Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:30:53Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:30:53Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:30:53Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:30:53Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:30:53Z- Filter 0 No beams used in subcloud update 2021-02-12T23:30:53Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:30:53Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:30:53Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 758.9 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:30:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172652.41 sec. 2021-02-12T23:30:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:30:53Z- TNavBF::mmse selected from filter -1 2021-02-12T23:30:53Z- variances([0], [2], and psi_berg) = 758.823061 833.041727 0.000000 2021-02-12T23:30:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:30:53Z- LcmTrn::publishEstimates() - MLE : 4057151.09 591037.35 5.08 2021-02-12T23:30:53Z- LcmTrn::publishEstimates() - MMSE : 4057167.59 591080.85 5.08 2021-02-12T23:30:53Z- LcmTrn::publishEstimates() - COVAR: 758.82 833.04 0.98 0.00 2021-02-12T23:30:53Z- AHRS_M2 msg: 1613172653.61 epoch sec; seqNo:4386 2021-02-12T23:30:53Z- AHRS_M2 msg: 0.77 phi; -0.22 theta; -0.20 psi 2021-02-12T23:30:53Z- handleDepth()-Depth_Keller msg: 1613172653612 epoch sec; seqNo:4430; depth 5.10 2021-02-12T23:30:53Z- handleDvl() - RDI_Pathfinder msg: 1613172653.62 epoch sec; seqNo:2165 2021-02-12T23:30:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 105.62 , 0.00 , 105.62 2021-02-12T23:30:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.61 , -0.43 , -0.24 2021-02-12T23:30:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172653.65 epoch sec; seqNo:4430; 4057167.97 north; 591080.85 east 2021-02-12T23:30:54Z- AHRS_M2 msg: 1613172654.01 epoch sec; seqNo:4387 2021-02-12T23:30:54Z- AHRS_M2 msg: 0.77 phi; -0.25 theta; -0.20 psi 2021-02-12T23:30:54Z- handleDepth()-Depth_Keller msg: 1613172654021 epoch sec; seqNo:4431; depth 5.14 2021-02-12T23:30:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172654.06 epoch sec; seqNo:4431; 4057168.34 north; 591080.84 east 2021-02-12T23:30:54Z- AHRS_M2 msg: 1613172654.44 epoch sec; seqNo:4388 2021-02-12T23:30:54Z- AHRS_M2 msg: 0.76 phi; -0.27 theta; -0.20 psi 2021-02-12T23:30:54Z- handleDepth()-Depth_Keller msg: 1613172654445 epoch sec; seqNo:4432; depth 5.18 2021-02-12T23:30:54Z- handleDvl() - RDI_Pathfinder msg: 1613172654.46 epoch sec; seqNo:2166 2021-02-12T23:30:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.29 , 105.22 , 83.58 , 110.62 2021-02-12T23:30:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.57 , -0.36 , 0.19 2021-02-12T23:30:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172654.48 epoch sec; seqNo:4432; 4057168.74 north; 591082.06 east 2021-02-12T23:30:54Z- AHRS_M2 msg: 1613172654.82 epoch sec; seqNo:4389 2021-02-12T23:30:54Z- AHRS_M2 msg: 0.75 phi; -0.29 theta; -0.20 psi 2021-02-12T23:30:54Z- handleDepth()-Depth_Keller msg: 1613172654824 epoch sec; seqNo:4433; depth 5.25 2021-02-12T23:30:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:54Z- handleDvl() - RDI_Pathfinder msg: 1613172654.83 epoch sec; seqNo:2167 2021-02-12T23:30:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.57 , -0.36 , 0.19 2021-02-12T23:30:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172654.85 epoch sec; seqNo:4433; 4057168.74 north; 591082.06 east 2021-02-12T23:30:55Z- AHRS_M2 msg: 1613172655.22 epoch sec; seqNo:4390 2021-02-12T23:30:55Z- AHRS_M2 msg: 0.74 phi; -0.31 theta; -0.21 psi 2021-02-12T23:30:55Z- handleDepth()-Depth_Keller msg: 1613172655229 epoch sec; seqNo:4434; depth 5.31 2021-02-12T23:30:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172655.24 epoch sec; seqNo:4434; 4057169.12 north; 591082.06 east 2021-02-12T23:30:55Z- AHRS_M2 msg: 1613172655.63 epoch sec; seqNo:4391 2021-02-12T23:30:55Z- AHRS_M2 msg: 0.72 phi; -0.32 theta; -0.22 psi 2021-02-12T23:30:55Z- handleDepth()-Depth_Keller msg: 1613172655638 epoch sec; seqNo:4435; depth 5.39 2021-02-12T23:30:55Z- handleDvl() - RDI_Pathfinder msg: 1613172655.65 epoch sec; seqNo:2168 2021-02-12T23:30:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.84 , 109.48 , 83.79 , 118.94 2021-02-12T23:30:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.01 , 0.41 2021-02-12T23:30:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172655.66 epoch sec; seqNo:4435; 4057169.12 north; 591082.06 east 2021-02-12T23:30:56Z- AHRS_M2 msg: 1613172656.04 epoch sec; seqNo:4392 2021-02-12T23:30:56Z- AHRS_M2 msg: 0.71 phi; -0.32 theta; -0.23 psi 2021-02-12T23:30:56Z- handleDepth()-Depth_Keller msg: 1613172656043 epoch sec; seqNo:4436; depth 5.48 2021-02-12T23:30:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:56Z- handleDvl() - RDI_Pathfinder msg: 1613172656.05 epoch sec; seqNo:2169 2021-02-12T23:30:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.01 , 0.41 2021-02-12T23:30:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172656.06 epoch sec; seqNo:4436; 4057169.49 north; 591082.05 east 2021-02-12T23:30:56Z- handleDepth()-Depth_Keller msg: 1613172656473 epoch sec; seqNo:4437; depth 5.56 2021-02-12T23:30:56Z- handleDvl() - RDI_Pathfinder msg: 1613172656.50 epoch sec; seqNo:2170 2021-02-12T23:30:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 109.21 , 82.17 , 124.08 2021-02-12T23:30:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.25 , 0.18 2021-02-12T23:30:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172656.54 epoch sec; seqNo:4437; 4057169.49 north; 591082.05 east 2021-02-12T23:30:56Z- AHRS_M2 msg: 1613172656.84 epoch sec; seqNo:4393 2021-02-12T23:30:56Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.24 psi 2021-02-12T23:30:56Z- handleDepth()-Depth_Keller msg: 1613172656845 epoch sec; seqNo:4438; depth 5.68 2021-02-12T23:30:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172656.86 epoch sec; seqNo:4438; 4057169.49 north; 591082.05 east 2021-02-12T23:30:57Z- AHRS_M2 msg: 1613172657.24 epoch sec; seqNo:4394 2021-02-12T23:30:57Z- AHRS_M2 msg: 0.69 phi; -0.33 theta; -0.24 psi 2021-02-12T23:30:57Z- handleDepth()-Depth_Keller msg: 1613172657249 epoch sec; seqNo:4439; depth 5.77 2021-02-12T23:30:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:30:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:30:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:30:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:30:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:30:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:30:57Z- handleDvl() - RDI_Pathfinder msg: 1613172657.25 epoch sec; seqNo:2171 2021-02-12T23:30:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:30:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.25 , 0.18 2021-02-12T23:30:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172657.26 epoch sec; seqNo:4439; 4057169.87 north; 591082.05 east 2021-02-12T23:30:57Z- AHRS_M2 msg: 1613172657.65 epoch sec; seqNo:4395 2021-02-12T23:30:57Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.25 psi 2021-02-12T23:30:57Z- handleDepth()-Depth_Keller msg: 1613172657661 epoch sec; seqNo:4440; depth 5.86 2021-02-12T23:30:57Z- handleDvl() - RDI_Pathfinder msg: 1613172657.67 epoch sec; seqNo:2172 2021-02-12T23:30:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.21 , 111.84 , 82.10 , 121.31 2021-02-12T23:30:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.15 , -0.14 2021-02-12T23:30:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172657.69 epoch sec; seqNo:4440; 4057169.89 north; 591083.27 east 2021-02-12T23:30:58Z- AHRS_M2 msg: 1613172658.05 epoch sec; seqNo:4396 2021-02-12T23:30:58Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.27 psi 2021-02-12T23:30:58Z- handleDepth()-Depth_Keller msg: 1613172658061 epoch sec; seqNo:4441; depth 5.96 2021-02-12T23:30:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172658.08 epoch sec; seqNo:4441; 4057170.26 north; 591083.26 east 2021-02-12T23:30:58Z- AHRS_M2 msg: 1613172658.46 epoch sec; seqNo:4397 2021-02-12T23:30:58Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.28 psi 2021-02-12T23:30:58Z- LcmTrn::handleMessages() - handling 44 messages took 0.74 ms and 129 calls... 2021-02-12T23:30:58Z- waiting for 1613172656.22, time is 1613172658.46 2021-02-12T23:30:58Z- LcmTrn::updateTrn() >>>> pose time 1613172658.46, meas time 1613172657.67 <<<< 2021-02-12T23:30:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172657.67 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:30:58Z- TerrainNav::motionUpdate() - currEstimate = (4057170, 591083.26, 5.96) 2021-02-12T23:30:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:30:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:30:58Z- TNavBF::Projecting Measurements 2021-02-12T23:30:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:30:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:30:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:30:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:30:58Z- TNavFilter::4 good beams. 2021-02-12T23:30:58Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:30:58Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:30:58Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:30:58Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:30:58Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:30:58Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:30:58Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:30:58Z- TNavBF::mmse selected from filter -1 2021-02-12T23:30:58Z- TerrainMapOctree: (vehN, vehE ) = (4057169.86, 591082.90). 2021-02-12T23:30:58Z- TerrainMapOctree: Min Distance = 5165.08. 2021-02-12T23:30:58Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:30:58Z- There is no specific good beam that all particles have in common. 2021-02-12T23:30:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:30:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 13.02 2021-02-12T23:30:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:30:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 20.01 2021-02-12T23:30:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:30:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 10.78 2021-02-12T23:30:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:30:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 23.54 2021-02-12T23:30:58Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:30:58Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:30:58Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:30:58Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:30:58Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:30:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:30:58Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:30:58Z- Filter 0 No beams used in subcloud update 2021-02-12T23:30:58Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:30:58Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:30:58Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 758.7 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:30:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172657.67 sec. 2021-02-12T23:30:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:30:58Z- TNavBF::mmse selected from filter -1 2021-02-12T23:30:58Z- variances([0], [2], and psi_berg) = 758.686648 833.041584 0.000000 2021-02-12T23:30:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:30:58Z- LcmTrn::publishEstimates() - MLE : 4057153.76 591039.76 5.96 2021-02-12T23:30:58Z- LcmTrn::publishEstimates() - MMSE : 4057170.26 591083.26 5.96 2021-02-12T23:30:58Z- LcmTrn::publishEstimates() - COVAR: 758.69 833.04 0.93 0.00 2021-02-12T23:30:58Z- handleDepth()-Depth_Keller msg: 1613172658460 epoch sec; seqNo:4442; depth 6.08 2021-02-12T23:30:58Z- handleDvl() - RDI_Pathfinder msg: 1613172658.47 epoch sec; seqNo:2173 2021-02-12T23:30:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 115.83 , 83.11 , 123.27 2021-02-12T23:30:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , -0.14 2021-02-12T23:30:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172658.50 epoch sec; seqNo:4442; 4057170.64 north; 591083.26 east 2021-02-12T23:30:58Z- AHRS_M2 msg: 1613172658.88 epoch sec; seqNo:4398 2021-02-12T23:30:58Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.28 psi 2021-02-12T23:30:58Z- handleDepth()-Depth_Keller msg: 1613172658881 epoch sec; seqNo:4443; depth 6.18 2021-02-12T23:30:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172658.89 epoch sec; seqNo:4443; 4057171.02 north; 591083.26 east 2021-02-12T23:30:59Z- AHRS_M2 msg: 1613172659.26 epoch sec; seqNo:4399 2021-02-12T23:30:59Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.28 psi 2021-02-12T23:30:59Z- handleDepth()-Depth_Keller msg: 1613172659270 epoch sec; seqNo:4444; depth 6.31 2021-02-12T23:30:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172659.28 epoch sec; seqNo:4444; 4057171.02 north; 591083.26 east 2021-02-12T23:30:59Z- AHRS_M2 msg: 1613172659.67 epoch sec; seqNo:4400 2021-02-12T23:30:59Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.27 psi 2021-02-12T23:30:59Z- handleDepth()-Depth_Keller msg: 1613172659677 epoch sec; seqNo:4445; depth 6.41 2021-02-12T23:30:59Z- handleDvl() - RDI_Pathfinder msg: 1613172659.70 epoch sec; seqNo:2174 2021-02-12T23:30:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.51 , 115.76 , 81.56 , 123.20 2021-02-12T23:30:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.16 , 0.09 2021-02-12T23:30:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172659.71 epoch sec; seqNo:4445; 4057171.40 north; 591083.25 east 2021-02-12T23:31:00Z- AHRS_M2 msg: 1613172660.07 epoch sec; seqNo:4401 2021-02-12T23:31:00Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.27 psi 2021-02-12T23:31:00Z- handleDepth()-Depth_Keller msg: 1613172660076 epoch sec; seqNo:4446; depth 6.55 2021-02-12T23:31:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172660.09 epoch sec; seqNo:4446; 4057171.79 north; 591084.47 east 2021-02-12T23:31:00Z- AHRS_M2 msg: 1613172660.48 epoch sec; seqNo:4402 2021-02-12T23:31:00Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.27 psi 2021-02-12T23:31:00Z- handleDepth()-Depth_Keller msg: 1613172660485 epoch sec; seqNo:4447; depth 6.67 2021-02-12T23:31:00Z- handleDvl() - RDI_Pathfinder msg: 1613172660.50 epoch sec; seqNo:2175 2021-02-12T23:31:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 115.76 , 81.56 , 121.71 2021-02-12T23:31:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.12 , 0.19 2021-02-12T23:31:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172660.51 epoch sec; seqNo:4447; 4057172.17 north; 591084.47 east 2021-02-12T23:31:00Z- AHRS_M2 msg: 1613172660.88 epoch sec; seqNo:4403 2021-02-12T23:31:00Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.26 psi 2021-02-12T23:31:00Z- handleDepth()-Depth_Keller msg: 1613172660885 epoch sec; seqNo:4448; depth 6.78 2021-02-12T23:31:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:31:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:31:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:31:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:31:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:31:00Z- handleDvl() - RDI_Pathfinder msg: 1613172660.89 epoch sec; seqNo:2176 2021-02-12T23:31:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:31:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.12 , 0.19 2021-02-12T23:31:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172660.90 epoch sec; seqNo:4448; 4057172.17 north; 591084.47 east 2021-02-12T23:31:01Z- AHRS_M2 msg: 1613172661.28 epoch sec; seqNo:4404 2021-02-12T23:31:01Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.25 psi 2021-02-12T23:31:01Z- handleDepth()-Depth_Keller msg: 1613172661288 epoch sec; seqNo:4449; depth 6.88 2021-02-12T23:31:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172661.30 epoch sec; seqNo:4449; 4057172.55 north; 591084.46 east 2021-02-12T23:31:01Z- AHRS_M2 msg: 1613172661.69 epoch sec; seqNo:4405 2021-02-12T23:31:01Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.23 psi 2021-02-12T23:31:01Z- handleDepth()-Depth_Keller msg: 1613172661697 epoch sec; seqNo:4450; depth 7.02 2021-02-12T23:31:01Z- handleDvl() - RDI_Pathfinder msg: 1613172661.71 epoch sec; seqNo:2177 2021-02-12T23:31:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.65 , 113.94 , 81.22 , 124.35 2021-02-12T23:31:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.04 , 0.24 2021-02-12T23:31:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172661.73 epoch sec; seqNo:4450; 4057172.55 north; 591084.46 east 2021-02-12T23:31:02Z- AHRS_M2 msg: 1613172662.09 epoch sec; seqNo:4406 2021-02-12T23:31:02Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.22 psi 2021-02-12T23:31:02Z- handleDepth()-Depth_Keller msg: 1613172662096 epoch sec; seqNo:4451; depth 7.15 2021-02-12T23:31:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:31:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:31:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:31:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:31:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:31:02Z- handleDvl() - RDI_Pathfinder msg: 1613172662.10 epoch sec; seqNo:2178 2021-02-12T23:31:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:31:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.04 , 0.24 2021-02-12T23:31:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172662.11 epoch sec; seqNo:4451; 4057172.93 north; 591084.46 east 2021-02-12T23:31:02Z- AHRS_M2 msg: 1613172662.50 epoch sec; seqNo:4407 2021-02-12T23:31:02Z- AHRS_M2 msg: 0.57 phi; -0.36 theta; -0.20 psi 2021-02-12T23:31:02Z- handleDepth()-Depth_Keller msg: 1613172662509 epoch sec; seqNo:4452; depth 7.27 2021-02-12T23:31:02Z- handleDvl() - RDI_Pathfinder msg: 1613172662.52 epoch sec; seqNo:2179 2021-02-12T23:31:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 109.81 , 0.00 , 129.15 2021-02-12T23:31:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.29 , 0.16 2021-02-12T23:31:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172662.53 epoch sec; seqNo:4452; 4057172.93 north; 591084.46 east 2021-02-12T23:31:02Z- AHRS_M2 msg: 1613172662.90 epoch sec; seqNo:4408 2021-02-12T23:31:02Z- AHRS_M2 msg: 0.56 phi; -0.36 theta; -0.19 psi 2021-02-12T23:31:02Z- handleDepth()-Depth_Keller msg: 1613172662904 epoch sec; seqNo:4453; depth 7.39 2021-02-12T23:31:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172662.92 epoch sec; seqNo:4453; 4057173.32 north; 591085.67 east 2021-02-12T23:31:03Z- AHRS_M2 msg: 1613172663.31 epoch sec; seqNo:4409 2021-02-12T23:31:03Z- AHRS_M2 msg: 0.56 phi; -0.36 theta; -0.20 psi 2021-02-12T23:31:03Z- handleDepth()-Depth_Keller msg: 1613172663313 epoch sec; seqNo:4454; depth 7.53 2021-02-12T23:31:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:31:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:31:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:31:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:31:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:31:03Z- handleDvl() - RDI_Pathfinder msg: 1613172663.32 epoch sec; seqNo:2180 2021-02-12T23:31:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:31:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.29 , 0.16 2021-02-12T23:31:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172663.34 epoch sec; seqNo:4454; 4057173.32 north; 591085.67 east 2021-02-12T23:31:03Z- LcmTrn::handleMessages() - handling 46 messages took 0.65 ms and 133 calls... 2021-02-12T23:31:03Z- waiting for 1613172661.46, time is 1613172663.34 2021-02-12T23:31:03Z- LcmTrn::updateTrn() equating pose 1613172663.34 and meas times 1613172663.34 2021-02-12T23:31:03Z- LcmTrn::updateTrn() >>>> pose time 1613172663.34, meas time 1613172663.34 <<<< 2021-02-12T23:31:03Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172663.34, ping # 0. 2021-02-12T23:31:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172663.34 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:31:03Z- TerrainNav::motionUpdate() - currEstimate = (4057173, 591085.67, 7.53) 2021-02-12T23:31:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:31:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:31:03Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:31:03Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:31:03Z- TNavBF::Projecting Measurements 2021-02-12T23:31:03Z- TNavFilter::Measurement from time = 1613172663.34 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:31:03Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:31:03Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:31:03Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:31:03Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:31:03Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:31:03Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:31:03Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:31:03Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 758.6 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:31:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:31:03Z- TNavBF::mmse selected from filter -1 2021-02-12T23:31:03Z- variances([0], [2], and psi_berg) = 758.578516 833.041472 0.000000 2021-02-12T23:31:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:31:03Z- LcmTrn::publishEstimates() - MLE : 4057156.82 591042.17 7.53 2021-02-12T23:31:03Z- LcmTrn::publishEstimates() - MMSE : 4057173.32 591085.67 7.53 2021-02-12T23:31:03Z- LcmTrn::publishEstimates() - COVAR: 758.58 833.04 1.13 0.00 2021-02-12T23:31:03Z- AHRS_M2 msg: 1613172663.71 epoch sec; seqNo:4410 2021-02-12T23:31:03Z- AHRS_M2 msg: 0.55 phi; -0.35 theta; -0.22 psi 2021-02-12T23:31:03Z- handleDepth()-Depth_Keller msg: 1613172663712 epoch sec; seqNo:4455; depth 7.64 2021-02-12T23:31:03Z- handleDvl() - RDI_Pathfinder msg: 1613172663.72 epoch sec; seqNo:2181 2021-02-12T23:31:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.87 , 106.23 , 79.46 , 124.08 2021-02-12T23:31:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.40 , -0.17 2021-02-12T23:31:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172663.73 epoch sec; seqNo:4455; 4057173.70 north; 591085.67 east 2021-02-12T23:31:04Z- AHRS_M2 msg: 1613172664.11 epoch sec; seqNo:4411 2021-02-12T23:31:04Z- AHRS_M2 msg: 0.54 phi; -0.34 theta; -0.24 psi 2021-02-12T23:31:04Z- handleDepth()-Depth_Keller msg: 1613172664121 epoch sec; seqNo:4456; depth 7.79 2021-02-12T23:31:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172664.13 epoch sec; seqNo:4456; 4057174.08 north; 591085.67 east 2021-02-12T23:31:04Z- AHRS_M2 msg: 1613172664.52 epoch sec; seqNo:4412 2021-02-12T23:31:04Z- AHRS_M2 msg: 0.54 phi; -0.33 theta; -0.27 psi 2021-02-12T23:31:04Z- handleDepth()-Depth_Keller msg: 1613172664520 epoch sec; seqNo:4457; depth 7.91 2021-02-12T23:31:04Z- handleDvl() - RDI_Pathfinder msg: 1613172664.54 epoch sec; seqNo:2182 2021-02-12T23:31:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 109.21 , 78.46 , 121.37 2021-02-12T23:31:04Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.12 , -0.47 2021-02-12T23:31:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172664.56 epoch sec; seqNo:4457; 4057174.47 north; 591086.88 east 2021-02-12T23:31:04Z- AHRS_M2 msg: 1613172664.92 epoch sec; seqNo:4413 2021-02-12T23:31:04Z- AHRS_M2 msg: 0.55 phi; -0.33 theta; -0.28 psi 2021-02-12T23:31:04Z- handleDepth()-Depth_Keller msg: 1613172664924 epoch sec; seqNo:4458; depth 8.02 2021-02-12T23:31:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172664.94 epoch sec; seqNo:4458; 4057174.85 north; 591086.88 east 2021-02-12T23:31:05Z- AHRS_M2 msg: 1613172665.32 epoch sec; seqNo:4414 2021-02-12T23:31:05Z- AHRS_M2 msg: 0.56 phi; -0.32 theta; -0.27 psi 2021-02-12T23:31:05Z- handleDepth()-Depth_Keller msg: 1613172665328 epoch sec; seqNo:4459; depth 8.16 2021-02-12T23:31:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172665.34 epoch sec; seqNo:4459; 4057175.23 north; 591086.88 east 2021-02-12T23:31:05Z- AHRS_M2 msg: 1613172665.73 epoch sec; seqNo:4415 2021-02-12T23:31:05Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.24 psi 2021-02-12T23:31:05Z- handleDepth()-Depth_Keller msg: 1613172665735 epoch sec; seqNo:4460; depth 8.30 2021-02-12T23:31:05Z- handleDvl() - RDI_Pathfinder msg: 1613172665.75 epoch sec; seqNo:2183 2021-02-12T23:31:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.29 , 110.62 , 80.88 , 118.74 2021-02-12T23:31:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.24 , -0.34 2021-02-12T23:31:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172665.76 epoch sec; seqNo:4460; 4057175.61 north; 591086.87 east 2021-02-12T23:31:06Z- AHRS_M2 msg: 1613172666.13 epoch sec; seqNo:4416 2021-02-12T23:31:06Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.21 psi 2021-02-12T23:31:06Z- handleDepth()-Depth_Keller msg: 1613172666151 epoch sec; seqNo:4461; depth 8.44 2021-02-12T23:31:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172666.16 epoch sec; seqNo:4461; 4057175.99 north; 591086.87 east 2021-02-12T23:31:06Z- AHRS_M2 msg: 1613172666.54 epoch sec; seqNo:4417 2021-02-12T23:31:06Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:31:06Z- handleDepth()-Depth_Keller msg: 1613172666540 epoch sec; seqNo:4462; depth 8.56 2021-02-12T23:31:06Z- handleDvl() - RDI_Pathfinder msg: 1613172666.56 epoch sec; seqNo:2184 2021-02-12T23:31:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 107.92 , 79.53 , 117.39 2021-02-12T23:31:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.37 , -0.03 2021-02-12T23:31:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172666.58 epoch sec; seqNo:4462; 4057176.37 north; 591086.86 east 2021-02-12T23:31:06Z- AHRS_M2 msg: 1613172666.94 epoch sec; seqNo:4418 2021-02-12T23:31:06Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.16 psi 2021-02-12T23:31:06Z- handleDepth()-Depth_Keller msg: 1613172666946 epoch sec; seqNo:4463; depth 8.68 2021-02-12T23:31:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:31:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:31:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:31:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:31:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:31:06Z- handleDvl() - RDI_Pathfinder msg: 1613172666.95 epoch sec; seqNo:2185 2021-02-12T23:31:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:31:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.37 , -0.03 2021-02-12T23:31:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172666.96 epoch sec; seqNo:4463; 4057176.74 north; 591086.86 east 2021-02-12T23:31:07Z- AHRS_M2 msg: 1613172667.34 epoch sec; seqNo:4419 2021-02-12T23:31:07Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.13 psi 2021-02-12T23:31:07Z- handleDepth()-Depth_Keller msg: 1613172667352 epoch sec; seqNo:4464; depth 8.79 2021-02-12T23:31:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172667.38 epoch sec; seqNo:4464; 4057176.74 north; 591086.86 east 2021-02-12T23:31:07Z- AHRS_M2 msg: 1613172667.75 epoch sec; seqNo:4420 2021-02-12T23:31:07Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.11 psi 2021-02-12T23:31:07Z- handleDepth()-Depth_Keller msg: 1613172667753 epoch sec; seqNo:4465; depth 8.89 2021-02-12T23:31:07Z- handleDvl() - RDI_Pathfinder msg: 1613172667.76 epoch sec; seqNo:2186 2021-02-12T23:31:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 102.44 , 76.83 , 117.32 2021-02-12T23:31:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.25 , 0.15 2021-02-12T23:31:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172667.79 epoch sec; seqNo:4465; 4057177.12 north; 591086.85 east 2021-02-12T23:31:08Z- AHRS_M2 msg: 1613172668.15 epoch sec; seqNo:4421 2021-02-12T23:31:08Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.09 psi 2021-02-12T23:31:08Z- handleDepth()-Depth_Keller msg: 1613172668161 epoch sec; seqNo:4466; depth 9.01 2021-02-12T23:31:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:31:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:31:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:31:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:31:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:31:08Z- handleDvl() - RDI_Pathfinder msg: 1613172668.16 epoch sec; seqNo:2187 2021-02-12T23:31:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:31:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.25 , 0.15 2021-02-12T23:31:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172668.17 epoch sec; seqNo:4466; 4057177.50 north; 591086.85 east 2021-02-12T23:31:08Z- AHRS_M2 msg: 1613172668.56 epoch sec; seqNo:4422 2021-02-12T23:31:08Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.08 psi 2021-02-12T23:31:08Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-12T23:31:08Z- waiting for 1613172666.34, time is 1613172668.56 2021-02-12T23:31:08Z- LcmTrn::updateTrn() >>>> pose time 1613172668.56, meas time 1613172668.16 <<<< 2021-02-12T23:31:08Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172668.16, ping # 0. 2021-02-12T23:31:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172668.16 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:31:08Z- TerrainNav::motionUpdate() - currEstimate = (4057178, 591086.85, 9.01) 2021-02-12T23:31:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:31:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:31:08Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:31:08Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:31:08Z- TNavBF::Projecting Measurements 2021-02-12T23:31:08Z- TNavFilter::Measurement from time = 1613172668.16 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:31:08Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:31:08Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:31:08Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:31:08Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:31:08Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:31:08Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:31:08Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:31:08Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 758.5 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-12T23:31:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:31:08Z- TNavBF::mmse selected from filter -1 2021-02-12T23:31:08Z- variances([0], [2], and psi_berg) = 758.437599 833.041325 0.000000 2021-02-12T23:31:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:31:08Z- LcmTrn::publishEstimates() - MLE : 4057161.00 591043.35 9.01 2021-02-12T23:31:08Z- LcmTrn::publishEstimates() - MMSE : 4057177.50 591086.85 9.01 2021-02-12T23:31:08Z- LcmTrn::publishEstimates() - COVAR: 758.44 833.04 1.35 0.00 2021-02-12T23:31:08Z- handleDepth()-Depth_Keller msg: 1613172668570 epoch sec; seqNo:4467; depth 9.13 2021-02-12T23:31:08Z- handleDvl() - RDI_Pathfinder msg: 1613172668.58 epoch sec; seqNo:2188 2021-02-12T23:31:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.20 , 95.96 , 77.24 , 114.88 2021-02-12T23:31:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.12 , 0.16 2021-02-12T23:31:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172668.59 epoch sec; seqNo:4467; 4057177.50 north; 591086.85 east 2021-02-12T23:31:08Z- AHRS_M2 msg: 1613172668.96 epoch sec; seqNo:4423 2021-02-12T23:31:08Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.08 psi 2021-02-12T23:31:08Z- handleDepth()-Depth_Keller msg: 1613172668965 epoch sec; seqNo:4468; depth 9.25 2021-02-12T23:31:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172668.97 epoch sec; seqNo:4468; 4057177.88 north; 591086.85 east 2021-02-12T23:31:09Z- AHRS_M2 msg: 1613172669.38 epoch sec; seqNo:4424 2021-02-12T23:31:09Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.09 psi 2021-02-12T23:31:09Z- handleDepth()-Depth_Keller msg: 1613172669387 epoch sec; seqNo:4469; depth 9.38 2021-02-12T23:31:09Z- handleDvl() - RDI_Pathfinder msg: 1613172669.40 epoch sec; seqNo:2189 2021-02-12T23:31:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.10 , 96.16 , 75.60 , 113.73 2021-02-12T23:31:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.08 2021-02-12T23:31:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172669.42 epoch sec; seqNo:4469; 4057178.27 north; 591088.06 east 2021-02-12T23:31:09Z- AHRS_M2 msg: 1613172669.77 epoch sec; seqNo:4425 2021-02-12T23:31:09Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.09 psi 2021-02-12T23:31:09Z- handleDepth()-Depth_Keller msg: 1613172669774 epoch sec; seqNo:4470; depth 9.50 2021-02-12T23:31:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172669.79 epoch sec; seqNo:4470; 4057178.65 north; 591088.06 east 2021-02-12T23:31:10Z- AHRS_M2 msg: 1613172670.17 epoch sec; seqNo:4426 2021-02-12T23:31:10Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.09 psi 2021-02-12T23:31:10Z- handleDepth()-Depth_Keller msg: 1613172670190 epoch sec; seqNo:4471; depth 9.61 2021-02-12T23:31:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172670.20 epoch sec; seqNo:4471; 4057178.65 north; 591088.06 east 2021-02-12T23:31:10Z- AHRS_M2 msg: 1613172670.58 epoch sec; seqNo:4427 2021-02-12T23:31:10Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.09 psi 2021-02-12T23:31:10Z- handleDepth()-Depth_Keller msg: 1613172670583 epoch sec; seqNo:4472; depth 9.74 2021-02-12T23:31:10Z- handleDvl() - RDI_Pathfinder msg: 1613172670.60 epoch sec; seqNo:2190 2021-02-12T23:31:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.40 , 96.16 , 75.19 , 113.73 2021-02-12T23:31:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , -0.00 2021-02-12T23:31:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172670.62 epoch sec; seqNo:4472; 4057179.03 north; 591088.06 east 2021-02-12T23:31:10Z- AHRS_M2 msg: 1613172670.98 epoch sec; seqNo:4428 2021-02-12T23:31:10Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.10 psi 2021-02-12T23:31:10Z- handleDepth()-Depth_Keller msg: 1613172670984 epoch sec; seqNo:4473; depth 9.86 2021-02-12T23:31:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172671.00 epoch sec; seqNo:4473; 4057179.41 north; 591088.05 east 2021-02-12T23:31:11Z- AHRS_M2 msg: 1613172671.39 epoch sec; seqNo:4429 2021-02-12T23:31:11Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.11 psi 2021-02-12T23:31:11Z- handleDepth()-Depth_Keller msg: 1613172671395 epoch sec; seqNo:4474; depth 9.99 2021-02-12T23:31:11Z- handleDvl() - RDI_Pathfinder msg: 1613172671.41 epoch sec; seqNo:2191 2021-02-12T23:31:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.11 , 96.23 , 76.42 , 113.80 2021-02-12T23:31:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.04 2021-02-12T23:31:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172671.42 epoch sec; seqNo:4474; 4057179.41 north; 591088.05 east 2021-02-12T23:31:11Z- AHRS_M2 msg: 1613172671.79 epoch sec; seqNo:4430 2021-02-12T23:31:11Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.12 psi 2021-02-12T23:31:11Z- handleDepth()-Depth_Keller msg: 1613172671792 epoch sec; seqNo:4475; depth 10.11 2021-02-12T23:31:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:31:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:31:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:31:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:31:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:31:11Z- handleDvl() - RDI_Pathfinder msg: 1613172671.79 epoch sec; seqNo:2192 2021-02-12T23:31:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:31:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.04 2021-02-12T23:31:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172671.82 epoch sec; seqNo:4475; 4057179.80 north; 591089.27 east 2021-02-12T23:31:12Z- AHRS_M2 msg: 1613172672.19 epoch sec; seqNo:4431 2021-02-12T23:31:12Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.14 psi 2021-02-12T23:31:12Z- handleDepth()-Depth_Keller msg: 1613172672201 epoch sec; seqNo:4476; depth 10.23 2021-02-12T23:31:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172672.22 epoch sec; seqNo:4476; 4057180.18 north; 591089.26 east 2021-02-12T23:31:12Z- AHRS_M2 msg: 1613172672.60 epoch sec; seqNo:4432 2021-02-12T23:31:12Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.15 psi 2021-02-12T23:31:12Z- handleDepth()-Depth_Keller msg: 1613172672613 epoch sec; seqNo:4477; depth 10.35 2021-02-12T23:31:12Z- handleDvl() - RDI_Pathfinder msg: 1613172672.64 epoch sec; seqNo:2193 2021-02-12T23:31:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.76 , 96.23 , 76.42 , 115.15 2021-02-12T23:31:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , -0.04 2021-02-12T23:31:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172672.66 epoch sec; seqNo:4477; 4057180.56 north; 591089.26 east 2021-02-12T23:31:13Z- AHRS_M2 msg: 1613172673.01 epoch sec; seqNo:4433 2021-02-12T23:31:13Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-12T23:31:13Z- handleDepth()-Depth_Keller msg: 1613172673011 epoch sec; seqNo:4478; depth 10.48 2021-02-12T23:31:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:31:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:31:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:31:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:31:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:31:13Z- handleDvl() - RDI_Pathfinder msg: 1613172673.01 epoch sec; seqNo:2194 2021-02-12T23:31:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:31:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , -0.04 2021-02-12T23:31:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172673.03 epoch sec; seqNo:4478; 4057180.56 north; 591089.26 east 2021-02-12T23:31:13Z- AHRS_M2 msg: 1613172673.41 epoch sec; seqNo:4434 2021-02-12T23:31:13Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-12T23:31:13Z- handleDepth()-Depth_Keller msg: 1613172673417 epoch sec; seqNo:4479; depth 10.61 2021-02-12T23:31:13Z- handleDvl() - RDI_Pathfinder msg: 1613172673.44 epoch sec; seqNo:2195 2021-02-12T23:31:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 100.28 , 75.60 , 112.45 2021-02-12T23:31:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.06 , 0.01 2021-02-12T23:31:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172673.45 epoch sec; seqNo:4479; 4057180.94 north; 591089.26 east 2021-02-12T23:31:13Z- LcmTrn::handleMessages() - handling 46 messages took 0.72 ms and 132 calls... 2021-02-12T23:31:13Z- waiting for 1613172671.56, time is 1613172673.45 2021-02-12T23:31:13Z- LcmTrn::updateTrn() equating pose 1613172673.45 and meas times 1613172673.45 2021-02-12T23:31:13Z- LcmTrn::updateTrn() >>>> pose time 1613172673.45, meas time 1613172673.45 <<<< 2021-02-12T23:31:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172673.45 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:31:13Z- TerrainNav::motionUpdate() - currEstimate = (4057181, 591089.26, 10.61) 2021-02-12T23:31:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:31:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:31:13Z- TNavBF::Projecting Measurements 2021-02-12T23:31:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:31:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:31:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:31:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:31:13Z- TNavFilter::4 good beams. 2021-02-12T23:31:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:31:13Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:31:13Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:31:13Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:31:13Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:31:13Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:31:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:31:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:31:13Z- TerrainMapOctree: (vehN, vehE ) = (4057180.94, 591089.26). 2021-02-12T23:31:13Z- TerrainMapOctree: Min Distance = 5152.43. 2021-02-12T23:31:13Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:31:13Z- There is no specific good beam that all particles have in common. 2021-02-12T23:31:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:31:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.56 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 11.67 2021-02-12T23:31:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:31:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 16.09 2021-02-12T23:31:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:31:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 9.14 2021-02-12T23:31:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:31:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 20.23 2021-02-12T23:31:13Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:31:13Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:31:13Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:31:13Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:31:13Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:31:13Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:31:13Z- Filter 0 No beams used in subcloud update 2021-02-12T23:31:13Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:31:13Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:31:13Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 758.3 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:31:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172673.45 sec. 2021-02-12T23:31:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:31:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:31:13Z- variances([0], [2], and psi_berg) = 758.325747 833.041209 0.000000 2021-02-12T23:31:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:31:13Z- LcmTrn::publishEstimates() - MLE : 4057164.44 591040.76 10.61 2021-02-12T23:31:13Z- LcmTrn::publishEstimates() - MMSE : 4057180.94 591089.26 10.61 2021-02-12T23:31:13Z- LcmTrn::publishEstimates() - COVAR: 758.33 833.04 1.10 0.00 2021-02-12T23:31:13Z- AHRS_M2 msg: 1613172673.81 epoch sec; seqNo:4435 2021-02-12T23:31:13Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.19 psi 2021-02-12T23:31:13Z- handleDepth()-Depth_Keller msg: 1613172673812 epoch sec; seqNo:4480; depth 10.74 2021-02-12T23:31:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172673.82 epoch sec; seqNo:4480; 4057181.32 north; 591089.25 east 2021-02-12T23:31:14Z- AHRS_M2 msg: 1613172674.21 epoch sec; seqNo:4436 2021-02-12T23:31:14Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.19 psi 2021-02-12T23:31:14Z- handleDepth()-Depth_Keller msg: 1613172674217 epoch sec; seqNo:4481; depth 10.84 2021-02-12T23:31:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:31:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:31:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:31:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:31:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:31:14Z- handleDvl() - RDI_Pathfinder msg: 1613172674.22 epoch sec; seqNo:2196 2021-02-12T23:31:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:31:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.06 , 0.01 2021-02-12T23:31:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172674.23 epoch sec; seqNo:4481; 4057181.32 north; 591089.25 east 2021-02-12T23:31:14Z- AHRS_M2 msg: 1613172674.62 epoch sec; seqNo:4437 2021-02-12T23:31:14Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-12T23:31:14Z- handleDepth()-Depth_Keller msg: 1613172674637 epoch sec; seqNo:4482; depth 10.98 2021-02-12T23:31:14Z- handleDvl() - RDI_Pathfinder msg: 1613172674.65 epoch sec; seqNo:2197 2021-02-12T23:31:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 101.63 , 75.60 , 113.80 2021-02-12T23:31:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , 0.06 2021-02-12T23:31:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172674.66 epoch sec; seqNo:4482; 4057181.71 north; 591090.47 east 2021-02-12T23:31:15Z- AHRS_M2 msg: 1613172675.02 epoch sec; seqNo:4438 2021-02-12T23:31:15Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-12T23:31:15Z- handleDepth()-Depth_Keller msg: 1613172675024 epoch sec; seqNo:4483; depth 11.11 2021-02-12T23:31:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172675.04 epoch sec; seqNo:4483; 4057182.09 north; 591090.47 east 2021-02-12T23:31:15Z- AHRS_M2 msg: 1613172675.45 epoch sec; seqNo:4439 2021-02-12T23:31:15Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-12T23:31:15Z- handleDepth()-Depth_Keller msg: 1613172675452 epoch sec; seqNo:4484; depth 11.24 2021-02-12T23:31:15Z- handleDvl() - RDI_Pathfinder msg: 1613172675.47 epoch sec; seqNo:2198 2021-02-12T23:31:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 101.57 , 76.42 , 113.73 2021-02-12T23:31:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.04 , 0.07 2021-02-12T23:31:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172675.48 epoch sec; seqNo:4484; 4057182.09 north; 591090.47 east 2021-02-12T23:31:15Z- AHRS_M2 msg: 1613172675.83 epoch sec; seqNo:4440 2021-02-12T23:31:15Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-12T23:31:15Z- handleDepth()-Depth_Keller msg: 1613172675832 epoch sec; seqNo:4485; depth 11.38 2021-02-12T23:31:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172675.85 epoch sec; seqNo:4485; 4057182.47 north; 591090.46 east 2021-02-12T23:31:16Z- AHRS_M2 msg: 1613172676.23 epoch sec; seqNo:4441 2021-02-12T23:31:16Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-12T23:31:16Z- handleDepth()-Depth_Keller msg: 1613172676241 epoch sec; seqNo:4486; depth 11.49 2021-02-12T23:31:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172676.25 epoch sec; seqNo:4486; 4057182.84 north; 591090.46 east 2021-02-12T23:31:16Z- AHRS_M2 msg: 1613172676.65 epoch sec; seqNo:4442 2021-02-12T23:31:16Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-12T23:31:16Z- handleDepth()-Depth_Keller msg: 1613172676651 epoch sec; seqNo:4487; depth 11.62 2021-02-12T23:31:16Z- handleDvl() - RDI_Pathfinder msg: 1613172676.66 epoch sec; seqNo:2199 2021-02-12T23:31:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 96.16 , 76.42 , 115.09 2021-02-12T23:31:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.10 , 0.06 2021-02-12T23:31:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172676.67 epoch sec; seqNo:4487; 4057182.84 north; 591090.46 east 2021-02-12T23:31:17Z- AHRS_M2 msg: 1613172677.04 epoch sec; seqNo:4443 2021-02-12T23:31:17Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.19 psi 2021-02-12T23:31:17Z- handleDepth()-Depth_Keller msg: 1613172677046 epoch sec; seqNo:4488; depth 11.73 2021-02-12T23:31:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172677.06 epoch sec; seqNo:4488; 4057183.24 north; 591091.67 east 2021-02-12T23:31:17Z- AHRS_M2 msg: 1613172677.44 epoch sec; seqNo:4444 2021-02-12T23:31:17Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-12T23:31:17Z- handleDepth()-Depth_Keller msg: 1613172677449 epoch sec; seqNo:4489; depth 11.86 2021-02-12T23:31:17Z- handleDvl() - RDI_Pathfinder msg: 1613172677.47 epoch sec; seqNo:2200 2021-02-12T23:31:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 96.29 , 75.19 , 112.52 2021-02-12T23:31:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.15 , -0.00 2021-02-12T23:31:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172677.48 epoch sec; seqNo:4489; 4057183.62 north; 591091.67 east 2021-02-12T23:31:17Z- AHRS_M2 msg: 1613172677.85 epoch sec; seqNo:4445 2021-02-12T23:31:17Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.21 psi 2021-02-12T23:31:17Z- handleDepth()-Depth_Keller msg: 1613172677852 epoch sec; seqNo:4490; depth 11.99 2021-02-12T23:31:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:31:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:31:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:31:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:31:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:31:17Z- handleDvl() - RDI_Pathfinder msg: 1613172677.85 epoch sec; seqNo:2201 2021-02-12T23:31:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:31:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.15 , -0.00 2021-02-12T23:31:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172677.87 epoch sec; seqNo:4490; 4057183.62 north; 591091.67 east 2021-02-12T23:31:18Z- AHRS_M2 msg: 1613172678.25 epoch sec; seqNo:4446 2021-02-12T23:31:18Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.21 psi 2021-02-12T23:31:18Z- handleDepth()-Depth_Keller msg: 1613172678256 epoch sec; seqNo:4491; depth 12.10 2021-02-12T23:31:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172678.27 epoch sec; seqNo:4491; 4057183.99 north; 591091.67 east 2021-02-12T23:31:18Z- AHRS_M2 msg: 1613172678.66 epoch sec; seqNo:4447 2021-02-12T23:31:18Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.22 psi 2021-02-12T23:31:18Z- handleDepth()-Depth_Keller msg: 1613172678669 epoch sec; seqNo:4492; depth 12.24 2021-02-12T23:31:18Z- handleDvl() - RDI_Pathfinder msg: 1613172678.68 epoch sec; seqNo:2202 2021-02-12T23:31:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.83 , 96.43 , 75.60 , 112.25 2021-02-12T23:31:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.14 , -0.12 2021-02-12T23:31:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172678.69 epoch sec; seqNo:4492; 4057184.37 north; 591091.66 east 2021-02-12T23:31:18Z- LcmTrn::handleMessages() - handling 46 messages took 0.70 ms and 132 calls... 2021-02-12T23:31:18Z- waiting for 1613172676.45, time is 1613172678.69 2021-02-12T23:31:18Z- LcmTrn::updateTrn() equating pose 1613172678.69 and meas times 1613172678.69 2021-02-12T23:31:18Z- LcmTrn::updateTrn() >>>> pose time 1613172678.69, meas time 1613172678.69 <<<< 2021-02-12T23:31:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172678.69 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:31:18Z- TerrainNav::motionUpdate() - currEstimate = (4057184, 591091.66, 12.24) 2021-02-12T23:31:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:31:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:31:18Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172678.69 sec.; No relevant navigation data available 2021-02-12T23:31:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:31:18Z- TNavBF::mmse selected from filter -1 2021-02-12T23:31:18Z- variances([0], [2], and psi_berg) = 758.214071 833.041093 0.000000 2021-02-12T23:31:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:31:18Z- LcmTrn::publishEstimates() - MLE : 4057167.87 591045.16 12.24 2021-02-12T23:31:18Z- LcmTrn::publishEstimates() - MMSE : 4057184.37 591091.66 12.24 2021-02-12T23:31:18Z- LcmTrn::publishEstimates() - COVAR: 758.21 833.04 1.31 0.00 2021-02-12T23:31:19Z- AHRS_M2 msg: 1613172679.06 epoch sec; seqNo:4448 2021-02-12T23:31:19Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.22 psi 2021-02-12T23:31:19Z- handleDepth()-Depth_Keller msg: 1613172679069 epoch sec; seqNo:4493; depth 12.38 2021-02-12T23:31:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:31:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:31:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:31:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:31:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:31:19Z- handleDvl() - RDI_Pathfinder msg: 1613172679.07 epoch sec; seqNo:2203 2021-02-12T23:31:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:31:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.14 , -0.12 2021-02-12T23:31:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172679.10 epoch sec; seqNo:4493; 4057184.76 north; 591092.88 east 2021-02-12T23:31:19Z- AHRS_M2 msg: 1613172679.46 epoch sec; seqNo:4449 2021-02-12T23:31:19Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.22 psi 2021-02-12T23:31:19Z- handleDepth()-Depth_Keller msg: 1613172679468 epoch sec; seqNo:4494; depth 12.49 2021-02-12T23:31:19Z- handleDvl() - RDI_Pathfinder msg: 1613172679.48 epoch sec; seqNo:2204 2021-02-12T23:31:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.69 , 106.16 , 74.37 , 111.03 2021-02-12T23:31:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , -0.20 2021-02-12T23:31:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172679.49 epoch sec; seqNo:4494; 4057184.76 north; 591092.88 east 2021-02-12T23:31:19Z- AHRS_M2 msg: 1613172679.87 epoch sec; seqNo:4450 2021-02-12T23:31:19Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.21 psi 2021-02-12T23:31:19Z- handleDepth()-Depth_Keller msg: 1613172679874 epoch sec; seqNo:4495; depth 12.62 2021-02-12T23:31:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172679.88 epoch sec; seqNo:4495; 4057185.14 north; 591092.88 east 2021-02-12T23:31:20Z- AHRS_M2 msg: 1613172680.27 epoch sec; seqNo:4451 2021-02-12T23:31:20Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-12T23:31:20Z- handleDepth()-Depth_Keller msg: 1613172680276 epoch sec; seqNo:4496; depth 12.74 2021-02-12T23:31:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:31:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:31:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:31:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:31:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:31:20Z- handleDvl() - RDI_Pathfinder msg: 1613172680.28 epoch sec; seqNo:2205 2021-02-12T23:31:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:31:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , -0.20 2021-02-12T23:31:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172680.29 epoch sec; seqNo:4496; 4057185.52 north; 591092.87 east 2021-02-12T23:31:20Z- AHRS_M2 msg: 1613172680.68 epoch sec; seqNo:4452 2021-02-12T23:31:20Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-12T23:31:20Z- handleDepth()-Depth_Keller msg: 1613172680680 epoch sec; seqNo:4497; depth 12.86 2021-02-12T23:31:20Z- handleDvl() - RDI_Pathfinder msg: 1613172680.69 epoch sec; seqNo:2206 2021-02-12T23:31:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.42 , 103.05 , 75.60 , 111.17 2021-02-12T23:31:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.16 , -0.19 2021-02-12T23:31:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172680.71 epoch sec; seqNo:4497; 4057185.90 north; 591092.87 east 2021-02-12T23:31:21Z- AHRS_M2 msg: 1613172681.08 epoch sec; seqNo:4453 2021-02-12T23:31:21Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-12T23:31:21Z- handleDepth()-Depth_Keller msg: 1613172681084 epoch sec; seqNo:4498; depth 12.99 2021-02-12T23:31:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172681.10 epoch sec; seqNo:4498; 4057186.28 north; 591092.86 east 2021-02-12T23:31:21Z- AHRS_M2 msg: 1613172681.49 epoch sec; seqNo:4454 2021-02-12T23:31:21Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.19 psi 2021-02-12T23:31:21Z- handleDepth()-Depth_Keller msg: 1613172681494 epoch sec; seqNo:4499; depth 13.09 2021-02-12T23:31:21Z- handleDvl() - RDI_Pathfinder msg: 1613172681.51 epoch sec; seqNo:2207 2021-02-12T23:31:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.07 , 96.29 , 73.96 , 109.81 2021-02-12T23:31:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.23 , -0.13 2021-02-12T23:31:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172681.52 epoch sec; seqNo:4499; 4057186.66 north; 591092.86 east 2021-02-12T23:31:21Z- AHRS_M2 msg: 1613172681.89 epoch sec; seqNo:4455 2021-02-12T23:31:21Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.19 psi 2021-02-12T23:31:21Z- handleDepth()-Depth_Keller msg: 1613172681897 epoch sec; seqNo:4500; depth 13.23 2021-02-12T23:31:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172681.92 epoch sec; seqNo:4500; 4057186.66 north; 591092.86 east 2021-02-12T23:31:22Z- AHRS_M2 msg: 1613172682.29 epoch sec; seqNo:4456 2021-02-12T23:31:22Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-12T23:31:22Z- handleDepth()-Depth_Keller msg: 1613172682296 epoch sec; seqNo:4501; depth 13.36 2021-02-12T23:31:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172682.30 epoch sec; seqNo:4501; 4057187.05 north; 591094.08 east 2021-02-12T23:31:22Z- AHRS_M2 msg: 1613172682.70 epoch sec; seqNo:4457 2021-02-12T23:31:22Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.18 psi 2021-02-12T23:31:22Z- handleDepth()-Depth_Keller msg: 1613172682702 epoch sec; seqNo:4502; depth 13.48 2021-02-12T23:31:22Z- handleDvl() - RDI_Pathfinder msg: 1613172682.71 epoch sec; seqNo:2208 2021-02-12T23:31:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.75 , 96.56 , 73.96 , 109.95 2021-02-12T23:31:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.23 , -0.05 2021-02-12T23:31:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172682.72 epoch sec; seqNo:4502; 4057187.43 north; 591094.07 east 2021-02-12T23:31:23Z- AHRS_M2 msg: 1613172683.10 epoch sec; seqNo:4458 2021-02-12T23:31:23Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-12T23:31:23Z- handleDepth()-Depth_Keller msg: 1613172683104 epoch sec; seqNo:4503; depth 13.62 2021-02-12T23:31:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172683.12 epoch sec; seqNo:4503; 4057187.81 north; 591094.07 east 2021-02-12T23:31:23Z- AHRS_M2 msg: 1613172683.50 epoch sec; seqNo:4459 2021-02-12T23:31:23Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-12T23:31:23Z- handleDepth()-Depth_Keller msg: 1613172683508 epoch sec; seqNo:4504; depth 13.73 2021-02-12T23:31:23Z- handleDvl() - RDI_Pathfinder msg: 1613172683.52 epoch sec; seqNo:2209 2021-02-12T23:31:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 96.50 , 71.92 , 109.88 2021-02-12T23:31:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.20 , 0.00 2021-02-12T23:31:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172683.53 epoch sec; seqNo:4504; 4057188.19 north; 591094.06 east 2021-02-12T23:31:23Z- LcmTrn::handleMessages() - handling 43 messages took 0.62 ms and 131 calls... 2021-02-12T23:31:23Z- waiting for 1613172681.69, time is 1613172683.53 2021-02-12T23:31:23Z- LcmTrn::updateTrn() equating pose 1613172683.53 and meas times 1613172683.53 2021-02-12T23:31:23Z- LcmTrn::updateTrn() >>>> pose time 1613172683.53, meas time 1613172683.53 <<<< 2021-02-12T23:31:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172683.53 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:31:23Z- TerrainNav::motionUpdate() - currEstimate = (4057188, 591094.06, 13.73) 2021-02-12T23:31:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:31:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:31:23Z- TNavBF::Projecting Measurements 2021-02-12T23:31:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:31:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:31:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:31:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:31:23Z- TNavFilter::4 good beams. 2021-02-12T23:31:23Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:31:23Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:31:23Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:31:23Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:31:23Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:31:23Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:31:23Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:31:23Z- TNavBF::mmse selected from filter -1 2021-02-12T23:31:23Z- TerrainMapOctree: (vehN, vehE ) = (4057188.19, 591094.06). 2021-02-12T23:31:23Z- TerrainMapOctree: Min Distance = 5143.76. 2021-02-12T23:31:23Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:31:23Z- There is no specific good beam that all particles have in common. 2021-02-12T23:31:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:31:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.54 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 10.41 2021-02-12T23:31:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:31:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 14.90 2021-02-12T23:31:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:31:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 8.28 2021-02-12T23:31:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:31:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 19.32 2021-02-12T23:31:23Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:31:23Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:31:23Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:31:23Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:31:23Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:31:23Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:31:23Z- Filter 0 No beams used in subcloud update 2021-02-12T23:31:23Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:31:23Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:31:23Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 758.1 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:31:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172683.53 sec. 2021-02-12T23:31:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:31:23Z- TNavBF::mmse selected from filter -1 2021-02-12T23:31:23Z- variances([0], [2], and psi_berg) = 758.098480 833.040974 0.000000 2021-02-12T23:31:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:31:23Z- LcmTrn::publishEstimates() - MLE : 4057171.69 591047.56 13.73 2021-02-12T23:31:23Z- LcmTrn::publishEstimates() - MMSE : 4057188.19 591094.06 13.73 2021-02-12T23:31:23Z- LcmTrn::publishEstimates() - COVAR: 758.10 833.04 1.07 0.00 2021-02-12T23:31:23Z- AHRS_M2 msg: 1613172683.91 epoch sec; seqNo:4460 2021-02-12T23:31:23Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-12T23:31:23Z- handleDepth()-Depth_Keller msg: 1613172683917 epoch sec; seqNo:4505; depth 13.85 2021-02-12T23:31:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:23Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:31:23Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:31:23Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:31:23Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:31:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:31:23Z- handleDvl() - RDI_Pathfinder msg: 1613172683.92 epoch sec; seqNo:2210 2021-02-12T23:31:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:31:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.20 , 0.00 2021-02-12T23:31:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172683.93 epoch sec; seqNo:4505; 4057188.57 north; 591094.06 east 2021-02-12T23:31:24Z- AHRS_M2 msg: 1613172684.31 epoch sec; seqNo:4461 2021-02-12T23:31:24Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-12T23:31:24Z- handleDepth()-Depth_Keller msg: 1613172684316 epoch sec; seqNo:4506; depth 13.97 2021-02-12T23:31:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172684.33 epoch sec; seqNo:4506; 4057188.57 north; 591094.06 east 2021-02-12T23:31:24Z- AHRS_M2 msg: 1613172684.72 epoch sec; seqNo:4462 2021-02-12T23:31:24Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-12T23:31:24Z- handleDepth()-Depth_Keller msg: 1613172684726 epoch sec; seqNo:4507; depth 14.10 2021-02-12T23:31:24Z- handleDvl() - RDI_Pathfinder msg: 1613172684.75 epoch sec; seqNo:2211 2021-02-12T23:31:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.75 , 95.35 , 72.33 , 107.51 2021-02-12T23:31:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.16 , 0.06 2021-02-12T23:31:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172684.76 epoch sec; seqNo:4507; 4057188.94 north; 591094.06 east 2021-02-12T23:31:25Z- AHRS_M2 msg: 1613172685.12 epoch sec; seqNo:4463 2021-02-12T23:31:25Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.18 psi 2021-02-12T23:31:25Z- handleDepth()-Depth_Keller msg: 1613172685129 epoch sec; seqNo:4508; depth 14.25 2021-02-12T23:31:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:31:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:31:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:31:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:31:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:31:25Z- handleDvl() - RDI_Pathfinder msg: 1613172685.13 epoch sec; seqNo:2212 2021-02-12T23:31:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:31:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.16 , 0.06 2021-02-12T23:31:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172685.15 epoch sec; seqNo:4508; 4057189.32 north; 591094.05 east 2021-02-12T23:31:25Z- AHRS_M2 msg: 1613172685.52 epoch sec; seqNo:4464 2021-02-12T23:31:25Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.18 psi 2021-02-12T23:31:25Z- handleDepth()-Depth_Keller msg: 1613172685530 epoch sec; seqNo:4509; depth 14.36 2021-02-12T23:31:25Z- handleDvl() - RDI_Pathfinder msg: 1613172685.54 epoch sec; seqNo:2213 2021-02-12T23:31:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.69 , 96.63 , 73.56 , 106.37 2021-02-12T23:31:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.10 , 0.10 2021-02-12T23:31:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172685.55 epoch sec; seqNo:4509; 4057189.70 north; 591094.05 east 2021-02-12T23:31:25Z- AHRS_M2 msg: 1613172685.93 epoch sec; seqNo:4465 2021-02-12T23:31:25Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.18 psi 2021-02-12T23:31:25Z- handleDepth()-Depth_Keller msg: 1613172685937 epoch sec; seqNo:4510; depth 14.48 2021-02-12T23:31:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172685.96 epoch sec; seqNo:4510; 4057189.70 north; 591094.05 east 2021-02-12T23:31:26Z- AHRS_M2 msg: 1613172686.34 epoch sec; seqNo:4466 2021-02-12T23:31:26Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.18 psi 2021-02-12T23:31:26Z- handleDepth()-Depth_Keller msg: 1613172686352 epoch sec; seqNo:4511; depth 14.59 2021-02-12T23:31:26Z- handleDvl() - RDI_Pathfinder msg: 1613172686.39 epoch sec; seqNo:2214 2021-02-12T23:31:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.28 , 97.85 , 0.00 , 106.37 2021-02-12T23:31:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.06 , 0.12 2021-02-12T23:31:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172686.41 epoch sec; seqNo:4511; 4057190.09 north; 591095.26 east 2021-02-12T23:31:26Z- AHRS_M2 msg: 1613172686.74 epoch sec; seqNo:4467 2021-02-12T23:31:26Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.19 psi 2021-02-12T23:31:26Z- handleDepth()-Depth_Keller msg: 1613172686742 epoch sec; seqNo:4512; depth 14.71 2021-02-12T23:31:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172686.75 epoch sec; seqNo:4512; 4057190.09 north; 591095.26 east 2021-02-12T23:31:27Z- AHRS_M2 msg: 1613172687.14 epoch sec; seqNo:4468 2021-02-12T23:31:27Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.19 psi 2021-02-12T23:31:27Z- handleDepth()-Depth_Keller msg: 1613172687147 epoch sec; seqNo:4513; depth 14.81 2021-02-12T23:31:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172687.16 epoch sec; seqNo:4513; 4057190.47 north; 591095.26 east 2021-02-12T23:31:27Z- AHRS_M2 msg: 1613172687.54 epoch sec; seqNo:4469 2021-02-12T23:31:27Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.19 psi 2021-02-12T23:31:27Z- handleDepth()-Depth_Keller msg: 1613172687549 epoch sec; seqNo:4514; depth 14.95 2021-02-12T23:31:27Z- handleDvl() - RDI_Pathfinder msg: 1613172687.56 epoch sec; seqNo:2215 2021-02-12T23:31:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.69 , 96.16 , 73.56 , 106.97 2021-02-12T23:31:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.01 , 0.13 2021-02-12T23:31:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172687.57 epoch sec; seqNo:4514; 4057190.85 north; 591095.26 east 2021-02-12T23:31:27Z- AHRS_M2 msg: 1613172687.95 epoch sec; seqNo:4470 2021-02-12T23:31:27Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.20 psi 2021-02-12T23:31:27Z- handleDepth()-Depth_Keller msg: 1613172687955 epoch sec; seqNo:4515; depth 15.07 2021-02-12T23:31:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172687.97 epoch sec; seqNo:4515; 4057190.85 north; 591095.26 east 2021-02-12T23:31:28Z- AHRS_M2 msg: 1613172688.35 epoch sec; seqNo:4471 2021-02-12T23:31:28Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.20 psi 2021-02-12T23:31:28Z- handleDepth()-Depth_Keller msg: 1613172688361 epoch sec; seqNo:4516; depth 15.18 2021-02-12T23:31:28Z- handleDvl() - RDI_Pathfinder msg: 1613172688.38 epoch sec; seqNo:2216 2021-02-12T23:31:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.28 , 96.63 , 73.15 , 107.58 2021-02-12T23:31:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.07 , 0.12 2021-02-12T23:31:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172688.39 epoch sec; seqNo:4516; 4057191.23 north; 591095.25 east 2021-02-12T23:31:28Z- AHRS_M2 msg: 1613172688.76 epoch sec; seqNo:4472 2021-02-12T23:31:28Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.20 psi 2021-02-12T23:31:28Z- handleDepth()-Depth_Keller msg: 1613172688760 epoch sec; seqNo:4517; depth 15.29 2021-02-12T23:31:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:31:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:31:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:31:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:31:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:31:28Z- handleDvl() - RDI_Pathfinder msg: 1613172688.77 epoch sec; seqNo:2217 2021-02-12T23:31:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:31:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.07 , 0.12 2021-02-12T23:31:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172688.80 epoch sec; seqNo:4517; 4057191.61 north; 591095.25 east 2021-02-12T23:31:28Z- LcmTrn::handleMessages() - handling 47 messages took 0.73 ms and 133 calls... 2021-02-12T23:31:28Z- waiting for 1613172686.53, time is 1613172688.80 2021-02-12T23:31:28Z- LcmTrn::updateTrn() equating pose 1613172688.80 and meas times 1613172688.80 2021-02-12T23:31:28Z- LcmTrn::updateTrn() >>>> pose time 1613172688.80, meas time 1613172688.80 <<<< 2021-02-12T23:31:28Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172688.80, ping # 0. 2021-02-12T23:31:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172688.80 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:31:28Z- TerrainNav::motionUpdate() - currEstimate = (4057192, 591095.25, 15.29) 2021-02-12T23:31:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:31:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:31:28Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:31:28Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:31:28Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172688.80 sec.; No relevant navigation data available 2021-02-12T23:31:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:31:28Z- TNavBF::mmse selected from filter -1 2021-02-12T23:31:28Z- variances([0], [2], and psi_berg) = 757.995034 833.040867 0.000000 2021-02-12T23:31:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:31:28Z- LcmTrn::publishEstimates() - MLE : 4057175.11 591048.75 15.29 2021-02-12T23:31:28Z- LcmTrn::publishEstimates() - MMSE : 4057191.61 591095.25 15.29 2021-02-12T23:31:28Z- LcmTrn::publishEstimates() - COVAR: 758.00 833.04 1.25 0.00 2021-02-12T23:31:29Z- AHRS_M2 msg: 1613172689.16 epoch sec; seqNo:4473 2021-02-12T23:31:29Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.20 psi 2021-02-12T23:31:29Z- handleDepth()-Depth_Keller msg: 1613172689169 epoch sec; seqNo:4518; depth 15.42 2021-02-12T23:31:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172689.18 epoch sec; seqNo:4518; 4057191.62 north; 591096.47 east 2021-02-12T23:31:29Z- AHRS_M2 msg: 1613172689.57 epoch sec; seqNo:4474 2021-02-12T23:31:29Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.20 psi 2021-02-12T23:31:29Z- handleDepth()-Depth_Keller msg: 1613172689576 epoch sec; seqNo:4519; depth 15.54 2021-02-12T23:31:29Z- handleDvl() - RDI_Pathfinder msg: 1613172689.59 epoch sec; seqNo:2218 2021-02-12T23:31:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.87 , 96.63 , 71.92 , 107.58 2021-02-12T23:31:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.11 , 0.04 2021-02-12T23:31:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172689.60 epoch sec; seqNo:4519; 4057192.00 north; 591096.47 east 2021-02-12T23:31:29Z- AHRS_M2 msg: 1613172689.97 epoch sec; seqNo:4475 2021-02-12T23:31:29Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.19 psi 2021-02-12T23:31:29Z- handleDepth()-Depth_Keller msg: 1613172689978 epoch sec; seqNo:4520; depth 15.66 2021-02-12T23:31:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:31:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:31:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:31:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:31:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:31:29Z- handleDvl() - RDI_Pathfinder msg: 1613172689.98 epoch sec; seqNo:2219 2021-02-12T23:31:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:31:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.11 , 0.04 2021-02-12T23:31:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172690.00 epoch sec; seqNo:4520; 4057192.38 north; 591096.46 east 2021-02-12T23:31:30Z- AHRS_M2 msg: 1613172690.38 epoch sec; seqNo:4476 2021-02-12T23:31:30Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.19 psi 2021-02-12T23:31:30Z- handleDepth()-Depth_Keller msg: 1613172690385 epoch sec; seqNo:4521; depth 15.77 2021-02-12T23:31:30Z- handleDvl() - RDI_Pathfinder msg: 1613172690.40 epoch sec; seqNo:2220 2021-02-12T23:31:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.05 , 96.63 , 73.15 , 107.58 2021-02-12T23:31:30Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.11 , -0.07 2021-02-12T23:31:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172690.41 epoch sec; seqNo:4521; 4057192.76 north; 591096.46 east 2021-02-12T23:31:30Z- AHRS_M2 msg: 1613172690.78 epoch sec; seqNo:4477 2021-02-12T23:31:30Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.18 psi 2021-02-12T23:31:30Z- handleDepth()-Depth_Keller msg: 1613172690780 epoch sec; seqNo:4522; depth 15.91 2021-02-12T23:31:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172690.79 epoch sec; seqNo:4522; 4057192.76 north; 591096.46 east 2021-02-12T23:31:31Z- AHRS_M2 msg: 1613172691.18 epoch sec; seqNo:4478 2021-02-12T23:31:31Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.18 psi 2021-02-12T23:31:31Z- handleDepth()-Depth_Keller msg: 1613172691190 epoch sec; seqNo:4523; depth 16.02 2021-02-12T23:31:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:31:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:31:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:31:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:31:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:31:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:31:31Z- handleDvl() - RDI_Pathfinder msg: 1613172691.19 epoch sec; seqNo:2221 2021-02-12T23:31:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:31:31Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.11 , -0.07 2021-02-12T23:31:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172691.20 epoch sec; seqNo:4523; 4057193.15 north; 591097.67 east 2021-02-12T23:31:31Z- AHRS_M2 msg: 1613172691.58 epoch sec; seqNo:4479 2021-02-12T23:31:31Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.17 psi 2021-02-12T23:31:31Z- handleDepth()-Depth_Keller msg: 1613172691588 epoch sec; seqNo:4524; depth 16.14 2021-02-12T23:31:31Z- handleDvl() - RDI_Pathfinder msg: 1613172691.60 epoch sec; seqNo:2222 2021-02-12T23:31:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.64 , 96.63 , 72.33 , 106.37 2021-02-12T23:31:31Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.01 , -0.17 2021-02-12T23:31:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172691.62 epoch sec; seqNo:4524; 4057193.53 north; 591097.67 east 2021-02-12T23:31:31Z- AHRS_M2 msg: 1613172691.99 epoch sec; seqNo:4480 2021-02-12T23:31:31Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.17 psi 2021-02-12T23:31:31Z- handleDepth()-Depth_Keller msg: 1613172691997 epoch sec; seqNo:4525; depth 16.26 2021-02-12T23:31:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172692.01 epoch sec; seqNo:4525; 4057193.91 north; 591097.67 east 2021-02-12T23:31:32Z- AHRS_M2 msg: 1613172692.41 epoch sec; seqNo:4481 2021-02-12T23:31:32Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.17 psi 2021-02-12T23:31:32Z- handleDepth()-Depth_Keller msg: 1613172692413 epoch sec; seqNo:4526; depth 16.41 2021-02-12T23:31:32Z- handleDvl() - RDI_Pathfinder msg: 1613172692.42 epoch sec; seqNo:2223 2021-02-12T23:31:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 93.05 , 70.29 , 106.43 2021-02-12T23:31:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.10 , -0.19 2021-02-12T23:31:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172692.43 epoch sec; seqNo:4526; 4057194.29 north; 591097.66 east 2021-02-12T23:31:32Z- AHRS_M2 msg: 1613172692.80 epoch sec; seqNo:4482 2021-02-12T23:31:32Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.16 psi 2021-02-12T23:31:32Z- handleDepth()-Depth_Keller msg: 1613172692800 epoch sec; seqNo:4527; depth 16.51 2021-02-12T23:31:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172692.82 epoch sec; seqNo:4527; 4057194.67 north; 591097.66 east 2021-02-12T23:31:33Z- AHRS_M2 msg: 1613172693.20 epoch sec; seqNo:4483 2021-02-12T23:31:33Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.16 psi 2021-02-12T23:31:33Z- handleDepth()-Depth_Keller msg: 1613172693209 epoch sec; seqNo:4528; depth 16.63 2021-02-12T23:31:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172693.22 epoch sec; seqNo:4528; 4057195.05 north; 591097.65 east 2021-02-12T23:31:33Z- AHRS_M2 msg: 1613172693.60 epoch sec; seqNo:4484 2021-02-12T23:31:33Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.16 psi 2021-02-12T23:31:33Z- handleDepth()-Depth_Keller msg: 1613172693608 epoch sec; seqNo:4529; depth 16.76 2021-02-12T23:31:33Z- handleDvl() - RDI_Pathfinder msg: 1613172693.62 epoch sec; seqNo:2224 2021-02-12T23:31:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 94.33 , 71.51 , 96.77 2021-02-12T23:31:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.19 , -0.16 2021-02-12T23:31:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172693.63 epoch sec; seqNo:4529; 4057195.06 north; 591098.88 east 2021-02-12T23:31:33Z- LcmTrn::handleMessages() - handling 43 messages took 0.62 ms and 131 calls... 2021-02-12T23:31:33Z- waiting for 1613172691.80, time is 1613172693.63 2021-02-12T23:31:33Z- LcmTrn::updateTrn() equating pose 1613172693.63 and meas times 1613172693.63 2021-02-12T23:31:33Z- LcmTrn::updateTrn() >>>> pose time 1613172693.63, meas time 1613172693.63 <<<< 2021-02-12T23:31:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172693.63 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:31:33Z- TerrainNav::motionUpdate() - currEstimate = (4057195, 591098.88, 16.76) 2021-02-12T23:31:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:31:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:31:33Z- TNavBF::Projecting Measurements 2021-02-12T23:31:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:31:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:31:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:31:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:31:33Z- TNavFilter::4 good beams. 2021-02-12T23:31:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:31:33Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:31:33Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:31:33Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:31:33Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:31:33Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:31:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:31:33Z- TNavBF::mmse selected from filter -1 2021-02-12T23:31:33Z- TerrainMapOctree: (vehN, vehE ) = (4057195.06, 591098.88). 2021-02-12T23:31:33Z- TerrainMapOctree: Min Distance = 5135.38. 2021-02-12T23:31:33Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:31:33Z- There is no specific good beam that all particles have in common. 2021-02-12T23:31:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:31:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.51 K: 0.119 ExpectedMeasDiff: nan currMeas.covariance: 9.44 2021-02-12T23:31:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:31:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 14.24 2021-02-12T23:31:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:31:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.110 ExpectedMeasDiff: nan currMeas.covariance: 8.18 2021-02-12T23:31:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:31:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 14.98 2021-02-12T23:31:33Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:31:33Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:31:33Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:31:33Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:31:33Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:31:33Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:31:33Z- Filter 0 No beams used in subcloud update 2021-02-12T23:31:33Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:31:33Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:31:33Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 757.9 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:31:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172693.63 sec. 2021-02-12T23:31:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:31:33Z- TNavBF::mmse selected from filter -1 2021-02-12T23:31:33Z- variances([0], [2], and psi_berg) = 757.873611 833.040742 0.000000 2021-02-12T23:31:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:31:33Z- LcmTrn::publishEstimates() - MLE : 4057178.56 591052.38 16.76 2021-02-12T23:31:33Z- LcmTrn::publishEstimates() - MMSE : 4057195.06 591098.88 16.76 2021-02-12T23:31:33Z- LcmTrn::publishEstimates() - COVAR: 757.87 833.04 1.02 0.00 2021-02-12T23:31:34Z- AHRS_M2 msg: 1613172694.01 epoch sec; seqNo:4485 2021-02-12T23:31:34Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.16 psi 2021-02-12T23:31:34Z- handleDepth()-Depth_Keller msg: 1613172694014 epoch sec; seqNo:4530; depth 16.87 2021-02-12T23:31:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172694.03 epoch sec; seqNo:4530; 4057195.44 north; 591098.87 east 2021-02-12T23:31:34Z- AHRS_M2 msg: 1613172694.41 epoch sec; seqNo:4486 2021-02-12T23:31:34Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.16 psi 2021-02-12T23:31:34Z- handleDepth()-Depth_Keller msg: 1613172694416 epoch sec; seqNo:4531; depth 16.99 2021-02-12T23:31:34Z- handleDvl() - RDI_Pathfinder msg: 1613172694.43 epoch sec; seqNo:2225 2021-02-12T23:31:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 93.32 , 69.47 , 103.05 2021-02-12T23:31:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.19 , -0.07 2021-02-12T23:31:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172694.44 epoch sec; seqNo:4531; 4057195.82 north; 591098.87 east 2021-02-12T23:31:34Z- AHRS_M2 msg: 1613172694.82 epoch sec; seqNo:4487 2021-02-12T23:31:34Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.16 psi 2021-02-12T23:31:34Z- handleDepth()-Depth_Keller msg: 1613172694820 epoch sec; seqNo:4532; depth 17.11 2021-02-12T23:31:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172694.83 epoch sec; seqNo:4532; 4057196.19 north; 591098.86 east 2021-02-12T23:31:35Z- AHRS_M2 msg: 1613172695.22 epoch sec; seqNo:4488 2021-02-12T23:31:35Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.16 psi 2021-02-12T23:31:35Z- handleDepth()-Depth_Keller msg: 1613172695229 epoch sec; seqNo:4533; depth 17.24 2021-02-12T23:31:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172695.25 epoch sec; seqNo:4533; 4057196.57 north; 591098.86 east 2021-02-12T23:31:35Z- AHRS_M2 msg: 1613172695.62 epoch sec; seqNo:4489 2021-02-12T23:31:35Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.17 psi 2021-02-12T23:31:35Z- handleDepth()-Depth_Keller msg: 1613172695637 epoch sec; seqNo:4534; depth 17.35 2021-02-12T23:31:35Z- handleDvl() - RDI_Pathfinder msg: 1613172695.65 epoch sec; seqNo:2226 2021-02-12T23:31:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 93.25 , 69.88 , 102.99 2021-02-12T23:31:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.15 , 0.02 2021-02-12T23:31:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172695.66 epoch sec; seqNo:4534; 4057196.57 north; 591098.86 east 2021-02-12T23:31:36Z- AHRS_M2 msg: 1613172696.03 epoch sec; seqNo:4490 2021-02-12T23:31:36Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.17 psi 2021-02-12T23:31:36Z- handleDepth()-Depth_Keller msg: 1613172696034 epoch sec; seqNo:4535; depth 17.48 2021-02-12T23:31:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172696.05 epoch sec; seqNo:4535; 4057196.95 north; 591098.86 east 2021-02-12T23:31:36Z- AHRS_M2 msg: 1613172696.43 epoch sec; seqNo:4491 2021-02-12T23:31:36Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.17 psi 2021-02-12T23:31:36Z- handleDepth()-Depth_Keller msg: 1613172696436 epoch sec; seqNo:4536; depth 17.58 2021-02-12T23:31:36Z- handleDvl() - RDI_Pathfinder msg: 1613172696.45 epoch sec; seqNo:2227 2021-02-12T23:31:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 93.25 , 69.88 , 104.20 2021-02-12T23:31:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.08 , 0.06 2021-02-12T23:31:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172696.47 epoch sec; seqNo:4536; 4057197.33 north; 591098.85 east 2021-02-12T23:31:36Z- AHRS_M2 msg: 1613172696.84 epoch sec; seqNo:4492 2021-02-12T23:31:36Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.18 psi 2021-02-12T23:31:36Z- handleDepth()-Depth_Keller msg: 1613172696840 epoch sec; seqNo:4537; depth 17.69 2021-02-12T23:31:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172696.85 epoch sec; seqNo:4537; 4057197.33 north; 591098.85 east 2021-02-12T23:31:37Z- AHRS_M2 msg: 1613172697.26 epoch sec; seqNo:4493 2021-02-12T23:31:37Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.18 psi 2021-02-12T23:31:37Z- handleDepth()-Depth_Keller msg: 1613172697273 epoch sec; seqNo:4538; depth 17.81 2021-02-12T23:31:37Z- handleDvl() - RDI_Pathfinder msg: 1613172697.28 epoch sec; seqNo:2228 2021-02-12T23:31:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 94.47 , 69.88 , 104.20 2021-02-12T23:31:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , 0.05 2021-02-12T23:31:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172697.30 epoch sec; seqNo:4538; 4057197.72 north; 591100.07 east 2021-02-12T23:31:37Z- AHRS_M2 msg: 1613172697.64 epoch sec; seqNo:4494 2021-02-12T23:31:37Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.18 psi 2021-02-12T23:31:37Z- handleDepth()-Depth_Keller msg: 1613172697648 epoch sec; seqNo:4539; depth 17.94 2021-02-12T23:31:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172697.67 epoch sec; seqNo:4539; 4057198.10 north; 591100.06 east 2021-02-12T23:31:38Z- AHRS_M2 msg: 1613172698.05 epoch sec; seqNo:4495 2021-02-12T23:31:38Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.18 psi 2021-02-12T23:31:38Z- handleDepth()-Depth_Keller msg: 1613172698057 epoch sec; seqNo:4540; depth 18.05 2021-02-12T23:31:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172698.07 epoch sec; seqNo:4540; 4057198.10 north; 591100.06 east 2021-02-12T23:31:38Z- AHRS_M2 msg: 1613172698.45 epoch sec; seqNo:4496 2021-02-12T23:31:38Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.18 psi 2021-02-12T23:31:38Z- handleDepth()-Depth_Keller msg: 1613172698456 epoch sec; seqNo:4541; depth 18.18 2021-02-12T23:31:38Z- handleDvl() - RDI_Pathfinder msg: 1613172698.47 epoch sec; seqNo:2229 2021-02-12T23:31:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.60 , 94.40 , 67.43 , 102.92 2021-02-12T23:31:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , -0.00 2021-02-12T23:31:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172698.48 epoch sec; seqNo:4541; 4057198.48 north; 591100.06 east 2021-02-12T23:31:38Z- AHRS_M2 msg: 1613172698.88 epoch sec; seqNo:4497 2021-02-12T23:31:38Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.18 psi 2021-02-12T23:31:38Z- handleDepth()-Depth_Keller msg: 1613172698889 epoch sec; seqNo:4542; depth 18.30 2021-02-12T23:31:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172698.90 epoch sec; seqNo:4542; 4057198.86 north; 591100.06 east 2021-02-12T23:31:39Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 132 calls... 2021-02-12T23:31:39Z- waiting for 1613172696.63, time is 1613172698.90 2021-02-12T23:31:39Z- LcmTrn::updateTrn() >>>> pose time 1613172698.90, meas time 1613172698.47 <<<< 2021-02-12T23:31:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172698.47 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:31:39Z- TerrainNav::motionUpdate() - currEstimate = (4057199, 591100.06, 18.30) 2021-02-12T23:31:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:31:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:31:39Z- TNavBF::Projecting Measurements 2021-02-12T23:31:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:31:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:31:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:31:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:31:39Z- TNavFilter::4 good beams. 2021-02-12T23:31:39Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:31:39Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:31:39Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:31:39Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:31:39Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:31:39Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:31:39Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:31:39Z- TNavBF::mmse selected from filter -1 2021-02-12T23:31:39Z- TerrainMapOctree: (vehN, vehE ) = (4057198.55, 591099.96). 2021-02-12T23:31:39Z- TerrainMapOctree: Min Distance = 5131.96. 2021-02-12T23:31:39Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:31:39Z- There is no specific good beam that all particles have in common. 2021-02-12T23:31:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:31:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 9.14 2021-02-12T23:31:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:31:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 14.26 2021-02-12T23:31:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:31:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 7.27 2021-02-12T23:31:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:31:39Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 16.95 2021-02-12T23:31:39Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:31:39Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:31:39Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:31:39Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:31:39Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:31:39Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:31:39Z- Filter 0 No beams used in subcloud update 2021-02-12T23:31:39Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:31:39Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:31:39Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 757.8 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:31:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172698.47 sec. 2021-02-12T23:31:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:31:39Z- TNavBF::mmse selected from filter -1 2021-02-12T23:31:39Z- variances([0], [2], and psi_berg) = 757.739101 833.040603 0.000000 2021-02-12T23:31:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:31:39Z- LcmTrn::publishEstimates() - MLE : 4057182.36 591053.56 18.30 2021-02-12T23:31:39Z- LcmTrn::publishEstimates() - MMSE : 4057198.86 591100.06 18.30 2021-02-12T23:31:39Z- LcmTrn::publishEstimates() - COVAR: 757.74 833.04 0.89 0.00 2021-02-12T23:31:39Z- AHRS_M2 msg: 1613172699.28 epoch sec; seqNo:4498 2021-02-12T23:31:39Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.17 psi 2021-02-12T23:31:39Z- handleDepth()-Depth_Keller msg: 1613172699280 epoch sec; seqNo:4543; depth 18.41 2021-02-12T23:31:39Z- handleDvl() - RDI_Pathfinder msg: 1613172699.29 epoch sec; seqNo:2230 2021-02-12T23:31:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.01 , 93.32 , 69.88 , 96.97 2021-02-12T23:31:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , -0.01 2021-02-12T23:31:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172699.30 epoch sec; seqNo:4543; 4057198.86 north; 591100.06 east 2021-02-12T23:31:39Z- AHRS_M2 msg: 1613172699.66 epoch sec; seqNo:4499 2021-02-12T23:31:39Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.17 psi 2021-02-12T23:31:39Z- handleDepth()-Depth_Keller msg: 1613172699668 epoch sec; seqNo:4544; depth 18.54 2021-02-12T23:31:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172699.68 epoch sec; seqNo:4544; 4057199.25 north; 591101.27 east 2021-02-12T23:31:40Z- AHRS_M2 msg: 1613172700.07 epoch sec; seqNo:4500 2021-02-12T23:31:40Z- AHRS_M2 msg: 0.69 phi; -0.32 theta; -0.16 psi 2021-02-12T23:31:40Z- handleDepth()-Depth_Keller msg: 1613172700077 epoch sec; seqNo:4545; depth 18.64 2021-02-12T23:31:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172700.09 epoch sec; seqNo:4545; 4057199.63 north; 591101.27 east 2021-02-12T23:31:40Z- AHRS_M2 msg: 1613172700.47 epoch sec; seqNo:4501 2021-02-12T23:31:40Z- AHRS_M2 msg: 0.69 phi; -0.32 theta; -0.16 psi 2021-02-12T23:31:40Z- handleDepth()-Depth_Keller msg: 1613172700478 epoch sec; seqNo:4546; depth 18.77 2021-02-12T23:31:40Z- handleDvl() - RDI_Pathfinder msg: 1613172700.49 epoch sec; seqNo:2231 2021-02-12T23:31:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.60 , 92.17 , 68.65 , 100.69 2021-02-12T23:31:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.07 , 0.00 2021-02-12T23:31:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172700.50 epoch sec; seqNo:4546; 4057200.01 north; 591101.27 east 2021-02-12T23:31:40Z- AHRS_M2 msg: 1613172700.88 epoch sec; seqNo:4502 2021-02-12T23:31:40Z- AHRS_M2 msg: 0.69 phi; -0.32 theta; -0.16 psi 2021-02-12T23:31:40Z- handleDepth()-Depth_Keller msg: 1613172700880 epoch sec; seqNo:4547; depth 18.89 2021-02-12T23:31:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172700.90 epoch sec; seqNo:4547; 4057200.01 north; 591101.27 east 2021-02-12T23:31:41Z- AHRS_M2 msg: 1613172701.28 epoch sec; seqNo:4503 2021-02-12T23:31:41Z- AHRS_M2 msg: 0.69 phi; -0.32 theta; -0.16 psi 2021-02-12T23:31:41Z- handleDepth()-Depth_Keller msg: 1613172701293 epoch sec; seqNo:4548; depth 19.00 2021-02-12T23:31:41Z- handleDvl() - RDI_Pathfinder msg: 1613172701.31 epoch sec; seqNo:2232 2021-02-12T23:31:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.01 , 90.95 , 69.06 , 101.90 2021-02-12T23:31:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.05 2021-02-12T23:31:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172701.32 epoch sec; seqNo:4548; 4057200.39 north; 591101.26 east 2021-02-12T23:31:41Z- AHRS_M2 msg: 1613172701.68 epoch sec; seqNo:4504 2021-02-12T23:31:41Z- AHRS_M2 msg: 0.69 phi; -0.32 theta; -0.15 psi 2021-02-12T23:31:41Z- handleDepth()-Depth_Keller msg: 1613172701693 epoch sec; seqNo:4549; depth 19.13 2021-02-12T23:31:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172701.70 epoch sec; seqNo:4549; 4057200.77 north; 591101.26 east 2021-02-12T23:31:42Z- AHRS_M2 msg: 1613172702.09 epoch sec; seqNo:4505 2021-02-12T23:31:42Z- AHRS_M2 msg: 0.69 phi; -0.32 theta; -0.16 psi 2021-02-12T23:31:42Z- handleDepth()-Depth_Keller msg: 1613172702094 epoch sec; seqNo:4550; depth 19.24 2021-02-12T23:31:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172702.10 epoch sec; seqNo:4550; 4057200.78 north; 591102.48 east 2021-02-12T23:31:42Z- AHRS_M2 msg: 1613172702.49 epoch sec; seqNo:4506 2021-02-12T23:31:42Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.17 psi 2021-02-12T23:31:42Z- handleDepth()-Depth_Keller msg: 1613172702500 epoch sec; seqNo:4551; depth 19.37 2021-02-12T23:31:42Z- handleDvl() - RDI_Pathfinder msg: 1613172702.53 epoch sec; seqNo:2233 2021-02-12T23:31:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.01 , 88.52 , 68.24 , 97.04 2021-02-12T23:31:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.00 , 0.07 2021-02-12T23:31:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172702.54 epoch sec; seqNo:4551; 4057201.16 north; 591102.47 east 2021-02-12T23:31:42Z- AHRS_M2 msg: 1613172702.89 epoch sec; seqNo:4507 2021-02-12T23:31:42Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.17 psi 2021-02-12T23:31:42Z- handleDepth()-Depth_Keller msg: 1613172702905 epoch sec; seqNo:4552; depth 19.48 2021-02-12T23:31:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172702.91 epoch sec; seqNo:4552; 4057201.54 north; 591102.47 east 2021-02-12T23:31:43Z- AHRS_M2 msg: 1613172703.30 epoch sec; seqNo:4508 2021-02-12T23:31:43Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.19 psi 2021-02-12T23:31:43Z- handleDepth()-Depth_Keller msg: 1613172703304 epoch sec; seqNo:4553; depth 19.60 2021-02-12T23:31:43Z- handleDvl() - RDI_Pathfinder msg: 1613172703.32 epoch sec; seqNo:2234 2021-02-12T23:31:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.78 , 92.31 , 68.65 , 103.26 2021-02-12T23:31:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.04 , 0.04 2021-02-12T23:31:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172703.33 epoch sec; seqNo:4553; 4057201.54 north; 591102.47 east 2021-02-12T23:31:43Z- AHRS_M2 msg: 1613172703.71 epoch sec; seqNo:4509 2021-02-12T23:31:43Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.20 psi 2021-02-12T23:31:43Z- handleDepth()-Depth_Keller msg: 1613172703718 epoch sec; seqNo:4554; depth 19.71 2021-02-12T23:31:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172703.73 epoch sec; seqNo:4554; 4057201.92 north; 591102.47 east 2021-02-12T23:31:44Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-12T23:31:44Z- waiting for 1613172701.90, time is 1613172703.73 2021-02-12T23:31:44Z- LcmTrn::updateTrn() >>>> pose time 1613172703.73, meas time 1613172703.32 <<<< 2021-02-12T23:31:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172703.32 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:31:44Z- TerrainNav::motionUpdate() - currEstimate = (4057202, 591102.47, 19.71) 2021-02-12T23:31:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:31:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:31:44Z- TNavBF::Projecting Measurements 2021-02-12T23:31:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:31:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:31:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:31:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:31:44Z- TNavFilter::4 good beams. 2021-02-12T23:31:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:31:44Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:31:44Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:31:44Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:31:44Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:31:44Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:31:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:31:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:31:44Z- TerrainMapOctree: (vehN, vehE ) = (4057201.65, 591102.26). 2021-02-12T23:31:44Z- TerrainMapOctree: Min Distance = 5128.10. 2021-02-12T23:31:44Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:31:44Z- There is no specific good beam that all particles have in common. 2021-02-12T23:31:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:31:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 8.95 2021-02-12T23:31:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:31:44Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 13.63 2021-02-12T23:31:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:31:44Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 7.54 2021-02-12T23:31:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:31:44Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 17.06 2021-02-12T23:31:44Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:31:44Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:31:44Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:31:44Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:31:44Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:31:44Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:31:44Z- Filter 0 No beams used in subcloud update 2021-02-12T23:31:44Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:31:44Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:31:44Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 757.6 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:31:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172703.32 sec. 2021-02-12T23:31:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:31:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:31:44Z- variances([0], [2], and psi_berg) = 757.605909 833.040466 0.000000 2021-02-12T23:31:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:31:44Z- LcmTrn::publishEstimates() - MLE : 4057185.42 591058.97 19.71 2021-02-12T23:31:44Z- LcmTrn::publishEstimates() - MMSE : 4057201.92 591102.47 19.71 2021-02-12T23:31:44Z- LcmTrn::publishEstimates() - COVAR: 757.61 833.04 0.81 0.00 2021-02-12T23:31:44Z- AHRS_M2 msg: 1613172704.11 epoch sec; seqNo:4510 2021-02-12T23:31:44Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.21 psi 2021-02-12T23:31:44Z- handleDepth()-Depth_Keller msg: 1613172704117 epoch sec; seqNo:4555; depth 19.84 2021-02-12T23:31:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172704.13 epoch sec; seqNo:4555; 4057202.30 north; 591102.46 east 2021-02-12T23:31:44Z- AHRS_M2 msg: 1613172704.51 epoch sec; seqNo:4511 2021-02-12T23:31:44Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.22 psi 2021-02-12T23:31:44Z- handleDepth()-Depth_Keller msg: 1613172704518 epoch sec; seqNo:4556; depth 19.95 2021-02-12T23:31:44Z- handleDvl() - RDI_Pathfinder msg: 1613172704.53 epoch sec; seqNo:2235 2021-02-12T23:31:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 93.32 , 69.06 , 103.05 2021-02-12T23:31:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , 0.00 2021-02-12T23:31:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172704.54 epoch sec; seqNo:4556; 4057202.31 north; 591103.68 east 2021-02-12T23:31:44Z- AHRS_M2 msg: 1613172704.92 epoch sec; seqNo:4512 2021-02-12T23:31:44Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.22 psi 2021-02-12T23:31:44Z- handleDepth()-Depth_Keller msg: 1613172704924 epoch sec; seqNo:4557; depth 20.06 2021-02-12T23:31:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172704.95 epoch sec; seqNo:4557; 4057202.69 north; 591103.68 east 2021-02-12T23:31:45Z- AHRS_M2 msg: 1613172705.32 epoch sec; seqNo:4513 2021-02-12T23:31:45Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.23 psi 2021-02-12T23:31:45Z- handleDepth()-Depth_Keller msg: 1613172705324 epoch sec; seqNo:4558; depth 20.18 2021-02-12T23:31:45Z- handleDvl() - RDI_Pathfinder msg: 1613172705.34 epoch sec; seqNo:2236 2021-02-12T23:31:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 93.32 , 68.24 , 101.84 2021-02-12T23:31:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.04 2021-02-12T23:31:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172705.35 epoch sec; seqNo:4558; 4057203.07 north; 591103.67 east 2021-02-12T23:31:45Z- AHRS_M2 msg: 1613172705.72 epoch sec; seqNo:4514 2021-02-12T23:31:45Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.24 psi 2021-02-12T23:31:45Z- handleDepth()-Depth_Keller msg: 1613172705728 epoch sec; seqNo:4559; depth 20.31 2021-02-12T23:31:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172705.74 epoch sec; seqNo:4559; 4057203.44 north; 591103.67 east 2021-02-12T23:31:46Z- AHRS_M2 msg: 1613172706.13 epoch sec; seqNo:4515 2021-02-12T23:31:46Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.24 psi 2021-02-12T23:31:46Z- handleDepth()-Depth_Keller msg: 1613172706136 epoch sec; seqNo:4560; depth 20.44 2021-02-12T23:31:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172706.15 epoch sec; seqNo:4560; 4057203.44 north; 591103.67 east 2021-02-12T23:31:46Z- AHRS_M2 msg: 1613172706.53 epoch sec; seqNo:4516 2021-02-12T23:31:46Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.24 psi 2021-02-12T23:31:46Z- handleDepth()-Depth_Keller msg: 1613172706545 epoch sec; seqNo:4561; depth 20.54 2021-02-12T23:31:46Z- handleDvl() - RDI_Pathfinder msg: 1613172706.56 epoch sec; seqNo:2237 2021-02-12T23:31:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.74 , 95.82 , 68.24 , 101.90 2021-02-12T23:31:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.08 2021-02-12T23:31:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172706.57 epoch sec; seqNo:4561; 4057203.82 north; 591103.67 east 2021-02-12T23:31:46Z- AHRS_M2 msg: 1613172706.94 epoch sec; seqNo:4517 2021-02-12T23:31:46Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.24 psi 2021-02-12T23:31:46Z- handleDepth()-Depth_Keller msg: 1613172706942 epoch sec; seqNo:4562; depth 20.66 2021-02-12T23:31:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172706.96 epoch sec; seqNo:4562; 4057204.22 north; 591104.88 east 2021-02-12T23:31:47Z- AHRS_M2 msg: 1613172707.35 epoch sec; seqNo:4518 2021-02-12T23:31:47Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.23 psi 2021-02-12T23:31:47Z- handleDepth()-Depth_Keller msg: 1613172707350 epoch sec; seqNo:4563; depth 20.80 2021-02-12T23:31:47Z- handleDvl() - RDI_Pathfinder msg: 1613172707.36 epoch sec; seqNo:2238 2021-02-12T23:31:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 94.60 , 67.43 , 103.12 2021-02-12T23:31:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , -0.09 2021-02-12T23:31:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172707.38 epoch sec; seqNo:4563; 4057204.59 north; 591104.88 east 2021-02-12T23:31:47Z- AHRS_M2 msg: 1613172707.76 epoch sec; seqNo:4519 2021-02-12T23:31:47Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.23 psi 2021-02-12T23:31:47Z- handleDepth()-Depth_Keller msg: 1613172707763 epoch sec; seqNo:4564; depth 20.92 2021-02-12T23:31:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172707.77 epoch sec; seqNo:4564; 4057204.97 north; 591104.88 east 2021-02-12T23:31:48Z- AHRS_M2 msg: 1613172708.15 epoch sec; seqNo:4520 2021-02-12T23:31:48Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.23 psi 2021-02-12T23:31:48Z- handleDepth()-Depth_Keller msg: 1613172708155 epoch sec; seqNo:4565; depth 21.03 2021-02-12T23:31:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172708.16 epoch sec; seqNo:4565; 4057204.97 north; 591104.88 east 2021-02-12T23:31:48Z- AHRS_M2 msg: 1613172708.55 epoch sec; seqNo:4521 2021-02-12T23:31:48Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.22 psi 2021-02-12T23:31:48Z- handleDepth()-Depth_Keller msg: 1613172708566 epoch sec; seqNo:4566; depth 21.17 2021-02-12T23:31:48Z- handleDvl() - RDI_Pathfinder msg: 1613172708.58 epoch sec; seqNo:2239 2021-02-12T23:31:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 93.39 , 0.00 , 100.69 2021-02-12T23:31:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.11 , -0.09 2021-02-12T23:31:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172708.59 epoch sec; seqNo:4566; 4057205.35 north; 591104.87 east 2021-02-12T23:31:48Z- AHRS_M2 msg: 1613172708.95 epoch sec; seqNo:4522 2021-02-12T23:31:48Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.22 psi 2021-02-12T23:31:48Z- handleDepth()-Depth_Keller msg: 1613172708961 epoch sec; seqNo:4567; depth 21.29 2021-02-12T23:31:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172708.97 epoch sec; seqNo:4567; 4057205.73 north; 591104.87 east 2021-02-12T23:31:49Z- LcmTrn::handleMessages() - handling 44 messages took 0.61 ms and 132 calls... 2021-02-12T23:31:49Z- waiting for 1613172706.73, time is 1613172708.97 2021-02-12T23:31:49Z- LcmTrn::updateTrn() >>>> pose time 1613172708.97, meas time 1613172708.58 <<<< 2021-02-12T23:31:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172708.58 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:31:49Z- TerrainNav::motionUpdate() - currEstimate = (4057206, 591104.87, 21.29) 2021-02-12T23:31:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:31:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:31:49Z- TNavBF::Projecting Measurements 2021-02-12T23:31:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:31:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:31:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:31:49Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:31:49Z- TNavFilter::3 good beams. 2021-02-12T23:31:49Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:31:49Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:31:49Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:31:49Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:31:49Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:31:49Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:31:49Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:31:49Z- TNavBF::mmse selected from filter -1 2021-02-12T23:31:49Z- TerrainMapOctree: (vehN, vehE ) = (4057205.44, 591104.69). 2021-02-12T23:31:49Z- TerrainMapOctree: Min Distance = 5123.61. 2021-02-12T23:31:49Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:31:49Z- There is no specific good beam that all particles have in common. 2021-02-12T23:31:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:31:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-12T23:31:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:31:49Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 13.95 2021-02-12T23:31:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:31:49Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 16.22 2021-02-12T23:31:49Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:31:49Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:31:49Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:31:49Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:31:49Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:31:49Z- Filter 0 No beams used in subcloud update 2021-02-12T23:31:49Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:31:49Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:31:49Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 757.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:31:49Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172708.58 sec. 2021-02-12T23:31:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:31:49Z- TNavBF::mmse selected from filter -1 2021-02-12T23:31:49Z- variances([0], [2], and psi_berg) = 757.464324 833.040321 0.000000 2021-02-12T23:31:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:31:49Z- LcmTrn::publishEstimates() - MLE : 4057189.23 591061.37 21.29 2021-02-12T23:31:49Z- LcmTrn::publishEstimates() - MMSE : 4057205.73 591104.87 21.29 2021-02-12T23:31:49Z- LcmTrn::publishEstimates() - COVAR: 757.46 833.04 0.86 0.00 2021-02-12T23:31:49Z- AHRS_M2 msg: 1613172709.36 epoch sec; seqNo:4523 2021-02-12T23:31:49Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.22 psi 2021-02-12T23:31:49Z- handleDepth()-Depth_Keller msg: 1613172709369 epoch sec; seqNo:4568; depth 21.40 2021-02-12T23:31:49Z- handleDvl() - RDI_Pathfinder msg: 1613172709.38 epoch sec; seqNo:2240 2021-02-12T23:31:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 94.20 , 67.43 , 97.85 2021-02-12T23:31:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.11 , -0.03 2021-02-12T23:31:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172709.41 epoch sec; seqNo:4568; 4057206.11 north; 591104.86 east 2021-02-12T23:31:49Z- AHRS_M2 msg: 1613172709.78 epoch sec; seqNo:4524 2021-02-12T23:31:49Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.21 psi 2021-02-12T23:31:49Z- handleDepth()-Depth_Keller msg: 1613172709781 epoch sec; seqNo:4569; depth 21.54 2021-02-12T23:31:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172709.79 epoch sec; seqNo:4569; 4057206.50 north; 591106.08 east 2021-02-12T23:31:50Z- AHRS_M2 msg: 1613172710.17 epoch sec; seqNo:4525 2021-02-12T23:31:50Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.21 psi 2021-02-12T23:31:50Z- handleDepth()-Depth_Keller msg: 1613172710182 epoch sec; seqNo:4570; depth 21.64 2021-02-12T23:31:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172710.19 epoch sec; seqNo:4570; 4057206.50 north; 591106.08 east 2021-02-12T23:31:50Z- AHRS_M2 msg: 1613172710.57 epoch sec; seqNo:4526 2021-02-12T23:31:50Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.20 psi 2021-02-12T23:31:50Z- handleDepth()-Depth_Keller msg: 1613172710576 epoch sec; seqNo:4571; depth 21.77 2021-02-12T23:31:50Z- handleDvl() - RDI_Pathfinder msg: 1613172710.59 epoch sec; seqNo:2241 2021-02-12T23:31:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 92.31 , 66.20 , 99.61 2021-02-12T23:31:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , 0.03 2021-02-12T23:31:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172710.61 epoch sec; seqNo:4571; 4057206.88 north; 591106.08 east 2021-02-12T23:31:50Z- AHRS_M2 msg: 1613172710.98 epoch sec; seqNo:4527 2021-02-12T23:31:50Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.20 psi 2021-02-12T23:31:50Z- handleDepth()-Depth_Keller msg: 1613172710985 epoch sec; seqNo:4572; depth 21.90 2021-02-12T23:31:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172711.00 epoch sec; seqNo:4572; 4057207.26 north; 591106.07 east 2021-02-12T23:31:51Z- AHRS_M2 msg: 1613172711.38 epoch sec; seqNo:4528 2021-02-12T23:31:51Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:31:51Z- handleDepth()-Depth_Keller msg: 1613172711384 epoch sec; seqNo:4573; depth 22.02 2021-02-12T23:31:51Z- handleDvl() - RDI_Pathfinder msg: 1613172711.40 epoch sec; seqNo:2242 2021-02-12T23:31:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 89.87 , 65.79 , 98.39 2021-02-12T23:31:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , 0.05 2021-02-12T23:31:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172711.41 epoch sec; seqNo:4573; 4057207.64 north; 591106.07 east 2021-02-12T23:31:51Z- AHRS_M2 msg: 1613172711.78 epoch sec; seqNo:4529 2021-02-12T23:31:51Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:31:51Z- handleDepth()-Depth_Keller msg: 1613172711790 epoch sec; seqNo:4574; depth 22.15 2021-02-12T23:31:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172711.82 epoch sec; seqNo:4574; 4057207.64 north; 591106.07 east 2021-02-12T23:31:52Z- AHRS_M2 msg: 1613172712.19 epoch sec; seqNo:4530 2021-02-12T23:31:52Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:31:52Z- handleDepth()-Depth_Keller msg: 1613172712197 epoch sec; seqNo:4575; depth 22.24 2021-02-12T23:31:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172712.20 epoch sec; seqNo:4575; 4057208.02 north; 591106.06 east 2021-02-12T23:31:52Z- AHRS_M2 msg: 1613172712.59 epoch sec; seqNo:4531 2021-02-12T23:31:52Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:31:52Z- handleDepth()-Depth_Keller msg: 1613172712601 epoch sec; seqNo:4576; depth 22.38 2021-02-12T23:31:52Z- handleDvl() - RDI_Pathfinder msg: 1613172712.61 epoch sec; seqNo:2243 2021-02-12T23:31:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 89.80 , 65.38 , 99.54 2021-02-12T23:31:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , 0.03 2021-02-12T23:31:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172712.62 epoch sec; seqNo:4576; 4057208.40 north; 591106.06 east 2021-02-12T23:31:53Z- AHRS_M2 msg: 1613172713.00 epoch sec; seqNo:4532 2021-02-12T23:31:53Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:31:53Z- handleDepth()-Depth_Keller msg: 1613172713002 epoch sec; seqNo:4577; depth 22.50 2021-02-12T23:31:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172713.01 epoch sec; seqNo:4577; 4057208.41 north; 591107.28 east 2021-02-12T23:31:53Z- AHRS_M2 msg: 1613172713.40 epoch sec; seqNo:4533 2021-02-12T23:31:53Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:31:53Z- handleDepth()-Depth_Keller msg: 1613172713409 epoch sec; seqNo:4578; depth 22.61 2021-02-12T23:31:53Z- handleDvl() - RDI_Pathfinder msg: 1613172713.43 epoch sec; seqNo:2244 2021-02-12T23:31:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 90.21 , 65.38 , 101.03 2021-02-12T23:31:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.01 2021-02-12T23:31:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172713.44 epoch sec; seqNo:4578; 4057208.79 north; 591107.28 east 2021-02-12T23:31:53Z- AHRS_M2 msg: 1613172713.80 epoch sec; seqNo:4534 2021-02-12T23:31:53Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:31:53Z- handleDepth()-Depth_Keller msg: 1613172713813 epoch sec; seqNo:4579; depth 22.74 2021-02-12T23:31:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172713.82 epoch sec; seqNo:4579; 4057209.17 north; 591107.27 east 2021-02-12T23:31:54Z- AHRS_M2 msg: 1613172714.21 epoch sec; seqNo:4535 2021-02-12T23:31:54Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:31:54Z- handleDepth()-Depth_Keller msg: 1613172714212 epoch sec; seqNo:4580; depth 22.86 2021-02-12T23:31:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172714.22 epoch sec; seqNo:4580; 4057209.54 north; 591107.27 east 2021-02-12T23:31:54Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-12T23:31:54Z- waiting for 1613172711.97, time is 1613172714.22 2021-02-12T23:31:54Z- LcmTrn::updateTrn() >>>> pose time 1613172714.22, meas time 1613172713.43 <<<< 2021-02-12T23:31:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172713.43 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:31:54Z- TerrainNav::motionUpdate() - currEstimate = (4057210, 591107.27, 22.86) 2021-02-12T23:31:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:31:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:31:54Z- TNavBF::Projecting Measurements 2021-02-12T23:31:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:31:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:31:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:31:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:31:54Z- TNavFilter::4 good beams. 2021-02-12T23:31:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:31:54Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:31:54Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:31:54Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:31:54Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:31:54Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:31:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:31:54Z- TNavBF::mmse selected from filter -1 2021-02-12T23:31:54Z- TerrainMapOctree: (vehN, vehE ) = (4057208.97, 591106.91). 2021-02-12T23:31:54Z- TerrainMapOctree: Min Distance = 5119.47. 2021-02-12T23:31:54Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:31:54Z- There is no specific good beam that all particles have in common. 2021-02-12T23:31:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:31:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 8.18 2021-02-12T23:31:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:31:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 13.02 2021-02-12T23:31:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:31:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-12T23:31:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:31:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 16.33 2021-02-12T23:31:54Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:31:54Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:31:54Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:31:54Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:31:54Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:31:54Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:31:54Z- Filter 0 No beams used in subcloud update 2021-02-12T23:31:54Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:31:54Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:31:54Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 757.4 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:31:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172713.43 sec. 2021-02-12T23:31:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:31:54Z- TNavBF::mmse selected from filter -1 2021-02-12T23:31:54Z- variances([0], [2], and psi_berg) = 757.314530 833.040168 0.000000 2021-02-12T23:31:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:31:54Z- LcmTrn::publishEstimates() - MLE : 4057193.04 591063.77 22.86 2021-02-12T23:31:54Z- LcmTrn::publishEstimates() - MMSE : 4057209.54 591107.27 22.86 2021-02-12T23:31:54Z- LcmTrn::publishEstimates() - COVAR: 757.31 833.04 0.81 0.00 2021-02-12T23:31:54Z- AHRS_M2 msg: 1613172714.61 epoch sec; seqNo:4536 2021-02-12T23:31:54Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.19 psi 2021-02-12T23:31:54Z- handleDepth()-Depth_Keller msg: 1613172714618 epoch sec; seqNo:4581; depth 22.98 2021-02-12T23:31:54Z- handleDvl() - RDI_Pathfinder msg: 1613172714.63 epoch sec; seqNo:2245 2021-02-12T23:31:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 89.13 , 66.20 , 96.70 2021-02-12T23:31:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , -0.00 2021-02-12T23:31:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172714.64 epoch sec; seqNo:4581; 4057209.54 north; 591107.27 east 2021-02-12T23:31:55Z- AHRS_M2 msg: 1613172715.02 epoch sec; seqNo:4537 2021-02-12T23:31:55Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.19 psi 2021-02-12T23:31:55Z- handleDepth()-Depth_Keller msg: 1613172715020 epoch sec; seqNo:4582; depth 23.11 2021-02-12T23:31:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172715.04 epoch sec; seqNo:4582; 4057209.92 north; 591107.27 east 2021-02-12T23:31:55Z- AHRS_M2 msg: 1613172715.42 epoch sec; seqNo:4538 2021-02-12T23:31:55Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.19 psi 2021-02-12T23:31:55Z- handleDepth()-Depth_Keller msg: 1613172715424 epoch sec; seqNo:4583; depth 23.22 2021-02-12T23:31:55Z- handleDvl() - RDI_Pathfinder msg: 1613172715.44 epoch sec; seqNo:2246 2021-02-12T23:31:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 91.43 , 65.38 , 96.83 2021-02-12T23:31:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.08 , -0.01 2021-02-12T23:31:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172715.46 epoch sec; seqNo:4583; 4057210.32 north; 591108.48 east 2021-02-12T23:31:55Z- AHRS_M2 msg: 1613172715.82 epoch sec; seqNo:4539 2021-02-12T23:31:55Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.19 psi 2021-02-12T23:31:55Z- handleDepth()-Depth_Keller msg: 1613172715833 epoch sec; seqNo:4584; depth 23.36 2021-02-12T23:31:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172715.84 epoch sec; seqNo:4584; 4057210.69 north; 591108.48 east 2021-02-12T23:31:56Z- AHRS_M2 msg: 1613172716.23 epoch sec; seqNo:4540 2021-02-12T23:31:56Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.19 psi 2021-02-12T23:31:56Z- handleDepth()-Depth_Keller msg: 1613172716242 epoch sec; seqNo:4585; depth 23.48 2021-02-12T23:31:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172716.25 epoch sec; seqNo:4585; 4057211.07 north; 591108.47 east 2021-02-12T23:31:56Z- AHRS_M2 msg: 1613172716.63 epoch sec; seqNo:4541 2021-02-12T23:31:56Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.19 psi 2021-02-12T23:31:56Z- handleDepth()-Depth_Keller msg: 1613172716641 epoch sec; seqNo:4586; depth 23.61 2021-02-12T23:31:56Z- handleDvl() - RDI_Pathfinder msg: 1613172716.66 epoch sec; seqNo:2247 2021-02-12T23:31:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 89.20 , 64.57 , 97.85 2021-02-12T23:31:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.11 , -0.00 2021-02-12T23:31:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172716.67 epoch sec; seqNo:4586; 4057211.07 north; 591108.47 east 2021-02-12T23:31:57Z- AHRS_M2 msg: 1613172717.04 epoch sec; seqNo:4542 2021-02-12T23:31:57Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.18 psi 2021-02-12T23:31:57Z- handleDepth()-Depth_Keller msg: 1613172717045 epoch sec; seqNo:4587; depth 23.73 2021-02-12T23:31:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172717.06 epoch sec; seqNo:4587; 4057211.45 north; 591108.47 east 2021-02-12T23:31:57Z- AHRS_M2 msg: 1613172717.44 epoch sec; seqNo:4543 2021-02-12T23:31:57Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.18 psi 2021-02-12T23:31:57Z- handleDepth()-Depth_Keller msg: 1613172717451 epoch sec; seqNo:4588; depth 23.84 2021-02-12T23:31:57Z- handleDvl() - RDI_Pathfinder msg: 1613172717.46 epoch sec; seqNo:2248 2021-02-12T23:31:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 88.18 , 64.97 , 97.92 2021-02-12T23:31:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.07 , 0.02 2021-02-12T23:31:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172717.47 epoch sec; seqNo:4588; 4057211.83 north; 591108.47 east 2021-02-12T23:31:57Z- AHRS_M2 msg: 1613172717.84 epoch sec; seqNo:4544 2021-02-12T23:31:57Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.17 psi 2021-02-12T23:31:57Z- handleDepth()-Depth_Keller msg: 1613172717854 epoch sec; seqNo:4589; depth 23.97 2021-02-12T23:31:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172717.87 epoch sec; seqNo:4589; 4057212.21 north; 591108.46 east 2021-02-12T23:31:58Z- AHRS_M2 msg: 1613172718.25 epoch sec; seqNo:4545 2021-02-12T23:31:58Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-12T23:31:58Z- handleDepth()-Depth_Keller msg: 1613172718258 epoch sec; seqNo:4590; depth 24.09 2021-02-12T23:31:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172718.30 epoch sec; seqNo:4590; 4057212.59 north; 591108.46 east 2021-02-12T23:31:58Z- AHRS_M2 msg: 1613172718.65 epoch sec; seqNo:4546 2021-02-12T23:31:58Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-12T23:31:58Z- handleDepth()-Depth_Keller msg: 1613172718656 epoch sec; seqNo:4591; depth 24.22 2021-02-12T23:31:58Z- handleDvl() - RDI_Pathfinder msg: 1613172718.68 epoch sec; seqNo:2249 2021-02-12T23:31:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 87.10 , 63.34 , 94.67 2021-02-12T23:31:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , 0.03 2021-02-12T23:31:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172718.70 epoch sec; seqNo:4591; 4057212.60 north; 591109.68 east 2021-02-12T23:31:59Z- AHRS_M2 msg: 1613172719.06 epoch sec; seqNo:4547 2021-02-12T23:31:59Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-12T23:31:59Z- handleDepth()-Depth_Keller msg: 1613172719070 epoch sec; seqNo:4592; depth 24.33 2021-02-12T23:31:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172719.08 epoch sec; seqNo:4592; 4057212.98 north; 591109.67 east 2021-02-12T23:31:59Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-12T23:31:59Z- waiting for 1613172717.22, time is 1613172719.08 2021-02-12T23:31:59Z- LcmTrn::updateTrn() >>>> pose time 1613172719.08, meas time 1613172718.68 <<<< 2021-02-12T23:31:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172718.68 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:31:59Z- TerrainNav::motionUpdate() - currEstimate = (4057213, 591109.67, 24.33) 2021-02-12T23:31:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:31:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:31:59Z- TNavBF::Projecting Measurements 2021-02-12T23:31:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:31:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:31:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:31:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:31:59Z- TNavFilter::4 good beams. 2021-02-12T23:31:59Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:31:59Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:31:59Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:31:59Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:31:59Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:31:59Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:31:59Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:31:59Z- TNavBF::mmse selected from filter -1 2021-02-12T23:31:59Z- TerrainMapOctree: (vehN, vehE ) = (4057212.70, 591109.48). 2021-02-12T23:31:59Z- TerrainMapOctree: Min Distance = 5114.95. 2021-02-12T23:31:59Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:31:59Z- There is no specific good beam that all particles have in common. 2021-02-12T23:31:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:31:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-12T23:31:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:31:59Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 12.14 2021-02-12T23:31:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:31:59Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 6.42 2021-02-12T23:31:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:31:59Z- Old Depth Bias: 0.00 DepthBiasCov: 0.77 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 14.34 2021-02-12T23:31:59Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:31:59Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:31:59Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:31:59Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:31:59Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:31:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:31:59Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:31:59Z- Filter 0 No beams used in subcloud update 2021-02-12T23:31:59Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:31:59Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:31:59Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 757.2 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:31:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172718.68 sec. 2021-02-12T23:31:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:31:59Z- TNavBF::mmse selected from filter -1 2021-02-12T23:31:59Z- variances([0], [2], and psi_berg) = 757.177490 833.040028 0.000000 2021-02-12T23:31:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:31:59Z- LcmTrn::publishEstimates() - MLE : 4057196.48 591066.17 24.33 2021-02-12T23:31:59Z- LcmTrn::publishEstimates() - MMSE : 4057212.98 591109.67 24.33 2021-02-12T23:31:59Z- LcmTrn::publishEstimates() - COVAR: 757.18 833.04 0.76 0.00 2021-02-12T23:31:59Z- AHRS_M2 msg: 1613172719.46 epoch sec; seqNo:4548 2021-02-12T23:31:59Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-12T23:31:59Z- handleDepth()-Depth_Keller msg: 1613172719464 epoch sec; seqNo:4593; depth 24.45 2021-02-12T23:31:59Z- handleDvl() - RDI_Pathfinder msg: 1613172719.48 epoch sec; seqNo:2250 2021-02-12T23:31:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 87.17 , 62.93 , 96.90 2021-02-12T23:31:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , 0.04 2021-02-12T23:31:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172719.49 epoch sec; seqNo:4593; 4057213.36 north; 591109.67 east 2021-02-12T23:31:59Z- AHRS_M2 msg: 1613172719.87 epoch sec; seqNo:4549 2021-02-12T23:31:59Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-12T23:31:59Z- handleDepth()-Depth_Keller msg: 1613172719877 epoch sec; seqNo:4594; depth 24.56 2021-02-12T23:31:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172719.89 epoch sec; seqNo:4594; 4057213.36 north; 591109.67 east 2021-02-12T23:32:00Z- AHRS_M2 msg: 1613172720.29 epoch sec; seqNo:4550 2021-02-12T23:32:00Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-12T23:32:00Z- handleDepth()-Depth_Keller msg: 1613172720304 epoch sec; seqNo:4595; depth 24.67 2021-02-12T23:32:00Z- handleDvl() - RDI_Pathfinder msg: 1613172720.32 epoch sec; seqNo:2251 2021-02-12T23:32:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 85.00 , 63.34 , 97.98 2021-02-12T23:32:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , 0.02 2021-02-12T23:32:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172720.33 epoch sec; seqNo:4595; 4057213.74 north; 591109.67 east 2021-02-12T23:32:00Z- AHRS_M2 msg: 1613172720.67 epoch sec; seqNo:4551 2021-02-12T23:32:00Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.17 psi 2021-02-12T23:32:00Z- handleDepth()-Depth_Keller msg: 1613172720681 epoch sec; seqNo:4596; depth 24.81 2021-02-12T23:32:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172720.69 epoch sec; seqNo:4596; 4057214.12 north; 591109.66 east 2021-02-12T23:32:01Z- AHRS_M2 msg: 1613172721.08 epoch sec; seqNo:4552 2021-02-12T23:32:01Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-12T23:32:01Z- handleDepth()-Depth_Keller msg: 1613172721086 epoch sec; seqNo:4597; depth 24.91 2021-02-12T23:32:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172721.10 epoch sec; seqNo:4597; 4057214.50 north; 591109.66 east 2021-02-12T23:32:01Z- AHRS_M2 msg: 1613172721.48 epoch sec; seqNo:4553 2021-02-12T23:32:01Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-12T23:32:01Z- handleDepth()-Depth_Keller msg: 1613172721484 epoch sec; seqNo:4598; depth 25.04 2021-02-12T23:32:01Z- handleDvl() - RDI_Pathfinder msg: 1613172721.50 epoch sec; seqNo:2252 2021-02-12T23:32:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 87.17 , 63.34 , 95.82 2021-02-12T23:32:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , -0.00 2021-02-12T23:32:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172721.51 epoch sec; seqNo:4598; 4057214.51 north; 591110.88 east 2021-02-12T23:32:01Z- AHRS_M2 msg: 1613172721.88 epoch sec; seqNo:4554 2021-02-12T23:32:01Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-12T23:32:01Z- handleDepth()-Depth_Keller msg: 1613172721893 epoch sec; seqNo:4599; depth 25.17 2021-02-12T23:32:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172721.90 epoch sec; seqNo:4599; 4057214.89 north; 591110.88 east 2021-02-12T23:32:02Z- AHRS_M2 msg: 1613172722.30 epoch sec; seqNo:4555 2021-02-12T23:32:02Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.17 psi 2021-02-12T23:32:02Z- handleDepth()-Depth_Keller msg: 1613172722305 epoch sec; seqNo:4600; depth 25.27 2021-02-12T23:32:02Z- handleDvl() - RDI_Pathfinder msg: 1613172722.32 epoch sec; seqNo:2253 2021-02-12T23:32:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 85.00 , 63.75 , 97.98 2021-02-12T23:32:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , -0.05 2021-02-12T23:32:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172722.35 epoch sec; seqNo:4600; 4057215.27 north; 591110.87 east 2021-02-12T23:32:02Z- AHRS_M2 msg: 1613172722.69 epoch sec; seqNo:4556 2021-02-12T23:32:02Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.17 psi 2021-02-12T23:32:02Z- handleDepth()-Depth_Keller msg: 1613172722699 epoch sec; seqNo:4601; depth 25.40 2021-02-12T23:32:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172722.71 epoch sec; seqNo:4601; 4057215.65 north; 591110.87 east 2021-02-12T23:32:03Z- AHRS_M2 msg: 1613172723.10 epoch sec; seqNo:4557 2021-02-12T23:32:03Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.17 psi 2021-02-12T23:32:03Z- handleDepth()-Depth_Keller msg: 1613172723109 epoch sec; seqNo:4602; depth 25.51 2021-02-12T23:32:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172723.14 epoch sec; seqNo:4602; 4057215.65 north; 591110.87 east 2021-02-12T23:32:03Z- AHRS_M2 msg: 1613172723.50 epoch sec; seqNo:4558 2021-02-12T23:32:03Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.17 psi 2021-02-12T23:32:03Z- handleDepth()-Depth_Keller msg: 1613172723504 epoch sec; seqNo:4603; depth 25.66 2021-02-12T23:32:03Z- handleDvl() - RDI_Pathfinder msg: 1613172723.52 epoch sec; seqNo:2254 2021-02-12T23:32:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 85.00 , 63.34 , 92.58 2021-02-12T23:32:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , -0.07 2021-02-12T23:32:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172723.53 epoch sec; seqNo:4603; 4057216.02 north; 591110.86 east 2021-02-12T23:32:03Z- AHRS_M2 msg: 1613172723.91 epoch sec; seqNo:4559 2021-02-12T23:32:03Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.17 psi 2021-02-12T23:32:03Z- handleDepth()-Depth_Keller msg: 1613172723921 epoch sec; seqNo:4604; depth 25.76 2021-02-12T23:32:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172723.93 epoch sec; seqNo:4604; 4057216.42 north; 591112.08 east 2021-02-12T23:32:04Z- AHRS_M2 msg: 1613172724.32 epoch sec; seqNo:4560 2021-02-12T23:32:04Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.16 psi 2021-02-12T23:32:04Z- handleDepth()-Depth_Keller msg: 1613172724324 epoch sec; seqNo:4605; depth 25.87 2021-02-12T23:32:04Z- handleDvl() - RDI_Pathfinder msg: 1613172724.34 epoch sec; seqNo:2255 2021-02-12T23:32:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 85.00 , 62.53 , 97.98 2021-02-12T23:32:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.07 , -0.07 2021-02-12T23:32:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172724.35 epoch sec; seqNo:4605; 4057216.79 north; 591112.08 east 2021-02-12T23:32:04Z- LcmTrn::handleMessages() - handling 45 messages took 0.73 ms and 131 calls... 2021-02-12T23:32:04Z- waiting for 1613172722.08, time is 1613172724.35 2021-02-12T23:32:04Z- LcmTrn::updateTrn() equating pose 1613172724.35 and meas times 1613172724.35 2021-02-12T23:32:04Z- LcmTrn::updateTrn() >>>> pose time 1613172724.35, meas time 1613172724.35 <<<< 2021-02-12T23:32:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172724.35 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:32:04Z- TerrainNav::motionUpdate() - currEstimate = (4057217, 591112.08, 25.87) 2021-02-12T23:32:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:32:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:32:04Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172724.35 sec.; No relevant navigation data available 2021-02-12T23:32:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:32:04Z- TNavBF::mmse selected from filter -1 2021-02-12T23:32:04Z- variances([0], [2], and psi_berg) = 757.063566 833.039911 0.000000 2021-02-12T23:32:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:32:04Z- LcmTrn::publishEstimates() - MLE : 4057200.29 591068.58 25.87 2021-02-12T23:32:04Z- LcmTrn::publishEstimates() - MMSE : 4057216.79 591112.08 25.87 2021-02-12T23:32:04Z- LcmTrn::publishEstimates() - COVAR: 757.06 833.04 0.99 0.00 2021-02-12T23:32:04Z- AHRS_M2 msg: 1613172724.72 epoch sec; seqNo:4561 2021-02-12T23:32:04Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.16 psi 2021-02-12T23:32:04Z- handleDepth()-Depth_Keller msg: 1613172724721 epoch sec; seqNo:4606; depth 26.00 2021-02-12T23:32:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172724.73 epoch sec; seqNo:4606; 4057216.79 north; 591112.08 east 2021-02-12T23:32:05Z- AHRS_M2 msg: 1613172725.12 epoch sec; seqNo:4562 2021-02-12T23:32:05Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.16 psi 2021-02-12T23:32:05Z- handleDepth()-Depth_Keller msg: 1613172725124 epoch sec; seqNo:4607; depth 26.11 2021-02-12T23:32:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172725.13 epoch sec; seqNo:4607; 4057217.17 north; 591112.07 east 2021-02-12T23:32:05Z- AHRS_M2 msg: 1613172725.52 epoch sec; seqNo:4563 2021-02-12T23:32:05Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.16 psi 2021-02-12T23:32:05Z- handleDepth()-Depth_Keller msg: 1613172725526 epoch sec; seqNo:4608; depth 26.21 2021-02-12T23:32:05Z- handleDvl() - RDI_Pathfinder msg: 1613172725.54 epoch sec; seqNo:2256 2021-02-12T23:32:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.66 , 83.92 , 62.53 , 97.98 2021-02-12T23:32:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , -0.03 2021-02-12T23:32:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172725.55 epoch sec; seqNo:4608; 4057217.55 north; 591112.07 east 2021-02-12T23:32:05Z- AHRS_M2 msg: 1613172725.92 epoch sec; seqNo:4564 2021-02-12T23:32:05Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.16 psi 2021-02-12T23:32:05Z- handleDepth()-Depth_Keller msg: 1613172725928 epoch sec; seqNo:4609; depth 26.35 2021-02-12T23:32:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172725.95 epoch sec; seqNo:4609; 4057217.93 north; 591112.06 east 2021-02-12T23:32:06Z- AHRS_M2 msg: 1613172726.33 epoch sec; seqNo:4565 2021-02-12T23:32:06Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.16 psi 2021-02-12T23:32:06Z- handleDepth()-Depth_Keller msg: 1613172726337 epoch sec; seqNo:4610; depth 26.45 2021-02-12T23:32:06Z- handleDvl() - RDI_Pathfinder msg: 1613172726.35 epoch sec; seqNo:2257 2021-02-12T23:32:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 83.92 , 61.71 , 93.66 2021-02-12T23:32:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , -0.00 2021-02-12T23:32:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172726.37 epoch sec; seqNo:4610; 4057218.32 north; 591113.28 east 2021-02-12T23:32:06Z- AHRS_M2 msg: 1613172726.73 epoch sec; seqNo:4566 2021-02-12T23:32:06Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.16 psi 2021-02-12T23:32:06Z- handleDepth()-Depth_Keller msg: 1613172726736 epoch sec; seqNo:4611; depth 26.59 2021-02-12T23:32:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172726.75 epoch sec; seqNo:4611; 4057218.32 north; 591113.28 east 2021-02-12T23:32:07Z- AHRS_M2 msg: 1613172727.14 epoch sec; seqNo:4567 2021-02-12T23:32:07Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.16 psi 2021-02-12T23:32:07Z- handleDepth()-Depth_Keller msg: 1613172727149 epoch sec; seqNo:4612; depth 26.70 2021-02-12T23:32:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172727.16 epoch sec; seqNo:4612; 4057218.70 north; 591113.28 east 2021-02-12T23:32:07Z- AHRS_M2 msg: 1613172727.54 epoch sec; seqNo:4568 2021-02-12T23:32:07Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.16 psi 2021-02-12T23:32:07Z- handleDepth()-Depth_Keller msg: 1613172727549 epoch sec; seqNo:4613; depth 26.81 2021-02-12T23:32:07Z- handleDvl() - RDI_Pathfinder msg: 1613172727.57 epoch sec; seqNo:2258 2021-02-12T23:32:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 79.69 , 61.30 , 97.98 2021-02-12T23:32:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , 0.02 2021-02-12T23:32:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172727.58 epoch sec; seqNo:4613; 4057219.08 north; 591113.27 east 2021-02-12T23:32:07Z- AHRS_M2 msg: 1613172727.94 epoch sec; seqNo:4569 2021-02-12T23:32:07Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.17 psi 2021-02-12T23:32:07Z- handleDepth()-Depth_Keller msg: 1613172727949 epoch sec; seqNo:4614; depth 26.93 2021-02-12T23:32:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172727.96 epoch sec; seqNo:4614; 4057219.08 north; 591113.27 east 2021-02-12T23:32:08Z- AHRS_M2 msg: 1613172728.35 epoch sec; seqNo:4570 2021-02-12T23:32:08Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-12T23:32:08Z- handleDepth()-Depth_Keller msg: 1613172728354 epoch sec; seqNo:4615; depth 27.06 2021-02-12T23:32:08Z- handleDvl() - RDI_Pathfinder msg: 1613172728.37 epoch sec; seqNo:2259 2021-02-12T23:32:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 83.92 , 61.71 , 92.58 2021-02-12T23:32:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , 0.01 2021-02-12T23:32:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172728.38 epoch sec; seqNo:4615; 4057219.46 north; 591113.27 east 2021-02-12T23:32:08Z- AHRS_M2 msg: 1613172728.75 epoch sec; seqNo:4571 2021-02-12T23:32:08Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.18 psi 2021-02-12T23:32:08Z- handleDepth()-Depth_Keller msg: 1613172728757 epoch sec; seqNo:4616; depth 27.18 2021-02-12T23:32:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172728.78 epoch sec; seqNo:4616; 4057219.84 north; 591113.27 east 2021-02-12T23:32:09Z- AHRS_M2 msg: 1613172729.16 epoch sec; seqNo:4572 2021-02-12T23:32:09Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.18 psi 2021-02-12T23:32:09Z- handleDepth()-Depth_Keller msg: 1613172729161 epoch sec; seqNo:4617; depth 27.29 2021-02-12T23:32:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172729.17 epoch sec; seqNo:4617; 4057220.23 north; 591114.48 east 2021-02-12T23:32:09Z- LcmTrn::handleMessages() - handling 40 messages took 0.60 ms and 128 calls... 2021-02-12T23:32:09Z- waiting for 1613172727.35, time is 1613172729.17 2021-02-12T23:32:09Z- LcmTrn::updateTrn() >>>> pose time 1613172729.17, meas time 1613172728.37 <<<< 2021-02-12T23:32:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172728.37 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:32:09Z- TerrainNav::motionUpdate() - currEstimate = (4057220, 591114.48, 27.29) 2021-02-12T23:32:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:32:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:32:09Z- TNavBF::Projecting Measurements 2021-02-12T23:32:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:32:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:32:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:32:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:32:09Z- TNavFilter::4 good beams. 2021-02-12T23:32:09Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:32:09Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:32:09Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:32:09Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:32:09Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:32:09Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:32:09Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:32:09Z- TNavBF::mmse selected from filter -1 2021-02-12T23:32:09Z- TerrainMapOctree: (vehN, vehE ) = (4057219.66, 591114.08). 2021-02-12T23:32:09Z- TerrainMapOctree: Min Distance = 5106.63. 2021-02-12T23:32:09Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:32:09Z- There is no specific good beam that all particles have in common. 2021-02-12T23:32:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:32:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 7.36 2021-02-12T23:32:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:32:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 11.27 2021-02-12T23:32:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:32:09Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 6.09 2021-02-12T23:32:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:32:09Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 13.71 2021-02-12T23:32:09Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:32:09Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:32:09Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:32:09Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:32:09Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:32:09Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:32:09Z- Filter 0 No beams used in subcloud update 2021-02-12T23:32:09Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:32:09Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:32:09Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 757.0 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:32:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172728.37 sec. 2021-02-12T23:32:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:32:09Z- TNavBF::mmse selected from filter -1 2021-02-12T23:32:09Z- variances([0], [2], and psi_berg) = 756.918687 833.039764 0.000000 2021-02-12T23:32:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:32:09Z- LcmTrn::publishEstimates() - MLE : 4057203.73 591070.98 27.29 2021-02-12T23:32:09Z- LcmTrn::publishEstimates() - MMSE : 4057220.23 591114.48 27.29 2021-02-12T23:32:09Z- LcmTrn::publishEstimates() - COVAR: 756.92 833.04 0.84 0.00 2021-02-12T23:32:09Z- AHRS_M2 msg: 1613172729.56 epoch sec; seqNo:4573 2021-02-12T23:32:09Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.19 psi 2021-02-12T23:32:09Z- handleDepth()-Depth_Keller msg: 1613172729564 epoch sec; seqNo:4618; depth 27.43 2021-02-12T23:32:09Z- handleDvl() - RDI_Pathfinder msg: 1613172729.58 epoch sec; seqNo:2260 2021-02-12T23:32:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 83.92 , 60.07 , 97.98 2021-02-12T23:32:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.01 2021-02-12T23:32:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172729.59 epoch sec; seqNo:4618; 4057220.23 north; 591114.48 east 2021-02-12T23:32:09Z- AHRS_M2 msg: 1613172729.96 epoch sec; seqNo:4574 2021-02-12T23:32:09Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.19 psi 2021-02-12T23:32:09Z- handleDepth()-Depth_Keller msg: 1613172729971 epoch sec; seqNo:4619; depth 27.55 2021-02-12T23:32:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172729.98 epoch sec; seqNo:4619; 4057220.61 north; 591114.48 east 2021-02-12T23:32:10Z- AHRS_M2 msg: 1613172730.37 epoch sec; seqNo:4575 2021-02-12T23:32:10Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.19 psi 2021-02-12T23:32:10Z- handleDepth()-Depth_Keller msg: 1613172730377 epoch sec; seqNo:4620; depth 27.67 2021-02-12T23:32:10Z- handleDvl() - RDI_Pathfinder msg: 1613172730.39 epoch sec; seqNo:2261 2021-02-12T23:32:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 86.08 , 60.90 , 97.98 2021-02-12T23:32:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.02 2021-02-12T23:32:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172730.41 epoch sec; seqNo:4620; 4057220.99 north; 591114.47 east 2021-02-12T23:32:10Z- AHRS_M2 msg: 1613172730.77 epoch sec; seqNo:4576 2021-02-12T23:32:10Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.20 psi 2021-02-12T23:32:10Z- handleDepth()-Depth_Keller msg: 1613172730776 epoch sec; seqNo:4621; depth 27.79 2021-02-12T23:32:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172730.79 epoch sec; seqNo:4621; 4057221.37 north; 591114.47 east 2021-02-12T23:32:11Z- AHRS_M2 msg: 1613172731.18 epoch sec; seqNo:4577 2021-02-12T23:32:11Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.20 psi 2021-02-12T23:32:11Z- handleDepth()-Depth_Keller msg: 1613172731189 epoch sec; seqNo:4622; depth 27.92 2021-02-12T23:32:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172731.20 epoch sec; seqNo:4622; 4057221.37 north; 591114.47 east 2021-02-12T23:32:11Z- AHRS_M2 msg: 1613172731.58 epoch sec; seqNo:4578 2021-02-12T23:32:11Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.20 psi 2021-02-12T23:32:11Z- handleDepth()-Depth_Keller msg: 1613172731584 epoch sec; seqNo:4623; depth 28.05 2021-02-12T23:32:11Z- handleDvl() - RDI_Pathfinder msg: 1613172731.60 epoch sec; seqNo:2262 2021-02-12T23:32:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.62 , 82.84 , 60.90 , 94.74 2021-02-12T23:32:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , 0.03 2021-02-12T23:32:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172731.61 epoch sec; seqNo:4623; 4057221.75 north; 591114.47 east 2021-02-12T23:32:11Z- AHRS_M2 msg: 1613172731.98 epoch sec; seqNo:4579 2021-02-12T23:32:11Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.20 psi 2021-02-12T23:32:11Z- handleDepth()-Depth_Keller msg: 1613172731988 epoch sec; seqNo:4624; depth 28.19 2021-02-12T23:32:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172732.00 epoch sec; seqNo:4624; 4057222.14 north; 591115.68 east 2021-02-12T23:32:12Z- AHRS_M2 msg: 1613172732.39 epoch sec; seqNo:4580 2021-02-12T23:32:12Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.20 psi 2021-02-12T23:32:12Z- handleDepth()-Depth_Keller msg: 1613172732394 epoch sec; seqNo:4625; depth 28.31 2021-02-12T23:32:12Z- handleDvl() - RDI_Pathfinder msg: 1613172732.41 epoch sec; seqNo:2263 2021-02-12T23:32:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.21 , 86.08 , 60.90 , 97.98 2021-02-12T23:32:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , 0.03 2021-02-12T23:32:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172732.42 epoch sec; seqNo:4625; 4057222.14 north; 591115.68 east 2021-02-12T23:32:12Z- AHRS_M2 msg: 1613172732.79 epoch sec; seqNo:4581 2021-02-12T23:32:12Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.20 psi 2021-02-12T23:32:12Z- handleDepth()-Depth_Keller msg: 1613172732798 epoch sec; seqNo:4626; depth 28.43 2021-02-12T23:32:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172732.81 epoch sec; seqNo:4626; 4057222.52 north; 591115.68 east 2021-02-12T23:32:13Z- AHRS_M2 msg: 1613172733.20 epoch sec; seqNo:4582 2021-02-12T23:32:13Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.20 psi 2021-02-12T23:32:13Z- handleDepth()-Depth_Keller msg: 1613172733205 epoch sec; seqNo:4627; depth 28.56 2021-02-12T23:32:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172733.21 epoch sec; seqNo:4627; 4057222.90 north; 591115.67 east 2021-02-12T23:32:13Z- AHRS_M2 msg: 1613172733.60 epoch sec; seqNo:4583 2021-02-12T23:32:13Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.20 psi 2021-02-12T23:32:13Z- handleDepth()-Depth_Keller msg: 1613172733604 epoch sec; seqNo:4628; depth 28.69 2021-02-12T23:32:13Z- handleDvl() - RDI_Pathfinder msg: 1613172733.62 epoch sec; seqNo:2264 2021-02-12T23:32:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.21 , 85.00 , 60.49 , 94.74 2021-02-12T23:32:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , 0.01 2021-02-12T23:32:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172733.63 epoch sec; seqNo:4628; 4057222.90 north; 591115.67 east 2021-02-12T23:32:14Z- AHRS_M2 msg: 1613172734.00 epoch sec; seqNo:4584 2021-02-12T23:32:14Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-12T23:32:14Z- handleDepth()-Depth_Keller msg: 1613172734009 epoch sec; seqNo:4629; depth 28.82 2021-02-12T23:32:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172734.03 epoch sec; seqNo:4629; 4057223.27 north; 591115.67 east 2021-02-12T23:32:14Z- AHRS_M2 msg: 1613172734.41 epoch sec; seqNo:4585 2021-02-12T23:32:14Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-12T23:32:14Z- handleDepth()-Depth_Keller msg: 1613172734416 epoch sec; seqNo:4630; depth 28.95 2021-02-12T23:32:14Z- handleDvl() - RDI_Pathfinder msg: 1613172734.43 epoch sec; seqNo:2265 2021-02-12T23:32:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.39 , 85.00 , 60.49 , 97.98 2021-02-12T23:32:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.01 2021-02-12T23:32:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172734.44 epoch sec; seqNo:4630; 4057223.67 north; 591116.89 east 2021-02-12T23:32:14Z- LcmTrn::handleMessages() - handling 45 messages took 0.63 ms and 133 calls... 2021-02-12T23:32:14Z- waiting for 1613172732.17, time is 1613172734.44 2021-02-12T23:32:14Z- LcmTrn::updateTrn() equating pose 1613172734.44 and meas times 1613172734.44 2021-02-12T23:32:14Z- LcmTrn::updateTrn() >>>> pose time 1613172734.44, meas time 1613172734.44 <<<< 2021-02-12T23:32:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172734.44 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:32:14Z- TerrainNav::motionUpdate() - currEstimate = (4057224, 591116.89, 28.95) 2021-02-12T23:32:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:32:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:32:14Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172734.44 sec.; No relevant navigation data available 2021-02-12T23:32:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:32:14Z- TNavBF::mmse selected from filter -1 2021-02-12T23:32:14Z- variances([0], [2], and psi_berg) = 756.809191 833.039653 0.000000 2021-02-12T23:32:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:32:14Z- LcmTrn::publishEstimates() - MLE : 4057207.17 591073.39 28.95 2021-02-12T23:32:14Z- LcmTrn::publishEstimates() - MMSE : 4057223.67 591116.89 28.95 2021-02-12T23:32:14Z- LcmTrn::publishEstimates() - COVAR: 756.81 833.04 1.06 0.00 2021-02-12T23:32:14Z- AHRS_M2 msg: 1613172734.81 epoch sec; seqNo:4586 2021-02-12T23:32:14Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.19 psi 2021-02-12T23:32:14Z- handleDepth()-Depth_Keller msg: 1613172734816 epoch sec; seqNo:4631; depth 29.08 2021-02-12T23:32:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172734.84 epoch sec; seqNo:4631; 4057224.04 north; 591116.88 east 2021-02-12T23:32:15Z- AHRS_M2 msg: 1613172735.22 epoch sec; seqNo:4587 2021-02-12T23:32:15Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.19 psi 2021-02-12T23:32:15Z- handleDepth()-Depth_Keller msg: 1613172735220 epoch sec; seqNo:4632; depth 29.20 2021-02-12T23:32:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172735.23 epoch sec; seqNo:4632; 4057224.04 north; 591116.88 east 2021-02-12T23:32:15Z- AHRS_M2 msg: 1613172735.62 epoch sec; seqNo:4588 2021-02-12T23:32:15Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.19 psi 2021-02-12T23:32:15Z- handleDepth()-Depth_Keller msg: 1613172735626 epoch sec; seqNo:4633; depth 29.32 2021-02-12T23:32:15Z- handleDvl() - RDI_Pathfinder msg: 1613172735.64 epoch sec; seqNo:2266 2021-02-12T23:32:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.80 , 83.92 , 59.67 , 93.65 2021-02-12T23:32:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , -0.01 2021-02-12T23:32:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172735.65 epoch sec; seqNo:4633; 4057224.42 north; 591116.88 east 2021-02-12T23:32:16Z- AHRS_M2 msg: 1613172736.02 epoch sec; seqNo:4589 2021-02-12T23:32:16Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.19 psi 2021-02-12T23:32:16Z- handleDepth()-Depth_Keller msg: 1613172736033 epoch sec; seqNo:4634; depth 29.47 2021-02-12T23:32:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172736.04 epoch sec; seqNo:4634; 4057224.80 north; 591116.88 east 2021-02-12T23:32:16Z- AHRS_M2 msg: 1613172736.43 epoch sec; seqNo:4590 2021-02-12T23:32:16Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.19 psi 2021-02-12T23:32:16Z- handleDepth()-Depth_Keller msg: 1613172736433 epoch sec; seqNo:4635; depth 29.59 2021-02-12T23:32:16Z- handleDvl() - RDI_Pathfinder msg: 1613172736.44 epoch sec; seqNo:2267 2021-02-12T23:32:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.98 , 78.88 , 60.08 , 91.49 2021-02-12T23:32:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , -0.01 2021-02-12T23:32:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172736.45 epoch sec; seqNo:4635; 4057225.19 north; 591118.09 east 2021-02-12T23:32:16Z- AHRS_M2 msg: 1613172736.83 epoch sec; seqNo:4591 2021-02-12T23:32:16Z- AHRS_M2 msg: 0.64 phi; -0.27 theta; -0.19 psi 2021-02-12T23:32:16Z- handleDepth()-Depth_Keller msg: 1613172736849 epoch sec; seqNo:4636; depth 29.76 2021-02-12T23:32:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172736.86 epoch sec; seqNo:4636; 4057225.19 north; 591118.09 east 2021-02-12T23:32:17Z- AHRS_M2 msg: 1613172737.24 epoch sec; seqNo:4592 2021-02-12T23:32:17Z- AHRS_M2 msg: 0.63 phi; -0.23 theta; -0.18 psi 2021-02-12T23:32:17Z- handleDepth()-Depth_Keller msg: 1613172737245 epoch sec; seqNo:4637; depth 29.88 2021-02-12T23:32:17Z- handleDvl() - RDI_Pathfinder msg: 1613172737.27 epoch sec; seqNo:2268 2021-02-12T23:32:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.94 , 76.43 , 60.08 , 83.92 2021-02-12T23:32:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , -0.02 2021-02-12T23:32:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172737.29 epoch sec; seqNo:4637; 4057225.57 north; 591118.09 east 2021-02-12T23:32:17Z- AHRS_M2 msg: 1613172737.64 epoch sec; seqNo:4593 2021-02-12T23:32:17Z- AHRS_M2 msg: 0.62 phi; -0.19 theta; -0.17 psi 2021-02-12T23:32:17Z- handleDepth()-Depth_Keller msg: 1613172737644 epoch sec; seqNo:4638; depth 30.03 2021-02-12T23:32:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172737.65 epoch sec; seqNo:4638; 4057225.95 north; 591118.08 east 2021-02-12T23:32:18Z- AHRS_M2 msg: 1613172738.04 epoch sec; seqNo:4594 2021-02-12T23:32:18Z- AHRS_M2 msg: 0.61 phi; -0.15 theta; -0.16 psi 2021-02-12T23:32:18Z- handleDepth()-Depth_Keller msg: 1613172738048 epoch sec; seqNo:4639; depth 30.12 2021-02-12T23:32:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172738.06 epoch sec; seqNo:4639; 4057226.33 north; 591118.08 east 2021-02-12T23:32:18Z- AHRS_M2 msg: 1613172738.45 epoch sec; seqNo:4595 2021-02-12T23:32:18Z- AHRS_M2 msg: 0.61 phi; -0.11 theta; -0.15 psi 2021-02-12T23:32:18Z- handleDepth()-Depth_Keller msg: 1613172738455 epoch sec; seqNo:4640; depth 30.24 2021-02-12T23:32:18Z- handleDvl() - RDI_Pathfinder msg: 1613172738.47 epoch sec; seqNo:2269 2021-02-12T23:32:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.12 , 72.34 , 62.12 , 73.15 2021-02-12T23:32:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , -0.03 2021-02-12T23:32:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172738.48 epoch sec; seqNo:4640; 4057226.33 north; 591118.08 east 2021-02-12T23:32:18Z- AHRS_M2 msg: 1613172738.86 epoch sec; seqNo:4596 2021-02-12T23:32:18Z- AHRS_M2 msg: 0.60 phi; -0.06 theta; -0.15 psi 2021-02-12T23:32:18Z- handleDepth()-Depth_Keller msg: 1613172738918 epoch sec; seqNo:4641; depth 30.34 2021-02-12T23:32:18Z- handleDvl() - RDI_Pathfinder msg: 1613172738.95 epoch sec; seqNo:2270 2021-02-12T23:32:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.15 , 0.00 , 65.06 , 67.97 2021-02-12T23:32:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , -0.04 2021-02-12T23:32:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172738.95 epoch sec; seqNo:4641; 4057226.71 north; 591118.08 east 2021-02-12T23:32:19Z- handleDepth()-Depth_Keller msg: 1613172739273 epoch sec; seqNo:4642; depth 30.42 2021-02-12T23:32:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172739.29 epoch sec; seqNo:4642; 4057227.10 north; 591119.29 east 2021-02-12T23:32:19Z- LcmTrn::handleMessages() - handling 40 messages took 0.70 ms and 126 calls... 2021-02-12T23:32:19Z- waiting for 1613172737.44, time is 1613172739.29 2021-02-12T23:32:19Z- LcmTrn::updateTrn() >>>> pose time 1613172739.29, meas time 1613172738.95 <<<< 2021-02-12T23:32:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172738.95 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:32:19Z- TerrainNav::motionUpdate() - currEstimate = (4057227, 591119.29, 30.42) 2021-02-12T23:32:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:32:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:32:19Z- TNavBF::Projecting Measurements 2021-02-12T23:32:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:32:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:32:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:32:19Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:32:19Z- TNavFilter::3 good beams. 2021-02-12T23:32:19Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:32:19Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:32:19Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:32:19Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:32:19Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:32:19Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:32:19Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:32:19Z- TNavBF::mmse selected from filter -1 2021-02-12T23:32:19Z- TerrainMapOctree: (vehN, vehE ) = (4057226.86, 591119.12). 2021-02-12T23:32:19Z- TerrainMapOctree: Min Distance = 5097.85. 2021-02-12T23:32:19Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:32:19Z- There is no specific good beam that all particles have in common. 2021-02-12T23:32:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:32:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.137 ExpectedMeasDiff: nan currMeas.covariance: 6.18 2021-02-12T23:32:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:32:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 6.77 2021-02-12T23:32:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:32:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 7.39 2021-02-12T23:32:19Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:32:19Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:32:19Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:32:19Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:32:19Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:32:19Z- Filter 0 No beams used in subcloud update 2021-02-12T23:32:19Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:32:19Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:32:19Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 756.7 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:32:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172738.95 sec. 2021-02-12T23:32:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:32:19Z- TNavBF::mmse selected from filter -1 2021-02-12T23:32:19Z- variances([0], [2], and psi_berg) = 756.674739 833.039516 0.000000 2021-02-12T23:32:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:32:19Z- LcmTrn::publishEstimates() - MLE : 4057210.60 591075.79 30.42 2021-02-12T23:32:19Z- LcmTrn::publishEstimates() - MMSE : 4057227.10 591119.29 30.42 2021-02-12T23:32:19Z- LcmTrn::publishEstimates() - COVAR: 756.67 833.04 0.88 0.00 2021-02-12T23:32:19Z- AHRS_M2 msg: 1613172739.66 epoch sec; seqNo:4597 2021-02-12T23:32:19Z- AHRS_M2 msg: 0.58 phi; 0.02 theta; -0.15 psi 2021-02-12T23:32:19Z- handleDepth()-Depth_Keller msg: 1613172739666 epoch sec; seqNo:4643; depth 30.48 2021-02-12T23:32:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172739.67 epoch sec; seqNo:4643; 4057227.48 north; 591119.29 east 2021-02-12T23:32:20Z- AHRS_M2 msg: 1613172740.06 epoch sec; seqNo:4598 2021-02-12T23:32:20Z- AHRS_M2 msg: 0.57 phi; 0.06 theta; -0.15 psi 2021-02-12T23:32:20Z- handleDepth()-Depth_Keller msg: 1613172740069 epoch sec; seqNo:4644; depth 30.52 2021-02-12T23:32:20Z- handleDvl() - RDI_Pathfinder msg: 1613172740.09 epoch sec; seqNo:2271 2021-02-12T23:32:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.25 , 70.83 , 69.30 , 64.70 2021-02-12T23:32:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.07 , -0.03 2021-02-12T23:32:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172740.10 epoch sec; seqNo:4644; 4057227.86 north; 591119.29 east 2021-02-12T23:32:20Z- AHRS_M2 msg: 1613172740.47 epoch sec; seqNo:4599 2021-02-12T23:32:20Z- AHRS_M2 msg: 0.56 phi; 0.10 theta; -0.15 psi 2021-02-12T23:32:20Z- handleDepth()-Depth_Keller msg: 1613172740475 epoch sec; seqNo:4645; depth 30.56 2021-02-12T23:32:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172740.48 epoch sec; seqNo:4645; 4057227.86 north; 591119.29 east 2021-02-12T23:32:20Z- AHRS_M2 msg: 1613172740.90 epoch sec; seqNo:4600 2021-02-12T23:32:20Z- AHRS_M2 msg: 0.55 phi; 0.16 theta; -0.15 psi 2021-02-12T23:32:20Z- handleDepth()-Depth_Keller msg: 1613172740914 epoch sec; seqNo:4646; depth 30.57 2021-02-12T23:32:20Z- handleDvl() - RDI_Pathfinder msg: 1613172740.93 epoch sec; seqNo:2272 2021-02-12T23:32:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.86 , 72.37 , 73.60 , 62.25 2021-02-12T23:32:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , -0.03 2021-02-12T23:32:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172740.94 epoch sec; seqNo:4646; 4057228.24 north; 591119.28 east 2021-02-12T23:32:21Z- handleDepth()-Depth_Keller msg: 1613172741297 epoch sec; seqNo:4647; depth 30.57 2021-02-12T23:32:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172741.31 epoch sec; seqNo:4647; 4057228.62 north; 591119.28 east 2021-02-12T23:32:21Z- AHRS_M2 msg: 1613172741.69 epoch sec; seqNo:4601 2021-02-12T23:32:21Z- AHRS_M2 msg: 0.54 phi; 0.22 theta; -0.14 psi 2021-02-12T23:32:21Z- handleDepth()-Depth_Keller msg: 1613172741691 epoch sec; seqNo:4648; depth 30.56 2021-02-12T23:32:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172741.70 epoch sec; seqNo:4648; 4057229.00 north; 591119.27 east 2021-02-12T23:32:22Z- AHRS_M2 msg: 1613172742.09 epoch sec; seqNo:4602 2021-02-12T23:32:22Z- AHRS_M2 msg: 0.54 phi; 0.24 theta; -0.14 psi 2021-02-12T23:32:22Z- handleDepth()-Depth_Keller msg: 1613172742090 epoch sec; seqNo:4649; depth 30.55 2021-02-12T23:32:22Z- handleDvl() - RDI_Pathfinder msg: 1613172742.10 epoch sec; seqNo:2273 2021-02-12T23:32:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.76 , 72.83 , 0.00 , 60.85 2021-02-12T23:32:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.07 , -0.03 2021-02-12T23:32:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172742.11 epoch sec; seqNo:4649; 4057229.00 north; 591119.27 east 2021-02-12T23:32:22Z- AHRS_M2 msg: 1613172742.49 epoch sec; seqNo:4603 2021-02-12T23:32:22Z- AHRS_M2 msg: 0.54 phi; 0.26 theta; -0.14 psi 2021-02-12T23:32:22Z- handleDepth()-Depth_Keller msg: 1613172742498 epoch sec; seqNo:4650; depth 30.51 2021-02-12T23:32:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172742.51 epoch sec; seqNo:4650; 4057229.39 north; 591120.49 east 2021-02-12T23:32:22Z- AHRS_M2 msg: 1613172742.91 epoch sec; seqNo:4604 2021-02-12T23:32:22Z- AHRS_M2 msg: 0.54 phi; 0.28 theta; -0.13 psi 2021-02-12T23:32:22Z- handleDepth()-Depth_Keller msg: 1613172742918 epoch sec; seqNo:4651; depth 30.47 2021-02-12T23:32:22Z- handleDvl() - RDI_Pathfinder msg: 1613172742.93 epoch sec; seqNo:2274 2021-02-12T23:32:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.01 , 73.60 , 0.00 , 59.80 2021-02-12T23:32:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , -0.03 2021-02-12T23:32:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172742.94 epoch sec; seqNo:4651; 4057229.77 north; 591120.49 east 2021-02-12T23:32:23Z- AHRS_M2 msg: 1613172743.30 epoch sec; seqNo:4605 2021-02-12T23:32:23Z- AHRS_M2 msg: 0.54 phi; 0.29 theta; -0.13 psi 2021-02-12T23:32:23Z- handleDepth()-Depth_Keller msg: 1613172743300 epoch sec; seqNo:4652; depth 30.40 2021-02-12T23:32:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172743.31 epoch sec; seqNo:4652; 4057230.15 north; 591120.48 east 2021-02-12T23:32:23Z- AHRS_M2 msg: 1613172743.70 epoch sec; seqNo:4606 2021-02-12T23:32:23Z- AHRS_M2 msg: 0.54 phi; 0.30 theta; -0.13 psi 2021-02-12T23:32:23Z- handleDepth()-Depth_Keller msg: 1613172743706 epoch sec; seqNo:4653; depth 30.36 2021-02-12T23:32:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172743.71 epoch sec; seqNo:4653; 4057230.15 north; 591120.48 east 2021-02-12T23:32:24Z- AHRS_M2 msg: 1613172744.10 epoch sec; seqNo:4607 2021-02-12T23:32:24Z- AHRS_M2 msg: 0.54 phi; 0.31 theta; -0.12 psi 2021-02-12T23:32:24Z- handleDepth()-Depth_Keller msg: 1613172744111 epoch sec; seqNo:4654; depth 30.29 2021-02-12T23:32:24Z- handleDvl() - RDI_Pathfinder msg: 1613172744.12 epoch sec; seqNo:2275 2021-02-12T23:32:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.24 , 73.90 , 0.00 , 59.18 2021-02-12T23:32:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , -0.01 2021-02-12T23:32:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172744.14 epoch sec; seqNo:4654; 4057230.52 north; 591120.48 east 2021-02-12T23:32:24Z- AHRS_M2 msg: 1613172744.51 epoch sec; seqNo:4608 2021-02-12T23:32:24Z- AHRS_M2 msg: 0.54 phi; 0.32 theta; -0.12 psi 2021-02-12T23:32:24Z- handleDepth()-Depth_Keller msg: 1613172744519 epoch sec; seqNo:4655; depth 30.20 2021-02-12T23:32:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172744.55 epoch sec; seqNo:4655; 4057230.90 north; 591120.47 east 2021-02-12T23:32:24Z- LcmTrn::handleMessages() - handling 43 messages took 0.57 ms and 132 calls... 2021-02-12T23:32:24Z- waiting for 1613172742.29, time is 1613172744.55 2021-02-12T23:32:24Z- LcmTrn::updateTrn() >>>> pose time 1613172744.55, meas time 1613172744.12 <<<< 2021-02-12T23:32:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172744.12 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:32:24Z- TerrainNav::motionUpdate() - currEstimate = (4057231, 591120.47, 30.20) 2021-02-12T23:32:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:32:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:32:24Z- TNavBF::Projecting Measurements 2021-02-12T23:32:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:32:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:32:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:32:24Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:32:24Z- TNavFilter::3 good beams. 2021-02-12T23:32:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:32:24Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:32:24Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:32:24Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:32:24Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:32:24Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:32:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:32:24Z- TNavBF::mmse selected from filter -1 2021-02-12T23:32:24Z- TerrainMapOctree: (vehN, vehE ) = (4057230.60, 591120.38). 2021-02-12T23:32:24Z- TerrainMapOctree: Min Distance = 5094.13. 2021-02-12T23:32:24Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:32:24Z- There is no specific good beam that all particles have in common. 2021-02-12T23:32:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:32:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 7.02 2021-02-12T23:32:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:32:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 8.74 2021-02-12T23:32:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:32:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 5.60 2021-02-12T23:32:24Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:32:24Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:32:24Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:32:24Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:32:24Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:32:24Z- Filter 0 No beams used in subcloud update 2021-02-12T23:32:24Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:32:24Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:32:24Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 756.6 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:32:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172744.12 sec. 2021-02-12T23:32:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:32:24Z- TNavBF::mmse selected from filter -1 2021-02-12T23:32:24Z- variances([0], [2], and psi_berg) = 756.542664 833.039382 0.000000 2021-02-12T23:32:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:32:24Z- LcmTrn::publishEstimates() - MLE : 4057215.40 591071.97 30.20 2021-02-12T23:32:24Z- LcmTrn::publishEstimates() - MMSE : 4057230.90 591120.47 30.20 2021-02-12T23:32:24Z- LcmTrn::publishEstimates() - COVAR: 756.54 833.04 0.80 0.00 2021-02-12T23:32:24Z- AHRS_M2 msg: 1613172744.92 epoch sec; seqNo:4609 2021-02-12T23:32:24Z- AHRS_M2 msg: 0.55 phi; 0.33 theta; -0.11 psi 2021-02-12T23:32:24Z- handleDepth()-Depth_Keller msg: 1613172744925 epoch sec; seqNo:4656; depth 30.10 2021-02-12T23:32:24Z- handleDvl() - RDI_Pathfinder msg: 1613172744.94 epoch sec; seqNo:2276 2021-02-12T23:32:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.46 , 74.51 , 0.00 , 59.49 2021-02-12T23:32:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.05 , 0.01 2021-02-12T23:32:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172744.95 epoch sec; seqNo:4656; 4057231.28 north; 591120.47 east 2021-02-12T23:32:25Z- AHRS_M2 msg: 1613172745.32 epoch sec; seqNo:4610 2021-02-12T23:32:25Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.11 psi 2021-02-12T23:32:25Z- handleDepth()-Depth_Keller msg: 1613172745322 epoch sec; seqNo:4657; depth 30.03 2021-02-12T23:32:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172745.33 epoch sec; seqNo:4657; 4057231.28 north; 591120.47 east 2021-02-12T23:32:25Z- AHRS_M2 msg: 1613172745.72 epoch sec; seqNo:4611 2021-02-12T23:32:25Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.11 psi 2021-02-12T23:32:25Z- handleDepth()-Depth_Keller msg: 1613172745724 epoch sec; seqNo:4658; depth 29.93 2021-02-12T23:32:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172745.73 epoch sec; seqNo:4658; 4057231.66 north; 591120.47 east 2021-02-12T23:32:26Z- AHRS_M2 msg: 1613172746.13 epoch sec; seqNo:4612 2021-02-12T23:32:26Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.11 psi 2021-02-12T23:32:26Z- handleDepth()-Depth_Keller msg: 1613172746137 epoch sec; seqNo:4659; depth 29.84 2021-02-12T23:32:26Z- handleDvl() - RDI_Pathfinder msg: 1613172746.16 epoch sec; seqNo:2277 2021-02-12T23:32:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.38 , 74.51 , 0.00 , 58.57 2021-02-12T23:32:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , 0.03 2021-02-12T23:32:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172746.17 epoch sec; seqNo:4659; 4057232.05 north; 591121.68 east 2021-02-12T23:32:26Z- AHRS_M2 msg: 1613172746.53 epoch sec; seqNo:4613 2021-02-12T23:32:26Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.11 psi 2021-02-12T23:32:26Z- handleDepth()-Depth_Keller msg: 1613172746533 epoch sec; seqNo:4660; depth 29.73 2021-02-12T23:32:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172746.54 epoch sec; seqNo:4660; 4057232.05 north; 591121.68 east 2021-02-12T23:32:26Z- AHRS_M2 msg: 1613172746.94 epoch sec; seqNo:4614 2021-02-12T23:32:26Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.11 psi 2021-02-12T23:32:26Z- handleDepth()-Depth_Keller msg: 1613172746949 epoch sec; seqNo:4661; depth 29.62 2021-02-12T23:32:26Z- handleDvl() - RDI_Pathfinder msg: 1613172746.96 epoch sec; seqNo:2278 2021-02-12T23:32:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.00 , 75.43 , 0.00 , 60.41 2021-02-12T23:32:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , 0.03 2021-02-12T23:32:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172746.97 epoch sec; seqNo:4661; 4057232.43 north; 591121.68 east 2021-02-12T23:32:27Z- AHRS_M2 msg: 1613172747.34 epoch sec; seqNo:4615 2021-02-12T23:32:27Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.11 psi 2021-02-12T23:32:27Z- handleDepth()-Depth_Keller msg: 1613172747350 epoch sec; seqNo:4662; depth 29.50 2021-02-12T23:32:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172747.36 epoch sec; seqNo:4662; 4057232.81 north; 591121.68 east 2021-02-12T23:32:27Z- AHRS_M2 msg: 1613172747.74 epoch sec; seqNo:4616 2021-02-12T23:32:27Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.11 psi 2021-02-12T23:32:27Z- handleDepth()-Depth_Keller msg: 1613172747744 epoch sec; seqNo:4663; depth 29.41 2021-02-12T23:32:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172747.75 epoch sec; seqNo:4663; 4057233.19 north; 591121.67 east 2021-02-12T23:32:28Z- AHRS_M2 msg: 1613172748.15 epoch sec; seqNo:4617 2021-02-12T23:32:28Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.11 psi 2021-02-12T23:32:28Z- handleDepth()-Depth_Keller msg: 1613172748152 epoch sec; seqNo:4664; depth 29.28 2021-02-12T23:32:28Z- handleDvl() - RDI_Pathfinder msg: 1613172748.16 epoch sec; seqNo:2279 2021-02-12T23:32:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.94 , 76.07 , 0.00 , 60.53 2021-02-12T23:32:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.01 2021-02-12T23:32:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172748.17 epoch sec; seqNo:4664; 4057233.19 north; 591121.67 east 2021-02-12T23:32:28Z- AHRS_M2 msg: 1613172748.55 epoch sec; seqNo:4618 2021-02-12T23:32:28Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.11 psi 2021-02-12T23:32:28Z- handleDepth()-Depth_Keller msg: 1613172748552 epoch sec; seqNo:4665; depth 29.16 2021-02-12T23:32:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172748.57 epoch sec; seqNo:4665; 4057233.57 north; 591121.67 east 2021-02-12T23:32:28Z- AHRS_M2 msg: 1613172748.95 epoch sec; seqNo:4619 2021-02-12T23:32:28Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.11 psi 2021-02-12T23:32:28Z- handleDepth()-Depth_Keller msg: 1613172748956 epoch sec; seqNo:4666; depth 29.06 2021-02-12T23:32:28Z- handleDvl() - RDI_Pathfinder msg: 1613172748.97 epoch sec; seqNo:2280 2021-02-12T23:32:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.59 , 76.07 , 0.00 , 59.23 2021-02-12T23:32:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.02 2021-02-12T23:32:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172748.99 epoch sec; seqNo:4666; 4057233.96 north; 591122.88 east 2021-02-12T23:32:29Z- AHRS_M2 msg: 1613172749.36 epoch sec; seqNo:4620 2021-02-12T23:32:29Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.11 psi 2021-02-12T23:32:29Z- handleDepth()-Depth_Keller msg: 1613172749360 epoch sec; seqNo:4667; depth 28.93 2021-02-12T23:32:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:32:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:32:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:32:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:32:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:32:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:32:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:32:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:32:29Z- handleDvl() - RDI_Pathfinder msg: 1613172749.36 epoch sec; seqNo:2281 2021-02-12T23:32:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:32:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.02 2021-02-12T23:32:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172749.37 epoch sec; seqNo:4667; 4057233.96 north; 591122.88 east 2021-02-12T23:32:29Z- AHRS_M2 msg: 1613172749.76 epoch sec; seqNo:4621 2021-02-12T23:32:29Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.11 psi 2021-02-12T23:32:29Z- handleDepth()-Depth_Keller msg: 1613172749766 epoch sec; seqNo:4668; depth 28.82 2021-02-12T23:32:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172749.78 epoch sec; seqNo:4668; 4057234.34 north; 591122.88 east 2021-02-12T23:32:29Z- LcmTrn::handleMessages() - handling 45 messages took 0.65 ms and 132 calls... 2021-02-12T23:32:29Z- waiting for 1613172747.55, time is 1613172749.78 2021-02-12T23:32:29Z- LcmTrn::updateTrn() >>>> pose time 1613172749.78, meas time 1613172749.36 <<<< 2021-02-12T23:32:29Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172749.36, ping # 0. 2021-02-12T23:32:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172749.36 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:32:29Z- TerrainNav::motionUpdate() - currEstimate = (4057234, 591122.88, 28.82) 2021-02-12T23:32:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:32:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:32:29Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:32:29Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:32:29Z- TNavBF::Projecting Measurements 2021-02-12T23:32:29Z- TNavFilter::Measurement from time = 1613172749.36 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:32:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:32:29Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:32:29Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:32:29Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:32:29Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:32:29Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:32:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:32:29Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 756.4 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:32:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:32:29Z- TNavBF::mmse selected from filter -1 2021-02-12T23:32:29Z- variances([0], [2], and psi_berg) = 756.407558 833.039246 0.000000 2021-02-12T23:32:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:32:29Z- LcmTrn::publishEstimates() - MLE : 4057219.84 591075.38 28.82 2021-02-12T23:32:29Z- LcmTrn::publishEstimates() - MMSE : 4057234.34 591122.88 28.82 2021-02-12T23:32:29Z- LcmTrn::publishEstimates() - COVAR: 756.41 833.04 1.01 0.00 2021-02-12T23:32:30Z- AHRS_M2 msg: 1613172750.17 epoch sec; seqNo:4622 2021-02-12T23:32:30Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.10 psi 2021-02-12T23:32:30Z- handleDepth()-Depth_Keller msg: 1613172750173 epoch sec; seqNo:4669; depth 28.70 2021-02-12T23:32:30Z- handleDvl() - RDI_Pathfinder msg: 1613172750.19 epoch sec; seqNo:2282 2021-02-12T23:32:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.92 , 76.39 , 0.00 , 60.53 2021-02-12T23:32:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.03 2021-02-12T23:32:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172750.20 epoch sec; seqNo:4669; 4057234.72 north; 591122.88 east 2021-02-12T23:32:30Z- AHRS_M2 msg: 1613172750.57 epoch sec; seqNo:4623 2021-02-12T23:32:30Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.10 psi 2021-02-12T23:32:30Z- handleDepth()-Depth_Keller msg: 1613172750572 epoch sec; seqNo:4670; depth 28.58 2021-02-12T23:32:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:32:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:32:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:32:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:32:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:32:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:32:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:32:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:32:30Z- handleDvl() - RDI_Pathfinder msg: 1613172750.57 epoch sec; seqNo:2283 2021-02-12T23:32:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:32:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.03 2021-02-12T23:32:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172750.59 epoch sec; seqNo:4670; 4057235.10 north; 591122.87 east 2021-02-12T23:32:30Z- AHRS_M2 msg: 1613172750.98 epoch sec; seqNo:4624 2021-02-12T23:32:30Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.10 psi 2021-02-12T23:32:30Z- handleDepth()-Depth_Keller msg: 1613172750984 epoch sec; seqNo:4671; depth 28.46 2021-02-12T23:32:31Z- handleDvl() - RDI_Pathfinder msg: 1613172751.01 epoch sec; seqNo:2284 2021-02-12T23:32:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.24 , 76.71 , 0.00 , 61.18 2021-02-12T23:32:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , -0.02 2021-02-12T23:32:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172751.04 epoch sec; seqNo:4671; 4057235.10 north; 591122.87 east 2021-02-12T23:32:31Z- AHRS_M2 msg: 1613172751.38 epoch sec; seqNo:4625 2021-02-12T23:32:31Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.10 psi 2021-02-12T23:32:31Z- handleDepth()-Depth_Keller msg: 1613172751380 epoch sec; seqNo:4672; depth 28.34 2021-02-12T23:32:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172751.40 epoch sec; seqNo:4672; 4057235.47 north; 591122.87 east 2021-02-12T23:32:31Z- AHRS_M2 msg: 1613172751.78 epoch sec; seqNo:4626 2021-02-12T23:32:31Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.10 psi 2021-02-12T23:32:31Z- handleDepth()-Depth_Keller msg: 1613172751784 epoch sec; seqNo:4673; depth 28.23 2021-02-12T23:32:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172751.79 epoch sec; seqNo:4673; 4057235.87 north; 591124.08 east 2021-02-12T23:32:32Z- AHRS_M2 msg: 1613172752.18 epoch sec; seqNo:4627 2021-02-12T23:32:32Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.11 psi 2021-02-12T23:32:32Z- handleDepth()-Depth_Keller msg: 1613172752193 epoch sec; seqNo:4674; depth 28.10 2021-02-12T23:32:32Z- handleDvl() - RDI_Pathfinder msg: 1613172752.20 epoch sec; seqNo:2285 2021-02-12T23:32:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 70.24 , 0.00 , 0.00 , 60.21 2021-02-12T23:32:32Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:32:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172752.21 epoch sec; seqNo:4674; 4057235.87 north; 591124.08 east 2021-02-12T23:32:32Z- AHRS_M2 msg: 1613172752.59 epoch sec; seqNo:4628 2021-02-12T23:32:32Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.11 psi 2021-02-12T23:32:32Z- handleDepth()-Depth_Keller msg: 1613172752596 epoch sec; seqNo:4675; depth 27.98 2021-02-12T23:32:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172752.61 epoch sec; seqNo:4675; 4057236.25 north; 591124.08 east 2021-02-12T23:32:33Z- AHRS_M2 msg: 1613172753.00 epoch sec; seqNo:4629 2021-02-12T23:32:33Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.11 psi 2021-02-12T23:32:33Z- handleDepth()-Depth_Keller msg: 1613172753008 epoch sec; seqNo:4676; depth 27.85 2021-02-12T23:32:33Z- handleDvl() - RDI_Pathfinder msg: 1613172753.02 epoch sec; seqNo:2286 2021-02-12T23:32:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.27 , 77.09 , 0.00 , 61.33 2021-02-12T23:32:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.04 , 0.01 2021-02-12T23:32:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172753.03 epoch sec; seqNo:4676; 4057236.62 north; 591124.08 east 2021-02-12T23:32:33Z- AHRS_M2 msg: 1613172753.41 epoch sec; seqNo:4630 2021-02-12T23:32:33Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.12 psi 2021-02-12T23:32:33Z- handleDepth()-Depth_Keller msg: 1613172753422 epoch sec; seqNo:4677; depth 27.71 2021-02-12T23:32:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172753.44 epoch sec; seqNo:4677; 4057237.00 north; 591124.07 east 2021-02-12T23:32:33Z- AHRS_M2 msg: 1613172753.80 epoch sec; seqNo:4631 2021-02-12T23:32:33Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.12 psi 2021-02-12T23:32:33Z- handleDepth()-Depth_Keller msg: 1613172753804 epoch sec; seqNo:4678; depth 27.61 2021-02-12T23:32:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172753.81 epoch sec; seqNo:4678; 4057237.00 north; 591124.07 east 2021-02-12T23:32:34Z- AHRS_M2 msg: 1613172754.20 epoch sec; seqNo:4632 2021-02-12T23:32:34Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.13 psi 2021-02-12T23:32:34Z- handleDepth()-Depth_Keller msg: 1613172754217 epoch sec; seqNo:4679; depth 27.48 2021-02-12T23:32:34Z- handleDvl() - RDI_Pathfinder msg: 1613172754.23 epoch sec; seqNo:2287 2021-02-12T23:32:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.24 , 78.98 , 0.00 , 61.18 2021-02-12T23:32:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , 0.01 2021-02-12T23:32:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172754.24 epoch sec; seqNo:4679; 4057237.38 north; 591124.07 east 2021-02-12T23:32:34Z- AHRS_M2 msg: 1613172754.61 epoch sec; seqNo:4633 2021-02-12T23:32:34Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.13 psi 2021-02-12T23:32:34Z- handleDepth()-Depth_Keller msg: 1613172754612 epoch sec; seqNo:4680; depth 27.36 2021-02-12T23:32:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172754.62 epoch sec; seqNo:4680; 4057237.77 north; 591125.29 east 2021-02-12T23:32:34Z- LcmTrn::handleMessages() - handling 42 messages took 0.68 ms and 129 calls... 2021-02-12T23:32:34Z- waiting for 1613172752.78, time is 1613172754.62 2021-02-12T23:32:34Z- LcmTrn::updateTrn() >>>> pose time 1613172754.62, meas time 1613172754.23 <<<< 2021-02-12T23:32:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172754.23 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:32:34Z- TerrainNav::motionUpdate() - currEstimate = (4057238, 591125.29, 27.36) 2021-02-12T23:32:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:32:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:32:34Z- TNavBF::Projecting Measurements 2021-02-12T23:32:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:32:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:32:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:32:34Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:32:34Z- TNavFilter::3 good beams. 2021-02-12T23:32:34Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:32:34Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:32:34Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:32:34Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:32:34Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:32:34Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:32:34Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:32:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:32:34Z- TerrainMapOctree: (vehN, vehE ) = (4057237.49, 591125.09). 2021-02-12T23:32:34Z- TerrainMapOctree: Min Distance = 5085.79. 2021-02-12T23:32:34Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:32:34Z- There is no specific good beam that all particles have in common. 2021-02-12T23:32:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:32:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 7.89 2021-02-12T23:32:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:32:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 9.98 2021-02-12T23:32:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:32:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 5.99 2021-02-12T23:32:34Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:32:34Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:32:34Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:32:34Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:32:34Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:32:34Z- Filter 0 No beams used in subcloud update 2021-02-12T23:32:34Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:32:34Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:32:34Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 756.3 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:32:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172754.23 sec. 2021-02-12T23:32:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:32:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:32:34Z- variances([0], [2], and psi_berg) = 756.272403 833.039109 0.000000 2021-02-12T23:32:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:32:34Z- LcmTrn::publishEstimates() - MLE : 4057223.27 591077.79 27.36 2021-02-12T23:32:34Z- LcmTrn::publishEstimates() - MMSE : 4057237.77 591125.29 27.36 2021-02-12T23:32:34Z- LcmTrn::publishEstimates() - COVAR: 756.27 833.04 0.89 0.00 2021-02-12T23:32:35Z- AHRS_M2 msg: 1613172755.01 epoch sec; seqNo:4634 2021-02-12T23:32:35Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.14 psi 2021-02-12T23:32:35Z- handleDepth()-Depth_Keller msg: 1613172755021 epoch sec; seqNo:4681; depth 27.23 2021-02-12T23:32:35Z- handleDvl() - RDI_Pathfinder msg: 1613172755.03 epoch sec; seqNo:2288 2021-02-12T23:32:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.92 , 79.95 , 0.00 , 60.85 2021-02-12T23:32:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.04 , 0.01 2021-02-12T23:32:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172755.04 epoch sec; seqNo:4681; 4057237.77 north; 591125.29 east 2021-02-12T23:32:35Z- AHRS_M2 msg: 1613172755.42 epoch sec; seqNo:4635 2021-02-12T23:32:35Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-12T23:32:35Z- handleDepth()-Depth_Keller msg: 1613172755423 epoch sec; seqNo:4682; depth 27.10 2021-02-12T23:32:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172755.43 epoch sec; seqNo:4682; 4057238.15 north; 591125.28 east 2021-02-12T23:32:35Z- AHRS_M2 msg: 1613172755.82 epoch sec; seqNo:4636 2021-02-12T23:32:35Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-12T23:32:35Z- handleDepth()-Depth_Keller msg: 1613172755829 epoch sec; seqNo:4683; depth 26.98 2021-02-12T23:32:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172755.84 epoch sec; seqNo:4683; 4057238.53 north; 591125.28 east 2021-02-12T23:32:36Z- AHRS_M2 msg: 1613172756.22 epoch sec; seqNo:4637 2021-02-12T23:32:36Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:36Z- handleDepth()-Depth_Keller msg: 1613172756233 epoch sec; seqNo:4684; depth 26.86 2021-02-12T23:32:36Z- handleDvl() - RDI_Pathfinder msg: 1613172756.24 epoch sec; seqNo:2289 2021-02-12T23:32:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.59 , 81.24 , 0.00 , 62.47 2021-02-12T23:32:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , 0.03 2021-02-12T23:32:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172756.27 epoch sec; seqNo:4684; 4057238.53 north; 591125.28 east 2021-02-12T23:32:36Z- AHRS_M2 msg: 1613172756.63 epoch sec; seqNo:4638 2021-02-12T23:32:36Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:36Z- handleDepth()-Depth_Keller msg: 1613172756633 epoch sec; seqNo:4685; depth 26.73 2021-02-12T23:32:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172756.64 epoch sec; seqNo:4685; 4057238.91 north; 591125.27 east 2021-02-12T23:32:37Z- AHRS_M2 msg: 1613172757.03 epoch sec; seqNo:4639 2021-02-12T23:32:37Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.17 psi 2021-02-12T23:32:37Z- handleDepth()-Depth_Keller msg: 1613172757046 epoch sec; seqNo:4686; depth 26.60 2021-02-12T23:32:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172757.06 epoch sec; seqNo:4686; 4057239.29 north; 591125.27 east 2021-02-12T23:32:37Z- AHRS_M2 msg: 1613172757.44 epoch sec; seqNo:4640 2021-02-12T23:32:37Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-12T23:32:37Z- handleDepth()-Depth_Keller msg: 1613172757441 epoch sec; seqNo:4687; depth 26.47 2021-02-12T23:32:37Z- handleDvl() - RDI_Pathfinder msg: 1613172757.45 epoch sec; seqNo:2290 2021-02-12T23:32:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 85.00 , 96.90 , 61.30 2021-02-12T23:32:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.06 , 0.06 2021-02-12T23:32:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172757.48 epoch sec; seqNo:4687; 4057239.68 north; 591126.49 east 2021-02-12T23:32:37Z- AHRS_M2 msg: 1613172757.84 epoch sec; seqNo:4641 2021-02-12T23:32:37Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-12T23:32:37Z- handleDepth()-Depth_Keller msg: 1613172757849 epoch sec; seqNo:4688; depth 26.35 2021-02-12T23:32:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172757.87 epoch sec; seqNo:4688; 4057239.68 north; 591126.49 east 2021-02-12T23:32:38Z- AHRS_M2 msg: 1613172758.25 epoch sec; seqNo:4642 2021-02-12T23:32:38Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-12T23:32:38Z- handleDepth()-Depth_Keller msg: 1613172758273 epoch sec; seqNo:4689; depth 26.22 2021-02-12T23:32:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172758.30 epoch sec; seqNo:4689; 4057240.06 north; 591126.48 east 2021-02-12T23:32:38Z- AHRS_M2 msg: 1613172758.65 epoch sec; seqNo:4643 2021-02-12T23:32:38Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-12T23:32:38Z- handleDepth()-Depth_Keller msg: 1613172758657 epoch sec; seqNo:4690; depth 26.10 2021-02-12T23:32:38Z- handleDvl() - RDI_Pathfinder msg: 1613172758.67 epoch sec; seqNo:2291 2021-02-12T23:32:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 87.16 , 99.06 , 63.35 2021-02-12T23:32:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.09 2021-02-12T23:32:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172758.68 epoch sec; seqNo:4690; 4057240.44 north; 591126.48 east 2021-02-12T23:32:39Z- AHRS_M2 msg: 1613172759.06 epoch sec; seqNo:4644 2021-02-12T23:32:39Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:39Z- handleDepth()-Depth_Keller msg: 1613172759063 epoch sec; seqNo:4691; depth 25.98 2021-02-12T23:32:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172759.07 epoch sec; seqNo:4691; 4057240.82 north; 591126.47 east 2021-02-12T23:32:39Z- AHRS_M2 msg: 1613172759.46 epoch sec; seqNo:4645 2021-02-12T23:32:39Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:39Z- handleDepth()-Depth_Keller msg: 1613172759463 epoch sec; seqNo:4692; depth 25.86 2021-02-12T23:32:39Z- handleDvl() - RDI_Pathfinder msg: 1613172759.48 epoch sec; seqNo:2292 2021-02-12T23:32:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:32:39Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:32:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172759.49 epoch sec; seqNo:4692; 4057240.82 north; 591126.47 east 2021-02-12T23:32:39Z- AHRS_M2 msg: 1613172759.86 epoch sec; seqNo:4646 2021-02-12T23:32:39Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:39Z- handleDepth()-Depth_Keller msg: 1613172759864 epoch sec; seqNo:4693; depth 25.74 2021-02-12T23:32:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172759.88 epoch sec; seqNo:4693; 4057241.20 north; 591126.47 east 2021-02-12T23:32:39Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-12T23:32:39Z- waiting for 1613172757.62, time is 1613172759.88 2021-02-12T23:32:39Z- LcmTrn::updateTrn() >>>> pose time 1613172759.88, meas time 1613172759.48 <<<< 2021-02-12T23:32:39Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172759.48, ping # 0. 2021-02-12T23:32:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172759.48 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:32:39Z- TerrainNav::motionUpdate() - currEstimate = (4057241, 591126.47, 25.74) 2021-02-12T23:32:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:32:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:32:39Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:32:39Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:32:39Z- TNavBF::Projecting Measurements 2021-02-12T23:32:39Z- TNavFilter::Measurement from time = 1613172759.48 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:32:39Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:32:39Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:32:39Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:32:39Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:32:39Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:32:39Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:32:39Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:32:39Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 756.2 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:32:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:32:39Z- TNavBF::mmse selected from filter -1 2021-02-12T23:32:39Z- variances([0], [2], and psi_berg) = 756.147862 833.038984 0.000000 2021-02-12T23:32:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:32:39Z- LcmTrn::publishEstimates() - MLE : 4057226.70 591078.97 25.74 2021-02-12T23:32:39Z- LcmTrn::publishEstimates() - MMSE : 4057241.20 591126.47 25.74 2021-02-12T23:32:39Z- LcmTrn::publishEstimates() - COVAR: 756.15 833.04 1.07 0.00 2021-02-12T23:32:40Z- AHRS_M2 msg: 1613172760.27 epoch sec; seqNo:4647 2021-02-12T23:32:40Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:40Z- handleDepth()-Depth_Keller msg: 1613172760277 epoch sec; seqNo:4694; depth 25.62 2021-02-12T23:32:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172760.29 epoch sec; seqNo:4694; 4057241.59 north; 591127.69 east 2021-02-12T23:32:40Z- AHRS_M2 msg: 1613172760.67 epoch sec; seqNo:4648 2021-02-12T23:32:40Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:40Z- handleDepth()-Depth_Keller msg: 1613172760672 epoch sec; seqNo:4695; depth 25.50 2021-02-12T23:32:40Z- handleDvl() - RDI_Pathfinder msg: 1613172760.69 epoch sec; seqNo:2293 2021-02-12T23:32:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 86.56 , 96.02 , 63.76 2021-02-12T23:32:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , 0.02 2021-02-12T23:32:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172760.70 epoch sec; seqNo:4695; 4057241.97 north; 591127.68 east 2021-02-12T23:32:41Z- AHRS_M2 msg: 1613172761.07 epoch sec; seqNo:4649 2021-02-12T23:32:41Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:41Z- handleDepth()-Depth_Keller msg: 1613172761081 epoch sec; seqNo:4696; depth 25.37 2021-02-12T23:32:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172761.09 epoch sec; seqNo:4696; 4057241.97 north; 591127.68 east 2021-02-12T23:32:41Z- AHRS_M2 msg: 1613172761.48 epoch sec; seqNo:4650 2021-02-12T23:32:41Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:41Z- handleDepth()-Depth_Keller msg: 1613172761482 epoch sec; seqNo:4697; depth 25.26 2021-02-12T23:32:41Z- handleDvl() - RDI_Pathfinder msg: 1613172761.49 epoch sec; seqNo:2294 2021-02-12T23:32:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 0.00 , 99.06 , 63.35 2021-02-12T23:32:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , -0.03 2021-02-12T23:32:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172761.50 epoch sec; seqNo:4697; 4057242.35 north; 591127.68 east 2021-02-12T23:32:41Z- AHRS_M2 msg: 1613172761.88 epoch sec; seqNo:4651 2021-02-12T23:32:41Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:41Z- handleDepth()-Depth_Keller msg: 1613172761889 epoch sec; seqNo:4698; depth 25.12 2021-02-12T23:32:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172761.91 epoch sec; seqNo:4698; 4057242.72 north; 591127.68 east 2021-02-12T23:32:42Z- AHRS_M2 msg: 1613172762.30 epoch sec; seqNo:4652 2021-02-12T23:32:42Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.17 psi 2021-02-12T23:32:42Z- handleDepth()-Depth_Keller msg: 1613172762303 epoch sec; seqNo:4699; depth 25.01 2021-02-12T23:32:42Z- handleDvl() - RDI_Pathfinder msg: 1613172762.32 epoch sec; seqNo:2295 2021-02-12T23:32:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 86.08 , 99.06 , 64.16 2021-02-12T23:32:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , -0.06 2021-02-12T23:32:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172762.33 epoch sec; seqNo:4699; 4057242.72 north; 591127.68 east 2021-02-12T23:32:42Z- AHRS_M2 msg: 1613172762.69 epoch sec; seqNo:4653 2021-02-12T23:32:42Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.17 psi 2021-02-12T23:32:42Z- handleDepth()-Depth_Keller msg: 1613172762701 epoch sec; seqNo:4700; depth 24.88 2021-02-12T23:32:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172762.71 epoch sec; seqNo:4700; 4057243.12 north; 591128.89 east 2021-02-12T23:32:43Z- AHRS_M2 msg: 1613172763.09 epoch sec; seqNo:4654 2021-02-12T23:32:43Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.18 psi 2021-02-12T23:32:43Z- handleDepth()-Depth_Keller msg: 1613172763097 epoch sec; seqNo:4701; depth 24.77 2021-02-12T23:32:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172763.11 epoch sec; seqNo:4701; 4057243.50 north; 591128.89 east 2021-02-12T23:32:43Z- AHRS_M2 msg: 1613172763.50 epoch sec; seqNo:4655 2021-02-12T23:32:43Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.18 psi 2021-02-12T23:32:43Z- handleDepth()-Depth_Keller msg: 1613172763500 epoch sec; seqNo:4702; depth 24.65 2021-02-12T23:32:43Z- handleDvl() - RDI_Pathfinder msg: 1613172763.51 epoch sec; seqNo:2296 2021-02-12T23:32:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.52 , 86.01 , 101.16 , 64.16 2021-02-12T23:32:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , -0.05 2021-02-12T23:32:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172763.52 epoch sec; seqNo:4702; 4057243.50 north; 591128.89 east 2021-02-12T23:32:43Z- AHRS_M2 msg: 1613172763.90 epoch sec; seqNo:4656 2021-02-12T23:32:43Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.18 psi 2021-02-12T23:32:43Z- handleDepth()-Depth_Keller msg: 1613172763915 epoch sec; seqNo:4703; depth 24.52 2021-02-12T23:32:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172763.94 epoch sec; seqNo:4703; 4057243.87 north; 591128.88 east 2021-02-12T23:32:44Z- AHRS_M2 msg: 1613172764.31 epoch sec; seqNo:4657 2021-02-12T23:32:44Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.18 psi 2021-02-12T23:32:44Z- handleDepth()-Depth_Keller msg: 1613172764311 epoch sec; seqNo:4704; depth 24.40 2021-02-12T23:32:44Z- handleDvl() - RDI_Pathfinder msg: 1613172764.33 epoch sec; seqNo:2297 2021-02-12T23:32:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 88.18 , 99.00 , 64.16 2021-02-12T23:32:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.08 , -0.02 2021-02-12T23:32:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172764.34 epoch sec; seqNo:4704; 4057244.25 north; 591128.88 east 2021-02-12T23:32:44Z- AHRS_M2 msg: 1613172764.71 epoch sec; seqNo:4658 2021-02-12T23:32:44Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.18 psi 2021-02-12T23:32:44Z- handleDepth()-Depth_Keller msg: 1613172764712 epoch sec; seqNo:4705; depth 24.28 2021-02-12T23:32:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172764.72 epoch sec; seqNo:4705; 4057244.63 north; 591128.88 east 2021-02-12T23:32:45Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-12T23:32:45Z- waiting for 1613172762.88, time is 1613172764.72 2021-02-12T23:32:45Z- LcmTrn::updateTrn() >>>> pose time 1613172764.72, meas time 1613172764.33 <<<< 2021-02-12T23:32:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172764.33 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:32:45Z- TerrainNav::motionUpdate() - currEstimate = (4057245, 591128.88, 24.28) 2021-02-12T23:32:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:32:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:32:45Z- TNavBF::Projecting Measurements 2021-02-12T23:32:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:32:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:32:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:32:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:32:45Z- TNavFilter::4 good beams. 2021-02-12T23:32:45Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:32:45Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:32:45Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:32:45Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:32:45Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:32:45Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:32:45Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:32:45Z- TNavBF::mmse selected from filter -1 2021-02-12T23:32:45Z- TerrainMapOctree: (vehN, vehE ) = (4057244.35, 591128.68). 2021-02-12T23:32:45Z- TerrainMapOctree: Min Distance = 5078.17. 2021-02-12T23:32:45Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:32:45Z- There is no specific good beam that all particles have in common. 2021-02-12T23:32:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:32:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.115 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-12T23:32:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:32:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 12.44 2021-02-12T23:32:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:32:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 15.68 2021-02-12T23:32:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:32:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 6.59 2021-02-12T23:32:45Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:32:45Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:32:45Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:32:45Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:32:45Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:32:45Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:32:45Z- Filter 0 No beams used in subcloud update 2021-02-12T23:32:45Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:32:45Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:32:45Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 756.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:32:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172764.33 sec. 2021-02-12T23:32:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:32:45Z- TNavBF::mmse selected from filter -1 2021-02-12T23:32:45Z- variances([0], [2], and psi_berg) = 756.013192 833.038848 0.000000 2021-02-12T23:32:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:32:45Z- LcmTrn::publishEstimates() - MLE : 4057230.13 591081.38 24.28 2021-02-12T23:32:45Z- LcmTrn::publishEstimates() - MMSE : 4057244.63 591128.88 24.28 2021-02-12T23:32:45Z- LcmTrn::publishEstimates() - COVAR: 756.01 833.04 0.90 0.00 2021-02-12T23:32:45Z- AHRS_M2 msg: 1613172765.12 epoch sec; seqNo:4659 2021-02-12T23:32:45Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.18 psi 2021-02-12T23:32:45Z- handleDepth()-Depth_Keller msg: 1613172765120 epoch sec; seqNo:4706; depth 24.17 2021-02-12T23:32:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172765.13 epoch sec; seqNo:4706; 4057244.63 north; 591128.88 east 2021-02-12T23:32:45Z- AHRS_M2 msg: 1613172765.52 epoch sec; seqNo:4660 2021-02-12T23:32:45Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.18 psi 2021-02-12T23:32:45Z- handleDepth()-Depth_Keller msg: 1613172765521 epoch sec; seqNo:4707; depth 24.03 2021-02-12T23:32:45Z- handleDvl() - RDI_Pathfinder msg: 1613172765.54 epoch sec; seqNo:2298 2021-02-12T23:32:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 89.26 , 101.16 , 64.16 2021-02-12T23:32:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.07 , 0.03 2021-02-12T23:32:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172765.55 epoch sec; seqNo:4707; 4057245.02 north; 591130.09 east 2021-02-12T23:32:45Z- AHRS_M2 msg: 1613172765.92 epoch sec; seqNo:4661 2021-02-12T23:32:45Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.18 psi 2021-02-12T23:32:45Z- handleDepth()-Depth_Keller msg: 1613172765924 epoch sec; seqNo:4708; depth 23.91 2021-02-12T23:32:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172765.94 epoch sec; seqNo:4708; 4057245.40 north; 591130.09 east 2021-02-12T23:32:46Z- AHRS_M2 msg: 1613172766.32 epoch sec; seqNo:4662 2021-02-12T23:32:46Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.18 psi 2021-02-12T23:32:46Z- handleDepth()-Depth_Keller msg: 1613172766328 epoch sec; seqNo:4709; depth 23.78 2021-02-12T23:32:46Z- handleDvl() - RDI_Pathfinder msg: 1613172766.34 epoch sec; seqNo:2299 2021-02-12T23:32:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 90.27 , 102.17 , 64.57 2021-02-12T23:32:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , 0.06 2021-02-12T23:32:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172766.35 epoch sec; seqNo:4709; 4057245.40 north; 591130.09 east 2021-02-12T23:32:46Z- AHRS_M2 msg: 1613172766.73 epoch sec; seqNo:4663 2021-02-12T23:32:46Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.18 psi 2021-02-12T23:32:46Z- handleDepth()-Depth_Keller msg: 1613172766734 epoch sec; seqNo:4710; depth 23.66 2021-02-12T23:32:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172766.74 epoch sec; seqNo:4710; 4057245.78 north; 591130.08 east 2021-02-12T23:32:47Z- AHRS_M2 msg: 1613172767.14 epoch sec; seqNo:4664 2021-02-12T23:32:47Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.19 psi 2021-02-12T23:32:47Z- handleDepth()-Depth_Keller msg: 1613172767152 epoch sec; seqNo:4711; depth 23.54 2021-02-12T23:32:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172767.17 epoch sec; seqNo:4711; 4057246.16 north; 591130.08 east 2021-02-12T23:32:47Z- AHRS_M2 msg: 1613172767.54 epoch sec; seqNo:4665 2021-02-12T23:32:47Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.19 psi 2021-02-12T23:32:47Z- handleDepth()-Depth_Keller msg: 1613172767540 epoch sec; seqNo:4712; depth 23.42 2021-02-12T23:32:47Z- handleDvl() - RDI_Pathfinder msg: 1613172767.55 epoch sec; seqNo:2300 2021-02-12T23:32:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.52 , 90.21 , 99.94 , 65.39 2021-02-12T23:32:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , 0.08 2021-02-12T23:32:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172767.57 epoch sec; seqNo:4712; 4057246.54 north; 591130.08 east 2021-02-12T23:32:47Z- AHRS_M2 msg: 1613172767.94 epoch sec; seqNo:4666 2021-02-12T23:32:47Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.19 psi 2021-02-12T23:32:47Z- handleDepth()-Depth_Keller msg: 1613172767949 epoch sec; seqNo:4713; depth 23.29 2021-02-12T23:32:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172767.96 epoch sec; seqNo:4713; 4057246.54 north; 591130.08 east 2021-02-12T23:32:48Z- AHRS_M2 msg: 1613172768.34 epoch sec; seqNo:4667 2021-02-12T23:32:48Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.19 psi 2021-02-12T23:32:48Z- handleDepth()-Depth_Keller msg: 1613172768351 epoch sec; seqNo:4714; depth 23.17 2021-02-12T23:32:48Z- handleDvl() - RDI_Pathfinder msg: 1613172768.36 epoch sec; seqNo:2301 2021-02-12T23:32:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.52 , 89.12 , 102.11 , 65.39 2021-02-12T23:32:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.10 , 0.08 2021-02-12T23:32:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172768.37 epoch sec; seqNo:4714; 4057246.93 north; 591131.29 east 2021-02-12T23:32:48Z- AHRS_M2 msg: 1613172768.75 epoch sec; seqNo:4668 2021-02-12T23:32:48Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.19 psi 2021-02-12T23:32:48Z- handleDepth()-Depth_Keller msg: 1613172768752 epoch sec; seqNo:4715; depth 23.05 2021-02-12T23:32:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172768.77 epoch sec; seqNo:4715; 4057247.31 north; 591131.29 east 2021-02-12T23:32:49Z- handleDepth()-Depth_Keller msg: 1613172769173 epoch sec; seqNo:4716; depth 22.93 2021-02-12T23:32:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172769.19 epoch sec; seqNo:4716; 4057247.69 north; 591131.29 east 2021-02-12T23:32:49Z- AHRS_M2 msg: 1613172769.57 epoch sec; seqNo:4669 2021-02-12T23:32:49Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.18 psi 2021-02-12T23:32:49Z- handleDepth()-Depth_Keller msg: 1613172769574 epoch sec; seqNo:4717; depth 22.80 2021-02-12T23:32:49Z- handleDvl() - RDI_Pathfinder msg: 1613172769.60 epoch sec; seqNo:2302 2021-02-12T23:32:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 89.12 , 96.70 , 66.21 2021-02-12T23:32:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , 0.08 2021-02-12T23:32:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172769.62 epoch sec; seqNo:4717; 4057248.07 north; 591131.28 east 2021-02-12T23:32:49Z- AHRS_M2 msg: 1613172769.96 epoch sec; seqNo:4670 2021-02-12T23:32:49Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.18 psi 2021-02-12T23:32:49Z- handleDepth()-Depth_Keller msg: 1613172769978 epoch sec; seqNo:4718; depth 22.67 2021-02-12T23:32:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172769.99 epoch sec; seqNo:4718; 4057248.07 north; 591131.28 east 2021-02-12T23:32:50Z- LcmTrn::handleMessages() - handling 43 messages took 0.68 ms and 130 calls... 2021-02-12T23:32:50Z- waiting for 1613172767.72, time is 1613172769.99 2021-02-12T23:32:50Z- LcmTrn::updateTrn() >>>> pose time 1613172769.99, meas time 1613172769.60 <<<< 2021-02-12T23:32:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172769.60 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:32:50Z- TerrainNav::motionUpdate() - currEstimate = (4057248, 591131.28, 22.67) 2021-02-12T23:32:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:32:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:32:50Z- TNavBF::Projecting Measurements 2021-02-12T23:32:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:32:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:32:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:32:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:32:50Z- TNavFilter::4 good beams. 2021-02-12T23:32:50Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:32:50Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:32:50Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:32:50Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:32:50Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:32:50Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:32:50Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:32:50Z- TNavBF::mmse selected from filter -1 2021-02-12T23:32:50Z- TerrainMapOctree: (vehN, vehE ) = (4057247.81, 591131.10). 2021-02-12T23:32:50Z- TerrainMapOctree: Min Distance = 5073.96. 2021-02-12T23:32:50Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:32:50Z- There is no specific good beam that all particles have in common. 2021-02-12T23:32:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:32:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 8.47 2021-02-12T23:32:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:32:50Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 12.71 2021-02-12T23:32:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:32:50Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 14.96 2021-02-12T23:32:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:32:50Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 7.01 2021-02-12T23:32:50Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:32:50Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:32:50Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:32:50Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:32:50Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:32:50Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:32:50Z- Filter 0 No beams used in subcloud update 2021-02-12T23:32:50Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:32:50Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:32:50Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 755.9 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:32:50Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172769.60 sec. 2021-02-12T23:32:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:32:50Z- TNavBF::mmse selected from filter -1 2021-02-12T23:32:50Z- variances([0], [2], and psi_berg) = 755.879738 833.038714 0.000000 2021-02-12T23:32:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:32:50Z- LcmTrn::publishEstimates() - MLE : 4057233.57 591083.78 22.67 2021-02-12T23:32:50Z- LcmTrn::publishEstimates() - MMSE : 4057248.07 591131.28 22.67 2021-02-12T23:32:50Z- LcmTrn::publishEstimates() - COVAR: 755.88 833.04 0.81 0.00 2021-02-12T23:32:50Z- AHRS_M2 msg: 1613172770.36 epoch sec; seqNo:4671 2021-02-12T23:32:50Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-12T23:32:50Z- handleDepth()-Depth_Keller msg: 1613172770368 epoch sec; seqNo:4719; depth 22.56 2021-02-12T23:32:50Z- handleDvl() - RDI_Pathfinder msg: 1613172770.38 epoch sec; seqNo:2303 2021-02-12T23:32:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.97 , 90.27 , 103.25 , 65.80 2021-02-12T23:32:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.01 , 0.04 2021-02-12T23:32:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172770.40 epoch sec; seqNo:4719; 4057248.45 north; 591131.28 east 2021-02-12T23:32:50Z- AHRS_M2 msg: 1613172770.77 epoch sec; seqNo:4672 2021-02-12T23:32:50Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-12T23:32:50Z- handleDepth()-Depth_Keller msg: 1613172770772 epoch sec; seqNo:4720; depth 22.44 2021-02-12T23:32:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172770.78 epoch sec; seqNo:4720; 4057248.83 north; 591131.27 east 2021-02-12T23:32:51Z- AHRS_M2 msg: 1613172771.17 epoch sec; seqNo:4673 2021-02-12T23:32:51Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:51Z- handleDepth()-Depth_Keller msg: 1613172771179 epoch sec; seqNo:4721; depth 22.32 2021-02-12T23:32:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172771.19 epoch sec; seqNo:4721; 4057249.22 north; 591132.49 east 2021-02-12T23:32:51Z- AHRS_M2 msg: 1613172771.58 epoch sec; seqNo:4674 2021-02-12T23:32:51Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:51Z- handleDepth()-Depth_Keller msg: 1613172771596 epoch sec; seqNo:4722; depth 22.19 2021-02-12T23:32:51Z- handleDvl() - RDI_Pathfinder msg: 1613172771.61 epoch sec; seqNo:2304 2021-02-12T23:32:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.79 , 89.80 , 97.10 , 67.03 2021-02-12T23:32:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.03 , -0.02 2021-02-12T23:32:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172771.62 epoch sec; seqNo:4722; 4057249.60 north; 591132.49 east 2021-02-12T23:32:51Z- AHRS_M2 msg: 1613172771.98 epoch sec; seqNo:4675 2021-02-12T23:32:51Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:51Z- handleDepth()-Depth_Keller msg: 1613172771988 epoch sec; seqNo:4723; depth 22.06 2021-02-12T23:32:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172772.00 epoch sec; seqNo:4723; 4057249.60 north; 591132.49 east 2021-02-12T23:32:52Z- AHRS_M2 msg: 1613172772.38 epoch sec; seqNo:4676 2021-02-12T23:32:52Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:52Z- handleDepth()-Depth_Keller msg: 1613172772393 epoch sec; seqNo:4724; depth 21.95 2021-02-12T23:32:52Z- handleDvl() - RDI_Pathfinder msg: 1613172772.40 epoch sec; seqNo:2305 2021-02-12T23:32:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.38 , 89.12 , 104.27 , 66.62 2021-02-12T23:32:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , -0.08 2021-02-12T23:32:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172772.41 epoch sec; seqNo:4724; 4057249.97 north; 591132.48 east 2021-02-12T23:32:52Z- AHRS_M2 msg: 1613172772.79 epoch sec; seqNo:4677 2021-02-12T23:32:52Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:52Z- handleDepth()-Depth_Keller msg: 1613172772801 epoch sec; seqNo:4725; depth 21.84 2021-02-12T23:32:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172772.81 epoch sec; seqNo:4725; 4057250.35 north; 591132.48 east 2021-02-12T23:32:53Z- AHRS_M2 msg: 1613172773.19 epoch sec; seqNo:4678 2021-02-12T23:32:53Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:53Z- handleDepth()-Depth_Keller msg: 1613172773197 epoch sec; seqNo:4726; depth 21.71 2021-02-12T23:32:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172773.21 epoch sec; seqNo:4726; 4057250.35 north; 591132.48 east 2021-02-12T23:32:53Z- AHRS_M2 msg: 1613172773.60 epoch sec; seqNo:4679 2021-02-12T23:32:53Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-12T23:32:53Z- handleDepth()-Depth_Keller msg: 1613172773602 epoch sec; seqNo:4727; depth 21.59 2021-02-12T23:32:53Z- handleDvl() - RDI_Pathfinder msg: 1613172773.61 epoch sec; seqNo:2306 2021-02-12T23:32:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.20 , 89.80 , 104.40 , 67.43 2021-02-12T23:32:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.07 , -0.08 2021-02-12T23:32:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172773.62 epoch sec; seqNo:4727; 4057250.75 north; 591133.70 east 2021-02-12T23:32:54Z- AHRS_M2 msg: 1613172774.00 epoch sec; seqNo:4680 2021-02-12T23:32:54Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.16 psi 2021-02-12T23:32:54Z- handleDepth()-Depth_Keller msg: 1613172774013 epoch sec; seqNo:4728; depth 21.47 2021-02-12T23:32:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172774.02 epoch sec; seqNo:4728; 4057251.12 north; 591133.69 east 2021-02-12T23:32:54Z- AHRS_M2 msg: 1613172774.40 epoch sec; seqNo:4681 2021-02-12T23:32:54Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-12T23:32:54Z- handleDepth()-Depth_Keller msg: 1613172774410 epoch sec; seqNo:4729; depth 21.35 2021-02-12T23:32:54Z- handleDvl() - RDI_Pathfinder msg: 1613172774.42 epoch sec; seqNo:2307 2021-02-12T23:32:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 89.73 , 97.03 , 65.80 2021-02-12T23:32:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.12 , -0.02 2021-02-12T23:32:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172774.43 epoch sec; seqNo:4729; 4057251.12 north; 591133.69 east 2021-02-12T23:32:54Z- AHRS_M2 msg: 1613172774.81 epoch sec; seqNo:4682 2021-02-12T23:32:54Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-12T23:32:54Z- handleDepth()-Depth_Keller msg: 1613172774812 epoch sec; seqNo:4730; depth 21.23 2021-02-12T23:32:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172774.83 epoch sec; seqNo:4730; 4057251.50 north; 591133.69 east 2021-02-12T23:32:55Z- AHRS_M2 msg: 1613172775.21 epoch sec; seqNo:4683 2021-02-12T23:32:55Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-12T23:32:55Z- handleDepth()-Depth_Keller msg: 1613172775216 epoch sec; seqNo:4731; depth 21.12 2021-02-12T23:32:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172775.22 epoch sec; seqNo:4731; 4057251.88 north; 591133.68 east 2021-02-12T23:32:55Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-12T23:32:55Z- waiting for 1613172772.99, time is 1613172775.22 2021-02-12T23:32:55Z- LcmTrn::updateTrn() >>>> pose time 1613172775.22, meas time 1613172774.42 <<<< 2021-02-12T23:32:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172774.42 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:32:55Z- TerrainNav::motionUpdate() - currEstimate = (4057252, 591133.68, 21.12) 2021-02-12T23:32:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:32:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:32:55Z- TNavBF::Projecting Measurements 2021-02-12T23:32:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:32:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:32:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:32:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:32:55Z- TNavFilter::4 good beams. 2021-02-12T23:32:55Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:32:55Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:32:55Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:32:55Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:32:55Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:32:55Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:32:55Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:32:55Z- TNavBF::mmse selected from filter -1 2021-02-12T23:32:55Z- TerrainMapOctree: (vehN, vehE ) = (4057251.30, 591133.31). 2021-02-12T23:32:55Z- TerrainMapOctree: Min Distance = 5069.85. 2021-02-12T23:32:55Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:32:55Z- There is no specific good beam that all particles have in common. 2021-02-12T23:32:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:32:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 9.15 2021-02-12T23:32:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:32:55Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 12.88 2021-02-12T23:32:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:32:55Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 15.06 2021-02-12T23:32:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:32:55Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 6.93 2021-02-12T23:32:55Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:32:55Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:32:55Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:32:55Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:32:55Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:32:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:32:55Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:32:55Z- Filter 0 No beams used in subcloud update 2021-02-12T23:32:55Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:32:55Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:32:55Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 755.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:32:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172774.42 sec. 2021-02-12T23:32:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:32:55Z- TNavBF::mmse selected from filter -1 2021-02-12T23:32:55Z- variances([0], [2], and psi_berg) = 755.733019 833.038567 0.000000 2021-02-12T23:32:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:32:55Z- LcmTrn::publishEstimates() - MLE : 4057237.38 591086.18 21.12 2021-02-12T23:32:55Z- LcmTrn::publishEstimates() - MMSE : 4057251.88 591133.68 21.12 2021-02-12T23:32:55Z- LcmTrn::publishEstimates() - COVAR: 755.73 833.04 0.79 0.00 2021-02-12T23:32:55Z- AHRS_M2 msg: 1613172775.62 epoch sec; seqNo:4684 2021-02-12T23:32:55Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.16 psi 2021-02-12T23:32:55Z- handleDepth()-Depth_Keller msg: 1613172775620 epoch sec; seqNo:4732; depth 20.98 2021-02-12T23:32:55Z- handleDvl() - RDI_Pathfinder msg: 1613172775.64 epoch sec; seqNo:2308 2021-02-12T23:32:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.79 , 91.29 , 106.43 , 67.03 2021-02-12T23:32:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.11 , 0.05 2021-02-12T23:32:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172775.65 epoch sec; seqNo:4732; 4057251.88 north; 591133.68 east 2021-02-12T23:32:56Z- AHRS_M2 msg: 1613172776.04 epoch sec; seqNo:4685 2021-02-12T23:32:56Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.16 psi 2021-02-12T23:32:56Z- handleDepth()-Depth_Keller msg: 1613172776040 epoch sec; seqNo:4733; depth 20.86 2021-02-12T23:32:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172776.05 epoch sec; seqNo:4733; 4057252.26 north; 591133.68 east 2021-02-12T23:32:56Z- AHRS_M2 msg: 1613172776.43 epoch sec; seqNo:4686 2021-02-12T23:32:56Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-12T23:32:56Z- handleDepth()-Depth_Keller msg: 1613172776438 epoch sec; seqNo:4734; depth 20.73 2021-02-12T23:32:56Z- handleDvl() - RDI_Pathfinder msg: 1613172776.45 epoch sec; seqNo:2309 2021-02-12T23:32:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:32:56Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:32:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172776.46 epoch sec; seqNo:4734; 4057252.64 north; 591133.68 east 2021-02-12T23:32:56Z- AHRS_M2 msg: 1613172776.83 epoch sec; seqNo:4687 2021-02-12T23:32:56Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-12T23:32:56Z- handleDepth()-Depth_Keller msg: 1613172776832 epoch sec; seqNo:4735; depth 20.61 2021-02-12T23:32:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172776.84 epoch sec; seqNo:4735; 4057253.02 north; 591133.67 east 2021-02-12T23:32:57Z- AHRS_M2 msg: 1613172777.24 epoch sec; seqNo:4688 2021-02-12T23:32:57Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:57Z- handleDepth()-Depth_Keller msg: 1613172777244 epoch sec; seqNo:4736; depth 20.49 2021-02-12T23:32:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172777.25 epoch sec; seqNo:4736; 4057253.03 north; 591134.89 east 2021-02-12T23:32:57Z- AHRS_M2 msg: 1613172777.63 epoch sec; seqNo:4689 2021-02-12T23:32:57Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:57Z- handleDepth()-Depth_Keller msg: 1613172777641 epoch sec; seqNo:4737; depth 20.38 2021-02-12T23:32:57Z- handleDvl() - RDI_Pathfinder msg: 1613172777.66 epoch sec; seqNo:2310 2021-02-12T23:32:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.71 , 90.88 , 105.76 , 68.66 2021-02-12T23:32:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.10 2021-02-12T23:32:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172777.67 epoch sec; seqNo:4737; 4057253.41 north; 591134.89 east 2021-02-12T23:32:58Z- AHRS_M2 msg: 1613172778.04 epoch sec; seqNo:4690 2021-02-12T23:32:58Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:58Z- handleDepth()-Depth_Keller msg: 1613172778055 epoch sec; seqNo:4738; depth 20.25 2021-02-12T23:32:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172778.07 epoch sec; seqNo:4738; 4057253.79 north; 591134.88 east 2021-02-12T23:32:58Z- AHRS_M2 msg: 1613172778.44 epoch sec; seqNo:4691 2021-02-12T23:32:58Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:58Z- handleDepth()-Depth_Keller msg: 1613172778450 epoch sec; seqNo:4739; depth 20.13 2021-02-12T23:32:58Z- handleDvl() - RDI_Pathfinder msg: 1613172778.46 epoch sec; seqNo:2311 2021-02-12T23:32:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.02 , 90.81 , 104.20 , 68.66 2021-02-12T23:32:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , 0.07 2021-02-12T23:32:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172778.47 epoch sec; seqNo:4739; 4057254.17 north; 591134.88 east 2021-02-12T23:32:58Z- AHRS_M2 msg: 1613172778.85 epoch sec; seqNo:4692 2021-02-12T23:32:58Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:58Z- handleDepth()-Depth_Keller msg: 1613172778857 epoch sec; seqNo:4740; depth 19.99 2021-02-12T23:32:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172778.87 epoch sec; seqNo:4740; 4057254.17 north; 591134.88 east 2021-02-12T23:32:59Z- AHRS_M2 msg: 1613172779.27 epoch sec; seqNo:4693 2021-02-12T23:32:59Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:59Z- handleDepth()-Depth_Keller msg: 1613172779279 epoch sec; seqNo:4741; depth 19.89 2021-02-12T23:32:59Z- handleDvl() - RDI_Pathfinder msg: 1613172779.30 epoch sec; seqNo:2312 2021-02-12T23:32:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 92.10 , 104.27 , 67.84 2021-02-12T23:32:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.04 2021-02-12T23:32:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172779.31 epoch sec; seqNo:4741; 4057254.55 north; 591134.88 east 2021-02-12T23:32:59Z- AHRS_M2 msg: 1613172779.66 epoch sec; seqNo:4694 2021-02-12T23:32:59Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-12T23:32:59Z- handleDepth()-Depth_Keller msg: 1613172779662 epoch sec; seqNo:4742; depth 19.74 2021-02-12T23:32:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172779.67 epoch sec; seqNo:4742; 4057254.94 north; 591136.09 east 2021-02-12T23:33:00Z- AHRS_M2 msg: 1613172780.06 epoch sec; seqNo:4695 2021-02-12T23:33:00Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-12T23:33:00Z- handleDepth()-Depth_Keller msg: 1613172780064 epoch sec; seqNo:4743; depth 19.65 2021-02-12T23:33:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172780.09 epoch sec; seqNo:4743; 4057255.32 north; 591136.09 east 2021-02-12T23:33:00Z- LcmTrn::handleMessages() - handling 41 messages took 0.66 ms and 128 calls... 2021-02-12T23:33:00Z- waiting for 1613172778.22, time is 1613172780.09 2021-02-12T23:33:00Z- LcmTrn::updateTrn() >>>> pose time 1613172780.09, meas time 1613172779.30 <<<< 2021-02-12T23:33:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172779.30 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:33:00Z- TerrainNav::motionUpdate() - currEstimate = (4057255, 591136.09, 19.65) 2021-02-12T23:33:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:33:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:33:00Z- TNavBF::Projecting Measurements 2021-02-12T23:33:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:33:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:33:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:33:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:33:00Z- TNavFilter::4 good beams. 2021-02-12T23:33:00Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:33:00Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:33:00Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:33:00Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:33:00Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:33:00Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:33:00Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:33:00Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:00Z- TerrainMapOctree: (vehN, vehE ) = (4057254.76, 591135.70). 2021-02-12T23:33:00Z- TerrainMapOctree: Min Distance = 5065.65. 2021-02-12T23:33:00Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:33:00Z- There is no specific good beam that all particles have in common. 2021-02-12T23:33:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:33:00Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 9.45 2021-02-12T23:33:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:33:00Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 13.57 2021-02-12T23:33:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:33:00Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 17.39 2021-02-12T23:33:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:33:00Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 7.36 2021-02-12T23:33:00Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:33:00Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:33:00Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:33:00Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:33:00Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:33:00Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:33:00Z- Filter 0 No beams used in subcloud update 2021-02-12T23:33:00Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:33:00Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:33:00Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 755.6 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:33:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172779.30 sec. 2021-02-12T23:33:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:33:00Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:00Z- variances([0], [2], and psi_berg) = 755.591124 833.038425 0.000000 2021-02-12T23:33:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:33:00Z- LcmTrn::publishEstimates() - MLE : 4057239.82 591087.59 19.65 2021-02-12T23:33:00Z- LcmTrn::publishEstimates() - MMSE : 4057255.32 591136.09 19.65 2021-02-12T23:33:00Z- LcmTrn::publishEstimates() - COVAR: 755.59 833.04 0.77 0.00 2021-02-12T23:33:00Z- AHRS_M2 msg: 1613172780.46 epoch sec; seqNo:4696 2021-02-12T23:33:00Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-12T23:33:00Z- handleDepth()-Depth_Keller msg: 1613172780468 epoch sec; seqNo:4744; depth 19.51 2021-02-12T23:33:00Z- handleDvl() - RDI_Pathfinder msg: 1613172780.48 epoch sec; seqNo:2313 2021-02-12T23:33:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.71 , 92.10 , 106.70 , 69.07 2021-02-12T23:33:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , 0.01 2021-02-12T23:33:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172780.50 epoch sec; seqNo:4744; 4057255.32 north; 591136.09 east 2021-02-12T23:33:00Z- AHRS_M2 msg: 1613172780.87 epoch sec; seqNo:4697 2021-02-12T23:33:00Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-12T23:33:00Z- handleDepth()-Depth_Keller msg: 1613172780872 epoch sec; seqNo:4745; depth 19.40 2021-02-12T23:33:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172780.88 epoch sec; seqNo:4745; 4057255.70 north; 591136.08 east 2021-02-12T23:33:01Z- AHRS_M2 msg: 1613172781.28 epoch sec; seqNo:4698 2021-02-12T23:33:01Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-12T23:33:01Z- handleDepth()-Depth_Keller msg: 1613172781281 epoch sec; seqNo:4746; depth 19.26 2021-02-12T23:33:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172781.30 epoch sec; seqNo:4746; 4057256.08 north; 591136.08 east 2021-02-12T23:33:01Z- AHRS_M2 msg: 1613172781.68 epoch sec; seqNo:4699 2021-02-12T23:33:01Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-12T23:33:01Z- handleDepth()-Depth_Keller msg: 1613172781685 epoch sec; seqNo:4747; depth 19.16 2021-02-12T23:33:01Z- handleDvl() - RDI_Pathfinder msg: 1613172781.70 epoch sec; seqNo:2314 2021-02-12T23:33:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.65 , 91.96 , 105.35 , 69.07 2021-02-12T23:33:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , -0.01 2021-02-12T23:33:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172781.71 epoch sec; seqNo:4747; 4057256.45 north; 591136.08 east 2021-02-12T23:33:02Z- AHRS_M2 msg: 1613172782.08 epoch sec; seqNo:4700 2021-02-12T23:33:02Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-12T23:33:02Z- handleDepth()-Depth_Keller msg: 1613172782089 epoch sec; seqNo:4748; depth 19.03 2021-02-12T23:33:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172782.10 epoch sec; seqNo:4748; 4057256.47 north; 591137.30 east 2021-02-12T23:33:02Z- AHRS_M2 msg: 1613172782.48 epoch sec; seqNo:4701 2021-02-12T23:33:02Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.16 psi 2021-02-12T23:33:02Z- handleDepth()-Depth_Keller msg: 1613172782497 epoch sec; seqNo:4749; depth 18.91 2021-02-12T23:33:02Z- handleDvl() - RDI_Pathfinder msg: 1613172782.51 epoch sec; seqNo:2315 2021-02-12T23:33:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.64 , 92.03 , 106.63 , 69.07 2021-02-12T23:33:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.04 2021-02-12T23:33:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172782.52 epoch sec; seqNo:4749; 4057256.85 north; 591137.29 east 2021-02-12T23:33:02Z- AHRS_M2 msg: 1613172782.89 epoch sec; seqNo:4702 2021-02-12T23:33:02Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.16 psi 2021-02-12T23:33:02Z- handleDepth()-Depth_Keller msg: 1613172782894 epoch sec; seqNo:4750; depth 18.78 2021-02-12T23:33:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172782.91 epoch sec; seqNo:4750; 4057257.22 north; 591137.29 east 2021-02-12T23:33:03Z- AHRS_M2 msg: 1613172783.31 epoch sec; seqNo:4703 2021-02-12T23:33:03Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.16 psi 2021-02-12T23:33:03Z- handleDepth()-Depth_Keller msg: 1613172783345 epoch sec; seqNo:4751; depth 18.68 2021-02-12T23:33:03Z- handleDvl() - RDI_Pathfinder msg: 1613172783.37 epoch sec; seqNo:2316 2021-02-12T23:33:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.65 , 94.40 , 106.57 , 69.07 2021-02-12T23:33:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , -0.03 2021-02-12T23:33:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172783.39 epoch sec; seqNo:4751; 4057257.60 north; 591137.29 east 2021-02-12T23:33:03Z- AHRS_M2 msg: 1613172783.69 epoch sec; seqNo:4704 2021-02-12T23:33:03Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-12T23:33:03Z- handleDepth()-Depth_Keller msg: 1613172783703 epoch sec; seqNo:4752; depth 18.52 2021-02-12T23:33:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172783.71 epoch sec; seqNo:4752; 4057257.60 north; 591137.29 east 2021-02-12T23:33:04Z- AHRS_M2 msg: 1613172784.10 epoch sec; seqNo:4705 2021-02-12T23:33:04Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-12T23:33:04Z- handleDepth()-Depth_Keller msg: 1613172784104 epoch sec; seqNo:4753; depth 18.42 2021-02-12T23:33:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172784.12 epoch sec; seqNo:4753; 4057257.98 north; 591137.28 east 2021-02-12T23:33:04Z- AHRS_M2 msg: 1613172784.50 epoch sec; seqNo:4706 2021-02-12T23:33:04Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-12T23:33:04Z- handleDepth()-Depth_Keller msg: 1613172784513 epoch sec; seqNo:4754; depth 18.30 2021-02-12T23:33:04Z- handleDvl() - RDI_Pathfinder msg: 1613172784.53 epoch sec; seqNo:2317 2021-02-12T23:33:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 94.40 , 106.57 , 69.89 2021-02-12T23:33:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.07 , -0.01 2021-02-12T23:33:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172784.54 epoch sec; seqNo:4754; 4057258.36 north; 591137.28 east 2021-02-12T23:33:04Z- AHRS_M2 msg: 1613172784.91 epoch sec; seqNo:4707 2021-02-12T23:33:04Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.18 psi 2021-02-12T23:33:04Z- handleDepth()-Depth_Keller msg: 1613172784913 epoch sec; seqNo:4755; depth 18.17 2021-02-12T23:33:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172784.92 epoch sec; seqNo:4755; 4057258.37 north; 591138.50 east 2021-02-12T23:33:05Z- AHRS_M2 msg: 1613172785.31 epoch sec; seqNo:4708 2021-02-12T23:33:05Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.18 psi 2021-02-12T23:33:05Z- handleDepth()-Depth_Keller msg: 1613172785316 epoch sec; seqNo:4756; depth 18.06 2021-02-12T23:33:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172785.34 epoch sec; seqNo:4756; 4057258.75 north; 591138.49 east 2021-02-12T23:33:05Z- LcmTrn::handleMessages() - handling 44 messages took 0.77 ms and 129 calls... 2021-02-12T23:33:05Z- waiting for 1613172783.09, time is 1613172785.34 2021-02-12T23:33:05Z- LcmTrn::updateTrn() >>>> pose time 1613172785.34, meas time 1613172784.53 <<<< 2021-02-12T23:33:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172784.53 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:33:05Z- TerrainNav::motionUpdate() - currEstimate = (4057259, 591138.49, 18.06) 2021-02-12T23:33:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:33:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:33:05Z- TNavBF::Projecting Measurements 2021-02-12T23:33:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:33:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:33:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:33:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:33:05Z- TNavFilter::4 good beams. 2021-02-12T23:33:05Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:33:05Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:33:05Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:33:05Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:33:05Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:33:05Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:33:05Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:33:05Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:05Z- TerrainMapOctree: (vehN, vehE ) = (4057258.22, 591138.12). 2021-02-12T23:33:05Z- TerrainMapOctree: Min Distance = 5061.43. 2021-02-12T23:33:05Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:33:05Z- There is no specific good beam that all particles have in common. 2021-02-12T23:33:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:33:05Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-12T23:33:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:33:05Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 14.26 2021-02-12T23:33:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:33:05Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 18.17 2021-02-12T23:33:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:33:05Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 7.81 2021-02-12T23:33:05Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:33:05Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:33:05Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:33:05Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:33:05Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:33:05Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:33:05Z- Filter 0 No beams used in subcloud update 2021-02-12T23:33:05Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:33:05Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:33:05Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 755.5 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:33:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172784.53 sec. 2021-02-12T23:33:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:33:05Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:05Z- variances([0], [2], and psi_berg) = 755.449984 833.038284 0.000000 2021-02-12T23:33:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:33:05Z- LcmTrn::publishEstimates() - MLE : 4057243.25 591089.99 18.06 2021-02-12T23:33:05Z- LcmTrn::publishEstimates() - MMSE : 4057258.75 591138.49 18.06 2021-02-12T23:33:05Z- LcmTrn::publishEstimates() - COVAR: 755.45 833.04 0.77 0.00 2021-02-12T23:33:05Z- AHRS_M2 msg: 1613172785.71 epoch sec; seqNo:4709 2021-02-12T23:33:05Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-12T23:33:05Z- handleDepth()-Depth_Keller msg: 1613172785732 epoch sec; seqNo:4757; depth 17.94 2021-02-12T23:33:05Z- handleDvl() - RDI_Pathfinder msg: 1613172785.77 epoch sec; seqNo:2318 2021-02-12T23:33:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 95.55 , 108.93 , 70.70 2021-02-12T23:33:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.02 2021-02-12T23:33:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172785.78 epoch sec; seqNo:4757; 4057259.13 north; 591138.49 east 2021-02-12T23:33:06Z- AHRS_M2 msg: 1613172786.12 epoch sec; seqNo:4710 2021-02-12T23:33:06Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-12T23:33:06Z- handleDepth()-Depth_Keller msg: 1613172786133 epoch sec; seqNo:4758; depth 17.82 2021-02-12T23:33:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172786.15 epoch sec; seqNo:4758; 4057259.51 north; 591138.49 east 2021-02-12T23:33:06Z- AHRS_M2 msg: 1613172786.52 epoch sec; seqNo:4711 2021-02-12T23:33:06Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-12T23:33:06Z- handleDepth()-Depth_Keller msg: 1613172786536 epoch sec; seqNo:4759; depth 17.69 2021-02-12T23:33:06Z- handleDvl() - RDI_Pathfinder msg: 1613172786.55 epoch sec; seqNo:2319 2021-02-12T23:33:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.66 , 94.26 , 107.65 , 70.70 2021-02-12T23:33:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.01 2021-02-12T23:33:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172786.56 epoch sec; seqNo:4759; 4057259.51 north; 591138.49 east 2021-02-12T23:33:06Z- AHRS_M2 msg: 1613172786.93 epoch sec; seqNo:4712 2021-02-12T23:33:06Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-12T23:33:06Z- handleDepth()-Depth_Keller msg: 1613172786932 epoch sec; seqNo:4760; depth 17.57 2021-02-12T23:33:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172786.95 epoch sec; seqNo:4760; 4057259.89 north; 591138.48 east 2021-02-12T23:33:07Z- AHRS_M2 msg: 1613172787.33 epoch sec; seqNo:4713 2021-02-12T23:33:07Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.14 psi 2021-02-12T23:33:07Z- handleDepth()-Depth_Keller msg: 1613172787336 epoch sec; seqNo:4761; depth 17.45 2021-02-12T23:33:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172787.35 epoch sec; seqNo:4761; 4057260.27 north; 591138.48 east 2021-02-12T23:33:07Z- AHRS_M2 msg: 1613172787.74 epoch sec; seqNo:4714 2021-02-12T23:33:07Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.13 psi 2021-02-12T23:33:07Z- handleDepth()-Depth_Keller msg: 1613172787745 epoch sec; seqNo:4762; depth 17.35 2021-02-12T23:33:07Z- handleDvl() - RDI_Pathfinder msg: 1613172787.76 epoch sec; seqNo:2320 2021-02-12T23:33:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.88 , 93.04 , 111.30 , 71.11 2021-02-12T23:33:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.01 , -0.03 2021-02-12T23:33:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172787.77 epoch sec; seqNo:4762; 4057260.28 north; 591139.70 east 2021-02-12T23:33:08Z- AHRS_M2 msg: 1613172788.14 epoch sec; seqNo:4715 2021-02-12T23:33:08Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.13 psi 2021-02-12T23:33:08Z- handleDepth()-Depth_Keller msg: 1613172788149 epoch sec; seqNo:4763; depth 17.22 2021-02-12T23:33:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172788.16 epoch sec; seqNo:4763; 4057260.66 north; 591139.70 east 2021-02-12T23:33:08Z- AHRS_M2 msg: 1613172788.54 epoch sec; seqNo:4716 2021-02-12T23:33:08Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.13 psi 2021-02-12T23:33:08Z- handleDepth()-Depth_Keller msg: 1613172788553 epoch sec; seqNo:4764; depth 17.11 2021-02-12T23:33:08Z- handleDvl() - RDI_Pathfinder msg: 1613172788.57 epoch sec; seqNo:2321 2021-02-12T23:33:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.03 , 92.98 , 108.80 , 71.52 2021-02-12T23:33:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.00 , -0.04 2021-02-12T23:33:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172788.58 epoch sec; seqNo:4764; 4057261.04 north; 591139.69 east 2021-02-12T23:33:08Z- AHRS_M2 msg: 1613172788.96 epoch sec; seqNo:4717 2021-02-12T23:33:08Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.13 psi 2021-02-12T23:33:08Z- handleDepth()-Depth_Keller msg: 1613172788967 epoch sec; seqNo:4765; depth 16.97 2021-02-12T23:33:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172788.99 epoch sec; seqNo:4765; 4057261.04 north; 591139.69 east 2021-02-12T23:33:09Z- AHRS_M2 msg: 1613172789.35 epoch sec; seqNo:4718 2021-02-12T23:33:09Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.14 psi 2021-02-12T23:33:09Z- handleDepth()-Depth_Keller msg: 1613172789358 epoch sec; seqNo:4766; depth 16.84 2021-02-12T23:33:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172789.37 epoch sec; seqNo:4766; 4057261.42 north; 591139.69 east 2021-02-12T23:33:09Z- AHRS_M2 msg: 1613172789.76 epoch sec; seqNo:4719 2021-02-12T23:33:09Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.16 psi 2021-02-12T23:33:09Z- handleDepth()-Depth_Keller msg: 1613172789760 epoch sec; seqNo:4767; depth 16.72 2021-02-12T23:33:09Z- handleDvl() - RDI_Pathfinder msg: 1613172789.78 epoch sec; seqNo:2322 2021-02-12T23:33:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.03 , 92.98 , 108.80 , 78.37 2021-02-12T23:33:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.05 , 0.02 2021-02-12T23:33:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172789.79 epoch sec; seqNo:4767; 4057261.80 north; 591139.68 east 2021-02-12T23:33:10Z- AHRS_M2 msg: 1613172790.16 epoch sec; seqNo:4720 2021-02-12T23:33:10Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.18 psi 2021-02-12T23:33:10Z- handleDepth()-Depth_Keller msg: 1613172790164 epoch sec; seqNo:4768; depth 16.58 2021-02-12T23:33:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172790.17 epoch sec; seqNo:4768; 4057261.81 north; 591140.90 east 2021-02-12T23:33:10Z- LcmTrn::handleMessages() - handling 41 messages took 0.66 ms and 128 calls... 2021-02-12T23:33:10Z- waiting for 1613172788.34, time is 1613172790.17 2021-02-12T23:33:10Z- LcmTrn::updateTrn() >>>> pose time 1613172790.17, meas time 1613172789.78 <<<< 2021-02-12T23:33:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172789.78 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:33:10Z- TerrainNav::motionUpdate() - currEstimate = (4057262, 591140.90, 16.58) 2021-02-12T23:33:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:33:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:33:10Z- TNavBF::Projecting Measurements 2021-02-12T23:33:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:33:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:33:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:33:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:33:10Z- TNavFilter::4 good beams. 2021-02-12T23:33:10Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:33:10Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:33:10Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:33:10Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:33:10Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:33:10Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:33:10Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:33:10Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:10Z- TerrainMapOctree: (vehN, vehE ) = (4057261.56, 591140.71). 2021-02-12T23:33:10Z- TerrainMapOctree: Min Distance = 5057.21. 2021-02-12T23:33:10Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:33:10Z- There is no specific good beam that all particles have in common. 2021-02-12T23:33:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:33:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 10.76 2021-02-12T23:33:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:33:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 13.83 2021-02-12T23:33:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:33:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 18.94 2021-02-12T23:33:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:33:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 9.83 2021-02-12T23:33:10Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:33:10Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:33:10Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:33:10Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:33:10Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:33:10Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:33:10Z- Filter 0 No beams used in subcloud update 2021-02-12T23:33:10Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:33:10Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:33:10Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 755.4 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:33:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172789.78 sec. 2021-02-12T23:33:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:33:10Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:10Z- variances([0], [2], and psi_berg) = 755.321378 833.038156 0.000000 2021-02-12T23:33:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:33:10Z- LcmTrn::publishEstimates() - MLE : 4057246.31 591092.40 16.58 2021-02-12T23:33:10Z- LcmTrn::publishEstimates() - MMSE : 4057261.81 591140.90 16.58 2021-02-12T23:33:10Z- LcmTrn::publishEstimates() - COVAR: 755.32 833.04 0.77 0.00 2021-02-12T23:33:10Z- AHRS_M2 msg: 1613172790.56 epoch sec; seqNo:4721 2021-02-12T23:33:10Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.20 psi 2021-02-12T23:33:10Z- handleDepth()-Depth_Keller msg: 1613172790568 epoch sec; seqNo:4769; depth 16.47 2021-02-12T23:33:10Z- handleDvl() - RDI_Pathfinder msg: 1613172790.58 epoch sec; seqNo:2323 2021-02-12T23:33:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.88 , 95.28 , 108.66 , 72.34 2021-02-12T23:33:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.09 , 0.12 2021-02-12T23:33:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172790.59 epoch sec; seqNo:4769; 4057262.19 north; 591140.90 east 2021-02-12T23:33:10Z- AHRS_M2 msg: 1613172790.97 epoch sec; seqNo:4722 2021-02-12T23:33:10Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.22 psi 2021-02-12T23:33:10Z- handleDepth()-Depth_Keller msg: 1613172790974 epoch sec; seqNo:4770; depth 16.33 2021-02-12T23:33:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172790.98 epoch sec; seqNo:4770; 4057262.57 north; 591140.90 east 2021-02-12T23:33:11Z- AHRS_M2 msg: 1613172791.37 epoch sec; seqNo:4723 2021-02-12T23:33:11Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.23 psi 2021-02-12T23:33:11Z- handleDepth()-Depth_Keller msg: 1613172791377 epoch sec; seqNo:4771; depth 16.20 2021-02-12T23:33:11Z- handleDvl() - RDI_Pathfinder msg: 1613172791.40 epoch sec; seqNo:2324 2021-02-12T23:33:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 100.28 , 111.23 , 70.29 2021-02-12T23:33:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , 0.16 2021-02-12T23:33:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172791.41 epoch sec; seqNo:4771; 4057262.95 north; 591140.89 east 2021-02-12T23:33:11Z- AHRS_M2 msg: 1613172791.78 epoch sec; seqNo:4724 2021-02-12T23:33:11Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.24 psi 2021-02-12T23:33:11Z- handleDepth()-Depth_Keller msg: 1613172791780 epoch sec; seqNo:4772; depth 16.07 2021-02-12T23:33:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172791.80 epoch sec; seqNo:4772; 4057262.95 north; 591140.89 east 2021-02-12T23:33:12Z- AHRS_M2 msg: 1613172792.18 epoch sec; seqNo:4725 2021-02-12T23:33:12Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.24 psi 2021-02-12T23:33:12Z- handleDepth()-Depth_Keller msg: 1613172792189 epoch sec; seqNo:4773; depth 15.97 2021-02-12T23:33:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172792.20 epoch sec; seqNo:4773; 4057263.33 north; 591140.89 east 2021-02-12T23:33:12Z- AHRS_M2 msg: 1613172792.58 epoch sec; seqNo:4726 2021-02-12T23:33:12Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.23 psi 2021-02-12T23:33:12Z- handleDepth()-Depth_Keller msg: 1613172792590 epoch sec; seqNo:4774; depth 15.83 2021-02-12T23:33:12Z- handleDvl() - RDI_Pathfinder msg: 1613172792.61 epoch sec; seqNo:2325 2021-02-12T23:33:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.06 , 96.56 , 112.38 , 73.56 2021-02-12T23:33:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.02 , 0.10 2021-02-12T23:33:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172792.62 epoch sec; seqNo:4774; 4057263.70 north; 591140.88 east 2021-02-12T23:33:12Z- AHRS_M2 msg: 1613172792.99 epoch sec; seqNo:4727 2021-02-12T23:33:12Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.22 psi 2021-02-12T23:33:12Z- handleDepth()-Depth_Keller msg: 1613172792997 epoch sec; seqNo:4775; depth 15.72 2021-02-12T23:33:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172793.02 epoch sec; seqNo:4775; 4057264.10 north; 591142.10 east 2021-02-12T23:33:13Z- AHRS_M2 msg: 1613172793.40 epoch sec; seqNo:4728 2021-02-12T23:33:13Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.21 psi 2021-02-12T23:33:13Z- handleDepth()-Depth_Keller msg: 1613172793405 epoch sec; seqNo:4776; depth 15.60 2021-02-12T23:33:13Z- handleDvl() - RDI_Pathfinder msg: 1613172793.43 epoch sec; seqNo:2326 2021-02-12T23:33:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.69 , 101.43 , 114.82 , 73.15 2021-02-12T23:33:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.06 , 0.01 2021-02-12T23:33:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172793.44 epoch sec; seqNo:4776; 4057264.47 north; 591142.10 east 2021-02-12T23:33:13Z- AHRS_M2 msg: 1613172793.80 epoch sec; seqNo:4729 2021-02-12T23:33:13Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.20 psi 2021-02-12T23:33:13Z- handleDepth()-Depth_Keller msg: 1613172793800 epoch sec; seqNo:4777; depth 15.46 2021-02-12T23:33:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172793.81 epoch sec; seqNo:4777; 4057264.47 north; 591142.10 east 2021-02-12T23:33:14Z- AHRS_M2 msg: 1613172794.20 epoch sec; seqNo:4730 2021-02-12T23:33:14Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.20 psi 2021-02-12T23:33:14Z- handleDepth()-Depth_Keller msg: 1613172794207 epoch sec; seqNo:4778; depth 15.36 2021-02-12T23:33:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172794.21 epoch sec; seqNo:4778; 4057264.85 north; 591142.09 east 2021-02-12T23:33:14Z- AHRS_M2 msg: 1613172794.60 epoch sec; seqNo:4731 2021-02-12T23:33:14Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.19 psi 2021-02-12T23:33:14Z- handleDepth()-Depth_Keller msg: 1613172794613 epoch sec; seqNo:4779; depth 15.22 2021-02-12T23:33:14Z- handleDvl() - RDI_Pathfinder msg: 1613172794.63 epoch sec; seqNo:2327 2021-02-12T23:33:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.48 , 96.36 , 113.94 , 73.15 2021-02-12T23:33:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.08 , -0.03 2021-02-12T23:33:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172794.64 epoch sec; seqNo:4779; 4057265.23 north; 591142.09 east 2021-02-12T23:33:15Z- AHRS_M2 msg: 1613172795.01 epoch sec; seqNo:4732 2021-02-12T23:33:15Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.18 psi 2021-02-12T23:33:15Z- handleDepth()-Depth_Keller msg: 1613172795012 epoch sec; seqNo:4780; depth 15.11 2021-02-12T23:33:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172795.02 epoch sec; seqNo:4780; 4057265.61 north; 591142.09 east 2021-02-12T23:33:15Z- AHRS_M2 msg: 1613172795.42 epoch sec; seqNo:4733 2021-02-12T23:33:15Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.18 psi 2021-02-12T23:33:15Z- handleDepth()-Depth_Keller msg: 1613172795424 epoch sec; seqNo:4781; depth 15.00 2021-02-12T23:33:15Z- handleDvl() - RDI_Pathfinder msg: 1613172795.43 epoch sec; seqNo:2328 2021-02-12T23:33:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 96.43 , 114.68 , 71.93 2021-02-12T23:33:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , 0.01 2021-02-12T23:33:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172795.45 epoch sec; seqNo:4781; 4057266.00 north; 591143.30 east 2021-02-12T23:33:15Z- LcmTrn::handleMessages() - handling 45 messages took 0.62 ms and 133 calls... 2021-02-12T23:33:15Z- waiting for 1613172793.17, time is 1613172795.45 2021-02-12T23:33:15Z- LcmTrn::updateTrn() equating pose 1613172795.45 and meas times 1613172795.45 2021-02-12T23:33:15Z- LcmTrn::updateTrn() >>>> pose time 1613172795.45, meas time 1613172795.45 <<<< 2021-02-12T23:33:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172795.45 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:33:15Z- TerrainNav::motionUpdate() - currEstimate = (4057266, 591143.30, 15.00) 2021-02-12T23:33:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:33:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:33:15Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172795.45 sec.; No relevant navigation data available 2021-02-12T23:33:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:33:15Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:15Z- variances([0], [2], and psi_berg) = 755.206442 833.038042 0.000000 2021-02-12T23:33:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:33:15Z- LcmTrn::publishEstimates() - MLE : 4057250.50 591094.80 15.00 2021-02-12T23:33:15Z- LcmTrn::publishEstimates() - MMSE : 4057266.00 591143.30 15.00 2021-02-12T23:33:15Z- LcmTrn::publishEstimates() - COVAR: 755.21 833.04 1.02 0.00 2021-02-12T23:33:15Z- AHRS_M2 msg: 1613172795.82 epoch sec; seqNo:4734 2021-02-12T23:33:15Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.17 psi 2021-02-12T23:33:15Z- handleDepth()-Depth_Keller msg: 1613172795829 epoch sec; seqNo:4782; depth 14.86 2021-02-12T23:33:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172795.84 epoch sec; seqNo:4782; 4057266.38 north; 591143.30 east 2021-02-12T23:33:16Z- AHRS_M2 msg: 1613172796.22 epoch sec; seqNo:4735 2021-02-12T23:33:16Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.18 psi 2021-02-12T23:33:16Z- handleDepth()-Depth_Keller msg: 1613172796225 epoch sec; seqNo:4783; depth 14.74 2021-02-12T23:33:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172796.24 epoch sec; seqNo:4783; 4057266.38 north; 591143.30 east 2021-02-12T23:33:16Z- AHRS_M2 msg: 1613172796.62 epoch sec; seqNo:4736 2021-02-12T23:33:16Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.18 psi 2021-02-12T23:33:16Z- handleDepth()-Depth_Keller msg: 1613172796628 epoch sec; seqNo:4784; depth 14.60 2021-02-12T23:33:16Z- handleDvl() - RDI_Pathfinder msg: 1613172796.64 epoch sec; seqNo:2329 2021-02-12T23:33:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 96.43 , 114.68 , 73.97 2021-02-12T23:33:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , 0.04 2021-02-12T23:33:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172796.66 epoch sec; seqNo:4784; 4057266.76 north; 591143.29 east 2021-02-12T23:33:17Z- AHRS_M2 msg: 1613172797.03 epoch sec; seqNo:4737 2021-02-12T23:33:17Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.19 psi 2021-02-12T23:33:17Z- handleDepth()-Depth_Keller msg: 1613172797032 epoch sec; seqNo:4785; depth 14.47 2021-02-12T23:33:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172797.04 epoch sec; seqNo:4785; 4057267.14 north; 591143.29 east 2021-02-12T23:33:17Z- AHRS_M2 msg: 1613172797.43 epoch sec; seqNo:4738 2021-02-12T23:33:17Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.20 psi 2021-02-12T23:33:17Z- handleDepth()-Depth_Keller msg: 1613172797439 epoch sec; seqNo:4786; depth 14.35 2021-02-12T23:33:17Z- handleDvl() - RDI_Pathfinder msg: 1613172797.45 epoch sec; seqNo:2330 2021-02-12T23:33:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 99.06 , 113.94 , 73.56 2021-02-12T23:33:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.02 2021-02-12T23:33:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172797.46 epoch sec; seqNo:4786; 4057267.52 north; 591143.29 east 2021-02-12T23:33:17Z- AHRS_M2 msg: 1613172797.84 epoch sec; seqNo:4739 2021-02-12T23:33:17Z- AHRS_M2 msg: 0.57 phi; 0.37 theta; -0.20 psi 2021-02-12T23:33:17Z- handleDepth()-Depth_Keller msg: 1613172797840 epoch sec; seqNo:4787; depth 14.22 2021-02-12T23:33:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172797.86 epoch sec; seqNo:4787; 4057267.52 north; 591143.29 east 2021-02-12T23:33:18Z- AHRS_M2 msg: 1613172798.24 epoch sec; seqNo:4740 2021-02-12T23:33:18Z- AHRS_M2 msg: 0.57 phi; 0.37 theta; -0.20 psi 2021-02-12T23:33:18Z- handleDepth()-Depth_Keller msg: 1613172798249 epoch sec; seqNo:4788; depth 14.09 2021-02-12T23:33:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172798.26 epoch sec; seqNo:4788; 4057267.90 north; 591143.28 east 2021-02-12T23:33:18Z- AHRS_M2 msg: 1613172798.64 epoch sec; seqNo:4741 2021-02-12T23:33:18Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.20 psi 2021-02-12T23:33:18Z- handleDepth()-Depth_Keller msg: 1613172798653 epoch sec; seqNo:4789; depth 13.96 2021-02-12T23:33:18Z- handleDvl() - RDI_Pathfinder msg: 1613172798.67 epoch sec; seqNo:2331 2021-02-12T23:33:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 101.70 , 116.57 , 74.38 2021-02-12T23:33:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.00 , -0.03 2021-02-12T23:33:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172798.68 epoch sec; seqNo:4789; 4057268.29 north; 591144.50 east 2021-02-12T23:33:19Z- AHRS_M2 msg: 1613172799.05 epoch sec; seqNo:4742 2021-02-12T23:33:19Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.20 psi 2021-02-12T23:33:19Z- handleDepth()-Depth_Keller msg: 1613172799060 epoch sec; seqNo:4790; depth 13.83 2021-02-12T23:33:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172799.07 epoch sec; seqNo:4790; 4057268.29 north; 591144.50 east 2021-02-12T23:33:19Z- AHRS_M2 msg: 1613172799.45 epoch sec; seqNo:4743 2021-02-12T23:33:19Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.19 psi 2021-02-12T23:33:19Z- handleDepth()-Depth_Keller msg: 1613172799464 epoch sec; seqNo:4791; depth 13.71 2021-02-12T23:33:19Z- handleDvl() - RDI_Pathfinder msg: 1613172799.50 epoch sec; seqNo:2332 2021-02-12T23:33:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.05 , 101.63 , 117.86 , 73.56 2021-02-12T23:33:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.01 , -0.07 2021-02-12T23:33:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172799.51 epoch sec; seqNo:4791; 4057268.67 north; 591144.49 east 2021-02-12T23:33:19Z- AHRS_M2 msg: 1613172799.86 epoch sec; seqNo:4744 2021-02-12T23:33:19Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.18 psi 2021-02-12T23:33:19Z- handleDepth()-Depth_Keller msg: 1613172799860 epoch sec; seqNo:4792; depth 13.58 2021-02-12T23:33:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172799.87 epoch sec; seqNo:4792; 4057269.05 north; 591144.49 east 2021-02-12T23:33:20Z- AHRS_M2 msg: 1613172800.26 epoch sec; seqNo:4745 2021-02-12T23:33:20Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.17 psi 2021-02-12T23:33:20Z- handleDepth()-Depth_Keller msg: 1613172800269 epoch sec; seqNo:4793; depth 13.47 2021-02-12T23:33:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172800.28 epoch sec; seqNo:4793; 4057269.05 north; 591144.49 east 2021-02-12T23:33:20Z- AHRS_M2 msg: 1613172800.66 epoch sec; seqNo:4746 2021-02-12T23:33:20Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-12T23:33:20Z- LcmTrn::handleMessages() - handling 41 messages took 0.75 ms and 126 calls... 2021-02-12T23:33:20Z- waiting for 1613172798.45, time is 1613172800.66 2021-02-12T23:33:20Z- LcmTrn::updateTrn() >>>> pose time 1613172800.66, meas time 1613172799.50 <<<< 2021-02-12T23:33:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172799.50 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:33:20Z- TerrainNav::motionUpdate() - currEstimate = (4057269, 591144.49, 13.47) 2021-02-12T23:33:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:33:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:33:20Z- TNavBF::Projecting Measurements 2021-02-12T23:33:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:33:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:33:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:33:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:33:20Z- TNavFilter::4 good beams. 2021-02-12T23:33:20Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:33:20Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:33:20Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:33:20Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:33:20Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:33:20Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:33:20Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:33:20Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:20Z- TerrainMapOctree: (vehN, vehE ) = (4057268.37, 591144.23). 2021-02-12T23:33:20Z- TerrainMapOctree: Min Distance = 5049.68. 2021-02-12T23:33:20Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:33:20Z- There is no specific good beam that all particles have in common. 2021-02-12T23:33:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:33:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 11.30 2021-02-12T23:33:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:33:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 16.53 2021-02-12T23:33:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:33:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 22.22 2021-02-12T23:33:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:33:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 8.66 2021-02-12T23:33:20Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:33:20Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:33:20Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:33:20Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:33:20Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:33:20Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:33:20Z- Filter 0 No beams used in subcloud update 2021-02-12T23:33:20Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:33:20Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:33:20Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 755.1 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:33:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172799.50 sec. 2021-02-12T23:33:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:33:20Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:20Z- variances([0], [2], and psi_berg) = 755.078723 833.037915 0.000000 2021-02-12T23:33:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:33:20Z- LcmTrn::publishEstimates() - MLE : 4057253.55 591105.99 13.47 2021-02-12T23:33:20Z- LcmTrn::publishEstimates() - MMSE : 4057269.05 591144.49 13.47 2021-02-12T23:33:20Z- LcmTrn::publishEstimates() - COVAR: 755.08 833.04 0.91 0.00 2021-02-12T23:33:20Z- handleDepth()-Depth_Keller msg: 1613172800677 epoch sec; seqNo:4794; depth 13.36 2021-02-12T23:33:20Z- handleDvl() - RDI_Pathfinder msg: 1613172800.69 epoch sec; seqNo:2333 2021-02-12T23:33:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 96.15 , 120.49 , 72.34 2021-02-12T23:33:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.03 , -0.05 2021-02-12T23:33:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172800.70 epoch sec; seqNo:4794; 4057269.43 north; 591144.49 east 2021-02-12T23:33:21Z- AHRS_M2 msg: 1613172801.07 epoch sec; seqNo:4747 2021-02-12T23:33:21Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.14 psi 2021-02-12T23:33:21Z- handleDepth()-Depth_Keller msg: 1613172801072 epoch sec; seqNo:4795; depth 13.20 2021-02-12T23:33:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172801.09 epoch sec; seqNo:4795; 4057269.80 north; 591144.48 east 2021-02-12T23:33:21Z- AHRS_M2 msg: 1613172801.47 epoch sec; seqNo:4748 2021-02-12T23:33:21Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-12T23:33:21Z- handleDepth()-Depth_Keller msg: 1613172801476 epoch sec; seqNo:4796; depth 13.08 2021-02-12T23:33:21Z- handleDvl() - RDI_Pathfinder msg: 1613172801.49 epoch sec; seqNo:2334 2021-02-12T23:33:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 99.00 , 117.93 , 73.15 2021-02-12T23:33:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , -0.10 , -0.02 2021-02-12T23:33:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172801.50 epoch sec; seqNo:4796; 4057269.82 north; 591145.70 east 2021-02-12T23:33:21Z- AHRS_M2 msg: 1613172801.88 epoch sec; seqNo:4749 2021-02-12T23:33:21Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-12T23:33:21Z- handleDepth()-Depth_Keller msg: 1613172801905 epoch sec; seqNo:4797; depth 12.93 2021-02-12T23:33:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172801.92 epoch sec; seqNo:4797; 4057270.20 north; 591145.70 east 2021-02-12T23:33:22Z- AHRS_M2 msg: 1613172802.30 epoch sec; seqNo:4750 2021-02-12T23:33:22Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-12T23:33:22Z- handleDepth()-Depth_Keller msg: 1613172802312 epoch sec; seqNo:4798; depth 12.79 2021-02-12T23:33:22Z- handleDvl() - RDI_Pathfinder msg: 1613172802.32 epoch sec; seqNo:2335 2021-02-12T23:33:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.22 , 113.80 , 74.79 2021-02-12T23:33:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.20 , 0.04 2021-02-12T23:33:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172802.33 epoch sec; seqNo:4798; 4057270.58 north; 591145.70 east 2021-02-12T23:33:22Z- AHRS_M2 msg: 1613172802.68 epoch sec; seqNo:4751 2021-02-12T23:33:22Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-12T23:33:22Z- handleDepth()-Depth_Keller msg: 1613172802688 epoch sec; seqNo:4799; depth 12.66 2021-02-12T23:33:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172802.70 epoch sec; seqNo:4799; 4057270.58 north; 591145.70 east 2021-02-12T23:33:23Z- AHRS_M2 msg: 1613172803.09 epoch sec; seqNo:4752 2021-02-12T23:33:23Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.16 psi 2021-02-12T23:33:23Z- handleDepth()-Depth_Keller msg: 1613172803097 epoch sec; seqNo:4800; depth 12.53 2021-02-12T23:33:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172803.11 epoch sec; seqNo:4800; 4057270.95 north; 591145.69 east 2021-02-12T23:33:23Z- AHRS_M2 msg: 1613172803.50 epoch sec; seqNo:4753 2021-02-12T23:33:23Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-12T23:33:23Z- handleDepth()-Depth_Keller msg: 1613172803502 epoch sec; seqNo:4801; depth 12.41 2021-02-12T23:33:23Z- handleDvl() - RDI_Pathfinder msg: 1613172803.51 epoch sec; seqNo:2336 2021-02-12T23:33:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.27 , 98.79 , 113.67 , 76.02 2021-02-12T23:33:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.23 , 0.14 2021-02-12T23:33:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172803.53 epoch sec; seqNo:4801; 4057271.33 north; 591145.69 east 2021-02-12T23:33:23Z- AHRS_M2 msg: 1613172803.89 epoch sec; seqNo:4754 2021-02-12T23:33:23Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-12T23:33:23Z- handleDepth()-Depth_Keller msg: 1613172803900 epoch sec; seqNo:4802; depth 12.27 2021-02-12T23:33:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172803.92 epoch sec; seqNo:4802; 4057271.71 north; 591145.68 east 2021-02-12T23:33:24Z- AHRS_M2 msg: 1613172804.31 epoch sec; seqNo:4755 2021-02-12T23:33:24Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-12T23:33:24Z- handleDepth()-Depth_Keller msg: 1613172804314 epoch sec; seqNo:4803; depth 12.16 2021-02-12T23:33:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172804.33 epoch sec; seqNo:4803; 4057272.09 north; 591145.68 east 2021-02-12T23:33:24Z- AHRS_M2 msg: 1613172804.70 epoch sec; seqNo:4756 2021-02-12T23:33:24Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-12T23:33:24Z- handleDepth()-Depth_Keller msg: 1613172804708 epoch sec; seqNo:4804; depth 12.03 2021-02-12T23:33:24Z- handleDvl() - RDI_Pathfinder msg: 1613172804.72 epoch sec; seqNo:2337 2021-02-12T23:33:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 100.28 , 113.80 , 73.56 2021-02-12T23:33:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.14 , 0.20 2021-02-12T23:33:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172804.73 epoch sec; seqNo:4804; 4057272.47 north; 591145.68 east 2021-02-12T23:33:25Z- AHRS_M2 msg: 1613172805.11 epoch sec; seqNo:4757 2021-02-12T23:33:25Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-12T23:33:25Z- handleDepth()-Depth_Keller msg: 1613172805115 epoch sec; seqNo:4805; depth 11.90 2021-02-12T23:33:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172805.13 epoch sec; seqNo:4805; 4057272.86 north; 591146.89 east 2021-02-12T23:33:25Z- AHRS_M2 msg: 1613172805.51 epoch sec; seqNo:4758 2021-02-12T23:33:25Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-12T23:33:25Z- handleDepth()-Depth_Keller msg: 1613172805521 epoch sec; seqNo:4806; depth 11.79 2021-02-12T23:33:25Z- handleDvl() - RDI_Pathfinder msg: 1613172805.55 epoch sec; seqNo:2338 2021-02-12T23:33:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.83 , 97.65 , 115.22 , 75.60 2021-02-12T23:33:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , 0.13 2021-02-12T23:33:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172805.56 epoch sec; seqNo:4806; 4057272.86 north; 591146.89 east 2021-02-12T23:33:25Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-12T23:33:25Z- waiting for 1613172803.66, time is 1613172805.56 2021-02-12T23:33:25Z- LcmTrn::updateTrn() equating pose 1613172805.56 and meas times 1613172805.56 2021-02-12T23:33:25Z- LcmTrn::updateTrn() >>>> pose time 1613172805.56, meas time 1613172805.56 <<<< 2021-02-12T23:33:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172805.56 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:33:25Z- TerrainNav::motionUpdate() - currEstimate = (4057273, 591146.89, 11.79) 2021-02-12T23:33:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:33:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:33:25Z- TNavBF::Projecting Measurements 2021-02-12T23:33:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:33:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:33:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:33:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:33:25Z- TNavFilter::4 good beams. 2021-02-12T23:33:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:33:25Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:33:25Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:33:25Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:33:25Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:33:25Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:33:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:33:25Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:25Z- TerrainMapOctree: (vehN, vehE ) = (4057272.86, 591146.89). 2021-02-12T23:33:25Z- TerrainMapOctree: Min Distance = 5044.49. 2021-02-12T23:33:25Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:33:25Z- There is no specific good beam that all particles have in common. 2021-02-12T23:33:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:33:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 12.06 2021-02-12T23:33:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:33:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 15.26 2021-02-12T23:33:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:33:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 21.24 2021-02-12T23:33:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:33:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 9.14 2021-02-12T23:33:25Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:33:25Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:33:25Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:33:25Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:33:25Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:33:25Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:33:25Z- Filter 0 No beams used in subcloud update 2021-02-12T23:33:25Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:33:25Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:33:25Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 755.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:33:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172805.56 sec. 2021-02-12T23:33:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:33:25Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:25Z- variances([0], [2], and psi_berg) = 754.968064 833.037805 0.000000 2021-02-12T23:33:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:33:25Z- LcmTrn::publishEstimates() - MLE : 4057258.36 591108.39 11.79 2021-02-12T23:33:25Z- LcmTrn::publishEstimates() - MMSE : 4057272.86 591146.89 11.79 2021-02-12T23:33:25Z- LcmTrn::publishEstimates() - COVAR: 754.97 833.04 0.88 0.00 2021-02-12T23:33:25Z- AHRS_M2 msg: 1613172805.92 epoch sec; seqNo:4759 2021-02-12T23:33:25Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.12 psi 2021-02-12T23:33:25Z- handleDepth()-Depth_Keller msg: 1613172805920 epoch sec; seqNo:4807; depth 11.68 2021-02-12T23:33:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172805.93 epoch sec; seqNo:4807; 4057273.24 north; 591146.89 east 2021-02-12T23:33:26Z- AHRS_M2 msg: 1613172806.33 epoch sec; seqNo:4760 2021-02-12T23:33:26Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.12 psi 2021-02-12T23:33:26Z- handleDepth()-Depth_Keller msg: 1613172806329 epoch sec; seqNo:4808; depth 11.55 2021-02-12T23:33:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172806.34 epoch sec; seqNo:4808; 4057273.62 north; 591146.88 east 2021-02-12T23:33:26Z- AHRS_M2 msg: 1613172806.72 epoch sec; seqNo:4761 2021-02-12T23:33:26Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.12 psi 2021-02-12T23:33:26Z- handleDepth()-Depth_Keller msg: 1613172806738 epoch sec; seqNo:4809; depth 11.44 2021-02-12T23:33:26Z- handleDvl() - RDI_Pathfinder msg: 1613172806.75 epoch sec; seqNo:2339 2021-02-12T23:33:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.76 , 97.57 , 113.80 , 76.83 2021-02-12T23:33:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.03 2021-02-12T23:33:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172806.76 epoch sec; seqNo:4809; 4057274.00 north; 591146.88 east 2021-02-12T23:33:27Z- AHRS_M2 msg: 1613172807.13 epoch sec; seqNo:4762 2021-02-12T23:33:27Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.13 psi 2021-02-12T23:33:27Z- handleDepth()-Depth_Keller msg: 1613172807138 epoch sec; seqNo:4810; depth 11.30 2021-02-12T23:33:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172807.16 epoch sec; seqNo:4810; 4057274.38 north; 591146.88 east 2021-02-12T23:33:27Z- AHRS_M2 msg: 1613172807.53 epoch sec; seqNo:4763 2021-02-12T23:33:27Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.13 psi 2021-02-12T23:33:27Z- handleDepth()-Depth_Keller msg: 1613172807536 epoch sec; seqNo:4811; depth 11.19 2021-02-12T23:33:27Z- handleDvl() - RDI_Pathfinder msg: 1613172807.55 epoch sec; seqNo:2340 2021-02-12T23:33:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.11 , 96.22 , 115.15 , 74.79 2021-02-12T23:33:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.07 , -0.01 2021-02-12T23:33:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172807.56 epoch sec; seqNo:4811; 4057274.39 north; 591148.10 east 2021-02-12T23:33:27Z- AHRS_M2 msg: 1613172807.94 epoch sec; seqNo:4764 2021-02-12T23:33:27Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.14 psi 2021-02-12T23:33:27Z- handleDepth()-Depth_Keller msg: 1613172807945 epoch sec; seqNo:4812; depth 11.07 2021-02-12T23:33:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172807.96 epoch sec; seqNo:4812; 4057274.77 north; 591148.09 east 2021-02-12T23:33:28Z- AHRS_M2 msg: 1613172808.35 epoch sec; seqNo:4765 2021-02-12T23:33:28Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.13 psi 2021-02-12T23:33:28Z- handleDepth()-Depth_Keller msg: 1613172808378 epoch sec; seqNo:4813; depth 10.94 2021-02-12T23:33:28Z- handleDvl() - RDI_Pathfinder msg: 1613172808.39 epoch sec; seqNo:2341 2021-02-12T23:33:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.76 , 98.93 , 112.45 , 76.42 2021-02-12T23:33:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , 0.00 2021-02-12T23:33:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172808.40 epoch sec; seqNo:4813; 4057275.15 north; 591148.09 east 2021-02-12T23:33:28Z- AHRS_M2 msg: 1613172808.74 epoch sec; seqNo:4766 2021-02-12T23:33:28Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.13 psi 2021-02-12T23:33:28Z- handleDepth()-Depth_Keller msg: 1613172808748 epoch sec; seqNo:4814; depth 10.83 2021-02-12T23:33:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:33:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:33:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:33:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:33:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:33:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:33:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:33:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:33:28Z- handleDvl() - RDI_Pathfinder msg: 1613172808.75 epoch sec; seqNo:2342 2021-02-12T23:33:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:33:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , 0.00 2021-02-12T23:33:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172808.76 epoch sec; seqNo:4814; 4057275.15 north; 591148.09 east 2021-02-12T23:33:29Z- AHRS_M2 msg: 1613172809.15 epoch sec; seqNo:4767 2021-02-12T23:33:29Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.13 psi 2021-02-12T23:33:29Z- handleDepth()-Depth_Keller msg: 1613172809159 epoch sec; seqNo:4815; depth 10.74 2021-02-12T23:33:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172809.17 epoch sec; seqNo:4815; 4057275.53 north; 591148.09 east 2021-02-12T23:33:29Z- AHRS_M2 msg: 1613172809.55 epoch sec; seqNo:4768 2021-02-12T23:33:29Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.14 psi 2021-02-12T23:33:29Z- handleDepth()-Depth_Keller msg: 1613172809556 epoch sec; seqNo:4816; depth 10.62 2021-02-12T23:33:29Z- handleDvl() - RDI_Pathfinder msg: 1613172809.58 epoch sec; seqNo:2343 2021-02-12T23:33:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.76 , 97.58 , 112.45 , 75.60 2021-02-12T23:33:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.06 , 0.03 2021-02-12T23:33:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172809.59 epoch sec; seqNo:4816; 4057275.90 north; 591148.08 east 2021-02-12T23:33:29Z- AHRS_M2 msg: 1613172809.96 epoch sec; seqNo:4769 2021-02-12T23:33:29Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.15 psi 2021-02-12T23:33:29Z- handleDepth()-Depth_Keller msg: 1613172809970 epoch sec; seqNo:4817; depth 10.51 2021-02-12T23:33:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:33:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:33:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:33:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:33:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:33:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:33:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:33:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:33:29Z- handleDvl() - RDI_Pathfinder msg: 1613172809.98 epoch sec; seqNo:2344 2021-02-12T23:33:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:33:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.06 , 0.03 2021-02-12T23:33:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172809.99 epoch sec; seqNo:4817; 4057275.90 north; 591148.08 east 2021-02-12T23:33:30Z- AHRS_M2 msg: 1613172810.38 epoch sec; seqNo:4770 2021-02-12T23:33:30Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-12T23:33:30Z- handleDepth()-Depth_Keller msg: 1613172810384 epoch sec; seqNo:4818; depth 10.39 2021-02-12T23:33:30Z- handleDvl() - RDI_Pathfinder msg: 1613172810.40 epoch sec; seqNo:2345 2021-02-12T23:33:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.48 , 99.00 , 113.87 , 76.42 2021-02-12T23:33:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , 0.08 2021-02-12T23:33:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172810.41 epoch sec; seqNo:4818; 4057276.30 north; 591149.30 east 2021-02-12T23:33:30Z- AHRS_M2 msg: 1613172810.77 epoch sec; seqNo:4771 2021-02-12T23:33:30Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:33:30Z- handleDepth()-Depth_Keller msg: 1613172810772 epoch sec; seqNo:4819; depth 10.26 2021-02-12T23:33:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172810.78 epoch sec; seqNo:4819; 4057276.68 north; 591149.29 east 2021-02-12T23:33:30Z- LcmTrn::handleMessages() - handling 46 messages took 0.69 ms and 132 calls... 2021-02-12T23:33:30Z- waiting for 1613172808.56, time is 1613172810.78 2021-02-12T23:33:30Z- LcmTrn::updateTrn() >>>> pose time 1613172810.78, meas time 1613172810.40 <<<< 2021-02-12T23:33:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172810.40 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:33:30Z- TerrainNav::motionUpdate() - currEstimate = (4057277, 591149.29, 10.26) 2021-02-12T23:33:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:33:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:33:30Z- TNavBF::Projecting Measurements 2021-02-12T23:33:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:33:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:33:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:33:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:33:30Z- TNavFilter::4 good beams. 2021-02-12T23:33:30Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:33:30Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:33:30Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:33:30Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:33:30Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:33:30Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:33:30Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:33:30Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:30Z- TerrainMapOctree: (vehN, vehE ) = (4057276.40, 591149.12). 2021-02-12T23:33:30Z- TerrainMapOctree: Min Distance = 5040.34. 2021-02-12T23:33:30Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:33:30Z- There is no specific good beam that all particles have in common. 2021-02-12T23:33:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:33:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 11.69 2021-02-12T23:33:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:33:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.68 2021-02-12T23:33:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:33:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 20.75 2021-02-12T23:33:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:33:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 9.34 2021-02-12T23:33:30Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:33:30Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:33:30Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:33:30Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:33:30Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:33:30Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:33:30Z- Filter 0 No beams used in subcloud update 2021-02-12T23:33:30Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:33:30Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:33:30Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 754.9 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:33:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172810.40 sec. 2021-02-12T23:33:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:33:30Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:30Z- variances([0], [2], and psi_berg) = 754.831865 833.037670 0.000000 2021-02-12T23:33:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:33:30Z- LcmTrn::publishEstimates() - MLE : 4057262.18 591110.79 10.26 2021-02-12T23:33:30Z- LcmTrn::publishEstimates() - MMSE : 4057276.68 591149.29 10.26 2021-02-12T23:33:30Z- LcmTrn::publishEstimates() - COVAR: 754.83 833.04 0.86 0.00 2021-02-12T23:33:31Z- AHRS_M2 msg: 1613172811.17 epoch sec; seqNo:4772 2021-02-12T23:33:31Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.18 psi 2021-02-12T23:33:31Z- handleDepth()-Depth_Keller msg: 1613172811178 epoch sec; seqNo:4820; depth 10.18 2021-02-12T23:33:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:33:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:33:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:33:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:33:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:33:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:33:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:33:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:33:31Z- handleDvl() - RDI_Pathfinder msg: 1613172811.18 epoch sec; seqNo:2346 2021-02-12T23:33:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:33:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , 0.08 2021-02-12T23:33:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172811.19 epoch sec; seqNo:4820; 4057277.05 north; 591149.29 east 2021-02-12T23:33:31Z- AHRS_M2 msg: 1613172811.57 epoch sec; seqNo:4773 2021-02-12T23:33:31Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.19 psi 2021-02-12T23:33:31Z- handleDepth()-Depth_Keller msg: 1613172811576 epoch sec; seqNo:4821; depth 10.06 2021-02-12T23:33:31Z- handleDvl() - RDI_Pathfinder msg: 1613172811.60 epoch sec; seqNo:2347 2021-02-12T23:33:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.76 , 101.63 , 113.80 , 78.65 2021-02-12T23:33:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , 0.09 2021-02-12T23:33:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172811.61 epoch sec; seqNo:4821; 4057277.43 north; 591149.29 east 2021-02-12T23:33:31Z- AHRS_M2 msg: 1613172811.98 epoch sec; seqNo:4774 2021-02-12T23:33:31Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.19 psi 2021-02-12T23:33:31Z- handleDepth()-Depth_Keller msg: 1613172811980 epoch sec; seqNo:4822; depth 9.95 2021-02-12T23:33:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172811.99 epoch sec; seqNo:4822; 4057277.43 north; 591149.29 east 2021-02-12T23:33:32Z- AHRS_M2 msg: 1613172812.39 epoch sec; seqNo:4775 2021-02-12T23:33:32Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.18 psi 2021-02-12T23:33:32Z- handleDepth()-Depth_Keller msg: 1613172812390 epoch sec; seqNo:4823; depth 9.83 2021-02-12T23:33:32Z- handleDvl() - RDI_Pathfinder msg: 1613172812.40 epoch sec; seqNo:2348 2021-02-12T23:33:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 102.85 , 115.02 , 78.46 2021-02-12T23:33:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.08 , 0.03 2021-02-12T23:33:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172812.41 epoch sec; seqNo:4823; 4057277.81 north; 591149.28 east 2021-02-12T23:33:32Z- AHRS_M2 msg: 1613172812.78 epoch sec; seqNo:4776 2021-02-12T23:33:32Z- AHRS_M2 msg: 0.65 phi; 0.29 theta; -0.17 psi 2021-02-12T23:33:32Z- handleDepth()-Depth_Keller msg: 1613172812791 epoch sec; seqNo:4824; depth 9.74 2021-02-12T23:33:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172812.80 epoch sec; seqNo:4824; 4057278.20 north; 591150.50 east 2021-02-12T23:33:33Z- AHRS_M2 msg: 1613172813.19 epoch sec; seqNo:4777 2021-02-12T23:33:33Z- AHRS_M2 msg: 0.65 phi; 0.28 theta; -0.17 psi 2021-02-12T23:33:33Z- handleDepth()-Depth_Keller msg: 1613172813194 epoch sec; seqNo:4825; depth 9.64 2021-02-12T23:33:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172813.22 epoch sec; seqNo:4825; 4057278.20 north; 591150.50 east 2021-02-12T23:33:33Z- AHRS_M2 msg: 1613172813.59 epoch sec; seqNo:4778 2021-02-12T23:33:33Z- AHRS_M2 msg: 0.65 phi; 0.28 theta; -0.17 psi 2021-02-12T23:33:33Z- handleDepth()-Depth_Keller msg: 1613172813596 epoch sec; seqNo:4826; depth 9.52 2021-02-12T23:33:33Z- handleDvl() - RDI_Pathfinder msg: 1613172813.61 epoch sec; seqNo:2349 2021-02-12T23:33:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.21 , 101.43 , 112.25 , 79.28 2021-02-12T23:33:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.09 , -0.06 2021-02-12T23:33:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172813.63 epoch sec; seqNo:4826; 4057278.58 north; 591150.49 east 2021-02-12T23:33:33Z- AHRS_M2 msg: 1613172813.99 epoch sec; seqNo:4779 2021-02-12T23:33:33Z- AHRS_M2 msg: 0.66 phi; 0.27 theta; -0.17 psi 2021-02-12T23:33:34Z- handleDepth()-Depth_Keller msg: 1613172814000 epoch sec; seqNo:4827; depth 9.42 2021-02-12T23:33:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172814.01 epoch sec; seqNo:4827; 4057278.58 north; 591150.49 east 2021-02-12T23:33:34Z- AHRS_M2 msg: 1613172814.40 epoch sec; seqNo:4780 2021-02-12T23:33:34Z- AHRS_M2 msg: 0.65 phi; 0.27 theta; -0.17 psi 2021-02-12T23:33:34Z- handleDepth()-Depth_Keller msg: 1613172814412 epoch sec; seqNo:4828; depth 9.32 2021-02-12T23:33:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172814.44 epoch sec; seqNo:4828; 4057278.96 north; 591150.49 east 2021-02-12T23:33:34Z- AHRS_M2 msg: 1613172814.80 epoch sec; seqNo:4781 2021-02-12T23:33:34Z- AHRS_M2 msg: 0.66 phi; 0.26 theta; -0.17 psi 2021-02-12T23:33:34Z- handleDepth()-Depth_Keller msg: 1613172814808 epoch sec; seqNo:4829; depth 9.24 2021-02-12T23:33:34Z- handleDvl() - RDI_Pathfinder msg: 1613172814.83 epoch sec; seqNo:2350 2021-02-12T23:33:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 96.16 , 111.03 , 79.93 2021-02-12T23:33:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.03 , -0.06 2021-02-12T23:33:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172814.84 epoch sec; seqNo:4829; 4057279.34 north; 591150.49 east 2021-02-12T23:33:35Z- AHRS_M2 msg: 1613172815.21 epoch sec; seqNo:4782 2021-02-12T23:33:35Z- AHRS_M2 msg: 0.66 phi; 0.26 theta; -0.16 psi 2021-02-12T23:33:35Z- handleDepth()-Depth_Keller msg: 1613172815212 epoch sec; seqNo:4830; depth 9.15 2021-02-12T23:33:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172815.22 epoch sec; seqNo:4830; 4057279.35 north; 591151.71 east 2021-02-12T23:33:35Z- AHRS_M2 msg: 1613172815.62 epoch sec; seqNo:4783 2021-02-12T23:33:35Z- AHRS_M2 msg: 0.66 phi; 0.26 theta; -0.16 psi 2021-02-12T23:33:35Z- handleDepth()-Depth_Keller msg: 1613172815627 epoch sec; seqNo:4831; depth 9.07 2021-02-12T23:33:35Z- handleDvl() - RDI_Pathfinder msg: 1613172815.65 epoch sec; seqNo:2351 2021-02-12T23:33:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 101.63 , 111.10 , 80.00 2021-02-12T23:33:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.04 , -0.09 2021-02-12T23:33:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172815.68 epoch sec; seqNo:4831; 4057279.73 north; 591151.70 east 2021-02-12T23:33:35Z- LcmTrn::handleMessages() - handling 42 messages took 0.66 ms and 129 calls... 2021-02-12T23:33:35Z- waiting for 1613172813.78, time is 1613172815.68 2021-02-12T23:33:35Z- LcmTrn::updateTrn() equating pose 1613172815.68 and meas times 1613172815.68 2021-02-12T23:33:35Z- LcmTrn::updateTrn() >>>> pose time 1613172815.68, meas time 1613172815.68 <<<< 2021-02-12T23:33:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172815.68 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:33:35Z- TerrainNav::motionUpdate() - currEstimate = (4057280, 591151.70, 9.07) 2021-02-12T23:33:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:33:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:33:35Z- TNavBF::Projecting Measurements 2021-02-12T23:33:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:33:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:33:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:33:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:33:35Z- TNavFilter::4 good beams. 2021-02-12T23:33:35Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:33:35Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:33:35Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:33:35Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:33:35Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:33:35Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:33:35Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:33:35Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:35Z- TerrainMapOctree: (vehN, vehE ) = (4057279.73, 591151.70). 2021-02-12T23:33:35Z- TerrainMapOctree: Min Distance = 5036.12. 2021-02-12T23:33:35Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:33:35Z- There is no specific good beam that all particles have in common. 2021-02-12T23:33:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:33:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 11.67 2021-02-12T23:33:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:33:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 16.53 2021-02-12T23:33:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:33:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 19.75 2021-02-12T23:33:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:33:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 10.24 2021-02-12T23:33:35Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:33:35Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:33:35Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:33:35Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:33:36Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:33:36Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:33:36Z- Filter 0 No beams used in subcloud update 2021-02-12T23:33:36Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:33:36Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:33:36Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 754.7 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:33:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172815.68 sec. 2021-02-12T23:33:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:33:36Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:36Z- variances([0], [2], and psi_berg) = 754.729383 833.037569 0.000000 2021-02-12T23:33:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:33:36Z- LcmTrn::publishEstimates() - MLE : 4057265.23 591113.20 9.07 2021-02-12T23:33:36Z- LcmTrn::publishEstimates() - MMSE : 4057279.73 591151.70 9.07 2021-02-12T23:33:36Z- LcmTrn::publishEstimates() - COVAR: 754.73 833.04 0.83 0.00 2021-02-12T23:33:36Z- AHRS_M2 msg: 1613172816.02 epoch sec; seqNo:4784 2021-02-12T23:33:36Z- AHRS_M2 msg: 0.67 phi; 0.26 theta; -0.15 psi 2021-02-12T23:33:36Z- handleDepth()-Depth_Keller msg: 1613172816020 epoch sec; seqNo:4832; depth 8.96 2021-02-12T23:33:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172816.04 epoch sec; seqNo:4832; 4057279.73 north; 591151.70 east 2021-02-12T23:33:36Z- AHRS_M2 msg: 1613172816.42 epoch sec; seqNo:4785 2021-02-12T23:33:36Z- AHRS_M2 msg: 0.67 phi; 0.27 theta; -0.16 psi 2021-02-12T23:33:36Z- handleDepth()-Depth_Keller msg: 1613172816429 epoch sec; seqNo:4833; depth 8.90 2021-02-12T23:33:36Z- handleDvl() - RDI_Pathfinder msg: 1613172816.44 epoch sec; seqNo:2352 2021-02-12T23:33:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 101.57 , 109.68 , 79.69 2021-02-12T23:33:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.05 , -0.14 2021-02-12T23:33:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172816.46 epoch sec; seqNo:4833; 4057280.11 north; 591151.70 east 2021-02-12T23:33:36Z- AHRS_M2 msg: 1613172816.83 epoch sec; seqNo:4786 2021-02-12T23:33:36Z- AHRS_M2 msg: 0.67 phi; 0.27 theta; -0.16 psi 2021-02-12T23:33:36Z- handleDepth()-Depth_Keller msg: 1613172816833 epoch sec; seqNo:4834; depth 8.81 2021-02-12T23:33:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172816.84 epoch sec; seqNo:4834; 4057280.49 north; 591151.70 east 2021-02-12T23:33:37Z- AHRS_M2 msg: 1613172817.23 epoch sec; seqNo:4787 2021-02-12T23:33:37Z- AHRS_M2 msg: 0.67 phi; 0.28 theta; -0.16 psi 2021-02-12T23:33:37Z- handleDepth()-Depth_Keller msg: 1613172817234 epoch sec; seqNo:4835; depth 8.74 2021-02-12T23:33:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172817.24 epoch sec; seqNo:4835; 4057280.49 north; 591151.70 east 2021-02-12T23:33:37Z- AHRS_M2 msg: 1613172817.63 epoch sec; seqNo:4788 2021-02-12T23:33:37Z- AHRS_M2 msg: 0.67 phi; 0.29 theta; -0.16 psi 2021-02-12T23:33:37Z- handleDepth()-Depth_Keller msg: 1613172817636 epoch sec; seqNo:4836; depth 8.64 2021-02-12T23:33:37Z- handleDvl() - RDI_Pathfinder msg: 1613172817.65 epoch sec; seqNo:2353 2021-02-12T23:33:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 96.16 , 112.38 , 79.69 2021-02-12T23:33:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.17 , -0.09 2021-02-12T23:33:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172817.66 epoch sec; seqNo:4836; 4057280.87 north; 591151.69 east 2021-02-12T23:33:38Z- AHRS_M2 msg: 1613172818.04 epoch sec; seqNo:4789 2021-02-12T23:33:38Z- AHRS_M2 msg: 0.66 phi; 0.29 theta; -0.16 psi 2021-02-12T23:33:38Z- handleDepth()-Depth_Keller msg: 1613172818044 epoch sec; seqNo:4837; depth 8.55 2021-02-12T23:33:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172818.11 epoch sec; seqNo:4837; 4057281.26 north; 591152.91 east 2021-02-12T23:33:38Z- AHRS_M2 msg: 1613172818.44 epoch sec; seqNo:4790 2021-02-12T23:33:38Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.17 psi 2021-02-12T23:33:38Z- handleDepth()-Depth_Keller msg: 1613172818446 epoch sec; seqNo:4838; depth 8.45 2021-02-12T23:33:38Z- handleDvl() - RDI_Pathfinder msg: 1613172818.46 epoch sec; seqNo:2354 2021-02-12T23:33:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.76 , 101.63 , 112.45 , 78.87 2021-02-12T23:33:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.24 , 0.06 2021-02-12T23:33:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172818.47 epoch sec; seqNo:4838; 4057281.26 north; 591152.91 east 2021-02-12T23:33:38Z- AHRS_M2 msg: 1613172818.84 epoch sec; seqNo:4791 2021-02-12T23:33:38Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-12T23:33:38Z- handleDepth()-Depth_Keller msg: 1613172818853 epoch sec; seqNo:4839; depth 8.35 2021-02-12T23:33:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172818.87 epoch sec; seqNo:4839; 4057281.64 north; 591152.90 east 2021-02-12T23:33:39Z- AHRS_M2 msg: 1613172819.25 epoch sec; seqNo:4792 2021-02-12T23:33:39Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.20 psi 2021-02-12T23:33:39Z- handleDepth()-Depth_Keller msg: 1613172819252 epoch sec; seqNo:4840; depth 8.25 2021-02-12T23:33:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172819.26 epoch sec; seqNo:4840; 4057282.02 north; 591152.90 east 2021-02-12T23:33:39Z- AHRS_M2 msg: 1613172819.65 epoch sec; seqNo:4793 2021-02-12T23:33:39Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.21 psi 2021-02-12T23:33:39Z- handleDepth()-Depth_Keller msg: 1613172819657 epoch sec; seqNo:4841; depth 8.14 2021-02-12T23:33:39Z- handleDvl() - RDI_Pathfinder msg: 1613172819.67 epoch sec; seqNo:2355 2021-02-12T23:33:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 105.56 , 117.72 , 79.69 2021-02-12T23:33:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.21 , 0.17 2021-02-12T23:33:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172819.68 epoch sec; seqNo:4841; 4057282.40 north; 591152.90 east 2021-02-12T23:33:40Z- AHRS_M2 msg: 1613172820.06 epoch sec; seqNo:4794 2021-02-12T23:33:40Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.22 psi 2021-02-12T23:33:40Z- handleDepth()-Depth_Keller msg: 1613172820069 epoch sec; seqNo:4842; depth 8.03 2021-02-12T23:33:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172820.08 epoch sec; seqNo:4842; 4057282.78 north; 591152.89 east 2021-02-12T23:33:40Z- AHRS_M2 msg: 1613172820.46 epoch sec; seqNo:4795 2021-02-12T23:33:40Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.22 psi 2021-02-12T23:33:40Z- handleDepth()-Depth_Keller msg: 1613172820468 epoch sec; seqNo:4843; depth 7.90 2021-02-12T23:33:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172820.51 epoch sec; seqNo:4843; 4057283.15 north; 591152.89 east 2021-02-12T23:33:40Z- AHRS_M2 msg: 1613172820.86 epoch sec; seqNo:4796 2021-02-12T23:33:40Z- AHRS_M2 msg: 0.55 phi; 0.35 theta; -0.22 psi 2021-02-12T23:33:40Z- handleDepth()-Depth_Keller msg: 1613172820868 epoch sec; seqNo:4844; depth 7.79 2021-02-12T23:33:40Z- handleDvl() - RDI_Pathfinder msg: 1613172820.88 epoch sec; seqNo:2356 2021-02-12T23:33:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.78 , 108.06 , 118.87 , 81.02 2021-02-12T23:33:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.08 , 0.22 2021-02-12T23:33:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172820.89 epoch sec; seqNo:4844; 4057283.15 north; 591152.89 east 2021-02-12T23:33:41Z- LcmTrn::handleMessages() - handling 44 messages took 0.74 ms and 130 calls... 2021-02-12T23:33:41Z- waiting for 1613172818.68, time is 1613172820.89 2021-02-12T23:33:41Z- LcmTrn::updateTrn() equating pose 1613172820.89 and meas times 1613172820.89 2021-02-12T23:33:41Z- LcmTrn::updateTrn() >>>> pose time 1613172820.89, meas time 1613172820.89 <<<< 2021-02-12T23:33:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172820.89 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:33:41Z- TerrainNav::motionUpdate() - currEstimate = (4057283, 591152.89, 7.79) 2021-02-12T23:33:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:33:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:33:41Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172820.89 sec.; No relevant navigation data available 2021-02-12T23:33:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:33:41Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:41Z- variances([0], [2], and psi_berg) = 754.630666 833.037472 0.000000 2021-02-12T23:33:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:33:41Z- LcmTrn::publishEstimates() - MLE : 4057268.65 591114.39 7.79 2021-02-12T23:33:41Z- LcmTrn::publishEstimates() - MMSE : 4057283.15 591152.89 7.79 2021-02-12T23:33:41Z- LcmTrn::publishEstimates() - COVAR: 754.63 833.04 1.02 0.00 2021-02-12T23:33:41Z- AHRS_M2 msg: 1613172821.27 epoch sec; seqNo:4797 2021-02-12T23:33:41Z- AHRS_M2 msg: 0.55 phi; 0.37 theta; -0.22 psi 2021-02-12T23:33:41Z- handleDepth()-Depth_Keller msg: 1613172821273 epoch sec; seqNo:4845; depth 7.69 2021-02-12T23:33:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172821.29 epoch sec; seqNo:4845; 4057283.53 north; 591152.88 east 2021-02-12T23:33:41Z- AHRS_M2 msg: 1613172821.67 epoch sec; seqNo:4798 2021-02-12T23:33:41Z- AHRS_M2 msg: 0.56 phi; 0.37 theta; -0.20 psi 2021-02-12T23:33:41Z- handleDepth()-Depth_Keller msg: 1613172821676 epoch sec; seqNo:4846; depth 7.59 2021-02-12T23:33:41Z- handleDvl() - RDI_Pathfinder msg: 1613172821.70 epoch sec; seqNo:2357 2021-02-12T23:33:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 110.62 , 122.79 , 80.88 2021-02-12T23:33:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.06 , 0.16 2021-02-12T23:33:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172821.71 epoch sec; seqNo:4846; 4057283.93 north; 591154.10 east 2021-02-12T23:33:42Z- AHRS_M2 msg: 1613172822.08 epoch sec; seqNo:4799 2021-02-12T23:33:42Z- AHRS_M2 msg: 0.57 phi; 0.37 theta; -0.17 psi 2021-02-12T23:33:42Z- handleDepth()-Depth_Keller msg: 1613172822080 epoch sec; seqNo:4847; depth 7.46 2021-02-12T23:33:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172822.09 epoch sec; seqNo:4847; 4057284.30 north; 591154.10 east 2021-02-12T23:33:42Z- AHRS_M2 msg: 1613172822.48 epoch sec; seqNo:4800 2021-02-12T23:33:42Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.16 psi 2021-02-12T23:33:42Z- handleDepth()-Depth_Keller msg: 1613172822494 epoch sec; seqNo:4848; depth 7.36 2021-02-12T23:33:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172822.53 epoch sec; seqNo:4848; 4057284.68 north; 591154.09 east 2021-02-12T23:33:42Z- AHRS_M2 msg: 1613172822.88 epoch sec; seqNo:4801 2021-02-12T23:33:42Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.14 psi 2021-02-12T23:33:42Z- handleDepth()-Depth_Keller msg: 1613172822890 epoch sec; seqNo:4849; depth 7.24 2021-02-12T23:33:42Z- handleDvl() - RDI_Pathfinder msg: 1613172822.90 epoch sec; seqNo:2358 2021-02-12T23:33:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.21 , 109.14 , 126.71 , 82.10 2021-02-12T23:33:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.09 , -0.05 2021-02-12T23:33:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172822.91 epoch sec; seqNo:4849; 4057284.68 north; 591154.09 east 2021-02-12T23:33:43Z- AHRS_M2 msg: 1613172823.29 epoch sec; seqNo:4802 2021-02-12T23:33:43Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-12T23:33:43Z- handleDepth()-Depth_Keller msg: 1613172823292 epoch sec; seqNo:4850; depth 7.11 2021-02-12T23:33:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172823.31 epoch sec; seqNo:4850; 4057285.06 north; 591154.09 east 2021-02-12T23:33:43Z- AHRS_M2 msg: 1613172823.69 epoch sec; seqNo:4803 2021-02-12T23:33:43Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.13 psi 2021-02-12T23:33:43Z- handleDepth()-Depth_Keller msg: 1613172823696 epoch sec; seqNo:4851; depth 7.01 2021-02-12T23:33:43Z- handleDvl() - RDI_Pathfinder msg: 1613172823.71 epoch sec; seqNo:2359 2021-02-12T23:33:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.97 , 105.35 , 123.20 , 81.56 2021-02-12T23:33:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.05 , -0.14 2021-02-12T23:33:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172823.72 epoch sec; seqNo:4851; 4057285.44 north; 591154.09 east 2021-02-12T23:33:44Z- AHRS_M2 msg: 1613172824.10 epoch sec; seqNo:4804 2021-02-12T23:33:44Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.13 psi 2021-02-12T23:33:44Z- handleDepth()-Depth_Keller msg: 1613172824109 epoch sec; seqNo:4852; depth 6.87 2021-02-12T23:33:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172824.12 epoch sec; seqNo:4852; 4057285.45 north; 591155.31 east 2021-02-12T23:33:44Z- AHRS_M2 msg: 1613172824.50 epoch sec; seqNo:4805 2021-02-12T23:33:44Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.14 psi 2021-02-12T23:33:44Z- handleDepth()-Depth_Keller msg: 1613172824504 epoch sec; seqNo:4853; depth 6.76 2021-02-12T23:33:44Z- handleDvl() - RDI_Pathfinder msg: 1613172824.52 epoch sec; seqNo:2360 2021-02-12T23:33:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 105.08 , 118.60 , 78.05 2021-02-12T23:33:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.14 , 0.06 2021-02-12T23:33:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172824.53 epoch sec; seqNo:4853; 4057285.83 north; 591155.30 east 2021-02-12T23:33:44Z- AHRS_M2 msg: 1613172824.90 epoch sec; seqNo:4806 2021-02-12T23:33:44Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.14 psi 2021-02-12T23:33:44Z- handleDepth()-Depth_Keller msg: 1613172824908 epoch sec; seqNo:4854; depth 6.63 2021-02-12T23:33:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172824.92 epoch sec; seqNo:4854; 4057286.21 north; 591155.30 east 2021-02-12T23:33:45Z- AHRS_M2 msg: 1613172825.31 epoch sec; seqNo:4807 2021-02-12T23:33:45Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.14 psi 2021-02-12T23:33:45Z- handleDepth()-Depth_Keller msg: 1613172825312 epoch sec; seqNo:4855; depth 6.52 2021-02-12T23:33:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172825.32 epoch sec; seqNo:4855; 4057286.21 north; 591155.30 east 2021-02-12T23:33:45Z- AHRS_M2 msg: 1613172825.71 epoch sec; seqNo:4808 2021-02-12T23:33:45Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-12T23:33:45Z- handleDepth()-Depth_Keller msg: 1613172825718 epoch sec; seqNo:4856; depth 6.43 2021-02-12T23:33:45Z- handleDvl() - RDI_Pathfinder msg: 1613172825.73 epoch sec; seqNo:2361 2021-02-12T23:33:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 103.80 , 118.67 , 79.69 2021-02-12T23:33:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.05 , 0.26 2021-02-12T23:33:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172825.75 epoch sec; seqNo:4856; 4057286.59 north; 591155.29 east 2021-02-12T23:33:46Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-12T23:33:46Z- waiting for 1613172823.89, time is 1613172825.75 2021-02-12T23:33:46Z- LcmTrn::updateTrn() equating pose 1613172825.75 and meas times 1613172825.75 2021-02-12T23:33:46Z- LcmTrn::updateTrn() >>>> pose time 1613172825.75, meas time 1613172825.75 <<<< 2021-02-12T23:33:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172825.75 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:33:46Z- TerrainNav::motionUpdate() - currEstimate = (4057287, 591155.29, 6.43) 2021-02-12T23:33:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:33:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:33:46Z- TNavBF::Projecting Measurements 2021-02-12T23:33:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:33:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:33:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:33:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:33:46Z- TNavFilter::4 good beams. 2021-02-12T23:33:46Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:33:46Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:33:46Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:33:46Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:33:46Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:33:46Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:33:46Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:33:46Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:46Z- TerrainMapOctree: (vehN, vehE ) = (4057286.59, 591155.29). 2021-02-12T23:33:46Z- TerrainMapOctree: Min Distance = 5028.50. 2021-02-12T23:33:46Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:33:46Z- There is no specific good beam that all particles have in common. 2021-02-12T23:33:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:33:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 15.07 2021-02-12T23:33:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:33:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 17.24 2021-02-12T23:33:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:33:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 22.53 2021-02-12T23:33:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:33:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 10.16 2021-02-12T23:33:46Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:33:46Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:33:46Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:33:46Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:33:46Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:33:46Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:33:46Z- Filter 0 No beams used in subcloud update 2021-02-12T23:33:46Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:33:46Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:33:46Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 754.5 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:33:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172825.75 sec. 2021-02-12T23:33:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:33:46Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:46Z- variances([0], [2], and psi_berg) = 754.524578 833.037367 0.000000 2021-02-12T23:33:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:33:46Z- LcmTrn::publishEstimates() - MLE : 4057272.09 591116.79 6.43 2021-02-12T23:33:46Z- LcmTrn::publishEstimates() - MMSE : 4057286.59 591155.29 6.43 2021-02-12T23:33:46Z- LcmTrn::publishEstimates() - COVAR: 754.52 833.04 0.95 0.00 2021-02-12T23:33:46Z- AHRS_M2 msg: 1613172826.12 epoch sec; seqNo:4809 2021-02-12T23:33:46Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.13 psi 2021-02-12T23:33:46Z- handleDepth()-Depth_Keller msg: 1613172826123 epoch sec; seqNo:4857; depth 6.30 2021-02-12T23:33:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172826.15 epoch sec; seqNo:4857; 4057286.97 north; 591155.29 east 2021-02-12T23:33:46Z- AHRS_M2 msg: 1613172826.52 epoch sec; seqNo:4810 2021-02-12T23:33:46Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.13 psi 2021-02-12T23:33:46Z- handleDepth()-Depth_Keller msg: 1613172826528 epoch sec; seqNo:4858; depth 6.20 2021-02-12T23:33:46Z- handleDvl() - RDI_Pathfinder msg: 1613172826.55 epoch sec; seqNo:2362 2021-02-12T23:33:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.98 , 102.44 , 120.02 , 82.17 2021-02-12T23:33:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.24 , 0.13 2021-02-12T23:33:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172826.56 epoch sec; seqNo:4858; 4057286.97 north; 591155.29 east 2021-02-12T23:33:46Z- AHRS_M2 msg: 1613172826.92 epoch sec; seqNo:4811 2021-02-12T23:33:46Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.13 psi 2021-02-12T23:33:46Z- handleDepth()-Depth_Keller msg: 1613172826928 epoch sec; seqNo:4859; depth 6.08 2021-02-12T23:33:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172826.94 epoch sec; seqNo:4859; 4057287.36 north; 591156.51 east 2021-02-12T23:33:47Z- AHRS_M2 msg: 1613172827.33 epoch sec; seqNo:4812 2021-02-12T23:33:47Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.14 psi 2021-02-12T23:33:47Z- handleDepth()-Depth_Keller msg: 1613172827338 epoch sec; seqNo:4860; depth 5.96 2021-02-12T23:33:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172827.35 epoch sec; seqNo:4860; 4057287.36 north; 591156.51 east 2021-02-12T23:33:47Z- AHRS_M2 msg: 1613172827.74 epoch sec; seqNo:4813 2021-02-12T23:33:47Z- AHRS_M2 msg: 0.64 phi; 0.26 theta; -0.20 psi 2021-02-12T23:33:47Z- handleDepth()-Depth_Keller msg: 1613172827746 epoch sec; seqNo:4861; depth 5.86 2021-02-12T23:33:47Z- handleDvl() - RDI_Pathfinder msg: 1613172827.76 epoch sec; seqNo:2363 2021-02-12T23:33:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.70 , 102.51 , 116.03 , 82.23 2021-02-12T23:33:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.10 , -0.08 2021-02-12T23:33:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172827.77 epoch sec; seqNo:4861; 4057287.74 north; 591156.50 east 2021-02-12T23:33:48Z- AHRS_M2 msg: 1613172828.14 epoch sec; seqNo:4814 2021-02-12T23:33:48Z- AHRS_M2 msg: 0.63 phi; 0.23 theta; -0.24 psi 2021-02-12T23:33:48Z- handleDepth()-Depth_Keller msg: 1613172828145 epoch sec; seqNo:4862; depth 5.74 2021-02-12T23:33:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172828.17 epoch sec; seqNo:4862; 4057288.12 north; 591156.50 east 2021-02-12T23:33:48Z- AHRS_M2 msg: 1613172828.54 epoch sec; seqNo:4815 2021-02-12T23:33:48Z- AHRS_M2 msg: 0.63 phi; 0.20 theta; -0.25 psi 2021-02-12T23:33:48Z- handleDepth()-Depth_Keller msg: 1613172828549 epoch sec; seqNo:4863; depth 5.61 2021-02-12T23:33:48Z- handleDvl() - RDI_Pathfinder msg: 1613172828.56 epoch sec; seqNo:2364 2021-02-12T23:33:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.71 , 106.64 , 112.04 , 85.00 2021-02-12T23:33:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.07 , -0.02 2021-02-12T23:33:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172828.57 epoch sec; seqNo:4863; 4057288.50 north; 591156.49 east 2021-02-12T23:33:48Z- AHRS_M2 msg: 1613172828.94 epoch sec; seqNo:4816 2021-02-12T23:33:48Z- AHRS_M2 msg: 0.64 phi; 0.17 theta; -0.25 psi 2021-02-12T23:33:48Z- handleDepth()-Depth_Keller msg: 1613172828950 epoch sec; seqNo:4864; depth 5.51 2021-02-12T23:33:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172828.98 epoch sec; seqNo:4864; 4057288.51 north; 591157.72 east 2021-02-12T23:33:49Z- AHRS_M2 msg: 1613172829.35 epoch sec; seqNo:4817 2021-02-12T23:33:49Z- AHRS_M2 msg: 0.65 phi; 0.14 theta; -0.22 psi 2021-02-12T23:33:49Z- handleDepth()-Depth_Keller msg: 1613172829354 epoch sec; seqNo:4865; depth 5.44 2021-02-12T23:33:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172829.37 epoch sec; seqNo:4865; 4057288.89 north; 591157.71 east 2021-02-12T23:33:49Z- AHRS_M2 msg: 1613172829.75 epoch sec; seqNo:4818 2021-02-12T23:33:49Z- AHRS_M2 msg: 0.67 phi; 0.10 theta; -0.19 psi 2021-02-12T23:33:49Z- handleDepth()-Depth_Keller msg: 1613172829757 epoch sec; seqNo:4866; depth 5.37 2021-02-12T23:33:49Z- handleDvl() - RDI_Pathfinder msg: 1613172829.77 epoch sec; seqNo:2365 2021-02-12T23:33:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.42 , 109.41 , 108.06 , 87.78 2021-02-12T23:33:49Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.08 , 0.09 2021-02-12T23:33:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172829.79 epoch sec; seqNo:4866; 4057289.27 north; 591157.71 east 2021-02-12T23:33:50Z- AHRS_M2 msg: 1613172830.16 epoch sec; seqNo:4819 2021-02-12T23:33:50Z- AHRS_M2 msg: 0.68 phi; 0.07 theta; -0.16 psi 2021-02-12T23:33:50Z- handleDepth()-Depth_Keller msg: 1613172830161 epoch sec; seqNo:4867; depth 5.30 2021-02-12T23:33:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172830.17 epoch sec; seqNo:4867; 4057289.65 north; 591157.70 east 2021-02-12T23:33:50Z- AHRS_M2 msg: 1613172830.56 epoch sec; seqNo:4820 2021-02-12T23:33:50Z- AHRS_M2 msg: 0.70 phi; 0.03 theta; -0.13 psi 2021-02-12T23:33:50Z- handleDepth()-Depth_Keller msg: 1613172830566 epoch sec; seqNo:4868; depth 5.26 2021-02-12T23:33:50Z- handleDvl() - RDI_Pathfinder msg: 1613172830.58 epoch sec; seqNo:2366 2021-02-12T23:33:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.65 , 101.30 , 97.24 2021-02-12T23:33:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.34 , -0.18 2021-02-12T23:33:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172830.59 epoch sec; seqNo:4868; 4057289.66 north; 591158.92 east 2021-02-12T23:33:50Z- AHRS_M2 msg: 1613172830.96 epoch sec; seqNo:4821 2021-02-12T23:33:50Z- AHRS_M2 msg: 0.72 phi; -0.00 theta; -0.12 psi 2021-02-12T23:33:50Z- handleDepth()-Depth_Keller msg: 1613172830968 epoch sec; seqNo:4869; depth 5.22 2021-02-12T23:33:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172830.99 epoch sec; seqNo:4869; 4057290.04 north; 591158.92 east 2021-02-12T23:33:51Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-12T23:33:51Z- waiting for 1613172828.75, time is 1613172830.99 2021-02-12T23:33:51Z- LcmTrn::updateTrn() >>>> pose time 1613172830.99, meas time 1613172830.58 <<<< 2021-02-12T23:33:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172830.58 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:33:51Z- TerrainNav::motionUpdate() - currEstimate = (4057290, 591158.92, 5.22) 2021-02-12T23:33:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:33:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:33:51Z- TNavBF::Projecting Measurements 2021-02-12T23:33:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:33:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:33:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:33:51Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:33:51Z- TNavFilter::3 good beams. 2021-02-12T23:33:51Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:33:51Z- TNavBF:: beamIndex[0] = 1 2021-02-12T23:33:51Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:33:51Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:33:51Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:33:51Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:33:51Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:33:51Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:51Z- TerrainMapOctree: (vehN, vehE ) = (4057289.77, 591158.64). 2021-02-12T23:33:51Z- TerrainMapOctree: Min Distance = 5023.94. 2021-02-12T23:33:51Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:33:51Z- There is no specific good beam that all particles have in common. 2021-02-12T23:33:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:33:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 16.86 2021-02-12T23:33:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:33:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 16.42 2021-02-12T23:33:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:33:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-12T23:33:51Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:33:51Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:33:51Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:33:51Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:33:51Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:33:51Z- Filter 0 No beams used in subcloud update 2021-02-12T23:33:51Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:33:51Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:33:51Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 754.4 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:33:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172830.58 sec. 2021-02-12T23:33:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:33:51Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:51Z- variances([0], [2], and psi_berg) = 754.381164 833.037226 0.000000 2021-02-12T23:33:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:33:51Z- LcmTrn::publishEstimates() - MLE : 4057275.54 591120.42 5.22 2021-02-12T23:33:51Z- LcmTrn::publishEstimates() - MMSE : 4057290.04 591158.92 5.22 2021-02-12T23:33:51Z- LcmTrn::publishEstimates() - COVAR: 754.38 833.04 1.01 0.00 2021-02-12T23:33:51Z- AHRS_M2 msg: 1613172831.37 epoch sec; seqNo:4822 2021-02-12T23:33:51Z- AHRS_M2 msg: 0.73 phi; -0.03 theta; -0.12 psi 2021-02-12T23:33:51Z- handleDepth()-Depth_Keller msg: 1613172831377 epoch sec; seqNo:4870; depth 5.18 2021-02-12T23:33:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172831.39 epoch sec; seqNo:4870; 4057290.04 north; 591158.92 east 2021-02-12T23:33:51Z- AHRS_M2 msg: 1613172831.78 epoch sec; seqNo:4823 2021-02-12T23:33:51Z- AHRS_M2 msg: 0.75 phi; -0.06 theta; -0.11 psi 2021-02-12T23:33:51Z- handleDepth()-Depth_Keller msg: 1613172831789 epoch sec; seqNo:4871; depth 5.13 2021-02-12T23:33:51Z- handleDvl() - RDI_Pathfinder msg: 1613172831.82 epoch sec; seqNo:2367 2021-02-12T23:33:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 96.97 , 96.97 , 92.91 2021-02-12T23:33:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.09 , -0.56 2021-02-12T23:33:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172831.83 epoch sec; seqNo:4871; 4057290.42 north; 591158.92 east 2021-02-12T23:33:52Z- AHRS_M2 msg: 1613172832.18 epoch sec; seqNo:4824 2021-02-12T23:33:52Z- AHRS_M2 msg: 0.76 phi; -0.10 theta; -0.12 psi 2021-02-12T23:33:52Z- handleDepth()-Depth_Keller msg: 1613172832182 epoch sec; seqNo:4872; depth 5.14 2021-02-12T23:33:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172832.19 epoch sec; seqNo:4872; 4057290.80 north; 591158.91 east 2021-02-12T23:33:52Z- AHRS_M2 msg: 1613172832.58 epoch sec; seqNo:4825 2021-02-12T23:33:52Z- AHRS_M2 msg: 0.77 phi; -0.13 theta; -0.14 psi 2021-02-12T23:33:52Z- handleDepth()-Depth_Keller msg: 1613172832584 epoch sec; seqNo:4873; depth 5.13 2021-02-12T23:33:52Z- handleDvl() - RDI_Pathfinder msg: 1613172832.60 epoch sec; seqNo:2368 2021-02-12T23:33:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.38 , 96.02 , 97.38 , 96.02 2021-02-12T23:33:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , -0.28 , -0.56 2021-02-12T23:33:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172832.61 epoch sec; seqNo:4873; 4057290.81 north; 591160.13 east 2021-02-12T23:33:52Z- AHRS_M2 msg: 1613172832.98 epoch sec; seqNo:4826 2021-02-12T23:33:52Z- AHRS_M2 msg: 0.77 phi; -0.17 theta; -0.18 psi 2021-02-12T23:33:52Z- handleDepth()-Depth_Keller msg: 1613172832988 epoch sec; seqNo:4874; depth 5.12 2021-02-12T23:33:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172833.00 epoch sec; seqNo:4874; 4057291.19 north; 591160.13 east 2021-02-12T23:33:53Z- AHRS_M2 msg: 1613172833.39 epoch sec; seqNo:4827 2021-02-12T23:33:53Z- AHRS_M2 msg: 0.76 phi; -0.21 theta; -0.22 psi 2021-02-12T23:33:53Z- handleDepth()-Depth_Keller msg: 1613172833405 epoch sec; seqNo:4875; depth 5.13 2021-02-12T23:33:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172833.44 epoch sec; seqNo:4875; 4057291.57 north; 591160.13 east 2021-02-12T23:33:53Z- AHRS_M2 msg: 1613172833.80 epoch sec; seqNo:4828 2021-02-12T23:33:53Z- AHRS_M2 msg: 0.75 phi; -0.25 theta; -0.25 psi 2021-02-12T23:33:53Z- handleDepth()-Depth_Keller msg: 1613172833801 epoch sec; seqNo:4876; depth 5.14 2021-02-12T23:33:53Z- handleDvl() - RDI_Pathfinder msg: 1613172833.81 epoch sec; seqNo:2369 2021-02-12T23:33:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.36 , 86.49 , 102.72 2021-02-12T23:33:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.45 , -0.59 , -0.18 2021-02-12T23:33:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172833.82 epoch sec; seqNo:4876; 4057291.95 north; 591160.12 east 2021-02-12T23:33:54Z- AHRS_M2 msg: 1613172834.20 epoch sec; seqNo:4829 2021-02-12T23:33:54Z- AHRS_M2 msg: 0.74 phi; -0.28 theta; -0.26 psi 2021-02-12T23:33:54Z- handleDepth()-Depth_Keller msg: 1613172834215 epoch sec; seqNo:4877; depth 5.16 2021-02-12T23:33:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172834.23 epoch sec; seqNo:4877; 4057292.32 north; 591160.12 east 2021-02-12T23:33:54Z- AHRS_M2 msg: 1613172834.60 epoch sec; seqNo:4830 2021-02-12T23:33:54Z- AHRS_M2 msg: 0.73 phi; -0.31 theta; -0.26 psi 2021-02-12T23:33:54Z- handleDepth()-Depth_Keller msg: 1613172834604 epoch sec; seqNo:4878; depth 5.19 2021-02-12T23:33:54Z- handleDvl() - RDI_Pathfinder msg: 1613172834.62 epoch sec; seqNo:2370 2021-02-12T23:33:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 112.18 , 85.14 , 113.53 2021-02-12T23:33:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.45 , -0.39 , 0.50 2021-02-12T23:33:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172834.64 epoch sec; seqNo:4878; 4057292.32 north; 591160.12 east 2021-02-12T23:33:55Z- AHRS_M2 msg: 1613172835.00 epoch sec; seqNo:4831 2021-02-12T23:33:55Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.25 psi 2021-02-12T23:33:55Z- handleDepth()-Depth_Keller msg: 1613172835008 epoch sec; seqNo:4879; depth 5.25 2021-02-12T23:33:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172835.02 epoch sec; seqNo:4879; 4057292.32 north; 591160.12 east 2021-02-12T23:33:55Z- AHRS_M2 msg: 1613172835.41 epoch sec; seqNo:4832 2021-02-12T23:33:55Z- AHRS_M2 msg: 0.70 phi; -0.34 theta; -0.25 psi 2021-02-12T23:33:55Z- handleDepth()-Depth_Keller msg: 1613172835414 epoch sec; seqNo:4880; depth 5.32 2021-02-12T23:33:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172835.42 epoch sec; seqNo:4880; 4057292.70 north; 591160.11 east 2021-02-12T23:33:55Z- AHRS_M2 msg: 1613172835.81 epoch sec; seqNo:4833 2021-02-12T23:33:55Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.26 psi 2021-02-12T23:33:55Z- handleDepth()-Depth_Keller msg: 1613172835818 epoch sec; seqNo:4881; depth 5.40 2021-02-12T23:33:55Z- handleDvl() - RDI_Pathfinder msg: 1613172835.84 epoch sec; seqNo:2371 2021-02-12T23:33:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 113.33 , 83.58 , 124.14 2021-02-12T23:33:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.57 , 0.14 , 0.56 2021-02-12T23:33:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172835.85 epoch sec; seqNo:4881; 4057292.70 north; 591160.11 east 2021-02-12T23:33:56Z- AHRS_M2 msg: 1613172836.22 epoch sec; seqNo:4834 2021-02-12T23:33:56Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.27 psi 2021-02-12T23:33:56Z- handleDepth()-Depth_Keller msg: 1613172836220 epoch sec; seqNo:4882; depth 5.48 2021-02-12T23:33:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172836.23 epoch sec; seqNo:4882; 4057292.70 north; 591160.11 east 2021-02-12T23:33:56Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-12T23:33:56Z- waiting for 1613172833.99, time is 1613172836.23 2021-02-12T23:33:56Z- LcmTrn::updateTrn() >>>> pose time 1613172836.23, meas time 1613172835.84 <<<< 2021-02-12T23:33:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172835.84 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:33:56Z- TerrainNav::motionUpdate() - currEstimate = (4057293, 591160.11, 5.48) 2021-02-12T23:33:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:33:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:33:56Z- TNavBF::Projecting Measurements 2021-02-12T23:33:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:33:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:33:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:33:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:33:56Z- TNavFilter::4 good beams. 2021-02-12T23:33:56Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:33:56Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:33:56Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:33:56Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:33:56Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:33:56Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:33:56Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:33:56Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:56Z- TerrainMapOctree: (vehN, vehE ) = (4057292.50, 591160.02). 2021-02-12T23:33:56Z- TerrainMapOctree: Min Distance = 5020.93. 2021-02-12T23:33:56Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:33:56Z- There is no specific good beam that all particles have in common. 2021-02-12T23:33:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:33:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 12.29 2021-02-12T23:33:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:33:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 20.55 2021-02-12T23:33:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:33:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 11.18 2021-02-12T23:33:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:33:56Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.034 ExpectedMeasDiff: nan currMeas.covariance: 24.66 2021-02-12T23:33:56Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:33:56Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:33:56Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:33:56Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:33:56Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:33:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:33:56Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:33:56Z- Filter 0 No beams used in subcloud update 2021-02-12T23:33:56Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:33:56Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:33:56Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 754.3 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:33:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172835.84 sec. 2021-02-12T23:33:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:33:56Z- TNavBF::mmse selected from filter -1 2021-02-12T23:33:56Z- variances([0], [2], and psi_berg) = 754.272158 833.037119 0.000000 2021-02-12T23:33:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:33:56Z- LcmTrn::publishEstimates() - MLE : 4057278.20 591121.61 5.48 2021-02-12T23:33:56Z- LcmTrn::publishEstimates() - MMSE : 4057292.70 591160.11 5.48 2021-02-12T23:33:56Z- LcmTrn::publishEstimates() - COVAR: 754.27 833.04 0.92 0.00 2021-02-12T23:33:56Z- AHRS_M2 msg: 1613172836.62 epoch sec; seqNo:4835 2021-02-12T23:33:56Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.28 psi 2021-02-12T23:33:56Z- handleDepth()-Depth_Keller msg: 1613172836629 epoch sec; seqNo:4883; depth 5.58 2021-02-12T23:33:56Z- handleDvl() - RDI_Pathfinder msg: 1613172836.64 epoch sec; seqNo:2372 2021-02-12T23:33:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 114.28 , 83.04 , 126.17 2021-02-12T23:33:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.29 , 0.16 2021-02-12T23:33:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172836.65 epoch sec; seqNo:4883; 4057292.70 north; 591160.11 east 2021-02-12T23:33:57Z- AHRS_M2 msg: 1613172837.03 epoch sec; seqNo:4836 2021-02-12T23:33:57Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.27 psi 2021-02-12T23:33:57Z- handleDepth()-Depth_Keller msg: 1613172837036 epoch sec; seqNo:4884; depth 5.71 2021-02-12T23:33:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172837.05 epoch sec; seqNo:4884; 4057293.08 north; 591160.11 east 2021-02-12T23:33:57Z- AHRS_M2 msg: 1613172837.43 epoch sec; seqNo:4837 2021-02-12T23:33:57Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.27 psi 2021-02-12T23:33:57Z- handleDepth()-Depth_Keller msg: 1613172837432 epoch sec; seqNo:4885; depth 5.81 2021-02-12T23:33:57Z- handleDvl() - RDI_Pathfinder msg: 1613172837.45 epoch sec; seqNo:2373 2021-02-12T23:33:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.44 , 117.18 , 0.00 , 126.11 2021-02-12T23:33:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.10 , -0.05 2021-02-12T23:33:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172837.46 epoch sec; seqNo:4885; 4057293.10 north; 591161.33 east 2021-02-12T23:33:57Z- AHRS_M2 msg: 1613172837.83 epoch sec; seqNo:4838 2021-02-12T23:33:57Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.26 psi 2021-02-12T23:33:57Z- handleDepth()-Depth_Keller msg: 1613172837836 epoch sec; seqNo:4886; depth 5.93 2021-02-12T23:33:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172837.85 epoch sec; seqNo:4886; 4057293.47 north; 591161.33 east 2021-02-12T23:33:58Z- AHRS_M2 msg: 1613172838.23 epoch sec; seqNo:4839 2021-02-12T23:33:58Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.25 psi 2021-02-12T23:33:58Z- handleDepth()-Depth_Keller msg: 1613172838249 epoch sec; seqNo:4887; depth 6.02 2021-02-12T23:33:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172838.26 epoch sec; seqNo:4887; 4057293.47 north; 591161.33 east 2021-02-12T23:33:58Z- AHRS_M2 msg: 1613172838.64 epoch sec; seqNo:4840 2021-02-12T23:33:58Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.23 psi 2021-02-12T23:33:58Z- handleDepth()-Depth_Keller msg: 1613172838649 epoch sec; seqNo:4888; depth 6.16 2021-02-12T23:33:58Z- handleDvl() - RDI_Pathfinder msg: 1613172838.67 epoch sec; seqNo:2374 2021-02-12T23:33:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.32 , 116.57 , 83.86 , 124.01 2021-02-12T23:33:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , 0.06 , 0.07 2021-02-12T23:33:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172838.68 epoch sec; seqNo:4888; 4057293.85 north; 591161.32 east 2021-02-12T23:33:59Z- AHRS_M2 msg: 1613172839.04 epoch sec; seqNo:4841 2021-02-12T23:33:59Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.21 psi 2021-02-12T23:33:59Z- handleDepth()-Depth_Keller msg: 1613172839048 epoch sec; seqNo:4889; depth 6.27 2021-02-12T23:33:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172839.06 epoch sec; seqNo:4889; 4057293.85 north; 591161.32 east 2021-02-12T23:33:59Z- AHRS_M2 msg: 1613172839.45 epoch sec; seqNo:4842 2021-02-12T23:33:59Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.20 psi 2021-02-12T23:33:59Z- handleDepth()-Depth_Keller msg: 1613172839457 epoch sec; seqNo:4890; depth 6.38 2021-02-12T23:33:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172839.47 epoch sec; seqNo:4890; 4057294.23 north; 591161.32 east 2021-02-12T23:33:59Z- AHRS_M2 msg: 1613172839.85 epoch sec; seqNo:4843 2021-02-12T23:33:59Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.20 psi 2021-02-12T23:33:59Z- handleDepth()-Depth_Keller msg: 1613172839858 epoch sec; seqNo:4891; depth 6.52 2021-02-12T23:33:59Z- handleDvl() - RDI_Pathfinder msg: 1613172839.87 epoch sec; seqNo:2375 2021-02-12T23:33:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 112.72 , 82.98 , 126.11 2021-02-12T23:33:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.21 , 0.16 2021-02-12T23:33:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172839.88 epoch sec; seqNo:4891; 4057294.24 north; 591162.54 east 2021-02-12T23:34:00Z- AHRS_M2 msg: 1613172840.26 epoch sec; seqNo:4844 2021-02-12T23:34:00Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.21 psi 2021-02-12T23:34:00Z- handleDepth()-Depth_Keller msg: 1613172840263 epoch sec; seqNo:4892; depth 6.63 2021-02-12T23:34:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172840.29 epoch sec; seqNo:4892; 4057294.62 north; 591162.54 east 2021-02-12T23:34:00Z- AHRS_M2 msg: 1613172840.66 epoch sec; seqNo:4845 2021-02-12T23:34:00Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.22 psi 2021-02-12T23:34:00Z- handleDepth()-Depth_Keller msg: 1613172840664 epoch sec; seqNo:4893; depth 6.75 2021-02-12T23:34:00Z- handleDvl() - RDI_Pathfinder msg: 1613172840.68 epoch sec; seqNo:2376 2021-02-12T23:34:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.28 , 111.10 , 82.84 , 130.43 2021-02-12T23:34:00Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.30 , 0.01 2021-02-12T23:34:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172840.70 epoch sec; seqNo:4893; 4057295.00 north; 591162.53 east 2021-02-12T23:34:01Z- AHRS_M2 msg: 1613172841.06 epoch sec; seqNo:4846 2021-02-12T23:34:01Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.25 psi 2021-02-12T23:34:01Z- handleDepth()-Depth_Keller msg: 1613172841068 epoch sec; seqNo:4894; depth 6.89 2021-02-12T23:34:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172841.08 epoch sec; seqNo:4894; 4057295.00 north; 591162.53 east 2021-02-12T23:34:01Z- LcmTrn::handleMessages() - handling 41 messages took 0.61 ms and 129 calls... 2021-02-12T23:34:01Z- waiting for 1613172839.23, time is 1613172841.08 2021-02-12T23:34:01Z- LcmTrn::updateTrn() >>>> pose time 1613172841.08, meas time 1613172840.68 <<<< 2021-02-12T23:34:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172840.68 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:34:01Z- TerrainNav::motionUpdate() - currEstimate = (4057295, 591162.53, 6.89) 2021-02-12T23:34:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:34:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:34:01Z- TNavBF::Projecting Measurements 2021-02-12T23:34:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:34:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:34:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:34:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:34:01Z- TNavFilter::4 good beams. 2021-02-12T23:34:01Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:34:01Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:34:01Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:34:01Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:34:01Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:34:01Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:34:01Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:34:01Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:01Z- TerrainMapOctree: (vehN, vehE ) = (4057294.81, 591162.33). 2021-02-12T23:34:01Z- TerrainMapOctree: Min Distance = 5017.69. 2021-02-12T23:34:01Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:34:01Z- There is no specific good beam that all particles have in common. 2021-02-12T23:34:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:34:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 13.04 2021-02-12T23:34:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:34:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 19.75 2021-02-12T23:34:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:34:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 10.98 2021-02-12T23:34:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:34:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.030 ExpectedMeasDiff: nan currMeas.covariance: 27.22 2021-02-12T23:34:01Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:34:01Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:34:01Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:34:01Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:34:01Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:34:01Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:34:01Z- Filter 0 No beams used in subcloud update 2021-02-12T23:34:01Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:34:01Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:34:01Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 754.2 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:34:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172840.68 sec. 2021-02-12T23:34:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:34:01Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:01Z- variances([0], [2], and psi_berg) = 754.154890 833.037004 0.000000 2021-02-12T23:34:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:34:01Z- LcmTrn::publishEstimates() - MLE : 4057280.50 591124.03 6.89 2021-02-12T23:34:01Z- LcmTrn::publishEstimates() - MMSE : 4057295.00 591162.53 6.89 2021-02-12T23:34:01Z- LcmTrn::publishEstimates() - COVAR: 754.15 833.04 0.88 0.00 2021-02-12T23:34:01Z- AHRS_M2 msg: 1613172841.47 epoch sec; seqNo:4847 2021-02-12T23:34:01Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.26 psi 2021-02-12T23:34:01Z- handleDepth()-Depth_Keller msg: 1613172841478 epoch sec; seqNo:4895; depth 7.00 2021-02-12T23:34:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172841.50 epoch sec; seqNo:4895; 4057295.39 north; 591163.75 east 2021-02-12T23:34:01Z- AHRS_M2 msg: 1613172841.87 epoch sec; seqNo:4848 2021-02-12T23:34:01Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.27 psi 2021-02-12T23:34:01Z- handleDepth()-Depth_Keller msg: 1613172841876 epoch sec; seqNo:4896; depth 7.15 2021-02-12T23:34:01Z- handleDvl() - RDI_Pathfinder msg: 1613172841.90 epoch sec; seqNo:2377 2021-02-12T23:34:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.37 , 112.65 , 82.91 , 127.52 2021-02-12T23:34:01Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , 0.12 , -0.21 2021-02-12T23:34:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172841.91 epoch sec; seqNo:4896; 4057295.77 north; 591163.74 east 2021-02-12T23:34:02Z- AHRS_M2 msg: 1613172842.28 epoch sec; seqNo:4849 2021-02-12T23:34:02Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.27 psi 2021-02-12T23:34:02Z- handleDepth()-Depth_Keller msg: 1613172842284 epoch sec; seqNo:4897; depth 7.28 2021-02-12T23:34:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172842.30 epoch sec; seqNo:4897; 4057296.15 north; 591163.74 east 2021-02-12T23:34:02Z- AHRS_M2 msg: 1613172842.68 epoch sec; seqNo:4850 2021-02-12T23:34:02Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.26 psi 2021-02-12T23:34:02Z- handleDepth()-Depth_Keller msg: 1613172842687 epoch sec; seqNo:4898; depth 7.41 2021-02-12T23:34:02Z- handleDvl() - RDI_Pathfinder msg: 1613172842.70 epoch sec; seqNo:2378 2021-02-12T23:34:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.02 , 115.76 , 83.04 , 123.20 2021-02-12T23:34:02Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , -0.12 , -0.25 2021-02-12T23:34:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172842.71 epoch sec; seqNo:4898; 4057296.53 north; 591163.74 east 2021-02-12T23:34:03Z- AHRS_M2 msg: 1613172843.08 epoch sec; seqNo:4851 2021-02-12T23:34:03Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.24 psi 2021-02-12T23:34:03Z- handleDepth()-Depth_Keller msg: 1613172843097 epoch sec; seqNo:4899; depth 7.54 2021-02-12T23:34:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172843.11 epoch sec; seqNo:4899; 4057296.91 north; 591163.73 east 2021-02-12T23:34:03Z- AHRS_M2 msg: 1613172843.50 epoch sec; seqNo:4852 2021-02-12T23:34:03Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.22 psi 2021-02-12T23:34:03Z- handleDepth()-Depth_Keller msg: 1613172843525 epoch sec; seqNo:4900; depth 7.68 2021-02-12T23:34:03Z- handleDvl() - RDI_Pathfinder msg: 1613172843.54 epoch sec; seqNo:2379 2021-02-12T23:34:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.02 , 112.79 , 81.56 , 117.25 2021-02-12T23:34:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.22 , -0.17 2021-02-12T23:34:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172843.55 epoch sec; seqNo:4900; 4057297.29 north; 591163.73 east 2021-02-12T23:34:03Z- AHRS_M2 msg: 1613172843.89 epoch sec; seqNo:4853 2021-02-12T23:34:03Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.20 psi 2021-02-12T23:34:03Z- handleDepth()-Depth_Keller msg: 1613172843896 epoch sec; seqNo:4901; depth 7.79 2021-02-12T23:34:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172843.91 epoch sec; seqNo:4901; 4057297.68 north; 591164.94 east 2021-02-12T23:34:04Z- AHRS_M2 msg: 1613172844.31 epoch sec; seqNo:4854 2021-02-12T23:34:04Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.18 psi 2021-02-12T23:34:04Z- handleDepth()-Depth_Keller msg: 1613172844317 epoch sec; seqNo:4902; depth 7.91 2021-02-12T23:34:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172844.33 epoch sec; seqNo:4902; 4057297.68 north; 591164.94 east 2021-02-12T23:34:04Z- AHRS_M2 msg: 1613172844.70 epoch sec; seqNo:4855 2021-02-12T23:34:04Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.18 psi 2021-02-12T23:34:04Z- handleDepth()-Depth_Keller msg: 1613172844709 epoch sec; seqNo:4903; depth 8.04 2021-02-12T23:34:04Z- handleDvl() - RDI_Pathfinder msg: 1613172844.73 epoch sec; seqNo:2380 2021-02-12T23:34:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.29 , 106.57 , 79.28 , 114.68 2021-02-12T23:34:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.18 , -0.04 2021-02-12T23:34:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172844.74 epoch sec; seqNo:4903; 4057298.06 north; 591164.94 east 2021-02-12T23:34:05Z- AHRS_M2 msg: 1613172845.11 epoch sec; seqNo:4856 2021-02-12T23:34:05Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.18 psi 2021-02-12T23:34:05Z- handleDepth()-Depth_Keller msg: 1613172845113 epoch sec; seqNo:4904; depth 8.16 2021-02-12T23:34:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172845.13 epoch sec; seqNo:4904; 4057298.44 north; 591164.94 east 2021-02-12T23:34:05Z- AHRS_M2 msg: 1613172845.51 epoch sec; seqNo:4857 2021-02-12T23:34:05Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-12T23:34:05Z- handleDepth()-Depth_Keller msg: 1613172845518 epoch sec; seqNo:4905; depth 8.26 2021-02-12T23:34:05Z- handleDvl() - RDI_Pathfinder msg: 1613172845.53 epoch sec; seqNo:2381 2021-02-12T23:34:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.84 , 104.07 , 78.87 , 114.88 2021-02-12T23:34:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.07 , 0.02 2021-02-12T23:34:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172845.54 epoch sec; seqNo:4905; 4057298.82 north; 591164.93 east 2021-02-12T23:34:05Z- AHRS_M2 msg: 1613172845.91 epoch sec; seqNo:4858 2021-02-12T23:34:05Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.19 psi 2021-02-12T23:34:05Z- handleDepth()-Depth_Keller msg: 1613172845916 epoch sec; seqNo:4906; depth 8.37 2021-02-12T23:34:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172845.93 epoch sec; seqNo:4906; 4057298.82 north; 591164.93 east 2021-02-12T23:34:06Z- AHRS_M2 msg: 1613172846.32 epoch sec; seqNo:4859 2021-02-12T23:34:06Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.20 psi 2021-02-12T23:34:06Z- handleDepth()-Depth_Keller msg: 1613172846320 epoch sec; seqNo:4907; depth 8.49 2021-02-12T23:34:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172846.33 epoch sec; seqNo:4907; 4057299.20 north; 591164.93 east 2021-02-12T23:34:06Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-12T23:34:06Z- waiting for 1613172844.08, time is 1613172846.33 2021-02-12T23:34:06Z- LcmTrn::updateTrn() >>>> pose time 1613172846.33, meas time 1613172845.53 <<<< 2021-02-12T23:34:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172845.53 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:34:06Z- TerrainNav::motionUpdate() - currEstimate = (4057299, 591164.93, 8.49) 2021-02-12T23:34:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:34:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:34:06Z- TNavBF::Projecting Measurements 2021-02-12T23:34:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:34:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:34:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:34:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:34:06Z- TNavFilter::4 good beams. 2021-02-12T23:34:06Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:34:06Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:34:06Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:34:06Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:34:06Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:34:06Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:34:06Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:34:06Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:06Z- TerrainMapOctree: (vehN, vehE ) = (4057298.56, 591164.56). 2021-02-12T23:34:06Z- TerrainMapOctree: Min Distance = 5013.36. 2021-02-12T23:34:06Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:34:06Z- There is no specific good beam that all particles have in common. 2021-02-12T23:34:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:34:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 12.35 2021-02-12T23:34:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:34:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 17.33 2021-02-12T23:34:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:34:06Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 9.95 2021-02-12T23:34:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:34:06Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 21.12 2021-02-12T23:34:06Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:34:06Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:34:06Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:34:06Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:34:06Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:34:06Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:34:06Z- Filter 0 No beams used in subcloud update 2021-02-12T23:34:06Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:34:06Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:34:06Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 754.1 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:34:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172845.53 sec. 2021-02-12T23:34:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:34:06Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:06Z- variances([0], [2], and psi_berg) = 754.006693 833.036859 0.000000 2021-02-12T23:34:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:34:06Z- LcmTrn::publishEstimates() - MLE : 4057284.70 591126.43 8.49 2021-02-12T23:34:06Z- LcmTrn::publishEstimates() - MMSE : 4057299.20 591164.93 8.49 2021-02-12T23:34:06Z- LcmTrn::publishEstimates() - COVAR: 754.01 833.04 0.89 0.00 2021-02-12T23:34:06Z- AHRS_M2 msg: 1613172846.72 epoch sec; seqNo:4860 2021-02-12T23:34:06Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.21 psi 2021-02-12T23:34:06Z- handleDepth()-Depth_Keller msg: 1613172846723 epoch sec; seqNo:4908; depth 8.59 2021-02-12T23:34:06Z- handleDvl() - RDI_Pathfinder msg: 1613172846.73 epoch sec; seqNo:2382 2021-02-12T23:34:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 104.07 , 78.05 , 116.24 2021-02-12T23:34:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.01 , -0.02 2021-02-12T23:34:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172846.74 epoch sec; seqNo:4908; 4057299.57 north; 591164.92 east 2021-02-12T23:34:07Z- AHRS_M2 msg: 1613172847.13 epoch sec; seqNo:4861 2021-02-12T23:34:07Z- AHRS_M2 msg: 0.58 phi; -0.29 theta; -0.21 psi 2021-02-12T23:34:07Z- handleDepth()-Depth_Keller msg: 1613172847134 epoch sec; seqNo:4909; depth 8.69 2021-02-12T23:34:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172847.15 epoch sec; seqNo:4909; 4057299.57 north; 591164.92 east 2021-02-12T23:34:07Z- AHRS_M2 msg: 1613172847.55 epoch sec; seqNo:4862 2021-02-12T23:34:07Z- AHRS_M2 msg: 0.58 phi; -0.29 theta; -0.21 psi 2021-02-12T23:34:07Z- handleDepth()-Depth_Keller msg: 1613172847555 epoch sec; seqNo:4910; depth 8.81 2021-02-12T23:34:07Z- handleDvl() - RDI_Pathfinder msg: 1613172847.57 epoch sec; seqNo:2383 2021-02-12T23:34:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.21 , 104.14 , 81.15 , 114.95 2021-02-12T23:34:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , -0.04 2021-02-12T23:34:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172847.58 epoch sec; seqNo:4910; 4057299.95 north; 591164.92 east 2021-02-12T23:34:07Z- AHRS_M2 msg: 1613172847.94 epoch sec; seqNo:4863 2021-02-12T23:34:07Z- AHRS_M2 msg: 0.58 phi; -0.29 theta; -0.20 psi 2021-02-12T23:34:07Z- handleDepth()-Depth_Keller msg: 1613172847944 epoch sec; seqNo:4911; depth 8.92 2021-02-12T23:34:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172847.98 epoch sec; seqNo:4911; 4057300.35 north; 591166.14 east 2021-02-12T23:34:08Z- AHRS_M2 msg: 1613172848.34 epoch sec; seqNo:4864 2021-02-12T23:34:08Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.19 psi 2021-02-12T23:34:08Z- handleDepth()-Depth_Keller msg: 1613172848340 epoch sec; seqNo:4912; depth 9.04 2021-02-12T23:34:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172848.35 epoch sec; seqNo:4912; 4057300.35 north; 591166.14 east 2021-02-12T23:34:08Z- AHRS_M2 msg: 1613172848.74 epoch sec; seqNo:4865 2021-02-12T23:34:08Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.18 psi 2021-02-12T23:34:08Z- handleDepth()-Depth_Keller msg: 1613172848746 epoch sec; seqNo:4913; depth 9.13 2021-02-12T23:34:08Z- handleDvl() - RDI_Pathfinder msg: 1613172848.76 epoch sec; seqNo:2384 2021-02-12T23:34:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.21 , 104.14 , 78.46 , 112.25 2021-02-12T23:34:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.09 , -0.07 2021-02-12T23:34:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172848.77 epoch sec; seqNo:4913; 4057300.72 north; 591166.13 east 2021-02-12T23:34:09Z- AHRS_M2 msg: 1613172849.15 epoch sec; seqNo:4866 2021-02-12T23:34:09Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.19 psi 2021-02-12T23:34:09Z- handleDepth()-Depth_Keller msg: 1613172849151 epoch sec; seqNo:4914; depth 9.24 2021-02-12T23:34:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172849.17 epoch sec; seqNo:4914; 4057301.10 north; 591166.13 east 2021-02-12T23:34:09Z- AHRS_M2 msg: 1613172849.55 epoch sec; seqNo:4867 2021-02-12T23:34:09Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.20 psi 2021-02-12T23:34:09Z- handleDepth()-Depth_Keller msg: 1613172849552 epoch sec; seqNo:4915; depth 9.34 2021-02-12T23:34:09Z- handleDvl() - RDI_Pathfinder msg: 1613172849.56 epoch sec; seqNo:2385 2021-02-12T23:34:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 104.20 , 77.64 , 110.96 2021-02-12T23:34:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.20 , -0.08 2021-02-12T23:34:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172849.59 epoch sec; seqNo:4915; 4057301.48 north; 591166.13 east 2021-02-12T23:34:09Z- AHRS_M2 msg: 1613172849.95 epoch sec; seqNo:4868 2021-02-12T23:34:09Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.19 psi 2021-02-12T23:34:09Z- handleDepth()-Depth_Keller msg: 1613172849958 epoch sec; seqNo:4916; depth 9.44 2021-02-12T23:34:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172849.97 epoch sec; seqNo:4916; 4057301.48 north; 591166.13 east 2021-02-12T23:34:10Z- AHRS_M2 msg: 1613172850.36 epoch sec; seqNo:4869 2021-02-12T23:34:10Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.18 psi 2021-02-12T23:34:10Z- handleDepth()-Depth_Keller msg: 1613172850368 epoch sec; seqNo:4917; depth 9.54 2021-02-12T23:34:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172850.39 epoch sec; seqNo:4917; 4057301.86 north; 591166.12 east 2021-02-12T23:34:10Z- AHRS_M2 msg: 1613172850.76 epoch sec; seqNo:4870 2021-02-12T23:34:10Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.17 psi 2021-02-12T23:34:10Z- handleDepth()-Depth_Keller msg: 1613172850769 epoch sec; seqNo:4918; depth 9.63 2021-02-12T23:34:10Z- handleDvl() - RDI_Pathfinder msg: 1613172850.78 epoch sec; seqNo:2386 2021-02-12T23:34:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 102.91 , 79.93 , 113.73 2021-02-12T23:34:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.29 , 0.06 2021-02-12T23:34:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172850.80 epoch sec; seqNo:4918; 4057302.24 north; 591166.12 east 2021-02-12T23:34:11Z- AHRS_M2 msg: 1613172851.16 epoch sec; seqNo:4871 2021-02-12T23:34:11Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.17 psi 2021-02-12T23:34:11Z- handleDepth()-Depth_Keller msg: 1613172851168 epoch sec; seqNo:4919; depth 9.73 2021-02-12T23:34:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172851.18 epoch sec; seqNo:4919; 4057302.62 north; 591166.11 east 2021-02-12T23:34:11Z- AHRS_M2 msg: 1613172851.57 epoch sec; seqNo:4872 2021-02-12T23:34:11Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.16 psi 2021-02-12T23:34:11Z- LcmTrn::handleMessages() - handling 42 messages took 0.60 ms and 130 calls... 2021-02-12T23:34:11Z- waiting for 1613172849.33, time is 1613172851.57 2021-02-12T23:34:11Z- LcmTrn::updateTrn() >>>> pose time 1613172851.57, meas time 1613172850.78 <<<< 2021-02-12T23:34:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172850.78 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:34:11Z- TerrainNav::motionUpdate() - currEstimate = (4057303, 591166.11, 9.73) 2021-02-12T23:34:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:34:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:34:11Z- TNavBF::Projecting Measurements 2021-02-12T23:34:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:34:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:34:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:34:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:34:11Z- TNavFilter::4 good beams. 2021-02-12T23:34:11Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:34:11Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:34:11Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:34:11Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:34:11Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:34:11Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:34:11Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:34:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:11Z- TerrainMapOctree: (vehN, vehE ) = (4057302.10, 591165.94). 2021-02-12T23:34:11Z- TerrainMapOctree: Min Distance = 5009.72. 2021-02-12T23:34:11Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:34:11Z- There is no specific good beam that all particles have in common. 2021-02-12T23:34:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:34:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 11.65 2021-02-12T23:34:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:34:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 16.95 2021-02-12T23:34:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:34:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 10.22 2021-02-12T23:34:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:34:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 20.69 2021-02-12T23:34:11Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:34:11Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:34:11Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:34:11Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:34:11Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:34:11Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:34:11Z- Filter 0 No beams used in subcloud update 2021-02-12T23:34:11Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:34:11Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:34:11Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 753.9 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:34:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172850.78 sec. 2021-02-12T23:34:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:34:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:11Z- variances([0], [2], and psi_berg) = 753.878764 833.036733 0.000000 2021-02-12T23:34:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:34:11Z- LcmTrn::publishEstimates() - MLE : 4057288.12 591127.61 9.73 2021-02-12T23:34:11Z- LcmTrn::publishEstimates() - MMSE : 4057302.62 591166.11 9.73 2021-02-12T23:34:11Z- LcmTrn::publishEstimates() - COVAR: 753.88 833.04 0.85 0.00 2021-02-12T23:34:11Z- handleDepth()-Depth_Keller msg: 1613172851582 epoch sec; seqNo:4920; depth 9.84 2021-02-12T23:34:11Z- handleDvl() - RDI_Pathfinder msg: 1613172851.59 epoch sec; seqNo:2387 2021-02-12T23:34:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.27 , 102.85 , 78.45 , 112.31 2021-02-12T23:34:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.17 , 0.29 2021-02-12T23:34:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172851.60 epoch sec; seqNo:4920; 4057302.62 north; 591166.11 east 2021-02-12T23:34:11Z- AHRS_M2 msg: 1613172851.97 epoch sec; seqNo:4873 2021-02-12T23:34:11Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.15 psi 2021-02-12T23:34:11Z- handleDepth()-Depth_Keller msg: 1613172851976 epoch sec; seqNo:4921; depth 9.94 2021-02-12T23:34:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172851.99 epoch sec; seqNo:4921; 4057303.00 north; 591166.11 east 2021-02-12T23:34:12Z- AHRS_M2 msg: 1613172852.39 epoch sec; seqNo:4874 2021-02-12T23:34:12Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.14 psi 2021-02-12T23:34:12Z- handleDepth()-Depth_Keller msg: 1613172852401 epoch sec; seqNo:4922; depth 10.05 2021-02-12T23:34:12Z- handleDvl() - RDI_Pathfinder msg: 1613172852.43 epoch sec; seqNo:2388 2021-02-12T23:34:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 100.21 , 77.23 , 113.73 2021-02-12T23:34:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.10 , 0.36 2021-02-12T23:34:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172852.44 epoch sec; seqNo:4922; 4057303.39 north; 591167.33 east 2021-02-12T23:34:12Z- AHRS_M2 msg: 1613172852.78 epoch sec; seqNo:4875 2021-02-12T23:34:12Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.14 psi 2021-02-12T23:34:12Z- handleDepth()-Depth_Keller msg: 1613172852786 epoch sec; seqNo:4923; depth 10.16 2021-02-12T23:34:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172852.80 epoch sec; seqNo:4923; 4057303.39 north; 591167.33 east 2021-02-12T23:34:13Z- AHRS_M2 msg: 1613172853.18 epoch sec; seqNo:4876 2021-02-12T23:34:13Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.15 psi 2021-02-12T23:34:13Z- handleDepth()-Depth_Keller msg: 1613172853188 epoch sec; seqNo:4924; depth 10.26 2021-02-12T23:34:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172853.20 epoch sec; seqNo:4924; 4057303.77 north; 591167.32 east 2021-02-12T23:34:13Z- AHRS_M2 msg: 1613172853.59 epoch sec; seqNo:4877 2021-02-12T23:34:13Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.16 psi 2021-02-12T23:34:13Z- handleDepth()-Depth_Keller msg: 1613172853592 epoch sec; seqNo:4925; depth 10.37 2021-02-12T23:34:13Z- handleDvl() - RDI_Pathfinder msg: 1613172853.60 epoch sec; seqNo:2389 2021-02-12T23:34:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.15 , 76.41 , 115.08 2021-02-12T23:34:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.29 , 0.13 2021-02-12T23:34:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172853.61 epoch sec; seqNo:4925; 4057303.77 north; 591167.32 east 2021-02-12T23:34:14Z- AHRS_M2 msg: 1613172854.01 epoch sec; seqNo:4878 2021-02-12T23:34:14Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.16 psi 2021-02-12T23:34:14Z- handleDepth()-Depth_Keller msg: 1613172854017 epoch sec; seqNo:4926; depth 10.46 2021-02-12T23:34:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172854.03 epoch sec; seqNo:4926; 4057304.15 north; 591167.32 east 2021-02-12T23:34:14Z- AHRS_M2 msg: 1613172854.40 epoch sec; seqNo:4879 2021-02-12T23:34:14Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-12T23:34:14Z- handleDepth()-Depth_Keller msg: 1613172854407 epoch sec; seqNo:4927; depth 10.57 2021-02-12T23:34:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172854.42 epoch sec; seqNo:4927; 4057304.16 north; 591168.54 east 2021-02-12T23:34:14Z- AHRS_M2 msg: 1613172854.80 epoch sec; seqNo:4880 2021-02-12T23:34:14Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-12T23:34:14Z- handleDepth()-Depth_Keller msg: 1613172854809 epoch sec; seqNo:4928; depth 10.68 2021-02-12T23:34:14Z- handleDvl() - RDI_Pathfinder msg: 1613172854.82 epoch sec; seqNo:2390 2021-02-12T23:34:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.21 , 101.43 , 78.45 , 117.65 2021-02-12T23:34:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.15 , -0.14 2021-02-12T23:34:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172854.83 epoch sec; seqNo:4928; 4057304.54 north; 591168.54 east 2021-02-12T23:34:15Z- AHRS_M2 msg: 1613172855.20 epoch sec; seqNo:4881 2021-02-12T23:34:15Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-12T23:34:15Z- handleDepth()-Depth_Keller msg: 1613172855217 epoch sec; seqNo:4929; depth 10.81 2021-02-12T23:34:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172855.23 epoch sec; seqNo:4929; 4057304.92 north; 591168.53 east 2021-02-12T23:34:15Z- AHRS_M2 msg: 1613172855.61 epoch sec; seqNo:4882 2021-02-12T23:34:15Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-12T23:34:15Z- handleDepth()-Depth_Keller msg: 1613172855613 epoch sec; seqNo:4930; depth 10.92 2021-02-12T23:34:15Z- handleDvl() - RDI_Pathfinder msg: 1613172855.62 epoch sec; seqNo:2391 2021-02-12T23:34:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.78 , 78.44 , 113.59 2021-02-12T23:34:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.08 , -0.20 2021-02-12T23:34:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172855.63 epoch sec; seqNo:4930; 4057305.30 north; 591168.53 east 2021-02-12T23:34:16Z- AHRS_M2 msg: 1613172856.01 epoch sec; seqNo:4883 2021-02-12T23:34:16Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-12T23:34:16Z- handleDepth()-Depth_Keller msg: 1613172856018 epoch sec; seqNo:4931; depth 11.02 2021-02-12T23:34:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172856.03 epoch sec; seqNo:4931; 4057305.68 north; 591168.52 east 2021-02-12T23:34:16Z- AHRS_M2 msg: 1613172856.41 epoch sec; seqNo:4884 2021-02-12T23:34:16Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-12T23:34:16Z- handleDepth()-Depth_Keller msg: 1613172856437 epoch sec; seqNo:4932; depth 11.14 2021-02-12T23:34:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172856.45 epoch sec; seqNo:4932; 4057306.07 north; 591169.74 east 2021-02-12T23:34:16Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-12T23:34:16Z- waiting for 1613172854.57, time is 1613172856.45 2021-02-12T23:34:16Z- LcmTrn::updateTrn() >>>> pose time 1613172856.45, meas time 1613172855.62 <<<< 2021-02-12T23:34:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172855.62 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:34:16Z- TerrainNav::motionUpdate() - currEstimate = (4057306, 591169.74, 11.14) 2021-02-12T23:34:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:34:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:34:16Z- TNavBF::Projecting Measurements 2021-02-12T23:34:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:34:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:34:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:34:16Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:34:16Z- TNavFilter::3 good beams. 2021-02-12T23:34:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:34:16Z- TNavBF:: beamIndex[0] = 1 2021-02-12T23:34:16Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:34:16Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:34:16Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:34:16Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:34:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:34:16Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:16Z- TerrainMapOctree: (vehN, vehE ) = (4057305.49, 591169.13). 2021-02-12T23:34:16Z- TerrainMapOctree: Min Distance = 5005.09. 2021-02-12T23:34:16Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:34:16Z- There is no specific good beam that all particles have in common. 2021-02-12T23:34:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:34:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 16.90 2021-02-12T23:34:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:34:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-12T23:34:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:34:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 20.65 2021-02-12T23:34:16Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:34:16Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:34:16Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:34:16Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:34:16Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:34:16Z- Filter 0 No beams used in subcloud update 2021-02-12T23:34:16Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:34:16Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:34:16Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 753.8 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:34:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172855.62 sec. 2021-02-12T23:34:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:34:16Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:16Z- variances([0], [2], and psi_berg) = 753.727178 833.036585 0.000000 2021-02-12T23:34:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:34:16Z- LcmTrn::publishEstimates() - MLE : 4057291.57 591131.24 11.14 2021-02-12T23:34:16Z- LcmTrn::publishEstimates() - MMSE : 4057306.07 591169.74 11.14 2021-02-12T23:34:16Z- LcmTrn::publishEstimates() - COVAR: 753.73 833.04 0.93 0.00 2021-02-12T23:34:16Z- AHRS_M2 msg: 1613172856.82 epoch sec; seqNo:4885 2021-02-12T23:34:16Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-12T23:34:16Z- handleDepth()-Depth_Keller msg: 1613172856829 epoch sec; seqNo:4933; depth 11.26 2021-02-12T23:34:16Z- handleDvl() - RDI_Pathfinder msg: 1613172856.84 epoch sec; seqNo:2392 2021-02-12T23:34:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.65 , 102.57 , 78.24 , 113.39 2021-02-12T23:34:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.16 , -0.11 2021-02-12T23:34:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172856.86 epoch sec; seqNo:4933; 4057306.45 north; 591169.74 east 2021-02-12T23:34:17Z- AHRS_M2 msg: 1613172857.22 epoch sec; seqNo:4886 2021-02-12T23:34:17Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.17 psi 2021-02-12T23:34:17Z- handleDepth()-Depth_Keller msg: 1613172857233 epoch sec; seqNo:4934; depth 11.37 2021-02-12T23:34:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172857.24 epoch sec; seqNo:4934; 4057306.45 north; 591169.74 east 2021-02-12T23:34:17Z- AHRS_M2 msg: 1613172857.63 epoch sec; seqNo:4887 2021-02-12T23:34:17Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.16 psi 2021-02-12T23:34:17Z- handleDepth()-Depth_Keller msg: 1613172857637 epoch sec; seqNo:4935; depth 11.49 2021-02-12T23:34:17Z- handleDvl() - RDI_Pathfinder msg: 1613172857.65 epoch sec; seqNo:2393 2021-02-12T23:34:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 101.57 , 74.78 , 112.38 2021-02-12T23:34:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.13 , -0.03 2021-02-12T23:34:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172857.66 epoch sec; seqNo:4935; 4057306.82 north; 591169.73 east 2021-02-12T23:34:18Z- AHRS_M2 msg: 1613172858.03 epoch sec; seqNo:4888 2021-02-12T23:34:18Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.16 psi 2021-02-12T23:34:18Z- handleDepth()-Depth_Keller msg: 1613172858036 epoch sec; seqNo:4936; depth 11.62 2021-02-12T23:34:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172858.06 epoch sec; seqNo:4936; 4057307.20 north; 591169.73 east 2021-02-12T23:34:18Z- AHRS_M2 msg: 1613172858.43 epoch sec; seqNo:4889 2021-02-12T23:34:18Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.15 psi 2021-02-12T23:34:18Z- handleDepth()-Depth_Keller msg: 1613172858440 epoch sec; seqNo:4937; depth 11.73 2021-02-12T23:34:18Z- handleDvl() - RDI_Pathfinder msg: 1613172858.46 epoch sec; seqNo:2394 2021-02-12T23:34:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.13 , 96.29 , 76.83 , 111.17 2021-02-12T23:34:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.06 , -0.01 2021-02-12T23:34:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172858.48 epoch sec; seqNo:4937; 4057307.58 north; 591169.72 east 2021-02-12T23:34:18Z- AHRS_M2 msg: 1613172858.84 epoch sec; seqNo:4890 2021-02-12T23:34:18Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.15 psi 2021-02-12T23:34:18Z- handleDepth()-Depth_Keller msg: 1613172858849 epoch sec; seqNo:4938; depth 11.85 2021-02-12T23:34:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172858.86 epoch sec; seqNo:4938; 4057307.58 north; 591169.72 east 2021-02-12T23:34:19Z- AHRS_M2 msg: 1613172859.24 epoch sec; seqNo:4891 2021-02-12T23:34:19Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.16 psi 2021-02-12T23:34:19Z- handleDepth()-Depth_Keller msg: 1613172859254 epoch sec; seqNo:4939; depth 11.95 2021-02-12T23:34:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172859.26 epoch sec; seqNo:4939; 4057307.97 north; 591170.94 east 2021-02-12T23:34:19Z- AHRS_M2 msg: 1613172859.65 epoch sec; seqNo:4892 2021-02-12T23:34:19Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.17 psi 2021-02-12T23:34:19Z- handleDepth()-Depth_Keller msg: 1613172859657 epoch sec; seqNo:4940; depth 12.06 2021-02-12T23:34:19Z- handleDvl() - RDI_Pathfinder msg: 1613172859.67 epoch sec; seqNo:2395 2021-02-12T23:34:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.36 , 75.60 , 113.94 2021-02-12T23:34:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.06 , -0.01 2021-02-12T23:34:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172859.68 epoch sec; seqNo:4940; 4057308.35 north; 591170.94 east 2021-02-12T23:34:20Z- AHRS_M2 msg: 1613172860.05 epoch sec; seqNo:4893 2021-02-12T23:34:20Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.18 psi 2021-02-12T23:34:20Z- handleDepth()-Depth_Keller msg: 1613172860056 epoch sec; seqNo:4941; depth 12.19 2021-02-12T23:34:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172860.07 epoch sec; seqNo:4941; 4057308.35 north; 591170.94 east 2021-02-12T23:34:20Z- AHRS_M2 msg: 1613172860.46 epoch sec; seqNo:4894 2021-02-12T23:34:20Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.19 psi 2021-02-12T23:34:20Z- handleDepth()-Depth_Keller msg: 1613172860465 epoch sec; seqNo:4942; depth 12.30 2021-02-12T23:34:20Z- handleDvl() - RDI_Pathfinder msg: 1613172860.48 epoch sec; seqNo:2396 2021-02-12T23:34:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 101.50 , 75.19 , 112.32 2021-02-12T23:34:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , 0.00 2021-02-12T23:34:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172860.49 epoch sec; seqNo:4942; 4057308.73 north; 591170.93 east 2021-02-12T23:34:20Z- AHRS_M2 msg: 1613172860.86 epoch sec; seqNo:4895 2021-02-12T23:34:20Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-12T23:34:20Z- handleDepth()-Depth_Keller msg: 1613172860866 epoch sec; seqNo:4943; depth 12.42 2021-02-12T23:34:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172860.88 epoch sec; seqNo:4943; 4057309.11 north; 591170.93 east 2021-02-12T23:34:21Z- AHRS_M2 msg: 1613172861.26 epoch sec; seqNo:4896 2021-02-12T23:34:21Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.20 psi 2021-02-12T23:34:21Z- handleDepth()-Depth_Keller msg: 1613172861268 epoch sec; seqNo:4944; depth 12.53 2021-02-12T23:34:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172861.29 epoch sec; seqNo:4944; 4057309.49 north; 591170.92 east 2021-02-12T23:34:21Z- AHRS_M2 msg: 1613172861.67 epoch sec; seqNo:4897 2021-02-12T23:34:21Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.19 psi 2021-02-12T23:34:21Z- handleDepth()-Depth_Keller msg: 1613172861677 epoch sec; seqNo:4945; depth 12.65 2021-02-12T23:34:21Z- handleDvl() - RDI_Pathfinder msg: 1613172861.69 epoch sec; seqNo:2397 2021-02-12T23:34:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.42 , 103.05 , 74.78 , 112.52 2021-02-12T23:34:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.12 , 0.03 2021-02-12T23:34:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172861.70 epoch sec; seqNo:4945; 4057309.49 north; 591170.92 east 2021-02-12T23:34:21Z- LcmTrn::handleMessages() - handling 45 messages took 0.68 ms and 132 calls... 2021-02-12T23:34:21Z- waiting for 1613172859.45, time is 1613172861.70 2021-02-12T23:34:21Z- LcmTrn::updateTrn() equating pose 1613172861.70 and meas times 1613172861.70 2021-02-12T23:34:21Z- LcmTrn::updateTrn() >>>> pose time 1613172861.70, meas time 1613172861.70 <<<< 2021-02-12T23:34:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172861.70 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:34:21Z- TerrainNav::motionUpdate() - currEstimate = (4057309, 591170.92, 12.65) 2021-02-12T23:34:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:34:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:34:21Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172861.70 sec.; No relevant navigation data available 2021-02-12T23:34:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:34:21Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:21Z- variances([0], [2], and psi_berg) = 753.629803 833.036491 0.000000 2021-02-12T23:34:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:34:21Z- LcmTrn::publishEstimates() - MLE : 4057294.99 591132.42 12.65 2021-02-12T23:34:21Z- LcmTrn::publishEstimates() - MMSE : 4057309.49 591170.92 12.65 2021-02-12T23:34:21Z- LcmTrn::publishEstimates() - COVAR: 753.63 833.04 1.12 0.00 2021-02-12T23:34:22Z- AHRS_M2 msg: 1613172862.08 epoch sec; seqNo:4898 2021-02-12T23:34:22Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.19 psi 2021-02-12T23:34:22Z- handleDepth()-Depth_Keller msg: 1613172862083 epoch sec; seqNo:4946; depth 12.78 2021-02-12T23:34:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172862.10 epoch sec; seqNo:4946; 4057309.87 north; 591170.92 east 2021-02-12T23:34:22Z- AHRS_M2 msg: 1613172862.48 epoch sec; seqNo:4899 2021-02-12T23:34:22Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-12T23:34:22Z- handleDepth()-Depth_Keller msg: 1613172862482 epoch sec; seqNo:4947; depth 12.89 2021-02-12T23:34:22Z- handleDvl() - RDI_Pathfinder msg: 1613172862.50 epoch sec; seqNo:2398 2021-02-12T23:34:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.42 , 96.29 , 75.60 , 112.52 2021-02-12T23:34:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , 0.01 2021-02-12T23:34:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172862.51 epoch sec; seqNo:4947; 4057310.26 north; 591172.14 east 2021-02-12T23:34:22Z- AHRS_M2 msg: 1613172862.88 epoch sec; seqNo:4900 2021-02-12T23:34:22Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.18 psi 2021-02-12T23:34:22Z- handleDepth()-Depth_Keller msg: 1613172862889 epoch sec; seqNo:4948; depth 13.02 2021-02-12T23:34:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172862.90 epoch sec; seqNo:4948; 4057310.64 north; 591172.13 east 2021-02-12T23:34:23Z- AHRS_M2 msg: 1613172863.29 epoch sec; seqNo:4901 2021-02-12T23:34:23Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-12T23:34:23Z- handleDepth()-Depth_Keller msg: 1613172863290 epoch sec; seqNo:4949; depth 13.13 2021-02-12T23:34:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172863.30 epoch sec; seqNo:4949; 4057310.64 north; 591172.13 east 2021-02-12T23:34:23Z- AHRS_M2 msg: 1613172863.69 epoch sec; seqNo:4902 2021-02-12T23:34:23Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-12T23:34:23Z- handleDepth()-Depth_Keller msg: 1613172863697 epoch sec; seqNo:4950; depth 13.26 2021-02-12T23:34:23Z- handleDvl() - RDI_Pathfinder msg: 1613172863.71 epoch sec; seqNo:2399 2021-02-12T23:34:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 96.43 , 78.18 , 111.03 2021-02-12T23:34:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.07 , -0.02 2021-02-12T23:34:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172863.72 epoch sec; seqNo:4950; 4057311.02 north; 591172.13 east 2021-02-12T23:34:24Z- AHRS_M2 msg: 1613172864.10 epoch sec; seqNo:4903 2021-02-12T23:34:24Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-12T23:34:24Z- handleDepth()-Depth_Keller msg: 1613172864110 epoch sec; seqNo:4951; depth 13.39 2021-02-12T23:34:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172864.14 epoch sec; seqNo:4951; 4057311.40 north; 591172.13 east 2021-02-12T23:34:24Z- AHRS_M2 msg: 1613172864.50 epoch sec; seqNo:4904 2021-02-12T23:34:24Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.18 psi 2021-02-12T23:34:24Z- handleDepth()-Depth_Keller msg: 1613172864506 epoch sec; seqNo:4952; depth 13.52 2021-02-12T23:34:24Z- handleDvl() - RDI_Pathfinder msg: 1613172864.52 epoch sec; seqNo:2400 2021-02-12T23:34:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 96.36 , 78.79 , 112.58 2021-02-12T23:34:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.07 , -0.03 2021-02-12T23:34:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172864.53 epoch sec; seqNo:4952; 4057311.40 north; 591172.13 east 2021-02-12T23:34:24Z- AHRS_M2 msg: 1613172864.90 epoch sec; seqNo:4905 2021-02-12T23:34:24Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.17 psi 2021-02-12T23:34:24Z- handleDepth()-Depth_Keller msg: 1613172864904 epoch sec; seqNo:4953; depth 13.63 2021-02-12T23:34:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172864.92 epoch sec; seqNo:4953; 4057311.78 north; 591172.12 east 2021-02-12T23:34:25Z- AHRS_M2 msg: 1613172865.30 epoch sec; seqNo:4906 2021-02-12T23:34:25Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.16 psi 2021-02-12T23:34:25Z- handleDepth()-Depth_Keller msg: 1613172865308 epoch sec; seqNo:4954; depth 13.76 2021-02-12T23:34:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172865.32 epoch sec; seqNo:4954; 4057312.15 north; 591172.12 east 2021-02-12T23:34:25Z- AHRS_M2 msg: 1613172865.71 epoch sec; seqNo:4907 2021-02-12T23:34:25Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.16 psi 2021-02-12T23:34:25Z- handleDepth()-Depth_Keller msg: 1613172865712 epoch sec; seqNo:4955; depth 13.87 2021-02-12T23:34:25Z- handleDvl() - RDI_Pathfinder msg: 1613172865.72 epoch sec; seqNo:2401 2021-02-12T23:34:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 99.00 , 73.96 , 111.17 2021-02-12T23:34:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.11 , -0.01 2021-02-12T23:34:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172865.73 epoch sec; seqNo:4955; 4057312.55 north; 591173.33 east 2021-02-12T23:34:26Z- AHRS_M2 msg: 1613172866.12 epoch sec; seqNo:4908 2021-02-12T23:34:26Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.16 psi 2021-02-12T23:34:26Z- handleDepth()-Depth_Keller msg: 1613172866121 epoch sec; seqNo:4956; depth 14.01 2021-02-12T23:34:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172866.13 epoch sec; seqNo:4956; 4057312.55 north; 591173.33 east 2021-02-12T23:34:26Z- AHRS_M2 msg: 1613172866.52 epoch sec; seqNo:4909 2021-02-12T23:34:26Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.16 psi 2021-02-12T23:34:26Z- handleDepth()-Depth_Keller msg: 1613172866561 epoch sec; seqNo:4957; depth 14.12 2021-02-12T23:34:26Z- handleDvl() - RDI_Pathfinder msg: 1613172866.57 epoch sec; seqNo:2402 2021-02-12T23:34:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 96.36 , 78.79 , 111.23 2021-02-12T23:34:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.12 , 0.05 2021-02-12T23:34:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172866.61 epoch sec; seqNo:4957; 4057312.93 north; 591173.33 east 2021-02-12T23:34:26Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-12T23:34:26Z- waiting for 1613172864.70, time is 1613172866.61 2021-02-12T23:34:26Z- LcmTrn::updateTrn() equating pose 1613172866.61 and meas times 1613172866.61 2021-02-12T23:34:26Z- LcmTrn::updateTrn() >>>> pose time 1613172866.61, meas time 1613172866.61 <<<< 2021-02-12T23:34:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172866.61 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:34:26Z- TerrainNav::motionUpdate() - currEstimate = (4057313, 591173.33, 14.12) 2021-02-12T23:34:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:34:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:34:26Z- TNavBF::Projecting Measurements 2021-02-12T23:34:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:34:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:34:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:34:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:34:26Z- TNavFilter::4 good beams. 2021-02-12T23:34:26Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:34:26Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:34:26Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:34:26Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:34:26Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:34:26Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:34:26Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:34:26Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:26Z- TerrainMapOctree: (vehN, vehE ) = (4057312.93, 591173.33). 2021-02-12T23:34:26Z- TerrainMapOctree: Min Distance = 4996.64. 2021-02-12T23:34:26Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:34:26Z- There is no specific good beam that all particles have in common. 2021-02-12T23:34:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:34:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 10.98 2021-02-12T23:34:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:34:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 14.86 2021-02-12T23:34:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:34:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 9.93 2021-02-12T23:34:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:34:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 19.80 2021-02-12T23:34:26Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:34:26Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:34:26Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:34:26Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:34:26Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:34:26Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:34:26Z- Filter 0 No beams used in subcloud update 2021-02-12T23:34:26Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:34:26Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:34:26Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 753.5 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:34:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172866.61 sec. 2021-02-12T23:34:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:34:26Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:26Z- variances([0], [2], and psi_berg) = 753.525155 833.036389 0.000000 2021-02-12T23:34:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:34:26Z- LcmTrn::publishEstimates() - MLE : 4057298.43 591134.83 14.12 2021-02-12T23:34:26Z- LcmTrn::publishEstimates() - MMSE : 4057312.93 591173.33 14.12 2021-02-12T23:34:26Z- LcmTrn::publishEstimates() - COVAR: 753.53 833.04 0.98 0.00 2021-02-12T23:34:26Z- AHRS_M2 msg: 1613172866.92 epoch sec; seqNo:4910 2021-02-12T23:34:26Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-12T23:34:26Z- handleDepth()-Depth_Keller msg: 1613172866924 epoch sec; seqNo:4958; depth 14.26 2021-02-12T23:34:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172866.94 epoch sec; seqNo:4958; 4057313.30 north; 591173.33 east 2021-02-12T23:34:27Z- AHRS_M2 msg: 1613172867.32 epoch sec; seqNo:4911 2021-02-12T23:34:27Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.18 psi 2021-02-12T23:34:27Z- handleDepth()-Depth_Keller msg: 1613172867328 epoch sec; seqNo:4959; depth 14.36 2021-02-12T23:34:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172867.34 epoch sec; seqNo:4959; 4057313.68 north; 591173.32 east 2021-02-12T23:34:27Z- AHRS_M2 msg: 1613172867.73 epoch sec; seqNo:4912 2021-02-12T23:34:27Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-12T23:34:27Z- handleDepth()-Depth_Keller msg: 1613172867741 epoch sec; seqNo:4960; depth 14.49 2021-02-12T23:34:27Z- handleDvl() - RDI_Pathfinder msg: 1613172867.75 epoch sec; seqNo:2403 2021-02-12T23:34:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.49 , 99.06 , 78.78 , 111.23 2021-02-12T23:34:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.09 , 0.08 2021-02-12T23:34:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172867.76 epoch sec; seqNo:4960; 4057314.06 north; 591173.32 east 2021-02-12T23:34:28Z- AHRS_M2 msg: 1613172868.13 epoch sec; seqNo:4913 2021-02-12T23:34:28Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-12T23:34:28Z- handleDepth()-Depth_Keller msg: 1613172868140 epoch sec; seqNo:4961; depth 14.61 2021-02-12T23:34:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172868.16 epoch sec; seqNo:4961; 4057314.06 north; 591173.32 east 2021-02-12T23:34:28Z- AHRS_M2 msg: 1613172868.54 epoch sec; seqNo:4914 2021-02-12T23:34:28Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-12T23:34:28Z- handleDepth()-Depth_Keller msg: 1613172868545 epoch sec; seqNo:4962; depth 14.73 2021-02-12T23:34:28Z- handleDvl() - RDI_Pathfinder msg: 1613172868.56 epoch sec; seqNo:2404 2021-02-12T23:34:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.68 , 96.29 , 73.96 , 109.81 2021-02-12T23:34:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , 0.05 2021-02-12T23:34:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172868.58 epoch sec; seqNo:4962; 4057314.45 north; 591174.54 east 2021-02-12T23:34:28Z- AHRS_M2 msg: 1613172868.94 epoch sec; seqNo:4915 2021-02-12T23:34:28Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-12T23:34:28Z- handleDepth()-Depth_Keller msg: 1613172868953 epoch sec; seqNo:4963; depth 14.86 2021-02-12T23:34:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172868.96 epoch sec; seqNo:4963; 4057314.83 north; 591174.53 east 2021-02-12T23:34:29Z- AHRS_M2 msg: 1613172869.34 epoch sec; seqNo:4916 2021-02-12T23:34:29Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-12T23:34:29Z- handleDepth()-Depth_Keller msg: 1613172869348 epoch sec; seqNo:4964; depth 14.98 2021-02-12T23:34:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172869.36 epoch sec; seqNo:4964; 4057315.21 north; 591174.53 east 2021-02-12T23:34:29Z- AHRS_M2 msg: 1613172869.75 epoch sec; seqNo:4917 2021-02-12T23:34:29Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-12T23:34:29Z- handleDepth()-Depth_Keller msg: 1613172869757 epoch sec; seqNo:4965; depth 15.09 2021-02-12T23:34:29Z- handleDvl() - RDI_Pathfinder msg: 1613172869.78 epoch sec; seqNo:2405 2021-02-12T23:34:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.27 , 96.56 , 78.31 , 109.94 2021-02-12T23:34:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.01 2021-02-12T23:34:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172869.79 epoch sec; seqNo:4965; 4057315.21 north; 591174.53 east 2021-02-12T23:34:30Z- AHRS_M2 msg: 1613172870.15 epoch sec; seqNo:4918 2021-02-12T23:34:30Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-12T23:34:30Z- handleDepth()-Depth_Keller msg: 1613172870157 epoch sec; seqNo:4966; depth 15.21 2021-02-12T23:34:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172870.17 epoch sec; seqNo:4966; 4057315.59 north; 591174.52 east 2021-02-12T23:34:30Z- AHRS_M2 msg: 1613172870.56 epoch sec; seqNo:4919 2021-02-12T23:34:30Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.17 psi 2021-02-12T23:34:30Z- handleDepth()-Depth_Keller msg: 1613172870563 epoch sec; seqNo:4967; depth 15.35 2021-02-12T23:34:30Z- handleDvl() - RDI_Pathfinder msg: 1613172870.57 epoch sec; seqNo:2406 2021-02-12T23:34:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.68 , 96.42 , 72.32 , 108.59 2021-02-12T23:34:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.01 , -0.01 2021-02-12T23:34:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172870.59 epoch sec; seqNo:4967; 4057315.97 north; 591174.52 east 2021-02-12T23:34:30Z- AHRS_M2 msg: 1613172870.96 epoch sec; seqNo:4920 2021-02-12T23:34:30Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.17 psi 2021-02-12T23:34:30Z- handleDepth()-Depth_Keller msg: 1613172870970 epoch sec; seqNo:4968; depth 15.47 2021-02-12T23:34:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172870.98 epoch sec; seqNo:4968; 4057315.97 north; 591174.52 east 2021-02-12T23:34:31Z- AHRS_M2 msg: 1613172871.38 epoch sec; seqNo:4921 2021-02-12T23:34:31Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-12T23:34:31Z- handleDepth()-Depth_Keller msg: 1613172871387 epoch sec; seqNo:4969; depth 15.56 2021-02-12T23:34:31Z- handleDvl() - RDI_Pathfinder msg: 1613172871.40 epoch sec; seqNo:2407 2021-02-12T23:34:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.86 , 96.63 , 71.51 , 110.01 2021-02-12T23:34:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.03 2021-02-12T23:34:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172871.41 epoch sec; seqNo:4969; 4057316.36 north; 591175.74 east 2021-02-12T23:34:31Z- AHRS_M2 msg: 1613172871.77 epoch sec; seqNo:4922 2021-02-12T23:34:31Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.16 psi 2021-02-12T23:34:31Z- handleDepth()-Depth_Keller msg: 1613172871772 epoch sec; seqNo:4970; depth 15.70 2021-02-12T23:34:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172871.78 epoch sec; seqNo:4970; 4057316.74 north; 591175.73 east 2021-02-12T23:34:31Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-12T23:34:31Z- waiting for 1613172869.61, time is 1613172871.78 2021-02-12T23:34:31Z- LcmTrn::updateTrn() >>>> pose time 1613172871.78, meas time 1613172871.40 <<<< 2021-02-12T23:34:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172871.40 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:34:31Z- TerrainNav::motionUpdate() - currEstimate = (4057317, 591175.73, 15.70) 2021-02-12T23:34:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:34:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:34:31Z- TNavBF::Projecting Measurements 2021-02-12T23:34:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:34:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:34:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:34:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:34:31Z- TNavFilter::4 good beams. 2021-02-12T23:34:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:34:31Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:34:31Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:34:31Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:34:31Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:34:31Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:34:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:34:31Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:31Z- TerrainMapOctree: (vehN, vehE ) = (4057316.46, 591175.55). 2021-02-12T23:34:31Z- TerrainMapOctree: Min Distance = 4992.49. 2021-02-12T23:34:31Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:34:31Z- There is no specific good beam that all particles have in common. 2021-02-12T23:34:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:34:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 9.95 2021-02-12T23:34:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:34:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 14.94 2021-02-12T23:34:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:34:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 8.18 2021-02-12T23:34:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:34:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 19.36 2021-02-12T23:34:31Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:34:31Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:34:31Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:34:31Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:34:31Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:34:31Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:34:31Z- Filter 0 No beams used in subcloud update 2021-02-12T23:34:31Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:34:31Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:34:31Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 753.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:34:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172871.40 sec. 2021-02-12T23:34:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:34:31Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:31Z- variances([0], [2], and psi_berg) = 753.391459 833.036259 0.000000 2021-02-12T23:34:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:34:31Z- LcmTrn::publishEstimates() - MLE : 4057302.24 591137.23 15.70 2021-02-12T23:34:31Z- LcmTrn::publishEstimates() - MMSE : 4057316.74 591175.73 15.70 2021-02-12T23:34:31Z- LcmTrn::publishEstimates() - COVAR: 753.39 833.04 0.90 0.00 2021-02-12T23:34:32Z- AHRS_M2 msg: 1613172872.17 epoch sec; seqNo:4923 2021-02-12T23:34:32Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.16 psi 2021-02-12T23:34:32Z- handleDepth()-Depth_Keller msg: 1613172872177 epoch sec; seqNo:4971; depth 15.81 2021-02-12T23:34:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172872.19 epoch sec; seqNo:4971; 4057317.12 north; 591175.73 east 2021-02-12T23:34:32Z- AHRS_M2 msg: 1613172872.58 epoch sec; seqNo:4924 2021-02-12T23:34:32Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.16 psi 2021-02-12T23:34:32Z- handleDepth()-Depth_Keller msg: 1613172872590 epoch sec; seqNo:4972; depth 15.93 2021-02-12T23:34:32Z- handleDvl() - RDI_Pathfinder msg: 1613172872.60 epoch sec; seqNo:2408 2021-02-12T23:34:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.27 , 96.63 , 71.51 , 108.79 2021-02-12T23:34:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , -0.04 2021-02-12T23:34:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172872.61 epoch sec; seqNo:4972; 4057317.12 north; 591175.73 east 2021-02-12T23:34:32Z- AHRS_M2 msg: 1613172872.98 epoch sec; seqNo:4925 2021-02-12T23:34:32Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.17 psi 2021-02-12T23:34:32Z- handleDepth()-Depth_Keller msg: 1613172872984 epoch sec; seqNo:4973; depth 16.07 2021-02-12T23:34:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172872.99 epoch sec; seqNo:4973; 4057317.50 north; 591175.72 east 2021-02-12T23:34:33Z- AHRS_M2 msg: 1613172873.38 epoch sec; seqNo:4926 2021-02-12T23:34:33Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.17 psi 2021-02-12T23:34:33Z- handleDepth()-Depth_Keller msg: 1613172873391 epoch sec; seqNo:4974; depth 16.19 2021-02-12T23:34:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172873.40 epoch sec; seqNo:4974; 4057317.88 north; 591175.72 east 2021-02-12T23:34:33Z- AHRS_M2 msg: 1613172873.79 epoch sec; seqNo:4927 2021-02-12T23:34:33Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.17 psi 2021-02-12T23:34:33Z- handleDepth()-Depth_Keller msg: 1613172873798 epoch sec; seqNo:4975; depth 16.30 2021-02-12T23:34:33Z- handleDvl() - RDI_Pathfinder msg: 1613172873.81 epoch sec; seqNo:2409 2021-02-12T23:34:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.28 , 96.63 , 78.38 , 108.80 2021-02-12T23:34:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , -0.02 2021-02-12T23:34:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172873.82 epoch sec; seqNo:4975; 4057317.89 north; 591176.94 east 2021-02-12T23:34:34Z- AHRS_M2 msg: 1613172874.19 epoch sec; seqNo:4928 2021-02-12T23:34:34Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.17 psi 2021-02-12T23:34:34Z- handleDepth()-Depth_Keller msg: 1613172874196 epoch sec; seqNo:4976; depth 16.42 2021-02-12T23:34:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172874.21 epoch sec; seqNo:4976; 4057318.27 north; 591176.94 east 2021-02-12T23:34:34Z- AHRS_M2 msg: 1613172874.60 epoch sec; seqNo:4929 2021-02-12T23:34:34Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.17 psi 2021-02-12T23:34:34Z- handleDepth()-Depth_Keller msg: 1613172874603 epoch sec; seqNo:4977; depth 16.54 2021-02-12T23:34:34Z- handleDvl() - RDI_Pathfinder msg: 1613172874.62 epoch sec; seqNo:2410 2021-02-12T23:34:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.28 , 95.28 , 71.92 , 108.66 2021-02-12T23:34:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , -0.01 2021-02-12T23:34:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172874.65 epoch sec; seqNo:4977; 4057318.65 north; 591176.93 east 2021-02-12T23:34:35Z- AHRS_M2 msg: 1613172875.00 epoch sec; seqNo:4930 2021-02-12T23:34:35Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.17 psi 2021-02-12T23:34:35Z- handleDepth()-Depth_Keller msg: 1613172875005 epoch sec; seqNo:4978; depth 16.67 2021-02-12T23:34:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172875.02 epoch sec; seqNo:4978; 4057319.03 north; 591176.93 east 2021-02-12T23:34:35Z- AHRS_M2 msg: 1613172875.41 epoch sec; seqNo:4931 2021-02-12T23:34:35Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.17 psi 2021-02-12T23:34:35Z- handleDepth()-Depth_Keller msg: 1613172875410 epoch sec; seqNo:4979; depth 16.80 2021-02-12T23:34:35Z- handleDvl() - RDI_Pathfinder msg: 1613172875.43 epoch sec; seqNo:2411 2021-02-12T23:34:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 97.84 , 71.51 , 110.01 2021-02-12T23:34:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.02 2021-02-12T23:34:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172875.44 epoch sec; seqNo:4979; 4057319.03 north; 591176.93 east 2021-02-12T23:34:35Z- AHRS_M2 msg: 1613172875.81 epoch sec; seqNo:4932 2021-02-12T23:34:35Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.18 psi 2021-02-12T23:34:35Z- handleDepth()-Depth_Keller msg: 1613172875812 epoch sec; seqNo:4980; depth 16.93 2021-02-12T23:34:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172875.82 epoch sec; seqNo:4980; 4057319.40 north; 591176.92 east 2021-02-12T23:34:36Z- AHRS_M2 msg: 1613172876.21 epoch sec; seqNo:4933 2021-02-12T23:34:36Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.19 psi 2021-02-12T23:34:36Z- handleDepth()-Depth_Keller msg: 1613172876216 epoch sec; seqNo:4981; depth 17.04 2021-02-12T23:34:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172876.22 epoch sec; seqNo:4981; 4057319.80 north; 591178.14 east 2021-02-12T23:34:36Z- AHRS_M2 msg: 1613172876.62 epoch sec; seqNo:4934 2021-02-12T23:34:36Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.19 psi 2021-02-12T23:34:36Z- handleDepth()-Depth_Keller msg: 1613172876627 epoch sec; seqNo:4982; depth 17.17 2021-02-12T23:34:36Z- handleDvl() - RDI_Pathfinder msg: 1613172876.64 epoch sec; seqNo:2412 2021-02-12T23:34:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 96.56 , 70.69 , 108.72 2021-02-12T23:34:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , 0.01 2021-02-12T23:34:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172876.65 epoch sec; seqNo:4982; 4057319.80 north; 591178.14 east 2021-02-12T23:34:36Z- LcmTrn::handleMessages() - handling 41 messages took 0.68 ms and 128 calls... 2021-02-12T23:34:36Z- waiting for 1613172874.78, time is 1613172876.65 2021-02-12T23:34:36Z- LcmTrn::updateTrn() equating pose 1613172876.65 and meas times 1613172876.65 2021-02-12T23:34:36Z- LcmTrn::updateTrn() >>>> pose time 1613172876.65, meas time 1613172876.65 <<<< 2021-02-12T23:34:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172876.65 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:34:36Z- TerrainNav::motionUpdate() - currEstimate = (4057320, 591178.14, 17.17) 2021-02-12T23:34:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:34:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:34:36Z- TNavBF::Projecting Measurements 2021-02-12T23:34:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:34:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:34:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:34:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:34:36Z- TNavFilter::4 good beams. 2021-02-12T23:34:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:34:36Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:34:36Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:34:36Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:34:36Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:34:36Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:34:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:34:36Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:36Z- TerrainMapOctree: (vehN, vehE ) = (4057319.80, 591178.14). 2021-02-12T23:34:36Z- TerrainMapOctree: Min Distance = 4988.27. 2021-02-12T23:34:36Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:34:36Z- There is no specific good beam that all particles have in common. 2021-02-12T23:34:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:34:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 10.43 2021-02-12T23:34:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:34:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 14.92 2021-02-12T23:34:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:34:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-12T23:34:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:34:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 18.91 2021-02-12T23:34:36Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:34:37Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:34:37Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:34:37Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:34:37Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:34:37Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:34:37Z- Filter 0 No beams used in subcloud update 2021-02-12T23:34:37Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:34:37Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:34:37Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 753.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:34:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172876.65 sec. 2021-02-12T23:34:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:34:37Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:37Z- variances([0], [2], and psi_berg) = 753.290973 833.036161 0.000000 2021-02-12T23:34:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:34:37Z- LcmTrn::publishEstimates() - MLE : 4057305.30 591139.64 17.17 2021-02-12T23:34:37Z- LcmTrn::publishEstimates() - MMSE : 4057319.80 591178.14 17.17 2021-02-12T23:34:37Z- LcmTrn::publishEstimates() - COVAR: 753.29 833.04 0.83 0.00 2021-02-12T23:34:37Z- AHRS_M2 msg: 1613172877.02 epoch sec; seqNo:4935 2021-02-12T23:34:37Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.20 psi 2021-02-12T23:34:37Z- handleDepth()-Depth_Keller msg: 1613172877025 epoch sec; seqNo:4983; depth 17.30 2021-02-12T23:34:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172877.04 epoch sec; seqNo:4983; 4057320.17 north; 591178.14 east 2021-02-12T23:34:37Z- AHRS_M2 msg: 1613172877.45 epoch sec; seqNo:4936 2021-02-12T23:34:37Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.20 psi 2021-02-12T23:34:37Z- handleDepth()-Depth_Keller msg: 1613172877454 epoch sec; seqNo:4984; depth 17.44 2021-02-12T23:34:37Z- handleDvl() - RDI_Pathfinder msg: 1613172877.48 epoch sec; seqNo:2413 2021-02-12T23:34:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.59 , 97.84 , 69.87 , 110.01 2021-02-12T23:34:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , -0.02 2021-02-12T23:34:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172877.49 epoch sec; seqNo:4984; 4057320.55 north; 591178.13 east 2021-02-12T23:34:37Z- AHRS_M2 msg: 1613172877.83 epoch sec; seqNo:4937 2021-02-12T23:34:37Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.20 psi 2021-02-12T23:34:37Z- handleDepth()-Depth_Keller msg: 1613172877834 epoch sec; seqNo:4985; depth 17.58 2021-02-12T23:34:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172877.85 epoch sec; seqNo:4985; 4057320.55 north; 591178.13 east 2021-02-12T23:34:38Z- AHRS_M2 msg: 1613172878.23 epoch sec; seqNo:4938 2021-02-12T23:34:38Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.20 psi 2021-02-12T23:34:38Z- handleDepth()-Depth_Keller msg: 1613172878237 epoch sec; seqNo:4986; depth 17.69 2021-02-12T23:34:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172878.26 epoch sec; seqNo:4986; 4057320.93 north; 591178.13 east 2021-02-12T23:34:38Z- AHRS_M2 msg: 1613172878.63 epoch sec; seqNo:4939 2021-02-12T23:34:38Z- AHRS_M2 msg: 0.70 phi; -0.35 theta; -0.19 psi 2021-02-12T23:34:38Z- handleDepth()-Depth_Keller msg: 1613172878647 epoch sec; seqNo:4987; depth 17.82 2021-02-12T23:34:38Z- handleDvl() - RDI_Pathfinder msg: 1613172878.66 epoch sec; seqNo:2414 2021-02-12T23:34:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.41 , 97.84 , 71.10 , 108.79 2021-02-12T23:34:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , -0.05 2021-02-12T23:34:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172878.68 epoch sec; seqNo:4987; 4057321.32 north; 591179.35 east 2021-02-12T23:34:39Z- AHRS_M2 msg: 1613172879.05 epoch sec; seqNo:4940 2021-02-12T23:34:39Z- AHRS_M2 msg: 0.71 phi; -0.35 theta; -0.18 psi 2021-02-12T23:34:39Z- handleDepth()-Depth_Keller msg: 1613172879055 epoch sec; seqNo:4988; depth 17.95 2021-02-12T23:34:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172879.07 epoch sec; seqNo:4988; 4057321.70 north; 591179.34 east 2021-02-12T23:34:39Z- AHRS_M2 msg: 1613172879.45 epoch sec; seqNo:4941 2021-02-12T23:34:39Z- AHRS_M2 msg: 0.71 phi; -0.35 theta; -0.17 psi 2021-02-12T23:34:39Z- handleDepth()-Depth_Keller msg: 1613172879458 epoch sec; seqNo:4989; depth 18.08 2021-02-12T23:34:39Z- handleDvl() - RDI_Pathfinder msg: 1613172879.47 epoch sec; seqNo:2415 2021-02-12T23:34:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.64 , 95.48 , 69.88 , 108.87 2021-02-12T23:34:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , -0.06 2021-02-12T23:34:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172879.48 epoch sec; seqNo:4989; 4057321.70 north; 591179.34 east 2021-02-12T23:34:39Z- AHRS_M2 msg: 1613172879.85 epoch sec; seqNo:4942 2021-02-12T23:34:39Z- AHRS_M2 msg: 0.70 phi; -0.35 theta; -0.17 psi 2021-02-12T23:34:39Z- handleDepth()-Depth_Keller msg: 1613172879852 epoch sec; seqNo:4990; depth 18.19 2021-02-12T23:34:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172879.87 epoch sec; seqNo:4990; 4057322.08 north; 591179.34 east 2021-02-12T23:34:40Z- AHRS_M2 msg: 1613172880.26 epoch sec; seqNo:4943 2021-02-12T23:34:40Z- AHRS_M2 msg: 0.70 phi; -0.35 theta; -0.17 psi 2021-02-12T23:34:40Z- handleDepth()-Depth_Keller msg: 1613172880268 epoch sec; seqNo:4991; depth 18.32 2021-02-12T23:34:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:34:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:34:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:34:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:34:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:34:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:34:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:34:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:34:40Z- handleDvl() - RDI_Pathfinder msg: 1613172880.29 epoch sec; seqNo:2416 2021-02-12T23:34:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:34:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , -0.06 2021-02-12T23:34:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172880.30 epoch sec; seqNo:4991; 4057322.46 north; 591179.33 east 2021-02-12T23:34:40Z- AHRS_M2 msg: 1613172880.66 epoch sec; seqNo:4944 2021-02-12T23:34:40Z- AHRS_M2 msg: 0.70 phi; -0.35 theta; -0.18 psi 2021-02-12T23:34:40Z- handleDepth()-Depth_Keller msg: 1613172880660 epoch sec; seqNo:4992; depth 18.45 2021-02-12T23:34:40Z- handleDvl() - RDI_Pathfinder msg: 1613172880.67 epoch sec; seqNo:2417 2021-02-12T23:34:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 94.27 , 68.24 , 104.00 2021-02-12T23:34:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , -0.05 2021-02-12T23:34:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172880.68 epoch sec; seqNo:4992; 4057322.46 north; 591179.33 east 2021-02-12T23:34:41Z- AHRS_M2 msg: 1613172881.06 epoch sec; seqNo:4945 2021-02-12T23:34:41Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.18 psi 2021-02-12T23:34:41Z- handleDepth()-Depth_Keller msg: 1613172881067 epoch sec; seqNo:4993; depth 18.57 2021-02-12T23:34:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172881.08 epoch sec; seqNo:4993; 4057322.85 north; 591180.55 east 2021-02-12T23:34:41Z- AHRS_M2 msg: 1613172881.47 epoch sec; seqNo:4946 2021-02-12T23:34:41Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.19 psi 2021-02-12T23:34:41Z- handleDepth()-Depth_Keller msg: 1613172881473 epoch sec; seqNo:4994; depth 18.68 2021-02-12T23:34:41Z- handleDvl() - RDI_Pathfinder msg: 1613172881.50 epoch sec; seqNo:2418 2021-02-12T23:34:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.82 , 93.25 , 69.87 , 107.85 2021-02-12T23:34:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , -0.00 2021-02-12T23:34:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172881.51 epoch sec; seqNo:4994; 4057323.23 north; 591180.55 east 2021-02-12T23:34:41Z- AHRS_M2 msg: 1613172881.87 epoch sec; seqNo:4947 2021-02-12T23:34:41Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.20 psi 2021-02-12T23:34:41Z- handleDepth()-Depth_Keller msg: 1613172881872 epoch sec; seqNo:4995; depth 18.82 2021-02-12T23:34:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172881.88 epoch sec; seqNo:4995; 4057323.61 north; 591180.54 east 2021-02-12T23:34:42Z- LcmTrn::handleMessages() - handling 45 messages took 0.64 ms and 132 calls... 2021-02-12T23:34:42Z- waiting for 1613172879.65, time is 1613172881.88 2021-02-12T23:34:42Z- LcmTrn::updateTrn() >>>> pose time 1613172881.88, meas time 1613172881.50 <<<< 2021-02-12T23:34:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172881.50 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:34:42Z- TerrainNav::motionUpdate() - currEstimate = (4057324, 591180.54, 18.82) 2021-02-12T23:34:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:34:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:34:42Z- TNavBF::Projecting Measurements 2021-02-12T23:34:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:34:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:34:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:34:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:34:42Z- TNavFilter::4 good beams. 2021-02-12T23:34:42Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:34:42Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:34:42Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:34:42Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:34:42Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:34:42Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:34:42Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:34:42Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:42Z- TerrainMapOctree: (vehN, vehE ) = (4057323.33, 591180.37). 2021-02-12T23:34:42Z- TerrainMapOctree: Min Distance = 4984.11. 2021-02-12T23:34:42Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:34:42Z- There is no specific good beam that all particles have in common. 2021-02-12T23:34:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:34:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 9.44 2021-02-12T23:34:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:34:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 13.91 2021-02-12T23:34:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:34:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 7.81 2021-02-12T23:34:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:34:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 18.61 2021-02-12T23:34:42Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:34:42Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:34:42Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:34:42Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:34:42Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:34:42Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:34:42Z- Filter 0 No beams used in subcloud update 2021-02-12T23:34:42Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:34:42Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:34:42Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 753.2 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:34:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172881.50 sec. 2021-02-12T23:34:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:34:42Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:42Z- variances([0], [2], and psi_berg) = 753.157790 833.036032 0.000000 2021-02-12T23:34:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:34:42Z- LcmTrn::publishEstimates() - MLE : 4057309.11 591142.04 18.82 2021-02-12T23:34:42Z- LcmTrn::publishEstimates() - MMSE : 4057323.61 591180.54 18.82 2021-02-12T23:34:42Z- LcmTrn::publishEstimates() - COVAR: 753.16 833.04 0.81 0.00 2021-02-12T23:34:42Z- AHRS_M2 msg: 1613172882.27 epoch sec; seqNo:4948 2021-02-12T23:34:42Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.21 psi 2021-02-12T23:34:42Z- handleDepth()-Depth_Keller msg: 1613172882277 epoch sec; seqNo:4996; depth 18.94 2021-02-12T23:34:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172882.29 epoch sec; seqNo:4996; 4057323.61 north; 591180.54 east 2021-02-12T23:34:42Z- AHRS_M2 msg: 1613172882.68 epoch sec; seqNo:4949 2021-02-12T23:34:42Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.22 psi 2021-02-12T23:34:42Z- handleDepth()-Depth_Keller msg: 1613172882697 epoch sec; seqNo:4997; depth 19.08 2021-02-12T23:34:42Z- handleDvl() - RDI_Pathfinder msg: 1613172882.72 epoch sec; seqNo:2419 2021-02-12T23:34:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.00 , 94.40 , 69.87 , 106.56 2021-02-12T23:34:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , 0.07 2021-02-12T23:34:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172882.75 epoch sec; seqNo:4997; 4057323.99 north; 591180.54 east 2021-02-12T23:34:43Z- AHRS_M2 msg: 1613172883.08 epoch sec; seqNo:4950 2021-02-12T23:34:43Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.22 psi 2021-02-12T23:34:43Z- handleDepth()-Depth_Keller msg: 1613172883089 epoch sec; seqNo:4998; depth 19.21 2021-02-12T23:34:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172883.11 epoch sec; seqNo:4998; 4057324.37 north; 591180.54 east 2021-02-12T23:34:43Z- AHRS_M2 msg: 1613172883.48 epoch sec; seqNo:4951 2021-02-12T23:34:43Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.23 psi 2021-02-12T23:34:43Z- handleDepth()-Depth_Keller msg: 1613172883488 epoch sec; seqNo:4999; depth 19.32 2021-02-12T23:34:43Z- handleDvl() - RDI_Pathfinder msg: 1613172883.50 epoch sec; seqNo:2420 2021-02-12T23:34:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.19 , 95.62 , 69.06 , 107.79 2021-02-12T23:34:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , 0.10 2021-02-12T23:34:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172883.51 epoch sec; seqNo:4999; 4057324.37 north; 591180.54 east 2021-02-12T23:34:43Z- AHRS_M2 msg: 1613172883.89 epoch sec; seqNo:4952 2021-02-12T23:34:43Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.23 psi 2021-02-12T23:34:43Z- handleDepth()-Depth_Keller msg: 1613172883893 epoch sec; seqNo:5000; depth 19.46 2021-02-12T23:34:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:34:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:34:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:34:43Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:34:43Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:34:43Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:34:43Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:34:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:34:43Z- handleDvl() - RDI_Pathfinder msg: 1613172883.89 epoch sec; seqNo:2421 2021-02-12T23:34:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:34:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , 0.10 2021-02-12T23:34:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172883.91 epoch sec; seqNo:5000; 4057324.76 north; 591181.75 east 2021-02-12T23:34:44Z- AHRS_M2 msg: 1613172884.29 epoch sec; seqNo:4953 2021-02-12T23:34:44Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.23 psi 2021-02-12T23:34:44Z- handleDepth()-Depth_Keller msg: 1613172884305 epoch sec; seqNo:5001; depth 19.59 2021-02-12T23:34:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172884.31 epoch sec; seqNo:5001; 4057325.14 north; 591181.75 east 2021-02-12T23:34:44Z- AHRS_M2 msg: 1613172884.70 epoch sec; seqNo:4954 2021-02-12T23:34:44Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.23 psi 2021-02-12T23:34:44Z- handleDepth()-Depth_Keller msg: 1613172884700 epoch sec; seqNo:5002; depth 19.70 2021-02-12T23:34:44Z- handleDvl() - RDI_Pathfinder msg: 1613172884.72 epoch sec; seqNo:2422 2021-02-12T23:34:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.19 , 96.83 , 78.58 , 106.57 2021-02-12T23:34:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.09 , 0.07 2021-02-12T23:34:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172884.73 epoch sec; seqNo:5002; 4057325.14 north; 591181.75 east 2021-02-12T23:34:45Z- AHRS_M2 msg: 1613172885.10 epoch sec; seqNo:4955 2021-02-12T23:34:45Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.23 psi 2021-02-12T23:34:45Z- handleDepth()-Depth_Keller msg: 1613172885104 epoch sec; seqNo:5003; depth 19.83 2021-02-12T23:34:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:34:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:34:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:34:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:34:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:34:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:34:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:34:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:34:45Z- handleDvl() - RDI_Pathfinder msg: 1613172885.11 epoch sec; seqNo:2423 2021-02-12T23:34:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:34:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.09 , 0.07 2021-02-12T23:34:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172885.12 epoch sec; seqNo:5003; 4057325.52 north; 591181.74 east 2021-02-12T23:34:45Z- AHRS_M2 msg: 1613172885.51 epoch sec; seqNo:4956 2021-02-12T23:34:45Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.23 psi 2021-02-12T23:34:45Z- handleDepth()-Depth_Keller msg: 1613172885510 epoch sec; seqNo:5004; depth 19.94 2021-02-12T23:34:45Z- handleDvl() - RDI_Pathfinder msg: 1613172885.52 epoch sec; seqNo:2424 2021-02-12T23:34:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 97.85 , 69.88 , 107.58 2021-02-12T23:34:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.10 , -0.01 2021-02-12T23:34:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172885.54 epoch sec; seqNo:5004; 4057325.90 north; 591181.74 east 2021-02-12T23:34:45Z- AHRS_M2 msg: 1613172885.91 epoch sec; seqNo:4957 2021-02-12T23:34:45Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.23 psi 2021-02-12T23:34:45Z- handleDepth()-Depth_Keller msg: 1613172885914 epoch sec; seqNo:5005; depth 20.08 2021-02-12T23:34:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172885.92 epoch sec; seqNo:5005; 4057325.90 north; 591181.74 east 2021-02-12T23:34:46Z- AHRS_M2 msg: 1613172886.31 epoch sec; seqNo:4958 2021-02-12T23:34:46Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.22 psi 2021-02-12T23:34:46Z- handleDepth()-Depth_Keller msg: 1613172886320 epoch sec; seqNo:5006; depth 20.18 2021-02-12T23:34:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:34:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:34:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:34:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:34:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:34:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:34:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:34:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:34:46Z- handleDvl() - RDI_Pathfinder msg: 1613172886.33 epoch sec; seqNo:2425 2021-02-12T23:34:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:34:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.10 , -0.01 2021-02-12T23:34:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172886.35 epoch sec; seqNo:5006; 4057326.29 north; 591182.96 east 2021-02-12T23:34:46Z- AHRS_M2 msg: 1613172886.72 epoch sec; seqNo:4959 2021-02-12T23:34:46Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.22 psi 2021-02-12T23:34:46Z- handleDepth()-Depth_Keller msg: 1613172886720 epoch sec; seqNo:5007; depth 20.32 2021-02-12T23:34:46Z- handleDvl() - RDI_Pathfinder msg: 1613172886.73 epoch sec; seqNo:2426 2021-02-12T23:34:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 96.77 , 67.84 , 105.28 2021-02-12T23:34:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.04 , -0.07 2021-02-12T23:34:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172886.76 epoch sec; seqNo:5007; 4057326.67 north; 591182.95 east 2021-02-12T23:34:47Z- AHRS_M2 msg: 1613172887.12 epoch sec; seqNo:4960 2021-02-12T23:34:47Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.22 psi 2021-02-12T23:34:47Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-12T23:34:47Z- waiting for 1613172884.88, time is 1613172887.12 2021-02-12T23:34:47Z- LcmTrn::updateTrn() >>>> pose time 1613172887.12, meas time 1613172886.73 <<<< 2021-02-12T23:34:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172886.73 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:34:47Z- TerrainNav::motionUpdate() - currEstimate = (4057327, 591182.95, 20.32) 2021-02-12T23:34:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:34:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:34:47Z- TNavBF::Projecting Measurements 2021-02-12T23:34:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:34:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:34:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:34:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:34:47Z- TNavFilter::4 good beams. 2021-02-12T23:34:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:34:47Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:34:47Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:34:47Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:34:47Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:34:47Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:34:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:34:47Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:47Z- TerrainMapOctree: (vehN, vehE ) = (4057326.44, 591182.77). 2021-02-12T23:34:47Z- TerrainMapOctree: Min Distance = 4980.18. 2021-02-12T23:34:47Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:34:47Z- There is no specific good beam that all particles have in common. 2021-02-12T23:34:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:34:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-12T23:34:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:34:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 14.98 2021-02-12T23:34:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:34:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 7.36 2021-02-12T23:34:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:34:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 17.74 2021-02-12T23:34:47Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:34:47Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:34:47Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:34:47Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:34:47Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:34:47Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:34:47Z- Filter 0 No beams used in subcloud update 2021-02-12T23:34:47Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:34:47Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:34:47Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 753.1 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:34:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172886.73 sec. 2021-02-12T23:34:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:34:47Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:47Z- variances([0], [2], and psi_berg) = 753.034103 833.035913 0.000000 2021-02-12T23:34:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:34:47Z- LcmTrn::publishEstimates() - MLE : 4057312.17 591144.45 20.32 2021-02-12T23:34:47Z- LcmTrn::publishEstimates() - MMSE : 4057326.67 591182.95 20.32 2021-02-12T23:34:47Z- LcmTrn::publishEstimates() - COVAR: 753.03 833.04 0.77 0.00 2021-02-12T23:34:47Z- handleDepth()-Depth_Keller msg: 1613172887133 epoch sec; seqNo:5008; depth 20.46 2021-02-12T23:34:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172887.15 epoch sec; seqNo:5008; 4057327.05 north; 591182.95 east 2021-02-12T23:34:47Z- AHRS_M2 msg: 1613172887.53 epoch sec; seqNo:4961 2021-02-12T23:34:47Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.22 psi 2021-02-12T23:34:47Z- handleDepth()-Depth_Keller msg: 1613172887534 epoch sec; seqNo:5009; depth 20.58 2021-02-12T23:34:47Z- handleDvl() - RDI_Pathfinder msg: 1613172887.55 epoch sec; seqNo:2427 2021-02-12T23:34:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 95.68 , 69.06 , 102.99 2021-02-12T23:34:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.07 2021-02-12T23:34:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172887.56 epoch sec; seqNo:5009; 4057327.05 north; 591182.95 east 2021-02-12T23:34:47Z- AHRS_M2 msg: 1613172887.93 epoch sec; seqNo:4962 2021-02-12T23:34:47Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.22 psi 2021-02-12T23:34:47Z- handleDepth()-Depth_Keller msg: 1613172887932 epoch sec; seqNo:5010; depth 20.67 2021-02-12T23:34:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172887.95 epoch sec; seqNo:5010; 4057327.42 north; 591182.94 east 2021-02-12T23:34:48Z- AHRS_M2 msg: 1613172888.34 epoch sec; seqNo:4963 2021-02-12T23:34:48Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.22 psi 2021-02-12T23:34:48Z- handleDepth()-Depth_Keller msg: 1613172888340 epoch sec; seqNo:5011; depth 20.80 2021-02-12T23:34:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172888.38 epoch sec; seqNo:5011; 4057327.80 north; 591182.94 east 2021-02-12T23:34:48Z- AHRS_M2 msg: 1613172888.74 epoch sec; seqNo:4964 2021-02-12T23:34:48Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.21 psi 2021-02-12T23:34:48Z- handleDepth()-Depth_Keller msg: 1613172888745 epoch sec; seqNo:5012; depth 20.92 2021-02-12T23:34:48Z- handleDvl() - RDI_Pathfinder msg: 1613172888.76 epoch sec; seqNo:2428 2021-02-12T23:34:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 94.54 , 78.72 , 104.27 2021-02-12T23:34:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , -0.04 2021-02-12T23:34:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172888.78 epoch sec; seqNo:5012; 4057328.20 north; 591184.16 east 2021-02-12T23:34:49Z- AHRS_M2 msg: 1613172889.14 epoch sec; seqNo:4965 2021-02-12T23:34:49Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.21 psi 2021-02-12T23:34:49Z- handleDepth()-Depth_Keller msg: 1613172889148 epoch sec; seqNo:5013; depth 21.05 2021-02-12T23:34:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172889.16 epoch sec; seqNo:5013; 4057328.20 north; 591184.16 east 2021-02-12T23:34:49Z- AHRS_M2 msg: 1613172889.54 epoch sec; seqNo:4966 2021-02-12T23:34:49Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.20 psi 2021-02-12T23:34:49Z- handleDepth()-Depth_Keller msg: 1613172889550 epoch sec; seqNo:5014; depth 21.18 2021-02-12T23:34:49Z- handleDvl() - RDI_Pathfinder msg: 1613172889.56 epoch sec; seqNo:2429 2021-02-12T23:34:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 93.12 , 67.84 , 104.07 2021-02-12T23:34:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.00 2021-02-12T23:34:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172889.57 epoch sec; seqNo:5014; 4057328.57 north; 591184.15 east 2021-02-12T23:34:49Z- AHRS_M2 msg: 1613172889.95 epoch sec; seqNo:4967 2021-02-12T23:34:49Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.20 psi 2021-02-12T23:34:49Z- handleDepth()-Depth_Keller msg: 1613172889953 epoch sec; seqNo:5015; depth 21.30 2021-02-12T23:34:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172889.97 epoch sec; seqNo:5015; 4057328.95 north; 591184.15 east 2021-02-12T23:34:50Z- AHRS_M2 msg: 1613172890.37 epoch sec; seqNo:4968 2021-02-12T23:34:50Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.20 psi 2021-02-12T23:34:50Z- handleDepth()-Depth_Keller msg: 1613172890388 epoch sec; seqNo:5016; depth 21.42 2021-02-12T23:34:50Z- handleDvl() - RDI_Pathfinder msg: 1613172890.40 epoch sec; seqNo:2430 2021-02-12T23:34:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.79 , 98.25 , 67.84 , 97.04 2021-02-12T23:34:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , 0.01 2021-02-12T23:34:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172890.41 epoch sec; seqNo:5016; 4057329.33 north; 591184.15 east 2021-02-12T23:34:50Z- AHRS_M2 msg: 1613172890.76 epoch sec; seqNo:4969 2021-02-12T23:34:50Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-12T23:34:50Z- handleDepth()-Depth_Keller msg: 1613172890769 epoch sec; seqNo:5017; depth 21.55 2021-02-12T23:34:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172890.78 epoch sec; seqNo:5017; 4057329.33 north; 591184.15 east 2021-02-12T23:34:51Z- AHRS_M2 msg: 1613172891.16 epoch sec; seqNo:4970 2021-02-12T23:34:51Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-12T23:34:51Z- handleDepth()-Depth_Keller msg: 1613172891167 epoch sec; seqNo:5018; depth 21.66 2021-02-12T23:34:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172891.19 epoch sec; seqNo:5018; 4057329.71 north; 591184.14 east 2021-02-12T23:34:51Z- AHRS_M2 msg: 1613172891.57 epoch sec; seqNo:4971 2021-02-12T23:34:51Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-12T23:34:51Z- handleDepth()-Depth_Keller msg: 1613172891572 epoch sec; seqNo:5019; depth 21.78 2021-02-12T23:34:51Z- handleDvl() - RDI_Pathfinder msg: 1613172891.58 epoch sec; seqNo:2431 2021-02-12T23:34:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 98.12 , 67.43 , 96.90 2021-02-12T23:34:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.05 , -0.00 2021-02-12T23:34:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172891.59 epoch sec; seqNo:5019; 4057330.10 north; 591185.36 east 2021-02-12T23:34:51Z- AHRS_M2 msg: 1613172891.97 epoch sec; seqNo:4972 2021-02-12T23:34:51Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-12T23:34:51Z- handleDepth()-Depth_Keller msg: 1613172891981 epoch sec; seqNo:5020; depth 21.93 2021-02-12T23:34:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172891.99 epoch sec; seqNo:5020; 4057330.48 north; 591185.35 east 2021-02-12T23:34:52Z- LcmTrn::handleMessages() - handling 43 messages took 0.73 ms and 129 calls... 2021-02-12T23:34:52Z- waiting for 1613172890.12, time is 1613172891.99 2021-02-12T23:34:52Z- LcmTrn::updateTrn() >>>> pose time 1613172891.99, meas time 1613172891.58 <<<< 2021-02-12T23:34:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172891.58 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:34:52Z- TerrainNav::motionUpdate() - currEstimate = (4057330, 591185.35, 21.93) 2021-02-12T23:34:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:34:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:34:52Z- TNavBF::Projecting Measurements 2021-02-12T23:34:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:34:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:34:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:34:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:34:52Z- TNavFilter::4 good beams. 2021-02-12T23:34:52Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:34:52Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:34:52Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:34:52Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:34:52Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:34:52Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:34:52Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:34:52Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:52Z- TerrainMapOctree: (vehN, vehE ) = (4057330.16, 591185.15). 2021-02-12T23:34:52Z- TerrainMapOctree: Min Distance = 4975.78. 2021-02-12T23:34:52Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:34:52Z- There is no specific good beam that all particles have in common. 2021-02-12T23:34:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:34:52Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 8.66 2021-02-12T23:34:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:34:52Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 15.40 2021-02-12T23:34:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:34:52Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 7.27 2021-02-12T23:34:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:34:52Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 15.02 2021-02-12T23:34:52Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:34:52Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:34:52Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:34:52Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:34:52Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:34:52Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:34:52Z- Filter 0 No beams used in subcloud update 2021-02-12T23:34:52Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:34:52Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:34:52Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 752.9 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:34:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172891.58 sec. 2021-02-12T23:34:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:34:52Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:52Z- variances([0], [2], and psi_berg) = 752.900019 833.035783 0.000000 2021-02-12T23:34:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:34:52Z- LcmTrn::publishEstimates() - MLE : 4057315.98 591146.85 21.93 2021-02-12T23:34:52Z- LcmTrn::publishEstimates() - MMSE : 4057330.48 591185.35 21.93 2021-02-12T23:34:52Z- LcmTrn::publishEstimates() - COVAR: 752.90 833.04 0.76 0.00 2021-02-12T23:34:52Z- AHRS_M2 msg: 1613172892.37 epoch sec; seqNo:4973 2021-02-12T23:34:52Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-12T23:34:52Z- handleDepth()-Depth_Keller msg: 1613172892376 epoch sec; seqNo:5021; depth 22.04 2021-02-12T23:34:52Z- handleDvl() - RDI_Pathfinder msg: 1613172892.39 epoch sec; seqNo:2432 2021-02-12T23:34:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 89.74 , 66.61 , 101.90 2021-02-12T23:34:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , -0.01 2021-02-12T23:34:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172892.40 epoch sec; seqNo:5021; 4057330.86 north; 591185.35 east 2021-02-12T23:34:52Z- AHRS_M2 msg: 1613172892.78 epoch sec; seqNo:4974 2021-02-12T23:34:52Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-12T23:34:52Z- handleDepth()-Depth_Keller msg: 1613172892779 epoch sec; seqNo:5022; depth 22.16 2021-02-12T23:34:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172892.79 epoch sec; seqNo:5022; 4057330.86 north; 591185.35 east 2021-02-12T23:34:53Z- AHRS_M2 msg: 1613172893.18 epoch sec; seqNo:4975 2021-02-12T23:34:53Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-12T23:34:53Z- handleDepth()-Depth_Keller msg: 1613172893189 epoch sec; seqNo:5023; depth 22.27 2021-02-12T23:34:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172893.20 epoch sec; seqNo:5023; 4057331.24 north; 591185.35 east 2021-02-12T23:34:53Z- AHRS_M2 msg: 1613172893.59 epoch sec; seqNo:4976 2021-02-12T23:34:53Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-12T23:34:53Z- handleDepth()-Depth_Keller msg: 1613172893589 epoch sec; seqNo:5024; depth 22.40 2021-02-12T23:34:53Z- handleDvl() - RDI_Pathfinder msg: 1613172893.60 epoch sec; seqNo:2433 2021-02-12T23:34:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 89.87 , 66.20 , 100.82 2021-02-12T23:34:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , 0.01 2021-02-12T23:34:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172893.61 epoch sec; seqNo:5024; 4057331.62 north; 591185.34 east 2021-02-12T23:34:53Z- AHRS_M2 msg: 1613172893.99 epoch sec; seqNo:4977 2021-02-12T23:34:53Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-12T23:34:53Z- handleDepth()-Depth_Keller msg: 1613172893992 epoch sec; seqNo:5025; depth 22.54 2021-02-12T23:34:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172894.01 epoch sec; seqNo:5025; 4057332.01 north; 591186.56 east 2021-02-12T23:34:54Z- AHRS_M2 msg: 1613172894.41 epoch sec; seqNo:4978 2021-02-12T23:34:54Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-12T23:34:54Z- handleDepth()-Depth_Keller msg: 1613172894420 epoch sec; seqNo:5026; depth 22.66 2021-02-12T23:34:54Z- handleDvl() - RDI_Pathfinder msg: 1613172894.44 epoch sec; seqNo:2434 2021-02-12T23:34:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 90.21 , 64.97 , 96.70 2021-02-12T23:34:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.07 , 0.03 2021-02-12T23:34:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172894.45 epoch sec; seqNo:5026; 4057332.01 north; 591186.56 east 2021-02-12T23:34:54Z- AHRS_M2 msg: 1613172894.80 epoch sec; seqNo:4979 2021-02-12T23:34:54Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-12T23:34:54Z- handleDepth()-Depth_Keller msg: 1613172894805 epoch sec; seqNo:5027; depth 22.79 2021-02-12T23:34:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172894.82 epoch sec; seqNo:5027; 4057332.39 north; 591186.56 east 2021-02-12T23:34:55Z- AHRS_M2 msg: 1613172895.20 epoch sec; seqNo:4980 2021-02-12T23:34:55Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.19 psi 2021-02-12T23:34:55Z- handleDepth()-Depth_Keller msg: 1613172895205 epoch sec; seqNo:5028; depth 22.90 2021-02-12T23:34:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172895.21 epoch sec; seqNo:5028; 4057332.77 north; 591186.55 east 2021-02-12T23:34:55Z- AHRS_M2 msg: 1613172895.60 epoch sec; seqNo:4981 2021-02-12T23:34:55Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.19 psi 2021-02-12T23:34:55Z- handleDepth()-Depth_Keller msg: 1613172895622 epoch sec; seqNo:5029; depth 23.02 2021-02-12T23:34:55Z- handleDvl() - RDI_Pathfinder msg: 1613172895.63 epoch sec; seqNo:2435 2021-02-12T23:34:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 89.87 , 66.61 , 97.17 2021-02-12T23:34:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , 0.04 2021-02-12T23:34:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172895.64 epoch sec; seqNo:5029; 4057333.15 north; 591186.55 east 2021-02-12T23:34:56Z- AHRS_M2 msg: 1613172896.01 epoch sec; seqNo:4982 2021-02-12T23:34:56Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.19 psi 2021-02-12T23:34:56Z- handleDepth()-Depth_Keller msg: 1613172896014 epoch sec; seqNo:5030; depth 23.15 2021-02-12T23:34:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172896.03 epoch sec; seqNo:5030; 4057333.15 north; 591186.55 east 2021-02-12T23:34:56Z- AHRS_M2 msg: 1613172896.42 epoch sec; seqNo:4983 2021-02-12T23:34:56Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:34:56Z- handleDepth()-Depth_Keller msg: 1613172896431 epoch sec; seqNo:5031; depth 23.27 2021-02-12T23:34:56Z- handleDvl() - RDI_Pathfinder msg: 1613172896.46 epoch sec; seqNo:2436 2021-02-12T23:34:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 87.03 , 65.79 , 96.77 2021-02-12T23:34:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , 0.05 2021-02-12T23:34:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172896.48 epoch sec; seqNo:5031; 4057333.53 north; 591186.54 east 2021-02-12T23:34:56Z- AHRS_M2 msg: 1613172896.83 epoch sec; seqNo:4984 2021-02-12T23:34:56Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:34:56Z- handleDepth()-Depth_Keller msg: 1613172896831 epoch sec; seqNo:5032; depth 23.39 2021-02-12T23:34:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172896.84 epoch sec; seqNo:5032; 4057333.90 north; 591186.54 east 2021-02-12T23:34:57Z- AHRS_M2 msg: 1613172897.22 epoch sec; seqNo:4985 2021-02-12T23:34:57Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:34:57Z- handleDepth()-Depth_Keller msg: 1613172897225 epoch sec; seqNo:5033; depth 23.50 2021-02-12T23:34:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172897.24 epoch sec; seqNo:5033; 4057333.92 north; 591187.76 east 2021-02-12T23:34:57Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-12T23:34:57Z- waiting for 1613172894.99, time is 1613172897.24 2021-02-12T23:34:57Z- LcmTrn::updateTrn() >>>> pose time 1613172897.24, meas time 1613172896.46 <<<< 2021-02-12T23:34:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172896.46 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:34:57Z- TerrainNav::motionUpdate() - currEstimate = (4057334, 591187.76, 23.50) 2021-02-12T23:34:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:34:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:34:57Z- TNavBF::Projecting Measurements 2021-02-12T23:34:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:34:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:34:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:34:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:34:57Z- TNavFilter::4 good beams. 2021-02-12T23:34:57Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:34:57Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:34:57Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:34:57Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:34:57Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:34:57Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:34:57Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:34:57Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:57Z- TerrainMapOctree: (vehN, vehE ) = (4057333.40, 591187.40). 2021-02-12T23:34:57Z- TerrainMapOctree: Min Distance = 4971.84. 2021-02-12T23:34:57Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:34:57Z- There is no specific good beam that all particles have in common. 2021-02-12T23:34:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:34:57Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-12T23:34:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:34:57Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 12.12 2021-02-12T23:34:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:34:57Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 6.93 2021-02-12T23:34:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:34:57Z- Old Depth Bias: 0.00 DepthBiasCov: 0.74 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 14.98 2021-02-12T23:34:57Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:34:57Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:34:57Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:34:57Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:34:57Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:34:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:34:57Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:34:57Z- Filter 0 No beams used in subcloud update 2021-02-12T23:34:57Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:34:57Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:34:57Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 752.8 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:34:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172896.46 sec. 2021-02-12T23:34:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:34:57Z- TNavBF::mmse selected from filter -1 2021-02-12T23:34:57Z- variances([0], [2], and psi_berg) = 752.764475 833.035653 0.000000 2021-02-12T23:34:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:34:57Z- LcmTrn::publishEstimates() - MLE : 4057319.42 591149.26 23.50 2021-02-12T23:34:57Z- LcmTrn::publishEstimates() - MMSE : 4057333.92 591187.76 23.50 2021-02-12T23:34:57Z- LcmTrn::publishEstimates() - COVAR: 752.76 833.04 0.74 0.00 2021-02-12T23:34:57Z- AHRS_M2 msg: 1613172897.62 epoch sec; seqNo:4986 2021-02-12T23:34:57Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:34:57Z- handleDepth()-Depth_Keller msg: 1613172897628 epoch sec; seqNo:5034; depth 23.62 2021-02-12T23:34:57Z- handleDvl() - RDI_Pathfinder msg: 1613172897.64 epoch sec; seqNo:2437 2021-02-12T23:34:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 89.26 , 65.38 , 100.08 2021-02-12T23:34:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , 0.02 2021-02-12T23:34:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172897.65 epoch sec; seqNo:5034; 4057334.30 north; 591187.76 east 2021-02-12T23:34:58Z- AHRS_M2 msg: 1613172898.03 epoch sec; seqNo:4987 2021-02-12T23:34:58Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:34:58Z- handleDepth()-Depth_Keller msg: 1613172898033 epoch sec; seqNo:5035; depth 23.75 2021-02-12T23:34:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172898.04 epoch sec; seqNo:5035; 4057334.67 north; 591187.75 east 2021-02-12T23:34:58Z- AHRS_M2 msg: 1613172898.43 epoch sec; seqNo:4988 2021-02-12T23:34:58Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:34:58Z- handleDepth()-Depth_Keller msg: 1613172898436 epoch sec; seqNo:5036; depth 23.85 2021-02-12T23:34:58Z- handleDvl() - RDI_Pathfinder msg: 1613172898.45 epoch sec; seqNo:2438 2021-02-12T23:34:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 90.28 , 64.97 , 96.77 2021-02-12T23:34:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , 0.00 2021-02-12T23:34:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172898.46 epoch sec; seqNo:5036; 4057335.05 north; 591187.75 east 2021-02-12T23:34:58Z- AHRS_M2 msg: 1613172898.85 epoch sec; seqNo:4989 2021-02-12T23:34:58Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-12T23:34:58Z- handleDepth()-Depth_Keller msg: 1613172898851 epoch sec; seqNo:5037; depth 23.99 2021-02-12T23:34:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172898.87 epoch sec; seqNo:5037; 4057335.05 north; 591187.75 east 2021-02-12T23:34:59Z- AHRS_M2 msg: 1613172899.24 epoch sec; seqNo:4990 2021-02-12T23:34:59Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.19 psi 2021-02-12T23:34:59Z- handleDepth()-Depth_Keller msg: 1613172899249 epoch sec; seqNo:5038; depth 24.10 2021-02-12T23:34:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172899.26 epoch sec; seqNo:5038; 4057335.43 north; 591187.74 east 2021-02-12T23:34:59Z- AHRS_M2 msg: 1613172899.65 epoch sec; seqNo:4991 2021-02-12T23:34:59Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.18 psi 2021-02-12T23:34:59Z- handleDepth()-Depth_Keller msg: 1613172899653 epoch sec; seqNo:5039; depth 24.22 2021-02-12T23:34:59Z- handleDvl() - RDI_Pathfinder msg: 1613172899.66 epoch sec; seqNo:2439 2021-02-12T23:34:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 91.43 , 65.38 , 96.83 2021-02-12T23:34:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.01 2021-02-12T23:34:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172899.67 epoch sec; seqNo:5039; 4057335.82 north; 591188.96 east 2021-02-12T23:35:00Z- AHRS_M2 msg: 1613172900.05 epoch sec; seqNo:4992 2021-02-12T23:35:00Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.18 psi 2021-02-12T23:35:00Z- handleDepth()-Depth_Keller msg: 1613172900057 epoch sec; seqNo:5040; depth 24.34 2021-02-12T23:35:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172900.08 epoch sec; seqNo:5040; 4057336.20 north; 591188.96 east 2021-02-12T23:35:00Z- AHRS_M2 msg: 1613172900.45 epoch sec; seqNo:4993 2021-02-12T23:35:00Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.17 psi 2021-02-12T23:35:00Z- handleDepth()-Depth_Keller msg: 1613172900456 epoch sec; seqNo:5041; depth 24.46 2021-02-12T23:35:00Z- handleDvl() - RDI_Pathfinder msg: 1613172900.47 epoch sec; seqNo:2440 2021-02-12T23:35:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 88.11 , 64.97 , 98.93 2021-02-12T23:35:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , -0.02 2021-02-12T23:35:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172900.49 epoch sec; seqNo:5041; 4057336.20 north; 591188.96 east 2021-02-12T23:35:00Z- AHRS_M2 msg: 1613172900.86 epoch sec; seqNo:4994 2021-02-12T23:35:00Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-12T23:35:00Z- handleDepth()-Depth_Keller msg: 1613172900860 epoch sec; seqNo:5042; depth 24.59 2021-02-12T23:35:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172900.87 epoch sec; seqNo:5042; 4057336.58 north; 591188.95 east 2021-02-12T23:35:01Z- AHRS_M2 msg: 1613172901.26 epoch sec; seqNo:4995 2021-02-12T23:35:01Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-12T23:35:01Z- handleDepth()-Depth_Keller msg: 1613172901270 epoch sec; seqNo:5043; depth 24.70 2021-02-12T23:35:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172901.28 epoch sec; seqNo:5043; 4057336.96 north; 591188.95 east 2021-02-12T23:35:01Z- AHRS_M2 msg: 1613172901.66 epoch sec; seqNo:4996 2021-02-12T23:35:01Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-12T23:35:01Z- handleDepth()-Depth_Keller msg: 1613172901668 epoch sec; seqNo:5044; depth 24.82 2021-02-12T23:35:01Z- handleDvl() - RDI_Pathfinder msg: 1613172901.68 epoch sec; seqNo:2441 2021-02-12T23:35:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 88.18 , 64.97 , 96.83 2021-02-12T23:35:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , -0.04 2021-02-12T23:35:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172901.70 epoch sec; seqNo:5044; 4057337.34 north; 591188.94 east 2021-02-12T23:35:02Z- AHRS_M2 msg: 1613172902.07 epoch sec; seqNo:4997 2021-02-12T23:35:02Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.17 psi 2021-02-12T23:35:02Z- handleDepth()-Depth_Keller msg: 1613172902072 epoch sec; seqNo:5045; depth 24.93 2021-02-12T23:35:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172902.08 epoch sec; seqNo:5045; 4057337.73 north; 591190.16 east 2021-02-12T23:35:02Z- LcmTrn::handleMessages() - handling 41 messages took 0.65 ms and 128 calls... 2021-02-12T23:35:02Z- waiting for 1613172900.24, time is 1613172902.08 2021-02-12T23:35:02Z- LcmTrn::updateTrn() >>>> pose time 1613172902.08, meas time 1613172901.68 <<<< 2021-02-12T23:35:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172901.68 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:35:02Z- TerrainNav::motionUpdate() - currEstimate = (4057338, 591190.16, 24.93) 2021-02-12T23:35:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:35:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:35:02Z- TNavBF::Projecting Measurements 2021-02-12T23:35:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:35:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:35:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:35:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:35:02Z- TNavFilter::4 good beams. 2021-02-12T23:35:02Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:35:02Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:35:02Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:35:02Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:35:02Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:35:02Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:35:02Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:35:02Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:02Z- TerrainMapOctree: (vehN, vehE ) = (4057337.41, 591189.96). 2021-02-12T23:35:02Z- TerrainMapOctree: Min Distance = 4967.11. 2021-02-12T23:35:02Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:35:02Z- There is no specific good beam that all particles have in common. 2021-02-12T23:35:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:35:02Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 7.90 2021-02-12T23:35:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:35:02Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 12.44 2021-02-12T23:35:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:35:02Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 6.75 2021-02-12T23:35:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:35:02Z- Old Depth Bias: 0.00 DepthBiasCov: 0.75 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 15.00 2021-02-12T23:35:02Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:35:02Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:35:02Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:35:02Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:35:02Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:35:02Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:35:02Z- Filter 0 No beams used in subcloud update 2021-02-12T23:35:02Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:35:02Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:35:02Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 752.7 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:35:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172901.68 sec. 2021-02-12T23:35:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:35:02Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:02Z- variances([0], [2], and psi_berg) = 752.630889 833.035524 0.000000 2021-02-12T23:35:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:35:02Z- LcmTrn::publishEstimates() - MLE : 4057323.23 591151.66 24.93 2021-02-12T23:35:02Z- LcmTrn::publishEstimates() - MMSE : 4057337.73 591190.16 24.93 2021-02-12T23:35:02Z- LcmTrn::publishEstimates() - COVAR: 752.63 833.04 0.73 0.00 2021-02-12T23:35:02Z- AHRS_M2 msg: 1613172902.48 epoch sec; seqNo:4998 2021-02-12T23:35:02Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.16 psi 2021-02-12T23:35:02Z- handleDepth()-Depth_Keller msg: 1613172902481 epoch sec; seqNo:5046; depth 25.05 2021-02-12T23:35:02Z- handleDvl() - RDI_Pathfinder msg: 1613172902.49 epoch sec; seqNo:2442 2021-02-12T23:35:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 88.18 , 63.34 , 97.92 2021-02-12T23:35:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , -0.04 2021-02-12T23:35:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172902.50 epoch sec; seqNo:5046; 4057337.73 north; 591190.16 east 2021-02-12T23:35:02Z- AHRS_M2 msg: 1613172902.88 epoch sec; seqNo:4999 2021-02-12T23:35:02Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.16 psi 2021-02-12T23:35:02Z- handleDepth()-Depth_Keller msg: 1613172902880 epoch sec; seqNo:5047; depth 25.17 2021-02-12T23:35:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172902.90 epoch sec; seqNo:5047; 4057338.11 north; 591190.16 east 2021-02-12T23:35:03Z- AHRS_M2 msg: 1613172903.28 epoch sec; seqNo:5000 2021-02-12T23:35:03Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.16 psi 2021-02-12T23:35:03Z- handleDepth()-Depth_Keller msg: 1613172903287 epoch sec; seqNo:5048; depth 25.29 2021-02-12T23:35:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172903.32 epoch sec; seqNo:5048; 4057338.49 north; 591190.15 east 2021-02-12T23:35:03Z- AHRS_M2 msg: 1613172903.68 epoch sec; seqNo:5001 2021-02-12T23:35:03Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.16 psi 2021-02-12T23:35:03Z- handleDepth()-Depth_Keller msg: 1613172903688 epoch sec; seqNo:5049; depth 25.42 2021-02-12T23:35:03Z- handleDvl() - RDI_Pathfinder msg: 1613172903.70 epoch sec; seqNo:2443 2021-02-12T23:35:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 88.18 , 64.16 , 96.83 2021-02-12T23:35:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , -0.03 2021-02-12T23:35:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172903.71 epoch sec; seqNo:5049; 4057338.87 north; 591190.15 east 2021-02-12T23:35:04Z- AHRS_M2 msg: 1613172904.09 epoch sec; seqNo:5002 2021-02-12T23:35:04Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.16 psi 2021-02-12T23:35:04Z- handleDepth()-Depth_Keller msg: 1613172904096 epoch sec; seqNo:5050; depth 25.54 2021-02-12T23:35:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172904.11 epoch sec; seqNo:5050; 4057338.87 north; 591190.15 east 2021-02-12T23:35:04Z- AHRS_M2 msg: 1613172904.49 epoch sec; seqNo:5003 2021-02-12T23:35:04Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.16 psi 2021-02-12T23:35:04Z- handleDepth()-Depth_Keller msg: 1613172904496 epoch sec; seqNo:5051; depth 25.66 2021-02-12T23:35:04Z- handleDvl() - RDI_Pathfinder msg: 1613172904.51 epoch sec; seqNo:2444 2021-02-12T23:35:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 87.17 , 62.52 , 95.82 2021-02-12T23:35:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , 0.00 2021-02-12T23:35:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172904.52 epoch sec; seqNo:5051; 4057339.25 north; 591190.15 east 2021-02-12T23:35:04Z- AHRS_M2 msg: 1613172904.90 epoch sec; seqNo:5004 2021-02-12T23:35:04Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.17 psi 2021-02-12T23:35:04Z- handleDepth()-Depth_Keller msg: 1613172904905 epoch sec; seqNo:5052; depth 25.78 2021-02-12T23:35:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172904.92 epoch sec; seqNo:5052; 4057339.64 north; 591191.36 east 2021-02-12T23:35:05Z- AHRS_M2 msg: 1613172905.31 epoch sec; seqNo:5005 2021-02-12T23:35:05Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.17 psi 2021-02-12T23:35:05Z- handleDepth()-Depth_Keller msg: 1613172905323 epoch sec; seqNo:5053; depth 25.89 2021-02-12T23:35:05Z- handleDvl() - RDI_Pathfinder msg: 1613172905.34 epoch sec; seqNo:2445 2021-02-12T23:35:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 88.25 , 63.34 , 95.82 2021-02-12T23:35:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , 0.04 2021-02-12T23:35:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172905.36 epoch sec; seqNo:5053; 4057340.02 north; 591191.36 east 2021-02-12T23:35:05Z- AHRS_M2 msg: 1613172905.70 epoch sec; seqNo:5006 2021-02-12T23:35:05Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.17 psi 2021-02-12T23:35:05Z- handleDepth()-Depth_Keller msg: 1613172905708 epoch sec; seqNo:5054; depth 26.01 2021-02-12T23:35:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172905.72 epoch sec; seqNo:5054; 4057340.02 north; 591191.36 east 2021-02-12T23:35:06Z- AHRS_M2 msg: 1613172906.11 epoch sec; seqNo:5007 2021-02-12T23:35:06Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.17 psi 2021-02-12T23:35:06Z- handleDepth()-Depth_Keller msg: 1613172906113 epoch sec; seqNo:5055; depth 26.13 2021-02-12T23:35:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172906.12 epoch sec; seqNo:5055; 4057340.40 north; 591191.35 east 2021-02-12T23:35:06Z- AHRS_M2 msg: 1613172906.51 epoch sec; seqNo:5008 2021-02-12T23:35:06Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-12T23:35:06Z- handleDepth()-Depth_Keller msg: 1613172906516 epoch sec; seqNo:5056; depth 26.25 2021-02-12T23:35:06Z- handleDvl() - RDI_Pathfinder msg: 1613172906.53 epoch sec; seqNo:2446 2021-02-12T23:35:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 0.00 , 64.16 , 96.90 2021-02-12T23:35:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.06 2021-02-12T23:35:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172906.54 epoch sec; seqNo:5056; 4057340.78 north; 591191.35 east 2021-02-12T23:35:06Z- AHRS_M2 msg: 1613172906.91 epoch sec; seqNo:5009 2021-02-12T23:35:06Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-12T23:35:06Z- handleDepth()-Depth_Keller msg: 1613172906923 epoch sec; seqNo:5057; depth 26.38 2021-02-12T23:35:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172906.93 epoch sec; seqNo:5057; 4057340.78 north; 591191.35 east 2021-02-12T23:35:07Z- AHRS_M2 msg: 1613172907.34 epoch sec; seqNo:5010 2021-02-12T23:35:07Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-12T23:35:07Z- handleDepth()-Depth_Keller msg: 1613172907343 epoch sec; seqNo:5058; depth 26.48 2021-02-12T23:35:07Z- handleDvl() - RDI_Pathfinder msg: 1613172907.36 epoch sec; seqNo:2447 2021-02-12T23:35:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 87.17 , 63.34 , 94.74 2021-02-12T23:35:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.07 2021-02-12T23:35:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172907.37 epoch sec; seqNo:5058; 4057341.17 north; 591192.57 east 2021-02-12T23:35:07Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-12T23:35:07Z- waiting for 1613172905.08, time is 1613172907.37 2021-02-12T23:35:07Z- LcmTrn::updateTrn() equating pose 1613172907.37 and meas times 1613172907.37 2021-02-12T23:35:07Z- LcmTrn::updateTrn() >>>> pose time 1613172907.37, meas time 1613172907.37 <<<< 2021-02-12T23:35:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172907.37 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:35:07Z- TerrainNav::motionUpdate() - currEstimate = (4057341, 591192.57, 26.48) 2021-02-12T23:35:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:35:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:35:07Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172907.37 sec.; No relevant navigation data available 2021-02-12T23:35:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:35:07Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:07Z- variances([0], [2], and psi_berg) = 752.527650 833.035425 0.000000 2021-02-12T23:35:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:35:07Z- LcmTrn::publishEstimates() - MLE : 4057326.67 591154.07 26.48 2021-02-12T23:35:07Z- LcmTrn::publishEstimates() - MMSE : 4057341.17 591192.57 26.48 2021-02-12T23:35:07Z- LcmTrn::publishEstimates() - COVAR: 752.53 833.04 0.95 0.00 2021-02-12T23:35:07Z- AHRS_M2 msg: 1613172907.72 epoch sec; seqNo:5011 2021-02-12T23:35:07Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-12T23:35:07Z- handleDepth()-Depth_Keller msg: 1613172907728 epoch sec; seqNo:5059; depth 26.62 2021-02-12T23:35:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172907.74 epoch sec; seqNo:5059; 4057341.55 north; 591192.56 east 2021-02-12T23:35:08Z- AHRS_M2 msg: 1613172908.13 epoch sec; seqNo:5012 2021-02-12T23:35:08Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-12T23:35:08Z- handleDepth()-Depth_Keller msg: 1613172908135 epoch sec; seqNo:5060; depth 26.72 2021-02-12T23:35:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172908.15 epoch sec; seqNo:5060; 4057341.92 north; 591192.56 east 2021-02-12T23:35:08Z- AHRS_M2 msg: 1613172908.53 epoch sec; seqNo:5013 2021-02-12T23:35:08Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-12T23:35:08Z- handleDepth()-Depth_Keller msg: 1613172908538 epoch sec; seqNo:5061; depth 26.84 2021-02-12T23:35:08Z- handleDvl() - RDI_Pathfinder msg: 1613172908.55 epoch sec; seqNo:2448 2021-02-12T23:35:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 86.09 , 62.12 , 97.98 2021-02-12T23:35:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , 0.04 2021-02-12T23:35:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172908.56 epoch sec; seqNo:5061; 4057341.92 north; 591192.56 east 2021-02-12T23:35:08Z- AHRS_M2 msg: 1613172908.93 epoch sec; seqNo:5014 2021-02-12T23:35:08Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-12T23:35:08Z- handleDepth()-Depth_Keller msg: 1613172908941 epoch sec; seqNo:5062; depth 26.98 2021-02-12T23:35:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172908.96 epoch sec; seqNo:5062; 4057342.30 north; 591192.56 east 2021-02-12T23:35:09Z- AHRS_M2 msg: 1613172909.35 epoch sec; seqNo:5015 2021-02-12T23:35:09Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.17 psi 2021-02-12T23:35:09Z- handleDepth()-Depth_Keller msg: 1613172909350 epoch sec; seqNo:5063; depth 27.09 2021-02-12T23:35:09Z- handleDvl() - RDI_Pathfinder msg: 1613172909.36 epoch sec; seqNo:2449 2021-02-12T23:35:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 86.09 , 61.30 , 96.90 2021-02-12T23:35:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , 0.01 2021-02-12T23:35:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172909.39 epoch sec; seqNo:5063; 4057342.68 north; 591192.55 east 2021-02-12T23:35:09Z- AHRS_M2 msg: 1613172909.74 epoch sec; seqNo:5016 2021-02-12T23:35:09Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.17 psi 2021-02-12T23:35:09Z- handleDepth()-Depth_Keller msg: 1613172909748 epoch sec; seqNo:5064; depth 27.22 2021-02-12T23:35:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172909.76 epoch sec; seqNo:5064; 4057342.70 north; 591193.77 east 2021-02-12T23:35:10Z- AHRS_M2 msg: 1613172910.15 epoch sec; seqNo:5017 2021-02-12T23:35:10Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.17 psi 2021-02-12T23:35:10Z- handleDepth()-Depth_Keller msg: 1613172910158 epoch sec; seqNo:5065; depth 27.32 2021-02-12T23:35:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172910.17 epoch sec; seqNo:5065; 4057343.07 north; 591193.77 east 2021-02-12T23:35:10Z- AHRS_M2 msg: 1613172910.55 epoch sec; seqNo:5018 2021-02-12T23:35:10Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.17 psi 2021-02-12T23:35:10Z- handleDepth()-Depth_Keller msg: 1613172910556 epoch sec; seqNo:5066; depth 27.46 2021-02-12T23:35:10Z- handleDvl() - RDI_Pathfinder msg: 1613172910.58 epoch sec; seqNo:2450 2021-02-12T23:35:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 87.17 , 61.71 , 96.90 2021-02-12T23:35:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , -0.01 2021-02-12T23:35:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172910.59 epoch sec; seqNo:5066; 4057343.45 north; 591193.76 east 2021-02-12T23:35:10Z- AHRS_M2 msg: 1613172910.95 epoch sec; seqNo:5019 2021-02-12T23:35:10Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-12T23:35:10Z- handleDepth()-Depth_Keller msg: 1613172910965 epoch sec; seqNo:5067; depth 27.58 2021-02-12T23:35:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172910.98 epoch sec; seqNo:5067; 4057343.83 north; 591193.76 east 2021-02-12T23:35:11Z- AHRS_M2 msg: 1613172911.36 epoch sec; seqNo:5020 2021-02-12T23:35:11Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.18 psi 2021-02-12T23:35:11Z- handleDepth()-Depth_Keller msg: 1613172911369 epoch sec; seqNo:5068; depth 27.72 2021-02-12T23:35:11Z- handleDvl() - RDI_Pathfinder msg: 1613172911.38 epoch sec; seqNo:2451 2021-02-12T23:35:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 86.09 , 61.71 , 97.98 2021-02-12T23:35:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.03 , -0.04 2021-02-12T23:35:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172911.39 epoch sec; seqNo:5068; 4057343.83 north; 591193.76 east 2021-02-12T23:35:11Z- AHRS_M2 msg: 1613172911.77 epoch sec; seqNo:5021 2021-02-12T23:35:11Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.18 psi 2021-02-12T23:35:11Z- handleDepth()-Depth_Keller msg: 1613172911770 epoch sec; seqNo:5069; depth 27.83 2021-02-12T23:35:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172911.78 epoch sec; seqNo:5069; 4057344.21 north; 591193.76 east 2021-02-12T23:35:12Z- AHRS_M2 msg: 1613172912.17 epoch sec; seqNo:5022 2021-02-12T23:35:12Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.18 psi 2021-02-12T23:35:12Z- handleDepth()-Depth_Keller msg: 1613172912172 epoch sec; seqNo:5070; depth 27.96 2021-02-12T23:35:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172912.19 epoch sec; seqNo:5070; 4057344.60 north; 591194.97 east 2021-02-12T23:35:12Z- LcmTrn::handleMessages() - handling 40 messages took 0.65 ms and 127 calls... 2021-02-12T23:35:12Z- waiting for 1613172910.37, time is 1613172912.19 2021-02-12T23:35:12Z- LcmTrn::updateTrn() >>>> pose time 1613172912.19, meas time 1613172911.38 <<<< 2021-02-12T23:35:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172911.38 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:35:12Z- TerrainNav::motionUpdate() - currEstimate = (4057345, 591194.97, 27.96) 2021-02-12T23:35:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:35:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:35:12Z- TNavBF::Projecting Measurements 2021-02-12T23:35:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:35:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:35:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:35:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:35:12Z- TNavFilter::4 good beams. 2021-02-12T23:35:12Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:35:12Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:35:12Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:35:12Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:35:12Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:35:12Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:35:12Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:35:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:12Z- TerrainMapOctree: (vehN, vehE ) = (4057344.03, 591194.57). 2021-02-12T23:35:12Z- TerrainMapOctree: Min Distance = 4959.06. 2021-02-12T23:35:12Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:35:12Z- There is no specific good beam that all particles have in common. 2021-02-12T23:35:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:35:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.110 ExpectedMeasDiff: nan currMeas.covariance: 7.36 2021-02-12T23:35:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:35:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 11.86 2021-02-12T23:35:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:35:12Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 6.09 2021-02-12T23:35:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:35:12Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-12T23:35:12Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:35:12Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:35:12Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:35:12Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:35:12Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:35:12Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:35:12Z- Filter 0 No beams used in subcloud update 2021-02-12T23:35:12Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:35:12Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:35:12Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 752.4 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:35:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172911.38 sec. 2021-02-12T23:35:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:35:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:12Z- variances([0], [2], and psi_berg) = 752.391450 833.035294 0.000000 2021-02-12T23:35:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:35:12Z- LcmTrn::publishEstimates() - MLE : 4057330.10 591156.47 27.96 2021-02-12T23:35:12Z- LcmTrn::publishEstimates() - MMSE : 4057344.60 591194.97 27.96 2021-02-12T23:35:12Z- LcmTrn::publishEstimates() - COVAR: 752.39 833.04 0.83 0.00 2021-02-12T23:35:12Z- AHRS_M2 msg: 1613172912.58 epoch sec; seqNo:5023 2021-02-12T23:35:12Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.18 psi 2021-02-12T23:35:12Z- handleDepth()-Depth_Keller msg: 1613172912614 epoch sec; seqNo:5071; depth 28.07 2021-02-12T23:35:12Z- handleDvl() - RDI_Pathfinder msg: 1613172912.64 epoch sec; seqNo:2452 2021-02-12T23:35:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 86.09 , 60.89 , 97.98 2021-02-12T23:35:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , -0.06 2021-02-12T23:35:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172912.66 epoch sec; seqNo:5071; 4057344.98 north; 591194.97 east 2021-02-12T23:35:12Z- AHRS_M2 msg: 1613172912.97 epoch sec; seqNo:5024 2021-02-12T23:35:12Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.18 psi 2021-02-12T23:35:12Z- handleDepth()-Depth_Keller msg: 1613172912980 epoch sec; seqNo:5072; depth 28.23 2021-02-12T23:35:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172912.99 epoch sec; seqNo:5072; 4057344.98 north; 591194.97 east 2021-02-12T23:35:13Z- AHRS_M2 msg: 1613172913.39 epoch sec; seqNo:5025 2021-02-12T23:35:13Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.18 psi 2021-02-12T23:35:13Z- handleDepth()-Depth_Keller msg: 1613172913391 epoch sec; seqNo:5073; depth 28.34 2021-02-12T23:35:13Z- handleDvl() - RDI_Pathfinder msg: 1613172913.40 epoch sec; seqNo:2453 2021-02-12T23:35:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 86.09 , 60.89 , 94.74 2021-02-12T23:35:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , -0.06 2021-02-12T23:35:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172913.41 epoch sec; seqNo:5073; 4057345.36 north; 591194.96 east 2021-02-12T23:35:13Z- AHRS_M2 msg: 1613172913.79 epoch sec; seqNo:5026 2021-02-12T23:35:13Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.19 psi 2021-02-12T23:35:13Z- handleDepth()-Depth_Keller msg: 1613172913793 epoch sec; seqNo:5074; depth 28.46 2021-02-12T23:35:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172913.81 epoch sec; seqNo:5074; 4057345.74 north; 591194.96 east 2021-02-12T23:35:14Z- AHRS_M2 msg: 1613172914.19 epoch sec; seqNo:5027 2021-02-12T23:35:14Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.19 psi 2021-02-12T23:35:14Z- handleDepth()-Depth_Keller msg: 1613172914192 epoch sec; seqNo:5075; depth 28.59 2021-02-12T23:35:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172914.20 epoch sec; seqNo:5075; 4057346.13 north; 591196.18 east 2021-02-12T23:35:14Z- AHRS_M2 msg: 1613172914.60 epoch sec; seqNo:5028 2021-02-12T23:35:14Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.19 psi 2021-02-12T23:35:14Z- handleDepth()-Depth_Keller msg: 1613172914613 epoch sec; seqNo:5076; depth 28.72 2021-02-12T23:35:14Z- handleDvl() - RDI_Pathfinder msg: 1613172914.62 epoch sec; seqNo:2454 2021-02-12T23:35:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.02 , 0.00 , 60.89 , 97.98 2021-02-12T23:35:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.06 2021-02-12T23:35:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172914.63 epoch sec; seqNo:5076; 4057346.13 north; 591196.18 east 2021-02-12T23:35:15Z- AHRS_M2 msg: 1613172915.02 epoch sec; seqNo:5029 2021-02-12T23:35:15Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.20 psi 2021-02-12T23:35:15Z- handleDepth()-Depth_Keller msg: 1613172915022 epoch sec; seqNo:5077; depth 28.83 2021-02-12T23:35:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172915.04 epoch sec; seqNo:5077; 4057346.51 north; 591196.17 east 2021-02-12T23:35:15Z- AHRS_M2 msg: 1613172915.40 epoch sec; seqNo:5030 2021-02-12T23:35:15Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.20 psi 2021-02-12T23:35:15Z- handleDepth()-Depth_Keller msg: 1613172915404 epoch sec; seqNo:5078; depth 28.97 2021-02-12T23:35:15Z- handleDvl() - RDI_Pathfinder msg: 1613172915.42 epoch sec; seqNo:2455 2021-02-12T23:35:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.20 , 86.09 , 60.48 , 94.74 2021-02-12T23:35:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , -0.02 2021-02-12T23:35:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172915.44 epoch sec; seqNo:5078; 4057346.89 north; 591196.17 east 2021-02-12T23:35:15Z- AHRS_M2 msg: 1613172915.80 epoch sec; seqNo:5031 2021-02-12T23:35:15Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.20 psi 2021-02-12T23:35:15Z- handleDepth()-Depth_Keller msg: 1613172915808 epoch sec; seqNo:5079; depth 29.09 2021-02-12T23:35:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172915.82 epoch sec; seqNo:5079; 4057347.27 north; 591196.17 east 2021-02-12T23:35:16Z- AHRS_M2 msg: 1613172916.21 epoch sec; seqNo:5032 2021-02-12T23:35:16Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.20 psi 2021-02-12T23:35:16Z- handleDepth()-Depth_Keller msg: 1613172916213 epoch sec; seqNo:5080; depth 29.22 2021-02-12T23:35:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172916.22 epoch sec; seqNo:5080; 4057347.27 north; 591196.17 east 2021-02-12T23:35:16Z- AHRS_M2 msg: 1613172916.61 epoch sec; seqNo:5033 2021-02-12T23:35:16Z- AHRS_M2 msg: 0.63 phi; -0.28 theta; -0.20 psi 2021-02-12T23:35:16Z- handleDepth()-Depth_Keller msg: 1613172916616 epoch sec; seqNo:5081; depth 29.34 2021-02-12T23:35:16Z- handleDvl() - RDI_Pathfinder msg: 1613172916.64 epoch sec; seqNo:2456 2021-02-12T23:35:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.97 , 87.17 , 60.89 , 94.74 2021-02-12T23:35:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , 0.01 2021-02-12T23:35:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172916.65 epoch sec; seqNo:5081; 4057347.65 north; 591196.16 east 2021-02-12T23:35:17Z- AHRS_M2 msg: 1613172917.02 epoch sec; seqNo:5034 2021-02-12T23:35:17Z- AHRS_M2 msg: 0.62 phi; -0.24 theta; -0.19 psi 2021-02-12T23:35:17Z- handleDepth()-Depth_Keller msg: 1613172917025 epoch sec; seqNo:5082; depth 29.50 2021-02-12T23:35:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172917.04 epoch sec; seqNo:5082; 4057348.04 north; 591197.38 east 2021-02-12T23:35:17Z- AHRS_M2 msg: 1613172917.42 epoch sec; seqNo:5035 2021-02-12T23:35:17Z- AHRS_M2 msg: 0.61 phi; -0.20 theta; -0.18 psi 2021-02-12T23:35:17Z- handleDepth()-Depth_Keller msg: 1613172917424 epoch sec; seqNo:5083; depth 29.64 2021-02-12T23:35:17Z- handleDvl() - RDI_Pathfinder msg: 1613172917.43 epoch sec; seqNo:2457 2021-02-12T23:35:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.75 , 82.84 , 61.30 , 86.09 2021-02-12T23:35:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.04 2021-02-12T23:35:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172917.45 epoch sec; seqNo:5083; 4057348.42 north; 591197.37 east 2021-02-12T23:35:17Z- LcmTrn::handleMessages() - handling 45 messages took 0.67 ms and 132 calls... 2021-02-12T23:35:17Z- waiting for 1613172915.19, time is 1613172917.45 2021-02-12T23:35:17Z- LcmTrn::updateTrn() equating pose 1613172917.45 and meas times 1613172917.45 2021-02-12T23:35:17Z- LcmTrn::updateTrn() >>>> pose time 1613172917.45, meas time 1613172917.45 <<<< 2021-02-12T23:35:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172917.45 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:35:17Z- TerrainNav::motionUpdate() - currEstimate = (4057348, 591197.37, 29.64) 2021-02-12T23:35:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:35:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:35:17Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172917.45 sec.; No relevant navigation data available 2021-02-12T23:35:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:35:17Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:17Z- variances([0], [2], and psi_berg) = 752.284768 833.035192 0.000000 2021-02-12T23:35:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:35:17Z- LcmTrn::publishEstimates() - MLE : 4057333.92 591154.87 29.64 2021-02-12T23:35:17Z- LcmTrn::publishEstimates() - MMSE : 4057348.42 591197.37 29.64 2021-02-12T23:35:17Z- LcmTrn::publishEstimates() - COVAR: 752.28 833.04 1.06 0.00 2021-02-12T23:35:17Z- AHRS_M2 msg: 1613172917.83 epoch sec; seqNo:5036 2021-02-12T23:35:17Z- AHRS_M2 msg: 0.60 phi; -0.16 theta; -0.17 psi 2021-02-12T23:35:17Z- handleDepth()-Depth_Keller msg: 1613172917829 epoch sec; seqNo:5084; depth 29.76 2021-02-12T23:35:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172917.84 epoch sec; seqNo:5084; 4057348.42 north; 591197.37 east 2021-02-12T23:35:18Z- AHRS_M2 msg: 1613172918.26 epoch sec; seqNo:5037 2021-02-12T23:35:18Z- AHRS_M2 msg: 0.59 phi; -0.12 theta; -0.16 psi 2021-02-12T23:35:18Z- handleDepth()-Depth_Keller msg: 1613172918266 epoch sec; seqNo:5085; depth 29.87 2021-02-12T23:35:18Z- handleDvl() - RDI_Pathfinder msg: 1613172918.28 epoch sec; seqNo:2458 2021-02-12T23:35:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.93 , 75.19 , 62.12 , 75.19 2021-02-12T23:35:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.04 2021-02-12T23:35:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172918.30 epoch sec; seqNo:5085; 4057348.80 north; 591197.37 east 2021-02-12T23:35:18Z- AHRS_M2 msg: 1613172918.63 epoch sec; seqNo:5038 2021-02-12T23:35:18Z- AHRS_M2 msg: 0.58 phi; -0.08 theta; -0.15 psi 2021-02-12T23:35:18Z- handleDepth()-Depth_Keller msg: 1613172918636 epoch sec; seqNo:5086; depth 30.01 2021-02-12T23:35:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172918.65 epoch sec; seqNo:5086; 4057349.17 north; 591197.37 east 2021-02-12T23:35:19Z- AHRS_M2 msg: 1613172919.04 epoch sec; seqNo:5039 2021-02-12T23:35:19Z- AHRS_M2 msg: 0.57 phi; -0.03 theta; -0.15 psi 2021-02-12T23:35:19Z- handleDepth()-Depth_Keller msg: 1613172919040 epoch sec; seqNo:5087; depth 30.08 2021-02-12T23:35:19Z- handleDvl() - RDI_Pathfinder msg: 1613172919.09 epoch sec; seqNo:2459 2021-02-12T23:35:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.11 , 73.14 , 65.70 , 69.58 2021-02-12T23:35:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.03 2021-02-12T23:35:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172919.10 epoch sec; seqNo:5087; 4057349.55 north; 591197.36 east 2021-02-12T23:35:19Z- AHRS_M2 msg: 1613172919.44 epoch sec; seqNo:5040 2021-02-12T23:35:19Z- AHRS_M2 msg: 0.56 phi; 0.01 theta; -0.15 psi 2021-02-12T23:35:19Z- handleDepth()-Depth_Keller msg: 1613172919447 epoch sec; seqNo:5088; depth 30.16 2021-02-12T23:35:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172919.46 epoch sec; seqNo:5088; 4057349.93 north; 591197.36 east 2021-02-12T23:35:19Z- AHRS_M2 msg: 1613172919.84 epoch sec; seqNo:5041 2021-02-12T23:35:19Z- AHRS_M2 msg: 0.56 phi; 0.05 theta; -0.14 psi 2021-02-12T23:35:19Z- handleDepth()-Depth_Keller msg: 1613172919848 epoch sec; seqNo:5089; depth 30.21 2021-02-12T23:35:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172919.87 epoch sec; seqNo:5089; 4057349.95 north; 591198.58 east 2021-02-12T23:35:20Z- AHRS_M2 msg: 1613172920.25 epoch sec; seqNo:5042 2021-02-12T23:35:20Z- AHRS_M2 msg: 0.55 phi; 0.09 theta; -0.14 psi 2021-02-12T23:35:20Z- handleDepth()-Depth_Keller msg: 1613172920252 epoch sec; seqNo:5090; depth 30.27 2021-02-12T23:35:20Z- handleDvl() - RDI_Pathfinder msg: 1613172920.26 epoch sec; seqNo:2460 2021-02-12T23:35:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.44 , 0.00 , 69.58 , 65.70 2021-02-12T23:35:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.00 , 0.02 2021-02-12T23:35:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172920.27 epoch sec; seqNo:5090; 4057350.32 north; 591198.58 east 2021-02-12T23:35:20Z- AHRS_M2 msg: 1613172920.66 epoch sec; seqNo:5043 2021-02-12T23:35:20Z- AHRS_M2 msg: 0.54 phi; 0.12 theta; -0.14 psi 2021-02-12T23:35:20Z- handleDepth()-Depth_Keller msg: 1613172920662 epoch sec; seqNo:5091; depth 30.29 2021-02-12T23:35:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172920.67 epoch sec; seqNo:5091; 4057350.70 north; 591198.57 east 2021-02-12T23:35:21Z- AHRS_M2 msg: 1613172921.06 epoch sec; seqNo:5044 2021-02-12T23:35:21Z- AHRS_M2 msg: 0.54 phi; 0.15 theta; -0.14 psi 2021-02-12T23:35:21Z- handleDepth()-Depth_Keller msg: 1613172921060 epoch sec; seqNo:5092; depth 30.31 2021-02-12T23:35:21Z- handleDvl() - RDI_Pathfinder msg: 1613172921.08 epoch sec; seqNo:2461 2021-02-12T23:35:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.08 , 0.00 , 74.20 , 63.47 2021-02-12T23:35:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , 0.00 2021-02-12T23:35:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172921.09 epoch sec; seqNo:5092; 4057351.08 north; 591198.57 east 2021-02-12T23:35:21Z- AHRS_M2 msg: 1613172921.46 epoch sec; seqNo:5045 2021-02-12T23:35:21Z- AHRS_M2 msg: 0.53 phi; 0.18 theta; -0.14 psi 2021-02-12T23:35:21Z- handleDepth()-Depth_Keller msg: 1613172921464 epoch sec; seqNo:5093; depth 30.31 2021-02-12T23:35:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172921.47 epoch sec; seqNo:5093; 4057351.08 north; 591198.57 east 2021-02-12T23:35:21Z- AHRS_M2 msg: 1613172921.86 epoch sec; seqNo:5046 2021-02-12T23:35:21Z- AHRS_M2 msg: 0.53 phi; 0.21 theta; -0.14 psi 2021-02-12T23:35:21Z- handleDepth()-Depth_Keller msg: 1613172921868 epoch sec; seqNo:5094; depth 30.29 2021-02-12T23:35:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172921.88 epoch sec; seqNo:5094; 4057351.46 north; 591198.56 east 2021-02-12T23:35:22Z- AHRS_M2 msg: 1613172922.27 epoch sec; seqNo:5047 2021-02-12T23:35:22Z- AHRS_M2 msg: 0.53 phi; 0.23 theta; -0.13 psi 2021-02-12T23:35:22Z- handleDepth()-Depth_Keller msg: 1613172922277 epoch sec; seqNo:5095; depth 30.28 2021-02-12T23:35:22Z- handleDvl() - RDI_Pathfinder msg: 1613172922.30 epoch sec; seqNo:2462 2021-02-12T23:35:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.38 , 74.12 , 0.00 , 62.14 2021-02-12T23:35:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.03 , -0.03 2021-02-12T23:35:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172922.31 epoch sec; seqNo:5095; 4057351.85 north; 591199.78 east 2021-02-12T23:35:22Z- LcmTrn::handleMessages() - handling 41 messages took 0.69 ms and 127 calls... 2021-02-12T23:35:22Z- waiting for 1613172920.45, time is 1613172922.31 2021-02-12T23:35:22Z- LcmTrn::updateTrn() equating pose 1613172922.31 and meas times 1613172922.31 2021-02-12T23:35:22Z- LcmTrn::updateTrn() >>>> pose time 1613172922.31, meas time 1613172922.31 <<<< 2021-02-12T23:35:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172922.31 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:35:22Z- TerrainNav::motionUpdate() - currEstimate = (4057352, 591199.78, 30.28) 2021-02-12T23:35:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:35:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:35:22Z- TNavBF::Projecting Measurements 2021-02-12T23:35:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:35:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:35:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:35:22Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:35:22Z- TNavFilter::3 good beams. 2021-02-12T23:35:22Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:35:22Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:35:22Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:35:22Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:35:22Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:35:22Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:35:22Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:35:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:22Z- TerrainMapOctree: (vehN, vehE ) = (4057351.85, 591199.78). 2021-02-12T23:35:22Z- TerrainMapOctree: Min Distance = 4949.69. 2021-02-12T23:35:22Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:35:22Z- There is no specific good beam that all particles have in common. 2021-02-12T23:35:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:35:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.129 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-12T23:35:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:35:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 8.79 2021-02-12T23:35:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:35:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 6.18 2021-02-12T23:35:22Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:35:22Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:35:22Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:35:22Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:35:22Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:35:22Z- Filter 0 No beams used in subcloud update 2021-02-12T23:35:22Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:35:22Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:35:22Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 752.2 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:35:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172922.31 sec. 2021-02-12T23:35:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:35:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:22Z- variances([0], [2], and psi_berg) = 752.182010 833.035094 0.000000 2021-02-12T23:35:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:35:22Z- LcmTrn::publishEstimates() - MLE : 4057337.35 591157.28 30.28 2021-02-12T23:35:22Z- LcmTrn::publishEstimates() - MMSE : 4057351.85 591199.78 30.28 2021-02-12T23:35:22Z- LcmTrn::publishEstimates() - COVAR: 752.18 833.04 0.89 0.00 2021-02-12T23:35:22Z- AHRS_M2 msg: 1613172922.67 epoch sec; seqNo:5048 2021-02-12T23:35:22Z- AHRS_M2 msg: 0.53 phi; 0.26 theta; -0.13 psi 2021-02-12T23:35:22Z- handleDepth()-Depth_Keller msg: 1613172922676 epoch sec; seqNo:5096; depth 30.24 2021-02-12T23:35:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172922.69 epoch sec; seqNo:5096; 4057352.23 north; 591199.78 east 2021-02-12T23:35:23Z- AHRS_M2 msg: 1613172923.08 epoch sec; seqNo:5049 2021-02-12T23:35:23Z- AHRS_M2 msg: 0.54 phi; 0.28 theta; -0.12 psi 2021-02-12T23:35:23Z- handleDepth()-Depth_Keller msg: 1613172923079 epoch sec; seqNo:5097; depth 30.20 2021-02-12T23:35:23Z- handleDvl() - RDI_Pathfinder msg: 1613172923.09 epoch sec; seqNo:2463 2021-02-12T23:35:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.54 , 74.51 , 0.00 , 60.40 2021-02-12T23:35:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.04 , -0.07 2021-02-12T23:35:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172923.10 epoch sec; seqNo:5097; 4057352.23 north; 591199.78 east 2021-02-12T23:35:23Z- AHRS_M2 msg: 1613172923.48 epoch sec; seqNo:5050 2021-02-12T23:35:23Z- AHRS_M2 msg: 0.54 phi; 0.29 theta; -0.12 psi 2021-02-12T23:35:23Z- handleDepth()-Depth_Keller msg: 1613172923486 epoch sec; seqNo:5098; depth 30.14 2021-02-12T23:35:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172923.50 epoch sec; seqNo:5098; 4057352.61 north; 591199.77 east 2021-02-12T23:35:23Z- AHRS_M2 msg: 1613172923.91 epoch sec; seqNo:5051 2021-02-12T23:35:23Z- AHRS_M2 msg: 0.54 phi; 0.31 theta; -0.11 psi 2021-02-12T23:35:23Z- handleDepth()-Depth_Keller msg: 1613172923917 epoch sec; seqNo:5099; depth 30.07 2021-02-12T23:35:23Z- handleDvl() - RDI_Pathfinder msg: 1613172923.93 epoch sec; seqNo:2464 2021-02-12T23:35:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 68.07 , 0.00 , 0.00 , 60.10 2021-02-12T23:35:23Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:35:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172923.94 epoch sec; seqNo:5099; 4057352.99 north; 591199.77 east 2021-02-12T23:35:24Z- AHRS_M2 msg: 1613172924.30 epoch sec; seqNo:5052 2021-02-12T23:35:24Z- AHRS_M2 msg: 0.55 phi; 0.32 theta; -0.11 psi 2021-02-12T23:35:24Z- handleDepth()-Depth_Keller msg: 1613172924301 epoch sec; seqNo:5100; depth 29.99 2021-02-12T23:35:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172924.32 epoch sec; seqNo:5100; 4057353.37 north; 591199.76 east 2021-02-12T23:35:24Z- AHRS_M2 msg: 1613172924.70 epoch sec; seqNo:5053 2021-02-12T23:35:24Z- AHRS_M2 msg: 0.55 phi; 0.33 theta; -0.11 psi 2021-02-12T23:35:24Z- handleDepth()-Depth_Keller msg: 1613172924704 epoch sec; seqNo:5101; depth 29.93 2021-02-12T23:35:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172924.71 epoch sec; seqNo:5101; 4057353.37 north; 591199.76 east 2021-02-12T23:35:25Z- AHRS_M2 msg: 1613172925.10 epoch sec; seqNo:5054 2021-02-12T23:35:25Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.10 psi 2021-02-12T23:35:25Z- handleDepth()-Depth_Keller msg: 1613172925108 epoch sec; seqNo:5102; depth 29.84 2021-02-12T23:35:25Z- handleDvl() - RDI_Pathfinder msg: 1613172925.12 epoch sec; seqNo:2465 2021-02-12T23:35:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.99 , 75.80 , 0.00 , 59.62 2021-02-12T23:35:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.01 , -0.10 2021-02-12T23:35:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172925.14 epoch sec; seqNo:5102; 4057353.76 north; 591200.98 east 2021-02-12T23:35:25Z- AHRS_M2 msg: 1613172925.52 epoch sec; seqNo:5055 2021-02-12T23:35:25Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.10 psi 2021-02-12T23:35:25Z- handleDepth()-Depth_Keller msg: 1613172925519 epoch sec; seqNo:5103; depth 29.74 2021-02-12T23:35:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172925.53 epoch sec; seqNo:5103; 4057354.14 north; 591200.98 east 2021-02-12T23:35:25Z- AHRS_M2 msg: 1613172925.91 epoch sec; seqNo:5056 2021-02-12T23:35:25Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.10 psi 2021-02-12T23:35:25Z- handleDepth()-Depth_Keller msg: 1613172925941 epoch sec; seqNo:5104; depth 29.63 2021-02-12T23:35:25Z- handleDvl() - RDI_Pathfinder msg: 1613172925.96 epoch sec; seqNo:2466 2021-02-12T23:35:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.58 , 76.06 , 0.00 , 60.20 2021-02-12T23:35:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.01 , -0.08 2021-02-12T23:35:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172925.97 epoch sec; seqNo:5104; 4057354.14 north; 591200.98 east 2021-02-12T23:35:26Z- AHRS_M2 msg: 1613172926.32 epoch sec; seqNo:5057 2021-02-12T23:35:26Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.10 psi 2021-02-12T23:35:26Z- handleDepth()-Depth_Keller msg: 1613172926325 epoch sec; seqNo:5105; depth 29.52 2021-02-12T23:35:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172926.34 epoch sec; seqNo:5105; 4057354.52 north; 591200.97 east 2021-02-12T23:35:26Z- AHRS_M2 msg: 1613172926.72 epoch sec; seqNo:5058 2021-02-12T23:35:26Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.10 psi 2021-02-12T23:35:26Z- handleDepth()-Depth_Keller msg: 1613172926729 epoch sec; seqNo:5106; depth 29.41 2021-02-12T23:35:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172926.74 epoch sec; seqNo:5106; 4057354.90 north; 591200.97 east 2021-02-12T23:35:27Z- AHRS_M2 msg: 1613172927.13 epoch sec; seqNo:5059 2021-02-12T23:35:27Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.10 psi 2021-02-12T23:35:27Z- handleDepth()-Depth_Keller msg: 1613172927134 epoch sec; seqNo:5107; depth 29.30 2021-02-12T23:35:27Z- handleDvl() - RDI_Pathfinder msg: 1613172927.15 epoch sec; seqNo:2467 2021-02-12T23:35:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 70.23 , 0.00 , 0.00 , 60.85 2021-02-12T23:35:27Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:35:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172927.16 epoch sec; seqNo:5107; 4057354.90 north; 591200.97 east 2021-02-12T23:35:27Z- AHRS_M2 msg: 1613172927.53 epoch sec; seqNo:5060 2021-02-12T23:35:27Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.10 psi 2021-02-12T23:35:27Z- handleDepth()-Depth_Keller msg: 1613172927534 epoch sec; seqNo:5108; depth 29.19 2021-02-12T23:35:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172927.56 epoch sec; seqNo:5108; 4057355.28 north; 591200.96 east 2021-02-12T23:35:27Z- LcmTrn::handleMessages() - handling 44 messages took 0.63 ms and 132 calls... 2021-02-12T23:35:27Z- waiting for 1613172925.31, time is 1613172927.56 2021-02-12T23:35:27Z- LcmTrn::updateTrn() >>>> pose time 1613172927.56, meas time 1613172927.15 <<<< 2021-02-12T23:35:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172927.15 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:35:27Z- TerrainNav::motionUpdate() - currEstimate = (4057355, 591200.96, 29.19) 2021-02-12T23:35:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:35:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:35:27Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:35:27Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:35:27Z- TNavBF::Projecting Measurements 2021-02-12T23:35:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:35:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:35:27Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-12T23:35:27Z- TNavFilter::2 good beams. 2021-02-12T23:35:27Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:35:27Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:35:27Z- TNavBF:: beamIndex[1] = 3 2021-02-12T23:35:27Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:35:27Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:35:27Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-12T23:35:27Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:35:27Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:27Z- TerrainMapOctree: (vehN, vehE ) = (4057355.01, 591200.87). 2021-02-12T23:35:27Z- TerrainMapOctree: Min Distance = 4946.53. 2021-02-12T23:35:27Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:35:27Z- There is no specific good beam that all particles have in common. 2021-02-12T23:35:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:35:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 7.89 2021-02-12T23:35:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:35:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 5.92 2021-02-12T23:35:27Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:35:27Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:35:27Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:35:27Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:35:27Z- Filter 0 No beams used in subcloud update 2021-02-12T23:35:27Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:35:27Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:35:27Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 752.1 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:35:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172927.15 sec. 2021-02-12T23:35:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:35:27Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:27Z- variances([0], [2], and psi_berg) = 752.063930 833.034981 0.000000 2021-02-12T23:35:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:35:27Z- LcmTrn::publishEstimates() - MLE : 4057340.78 591158.46 29.19 2021-02-12T23:35:27Z- LcmTrn::publishEstimates() - MMSE : 4057355.28 591200.96 29.19 2021-02-12T23:35:27Z- LcmTrn::publishEstimates() - COVAR: 752.06 833.03 0.86 0.00 2021-02-12T23:35:27Z- AHRS_M2 msg: 1613172927.93 epoch sec; seqNo:5061 2021-02-12T23:35:27Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.11 psi 2021-02-12T23:35:27Z- handleDepth()-Depth_Keller msg: 1613172927936 epoch sec; seqNo:5109; depth 29.08 2021-02-12T23:35:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172927.95 epoch sec; seqNo:5109; 4057355.65 north; 591200.96 east 2021-02-12T23:35:28Z- AHRS_M2 msg: 1613172928.34 epoch sec; seqNo:5062 2021-02-12T23:35:28Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.11 psi 2021-02-12T23:35:28Z- handleDepth()-Depth_Keller msg: 1613172928342 epoch sec; seqNo:5110; depth 28.96 2021-02-12T23:35:28Z- handleDvl() - RDI_Pathfinder msg: 1613172928.35 epoch sec; seqNo:2468 2021-02-12T23:35:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 69.84 , 0.00 , 0.00 , 60.90 2021-02-12T23:35:28Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:35:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172928.36 epoch sec; seqNo:5110; 4057355.67 north; 591202.18 east 2021-02-12T23:35:28Z- AHRS_M2 msg: 1613172928.75 epoch sec; seqNo:5063 2021-02-12T23:35:28Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.11 psi 2021-02-12T23:35:28Z- handleDepth()-Depth_Keller msg: 1613172928759 epoch sec; seqNo:5111; depth 28.84 2021-02-12T23:35:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172928.78 epoch sec; seqNo:5111; 4057356.05 north; 591202.18 east 2021-02-12T23:35:29Z- AHRS_M2 msg: 1613172929.15 epoch sec; seqNo:5064 2021-02-12T23:35:29Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.12 psi 2021-02-12T23:35:29Z- handleDepth()-Depth_Keller msg: 1613172929157 epoch sec; seqNo:5112; depth 28.71 2021-02-12T23:35:29Z- handleDvl() - RDI_Pathfinder msg: 1613172929.18 epoch sec; seqNo:2469 2021-02-12T23:35:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 70.69 , 0.00 , 0.00 , 60.47 2021-02-12T23:35:29Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:35:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172929.19 epoch sec; seqNo:5112; 4057356.05 north; 591202.18 east 2021-02-12T23:35:29Z- AHRS_M2 msg: 1613172929.55 epoch sec; seqNo:5065 2021-02-12T23:35:29Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.12 psi 2021-02-12T23:35:29Z- handleDepth()-Depth_Keller msg: 1613172929557 epoch sec; seqNo:5113; depth 28.58 2021-02-12T23:35:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172929.57 epoch sec; seqNo:5113; 4057356.42 north; 591202.17 east 2021-02-12T23:35:29Z- AHRS_M2 msg: 1613172929.95 epoch sec; seqNo:5066 2021-02-12T23:35:29Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.12 psi 2021-02-12T23:35:29Z- handleDepth()-Depth_Keller msg: 1613172929961 epoch sec; seqNo:5114; depth 28.46 2021-02-12T23:35:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172929.97 epoch sec; seqNo:5114; 4057356.80 north; 591202.17 east 2021-02-12T23:35:30Z- AHRS_M2 msg: 1613172930.36 epoch sec; seqNo:5067 2021-02-12T23:35:30Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.13 psi 2021-02-12T23:35:30Z- handleDepth()-Depth_Keller msg: 1613172930371 epoch sec; seqNo:5115; depth 28.35 2021-02-12T23:35:30Z- handleDvl() - RDI_Pathfinder msg: 1613172930.38 epoch sec; seqNo:2470 2021-02-12T23:35:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.84 , 78.36 , 0.00 , 61.75 2021-02-12T23:35:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , 0.03 2021-02-12T23:35:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172930.39 epoch sec; seqNo:5115; 4057356.80 north; 591202.17 east 2021-02-12T23:35:30Z- AHRS_M2 msg: 1613172930.76 epoch sec; seqNo:5068 2021-02-12T23:35:30Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-12T23:35:30Z- handleDepth()-Depth_Keller msg: 1613172930767 epoch sec; seqNo:5116; depth 28.22 2021-02-12T23:35:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172930.78 epoch sec; seqNo:5116; 4057357.18 north; 591202.17 east 2021-02-12T23:35:31Z- AHRS_M2 msg: 1613172931.16 epoch sec; seqNo:5069 2021-02-12T23:35:31Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-12T23:35:31Z- handleDepth()-Depth_Keller msg: 1613172931177 epoch sec; seqNo:5117; depth 28.10 2021-02-12T23:35:31Z- handleDvl() - RDI_Pathfinder msg: 1613172931.21 epoch sec; seqNo:2471 2021-02-12T23:35:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.91 , 79.29 , 0.00 , 60.85 2021-02-12T23:35:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , 0.05 2021-02-12T23:35:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172931.23 epoch sec; seqNo:5117; 4057357.56 north; 591202.16 east 2021-02-12T23:35:31Z- AHRS_M2 msg: 1613172931.57 epoch sec; seqNo:5070 2021-02-12T23:35:31Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-12T23:35:31Z- handleDepth()-Depth_Keller msg: 1613172931574 epoch sec; seqNo:5118; depth 27.96 2021-02-12T23:35:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172931.59 epoch sec; seqNo:5118; 4057357.95 north; 591203.38 east 2021-02-12T23:35:31Z- AHRS_M2 msg: 1613172931.97 epoch sec; seqNo:5071 2021-02-12T23:35:31Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-12T23:35:31Z- handleDepth()-Depth_Keller msg: 1613172931980 epoch sec; seqNo:5119; depth 27.84 2021-02-12T23:35:32Z- handleDvl() - RDI_Pathfinder msg: 1613172932.00 epoch sec; seqNo:2472 2021-02-12T23:35:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 69.91 , 0.00 , 0.00 , 61.82 2021-02-12T23:35:32Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:35:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172932.01 epoch sec; seqNo:5119; 4057358.33 north; 591203.37 east 2021-02-12T23:35:32Z- AHRS_M2 msg: 1613172932.38 epoch sec; seqNo:5072 2021-02-12T23:35:32Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-12T23:35:32Z- handleDepth()-Depth_Keller msg: 1613172932380 epoch sec; seqNo:5120; depth 27.72 2021-02-12T23:35:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172932.39 epoch sec; seqNo:5120; 4057358.33 north; 591203.37 east 2021-02-12T23:35:32Z- AHRS_M2 msg: 1613172932.78 epoch sec; seqNo:5073 2021-02-12T23:35:32Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.14 psi 2021-02-12T23:35:32Z- handleDepth()-Depth_Keller msg: 1613172932784 epoch sec; seqNo:5121; depth 27.59 2021-02-12T23:35:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172932.79 epoch sec; seqNo:5121; 4057358.71 north; 591203.37 east 2021-02-12T23:35:32Z- LcmTrn::handleMessages() - handling 44 messages took 0.73 ms and 130 calls... 2021-02-12T23:35:32Z- waiting for 1613172930.56, time is 1613172932.79 2021-02-12T23:35:32Z- LcmTrn::updateTrn() >>>> pose time 1613172932.79, meas time 1613172932.00 <<<< 2021-02-12T23:35:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172932.00 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:35:32Z- TerrainNav::motionUpdate() - currEstimate = (4057359, 591203.37, 27.59) 2021-02-12T23:35:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:35:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:35:32Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:35:32Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:35:32Z- TNavBF::Projecting Measurements 2021-02-12T23:35:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:35:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:35:32Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-12T23:35:32Z- TNavFilter::2 good beams. 2021-02-12T23:35:32Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:35:32Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:35:32Z- TNavBF:: beamIndex[1] = 3 2021-02-12T23:35:32Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:35:32Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:35:32Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-12T23:35:32Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:35:32Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:32Z- TerrainMapOctree: (vehN, vehE ) = (4057358.19, 591203.01). 2021-02-12T23:35:32Z- TerrainMapOctree: Min Distance = 4942.71. 2021-02-12T23:35:32Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:35:32Z- There is no specific good beam that all particles have in common. 2021-02-12T23:35:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:35:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 7.82 2021-02-12T23:35:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:35:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 6.11 2021-02-12T23:35:32Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:35:32Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:35:32Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:35:32Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:35:32Z- Filter 0 No beams used in subcloud update 2021-02-12T23:35:32Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:35:32Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:35:32Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 752.0 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:35:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172932.00 sec. 2021-02-12T23:35:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:35:32Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:32Z- variances([0], [2], and psi_berg) = 751.929726 833.034853 0.000000 2021-02-12T23:35:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:35:32Z- LcmTrn::publishEstimates() - MLE : 4057345.21 591159.87 27.59 2021-02-12T23:35:32Z- LcmTrn::publishEstimates() - MMSE : 4057358.71 591203.37 27.59 2021-02-12T23:35:32Z- LcmTrn::publishEstimates() - COVAR: 751.93 833.03 0.87 0.00 2021-02-12T23:35:33Z- AHRS_M2 msg: 1613172933.18 epoch sec; seqNo:5074 2021-02-12T23:35:33Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-12T23:35:33Z- handleDepth()-Depth_Keller msg: 1613172933188 epoch sec; seqNo:5122; depth 27.48 2021-02-12T23:35:33Z- handleDvl() - RDI_Pathfinder msg: 1613172933.21 epoch sec; seqNo:2473 2021-02-12T23:35:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 70.42 , 0.00 , 0.00 , 62.89 2021-02-12T23:35:33Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:35:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172933.22 epoch sec; seqNo:5122; 4057359.09 north; 591203.37 east 2021-02-12T23:35:33Z- AHRS_M2 msg: 1613172933.59 epoch sec; seqNo:5075 2021-02-12T23:35:33Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-12T23:35:33Z- handleDepth()-Depth_Keller msg: 1613172933592 epoch sec; seqNo:5123; depth 27.34 2021-02-12T23:35:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172933.60 epoch sec; seqNo:5123; 4057359.47 north; 591203.36 east 2021-02-12T23:35:34Z- AHRS_M2 msg: 1613172934.01 epoch sec; seqNo:5076 2021-02-12T23:35:34Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-12T23:35:34Z- handleDepth()-Depth_Keller msg: 1613172934016 epoch sec; seqNo:5124; depth 27.23 2021-02-12T23:35:34Z- handleDvl() - RDI_Pathfinder msg: 1613172934.04 epoch sec; seqNo:2474 2021-02-12T23:35:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 71.31 , 0.00 , 0.00 , 63.33 2021-02-12T23:35:34Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:35:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172934.06 epoch sec; seqNo:5124; 4057359.47 north; 591203.36 east 2021-02-12T23:35:34Z- AHRS_M2 msg: 1613172934.40 epoch sec; seqNo:5077 2021-02-12T23:35:34Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-12T23:35:34Z- handleDepth()-Depth_Keller msg: 1613172934401 epoch sec; seqNo:5125; depth 27.10 2021-02-12T23:35:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172934.41 epoch sec; seqNo:5125; 4057359.86 north; 591204.58 east 2021-02-12T23:35:34Z- AHRS_M2 msg: 1613172934.80 epoch sec; seqNo:5078 2021-02-12T23:35:34Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.13 psi 2021-02-12T23:35:34Z- handleDepth()-Depth_Keller msg: 1613172934809 epoch sec; seqNo:5126; depth 26.99 2021-02-12T23:35:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172934.83 epoch sec; seqNo:5126; 4057360.24 north; 591204.58 east 2021-02-12T23:35:35Z- AHRS_M2 msg: 1613172935.20 epoch sec; seqNo:5079 2021-02-12T23:35:35Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.13 psi 2021-02-12T23:35:35Z- handleDepth()-Depth_Keller msg: 1613172935209 epoch sec; seqNo:5127; depth 26.87 2021-02-12T23:35:35Z- handleDvl() - RDI_Pathfinder msg: 1613172935.22 epoch sec; seqNo:2475 2021-02-12T23:35:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.31 , 80.16 , 0.00 , 62.89 2021-02-12T23:35:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , 0.05 2021-02-12T23:35:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172935.25 epoch sec; seqNo:5127; 4057360.62 north; 591204.57 east 2021-02-12T23:35:35Z- AHRS_M2 msg: 1613172935.61 epoch sec; seqNo:5080 2021-02-12T23:35:35Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.13 psi 2021-02-12T23:35:35Z- handleDepth()-Depth_Keller msg: 1613172935612 epoch sec; seqNo:5128; depth 26.75 2021-02-12T23:35:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172935.62 epoch sec; seqNo:5128; 4057360.62 north; 591204.57 east 2021-02-12T23:35:36Z- AHRS_M2 msg: 1613172936.01 epoch sec; seqNo:5081 2021-02-12T23:35:36Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.13 psi 2021-02-12T23:35:36Z- handleDepth()-Depth_Keller msg: 1613172936018 epoch sec; seqNo:5129; depth 26.65 2021-02-12T23:35:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172936.03 epoch sec; seqNo:5129; 4057361.00 north; 591204.57 east 2021-02-12T23:35:36Z- AHRS_M2 msg: 1613172936.42 epoch sec; seqNo:5082 2021-02-12T23:35:36Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.13 psi 2021-02-12T23:35:36Z- handleDepth()-Depth_Keller msg: 1613172936420 epoch sec; seqNo:5130; depth 26.52 2021-02-12T23:35:36Z- handleDvl() - RDI_Pathfinder msg: 1613172936.44 epoch sec; seqNo:2476 2021-02-12T23:35:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 0.00 , 94.74 , 63.75 2021-02-12T23:35:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , -0.00 2021-02-12T23:35:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172936.45 epoch sec; seqNo:5130; 4057361.38 north; 591204.56 east 2021-02-12T23:35:36Z- AHRS_M2 msg: 1613172936.82 epoch sec; seqNo:5083 2021-02-12T23:35:36Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.13 psi 2021-02-12T23:35:36Z- handleDepth()-Depth_Keller msg: 1613172936829 epoch sec; seqNo:5131; depth 26.39 2021-02-12T23:35:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172936.84 epoch sec; seqNo:5131; 4057361.39 north; 591205.78 east 2021-02-12T23:35:37Z- AHRS_M2 msg: 1613172937.23 epoch sec; seqNo:5084 2021-02-12T23:35:37Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.13 psi 2021-02-12T23:35:37Z- handleDepth()-Depth_Keller msg: 1613172937230 epoch sec; seqNo:5132; depth 26.28 2021-02-12T23:35:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172937.24 epoch sec; seqNo:5132; 4057361.77 north; 591205.78 east 2021-02-12T23:35:37Z- AHRS_M2 msg: 1613172937.63 epoch sec; seqNo:5085 2021-02-12T23:35:37Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.14 psi 2021-02-12T23:35:37Z- handleDepth()-Depth_Keller msg: 1613172937645 epoch sec; seqNo:5133; depth 26.16 2021-02-12T23:35:37Z- handleDvl() - RDI_Pathfinder msg: 1613172937.66 epoch sec; seqNo:2477 2021-02-12T23:35:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 82.84 , 94.74 , 63.34 2021-02-12T23:35:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.08 , -0.06 2021-02-12T23:35:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172937.67 epoch sec; seqNo:5133; 4057362.15 north; 591205.78 east 2021-02-12T23:35:37Z- LcmTrn::handleMessages() - handling 41 messages took 0.71 ms and 127 calls... 2021-02-12T23:35:37Z- waiting for 1613172935.79, time is 1613172937.67 2021-02-12T23:35:37Z- LcmTrn::updateTrn() equating pose 1613172937.67 and meas times 1613172937.67 2021-02-12T23:35:37Z- LcmTrn::updateTrn() >>>> pose time 1613172937.67, meas time 1613172937.67 <<<< 2021-02-12T23:35:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172937.67 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:35:37Z- TerrainNav::motionUpdate() - currEstimate = (4057362, 591205.78, 26.16) 2021-02-12T23:35:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:35:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:35:37Z- TNavBF::Projecting Measurements 2021-02-12T23:35:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:35:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:35:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:35:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:35:37Z- TNavFilter::4 good beams. 2021-02-12T23:35:37Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:35:37Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:35:37Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:35:37Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:35:37Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:35:37Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:35:37Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:35:37Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:37Z- TerrainMapOctree: (vehN, vehE ) = (4057362.15, 591205.78). 2021-02-12T23:35:37Z- TerrainMapOctree: Min Distance = 4937.88. 2021-02-12T23:35:37Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:35:37Z- There is no specific good beam that all particles have in common. 2021-02-12T23:35:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:35:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-12T23:35:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:35:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 10.98 2021-02-12T23:35:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:35:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 14.36 2021-02-12T23:35:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:35:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 6.42 2021-02-12T23:35:37Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:35:37Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:35:37Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:35:37Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:35:37Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:35:37Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:35:37Z- Filter 0 No beams used in subcloud update 2021-02-12T23:35:37Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:35:37Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:35:37Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 751.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:35:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172937.67 sec. 2021-02-12T23:35:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:35:37Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:37Z- variances([0], [2], and psi_berg) = 751.827458 833.034756 0.000000 2021-02-12T23:35:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:35:37Z- LcmTrn::publishEstimates() - MLE : 4057348.65 591162.28 26.16 2021-02-12T23:35:37Z- LcmTrn::publishEstimates() - MMSE : 4057362.15 591205.78 26.16 2021-02-12T23:35:37Z- LcmTrn::publishEstimates() - COVAR: 751.83 833.03 0.78 0.00 2021-02-12T23:35:38Z- AHRS_M2 msg: 1613172938.03 epoch sec; seqNo:5086 2021-02-12T23:35:38Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.14 psi 2021-02-12T23:35:38Z- handleDepth()-Depth_Keller msg: 1613172938036 epoch sec; seqNo:5134; depth 26.05 2021-02-12T23:35:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172938.05 epoch sec; seqNo:5134; 4057362.15 north; 591205.78 east 2021-02-12T23:35:38Z- AHRS_M2 msg: 1613172938.44 epoch sec; seqNo:5087 2021-02-12T23:35:38Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.15 psi 2021-02-12T23:35:38Z- handleDepth()-Depth_Keller msg: 1613172938440 epoch sec; seqNo:5135; depth 25.93 2021-02-12T23:35:38Z- handleDvl() - RDI_Pathfinder msg: 1613172938.45 epoch sec; seqNo:2478 2021-02-12T23:35:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 85.00 , 97.98 , 63.75 2021-02-12T23:35:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , -0.11 2021-02-12T23:35:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172938.46 epoch sec; seqNo:5135; 4057362.53 north; 591205.77 east 2021-02-12T23:35:38Z- AHRS_M2 msg: 1613172938.84 epoch sec; seqNo:5088 2021-02-12T23:35:38Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.15 psi 2021-02-12T23:35:38Z- handleDepth()-Depth_Keller msg: 1613172938846 epoch sec; seqNo:5136; depth 25.80 2021-02-12T23:35:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172938.86 epoch sec; seqNo:5136; 4057362.92 north; 591206.99 east 2021-02-12T23:35:39Z- AHRS_M2 msg: 1613172939.24 epoch sec; seqNo:5089 2021-02-12T23:35:39Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.16 psi 2021-02-12T23:35:39Z- handleDepth()-Depth_Keller msg: 1613172939258 epoch sec; seqNo:5137; depth 25.70 2021-02-12T23:35:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172939.27 epoch sec; seqNo:5137; 4057362.92 north; 591206.99 east 2021-02-12T23:35:39Z- AHRS_M2 msg: 1613172939.65 epoch sec; seqNo:5090 2021-02-12T23:35:39Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.16 psi 2021-02-12T23:35:39Z- handleDepth()-Depth_Keller msg: 1613172939652 epoch sec; seqNo:5138; depth 25.57 2021-02-12T23:35:39Z- handleDvl() - RDI_Pathfinder msg: 1613172939.66 epoch sec; seqNo:2479 2021-02-12T23:35:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 86.09 , 97.98 , 64.57 2021-02-12T23:35:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , -0.09 2021-02-12T23:35:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172939.70 epoch sec; seqNo:5138; 4057363.30 north; 591206.98 east 2021-02-12T23:35:40Z- AHRS_M2 msg: 1613172940.05 epoch sec; seqNo:5091 2021-02-12T23:35:40Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.16 psi 2021-02-12T23:35:40Z- handleDepth()-Depth_Keller msg: 1613172940056 epoch sec; seqNo:5139; depth 25.44 2021-02-12T23:35:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172940.07 epoch sec; seqNo:5139; 4057363.67 north; 591206.98 east 2021-02-12T23:35:40Z- AHRS_M2 msg: 1613172940.46 epoch sec; seqNo:5092 2021-02-12T23:35:40Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.17 psi 2021-02-12T23:35:40Z- handleDepth()-Depth_Keller msg: 1613172940461 epoch sec; seqNo:5140; depth 25.33 2021-02-12T23:35:40Z- handleDvl() - RDI_Pathfinder msg: 1613172940.47 epoch sec; seqNo:2480 2021-02-12T23:35:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 86.09 , 97.98 , 63.75 2021-02-12T23:35:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.07 , -0.05 2021-02-12T23:35:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172940.48 epoch sec; seqNo:5140; 4057363.67 north; 591206.98 east 2021-02-12T23:35:40Z- AHRS_M2 msg: 1613172940.86 epoch sec; seqNo:5093 2021-02-12T23:35:40Z- AHRS_M2 msg: 0.66 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:40Z- handleDepth()-Depth_Keller msg: 1613172940864 epoch sec; seqNo:5141; depth 25.22 2021-02-12T23:35:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:35:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:35:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:35:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:35:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:35:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:35:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:35:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:35:40Z- handleDvl() - RDI_Pathfinder msg: 1613172940.87 epoch sec; seqNo:2481 2021-02-12T23:35:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:35:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.07 , -0.05 2021-02-12T23:35:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172940.88 epoch sec; seqNo:5141; 4057364.05 north; 591206.98 east 2021-02-12T23:35:41Z- AHRS_M2 msg: 1613172941.26 epoch sec; seqNo:5094 2021-02-12T23:35:41Z- AHRS_M2 msg: 0.65 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:41Z- handleDepth()-Depth_Keller msg: 1613172941273 epoch sec; seqNo:5142; depth 25.12 2021-02-12T23:35:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172941.28 epoch sec; seqNo:5142; 4057364.43 north; 591206.97 east 2021-02-12T23:35:41Z- AHRS_M2 msg: 1613172941.67 epoch sec; seqNo:5095 2021-02-12T23:35:41Z- AHRS_M2 msg: 0.65 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:41Z- handleDepth()-Depth_Keller msg: 1613172941672 epoch sec; seqNo:5143; depth 25.00 2021-02-12T23:35:41Z- handleDvl() - RDI_Pathfinder msg: 1613172941.68 epoch sec; seqNo:2482 2021-02-12T23:35:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 85.88 , 93.46 , 64.16 2021-02-12T23:35:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , 0.02 2021-02-12T23:35:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172941.69 epoch sec; seqNo:5143; 4057364.45 north; 591208.19 east 2021-02-12T23:35:42Z- AHRS_M2 msg: 1613172942.07 epoch sec; seqNo:5096 2021-02-12T23:35:42Z- AHRS_M2 msg: 0.65 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:42Z- handleDepth()-Depth_Keller msg: 1613172942079 epoch sec; seqNo:5144; depth 24.89 2021-02-12T23:35:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:35:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:35:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:35:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:35:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:35:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:35:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:35:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:35:42Z- handleDvl() - RDI_Pathfinder msg: 1613172942.08 epoch sec; seqNo:2483 2021-02-12T23:35:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:35:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , 0.02 2021-02-12T23:35:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172942.09 epoch sec; seqNo:5144; 4057364.82 north; 591208.19 east 2021-02-12T23:35:42Z- AHRS_M2 msg: 1613172942.47 epoch sec; seqNo:5097 2021-02-12T23:35:42Z- AHRS_M2 msg: 0.65 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:42Z- handleDepth()-Depth_Keller msg: 1613172942481 epoch sec; seqNo:5145; depth 24.78 2021-02-12T23:35:42Z- handleDvl() - RDI_Pathfinder msg: 1613172942.49 epoch sec; seqNo:2484 2021-02-12T23:35:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 87.10 , 94.67 , 64.57 2021-02-12T23:35:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.02 , 0.06 2021-02-12T23:35:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172942.50 epoch sec; seqNo:5145; 4057365.20 north; 591208.19 east 2021-02-12T23:35:42Z- AHRS_M2 msg: 1613172942.88 epoch sec; seqNo:5098 2021-02-12T23:35:42Z- AHRS_M2 msg: 0.65 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:42Z- handleDepth()-Depth_Keller msg: 1613172942884 epoch sec; seqNo:5146; depth 24.66 2021-02-12T23:35:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172942.89 epoch sec; seqNo:5146; 4057365.20 north; 591208.19 east 2021-02-12T23:35:42Z- LcmTrn::handleMessages() - handling 46 messages took 0.74 ms and 131 calls... 2021-02-12T23:35:43Z- waiting for 1613172940.67, time is 1613172942.89 2021-02-12T23:35:43Z- LcmTrn::updateTrn() >>>> pose time 1613172942.89, meas time 1613172942.49 <<<< 2021-02-12T23:35:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172942.49 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:35:43Z- TerrainNav::motionUpdate() - currEstimate = (4057365, 591208.19, 24.66) 2021-02-12T23:35:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:35:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:35:43Z- TNavBF::Projecting Measurements 2021-02-12T23:35:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:35:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:35:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:35:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:35:43Z- TNavFilter::4 good beams. 2021-02-12T23:35:43Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:35:43Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:35:43Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:35:43Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:35:43Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:35:43Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:35:43Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:35:43Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:43Z- TerrainMapOctree: (vehN, vehE ) = (4057364.97, 591208.00). 2021-02-12T23:35:43Z- TerrainMapOctree: Min Distance = 4934.29. 2021-02-12T23:35:43Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:35:43Z- There is no specific good beam that all particles have in common. 2021-02-12T23:35:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:35:43Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-12T23:35:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:35:43Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 12.14 2021-02-12T23:35:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:35:43Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 14.34 2021-02-12T23:35:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:35:43Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 6.67 2021-02-12T23:35:43Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:35:43Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:35:43Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:35:43Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:35:43Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:35:43Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:35:43Z- Filter 0 No beams used in subcloud update 2021-02-12T23:35:43Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:35:43Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:35:43Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 751.7 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:35:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172942.49 sec. 2021-02-12T23:35:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:35:43Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:43Z- variances([0], [2], and psi_berg) = 751.705689 833.034640 0.000000 2021-02-12T23:35:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:35:43Z- LcmTrn::publishEstimates() - MLE : 4057351.70 591164.69 24.66 2021-02-12T23:35:43Z- LcmTrn::publishEstimates() - MMSE : 4057365.20 591208.19 24.66 2021-02-12T23:35:43Z- LcmTrn::publishEstimates() - COVAR: 751.71 833.03 0.73 0.00 2021-02-12T23:35:43Z- AHRS_M2 msg: 1613172943.29 epoch sec; seqNo:5099 2021-02-12T23:35:43Z- AHRS_M2 msg: 0.65 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:43Z- handleDepth()-Depth_Keller msg: 1613172943293 epoch sec; seqNo:5147; depth 24.56 2021-02-12T23:35:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:35:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:35:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:35:43Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:35:43Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:35:43Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:35:43Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:35:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:35:43Z- handleDvl() - RDI_Pathfinder msg: 1613172943.30 epoch sec; seqNo:2485 2021-02-12T23:35:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:35:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.02 , 0.06 2021-02-12T23:35:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172943.32 epoch sec; seqNo:5147; 4057365.58 north; 591208.18 east 2021-02-12T23:35:43Z- AHRS_M2 msg: 1613172943.69 epoch sec; seqNo:5100 2021-02-12T23:35:43Z- AHRS_M2 msg: 0.65 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:43Z- handleDepth()-Depth_Keller msg: 1613172943692 epoch sec; seqNo:5148; depth 24.45 2021-02-12T23:35:43Z- handleDvl() - RDI_Pathfinder msg: 1613172943.71 epoch sec; seqNo:2486 2021-02-12T23:35:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 87.17 , 97.98 , 64.97 2021-02-12T23:35:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , 0.08 2021-02-12T23:35:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172943.72 epoch sec; seqNo:5148; 4057365.96 north; 591208.18 east 2021-02-12T23:35:44Z- AHRS_M2 msg: 1613172944.10 epoch sec; seqNo:5101 2021-02-12T23:35:44Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:44Z- handleDepth()-Depth_Keller msg: 1613172944101 epoch sec; seqNo:5149; depth 24.33 2021-02-12T23:35:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172944.11 epoch sec; seqNo:5149; 4057365.97 north; 591209.40 east 2021-02-12T23:35:44Z- AHRS_M2 msg: 1613172944.50 epoch sec; seqNo:5102 2021-02-12T23:35:44Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.18 psi 2021-02-12T23:35:44Z- handleDepth()-Depth_Keller msg: 1613172944503 epoch sec; seqNo:5150; depth 24.24 2021-02-12T23:35:44Z- handleDvl() - RDI_Pathfinder msg: 1613172944.52 epoch sec; seqNo:2487 2021-02-12T23:35:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 88.18 , 95.75 , 64.97 2021-02-12T23:35:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , 0.10 2021-02-12T23:35:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172944.53 epoch sec; seqNo:5150; 4057366.35 north; 591209.39 east 2021-02-12T23:35:44Z- AHRS_M2 msg: 1613172944.90 epoch sec; seqNo:5103 2021-02-12T23:35:44Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.18 psi 2021-02-12T23:35:44Z- handleDepth()-Depth_Keller msg: 1613172944914 epoch sec; seqNo:5151; depth 24.12 2021-02-12T23:35:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172944.94 epoch sec; seqNo:5151; 4057366.73 north; 591209.39 east 2021-02-12T23:35:45Z- AHRS_M2 msg: 1613172945.33 epoch sec; seqNo:5104 2021-02-12T23:35:45Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.18 psi 2021-02-12T23:35:45Z- handleDepth()-Depth_Keller msg: 1613172945339 epoch sec; seqNo:5152; depth 24.01 2021-02-12T23:35:45Z- handleDvl() - RDI_Pathfinder msg: 1613172945.35 epoch sec; seqNo:2488 2021-02-12T23:35:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 86.09 , 96.90 , 66.61 2021-02-12T23:35:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.09 2021-02-12T23:35:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172945.38 epoch sec; seqNo:5152; 4057367.11 north; 591209.39 east 2021-02-12T23:35:45Z- AHRS_M2 msg: 1613172945.71 epoch sec; seqNo:5105 2021-02-12T23:35:45Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.18 psi 2021-02-12T23:35:45Z- handleDepth()-Depth_Keller msg: 1613172945717 epoch sec; seqNo:5153; depth 23.91 2021-02-12T23:35:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:35:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:35:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:35:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:35:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:35:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:35:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:35:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:35:45Z- handleDvl() - RDI_Pathfinder msg: 1613172945.72 epoch sec; seqNo:2489 2021-02-12T23:35:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:35:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.09 2021-02-12T23:35:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172945.75 epoch sec; seqNo:5153; 4057367.49 north; 591209.38 east 2021-02-12T23:35:46Z- AHRS_M2 msg: 1613172946.11 epoch sec; seqNo:5106 2021-02-12T23:35:46Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.18 psi 2021-02-12T23:35:46Z- handleDepth()-Depth_Keller msg: 1613172946118 epoch sec; seqNo:5154; depth 23.81 2021-02-12T23:35:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172946.13 epoch sec; seqNo:5154; 4057367.49 north; 591209.38 east 2021-02-12T23:35:46Z- AHRS_M2 msg: 1613172946.52 epoch sec; seqNo:5107 2021-02-12T23:35:46Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.18 psi 2021-02-12T23:35:46Z- handleDepth()-Depth_Keller msg: 1613172946522 epoch sec; seqNo:5155; depth 23.72 2021-02-12T23:35:46Z- handleDvl() - RDI_Pathfinder msg: 1613172946.53 epoch sec; seqNo:2490 2021-02-12T23:35:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 87.10 , 95.75 , 66.61 2021-02-12T23:35:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.03 , 0.08 2021-02-12T23:35:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172946.55 epoch sec; seqNo:5155; 4057367.88 north; 591210.60 east 2021-02-12T23:35:46Z- AHRS_M2 msg: 1613172946.92 epoch sec; seqNo:5108 2021-02-12T23:35:46Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.18 psi 2021-02-12T23:35:46Z- handleDepth()-Depth_Keller msg: 1613172946930 epoch sec; seqNo:5156; depth 23.61 2021-02-12T23:35:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:35:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:35:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:35:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:35:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:35:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:35:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:35:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:35:46Z- handleDvl() - RDI_Pathfinder msg: 1613172946.94 epoch sec; seqNo:2491 2021-02-12T23:35:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:35:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.03 , 0.08 2021-02-12T23:35:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172946.95 epoch sec; seqNo:5156; 4057368.26 north; 591210.60 east 2021-02-12T23:35:47Z- AHRS_M2 msg: 1613172947.35 epoch sec; seqNo:5109 2021-02-12T23:35:47Z- AHRS_M2 msg: 0.63 phi; 0.29 theta; -0.17 psi 2021-02-12T23:35:47Z- handleDepth()-Depth_Keller msg: 1613172947360 epoch sec; seqNo:5157; depth 23.50 2021-02-12T23:35:47Z- handleDvl() - RDI_Pathfinder msg: 1613172947.39 epoch sec; seqNo:2492 2021-02-12T23:35:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 88.18 , 96.83 , 66.20 2021-02-12T23:35:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.06 , 0.03 2021-02-12T23:35:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172947.42 epoch sec; seqNo:5157; 4057368.64 north; 591210.59 east 2021-02-12T23:35:47Z- AHRS_M2 msg: 1613172947.73 epoch sec; seqNo:5110 2021-02-12T23:35:47Z- AHRS_M2 msg: 0.63 phi; 0.29 theta; -0.17 psi 2021-02-12T23:35:47Z- handleDepth()-Depth_Keller msg: 1613172947740 epoch sec; seqNo:5158; depth 23.38 2021-02-12T23:35:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172947.75 epoch sec; seqNo:5158; 4057368.64 north; 591210.59 east 2021-02-12T23:35:48Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-12T23:35:48Z- waiting for 1613172945.89, time is 1613172947.75 2021-02-12T23:35:48Z- LcmTrn::updateTrn() >>>> pose time 1613172947.75, meas time 1613172947.39 <<<< 2021-02-12T23:35:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172947.39 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:35:48Z- TerrainNav::motionUpdate() - currEstimate = (4057369, 591210.59, 23.38) 2021-02-12T23:35:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:35:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:35:48Z- TNavBF::Projecting Measurements 2021-02-12T23:35:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:35:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:35:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:35:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:35:48Z- TNavFilter::4 good beams. 2021-02-12T23:35:48Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:35:48Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:35:48Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:35:48Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:35:48Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:35:48Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:35:48Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:35:48Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:48Z- TerrainMapOctree: (vehN, vehE ) = (4057368.38, 591210.41). 2021-02-12T23:35:48Z- TerrainMapOctree: Min Distance = 4930.12. 2021-02-12T23:35:48Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:35:48Z- There is no specific good beam that all particles have in common. 2021-02-12T23:35:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:35:48Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-12T23:35:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:35:48Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 12.44 2021-02-12T23:35:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:35:48Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 15.00 2021-02-12T23:35:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:35:48Z- Old Depth Bias: 0.00 DepthBiasCov: 0.77 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 7.01 2021-02-12T23:35:48Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:35:48Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:35:48Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:35:48Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:35:48Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:35:48Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:35:48Z- Filter 0 No beams used in subcloud update 2021-02-12T23:35:48Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:35:48Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:35:48Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 751.6 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:35:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172947.39 sec. 2021-02-12T23:35:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:35:48Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:48Z- variances([0], [2], and psi_berg) = 751.579781 833.034521 0.000000 2021-02-12T23:35:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:35:48Z- LcmTrn::publishEstimates() - MLE : 4057355.14 591167.09 23.38 2021-02-12T23:35:48Z- LcmTrn::publishEstimates() - MMSE : 4057368.64 591210.59 23.38 2021-02-12T23:35:48Z- LcmTrn::publishEstimates() - COVAR: 751.58 833.03 0.72 0.00 2021-02-12T23:35:48Z- AHRS_M2 msg: 1613172948.14 epoch sec; seqNo:5111 2021-02-12T23:35:48Z- AHRS_M2 msg: 0.63 phi; 0.29 theta; -0.17 psi 2021-02-12T23:35:48Z- handleDepth()-Depth_Keller msg: 1613172948142 epoch sec; seqNo:5159; depth 23.30 2021-02-12T23:35:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:35:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:35:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:35:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:35:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:35:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:35:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:35:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:35:48Z- handleDvl() - RDI_Pathfinder msg: 1613172948.14 epoch sec; seqNo:2493 2021-02-12T23:35:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:35:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.06 , 0.03 2021-02-12T23:35:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172948.15 epoch sec; seqNo:5159; 4057369.02 north; 591210.59 east 2021-02-12T23:35:48Z- AHRS_M2 msg: 1613172948.54 epoch sec; seqNo:5112 2021-02-12T23:35:48Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.17 psi 2021-02-12T23:35:48Z- handleDepth()-Depth_Keller msg: 1613172948540 epoch sec; seqNo:5160; depth 23.20 2021-02-12T23:35:48Z- handleDvl() - RDI_Pathfinder msg: 1613172948.56 epoch sec; seqNo:2494 2021-02-12T23:35:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 87.03 , 96.77 , 66.20 2021-02-12T23:35:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.09 , -0.02 2021-02-12T23:35:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172948.57 epoch sec; seqNo:5160; 4057369.41 north; 591211.80 east 2021-02-12T23:35:48Z- AHRS_M2 msg: 1613172948.94 epoch sec; seqNo:5113 2021-02-12T23:35:48Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.16 psi 2021-02-12T23:35:48Z- handleDepth()-Depth_Keller msg: 1613172948944 epoch sec; seqNo:5161; depth 23.11 2021-02-12T23:35:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172948.95 epoch sec; seqNo:5161; 4057369.41 north; 591211.80 east 2021-02-12T23:35:49Z- AHRS_M2 msg: 1613172949.35 epoch sec; seqNo:5114 2021-02-12T23:35:49Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.16 psi 2021-02-12T23:35:49Z- handleDepth()-Depth_Keller msg: 1613172949351 epoch sec; seqNo:5162; depth 23.00 2021-02-12T23:35:49Z- handleDvl() - RDI_Pathfinder msg: 1613172949.36 epoch sec; seqNo:2495 2021-02-12T23:35:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.74 , 87.10 , 96.83 , 67.84 2021-02-12T23:35:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.05 , -0.07 2021-02-12T23:35:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172949.37 epoch sec; seqNo:5162; 4057369.79 north; 591211.80 east 2021-02-12T23:35:49Z- AHRS_M2 msg: 1613172949.75 epoch sec; seqNo:5115 2021-02-12T23:35:49Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.17 psi 2021-02-12T23:35:49Z- handleDepth()-Depth_Keller msg: 1613172949757 epoch sec; seqNo:5163; depth 22.88 2021-02-12T23:35:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172949.77 epoch sec; seqNo:5163; 4057370.17 north; 591211.80 east 2021-02-12T23:35:50Z- AHRS_M2 msg: 1613172950.15 epoch sec; seqNo:5116 2021-02-12T23:35:50Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.17 psi 2021-02-12T23:35:50Z- handleDepth()-Depth_Keller msg: 1613172950157 epoch sec; seqNo:5164; depth 22.77 2021-02-12T23:35:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172950.17 epoch sec; seqNo:5164; 4057370.55 north; 591211.79 east 2021-02-12T23:35:50Z- AHRS_M2 msg: 1613172950.56 epoch sec; seqNo:5117 2021-02-12T23:35:50Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.17 psi 2021-02-12T23:35:50Z- handleDepth()-Depth_Keller msg: 1613172950560 epoch sec; seqNo:5165; depth 22.69 2021-02-12T23:35:50Z- handleDvl() - RDI_Pathfinder msg: 1613172950.57 epoch sec; seqNo:2496 2021-02-12T23:35:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 89.20 , 97.85 , 66.61 2021-02-12T23:35:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , -0.08 2021-02-12T23:35:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172950.58 epoch sec; seqNo:5165; 4057370.55 north; 591211.79 east 2021-02-12T23:35:50Z- AHRS_M2 msg: 1613172950.97 epoch sec; seqNo:5118 2021-02-12T23:35:50Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.17 psi 2021-02-12T23:35:50Z- handleDepth()-Depth_Keller msg: 1613172950971 epoch sec; seqNo:5166; depth 22.58 2021-02-12T23:35:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172950.98 epoch sec; seqNo:5166; 4057370.94 north; 591213.01 east 2021-02-12T23:35:51Z- AHRS_M2 msg: 1613172951.36 epoch sec; seqNo:5119 2021-02-12T23:35:51Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.17 psi 2021-02-12T23:35:51Z- handleDepth()-Depth_Keller msg: 1613172951373 epoch sec; seqNo:5167; depth 22.49 2021-02-12T23:35:51Z- handleDvl() - RDI_Pathfinder msg: 1613172951.39 epoch sec; seqNo:2497 2021-02-12T23:35:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.74 , 0.00 , 99.94 , 67.43 2021-02-12T23:35:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.06 2021-02-12T23:35:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172951.40 epoch sec; seqNo:5167; 4057371.32 north; 591213.00 east 2021-02-12T23:35:51Z- AHRS_M2 msg: 1613172951.77 epoch sec; seqNo:5120 2021-02-12T23:35:51Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.17 psi 2021-02-12T23:35:51Z- handleDepth()-Depth_Keller msg: 1613172951776 epoch sec; seqNo:5168; depth 22.39 2021-02-12T23:35:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172951.79 epoch sec; seqNo:5168; 4057371.32 north; 591213.00 east 2021-02-12T23:35:52Z- AHRS_M2 msg: 1613172952.17 epoch sec; seqNo:5121 2021-02-12T23:35:52Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.17 psi 2021-02-12T23:35:52Z- handleDepth()-Depth_Keller msg: 1613172952177 epoch sec; seqNo:5169; depth 22.26 2021-02-12T23:35:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172952.19 epoch sec; seqNo:5169; 4057371.70 north; 591213.00 east 2021-02-12T23:35:52Z- AHRS_M2 msg: 1613172952.58 epoch sec; seqNo:5122 2021-02-12T23:35:52Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.17 psi 2021-02-12T23:35:52Z- handleDepth()-Depth_Keller msg: 1613172952581 epoch sec; seqNo:5170; depth 22.17 2021-02-12T23:35:52Z- handleDvl() - RDI_Pathfinder msg: 1613172952.60 epoch sec; seqNo:2498 2021-02-12T23:35:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 0.00 , 99.00 , 67.84 2021-02-12T23:35:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.05 2021-02-12T23:35:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172952.61 epoch sec; seqNo:5170; 4057372.07 north; 591213.00 east 2021-02-12T23:35:52Z- AHRS_M2 msg: 1613172952.98 epoch sec; seqNo:5123 2021-02-12T23:35:52Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:52Z- handleDepth()-Depth_Keller msg: 1613172952984 epoch sec; seqNo:5171; depth 22.07 2021-02-12T23:35:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172952.99 epoch sec; seqNo:5171; 4057372.07 north; 591213.00 east 2021-02-12T23:35:53Z- LcmTrn::handleMessages() - handling 45 messages took 0.60 ms and 133 calls... 2021-02-12T23:35:53Z- waiting for 1613172950.75, time is 1613172952.99 2021-02-12T23:35:53Z- LcmTrn::updateTrn() >>>> pose time 1613172952.99, meas time 1613172952.60 <<<< 2021-02-12T23:35:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172952.60 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:35:53Z- TerrainNav::motionUpdate() - currEstimate = (4057372, 591213.00, 22.07) 2021-02-12T23:35:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:35:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:35:53Z- TNavBF::Projecting Measurements 2021-02-12T23:35:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:35:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:35:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:35:53Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:35:53Z- TNavFilter::3 good beams. 2021-02-12T23:35:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:35:53Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:35:53Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:35:53Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:35:53Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:35:53Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:35:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:35:53Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:53Z- TerrainMapOctree: (vehN, vehE ) = (4057371.81, 591212.81). 2021-02-12T23:35:53Z- TerrainMapOctree: Min Distance = 4925.93. 2021-02-12T23:35:53Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:35:53Z- There is no specific good beam that all particles have in common. 2021-02-12T23:35:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:35:53Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-12T23:35:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:35:53Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 15.68 2021-02-12T23:35:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:35:53Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 7.36 2021-02-12T23:35:53Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:35:53Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:35:53Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:35:53Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:35:53Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:35:53Z- Filter 0 No beams used in subcloud update 2021-02-12T23:35:53Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:35:53Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:35:53Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 751.5 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:35:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172952.60 sec. 2021-02-12T23:35:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:35:53Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:53Z- variances([0], [2], and psi_berg) = 751.453948 833.034402 0.000000 2021-02-12T23:35:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:35:53Z- LcmTrn::publishEstimates() - MLE : 4057358.57 591169.50 22.07 2021-02-12T23:35:53Z- LcmTrn::publishEstimates() - MMSE : 4057372.07 591213.00 22.07 2021-02-12T23:35:53Z- LcmTrn::publishEstimates() - COVAR: 751.45 833.03 0.75 0.00 2021-02-12T23:35:53Z- AHRS_M2 msg: 1613172953.38 epoch sec; seqNo:5124 2021-02-12T23:35:53Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:53Z- handleDepth()-Depth_Keller msg: 1613172953393 epoch sec; seqNo:5172; depth 21.96 2021-02-12T23:35:53Z- handleDvl() - RDI_Pathfinder msg: 1613172953.40 epoch sec; seqNo:2499 2021-02-12T23:35:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 89.33 , 96.90 , 68.24 2021-02-12T23:35:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.04 2021-02-12T23:35:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172953.41 epoch sec; seqNo:5172; 4057372.47 north; 591214.21 east 2021-02-12T23:35:53Z- AHRS_M2 msg: 1613172953.79 epoch sec; seqNo:5125 2021-02-12T23:35:53Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:53Z- handleDepth()-Depth_Keller msg: 1613172953805 epoch sec; seqNo:5173; depth 21.86 2021-02-12T23:35:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172953.82 epoch sec; seqNo:5173; 4057372.84 north; 591214.21 east 2021-02-12T23:35:54Z- AHRS_M2 msg: 1613172954.19 epoch sec; seqNo:5126 2021-02-12T23:35:54Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:54Z- handleDepth()-Depth_Keller msg: 1613172954197 epoch sec; seqNo:5174; depth 21.76 2021-02-12T23:35:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172954.22 epoch sec; seqNo:5174; 4057373.22 north; 591214.21 east 2021-02-12T23:35:54Z- AHRS_M2 msg: 1613172954.60 epoch sec; seqNo:5127 2021-02-12T23:35:54Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:54Z- handleDepth()-Depth_Keller msg: 1613172954605 epoch sec; seqNo:5175; depth 21.64 2021-02-12T23:35:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172954.61 epoch sec; seqNo:5175; 4057373.22 north; 591214.21 east 2021-02-12T23:35:55Z- AHRS_M2 msg: 1613172955.00 epoch sec; seqNo:5128 2021-02-12T23:35:55Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:55Z- handleDepth()-Depth_Keller msg: 1613172955009 epoch sec; seqNo:5176; depth 21.54 2021-02-12T23:35:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172955.02 epoch sec; seqNo:5176; 4057373.60 north; 591214.20 east 2021-02-12T23:35:55Z- AHRS_M2 msg: 1613172955.40 epoch sec; seqNo:5129 2021-02-12T23:35:55Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:55Z- handleDepth()-Depth_Keller msg: 1613172955408 epoch sec; seqNo:5177; depth 21.44 2021-02-12T23:35:55Z- handleDvl() - RDI_Pathfinder msg: 1613172955.43 epoch sec; seqNo:2500 2021-02-12T23:35:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.74 , 90.21 , 99.94 , 66.61 2021-02-12T23:35:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.01 2021-02-12T23:35:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172955.44 epoch sec; seqNo:5177; 4057373.98 north; 591214.20 east 2021-02-12T23:35:55Z- AHRS_M2 msg: 1613172955.81 epoch sec; seqNo:5130 2021-02-12T23:35:55Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:55Z- handleDepth()-Depth_Keller msg: 1613172955812 epoch sec; seqNo:5178; depth 21.34 2021-02-12T23:35:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172955.82 epoch sec; seqNo:5178; 4057373.98 north; 591214.20 east 2021-02-12T23:35:56Z- AHRS_M2 msg: 1613172956.21 epoch sec; seqNo:5131 2021-02-12T23:35:56Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:56Z- handleDepth()-Depth_Keller msg: 1613172956221 epoch sec; seqNo:5179; depth 21.21 2021-02-12T23:35:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172956.23 epoch sec; seqNo:5179; 4057374.37 north; 591215.41 east 2021-02-12T23:35:56Z- AHRS_M2 msg: 1613172956.62 epoch sec; seqNo:5132 2021-02-12T23:35:56Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:56Z- handleDepth()-Depth_Keller msg: 1613172956627 epoch sec; seqNo:5180; depth 21.12 2021-02-12T23:35:56Z- handleDvl() - RDI_Pathfinder msg: 1613172956.64 epoch sec; seqNo:2501 2021-02-12T23:35:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 91.09 , 97.17 , 67.43 2021-02-12T23:35:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.07 2021-02-12T23:35:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172956.65 epoch sec; seqNo:5180; 4057374.75 north; 591215.41 east 2021-02-12T23:35:57Z- AHRS_M2 msg: 1613172957.02 epoch sec; seqNo:5133 2021-02-12T23:35:57Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:57Z- handleDepth()-Depth_Keller msg: 1613172957029 epoch sec; seqNo:5181; depth 21.02 2021-02-12T23:35:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172957.04 epoch sec; seqNo:5181; 4057374.75 north; 591215.41 east 2021-02-12T23:35:57Z- AHRS_M2 msg: 1613172957.42 epoch sec; seqNo:5134 2021-02-12T23:35:57Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:57Z- handleDepth()-Depth_Keller msg: 1613172957427 epoch sec; seqNo:5182; depth 20.90 2021-02-12T23:35:57Z- handleDvl() - RDI_Pathfinder msg: 1613172957.44 epoch sec; seqNo:2502 2021-02-12T23:35:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 91.09 , 98.39 , 68.24 2021-02-12T23:35:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , 0.12 2021-02-12T23:35:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172957.45 epoch sec; seqNo:5182; 4057375.13 north; 591215.41 east 2021-02-12T23:35:57Z- AHRS_M2 msg: 1613172957.84 epoch sec; seqNo:5135 2021-02-12T23:35:57Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.17 psi 2021-02-12T23:35:57Z- handleDepth()-Depth_Keller msg: 1613172957840 epoch sec; seqNo:5183; depth 20.80 2021-02-12T23:35:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172957.85 epoch sec; seqNo:5183; 4057375.51 north; 591215.40 east 2021-02-12T23:35:58Z- AHRS_M2 msg: 1613172958.23 epoch sec; seqNo:5136 2021-02-12T23:35:58Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.18 psi 2021-02-12T23:35:58Z- handleDepth()-Depth_Keller msg: 1613172958246 epoch sec; seqNo:5184; depth 20.71 2021-02-12T23:35:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172958.26 epoch sec; seqNo:5184; 4057375.89 north; 591215.40 east 2021-02-12T23:35:58Z- LcmTrn::handleMessages() - handling 43 messages took 0.69 ms and 129 calls... 2021-02-12T23:35:58Z- waiting for 1613172955.99, time is 1613172958.26 2021-02-12T23:35:58Z- LcmTrn::updateTrn() >>>> pose time 1613172958.26, meas time 1613172957.44 <<<< 2021-02-12T23:35:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172957.44 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:35:58Z- TerrainNav::motionUpdate() - currEstimate = (4057376, 591215.40, 20.71) 2021-02-12T23:35:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:35:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:35:58Z- TNavBF::Projecting Measurements 2021-02-12T23:35:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:35:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:35:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:35:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:35:58Z- TNavFilter::4 good beams. 2021-02-12T23:35:58Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:35:58Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:35:58Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:35:58Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:35:58Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:35:58Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:35:58Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:35:58Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:58Z- TerrainMapOctree: (vehN, vehE ) = (4057375.29, 591215.02). 2021-02-12T23:35:58Z- TerrainMapOctree: Min Distance = 4921.83. 2021-02-12T23:35:58Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:35:58Z- There is no specific good beam that all particles have in common. 2021-02-12T23:35:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:35:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 8.66 2021-02-12T23:35:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:35:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 13.28 2021-02-12T23:35:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:35:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 15.49 2021-02-12T23:35:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:35:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-12T23:35:58Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:35:58Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:35:58Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:35:58Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:35:58Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:35:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:35:58Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:35:58Z- Filter 0 No beams used in subcloud update 2021-02-12T23:35:58Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:35:58Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:35:58Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 751.4 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:35:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172957.44 sec. 2021-02-12T23:35:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:35:58Z- TNavBF::mmse selected from filter -1 2021-02-12T23:35:58Z- variances([0], [2], and psi_berg) = 751.315646 833.034271 0.000000 2021-02-12T23:35:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:35:58Z- LcmTrn::publishEstimates() - MLE : 4057362.39 591171.90 20.71 2021-02-12T23:35:58Z- LcmTrn::publishEstimates() - MMSE : 4057375.89 591215.40 20.71 2021-02-12T23:35:58Z- LcmTrn::publishEstimates() - COVAR: 751.32 833.03 0.76 0.00 2021-02-12T23:35:58Z- AHRS_M2 msg: 1613172958.64 epoch sec; seqNo:5137 2021-02-12T23:35:58Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.18 psi 2021-02-12T23:35:58Z- handleDepth()-Depth_Keller msg: 1613172958645 epoch sec; seqNo:5185; depth 20.60 2021-02-12T23:35:58Z- handleDvl() - RDI_Pathfinder msg: 1613172958.66 epoch sec; seqNo:2503 2021-02-12T23:35:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.78 , 92.31 , 99.61 , 69.47 2021-02-12T23:35:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.05 , 0.12 2021-02-12T23:35:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172958.68 epoch sec; seqNo:5185; 4057376.28 north; 591216.62 east 2021-02-12T23:35:59Z- AHRS_M2 msg: 1613172959.04 epoch sec; seqNo:5138 2021-02-12T23:35:59Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.18 psi 2021-02-12T23:35:59Z- handleDepth()-Depth_Keller msg: 1613172959045 epoch sec; seqNo:5186; depth 20.49 2021-02-12T23:35:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172959.06 epoch sec; seqNo:5186; 4057376.28 north; 591216.62 east 2021-02-12T23:35:59Z- AHRS_M2 msg: 1613172959.44 epoch sec; seqNo:5139 2021-02-12T23:35:59Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.18 psi 2021-02-12T23:35:59Z- handleDepth()-Depth_Keller msg: 1613172959448 epoch sec; seqNo:5187; depth 20.37 2021-02-12T23:35:59Z- handleDvl() - RDI_Pathfinder msg: 1613172959.47 epoch sec; seqNo:2504 2021-02-12T23:35:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.79 , 98.25 , 101.90 , 70.29 2021-02-12T23:35:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.05 , 0.08 2021-02-12T23:35:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172959.48 epoch sec; seqNo:5187; 4057376.66 north; 591216.61 east 2021-02-12T23:35:59Z- AHRS_M2 msg: 1613172959.85 epoch sec; seqNo:5140 2021-02-12T23:35:59Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.18 psi 2021-02-12T23:35:59Z- handleDepth()-Depth_Keller msg: 1613172959852 epoch sec; seqNo:5188; depth 20.27 2021-02-12T23:35:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172959.86 epoch sec; seqNo:5188; 4057377.04 north; 591216.61 east 2021-02-12T23:36:00Z- AHRS_M2 msg: 1613172960.25 epoch sec; seqNo:5141 2021-02-12T23:36:00Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-12T23:36:00Z- handleDepth()-Depth_Keller msg: 1613172960256 epoch sec; seqNo:5189; depth 20.16 2021-02-12T23:36:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172960.27 epoch sec; seqNo:5189; 4057377.42 north; 591216.60 east 2021-02-12T23:36:00Z- AHRS_M2 msg: 1613172960.66 epoch sec; seqNo:5142 2021-02-12T23:36:00Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-12T23:36:00Z- handleDepth()-Depth_Keller msg: 1613172960661 epoch sec; seqNo:5190; depth 20.07 2021-02-12T23:36:00Z- handleDvl() - RDI_Pathfinder msg: 1613172960.67 epoch sec; seqNo:2505 2021-02-12T23:36:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.01 , 91.90 , 101.63 , 69.47 2021-02-12T23:36:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.10 , 0.01 2021-02-12T23:36:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172960.68 epoch sec; seqNo:5190; 4057377.43 north; 591217.82 east 2021-02-12T23:36:01Z- AHRS_M2 msg: 1613172961.06 epoch sec; seqNo:5143 2021-02-12T23:36:01Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.16 psi 2021-02-12T23:36:01Z- handleDepth()-Depth_Keller msg: 1613172961085 epoch sec; seqNo:5191; depth 19.95 2021-02-12T23:36:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172961.10 epoch sec; seqNo:5191; 4057377.81 north; 591217.82 east 2021-02-12T23:36:01Z- AHRS_M2 msg: 1613172961.46 epoch sec; seqNo:5144 2021-02-12T23:36:01Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.16 psi 2021-02-12T23:36:01Z- handleDepth()-Depth_Keller msg: 1613172961468 epoch sec; seqNo:5192; depth 19.85 2021-02-12T23:36:01Z- handleDvl() - RDI_Pathfinder msg: 1613172961.48 epoch sec; seqNo:2506 2021-02-12T23:36:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.42 , 92.10 , 103.05 , 69.88 2021-02-12T23:36:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.10 , -0.05 2021-02-12T23:36:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172961.50 epoch sec; seqNo:5192; 4057378.19 north; 591217.82 east 2021-02-12T23:36:01Z- AHRS_M2 msg: 1613172961.87 epoch sec; seqNo:5145 2021-02-12T23:36:01Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-12T23:36:01Z- handleDepth()-Depth_Keller msg: 1613172961872 epoch sec; seqNo:5193; depth 19.74 2021-02-12T23:36:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172961.88 epoch sec; seqNo:5193; 4057378.57 north; 591217.81 east 2021-02-12T23:36:02Z- AHRS_M2 msg: 1613172962.28 epoch sec; seqNo:5146 2021-02-12T23:36:02Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.14 psi 2021-02-12T23:36:02Z- handleDepth()-Depth_Keller msg: 1613172962294 epoch sec; seqNo:5194; depth 19.64 2021-02-12T23:36:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172962.34 epoch sec; seqNo:5194; 4057378.57 north; 591217.81 east 2021-02-12T23:36:02Z- AHRS_M2 msg: 1613172962.68 epoch sec; seqNo:5147 2021-02-12T23:36:02Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.14 psi 2021-02-12T23:36:02Z- handleDepth()-Depth_Keller msg: 1613172962680 epoch sec; seqNo:5195; depth 19.52 2021-02-12T23:36:02Z- handleDvl() - RDI_Pathfinder msg: 1613172962.70 epoch sec; seqNo:2507 2021-02-12T23:36:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 98.19 , 103.05 , 69.06 2021-02-12T23:36:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.13 , -0.10 2021-02-12T23:36:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172962.71 epoch sec; seqNo:5195; 4057378.96 north; 591219.03 east 2021-02-12T23:36:03Z- AHRS_M2 msg: 1613172963.08 epoch sec; seqNo:5148 2021-02-12T23:36:03Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-12T23:36:03Z- handleDepth()-Depth_Keller msg: 1613172963084 epoch sec; seqNo:5196; depth 19.41 2021-02-12T23:36:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172963.10 epoch sec; seqNo:5196; 4057378.96 north; 591219.03 east 2021-02-12T23:36:03Z- LcmTrn::handleMessages() - handling 41 messages took 0.60 ms and 129 calls... 2021-02-12T23:36:03Z- waiting for 1613172961.26, time is 1613172963.10 2021-02-12T23:36:03Z- LcmTrn::updateTrn() >>>> pose time 1613172963.10, meas time 1613172962.70 <<<< 2021-02-12T23:36:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172962.70 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:36:03Z- TerrainNav::motionUpdate() - currEstimate = (4057379, 591219.03, 19.41) 2021-02-12T23:36:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:36:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:36:03Z- TNavBF::Projecting Measurements 2021-02-12T23:36:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:36:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:36:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:36:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:36:03Z- TNavFilter::4 good beams. 2021-02-12T23:36:03Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:36:03Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:36:03Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:36:03Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:36:03Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:36:03Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:36:03Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:36:03Z- TNavBF::mmse selected from filter -1 2021-02-12T23:36:03Z- TerrainMapOctree: (vehN, vehE ) = (4057378.71, 591218.73). 2021-02-12T23:36:03Z- TerrainMapOctree: Min Distance = 4916.86. 2021-02-12T23:36:03Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:36:03Z- There is no specific good beam that all particles have in common. 2021-02-12T23:36:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:36:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 9.44 2021-02-12T23:36:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:36:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 15.43 2021-02-12T23:36:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:36:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 16.99 2021-02-12T23:36:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:36:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-12T23:36:03Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:36:03Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:36:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:36:03Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:36:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:36:03Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:36:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:36:03Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:36:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:36:03Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:36:03Z- Filter 0 No beams used in subcloud update 2021-02-12T23:36:03Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:36:03Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:36:03Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 751.2 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:36:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172962.70 sec. 2021-02-12T23:36:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:36:03Z- TNavBF::mmse selected from filter -1 2021-02-12T23:36:03Z- variances([0], [2], and psi_berg) = 751.181315 833.034144 0.000000 2021-02-12T23:36:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:36:03Z- LcmTrn::publishEstimates() - MLE : 4057364.46 591177.53 19.41 2021-02-12T23:36:03Z- LcmTrn::publishEstimates() - MMSE : 4057378.96 591219.03 19.41 2021-02-12T23:36:03Z- LcmTrn::publishEstimates() - COVAR: 751.18 833.03 0.77 0.00 2021-02-12T23:36:03Z- AHRS_M2 msg: 1613172963.48 epoch sec; seqNo:5149 2021-02-12T23:36:03Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-12T23:36:03Z- handleDepth()-Depth_Keller msg: 1613172963514 epoch sec; seqNo:5197; depth 19.31 2021-02-12T23:36:03Z- handleDvl() - RDI_Pathfinder msg: 1613172963.53 epoch sec; seqNo:2508 2021-02-12T23:36:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 91.96 , 104.13 , 69.48 2021-02-12T23:36:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , -0.10 2021-02-12T23:36:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172963.56 epoch sec; seqNo:5197; 4057379.34 north; 591219.02 east 2021-02-12T23:36:03Z- AHRS_M2 msg: 1613172963.89 epoch sec; seqNo:5150 2021-02-12T23:36:03Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-12T23:36:03Z- handleDepth()-Depth_Keller msg: 1613172963901 epoch sec; seqNo:5198; depth 19.19 2021-02-12T23:36:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172963.92 epoch sec; seqNo:5198; 4057379.72 north; 591219.02 east 2021-02-12T23:36:04Z- AHRS_M2 msg: 1613172964.32 epoch sec; seqNo:5151 2021-02-12T23:36:04Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.21 psi 2021-02-12T23:36:04Z- handleDepth()-Depth_Keller msg: 1613172964320 epoch sec; seqNo:5199; depth 19.09 2021-02-12T23:36:04Z- handleDvl() - RDI_Pathfinder msg: 1613172964.33 epoch sec; seqNo:2509 2021-02-12T23:36:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.02 , 98.12 , 102.98 , 70.70 2021-02-12T23:36:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.07 2021-02-12T23:36:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172964.34 epoch sec; seqNo:5199; 4057380.09 north; 591219.02 east 2021-02-12T23:36:04Z- AHRS_M2 msg: 1613172964.70 epoch sec; seqNo:5152 2021-02-12T23:36:04Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.21 psi 2021-02-12T23:36:04Z- handleDepth()-Depth_Keller msg: 1613172964705 epoch sec; seqNo:5200; depth 18.97 2021-02-12T23:36:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172964.72 epoch sec; seqNo:5200; 4057380.09 north; 591219.02 east 2021-02-12T23:36:05Z- AHRS_M2 msg: 1613172965.10 epoch sec; seqNo:5153 2021-02-12T23:36:05Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.22 psi 2021-02-12T23:36:05Z- handleDepth()-Depth_Keller msg: 1613172965108 epoch sec; seqNo:5201; depth 18.87 2021-02-12T23:36:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172965.12 epoch sec; seqNo:5201; 4057380.47 north; 591219.01 east 2021-02-12T23:36:05Z- AHRS_M2 msg: 1613172965.51 epoch sec; seqNo:5154 2021-02-12T23:36:05Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.22 psi 2021-02-12T23:36:05Z- handleDepth()-Depth_Keller msg: 1613172965509 epoch sec; seqNo:5202; depth 18.76 2021-02-12T23:36:05Z- handleDvl() - RDI_Pathfinder msg: 1613172965.52 epoch sec; seqNo:2510 2021-02-12T23:36:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.19 , 96.83 , 105.35 , 71.51 2021-02-12T23:36:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , -0.03 2021-02-12T23:36:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172965.53 epoch sec; seqNo:5202; 4057380.87 north; 591220.23 east 2021-02-12T23:36:05Z- AHRS_M2 msg: 1613172965.91 epoch sec; seqNo:5155 2021-02-12T23:36:05Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.22 psi 2021-02-12T23:36:05Z- handleDepth()-Depth_Keller msg: 1613172965912 epoch sec; seqNo:5203; depth 18.65 2021-02-12T23:36:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172965.93 epoch sec; seqNo:5203; 4057380.87 north; 591220.23 east 2021-02-12T23:36:06Z- AHRS_M2 msg: 1613172966.31 epoch sec; seqNo:5156 2021-02-12T23:36:06Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.22 psi 2021-02-12T23:36:06Z- handleDepth()-Depth_Keller msg: 1613172966320 epoch sec; seqNo:5204; depth 18.55 2021-02-12T23:36:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172966.33 epoch sec; seqNo:5204; 4057381.24 north; 591220.23 east 2021-02-12T23:36:06Z- AHRS_M2 msg: 1613172966.72 epoch sec; seqNo:5157 2021-02-12T23:36:06Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.22 psi 2021-02-12T23:36:06Z- handleDepth()-Depth_Keller msg: 1613172966720 epoch sec; seqNo:5205; depth 18.44 2021-02-12T23:36:06Z- handleDvl() - RDI_Pathfinder msg: 1613172966.73 epoch sec; seqNo:2511 2021-02-12T23:36:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.42 , 96.77 , 105.28 , 70.29 2021-02-12T23:36:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.02 , -0.01 2021-02-12T23:36:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172966.74 epoch sec; seqNo:5205; 4057381.62 north; 591220.22 east 2021-02-12T23:36:07Z- AHRS_M2 msg: 1613172967.12 epoch sec; seqNo:5158 2021-02-12T23:36:07Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.21 psi 2021-02-12T23:36:07Z- handleDepth()-Depth_Keller msg: 1613172967134 epoch sec; seqNo:5206; depth 18.31 2021-02-12T23:36:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172967.15 epoch sec; seqNo:5206; 4057382.00 north; 591220.22 east 2021-02-12T23:36:07Z- AHRS_M2 msg: 1613172967.52 epoch sec; seqNo:5159 2021-02-12T23:36:07Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.20 psi 2021-02-12T23:36:07Z- handleDepth()-Depth_Keller msg: 1613172967528 epoch sec; seqNo:5207; depth 18.21 2021-02-12T23:36:07Z- handleDvl() - RDI_Pathfinder msg: 1613172967.54 epoch sec; seqNo:2512 2021-02-12T23:36:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 96.77 , 107.72 , 71.51 2021-02-12T23:36:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.02 2021-02-12T23:36:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172967.56 epoch sec; seqNo:5207; 4057382.00 north; 591220.22 east 2021-02-12T23:36:07Z- AHRS_M2 msg: 1613172967.93 epoch sec; seqNo:5160 2021-02-12T23:36:07Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.20 psi 2021-02-12T23:36:07Z- handleDepth()-Depth_Keller msg: 1613172967932 epoch sec; seqNo:5208; depth 18.10 2021-02-12T23:36:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172967.94 epoch sec; seqNo:5208; 4057382.38 north; 591220.21 east 2021-02-12T23:36:08Z- AHRS_M2 msg: 1613172968.34 epoch sec; seqNo:5161 2021-02-12T23:36:08Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.19 psi 2021-02-12T23:36:08Z- handleDepth()-Depth_Keller msg: 1613172968353 epoch sec; seqNo:5209; depth 17.98 2021-02-12T23:36:08Z- handleDvl() - RDI_Pathfinder msg: 1613172968.39 epoch sec; seqNo:2513 2021-02-12T23:36:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.64 , 96.70 , 107.65 , 71.92 2021-02-12T23:36:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , -0.02 2021-02-12T23:36:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172968.40 epoch sec; seqNo:5209; 4057382.76 north; 591220.21 east 2021-02-12T23:36:08Z- LcmTrn::handleMessages() - handling 45 messages took 0.72 ms and 131 calls... 2021-02-12T23:36:08Z- waiting for 1613172966.10, time is 1613172968.40 2021-02-12T23:36:08Z- LcmTrn::updateTrn() equating pose 1613172968.40 and meas times 1613172968.40 2021-02-12T23:36:08Z- LcmTrn::updateTrn() >>>> pose time 1613172968.40, meas time 1613172968.40 <<<< 2021-02-12T23:36:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172968.40 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:36:08Z- TerrainNav::motionUpdate() - currEstimate = (4057383, 591220.21, 17.98) 2021-02-12T23:36:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:36:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:36:08Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172968.40 sec.; No relevant navigation data available 2021-02-12T23:36:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:36:08Z- TNavBF::mmse selected from filter -1 2021-02-12T23:36:08Z- variances([0], [2], and psi_berg) = 751.082680 833.034051 0.000000 2021-02-12T23:36:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:36:08Z- LcmTrn::publishEstimates() - MLE : 4057368.26 591180.71 17.98 2021-02-12T23:36:08Z- LcmTrn::publishEstimates() - MMSE : 4057382.76 591220.21 17.98 2021-02-12T23:36:08Z- LcmTrn::publishEstimates() - COVAR: 751.08 833.03 0.98 0.00 2021-02-12T23:36:08Z- AHRS_M2 msg: 1613172968.74 epoch sec; seqNo:5162 2021-02-12T23:36:08Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.19 psi 2021-02-12T23:36:08Z- handleDepth()-Depth_Keller msg: 1613172968740 epoch sec; seqNo:5210; depth 17.86 2021-02-12T23:36:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172968.76 epoch sec; seqNo:5210; 4057382.77 north; 591221.43 east 2021-02-12T23:36:09Z- AHRS_M2 msg: 1613172969.14 epoch sec; seqNo:5163 2021-02-12T23:36:09Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-12T23:36:09Z- handleDepth()-Depth_Keller msg: 1613172969148 epoch sec; seqNo:5211; depth 17.76 2021-02-12T23:36:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172969.16 epoch sec; seqNo:5211; 4057383.15 north; 591221.43 east 2021-02-12T23:36:09Z- AHRS_M2 msg: 1613172969.54 epoch sec; seqNo:5164 2021-02-12T23:36:09Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-12T23:36:09Z- handleDepth()-Depth_Keller msg: 1613172969548 epoch sec; seqNo:5212; depth 17.62 2021-02-12T23:36:09Z- handleDvl() - RDI_Pathfinder msg: 1613172969.56 epoch sec; seqNo:2514 2021-02-12T23:36:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 97.92 , 107.65 , 71.11 2021-02-12T23:36:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.11 , 0.03 2021-02-12T23:36:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172969.57 epoch sec; seqNo:5212; 4057383.53 north; 591221.42 east 2021-02-12T23:36:09Z- AHRS_M2 msg: 1613172969.95 epoch sec; seqNo:5165 2021-02-12T23:36:09Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-12T23:36:09Z- handleDepth()-Depth_Keller msg: 1613172969954 epoch sec; seqNo:5213; depth 17.53 2021-02-12T23:36:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172969.96 epoch sec; seqNo:5213; 4057383.91 north; 591221.42 east 2021-02-12T23:36:10Z- AHRS_M2 msg: 1613172970.35 epoch sec; seqNo:5166 2021-02-12T23:36:10Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.17 psi 2021-02-12T23:36:10Z- handleDepth()-Depth_Keller msg: 1613172970357 epoch sec; seqNo:5214; depth 17.40 2021-02-12T23:36:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172970.38 epoch sec; seqNo:5214; 4057383.91 north; 591221.42 east 2021-02-12T23:36:10Z- AHRS_M2 msg: 1613172970.76 epoch sec; seqNo:5167 2021-02-12T23:36:10Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.17 psi 2021-02-12T23:36:10Z- handleDepth()-Depth_Keller msg: 1613172970765 epoch sec; seqNo:5215; depth 17.28 2021-02-12T23:36:10Z- handleDvl() - RDI_Pathfinder msg: 1613172970.78 epoch sec; seqNo:2515 2021-02-12T23:36:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.67 , 95.48 , 106.43 , 71.11 2021-02-12T23:36:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.17 , 0.13 2021-02-12T23:36:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172970.80 epoch sec; seqNo:5215; 4057384.29 north; 591221.41 east 2021-02-12T23:36:11Z- AHRS_M2 msg: 1613172971.16 epoch sec; seqNo:5168 2021-02-12T23:36:11Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.16 psi 2021-02-12T23:36:11Z- handleDepth()-Depth_Keller msg: 1613172971168 epoch sec; seqNo:5216; depth 17.15 2021-02-12T23:36:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172971.18 epoch sec; seqNo:5216; 4057384.67 north; 591221.41 east 2021-02-12T23:36:11Z- AHRS_M2 msg: 1613172971.56 epoch sec; seqNo:5169 2021-02-12T23:36:11Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.16 psi 2021-02-12T23:36:11Z- handleDepth()-Depth_Keller msg: 1613172971577 epoch sec; seqNo:5217; depth 17.04 2021-02-12T23:36:11Z- handleDvl() - RDI_Pathfinder msg: 1613172971.59 epoch sec; seqNo:2516 2021-02-12T23:36:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.28 , 95.48 , 112.52 , 71.51 2021-02-12T23:36:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.11 , 0.24 2021-02-12T23:36:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172971.60 epoch sec; seqNo:5217; 4057385.05 north; 591221.41 east 2021-02-12T23:36:11Z- AHRS_M2 msg: 1613172971.97 epoch sec; seqNo:5170 2021-02-12T23:36:11Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.16 psi 2021-02-12T23:36:11Z- handleDepth()-Depth_Keller msg: 1613172971972 epoch sec; seqNo:5218; depth 16.92 2021-02-12T23:36:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172971.99 epoch sec; seqNo:5218; 4057385.44 north; 591222.62 east 2021-02-12T23:36:12Z- AHRS_M2 msg: 1613172972.37 epoch sec; seqNo:5171 2021-02-12T23:36:12Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.16 psi 2021-02-12T23:36:12Z- handleDepth()-Depth_Keller msg: 1613172972376 epoch sec; seqNo:5219; depth 16.82 2021-02-12T23:36:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172972.39 epoch sec; seqNo:5219; 4057385.82 north; 591222.62 east 2021-02-12T23:36:12Z- AHRS_M2 msg: 1613172972.78 epoch sec; seqNo:5172 2021-02-12T23:36:12Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.16 psi 2021-02-12T23:36:12Z- handleDepth()-Depth_Keller msg: 1613172972785 epoch sec; seqNo:5220; depth 16.68 2021-02-12T23:36:12Z- handleDvl() - RDI_Pathfinder msg: 1613172972.80 epoch sec; seqNo:2517 2021-02-12T23:36:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 95.35 , 108.73 , 72.74 2021-02-12T23:36:12Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.03 , 0.23 2021-02-12T23:36:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172972.81 epoch sec; seqNo:5220; 4057386.20 north; 591222.62 east 2021-02-12T23:36:13Z- AHRS_M2 msg: 1613172973.18 epoch sec; seqNo:5173 2021-02-12T23:36:13Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.16 psi 2021-02-12T23:36:13Z- handleDepth()-Depth_Keller msg: 1613172973193 epoch sec; seqNo:5221; depth 16.57 2021-02-12T23:36:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172973.20 epoch sec; seqNo:5221; 4057386.57 north; 591222.61 east 2021-02-12T23:36:13Z- LcmTrn::handleMessages() - handling 40 messages took 0.64 ms and 128 calls... 2021-02-12T23:36:13Z- waiting for 1613172971.40, time is 1613172973.20 2021-02-12T23:36:13Z- LcmTrn::updateTrn() >>>> pose time 1613172973.20, meas time 1613172972.80 <<<< 2021-02-12T23:36:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172972.80 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:36:13Z- TerrainNav::motionUpdate() - currEstimate = (4057387, 591222.61, 16.57) 2021-02-12T23:36:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:36:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:36:13Z- TNavBF::Projecting Measurements 2021-02-12T23:36:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:36:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:36:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:36:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:36:13Z- TNavFilter::4 good beams. 2021-02-12T23:36:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:36:13Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:36:13Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:36:13Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:36:13Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:36:13Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:36:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:36:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:36:13Z- TerrainMapOctree: (vehN, vehE ) = (4057386.25, 591222.41). 2021-02-12T23:36:13Z- TerrainMapOctree: Min Distance = 4908.65. 2021-02-12T23:36:13Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:36:13Z- There is no specific good beam that all particles have in common. 2021-02-12T23:36:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:36:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 10.75 2021-02-12T23:36:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:36:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 14.55 2021-02-12T23:36:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:36:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 18.92 2021-02-12T23:36:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:36:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 8.47 2021-02-12T23:36:13Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:36:13Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:36:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:36:13Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:36:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:36:13Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:36:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:36:13Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:36:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:36:13Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:36:13Z- Filter 0 No beams used in subcloud update 2021-02-12T23:36:13Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:36:13Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:36:13Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 751.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:36:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172972.80 sec. 2021-02-12T23:36:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:36:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:36:13Z- variances([0], [2], and psi_berg) = 750.951930 833.033928 0.000000 2021-02-12T23:36:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:36:13Z- LcmTrn::publishEstimates() - MLE : 4057372.07 591272.11 16.57 2021-02-12T23:36:13Z- LcmTrn::publishEstimates() - MMSE : 4057386.57 591222.61 16.57 2021-02-12T23:36:13Z- LcmTrn::publishEstimates() - COVAR: 750.95 833.03 0.90 0.00 2021-02-12T23:36:13Z- AHRS_M2 msg: 1613172973.58 epoch sec; seqNo:5174 2021-02-12T23:36:13Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.15 psi 2021-02-12T23:36:13Z- handleDepth()-Depth_Keller msg: 1613172973588 epoch sec; seqNo:5222; depth 16.46 2021-02-12T23:36:13Z- handleDvl() - RDI_Pathfinder msg: 1613172973.61 epoch sec; seqNo:2518 2021-02-12T23:36:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 97.78 , 111.17 , 71.92 2021-02-12T23:36:13Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.13 , 0.10 2021-02-12T23:36:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172973.62 epoch sec; seqNo:5222; 4057386.57 north; 591222.61 east 2021-02-12T23:36:13Z- AHRS_M2 msg: 1613172973.99 epoch sec; seqNo:5175 2021-02-12T23:36:13Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.14 psi 2021-02-12T23:36:13Z- handleDepth()-Depth_Keller msg: 1613172973992 epoch sec; seqNo:5223; depth 16.34 2021-02-12T23:36:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172974.00 epoch sec; seqNo:5223; 4057386.97 north; 591223.83 east 2021-02-12T23:36:14Z- AHRS_M2 msg: 1613172974.39 epoch sec; seqNo:5176 2021-02-12T23:36:14Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.13 psi 2021-02-12T23:36:14Z- handleDepth()-Depth_Keller msg: 1613172974400 epoch sec; seqNo:5224; depth 16.23 2021-02-12T23:36:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172974.42 epoch sec; seqNo:5224; 4057387.34 north; 591223.82 east 2021-02-12T23:36:14Z- AHRS_M2 msg: 1613172974.80 epoch sec; seqNo:5177 2021-02-12T23:36:14Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.13 psi 2021-02-12T23:36:14Z- handleDepth()-Depth_Keller msg: 1613172974809 epoch sec; seqNo:5225; depth 16.12 2021-02-12T23:36:14Z- handleDvl() - RDI_Pathfinder msg: 1613172974.82 epoch sec; seqNo:2519 2021-02-12T23:36:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 95.28 , 111.10 , 71.51 2021-02-12T23:36:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.16 , -0.09 2021-02-12T23:36:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172974.83 epoch sec; seqNo:5225; 4057387.72 north; 591223.82 east 2021-02-12T23:36:15Z- AHRS_M2 msg: 1613172975.20 epoch sec; seqNo:5178 2021-02-12T23:36:15Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.12 psi 2021-02-12T23:36:15Z- handleDepth()-Depth_Keller msg: 1613172975214 epoch sec; seqNo:5226; depth 15.97 2021-02-12T23:36:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172975.23 epoch sec; seqNo:5226; 4057387.72 north; 591223.82 east 2021-02-12T23:36:15Z- AHRS_M2 msg: 1613172975.60 epoch sec; seqNo:5179 2021-02-12T23:36:15Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.12 psi 2021-02-12T23:36:15Z- handleDepth()-Depth_Keller msg: 1613172975613 epoch sec; seqNo:5227; depth 15.87 2021-02-12T23:36:15Z- handleDvl() - RDI_Pathfinder msg: 1613172975.62 epoch sec; seqNo:2520 2021-02-12T23:36:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 95.28 , 112.32 , 71.11 2021-02-12T23:36:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.04 , -0.22 2021-02-12T23:36:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172975.64 epoch sec; seqNo:5227; 4057388.10 north; 591223.82 east 2021-02-12T23:36:16Z- AHRS_M2 msg: 1613172976.01 epoch sec; seqNo:5180 2021-02-12T23:36:16Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.12 psi 2021-02-12T23:36:16Z- handleDepth()-Depth_Keller msg: 1613172976013 epoch sec; seqNo:5228; depth 15.75 2021-02-12T23:36:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172976.02 epoch sec; seqNo:5228; 4057388.10 north; 591223.82 east 2021-02-12T23:36:16Z- AHRS_M2 msg: 1613172976.41 epoch sec; seqNo:5181 2021-02-12T23:36:16Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.12 psi 2021-02-12T23:36:16Z- handleDepth()-Depth_Keller msg: 1613172976418 epoch sec; seqNo:5229; depth 15.63 2021-02-12T23:36:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172976.43 epoch sec; seqNo:5229; 4057388.49 north; 591225.03 east 2021-02-12T23:36:16Z- AHRS_M2 msg: 1613172976.82 epoch sec; seqNo:5182 2021-02-12T23:36:16Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.11 psi 2021-02-12T23:36:16Z- handleDepth()-Depth_Keller msg: 1613172976820 epoch sec; seqNo:5230; depth 15.50 2021-02-12T23:36:16Z- handleDvl() - RDI_Pathfinder msg: 1613172976.84 epoch sec; seqNo:2521 2021-02-12T23:36:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.13 , 94.94 , 109.81 , 78.72 2021-02-12T23:36:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.08 , -0.17 2021-02-12T23:36:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172976.85 epoch sec; seqNo:5230; 4057388.87 north; 591225.03 east 2021-02-12T23:36:17Z- AHRS_M2 msg: 1613172977.22 epoch sec; seqNo:5183 2021-02-12T23:36:17Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.11 psi 2021-02-12T23:36:17Z- handleDepth()-Depth_Keller msg: 1613172977229 epoch sec; seqNo:5231; depth 15.39 2021-02-12T23:36:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172977.24 epoch sec; seqNo:5231; 4057388.87 north; 591225.03 east 2021-02-12T23:36:17Z- AHRS_M2 msg: 1613172977.62 epoch sec; seqNo:5184 2021-02-12T23:36:17Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.12 psi 2021-02-12T23:36:17Z- handleDepth()-Depth_Keller msg: 1613172977633 epoch sec; seqNo:5232; depth 15.25 2021-02-12T23:36:17Z- handleDvl() - RDI_Pathfinder msg: 1613172977.64 epoch sec; seqNo:2522 2021-02-12T23:36:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 93.66 , 107.18 , 71.92 2021-02-12T23:36:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.13 , -0.05 2021-02-12T23:36:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172977.65 epoch sec; seqNo:5232; 4057389.25 north; 591225.02 east 2021-02-12T23:36:18Z- AHRS_M2 msg: 1613172978.03 epoch sec; seqNo:5185 2021-02-12T23:36:18Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.13 psi 2021-02-12T23:36:18Z- handleDepth()-Depth_Keller msg: 1613172978038 epoch sec; seqNo:5233; depth 15.12 2021-02-12T23:36:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172978.05 epoch sec; seqNo:5233; 4057389.63 north; 591225.02 east 2021-02-12T23:36:18Z- AHRS_M2 msg: 1613172978.43 epoch sec; seqNo:5186 2021-02-12T23:36:18Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.13 psi 2021-02-12T23:36:18Z- handleDepth()-Depth_Keller msg: 1613172978436 epoch sec; seqNo:5234; depth 15.01 2021-02-12T23:36:18Z- handleDvl() - RDI_Pathfinder msg: 1613172978.46 epoch sec; seqNo:2523 2021-02-12T23:36:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 95.42 , 108.80 , 71.92 2021-02-12T23:36:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.08 , 0.06 2021-02-12T23:36:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172978.47 epoch sec; seqNo:5234; 4057390.01 north; 591225.02 east 2021-02-12T23:36:18Z- LcmTrn::handleMessages() - handling 45 messages took 0.68 ms and 132 calls... 2021-02-12T23:36:18Z- waiting for 1613172976.20, time is 1613172978.47 2021-02-12T23:36:18Z- LcmTrn::updateTrn() equating pose 1613172978.47 and meas times 1613172978.47 2021-02-12T23:36:18Z- LcmTrn::updateTrn() >>>> pose time 1613172978.47, meas time 1613172978.47 <<<< 2021-02-12T23:36:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172978.47 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:36:18Z- TerrainNav::motionUpdate() - currEstimate = (4057390, 591225.02, 15.01) 2021-02-12T23:36:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:36:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:36:18Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172978.47 sec.; No relevant navigation data available 2021-02-12T23:36:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:36:18Z- TNavBF::mmse selected from filter -1 2021-02-12T23:36:18Z- variances([0], [2], and psi_berg) = 750.850994 833.033833 0.000000 2021-02-12T23:36:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:36:18Z- LcmTrn::publishEstimates() - MLE : 4057375.51 591274.52 15.01 2021-02-12T23:36:18Z- LcmTrn::publishEstimates() - MMSE : 4057390.01 591225.02 15.01 2021-02-12T23:36:18Z- LcmTrn::publishEstimates() - COVAR: 750.85 833.03 1.12 0.00 2021-02-12T23:36:18Z- AHRS_M2 msg: 1613172978.84 epoch sec; seqNo:5187 2021-02-12T23:36:18Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.14 psi 2021-02-12T23:36:18Z- handleDepth()-Depth_Keller msg: 1613172978845 epoch sec; seqNo:5235; depth 14.87 2021-02-12T23:36:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172978.86 epoch sec; seqNo:5235; 4057390.01 north; 591225.02 east 2021-02-12T23:36:19Z- AHRS_M2 msg: 1613172979.24 epoch sec; seqNo:5188 2021-02-12T23:36:19Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.14 psi 2021-02-12T23:36:19Z- handleDepth()-Depth_Keller msg: 1613172979253 epoch sec; seqNo:5236; depth 14.76 2021-02-12T23:36:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172979.26 epoch sec; seqNo:5236; 4057390.39 north; 591225.01 east 2021-02-12T23:36:19Z- AHRS_M2 msg: 1613172979.64 epoch sec; seqNo:5189 2021-02-12T23:36:19Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.13 psi 2021-02-12T23:36:19Z- handleDepth()-Depth_Keller msg: 1613172979654 epoch sec; seqNo:5237; depth 14.64 2021-02-12T23:36:19Z- handleDvl() - RDI_Pathfinder msg: 1613172979.68 epoch sec; seqNo:2524 2021-02-12T23:36:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 97.71 , 109.88 , 78.79 2021-02-12T23:36:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.01 , 0.04 2021-02-12T23:36:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172979.72 epoch sec; seqNo:5237; 4057390.78 north; 591226.23 east 2021-02-12T23:36:20Z- AHRS_M2 msg: 1613172980.05 epoch sec; seqNo:5190 2021-02-12T23:36:20Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.13 psi 2021-02-12T23:36:20Z- handleDepth()-Depth_Keller msg: 1613172980052 epoch sec; seqNo:5238; depth 14.52 2021-02-12T23:36:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172980.06 epoch sec; seqNo:5238; 4057390.78 north; 591226.23 east 2021-02-12T23:36:20Z- AHRS_M2 msg: 1613172980.45 epoch sec; seqNo:5191 2021-02-12T23:36:20Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.13 psi 2021-02-12T23:36:20Z- handleDepth()-Depth_Keller msg: 1613172980458 epoch sec; seqNo:5239; depth 14.41 2021-02-12T23:36:20Z- handleDvl() - RDI_Pathfinder msg: 1613172980.47 epoch sec; seqNo:2525 2021-02-12T23:36:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.48 , 94.94 , 111.17 , 72.74 2021-02-12T23:36:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.02 , -0.06 2021-02-12T23:36:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172980.50 epoch sec; seqNo:5239; 4057391.16 north; 591226.23 east 2021-02-12T23:36:20Z- AHRS_M2 msg: 1613172980.86 epoch sec; seqNo:5192 2021-02-12T23:36:20Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.14 psi 2021-02-12T23:36:20Z- handleDepth()-Depth_Keller msg: 1613172980861 epoch sec; seqNo:5240; depth 14.28 2021-02-12T23:36:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172980.87 epoch sec; seqNo:5240; 4057391.54 north; 591226.22 east 2021-02-12T23:36:21Z- AHRS_M2 msg: 1613172981.26 epoch sec; seqNo:5193 2021-02-12T23:36:21Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.14 psi 2021-02-12T23:36:21Z- handleDepth()-Depth_Keller msg: 1613172981275 epoch sec; seqNo:5241; depth 14.19 2021-02-12T23:36:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172981.29 epoch sec; seqNo:5241; 4057391.54 north; 591226.22 east 2021-02-12T23:36:21Z- AHRS_M2 msg: 1613172981.66 epoch sec; seqNo:5194 2021-02-12T23:36:21Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-12T23:36:21Z- handleDepth()-Depth_Keller msg: 1613172981672 epoch sec; seqNo:5242; depth 14.06 2021-02-12T23:36:21Z- handleDvl() - RDI_Pathfinder msg: 1613172981.68 epoch sec; seqNo:2526 2021-02-12T23:36:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.26 , 97.65 , 109.81 , 74.37 2021-02-12T23:36:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.12 , -0.09 2021-02-12T23:36:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172981.70 epoch sec; seqNo:5242; 4057391.92 north; 591226.22 east 2021-02-12T23:36:22Z- AHRS_M2 msg: 1613172982.07 epoch sec; seqNo:5195 2021-02-12T23:36:22Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:36:22Z- handleDepth()-Depth_Keller msg: 1613172982078 epoch sec; seqNo:5243; depth 13.94 2021-02-12T23:36:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172982.09 epoch sec; seqNo:5243; 4057392.30 north; 591226.21 east 2021-02-12T23:36:22Z- AHRS_M2 msg: 1613172982.48 epoch sec; seqNo:5196 2021-02-12T23:36:22Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-12T23:36:22Z- handleDepth()-Depth_Keller msg: 1613172982491 epoch sec; seqNo:5244; depth 13.84 2021-02-12T23:36:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172982.52 epoch sec; seqNo:5244; 4057392.31 north; 591227.43 east 2021-02-12T23:36:22Z- AHRS_M2 msg: 1613172982.88 epoch sec; seqNo:5197 2021-02-12T23:36:22Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.19 psi 2021-02-12T23:36:22Z- handleDepth()-Depth_Keller msg: 1613172982880 epoch sec; seqNo:5245; depth 13.71 2021-02-12T23:36:22Z- handleDvl() - RDI_Pathfinder msg: 1613172982.90 epoch sec; seqNo:2527 2021-02-12T23:36:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 96.43 , 111.03 , 78.18 2021-02-12T23:36:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.24 , 0.05 2021-02-12T23:36:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172982.91 epoch sec; seqNo:5245; 4057392.69 north; 591227.43 east 2021-02-12T23:36:23Z- AHRS_M2 msg: 1613172983.28 epoch sec; seqNo:5198 2021-02-12T23:36:23Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.20 psi 2021-02-12T23:36:23Z- handleDepth()-Depth_Keller msg: 1613172983284 epoch sec; seqNo:5246; depth 13.59 2021-02-12T23:36:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172983.30 epoch sec; seqNo:5246; 4057393.07 north; 591227.43 east 2021-02-12T23:36:23Z- LcmTrn::handleMessages() - handling 40 messages took 0.71 ms and 126 calls... 2021-02-12T23:36:23Z- waiting for 1613172981.47, time is 1613172983.30 2021-02-12T23:36:23Z- LcmTrn::updateTrn() >>>> pose time 1613172983.30, meas time 1613172982.90 <<<< 2021-02-12T23:36:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172982.90 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:36:23Z- TerrainNav::motionUpdate() - currEstimate = (4057393, 591227.43, 13.59) 2021-02-12T23:36:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:36:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:36:23Z- TNavBF::Projecting Measurements 2021-02-12T23:36:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:36:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:36:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:36:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:36:23Z- TNavFilter::4 good beams. 2021-02-12T23:36:23Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:36:23Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:36:23Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:36:23Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:36:23Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:36:23Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:36:23Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:36:23Z- TNavBF::mmse selected from filter -1 2021-02-12T23:36:23Z- TerrainMapOctree: (vehN, vehE ) = (4057392.81, 591227.23). 2021-02-12T23:36:23Z- TerrainMapOctree: Min Distance = 4900.52. 2021-02-12T23:36:23Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:36:23Z- There is no specific good beam that all particles have in common. 2021-02-12T23:36:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:36:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 11.03 2021-02-12T23:36:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:36:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 14.88 2021-02-12T23:36:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:36:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 19.72 2021-02-12T23:36:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:36:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 9.78 2021-02-12T23:36:23Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:36:23Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:36:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:36:23Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:36:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:36:23Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:36:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:36:23Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:36:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:36:23Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:36:23Z- Filter 0 No beams used in subcloud update 2021-02-12T23:36:23Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:36:23Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:36:23Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 750.8 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:36:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613172982.90 sec. 2021-02-12T23:36:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:36:23Z- TNavBF::mmse selected from filter -1 2021-02-12T23:36:23Z- variances([0], [2], and psi_berg) = 750.730058 833.033720 0.000000 2021-02-12T23:36:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:36:23Z- LcmTrn::publishEstimates() - MLE : 4057378.57 591276.93 13.59 2021-02-12T23:36:23Z- LcmTrn::publishEstimates() - MMSE : 4057393.07 591227.43 13.59 2021-02-12T23:36:23Z- LcmTrn::publishEstimates() - COVAR: 750.73 833.03 0.98 0.00 2021-02-12T23:36:23Z- AHRS_M2 msg: 1613172983.69 epoch sec; seqNo:5199 2021-02-12T23:36:23Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.19 psi 2021-02-12T23:36:23Z- handleDepth()-Depth_Keller msg: 1613172983694 epoch sec; seqNo:5247; depth 13.47 2021-02-12T23:36:23Z- handleDvl() - RDI_Pathfinder msg: 1613172983.71 epoch sec; seqNo:2528 2021-02-12T23:36:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.36 , 111.23 , 74.37 2021-02-12T23:36:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.23 , 0.24 2021-02-12T23:36:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172983.72 epoch sec; seqNo:5247; 4057393.45 north; 591227.42 east 2021-02-12T23:36:24Z- AHRS_M2 msg: 1613172984.09 epoch sec; seqNo:5200 2021-02-12T23:36:24Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.19 psi 2021-02-12T23:36:24Z- handleDepth()-Depth_Keller msg: 1613172984104 epoch sec; seqNo:5248; depth 13.35 2021-02-12T23:36:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172984.13 epoch sec; seqNo:5248; 4057393.82 north; 591227.42 east 2021-02-12T23:36:24Z- AHRS_M2 msg: 1613172984.49 epoch sec; seqNo:5201 2021-02-12T23:36:24Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.18 psi 2021-02-12T23:36:24Z- handleDepth()-Depth_Keller msg: 1613172984497 epoch sec; seqNo:5249; depth 13.23 2021-02-12T23:36:24Z- handleDvl() - RDI_Pathfinder msg: 1613172984.52 epoch sec; seqNo:2529 2021-02-12T23:36:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.71 , 96.23 , 113.80 , 75.19 2021-02-12T23:36:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.05 , 0.31 2021-02-12T23:36:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172984.53 epoch sec; seqNo:5249; 4057394.20 north; 591227.41 east 2021-02-12T23:36:24Z- AHRS_M2 msg: 1613172984.90 epoch sec; seqNo:5202 2021-02-12T23:36:24Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.17 psi 2021-02-12T23:36:24Z- handleDepth()-Depth_Keller msg: 1613172984900 epoch sec; seqNo:5250; depth 13.12 2021-02-12T23:36:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172984.91 epoch sec; seqNo:5250; 4057394.58 north; 591227.41 east 2021-02-12T23:36:25Z- AHRS_M2 msg: 1613172985.30 epoch sec; seqNo:5203 2021-02-12T23:36:25Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.16 psi 2021-02-12T23:36:25Z- handleDepth()-Depth_Keller msg: 1613172985304 epoch sec; seqNo:5251; depth 12.99 2021-02-12T23:36:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172985.31 epoch sec; seqNo:5251; 4057394.97 north; 591228.63 east 2021-02-12T23:36:25Z- AHRS_M2 msg: 1613172985.70 epoch sec; seqNo:5204 2021-02-12T23:36:25Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.16 psi 2021-02-12T23:36:25Z- handleDepth()-Depth_Keller msg: 1613172985710 epoch sec; seqNo:5252; depth 12.88 2021-02-12T23:36:25Z- handleDvl() - RDI_Pathfinder msg: 1613172985.72 epoch sec; seqNo:2530 2021-02-12T23:36:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.55 , 96.36 , 115.29 , 75.60 2021-02-12T23:36:25Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , 0.11 , 0.19 2021-02-12T23:36:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172985.73 epoch sec; seqNo:5252; 4057394.97 north; 591228.63 east 2021-02-12T23:36:26Z- AHRS_M2 msg: 1613172986.11 epoch sec; seqNo:5205 2021-02-12T23:36:26Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.15 psi 2021-02-12T23:36:26Z- handleDepth()-Depth_Keller msg: 1613172986113 epoch sec; seqNo:5253; depth 12.76 2021-02-12T23:36:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172986.14 epoch sec; seqNo:5253; 4057395.35 north; 591228.62 east 2021-02-12T23:36:26Z- AHRS_M2 msg: 1613172986.51 epoch sec; seqNo:5206 2021-02-12T23:36:26Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.15 psi 2021-02-12T23:36:26Z- handleDepth()-Depth_Keller msg: 1613172986517 epoch sec; seqNo:5254; depth 12.66 2021-02-12T23:36:26Z- handleDvl() - RDI_Pathfinder msg: 1613172986.53 epoch sec; seqNo:2531 2021-02-12T23:36:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.48 , 100.35 , 116.57 , 76.01 2021-02-12T23:36:26Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.17 , -0.00 2021-02-12T23:36:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172986.55 epoch sec; seqNo:5254; 4057395.73 north; 591228.62 east 2021-02-12T23:36:26Z- AHRS_M2 msg: 1613172986.92 epoch sec; seqNo:5207 2021-02-12T23:36:26Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.13 psi 2021-02-12T23:36:26Z- handleDepth()-Depth_Keller msg: 1613172986924 epoch sec; seqNo:5255; depth 12.55 2021-02-12T23:36:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172986.93 epoch sec; seqNo:5255; 4057396.11 north; 591228.61 east 2021-02-12T23:36:27Z- AHRS_M2 msg: 1613172987.32 epoch sec; seqNo:5208 2021-02-12T23:36:27Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-12T23:36:27Z- handleDepth()-Depth_Keller msg: 1613172987333 epoch sec; seqNo:5256; depth 12.43 2021-02-12T23:36:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172987.34 epoch sec; seqNo:5256; 4057396.50 north; 591229.83 east 2021-02-12T23:36:27Z- AHRS_M2 msg: 1613172987.72 epoch sec; seqNo:5209 2021-02-12T23:36:27Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.13 psi 2021-02-12T23:36:27Z- handleDepth()-Depth_Keller msg: 1613172987728 epoch sec; seqNo:5257; depth 12.31 2021-02-12T23:36:27Z- handleDvl() - RDI_Pathfinder msg: 1613172987.75 epoch sec; seqNo:2532 2021-02-12T23:36:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 100.21 , 115.09 , 76.01 2021-02-12T23:36:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.12 , -0.15 2021-02-12T23:36:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172987.76 epoch sec; seqNo:5257; 4057396.50 north; 591229.83 east 2021-02-12T23:36:28Z- AHRS_M2 msg: 1613172988.13 epoch sec; seqNo:5210 2021-02-12T23:36:28Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.13 psi 2021-02-12T23:36:28Z- handleDepth()-Depth_Keller msg: 1613172988132 epoch sec; seqNo:5258; depth 12.19 2021-02-12T23:36:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172988.14 epoch sec; seqNo:5258; 4057396.88 north; 591229.83 east 2021-02-12T23:36:28Z- AHRS_M2 msg: 1613172988.53 epoch sec; seqNo:5211 2021-02-12T23:36:28Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.13 psi 2021-02-12T23:36:28Z- handleDepth()-Depth_Keller msg: 1613172988536 epoch sec; seqNo:5259; depth 12.10 2021-02-12T23:36:28Z- handleDvl() - RDI_Pathfinder msg: 1613172988.55 epoch sec; seqNo:2533 2021-02-12T23:36:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 98.86 , 116.44 , 75.60 2021-02-12T23:36:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.03 , -0.20 2021-02-12T23:36:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172988.56 epoch sec; seqNo:5259; 4057396.88 north; 591229.83 east 2021-02-12T23:36:28Z- LcmTrn::handleMessages() - handling 45 messages took 0.62 ms and 133 calls... 2021-02-12T23:36:28Z- waiting for 1613172986.30, time is 1613172988.56 2021-02-12T23:36:28Z- LcmTrn::updateTrn() equating pose 1613172988.56 and meas times 1613172988.56 2021-02-12T23:36:28Z- LcmTrn::updateTrn() >>>> pose time 1613172988.56, meas time 1613172988.56 <<<< 2021-02-12T23:36:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172988.56 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:36:28Z- TerrainNav::motionUpdate() - currEstimate = (4057397, 591229.83, 12.10) 2021-02-12T23:36:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:36:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:36:28Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172988.56 sec.; No relevant navigation data available 2021-02-12T23:36:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:36:28Z- TNavBF::mmse selected from filter -1 2021-02-12T23:36:28Z- variances([0], [2], and psi_berg) = 750.625727 833.033622 0.000000 2021-02-12T23:36:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:36:28Z- LcmTrn::publishEstimates() - MLE : 4057382.38 591279.33 12.10 2021-02-12T23:36:28Z- LcmTrn::publishEstimates() - MMSE : 4057396.88 591229.83 12.10 2021-02-12T23:36:28Z- LcmTrn::publishEstimates() - COVAR: 750.63 833.03 1.21 0.00 2021-02-12T23:36:28Z- AHRS_M2 msg: 1613172988.95 epoch sec; seqNo:5212 2021-02-12T23:36:28Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.13 psi 2021-02-12T23:36:28Z- handleDepth()-Depth_Keller msg: 1613172988950 epoch sec; seqNo:5260; depth 11.97 2021-02-12T23:36:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172988.96 epoch sec; seqNo:5260; 4057397.26 north; 591229.82 east 2021-02-12T23:36:29Z- AHRS_M2 msg: 1613172989.34 epoch sec; seqNo:5213 2021-02-12T23:36:29Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-12T23:36:29Z- handleDepth()-Depth_Keller msg: 1613172989347 epoch sec; seqNo:5261; depth 11.85 2021-02-12T23:36:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172989.37 epoch sec; seqNo:5261; 4057397.64 north; 591229.82 east 2021-02-12T23:36:29Z- AHRS_M2 msg: 1613172989.74 epoch sec; seqNo:5214 2021-02-12T23:36:29Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-12T23:36:29Z- handleDepth()-Depth_Keller msg: 1613172989748 epoch sec; seqNo:5262; depth 11.74 2021-02-12T23:36:29Z- handleDvl() - RDI_Pathfinder msg: 1613172989.76 epoch sec; seqNo:2534 2021-02-12T23:36:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 100.21 , 113.73 , 75.19 2021-02-12T23:36:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.09 , -0.13 2021-02-12T23:36:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172989.78 epoch sec; seqNo:5262; 4057397.65 north; 591231.04 east 2021-02-12T23:36:30Z- AHRS_M2 msg: 1613172990.15 epoch sec; seqNo:5215 2021-02-12T23:36:30Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-12T23:36:30Z- handleDepth()-Depth_Keller msg: 1613172990154 epoch sec; seqNo:5263; depth 11.60 2021-02-12T23:36:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172990.16 epoch sec; seqNo:5263; 4057398.03 north; 591231.04 east 2021-02-12T23:36:30Z- AHRS_M2 msg: 1613172990.55 epoch sec; seqNo:5216 2021-02-12T23:36:30Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-12T23:36:30Z- handleDepth()-Depth_Keller msg: 1613172990562 epoch sec; seqNo:5264; depth 11.50 2021-02-12T23:36:30Z- handleDvl() - RDI_Pathfinder msg: 1613172990.57 epoch sec; seqNo:2535 2021-02-12T23:36:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.63 , 116.51 , 77.24 2021-02-12T23:36:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.09 , -0.01 2021-02-12T23:36:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172990.58 epoch sec; seqNo:5264; 4057398.41 north; 591231.03 east 2021-02-12T23:36:30Z- AHRS_M2 msg: 1613172990.95 epoch sec; seqNo:5217 2021-02-12T23:36:30Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-12T23:36:30Z- handleDepth()-Depth_Keller msg: 1613172990960 epoch sec; seqNo:5265; depth 11.36 2021-02-12T23:36:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172990.97 epoch sec; seqNo:5265; 4057398.41 north; 591231.03 east 2021-02-12T23:36:31Z- AHRS_M2 msg: 1613172991.36 epoch sec; seqNo:5218 2021-02-12T23:36:31Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-12T23:36:31Z- handleDepth()-Depth_Keller msg: 1613172991366 epoch sec; seqNo:5266; depth 11.26 2021-02-12T23:36:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172991.37 epoch sec; seqNo:5266; 4057398.79 north; 591231.03 east 2021-02-12T23:36:31Z- AHRS_M2 msg: 1613172991.76 epoch sec; seqNo:5219 2021-02-12T23:36:31Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-12T23:36:31Z- handleDepth()-Depth_Keller msg: 1613172991768 epoch sec; seqNo:5267; depth 11.13 2021-02-12T23:36:31Z- handleDvl() - RDI_Pathfinder msg: 1613172991.79 epoch sec; seqNo:2536 2021-02-12T23:36:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.71 , 102.58 , 116.10 , 77.24 2021-02-12T23:36:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.07 , 0.06 2021-02-12T23:36:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172991.80 epoch sec; seqNo:5267; 4057399.17 north; 591231.02 east 2021-02-12T23:36:32Z- AHRS_M2 msg: 1613172992.17 epoch sec; seqNo:5220 2021-02-12T23:36:32Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-12T23:36:32Z- handleDepth()-Depth_Keller msg: 1613172992173 epoch sec; seqNo:5268; depth 10.99 2021-02-12T23:36:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172992.18 epoch sec; seqNo:5268; 4057399.17 north; 591231.02 east 2021-02-12T23:36:32Z- AHRS_M2 msg: 1613172992.57 epoch sec; seqNo:5221 2021-02-12T23:36:32Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-12T23:36:32Z- handleDepth()-Depth_Keller msg: 1613172992576 epoch sec; seqNo:5269; depth 10.89 2021-02-12T23:36:32Z- handleDvl() - RDI_Pathfinder msg: 1613172992.59 epoch sec; seqNo:2537 2021-02-12T23:36:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:36:32Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:36:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172992.60 epoch sec; seqNo:5269; 4057399.55 north; 591231.02 east 2021-02-12T23:36:32Z- AHRS_M2 msg: 1613172992.98 epoch sec; seqNo:5222 2021-02-12T23:36:32Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-12T23:36:32Z- handleDepth()-Depth_Keller msg: 1613172992987 epoch sec; seqNo:5270; depth 10.76 2021-02-12T23:36:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172993.00 epoch sec; seqNo:5270; 4057399.92 north; 591231.02 east 2021-02-12T23:36:33Z- AHRS_M2 msg: 1613172993.38 epoch sec; seqNo:5223 2021-02-12T23:36:33Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-12T23:36:33Z- handleDepth()-Depth_Keller msg: 1613172993398 epoch sec; seqNo:5271; depth 10.65 2021-02-12T23:36:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172993.41 epoch sec; seqNo:5271; 4057399.94 north; 591232.24 east 2021-02-12T23:36:33Z- LcmTrn::handleMessages() - handling 40 messages took 0.64 ms and 127 calls... 2021-02-12T23:36:33Z- waiting for 1613172991.56, time is 1613172993.41 2021-02-12T23:36:33Z- LcmTrn::updateTrn() >>>> pose time 1613172993.41, meas time 1613172992.59 <<<< 2021-02-12T23:36:33Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613172992.59, ping # 0. 2021-02-12T23:36:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172992.59 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:36:33Z- TerrainNav::motionUpdate() - currEstimate = (4057400, 591232.24, 10.65) 2021-02-12T23:36:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:36:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:36:33Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:36:33Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:36:33Z- TNavBF::Projecting Measurements 2021-02-12T23:36:33Z- TNavFilter::Measurement from time = 1613172992.59 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:36:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:36:33Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:36:33Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:36:33Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:36:33Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:36:33Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:36:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:36:33Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 750.5 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-12T23:36:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:36:33Z- TNavBF::mmse selected from filter -1 2021-02-12T23:36:33Z- variances([0], [2], and psi_berg) = 750.497651 833.033502 0.000000 2021-02-12T23:36:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:36:33Z- LcmTrn::publishEstimates() - MLE : 4057386.44 591185.74 10.65 2021-02-12T23:36:33Z- LcmTrn::publishEstimates() - MMSE : 4057399.94 591232.24 10.65 2021-02-12T23:36:33Z- LcmTrn::publishEstimates() - COVAR: 750.50 833.03 1.40 0.00 2021-02-12T23:36:33Z- AHRS_M2 msg: 1613172993.78 epoch sec; seqNo:5224 2021-02-12T23:36:33Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-12T23:36:33Z- handleDepth()-Depth_Keller msg: 1613172993793 epoch sec; seqNo:5272; depth 10.51 2021-02-12T23:36:33Z- handleDvl() - RDI_Pathfinder msg: 1613172993.81 epoch sec; seqNo:2538 2021-02-12T23:36:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.83 , 103.19 , 116.57 , 78.36 2021-02-12T23:36:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , -0.05 2021-02-12T23:36:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172993.84 epoch sec; seqNo:5272; 4057400.32 north; 591232.23 east 2021-02-12T23:36:34Z- AHRS_M2 msg: 1613172994.19 epoch sec; seqNo:5225 2021-02-12T23:36:34Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-12T23:36:34Z- handleDepth()-Depth_Keller msg: 1613172994197 epoch sec; seqNo:5273; depth 10.38 2021-02-12T23:36:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172994.21 epoch sec; seqNo:5273; 4057400.70 north; 591232.23 east 2021-02-12T23:36:34Z- AHRS_M2 msg: 1613172994.60 epoch sec; seqNo:5226 2021-02-12T23:36:34Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-12T23:36:34Z- handleDepth()-Depth_Keller msg: 1613172994599 epoch sec; seqNo:5274; depth 10.27 2021-02-12T23:36:34Z- handleDvl() - RDI_Pathfinder msg: 1613172994.61 epoch sec; seqNo:2539 2021-02-12T23:36:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.02 , 117.65 , 77.64 2021-02-12T23:36:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.12 , -0.04 2021-02-12T23:36:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172994.62 epoch sec; seqNo:5274; 4057401.07 north; 591232.23 east 2021-02-12T23:36:34Z- AHRS_M2 msg: 1613172995.00 epoch sec; seqNo:5227 2021-02-12T23:36:34Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.16 psi 2021-02-12T23:36:35Z- handleDepth()-Depth_Keller msg: 1613172995005 epoch sec; seqNo:5275; depth 10.16 2021-02-12T23:36:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172995.02 epoch sec; seqNo:5275; 4057401.07 north; 591232.23 east 2021-02-12T23:36:35Z- AHRS_M2 msg: 1613172995.40 epoch sec; seqNo:5228 2021-02-12T23:36:35Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-12T23:36:35Z- handleDepth()-Depth_Keller msg: 1613172995404 epoch sec; seqNo:5276; depth 10.03 2021-02-12T23:36:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172995.41 epoch sec; seqNo:5276; 4057401.45 north; 591232.22 east 2021-02-12T23:36:35Z- AHRS_M2 msg: 1613172995.82 epoch sec; seqNo:5229 2021-02-12T23:36:35Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.18 psi 2021-02-12T23:36:35Z- handleDepth()-Depth_Keller msg: 1613172995833 epoch sec; seqNo:5277; depth 9.91 2021-02-12T23:36:35Z- handleDvl() - RDI_Pathfinder msg: 1613172995.86 epoch sec; seqNo:2540 2021-02-12T23:36:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 105.42 , 117.59 , 78.38 2021-02-12T23:36:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.16 , 0.04 2021-02-12T23:36:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172995.87 epoch sec; seqNo:5277; 4057401.83 north; 591232.22 east 2021-02-12T23:36:36Z- AHRS_M2 msg: 1613172996.21 epoch sec; seqNo:5230 2021-02-12T23:36:36Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.18 psi 2021-02-12T23:36:36Z- handleDepth()-Depth_Keller msg: 1613172996212 epoch sec; seqNo:5278; depth 9.78 2021-02-12T23:36:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172996.23 epoch sec; seqNo:5278; 4057402.21 north; 591232.21 east 2021-02-12T23:36:36Z- AHRS_M2 msg: 1613172996.61 epoch sec; seqNo:5231 2021-02-12T23:36:36Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.18 psi 2021-02-12T23:36:36Z- handleDepth()-Depth_Keller msg: 1613172996616 epoch sec; seqNo:5279; depth 9.66 2021-02-12T23:36:36Z- handleDvl() - RDI_Pathfinder msg: 1613172996.63 epoch sec; seqNo:2541 2021-02-12T23:36:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.84 , 104.07 , 120.29 , 79.73 2021-02-12T23:36:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.13 , 0.14 2021-02-12T23:36:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172996.65 epoch sec; seqNo:5279; 4057402.60 north; 591233.43 east 2021-02-12T23:36:37Z- AHRS_M2 msg: 1613172997.02 epoch sec; seqNo:5232 2021-02-12T23:36:37Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-12T23:36:37Z- handleDepth()-Depth_Keller msg: 1613172997025 epoch sec; seqNo:5280; depth 9.55 2021-02-12T23:36:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172997.04 epoch sec; seqNo:5280; 4057402.98 north; 591233.43 east 2021-02-12T23:36:37Z- AHRS_M2 msg: 1613172997.42 epoch sec; seqNo:5233 2021-02-12T23:36:37Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-12T23:36:37Z- handleDepth()-Depth_Keller msg: 1613172997430 epoch sec; seqNo:5281; depth 9.44 2021-02-12T23:36:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172997.45 epoch sec; seqNo:5281; 4057402.98 north; 591233.43 east 2021-02-12T23:36:37Z- AHRS_M2 msg: 1613172997.82 epoch sec; seqNo:5234 2021-02-12T23:36:37Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-12T23:36:37Z- handleDepth()-Depth_Keller msg: 1613172997828 epoch sec; seqNo:5282; depth 9.33 2021-02-12T23:36:37Z- handleDvl() - RDI_Pathfinder msg: 1613172997.85 epoch sec; seqNo:2542 2021-02-12T23:36:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.71 , 103.93 , 117.45 , 0.00 2021-02-12T23:36:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , 0.18 2021-02-12T23:36:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172997.86 epoch sec; seqNo:5282; 4057403.36 north; 591233.42 east 2021-02-12T23:36:38Z- AHRS_M2 msg: 1613172998.23 epoch sec; seqNo:5235 2021-02-12T23:36:38Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.14 psi 2021-02-12T23:36:38Z- handleDepth()-Depth_Keller msg: 1613172998233 epoch sec; seqNo:5283; depth 9.21 2021-02-12T23:36:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172998.24 epoch sec; seqNo:5283; 4057403.74 north; 591233.42 east 2021-02-12T23:36:38Z- AHRS_M2 msg: 1613172998.63 epoch sec; seqNo:5236 2021-02-12T23:36:38Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.13 psi 2021-02-12T23:36:38Z- handleDepth()-Depth_Keller msg: 1613172998636 epoch sec; seqNo:5284; depth 9.11 2021-02-12T23:36:38Z- handleDvl() - RDI_Pathfinder msg: 1613172998.65 epoch sec; seqNo:2543 2021-02-12T23:36:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 102.31 , 118.67 , 77.64 2021-02-12T23:36:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.16 2021-02-12T23:36:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172998.66 epoch sec; seqNo:5284; 4057404.12 north; 591233.41 east 2021-02-12T23:36:38Z- LcmTrn::handleMessages() - handling 45 messages took 0.72 ms and 131 calls... 2021-02-12T23:36:38Z- waiting for 1613172996.41, time is 1613172998.66 2021-02-12T23:36:38Z- LcmTrn::updateTrn() equating pose 1613172998.66 and meas times 1613172998.66 2021-02-12T23:36:38Z- LcmTrn::updateTrn() >>>> pose time 1613172998.66, meas time 1613172998.66 <<<< 2021-02-12T23:36:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613172998.66 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:36:38Z- TerrainNav::motionUpdate() - currEstimate = (4057404, 591233.41, 9.11) 2021-02-12T23:36:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:36:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:36:38Z- TerrainNav::Measurement type 1 not incorporated from time = 1613172998.66 sec.; No relevant navigation data available 2021-02-12T23:36:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:36:38Z- TNavBF::mmse selected from filter -1 2021-02-12T23:36:38Z- variances([0], [2], and psi_berg) = 750.395562 833.033407 0.000000 2021-02-12T23:36:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:36:38Z- LcmTrn::publishEstimates() - MLE : 4057390.62 591186.91 9.11 2021-02-12T23:36:38Z- LcmTrn::publishEstimates() - MMSE : 4057404.12 591233.41 9.11 2021-02-12T23:36:38Z- LcmTrn::publishEstimates() - COVAR: 750.40 833.03 1.63 0.00 2021-02-12T23:36:39Z- AHRS_M2 msg: 1613172999.04 epoch sec; seqNo:5237 2021-02-12T23:36:39Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.12 psi 2021-02-12T23:36:39Z- handleDepth()-Depth_Keller msg: 1613172999047 epoch sec; seqNo:5285; depth 8.96 2021-02-12T23:36:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172999.06 epoch sec; seqNo:5285; 4057404.13 north; 591234.63 east 2021-02-12T23:36:39Z- AHRS_M2 msg: 1613172999.46 epoch sec; seqNo:5238 2021-02-12T23:36:39Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.12 psi 2021-02-12T23:36:39Z- handleDepth()-Depth_Keller msg: 1613172999484 epoch sec; seqNo:5286; depth 8.84 2021-02-12T23:36:39Z- handleDvl() - RDI_Pathfinder msg: 1613172999.50 epoch sec; seqNo:2544 2021-02-12T23:36:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.31 , 101.36 , 117.59 , 77.64 2021-02-12T23:36:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.03 , 0.16 2021-02-12T23:36:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172999.51 epoch sec; seqNo:5286; 4057404.51 north; 591234.63 east 2021-02-12T23:36:39Z- AHRS_M2 msg: 1613172999.84 epoch sec; seqNo:5239 2021-02-12T23:36:39Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.13 psi 2021-02-12T23:36:39Z- handleDepth()-Depth_Keller msg: 1613172999849 epoch sec; seqNo:5287; depth 8.72 2021-02-12T23:36:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613172999.86 epoch sec; seqNo:5287; 4057404.89 north; 591234.63 east 2021-02-12T23:36:40Z- AHRS_M2 msg: 1613173000.25 epoch sec; seqNo:5240 2021-02-12T23:36:40Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.14 psi 2021-02-12T23:36:40Z- handleDepth()-Depth_Keller msg: 1613173000261 epoch sec; seqNo:5288; depth 8.61 2021-02-12T23:36:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173000.27 epoch sec; seqNo:5288; 4057405.27 north; 591234.62 east 2021-02-12T23:36:40Z- AHRS_M2 msg: 1613173000.65 epoch sec; seqNo:5241 2021-02-12T23:36:40Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.14 psi 2021-02-12T23:36:40Z- handleDepth()-Depth_Keller msg: 1613173000661 epoch sec; seqNo:5289; depth 8.50 2021-02-12T23:36:40Z- handleDvl() - RDI_Pathfinder msg: 1613173000.68 epoch sec; seqNo:2545 2021-02-12T23:36:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.12 , 102.58 , 122.86 , 78.46 2021-02-12T23:36:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.14 , 0.06 2021-02-12T23:36:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173000.69 epoch sec; seqNo:5289; 4057405.65 north; 591234.62 east 2021-02-12T23:36:41Z- AHRS_M2 msg: 1613173001.06 epoch sec; seqNo:5242 2021-02-12T23:36:41Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.15 psi 2021-02-12T23:36:41Z- handleDepth()-Depth_Keller msg: 1613173001065 epoch sec; seqNo:5290; depth 8.35 2021-02-12T23:36:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173001.08 epoch sec; seqNo:5290; 4057405.66 north; 591235.84 east 2021-02-12T23:36:41Z- AHRS_M2 msg: 1613173001.46 epoch sec; seqNo:5243 2021-02-12T23:36:41Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-12T23:36:41Z- handleDepth()-Depth_Keller msg: 1613173001467 epoch sec; seqNo:5291; depth 8.24 2021-02-12T23:36:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173001.48 epoch sec; seqNo:5291; 4057406.04 north; 591235.84 east 2021-02-12T23:36:41Z- AHRS_M2 msg: 1613173001.86 epoch sec; seqNo:5244 2021-02-12T23:36:41Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.19 psi 2021-02-12T23:36:41Z- handleDepth()-Depth_Keller msg: 1613173001867 epoch sec; seqNo:5292; depth 8.13 2021-02-12T23:36:41Z- handleDvl() - RDI_Pathfinder msg: 1613173001.88 epoch sec; seqNo:2546 2021-02-12T23:36:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.70 , 103.86 , 124.14 , 80.88 2021-02-12T23:36:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.19 , -0.17 2021-02-12T23:36:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173001.90 epoch sec; seqNo:5292; 4057406.42 north; 591235.83 east 2021-02-12T23:36:42Z- AHRS_M2 msg: 1613173002.27 epoch sec; seqNo:5245 2021-02-12T23:36:42Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.21 psi 2021-02-12T23:36:42Z- handleDepth()-Depth_Keller msg: 1613173002277 epoch sec; seqNo:5293; depth 7.99 2021-02-12T23:36:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173002.29 epoch sec; seqNo:5293; 4057406.42 north; 591235.83 east 2021-02-12T23:36:42Z- AHRS_M2 msg: 1613173002.67 epoch sec; seqNo:5246 2021-02-12T23:36:42Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.22 psi 2021-02-12T23:36:42Z- handleDepth()-Depth_Keller msg: 1613173002679 epoch sec; seqNo:5294; depth 7.87 2021-02-12T23:36:42Z- handleDvl() - RDI_Pathfinder msg: 1613173002.69 epoch sec; seqNo:2547 2021-02-12T23:36:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 107.85 , 124.08 , 82.17 2021-02-12T23:36:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.03 , -0.27 2021-02-12T23:36:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173002.70 epoch sec; seqNo:5294; 4057406.80 north; 591235.83 east 2021-02-12T23:36:43Z- AHRS_M2 msg: 1613173003.08 epoch sec; seqNo:5247 2021-02-12T23:36:43Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.21 psi 2021-02-12T23:36:43Z- handleDepth()-Depth_Keller msg: 1613173003081 epoch sec; seqNo:5295; depth 7.73 2021-02-12T23:36:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173003.10 epoch sec; seqNo:5295; 4057406.80 north; 591235.83 east 2021-02-12T23:36:43Z- AHRS_M2 msg: 1613173003.48 epoch sec; seqNo:5248 2021-02-12T23:36:43Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.19 psi 2021-02-12T23:36:43Z- handleDepth()-Depth_Keller msg: 1613173003485 epoch sec; seqNo:5296; depth 7.64 2021-02-12T23:36:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173003.49 epoch sec; seqNo:5296; 4057407.19 north; 591237.04 east 2021-02-12T23:36:43Z- LcmTrn::handleMessages() - handling 40 messages took 0.61 ms and 128 calls... 2021-02-12T23:36:43Z- waiting for 1613173001.66, time is 1613173003.49 2021-02-12T23:36:43Z- LcmTrn::updateTrn() >>>> pose time 1613173003.49, meas time 1613173002.69 <<<< 2021-02-12T23:36:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173002.69 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:36:43Z- TerrainNav::motionUpdate() - currEstimate = (4057407, 591237.04, 7.64) 2021-02-12T23:36:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:36:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:36:43Z- TNavBF::Projecting Measurements 2021-02-12T23:36:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:36:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:36:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:36:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:36:43Z- TNavFilter::4 good beams. 2021-02-12T23:36:43Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:36:43Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:36:43Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:36:43Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:36:43Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:36:43Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:36:43Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:36:43Z- TNavBF::mmse selected from filter -1 2021-02-12T23:36:43Z- TerrainMapOctree: (vehN, vehE ) = (4057406.68, 591236.44). 2021-02-12T23:36:43Z- TerrainMapOctree: Min Distance = 4883.92. 2021-02-12T23:36:43Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:36:43Z- There is no specific good beam that all particles have in common. 2021-02-12T23:36:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:36:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.83 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 12.65 2021-02-12T23:36:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:36:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.62 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 18.61 2021-02-12T23:36:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:36:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.50 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 24.63 2021-02-12T23:36:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:36:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.42 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 10.80 2021-02-12T23:36:43Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:36:43Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:36:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:36:43Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:36:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:36:43Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:36:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:36:43Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:36:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:36:43Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:36:43Z- Filter 0 No beams used in subcloud update 2021-02-12T23:36:43Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:36:43Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:36:43Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 750.3 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-12T23:36:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173002.69 sec. 2021-02-12T23:36:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:36:43Z- TNavBF::mmse selected from filter -1 2021-02-12T23:36:43Z- variances([0], [2], and psi_berg) = 750.254726 833.033275 0.000000 2021-02-12T23:36:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:36:43Z- LcmTrn::publishEstimates() - MLE : 4057393.69 591190.54 7.64 2021-02-12T23:36:43Z- LcmTrn::publishEstimates() - MMSE : 4057407.19 591237.04 7.64 2021-02-12T23:36:43Z- LcmTrn::publishEstimates() - COVAR: 750.25 833.03 1.32 0.00 2021-02-12T23:36:43Z- AHRS_M2 msg: 1613173003.88 epoch sec; seqNo:5249 2021-02-12T23:36:43Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.18 psi 2021-02-12T23:36:43Z- handleDepth()-Depth_Keller msg: 1613173003893 epoch sec; seqNo:5297; depth 7.53 2021-02-12T23:36:43Z- handleDvl() - RDI_Pathfinder msg: 1613173003.90 epoch sec; seqNo:2548 2021-02-12T23:36:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 109.14 , 121.31 , 82.10 2021-02-12T23:36:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.05 , -0.09 2021-02-12T23:36:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173003.91 epoch sec; seqNo:5297; 4057407.57 north; 591237.04 east 2021-02-12T23:36:44Z- AHRS_M2 msg: 1613173004.29 epoch sec; seqNo:5250 2021-02-12T23:36:44Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-12T23:36:44Z- handleDepth()-Depth_Keller msg: 1613173004297 epoch sec; seqNo:5298; depth 7.43 2021-02-12T23:36:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173004.31 epoch sec; seqNo:5298; 4057407.57 north; 591237.04 east 2021-02-12T23:36:44Z- AHRS_M2 msg: 1613173004.69 epoch sec; seqNo:5251 2021-02-12T23:36:44Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.15 psi 2021-02-12T23:36:44Z- handleDepth()-Depth_Keller msg: 1613173004696 epoch sec; seqNo:5299; depth 7.33 2021-02-12T23:36:44Z- handleDvl() - RDI_Pathfinder msg: 1613173004.71 epoch sec; seqNo:2549 2021-02-12T23:36:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 105.35 , 120.22 , 81.56 2021-02-12T23:36:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.06 , 0.01 2021-02-12T23:36:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173004.73 epoch sec; seqNo:5299; 4057407.95 north; 591237.04 east 2021-02-12T23:36:45Z- AHRS_M2 msg: 1613173005.10 epoch sec; seqNo:5252 2021-02-12T23:36:45Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.15 psi 2021-02-12T23:36:45Z- handleDepth()-Depth_Keller msg: 1613173005100 epoch sec; seqNo:5300; depth 7.22 2021-02-12T23:36:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:36:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:36:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:36:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:36:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:36:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:36:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:36:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:36:45Z- handleDvl() - RDI_Pathfinder msg: 1613173005.10 epoch sec; seqNo:2550 2021-02-12T23:36:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:36:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.06 , 0.01 2021-02-12T23:36:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173005.12 epoch sec; seqNo:5300; 4057407.95 north; 591237.04 east 2021-02-12T23:36:45Z- AHRS_M2 msg: 1613173005.52 epoch sec; seqNo:5253 2021-02-12T23:36:45Z- AHRS_M2 msg: 0.66 phi; 0.30 theta; -0.16 psi 2021-02-12T23:36:45Z- handleDepth()-Depth_Keller msg: 1613173005527 epoch sec; seqNo:5301; depth 7.12 2021-02-12T23:36:45Z- handleDvl() - RDI_Pathfinder msg: 1613173005.54 epoch sec; seqNo:2551 2021-02-12T23:36:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.55 , 103.93 , 118.80 , 81.62 2021-02-12T23:36:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.00 , -0.07 2021-02-12T23:36:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173005.56 epoch sec; seqNo:5301; 4057408.32 north; 591237.03 east 2021-02-12T23:36:45Z- AHRS_M2 msg: 1613173005.90 epoch sec; seqNo:5254 2021-02-12T23:36:45Z- AHRS_M2 msg: 0.66 phi; 0.30 theta; -0.16 psi 2021-02-12T23:36:45Z- handleDepth()-Depth_Keller msg: 1613173005908 epoch sec; seqNo:5302; depth 7.02 2021-02-12T23:36:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173005.92 epoch sec; seqNo:5302; 4057408.70 north; 591237.03 east 2021-02-12T23:36:46Z- AHRS_M2 msg: 1613173006.31 epoch sec; seqNo:5255 2021-02-12T23:36:46Z- AHRS_M2 msg: 0.65 phi; 0.30 theta; -0.18 psi 2021-02-12T23:36:46Z- handleDepth()-Depth_Keller msg: 1613173006312 epoch sec; seqNo:5303; depth 6.92 2021-02-12T23:36:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:36:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:36:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:36:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:36:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:36:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:36:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:36:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:36:46Z- handleDvl() - RDI_Pathfinder msg: 1613173006.31 epoch sec; seqNo:2552 2021-02-12T23:36:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:36:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.00 , -0.07 2021-02-12T23:36:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173006.32 epoch sec; seqNo:5303; 4057408.72 north; 591238.25 east 2021-02-12T23:36:46Z- AHRS_M2 msg: 1613173006.71 epoch sec; seqNo:5256 2021-02-12T23:36:46Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.19 psi 2021-02-12T23:36:46Z- handleDepth()-Depth_Keller msg: 1613173006716 epoch sec; seqNo:5304; depth 6.82 2021-02-12T23:36:46Z- handleDvl() - RDI_Pathfinder msg: 1613173006.73 epoch sec; seqNo:2553 2021-02-12T23:36:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 105.28 , 117.45 , 82.30 2021-02-12T23:36:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.19 , -0.05 2021-02-12T23:36:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173006.74 epoch sec; seqNo:5304; 4057409.09 north; 591238.25 east 2021-02-12T23:36:47Z- AHRS_M2 msg: 1613173007.12 epoch sec; seqNo:5257 2021-02-12T23:36:47Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.20 psi 2021-02-12T23:36:47Z- handleDepth()-Depth_Keller msg: 1613173007126 epoch sec; seqNo:5305; depth 6.71 2021-02-12T23:36:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173007.14 epoch sec; seqNo:5305; 4057409.47 north; 591238.24 east 2021-02-12T23:36:47Z- AHRS_M2 msg: 1613173007.54 epoch sec; seqNo:5258 2021-02-12T23:36:47Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.20 psi 2021-02-12T23:36:47Z- handleDepth()-Depth_Keller msg: 1613173007548 epoch sec; seqNo:5306; depth 6.62 2021-02-12T23:36:47Z- handleDvl() - RDI_Pathfinder msg: 1613173007.57 epoch sec; seqNo:2554 2021-02-12T23:36:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.17 , 109.34 , 118.80 , 82.30 2021-02-12T23:36:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.17 , 0.06 2021-02-12T23:36:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173007.59 epoch sec; seqNo:5306; 4057409.85 north; 591238.24 east 2021-02-12T23:36:47Z- AHRS_M2 msg: 1613173007.92 epoch sec; seqNo:5259 2021-02-12T23:36:47Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.18 psi 2021-02-12T23:36:47Z- handleDepth()-Depth_Keller msg: 1613173007933 epoch sec; seqNo:5307; depth 6.53 2021-02-12T23:36:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173007.96 epoch sec; seqNo:5307; 4057409.85 north; 591238.24 east 2021-02-12T23:36:48Z- AHRS_M2 msg: 1613173008.33 epoch sec; seqNo:5260 2021-02-12T23:36:48Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-12T23:36:48Z- handleDepth()-Depth_Keller msg: 1613173008332 epoch sec; seqNo:5308; depth 6.45 2021-02-12T23:36:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173008.34 epoch sec; seqNo:5308; 4057410.23 north; 591238.23 east 2021-02-12T23:36:48Z- AHRS_M2 msg: 1613173008.73 epoch sec; seqNo:5261 2021-02-12T23:36:48Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-12T23:36:48Z- handleDepth()-Depth_Keller msg: 1613173008741 epoch sec; seqNo:5309; depth 6.35 2021-02-12T23:36:48Z- handleDvl() - RDI_Pathfinder msg: 1613173008.75 epoch sec; seqNo:2555 2021-02-12T23:36:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.48 , 106.84 , 120.22 , 0.00 2021-02-12T23:36:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , 0.07 2021-02-12T23:36:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173008.76 epoch sec; seqNo:5309; 4057410.61 north; 591238.23 east 2021-02-12T23:36:48Z- LcmTrn::handleMessages() - handling 47 messages took 0.68 ms and 134 calls... 2021-02-12T23:36:48Z- waiting for 1613173006.49, time is 1613173008.76 2021-02-12T23:36:48Z- LcmTrn::updateTrn() equating pose 1613173008.76 and meas times 1613173008.76 2021-02-12T23:36:48Z- LcmTrn::updateTrn() >>>> pose time 1613173008.76, meas time 1613173008.76 <<<< 2021-02-12T23:36:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173008.76 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:36:48Z- TerrainNav::motionUpdate() - currEstimate = (4057411, 591238.23, 6.35) 2021-02-12T23:36:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:36:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:36:48Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173008.76 sec.; No relevant navigation data available 2021-02-12T23:36:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:36:48Z- TNavBF::mmse selected from filter -1 2021-02-12T23:36:48Z- variances([0], [2], and psi_berg) = 750.161797 833.033189 0.000000 2021-02-12T23:36:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:36:48Z- LcmTrn::publishEstimates() - MLE : 4057397.11 591191.73 6.35 2021-02-12T23:36:48Z- LcmTrn::publishEstimates() - MMSE : 4057410.61 591238.23 6.35 2021-02-12T23:36:48Z- LcmTrn::publishEstimates() - COVAR: 750.16 833.03 1.50 0.00 2021-02-12T23:36:49Z- AHRS_M2 msg: 1613173009.14 epoch sec; seqNo:5262 2021-02-12T23:36:49Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-12T23:36:49Z- handleDepth()-Depth_Keller msg: 1613173009162 epoch sec; seqNo:5310; depth 6.25 2021-02-12T23:36:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173009.17 epoch sec; seqNo:5310; 4057410.99 north; 591238.23 east 2021-02-12T23:36:49Z- AHRS_M2 msg: 1613173009.55 epoch sec; seqNo:5263 2021-02-12T23:36:49Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.16 psi 2021-02-12T23:36:49Z- handleDepth()-Depth_Keller msg: 1613173009589 epoch sec; seqNo:5311; depth 6.15 2021-02-12T23:36:49Z- handleDvl() - RDI_Pathfinder msg: 1613173009.61 epoch sec; seqNo:2556 2021-02-12T23:36:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.58 , 107.99 , 124.35 , 82.71 2021-02-12T23:36:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.00 , 0.05 2021-02-12T23:36:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173009.63 epoch sec; seqNo:5311; 4057411.00 north; 591239.45 east 2021-02-12T23:36:49Z- AHRS_M2 msg: 1613173009.94 epoch sec; seqNo:5264 2021-02-12T23:36:49Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-12T23:36:49Z- handleDepth()-Depth_Keller msg: 1613173009949 epoch sec; seqNo:5312; depth 6.01 2021-02-12T23:36:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:36:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:36:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:36:49Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:36:49Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:36:49Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:36:49Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:36:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:36:49Z- handleDvl() - RDI_Pathfinder msg: 1613173009.95 epoch sec; seqNo:2557 2021-02-12T23:36:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:36:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.00 , 0.05 2021-02-12T23:36:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173009.96 epoch sec; seqNo:5312; 4057411.38 north; 591239.44 east 2021-02-12T23:36:50Z- AHRS_M2 msg: 1613173010.35 epoch sec; seqNo:5265 2021-02-12T23:36:50Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-12T23:36:50Z- handleDepth()-Depth_Keller msg: 1613173010354 epoch sec; seqNo:5313; depth 5.93 2021-02-12T23:36:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173010.36 epoch sec; seqNo:5313; 4057411.38 north; 591239.44 east 2021-02-12T23:36:50Z- AHRS_M2 msg: 1613173010.76 epoch sec; seqNo:5266 2021-02-12T23:36:50Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.19 psi 2021-02-12T23:36:50Z- handleDepth()-Depth_Keller msg: 1613173010772 epoch sec; seqNo:5314; depth 5.81 2021-02-12T23:36:50Z- handleDvl() - RDI_Pathfinder msg: 1613173010.79 epoch sec; seqNo:2558 2021-02-12T23:36:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:36:50Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:36:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173010.80 epoch sec; seqNo:5314; 4057411.76 north; 591239.44 east 2021-02-12T23:36:51Z- AHRS_M2 msg: 1613173011.16 epoch sec; seqNo:5267 2021-02-12T23:36:51Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.21 psi 2021-02-12T23:36:51Z- handleDepth()-Depth_Keller msg: 1613173011161 epoch sec; seqNo:5315; depth 5.67 2021-02-12T23:36:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:36:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:36:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:36:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:36:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:36:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:36:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:36:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:36:51Z- handleDvl() - RDI_Pathfinder msg: 1613173011.16 epoch sec; seqNo:2559 2021-02-12T23:36:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:36:51Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:36:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173011.17 epoch sec; seqNo:5315; 4057412.14 north; 591239.43 east 2021-02-12T23:36:51Z- AHRS_M2 msg: 1613173011.56 epoch sec; seqNo:5268 2021-02-12T23:36:51Z- AHRS_M2 msg: 0.58 phi; 0.38 theta; -0.22 psi 2021-02-12T23:36:51Z- handleDepth()-Depth_Keller msg: 1613173011569 epoch sec; seqNo:5316; depth 5.54 2021-02-12T23:36:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173011.58 epoch sec; seqNo:5316; 4057412.14 north; 591239.43 east 2021-02-12T23:36:51Z- AHRS_M2 msg: 1613173011.97 epoch sec; seqNo:5269 2021-02-12T23:36:51Z- AHRS_M2 msg: 0.58 phi; 0.39 theta; -0.21 psi 2021-02-12T23:36:51Z- handleDepth()-Depth_Keller msg: 1613173011976 epoch sec; seqNo:5317; depth 5.43 2021-02-12T23:36:52Z- handleDvl() - RDI_Pathfinder msg: 1613173012.00 epoch sec; seqNo:2560 2021-02-12T23:36:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.29 , 112.11 , 128.47 , 83.86 2021-02-12T23:36:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.06 , 0.09 2021-02-12T23:36:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173012.04 epoch sec; seqNo:5317; 4057412.52 north; 591239.43 east 2021-02-12T23:36:52Z- AHRS_M2 msg: 1613173012.37 epoch sec; seqNo:5270 2021-02-12T23:36:52Z- AHRS_M2 msg: 0.58 phi; 0.40 theta; -0.20 psi 2021-02-12T23:36:52Z- handleDepth()-Depth_Keller msg: 1613173012372 epoch sec; seqNo:5318; depth 5.31 2021-02-12T23:36:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:36:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:36:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:36:52Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:36:52Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:36:52Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:36:52Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:36:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:36:52Z- handleDvl() - RDI_Pathfinder msg: 1613173012.37 epoch sec; seqNo:2561 2021-02-12T23:36:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:36:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.06 , 0.09 2021-02-12T23:36:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173012.39 epoch sec; seqNo:5318; 4057412.91 north; 591240.65 east 2021-02-12T23:36:52Z- AHRS_M2 msg: 1613173012.77 epoch sec; seqNo:5271 2021-02-12T23:36:52Z- AHRS_M2 msg: 0.58 phi; 0.41 theta; -0.19 psi 2021-02-12T23:36:52Z- handleDepth()-Depth_Keller msg: 1613173012776 epoch sec; seqNo:5319; depth 5.21 2021-02-12T23:36:52Z- handleDvl() - RDI_Pathfinder msg: 1613173012.79 epoch sec; seqNo:2562 2021-02-12T23:36:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.32 , 114.14 , 131.99 , 84.40 2021-02-12T23:36:52Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.12 , 0.13 2021-02-12T23:36:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173012.81 epoch sec; seqNo:5319; 4057413.29 north; 591240.64 east 2021-02-12T23:36:53Z- AHRS_M2 msg: 1613173013.18 epoch sec; seqNo:5272 2021-02-12T23:36:53Z- AHRS_M2 msg: 0.59 phi; 0.40 theta; -0.17 psi 2021-02-12T23:36:53Z- handleDepth()-Depth_Keller msg: 1613173013180 epoch sec; seqNo:5320; depth 5.07 2021-02-12T23:36:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173013.19 epoch sec; seqNo:5320; 4057413.67 north; 591240.64 east 2021-02-12T23:36:53Z- AHRS_M2 msg: 1613173013.58 epoch sec; seqNo:5273 2021-02-12T23:36:53Z- AHRS_M2 msg: 0.60 phi; 0.38 theta; -0.15 psi 2021-02-12T23:36:53Z- handleDepth()-Depth_Keller msg: 1613173013586 epoch sec; seqNo:5321; depth 4.94 2021-02-12T23:36:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:36:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:36:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:36:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:36:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:36:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:36:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:36:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:36:53Z- handleDvl() - RDI_Pathfinder msg: 1613173013.59 epoch sec; seqNo:2563 2021-02-12T23:36:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:36:53Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.12 , 0.13 2021-02-12T23:36:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173013.60 epoch sec; seqNo:5321; 4057414.05 north; 591240.63 east 2021-02-12T23:36:53Z- AHRS_M2 msg: 1613173013.98 epoch sec; seqNo:5274 2021-02-12T23:36:53Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-12T23:36:53Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-12T23:36:53Z- waiting for 1613173011.76, time is 1613173013.98 2021-02-12T23:36:53Z- LcmTrn::updateTrn() >>>> pose time 1613173013.98, meas time 1613173013.59 <<<< 2021-02-12T23:36:53Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173013.59, ping # 0. 2021-02-12T23:36:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173013.59 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:36:53Z- TerrainNav::motionUpdate() - currEstimate = (4057414, 591240.63, 4.94) 2021-02-12T23:36:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:36:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:36:53Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:36:53Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:36:53Z- TNavBF::Projecting Measurements 2021-02-12T23:36:53Z- TNavFilter::Measurement from time = 1613173013.59 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:36:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:36:53Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:36:53Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:36:53Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:36:53Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:36:53Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:36:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:36:53Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 750.1 EastVariance: 833.0 DepthVariance: 1.7 this->DepthBiasCov: 1.7 2021-02-12T23:36:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:36:53Z- TNavBF::mmse selected from filter -1 2021-02-12T23:36:53Z- variances([0], [2], and psi_berg) = 750.038333 833.033074 0.000000 2021-02-12T23:36:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:36:53Z- LcmTrn::publishEstimates() - MLE : 4057400.55 591194.13 4.94 2021-02-12T23:36:53Z- LcmTrn::publishEstimates() - MMSE : 4057414.05 591240.63 4.94 2021-02-12T23:36:53Z- LcmTrn::publishEstimates() - COVAR: 750.04 833.03 1.71 0.00 2021-02-12T23:36:53Z- handleDepth()-Depth_Keller msg: 1613173013998 epoch sec; seqNo:5322; depth 4.80 2021-02-12T23:36:54Z- handleDvl() - RDI_Pathfinder msg: 1613173014.01 epoch sec; seqNo:2564 2021-02-12T23:36:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:36:54Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:36:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173014.02 epoch sec; seqNo:5322; 4057414.06 north; 591241.86 east 2021-02-12T23:36:54Z- AHRS_M2 msg: 1613173014.39 epoch sec; seqNo:5275 2021-02-12T23:36:54Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-12T23:36:54Z- handleDepth()-Depth_Keller msg: 1613173014397 epoch sec; seqNo:5323; depth 4.63 2021-02-12T23:36:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173014.41 epoch sec; seqNo:5323; 4057414.44 north; 591241.85 east 2021-02-12T23:36:54Z- AHRS_M2 msg: 1613173014.80 epoch sec; seqNo:5276 2021-02-12T23:36:54Z- AHRS_M2 msg: 0.63 phi; 0.28 theta; -0.18 psi 2021-02-12T23:36:54Z- handleDepth()-Depth_Keller msg: 1613173014799 epoch sec; seqNo:5324; depth 4.48 2021-02-12T23:36:54Z- handleDvl() - RDI_Pathfinder msg: 1613173014.81 epoch sec; seqNo:2565 2021-02-12T23:36:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 109.14 , 122.52 , 83.86 2021-02-12T23:36:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.00 , -0.19 2021-02-12T23:36:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173014.82 epoch sec; seqNo:5324; 4057414.82 north; 591241.85 east 2021-02-12T23:36:55Z- AHRS_M2 msg: 1613173015.20 epoch sec; seqNo:5277 2021-02-12T23:36:55Z- AHRS_M2 msg: 0.64 phi; 0.24 theta; -0.20 psi 2021-02-12T23:36:55Z- handleDepth()-Depth_Keller msg: 1613173015201 epoch sec; seqNo:5325; depth 4.32 2021-02-12T23:36:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173015.21 epoch sec; seqNo:5325; 4057414.82 north; 591241.85 east 2021-02-12T23:36:55Z- AHRS_M2 msg: 1613173015.60 epoch sec; seqNo:5278 2021-02-12T23:36:55Z- AHRS_M2 msg: 0.65 phi; 0.19 theta; -0.20 psi 2021-02-12T23:36:55Z- handleDepth()-Depth_Keller msg: 1613173015604 epoch sec; seqNo:5326; depth 4.19 2021-02-12T23:36:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173015.62 epoch sec; seqNo:5326; 4057415.20 north; 591241.84 east 2021-02-12T23:36:56Z- AHRS_M2 msg: 1613173016.00 epoch sec; seqNo:5279 2021-02-12T23:36:56Z- AHRS_M2 msg: 0.65 phi; 0.14 theta; -0.20 psi 2021-02-12T23:36:56Z- handleDepth()-Depth_Keller msg: 1613173016012 epoch sec; seqNo:5327; depth 4.06 2021-02-12T23:36:56Z- handleDvl() - RDI_Pathfinder msg: 1613173016.02 epoch sec; seqNo:2566 2021-02-12T23:36:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 109.75 , 112.72 , 85.95 2021-02-12T23:36:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.13 , -0.05 2021-02-12T23:36:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173016.04 epoch sec; seqNo:5327; 4057415.57 north; 591241.84 east 2021-02-12T23:36:56Z- AHRS_M2 msg: 1613173016.42 epoch sec; seqNo:5280 2021-02-12T23:36:56Z- AHRS_M2 msg: 0.66 phi; 0.05 theta; -0.19 psi 2021-02-12T23:36:56Z- handleDepth()-Depth_Keller msg: 1613173016422 epoch sec; seqNo:5328; depth 3.94 2021-02-12T23:36:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173016.43 epoch sec; seqNo:5328; 4057415.57 north; 591241.84 east 2021-02-12T23:36:56Z- AHRS_M2 msg: 1613173016.82 epoch sec; seqNo:5281 2021-02-12T23:36:56Z- AHRS_M2 msg: 0.67 phi; 0.01 theta; -0.18 psi 2021-02-12T23:36:56Z- handleDepth()-Depth_Keller msg: 1613173016822 epoch sec; seqNo:5329; depth 3.87 2021-02-12T23:36:56Z- handleDvl() - RDI_Pathfinder msg: 1613173016.83 epoch sec; seqNo:2567 2021-02-12T23:36:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 106.91 , 102.44 , 90.55 2021-02-12T23:36:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.12 , 0.12 2021-02-12T23:36:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173016.84 epoch sec; seqNo:5329; 4057415.95 north; 591241.84 east 2021-02-12T23:36:57Z- AHRS_M2 msg: 1613173017.22 epoch sec; seqNo:5282 2021-02-12T23:36:57Z- AHRS_M2 msg: 0.69 phi; -0.03 theta; -0.17 psi 2021-02-12T23:36:57Z- handleDepth()-Depth_Keller msg: 1613173017227 epoch sec; seqNo:5330; depth 3.78 2021-02-12T23:36:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173017.24 epoch sec; seqNo:5330; 4057416.34 north; 591243.05 east 2021-02-12T23:36:57Z- AHRS_M2 msg: 1613173017.62 epoch sec; seqNo:5283 2021-02-12T23:36:57Z- AHRS_M2 msg: 0.71 phi; -0.07 theta; -0.15 psi 2021-02-12T23:36:57Z- handleDepth()-Depth_Keller msg: 1613173017624 epoch sec; seqNo:5331; depth 3.73 2021-02-12T23:36:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173017.63 epoch sec; seqNo:5331; 4057416.34 north; 591243.05 east 2021-02-12T23:36:58Z- AHRS_M2 msg: 1613173018.02 epoch sec; seqNo:5284 2021-02-12T23:36:58Z- AHRS_M2 msg: 0.72 phi; -0.12 theta; -0.12 psi 2021-02-12T23:36:58Z- handleDepth()-Depth_Keller msg: 1613173018028 epoch sec; seqNo:5332; depth 3.70 2021-02-12T23:36:58Z- handleDvl() - RDI_Pathfinder msg: 1613173018.04 epoch sec; seqNo:2568 2021-02-12T23:36:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 106.50 , 95.68 , 95.68 2021-02-12T23:36:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.08 , 0.18 2021-02-12T23:36:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173018.05 epoch sec; seqNo:5332; 4057416.72 north; 591243.05 east 2021-02-12T23:36:58Z- AHRS_M2 msg: 1613173018.43 epoch sec; seqNo:5285 2021-02-12T23:36:58Z- AHRS_M2 msg: 0.74 phi; -0.16 theta; -0.10 psi 2021-02-12T23:36:58Z- handleDepth()-Depth_Keller msg: 1613173018434 epoch sec; seqNo:5333; depth 3.68 2021-02-12T23:36:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173018.44 epoch sec; seqNo:5333; 4057417.10 north; 591243.04 east 2021-02-12T23:36:58Z- AHRS_M2 msg: 1613173018.83 epoch sec; seqNo:5286 2021-02-12T23:36:58Z- AHRS_M2 msg: 0.75 phi; -0.19 theta; -0.09 psi 2021-02-12T23:36:58Z- handleDepth()-Depth_Keller msg: 1613173018836 epoch sec; seqNo:5334; depth 3.68 2021-02-12T23:36:58Z- handleDvl() - RDI_Pathfinder msg: 1613173018.85 epoch sec; seqNo:2569 2021-02-12T23:36:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.62 , 101.43 , 90.62 , 104.14 2021-02-12T23:36:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , 0.17 , -0.05 2021-02-12T23:36:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173018.87 epoch sec; seqNo:5334; 4057417.10 north; 591243.04 east 2021-02-12T23:36:59Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-12T23:36:59Z- waiting for 1613173016.98, time is 1613173018.87 2021-02-12T23:36:59Z- LcmTrn::updateTrn() equating pose 1613173018.87 and meas times 1613173018.87 2021-02-12T23:36:59Z- LcmTrn::updateTrn() >>>> pose time 1613173018.87, meas time 1613173018.87 <<<< 2021-02-12T23:36:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173018.87 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:36:59Z- TerrainNav::motionUpdate() - currEstimate = (4057417, 591243.04, 3.68) 2021-02-12T23:36:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:36:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:36:59Z- TNavBF::Projecting Measurements 2021-02-12T23:36:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:36:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:36:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:36:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:36:59Z- TNavFilter::4 good beams. 2021-02-12T23:36:59Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:36:59Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:36:59Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:36:59Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:36:59Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:36:59Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:36:59Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:36:59Z- TNavBF::mmse selected from filter -1 2021-02-12T23:36:59Z- TerrainMapOctree: (vehN, vehE ) = (4057417.10, 591243.04). 2021-02-12T23:36:59Z- TerrainMapOctree: Min Distance = 4871.64. 2021-02-12T23:36:59Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:36:59Z- There is no specific good beam that all particles have in common. 2021-02-12T23:36:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:36:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.91 K: 0.114 ExpectedMeasDiff: nan currMeas.covariance: 13.14 2021-02-12T23:36:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:36:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.70 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 16.46 2021-02-12T23:36:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:36:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.55 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 13.14 2021-02-12T23:36:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:36:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 17.35 2021-02-12T23:36:59Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:36:59Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:36:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:36:59Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:36:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:36:59Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:36:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:36:59Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:36:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:36:59Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:36:59Z- Filter 0 No beams used in subcloud update 2021-02-12T23:36:59Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:36:59Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:36:59Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 749.9 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-12T23:36:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173018.87 sec. 2021-02-12T23:36:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:36:59Z- TNavBF::mmse selected from filter -1 2021-02-12T23:36:59Z- variances([0], [2], and psi_berg) = 749.942273 833.032985 0.000000 2021-02-12T23:36:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:36:59Z- LcmTrn::publishEstimates() - MLE : 4057403.60 591200.54 3.68 2021-02-12T23:36:59Z- LcmTrn::publishEstimates() - MMSE : 4057417.10 591243.04 3.68 2021-02-12T23:36:59Z- LcmTrn::publishEstimates() - COVAR: 749.94 833.03 1.31 0.00 2021-02-12T23:36:59Z- AHRS_M2 msg: 1613173019.24 epoch sec; seqNo:5287 2021-02-12T23:36:59Z- AHRS_M2 msg: 0.75 phi; -0.23 theta; -0.11 psi 2021-02-12T23:36:59Z- handleDepth()-Depth_Keller msg: 1613173019240 epoch sec; seqNo:5335; depth 3.67 2021-02-12T23:36:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173019.25 epoch sec; seqNo:5335; 4057417.12 north; 591244.27 east 2021-02-12T23:36:59Z- AHRS_M2 msg: 1613173019.64 epoch sec; seqNo:5288 2021-02-12T23:36:59Z- AHRS_M2 msg: 0.75 phi; -0.25 theta; -0.15 psi 2021-02-12T23:36:59Z- handleDepth()-Depth_Keller msg: 1613173019644 epoch sec; seqNo:5336; depth 3.67 2021-02-12T23:36:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173019.66 epoch sec; seqNo:5336; 4057417.49 north; 591244.26 east 2021-02-12T23:37:00Z- AHRS_M2 msg: 1613173020.05 epoch sec; seqNo:5289 2021-02-12T23:37:00Z- AHRS_M2 msg: 0.75 phi; -0.27 theta; -0.19 psi 2021-02-12T23:37:00Z- handleDepth()-Depth_Keller msg: 1613173020050 epoch sec; seqNo:5337; depth 3.71 2021-02-12T23:37:00Z- handleDvl() - RDI_Pathfinder msg: 1613173020.06 epoch sec; seqNo:2570 2021-02-12T23:37:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.90 , 100.01 , 86.49 , 110.83 2021-02-12T23:37:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.07 , -0.22 2021-02-12T23:37:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173020.08 epoch sec; seqNo:5337; 4057417.87 north; 591244.26 east 2021-02-12T23:37:00Z- AHRS_M2 msg: 1613173020.45 epoch sec; seqNo:5290 2021-02-12T23:37:00Z- AHRS_M2 msg: 0.74 phi; -0.29 theta; -0.22 psi 2021-02-12T23:37:00Z- handleDepth()-Depth_Keller msg: 1613173020452 epoch sec; seqNo:5338; depth 3.73 2021-02-12T23:37:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173020.47 epoch sec; seqNo:5338; 4057417.87 north; 591244.26 east 2021-02-12T23:37:00Z- AHRS_M2 msg: 1613173020.85 epoch sec; seqNo:5291 2021-02-12T23:37:00Z- AHRS_M2 msg: 0.73 phi; -0.31 theta; -0.24 psi 2021-02-12T23:37:00Z- handleDepth()-Depth_Keller msg: 1613173020856 epoch sec; seqNo:5339; depth 3.78 2021-02-12T23:37:00Z- handleDvl() - RDI_Pathfinder msg: 1613173020.87 epoch sec; seqNo:2571 2021-02-12T23:37:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:37:00Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:37:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173020.89 epoch sec; seqNo:5339; 4057418.25 north; 591244.25 east 2021-02-12T23:37:01Z- AHRS_M2 msg: 1613173021.26 epoch sec; seqNo:5292 2021-02-12T23:37:01Z- AHRS_M2 msg: 0.72 phi; -0.32 theta; -0.24 psi 2021-02-12T23:37:01Z- handleDepth()-Depth_Keller msg: 1613173021260 epoch sec; seqNo:5340; depth 3.86 2021-02-12T23:37:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173021.27 epoch sec; seqNo:5340; 4057418.63 north; 591244.25 east 2021-02-12T23:37:01Z- AHRS_M2 msg: 1613173021.66 epoch sec; seqNo:5293 2021-02-12T23:37:01Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.22 psi 2021-02-12T23:37:01Z- handleDepth()-Depth_Keller msg: 1613173021666 epoch sec; seqNo:5341; depth 3.94 2021-02-12T23:37:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173021.67 epoch sec; seqNo:5341; 4057418.63 north; 591244.25 east 2021-02-12T23:37:02Z- AHRS_M2 msg: 1613173022.07 epoch sec; seqNo:5294 2021-02-12T23:37:02Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.22 psi 2021-02-12T23:37:02Z- handleDepth()-Depth_Keller msg: 1613173022069 epoch sec; seqNo:5342; depth 4.04 2021-02-12T23:37:02Z- handleDvl() - RDI_Pathfinder msg: 1613173022.10 epoch sec; seqNo:2572 2021-02-12T23:37:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.80 , 115.09 , 85.34 , 124.01 2021-02-12T23:37:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.02 , 0.18 2021-02-12T23:37:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173022.12 epoch sec; seqNo:5342; 4057419.02 north; 591245.47 east 2021-02-12T23:37:02Z- AHRS_M2 msg: 1613173022.47 epoch sec; seqNo:5295 2021-02-12T23:37:02Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.22 psi 2021-02-12T23:37:02Z- handleDepth()-Depth_Keller msg: 1613173022472 epoch sec; seqNo:5343; depth 4.14 2021-02-12T23:37:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173022.48 epoch sec; seqNo:5343; 4057419.02 north; 591245.47 east 2021-02-12T23:37:02Z- AHRS_M2 msg: 1613173022.87 epoch sec; seqNo:5296 2021-02-12T23:37:02Z- AHRS_M2 msg: 0.71 phi; -0.34 theta; -0.24 psi 2021-02-12T23:37:02Z- handleDepth()-Depth_Keller msg: 1613173022881 epoch sec; seqNo:5344; depth 4.24 2021-02-12T23:37:02Z- handleDvl() - RDI_Pathfinder msg: 1613173022.89 epoch sec; seqNo:2573 2021-02-12T23:37:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.35 , 112.65 , 84.40 , 126.04 2021-02-12T23:37:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.12 , -0.11 2021-02-12T23:37:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173022.90 epoch sec; seqNo:5344; 4057419.40 north; 591245.46 east 2021-02-12T23:37:03Z- AHRS_M2 msg: 1613173023.28 epoch sec; seqNo:5297 2021-02-12T23:37:03Z- AHRS_M2 msg: 0.72 phi; -0.36 theta; -0.25 psi 2021-02-12T23:37:03Z- handleDepth()-Depth_Keller msg: 1613173023286 epoch sec; seqNo:5345; depth 4.35 2021-02-12T23:37:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173023.30 epoch sec; seqNo:5345; 4057419.78 north; 591245.46 east 2021-02-12T23:37:03Z- AHRS_M2 msg: 1613173023.68 epoch sec; seqNo:5298 2021-02-12T23:37:03Z- AHRS_M2 msg: 0.72 phi; -0.38 theta; -0.26 psi 2021-02-12T23:37:03Z- handleDepth()-Depth_Keller msg: 1613173023691 epoch sec; seqNo:5346; depth 4.45 2021-02-12T23:37:03Z- handleDvl() - RDI_Pathfinder msg: 1613173023.73 epoch sec; seqNo:2574 2021-02-12T23:37:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 115.69 , 84.46 , 129.08 2021-02-12T23:37:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.11 , -0.08 2021-02-12T23:37:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173023.74 epoch sec; seqNo:5346; 4057419.78 north; 591245.46 east 2021-02-12T23:37:04Z- AHRS_M2 msg: 1613173024.09 epoch sec; seqNo:5299 2021-02-12T23:37:04Z- AHRS_M2 msg: 0.71 phi; -0.39 theta; -0.26 psi 2021-02-12T23:37:04Z- handleDepth()-Depth_Keller msg: 1613173024091 epoch sec; seqNo:5347; depth 4.57 2021-02-12T23:37:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173024.10 epoch sec; seqNo:5347; 4057420.16 north; 591245.45 east 2021-02-12T23:37:04Z- LcmTrn::handleMessages() - handling 44 messages took 0.80 ms and 128 calls... 2021-02-12T23:37:04Z- waiting for 1613173021.87, time is 1613173024.10 2021-02-12T23:37:04Z- LcmTrn::updateTrn() >>>> pose time 1613173024.10, meas time 1613173023.73 <<<< 2021-02-12T23:37:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173023.73 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:37:04Z- TerrainNav::motionUpdate() - currEstimate = (4057420, 591245.45, 4.57) 2021-02-12T23:37:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:37:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:37:04Z- TNavBF::Projecting Measurements 2021-02-12T23:37:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:37:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:37:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:37:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:37:04Z- TNavFilter::4 good beams. 2021-02-12T23:37:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:37:04Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:37:04Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:37:04Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:37:04Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:37:04Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:37:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:37:04Z- TNavBF::mmse selected from filter -1 2021-02-12T23:37:04Z- TerrainMapOctree: (vehN, vehE ) = (4057419.94, 591245.28). 2021-02-12T23:37:04Z- TerrainMapOctree: Min Distance = 4868.03. 2021-02-12T23:37:04Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:37:04Z- There is no specific good beam that all particles have in common. 2021-02-12T23:37:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:37:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.49 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-12T23:37:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:37:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 21.42 2021-02-12T23:37:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:37:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 11.41 2021-02-12T23:37:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:37:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 26.66 2021-02-12T23:37:04Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:37:04Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:37:04Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:37:04Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:37:04Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:37:04Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:37:04Z- Filter 0 No beams used in subcloud update 2021-02-12T23:37:04Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:37:04Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:37:04Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 749.8 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:37:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173023.73 sec. 2021-02-12T23:37:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:37:04Z- TNavBF::mmse selected from filter -1 2021-02-12T23:37:04Z- variances([0], [2], and psi_berg) = 749.824269 833.032875 0.000000 2021-02-12T23:37:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:37:04Z- LcmTrn::publishEstimates() - MLE : 4057406.66 591202.95 4.57 2021-02-12T23:37:04Z- LcmTrn::publishEstimates() - MMSE : 4057420.16 591245.45 4.57 2021-02-12T23:37:04Z- LcmTrn::publishEstimates() - COVAR: 749.82 833.03 1.15 0.00 2021-02-12T23:37:04Z- AHRS_M2 msg: 1613173024.49 epoch sec; seqNo:5300 2021-02-12T23:37:04Z- AHRS_M2 msg: 0.70 phi; -0.40 theta; -0.25 psi 2021-02-12T23:37:04Z- handleDepth()-Depth_Keller msg: 1613173024493 epoch sec; seqNo:5348; depth 4.68 2021-02-12T23:37:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173024.50 epoch sec; seqNo:5348; 4057420.17 north; 591246.67 east 2021-02-12T23:37:04Z- AHRS_M2 msg: 1613173024.89 epoch sec; seqNo:5301 2021-02-12T23:37:04Z- AHRS_M2 msg: 0.68 phi; -0.41 theta; -0.24 psi 2021-02-12T23:37:04Z- handleDepth()-Depth_Keller msg: 1613173024896 epoch sec; seqNo:5349; depth 4.81 2021-02-12T23:37:04Z- handleDvl() - RDI_Pathfinder msg: 1613173024.91 epoch sec; seqNo:2575 2021-02-12T23:37:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.51 , 119.82 , 84.13 , 136.18 2021-02-12T23:37:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.62 , -0.12 , 0.15 2021-02-12T23:37:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173024.92 epoch sec; seqNo:5349; 4057420.55 north; 591246.67 east 2021-02-12T23:37:05Z- AHRS_M2 msg: 1613173025.30 epoch sec; seqNo:5302 2021-02-12T23:37:05Z- AHRS_M2 msg: 0.66 phi; -0.41 theta; -0.24 psi 2021-02-12T23:37:05Z- handleDepth()-Depth_Keller msg: 1613173025305 epoch sec; seqNo:5350; depth 4.92 2021-02-12T23:37:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173025.32 epoch sec; seqNo:5350; 4057420.55 north; 591246.67 east 2021-02-12T23:37:05Z- AHRS_M2 msg: 1613173025.70 epoch sec; seqNo:5303 2021-02-12T23:37:05Z- AHRS_M2 msg: 0.64 phi; -0.41 theta; -0.25 psi 2021-02-12T23:37:05Z- handleDepth()-Depth_Keller msg: 1613173025708 epoch sec; seqNo:5351; depth 5.05 2021-02-12T23:37:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173025.73 epoch sec; seqNo:5351; 4057420.93 north; 591246.67 east 2021-02-12T23:37:06Z- AHRS_M2 msg: 1613173026.11 epoch sec; seqNo:5304 2021-02-12T23:37:06Z- AHRS_M2 msg: 0.63 phi; -0.40 theta; -0.25 psi 2021-02-12T23:37:06Z- handleDepth()-Depth_Keller msg: 1613173026115 epoch sec; seqNo:5352; depth 5.20 2021-02-12T23:37:06Z- handleDvl() - RDI_Pathfinder msg: 1613173026.13 epoch sec; seqNo:2576 2021-02-12T23:37:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.85 , 116.64 , 82.44 , 138.95 2021-02-12T23:37:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.13 , 0.22 2021-02-12T23:37:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173026.14 epoch sec; seqNo:5352; 4057420.93 north; 591246.67 east 2021-02-12T23:37:06Z- AHRS_M2 msg: 1613173026.51 epoch sec; seqNo:5305 2021-02-12T23:37:06Z- AHRS_M2 msg: 0.61 phi; -0.39 theta; -0.25 psi 2021-02-12T23:37:06Z- handleDepth()-Depth_Keller msg: 1613173026512 epoch sec; seqNo:5353; depth 5.34 2021-02-12T23:37:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173026.53 epoch sec; seqNo:5353; 4057421.31 north; 591246.66 east 2021-02-12T23:37:06Z- AHRS_M2 msg: 1613173026.91 epoch sec; seqNo:5306 2021-02-12T23:37:06Z- AHRS_M2 msg: 0.60 phi; -0.38 theta; -0.26 psi 2021-02-12T23:37:06Z- handleDepth()-Depth_Keller msg: 1613173026921 epoch sec; seqNo:5354; depth 5.48 2021-02-12T23:37:06Z- handleDvl() - RDI_Pathfinder msg: 1613173026.94 epoch sec; seqNo:2577 2021-02-12T23:37:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.44 , 118.26 , 0.00 , 137.60 2021-02-12T23:37:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.21 , -0.03 2021-02-12T23:37:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173026.95 epoch sec; seqNo:5354; 4057421.31 north; 591246.66 east 2021-02-12T23:37:07Z- AHRS_M2 msg: 1613173027.32 epoch sec; seqNo:5307 2021-02-12T23:37:07Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.27 psi 2021-02-12T23:37:07Z- handleDepth()-Depth_Keller msg: 1613173027321 epoch sec; seqNo:5355; depth 5.63 2021-02-12T23:37:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173027.33 epoch sec; seqNo:5355; 4057421.70 north; 591247.88 east 2021-02-12T23:37:07Z- AHRS_M2 msg: 1613173027.72 epoch sec; seqNo:5308 2021-02-12T23:37:07Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.27 psi 2021-02-12T23:37:07Z- handleDepth()-Depth_Keller msg: 1613173027726 epoch sec; seqNo:5356; depth 5.77 2021-02-12T23:37:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173027.73 epoch sec; seqNo:5356; 4057422.08 north; 591247.88 east 2021-02-12T23:37:08Z- handleDepth()-Depth_Keller msg: 1613173028153 epoch sec; seqNo:5357; depth 5.92 2021-02-12T23:37:08Z- handleDvl() - RDI_Pathfinder msg: 1613173028.18 epoch sec; seqNo:2578 2021-02-12T23:37:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.80 , 118.06 , 82.37 , 131.45 2021-02-12T23:37:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , -0.26 2021-02-12T23:37:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173028.20 epoch sec; seqNo:5357; 4057422.46 north; 591247.87 east 2021-02-12T23:37:08Z- AHRS_M2 msg: 1613173028.53 epoch sec; seqNo:5309 2021-02-12T23:37:08Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.28 psi 2021-02-12T23:37:08Z- handleDepth()-Depth_Keller msg: 1613173028533 epoch sec; seqNo:5358; depth 6.09 2021-02-12T23:37:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173028.54 epoch sec; seqNo:5358; 4057422.84 north; 591247.87 east 2021-02-12T23:37:08Z- AHRS_M2 msg: 1613173028.93 epoch sec; seqNo:5310 2021-02-12T23:37:08Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.27 psi 2021-02-12T23:37:08Z- handleDepth()-Depth_Keller msg: 1613173028938 epoch sec; seqNo:5359; depth 6.21 2021-02-12T23:37:08Z- handleDvl() - RDI_Pathfinder msg: 1613173028.95 epoch sec; seqNo:2579 2021-02-12T23:37:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.24 , 118.47 , 84.26 , 124.42 2021-02-12T23:37:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.23 , -0.22 2021-02-12T23:37:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173028.96 epoch sec; seqNo:5359; 4057423.22 north; 591247.86 east 2021-02-12T23:37:09Z- LcmTrn::handleMessages() - handling 40 messages took 0.62 ms and 128 calls... 2021-02-12T23:37:09Z- waiting for 1613173027.10, time is 1613173028.96 2021-02-12T23:37:09Z- LcmTrn::updateTrn() equating pose 1613173028.96 and meas times 1613173028.96 2021-02-12T23:37:09Z- LcmTrn::updateTrn() >>>> pose time 1613173028.96, meas time 1613173028.96 <<<< 2021-02-12T23:37:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173028.96 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:37:09Z- TerrainNav::motionUpdate() - currEstimate = (4057423, 591247.86, 6.21) 2021-02-12T23:37:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:37:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:37:09Z- TNavBF::Projecting Measurements 2021-02-12T23:37:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:37:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:37:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:37:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:37:09Z- TNavFilter::4 good beams. 2021-02-12T23:37:09Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:37:09Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:37:09Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:37:09Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:37:09Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:37:09Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:37:09Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:37:09Z- TNavBF::mmse selected from filter -1 2021-02-12T23:37:09Z- TerrainMapOctree: (vehN, vehE ) = (4057423.22, 591247.86). 2021-02-12T23:37:09Z- TerrainMapOctree: Min Distance = 4863.86. 2021-02-12T23:37:09Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:37:09Z- There is no specific good beam that all particles have in common. 2021-02-12T23:37:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:37:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 12.18 2021-02-12T23:37:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:37:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 22.45 2021-02-12T23:37:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:37:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 11.36 2021-02-12T23:37:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:37:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 24.77 2021-02-12T23:37:09Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:37:09Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:37:09Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:37:09Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:37:09Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:37:09Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:37:09Z- Filter 0 No beams used in subcloud update 2021-02-12T23:37:09Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:37:09Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:37:09Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 749.7 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:37:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173028.96 sec. 2021-02-12T23:37:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:37:09Z- TNavBF::mmse selected from filter -1 2021-02-12T23:37:09Z- variances([0], [2], and psi_berg) = 749.728481 833.032786 0.000000 2021-02-12T23:37:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:37:09Z- LcmTrn::publishEstimates() - MLE : 4057409.72 591205.36 6.21 2021-02-12T23:37:09Z- LcmTrn::publishEstimates() - MMSE : 4057423.22 591247.86 6.21 2021-02-12T23:37:09Z- LcmTrn::publishEstimates() - COVAR: 749.73 833.03 1.03 0.00 2021-02-12T23:37:09Z- AHRS_M2 msg: 1613173029.34 epoch sec; seqNo:5311 2021-02-12T23:37:09Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.25 psi 2021-02-12T23:37:09Z- handleDepth()-Depth_Keller msg: 1613173029342 epoch sec; seqNo:5360; depth 6.35 2021-02-12T23:37:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173029.35 epoch sec; seqNo:5360; 4057423.22 north; 591247.86 east 2021-02-12T23:37:09Z- AHRS_M2 msg: 1613173029.74 epoch sec; seqNo:5312 2021-02-12T23:37:09Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.22 psi 2021-02-12T23:37:09Z- handleDepth()-Depth_Keller msg: 1613173029744 epoch sec; seqNo:5361; depth 6.47 2021-02-12T23:37:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173029.77 epoch sec; seqNo:5361; 4057423.59 north; 591247.86 east 2021-02-12T23:37:10Z- AHRS_M2 msg: 1613173030.15 epoch sec; seqNo:5313 2021-02-12T23:37:10Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.20 psi 2021-02-12T23:37:10Z- handleDepth()-Depth_Keller msg: 1613173030154 epoch sec; seqNo:5362; depth 6.62 2021-02-12T23:37:10Z- handleDvl() - RDI_Pathfinder msg: 1613173030.17 epoch sec; seqNo:2580 2021-02-12T23:37:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.88 , 112.65 , 82.91 , 118.60 2021-02-12T23:37:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.23 , 0.04 2021-02-12T23:37:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173030.19 epoch sec; seqNo:5362; 4057423.97 north; 591247.86 east 2021-02-12T23:37:10Z- AHRS_M2 msg: 1613173030.55 epoch sec; seqNo:5314 2021-02-12T23:37:10Z- AHRS_M2 msg: 0.57 phi; -0.30 theta; -0.18 psi 2021-02-12T23:37:10Z- handleDepth()-Depth_Keller msg: 1613173030552 epoch sec; seqNo:5363; depth 6.76 2021-02-12T23:37:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173030.56 epoch sec; seqNo:5363; 4057424.35 north; 591247.85 east 2021-02-12T23:37:10Z- AHRS_M2 msg: 1613173030.95 epoch sec; seqNo:5315 2021-02-12T23:37:10Z- AHRS_M2 msg: 0.56 phi; -0.29 theta; -0.17 psi 2021-02-12T23:37:10Z- handleDepth()-Depth_Keller msg: 1613173030958 epoch sec; seqNo:5364; depth 6.87 2021-02-12T23:37:10Z- handleDvl() - RDI_Pathfinder msg: 1613173030.97 epoch sec; seqNo:2581 2021-02-12T23:37:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 106.50 , 82.17 , 115.96 2021-02-12T23:37:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.03 , 0.25 2021-02-12T23:37:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173030.98 epoch sec; seqNo:5364; 4057424.74 north; 591249.07 east 2021-02-12T23:37:11Z- AHRS_M2 msg: 1613173031.36 epoch sec; seqNo:5316 2021-02-12T23:37:11Z- AHRS_M2 msg: 0.56 phi; -0.29 theta; -0.17 psi 2021-02-12T23:37:11Z- handleDepth()-Depth_Keller msg: 1613173031360 epoch sec; seqNo:5365; depth 6.99 2021-02-12T23:37:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173031.38 epoch sec; seqNo:5365; 4057424.74 north; 591249.07 east 2021-02-12T23:37:11Z- AHRS_M2 msg: 1613173031.76 epoch sec; seqNo:5317 2021-02-12T23:37:11Z- AHRS_M2 msg: 0.56 phi; -0.28 theta; -0.16 psi 2021-02-12T23:37:11Z- handleDepth()-Depth_Keller msg: 1613173031764 epoch sec; seqNo:5366; depth 7.10 2021-02-12T23:37:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173031.77 epoch sec; seqNo:5366; 4057425.12 north; 591249.06 east 2021-02-12T23:37:12Z- AHRS_M2 msg: 1613173032.17 epoch sec; seqNo:5318 2021-02-12T23:37:12Z- AHRS_M2 msg: 0.56 phi; -0.28 theta; -0.16 psi 2021-02-12T23:37:12Z- handleDepth()-Depth_Keller msg: 1613173032171 epoch sec; seqNo:5367; depth 7.19 2021-02-12T23:37:12Z- handleDvl() - RDI_Pathfinder msg: 1613173032.19 epoch sec; seqNo:2582 2021-02-12T23:37:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 103.93 , 0.00 , 114.75 2021-02-12T23:37:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.18 , 0.18 2021-02-12T23:37:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173032.22 epoch sec; seqNo:5367; 4057425.12 north; 591249.06 east 2021-02-12T23:37:12Z- AHRS_M2 msg: 1613173032.57 epoch sec; seqNo:5319 2021-02-12T23:37:12Z- AHRS_M2 msg: 0.56 phi; -0.27 theta; -0.15 psi 2021-02-12T23:37:12Z- handleDepth()-Depth_Keller msg: 1613173032574 epoch sec; seqNo:5368; depth 7.31 2021-02-12T23:37:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173032.59 epoch sec; seqNo:5368; 4057425.50 north; 591249.06 east 2021-02-12T23:37:12Z- AHRS_M2 msg: 1613173032.97 epoch sec; seqNo:5320 2021-02-12T23:37:12Z- AHRS_M2 msg: 0.57 phi; -0.27 theta; -0.15 psi 2021-02-12T23:37:12Z- handleDepth()-Depth_Keller msg: 1613173032976 epoch sec; seqNo:5369; depth 7.41 2021-02-12T23:37:13Z- handleDvl() - RDI_Pathfinder msg: 1613173033.00 epoch sec; seqNo:2583 2021-02-12T23:37:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 102.38 , 0.00 , 112.79 2021-02-12T23:37:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.18 , -0.05 2021-02-12T23:37:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173033.01 epoch sec; seqNo:5369; 4057425.50 north; 591249.06 east 2021-02-12T23:37:13Z- AHRS_M2 msg: 1613173033.38 epoch sec; seqNo:5321 2021-02-12T23:37:13Z- AHRS_M2 msg: 0.58 phi; -0.26 theta; -0.16 psi 2021-02-12T23:37:13Z- handleDepth()-Depth_Keller msg: 1613173033393 epoch sec; seqNo:5370; depth 7.50 2021-02-12T23:37:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173033.40 epoch sec; seqNo:5370; 4057425.89 north; 591250.28 east 2021-02-12T23:37:13Z- AHRS_M2 msg: 1613173033.79 epoch sec; seqNo:5322 2021-02-12T23:37:13Z- AHRS_M2 msg: 0.59 phi; -0.26 theta; -0.16 psi 2021-02-12T23:37:13Z- handleDepth()-Depth_Keller msg: 1613173033789 epoch sec; seqNo:5371; depth 7.59 2021-02-12T23:37:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173033.80 epoch sec; seqNo:5371; 4057426.27 north; 591250.27 east 2021-02-12T23:37:14Z- AHRS_M2 msg: 1613173034.19 epoch sec; seqNo:5323 2021-02-12T23:37:14Z- AHRS_M2 msg: 0.60 phi; -0.26 theta; -0.16 psi 2021-02-12T23:37:14Z- handleDepth()-Depth_Keller msg: 1613173034198 epoch sec; seqNo:5372; depth 7.69 2021-02-12T23:37:14Z- handleDvl() - RDI_Pathfinder msg: 1613173034.21 epoch sec; seqNo:2584 2021-02-12T23:37:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:37:14Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:37:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173034.22 epoch sec; seqNo:5372; 4057426.27 north; 591250.27 east 2021-02-12T23:37:14Z- LcmTrn::handleMessages() - handling 44 messages took 0.73 ms and 130 calls... 2021-02-12T23:37:14Z- waiting for 1613173031.96, time is 1613173034.22 2021-02-12T23:37:14Z- LcmTrn::updateTrn() equating pose 1613173034.22 and meas times 1613173034.22 2021-02-12T23:37:14Z- LcmTrn::updateTrn() >>>> pose time 1613173034.22, meas time 1613173034.22 <<<< 2021-02-12T23:37:14Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173034.22, ping # 0. 2021-02-12T23:37:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173034.22 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:37:14Z- TerrainNav::motionUpdate() - currEstimate = (4057426, 591250.27, 7.69) 2021-02-12T23:37:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:37:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:37:14Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:37:14Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:37:14Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173034.22 sec.; No relevant navigation data available 2021-02-12T23:37:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:37:14Z- TNavBF::mmse selected from filter -1 2021-02-12T23:37:14Z- variances([0], [2], and psi_berg) = 749.632815 833.032698 0.000000 2021-02-12T23:37:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:37:14Z- LcmTrn::publishEstimates() - MLE : 4057412.77 591207.77 7.69 2021-02-12T23:37:14Z- LcmTrn::publishEstimates() - MMSE : 4057426.27 591250.27 7.69 2021-02-12T23:37:14Z- LcmTrn::publishEstimates() - COVAR: 749.63 833.03 1.23 0.00 2021-02-12T23:37:14Z- AHRS_M2 msg: 1613173034.59 epoch sec; seqNo:5324 2021-02-12T23:37:14Z- AHRS_M2 msg: 0.61 phi; -0.26 theta; -0.16 psi 2021-02-12T23:37:14Z- handleDepth()-Depth_Keller msg: 1613173034592 epoch sec; seqNo:5373; depth 7.79 2021-02-12T23:37:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173034.61 epoch sec; seqNo:5373; 4057426.65 north; 591250.27 east 2021-02-12T23:37:14Z- AHRS_M2 msg: 1613173034.99 epoch sec; seqNo:5325 2021-02-12T23:37:14Z- AHRS_M2 msg: 0.61 phi; -0.26 theta; -0.15 psi 2021-02-12T23:37:14Z- handleDepth()-Depth_Keller msg: 1613173034996 epoch sec; seqNo:5374; depth 7.88 2021-02-12T23:37:15Z- handleDvl() - RDI_Pathfinder msg: 1613173035.01 epoch sec; seqNo:2585 2021-02-12T23:37:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.37 , 105.22 , 81.42 , 111.17 2021-02-12T23:37:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.13 , -0.18 2021-02-12T23:37:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173035.03 epoch sec; seqNo:5374; 4057427.03 north; 591250.27 east 2021-02-12T23:37:15Z- AHRS_M2 msg: 1613173035.40 epoch sec; seqNo:5326 2021-02-12T23:37:15Z- AHRS_M2 msg: 0.61 phi; -0.26 theta; -0.14 psi 2021-02-12T23:37:15Z- handleDepth()-Depth_Keller msg: 1613173035400 epoch sec; seqNo:5375; depth 7.98 2021-02-12T23:37:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173035.41 epoch sec; seqNo:5375; 4057427.41 north; 591250.26 east 2021-02-12T23:37:15Z- AHRS_M2 msg: 1613173035.80 epoch sec; seqNo:5327 2021-02-12T23:37:15Z- AHRS_M2 msg: 0.61 phi; -0.26 theta; -0.14 psi 2021-02-12T23:37:15Z- handleDepth()-Depth_Keller msg: 1613173035813 epoch sec; seqNo:5376; depth 8.10 2021-02-12T23:37:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173035.82 epoch sec; seqNo:5376; 4057427.80 north; 591251.48 east 2021-02-12T23:37:16Z- AHRS_M2 msg: 1613173036.20 epoch sec; seqNo:5328 2021-02-12T23:37:16Z- AHRS_M2 msg: 0.61 phi; -0.26 theta; -0.13 psi 2021-02-12T23:37:16Z- handleDepth()-Depth_Keller msg: 1613173036208 epoch sec; seqNo:5377; depth 8.19 2021-02-12T23:37:16Z- handleDvl() - RDI_Pathfinder msg: 1613173036.23 epoch sec; seqNo:2586 2021-02-12T23:37:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 101.43 , 82.50 , 109.54 2021-02-12T23:37:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.21 , -0.10 2021-02-12T23:37:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173036.24 epoch sec; seqNo:5377; 4057428.18 north; 591251.47 east 2021-02-12T23:37:16Z- AHRS_M2 msg: 1613173036.61 epoch sec; seqNo:5329 2021-02-12T23:37:16Z- AHRS_M2 msg: 0.61 phi; -0.26 theta; -0.14 psi 2021-02-12T23:37:16Z- handleDepth()-Depth_Keller msg: 1613173036612 epoch sec; seqNo:5378; depth 8.30 2021-02-12T23:37:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173036.62 epoch sec; seqNo:5378; 4057428.56 north; 591251.47 east 2021-02-12T23:37:17Z- AHRS_M2 msg: 1613173037.01 epoch sec; seqNo:5330 2021-02-12T23:37:17Z- AHRS_M2 msg: 0.61 phi; -0.26 theta; -0.15 psi 2021-02-12T23:37:17Z- handleDepth()-Depth_Keller msg: 1613173037015 epoch sec; seqNo:5379; depth 8.39 2021-02-12T23:37:17Z- handleDvl() - RDI_Pathfinder msg: 1613173037.03 epoch sec; seqNo:2587 2021-02-12T23:37:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.98 , 96.09 , 79.69 , 109.61 2021-02-12T23:37:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.19 , 0.01 2021-02-12T23:37:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173037.04 epoch sec; seqNo:5379; 4057428.94 north; 591251.47 east 2021-02-12T23:37:17Z- AHRS_M2 msg: 1613173037.41 epoch sec; seqNo:5331 2021-02-12T23:37:17Z- AHRS_M2 msg: 0.61 phi; -0.26 theta; -0.15 psi 2021-02-12T23:37:17Z- handleDepth()-Depth_Keller msg: 1613173037429 epoch sec; seqNo:5380; depth 8.50 2021-02-12T23:37:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173037.44 epoch sec; seqNo:5380; 4057428.94 north; 591251.47 east 2021-02-12T23:37:17Z- AHRS_M2 msg: 1613173037.85 epoch sec; seqNo:5332 2021-02-12T23:37:17Z- AHRS_M2 msg: 0.61 phi; -0.26 theta; -0.15 psi 2021-02-12T23:37:17Z- handleDepth()-Depth_Keller msg: 1613173037855 epoch sec; seqNo:5381; depth 8.61 2021-02-12T23:37:17Z- handleDvl() - RDI_Pathfinder msg: 1613173037.87 epoch sec; seqNo:2588 2021-02-12T23:37:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.76 , 96.23 , 81.36 , 108.39 2021-02-12T23:37:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.11 , 0.06 2021-02-12T23:37:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173037.88 epoch sec; seqNo:5381; 4057429.32 north; 591251.46 east 2021-02-12T23:37:18Z- AHRS_M2 msg: 1613173038.22 epoch sec; seqNo:5333 2021-02-12T23:37:18Z- AHRS_M2 msg: 0.61 phi; -0.27 theta; -0.16 psi 2021-02-12T23:37:18Z- handleDepth()-Depth_Keller msg: 1613173038233 epoch sec; seqNo:5382; depth 8.73 2021-02-12T23:37:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173038.24 epoch sec; seqNo:5382; 4057429.70 north; 591251.46 east 2021-02-12T23:37:18Z- AHRS_M2 msg: 1613173038.63 epoch sec; seqNo:5334 2021-02-12T23:37:18Z- AHRS_M2 msg: 0.62 phi; -0.27 theta; -0.16 psi 2021-02-12T23:37:18Z- handleDepth()-Depth_Keller msg: 1613173038634 epoch sec; seqNo:5383; depth 8.81 2021-02-12T23:37:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173038.64 epoch sec; seqNo:5383; 4057430.07 north; 591251.45 east 2021-02-12T23:37:19Z- AHRS_M2 msg: 1613173039.03 epoch sec; seqNo:5335 2021-02-12T23:37:19Z- AHRS_M2 msg: 0.62 phi; -0.28 theta; -0.16 psi 2021-02-12T23:37:19Z- handleDepth()-Depth_Keller msg: 1613173039036 epoch sec; seqNo:5384; depth 8.90 2021-02-12T23:37:19Z- handleDvl() - RDI_Pathfinder msg: 1613173039.05 epoch sec; seqNo:2589 2021-02-12T23:37:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.83 , 100.35 , 80.07 , 109.81 2021-02-12T23:37:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , 0.02 , 0.01 2021-02-12T23:37:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173039.06 epoch sec; seqNo:5384; 4057430.09 north; 591252.67 east 2021-02-12T23:37:19Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-12T23:37:19Z- waiting for 1613173037.22, time is 1613173039.06 2021-02-12T23:37:19Z- LcmTrn::updateTrn() equating pose 1613173039.06 and meas times 1613173039.06 2021-02-12T23:37:19Z- LcmTrn::updateTrn() >>>> pose time 1613173039.06, meas time 1613173039.06 <<<< 2021-02-12T23:37:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173039.06 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:37:19Z- TerrainNav::motionUpdate() - currEstimate = (4057430, 591252.67, 8.90) 2021-02-12T23:37:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:37:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:37:19Z- TNavBF::Projecting Measurements 2021-02-12T23:37:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:37:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:37:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:37:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:37:19Z- TNavFilter::4 good beams. 2021-02-12T23:37:19Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:37:19Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:37:19Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:37:19Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:37:19Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:37:19Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:37:19Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:37:19Z- TNavBF::mmse selected from filter -1 2021-02-12T23:37:19Z- TerrainMapOctree: (vehN, vehE ) = (4057430.09, 591252.67). 2021-02-12T23:37:19Z- TerrainMapOctree: Min Distance = 4855.48. 2021-02-12T23:37:19Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:37:19Z- There is no specific good beam that all particles have in common. 2021-02-12T23:37:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:37:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.46 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 12.06 2021-02-12T23:37:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:37:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.32 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 16.11 2021-02-12T23:37:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:37:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 10.26 2021-02-12T23:37:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:37:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 19.29 2021-02-12T23:37:19Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:37:19Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:37:19Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:37:19Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:37:19Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:37:19Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:37:19Z- Filter 0 No beams used in subcloud update 2021-02-12T23:37:19Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:37:19Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:37:19Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 749.5 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:37:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173039.06 sec. 2021-02-12T23:37:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:37:19Z- TNavBF::mmse selected from filter -1 2021-02-12T23:37:19Z- variances([0], [2], and psi_berg) = 749.529996 833.032603 0.000000 2021-02-12T23:37:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:37:19Z- LcmTrn::publishEstimates() - MLE : 4057416.59 591210.17 8.90 2021-02-12T23:37:19Z- LcmTrn::publishEstimates() - MMSE : 4057430.09 591252.67 8.90 2021-02-12T23:37:19Z- LcmTrn::publishEstimates() - COVAR: 749.53 833.03 1.06 0.00 2021-02-12T23:37:19Z- AHRS_M2 msg: 1613173039.43 epoch sec; seqNo:5336 2021-02-12T23:37:19Z- AHRS_M2 msg: 0.62 phi; -0.28 theta; -0.16 psi 2021-02-12T23:37:19Z- handleDepth()-Depth_Keller msg: 1613173039439 epoch sec; seqNo:5385; depth 9.00 2021-02-12T23:37:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173039.45 epoch sec; seqNo:5385; 4057430.47 north; 591252.67 east 2021-02-12T23:37:19Z- AHRS_M2 msg: 1613173039.86 epoch sec; seqNo:5337 2021-02-12T23:37:19Z- AHRS_M2 msg: 0.62 phi; -0.28 theta; -0.17 psi 2021-02-12T23:37:19Z- handleDepth()-Depth_Keller msg: 1613173039862 epoch sec; seqNo:5386; depth 9.11 2021-02-12T23:37:19Z- handleDvl() - RDI_Pathfinder msg: 1613173039.87 epoch sec; seqNo:2590 2021-02-12T23:37:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 96.16 , 81.29 , 111.03 2021-02-12T23:37:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , 0.04 , -0.03 2021-02-12T23:37:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173039.88 epoch sec; seqNo:5386; 4057430.47 north; 591252.67 east 2021-02-12T23:37:20Z- AHRS_M2 msg: 1613173040.24 epoch sec; seqNo:5338 2021-02-12T23:37:20Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.17 psi 2021-02-12T23:37:20Z- handleDepth()-Depth_Keller msg: 1613173040253 epoch sec; seqNo:5387; depth 9.20 2021-02-12T23:37:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173040.27 epoch sec; seqNo:5387; 4057430.84 north; 591252.67 east 2021-02-12T23:37:20Z- AHRS_M2 msg: 1613173040.65 epoch sec; seqNo:5339 2021-02-12T23:37:20Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.17 psi 2021-02-12T23:37:20Z- handleDepth()-Depth_Keller msg: 1613173040653 epoch sec; seqNo:5388; depth 9.29 2021-02-12T23:37:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173040.66 epoch sec; seqNo:5388; 4057431.22 north; 591252.66 east 2021-02-12T23:37:21Z- AHRS_M2 msg: 1613173041.05 epoch sec; seqNo:5340 2021-02-12T23:37:21Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.18 psi 2021-02-12T23:37:21Z- handleDepth()-Depth_Keller msg: 1613173041056 epoch sec; seqNo:5389; depth 9.37 2021-02-12T23:37:21Z- handleDvl() - RDI_Pathfinder msg: 1613173041.07 epoch sec; seqNo:2591 2021-02-12T23:37:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.76 , 100.28 , 79.28 , 113.80 2021-02-12T23:37:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.03 , -0.04 2021-02-12T23:37:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173041.08 epoch sec; seqNo:5389; 4057431.22 north; 591252.66 east 2021-02-12T23:37:21Z- AHRS_M2 msg: 1613173041.46 epoch sec; seqNo:5341 2021-02-12T23:37:21Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.18 psi 2021-02-12T23:37:21Z- handleDepth()-Depth_Keller msg: 1613173041462 epoch sec; seqNo:5390; depth 9.48 2021-02-12T23:37:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173041.47 epoch sec; seqNo:5390; 4057431.60 north; 591252.66 east 2021-02-12T23:37:21Z- AHRS_M2 msg: 1613173041.86 epoch sec; seqNo:5342 2021-02-12T23:37:21Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.18 psi 2021-02-12T23:37:21Z- handleDepth()-Depth_Keller msg: 1613173041877 epoch sec; seqNo:5391; depth 9.59 2021-02-12T23:37:21Z- handleDvl() - RDI_Pathfinder msg: 1613173041.90 epoch sec; seqNo:2592 2021-02-12T23:37:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 102.85 , 0.00 , 113.67 2021-02-12T23:37:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , -0.01 2021-02-12T23:37:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173041.91 epoch sec; seqNo:5391; 4057431.99 north; 591253.88 east 2021-02-12T23:37:22Z- AHRS_M2 msg: 1613173042.27 epoch sec; seqNo:5343 2021-02-12T23:37:22Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.18 psi 2021-02-12T23:37:22Z- handleDepth()-Depth_Keller msg: 1613173042270 epoch sec; seqNo:5392; depth 9.69 2021-02-12T23:37:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173042.28 epoch sec; seqNo:5392; 4057431.99 north; 591253.88 east 2021-02-12T23:37:22Z- AHRS_M2 msg: 1613173042.67 epoch sec; seqNo:5344 2021-02-12T23:37:22Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.19 psi 2021-02-12T23:37:22Z- handleDepth()-Depth_Keller msg: 1613173042672 epoch sec; seqNo:5393; depth 9.80 2021-02-12T23:37:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173042.69 epoch sec; seqNo:5393; 4057432.37 north; 591253.87 east 2021-02-12T23:37:23Z- AHRS_M2 msg: 1613173043.07 epoch sec; seqNo:5345 2021-02-12T23:37:23Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.19 psi 2021-02-12T23:37:23Z- handleDepth()-Depth_Keller msg: 1613173043081 epoch sec; seqNo:5394; depth 9.91 2021-02-12T23:37:23Z- handleDvl() - RDI_Pathfinder msg: 1613173043.10 epoch sec; seqNo:2593 2021-02-12T23:37:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.15 , 103.73 , 77.64 , 111.84 2021-02-12T23:37:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.07 , 0.02 2021-02-12T23:37:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173043.11 epoch sec; seqNo:5394; 4057432.75 north; 591253.87 east 2021-02-12T23:37:23Z- AHRS_M2 msg: 1613173043.48 epoch sec; seqNo:5346 2021-02-12T23:37:23Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.20 psi 2021-02-12T23:37:23Z- handleDepth()-Depth_Keller msg: 1613173043487 epoch sec; seqNo:5395; depth 10.03 2021-02-12T23:37:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173043.50 epoch sec; seqNo:5395; 4057433.13 north; 591253.86 east 2021-02-12T23:37:23Z- AHRS_M2 msg: 1613173043.88 epoch sec; seqNo:5347 2021-02-12T23:37:23Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.20 psi 2021-02-12T23:37:23Z- handleDepth()-Depth_Keller msg: 1613173043886 epoch sec; seqNo:5396; depth 10.15 2021-02-12T23:37:23Z- handleDvl() - RDI_Pathfinder msg: 1613173043.90 epoch sec; seqNo:2594 2021-02-12T23:37:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.28 , 104.20 , 78.87 , 113.67 2021-02-12T23:37:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.13 , 0.08 2021-02-12T23:37:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173043.91 epoch sec; seqNo:5396; 4057433.51 north; 591253.86 east 2021-02-12T23:37:24Z- AHRS_M2 msg: 1613173044.28 epoch sec; seqNo:5348 2021-02-12T23:37:24Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.19 psi 2021-02-12T23:37:24Z- handleDepth()-Depth_Keller msg: 1613173044289 epoch sec; seqNo:5397; depth 10.28 2021-02-12T23:37:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173044.31 epoch sec; seqNo:5397; 4057433.52 north; 591255.08 east 2021-02-12T23:37:24Z- LcmTrn::handleMessages() - handling 44 messages took 0.61 ms and 132 calls... 2021-02-12T23:37:24Z- waiting for 1613173042.06, time is 1613173044.31 2021-02-12T23:37:24Z- LcmTrn::updateTrn() >>>> pose time 1613173044.31, meas time 1613173043.90 <<<< 2021-02-12T23:37:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173043.90 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:37:24Z- TerrainNav::motionUpdate() - currEstimate = (4057434, 591255.08, 10.28) 2021-02-12T23:37:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:37:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:37:24Z- TNavBF::Projecting Measurements 2021-02-12T23:37:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:37:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:37:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:37:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:37:24Z- TNavFilter::4 good beams. 2021-02-12T23:37:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:37:24Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:37:24Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:37:24Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:37:24Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:37:24Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:37:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:37:24Z- TNavBF::mmse selected from filter -1 2021-02-12T23:37:24Z- TerrainMapOctree: (vehN, vehE ) = (4057433.25, 591254.89). 2021-02-12T23:37:24Z- TerrainMapOctree: Min Distance = 4851.62. 2021-02-12T23:37:24Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:37:24Z- There is no specific good beam that all particles have in common. 2021-02-12T23:37:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:37:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 11.63 2021-02-12T23:37:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:37:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 17.37 2021-02-12T23:37:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:37:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 9.95 2021-02-12T23:37:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:37:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 20.67 2021-02-12T23:37:24Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:37:24Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:37:24Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:37:24Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:37:24Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:37:24Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:37:24Z- Filter 0 No beams used in subcloud update 2021-02-12T23:37:24Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:37:24Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:37:24Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 749.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:37:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173043.90 sec. 2021-02-12T23:37:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:37:24Z- TNavBF::mmse selected from filter -1 2021-02-12T23:37:24Z- variances([0], [2], and psi_berg) = 749.407132 833.032490 0.000000 2021-02-12T23:37:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:37:24Z- LcmTrn::publishEstimates() - MLE : 4057421.02 591208.58 10.28 2021-02-12T23:37:24Z- LcmTrn::publishEstimates() - MMSE : 4057433.52 591255.08 10.28 2021-02-12T23:37:24Z- LcmTrn::publishEstimates() - COVAR: 749.41 833.03 0.97 0.00 2021-02-12T23:37:24Z- AHRS_M2 msg: 1613173044.69 epoch sec; seqNo:5349 2021-02-12T23:37:24Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.18 psi 2021-02-12T23:37:24Z- handleDepth()-Depth_Keller msg: 1613173044692 epoch sec; seqNo:5398; depth 10.40 2021-02-12T23:37:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173044.70 epoch sec; seqNo:5398; 4057433.90 north; 591255.08 east 2021-02-12T23:37:25Z- AHRS_M2 msg: 1613173045.09 epoch sec; seqNo:5350 2021-02-12T23:37:25Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.17 psi 2021-02-12T23:37:25Z- handleDepth()-Depth_Keller msg: 1613173045096 epoch sec; seqNo:5399; depth 10.52 2021-02-12T23:37:25Z- handleDvl() - RDI_Pathfinder msg: 1613173045.11 epoch sec; seqNo:2595 2021-02-12T23:37:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 102.85 , 76.83 , 116.37 2021-02-12T23:37:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.09 , 0.11 2021-02-12T23:37:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173045.12 epoch sec; seqNo:5399; 4057434.28 north; 591255.07 east 2021-02-12T23:37:25Z- AHRS_M2 msg: 1613173045.50 epoch sec; seqNo:5351 2021-02-12T23:37:25Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.16 psi 2021-02-12T23:37:25Z- handleDepth()-Depth_Keller msg: 1613173045502 epoch sec; seqNo:5400; depth 10.64 2021-02-12T23:37:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173045.51 epoch sec; seqNo:5400; 4057434.66 north; 591255.07 east 2021-02-12T23:37:25Z- AHRS_M2 msg: 1613173045.90 epoch sec; seqNo:5352 2021-02-12T23:37:25Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.15 psi 2021-02-12T23:37:25Z- handleDepth()-Depth_Keller msg: 1613173045904 epoch sec; seqNo:5401; depth 10.76 2021-02-12T23:37:25Z- handleDvl() - RDI_Pathfinder msg: 1613173045.92 epoch sec; seqNo:2596 2021-02-12T23:37:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 96.02 , 78.04 , 113.59 2021-02-12T23:37:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.03 , 0.06 2021-02-12T23:37:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173045.93 epoch sec; seqNo:5401; 4057434.66 north; 591255.07 east 2021-02-12T23:37:26Z- AHRS_M2 msg: 1613173046.30 epoch sec; seqNo:5353 2021-02-12T23:37:26Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.15 psi 2021-02-12T23:37:26Z- handleDepth()-Depth_Keller msg: 1613173046308 epoch sec; seqNo:5402; depth 10.88 2021-02-12T23:37:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173046.32 epoch sec; seqNo:5402; 4057435.04 north; 591255.06 east 2021-02-12T23:37:26Z- AHRS_M2 msg: 1613173046.71 epoch sec; seqNo:5354 2021-02-12T23:37:26Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.15 psi 2021-02-12T23:37:26Z- handleDepth()-Depth_Keller msg: 1613173046712 epoch sec; seqNo:5403; depth 11.00 2021-02-12T23:37:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173046.72 epoch sec; seqNo:5403; 4057435.05 north; 591256.29 east 2021-02-12T23:37:27Z- AHRS_M2 msg: 1613173047.11 epoch sec; seqNo:5355 2021-02-12T23:37:27Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.16 psi 2021-02-12T23:37:27Z- handleDepth()-Depth_Keller msg: 1613173047120 epoch sec; seqNo:5404; depth 11.12 2021-02-12T23:37:27Z- handleDvl() - RDI_Pathfinder msg: 1613173047.14 epoch sec; seqNo:2597 2021-02-12T23:37:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.28 , 78.65 , 113.79 2021-02-12T23:37:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.09 , -0.04 2021-02-12T23:37:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173047.15 epoch sec; seqNo:5404; 4057435.43 north; 591256.28 east 2021-02-12T23:37:27Z- AHRS_M2 msg: 1613173047.52 epoch sec; seqNo:5356 2021-02-12T23:37:27Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.17 psi 2021-02-12T23:37:27Z- handleDepth()-Depth_Keller msg: 1613173047520 epoch sec; seqNo:5405; depth 11.22 2021-02-12T23:37:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173047.54 epoch sec; seqNo:5405; 4057435.81 north; 591256.28 east 2021-02-12T23:37:27Z- AHRS_M2 msg: 1613173047.92 epoch sec; seqNo:5357 2021-02-12T23:37:27Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-12T23:37:27Z- handleDepth()-Depth_Keller msg: 1613173047927 epoch sec; seqNo:5406; depth 11.35 2021-02-12T23:37:27Z- handleDvl() - RDI_Pathfinder msg: 1613173047.94 epoch sec; seqNo:2598 2021-02-12T23:37:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.07 , 101.30 , 78.31 , 117.52 2021-02-12T23:37:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , -0.10 2021-02-12T23:37:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173047.95 epoch sec; seqNo:5406; 4057436.19 north; 591256.27 east 2021-02-12T23:37:28Z- AHRS_M2 msg: 1613173048.33 epoch sec; seqNo:5358 2021-02-12T23:37:28Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-12T23:37:28Z- handleDepth()-Depth_Keller msg: 1613173048330 epoch sec; seqNo:5407; depth 11.48 2021-02-12T23:37:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173048.34 epoch sec; seqNo:5407; 4057436.19 north; 591256.27 east 2021-02-12T23:37:28Z- AHRS_M2 msg: 1613173048.73 epoch sec; seqNo:5359 2021-02-12T23:37:28Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.20 psi 2021-02-12T23:37:28Z- handleDepth()-Depth_Keller msg: 1613173048742 epoch sec; seqNo:5408; depth 11.60 2021-02-12T23:37:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173048.78 epoch sec; seqNo:5408; 4057436.57 north; 591256.27 east 2021-02-12T23:37:29Z- AHRS_M2 msg: 1613173049.14 epoch sec; seqNo:5360 2021-02-12T23:37:29Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.20 psi 2021-02-12T23:37:29Z- handleDepth()-Depth_Keller msg: 1613173049148 epoch sec; seqNo:5409; depth 11.71 2021-02-12T23:37:29Z- handleDvl() - RDI_Pathfinder msg: 1613173049.19 epoch sec; seqNo:2599 2021-02-12T23:37:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 105.56 , 76.42 , 119.07 2021-02-12T23:37:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.08 , -0.10 2021-02-12T23:37:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173049.21 epoch sec; seqNo:5409; 4057436.96 north; 591257.49 east 2021-02-12T23:37:29Z- handleDepth()-Depth_Keller msg: 1613173049557 epoch sec; seqNo:5410; depth 11.86 2021-02-12T23:37:29Z- LcmTrn::handleMessages() - handling 42 messages took 0.75 ms and 127 calls... 2021-02-12T23:37:29Z- waiting for 1613173047.31, time is 1613173049.21 2021-02-12T23:37:29Z- LcmTrn::updateTrn() equating pose 1613173049.21 and meas times 1613173049.21 2021-02-12T23:37:29Z- LcmTrn::updateTrn() >>>> pose time 1613173049.21, meas time 1613173049.21 <<<< 2021-02-12T23:37:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173049.21 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:37:29Z- TerrainNav::motionUpdate() - currEstimate = (4057437, 591257.49, 11.86) 2021-02-12T23:37:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:37:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:37:29Z- TNavBF::Projecting Measurements 2021-02-12T23:37:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:37:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:37:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:37:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:37:29Z- TNavFilter::4 good beams. 2021-02-12T23:37:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:37:29Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:37:29Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:37:29Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:37:29Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:37:29Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:37:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:37:29Z- TNavBF::mmse selected from filter -1 2021-02-12T23:37:29Z- TerrainMapOctree: (vehN, vehE ) = (4057436.96, 591257.49). 2021-02-12T23:37:29Z- TerrainMapOctree: Min Distance = 4847.11. 2021-02-12T23:37:29Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:37:29Z- There is no specific good beam that all particles have in common. 2021-02-12T23:37:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:37:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 11.27 2021-02-12T23:37:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:37:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 17.83 2021-02-12T23:37:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:37:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 9.34 2021-02-12T23:37:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:37:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 22.69 2021-02-12T23:37:29Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:37:29Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:37:29Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:37:29Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:37:29Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:37:29Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:37:29Z- Filter 0 No beams used in subcloud update 2021-02-12T23:37:29Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:37:29Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:37:29Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 749.3 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:37:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173049.21 sec. 2021-02-12T23:37:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:37:29Z- TNavBF::mmse selected from filter -1 2021-02-12T23:37:29Z- variances([0], [2], and psi_berg) = 749.308248 833.032398 0.000000 2021-02-12T23:37:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:37:29Z- LcmTrn::publishEstimates() - MLE : 4057424.46 591210.99 11.86 2021-02-12T23:37:29Z- LcmTrn::publishEstimates() - MMSE : 4057436.96 591257.49 11.86 2021-02-12T23:37:29Z- LcmTrn::publishEstimates() - COVAR: 749.31 833.03 0.90 0.00 2021-02-12T23:37:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173049.57 epoch sec; seqNo:5410; 4057437.34 north; 591257.48 east 2021-02-12T23:37:29Z- AHRS_M2 msg: 1613173049.94 epoch sec; seqNo:5361 2021-02-12T23:37:29Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.20 psi 2021-02-12T23:37:29Z- handleDepth()-Depth_Keller msg: 1613173049946 epoch sec; seqNo:5411; depth 11.99 2021-02-12T23:37:29Z- handleDvl() - RDI_Pathfinder msg: 1613173049.97 epoch sec; seqNo:2600 2021-02-12T23:37:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.86 , 105.49 , 76.42 , 119.01 2021-02-12T23:37:29Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.19 , -0.07 2021-02-12T23:37:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173049.98 epoch sec; seqNo:5411; 4057437.72 north; 591257.48 east 2021-02-12T23:37:30Z- AHRS_M2 msg: 1613173050.34 epoch sec; seqNo:5362 2021-02-12T23:37:30Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.20 psi 2021-02-12T23:37:30Z- handleDepth()-Depth_Keller msg: 1613173050350 epoch sec; seqNo:5412; depth 12.13 2021-02-12T23:37:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173050.36 epoch sec; seqNo:5412; 4057438.09 north; 591257.47 east 2021-02-12T23:37:30Z- AHRS_M2 msg: 1613173050.75 epoch sec; seqNo:5363 2021-02-12T23:37:30Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.20 psi 2021-02-12T23:37:30Z- handleDepth()-Depth_Keller msg: 1613173050758 epoch sec; seqNo:5413; depth 12.25 2021-02-12T23:37:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173050.77 epoch sec; seqNo:5413; 4057438.09 north; 591257.47 east 2021-02-12T23:37:31Z- AHRS_M2 msg: 1613173051.15 epoch sec; seqNo:5364 2021-02-12T23:37:31Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.20 psi 2021-02-12T23:37:31Z- handleDepth()-Depth_Keller msg: 1613173051164 epoch sec; seqNo:5414; depth 12.41 2021-02-12T23:37:31Z- handleDvl() - RDI_Pathfinder msg: 1613173051.17 epoch sec; seqNo:2601 2021-02-12T23:37:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.86 , 105.49 , 76.01 , 117.65 2021-02-12T23:37:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.23 , -0.01 2021-02-12T23:37:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173051.19 epoch sec; seqNo:5414; 4057438.47 north; 591257.47 east 2021-02-12T23:37:31Z- AHRS_M2 msg: 1613173051.56 epoch sec; seqNo:5365 2021-02-12T23:37:31Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.20 psi 2021-02-12T23:37:31Z- handleDepth()-Depth_Keller msg: 1613173051569 epoch sec; seqNo:5415; depth 12.55 2021-02-12T23:37:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173051.59 epoch sec; seqNo:5415; 4057438.85 north; 591257.47 east 2021-02-12T23:37:31Z- AHRS_M2 msg: 1613173051.96 epoch sec; seqNo:5366 2021-02-12T23:37:31Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.20 psi 2021-02-12T23:37:31Z- handleDepth()-Depth_Keller msg: 1613173051964 epoch sec; seqNo:5416; depth 12.71 2021-02-12T23:37:31Z- handleDvl() - RDI_Pathfinder msg: 1613173051.98 epoch sec; seqNo:2602 2021-02-12T23:37:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.86 , 104.14 , 74.37 , 117.65 2021-02-12T23:37:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.17 , 0.02 2021-02-12T23:37:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173051.99 epoch sec; seqNo:5416; 4057439.24 north; 591258.68 east 2021-02-12T23:37:32Z- AHRS_M2 msg: 1613173052.37 epoch sec; seqNo:5367 2021-02-12T23:37:32Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.19 psi 2021-02-12T23:37:32Z- handleDepth()-Depth_Keller msg: 1613173052377 epoch sec; seqNo:5417; depth 12.85 2021-02-12T23:37:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173052.39 epoch sec; seqNo:5417; 4057439.62 north; 591258.68 east 2021-02-12T23:37:32Z- AHRS_M2 msg: 1613173052.78 epoch sec; seqNo:5368 2021-02-12T23:37:32Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.18 psi 2021-02-12T23:37:32Z- handleDepth()-Depth_Keller msg: 1613173052786 epoch sec; seqNo:5418; depth 12.99 2021-02-12T23:37:32Z- handleDvl() - RDI_Pathfinder msg: 1613173052.80 epoch sec; seqNo:2603 2021-02-12T23:37:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.28 , 101.50 , 73.96 , 119.07 2021-02-12T23:37:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , 0.00 2021-02-12T23:37:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173052.81 epoch sec; seqNo:5418; 4057440.00 north; 591258.67 east 2021-02-12T23:37:33Z- AHRS_M2 msg: 1613173053.17 epoch sec; seqNo:5369 2021-02-12T23:37:33Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.18 psi 2021-02-12T23:37:33Z- handleDepth()-Depth_Keller msg: 1613173053176 epoch sec; seqNo:5419; depth 13.15 2021-02-12T23:37:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173053.19 epoch sec; seqNo:5419; 4057440.00 north; 591258.67 east 2021-02-12T23:37:33Z- AHRS_M2 msg: 1613173053.58 epoch sec; seqNo:5370 2021-02-12T23:37:33Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.18 psi 2021-02-12T23:37:33Z- handleDepth()-Depth_Keller msg: 1613173053589 epoch sec; seqNo:5420; depth 13.28 2021-02-12T23:37:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173053.60 epoch sec; seqNo:5420; 4057440.38 north; 591258.67 east 2021-02-12T23:37:33Z- AHRS_M2 msg: 1613173053.98 epoch sec; seqNo:5371 2021-02-12T23:37:33Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.18 psi 2021-02-12T23:37:33Z- handleDepth()-Depth_Keller msg: 1613173053989 epoch sec; seqNo:5421; depth 13.44 2021-02-12T23:37:34Z- handleDvl() - RDI_Pathfinder msg: 1613173054.01 epoch sec; seqNo:2604 2021-02-12T23:37:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.28 , 96.09 , 73.56 , 119.07 2021-02-12T23:37:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.00 , -0.05 2021-02-12T23:37:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173054.02 epoch sec; seqNo:5421; 4057440.76 north; 591258.67 east 2021-02-12T23:37:34Z- AHRS_M2 msg: 1613173054.38 epoch sec; seqNo:5372 2021-02-12T23:37:34Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.18 psi 2021-02-12T23:37:34Z- handleDepth()-Depth_Keller msg: 1613173054393 epoch sec; seqNo:5422; depth 13.58 2021-02-12T23:37:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173054.40 epoch sec; seqNo:5422; 4057440.76 north; 591258.67 east 2021-02-12T23:37:34Z- LcmTrn::handleMessages() - handling 42 messages took 0.61 ms and 130 calls... 2021-02-12T23:37:34Z- waiting for 1613173052.21, time is 1613173054.40 2021-02-12T23:37:34Z- LcmTrn::updateTrn() >>>> pose time 1613173054.40, meas time 1613173054.01 <<<< 2021-02-12T23:37:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173054.01 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:37:34Z- TerrainNav::motionUpdate() - currEstimate = (4057441, 591258.67, 13.58) 2021-02-12T23:37:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:37:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:37:34Z- TNavBF::Projecting Measurements 2021-02-12T23:37:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:37:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:37:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:37:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:37:34Z- TNavFilter::4 good beams. 2021-02-12T23:37:34Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:37:34Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:37:34Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:37:34Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:37:34Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:37:34Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:37:34Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:37:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:37:34Z- TerrainMapOctree: (vehN, vehE ) = (4057440.47, 591258.58). 2021-02-12T23:37:34Z- TerrainMapOctree: Min Distance = 4843.67. 2021-02-12T23:37:34Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:37:34Z- There is no specific good beam that all particles have in common. 2021-02-12T23:37:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:37:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 11.63 2021-02-12T23:37:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:37:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 14.77 2021-02-12T23:37:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:37:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 8.66 2021-02-12T23:37:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:37:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.034 ExpectedMeasDiff: nan currMeas.covariance: 22.69 2021-02-12T23:37:34Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:37:34Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:37:34Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:37:34Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:37:34Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:37:34Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:37:34Z- Filter 0 No beams used in subcloud update 2021-02-12T23:37:34Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:37:34Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:37:34Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 749.2 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:37:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173054.01 sec. 2021-02-12T23:37:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:37:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:37:34Z- variances([0], [2], and psi_berg) = 749.189232 833.032289 0.000000 2021-02-12T23:37:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:37:34Z- LcmTrn::publishEstimates() - MLE : 4057428.26 591212.17 13.58 2021-02-12T23:37:34Z- LcmTrn::publishEstimates() - MMSE : 4057440.76 591258.67 13.58 2021-02-12T23:37:34Z- LcmTrn::publishEstimates() - COVAR: 749.19 833.03 0.86 0.00 2021-02-12T23:37:34Z- AHRS_M2 msg: 1613173054.79 epoch sec; seqNo:5373 2021-02-12T23:37:34Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.18 psi 2021-02-12T23:37:34Z- handleDepth()-Depth_Keller msg: 1613173054795 epoch sec; seqNo:5423; depth 13.74 2021-02-12T23:37:34Z- handleDvl() - RDI_Pathfinder msg: 1613173054.81 epoch sec; seqNo:2605 2021-02-12T23:37:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 101.63 , 72.33 , 116.51 2021-02-12T23:37:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , -0.07 2021-02-12T23:37:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173054.82 epoch sec; seqNo:5423; 4057441.14 north; 591258.66 east 2021-02-12T23:37:35Z- AHRS_M2 msg: 1613173055.19 epoch sec; seqNo:5374 2021-02-12T23:37:35Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.19 psi 2021-02-12T23:37:35Z- handleDepth()-Depth_Keller msg: 1613173055206 epoch sec; seqNo:5424; depth 13.87 2021-02-12T23:37:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173055.22 epoch sec; seqNo:5424; 4057441.53 north; 591259.88 east 2021-02-12T23:37:35Z- AHRS_M2 msg: 1613173055.60 epoch sec; seqNo:5375 2021-02-12T23:37:35Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.19 psi 2021-02-12T23:37:35Z- handleDepth()-Depth_Keller msg: 1613173055600 epoch sec; seqNo:5425; depth 14.03 2021-02-12T23:37:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173055.61 epoch sec; seqNo:5425; 4057441.53 north; 591259.88 east 2021-02-12T23:37:36Z- AHRS_M2 msg: 1613173056.00 epoch sec; seqNo:5376 2021-02-12T23:37:36Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.19 psi 2021-02-12T23:37:36Z- handleDepth()-Depth_Keller msg: 1613173056004 epoch sec; seqNo:5426; depth 14.17 2021-02-12T23:37:36Z- handleDvl() - RDI_Pathfinder msg: 1613173056.02 epoch sec; seqNo:2606 2021-02-12T23:37:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 102.99 , 71.92 , 121.91 2021-02-12T23:37:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , -0.01 2021-02-12T23:37:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173056.03 epoch sec; seqNo:5426; 4057441.91 north; 591259.88 east 2021-02-12T23:37:36Z- AHRS_M2 msg: 1613173056.40 epoch sec; seqNo:5377 2021-02-12T23:37:36Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.19 psi 2021-02-12T23:37:36Z- handleDepth()-Depth_Keller msg: 1613173056410 epoch sec; seqNo:5427; depth 14.33 2021-02-12T23:37:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173056.42 epoch sec; seqNo:5427; 4057442.29 north; 591259.87 east 2021-02-12T23:37:36Z- AHRS_M2 msg: 1613173056.81 epoch sec; seqNo:5378 2021-02-12T23:37:36Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.19 psi 2021-02-12T23:37:36Z- handleDepth()-Depth_Keller msg: 1613173056817 epoch sec; seqNo:5428; depth 14.49 2021-02-12T23:37:36Z- handleDvl() - RDI_Pathfinder msg: 1613173056.84 epoch sec; seqNo:2607 2021-02-12T23:37:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.09 , 78.51 , 116.37 2021-02-12T23:37:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , 0.06 2021-02-12T23:37:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173056.85 epoch sec; seqNo:5428; 4057442.67 north; 591259.87 east 2021-02-12T23:37:37Z- AHRS_M2 msg: 1613173057.21 epoch sec; seqNo:5379 2021-02-12T23:37:37Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.19 psi 2021-02-12T23:37:37Z- handleDepth()-Depth_Keller msg: 1613173057216 epoch sec; seqNo:5429; depth 14.64 2021-02-12T23:37:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173057.23 epoch sec; seqNo:5429; 4057442.67 north; 591259.87 east 2021-02-12T23:37:37Z- AHRS_M2 msg: 1613173057.62 epoch sec; seqNo:5380 2021-02-12T23:37:37Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.19 psi 2021-02-12T23:37:37Z- handleDepth()-Depth_Keller msg: 1613173057625 epoch sec; seqNo:5430; depth 14.79 2021-02-12T23:37:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173057.63 epoch sec; seqNo:5430; 4057443.05 north; 591259.86 east 2021-02-12T23:37:38Z- AHRS_M2 msg: 1613173058.02 epoch sec; seqNo:5381 2021-02-12T23:37:38Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.19 psi 2021-02-12T23:37:38Z- handleDepth()-Depth_Keller msg: 1613173058028 epoch sec; seqNo:5431; depth 14.95 2021-02-12T23:37:38Z- handleDvl() - RDI_Pathfinder msg: 1613173058.06 epoch sec; seqNo:2608 2021-02-12T23:37:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.79 , 95.96 , 73.56 , 118.94 2021-02-12T23:37:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , 0.09 2021-02-12T23:37:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173058.07 epoch sec; seqNo:5431; 4057443.44 north; 591261.08 east 2021-02-12T23:37:38Z- AHRS_M2 msg: 1613173058.42 epoch sec; seqNo:5382 2021-02-12T23:37:38Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.19 psi 2021-02-12T23:37:38Z- handleDepth()-Depth_Keller msg: 1613173058428 epoch sec; seqNo:5432; depth 15.10 2021-02-12T23:37:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173058.44 epoch sec; seqNo:5432; 4057443.44 north; 591261.08 east 2021-02-12T23:37:38Z- AHRS_M2 msg: 1613173058.83 epoch sec; seqNo:5383 2021-02-12T23:37:38Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.18 psi 2021-02-12T23:37:38Z- handleDepth()-Depth_Keller msg: 1613173058837 epoch sec; seqNo:5433; depth 15.25 2021-02-12T23:37:38Z- handleDvl() - RDI_Pathfinder msg: 1613173058.85 epoch sec; seqNo:2609 2021-02-12T23:37:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.13 , 100.35 , 72.74 , 116.57 2021-02-12T23:37:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.08 , -0.01 2021-02-12T23:37:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173058.86 epoch sec; seqNo:5433; 4057443.82 north; 591261.08 east 2021-02-12T23:37:39Z- AHRS_M2 msg: 1613173059.24 epoch sec; seqNo:5384 2021-02-12T23:37:39Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.19 psi 2021-02-12T23:37:39Z- handleDepth()-Depth_Keller msg: 1613173059247 epoch sec; seqNo:5434; depth 15.41 2021-02-12T23:37:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173059.26 epoch sec; seqNo:5434; 4057444.20 north; 591261.07 east 2021-02-12T23:37:39Z- AHRS_M2 msg: 1613173059.64 epoch sec; seqNo:5385 2021-02-12T23:37:39Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.19 psi 2021-02-12T23:37:39Z- handleDepth()-Depth_Keller msg: 1613173059640 epoch sec; seqNo:5435; depth 15.56 2021-02-12T23:37:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173059.67 epoch sec; seqNo:5435; 4057444.20 north; 591261.07 east 2021-02-12T23:37:39Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-12T23:37:39Z- waiting for 1613173057.40, time is 1613173059.67 2021-02-12T23:37:39Z- LcmTrn::updateTrn() >>>> pose time 1613173059.67, meas time 1613173058.85 <<<< 2021-02-12T23:37:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173058.85 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:37:39Z- TerrainNav::motionUpdate() - currEstimate = (4057444, 591261.07, 15.56) 2021-02-12T23:37:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:37:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:37:39Z- TNavBF::Projecting Measurements 2021-02-12T23:37:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:37:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:37:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:37:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:37:39Z- TNavFilter::4 good beams. 2021-02-12T23:37:39Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:37:39Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:37:39Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:37:39Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:37:39Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:37:39Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:37:39Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:37:39Z- TNavBF::mmse selected from filter -1 2021-02-12T23:37:39Z- TerrainMapOctree: (vehN, vehE ) = (4057443.66, 591260.70). 2021-02-12T23:37:39Z- TerrainMapOctree: Min Distance = 4839.85. 2021-02-12T23:37:39Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:37:39Z- There is no specific good beam that all particles have in common. 2021-02-12T23:37:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:37:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 11.32 2021-02-12T23:37:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:37:39Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 16.11 2021-02-12T23:37:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:37:39Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 8.47 2021-02-12T23:37:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:37:39Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.034 ExpectedMeasDiff: nan currMeas.covariance: 21.74 2021-02-12T23:37:39Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:37:39Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:37:39Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:37:39Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:37:39Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:37:39Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:37:39Z- Filter 0 No beams used in subcloud update 2021-02-12T23:37:39Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:37:39Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:37:39Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 749.1 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:37:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173058.85 sec. 2021-02-12T23:37:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:37:39Z- TNavBF::mmse selected from filter -1 2021-02-12T23:37:39Z- variances([0], [2], and psi_berg) = 749.059773 833.032170 0.000000 2021-02-12T23:37:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:37:39Z- LcmTrn::publishEstimates() - MLE : 4057431.70 591214.57 15.56 2021-02-12T23:37:39Z- LcmTrn::publishEstimates() - MMSE : 4057444.20 591261.07 15.56 2021-02-12T23:37:39Z- LcmTrn::publishEstimates() - COVAR: 749.06 833.03 0.84 0.00 2021-02-12T23:37:40Z- AHRS_M2 msg: 1613173060.04 epoch sec; seqNo:5386 2021-02-12T23:37:40Z- AHRS_M2 msg: 0.66 phi; -0.38 theta; -0.19 psi 2021-02-12T23:37:40Z- handleDepth()-Depth_Keller msg: 1613173060044 epoch sec; seqNo:5436; depth 15.71 2021-02-12T23:37:40Z- handleDvl() - RDI_Pathfinder msg: 1613173060.06 epoch sec; seqNo:2610 2021-02-12T23:37:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.71 , 96.23 , 69.88 , 116.51 2021-02-12T23:37:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , -0.14 2021-02-12T23:37:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173060.08 epoch sec; seqNo:5436; 4057444.59 north; 591262.29 east 2021-02-12T23:37:40Z- AHRS_M2 msg: 1613173060.44 epoch sec; seqNo:5387 2021-02-12T23:37:40Z- AHRS_M2 msg: 0.66 phi; -0.38 theta; -0.19 psi 2021-02-12T23:37:40Z- handleDepth()-Depth_Keller msg: 1613173060452 epoch sec; seqNo:5437; depth 15.88 2021-02-12T23:37:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173060.47 epoch sec; seqNo:5437; 4057444.97 north; 591262.28 east 2021-02-12T23:37:40Z- AHRS_M2 msg: 1613173060.85 epoch sec; seqNo:5388 2021-02-12T23:37:40Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.19 psi 2021-02-12T23:37:40Z- handleDepth()-Depth_Keller msg: 1613173060854 epoch sec; seqNo:5438; depth 16.02 2021-02-12T23:37:40Z- handleDvl() - RDI_Pathfinder msg: 1613173060.87 epoch sec; seqNo:2611 2021-02-12T23:37:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 100.08 , 0.00 , 114.68 2021-02-12T23:37:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.09 , -0.18 2021-02-12T23:37:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173060.88 epoch sec; seqNo:5438; 4057445.34 north; 591262.28 east 2021-02-12T23:37:41Z- AHRS_M2 msg: 1613173061.25 epoch sec; seqNo:5389 2021-02-12T23:37:41Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.19 psi 2021-02-12T23:37:41Z- handleDepth()-Depth_Keller msg: 1613173061256 epoch sec; seqNo:5439; depth 16.16 2021-02-12T23:37:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173061.27 epoch sec; seqNo:5439; 4057445.34 north; 591262.28 east 2021-02-12T23:37:41Z- AHRS_M2 msg: 1613173061.66 epoch sec; seqNo:5390 2021-02-12T23:37:41Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.19 psi 2021-02-12T23:37:41Z- handleDepth()-Depth_Keller msg: 1613173061660 epoch sec; seqNo:5440; depth 16.31 2021-02-12T23:37:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173061.67 epoch sec; seqNo:5440; 4057445.72 north; 591262.28 east 2021-02-12T23:37:42Z- AHRS_M2 msg: 1613173062.06 epoch sec; seqNo:5391 2021-02-12T23:37:42Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.19 psi 2021-02-12T23:37:42Z- handleDepth()-Depth_Keller msg: 1613173062066 epoch sec; seqNo:5441; depth 16.45 2021-02-12T23:37:42Z- handleDvl() - RDI_Pathfinder msg: 1613173062.08 epoch sec; seqNo:2612 2021-02-12T23:37:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.95 , 95.82 , 0.00 , 113.40 2021-02-12T23:37:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.20 , -0.14 2021-02-12T23:37:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173062.09 epoch sec; seqNo:5441; 4057446.10 north; 591262.27 east 2021-02-12T23:37:42Z- AHRS_M2 msg: 1613173062.46 epoch sec; seqNo:5392 2021-02-12T23:37:42Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.19 psi 2021-02-12T23:37:42Z- handleDepth()-Depth_Keller msg: 1613173062468 epoch sec; seqNo:5442; depth 16.61 2021-02-12T23:37:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173062.49 epoch sec; seqNo:5442; 4057446.48 north; 591262.27 east 2021-02-12T23:37:42Z- AHRS_M2 msg: 1613173062.87 epoch sec; seqNo:5393 2021-02-12T23:37:42Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.19 psi 2021-02-12T23:37:42Z- handleDepth()-Depth_Keller msg: 1613173062879 epoch sec; seqNo:5443; depth 16.76 2021-02-12T23:37:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173062.92 epoch sec; seqNo:5443; 4057446.87 north; 591263.49 east 2021-02-12T23:37:43Z- AHRS_M2 msg: 1613173063.27 epoch sec; seqNo:5394 2021-02-12T23:37:43Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.19 psi 2021-02-12T23:37:43Z- handleDepth()-Depth_Keller msg: 1613173063276 epoch sec; seqNo:5444; depth 16.90 2021-02-12T23:37:43Z- handleDvl() - RDI_Pathfinder msg: 1613173063.29 epoch sec; seqNo:2613 2021-02-12T23:37:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.73 , 95.95 , 78.38 , 112.17 2021-02-12T23:37:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.22 , -0.01 2021-02-12T23:37:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173063.30 epoch sec; seqNo:5444; 4057447.25 north; 591263.48 east 2021-02-12T23:37:43Z- AHRS_M2 msg: 1613173063.68 epoch sec; seqNo:5395 2021-02-12T23:37:43Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.19 psi 2021-02-12T23:37:43Z- handleDepth()-Depth_Keller msg: 1613173063681 epoch sec; seqNo:5445; depth 17.06 2021-02-12T23:37:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173063.69 epoch sec; seqNo:5445; 4057447.63 north; 591263.48 east 2021-02-12T23:37:44Z- AHRS_M2 msg: 1613173064.08 epoch sec; seqNo:5396 2021-02-12T23:37:44Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.19 psi 2021-02-12T23:37:44Z- handleDepth()-Depth_Keller msg: 1613173064090 epoch sec; seqNo:5446; depth 17.18 2021-02-12T23:37:44Z- handleDvl() - RDI_Pathfinder msg: 1613173064.10 epoch sec; seqNo:2614 2021-02-12T23:37:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.39 , 96.43 , 71.11 , 112.25 2021-02-12T23:37:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.17 , 0.05 2021-02-12T23:37:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173064.11 epoch sec; seqNo:5446; 4057448.01 north; 591263.47 east 2021-02-12T23:37:44Z- AHRS_M2 msg: 1613173064.48 epoch sec; seqNo:5397 2021-02-12T23:37:44Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.20 psi 2021-02-12T23:37:44Z- handleDepth()-Depth_Keller msg: 1613173064488 epoch sec; seqNo:5447; depth 17.35 2021-02-12T23:37:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173064.50 epoch sec; seqNo:5447; 4057448.01 north; 591263.47 east 2021-02-12T23:37:44Z- LcmTrn::handleMessages() - handling 41 messages took 0.65 ms and 128 calls... 2021-02-12T23:37:44Z- waiting for 1613173062.67, time is 1613173064.50 2021-02-12T23:37:44Z- LcmTrn::updateTrn() >>>> pose time 1613173064.50, meas time 1613173064.10 <<<< 2021-02-12T23:37:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173064.10 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:37:44Z- TerrainNav::motionUpdate() - currEstimate = (4057448, 591263.47, 17.35) 2021-02-12T23:37:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:37:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:37:44Z- TNavBF::Projecting Measurements 2021-02-12T23:37:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:37:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:37:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:37:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:37:44Z- TNavFilter::4 good beams. 2021-02-12T23:37:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:37:44Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:37:44Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:37:44Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:37:44Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:37:44Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:37:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:37:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:37:44Z- TerrainMapOctree: (vehN, vehE ) = (4057447.70, 591263.28). 2021-02-12T23:37:44Z- TerrainMapOctree: Min Distance = 4835.08. 2021-02-12T23:37:44Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:37:44Z- There is no specific good beam that all particles have in common. 2021-02-12T23:37:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:37:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 10.09 2021-02-12T23:37:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:37:44Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 14.88 2021-02-12T23:37:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:37:44Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-12T23:37:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:37:44Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 20.16 2021-02-12T23:37:44Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:37:44Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:37:44Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:37:44Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:37:44Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:37:44Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:37:44Z- Filter 0 No beams used in subcloud update 2021-02-12T23:37:44Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:37:44Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:37:44Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 749.0 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:37:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173064.10 sec. 2021-02-12T23:37:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:37:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:37:44Z- variances([0], [2], and psi_berg) = 748.932771 833.032053 0.000000 2021-02-12T23:37:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:37:44Z- LcmTrn::publishEstimates() - MLE : 4057435.51 591216.97 17.35 2021-02-12T23:37:44Z- LcmTrn::publishEstimates() - MMSE : 4057448.01 591263.47 17.35 2021-02-12T23:37:44Z- LcmTrn::publishEstimates() - COVAR: 748.93 833.03 0.82 0.00 2021-02-12T23:37:44Z- AHRS_M2 msg: 1613173064.89 epoch sec; seqNo:5398 2021-02-12T23:37:44Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.20 psi 2021-02-12T23:37:44Z- handleDepth()-Depth_Keller msg: 1613173064892 epoch sec; seqNo:5448; depth 17.47 2021-02-12T23:37:44Z- handleDvl() - RDI_Pathfinder msg: 1613173064.91 epoch sec; seqNo:2615 2021-02-12T23:37:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 97.78 , 69.47 , 109.95 2021-02-12T23:37:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.09 , 0.05 2021-02-12T23:37:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173064.94 epoch sec; seqNo:5448; 4057448.39 north; 591263.47 east 2021-02-12T23:37:45Z- AHRS_M2 msg: 1613173065.30 epoch sec; seqNo:5399 2021-02-12T23:37:45Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.20 psi 2021-02-12T23:37:45Z- handleDepth()-Depth_Keller msg: 1613173065303 epoch sec; seqNo:5449; depth 17.63 2021-02-12T23:37:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173065.31 epoch sec; seqNo:5449; 4057448.77 north; 591263.47 east 2021-02-12T23:37:45Z- AHRS_M2 msg: 1613173065.70 epoch sec; seqNo:5400 2021-02-12T23:37:45Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.21 psi 2021-02-12T23:37:45Z- handleDepth()-Depth_Keller msg: 1613173065716 epoch sec; seqNo:5450; depth 17.75 2021-02-12T23:37:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173065.74 epoch sec; seqNo:5450; 4057449.16 north; 591264.68 east 2021-02-12T23:37:46Z- AHRS_M2 msg: 1613173066.10 epoch sec; seqNo:5401 2021-02-12T23:37:46Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.21 psi 2021-02-12T23:37:46Z- handleDepth()-Depth_Keller msg: 1613173066104 epoch sec; seqNo:5451; depth 17.91 2021-02-12T23:37:46Z- handleDvl() - RDI_Pathfinder msg: 1613173066.12 epoch sec; seqNo:2616 2021-02-12T23:37:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 97.85 , 71.51 , 110.02 2021-02-12T23:37:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , 0.01 2021-02-12T23:37:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173066.15 epoch sec; seqNo:5451; 4057449.54 north; 591264.68 east 2021-02-12T23:37:46Z- AHRS_M2 msg: 1613173066.50 epoch sec; seqNo:5402 2021-02-12T23:37:46Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.21 psi 2021-02-12T23:37:46Z- handleDepth()-Depth_Keller msg: 1613173066508 epoch sec; seqNo:5452; depth 18.06 2021-02-12T23:37:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173066.52 epoch sec; seqNo:5452; 4057449.54 north; 591264.68 east 2021-02-12T23:37:46Z- AHRS_M2 msg: 1613173066.91 epoch sec; seqNo:5403 2021-02-12T23:37:46Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.21 psi 2021-02-12T23:37:46Z- handleDepth()-Depth_Keller msg: 1613173066912 epoch sec; seqNo:5453; depth 18.18 2021-02-12T23:37:46Z- handleDvl() - RDI_Pathfinder msg: 1613173066.93 epoch sec; seqNo:2617 2021-02-12T23:37:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 97.85 , 70.70 , 108.80 2021-02-12T23:37:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , -0.05 2021-02-12T23:37:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173066.94 epoch sec; seqNo:5453; 4057449.92 north; 591264.67 east 2021-02-12T23:37:47Z- AHRS_M2 msg: 1613173067.31 epoch sec; seqNo:5404 2021-02-12T23:37:47Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.21 psi 2021-02-12T23:37:47Z- handleDepth()-Depth_Keller msg: 1613173067323 epoch sec; seqNo:5454; depth 18.33 2021-02-12T23:37:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173067.34 epoch sec; seqNo:5454; 4057450.30 north; 591264.67 east 2021-02-12T23:37:47Z- AHRS_M2 msg: 1613173067.72 epoch sec; seqNo:5405 2021-02-12T23:37:47Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.20 psi 2021-02-12T23:37:47Z- handleDepth()-Depth_Keller msg: 1613173067726 epoch sec; seqNo:5455; depth 18.47 2021-02-12T23:37:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173067.73 epoch sec; seqNo:5455; 4057450.30 north; 591264.67 east 2021-02-12T23:37:48Z- AHRS_M2 msg: 1613173068.13 epoch sec; seqNo:5406 2021-02-12T23:37:48Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.20 psi 2021-02-12T23:37:48Z- handleDepth()-Depth_Keller msg: 1613173068134 epoch sec; seqNo:5456; depth 18.60 2021-02-12T23:37:48Z- handleDvl() - RDI_Pathfinder msg: 1613173068.15 epoch sec; seqNo:2618 2021-02-12T23:37:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 97.85 , 70.70 , 106.37 2021-02-12T23:37:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.10 2021-02-12T23:37:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173068.16 epoch sec; seqNo:5456; 4057450.69 north; 591265.89 east 2021-02-12T23:37:48Z- AHRS_M2 msg: 1613173068.52 epoch sec; seqNo:5407 2021-02-12T23:37:48Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.20 psi 2021-02-12T23:37:48Z- handleDepth()-Depth_Keller msg: 1613173068528 epoch sec; seqNo:5457; depth 18.74 2021-02-12T23:37:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173068.55 epoch sec; seqNo:5457; 4057451.07 north; 591265.88 east 2021-02-12T23:37:48Z- AHRS_M2 msg: 1613173068.93 epoch sec; seqNo:5408 2021-02-12T23:37:48Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.19 psi 2021-02-12T23:37:48Z- handleDepth()-Depth_Keller msg: 1613173068932 epoch sec; seqNo:5458; depth 18.87 2021-02-12T23:37:48Z- handleDvl() - RDI_Pathfinder msg: 1613173068.94 epoch sec; seqNo:2619 2021-02-12T23:37:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 97.92 , 71.11 , 105.22 2021-02-12T23:37:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.08 2021-02-12T23:37:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173068.96 epoch sec; seqNo:5458; 4057451.45 north; 591265.88 east 2021-02-12T23:37:49Z- AHRS_M2 msg: 1613173069.33 epoch sec; seqNo:5409 2021-02-12T23:37:49Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.18 psi 2021-02-12T23:37:49Z- handleDepth()-Depth_Keller msg: 1613173069336 epoch sec; seqNo:5459; depth 19.00 2021-02-12T23:37:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173069.35 epoch sec; seqNo:5459; 4057451.45 north; 591265.88 east 2021-02-12T23:37:49Z- AHRS_M2 msg: 1613173069.75 epoch sec; seqNo:5410 2021-02-12T23:37:49Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.17 psi 2021-02-12T23:37:49Z- handleDepth()-Depth_Keller msg: 1613173069751 epoch sec; seqNo:5460; depth 19.13 2021-02-12T23:37:49Z- handleDvl() - RDI_Pathfinder msg: 1613173069.76 epoch sec; seqNo:2620 2021-02-12T23:37:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.01 , 97.92 , 70.70 , 102.78 2021-02-12T23:37:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.07 , -0.03 2021-02-12T23:37:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173069.77 epoch sec; seqNo:5460; 4057451.82 north; 591265.88 east 2021-02-12T23:37:49Z- LcmTrn::handleMessages() - handling 45 messages took 0.62 ms and 133 calls... 2021-02-12T23:37:49Z- waiting for 1613173067.50, time is 1613173069.77 2021-02-12T23:37:49Z- LcmTrn::updateTrn() equating pose 1613173069.77 and meas times 1613173069.77 2021-02-12T23:37:49Z- LcmTrn::updateTrn() >>>> pose time 1613173069.77, meas time 1613173069.77 <<<< 2021-02-12T23:37:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173069.77 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:37:49Z- TerrainNav::motionUpdate() - currEstimate = (4057452, 591265.88, 19.13) 2021-02-12T23:37:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:37:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:37:49Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173069.77 sec.; No relevant navigation data available 2021-02-12T23:37:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:37:49Z- TNavBF::mmse selected from filter -1 2021-02-12T23:37:49Z- variances([0], [2], and psi_berg) = 748.830899 833.031960 0.000000 2021-02-12T23:37:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:37:49Z- LcmTrn::publishEstimates() - MLE : 4057439.32 591219.38 19.13 2021-02-12T23:37:49Z- LcmTrn::publishEstimates() - MMSE : 4057451.82 591265.88 19.13 2021-02-12T23:37:49Z- LcmTrn::publishEstimates() - COVAR: 748.83 833.03 1.05 0.00 2021-02-12T23:37:50Z- AHRS_M2 msg: 1613173070.14 epoch sec; seqNo:5411 2021-02-12T23:37:50Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.16 psi 2021-02-12T23:37:50Z- handleDepth()-Depth_Keller msg: 1613173070148 epoch sec; seqNo:5461; depth 19.28 2021-02-12T23:37:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173070.16 epoch sec; seqNo:5461; 4057452.20 north; 591265.87 east 2021-02-12T23:37:50Z- AHRS_M2 msg: 1613173070.55 epoch sec; seqNo:5412 2021-02-12T23:37:50Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.15 psi 2021-02-12T23:37:50Z- handleDepth()-Depth_Keller msg: 1613173070554 epoch sec; seqNo:5462; depth 19.39 2021-02-12T23:37:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173070.58 epoch sec; seqNo:5462; 4057452.58 north; 591265.87 east 2021-02-12T23:37:50Z- AHRS_M2 msg: 1613173070.95 epoch sec; seqNo:5413 2021-02-12T23:37:50Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.15 psi 2021-02-12T23:37:50Z- handleDepth()-Depth_Keller msg: 1613173070952 epoch sec; seqNo:5463; depth 19.53 2021-02-12T23:37:50Z- handleDvl() - RDI_Pathfinder msg: 1613173070.96 epoch sec; seqNo:2621 2021-02-12T23:37:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.42 , 90.75 , 68.65 , 101.70 2021-02-12T23:37:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , 0.04 2021-02-12T23:37:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173070.97 epoch sec; seqNo:5463; 4057452.59 north; 591267.09 east 2021-02-12T23:37:51Z- AHRS_M2 msg: 1613173071.35 epoch sec; seqNo:5414 2021-02-12T23:37:51Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.15 psi 2021-02-12T23:37:51Z- handleDepth()-Depth_Keller msg: 1613173071358 epoch sec; seqNo:5464; depth 19.66 2021-02-12T23:37:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173071.37 epoch sec; seqNo:5464; 4057452.97 north; 591267.08 east 2021-02-12T23:37:51Z- AHRS_M2 msg: 1613173071.76 epoch sec; seqNo:5415 2021-02-12T23:37:51Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.15 psi 2021-02-12T23:37:51Z- handleDepth()-Depth_Keller msg: 1613173071760 epoch sec; seqNo:5465; depth 19.78 2021-02-12T23:37:51Z- handleDvl() - RDI_Pathfinder msg: 1613173071.78 epoch sec; seqNo:2622 2021-02-12T23:37:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 90.95 , 69.88 , 104.34 2021-02-12T23:37:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.04 , 0.06 2021-02-12T23:37:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173071.79 epoch sec; seqNo:5465; 4057453.35 north; 591267.08 east 2021-02-12T23:37:52Z- AHRS_M2 msg: 1613173072.16 epoch sec; seqNo:5416 2021-02-12T23:37:52Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.16 psi 2021-02-12T23:37:52Z- handleDepth()-Depth_Keller msg: 1613173072168 epoch sec; seqNo:5466; depth 19.91 2021-02-12T23:37:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173072.18 epoch sec; seqNo:5466; 4057453.35 north; 591267.08 east 2021-02-12T23:37:52Z- AHRS_M2 msg: 1613173072.56 epoch sec; seqNo:5417 2021-02-12T23:37:52Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.17 psi 2021-02-12T23:37:52Z- handleDepth()-Depth_Keller msg: 1613173072568 epoch sec; seqNo:5467; depth 20.04 2021-02-12T23:37:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173072.58 epoch sec; seqNo:5467; 4057453.73 north; 591267.08 east 2021-02-12T23:37:52Z- AHRS_M2 msg: 1613173072.97 epoch sec; seqNo:5418 2021-02-12T23:37:52Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.18 psi 2021-02-12T23:37:52Z- handleDepth()-Depth_Keller msg: 1613173072975 epoch sec; seqNo:5468; depth 20.14 2021-02-12T23:37:52Z- handleDvl() - RDI_Pathfinder msg: 1613173072.99 epoch sec; seqNo:2623 2021-02-12T23:37:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 98.12 , 69.88 , 102.99 2021-02-12T23:37:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.09 , 0.00 2021-02-12T23:37:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173073.00 epoch sec; seqNo:5468; 4057454.11 north; 591267.07 east 2021-02-12T23:37:53Z- AHRS_M2 msg: 1613173073.37 epoch sec; seqNo:5419 2021-02-12T23:37:53Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.19 psi 2021-02-12T23:37:53Z- handleDepth()-Depth_Keller msg: 1613173073381 epoch sec; seqNo:5469; depth 20.28 2021-02-12T23:37:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173073.39 epoch sec; seqNo:5469; 4057454.12 north; 591268.29 east 2021-02-12T23:37:53Z- AHRS_M2 msg: 1613173073.80 epoch sec; seqNo:5420 2021-02-12T23:37:53Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.20 psi 2021-02-12T23:37:53Z- handleDepth()-Depth_Keller msg: 1613173073807 epoch sec; seqNo:5470; depth 20.41 2021-02-12T23:37:53Z- handleDvl() - RDI_Pathfinder msg: 1613173073.83 epoch sec; seqNo:2624 2021-02-12T23:37:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 93.12 , 78.51 , 101.63 2021-02-12T23:37:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , -0.09 2021-02-12T23:37:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173073.84 epoch sec; seqNo:5470; 4057454.50 north; 591268.29 east 2021-02-12T23:37:54Z- AHRS_M2 msg: 1613173074.21 epoch sec; seqNo:5421 2021-02-12T23:37:54Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.21 psi 2021-02-12T23:37:54Z- handleDepth()-Depth_Keller msg: 1613173074217 epoch sec; seqNo:5471; depth 20.55 2021-02-12T23:37:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173074.25 epoch sec; seqNo:5471; 4057454.88 north; 591268.28 east 2021-02-12T23:37:54Z- AHRS_M2 msg: 1613173074.58 epoch sec; seqNo:5422 2021-02-12T23:37:54Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.21 psi 2021-02-12T23:37:54Z- handleDepth()-Depth_Keller msg: 1613173074588 epoch sec; seqNo:5472; depth 20.66 2021-02-12T23:37:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173074.61 epoch sec; seqNo:5472; 4057454.88 north; 591268.28 east 2021-02-12T23:37:54Z- AHRS_M2 msg: 1613173074.99 epoch sec; seqNo:5423 2021-02-12T23:37:54Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.22 psi 2021-02-12T23:37:54Z- LcmTrn::handleMessages() - handling 41 messages took 0.70 ms and 127 calls... 2021-02-12T23:37:54Z- waiting for 1613173072.77, time is 1613173074.99 2021-02-12T23:37:54Z- LcmTrn::updateTrn() >>>> pose time 1613173074.99, meas time 1613173073.83 <<<< 2021-02-12T23:37:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173073.83 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:37:54Z- TerrainNav::motionUpdate() - currEstimate = (4057455, 591268.28, 20.66) 2021-02-12T23:37:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:37:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:37:54Z- TNavBF::Projecting Measurements 2021-02-12T23:37:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:37:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:37:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:37:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:37:54Z- TNavFilter::4 good beams. 2021-02-12T23:37:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:37:54Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:37:54Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:37:54Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:37:54Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:37:54Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:37:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:37:54Z- TNavBF::mmse selected from filter -1 2021-02-12T23:37:54Z- TerrainMapOctree: (vehN, vehE ) = (4057454.20, 591267.75). 2021-02-12T23:37:54Z- TerrainMapOctree: Min Distance = 4827.21. 2021-02-12T23:37:54Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:37:55Z- There is no specific good beam that all particles have in common. 2021-02-12T23:37:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:37:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 8.75 2021-02-12T23:37:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:37:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 13.87 2021-02-12T23:37:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:37:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-12T23:37:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:37:55Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 16.53 2021-02-12T23:37:55Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:37:55Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:37:55Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:37:55Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:37:55Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:37:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:37:55Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:37:55Z- Filter 0 No beams used in subcloud update 2021-02-12T23:37:55Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:37:55Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:37:55Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 748.7 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:37:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173073.83 sec. 2021-02-12T23:37:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:37:55Z- TNavBF::mmse selected from filter -1 2021-02-12T23:37:55Z- variances([0], [2], and psi_berg) = 748.702971 833.031843 0.000000 2021-02-12T23:37:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:37:55Z- LcmTrn::publishEstimates() - MLE : 4057442.38 591226.78 20.66 2021-02-12T23:37:55Z- LcmTrn::publishEstimates() - MMSE : 4057454.88 591268.28 20.66 2021-02-12T23:37:55Z- LcmTrn::publishEstimates() - COVAR: 748.70 833.03 0.93 0.00 2021-02-12T23:37:55Z- handleDepth()-Depth_Keller msg: 1613173074992 epoch sec; seqNo:5473; depth 20.80 2021-02-12T23:37:55Z- handleDvl() - RDI_Pathfinder msg: 1613173075.00 epoch sec; seqNo:2625 2021-02-12T23:37:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 95.62 , 69.88 , 104.14 2021-02-12T23:37:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.02 , -0.15 2021-02-12T23:37:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173075.03 epoch sec; seqNo:5473; 4057455.26 north; 591268.28 east 2021-02-12T23:37:55Z- AHRS_M2 msg: 1613173075.39 epoch sec; seqNo:5424 2021-02-12T23:37:55Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.22 psi 2021-02-12T23:37:55Z- handleDepth()-Depth_Keller msg: 1613173075401 epoch sec; seqNo:5474; depth 20.93 2021-02-12T23:37:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173075.41 epoch sec; seqNo:5474; 4057455.65 north; 591269.50 east 2021-02-12T23:37:55Z- AHRS_M2 msg: 1613173075.80 epoch sec; seqNo:5425 2021-02-12T23:37:55Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.21 psi 2021-02-12T23:37:55Z- handleDepth()-Depth_Keller msg: 1613173075802 epoch sec; seqNo:5475; depth 21.07 2021-02-12T23:37:55Z- handleDvl() - RDI_Pathfinder msg: 1613173075.81 epoch sec; seqNo:2626 2021-02-12T23:37:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 94.47 , 69.47 , 101.77 2021-02-12T23:37:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.12 , -0.15 2021-02-12T23:37:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173075.82 epoch sec; seqNo:5475; 4057456.03 north; 591269.49 east 2021-02-12T23:37:56Z- AHRS_M2 msg: 1613173076.20 epoch sec; seqNo:5426 2021-02-12T23:37:56Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.21 psi 2021-02-12T23:37:56Z- handleDepth()-Depth_Keller msg: 1613173076213 epoch sec; seqNo:5476; depth 21.21 2021-02-12T23:37:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173076.22 epoch sec; seqNo:5476; 4057456.41 north; 591269.49 east 2021-02-12T23:37:56Z- AHRS_M2 msg: 1613173076.60 epoch sec; seqNo:5427 2021-02-12T23:37:56Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.21 psi 2021-02-12T23:37:56Z- handleDepth()-Depth_Keller msg: 1613173076629 epoch sec; seqNo:5477; depth 21.33 2021-02-12T23:37:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173076.68 epoch sec; seqNo:5477; 4057456.79 north; 591269.49 east 2021-02-12T23:37:57Z- AHRS_M2 msg: 1613173077.01 epoch sec; seqNo:5428 2021-02-12T23:37:57Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.21 psi 2021-02-12T23:37:57Z- handleDepth()-Depth_Keller msg: 1613173077014 epoch sec; seqNo:5478; depth 21.46 2021-02-12T23:37:57Z- handleDvl() - RDI_Pathfinder msg: 1613173077.03 epoch sec; seqNo:2627 2021-02-12T23:37:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 94.54 , 68.24 , 100.62 2021-02-12T23:37:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.17 , -0.07 2021-02-12T23:37:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173077.04 epoch sec; seqNo:5478; 4057457.17 north; 591269.48 east 2021-02-12T23:37:57Z- AHRS_M2 msg: 1613173077.41 epoch sec; seqNo:5429 2021-02-12T23:37:57Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.20 psi 2021-02-12T23:37:57Z- handleDepth()-Depth_Keller msg: 1613173077417 epoch sec; seqNo:5479; depth 21.58 2021-02-12T23:37:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173077.44 epoch sec; seqNo:5479; 4057457.55 north; 591269.48 east 2021-02-12T23:37:57Z- AHRS_M2 msg: 1613173077.82 epoch sec; seqNo:5430 2021-02-12T23:37:57Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.20 psi 2021-02-12T23:37:57Z- handleDepth()-Depth_Keller msg: 1613173077820 epoch sec; seqNo:5480; depth 21.71 2021-02-12T23:37:57Z- handleDvl() - RDI_Pathfinder msg: 1613173077.83 epoch sec; seqNo:2628 2021-02-12T23:37:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:37:57Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:37:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173077.84 epoch sec; seqNo:5480; 4057457.55 north; 591269.48 east 2021-02-12T23:37:58Z- AHRS_M2 msg: 1613173078.22 epoch sec; seqNo:5431 2021-02-12T23:37:58Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.20 psi 2021-02-12T23:37:58Z- handleDepth()-Depth_Keller msg: 1613173078226 epoch sec; seqNo:5481; depth 21.83 2021-02-12T23:37:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173078.24 epoch sec; seqNo:5481; 4057457.94 north; 591270.69 east 2021-02-12T23:37:58Z- AHRS_M2 msg: 1613173078.62 epoch sec; seqNo:5432 2021-02-12T23:37:58Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.20 psi 2021-02-12T23:37:58Z- handleDepth()-Depth_Keller msg: 1613173078628 epoch sec; seqNo:5482; depth 21.97 2021-02-12T23:37:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173078.64 epoch sec; seqNo:5482; 4057458.32 north; 591270.69 east 2021-02-12T23:37:59Z- AHRS_M2 msg: 1613173079.03 epoch sec; seqNo:5433 2021-02-12T23:37:59Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.20 psi 2021-02-12T23:37:59Z- handleDepth()-Depth_Keller msg: 1613173079032 epoch sec; seqNo:5483; depth 22.09 2021-02-12T23:37:59Z- handleDvl() - RDI_Pathfinder msg: 1613173079.04 epoch sec; seqNo:2629 2021-02-12T23:37:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.74 , 91.63 , 67.43 , 101.36 2021-02-12T23:37:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , 0.08 2021-02-12T23:37:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173079.07 epoch sec; seqNo:5483; 4057458.70 north; 591270.69 east 2021-02-12T23:37:59Z- AHRS_M2 msg: 1613173079.43 epoch sec; seqNo:5434 2021-02-12T23:37:59Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.20 psi 2021-02-12T23:37:59Z- handleDepth()-Depth_Keller msg: 1613173079436 epoch sec; seqNo:5484; depth 22.22 2021-02-12T23:37:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173079.45 epoch sec; seqNo:5484; 4057458.70 north; 591270.69 east 2021-02-12T23:37:59Z- AHRS_M2 msg: 1613173079.84 epoch sec; seqNo:5435 2021-02-12T23:37:59Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.20 psi 2021-02-12T23:37:59Z- handleDepth()-Depth_Keller msg: 1613173079845 epoch sec; seqNo:5485; depth 22.33 2021-02-12T23:37:59Z- handleDvl() - RDI_Pathfinder msg: 1613173079.86 epoch sec; seqNo:2630 2021-02-12T23:37:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 89.80 , 67.02 , 99.54 2021-02-12T23:37:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , 0.09 2021-02-12T23:37:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173079.87 epoch sec; seqNo:5485; 4057459.07 north; 591270.68 east 2021-02-12T23:38:00Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-12T23:38:00Z- waiting for 1613173077.99, time is 1613173079.87 2021-02-12T23:38:00Z- LcmTrn::updateTrn() equating pose 1613173079.87 and meas times 1613173079.87 2021-02-12T23:38:00Z- LcmTrn::updateTrn() >>>> pose time 1613173079.87, meas time 1613173079.87 <<<< 2021-02-12T23:38:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173079.87 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:38:00Z- TerrainNav::motionUpdate() - currEstimate = (4057459, 591270.68, 22.33) 2021-02-12T23:38:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:38:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:38:00Z- TNavBF::Projecting Measurements 2021-02-12T23:38:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:38:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:38:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:38:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:38:00Z- TNavFilter::4 good beams. 2021-02-12T23:38:00Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:38:00Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:38:00Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:38:00Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:38:00Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:38:00Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:38:00Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:38:00Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:00Z- TerrainMapOctree: (vehN, vehE ) = (4057459.07, 591270.68). 2021-02-12T23:38:00Z- TerrainMapOctree: Min Distance = 4821.56. 2021-02-12T23:38:00Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:38:00Z- There is no specific good beam that all particles have in common. 2021-02-12T23:38:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:38:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 9.95 2021-02-12T23:38:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:38:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 12.90 2021-02-12T23:38:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:38:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 7.19 2021-02-12T23:38:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:38:00Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 15.85 2021-02-12T23:38:00Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:38:00Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:38:00Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:38:00Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:38:00Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:38:00Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:38:00Z- Filter 0 No beams used in subcloud update 2021-02-12T23:38:00Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:38:00Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:38:00Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 748.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:38:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173079.87 sec. 2021-02-12T23:38:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:38:00Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:00Z- variances([0], [2], and psi_berg) = 748.597832 833.031747 0.000000 2021-02-12T23:38:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:38:00Z- LcmTrn::publishEstimates() - MLE : 4057446.57 591229.18 22.33 2021-02-12T23:38:00Z- LcmTrn::publishEstimates() - MMSE : 4057459.07 591270.68 22.33 2021-02-12T23:38:00Z- LcmTrn::publishEstimates() - COVAR: 748.60 833.03 0.85 0.00 2021-02-12T23:38:00Z- AHRS_M2 msg: 1613173080.24 epoch sec; seqNo:5436 2021-02-12T23:38:00Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.20 psi 2021-02-12T23:38:00Z- handleDepth()-Depth_Keller msg: 1613173080244 epoch sec; seqNo:5486; depth 22.46 2021-02-12T23:38:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173080.26 epoch sec; seqNo:5486; 4057459.45 north; 591270.68 east 2021-02-12T23:38:00Z- AHRS_M2 msg: 1613173080.66 epoch sec; seqNo:5437 2021-02-12T23:38:00Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.20 psi 2021-02-12T23:38:00Z- handleDepth()-Depth_Keller msg: 1613173080676 epoch sec; seqNo:5487; depth 22.59 2021-02-12T23:38:00Z- handleDvl() - RDI_Pathfinder msg: 1613173080.69 epoch sec; seqNo:2631 2021-02-12T23:38:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 92.24 , 67.84 , 97.11 2021-02-12T23:38:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.10 , 0.04 2021-02-12T23:38:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173080.70 epoch sec; seqNo:5487; 4057459.83 north; 591270.67 east 2021-02-12T23:38:01Z- AHRS_M2 msg: 1613173081.05 epoch sec; seqNo:5438 2021-02-12T23:38:01Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.20 psi 2021-02-12T23:38:01Z- handleDepth()-Depth_Keller msg: 1613173081052 epoch sec; seqNo:5488; depth 22.73 2021-02-12T23:38:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173081.06 epoch sec; seqNo:5488; 4057459.84 north; 591271.90 east 2021-02-12T23:38:01Z- AHRS_M2 msg: 1613173081.45 epoch sec; seqNo:5439 2021-02-12T23:38:01Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.20 psi 2021-02-12T23:38:01Z- handleDepth()-Depth_Keller msg: 1613173081459 epoch sec; seqNo:5489; depth 22.85 2021-02-12T23:38:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173081.47 epoch sec; seqNo:5489; 4057460.22 north; 591271.89 east 2021-02-12T23:38:01Z- AHRS_M2 msg: 1613173081.86 epoch sec; seqNo:5440 2021-02-12T23:38:01Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.21 psi 2021-02-12T23:38:01Z- handleDepth()-Depth_Keller msg: 1613173081874 epoch sec; seqNo:5490; depth 22.97 2021-02-12T23:38:01Z- handleDvl() - RDI_Pathfinder msg: 1613173081.89 epoch sec; seqNo:2632 2021-02-12T23:38:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 91.29 , 67.02 , 97.78 2021-02-12T23:38:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.09 , -0.04 2021-02-12T23:38:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173081.90 epoch sec; seqNo:5490; 4057460.60 north; 591271.89 east 2021-02-12T23:38:02Z- AHRS_M2 msg: 1613173082.26 epoch sec; seqNo:5441 2021-02-12T23:38:02Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.21 psi 2021-02-12T23:38:02Z- handleDepth()-Depth_Keller msg: 1613173082264 epoch sec; seqNo:5491; depth 23.11 2021-02-12T23:38:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173082.28 epoch sec; seqNo:5491; 4057460.60 north; 591271.89 east 2021-02-12T23:38:02Z- AHRS_M2 msg: 1613173082.67 epoch sec; seqNo:5442 2021-02-12T23:38:02Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.21 psi 2021-02-12T23:38:02Z- handleDepth()-Depth_Keller msg: 1613173082682 epoch sec; seqNo:5492; depth 23.23 2021-02-12T23:38:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173082.71 epoch sec; seqNo:5492; 4057460.98 north; 591271.88 east 2021-02-12T23:38:03Z- AHRS_M2 msg: 1613173083.07 epoch sec; seqNo:5443 2021-02-12T23:38:03Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.21 psi 2021-02-12T23:38:03Z- handleDepth()-Depth_Keller msg: 1613173083081 epoch sec; seqNo:5493; depth 23.34 2021-02-12T23:38:03Z- handleDvl() - RDI_Pathfinder msg: 1613173083.09 epoch sec; seqNo:2633 2021-02-12T23:38:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 91.29 , 65.38 , 96.70 2021-02-12T23:38:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , -0.09 2021-02-12T23:38:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173083.10 epoch sec; seqNo:5493; 4057461.37 north; 591273.10 east 2021-02-12T23:38:03Z- AHRS_M2 msg: 1613173083.47 epoch sec; seqNo:5444 2021-02-12T23:38:03Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.21 psi 2021-02-12T23:38:03Z- handleDepth()-Depth_Keller msg: 1613173083486 epoch sec; seqNo:5494; depth 23.46 2021-02-12T23:38:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173083.50 epoch sec; seqNo:5494; 4057461.37 north; 591273.10 east 2021-02-12T23:38:03Z- AHRS_M2 msg: 1613173083.88 epoch sec; seqNo:5445 2021-02-12T23:38:03Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.21 psi 2021-02-12T23:38:03Z- handleDepth()-Depth_Keller msg: 1613173083880 epoch sec; seqNo:5495; depth 23.59 2021-02-12T23:38:03Z- handleDvl() - RDI_Pathfinder msg: 1613173083.89 epoch sec; seqNo:2634 2021-02-12T23:38:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 90.28 , 0.00 , 98.93 2021-02-12T23:38:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , -0.11 2021-02-12T23:38:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173083.92 epoch sec; seqNo:5495; 4057461.75 north; 591273.10 east 2021-02-12T23:38:04Z- AHRS_M2 msg: 1613173084.28 epoch sec; seqNo:5446 2021-02-12T23:38:04Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.21 psi 2021-02-12T23:38:04Z- handleDepth()-Depth_Keller msg: 1613173084284 epoch sec; seqNo:5496; depth 23.73 2021-02-12T23:38:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173084.30 epoch sec; seqNo:5496; 4057462.13 north; 591273.09 east 2021-02-12T23:38:04Z- AHRS_M2 msg: 1613173084.71 epoch sec; seqNo:5447 2021-02-12T23:38:04Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.20 psi 2021-02-12T23:38:04Z- handleDepth()-Depth_Keller msg: 1613173084718 epoch sec; seqNo:5497; depth 23.83 2021-02-12T23:38:04Z- handleDvl() - RDI_Pathfinder msg: 1613173084.74 epoch sec; seqNo:2635 2021-02-12T23:38:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 90.75 , 66.61 , 98.05 2021-02-12T23:38:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , -0.08 2021-02-12T23:38:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173084.76 epoch sec; seqNo:5497; 4057462.51 north; 591273.09 east 2021-02-12T23:38:05Z- AHRS_M2 msg: 1613173085.09 epoch sec; seqNo:5448 2021-02-12T23:38:05Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.20 psi 2021-02-12T23:38:05Z- handleDepth()-Depth_Keller msg: 1613173085097 epoch sec; seqNo:5498; depth 23.99 2021-02-12T23:38:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173085.11 epoch sec; seqNo:5498; 4057462.89 north; 591273.08 east 2021-02-12T23:38:05Z- LcmTrn::handleMessages() - handling 44 messages took 0.77 ms and 129 calls... 2021-02-12T23:38:05Z- waiting for 1613173082.87, time is 1613173085.11 2021-02-12T23:38:05Z- LcmTrn::updateTrn() >>>> pose time 1613173085.11, meas time 1613173084.74 <<<< 2021-02-12T23:38:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173084.74 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:38:05Z- TerrainNav::motionUpdate() - currEstimate = (4057463, 591273.08, 23.99) 2021-02-12T23:38:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:38:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:38:05Z- TNavBF::Projecting Measurements 2021-02-12T23:38:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:38:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:38:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:38:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:38:05Z- TNavFilter::4 good beams. 2021-02-12T23:38:05Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:38:05Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:38:05Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:38:05Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:38:05Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:38:05Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:38:05Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:38:05Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:05Z- TerrainMapOctree: (vehN, vehE ) = (4057462.62, 591272.92). 2021-02-12T23:38:05Z- TerrainMapOctree: Min Distance = 4817.39. 2021-02-12T23:38:05Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:38:05Z- There is no specific good beam that all particles have in common. 2021-02-12T23:38:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:38:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-12T23:38:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:38:05Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 13.18 2021-02-12T23:38:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:38:05Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 7.10 2021-02-12T23:38:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:38:05Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 15.38 2021-02-12T23:38:05Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:38:05Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:38:05Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:38:05Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:38:05Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:38:05Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:38:05Z- Filter 0 No beams used in subcloud update 2021-02-12T23:38:05Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:38:05Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:38:05Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 748.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:38:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173084.74 sec. 2021-02-12T23:38:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:38:05Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:05Z- variances([0], [2], and psi_berg) = 748.473328 833.031634 0.000000 2021-02-12T23:38:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:38:05Z- LcmTrn::publishEstimates() - MLE : 4057450.39 591231.58 23.99 2021-02-12T23:38:05Z- LcmTrn::publishEstimates() - MMSE : 4057462.89 591273.08 23.99 2021-02-12T23:38:05Z- LcmTrn::publishEstimates() - COVAR: 748.47 833.03 0.80 0.00 2021-02-12T23:38:05Z- AHRS_M2 msg: 1613173085.49 epoch sec; seqNo:5449 2021-02-12T23:38:05Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.20 psi 2021-02-12T23:38:05Z- handleDepth()-Depth_Keller msg: 1613173085498 epoch sec; seqNo:5499; depth 24.09 2021-02-12T23:38:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173085.51 epoch sec; seqNo:5499; 4057462.89 north; 591273.08 east 2021-02-12T23:38:05Z- AHRS_M2 msg: 1613173085.90 epoch sec; seqNo:5450 2021-02-12T23:38:05Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.20 psi 2021-02-12T23:38:05Z- handleDepth()-Depth_Keller msg: 1613173085902 epoch sec; seqNo:5500; depth 24.21 2021-02-12T23:38:05Z- handleDvl() - RDI_Pathfinder msg: 1613173085.91 epoch sec; seqNo:2636 2021-02-12T23:38:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 89.20 , 64.97 , 97.85 2021-02-12T23:38:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , -0.04 2021-02-12T23:38:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173085.92 epoch sec; seqNo:5500; 4057463.28 north; 591274.30 east 2021-02-12T23:38:06Z- AHRS_M2 msg: 1613173086.30 epoch sec; seqNo:5451 2021-02-12T23:38:06Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.20 psi 2021-02-12T23:38:06Z- handleDepth()-Depth_Keller msg: 1613173086304 epoch sec; seqNo:5501; depth 24.35 2021-02-12T23:38:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173086.32 epoch sec; seqNo:5501; 4057463.66 north; 591274.30 east 2021-02-12T23:38:06Z- AHRS_M2 msg: 1613173086.71 epoch sec; seqNo:5452 2021-02-12T23:38:06Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.20 psi 2021-02-12T23:38:06Z- handleDepth()-Depth_Keller msg: 1613173086712 epoch sec; seqNo:5502; depth 24.46 2021-02-12T23:38:06Z- handleDvl() - RDI_Pathfinder msg: 1613173086.72 epoch sec; seqNo:2637 2021-02-12T23:38:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 88.18 , 65.38 , 97.92 2021-02-12T23:38:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , -0.03 2021-02-12T23:38:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173086.73 epoch sec; seqNo:5502; 4057464.04 north; 591274.29 east 2021-02-12T23:38:07Z- AHRS_M2 msg: 1613173087.11 epoch sec; seqNo:5453 2021-02-12T23:38:07Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.20 psi 2021-02-12T23:38:07Z- handleDepth()-Depth_Keller msg: 1613173087115 epoch sec; seqNo:5503; depth 24.59 2021-02-12T23:38:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173087.13 epoch sec; seqNo:5503; 4057464.42 north; 591274.29 east 2021-02-12T23:38:07Z- AHRS_M2 msg: 1613173087.51 epoch sec; seqNo:5454 2021-02-12T23:38:07Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.20 psi 2021-02-12T23:38:07Z- handleDepth()-Depth_Keller msg: 1613173087516 epoch sec; seqNo:5504; depth 24.71 2021-02-12T23:38:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173087.53 epoch sec; seqNo:5504; 4057464.42 north; 591274.29 east 2021-02-12T23:38:07Z- AHRS_M2 msg: 1613173087.92 epoch sec; seqNo:5455 2021-02-12T23:38:07Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.20 psi 2021-02-12T23:38:07Z- handleDepth()-Depth_Keller msg: 1613173087920 epoch sec; seqNo:5505; depth 24.84 2021-02-12T23:38:07Z- handleDvl() - RDI_Pathfinder msg: 1613173087.93 epoch sec; seqNo:2638 2021-02-12T23:38:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 88.18 , 65.79 , 97.92 2021-02-12T23:38:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.08 , -0.02 2021-02-12T23:38:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173087.96 epoch sec; seqNo:5505; 4057464.80 north; 591274.28 east 2021-02-12T23:38:08Z- AHRS_M2 msg: 1613173088.32 epoch sec; seqNo:5456 2021-02-12T23:38:08Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.20 psi 2021-02-12T23:38:08Z- handleDepth()-Depth_Keller msg: 1613173088329 epoch sec; seqNo:5506; depth 24.98 2021-02-12T23:38:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173088.34 epoch sec; seqNo:5506; 4057465.17 north; 591274.28 east 2021-02-12T23:38:08Z- AHRS_M2 msg: 1613173088.72 epoch sec; seqNo:5457 2021-02-12T23:38:08Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.20 psi 2021-02-12T23:38:08Z- handleDepth()-Depth_Keller msg: 1613173088730 epoch sec; seqNo:5507; depth 25.10 2021-02-12T23:38:08Z- handleDvl() - RDI_Pathfinder msg: 1613173088.74 epoch sec; seqNo:2639 2021-02-12T23:38:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 88.18 , 64.57 , 99.00 2021-02-12T23:38:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.08 , -0.03 2021-02-12T23:38:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173088.75 epoch sec; seqNo:5507; 4057465.57 north; 591275.50 east 2021-02-12T23:38:09Z- AHRS_M2 msg: 1613173089.13 epoch sec; seqNo:5458 2021-02-12T23:38:09Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.20 psi 2021-02-12T23:38:09Z- handleDepth()-Depth_Keller msg: 1613173089136 epoch sec; seqNo:5508; depth 25.24 2021-02-12T23:38:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173089.16 epoch sec; seqNo:5508; 4057465.95 north; 591275.49 east 2021-02-12T23:38:09Z- AHRS_M2 msg: 1613173089.53 epoch sec; seqNo:5459 2021-02-12T23:38:09Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.20 psi 2021-02-12T23:38:09Z- handleDepth()-Depth_Keller msg: 1613173089536 epoch sec; seqNo:5509; depth 25.36 2021-02-12T23:38:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173089.54 epoch sec; seqNo:5509; 4057465.95 north; 591275.49 east 2021-02-12T23:38:09Z- AHRS_M2 msg: 1613173089.94 epoch sec; seqNo:5460 2021-02-12T23:38:09Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.20 psi 2021-02-12T23:38:09Z- handleDepth()-Depth_Keller msg: 1613173089940 epoch sec; seqNo:5510; depth 25.49 2021-02-12T23:38:09Z- handleDvl() - RDI_Pathfinder msg: 1613173089.95 epoch sec; seqNo:2640 2021-02-12T23:38:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 87.10 , 64.97 , 96.83 2021-02-12T23:38:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , -0.01 2021-02-12T23:38:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173089.96 epoch sec; seqNo:5510; 4057466.32 north; 591275.49 east 2021-02-12T23:38:10Z- LcmTrn::handleMessages() - handling 41 messages took 0.65 ms and 128 calls... 2021-02-12T23:38:10Z- waiting for 1613173088.11, time is 1613173089.96 2021-02-12T23:38:10Z- LcmTrn::updateTrn() equating pose 1613173089.96 and meas times 1613173089.96 2021-02-12T23:38:10Z- LcmTrn::updateTrn() >>>> pose time 1613173089.96, meas time 1613173089.96 <<<< 2021-02-12T23:38:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173089.96 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:38:10Z- TerrainNav::motionUpdate() - currEstimate = (4057466, 591275.49, 25.49) 2021-02-12T23:38:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:38:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:38:10Z- TNavBF::Projecting Measurements 2021-02-12T23:38:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:38:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:38:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:38:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:38:10Z- TNavFilter::4 good beams. 2021-02-12T23:38:10Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:38:10Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:38:10Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:38:10Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:38:10Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:38:10Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:38:10Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:38:10Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:10Z- TerrainMapOctree: (vehN, vehE ) = (4057466.32, 591275.49). 2021-02-12T23:38:10Z- TerrainMapOctree: Min Distance = 4812.89. 2021-02-12T23:38:10Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:38:10Z- There is no specific good beam that all particles have in common. 2021-02-12T23:38:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:38:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 7.81 2021-02-12T23:38:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:38:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 12.14 2021-02-12T23:38:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:38:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 6.75 2021-02-12T23:38:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:38:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 15.00 2021-02-12T23:38:10Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:38:10Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:38:10Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:38:10Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:38:10Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:38:10Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:38:10Z- Filter 0 No beams used in subcloud update 2021-02-12T23:38:10Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:38:10Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:38:10Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 748.4 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:38:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173089.96 sec. 2021-02-12T23:38:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:38:10Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:10Z- variances([0], [2], and psi_berg) = 748.375654 833.031545 0.000000 2021-02-12T23:38:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:38:10Z- LcmTrn::publishEstimates() - MLE : 4057453.82 591233.99 25.49 2021-02-12T23:38:10Z- LcmTrn::publishEstimates() - MMSE : 4057466.32 591275.49 25.49 2021-02-12T23:38:10Z- LcmTrn::publishEstimates() - COVAR: 748.38 833.03 0.74 0.00 2021-02-12T23:38:10Z- AHRS_M2 msg: 1613173090.34 epoch sec; seqNo:5461 2021-02-12T23:38:10Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.20 psi 2021-02-12T23:38:10Z- handleDepth()-Depth_Keller msg: 1613173090352 epoch sec; seqNo:5511; depth 25.64 2021-02-12T23:38:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173090.38 epoch sec; seqNo:5511; 4057466.70 north; 591275.49 east 2021-02-12T23:38:10Z- AHRS_M2 msg: 1613173090.74 epoch sec; seqNo:5462 2021-02-12T23:38:10Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.20 psi 2021-02-12T23:38:10Z- handleDepth()-Depth_Keller msg: 1613173090749 epoch sec; seqNo:5512; depth 25.76 2021-02-12T23:38:10Z- handleDvl() - RDI_Pathfinder msg: 1613173090.77 epoch sec; seqNo:2641 2021-02-12T23:38:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 88.25 , 64.16 , 97.98 2021-02-12T23:38:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , -0.00 2021-02-12T23:38:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173090.78 epoch sec; seqNo:5512; 4057467.08 north; 591275.48 east 2021-02-12T23:38:11Z- AHRS_M2 msg: 1613173091.15 epoch sec; seqNo:5463 2021-02-12T23:38:11Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.20 psi 2021-02-12T23:38:11Z- handleDepth()-Depth_Keller msg: 1613173091156 epoch sec; seqNo:5513; depth 25.88 2021-02-12T23:38:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173091.17 epoch sec; seqNo:5513; 4057467.46 north; 591275.48 east 2021-02-12T23:38:11Z- AHRS_M2 msg: 1613173091.55 epoch sec; seqNo:5464 2021-02-12T23:38:11Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.19 psi 2021-02-12T23:38:11Z- handleDepth()-Depth_Keller msg: 1613173091557 epoch sec; seqNo:5514; depth 26.02 2021-02-12T23:38:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173091.57 epoch sec; seqNo:5514; 4057467.47 north; 591276.70 east 2021-02-12T23:38:11Z- AHRS_M2 msg: 1613173091.96 epoch sec; seqNo:5465 2021-02-12T23:38:11Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.19 psi 2021-02-12T23:38:11Z- handleDepth()-Depth_Keller msg: 1613173091960 epoch sec; seqNo:5515; depth 26.15 2021-02-12T23:38:11Z- handleDvl() - RDI_Pathfinder msg: 1613173091.98 epoch sec; seqNo:2642 2021-02-12T23:38:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 87.17 , 62.93 , 97.98 2021-02-12T23:38:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.08 , -0.01 2021-02-12T23:38:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173092.00 epoch sec; seqNo:5515; 4057467.85 north; 591276.69 east 2021-02-12T23:38:12Z- AHRS_M2 msg: 1613173092.36 epoch sec; seqNo:5466 2021-02-12T23:38:12Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.19 psi 2021-02-12T23:38:12Z- handleDepth()-Depth_Keller msg: 1613173092364 epoch sec; seqNo:5516; depth 26.29 2021-02-12T23:38:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173092.38 epoch sec; seqNo:5516; 4057468.23 north; 591276.69 east 2021-02-12T23:38:12Z- AHRS_M2 msg: 1613173092.76 epoch sec; seqNo:5467 2021-02-12T23:38:12Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.19 psi 2021-02-12T23:38:12Z- handleDepth()-Depth_Keller msg: 1613173092781 epoch sec; seqNo:5517; depth 26.42 2021-02-12T23:38:12Z- handleDvl() - RDI_Pathfinder msg: 1613173092.81 epoch sec; seqNo:2643 2021-02-12T23:38:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 86.09 , 62.52 , 97.98 2021-02-12T23:38:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , 0.00 2021-02-12T23:38:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173092.83 epoch sec; seqNo:5517; 4057468.61 north; 591276.69 east 2021-02-12T23:38:13Z- AHRS_M2 msg: 1613173093.17 epoch sec; seqNo:5468 2021-02-12T23:38:13Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.18 psi 2021-02-12T23:38:13Z- handleDepth()-Depth_Keller msg: 1613173093174 epoch sec; seqNo:5518; depth 26.56 2021-02-12T23:38:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173093.18 epoch sec; seqNo:5518; 4057468.99 north; 591276.68 east 2021-02-12T23:38:13Z- AHRS_M2 msg: 1613173093.58 epoch sec; seqNo:5469 2021-02-12T23:38:13Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.18 psi 2021-02-12T23:38:13Z- handleDepth()-Depth_Keller msg: 1613173093599 epoch sec; seqNo:5519; depth 26.68 2021-02-12T23:38:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173093.62 epoch sec; seqNo:5519; 4057468.99 north; 591276.68 east 2021-02-12T23:38:13Z- AHRS_M2 msg: 1613173093.98 epoch sec; seqNo:5470 2021-02-12T23:38:13Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.17 psi 2021-02-12T23:38:13Z- handleDepth()-Depth_Keller msg: 1613173093980 epoch sec; seqNo:5520; depth 26.81 2021-02-12T23:38:13Z- handleDvl() - RDI_Pathfinder msg: 1613173093.99 epoch sec; seqNo:2644 2021-02-12T23:38:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 0.00 , 62.93 , 95.82 2021-02-12T23:38:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.01 2021-02-12T23:38:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173094.02 epoch sec; seqNo:5520; 4057469.37 north; 591276.68 east 2021-02-12T23:38:14Z- AHRS_M2 msg: 1613173094.38 epoch sec; seqNo:5471 2021-02-12T23:38:14Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.17 psi 2021-02-12T23:38:14Z- handleDepth()-Depth_Keller msg: 1613173094387 epoch sec; seqNo:5521; depth 26.94 2021-02-12T23:38:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173094.40 epoch sec; seqNo:5521; 4057469.76 north; 591277.89 east 2021-02-12T23:38:14Z- AHRS_M2 msg: 1613173094.79 epoch sec; seqNo:5472 2021-02-12T23:38:14Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.16 psi 2021-02-12T23:38:14Z- handleDepth()-Depth_Keller msg: 1613173094790 epoch sec; seqNo:5522; depth 27.07 2021-02-12T23:38:14Z- handleDvl() - RDI_Pathfinder msg: 1613173094.80 epoch sec; seqNo:2645 2021-02-12T23:38:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 83.92 , 62.93 , 93.66 2021-02-12T23:38:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , -0.01 2021-02-12T23:38:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173094.81 epoch sec; seqNo:5522; 4057470.14 north; 591277.89 east 2021-02-12T23:38:15Z- AHRS_M2 msg: 1613173095.19 epoch sec; seqNo:5473 2021-02-12T23:38:15Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.16 psi 2021-02-12T23:38:15Z- handleDepth()-Depth_Keller msg: 1613173095192 epoch sec; seqNo:5523; depth 27.20 2021-02-12T23:38:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173095.21 epoch sec; seqNo:5523; 4057470.14 north; 591277.89 east 2021-02-12T23:38:15Z- LcmTrn::handleMessages() - handling 44 messages took 0.76 ms and 129 calls... 2021-02-12T23:38:15Z- waiting for 1613173092.96, time is 1613173095.21 2021-02-12T23:38:15Z- LcmTrn::updateTrn() >>>> pose time 1613173095.21, meas time 1613173094.80 <<<< 2021-02-12T23:38:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173094.80 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:38:15Z- TerrainNav::motionUpdate() - currEstimate = (4057470, 591277.89, 27.20) 2021-02-12T23:38:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:38:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:38:15Z- TNavBF::Projecting Measurements 2021-02-12T23:38:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:38:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:38:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:38:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:38:15Z- TNavFilter::4 good beams. 2021-02-12T23:38:15Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:38:15Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:38:15Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:38:15Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:38:15Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:38:15Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:38:15Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:38:15Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:15Z- TerrainMapOctree: (vehN, vehE ) = (4057469.84, 591277.70). 2021-02-12T23:38:15Z- TerrainMapOctree: Min Distance = 4808.76. 2021-02-12T23:38:15Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:38:15Z- There is no specific good beam that all particles have in common. 2021-02-12T23:38:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:38:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 7.72 2021-02-12T23:38:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:38:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 11.27 2021-02-12T23:38:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:38:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 6.34 2021-02-12T23:38:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:38:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.74 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 14.03 2021-02-12T23:38:15Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:38:15Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:38:15Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:38:15Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:38:15Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:38:15Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:38:15Z- Filter 0 No beams used in subcloud update 2021-02-12T23:38:15Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:38:15Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:38:15Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 748.3 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:38:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173094.80 sec. 2021-02-12T23:38:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:38:15Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:15Z- variances([0], [2], and psi_berg) = 748.250352 833.031431 0.000000 2021-02-12T23:38:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:38:15Z- LcmTrn::publishEstimates() - MLE : 4057457.64 591236.39 27.20 2021-02-12T23:38:15Z- LcmTrn::publishEstimates() - MMSE : 4057470.14 591277.89 27.20 2021-02-12T23:38:15Z- LcmTrn::publishEstimates() - COVAR: 748.25 833.03 0.72 0.00 2021-02-12T23:38:15Z- AHRS_M2 msg: 1613173095.59 epoch sec; seqNo:5474 2021-02-12T23:38:15Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.16 psi 2021-02-12T23:38:15Z- handleDepth()-Depth_Keller msg: 1613173095597 epoch sec; seqNo:5524; depth 27.33 2021-02-12T23:38:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173095.61 epoch sec; seqNo:5524; 4057470.52 north; 591277.89 east 2021-02-12T23:38:15Z- AHRS_M2 msg: 1613173096.00 epoch sec; seqNo:5475 2021-02-12T23:38:15Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.15 psi 2021-02-12T23:38:16Z- handleDepth()-Depth_Keller msg: 1613173096000 epoch sec; seqNo:5525; depth 27.46 2021-02-12T23:38:16Z- handleDvl() - RDI_Pathfinder msg: 1613173096.01 epoch sec; seqNo:2646 2021-02-12T23:38:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 83.92 , 62.12 , 94.74 2021-02-12T23:38:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.02 2021-02-12T23:38:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173096.02 epoch sec; seqNo:5525; 4057470.90 north; 591277.88 east 2021-02-12T23:38:16Z- AHRS_M2 msg: 1613173096.40 epoch sec; seqNo:5476 2021-02-12T23:38:16Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.15 psi 2021-02-12T23:38:16Z- handleDepth()-Depth_Keller msg: 1613173096410 epoch sec; seqNo:5526; depth 27.59 2021-02-12T23:38:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173096.42 epoch sec; seqNo:5526; 4057471.27 north; 591277.88 east 2021-02-12T23:38:16Z- AHRS_M2 msg: 1613173096.80 epoch sec; seqNo:5477 2021-02-12T23:38:16Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.15 psi 2021-02-12T23:38:16Z- handleDepth()-Depth_Keller msg: 1613173096809 epoch sec; seqNo:5527; depth 27.73 2021-02-12T23:38:16Z- handleDvl() - RDI_Pathfinder msg: 1613173096.83 epoch sec; seqNo:2647 2021-02-12T23:38:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 79.69 , 62.12 , 97.98 2021-02-12T23:38:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , -0.05 2021-02-12T23:38:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173096.84 epoch sec; seqNo:5527; 4057471.29 north; 591279.10 east 2021-02-12T23:38:17Z- AHRS_M2 msg: 1613173097.21 epoch sec; seqNo:5478 2021-02-12T23:38:17Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.15 psi 2021-02-12T23:38:17Z- handleDepth()-Depth_Keller msg: 1613173097217 epoch sec; seqNo:5528; depth 27.87 2021-02-12T23:38:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173097.23 epoch sec; seqNo:5528; 4057471.67 north; 591279.10 east 2021-02-12T23:38:17Z- AHRS_M2 msg: 1613173097.63 epoch sec; seqNo:5479 2021-02-12T23:38:17Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.15 psi 2021-02-12T23:38:17Z- handleDepth()-Depth_Keller msg: 1613173097636 epoch sec; seqNo:5529; depth 28.00 2021-02-12T23:38:17Z- handleDvl() - RDI_Pathfinder msg: 1613173097.66 epoch sec; seqNo:2648 2021-02-12T23:38:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 78.87 , 61.30 , 97.98 2021-02-12T23:38:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , -0.08 2021-02-12T23:38:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173097.67 epoch sec; seqNo:5529; 4057472.05 north; 591279.09 east 2021-02-12T23:38:18Z- AHRS_M2 msg: 1613173098.02 epoch sec; seqNo:5480 2021-02-12T23:38:18Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.15 psi 2021-02-12T23:38:18Z- handleDepth()-Depth_Keller msg: 1613173098029 epoch sec; seqNo:5530; depth 28.15 2021-02-12T23:38:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173098.04 epoch sec; seqNo:5530; 4057472.42 north; 591279.09 east 2021-02-12T23:38:18Z- AHRS_M2 msg: 1613173098.42 epoch sec; seqNo:5481 2021-02-12T23:38:18Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.16 psi 2021-02-12T23:38:18Z- handleDepth()-Depth_Keller msg: 1613173098424 epoch sec; seqNo:5531; depth 28.27 2021-02-12T23:38:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173098.44 epoch sec; seqNo:5531; 4057472.80 north; 591279.08 east 2021-02-12T23:38:18Z- AHRS_M2 msg: 1613173098.82 epoch sec; seqNo:5482 2021-02-12T23:38:18Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.16 psi 2021-02-12T23:38:18Z- handleDepth()-Depth_Keller msg: 1613173098828 epoch sec; seqNo:5532; depth 28.40 2021-02-12T23:38:18Z- handleDvl() - RDI_Pathfinder msg: 1613173098.84 epoch sec; seqNo:2649 2021-02-12T23:38:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 79.28 , 60.89 , 97.98 2021-02-12T23:38:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , -0.09 2021-02-12T23:38:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173098.86 epoch sec; seqNo:5532; 4057472.80 north; 591279.08 east 2021-02-12T23:38:19Z- AHRS_M2 msg: 1613173099.23 epoch sec; seqNo:5483 2021-02-12T23:38:19Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.16 psi 2021-02-12T23:38:19Z- handleDepth()-Depth_Keller msg: 1613173099235 epoch sec; seqNo:5533; depth 28.54 2021-02-12T23:38:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173099.25 epoch sec; seqNo:5533; 4057473.20 north; 591280.30 east 2021-02-12T23:38:19Z- AHRS_M2 msg: 1613173099.63 epoch sec; seqNo:5484 2021-02-12T23:38:19Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.17 psi 2021-02-12T23:38:19Z- handleDepth()-Depth_Keller msg: 1613173099641 epoch sec; seqNo:5534; depth 28.66 2021-02-12T23:38:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173099.65 epoch sec; seqNo:5534; 4057473.57 north; 591280.30 east 2021-02-12T23:38:20Z- AHRS_M2 msg: 1613173100.04 epoch sec; seqNo:5485 2021-02-12T23:38:20Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.17 psi 2021-02-12T23:38:20Z- handleDepth()-Depth_Keller msg: 1613173100040 epoch sec; seqNo:5535; depth 28.80 2021-02-12T23:38:20Z- handleDvl() - RDI_Pathfinder msg: 1613173100.06 epoch sec; seqNo:2650 2021-02-12T23:38:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 82.84 , 60.07 , 92.58 2021-02-12T23:38:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.09 2021-02-12T23:38:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173100.07 epoch sec; seqNo:5535; 4057473.95 north; 591280.29 east 2021-02-12T23:38:20Z- AHRS_M2 msg: 1613173100.44 epoch sec; seqNo:5486 2021-02-12T23:38:20Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.18 psi 2021-02-12T23:38:20Z- LcmTrn::handleMessages() - handling 42 messages took 0.64 ms and 129 calls... 2021-02-12T23:38:20Z- waiting for 1613173098.21, time is 1613173100.44 2021-02-12T23:38:20Z- LcmTrn::updateTrn() >>>> pose time 1613173100.44, meas time 1613173100.06 <<<< 2021-02-12T23:38:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173100.06 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:38:20Z- TerrainNav::motionUpdate() - currEstimate = (4057474, 591280.29, 28.80) 2021-02-12T23:38:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:38:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:38:20Z- TNavBF::Projecting Measurements 2021-02-12T23:38:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:38:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:38:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:38:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:38:20Z- TNavFilter::4 good beams. 2021-02-12T23:38:20Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:38:20Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:38:20Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:38:20Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:38:20Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:38:20Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:38:20Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:38:20Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:20Z- TerrainMapOctree: (vehN, vehE ) = (4057473.67, 591280.12). 2021-02-12T23:38:20Z- TerrainMapOctree: Min Distance = 4804.25. 2021-02-12T23:38:20Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:38:20Z- There is no specific good beam that all particles have in common. 2021-02-12T23:38:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:38:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 7.27 2021-02-12T23:38:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:38:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 10.98 2021-02-12T23:38:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:38:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 5.77 2021-02-12T23:38:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:38:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.72 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 13.71 2021-02-12T23:38:20Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:38:20Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:38:20Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:38:20Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:38:20Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:38:20Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:38:20Z- Filter 0 No beams used in subcloud update 2021-02-12T23:38:20Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:38:20Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:38:20Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 748.2 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:38:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173100.06 sec. 2021-02-12T23:38:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:38:20Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:20Z- variances([0], [2], and psi_berg) = 748.125898 833.031318 0.000000 2021-02-12T23:38:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:38:20Z- LcmTrn::publishEstimates() - MLE : 4057461.45 591238.79 28.80 2021-02-12T23:38:20Z- LcmTrn::publishEstimates() - MMSE : 4057473.95 591280.29 28.80 2021-02-12T23:38:20Z- LcmTrn::publishEstimates() - COVAR: 748.13 833.03 0.70 0.00 2021-02-12T23:38:20Z- handleDepth()-Depth_Keller msg: 1613173100444 epoch sec; seqNo:5536; depth 28.94 2021-02-12T23:38:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173100.45 epoch sec; seqNo:5536; 4057473.95 north; 591280.29 east 2021-02-12T23:38:20Z- AHRS_M2 msg: 1613173100.84 epoch sec; seqNo:5487 2021-02-12T23:38:20Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.18 psi 2021-02-12T23:38:20Z- handleDepth()-Depth_Keller msg: 1613173100848 epoch sec; seqNo:5537; depth 29.03 2021-02-12T23:38:20Z- handleDvl() - RDI_Pathfinder msg: 1613173100.86 epoch sec; seqNo:2651 2021-02-12T23:38:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.20 , 85.00 , 61.30 , 97.98 2021-02-12T23:38:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , -0.06 2021-02-12T23:38:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173100.87 epoch sec; seqNo:5537; 4057474.33 north; 591280.29 east 2021-02-12T23:38:21Z- AHRS_M2 msg: 1613173101.25 epoch sec; seqNo:5488 2021-02-12T23:38:21Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.19 psi 2021-02-12T23:38:21Z- handleDepth()-Depth_Keller msg: 1613173101252 epoch sec; seqNo:5538; depth 29.19 2021-02-12T23:38:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173101.27 epoch sec; seqNo:5538; 4057474.71 north; 591280.28 east 2021-02-12T23:38:21Z- AHRS_M2 msg: 1613173101.65 epoch sec; seqNo:5489 2021-02-12T23:38:21Z- AHRS_M2 msg: 0.65 phi; -0.28 theta; -0.19 psi 2021-02-12T23:38:21Z- handleDepth()-Depth_Keller msg: 1613173101656 epoch sec; seqNo:5539; depth 29.32 2021-02-12T23:38:21Z- handleDvl() - RDI_Pathfinder msg: 1613173101.68 epoch sec; seqNo:2652 2021-02-12T23:38:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.38 , 79.28 , 60.89 , 89.33 2021-02-12T23:38:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , 0.00 2021-02-12T23:38:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173101.69 epoch sec; seqNo:5539; 4057475.10 north; 591281.50 east 2021-02-12T23:38:22Z- AHRS_M2 msg: 1613173102.06 epoch sec; seqNo:5490 2021-02-12T23:38:22Z- AHRS_M2 msg: 0.63 phi; -0.22 theta; -0.18 psi 2021-02-12T23:38:22Z- handleDepth()-Depth_Keller msg: 1613173102066 epoch sec; seqNo:5540; depth 29.47 2021-02-12T23:38:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173102.08 epoch sec; seqNo:5540; 4057475.10 north; 591281.50 east 2021-02-12T23:38:22Z- AHRS_M2 msg: 1613173102.48 epoch sec; seqNo:5491 2021-02-12T23:38:22Z- AHRS_M2 msg: 0.62 phi; -0.18 theta; -0.17 psi 2021-02-12T23:38:22Z- handleDepth()-Depth_Keller msg: 1613173102480 epoch sec; seqNo:5541; depth 29.61 2021-02-12T23:38:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173102.49 epoch sec; seqNo:5541; 4057475.48 north; 591281.50 east 2021-02-12T23:38:22Z- AHRS_M2 msg: 1613173102.87 epoch sec; seqNo:5492 2021-02-12T23:38:22Z- AHRS_M2 msg: 0.61 phi; -0.13 theta; -0.16 psi 2021-02-12T23:38:22Z- handleDepth()-Depth_Keller msg: 1613173102881 epoch sec; seqNo:5542; depth 29.75 2021-02-12T23:38:22Z- handleDvl() - RDI_Pathfinder msg: 1613173102.89 epoch sec; seqNo:2653 2021-02-12T23:38:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.57 , 81.76 , 62.12 , 82.84 2021-02-12T23:38:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , 0.03 2021-02-12T23:38:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173102.90 epoch sec; seqNo:5542; 4057475.86 north; 591281.49 east 2021-02-12T23:38:23Z- AHRS_M2 msg: 1613173103.28 epoch sec; seqNo:5493 2021-02-12T23:38:23Z- AHRS_M2 msg: 0.59 phi; -0.09 theta; -0.16 psi 2021-02-12T23:38:23Z- handleDepth()-Depth_Keller msg: 1613173103283 epoch sec; seqNo:5543; depth 29.86 2021-02-12T23:38:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173103.30 epoch sec; seqNo:5543; 4057476.24 north; 591281.49 east 2021-02-12T23:38:23Z- AHRS_M2 msg: 1613173103.67 epoch sec; seqNo:5494 2021-02-12T23:38:23Z- AHRS_M2 msg: 0.58 phi; -0.05 theta; -0.16 psi 2021-02-12T23:38:23Z- handleDepth()-Depth_Keller msg: 1613173103681 epoch sec; seqNo:5544; depth 29.97 2021-02-12T23:38:23Z- handleDvl() - RDI_Pathfinder msg: 1613173103.69 epoch sec; seqNo:2654 2021-02-12T23:38:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.75 , 74.37 , 64.57 , 72.33 2021-02-12T23:38:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , 0.04 2021-02-12T23:38:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173103.72 epoch sec; seqNo:5544; 4057476.62 north; 591281.48 east 2021-02-12T23:38:24Z- AHRS_M2 msg: 1613173104.08 epoch sec; seqNo:5495 2021-02-12T23:38:24Z- AHRS_M2 msg: 0.57 phi; -0.00 theta; -0.16 psi 2021-02-12T23:38:24Z- handleDepth()-Depth_Keller msg: 1613173104089 epoch sec; seqNo:5545; depth 30.07 2021-02-12T23:38:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173104.10 epoch sec; seqNo:5545; 4057476.62 north; 591281.48 east 2021-02-12T23:38:24Z- AHRS_M2 msg: 1613173104.48 epoch sec; seqNo:5496 2021-02-12T23:38:24Z- AHRS_M2 msg: 0.56 phi; 0.04 theta; -0.16 psi 2021-02-12T23:38:24Z- handleDepth()-Depth_Keller msg: 1613173104493 epoch sec; seqNo:5546; depth 30.14 2021-02-12T23:38:24Z- handleDvl() - RDI_Pathfinder msg: 1613173104.51 epoch sec; seqNo:2655 2021-02-12T23:38:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.11 , 74.44 , 68.29 , 67.97 2021-02-12T23:38:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.01 2021-02-12T23:38:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173104.52 epoch sec; seqNo:5546; 4057477.01 north; 591282.70 east 2021-02-12T23:38:24Z- AHRS_M2 msg: 1613173104.89 epoch sec; seqNo:5497 2021-02-12T23:38:24Z- AHRS_M2 msg: 0.56 phi; 0.08 theta; -0.15 psi 2021-02-12T23:38:24Z- handleDepth()-Depth_Keller msg: 1613173104892 epoch sec; seqNo:5547; depth 30.20 2021-02-12T23:38:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173104.92 epoch sec; seqNo:5547; 4057477.39 north; 591282.70 east 2021-02-12T23:38:25Z- AHRS_M2 msg: 1613173105.30 epoch sec; seqNo:5498 2021-02-12T23:38:25Z- AHRS_M2 msg: 0.55 phi; 0.12 theta; -0.15 psi 2021-02-12T23:38:25Z- handleDepth()-Depth_Keller msg: 1613173105315 epoch sec; seqNo:5548; depth 30.25 2021-02-12T23:38:25Z- handleDvl() - RDI_Pathfinder msg: 1613173105.34 epoch sec; seqNo:2656 2021-02-12T23:38:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.08 , 72.82 , 72.82 , 64.73 2021-02-12T23:38:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , -0.04 2021-02-12T23:38:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173105.35 epoch sec; seqNo:5548; 4057477.77 north; 591282.69 east 2021-02-12T23:38:25Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-12T23:38:25Z- waiting for 1613173103.44, time is 1613173105.35 2021-02-12T23:38:25Z- LcmTrn::updateTrn() equating pose 1613173105.35 and meas times 1613173105.35 2021-02-12T23:38:25Z- LcmTrn::updateTrn() >>>> pose time 1613173105.35, meas time 1613173105.35 <<<< 2021-02-12T23:38:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173105.35 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:38:25Z- TerrainNav::motionUpdate() - currEstimate = (4057478, 591282.69, 30.25) 2021-02-12T23:38:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:38:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:38:25Z- TNavBF::Projecting Measurements 2021-02-12T23:38:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:38:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:38:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:38:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:38:25Z- TNavFilter::4 good beams. 2021-02-12T23:38:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:38:25Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:38:25Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:38:25Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:38:25Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:38:25Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:38:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:38:25Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:25Z- TerrainMapOctree: (vehN, vehE ) = (4057477.77, 591282.69). 2021-02-12T23:38:25Z- TerrainMapOctree: Min Distance = 4799.44. 2021-02-12T23:38:25Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:38:25Z- There is no specific good beam that all particles have in common. 2021-02-12T23:38:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:38:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 6.57 2021-02-12T23:38:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:38:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 8.48 2021-02-12T23:38:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:38:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 8.48 2021-02-12T23:38:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:38:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.72 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 6.70 2021-02-12T23:38:25Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:38:25Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:38:25Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:38:25Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:38:25Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:38:25Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:38:25Z- Filter 0 No beams used in subcloud update 2021-02-12T23:38:25Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:38:25Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:38:25Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 748.0 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:38:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173105.35 sec. 2021-02-12T23:38:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:38:25Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:25Z- variances([0], [2], and psi_berg) = 748.025101 833.031226 0.000000 2021-02-12T23:38:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:38:25Z- LcmTrn::publishEstimates() - MLE : 4057465.27 591241.19 30.25 2021-02-12T23:38:25Z- LcmTrn::publishEstimates() - MMSE : 4057477.77 591282.69 30.25 2021-02-12T23:38:25Z- LcmTrn::publishEstimates() - COVAR: 748.03 833.03 0.66 0.00 2021-02-12T23:38:25Z- AHRS_M2 msg: 1613173105.70 epoch sec; seqNo:5499 2021-02-12T23:38:25Z- AHRS_M2 msg: 0.55 phi; 0.15 theta; -0.15 psi 2021-02-12T23:38:25Z- handleDepth()-Depth_Keller msg: 1613173105700 epoch sec; seqNo:5549; depth 30.29 2021-02-12T23:38:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173105.71 epoch sec; seqNo:5549; 4057478.15 north; 591282.69 east 2021-02-12T23:38:26Z- AHRS_M2 msg: 1613173106.10 epoch sec; seqNo:5500 2021-02-12T23:38:26Z- AHRS_M2 msg: 0.54 phi; 0.18 theta; -0.15 psi 2021-02-12T23:38:26Z- handleDepth()-Depth_Keller msg: 1613173106106 epoch sec; seqNo:5550; depth 30.31 2021-02-12T23:38:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173106.12 epoch sec; seqNo:5550; 4057478.15 north; 591282.69 east 2021-02-12T23:38:26Z- AHRS_M2 msg: 1613173106.50 epoch sec; seqNo:5501 2021-02-12T23:38:26Z- AHRS_M2 msg: 0.54 phi; 0.20 theta; -0.15 psi 2021-02-12T23:38:26Z- handleDepth()-Depth_Keller msg: 1613173106519 epoch sec; seqNo:5551; depth 30.31 2021-02-12T23:38:26Z- handleDvl() - RDI_Pathfinder msg: 1613173106.54 epoch sec; seqNo:2657 2021-02-12T23:38:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.73 , 74.44 , 0.00 , 62.79 2021-02-12T23:38:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.00 , -0.09 2021-02-12T23:38:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173106.56 epoch sec; seqNo:5551; 4057478.52 north; 591282.69 east 2021-02-12T23:38:26Z- AHRS_M2 msg: 1613173106.91 epoch sec; seqNo:5502 2021-02-12T23:38:26Z- AHRS_M2 msg: 0.54 phi; 0.23 theta; -0.15 psi 2021-02-12T23:38:26Z- handleDepth()-Depth_Keller msg: 1613173106913 epoch sec; seqNo:5552; depth 30.30 2021-02-12T23:38:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173106.92 epoch sec; seqNo:5552; 4057478.90 north; 591282.68 east 2021-02-12T23:38:27Z- AHRS_M2 msg: 1613173107.32 epoch sec; seqNo:5503 2021-02-12T23:38:27Z- AHRS_M2 msg: 0.54 phi; 0.25 theta; -0.15 psi 2021-02-12T23:38:27Z- handleDepth()-Depth_Keller msg: 1613173107324 epoch sec; seqNo:5553; depth 30.27 2021-02-12T23:38:27Z- handleDvl() - RDI_Pathfinder msg: 1613173107.34 epoch sec; seqNo:2658 2021-02-12T23:38:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.70 , 76.38 , 0.00 , 61.82 2021-02-12T23:38:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , -0.12 2021-02-12T23:38:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173107.36 epoch sec; seqNo:5553; 4057479.30 north; 591283.90 east 2021-02-12T23:38:27Z- AHRS_M2 msg: 1613173107.72 epoch sec; seqNo:5504 2021-02-12T23:38:27Z- AHRS_M2 msg: 0.54 phi; 0.27 theta; -0.15 psi 2021-02-12T23:38:27Z- handleDepth()-Depth_Keller msg: 1613173107722 epoch sec; seqNo:5554; depth 30.24 2021-02-12T23:38:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173107.73 epoch sec; seqNo:5554; 4057479.30 north; 591283.90 east 2021-02-12T23:38:28Z- AHRS_M2 msg: 1613173108.12 epoch sec; seqNo:5505 2021-02-12T23:38:28Z- AHRS_M2 msg: 0.54 phi; 0.28 theta; -0.14 psi 2021-02-12T23:38:28Z- handleDepth()-Depth_Keller msg: 1613173108124 epoch sec; seqNo:5555; depth 30.20 2021-02-12T23:38:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173108.13 epoch sec; seqNo:5555; 4057479.67 north; 591283.89 east 2021-02-12T23:38:28Z- AHRS_M2 msg: 1613173108.52 epoch sec; seqNo:5506 2021-02-12T23:38:28Z- AHRS_M2 msg: 0.54 phi; 0.29 theta; -0.14 psi 2021-02-12T23:38:28Z- handleDepth()-Depth_Keller msg: 1613173108529 epoch sec; seqNo:5556; depth 30.14 2021-02-12T23:38:28Z- handleDvl() - RDI_Pathfinder msg: 1613173108.54 epoch sec; seqNo:2659 2021-02-12T23:38:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.35 , 76.71 , 0.00 , 61.17 2021-02-12T23:38:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.04 , -0.10 2021-02-12T23:38:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173108.55 epoch sec; seqNo:5556; 4057480.05 north; 591283.89 east 2021-02-12T23:38:28Z- AHRS_M2 msg: 1613173108.94 epoch sec; seqNo:5507 2021-02-12T23:38:28Z- AHRS_M2 msg: 0.54 phi; 0.31 theta; -0.14 psi 2021-02-12T23:38:28Z- handleDepth()-Depth_Keller msg: 1613173108946 epoch sec; seqNo:5557; depth 30.09 2021-02-12T23:38:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173108.96 epoch sec; seqNo:5557; 4057480.05 north; 591283.89 east 2021-02-12T23:38:29Z- AHRS_M2 msg: 1613173109.33 epoch sec; seqNo:5508 2021-02-12T23:38:29Z- AHRS_M2 msg: 0.54 phi; 0.32 theta; -0.13 psi 2021-02-12T23:38:29Z- handleDepth()-Depth_Keller msg: 1613173109336 epoch sec; seqNo:5558; depth 30.02 2021-02-12T23:38:29Z- handleDvl() - RDI_Pathfinder msg: 1613173109.35 epoch sec; seqNo:2660 2021-02-12T23:38:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 67.64 , 0.00 , 0.00 , 60.20 2021-02-12T23:38:29Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:38:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173109.37 epoch sec; seqNo:5558; 4057480.43 north; 591283.89 east 2021-02-12T23:38:29Z- AHRS_M2 msg: 1613173109.74 epoch sec; seqNo:5509 2021-02-12T23:38:29Z- AHRS_M2 msg: 0.55 phi; 0.32 theta; -0.13 psi 2021-02-12T23:38:29Z- handleDepth()-Depth_Keller msg: 1613173109745 epoch sec; seqNo:5559; depth 29.94 2021-02-12T23:38:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173109.76 epoch sec; seqNo:5559; 4057480.81 north; 591283.88 east 2021-02-12T23:38:30Z- AHRS_M2 msg: 1613173110.14 epoch sec; seqNo:5510 2021-02-12T23:38:30Z- AHRS_M2 msg: 0.55 phi; 0.33 theta; -0.13 psi 2021-02-12T23:38:30Z- handleDepth()-Depth_Keller msg: 1613173110148 epoch sec; seqNo:5560; depth 29.86 2021-02-12T23:38:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173110.16 epoch sec; seqNo:5560; 4057480.81 north; 591283.88 east 2021-02-12T23:38:30Z- AHRS_M2 msg: 1613173110.55 epoch sec; seqNo:5511 2021-02-12T23:38:30Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.12 psi 2021-02-12T23:38:30Z- handleDepth()-Depth_Keller msg: 1613173110551 epoch sec; seqNo:5561; depth 29.78 2021-02-12T23:38:30Z- handleDvl() - RDI_Pathfinder msg: 1613173110.56 epoch sec; seqNo:2661 2021-02-12T23:38:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.56 , 76.65 , 0.00 , 60.47 2021-02-12T23:38:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , -0.02 2021-02-12T23:38:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173110.57 epoch sec; seqNo:5561; 4057481.19 north; 591283.88 east 2021-02-12T23:38:30Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 131 calls... 2021-02-12T23:38:30Z- waiting for 1613173108.35, time is 1613173110.57 2021-02-12T23:38:30Z- LcmTrn::updateTrn() equating pose 1613173110.57 and meas times 1613173110.57 2021-02-12T23:38:30Z- LcmTrn::updateTrn() >>>> pose time 1613173110.57, meas time 1613173110.57 <<<< 2021-02-12T23:38:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173110.57 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:38:30Z- TerrainNav::motionUpdate() - currEstimate = (4057481, 591283.88, 29.78) 2021-02-12T23:38:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:38:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:38:30Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173110.57 sec.; No relevant navigation data available 2021-02-12T23:38:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:38:30Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:30Z- variances([0], [2], and psi_berg) = 747.934867 833.031145 0.000000 2021-02-12T23:38:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:38:30Z- LcmTrn::publishEstimates() - MLE : 4057468.69 591242.38 29.78 2021-02-12T23:38:30Z- LcmTrn::publishEstimates() - MMSE : 4057481.19 591283.88 29.78 2021-02-12T23:38:30Z- LcmTrn::publishEstimates() - COVAR: 747.93 833.03 0.85 0.00 2021-02-12T23:38:30Z- AHRS_M2 msg: 1613173110.95 epoch sec; seqNo:5512 2021-02-12T23:38:30Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.12 psi 2021-02-12T23:38:30Z- handleDepth()-Depth_Keller msg: 1613173110952 epoch sec; seqNo:5562; depth 29.69 2021-02-12T23:38:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173110.96 epoch sec; seqNo:5562; 4057481.58 north; 591285.10 east 2021-02-12T23:38:31Z- AHRS_M2 msg: 1613173111.35 epoch sec; seqNo:5513 2021-02-12T23:38:31Z- AHRS_M2 msg: 0.55 phi; 0.35 theta; -0.12 psi 2021-02-12T23:38:31Z- handleDepth()-Depth_Keller msg: 1613173111356 epoch sec; seqNo:5563; depth 29.61 2021-02-12T23:38:31Z- handleDvl() - RDI_Pathfinder msg: 1613173111.37 epoch sec; seqNo:2662 2021-02-12T23:38:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.26 , 77.35 , 0.00 , 61.17 2021-02-12T23:38:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.01 2021-02-12T23:38:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173111.38 epoch sec; seqNo:5563; 4057481.96 north; 591285.09 east 2021-02-12T23:38:31Z- AHRS_M2 msg: 1613173111.76 epoch sec; seqNo:5514 2021-02-12T23:38:31Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.12 psi 2021-02-12T23:38:31Z- handleDepth()-Depth_Keller msg: 1613173111763 epoch sec; seqNo:5564; depth 29.49 2021-02-12T23:38:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173111.77 epoch sec; seqNo:5564; 4057481.96 north; 591285.09 east 2021-02-12T23:38:32Z- AHRS_M2 msg: 1613173112.16 epoch sec; seqNo:5515 2021-02-12T23:38:32Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.11 psi 2021-02-12T23:38:32Z- handleDepth()-Depth_Keller msg: 1613173112164 epoch sec; seqNo:5565; depth 29.39 2021-02-12T23:38:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173112.18 epoch sec; seqNo:5565; 4057482.34 north; 591285.09 east 2021-02-12T23:38:32Z- AHRS_M2 msg: 1613173112.57 epoch sec; seqNo:5516 2021-02-12T23:38:32Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.11 psi 2021-02-12T23:38:32Z- handleDepth()-Depth_Keller msg: 1613173112571 epoch sec; seqNo:5566; depth 29.29 2021-02-12T23:38:32Z- handleDvl() - RDI_Pathfinder msg: 1613173112.58 epoch sec; seqNo:2663 2021-02-12T23:38:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.23 , 77.35 , 0.00 , 60.52 2021-02-12T23:38:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , 0.03 2021-02-12T23:38:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173112.61 epoch sec; seqNo:5566; 4057482.72 north; 591285.08 east 2021-02-12T23:38:32Z- AHRS_M2 msg: 1613173112.97 epoch sec; seqNo:5517 2021-02-12T23:38:32Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.11 psi 2021-02-12T23:38:32Z- handleDepth()-Depth_Keller msg: 1613173112972 epoch sec; seqNo:5567; depth 29.19 2021-02-12T23:38:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173112.98 epoch sec; seqNo:5567; 4057482.72 north; 591285.08 east 2021-02-12T23:38:33Z- AHRS_M2 msg: 1613173113.37 epoch sec; seqNo:5518 2021-02-12T23:38:33Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.10 psi 2021-02-12T23:38:33Z- handleDepth()-Depth_Keller msg: 1613173113378 epoch sec; seqNo:5568; depth 29.08 2021-02-12T23:38:33Z- handleDvl() - RDI_Pathfinder msg: 1613173113.39 epoch sec; seqNo:2664 2021-02-12T23:38:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.23 , 77.35 , 0.00 , 59.88 2021-02-12T23:38:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.01 , 0.01 2021-02-12T23:38:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173113.40 epoch sec; seqNo:5568; 4057483.10 north; 591285.08 east 2021-02-12T23:38:33Z- AHRS_M2 msg: 1613173113.78 epoch sec; seqNo:5519 2021-02-12T23:38:33Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.10 psi 2021-02-12T23:38:33Z- handleDepth()-Depth_Keller msg: 1613173113790 epoch sec; seqNo:5569; depth 28.99 2021-02-12T23:38:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173113.80 epoch sec; seqNo:5569; 4057483.49 north; 591286.30 east 2021-02-12T23:38:34Z- AHRS_M2 msg: 1613173114.18 epoch sec; seqNo:5520 2021-02-12T23:38:34Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.10 psi 2021-02-12T23:38:34Z- handleDepth()-Depth_Keller msg: 1613173114184 epoch sec; seqNo:5570; depth 28.86 2021-02-12T23:38:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173114.19 epoch sec; seqNo:5570; 4057483.87 north; 591286.29 east 2021-02-12T23:38:34Z- AHRS_M2 msg: 1613173114.59 epoch sec; seqNo:5521 2021-02-12T23:38:34Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.10 psi 2021-02-12T23:38:34Z- handleDepth()-Depth_Keller msg: 1613173114591 epoch sec; seqNo:5571; depth 28.75 2021-02-12T23:38:34Z- handleDvl() - RDI_Pathfinder msg: 1613173114.60 epoch sec; seqNo:2665 2021-02-12T23:38:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.88 , 77.03 , 0.00 , 61.82 2021-02-12T23:38:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.00 , 0.00 2021-02-12T23:38:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173114.61 epoch sec; seqNo:5571; 4057483.87 north; 591286.29 east 2021-02-12T23:38:34Z- AHRS_M2 msg: 1613173114.99 epoch sec; seqNo:5522 2021-02-12T23:38:34Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.09 psi 2021-02-12T23:38:34Z- handleDepth()-Depth_Keller msg: 1613173114994 epoch sec; seqNo:5572; depth 28.65 2021-02-12T23:38:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173115.01 epoch sec; seqNo:5572; 4057484.25 north; 591286.29 east 2021-02-12T23:38:35Z- AHRS_M2 msg: 1613173115.39 epoch sec; seqNo:5523 2021-02-12T23:38:35Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.09 psi 2021-02-12T23:38:35Z- handleDepth()-Depth_Keller msg: 1613173115396 epoch sec; seqNo:5573; depth 28.54 2021-02-12T23:38:35Z- handleDvl() - RDI_Pathfinder msg: 1613173115.41 epoch sec; seqNo:2666 2021-02-12T23:38:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.21 , 77.04 , 0.00 , 61.82 2021-02-12T23:38:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , -0.02 2021-02-12T23:38:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173115.42 epoch sec; seqNo:5573; 4057484.63 north; 591286.28 east 2021-02-12T23:38:35Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-12T23:38:35Z- waiting for 1613173113.57, time is 1613173115.42 2021-02-12T23:38:35Z- LcmTrn::updateTrn() equating pose 1613173115.42 and meas times 1613173115.42 2021-02-12T23:38:35Z- LcmTrn::updateTrn() >>>> pose time 1613173115.42, meas time 1613173115.42 <<<< 2021-02-12T23:38:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173115.42 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:38:35Z- TerrainNav::motionUpdate() - currEstimate = (4057485, 591286.28, 28.54) 2021-02-12T23:38:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:38:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:38:35Z- TNavBF::Projecting Measurements 2021-02-12T23:38:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:38:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:38:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:38:35Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:38:35Z- TNavFilter::3 good beams. 2021-02-12T23:38:35Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:38:35Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:38:35Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:38:35Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:38:35Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:38:35Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:38:35Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:38:35Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:35Z- TerrainMapOctree: (vehN, vehE ) = (4057484.63, 591286.28). 2021-02-12T23:38:35Z- TerrainMapOctree: Min Distance = 4791.83. 2021-02-12T23:38:35Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:38:35Z- There is no specific good beam that all particles have in common. 2021-02-12T23:38:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:38:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 8.11 2021-02-12T23:38:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:38:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 9.50 2021-02-12T23:38:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:38:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 6.12 2021-02-12T23:38:35Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:38:35Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:38:35Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:38:35Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:38:35Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:38:35Z- Filter 0 No beams used in subcloud update 2021-02-12T23:38:35Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:38:35Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:38:35Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 747.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:38:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173115.42 sec. 2021-02-12T23:38:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:38:35Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:35Z- variances([0], [2], and psi_berg) = 747.837881 833.031057 0.000000 2021-02-12T23:38:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:38:35Z- LcmTrn::publishEstimates() - MLE : 4057472.13 591244.78 28.54 2021-02-12T23:38:35Z- LcmTrn::publishEstimates() - MMSE : 4057484.63 591286.28 28.54 2021-02-12T23:38:35Z- LcmTrn::publishEstimates() - COVAR: 747.84 833.03 0.79 0.00 2021-02-12T23:38:35Z- AHRS_M2 msg: 1613173115.80 epoch sec; seqNo:5524 2021-02-12T23:38:35Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.09 psi 2021-02-12T23:38:35Z- handleDepth()-Depth_Keller msg: 1613173115800 epoch sec; seqNo:5574; depth 28.44 2021-02-12T23:38:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173115.81 epoch sec; seqNo:5574; 4057484.63 north; 591286.28 east 2021-02-12T23:38:36Z- AHRS_M2 msg: 1613173116.20 epoch sec; seqNo:5525 2021-02-12T23:38:36Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.09 psi 2021-02-12T23:38:36Z- handleDepth()-Depth_Keller msg: 1613173116204 epoch sec; seqNo:5575; depth 28.33 2021-02-12T23:38:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173116.21 epoch sec; seqNo:5575; 4057485.00 north; 591286.28 east 2021-02-12T23:38:36Z- AHRS_M2 msg: 1613173116.61 epoch sec; seqNo:5526 2021-02-12T23:38:36Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.10 psi 2021-02-12T23:38:36Z- handleDepth()-Depth_Keller msg: 1613173116617 epoch sec; seqNo:5576; depth 28.21 2021-02-12T23:38:36Z- handleDvl() - RDI_Pathfinder msg: 1613173116.63 epoch sec; seqNo:2667 2021-02-12T23:38:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.21 , 77.68 , 0.00 , 62.47 2021-02-12T23:38:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , -0.02 2021-02-12T23:38:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173116.64 epoch sec; seqNo:5576; 4057485.40 north; 591287.50 east 2021-02-12T23:38:37Z- AHRS_M2 msg: 1613173117.01 epoch sec; seqNo:5527 2021-02-12T23:38:37Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.10 psi 2021-02-12T23:38:37Z- handleDepth()-Depth_Keller msg: 1613173117022 epoch sec; seqNo:5577; depth 28.08 2021-02-12T23:38:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173117.03 epoch sec; seqNo:5577; 4057485.40 north; 591287.50 east 2021-02-12T23:38:37Z- AHRS_M2 msg: 1613173117.41 epoch sec; seqNo:5528 2021-02-12T23:38:37Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.11 psi 2021-02-12T23:38:37Z- handleDepth()-Depth_Keller msg: 1613173117421 epoch sec; seqNo:5578; depth 27.97 2021-02-12T23:38:37Z- handleDvl() - RDI_Pathfinder msg: 1613173117.44 epoch sec; seqNo:2668 2021-02-12T23:38:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.20 , 78.65 , 0.00 , 62.47 2021-02-12T23:38:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.02 2021-02-12T23:38:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173117.45 epoch sec; seqNo:5578; 4057485.77 north; 591287.49 east 2021-02-12T23:38:37Z- AHRS_M2 msg: 1613173117.82 epoch sec; seqNo:5529 2021-02-12T23:38:37Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.11 psi 2021-02-12T23:38:37Z- handleDepth()-Depth_Keller msg: 1613173117820 epoch sec; seqNo:5579; depth 27.85 2021-02-12T23:38:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173117.83 epoch sec; seqNo:5579; 4057486.15 north; 591287.49 east 2021-02-12T23:38:38Z- AHRS_M2 msg: 1613173118.22 epoch sec; seqNo:5530 2021-02-12T23:38:38Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.12 psi 2021-02-12T23:38:38Z- handleDepth()-Depth_Keller msg: 1613173118226 epoch sec; seqNo:5580; depth 27.74 2021-02-12T23:38:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173118.23 epoch sec; seqNo:5580; 4057486.15 north; 591287.49 east 2021-02-12T23:38:38Z- AHRS_M2 msg: 1613173118.62 epoch sec; seqNo:5531 2021-02-12T23:38:38Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.13 psi 2021-02-12T23:38:38Z- handleDepth()-Depth_Keller msg: 1613173118629 epoch sec; seqNo:5581; depth 27.62 2021-02-12T23:38:38Z- handleDvl() - RDI_Pathfinder msg: 1613173118.65 epoch sec; seqNo:2669 2021-02-12T23:38:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.56 , 79.62 , 0.00 , 62.79 2021-02-12T23:38:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , -0.01 2021-02-12T23:38:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173118.66 epoch sec; seqNo:5581; 4057486.53 north; 591287.48 east 2021-02-12T23:38:39Z- AHRS_M2 msg: 1613173119.03 epoch sec; seqNo:5532 2021-02-12T23:38:39Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-12T23:38:39Z- handleDepth()-Depth_Keller msg: 1613173119037 epoch sec; seqNo:5582; depth 27.52 2021-02-12T23:38:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173119.05 epoch sec; seqNo:5582; 4057486.91 north; 591287.48 east 2021-02-12T23:38:39Z- AHRS_M2 msg: 1613173119.44 epoch sec; seqNo:5533 2021-02-12T23:38:39Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-12T23:38:39Z- handleDepth()-Depth_Keller msg: 1613173119442 epoch sec; seqNo:5583; depth 27.40 2021-02-12T23:38:39Z- handleDvl() - RDI_Pathfinder msg: 1613173119.45 epoch sec; seqNo:2670 2021-02-12T23:38:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.18 , 80.74 , 0.00 , 62.69 2021-02-12T23:38:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , -0.02 2021-02-12T23:38:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173119.46 epoch sec; seqNo:5583; 4057487.30 north; 591288.70 east 2021-02-12T23:38:39Z- AHRS_M2 msg: 1613173119.84 epoch sec; seqNo:5534 2021-02-12T23:38:39Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-12T23:38:39Z- handleDepth()-Depth_Keller msg: 1613173119842 epoch sec; seqNo:5584; depth 27.28 2021-02-12T23:38:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173119.85 epoch sec; seqNo:5584; 4057487.30 north; 591288.70 east 2021-02-12T23:38:40Z- AHRS_M2 msg: 1613173120.24 epoch sec; seqNo:5535 2021-02-12T23:38:40Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:38:40Z- handleDepth()-Depth_Keller msg: 1613173120245 epoch sec; seqNo:5585; depth 27.18 2021-02-12T23:38:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173120.26 epoch sec; seqNo:5585; 4057487.68 north; 591288.69 east 2021-02-12T23:38:40Z- AHRS_M2 msg: 1613173120.65 epoch sec; seqNo:5536 2021-02-12T23:38:40Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:38:40Z- handleDepth()-Depth_Keller msg: 1613173120652 epoch sec; seqNo:5586; depth 27.07 2021-02-12T23:38:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173120.67 epoch sec; seqNo:5586; 4057488.06 north; 591288.69 east 2021-02-12T23:38:40Z- LcmTrn::handleMessages() - handling 43 messages took 0.68 ms and 130 calls... 2021-02-12T23:38:40Z- waiting for 1613173118.42, time is 1613173120.67 2021-02-12T23:38:40Z- LcmTrn::updateTrn() >>>> pose time 1613173120.67, meas time 1613173119.45 <<<< 2021-02-12T23:38:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173119.45 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:38:40Z- TerrainNav::motionUpdate() - currEstimate = (4057488, 591288.69, 27.07) 2021-02-12T23:38:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:38:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:38:40Z- TNavBF::Projecting Measurements 2021-02-12T23:38:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:38:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:38:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:38:40Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:38:40Z- TNavFilter::3 good beams. 2021-02-12T23:38:40Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:38:40Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:38:40Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:38:40Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:38:40Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:38:40Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:38:40Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:38:40Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:40Z- TerrainMapOctree: (vehN, vehE ) = (4057487.27, 591288.13). 2021-02-12T23:38:40Z- TerrainMapOctree: Min Distance = 4788.62. 2021-02-12T23:38:40Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:38:40Z- There is no specific good beam that all particles have in common. 2021-02-12T23:38:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:38:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 7.88 2021-02-12T23:38:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:38:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 10.43 2021-02-12T23:38:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:38:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 6.29 2021-02-12T23:38:40Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:38:40Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:38:40Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:38:40Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:38:40Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:38:40Z- Filter 0 No beams used in subcloud update 2021-02-12T23:38:40Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:38:40Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:38:40Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 747.8 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:38:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173119.45 sec. 2021-02-12T23:38:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:38:40Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:40Z- variances([0], [2], and psi_berg) = 747.706410 833.030938 0.000000 2021-02-12T23:38:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:38:40Z- LcmTrn::publishEstimates() - MLE : 4057475.56 591247.19 27.07 2021-02-12T23:38:40Z- LcmTrn::publishEstimates() - MMSE : 4057488.06 591288.69 27.07 2021-02-12T23:38:40Z- LcmTrn::publishEstimates() - COVAR: 747.71 833.03 0.79 0.00 2021-02-12T23:38:41Z- AHRS_M2 msg: 1613173121.05 epoch sec; seqNo:5537 2021-02-12T23:38:41Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:38:41Z- handleDepth()-Depth_Keller msg: 1613173121052 epoch sec; seqNo:5587; depth 26.96 2021-02-12T23:38:41Z- handleDvl() - RDI_Pathfinder msg: 1613173121.07 epoch sec; seqNo:2671 2021-02-12T23:38:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 85.00 , 97.98 , 63.34 2021-02-12T23:38:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.04 2021-02-12T23:38:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173121.08 epoch sec; seqNo:5587; 4057488.44 north; 591288.69 east 2021-02-12T23:38:41Z- AHRS_M2 msg: 1613173121.45 epoch sec; seqNo:5538 2021-02-12T23:38:41Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-12T23:38:41Z- handleDepth()-Depth_Keller msg: 1613173121456 epoch sec; seqNo:5588; depth 26.85 2021-02-12T23:38:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173121.47 epoch sec; seqNo:5588; 4057488.44 north; 591288.69 east 2021-02-12T23:38:41Z- AHRS_M2 msg: 1613173121.86 epoch sec; seqNo:5539 2021-02-12T23:38:41Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-12T23:38:41Z- handleDepth()-Depth_Keller msg: 1613173121861 epoch sec; seqNo:5589; depth 26.74 2021-02-12T23:38:41Z- handleDvl() - RDI_Pathfinder msg: 1613173121.87 epoch sec; seqNo:2672 2021-02-12T23:38:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 87.17 , 96.90 , 62.93 2021-02-12T23:38:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , -0.05 2021-02-12T23:38:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173121.88 epoch sec; seqNo:5589; 4057488.83 north; 591289.90 east 2021-02-12T23:38:42Z- AHRS_M2 msg: 1613173122.26 epoch sec; seqNo:5540 2021-02-12T23:38:42Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-12T23:38:42Z- handleDepth()-Depth_Keller msg: 1613173122267 epoch sec; seqNo:5590; depth 26.62 2021-02-12T23:38:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173122.28 epoch sec; seqNo:5590; 4057489.21 north; 591289.90 east 2021-02-12T23:38:42Z- AHRS_M2 msg: 1613173122.70 epoch sec; seqNo:5541 2021-02-12T23:38:42Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-12T23:38:42Z- handleDepth()-Depth_Keller msg: 1613173122708 epoch sec; seqNo:5591; depth 26.50 2021-02-12T23:38:42Z- handleDvl() - RDI_Pathfinder msg: 1613173122.74 epoch sec; seqNo:2673 2021-02-12T23:38:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 86.09 , 97.98 , 63.75 2021-02-12T23:38:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.09 , -0.04 2021-02-12T23:38:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173122.75 epoch sec; seqNo:5591; 4057489.59 north; 591289.89 east 2021-02-12T23:38:43Z- AHRS_M2 msg: 1613173123.07 epoch sec; seqNo:5542 2021-02-12T23:38:43Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-12T23:38:43Z- handleDepth()-Depth_Keller msg: 1613173123077 epoch sec; seqNo:5592; depth 26.38 2021-02-12T23:38:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173123.09 epoch sec; seqNo:5592; 4057489.59 north; 591289.89 east 2021-02-12T23:38:43Z- AHRS_M2 msg: 1613173123.47 epoch sec; seqNo:5543 2021-02-12T23:38:43Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-12T23:38:43Z- handleDepth()-Depth_Keller msg: 1613173123477 epoch sec; seqNo:5593; depth 26.31 2021-02-12T23:38:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173123.48 epoch sec; seqNo:5593; 4057489.97 north; 591289.89 east 2021-02-12T23:38:43Z- AHRS_M2 msg: 1613173123.88 epoch sec; seqNo:5544 2021-02-12T23:38:43Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-12T23:38:43Z- handleDepth()-Depth_Keller msg: 1613173123882 epoch sec; seqNo:5594; depth 26.21 2021-02-12T23:38:43Z- handleDvl() - RDI_Pathfinder msg: 1613173123.89 epoch sec; seqNo:2674 2021-02-12T23:38:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 87.16 , 97.98 , 63.35 2021-02-12T23:38:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , 0.00 2021-02-12T23:38:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173123.90 epoch sec; seqNo:5594; 4057490.35 north; 591289.89 east 2021-02-12T23:38:44Z- AHRS_M2 msg: 1613173124.28 epoch sec; seqNo:5545 2021-02-12T23:38:44Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-12T23:38:44Z- handleDepth()-Depth_Keller msg: 1613173124289 epoch sec; seqNo:5595; depth 26.09 2021-02-12T23:38:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173124.30 epoch sec; seqNo:5595; 4057490.35 north; 591289.89 east 2021-02-12T23:38:44Z- AHRS_M2 msg: 1613173124.70 epoch sec; seqNo:5546 2021-02-12T23:38:44Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-12T23:38:44Z- handleDepth()-Depth_Keller msg: 1613173124708 epoch sec; seqNo:5596; depth 25.98 2021-02-12T23:38:44Z- handleDvl() - RDI_Pathfinder msg: 1613173124.73 epoch sec; seqNo:2675 2021-02-12T23:38:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 88.25 , 97.98 , 64.57 2021-02-12T23:38:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , 0.04 2021-02-12T23:38:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173124.74 epoch sec; seqNo:5596; 4057490.73 north; 591289.88 east 2021-02-12T23:38:45Z- AHRS_M2 msg: 1613173125.09 epoch sec; seqNo:5547 2021-02-12T23:38:45Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-12T23:38:45Z- handleDepth()-Depth_Keller msg: 1613173125097 epoch sec; seqNo:5597; depth 25.85 2021-02-12T23:38:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173125.12 epoch sec; seqNo:5597; 4057491.12 north; 591291.10 east 2021-02-12T23:38:45Z- AHRS_M2 msg: 1613173125.49 epoch sec; seqNo:5548 2021-02-12T23:38:45Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-12T23:38:45Z- handleDepth()-Depth_Keller msg: 1613173125496 epoch sec; seqNo:5598; depth 25.77 2021-02-12T23:38:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173125.52 epoch sec; seqNo:5598; 4057491.50 north; 591291.10 east 2021-02-12T23:38:45Z- AHRS_M2 msg: 1613173125.89 epoch sec; seqNo:5549 2021-02-12T23:38:45Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-12T23:38:45Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-12T23:38:45Z- waiting for 1613173123.67, time is 1613173125.89 2021-02-12T23:38:45Z- LcmTrn::updateTrn() >>>> pose time 1613173125.89, meas time 1613173124.73 <<<< 2021-02-12T23:38:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173124.73 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:38:45Z- TerrainNav::motionUpdate() - currEstimate = (4057491, 591291.10, 25.77) 2021-02-12T23:38:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:38:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:38:45Z- TNavBF::Projecting Measurements 2021-02-12T23:38:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:38:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:38:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:38:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:38:45Z- TNavFilter::4 good beams. 2021-02-12T23:38:45Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:38:45Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:38:45Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:38:45Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:38:45Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:38:45Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:38:45Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:38:45Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:45Z- TerrainMapOctree: (vehN, vehE ) = (4057490.73, 591290.56). 2021-02-12T23:38:45Z- TerrainMapOctree: Min Distance = 4784.39. 2021-02-12T23:38:45Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:38:45Z- There is no specific good beam that all particles have in common. 2021-02-12T23:38:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:38:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 7.91 2021-02-12T23:38:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:38:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 12.46 2021-02-12T23:38:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:38:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-12T23:38:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:38:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 6.67 2021-02-12T23:38:45Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:38:45Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:38:45Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:38:45Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:38:45Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:38:45Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:38:45Z- Filter 0 No beams used in subcloud update 2021-02-12T23:38:45Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:38:45Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:38:45Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 747.6 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:38:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173124.73 sec. 2021-02-12T23:38:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:38:45Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:45Z- variances([0], [2], and psi_berg) = 747.575550 833.030820 0.000000 2021-02-12T23:38:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:38:45Z- LcmTrn::publishEstimates() - MLE : 4057479.00 591249.60 25.77 2021-02-12T23:38:45Z- LcmTrn::publishEstimates() - MMSE : 4057491.50 591291.10 25.77 2021-02-12T23:38:45Z- LcmTrn::publishEstimates() - COVAR: 747.58 833.03 0.76 0.00 2021-02-12T23:38:45Z- handleDepth()-Depth_Keller msg: 1613173125905 epoch sec; seqNo:5599; depth 25.67 2021-02-12T23:38:45Z- handleDvl() - RDI_Pathfinder msg: 1613173125.92 epoch sec; seqNo:2676 2021-02-12T23:38:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 87.10 , 97.92 , 63.75 2021-02-12T23:38:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.05 , 0.06 2021-02-12T23:38:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173125.93 epoch sec; seqNo:5599; 4057491.50 north; 591291.10 east 2021-02-12T23:38:46Z- AHRS_M2 msg: 1613173126.30 epoch sec; seqNo:5550 2021-02-12T23:38:46Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-12T23:38:46Z- handleDepth()-Depth_Keller msg: 1613173126304 epoch sec; seqNo:5600; depth 25.55 2021-02-12T23:38:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173126.32 epoch sec; seqNo:5600; 4057491.88 north; 591291.09 east 2021-02-12T23:38:46Z- AHRS_M2 msg: 1613173126.71 epoch sec; seqNo:5551 2021-02-12T23:38:46Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-12T23:38:46Z- handleDepth()-Depth_Keller msg: 1613173126714 epoch sec; seqNo:5601; depth 25.45 2021-02-12T23:38:46Z- handleDvl() - RDI_Pathfinder msg: 1613173126.73 epoch sec; seqNo:2677 2021-02-12T23:38:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 87.17 , 99.07 , 65.79 2021-02-12T23:38:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , 0.07 2021-02-12T23:38:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173126.74 epoch sec; seqNo:5601; 4057492.25 north; 591291.09 east 2021-02-12T23:38:47Z- AHRS_M2 msg: 1613173127.11 epoch sec; seqNo:5552 2021-02-12T23:38:47Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.18 psi 2021-02-12T23:38:47Z- handleDepth()-Depth_Keller msg: 1613173127117 epoch sec; seqNo:5602; depth 25.34 2021-02-12T23:38:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173127.14 epoch sec; seqNo:5602; 4057492.25 north; 591291.09 east 2021-02-12T23:38:47Z- AHRS_M2 msg: 1613173127.52 epoch sec; seqNo:5553 2021-02-12T23:38:47Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-12T23:38:47Z- handleDepth()-Depth_Keller msg: 1613173127520 epoch sec; seqNo:5603; depth 25.22 2021-02-12T23:38:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173127.53 epoch sec; seqNo:5603; 4057492.63 north; 591291.08 east 2021-02-12T23:38:47Z- AHRS_M2 msg: 1613173127.91 epoch sec; seqNo:5554 2021-02-12T23:38:47Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-12T23:38:47Z- handleDepth()-Depth_Keller msg: 1613173127923 epoch sec; seqNo:5604; depth 25.12 2021-02-12T23:38:47Z- handleDvl() - RDI_Pathfinder msg: 1613173127.93 epoch sec; seqNo:2678 2021-02-12T23:38:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 88.18 , 99.00 , 65.38 2021-02-12T23:38:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , 0.08 2021-02-12T23:38:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173127.94 epoch sec; seqNo:5604; 4057493.02 north; 591292.30 east 2021-02-12T23:38:48Z- AHRS_M2 msg: 1613173128.32 epoch sec; seqNo:5555 2021-02-12T23:38:48Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-12T23:38:48Z- handleDepth()-Depth_Keller msg: 1613173128324 epoch sec; seqNo:5605; depth 25.00 2021-02-12T23:38:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173128.34 epoch sec; seqNo:5605; 4057493.40 north; 591292.30 east 2021-02-12T23:38:48Z- AHRS_M2 msg: 1613173128.72 epoch sec; seqNo:5556 2021-02-12T23:38:48Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-12T23:38:48Z- handleDepth()-Depth_Keller msg: 1613173128728 epoch sec; seqNo:5606; depth 24.90 2021-02-12T23:38:48Z- handleDvl() - RDI_Pathfinder msg: 1613173128.74 epoch sec; seqNo:2679 2021-02-12T23:38:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 88.11 , 98.93 , 65.38 2021-02-12T23:38:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , 0.06 2021-02-12T23:38:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173128.75 epoch sec; seqNo:5606; 4057493.40 north; 591292.30 east 2021-02-12T23:38:49Z- AHRS_M2 msg: 1613173129.13 epoch sec; seqNo:5557 2021-02-12T23:38:49Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-12T23:38:49Z- handleDepth()-Depth_Keller msg: 1613173129137 epoch sec; seqNo:5607; depth 24.80 2021-02-12T23:38:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173129.15 epoch sec; seqNo:5607; 4057493.78 north; 591292.29 east 2021-02-12T23:38:49Z- AHRS_M2 msg: 1613173129.53 epoch sec; seqNo:5558 2021-02-12T23:38:49Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-12T23:38:49Z- handleDepth()-Depth_Keller msg: 1613173129541 epoch sec; seqNo:5608; depth 24.69 2021-02-12T23:38:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173129.55 epoch sec; seqNo:5608; 4057494.16 north; 591292.29 east 2021-02-12T23:38:49Z- AHRS_M2 msg: 1613173129.93 epoch sec; seqNo:5559 2021-02-12T23:38:49Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.15 psi 2021-02-12T23:38:49Z- handleDepth()-Depth_Keller msg: 1613173129940 epoch sec; seqNo:5609; depth 24.58 2021-02-12T23:38:49Z- handleDvl() - RDI_Pathfinder msg: 1613173129.95 epoch sec; seqNo:2680 2021-02-12T23:38:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.74 , 88.11 , 97.85 , 65.79 2021-02-12T23:38:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , 0.01 2021-02-12T23:38:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173129.96 epoch sec; seqNo:5609; 4057494.16 north; 591292.29 east 2021-02-12T23:38:50Z- AHRS_M2 msg: 1613173130.35 epoch sec; seqNo:5560 2021-02-12T23:38:50Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.15 psi 2021-02-12T23:38:50Z- handleDepth()-Depth_Keller msg: 1613173130352 epoch sec; seqNo:5610; depth 24.47 2021-02-12T23:38:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173130.36 epoch sec; seqNo:5610; 4057494.55 north; 591293.50 east 2021-02-12T23:38:50Z- AHRS_M2 msg: 1613173130.74 epoch sec; seqNo:5561 2021-02-12T23:38:50Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.14 psi 2021-02-12T23:38:50Z- handleDepth()-Depth_Keller msg: 1613173130748 epoch sec; seqNo:5611; depth 24.37 2021-02-12T23:38:50Z- handleDvl() - RDI_Pathfinder msg: 1613173130.76 epoch sec; seqNo:2681 2021-02-12T23:38:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 85.95 , 100.01 , 65.79 2021-02-12T23:38:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , -0.04 2021-02-12T23:38:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173130.79 epoch sec; seqNo:5611; 4057494.93 north; 591293.50 east 2021-02-12T23:38:50Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-12T23:38:50Z- waiting for 1613173128.89, time is 1613173130.79 2021-02-12T23:38:50Z- LcmTrn::updateTrn() equating pose 1613173130.79 and meas times 1613173130.79 2021-02-12T23:38:50Z- LcmTrn::updateTrn() >>>> pose time 1613173130.79, meas time 1613173130.79 <<<< 2021-02-12T23:38:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173130.79 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:38:50Z- TerrainNav::motionUpdate() - currEstimate = (4057495, 591293.50, 24.37) 2021-02-12T23:38:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:38:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:38:50Z- TNavBF::Projecting Measurements 2021-02-12T23:38:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:38:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:38:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:38:50Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:38:50Z- TNavFilter::3 good beams. 2021-02-12T23:38:50Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:38:50Z- TNavBF:: beamIndex[0] = 1 2021-02-12T23:38:50Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:38:50Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:38:50Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:38:50Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:38:50Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:38:51Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:51Z- TerrainMapOctree: (vehN, vehE ) = (4057494.93, 591293.50). 2021-02-12T23:38:51Z- TerrainMapOctree: Min Distance = 4779.27. 2021-02-12T23:38:51Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:38:51Z- There is no specific good beam that all particles have in common. 2021-02-12T23:38:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:38:51Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 11.82 2021-02-12T23:38:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:38:51Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 16.00 2021-02-12T23:38:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:38:51Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 6.93 2021-02-12T23:38:51Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:38:51Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:38:51Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:38:51Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:38:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:38:51Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:38:51Z- Filter 0 No beams used in subcloud update 2021-02-12T23:38:51Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:38:51Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:38:51Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 747.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:38:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173130.79 sec. 2021-02-12T23:38:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:38:51Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:51Z- variances([0], [2], and psi_berg) = 747.479019 833.030733 0.000000 2021-02-12T23:38:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:38:51Z- LcmTrn::publishEstimates() - MLE : 4057482.43 591252.00 24.37 2021-02-12T23:38:51Z- LcmTrn::publishEstimates() - MMSE : 4057494.93 591293.50 24.37 2021-02-12T23:38:51Z- LcmTrn::publishEstimates() - COVAR: 747.48 833.03 0.79 0.00 2021-02-12T23:38:51Z- AHRS_M2 msg: 1613173131.15 epoch sec; seqNo:5562 2021-02-12T23:38:51Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.14 psi 2021-02-12T23:38:51Z- handleDepth()-Depth_Keller msg: 1613173131154 epoch sec; seqNo:5612; depth 24.25 2021-02-12T23:38:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173131.16 epoch sec; seqNo:5612; 4057495.31 north; 591293.50 east 2021-02-12T23:38:51Z- AHRS_M2 msg: 1613173131.56 epoch sec; seqNo:5563 2021-02-12T23:38:51Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.14 psi 2021-02-12T23:38:51Z- handleDepth()-Depth_Keller msg: 1613173131563 epoch sec; seqNo:5613; depth 24.15 2021-02-12T23:38:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173131.57 epoch sec; seqNo:5613; 4057495.31 north; 591293.50 east 2021-02-12T23:38:51Z- AHRS_M2 msg: 1613173131.96 epoch sec; seqNo:5564 2021-02-12T23:38:51Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-12T23:38:51Z- handleDepth()-Depth_Keller msg: 1613173131966 epoch sec; seqNo:5614; depth 24.03 2021-02-12T23:38:51Z- handleDvl() - RDI_Pathfinder msg: 1613173131.98 epoch sec; seqNo:2682 2021-02-12T23:38:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 84.87 , 99.47 , 65.79 2021-02-12T23:38:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.06 2021-02-12T23:38:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173131.99 epoch sec; seqNo:5614; 4057495.69 north; 591293.49 east 2021-02-12T23:38:52Z- AHRS_M2 msg: 1613173132.36 epoch sec; seqNo:5565 2021-02-12T23:38:52Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-12T23:38:52Z- handleDepth()-Depth_Keller msg: 1613173132364 epoch sec; seqNo:5615; depth 23.92 2021-02-12T23:38:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173132.38 epoch sec; seqNo:5615; 4057496.07 north; 591293.49 east 2021-02-12T23:38:52Z- AHRS_M2 msg: 1613173132.76 epoch sec; seqNo:5566 2021-02-12T23:38:52Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-12T23:38:52Z- handleDepth()-Depth_Keller msg: 1613173132773 epoch sec; seqNo:5616; depth 23.82 2021-02-12T23:38:52Z- handleDvl() - RDI_Pathfinder msg: 1613173132.79 epoch sec; seqNo:2683 2021-02-12T23:38:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 0.00 , 97.85 , 66.61 2021-02-12T23:38:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , -0.07 2021-02-12T23:38:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173132.81 epoch sec; seqNo:5616; 4057496.46 north; 591294.71 east 2021-02-12T23:38:53Z- AHRS_M2 msg: 1613173133.17 epoch sec; seqNo:5567 2021-02-12T23:38:53Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-12T23:38:53Z- handleDepth()-Depth_Keller msg: 1613173133172 epoch sec; seqNo:5617; depth 23.70 2021-02-12T23:38:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173133.18 epoch sec; seqNo:5617; 4057496.46 north; 591294.71 east 2021-02-12T23:38:53Z- AHRS_M2 msg: 1613173133.57 epoch sec; seqNo:5568 2021-02-12T23:38:53Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-12T23:38:53Z- handleDepth()-Depth_Keller msg: 1613173133583 epoch sec; seqNo:5618; depth 23.61 2021-02-12T23:38:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173133.59 epoch sec; seqNo:5618; 4057496.84 north; 591294.70 east 2021-02-12T23:38:53Z- AHRS_M2 msg: 1613173133.97 epoch sec; seqNo:5569 2021-02-12T23:38:53Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-12T23:38:53Z- handleDepth()-Depth_Keller msg: 1613173133985 epoch sec; seqNo:5619; depth 23.50 2021-02-12T23:38:54Z- handleDvl() - RDI_Pathfinder msg: 1613173134.00 epoch sec; seqNo:2684 2021-02-12T23:38:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 87.16 , 99.06 , 65.80 2021-02-12T23:38:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.09 , -0.06 2021-02-12T23:38:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173134.01 epoch sec; seqNo:5619; 4057497.22 north; 591294.70 east 2021-02-12T23:38:54Z- AHRS_M2 msg: 1613173134.38 epoch sec; seqNo:5570 2021-02-12T23:38:54Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-12T23:38:54Z- handleDepth()-Depth_Keller msg: 1613173134384 epoch sec; seqNo:5620; depth 23.38 2021-02-12T23:38:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173134.39 epoch sec; seqNo:5620; 4057497.22 north; 591294.70 east 2021-02-12T23:38:54Z- AHRS_M2 msg: 1613173134.78 epoch sec; seqNo:5571 2021-02-12T23:38:54Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-12T23:38:54Z- handleDepth()-Depth_Keller msg: 1613173134788 epoch sec; seqNo:5621; depth 23.29 2021-02-12T23:38:54Z- handleDvl() - RDI_Pathfinder msg: 1613173134.80 epoch sec; seqNo:2685 2021-02-12T23:38:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 89.19 , 97.85 , 67.03 2021-02-12T23:38:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , -0.05 2021-02-12T23:38:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173134.81 epoch sec; seqNo:5621; 4057497.60 north; 591294.69 east 2021-02-12T23:38:55Z- AHRS_M2 msg: 1613173135.19 epoch sec; seqNo:5572 2021-02-12T23:38:55Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-12T23:38:55Z- handleDepth()-Depth_Keller msg: 1613173135193 epoch sec; seqNo:5622; depth 23.18 2021-02-12T23:38:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173135.20 epoch sec; seqNo:5622; 4057497.98 north; 591294.69 east 2021-02-12T23:38:55Z- AHRS_M2 msg: 1613173135.59 epoch sec; seqNo:5573 2021-02-12T23:38:55Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-12T23:38:55Z- handleDepth()-Depth_Keller msg: 1613173135606 epoch sec; seqNo:5623; depth 23.08 2021-02-12T23:38:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173135.62 epoch sec; seqNo:5623; 4057498.37 north; 591295.91 east 2021-02-12T23:38:56Z- AHRS_M2 msg: 1613173136.00 epoch sec; seqNo:5574 2021-02-12T23:38:56Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.15 psi 2021-02-12T23:38:56Z- handleDepth()-Depth_Keller msg: 1613173136008 epoch sec; seqNo:5624; depth 22.96 2021-02-12T23:38:56Z- handleDvl() - RDI_Pathfinder msg: 1613173136.03 epoch sec; seqNo:2686 2021-02-12T23:38:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 89.13 , 99.94 , 133.47 2021-02-12T23:38:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.10 , -0.06 2021-02-12T23:38:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173136.04 epoch sec; seqNo:5624; 4057498.75 north; 591295.90 east 2021-02-12T23:38:56Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-12T23:38:56Z- waiting for 1613173133.79, time is 1613173136.04 2021-02-12T23:38:56Z- LcmTrn::updateTrn() equating pose 1613173136.04 and meas times 1613173136.04 2021-02-12T23:38:56Z- LcmTrn::updateTrn() >>>> pose time 1613173136.04, meas time 1613173136.04 <<<< 2021-02-12T23:38:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173136.04 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:38:56Z- TerrainNav::motionUpdate() - currEstimate = (4057499, 591295.90, 22.96) 2021-02-12T23:38:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:38:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:38:56Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173136.04 sec.; No relevant navigation data available 2021-02-12T23:38:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:38:56Z- TNavBF::mmse selected from filter -1 2021-02-12T23:38:56Z- variances([0], [2], and psi_berg) = 747.379072 833.030643 0.000000 2021-02-12T23:38:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:38:56Z- LcmTrn::publishEstimates() - MLE : 4057486.25 591254.40 22.96 2021-02-12T23:38:56Z- LcmTrn::publishEstimates() - MMSE : 4057498.75 591295.90 22.96 2021-02-12T23:38:56Z- LcmTrn::publishEstimates() - COVAR: 747.38 833.03 1.02 0.00 2021-02-12T23:38:56Z- AHRS_M2 msg: 1613173136.41 epoch sec; seqNo:5575 2021-02-12T23:38:56Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.16 psi 2021-02-12T23:38:56Z- handleDepth()-Depth_Keller msg: 1613173136410 epoch sec; seqNo:5625; depth 22.87 2021-02-12T23:38:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173136.42 epoch sec; seqNo:5625; 4057498.75 north; 591295.90 east 2021-02-12T23:38:56Z- AHRS_M2 msg: 1613173136.83 epoch sec; seqNo:5576 2021-02-12T23:38:56Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.16 psi 2021-02-12T23:38:56Z- handleDepth()-Depth_Keller msg: 1613173136839 epoch sec; seqNo:5626; depth 22.76 2021-02-12T23:38:56Z- handleDvl() - RDI_Pathfinder msg: 1613173136.86 epoch sec; seqNo:2687 2021-02-12T23:38:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.11 , 90.28 , 101.09 , 67.02 2021-02-12T23:38:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.12 , -0.01 2021-02-12T23:38:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173136.87 epoch sec; seqNo:5626; 4057499.13 north; 591295.90 east 2021-02-12T23:38:57Z- AHRS_M2 msg: 1613173137.21 epoch sec; seqNo:5577 2021-02-12T23:38:57Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.16 psi 2021-02-12T23:38:57Z- handleDepth()-Depth_Keller msg: 1613173137212 epoch sec; seqNo:5627; depth 22.66 2021-02-12T23:38:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173137.23 epoch sec; seqNo:5627; 4057499.50 north; 591295.89 east 2021-02-12T23:38:57Z- AHRS_M2 msg: 1613173137.61 epoch sec; seqNo:5578 2021-02-12T23:38:57Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-12T23:38:57Z- handleDepth()-Depth_Keller msg: 1613173137625 epoch sec; seqNo:5628; depth 22.57 2021-02-12T23:38:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173137.66 epoch sec; seqNo:5628; 4057499.50 north; 591295.89 east 2021-02-12T23:38:58Z- AHRS_M2 msg: 1613173138.02 epoch sec; seqNo:5579 2021-02-12T23:38:58Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-12T23:38:58Z- handleDepth()-Depth_Keller msg: 1613173138025 epoch sec; seqNo:5629; depth 22.45 2021-02-12T23:38:58Z- handleDvl() - RDI_Pathfinder msg: 1613173138.04 epoch sec; seqNo:2688 2021-02-12T23:38:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 88.52 , 98.25 , 67.43 2021-02-12T23:38:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.11 , 0.04 2021-02-12T23:38:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173138.05 epoch sec; seqNo:5629; 4057499.88 north; 591295.89 east 2021-02-12T23:38:58Z- AHRS_M2 msg: 1613173138.42 epoch sec; seqNo:5580 2021-02-12T23:38:58Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.17 psi 2021-02-12T23:38:58Z- handleDepth()-Depth_Keller msg: 1613173138425 epoch sec; seqNo:5630; depth 22.34 2021-02-12T23:38:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173138.44 epoch sec; seqNo:5630; 4057500.26 north; 591295.89 east 2021-02-12T23:38:58Z- AHRS_M2 msg: 1613173138.82 epoch sec; seqNo:5581 2021-02-12T23:38:58Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:38:58Z- handleDepth()-Depth_Keller msg: 1613173138832 epoch sec; seqNo:5631; depth 22.26 2021-02-12T23:38:58Z- handleDvl() - RDI_Pathfinder msg: 1613173138.86 epoch sec; seqNo:2689 2021-02-12T23:38:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 97.78 , 98.86 , 67.43 2021-02-12T23:38:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.15 , 0.14 2021-02-12T23:38:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173138.87 epoch sec; seqNo:5631; 4057500.65 north; 591297.10 east 2021-02-12T23:38:59Z- AHRS_M2 msg: 1613173139.27 epoch sec; seqNo:5582 2021-02-12T23:38:59Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:38:59Z- handleDepth()-Depth_Keller msg: 1613173139270 epoch sec; seqNo:5632; depth 22.15 2021-02-12T23:38:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173139.28 epoch sec; seqNo:5632; 4057500.65 north; 591297.10 east 2021-02-12T23:38:59Z- AHRS_M2 msg: 1613173139.63 epoch sec; seqNo:5583 2021-02-12T23:38:59Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-12T23:38:59Z- handleDepth()-Depth_Keller msg: 1613173139646 epoch sec; seqNo:5633; depth 22.04 2021-02-12T23:38:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173139.68 epoch sec; seqNo:5633; 4057501.03 north; 591297.10 east 2021-02-12T23:39:00Z- AHRS_M2 msg: 1613173140.04 epoch sec; seqNo:5584 2021-02-12T23:39:00Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-12T23:39:00Z- handleDepth()-Depth_Keller msg: 1613173140042 epoch sec; seqNo:5634; depth 21.95 2021-02-12T23:39:00Z- handleDvl() - RDI_Pathfinder msg: 1613173140.09 epoch sec; seqNo:2690 2021-02-12T23:39:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 88.52 , 99.47 , 68.65 2021-02-12T23:39:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , 0.17 2021-02-12T23:39:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173140.10 epoch sec; seqNo:5634; 4057501.41 north; 591297.09 east 2021-02-12T23:39:00Z- AHRS_M2 msg: 1613173140.44 epoch sec; seqNo:5585 2021-02-12T23:39:00Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-12T23:39:00Z- handleDepth()-Depth_Keller msg: 1613173140444 epoch sec; seqNo:5635; depth 21.84 2021-02-12T23:39:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173140.45 epoch sec; seqNo:5635; 4057501.41 north; 591297.09 east 2021-02-12T23:39:00Z- AHRS_M2 msg: 1613173140.84 epoch sec; seqNo:5586 2021-02-12T23:39:00Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-12T23:39:00Z- handleDepth()-Depth_Keller msg: 1613173140848 epoch sec; seqNo:5636; depth 21.75 2021-02-12T23:39:00Z- handleDvl() - RDI_Pathfinder msg: 1613173140.86 epoch sec; seqNo:2691 2021-02-12T23:39:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 0.00 , 99.61 , 68.24 2021-02-12T23:39:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.06 , 0.14 2021-02-12T23:39:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173140.87 epoch sec; seqNo:5636; 4057501.79 north; 591297.09 east 2021-02-12T23:39:01Z- LcmTrn::handleMessages() - handling 41 messages took 0.73 ms and 127 calls... 2021-02-12T23:39:01Z- waiting for 1613173139.04, time is 1613173140.87 2021-02-12T23:39:01Z- LcmTrn::updateTrn() equating pose 1613173140.87 and meas times 1613173140.87 2021-02-12T23:39:01Z- LcmTrn::updateTrn() >>>> pose time 1613173140.87, meas time 1613173140.87 <<<< 2021-02-12T23:39:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173140.87 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:39:01Z- TerrainNav::motionUpdate() - currEstimate = (4057502, 591297.09, 21.75) 2021-02-12T23:39:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:39:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:39:01Z- TNavBF::Projecting Measurements 2021-02-12T23:39:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:39:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:39:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:39:01Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:39:01Z- TNavFilter::3 good beams. 2021-02-12T23:39:01Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:39:01Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:39:01Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:39:01Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:39:01Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:39:01Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:39:01Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:39:01Z- TNavBF::mmse selected from filter -1 2021-02-12T23:39:01Z- TerrainMapOctree: (vehN, vehE ) = (4057501.79, 591297.09). 2021-02-12T23:39:01Z- TerrainMapOctree: Min Distance = 4771.66. 2021-02-12T23:39:01Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:39:01Z- There is no specific good beam that all particles have in common. 2021-02-12T23:39:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:39:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 9.97 2021-02-12T23:39:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:39:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 15.87 2021-02-12T23:39:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:39:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-12T23:39:01Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:39:01Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:39:01Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:39:01Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:39:01Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:39:01Z- Filter 0 No beams used in subcloud update 2021-02-12T23:39:01Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:39:01Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:39:01Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 747.3 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:39:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173140.87 sec. 2021-02-12T23:39:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:39:01Z- TNavBF::mmse selected from filter -1 2021-02-12T23:39:01Z- variances([0], [2], and psi_berg) = 747.294086 833.030567 0.000000 2021-02-12T23:39:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:39:01Z- LcmTrn::publishEstimates() - MLE : 4057489.29 591255.59 21.75 2021-02-12T23:39:01Z- LcmTrn::publishEstimates() - MMSE : 4057501.79 591297.09 21.75 2021-02-12T23:39:01Z- LcmTrn::publishEstimates() - COVAR: 747.29 833.03 0.91 0.00 2021-02-12T23:39:01Z- AHRS_M2 msg: 1613173141.25 epoch sec; seqNo:5587 2021-02-12T23:39:01Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-12T23:39:01Z- handleDepth()-Depth_Keller msg: 1613173141253 epoch sec; seqNo:5637; depth 21.64 2021-02-12T23:39:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173141.27 epoch sec; seqNo:5637; 4057502.17 north; 591297.09 east 2021-02-12T23:39:01Z- AHRS_M2 msg: 1613173141.65 epoch sec; seqNo:5588 2021-02-12T23:39:01Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:39:01Z- handleDepth()-Depth_Keller msg: 1613173141666 epoch sec; seqNo:5638; depth 21.53 2021-02-12T23:39:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173141.67 epoch sec; seqNo:5638; 4057502.56 north; 591298.30 east 2021-02-12T23:39:02Z- AHRS_M2 msg: 1613173142.06 epoch sec; seqNo:5589 2021-02-12T23:39:02Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:39:02Z- handleDepth()-Depth_Keller msg: 1613173142061 epoch sec; seqNo:5639; depth 21.44 2021-02-12T23:39:02Z- handleDvl() - RDI_Pathfinder msg: 1613173142.07 epoch sec; seqNo:2692 2021-02-12T23:39:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 92.37 , 99.94 , 78.31 2021-02-12T23:39:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.13 , 0.03 2021-02-12T23:39:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173142.10 epoch sec; seqNo:5639; 4057502.56 north; 591298.30 east 2021-02-12T23:39:02Z- AHRS_M2 msg: 1613173142.46 epoch sec; seqNo:5590 2021-02-12T23:39:02Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-12T23:39:02Z- handleDepth()-Depth_Keller msg: 1613173142469 epoch sec; seqNo:5640; depth 21.33 2021-02-12T23:39:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173142.48 epoch sec; seqNo:5640; 4057502.94 north; 591298.30 east 2021-02-12T23:39:02Z- AHRS_M2 msg: 1613173142.86 epoch sec; seqNo:5591 2021-02-12T23:39:02Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-12T23:39:02Z- handleDepth()-Depth_Keller msg: 1613173142878 epoch sec; seqNo:5641; depth 21.23 2021-02-12T23:39:02Z- handleDvl() - RDI_Pathfinder msg: 1613173142.89 epoch sec; seqNo:2693 2021-02-12T23:39:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.60 , 0.00 , 102.99 , 69.06 2021-02-12T23:39:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.12 , -0.10 2021-02-12T23:39:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173142.90 epoch sec; seqNo:5641; 4057503.32 north; 591298.30 east 2021-02-12T23:39:03Z- AHRS_M2 msg: 1613173143.27 epoch sec; seqNo:5592 2021-02-12T23:39:03Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-12T23:39:03Z- handleDepth()-Depth_Keller msg: 1613173143272 epoch sec; seqNo:5642; depth 21.13 2021-02-12T23:39:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173143.28 epoch sec; seqNo:5642; 4057503.32 north; 591298.30 east 2021-02-12T23:39:03Z- AHRS_M2 msg: 1613173143.68 epoch sec; seqNo:5593 2021-02-12T23:39:03Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.16 psi 2021-02-12T23:39:03Z- handleDepth()-Depth_Keller msg: 1613173143686 epoch sec; seqNo:5643; depth 21.03 2021-02-12T23:39:03Z- handleDvl() - RDI_Pathfinder msg: 1613173143.70 epoch sec; seqNo:2694 2021-02-12T23:39:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.02 , 98.39 , 100.82 , 69.07 2021-02-12T23:39:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , -0.16 2021-02-12T23:39:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173143.71 epoch sec; seqNo:5643; 4057503.71 north; 591299.51 east 2021-02-12T23:39:04Z- AHRS_M2 msg: 1613173144.08 epoch sec; seqNo:5594 2021-02-12T23:39:04Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.16 psi 2021-02-12T23:39:04Z- handleDepth()-Depth_Keller msg: 1613173144090 epoch sec; seqNo:5644; depth 20.91 2021-02-12T23:39:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173144.10 epoch sec; seqNo:5644; 4057504.09 north; 591299.51 east 2021-02-12T23:39:04Z- AHRS_M2 msg: 1613173144.48 epoch sec; seqNo:5595 2021-02-12T23:39:04Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.16 psi 2021-02-12T23:39:04Z- handleDepth()-Depth_Keller msg: 1613173144484 epoch sec; seqNo:5645; depth 20.83 2021-02-12T23:39:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173144.49 epoch sec; seqNo:5645; 4057504.09 north; 591299.51 east 2021-02-12T23:39:04Z- AHRS_M2 msg: 1613173144.88 epoch sec; seqNo:5596 2021-02-12T23:39:04Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.16 psi 2021-02-12T23:39:04Z- handleDepth()-Depth_Keller msg: 1613173144888 epoch sec; seqNo:5646; depth 20.72 2021-02-12T23:39:04Z- handleDvl() - RDI_Pathfinder msg: 1613173144.90 epoch sec; seqNo:2695 2021-02-12T23:39:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 98.12 , 101.77 , 68.66 2021-02-12T23:39:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , -0.14 2021-02-12T23:39:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173144.91 epoch sec; seqNo:5646; 4057504.47 north; 591299.50 east 2021-02-12T23:39:05Z- AHRS_M2 msg: 1613173145.29 epoch sec; seqNo:5597 2021-02-12T23:39:05Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.16 psi 2021-02-12T23:39:05Z- handleDepth()-Depth_Keller msg: 1613173145293 epoch sec; seqNo:5647; depth 20.61 2021-02-12T23:39:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173145.31 epoch sec; seqNo:5647; 4057504.85 north; 591299.50 east 2021-02-12T23:39:05Z- AHRS_M2 msg: 1613173145.69 epoch sec; seqNo:5598 2021-02-12T23:39:05Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-12T23:39:05Z- handleDepth()-Depth_Keller msg: 1613173145697 epoch sec; seqNo:5648; depth 20.50 2021-02-12T23:39:05Z- handleDvl() - RDI_Pathfinder msg: 1613173145.71 epoch sec; seqNo:2696 2021-02-12T23:39:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.02 , 90.81 , 101.77 , 69.07 2021-02-12T23:39:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.15 , -0.02 2021-02-12T23:39:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173145.73 epoch sec; seqNo:5648; 4057504.85 north; 591299.50 east 2021-02-12T23:39:06Z- AHRS_M2 msg: 1613173146.10 epoch sec; seqNo:5599 2021-02-12T23:39:06Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-12T23:39:06Z- handleDepth()-Depth_Keller msg: 1613173146100 epoch sec; seqNo:5649; depth 20.41 2021-02-12T23:39:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173146.11 epoch sec; seqNo:5649; 4057505.23 north; 591299.50 east 2021-02-12T23:39:06Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-12T23:39:06Z- waiting for 1613173143.87, time is 1613173146.11 2021-02-12T23:39:06Z- LcmTrn::updateTrn() >>>> pose time 1613173146.11, meas time 1613173145.71 <<<< 2021-02-12T23:39:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173145.71 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:39:06Z- TerrainNav::motionUpdate() - currEstimate = (4057505, 591299.50, 20.41) 2021-02-12T23:39:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:39:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:39:06Z- TNavBF::Projecting Measurements 2021-02-12T23:39:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:39:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:39:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:39:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:39:06Z- TNavFilter::4 good beams. 2021-02-12T23:39:06Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:39:06Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:39:06Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:39:06Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:39:06Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:39:06Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:39:06Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:39:06Z- TNavBF::mmse selected from filter -1 2021-02-12T23:39:06Z- TerrainMapOctree: (vehN, vehE ) = (4057504.96, 591299.31). 2021-02-12T23:39:06Z- TerrainMapOctree: Min Distance = 4767.79. 2021-02-12T23:39:06Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:39:06Z- There is no specific good beam that all particles have in common. 2021-02-12T23:39:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:39:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 9.25 2021-02-12T23:39:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:39:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 13.20 2021-02-12T23:39:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:39:06Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 16.57 2021-02-12T23:39:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:39:06Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-12T23:39:06Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:39:06Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:39:06Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:39:06Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:39:06Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:39:06Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:39:06Z- Filter 0 No beams used in subcloud update 2021-02-12T23:39:06Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:39:06Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:39:06Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 747.2 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:39:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173145.71 sec. 2021-02-12T23:39:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:39:06Z- TNavBF::mmse selected from filter -1 2021-02-12T23:39:06Z- variances([0], [2], and psi_berg) = 747.175068 833.030460 0.000000 2021-02-12T23:39:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:39:06Z- LcmTrn::publishEstimates() - MLE : 4057492.73 591258.00 20.41 2021-02-12T23:39:06Z- LcmTrn::publishEstimates() - MMSE : 4057505.23 591299.50 20.41 2021-02-12T23:39:06Z- LcmTrn::publishEstimates() - COVAR: 747.18 833.03 0.84 0.00 2021-02-12T23:39:06Z- AHRS_M2 msg: 1613173146.50 epoch sec; seqNo:5600 2021-02-12T23:39:06Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-12T23:39:06Z- handleDepth()-Depth_Keller msg: 1613173146506 epoch sec; seqNo:5650; depth 20.29 2021-02-12T23:39:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173146.51 epoch sec; seqNo:5650; 4057505.62 north; 591300.71 east 2021-02-12T23:39:06Z- AHRS_M2 msg: 1613173146.90 epoch sec; seqNo:5601 2021-02-12T23:39:06Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:39:06Z- handleDepth()-Depth_Keller msg: 1613173146908 epoch sec; seqNo:5651; depth 20.19 2021-02-12T23:39:06Z- handleDvl() - RDI_Pathfinder msg: 1613173146.92 epoch sec; seqNo:2697 2021-02-12T23:39:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.78 , 92.17 , 101.90 , 69.07 2021-02-12T23:39:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.09 , 0.06 2021-02-12T23:39:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173146.94 epoch sec; seqNo:5651; 4057506.00 north; 591300.71 east 2021-02-12T23:39:07Z- AHRS_M2 msg: 1613173147.31 epoch sec; seqNo:5602 2021-02-12T23:39:07Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:39:07Z- handleDepth()-Depth_Keller msg: 1613173147317 epoch sec; seqNo:5652; depth 20.09 2021-02-12T23:39:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173147.33 epoch sec; seqNo:5652; 4057506.00 north; 591300.71 east 2021-02-12T23:39:07Z- AHRS_M2 msg: 1613173147.71 epoch sec; seqNo:5603 2021-02-12T23:39:07Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:39:07Z- handleDepth()-Depth_Keller msg: 1613173147717 epoch sec; seqNo:5653; depth 19.99 2021-02-12T23:39:07Z- handleDvl() - RDI_Pathfinder msg: 1613173147.73 epoch sec; seqNo:2698 2021-02-12T23:39:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 98.12 , 101.77 , 70.29 2021-02-12T23:39:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , 0.07 2021-02-12T23:39:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173147.74 epoch sec; seqNo:5653; 4057506.38 north; 591300.71 east 2021-02-12T23:39:08Z- AHRS_M2 msg: 1613173148.12 epoch sec; seqNo:5604 2021-02-12T23:39:08Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-12T23:39:08Z- handleDepth()-Depth_Keller msg: 1613173148122 epoch sec; seqNo:5654; depth 19.88 2021-02-12T23:39:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173148.14 epoch sec; seqNo:5654; 4057506.75 north; 591300.70 east 2021-02-12T23:39:08Z- AHRS_M2 msg: 1613173148.52 epoch sec; seqNo:5605 2021-02-12T23:39:08Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-12T23:39:08Z- handleDepth()-Depth_Keller msg: 1613173148524 epoch sec; seqNo:5655; depth 19.77 2021-02-12T23:39:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173148.53 epoch sec; seqNo:5655; 4057507.13 north; 591300.70 east 2021-02-12T23:39:08Z- AHRS_M2 msg: 1613173148.92 epoch sec; seqNo:5606 2021-02-12T23:39:08Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-12T23:39:08Z- handleDepth()-Depth_Keller msg: 1613173148931 epoch sec; seqNo:5656; depth 19.67 2021-02-12T23:39:08Z- handleDvl() - RDI_Pathfinder msg: 1613173148.94 epoch sec; seqNo:2699 2021-02-12T23:39:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 90.75 , 102.92 , 70.29 2021-02-12T23:39:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.05 , 0.01 2021-02-12T23:39:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173148.95 epoch sec; seqNo:5656; 4057507.13 north; 591300.70 east 2021-02-12T23:39:09Z- AHRS_M2 msg: 1613173149.33 epoch sec; seqNo:5607 2021-02-12T23:39:09Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-12T23:39:09Z- handleDepth()-Depth_Keller msg: 1613173149334 epoch sec; seqNo:5657; depth 19.55 2021-02-12T23:39:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173149.35 epoch sec; seqNo:5657; 4057507.52 north; 591301.91 east 2021-02-12T23:39:09Z- AHRS_M2 msg: 1613173149.75 epoch sec; seqNo:5608 2021-02-12T23:39:09Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-12T23:39:09Z- handleDepth()-Depth_Keller msg: 1613173149753 epoch sec; seqNo:5658; depth 19.46 2021-02-12T23:39:09Z- handleDvl() - RDI_Pathfinder msg: 1613173149.77 epoch sec; seqNo:2700 2021-02-12T23:39:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 92.10 , 104.27 , 70.70 2021-02-12T23:39:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.04 , -0.05 2021-02-12T23:39:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173149.78 epoch sec; seqNo:5658; 4057507.90 north; 591301.91 east 2021-02-12T23:39:10Z- AHRS_M2 msg: 1613173150.14 epoch sec; seqNo:5609 2021-02-12T23:39:10Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-12T23:39:10Z- handleDepth()-Depth_Keller msg: 1613173150149 epoch sec; seqNo:5659; depth 19.34 2021-02-12T23:39:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173150.16 epoch sec; seqNo:5659; 4057507.90 north; 591301.91 east 2021-02-12T23:39:10Z- AHRS_M2 msg: 1613173150.54 epoch sec; seqNo:5610 2021-02-12T23:39:10Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.16 psi 2021-02-12T23:39:10Z- handleDepth()-Depth_Keller msg: 1613173150544 epoch sec; seqNo:5660; depth 19.23 2021-02-12T23:39:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173150.55 epoch sec; seqNo:5660; 4057508.28 north; 591301.91 east 2021-02-12T23:39:10Z- AHRS_M2 msg: 1613173150.94 epoch sec; seqNo:5611 2021-02-12T23:39:10Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.16 psi 2021-02-12T23:39:10Z- handleDepth()-Depth_Keller msg: 1613173150953 epoch sec; seqNo:5661; depth 19.13 2021-02-12T23:39:10Z- handleDvl() - RDI_Pathfinder msg: 1613173150.96 epoch sec; seqNo:2701 2021-02-12T23:39:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.64 , 94.40 , 104.14 , 69.06 2021-02-12T23:39:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , -0.05 2021-02-12T23:39:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173150.97 epoch sec; seqNo:5661; 4057508.66 north; 591301.90 east 2021-02-12T23:39:11Z- LcmTrn::handleMessages() - handling 41 messages took 0.58 ms and 130 calls... 2021-02-12T23:39:11Z- waiting for 1613173149.11, time is 1613173150.97 2021-02-12T23:39:11Z- LcmTrn::updateTrn() equating pose 1613173150.97 and meas times 1613173150.97 2021-02-12T23:39:11Z- LcmTrn::updateTrn() >>>> pose time 1613173150.97, meas time 1613173150.97 <<<< 2021-02-12T23:39:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173150.97 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:39:11Z- TerrainNav::motionUpdate() - currEstimate = (4057509, 591301.90, 19.13) 2021-02-12T23:39:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:39:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:39:11Z- TNavBF::Projecting Measurements 2021-02-12T23:39:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:39:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:39:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:39:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:39:11Z- TNavFilter::4 good beams. 2021-02-12T23:39:11Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:39:11Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:39:11Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:39:11Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:39:11Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:39:11Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:39:11Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:39:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:39:11Z- TerrainMapOctree: (vehN, vehE ) = (4057508.66, 591301.90). 2021-02-12T23:39:11Z- TerrainMapOctree: Min Distance = 4763.29. 2021-02-12T23:39:11Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:39:11Z- There is no specific good beam that all particles have in common. 2021-02-12T23:39:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:39:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 9.65 2021-02-12T23:39:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:39:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 14.26 2021-02-12T23:39:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:39:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 17.35 2021-02-12T23:39:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:39:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-12T23:39:11Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:39:11Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:39:11Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:39:11Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:39:11Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:39:11Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:39:11Z- Filter 0 No beams used in subcloud update 2021-02-12T23:39:11Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:39:11Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:39:11Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 747.1 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:39:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173150.97 sec. 2021-02-12T23:39:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:39:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:39:11Z- variances([0], [2], and psi_berg) = 747.079040 833.030374 0.000000 2021-02-12T23:39:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:39:11Z- LcmTrn::publishEstimates() - MLE : 4057496.16 591260.40 19.13 2021-02-12T23:39:11Z- LcmTrn::publishEstimates() - MMSE : 4057508.66 591301.90 19.13 2021-02-12T23:39:11Z- LcmTrn::publishEstimates() - COVAR: 747.08 833.03 0.79 0.00 2021-02-12T23:39:11Z- AHRS_M2 msg: 1613173151.35 epoch sec; seqNo:5612 2021-02-12T23:39:11Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.16 psi 2021-02-12T23:39:11Z- handleDepth()-Depth_Keller msg: 1613173151353 epoch sec; seqNo:5662; depth 19.01 2021-02-12T23:39:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173151.37 epoch sec; seqNo:5662; 4057508.66 north; 591301.90 east 2021-02-12T23:39:11Z- AHRS_M2 msg: 1613173151.76 epoch sec; seqNo:5613 2021-02-12T23:39:11Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.17 psi 2021-02-12T23:39:11Z- handleDepth()-Depth_Keller msg: 1613173151762 epoch sec; seqNo:5663; depth 18.91 2021-02-12T23:39:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173151.81 epoch sec; seqNo:5663; 4057509.05 north; 591303.12 east 2021-02-12T23:39:12Z- AHRS_M2 msg: 1613173152.16 epoch sec; seqNo:5614 2021-02-12T23:39:12Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:39:12Z- handleDepth()-Depth_Keller msg: 1613173152165 epoch sec; seqNo:5664; depth 18.78 2021-02-12T23:39:12Z- handleDvl() - RDI_Pathfinder msg: 1613173152.18 epoch sec; seqNo:2702 2021-02-12T23:39:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 94.40 , 104.13 , 70.29 2021-02-12T23:39:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.12 , 0.01 2021-02-12T23:39:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173152.20 epoch sec; seqNo:5664; 4057509.43 north; 591303.11 east 2021-02-12T23:39:12Z- AHRS_M2 msg: 1613173152.56 epoch sec; seqNo:5615 2021-02-12T23:39:12Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.18 psi 2021-02-12T23:39:12Z- handleDepth()-Depth_Keller msg: 1613173152573 epoch sec; seqNo:5665; depth 18.67 2021-02-12T23:39:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173152.59 epoch sec; seqNo:5665; 4057509.81 north; 591303.11 east 2021-02-12T23:39:12Z- AHRS_M2 msg: 1613173152.96 epoch sec; seqNo:5616 2021-02-12T23:39:12Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-12T23:39:12Z- handleDepth()-Depth_Keller msg: 1613173152968 epoch sec; seqNo:5666; depth 18.56 2021-02-12T23:39:12Z- handleDvl() - RDI_Pathfinder msg: 1613173152.99 epoch sec; seqNo:2703 2021-02-12T23:39:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 93.18 , 105.35 , 71.11 2021-02-12T23:39:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.10 , 0.09 2021-02-12T23:39:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173153.01 epoch sec; seqNo:5666; 4057509.81 north; 591303.11 east 2021-02-12T23:39:13Z- AHRS_M2 msg: 1613173153.37 epoch sec; seqNo:5617 2021-02-12T23:39:13Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.18 psi 2021-02-12T23:39:13Z- handleDepth()-Depth_Keller msg: 1613173153376 epoch sec; seqNo:5667; depth 18.45 2021-02-12T23:39:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173153.39 epoch sec; seqNo:5667; 4057510.19 north; 591303.11 east 2021-02-12T23:39:13Z- AHRS_M2 msg: 1613173153.79 epoch sec; seqNo:5618 2021-02-12T23:39:13Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.19 psi 2021-02-12T23:39:13Z- handleDepth()-Depth_Keller msg: 1613173153811 epoch sec; seqNo:5668; depth 18.36 2021-02-12T23:39:13Z- handleDvl() - RDI_Pathfinder msg: 1613173153.82 epoch sec; seqNo:2704 2021-02-12T23:39:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 94.40 , 107.79 , 71.11 2021-02-12T23:39:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.13 2021-02-12T23:39:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173153.83 epoch sec; seqNo:5668; 4057510.57 north; 591303.10 east 2021-02-12T23:39:14Z- AHRS_M2 msg: 1613173154.18 epoch sec; seqNo:5619 2021-02-12T23:39:14Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.19 psi 2021-02-12T23:39:14Z- handleDepth()-Depth_Keller msg: 1613173154182 epoch sec; seqNo:5669; depth 18.22 2021-02-12T23:39:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173154.19 epoch sec; seqNo:5669; 4057510.95 north; 591303.10 east 2021-02-12T23:39:14Z- AHRS_M2 msg: 1613173154.58 epoch sec; seqNo:5620 2021-02-12T23:39:14Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.19 psi 2021-02-12T23:39:14Z- handleDepth()-Depth_Keller msg: 1613173154584 epoch sec; seqNo:5670; depth 18.12 2021-02-12T23:39:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173154.60 epoch sec; seqNo:5670; 4057510.96 north; 591304.32 east 2021-02-12T23:39:14Z- AHRS_M2 msg: 1613173154.99 epoch sec; seqNo:5621 2021-02-12T23:39:14Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.19 psi 2021-02-12T23:39:14Z- handleDepth()-Depth_Keller msg: 1613173154995 epoch sec; seqNo:5671; depth 18.00 2021-02-12T23:39:15Z- handleDvl() - RDI_Pathfinder msg: 1613173155.03 epoch sec; seqNo:2705 2021-02-12T23:39:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.64 , 95.48 , 106.43 , 72.33 2021-02-12T23:39:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , 0.11 2021-02-12T23:39:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173155.04 epoch sec; seqNo:5671; 4057511.34 north; 591304.32 east 2021-02-12T23:39:15Z- AHRS_M2 msg: 1613173155.39 epoch sec; seqNo:5622 2021-02-12T23:39:15Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.19 psi 2021-02-12T23:39:15Z- handleDepth()-Depth_Keller msg: 1613173155392 epoch sec; seqNo:5672; depth 17.89 2021-02-12T23:39:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173155.40 epoch sec; seqNo:5672; 4057511.72 north; 591304.31 east 2021-02-12T23:39:15Z- AHRS_M2 msg: 1613173155.79 epoch sec; seqNo:5623 2021-02-12T23:39:15Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.19 psi 2021-02-12T23:39:15Z- handleDepth()-Depth_Keller msg: 1613173155797 epoch sec; seqNo:5673; depth 17.80 2021-02-12T23:39:15Z- handleDvl() - RDI_Pathfinder msg: 1613173155.81 epoch sec; seqNo:2706 2021-02-12T23:39:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.46 , 97.92 , 108.87 , 71.51 2021-02-12T23:39:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.08 , 0.04 2021-02-12T23:39:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173155.82 epoch sec; seqNo:5673; 4057512.10 north; 591304.31 east 2021-02-12T23:39:16Z- AHRS_M2 msg: 1613173156.19 epoch sec; seqNo:5624 2021-02-12T23:39:16Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.19 psi 2021-02-12T23:39:16Z- handleDepth()-Depth_Keller msg: 1613173156210 epoch sec; seqNo:5674; depth 17.67 2021-02-12T23:39:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173156.22 epoch sec; seqNo:5674; 4057512.10 north; 591304.31 east 2021-02-12T23:39:16Z- LcmTrn::handleMessages() - handling 44 messages took 0.76 ms and 129 calls... 2021-02-12T23:39:16Z- waiting for 1613173153.97, time is 1613173156.22 2021-02-12T23:39:16Z- LcmTrn::updateTrn() >>>> pose time 1613173156.22, meas time 1613173155.81 <<<< 2021-02-12T23:39:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173155.81 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:39:16Z- TerrainNav::motionUpdate() - currEstimate = (4057512, 591304.31, 17.67) 2021-02-12T23:39:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:39:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:39:16Z- TNavBF::Projecting Measurements 2021-02-12T23:39:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:39:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:39:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:39:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:39:16Z- TNavFilter::4 good beams. 2021-02-12T23:39:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:39:16Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:39:16Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:39:16Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:39:16Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:39:16Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:39:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:39:16Z- TNavBF::mmse selected from filter -1 2021-02-12T23:39:16Z- TerrainMapOctree: (vehN, vehE ) = (4057511.83, 591304.12). 2021-02-12T23:39:16Z- TerrainMapOctree: Min Distance = 4759.43. 2021-02-12T23:39:16Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:39:16Z- There is no specific good beam that all particles have in common. 2021-02-12T23:39:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:39:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-12T23:39:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:39:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-12T23:39:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:39:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 18.96 2021-02-12T23:39:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:39:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 8.18 2021-02-12T23:39:16Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:39:16Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:39:16Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:39:16Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:39:16Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:39:16Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:39:16Z- Filter 0 No beams used in subcloud update 2021-02-12T23:39:16Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:39:16Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:39:16Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 747.0 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:39:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173155.81 sec. 2021-02-12T23:39:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:39:16Z- TNavBF::mmse selected from filter -1 2021-02-12T23:39:16Z- variances([0], [2], and psi_berg) = 746.960132 833.030268 0.000000 2021-02-12T23:39:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:39:16Z- LcmTrn::publishEstimates() - MLE : 4057499.60 591262.81 17.67 2021-02-12T23:39:16Z- LcmTrn::publishEstimates() - MMSE : 4057512.10 591304.31 17.67 2021-02-12T23:39:16Z- LcmTrn::publishEstimates() - COVAR: 746.96 833.03 0.78 0.00 2021-02-12T23:39:16Z- AHRS_M2 msg: 1613173156.60 epoch sec; seqNo:5625 2021-02-12T23:39:16Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.19 psi 2021-02-12T23:39:16Z- handleDepth()-Depth_Keller msg: 1613173156604 epoch sec; seqNo:5675; depth 17.57 2021-02-12T23:39:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173156.61 epoch sec; seqNo:5675; 4057512.48 north; 591304.30 east 2021-02-12T23:39:17Z- AHRS_M2 msg: 1613173157.00 epoch sec; seqNo:5626 2021-02-12T23:39:17Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.19 psi 2021-02-12T23:39:17Z- handleDepth()-Depth_Keller msg: 1613173157008 epoch sec; seqNo:5676; depth 17.46 2021-02-12T23:39:17Z- handleDvl() - RDI_Pathfinder msg: 1613173157.02 epoch sec; seqNo:2707 2021-02-12T23:39:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.60 , 97.85 , 110.02 , 72.33 2021-02-12T23:39:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.08 , -0.05 2021-02-12T23:39:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173157.03 epoch sec; seqNo:5676; 4057512.87 north; 591305.52 east 2021-02-12T23:39:17Z- AHRS_M2 msg: 1613173157.43 epoch sec; seqNo:5627 2021-02-12T23:39:17Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.19 psi 2021-02-12T23:39:17Z- handleDepth()-Depth_Keller msg: 1613173157431 epoch sec; seqNo:5677; depth 17.33 2021-02-12T23:39:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173157.47 epoch sec; seqNo:5677; 4057512.87 north; 591305.52 east 2021-02-12T23:39:17Z- AHRS_M2 msg: 1613173157.81 epoch sec; seqNo:5628 2021-02-12T23:39:17Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.19 psi 2021-02-12T23:39:17Z- handleDepth()-Depth_Keller msg: 1613173157816 epoch sec; seqNo:5678; depth 17.23 2021-02-12T23:39:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173157.84 epoch sec; seqNo:5678; 4057513.25 north; 591305.52 east 2021-02-12T23:39:18Z- AHRS_M2 msg: 1613173158.21 epoch sec; seqNo:5629 2021-02-12T23:39:18Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.19 psi 2021-02-12T23:39:18Z- handleDepth()-Depth_Keller msg: 1613173158221 epoch sec; seqNo:5679; depth 17.12 2021-02-12T23:39:18Z- handleDvl() - RDI_Pathfinder msg: 1613173158.23 epoch sec; seqNo:2708 2021-02-12T23:39:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.53 , 99.00 , 111.17 , 78.31 2021-02-12T23:39:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.01 , -0.10 2021-02-12T23:39:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173158.26 epoch sec; seqNo:5679; 4057513.63 north; 591305.51 east 2021-02-12T23:39:18Z- AHRS_M2 msg: 1613173158.62 epoch sec; seqNo:5630 2021-02-12T23:39:18Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.20 psi 2021-02-12T23:39:18Z- handleDepth()-Depth_Keller msg: 1613173158624 epoch sec; seqNo:5680; depth 17.01 2021-02-12T23:39:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173158.63 epoch sec; seqNo:5680; 4057513.63 north; 591305.51 east 2021-02-12T23:39:19Z- AHRS_M2 msg: 1613173159.03 epoch sec; seqNo:5631 2021-02-12T23:39:19Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.20 psi 2021-02-12T23:39:19Z- handleDepth()-Depth_Keller msg: 1613173159030 epoch sec; seqNo:5681; depth 16.90 2021-02-12T23:39:19Z- handleDvl() - RDI_Pathfinder msg: 1613173159.04 epoch sec; seqNo:2709 2021-02-12T23:39:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.36 , 108.53 , 71.51 2021-02-12T23:39:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , -0.08 2021-02-12T23:39:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173159.05 epoch sec; seqNo:5681; 4057514.00 north; 591305.51 east 2021-02-12T23:39:19Z- AHRS_M2 msg: 1613173159.43 epoch sec; seqNo:5632 2021-02-12T23:39:19Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.20 psi 2021-02-12T23:39:19Z- handleDepth()-Depth_Keller msg: 1613173159432 epoch sec; seqNo:5682; depth 16.78 2021-02-12T23:39:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173159.45 epoch sec; seqNo:5682; 4057514.38 north; 591305.50 east 2021-02-12T23:39:19Z- AHRS_M2 msg: 1613173159.83 epoch sec; seqNo:5633 2021-02-12T23:39:19Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.20 psi 2021-02-12T23:39:19Z- handleDepth()-Depth_Keller msg: 1613173159841 epoch sec; seqNo:5683; depth 16.67 2021-02-12T23:39:19Z- handleDvl() - RDI_Pathfinder msg: 1613173159.85 epoch sec; seqNo:2710 2021-02-12T23:39:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.05 , 100.42 , 104.47 , 72.74 2021-02-12T23:39:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.14 , -0.05 2021-02-12T23:39:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173159.87 epoch sec; seqNo:5683; 4057514.77 north; 591306.72 east 2021-02-12T23:39:20Z- AHRS_M2 msg: 1613173160.23 epoch sec; seqNo:5634 2021-02-12T23:39:20Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.20 psi 2021-02-12T23:39:20Z- handleDepth()-Depth_Keller msg: 1613173160240 epoch sec; seqNo:5684; depth 16.56 2021-02-12T23:39:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:39:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:39:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:39:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:39:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:39:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:39:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:39:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:39:20Z- handleDvl() - RDI_Pathfinder msg: 1613173160.24 epoch sec; seqNo:2711 2021-02-12T23:39:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:39:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.14 , -0.05 2021-02-12T23:39:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173160.25 epoch sec; seqNo:5684; 4057514.77 north; 591306.72 east 2021-02-12T23:39:20Z- AHRS_M2 msg: 1613173160.64 epoch sec; seqNo:5635 2021-02-12T23:39:20Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.20 psi 2021-02-12T23:39:20Z- handleDepth()-Depth_Keller msg: 1613173160658 epoch sec; seqNo:5685; depth 16.47 2021-02-12T23:39:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173160.67 epoch sec; seqNo:5685; 4057515.15 north; 591306.72 east 2021-02-12T23:39:21Z- AHRS_M2 msg: 1613173161.04 epoch sec; seqNo:5636 2021-02-12T23:39:21Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.19 psi 2021-02-12T23:39:21Z- handleDepth()-Depth_Keller msg: 1613173161053 epoch sec; seqNo:5686; depth 16.36 2021-02-12T23:39:21Z- handleDvl() - RDI_Pathfinder msg: 1613173161.06 epoch sec; seqNo:2712 2021-02-12T23:39:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.28 , 110.02 , 78.38 2021-02-12T23:39:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.15 , 0.01 2021-02-12T23:39:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173161.08 epoch sec; seqNo:5686; 4057515.53 north; 591306.71 east 2021-02-12T23:39:21Z- AHRS_M2 msg: 1613173161.45 epoch sec; seqNo:5637 2021-02-12T23:39:21Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.19 psi 2021-02-12T23:39:21Z- handleDepth()-Depth_Keller msg: 1613173161452 epoch sec; seqNo:5687; depth 16.24 2021-02-12T23:39:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173161.46 epoch sec; seqNo:5687; 4057515.91 north; 591306.71 east 2021-02-12T23:39:21Z- LcmTrn::handleMessages() - handling 45 messages took 0.71 ms and 131 calls... 2021-02-12T23:39:21Z- waiting for 1613173159.22, time is 1613173161.46 2021-02-12T23:39:21Z- LcmTrn::updateTrn() >>>> pose time 1613173161.46, meas time 1613173161.06 <<<< 2021-02-12T23:39:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173161.06 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:39:21Z- TerrainNav::motionUpdate() - currEstimate = (4057516, 591306.71, 16.24) 2021-02-12T23:39:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:39:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:39:21Z- TNavBF::Projecting Measurements 2021-02-12T23:39:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:39:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:39:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:39:21Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:39:21Z- TNavFilter::3 good beams. 2021-02-12T23:39:21Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:39:21Z- TNavBF:: beamIndex[0] = 1 2021-02-12T23:39:21Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:39:21Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:39:21Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:39:21Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:39:21Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:39:21Z- TNavBF::mmse selected from filter -1 2021-02-12T23:39:21Z- TerrainMapOctree: (vehN, vehE ) = (4057515.62, 591306.53). 2021-02-12T23:39:21Z- TerrainMapOctree: Min Distance = 4754.96. 2021-02-12T23:39:21Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:39:21Z- There is no specific good beam that all particles have in common. 2021-02-12T23:39:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:39:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 16.09 2021-02-12T23:39:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:39:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 19.37 2021-02-12T23:39:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:39:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 9.83 2021-02-12T23:39:21Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:39:21Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:39:21Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:39:21Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:39:21Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:39:21Z- Filter 0 No beams used in subcloud update 2021-02-12T23:39:21Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:39:21Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:39:21Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 746.9 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:39:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173161.06 sec. 2021-02-12T23:39:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:39:21Z- TNavBF::mmse selected from filter -1 2021-02-12T23:39:21Z- variances([0], [2], and psi_berg) = 746.837359 833.030158 0.000000 2021-02-12T23:39:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:39:21Z- LcmTrn::publishEstimates() - MLE : 4057503.41 591265.21 16.24 2021-02-12T23:39:21Z- LcmTrn::publishEstimates() - MMSE : 4057515.91 591306.71 16.24 2021-02-12T23:39:21Z- LcmTrn::publishEstimates() - COVAR: 746.84 833.03 0.85 0.00 2021-02-12T23:39:21Z- AHRS_M2 msg: 1613173161.85 epoch sec; seqNo:5638 2021-02-12T23:39:21Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.18 psi 2021-02-12T23:39:21Z- handleDepth()-Depth_Keller msg: 1613173161857 epoch sec; seqNo:5688; depth 16.15 2021-02-12T23:39:21Z- handleDvl() - RDI_Pathfinder msg: 1613173161.87 epoch sec; seqNo:2713 2021-02-12T23:39:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.80 , 96.02 , 108.19 , 73.56 2021-02-12T23:39:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.10 , 0.03 2021-02-12T23:39:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173161.88 epoch sec; seqNo:5688; 4057516.29 north; 591306.70 east 2021-02-12T23:39:22Z- AHRS_M2 msg: 1613173162.26 epoch sec; seqNo:5639 2021-02-12T23:39:22Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.17 psi 2021-02-12T23:39:22Z- handleDepth()-Depth_Keller msg: 1613173162263 epoch sec; seqNo:5689; depth 16.04 2021-02-12T23:39:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173162.28 epoch sec; seqNo:5689; 4057516.29 north; 591306.70 east 2021-02-12T23:39:22Z- AHRS_M2 msg: 1613173162.66 epoch sec; seqNo:5640 2021-02-12T23:39:22Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.17 psi 2021-02-12T23:39:22Z- handleDepth()-Depth_Keller msg: 1613173162668 epoch sec; seqNo:5690; depth 15.94 2021-02-12T23:39:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173162.68 epoch sec; seqNo:5690; 4057516.67 north; 591306.70 east 2021-02-12T23:39:23Z- AHRS_M2 msg: 1613173163.07 epoch sec; seqNo:5641 2021-02-12T23:39:23Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.16 psi 2021-02-12T23:39:23Z- handleDepth()-Depth_Keller msg: 1613173163074 epoch sec; seqNo:5691; depth 15.85 2021-02-12T23:39:23Z- handleDvl() - RDI_Pathfinder msg: 1613173163.09 epoch sec; seqNo:2714 2021-02-12T23:39:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.75 , 97.78 , 108.73 , 72.33 2021-02-12T23:39:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.08 , 0.03 2021-02-12T23:39:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173163.10 epoch sec; seqNo:5691; 4057517.06 north; 591307.92 east 2021-02-12T23:39:23Z- AHRS_M2 msg: 1613173163.47 epoch sec; seqNo:5642 2021-02-12T23:39:23Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-12T23:39:23Z- handleDepth()-Depth_Keller msg: 1613173163473 epoch sec; seqNo:5692; depth 15.74 2021-02-12T23:39:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173163.48 epoch sec; seqNo:5692; 4057517.44 north; 591307.91 east 2021-02-12T23:39:23Z- AHRS_M2 msg: 1613173163.87 epoch sec; seqNo:5643 2021-02-12T23:39:23Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-12T23:39:23Z- handleDepth()-Depth_Keller msg: 1613173163882 epoch sec; seqNo:5693; depth 15.64 2021-02-12T23:39:23Z- handleDvl() - RDI_Pathfinder msg: 1613173163.89 epoch sec; seqNo:2715 2021-02-12T23:39:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 97.78 , 108.73 , 73.56 2021-02-12T23:39:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.05 , 0.03 2021-02-12T23:39:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173163.90 epoch sec; seqNo:5693; 4057517.44 north; 591307.91 east 2021-02-12T23:39:24Z- AHRS_M2 msg: 1613173164.28 epoch sec; seqNo:5644 2021-02-12T23:39:24Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-12T23:39:24Z- handleDepth()-Depth_Keller msg: 1613173164285 epoch sec; seqNo:5694; depth 15.53 2021-02-12T23:39:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173164.30 epoch sec; seqNo:5694; 4057517.82 north; 591307.91 east 2021-02-12T23:39:24Z- AHRS_M2 msg: 1613173164.68 epoch sec; seqNo:5645 2021-02-12T23:39:24Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-12T23:39:24Z- handleDepth()-Depth_Keller msg: 1613173164690 epoch sec; seqNo:5695; depth 15.40 2021-02-12T23:39:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173164.70 epoch sec; seqNo:5695; 4057518.20 north; 591307.91 east 2021-02-12T23:39:25Z- AHRS_M2 msg: 1613173165.08 epoch sec; seqNo:5646 2021-02-12T23:39:25Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.16 psi 2021-02-12T23:39:25Z- handleDepth()-Depth_Keller msg: 1613173165090 epoch sec; seqNo:5696; depth 15.31 2021-02-12T23:39:25Z- handleDvl() - RDI_Pathfinder msg: 1613173165.10 epoch sec; seqNo:2716 2021-02-12T23:39:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 97.71 , 108.66 , 74.37 2021-02-12T23:39:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.09 , 0.05 2021-02-12T23:39:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173165.11 epoch sec; seqNo:5696; 4057518.20 north; 591307.91 east 2021-02-12T23:39:25Z- AHRS_M2 msg: 1613173165.49 epoch sec; seqNo:5647 2021-02-12T23:39:25Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.16 psi 2021-02-12T23:39:25Z- handleDepth()-Depth_Keller msg: 1613173165497 epoch sec; seqNo:5697; depth 15.20 2021-02-12T23:39:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173165.52 epoch sec; seqNo:5697; 4057518.58 north; 591307.90 east 2021-02-12T23:39:25Z- AHRS_M2 msg: 1613173165.89 epoch sec; seqNo:5648 2021-02-12T23:39:25Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.17 psi 2021-02-12T23:39:25Z- handleDepth()-Depth_Keller msg: 1613173165896 epoch sec; seqNo:5698; depth 15.09 2021-02-12T23:39:25Z- handleDvl() - RDI_Pathfinder msg: 1613173165.91 epoch sec; seqNo:2717 2021-02-12T23:39:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 97.65 , 109.81 , 75.19 2021-02-12T23:39:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.11 , 0.13 2021-02-12T23:39:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173165.93 epoch sec; seqNo:5698; 4057518.96 north; 591307.90 east 2021-02-12T23:39:26Z- AHRS_M2 msg: 1613173166.30 epoch sec; seqNo:5649 2021-02-12T23:39:26Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.17 psi 2021-02-12T23:39:26Z- handleDepth()-Depth_Keller msg: 1613173166302 epoch sec; seqNo:5699; depth 14.97 2021-02-12T23:39:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173166.31 epoch sec; seqNo:5699; 4057519.35 north; 591309.11 east 2021-02-12T23:39:26Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-12T23:39:26Z- waiting for 1613173164.46, time is 1613173166.31 2021-02-12T23:39:26Z- LcmTrn::updateTrn() >>>> pose time 1613173166.31, meas time 1613173165.91 <<<< 2021-02-12T23:39:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173165.91 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:39:26Z- TerrainNav::motionUpdate() - currEstimate = (4057519, 591309.11, 14.97) 2021-02-12T23:39:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:39:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:39:26Z- TNavBF::Projecting Measurements 2021-02-12T23:39:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:39:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:39:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:39:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:39:26Z- TNavFilter::4 good beams. 2021-02-12T23:39:26Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:39:26Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:39:26Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:39:26Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:39:26Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:39:26Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:39:26Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:39:26Z- TNavBF::mmse selected from filter -1 2021-02-12T23:39:26Z- TerrainMapOctree: (vehN, vehE ) = (4057519.06, 591308.91). 2021-02-12T23:39:26Z- TerrainMapOctree: Min Distance = 4750.78. 2021-02-12T23:39:26Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:39:26Z- There is no specific good beam that all particles have in common. 2021-02-12T23:39:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:39:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 11.03 2021-02-12T23:39:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:39:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.26 2021-02-12T23:39:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:39:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 19.29 2021-02-12T23:39:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:39:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 9.05 2021-02-12T23:39:26Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:39:26Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:39:26Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:39:26Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:39:26Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:39:26Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:39:26Z- Filter 0 No beams used in subcloud update 2021-02-12T23:39:26Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:39:26Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:39:26Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 746.7 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:39:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173165.91 sec. 2021-02-12T23:39:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:39:26Z- TNavBF::mmse selected from filter -1 2021-02-12T23:39:26Z- variances([0], [2], and psi_berg) = 746.718239 833.030052 0.000000 2021-02-12T23:39:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:39:26Z- LcmTrn::publishEstimates() - MLE : 4057506.85 591267.61 14.97 2021-02-12T23:39:26Z- LcmTrn::publishEstimates() - MMSE : 4057519.35 591309.11 14.97 2021-02-12T23:39:26Z- LcmTrn::publishEstimates() - COVAR: 746.72 833.03 0.83 0.00 2021-02-12T23:39:26Z- AHRS_M2 msg: 1613173166.70 epoch sec; seqNo:5650 2021-02-12T23:39:26Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.17 psi 2021-02-12T23:39:26Z- handleDepth()-Depth_Keller msg: 1613173166704 epoch sec; seqNo:5700; depth 14.88 2021-02-12T23:39:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173166.72 epoch sec; seqNo:5700; 4057519.73 north; 591309.11 east 2021-02-12T23:39:27Z- AHRS_M2 msg: 1613173167.10 epoch sec; seqNo:5651 2021-02-12T23:39:27Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.16 psi 2021-02-12T23:39:27Z- handleDepth()-Depth_Keller msg: 1613173167113 epoch sec; seqNo:5701; depth 14.78 2021-02-12T23:39:27Z- handleDvl() - RDI_Pathfinder msg: 1613173167.13 epoch sec; seqNo:2718 2021-02-12T23:39:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 98.86 , 111.03 , 78.18 2021-02-12T23:39:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.03 , 0.16 2021-02-12T23:39:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173167.15 epoch sec; seqNo:5701; 4057519.73 north; 591309.11 east 2021-02-12T23:39:27Z- AHRS_M2 msg: 1613173167.51 epoch sec; seqNo:5652 2021-02-12T23:39:27Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.16 psi 2021-02-12T23:39:27Z- handleDepth()-Depth_Keller msg: 1613173167512 epoch sec; seqNo:5702; depth 14.67 2021-02-12T23:39:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173167.52 epoch sec; seqNo:5702; 4057520.10 north; 591309.11 east 2021-02-12T23:39:27Z- AHRS_M2 msg: 1613173167.91 epoch sec; seqNo:5653 2021-02-12T23:39:27Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.15 psi 2021-02-12T23:39:27Z- handleDepth()-Depth_Keller msg: 1613173167925 epoch sec; seqNo:5703; depth 14.55 2021-02-12T23:39:27Z- handleDvl() - RDI_Pathfinder msg: 1613173167.94 epoch sec; seqNo:2719 2021-02-12T23:39:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.13 , 99.00 , 112.52 , 75.60 2021-02-12T23:39:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.03 , 0.08 2021-02-12T23:39:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173167.95 epoch sec; seqNo:5703; 4057520.48 north; 591309.10 east 2021-02-12T23:39:28Z- AHRS_M2 msg: 1613173168.32 epoch sec; seqNo:5654 2021-02-12T23:39:28Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.14 psi 2021-02-12T23:39:28Z- handleDepth()-Depth_Keller msg: 1613173168320 epoch sec; seqNo:5704; depth 14.44 2021-02-12T23:39:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173168.34 epoch sec; seqNo:5704; 4057520.86 north; 591309.10 east 2021-02-12T23:39:28Z- AHRS_M2 msg: 1613173168.72 epoch sec; seqNo:5655 2021-02-12T23:39:28Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.14 psi 2021-02-12T23:39:28Z- handleDepth()-Depth_Keller msg: 1613173168729 epoch sec; seqNo:5705; depth 14.34 2021-02-12T23:39:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173168.76 epoch sec; seqNo:5705; 4057521.25 north; 591310.32 east 2021-02-12T23:39:29Z- AHRS_M2 msg: 1613173169.13 epoch sec; seqNo:5656 2021-02-12T23:39:29Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.14 psi 2021-02-12T23:39:29Z- handleDepth()-Depth_Keller msg: 1613173169132 epoch sec; seqNo:5706; depth 14.24 2021-02-12T23:39:29Z- handleDvl() - RDI_Pathfinder msg: 1613173169.15 epoch sec; seqNo:2720 2021-02-12T23:39:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.48 , 97.65 , 112.52 , 73.56 2021-02-12T23:39:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.07 2021-02-12T23:39:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173169.17 epoch sec; seqNo:5706; 4057521.25 north; 591310.32 east 2021-02-12T23:39:29Z- AHRS_M2 msg: 1613173169.53 epoch sec; seqNo:5657 2021-02-12T23:39:29Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.14 psi 2021-02-12T23:39:29Z- handleDepth()-Depth_Keller msg: 1613173169541 epoch sec; seqNo:5707; depth 14.12 2021-02-12T23:39:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173169.55 epoch sec; seqNo:5707; 4057521.63 north; 591310.31 east 2021-02-12T23:39:29Z- AHRS_M2 msg: 1613173169.93 epoch sec; seqNo:5658 2021-02-12T23:39:29Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.14 psi 2021-02-12T23:39:29Z- handleDepth()-Depth_Keller msg: 1613173169941 epoch sec; seqNo:5708; depth 14.01 2021-02-12T23:39:29Z- handleDvl() - RDI_Pathfinder msg: 1613173169.96 epoch sec; seqNo:2721 2021-02-12T23:39:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 96.36 , 112.58 , 73.96 2021-02-12T23:39:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.17 2021-02-12T23:39:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173169.97 epoch sec; seqNo:5708; 4057522.01 north; 591310.31 east 2021-02-12T23:39:30Z- AHRS_M2 msg: 1613173170.34 epoch sec; seqNo:5659 2021-02-12T23:39:30Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.15 psi 2021-02-12T23:39:30Z- handleDepth()-Depth_Keller msg: 1613173170347 epoch sec; seqNo:5709; depth 13.89 2021-02-12T23:39:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173170.36 epoch sec; seqNo:5709; 4057522.01 north; 591310.31 east 2021-02-12T23:39:30Z- AHRS_M2 msg: 1613173170.78 epoch sec; seqNo:5660 2021-02-12T23:39:30Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:39:30Z- handleDepth()-Depth_Keller msg: 1613173170779 epoch sec; seqNo:5710; depth 13.79 2021-02-12T23:39:30Z- handleDvl() - RDI_Pathfinder msg: 1613173170.80 epoch sec; seqNo:2722 2021-02-12T23:39:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 96.36 , 112.58 , 73.96 2021-02-12T23:39:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.19 , -0.14 2021-02-12T23:39:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173170.81 epoch sec; seqNo:5710; 4057522.39 north; 591310.30 east 2021-02-12T23:39:31Z- AHRS_M2 msg: 1613173171.16 epoch sec; seqNo:5661 2021-02-12T23:39:31Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:39:31Z- handleDepth()-Depth_Keller msg: 1613173171171 epoch sec; seqNo:5711; depth 13.67 2021-02-12T23:39:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173171.19 epoch sec; seqNo:5711; 4057522.77 north; 591310.30 east 2021-02-12T23:39:31Z- AHRS_M2 msg: 1613173171.55 epoch sec; seqNo:5662 2021-02-12T23:39:31Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-12T23:39:31Z- handleDepth()-Depth_Keller msg: 1613173171557 epoch sec; seqNo:5712; depth 13.55 2021-02-12T23:39:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173171.57 epoch sec; seqNo:5712; 4057523.15 north; 591310.29 east 2021-02-12T23:39:31Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 132 calls... 2021-02-12T23:39:31Z- waiting for 1613173169.31, time is 1613173171.57 2021-02-12T23:39:31Z- LcmTrn::updateTrn() >>>> pose time 1613173171.57, meas time 1613173170.80 <<<< 2021-02-12T23:39:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173170.80 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:39:31Z- TerrainNav::motionUpdate() - currEstimate = (4057523, 591310.29, 13.55) 2021-02-12T23:39:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:39:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:39:31Z- TNavBF::Projecting Measurements 2021-02-12T23:39:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:39:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:39:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:39:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:39:31Z- TNavFilter::4 good beams. 2021-02-12T23:39:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:39:31Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:39:31Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:39:31Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:39:31Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:39:31Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:39:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:39:31Z- TNavBF::mmse selected from filter -1 2021-02-12T23:39:31Z- TerrainMapOctree: (vehN, vehE ) = (4057522.59, 591310.12). 2021-02-12T23:39:31Z- TerrainMapOctree: Min Distance = 4747.26. 2021-02-12T23:39:31Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:39:31Z- There is no specific good beam that all particles have in common. 2021-02-12T23:39:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:39:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 11.34 2021-02-12T23:39:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:39:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 14.86 2021-02-12T23:39:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:39:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 20.28 2021-02-12T23:39:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:39:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 8.75 2021-02-12T23:39:31Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:39:31Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:39:31Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:39:31Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:39:31Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:39:31Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:39:31Z- Filter 0 No beams used in subcloud update 2021-02-12T23:39:31Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:39:31Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:39:31Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 746.6 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:39:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173170.80 sec. 2021-02-12T23:39:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:39:31Z- TNavBF::mmse selected from filter -1 2021-02-12T23:39:31Z- variances([0], [2], and psi_berg) = 746.596778 833.029944 0.000000 2021-02-12T23:39:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:39:31Z- LcmTrn::publishEstimates() - MLE : 4057510.65 591268.79 13.55 2021-02-12T23:39:31Z- LcmTrn::publishEstimates() - MMSE : 4057523.15 591310.29 13.55 2021-02-12T23:39:31Z- LcmTrn::publishEstimates() - COVAR: 746.60 833.03 0.81 0.00 2021-02-12T23:39:31Z- AHRS_M2 msg: 1613173171.95 epoch sec; seqNo:5663 2021-02-12T23:39:31Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-12T23:39:31Z- handleDepth()-Depth_Keller msg: 1613173171956 epoch sec; seqNo:5713; depth 13.45 2021-02-12T23:39:31Z- handleDvl() - RDI_Pathfinder msg: 1613173171.97 epoch sec; seqNo:2723 2021-02-12T23:39:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 96.36 , 112.58 , 74.37 2021-02-12T23:39:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.22 , -0.04 2021-02-12T23:39:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173171.98 epoch sec; seqNo:5713; 4057523.15 north; 591310.29 east 2021-02-12T23:39:32Z- AHRS_M2 msg: 1613173172.36 epoch sec; seqNo:5664 2021-02-12T23:39:32Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-12T23:39:32Z- handleDepth()-Depth_Keller msg: 1613173172362 epoch sec; seqNo:5714; depth 13.35 2021-02-12T23:39:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173172.37 epoch sec; seqNo:5714; 4057523.53 north; 591310.29 east 2021-02-12T23:39:32Z- AHRS_M2 msg: 1613173172.76 epoch sec; seqNo:5665 2021-02-12T23:39:32Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.17 psi 2021-02-12T23:39:32Z- handleDepth()-Depth_Keller msg: 1613173172764 epoch sec; seqNo:5715; depth 13.24 2021-02-12T23:39:32Z- handleDvl() - RDI_Pathfinder msg: 1613173172.78 epoch sec; seqNo:2724 2021-02-12T23:39:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 99.07 , 113.94 , 74.37 2021-02-12T23:39:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.19 , 0.06 2021-02-12T23:39:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173172.80 epoch sec; seqNo:5715; 4057523.92 north; 591311.51 east 2021-02-12T23:39:33Z- AHRS_M2 msg: 1613173173.16 epoch sec; seqNo:5666 2021-02-12T23:39:33Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.17 psi 2021-02-12T23:39:33Z- handleDepth()-Depth_Keller msg: 1613173173173 epoch sec; seqNo:5716; depth 13.11 2021-02-12T23:39:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173173.18 epoch sec; seqNo:5716; 4057524.30 north; 591311.50 east 2021-02-12T23:39:33Z- AHRS_M2 msg: 1613173173.58 epoch sec; seqNo:5667 2021-02-12T23:39:33Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-12T23:39:33Z- handleDepth()-Depth_Keller msg: 1613173173584 epoch sec; seqNo:5717; depth 13.01 2021-02-12T23:39:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173173.60 epoch sec; seqNo:5717; 4057524.68 north; 591311.50 east 2021-02-12T23:39:33Z- handleDepth()-Depth_Keller msg: 1613173173993 epoch sec; seqNo:5718; depth 12.90 2021-02-12T23:39:34Z- handleDvl() - RDI_Pathfinder msg: 1613173174.01 epoch sec; seqNo:2725 2021-02-12T23:39:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.21 , 101.43 , 114.95 , 78.45 2021-02-12T23:39:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.14 , 0.08 2021-02-12T23:39:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173174.02 epoch sec; seqNo:5718; 4057524.68 north; 591311.50 east 2021-02-12T23:39:34Z- AHRS_M2 msg: 1613173174.38 epoch sec; seqNo:5668 2021-02-12T23:39:34Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-12T23:39:34Z- handleDepth()-Depth_Keller msg: 1613173174385 epoch sec; seqNo:5719; depth 12.77 2021-02-12T23:39:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173174.39 epoch sec; seqNo:5719; 4057525.06 north; 591311.50 east 2021-02-12T23:39:34Z- AHRS_M2 msg: 1613173174.79 epoch sec; seqNo:5669 2021-02-12T23:39:34Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-12T23:39:34Z- handleDepth()-Depth_Keller msg: 1613173174794 epoch sec; seqNo:5720; depth 12.66 2021-02-12T23:39:34Z- handleDvl() - RDI_Pathfinder msg: 1613173174.81 epoch sec; seqNo:2726 2021-02-12T23:39:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.28 , 101.50 , 115.02 , 75.19 2021-02-12T23:39:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.09 , 0.03 2021-02-12T23:39:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173174.83 epoch sec; seqNo:5720; 4057525.43 north; 591311.49 east 2021-02-12T23:39:35Z- AHRS_M2 msg: 1613173175.19 epoch sec; seqNo:5670 2021-02-12T23:39:35Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-12T23:39:35Z- handleDepth()-Depth_Keller msg: 1613173175190 epoch sec; seqNo:5721; depth 12.55 2021-02-12T23:39:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173175.20 epoch sec; seqNo:5721; 4057525.81 north; 591311.49 east 2021-02-12T23:39:35Z- AHRS_M2 msg: 1613173175.59 epoch sec; seqNo:5671 2021-02-12T23:39:35Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-12T23:39:35Z- handleDepth()-Depth_Keller msg: 1613173175594 epoch sec; seqNo:5722; depth 12.44 2021-02-12T23:39:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173175.61 epoch sec; seqNo:5722; 4057525.81 north; 591311.49 east 2021-02-12T23:39:35Z- AHRS_M2 msg: 1613173175.99 epoch sec; seqNo:5672 2021-02-12T23:39:35Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-12T23:39:35Z- handleDepth()-Depth_Keller msg: 1613173175996 epoch sec; seqNo:5723; depth 12.36 2021-02-12T23:39:36Z- handleDvl() - RDI_Pathfinder msg: 1613173176.01 epoch sec; seqNo:2727 2021-02-12T23:39:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 102.92 , 115.09 , 78.58 2021-02-12T23:39:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.09 , -0.03 2021-02-12T23:39:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173176.03 epoch sec; seqNo:5723; 4057526.21 north; 591312.70 east 2021-02-12T23:39:36Z- AHRS_M2 msg: 1613173176.39 epoch sec; seqNo:5673 2021-02-12T23:39:36Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-12T23:39:36Z- handleDepth()-Depth_Keller msg: 1613173176405 epoch sec; seqNo:5724; depth 12.23 2021-02-12T23:39:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173176.42 epoch sec; seqNo:5724; 4057526.58 north; 591312.70 east 2021-02-12T23:39:36Z- AHRS_M2 msg: 1613173176.81 epoch sec; seqNo:5674 2021-02-12T23:39:36Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-12T23:39:36Z- handleDepth()-Depth_Keller msg: 1613173176810 epoch sec; seqNo:5725; depth 12.12 2021-02-12T23:39:36Z- LcmTrn::handleMessages() - handling 42 messages took 0.74 ms and 127 calls... 2021-02-12T23:39:36Z- waiting for 1613173174.57, time is 1613173176.81 2021-02-12T23:39:36Z- LcmTrn::updateTrn() >>>> pose time 1613173176.81, meas time 1613173176.01 <<<< 2021-02-12T23:39:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173176.01 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:39:36Z- TerrainNav::motionUpdate() - currEstimate = (4057527, 591312.70, 12.12) 2021-02-12T23:39:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:39:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:39:36Z- TNavBF::Projecting Measurements 2021-02-12T23:39:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:39:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:39:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:39:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:39:36Z- TNavFilter::4 good beams. 2021-02-12T23:39:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:39:36Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:39:36Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:39:36Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:39:36Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:39:36Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:39:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:39:36Z- TNavBF::mmse selected from filter -1 2021-02-12T23:39:36Z- TerrainMapOctree: (vehN, vehE ) = (4057526.06, 591312.33). 2021-02-12T23:39:36Z- TerrainMapOctree: Min Distance = 4743.16. 2021-02-12T23:39:36Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:39:36Z- There is no specific good beam that all particles have in common. 2021-02-12T23:39:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:39:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 11.65 2021-02-12T23:39:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:39:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 16.95 2021-02-12T23:39:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:39:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 21.19 2021-02-12T23:39:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:39:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-12T23:39:36Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:39:36Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:39:36Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:39:36Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:39:36Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:39:36Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:39:36Z- Filter 0 No beams used in subcloud update 2021-02-12T23:39:36Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:39:36Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:39:36Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 746.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:39:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173176.01 sec. 2021-02-12T23:39:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:39:36Z- TNavBF::mmse selected from filter -1 2021-02-12T23:39:36Z- variances([0], [2], and psi_berg) = 746.471879 833.029833 0.000000 2021-02-12T23:39:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:39:36Z- LcmTrn::publishEstimates() - MLE : 4057514.08 591271.20 12.12 2021-02-12T23:39:36Z- LcmTrn::publishEstimates() - MMSE : 4057526.58 591312.70 12.12 2021-02-12T23:39:36Z- LcmTrn::publishEstimates() - COVAR: 746.47 833.03 0.83 0.00 2021-02-12T23:39:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173176.84 epoch sec; seqNo:5725; 4057526.96 north; 591312.70 east 2021-02-12T23:39:37Z- AHRS_M2 msg: 1613173177.20 epoch sec; seqNo:5675 2021-02-12T23:39:37Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-12T23:39:37Z- handleDepth()-Depth_Keller msg: 1613173177208 epoch sec; seqNo:5726; depth 12.01 2021-02-12T23:39:37Z- handleDvl() - RDI_Pathfinder msg: 1613173177.23 epoch sec; seqNo:2728 2021-02-12T23:39:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.28 , 101.50 , 115.02 , 78.51 2021-02-12T23:39:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.15 , 0.02 2021-02-12T23:39:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173177.24 epoch sec; seqNo:5726; 4057526.96 north; 591312.70 east 2021-02-12T23:39:37Z- AHRS_M2 msg: 1613173177.61 epoch sec; seqNo:5676 2021-02-12T23:39:37Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-12T23:39:37Z- handleDepth()-Depth_Keller msg: 1613173177617 epoch sec; seqNo:5727; depth 11.90 2021-02-12T23:39:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173177.63 epoch sec; seqNo:5727; 4057527.34 north; 591312.69 east 2021-02-12T23:39:38Z- AHRS_M2 msg: 1613173178.01 epoch sec; seqNo:5677 2021-02-12T23:39:38Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-12T23:39:38Z- handleDepth()-Depth_Keller msg: 1613173178017 epoch sec; seqNo:5728; depth 11.80 2021-02-12T23:39:38Z- handleDvl() - RDI_Pathfinder msg: 1613173178.03 epoch sec; seqNo:2729 2021-02-12T23:39:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.28 , 101.50 , 113.67 , 78.51 2021-02-12T23:39:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.15 , 0.19 2021-02-12T23:39:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173178.04 epoch sec; seqNo:5728; 4057527.72 north; 591312.69 east 2021-02-12T23:39:38Z- AHRS_M2 msg: 1613173178.42 epoch sec; seqNo:5678 2021-02-12T23:39:38Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-12T23:39:38Z- handleDepth()-Depth_Keller msg: 1613173178422 epoch sec; seqNo:5729; depth 11.68 2021-02-12T23:39:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173178.43 epoch sec; seqNo:5729; 4057528.10 north; 591312.68 east 2021-02-12T23:39:38Z- AHRS_M2 msg: 1613173178.82 epoch sec; seqNo:5679 2021-02-12T23:39:38Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.14 psi 2021-02-12T23:39:38Z- handleDepth()-Depth_Keller msg: 1613173178824 epoch sec; seqNo:5730; depth 11.58 2021-02-12T23:39:38Z- handleDvl() - RDI_Pathfinder msg: 1613173178.84 epoch sec; seqNo:2730 2021-02-12T23:39:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 100.21 , 113.73 , 76.83 2021-02-12T23:39:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , 0.30 2021-02-12T23:39:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173178.87 epoch sec; seqNo:5730; 4057528.10 north; 591312.68 east 2021-02-12T23:39:39Z- AHRS_M2 msg: 1613173179.22 epoch sec; seqNo:5680 2021-02-12T23:39:39Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.13 psi 2021-02-12T23:39:39Z- handleDepth()-Depth_Keller msg: 1613173179233 epoch sec; seqNo:5731; depth 11.45 2021-02-12T23:39:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173179.24 epoch sec; seqNo:5731; 4057528.49 north; 591313.90 east 2021-02-12T23:39:39Z- AHRS_M2 msg: 1613173179.63 epoch sec; seqNo:5681 2021-02-12T23:39:39Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.12 psi 2021-02-12T23:39:39Z- handleDepth()-Depth_Keller msg: 1613173179634 epoch sec; seqNo:5732; depth 11.36 2021-02-12T23:39:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173179.64 epoch sec; seqNo:5732; 4057528.87 north; 591313.90 east 2021-02-12T23:39:40Z- AHRS_M2 msg: 1613173180.03 epoch sec; seqNo:5682 2021-02-12T23:39:40Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.13 psi 2021-02-12T23:39:40Z- handleDepth()-Depth_Keller msg: 1613173180046 epoch sec; seqNo:5733; depth 11.23 2021-02-12T23:39:40Z- handleDvl() - RDI_Pathfinder msg: 1613173180.06 epoch sec; seqNo:2731 2021-02-12T23:39:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.51 , 100.21 , 117.79 , 78.05 2021-02-12T23:39:40Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.13 , 0.16 2021-02-12T23:39:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173180.07 epoch sec; seqNo:5733; 4057529.25 north; 591313.89 east 2021-02-12T23:39:40Z- AHRS_M2 msg: 1613173180.43 epoch sec; seqNo:5683 2021-02-12T23:39:40Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.13 psi 2021-02-12T23:39:40Z- handleDepth()-Depth_Keller msg: 1613173180445 epoch sec; seqNo:5734; depth 11.12 2021-02-12T23:39:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173180.45 epoch sec; seqNo:5734; 4057529.63 north; 591313.89 east 2021-02-12T23:39:40Z- AHRS_M2 msg: 1613173180.84 epoch sec; seqNo:5684 2021-02-12T23:39:40Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.14 psi 2021-02-12T23:39:40Z- handleDepth()-Depth_Keller msg: 1613173180844 epoch sec; seqNo:5735; depth 11.02 2021-02-12T23:39:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173180.85 epoch sec; seqNo:5735; 4057529.63 north; 591313.89 east 2021-02-12T23:39:41Z- AHRS_M2 msg: 1613173181.24 epoch sec; seqNo:5685 2021-02-12T23:39:41Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.14 psi 2021-02-12T23:39:41Z- handleDepth()-Depth_Keller msg: 1613173181249 epoch sec; seqNo:5736; depth 10.89 2021-02-12T23:39:41Z- handleDvl() - RDI_Pathfinder msg: 1613173181.26 epoch sec; seqNo:2732 2021-02-12T23:39:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:39:41Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:39:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173181.27 epoch sec; seqNo:5736; 4057530.02 north; 591315.11 east 2021-02-12T23:39:41Z- AHRS_M2 msg: 1613173181.65 epoch sec; seqNo:5686 2021-02-12T23:39:41Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.14 psi 2021-02-12T23:39:41Z- handleDepth()-Depth_Keller msg: 1613173181657 epoch sec; seqNo:5737; depth 10.78 2021-02-12T23:39:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173181.67 epoch sec; seqNo:5737; 4057530.40 north; 591315.10 east 2021-02-12T23:39:41Z- LcmTrn::handleMessages() - handling 42 messages took 0.63 ms and 130 calls... 2021-02-12T23:39:41Z- waiting for 1613173179.81, time is 1613173181.67 2021-02-12T23:39:41Z- LcmTrn::updateTrn() >>>> pose time 1613173181.67, meas time 1613173181.26 <<<< 2021-02-12T23:39:41Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173181.26, ping # 0. 2021-02-12T23:39:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173181.26 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:39:41Z- TerrainNav::motionUpdate() - currEstimate = (4057530, 591315.10, 10.78) 2021-02-12T23:39:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:39:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:39:41Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:39:41Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:39:41Z- TNavBF::Projecting Measurements 2021-02-12T23:39:41Z- TNavFilter::Measurement from time = 1613173181.26 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:39:41Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:39:41Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:39:41Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:39:41Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:39:41Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:39:41Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:39:41Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:39:41Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 746.4 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:39:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:39:41Z- TNavBF::mmse selected from filter -1 2021-02-12T23:39:41Z- variances([0], [2], and psi_berg) = 746.348793 833.029724 0.000000 2021-02-12T23:39:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:39:41Z- LcmTrn::publishEstimates() - MLE : 4057517.90 591273.60 10.78 2021-02-12T23:39:41Z- LcmTrn::publishEstimates() - MMSE : 4057530.40 591315.10 10.78 2021-02-12T23:39:41Z- LcmTrn::publishEstimates() - COVAR: 746.35 833.03 1.05 0.00 2021-02-12T23:39:42Z- AHRS_M2 msg: 1613173182.05 epoch sec; seqNo:5687 2021-02-12T23:39:42Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.15 psi 2021-02-12T23:39:42Z- handleDepth()-Depth_Keller msg: 1613173182056 epoch sec; seqNo:5738; depth 10.67 2021-02-12T23:39:42Z- handleDvl() - RDI_Pathfinder msg: 1613173182.07 epoch sec; seqNo:2733 2021-02-12T23:39:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.65 , 103.53 , 121.37 , 76.42 2021-02-12T23:39:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.10 , -0.21 2021-02-12T23:39:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173182.08 epoch sec; seqNo:5738; 4057530.78 north; 591315.10 east 2021-02-12T23:39:42Z- AHRS_M2 msg: 1613173182.46 epoch sec; seqNo:5688 2021-02-12T23:39:42Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.15 psi 2021-02-12T23:39:42Z- handleDepth()-Depth_Keller msg: 1613173182465 epoch sec; seqNo:5739; depth 10.54 2021-02-12T23:39:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173182.48 epoch sec; seqNo:5739; 4057530.78 north; 591315.10 east 2021-02-12T23:39:42Z- AHRS_M2 msg: 1613173182.87 epoch sec; seqNo:5689 2021-02-12T23:39:42Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.14 psi 2021-02-12T23:39:42Z- handleDepth()-Depth_Keller msg: 1613173182870 epoch sec; seqNo:5740; depth 10.43 2021-02-12T23:39:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173182.89 epoch sec; seqNo:5740; 4057531.16 north; 591315.09 east 2021-02-12T23:39:43Z- AHRS_M2 msg: 1613173183.26 epoch sec; seqNo:5690 2021-02-12T23:39:43Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.14 psi 2021-02-12T23:39:43Z- handleDepth()-Depth_Keller msg: 1613173183268 epoch sec; seqNo:5741; depth 10.31 2021-02-12T23:39:43Z- handleDvl() - RDI_Pathfinder msg: 1613173183.29 epoch sec; seqNo:2734 2021-02-12T23:39:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 104.00 , 121.58 , 78.31 2021-02-12T23:39:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.17 , -0.16 2021-02-12T23:39:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173183.30 epoch sec; seqNo:5741; 4057531.53 north; 591315.09 east 2021-02-12T23:39:43Z- AHRS_M2 msg: 1613173183.67 epoch sec; seqNo:5691 2021-02-12T23:39:43Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.13 psi 2021-02-12T23:39:43Z- handleDepth()-Depth_Keller msg: 1613173183672 epoch sec; seqNo:5742; depth 10.19 2021-02-12T23:39:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173183.68 epoch sec; seqNo:5742; 4057531.93 north; 591316.31 east 2021-02-12T23:39:44Z- AHRS_M2 msg: 1613173184.07 epoch sec; seqNo:5692 2021-02-12T23:39:44Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.13 psi 2021-02-12T23:39:44Z- handleDepth()-Depth_Keller msg: 1613173184077 epoch sec; seqNo:5743; depth 10.08 2021-02-12T23:39:44Z- handleDvl() - RDI_Pathfinder msg: 1613173184.09 epoch sec; seqNo:2735 2021-02-12T23:39:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 102.44 , 120.02 , 79.46 2021-02-12T23:39:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.18 , -0.12 2021-02-12T23:39:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173184.10 epoch sec; seqNo:5743; 4057531.93 north; 591316.31 east 2021-02-12T23:39:44Z- AHRS_M2 msg: 1613173184.48 epoch sec; seqNo:5693 2021-02-12T23:39:44Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.13 psi 2021-02-12T23:39:44Z- handleDepth()-Depth_Keller msg: 1613173184486 epoch sec; seqNo:5744; depth 9.94 2021-02-12T23:39:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173184.50 epoch sec; seqNo:5744; 4057532.31 north; 591316.30 east 2021-02-12T23:39:44Z- AHRS_M2 msg: 1613173184.89 epoch sec; seqNo:5694 2021-02-12T23:39:44Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.13 psi 2021-02-12T23:39:44Z- handleDepth()-Depth_Keller msg: 1613173184917 epoch sec; seqNo:5745; depth 9.81 2021-02-12T23:39:44Z- handleDvl() - RDI_Pathfinder msg: 1613173184.93 epoch sec; seqNo:2736 2021-02-12T23:39:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 101.30 , 118.87 , 79.67 2021-02-12T23:39:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.23 , -0.06 2021-02-12T23:39:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173184.94 epoch sec; seqNo:5745; 4057532.68 north; 591316.30 east 2021-02-12T23:39:45Z- AHRS_M2 msg: 1613173185.28 epoch sec; seqNo:5695 2021-02-12T23:39:45Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.15 psi 2021-02-12T23:39:45Z- handleDepth()-Depth_Keller msg: 1613173185288 epoch sec; seqNo:5746; depth 9.70 2021-02-12T23:39:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173185.30 epoch sec; seqNo:5746; 4057532.68 north; 591316.30 east 2021-02-12T23:39:45Z- AHRS_M2 msg: 1613173185.69 epoch sec; seqNo:5696 2021-02-12T23:39:45Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.16 psi 2021-02-12T23:39:45Z- handleDepth()-Depth_Keller msg: 1613173185696 epoch sec; seqNo:5747; depth 9.58 2021-02-12T23:39:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173185.70 epoch sec; seqNo:5747; 4057533.06 north; 591316.29 east 2021-02-12T23:39:46Z- AHRS_M2 msg: 1613173186.09 epoch sec; seqNo:5697 2021-02-12T23:39:46Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-12T23:39:46Z- handleDepth()-Depth_Keller msg: 1613173186096 epoch sec; seqNo:5748; depth 9.46 2021-02-12T23:39:46Z- handleDvl() - RDI_Pathfinder msg: 1613173186.11 epoch sec; seqNo:2737 2021-02-12T23:39:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 102.51 , 117.39 , 79.69 2021-02-12T23:39:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.28 , 0.11 2021-02-12T23:39:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173186.13 epoch sec; seqNo:5748; 4057533.44 north; 591316.29 east 2021-02-12T23:39:46Z- AHRS_M2 msg: 1613173186.49 epoch sec; seqNo:5698 2021-02-12T23:39:46Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-12T23:39:46Z- handleDepth()-Depth_Keller msg: 1613173186505 epoch sec; seqNo:5749; depth 9.32 2021-02-12T23:39:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173186.52 epoch sec; seqNo:5749; 4057533.82 north; 591316.29 east 2021-02-12T23:39:46Z- AHRS_M2 msg: 1613173186.92 epoch sec; seqNo:5699 2021-02-12T23:39:46Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.18 psi 2021-02-12T23:39:46Z- handleDepth()-Depth_Keller msg: 1613173186925 epoch sec; seqNo:5750; depth 9.22 2021-02-12T23:39:46Z- handleDvl() - RDI_Pathfinder msg: 1613173186.94 epoch sec; seqNo:2738 2021-02-12T23:39:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.31 , 105.42 , 121.64 , 79.73 2021-02-12T23:39:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.12 , 0.19 2021-02-12T23:39:46Z- LcmTrn::handleMessages() - handling 44 messages took 0.74 ms and 129 calls... 2021-02-12T23:39:46Z- waiting for 1613173184.67, time is 1613173186.94 2021-02-12T23:39:46Z- LcmTrn::updateTrn() equating pose 1613173186.94 and meas times 1613173186.94 2021-02-12T23:39:46Z- LcmTrn::updateTrn() >>>> pose time 1613173186.94, meas time 1613173186.94 <<<< 2021-02-12T23:39:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173186.94 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:39:46Z- TerrainNav::motionUpdate() - currEstimate = (4057534, 591316.29, 9.22) 2021-02-12T23:39:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:39:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:39:46Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173186.94 sec.; No relevant navigation data available 2021-02-12T23:39:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:39:46Z- TNavBF::mmse selected from filter -1 2021-02-12T23:39:46Z- variances([0], [2], and psi_berg) = 746.260511 833.029645 0.000000 2021-02-12T23:39:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:39:46Z- LcmTrn::publishEstimates() - MLE : 4057521.32 591274.79 9.22 2021-02-12T23:39:46Z- LcmTrn::publishEstimates() - MMSE : 4057533.82 591316.29 9.22 2021-02-12T23:39:46Z- LcmTrn::publishEstimates() - COVAR: 746.26 833.03 1.24 0.00 2021-02-12T23:39:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173186.96 epoch sec; seqNo:5750; 4057534.20 north; 591316.28 east 2021-02-12T23:39:47Z- AHRS_M2 msg: 1613173187.30 epoch sec; seqNo:5700 2021-02-12T23:39:47Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.17 psi 2021-02-12T23:39:47Z- handleDepth()-Depth_Keller msg: 1613173187312 epoch sec; seqNo:5751; depth 9.10 2021-02-12T23:39:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173187.35 epoch sec; seqNo:5751; 4057534.20 north; 591316.28 east 2021-02-12T23:39:47Z- AHRS_M2 msg: 1613173187.71 epoch sec; seqNo:5701 2021-02-12T23:39:47Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.16 psi 2021-02-12T23:39:47Z- handleDepth()-Depth_Keller msg: 1613173187717 epoch sec; seqNo:5752; depth 8.97 2021-02-12T23:39:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173187.72 epoch sec; seqNo:5752; 4057534.59 north; 591317.50 east 2021-02-12T23:39:48Z- AHRS_M2 msg: 1613173188.12 epoch sec; seqNo:5702 2021-02-12T23:39:48Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.15 psi 2021-02-12T23:39:48Z- handleDepth()-Depth_Keller msg: 1613173188125 epoch sec; seqNo:5753; depth 8.86 2021-02-12T23:39:48Z- handleDvl() - RDI_Pathfinder msg: 1613173188.14 epoch sec; seqNo:2739 2021-02-12T23:39:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 105.08 , 122.66 , 80.75 2021-02-12T23:39:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.00 , 0.08 2021-02-12T23:39:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173188.16 epoch sec; seqNo:5753; 4057534.97 north; 591317.50 east 2021-02-12T23:39:48Z- AHRS_M2 msg: 1613173188.52 epoch sec; seqNo:5703 2021-02-12T23:39:48Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.15 psi 2021-02-12T23:39:48Z- handleDepth()-Depth_Keller msg: 1613173188529 epoch sec; seqNo:5754; depth 8.72 2021-02-12T23:39:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173188.54 epoch sec; seqNo:5754; 4057535.35 north; 591317.49 east 2021-02-12T23:39:48Z- AHRS_M2 msg: 1613173188.92 epoch sec; seqNo:5704 2021-02-12T23:39:48Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.15 psi 2021-02-12T23:39:48Z- handleDepth()-Depth_Keller msg: 1613173188929 epoch sec; seqNo:5755; depth 8.58 2021-02-12T23:39:48Z- handleDvl() - RDI_Pathfinder msg: 1613173188.95 epoch sec; seqNo:2740 2021-02-12T23:39:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.55 , 105.42 , 121.78 , 80.14 2021-02-12T23:39:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.07 , -0.01 2021-02-12T23:39:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173188.96 epoch sec; seqNo:5755; 4057535.35 north; 591317.49 east 2021-02-12T23:39:49Z- AHRS_M2 msg: 1613173189.32 epoch sec; seqNo:5705 2021-02-12T23:39:49Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.15 psi 2021-02-12T23:39:49Z- handleDepth()-Depth_Keller msg: 1613173189328 epoch sec; seqNo:5756; depth 8.46 2021-02-12T23:39:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173189.34 epoch sec; seqNo:5756; 4057535.73 north; 591317.49 east 2021-02-12T23:39:49Z- AHRS_M2 msg: 1613173189.73 epoch sec; seqNo:5706 2021-02-12T23:39:49Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.15 psi 2021-02-12T23:39:49Z- handleDepth()-Depth_Keller msg: 1613173189736 epoch sec; seqNo:5757; depth 8.33 2021-02-12T23:39:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173189.76 epoch sec; seqNo:5757; 4057536.11 north; 591317.48 east 2021-02-12T23:39:50Z- AHRS_M2 msg: 1613173190.14 epoch sec; seqNo:5707 2021-02-12T23:39:50Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.15 psi 2021-02-12T23:39:50Z- handleDepth()-Depth_Keller msg: 1613173190151 epoch sec; seqNo:5758; depth 8.19 2021-02-12T23:39:50Z- handleDvl() - RDI_Pathfinder msg: 1613173190.16 epoch sec; seqNo:2741 2021-02-12T23:39:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.34 , 106.56 , 124.14 , 80.88 2021-02-12T23:39:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.11 , 0.09 2021-02-12T23:39:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173190.17 epoch sec; seqNo:5758; 4057536.11 north; 591317.48 east 2021-02-12T23:39:50Z- AHRS_M2 msg: 1613173190.54 epoch sec; seqNo:5708 2021-02-12T23:39:50Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.16 psi 2021-02-12T23:39:50Z- handleDepth()-Depth_Keller msg: 1613173190540 epoch sec; seqNo:5759; depth 8.06 2021-02-12T23:39:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173190.55 epoch sec; seqNo:5759; 4057536.50 north; 591318.70 east 2021-02-12T23:39:50Z- AHRS_M2 msg: 1613173190.94 epoch sec; seqNo:5709 2021-02-12T23:39:50Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.15 psi 2021-02-12T23:39:50Z- handleDepth()-Depth_Keller msg: 1613173190945 epoch sec; seqNo:5760; depth 7.93 2021-02-12T23:39:50Z- handleDvl() - RDI_Pathfinder msg: 1613173190.96 epoch sec; seqNo:2742 2021-02-12T23:39:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.21 , 105.07 , 125.35 , 82.10 2021-02-12T23:39:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , 0.18 2021-02-12T23:39:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173190.97 epoch sec; seqNo:5760; 4057536.88 north; 591318.70 east 2021-02-12T23:39:51Z- AHRS_M2 msg: 1613173191.35 epoch sec; seqNo:5710 2021-02-12T23:39:51Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.15 psi 2021-02-12T23:39:51Z- handleDepth()-Depth_Keller msg: 1613173191349 epoch sec; seqNo:5761; depth 7.80 2021-02-12T23:39:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173191.36 epoch sec; seqNo:5761; 4057537.26 north; 591318.69 east 2021-02-12T23:39:51Z- AHRS_M2 msg: 1613173191.75 epoch sec; seqNo:5711 2021-02-12T23:39:51Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.15 psi 2021-02-12T23:39:51Z- handleDepth()-Depth_Keller msg: 1613173191752 epoch sec; seqNo:5762; depth 7.67 2021-02-12T23:39:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173191.77 epoch sec; seqNo:5762; 4057537.26 north; 591318.69 east 2021-02-12T23:39:51Z- LcmTrn::handleMessages() - handling 41 messages took 0.66 ms and 128 calls... 2021-02-12T23:39:51Z- waiting for 1613173189.94, time is 1613173191.77 2021-02-12T23:39:51Z- LcmTrn::updateTrn() >>>> pose time 1613173191.77, meas time 1613173190.96 <<<< 2021-02-12T23:39:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173190.96 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:39:51Z- TerrainNav::motionUpdate() - currEstimate = (4057537, 591318.69, 7.67) 2021-02-12T23:39:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:39:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:39:51Z- TNavBF::Projecting Measurements 2021-02-12T23:39:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:39:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:39:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:39:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:39:51Z- TNavFilter::4 good beams. 2021-02-12T23:39:51Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:39:51Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:39:51Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:39:51Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:39:51Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:39:51Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:39:51Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:39:51Z- TNavBF::mmse selected from filter -1 2021-02-12T23:39:51Z- TerrainMapOctree: (vehN, vehE ) = (4057536.68, 591318.29). 2021-02-12T23:39:51Z- TerrainMapOctree: Min Distance = 4731.14. 2021-02-12T23:39:51Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:39:51Z- There is no specific good beam that all particles have in common. 2021-02-12T23:39:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:39:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.42 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 13.02 2021-02-12T23:39:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:39:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 17.67 2021-02-12T23:39:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:39:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 25.14 2021-02-12T23:39:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:39:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 10.78 2021-02-12T23:39:52Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:39:52Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:39:52Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:39:52Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:39:52Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:39:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:39:52Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:39:52Z- Filter 0 No beams used in subcloud update 2021-02-12T23:39:52Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:39:52Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:39:52Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 746.2 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:39:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173190.96 sec. 2021-02-12T23:39:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:39:52Z- TNavBF::mmse selected from filter -1 2021-02-12T23:39:52Z- variances([0], [2], and psi_berg) = 746.135079 833.029534 0.000000 2021-02-12T23:39:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:39:52Z- LcmTrn::publishEstimates() - MLE : 4057524.76 591277.19 7.67 2021-02-12T23:39:52Z- LcmTrn::publishEstimates() - MMSE : 4057537.26 591318.69 7.67 2021-02-12T23:39:52Z- LcmTrn::publishEstimates() - COVAR: 746.14 833.03 1.10 0.00 2021-02-12T23:39:52Z- AHRS_M2 msg: 1613173192.15 epoch sec; seqNo:5712 2021-02-12T23:39:52Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.16 psi 2021-02-12T23:39:52Z- handleDepth()-Depth_Keller msg: 1613173192161 epoch sec; seqNo:5763; depth 7.55 2021-02-12T23:39:52Z- handleDvl() - RDI_Pathfinder msg: 1613173192.17 epoch sec; seqNo:2743 2021-02-12T23:39:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.96 , 105.07 , 125.35 , 82.10 2021-02-12T23:39:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.07 , 0.15 2021-02-12T23:39:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173192.18 epoch sec; seqNo:5763; 4057537.64 north; 591318.69 east 2021-02-12T23:39:52Z- AHRS_M2 msg: 1613173192.56 epoch sec; seqNo:5713 2021-02-12T23:39:52Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.17 psi 2021-02-12T23:39:52Z- handleDepth()-Depth_Keller msg: 1613173192569 epoch sec; seqNo:5764; depth 7.40 2021-02-12T23:39:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173192.58 epoch sec; seqNo:5764; 4057538.01 north; 591318.68 east 2021-02-12T23:39:52Z- AHRS_M2 msg: 1613173192.96 epoch sec; seqNo:5714 2021-02-12T23:39:52Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.17 psi 2021-02-12T23:39:52Z- handleDepth()-Depth_Keller msg: 1613173192964 epoch sec; seqNo:5765; depth 7.29 2021-02-12T23:39:52Z- handleDvl() - RDI_Pathfinder msg: 1613173192.98 epoch sec; seqNo:2744 2021-02-12T23:39:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.96 , 108.32 , 123.19 , 0.00 2021-02-12T23:39:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.11 , 0.11 2021-02-12T23:39:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173192.99 epoch sec; seqNo:5765; 4057538.41 north; 591319.90 east 2021-02-12T23:39:53Z- AHRS_M2 msg: 1613173193.36 epoch sec; seqNo:5715 2021-02-12T23:39:53Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.17 psi 2021-02-12T23:39:53Z- handleDepth()-Depth_Keller msg: 1613173193368 epoch sec; seqNo:5766; depth 7.15 2021-02-12T23:39:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173193.38 epoch sec; seqNo:5766; 4057538.41 north; 591319.90 east 2021-02-12T23:39:53Z- AHRS_M2 msg: 1613173193.77 epoch sec; seqNo:5716 2021-02-12T23:39:53Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.17 psi 2021-02-12T23:39:53Z- handleDepth()-Depth_Keller msg: 1613173193772 epoch sec; seqNo:5767; depth 7.03 2021-02-12T23:39:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173193.78 epoch sec; seqNo:5767; 4057538.79 north; 591319.90 east 2021-02-12T23:39:54Z- AHRS_M2 msg: 1613173194.17 epoch sec; seqNo:5717 2021-02-12T23:39:54Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.18 psi 2021-02-12T23:39:54Z- handleDepth()-Depth_Keller msg: 1613173194178 epoch sec; seqNo:5768; depth 6.90 2021-02-12T23:39:54Z- handleDvl() - RDI_Pathfinder msg: 1613173194.19 epoch sec; seqNo:2745 2021-02-12T23:39:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.44 , 107.85 , 128.66 , 82.57 2021-02-12T23:39:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , 0.03 2021-02-12T23:39:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173194.20 epoch sec; seqNo:5768; 4057539.16 north; 591319.89 east 2021-02-12T23:39:54Z- AHRS_M2 msg: 1613173194.58 epoch sec; seqNo:5718 2021-02-12T23:39:54Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.18 psi 2021-02-12T23:39:54Z- handleDepth()-Depth_Keller msg: 1613173194585 epoch sec; seqNo:5769; depth 6.77 2021-02-12T23:39:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173194.60 epoch sec; seqNo:5769; 4057539.16 north; 591319.89 east 2021-02-12T23:39:54Z- AHRS_M2 msg: 1613173194.98 epoch sec; seqNo:5719 2021-02-12T23:39:54Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.18 psi 2021-02-12T23:39:54Z- handleDepth()-Depth_Keller msg: 1613173194984 epoch sec; seqNo:5770; depth 6.68 2021-02-12T23:39:55Z- handleDvl() - RDI_Pathfinder msg: 1613173195.00 epoch sec; seqNo:2746 2021-02-12T23:39:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.35 , 111.17 , 127.52 , 82.91 2021-02-12T23:39:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , -0.07 2021-02-12T23:39:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173195.01 epoch sec; seqNo:5770; 4057539.56 north; 591321.11 east 2021-02-12T23:39:55Z- AHRS_M2 msg: 1613173195.38 epoch sec; seqNo:5720 2021-02-12T23:39:55Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-12T23:39:55Z- handleDepth()-Depth_Keller msg: 1613173195390 epoch sec; seqNo:5771; depth 6.54 2021-02-12T23:39:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173195.40 epoch sec; seqNo:5771; 4057539.93 north; 591321.11 east 2021-02-12T23:39:55Z- AHRS_M2 msg: 1613173195.79 epoch sec; seqNo:5721 2021-02-12T23:39:55Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.17 psi 2021-02-12T23:39:55Z- handleDepth()-Depth_Keller msg: 1613173195796 epoch sec; seqNo:5772; depth 6.43 2021-02-12T23:39:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173195.82 epoch sec; seqNo:5772; 4057540.31 north; 591321.10 east 2021-02-12T23:39:56Z- AHRS_M2 msg: 1613173196.19 epoch sec; seqNo:5722 2021-02-12T23:39:56Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.16 psi 2021-02-12T23:39:56Z- handleDepth()-Depth_Keller msg: 1613173196196 epoch sec; seqNo:5773; depth 6.34 2021-02-12T23:39:56Z- handleDvl() - RDI_Pathfinder msg: 1613173196.21 epoch sec; seqNo:2747 2021-02-12T23:39:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.90 , 109.75 , 124.62 , 82.98 2021-02-12T23:39:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , -0.13 2021-02-12T23:39:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173196.23 epoch sec; seqNo:5773; 4057540.31 north; 591321.10 east 2021-02-12T23:39:56Z- AHRS_M2 msg: 1613173196.60 epoch sec; seqNo:5723 2021-02-12T23:39:56Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.15 psi 2021-02-12T23:39:56Z- handleDepth()-Depth_Keller msg: 1613173196605 epoch sec; seqNo:5774; depth 6.24 2021-02-12T23:39:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173196.62 epoch sec; seqNo:5774; 4057540.69 north; 591321.10 east 2021-02-12T23:39:57Z- AHRS_M2 msg: 1613173197.00 epoch sec; seqNo:5724 2021-02-12T23:39:57Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.14 psi 2021-02-12T23:39:57Z- handleDepth()-Depth_Keller msg: 1613173197006 epoch sec; seqNo:5775; depth 6.15 2021-02-12T23:39:57Z- handleDvl() - RDI_Pathfinder msg: 1613173197.02 epoch sec; seqNo:2748 2021-02-12T23:39:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.46 , 106.84 , 126.17 , 83.04 2021-02-12T23:39:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.02 , -0.23 2021-02-12T23:39:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173197.03 epoch sec; seqNo:5775; 4057541.07 north; 591321.09 east 2021-02-12T23:39:57Z- LcmTrn::handleMessages() - handling 45 messages took 0.71 ms and 131 calls... 2021-02-12T23:39:57Z- waiting for 1613173194.77, time is 1613173197.03 2021-02-12T23:39:57Z- LcmTrn::updateTrn() equating pose 1613173197.03 and meas times 1613173197.03 2021-02-12T23:39:57Z- LcmTrn::updateTrn() >>>> pose time 1613173197.03, meas time 1613173197.03 <<<< 2021-02-12T23:39:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173197.03 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:39:57Z- TerrainNav::motionUpdate() - currEstimate = (4057541, 591321.09, 6.15) 2021-02-12T23:39:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:39:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:39:57Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173197.03 sec.; No relevant navigation data available 2021-02-12T23:39:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:39:57Z- TNavBF::mmse selected from filter -1 2021-02-12T23:39:57Z- variances([0], [2], and psi_berg) = 746.036864 833.029447 0.000000 2021-02-12T23:39:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:39:57Z- LcmTrn::publishEstimates() - MLE : 4057529.57 591279.59 6.15 2021-02-12T23:39:57Z- LcmTrn::publishEstimates() - MMSE : 4057541.07 591321.09 6.15 2021-02-12T23:39:57Z- LcmTrn::publishEstimates() - COVAR: 746.04 833.03 1.33 0.00 2021-02-12T23:39:57Z- AHRS_M2 msg: 1613173197.40 epoch sec; seqNo:5725 2021-02-12T23:39:57Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.15 psi 2021-02-12T23:39:57Z- handleDepth()-Depth_Keller msg: 1613173197408 epoch sec; seqNo:5776; depth 6.03 2021-02-12T23:39:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173197.43 epoch sec; seqNo:5776; 4057541.07 north; 591321.09 east 2021-02-12T23:39:57Z- AHRS_M2 msg: 1613173197.81 epoch sec; seqNo:5726 2021-02-12T23:39:57Z- AHRS_M2 msg: 0.65 phi; 0.30 theta; -0.17 psi 2021-02-12T23:39:57Z- handleDepth()-Depth_Keller msg: 1613173197812 epoch sec; seqNo:5777; depth 5.92 2021-02-12T23:39:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173197.83 epoch sec; seqNo:5777; 4057541.46 north; 591322.31 east 2021-02-12T23:39:58Z- AHRS_M2 msg: 1613173198.21 epoch sec; seqNo:5727 2021-02-12T23:39:58Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.19 psi 2021-02-12T23:39:58Z- handleDepth()-Depth_Keller msg: 1613173198221 epoch sec; seqNo:5778; depth 5.82 2021-02-12T23:39:58Z- handleDvl() - RDI_Pathfinder msg: 1613173198.23 epoch sec; seqNo:2749 2021-02-12T23:39:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 108.33 , 121.71 , 84.53 2021-02-12T23:39:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.58 , -0.21 , -0.21 2021-02-12T23:39:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173198.24 epoch sec; seqNo:5778; 4057541.46 north; 591322.31 east 2021-02-12T23:39:58Z- AHRS_M2 msg: 1613173198.62 epoch sec; seqNo:5728 2021-02-12T23:39:58Z- AHRS_M2 msg: 0.63 phi; 0.28 theta; -0.21 psi 2021-02-12T23:39:58Z- handleDepth()-Depth_Keller msg: 1613173198622 epoch sec; seqNo:5779; depth 5.67 2021-02-12T23:39:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173198.63 epoch sec; seqNo:5779; 4057541.84 north; 591322.31 east 2021-02-12T23:39:59Z- AHRS_M2 msg: 1613173199.02 epoch sec; seqNo:5729 2021-02-12T23:39:59Z- AHRS_M2 msg: 0.63 phi; 0.26 theta; -0.22 psi 2021-02-12T23:39:59Z- handleDepth()-Depth_Keller msg: 1613173199024 epoch sec; seqNo:5780; depth 5.56 2021-02-12T23:39:59Z- handleDvl() - RDI_Pathfinder msg: 1613173199.04 epoch sec; seqNo:2750 2021-02-12T23:39:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 111.23 , 118.67 , 84.46 2021-02-12T23:39:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.31 , 0.08 2021-02-12T23:39:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173199.05 epoch sec; seqNo:5780; 4057542.22 north; 591322.30 east 2021-02-12T23:39:59Z- AHRS_M2 msg: 1613173199.42 epoch sec; seqNo:5730 2021-02-12T23:39:59Z- AHRS_M2 msg: 0.63 phi; 0.25 theta; -0.22 psi 2021-02-12T23:39:59Z- handleDepth()-Depth_Keller msg: 1613173199429 epoch sec; seqNo:5781; depth 5.41 2021-02-12T23:39:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173199.46 epoch sec; seqNo:5781; 4057542.60 north; 591322.30 east 2021-02-12T23:39:59Z- AHRS_M2 msg: 1613173199.86 epoch sec; seqNo:5731 2021-02-12T23:39:59Z- AHRS_M2 msg: 0.63 phi; 0.22 theta; -0.20 psi 2021-02-12T23:39:59Z- handleDepth()-Depth_Keller msg: 1613173199860 epoch sec; seqNo:5782; depth 5.32 2021-02-12T23:39:59Z- handleDvl() - RDI_Pathfinder msg: 1613173199.88 epoch sec; seqNo:2751 2021-02-12T23:39:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 111.23 , 112.72 , 0.00 2021-02-12T23:39:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.07 , 0.20 2021-02-12T23:39:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173199.89 epoch sec; seqNo:5782; 4057542.60 north; 591322.30 east 2021-02-12T23:40:00Z- AHRS_M2 msg: 1613173200.23 epoch sec; seqNo:5732 2021-02-12T23:40:00Z- AHRS_M2 msg: 0.64 phi; 0.19 theta; -0.18 psi 2021-02-12T23:40:00Z- handleDepth()-Depth_Keller msg: 1613173200238 epoch sec; seqNo:5783; depth 5.19 2021-02-12T23:40:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173200.25 epoch sec; seqNo:5783; 4057542.98 north; 591322.29 east 2021-02-12T23:40:00Z- AHRS_M2 msg: 1613173200.63 epoch sec; seqNo:5733 2021-02-12T23:40:00Z- AHRS_M2 msg: 0.66 phi; 0.15 theta; -0.17 psi 2021-02-12T23:40:00Z- handleDepth()-Depth_Keller msg: 1613173200646 epoch sec; seqNo:5784; depth 5.12 2021-02-12T23:40:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173200.65 epoch sec; seqNo:5784; 4057543.36 north; 591322.29 east 2021-02-12T23:40:01Z- AHRS_M2 msg: 1613173201.04 epoch sec; seqNo:5734 2021-02-12T23:40:01Z- AHRS_M2 msg: 0.68 phi; 0.10 theta; -0.17 psi 2021-02-12T23:40:01Z- handleDepth()-Depth_Keller msg: 1613173201049 epoch sec; seqNo:5785; depth 5.00 2021-02-12T23:40:01Z- handleDvl() - RDI_Pathfinder msg: 1613173201.07 epoch sec; seqNo:2752 2021-02-12T23:40:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.58 , 104.94 , 106.43 , 88.58 2021-02-12T23:40:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , 0.02 2021-02-12T23:40:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173201.08 epoch sec; seqNo:5785; 4057543.74 north; 591322.29 east 2021-02-12T23:40:01Z- AHRS_M2 msg: 1613173201.44 epoch sec; seqNo:5735 2021-02-12T23:40:01Z- AHRS_M2 msg: 0.69 phi; 0.05 theta; -0.17 psi 2021-02-12T23:40:01Z- handleDepth()-Depth_Keller msg: 1613173201448 epoch sec; seqNo:5786; depth 4.91 2021-02-12T23:40:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173201.46 epoch sec; seqNo:5786; 4057543.75 north; 591323.51 east 2021-02-12T23:40:01Z- AHRS_M2 msg: 1613173201.86 epoch sec; seqNo:5736 2021-02-12T23:40:01Z- AHRS_M2 msg: 0.70 phi; 0.01 theta; -0.17 psi 2021-02-12T23:40:01Z- handleDepth()-Depth_Keller msg: 1613173201862 epoch sec; seqNo:5787; depth 4.84 2021-02-12T23:40:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173201.87 epoch sec; seqNo:5787; 4057544.13 north; 591323.50 east 2021-02-12T23:40:02Z- LcmTrn::handleMessages() - handling 40 messages took 0.69 ms and 126 calls... 2021-02-12T23:40:02Z- waiting for 1613173200.03, time is 1613173201.87 2021-02-12T23:40:02Z- LcmTrn::updateTrn() >>>> pose time 1613173201.87, meas time 1613173201.07 <<<< 2021-02-12T23:40:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173201.07 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:40:02Z- TerrainNav::motionUpdate() - currEstimate = (4057544, 591323.50, 4.84) 2021-02-12T23:40:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:40:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:40:02Z- TNavBF::Projecting Measurements 2021-02-12T23:40:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:40:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:40:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:40:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:40:02Z- TNavFilter::4 good beams. 2021-02-12T23:40:02Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:40:02Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:40:02Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:40:02Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:40:02Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:40:02Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:40:02Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:40:02Z- TNavBF::mmse selected from filter -1 2021-02-12T23:40:02Z- TerrainMapOctree: (vehN, vehE ) = (4057543.62, 591323.10). 2021-02-12T23:40:02Z- TerrainMapOctree: Min Distance = 4722.71. 2021-02-12T23:40:02Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:40:02Z- There is no specific good beam that all particles have in common. 2021-02-12T23:40:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:40:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.49 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 12.56 2021-02-12T23:40:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:40:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.35 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 17.62 2021-02-12T23:40:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:40:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 18.12 2021-02-12T23:40:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:40:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 12.56 2021-02-12T23:40:02Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:40:02Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:40:02Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:40:02Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:40:02Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:40:02Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:40:02Z- Filter 0 No beams used in subcloud update 2021-02-12T23:40:02Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:40:02Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:40:02Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 746.0 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:40:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173201.07 sec. 2021-02-12T23:40:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:40:02Z- TNavBF::mmse selected from filter -1 2021-02-12T23:40:02Z- variances([0], [2], and psi_berg) = 745.916526 833.029341 0.000000 2021-02-12T23:40:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:40:02Z- LcmTrn::publishEstimates() - MLE : 4057532.63 591282.00 4.84 2021-02-12T23:40:02Z- LcmTrn::publishEstimates() - MMSE : 4057544.13 591323.50 4.84 2021-02-12T23:40:02Z- LcmTrn::publishEstimates() - COVAR: 745.92 833.03 1.13 0.00 2021-02-12T23:40:02Z- AHRS_M2 msg: 1613173202.25 epoch sec; seqNo:5737 2021-02-12T23:40:02Z- AHRS_M2 msg: 0.71 phi; -0.04 theta; -0.19 psi 2021-02-12T23:40:02Z- handleDepth()-Depth_Keller msg: 1613173202257 epoch sec; seqNo:5788; depth 4.78 2021-02-12T23:40:02Z- handleDvl() - RDI_Pathfinder msg: 1613173202.28 epoch sec; seqNo:2753 2021-02-12T23:40:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.84 , 104.07 , 95.96 , 97.31 2021-02-12T23:40:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.16 , 0.09 2021-02-12T23:40:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173202.29 epoch sec; seqNo:5788; 4057544.51 north; 591323.50 east 2021-02-12T23:40:02Z- AHRS_M2 msg: 1613173202.65 epoch sec; seqNo:5738 2021-02-12T23:40:02Z- AHRS_M2 msg: 0.72 phi; -0.08 theta; -0.19 psi 2021-02-12T23:40:02Z- handleDepth()-Depth_Keller msg: 1613173202660 epoch sec; seqNo:5789; depth 4.74 2021-02-12T23:40:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173202.67 epoch sec; seqNo:5789; 4057544.51 north; 591323.50 east 2021-02-12T23:40:03Z- AHRS_M2 msg: 1613173203.06 epoch sec; seqNo:5739 2021-02-12T23:40:03Z- AHRS_M2 msg: 0.73 phi; -0.12 theta; -0.17 psi 2021-02-12T23:40:03Z- handleDepth()-Depth_Keller msg: 1613173203064 epoch sec; seqNo:5790; depth 4.74 2021-02-12T23:40:03Z- handleDvl() - RDI_Pathfinder msg: 1613173203.08 epoch sec; seqNo:2754 2021-02-12T23:40:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.91 , 106.84 , 91.97 , 100.08 2021-02-12T23:40:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.08 , 0.27 2021-02-12T23:40:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173203.09 epoch sec; seqNo:5790; 4057544.89 north; 591323.49 east 2021-02-12T23:40:03Z- AHRS_M2 msg: 1613173203.47 epoch sec; seqNo:5740 2021-02-12T23:40:03Z- AHRS_M2 msg: 0.73 phi; -0.15 theta; -0.14 psi 2021-02-12T23:40:03Z- handleDepth()-Depth_Keller msg: 1613173203474 epoch sec; seqNo:5791; depth 4.73 2021-02-12T23:40:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173203.49 epoch sec; seqNo:5791; 4057545.26 north; 591323.49 east 2021-02-12T23:40:03Z- AHRS_M2 msg: 1613173203.87 epoch sec; seqNo:5741 2021-02-12T23:40:03Z- AHRS_M2 msg: 0.72 phi; -0.18 theta; -0.13 psi 2021-02-12T23:40:03Z- handleDepth()-Depth_Keller msg: 1613173203872 epoch sec; seqNo:5792; depth 4.76 2021-02-12T23:40:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173203.89 epoch sec; seqNo:5792; 4057545.26 north; 591323.49 east 2021-02-12T23:40:04Z- AHRS_M2 msg: 1613173204.27 epoch sec; seqNo:5742 2021-02-12T23:40:04Z- AHRS_M2 msg: 0.71 phi; -0.21 theta; -0.15 psi 2021-02-12T23:40:04Z- handleDepth()-Depth_Keller msg: 1613173204308 epoch sec; seqNo:5793; depth 4.75 2021-02-12T23:40:04Z- handleDvl() - RDI_Pathfinder msg: 1613173204.32 epoch sec; seqNo:2755 2021-02-12T23:40:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.19 , 102.71 , 87.84 , 105.41 2021-02-12T23:40:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , 0.10 , 0.15 2021-02-12T23:40:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173204.33 epoch sec; seqNo:5793; 4057545.66 north; 591324.71 east 2021-02-12T23:40:04Z- AHRS_M2 msg: 1613173204.68 epoch sec; seqNo:5743 2021-02-12T23:40:04Z- AHRS_M2 msg: 0.70 phi; -0.23 theta; -0.18 psi 2021-02-12T23:40:04Z- handleDepth()-Depth_Keller msg: 1613173204680 epoch sec; seqNo:5794; depth 4.80 2021-02-12T23:40:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173204.69 epoch sec; seqNo:5794; 4057545.66 north; 591324.71 east 2021-02-12T23:40:05Z- AHRS_M2 msg: 1613173205.09 epoch sec; seqNo:5744 2021-02-12T23:40:05Z- AHRS_M2 msg: 0.69 phi; -0.26 theta; -0.26 psi 2021-02-12T23:40:05Z- handleDepth()-Depth_Keller msg: 1613173205092 epoch sec; seqNo:5795; depth 4.84 2021-02-12T23:40:05Z- handleDvl() - RDI_Pathfinder msg: 1613173205.10 epoch sec; seqNo:2756 2021-02-12T23:40:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.37 , 106.83 , 86.56 , 112.24 2021-02-12T23:40:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.14 , -0.04 2021-02-12T23:40:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173205.12 epoch sec; seqNo:5795; 4057546.04 north; 591324.70 east 2021-02-12T23:40:05Z- AHRS_M2 msg: 1613173205.49 epoch sec; seqNo:5745 2021-02-12T23:40:05Z- AHRS_M2 msg: 0.69 phi; -0.28 theta; -0.29 psi 2021-02-12T23:40:05Z- handleDepth()-Depth_Keller msg: 1613173205498 epoch sec; seqNo:5796; depth 4.89 2021-02-12T23:40:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173205.51 epoch sec; seqNo:5796; 4057546.04 north; 591324.70 east 2021-02-12T23:40:05Z- AHRS_M2 msg: 1613173205.93 epoch sec; seqNo:5746 2021-02-12T23:40:05Z- AHRS_M2 msg: 0.69 phi; -0.29 theta; -0.30 psi 2021-02-12T23:40:05Z- handleDepth()-Depth_Keller msg: 1613173205946 epoch sec; seqNo:5797; depth 4.95 2021-02-12T23:40:05Z- handleDvl() - RDI_Pathfinder msg: 1613173205.97 epoch sec; seqNo:2757 2021-02-12T23:40:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.51 , 117.25 , 0.00 , 118.60 2021-02-12T23:40:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.06 , -0.08 2021-02-12T23:40:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173205.99 epoch sec; seqNo:5797; 4057546.41 north; 591324.70 east 2021-02-12T23:40:06Z- AHRS_M2 msg: 1613173206.29 epoch sec; seqNo:5747 2021-02-12T23:40:06Z- AHRS_M2 msg: 0.69 phi; -0.30 theta; -0.31 psi 2021-02-12T23:40:06Z- handleDepth()-Depth_Keller msg: 1613173206296 epoch sec; seqNo:5798; depth 5.04 2021-02-12T23:40:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173206.31 epoch sec; seqNo:5798; 4057546.81 north; 591325.92 east 2021-02-12T23:40:06Z- AHRS_M2 msg: 1613173206.70 epoch sec; seqNo:5748 2021-02-12T23:40:06Z- AHRS_M2 msg: 0.69 phi; -0.32 theta; -0.29 psi 2021-02-12T23:40:06Z- handleDepth()-Depth_Keller msg: 1613173206702 epoch sec; seqNo:5799; depth 5.12 2021-02-12T23:40:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173206.71 epoch sec; seqNo:5799; 4057546.81 north; 591325.92 east 2021-02-12T23:40:07Z- AHRS_M2 msg: 1613173207.10 epoch sec; seqNo:5749 2021-02-12T23:40:07Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.28 psi 2021-02-12T23:40:07Z- handleDepth()-Depth_Keller msg: 1613173207104 epoch sec; seqNo:5800; depth 5.22 2021-02-12T23:40:07Z- handleDvl() - RDI_Pathfinder msg: 1613173207.13 epoch sec; seqNo:2758 2021-02-12T23:40:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.03 , 122.72 , 88.52 , 125.70 2021-02-12T23:40:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , -0.02 2021-02-12T23:40:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173207.14 epoch sec; seqNo:5800; 4057547.18 north; 591325.91 east 2021-02-12T23:40:07Z- LcmTrn::handleMessages() - handling 45 messages took 0.82 ms and 129 calls... 2021-02-12T23:40:07Z- waiting for 1613173204.87, time is 1613173207.14 2021-02-12T23:40:07Z- LcmTrn::updateTrn() equating pose 1613173207.14 and meas times 1613173207.14 2021-02-12T23:40:07Z- LcmTrn::updateTrn() >>>> pose time 1613173207.14, meas time 1613173207.14 <<<< 2021-02-12T23:40:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173207.14 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:40:07Z- TerrainNav::motionUpdate() - currEstimate = (4057547, 591325.91, 5.22) 2021-02-12T23:40:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:40:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:40:07Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173207.14 sec.; No relevant navigation data available 2021-02-12T23:40:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:40:07Z- TNavBF::mmse selected from filter -1 2021-02-12T23:40:07Z- variances([0], [2], and psi_berg) = 745.825518 833.029261 0.000000 2021-02-12T23:40:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:40:07Z- LcmTrn::publishEstimates() - MLE : 4057535.68 591284.41 5.22 2021-02-12T23:40:07Z- LcmTrn::publishEstimates() - MMSE : 4057547.18 591325.91 5.22 2021-02-12T23:40:07Z- LcmTrn::publishEstimates() - COVAR: 745.83 833.03 1.33 0.00 2021-02-12T23:40:07Z- AHRS_M2 msg: 1613173207.50 epoch sec; seqNo:5750 2021-02-12T23:40:07Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.27 psi 2021-02-12T23:40:07Z- handleDepth()-Depth_Keller msg: 1613173207508 epoch sec; seqNo:5801; depth 5.34 2021-02-12T23:40:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173207.52 epoch sec; seqNo:5801; 4057547.56 north; 591325.91 east 2021-02-12T23:40:07Z- AHRS_M2 msg: 1613173207.91 epoch sec; seqNo:5751 2021-02-12T23:40:07Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.26 psi 2021-02-12T23:40:07Z- handleDepth()-Depth_Keller msg: 1613173207914 epoch sec; seqNo:5802; depth 5.44 2021-02-12T23:40:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173207.92 epoch sec; seqNo:5802; 4057547.56 north; 591325.91 east 2021-02-12T23:40:08Z- AHRS_M2 msg: 1613173208.31 epoch sec; seqNo:5752 2021-02-12T23:40:08Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.24 psi 2021-02-12T23:40:08Z- handleDepth()-Depth_Keller msg: 1613173208317 epoch sec; seqNo:5803; depth 5.54 2021-02-12T23:40:08Z- handleDvl() - RDI_Pathfinder msg: 1613173208.33 epoch sec; seqNo:2759 2021-02-12T23:40:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.65 , 119.88 , 0.00 , 127.31 2021-02-12T23:40:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , 0.03 2021-02-12T23:40:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173208.34 epoch sec; seqNo:5803; 4057547.94 north; 591325.90 east 2021-02-12T23:40:08Z- AHRS_M2 msg: 1613173208.72 epoch sec; seqNo:5753 2021-02-12T23:40:08Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.23 psi 2021-02-12T23:40:08Z- handleDepth()-Depth_Keller msg: 1613173208720 epoch sec; seqNo:5804; depth 5.68 2021-02-12T23:40:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173208.74 epoch sec; seqNo:5804; 4057548.33 north; 591327.12 east 2021-02-12T23:40:09Z- AHRS_M2 msg: 1613173209.12 epoch sec; seqNo:5754 2021-02-12T23:40:09Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.22 psi 2021-02-12T23:40:09Z- handleDepth()-Depth_Keller msg: 1613173209128 epoch sec; seqNo:5805; depth 5.80 2021-02-12T23:40:09Z- handleDvl() - RDI_Pathfinder msg: 1613173209.15 epoch sec; seqNo:2760 2021-02-12T23:40:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.80 , 116.57 , 0.00 , 129.95 2021-02-12T23:40:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , 0.00 2021-02-12T23:40:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173209.16 epoch sec; seqNo:5805; 4057548.33 north; 591327.12 east 2021-02-12T23:40:09Z- AHRS_M2 msg: 1613173209.52 epoch sec; seqNo:5755 2021-02-12T23:40:09Z- AHRS_M2 msg: 0.69 phi; -0.36 theta; -0.21 psi 2021-02-12T23:40:09Z- handleDepth()-Depth_Keller msg: 1613173209533 epoch sec; seqNo:5806; depth 5.93 2021-02-12T23:40:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173209.54 epoch sec; seqNo:5806; 4057548.71 north; 591327.12 east 2021-02-12T23:40:09Z- AHRS_M2 msg: 1613173209.93 epoch sec; seqNo:5756 2021-02-12T23:40:09Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.20 psi 2021-02-12T23:40:09Z- handleDepth()-Depth_Keller msg: 1613173209934 epoch sec; seqNo:5807; depth 6.07 2021-02-12T23:40:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173209.94 epoch sec; seqNo:5807; 4057549.09 north; 591327.11 east 2021-02-12T23:40:10Z- AHRS_M2 msg: 1613173210.33 epoch sec; seqNo:5757 2021-02-12T23:40:10Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.19 psi 2021-02-12T23:40:10Z- handleDepth()-Depth_Keller msg: 1613173210337 epoch sec; seqNo:5808; depth 6.19 2021-02-12T23:40:10Z- handleDvl() - RDI_Pathfinder msg: 1613173210.36 epoch sec; seqNo:2761 2021-02-12T23:40:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.29 , 113.59 , 83.85 , 132.92 2021-02-12T23:40:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.06 , -0.13 2021-02-12T23:40:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173210.37 epoch sec; seqNo:5808; 4057549.09 north; 591327.11 east 2021-02-12T23:40:10Z- AHRS_M2 msg: 1613173210.73 epoch sec; seqNo:5758 2021-02-12T23:40:10Z- AHRS_M2 msg: 0.68 phi; -0.38 theta; -0.19 psi 2021-02-12T23:40:10Z- handleDepth()-Depth_Keller msg: 1613173210745 epoch sec; seqNo:5809; depth 6.35 2021-02-12T23:40:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173210.76 epoch sec; seqNo:5809; 4057549.47 north; 591327.11 east 2021-02-12T23:40:11Z- AHRS_M2 msg: 1613173211.14 epoch sec; seqNo:5759 2021-02-12T23:40:11Z- AHRS_M2 msg: 0.67 phi; -0.38 theta; -0.19 psi 2021-02-12T23:40:11Z- handleDepth()-Depth_Keller msg: 1613173211148 epoch sec; seqNo:5810; depth 6.47 2021-02-12T23:40:11Z- handleDvl() - RDI_Pathfinder msg: 1613173211.16 epoch sec; seqNo:2762 2021-02-12T23:40:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.69 , 111.02 , 82.77 , 128.87 2021-02-12T23:40:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.18 , -0.16 2021-02-12T23:40:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173211.17 epoch sec; seqNo:5810; 4057549.85 north; 591327.11 east 2021-02-12T23:40:11Z- AHRS_M2 msg: 1613173211.54 epoch sec; seqNo:5760 2021-02-12T23:40:11Z- AHRS_M2 msg: 0.66 phi; -0.38 theta; -0.21 psi 2021-02-12T23:40:11Z- handleDepth()-Depth_Keller msg: 1613173211550 epoch sec; seqNo:5811; depth 6.62 2021-02-12T23:40:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173211.57 epoch sec; seqNo:5811; 4057549.86 north; 591328.33 east 2021-02-12T23:40:11Z- AHRS_M2 msg: 1613173211.95 epoch sec; seqNo:5761 2021-02-12T23:40:11Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.21 psi 2021-02-12T23:40:11Z- handleDepth()-Depth_Keller msg: 1613173211957 epoch sec; seqNo:5812; depth 6.75 2021-02-12T23:40:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173211.98 epoch sec; seqNo:5812; 4057550.24 north; 591328.32 east 2021-02-12T23:40:12Z- LcmTrn::handleMessages() - handling 40 messages took 0.63 ms and 128 calls... 2021-02-12T23:40:12Z- waiting for 1613173210.14, time is 1613173211.98 2021-02-12T23:40:12Z- LcmTrn::updateTrn() >>>> pose time 1613173211.98, meas time 1613173211.16 <<<< 2021-02-12T23:40:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173211.16 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:40:12Z- TerrainNav::motionUpdate() - currEstimate = (4057550, 591328.32, 6.75) 2021-02-12T23:40:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:40:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:40:12Z- TNavBF::Projecting Measurements 2021-02-12T23:40:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:40:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:40:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:40:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:40:12Z- TNavFilter::4 good beams. 2021-02-12T23:40:12Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:40:12Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:40:12Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:40:12Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:40:12Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:40:12Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:40:12Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:40:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:40:12Z- TerrainMapOctree: (vehN, vehE ) = (4057549.73, 591327.92). 2021-02-12T23:40:12Z- TerrainMapOctree: Min Distance = 4714.94. 2021-02-12T23:40:12Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:40:12Z- There is no specific good beam that all particles have in common. 2021-02-12T23:40:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:40:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.49 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 13.45 2021-02-12T23:40:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:40:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.36 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 19.72 2021-02-12T23:40:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:40:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 10.96 2021-02-12T23:40:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:40:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 26.57 2021-02-12T23:40:12Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:40:12Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:40:12Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:40:12Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:40:12Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:40:12Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:40:12Z- Filter 0 No beams used in subcloud update 2021-02-12T23:40:12Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:40:12Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:40:12Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 745.7 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:40:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173211.16 sec. 2021-02-12T23:40:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:40:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:40:12Z- variances([0], [2], and psi_berg) = 745.705333 833.029155 0.000000 2021-02-12T23:40:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:40:12Z- LcmTrn::publishEstimates() - MLE : 4057538.74 591288.82 6.75 2021-02-12T23:40:12Z- LcmTrn::publishEstimates() - MMSE : 4057550.24 591328.32 6.75 2021-02-12T23:40:12Z- LcmTrn::publishEstimates() - COVAR: 745.71 833.03 1.15 0.00 2021-02-12T23:40:12Z- AHRS_M2 msg: 1613173212.36 epoch sec; seqNo:5762 2021-02-12T23:40:12Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.22 psi 2021-02-12T23:40:12Z- handleDepth()-Depth_Keller msg: 1613173212365 epoch sec; seqNo:5813; depth 6.90 2021-02-12T23:40:12Z- handleDvl() - RDI_Pathfinder msg: 1613173212.38 epoch sec; seqNo:2763 2021-02-12T23:40:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.86 , 112.65 , 81.42 , 130.50 2021-02-12T23:40:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.23 , -0.01 2021-02-12T23:40:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173212.40 epoch sec; seqNo:5813; 4057550.62 north; 591328.32 east 2021-02-12T23:40:12Z- AHRS_M2 msg: 1613173212.76 epoch sec; seqNo:5763 2021-02-12T23:40:12Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.23 psi 2021-02-12T23:40:12Z- handleDepth()-Depth_Keller msg: 1613173212765 epoch sec; seqNo:5814; depth 7.05 2021-02-12T23:40:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173212.78 epoch sec; seqNo:5814; 4057551.00 north; 591328.31 east 2021-02-12T23:40:13Z- AHRS_M2 msg: 1613173213.16 epoch sec; seqNo:5764 2021-02-12T23:40:13Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.23 psi 2021-02-12T23:40:13Z- handleDepth()-Depth_Keller msg: 1613173213173 epoch sec; seqNo:5815; depth 7.18 2021-02-12T23:40:13Z- handleDvl() - RDI_Pathfinder msg: 1613173213.18 epoch sec; seqNo:2764 2021-02-12T23:40:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.41 , 114.21 , 81.49 , 129.08 2021-02-12T23:40:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.14 , 0.08 2021-02-12T23:40:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173213.19 epoch sec; seqNo:5815; 4057551.38 north; 591328.31 east 2021-02-12T23:40:13Z- AHRS_M2 msg: 1613173213.56 epoch sec; seqNo:5765 2021-02-12T23:40:13Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.23 psi 2021-02-12T23:40:13Z- handleDepth()-Depth_Keller msg: 1613173213568 epoch sec; seqNo:5816; depth 7.32 2021-02-12T23:40:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173213.59 epoch sec; seqNo:5816; 4057551.38 north; 591328.31 east 2021-02-12T23:40:13Z- AHRS_M2 msg: 1613173213.97 epoch sec; seqNo:5766 2021-02-12T23:40:13Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.22 psi 2021-02-12T23:40:13Z- handleDepth()-Depth_Keller msg: 1613173213972 epoch sec; seqNo:5817; depth 7.48 2021-02-12T23:40:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173213.98 epoch sec; seqNo:5817; 4057551.76 north; 591328.31 east 2021-02-12T23:40:14Z- AHRS_M2 msg: 1613173214.37 epoch sec; seqNo:5767 2021-02-12T23:40:14Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.21 psi 2021-02-12T23:40:14Z- handleDepth()-Depth_Keller msg: 1613173214376 epoch sec; seqNo:5818; depth 7.61 2021-02-12T23:40:14Z- handleDvl() - RDI_Pathfinder msg: 1613173214.39 epoch sec; seqNo:2765 2021-02-12T23:40:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.86 , 112.65 , 82.91 , 124.55 2021-02-12T23:40:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.02 2021-02-12T23:40:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173214.40 epoch sec; seqNo:5818; 4057552.14 north; 591328.30 east 2021-02-12T23:40:14Z- AHRS_M2 msg: 1613173214.78 epoch sec; seqNo:5768 2021-02-12T23:40:14Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.20 psi 2021-02-12T23:40:14Z- handleDepth()-Depth_Keller msg: 1613173214782 epoch sec; seqNo:5819; depth 7.76 2021-02-12T23:40:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173214.79 epoch sec; seqNo:5819; 4057552.51 north; 591328.30 east 2021-02-12T23:40:15Z- AHRS_M2 msg: 1613173215.18 epoch sec; seqNo:5769 2021-02-12T23:40:15Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.18 psi 2021-02-12T23:40:15Z- handleDepth()-Depth_Keller msg: 1613173215184 epoch sec; seqNo:5820; depth 7.90 2021-02-12T23:40:15Z- handleDvl() - RDI_Pathfinder msg: 1613173215.20 epoch sec; seqNo:2766 2021-02-12T23:40:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 111.30 , 81.56 , 121.71 2021-02-12T23:40:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , -0.06 2021-02-12T23:40:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173215.21 epoch sec; seqNo:5820; 4057552.53 north; 591329.52 east 2021-02-12T23:40:15Z- AHRS_M2 msg: 1613173215.58 epoch sec; seqNo:5770 2021-02-12T23:40:15Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.17 psi 2021-02-12T23:40:15Z- handleDepth()-Depth_Keller msg: 1613173215593 epoch sec; seqNo:5821; depth 8.04 2021-02-12T23:40:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173215.60 epoch sec; seqNo:5821; 4057552.91 north; 591329.51 east 2021-02-12T23:40:15Z- AHRS_M2 msg: 1613173215.99 epoch sec; seqNo:5771 2021-02-12T23:40:15Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.17 psi 2021-02-12T23:40:15Z- handleDepth()-Depth_Keller msg: 1613173215994 epoch sec; seqNo:5822; depth 8.19 2021-02-12T23:40:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173216.00 epoch sec; seqNo:5822; 4057553.29 north; 591329.51 east 2021-02-12T23:40:16Z- AHRS_M2 msg: 1613173216.39 epoch sec; seqNo:5772 2021-02-12T23:40:16Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.17 psi 2021-02-12T23:40:16Z- handleDepth()-Depth_Keller msg: 1613173216397 epoch sec; seqNo:5823; depth 8.32 2021-02-12T23:40:16Z- handleDvl() - RDI_Pathfinder msg: 1613173216.41 epoch sec; seqNo:2767 2021-02-12T23:40:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.92 , 106.76 , 81.49 , 120.15 2021-02-12T23:40:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , 0.00 2021-02-12T23:40:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173216.43 epoch sec; seqNo:5823; 4057553.66 north; 591329.51 east 2021-02-12T23:40:16Z- AHRS_M2 msg: 1613173216.80 epoch sec; seqNo:5773 2021-02-12T23:40:16Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.17 psi 2021-02-12T23:40:16Z- handleDepth()-Depth_Keller msg: 1613173216805 epoch sec; seqNo:5824; depth 8.47 2021-02-12T23:40:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173216.82 epoch sec; seqNo:5824; 4057554.04 north; 591329.50 east 2021-02-12T23:40:17Z- AHRS_M2 msg: 1613173217.21 epoch sec; seqNo:5774 2021-02-12T23:40:17Z- AHRS_M2 msg: 0.57 phi; -0.31 theta; -0.16 psi 2021-02-12T23:40:17Z- handleDepth()-Depth_Keller msg: 1613173217212 epoch sec; seqNo:5825; depth 8.62 2021-02-12T23:40:17Z- handleDvl() - RDI_Pathfinder msg: 1613173217.23 epoch sec; seqNo:2768 2021-02-12T23:40:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 105.21 , 80.88 , 117.38 2021-02-12T23:40:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , 0.07 2021-02-12T23:40:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173217.24 epoch sec; seqNo:5825; 4057554.04 north; 591329.50 east 2021-02-12T23:40:17Z- LcmTrn::handleMessages() - handling 45 messages took 0.75 ms and 130 calls... 2021-02-12T23:40:17Z- waiting for 1613173214.98, time is 1613173217.24 2021-02-12T23:40:17Z- LcmTrn::updateTrn() equating pose 1613173217.24 and meas times 1613173217.24 2021-02-12T23:40:17Z- LcmTrn::updateTrn() >>>> pose time 1613173217.24, meas time 1613173217.24 <<<< 2021-02-12T23:40:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173217.24 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:40:17Z- TerrainNav::motionUpdate() - currEstimate = (4057554, 591329.50, 8.62) 2021-02-12T23:40:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:40:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:40:17Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173217.24 sec.; No relevant navigation data available 2021-02-12T23:40:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:40:17Z- TNavBF::mmse selected from filter -1 2021-02-12T23:40:17Z- variances([0], [2], and psi_berg) = 745.613549 833.029074 0.000000 2021-02-12T23:40:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:40:17Z- LcmTrn::publishEstimates() - MLE : 4057542.54 591290.00 8.62 2021-02-12T23:40:17Z- LcmTrn::publishEstimates() - MMSE : 4057554.04 591329.50 8.62 2021-02-12T23:40:17Z- LcmTrn::publishEstimates() - COVAR: 745.61 833.03 1.35 0.00 2021-02-12T23:40:17Z- AHRS_M2 msg: 1613173217.60 epoch sec; seqNo:5775 2021-02-12T23:40:17Z- AHRS_M2 msg: 0.57 phi; -0.30 theta; -0.14 psi 2021-02-12T23:40:17Z- handleDepth()-Depth_Keller msg: 1613173217608 epoch sec; seqNo:5826; depth 8.77 2021-02-12T23:40:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173217.63 epoch sec; seqNo:5826; 4057554.42 north; 591329.50 east 2021-02-12T23:40:18Z- AHRS_M2 msg: 1613173218.01 epoch sec; seqNo:5776 2021-02-12T23:40:18Z- AHRS_M2 msg: 0.56 phi; -0.30 theta; -0.13 psi 2021-02-12T23:40:18Z- handleDepth()-Depth_Keller msg: 1613173218012 epoch sec; seqNo:5827; depth 8.91 2021-02-12T23:40:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173218.02 epoch sec; seqNo:5827; 4057554.81 north; 591330.72 east 2021-02-12T23:40:18Z- AHRS_M2 msg: 1613173218.41 epoch sec; seqNo:5777 2021-02-12T23:40:18Z- AHRS_M2 msg: 0.56 phi; -0.29 theta; -0.15 psi 2021-02-12T23:40:18Z- handleDepth()-Depth_Keller msg: 1613173218416 epoch sec; seqNo:5828; depth 9.02 2021-02-12T23:40:18Z- handleDvl() - RDI_Pathfinder msg: 1613173218.43 epoch sec; seqNo:2769 2021-02-12T23:40:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.12 , 101.29 , 81.01 , 116.16 2021-02-12T23:40:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.12 , -0.06 2021-02-12T23:40:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173218.44 epoch sec; seqNo:5828; 4057554.81 north; 591330.72 east 2021-02-12T23:40:18Z- AHRS_M2 msg: 1613173218.82 epoch sec; seqNo:5778 2021-02-12T23:40:18Z- AHRS_M2 msg: 0.56 phi; -0.29 theta; -0.18 psi 2021-02-12T23:40:18Z- handleDepth()-Depth_Keller msg: 1613173218829 epoch sec; seqNo:5829; depth 9.13 2021-02-12T23:40:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173218.84 epoch sec; seqNo:5829; 4057555.19 north; 591330.71 east 2021-02-12T23:40:19Z- AHRS_M2 msg: 1613173219.22 epoch sec; seqNo:5779 2021-02-12T23:40:19Z- AHRS_M2 msg: 0.55 phi; -0.28 theta; -0.20 psi 2021-02-12T23:40:19Z- handleDepth()-Depth_Keller msg: 1613173219225 epoch sec; seqNo:5830; depth 9.25 2021-02-12T23:40:19Z- handleDvl() - RDI_Pathfinder msg: 1613173219.24 epoch sec; seqNo:2770 2021-02-12T23:40:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 102.71 , 81.08 , 114.88 2021-02-12T23:40:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.05 , -0.28 2021-02-12T23:40:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173219.25 epoch sec; seqNo:5830; 4057555.57 north; 591330.71 east 2021-02-12T23:40:19Z- AHRS_M2 msg: 1613173219.62 epoch sec; seqNo:5780 2021-02-12T23:40:19Z- AHRS_M2 msg: 0.56 phi; -0.28 theta; -0.22 psi 2021-02-12T23:40:19Z- handleDepth()-Depth_Keller msg: 1613173219632 epoch sec; seqNo:5831; depth 9.36 2021-02-12T23:40:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173219.66 epoch sec; seqNo:5831; 4057555.95 north; 591330.70 east 2021-02-12T23:40:20Z- AHRS_M2 msg: 1613173220.03 epoch sec; seqNo:5781 2021-02-12T23:40:20Z- AHRS_M2 msg: 0.56 phi; -0.27 theta; -0.23 psi 2021-02-12T23:40:20Z- handleDepth()-Depth_Keller msg: 1613173220032 epoch sec; seqNo:5832; depth 9.46 2021-02-12T23:40:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173220.04 epoch sec; seqNo:5832; 4057555.95 north; 591330.70 east 2021-02-12T23:40:20Z- AHRS_M2 msg: 1613173220.43 epoch sec; seqNo:5782 2021-02-12T23:40:20Z- AHRS_M2 msg: 0.56 phi; -0.27 theta; -0.23 psi 2021-02-12T23:40:20Z- handleDepth()-Depth_Keller msg: 1613173220438 epoch sec; seqNo:5833; depth 9.57 2021-02-12T23:40:20Z- handleDvl() - RDI_Pathfinder msg: 1613173220.45 epoch sec; seqNo:2771 2021-02-12T23:40:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.85 , 109.54 , 81.15 , 112.24 2021-02-12T23:40:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.26 , -0.21 2021-02-12T23:40:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173220.46 epoch sec; seqNo:5833; 4057556.33 north; 591330.70 east 2021-02-12T23:40:20Z- AHRS_M2 msg: 1613173220.84 epoch sec; seqNo:5783 2021-02-12T23:40:20Z- AHRS_M2 msg: 0.57 phi; -0.27 theta; -0.22 psi 2021-02-12T23:40:20Z- handleDepth()-Depth_Keller msg: 1613173220840 epoch sec; seqNo:5834; depth 9.67 2021-02-12T23:40:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173220.85 epoch sec; seqNo:5834; 4057556.72 north; 591331.92 east 2021-02-12T23:40:21Z- AHRS_M2 msg: 1613173221.24 epoch sec; seqNo:5784 2021-02-12T23:40:21Z- AHRS_M2 msg: 0.57 phi; -0.27 theta; -0.20 psi 2021-02-12T23:40:21Z- handleDepth()-Depth_Keller msg: 1613173221244 epoch sec; seqNo:5835; depth 9.77 2021-02-12T23:40:21Z- handleDvl() - RDI_Pathfinder msg: 1613173221.26 epoch sec; seqNo:2772 2021-02-12T23:40:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 109.47 , 79.68 , 110.82 2021-02-12T23:40:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.34 , 0.11 2021-02-12T23:40:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173221.27 epoch sec; seqNo:5835; 4057557.10 north; 591331.91 east 2021-02-12T23:40:21Z- AHRS_M2 msg: 1613173221.64 epoch sec; seqNo:5785 2021-02-12T23:40:21Z- AHRS_M2 msg: 0.58 phi; -0.27 theta; -0.17 psi 2021-02-12T23:40:21Z- handleDepth()-Depth_Keller msg: 1613173221650 epoch sec; seqNo:5836; depth 9.89 2021-02-12T23:40:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173221.66 epoch sec; seqNo:5836; 4057557.48 north; 591331.91 east 2021-02-12T23:40:22Z- AHRS_M2 msg: 1613173222.07 epoch sec; seqNo:5786 2021-02-12T23:40:22Z- AHRS_M2 msg: 0.58 phi; -0.27 theta; -0.13 psi 2021-02-12T23:40:22Z- handleDepth()-Depth_Keller msg: 1613173222078 epoch sec; seqNo:5837; depth 9.99 2021-02-12T23:40:22Z- handleDvl() - RDI_Pathfinder msg: 1613173222.10 epoch sec; seqNo:2773 2021-02-12T23:40:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.87 , 103.79 , 78.45 , 109.20 2021-02-12T23:40:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.15 , 0.30 2021-02-12T23:40:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173222.13 epoch sec; seqNo:5837; 4057557.86 north; 591331.90 east 2021-02-12T23:40:22Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-12T23:40:22Z- waiting for 1613173220.24, time is 1613173222.13 2021-02-12T23:40:22Z- LcmTrn::updateTrn() equating pose 1613173222.13 and meas times 1613173222.13 2021-02-12T23:40:22Z- LcmTrn::updateTrn() >>>> pose time 1613173222.13, meas time 1613173222.13 <<<< 2021-02-12T23:40:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173222.13 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:40:22Z- TerrainNav::motionUpdate() - currEstimate = (4057558, 591331.90, 9.99) 2021-02-12T23:40:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:40:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:40:22Z- TNavBF::Projecting Measurements 2021-02-12T23:40:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:40:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:40:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:40:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:40:22Z- TNavFilter::4 good beams. 2021-02-12T23:40:22Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:40:22Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:40:22Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:40:22Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:40:22Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:40:22Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:40:22Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:40:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:40:22Z- TerrainMapOctree: (vehN, vehE ) = (4057557.86, 591331.90). 2021-02-12T23:40:22Z- TerrainMapOctree: Min Distance = 4706.08. 2021-02-12T23:40:22Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:40:22Z- There is no specific good beam that all particles have in common. 2021-02-12T23:40:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:40:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.58 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 11.52 2021-02-12T23:40:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:40:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.41 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 17.24 2021-02-12T23:40:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:40:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-12T23:40:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:40:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 19.08 2021-02-12T23:40:22Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:40:22Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:40:22Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:40:22Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:40:22Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:40:22Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:40:22Z- Filter 0 No beams used in subcloud update 2021-02-12T23:40:22Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:40:22Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:40:22Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 745.5 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:40:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173222.13 sec. 2021-02-12T23:40:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:40:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:40:22Z- variances([0], [2], and psi_berg) = 745.515994 833.028989 0.000000 2021-02-12T23:40:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:40:22Z- LcmTrn::publishEstimates() - MLE : 4057546.36 591292.40 9.99 2021-02-12T23:40:22Z- LcmTrn::publishEstimates() - MMSE : 4057557.86 591331.90 9.99 2021-02-12T23:40:22Z- LcmTrn::publishEstimates() - COVAR: 745.52 833.03 1.12 0.00 2021-02-12T23:40:22Z- AHRS_M2 msg: 1613173222.46 epoch sec; seqNo:5787 2021-02-12T23:40:22Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.11 psi 2021-02-12T23:40:22Z- handleDepth()-Depth_Keller msg: 1613173222466 epoch sec; seqNo:5838; depth 10.12 2021-02-12T23:40:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173222.48 epoch sec; seqNo:5838; 4057557.86 north; 591331.90 east 2021-02-12T23:40:22Z- AHRS_M2 msg: 1613173222.86 epoch sec; seqNo:5788 2021-02-12T23:40:22Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.11 psi 2021-02-12T23:40:22Z- handleDepth()-Depth_Keller msg: 1613173222866 epoch sec; seqNo:5839; depth 10.20 2021-02-12T23:40:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173222.88 epoch sec; seqNo:5839; 4057558.24 north; 591331.90 east 2021-02-12T23:40:23Z- AHRS_M2 msg: 1613173223.26 epoch sec; seqNo:5789 2021-02-12T23:40:23Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.10 psi 2021-02-12T23:40:23Z- handleDepth()-Depth_Keller msg: 1613173223265 epoch sec; seqNo:5840; depth 10.29 2021-02-12T23:40:23Z- handleDvl() - RDI_Pathfinder msg: 1613173223.28 epoch sec; seqNo:2774 2021-02-12T23:40:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.04 , 98.86 , 79.27 , 109.67 2021-02-12T23:40:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.05 , 0.21 2021-02-12T23:40:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173223.29 epoch sec; seqNo:5840; 4057558.61 north; 591331.90 east 2021-02-12T23:40:23Z- AHRS_M2 msg: 1613173223.66 epoch sec; seqNo:5790 2021-02-12T23:40:23Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.10 psi 2021-02-12T23:40:23Z- handleDepth()-Depth_Keller msg: 1613173223678 epoch sec; seqNo:5841; depth 10.39 2021-02-12T23:40:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173223.69 epoch sec; seqNo:5841; 4057558.61 north; 591331.90 east 2021-02-12T23:40:24Z- AHRS_M2 msg: 1613173224.07 epoch sec; seqNo:5791 2021-02-12T23:40:24Z- AHRS_M2 msg: 0.61 phi; -0.28 theta; -0.11 psi 2021-02-12T23:40:24Z- handleDepth()-Depth_Keller msg: 1613173224072 epoch sec; seqNo:5842; depth 10.50 2021-02-12T23:40:24Z- handleDvl() - RDI_Pathfinder msg: 1613173224.08 epoch sec; seqNo:2775 2021-02-12T23:40:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.11 , 96.22 , 78.04 , 109.74 2021-02-12T23:40:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , -0.00 2021-02-12T23:40:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173224.11 epoch sec; seqNo:5842; 4057558.99 north; 591331.89 east 2021-02-12T23:40:24Z- AHRS_M2 msg: 1613173224.47 epoch sec; seqNo:5792 2021-02-12T23:40:24Z- AHRS_M2 msg: 0.62 phi; -0.28 theta; -0.12 psi 2021-02-12T23:40:24Z- handleDepth()-Depth_Keller msg: 1613173224476 epoch sec; seqNo:5843; depth 10.61 2021-02-12T23:40:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173224.49 epoch sec; seqNo:5843; 4057559.39 north; 591333.11 east 2021-02-12T23:40:24Z- AHRS_M2 msg: 1613173224.88 epoch sec; seqNo:5793 2021-02-12T23:40:24Z- AHRS_M2 msg: 0.62 phi; -0.28 theta; -0.13 psi 2021-02-12T23:40:24Z- handleDepth()-Depth_Keller msg: 1613173224889 epoch sec; seqNo:5844; depth 10.71 2021-02-12T23:40:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173224.90 epoch sec; seqNo:5844; 4057559.76 north; 591333.10 east 2021-02-12T23:40:25Z- AHRS_M2 msg: 1613173225.28 epoch sec; seqNo:5794 2021-02-12T23:40:25Z- AHRS_M2 msg: 0.63 phi; -0.28 theta; -0.14 psi 2021-02-12T23:40:25Z- handleDepth()-Depth_Keller msg: 1613173225285 epoch sec; seqNo:5845; depth 10.81 2021-02-12T23:40:25Z- handleDvl() - RDI_Pathfinder msg: 1613173225.30 epoch sec; seqNo:2776 2021-02-12T23:40:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.99 , 78.72 , 109.81 2021-02-12T23:40:25Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.09 , -0.13 2021-02-12T23:40:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173225.31 epoch sec; seqNo:5845; 4057560.14 north; 591333.10 east 2021-02-12T23:40:25Z- AHRS_M2 msg: 1613173225.68 epoch sec; seqNo:5795 2021-02-12T23:40:25Z- AHRS_M2 msg: 0.63 phi; -0.28 theta; -0.15 psi 2021-02-12T23:40:25Z- handleDepth()-Depth_Keller msg: 1613173225688 epoch sec; seqNo:5846; depth 10.92 2021-02-12T23:40:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173225.70 epoch sec; seqNo:5846; 4057560.14 north; 591333.10 east 2021-02-12T23:40:26Z- AHRS_M2 msg: 1613173226.09 epoch sec; seqNo:5796 2021-02-12T23:40:26Z- AHRS_M2 msg: 0.64 phi; -0.28 theta; -0.15 psi 2021-02-12T23:40:26Z- handleDepth()-Depth_Keller msg: 1613173226092 epoch sec; seqNo:5847; depth 11.03 2021-02-12T23:40:26Z- handleDvl() - RDI_Pathfinder msg: 1613173226.10 epoch sec; seqNo:2777 2021-02-12T23:40:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.20 , 96.02 , 77.64 , 109.54 2021-02-12T23:40:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.16 , -0.13 2021-02-12T23:40:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173226.12 epoch sec; seqNo:5847; 4057560.52 north; 591333.10 east 2021-02-12T23:40:26Z- AHRS_M2 msg: 1613173226.49 epoch sec; seqNo:5797 2021-02-12T23:40:26Z- AHRS_M2 msg: 0.64 phi; -0.28 theta; -0.15 psi 2021-02-12T23:40:26Z- handleDepth()-Depth_Keller msg: 1613173226505 epoch sec; seqNo:5848; depth 11.13 2021-02-12T23:40:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173226.52 epoch sec; seqNo:5848; 4057560.90 north; 591333.09 east 2021-02-12T23:40:26Z- AHRS_M2 msg: 1613173226.90 epoch sec; seqNo:5798 2021-02-12T23:40:26Z- AHRS_M2 msg: 0.64 phi; -0.28 theta; -0.15 psi 2021-02-12T23:40:26Z- handleDepth()-Depth_Keller msg: 1613173226900 epoch sec; seqNo:5849; depth 11.25 2021-02-12T23:40:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173226.92 epoch sec; seqNo:5849; 4057561.29 north; 591334.31 east 2021-02-12T23:40:27Z- AHRS_M2 msg: 1613173227.30 epoch sec; seqNo:5799 2021-02-12T23:40:27Z- AHRS_M2 msg: 0.64 phi; -0.29 theta; -0.15 psi 2021-02-12T23:40:27Z- handleDepth()-Depth_Keller msg: 1613173227309 epoch sec; seqNo:5850; depth 11.36 2021-02-12T23:40:27Z- handleDvl() - RDI_Pathfinder msg: 1613173227.32 epoch sec; seqNo:2778 2021-02-12T23:40:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 96.36 , 78.78 , 108.52 2021-02-12T23:40:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.17 , -0.07 2021-02-12T23:40:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173227.34 epoch sec; seqNo:5850; 4057561.67 north; 591334.31 east 2021-02-12T23:40:27Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-12T23:40:27Z- waiting for 1613173225.13, time is 1613173227.34 2021-02-12T23:40:27Z- LcmTrn::updateTrn() equating pose 1613173227.34 and meas times 1613173227.34 2021-02-12T23:40:27Z- LcmTrn::updateTrn() >>>> pose time 1613173227.34, meas time 1613173227.34 <<<< 2021-02-12T23:40:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173227.34 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:40:27Z- TerrainNav::motionUpdate() - currEstimate = (4057562, 591334.31, 11.36) 2021-02-12T23:40:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:40:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:40:27Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173227.34 sec.; No relevant navigation data available 2021-02-12T23:40:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:40:27Z- TNavBF::mmse selected from filter -1 2021-02-12T23:40:27Z- variances([0], [2], and psi_berg) = 745.418562 833.028903 0.000000 2021-02-12T23:40:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:40:27Z- LcmTrn::publishEstimates() - MLE : 4057550.17 591294.81 11.36 2021-02-12T23:40:27Z- LcmTrn::publishEstimates() - MMSE : 4057561.67 591334.31 11.36 2021-02-12T23:40:27Z- LcmTrn::publishEstimates() - COVAR: 745.42 833.03 1.35 0.00 2021-02-12T23:40:27Z- AHRS_M2 msg: 1613173227.70 epoch sec; seqNo:5800 2021-02-12T23:40:27Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.15 psi 2021-02-12T23:40:27Z- handleDepth()-Depth_Keller msg: 1613173227709 epoch sec; seqNo:5851; depth 11.47 2021-02-12T23:40:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173227.72 epoch sec; seqNo:5851; 4057562.05 north; 591334.30 east 2021-02-12T23:40:28Z- AHRS_M2 msg: 1613173228.11 epoch sec; seqNo:5801 2021-02-12T23:40:28Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.15 psi 2021-02-12T23:40:28Z- handleDepth()-Depth_Keller msg: 1613173228113 epoch sec; seqNo:5852; depth 11.57 2021-02-12T23:40:28Z- handleDvl() - RDI_Pathfinder msg: 1613173228.13 epoch sec; seqNo:2779 2021-02-12T23:40:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.54 , 99.06 , 76.82 , 109.87 2021-02-12T23:40:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.12 , 0.01 2021-02-12T23:40:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173228.15 epoch sec; seqNo:5852; 4057562.43 north; 591334.30 east 2021-02-12T23:40:28Z- AHRS_M2 msg: 1613173228.52 epoch sec; seqNo:5802 2021-02-12T23:40:28Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.15 psi 2021-02-12T23:40:28Z- handleDepth()-Depth_Keller msg: 1613173228525 epoch sec; seqNo:5853; depth 11.69 2021-02-12T23:40:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173228.54 epoch sec; seqNo:5853; 4057562.43 north; 591334.30 east 2021-02-12T23:40:28Z- AHRS_M2 msg: 1613173228.91 epoch sec; seqNo:5803 2021-02-12T23:40:28Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.16 psi 2021-02-12T23:40:28Z- handleDepth()-Depth_Keller msg: 1613173228930 epoch sec; seqNo:5854; depth 11.80 2021-02-12T23:40:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173228.94 epoch sec; seqNo:5854; 4057562.81 north; 591334.29 east 2021-02-12T23:40:29Z- AHRS_M2 msg: 1613173229.34 epoch sec; seqNo:5804 2021-02-12T23:40:29Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.16 psi 2021-02-12T23:40:29Z- handleDepth()-Depth_Keller msg: 1613173229346 epoch sec; seqNo:5855; depth 11.89 2021-02-12T23:40:29Z- handleDvl() - RDI_Pathfinder msg: 1613173229.37 epoch sec; seqNo:2780 2021-02-12T23:40:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 96.29 , 78.72 , 111.16 2021-02-12T23:40:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , 0.04 2021-02-12T23:40:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173229.38 epoch sec; seqNo:5855; 4057563.19 north; 591334.29 east 2021-02-12T23:40:29Z- AHRS_M2 msg: 1613173229.72 epoch sec; seqNo:5805 2021-02-12T23:40:29Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.16 psi 2021-02-12T23:40:29Z- handleDepth()-Depth_Keller msg: 1613173229737 epoch sec; seqNo:5856; depth 12.01 2021-02-12T23:40:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173229.75 epoch sec; seqNo:5856; 4057563.57 north; 591334.29 east 2021-02-12T23:40:30Z- AHRS_M2 msg: 1613173230.13 epoch sec; seqNo:5806 2021-02-12T23:40:30Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.17 psi 2021-02-12T23:40:30Z- handleDepth()-Depth_Keller msg: 1613173230135 epoch sec; seqNo:5857; depth 12.11 2021-02-12T23:40:30Z- handleDvl() - RDI_Pathfinder msg: 1613173230.16 epoch sec; seqNo:2781 2021-02-12T23:40:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 98.99 , 77.64 , 111.16 2021-02-12T23:40:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.02 , 0.00 2021-02-12T23:40:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173230.17 epoch sec; seqNo:5857; 4057563.58 north; 591335.51 east 2021-02-12T23:40:30Z- AHRS_M2 msg: 1613173230.53 epoch sec; seqNo:5807 2021-02-12T23:40:30Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.18 psi 2021-02-12T23:40:30Z- handleDepth()-Depth_Keller msg: 1613173230541 epoch sec; seqNo:5858; depth 12.23 2021-02-12T23:40:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173230.55 epoch sec; seqNo:5858; 4057563.96 north; 591335.50 east 2021-02-12T23:40:30Z- AHRS_M2 msg: 1613173230.94 epoch sec; seqNo:5808 2021-02-12T23:40:30Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.19 psi 2021-02-12T23:40:30Z- handleDepth()-Depth_Keller msg: 1613173230945 epoch sec; seqNo:5859; depth 12.33 2021-02-12T23:40:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173230.96 epoch sec; seqNo:5859; 4057564.34 north; 591335.50 east 2021-02-12T23:40:31Z- AHRS_M2 msg: 1613173231.34 epoch sec; seqNo:5809 2021-02-12T23:40:31Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.19 psi 2021-02-12T23:40:31Z- handleDepth()-Depth_Keller msg: 1613173231346 epoch sec; seqNo:5860; depth 12.44 2021-02-12T23:40:31Z- handleDvl() - RDI_Pathfinder msg: 1613173231.36 epoch sec; seqNo:2782 2021-02-12T23:40:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 101.63 , 78.65 , 112.44 2021-02-12T23:40:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , -0.01 2021-02-12T23:40:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173231.37 epoch sec; seqNo:5860; 4057564.34 north; 591335.50 east 2021-02-12T23:40:31Z- AHRS_M2 msg: 1613173231.74 epoch sec; seqNo:5810 2021-02-12T23:40:31Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.19 psi 2021-02-12T23:40:31Z- handleDepth()-Depth_Keller msg: 1613173231748 epoch sec; seqNo:5861; depth 12.56 2021-02-12T23:40:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173231.76 epoch sec; seqNo:5861; 4057564.72 north; 591335.49 east 2021-02-12T23:40:32Z- AHRS_M2 msg: 1613173232.15 epoch sec; seqNo:5811 2021-02-12T23:40:32Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.18 psi 2021-02-12T23:40:32Z- handleDepth()-Depth_Keller msg: 1613173232154 epoch sec; seqNo:5862; depth 12.67 2021-02-12T23:40:32Z- handleDvl() - RDI_Pathfinder msg: 1613173232.17 epoch sec; seqNo:2783 2021-02-12T23:40:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 96.15 , 78.58 , 111.02 2021-02-12T23:40:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.10 , 0.02 2021-02-12T23:40:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173232.18 epoch sec; seqNo:5862; 4057565.09 north; 591335.49 east 2021-02-12T23:40:32Z- LcmTrn::handleMessages() - handling 41 messages took 0.61 ms and 129 calls... 2021-02-12T23:40:32Z- waiting for 1613173230.34, time is 1613173232.18 2021-02-12T23:40:32Z- LcmTrn::updateTrn() equating pose 1613173232.18 and meas times 1613173232.18 2021-02-12T23:40:32Z- LcmTrn::updateTrn() >>>> pose time 1613173232.18, meas time 1613173232.18 <<<< 2021-02-12T23:40:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173232.18 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:40:32Z- TerrainNav::motionUpdate() - currEstimate = (4057565, 591335.49, 12.67) 2021-02-12T23:40:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:40:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:40:32Z- TNavBF::Projecting Measurements 2021-02-12T23:40:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:40:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:40:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:40:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:40:32Z- TNavFilter::4 good beams. 2021-02-12T23:40:32Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:40:32Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:40:32Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:40:32Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:40:32Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:40:32Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:40:32Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:40:32Z- TNavBF::mmse selected from filter -1 2021-02-12T23:40:32Z- TerrainMapOctree: (vehN, vehE ) = (4057565.09, 591335.49). 2021-02-12T23:40:32Z- TerrainMapOctree: Min Distance = 4698.18. 2021-02-12T23:40:32Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:40:32Z- There is no specific good beam that all particles have in common. 2021-02-12T23:40:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:40:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.53 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 11.29 2021-02-12T23:40:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:40:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 14.79 2021-02-12T23:40:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:40:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-12T23:40:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:40:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 19.72 2021-02-12T23:40:32Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:40:32Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:40:32Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:40:32Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:40:32Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:40:32Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:40:32Z- Filter 0 No beams used in subcloud update 2021-02-12T23:40:32Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:40:32Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:40:32Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 745.3 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:40:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173232.18 sec. 2021-02-12T23:40:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:40:32Z- TNavBF::mmse selected from filter -1 2021-02-12T23:40:32Z- variances([0], [2], and psi_berg) = 745.331335 833.028827 0.000000 2021-02-12T23:40:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:40:32Z- LcmTrn::publishEstimates() - MLE : 4057553.59 591295.99 12.67 2021-02-12T23:40:32Z- LcmTrn::publishEstimates() - MMSE : 4057565.09 591335.49 12.67 2021-02-12T23:40:32Z- LcmTrn::publishEstimates() - COVAR: 745.33 833.03 1.08 0.00 2021-02-12T23:40:32Z- AHRS_M2 msg: 1613173232.56 epoch sec; seqNo:5812 2021-02-12T23:40:32Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.18 psi 2021-02-12T23:40:32Z- handleDepth()-Depth_Keller msg: 1613173232559 epoch sec; seqNo:5863; depth 12.79 2021-02-12T23:40:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173232.57 epoch sec; seqNo:5863; 4057565.47 north; 591335.49 east 2021-02-12T23:40:32Z- AHRS_M2 msg: 1613173232.96 epoch sec; seqNo:5813 2021-02-12T23:40:32Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.18 psi 2021-02-12T23:40:32Z- handleDepth()-Depth_Keller msg: 1613173232960 epoch sec; seqNo:5864; depth 12.90 2021-02-12T23:40:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173233.00 epoch sec; seqNo:5864; 4057565.86 north; 591336.70 east 2021-02-12T23:40:33Z- AHRS_M2 msg: 1613173233.36 epoch sec; seqNo:5814 2021-02-12T23:40:33Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.18 psi 2021-02-12T23:40:33Z- handleDepth()-Depth_Keller msg: 1613173233369 epoch sec; seqNo:5865; depth 13.03 2021-02-12T23:40:33Z- handleDvl() - RDI_Pathfinder msg: 1613173233.38 epoch sec; seqNo:2784 2021-02-12T23:40:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 100.41 , 78.78 , 109.87 2021-02-12T23:40:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , 0.02 2021-02-12T23:40:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173233.40 epoch sec; seqNo:5865; 4057565.86 north; 591336.70 east 2021-02-12T23:40:33Z- AHRS_M2 msg: 1613173233.76 epoch sec; seqNo:5815 2021-02-12T23:40:33Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.18 psi 2021-02-12T23:40:33Z- handleDepth()-Depth_Keller msg: 1613173233768 epoch sec; seqNo:5866; depth 13.14 2021-02-12T23:40:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173233.78 epoch sec; seqNo:5866; 4057566.24 north; 591336.70 east 2021-02-12T23:40:34Z- AHRS_M2 msg: 1613173234.17 epoch sec; seqNo:5816 2021-02-12T23:40:34Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.18 psi 2021-02-12T23:40:34Z- handleDepth()-Depth_Keller msg: 1613173234173 epoch sec; seqNo:5867; depth 13.26 2021-02-12T23:40:34Z- handleDvl() - RDI_Pathfinder msg: 1613173234.18 epoch sec; seqNo:2785 2021-02-12T23:40:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 96.29 , 76.41 , 109.81 2021-02-12T23:40:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , -0.05 2021-02-12T23:40:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173234.19 epoch sec; seqNo:5867; 4057566.62 north; 591336.69 east 2021-02-12T23:40:34Z- AHRS_M2 msg: 1613173234.57 epoch sec; seqNo:5817 2021-02-12T23:40:34Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.18 psi 2021-02-12T23:40:34Z- handleDepth()-Depth_Keller msg: 1613173234585 epoch sec; seqNo:5868; depth 13.36 2021-02-12T23:40:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173234.61 epoch sec; seqNo:5868; 4057567.00 north; 591336.69 east 2021-02-12T23:40:35Z- AHRS_M2 msg: 1613173235.00 epoch sec; seqNo:5818 2021-02-12T23:40:35Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.18 psi 2021-02-12T23:40:35Z- handleDepth()-Depth_Keller msg: 1613173235019 epoch sec; seqNo:5869; depth 13.48 2021-02-12T23:40:35Z- handleDvl() - RDI_Pathfinder msg: 1613173235.03 epoch sec; seqNo:2786 2021-02-12T23:40:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.26 , 100.07 , 75.18 , 111.02 2021-02-12T23:40:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.09 , -0.08 2021-02-12T23:40:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173235.04 epoch sec; seqNo:5869; 4057567.00 north; 591336.69 east 2021-02-12T23:40:35Z- AHRS_M2 msg: 1613173235.38 epoch sec; seqNo:5819 2021-02-12T23:40:35Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.18 psi 2021-02-12T23:40:35Z- handleDepth()-Depth_Keller msg: 1613173235386 epoch sec; seqNo:5870; depth 13.61 2021-02-12T23:40:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173235.40 epoch sec; seqNo:5870; 4057567.38 north; 591336.69 east 2021-02-12T23:40:35Z- AHRS_M2 msg: 1613173235.78 epoch sec; seqNo:5820 2021-02-12T23:40:35Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.18 psi 2021-02-12T23:40:35Z- handleDepth()-Depth_Keller msg: 1613173235788 epoch sec; seqNo:5871; depth 13.71 2021-02-12T23:40:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173235.80 epoch sec; seqNo:5871; 4057567.77 north; 591337.90 east 2021-02-12T23:40:36Z- AHRS_M2 msg: 1613173236.19 epoch sec; seqNo:5821 2021-02-12T23:40:36Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.18 psi 2021-02-12T23:40:36Z- handleDepth()-Depth_Keller msg: 1613173236192 epoch sec; seqNo:5872; depth 13.82 2021-02-12T23:40:36Z- handleDvl() - RDI_Pathfinder msg: 1613173236.20 epoch sec; seqNo:2787 2021-02-12T23:40:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 96.29 , 75.18 , 109.81 2021-02-12T23:40:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.15 , -0.05 2021-02-12T23:40:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173236.21 epoch sec; seqNo:5872; 4057568.15 north; 591337.90 east 2021-02-12T23:40:36Z- AHRS_M2 msg: 1613173236.60 epoch sec; seqNo:5822 2021-02-12T23:40:36Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.19 psi 2021-02-12T23:40:36Z- handleDepth()-Depth_Keller msg: 1613173236609 epoch sec; seqNo:5873; depth 13.93 2021-02-12T23:40:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173236.62 epoch sec; seqNo:5873; 4057568.15 north; 591337.90 east 2021-02-12T23:40:37Z- AHRS_M2 msg: 1613173237.01 epoch sec; seqNo:5823 2021-02-12T23:40:37Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.19 psi 2021-02-12T23:40:37Z- handleDepth()-Depth_Keller msg: 1613173237022 epoch sec; seqNo:5874; depth 14.06 2021-02-12T23:40:37Z- handleDvl() - RDI_Pathfinder msg: 1613173237.03 epoch sec; seqNo:2788 2021-02-12T23:40:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 96.42 , 74.37 , 111.02 2021-02-12T23:40:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.17 , 0.02 2021-02-12T23:40:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173237.04 epoch sec; seqNo:5874; 4057568.53 north; 591337.90 east 2021-02-12T23:40:37Z- AHRS_M2 msg: 1613173237.40 epoch sec; seqNo:5824 2021-02-12T23:40:37Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.20 psi 2021-02-12T23:40:37Z- handleDepth()-Depth_Keller msg: 1613173237404 epoch sec; seqNo:5875; depth 14.19 2021-02-12T23:40:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173237.41 epoch sec; seqNo:5875; 4057568.91 north; 591337.89 east 2021-02-12T23:40:37Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-12T23:40:37Z- waiting for 1613173235.18, time is 1613173237.41 2021-02-12T23:40:37Z- LcmTrn::updateTrn() >>>> pose time 1613173237.41, meas time 1613173237.03 <<<< 2021-02-12T23:40:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173237.03 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:40:37Z- TerrainNav::motionUpdate() - currEstimate = (4057569, 591337.89, 14.19) 2021-02-12T23:40:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:40:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:40:37Z- TNavBF::Projecting Measurements 2021-02-12T23:40:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:40:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:40:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:40:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:40:37Z- TNavFilter::4 good beams. 2021-02-12T23:40:37Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:40:37Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:40:37Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:40:37Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:40:37Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:40:37Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:40:37Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:40:37Z- TNavBF::mmse selected from filter -1 2021-02-12T23:40:37Z- TerrainMapOctree: (vehN, vehE ) = (4057568.63, 591337.72). 2021-02-12T23:40:37Z- TerrainMapOctree: Min Distance = 4694.02. 2021-02-12T23:40:37Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:40:37Z- There is no specific good beam that all particles have in common. 2021-02-12T23:40:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:40:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 10.71 2021-02-12T23:40:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:40:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 14.88 2021-02-12T23:40:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:40:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-12T23:40:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:40:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 19.72 2021-02-12T23:40:37Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:40:37Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:40:37Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:40:37Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:40:37Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:40:37Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:40:37Z- Filter 0 No beams used in subcloud update 2021-02-12T23:40:37Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:40:37Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:40:37Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 745.2 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:40:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173237.03 sec. 2021-02-12T23:40:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:40:37Z- TNavBF::mmse selected from filter -1 2021-02-12T23:40:37Z- variances([0], [2], and psi_berg) = 745.211557 833.028722 0.000000 2021-02-12T23:40:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:40:37Z- LcmTrn::publishEstimates() - MLE : 4057557.41 591298.39 14.19 2021-02-12T23:40:37Z- LcmTrn::publishEstimates() - MMSE : 4057568.91 591337.89 14.19 2021-02-12T23:40:37Z- LcmTrn::publishEstimates() - COVAR: 745.21 833.03 0.97 0.00 2021-02-12T23:40:37Z- AHRS_M2 msg: 1613173237.80 epoch sec; seqNo:5825 2021-02-12T23:40:37Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.19 psi 2021-02-12T23:40:37Z- handleDepth()-Depth_Keller msg: 1613173237808 epoch sec; seqNo:5876; depth 14.28 2021-02-12T23:40:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173237.82 epoch sec; seqNo:5876; 4057568.91 north; 591337.89 east 2021-02-12T23:40:38Z- AHRS_M2 msg: 1613173238.21 epoch sec; seqNo:5826 2021-02-12T23:40:38Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.19 psi 2021-02-12T23:40:38Z- handleDepth()-Depth_Keller msg: 1613173238225 epoch sec; seqNo:5877; depth 14.39 2021-02-12T23:40:38Z- handleDvl() - RDI_Pathfinder msg: 1613173238.25 epoch sec; seqNo:2789 2021-02-12T23:40:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 96.49 , 76.41 , 111.09 2021-02-12T23:40:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.12 , 0.04 2021-02-12T23:40:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173238.28 epoch sec; seqNo:5877; 4057569.29 north; 591337.89 east 2021-02-12T23:40:38Z- AHRS_M2 msg: 1613173238.61 epoch sec; seqNo:5827 2021-02-12T23:40:38Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.19 psi 2021-02-12T23:40:38Z- handleDepth()-Depth_Keller msg: 1613173238617 epoch sec; seqNo:5878; depth 14.51 2021-02-12T23:40:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173238.64 epoch sec; seqNo:5878; 4057569.67 north; 591337.88 east 2021-02-12T23:40:39Z- AHRS_M2 msg: 1613173239.02 epoch sec; seqNo:5828 2021-02-12T23:40:39Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.19 psi 2021-02-12T23:40:39Z- handleDepth()-Depth_Keller msg: 1613173239020 epoch sec; seqNo:5879; depth 14.62 2021-02-12T23:40:39Z- handleDvl() - RDI_Pathfinder msg: 1613173239.04 epoch sec; seqNo:2790 2021-02-12T23:40:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.68 , 100.07 , 73.55 , 107.37 2021-02-12T23:40:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.12 , 0.03 2021-02-12T23:40:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173239.06 epoch sec; seqNo:5879; 4057570.05 north; 591337.88 east 2021-02-12T23:40:39Z- AHRS_M2 msg: 1613173239.43 epoch sec; seqNo:5829 2021-02-12T23:40:39Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.19 psi 2021-02-12T23:40:39Z- handleDepth()-Depth_Keller msg: 1613173239430 epoch sec; seqNo:5880; depth 14.76 2021-02-12T23:40:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173239.44 epoch sec; seqNo:5880; 4057570.44 north; 591339.10 east 2021-02-12T23:40:39Z- AHRS_M2 msg: 1613173239.83 epoch sec; seqNo:5830 2021-02-12T23:40:39Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.19 psi 2021-02-12T23:40:39Z- handleDepth()-Depth_Keller msg: 1613173239834 epoch sec; seqNo:5881; depth 14.88 2021-02-12T23:40:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173239.84 epoch sec; seqNo:5881; 4057570.44 north; 591339.10 east 2021-02-12T23:40:40Z- AHRS_M2 msg: 1613173240.23 epoch sec; seqNo:5831 2021-02-12T23:40:40Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.19 psi 2021-02-12T23:40:40Z- handleDepth()-Depth_Keller msg: 1613173240233 epoch sec; seqNo:5882; depth 15.00 2021-02-12T23:40:40Z- handleDvl() - RDI_Pathfinder msg: 1613173240.25 epoch sec; seqNo:2791 2021-02-12T23:40:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 98.93 , 73.14 , 109.87 2021-02-12T23:40:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.10 , 0.00 2021-02-12T23:40:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173240.26 epoch sec; seqNo:5882; 4057570.82 north; 591339.09 east 2021-02-12T23:40:40Z- AHRS_M2 msg: 1613173240.63 epoch sec; seqNo:5832 2021-02-12T23:40:40Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.19 psi 2021-02-12T23:40:40Z- handleDepth()-Depth_Keller msg: 1613173240641 epoch sec; seqNo:5883; depth 15.11 2021-02-12T23:40:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173240.65 epoch sec; seqNo:5883; 4057571.19 north; 591339.09 east 2021-02-12T23:40:41Z- AHRS_M2 msg: 1613173241.04 epoch sec; seqNo:5833 2021-02-12T23:40:41Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.18 psi 2021-02-12T23:40:41Z- handleDepth()-Depth_Keller msg: 1613173241046 epoch sec; seqNo:5884; depth 15.23 2021-02-12T23:40:41Z- handleDvl() - RDI_Pathfinder msg: 1613173241.06 epoch sec; seqNo:2792 2021-02-12T23:40:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 98.99 , 73.96 , 108.72 2021-02-12T23:40:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , 0.02 2021-02-12T23:40:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173241.07 epoch sec; seqNo:5884; 4057571.57 north; 591339.08 east 2021-02-12T23:40:41Z- AHRS_M2 msg: 1613173241.44 epoch sec; seqNo:5834 2021-02-12T23:40:41Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.18 psi 2021-02-12T23:40:41Z- handleDepth()-Depth_Keller msg: 1613173241454 epoch sec; seqNo:5885; depth 15.36 2021-02-12T23:40:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173241.47 epoch sec; seqNo:5885; 4057571.57 north; 591339.08 east 2021-02-12T23:40:41Z- AHRS_M2 msg: 1613173241.84 epoch sec; seqNo:5835 2021-02-12T23:40:41Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.17 psi 2021-02-12T23:40:41Z- handleDepth()-Depth_Keller msg: 1613173241853 epoch sec; seqNo:5886; depth 15.46 2021-02-12T23:40:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173241.86 epoch sec; seqNo:5886; 4057571.95 north; 591339.08 east 2021-02-12T23:40:42Z- AHRS_M2 msg: 1613173242.25 epoch sec; seqNo:5836 2021-02-12T23:40:42Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.17 psi 2021-02-12T23:40:42Z- handleDepth()-Depth_Keller msg: 1613173242257 epoch sec; seqNo:5887; depth 15.58 2021-02-12T23:40:42Z- handleDvl() - RDI_Pathfinder msg: 1613173242.27 epoch sec; seqNo:2793 2021-02-12T23:40:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 96.49 , 74.78 , 107.44 2021-02-12T23:40:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , 0.05 2021-02-12T23:40:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173242.28 epoch sec; seqNo:5887; 4057572.34 north; 591340.30 east 2021-02-12T23:40:42Z- AHRS_M2 msg: 1613173242.65 epoch sec; seqNo:5837 2021-02-12T23:40:42Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.16 psi 2021-02-12T23:40:42Z- handleDepth()-Depth_Keller msg: 1613173242665 epoch sec; seqNo:5888; depth 15.71 2021-02-12T23:40:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173242.68 epoch sec; seqNo:5888; 4057572.72 north; 591340.29 east 2021-02-12T23:40:42Z- LcmTrn::handleMessages() - handling 44 messages took 0.73 ms and 130 calls... 2021-02-12T23:40:42Z- waiting for 1613173240.41, time is 1613173242.68 2021-02-12T23:40:42Z- LcmTrn::updateTrn() >>>> pose time 1613173242.68, meas time 1613173242.27 <<<< 2021-02-12T23:40:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173242.27 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:40:42Z- TerrainNav::motionUpdate() - currEstimate = (4057573, 591340.29, 15.71) 2021-02-12T23:40:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:40:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:40:42Z- TNavBF::Projecting Measurements 2021-02-12T23:40:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:40:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:40:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:40:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:40:42Z- TNavFilter::4 good beams. 2021-02-12T23:40:42Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:40:42Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:40:42Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:40:42Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:40:42Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:40:42Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:40:42Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:40:42Z- TNavBF::mmse selected from filter -1 2021-02-12T23:40:42Z- TerrainMapOctree: (vehN, vehE ) = (4057572.43, 591340.11). 2021-02-12T23:40:42Z- TerrainMapOctree: Min Distance = 4689.56. 2021-02-12T23:40:42Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:40:42Z- There is no specific good beam that all particles have in common. 2021-02-12T23:40:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:40:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 10.41 2021-02-12T23:40:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:40:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 14.90 2021-02-12T23:40:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:40:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 8.95 2021-02-12T23:40:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:40:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 18.47 2021-02-12T23:40:42Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:40:42Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:40:42Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:40:42Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:40:42Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:40:42Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:40:42Z- Filter 0 No beams used in subcloud update 2021-02-12T23:40:42Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:40:42Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:40:42Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 745.1 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:40:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173242.27 sec. 2021-02-12T23:40:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:40:42Z- TNavBF::mmse selected from filter -1 2021-02-12T23:40:42Z- variances([0], [2], and psi_berg) = 745.091377 833.028617 0.000000 2021-02-12T23:40:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:40:42Z- LcmTrn::publishEstimates() - MLE : 4057561.22 591300.79 15.71 2021-02-12T23:40:42Z- LcmTrn::publishEstimates() - MMSE : 4057572.72 591340.29 15.71 2021-02-12T23:40:42Z- LcmTrn::publishEstimates() - COVAR: 745.09 833.03 0.90 0.00 2021-02-12T23:40:43Z- AHRS_M2 msg: 1613173243.06 epoch sec; seqNo:5838 2021-02-12T23:40:43Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.15 psi 2021-02-12T23:40:43Z- handleDepth()-Depth_Keller msg: 1613173243065 epoch sec; seqNo:5889; depth 15.83 2021-02-12T23:40:43Z- handleDvl() - RDI_Pathfinder msg: 1613173243.08 epoch sec; seqNo:2794 2021-02-12T23:40:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 97.78 , 74.37 , 107.51 2021-02-12T23:40:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.05 2021-02-12T23:40:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173243.09 epoch sec; seqNo:5889; 4057572.72 north; 591340.29 east 2021-02-12T23:40:43Z- AHRS_M2 msg: 1613173243.46 epoch sec; seqNo:5839 2021-02-12T23:40:43Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.15 psi 2021-02-12T23:40:43Z- handleDepth()-Depth_Keller msg: 1613173243469 epoch sec; seqNo:5890; depth 15.95 2021-02-12T23:40:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173243.48 epoch sec; seqNo:5890; 4057573.10 north; 591340.29 east 2021-02-12T23:40:43Z- AHRS_M2 msg: 1613173243.87 epoch sec; seqNo:5840 2021-02-12T23:40:43Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.16 psi 2021-02-12T23:40:43Z- handleDepth()-Depth_Keller msg: 1613173243869 epoch sec; seqNo:5891; depth 16.07 2021-02-12T23:40:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173243.88 epoch sec; seqNo:5891; 4057573.48 north; 591340.29 east 2021-02-12T23:40:44Z- AHRS_M2 msg: 1613173244.27 epoch sec; seqNo:5841 2021-02-12T23:40:44Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.17 psi 2021-02-12T23:40:44Z- handleDepth()-Depth_Keller msg: 1613173244277 epoch sec; seqNo:5892; depth 16.18 2021-02-12T23:40:44Z- handleDvl() - RDI_Pathfinder msg: 1613173244.29 epoch sec; seqNo:2795 2021-02-12T23:40:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 96.49 , 71.51 , 108.66 2021-02-12T23:40:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , -0.00 2021-02-12T23:40:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173244.31 epoch sec; seqNo:5892; 4057573.48 north; 591340.29 east 2021-02-12T23:40:44Z- AHRS_M2 msg: 1613173244.67 epoch sec; seqNo:5842 2021-02-12T23:40:44Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.17 psi 2021-02-12T23:40:44Z- handleDepth()-Depth_Keller msg: 1613173244678 epoch sec; seqNo:5893; depth 16.31 2021-02-12T23:40:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173244.70 epoch sec; seqNo:5893; 4057573.87 north; 591341.50 east 2021-02-12T23:40:45Z- AHRS_M2 msg: 1613173245.08 epoch sec; seqNo:5843 2021-02-12T23:40:45Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.18 psi 2021-02-12T23:40:45Z- handleDepth()-Depth_Keller msg: 1613173245080 epoch sec; seqNo:5894; depth 16.44 2021-02-12T23:40:45Z- handleDvl() - RDI_Pathfinder msg: 1613173245.09 epoch sec; seqNo:2796 2021-02-12T23:40:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 96.56 , 73.14 , 107.51 2021-02-12T23:40:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , -0.06 2021-02-12T23:40:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173245.12 epoch sec; seqNo:5894; 4057574.25 north; 591341.50 east 2021-02-12T23:40:45Z- AHRS_M2 msg: 1613173245.48 epoch sec; seqNo:5844 2021-02-12T23:40:45Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.18 psi 2021-02-12T23:40:45Z- handleDepth()-Depth_Keller msg: 1613173245485 epoch sec; seqNo:5895; depth 16.54 2021-02-12T23:40:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173245.50 epoch sec; seqNo:5895; 4057574.63 north; 591341.49 east 2021-02-12T23:40:45Z- AHRS_M2 msg: 1613173245.89 epoch sec; seqNo:5845 2021-02-12T23:40:45Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.19 psi 2021-02-12T23:40:45Z- handleDepth()-Depth_Keller msg: 1613173245895 epoch sec; seqNo:5896; depth 16.65 2021-02-12T23:40:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173245.92 epoch sec; seqNo:5896; 4057575.01 north; 591341.49 east 2021-02-12T23:40:46Z- AHRS_M2 msg: 1613173246.29 epoch sec; seqNo:5846 2021-02-12T23:40:46Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.19 psi 2021-02-12T23:40:46Z- handleDepth()-Depth_Keller msg: 1613173246292 epoch sec; seqNo:5897; depth 16.79 2021-02-12T23:40:46Z- handleDvl() - RDI_Pathfinder msg: 1613173246.30 epoch sec; seqNo:2797 2021-02-12T23:40:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 96.56 , 73.14 , 108.72 2021-02-12T23:40:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , -0.08 2021-02-12T23:40:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173246.32 epoch sec; seqNo:5897; 4057575.01 north; 591341.49 east 2021-02-12T23:40:46Z- AHRS_M2 msg: 1613173246.69 epoch sec; seqNo:5847 2021-02-12T23:40:46Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.19 psi 2021-02-12T23:40:46Z- handleDepth()-Depth_Keller msg: 1613173246705 epoch sec; seqNo:5898; depth 16.91 2021-02-12T23:40:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173246.72 epoch sec; seqNo:5898; 4057575.39 north; 591341.49 east 2021-02-12T23:40:47Z- AHRS_M2 msg: 1613173247.10 epoch sec; seqNo:5848 2021-02-12T23:40:47Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.19 psi 2021-02-12T23:40:47Z- handleDepth()-Depth_Keller msg: 1613173247100 epoch sec; seqNo:5899; depth 17.04 2021-02-12T23:40:47Z- handleDvl() - RDI_Pathfinder msg: 1613173247.11 epoch sec; seqNo:2798 2021-02-12T23:40:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 97.84 , 72.73 , 107.58 2021-02-12T23:40:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.11 , -0.06 2021-02-12T23:40:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173247.13 epoch sec; seqNo:5899; 4057575.78 north; 591342.70 east 2021-02-12T23:40:47Z- AHRS_M2 msg: 1613173247.50 epoch sec; seqNo:5849 2021-02-12T23:40:47Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.19 psi 2021-02-12T23:40:47Z- handleDepth()-Depth_Keller msg: 1613173247507 epoch sec; seqNo:5900; depth 17.17 2021-02-12T23:40:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173247.52 epoch sec; seqNo:5900; 4057576.16 north; 591342.70 east 2021-02-12T23:40:47Z- LcmTrn::handleMessages() - handling 41 messages took 0.61 ms and 129 calls... 2021-02-12T23:40:47Z- waiting for 1613173245.68, time is 1613173247.52 2021-02-12T23:40:47Z- LcmTrn::updateTrn() >>>> pose time 1613173247.52, meas time 1613173247.11 <<<< 2021-02-12T23:40:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173247.11 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:40:47Z- TerrainNav::motionUpdate() - currEstimate = (4057576, 591342.70, 17.17) 2021-02-12T23:40:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:40:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:40:47Z- TNavBF::Projecting Measurements 2021-02-12T23:40:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:40:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:40:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:40:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:40:47Z- TNavFilter::4 good beams. 2021-02-12T23:40:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:40:47Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:40:47Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:40:47Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:40:47Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:40:47Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:40:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:40:47Z- TNavBF::mmse selected from filter -1 2021-02-12T23:40:47Z- TerrainMapOctree: (vehN, vehE ) = (4057575.87, 591342.50). 2021-02-12T23:40:47Z- TerrainMapOctree: Min Distance = 4685.38. 2021-02-12T23:40:47Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:40:47Z- There is no specific good beam that all particles have in common. 2021-02-12T23:40:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:40:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 9.75 2021-02-12T23:40:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:40:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-12T23:40:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:40:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 8.46 2021-02-12T23:40:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:40:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 18.52 2021-02-12T23:40:47Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:40:47Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:40:47Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:40:47Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:40:47Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:40:47Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:40:47Z- Filter 0 No beams used in subcloud update 2021-02-12T23:40:47Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:40:47Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:40:47Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 745.0 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:40:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173247.11 sec. 2021-02-12T23:40:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:40:47Z- TNavBF::mmse selected from filter -1 2021-02-12T23:40:47Z- variances([0], [2], and psi_berg) = 744.974901 833.028515 0.000000 2021-02-12T23:40:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:40:47Z- LcmTrn::publishEstimates() - MLE : 4057564.66 591303.20 17.17 2021-02-12T23:40:47Z- LcmTrn::publishEstimates() - MMSE : 4057576.16 591342.70 17.17 2021-02-12T23:40:47Z- LcmTrn::publishEstimates() - COVAR: 744.97 833.03 0.85 0.00 2021-02-12T23:40:47Z- AHRS_M2 msg: 1613173247.92 epoch sec; seqNo:5850 2021-02-12T23:40:47Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.20 psi 2021-02-12T23:40:47Z- handleDepth()-Depth_Keller msg: 1613173247934 epoch sec; seqNo:5901; depth 17.29 2021-02-12T23:40:47Z- handleDvl() - RDI_Pathfinder msg: 1613173247.95 epoch sec; seqNo:2799 2021-02-12T23:40:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 96.63 , 70.69 , 106.36 2021-02-12T23:40:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , -0.05 2021-02-12T23:40:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173247.96 epoch sec; seqNo:5901; 4057576.54 north; 591342.69 east 2021-02-12T23:40:48Z- AHRS_M2 msg: 1613173248.31 epoch sec; seqNo:5851 2021-02-12T23:40:48Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.20 psi 2021-02-12T23:40:48Z- handleDepth()-Depth_Keller msg: 1613173248313 epoch sec; seqNo:5902; depth 17.44 2021-02-12T23:40:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173248.32 epoch sec; seqNo:5902; 4057576.54 north; 591342.69 east 2021-02-12T23:40:48Z- AHRS_M2 msg: 1613173248.71 epoch sec; seqNo:5852 2021-02-12T23:40:48Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.21 psi 2021-02-12T23:40:48Z- handleDepth()-Depth_Keller msg: 1613173248716 epoch sec; seqNo:5903; depth 17.54 2021-02-12T23:40:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173248.72 epoch sec; seqNo:5903; 4057576.92 north; 591342.69 east 2021-02-12T23:40:49Z- AHRS_M2 msg: 1613173249.12 epoch sec; seqNo:5853 2021-02-12T23:40:49Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.21 psi 2021-02-12T23:40:49Z- handleDepth()-Depth_Keller msg: 1613173249132 epoch sec; seqNo:5904; depth 17.67 2021-02-12T23:40:49Z- handleDvl() - RDI_Pathfinder msg: 1613173249.15 epoch sec; seqNo:2800 2021-02-12T23:40:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 97.91 , 71.92 , 107.64 2021-02-12T23:40:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.09 , -0.03 2021-02-12T23:40:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173249.16 epoch sec; seqNo:5904; 4057577.30 north; 591342.69 east 2021-02-12T23:40:49Z- AHRS_M2 msg: 1613173249.52 epoch sec; seqNo:5854 2021-02-12T23:40:49Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.20 psi 2021-02-12T23:40:49Z- handleDepth()-Depth_Keller msg: 1613173249529 epoch sec; seqNo:5905; depth 17.81 2021-02-12T23:40:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173249.54 epoch sec; seqNo:5905; 4057577.30 north; 591342.69 east 2021-02-12T23:40:49Z- AHRS_M2 msg: 1613173249.93 epoch sec; seqNo:5855 2021-02-12T23:40:49Z- AHRS_M2 msg: 0.69 phi; -0.33 theta; -0.19 psi 2021-02-12T23:40:49Z- handleDepth()-Depth_Keller msg: 1613173249937 epoch sec; seqNo:5906; depth 17.92 2021-02-12T23:40:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173249.95 epoch sec; seqNo:5906; 4057577.69 north; 591343.90 east 2021-02-12T23:40:50Z- AHRS_M2 msg: 1613173250.33 epoch sec; seqNo:5856 2021-02-12T23:40:50Z- AHRS_M2 msg: 0.69 phi; -0.33 theta; -0.19 psi 2021-02-12T23:40:50Z- handleDepth()-Depth_Keller msg: 1613173250332 epoch sec; seqNo:5907; depth 18.06 2021-02-12T23:40:50Z- handleDvl() - RDI_Pathfinder msg: 1613173250.35 epoch sec; seqNo:2801 2021-02-12T23:40:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.23 , 96.63 , 78.38 , 106.36 2021-02-12T23:40:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.08 , -0.02 2021-02-12T23:40:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173250.36 epoch sec; seqNo:5907; 4057578.07 north; 591343.90 east 2021-02-12T23:40:50Z- AHRS_M2 msg: 1613173250.73 epoch sec; seqNo:5857 2021-02-12T23:40:50Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.18 psi 2021-02-12T23:40:50Z- handleDepth()-Depth_Keller msg: 1613173250736 epoch sec; seqNo:5908; depth 18.17 2021-02-12T23:40:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173250.75 epoch sec; seqNo:5908; 4057578.44 north; 591343.90 east 2021-02-12T23:40:51Z- AHRS_M2 msg: 1613173251.14 epoch sec; seqNo:5858 2021-02-12T23:40:51Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.17 psi 2021-02-12T23:40:51Z- handleDepth()-Depth_Keller msg: 1613173251145 epoch sec; seqNo:5909; depth 18.28 2021-02-12T23:40:51Z- handleDvl() - RDI_Pathfinder msg: 1613173251.16 epoch sec; seqNo:2802 2021-02-12T23:40:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:40:51Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:40:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173251.18 epoch sec; seqNo:5909; 4057578.44 north; 591343.90 east 2021-02-12T23:40:51Z- AHRS_M2 msg: 1613173251.54 epoch sec; seqNo:5859 2021-02-12T23:40:51Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.17 psi 2021-02-12T23:40:51Z- handleDepth()-Depth_Keller msg: 1613173251545 epoch sec; seqNo:5910; depth 18.41 2021-02-12T23:40:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173251.56 epoch sec; seqNo:5910; 4057578.82 north; 591343.89 east 2021-02-12T23:40:51Z- AHRS_M2 msg: 1613173251.94 epoch sec; seqNo:5860 2021-02-12T23:40:51Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.16 psi 2021-02-12T23:40:51Z- handleDepth()-Depth_Keller msg: 1613173251952 epoch sec; seqNo:5911; depth 18.54 2021-02-12T23:40:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173251.97 epoch sec; seqNo:5911; 4057579.20 north; 591343.89 east 2021-02-12T23:40:52Z- AHRS_M2 msg: 1613173252.35 epoch sec; seqNo:5861 2021-02-12T23:40:52Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.17 psi 2021-02-12T23:40:52Z- handleDepth()-Depth_Keller msg: 1613173252352 epoch sec; seqNo:5912; depth 18.67 2021-02-12T23:40:52Z- handleDvl() - RDI_Pathfinder msg: 1613173252.37 epoch sec; seqNo:2803 2021-02-12T23:40:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 96.96 , 78.04 , 105.07 2021-02-12T23:40:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , -0.01 2021-02-12T23:40:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173252.38 epoch sec; seqNo:5912; 4057579.20 north; 591343.89 east 2021-02-12T23:40:52Z- AHRS_M2 msg: 1613173252.75 epoch sec; seqNo:5862 2021-02-12T23:40:52Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.17 psi 2021-02-12T23:40:52Z- handleDepth()-Depth_Keller msg: 1613173252761 epoch sec; seqNo:5913; depth 18.80 2021-02-12T23:40:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173252.77 epoch sec; seqNo:5913; 4057579.59 north; 591345.10 east 2021-02-12T23:40:52Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-12T23:40:52Z- waiting for 1613173250.52, time is 1613173252.77 2021-02-12T23:40:52Z- LcmTrn::updateTrn() >>>> pose time 1613173252.77, meas time 1613173252.37 <<<< 2021-02-12T23:40:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173252.37 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:40:52Z- TerrainNav::motionUpdate() - currEstimate = (4057580, 591345.10, 18.80) 2021-02-12T23:40:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:40:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:40:52Z- TNavBF::Projecting Measurements 2021-02-12T23:40:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:40:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:40:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:40:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:40:52Z- TNavFilter::4 good beams. 2021-02-12T23:40:52Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:40:52Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:40:52Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:40:52Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:40:52Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:40:52Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:40:52Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:40:52Z- TNavBF::mmse selected from filter -1 2021-02-12T23:40:52Z- TerrainMapOctree: (vehN, vehE ) = (4057579.33, 591344.92). 2021-02-12T23:40:52Z- TerrainMapOctree: Min Distance = 4681.16. 2021-02-12T23:40:52Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:40:52Z- There is no specific good beam that all particles have in common. 2021-02-12T23:40:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:40:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 10.43 2021-02-12T23:40:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:40:52Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.04 2021-02-12T23:40:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:40:52Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 9.74 2021-02-12T23:40:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:40:52Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 17.67 2021-02-12T23:40:52Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:40:52Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:40:52Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:40:52Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:40:52Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:40:52Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:40:52Z- Filter 0 No beams used in subcloud update 2021-02-12T23:40:52Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:40:52Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:40:52Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 744.9 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:40:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173252.37 sec. 2021-02-12T23:40:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:40:53Z- TNavBF::mmse selected from filter -1 2021-02-12T23:40:53Z- variances([0], [2], and psi_berg) = 744.859451 833.028415 0.000000 2021-02-12T23:40:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:40:53Z- LcmTrn::publishEstimates() - MLE : 4057568.09 591305.60 18.80 2021-02-12T23:40:53Z- LcmTrn::publishEstimates() - MMSE : 4057579.59 591345.10 18.80 2021-02-12T23:40:53Z- LcmTrn::publishEstimates() - COVAR: 744.86 833.03 0.82 0.00 2021-02-12T23:40:53Z- AHRS_M2 msg: 1613173253.16 epoch sec; seqNo:5863 2021-02-12T23:40:53Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.18 psi 2021-02-12T23:40:53Z- handleDepth()-Depth_Keller msg: 1613173253161 epoch sec; seqNo:5914; depth 18.91 2021-02-12T23:40:53Z- handleDvl() - RDI_Pathfinder msg: 1613173253.17 epoch sec; seqNo:2804 2021-02-12T23:40:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 97.71 , 70.29 , 107.45 2021-02-12T23:40:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , -0.01 2021-02-12T23:40:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173253.18 epoch sec; seqNo:5914; 4057579.97 north; 591345.10 east 2021-02-12T23:40:53Z- AHRS_M2 msg: 1613173253.56 epoch sec; seqNo:5864 2021-02-12T23:40:53Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.19 psi 2021-02-12T23:40:53Z- handleDepth()-Depth_Keller msg: 1613173253567 epoch sec; seqNo:5915; depth 19.03 2021-02-12T23:40:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173253.58 epoch sec; seqNo:5915; 4057579.97 north; 591345.10 east 2021-02-12T23:40:53Z- AHRS_M2 msg: 1613173253.96 epoch sec; seqNo:5865 2021-02-12T23:40:53Z- AHRS_M2 msg: 0.70 phi; -0.34 theta; -0.20 psi 2021-02-12T23:40:53Z- handleDepth()-Depth_Keller msg: 1613173253975 epoch sec; seqNo:5916; depth 19.16 2021-02-12T23:40:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173254.01 epoch sec; seqNo:5916; 4057580.35 north; 591345.10 east 2021-02-12T23:40:54Z- AHRS_M2 msg: 1613173254.39 epoch sec; seqNo:5866 2021-02-12T23:40:54Z- AHRS_M2 msg: 0.70 phi; -0.34 theta; -0.21 psi 2021-02-12T23:40:54Z- handleDepth()-Depth_Keller msg: 1613173254394 epoch sec; seqNo:5917; depth 19.28 2021-02-12T23:40:54Z- handleDvl() - RDI_Pathfinder msg: 1613173254.43 epoch sec; seqNo:2805 2021-02-12T23:40:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 97.92 , 70.29 , 106.43 2021-02-12T23:40:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , 0.00 2021-02-12T23:40:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173254.47 epoch sec; seqNo:5917; 4057580.73 north; 591345.09 east 2021-02-12T23:40:54Z- AHRS_M2 msg: 1613173254.77 epoch sec; seqNo:5867 2021-02-12T23:40:54Z- AHRS_M2 msg: 0.70 phi; -0.34 theta; -0.21 psi 2021-02-12T23:40:54Z- handleDepth()-Depth_Keller msg: 1613173254782 epoch sec; seqNo:5918; depth 19.44 2021-02-12T23:40:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173254.79 epoch sec; seqNo:5918; 4057580.73 north; 591345.09 east 2021-02-12T23:40:55Z- AHRS_M2 msg: 1613173255.18 epoch sec; seqNo:5868 2021-02-12T23:40:55Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.22 psi 2021-02-12T23:40:55Z- handleDepth()-Depth_Keller msg: 1613173255180 epoch sec; seqNo:5919; depth 19.54 2021-02-12T23:40:55Z- handleDvl() - RDI_Pathfinder msg: 1613173255.20 epoch sec; seqNo:2806 2021-02-12T23:40:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 97.92 , 70.70 , 106.43 2021-02-12T23:40:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , 0.02 2021-02-12T23:40:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173255.21 epoch sec; seqNo:5919; 4057581.12 north; 591346.31 east 2021-02-12T23:40:55Z- AHRS_M2 msg: 1613173255.58 epoch sec; seqNo:5869 2021-02-12T23:40:55Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.22 psi 2021-02-12T23:40:55Z- handleDepth()-Depth_Keller msg: 1613173255588 epoch sec; seqNo:5920; depth 19.69 2021-02-12T23:40:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173255.60 epoch sec; seqNo:5920; 4057581.50 north; 591346.30 east 2021-02-12T23:40:55Z- AHRS_M2 msg: 1613173255.98 epoch sec; seqNo:5870 2021-02-12T23:40:55Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.22 psi 2021-02-12T23:40:55Z- handleDepth()-Depth_Keller msg: 1613173255988 epoch sec; seqNo:5921; depth 19.82 2021-02-12T23:40:56Z- handleDvl() - RDI_Pathfinder msg: 1613173256.01 epoch sec; seqNo:2807 2021-02-12T23:40:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.60 , 96.70 , 70.70 , 107.65 2021-02-12T23:40:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.05 2021-02-12T23:40:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173256.02 epoch sec; seqNo:5921; 4057581.88 north; 591346.30 east 2021-02-12T23:40:56Z- AHRS_M2 msg: 1613173256.39 epoch sec; seqNo:5871 2021-02-12T23:40:56Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.23 psi 2021-02-12T23:40:56Z- handleDepth()-Depth_Keller msg: 1613173256399 epoch sec; seqNo:5922; depth 19.94 2021-02-12T23:40:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173256.41 epoch sec; seqNo:5922; 4057581.88 north; 591346.30 east 2021-02-12T23:40:56Z- AHRS_M2 msg: 1613173256.80 epoch sec; seqNo:5872 2021-02-12T23:40:56Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.23 psi 2021-02-12T23:40:56Z- handleDepth()-Depth_Keller msg: 1613173256802 epoch sec; seqNo:5923; depth 20.07 2021-02-12T23:40:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173256.82 epoch sec; seqNo:5923; 4057582.26 north; 591346.30 east 2021-02-12T23:40:57Z- AHRS_M2 msg: 1613173257.20 epoch sec; seqNo:5873 2021-02-12T23:40:57Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.23 psi 2021-02-12T23:40:57Z- handleDepth()-Depth_Keller msg: 1613173257202 epoch sec; seqNo:5924; depth 20.20 2021-02-12T23:40:57Z- handleDvl() - RDI_Pathfinder msg: 1613173257.21 epoch sec; seqNo:2808 2021-02-12T23:40:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.18 , 96.76 , 69.87 , 106.50 2021-02-12T23:40:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , 0.04 2021-02-12T23:40:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173257.24 epoch sec; seqNo:5924; 4057582.64 north; 591346.29 east 2021-02-12T23:40:57Z- AHRS_M2 msg: 1613173257.60 epoch sec; seqNo:5874 2021-02-12T23:40:57Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.23 psi 2021-02-12T23:40:57Z- handleDepth()-Depth_Keller msg: 1613173257604 epoch sec; seqNo:5925; depth 20.33 2021-02-12T23:40:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173257.62 epoch sec; seqNo:5925; 4057582.65 north; 591347.51 east 2021-02-12T23:40:58Z- AHRS_M2 msg: 1613173258.00 epoch sec; seqNo:5875 2021-02-12T23:40:58Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.23 psi 2021-02-12T23:40:58Z- LcmTrn::handleMessages() - handling 42 messages took 0.69 ms and 128 calls... 2021-02-12T23:40:58Z- waiting for 1613173255.77, time is 1613173258.00 2021-02-12T23:40:58Z- LcmTrn::updateTrn() >>>> pose time 1613173258.00, meas time 1613173257.21 <<<< 2021-02-12T23:40:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173257.21 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:40:58Z- TerrainNav::motionUpdate() - currEstimate = (4057583, 591347.51, 20.33) 2021-02-12T23:40:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:40:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:40:58Z- TNavBF::Projecting Measurements 2021-02-12T23:40:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:40:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:40:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:40:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:40:58Z- TNavFilter::4 good beams. 2021-02-12T23:40:58Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:40:58Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:40:58Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:40:58Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:40:58Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:40:58Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:40:58Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:40:58Z- TNavBF::mmse selected from filter -1 2021-02-12T23:40:58Z- TerrainMapOctree: (vehN, vehE ) = (4057582.19, 591347.15). 2021-02-12T23:40:58Z- TerrainMapOctree: Min Distance = 4677.54. 2021-02-12T23:40:58Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:40:58Z- There is no specific good beam that all particles have in common. 2021-02-12T23:40:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:40:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 9.04 2021-02-12T23:40:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:40:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 14.98 2021-02-12T23:40:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:40:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 7.81 2021-02-12T23:40:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:40:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.79 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 18.15 2021-02-12T23:40:58Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:40:58Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:40:58Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:40:58Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:40:58Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:40:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:40:58Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:40:58Z- Filter 0 No beams used in subcloud update 2021-02-12T23:40:58Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:40:58Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:40:58Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 744.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:40:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173257.21 sec. 2021-02-12T23:40:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:40:58Z- TNavBF::mmse selected from filter -1 2021-02-12T23:40:58Z- variances([0], [2], and psi_berg) = 744.741894 833.028312 0.000000 2021-02-12T23:40:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:40:58Z- LcmTrn::publishEstimates() - MLE : 4057571.15 591308.01 20.33 2021-02-12T23:40:58Z- LcmTrn::publishEstimates() - MMSE : 4057582.65 591347.51 20.33 2021-02-12T23:40:58Z- LcmTrn::publishEstimates() - COVAR: 744.74 833.03 0.79 0.00 2021-02-12T23:40:58Z- handleDepth()-Depth_Keller msg: 1613173258014 epoch sec; seqNo:5926; depth 20.45 2021-02-12T23:40:58Z- handleDvl() - RDI_Pathfinder msg: 1613173258.03 epoch sec; seqNo:2809 2021-02-12T23:40:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.78 , 97.98 , 69.46 , 106.50 2021-02-12T23:40:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.06 , -0.02 2021-02-12T23:40:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173258.04 epoch sec; seqNo:5926; 4057583.03 north; 591347.51 east 2021-02-12T23:40:58Z- AHRS_M2 msg: 1613173258.41 epoch sec; seqNo:5876 2021-02-12T23:40:58Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.23 psi 2021-02-12T23:40:58Z- handleDepth()-Depth_Keller msg: 1613173258412 epoch sec; seqNo:5927; depth 20.58 2021-02-12T23:40:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173258.42 epoch sec; seqNo:5927; 4057583.41 north; 591347.51 east 2021-02-12T23:40:58Z- AHRS_M2 msg: 1613173258.81 epoch sec; seqNo:5877 2021-02-12T23:40:58Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.24 psi 2021-02-12T23:40:58Z- handleDepth()-Depth_Keller msg: 1613173258821 epoch sec; seqNo:5928; depth 20.73 2021-02-12T23:40:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173258.83 epoch sec; seqNo:5928; 4057583.41 north; 591347.51 east 2021-02-12T23:40:59Z- AHRS_M2 msg: 1613173259.22 epoch sec; seqNo:5878 2021-02-12T23:40:59Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.24 psi 2021-02-12T23:40:59Z- handleDepth()-Depth_Keller msg: 1613173259221 epoch sec; seqNo:5929; depth 20.84 2021-02-12T23:40:59Z- handleDvl() - RDI_Pathfinder msg: 1613173259.23 epoch sec; seqNo:2810 2021-02-12T23:40:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 69.47 , 106.50 2021-02-12T23:40:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , -0.08 2021-02-12T23:40:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173259.24 epoch sec; seqNo:5929; 4057583.79 north; 591347.50 east 2021-02-12T23:40:59Z- AHRS_M2 msg: 1613173259.62 epoch sec; seqNo:5879 2021-02-12T23:40:59Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.24 psi 2021-02-12T23:40:59Z- handleDepth()-Depth_Keller msg: 1613173259633 epoch sec; seqNo:5930; depth 20.96 2021-02-12T23:40:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173259.64 epoch sec; seqNo:5930; 4057584.18 north; 591348.72 east 2021-02-12T23:41:00Z- AHRS_M2 msg: 1613173260.02 epoch sec; seqNo:5880 2021-02-12T23:41:00Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.24 psi 2021-02-12T23:41:00Z- handleDepth()-Depth_Keller msg: 1613173260028 epoch sec; seqNo:5931; depth 21.10 2021-02-12T23:41:00Z- handleDvl() - RDI_Pathfinder msg: 1613173260.04 epoch sec; seqNo:2811 2021-02-12T23:41:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 97.92 , 69.47 , 105.22 2021-02-12T23:41:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , -0.10 2021-02-12T23:41:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173260.06 epoch sec; seqNo:5931; 4057584.56 north; 591348.71 east 2021-02-12T23:41:00Z- AHRS_M2 msg: 1613173260.43 epoch sec; seqNo:5881 2021-02-12T23:41:00Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.24 psi 2021-02-12T23:41:00Z- handleDepth()-Depth_Keller msg: 1613173260433 epoch sec; seqNo:5932; depth 21.23 2021-02-12T23:41:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173260.44 epoch sec; seqNo:5932; 4057584.56 north; 591348.71 east 2021-02-12T23:41:00Z- AHRS_M2 msg: 1613173260.83 epoch sec; seqNo:5882 2021-02-12T23:41:00Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.23 psi 2021-02-12T23:41:00Z- handleDepth()-Depth_Keller msg: 1613173260841 epoch sec; seqNo:5933; depth 21.35 2021-02-12T23:41:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173260.85 epoch sec; seqNo:5933; 4057584.94 north; 591348.71 east 2021-02-12T23:41:01Z- AHRS_M2 msg: 1613173261.24 epoch sec; seqNo:5883 2021-02-12T23:41:01Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.23 psi 2021-02-12T23:41:01Z- handleDepth()-Depth_Keller msg: 1613173261249 epoch sec; seqNo:5934; depth 21.49 2021-02-12T23:41:01Z- handleDvl() - RDI_Pathfinder msg: 1613173261.26 epoch sec; seqNo:2812 2021-02-12T23:41:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 96.83 , 67.84 , 105.35 2021-02-12T23:41:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , -0.06 2021-02-12T23:41:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173261.27 epoch sec; seqNo:5934; 4057585.32 north; 591348.71 east 2021-02-12T23:41:01Z- AHRS_M2 msg: 1613173261.64 epoch sec; seqNo:5884 2021-02-12T23:41:01Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.23 psi 2021-02-12T23:41:01Z- handleDepth()-Depth_Keller msg: 1613173261644 epoch sec; seqNo:5935; depth 21.62 2021-02-12T23:41:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173261.66 epoch sec; seqNo:5935; 4057585.69 north; 591348.70 east 2021-02-12T23:41:02Z- AHRS_M2 msg: 1613173262.04 epoch sec; seqNo:5885 2021-02-12T23:41:02Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.22 psi 2021-02-12T23:41:02Z- handleDepth()-Depth_Keller msg: 1613173262048 epoch sec; seqNo:5936; depth 21.76 2021-02-12T23:41:02Z- handleDvl() - RDI_Pathfinder msg: 1613173262.06 epoch sec; seqNo:2813 2021-02-12T23:41:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 97.98 , 69.06 , 104.07 2021-02-12T23:41:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.11 , -0.01 2021-02-12T23:41:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173262.09 epoch sec; seqNo:5936; 4057585.69 north; 591348.70 east 2021-02-12T23:41:02Z- AHRS_M2 msg: 1613173262.45 epoch sec; seqNo:5886 2021-02-12T23:41:02Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.22 psi 2021-02-12T23:41:02Z- handleDepth()-Depth_Keller msg: 1613173262455 epoch sec; seqNo:5937; depth 21.87 2021-02-12T23:41:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173262.47 epoch sec; seqNo:5937; 4057586.09 north; 591349.92 east 2021-02-12T23:41:02Z- AHRS_M2 msg: 1613173262.88 epoch sec; seqNo:5887 2021-02-12T23:41:02Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.21 psi 2021-02-12T23:41:02Z- handleDepth()-Depth_Keller msg: 1613173262889 epoch sec; seqNo:5938; depth 22.00 2021-02-12T23:41:02Z- handleDvl() - RDI_Pathfinder msg: 1613173262.90 epoch sec; seqNo:2814 2021-02-12T23:41:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.14 , 94.46 , 0.00 , 104.20 2021-02-12T23:41:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.10 , 0.01 2021-02-12T23:41:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173262.91 epoch sec; seqNo:5938; 4057586.47 north; 591349.92 east 2021-02-12T23:41:03Z- LcmTrn::handleMessages() - handling 44 messages took 0.62 ms and 132 calls... 2021-02-12T23:41:03Z- waiting for 1613173261.00, time is 1613173262.91 2021-02-12T23:41:03Z- LcmTrn::updateTrn() equating pose 1613173262.91 and meas times 1613173262.91 2021-02-12T23:41:03Z- LcmTrn::updateTrn() >>>> pose time 1613173262.91, meas time 1613173262.91 <<<< 2021-02-12T23:41:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173262.91 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:41:03Z- TerrainNav::motionUpdate() - currEstimate = (4057586, 591349.92, 22.00) 2021-02-12T23:41:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:41:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:41:03Z- TNavBF::Projecting Measurements 2021-02-12T23:41:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:41:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:41:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:41:03Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:41:03Z- TNavFilter::3 good beams. 2021-02-12T23:41:03Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:41:03Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:41:03Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:41:03Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:41:03Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:41:03Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:41:03Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:41:03Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:03Z- TerrainMapOctree: (vehN, vehE ) = (4057586.47, 591349.92). 2021-02-12T23:41:03Z- TerrainMapOctree: Min Distance = 4672.47. 2021-02-12T23:41:03Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:41:03Z- There is no specific good beam that all particles have in common. 2021-02-12T23:41:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:41:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-12T23:41:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:41:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 14.28 2021-02-12T23:41:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:41:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 17.37 2021-02-12T23:41:03Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:41:03Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:41:03Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:41:03Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:41:03Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:41:03Z- Filter 0 No beams used in subcloud update 2021-02-12T23:41:03Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:41:03Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:41:03Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 744.6 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:41:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173262.91 sec. 2021-02-12T23:41:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:41:03Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:03Z- variances([0], [2], and psi_berg) = 744.645428 833.028228 0.000000 2021-02-12T23:41:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:41:03Z- LcmTrn::publishEstimates() - MLE : 4057574.97 591308.42 22.00 2021-02-12T23:41:03Z- LcmTrn::publishEstimates() - MMSE : 4057586.47 591349.92 22.00 2021-02-12T23:41:03Z- LcmTrn::publishEstimates() - COVAR: 744.65 833.03 0.84 0.00 2021-02-12T23:41:03Z- AHRS_M2 msg: 1613173263.26 epoch sec; seqNo:5888 2021-02-12T23:41:03Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.21 psi 2021-02-12T23:41:03Z- handleDepth()-Depth_Keller msg: 1613173263260 epoch sec; seqNo:5939; depth 22.14 2021-02-12T23:41:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173263.27 epoch sec; seqNo:5939; 4057586.84 north; 591349.91 east 2021-02-12T23:41:03Z- AHRS_M2 msg: 1613173263.66 epoch sec; seqNo:5889 2021-02-12T23:41:03Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.21 psi 2021-02-12T23:41:03Z- handleDepth()-Depth_Keller msg: 1613173263664 epoch sec; seqNo:5940; depth 22.26 2021-02-12T23:41:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173263.67 epoch sec; seqNo:5940; 4057587.22 north; 591349.91 east 2021-02-12T23:41:04Z- AHRS_M2 msg: 1613173264.06 epoch sec; seqNo:5890 2021-02-12T23:41:04Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.21 psi 2021-02-12T23:41:04Z- handleDepth()-Depth_Keller msg: 1613173264070 epoch sec; seqNo:5941; depth 22.38 2021-02-12T23:41:04Z- handleDvl() - RDI_Pathfinder msg: 1613173264.08 epoch sec; seqNo:2815 2021-02-12T23:41:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.14 , 92.84 , 67.01 , 103.79 2021-02-12T23:41:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , 0.02 2021-02-12T23:41:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173264.09 epoch sec; seqNo:5941; 4057587.22 north; 591349.91 east 2021-02-12T23:41:04Z- AHRS_M2 msg: 1613173264.47 epoch sec; seqNo:5891 2021-02-12T23:41:04Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.20 psi 2021-02-12T23:41:04Z- handleDepth()-Depth_Keller msg: 1613173264473 epoch sec; seqNo:5942; depth 22.52 2021-02-12T23:41:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173264.49 epoch sec; seqNo:5942; 4057587.60 north; 591349.90 east 2021-02-12T23:41:04Z- AHRS_M2 msg: 1613173264.87 epoch sec; seqNo:5892 2021-02-12T23:41:04Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.20 psi 2021-02-12T23:41:04Z- handleDepth()-Depth_Keller msg: 1613173264881 epoch sec; seqNo:5943; depth 22.64 2021-02-12T23:41:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173264.89 epoch sec; seqNo:5943; 4057587.98 north; 591349.90 east 2021-02-12T23:41:05Z- AHRS_M2 msg: 1613173265.28 epoch sec; seqNo:5893 2021-02-12T23:41:05Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.20 psi 2021-02-12T23:41:05Z- handleDepth()-Depth_Keller msg: 1613173265285 epoch sec; seqNo:5944; depth 22.77 2021-02-12T23:41:05Z- handleDvl() - RDI_Pathfinder msg: 1613173265.30 epoch sec; seqNo:2816 2021-02-12T23:41:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.79 , 98.25 , 67.84 , 103.12 2021-02-12T23:41:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , -0.03 2021-02-12T23:41:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173265.31 epoch sec; seqNo:5944; 4057588.36 north; 591349.89 east 2021-02-12T23:41:05Z- AHRS_M2 msg: 1613173265.68 epoch sec; seqNo:5894 2021-02-12T23:41:05Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.20 psi 2021-02-12T23:41:05Z- handleDepth()-Depth_Keller msg: 1613173265686 epoch sec; seqNo:5945; depth 22.90 2021-02-12T23:41:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173265.70 epoch sec; seqNo:5945; 4057588.37 north; 591351.12 east 2021-02-12T23:41:06Z- AHRS_M2 msg: 1613173266.08 epoch sec; seqNo:5895 2021-02-12T23:41:06Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.20 psi 2021-02-12T23:41:06Z- handleDepth()-Depth_Keller msg: 1613173266098 epoch sec; seqNo:5946; depth 23.03 2021-02-12T23:41:06Z- handleDvl() - RDI_Pathfinder msg: 1613173266.11 epoch sec; seqNo:2817 2021-02-12T23:41:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.05 , 67.43 , 101.70 2021-02-12T23:41:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , -0.04 2021-02-12T23:41:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173266.12 epoch sec; seqNo:5946; 4057588.75 north; 591351.11 east 2021-02-12T23:41:06Z- AHRS_M2 msg: 1613173266.49 epoch sec; seqNo:5896 2021-02-12T23:41:06Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.19 psi 2021-02-12T23:41:06Z- handleDepth()-Depth_Keller msg: 1613173266492 epoch sec; seqNo:5947; depth 23.16 2021-02-12T23:41:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173266.50 epoch sec; seqNo:5947; 4057589.13 north; 591351.11 east 2021-02-12T23:41:06Z- AHRS_M2 msg: 1613173266.90 epoch sec; seqNo:5897 2021-02-12T23:41:06Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.19 psi 2021-02-12T23:41:06Z- handleDepth()-Depth_Keller msg: 1613173266909 epoch sec; seqNo:5948; depth 23.27 2021-02-12T23:41:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173266.94 epoch sec; seqNo:5948; 4057589.51 north; 591351.10 east 2021-02-12T23:41:07Z- AHRS_M2 msg: 1613173267.30 epoch sec; seqNo:5898 2021-02-12T23:41:07Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.19 psi 2021-02-12T23:41:07Z- handleDepth()-Depth_Keller msg: 1613173267302 epoch sec; seqNo:5949; depth 23.39 2021-02-12T23:41:07Z- handleDvl() - RDI_Pathfinder msg: 1613173267.31 epoch sec; seqNo:2818 2021-02-12T23:41:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 91.76 , 65.38 , 99.07 2021-02-12T23:41:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , -0.04 2021-02-12T23:41:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173267.32 epoch sec; seqNo:5949; 4057589.51 north; 591351.10 east 2021-02-12T23:41:07Z- AHRS_M2 msg: 1613173267.70 epoch sec; seqNo:5899 2021-02-12T23:41:07Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-12T23:41:07Z- handleDepth()-Depth_Keller msg: 1613173267709 epoch sec; seqNo:5950; depth 23.52 2021-02-12T23:41:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173267.72 epoch sec; seqNo:5950; 4057589.89 north; 591351.10 east 2021-02-12T23:41:08Z- AHRS_M2 msg: 1613173268.10 epoch sec; seqNo:5900 2021-02-12T23:41:08Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-12T23:41:08Z- handleDepth()-Depth_Keller msg: 1613173268121 epoch sec; seqNo:5951; depth 23.65 2021-02-12T23:41:08Z- handleDvl() - RDI_Pathfinder msg: 1613173268.16 epoch sec; seqNo:2819 2021-02-12T23:41:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 90.21 , 66.20 , 101.03 2021-02-12T23:41:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.10 , -0.02 2021-02-12T23:41:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173268.17 epoch sec; seqNo:5951; 4057590.27 north; 591351.10 east 2021-02-12T23:41:08Z- LcmTrn::handleMessages() - handling 44 messages took 0.74 ms and 129 calls... 2021-02-12T23:41:08Z- waiting for 1613173265.91, time is 1613173268.17 2021-02-12T23:41:08Z- LcmTrn::updateTrn() equating pose 1613173268.17 and meas times 1613173268.17 2021-02-12T23:41:08Z- LcmTrn::updateTrn() >>>> pose time 1613173268.17, meas time 1613173268.17 <<<< 2021-02-12T23:41:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173268.17 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:41:08Z- TerrainNav::motionUpdate() - currEstimate = (4057590, 591351.10, 23.65) 2021-02-12T23:41:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:41:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:41:08Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173268.17 sec.; No relevant navigation data available 2021-02-12T23:41:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:41:08Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:08Z- variances([0], [2], and psi_berg) = 744.554888 833.028150 0.000000 2021-02-12T23:41:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:41:08Z- LcmTrn::publishEstimates() - MLE : 4057579.77 591309.60 23.65 2021-02-12T23:41:08Z- LcmTrn::publishEstimates() - MMSE : 4057590.27 591351.10 23.65 2021-02-12T23:41:08Z- LcmTrn::publishEstimates() - COVAR: 744.55 833.03 1.04 0.00 2021-02-12T23:41:08Z- AHRS_M2 msg: 1613173268.51 epoch sec; seqNo:5901 2021-02-12T23:41:08Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-12T23:41:08Z- handleDepth()-Depth_Keller msg: 1613173268521 epoch sec; seqNo:5952; depth 23.79 2021-02-12T23:41:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173268.53 epoch sec; seqNo:5952; 4057590.65 north; 591351.09 east 2021-02-12T23:41:08Z- AHRS_M2 msg: 1613173268.91 epoch sec; seqNo:5902 2021-02-12T23:41:08Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-12T23:41:08Z- handleDepth()-Depth_Keller msg: 1613173268916 epoch sec; seqNo:5953; depth 23.89 2021-02-12T23:41:08Z- handleDvl() - RDI_Pathfinder msg: 1613173268.93 epoch sec; seqNo:2820 2021-02-12T23:41:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 89.87 , 64.97 , 99.61 2021-02-12T23:41:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.07 , 0.02 2021-02-12T23:41:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173268.94 epoch sec; seqNo:5953; 4057590.66 north; 591352.31 east 2021-02-12T23:41:09Z- AHRS_M2 msg: 1613173269.32 epoch sec; seqNo:5903 2021-02-12T23:41:09Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-12T23:41:09Z- handleDepth()-Depth_Keller msg: 1613173269320 epoch sec; seqNo:5954; depth 24.03 2021-02-12T23:41:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173269.33 epoch sec; seqNo:5954; 4057591.04 north; 591352.31 east 2021-02-12T23:41:09Z- AHRS_M2 msg: 1613173269.72 epoch sec; seqNo:5904 2021-02-12T23:41:09Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-12T23:41:09Z- handleDepth()-Depth_Keller msg: 1613173269729 epoch sec; seqNo:5955; depth 24.15 2021-02-12T23:41:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173269.74 epoch sec; seqNo:5955; 4057591.42 north; 591352.30 east 2021-02-12T23:41:10Z- AHRS_M2 msg: 1613173270.13 epoch sec; seqNo:5905 2021-02-12T23:41:10Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.18 psi 2021-02-12T23:41:10Z- handleDepth()-Depth_Keller msg: 1613173270135 epoch sec; seqNo:5956; depth 24.28 2021-02-12T23:41:10Z- handleDvl() - RDI_Pathfinder msg: 1613173270.16 epoch sec; seqNo:2821 2021-02-12T23:41:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 88.05 , 64.97 , 98.86 2021-02-12T23:41:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , 0.05 2021-02-12T23:41:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173270.17 epoch sec; seqNo:5956; 4057591.80 north; 591352.30 east 2021-02-12T23:41:10Z- AHRS_M2 msg: 1613173270.53 epoch sec; seqNo:5906 2021-02-12T23:41:10Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.18 psi 2021-02-12T23:41:10Z- handleDepth()-Depth_Keller msg: 1613173270536 epoch sec; seqNo:5957; depth 24.41 2021-02-12T23:41:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173270.57 epoch sec; seqNo:5957; 4057591.80 north; 591352.30 east 2021-02-12T23:41:10Z- AHRS_M2 msg: 1613173270.94 epoch sec; seqNo:5907 2021-02-12T23:41:10Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.17 psi 2021-02-12T23:41:10Z- handleDepth()-Depth_Keller msg: 1613173270956 epoch sec; seqNo:5958; depth 24.50 2021-02-12T23:41:10Z- handleDvl() - RDI_Pathfinder msg: 1613173270.98 epoch sec; seqNo:2822 2021-02-12T23:41:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 88.11 , 65.38 , 98.93 2021-02-12T23:41:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , 0.05 2021-02-12T23:41:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173270.99 epoch sec; seqNo:5958; 4057592.17 north; 591352.30 east 2021-02-12T23:41:11Z- AHRS_M2 msg: 1613173271.34 epoch sec; seqNo:5908 2021-02-12T23:41:11Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.17 psi 2021-02-12T23:41:11Z- handleDepth()-Depth_Keller msg: 1613173271340 epoch sec; seqNo:5959; depth 24.65 2021-02-12T23:41:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173271.37 epoch sec; seqNo:5959; 4057592.55 north; 591352.29 east 2021-02-12T23:41:11Z- AHRS_M2 msg: 1613173271.74 epoch sec; seqNo:5909 2021-02-12T23:41:11Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.17 psi 2021-02-12T23:41:11Z- handleDepth()-Depth_Keller msg: 1613173271744 epoch sec; seqNo:5960; depth 24.76 2021-02-12T23:41:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173271.75 epoch sec; seqNo:5960; 4057592.57 north; 591353.51 east 2021-02-12T23:41:12Z- AHRS_M2 msg: 1613173272.15 epoch sec; seqNo:5910 2021-02-12T23:41:12Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-12T23:41:12Z- handleDepth()-Depth_Keller msg: 1613173272156 epoch sec; seqNo:5961; depth 24.87 2021-02-12T23:41:12Z- handleDvl() - RDI_Pathfinder msg: 1613173272.17 epoch sec; seqNo:2823 2021-02-12T23:41:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 87.44 , 65.38 , 98.39 2021-02-12T23:41:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , 0.05 2021-02-12T23:41:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173272.18 epoch sec; seqNo:5961; 4057592.94 north; 591353.51 east 2021-02-12T23:41:12Z- AHRS_M2 msg: 1613173272.55 epoch sec; seqNo:5911 2021-02-12T23:41:12Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-12T23:41:12Z- handleDepth()-Depth_Keller msg: 1613173272553 epoch sec; seqNo:5962; depth 25.00 2021-02-12T23:41:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173272.57 epoch sec; seqNo:5962; 4057593.32 north; 591353.51 east 2021-02-12T23:41:12Z- AHRS_M2 msg: 1613173272.95 epoch sec; seqNo:5912 2021-02-12T23:41:12Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.16 psi 2021-02-12T23:41:12Z- handleDepth()-Depth_Keller msg: 1613173272956 epoch sec; seqNo:5963; depth 25.11 2021-02-12T23:41:12Z- handleDvl() - RDI_Pathfinder msg: 1613173272.98 epoch sec; seqNo:2824 2021-02-12T23:41:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 0.00 , 64.97 , 98.32 2021-02-12T23:41:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , -0.01 2021-02-12T23:41:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173272.99 epoch sec; seqNo:5963; 4057593.70 north; 591353.50 east 2021-02-12T23:41:13Z- LcmTrn::handleMessages() - handling 41 messages took 0.75 ms and 126 calls... 2021-02-12T23:41:13Z- waiting for 1613173271.17, time is 1613173272.99 2021-02-12T23:41:13Z- LcmTrn::updateTrn() equating pose 1613173272.99 and meas times 1613173272.99 2021-02-12T23:41:13Z- LcmTrn::updateTrn() >>>> pose time 1613173272.99, meas time 1613173272.99 <<<< 2021-02-12T23:41:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173272.99 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:41:13Z- TerrainNav::motionUpdate() - currEstimate = (4057594, 591353.50, 25.11) 2021-02-12T23:41:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:41:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:41:13Z- TNavBF::Projecting Measurements 2021-02-12T23:41:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:41:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:41:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:41:13Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:41:13Z- TNavFilter::3 good beams. 2021-02-12T23:41:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:41:13Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:41:13Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:41:13Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:41:13Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:41:13Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:41:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:41:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:13Z- TerrainMapOctree: (vehN, vehE ) = (4057593.70, 591353.50). 2021-02-12T23:41:13Z- TerrainMapOctree: Min Distance = 4664.57. 2021-02-12T23:41:13Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:41:13Z- There is no specific good beam that all particles have in common. 2021-02-12T23:41:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:41:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-12T23:41:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:41:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.116 ExpectedMeasDiff: nan currMeas.covariance: 6.75 2021-02-12T23:41:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:41:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.47 2021-02-12T23:41:13Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:41:13Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:41:13Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:41:13Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:41:13Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:41:13Z- Filter 0 No beams used in subcloud update 2021-02-12T23:41:13Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:41:13Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:41:13Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 744.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:41:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173272.99 sec. 2021-02-12T23:41:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:41:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:13Z- variances([0], [2], and psi_berg) = 744.462060 833.028069 0.000000 2021-02-12T23:41:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:41:13Z- LcmTrn::publishEstimates() - MLE : 4057583.20 591312.00 25.11 2021-02-12T23:41:13Z- LcmTrn::publishEstimates() - MMSE : 4057593.70 591353.50 25.11 2021-02-12T23:41:13Z- LcmTrn::publishEstimates() - COVAR: 744.46 833.03 0.93 0.00 2021-02-12T23:41:13Z- AHRS_M2 msg: 1613173273.36 epoch sec; seqNo:5913 2021-02-12T23:41:13Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.16 psi 2021-02-12T23:41:13Z- handleDepth()-Depth_Keller msg: 1613173273365 epoch sec; seqNo:5964; depth 25.25 2021-02-12T23:41:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173273.39 epoch sec; seqNo:5964; 4057593.70 north; 591353.50 east 2021-02-12T23:41:13Z- AHRS_M2 msg: 1613173273.77 epoch sec; seqNo:5914 2021-02-12T23:41:13Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.16 psi 2021-02-12T23:41:13Z- handleDepth()-Depth_Keller msg: 1613173273770 epoch sec; seqNo:5965; depth 25.35 2021-02-12T23:41:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173273.78 epoch sec; seqNo:5965; 4057594.08 north; 591353.50 east 2021-02-12T23:41:14Z- AHRS_M2 msg: 1613173274.18 epoch sec; seqNo:5915 2021-02-12T23:41:14Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.16 psi 2021-02-12T23:41:14Z- handleDepth()-Depth_Keller msg: 1613173274183 epoch sec; seqNo:5966; depth 25.47 2021-02-12T23:41:14Z- handleDvl() - RDI_Pathfinder msg: 1613173274.19 epoch sec; seqNo:2825 2021-02-12T23:41:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 87.17 , 64.16 , 96.90 2021-02-12T23:41:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , -0.06 2021-02-12T23:41:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173274.20 epoch sec; seqNo:5966; 4057594.47 north; 591354.71 east 2021-02-12T23:41:14Z- AHRS_M2 msg: 1613173274.57 epoch sec; seqNo:5916 2021-02-12T23:41:14Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.17 psi 2021-02-12T23:41:14Z- handleDepth()-Depth_Keller msg: 1613173274572 epoch sec; seqNo:5967; depth 25.61 2021-02-12T23:41:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173274.59 epoch sec; seqNo:5967; 4057594.85 north; 591354.71 east 2021-02-12T23:41:14Z- AHRS_M2 msg: 1613173274.99 epoch sec; seqNo:5917 2021-02-12T23:41:14Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.17 psi 2021-02-12T23:41:14Z- handleDepth()-Depth_Keller msg: 1613173274995 epoch sec; seqNo:5968; depth 25.73 2021-02-12T23:41:15Z- handleDvl() - RDI_Pathfinder msg: 1613173275.01 epoch sec; seqNo:2826 2021-02-12T23:41:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 87.17 , 0.00 , 97.98 2021-02-12T23:41:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.11 , -0.07 2021-02-12T23:41:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173275.02 epoch sec; seqNo:5968; 4057595.23 north; 591354.71 east 2021-02-12T23:41:15Z- AHRS_M2 msg: 1613173275.38 epoch sec; seqNo:5918 2021-02-12T23:41:15Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.16 psi 2021-02-12T23:41:15Z- handleDepth()-Depth_Keller msg: 1613173275380 epoch sec; seqNo:5969; depth 25.83 2021-02-12T23:41:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173275.40 epoch sec; seqNo:5969; 4057595.23 north; 591354.71 east 2021-02-12T23:41:15Z- AHRS_M2 msg: 1613173275.78 epoch sec; seqNo:5919 2021-02-12T23:41:15Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.16 psi 2021-02-12T23:41:15Z- handleDepth()-Depth_Keller msg: 1613173275784 epoch sec; seqNo:5970; depth 25.96 2021-02-12T23:41:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173275.79 epoch sec; seqNo:5970; 4057595.61 north; 591354.70 east 2021-02-12T23:41:16Z- AHRS_M2 msg: 1613173276.19 epoch sec; seqNo:5920 2021-02-12T23:41:16Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.16 psi 2021-02-12T23:41:16Z- handleDepth()-Depth_Keller msg: 1613173276195 epoch sec; seqNo:5971; depth 26.08 2021-02-12T23:41:16Z- handleDvl() - RDI_Pathfinder msg: 1613173276.21 epoch sec; seqNo:2827 2021-02-12T23:41:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 85.75 , 64.16 , 97.92 2021-02-12T23:41:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.16 , -0.04 2021-02-12T23:41:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173276.22 epoch sec; seqNo:5971; 4057595.99 north; 591354.70 east 2021-02-12T23:41:16Z- AHRS_M2 msg: 1613173276.59 epoch sec; seqNo:5921 2021-02-12T23:41:16Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.16 psi 2021-02-12T23:41:16Z- handleDepth()-Depth_Keller msg: 1613173276592 epoch sec; seqNo:5972; depth 26.20 2021-02-12T23:41:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173276.61 epoch sec; seqNo:5972; 4057596.37 north; 591354.69 east 2021-02-12T23:41:16Z- AHRS_M2 msg: 1613173276.99 epoch sec; seqNo:5922 2021-02-12T23:41:16Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.16 psi 2021-02-12T23:41:17Z- handleDepth()-Depth_Keller msg: 1613173277000 epoch sec; seqNo:5973; depth 26.32 2021-02-12T23:41:17Z- handleDvl() - RDI_Pathfinder msg: 1613173277.02 epoch sec; seqNo:2828 2021-02-12T23:41:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 85.00 , 63.75 , 97.98 2021-02-12T23:41:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.13 , 0.03 2021-02-12T23:41:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173277.03 epoch sec; seqNo:5973; 4057596.75 north; 591354.69 east 2021-02-12T23:41:17Z- AHRS_M2 msg: 1613173277.39 epoch sec; seqNo:5923 2021-02-12T23:41:17Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.16 psi 2021-02-12T23:41:17Z- handleDepth()-Depth_Keller msg: 1613173277400 epoch sec; seqNo:5974; depth 26.44 2021-02-12T23:41:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173277.41 epoch sec; seqNo:5974; 4057596.76 north; 591355.91 east 2021-02-12T23:41:17Z- AHRS_M2 msg: 1613173277.82 epoch sec; seqNo:5924 2021-02-12T23:41:17Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.16 psi 2021-02-12T23:41:17Z- handleDepth()-Depth_Keller msg: 1613173277830 epoch sec; seqNo:5975; depth 26.54 2021-02-12T23:41:17Z- handleDvl() - RDI_Pathfinder msg: 1613173277.84 epoch sec; seqNo:2829 2021-02-12T23:41:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 0.00 , 63.34 , 95.82 2021-02-12T23:41:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.04 2021-02-12T23:41:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173277.86 epoch sec; seqNo:5975; 4057597.14 north; 591355.91 east 2021-02-12T23:41:18Z- AHRS_M2 msg: 1613173278.20 epoch sec; seqNo:5925 2021-02-12T23:41:18Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.16 psi 2021-02-12T23:41:18Z- handleDepth()-Depth_Keller msg: 1613173278226 epoch sec; seqNo:5976; depth 26.67 2021-02-12T23:41:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173278.24 epoch sec; seqNo:5976; 4057597.52 north; 591355.90 east 2021-02-12T23:41:18Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-12T23:41:18Z- waiting for 1613173275.99, time is 1613173278.24 2021-02-12T23:41:18Z- LcmTrn::updateTrn() >>>> pose time 1613173278.24, meas time 1613173277.84 <<<< 2021-02-12T23:41:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173277.84 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:41:18Z- TerrainNav::motionUpdate() - currEstimate = (4057598, 591355.90, 26.67) 2021-02-12T23:41:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:41:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:41:18Z- TNavBF::Projecting Measurements 2021-02-12T23:41:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:41:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:41:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:41:18Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:41:18Z- TNavFilter::3 good beams. 2021-02-12T23:41:18Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:41:18Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:41:18Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:41:18Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:41:18Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:41:18Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:41:18Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:41:18Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:18Z- TerrainMapOctree: (vehN, vehE ) = (4057597.23, 591355.72). 2021-02-12T23:41:18Z- TerrainMapOctree: Min Distance = 4660.43. 2021-02-12T23:41:18Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:41:18Z- There is no specific good beam that all particles have in common. 2021-02-12T23:41:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:41:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-12T23:41:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:41:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 6.42 2021-02-12T23:41:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:41:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 14.69 2021-02-12T23:41:18Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:41:18Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:41:18Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:41:18Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:41:18Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:41:18Z- Filter 0 No beams used in subcloud update 2021-02-12T23:41:18Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:41:18Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:41:18Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 744.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:41:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173277.84 sec. 2021-02-12T23:41:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:41:18Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:18Z- variances([0], [2], and psi_berg) = 744.343284 833.027966 0.000000 2021-02-12T23:41:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:41:18Z- LcmTrn::publishEstimates() - MLE : 4057587.02 591314.40 26.67 2021-02-12T23:41:18Z- LcmTrn::publishEstimates() - MMSE : 4057597.52 591355.90 26.67 2021-02-12T23:41:18Z- LcmTrn::publishEstimates() - COVAR: 744.34 833.03 0.88 0.00 2021-02-12T23:41:18Z- AHRS_M2 msg: 1613173278.61 epoch sec; seqNo:5926 2021-02-12T23:41:18Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.17 psi 2021-02-12T23:41:18Z- handleDepth()-Depth_Keller msg: 1613173278612 epoch sec; seqNo:5977; depth 26.76 2021-02-12T23:41:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173278.62 epoch sec; seqNo:5977; 4057597.90 north; 591355.90 east 2021-02-12T23:41:19Z- AHRS_M2 msg: 1613173279.01 epoch sec; seqNo:5927 2021-02-12T23:41:19Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.17 psi 2021-02-12T23:41:19Z- handleDepth()-Depth_Keller msg: 1613173279016 epoch sec; seqNo:5978; depth 26.88 2021-02-12T23:41:19Z- handleDvl() - RDI_Pathfinder msg: 1613173279.03 epoch sec; seqNo:2830 2021-02-12T23:41:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 85.00 , 63.34 , 97.98 2021-02-12T23:41:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , 0.03 2021-02-12T23:41:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173279.04 epoch sec; seqNo:5978; 4057597.90 north; 591355.90 east 2021-02-12T23:41:19Z- AHRS_M2 msg: 1613173279.42 epoch sec; seqNo:5928 2021-02-12T23:41:19Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.17 psi 2021-02-12T23:41:19Z- handleDepth()-Depth_Keller msg: 1613173279425 epoch sec; seqNo:5979; depth 27.01 2021-02-12T23:41:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173279.44 epoch sec; seqNo:5979; 4057598.27 north; 591355.89 east 2021-02-12T23:41:19Z- AHRS_M2 msg: 1613173279.82 epoch sec; seqNo:5929 2021-02-12T23:41:19Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.17 psi 2021-02-12T23:41:19Z- handleDepth()-Depth_Keller msg: 1613173279834 epoch sec; seqNo:5980; depth 27.13 2021-02-12T23:41:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173279.85 epoch sec; seqNo:5980; 4057598.67 north; 591357.11 east 2021-02-12T23:41:20Z- AHRS_M2 msg: 1613173280.22 epoch sec; seqNo:5930 2021-02-12T23:41:20Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.18 psi 2021-02-12T23:41:20Z- handleDepth()-Depth_Keller msg: 1613173280233 epoch sec; seqNo:5981; depth 27.25 2021-02-12T23:41:20Z- handleDvl() - RDI_Pathfinder msg: 1613173280.24 epoch sec; seqNo:2831 2021-02-12T23:41:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 85.00 , 62.93 , 97.98 2021-02-12T23:41:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , -0.01 2021-02-12T23:41:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173280.27 epoch sec; seqNo:5981; 4057599.05 north; 591357.11 east 2021-02-12T23:41:20Z- AHRS_M2 msg: 1613173280.63 epoch sec; seqNo:5931 2021-02-12T23:41:20Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.18 psi 2021-02-12T23:41:20Z- handleDepth()-Depth_Keller msg: 1613173280633 epoch sec; seqNo:5982; depth 27.37 2021-02-12T23:41:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173280.64 epoch sec; seqNo:5982; 4057599.05 north; 591357.11 east 2021-02-12T23:41:21Z- AHRS_M2 msg: 1613173281.03 epoch sec; seqNo:5932 2021-02-12T23:41:21Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.18 psi 2021-02-12T23:41:21Z- handleDepth()-Depth_Keller msg: 1613173281038 epoch sec; seqNo:5983; depth 27.48 2021-02-12T23:41:21Z- handleDvl() - RDI_Pathfinder msg: 1613173281.05 epoch sec; seqNo:2832 2021-02-12T23:41:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 84.93 , 63.33 , 95.75 2021-02-12T23:41:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , -0.04 2021-02-12T23:41:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173281.06 epoch sec; seqNo:5983; 4057599.42 north; 591357.10 east 2021-02-12T23:41:21Z- AHRS_M2 msg: 1613173281.44 epoch sec; seqNo:5933 2021-02-12T23:41:21Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.18 psi 2021-02-12T23:41:21Z- handleDepth()-Depth_Keller msg: 1613173281440 epoch sec; seqNo:5984; depth 27.61 2021-02-12T23:41:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173281.46 epoch sec; seqNo:5984; 4057599.80 north; 591357.10 east 2021-02-12T23:41:21Z- AHRS_M2 msg: 1613173281.84 epoch sec; seqNo:5934 2021-02-12T23:41:21Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.18 psi 2021-02-12T23:41:21Z- handleDepth()-Depth_Keller msg: 1613173281848 epoch sec; seqNo:5985; depth 27.72 2021-02-12T23:41:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173281.86 epoch sec; seqNo:5985; 4057599.80 north; 591357.10 east 2021-02-12T23:41:22Z- AHRS_M2 msg: 1613173282.25 epoch sec; seqNo:5935 2021-02-12T23:41:22Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.19 psi 2021-02-12T23:41:22Z- handleDepth()-Depth_Keller msg: 1613173282261 epoch sec; seqNo:5986; depth 27.84 2021-02-12T23:41:22Z- handleDvl() - RDI_Pathfinder msg: 1613173282.27 epoch sec; seqNo:2833 2021-02-12T23:41:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 86.01 , 62.52 , 94.67 2021-02-12T23:41:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.10 , -0.02 2021-02-12T23:41:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173282.28 epoch sec; seqNo:5986; 4057600.19 north; 591358.32 east 2021-02-12T23:41:22Z- AHRS_M2 msg: 1613173282.65 epoch sec; seqNo:5936 2021-02-12T23:41:22Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.19 psi 2021-02-12T23:41:22Z- handleDepth()-Depth_Keller msg: 1613173282663 epoch sec; seqNo:5987; depth 27.97 2021-02-12T23:41:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173282.67 epoch sec; seqNo:5987; 4057600.57 north; 591358.31 east 2021-02-12T23:41:23Z- AHRS_M2 msg: 1613173283.05 epoch sec; seqNo:5937 2021-02-12T23:41:23Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.19 psi 2021-02-12T23:41:23Z- handleDepth()-Depth_Keller msg: 1613173283061 epoch sec; seqNo:5988; depth 28.10 2021-02-12T23:41:23Z- handleDvl() - RDI_Pathfinder msg: 1613173283.07 epoch sec; seqNo:2834 2021-02-12T23:41:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 85.00 , 61.71 , 97.98 2021-02-12T23:41:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , 0.01 2021-02-12T23:41:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173283.08 epoch sec; seqNo:5988; 4057600.95 north; 591358.31 east 2021-02-12T23:41:23Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-12T23:41:23Z- waiting for 1613173281.24, time is 1613173283.08 2021-02-12T23:41:23Z- LcmTrn::updateTrn() equating pose 1613173283.08 and meas times 1613173283.08 2021-02-12T23:41:23Z- LcmTrn::updateTrn() >>>> pose time 1613173283.08, meas time 1613173283.08 <<<< 2021-02-12T23:41:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173283.08 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:41:23Z- TerrainNav::motionUpdate() - currEstimate = (4057601, 591358.31, 28.10) 2021-02-12T23:41:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:41:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:41:23Z- TNavBF::Projecting Measurements 2021-02-12T23:41:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:41:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:41:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:41:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:41:23Z- TNavFilter::4 good beams. 2021-02-12T23:41:23Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:41:23Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:41:23Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:41:23Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:41:23Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:41:23Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:41:23Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:41:23Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:23Z- TerrainMapOctree: (vehN, vehE ) = (4057600.95, 591358.31). 2021-02-12T23:41:23Z- TerrainMapOctree: Min Distance = 4655.90. 2021-02-12T23:41:23Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:41:23Z- There is no specific good beam that all particles have in common. 2021-02-12T23:41:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:41:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-12T23:41:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:41:23Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 11.56 2021-02-12T23:41:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:41:23Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 6.09 2021-02-12T23:41:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:41:23Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-12T23:41:23Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:41:23Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:41:23Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:41:23Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:41:23Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:41:23Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:41:23Z- Filter 0 No beams used in subcloud update 2021-02-12T23:41:23Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:41:23Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:41:23Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 744.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:41:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173283.08 sec. 2021-02-12T23:41:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:41:23Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:23Z- variances([0], [2], and psi_berg) = 744.250707 833.027886 0.000000 2021-02-12T23:41:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:41:23Z- LcmTrn::publishEstimates() - MLE : 4057590.45 591316.81 28.10 2021-02-12T23:41:23Z- LcmTrn::publishEstimates() - MMSE : 4057600.95 591358.31 28.10 2021-02-12T23:41:23Z- LcmTrn::publishEstimates() - COVAR: 744.25 833.03 0.78 0.00 2021-02-12T23:41:23Z- AHRS_M2 msg: 1613173283.46 epoch sec; seqNo:5938 2021-02-12T23:41:23Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.19 psi 2021-02-12T23:41:23Z- handleDepth()-Depth_Keller msg: 1613173283462 epoch sec; seqNo:5989; depth 28.20 2021-02-12T23:41:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173283.47 epoch sec; seqNo:5989; 4057601.33 north; 591358.30 east 2021-02-12T23:41:23Z- AHRS_M2 msg: 1613173283.87 epoch sec; seqNo:5939 2021-02-12T23:41:23Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.20 psi 2021-02-12T23:41:23Z- handleDepth()-Depth_Keller msg: 1613173283886 epoch sec; seqNo:5990; depth 28.32 2021-02-12T23:41:23Z- handleDvl() - RDI_Pathfinder msg: 1613173283.91 epoch sec; seqNo:2835 2021-02-12T23:41:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 87.17 , 63.34 , 97.98 2021-02-12T23:41:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.06 , 0.05 2021-02-12T23:41:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173283.92 epoch sec; seqNo:5990; 4057601.33 north; 591358.30 east 2021-02-12T23:41:24Z- AHRS_M2 msg: 1613173284.26 epoch sec; seqNo:5940 2021-02-12T23:41:24Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.20 psi 2021-02-12T23:41:24Z- handleDepth()-Depth_Keller msg: 1613173284281 epoch sec; seqNo:5991; depth 28.44 2021-02-12T23:41:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173284.31 epoch sec; seqNo:5991; 4057601.71 north; 591358.30 east 2021-02-12T23:41:24Z- AHRS_M2 msg: 1613173284.67 epoch sec; seqNo:5941 2021-02-12T23:41:24Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.20 psi 2021-02-12T23:41:24Z- handleDepth()-Depth_Keller msg: 1613173284674 epoch sec; seqNo:5992; depth 28.55 2021-02-12T23:41:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173284.68 epoch sec; seqNo:5992; 4057602.09 north; 591358.30 east 2021-02-12T23:41:25Z- AHRS_M2 msg: 1613173285.07 epoch sec; seqNo:5942 2021-02-12T23:41:25Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.20 psi 2021-02-12T23:41:25Z- handleDepth()-Depth_Keller msg: 1613173285076 epoch sec; seqNo:5993; depth 28.67 2021-02-12T23:41:25Z- handleDvl() - RDI_Pathfinder msg: 1613173285.10 epoch sec; seqNo:2836 2021-02-12T23:41:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.02 , 86.09 , 61.71 , 93.66 2021-02-12T23:41:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.00 , 0.07 2021-02-12T23:41:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173285.11 epoch sec; seqNo:5993; 4057602.10 north; 591359.52 east 2021-02-12T23:41:25Z- AHRS_M2 msg: 1613173285.48 epoch sec; seqNo:5943 2021-02-12T23:41:25Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.20 psi 2021-02-12T23:41:25Z- handleDepth()-Depth_Keller msg: 1613173285480 epoch sec; seqNo:5994; depth 28.78 2021-02-12T23:41:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173285.49 epoch sec; seqNo:5994; 4057602.48 north; 591359.51 east 2021-02-12T23:41:25Z- AHRS_M2 msg: 1613173285.90 epoch sec; seqNo:5944 2021-02-12T23:41:25Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.20 psi 2021-02-12T23:41:25Z- handleDepth()-Depth_Keller msg: 1613173285904 epoch sec; seqNo:5995; depth 28.91 2021-02-12T23:41:25Z- handleDvl() - RDI_Pathfinder msg: 1613173285.91 epoch sec; seqNo:2837 2021-02-12T23:41:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.61 , 85.00 , 61.71 , 97.98 2021-02-12T23:41:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.04 , 0.05 2021-02-12T23:41:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173285.94 epoch sec; seqNo:5995; 4057602.86 north; 591359.51 east 2021-02-12T23:41:26Z- AHRS_M2 msg: 1613173286.28 epoch sec; seqNo:5945 2021-02-12T23:41:26Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.20 psi 2021-02-12T23:41:26Z- handleDepth()-Depth_Keller msg: 1613173286294 epoch sec; seqNo:5996; depth 29.03 2021-02-12T23:41:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173286.31 epoch sec; seqNo:5996; 4057602.86 north; 591359.51 east 2021-02-12T23:41:26Z- AHRS_M2 msg: 1613173286.69 epoch sec; seqNo:5946 2021-02-12T23:41:26Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.20 psi 2021-02-12T23:41:26Z- handleDepth()-Depth_Keller msg: 1613173286700 epoch sec; seqNo:5997; depth 29.14 2021-02-12T23:41:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173286.72 epoch sec; seqNo:5997; 4057603.24 north; 591359.50 east 2021-02-12T23:41:27Z- AHRS_M2 msg: 1613173287.09 epoch sec; seqNo:5947 2021-02-12T23:41:27Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.20 psi 2021-02-12T23:41:27Z- handleDepth()-Depth_Keller msg: 1613173287096 epoch sec; seqNo:5998; depth 29.25 2021-02-12T23:41:27Z- handleDvl() - RDI_Pathfinder msg: 1613173287.11 epoch sec; seqNo:2838 2021-02-12T23:41:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.61 , 86.09 , 60.48 , 97.98 2021-02-12T23:41:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.07 , 0.02 2021-02-12T23:41:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173287.14 epoch sec; seqNo:5998; 4057603.62 north; 591359.50 east 2021-02-12T23:41:27Z- AHRS_M2 msg: 1613173287.50 epoch sec; seqNo:5948 2021-02-12T23:41:27Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.21 psi 2021-02-12T23:41:27Z- handleDepth()-Depth_Keller msg: 1613173287502 epoch sec; seqNo:5999; depth 29.38 2021-02-12T23:41:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173287.51 epoch sec; seqNo:5999; 4057603.63 north; 591360.72 east 2021-02-12T23:41:27Z- AHRS_M2 msg: 1613173287.91 epoch sec; seqNo:5949 2021-02-12T23:41:27Z- AHRS_M2 msg: 0.62 phi; -0.28 theta; -0.21 psi 2021-02-12T23:41:27Z- handleDepth()-Depth_Keller msg: 1613173287913 epoch sec; seqNo:6000; depth 29.50 2021-02-12T23:41:27Z- handleDvl() - RDI_Pathfinder msg: 1613173287.92 epoch sec; seqNo:2839 2021-02-12T23:41:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.38 , 86.09 , 62.12 , 97.98 2021-02-12T23:41:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.02 , -0.00 2021-02-12T23:41:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173287.93 epoch sec; seqNo:6000; 4057604.01 north; 591360.72 east 2021-02-12T23:41:28Z- AHRS_M2 msg: 1613173288.31 epoch sec; seqNo:5950 2021-02-12T23:41:28Z- AHRS_M2 msg: 0.61 phi; -0.25 theta; -0.20 psi 2021-02-12T23:41:28Z- handleDepth()-Depth_Keller msg: 1613173288322 epoch sec; seqNo:6001; depth 29.65 2021-02-12T23:41:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173288.33 epoch sec; seqNo:6001; 4057604.39 north; 591360.71 east 2021-02-12T23:41:28Z- LcmTrn::handleMessages() - handling 44 messages took 0.75 ms and 129 calls... 2021-02-12T23:41:28Z- waiting for 1613173286.08, time is 1613173288.33 2021-02-12T23:41:28Z- LcmTrn::updateTrn() >>>> pose time 1613173288.33, meas time 1613173287.92 <<<< 2021-02-12T23:41:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173287.92 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:41:28Z- TerrainNav::motionUpdate() - currEstimate = (4057604, 591360.71, 29.65) 2021-02-12T23:41:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:41:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:41:28Z- TNavBF::Projecting Measurements 2021-02-12T23:41:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:41:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:41:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:41:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:41:28Z- TNavFilter::4 good beams. 2021-02-12T23:41:28Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:41:28Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:41:28Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:41:28Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:41:28Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:41:28Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:41:28Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:41:28Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:28Z- TerrainMapOctree: (vehN, vehE ) = (4057604.12, 591360.53). 2021-02-12T23:41:28Z- TerrainMapOctree: Min Distance = 4652.04. 2021-02-12T23:41:28Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:41:28Z- There is no specific good beam that all particles have in common. 2021-02-12T23:41:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:41:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-12T23:41:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:41:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 11.86 2021-02-12T23:41:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:41:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 6.17 2021-02-12T23:41:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:41:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.74 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-12T23:41:28Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:41:28Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:41:28Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:41:28Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:41:28Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:41:28Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:41:28Z- Filter 0 No beams used in subcloud update 2021-02-12T23:41:28Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:41:28Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:41:28Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 744.2 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:41:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173287.92 sec. 2021-02-12T23:41:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:41:28Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:28Z- variances([0], [2], and psi_berg) = 744.136159 833.027787 0.000000 2021-02-12T23:41:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:41:28Z- LcmTrn::publishEstimates() - MLE : 4057593.89 591319.21 29.65 2021-02-12T23:41:28Z- LcmTrn::publishEstimates() - MMSE : 4057604.39 591360.71 29.65 2021-02-12T23:41:28Z- LcmTrn::publishEstimates() - COVAR: 744.14 833.03 0.73 0.00 2021-02-12T23:41:28Z- AHRS_M2 msg: 1613173288.71 epoch sec; seqNo:5951 2021-02-12T23:41:28Z- AHRS_M2 msg: 0.60 phi; -0.22 theta; -0.19 psi 2021-02-12T23:41:28Z- handleDepth()-Depth_Keller msg: 1613173288712 epoch sec; seqNo:6002; depth 29.78 2021-02-12T23:41:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173288.72 epoch sec; seqNo:6002; 4057604.77 north; 591360.71 east 2021-02-12T23:41:29Z- AHRS_M2 msg: 1613173289.11 epoch sec; seqNo:5952 2021-02-12T23:41:29Z- AHRS_M2 msg: 0.59 phi; -0.18 theta; -0.18 psi 2021-02-12T23:41:29Z- handleDepth()-Depth_Keller msg: 1613173289118 epoch sec; seqNo:6003; depth 29.88 2021-02-12T23:41:29Z- handleDvl() - RDI_Pathfinder msg: 1613173289.13 epoch sec; seqNo:2840 2021-02-12T23:41:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.75 , 82.84 , 63.34 , 83.92 2021-02-12T23:41:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , -0.03 2021-02-12T23:41:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173289.15 epoch sec; seqNo:6003; 4057604.77 north; 591360.71 east 2021-02-12T23:41:29Z- AHRS_M2 msg: 1613173289.52 epoch sec; seqNo:5953 2021-02-12T23:41:29Z- AHRS_M2 msg: 0.58 phi; -0.14 theta; -0.17 psi 2021-02-12T23:41:29Z- handleDepth()-Depth_Keller msg: 1613173289522 epoch sec; seqNo:6004; depth 30.00 2021-02-12T23:41:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173289.53 epoch sec; seqNo:6004; 4057605.15 north; 591360.71 east 2021-02-12T23:41:29Z- AHRS_M2 msg: 1613173289.92 epoch sec; seqNo:5954 2021-02-12T23:41:29Z- AHRS_M2 msg: 0.57 phi; -0.10 theta; -0.16 psi 2021-02-12T23:41:29Z- handleDepth()-Depth_Keller msg: 1613173289924 epoch sec; seqNo:6005; depth 30.10 2021-02-12T23:41:29Z- handleDvl() - RDI_Pathfinder msg: 1613173289.94 epoch sec; seqNo:2841 2021-02-12T23:41:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.75 , 75.60 , 64.16 , 78.51 2021-02-12T23:41:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.05 2021-02-12T23:41:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173289.96 epoch sec; seqNo:6005; 4057605.54 north; 591361.92 east 2021-02-12T23:41:30Z- AHRS_M2 msg: 1613173290.32 epoch sec; seqNo:5955 2021-02-12T23:41:30Z- AHRS_M2 msg: 0.57 phi; -0.05 theta; -0.16 psi 2021-02-12T23:41:30Z- handleDepth()-Depth_Keller msg: 1613173290328 epoch sec; seqNo:6006; depth 30.19 2021-02-12T23:41:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173290.34 epoch sec; seqNo:6006; 4057605.92 north; 591361.92 east 2021-02-12T23:41:30Z- AHRS_M2 msg: 1613173290.73 epoch sec; seqNo:5956 2021-02-12T23:41:30Z- AHRS_M2 msg: 0.56 phi; -0.01 theta; -0.16 psi 2021-02-12T23:41:30Z- handleDepth()-Depth_Keller msg: 1613173290732 epoch sec; seqNo:6007; depth 30.25 2021-02-12T23:41:30Z- handleDvl() - RDI_Pathfinder msg: 1613173290.75 epoch sec; seqNo:2842 2021-02-12T23:41:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.44 , 74.76 , 0.00 , 68.94 2021-02-12T23:41:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.09 , -0.05 2021-02-12T23:41:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173290.76 epoch sec; seqNo:6007; 4057606.30 north; 591361.91 east 2021-02-12T23:41:31Z- AHRS_M2 msg: 1613173291.14 epoch sec; seqNo:5957 2021-02-12T23:41:31Z- AHRS_M2 msg: 0.54 phi; 0.07 theta; -0.15 psi 2021-02-12T23:41:31Z- handleDepth()-Depth_Keller msg: 1613173291149 epoch sec; seqNo:6008; depth 30.32 2021-02-12T23:41:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173291.16 epoch sec; seqNo:6008; 4057606.67 north; 591361.91 east 2021-02-12T23:41:31Z- AHRS_M2 msg: 1613173291.54 epoch sec; seqNo:5958 2021-02-12T23:41:31Z- AHRS_M2 msg: 0.54 phi; 0.10 theta; -0.14 psi 2021-02-12T23:41:31Z- handleDepth()-Depth_Keller msg: 1613173291543 epoch sec; seqNo:6009; depth 30.37 2021-02-12T23:41:31Z- handleDvl() - RDI_Pathfinder msg: 1613173291.56 epoch sec; seqNo:2843 2021-02-12T23:41:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.08 , 74.12 , 0.00 , 65.38 2021-02-12T23:41:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.09 , -0.05 2021-02-12T23:41:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173291.57 epoch sec; seqNo:6009; 4057606.67 north; 591361.91 east 2021-02-12T23:41:31Z- AHRS_M2 msg: 1613173291.94 epoch sec; seqNo:5959 2021-02-12T23:41:31Z- AHRS_M2 msg: 0.53 phi; 0.14 theta; -0.13 psi 2021-02-12T23:41:31Z- handleDepth()-Depth_Keller msg: 1613173291944 epoch sec; seqNo:6010; depth 30.39 2021-02-12T23:41:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173291.95 epoch sec; seqNo:6010; 4057607.05 north; 591361.91 east 2021-02-12T23:41:32Z- AHRS_M2 msg: 1613173292.34 epoch sec; seqNo:5960 2021-02-12T23:41:32Z- AHRS_M2 msg: 0.53 phi; 0.17 theta; -0.13 psi 2021-02-12T23:41:32Z- handleDepth()-Depth_Keller msg: 1613173292353 epoch sec; seqNo:6011; depth 30.40 2021-02-12T23:41:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173292.36 epoch sec; seqNo:6011; 4057607.43 north; 591361.90 east 2021-02-12T23:41:32Z- AHRS_M2 msg: 1613173292.75 epoch sec; seqNo:5961 2021-02-12T23:41:32Z- AHRS_M2 msg: 0.53 phi; 0.20 theta; -0.12 psi 2021-02-12T23:41:32Z- handleDepth()-Depth_Keller msg: 1613173292754 epoch sec; seqNo:6012; depth 30.41 2021-02-12T23:41:32Z- handleDvl() - RDI_Pathfinder msg: 1613173292.77 epoch sec; seqNo:2844 2021-02-12T23:41:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 65.05 , 0.00 , 0.00 , 63.44 2021-02-12T23:41:32Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:41:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173292.78 epoch sec; seqNo:6012; 4057607.81 north; 591361.90 east 2021-02-12T23:41:33Z- AHRS_M2 msg: 1613173293.15 epoch sec; seqNo:5962 2021-02-12T23:41:33Z- AHRS_M2 msg: 0.53 phi; 0.22 theta; -0.12 psi 2021-02-12T23:41:33Z- handleDepth()-Depth_Keller msg: 1613173293158 epoch sec; seqNo:6013; depth 30.38 2021-02-12T23:41:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173293.18 epoch sec; seqNo:6013; 4057608.20 north; 591363.11 east 2021-02-12T23:41:33Z- LcmTrn::handleMessages() - handling 41 messages took 0.61 ms and 129 calls... 2021-02-12T23:41:33Z- waiting for 1613173291.33, time is 1613173293.18 2021-02-12T23:41:33Z- LcmTrn::updateTrn() >>>> pose time 1613173293.18, meas time 1613173292.77 <<<< 2021-02-12T23:41:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173292.77 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:41:33Z- TerrainNav::motionUpdate() - currEstimate = (4057608, 591363.11, 30.38) 2021-02-12T23:41:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:41:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:41:33Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:41:33Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:41:33Z- TNavBF::Projecting Measurements 2021-02-12T23:41:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:41:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:41:33Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-12T23:41:33Z- TNavFilter::2 good beams. 2021-02-12T23:41:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:41:33Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:41:33Z- TNavBF:: beamIndex[1] = 3 2021-02-12T23:41:33Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:41:33Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:41:33Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-12T23:41:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:41:33Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:33Z- TerrainMapOctree: (vehN, vehE ) = (4057607.88, 591362.91). 2021-02-12T23:41:33Z- TerrainMapOctree: Min Distance = 4647.61. 2021-02-12T23:41:33Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:41:33Z- There is no specific good beam that all particles have in common. 2021-02-12T23:41:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:41:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 6.77 2021-02-12T23:41:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:41:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 6.44 2021-02-12T23:41:33Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:41:33Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:41:33Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:41:33Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:41:33Z- Filter 0 No beams used in subcloud update 2021-02-12T23:41:33Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:41:33Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:41:33Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 744.0 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:41:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173292.77 sec. 2021-02-12T23:41:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:41:33Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:33Z- variances([0], [2], and psi_berg) = 744.016765 833.027684 0.000000 2021-02-12T23:41:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:41:33Z- LcmTrn::publishEstimates() - MLE : 4057597.70 591321.61 30.38 2021-02-12T23:41:33Z- LcmTrn::publishEstimates() - MMSE : 4057608.20 591363.11 30.38 2021-02-12T23:41:33Z- LcmTrn::publishEstimates() - COVAR: 744.02 833.03 0.79 0.00 2021-02-12T23:41:33Z- AHRS_M2 msg: 1613173293.57 epoch sec; seqNo:5963 2021-02-12T23:41:33Z- AHRS_M2 msg: 0.53 phi; 0.24 theta; -0.12 psi 2021-02-12T23:41:33Z- handleDepth()-Depth_Keller msg: 1613173293573 epoch sec; seqNo:6014; depth 30.35 2021-02-12T23:41:33Z- handleDvl() - RDI_Pathfinder msg: 1613173293.59 epoch sec; seqNo:2845 2021-02-12T23:41:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 66.43 , 0.00 , 0.00 , 62.17 2021-02-12T23:41:33Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:41:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173293.60 epoch sec; seqNo:6014; 4057608.20 north; 591363.11 east 2021-02-12T23:41:33Z- AHRS_M2 msg: 1613173293.96 epoch sec; seqNo:5964 2021-02-12T23:41:33Z- AHRS_M2 msg: 0.53 phi; 0.26 theta; -0.12 psi 2021-02-12T23:41:33Z- handleDepth()-Depth_Keller msg: 1613173293964 epoch sec; seqNo:6015; depth 30.33 2021-02-12T23:41:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173293.97 epoch sec; seqNo:6015; 4057608.58 north; 591363.11 east 2021-02-12T23:41:34Z- AHRS_M2 msg: 1613173294.37 epoch sec; seqNo:5965 2021-02-12T23:41:34Z- AHRS_M2 msg: 0.53 phi; 0.28 theta; -0.12 psi 2021-02-12T23:41:34Z- handleDepth()-Depth_Keller msg: 1613173294380 epoch sec; seqNo:6016; depth 30.27 2021-02-12T23:41:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173294.41 epoch sec; seqNo:6016; 4057608.96 north; 591363.11 east 2021-02-12T23:41:34Z- AHRS_M2 msg: 1613173294.77 epoch sec; seqNo:5966 2021-02-12T23:41:34Z- AHRS_M2 msg: 0.53 phi; 0.29 theta; -0.11 psi 2021-02-12T23:41:34Z- handleDepth()-Depth_Keller msg: 1613173294772 epoch sec; seqNo:6017; depth 30.21 2021-02-12T23:41:34Z- handleDvl() - RDI_Pathfinder msg: 1613173294.79 epoch sec; seqNo:2846 2021-02-12T23:41:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 67.71 , 0.00 , 0.00 , 61.32 2021-02-12T23:41:34Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:41:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173294.80 epoch sec; seqNo:6017; 4057609.34 north; 591363.10 east 2021-02-12T23:41:35Z- AHRS_M2 msg: 1613173295.17 epoch sec; seqNo:5967 2021-02-12T23:41:35Z- AHRS_M2 msg: 0.53 phi; 0.30 theta; -0.11 psi 2021-02-12T23:41:35Z- handleDepth()-Depth_Keller msg: 1613173295181 epoch sec; seqNo:6018; depth 30.14 2021-02-12T23:41:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173295.19 epoch sec; seqNo:6018; 4057609.72 north; 591363.10 east 2021-02-12T23:41:35Z- AHRS_M2 msg: 1613173295.58 epoch sec; seqNo:5968 2021-02-12T23:41:35Z- AHRS_M2 msg: 0.53 phi; 0.31 theta; -0.11 psi 2021-02-12T23:41:35Z- handleDepth()-Depth_Keller msg: 1613173295580 epoch sec; seqNo:6019; depth 30.08 2021-02-12T23:41:35Z- handleDvl() - RDI_Pathfinder msg: 1613173295.59 epoch sec; seqNo:2847 2021-02-12T23:41:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 68.99 , 0.00 , 0.00 , 61.32 2021-02-12T23:41:35Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:41:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173295.60 epoch sec; seqNo:6019; 4057609.72 north; 591363.10 east 2021-02-12T23:41:35Z- AHRS_M2 msg: 1613173295.98 epoch sec; seqNo:5969 2021-02-12T23:41:35Z- AHRS_M2 msg: 0.53 phi; 0.32 theta; -0.11 psi 2021-02-12T23:41:35Z- handleDepth()-Depth_Keller msg: 1613173295986 epoch sec; seqNo:6020; depth 29.97 2021-02-12T23:41:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173296.00 epoch sec; seqNo:6020; 4057610.10 north; 591363.09 east 2021-02-12T23:41:36Z- AHRS_M2 msg: 1613173296.38 epoch sec; seqNo:5970 2021-02-12T23:41:36Z- AHRS_M2 msg: 0.54 phi; 0.33 theta; -0.10 psi 2021-02-12T23:41:36Z- handleDepth()-Depth_Keller msg: 1613173296398 epoch sec; seqNo:6021; depth 29.90 2021-02-12T23:41:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173296.41 epoch sec; seqNo:6021; 4057610.48 north; 591363.09 east 2021-02-12T23:41:36Z- AHRS_M2 msg: 1613173296.79 epoch sec; seqNo:5971 2021-02-12T23:41:36Z- AHRS_M2 msg: 0.54 phi; 0.34 theta; -0.10 psi 2021-02-12T23:41:36Z- handleDepth()-Depth_Keller msg: 1613173296793 epoch sec; seqNo:6022; depth 29.81 2021-02-12T23:41:36Z- handleDvl() - RDI_Pathfinder msg: 1613173296.81 epoch sec; seqNo:2848 2021-02-12T23:41:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:41:36Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:41:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173296.84 epoch sec; seqNo:6022; 4057610.87 north; 591364.31 east 2021-02-12T23:41:37Z- AHRS_M2 msg: 1613173297.19 epoch sec; seqNo:5972 2021-02-12T23:41:37Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.10 psi 2021-02-12T23:41:37Z- handleDepth()-Depth_Keller msg: 1613173297196 epoch sec; seqNo:6023; depth 29.71 2021-02-12T23:41:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173297.21 epoch sec; seqNo:6023; 4057610.87 north; 591364.31 east 2021-02-12T23:41:37Z- AHRS_M2 msg: 1613173297.60 epoch sec; seqNo:5973 2021-02-12T23:41:37Z- AHRS_M2 msg: 0.55 phi; 0.35 theta; -0.09 psi 2021-02-12T23:41:37Z- handleDepth()-Depth_Keller msg: 1613173297602 epoch sec; seqNo:6024; depth 29.62 2021-02-12T23:41:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173297.61 epoch sec; seqNo:6024; 4057611.25 north; 591364.30 east 2021-02-12T23:41:38Z- AHRS_M2 msg: 1613173298.00 epoch sec; seqNo:5974 2021-02-12T23:41:38Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.09 psi 2021-02-12T23:41:38Z- handleDepth()-Depth_Keller msg: 1613173298014 epoch sec; seqNo:6025; depth 29.51 2021-02-12T23:41:38Z- handleDvl() - RDI_Pathfinder msg: 1613173298.03 epoch sec; seqNo:2849 2021-02-12T23:41:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 76.83 , 97.98 , 60.48 2021-02-12T23:41:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , 0.01 2021-02-12T23:41:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173298.04 epoch sec; seqNo:6025; 4057611.63 north; 591364.30 east 2021-02-12T23:41:38Z- AHRS_M2 msg: 1613173298.40 epoch sec; seqNo:5975 2021-02-12T23:41:38Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.09 psi 2021-02-12T23:41:38Z- handleDepth()-Depth_Keller msg: 1613173298408 epoch sec; seqNo:6026; depth 29.39 2021-02-12T23:41:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173298.42 epoch sec; seqNo:6026; 4057612.00 north; 591364.30 east 2021-02-12T23:41:38Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-12T23:41:38Z- waiting for 1613173296.18, time is 1613173298.42 2021-02-12T23:41:38Z- LcmTrn::updateTrn() >>>> pose time 1613173298.42, meas time 1613173298.03 <<<< 2021-02-12T23:41:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173298.03 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:41:38Z- TerrainNav::motionUpdate() - currEstimate = (4057612, 591364.30, 29.39) 2021-02-12T23:41:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:41:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:41:38Z- TNavBF::Projecting Measurements 2021-02-12T23:41:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:41:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:41:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:41:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:41:38Z- TNavFilter::4 good beams. 2021-02-12T23:41:38Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:41:38Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:41:38Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:41:38Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:41:38Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:41:38Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:41:38Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:41:38Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:38Z- TerrainMapOctree: (vehN, vehE ) = (4057611.72, 591364.21). 2021-02-12T23:41:38Z- TerrainMapOctree: Min Distance = 4643.80. 2021-02-12T23:41:38Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:41:38Z- There is no specific good beam that all particles have in common. 2021-02-12T23:41:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:41:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 7.90 2021-02-12T23:41:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:41:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 9.44 2021-02-12T23:41:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:41:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-12T23:41:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:41:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 5.85 2021-02-12T23:41:38Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:41:38Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:41:38Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:41:38Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:41:38Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:41:38Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:41:38Z- Filter 0 No beams used in subcloud update 2021-02-12T23:41:38Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:41:38Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:41:38Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 743.9 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:41:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173298.03 sec. 2021-02-12T23:41:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:41:38Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:38Z- variances([0], [2], and psi_berg) = 743.905781 833.027589 0.000000 2021-02-12T23:41:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:41:38Z- LcmTrn::publishEstimates() - MLE : 4057601.50 591322.80 29.39 2021-02-12T23:41:38Z- LcmTrn::publishEstimates() - MMSE : 4057612.00 591364.30 29.39 2021-02-12T23:41:38Z- LcmTrn::publishEstimates() - COVAR: 743.91 833.03 0.72 0.00 2021-02-12T23:41:38Z- AHRS_M2 msg: 1613173298.81 epoch sec; seqNo:5976 2021-02-12T23:41:38Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.08 psi 2021-02-12T23:41:38Z- handleDepth()-Depth_Keller msg: 1613173298812 epoch sec; seqNo:6027; depth 29.29 2021-02-12T23:41:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173298.82 epoch sec; seqNo:6027; 4057612.38 north; 591364.29 east 2021-02-12T23:41:39Z- AHRS_M2 msg: 1613173299.21 epoch sec; seqNo:5977 2021-02-12T23:41:39Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.08 psi 2021-02-12T23:41:39Z- handleDepth()-Depth_Keller msg: 1613173299222 epoch sec; seqNo:6028; depth 29.19 2021-02-12T23:41:39Z- handleDvl() - RDI_Pathfinder msg: 1613173299.23 epoch sec; seqNo:2850 2021-02-12T23:41:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 80.68 , 95.82 , 60.48 2021-02-12T23:41:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.04 , -0.03 2021-02-12T23:41:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173299.24 epoch sec; seqNo:6028; 4057612.38 north; 591364.29 east 2021-02-12T23:41:39Z- AHRS_M2 msg: 1613173299.62 epoch sec; seqNo:5978 2021-02-12T23:41:39Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.08 psi 2021-02-12T23:41:39Z- handleDepth()-Depth_Keller msg: 1613173299620 epoch sec; seqNo:6029; depth 29.05 2021-02-12T23:41:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173299.63 epoch sec; seqNo:6029; 4057612.77 north; 591365.51 east 2021-02-12T23:41:40Z- AHRS_M2 msg: 1613173300.02 epoch sec; seqNo:5979 2021-02-12T23:41:40Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.07 psi 2021-02-12T23:41:40Z- handleDepth()-Depth_Keller msg: 1613173300024 epoch sec; seqNo:6030; depth 28.94 2021-02-12T23:41:40Z- handleDvl() - RDI_Pathfinder msg: 1613173300.04 epoch sec; seqNo:2851 2021-02-12T23:41:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 0.00 , 95.82 , 61.30 2021-02-12T23:41:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.03 , -0.06 2021-02-12T23:41:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173300.05 epoch sec; seqNo:6030; 4057613.15 north; 591365.50 east 2021-02-12T23:41:40Z- AHRS_M2 msg: 1613173300.42 epoch sec; seqNo:5980 2021-02-12T23:41:40Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.07 psi 2021-02-12T23:41:40Z- handleDepth()-Depth_Keller msg: 1613173300441 epoch sec; seqNo:6031; depth 28.83 2021-02-12T23:41:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173300.46 epoch sec; seqNo:6031; 4057613.15 north; 591365.50 east 2021-02-12T23:41:40Z- AHRS_M2 msg: 1613173300.83 epoch sec; seqNo:5981 2021-02-12T23:41:40Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.07 psi 2021-02-12T23:41:40Z- handleDepth()-Depth_Keller msg: 1613173300832 epoch sec; seqNo:6032; depth 28.71 2021-02-12T23:41:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173300.85 epoch sec; seqNo:6032; 4057613.53 north; 591365.50 east 2021-02-12T23:41:41Z- AHRS_M2 msg: 1613173301.23 epoch sec; seqNo:5982 2021-02-12T23:41:41Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.08 psi 2021-02-12T23:41:41Z- handleDepth()-Depth_Keller msg: 1613173301241 epoch sec; seqNo:6033; depth 28.61 2021-02-12T23:41:41Z- handleDvl() - RDI_Pathfinder msg: 1613173301.25 epoch sec; seqNo:2852 2021-02-12T23:41:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:41:41Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:41:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173301.28 epoch sec; seqNo:6033; 4057613.91 north; 591365.50 east 2021-02-12T23:41:41Z- AHRS_M2 msg: 1613173301.64 epoch sec; seqNo:5983 2021-02-12T23:41:41Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.08 psi 2021-02-12T23:41:41Z- handleDepth()-Depth_Keller msg: 1613173301640 epoch sec; seqNo:6034; depth 28.46 2021-02-12T23:41:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173301.65 epoch sec; seqNo:6034; 4057613.91 north; 591365.50 east 2021-02-12T23:41:42Z- AHRS_M2 msg: 1613173302.04 epoch sec; seqNo:5984 2021-02-12T23:41:42Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.09 psi 2021-02-12T23:41:42Z- handleDepth()-Depth_Keller msg: 1613173302047 epoch sec; seqNo:6035; depth 28.36 2021-02-12T23:41:42Z- handleDvl() - RDI_Pathfinder msg: 1613173302.06 epoch sec; seqNo:2853 2021-02-12T23:41:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 79.87 , 98.12 , 61.71 2021-02-12T23:41:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.03 2021-02-12T23:41:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173302.07 epoch sec; seqNo:6035; 4057614.29 north; 591365.49 east 2021-02-12T23:41:42Z- AHRS_M2 msg: 1613173302.44 epoch sec; seqNo:5985 2021-02-12T23:41:42Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.10 psi 2021-02-12T23:41:42Z- handleDepth()-Depth_Keller msg: 1613173302448 epoch sec; seqNo:6036; depth 28.24 2021-02-12T23:41:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173302.47 epoch sec; seqNo:6036; 4057614.68 north; 591366.71 east 2021-02-12T23:41:42Z- AHRS_M2 msg: 1613173302.85 epoch sec; seqNo:5986 2021-02-12T23:41:42Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.11 psi 2021-02-12T23:41:42Z- handleDepth()-Depth_Keller msg: 1613173302871 epoch sec; seqNo:6037; depth 28.13 2021-02-12T23:41:42Z- handleDvl() - RDI_Pathfinder msg: 1613173302.92 epoch sec; seqNo:2854 2021-02-12T23:41:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 79.60 , 97.98 , 63.34 2021-02-12T23:41:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.00 2021-02-12T23:41:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173302.96 epoch sec; seqNo:6037; 4057615.06 north; 591366.70 east 2021-02-12T23:41:43Z- AHRS_M2 msg: 1613173303.27 epoch sec; seqNo:5987 2021-02-12T23:41:43Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.11 psi 2021-02-12T23:41:43Z- handleDepth()-Depth_Keller msg: 1613173303273 epoch sec; seqNo:6038; depth 28.00 2021-02-12T23:41:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173303.28 epoch sec; seqNo:6038; 4057615.06 north; 591366.70 east 2021-02-12T23:41:43Z- AHRS_M2 msg: 1613173303.67 epoch sec; seqNo:5988 2021-02-12T23:41:43Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.12 psi 2021-02-12T23:41:43Z- LcmTrn::handleMessages() - handling 42 messages took 0.75 ms and 127 calls... 2021-02-12T23:41:43Z- waiting for 1613173301.42, time is 1613173303.67 2021-02-12T23:41:43Z- LcmTrn::updateTrn() >>>> pose time 1613173303.67, meas time 1613173302.92 <<<< 2021-02-12T23:41:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173302.92 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:41:43Z- TerrainNav::motionUpdate() - currEstimate = (4057615, 591366.70, 28.00) 2021-02-12T23:41:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:41:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:41:43Z- TNavBF::Projecting Measurements 2021-02-12T23:41:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:41:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:41:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:41:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:41:43Z- TNavFilter::4 good beams. 2021-02-12T23:41:43Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:41:43Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:41:43Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:41:43Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:41:43Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:41:43Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:41:43Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:41:43Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:43Z- TerrainMapOctree: (vehN, vehE ) = (4057614.62, 591366.36). 2021-02-12T23:41:43Z- TerrainMapOctree: Min Distance = 4640.19. 2021-02-12T23:41:43Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:41:43Z- There is no specific good beam that all particles have in common. 2021-02-12T23:41:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:41:43Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-12T23:41:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:41:43Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 10.14 2021-02-12T23:41:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:41:43Z- Old Depth Bias: 0.00 DepthBiasCov: 0.77 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-12T23:41:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:41:43Z- Old Depth Bias: 0.00 DepthBiasCov: 0.73 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 6.42 2021-02-12T23:41:43Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:41:43Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:41:43Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:41:43Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:41:43Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:41:43Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:41:43Z- Filter 0 No beams used in subcloud update 2021-02-12T23:41:43Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:41:43Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:41:43Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 743.8 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:41:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173302.92 sec. 2021-02-12T23:41:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:41:43Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:43Z- variances([0], [2], and psi_berg) = 743.790193 833.027489 0.000000 2021-02-12T23:41:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:41:43Z- LcmTrn::publishEstimates() - MLE : 4057604.56 591325.20 28.00 2021-02-12T23:41:43Z- LcmTrn::publishEstimates() - MMSE : 4057615.06 591366.70 28.00 2021-02-12T23:41:43Z- LcmTrn::publishEstimates() - COVAR: 743.79 833.03 0.70 0.00 2021-02-12T23:41:43Z- handleDepth()-Depth_Keller msg: 1613173303681 epoch sec; seqNo:6039; depth 27.87 2021-02-12T23:41:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173303.69 epoch sec; seqNo:6039; 4057615.44 north; 591366.70 east 2021-02-12T23:41:44Z- AHRS_M2 msg: 1613173304.08 epoch sec; seqNo:5989 2021-02-12T23:41:44Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.13 psi 2021-02-12T23:41:44Z- handleDepth()-Depth_Keller msg: 1613173304090 epoch sec; seqNo:6040; depth 27.77 2021-02-12T23:41:44Z- handleDvl() - RDI_Pathfinder msg: 1613173304.10 epoch sec; seqNo:2855 2021-02-12T23:41:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 80.68 , 97.98 , 62.93 2021-02-12T23:41:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.00 2021-02-12T23:41:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173304.11 epoch sec; seqNo:6040; 4057615.82 north; 591366.70 east 2021-02-12T23:41:44Z- AHRS_M2 msg: 1613173304.48 epoch sec; seqNo:5990 2021-02-12T23:41:44Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.14 psi 2021-02-12T23:41:44Z- handleDepth()-Depth_Keller msg: 1613173304484 epoch sec; seqNo:6041; depth 27.64 2021-02-12T23:41:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173304.49 epoch sec; seqNo:6041; 4057615.82 north; 591366.70 east 2021-02-12T23:41:44Z- AHRS_M2 msg: 1613173304.90 epoch sec; seqNo:5991 2021-02-12T23:41:44Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.14 psi 2021-02-12T23:41:44Z- handleDepth()-Depth_Keller msg: 1613173304905 epoch sec; seqNo:6042; depth 27.53 2021-02-12T23:41:44Z- handleDvl() - RDI_Pathfinder msg: 1613173304.92 epoch sec; seqNo:2856 2021-02-12T23:41:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 0.00 , 97.98 , 63.75 2021-02-12T23:41:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , 0.01 2021-02-12T23:41:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173304.93 epoch sec; seqNo:6042; 4057616.21 north; 591367.91 east 2021-02-12T23:41:45Z- AHRS_M2 msg: 1613173305.29 epoch sec; seqNo:5992 2021-02-12T23:41:45Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.14 psi 2021-02-12T23:41:45Z- handleDepth()-Depth_Keller msg: 1613173305294 epoch sec; seqNo:6043; depth 27.42 2021-02-12T23:41:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173305.31 epoch sec; seqNo:6043; 4057616.59 north; 591367.91 east 2021-02-12T23:41:45Z- AHRS_M2 msg: 1613173305.69 epoch sec; seqNo:5993 2021-02-12T23:41:45Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.14 psi 2021-02-12T23:41:45Z- handleDepth()-Depth_Keller msg: 1613173305696 epoch sec; seqNo:6044; depth 27.32 2021-02-12T23:41:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173305.71 epoch sec; seqNo:6044; 4057616.97 north; 591367.91 east 2021-02-12T23:41:46Z- AHRS_M2 msg: 1613173306.10 epoch sec; seqNo:5994 2021-02-12T23:41:46Z- AHRS_M2 msg: 0.65 phi; 0.29 theta; -0.13 psi 2021-02-12T23:41:46Z- handleDepth()-Depth_Keller msg: 1613173306100 epoch sec; seqNo:6045; depth 27.20 2021-02-12T23:41:46Z- handleDvl() - RDI_Pathfinder msg: 1613173306.11 epoch sec; seqNo:2857 2021-02-12T23:41:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 79.28 , 97.92 , 64.57 2021-02-12T23:41:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.09 , -0.01 2021-02-12T23:41:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173306.15 epoch sec; seqNo:6045; 4057616.97 north; 591367.91 east 2021-02-12T23:41:46Z- AHRS_M2 msg: 1613173306.50 epoch sec; seqNo:5995 2021-02-12T23:41:46Z- AHRS_M2 msg: 0.65 phi; 0.29 theta; -0.13 psi 2021-02-12T23:41:46Z- handleDepth()-Depth_Keller msg: 1613173306504 epoch sec; seqNo:6046; depth 27.09 2021-02-12T23:41:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173306.52 epoch sec; seqNo:6046; 4057617.35 north; 591367.90 east 2021-02-12T23:41:46Z- AHRS_M2 msg: 1613173306.90 epoch sec; seqNo:5996 2021-02-12T23:41:46Z- AHRS_M2 msg: 0.65 phi; 0.28 theta; -0.13 psi 2021-02-12T23:41:46Z- handleDepth()-Depth_Keller msg: 1613173306909 epoch sec; seqNo:6047; depth 26.99 2021-02-12T23:41:46Z- handleDvl() - RDI_Pathfinder msg: 1613173306.93 epoch sec; seqNo:2858 2021-02-12T23:41:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 0.00 , 97.98 , 63.75 2021-02-12T23:41:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , -0.01 2021-02-12T23:41:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173306.94 epoch sec; seqNo:6047; 4057617.73 north; 591367.90 east 2021-02-12T23:41:47Z- AHRS_M2 msg: 1613173307.31 epoch sec; seqNo:5997 2021-02-12T23:41:47Z- AHRS_M2 msg: 0.65 phi; 0.28 theta; -0.14 psi 2021-02-12T23:41:47Z- handleDepth()-Depth_Keller msg: 1613173307315 epoch sec; seqNo:6048; depth 26.88 2021-02-12T23:41:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173307.34 epoch sec; seqNo:6048; 4057618.10 north; 591367.89 east 2021-02-12T23:41:47Z- AHRS_M2 msg: 1613173307.72 epoch sec; seqNo:5998 2021-02-12T23:41:47Z- AHRS_M2 msg: 0.65 phi; 0.28 theta; -0.16 psi 2021-02-12T23:41:47Z- handleDepth()-Depth_Keller msg: 1613173307729 epoch sec; seqNo:6049; depth 26.79 2021-02-12T23:41:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173307.75 epoch sec; seqNo:6049; 4057618.12 north; 591369.11 east 2021-02-12T23:41:48Z- AHRS_M2 msg: 1613173308.12 epoch sec; seqNo:5999 2021-02-12T23:41:48Z- AHRS_M2 msg: 0.64 phi; 0.27 theta; -0.17 psi 2021-02-12T23:41:48Z- handleDepth()-Depth_Keller msg: 1613173308125 epoch sec; seqNo:6050; depth 26.69 2021-02-12T23:41:48Z- handleDvl() - RDI_Pathfinder msg: 1613173308.14 epoch sec; seqNo:2859 2021-02-12T23:41:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:41:48Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:41:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173308.17 epoch sec; seqNo:6050; 4057618.50 north; 591369.11 east 2021-02-12T23:41:48Z- AHRS_M2 msg: 1613173308.52 epoch sec; seqNo:6000 2021-02-12T23:41:48Z- AHRS_M2 msg: 0.63 phi; 0.27 theta; -0.18 psi 2021-02-12T23:41:48Z- handleDepth()-Depth_Keller msg: 1613173308526 epoch sec; seqNo:6051; depth 26.58 2021-02-12T23:41:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173308.54 epoch sec; seqNo:6051; 4057618.88 north; 591369.11 east 2021-02-12T23:41:48Z- LcmTrn::handleMessages() - handling 43 messages took 0.58 ms and 132 calls... 2021-02-12T23:41:48Z- waiting for 1613173306.67, time is 1613173308.54 2021-02-12T23:41:48Z- LcmTrn::updateTrn() >>>> pose time 1613173308.54, meas time 1613173308.14 <<<< 2021-02-12T23:41:48Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173308.14, ping # 0. 2021-02-12T23:41:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173308.14 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:41:48Z- TerrainNav::motionUpdate() - currEstimate = (4057619, 591369.11, 26.58) 2021-02-12T23:41:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:41:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:41:48Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:41:48Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:41:48Z- TNavBF::Projecting Measurements 2021-02-12T23:41:48Z- TNavFilter::Measurement from time = 1613173308.14 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:41:48Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:41:48Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:41:48Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:41:48Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:41:48Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:41:48Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:41:48Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:41:48Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 743.7 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:41:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:41:48Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:48Z- variances([0], [2], and psi_berg) = 743.671677 833.027388 0.000000 2021-02-12T23:41:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:41:48Z- LcmTrn::publishEstimates() - MLE : 4057608.38 591327.61 26.58 2021-02-12T23:41:48Z- LcmTrn::publishEstimates() - MMSE : 4057618.88 591369.11 26.58 2021-02-12T23:41:48Z- LcmTrn::publishEstimates() - COVAR: 743.67 833.03 0.93 0.00 2021-02-12T23:41:48Z- AHRS_M2 msg: 1613173308.94 epoch sec; seqNo:6001 2021-02-12T23:41:48Z- AHRS_M2 msg: 0.63 phi; 0.27 theta; -0.19 psi 2021-02-12T23:41:48Z- handleDepth()-Depth_Keller msg: 1613173308945 epoch sec; seqNo:6052; depth 26.49 2021-02-12T23:41:48Z- handleDvl() - RDI_Pathfinder msg: 1613173308.96 epoch sec; seqNo:2860 2021-02-12T23:41:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 84.40 , 91.70 , 64.16 2021-02-12T23:41:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , 0.11 2021-02-12T23:41:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173308.97 epoch sec; seqNo:6052; 4057619.25 north; 591369.10 east 2021-02-12T23:41:49Z- AHRS_M2 msg: 1613173309.33 epoch sec; seqNo:6002 2021-02-12T23:41:49Z- AHRS_M2 msg: 0.62 phi; 0.27 theta; -0.19 psi 2021-02-12T23:41:49Z- handleDepth()-Depth_Keller msg: 1613173309332 epoch sec; seqNo:6053; depth 26.37 2021-02-12T23:41:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173309.35 epoch sec; seqNo:6053; 4057619.63 north; 591369.10 east 2021-02-12T23:41:49Z- AHRS_M2 msg: 1613173309.73 epoch sec; seqNo:6003 2021-02-12T23:41:49Z- AHRS_M2 msg: 0.62 phi; 0.27 theta; -0.20 psi 2021-02-12T23:41:49Z- handleDepth()-Depth_Keller msg: 1613173309738 epoch sec; seqNo:6054; depth 26.27 2021-02-12T23:41:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173309.75 epoch sec; seqNo:6054; 4057619.63 north; 591369.10 east 2021-02-12T23:41:50Z- AHRS_M2 msg: 1613173310.14 epoch sec; seqNo:6004 2021-02-12T23:41:50Z- AHRS_M2 msg: 0.62 phi; 0.27 theta; -0.20 psi 2021-02-12T23:41:50Z- handleDepth()-Depth_Keller msg: 1613173310144 epoch sec; seqNo:6055; depth 26.18 2021-02-12T23:41:50Z- handleDvl() - RDI_Pathfinder msg: 1613173310.16 epoch sec; seqNo:2861 2021-02-12T23:41:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 128.27 , 0.00 , 92.58 , 65.79 2021-02-12T23:41:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -4.37 , 0.11 2021-02-12T23:41:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173310.17 epoch sec; seqNo:6055; 4057621.14 north; 591367.86 east 2021-02-12T23:41:50Z- AHRS_M2 msg: 1613173310.54 epoch sec; seqNo:6005 2021-02-12T23:41:50Z- AHRS_M2 msg: 0.62 phi; 0.27 theta; -0.20 psi 2021-02-12T23:41:50Z- handleDepth()-Depth_Keller msg: 1613173310549 epoch sec; seqNo:6056; depth 26.08 2021-02-12T23:41:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173310.56 epoch sec; seqNo:6056; 4057622.65 north; 591367.85 east 2021-02-12T23:41:50Z- AHRS_M2 msg: 1613173310.94 epoch sec; seqNo:6006 2021-02-12T23:41:50Z- AHRS_M2 msg: 0.62 phi; 0.27 theta; -0.19 psi 2021-02-12T23:41:50Z- handleDepth()-Depth_Keller msg: 1613173310948 epoch sec; seqNo:6057; depth 26.00 2021-02-12T23:41:50Z- handleDvl() - RDI_Pathfinder msg: 1613173310.96 epoch sec; seqNo:2862 2021-02-12T23:41:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 0.00 , 97.44 , 64.97 2021-02-12T23:41:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.06 , 0.07 2021-02-12T23:41:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173310.97 epoch sec; seqNo:6057; 4057622.65 north; 591367.85 east 2021-02-12T23:41:51Z- AHRS_M2 msg: 1613173311.36 epoch sec; seqNo:6007 2021-02-12T23:41:51Z- AHRS_M2 msg: 0.62 phi; 0.27 theta; -0.19 psi 2021-02-12T23:41:51Z- handleDepth()-Depth_Keller msg: 1613173311363 epoch sec; seqNo:6058; depth 25.89 2021-02-12T23:41:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173311.37 epoch sec; seqNo:6058; 4057623.03 north; 591367.84 east 2021-02-12T23:41:51Z- AHRS_M2 msg: 1613173311.75 epoch sec; seqNo:6008 2021-02-12T23:41:51Z- AHRS_M2 msg: 0.63 phi; 0.27 theta; -0.19 psi 2021-02-12T23:41:51Z- handleDepth()-Depth_Keller msg: 1613173311756 epoch sec; seqNo:6059; depth 25.79 2021-02-12T23:41:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173311.77 epoch sec; seqNo:6059; 4057623.41 north; 591367.84 east 2021-02-12T23:41:52Z- AHRS_M2 msg: 1613173312.16 epoch sec; seqNo:6009 2021-02-12T23:41:52Z- AHRS_M2 msg: 0.63 phi; 0.27 theta; -0.19 psi 2021-02-12T23:41:52Z- handleDepth()-Depth_Keller msg: 1613173312165 epoch sec; seqNo:6060; depth 25.69 2021-02-12T23:41:52Z- handleDvl() - RDI_Pathfinder msg: 1613173312.18 epoch sec; seqNo:2863 2021-02-12T23:41:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 0.00 , 97.92 , 66.61 2021-02-12T23:41:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.10 , -0.03 2021-02-12T23:41:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173312.20 epoch sec; seqNo:6060; 4057623.79 north; 591367.83 east 2021-02-12T23:41:52Z- AHRS_M2 msg: 1613173312.56 epoch sec; seqNo:6010 2021-02-12T23:41:52Z- AHRS_M2 msg: 0.63 phi; 0.27 theta; -0.18 psi 2021-02-12T23:41:52Z- handleDepth()-Depth_Keller msg: 1613173312569 epoch sec; seqNo:6061; depth 25.61 2021-02-12T23:41:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173312.58 epoch sec; seqNo:6061; 4057623.79 north; 591367.83 east 2021-02-12T23:41:52Z- AHRS_M2 msg: 1613173312.96 epoch sec; seqNo:6011 2021-02-12T23:41:52Z- AHRS_M2 msg: 0.64 phi; 0.27 theta; -0.18 psi 2021-02-12T23:41:52Z- handleDepth()-Depth_Keller msg: 1613173312970 epoch sec; seqNo:6062; depth 25.51 2021-02-12T23:41:52Z- handleDvl() - RDI_Pathfinder msg: 1613173312.98 epoch sec; seqNo:2864 2021-02-12T23:41:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 0.00 , 92.44 , 66.20 2021-02-12T23:41:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , -0.06 2021-02-12T23:41:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173312.99 epoch sec; seqNo:6062; 4057624.18 north; 591369.05 east 2021-02-12T23:41:53Z- AHRS_M2 msg: 1613173313.37 epoch sec; seqNo:6012 2021-02-12T23:41:53Z- AHRS_M2 msg: 0.64 phi; 0.27 theta; -0.18 psi 2021-02-12T23:41:53Z- handleDepth()-Depth_Keller msg: 1613173313372 epoch sec; seqNo:6063; depth 25.43 2021-02-12T23:41:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173313.38 epoch sec; seqNo:6063; 4057624.56 north; 591369.05 east 2021-02-12T23:41:53Z- AHRS_M2 msg: 1613173313.77 epoch sec; seqNo:6013 2021-02-12T23:41:53Z- AHRS_M2 msg: 0.64 phi; 0.27 theta; -0.19 psi 2021-02-12T23:41:53Z- handleDepth()-Depth_Keller msg: 1613173313781 epoch sec; seqNo:6064; depth 25.33 2021-02-12T23:41:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173313.79 epoch sec; seqNo:6064; 4057624.56 north; 591369.05 east 2021-02-12T23:41:53Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-12T23:41:53Z- waiting for 1613173311.54, time is 1613173313.79 2021-02-12T23:41:53Z- LcmTrn::updateTrn() >>>> pose time 1613173313.79, meas time 1613173312.98 <<<< 2021-02-12T23:41:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173312.98 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:41:53Z- TerrainNav::motionUpdate() - currEstimate = (4057625, 591369.05, 25.33) 2021-02-12T23:41:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:41:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:41:53Z- TNavBF::Projecting Measurements 2021-02-12T23:41:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:41:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:41:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:41:53Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:41:53Z- TNavFilter::3 good beams. 2021-02-12T23:41:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:41:53Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:41:53Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:41:53Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:41:53Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:41:53Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:41:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:41:53Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:53Z- TerrainMapOctree: (vehN, vehE ) = (4057623.68, 591369.06). 2021-02-12T23:41:53Z- TerrainMapOctree: Min Distance = 4631.42. 2021-02-12T23:41:53Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:41:53Z- There is no specific good beam that all particles have in common. 2021-02-12T23:41:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:41:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 9.83 2021-02-12T23:41:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:41:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 13.67 2021-02-12T23:41:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:41:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 7.01 2021-02-12T23:41:53Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:41:53Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:41:53Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:41:53Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:41:53Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:41:53Z- Filter 0 No beams used in subcloud update 2021-02-12T23:41:53Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:41:53Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:41:53Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 743.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:41:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173312.98 sec. 2021-02-12T23:41:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:41:53Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:53Z- variances([0], [2], and psi_berg) = 743.531558 833.027268 0.000000 2021-02-12T23:41:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:41:53Z- LcmTrn::publishEstimates() - MLE : 4057614.06 591327.55 25.33 2021-02-12T23:41:53Z- LcmTrn::publishEstimates() - MMSE : 4057624.56 591369.05 25.33 2021-02-12T23:41:53Z- LcmTrn::publishEstimates() - COVAR: 743.53 833.03 0.94 0.00 2021-02-12T23:41:54Z- AHRS_M2 msg: 1613173314.17 epoch sec; seqNo:6014 2021-02-12T23:41:54Z- AHRS_M2 msg: 0.64 phi; 0.27 theta; -0.19 psi 2021-02-12T23:41:54Z- handleDepth()-Depth_Keller msg: 1613173314182 epoch sec; seqNo:6065; depth 25.23 2021-02-12T23:41:54Z- handleDvl() - RDI_Pathfinder msg: 1613173314.19 epoch sec; seqNo:2865 2021-02-12T23:41:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:41:54Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:41:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173314.20 epoch sec; seqNo:6065; 4057624.94 north; 591369.04 east 2021-02-12T23:41:54Z- handleDepth()-Depth_Keller msg: 1613173314601 epoch sec; seqNo:6066; depth 25.12 2021-02-12T23:41:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173314.61 epoch sec; seqNo:6066; 4057625.32 north; 591369.04 east 2021-02-12T23:41:54Z- AHRS_M2 msg: 1613173314.99 epoch sec; seqNo:6015 2021-02-12T23:41:54Z- AHRS_M2 msg: 0.64 phi; 0.27 theta; -0.19 psi 2021-02-12T23:41:54Z- handleDepth()-Depth_Keller msg: 1613173314994 epoch sec; seqNo:6067; depth 25.03 2021-02-12T23:41:55Z- handleDvl() - RDI_Pathfinder msg: 1613173315.01 epoch sec; seqNo:2866 2021-02-12T23:41:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 96.97 , 94.27 , 65.79 2021-02-12T23:41:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.10 , -0.06 2021-02-12T23:41:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173315.03 epoch sec; seqNo:6067; 4057625.71 north; 591370.25 east 2021-02-12T23:41:55Z- AHRS_M2 msg: 1613173315.39 epoch sec; seqNo:6016 2021-02-12T23:41:55Z- AHRS_M2 msg: 0.63 phi; 0.27 theta; -0.19 psi 2021-02-12T23:41:55Z- handleDepth()-Depth_Keller msg: 1613173315397 epoch sec; seqNo:6068; depth 24.92 2021-02-12T23:41:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173315.41 epoch sec; seqNo:6068; 4057625.71 north; 591370.25 east 2021-02-12T23:41:55Z- AHRS_M2 msg: 1613173315.79 epoch sec; seqNo:6017 2021-02-12T23:41:55Z- AHRS_M2 msg: 0.63 phi; 0.27 theta; -0.19 psi 2021-02-12T23:41:55Z- handleDepth()-Depth_Keller msg: 1613173315798 epoch sec; seqNo:6069; depth 24.83 2021-02-12T23:41:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173315.81 epoch sec; seqNo:6069; 4057626.09 north; 591370.25 east 2021-02-12T23:41:56Z- AHRS_M2 msg: 1613173316.20 epoch sec; seqNo:6018 2021-02-12T23:41:56Z- AHRS_M2 msg: 0.62 phi; 0.27 theta; -0.19 psi 2021-02-12T23:41:56Z- handleDepth()-Depth_Keller msg: 1613173316200 epoch sec; seqNo:6070; depth 24.75 2021-02-12T23:41:56Z- handleDvl() - RDI_Pathfinder msg: 1613173316.22 epoch sec; seqNo:2867 2021-02-12T23:41:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 88.11 , 97.85 , 67.02 2021-02-12T23:41:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.14 , -0.02 2021-02-12T23:41:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173316.23 epoch sec; seqNo:6070; 4057626.47 north; 591370.25 east 2021-02-12T23:41:56Z- AHRS_M2 msg: 1613173316.60 epoch sec; seqNo:6019 2021-02-12T23:41:56Z- AHRS_M2 msg: 0.62 phi; 0.27 theta; -0.19 psi 2021-02-12T23:41:56Z- handleDepth()-Depth_Keller msg: 1613173316609 epoch sec; seqNo:6071; depth 24.63 2021-02-12T23:41:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173316.62 epoch sec; seqNo:6071; 4057626.84 north; 591370.24 east 2021-02-12T23:41:57Z- AHRS_M2 msg: 1613173317.00 epoch sec; seqNo:6020 2021-02-12T23:41:57Z- AHRS_M2 msg: 0.61 phi; 0.27 theta; -0.19 psi 2021-02-12T23:41:57Z- handleDepth()-Depth_Keller msg: 1613173317009 epoch sec; seqNo:6072; depth 24.56 2021-02-12T23:41:57Z- handleDvl() - RDI_Pathfinder msg: 1613173317.02 epoch sec; seqNo:2868 2021-02-12T23:41:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:41:57Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:41:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173317.03 epoch sec; seqNo:6072; 4057627.22 north; 591370.24 east 2021-02-12T23:41:57Z- AHRS_M2 msg: 1613173317.41 epoch sec; seqNo:6021 2021-02-12T23:41:57Z- AHRS_M2 msg: 0.61 phi; 0.28 theta; -0.18 psi 2021-02-12T23:41:57Z- handleDepth()-Depth_Keller msg: 1613173317418 epoch sec; seqNo:6073; depth 24.46 2021-02-12T23:41:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173317.43 epoch sec; seqNo:6073; 4057627.22 north; 591370.24 east 2021-02-12T23:41:57Z- AHRS_M2 msg: 1613173317.81 epoch sec; seqNo:6022 2021-02-12T23:41:57Z- AHRS_M2 msg: 0.61 phi; 0.28 theta; -0.17 psi 2021-02-12T23:41:57Z- handleDepth()-Depth_Keller msg: 1613173317816 epoch sec; seqNo:6074; depth 24.36 2021-02-12T23:41:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173317.84 epoch sec; seqNo:6074; 4057627.60 north; 591370.23 east 2021-02-12T23:41:58Z- AHRS_M2 msg: 1613173318.22 epoch sec; seqNo:6023 2021-02-12T23:41:58Z- AHRS_M2 msg: 0.61 phi; 0.28 theta; -0.17 psi 2021-02-12T23:41:58Z- handleDepth()-Depth_Keller msg: 1613173318221 epoch sec; seqNo:6075; depth 24.27 2021-02-12T23:41:58Z- handleDvl() - RDI_Pathfinder msg: 1613173318.23 epoch sec; seqNo:2869 2021-02-12T23:41:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.25 , 89.20 , 97.71 , 65.38 2021-02-12T23:41:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , 0.05 2021-02-12T23:41:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173318.26 epoch sec; seqNo:6075; 4057627.99 north; 591371.45 east 2021-02-12T23:41:58Z- AHRS_M2 msg: 1613173318.62 epoch sec; seqNo:6024 2021-02-12T23:41:58Z- AHRS_M2 msg: 0.61 phi; 0.28 theta; -0.17 psi 2021-02-12T23:41:58Z- handleDepth()-Depth_Keller msg: 1613173318629 epoch sec; seqNo:6076; depth 24.16 2021-02-12T23:41:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173318.64 epoch sec; seqNo:6076; 4057628.37 north; 591371.45 east 2021-02-12T23:41:58Z- LcmTrn::handleMessages() - handling 40 messages took 0.62 ms and 128 calls... 2021-02-12T23:41:58Z- waiting for 1613173316.79, time is 1613173318.64 2021-02-12T23:41:58Z- LcmTrn::updateTrn() >>>> pose time 1613173318.64, meas time 1613173318.23 <<<< 2021-02-12T23:41:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173318.23 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:41:58Z- TerrainNav::motionUpdate() - currEstimate = (4057628, 591371.45, 24.16) 2021-02-12T23:41:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:41:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:41:58Z- TNavBF::Projecting Measurements 2021-02-12T23:41:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:41:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:41:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:41:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:41:58Z- TNavFilter::4 good beams. 2021-02-12T23:41:58Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:41:58Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:41:58Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:41:58Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:41:58Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:41:58Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:41:58Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:41:58Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:58Z- TerrainMapOctree: (vehN, vehE ) = (4057628.05, 591371.25). 2021-02-12T23:41:58Z- TerrainMapOctree: Min Distance = 4626.63. 2021-02-12T23:41:58Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:41:58Z- There is no specific good beam that all particles have in common. 2021-02-12T23:41:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:41:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 9.80 2021-02-12T23:41:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:41:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 12.73 2021-02-12T23:41:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:41:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-12T23:41:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:41:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-12T23:41:58Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:41:58Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:41:58Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:41:59Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:41:59Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:41:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:41:59Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:41:59Z- Filter 0 No beams used in subcloud update 2021-02-12T23:41:59Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:41:59Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:41:59Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 743.4 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:41:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173318.23 sec. 2021-02-12T23:41:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:41:59Z- TNavBF::mmse selected from filter -1 2021-02-12T23:41:59Z- variances([0], [2], and psi_berg) = 743.413213 833.027166 0.000000 2021-02-12T23:41:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:41:59Z- LcmTrn::publishEstimates() - MLE : 4057617.87 591329.95 24.16 2021-02-12T23:41:59Z- LcmTrn::publishEstimates() - MMSE : 4057628.37 591371.45 24.16 2021-02-12T23:41:59Z- LcmTrn::publishEstimates() - COVAR: 743.41 833.03 0.85 0.00 2021-02-12T23:41:59Z- AHRS_M2 msg: 1613173319.03 epoch sec; seqNo:6025 2021-02-12T23:41:59Z- AHRS_M2 msg: 0.61 phi; 0.28 theta; -0.16 psi 2021-02-12T23:41:59Z- handleDepth()-Depth_Keller msg: 1613173319037 epoch sec; seqNo:6077; depth 24.07 2021-02-12T23:41:59Z- handleDvl() - RDI_Pathfinder msg: 1613173319.06 epoch sec; seqNo:2870 2021-02-12T23:41:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 86.22 , 95.96 , 78.92 2021-02-12T23:41:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.05 , 0.02 2021-02-12T23:41:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173319.07 epoch sec; seqNo:6077; 4057628.37 north; 591371.45 east 2021-02-12T23:41:59Z- AHRS_M2 msg: 1613173319.45 epoch sec; seqNo:6026 2021-02-12T23:41:59Z- AHRS_M2 msg: 0.61 phi; 0.29 theta; -0.16 psi 2021-02-12T23:41:59Z- handleDepth()-Depth_Keller msg: 1613173319451 epoch sec; seqNo:6078; depth 23.96 2021-02-12T23:41:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173319.46 epoch sec; seqNo:6078; 4057628.75 north; 591371.44 east 2021-02-12T23:41:59Z- AHRS_M2 msg: 1613173319.84 epoch sec; seqNo:6027 2021-02-12T23:41:59Z- AHRS_M2 msg: 0.61 phi; 0.29 theta; -0.16 psi 2021-02-12T23:41:59Z- handleDepth()-Depth_Keller msg: 1613173319841 epoch sec; seqNo:6079; depth 23.87 2021-02-12T23:41:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173319.89 epoch sec; seqNo:6079; 4057629.13 north; 591371.44 east 2021-02-12T23:42:00Z- AHRS_M2 msg: 1613173320.24 epoch sec; seqNo:6028 2021-02-12T23:42:00Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.16 psi 2021-02-12T23:42:00Z- handleDepth()-Depth_Keller msg: 1613173320245 epoch sec; seqNo:6080; depth 23.76 2021-02-12T23:42:00Z- handleDvl() - RDI_Pathfinder msg: 1613173320.26 epoch sec; seqNo:2871 2021-02-12T23:42:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 0.00 , 98.19 , 68.24 2021-02-12T23:42:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , 0.03 2021-02-12T23:42:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173320.27 epoch sec; seqNo:6080; 4057629.13 north; 591371.44 east 2021-02-12T23:42:00Z- AHRS_M2 msg: 1613173320.64 epoch sec; seqNo:6029 2021-02-12T23:42:00Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.16 psi 2021-02-12T23:42:00Z- handleDepth()-Depth_Keller msg: 1613173320653 epoch sec; seqNo:6081; depth 23.68 2021-02-12T23:42:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173320.66 epoch sec; seqNo:6081; 4057629.51 north; 591371.44 east 2021-02-12T23:42:01Z- AHRS_M2 msg: 1613173321.04 epoch sec; seqNo:6030 2021-02-12T23:42:01Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.15 psi 2021-02-12T23:42:01Z- handleDepth()-Depth_Keller msg: 1613173321049 epoch sec; seqNo:6082; depth 23.56 2021-02-12T23:42:01Z- handleDvl() - RDI_Pathfinder msg: 1613173321.07 epoch sec; seqNo:2872 2021-02-12T23:42:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:42:01Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:42:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173321.08 epoch sec; seqNo:6082; 4057629.89 north; 591371.43 east 2021-02-12T23:42:01Z- AHRS_M2 msg: 1613173321.45 epoch sec; seqNo:6031 2021-02-12T23:42:01Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.15 psi 2021-02-12T23:42:01Z- handleDepth()-Depth_Keller msg: 1613173321452 epoch sec; seqNo:6083; depth 23.45 2021-02-12T23:42:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173321.46 epoch sec; seqNo:6083; 4057630.28 north; 591372.65 east 2021-02-12T23:42:01Z- AHRS_M2 msg: 1613173321.85 epoch sec; seqNo:6032 2021-02-12T23:42:01Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.14 psi 2021-02-12T23:42:01Z- handleDepth()-Depth_Keller msg: 1613173321860 epoch sec; seqNo:6084; depth 23.37 2021-02-12T23:42:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173321.87 epoch sec; seqNo:6084; 4057630.28 north; 591372.65 east 2021-02-12T23:42:02Z- AHRS_M2 msg: 1613173322.26 epoch sec; seqNo:6033 2021-02-12T23:42:02Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.14 psi 2021-02-12T23:42:02Z- handleDepth()-Depth_Keller msg: 1613173322260 epoch sec; seqNo:6085; depth 23.26 2021-02-12T23:42:02Z- handleDvl() - RDI_Pathfinder msg: 1613173322.28 epoch sec; seqNo:2873 2021-02-12T23:42:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 88.05 , 96.16 , 0.00 2021-02-12T23:42:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , 0.06 2021-02-12T23:42:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173322.29 epoch sec; seqNo:6085; 4057630.66 north; 591372.64 east 2021-02-12T23:42:02Z- AHRS_M2 msg: 1613173322.66 epoch sec; seqNo:6034 2021-02-12T23:42:02Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.14 psi 2021-02-12T23:42:02Z- handleDepth()-Depth_Keller msg: 1613173322664 epoch sec; seqNo:6086; depth 23.16 2021-02-12T23:42:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173322.67 epoch sec; seqNo:6086; 4057631.04 north; 591372.64 east 2021-02-12T23:42:03Z- AHRS_M2 msg: 1613173323.06 epoch sec; seqNo:6035 2021-02-12T23:42:03Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.14 psi 2021-02-12T23:42:03Z- handleDepth()-Depth_Keller msg: 1613173323068 epoch sec; seqNo:6087; depth 23.07 2021-02-12T23:42:03Z- handleDvl() - RDI_Pathfinder msg: 1613173323.08 epoch sec; seqNo:2874 2021-02-12T23:42:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 87.17 , 98.12 , 67.43 2021-02-12T23:42:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , 0.09 2021-02-12T23:42:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173323.09 epoch sec; seqNo:6087; 4057631.42 north; 591372.64 east 2021-02-12T23:42:03Z- AHRS_M2 msg: 1613173323.47 epoch sec; seqNo:6036 2021-02-12T23:42:03Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.14 psi 2021-02-12T23:42:03Z- handleDepth()-Depth_Keller msg: 1613173323477 epoch sec; seqNo:6088; depth 22.95 2021-02-12T23:42:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173323.49 epoch sec; seqNo:6088; 4057631.79 north; 591372.63 east 2021-02-12T23:42:03Z- AHRS_M2 msg: 1613173323.87 epoch sec; seqNo:6037 2021-02-12T23:42:03Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.14 psi 2021-02-12T23:42:03Z- handleDepth()-Depth_Keller msg: 1613173323878 epoch sec; seqNo:6089; depth 22.84 2021-02-12T23:42:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173323.89 epoch sec; seqNo:6089; 4057631.81 north; 591373.85 east 2021-02-12T23:42:04Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-12T23:42:04Z- waiting for 1613173321.64, time is 1613173323.89 2021-02-12T23:42:04Z- LcmTrn::updateTrn() >>>> pose time 1613173323.89, meas time 1613173323.08 <<<< 2021-02-12T23:42:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173323.08 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:42:04Z- TerrainNav::motionUpdate() - currEstimate = (4057632, 591373.85, 22.84) 2021-02-12T23:42:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:42:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:42:04Z- TNavBF::Projecting Measurements 2021-02-12T23:42:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:42:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:42:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:42:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:42:04Z- TNavFilter::4 good beams. 2021-02-12T23:42:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:42:04Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:42:04Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:42:04Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:42:04Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:42:04Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:42:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:42:04Z- TNavBF::mmse selected from filter -1 2021-02-12T23:42:04Z- TerrainMapOctree: (vehN, vehE ) = (4057631.28, 591373.48). 2021-02-12T23:42:04Z- TerrainMapOctree: Min Distance = 4622.72. 2021-02-12T23:42:04Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:42:04Z- There is no specific good beam that all particles have in common. 2021-02-12T23:42:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:42:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 8.75 2021-02-12T23:42:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:42:04Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 12.16 2021-02-12T23:42:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:42:04Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 15.40 2021-02-12T23:42:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:42:04Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 7.27 2021-02-12T23:42:04Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:42:04Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:42:04Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:42:04Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:42:04Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:42:04Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:42:04Z- Filter 0 No beams used in subcloud update 2021-02-12T23:42:04Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:42:04Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:42:04Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 743.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:42:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173323.08 sec. 2021-02-12T23:42:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:42:04Z- TNavBF::mmse selected from filter -1 2021-02-12T23:42:04Z- variances([0], [2], and psi_berg) = 743.293213 833.027064 0.000000 2021-02-12T23:42:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:42:04Z- LcmTrn::publishEstimates() - MLE : 4057621.31 591332.35 22.84 2021-02-12T23:42:04Z- LcmTrn::publishEstimates() - MMSE : 4057631.81 591373.85 22.84 2021-02-12T23:42:04Z- LcmTrn::publishEstimates() - COVAR: 743.29 833.03 0.80 0.00 2021-02-12T23:42:04Z- AHRS_M2 msg: 1613173324.28 epoch sec; seqNo:6038 2021-02-12T23:42:04Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.14 psi 2021-02-12T23:42:04Z- handleDepth()-Depth_Keller msg: 1613173324280 epoch sec; seqNo:6090; depth 22.74 2021-02-12T23:42:04Z- handleDvl() - RDI_Pathfinder msg: 1613173324.30 epoch sec; seqNo:2875 2021-02-12T23:42:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 160.51 , 96.70 , 66.61 2021-02-12T23:42:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.06 , 0.07 2021-02-12T23:42:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173324.31 epoch sec; seqNo:6090; 4057632.19 north; 591373.85 east 2021-02-12T23:42:04Z- AHRS_M2 msg: 1613173324.69 epoch sec; seqNo:6039 2021-02-12T23:42:04Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.13 psi 2021-02-12T23:42:04Z- handleDepth()-Depth_Keller msg: 1613173324697 epoch sec; seqNo:6091; depth 22.64 2021-02-12T23:42:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173324.71 epoch sec; seqNo:6091; 4057632.57 north; 591373.84 east 2021-02-12T23:42:05Z- AHRS_M2 msg: 1613173325.09 epoch sec; seqNo:6040 2021-02-12T23:42:05Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.13 psi 2021-02-12T23:42:05Z- handleDepth()-Depth_Keller msg: 1613173325101 epoch sec; seqNo:6092; depth 22.52 2021-02-12T23:42:05Z- handleDvl() - RDI_Pathfinder msg: 1613173325.11 epoch sec; seqNo:2876 2021-02-12T23:42:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 87.51 , 99.67 , 66.20 2021-02-12T23:42:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.08 , 0.02 2021-02-12T23:42:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173325.13 epoch sec; seqNo:6092; 4057632.94 north; 591373.84 east 2021-02-12T23:42:05Z- AHRS_M2 msg: 1613173325.49 epoch sec; seqNo:6041 2021-02-12T23:42:05Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.13 psi 2021-02-12T23:42:05Z- handleDepth()-Depth_Keller msg: 1613173325494 epoch sec; seqNo:6093; depth 22.41 2021-02-12T23:42:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173325.52 epoch sec; seqNo:6093; 4057632.94 north; 591373.84 east 2021-02-12T23:42:05Z- AHRS_M2 msg: 1613173325.90 epoch sec; seqNo:6042 2021-02-12T23:42:05Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.13 psi 2021-02-12T23:42:05Z- handleDepth()-Depth_Keller msg: 1613173325904 epoch sec; seqNo:6094; depth 22.32 2021-02-12T23:42:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173325.95 epoch sec; seqNo:6094; 4057633.32 north; 591373.84 east 2021-02-12T23:42:06Z- AHRS_M2 msg: 1613173326.30 epoch sec; seqNo:6043 2021-02-12T23:42:06Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.13 psi 2021-02-12T23:42:06Z- handleDepth()-Depth_Keller msg: 1613173326300 epoch sec; seqNo:6095; depth 22.19 2021-02-12T23:42:06Z- handleDvl() - RDI_Pathfinder msg: 1613173326.31 epoch sec; seqNo:2877 2021-02-12T23:42:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 87.44 , 99.61 , 69.06 2021-02-12T23:42:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.08 , -0.09 2021-02-12T23:42:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173326.32 epoch sec; seqNo:6095; 4057633.72 north; 591375.05 east 2021-02-12T23:42:06Z- AHRS_M2 msg: 1613173326.70 epoch sec; seqNo:6044 2021-02-12T23:42:06Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.13 psi 2021-02-12T23:42:06Z- handleDepth()-Depth_Keller msg: 1613173326707 epoch sec; seqNo:6096; depth 22.09 2021-02-12T23:42:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173326.72 epoch sec; seqNo:6096; 4057633.72 north; 591375.05 east 2021-02-12T23:42:07Z- AHRS_M2 msg: 1613173327.10 epoch sec; seqNo:6045 2021-02-12T23:42:07Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.13 psi 2021-02-12T23:42:07Z- handleDepth()-Depth_Keller msg: 1613173327113 epoch sec; seqNo:6097; depth 21.99 2021-02-12T23:42:07Z- handleDvl() - RDI_Pathfinder msg: 1613173327.17 epoch sec; seqNo:2878 2021-02-12T23:42:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 88.59 , 100.75 , 67.43 2021-02-12T23:42:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.02 , -0.15 2021-02-12T23:42:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173327.20 epoch sec; seqNo:6097; 4057634.09 north; 591375.05 east 2021-02-12T23:42:07Z- AHRS_M2 msg: 1613173327.51 epoch sec; seqNo:6046 2021-02-12T23:42:07Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.13 psi 2021-02-12T23:42:07Z- handleDepth()-Depth_Keller msg: 1613173327516 epoch sec; seqNo:6098; depth 21.86 2021-02-12T23:42:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173327.53 epoch sec; seqNo:6098; 4057634.09 north; 591375.05 east 2021-02-12T23:42:07Z- AHRS_M2 msg: 1613173327.91 epoch sec; seqNo:6047 2021-02-12T23:42:07Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.14 psi 2021-02-12T23:42:07Z- handleDepth()-Depth_Keller msg: 1613173327919 epoch sec; seqNo:6099; depth 21.74 2021-02-12T23:42:07Z- handleDvl() - RDI_Pathfinder msg: 1613173327.93 epoch sec; seqNo:2879 2021-02-12T23:42:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:42:07Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:42:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173327.94 epoch sec; seqNo:6099; 4057634.47 north; 591375.05 east 2021-02-12T23:42:08Z- AHRS_M2 msg: 1613173328.32 epoch sec; seqNo:6048 2021-02-12T23:42:08Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.14 psi 2021-02-12T23:42:08Z- handleDepth()-Depth_Keller msg: 1613173328322 epoch sec; seqNo:6100; depth 21.64 2021-02-12T23:42:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173328.33 epoch sec; seqNo:6100; 4057634.85 north; 591375.04 east 2021-02-12T23:42:08Z- AHRS_M2 msg: 1613173328.72 epoch sec; seqNo:6049 2021-02-12T23:42:08Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.15 psi 2021-02-12T23:42:08Z- handleDepth()-Depth_Keller msg: 1613173328724 epoch sec; seqNo:6101; depth 21.52 2021-02-12T23:42:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173328.74 epoch sec; seqNo:6101; 4057634.86 north; 591376.26 east 2021-02-12T23:42:09Z- AHRS_M2 msg: 1613173329.13 epoch sec; seqNo:6050 2021-02-12T23:42:09Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.15 psi 2021-02-12T23:42:09Z- LcmTrn::handleMessages() - handling 42 messages took 0.70 ms and 128 calls... 2021-02-12T23:42:09Z- waiting for 1613173326.89, time is 1613173329.13 2021-02-12T23:42:09Z- LcmTrn::updateTrn() >>>> pose time 1613173329.13, meas time 1613173327.93 <<<< 2021-02-12T23:42:09Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173327.93, ping # 0. 2021-02-12T23:42:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173327.93 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:42:09Z- TerrainNav::motionUpdate() - currEstimate = (4057635, 591376.26, 21.52) 2021-02-12T23:42:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:42:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:42:09Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:42:09Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:42:09Z- TNavBF::Projecting Measurements 2021-02-12T23:42:09Z- TNavFilter::Measurement from time = 1613173327.93 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:42:09Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:42:09Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:42:09Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:42:09Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:42:09Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:42:09Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:42:09Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:42:09Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 743.2 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:42:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:42:09Z- TNavBF::mmse selected from filter -1 2021-02-12T23:42:09Z- variances([0], [2], and psi_berg) = 743.173917 833.026962 0.000000 2021-02-12T23:42:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:42:09Z- LcmTrn::publishEstimates() - MLE : 4057624.36 591334.76 21.52 2021-02-12T23:42:09Z- LcmTrn::publishEstimates() - MMSE : 4057634.86 591376.26 21.52 2021-02-12T23:42:09Z- LcmTrn::publishEstimates() - COVAR: 743.17 833.03 1.00 0.00 2021-02-12T23:42:09Z- handleDepth()-Depth_Keller msg: 1613173329131 epoch sec; seqNo:6102; depth 21.40 2021-02-12T23:42:09Z- handleDvl() - RDI_Pathfinder msg: 1613173329.15 epoch sec; seqNo:2880 2021-02-12T23:42:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 89.80 , 101.97 , 0.00 2021-02-12T23:42:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.15 , -0.02 2021-02-12T23:42:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173329.18 epoch sec; seqNo:6102; 4057635.24 north; 591376.26 east 2021-02-12T23:42:09Z- AHRS_M2 msg: 1613173329.53 epoch sec; seqNo:6051 2021-02-12T23:42:09Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.16 psi 2021-02-12T23:42:09Z- handleDepth()-Depth_Keller msg: 1613173329532 epoch sec; seqNo:6103; depth 21.28 2021-02-12T23:42:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173329.54 epoch sec; seqNo:6103; 4057635.62 north; 591376.25 east 2021-02-12T23:42:09Z- AHRS_M2 msg: 1613173329.93 epoch sec; seqNo:6052 2021-02-12T23:42:09Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-12T23:42:09Z- handleDepth()-Depth_Keller msg: 1613173329946 epoch sec; seqNo:6104; depth 21.17 2021-02-12T23:42:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173329.96 epoch sec; seqNo:6104; 4057635.62 north; 591376.25 east 2021-02-12T23:42:10Z- AHRS_M2 msg: 1613173330.34 epoch sec; seqNo:6053 2021-02-12T23:42:10Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-12T23:42:10Z- handleDepth()-Depth_Keller msg: 1613173330345 epoch sec; seqNo:6105; depth 21.04 2021-02-12T23:42:10Z- handleDvl() - RDI_Pathfinder msg: 1613173330.36 epoch sec; seqNo:2881 2021-02-12T23:42:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.01 , 90.41 , 105.01 , 78.25 2021-02-12T23:42:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.14 , 0.09 2021-02-12T23:42:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173330.37 epoch sec; seqNo:6105; 4057636.00 north; 591376.25 east 2021-02-12T23:42:10Z- AHRS_M2 msg: 1613173330.74 epoch sec; seqNo:6054 2021-02-12T23:42:10Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:42:10Z- handleDepth()-Depth_Keller msg: 1613173330749 epoch sec; seqNo:6106; depth 20.92 2021-02-12T23:42:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173330.76 epoch sec; seqNo:6106; 4057636.38 north; 591376.25 east 2021-02-12T23:42:11Z- AHRS_M2 msg: 1613173331.15 epoch sec; seqNo:6055 2021-02-12T23:42:11Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:42:11Z- handleDepth()-Depth_Keller msg: 1613173331158 epoch sec; seqNo:6107; depth 20.83 2021-02-12T23:42:11Z- handleDvl() - RDI_Pathfinder msg: 1613173331.17 epoch sec; seqNo:2882 2021-02-12T23:42:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.60 , 91.90 , 102.85 , 68.65 2021-02-12T23:42:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.11 , 0.15 2021-02-12T23:42:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173331.18 epoch sec; seqNo:6107; 4057636.76 north; 591376.24 east 2021-02-12T23:42:11Z- AHRS_M2 msg: 1613173331.55 epoch sec; seqNo:6056 2021-02-12T23:42:11Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-12T23:42:11Z- handleDepth()-Depth_Keller msg: 1613173331552 epoch sec; seqNo:6108; depth 20.69 2021-02-12T23:42:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173331.57 epoch sec; seqNo:6108; 4057636.76 north; 591376.24 east 2021-02-12T23:42:11Z- AHRS_M2 msg: 1613173331.96 epoch sec; seqNo:6057 2021-02-12T23:42:11Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-12T23:42:11Z- handleDepth()-Depth_Keller msg: 1613173331961 epoch sec; seqNo:6109; depth 20.59 2021-02-12T23:42:11Z- handleDvl() - RDI_Pathfinder msg: 1613173331.98 epoch sec; seqNo:2883 2021-02-12T23:42:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 93.32 , 105.49 , 68.65 2021-02-12T23:42:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.16 2021-02-12T23:42:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173331.99 epoch sec; seqNo:6109; 4057637.15 north; 591377.46 east 2021-02-12T23:42:12Z- AHRS_M2 msg: 1613173332.36 epoch sec; seqNo:6058 2021-02-12T23:42:12Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-12T23:42:12Z- handleDepth()-Depth_Keller msg: 1613173332360 epoch sec; seqNo:6110; depth 20.45 2021-02-12T23:42:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173332.37 epoch sec; seqNo:6110; 4057637.53 north; 591377.45 east 2021-02-12T23:42:12Z- AHRS_M2 msg: 1613173332.76 epoch sec; seqNo:6059 2021-02-12T23:42:12Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-12T23:42:12Z- handleDepth()-Depth_Keller msg: 1613173332765 epoch sec; seqNo:6111; depth 20.34 2021-02-12T23:42:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173332.78 epoch sec; seqNo:6111; 4057637.91 north; 591377.45 east 2021-02-12T23:42:13Z- AHRS_M2 msg: 1613173333.16 epoch sec; seqNo:6060 2021-02-12T23:42:13Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-12T23:42:13Z- handleDepth()-Depth_Keller msg: 1613173333178 epoch sec; seqNo:6112; depth 20.20 2021-02-12T23:42:13Z- handleDvl() - RDI_Pathfinder msg: 1613173333.19 epoch sec; seqNo:2884 2021-02-12T23:42:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.05 , 105.35 , 70.29 2021-02-12T23:42:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , 0.14 2021-02-12T23:42:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173333.20 epoch sec; seqNo:6112; 4057638.29 north; 591377.45 east 2021-02-12T23:42:13Z- AHRS_M2 msg: 1613173333.57 epoch sec; seqNo:6061 2021-02-12T23:42:13Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-12T23:42:13Z- handleDepth()-Depth_Keller msg: 1613173333572 epoch sec; seqNo:6113; depth 20.08 2021-02-12T23:42:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173333.58 epoch sec; seqNo:6113; 4057638.29 north; 591377.45 east 2021-02-12T23:42:13Z- AHRS_M2 msg: 1613173333.98 epoch sec; seqNo:6062 2021-02-12T23:42:13Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-12T23:42:13Z- handleDepth()-Depth_Keller msg: 1613173333993 epoch sec; seqNo:6114; depth 19.97 2021-02-12T23:42:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173334.02 epoch sec; seqNo:6114; 4057638.67 north; 591377.44 east 2021-02-12T23:42:14Z- LcmTrn::handleMessages() - handling 43 messages took 0.67 ms and 130 calls... 2021-02-12T23:42:14Z- waiting for 1613173332.13, time is 1613173334.02 2021-02-12T23:42:14Z- LcmTrn::updateTrn() >>>> pose time 1613173334.02, meas time 1613173333.19 <<<< 2021-02-12T23:42:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173333.19 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:42:14Z- TerrainNav::motionUpdate() - currEstimate = (4057639, 591377.44, 19.97) 2021-02-12T23:42:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:42:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:42:14Z- TNavBF::Projecting Measurements 2021-02-12T23:42:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:42:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:42:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:42:14Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:42:14Z- TNavFilter::3 good beams. 2021-02-12T23:42:14Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:42:14Z- TNavBF:: beamIndex[0] = 1 2021-02-12T23:42:14Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:42:14Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:42:14Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:42:14Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:42:14Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:42:14Z- TNavBF::mmse selected from filter -1 2021-02-12T23:42:14Z- TerrainMapOctree: (vehN, vehE ) = (4057638.02, 591377.24). 2021-02-12T23:42:14Z- TerrainMapOctree: Min Distance = 4615.10. 2021-02-12T23:42:14Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:42:14Z- There is no specific good beam that all particles have in common. 2021-02-12T23:42:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:42:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 15.38 2021-02-12T23:42:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:42:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 17.76 2021-02-12T23:42:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:42:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 7.90 2021-02-12T23:42:14Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:42:14Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:42:14Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:42:14Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:42:14Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:42:14Z- Filter 0 No beams used in subcloud update 2021-02-12T23:42:14Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:42:14Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:42:14Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 743.1 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:42:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173333.19 sec. 2021-02-12T23:42:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:42:14Z- TNavBF::mmse selected from filter -1 2021-02-12T23:42:14Z- variances([0], [2], and psi_berg) = 743.056385 833.026862 0.000000 2021-02-12T23:42:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:42:14Z- LcmTrn::publishEstimates() - MLE : 4057628.17 591335.94 19.97 2021-02-12T23:42:14Z- LcmTrn::publishEstimates() - MMSE : 4057638.67 591377.44 19.97 2021-02-12T23:42:14Z- LcmTrn::publishEstimates() - COVAR: 743.06 833.03 0.97 0.00 2021-02-12T23:42:14Z- AHRS_M2 msg: 1613173334.38 epoch sec; seqNo:6063 2021-02-12T23:42:14Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-12T23:42:14Z- handleDepth()-Depth_Keller msg: 1613173334381 epoch sec; seqNo:6115; depth 19.84 2021-02-12T23:42:14Z- handleDvl() - RDI_Pathfinder msg: 1613173334.39 epoch sec; seqNo:2885 2021-02-12T23:42:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.18 , 97.65 , 106.16 , 69.06 2021-02-12T23:42:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.09 , 0.10 2021-02-12T23:42:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173334.40 epoch sec; seqNo:6115; 4057639.06 north; 591378.66 east 2021-02-12T23:42:14Z- AHRS_M2 msg: 1613173334.78 epoch sec; seqNo:6064 2021-02-12T23:42:14Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-12T23:42:14Z- handleDepth()-Depth_Keller msg: 1613173334784 epoch sec; seqNo:6116; depth 19.73 2021-02-12T23:42:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173334.80 epoch sec; seqNo:6116; 4057639.06 north; 591378.66 east 2021-02-12T23:42:15Z- AHRS_M2 msg: 1613173335.18 epoch sec; seqNo:6065 2021-02-12T23:42:15Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-12T23:42:15Z- handleDepth()-Depth_Keller msg: 1613173335192 epoch sec; seqNo:6117; depth 19.60 2021-02-12T23:42:15Z- handleDvl() - RDI_Pathfinder msg: 1613173335.23 epoch sec; seqNo:2886 2021-02-12T23:42:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 94.33 , 108.93 , 69.06 2021-02-12T23:42:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.13 , 0.04 2021-02-12T23:42:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173335.24 epoch sec; seqNo:6117; 4057639.44 north; 591378.66 east 2021-02-12T23:42:15Z- AHRS_M2 msg: 1613173335.59 epoch sec; seqNo:6066 2021-02-12T23:42:15Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-12T23:42:15Z- handleDepth()-Depth_Keller msg: 1613173335592 epoch sec; seqNo:6118; depth 19.48 2021-02-12T23:42:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173335.60 epoch sec; seqNo:6118; 4057639.82 north; 591378.65 east 2021-02-12T23:42:16Z- AHRS_M2 msg: 1613173336.00 epoch sec; seqNo:6067 2021-02-12T23:42:16Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.18 psi 2021-02-12T23:42:16Z- handleDepth()-Depth_Keller msg: 1613173336002 epoch sec; seqNo:6119; depth 19.36 2021-02-12T23:42:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173336.01 epoch sec; seqNo:6119; 4057639.82 north; 591378.65 east 2021-02-12T23:42:16Z- AHRS_M2 msg: 1613173336.40 epoch sec; seqNo:6068 2021-02-12T23:42:16Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.18 psi 2021-02-12T23:42:16Z- handleDepth()-Depth_Keller msg: 1613173336400 epoch sec; seqNo:6120; depth 19.23 2021-02-12T23:42:16Z- handleDvl() - RDI_Pathfinder msg: 1613173336.42 epoch sec; seqNo:2887 2021-02-12T23:42:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:42:16Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:42:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173336.43 epoch sec; seqNo:6120; 4057640.21 north; 591379.87 east 2021-02-12T23:42:16Z- AHRS_M2 msg: 1613173336.80 epoch sec; seqNo:6069 2021-02-12T23:42:16Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.19 psi 2021-02-12T23:42:16Z- handleDepth()-Depth_Keller msg: 1613173336813 epoch sec; seqNo:6121; depth 19.10 2021-02-12T23:42:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173336.83 epoch sec; seqNo:6121; 4057640.59 north; 591379.86 east 2021-02-12T23:42:17Z- AHRS_M2 msg: 1613173337.20 epoch sec; seqNo:6070 2021-02-12T23:42:17Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.19 psi 2021-02-12T23:42:17Z- handleDepth()-Depth_Keller msg: 1613173337208 epoch sec; seqNo:6122; depth 18.98 2021-02-12T23:42:17Z- handleDvl() - RDI_Pathfinder msg: 1613173337.22 epoch sec; seqNo:2888 2021-02-12T23:42:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:42:17Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:42:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173337.23 epoch sec; seqNo:6122; 4057640.97 north; 591379.86 east 2021-02-12T23:42:17Z- AHRS_M2 msg: 1613173337.61 epoch sec; seqNo:6071 2021-02-12T23:42:17Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.20 psi 2021-02-12T23:42:17Z- handleDepth()-Depth_Keller msg: 1613173337614 epoch sec; seqNo:6123; depth 18.86 2021-02-12T23:42:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173337.62 epoch sec; seqNo:6123; 4057640.97 north; 591379.86 east 2021-02-12T23:42:18Z- AHRS_M2 msg: 1613173338.01 epoch sec; seqNo:6072 2021-02-12T23:42:18Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.20 psi 2021-02-12T23:42:18Z- handleDepth()-Depth_Keller msg: 1613173338016 epoch sec; seqNo:6124; depth 18.75 2021-02-12T23:42:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173338.04 epoch sec; seqNo:6124; 4057641.36 north; 591381.08 east 2021-02-12T23:42:18Z- AHRS_M2 msg: 1613173338.41 epoch sec; seqNo:6073 2021-02-12T23:42:18Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.21 psi 2021-02-12T23:42:18Z- handleDepth()-Depth_Keller msg: 1613173338420 epoch sec; seqNo:6125; depth 18.62 2021-02-12T23:42:18Z- handleDvl() - RDI_Pathfinder msg: 1613173338.43 epoch sec; seqNo:2889 2021-02-12T23:42:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.91 , 97.24 , 107.65 , 77.91 2021-02-12T23:42:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.04 , -0.11 2021-02-12T23:42:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173338.47 epoch sec; seqNo:6125; 4057641.74 north; 591381.07 east 2021-02-12T23:42:18Z- AHRS_M2 msg: 1613173338.82 epoch sec; seqNo:6074 2021-02-12T23:42:18Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.21 psi 2021-02-12T23:42:18Z- handleDepth()-Depth_Keller msg: 1613173338829 epoch sec; seqNo:6126; depth 18.50 2021-02-12T23:42:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173338.84 epoch sec; seqNo:6126; 4057641.74 north; 591381.07 east 2021-02-12T23:42:19Z- AHRS_M2 msg: 1613173339.23 epoch sec; seqNo:6075 2021-02-12T23:42:19Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.22 psi 2021-02-12T23:42:19Z- handleDepth()-Depth_Keller msg: 1613173339237 epoch sec; seqNo:6127; depth 18.39 2021-02-12T23:42:19Z- LcmTrn::handleMessages() - handling 43 messages took 0.79 ms and 127 calls... 2021-02-12T23:42:19Z- waiting for 1613173337.02, time is 1613173339.23 2021-02-12T23:42:19Z- LcmTrn::updateTrn() >>>> pose time 1613173339.23, meas time 1613173338.43 <<<< 2021-02-12T23:42:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173338.43 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:42:19Z- TerrainNav::motionUpdate() - currEstimate = (4057642, 591381.07, 18.39) 2021-02-12T23:42:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:42:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:42:19Z- TNavBF::Projecting Measurements 2021-02-12T23:42:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:42:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:42:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:42:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:42:19Z- TNavFilter::4 good beams. 2021-02-12T23:42:19Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:42:19Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:42:19Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:42:19Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:42:19Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:42:19Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:42:19Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:42:19Z- TNavBF::mmse selected from filter -1 2021-02-12T23:42:19Z- TerrainMapOctree: (vehN, vehE ) = (4057641.27, 591380.52). 2021-02-12T23:42:19Z- TerrainMapOctree: Min Distance = 4610.52. 2021-02-12T23:42:19Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:42:19Z- There is no specific good beam that all particles have in common. 2021-02-12T23:42:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:42:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 9.71 2021-02-12T23:42:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:42:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-12T23:42:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:42:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 18.54 2021-02-12T23:42:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:42:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 9.71 2021-02-12T23:42:19Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:42:19Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:42:19Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:42:19Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:42:19Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:42:19Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:42:19Z- Filter 0 No beams used in subcloud update 2021-02-12T23:42:19Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:42:19Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:42:19Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 743.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:42:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173338.43 sec. 2021-02-12T23:42:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:42:19Z- TNavBF::mmse selected from filter -1 2021-02-12T23:42:19Z- variances([0], [2], and psi_berg) = 742.929302 833.026754 0.000000 2021-02-12T23:42:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:42:19Z- LcmTrn::publishEstimates() - MLE : 4057631.24 591339.57 18.39 2021-02-12T23:42:19Z- LcmTrn::publishEstimates() - MMSE : 4057641.74 591381.07 18.39 2021-02-12T23:42:19Z- LcmTrn::publishEstimates() - COVAR: 742.93 833.03 0.92 0.00 2021-02-12T23:42:19Z- handleDvl() - RDI_Pathfinder msg: 1613173339.25 epoch sec; seqNo:2890 2021-02-12T23:42:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.93 , 108.66 , 71.92 2021-02-12T23:42:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.11 , -0.08 2021-02-12T23:42:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173339.26 epoch sec; seqNo:6127; 4057642.11 north; 591381.07 east 2021-02-12T23:42:19Z- AHRS_M2 msg: 1613173339.63 epoch sec; seqNo:6076 2021-02-12T23:42:19Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.22 psi 2021-02-12T23:42:19Z- handleDepth()-Depth_Keller msg: 1613173339633 epoch sec; seqNo:6128; depth 18.27 2021-02-12T23:42:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173339.65 epoch sec; seqNo:6128; 4057642.49 north; 591381.07 east 2021-02-12T23:42:20Z- AHRS_M2 msg: 1613173340.03 epoch sec; seqNo:6077 2021-02-12T23:42:20Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.21 psi 2021-02-12T23:42:20Z- handleDepth()-Depth_Keller msg: 1613173340038 epoch sec; seqNo:6129; depth 18.17 2021-02-12T23:42:20Z- handleDvl() - RDI_Pathfinder msg: 1613173340.05 epoch sec; seqNo:2891 2021-02-12T23:42:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 100.35 , 108.46 , 71.92 2021-02-12T23:42:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.11 , -0.04 2021-02-12T23:42:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173340.07 epoch sec; seqNo:6129; 4057642.49 north; 591381.07 east 2021-02-12T23:42:20Z- AHRS_M2 msg: 1613173340.44 epoch sec; seqNo:6078 2021-02-12T23:42:20Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.21 psi 2021-02-12T23:42:20Z- handleDepth()-Depth_Keller msg: 1613173340441 epoch sec; seqNo:6130; depth 18.06 2021-02-12T23:42:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173340.45 epoch sec; seqNo:6130; 4057642.87 north; 591381.06 east 2021-02-12T23:42:20Z- AHRS_M2 msg: 1613173340.84 epoch sec; seqNo:6079 2021-02-12T23:42:20Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.20 psi 2021-02-12T23:42:20Z- handleDepth()-Depth_Keller msg: 1613173340847 epoch sec; seqNo:6131; depth 17.93 2021-02-12T23:42:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173340.86 epoch sec; seqNo:6131; 4057643.25 north; 591381.06 east 2021-02-12T23:42:21Z- AHRS_M2 msg: 1613173341.24 epoch sec; seqNo:6080 2021-02-12T23:42:21Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.20 psi 2021-02-12T23:42:21Z- handleDepth()-Depth_Keller msg: 1613173341248 epoch sec; seqNo:6132; depth 17.82 2021-02-12T23:42:21Z- handleDvl() - RDI_Pathfinder msg: 1613173341.26 epoch sec; seqNo:2892 2021-02-12T23:42:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.56 , 108.73 , 73.15 2021-02-12T23:42:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.15 , 0.00 2021-02-12T23:42:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173341.29 epoch sec; seqNo:6132; 4057643.63 north; 591381.05 east 2021-02-12T23:42:21Z- AHRS_M2 msg: 1613173341.65 epoch sec; seqNo:6081 2021-02-12T23:42:21Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.20 psi 2021-02-12T23:42:21Z- handleDepth()-Depth_Keller msg: 1613173341654 epoch sec; seqNo:6133; depth 17.69 2021-02-12T23:42:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173341.67 epoch sec; seqNo:6133; 4057643.63 north; 591381.05 east 2021-02-12T23:42:22Z- AHRS_M2 msg: 1613173342.06 epoch sec; seqNo:6082 2021-02-12T23:42:22Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.20 psi 2021-02-12T23:42:22Z- handleDepth()-Depth_Keller msg: 1613173342063 epoch sec; seqNo:6134; depth 17.59 2021-02-12T23:42:22Z- handleDvl() - RDI_Pathfinder msg: 1613173342.08 epoch sec; seqNo:2893 2021-02-12T23:42:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.65 , 108.46 , 139.56 2021-02-12T23:42:22Z- handleDvl() - RDI_Pathfinder msg: velocities 1.70 , 0.78 , 0.60 2021-02-12T23:42:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173342.09 epoch sec; seqNo:6134; 4057644.02 north; 591382.27 east 2021-02-12T23:42:22Z- AHRS_M2 msg: 1613173342.46 epoch sec; seqNo:6083 2021-02-12T23:42:22Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.19 psi 2021-02-12T23:42:22Z- handleDepth()-Depth_Keller msg: 1613173342462 epoch sec; seqNo:6135; depth 17.46 2021-02-12T23:42:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173342.49 epoch sec; seqNo:6135; 4057644.41 north; 591383.49 east 2021-02-12T23:42:22Z- AHRS_M2 msg: 1613173342.86 epoch sec; seqNo:6084 2021-02-12T23:42:22Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.19 psi 2021-02-12T23:42:22Z- handleDepth()-Depth_Keller msg: 1613173342869 epoch sec; seqNo:6136; depth 17.34 2021-02-12T23:42:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173342.88 epoch sec; seqNo:6136; 4057644.79 north; 591383.48 east 2021-02-12T23:42:23Z- AHRS_M2 msg: 1613173343.26 epoch sec; seqNo:6085 2021-02-12T23:42:23Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.19 psi 2021-02-12T23:42:23Z- handleDepth()-Depth_Keller msg: 1613173343268 epoch sec; seqNo:6137; depth 17.21 2021-02-12T23:42:23Z- handleDvl() - RDI_Pathfinder msg: 1613173343.28 epoch sec; seqNo:2894 2021-02-12T23:42:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.88 , 98.73 , 109.14 , 0.00 2021-02-12T23:42:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.17 , 0.16 2021-02-12T23:42:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173343.29 epoch sec; seqNo:6137; 4057645.17 north; 591383.48 east 2021-02-12T23:42:23Z- AHRS_M2 msg: 1613173343.67 epoch sec; seqNo:6086 2021-02-12T23:42:23Z- AHRS_M2 msg: 0.56 phi; 0.33 theta; -0.19 psi 2021-02-12T23:42:23Z- handleDepth()-Depth_Keller msg: 1613173343674 epoch sec; seqNo:6138; depth 17.08 2021-02-12T23:42:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173343.68 epoch sec; seqNo:6138; 4057645.55 north; 591383.47 east 2021-02-12T23:42:24Z- AHRS_M2 msg: 1613173344.09 epoch sec; seqNo:6087 2021-02-12T23:42:24Z- AHRS_M2 msg: 0.55 phi; 0.33 theta; -0.18 psi 2021-02-12T23:42:24Z- handleDepth()-Depth_Keller msg: 1613173344095 epoch sec; seqNo:6139; depth 16.97 2021-02-12T23:42:24Z- handleDvl() - RDI_Pathfinder msg: 1613173344.11 epoch sec; seqNo:2895 2021-02-12T23:42:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.28 , 97.38 , 108.19 , 78.45 2021-02-12T23:42:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.12 , 0.24 2021-02-12T23:42:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173344.12 epoch sec; seqNo:6139; 4057645.94 north; 591384.69 east 2021-02-12T23:42:24Z- LcmTrn::handleMessages() - handling 43 messages took 0.65 ms and 130 calls... 2021-02-12T23:42:24Z- waiting for 1613173342.23, time is 1613173344.12 2021-02-12T23:42:24Z- LcmTrn::updateTrn() equating pose 1613173344.12 and meas times 1613173344.12 2021-02-12T23:42:24Z- LcmTrn::updateTrn() >>>> pose time 1613173344.12, meas time 1613173344.12 <<<< 2021-02-12T23:42:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173344.12 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:42:24Z- TerrainNav::motionUpdate() - currEstimate = (4057646, 591384.69, 16.97) 2021-02-12T23:42:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:42:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:42:24Z- TNavBF::Projecting Measurements 2021-02-12T23:42:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:42:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:42:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:42:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:42:24Z- TNavFilter::4 good beams. 2021-02-12T23:42:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:42:24Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:42:24Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:42:24Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:42:24Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:42:24Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:42:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:42:24Z- TNavBF::mmse selected from filter -1 2021-02-12T23:42:24Z- TerrainMapOctree: (vehN, vehE ) = (4057645.94, 591384.69). 2021-02-12T23:42:24Z- TerrainMapOctree: Min Distance = 4604.26. 2021-02-12T23:42:24Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:42:24Z- There is no specific good beam that all particles have in common. 2021-02-12T23:42:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:42:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 10.06 2021-02-12T23:42:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:42:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 15.17 2021-02-12T23:42:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:42:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 18.73 2021-02-12T23:42:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:42:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-12T23:42:24Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:42:24Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:42:24Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:42:24Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:42:24Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:42:24Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:42:24Z- Filter 0 No beams used in subcloud update 2021-02-12T23:42:24Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:42:24Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:42:24Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 742.8 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:42:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173344.12 sec. 2021-02-12T23:42:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:42:24Z- TNavBF::mmse selected from filter -1 2021-02-12T23:42:24Z- variances([0], [2], and psi_berg) = 742.824724 833.026665 0.000000 2021-02-12T23:42:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:42:24Z- LcmTrn::publishEstimates() - MLE : 4057635.44 591343.19 16.97 2021-02-12T23:42:24Z- LcmTrn::publishEstimates() - MMSE : 4057645.94 591384.69 16.97 2021-02-12T23:42:24Z- LcmTrn::publishEstimates() - COVAR: 742.82 833.03 0.90 0.00 2021-02-12T23:42:24Z- AHRS_M2 msg: 1613173344.48 epoch sec; seqNo:6088 2021-02-12T23:42:24Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.18 psi 2021-02-12T23:42:24Z- handleDepth()-Depth_Keller msg: 1613173344484 epoch sec; seqNo:6140; depth 16.85 2021-02-12T23:42:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173344.49 epoch sec; seqNo:6140; 4057646.32 north; 591384.69 east 2021-02-12T23:42:24Z- AHRS_M2 msg: 1613173344.88 epoch sec; seqNo:6089 2021-02-12T23:42:24Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.17 psi 2021-02-12T23:42:24Z- handleDepth()-Depth_Keller msg: 1613173344889 epoch sec; seqNo:6141; depth 16.74 2021-02-12T23:42:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173344.90 epoch sec; seqNo:6141; 4057646.32 north; 591384.69 east 2021-02-12T23:42:25Z- AHRS_M2 msg: 1613173345.29 epoch sec; seqNo:6090 2021-02-12T23:42:25Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.16 psi 2021-02-12T23:42:25Z- handleDepth()-Depth_Keller msg: 1613173345290 epoch sec; seqNo:6142; depth 16.61 2021-02-12T23:42:25Z- handleDvl() - RDI_Pathfinder msg: 1613173345.30 epoch sec; seqNo:2896 2021-02-12T23:42:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 97.71 , 109.88 , 78.25 2021-02-12T23:42:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.01 , 0.25 2021-02-12T23:42:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173345.31 epoch sec; seqNo:6142; 4057646.70 north; 591384.68 east 2021-02-12T23:42:25Z- AHRS_M2 msg: 1613173345.69 epoch sec; seqNo:6091 2021-02-12T23:42:25Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.15 psi 2021-02-12T23:42:25Z- handleDepth()-Depth_Keller msg: 1613173345692 epoch sec; seqNo:6143; depth 16.49 2021-02-12T23:42:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173345.71 epoch sec; seqNo:6143; 4057647.08 north; 591384.68 east 2021-02-12T23:42:26Z- AHRS_M2 msg: 1613173346.09 epoch sec; seqNo:6092 2021-02-12T23:42:26Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.15 psi 2021-02-12T23:42:26Z- handleDepth()-Depth_Keller msg: 1613173346096 epoch sec; seqNo:6144; depth 16.40 2021-02-12T23:42:26Z- handleDvl() - RDI_Pathfinder msg: 1613173346.11 epoch sec; seqNo:2897 2021-02-12T23:42:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 97.71 , 111.23 , 0.00 2021-02-12T23:42:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.08 , 0.16 2021-02-12T23:42:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173346.13 epoch sec; seqNo:6144; 4057647.46 north; 591384.68 east 2021-02-12T23:42:26Z- AHRS_M2 msg: 1613173346.49 epoch sec; seqNo:6093 2021-02-12T23:42:26Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.14 psi 2021-02-12T23:42:26Z- handleDepth()-Depth_Keller msg: 1613173346505 epoch sec; seqNo:6145; depth 16.26 2021-02-12T23:42:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173346.52 epoch sec; seqNo:6145; 4057647.85 north; 591385.89 east 2021-02-12T23:42:26Z- AHRS_M2 msg: 1613173346.90 epoch sec; seqNo:6094 2021-02-12T23:42:26Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.14 psi 2021-02-12T23:42:26Z- handleDepth()-Depth_Keller msg: 1613173346907 epoch sec; seqNo:6146; depth 16.15 2021-02-12T23:42:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173346.91 epoch sec; seqNo:6146; 4057648.23 north; 591385.89 east 2021-02-12T23:42:27Z- AHRS_M2 msg: 1613173347.30 epoch sec; seqNo:6095 2021-02-12T23:42:27Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.13 psi 2021-02-12T23:42:27Z- handleDepth()-Depth_Keller msg: 1613173347309 epoch sec; seqNo:6147; depth 16.04 2021-02-12T23:42:27Z- handleDvl() - RDI_Pathfinder msg: 1613173347.32 epoch sec; seqNo:2898 2021-02-12T23:42:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.72 , 97.24 , 110.76 , 74.37 2021-02-12T23:42:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.09 , 0.02 2021-02-12T23:42:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173347.34 epoch sec; seqNo:6147; 4057648.23 north; 591385.89 east 2021-02-12T23:42:27Z- AHRS_M2 msg: 1613173347.71 epoch sec; seqNo:6096 2021-02-12T23:42:27Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.13 psi 2021-02-12T23:42:27Z- handleDepth()-Depth_Keller msg: 1613173347712 epoch sec; seqNo:6148; depth 15.90 2021-02-12T23:42:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173347.72 epoch sec; seqNo:6148; 4057648.61 north; 591385.88 east 2021-02-12T23:42:28Z- AHRS_M2 msg: 1613173348.12 epoch sec; seqNo:6097 2021-02-12T23:42:28Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.13 psi 2021-02-12T23:42:28Z- handleDepth()-Depth_Keller msg: 1613173348124 epoch sec; seqNo:6149; depth 15.79 2021-02-12T23:42:28Z- handleDvl() - RDI_Pathfinder msg: 1613173348.14 epoch sec; seqNo:2899 2021-02-12T23:42:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.65 , 112.25 , 72.33 2021-02-12T23:42:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , -0.07 2021-02-12T23:42:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173348.15 epoch sec; seqNo:6149; 4057648.99 north; 591385.88 east 2021-02-12T23:42:28Z- AHRS_M2 msg: 1613173348.52 epoch sec; seqNo:6098 2021-02-12T23:42:28Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-12T23:42:28Z- handleDepth()-Depth_Keller msg: 1613173348520 epoch sec; seqNo:6150; depth 15.66 2021-02-12T23:42:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173348.55 epoch sec; seqNo:6150; 4057649.36 north; 591385.88 east 2021-02-12T23:42:28Z- AHRS_M2 msg: 1613173348.92 epoch sec; seqNo:6099 2021-02-12T23:42:28Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.13 psi 2021-02-12T23:42:28Z- handleDepth()-Depth_Keller msg: 1613173348928 epoch sec; seqNo:6151; depth 15.57 2021-02-12T23:42:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173348.94 epoch sec; seqNo:6151; 4057649.38 north; 591387.10 east 2021-02-12T23:42:29Z- AHRS_M2 msg: 1613173349.32 epoch sec; seqNo:6100 2021-02-12T23:42:29Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.13 psi 2021-02-12T23:42:29Z- handleDepth()-Depth_Keller msg: 1613173349328 epoch sec; seqNo:6152; depth 15.43 2021-02-12T23:42:29Z- handleDvl() - RDI_Pathfinder msg: 1613173349.34 epoch sec; seqNo:2900 2021-02-12T23:42:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 97.51 , 111.03 , 78.58 2021-02-12T23:42:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , -0.14 2021-02-12T23:42:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173349.35 epoch sec; seqNo:6152; 4057649.76 north; 591387.09 east 2021-02-12T23:42:29Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 130 calls... 2021-02-12T23:42:29Z- waiting for 1613173347.12, time is 1613173349.35 2021-02-12T23:42:29Z- LcmTrn::updateTrn() equating pose 1613173349.35 and meas times 1613173349.35 2021-02-12T23:42:29Z- LcmTrn::updateTrn() >>>> pose time 1613173349.35, meas time 1613173349.35 <<<< 2021-02-12T23:42:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173349.35 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:42:29Z- TerrainNav::motionUpdate() - currEstimate = (4057650, 591387.09, 15.43) 2021-02-12T23:42:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:42:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:42:29Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173349.35 sec.; No relevant navigation data available 2021-02-12T23:42:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:42:29Z- TNavBF::mmse selected from filter -1 2021-02-12T23:42:29Z- variances([0], [2], and psi_berg) = 742.730592 833.026585 0.000000 2021-02-12T23:42:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:42:29Z- LcmTrn::publishEstimates() - MLE : 4057639.26 591345.59 15.43 2021-02-12T23:42:29Z- LcmTrn::publishEstimates() - MMSE : 4057649.76 591387.09 15.43 2021-02-12T23:42:29Z- LcmTrn::publishEstimates() - COVAR: 742.73 833.03 1.13 0.00 2021-02-12T23:42:29Z- AHRS_M2 msg: 1613173349.73 epoch sec; seqNo:6101 2021-02-12T23:42:29Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.12 psi 2021-02-12T23:42:29Z- handleDepth()-Depth_Keller msg: 1613173349733 epoch sec; seqNo:6153; depth 15.31 2021-02-12T23:42:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173349.74 epoch sec; seqNo:6153; 4057650.14 north; 591387.09 east 2021-02-12T23:42:30Z- AHRS_M2 msg: 1613173350.13 epoch sec; seqNo:6102 2021-02-12T23:42:30Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.11 psi 2021-02-12T23:42:30Z- handleDepth()-Depth_Keller msg: 1613173350140 epoch sec; seqNo:6154; depth 15.20 2021-02-12T23:42:30Z- handleDvl() - RDI_Pathfinder msg: 1613173350.16 epoch sec; seqNo:2901 2021-02-12T23:42:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.13 , 97.65 , 111.17 , 78.72 2021-02-12T23:42:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.09 , -0.13 2021-02-12T23:42:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173350.17 epoch sec; seqNo:6154; 4057650.14 north; 591387.09 east 2021-02-12T23:42:30Z- AHRS_M2 msg: 1613173350.54 epoch sec; seqNo:6103 2021-02-12T23:42:30Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.11 psi 2021-02-12T23:42:30Z- handleDepth()-Depth_Keller msg: 1613173350540 epoch sec; seqNo:6155; depth 15.08 2021-02-12T23:42:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173350.55 epoch sec; seqNo:6155; 4057650.51 north; 591387.08 east 2021-02-12T23:42:30Z- AHRS_M2 msg: 1613173350.94 epoch sec; seqNo:6104 2021-02-12T23:42:30Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.11 psi 2021-02-12T23:42:30Z- handleDepth()-Depth_Keller msg: 1613173350949 epoch sec; seqNo:6156; depth 14.96 2021-02-12T23:42:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173350.96 epoch sec; seqNo:6156; 4057650.89 north; 591387.08 east 2021-02-12T23:42:31Z- AHRS_M2 msg: 1613173351.35 epoch sec; seqNo:6105 2021-02-12T23:42:31Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.11 psi 2021-02-12T23:42:31Z- handleDepth()-Depth_Keller msg: 1613173351361 epoch sec; seqNo:6157; depth 14.84 2021-02-12T23:42:31Z- handleDvl() - RDI_Pathfinder msg: 1613173351.37 epoch sec; seqNo:2902 2021-02-12T23:42:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.48 , 94.94 , 111.17 , 78.72 2021-02-12T23:42:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.12 , -0.08 2021-02-12T23:42:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173351.41 epoch sec; seqNo:6157; 4057651.27 north; 591387.08 east 2021-02-12T23:42:31Z- AHRS_M2 msg: 1613173351.75 epoch sec; seqNo:6106 2021-02-12T23:42:31Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.12 psi 2021-02-12T23:42:31Z- handleDepth()-Depth_Keller msg: 1613173351752 epoch sec; seqNo:6158; depth 14.71 2021-02-12T23:42:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173351.77 epoch sec; seqNo:6158; 4057651.27 north; 591387.08 east 2021-02-12T23:42:32Z- AHRS_M2 msg: 1613173352.15 epoch sec; seqNo:6107 2021-02-12T23:42:32Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.13 psi 2021-02-12T23:42:32Z- handleDepth()-Depth_Keller msg: 1613173352165 epoch sec; seqNo:6159; depth 14.59 2021-02-12T23:42:32Z- handleDvl() - RDI_Pathfinder msg: 1613173352.18 epoch sec; seqNo:2903 2021-02-12T23:42:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.13 , 97.65 , 112.52 , 74.78 2021-02-12T23:42:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.10 , -0.03 2021-02-12T23:42:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173352.19 epoch sec; seqNo:6159; 4057651.66 north; 591388.29 east 2021-02-12T23:42:32Z- AHRS_M2 msg: 1613173352.56 epoch sec; seqNo:6108 2021-02-12T23:42:32Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.14 psi 2021-02-12T23:42:32Z- handleDepth()-Depth_Keller msg: 1613173352564 epoch sec; seqNo:6160; depth 14.46 2021-02-12T23:42:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173352.58 epoch sec; seqNo:6160; 4057652.04 north; 591388.29 east 2021-02-12T23:42:32Z- AHRS_M2 msg: 1613173352.96 epoch sec; seqNo:6109 2021-02-12T23:42:32Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.15 psi 2021-02-12T23:42:32Z- handleDepth()-Depth_Keller msg: 1613173352966 epoch sec; seqNo:6161; depth 14.36 2021-02-12T23:42:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173352.97 epoch sec; seqNo:6161; 4057652.04 north; 591388.29 east 2021-02-12T23:42:33Z- AHRS_M2 msg: 1613173353.36 epoch sec; seqNo:6110 2021-02-12T23:42:33Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:42:33Z- handleDepth()-Depth_Keller msg: 1613173353368 epoch sec; seqNo:6162; depth 14.24 2021-02-12T23:42:33Z- handleDvl() - RDI_Pathfinder msg: 1613173353.38 epoch sec; seqNo:2904 2021-02-12T23:42:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 96.36 , 112.58 , 78.78 2021-02-12T23:42:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.09 , 0.01 2021-02-12T23:42:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173353.40 epoch sec; seqNo:6162; 4057652.42 north; 591388.29 east 2021-02-12T23:42:33Z- AHRS_M2 msg: 1613173353.77 epoch sec; seqNo:6111 2021-02-12T23:42:33Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-12T23:42:33Z- handleDepth()-Depth_Keller msg: 1613173353772 epoch sec; seqNo:6163; depth 14.11 2021-02-12T23:42:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173353.78 epoch sec; seqNo:6163; 4057652.80 north; 591388.28 east 2021-02-12T23:42:34Z- AHRS_M2 msg: 1613173354.18 epoch sec; seqNo:6112 2021-02-12T23:42:34Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.17 psi 2021-02-12T23:42:34Z- handleDepth()-Depth_Keller msg: 1613173354181 epoch sec; seqNo:6164; depth 13.98 2021-02-12T23:42:34Z- handleDvl() - RDI_Pathfinder msg: 1613173354.19 epoch sec; seqNo:2905 2021-02-12T23:42:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 100.34 , 112.51 , 75.18 2021-02-12T23:42:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.12 , 0.06 2021-02-12T23:42:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173354.20 epoch sec; seqNo:6164; 4057653.18 north; 591388.28 east 2021-02-12T23:42:34Z- LcmTrn::handleMessages() - handling 41 messages took 0.69 ms and 128 calls... 2021-02-12T23:42:34Z- waiting for 1613173352.35, time is 1613173354.20 2021-02-12T23:42:34Z- LcmTrn::updateTrn() equating pose 1613173354.20 and meas times 1613173354.20 2021-02-12T23:42:34Z- LcmTrn::updateTrn() >>>> pose time 1613173354.20, meas time 1613173354.20 <<<< 2021-02-12T23:42:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173354.20 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:42:34Z- TerrainNav::motionUpdate() - currEstimate = (4057653, 591388.28, 13.98) 2021-02-12T23:42:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:42:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:42:34Z- TNavBF::Projecting Measurements 2021-02-12T23:42:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:42:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:42:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:42:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:42:34Z- TNavFilter::4 good beams. 2021-02-12T23:42:34Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:42:34Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:42:34Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:42:34Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:42:34Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:42:34Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:42:34Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:42:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:42:34Z- TerrainMapOctree: (vehN, vehE ) = (4057653.18, 591388.28). 2021-02-12T23:42:34Z- TerrainMapOctree: Min Distance = 4596.37. 2021-02-12T23:42:34Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:42:34Z- There is no specific good beam that all particles have in common. 2021-02-12T23:42:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:42:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 11.32 2021-02-12T23:42:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:42:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 16.11 2021-02-12T23:42:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:42:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 20.25 2021-02-12T23:42:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:42:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 9.04 2021-02-12T23:42:34Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:42:34Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:42:34Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:42:34Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:42:34Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:42:34Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:42:34Z- Filter 0 No beams used in subcloud update 2021-02-12T23:42:34Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:42:34Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:42:34Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 742.6 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:42:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173354.20 sec. 2021-02-12T23:42:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:42:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:42:34Z- variances([0], [2], and psi_berg) = 742.646313 833.026513 0.000000 2021-02-12T23:42:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:42:34Z- LcmTrn::publishEstimates() - MLE : 4057642.68 591346.78 13.98 2021-02-12T23:42:34Z- LcmTrn::publishEstimates() - MMSE : 4057653.18 591388.28 13.98 2021-02-12T23:42:34Z- LcmTrn::publishEstimates() - COVAR: 742.65 833.03 0.97 0.00 2021-02-12T23:42:34Z- AHRS_M2 msg: 1613173354.58 epoch sec; seqNo:6113 2021-02-12T23:42:34Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:42:34Z- handleDepth()-Depth_Keller msg: 1613173354580 epoch sec; seqNo:6165; depth 13.87 2021-02-12T23:42:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173354.60 epoch sec; seqNo:6165; 4057653.18 north; 591388.28 east 2021-02-12T23:42:34Z- AHRS_M2 msg: 1613173354.99 epoch sec; seqNo:6114 2021-02-12T23:42:34Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:42:34Z- handleDepth()-Depth_Keller msg: 1613173354998 epoch sec; seqNo:6166; depth 13.75 2021-02-12T23:42:35Z- handleDvl() - RDI_Pathfinder msg: 1613173355.03 epoch sec; seqNo:2906 2021-02-12T23:42:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.13 , 100.35 , 111.17 , 76.01 2021-02-12T23:42:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.16 , 0.13 2021-02-12T23:42:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173355.04 epoch sec; seqNo:6166; 4057653.56 north; 591388.27 east 2021-02-12T23:42:35Z- AHRS_M2 msg: 1613173355.38 epoch sec; seqNo:6115 2021-02-12T23:42:35Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-12T23:42:35Z- handleDepth()-Depth_Keller msg: 1613173355388 epoch sec; seqNo:6167; depth 13.61 2021-02-12T23:42:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173355.40 epoch sec; seqNo:6167; 4057653.95 north; 591389.49 east 2021-02-12T23:42:35Z- AHRS_M2 msg: 1613173355.79 epoch sec; seqNo:6116 2021-02-12T23:42:35Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.18 psi 2021-02-12T23:42:35Z- handleDepth()-Depth_Keller msg: 1613173355792 epoch sec; seqNo:6168; depth 13.50 2021-02-12T23:42:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173355.81 epoch sec; seqNo:6168; 4057654.33 north; 591389.49 east 2021-02-12T23:42:36Z- AHRS_M2 msg: 1613173356.19 epoch sec; seqNo:6117 2021-02-12T23:42:36Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-12T23:42:36Z- handleDepth()-Depth_Keller msg: 1613173356196 epoch sec; seqNo:6169; depth 13.41 2021-02-12T23:42:36Z- handleDvl() - RDI_Pathfinder msg: 1613173356.21 epoch sec; seqNo:2907 2021-02-12T23:42:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 99.00 , 112.52 , 76.83 2021-02-12T23:42:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.11 , 0.21 2021-02-12T23:42:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173356.23 epoch sec; seqNo:6169; 4057654.33 north; 591389.49 east 2021-02-12T23:42:36Z- AHRS_M2 msg: 1613173356.60 epoch sec; seqNo:6118 2021-02-12T23:42:36Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-12T23:42:36Z- handleDepth()-Depth_Keller msg: 1613173356600 epoch sec; seqNo:6170; depth 13.27 2021-02-12T23:42:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173356.61 epoch sec; seqNo:6170; 4057654.71 north; 591389.48 east 2021-02-12T23:42:37Z- AHRS_M2 msg: 1613173357.01 epoch sec; seqNo:6119 2021-02-12T23:42:37Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-12T23:42:37Z- handleDepth()-Depth_Keller msg: 1613173357011 epoch sec; seqNo:6171; depth 13.17 2021-02-12T23:42:37Z- handleDvl() - RDI_Pathfinder msg: 1613173357.02 epoch sec; seqNo:2908 2021-02-12T23:42:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.43 , 112.25 , 77.24 2021-02-12T23:42:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , 0.18 2021-02-12T23:42:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173357.03 epoch sec; seqNo:6171; 4057655.09 north; 591389.48 east 2021-02-12T23:42:37Z- AHRS_M2 msg: 1613173357.40 epoch sec; seqNo:6120 2021-02-12T23:42:37Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.17 psi 2021-02-12T23:42:37Z- handleDepth()-Depth_Keller msg: 1613173357418 epoch sec; seqNo:6172; depth 13.05 2021-02-12T23:42:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173357.43 epoch sec; seqNo:6172; 4057655.47 north; 591389.47 east 2021-02-12T23:42:37Z- AHRS_M2 msg: 1613173357.81 epoch sec; seqNo:6121 2021-02-12T23:42:37Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-12T23:42:37Z- handleDepth()-Depth_Keller msg: 1613173357817 epoch sec; seqNo:6173; depth 12.93 2021-02-12T23:42:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173357.83 epoch sec; seqNo:6173; 4057655.86 north; 591390.69 east 2021-02-12T23:42:38Z- AHRS_M2 msg: 1613173358.21 epoch sec; seqNo:6122 2021-02-12T23:42:38Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-12T23:42:38Z- handleDepth()-Depth_Keller msg: 1613173358220 epoch sec; seqNo:6174; depth 12.83 2021-02-12T23:42:38Z- handleDvl() - RDI_Pathfinder msg: 1613173358.24 epoch sec; seqNo:2909 2021-02-12T23:42:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 100.01 , 113.53 , 78.38 2021-02-12T23:42:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.08 , 0.04 2021-02-12T23:42:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173358.26 epoch sec; seqNo:6174; 4057655.86 north; 591390.69 east 2021-02-12T23:42:38Z- AHRS_M2 msg: 1613173358.62 epoch sec; seqNo:6123 2021-02-12T23:42:38Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.14 psi 2021-02-12T23:42:38Z- handleDepth()-Depth_Keller msg: 1613173358622 epoch sec; seqNo:6175; depth 12.70 2021-02-12T23:42:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173358.64 epoch sec; seqNo:6175; 4057656.24 north; 591390.69 east 2021-02-12T23:42:39Z- AHRS_M2 msg: 1613173359.03 epoch sec; seqNo:6124 2021-02-12T23:42:39Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.14 psi 2021-02-12T23:42:39Z- handleDepth()-Depth_Keller msg: 1613173359038 epoch sec; seqNo:6176; depth 12.59 2021-02-12T23:42:39Z- handleDvl() - RDI_Pathfinder msg: 1613173359.05 epoch sec; seqNo:2910 2021-02-12T23:42:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 96.16 , 115.09 , 77.64 2021-02-12T23:42:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , -0.11 2021-02-12T23:42:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173359.06 epoch sec; seqNo:6176; 4057656.61 north; 591390.68 east 2021-02-12T23:42:39Z- AHRS_M2 msg: 1613173359.43 epoch sec; seqNo:6125 2021-02-12T23:42:39Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.14 psi 2021-02-12T23:42:39Z- handleDepth()-Depth_Keller msg: 1613173359465 epoch sec; seqNo:6177; depth 12.47 2021-02-12T23:42:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173359.48 epoch sec; seqNo:6177; 4057656.61 north; 591390.68 east 2021-02-12T23:42:39Z- LcmTrn::handleMessages() - handling 44 messages took 0.79 ms and 129 calls... 2021-02-12T23:42:39Z- waiting for 1613173357.20, time is 1613173359.48 2021-02-12T23:42:39Z- LcmTrn::updateTrn() >>>> pose time 1613173359.48, meas time 1613173359.05 <<<< 2021-02-12T23:42:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173359.05 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:42:39Z- TerrainNav::motionUpdate() - currEstimate = (4057657, 591390.68, 12.47) 2021-02-12T23:42:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:42:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:42:39Z- TNavBF::Projecting Measurements 2021-02-12T23:42:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:42:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:42:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:42:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:42:39Z- TNavFilter::4 good beams. 2021-02-12T23:42:39Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:42:39Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:42:39Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:42:39Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:42:39Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:42:39Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:42:39Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:42:39Z- TNavBF::mmse selected from filter -1 2021-02-12T23:42:39Z- TerrainMapOctree: (vehN, vehE ) = (4057656.34, 591390.49). 2021-02-12T23:42:39Z- TerrainMapOctree: Min Distance = 4592.52. 2021-02-12T23:42:39Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:42:39Z- There is no specific good beam that all particles have in common. 2021-02-12T23:42:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:42:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 12.03 2021-02-12T23:42:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:42:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 14.79 2021-02-12T23:42:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:42:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 21.19 2021-02-12T23:42:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:42:39Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 9.65 2021-02-12T23:42:39Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:42:39Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:42:39Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:42:39Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:42:39Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:42:39Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:42:39Z- Filter 0 No beams used in subcloud update 2021-02-12T23:42:39Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:42:39Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:42:39Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 742.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:42:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173359.05 sec. 2021-02-12T23:42:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:42:39Z- TNavBF::mmse selected from filter -1 2021-02-12T23:42:39Z- variances([0], [2], and psi_berg) = 742.533735 833.026418 0.000000 2021-02-12T23:42:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:42:39Z- LcmTrn::publishEstimates() - MLE : 4057646.11 591349.18 12.47 2021-02-12T23:42:39Z- LcmTrn::publishEstimates() - MMSE : 4057656.61 591390.68 12.47 2021-02-12T23:42:39Z- LcmTrn::publishEstimates() - COVAR: 742.53 833.03 0.91 0.00 2021-02-12T23:42:39Z- AHRS_M2 msg: 1613173359.83 epoch sec; seqNo:6126 2021-02-12T23:42:39Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.15 psi 2021-02-12T23:42:39Z- handleDepth()-Depth_Keller msg: 1613173359837 epoch sec; seqNo:6178; depth 12.35 2021-02-12T23:42:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173359.85 epoch sec; seqNo:6178; 4057656.99 north; 591390.68 east 2021-02-12T23:42:40Z- AHRS_M2 msg: 1613173360.23 epoch sec; seqNo:6127 2021-02-12T23:42:40Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.16 psi 2021-02-12T23:42:40Z- handleDepth()-Depth_Keller msg: 1613173360245 epoch sec; seqNo:6179; depth 12.24 2021-02-12T23:42:40Z- handleDvl() - RDI_Pathfinder msg: 1613173360.26 epoch sec; seqNo:2911 2021-02-12T23:42:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.63 , 113.80 , 78.65 2021-02-12T23:42:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.12 , -0.12 2021-02-12T23:42:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173360.27 epoch sec; seqNo:6179; 4057657.39 north; 591391.90 east 2021-02-12T23:42:40Z- AHRS_M2 msg: 1613173360.64 epoch sec; seqNo:6128 2021-02-12T23:42:40Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-12T23:42:40Z- handleDepth()-Depth_Keller msg: 1613173360640 epoch sec; seqNo:6180; depth 12.11 2021-02-12T23:42:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173360.66 epoch sec; seqNo:6180; 4057657.76 north; 591391.89 east 2021-02-12T23:42:41Z- AHRS_M2 msg: 1613173361.04 epoch sec; seqNo:6129 2021-02-12T23:42:41Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:42:41Z- handleDepth()-Depth_Keller msg: 1613173361051 epoch sec; seqNo:6181; depth 12.00 2021-02-12T23:42:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173361.09 epoch sec; seqNo:6181; 4057657.76 north; 591391.89 east 2021-02-12T23:42:41Z- AHRS_M2 msg: 1613173361.44 epoch sec; seqNo:6130 2021-02-12T23:42:41Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:42:41Z- handleDepth()-Depth_Keller msg: 1613173361448 epoch sec; seqNo:6182; depth 11.88 2021-02-12T23:42:41Z- handleDvl() - RDI_Pathfinder msg: 1613173361.46 epoch sec; seqNo:2912 2021-02-12T23:42:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 101.22 , 116.09 , 78.45 2021-02-12T23:42:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.21 , -0.01 2021-02-12T23:42:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173361.47 epoch sec; seqNo:6182; 4057658.14 north; 591391.89 east 2021-02-12T23:42:41Z- AHRS_M2 msg: 1613173361.85 epoch sec; seqNo:6131 2021-02-12T23:42:41Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-12T23:42:41Z- handleDepth()-Depth_Keller msg: 1613173361853 epoch sec; seqNo:6183; depth 11.76 2021-02-12T23:42:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173361.87 epoch sec; seqNo:6183; 4057658.52 north; 591391.88 east 2021-02-12T23:42:42Z- AHRS_M2 msg: 1613173362.25 epoch sec; seqNo:6132 2021-02-12T23:42:42Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-12T23:42:42Z- handleDepth()-Depth_Keller msg: 1613173362266 epoch sec; seqNo:6184; depth 11.64 2021-02-12T23:42:42Z- handleDvl() - RDI_Pathfinder msg: 1613173362.28 epoch sec; seqNo:2913 2021-02-12T23:42:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.62 , 101.49 , 116.36 , 76.41 2021-02-12T23:42:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.17 , 0.10 2021-02-12T23:42:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173362.29 epoch sec; seqNo:6184; 4057658.90 north; 591391.88 east 2021-02-12T23:42:42Z- AHRS_M2 msg: 1613173362.65 epoch sec; seqNo:6133 2021-02-12T23:42:42Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-12T23:42:42Z- handleDepth()-Depth_Keller msg: 1613173362660 epoch sec; seqNo:6185; depth 11.51 2021-02-12T23:42:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173362.67 epoch sec; seqNo:6185; 4057658.90 north; 591391.88 east 2021-02-12T23:42:43Z- AHRS_M2 msg: 1613173363.08 epoch sec; seqNo:6134 2021-02-12T23:42:43Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-12T23:42:43Z- handleDepth()-Depth_Keller msg: 1613173363088 epoch sec; seqNo:6186; depth 11.41 2021-02-12T23:42:43Z- handleDvl() - RDI_Pathfinder msg: 1613173363.11 epoch sec; seqNo:2914 2021-02-12T23:42:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.62 , 100.14 , 113.66 , 78.51 2021-02-12T23:42:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.06 , 0.15 2021-02-12T23:42:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173363.12 epoch sec; seqNo:6186; 4057659.28 north; 591391.88 east 2021-02-12T23:42:43Z- AHRS_M2 msg: 1613173363.46 epoch sec; seqNo:6135 2021-02-12T23:42:43Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-12T23:42:43Z- handleDepth()-Depth_Keller msg: 1613173363469 epoch sec; seqNo:6187; depth 11.29 2021-02-12T23:42:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173363.48 epoch sec; seqNo:6187; 4057659.66 north; 591391.87 east 2021-02-12T23:42:43Z- AHRS_M2 msg: 1613173363.87 epoch sec; seqNo:6136 2021-02-12T23:42:43Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-12T23:42:43Z- handleDepth()-Depth_Keller msg: 1613173363884 epoch sec; seqNo:6188; depth 11.19 2021-02-12T23:42:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173363.89 epoch sec; seqNo:6188; 4057660.05 north; 591393.09 east 2021-02-12T23:42:44Z- AHRS_M2 msg: 1613173364.27 epoch sec; seqNo:6137 2021-02-12T23:42:44Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-12T23:42:44Z- handleDepth()-Depth_Keller msg: 1613173364276 epoch sec; seqNo:6189; depth 11.07 2021-02-12T23:42:44Z- handleDvl() - RDI_Pathfinder msg: 1613173364.29 epoch sec; seqNo:2915 2021-02-12T23:42:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.04 , 102.91 , 115.08 , 78.04 2021-02-12T23:42:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.08 , 0.14 2021-02-12T23:42:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173364.31 epoch sec; seqNo:6189; 4057660.43 north; 591393.08 east 2021-02-12T23:42:44Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-12T23:42:44Z- waiting for 1613173362.48, time is 1613173364.31 2021-02-12T23:42:44Z- LcmTrn::updateTrn() equating pose 1613173364.31 and meas times 1613173364.31 2021-02-12T23:42:44Z- LcmTrn::updateTrn() >>>> pose time 1613173364.31, meas time 1613173364.31 <<<< 2021-02-12T23:42:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173364.31 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:42:44Z- TerrainNav::motionUpdate() - currEstimate = (4057660, 591393.08, 11.07) 2021-02-12T23:42:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:42:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:42:44Z- TNavBF::Projecting Measurements 2021-02-12T23:42:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:42:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:42:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:42:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:42:44Z- TNavFilter::4 good beams. 2021-02-12T23:42:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:42:44Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:42:44Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:42:44Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:42:44Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:42:44Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:42:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:42:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:42:44Z- TerrainMapOctree: (vehN, vehE ) = (4057660.43, 591393.08). 2021-02-12T23:42:44Z- TerrainMapOctree: Min Distance = 4587.70. 2021-02-12T23:42:44Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:42:44Z- There is no specific good beam that all particles have in common. 2021-02-12T23:42:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:42:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 12.40 2021-02-12T23:42:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:42:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 16.95 2021-02-12T23:42:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:42:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 21.19 2021-02-12T23:42:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:42:44Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 9.75 2021-02-12T23:42:44Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:42:44Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:42:44Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:42:44Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:42:44Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:42:44Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:42:44Z- Filter 0 No beams used in subcloud update 2021-02-12T23:42:44Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:42:44Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:42:44Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 742.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:42:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173364.31 sec. 2021-02-12T23:42:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:42:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:42:44Z- variances([0], [2], and psi_berg) = 742.439949 833.026339 0.000000 2021-02-12T23:42:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:42:44Z- LcmTrn::publishEstimates() - MLE : 4057649.93 591351.58 11.07 2021-02-12T23:42:44Z- LcmTrn::publishEstimates() - MMSE : 4057660.43 591393.08 11.07 2021-02-12T23:42:44Z- LcmTrn::publishEstimates() - COVAR: 742.44 833.03 0.88 0.00 2021-02-12T23:42:44Z- AHRS_M2 msg: 1613173364.67 epoch sec; seqNo:6138 2021-02-12T23:42:44Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-12T23:42:44Z- handleDepth()-Depth_Keller msg: 1613173364685 epoch sec; seqNo:6190; depth 10.96 2021-02-12T23:42:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173364.69 epoch sec; seqNo:6190; 4057660.43 north; 591393.08 east 2021-02-12T23:42:45Z- AHRS_M2 msg: 1613173365.09 epoch sec; seqNo:6139 2021-02-12T23:42:45Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-12T23:42:45Z- handleDepth()-Depth_Keller msg: 1613173365096 epoch sec; seqNo:6191; depth 10.86 2021-02-12T23:42:45Z- handleDvl() - RDI_Pathfinder msg: 1613173365.13 epoch sec; seqNo:2916 2021-02-12T23:42:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.78 , 114.95 , 79.80 2021-02-12T23:42:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.19 , 0.04 2021-02-12T23:42:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173365.16 epoch sec; seqNo:6191; 4057660.81 north; 591393.08 east 2021-02-12T23:42:45Z- AHRS_M2 msg: 1613173365.48 epoch sec; seqNo:6140 2021-02-12T23:42:45Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-12T23:42:45Z- handleDepth()-Depth_Keller msg: 1613173365488 epoch sec; seqNo:6192; depth 10.74 2021-02-12T23:42:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173365.51 epoch sec; seqNo:6192; 4057660.81 north; 591393.08 east 2021-02-12T23:42:45Z- AHRS_M2 msg: 1613173365.89 epoch sec; seqNo:6141 2021-02-12T23:42:45Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-12T23:42:45Z- handleDepth()-Depth_Keller msg: 1613173365892 epoch sec; seqNo:6193; depth 10.63 2021-02-12T23:42:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173365.90 epoch sec; seqNo:6193; 4057661.20 north; 591394.30 east 2021-02-12T23:42:46Z- AHRS_M2 msg: 1613173366.29 epoch sec; seqNo:6142 2021-02-12T23:42:46Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-12T23:42:46Z- handleDepth()-Depth_Keller msg: 1613173366296 epoch sec; seqNo:6194; depth 10.53 2021-02-12T23:42:46Z- handleDvl() - RDI_Pathfinder msg: 1613173366.31 epoch sec; seqNo:2917 2021-02-12T23:42:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 101.16 , 114.68 , 79.53 2021-02-12T23:42:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.10 , -0.13 2021-02-12T23:42:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173366.32 epoch sec; seqNo:6194; 4057661.58 north; 591394.29 east 2021-02-12T23:42:46Z- AHRS_M2 msg: 1613173366.70 epoch sec; seqNo:6143 2021-02-12T23:42:46Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-12T23:42:46Z- handleDepth()-Depth_Keller msg: 1613173366701 epoch sec; seqNo:6195; depth 10.39 2021-02-12T23:42:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173366.71 epoch sec; seqNo:6195; 4057661.58 north; 591394.29 east 2021-02-12T23:42:47Z- AHRS_M2 msg: 1613173367.10 epoch sec; seqNo:6144 2021-02-12T23:42:47Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:42:47Z- handleDepth()-Depth_Keller msg: 1613173367109 epoch sec; seqNo:6196; depth 10.28 2021-02-12T23:42:47Z- handleDvl() - RDI_Pathfinder msg: 1613173367.13 epoch sec; seqNo:2918 2021-02-12T23:42:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.78 , 119.00 , 79.80 2021-02-12T23:42:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.15 , -0.17 2021-02-12T23:42:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173367.15 epoch sec; seqNo:6196; 4057661.96 north; 591394.29 east 2021-02-12T23:42:47Z- AHRS_M2 msg: 1613173367.50 epoch sec; seqNo:6145 2021-02-12T23:42:47Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:42:47Z- handleDepth()-Depth_Keller msg: 1613173367516 epoch sec; seqNo:6197; depth 10.16 2021-02-12T23:42:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173367.55 epoch sec; seqNo:6197; 4057662.34 north; 591394.28 east 2021-02-12T23:42:47Z- AHRS_M2 msg: 1613173367.91 epoch sec; seqNo:6146 2021-02-12T23:42:47Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.18 psi 2021-02-12T23:42:47Z- handleDepth()-Depth_Keller msg: 1613173367917 epoch sec; seqNo:6198; depth 10.05 2021-02-12T23:42:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173367.93 epoch sec; seqNo:6198; 4057662.72 north; 591394.28 east 2021-02-12T23:42:48Z- AHRS_M2 msg: 1613173368.31 epoch sec; seqNo:6147 2021-02-12T23:42:48Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.17 psi 2021-02-12T23:42:48Z- handleDepth()-Depth_Keller msg: 1613173368318 epoch sec; seqNo:6199; depth 9.94 2021-02-12T23:42:48Z- handleDvl() - RDI_Pathfinder msg: 1613173368.33 epoch sec; seqNo:2919 2021-02-12T23:42:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.50 , 105.07 , 117.24 , 0.00 2021-02-12T23:42:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.30 , 0.00 2021-02-12T23:42:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173368.34 epoch sec; seqNo:6199; 4057663.09 north; 591394.28 east 2021-02-12T23:42:48Z- AHRS_M2 msg: 1613173368.71 epoch sec; seqNo:6148 2021-02-12T23:42:48Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.17 psi 2021-02-12T23:42:48Z- handleDepth()-Depth_Keller msg: 1613173368721 epoch sec; seqNo:6200; depth 9.82 2021-02-12T23:42:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173368.74 epoch sec; seqNo:6200; 4057663.49 north; 591395.49 east 2021-02-12T23:42:49Z- AHRS_M2 msg: 1613173369.13 epoch sec; seqNo:6149 2021-02-12T23:42:49Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-12T23:42:49Z- handleDepth()-Depth_Keller msg: 1613173369137 epoch sec; seqNo:6201; depth 9.71 2021-02-12T23:42:49Z- handleDvl() - RDI_Pathfinder msg: 1613173369.16 epoch sec; seqNo:2920 2021-02-12T23:42:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.92 , 103.79 , 120.15 , 80.00 2021-02-12T23:42:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.20 , 0.16 2021-02-12T23:42:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173369.17 epoch sec; seqNo:6201; 4057663.49 north; 591395.49 east 2021-02-12T23:42:49Z- AHRS_M2 msg: 1613173369.53 epoch sec; seqNo:6150 2021-02-12T23:42:49Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-12T23:42:49Z- handleDepth()-Depth_Keller msg: 1613173369532 epoch sec; seqNo:6202; depth 9.58 2021-02-12T23:42:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173369.54 epoch sec; seqNo:6202; 4057663.87 north; 591395.49 east 2021-02-12T23:42:49Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-12T23:42:49Z- waiting for 1613173367.31, time is 1613173369.54 2021-02-12T23:42:49Z- LcmTrn::updateTrn() >>>> pose time 1613173369.54, meas time 1613173369.16 <<<< 2021-02-12T23:42:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173369.16 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:42:49Z- TerrainNav::motionUpdate() - currEstimate = (4057664, 591395.49, 9.58) 2021-02-12T23:42:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:42:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:42:49Z- TNavBF::Projecting Measurements 2021-02-12T23:42:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:42:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:42:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:42:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:42:49Z- TNavFilter::4 good beams. 2021-02-12T23:42:49Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:42:49Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:42:49Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:42:49Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:42:49Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:42:49Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:42:49Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:42:49Z- TNavBF::mmse selected from filter -1 2021-02-12T23:42:49Z- TerrainMapOctree: (vehN, vehE ) = (4057663.61, 591395.31). 2021-02-12T23:42:49Z- TerrainMapOctree: Min Distance = 4583.82. 2021-02-12T23:42:49Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:42:49Z- There is no specific good beam that all particles have in common. 2021-02-12T23:42:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:42:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 12.65 2021-02-12T23:42:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:42:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 17.24 2021-02-12T23:42:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:42:49Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 23.10 2021-02-12T23:42:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:42:49Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 10.24 2021-02-12T23:42:49Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:42:49Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:42:49Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:42:49Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:42:49Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:42:49Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:42:49Z- Filter 0 No beams used in subcloud update 2021-02-12T23:42:49Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:42:49Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:42:49Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 742.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:42:49Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173369.16 sec. 2021-02-12T23:42:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:42:49Z- TNavBF::mmse selected from filter -1 2021-02-12T23:42:49Z- variances([0], [2], and psi_berg) = 742.328476 833.026244 0.000000 2021-02-12T23:42:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:42:49Z- LcmTrn::publishEstimates() - MLE : 4057653.37 591353.99 9.58 2021-02-12T23:42:49Z- LcmTrn::publishEstimates() - MMSE : 4057663.87 591395.49 9.58 2021-02-12T23:42:49Z- LcmTrn::publishEstimates() - COVAR: 742.33 833.03 0.87 0.00 2021-02-12T23:42:49Z- AHRS_M2 msg: 1613173369.93 epoch sec; seqNo:6151 2021-02-12T23:42:49Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-12T23:42:49Z- handleDepth()-Depth_Keller msg: 1613173369932 epoch sec; seqNo:6203; depth 9.47 2021-02-12T23:42:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173369.95 epoch sec; seqNo:6203; 4057664.24 north; 591395.49 east 2021-02-12T23:42:50Z- AHRS_M2 msg: 1613173370.33 epoch sec; seqNo:6152 2021-02-12T23:42:50Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-12T23:42:50Z- handleDepth()-Depth_Keller msg: 1613173370336 epoch sec; seqNo:6204; depth 9.35 2021-02-12T23:42:50Z- handleDvl() - RDI_Pathfinder msg: 1613173370.35 epoch sec; seqNo:2921 2021-02-12T23:42:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 103.93 , 120.15 , 0.00 2021-02-12T23:42:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.00 , 0.15 2021-02-12T23:42:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173370.36 epoch sec; seqNo:6204; 4057664.62 north; 591395.48 east 2021-02-12T23:42:50Z- AHRS_M2 msg: 1613173370.74 epoch sec; seqNo:6153 2021-02-12T23:42:50Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.13 psi 2021-02-12T23:42:50Z- handleDepth()-Depth_Keller msg: 1613173370742 epoch sec; seqNo:6205; depth 9.22 2021-02-12T23:42:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173370.75 epoch sec; seqNo:6205; 4057664.62 north; 591395.48 east 2021-02-12T23:42:51Z- AHRS_M2 msg: 1613173371.14 epoch sec; seqNo:6154 2021-02-12T23:42:51Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.12 psi 2021-02-12T23:42:51Z- handleDepth()-Depth_Keller msg: 1613173371148 epoch sec; seqNo:6206; depth 9.08 2021-02-12T23:42:51Z- handleDvl() - RDI_Pathfinder msg: 1613173371.16 epoch sec; seqNo:2922 2021-02-12T23:42:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.27 , 102.17 , 120.01 , 81.35 2021-02-12T23:42:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.10 , 0.09 2021-02-12T23:42:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173371.18 epoch sec; seqNo:6206; 4057665.00 north; 591395.48 east 2021-02-12T23:42:51Z- AHRS_M2 msg: 1613173371.54 epoch sec; seqNo:6155 2021-02-12T23:42:51Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.12 psi 2021-02-12T23:42:51Z- handleDepth()-Depth_Keller msg: 1613173371548 epoch sec; seqNo:6207; depth 8.96 2021-02-12T23:42:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173371.56 epoch sec; seqNo:6207; 4057665.39 north; 591396.69 east 2021-02-12T23:42:51Z- AHRS_M2 msg: 1613173371.95 epoch sec; seqNo:6156 2021-02-12T23:42:51Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.13 psi 2021-02-12T23:42:51Z- handleDepth()-Depth_Keller msg: 1613173371961 epoch sec; seqNo:6208; depth 8.84 2021-02-12T23:42:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173371.97 epoch sec; seqNo:6208; 4057665.39 north; 591396.69 east 2021-02-12T23:42:52Z- AHRS_M2 msg: 1613173372.35 epoch sec; seqNo:6157 2021-02-12T23:42:52Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.14 psi 2021-02-12T23:42:52Z- handleDepth()-Depth_Keller msg: 1613173372356 epoch sec; seqNo:6209; depth 8.72 2021-02-12T23:42:52Z- handleDvl() - RDI_Pathfinder msg: 1613173372.37 epoch sec; seqNo:2923 2021-02-12T23:42:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.17 , 101.22 , 121.50 , 80.95 2021-02-12T23:42:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.11 , 0.01 2021-02-12T23:42:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173372.38 epoch sec; seqNo:6209; 4057665.77 north; 591396.69 east 2021-02-12T23:42:52Z- AHRS_M2 msg: 1613173372.76 epoch sec; seqNo:6158 2021-02-12T23:42:52Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.15 psi 2021-02-12T23:42:52Z- handleDepth()-Depth_Keller msg: 1613173372768 epoch sec; seqNo:6210; depth 8.58 2021-02-12T23:42:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173372.78 epoch sec; seqNo:6210; 4057665.77 north; 591396.69 east 2021-02-12T23:42:53Z- AHRS_M2 msg: 1613173373.17 epoch sec; seqNo:6159 2021-02-12T23:42:53Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.17 psi 2021-02-12T23:42:53Z- handleDepth()-Depth_Keller msg: 1613173373170 epoch sec; seqNo:6211; depth 8.45 2021-02-12T23:42:53Z- handleDvl() - RDI_Pathfinder msg: 1613173373.18 epoch sec; seqNo:2924 2021-02-12T23:42:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:42:53Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:42:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173373.19 epoch sec; seqNo:6211; 4057666.15 north; 591396.69 east 2021-02-12T23:42:53Z- AHRS_M2 msg: 1613173373.56 epoch sec; seqNo:6160 2021-02-12T23:42:53Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.18 psi 2021-02-12T23:42:53Z- handleDepth()-Depth_Keller msg: 1613173373570 epoch sec; seqNo:6212; depth 8.33 2021-02-12T23:42:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173373.58 epoch sec; seqNo:6212; 4057666.15 north; 591396.69 east 2021-02-12T23:42:53Z- AHRS_M2 msg: 1613173373.97 epoch sec; seqNo:6161 2021-02-12T23:42:53Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.19 psi 2021-02-12T23:42:53Z- handleDepth()-Depth_Keller msg: 1613173373972 epoch sec; seqNo:6213; depth 8.19 2021-02-12T23:42:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173373.99 epoch sec; seqNo:6213; 4057666.54 north; 591397.90 east 2021-02-12T23:42:54Z- AHRS_M2 msg: 1613173374.37 epoch sec; seqNo:6162 2021-02-12T23:42:54Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.20 psi 2021-02-12T23:42:54Z- handleDepth()-Depth_Keller msg: 1613173374376 epoch sec; seqNo:6214; depth 8.05 2021-02-12T23:42:54Z- handleDvl() - RDI_Pathfinder msg: 1613173374.39 epoch sec; seqNo:2925 2021-02-12T23:42:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.32 , 107.65 , 121.04 , 82.37 2021-02-12T23:42:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.28 , -0.05 2021-02-12T23:42:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173374.42 epoch sec; seqNo:6214; 4057666.92 north; 591397.90 east 2021-02-12T23:42:54Z- AHRS_M2 msg: 1613173374.78 epoch sec; seqNo:6163 2021-02-12T23:42:54Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.18 psi 2021-02-12T23:42:54Z- handleDepth()-Depth_Keller msg: 1613173374780 epoch sec; seqNo:6215; depth 7.93 2021-02-12T23:42:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173374.79 epoch sec; seqNo:6215; 4057667.30 north; 591397.90 east 2021-02-12T23:42:54Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-12T23:42:54Z- waiting for 1613173372.54, time is 1613173374.79 2021-02-12T23:42:54Z- LcmTrn::updateTrn() >>>> pose time 1613173374.79, meas time 1613173374.39 <<<< 2021-02-12T23:42:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173374.39 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:42:54Z- TerrainNav::motionUpdate() - currEstimate = (4057667, 591397.90, 7.93) 2021-02-12T23:42:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:42:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:42:54Z- TNavBF::Projecting Measurements 2021-02-12T23:42:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:42:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:42:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:42:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:42:54Z- TNavFilter::4 good beams. 2021-02-12T23:42:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:42:54Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:42:54Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:42:54Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:42:54Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:42:54Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:42:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:42:54Z- TNavBF::mmse selected from filter -1 2021-02-12T23:42:54Z- TerrainMapOctree: (vehN, vehE ) = (4057667.04, 591397.71). 2021-02-12T23:42:54Z- TerrainMapOctree: Min Distance = 4579.65. 2021-02-12T23:42:54Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:42:54Z- There is no specific good beam that all particles have in common. 2021-02-12T23:42:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:42:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 12.48 2021-02-12T23:42:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:42:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 18.54 2021-02-12T23:42:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:42:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 23.44 2021-02-12T23:42:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:42:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 10.86 2021-02-12T23:42:54Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:42:54Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:42:54Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:42:54Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:42:54Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:42:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:42:54Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:42:54Z- Filter 0 No beams used in subcloud update 2021-02-12T23:42:54Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:42:54Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:42:54Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 742.2 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:42:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173374.39 sec. 2021-02-12T23:42:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:42:54Z- TNavBF::mmse selected from filter -1 2021-02-12T23:42:54Z- variances([0], [2], and psi_berg) = 742.216804 833.026150 0.000000 2021-02-12T23:42:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:42:54Z- LcmTrn::publishEstimates() - MLE : 4057656.80 591356.40 7.93 2021-02-12T23:42:54Z- LcmTrn::publishEstimates() - MMSE : 4057667.30 591397.90 7.93 2021-02-12T23:42:54Z- LcmTrn::publishEstimates() - COVAR: 742.22 833.03 0.87 0.00 2021-02-12T23:42:55Z- AHRS_M2 msg: 1613173375.20 epoch sec; seqNo:6164 2021-02-12T23:42:55Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.17 psi 2021-02-12T23:42:55Z- handleDepth()-Depth_Keller msg: 1613173375211 epoch sec; seqNo:6216; depth 7.79 2021-02-12T23:42:55Z- handleDvl() - RDI_Pathfinder msg: 1613173375.22 epoch sec; seqNo:2926 2021-02-12T23:42:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.92 , 107.85 , 121.36 , 82.16 2021-02-12T23:42:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.32 , 0.18 2021-02-12T23:42:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173375.24 epoch sec; seqNo:6216; 4057667.68 north; 591397.89 east 2021-02-12T23:42:55Z- AHRS_M2 msg: 1613173375.58 epoch sec; seqNo:6165 2021-02-12T23:42:55Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.17 psi 2021-02-12T23:42:55Z- handleDepth()-Depth_Keller msg: 1613173375598 epoch sec; seqNo:6217; depth 7.67 2021-02-12T23:42:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173375.62 epoch sec; seqNo:6217; 4057667.68 north; 591397.89 east 2021-02-12T23:42:55Z- AHRS_M2 msg: 1613173375.99 epoch sec; seqNo:6166 2021-02-12T23:42:55Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-12T23:42:55Z- handleDepth()-Depth_Keller msg: 1613173375992 epoch sec; seqNo:6218; depth 7.55 2021-02-12T23:42:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173376.01 epoch sec; seqNo:6218; 4057668.06 north; 591397.89 east 2021-02-12T23:42:56Z- AHRS_M2 msg: 1613173376.40 epoch sec; seqNo:6167 2021-02-12T23:42:56Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.14 psi 2021-02-12T23:42:56Z- handleDepth()-Depth_Keller msg: 1613173376406 epoch sec; seqNo:6219; depth 7.45 2021-02-12T23:42:56Z- handleDvl() - RDI_Pathfinder msg: 1613173376.42 epoch sec; seqNo:2927 2021-02-12T23:42:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.27 , 107.85 , 121.36 , 82.16 2021-02-12T23:42:56Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.06 , 0.29 2021-02-12T23:42:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173376.43 epoch sec; seqNo:6219; 4057668.44 north; 591397.88 east 2021-02-12T23:42:56Z- AHRS_M2 msg: 1613173376.80 epoch sec; seqNo:6168 2021-02-12T23:42:56Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.13 psi 2021-02-12T23:42:56Z- handleDepth()-Depth_Keller msg: 1613173376810 epoch sec; seqNo:6220; depth 7.33 2021-02-12T23:42:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173376.82 epoch sec; seqNo:6220; 4057668.83 north; 591399.10 east 2021-02-12T23:42:57Z- AHRS_M2 msg: 1613173377.21 epoch sec; seqNo:6169 2021-02-12T23:42:57Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.13 psi 2021-02-12T23:42:57Z- handleDepth()-Depth_Keller msg: 1613173377215 epoch sec; seqNo:6221; depth 7.25 2021-02-12T23:42:57Z- handleDvl() - RDI_Pathfinder msg: 1613173377.23 epoch sec; seqNo:2928 2021-02-12T23:42:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.56 , 106.43 , 119.94 , 83.45 2021-02-12T23:42:57Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.21 , 0.17 2021-02-12T23:42:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173377.24 epoch sec; seqNo:6221; 4057669.21 north; 591399.10 east 2021-02-12T23:42:57Z- AHRS_M2 msg: 1613173377.60 epoch sec; seqNo:6170 2021-02-12T23:42:57Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.13 psi 2021-02-12T23:42:57Z- handleDepth()-Depth_Keller msg: 1613173377611 epoch sec; seqNo:6222; depth 7.12 2021-02-12T23:42:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173377.63 epoch sec; seqNo:6222; 4057669.59 north; 591399.09 east 2021-02-12T23:42:58Z- AHRS_M2 msg: 1613173378.01 epoch sec; seqNo:6171 2021-02-12T23:42:58Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.12 psi 2021-02-12T23:42:58Z- handleDepth()-Depth_Keller msg: 1613173378012 epoch sec; seqNo:6223; depth 7.03 2021-02-12T23:42:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173378.03 epoch sec; seqNo:6223; 4057669.59 north; 591399.09 east 2021-02-12T23:42:58Z- AHRS_M2 msg: 1613173378.41 epoch sec; seqNo:6172 2021-02-12T23:42:58Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.12 psi 2021-02-12T23:42:58Z- handleDepth()-Depth_Keller msg: 1613173378416 epoch sec; seqNo:6224; depth 6.92 2021-02-12T23:42:58Z- handleDvl() - RDI_Pathfinder msg: 1613173378.43 epoch sec; seqNo:2929 2021-02-12T23:42:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.26 , 105.07 , 121.30 , 0.00 2021-02-12T23:42:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.34 , -0.06 2021-02-12T23:42:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173378.45 epoch sec; seqNo:6224; 4057669.98 north; 591400.31 east 2021-02-12T23:42:58Z- AHRS_M2 msg: 1613173378.82 epoch sec; seqNo:6173 2021-02-12T23:42:58Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.13 psi 2021-02-12T23:42:58Z- handleDepth()-Depth_Keller msg: 1613173378821 epoch sec; seqNo:6225; depth 6.82 2021-02-12T23:42:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173378.84 epoch sec; seqNo:6225; 4057669.98 north; 591400.31 east 2021-02-12T23:42:59Z- AHRS_M2 msg: 1613173379.24 epoch sec; seqNo:6174 2021-02-12T23:42:59Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.14 psi 2021-02-12T23:42:59Z- handleDepth()-Depth_Keller msg: 1613173379244 epoch sec; seqNo:6226; depth 6.71 2021-02-12T23:42:59Z- handleDvl() - RDI_Pathfinder msg: 1613173379.26 epoch sec; seqNo:2930 2021-02-12T23:42:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.50 , 104.94 , 122.78 , 84.12 2021-02-12T23:42:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.22 , -0.27 2021-02-12T23:42:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173379.27 epoch sec; seqNo:6226; 4057670.36 north; 591400.30 east 2021-02-12T23:42:59Z- AHRS_M2 msg: 1613173379.62 epoch sec; seqNo:6175 2021-02-12T23:42:59Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.16 psi 2021-02-12T23:42:59Z- handleDepth()-Depth_Keller msg: 1613173379631 epoch sec; seqNo:6227; depth 6.59 2021-02-12T23:42:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173379.66 epoch sec; seqNo:6227; 4057670.36 north; 591400.30 east 2021-02-12T23:42:59Z- LcmTrn::handleMessages() - handling 41 messages took 0.68 ms and 128 calls... 2021-02-12T23:42:59Z- waiting for 1613173377.79, time is 1613173379.66 2021-02-12T23:42:59Z- LcmTrn::updateTrn() >>>> pose time 1613173379.66, meas time 1613173379.26 <<<< 2021-02-12T23:42:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173379.26 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:42:59Z- TerrainNav::motionUpdate() - currEstimate = (4057670, 591400.30, 6.59) 2021-02-12T23:42:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:42:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:42:59Z- TNavBF::Projecting Measurements 2021-02-12T23:42:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:42:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:42:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:42:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:42:59Z- TNavFilter::4 good beams. 2021-02-12T23:42:59Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:42:59Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:42:59Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:42:59Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:42:59Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:42:59Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:42:59Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:42:59Z- TNavBF::mmse selected from filter -1 2021-02-12T23:42:59Z- TerrainMapOctree: (vehN, vehE ) = (4057670.10, 591400.10). 2021-02-12T23:42:59Z- TerrainMapOctree: Min Distance = 4575.76. 2021-02-12T23:42:59Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:42:59Z- There is no specific good beam that all particles have in common. 2021-02-12T23:42:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:42:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 15.21 2021-02-12T23:42:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:42:59Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 17.62 2021-02-12T23:42:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:42:59Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.035 ExpectedMeasDiff: nan currMeas.covariance: 24.12 2021-02-12T23:43:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:43:00Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 11.32 2021-02-12T23:43:00Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:43:00Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:43:00Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:43:00Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:43:00Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:43:00Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:43:00Z- Filter 0 No beams used in subcloud update 2021-02-12T23:43:00Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:43:00Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:43:00Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 742.1 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:43:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173379.26 sec. 2021-02-12T23:43:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:43:00Z- TNavBF::mmse selected from filter -1 2021-02-12T23:43:00Z- variances([0], [2], and psi_berg) = 742.108704 833.026059 0.000000 2021-02-12T23:43:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:43:00Z- LcmTrn::publishEstimates() - MLE : 4057659.86 591358.80 6.59 2021-02-12T23:43:00Z- LcmTrn::publishEstimates() - MMSE : 4057670.36 591400.30 6.59 2021-02-12T23:43:00Z- LcmTrn::publishEstimates() - COVAR: 742.11 833.03 0.86 0.00 2021-02-12T23:43:00Z- AHRS_M2 msg: 1613173380.03 epoch sec; seqNo:6176 2021-02-12T23:43:00Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.18 psi 2021-02-12T23:43:00Z- handleDepth()-Depth_Keller msg: 1613173380032 epoch sec; seqNo:6228; depth 6.48 2021-02-12T23:43:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173380.04 epoch sec; seqNo:6228; 4057670.74 north; 591400.30 east 2021-02-12T23:43:00Z- AHRS_M2 msg: 1613173380.43 epoch sec; seqNo:6177 2021-02-12T23:43:00Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.20 psi 2021-02-12T23:43:00Z- handleDepth()-Depth_Keller msg: 1613173380436 epoch sec; seqNo:6229; depth 6.35 2021-02-12T23:43:00Z- handleDvl() - RDI_Pathfinder msg: 1613173380.45 epoch sec; seqNo:2931 2021-02-12T23:43:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.48 , 108.32 , 124.68 , 83.04 2021-02-12T23:43:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.14 , -0.27 2021-02-12T23:43:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173380.46 epoch sec; seqNo:6229; 4057670.74 north; 591400.30 east 2021-02-12T23:43:00Z- AHRS_M2 msg: 1613173380.84 epoch sec; seqNo:6178 2021-02-12T23:43:00Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.21 psi 2021-02-12T23:43:00Z- handleDepth()-Depth_Keller msg: 1613173380842 epoch sec; seqNo:6230; depth 6.22 2021-02-12T23:43:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173380.85 epoch sec; seqNo:6230; 4057671.13 north; 591401.52 east 2021-02-12T23:43:01Z- AHRS_M2 msg: 1613173381.24 epoch sec; seqNo:6179 2021-02-12T23:43:01Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.20 psi 2021-02-12T23:43:01Z- handleDepth()-Depth_Keller msg: 1613173381245 epoch sec; seqNo:6231; depth 6.11 2021-02-12T23:43:01Z- handleDvl() - RDI_Pathfinder msg: 1613173381.27 epoch sec; seqNo:2932 2021-02-12T23:43:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.98 , 112.85 , 124.74 , 0.00 2021-02-12T23:43:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.30 , -0.04 2021-02-12T23:43:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173381.28 epoch sec; seqNo:6231; 4057671.51 north; 591401.51 east 2021-02-12T23:43:01Z- AHRS_M2 msg: 1613173381.65 epoch sec; seqNo:6180 2021-02-12T23:43:01Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.20 psi 2021-02-12T23:43:01Z- handleDepth()-Depth_Keller msg: 1613173381651 epoch sec; seqNo:6232; depth 5.97 2021-02-12T23:43:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173381.66 epoch sec; seqNo:6232; 4057671.89 north; 591401.51 east 2021-02-12T23:43:02Z- AHRS_M2 msg: 1613173382.05 epoch sec; seqNo:6181 2021-02-12T23:43:02Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.20 psi 2021-02-12T23:43:02Z- handleDepth()-Depth_Keller msg: 1613173382055 epoch sec; seqNo:6233; depth 5.84 2021-02-12T23:43:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173382.06 epoch sec; seqNo:6233; 4057671.89 north; 591401.51 east 2021-02-12T23:43:02Z- AHRS_M2 msg: 1613173382.45 epoch sec; seqNo:6182 2021-02-12T23:43:02Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.20 psi 2021-02-12T23:43:02Z- handleDepth()-Depth_Keller msg: 1613173382456 epoch sec; seqNo:6234; depth 5.73 2021-02-12T23:43:02Z- handleDvl() - RDI_Pathfinder msg: 1613173382.47 epoch sec; seqNo:2933 2021-02-12T23:43:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.29 , 112.11 , 125.49 , 83.85 2021-02-12T23:43:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.26 , 0.14 2021-02-12T23:43:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173382.48 epoch sec; seqNo:6234; 4057672.26 north; 591401.51 east 2021-02-12T23:43:02Z- AHRS_M2 msg: 1613173382.86 epoch sec; seqNo:6183 2021-02-12T23:43:02Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.20 psi 2021-02-12T23:43:02Z- handleDepth()-Depth_Keller msg: 1613173382865 epoch sec; seqNo:6235; depth 5.61 2021-02-12T23:43:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173382.88 epoch sec; seqNo:6235; 4057672.64 north; 591401.50 east 2021-02-12T23:43:03Z- AHRS_M2 msg: 1613173383.26 epoch sec; seqNo:6184 2021-02-12T23:43:03Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.21 psi 2021-02-12T23:43:03Z- handleDepth()-Depth_Keller msg: 1613173383267 epoch sec; seqNo:6236; depth 5.49 2021-02-12T23:43:03Z- handleDvl() - RDI_Pathfinder msg: 1613173383.28 epoch sec; seqNo:2934 2021-02-12T23:43:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.90 , 112.71 , 123.12 , 85.95 2021-02-12T23:43:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.17 , 0.23 2021-02-12T23:43:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173383.29 epoch sec; seqNo:6236; 4057673.02 north; 591401.50 east 2021-02-12T23:43:03Z- AHRS_M2 msg: 1613173383.66 epoch sec; seqNo:6185 2021-02-12T23:43:03Z- AHRS_M2 msg: 0.60 phi; 0.29 theta; -0.22 psi 2021-02-12T23:43:03Z- handleDepth()-Depth_Keller msg: 1613173383673 epoch sec; seqNo:6237; depth 5.34 2021-02-12T23:43:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173383.70 epoch sec; seqNo:6237; 4057673.40 north; 591401.49 east 2021-02-12T23:43:04Z- AHRS_M2 msg: 1613173384.07 epoch sec; seqNo:6186 2021-02-12T23:43:04Z- AHRS_M2 msg: 0.60 phi; 0.26 theta; -0.22 psi 2021-02-12T23:43:04Z- handleDepth()-Depth_Keller msg: 1613173384073 epoch sec; seqNo:6238; depth 5.20 2021-02-12T23:43:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173384.08 epoch sec; seqNo:6238; 4057673.78 north; 591401.49 east 2021-02-12T23:43:04Z- AHRS_M2 msg: 1613173384.49 epoch sec; seqNo:6187 2021-02-12T23:43:04Z- AHRS_M2 msg: 0.61 phi; 0.23 theta; -0.21 psi 2021-02-12T23:43:04Z- handleDepth()-Depth_Keller msg: 1613173384492 epoch sec; seqNo:6239; depth 5.08 2021-02-12T23:43:04Z- handleDvl() - RDI_Pathfinder msg: 1613173384.50 epoch sec; seqNo:2935 2021-02-12T23:43:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.34 , 112.64 , 120.08 , 85.88 2021-02-12T23:43:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.28 2021-02-12T23:43:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173384.53 epoch sec; seqNo:6239; 4057674.17 north; 591402.71 east 2021-02-12T23:43:04Z- AHRS_M2 msg: 1613173384.88 epoch sec; seqNo:6188 2021-02-12T23:43:04Z- AHRS_M2 msg: 0.62 phi; 0.19 theta; -0.19 psi 2021-02-12T23:43:04Z- handleDepth()-Depth_Keller msg: 1613173384882 epoch sec; seqNo:6240; depth 4.95 2021-02-12T23:43:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173384.89 epoch sec; seqNo:6240; 4057674.17 north; 591402.71 east 2021-02-12T23:43:05Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-12T23:43:05Z- waiting for 1613173382.66, time is 1613173384.89 2021-02-12T23:43:05Z- LcmTrn::updateTrn() >>>> pose time 1613173384.89, meas time 1613173384.50 <<<< 2021-02-12T23:43:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173384.50 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:43:05Z- TerrainNav::motionUpdate() - currEstimate = (4057674, 591402.71, 4.95) 2021-02-12T23:43:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:43:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:43:05Z- TNavBF::Projecting Measurements 2021-02-12T23:43:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:43:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:43:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:43:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:43:05Z- TNavFilter::4 good beams. 2021-02-12T23:43:05Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:43:05Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:43:05Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:43:05Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:43:05Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:43:05Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:43:05Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:43:05Z- TNavBF::mmse selected from filter -1 2021-02-12T23:43:05Z- TerrainMapOctree: (vehN, vehE ) = (4057673.89, 591402.53). 2021-02-12T23:43:05Z- TerrainMapOctree: Min Distance = 4571.29. 2021-02-12T23:43:05Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:43:05Z- There is no specific good beam that all particles have in common. 2021-02-12T23:43:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:43:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 13.06 2021-02-12T23:43:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:43:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 20.30 2021-02-12T23:43:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:43:05Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 23.07 2021-02-12T23:43:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:43:05Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 11.80 2021-02-12T23:43:05Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:43:05Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:43:05Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:43:05Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:43:05Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:43:05Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:43:05Z- Filter 0 No beams used in subcloud update 2021-02-12T23:43:05Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:43:05Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:43:05Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 742.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:43:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173384.50 sec. 2021-02-12T23:43:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:43:05Z- TNavBF::mmse selected from filter -1 2021-02-12T23:43:05Z- variances([0], [2], and psi_berg) = 741.993555 833.025962 0.000000 2021-02-12T23:43:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:43:05Z- LcmTrn::publishEstimates() - MLE : 4057663.67 591361.21 4.95 2021-02-12T23:43:05Z- LcmTrn::publishEstimates() - MMSE : 4057674.17 591402.71 4.95 2021-02-12T23:43:05Z- LcmTrn::publishEstimates() - COVAR: 741.99 833.03 0.88 0.00 2021-02-12T23:43:05Z- AHRS_M2 msg: 1613173385.28 epoch sec; seqNo:6189 2021-02-12T23:43:05Z- AHRS_M2 msg: 0.63 phi; 0.15 theta; -0.16 psi 2021-02-12T23:43:05Z- handleDepth()-Depth_Keller msg: 1613173385290 epoch sec; seqNo:6241; depth 4.87 2021-02-12T23:43:05Z- handleDvl() - RDI_Pathfinder msg: 1613173385.32 epoch sec; seqNo:2936 2021-02-12T23:43:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.91 , 108.32 , 112.78 , 0.00 2021-02-12T23:43:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.19 , 0.17 2021-02-12T23:43:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173385.33 epoch sec; seqNo:6241; 4057674.55 north; 591402.70 east 2021-02-12T23:43:05Z- AHRS_M2 msg: 1613173385.68 epoch sec; seqNo:6190 2021-02-12T23:43:05Z- AHRS_M2 msg: 0.64 phi; 0.10 theta; -0.14 psi 2021-02-12T23:43:05Z- handleDepth()-Depth_Keller msg: 1613173385693 epoch sec; seqNo:6242; depth 4.75 2021-02-12T23:43:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173385.70 epoch sec; seqNo:6242; 4057674.93 north; 591402.70 east 2021-02-12T23:43:06Z- AHRS_M2 msg: 1613173386.09 epoch sec; seqNo:6191 2021-02-12T23:43:06Z- AHRS_M2 msg: 0.66 phi; 0.06 theta; -0.11 psi 2021-02-12T23:43:06Z- handleDepth()-Depth_Keller msg: 1613173386093 epoch sec; seqNo:6243; depth 4.69 2021-02-12T23:43:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173386.10 epoch sec; seqNo:6243; 4057674.93 north; 591402.70 east 2021-02-12T23:43:06Z- AHRS_M2 msg: 1613173386.49 epoch sec; seqNo:6192 2021-02-12T23:43:06Z- AHRS_M2 msg: 0.67 phi; 0.01 theta; -0.11 psi 2021-02-12T23:43:06Z- handleDepth()-Depth_Keller msg: 1613173386501 epoch sec; seqNo:6244; depth 4.62 2021-02-12T23:43:06Z- handleDvl() - RDI_Pathfinder msg: 1613173386.51 epoch sec; seqNo:2937 2021-02-12T23:43:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.14 , 102.03 , 102.03 , 97.57 2021-02-12T23:43:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.20 , -0.09 2021-02-12T23:43:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173386.52 epoch sec; seqNo:6244; 4057675.32 north; 591403.91 east 2021-02-12T23:43:06Z- AHRS_M2 msg: 1613173386.89 epoch sec; seqNo:6193 2021-02-12T23:43:06Z- AHRS_M2 msg: 0.68 phi; -0.03 theta; -0.13 psi 2021-02-12T23:43:06Z- handleDepth()-Depth_Keller msg: 1613173386900 epoch sec; seqNo:6245; depth 4.54 2021-02-12T23:43:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173386.92 epoch sec; seqNo:6245; 4057675.32 north; 591403.91 east 2021-02-12T23:43:07Z- AHRS_M2 msg: 1613173387.32 epoch sec; seqNo:6194 2021-02-12T23:43:07Z- AHRS_M2 msg: 0.69 phi; -0.06 theta; -0.16 psi 2021-02-12T23:43:07Z- handleDepth()-Depth_Keller msg: 1613173387322 epoch sec; seqNo:6246; depth 4.48 2021-02-12T23:43:07Z- handleDvl() - RDI_Pathfinder msg: 1613173387.34 epoch sec; seqNo:2938 2021-02-12T23:43:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 99.94 , 97.24 , 97.24 2021-02-12T23:43:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , -0.24 2021-02-12T23:43:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173387.35 epoch sec; seqNo:6246; 4057675.70 north; 591403.91 east 2021-02-12T23:43:07Z- AHRS_M2 msg: 1613173387.70 epoch sec; seqNo:6195 2021-02-12T23:43:07Z- AHRS_M2 msg: 0.70 phi; -0.10 theta; -0.19 psi 2021-02-12T23:43:07Z- handleDepth()-Depth_Keller msg: 1613173387709 epoch sec; seqNo:6247; depth 4.45 2021-02-12T23:43:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173387.72 epoch sec; seqNo:6247; 4057676.08 north; 591403.91 east 2021-02-12T23:43:08Z- AHRS_M2 msg: 1613173388.11 epoch sec; seqNo:6196 2021-02-12T23:43:08Z- AHRS_M2 msg: 0.71 phi; -0.13 theta; -0.22 psi 2021-02-12T23:43:08Z- handleDepth()-Depth_Keller msg: 1613173388115 epoch sec; seqNo:6248; depth 4.42 2021-02-12T23:43:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173388.13 epoch sec; seqNo:6248; 4057676.46 north; 591403.90 east 2021-02-12T23:43:08Z- AHRS_M2 msg: 1613173388.51 epoch sec; seqNo:6197 2021-02-12T23:43:08Z- AHRS_M2 msg: 0.71 phi; -0.17 theta; -0.23 psi 2021-02-12T23:43:08Z- handleDepth()-Depth_Keller msg: 1613173388516 epoch sec; seqNo:6249; depth 4.41 2021-02-12T23:43:08Z- handleDvl() - RDI_Pathfinder msg: 1613173388.53 epoch sec; seqNo:2939 2021-02-12T23:43:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 108.05 , 91.83 , 103.99 2021-02-12T23:43:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.23 , -0.05 2021-02-12T23:43:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173388.54 epoch sec; seqNo:6249; 4057676.46 north; 591403.90 east 2021-02-12T23:43:08Z- AHRS_M2 msg: 1613173388.92 epoch sec; seqNo:6198 2021-02-12T23:43:08Z- AHRS_M2 msg: 0.71 phi; -0.20 theta; -0.23 psi 2021-02-12T23:43:08Z- handleDepth()-Depth_Keller msg: 1613173388920 epoch sec; seqNo:6250; depth 4.41 2021-02-12T23:43:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173388.93 epoch sec; seqNo:6250; 4057676.85 north; 591405.12 east 2021-02-12T23:43:09Z- AHRS_M2 msg: 1613173389.32 epoch sec; seqNo:6199 2021-02-12T23:43:09Z- AHRS_M2 msg: 0.70 phi; -0.22 theta; -0.24 psi 2021-02-12T23:43:09Z- handleDepth()-Depth_Keller msg: 1613173389324 epoch sec; seqNo:6251; depth 4.43 2021-02-12T23:43:09Z- handleDvl() - RDI_Pathfinder msg: 1613173389.34 epoch sec; seqNo:2940 2021-02-12T23:43:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 112.85 , 89.06 , 111.36 2021-02-12T23:43:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.15 , 0.19 2021-02-12T23:43:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173389.35 epoch sec; seqNo:6251; 4057677.23 north; 591405.12 east 2021-02-12T23:43:09Z- AHRS_M2 msg: 1613173389.73 epoch sec; seqNo:6200 2021-02-12T23:43:09Z- AHRS_M2 msg: 0.70 phi; -0.25 theta; -0.24 psi 2021-02-12T23:43:09Z- handleDepth()-Depth_Keller msg: 1613173389733 epoch sec; seqNo:6252; depth 4.46 2021-02-12T23:43:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173389.74 epoch sec; seqNo:6252; 4057677.23 north; 591405.12 east 2021-02-12T23:43:10Z- AHRS_M2 msg: 1613173390.13 epoch sec; seqNo:6201 2021-02-12T23:43:10Z- AHRS_M2 msg: 0.70 phi; -0.27 theta; -0.22 psi 2021-02-12T23:43:10Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-12T23:43:10Z- waiting for 1613173387.89, time is 1613173390.13 2021-02-12T23:43:10Z- LcmTrn::updateTrn() >>>> pose time 1613173390.13, meas time 1613173389.34 <<<< 2021-02-12T23:43:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173389.34 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:43:10Z- TerrainNav::motionUpdate() - currEstimate = (4057677, 591405.12, 4.46) 2021-02-12T23:43:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:43:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:43:10Z- TNavBF::Projecting Measurements 2021-02-12T23:43:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:43:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:43:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:43:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:43:10Z- TNavFilter::4 good beams. 2021-02-12T23:43:10Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:43:10Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:43:10Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:43:10Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:43:10Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:43:10Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:43:10Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:43:10Z- TNavBF::mmse selected from filter -1 2021-02-12T23:43:10Z- TerrainMapOctree: (vehN, vehE ) = (4057676.76, 591404.75). 2021-02-12T23:43:10Z- TerrainMapOctree: Min Distance = 4567.65. 2021-02-12T23:43:10Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:43:10Z- There is no specific good beam that all particles have in common. 2021-02-12T23:43:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:43:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 12.27 2021-02-12T23:43:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:43:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 20.38 2021-02-12T23:43:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:43:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 12.69 2021-02-12T23:43:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:43:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 19.84 2021-02-12T23:43:10Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:43:10Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:43:10Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:43:10Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:43:10Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:43:10Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:43:10Z- Filter 0 No beams used in subcloud update 2021-02-12T23:43:10Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:43:10Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:43:10Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 741.9 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:43:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173389.34 sec. 2021-02-12T23:43:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:43:10Z- TNavBF::mmse selected from filter -1 2021-02-12T23:43:10Z- variances([0], [2], and psi_berg) = 741.880140 833.025867 0.000000 2021-02-12T23:43:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:43:10Z- LcmTrn::publishEstimates() - MLE : 4057666.73 591363.62 4.46 2021-02-12T23:43:10Z- LcmTrn::publishEstimates() - MMSE : 4057677.23 591405.12 4.46 2021-02-12T23:43:10Z- LcmTrn::publishEstimates() - COVAR: 741.88 833.03 0.87 0.00 2021-02-12T23:43:10Z- handleDepth()-Depth_Keller msg: 1613173390145 epoch sec; seqNo:6253; depth 4.51 2021-02-12T23:43:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173390.15 epoch sec; seqNo:6253; 4057677.61 north; 591405.11 east 2021-02-12T23:43:10Z- AHRS_M2 msg: 1613173390.53 epoch sec; seqNo:6202 2021-02-12T23:43:10Z- AHRS_M2 msg: 0.70 phi; -0.29 theta; -0.20 psi 2021-02-12T23:43:10Z- handleDepth()-Depth_Keller msg: 1613173390536 epoch sec; seqNo:6254; depth 4.58 2021-02-12T23:43:10Z- handleDvl() - RDI_Pathfinder msg: 1613173390.55 epoch sec; seqNo:2941 2021-02-12T23:43:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.03 , 113.25 , 0.00 , 117.31 2021-02-12T23:43:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.14 , 0.36 2021-02-12T23:43:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173390.57 epoch sec; seqNo:6254; 4057677.61 north; 591405.11 east 2021-02-12T23:43:10Z- AHRS_M2 msg: 1613173390.94 epoch sec; seqNo:6203 2021-02-12T23:43:10Z- AHRS_M2 msg: 0.69 phi; -0.31 theta; -0.18 psi 2021-02-12T23:43:10Z- handleDepth()-Depth_Keller msg: 1613173390947 epoch sec; seqNo:6255; depth 4.63 2021-02-12T23:43:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173390.96 epoch sec; seqNo:6255; 4057677.99 north; 591405.11 east 2021-02-12T23:43:11Z- AHRS_M2 msg: 1613173391.36 epoch sec; seqNo:6204 2021-02-12T23:43:11Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.19 psi 2021-02-12T23:43:11Z- handleDepth()-Depth_Keller msg: 1613173391365 epoch sec; seqNo:6256; depth 4.72 2021-02-12T23:43:11Z- handleDvl() - RDI_Pathfinder msg: 1613173391.38 epoch sec; seqNo:2942 2021-02-12T23:43:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.30 , 112.17 , 85.41 , 122.58 2021-02-12T23:43:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.30 , 0.06 2021-02-12T23:43:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173391.39 epoch sec; seqNo:6256; 4057678.00 north; 591406.33 east 2021-02-12T23:43:11Z- AHRS_M2 msg: 1613173391.74 epoch sec; seqNo:6205 2021-02-12T23:43:11Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.23 psi 2021-02-12T23:43:11Z- handleDepth()-Depth_Keller msg: 1613173391748 epoch sec; seqNo:6257; depth 4.79 2021-02-12T23:43:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173391.77 epoch sec; seqNo:6257; 4057678.38 north; 591406.32 east 2021-02-12T23:43:12Z- AHRS_M2 msg: 1613173392.15 epoch sec; seqNo:6206 2021-02-12T23:43:12Z- AHRS_M2 msg: 0.66 phi; -0.31 theta; -0.26 psi 2021-02-12T23:43:12Z- handleDepth()-Depth_Keller msg: 1613173392154 epoch sec; seqNo:6258; depth 4.90 2021-02-12T23:43:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173392.16 epoch sec; seqNo:6258; 4057678.38 north; 591406.32 east 2021-02-12T23:43:12Z- AHRS_M2 msg: 1613173392.55 epoch sec; seqNo:6207 2021-02-12T23:43:12Z- AHRS_M2 msg: 0.67 phi; -0.30 theta; -0.28 psi 2021-02-12T23:43:12Z- handleDepth()-Depth_Keller msg: 1613173392556 epoch sec; seqNo:6259; depth 5.00 2021-02-12T23:43:12Z- handleDvl() - RDI_Pathfinder msg: 1613173392.57 epoch sec; seqNo:2943 2021-02-12T23:43:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 120.01 , 0.00 , 125.96 2021-02-12T23:43:12Z- handleDvl() - RDI_Pathfinder msg: velocities 1.13 , 0.08 , -0.31 2021-02-12T23:43:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173392.58 epoch sec; seqNo:6259; 4057679.14 north; 591406.32 east 2021-02-12T23:43:12Z- AHRS_M2 msg: 1613173392.96 epoch sec; seqNo:6208 2021-02-12T23:43:12Z- AHRS_M2 msg: 0.68 phi; -0.30 theta; -0.29 psi 2021-02-12T23:43:12Z- handleDepth()-Depth_Keller msg: 1613173392962 epoch sec; seqNo:6260; depth 5.09 2021-02-12T23:43:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173392.98 epoch sec; seqNo:6260; 4057679.15 north; 591407.54 east 2021-02-12T23:43:13Z- AHRS_M2 msg: 1613173393.36 epoch sec; seqNo:6209 2021-02-12T23:43:13Z- AHRS_M2 msg: 0.69 phi; -0.30 theta; -0.27 psi 2021-02-12T23:43:13Z- handleDepth()-Depth_Keller msg: 1613173393369 epoch sec; seqNo:6261; depth 5.22 2021-02-12T23:43:13Z- handleDvl() - RDI_Pathfinder msg: 1613173393.39 epoch sec; seqNo:2944 2021-02-12T23:43:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.31 , 118.05 , 0.00 , 119.54 2021-02-12T23:43:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.24 , -0.26 2021-02-12T23:43:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173393.40 epoch sec; seqNo:6261; 4057679.53 north; 591407.53 east 2021-02-12T23:43:13Z- AHRS_M2 msg: 1613173393.77 epoch sec; seqNo:6210 2021-02-12T23:43:13Z- AHRS_M2 msg: 0.71 phi; -0.32 theta; -0.22 psi 2021-02-12T23:43:13Z- handleDepth()-Depth_Keller msg: 1613173393781 epoch sec; seqNo:6262; depth 5.33 2021-02-12T23:43:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173393.80 epoch sec; seqNo:6262; 4057679.91 north; 591407.53 east 2021-02-12T23:43:14Z- AHRS_M2 msg: 1613173394.17 epoch sec; seqNo:6211 2021-02-12T23:43:14Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.19 psi 2021-02-12T23:43:14Z- handleDepth()-Depth_Keller msg: 1613173394172 epoch sec; seqNo:6263; depth 5.44 2021-02-12T23:43:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173394.18 epoch sec; seqNo:6263; 4057680.29 north; 591407.52 east 2021-02-12T23:43:14Z- AHRS_M2 msg: 1613173394.57 epoch sec; seqNo:6212 2021-02-12T23:43:14Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.18 psi 2021-02-12T23:43:14Z- handleDepth()-Depth_Keller msg: 1613173394578 epoch sec; seqNo:6264; depth 5.55 2021-02-12T23:43:14Z- handleDvl() - RDI_Pathfinder msg: 1613173394.59 epoch sec; seqNo:2945 2021-02-12T23:43:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.37 , 115.21 , 0.00 , 122.65 2021-02-12T23:43:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , -0.25 , -0.04 2021-02-12T23:43:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173394.60 epoch sec; seqNo:6264; 4057680.66 north; 591407.52 east 2021-02-12T23:43:14Z- AHRS_M2 msg: 1613173394.98 epoch sec; seqNo:6213 2021-02-12T23:43:14Z- AHRS_M2 msg: 0.69 phi; -0.33 theta; -0.17 psi 2021-02-12T23:43:14Z- handleDepth()-Depth_Keller msg: 1613173394980 epoch sec; seqNo:6265; depth 5.66 2021-02-12T23:43:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173394.99 epoch sec; seqNo:6265; 4057681.04 north; 591407.52 east 2021-02-12T23:43:15Z- LcmTrn::handleMessages() - handling 43 messages took 0.68 ms and 130 calls... 2021-02-12T23:43:15Z- waiting for 1613173393.13, time is 1613173394.99 2021-02-12T23:43:15Z- LcmTrn::updateTrn() >>>> pose time 1613173394.99, meas time 1613173394.59 <<<< 2021-02-12T23:43:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173394.59 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:43:15Z- TerrainNav::motionUpdate() - currEstimate = (4057681, 591407.52, 5.66) 2021-02-12T23:43:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:43:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:43:15Z- TNavBF::Projecting Measurements 2021-02-12T23:43:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:43:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:43:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:43:15Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:43:15Z- TNavFilter::3 good beams. 2021-02-12T23:43:15Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:43:15Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:43:15Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:43:15Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:43:15Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:43:15Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:43:15Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:43:15Z- TNavBF::mmse selected from filter -1 2021-02-12T23:43:15Z- TerrainMapOctree: (vehN, vehE ) = (4057680.73, 591407.32). 2021-02-12T23:43:15Z- TerrainMapOctree: Min Distance = 4562.95. 2021-02-12T23:43:15Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:43:15Z- There is no specific good beam that all particles have in common. 2021-02-12T23:43:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:43:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.17 2021-02-12T23:43:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:43:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 21.24 2021-02-12T23:43:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:43:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 24.07 2021-02-12T23:43:15Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:43:15Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:43:15Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:43:15Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:43:15Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:43:15Z- Filter 0 No beams used in subcloud update 2021-02-12T23:43:15Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:43:15Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:43:15Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 741.8 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:43:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173394.59 sec. 2021-02-12T23:43:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:43:15Z- TNavBF::mmse selected from filter -1 2021-02-12T23:43:15Z- variances([0], [2], and psi_berg) = 741.764359 833.025770 0.000000 2021-02-12T23:43:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:43:15Z- LcmTrn::publishEstimates() - MLE : 4057670.54 591366.02 5.66 2021-02-12T23:43:15Z- LcmTrn::publishEstimates() - MMSE : 4057681.04 591407.52 5.66 2021-02-12T23:43:15Z- LcmTrn::publishEstimates() - COVAR: 741.76 833.03 0.96 0.00 2021-02-12T23:43:15Z- AHRS_M2 msg: 1613173395.40 epoch sec; seqNo:6214 2021-02-12T23:43:15Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.18 psi 2021-02-12T23:43:15Z- handleDepth()-Depth_Keller msg: 1613173395404 epoch sec; seqNo:6266; depth 5.76 2021-02-12T23:43:15Z- handleDvl() - RDI_Pathfinder msg: 1613173395.42 epoch sec; seqNo:2946 2021-02-12T23:43:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.29 , 109.13 , 83.85 , 122.51 2021-02-12T23:43:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.55 , -0.19 , 0.11 2021-02-12T23:43:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173395.43 epoch sec; seqNo:6266; 4057681.04 north; 591407.52 east 2021-02-12T23:43:15Z- AHRS_M2 msg: 1613173395.79 epoch sec; seqNo:6215 2021-02-12T23:43:15Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.20 psi 2021-02-12T23:43:15Z- handleDepth()-Depth_Keller msg: 1613173395790 epoch sec; seqNo:6267; depth 5.87 2021-02-12T23:43:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173395.80 epoch sec; seqNo:6267; 4057681.42 north; 591407.51 east 2021-02-12T23:43:16Z- AHRS_M2 msg: 1613173396.19 epoch sec; seqNo:6216 2021-02-12T23:43:16Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.21 psi 2021-02-12T23:43:16Z- handleDepth()-Depth_Keller msg: 1613173396192 epoch sec; seqNo:6268; depth 5.96 2021-02-12T23:43:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173396.21 epoch sec; seqNo:6268; 4057681.42 north; 591407.51 east 2021-02-12T23:43:16Z- AHRS_M2 msg: 1613173396.60 epoch sec; seqNo:6217 2021-02-12T23:43:16Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.23 psi 2021-02-12T23:43:16Z- handleDepth()-Depth_Keller msg: 1613173396601 epoch sec; seqNo:6269; depth 6.07 2021-02-12T23:43:16Z- handleDvl() - RDI_Pathfinder msg: 1613173396.62 epoch sec; seqNo:2947 2021-02-12T23:43:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.98 , 111.36 , 0.00 , 124.74 2021-02-12T23:43:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.63 , -0.14 , 0.23 2021-02-12T23:43:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173396.63 epoch sec; seqNo:6269; 4057681.80 north; 591407.51 east 2021-02-12T23:43:16Z- AHRS_M2 msg: 1613173397.00 epoch sec; seqNo:6218 2021-02-12T23:43:16Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.23 psi 2021-02-12T23:43:17Z- handleDepth()-Depth_Keller msg: 1613173397000 epoch sec; seqNo:6270; depth 6.17 2021-02-12T23:43:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173397.01 epoch sec; seqNo:6270; 4057681.80 north; 591407.51 east 2021-02-12T23:43:17Z- AHRS_M2 msg: 1613173397.41 epoch sec; seqNo:6219 2021-02-12T23:43:17Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.23 psi 2021-02-12T23:43:17Z- handleDepth()-Depth_Keller msg: 1613173397441 epoch sec; seqNo:6271; depth 6.28 2021-02-12T23:43:17Z- handleDvl() - RDI_Pathfinder msg: 1613173397.46 epoch sec; seqNo:2948 2021-02-12T23:43:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 113.59 , 83.85 , 128.46 2021-02-12T23:43:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , 0.30 2021-02-12T23:43:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173397.47 epoch sec; seqNo:6271; 4057682.19 north; 591408.73 east 2021-02-12T23:43:17Z- AHRS_M2 msg: 1613173397.80 epoch sec; seqNo:6220 2021-02-12T23:43:17Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.23 psi 2021-02-12T23:43:17Z- handleDepth()-Depth_Keller msg: 1613173397808 epoch sec; seqNo:6272; depth 6.40 2021-02-12T23:43:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173397.83 epoch sec; seqNo:6272; 4057682.19 north; 591408.73 east 2021-02-12T23:43:18Z- AHRS_M2 msg: 1613173398.21 epoch sec; seqNo:6221 2021-02-12T23:43:18Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.23 psi 2021-02-12T23:43:18Z- handleDepth()-Depth_Keller msg: 1613173398212 epoch sec; seqNo:6273; depth 6.51 2021-02-12T23:43:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173398.22 epoch sec; seqNo:6273; 4057682.57 north; 591408.72 east 2021-02-12T23:43:18Z- AHRS_M2 msg: 1613173398.61 epoch sec; seqNo:6222 2021-02-12T23:43:18Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.23 psi 2021-02-12T23:43:18Z- handleDepth()-Depth_Keller msg: 1613173398616 epoch sec; seqNo:6274; depth 6.62 2021-02-12T23:43:18Z- handleDvl() - RDI_Pathfinder msg: 1613173398.63 epoch sec; seqNo:2949 2021-02-12T23:43:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.57 , 113.93 , 84.19 , 128.80 2021-02-12T23:43:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.15 , 0.15 2021-02-12T23:43:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173398.64 epoch sec; seqNo:6274; 4057682.57 north; 591408.72 east 2021-02-12T23:43:19Z- AHRS_M2 msg: 1613173399.02 epoch sec; seqNo:6223 2021-02-12T23:43:19Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.23 psi 2021-02-12T23:43:19Z- handleDepth()-Depth_Keller msg: 1613173399022 epoch sec; seqNo:6275; depth 6.74 2021-02-12T23:43:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173399.04 epoch sec; seqNo:6275; 4057682.95 north; 591408.72 east 2021-02-12T23:43:19Z- AHRS_M2 msg: 1613173399.42 epoch sec; seqNo:6224 2021-02-12T23:43:19Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.23 psi 2021-02-12T23:43:19Z- handleDepth()-Depth_Keller msg: 1613173399434 epoch sec; seqNo:6276; depth 6.86 2021-02-12T23:43:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173399.44 epoch sec; seqNo:6276; 4057683.34 north; 591409.93 east 2021-02-12T23:43:19Z- AHRS_M2 msg: 1613173399.82 epoch sec; seqNo:6225 2021-02-12T23:43:19Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.23 psi 2021-02-12T23:43:19Z- handleDepth()-Depth_Keller msg: 1613173399829 epoch sec; seqNo:6277; depth 7.01 2021-02-12T23:43:19Z- handleDvl() - RDI_Pathfinder msg: 1613173399.86 epoch sec; seqNo:2950 2021-02-12T23:43:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.14 , 113.93 , 84.19 , 122.85 2021-02-12T23:43:19Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , 0.08 , -0.09 2021-02-12T23:43:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173399.88 epoch sec; seqNo:6277; 4057683.72 north; 591409.93 east 2021-02-12T23:43:20Z- AHRS_M2 msg: 1613173400.23 epoch sec; seqNo:6226 2021-02-12T23:43:20Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.22 psi 2021-02-12T23:43:20Z- handleDepth()-Depth_Keller msg: 1613173400235 epoch sec; seqNo:6278; depth 7.15 2021-02-12T23:43:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173400.25 epoch sec; seqNo:6278; 4057683.72 north; 591409.93 east 2021-02-12T23:43:20Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-12T23:43:20Z- waiting for 1613173397.99, time is 1613173400.25 2021-02-12T23:43:20Z- LcmTrn::updateTrn() >>>> pose time 1613173400.25, meas time 1613173399.86 <<<< 2021-02-12T23:43:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173399.86 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:43:20Z- TerrainNav::motionUpdate() - currEstimate = (4057684, 591409.93, 7.15) 2021-02-12T23:43:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:43:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:43:20Z- TNavBF::Projecting Measurements 2021-02-12T23:43:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:43:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:43:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:43:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:43:20Z- TNavFilter::4 good beams. 2021-02-12T23:43:20Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:43:20Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:43:20Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:43:20Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:43:20Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:43:20Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:43:20Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:43:20Z- TNavBF::mmse selected from filter -1 2021-02-12T23:43:20Z- TerrainMapOctree: (vehN, vehE ) = (4057683.52, 591409.75). 2021-02-12T23:43:20Z- TerrainMapOctree: Min Distance = 4559.25. 2021-02-12T23:43:20Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:43:20Z- There is no specific good beam that all particles have in common. 2021-02-12T23:43:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:43:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 13.00 2021-02-12T23:43:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:43:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 20.77 2021-02-12T23:43:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:43:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 11.34 2021-02-12T23:43:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:43:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.035 ExpectedMeasDiff: nan currMeas.covariance: 24.15 2021-02-12T23:43:20Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:43:20Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:43:20Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:43:20Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:43:20Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:43:20Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:43:20Z- Filter 0 No beams used in subcloud update 2021-02-12T23:43:20Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:43:20Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:43:20Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 741.7 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:43:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173399.86 sec. 2021-02-12T23:43:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:43:20Z- TNavBF::mmse selected from filter -1 2021-02-12T23:43:20Z- variances([0], [2], and psi_berg) = 741.661900 833.025684 0.000000 2021-02-12T23:43:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:43:20Z- LcmTrn::publishEstimates() - MLE : 4057673.22 591368.43 7.15 2021-02-12T23:43:20Z- LcmTrn::publishEstimates() - MMSE : 4057683.72 591409.93 7.15 2021-02-12T23:43:20Z- LcmTrn::publishEstimates() - COVAR: 741.66 833.03 0.91 0.00 2021-02-12T23:43:20Z- AHRS_M2 msg: 1613173400.63 epoch sec; seqNo:6227 2021-02-12T23:43:20Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.21 psi 2021-02-12T23:43:20Z- handleDepth()-Depth_Keller msg: 1613173400636 epoch sec; seqNo:6279; depth 7.25 2021-02-12T23:43:20Z- handleDvl() - RDI_Pathfinder msg: 1613173400.65 epoch sec; seqNo:2951 2021-02-12T23:43:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.84 , 114.20 , 84.46 , 121.64 2021-02-12T23:43:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.08 , -0.17 2021-02-12T23:43:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173400.66 epoch sec; seqNo:6279; 4057684.10 north; 591409.93 east 2021-02-12T23:43:21Z- AHRS_M2 msg: 1613173401.04 epoch sec; seqNo:6228 2021-02-12T23:43:21Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.20 psi 2021-02-12T23:43:21Z- handleDepth()-Depth_Keller msg: 1613173401042 epoch sec; seqNo:6280; depth 7.39 2021-02-12T23:43:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173401.05 epoch sec; seqNo:6280; 4057684.48 north; 591409.92 east 2021-02-12T23:43:21Z- AHRS_M2 msg: 1613173401.44 epoch sec; seqNo:6229 2021-02-12T23:43:21Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-12T23:43:21Z- handleDepth()-Depth_Keller msg: 1613173401450 epoch sec; seqNo:6281; depth 7.50 2021-02-12T23:43:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173401.47 epoch sec; seqNo:6281; 4057684.86 north; 591409.92 east 2021-02-12T23:43:21Z- AHRS_M2 msg: 1613173401.84 epoch sec; seqNo:6230 2021-02-12T23:43:21Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-12T23:43:21Z- handleDepth()-Depth_Keller msg: 1613173401850 epoch sec; seqNo:6282; depth 7.61 2021-02-12T23:43:21Z- handleDvl() - RDI_Pathfinder msg: 1613173401.86 epoch sec; seqNo:2952 2021-02-12T23:43:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 109.60 , 82.84 , 120.01 2021-02-12T23:43:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.07 , -0.11 2021-02-12T23:43:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173401.87 epoch sec; seqNo:6282; 4057685.25 north; 591411.14 east 2021-02-12T23:43:22Z- AHRS_M2 msg: 1613173402.25 epoch sec; seqNo:6231 2021-02-12T23:43:22Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.18 psi 2021-02-12T23:43:22Z- handleDepth()-Depth_Keller msg: 1613173402257 epoch sec; seqNo:6283; depth 7.73 2021-02-12T23:43:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173402.27 epoch sec; seqNo:6283; 4057685.63 north; 591411.13 east 2021-02-12T23:43:22Z- AHRS_M2 msg: 1613173402.65 epoch sec; seqNo:6232 2021-02-12T23:43:22Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.19 psi 2021-02-12T23:43:22Z- handleDepth()-Depth_Keller msg: 1613173402656 epoch sec; seqNo:6284; depth 7.85 2021-02-12T23:43:22Z- handleDvl() - RDI_Pathfinder msg: 1613173402.67 epoch sec; seqNo:2953 2021-02-12T23:43:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.48 , 108.32 , 81.55 , 118.73 2021-02-12T23:43:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.01 , 0.04 2021-02-12T23:43:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173402.69 epoch sec; seqNo:6284; 4057685.63 north; 591411.13 east 2021-02-12T23:43:23Z- AHRS_M2 msg: 1613173403.06 epoch sec; seqNo:6233 2021-02-12T23:43:23Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.19 psi 2021-02-12T23:43:23Z- handleDepth()-Depth_Keller msg: 1613173403065 epoch sec; seqNo:6285; depth 7.96 2021-02-12T23:43:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173403.08 epoch sec; seqNo:6285; 4057686.01 north; 591411.13 east 2021-02-12T23:43:23Z- AHRS_M2 msg: 1613173403.46 epoch sec; seqNo:6234 2021-02-12T23:43:23Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-12T23:43:23Z- handleDepth()-Depth_Keller msg: 1613173403473 epoch sec; seqNo:6286; depth 8.09 2021-02-12T23:43:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173403.49 epoch sec; seqNo:6286; 4057686.01 north; 591411.13 east 2021-02-12T23:43:23Z- AHRS_M2 msg: 1613173403.86 epoch sec; seqNo:6235 2021-02-12T23:43:23Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.20 psi 2021-02-12T23:43:23Z- handleDepth()-Depth_Keller msg: 1613173403868 epoch sec; seqNo:6287; depth 8.20 2021-02-12T23:43:23Z- handleDvl() - RDI_Pathfinder msg: 1613173403.89 epoch sec; seqNo:2954 2021-02-12T23:43:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.03 , 109.20 , 82.16 , 124.07 2021-02-12T23:43:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.10 , 0.14 2021-02-12T23:43:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173403.90 epoch sec; seqNo:6287; 4057686.39 north; 591411.12 east 2021-02-12T23:43:24Z- AHRS_M2 msg: 1613173404.27 epoch sec; seqNo:6236 2021-02-12T23:43:24Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-12T23:43:24Z- handleDepth()-Depth_Keller msg: 1613173404272 epoch sec; seqNo:6288; depth 8.32 2021-02-12T23:43:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173404.28 epoch sec; seqNo:6288; 4057686.76 north; 591411.12 east 2021-02-12T23:43:24Z- AHRS_M2 msg: 1613173404.67 epoch sec; seqNo:6237 2021-02-12T23:43:24Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-12T23:43:24Z- handleDepth()-Depth_Keller msg: 1613173404678 epoch sec; seqNo:6289; depth 8.46 2021-02-12T23:43:24Z- handleDvl() - RDI_Pathfinder msg: 1613173404.69 epoch sec; seqNo:2955 2021-02-12T23:43:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.92 , 109.74 , 0.00 , 127.58 2021-02-12T23:43:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.15 , -0.04 2021-02-12T23:43:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173404.70 epoch sec; seqNo:6289; 4057686.78 north; 591412.34 east 2021-02-12T23:43:25Z- AHRS_M2 msg: 1613173405.08 epoch sec; seqNo:6238 2021-02-12T23:43:25Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.21 psi 2021-02-12T23:43:25Z- handleDepth()-Depth_Keller msg: 1613173405081 epoch sec; seqNo:6290; depth 8.59 2021-02-12T23:43:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173405.10 epoch sec; seqNo:6290; 4057687.16 north; 591412.34 east 2021-02-12T23:43:25Z- LcmTrn::handleMessages() - handling 41 messages took 0.67 ms and 128 calls... 2021-02-12T23:43:25Z- waiting for 1613173403.25, time is 1613173405.10 2021-02-12T23:43:25Z- LcmTrn::updateTrn() >>>> pose time 1613173405.10, meas time 1613173404.69 <<<< 2021-02-12T23:43:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173404.69 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:43:25Z- TerrainNav::motionUpdate() - currEstimate = (4057687, 591412.34, 8.59) 2021-02-12T23:43:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:43:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:43:25Z- TNavBF::Projecting Measurements 2021-02-12T23:43:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:43:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:43:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:43:25Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:43:25Z- TNavFilter::3 good beams. 2021-02-12T23:43:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:43:25Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:43:25Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:43:25Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:43:25Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:43:25Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:43:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:43:25Z- TNavBF::mmse selected from filter -1 2021-02-12T23:43:25Z- TerrainMapOctree: (vehN, vehE ) = (4057686.87, 591412.13). 2021-02-12T23:43:25Z- TerrainMapOctree: Min Distance = 4555.15. 2021-02-12T23:43:25Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:43:25Z- There is no specific good beam that all particles have in common. 2021-02-12T23:43:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:43:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 12.65 2021-02-12T23:43:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:43:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 19.27 2021-02-12T23:43:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:43:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.034 ExpectedMeasDiff: nan currMeas.covariance: 26.04 2021-02-12T23:43:25Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:43:25Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:43:25Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:43:25Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:43:25Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:43:25Z- Filter 0 No beams used in subcloud update 2021-02-12T23:43:25Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:43:25Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:43:25Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 741.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:43:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173404.69 sec. 2021-02-12T23:43:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:43:25Z- TNavBF::mmse selected from filter -1 2021-02-12T23:43:25Z- variances([0], [2], and psi_berg) = 741.550291 833.025591 0.000000 2021-02-12T23:43:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:43:25Z- LcmTrn::publishEstimates() - MLE : 4057676.66 591370.84 8.59 2021-02-12T23:43:25Z- LcmTrn::publishEstimates() - MMSE : 4057687.16 591412.34 8.59 2021-02-12T23:43:25Z- LcmTrn::publishEstimates() - COVAR: 741.55 833.03 0.97 0.00 2021-02-12T23:43:25Z- AHRS_M2 msg: 1613173405.48 epoch sec; seqNo:6239 2021-02-12T23:43:25Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.22 psi 2021-02-12T23:43:25Z- handleDepth()-Depth_Keller msg: 1613173405484 epoch sec; seqNo:6291; depth 8.74 2021-02-12T23:43:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173405.49 epoch sec; seqNo:6291; 4057687.54 north; 591412.33 east 2021-02-12T23:43:25Z- AHRS_M2 msg: 1613173405.88 epoch sec; seqNo:6240 2021-02-12T23:43:25Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.23 psi 2021-02-12T23:43:25Z- handleDepth()-Depth_Keller msg: 1613173405888 epoch sec; seqNo:6292; depth 8.88 2021-02-12T23:43:25Z- handleDvl() - RDI_Pathfinder msg: 1613173405.90 epoch sec; seqNo:2956 2021-02-12T23:43:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.37 , 110.75 , 81.01 , 124.14 2021-02-12T23:43:25Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , -0.07 , -0.28 2021-02-12T23:43:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173405.91 epoch sec; seqNo:6292; 4057687.91 north; 591412.33 east 2021-02-12T23:43:26Z- AHRS_M2 msg: 1613173406.29 epoch sec; seqNo:6241 2021-02-12T23:43:26Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.24 psi 2021-02-12T23:43:26Z- handleDepth()-Depth_Keller msg: 1613173406294 epoch sec; seqNo:6293; depth 9.02 2021-02-12T23:43:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173406.31 epoch sec; seqNo:6293; 4057688.29 north; 591412.32 east 2021-02-12T23:43:26Z- AHRS_M2 msg: 1613173406.69 epoch sec; seqNo:6242 2021-02-12T23:43:26Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.23 psi 2021-02-12T23:43:26Z- handleDepth()-Depth_Keller msg: 1613173406696 epoch sec; seqNo:6294; depth 9.14 2021-02-12T23:43:26Z- handleDvl() - RDI_Pathfinder msg: 1613173406.71 epoch sec; seqNo:2957 2021-02-12T23:43:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.95 , 112.71 , 81.49 , 121.64 2021-02-12T23:43:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.32 , -0.24 2021-02-12T23:43:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173406.73 epoch sec; seqNo:6294; 4057688.69 north; 591413.54 east 2021-02-12T23:43:27Z- AHRS_M2 msg: 1613173407.10 epoch sec; seqNo:6243 2021-02-12T23:43:27Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.22 psi 2021-02-12T23:43:27Z- handleDepth()-Depth_Keller msg: 1613173407103 epoch sec; seqNo:6295; depth 9.28 2021-02-12T23:43:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173407.12 epoch sec; seqNo:6295; 4057689.06 north; 591413.54 east 2021-02-12T23:43:27Z- AHRS_M2 msg: 1613173407.50 epoch sec; seqNo:6244 2021-02-12T23:43:27Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.20 psi 2021-02-12T23:43:27Z- handleDepth()-Depth_Keller msg: 1613173407505 epoch sec; seqNo:6296; depth 9.41 2021-02-12T23:43:27Z- handleDvl() - RDI_Pathfinder msg: 1613173407.52 epoch sec; seqNo:2958 2021-02-12T23:43:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 110.48 , 80.74 , 121.30 2021-02-12T23:43:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.31 , 0.04 2021-02-12T23:43:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173407.53 epoch sec; seqNo:6296; 4057689.44 north; 591413.53 east 2021-02-12T23:43:27Z- AHRS_M2 msg: 1613173407.90 epoch sec; seqNo:6245 2021-02-12T23:43:27Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.18 psi 2021-02-12T23:43:27Z- handleDepth()-Depth_Keller msg: 1613173407910 epoch sec; seqNo:6297; depth 9.56 2021-02-12T23:43:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173407.92 epoch sec; seqNo:6297; 4057689.82 north; 591413.53 east 2021-02-12T23:43:28Z- AHRS_M2 msg: 1613173408.31 epoch sec; seqNo:6246 2021-02-12T23:43:28Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.16 psi 2021-02-12T23:43:28Z- handleDepth()-Depth_Keller msg: 1613173408312 epoch sec; seqNo:6298; depth 9.69 2021-02-12T23:43:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173408.32 epoch sec; seqNo:6298; 4057689.82 north; 591413.53 east 2021-02-12T23:43:28Z- AHRS_M2 msg: 1613173408.71 epoch sec; seqNo:6247 2021-02-12T23:43:28Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.15 psi 2021-02-12T23:43:28Z- handleDepth()-Depth_Keller msg: 1613173408716 epoch sec; seqNo:6299; depth 9.81 2021-02-12T23:43:28Z- handleDvl() - RDI_Pathfinder msg: 1613173408.73 epoch sec; seqNo:2959 2021-02-12T23:43:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 106.70 , 79.93 , 121.57 2021-02-12T23:43:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.12 , 0.20 2021-02-12T23:43:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173408.74 epoch sec; seqNo:6299; 4057690.20 north; 591413.52 east 2021-02-12T23:43:29Z- AHRS_M2 msg: 1613173409.13 epoch sec; seqNo:6248 2021-02-12T23:43:29Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.15 psi 2021-02-12T23:43:29Z- handleDepth()-Depth_Keller msg: 1613173409134 epoch sec; seqNo:6300; depth 9.92 2021-02-12T23:43:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173409.15 epoch sec; seqNo:6300; 4057690.58 north; 591413.52 east 2021-02-12T23:43:29Z- AHRS_M2 msg: 1613173409.53 epoch sec; seqNo:6249 2021-02-12T23:43:29Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.15 psi 2021-02-12T23:43:29Z- handleDepth()-Depth_Keller msg: 1613173409535 epoch sec; seqNo:6301; depth 10.06 2021-02-12T23:43:29Z- handleDvl() - RDI_Pathfinder msg: 1613173409.56 epoch sec; seqNo:2960 2021-02-12T23:43:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 105.41 , 80.14 , 124.74 2021-02-12T23:43:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.03 , 0.14 2021-02-12T23:43:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173409.57 epoch sec; seqNo:6301; 4057690.58 north; 591413.52 east 2021-02-12T23:43:29Z- AHRS_M2 msg: 1613173409.92 epoch sec; seqNo:6250 2021-02-12T23:43:29Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.15 psi 2021-02-12T23:43:29Z- handleDepth()-Depth_Keller msg: 1613173409928 epoch sec; seqNo:6302; depth 10.21 2021-02-12T23:43:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173409.94 epoch sec; seqNo:6302; 4057690.96 north; 591413.52 east 2021-02-12T23:43:30Z- AHRS_M2 msg: 1613173410.33 epoch sec; seqNo:6251 2021-02-12T23:43:30Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.15 psi 2021-02-12T23:43:30Z- handleDepth()-Depth_Keller msg: 1613173410340 epoch sec; seqNo:6303; depth 10.34 2021-02-12T23:43:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173410.35 epoch sec; seqNo:6303; 4057690.96 north; 591413.52 east 2021-02-12T23:43:30Z- LcmTrn::handleMessages() - handling 44 messages took 0.63 ms and 132 calls... 2021-02-12T23:43:30Z- waiting for 1613173408.10, time is 1613173410.35 2021-02-12T23:43:30Z- LcmTrn::updateTrn() >>>> pose time 1613173410.35, meas time 1613173409.56 <<<< 2021-02-12T23:43:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173409.56 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:43:30Z- TerrainNav::motionUpdate() - currEstimate = (4057691, 591413.52, 10.34) 2021-02-12T23:43:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:43:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:43:30Z- TNavBF::Projecting Measurements 2021-02-12T23:43:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:43:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:43:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:43:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:43:30Z- TNavFilter::4 good beams. 2021-02-12T23:43:30Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:43:30Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:43:30Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:43:30Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:43:30Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:43:30Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:43:30Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:43:30Z- TNavBF::mmse selected from filter -1 2021-02-12T23:43:30Z- TerrainMapOctree: (vehN, vehE ) = (4057690.39, 591413.34). 2021-02-12T23:43:30Z- TerrainMapOctree: Min Distance = 4551.65. 2021-02-12T23:43:30Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:43:30Z- There is no specific good beam that all particles have in common. 2021-02-12T23:43:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:43:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 12.69 2021-02-12T23:43:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:43:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 17.78 2021-02-12T23:43:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:43:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 10.27 2021-02-12T23:43:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:43:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.034 ExpectedMeasDiff: nan currMeas.covariance: 24.90 2021-02-12T23:43:30Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:43:30Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:43:30Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:43:30Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:43:30Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:43:30Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:43:30Z- Filter 0 No beams used in subcloud update 2021-02-12T23:43:30Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:43:30Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:43:30Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 741.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:43:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173409.56 sec. 2021-02-12T23:43:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:43:30Z- TNavBF::mmse selected from filter -1 2021-02-12T23:43:30Z- variances([0], [2], and psi_berg) = 741.436331 833.025495 0.000000 2021-02-12T23:43:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:43:30Z- LcmTrn::publishEstimates() - MLE : 4057680.46 591372.02 10.34 2021-02-12T23:43:30Z- LcmTrn::publishEstimates() - MMSE : 4057690.96 591413.52 10.34 2021-02-12T23:43:30Z- LcmTrn::publishEstimates() - COVAR: 741.44 833.03 0.92 0.00 2021-02-12T23:43:30Z- AHRS_M2 msg: 1613173410.74 epoch sec; seqNo:6252 2021-02-12T23:43:30Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.17 psi 2021-02-12T23:43:30Z- handleDepth()-Depth_Keller msg: 1613173410742 epoch sec; seqNo:6304; depth 10.49 2021-02-12T23:43:30Z- handleDvl() - RDI_Pathfinder msg: 1613173410.77 epoch sec; seqNo:2961 2021-02-12T23:43:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.03 , 103.79 , 76.41 , 122.72 2021-02-12T23:43:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , 0.01 2021-02-12T23:43:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173410.78 epoch sec; seqNo:6304; 4057691.35 north; 591414.73 east 2021-02-12T23:43:31Z- AHRS_M2 msg: 1613173411.14 epoch sec; seqNo:6253 2021-02-12T23:43:31Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-12T23:43:31Z- handleDepth()-Depth_Keller msg: 1613173411148 epoch sec; seqNo:6305; depth 10.61 2021-02-12T23:43:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173411.16 epoch sec; seqNo:6305; 4057691.73 north; 591414.73 east 2021-02-12T23:43:31Z- AHRS_M2 msg: 1613173411.56 epoch sec; seqNo:6254 2021-02-12T23:43:31Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.19 psi 2021-02-12T23:43:31Z- handleDepth()-Depth_Keller msg: 1613173411563 epoch sec; seqNo:6306; depth 10.75 2021-02-12T23:43:31Z- handleDvl() - RDI_Pathfinder msg: 1613173411.58 epoch sec; seqNo:2962 2021-02-12T23:43:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 105.14 , 79.46 , 121.36 2021-02-12T23:43:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.06 , -0.03 2021-02-12T23:43:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173411.59 epoch sec; seqNo:6306; 4057691.73 north; 591414.73 east 2021-02-12T23:43:31Z- AHRS_M2 msg: 1613173411.94 epoch sec; seqNo:6255 2021-02-12T23:43:31Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.20 psi 2021-02-12T23:43:31Z- handleDepth()-Depth_Keller msg: 1613173411949 epoch sec; seqNo:6307; depth 10.90 2021-02-12T23:43:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173411.97 epoch sec; seqNo:6307; 4057692.11 north; 591414.73 east 2021-02-12T23:43:32Z- AHRS_M2 msg: 1613173412.35 epoch sec; seqNo:6256 2021-02-12T23:43:32Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-12T23:43:32Z- handleDepth()-Depth_Keller msg: 1613173412352 epoch sec; seqNo:6308; depth 11.04 2021-02-12T23:43:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173412.36 epoch sec; seqNo:6308; 4057692.49 north; 591414.72 east 2021-02-12T23:43:32Z- AHRS_M2 msg: 1613173412.75 epoch sec; seqNo:6257 2021-02-12T23:43:32Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.19 psi 2021-02-12T23:43:32Z- handleDepth()-Depth_Keller msg: 1613173412756 epoch sec; seqNo:6309; depth 11.16 2021-02-12T23:43:32Z- handleDvl() - RDI_Pathfinder msg: 1613173412.77 epoch sec; seqNo:2963 2021-02-12T23:43:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.35 , 106.63 , 0.00 , 118.80 2021-02-12T23:43:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.08 , 0.04 2021-02-12T23:43:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173412.78 epoch sec; seqNo:6309; 4057692.87 north; 591414.72 east 2021-02-12T23:43:33Z- AHRS_M2 msg: 1613173413.16 epoch sec; seqNo:6258 2021-02-12T23:43:33Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.19 psi 2021-02-12T23:43:33Z- handleDepth()-Depth_Keller msg: 1613173413167 epoch sec; seqNo:6310; depth 11.29 2021-02-12T23:43:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173413.18 epoch sec; seqNo:6310; 4057692.87 north; 591414.72 east 2021-02-12T23:43:33Z- AHRS_M2 msg: 1613173413.56 epoch sec; seqNo:6259 2021-02-12T23:43:33Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-12T23:43:33Z- handleDepth()-Depth_Keller msg: 1613173413573 epoch sec; seqNo:6311; depth 11.41 2021-02-12T23:43:33Z- handleDvl() - RDI_Pathfinder msg: 1613173413.59 epoch sec; seqNo:2964 2021-02-12T23:43:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.21 , 78.44 , 117.11 2021-02-12T23:43:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , 0.09 2021-02-12T23:43:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173413.60 epoch sec; seqNo:6311; 4057693.26 north; 591415.93 east 2021-02-12T23:43:33Z- AHRS_M2 msg: 1613173413.96 epoch sec; seqNo:6260 2021-02-12T23:43:33Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.18 psi 2021-02-12T23:43:33Z- handleDepth()-Depth_Keller msg: 1613173413968 epoch sec; seqNo:6312; depth 11.54 2021-02-12T23:43:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173413.98 epoch sec; seqNo:6312; 4057693.64 north; 591415.93 east 2021-02-12T23:43:34Z- AHRS_M2 msg: 1613173414.37 epoch sec; seqNo:6261 2021-02-12T23:43:34Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.19 psi 2021-02-12T23:43:34Z- handleDepth()-Depth_Keller msg: 1613173414374 epoch sec; seqNo:6313; depth 11.64 2021-02-12T23:43:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173414.38 epoch sec; seqNo:6313; 4057693.64 north; 591415.93 east 2021-02-12T23:43:34Z- AHRS_M2 msg: 1613173414.78 epoch sec; seqNo:6262 2021-02-12T23:43:34Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.20 psi 2021-02-12T23:43:34Z- handleDepth()-Depth_Keller msg: 1613173414791 epoch sec; seqNo:6314; depth 11.76 2021-02-12T23:43:34Z- handleDvl() - RDI_Pathfinder msg: 1613173414.80 epoch sec; seqNo:2965 2021-02-12T23:43:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 102.44 , 78.04 , 115.96 2021-02-12T23:43:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , 0.06 2021-02-12T23:43:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173414.81 epoch sec; seqNo:6314; 4057694.01 north; 591415.93 east 2021-02-12T23:43:35Z- AHRS_M2 msg: 1613173415.18 epoch sec; seqNo:6263 2021-02-12T23:43:35Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.20 psi 2021-02-12T23:43:35Z- handleDepth()-Depth_Keller msg: 1613173415183 epoch sec; seqNo:6315; depth 11.89 2021-02-12T23:43:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173415.19 epoch sec; seqNo:6315; 4057694.39 north; 591415.92 east 2021-02-12T23:43:35Z- AHRS_M2 msg: 1613173415.58 epoch sec; seqNo:6264 2021-02-12T23:43:35Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.19 psi 2021-02-12T23:43:35Z- handleDepth()-Depth_Keller msg: 1613173415584 epoch sec; seqNo:6316; depth 12.00 2021-02-12T23:43:35Z- handleDvl() - RDI_Pathfinder msg: 1613173415.60 epoch sec; seqNo:2966 2021-02-12T23:43:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.20 , 104.13 , 0.00 , 116.29 2021-02-12T23:43:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , 0.03 2021-02-12T23:43:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173415.61 epoch sec; seqNo:6316; 4057694.39 north; 591415.92 east 2021-02-12T23:43:35Z- LcmTrn::handleMessages() - handling 45 messages took 0.67 ms and 132 calls... 2021-02-12T23:43:35Z- waiting for 1613173413.35, time is 1613173415.61 2021-02-12T23:43:35Z- LcmTrn::updateTrn() equating pose 1613173415.61 and meas times 1613173415.61 2021-02-12T23:43:35Z- LcmTrn::updateTrn() >>>> pose time 1613173415.61, meas time 1613173415.61 <<<< 2021-02-12T23:43:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173415.61 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:43:35Z- TerrainNav::motionUpdate() - currEstimate = (4057694, 591415.92, 12.00) 2021-02-12T23:43:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:43:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:43:35Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173415.61 sec.; No relevant navigation data available 2021-02-12T23:43:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:43:35Z- TNavBF::mmse selected from filter -1 2021-02-12T23:43:35Z- variances([0], [2], and psi_berg) = 741.347111 833.025421 0.000000 2021-02-12T23:43:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:43:35Z- LcmTrn::publishEstimates() - MLE : 4057683.89 591374.42 12.00 2021-02-12T23:43:35Z- LcmTrn::publishEstimates() - MMSE : 4057694.39 591415.92 12.00 2021-02-12T23:43:35Z- LcmTrn::publishEstimates() - COVAR: 741.35 833.03 1.14 0.00 2021-02-12T23:43:35Z- AHRS_M2 msg: 1613173415.99 epoch sec; seqNo:6265 2021-02-12T23:43:35Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-12T23:43:35Z- handleDepth()-Depth_Keller msg: 1613173415997 epoch sec; seqNo:6317; depth 12.10 2021-02-12T23:43:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173416.02 epoch sec; seqNo:6317; 4057694.79 north; 591417.14 east 2021-02-12T23:43:36Z- AHRS_M2 msg: 1613173416.39 epoch sec; seqNo:6266 2021-02-12T23:43:36Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-12T23:43:36Z- handleDepth()-Depth_Keller msg: 1613173416392 epoch sec; seqNo:6318; depth 12.23 2021-02-12T23:43:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173416.41 epoch sec; seqNo:6318; 4057695.16 north; 591417.13 east 2021-02-12T23:43:36Z- AHRS_M2 msg: 1613173416.79 epoch sec; seqNo:6267 2021-02-12T23:43:36Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.17 psi 2021-02-12T23:43:36Z- handleDepth()-Depth_Keller msg: 1613173416796 epoch sec; seqNo:6319; depth 12.36 2021-02-12T23:43:36Z- handleDvl() - RDI_Pathfinder msg: 1613173416.81 epoch sec; seqNo:2967 2021-02-12T23:43:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.53 , 102.37 , 78.58 , 114.27 2021-02-12T23:43:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.04 , -0.01 2021-02-12T23:43:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173416.83 epoch sec; seqNo:6319; 4057695.54 north; 591417.13 east 2021-02-12T23:43:37Z- AHRS_M2 msg: 1613173417.20 epoch sec; seqNo:6268 2021-02-12T23:43:37Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.17 psi 2021-02-12T23:43:37Z- handleDepth()-Depth_Keller msg: 1613173417205 epoch sec; seqNo:6320; depth 12.50 2021-02-12T23:43:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173417.22 epoch sec; seqNo:6320; 4057695.92 north; 591417.13 east 2021-02-12T23:43:37Z- AHRS_M2 msg: 1613173417.60 epoch sec; seqNo:6269 2021-02-12T23:43:37Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.17 psi 2021-02-12T23:43:37Z- handleDepth()-Depth_Keller msg: 1613173417605 epoch sec; seqNo:6321; depth 12.59 2021-02-12T23:43:37Z- handleDvl() - RDI_Pathfinder msg: 1613173417.63 epoch sec; seqNo:2968 2021-02-12T23:43:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.27 , 101.49 , 78.51 , 113.66 2021-02-12T23:43:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.03 , -0.10 2021-02-12T23:43:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173417.64 epoch sec; seqNo:6321; 4057695.92 north; 591417.13 east 2021-02-12T23:43:38Z- AHRS_M2 msg: 1613173418.01 epoch sec; seqNo:6270 2021-02-12T23:43:38Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-12T23:43:38Z- handleDepth()-Depth_Keller msg: 1613173418012 epoch sec; seqNo:6322; depth 12.73 2021-02-12T23:43:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173418.03 epoch sec; seqNo:6322; 4057696.30 north; 591417.12 east 2021-02-12T23:43:38Z- AHRS_M2 msg: 1613173418.41 epoch sec; seqNo:6271 2021-02-12T23:43:38Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.18 psi 2021-02-12T23:43:38Z- handleDepth()-Depth_Keller msg: 1613173418412 epoch sec; seqNo:6323; depth 12.85 2021-02-12T23:43:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173418.43 epoch sec; seqNo:6323; 4057696.69 north; 591418.34 east 2021-02-12T23:43:38Z- AHRS_M2 msg: 1613173418.81 epoch sec; seqNo:6272 2021-02-12T23:43:38Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.18 psi 2021-02-12T23:43:38Z- handleDepth()-Depth_Keller msg: 1613173418821 epoch sec; seqNo:6324; depth 12.97 2021-02-12T23:43:38Z- handleDvl() - RDI_Pathfinder msg: 1613173418.83 epoch sec; seqNo:2969 2021-02-12T23:43:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 101.56 , 78.58 , 113.73 2021-02-12T23:43:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.05 , -0.19 2021-02-12T23:43:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173418.85 epoch sec; seqNo:6324; 4057697.07 north; 591418.34 east 2021-02-12T23:43:39Z- AHRS_M2 msg: 1613173419.22 epoch sec; seqNo:6273 2021-02-12T23:43:39Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.18 psi 2021-02-12T23:43:39Z- handleDepth()-Depth_Keller msg: 1613173419221 epoch sec; seqNo:6325; depth 13.09 2021-02-12T23:43:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173419.23 epoch sec; seqNo:6325; 4057697.45 north; 591418.33 east 2021-02-12T23:43:39Z- AHRS_M2 msg: 1613173419.62 epoch sec; seqNo:6274 2021-02-12T23:43:39Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.18 psi 2021-02-12T23:43:39Z- handleDepth()-Depth_Keller msg: 1613173419624 epoch sec; seqNo:6326; depth 13.20 2021-02-12T23:43:39Z- handleDvl() - RDI_Pathfinder msg: 1613173419.64 epoch sec; seqNo:2970 2021-02-12T23:43:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 101.56 , 78.58 , 112.38 2021-02-12T23:43:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.17 , -0.15 2021-02-12T23:43:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173419.65 epoch sec; seqNo:6326; 4057697.83 north; 591418.33 east 2021-02-12T23:43:40Z- AHRS_M2 msg: 1613173420.02 epoch sec; seqNo:6275 2021-02-12T23:43:40Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.18 psi 2021-02-12T23:43:40Z- handleDepth()-Depth_Keller msg: 1613173420028 epoch sec; seqNo:6327; depth 13.32 2021-02-12T23:43:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173420.04 epoch sec; seqNo:6327; 4057698.21 north; 591418.32 east 2021-02-12T23:43:40Z- AHRS_M2 msg: 1613173420.43 epoch sec; seqNo:6276 2021-02-12T23:43:40Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.17 psi 2021-02-12T23:43:40Z- handleDepth()-Depth_Keller msg: 1613173420435 epoch sec; seqNo:6328; depth 13.43 2021-02-12T23:43:40Z- handleDvl() - RDI_Pathfinder msg: 1613173420.45 epoch sec; seqNo:2971 2021-02-12T23:43:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.64 , 100.21 , 76.00 , 111.02 2021-02-12T23:43:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.20 , -0.04 2021-02-12T23:43:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173420.46 epoch sec; seqNo:6328; 4057698.59 north; 591418.32 east 2021-02-12T23:43:40Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-12T23:43:40Z- waiting for 1613173418.61, time is 1613173420.46 2021-02-12T23:43:40Z- LcmTrn::updateTrn() equating pose 1613173420.46 and meas times 1613173420.46 2021-02-12T23:43:40Z- LcmTrn::updateTrn() >>>> pose time 1613173420.46, meas time 1613173420.46 <<<< 2021-02-12T23:43:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173420.46 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:43:40Z- TerrainNav::motionUpdate() - currEstimate = (4057699, 591418.32, 13.43) 2021-02-12T23:43:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:43:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:43:40Z- TNavBF::Projecting Measurements 2021-02-12T23:43:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:43:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:43:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:43:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:43:40Z- TNavFilter::4 good beams. 2021-02-12T23:43:40Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:43:40Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:43:40Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:43:40Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:43:40Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:43:40Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:43:40Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:43:40Z- TNavBF::mmse selected from filter -1 2021-02-12T23:43:40Z- TerrainMapOctree: (vehN, vehE ) = (4057698.59, 591418.32). 2021-02-12T23:43:40Z- TerrainMapOctree: Min Distance = 4542.13. 2021-02-12T23:43:40Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:43:40Z- There is no specific good beam that all particles have in common. 2021-02-12T23:43:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:43:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 10.93 2021-02-12T23:43:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:43:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 16.07 2021-02-12T23:43:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:43:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 9.24 2021-02-12T23:43:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:43:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 19.72 2021-02-12T23:43:40Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:43:40Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:43:40Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:43:40Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:43:40Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:43:40Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:43:40Z- Filter 0 No beams used in subcloud update 2021-02-12T23:43:40Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:43:40Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:43:40Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 741.3 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:43:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173420.46 sec. 2021-02-12T23:43:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:43:40Z- TNavBF::mmse selected from filter -1 2021-02-12T23:43:40Z- variances([0], [2], and psi_berg) = 741.251470 833.025341 0.000000 2021-02-12T23:43:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:43:40Z- LcmTrn::publishEstimates() - MLE : 4057688.09 591376.82 13.43 2021-02-12T23:43:40Z- LcmTrn::publishEstimates() - MMSE : 4057698.59 591418.32 13.43 2021-02-12T23:43:40Z- LcmTrn::publishEstimates() - COVAR: 741.25 833.03 1.00 0.00 2021-02-12T23:43:40Z- AHRS_M2 msg: 1613173420.83 epoch sec; seqNo:6277 2021-02-12T23:43:40Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.17 psi 2021-02-12T23:43:40Z- handleDepth()-Depth_Keller msg: 1613173420843 epoch sec; seqNo:6329; depth 13.55 2021-02-12T23:43:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173420.85 epoch sec; seqNo:6329; 4057698.59 north; 591418.32 east 2021-02-12T23:43:41Z- AHRS_M2 msg: 1613173421.24 epoch sec; seqNo:6278 2021-02-12T23:43:41Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.16 psi 2021-02-12T23:43:41Z- handleDepth()-Depth_Keller msg: 1613173421245 epoch sec; seqNo:6330; depth 13.66 2021-02-12T23:43:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173421.25 epoch sec; seqNo:6330; 4057698.98 north; 591419.54 east 2021-02-12T23:43:41Z- AHRS_M2 msg: 1613173421.64 epoch sec; seqNo:6279 2021-02-12T23:43:41Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.15 psi 2021-02-12T23:43:41Z- handleDepth()-Depth_Keller msg: 1613173421649 epoch sec; seqNo:6331; depth 13.78 2021-02-12T23:43:41Z- handleDvl() - RDI_Pathfinder msg: 1613173421.66 epoch sec; seqNo:2972 2021-02-12T23:43:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 96.36 , 76.41 , 108.52 2021-02-12T23:43:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.14 , 0.06 2021-02-12T23:43:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173421.67 epoch sec; seqNo:6331; 4057699.36 north; 591419.53 east 2021-02-12T23:43:42Z- AHRS_M2 msg: 1613173422.04 epoch sec; seqNo:6280 2021-02-12T23:43:42Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.14 psi 2021-02-12T23:43:42Z- handleDepth()-Depth_Keller msg: 1613173422051 epoch sec; seqNo:6332; depth 13.90 2021-02-12T23:43:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173422.06 epoch sec; seqNo:6332; 4057699.74 north; 591419.53 east 2021-02-12T23:43:42Z- AHRS_M2 msg: 1613173422.46 epoch sec; seqNo:6281 2021-02-12T23:43:42Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.14 psi 2021-02-12T23:43:42Z- handleDepth()-Depth_Keller msg: 1613173422465 epoch sec; seqNo:6333; depth 14.00 2021-02-12T23:43:42Z- handleDvl() - RDI_Pathfinder msg: 1613173422.48 epoch sec; seqNo:2973 2021-02-12T23:43:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 97.64 , 73.96 , 108.59 2021-02-12T23:43:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , 0.06 2021-02-12T23:43:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173422.49 epoch sec; seqNo:6333; 4057699.74 north; 591419.53 east 2021-02-12T23:43:42Z- AHRS_M2 msg: 1613173422.85 epoch sec; seqNo:6282 2021-02-12T23:43:42Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.14 psi 2021-02-12T23:43:42Z- handleDepth()-Depth_Keller msg: 1613173422858 epoch sec; seqNo:6334; depth 14.12 2021-02-12T23:43:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173422.87 epoch sec; seqNo:6334; 4057700.12 north; 591419.52 east 2021-02-12T23:43:43Z- AHRS_M2 msg: 1613173423.26 epoch sec; seqNo:6283 2021-02-12T23:43:43Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.15 psi 2021-02-12T23:43:43Z- handleDepth()-Depth_Keller msg: 1613173423260 epoch sec; seqNo:6335; depth 14.22 2021-02-12T23:43:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173423.28 epoch sec; seqNo:6335; 4057700.49 north; 591419.52 east 2021-02-12T23:43:43Z- AHRS_M2 msg: 1613173423.66 epoch sec; seqNo:6284 2021-02-12T23:43:43Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.16 psi 2021-02-12T23:43:43Z- handleDepth()-Depth_Keller msg: 1613173423673 epoch sec; seqNo:6336; depth 14.32 2021-02-12T23:43:43Z- handleDvl() - RDI_Pathfinder msg: 1613173423.69 epoch sec; seqNo:2974 2021-02-12T23:43:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.33 , 97.71 , 73.55 , 107.44 2021-02-12T23:43:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , 0.05 2021-02-12T23:43:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173423.70 epoch sec; seqNo:6336; 4057700.49 north; 591419.52 east 2021-02-12T23:43:44Z- AHRS_M2 msg: 1613173424.06 epoch sec; seqNo:6285 2021-02-12T23:43:44Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.17 psi 2021-02-12T23:43:44Z- handleDepth()-Depth_Keller msg: 1613173424073 epoch sec; seqNo:6337; depth 14.43 2021-02-12T23:43:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173424.08 epoch sec; seqNo:6337; 4057700.87 north; 591419.52 east 2021-02-12T23:43:44Z- AHRS_M2 msg: 1613173424.47 epoch sec; seqNo:6286 2021-02-12T23:43:44Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.18 psi 2021-02-12T23:43:44Z- handleDepth()-Depth_Keller msg: 1613173424474 epoch sec; seqNo:6338; depth 14.57 2021-02-12T23:43:44Z- handleDvl() - RDI_Pathfinder msg: 1613173424.51 epoch sec; seqNo:2975 2021-02-12T23:43:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.49 , 78.24 , 111.09 2021-02-12T23:43:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.06 , 0.04 2021-02-12T23:43:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173424.52 epoch sec; seqNo:6338; 4057701.26 north; 591420.73 east 2021-02-12T23:43:44Z- AHRS_M2 msg: 1613173424.87 epoch sec; seqNo:6287 2021-02-12T23:43:44Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.19 psi 2021-02-12T23:43:44Z- handleDepth()-Depth_Keller msg: 1613173424876 epoch sec; seqNo:6339; depth 14.66 2021-02-12T23:43:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173424.90 epoch sec; seqNo:6339; 4057701.64 north; 591420.73 east 2021-02-12T23:43:45Z- AHRS_M2 msg: 1613173425.28 epoch sec; seqNo:6288 2021-02-12T23:43:45Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.19 psi 2021-02-12T23:43:45Z- handleDepth()-Depth_Keller msg: 1613173425280 epoch sec; seqNo:6340; depth 14.77 2021-02-12T23:43:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173425.29 epoch sec; seqNo:6340; 4057701.64 north; 591420.73 east 2021-02-12T23:43:45Z- AHRS_M2 msg: 1613173425.68 epoch sec; seqNo:6289 2021-02-12T23:43:45Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.19 psi 2021-02-12T23:43:45Z- handleDepth()-Depth_Keller msg: 1613173425684 epoch sec; seqNo:6341; depth 14.88 2021-02-12T23:43:45Z- handleDvl() - RDI_Pathfinder msg: 1613173425.70 epoch sec; seqNo:2976 2021-02-12T23:43:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.22 , 98.79 , 75.18 , 108.25 2021-02-12T23:43:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , 0.06 2021-02-12T23:43:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173425.71 epoch sec; seqNo:6341; 4057702.02 north; 591420.72 east 2021-02-12T23:43:45Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-12T23:43:45Z- waiting for 1613173423.46, time is 1613173425.71 2021-02-12T23:43:45Z- LcmTrn::updateTrn() equating pose 1613173425.71 and meas times 1613173425.71 2021-02-12T23:43:45Z- LcmTrn::updateTrn() >>>> pose time 1613173425.71, meas time 1613173425.71 <<<< 2021-02-12T23:43:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173425.71 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:43:45Z- TerrainNav::motionUpdate() - currEstimate = (4057702, 591420.72, 14.88) 2021-02-12T23:43:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:43:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:43:45Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173425.71 sec.; No relevant navigation data available 2021-02-12T23:43:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:43:45Z- TNavBF::mmse selected from filter -1 2021-02-12T23:43:45Z- variances([0], [2], and psi_berg) = 741.162457 833.025267 0.000000 2021-02-12T23:43:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:43:45Z- LcmTrn::publishEstimates() - MLE : 4057691.52 591379.22 14.88 2021-02-12T23:43:45Z- LcmTrn::publishEstimates() - MMSE : 4057702.02 591420.72 14.88 2021-02-12T23:43:45Z- LcmTrn::publishEstimates() - COVAR: 741.16 833.03 1.22 0.00 2021-02-12T23:43:46Z- AHRS_M2 msg: 1613173426.08 epoch sec; seqNo:6290 2021-02-12T23:43:46Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.18 psi 2021-02-12T23:43:46Z- handleDepth()-Depth_Keller msg: 1613173426090 epoch sec; seqNo:6342; depth 15.01 2021-02-12T23:43:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173426.10 epoch sec; seqNo:6342; 4057702.40 north; 591420.72 east 2021-02-12T23:43:46Z- AHRS_M2 msg: 1613173426.50 epoch sec; seqNo:6291 2021-02-12T23:43:46Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.18 psi 2021-02-12T23:43:46Z- handleDepth()-Depth_Keller msg: 1613173426523 epoch sec; seqNo:6343; depth 15.11 2021-02-12T23:43:46Z- handleDvl() - RDI_Pathfinder msg: 1613173426.54 epoch sec; seqNo:2977 2021-02-12T23:43:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 98.93 , 78.24 , 108.66 2021-02-12T23:43:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , 0.05 2021-02-12T23:43:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173426.55 epoch sec; seqNo:6343; 4057702.78 north; 591420.72 east 2021-02-12T23:43:46Z- AHRS_M2 msg: 1613173426.89 epoch sec; seqNo:6292 2021-02-12T23:43:46Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.17 psi 2021-02-12T23:43:46Z- handleDepth()-Depth_Keller msg: 1613173426896 epoch sec; seqNo:6344; depth 15.22 2021-02-12T23:43:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173426.91 epoch sec; seqNo:6344; 4057702.78 north; 591420.72 east 2021-02-12T23:43:47Z- AHRS_M2 msg: 1613173427.30 epoch sec; seqNo:6293 2021-02-12T23:43:47Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.17 psi 2021-02-12T23:43:47Z- handleDepth()-Depth_Keller msg: 1613173427306 epoch sec; seqNo:6345; depth 15.34 2021-02-12T23:43:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173427.32 epoch sec; seqNo:6345; 4057703.17 north; 591421.93 east 2021-02-12T23:43:47Z- AHRS_M2 msg: 1613173427.70 epoch sec; seqNo:6294 2021-02-12T23:43:47Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.17 psi 2021-02-12T23:43:47Z- handleDepth()-Depth_Keller msg: 1613173427709 epoch sec; seqNo:6346; depth 15.45 2021-02-12T23:43:47Z- handleDvl() - RDI_Pathfinder msg: 1613173427.73 epoch sec; seqNo:2978 2021-02-12T23:43:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.49 , 99.06 , 75.18 , 109.87 2021-02-12T23:43:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.04 2021-02-12T23:43:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173427.74 epoch sec; seqNo:6346; 4057703.55 north; 591421.93 east 2021-02-12T23:43:48Z- AHRS_M2 msg: 1613173428.11 epoch sec; seqNo:6295 2021-02-12T23:43:48Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.18 psi 2021-02-12T23:43:48Z- handleDepth()-Depth_Keller msg: 1613173428115 epoch sec; seqNo:6347; depth 15.57 2021-02-12T23:43:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173428.14 epoch sec; seqNo:6347; 4057703.55 north; 591421.93 east 2021-02-12T23:43:48Z- AHRS_M2 msg: 1613173428.51 epoch sec; seqNo:6296 2021-02-12T23:43:48Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.18 psi 2021-02-12T23:43:48Z- handleDepth()-Depth_Keller msg: 1613173428517 epoch sec; seqNo:6348; depth 15.68 2021-02-12T23:43:48Z- handleDvl() - RDI_Pathfinder msg: 1613173428.53 epoch sec; seqNo:2979 2021-02-12T23:43:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 97.64 , 74.78 , 107.37 2021-02-12T23:43:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , 0.03 2021-02-12T23:43:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173428.54 epoch sec; seqNo:6348; 4057703.93 north; 591421.93 east 2021-02-12T23:43:48Z- AHRS_M2 msg: 1613173428.91 epoch sec; seqNo:6297 2021-02-12T23:43:48Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.19 psi 2021-02-12T23:43:48Z- handleDepth()-Depth_Keller msg: 1613173428925 epoch sec; seqNo:6349; depth 15.79 2021-02-12T23:43:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173428.94 epoch sec; seqNo:6349; 4057704.31 north; 591421.92 east 2021-02-12T23:43:49Z- AHRS_M2 msg: 1613173429.32 epoch sec; seqNo:6298 2021-02-12T23:43:49Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.19 psi 2021-02-12T23:43:49Z- handleDepth()-Depth_Keller msg: 1613173429325 epoch sec; seqNo:6350; depth 15.91 2021-02-12T23:43:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173429.33 epoch sec; seqNo:6350; 4057704.69 north; 591421.92 east 2021-02-12T23:43:49Z- AHRS_M2 msg: 1613173429.72 epoch sec; seqNo:6299 2021-02-12T23:43:49Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.19 psi 2021-02-12T23:43:49Z- handleDepth()-Depth_Keller msg: 1613173429725 epoch sec; seqNo:6351; depth 16.03 2021-02-12T23:43:49Z- handleDvl() - RDI_Pathfinder msg: 1613173429.74 epoch sec; seqNo:2980 2021-02-12T23:43:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 96.49 , 78.24 , 108.66 2021-02-12T23:43:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , 0.01 2021-02-12T23:43:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173429.76 epoch sec; seqNo:6351; 4057704.70 north; 591423.14 east 2021-02-12T23:43:50Z- AHRS_M2 msg: 1613173430.13 epoch sec; seqNo:6300 2021-02-12T23:43:50Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.19 psi 2021-02-12T23:43:50Z- handleDepth()-Depth_Keller msg: 1613173430133 epoch sec; seqNo:6352; depth 16.15 2021-02-12T23:43:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173430.15 epoch sec; seqNo:6352; 4057705.08 north; 591423.13 east 2021-02-12T23:43:50Z- AHRS_M2 msg: 1613173430.53 epoch sec; seqNo:6301 2021-02-12T23:43:50Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.19 psi 2021-02-12T23:43:50Z- handleDepth()-Depth_Keller msg: 1613173430532 epoch sec; seqNo:6353; depth 16.26 2021-02-12T23:43:50Z- handleDvl() - RDI_Pathfinder msg: 1613173430.55 epoch sec; seqNo:2981 2021-02-12T23:43:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.27 , 96.42 , 78.18 , 106.16 2021-02-12T23:43:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , -0.04 2021-02-12T23:43:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173430.56 epoch sec; seqNo:6353; 4057705.46 north; 591423.13 east 2021-02-12T23:43:50Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 129 calls... 2021-02-12T23:43:50Z- waiting for 1613173428.71, time is 1613173430.56 2021-02-12T23:43:50Z- LcmTrn::updateTrn() equating pose 1613173430.56 and meas times 1613173430.56 2021-02-12T23:43:50Z- LcmTrn::updateTrn() >>>> pose time 1613173430.56, meas time 1613173430.56 <<<< 2021-02-12T23:43:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173430.56 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:43:50Z- TerrainNav::motionUpdate() - currEstimate = (4057705, 591423.13, 16.26) 2021-02-12T23:43:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:43:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:43:50Z- TNavBF::Projecting Measurements 2021-02-12T23:43:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:43:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:43:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:43:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:43:50Z- TNavFilter::4 good beams. 2021-02-12T23:43:50Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:43:50Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:43:50Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:43:50Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:43:50Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:43:50Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:43:50Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:43:50Z- TNavBF::mmse selected from filter -1 2021-02-12T23:43:50Z- TerrainMapOctree: (vehN, vehE ) = (4057705.46, 591423.13). 2021-02-12T23:43:50Z- TerrainMapOctree: Min Distance = 4533.76. 2021-02-12T23:43:50Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:43:50Z- There is no specific good beam that all particles have in common. 2021-02-12T23:43:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:43:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.108 ExpectedMeasDiff: nan currMeas.covariance: 10.05 2021-02-12T23:43:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:43:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 14.88 2021-02-12T23:43:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:43:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 9.78 2021-02-12T23:43:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:43:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 18.03 2021-02-12T23:43:50Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:43:50Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:43:50Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:43:50Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:43:50Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:43:50Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:43:50Z- Filter 0 No beams used in subcloud update 2021-02-12T23:43:50Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:43:50Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:43:50Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 741.1 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:43:50Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173430.56 sec. 2021-02-12T23:43:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:43:50Z- TNavBF::mmse selected from filter -1 2021-02-12T23:43:50Z- variances([0], [2], and psi_berg) = 741.073544 833.025193 0.000000 2021-02-12T23:43:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:43:50Z- LcmTrn::publishEstimates() - MLE : 4057694.96 591381.63 16.26 2021-02-12T23:43:50Z- LcmTrn::publishEstimates() - MMSE : 4057705.46 591423.13 16.26 2021-02-12T23:43:50Z- LcmTrn::publishEstimates() - COVAR: 741.07 833.03 1.02 0.00 2021-02-12T23:43:50Z- AHRS_M2 msg: 1613173430.93 epoch sec; seqNo:6302 2021-02-12T23:43:50Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.19 psi 2021-02-12T23:43:50Z- handleDepth()-Depth_Keller msg: 1613173430936 epoch sec; seqNo:6354; depth 16.37 2021-02-12T23:43:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173430.95 epoch sec; seqNo:6354; 4057705.84 north; 591423.13 east 2021-02-12T23:43:51Z- AHRS_M2 msg: 1613173431.35 epoch sec; seqNo:6303 2021-02-12T23:43:51Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.20 psi 2021-02-12T23:43:51Z- handleDepth()-Depth_Keller msg: 1613173431357 epoch sec; seqNo:6355; depth 16.48 2021-02-12T23:43:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173431.38 epoch sec; seqNo:6355; 4057705.84 north; 591423.13 east 2021-02-12T23:43:51Z- AHRS_M2 msg: 1613173431.74 epoch sec; seqNo:6304 2021-02-12T23:43:51Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.20 psi 2021-02-12T23:43:51Z- handleDepth()-Depth_Keller msg: 1613173431746 epoch sec; seqNo:6356; depth 16.59 2021-02-12T23:43:51Z- handleDvl() - RDI_Pathfinder msg: 1613173431.76 epoch sec; seqNo:2982 2021-02-12T23:43:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.43 , 73.55 , 107.51 2021-02-12T23:43:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.04 , -0.10 2021-02-12T23:43:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173431.77 epoch sec; seqNo:6356; 4057706.23 north; 591424.34 east 2021-02-12T23:43:52Z- AHRS_M2 msg: 1613173432.15 epoch sec; seqNo:6305 2021-02-12T23:43:52Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.21 psi 2021-02-12T23:43:52Z- handleDepth()-Depth_Keller msg: 1613173432163 epoch sec; seqNo:6357; depth 16.72 2021-02-12T23:43:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173432.19 epoch sec; seqNo:6357; 4057706.61 north; 591424.34 east 2021-02-12T23:43:52Z- AHRS_M2 msg: 1613173432.55 epoch sec; seqNo:6306 2021-02-12T23:43:52Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.22 psi 2021-02-12T23:43:52Z- handleDepth()-Depth_Keller msg: 1613173432552 epoch sec; seqNo:6358; depth 16.83 2021-02-12T23:43:52Z- handleDvl() - RDI_Pathfinder msg: 1613173432.56 epoch sec; seqNo:2983 2021-02-12T23:43:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.93 , 100.21 , 78.58 , 109.67 2021-02-12T23:43:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.13 2021-02-12T23:43:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173432.59 epoch sec; seqNo:6358; 4057706.99 north; 591424.33 east 2021-02-12T23:43:52Z- AHRS_M2 msg: 1613173432.95 epoch sec; seqNo:6307 2021-02-12T23:43:52Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.22 psi 2021-02-12T23:43:52Z- handleDepth()-Depth_Keller msg: 1613173432957 epoch sec; seqNo:6359; depth 16.94 2021-02-12T23:43:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173432.97 epoch sec; seqNo:6359; 4057707.37 north; 591424.33 east 2021-02-12T23:43:53Z- AHRS_M2 msg: 1613173433.36 epoch sec; seqNo:6308 2021-02-12T23:43:53Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.22 psi 2021-02-12T23:43:53Z- handleDepth()-Depth_Keller msg: 1613173433360 epoch sec; seqNo:6360; depth 17.05 2021-02-12T23:43:53Z- handleDvl() - RDI_Pathfinder msg: 1613173433.37 epoch sec; seqNo:2984 2021-02-12T23:43:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.23 , 96.36 , 72.73 , 111.23 2021-02-12T23:43:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.13 , -0.09 2021-02-12T23:43:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173433.39 epoch sec; seqNo:6360; 4057707.37 north; 591424.33 east 2021-02-12T23:43:53Z- AHRS_M2 msg: 1613173433.76 epoch sec; seqNo:6309 2021-02-12T23:43:53Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.21 psi 2021-02-12T23:43:53Z- handleDepth()-Depth_Keller msg: 1613173433769 epoch sec; seqNo:6361; depth 17.18 2021-02-12T23:43:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173433.78 epoch sec; seqNo:6361; 4057707.74 north; 591424.33 east 2021-02-12T23:43:54Z- AHRS_M2 msg: 1613173434.16 epoch sec; seqNo:6310 2021-02-12T23:43:54Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.21 psi 2021-02-12T23:43:54Z- handleDepth()-Depth_Keller msg: 1613173434169 epoch sec; seqNo:6362; depth 17.30 2021-02-12T23:43:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173434.18 epoch sec; seqNo:6362; 4057708.12 north; 591424.32 east 2021-02-12T23:43:54Z- AHRS_M2 msg: 1613173434.57 epoch sec; seqNo:6311 2021-02-12T23:43:54Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.20 psi 2021-02-12T23:43:54Z- handleDepth()-Depth_Keller msg: 1613173434574 epoch sec; seqNo:6363; depth 17.42 2021-02-12T23:43:54Z- handleDvl() - RDI_Pathfinder msg: 1613173434.59 epoch sec; seqNo:2985 2021-02-12T23:43:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.41 , 96.56 , 78.31 , 107.51 2021-02-12T23:43:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.19 , -0.02 2021-02-12T23:43:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173434.60 epoch sec; seqNo:6363; 4057708.52 north; 591425.54 east 2021-02-12T23:43:54Z- AHRS_M2 msg: 1613173434.97 epoch sec; seqNo:6312 2021-02-12T23:43:54Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.19 psi 2021-02-12T23:43:54Z- handleDepth()-Depth_Keller msg: 1613173434981 epoch sec; seqNo:6364; depth 17.53 2021-02-12T23:43:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173434.99 epoch sec; seqNo:6364; 4057708.89 north; 591425.54 east 2021-02-12T23:43:55Z- AHRS_M2 msg: 1613173435.39 epoch sec; seqNo:6313 2021-02-12T23:43:55Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.18 psi 2021-02-12T23:43:55Z- handleDepth()-Depth_Keller msg: 1613173435393 epoch sec; seqNo:6365; depth 17.65 2021-02-12T23:43:55Z- handleDvl() - RDI_Pathfinder msg: 1613173435.40 epoch sec; seqNo:2986 2021-02-12T23:43:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.64 , 97.78 , 73.14 , 106.29 2021-02-12T23:43:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.16 , 0.01 2021-02-12T23:43:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173435.42 epoch sec; seqNo:6365; 4057709.27 north; 591425.53 east 2021-02-12T23:43:55Z- AHRS_M2 msg: 1613173435.78 epoch sec; seqNo:6314 2021-02-12T23:43:55Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.17 psi 2021-02-12T23:43:55Z- handleDepth()-Depth_Keller msg: 1613173435786 epoch sec; seqNo:6366; depth 17.78 2021-02-12T23:43:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173435.80 epoch sec; seqNo:6366; 4057709.27 north; 591425.53 east 2021-02-12T23:43:55Z- LcmTrn::handleMessages() - handling 44 messages took 0.60 ms and 132 calls... 2021-02-12T23:43:55Z- waiting for 1613173433.56, time is 1613173435.80 2021-02-12T23:43:55Z- LcmTrn::updateTrn() >>>> pose time 1613173435.80, meas time 1613173435.40 <<<< 2021-02-12T23:43:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173435.40 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:43:55Z- TerrainNav::motionUpdate() - currEstimate = (4057709, 591425.53, 17.78) 2021-02-12T23:43:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:43:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:43:55Z- TNavBF::Projecting Measurements 2021-02-12T23:43:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:43:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:43:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:43:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:43:55Z- TNavFilter::4 good beams. 2021-02-12T23:43:55Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:43:55Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:43:55Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:43:55Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:43:55Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:43:55Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:43:55Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:43:55Z- TNavBF::mmse selected from filter -1 2021-02-12T23:43:55Z- TerrainMapOctree: (vehN, vehE ) = (4057708.99, 591425.35). 2021-02-12T23:43:55Z- TerrainMapOctree: Min Distance = 4529.62. 2021-02-12T23:43:55Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:43:55Z- There is no specific good beam that all particles have in common. 2021-02-12T23:43:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:43:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 9.64 2021-02-12T23:43:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:43:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-12T23:43:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:43:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-12T23:43:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:43:55Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 18.08 2021-02-12T23:43:55Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:43:56Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:43:56Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:43:56Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:43:56Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:43:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:43:56Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:43:56Z- Filter 0 No beams used in subcloud update 2021-02-12T23:43:56Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:43:56Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:43:56Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 741.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:43:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173435.40 sec. 2021-02-12T23:43:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:43:56Z- TNavBF::mmse selected from filter -1 2021-02-12T23:43:56Z- variances([0], [2], and psi_berg) = 740.959801 833.025099 0.000000 2021-02-12T23:43:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:43:56Z- LcmTrn::publishEstimates() - MLE : 4057699.77 591378.03 17.78 2021-02-12T23:43:56Z- LcmTrn::publishEstimates() - MMSE : 4057709.27 591425.53 17.78 2021-02-12T23:43:56Z- LcmTrn::publishEstimates() - COVAR: 740.96 833.03 0.92 0.00 2021-02-12T23:43:56Z- AHRS_M2 msg: 1613173436.18 epoch sec; seqNo:6315 2021-02-12T23:43:56Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.16 psi 2021-02-12T23:43:56Z- handleDepth()-Depth_Keller msg: 1613173436188 epoch sec; seqNo:6367; depth 17.89 2021-02-12T23:43:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173436.21 epoch sec; seqNo:6367; 4057709.65 north; 591425.53 east 2021-02-12T23:43:56Z- AHRS_M2 msg: 1613173436.59 epoch sec; seqNo:6316 2021-02-12T23:43:56Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.16 psi 2021-02-12T23:43:56Z- handleDepth()-Depth_Keller msg: 1613173436593 epoch sec; seqNo:6368; depth 18.00 2021-02-12T23:43:56Z- handleDvl() - RDI_Pathfinder msg: 1613173436.61 epoch sec; seqNo:2987 2021-02-12T23:43:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.64 , 97.84 , 70.28 , 103.93 2021-02-12T23:43:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.14 , 0.02 2021-02-12T23:43:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173436.62 epoch sec; seqNo:6368; 4057710.03 north; 591425.52 east 2021-02-12T23:43:56Z- AHRS_M2 msg: 1613173436.99 epoch sec; seqNo:6317 2021-02-12T23:43:56Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.16 psi 2021-02-12T23:43:56Z- handleDepth()-Depth_Keller msg: 1613173436997 epoch sec; seqNo:6369; depth 18.12 2021-02-12T23:43:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173437.01 epoch sec; seqNo:6369; 4057710.41 north; 591425.52 east 2021-02-12T23:43:57Z- AHRS_M2 msg: 1613173437.40 epoch sec; seqNo:6318 2021-02-12T23:43:57Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.16 psi 2021-02-12T23:43:57Z- handleDepth()-Depth_Keller msg: 1613173437408 epoch sec; seqNo:6370; depth 18.24 2021-02-12T23:43:57Z- handleDvl() - RDI_Pathfinder msg: 1613173437.42 epoch sec; seqNo:2988 2021-02-12T23:43:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 94.33 , 72.32 , 104.06 2021-02-12T23:43:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.11 , 0.05 2021-02-12T23:43:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173437.43 epoch sec; seqNo:6370; 4057710.79 north; 591425.52 east 2021-02-12T23:43:57Z- AHRS_M2 msg: 1613173437.80 epoch sec; seqNo:6319 2021-02-12T23:43:57Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.16 psi 2021-02-12T23:43:57Z- handleDepth()-Depth_Keller msg: 1613173437804 epoch sec; seqNo:6371; depth 18.35 2021-02-12T23:43:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173437.82 epoch sec; seqNo:6371; 4057710.80 north; 591426.74 east 2021-02-12T23:43:58Z- AHRS_M2 msg: 1613173438.20 epoch sec; seqNo:6320 2021-02-12T23:43:58Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.17 psi 2021-02-12T23:43:58Z- handleDepth()-Depth_Keller msg: 1613173438208 epoch sec; seqNo:6372; depth 18.46 2021-02-12T23:43:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173438.22 epoch sec; seqNo:6372; 4057711.18 north; 591426.73 east 2021-02-12T23:43:58Z- AHRS_M2 msg: 1613173438.61 epoch sec; seqNo:6321 2021-02-12T23:43:58Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.18 psi 2021-02-12T23:43:58Z- handleDepth()-Depth_Keller msg: 1613173438617 epoch sec; seqNo:6373; depth 18.57 2021-02-12T23:43:58Z- handleDvl() - RDI_Pathfinder msg: 1613173438.63 epoch sec; seqNo:2989 2021-02-12T23:43:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 97.98 , 71.92 , 105.28 2021-02-12T23:43:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.06 , 0.07 2021-02-12T23:43:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173438.64 epoch sec; seqNo:6373; 4057711.56 north; 591426.73 east 2021-02-12T23:43:59Z- AHRS_M2 msg: 1613173439.01 epoch sec; seqNo:6322 2021-02-12T23:43:59Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.18 psi 2021-02-12T23:43:59Z- handleDepth()-Depth_Keller msg: 1613173439025 epoch sec; seqNo:6374; depth 18.70 2021-02-12T23:43:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173439.04 epoch sec; seqNo:6374; 4057711.56 north; 591426.73 east 2021-02-12T23:43:59Z- AHRS_M2 msg: 1613173439.44 epoch sec; seqNo:6323 2021-02-12T23:43:59Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.18 psi 2021-02-12T23:43:59Z- handleDepth()-Depth_Keller msg: 1613173439449 epoch sec; seqNo:6375; depth 18.80 2021-02-12T23:43:59Z- handleDvl() - RDI_Pathfinder msg: 1613173439.46 epoch sec; seqNo:2990 2021-02-12T23:43:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 70.28 , 105.21 2021-02-12T23:43:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.02 , 0.07 2021-02-12T23:43:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173439.48 epoch sec; seqNo:6375; 4057711.94 north; 591426.72 east 2021-02-12T23:43:59Z- AHRS_M2 msg: 1613173439.82 epoch sec; seqNo:6324 2021-02-12T23:43:59Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.18 psi 2021-02-12T23:43:59Z- handleDepth()-Depth_Keller msg: 1613173439824 epoch sec; seqNo:6376; depth 18.93 2021-02-12T23:43:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173439.84 epoch sec; seqNo:6376; 4057712.32 north; 591426.72 east 2021-02-12T23:44:00Z- AHRS_M2 msg: 1613173440.22 epoch sec; seqNo:6325 2021-02-12T23:44:00Z- AHRS_M2 msg: 0.66 phi; -0.31 theta; -0.19 psi 2021-02-12T23:44:00Z- handleDepth()-Depth_Keller msg: 1613173440229 epoch sec; seqNo:6377; depth 19.04 2021-02-12T23:44:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173440.24 epoch sec; seqNo:6377; 4057712.32 north; 591426.72 east 2021-02-12T23:44:00Z- AHRS_M2 msg: 1613173440.63 epoch sec; seqNo:6326 2021-02-12T23:44:00Z- AHRS_M2 msg: 0.66 phi; -0.31 theta; -0.19 psi 2021-02-12T23:44:00Z- handleDepth()-Depth_Keller msg: 1613173440632 epoch sec; seqNo:6378; depth 19.17 2021-02-12T23:44:00Z- handleDvl() - RDI_Pathfinder msg: 1613173440.65 epoch sec; seqNo:2991 2021-02-12T23:44:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.41 , 95.48 , 71.92 , 105.21 2021-02-12T23:44:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , 0.04 2021-02-12T23:44:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173440.66 epoch sec; seqNo:6378; 4057712.71 north; 591427.94 east 2021-02-12T23:44:01Z- AHRS_M2 msg: 1613173441.03 epoch sec; seqNo:6327 2021-02-12T23:44:01Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.19 psi 2021-02-12T23:44:01Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-12T23:44:01Z- waiting for 1613173438.80, time is 1613173441.03 2021-02-12T23:44:01Z- LcmTrn::updateTrn() >>>> pose time 1613173441.03, meas time 1613173440.65 <<<< 2021-02-12T23:44:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173440.65 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:44:01Z- TerrainNav::motionUpdate() - currEstimate = (4057713, 591427.94, 19.17) 2021-02-12T23:44:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:44:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:44:01Z- TNavBF::Projecting Measurements 2021-02-12T23:44:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:44:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:44:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:44:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:44:01Z- TNavFilter::4 good beams. 2021-02-12T23:44:01Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:44:01Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:44:01Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:44:01Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:44:01Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:44:01Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:44:01Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:44:01Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:01Z- TerrainMapOctree: (vehN, vehE ) = (4057712.46, 591427.76). 2021-02-12T23:44:01Z- TerrainMapOctree: Min Distance = 4525.40. 2021-02-12T23:44:01Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:44:01Z- There is no specific good beam that all particles have in common. 2021-02-12T23:44:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:44:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 9.34 2021-02-12T23:44:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:44:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 14.59 2021-02-12T23:44:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:44:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 8.27 2021-02-12T23:44:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:44:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 17.71 2021-02-12T23:44:01Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:44:01Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:44:01Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:44:01Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:44:01Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:44:01Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:44:01Z- Filter 0 No beams used in subcloud update 2021-02-12T23:44:01Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:44:01Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:44:01Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 740.9 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:44:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173440.65 sec. 2021-02-12T23:44:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:44:01Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:01Z- variances([0], [2], and psi_berg) = 740.850480 833.025008 0.000000 2021-02-12T23:44:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:44:01Z- LcmTrn::publishEstimates() - MLE : 4057703.21 591380.44 19.17 2021-02-12T23:44:01Z- LcmTrn::publishEstimates() - MMSE : 4057712.71 591427.94 19.17 2021-02-12T23:44:01Z- LcmTrn::publishEstimates() - COVAR: 740.85 833.03 0.85 0.00 2021-02-12T23:44:01Z- handleDepth()-Depth_Keller msg: 1613173441036 epoch sec; seqNo:6379; depth 19.28 2021-02-12T23:44:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173441.05 epoch sec; seqNo:6379; 4057713.09 north; 591427.93 east 2021-02-12T23:44:01Z- AHRS_M2 msg: 1613173441.45 epoch sec; seqNo:6328 2021-02-12T23:44:01Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.19 psi 2021-02-12T23:44:01Z- handleDepth()-Depth_Keller msg: 1613173441455 epoch sec; seqNo:6380; depth 19.40 2021-02-12T23:44:01Z- handleDvl() - RDI_Pathfinder msg: 1613173441.47 epoch sec; seqNo:2992 2021-02-12T23:44:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.82 , 97.98 , 78.51 , 104.06 2021-02-12T23:44:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.08 , -0.01 2021-02-12T23:44:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173441.48 epoch sec; seqNo:6380; 4057713.09 north; 591427.93 east 2021-02-12T23:44:01Z- AHRS_M2 msg: 1613173441.84 epoch sec; seqNo:6329 2021-02-12T23:44:01Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.19 psi 2021-02-12T23:44:01Z- handleDepth()-Depth_Keller msg: 1613173441844 epoch sec; seqNo:6381; depth 19.52 2021-02-12T23:44:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173441.86 epoch sec; seqNo:6381; 4057713.47 north; 591427.93 east 2021-02-12T23:44:02Z- AHRS_M2 msg: 1613173442.24 epoch sec; seqNo:6330 2021-02-12T23:44:02Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.19 psi 2021-02-12T23:44:02Z- handleDepth()-Depth_Keller msg: 1613173442250 epoch sec; seqNo:6382; depth 19.64 2021-02-12T23:44:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173442.26 epoch sec; seqNo:6382; 4057713.47 north; 591427.93 east 2021-02-12T23:44:02Z- AHRS_M2 msg: 1613173442.65 epoch sec; seqNo:6331 2021-02-12T23:44:02Z- AHRS_M2 msg: 0.69 phi; -0.32 theta; -0.19 psi 2021-02-12T23:44:02Z- handleDepth()-Depth_Keller msg: 1613173442652 epoch sec; seqNo:6383; depth 19.76 2021-02-12T23:44:02Z- handleDvl() - RDI_Pathfinder msg: 1613173442.67 epoch sec; seqNo:2993 2021-02-12T23:44:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 95.41 , 71.10 , 105.14 2021-02-12T23:44:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.09 , -0.06 2021-02-12T23:44:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173442.68 epoch sec; seqNo:6383; 4057713.86 north; 591429.15 east 2021-02-12T23:44:03Z- AHRS_M2 msg: 1613173443.05 epoch sec; seqNo:6332 2021-02-12T23:44:03Z- AHRS_M2 msg: 0.69 phi; -0.32 theta; -0.19 psi 2021-02-12T23:44:03Z- handleDepth()-Depth_Keller msg: 1613173443061 epoch sec; seqNo:6384; depth 19.89 2021-02-12T23:44:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173443.07 epoch sec; seqNo:6384; 4057714.24 north; 591429.14 east 2021-02-12T23:44:03Z- AHRS_M2 msg: 1613173443.45 epoch sec; seqNo:6333 2021-02-12T23:44:03Z- AHRS_M2 msg: 0.70 phi; -0.32 theta; -0.19 psi 2021-02-12T23:44:03Z- handleDepth()-Depth_Keller msg: 1613173443469 epoch sec; seqNo:6385; depth 20.00 2021-02-12T23:44:03Z- handleDvl() - RDI_Pathfinder msg: 1613173443.48 epoch sec; seqNo:2994 2021-02-12T23:44:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.82 , 97.98 , 70.69 , 104.06 2021-02-12T23:44:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.05 , -0.11 2021-02-12T23:44:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173443.49 epoch sec; seqNo:6385; 4057714.62 north; 591429.14 east 2021-02-12T23:44:03Z- AHRS_M2 msg: 1613173443.86 epoch sec; seqNo:6334 2021-02-12T23:44:03Z- AHRS_M2 msg: 0.70 phi; -0.32 theta; -0.18 psi 2021-02-12T23:44:03Z- handleDepth()-Depth_Keller msg: 1613173443864 epoch sec; seqNo:6386; depth 20.14 2021-02-12T23:44:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173443.90 epoch sec; seqNo:6386; 4057714.62 north; 591429.14 east 2021-02-12T23:44:04Z- AHRS_M2 msg: 1613173444.26 epoch sec; seqNo:6335 2021-02-12T23:44:04Z- AHRS_M2 msg: 0.71 phi; -0.32 theta; -0.18 psi 2021-02-12T23:44:04Z- handleDepth()-Depth_Keller msg: 1613173444268 epoch sec; seqNo:6387; depth 20.26 2021-02-12T23:44:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173444.28 epoch sec; seqNo:6387; 4057714.99 north; 591429.13 east 2021-02-12T23:44:04Z- AHRS_M2 msg: 1613173444.67 epoch sec; seqNo:6336 2021-02-12T23:44:04Z- AHRS_M2 msg: 0.71 phi; -0.32 theta; -0.18 psi 2021-02-12T23:44:04Z- handleDepth()-Depth_Keller msg: 1613173444672 epoch sec; seqNo:6388; depth 20.37 2021-02-12T23:44:04Z- handleDvl() - RDI_Pathfinder msg: 1613173444.69 epoch sec; seqNo:2995 2021-02-12T23:44:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.00 , 97.98 , 70.28 , 104.06 2021-02-12T23:44:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , -0.12 2021-02-12T23:44:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173444.70 epoch sec; seqNo:6388; 4057715.39 north; 591430.35 east 2021-02-12T23:44:05Z- AHRS_M2 msg: 1613173445.08 epoch sec; seqNo:6337 2021-02-12T23:44:05Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.18 psi 2021-02-12T23:44:05Z- handleDepth()-Depth_Keller msg: 1613173445087 epoch sec; seqNo:6389; depth 20.51 2021-02-12T23:44:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173445.10 epoch sec; seqNo:6389; 4057715.39 north; 591430.35 east 2021-02-12T23:44:05Z- AHRS_M2 msg: 1613173445.48 epoch sec; seqNo:6338 2021-02-12T23:44:05Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.19 psi 2021-02-12T23:44:05Z- handleDepth()-Depth_Keller msg: 1613173445484 epoch sec; seqNo:6390; depth 20.63 2021-02-12T23:44:05Z- handleDvl() - RDI_Pathfinder msg: 1613173445.50 epoch sec; seqNo:2996 2021-02-12T23:44:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.78 , 94.33 , 78.51 , 105.28 2021-02-12T23:44:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , -0.10 2021-02-12T23:44:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173445.51 epoch sec; seqNo:6390; 4057715.77 north; 591430.35 east 2021-02-12T23:44:05Z- AHRS_M2 msg: 1613173445.88 epoch sec; seqNo:6339 2021-02-12T23:44:05Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.19 psi 2021-02-12T23:44:05Z- handleDepth()-Depth_Keller msg: 1613173445884 epoch sec; seqNo:6391; depth 20.76 2021-02-12T23:44:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173445.92 epoch sec; seqNo:6391; 4057716.14 north; 591430.34 east 2021-02-12T23:44:06Z- LcmTrn::handleMessages() - handling 43 messages took 0.65 ms and 130 calls... 2021-02-12T23:44:06Z- waiting for 1613173444.03, time is 1613173445.92 2021-02-12T23:44:06Z- LcmTrn::updateTrn() >>>> pose time 1613173445.92, meas time 1613173445.50 <<<< 2021-02-12T23:44:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173445.50 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:44:06Z- TerrainNav::motionUpdate() - currEstimate = (4057716, 591430.34, 20.76) 2021-02-12T23:44:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:44:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:44:06Z- TNavBF::Projecting Measurements 2021-02-12T23:44:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:44:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:44:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:44:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:44:06Z- TNavFilter::4 good beams. 2021-02-12T23:44:06Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:44:06Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:44:06Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:44:06Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:44:06Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:44:06Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:44:06Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:44:06Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:06Z- TerrainMapOctree: (vehN, vehE ) = (4057715.85, 591430.14). 2021-02-12T23:44:06Z- TerrainMapOctree: Min Distance = 4521.27. 2021-02-12T23:44:06Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:44:06Z- There is no specific good beam that all particles have in common. 2021-02-12T23:44:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:44:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 8.95 2021-02-12T23:44:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:44:06Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 14.24 2021-02-12T23:44:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:44:06Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-12T23:44:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:44:06Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 17.73 2021-02-12T23:44:06Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:44:06Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:44:06Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:44:06Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:44:06Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:44:06Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:44:06Z- Filter 0 No beams used in subcloud update 2021-02-12T23:44:06Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:44:06Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:44:06Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 740.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:44:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173445.50 sec. 2021-02-12T23:44:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:44:06Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:06Z- variances([0], [2], and psi_berg) = 740.739916 833.024916 0.000000 2021-02-12T23:44:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:44:06Z- LcmTrn::publishEstimates() - MLE : 4057705.64 591388.84 20.76 2021-02-12T23:44:06Z- LcmTrn::publishEstimates() - MMSE : 4057716.14 591430.34 20.76 2021-02-12T23:44:06Z- LcmTrn::publishEstimates() - COVAR: 740.74 833.02 0.82 0.00 2021-02-12T23:44:06Z- AHRS_M2 msg: 1613173446.28 epoch sec; seqNo:6340 2021-02-12T23:44:06Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.20 psi 2021-02-12T23:44:06Z- handleDepth()-Depth_Keller msg: 1613173446288 epoch sec; seqNo:6392; depth 20.87 2021-02-12T23:44:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173446.30 epoch sec; seqNo:6392; 4057716.52 north; 591430.34 east 2021-02-12T23:44:06Z- AHRS_M2 msg: 1613173446.69 epoch sec; seqNo:6341 2021-02-12T23:44:06Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.20 psi 2021-02-12T23:44:06Z- handleDepth()-Depth_Keller msg: 1613173446693 epoch sec; seqNo:6393; depth 20.99 2021-02-12T23:44:06Z- handleDvl() - RDI_Pathfinder msg: 1613173446.71 epoch sec; seqNo:2997 2021-02-12T23:44:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 69.06 , 103.99 2021-02-12T23:44:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.09 , -0.04 2021-02-12T23:44:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173446.72 epoch sec; seqNo:6393; 4057716.52 north; 591430.34 east 2021-02-12T23:44:07Z- AHRS_M2 msg: 1613173447.09 epoch sec; seqNo:6342 2021-02-12T23:44:07Z- AHRS_M2 msg: 0.69 phi; -0.33 theta; -0.21 psi 2021-02-12T23:44:07Z- handleDepth()-Depth_Keller msg: 1613173447096 epoch sec; seqNo:6394; depth 21.12 2021-02-12T23:44:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173447.11 epoch sec; seqNo:6394; 4057716.91 north; 591431.55 east 2021-02-12T23:44:07Z- AHRS_M2 msg: 1613173447.49 epoch sec; seqNo:6343 2021-02-12T23:44:07Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.22 psi 2021-02-12T23:44:07Z- handleDepth()-Depth_Keller msg: 1613173447500 epoch sec; seqNo:6395; depth 21.24 2021-02-12T23:44:07Z- handleDvl() - RDI_Pathfinder msg: 1613173447.51 epoch sec; seqNo:2998 2021-02-12T23:44:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 94.33 , 68.65 , 104.06 2021-02-12T23:44:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , 0.01 2021-02-12T23:44:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173447.52 epoch sec; seqNo:6395; 4057717.29 north; 591431.55 east 2021-02-12T23:44:07Z- AHRS_M2 msg: 1613173447.90 epoch sec; seqNo:6344 2021-02-12T23:44:07Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.22 psi 2021-02-12T23:44:07Z- handleDepth()-Depth_Keller msg: 1613173447907 epoch sec; seqNo:6396; depth 21.35 2021-02-12T23:44:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173447.93 epoch sec; seqNo:6396; 4057717.67 north; 591431.55 east 2021-02-12T23:44:08Z- AHRS_M2 msg: 1613173448.30 epoch sec; seqNo:6345 2021-02-12T23:44:08Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.23 psi 2021-02-12T23:44:08Z- handleDepth()-Depth_Keller msg: 1613173448318 epoch sec; seqNo:6397; depth 21.50 2021-02-12T23:44:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173448.36 epoch sec; seqNo:6397; 4057717.67 north; 591431.55 east 2021-02-12T23:44:08Z- AHRS_M2 msg: 1613173448.71 epoch sec; seqNo:6346 2021-02-12T23:44:08Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.23 psi 2021-02-12T23:44:08Z- handleDepth()-Depth_Keller msg: 1613173448712 epoch sec; seqNo:6398; depth 21.60 2021-02-12T23:44:08Z- handleDvl() - RDI_Pathfinder msg: 1613173448.72 epoch sec; seqNo:2999 2021-02-12T23:44:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.14 , 95.61 , 0.00 , 102.91 2021-02-12T23:44:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , 0.04 2021-02-12T23:44:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173448.73 epoch sec; seqNo:6398; 4057718.05 north; 591431.54 east 2021-02-12T23:44:09Z- AHRS_M2 msg: 1613173449.11 epoch sec; seqNo:6347 2021-02-12T23:44:09Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.24 psi 2021-02-12T23:44:09Z- handleDepth()-Depth_Keller msg: 1613173449124 epoch sec; seqNo:6399; depth 21.73 2021-02-12T23:44:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173449.13 epoch sec; seqNo:6399; 4057718.43 north; 591431.54 east 2021-02-12T23:44:09Z- AHRS_M2 msg: 1613173449.54 epoch sec; seqNo:6348 2021-02-12T23:44:09Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.24 psi 2021-02-12T23:44:09Z- handleDepth()-Depth_Keller msg: 1613173449543 epoch sec; seqNo:6400; depth 21.86 2021-02-12T23:44:09Z- handleDvl() - RDI_Pathfinder msg: 1613173449.56 epoch sec; seqNo:3000 2021-02-12T23:44:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.14 , 97.71 , 69.46 , 106.22 2021-02-12T23:44:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.02 2021-02-12T23:44:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173449.57 epoch sec; seqNo:6400; 4057718.43 north; 591431.54 east 2021-02-12T23:44:09Z- AHRS_M2 msg: 1613173449.92 epoch sec; seqNo:6349 2021-02-12T23:44:09Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.24 psi 2021-02-12T23:44:09Z- handleDepth()-Depth_Keller msg: 1613173449924 epoch sec; seqNo:6401; depth 22.01 2021-02-12T23:44:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173449.93 epoch sec; seqNo:6401; 4057718.82 north; 591432.76 east 2021-02-12T23:44:10Z- AHRS_M2 msg: 1613173450.33 epoch sec; seqNo:6350 2021-02-12T23:44:10Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.24 psi 2021-02-12T23:44:10Z- handleDepth()-Depth_Keller msg: 1613173450333 epoch sec; seqNo:6402; depth 22.11 2021-02-12T23:44:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173450.36 epoch sec; seqNo:6402; 4057719.20 north; 591432.75 east 2021-02-12T23:44:10Z- AHRS_M2 msg: 1613173450.73 epoch sec; seqNo:6351 2021-02-12T23:44:10Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.24 psi 2021-02-12T23:44:10Z- handleDepth()-Depth_Keller msg: 1613173450741 epoch sec; seqNo:6403; depth 22.23 2021-02-12T23:44:10Z- handleDvl() - RDI_Pathfinder msg: 1613173450.76 epoch sec; seqNo:3001 2021-02-12T23:44:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.32 , 95.61 , 67.42 , 100.48 2021-02-12T23:44:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , -0.01 2021-02-12T23:44:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173450.77 epoch sec; seqNo:6403; 4057719.58 north; 591432.75 east 2021-02-12T23:44:11Z- AHRS_M2 msg: 1613173451.13 epoch sec; seqNo:6352 2021-02-12T23:44:11Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.24 psi 2021-02-12T23:44:11Z- handleDepth()-Depth_Keller msg: 1613173451141 epoch sec; seqNo:6404; depth 22.36 2021-02-12T23:44:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173451.16 epoch sec; seqNo:6404; 4057719.58 north; 591432.75 east 2021-02-12T23:44:11Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-12T23:44:11Z- waiting for 1613173448.92, time is 1613173451.16 2021-02-12T23:44:11Z- LcmTrn::updateTrn() >>>> pose time 1613173451.16, meas time 1613173450.76 <<<< 2021-02-12T23:44:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173450.76 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:44:11Z- TerrainNav::motionUpdate() - currEstimate = (4057720, 591432.75, 22.36) 2021-02-12T23:44:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:44:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:44:11Z- TNavBF::Projecting Measurements 2021-02-12T23:44:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:44:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:44:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:44:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:44:11Z- TNavFilter::4 good beams. 2021-02-12T23:44:11Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:44:11Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:44:11Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:44:11Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:44:11Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:44:11Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:44:11Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:44:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:11Z- TerrainMapOctree: (vehN, vehE ) = (4057719.31, 591432.56). 2021-02-12T23:44:11Z- TerrainMapOctree: Min Distance = 4517.04. 2021-02-12T23:44:11Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:44:11Z- There is no specific good beam that all particles have in common. 2021-02-12T23:44:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:44:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-12T23:44:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:44:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 14.63 2021-02-12T23:44:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:44:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 7.27 2021-02-12T23:44:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:44:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 16.15 2021-02-12T23:44:11Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:44:11Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:44:11Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:44:11Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:44:11Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:44:11Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:44:11Z- Filter 0 No beams used in subcloud update 2021-02-12T23:44:11Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:44:11Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:44:11Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 740.7 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:44:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173450.76 sec. 2021-02-12T23:44:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:44:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:11Z- variances([0], [2], and psi_berg) = 740.630422 833.024826 0.000000 2021-02-12T23:44:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:44:11Z- LcmTrn::publishEstimates() - MLE : 4057709.08 591391.25 22.36 2021-02-12T23:44:11Z- LcmTrn::publishEstimates() - MMSE : 4057719.58 591432.75 22.36 2021-02-12T23:44:11Z- LcmTrn::publishEstimates() - COVAR: 740.63 833.02 0.78 0.00 2021-02-12T23:44:11Z- AHRS_M2 msg: 1613173451.54 epoch sec; seqNo:6353 2021-02-12T23:44:11Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.24 psi 2021-02-12T23:44:11Z- handleDepth()-Depth_Keller msg: 1613173451540 epoch sec; seqNo:6405; depth 22.48 2021-02-12T23:44:11Z- handleDvl() - RDI_Pathfinder msg: 1613173451.55 epoch sec; seqNo:3002 2021-02-12T23:44:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.32 , 98.25 , 78.78 , 101.90 2021-02-12T23:44:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.00 , -0.00 2021-02-12T23:44:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173451.57 epoch sec; seqNo:6405; 4057719.96 north; 591432.74 east 2021-02-12T23:44:11Z- AHRS_M2 msg: 1613173451.95 epoch sec; seqNo:6354 2021-02-12T23:44:11Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.24 psi 2021-02-12T23:44:11Z- handleDepth()-Depth_Keller msg: 1613173451950 epoch sec; seqNo:6406; depth 22.60 2021-02-12T23:44:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173451.96 epoch sec; seqNo:6406; 4057720.34 north; 591432.74 east 2021-02-12T23:44:12Z- AHRS_M2 msg: 1613173452.37 epoch sec; seqNo:6355 2021-02-12T23:44:12Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.23 psi 2021-02-12T23:44:12Z- handleDepth()-Depth_Keller msg: 1613173452377 epoch sec; seqNo:6407; depth 22.72 2021-02-12T23:44:12Z- handleDvl() - RDI_Pathfinder msg: 1613173452.39 epoch sec; seqNo:3003 2021-02-12T23:44:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.73 , 95.48 , 67.83 , 102.78 2021-02-12T23:44:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.01 , 0.02 2021-02-12T23:44:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173452.40 epoch sec; seqNo:6407; 4057720.35 north; 591433.96 east 2021-02-12T23:44:12Z- AHRS_M2 msg: 1613173452.75 epoch sec; seqNo:6356 2021-02-12T23:44:12Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.23 psi 2021-02-12T23:44:12Z- handleDepth()-Depth_Keller msg: 1613173452752 epoch sec; seqNo:6408; depth 22.85 2021-02-12T23:44:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173452.76 epoch sec; seqNo:6408; 4057720.73 north; 591433.96 east 2021-02-12T23:44:13Z- AHRS_M2 msg: 1613173453.15 epoch sec; seqNo:6357 2021-02-12T23:44:13Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.23 psi 2021-02-12T23:44:13Z- handleDepth()-Depth_Keller msg: 1613173453157 epoch sec; seqNo:6409; depth 22.95 2021-02-12T23:44:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173453.17 epoch sec; seqNo:6409; 4057721.11 north; 591433.95 east 2021-02-12T23:44:13Z- AHRS_M2 msg: 1613173453.56 epoch sec; seqNo:6358 2021-02-12T23:44:13Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.22 psi 2021-02-12T23:44:13Z- handleDepth()-Depth_Keller msg: 1613173453565 epoch sec; seqNo:6410; depth 23.06 2021-02-12T23:44:13Z- handleDvl() - RDI_Pathfinder msg: 1613173453.58 epoch sec; seqNo:3004 2021-02-12T23:44:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 94.53 , 67.42 , 100.61 2021-02-12T23:44:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.05 , 0.02 2021-02-12T23:44:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173453.59 epoch sec; seqNo:6410; 4057721.11 north; 591433.95 east 2021-02-12T23:44:13Z- AHRS_M2 msg: 1613173453.96 epoch sec; seqNo:6359 2021-02-12T23:44:13Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.22 psi 2021-02-12T23:44:13Z- handleDepth()-Depth_Keller msg: 1613173453973 epoch sec; seqNo:6411; depth 23.20 2021-02-12T23:44:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173453.98 epoch sec; seqNo:6411; 4057721.49 north; 591433.95 east 2021-02-12T23:44:14Z- AHRS_M2 msg: 1613173454.37 epoch sec; seqNo:6360 2021-02-12T23:44:14Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.21 psi 2021-02-12T23:44:14Z- handleDepth()-Depth_Keller msg: 1613173454369 epoch sec; seqNo:6412; depth 23.31 2021-02-12T23:44:14Z- handleDvl() - RDI_Pathfinder msg: 1613173454.39 epoch sec; seqNo:3005 2021-02-12T23:44:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.18 , 68.24 , 100.61 2021-02-12T23:44:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.08 , -0.00 2021-02-12T23:44:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173454.40 epoch sec; seqNo:6412; 4057721.87 north; 591433.94 east 2021-02-12T23:44:14Z- AHRS_M2 msg: 1613173454.77 epoch sec; seqNo:6361 2021-02-12T23:44:14Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.21 psi 2021-02-12T23:44:14Z- handleDepth()-Depth_Keller msg: 1613173454777 epoch sec; seqNo:6413; depth 23.42 2021-02-12T23:44:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173454.79 epoch sec; seqNo:6413; 4057721.88 north; 591435.17 east 2021-02-12T23:44:15Z- AHRS_M2 msg: 1613173455.17 epoch sec; seqNo:6362 2021-02-12T23:44:15Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.20 psi 2021-02-12T23:44:15Z- handleDepth()-Depth_Keller msg: 1613173455179 epoch sec; seqNo:6414; depth 23.54 2021-02-12T23:44:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173455.19 epoch sec; seqNo:6414; 4057722.26 north; 591435.16 east 2021-02-12T23:44:15Z- AHRS_M2 msg: 1613173455.58 epoch sec; seqNo:6363 2021-02-12T23:44:15Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.20 psi 2021-02-12T23:44:15Z- handleDepth()-Depth_Keller msg: 1613173455582 epoch sec; seqNo:6415; depth 23.65 2021-02-12T23:44:15Z- handleDvl() - RDI_Pathfinder msg: 1613173455.59 epoch sec; seqNo:3006 2021-02-12T23:44:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 91.76 , 65.38 , 101.49 2021-02-12T23:44:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.08 , -0.05 2021-02-12T23:44:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173455.60 epoch sec; seqNo:6415; 4057722.64 north; 591435.16 east 2021-02-12T23:44:15Z- AHRS_M2 msg: 1613173455.98 epoch sec; seqNo:6364 2021-02-12T23:44:15Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.20 psi 2021-02-12T23:44:15Z- handleDepth()-Depth_Keller msg: 1613173455984 epoch sec; seqNo:6416; depth 23.79 2021-02-12T23:44:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173456.00 epoch sec; seqNo:6416; 4057723.02 north; 591435.15 east 2021-02-12T23:44:16Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-12T23:44:16Z- waiting for 1613173454.16, time is 1613173456.00 2021-02-12T23:44:16Z- LcmTrn::updateTrn() >>>> pose time 1613173456.00, meas time 1613173455.59 <<<< 2021-02-12T23:44:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173455.59 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:44:16Z- TerrainNav::motionUpdate() - currEstimate = (4057723, 591435.15, 23.79) 2021-02-12T23:44:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:44:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:44:16Z- TNavBF::Projecting Measurements 2021-02-12T23:44:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:44:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:44:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:44:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:44:16Z- TNavFilter::4 good beams. 2021-02-12T23:44:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:44:16Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:44:16Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:44:16Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:44:16Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:44:16Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:44:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:44:16Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:16Z- TerrainMapOctree: (vehN, vehE ) = (4057722.73, 591434.95). 2021-02-12T23:44:16Z- TerrainMapOctree: Min Distance = 4512.89. 2021-02-12T23:44:16Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:44:16Z- There is no specific good beam that all particles have in common. 2021-02-12T23:44:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:44:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 8.27 2021-02-12T23:44:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:44:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 13.47 2021-02-12T23:44:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:44:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-12T23:44:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:44:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.76 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 16.48 2021-02-12T23:44:16Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:44:16Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:44:16Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:44:16Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:44:16Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:44:16Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:44:16Z- Filter 0 No beams used in subcloud update 2021-02-12T23:44:16Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:44:16Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:44:16Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 740.5 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:44:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173455.59 sec. 2021-02-12T23:44:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:44:16Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:16Z- variances([0], [2], and psi_berg) = 740.520275 833.024735 0.000000 2021-02-12T23:44:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:44:16Z- LcmTrn::publishEstimates() - MLE : 4057712.52 591393.65 23.79 2021-02-12T23:44:16Z- LcmTrn::publishEstimates() - MMSE : 4057723.02 591435.15 23.79 2021-02-12T23:44:16Z- LcmTrn::publishEstimates() - COVAR: 740.52 833.02 0.75 0.00 2021-02-12T23:44:16Z- AHRS_M2 msg: 1613173456.39 epoch sec; seqNo:6365 2021-02-12T23:44:16Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.20 psi 2021-02-12T23:44:16Z- handleDepth()-Depth_Keller msg: 1613173456399 epoch sec; seqNo:6417; depth 23.88 2021-02-12T23:44:16Z- handleDvl() - RDI_Pathfinder msg: 1613173456.42 epoch sec; seqNo:3007 2021-02-12T23:44:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 89.87 , 117.85 , 99.60 2021-02-12T23:44:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , 0.04 , -0.03 2021-02-12T23:44:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173456.43 epoch sec; seqNo:6417; 4057723.02 north; 591435.15 east 2021-02-12T23:44:16Z- AHRS_M2 msg: 1613173456.79 epoch sec; seqNo:6366 2021-02-12T23:44:16Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.19 psi 2021-02-12T23:44:16Z- handleDepth()-Depth_Keller msg: 1613173456792 epoch sec; seqNo:6418; depth 24.03 2021-02-12T23:44:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173456.80 epoch sec; seqNo:6418; 4057723.39 north; 591435.15 east 2021-02-12T23:44:17Z- AHRS_M2 msg: 1613173457.19 epoch sec; seqNo:6367 2021-02-12T23:44:17Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.19 psi 2021-02-12T23:44:17Z- handleDepth()-Depth_Keller msg: 1613173457198 epoch sec; seqNo:6419; depth 24.13 2021-02-12T23:44:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173457.21 epoch sec; seqNo:6419; 4057723.39 north; 591435.15 east 2021-02-12T23:44:17Z- AHRS_M2 msg: 1613173457.60 epoch sec; seqNo:6368 2021-02-12T23:44:17Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.19 psi 2021-02-12T23:44:17Z- handleDepth()-Depth_Keller msg: 1613173457605 epoch sec; seqNo:6420; depth 24.25 2021-02-12T23:44:17Z- handleDvl() - RDI_Pathfinder msg: 1613173457.62 epoch sec; seqNo:3008 2021-02-12T23:44:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.10 , 90.75 , 64.97 , 98.86 2021-02-12T23:44:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.02 , -0.11 2021-02-12T23:44:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173457.64 epoch sec; seqNo:6420; 4057723.79 north; 591436.37 east 2021-02-12T23:44:18Z- AHRS_M2 msg: 1613173458.00 epoch sec; seqNo:6369 2021-02-12T23:44:18Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.19 psi 2021-02-12T23:44:18Z- handleDepth()-Depth_Keller msg: 1613173458005 epoch sec; seqNo:6421; depth 24.37 2021-02-12T23:44:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173458.02 epoch sec; seqNo:6421; 4057724.16 north; 591436.36 east 2021-02-12T23:44:18Z- AHRS_M2 msg: 1613173458.40 epoch sec; seqNo:6370 2021-02-12T23:44:18Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.19 psi 2021-02-12T23:44:18Z- handleDepth()-Depth_Keller msg: 1613173458408 epoch sec; seqNo:6422; depth 24.48 2021-02-12T23:44:18Z- handleDvl() - RDI_Pathfinder msg: 1613173458.43 epoch sec; seqNo:3009 2021-02-12T23:44:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.10 , 97.84 , 65.79 , 96.76 2021-02-12T23:44:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , -0.09 2021-02-12T23:44:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173458.44 epoch sec; seqNo:6422; 4057724.54 north; 591436.36 east 2021-02-12T23:44:18Z- AHRS_M2 msg: 1613173458.81 epoch sec; seqNo:6371 2021-02-12T23:44:18Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.19 psi 2021-02-12T23:44:18Z- handleDepth()-Depth_Keller msg: 1613173458812 epoch sec; seqNo:6423; depth 24.61 2021-02-12T23:44:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173458.82 epoch sec; seqNo:6423; 4057724.92 north; 591436.35 east 2021-02-12T23:44:19Z- AHRS_M2 msg: 1613173459.21 epoch sec; seqNo:6372 2021-02-12T23:44:19Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.19 psi 2021-02-12T23:44:19Z- handleDepth()-Depth_Keller msg: 1613173459228 epoch sec; seqNo:6424; depth 24.73 2021-02-12T23:44:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173459.26 epoch sec; seqNo:6424; 4057724.92 north; 591436.35 east 2021-02-12T23:44:19Z- AHRS_M2 msg: 1613173459.62 epoch sec; seqNo:6373 2021-02-12T23:44:19Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.19 psi 2021-02-12T23:44:19Z- handleDepth()-Depth_Keller msg: 1613173459629 epoch sec; seqNo:6425; depth 24.85 2021-02-12T23:44:19Z- handleDvl() - RDI_Pathfinder msg: 1613173459.64 epoch sec; seqNo:3010 2021-02-12T23:44:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.69 , 0.00 , 66.60 , 98.38 2021-02-12T23:44:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.12 , -0.02 2021-02-12T23:44:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173459.65 epoch sec; seqNo:6425; 4057725.30 north; 591436.35 east 2021-02-12T23:44:20Z- AHRS_M2 msg: 1613173460.02 epoch sec; seqNo:6374 2021-02-12T23:44:20Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.19 psi 2021-02-12T23:44:20Z- handleDepth()-Depth_Keller msg: 1613173460024 epoch sec; seqNo:6426; depth 24.97 2021-02-12T23:44:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173460.04 epoch sec; seqNo:6426; 4057725.68 north; 591436.35 east 2021-02-12T23:44:20Z- AHRS_M2 msg: 1613173460.42 epoch sec; seqNo:6375 2021-02-12T23:44:20Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.19 psi 2021-02-12T23:44:20Z- handleDepth()-Depth_Keller msg: 1613173460433 epoch sec; seqNo:6427; depth 25.07 2021-02-12T23:44:20Z- handleDvl() - RDI_Pathfinder msg: 1613173460.44 epoch sec; seqNo:3011 2021-02-12T23:44:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.69 , 89.26 , 65.38 , 97.91 2021-02-12T23:44:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.12 , 0.04 2021-02-12T23:44:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173460.45 epoch sec; seqNo:6427; 4057726.07 north; 591437.56 east 2021-02-12T23:44:20Z- AHRS_M2 msg: 1613173460.83 epoch sec; seqNo:6376 2021-02-12T23:44:20Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.19 psi 2021-02-12T23:44:20Z- handleDepth()-Depth_Keller msg: 1613173460832 epoch sec; seqNo:6428; depth 25.20 2021-02-12T23:44:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173460.85 epoch sec; seqNo:6428; 4057726.07 north; 591437.56 east 2021-02-12T23:44:21Z- AHRS_M2 msg: 1613173461.23 epoch sec; seqNo:6377 2021-02-12T23:44:21Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-12T23:44:21Z- handleDepth()-Depth_Keller msg: 1613173461237 epoch sec; seqNo:6429; depth 25.31 2021-02-12T23:44:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173461.24 epoch sec; seqNo:6429; 4057726.45 north; 591437.56 east 2021-02-12T23:44:21Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-12T23:44:21Z- waiting for 1613173459.00, time is 1613173461.24 2021-02-12T23:44:21Z- LcmTrn::updateTrn() >>>> pose time 1613173461.24, meas time 1613173460.44 <<<< 2021-02-12T23:44:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173460.44 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:44:21Z- TerrainNav::motionUpdate() - currEstimate = (4057726, 591437.56, 25.31) 2021-02-12T23:44:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:44:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:44:21Z- TNavBF::Projecting Measurements 2021-02-12T23:44:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:44:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:44:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:44:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:44:21Z- TNavFilter::4 good beams. 2021-02-12T23:44:21Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:44:21Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:44:21Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:44:21Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:44:21Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:44:21Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:44:21Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:44:21Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:21Z- TerrainMapOctree: (vehN, vehE ) = (4057725.93, 591437.19). 2021-02-12T23:44:21Z- TerrainMapOctree: Min Distance = 4508.99. 2021-02-12T23:44:21Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:44:21Z- There is no specific good beam that all particles have in common. 2021-02-12T23:44:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:44:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-12T23:44:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:44:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 12.75 2021-02-12T23:44:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:44:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-12T23:44:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:44:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.73 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-12T23:44:21Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:44:21Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:44:21Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:44:21Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:44:21Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:44:21Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:44:21Z- Filter 0 No beams used in subcloud update 2021-02-12T23:44:21Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:44:21Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:44:21Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 740.4 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:44:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173460.44 sec. 2021-02-12T23:44:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:44:21Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:21Z- variances([0], [2], and psi_berg) = 740.404666 833.024639 0.000000 2021-02-12T23:44:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:44:21Z- LcmTrn::publishEstimates() - MLE : 4057715.95 591396.06 25.31 2021-02-12T23:44:21Z- LcmTrn::publishEstimates() - MMSE : 4057726.45 591437.56 25.31 2021-02-12T23:44:21Z- LcmTrn::publishEstimates() - COVAR: 740.40 833.02 0.74 0.00 2021-02-12T23:44:21Z- AHRS_M2 msg: 1613173461.64 epoch sec; seqNo:6378 2021-02-12T23:44:21Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.18 psi 2021-02-12T23:44:21Z- handleDepth()-Depth_Keller msg: 1613173461641 epoch sec; seqNo:6430; depth 25.42 2021-02-12T23:44:21Z- handleDvl() - RDI_Pathfinder msg: 1613173461.66 epoch sec; seqNo:3012 2021-02-12T23:44:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.69 , 88.11 , 66.19 , 97.84 2021-02-12T23:44:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.10 , 0.08 2021-02-12T23:44:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173461.67 epoch sec; seqNo:6430; 4057726.83 north; 591437.55 east 2021-02-12T23:44:22Z- AHRS_M2 msg: 1613173462.04 epoch sec; seqNo:6379 2021-02-12T23:44:22Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.18 psi 2021-02-12T23:44:22Z- handleDepth()-Depth_Keller msg: 1613173462051 epoch sec; seqNo:6431; depth 25.53 2021-02-12T23:44:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173462.09 epoch sec; seqNo:6431; 4057727.21 north; 591437.55 east 2021-02-12T23:44:22Z- AHRS_M2 msg: 1613173462.45 epoch sec; seqNo:6380 2021-02-12T23:44:22Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.17 psi 2021-02-12T23:44:22Z- handleDepth()-Depth_Keller msg: 1613173462454 epoch sec; seqNo:6432; depth 25.66 2021-02-12T23:44:22Z- handleDvl() - RDI_Pathfinder msg: 1613173462.47 epoch sec; seqNo:3013 2021-02-12T23:44:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.28 , 0.00 , 65.79 , 97.91 2021-02-12T23:44:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , 0.08 2021-02-12T23:44:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173462.49 epoch sec; seqNo:6432; 4057727.21 north; 591437.55 east 2021-02-12T23:44:22Z- AHRS_M2 msg: 1613173462.85 epoch sec; seqNo:6381 2021-02-12T23:44:22Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.17 psi 2021-02-12T23:44:22Z- handleDepth()-Depth_Keller msg: 1613173462853 epoch sec; seqNo:6433; depth 25.78 2021-02-12T23:44:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173462.86 epoch sec; seqNo:6433; 4057727.59 north; 591437.55 east 2021-02-12T23:44:23Z- AHRS_M2 msg: 1613173463.25 epoch sec; seqNo:6382 2021-02-12T23:44:23Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.17 psi 2021-02-12T23:44:23Z- handleDepth()-Depth_Keller msg: 1613173463258 epoch sec; seqNo:6434; depth 25.88 2021-02-12T23:44:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173463.27 epoch sec; seqNo:6434; 4057727.97 north; 591437.54 east 2021-02-12T23:44:23Z- AHRS_M2 msg: 1613173463.66 epoch sec; seqNo:6383 2021-02-12T23:44:23Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.17 psi 2021-02-12T23:44:23Z- handleDepth()-Depth_Keller msg: 1613173463660 epoch sec; seqNo:6435; depth 25.99 2021-02-12T23:44:23Z- handleDvl() - RDI_Pathfinder msg: 1613173463.68 epoch sec; seqNo:3014 2021-02-12T23:44:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.69 , 86.01 , 64.56 , 97.91 2021-02-12T23:44:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , 0.05 2021-02-12T23:44:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173463.69 epoch sec; seqNo:6435; 4057728.36 north; 591438.76 east 2021-02-12T23:44:24Z- AHRS_M2 msg: 1613173464.06 epoch sec; seqNo:6384 2021-02-12T23:44:24Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.16 psi 2021-02-12T23:44:24Z- handleDepth()-Depth_Keller msg: 1613173464064 epoch sec; seqNo:6436; depth 26.10 2021-02-12T23:44:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173464.08 epoch sec; seqNo:6436; 4057728.36 north; 591438.76 east 2021-02-12T23:44:24Z- AHRS_M2 msg: 1613173464.46 epoch sec; seqNo:6385 2021-02-12T23:44:24Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.16 psi 2021-02-12T23:44:24Z- handleDepth()-Depth_Keller msg: 1613173464472 epoch sec; seqNo:6437; depth 26.23 2021-02-12T23:44:24Z- handleDvl() - RDI_Pathfinder msg: 1613173464.50 epoch sec; seqNo:3015 2021-02-12T23:44:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 85.00 , 64.57 , 97.98 2021-02-12T23:44:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , 0.01 2021-02-12T23:44:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173464.54 epoch sec; seqNo:6437; 4057728.74 north; 591438.75 east 2021-02-12T23:44:24Z- AHRS_M2 msg: 1613173464.87 epoch sec; seqNo:6386 2021-02-12T23:44:24Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.15 psi 2021-02-12T23:44:24Z- handleDepth()-Depth_Keller msg: 1613173464874 epoch sec; seqNo:6438; depth 26.35 2021-02-12T23:44:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173464.88 epoch sec; seqNo:6438; 4057729.12 north; 591438.75 east 2021-02-12T23:44:25Z- AHRS_M2 msg: 1613173465.27 epoch sec; seqNo:6387 2021-02-12T23:44:25Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.15 psi 2021-02-12T23:44:25Z- handleDepth()-Depth_Keller msg: 1613173465276 epoch sec; seqNo:6439; depth 26.45 2021-02-12T23:44:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173465.29 epoch sec; seqNo:6439; 4057729.12 north; 591438.75 east 2021-02-12T23:44:25Z- AHRS_M2 msg: 1613173465.68 epoch sec; seqNo:6388 2021-02-12T23:44:25Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.15 psi 2021-02-12T23:44:25Z- handleDepth()-Depth_Keller msg: 1613173465680 epoch sec; seqNo:6440; depth 26.56 2021-02-12T23:44:25Z- handleDvl() - RDI_Pathfinder msg: 1613173465.69 epoch sec; seqNo:3016 2021-02-12T23:44:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 83.79 , 63.34 , 97.85 2021-02-12T23:44:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.00 , -0.02 2021-02-12T23:44:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173465.71 epoch sec; seqNo:6440; 4057729.49 north; 591438.75 east 2021-02-12T23:44:26Z- AHRS_M2 msg: 1613173466.08 epoch sec; seqNo:6389 2021-02-12T23:44:26Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.15 psi 2021-02-12T23:44:26Z- handleDepth()-Depth_Keller msg: 1613173466084 epoch sec; seqNo:6441; depth 26.68 2021-02-12T23:44:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173466.10 epoch sec; seqNo:6441; 4057729.89 north; 591439.96 east 2021-02-12T23:44:26Z- AHRS_M2 msg: 1613173466.49 epoch sec; seqNo:6390 2021-02-12T23:44:26Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.15 psi 2021-02-12T23:44:26Z- LcmTrn::handleMessages() - handling 42 messages took 0.70 ms and 128 calls... 2021-02-12T23:44:26Z- waiting for 1613173464.24, time is 1613173466.49 2021-02-12T23:44:26Z- LcmTrn::updateTrn() >>>> pose time 1613173466.49, meas time 1613173465.69 <<<< 2021-02-12T23:44:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173465.69 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:44:26Z- TerrainNav::motionUpdate() - currEstimate = (4057730, 591439.96, 26.68) 2021-02-12T23:44:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:44:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:44:26Z- TNavBF::Projecting Measurements 2021-02-12T23:44:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:44:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:44:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:44:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:44:26Z- TNavFilter::4 good beams. 2021-02-12T23:44:26Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:44:26Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:44:26Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:44:26Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:44:26Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:44:26Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:44:26Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:44:26Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:26Z- TerrainMapOctree: (vehN, vehE ) = (4057729.37, 591439.60). 2021-02-12T23:44:26Z- TerrainMapOctree: Min Distance = 4504.80. 2021-02-12T23:44:26Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:44:26Z- There is no specific good beam that all particles have in common. 2021-02-12T23:44:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:44:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 9.83 2021-02-12T23:44:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:44:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 11.23 2021-02-12T23:44:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:44:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 6.42 2021-02-12T23:44:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:44:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.73 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-12T23:44:26Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:44:26Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:44:26Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:44:26Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:44:26Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:44:26Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:44:26Z- Filter 0 No beams used in subcloud update 2021-02-12T23:44:26Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:44:26Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:44:26Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 740.3 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:44:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173465.69 sec. 2021-02-12T23:44:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:44:26Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:26Z- variances([0], [2], and psi_berg) = 740.289297 833.024544 0.000000 2021-02-12T23:44:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:44:26Z- LcmTrn::publishEstimates() - MLE : 4057719.39 591398.46 26.68 2021-02-12T23:44:26Z- LcmTrn::publishEstimates() - MMSE : 4057729.89 591439.96 26.68 2021-02-12T23:44:26Z- LcmTrn::publishEstimates() - COVAR: 740.29 833.02 0.73 0.00 2021-02-12T23:44:26Z- handleDepth()-Depth_Keller msg: 1613173466490 epoch sec; seqNo:6442; depth 26.78 2021-02-12T23:44:26Z- handleDvl() - RDI_Pathfinder msg: 1613173466.50 epoch sec; seqNo:3017 2021-02-12T23:44:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.28 , 84.87 , 62.93 , 97.84 2021-02-12T23:44:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , -0.04 2021-02-12T23:44:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173466.51 epoch sec; seqNo:6442; 4057730.27 north; 591439.96 east 2021-02-12T23:44:26Z- AHRS_M2 msg: 1613173466.89 epoch sec; seqNo:6391 2021-02-12T23:44:26Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.15 psi 2021-02-12T23:44:26Z- handleDepth()-Depth_Keller msg: 1613173466892 epoch sec; seqNo:6443; depth 26.90 2021-02-12T23:44:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173466.91 epoch sec; seqNo:6443; 4057730.27 north; 591439.96 east 2021-02-12T23:44:27Z- AHRS_M2 msg: 1613173467.30 epoch sec; seqNo:6392 2021-02-12T23:44:27Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.15 psi 2021-02-12T23:44:27Z- handleDepth()-Depth_Keller msg: 1613173467310 epoch sec; seqNo:6444; depth 27.02 2021-02-12T23:44:27Z- handleDvl() - RDI_Pathfinder msg: 1613173467.35 epoch sec; seqNo:3018 2021-02-12T23:44:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.46 , 84.93 , 63.33 , 97.91 2021-02-12T23:44:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.04 , -0.03 2021-02-12T23:44:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173467.36 epoch sec; seqNo:6444; 4057730.64 north; 591439.96 east 2021-02-12T23:44:27Z- AHRS_M2 msg: 1613173467.70 epoch sec; seqNo:6393 2021-02-12T23:44:27Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.15 psi 2021-02-12T23:44:27Z- handleDepth()-Depth_Keller msg: 1613173467709 epoch sec; seqNo:6445; depth 27.14 2021-02-12T23:44:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173467.72 epoch sec; seqNo:6445; 4057731.02 north; 591439.95 east 2021-02-12T23:44:28Z- AHRS_M2 msg: 1613173468.10 epoch sec; seqNo:6394 2021-02-12T23:44:28Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.16 psi 2021-02-12T23:44:28Z- handleDepth()-Depth_Keller msg: 1613173468106 epoch sec; seqNo:6446; depth 27.24 2021-02-12T23:44:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173468.12 epoch sec; seqNo:6446; 4057731.02 north; 591439.95 east 2021-02-12T23:44:28Z- AHRS_M2 msg: 1613173468.50 epoch sec; seqNo:6395 2021-02-12T23:44:28Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.16 psi 2021-02-12T23:44:28Z- handleDepth()-Depth_Keller msg: 1613173468508 epoch sec; seqNo:6447; depth 27.35 2021-02-12T23:44:28Z- handleDvl() - RDI_Pathfinder msg: 1613173468.53 epoch sec; seqNo:3019 2021-02-12T23:44:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.87 , 83.85 , 62.52 , 97.91 2021-02-12T23:44:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , -0.04 2021-02-12T23:44:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173468.54 epoch sec; seqNo:6447; 4057731.41 north; 591441.17 east 2021-02-12T23:44:28Z- AHRS_M2 msg: 1613173468.91 epoch sec; seqNo:6396 2021-02-12T23:44:28Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.16 psi 2021-02-12T23:44:28Z- handleDepth()-Depth_Keller msg: 1613173468912 epoch sec; seqNo:6448; depth 27.47 2021-02-12T23:44:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173468.92 epoch sec; seqNo:6448; 4057731.79 north; 591441.16 east 2021-02-12T23:44:29Z- AHRS_M2 msg: 1613173469.33 epoch sec; seqNo:6397 2021-02-12T23:44:29Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.17 psi 2021-02-12T23:44:29Z- handleDepth()-Depth_Keller msg: 1613173469341 epoch sec; seqNo:6449; depth 27.59 2021-02-12T23:44:29Z- handleDvl() - RDI_Pathfinder msg: 1613173469.36 epoch sec; seqNo:3020 2021-02-12T23:44:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.87 , 83.85 , 62.11 , 97.91 2021-02-12T23:44:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , -0.02 2021-02-12T23:44:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173469.37 epoch sec; seqNo:6449; 4057731.79 north; 591441.16 east 2021-02-12T23:44:29Z- AHRS_M2 msg: 1613173469.72 epoch sec; seqNo:6398 2021-02-12T23:44:29Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.17 psi 2021-02-12T23:44:29Z- handleDepth()-Depth_Keller msg: 1613173469721 epoch sec; seqNo:6450; depth 27.73 2021-02-12T23:44:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173469.73 epoch sec; seqNo:6450; 4057732.17 north; 591441.16 east 2021-02-12T23:44:30Z- AHRS_M2 msg: 1613173470.12 epoch sec; seqNo:6399 2021-02-12T23:44:30Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.17 psi 2021-02-12T23:44:30Z- handleDepth()-Depth_Keller msg: 1613173470131 epoch sec; seqNo:6451; depth 27.84 2021-02-12T23:44:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173470.15 epoch sec; seqNo:6451; 4057732.55 north; 591441.16 east 2021-02-12T23:44:30Z- AHRS_M2 msg: 1613173470.52 epoch sec; seqNo:6400 2021-02-12T23:44:30Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.18 psi 2021-02-12T23:44:30Z- handleDepth()-Depth_Keller msg: 1613173470533 epoch sec; seqNo:6452; depth 27.96 2021-02-12T23:44:30Z- handleDvl() - RDI_Pathfinder msg: 1613173470.55 epoch sec; seqNo:3021 2021-02-12T23:44:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 134.76 , 62.12 , 96.90 2021-02-12T23:44:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.14 , -0.05 2021-02-12T23:44:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173470.56 epoch sec; seqNo:6452; 4057732.56 north; 591442.38 east 2021-02-12T23:44:30Z- AHRS_M2 msg: 1613173470.93 epoch sec; seqNo:6401 2021-02-12T23:44:30Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.18 psi 2021-02-12T23:44:30Z- handleDepth()-Depth_Keller msg: 1613173470933 epoch sec; seqNo:6453; depth 28.09 2021-02-12T23:44:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173470.94 epoch sec; seqNo:6453; 4057732.94 north; 591442.37 east 2021-02-12T23:44:31Z- AHRS_M2 msg: 1613173471.33 epoch sec; seqNo:6402 2021-02-12T23:44:31Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.19 psi 2021-02-12T23:44:31Z- handleDepth()-Depth_Keller msg: 1613173471360 epoch sec; seqNo:6454; depth 28.21 2021-02-12T23:44:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173471.38 epoch sec; seqNo:6454; 4057733.32 north; 591442.37 east 2021-02-12T23:44:31Z- LcmTrn::handleMessages() - handling 43 messages took 0.71 ms and 129 calls... 2021-02-12T23:44:31Z- waiting for 1613173469.49, time is 1613173471.38 2021-02-12T23:44:31Z- LcmTrn::updateTrn() >>>> pose time 1613173471.38, meas time 1613173470.55 <<<< 2021-02-12T23:44:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173470.55 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:44:31Z- TerrainNav::motionUpdate() - currEstimate = (4057733, 591442.37, 28.21) 2021-02-12T23:44:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:44:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:44:31Z- TNavBF::Projecting Measurements 2021-02-12T23:44:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:44:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:44:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:44:31Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:44:31Z- TNavFilter::3 good beams. 2021-02-12T23:44:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:44:31Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:44:31Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:44:31Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:44:31Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:44:31Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:44:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:44:31Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:31Z- TerrainMapOctree: (vehN, vehE ) = (4057732.74, 591441.96). 2021-02-12T23:44:31Z- TerrainMapOctree: Min Distance = 4500.69. 2021-02-12T23:44:31Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:44:31Z- There is no specific good beam that all particles have in common. 2021-02-12T23:44:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:44:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 7.72 2021-02-12T23:44:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:44:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 6.17 2021-02-12T23:44:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:44:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.75 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 15.02 2021-02-12T23:44:31Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:44:31Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:44:31Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:44:31Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:44:31Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:44:31Z- Filter 0 No beams used in subcloud update 2021-02-12T23:44:31Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:44:31Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:44:31Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 740.2 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:44:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173470.55 sec. 2021-02-12T23:44:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:44:31Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:31Z- variances([0], [2], and psi_berg) = 740.172918 833.024448 0.000000 2021-02-12T23:44:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:44:31Z- LcmTrn::publishEstimates() - MLE : 4057723.82 591394.87 28.21 2021-02-12T23:44:31Z- LcmTrn::publishEstimates() - MMSE : 4057733.32 591442.37 28.21 2021-02-12T23:44:31Z- LcmTrn::publishEstimates() - COVAR: 740.17 833.02 0.75 0.00 2021-02-12T23:44:31Z- AHRS_M2 msg: 1613173471.74 epoch sec; seqNo:6403 2021-02-12T23:44:31Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.19 psi 2021-02-12T23:44:31Z- handleDepth()-Depth_Keller msg: 1613173471740 epoch sec; seqNo:6455; depth 28.34 2021-02-12T23:44:31Z- handleDvl() - RDI_Pathfinder msg: 1613173471.75 epoch sec; seqNo:3022 2021-02-12T23:44:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 86.76 , 62.12 , 97.71 2021-02-12T23:44:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , -0.01 2021-02-12T23:44:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173471.77 epoch sec; seqNo:6455; 4057733.70 north; 591442.37 east 2021-02-12T23:44:32Z- AHRS_M2 msg: 1613173472.14 epoch sec; seqNo:6404 2021-02-12T23:44:32Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.19 psi 2021-02-12T23:44:32Z- handleDepth()-Depth_Keller msg: 1613173472148 epoch sec; seqNo:6456; depth 28.47 2021-02-12T23:44:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173472.16 epoch sec; seqNo:6456; 4057733.70 north; 591442.37 east 2021-02-12T23:44:32Z- AHRS_M2 msg: 1613173472.55 epoch sec; seqNo:6405 2021-02-12T23:44:32Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-12T23:44:32Z- handleDepth()-Depth_Keller msg: 1613173472550 epoch sec; seqNo:6457; depth 28.59 2021-02-12T23:44:32Z- handleDvl() - RDI_Pathfinder msg: 1613173472.56 epoch sec; seqNo:3023 2021-02-12T23:44:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 86.09 , 61.30 , 97.98 2021-02-12T23:44:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , -0.02 2021-02-12T23:44:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173472.57 epoch sec; seqNo:6457; 4057734.09 north; 591443.58 east 2021-02-12T23:44:32Z- AHRS_M2 msg: 1613173472.95 epoch sec; seqNo:6406 2021-02-12T23:44:32Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.20 psi 2021-02-12T23:44:32Z- handleDepth()-Depth_Keller msg: 1613173472953 epoch sec; seqNo:6458; depth 28.72 2021-02-12T23:44:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173472.97 epoch sec; seqNo:6458; 4057734.47 north; 591443.58 east 2021-02-12T23:44:33Z- AHRS_M2 msg: 1613173473.35 epoch sec; seqNo:6407 2021-02-12T23:44:33Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.20 psi 2021-02-12T23:44:33Z- handleDepth()-Depth_Keller msg: 1613173473361 epoch sec; seqNo:6459; depth 28.83 2021-02-12T23:44:33Z- handleDvl() - RDI_Pathfinder msg: 1613173473.38 epoch sec; seqNo:3024 2021-02-12T23:44:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 86.09 , 62.52 , 97.98 2021-02-12T23:44:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , -0.00 2021-02-12T23:44:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173473.39 epoch sec; seqNo:6459; 4057734.85 north; 591443.57 east 2021-02-12T23:44:33Z- AHRS_M2 msg: 1613173473.76 epoch sec; seqNo:6408 2021-02-12T23:44:33Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.21 psi 2021-02-12T23:44:33Z- handleDepth()-Depth_Keller msg: 1613173473760 epoch sec; seqNo:6460; depth 28.97 2021-02-12T23:44:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173473.77 epoch sec; seqNo:6460; 4057734.85 north; 591443.57 east 2021-02-12T23:44:34Z- AHRS_M2 msg: 1613173474.16 epoch sec; seqNo:6409 2021-02-12T23:44:34Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.22 psi 2021-02-12T23:44:34Z- handleDepth()-Depth_Keller msg: 1613173474167 epoch sec; seqNo:6461; depth 29.09 2021-02-12T23:44:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173474.18 epoch sec; seqNo:6461; 4057735.23 north; 591443.57 east 2021-02-12T23:44:34Z- AHRS_M2 msg: 1613173474.56 epoch sec; seqNo:6410 2021-02-12T23:44:34Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.22 psi 2021-02-12T23:44:34Z- handleDepth()-Depth_Keller msg: 1613173474580 epoch sec; seqNo:6462; depth 29.22 2021-02-12T23:44:34Z- handleDvl() - RDI_Pathfinder msg: 1613173474.59 epoch sec; seqNo:3025 2021-02-12T23:44:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.20 , 85.88 , 60.89 , 97.78 2021-02-12T23:44:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.01 2021-02-12T23:44:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173474.60 epoch sec; seqNo:6462; 4057735.61 north; 591443.57 east 2021-02-12T23:44:34Z- AHRS_M2 msg: 1613173474.97 epoch sec; seqNo:6411 2021-02-12T23:44:34Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.22 psi 2021-02-12T23:44:34Z- handleDepth()-Depth_Keller msg: 1613173474972 epoch sec; seqNo:6463; depth 29.38 2021-02-12T23:44:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173474.98 epoch sec; seqNo:6463; 4057735.62 north; 591444.79 east 2021-02-12T23:44:35Z- AHRS_M2 msg: 1613173475.37 epoch sec; seqNo:6412 2021-02-12T23:44:35Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.22 psi 2021-02-12T23:44:35Z- handleDepth()-Depth_Keller msg: 1613173475381 epoch sec; seqNo:6464; depth 29.51 2021-02-12T23:44:35Z- handleDvl() - RDI_Pathfinder msg: 1613173475.39 epoch sec; seqNo:3026 2021-02-12T23:44:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.02 , 86.09 , 60.89 , 97.98 2021-02-12T23:44:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , 0.02 2021-02-12T23:44:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173475.40 epoch sec; seqNo:6464; 4057736.00 north; 591444.78 east 2021-02-12T23:44:35Z- AHRS_M2 msg: 1613173475.78 epoch sec; seqNo:6413 2021-02-12T23:44:35Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.21 psi 2021-02-12T23:44:35Z- handleDepth()-Depth_Keller msg: 1613173475781 epoch sec; seqNo:6465; depth 29.67 2021-02-12T23:44:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173475.79 epoch sec; seqNo:6465; 4057736.38 north; 591444.78 east 2021-02-12T23:44:36Z- AHRS_M2 msg: 1613173476.18 epoch sec; seqNo:6414 2021-02-12T23:44:36Z- AHRS_M2 msg: 0.59 phi; -0.25 theta; -0.20 psi 2021-02-12T23:44:36Z- handleDepth()-Depth_Keller msg: 1613173476185 epoch sec; seqNo:6466; depth 29.80 2021-02-12T23:44:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173476.20 epoch sec; seqNo:6466; 4057736.76 north; 591444.77 east 2021-02-12T23:44:36Z- AHRS_M2 msg: 1613173476.58 epoch sec; seqNo:6415 2021-02-12T23:44:36Z- AHRS_M2 msg: 0.58 phi; -0.21 theta; -0.19 psi 2021-02-12T23:44:36Z- handleDepth()-Depth_Keller msg: 1613173476588 epoch sec; seqNo:6467; depth 29.93 2021-02-12T23:44:36Z- handleDvl() - RDI_Pathfinder msg: 1613173476.60 epoch sec; seqNo:3027 2021-02-12T23:44:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.57 , 0.00 , 61.71 , 132.59 2021-02-12T23:44:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.39 , 0.47 , -0.23 2021-02-12T23:44:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173476.62 epoch sec; seqNo:6467; 4057736.76 north; 591444.77 east 2021-02-12T23:44:36Z- LcmTrn::handleMessages() - handling 45 messages took 0.72 ms and 131 calls... 2021-02-12T23:44:36Z- waiting for 1613173474.38, time is 1613173476.62 2021-02-12T23:44:36Z- LcmTrn::updateTrn() equating pose 1613173476.62 and meas times 1613173476.62 2021-02-12T23:44:36Z- LcmTrn::updateTrn() >>>> pose time 1613173476.62, meas time 1613173476.62 <<<< 2021-02-12T23:44:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173476.62 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:44:36Z- TerrainNav::motionUpdate() - currEstimate = (4057737, 591444.77, 29.93) 2021-02-12T23:44:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:44:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:44:36Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173476.62 sec.; No relevant navigation data available 2021-02-12T23:44:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:44:36Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:36Z- variances([0], [2], and psi_berg) = 740.085102 833.024376 0.000000 2021-02-12T23:44:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:44:36Z- LcmTrn::publishEstimates() - MLE : 4057727.26 591397.27 29.93 2021-02-12T23:44:36Z- LcmTrn::publishEstimates() - MMSE : 4057736.76 591444.77 29.93 2021-02-12T23:44:36Z- LcmTrn::publishEstimates() - COVAR: 740.09 833.02 0.97 0.00 2021-02-12T23:44:36Z- AHRS_M2 msg: 1613173476.99 epoch sec; seqNo:6416 2021-02-12T23:44:36Z- AHRS_M2 msg: 0.57 phi; -0.17 theta; -0.17 psi 2021-02-12T23:44:36Z- handleDepth()-Depth_Keller msg: 1613173476992 epoch sec; seqNo:6468; depth 30.07 2021-02-12T23:44:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173477.00 epoch sec; seqNo:6468; 4057736.76 north; 591444.77 east 2021-02-12T23:44:37Z- AHRS_M2 msg: 1613173477.39 epoch sec; seqNo:6417 2021-02-12T23:44:37Z- AHRS_M2 msg: 0.56 phi; -0.12 theta; -0.16 psi 2021-02-12T23:44:37Z- handleDepth()-Depth_Keller msg: 1613173477399 epoch sec; seqNo:6469; depth 30.18 2021-02-12T23:44:37Z- handleDvl() - RDI_Pathfinder msg: 1613173477.41 epoch sec; seqNo:3028 2021-02-12T23:44:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.16 , 78.58 , 63.34 , 75.60 2021-02-12T23:44:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , 0.04 2021-02-12T23:44:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173477.42 epoch sec; seqNo:6469; 4057737.15 north; 591445.99 east 2021-02-12T23:44:37Z- AHRS_M2 msg: 1613173477.80 epoch sec; seqNo:6418 2021-02-12T23:44:37Z- AHRS_M2 msg: 0.55 phi; -0.08 theta; -0.16 psi 2021-02-12T23:44:37Z- handleDepth()-Depth_Keller msg: 1613173477806 epoch sec; seqNo:6470; depth 30.28 2021-02-12T23:44:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173477.83 epoch sec; seqNo:6470; 4057737.53 north; 591445.99 east 2021-02-12T23:44:38Z- AHRS_M2 msg: 1613173478.20 epoch sec; seqNo:6419 2021-02-12T23:44:38Z- AHRS_M2 msg: 0.54 phi; -0.03 theta; -0.15 psi 2021-02-12T23:44:38Z- handleDepth()-Depth_Keller msg: 1613173478207 epoch sec; seqNo:6471; depth 30.35 2021-02-12T23:44:38Z- handleDvl() - RDI_Pathfinder msg: 1613173478.24 epoch sec; seqNo:3029 2021-02-12T23:44:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.76 , 0.00 , 0.00 , 70.88 2021-02-12T23:44:38Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:44:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173478.25 epoch sec; seqNo:6471; 4057737.53 north; 591445.99 east 2021-02-12T23:44:38Z- AHRS_M2 msg: 1613173478.60 epoch sec; seqNo:6420 2021-02-12T23:44:38Z- AHRS_M2 msg: 0.53 phi; 0.01 theta; -0.15 psi 2021-02-12T23:44:38Z- handleDepth()-Depth_Keller msg: 1613173478608 epoch sec; seqNo:6472; depth 30.43 2021-02-12T23:44:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173478.62 epoch sec; seqNo:6472; 4057737.91 north; 591445.98 east 2021-02-12T23:44:39Z- AHRS_M2 msg: 1613173479.03 epoch sec; seqNo:6421 2021-02-12T23:44:39Z- AHRS_M2 msg: 0.52 phi; 0.10 theta; -0.14 psi 2021-02-12T23:44:39Z- handleDepth()-Depth_Keller msg: 1613173479037 epoch sec; seqNo:6473; depth 30.47 2021-02-12T23:44:39Z- handleDvl() - RDI_Pathfinder msg: 1613173479.05 epoch sec; seqNo:3030 2021-02-12T23:44:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.78 , 0.00 , 0.00 , 66.43 2021-02-12T23:44:39Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:44:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173479.06 epoch sec; seqNo:6473; 4057738.29 north; 591445.98 east 2021-02-12T23:44:39Z- AHRS_M2 msg: 1613173479.42 epoch sec; seqNo:6422 2021-02-12T23:44:39Z- AHRS_M2 msg: 0.52 phi; 0.13 theta; -0.13 psi 2021-02-12T23:44:39Z- handleDepth()-Depth_Keller msg: 1613173479421 epoch sec; seqNo:6474; depth 30.51 2021-02-12T23:44:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173479.44 epoch sec; seqNo:6474; 4057738.66 north; 591445.98 east 2021-02-12T23:44:39Z- AHRS_M2 msg: 1613173479.82 epoch sec; seqNo:6423 2021-02-12T23:44:39Z- AHRS_M2 msg: 0.52 phi; 0.17 theta; -0.13 psi 2021-02-12T23:44:39Z- handleDepth()-Depth_Keller msg: 1613173479825 epoch sec; seqNo:6475; depth 30.54 2021-02-12T23:44:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173479.83 epoch sec; seqNo:6475; 4057739.04 north; 591445.97 east 2021-02-12T23:44:40Z- AHRS_M2 msg: 1613173480.22 epoch sec; seqNo:6424 2021-02-12T23:44:40Z- AHRS_M2 msg: 0.52 phi; 0.20 theta; -0.12 psi 2021-02-12T23:44:40Z- handleDepth()-Depth_Keller msg: 1613173480227 epoch sec; seqNo:6476; depth 30.55 2021-02-12T23:44:40Z- handleDvl() - RDI_Pathfinder msg: 1613173480.24 epoch sec; seqNo:3031 2021-02-12T23:44:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.66 , 73.96 , 0.00 , 64.22 2021-02-12T23:44:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.05 2021-02-12T23:44:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173480.25 epoch sec; seqNo:6476; 4057739.06 north; 591447.19 east 2021-02-12T23:44:40Z- AHRS_M2 msg: 1613173480.63 epoch sec; seqNo:6425 2021-02-12T23:44:40Z- AHRS_M2 msg: 0.52 phi; 0.23 theta; -0.11 psi 2021-02-12T23:44:40Z- handleDepth()-Depth_Keller msg: 1613173480641 epoch sec; seqNo:6477; depth 30.53 2021-02-12T23:44:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173480.66 epoch sec; seqNo:6477; 4057739.44 north; 591447.19 east 2021-02-12T23:44:41Z- AHRS_M2 msg: 1613173481.03 epoch sec; seqNo:6426 2021-02-12T23:44:41Z- AHRS_M2 msg: 0.52 phi; 0.25 theta; -0.11 psi 2021-02-12T23:44:41Z- handleDepth()-Depth_Keller msg: 1613173481032 epoch sec; seqNo:6478; depth 30.51 2021-02-12T23:44:41Z- handleDvl() - RDI_Pathfinder msg: 1613173481.06 epoch sec; seqNo:3032 2021-02-12T23:44:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 67.00 , 0.00 , 0.00 , 62.47 2021-02-12T23:44:41Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:44:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173481.07 epoch sec; seqNo:6478; 4057739.81 north; 591447.18 east 2021-02-12T23:44:41Z- AHRS_M2 msg: 1613173481.43 epoch sec; seqNo:6427 2021-02-12T23:44:41Z- AHRS_M2 msg: 0.52 phi; 0.27 theta; -0.10 psi 2021-02-12T23:44:41Z- handleDepth()-Depth_Keller msg: 1613173481436 epoch sec; seqNo:6479; depth 30.48 2021-02-12T23:44:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173481.45 epoch sec; seqNo:6479; 4057740.19 north; 591447.18 east 2021-02-12T23:44:41Z- LcmTrn::handleMessages() - handling 41 messages took 0.65 ms and 128 calls... 2021-02-12T23:44:41Z- waiting for 1613173479.62, time is 1613173481.45 2021-02-12T23:44:41Z- LcmTrn::updateTrn() >>>> pose time 1613173481.45, meas time 1613173481.06 <<<< 2021-02-12T23:44:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173481.06 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:44:41Z- TerrainNav::motionUpdate() - currEstimate = (4057740, 591447.18, 30.48) 2021-02-12T23:44:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:44:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:44:41Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:44:41Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:44:41Z- TNavBF::Projecting Measurements 2021-02-12T23:44:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:44:41Z- TNavFilter::Excluded 3 beam(s) from correlation due to poor sonar data. 2021-02-12T23:44:41Z- TNavFilter::1 good beams. 2021-02-12T23:44:41Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:44:41Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:44:41Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:44:41Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:44:41Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:44:41Z- TNavBF:: *** beamsVF.Ncols() = 1 *** 2021-02-12T23:44:41Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:44:41Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:41Z- TerrainMapOctree: (vehN, vehE ) = (4057739.92, 591446.99). 2021-02-12T23:44:41Z- TerrainMapOctree: Min Distance = 4491.94. 2021-02-12T23:44:41Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:44:41Z- There is no specific good beam that all particles have in common. 2021-02-12T23:44:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:44:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.115 ExpectedMeasDiff: nan currMeas.covariance: 7.18 2021-02-12T23:44:41Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:44:41Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:44:41Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:44:41Z- Filter 0 No beams used in subcloud update 2021-02-12T23:44:41Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:44:41Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:44:41Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 740.0 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:44:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173481.06 sec. 2021-02-12T23:44:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:44:41Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:41Z- variances([0], [2], and psi_berg) = 739.976130 833.024287 0.000000 2021-02-12T23:44:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:44:41Z- LcmTrn::publishEstimates() - MLE : 4057730.69 591399.68 30.48 2021-02-12T23:44:41Z- LcmTrn::publishEstimates() - MMSE : 4057740.19 591447.18 30.48 2021-02-12T23:44:41Z- LcmTrn::publishEstimates() - COVAR: 739.98 833.02 1.05 0.00 2021-02-12T23:44:41Z- AHRS_M2 msg: 1613173481.84 epoch sec; seqNo:6428 2021-02-12T23:44:41Z- AHRS_M2 msg: 0.53 phi; 0.29 theta; -0.10 psi 2021-02-12T23:44:41Z- handleDepth()-Depth_Keller msg: 1613173481845 epoch sec; seqNo:6480; depth 30.43 2021-02-12T23:44:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173481.85 epoch sec; seqNo:6480; 4057740.19 north; 591447.18 east 2021-02-12T23:44:42Z- AHRS_M2 msg: 1613173482.24 epoch sec; seqNo:6429 2021-02-12T23:44:42Z- AHRS_M2 msg: 0.53 phi; 0.30 theta; -0.09 psi 2021-02-12T23:44:42Z- handleDepth()-Depth_Keller msg: 1613173482246 epoch sec; seqNo:6481; depth 30.38 2021-02-12T23:44:42Z- handleDvl() - RDI_Pathfinder msg: 1613173482.26 epoch sec; seqNo:3033 2021-02-12T23:44:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 68.56 , 0.00 , 0.00 , 61.74 2021-02-12T23:44:42Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:44:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173482.27 epoch sec; seqNo:6481; 4057740.57 north; 591447.18 east 2021-02-12T23:44:42Z- AHRS_M2 msg: 1613173482.64 epoch sec; seqNo:6430 2021-02-12T23:44:42Z- AHRS_M2 msg: 0.54 phi; 0.31 theta; -0.08 psi 2021-02-12T23:44:42Z- handleDepth()-Depth_Keller msg: 1613173482648 epoch sec; seqNo:6482; depth 30.30 2021-02-12T23:44:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173482.66 epoch sec; seqNo:6482; 4057740.95 north; 591447.17 east 2021-02-12T23:44:43Z- AHRS_M2 msg: 1613173483.05 epoch sec; seqNo:6431 2021-02-12T23:44:43Z- AHRS_M2 msg: 0.55 phi; 0.32 theta; -0.08 psi 2021-02-12T23:44:43Z- handleDepth()-Depth_Keller msg: 1613173483057 epoch sec; seqNo:6483; depth 30.23 2021-02-12T23:44:43Z- handleDvl() - RDI_Pathfinder msg: 1613173483.07 epoch sec; seqNo:3034 2021-02-12T23:44:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.83 , 76.22 , 0.00 , 61.32 2021-02-12T23:44:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.00 , -0.07 2021-02-12T23:44:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173483.10 epoch sec; seqNo:6483; 4057741.33 north; 591447.17 east 2021-02-12T23:44:43Z- AHRS_M2 msg: 1613173483.46 epoch sec; seqNo:6432 2021-02-12T23:44:43Z- AHRS_M2 msg: 0.55 phi; 0.33 theta; -0.08 psi 2021-02-12T23:44:43Z- handleDepth()-Depth_Keller msg: 1613173483465 epoch sec; seqNo:6484; depth 30.14 2021-02-12T23:44:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173483.47 epoch sec; seqNo:6484; 4057741.34 north; 591448.39 east 2021-02-12T23:44:43Z- AHRS_M2 msg: 1613173483.86 epoch sec; seqNo:6433 2021-02-12T23:44:43Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.08 psi 2021-02-12T23:44:43Z- handleDepth()-Depth_Keller msg: 1613173483862 epoch sec; seqNo:6485; depth 30.06 2021-02-12T23:44:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173483.87 epoch sec; seqNo:6485; 4057741.72 north; 591448.38 east 2021-02-12T23:44:44Z- AHRS_M2 msg: 1613173484.26 epoch sec; seqNo:6434 2021-02-12T23:44:44Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.07 psi 2021-02-12T23:44:44Z- handleDepth()-Depth_Keller msg: 1613173484265 epoch sec; seqNo:6486; depth 29.97 2021-02-12T23:44:44Z- handleDvl() - RDI_Pathfinder msg: 1613173484.28 epoch sec; seqNo:3035 2021-02-12T23:44:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 70.23 , 0.00 , 0.00 , 61.17 2021-02-12T23:44:44Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:44:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173484.30 epoch sec; seqNo:6486; 4057741.72 north; 591448.38 east 2021-02-12T23:44:44Z- handleDepth()-Depth_Keller msg: 1613173484685 epoch sec; seqNo:6487; depth 29.87 2021-02-12T23:44:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173484.70 epoch sec; seqNo:6487; 4057742.10 north; 591448.38 east 2021-02-12T23:44:45Z- AHRS_M2 msg: 1613173485.07 epoch sec; seqNo:6435 2021-02-12T23:44:45Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.07 psi 2021-02-12T23:44:45Z- handleDepth()-Depth_Keller msg: 1613173485079 epoch sec; seqNo:6488; depth 29.77 2021-02-12T23:44:45Z- handleDvl() - RDI_Pathfinder msg: 1613173485.09 epoch sec; seqNo:3036 2021-02-12T23:44:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 70.27 , 0.00 , 0.00 , 60.05 2021-02-12T23:44:45Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:44:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173485.10 epoch sec; seqNo:6488; 4057742.48 north; 591448.38 east 2021-02-12T23:44:45Z- AHRS_M2 msg: 1613173485.47 epoch sec; seqNo:6436 2021-02-12T23:44:45Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.07 psi 2021-02-12T23:44:45Z- handleDepth()-Depth_Keller msg: 1613173485486 epoch sec; seqNo:6489; depth 29.65 2021-02-12T23:44:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173485.55 epoch sec; seqNo:6489; 4057742.48 north; 591448.38 east 2021-02-12T23:44:45Z- AHRS_M2 msg: 1613173485.88 epoch sec; seqNo:6437 2021-02-12T23:44:45Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.07 psi 2021-02-12T23:44:45Z- handleDepth()-Depth_Keller msg: 1613173485880 epoch sec; seqNo:6490; depth 29.53 2021-02-12T23:44:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173485.89 epoch sec; seqNo:6490; 4057742.86 north; 591448.37 east 2021-02-12T23:44:46Z- AHRS_M2 msg: 1613173486.28 epoch sec; seqNo:6438 2021-02-12T23:44:46Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.08 psi 2021-02-12T23:44:46Z- handleDepth()-Depth_Keller msg: 1613173486284 epoch sec; seqNo:6491; depth 29.43 2021-02-12T23:44:46Z- handleDvl() - RDI_Pathfinder msg: 1613173486.30 epoch sec; seqNo:3037 2021-02-12T23:44:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 61.32 2021-02-12T23:44:46Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:44:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173486.31 epoch sec; seqNo:6491; 4057743.24 north; 591448.37 east 2021-02-12T23:44:46Z- AHRS_M2 msg: 1613173486.68 epoch sec; seqNo:6439 2021-02-12T23:44:46Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.08 psi 2021-02-12T23:44:46Z- handleDepth()-Depth_Keller msg: 1613173486689 epoch sec; seqNo:6492; depth 29.30 2021-02-12T23:44:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173486.70 epoch sec; seqNo:6492; 4057743.25 north; 591449.59 east 2021-02-12T23:44:46Z- LcmTrn::handleMessages() - handling 43 messages took 0.66 ms and 130 calls... 2021-02-12T23:44:46Z- waiting for 1613173484.45, time is 1613173486.70 2021-02-12T23:44:46Z- LcmTrn::updateTrn() >>>> pose time 1613173486.70, meas time 1613173486.30 <<<< 2021-02-12T23:44:46Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173486.30, ping # 0. 2021-02-12T23:44:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173486.30 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:44:46Z- TerrainNav::motionUpdate() - currEstimate = (4057743, 591449.59, 29.30) 2021-02-12T23:44:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:44:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:44:46Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:44:46Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:44:46Z- TNavBF::Projecting Measurements 2021-02-12T23:44:46Z- TNavFilter::Measurement from time = 1613173486.30 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:44:46Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:44:46Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:44:46Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:44:46Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:44:46Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:44:46Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:44:46Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:44:46Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 739.9 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:44:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:44:46Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:46Z- variances([0], [2], and psi_berg) = 739.871653 833.024201 0.000000 2021-02-12T23:44:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:44:46Z- LcmTrn::publishEstimates() - MLE : 4057733.75 591402.09 29.30 2021-02-12T23:44:46Z- LcmTrn::publishEstimates() - MMSE : 4057743.25 591449.59 29.30 2021-02-12T23:44:46Z- LcmTrn::publishEstimates() - COVAR: 739.87 833.02 1.25 0.00 2021-02-12T23:44:47Z- AHRS_M2 msg: 1613173487.09 epoch sec; seqNo:6440 2021-02-12T23:44:47Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.09 psi 2021-02-12T23:44:47Z- handleDepth()-Depth_Keller msg: 1613173487092 epoch sec; seqNo:6493; depth 29.18 2021-02-12T23:44:47Z- handleDvl() - RDI_Pathfinder msg: 1613173487.10 epoch sec; seqNo:3038 2021-02-12T23:44:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 62.17 2021-02-12T23:44:47Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:44:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173487.14 epoch sec; seqNo:6493; 4057743.63 north; 591449.59 east 2021-02-12T23:44:47Z- AHRS_M2 msg: 1613173487.49 epoch sec; seqNo:6441 2021-02-12T23:44:47Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.09 psi 2021-02-12T23:44:47Z- handleDepth()-Depth_Keller msg: 1613173487496 epoch sec; seqNo:6494; depth 29.08 2021-02-12T23:44:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173487.51 epoch sec; seqNo:6494; 4057744.01 north; 591449.58 east 2021-02-12T23:44:47Z- AHRS_M2 msg: 1613173487.90 epoch sec; seqNo:6442 2021-02-12T23:44:47Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.09 psi 2021-02-12T23:44:47Z- handleDepth()-Depth_Keller msg: 1613173487901 epoch sec; seqNo:6495; depth 28.98 2021-02-12T23:44:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173487.91 epoch sec; seqNo:6495; 4057744.01 north; 591449.58 east 2021-02-12T23:44:48Z- AHRS_M2 msg: 1613173488.30 epoch sec; seqNo:6443 2021-02-12T23:44:48Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.10 psi 2021-02-12T23:44:48Z- handleDepth()-Depth_Keller msg: 1613173488313 epoch sec; seqNo:6496; depth 28.85 2021-02-12T23:44:48Z- handleDvl() - RDI_Pathfinder msg: 1613173488.33 epoch sec; seqNo:3039 2021-02-12T23:44:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:44:48Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:44:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173488.34 epoch sec; seqNo:6496; 4057744.39 north; 591449.58 east 2021-02-12T23:44:48Z- AHRS_M2 msg: 1613173488.70 epoch sec; seqNo:6444 2021-02-12T23:44:48Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.10 psi 2021-02-12T23:44:48Z- handleDepth()-Depth_Keller msg: 1613173488713 epoch sec; seqNo:6497; depth 28.73 2021-02-12T23:44:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173488.73 epoch sec; seqNo:6497; 4057744.39 north; 591449.58 east 2021-02-12T23:44:49Z- AHRS_M2 msg: 1613173489.11 epoch sec; seqNo:6445 2021-02-12T23:44:49Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.09 psi 2021-02-12T23:44:49Z- handleDepth()-Depth_Keller msg: 1613173489115 epoch sec; seqNo:6498; depth 28.61 2021-02-12T23:44:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173489.13 epoch sec; seqNo:6498; 4057744.77 north; 591449.57 east 2021-02-12T23:44:49Z- AHRS_M2 msg: 1613173489.51 epoch sec; seqNo:6446 2021-02-12T23:44:49Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.09 psi 2021-02-12T23:44:49Z- handleDepth()-Depth_Keller msg: 1613173489521 epoch sec; seqNo:6499; depth 28.51 2021-02-12T23:44:49Z- handleDvl() - RDI_Pathfinder msg: 1613173489.54 epoch sec; seqNo:3040 2021-02-12T23:44:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 80.68 , 97.98 , 61.30 2021-02-12T23:44:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.08 2021-02-12T23:44:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173489.58 epoch sec; seqNo:6499; 4057745.16 north; 591450.79 east 2021-02-12T23:44:49Z- AHRS_M2 msg: 1613173489.92 epoch sec; seqNo:6447 2021-02-12T23:44:49Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.09 psi 2021-02-12T23:44:49Z- handleDepth()-Depth_Keller msg: 1613173489922 epoch sec; seqNo:6500; depth 28.37 2021-02-12T23:44:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173489.93 epoch sec; seqNo:6500; 4057745.54 north; 591450.79 east 2021-02-12T23:44:50Z- AHRS_M2 msg: 1613173490.34 epoch sec; seqNo:6448 2021-02-12T23:44:50Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.08 psi 2021-02-12T23:44:50Z- handleDepth()-Depth_Keller msg: 1613173490343 epoch sec; seqNo:6501; depth 28.25 2021-02-12T23:44:50Z- handleDvl() - RDI_Pathfinder msg: 1613173490.37 epoch sec; seqNo:3041 2021-02-12T23:44:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 79.60 , 97.98 , 62.93 2021-02-12T23:44:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , 0.05 2021-02-12T23:44:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173490.38 epoch sec; seqNo:6501; 4057745.54 north; 591450.79 east 2021-02-12T23:44:50Z- AHRS_M2 msg: 1613173490.72 epoch sec; seqNo:6449 2021-02-12T23:44:50Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.08 psi 2021-02-12T23:44:50Z- handleDepth()-Depth_Keller msg: 1613173490728 epoch sec; seqNo:6502; depth 28.13 2021-02-12T23:44:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173490.74 epoch sec; seqNo:6502; 4057745.92 north; 591450.78 east 2021-02-12T23:44:51Z- AHRS_M2 msg: 1613173491.14 epoch sec; seqNo:6450 2021-02-12T23:44:51Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.08 psi 2021-02-12T23:44:51Z- handleDepth()-Depth_Keller msg: 1613173491147 epoch sec; seqNo:6503; depth 28.03 2021-02-12T23:44:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173491.16 epoch sec; seqNo:6503; 4057746.29 north; 591450.78 east 2021-02-12T23:44:51Z- AHRS_M2 msg: 1613173491.53 epoch sec; seqNo:6451 2021-02-12T23:44:51Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.07 psi 2021-02-12T23:44:51Z- handleDepth()-Depth_Keller msg: 1613173491546 epoch sec; seqNo:6504; depth 27.90 2021-02-12T23:44:51Z- handleDvl() - RDI_Pathfinder msg: 1613173491.56 epoch sec; seqNo:3042 2021-02-12T23:44:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 79.60 , 97.98 , 63.34 2021-02-12T23:44:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.07 , 0.00 2021-02-12T23:44:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173491.57 epoch sec; seqNo:6504; 4057746.29 north; 591450.78 east 2021-02-12T23:44:51Z- LcmTrn::handleMessages() - handling 41 messages took 0.69 ms and 127 calls... 2021-02-12T23:44:51Z- waiting for 1613173489.70, time is 1613173491.57 2021-02-12T23:44:51Z- LcmTrn::updateTrn() equating pose 1613173491.57 and meas times 1613173491.57 2021-02-12T23:44:51Z- LcmTrn::updateTrn() >>>> pose time 1613173491.57, meas time 1613173491.57 <<<< 2021-02-12T23:44:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173491.57 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:44:51Z- TerrainNav::motionUpdate() - currEstimate = (4057746, 591450.78, 27.90) 2021-02-12T23:44:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:44:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:44:51Z- TNavBF::Projecting Measurements 2021-02-12T23:44:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:44:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:44:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:44:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:44:51Z- TNavFilter::4 good beams. 2021-02-12T23:44:51Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:44:51Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:44:51Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:44:51Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:44:51Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:44:51Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:44:51Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:44:51Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:51Z- TerrainMapOctree: (vehN, vehE ) = (4057746.29, 591450.78). 2021-02-12T23:44:51Z- TerrainMapOctree: Min Distance = 4484.60. 2021-02-12T23:44:51Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:44:51Z- There is no specific good beam that all particles have in common. 2021-02-12T23:44:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:44:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.41 K: 0.123 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-12T23:44:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:44:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 10.14 2021-02-12T23:44:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:44:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-12T23:44:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:44:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.114 ExpectedMeasDiff: nan currMeas.covariance: 6.42 2021-02-12T23:44:51Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:44:51Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:44:51Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:44:51Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:44:51Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:44:51Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:44:51Z- Filter 0 No beams used in subcloud update 2021-02-12T23:44:51Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:44:51Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:44:51Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 739.8 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:44:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173491.57 sec. 2021-02-12T23:44:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:44:51Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:51Z- variances([0], [2], and psi_berg) = 739.794432 833.024138 0.000000 2021-02-12T23:44:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:44:51Z- LcmTrn::publishEstimates() - MLE : 4057736.79 591403.28 27.90 2021-02-12T23:44:51Z- LcmTrn::publishEstimates() - MMSE : 4057746.29 591450.78 27.90 2021-02-12T23:44:51Z- LcmTrn::publishEstimates() - COVAR: 739.79 833.02 0.93 0.00 2021-02-12T23:44:51Z- AHRS_M2 msg: 1613173491.94 epoch sec; seqNo:6452 2021-02-12T23:44:51Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.08 psi 2021-02-12T23:44:51Z- handleDepth()-Depth_Keller msg: 1613173491949 epoch sec; seqNo:6505; depth 27.78 2021-02-12T23:44:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173491.96 epoch sec; seqNo:6505; 4057746.69 north; 591451.99 east 2021-02-12T23:44:52Z- AHRS_M2 msg: 1613173492.34 epoch sec; seqNo:6453 2021-02-12T23:44:52Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.08 psi 2021-02-12T23:44:52Z- handleDepth()-Depth_Keller msg: 1613173492344 epoch sec; seqNo:6506; depth 27.68 2021-02-12T23:44:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173492.37 epoch sec; seqNo:6506; 4057747.06 north; 591451.99 east 2021-02-12T23:44:52Z- AHRS_M2 msg: 1613173492.74 epoch sec; seqNo:6454 2021-02-12T23:44:52Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.10 psi 2021-02-12T23:44:52Z- handleDepth()-Depth_Keller msg: 1613173492748 epoch sec; seqNo:6507; depth 27.55 2021-02-12T23:44:52Z- handleDvl() - RDI_Pathfinder msg: 1613173492.76 epoch sec; seqNo:3043 2021-02-12T23:44:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 79.60 , 97.98 , 63.34 2021-02-12T23:44:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.05 , -0.05 2021-02-12T23:44:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173492.79 epoch sec; seqNo:6507; 4057747.06 north; 591451.99 east 2021-02-12T23:44:53Z- AHRS_M2 msg: 1613173493.15 epoch sec; seqNo:6455 2021-02-12T23:44:53Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.12 psi 2021-02-12T23:44:53Z- handleDepth()-Depth_Keller msg: 1613173493158 epoch sec; seqNo:6508; depth 27.42 2021-02-12T23:44:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173493.17 epoch sec; seqNo:6508; 4057747.44 north; 591451.99 east 2021-02-12T23:44:53Z- AHRS_M2 msg: 1613173493.55 epoch sec; seqNo:6456 2021-02-12T23:44:53Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.14 psi 2021-02-12T23:44:53Z- handleDepth()-Depth_Keller msg: 1613173493557 epoch sec; seqNo:6509; depth 27.29 2021-02-12T23:44:53Z- handleDvl() - RDI_Pathfinder msg: 1613173493.57 epoch sec; seqNo:3044 2021-02-12T23:44:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:44:53Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:44:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173493.59 epoch sec; seqNo:6509; 4057747.82 north; 591451.98 east 2021-02-12T23:44:53Z- AHRS_M2 msg: 1613173493.96 epoch sec; seqNo:6457 2021-02-12T23:44:53Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.16 psi 2021-02-12T23:44:53Z- handleDepth()-Depth_Keller msg: 1613173493965 epoch sec; seqNo:6510; depth 27.17 2021-02-12T23:44:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173493.98 epoch sec; seqNo:6510; 4057747.82 north; 591451.98 east 2021-02-12T23:44:54Z- AHRS_M2 msg: 1613173494.38 epoch sec; seqNo:6458 2021-02-12T23:44:54Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.17 psi 2021-02-12T23:44:54Z- handleDepth()-Depth_Keller msg: 1613173494381 epoch sec; seqNo:6511; depth 27.05 2021-02-12T23:44:54Z- handleDvl() - RDI_Pathfinder msg: 1613173494.39 epoch sec; seqNo:3045 2021-02-12T23:44:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 86.96 , 97.92 , 64.16 2021-02-12T23:44:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , -0.04 2021-02-12T23:44:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173494.42 epoch sec; seqNo:6511; 4057748.21 north; 591453.20 east 2021-02-12T23:44:54Z- AHRS_M2 msg: 1613173494.76 epoch sec; seqNo:6459 2021-02-12T23:44:54Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.19 psi 2021-02-12T23:44:54Z- handleDepth()-Depth_Keller msg: 1613173494770 epoch sec; seqNo:6512; depth 26.92 2021-02-12T23:44:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173494.78 epoch sec; seqNo:6512; 4057748.59 north; 591453.20 east 2021-02-12T23:44:55Z- AHRS_M2 msg: 1613173495.17 epoch sec; seqNo:6460 2021-02-12T23:44:55Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.20 psi 2021-02-12T23:44:55Z- handleDepth()-Depth_Keller msg: 1613173495177 epoch sec; seqNo:6513; depth 26.83 2021-02-12T23:44:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173495.19 epoch sec; seqNo:6513; 4057748.59 north; 591453.20 east 2021-02-12T23:44:55Z- AHRS_M2 msg: 1613173495.57 epoch sec; seqNo:6461 2021-02-12T23:44:55Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.20 psi 2021-02-12T23:44:55Z- handleDepth()-Depth_Keller msg: 1613173495576 epoch sec; seqNo:6514; depth 26.70 2021-02-12T23:44:55Z- handleDvl() - RDI_Pathfinder msg: 1613173495.59 epoch sec; seqNo:3046 2021-02-12T23:44:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 88.25 , 97.98 , 65.38 2021-02-12T23:44:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , 0.01 2021-02-12T23:44:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173495.60 epoch sec; seqNo:6514; 4057748.97 north; 591453.19 east 2021-02-12T23:44:55Z- AHRS_M2 msg: 1613173495.98 epoch sec; seqNo:6462 2021-02-12T23:44:55Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.21 psi 2021-02-12T23:44:55Z- handleDepth()-Depth_Keller msg: 1613173495982 epoch sec; seqNo:6515; depth 26.58 2021-02-12T23:44:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173496.00 epoch sec; seqNo:6515; 4057749.35 north; 591453.19 east 2021-02-12T23:44:56Z- AHRS_M2 msg: 1613173496.38 epoch sec; seqNo:6463 2021-02-12T23:44:56Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.21 psi 2021-02-12T23:44:56Z- handleDepth()-Depth_Keller msg: 1613173496387 epoch sec; seqNo:6516; depth 26.46 2021-02-12T23:44:56Z- handleDvl() - RDI_Pathfinder msg: 1613173496.42 epoch sec; seqNo:3047 2021-02-12T23:44:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 89.26 , 99.00 , 65.79 2021-02-12T23:44:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.03 2021-02-12T23:44:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173496.44 epoch sec; seqNo:6516; 4057749.35 north; 591453.19 east 2021-02-12T23:44:56Z- AHRS_M2 msg: 1613173496.79 epoch sec; seqNo:6464 2021-02-12T23:44:56Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.21 psi 2021-02-12T23:44:56Z- handleDepth()-Depth_Keller msg: 1613173496793 epoch sec; seqNo:6517; depth 26.32 2021-02-12T23:44:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173496.80 epoch sec; seqNo:6517; 4057749.73 north; 591453.18 east 2021-02-12T23:44:56Z- LcmTrn::handleMessages() - handling 44 messages took 0.79 ms and 129 calls... 2021-02-12T23:44:56Z- waiting for 1613173494.57, time is 1613173496.80 2021-02-12T23:44:56Z- LcmTrn::updateTrn() >>>> pose time 1613173496.80, meas time 1613173496.42 <<<< 2021-02-12T23:44:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173496.42 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:44:56Z- TerrainNav::motionUpdate() - currEstimate = (4057750, 591453.18, 26.32) 2021-02-12T23:44:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:44:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:44:56Z- TNavBF::Projecting Measurements 2021-02-12T23:44:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:44:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:44:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:44:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:44:56Z- TNavFilter::4 good beams. 2021-02-12T23:44:56Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:44:56Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:44:56Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:44:56Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:44:56Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:44:56Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:44:56Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:44:56Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:56Z- TerrainMapOctree: (vehN, vehE ) = (4057749.48, 591453.01). 2021-02-12T23:44:56Z- TerrainMapOctree: Min Distance = 4480.72. 2021-02-12T23:44:56Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:44:56Z- There is no specific good beam that all particles have in common. 2021-02-12T23:44:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:44:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 7.81 2021-02-12T23:44:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:44:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 12.75 2021-02-12T23:44:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:44:56Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 15.68 2021-02-12T23:44:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:44:56Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 6.93 2021-02-12T23:44:56Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:44:56Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:44:56Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:44:56Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:44:56Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:44:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:44:56Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:44:56Z- Filter 0 No beams used in subcloud update 2021-02-12T23:44:56Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:44:56Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:44:56Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 739.7 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:44:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173496.42 sec. 2021-02-12T23:44:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:44:56Z- TNavBF::mmse selected from filter -1 2021-02-12T23:44:56Z- variances([0], [2], and psi_berg) = 739.686698 833.024050 0.000000 2021-02-12T23:44:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:44:56Z- LcmTrn::publishEstimates() - MLE : 4057740.23 591405.68 26.32 2021-02-12T23:44:56Z- LcmTrn::publishEstimates() - MMSE : 4057749.73 591453.18 26.32 2021-02-12T23:44:56Z- LcmTrn::publishEstimates() - COVAR: 739.69 833.02 0.83 0.00 2021-02-12T23:44:57Z- AHRS_M2 msg: 1613173497.19 epoch sec; seqNo:6465 2021-02-12T23:44:57Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.21 psi 2021-02-12T23:44:57Z- handleDepth()-Depth_Keller msg: 1613173497195 epoch sec; seqNo:6518; depth 26.22 2021-02-12T23:44:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173497.20 epoch sec; seqNo:6518; 4057750.12 north; 591454.40 east 2021-02-12T23:44:57Z- AHRS_M2 msg: 1613173497.59 epoch sec; seqNo:6466 2021-02-12T23:44:57Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.20 psi 2021-02-12T23:44:57Z- handleDepth()-Depth_Keller msg: 1613173497601 epoch sec; seqNo:6519; depth 26.10 2021-02-12T23:44:57Z- handleDvl() - RDI_Pathfinder msg: 1613173497.62 epoch sec; seqNo:3048 2021-02-12T23:44:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 90.28 , 98.93 , 64.97 2021-02-12T23:44:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , 0.03 2021-02-12T23:44:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173497.63 epoch sec; seqNo:6519; 4057750.50 north; 591454.40 east 2021-02-12T23:44:57Z- AHRS_M2 msg: 1613173498.00 epoch sec; seqNo:6467 2021-02-12T23:44:57Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.20 psi 2021-02-12T23:44:58Z- handleDepth()-Depth_Keller msg: 1613173498000 epoch sec; seqNo:6520; depth 25.99 2021-02-12T23:44:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173498.01 epoch sec; seqNo:6520; 4057750.50 north; 591454.40 east 2021-02-12T23:44:58Z- AHRS_M2 msg: 1613173498.40 epoch sec; seqNo:6468 2021-02-12T23:44:58Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.20 psi 2021-02-12T23:44:58Z- handleDepth()-Depth_Keller msg: 1613173498413 epoch sec; seqNo:6521; depth 25.88 2021-02-12T23:44:58Z- handleDvl() - RDI_Pathfinder msg: 1613173498.42 epoch sec; seqNo:3049 2021-02-12T23:44:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 89.20 , 97.85 , 64.97 2021-02-12T23:44:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , 0.01 2021-02-12T23:44:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173498.43 epoch sec; seqNo:6521; 4057750.88 north; 591454.39 east 2021-02-12T23:44:58Z- AHRS_M2 msg: 1613173498.81 epoch sec; seqNo:6469 2021-02-12T23:44:58Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-12T23:44:58Z- handleDepth()-Depth_Keller msg: 1613173498821 epoch sec; seqNo:6522; depth 25.75 2021-02-12T23:44:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173498.83 epoch sec; seqNo:6522; 4057751.26 north; 591454.39 east 2021-02-12T23:44:59Z- AHRS_M2 msg: 1613173499.21 epoch sec; seqNo:6470 2021-02-12T23:44:59Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-12T23:44:59Z- handleDepth()-Depth_Keller msg: 1613173499222 epoch sec; seqNo:6523; depth 25.63 2021-02-12T23:44:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173499.24 epoch sec; seqNo:6523; 4057751.64 north; 591454.38 east 2021-02-12T23:44:59Z- handleDepth()-Depth_Keller msg: 1613173499637 epoch sec; seqNo:6524; depth 25.52 2021-02-12T23:44:59Z- handleDvl() - RDI_Pathfinder msg: 1613173499.65 epoch sec; seqNo:3050 2021-02-12T23:44:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 90.35 , 99.00 , 66.20 2021-02-12T23:44:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , -0.01 2021-02-12T23:44:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173499.66 epoch sec; seqNo:6524; 4057751.65 north; 591455.60 east 2021-02-12T23:45:00Z- AHRS_M2 msg: 1613173500.02 epoch sec; seqNo:6471 2021-02-12T23:45:00Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-12T23:45:00Z- handleDepth()-Depth_Keller msg: 1613173500025 epoch sec; seqNo:6525; depth 25.40 2021-02-12T23:45:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173500.04 epoch sec; seqNo:6525; 4057752.03 north; 591455.60 east 2021-02-12T23:45:00Z- AHRS_M2 msg: 1613173500.42 epoch sec; seqNo:6472 2021-02-12T23:45:00Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-12T23:45:00Z- handleDepth()-Depth_Keller msg: 1613173500428 epoch sec; seqNo:6526; depth 25.29 2021-02-12T23:45:00Z- handleDvl() - RDI_Pathfinder msg: 1613173500.45 epoch sec; seqNo:3051 2021-02-12T23:45:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 89.67 , 101.84 , 66.20 2021-02-12T23:45:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.01 2021-02-12T23:45:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173500.46 epoch sec; seqNo:6526; 4057752.41 north; 591455.60 east 2021-02-12T23:45:00Z- AHRS_M2 msg: 1613173500.83 epoch sec; seqNo:6473 2021-02-12T23:45:00Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-12T23:45:00Z- handleDepth()-Depth_Keller msg: 1613173500832 epoch sec; seqNo:6527; depth 25.17 2021-02-12T23:45:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173500.84 epoch sec; seqNo:6527; 4057752.41 north; 591455.60 east 2021-02-12T23:45:01Z- AHRS_M2 msg: 1613173501.23 epoch sec; seqNo:6474 2021-02-12T23:45:01Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-12T23:45:01Z- handleDepth()-Depth_Keller msg: 1613173501245 epoch sec; seqNo:6528; depth 25.06 2021-02-12T23:45:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173501.25 epoch sec; seqNo:6528; 4057752.79 north; 591455.59 east 2021-02-12T23:45:01Z- AHRS_M2 msg: 1613173501.63 epoch sec; seqNo:6475 2021-02-12T23:45:01Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-12T23:45:01Z- handleDepth()-Depth_Keller msg: 1613173501642 epoch sec; seqNo:6529; depth 24.95 2021-02-12T23:45:01Z- handleDvl() - RDI_Pathfinder msg: 1613173501.65 epoch sec; seqNo:3052 2021-02-12T23:45:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 90.21 , 101.03 , 65.79 2021-02-12T23:45:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , -0.02 2021-02-12T23:45:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173501.66 epoch sec; seqNo:6529; 4057753.17 north; 591455.59 east 2021-02-12T23:45:02Z- LcmTrn::handleMessages() - handling 40 messages took 0.63 ms and 128 calls... 2021-02-12T23:45:02Z- waiting for 1613173499.80, time is 1613173501.66 2021-02-12T23:45:02Z- LcmTrn::updateTrn() equating pose 1613173501.66 and meas times 1613173501.66 2021-02-12T23:45:02Z- LcmTrn::updateTrn() >>>> pose time 1613173501.66, meas time 1613173501.66 <<<< 2021-02-12T23:45:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173501.66 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:45:02Z- TerrainNav::motionUpdate() - currEstimate = (4057753, 591455.59, 24.95) 2021-02-12T23:45:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:45:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:45:02Z- TNavBF::Projecting Measurements 2021-02-12T23:45:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:45:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:45:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:45:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:45:02Z- TNavFilter::4 good beams. 2021-02-12T23:45:02Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:45:02Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:45:02Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:45:02Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:45:02Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:45:02Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:45:02Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:45:02Z- TNavBF::mmse selected from filter -1 2021-02-12T23:45:02Z- TerrainMapOctree: (vehN, vehE ) = (4057753.17, 591455.59). 2021-02-12T23:45:02Z- TerrainMapOctree: Min Distance = 4476.22. 2021-02-12T23:45:02Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:45:02Z- There is no specific good beam that all particles have in common. 2021-02-12T23:45:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:45:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-12T23:45:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:45:02Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 13.02 2021-02-12T23:45:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:45:02Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 16.33 2021-02-12T23:45:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:45:02Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 6.93 2021-02-12T23:45:02Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:45:02Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:45:02Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:45:02Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:45:02Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:45:02Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:45:02Z- Filter 0 No beams used in subcloud update 2021-02-12T23:45:02Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:45:02Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:45:02Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 739.6 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:45:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173501.66 sec. 2021-02-12T23:45:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:45:02Z- TNavBF::mmse selected from filter -1 2021-02-12T23:45:02Z- variances([0], [2], and psi_berg) = 739.599413 833.023978 0.000000 2021-02-12T23:45:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:45:02Z- LcmTrn::publishEstimates() - MLE : 4057743.67 591408.09 24.95 2021-02-12T23:45:02Z- LcmTrn::publishEstimates() - MMSE : 4057753.17 591455.59 24.95 2021-02-12T23:45:02Z- LcmTrn::publishEstimates() - COVAR: 739.60 833.02 0.77 0.00 2021-02-12T23:45:02Z- AHRS_M2 msg: 1613173502.04 epoch sec; seqNo:6476 2021-02-12T23:45:02Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.19 psi 2021-02-12T23:45:02Z- handleDepth()-Depth_Keller msg: 1613173502044 epoch sec; seqNo:6530; depth 24.83 2021-02-12T23:45:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173502.06 epoch sec; seqNo:6530; 4057753.56 north; 591456.81 east 2021-02-12T23:45:02Z- AHRS_M2 msg: 1613173502.44 epoch sec; seqNo:6477 2021-02-12T23:45:02Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.19 psi 2021-02-12T23:45:02Z- handleDepth()-Depth_Keller msg: 1613173502455 epoch sec; seqNo:6531; depth 24.72 2021-02-12T23:45:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173502.49 epoch sec; seqNo:6531; 4057753.94 north; 591456.80 east 2021-02-12T23:45:02Z- AHRS_M2 msg: 1613173502.85 epoch sec; seqNo:6478 2021-02-12T23:45:02Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.19 psi 2021-02-12T23:45:02Z- handleDepth()-Depth_Keller msg: 1613173502853 epoch sec; seqNo:6532; depth 24.59 2021-02-12T23:45:02Z- handleDvl() - RDI_Pathfinder msg: 1613173502.86 epoch sec; seqNo:3053 2021-02-12T23:45:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:45:02Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:45:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173502.88 epoch sec; seqNo:6532; 4057753.94 north; 591456.80 east 2021-02-12T23:45:03Z- AHRS_M2 msg: 1613173503.25 epoch sec; seqNo:6479 2021-02-12T23:45:03Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.19 psi 2021-02-12T23:45:03Z- handleDepth()-Depth_Keller msg: 1613173503267 epoch sec; seqNo:6533; depth 24.48 2021-02-12T23:45:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173503.28 epoch sec; seqNo:6533; 4057754.31 north; 591456.80 east 2021-02-12T23:45:03Z- AHRS_M2 msg: 1613173503.65 epoch sec; seqNo:6480 2021-02-12T23:45:03Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-12T23:45:03Z- handleDepth()-Depth_Keller msg: 1613173503660 epoch sec; seqNo:6534; depth 24.37 2021-02-12T23:45:03Z- handleDvl() - RDI_Pathfinder msg: 1613173503.67 epoch sec; seqNo:3054 2021-02-12T23:45:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 90.07 , 100.49 , 67.02 2021-02-12T23:45:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , 0.03 2021-02-12T23:45:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173503.70 epoch sec; seqNo:6534; 4057754.69 north; 591456.79 east 2021-02-12T23:45:04Z- AHRS_M2 msg: 1613173504.06 epoch sec; seqNo:6481 2021-02-12T23:45:04Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-12T23:45:04Z- handleDepth()-Depth_Keller msg: 1613173504069 epoch sec; seqNo:6535; depth 24.25 2021-02-12T23:45:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173504.08 epoch sec; seqNo:6535; 4057755.07 north; 591456.79 east 2021-02-12T23:45:04Z- AHRS_M2 msg: 1613173504.46 epoch sec; seqNo:6482 2021-02-12T23:45:04Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-12T23:45:04Z- handleDepth()-Depth_Keller msg: 1613173504471 epoch sec; seqNo:6536; depth 24.12 2021-02-12T23:45:04Z- handleDvl() - RDI_Pathfinder msg: 1613173504.48 epoch sec; seqNo:3055 2021-02-12T23:45:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:45:04Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:45:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173504.49 epoch sec; seqNo:6536; 4057755.07 north; 591456.79 east 2021-02-12T23:45:04Z- AHRS_M2 msg: 1613173504.87 epoch sec; seqNo:6483 2021-02-12T23:45:04Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-12T23:45:04Z- handleDepth()-Depth_Keller msg: 1613173504881 epoch sec; seqNo:6537; depth 24.01 2021-02-12T23:45:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173504.90 epoch sec; seqNo:6537; 4057755.46 north; 591458.01 east 2021-02-12T23:45:05Z- AHRS_M2 msg: 1613173505.27 epoch sec; seqNo:6484 2021-02-12T23:45:05Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.16 psi 2021-02-12T23:45:05Z- handleDepth()-Depth_Keller msg: 1613173505276 epoch sec; seqNo:6538; depth 23.89 2021-02-12T23:45:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173505.29 epoch sec; seqNo:6538; 4057755.84 north; 591458.00 east 2021-02-12T23:45:05Z- AHRS_M2 msg: 1613173505.68 epoch sec; seqNo:6485 2021-02-12T23:45:05Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-12T23:45:05Z- handleDepth()-Depth_Keller msg: 1613173505681 epoch sec; seqNo:6539; depth 23.78 2021-02-12T23:45:05Z- handleDvl() - RDI_Pathfinder msg: 1613173505.69 epoch sec; seqNo:3056 2021-02-12T23:45:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:45:05Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:45:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173505.70 epoch sec; seqNo:6539; 4057756.22 north; 591458.00 east 2021-02-12T23:45:06Z- AHRS_M2 msg: 1613173506.08 epoch sec; seqNo:6486 2021-02-12T23:45:06Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-12T23:45:06Z- handleDepth()-Depth_Keller msg: 1613173506094 epoch sec; seqNo:6540; depth 23.66 2021-02-12T23:45:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173506.11 epoch sec; seqNo:6540; 4057756.22 north; 591458.00 east 2021-02-12T23:45:06Z- AHRS_M2 msg: 1613173506.48 epoch sec; seqNo:6487 2021-02-12T23:45:06Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.14 psi 2021-02-12T23:45:06Z- handleDepth()-Depth_Keller msg: 1613173506493 epoch sec; seqNo:6541; depth 23.53 2021-02-12T23:45:06Z- handleDvl() - RDI_Pathfinder msg: 1613173506.51 epoch sec; seqNo:3057 2021-02-12T23:45:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 88.79 , 102.18 , 66.20 2021-02-12T23:45:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , 0.05 2021-02-12T23:45:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173506.53 epoch sec; seqNo:6541; 4057756.60 north; 591457.99 east 2021-02-12T23:45:06Z- AHRS_M2 msg: 1613173506.89 epoch sec; seqNo:6488 2021-02-12T23:45:06Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-12T23:45:06Z- handleDepth()-Depth_Keller msg: 1613173506901 epoch sec; seqNo:6542; depth 23.40 2021-02-12T23:45:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:45:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:45:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:45:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:45:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:45:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:45:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:45:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:45:06Z- handleDvl() - RDI_Pathfinder msg: 1613173506.90 epoch sec; seqNo:3058 2021-02-12T23:45:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:45:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , 0.05 2021-02-12T23:45:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173506.91 epoch sec; seqNo:6542; 4057756.98 north; 591457.99 east 2021-02-12T23:45:07Z- LcmTrn::handleMessages() - handling 45 messages took 0.61 ms and 133 calls... 2021-02-12T23:45:07Z- waiting for 1613173504.66, time is 1613173506.91 2021-02-12T23:45:07Z- LcmTrn::updateTrn() equating pose 1613173506.91 and meas times 1613173506.91 2021-02-12T23:45:07Z- LcmTrn::updateTrn() >>>> pose time 1613173506.91, meas time 1613173506.91 <<<< 2021-02-12T23:45:07Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173506.91, ping # 0. 2021-02-12T23:45:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173506.91 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:45:07Z- TerrainNav::motionUpdate() - currEstimate = (4057757, 591457.99, 23.40) 2021-02-12T23:45:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:45:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:45:07Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:45:07Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:45:07Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173506.91 sec.; No relevant navigation data available 2021-02-12T23:45:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:45:07Z- TNavBF::mmse selected from filter -1 2021-02-12T23:45:07Z- variances([0], [2], and psi_berg) = 739.509024 833.023904 0.000000 2021-02-12T23:45:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:45:07Z- LcmTrn::publishEstimates() - MLE : 4057747.48 591410.49 23.40 2021-02-12T23:45:07Z- LcmTrn::publishEstimates() - MMSE : 4057756.98 591457.99 23.40 2021-02-12T23:45:07Z- LcmTrn::publishEstimates() - COVAR: 739.51 833.02 1.00 0.00 2021-02-12T23:45:07Z- AHRS_M2 msg: 1613173507.31 epoch sec; seqNo:6489 2021-02-12T23:45:07Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.13 psi 2021-02-12T23:45:07Z- handleDepth()-Depth_Keller msg: 1613173507323 epoch sec; seqNo:6543; depth 23.29 2021-02-12T23:45:07Z- handleDvl() - RDI_Pathfinder msg: 1613173507.34 epoch sec; seqNo:3059 2021-02-12T23:45:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 0.00 , 103.19 , 68.24 2021-02-12T23:45:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.04 , 0.02 2021-02-12T23:45:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173507.35 epoch sec; seqNo:6543; 4057757.36 north; 591457.99 east 2021-02-12T23:45:07Z- AHRS_M2 msg: 1613173507.69 epoch sec; seqNo:6490 2021-02-12T23:45:07Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.13 psi 2021-02-12T23:45:07Z- handleDepth()-Depth_Keller msg: 1613173507705 epoch sec; seqNo:6544; depth 23.15 2021-02-12T23:45:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173507.71 epoch sec; seqNo:6544; 4057757.37 north; 591459.21 east 2021-02-12T23:45:08Z- AHRS_M2 msg: 1613173508.10 epoch sec; seqNo:6491 2021-02-12T23:45:08Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.13 psi 2021-02-12T23:45:08Z- handleDepth()-Depth_Keller msg: 1613173508104 epoch sec; seqNo:6545; depth 23.06 2021-02-12T23:45:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173508.11 epoch sec; seqNo:6545; 4057757.75 north; 591459.20 east 2021-02-12T23:45:08Z- AHRS_M2 msg: 1613173508.51 epoch sec; seqNo:6492 2021-02-12T23:45:08Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.13 psi 2021-02-12T23:45:08Z- handleDepth()-Depth_Keller msg: 1613173508514 epoch sec; seqNo:6546; depth 22.94 2021-02-12T23:45:08Z- handleDvl() - RDI_Pathfinder msg: 1613173508.53 epoch sec; seqNo:3060 2021-02-12T23:45:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.60 , 88.59 , 103.19 , 68.24 2021-02-12T23:45:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , -0.02 2021-02-12T23:45:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173508.54 epoch sec; seqNo:6546; 4057758.13 north; 591459.20 east 2021-02-12T23:45:08Z- AHRS_M2 msg: 1613173508.91 epoch sec; seqNo:6493 2021-02-12T23:45:08Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.13 psi 2021-02-12T23:45:08Z- handleDepth()-Depth_Keller msg: 1613173508922 epoch sec; seqNo:6547; depth 22.80 2021-02-12T23:45:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173508.93 epoch sec; seqNo:6547; 4057758.13 north; 591459.20 east 2021-02-12T23:45:09Z- AHRS_M2 msg: 1613173509.32 epoch sec; seqNo:6494 2021-02-12T23:45:09Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.13 psi 2021-02-12T23:45:09Z- handleDepth()-Depth_Keller msg: 1613173509326 epoch sec; seqNo:6548; depth 22.68 2021-02-12T23:45:09Z- handleDvl() - RDI_Pathfinder msg: 1613173509.34 epoch sec; seqNo:3061 2021-02-12T23:45:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.01 , 88.59 , 104.41 , 67.02 2021-02-12T23:45:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.04 , -0.07 2021-02-12T23:45:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173509.35 epoch sec; seqNo:6548; 4057758.51 north; 591459.19 east 2021-02-12T23:45:09Z- AHRS_M2 msg: 1613173509.72 epoch sec; seqNo:6495 2021-02-12T23:45:09Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.13 psi 2021-02-12T23:45:09Z- handleDepth()-Depth_Keller msg: 1613173509720 epoch sec; seqNo:6549; depth 22.56 2021-02-12T23:45:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173509.73 epoch sec; seqNo:6549; 4057758.51 north; 591459.19 east 2021-02-12T23:45:10Z- AHRS_M2 msg: 1613173510.12 epoch sec; seqNo:6496 2021-02-12T23:45:10Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.14 psi 2021-02-12T23:45:10Z- handleDepth()-Depth_Keller msg: 1613173510133 epoch sec; seqNo:6550; depth 22.45 2021-02-12T23:45:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173510.14 epoch sec; seqNo:6550; 4057758.90 north; 591460.41 east 2021-02-12T23:45:10Z- AHRS_M2 msg: 1613173510.53 epoch sec; seqNo:6497 2021-02-12T23:45:10Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.14 psi 2021-02-12T23:45:10Z- handleDepth()-Depth_Keller msg: 1613173510534 epoch sec; seqNo:6551; depth 22.31 2021-02-12T23:45:10Z- handleDvl() - RDI_Pathfinder msg: 1613173510.55 epoch sec; seqNo:3062 2021-02-12T23:45:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 89.74 , 101.90 , 68.65 2021-02-12T23:45:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.07 2021-02-12T23:45:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173510.56 epoch sec; seqNo:6551; 4057759.28 north; 591460.41 east 2021-02-12T23:45:10Z- AHRS_M2 msg: 1613173510.93 epoch sec; seqNo:6498 2021-02-12T23:45:10Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.14 psi 2021-02-12T23:45:10Z- handleDepth()-Depth_Keller msg: 1613173510932 epoch sec; seqNo:6552; depth 22.18 2021-02-12T23:45:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173510.94 epoch sec; seqNo:6552; 4057759.28 north; 591460.41 east 2021-02-12T23:45:11Z- AHRS_M2 msg: 1613173511.33 epoch sec; seqNo:6499 2021-02-12T23:45:11Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-12T23:45:11Z- handleDepth()-Depth_Keller msg: 1613173511336 epoch sec; seqNo:6553; depth 22.06 2021-02-12T23:45:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173511.35 epoch sec; seqNo:6553; 4057759.66 north; 591460.40 east 2021-02-12T23:45:11Z- AHRS_M2 msg: 1613173511.73 epoch sec; seqNo:6500 2021-02-12T23:45:11Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-12T23:45:11Z- handleDepth()-Depth_Keller msg: 1613173511740 epoch sec; seqNo:6554; depth 21.95 2021-02-12T23:45:11Z- handleDvl() - RDI_Pathfinder msg: 1613173511.76 epoch sec; seqNo:3063 2021-02-12T23:45:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 102.92 , 69.06 2021-02-12T23:45:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.03 2021-02-12T23:45:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173511.77 epoch sec; seqNo:6554; 4057760.04 north; 591460.40 east 2021-02-12T23:45:12Z- AHRS_M2 msg: 1613173512.14 epoch sec; seqNo:6501 2021-02-12T23:45:12Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.16 psi 2021-02-12T23:45:12Z- LcmTrn::handleMessages() - handling 42 messages took 0.72 ms and 128 calls... 2021-02-12T23:45:12Z- waiting for 1613173509.91, time is 1613173512.14 2021-02-12T23:45:12Z- LcmTrn::updateTrn() >>>> pose time 1613173512.14, meas time 1613173511.76 <<<< 2021-02-12T23:45:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173511.76 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:45:12Z- TerrainNav::motionUpdate() - currEstimate = (4057760, 591460.40, 21.95) 2021-02-12T23:45:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:45:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:45:12Z- TNavBF::Projecting Measurements 2021-02-12T23:45:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:45:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:45:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:45:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:45:12Z- TNavFilter::4 good beams. 2021-02-12T23:45:12Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:45:12Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:45:12Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:45:12Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:45:12Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:45:12Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:45:12Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:45:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:45:12Z- TerrainMapOctree: (vehN, vehE ) = (4057759.81, 591460.22). 2021-02-12T23:45:12Z- TerrainMapOctree: Min Distance = 4468.14. 2021-02-12T23:45:12Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:45:12Z- There is no specific good beam that all particles have in common. 2021-02-12T23:45:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:45:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-12T23:45:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:45:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 15.38 2021-02-12T23:45:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:45:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 16.95 2021-02-12T23:45:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:45:12Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-12T23:45:12Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:45:12Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:45:12Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:45:12Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:45:12Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:45:12Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:45:12Z- Filter 0 No beams used in subcloud update 2021-02-12T23:45:12Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:45:12Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:45:12Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 739.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:45:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173511.76 sec. 2021-02-12T23:45:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:45:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:45:12Z- variances([0], [2], and psi_berg) = 739.405551 833.023820 0.000000 2021-02-12T23:45:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:45:12Z- LcmTrn::publishEstimates() - MLE : 4057750.54 591412.90 21.95 2021-02-12T23:45:12Z- LcmTrn::publishEstimates() - MMSE : 4057760.04 591460.40 21.95 2021-02-12T23:45:12Z- LcmTrn::publishEstimates() - COVAR: 739.41 833.02 0.88 0.00 2021-02-12T23:45:12Z- handleDepth()-Depth_Keller msg: 1613173512154 epoch sec; seqNo:6555; depth 21.82 2021-02-12T23:45:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173512.21 epoch sec; seqNo:6555; 4057760.42 north; 591460.40 east 2021-02-12T23:45:12Z- AHRS_M2 msg: 1613173512.54 epoch sec; seqNo:6502 2021-02-12T23:45:12Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.16 psi 2021-02-12T23:45:12Z- handleDepth()-Depth_Keller msg: 1613173512548 epoch sec; seqNo:6556; depth 21.67 2021-02-12T23:45:12Z- handleDvl() - RDI_Pathfinder msg: 1613173512.56 epoch sec; seqNo:3064 2021-02-12T23:45:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:45:12Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:45:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173512.58 epoch sec; seqNo:6556; 4057760.42 north; 591460.40 east 2021-02-12T23:45:12Z- AHRS_M2 msg: 1613173512.95 epoch sec; seqNo:6503 2021-02-12T23:45:12Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-12T23:45:12Z- handleDepth()-Depth_Keller msg: 1613173512954 epoch sec; seqNo:6557; depth 21.56 2021-02-12T23:45:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173512.97 epoch sec; seqNo:6557; 4057760.81 north; 591461.61 east 2021-02-12T23:45:13Z- AHRS_M2 msg: 1613173513.35 epoch sec; seqNo:6504 2021-02-12T23:45:13Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.17 psi 2021-02-12T23:45:13Z- handleDepth()-Depth_Keller msg: 1613173513359 epoch sec; seqNo:6558; depth 21.45 2021-02-12T23:45:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173513.38 epoch sec; seqNo:6558; 4057761.19 north; 591461.61 east 2021-02-12T23:45:13Z- AHRS_M2 msg: 1613173513.76 epoch sec; seqNo:6505 2021-02-12T23:45:13Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-12T23:45:13Z- handleDepth()-Depth_Keller msg: 1613173513773 epoch sec; seqNo:6559; depth 21.32 2021-02-12T23:45:13Z- handleDvl() - RDI_Pathfinder msg: 1613173513.79 epoch sec; seqNo:3065 2021-02-12T23:45:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 106.50 , 67.43 2021-02-12T23:45:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , 0.06 2021-02-12T23:45:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173513.81 epoch sec; seqNo:6559; 4057761.56 north; 591461.60 east 2021-02-12T23:45:14Z- AHRS_M2 msg: 1613173514.16 epoch sec; seqNo:6506 2021-02-12T23:45:14Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-12T23:45:14Z- handleDepth()-Depth_Keller msg: 1613173514168 epoch sec; seqNo:6560; depth 21.20 2021-02-12T23:45:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173514.18 epoch sec; seqNo:6560; 4057761.56 north; 591461.60 east 2021-02-12T23:45:14Z- AHRS_M2 msg: 1613173514.59 epoch sec; seqNo:6507 2021-02-12T23:45:14Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-12T23:45:14Z- handleDepth()-Depth_Keller msg: 1613173514594 epoch sec; seqNo:6561; depth 21.07 2021-02-12T23:45:14Z- handleDvl() - RDI_Pathfinder msg: 1613173514.61 epoch sec; seqNo:3066 2021-02-12T23:45:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 93.12 , 106.50 , 78.51 2021-02-12T23:45:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.09 2021-02-12T23:45:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173514.62 epoch sec; seqNo:6561; 4057761.94 north; 591461.60 east 2021-02-12T23:45:14Z- AHRS_M2 msg: 1613173514.97 epoch sec; seqNo:6508 2021-02-12T23:45:14Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.17 psi 2021-02-12T23:45:14Z- handleDepth()-Depth_Keller msg: 1613173514977 epoch sec; seqNo:6562; depth 20.95 2021-02-12T23:45:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173514.99 epoch sec; seqNo:6562; 4057762.32 north; 591461.60 east 2021-02-12T23:45:15Z- AHRS_M2 msg: 1613173515.37 epoch sec; seqNo:6509 2021-02-12T23:45:15Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.17 psi 2021-02-12T23:45:15Z- handleDepth()-Depth_Keller msg: 1613173515377 epoch sec; seqNo:6563; depth 20.82 2021-02-12T23:45:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173515.39 epoch sec; seqNo:6563; 4057762.71 north; 591462.81 east 2021-02-12T23:45:15Z- AHRS_M2 msg: 1613173515.78 epoch sec; seqNo:6510 2021-02-12T23:45:15Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.16 psi 2021-02-12T23:45:15Z- handleDepth()-Depth_Keller msg: 1613173515790 epoch sec; seqNo:6564; depth 20.69 2021-02-12T23:45:15Z- handleDvl() - RDI_Pathfinder msg: 1613173515.80 epoch sec; seqNo:3067 2021-02-12T23:45:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 92.98 , 107.58 , 78.38 2021-02-12T23:45:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , 0.08 2021-02-12T23:45:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173515.81 epoch sec; seqNo:6564; 4057762.71 north; 591462.81 east 2021-02-12T23:45:16Z- AHRS_M2 msg: 1613173516.19 epoch sec; seqNo:6511 2021-02-12T23:45:16Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.16 psi 2021-02-12T23:45:16Z- handleDepth()-Depth_Keller msg: 1613173516190 epoch sec; seqNo:6565; depth 20.58 2021-02-12T23:45:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173516.22 epoch sec; seqNo:6565; 4057763.09 north; 591462.81 east 2021-02-12T23:45:16Z- AHRS_M2 msg: 1613173516.58 epoch sec; seqNo:6512 2021-02-12T23:45:16Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.16 psi 2021-02-12T23:45:16Z- handleDepth()-Depth_Keller msg: 1613173516588 epoch sec; seqNo:6566; depth 20.46 2021-02-12T23:45:16Z- handleDvl() - RDI_Pathfinder msg: 1613173516.60 epoch sec; seqNo:3068 2021-02-12T23:45:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.60 , 97.85 , 103.93 , 68.24 2021-02-12T23:45:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.09 , 0.04 2021-02-12T23:45:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173516.61 epoch sec; seqNo:6566; 4057763.47 north; 591462.80 east 2021-02-12T23:45:16Z- AHRS_M2 msg: 1613173516.99 epoch sec; seqNo:6513 2021-02-12T23:45:16Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.16 psi 2021-02-12T23:45:17Z- handleDepth()-Depth_Keller msg: 1613173517001 epoch sec; seqNo:6567; depth 20.33 2021-02-12T23:45:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173517.01 epoch sec; seqNo:6567; 4057763.47 north; 591462.80 east 2021-02-12T23:45:17Z- LcmTrn::handleMessages() - handling 43 messages took 0.58 ms and 132 calls... 2021-02-12T23:45:17Z- waiting for 1613173515.14, time is 1613173517.01 2021-02-12T23:45:17Z- LcmTrn::updateTrn() >>>> pose time 1613173517.01, meas time 1613173516.60 <<<< 2021-02-12T23:45:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173516.60 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:45:17Z- TerrainNav::motionUpdate() - currEstimate = (4057763, 591462.80, 20.33) 2021-02-12T23:45:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:45:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:45:17Z- TNavBF::Projecting Measurements 2021-02-12T23:45:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:45:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:45:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:45:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:45:17Z- TNavFilter::4 good beams. 2021-02-12T23:45:17Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:45:17Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:45:17Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:45:17Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:45:17Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:45:17Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:45:17Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:45:17Z- TNavBF::mmse selected from filter -1 2021-02-12T23:45:17Z- TerrainMapOctree: (vehN, vehE ) = (4057763.18, 591462.60). 2021-02-12T23:45:17Z- TerrainMapOctree: Min Distance = 4464.02. 2021-02-12T23:45:17Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:45:17Z- There is no specific good beam that all particles have in common. 2021-02-12T23:45:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:45:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 10.14 2021-02-12T23:45:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:45:17Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-12T23:45:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:45:17Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 17.28 2021-02-12T23:45:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:45:17Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-12T23:45:17Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:45:17Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:45:17Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:45:17Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:45:17Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:45:17Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:45:17Z- Filter 0 No beams used in subcloud update 2021-02-12T23:45:17Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:45:17Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:45:17Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 739.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:45:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173516.60 sec. 2021-02-12T23:45:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:45:17Z- TNavBF::mmse selected from filter -1 2021-02-12T23:45:17Z- variances([0], [2], and psi_berg) = 739.297061 833.023732 0.000000 2021-02-12T23:45:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:45:17Z- LcmTrn::publishEstimates() - MLE : 4057753.97 591415.30 20.33 2021-02-12T23:45:17Z- LcmTrn::publishEstimates() - MMSE : 4057763.47 591462.80 20.33 2021-02-12T23:45:17Z- LcmTrn::publishEstimates() - COVAR: 739.30 833.02 0.83 0.00 2021-02-12T23:45:17Z- AHRS_M2 msg: 1613173517.39 epoch sec; seqNo:6514 2021-02-12T23:45:17Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.16 psi 2021-02-12T23:45:17Z- handleDepth()-Depth_Keller msg: 1613173517401 epoch sec; seqNo:6568; depth 20.20 2021-02-12T23:45:17Z- handleDvl() - RDI_Pathfinder msg: 1613173517.42 epoch sec; seqNo:3069 2021-02-12T23:45:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 93.12 , 108.93 , 69.88 2021-02-12T23:45:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.12 , -0.01 2021-02-12T23:45:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173517.43 epoch sec; seqNo:6568; 4057763.85 north; 591462.80 east 2021-02-12T23:45:17Z- AHRS_M2 msg: 1613173517.80 epoch sec; seqNo:6515 2021-02-12T23:45:17Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.16 psi 2021-02-12T23:45:17Z- handleDepth()-Depth_Keller msg: 1613173517800 epoch sec; seqNo:6569; depth 20.07 2021-02-12T23:45:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173517.81 epoch sec; seqNo:6569; 4057764.24 north; 591464.02 east 2021-02-12T23:45:18Z- AHRS_M2 msg: 1613173518.20 epoch sec; seqNo:6516 2021-02-12T23:45:18Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.16 psi 2021-02-12T23:45:18Z- handleDepth()-Depth_Keller msg: 1613173518204 epoch sec; seqNo:6570; depth 19.97 2021-02-12T23:45:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173518.21 epoch sec; seqNo:6570; 4057764.24 north; 591464.02 east 2021-02-12T23:45:18Z- AHRS_M2 msg: 1613173518.60 epoch sec; seqNo:6517 2021-02-12T23:45:18Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.17 psi 2021-02-12T23:45:18Z- handleDepth()-Depth_Keller msg: 1613173518613 epoch sec; seqNo:6571; depth 19.82 2021-02-12T23:45:18Z- handleDvl() - RDI_Pathfinder msg: 1613173518.62 epoch sec; seqNo:3070 2021-02-12T23:45:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 97.71 , 108.66 , 70.70 2021-02-12T23:45:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.06 , -0.07 2021-02-12T23:45:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173518.64 epoch sec; seqNo:6571; 4057764.62 north; 591464.01 east 2021-02-12T23:45:19Z- AHRS_M2 msg: 1613173519.01 epoch sec; seqNo:6518 2021-02-12T23:45:19Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.18 psi 2021-02-12T23:45:19Z- handleDepth()-Depth_Keller msg: 1613173519012 epoch sec; seqNo:6572; depth 19.69 2021-02-12T23:45:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173519.02 epoch sec; seqNo:6572; 4057764.62 north; 591464.01 east 2021-02-12T23:45:19Z- AHRS_M2 msg: 1613173519.41 epoch sec; seqNo:6519 2021-02-12T23:45:19Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.19 psi 2021-02-12T23:45:19Z- handleDepth()-Depth_Keller msg: 1613173519421 epoch sec; seqNo:6573; depth 19.57 2021-02-12T23:45:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173519.43 epoch sec; seqNo:6573; 4057765.00 north; 591464.01 east 2021-02-12T23:45:19Z- AHRS_M2 msg: 1613173519.82 epoch sec; seqNo:6520 2021-02-12T23:45:19Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.21 psi 2021-02-12T23:45:19Z- handleDepth()-Depth_Keller msg: 1613173519829 epoch sec; seqNo:6574; depth 19.44 2021-02-12T23:45:19Z- handleDvl() - RDI_Pathfinder msg: 1613173519.84 epoch sec; seqNo:3071 2021-02-12T23:45:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.53 , 97.78 , 108.73 , 78.31 2021-02-12T23:45:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.04 , -0.06 2021-02-12T23:45:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173519.85 epoch sec; seqNo:6574; 4057765.38 north; 591464.01 east 2021-02-12T23:45:20Z- AHRS_M2 msg: 1613173520.22 epoch sec; seqNo:6521 2021-02-12T23:45:20Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.22 psi 2021-02-12T23:45:20Z- handleDepth()-Depth_Keller msg: 1613173520234 epoch sec; seqNo:6575; depth 19.30 2021-02-12T23:45:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173520.24 epoch sec; seqNo:6575; 4057765.39 north; 591465.23 east 2021-02-12T23:45:20Z- AHRS_M2 msg: 1613173520.62 epoch sec; seqNo:6522 2021-02-12T23:45:20Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.23 psi 2021-02-12T23:45:20Z- handleDepth()-Depth_Keller msg: 1613173520628 epoch sec; seqNo:6576; depth 19.17 2021-02-12T23:45:20Z- handleDvl() - RDI_Pathfinder msg: 1613173520.65 epoch sec; seqNo:3072 2021-02-12T23:45:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.18 , 98.86 , 109.81 , 78.18 2021-02-12T23:45:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.10 , -0.00 2021-02-12T23:45:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173520.66 epoch sec; seqNo:6576; 4057765.77 north; 591465.22 east 2021-02-12T23:45:21Z- AHRS_M2 msg: 1613173521.03 epoch sec; seqNo:6523 2021-02-12T23:45:21Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.24 psi 2021-02-12T23:45:21Z- handleDepth()-Depth_Keller msg: 1613173521032 epoch sec; seqNo:6577; depth 19.05 2021-02-12T23:45:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173521.04 epoch sec; seqNo:6577; 4057766.15 north; 591465.22 east 2021-02-12T23:45:21Z- AHRS_M2 msg: 1613173521.43 epoch sec; seqNo:6524 2021-02-12T23:45:21Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.24 psi 2021-02-12T23:45:21Z- handleDepth()-Depth_Keller msg: 1613173521438 epoch sec; seqNo:6578; depth 18.92 2021-02-12T23:45:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173521.45 epoch sec; seqNo:6578; 4057766.15 north; 591465.22 east 2021-02-12T23:45:21Z- AHRS_M2 msg: 1613173521.84 epoch sec; seqNo:6525 2021-02-12T23:45:21Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.24 psi 2021-02-12T23:45:21Z- handleDepth()-Depth_Keller msg: 1613173521845 epoch sec; seqNo:6579; depth 18.80 2021-02-12T23:45:21Z- handleDvl() - RDI_Pathfinder msg: 1613173521.86 epoch sec; seqNo:3073 2021-02-12T23:45:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.18 , 101.30 , 111.03 , 78.18 2021-02-12T23:45:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.09 , 0.05 2021-02-12T23:45:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173521.87 epoch sec; seqNo:6579; 4057766.53 north; 591465.21 east 2021-02-12T23:45:22Z- AHRS_M2 msg: 1613173522.24 epoch sec; seqNo:6526 2021-02-12T23:45:22Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.23 psi 2021-02-12T23:45:22Z- handleDepth()-Depth_Keller msg: 1613173522244 epoch sec; seqNo:6580; depth 18.68 2021-02-12T23:45:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173522.26 epoch sec; seqNo:6580; 4057766.91 north; 591465.21 east 2021-02-12T23:45:22Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-12T23:45:22Z- waiting for 1613173520.01, time is 1613173522.26 2021-02-12T23:45:22Z- LcmTrn::updateTrn() >>>> pose time 1613173522.26, meas time 1613173521.86 <<<< 2021-02-12T23:45:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173521.86 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:45:22Z- TerrainNav::motionUpdate() - currEstimate = (4057767, 591465.21, 18.68) 2021-02-12T23:45:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:45:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:45:22Z- TNavBF::Projecting Measurements 2021-02-12T23:45:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:45:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:45:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:45:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:45:22Z- TNavFilter::4 good beams. 2021-02-12T23:45:22Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:45:22Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:45:22Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:45:22Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:45:22Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:45:22Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:45:22Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:45:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:45:22Z- TerrainMapOctree: (vehN, vehE ) = (4057766.65, 591465.03). 2021-02-12T23:45:22Z- TerrainMapOctree: Min Distance = 4459.80. 2021-02-12T23:45:22Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:45:22Z- There is no specific good beam that all particles have in common. 2021-02-12T23:45:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:45:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 9.78 2021-02-12T23:45:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:45:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 16.42 2021-02-12T23:45:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:45:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 19.72 2021-02-12T23:45:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:45:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 9.78 2021-02-12T23:45:22Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:45:22Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:45:22Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:45:22Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:45:22Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:45:22Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:45:22Z- Filter 0 No beams used in subcloud update 2021-02-12T23:45:22Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:45:22Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:45:22Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 739.2 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:45:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173521.86 sec. 2021-02-12T23:45:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:45:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:45:22Z- variances([0], [2], and psi_berg) = 739.189785 833.023645 0.000000 2021-02-12T23:45:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:45:22Z- LcmTrn::publishEstimates() - MLE : 4057757.41 591417.71 18.68 2021-02-12T23:45:22Z- LcmTrn::publishEstimates() - MMSE : 4057766.91 591465.21 18.68 2021-02-12T23:45:22Z- LcmTrn::publishEstimates() - COVAR: 739.19 833.02 0.82 0.00 2021-02-12T23:45:22Z- AHRS_M2 msg: 1613173522.64 epoch sec; seqNo:6527 2021-02-12T23:45:22Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.23 psi 2021-02-12T23:45:22Z- handleDepth()-Depth_Keller msg: 1613173522648 epoch sec; seqNo:6581; depth 18.56 2021-02-12T23:45:22Z- handleDvl() - RDI_Pathfinder msg: 1613173522.66 epoch sec; seqNo:3074 2021-02-12T23:45:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.79 , 100.42 , 112.58 , 72.74 2021-02-12T23:45:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , 0.07 2021-02-12T23:45:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173522.67 epoch sec; seqNo:6581; 4057767.29 north; 591465.21 east 2021-02-12T23:45:23Z- AHRS_M2 msg: 1613173523.05 epoch sec; seqNo:6528 2021-02-12T23:45:23Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.22 psi 2021-02-12T23:45:23Z- handleDepth()-Depth_Keller msg: 1613173523061 epoch sec; seqNo:6582; depth 18.44 2021-02-12T23:45:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173523.07 epoch sec; seqNo:6582; 4057767.29 north; 591465.21 east 2021-02-12T23:45:23Z- AHRS_M2 msg: 1613173523.45 epoch sec; seqNo:6529 2021-02-12T23:45:23Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.20 psi 2021-02-12T23:45:23Z- handleDepth()-Depth_Keller msg: 1613173523461 epoch sec; seqNo:6583; depth 18.33 2021-02-12T23:45:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173523.48 epoch sec; seqNo:6583; 4057767.68 north; 591466.42 east 2021-02-12T23:45:23Z- AHRS_M2 msg: 1613173523.86 epoch sec; seqNo:6530 2021-02-12T23:45:23Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.19 psi 2021-02-12T23:45:23Z- handleDepth()-Depth_Keller msg: 1613173523860 epoch sec; seqNo:6584; depth 18.20 2021-02-12T23:45:23Z- handleDvl() - RDI_Pathfinder msg: 1613173523.87 epoch sec; seqNo:3075 2021-02-12T23:45:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.18 , 98.86 , 111.03 , 78.18 2021-02-12T23:45:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.00 , 0.08 2021-02-12T23:45:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173523.90 epoch sec; seqNo:6584; 4057768.06 north; 591466.42 east 2021-02-12T23:45:24Z- AHRS_M2 msg: 1613173524.26 epoch sec; seqNo:6531 2021-02-12T23:45:24Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.18 psi 2021-02-12T23:45:24Z- handleDepth()-Depth_Keller msg: 1613173524266 epoch sec; seqNo:6585; depth 18.08 2021-02-12T23:45:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173524.28 epoch sec; seqNo:6585; 4057768.44 north; 591466.41 east 2021-02-12T23:45:24Z- AHRS_M2 msg: 1613173524.66 epoch sec; seqNo:6532 2021-02-12T23:45:24Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.18 psi 2021-02-12T23:45:24Z- handleDepth()-Depth_Keller msg: 1613173524677 epoch sec; seqNo:6586; depth 17.96 2021-02-12T23:45:24Z- handleDvl() - RDI_Pathfinder msg: 1613173524.69 epoch sec; seqNo:3076 2021-02-12T23:45:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.39 , 96.43 , 109.81 , 78.18 2021-02-12T23:45:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , 0.06 2021-02-12T23:45:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173524.70 epoch sec; seqNo:6586; 4057768.44 north; 591466.41 east 2021-02-12T23:45:25Z- AHRS_M2 msg: 1613173525.07 epoch sec; seqNo:6533 2021-02-12T23:45:25Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.17 psi 2021-02-12T23:45:25Z- handleDepth()-Depth_Keller msg: 1613173525072 epoch sec; seqNo:6587; depth 17.83 2021-02-12T23:45:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173525.09 epoch sec; seqNo:6587; 4057768.81 north; 591466.41 east 2021-02-12T23:45:25Z- AHRS_M2 msg: 1613173525.47 epoch sec; seqNo:6534 2021-02-12T23:45:25Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.17 psi 2021-02-12T23:45:25Z- handleDepth()-Depth_Keller msg: 1613173525479 epoch sec; seqNo:6588; depth 17.73 2021-02-12T23:45:25Z- handleDvl() - RDI_Pathfinder msg: 1613173525.49 epoch sec; seqNo:3077 2021-02-12T23:45:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.83 , 98.86 , 111.03 , 71.92 2021-02-12T23:45:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , 0.03 2021-02-12T23:45:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173525.51 epoch sec; seqNo:6588; 4057769.19 north; 591466.41 east 2021-02-12T23:45:25Z- AHRS_M2 msg: 1613173525.88 epoch sec; seqNo:6535 2021-02-12T23:45:25Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.17 psi 2021-02-12T23:45:25Z- handleDepth()-Depth_Keller msg: 1613173525880 epoch sec; seqNo:6589; depth 17.59 2021-02-12T23:45:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173525.89 epoch sec; seqNo:6589; 4057769.19 north; 591466.41 east 2021-02-12T23:45:26Z- AHRS_M2 msg: 1613173526.28 epoch sec; seqNo:6536 2021-02-12T23:45:26Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.17 psi 2021-02-12T23:45:26Z- handleDepth()-Depth_Keller msg: 1613173526286 epoch sec; seqNo:6590; depth 17.48 2021-02-12T23:45:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173526.29 epoch sec; seqNo:6590; 4057769.59 north; 591467.62 east 2021-02-12T23:45:26Z- AHRS_M2 msg: 1613173526.68 epoch sec; seqNo:6537 2021-02-12T23:45:26Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.17 psi 2021-02-12T23:45:26Z- handleDepth()-Depth_Keller msg: 1613173526689 epoch sec; seqNo:6591; depth 17.36 2021-02-12T23:45:26Z- handleDvl() - RDI_Pathfinder msg: 1613173526.71 epoch sec; seqNo:3078 2021-02-12T23:45:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 97.71 , 112.32 , 71.51 2021-02-12T23:45:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , 0.01 2021-02-12T23:45:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173526.72 epoch sec; seqNo:6591; 4057769.96 north; 591467.62 east 2021-02-12T23:45:27Z- AHRS_M2 msg: 1613173527.09 epoch sec; seqNo:6538 2021-02-12T23:45:27Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.17 psi 2021-02-12T23:45:27Z- handleDepth()-Depth_Keller msg: 1613173527092 epoch sec; seqNo:6592; depth 17.22 2021-02-12T23:45:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173527.10 epoch sec; seqNo:6592; 4057770.34 north; 591467.62 east 2021-02-12T23:45:27Z- LcmTrn::handleMessages() - handling 41 messages took 0.61 ms and 129 calls... 2021-02-12T23:45:27Z- waiting for 1613173525.26, time is 1613173527.10 2021-02-12T23:45:27Z- LcmTrn::updateTrn() >>>> pose time 1613173527.10, meas time 1613173526.71 <<<< 2021-02-12T23:45:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173526.71 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:45:27Z- TerrainNav::motionUpdate() - currEstimate = (4057770, 591467.62, 17.22) 2021-02-12T23:45:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:45:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:45:27Z- TNavBF::Projecting Measurements 2021-02-12T23:45:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:45:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:45:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:45:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:45:27Z- TNavFilter::4 good beams. 2021-02-12T23:45:27Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:45:27Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:45:27Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:45:27Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:45:27Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:45:27Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:45:27Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:45:27Z- TNavBF::mmse selected from filter -1 2021-02-12T23:45:27Z- TerrainMapOctree: (vehN, vehE ) = (4057770.06, 591467.42). 2021-02-12T23:45:27Z- TerrainMapOctree: Min Distance = 4455.64. 2021-02-12T23:45:27Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:45:27Z- There is no specific good beam that all particles have in common. 2021-02-12T23:45:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:45:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 11.05 2021-02-12T23:45:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:45:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-12T23:45:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:45:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 20.18 2021-02-12T23:45:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:45:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 8.18 2021-02-12T23:45:27Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:45:27Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:45:27Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:45:27Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:45:27Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:45:27Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:45:27Z- Filter 0 No beams used in subcloud update 2021-02-12T23:45:27Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:45:27Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:45:27Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 739.1 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:45:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173526.71 sec. 2021-02-12T23:45:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:45:27Z- TNavBF::mmse selected from filter -1 2021-02-12T23:45:27Z- variances([0], [2], and psi_berg) = 739.081851 833.023557 0.000000 2021-02-12T23:45:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:45:27Z- LcmTrn::publishEstimates() - MLE : 4057760.84 591420.12 17.22 2021-02-12T23:45:27Z- LcmTrn::publishEstimates() - MMSE : 4057770.34 591467.62 17.22 2021-02-12T23:45:27Z- LcmTrn::publishEstimates() - COVAR: 739.08 833.02 0.80 0.00 2021-02-12T23:45:27Z- AHRS_M2 msg: 1613173527.51 epoch sec; seqNo:6539 2021-02-12T23:45:27Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.17 psi 2021-02-12T23:45:27Z- handleDepth()-Depth_Keller msg: 1613173527522 epoch sec; seqNo:6593; depth 17.10 2021-02-12T23:45:27Z- handleDvl() - RDI_Pathfinder msg: 1613173527.54 epoch sec; seqNo:3079 2021-02-12T23:45:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.83 , 97.65 , 112.25 , 78.18 2021-02-12T23:45:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , 0.01 2021-02-12T23:45:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173527.55 epoch sec; seqNo:6593; 4057770.72 north; 591467.61 east 2021-02-12T23:45:27Z- AHRS_M2 msg: 1613173527.89 epoch sec; seqNo:6540 2021-02-12T23:45:27Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.17 psi 2021-02-12T23:45:27Z- handleDepth()-Depth_Keller msg: 1613173527910 epoch sec; seqNo:6594; depth 16.97 2021-02-12T23:45:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173527.93 epoch sec; seqNo:6594; 4057770.72 north; 591467.61 east 2021-02-12T23:45:28Z- AHRS_M2 msg: 1613173528.30 epoch sec; seqNo:6541 2021-02-12T23:45:28Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.17 psi 2021-02-12T23:45:28Z- handleDepth()-Depth_Keller msg: 1613173528310 epoch sec; seqNo:6595; depth 16.86 2021-02-12T23:45:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173528.35 epoch sec; seqNo:6595; 4057771.10 north; 591467.61 east 2021-02-12T23:45:28Z- AHRS_M2 msg: 1613173528.70 epoch sec; seqNo:6542 2021-02-12T23:45:28Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-12T23:45:28Z- handleDepth()-Depth_Keller msg: 1613173528709 epoch sec; seqNo:6596; depth 16.74 2021-02-12T23:45:28Z- handleDvl() - RDI_Pathfinder msg: 1613173528.72 epoch sec; seqNo:3080 2021-02-12T23:45:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 99.07 , 113.94 , 71.92 2021-02-12T23:45:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.01 2021-02-12T23:45:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173528.73 epoch sec; seqNo:6596; 4057771.48 north; 591467.60 east 2021-02-12T23:45:29Z- AHRS_M2 msg: 1613173529.11 epoch sec; seqNo:6543 2021-02-12T23:45:29Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.16 psi 2021-02-12T23:45:29Z- handleDepth()-Depth_Keller msg: 1613173529118 epoch sec; seqNo:6597; depth 16.61 2021-02-12T23:45:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173529.14 epoch sec; seqNo:6597; 4057771.87 north; 591468.82 east 2021-02-12T23:45:29Z- AHRS_M2 msg: 1613173529.51 epoch sec; seqNo:6544 2021-02-12T23:45:29Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.16 psi 2021-02-12T23:45:29Z- handleDepth()-Depth_Keller msg: 1613173529521 epoch sec; seqNo:6598; depth 16.48 2021-02-12T23:45:29Z- handleDvl() - RDI_Pathfinder msg: 1613173529.53 epoch sec; seqNo:3081 2021-02-12T23:45:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.79 , 110.96 , 78.51 2021-02-12T23:45:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , 0.00 2021-02-12T23:45:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173529.56 epoch sec; seqNo:6598; 4057771.87 north; 591468.82 east 2021-02-12T23:45:29Z- AHRS_M2 msg: 1613173529.91 epoch sec; seqNo:6545 2021-02-12T23:45:29Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-12T23:45:29Z- handleDepth()-Depth_Keller msg: 1613173529920 epoch sec; seqNo:6599; depth 16.36 2021-02-12T23:45:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173529.93 epoch sec; seqNo:6599; 4057772.25 north; 591468.82 east 2021-02-12T23:45:30Z- AHRS_M2 msg: 1613173530.32 epoch sec; seqNo:6546 2021-02-12T23:45:30Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-12T23:45:30Z- handleDepth()-Depth_Keller msg: 1613173530325 epoch sec; seqNo:6600; depth 16.25 2021-02-12T23:45:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173530.34 epoch sec; seqNo:6600; 4057772.63 north; 591468.81 east 2021-02-12T23:45:30Z- AHRS_M2 msg: 1613173530.73 epoch sec; seqNo:6547 2021-02-12T23:45:30Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-12T23:45:30Z- handleDepth()-Depth_Keller msg: 1613173530734 epoch sec; seqNo:6601; depth 16.14 2021-02-12T23:45:30Z- handleDvl() - RDI_Pathfinder msg: 1613173530.75 epoch sec; seqNo:3082 2021-02-12T23:45:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.50 , 97.38 , 113.60 , 78.45 2021-02-12T23:45:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.06 , 0.00 2021-02-12T23:45:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173530.76 epoch sec; seqNo:6601; 4057772.63 north; 591468.81 east 2021-02-12T23:45:31Z- AHRS_M2 msg: 1613173531.14 epoch sec; seqNo:6548 2021-02-12T23:45:31Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-12T23:45:31Z- handleDepth()-Depth_Keller msg: 1613173531144 epoch sec; seqNo:6602; depth 15.99 2021-02-12T23:45:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173531.17 epoch sec; seqNo:6602; 4057773.01 north; 591468.81 east 2021-02-12T23:45:31Z- AHRS_M2 msg: 1613173531.53 epoch sec; seqNo:6549 2021-02-12T23:45:31Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-12T23:45:31Z- handleDepth()-Depth_Keller msg: 1613173531536 epoch sec; seqNo:6603; depth 15.87 2021-02-12T23:45:31Z- handleDvl() - RDI_Pathfinder msg: 1613173531.55 epoch sec; seqNo:3083 2021-02-12T23:45:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.13 , 97.65 , 111.17 , 74.37 2021-02-12T23:45:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.08 , 0.01 2021-02-12T23:45:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173531.56 epoch sec; seqNo:6603; 4057773.39 north; 591468.80 east 2021-02-12T23:45:31Z- AHRS_M2 msg: 1613173531.93 epoch sec; seqNo:6550 2021-02-12T23:45:31Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-12T23:45:31Z- handleDepth()-Depth_Keller msg: 1613173531946 epoch sec; seqNo:6604; depth 15.75 2021-02-12T23:45:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173531.96 epoch sec; seqNo:6604; 4057773.78 north; 591470.02 east 2021-02-12T23:45:32Z- AHRS_M2 msg: 1613173532.34 epoch sec; seqNo:6551 2021-02-12T23:45:32Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-12T23:45:32Z- handleDepth()-Depth_Keller msg: 1613173532344 epoch sec; seqNo:6605; depth 15.63 2021-02-12T23:45:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173532.35 epoch sec; seqNo:6605; 4057773.78 north; 591470.02 east 2021-02-12T23:45:32Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-12T23:45:32Z- waiting for 1613173530.10, time is 1613173532.35 2021-02-12T23:45:32Z- LcmTrn::updateTrn() >>>> pose time 1613173532.35, meas time 1613173531.55 <<<< 2021-02-12T23:45:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173531.55 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:45:32Z- TerrainNav::motionUpdate() - currEstimate = (4057774, 591470.02, 15.63) 2021-02-12T23:45:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:45:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:45:32Z- TNavBF::Projecting Measurements 2021-02-12T23:45:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:45:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:45:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:45:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:45:32Z- TNavFilter::4 good beams. 2021-02-12T23:45:32Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:45:32Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:45:32Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:45:32Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:45:32Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:45:32Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:45:32Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:45:32Z- TNavBF::mmse selected from filter -1 2021-02-12T23:45:32Z- TerrainMapOctree: (vehN, vehE ) = (4057773.25, 591469.65). 2021-02-12T23:45:32Z- TerrainMapOctree: Min Distance = 4451.75. 2021-02-12T23:45:32Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:45:32Z- There is no specific good beam that all particles have in common. 2021-02-12T23:45:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:45:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 11.32 2021-02-12T23:45:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:45:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 15.26 2021-02-12T23:45:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:45:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 19.77 2021-02-12T23:45:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:45:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-12T23:45:32Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:45:32Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:45:32Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:45:32Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:45:32Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:45:32Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:45:32Z- Filter 0 No beams used in subcloud update 2021-02-12T23:45:32Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:45:32Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:45:32Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 739.0 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:45:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173531.55 sec. 2021-02-12T23:45:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:45:32Z- TNavBF::mmse selected from filter -1 2021-02-12T23:45:32Z- variances([0], [2], and psi_berg) = 738.968250 833.023465 0.000000 2021-02-12T23:45:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:45:32Z- LcmTrn::publishEstimates() - MLE : 4057764.28 591422.52 15.63 2021-02-12T23:45:32Z- LcmTrn::publishEstimates() - MMSE : 4057773.78 591470.02 15.63 2021-02-12T23:45:32Z- LcmTrn::publishEstimates() - COVAR: 738.97 833.02 0.81 0.00 2021-02-12T23:45:32Z- AHRS_M2 msg: 1613173532.75 epoch sec; seqNo:6552 2021-02-12T23:45:32Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-12T23:45:32Z- handleDepth()-Depth_Keller msg: 1613173532751 epoch sec; seqNo:6606; depth 15.52 2021-02-12T23:45:32Z- handleDvl() - RDI_Pathfinder msg: 1613173532.76 epoch sec; seqNo:3084 2021-02-12T23:45:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.71 , 109.88 , 78.79 2021-02-12T23:45:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.08 , 0.02 2021-02-12T23:45:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173532.77 epoch sec; seqNo:6606; 4057774.16 north; 591470.02 east 2021-02-12T23:45:33Z- AHRS_M2 msg: 1613173533.16 epoch sec; seqNo:6553 2021-02-12T23:45:33Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-12T23:45:33Z- handleDepth()-Depth_Keller msg: 1613173533161 epoch sec; seqNo:6607; depth 15.39 2021-02-12T23:45:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173533.17 epoch sec; seqNo:6607; 4057774.54 north; 591470.01 east 2021-02-12T23:45:33Z- AHRS_M2 msg: 1613173533.55 epoch sec; seqNo:6554 2021-02-12T23:45:33Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-12T23:45:33Z- handleDepth()-Depth_Keller msg: 1613173533556 epoch sec; seqNo:6608; depth 15.27 2021-02-12T23:45:33Z- handleDvl() - RDI_Pathfinder msg: 1613173533.57 epoch sec; seqNo:3085 2021-02-12T23:45:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 97.44 , 110.96 , 78.51 2021-02-12T23:45:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , 0.04 2021-02-12T23:45:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173533.59 epoch sec; seqNo:6608; 4057774.92 north; 591470.01 east 2021-02-12T23:45:33Z- AHRS_M2 msg: 1613173533.96 epoch sec; seqNo:6555 2021-02-12T23:45:33Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-12T23:45:33Z- handleDepth()-Depth_Keller msg: 1613173533969 epoch sec; seqNo:6609; depth 15.16 2021-02-12T23:45:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173533.98 epoch sec; seqNo:6609; 4057774.92 north; 591470.01 east 2021-02-12T23:45:34Z- AHRS_M2 msg: 1613173534.36 epoch sec; seqNo:6556 2021-02-12T23:45:34Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-12T23:45:34Z- handleDepth()-Depth_Keller msg: 1613173534366 epoch sec; seqNo:6610; depth 15.05 2021-02-12T23:45:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173534.37 epoch sec; seqNo:6610; 4057775.29 north; 591470.00 east 2021-02-12T23:45:34Z- AHRS_M2 msg: 1613173534.76 epoch sec; seqNo:6557 2021-02-12T23:45:34Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-12T23:45:34Z- handleDepth()-Depth_Keller msg: 1613173534769 epoch sec; seqNo:6611; depth 14.92 2021-02-12T23:45:34Z- handleDvl() - RDI_Pathfinder msg: 1613173534.79 epoch sec; seqNo:3086 2021-02-12T23:45:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:45:34Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:45:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173534.80 epoch sec; seqNo:6611; 4057775.69 north; 591471.22 east 2021-02-12T23:45:35Z- AHRS_M2 msg: 1613173535.17 epoch sec; seqNo:6558 2021-02-12T23:45:35Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.13 psi 2021-02-12T23:45:35Z- handleDepth()-Depth_Keller msg: 1613173535181 epoch sec; seqNo:6612; depth 14.82 2021-02-12T23:45:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173535.21 epoch sec; seqNo:6612; 4057776.07 north; 591471.22 east 2021-02-12T23:45:35Z- AHRS_M2 msg: 1613173535.57 epoch sec; seqNo:6559 2021-02-12T23:45:35Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.12 psi 2021-02-12T23:45:35Z- handleDepth()-Depth_Keller msg: 1613173535576 epoch sec; seqNo:6613; depth 14.70 2021-02-12T23:45:35Z- handleDvl() - RDI_Pathfinder msg: 1613173535.59 epoch sec; seqNo:3087 2021-02-12T23:45:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:45:35Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:45:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173535.60 epoch sec; seqNo:6613; 4057776.07 north; 591471.22 east 2021-02-12T23:45:35Z- AHRS_M2 msg: 1613173535.98 epoch sec; seqNo:6560 2021-02-12T23:45:35Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.12 psi 2021-02-12T23:45:35Z- handleDepth()-Depth_Keller msg: 1613173535982 epoch sec; seqNo:6614; depth 14.59 2021-02-12T23:45:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173535.99 epoch sec; seqNo:6614; 4057776.44 north; 591471.21 east 2021-02-12T23:45:36Z- AHRS_M2 msg: 1613173536.38 epoch sec; seqNo:6561 2021-02-12T23:45:36Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.12 psi 2021-02-12T23:45:36Z- handleDepth()-Depth_Keller msg: 1613173536384 epoch sec; seqNo:6615; depth 14.48 2021-02-12T23:45:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173536.40 epoch sec; seqNo:6615; 4057776.82 north; 591471.21 east 2021-02-12T23:45:36Z- AHRS_M2 msg: 1613173536.78 epoch sec; seqNo:6562 2021-02-12T23:45:36Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.13 psi 2021-02-12T23:45:36Z- handleDepth()-Depth_Keller msg: 1613173536788 epoch sec; seqNo:6616; depth 14.37 2021-02-12T23:45:36Z- handleDvl() - RDI_Pathfinder msg: 1613173536.80 epoch sec; seqNo:3088 2021-02-12T23:45:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 97.24 , 110.62 , 77.91 2021-02-12T23:45:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.04 2021-02-12T23:45:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173536.83 epoch sec; seqNo:6616; 4057776.82 north; 591471.21 east 2021-02-12T23:45:37Z- AHRS_M2 msg: 1613173537.21 epoch sec; seqNo:6563 2021-02-12T23:45:37Z- AHRS_M2 msg: 0.67 phi; 0.30 theta; -0.13 psi 2021-02-12T23:45:37Z- handleDepth()-Depth_Keller msg: 1613173537217 epoch sec; seqNo:6617; depth 14.26 2021-02-12T23:45:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173537.23 epoch sec; seqNo:6617; 4057777.20 north; 591471.21 east 2021-02-12T23:45:37Z- AHRS_M2 msg: 1613173537.60 epoch sec; seqNo:6564 2021-02-12T23:45:37Z- AHRS_M2 msg: 0.67 phi; 0.30 theta; -0.14 psi 2021-02-12T23:45:37Z- handleDepth()-Depth_Keller msg: 1613173537603 epoch sec; seqNo:6618; depth 14.13 2021-02-12T23:45:37Z- handleDvl() - RDI_Pathfinder msg: 1613173537.62 epoch sec; seqNo:3089 2021-02-12T23:45:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.13 , 97.65 , 107.11 , 78.72 2021-02-12T23:45:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.07 , -0.07 2021-02-12T23:45:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173537.63 epoch sec; seqNo:6618; 4057777.59 north; 591472.42 east 2021-02-12T23:45:37Z- LcmTrn::handleMessages() - handling 45 messages took 0.74 ms and 130 calls... 2021-02-12T23:45:37Z- waiting for 1613173535.35, time is 1613173537.63 2021-02-12T23:45:37Z- LcmTrn::updateTrn() equating pose 1613173537.63 and meas times 1613173537.63 2021-02-12T23:45:37Z- LcmTrn::updateTrn() >>>> pose time 1613173537.63, meas time 1613173537.63 <<<< 2021-02-12T23:45:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173537.63 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:45:37Z- TerrainNav::motionUpdate() - currEstimate = (4057778, 591472.42, 14.13) 2021-02-12T23:45:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:45:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:45:37Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173537.63 sec.; No relevant navigation data available 2021-02-12T23:45:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:45:37Z- TNavBF::mmse selected from filter -1 2021-02-12T23:45:37Z- variances([0], [2], and psi_berg) = 738.878569 833.023392 0.000000 2021-02-12T23:45:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:45:37Z- LcmTrn::publishEstimates() - MLE : 4057768.09 591424.92 14.13 2021-02-12T23:45:37Z- LcmTrn::publishEstimates() - MMSE : 4057777.59 591472.42 14.13 2021-02-12T23:45:37Z- LcmTrn::publishEstimates() - COVAR: 738.88 833.02 1.04 0.00 2021-02-12T23:45:37Z- AHRS_M2 msg: 1613173537.99 epoch sec; seqNo:6565 2021-02-12T23:45:37Z- AHRS_M2 msg: 0.67 phi; 0.29 theta; -0.14 psi 2021-02-12T23:45:38Z- handleDepth()-Depth_Keller msg: 1613173538006 epoch sec; seqNo:6619; depth 14.03 2021-02-12T23:45:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173538.02 epoch sec; seqNo:6619; 4057777.59 north; 591472.42 east 2021-02-12T23:45:38Z- AHRS_M2 msg: 1613173538.40 epoch sec; seqNo:6566 2021-02-12T23:45:38Z- AHRS_M2 msg: 0.66 phi; 0.29 theta; -0.15 psi 2021-02-12T23:45:38Z- handleDepth()-Depth_Keller msg: 1613173538404 epoch sec; seqNo:6620; depth 13.91 2021-02-12T23:45:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173538.41 epoch sec; seqNo:6620; 4057777.97 north; 591472.42 east 2021-02-12T23:45:38Z- AHRS_M2 msg: 1613173538.80 epoch sec; seqNo:6567 2021-02-12T23:45:38Z- AHRS_M2 msg: 0.66 phi; 0.29 theta; -0.16 psi 2021-02-12T23:45:38Z- handleDepth()-Depth_Keller msg: 1613173538808 epoch sec; seqNo:6621; depth 13.81 2021-02-12T23:45:38Z- handleDvl() - RDI_Pathfinder msg: 1613173538.82 epoch sec; seqNo:3090 2021-02-12T23:45:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 97.71 , 108.53 , 78.79 2021-02-12T23:45:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.17 , -0.01 2021-02-12T23:45:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173538.83 epoch sec; seqNo:6621; 4057778.35 north; 591472.41 east 2021-02-12T23:45:39Z- AHRS_M2 msg: 1613173539.22 epoch sec; seqNo:6568 2021-02-12T23:45:39Z- AHRS_M2 msg: 0.65 phi; 0.29 theta; -0.17 psi 2021-02-12T23:45:39Z- handleDepth()-Depth_Keller msg: 1613173539222 epoch sec; seqNo:6622; depth 13.71 2021-02-12T23:45:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173539.23 epoch sec; seqNo:6622; 4057778.73 north; 591472.41 east 2021-02-12T23:45:39Z- AHRS_M2 msg: 1613173539.61 epoch sec; seqNo:6569 2021-02-12T23:45:39Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.18 psi 2021-02-12T23:45:39Z- handleDepth()-Depth_Keller msg: 1613173539618 epoch sec; seqNo:6623; depth 13.60 2021-02-12T23:45:39Z- handleDvl() - RDI_Pathfinder msg: 1613173539.64 epoch sec; seqNo:3091 2021-02-12T23:45:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 100.42 , 107.18 , 78.79 2021-02-12T23:45:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.19 , 0.09 2021-02-12T23:45:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173539.65 epoch sec; seqNo:6623; 4057778.73 north; 591472.41 east 2021-02-12T23:45:40Z- AHRS_M2 msg: 1613173540.02 epoch sec; seqNo:6570 2021-02-12T23:45:40Z- AHRS_M2 msg: 0.63 phi; 0.29 theta; -0.19 psi 2021-02-12T23:45:40Z- handleDepth()-Depth_Keller msg: 1613173540020 epoch sec; seqNo:6624; depth 13.50 2021-02-12T23:45:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173540.03 epoch sec; seqNo:6624; 4057779.11 north; 591472.41 east 2021-02-12T23:45:40Z- AHRS_M2 msg: 1613173540.42 epoch sec; seqNo:6571 2021-02-12T23:45:40Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.19 psi 2021-02-12T23:45:40Z- handleDepth()-Depth_Keller msg: 1613173540429 epoch sec; seqNo:6625; depth 13.39 2021-02-12T23:45:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173540.44 epoch sec; seqNo:6625; 4057779.49 north; 591472.40 east 2021-02-12T23:45:40Z- AHRS_M2 msg: 1613173540.83 epoch sec; seqNo:6572 2021-02-12T23:45:40Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.19 psi 2021-02-12T23:45:40Z- handleDepth()-Depth_Keller msg: 1613173540830 epoch sec; seqNo:6626; depth 13.29 2021-02-12T23:45:40Z- handleDvl() - RDI_Pathfinder msg: 1613173540.84 epoch sec; seqNo:3092 2021-02-12T23:45:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 101.77 , 105.82 , 78.79 2021-02-12T23:45:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.11 , 0.15 2021-02-12T23:45:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173540.85 epoch sec; seqNo:6626; 4057779.88 north; 591473.62 east 2021-02-12T23:45:41Z- AHRS_M2 msg: 1613173541.23 epoch sec; seqNo:6573 2021-02-12T23:45:41Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.19 psi 2021-02-12T23:45:41Z- handleDepth()-Depth_Keller msg: 1613173541233 epoch sec; seqNo:6627; depth 13.18 2021-02-12T23:45:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173541.24 epoch sec; seqNo:6627; 4057780.26 north; 591473.61 east 2021-02-12T23:45:41Z- AHRS_M2 msg: 1613173541.63 epoch sec; seqNo:6574 2021-02-12T23:45:41Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.19 psi 2021-02-12T23:45:41Z- handleDepth()-Depth_Keller msg: 1613173541636 epoch sec; seqNo:6628; depth 13.10 2021-02-12T23:45:41Z- handleDvl() - RDI_Pathfinder msg: 1613173541.65 epoch sec; seqNo:3093 2021-02-12T23:45:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 101.77 , 108.53 , 78.79 2021-02-12T23:45:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , 0.12 2021-02-12T23:45:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173541.66 epoch sec; seqNo:6628; 4057780.26 north; 591473.61 east 2021-02-12T23:45:42Z- AHRS_M2 msg: 1613173542.04 epoch sec; seqNo:6575 2021-02-12T23:45:42Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.18 psi 2021-02-12T23:45:42Z- handleDepth()-Depth_Keller msg: 1613173542042 epoch sec; seqNo:6629; depth 12.97 2021-02-12T23:45:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173542.05 epoch sec; seqNo:6629; 4057780.64 north; 591473.61 east 2021-02-12T23:45:42Z- AHRS_M2 msg: 1613173542.44 epoch sec; seqNo:6576 2021-02-12T23:45:42Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.18 psi 2021-02-12T23:45:42Z- handleDepth()-Depth_Keller msg: 1613173542444 epoch sec; seqNo:6630; depth 12.88 2021-02-12T23:45:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173542.46 epoch sec; seqNo:6630; 4057781.02 north; 591473.61 east 2021-02-12T23:45:42Z- LcmTrn::handleMessages() - handling 40 messages took 0.61 ms and 128 calls... 2021-02-12T23:45:42Z- waiting for 1613173540.63, time is 1613173542.46 2021-02-12T23:45:42Z- LcmTrn::updateTrn() >>>> pose time 1613173542.46, meas time 1613173541.65 <<<< 2021-02-12T23:45:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173541.65 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:45:42Z- TerrainNav::motionUpdate() - currEstimate = (4057781, 591473.61, 12.88) 2021-02-12T23:45:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:45:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:45:42Z- TNavBF::Projecting Measurements 2021-02-12T23:45:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:45:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:45:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:45:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:45:42Z- TNavFilter::4 good beams. 2021-02-12T23:45:42Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:45:42Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:45:42Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:45:42Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:45:42Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:45:42Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:45:42Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:45:42Z- TNavBF::mmse selected from filter -1 2021-02-12T23:45:42Z- TerrainMapOctree: (vehN, vehE ) = (4057780.44, 591473.41). 2021-02-12T23:45:42Z- TerrainMapOctree: Min Distance = 4443.79. 2021-02-12T23:45:42Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:45:42Z- There is no specific good beam that all particles have in common. 2021-02-12T23:45:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:45:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 11.34 2021-02-12T23:45:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:45:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 16.57 2021-02-12T23:45:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:45:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 18.85 2021-02-12T23:45:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:45:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 9.93 2021-02-12T23:45:42Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:45:42Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:45:42Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:45:42Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:45:42Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:45:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:45:42Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:45:42Z- Filter 0 No beams used in subcloud update 2021-02-12T23:45:42Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:45:42Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:45:42Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 738.8 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:45:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173541.65 sec. 2021-02-12T23:45:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:45:42Z- TNavBF::mmse selected from filter -1 2021-02-12T23:45:42Z- variances([0], [2], and psi_berg) = 738.772440 833.023306 0.000000 2021-02-12T23:45:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:45:42Z- LcmTrn::publishEstimates() - MLE : 4057771.52 591426.11 12.88 2021-02-12T23:45:42Z- LcmTrn::publishEstimates() - MMSE : 4057781.02 591473.61 12.88 2021-02-12T23:45:42Z- LcmTrn::publishEstimates() - COVAR: 738.77 833.02 0.94 0.00 2021-02-12T23:45:42Z- AHRS_M2 msg: 1613173542.84 epoch sec; seqNo:6577 2021-02-12T23:45:42Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.17 psi 2021-02-12T23:45:42Z- handleDepth()-Depth_Keller msg: 1613173542848 epoch sec; seqNo:6631; depth 12.79 2021-02-12T23:45:42Z- handleDvl() - RDI_Pathfinder msg: 1613173542.86 epoch sec; seqNo:3094 2021-02-12T23:45:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 101.63 , 111.10 , 78.65 2021-02-12T23:45:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , 0.08 2021-02-12T23:45:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173542.88 epoch sec; seqNo:6631; 4057781.39 north; 591473.60 east 2021-02-12T23:45:43Z- AHRS_M2 msg: 1613173543.25 epoch sec; seqNo:6578 2021-02-12T23:45:43Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.17 psi 2021-02-12T23:45:43Z- handleDepth()-Depth_Keller msg: 1613173543257 epoch sec; seqNo:6632; depth 12.67 2021-02-12T23:45:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173543.27 epoch sec; seqNo:6632; 4057781.79 north; 591474.82 east 2021-02-12T23:45:43Z- AHRS_M2 msg: 1613173543.65 epoch sec; seqNo:6579 2021-02-12T23:45:43Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.17 psi 2021-02-12T23:45:43Z- handleDepth()-Depth_Keller msg: 1613173543677 epoch sec; seqNo:6633; depth 12.59 2021-02-12T23:45:43Z- handleDvl() - RDI_Pathfinder msg: 1613173543.69 epoch sec; seqNo:3095 2021-02-12T23:45:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.63 , 109.75 , 78.65 2021-02-12T23:45:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , 0.05 2021-02-12T23:45:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173543.72 epoch sec; seqNo:6633; 4057781.79 north; 591474.82 east 2021-02-12T23:45:44Z- AHRS_M2 msg: 1613173544.06 epoch sec; seqNo:6580 2021-02-12T23:45:44Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.17 psi 2021-02-12T23:45:44Z- handleDepth()-Depth_Keller msg: 1613173544060 epoch sec; seqNo:6634; depth 12.48 2021-02-12T23:45:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173544.08 epoch sec; seqNo:6634; 4057782.17 north; 591474.82 east 2021-02-12T23:45:44Z- AHRS_M2 msg: 1613173544.46 epoch sec; seqNo:6581 2021-02-12T23:45:44Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.17 psi 2021-02-12T23:45:44Z- handleDepth()-Depth_Keller msg: 1613173544468 epoch sec; seqNo:6635; depth 12.39 2021-02-12T23:45:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173544.51 epoch sec; seqNo:6635; 4057782.54 north; 591474.81 east 2021-02-12T23:45:44Z- AHRS_M2 msg: 1613173544.86 epoch sec; seqNo:6582 2021-02-12T23:45:44Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.17 psi 2021-02-12T23:45:44Z- handleDepth()-Depth_Keller msg: 1613173544873 epoch sec; seqNo:6636; depth 12.30 2021-02-12T23:45:44Z- handleDvl() - RDI_Pathfinder msg: 1613173544.88 epoch sec; seqNo:3096 2021-02-12T23:45:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.36 , 110.83 , 79.73 2021-02-12T23:45:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.08 , 0.02 2021-02-12T23:45:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173544.89 epoch sec; seqNo:6636; 4057782.92 north; 591474.81 east 2021-02-12T23:45:45Z- AHRS_M2 msg: 1613173545.27 epoch sec; seqNo:6583 2021-02-12T23:45:45Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.17 psi 2021-02-12T23:45:45Z- handleDepth()-Depth_Keller msg: 1613173545275 epoch sec; seqNo:6637; depth 12.19 2021-02-12T23:45:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173545.29 epoch sec; seqNo:6637; 4057782.92 north; 591474.81 east 2021-02-12T23:45:45Z- AHRS_M2 msg: 1613173545.67 epoch sec; seqNo:6584 2021-02-12T23:45:45Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.17 psi 2021-02-12T23:45:45Z- handleDepth()-Depth_Keller msg: 1613173545681 epoch sec; seqNo:6638; depth 12.10 2021-02-12T23:45:45Z- handleDvl() - RDI_Pathfinder msg: 1613173545.69 epoch sec; seqNo:3097 2021-02-12T23:45:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.30 , 112.11 , 79.67 2021-02-12T23:45:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.08 , -0.08 2021-02-12T23:45:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173545.71 epoch sec; seqNo:6638; 4057783.32 north; 591476.02 east 2021-02-12T23:45:46Z- AHRS_M2 msg: 1613173546.08 epoch sec; seqNo:6585 2021-02-12T23:45:46Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.18 psi 2021-02-12T23:45:46Z- handleDepth()-Depth_Keller msg: 1613173546086 epoch sec; seqNo:6639; depth 11.98 2021-02-12T23:45:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173546.10 epoch sec; seqNo:6639; 4057783.69 north; 591476.02 east 2021-02-12T23:45:46Z- AHRS_M2 msg: 1613173546.48 epoch sec; seqNo:6586 2021-02-12T23:45:46Z- AHRS_M2 msg: 0.63 phi; 0.29 theta; -0.18 psi 2021-02-12T23:45:46Z- handleDepth()-Depth_Keller msg: 1613173546484 epoch sec; seqNo:6640; depth 11.89 2021-02-12T23:45:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173546.49 epoch sec; seqNo:6640; 4057783.69 north; 591476.02 east 2021-02-12T23:45:46Z- AHRS_M2 msg: 1613173546.88 epoch sec; seqNo:6587 2021-02-12T23:45:46Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.18 psi 2021-02-12T23:45:46Z- handleDepth()-Depth_Keller msg: 1613173546890 epoch sec; seqNo:6641; depth 11.78 2021-02-12T23:45:46Z- handleDvl() - RDI_Pathfinder msg: 1613173546.90 epoch sec; seqNo:3098 2021-02-12T23:45:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.07 , 104.00 , 114.82 , 79.67 2021-02-12T23:45:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.13 2021-02-12T23:45:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173546.92 epoch sec; seqNo:6641; 4057784.07 north; 591476.02 east 2021-02-12T23:45:47Z- AHRS_M2 msg: 1613173547.29 epoch sec; seqNo:6588 2021-02-12T23:45:47Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.18 psi 2021-02-12T23:45:47Z- handleDepth()-Depth_Keller msg: 1613173547292 epoch sec; seqNo:6642; depth 11.68 2021-02-12T23:45:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173547.31 epoch sec; seqNo:6642; 4057784.45 north; 591476.01 east 2021-02-12T23:45:47Z- AHRS_M2 msg: 1613173547.69 epoch sec; seqNo:6589 2021-02-12T23:45:47Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.18 psi 2021-02-12T23:45:47Z- handleDepth()-Depth_Keller msg: 1613173547696 epoch sec; seqNo:6643; depth 11.58 2021-02-12T23:45:47Z- handleDvl() - RDI_Pathfinder msg: 1613173547.71 epoch sec; seqNo:3099 2021-02-12T23:45:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.21 , 102.78 , 113.60 , 79.80 2021-02-12T23:45:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.18 , -0.06 2021-02-12T23:45:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173547.72 epoch sec; seqNo:6643; 4057784.45 north; 591476.01 east 2021-02-12T23:45:47Z- LcmTrn::handleMessages() - handling 45 messages took 0.78 ms and 130 calls... 2021-02-12T23:45:47Z- waiting for 1613173545.46, time is 1613173547.72 2021-02-12T23:45:47Z- LcmTrn::updateTrn() equating pose 1613173547.72 and meas times 1613173547.72 2021-02-12T23:45:47Z- LcmTrn::updateTrn() >>>> pose time 1613173547.72, meas time 1613173547.72 <<<< 2021-02-12T23:45:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173547.72 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:45:47Z- TerrainNav::motionUpdate() - currEstimate = (4057784, 591476.01, 11.58) 2021-02-12T23:45:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:45:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:45:47Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173547.72 sec.; No relevant navigation data available 2021-02-12T23:45:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:45:47Z- TNavBF::mmse selected from filter -1 2021-02-12T23:45:47Z- variances([0], [2], and psi_berg) = 738.686143 833.023236 0.000000 2021-02-12T23:45:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:45:47Z- LcmTrn::publishEstimates() - MLE : 4057774.95 591434.51 11.58 2021-02-12T23:45:47Z- LcmTrn::publishEstimates() - MMSE : 4057784.45 591476.01 11.58 2021-02-12T23:45:47Z- LcmTrn::publishEstimates() - COVAR: 738.69 833.02 1.15 0.00 2021-02-12T23:45:48Z- AHRS_M2 msg: 1613173548.10 epoch sec; seqNo:6590 2021-02-12T23:45:48Z- AHRS_M2 msg: 0.63 phi; 0.29 theta; -0.18 psi 2021-02-12T23:45:48Z- handleDepth()-Depth_Keller msg: 1613173548113 epoch sec; seqNo:6644; depth 11.46 2021-02-12T23:45:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173548.13 epoch sec; seqNo:6644; 4057784.83 north; 591476.01 east 2021-02-12T23:45:48Z- AHRS_M2 msg: 1613173548.50 epoch sec; seqNo:6591 2021-02-12T23:45:48Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.17 psi 2021-02-12T23:45:48Z- handleDepth()-Depth_Keller msg: 1613173548507 epoch sec; seqNo:6645; depth 11.37 2021-02-12T23:45:48Z- handleDvl() - RDI_Pathfinder msg: 1613173548.52 epoch sec; seqNo:3100 2021-02-12T23:45:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.85 , 109.61 , 78.05 2021-02-12T23:45:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.21 , 0.05 2021-02-12T23:45:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173548.54 epoch sec; seqNo:6645; 4057785.22 north; 591477.23 east 2021-02-12T23:45:48Z- AHRS_M2 msg: 1613173548.90 epoch sec; seqNo:6592 2021-02-12T23:45:48Z- AHRS_M2 msg: 0.61 phi; 0.29 theta; -0.17 psi 2021-02-12T23:45:48Z- handleDepth()-Depth_Keller msg: 1613173548913 epoch sec; seqNo:6646; depth 11.26 2021-02-12T23:45:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173548.92 epoch sec; seqNo:6646; 4057785.60 north; 591477.22 east 2021-02-12T23:45:49Z- AHRS_M2 msg: 1613173549.32 epoch sec; seqNo:6593 2021-02-12T23:45:49Z- AHRS_M2 msg: 0.61 phi; 0.29 theta; -0.17 psi 2021-02-12T23:45:49Z- handleDepth()-Depth_Keller msg: 1613173549327 epoch sec; seqNo:6647; depth 11.15 2021-02-12T23:45:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173549.34 epoch sec; seqNo:6647; 4057785.98 north; 591477.22 east 2021-02-12T23:45:49Z- AHRS_M2 msg: 1613173549.71 epoch sec; seqNo:6594 2021-02-12T23:45:49Z- AHRS_M2 msg: 0.61 phi; 0.29 theta; -0.16 psi 2021-02-12T23:45:49Z- handleDepth()-Depth_Keller msg: 1613173549726 epoch sec; seqNo:6648; depth 11.03 2021-02-12T23:45:49Z- handleDvl() - RDI_Pathfinder msg: 1613173549.74 epoch sec; seqNo:3101 2021-02-12T23:45:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 102.72 , 113.53 , 78.46 2021-02-12T23:45:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.11 , 0.04 2021-02-12T23:45:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173549.75 epoch sec; seqNo:6648; 4057785.98 north; 591477.22 east 2021-02-12T23:45:50Z- AHRS_M2 msg: 1613173550.12 epoch sec; seqNo:6595 2021-02-12T23:45:50Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.16 psi 2021-02-12T23:45:50Z- handleDepth()-Depth_Keller msg: 1613173550129 epoch sec; seqNo:6649; depth 10.94 2021-02-12T23:45:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173550.14 epoch sec; seqNo:6649; 4057786.36 north; 591477.21 east 2021-02-12T23:45:50Z- AHRS_M2 msg: 1613173550.54 epoch sec; seqNo:6596 2021-02-12T23:45:50Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.16 psi 2021-02-12T23:45:50Z- handleDepth()-Depth_Keller msg: 1613173550544 epoch sec; seqNo:6650; depth 10.84 2021-02-12T23:45:50Z- handleDvl() - RDI_Pathfinder msg: 1613173550.56 epoch sec; seqNo:3102 2021-02-12T23:45:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 102.85 , 110.96 , 79.87 2021-02-12T23:45:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , -0.06 2021-02-12T23:45:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173550.57 epoch sec; seqNo:6650; 4057786.74 north; 591477.21 east 2021-02-12T23:45:50Z- AHRS_M2 msg: 1613173550.92 epoch sec; seqNo:6597 2021-02-12T23:45:50Z- AHRS_M2 msg: 0.63 phi; 0.28 theta; -0.16 psi 2021-02-12T23:45:50Z- handleDepth()-Depth_Keller msg: 1613173550928 epoch sec; seqNo:6651; depth 10.75 2021-02-12T23:45:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173550.95 epoch sec; seqNo:6651; 4057787.12 north; 591477.20 east 2021-02-12T23:45:51Z- AHRS_M2 msg: 1613173551.33 epoch sec; seqNo:6598 2021-02-12T23:45:51Z- AHRS_M2 msg: 0.63 phi; 0.28 theta; -0.16 psi 2021-02-12T23:45:51Z- handleDepth()-Depth_Keller msg: 1613173551336 epoch sec; seqNo:6652; depth 10.65 2021-02-12T23:45:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173551.34 epoch sec; seqNo:6652; 4057787.12 north; 591477.20 east 2021-02-12T23:45:51Z- AHRS_M2 msg: 1613173551.73 epoch sec; seqNo:6599 2021-02-12T23:45:51Z- AHRS_M2 msg: 0.64 phi; 0.28 theta; -0.16 psi 2021-02-12T23:45:51Z- handleDepth()-Depth_Keller msg: 1613173551736 epoch sec; seqNo:6653; depth 10.54 2021-02-12T23:45:51Z- handleDvl() - RDI_Pathfinder msg: 1613173551.75 epoch sec; seqNo:3103 2021-02-12T23:45:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 101.50 , 110.96 , 79.87 2021-02-12T23:45:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.06 , -0.09 2021-02-12T23:45:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173551.76 epoch sec; seqNo:6653; 4057787.51 north; 591478.42 east 2021-02-12T23:45:52Z- AHRS_M2 msg: 1613173552.13 epoch sec; seqNo:6600 2021-02-12T23:45:52Z- AHRS_M2 msg: 0.64 phi; 0.28 theta; -0.16 psi 2021-02-12T23:45:52Z- handleDepth()-Depth_Keller msg: 1613173552149 epoch sec; seqNo:6654; depth 10.43 2021-02-12T23:45:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173552.16 epoch sec; seqNo:6654; 4057787.89 north; 591478.42 east 2021-02-12T23:45:52Z- AHRS_M2 msg: 1613173552.54 epoch sec; seqNo:6601 2021-02-12T23:45:52Z- AHRS_M2 msg: 0.63 phi; 0.28 theta; -0.16 psi 2021-02-12T23:45:52Z- handleDepth()-Depth_Keller msg: 1613173552547 epoch sec; seqNo:6655; depth 10.33 2021-02-12T23:45:52Z- handleDvl() - RDI_Pathfinder msg: 1613173552.56 epoch sec; seqNo:3104 2021-02-12T23:45:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:45:52Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:45:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173552.58 epoch sec; seqNo:6655; 4057787.89 north; 591478.42 east 2021-02-12T23:45:52Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-12T23:45:52Z- waiting for 1613173550.72, time is 1613173552.58 2021-02-12T23:45:52Z- LcmTrn::updateTrn() equating pose 1613173552.58 and meas times 1613173552.58 2021-02-12T23:45:52Z- LcmTrn::updateTrn() >>>> pose time 1613173552.58, meas time 1613173552.58 <<<< 2021-02-12T23:45:52Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173552.58, ping # 0. 2021-02-12T23:45:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173552.58 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:45:52Z- TerrainNav::motionUpdate() - currEstimate = (4057788, 591478.42, 10.33) 2021-02-12T23:45:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:45:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:45:52Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:45:52Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:45:52Z- TNavBF::Projecting Measurements 2021-02-12T23:45:52Z- TNavFilter::Measurement from time = 1613173552.58 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:45:52Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:45:52Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:45:52Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:45:52Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:45:52Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:45:52Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:45:52Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:45:52Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 738.6 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-12T23:45:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:45:52Z- TNavBF::mmse selected from filter -1 2021-02-12T23:45:52Z- variances([0], [2], and psi_berg) = 738.599939 833.023167 0.000000 2021-02-12T23:45:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:45:52Z- LcmTrn::publishEstimates() - MLE : 4057778.39 591436.92 10.33 2021-02-12T23:45:52Z- LcmTrn::publishEstimates() - MMSE : 4057787.89 591478.42 10.33 2021-02-12T23:45:52Z- LcmTrn::publishEstimates() - COVAR: 738.60 833.02 1.36 0.00 2021-02-12T23:45:52Z- AHRS_M2 msg: 1613173552.94 epoch sec; seqNo:6602 2021-02-12T23:45:52Z- AHRS_M2 msg: 0.63 phi; 0.28 theta; -0.16 psi 2021-02-12T23:45:52Z- handleDepth()-Depth_Keller msg: 1613173552948 epoch sec; seqNo:6656; depth 10.26 2021-02-12T23:45:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173552.96 epoch sec; seqNo:6656; 4057788.27 north; 591478.41 east 2021-02-12T23:45:53Z- AHRS_M2 msg: 1613173553.35 epoch sec; seqNo:6603 2021-02-12T23:45:53Z- AHRS_M2 msg: 0.63 phi; 0.28 theta; -0.16 psi 2021-02-12T23:45:53Z- handleDepth()-Depth_Keller msg: 1613173553358 epoch sec; seqNo:6657; depth 10.15 2021-02-12T23:45:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173553.37 epoch sec; seqNo:6657; 4057788.27 north; 591478.41 east 2021-02-12T23:45:53Z- AHRS_M2 msg: 1613173553.75 epoch sec; seqNo:6604 2021-02-12T23:45:53Z- AHRS_M2 msg: 0.63 phi; 0.28 theta; -0.16 psi 2021-02-12T23:45:53Z- handleDepth()-Depth_Keller msg: 1613173553766 epoch sec; seqNo:6658; depth 10.08 2021-02-12T23:45:53Z- handleDvl() - RDI_Pathfinder msg: 1613173553.78 epoch sec; seqNo:3105 2021-02-12T23:45:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.82 , 102.18 , 114.07 , 81.36 2021-02-12T23:45:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , 0.11 2021-02-12T23:45:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173553.79 epoch sec; seqNo:6658; 4057788.65 north; 591478.41 east 2021-02-12T23:45:54Z- AHRS_M2 msg: 1613173554.16 epoch sec; seqNo:6605 2021-02-12T23:45:54Z- AHRS_M2 msg: 0.63 phi; 0.29 theta; -0.16 psi 2021-02-12T23:45:54Z- handleDepth()-Depth_Keller msg: 1613173554161 epoch sec; seqNo:6659; depth 9.96 2021-02-12T23:45:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173554.17 epoch sec; seqNo:6659; 4057789.02 north; 591478.41 east 2021-02-12T23:45:54Z- AHRS_M2 msg: 1613173554.57 epoch sec; seqNo:6606 2021-02-12T23:45:54Z- AHRS_M2 msg: 0.63 phi; 0.29 theta; -0.16 psi 2021-02-12T23:45:54Z- handleDepth()-Depth_Keller msg: 1613173554584 epoch sec; seqNo:6660; depth 9.87 2021-02-12T23:45:54Z- handleDvl() - RDI_Pathfinder msg: 1613173554.63 epoch sec; seqNo:3106 2021-02-12T23:45:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 101.43 , 112.25 , 81.15 2021-02-12T23:45:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.10 2021-02-12T23:45:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173554.64 epoch sec; seqNo:6660; 4057789.42 north; 591479.62 east 2021-02-12T23:45:54Z- AHRS_M2 msg: 1613173554.96 epoch sec; seqNo:6607 2021-02-12T23:45:54Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.15 psi 2021-02-12T23:45:54Z- handleDepth()-Depth_Keller msg: 1613173554977 epoch sec; seqNo:6661; depth 9.76 2021-02-12T23:45:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173554.99 epoch sec; seqNo:6661; 4057789.42 north; 591479.62 east 2021-02-12T23:45:55Z- AHRS_M2 msg: 1613173555.37 epoch sec; seqNo:6608 2021-02-12T23:45:55Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.15 psi 2021-02-12T23:45:55Z- handleDepth()-Depth_Keller msg: 1613173555372 epoch sec; seqNo:6662; depth 9.70 2021-02-12T23:45:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173555.40 epoch sec; seqNo:6662; 4057789.79 north; 591479.62 east 2021-02-12T23:45:55Z- AHRS_M2 msg: 1613173555.77 epoch sec; seqNo:6609 2021-02-12T23:45:55Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.15 psi 2021-02-12T23:45:55Z- handleDepth()-Depth_Keller msg: 1613173555776 epoch sec; seqNo:6663; depth 9.60 2021-02-12T23:45:55Z- handleDvl() - RDI_Pathfinder msg: 1613173555.79 epoch sec; seqNo:3107 2021-02-12T23:45:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.85 , 113.67 , 81.22 2021-02-12T23:45:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.13 , 0.04 2021-02-12T23:45:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173555.81 epoch sec; seqNo:6663; 4057790.17 north; 591479.61 east 2021-02-12T23:45:56Z- AHRS_M2 msg: 1613173556.17 epoch sec; seqNo:6610 2021-02-12T23:45:56Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.16 psi 2021-02-12T23:45:56Z- handleDepth()-Depth_Keller msg: 1613173556180 epoch sec; seqNo:6664; depth 9.49 2021-02-12T23:45:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173556.19 epoch sec; seqNo:6664; 4057790.55 north; 591479.61 east 2021-02-12T23:45:56Z- AHRS_M2 msg: 1613173556.60 epoch sec; seqNo:6611 2021-02-12T23:45:56Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.17 psi 2021-02-12T23:45:56Z- handleDepth()-Depth_Keller msg: 1613173556604 epoch sec; seqNo:6665; depth 9.39 2021-02-12T23:45:56Z- handleDvl() - RDI_Pathfinder msg: 1613173556.62 epoch sec; seqNo:3108 2021-02-12T23:45:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 103.86 , 110.62 , 82.23 2021-02-12T23:45:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.17 , 0.07 2021-02-12T23:45:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173556.63 epoch sec; seqNo:6665; 4057790.93 north; 591479.61 east 2021-02-12T23:45:56Z- AHRS_M2 msg: 1613173556.98 epoch sec; seqNo:6612 2021-02-12T23:45:56Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.18 psi 2021-02-12T23:45:56Z- handleDepth()-Depth_Keller msg: 1613173556988 epoch sec; seqNo:6666; depth 9.27 2021-02-12T23:45:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173557.01 epoch sec; seqNo:6666; 4057791.31 north; 591479.60 east 2021-02-12T23:45:57Z- AHRS_M2 msg: 1613173557.39 epoch sec; seqNo:6613 2021-02-12T23:45:57Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.19 psi 2021-02-12T23:45:57Z- handleDepth()-Depth_Keller msg: 1613173557392 epoch sec; seqNo:6667; depth 9.19 2021-02-12T23:45:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173557.40 epoch sec; seqNo:6667; 4057791.70 north; 591480.82 east 2021-02-12T23:45:57Z- AHRS_M2 msg: 1613173557.79 epoch sec; seqNo:6614 2021-02-12T23:45:57Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-12T23:45:57Z- handleDepth()-Depth_Keller msg: 1613173557796 epoch sec; seqNo:6668; depth 9.10 2021-02-12T23:45:57Z- handleDvl() - RDI_Pathfinder msg: 1613173557.81 epoch sec; seqNo:3109 2021-02-12T23:45:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.91 , 106.84 , 117.65 , 79.28 2021-02-12T23:45:57Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , -0.04 , 0.19 2021-02-12T23:45:57Z- LcmTrn::handleMessages() - handling 43 messages took 0.71 ms and 129 calls... 2021-02-12T23:45:57Z- waiting for 1613173555.58, time is 1613173557.81 2021-02-12T23:45:57Z- LcmTrn::updateTrn() equating pose 1613173557.81 and meas times 1613173557.81 2021-02-12T23:45:57Z- LcmTrn::updateTrn() >>>> pose time 1613173557.81, meas time 1613173557.81 <<<< 2021-02-12T23:45:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173557.81 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:45:57Z- TerrainNav::motionUpdate() - currEstimate = (4057792, 591480.82, 9.10) 2021-02-12T23:45:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:45:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:45:57Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173557.81 sec.; No relevant navigation data available 2021-02-12T23:45:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:45:57Z- TNavBF::mmse selected from filter -1 2021-02-12T23:45:57Z- variances([0], [2], and psi_berg) = 738.510666 833.023095 0.000000 2021-02-12T23:45:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:45:57Z- LcmTrn::publishEstimates() - MLE : 4057782.20 591439.32 9.10 2021-02-12T23:45:57Z- LcmTrn::publishEstimates() - MMSE : 4057791.70 591480.82 9.10 2021-02-12T23:45:57Z- LcmTrn::publishEstimates() - COVAR: 738.51 833.02 1.59 0.00 2021-02-12T23:45:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173557.82 epoch sec; seqNo:6668; 4057792.08 north; 591480.81 east 2021-02-12T23:45:58Z- AHRS_M2 msg: 1613173558.19 epoch sec; seqNo:6615 2021-02-12T23:45:58Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.17 psi 2021-02-12T23:45:58Z- handleDepth()-Depth_Keller msg: 1613173558209 epoch sec; seqNo:6669; depth 9.00 2021-02-12T23:45:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173558.22 epoch sec; seqNo:6669; 4057792.46 north; 591480.81 east 2021-02-12T23:45:58Z- AHRS_M2 msg: 1613173558.60 epoch sec; seqNo:6616 2021-02-12T23:45:58Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-12T23:45:58Z- handleDepth()-Depth_Keller msg: 1613173558609 epoch sec; seqNo:6670; depth 8.91 2021-02-12T23:45:58Z- handleDvl() - RDI_Pathfinder msg: 1613173558.63 epoch sec; seqNo:3110 2021-02-12T23:45:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 106.70 , 117.52 , 0.00 2021-02-12T23:45:58Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.20 , 0.14 2021-02-12T23:45:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173558.64 epoch sec; seqNo:6670; 4057792.46 north; 591480.81 east 2021-02-12T23:45:59Z- AHRS_M2 msg: 1613173559.00 epoch sec; seqNo:6617 2021-02-12T23:45:59Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-12T23:45:59Z- handleDepth()-Depth_Keller msg: 1613173559008 epoch sec; seqNo:6671; depth 8.80 2021-02-12T23:45:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173559.02 epoch sec; seqNo:6671; 4057792.84 north; 591480.81 east 2021-02-12T23:45:59Z- AHRS_M2 msg: 1613173559.42 epoch sec; seqNo:6618 2021-02-12T23:45:59Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-12T23:45:59Z- handleDepth()-Depth_Keller msg: 1613173559430 epoch sec; seqNo:6672; depth 8.71 2021-02-12T23:45:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173559.45 epoch sec; seqNo:6672; 4057793.23 north; 591482.02 east 2021-02-12T23:45:59Z- AHRS_M2 msg: 1613173559.82 epoch sec; seqNo:6619 2021-02-12T23:45:59Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.14 psi 2021-02-12T23:45:59Z- handleDepth()-Depth_Keller msg: 1613173559833 epoch sec; seqNo:6673; depth 8.61 2021-02-12T23:45:59Z- handleDvl() - RDI_Pathfinder msg: 1613173559.84 epoch sec; seqNo:3111 2021-02-12T23:45:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.51 , 103.46 , 119.82 , 168.89 2021-02-12T23:45:59Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.27 , -0.08 2021-02-12T23:45:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173559.85 epoch sec; seqNo:6673; 4057793.23 north; 591482.02 east 2021-02-12T23:46:00Z- AHRS_M2 msg: 1613173560.22 epoch sec; seqNo:6620 2021-02-12T23:46:00Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-12T23:46:00Z- handleDepth()-Depth_Keller msg: 1613173560229 epoch sec; seqNo:6674; depth 8.49 2021-02-12T23:46:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173560.25 epoch sec; seqNo:6674; 4057793.61 north; 591482.02 east 2021-02-12T23:46:00Z- AHRS_M2 msg: 1613173560.63 epoch sec; seqNo:6621 2021-02-12T23:46:00Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.16 psi 2021-02-12T23:46:00Z- handleDepth()-Depth_Keller msg: 1613173560636 epoch sec; seqNo:6675; depth 8.37 2021-02-12T23:46:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173560.67 epoch sec; seqNo:6675; 4057793.99 north; 591482.02 east 2021-02-12T23:46:01Z- AHRS_M2 msg: 1613173561.03 epoch sec; seqNo:6622 2021-02-12T23:46:01Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.17 psi 2021-02-12T23:46:01Z- handleDepth()-Depth_Keller msg: 1613173561037 epoch sec; seqNo:6676; depth 8.26 2021-02-12T23:46:01Z- handleDvl() - RDI_Pathfinder msg: 1613173561.05 epoch sec; seqNo:3112 2021-02-12T23:46:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 103.18 , 118.05 , 82.36 2021-02-12T23:46:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , 0.07 , -0.28 2021-02-12T23:46:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173561.06 epoch sec; seqNo:6676; 4057794.00 north; 591483.24 east 2021-02-12T23:46:01Z- AHRS_M2 msg: 1613173561.44 epoch sec; seqNo:6623 2021-02-12T23:46:01Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.18 psi 2021-02-12T23:46:01Z- handleDepth()-Depth_Keller msg: 1613173561440 epoch sec; seqNo:6677; depth 8.15 2021-02-12T23:46:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173561.46 epoch sec; seqNo:6677; 4057794.38 north; 591483.23 east 2021-02-12T23:46:01Z- AHRS_M2 msg: 1613173561.84 epoch sec; seqNo:6624 2021-02-12T23:46:01Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.19 psi 2021-02-12T23:46:01Z- handleDepth()-Depth_Keller msg: 1613173561844 epoch sec; seqNo:6678; depth 8.02 2021-02-12T23:46:01Z- handleDvl() - RDI_Pathfinder msg: 1613173561.86 epoch sec; seqNo:3113 2021-02-12T23:46:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 106.50 , 118.67 , 82.17 2021-02-12T23:46:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.21 , -0.16 2021-02-12T23:46:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173561.88 epoch sec; seqNo:6678; 4057794.38 north; 591483.23 east 2021-02-12T23:46:02Z- AHRS_M2 msg: 1613173562.24 epoch sec; seqNo:6625 2021-02-12T23:46:02Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.20 psi 2021-02-12T23:46:02Z- handleDepth()-Depth_Keller msg: 1613173562253 epoch sec; seqNo:6679; depth 7.90 2021-02-12T23:46:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173562.26 epoch sec; seqNo:6679; 4057794.76 north; 591483.23 east 2021-02-12T23:46:02Z- AHRS_M2 msg: 1613173562.65 epoch sec; seqNo:6626 2021-02-12T23:46:02Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.20 psi 2021-02-12T23:46:02Z- handleDepth()-Depth_Keller msg: 1613173562659 epoch sec; seqNo:6680; depth 7.80 2021-02-12T23:46:02Z- handleDvl() - RDI_Pathfinder msg: 1613173562.67 epoch sec; seqNo:3114 2021-02-12T23:46:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.98 , 109.88 , 124.75 , 83.11 2021-02-12T23:46:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.25 , 0.01 2021-02-12T23:46:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173562.68 epoch sec; seqNo:6680; 4057795.14 north; 591483.22 east 2021-02-12T23:46:02Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-12T23:46:02Z- waiting for 1613173560.81, time is 1613173562.68 2021-02-12T23:46:02Z- LcmTrn::updateTrn() equating pose 1613173562.68 and meas times 1613173562.68 2021-02-12T23:46:02Z- LcmTrn::updateTrn() >>>> pose time 1613173562.68, meas time 1613173562.68 <<<< 2021-02-12T23:46:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173562.68 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:46:02Z- TerrainNav::motionUpdate() - currEstimate = (4057795, 591483.22, 7.80) 2021-02-12T23:46:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:46:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:46:02Z- TNavBF::Projecting Measurements 2021-02-12T23:46:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:46:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:46:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:46:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:46:02Z- TNavFilter::4 good beams. 2021-02-12T23:46:02Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:46:02Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:46:02Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:46:02Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:46:02Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:46:02Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:46:02Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:46:02Z- TNavBF::mmse selected from filter -1 2021-02-12T23:46:02Z- TerrainMapOctree: (vehN, vehE ) = (4057795.14, 591483.22). 2021-02-12T23:46:02Z- TerrainMapOctree: Min Distance = 4426.19. 2021-02-12T23:46:02Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:46:02Z- There is no specific good beam that all particles have in common. 2021-02-12T23:46:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:46:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.81 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-12T23:46:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:46:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.63 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 19.32 2021-02-12T23:46:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:46:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.52 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 24.90 2021-02-12T23:46:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:46:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 11.05 2021-02-12T23:46:02Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:46:02Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:46:02Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:46:02Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:46:02Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:46:02Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:46:02Z- Filter 0 No beams used in subcloud update 2021-02-12T23:46:02Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:46:02Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:46:02Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 738.4 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-12T23:46:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173562.68 sec. 2021-02-12T23:46:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:46:02Z- TNavBF::mmse selected from filter -1 2021-02-12T23:46:02Z- variances([0], [2], and psi_berg) = 738.424649 833.023025 0.000000 2021-02-12T23:46:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:46:02Z- LcmTrn::publishEstimates() - MLE : 4057785.64 591441.72 7.80 2021-02-12T23:46:02Z- LcmTrn::publishEstimates() - MMSE : 4057795.14 591483.22 7.80 2021-02-12T23:46:02Z- LcmTrn::publishEstimates() - COVAR: 738.42 833.02 1.29 0.00 2021-02-12T23:46:03Z- AHRS_M2 msg: 1613173563.05 epoch sec; seqNo:6627 2021-02-12T23:46:03Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-12T23:46:03Z- handleDepth()-Depth_Keller msg: 1613173563066 epoch sec; seqNo:6681; depth 7.68 2021-02-12T23:46:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173563.08 epoch sec; seqNo:6681; 4057795.52 north; 591483.22 east 2021-02-12T23:46:03Z- AHRS_M2 msg: 1613173563.46 epoch sec; seqNo:6628 2021-02-12T23:46:03Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-12T23:46:03Z- handleDepth()-Depth_Keller msg: 1613173563465 epoch sec; seqNo:6682; depth 7.56 2021-02-12T23:46:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173563.47 epoch sec; seqNo:6682; 4057795.90 north; 591483.22 east 2021-02-12T23:46:03Z- AHRS_M2 msg: 1613173563.86 epoch sec; seqNo:6629 2021-02-12T23:46:03Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-12T23:46:03Z- handleDepth()-Depth_Keller msg: 1613173563864 epoch sec; seqNo:6683; depth 7.45 2021-02-12T23:46:03Z- handleDvl() - RDI_Pathfinder msg: 1613173563.88 epoch sec; seqNo:3115 2021-02-12T23:46:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.35 , 109.68 , 126.04 , 82.91 2021-02-12T23:46:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.09 , 0.07 2021-02-12T23:46:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173563.89 epoch sec; seqNo:6683; 4057795.90 north; 591483.22 east 2021-02-12T23:46:04Z- AHRS_M2 msg: 1613173564.26 epoch sec; seqNo:6630 2021-02-12T23:46:04Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-12T23:46:04Z- handleDepth()-Depth_Keller msg: 1613173564278 epoch sec; seqNo:6684; depth 7.33 2021-02-12T23:46:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173564.29 epoch sec; seqNo:6684; 4057796.27 north; 591483.21 east 2021-02-12T23:46:04Z- AHRS_M2 msg: 1613173564.69 epoch sec; seqNo:6631 2021-02-12T23:46:04Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-12T23:46:04Z- handleDepth()-Depth_Keller msg: 1613173564697 epoch sec; seqNo:6685; depth 7.23 2021-02-12T23:46:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173564.71 epoch sec; seqNo:6685; 4057796.67 north; 591484.43 east 2021-02-12T23:46:05Z- AHRS_M2 msg: 1613173565.08 epoch sec; seqNo:6632 2021-02-12T23:46:05Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:46:05Z- handleDepth()-Depth_Keller msg: 1613173565081 epoch sec; seqNo:6686; depth 7.11 2021-02-12T23:46:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173565.09 epoch sec; seqNo:6686; 4057797.04 north; 591484.42 east 2021-02-12T23:46:05Z- AHRS_M2 msg: 1613173565.48 epoch sec; seqNo:6633 2021-02-12T23:46:05Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.18 psi 2021-02-12T23:46:05Z- handleDepth()-Depth_Keller msg: 1613173565482 epoch sec; seqNo:6687; depth 6.99 2021-02-12T23:46:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173565.49 epoch sec; seqNo:6687; 4057797.04 north; 591484.42 east 2021-02-12T23:46:05Z- AHRS_M2 msg: 1613173565.88 epoch sec; seqNo:6634 2021-02-12T23:46:05Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.18 psi 2021-02-12T23:46:05Z- handleDepth()-Depth_Keller msg: 1613173565884 epoch sec; seqNo:6688; depth 6.88 2021-02-12T23:46:05Z- handleDvl() - RDI_Pathfinder msg: 1613173565.90 epoch sec; seqNo:3116 2021-02-12T23:46:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 108.26 , 121.64 , 82.98 2021-02-12T23:46:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.03 , -0.08 2021-02-12T23:46:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173565.91 epoch sec; seqNo:6688; 4057797.42 north; 591484.42 east 2021-02-12T23:46:06Z- AHRS_M2 msg: 1613173566.28 epoch sec; seqNo:6635 2021-02-12T23:46:06Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.18 psi 2021-02-12T23:46:06Z- handleDepth()-Depth_Keller msg: 1613173566288 epoch sec; seqNo:6689; depth 6.75 2021-02-12T23:46:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173566.30 epoch sec; seqNo:6689; 4057797.80 north; 591484.42 east 2021-02-12T23:46:06Z- AHRS_M2 msg: 1613173566.69 epoch sec; seqNo:6636 2021-02-12T23:46:06Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.19 psi 2021-02-12T23:46:06Z- handleDepth()-Depth_Keller msg: 1613173566692 epoch sec; seqNo:6690; depth 6.65 2021-02-12T23:46:06Z- handleDvl() - RDI_Pathfinder msg: 1613173566.70 epoch sec; seqNo:3117 2021-02-12T23:46:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 109.75 , 123.13 , 84.46 2021-02-12T23:46:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.15 , 0.06 2021-02-12T23:46:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173566.71 epoch sec; seqNo:6690; 4057797.80 north; 591484.42 east 2021-02-12T23:46:07Z- AHRS_M2 msg: 1613173567.09 epoch sec; seqNo:6637 2021-02-12T23:46:07Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.19 psi 2021-02-12T23:46:07Z- handleDepth()-Depth_Keller msg: 1613173567098 epoch sec; seqNo:6691; depth 6.54 2021-02-12T23:46:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173567.11 epoch sec; seqNo:6691; 4057798.18 north; 591484.41 east 2021-02-12T23:46:07Z- AHRS_M2 msg: 1613173567.50 epoch sec; seqNo:6638 2021-02-12T23:46:07Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.19 psi 2021-02-12T23:46:07Z- handleDepth()-Depth_Keller msg: 1613173567500 epoch sec; seqNo:6692; depth 6.41 2021-02-12T23:46:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173567.52 epoch sec; seqNo:6692; 4057798.56 north; 591484.41 east 2021-02-12T23:46:07Z- AHRS_M2 msg: 1613173567.90 epoch sec; seqNo:6639 2021-02-12T23:46:07Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-12T23:46:07Z- handleDepth()-Depth_Keller msg: 1613173567909 epoch sec; seqNo:6693; depth 6.31 2021-02-12T23:46:07Z- LcmTrn::handleMessages() - handling 41 messages took 0.69 ms and 127 calls... 2021-02-12T23:46:07Z- waiting for 1613173565.68, time is 1613173567.90 2021-02-12T23:46:07Z- LcmTrn::updateTrn() >>>> pose time 1613173567.90, meas time 1613173566.70 <<<< 2021-02-12T23:46:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173566.70 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:46:07Z- TerrainNav::motionUpdate() - currEstimate = (4057799, 591484.41, 6.31) 2021-02-12T23:46:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:46:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:46:07Z- TNavBF::Projecting Measurements 2021-02-12T23:46:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:46:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:46:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:46:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:46:07Z- TNavFilter::4 good beams. 2021-02-12T23:46:07Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:46:07Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:46:07Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:46:07Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:46:07Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:46:07Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:46:07Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:46:07Z- TNavBF::mmse selected from filter -1 2021-02-12T23:46:07Z- TerrainMapOctree: (vehN, vehE ) = (4057797.78, 591484.14). 2021-02-12T23:46:07Z- TerrainMapOctree: Min Distance = 4423.56. 2021-02-12T23:46:07Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:46:07Z- There is no specific good beam that all particles have in common. 2021-02-12T23:46:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:46:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-12T23:46:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:46:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.32 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 19.27 2021-02-12T23:46:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:46:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 24.26 2021-02-12T23:46:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:46:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 11.41 2021-02-12T23:46:07Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:46:07Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:46:07Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:46:07Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:46:07Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:46:07Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:46:07Z- Filter 0 No beams used in subcloud update 2021-02-12T23:46:07Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:46:07Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:46:07Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 738.4 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:46:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173566.70 sec. 2021-02-12T23:46:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:46:07Z- TNavBF::mmse selected from filter -1 2021-02-12T23:46:07Z- variances([0], [2], and psi_berg) = 738.316360 833.022938 0.000000 2021-02-12T23:46:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:46:07Z- LcmTrn::publishEstimates() - MLE : 4057789.06 591442.91 6.31 2021-02-12T23:46:07Z- LcmTrn::publishEstimates() - MMSE : 4057798.56 591484.41 6.31 2021-02-12T23:46:07Z- LcmTrn::publishEstimates() - COVAR: 738.32 833.02 1.13 0.00 2021-02-12T23:46:07Z- handleDvl() - RDI_Pathfinder msg: 1613173567.92 epoch sec; seqNo:3118 2021-02-12T23:46:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 109.67 , 121.57 , 82.91 2021-02-12T23:46:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.26 2021-02-12T23:46:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173567.94 epoch sec; seqNo:6693; 4057798.95 north; 591485.63 east 2021-02-12T23:46:08Z- AHRS_M2 msg: 1613173568.30 epoch sec; seqNo:6640 2021-02-12T23:46:08Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-12T23:46:08Z- handleDepth()-Depth_Keller msg: 1613173568313 epoch sec; seqNo:6694; depth 6.17 2021-02-12T23:46:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173568.32 epoch sec; seqNo:6694; 4057799.33 north; 591485.62 east 2021-02-12T23:46:08Z- AHRS_M2 msg: 1613173568.71 epoch sec; seqNo:6641 2021-02-12T23:46:08Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-12T23:46:08Z- handleDepth()-Depth_Keller msg: 1613173568714 epoch sec; seqNo:6695; depth 6.07 2021-02-12T23:46:08Z- handleDvl() - RDI_Pathfinder msg: 1613173568.73 epoch sec; seqNo:3119 2021-02-12T23:46:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.90 , 109.74 , 121.64 , 0.00 2021-02-12T23:46:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.09 , 0.22 2021-02-12T23:46:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173568.74 epoch sec; seqNo:6695; 4057799.33 north; 591485.62 east 2021-02-12T23:46:09Z- AHRS_M2 msg: 1613173569.11 epoch sec; seqNo:6642 2021-02-12T23:46:09Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-12T23:46:09Z- handleDepth()-Depth_Keller msg: 1613173569116 epoch sec; seqNo:6696; depth 5.95 2021-02-12T23:46:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173569.13 epoch sec; seqNo:6696; 4057799.71 north; 591485.62 east 2021-02-12T23:46:09Z- AHRS_M2 msg: 1613173569.52 epoch sec; seqNo:6643 2021-02-12T23:46:09Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-12T23:46:09Z- handleDepth()-Depth_Keller msg: 1613173569520 epoch sec; seqNo:6697; depth 5.83 2021-02-12T23:46:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173569.53 epoch sec; seqNo:6697; 4057800.09 north; 591485.61 east 2021-02-12T23:46:09Z- AHRS_M2 msg: 1613173569.92 epoch sec; seqNo:6644 2021-02-12T23:46:09Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-12T23:46:09Z- handleDepth()-Depth_Keller msg: 1613173569924 epoch sec; seqNo:6698; depth 5.71 2021-02-12T23:46:09Z- handleDvl() - RDI_Pathfinder msg: 1613173569.94 epoch sec; seqNo:3120 2021-02-12T23:46:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.44 , 110.82 , 121.23 , 84.06 2021-02-12T23:46:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.10 , 0.03 2021-02-12T23:46:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173569.95 epoch sec; seqNo:6698; 4057800.10 north; 591486.83 east 2021-02-12T23:46:10Z- AHRS_M2 msg: 1613173570.33 epoch sec; seqNo:6645 2021-02-12T23:46:10Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.19 psi 2021-02-12T23:46:10Z- handleDepth()-Depth_Keller msg: 1613173570338 epoch sec; seqNo:6699; depth 5.59 2021-02-12T23:46:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173570.35 epoch sec; seqNo:6699; 4057800.48 north; 591486.83 east 2021-02-12T23:46:10Z- AHRS_M2 msg: 1613173570.73 epoch sec; seqNo:6646 2021-02-12T23:46:10Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.18 psi 2021-02-12T23:46:10Z- handleDepth()-Depth_Keller msg: 1613173570733 epoch sec; seqNo:6700; depth 5.50 2021-02-12T23:46:10Z- handleDvl() - RDI_Pathfinder msg: 1613173570.75 epoch sec; seqNo:3121 2021-02-12T23:46:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 111.23 , 127.59 , 84.46 2021-02-12T23:46:10Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.01 , -0.12 2021-02-12T23:46:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173570.76 epoch sec; seqNo:6700; 4057800.86 north; 591486.83 east 2021-02-12T23:46:11Z- AHRS_M2 msg: 1613173571.13 epoch sec; seqNo:6647 2021-02-12T23:46:11Z- AHRS_M2 msg: 0.66 phi; 0.29 theta; -0.18 psi 2021-02-12T23:46:11Z- handleDepth()-Depth_Keller msg: 1613173571141 epoch sec; seqNo:6701; depth 5.36 2021-02-12T23:46:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173571.15 epoch sec; seqNo:6701; 4057801.24 north; 591486.82 east 2021-02-12T23:46:11Z- AHRS_M2 msg: 1613173571.54 epoch sec; seqNo:6648 2021-02-12T23:46:11Z- AHRS_M2 msg: 0.68 phi; 0.24 theta; -0.17 psi 2021-02-12T23:46:11Z- handleDepth()-Depth_Keller msg: 1613173571540 epoch sec; seqNo:6702; depth 5.24 2021-02-12T23:46:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173571.55 epoch sec; seqNo:6702; 4057801.24 north; 591486.82 east 2021-02-12T23:46:11Z- AHRS_M2 msg: 1613173571.94 epoch sec; seqNo:6649 2021-02-12T23:46:11Z- AHRS_M2 msg: 0.69 phi; 0.19 theta; -0.16 psi 2021-02-12T23:46:11Z- handleDepth()-Depth_Keller msg: 1613173571947 epoch sec; seqNo:6703; depth 5.13 2021-02-12T23:46:11Z- handleDvl() - RDI_Pathfinder msg: 1613173571.96 epoch sec; seqNo:3122 2021-02-12T23:46:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.70 , 111.03 , 119.95 , 85.75 2021-02-12T23:46:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.09 , -0.19 2021-02-12T23:46:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173571.97 epoch sec; seqNo:6703; 4057801.62 north; 591486.82 east 2021-02-12T23:46:12Z- AHRS_M2 msg: 1613173572.34 epoch sec; seqNo:6650 2021-02-12T23:46:12Z- AHRS_M2 msg: 0.69 phi; 0.13 theta; -0.16 psi 2021-02-12T23:46:12Z- handleDepth()-Depth_Keller msg: 1613173572348 epoch sec; seqNo:6704; depth 5.02 2021-02-12T23:46:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173572.37 epoch sec; seqNo:6704; 4057802.01 north; 591488.04 east 2021-02-12T23:46:12Z- AHRS_M2 msg: 1613173572.75 epoch sec; seqNo:6651 2021-02-12T23:46:12Z- AHRS_M2 msg: 0.69 phi; 0.08 theta; -0.17 psi 2021-02-12T23:46:12Z- handleDepth()-Depth_Keller msg: 1613173572752 epoch sec; seqNo:6705; depth 4.92 2021-02-12T23:46:12Z- handleDvl() - RDI_Pathfinder msg: 1613173572.76 epoch sec; seqNo:3123 2021-02-12T23:46:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.91 , 106.83 , 108.32 , 97.91 2021-02-12T23:46:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.15 , -0.05 2021-02-12T23:46:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173572.79 epoch sec; seqNo:6705; 4057802.39 north; 591488.03 east 2021-02-12T23:46:12Z- LcmTrn::handleMessages() - handling 43 messages took 0.65 ms and 130 calls... 2021-02-12T23:46:12Z- waiting for 1613173570.90, time is 1613173572.79 2021-02-12T23:46:12Z- LcmTrn::updateTrn() equating pose 1613173572.79 and meas times 1613173572.79 2021-02-12T23:46:12Z- LcmTrn::updateTrn() >>>> pose time 1613173572.79, meas time 1613173572.79 <<<< 2021-02-12T23:46:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173572.79 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:46:12Z- TerrainNav::motionUpdate() - currEstimate = (4057802, 591488.03, 4.92) 2021-02-12T23:46:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:46:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:46:12Z- TNavBF::Projecting Measurements 2021-02-12T23:46:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:46:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:46:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:46:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:46:12Z- TNavFilter::4 good beams. 2021-02-12T23:46:12Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:46:12Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:46:12Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:46:12Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:46:12Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:46:12Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:46:12Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:46:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:46:12Z- TerrainMapOctree: (vehN, vehE ) = (4057802.39, 591488.03). 2021-02-12T23:46:12Z- TerrainMapOctree: Min Distance = 4417.52. 2021-02-12T23:46:12Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:46:13Z- There is no specific good beam that all particles have in common. 2021-02-12T23:46:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:46:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-12T23:46:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:46:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 18.26 2021-02-12T23:46:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:46:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 18.77 2021-02-12T23:46:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:46:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-12T23:46:13Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:46:13Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:46:13Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:46:13Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:46:13Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:46:13Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:46:13Z- Filter 0 No beams used in subcloud update 2021-02-12T23:46:13Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:46:13Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:46:13Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 738.2 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:46:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173572.79 sec. 2021-02-12T23:46:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:46:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:46:13Z- variances([0], [2], and psi_berg) = 738.220171 833.022861 0.000000 2021-02-12T23:46:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:46:13Z- LcmTrn::publishEstimates() - MLE : 4057792.89 591446.53 4.92 2021-02-12T23:46:13Z- LcmTrn::publishEstimates() - MMSE : 4057802.39 591488.03 4.92 2021-02-12T23:46:13Z- LcmTrn::publishEstimates() - COVAR: 738.22 833.02 1.07 0.00 2021-02-12T23:46:13Z- AHRS_M2 msg: 1613173573.15 epoch sec; seqNo:6652 2021-02-12T23:46:13Z- AHRS_M2 msg: 0.70 phi; 0.03 theta; -0.18 psi 2021-02-12T23:46:13Z- handleDepth()-Depth_Keller msg: 1613173573158 epoch sec; seqNo:6706; depth 4.83 2021-02-12T23:46:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173573.17 epoch sec; seqNo:6706; 4057802.39 north; 591488.03 east 2021-02-12T23:46:13Z- AHRS_M2 msg: 1613173573.56 epoch sec; seqNo:6653 2021-02-12T23:46:13Z- AHRS_M2 msg: 0.70 phi; -0.01 theta; -0.18 psi 2021-02-12T23:46:13Z- handleDepth()-Depth_Keller msg: 1613173573562 epoch sec; seqNo:6707; depth 4.76 2021-02-12T23:46:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173573.57 epoch sec; seqNo:6707; 4057802.77 north; 591488.03 east 2021-02-12T23:46:13Z- AHRS_M2 msg: 1613173573.96 epoch sec; seqNo:6654 2021-02-12T23:46:13Z- AHRS_M2 msg: 0.70 phi; -0.05 theta; -0.18 psi 2021-02-12T23:46:13Z- handleDepth()-Depth_Keller msg: 1613173573965 epoch sec; seqNo:6708; depth 4.70 2021-02-12T23:46:13Z- handleDvl() - RDI_Pathfinder msg: 1613173573.98 epoch sec; seqNo:3124 2021-02-12T23:46:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.43 , 105.28 , 97.84 , 97.84 2021-02-12T23:46:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , 0.03 , 0.20 2021-02-12T23:46:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173573.99 epoch sec; seqNo:6708; 4057803.15 north; 591488.02 east 2021-02-12T23:46:14Z- AHRS_M2 msg: 1613173574.36 epoch sec; seqNo:6655 2021-02-12T23:46:14Z- AHRS_M2 msg: 0.70 phi; -0.08 theta; -0.17 psi 2021-02-12T23:46:14Z- handleDepth()-Depth_Keller msg: 1613173574368 epoch sec; seqNo:6709; depth 4.67 2021-02-12T23:46:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173574.38 epoch sec; seqNo:6709; 4057803.15 north; 591488.02 east 2021-02-12T23:46:14Z- AHRS_M2 msg: 1613173574.77 epoch sec; seqNo:6656 2021-02-12T23:46:14Z- AHRS_M2 msg: 0.70 phi; -0.10 theta; -0.16 psi 2021-02-12T23:46:14Z- handleDepth()-Depth_Keller msg: 1613173574777 epoch sec; seqNo:6710; depth 4.64 2021-02-12T23:46:14Z- handleDvl() - RDI_Pathfinder msg: 1613173574.79 epoch sec; seqNo:3125 2021-02-12T23:46:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.19 , 105.41 , 97.30 , 100.01 2021-02-12T23:46:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , 0.21 , 0.15 2021-02-12T23:46:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173574.80 epoch sec; seqNo:6710; 4057803.15 north; 591488.02 east 2021-02-12T23:46:15Z- AHRS_M2 msg: 1613173575.17 epoch sec; seqNo:6657 2021-02-12T23:46:15Z- AHRS_M2 msg: 0.70 phi; -0.12 theta; -0.18 psi 2021-02-12T23:46:15Z- handleDepth()-Depth_Keller msg: 1613173575189 epoch sec; seqNo:6711; depth 4.63 2021-02-12T23:46:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173575.20 epoch sec; seqNo:6711; 4057803.54 north; 591489.24 east 2021-02-12T23:46:15Z- AHRS_M2 msg: 1613173575.58 epoch sec; seqNo:6658 2021-02-12T23:46:15Z- AHRS_M2 msg: 0.70 phi; -0.13 theta; -0.20 psi 2021-02-12T23:46:15Z- handleDepth()-Depth_Keller msg: 1613173575580 epoch sec; seqNo:6712; depth 4.62 2021-02-12T23:46:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173575.59 epoch sec; seqNo:6712; 4057803.54 north; 591489.24 east 2021-02-12T23:46:15Z- AHRS_M2 msg: 1613173575.98 epoch sec; seqNo:6659 2021-02-12T23:46:15Z- AHRS_M2 msg: 0.69 phi; -0.14 theta; -0.21 psi 2021-02-12T23:46:15Z- handleDepth()-Depth_Keller msg: 1613173575985 epoch sec; seqNo:6713; depth 4.63 2021-02-12T23:46:16Z- handleDvl() - RDI_Pathfinder msg: 1613173576.00 epoch sec; seqNo:3126 2021-02-12T23:46:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 106.63 , 90.41 , 103.93 2021-02-12T23:46:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.16 , 0.02 2021-02-12T23:46:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173576.01 epoch sec; seqNo:6713; 4057803.92 north; 591489.24 east 2021-02-12T23:46:16Z- AHRS_M2 msg: 1613173576.38 epoch sec; seqNo:6660 2021-02-12T23:46:16Z- AHRS_M2 msg: 0.69 phi; -0.15 theta; -0.21 psi 2021-02-12T23:46:16Z- handleDepth()-Depth_Keller msg: 1613173576388 epoch sec; seqNo:6714; depth 4.66 2021-02-12T23:46:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173576.41 epoch sec; seqNo:6714; 4057803.92 north; 591489.24 east 2021-02-12T23:46:16Z- AHRS_M2 msg: 1613173576.79 epoch sec; seqNo:6661 2021-02-12T23:46:16Z- AHRS_M2 msg: 0.69 phi; -0.16 theta; -0.21 psi 2021-02-12T23:46:16Z- handleDepth()-Depth_Keller msg: 1613173576792 epoch sec; seqNo:6715; depth 4.68 2021-02-12T23:46:16Z- handleDvl() - RDI_Pathfinder msg: 1613173576.80 epoch sec; seqNo:3127 2021-02-12T23:46:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.77 , 109.40 , 97.24 , 108.05 2021-02-12T23:46:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.16 , -0.14 2021-02-12T23:46:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173576.82 epoch sec; seqNo:6715; 4057804.31 north; 591490.45 east 2021-02-12T23:46:17Z- AHRS_M2 msg: 1613173577.19 epoch sec; seqNo:6662 2021-02-12T23:46:17Z- AHRS_M2 msg: 0.70 phi; -0.18 theta; -0.20 psi 2021-02-12T23:46:17Z- handleDepth()-Depth_Keller msg: 1613173577196 epoch sec; seqNo:6716; depth 4.72 2021-02-12T23:46:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173577.21 epoch sec; seqNo:6716; 4057804.69 north; 591490.45 east 2021-02-12T23:46:17Z- AHRS_M2 msg: 1613173577.60 epoch sec; seqNo:6663 2021-02-12T23:46:17Z- AHRS_M2 msg: 0.70 phi; -0.20 theta; -0.21 psi 2021-02-12T23:46:17Z- handleDepth()-Depth_Keller msg: 1613173577611 epoch sec; seqNo:6717; depth 4.78 2021-02-12T23:46:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173577.62 epoch sec; seqNo:6717; 4057804.69 north; 591490.45 east 2021-02-12T23:46:18Z- AHRS_M2 msg: 1613173578.00 epoch sec; seqNo:6664 2021-02-12T23:46:18Z- AHRS_M2 msg: 0.70 phi; -0.21 theta; -0.22 psi 2021-02-12T23:46:18Z- handleDepth()-Depth_Keller msg: 1613173578004 epoch sec; seqNo:6718; depth 4.80 2021-02-12T23:46:18Z- handleDvl() - RDI_Pathfinder msg: 1613173578.02 epoch sec; seqNo:3128 2021-02-12T23:46:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.96 , 109.13 , 96.96 , 110.48 2021-02-12T23:46:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.45 2021-02-12T23:46:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173578.03 epoch sec; seqNo:6718; 4057805.07 north; 591490.44 east 2021-02-12T23:46:18Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-12T23:46:18Z- waiting for 1613173575.79, time is 1613173578.03 2021-02-12T23:46:18Z- LcmTrn::updateTrn() equating pose 1613173578.03 and meas times 1613173578.03 2021-02-12T23:46:18Z- LcmTrn::updateTrn() >>>> pose time 1613173578.03, meas time 1613173578.03 <<<< 2021-02-12T23:46:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173578.03 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:46:18Z- TerrainNav::motionUpdate() - currEstimate = (4057805, 591490.44, 4.80) 2021-02-12T23:46:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:46:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:46:18Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173578.03 sec.; No relevant navigation data available 2021-02-12T23:46:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:46:18Z- TNavBF::mmse selected from filter -1 2021-02-12T23:46:18Z- variances([0], [2], and psi_berg) = 738.140708 833.022797 0.000000 2021-02-12T23:46:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:46:18Z- LcmTrn::publishEstimates() - MLE : 4057795.57 591448.94 4.80 2021-02-12T23:46:18Z- LcmTrn::publishEstimates() - MMSE : 4057805.07 591490.44 4.80 2021-02-12T23:46:18Z- LcmTrn::publishEstimates() - COVAR: 738.14 833.02 1.26 0.00 2021-02-12T23:46:18Z- AHRS_M2 msg: 1613173578.40 epoch sec; seqNo:6665 2021-02-12T23:46:18Z- AHRS_M2 msg: 0.71 phi; -0.23 theta; -0.23 psi 2021-02-12T23:46:18Z- handleDepth()-Depth_Keller msg: 1613173578408 epoch sec; seqNo:6719; depth 4.86 2021-02-12T23:46:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173578.42 epoch sec; seqNo:6719; 4057805.44 north; 591490.44 east 2021-02-12T23:46:18Z- AHRS_M2 msg: 1613173578.81 epoch sec; seqNo:6666 2021-02-12T23:46:18Z- AHRS_M2 msg: 0.72 phi; -0.25 theta; -0.25 psi 2021-02-12T23:46:18Z- handleDepth()-Depth_Keller msg: 1613173578812 epoch sec; seqNo:6720; depth 4.91 2021-02-12T23:46:18Z- handleDvl() - RDI_Pathfinder msg: 1613173578.82 epoch sec; seqNo:3129 2021-02-12T23:46:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.92 , 112.71 , 0.00 , 112.71 2021-02-12T23:46:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.45 , -0.46 2021-02-12T23:46:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173578.83 epoch sec; seqNo:6720; 4057805.82 north; 591490.44 east 2021-02-12T23:46:19Z- AHRS_M2 msg: 1613173579.21 epoch sec; seqNo:6667 2021-02-12T23:46:19Z- AHRS_M2 msg: 0.73 phi; -0.26 theta; -0.27 psi 2021-02-12T23:46:19Z- handleDepth()-Depth_Keller msg: 1613173579218 epoch sec; seqNo:6721; depth 4.98 2021-02-12T23:46:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173579.23 epoch sec; seqNo:6721; 4057806.20 north; 591490.43 east 2021-02-12T23:46:19Z- AHRS_M2 msg: 1613173579.62 epoch sec; seqNo:6668 2021-02-12T23:46:19Z- AHRS_M2 msg: 0.73 phi; -0.28 theta; -0.27 psi 2021-02-12T23:46:19Z- handleDepth()-Depth_Keller msg: 1613173579620 epoch sec; seqNo:6722; depth 5.05 2021-02-12T23:46:19Z- handleDvl() - RDI_Pathfinder msg: 1613173579.67 epoch sec; seqNo:3130 2021-02-12T23:46:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:46:19Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:46:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173579.69 epoch sec; seqNo:6722; 4057806.59 north; 591491.65 east 2021-02-12T23:46:20Z- AHRS_M2 msg: 1613173580.02 epoch sec; seqNo:6669 2021-02-12T23:46:20Z- AHRS_M2 msg: 0.74 phi; -0.31 theta; -0.25 psi 2021-02-12T23:46:20Z- handleDepth()-Depth_Keller msg: 1613173580024 epoch sec; seqNo:6723; depth 5.13 2021-02-12T23:46:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173580.04 epoch sec; seqNo:6723; 4057806.97 north; 591491.65 east 2021-02-12T23:46:20Z- AHRS_M2 msg: 1613173580.42 epoch sec; seqNo:6670 2021-02-12T23:46:20Z- AHRS_M2 msg: 0.74 phi; -0.33 theta; -0.21 psi 2021-02-12T23:46:20Z- handleDepth()-Depth_Keller msg: 1613173580437 epoch sec; seqNo:6724; depth 5.20 2021-02-12T23:46:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173580.45 epoch sec; seqNo:6724; 4057807.35 north; 591491.64 east 2021-02-12T23:46:20Z- AHRS_M2 msg: 1613173580.83 epoch sec; seqNo:6671 2021-02-12T23:46:20Z- AHRS_M2 msg: 0.73 phi; -0.35 theta; -0.18 psi 2021-02-12T23:46:20Z- handleDepth()-Depth_Keller msg: 1613173580834 epoch sec; seqNo:6725; depth 5.30 2021-02-12T23:46:20Z- handleDvl() - RDI_Pathfinder msg: 1613173580.85 epoch sec; seqNo:3131 2021-02-12T23:46:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.27 , 112.58 , 85.81 , 124.47 2021-02-12T23:46:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.49 , -0.22 , 0.62 2021-02-12T23:46:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173580.86 epoch sec; seqNo:6725; 4057807.73 north; 591491.64 east 2021-02-12T23:46:21Z- AHRS_M2 msg: 1613173581.23 epoch sec; seqNo:6672 2021-02-12T23:46:21Z- AHRS_M2 msg: 0.71 phi; -0.37 theta; -0.16 psi 2021-02-12T23:46:21Z- handleDepth()-Depth_Keller msg: 1613173581237 epoch sec; seqNo:6726; depth 5.38 2021-02-12T23:46:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173581.26 epoch sec; seqNo:6726; 4057807.73 north; 591491.64 east 2021-02-12T23:46:21Z- AHRS_M2 msg: 1613173581.64 epoch sec; seqNo:6673 2021-02-12T23:46:21Z- AHRS_M2 msg: 0.69 phi; -0.39 theta; -0.16 psi 2021-02-12T23:46:21Z- handleDepth()-Depth_Keller msg: 1613173581640 epoch sec; seqNo:6727; depth 5.48 2021-02-12T23:46:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173581.65 epoch sec; seqNo:6727; 4057807.73 north; 591491.64 east 2021-02-12T23:46:22Z- AHRS_M2 msg: 1613173582.05 epoch sec; seqNo:6674 2021-02-12T23:46:22Z- AHRS_M2 msg: 0.64 phi; -0.39 theta; -0.22 psi 2021-02-12T23:46:22Z- handleDepth()-Depth_Keller msg: 1613173582051 epoch sec; seqNo:6728; depth 5.57 2021-02-12T23:46:22Z- handleDvl() - RDI_Pathfinder msg: 1613173582.06 epoch sec; seqNo:3132 2021-02-12T23:46:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.84 , 111.23 , 82.97 , 130.56 2021-02-12T23:46:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , 0.38 , 0.60 2021-02-12T23:46:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173582.07 epoch sec; seqNo:6728; 4057807.73 north; 591491.64 east 2021-02-12T23:46:22Z- AHRS_M2 msg: 1613173582.45 epoch sec; seqNo:6675 2021-02-12T23:46:22Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.26 psi 2021-02-12T23:46:22Z- handleDepth()-Depth_Keller msg: 1613173582456 epoch sec; seqNo:6729; depth 5.66 2021-02-12T23:46:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173582.48 epoch sec; seqNo:6729; 4057807.73 north; 591491.64 east 2021-02-12T23:46:22Z- AHRS_M2 msg: 1613173582.85 epoch sec; seqNo:6676 2021-02-12T23:46:22Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.29 psi 2021-02-12T23:46:22Z- handleDepth()-Depth_Keller msg: 1613173582856 epoch sec; seqNo:6730; depth 5.76 2021-02-12T23:46:22Z- handleDvl() - RDI_Pathfinder msg: 1613173582.87 epoch sec; seqNo:3133 2021-02-12T23:46:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.64 , 115.48 , 84.26 , 133.33 2021-02-12T23:46:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.56 , 0.09 2021-02-12T23:46:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173582.89 epoch sec; seqNo:6730; 4057808.11 north; 591491.63 east 2021-02-12T23:46:23Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-12T23:46:23Z- waiting for 1613173581.03, time is 1613173582.89 2021-02-12T23:46:23Z- LcmTrn::updateTrn() equating pose 1613173582.89 and meas times 1613173582.89 2021-02-12T23:46:23Z- LcmTrn::updateTrn() >>>> pose time 1613173582.89, meas time 1613173582.89 <<<< 2021-02-12T23:46:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173582.89 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:46:23Z- TerrainNav::motionUpdate() - currEstimate = (4057808, 591491.63, 5.76) 2021-02-12T23:46:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:46:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:46:23Z- TNavBF::Projecting Measurements 2021-02-12T23:46:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:46:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:46:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:46:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:46:23Z- TNavFilter::4 good beams. 2021-02-12T23:46:23Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:46:23Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:46:23Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:46:23Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:46:23Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:46:23Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:46:23Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:46:23Z- TNavBF::mmse selected from filter -1 2021-02-12T23:46:23Z- TerrainMapOctree: (vehN, vehE ) = (4057808.11, 591491.63). 2021-02-12T23:46:23Z- TerrainMapOctree: Min Distance = 4410.80. 2021-02-12T23:46:23Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:46:23Z- There is no specific good beam that all particles have in common. 2021-02-12T23:46:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:46:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.42 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 15.25 2021-02-12T23:46:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:46:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 21.34 2021-02-12T23:46:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:46:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 11.36 2021-02-12T23:46:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:46:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 28.44 2021-02-12T23:46:23Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:46:23Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:46:23Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:46:23Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:46:23Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:46:23Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:46:23Z- Filter 0 No beams used in subcloud update 2021-02-12T23:46:23Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:46:23Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:46:23Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 738.1 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:46:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173582.89 sec. 2021-02-12T23:46:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:46:23Z- TNavBF::mmse selected from filter -1 2021-02-12T23:46:23Z- variances([0], [2], and psi_berg) = 738.065131 833.022736 0.000000 2021-02-12T23:46:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:46:23Z- LcmTrn::publishEstimates() - MLE : 4057798.61 591450.13 5.76 2021-02-12T23:46:23Z- LcmTrn::publishEstimates() - MMSE : 4057808.11 591491.63 5.76 2021-02-12T23:46:23Z- LcmTrn::publishEstimates() - COVAR: 738.07 833.02 1.09 0.00 2021-02-12T23:46:23Z- AHRS_M2 msg: 1613173583.25 epoch sec; seqNo:6677 2021-02-12T23:46:23Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.31 psi 2021-02-12T23:46:23Z- handleDepth()-Depth_Keller msg: 1613173583256 epoch sec; seqNo:6731; depth 5.89 2021-02-12T23:46:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173583.27 epoch sec; seqNo:6731; 4057808.12 north; 591492.85 east 2021-02-12T23:46:23Z- AHRS_M2 msg: 1613173583.66 epoch sec; seqNo:6678 2021-02-12T23:46:23Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.32 psi 2021-02-12T23:46:23Z- handleDepth()-Depth_Keller msg: 1613173583662 epoch sec; seqNo:6732; depth 5.99 2021-02-12T23:46:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173583.67 epoch sec; seqNo:6732; 4057808.50 north; 591492.85 east 2021-02-12T23:46:24Z- AHRS_M2 msg: 1613173584.06 epoch sec; seqNo:6679 2021-02-12T23:46:24Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.31 psi 2021-02-12T23:46:24Z- handleDepth()-Depth_Keller msg: 1613173584069 epoch sec; seqNo:6733; depth 6.13 2021-02-12T23:46:24Z- handleDvl() - RDI_Pathfinder msg: 1613173584.09 epoch sec; seqNo:3134 2021-02-12T23:46:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.52 , 122.72 , 85.54 , 136.10 2021-02-12T23:46:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , 0.15 , -0.28 2021-02-12T23:46:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173584.10 epoch sec; seqNo:6733; 4057808.50 north; 591492.85 east 2021-02-12T23:46:24Z- AHRS_M2 msg: 1613173584.46 epoch sec; seqNo:6680 2021-02-12T23:46:24Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.29 psi 2021-02-12T23:46:24Z- handleDepth()-Depth_Keller msg: 1613173584468 epoch sec; seqNo:6734; depth 6.26 2021-02-12T23:46:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173584.48 epoch sec; seqNo:6734; 4057808.89 north; 591494.07 east 2021-02-12T23:46:24Z- AHRS_M2 msg: 1613173584.88 epoch sec; seqNo:6681 2021-02-12T23:46:24Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.26 psi 2021-02-12T23:46:24Z- handleDepth()-Depth_Keller msg: 1613173584887 epoch sec; seqNo:6735; depth 6.38 2021-02-12T23:46:24Z- handleDvl() - RDI_Pathfinder msg: 1613173584.90 epoch sec; seqNo:3135 2021-02-12T23:46:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.37 , 124.14 , 85.47 , 130.08 2021-02-12T23:46:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.22 , -0.19 2021-02-12T23:46:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173584.92 epoch sec; seqNo:6735; 4057809.27 north; 591494.06 east 2021-02-12T23:46:25Z- AHRS_M2 msg: 1613173585.27 epoch sec; seqNo:6682 2021-02-12T23:46:25Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.23 psi 2021-02-12T23:46:25Z- handleDepth()-Depth_Keller msg: 1613173585277 epoch sec; seqNo:6736; depth 6.51 2021-02-12T23:46:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173585.29 epoch sec; seqNo:6736; 4057809.65 north; 591494.06 east 2021-02-12T23:46:25Z- AHRS_M2 msg: 1613173585.68 epoch sec; seqNo:6683 2021-02-12T23:46:25Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.20 psi 2021-02-12T23:46:25Z- handleDepth()-Depth_Keller msg: 1613173585680 epoch sec; seqNo:6737; depth 6.65 2021-02-12T23:46:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173585.69 epoch sec; seqNo:6737; 4057810.03 north; 591494.05 east 2021-02-12T23:46:26Z- AHRS_M2 msg: 1613173586.08 epoch sec; seqNo:6684 2021-02-12T23:46:26Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.19 psi 2021-02-12T23:46:26Z- handleDepth()-Depth_Keller msg: 1613173586084 epoch sec; seqNo:6738; depth 6.77 2021-02-12T23:46:26Z- handleDvl() - RDI_Pathfinder msg: 1613173586.10 epoch sec; seqNo:3136 2021-02-12T23:46:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.31 , 115.15 , 83.92 , 125.56 2021-02-12T23:46:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.26 , -0.01 2021-02-12T23:46:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173586.12 epoch sec; seqNo:6738; 4057810.41 north; 591494.05 east 2021-02-12T23:46:26Z- AHRS_M2 msg: 1613173586.48 epoch sec; seqNo:6685 2021-02-12T23:46:26Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.18 psi 2021-02-12T23:46:26Z- handleDepth()-Depth_Keller msg: 1613173586488 epoch sec; seqNo:6739; depth 6.89 2021-02-12T23:46:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173586.51 epoch sec; seqNo:6739; 4057810.79 north; 591494.05 east 2021-02-12T23:46:26Z- AHRS_M2 msg: 1613173586.89 epoch sec; seqNo:6686 2021-02-12T23:46:26Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.17 psi 2021-02-12T23:46:26Z- handleDepth()-Depth_Keller msg: 1613173586892 epoch sec; seqNo:6740; depth 7.02 2021-02-12T23:46:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173586.90 epoch sec; seqNo:6740; 4057811.17 north; 591494.04 east 2021-02-12T23:46:27Z- AHRS_M2 msg: 1613173587.29 epoch sec; seqNo:6687 2021-02-12T23:46:27Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.18 psi 2021-02-12T23:46:27Z- handleDepth()-Depth_Keller msg: 1613173587296 epoch sec; seqNo:6741; depth 7.14 2021-02-12T23:46:27Z- handleDvl() - RDI_Pathfinder msg: 1613173587.31 epoch sec; seqNo:3137 2021-02-12T23:46:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.65 , 109.54 , 82.77 , 124.42 2021-02-12T23:46:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.01 , 0.06 2021-02-12T23:46:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173587.32 epoch sec; seqNo:6741; 4057811.17 north; 591494.04 east 2021-02-12T23:46:27Z- AHRS_M2 msg: 1613173587.70 epoch sec; seqNo:6688 2021-02-12T23:46:27Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.19 psi 2021-02-12T23:46:27Z- handleDepth()-Depth_Keller msg: 1613173587709 epoch sec; seqNo:6742; depth 7.28 2021-02-12T23:46:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173587.72 epoch sec; seqNo:6742; 4057811.55 north; 591494.04 east 2021-02-12T23:46:28Z- AHRS_M2 msg: 1613173588.10 epoch sec; seqNo:6689 2021-02-12T23:46:28Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.20 psi 2021-02-12T23:46:28Z- handleDepth()-Depth_Keller msg: 1613173588104 epoch sec; seqNo:6743; depth 7.38 2021-02-12T23:46:28Z- handleDvl() - RDI_Pathfinder msg: 1613173588.12 epoch sec; seqNo:3138 2021-02-12T23:46:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 108.18 , 82.91 , 129.00 2021-02-12T23:46:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.17 , -0.02 2021-02-12T23:46:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173588.14 epoch sec; seqNo:6743; 4057811.56 north; 591495.26 east 2021-02-12T23:46:28Z- LcmTrn::handleMessages() - handling 44 messages took 0.76 ms and 129 calls... 2021-02-12T23:46:28Z- waiting for 1613173585.89, time is 1613173588.14 2021-02-12T23:46:28Z- LcmTrn::updateTrn() equating pose 1613173588.14 and meas times 1613173588.14 2021-02-12T23:46:28Z- LcmTrn::updateTrn() >>>> pose time 1613173588.14, meas time 1613173588.14 <<<< 2021-02-12T23:46:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173588.14 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:46:28Z- TerrainNav::motionUpdate() - currEstimate = (4057812, 591495.26, 7.38) 2021-02-12T23:46:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:46:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:46:28Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173588.14 sec.; No relevant navigation data available 2021-02-12T23:46:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:46:28Z- TNavBF::mmse selected from filter -1 2021-02-12T23:46:28Z- variances([0], [2], and psi_berg) = 737.971687 833.022662 0.000000 2021-02-12T23:46:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:46:28Z- LcmTrn::publishEstimates() - MLE : 4057802.06 591453.76 7.38 2021-02-12T23:46:28Z- LcmTrn::publishEstimates() - MMSE : 4057811.56 591495.26 7.38 2021-02-12T23:46:28Z- LcmTrn::publishEstimates() - COVAR: 737.97 833.02 1.35 0.00 2021-02-12T23:46:28Z- AHRS_M2 msg: 1613173588.50 epoch sec; seqNo:6690 2021-02-12T23:46:28Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.22 psi 2021-02-12T23:46:28Z- handleDepth()-Depth_Keller msg: 1613173588508 epoch sec; seqNo:6744; depth 7.50 2021-02-12T23:46:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173588.52 epoch sec; seqNo:6744; 4057811.94 north; 591495.26 east 2021-02-12T23:46:28Z- AHRS_M2 msg: 1613173588.91 epoch sec; seqNo:6691 2021-02-12T23:46:28Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.24 psi 2021-02-12T23:46:28Z- handleDepth()-Depth_Keller msg: 1613173588912 epoch sec; seqNo:6745; depth 7.62 2021-02-12T23:46:28Z- handleDvl() - RDI_Pathfinder msg: 1613173588.92 epoch sec; seqNo:3139 2021-02-12T23:46:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.27 , 111.09 , 0.00 , 121.50 2021-02-12T23:46:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.08 , -0.11 2021-02-12T23:46:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173588.93 epoch sec; seqNo:6745; 4057812.32 north; 591495.25 east 2021-02-12T23:46:29Z- AHRS_M2 msg: 1613173589.31 epoch sec; seqNo:6692 2021-02-12T23:46:29Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.25 psi 2021-02-12T23:46:29Z- handleDepth()-Depth_Keller msg: 1613173589319 epoch sec; seqNo:6746; depth 7.75 2021-02-12T23:46:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173589.33 epoch sec; seqNo:6746; 4057812.69 north; 591495.25 east 2021-02-12T23:46:29Z- AHRS_M2 msg: 1613173589.72 epoch sec; seqNo:6693 2021-02-12T23:46:29Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.25 psi 2021-02-12T23:46:29Z- handleDepth()-Depth_Keller msg: 1613173589725 epoch sec; seqNo:6747; depth 7.88 2021-02-12T23:46:29Z- handleDvl() - RDI_Pathfinder msg: 1613173589.74 epoch sec; seqNo:3140 2021-02-12T23:46:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:46:29Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:46:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173589.75 epoch sec; seqNo:6747; 4057812.69 north; 591495.25 east 2021-02-12T23:46:30Z- AHRS_M2 msg: 1613173590.12 epoch sec; seqNo:6694 2021-02-12T23:46:30Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.24 psi 2021-02-12T23:46:30Z- handleDepth()-Depth_Keller msg: 1613173590128 epoch sec; seqNo:6748; depth 8.01 2021-02-12T23:46:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173590.14 epoch sec; seqNo:6748; 4057813.09 north; 591496.46 east 2021-02-12T23:46:30Z- AHRS_M2 msg: 1613173590.53 epoch sec; seqNo:6695 2021-02-12T23:46:30Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.22 psi 2021-02-12T23:46:30Z- handleDepth()-Depth_Keller msg: 1613173590535 epoch sec; seqNo:6749; depth 8.13 2021-02-12T23:46:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173590.54 epoch sec; seqNo:6749; 4057813.47 north; 591496.46 east 2021-02-12T23:46:30Z- AHRS_M2 msg: 1613173590.93 epoch sec; seqNo:6696 2021-02-12T23:46:30Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.20 psi 2021-02-12T23:46:30Z- handleDepth()-Depth_Keller msg: 1613173590938 epoch sec; seqNo:6750; depth 8.26 2021-02-12T23:46:30Z- handleDvl() - RDI_Pathfinder msg: 1613173590.96 epoch sec; seqNo:3141 2021-02-12T23:46:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 109.34 , 82.57 , 118.26 2021-02-12T23:46:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.22 , -0.00 2021-02-12T23:46:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173590.98 epoch sec; seqNo:6750; 4057813.84 north; 591496.46 east 2021-02-12T23:46:31Z- AHRS_M2 msg: 1613173591.33 epoch sec; seqNo:6697 2021-02-12T23:46:31Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-12T23:46:31Z- handleDepth()-Depth_Keller msg: 1613173591336 epoch sec; seqNo:6751; depth 8.40 2021-02-12T23:46:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173591.35 epoch sec; seqNo:6751; 4057814.22 north; 591496.45 east 2021-02-12T23:46:31Z- AHRS_M2 msg: 1613173591.74 epoch sec; seqNo:6698 2021-02-12T23:46:31Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-12T23:46:31Z- handleDepth()-Depth_Keller msg: 1613173591745 epoch sec; seqNo:6752; depth 8.50 2021-02-12T23:46:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173591.76 epoch sec; seqNo:6752; 4057814.22 north; 591496.45 east 2021-02-12T23:46:32Z- AHRS_M2 msg: 1613173592.14 epoch sec; seqNo:6699 2021-02-12T23:46:32Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-12T23:46:32Z- handleDepth()-Depth_Keller msg: 1613173592149 epoch sec; seqNo:6753; depth 8.62 2021-02-12T23:46:32Z- handleDvl() - RDI_Pathfinder msg: 1613173592.16 epoch sec; seqNo:3142 2021-02-12T23:46:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.84 , 108.25 , 81.49 , 120.15 2021-02-12T23:46:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.16 , 0.01 2021-02-12T23:46:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173592.17 epoch sec; seqNo:6753; 4057814.60 north; 591496.45 east 2021-02-12T23:46:32Z- AHRS_M2 msg: 1613173592.54 epoch sec; seqNo:6700 2021-02-12T23:46:32Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-12T23:46:32Z- handleDepth()-Depth_Keller msg: 1613173592548 epoch sec; seqNo:6754; depth 8.74 2021-02-12T23:46:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173592.57 epoch sec; seqNo:6754; 4057814.98 north; 591496.44 east 2021-02-12T23:46:32Z- AHRS_M2 msg: 1613173592.95 epoch sec; seqNo:6701 2021-02-12T23:46:32Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.19 psi 2021-02-12T23:46:32Z- handleDepth()-Depth_Keller msg: 1613173592953 epoch sec; seqNo:6755; depth 8.83 2021-02-12T23:46:32Z- handleDvl() - RDI_Pathfinder msg: 1613173592.96 epoch sec; seqNo:3143 2021-02-12T23:46:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 108.32 , 81.55 , 114.27 2021-02-12T23:46:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.13 , 0.04 2021-02-12T23:46:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173592.99 epoch sec; seqNo:6755; 4057814.98 north; 591496.44 east 2021-02-12T23:46:33Z- LcmTrn::handleMessages() - handling 41 messages took 0.68 ms and 128 calls... 2021-02-12T23:46:33Z- waiting for 1613173591.14, time is 1613173592.99 2021-02-12T23:46:33Z- LcmTrn::updateTrn() equating pose 1613173592.99 and meas times 1613173592.99 2021-02-12T23:46:33Z- LcmTrn::updateTrn() >>>> pose time 1613173592.99, meas time 1613173592.99 <<<< 2021-02-12T23:46:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173592.99 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:46:33Z- TerrainNav::motionUpdate() - currEstimate = (4057815, 591496.44, 8.83) 2021-02-12T23:46:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:46:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:46:33Z- TNavBF::Projecting Measurements 2021-02-12T23:46:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:46:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:46:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:46:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:46:33Z- TNavFilter::4 good beams. 2021-02-12T23:46:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:46:33Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:46:33Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:46:33Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:46:33Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:46:33Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:46:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:46:33Z- TNavBF::mmse selected from filter -1 2021-02-12T23:46:33Z- TerrainMapOctree: (vehN, vehE ) = (4057814.98, 591496.44). 2021-02-12T23:46:33Z- TerrainMapOctree: Min Distance = 4402.43. 2021-02-12T23:46:33Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:46:33Z- There is no specific good beam that all particles have in common. 2021-02-12T23:46:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:46:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.53 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 12.67 2021-02-12T23:46:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:46:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.39 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 18.77 2021-02-12T23:46:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:46:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 10.64 2021-02-12T23:46:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:46:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 20.89 2021-02-12T23:46:33Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:46:33Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:46:33Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:46:33Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:46:33Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:46:33Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:46:33Z- Filter 0 No beams used in subcloud update 2021-02-12T23:46:33Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:46:33Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:46:33Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 737.9 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:46:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173592.99 sec. 2021-02-12T23:46:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:46:33Z- TNavBF::mmse selected from filter -1 2021-02-12T23:46:33Z- variances([0], [2], and psi_berg) = 737.892284 833.022598 0.000000 2021-02-12T23:46:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:46:33Z- LcmTrn::publishEstimates() - MLE : 4057805.48 591454.94 8.83 2021-02-12T23:46:33Z- LcmTrn::publishEstimates() - MMSE : 4057814.98 591496.44 8.83 2021-02-12T23:46:33Z- LcmTrn::publishEstimates() - COVAR: 737.89 833.02 1.12 0.00 2021-02-12T23:46:33Z- AHRS_M2 msg: 1613173593.35 epoch sec; seqNo:6702 2021-02-12T23:46:33Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.19 psi 2021-02-12T23:46:33Z- handleDepth()-Depth_Keller msg: 1613173593361 epoch sec; seqNo:6756; depth 8.95 2021-02-12T23:46:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173593.37 epoch sec; seqNo:6756; 4057815.36 north; 591496.44 east 2021-02-12T23:46:33Z- AHRS_M2 msg: 1613173593.76 epoch sec; seqNo:6703 2021-02-12T23:46:33Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.19 psi 2021-02-12T23:46:33Z- handleDepth()-Depth_Keller msg: 1613173593770 epoch sec; seqNo:6757; depth 9.06 2021-02-12T23:46:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173593.78 epoch sec; seqNo:6757; 4057815.75 north; 591497.66 east 2021-02-12T23:46:34Z- AHRS_M2 msg: 1613173594.16 epoch sec; seqNo:6704 2021-02-12T23:46:34Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.19 psi 2021-02-12T23:46:34Z- handleDepth()-Depth_Keller msg: 1613173594164 epoch sec; seqNo:6758; depth 9.17 2021-02-12T23:46:34Z- handleDvl() - RDI_Pathfinder msg: 1613173594.18 epoch sec; seqNo:3144 2021-02-12T23:46:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.71 , 106.63 , 80.95 , 120.15 2021-02-12T23:46:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , 0.12 2021-02-12T23:46:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173594.20 epoch sec; seqNo:6758; 4057816.13 north; 591497.65 east 2021-02-12T23:46:34Z- AHRS_M2 msg: 1613173594.57 epoch sec; seqNo:6705 2021-02-12T23:46:34Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-12T23:46:34Z- handleDepth()-Depth_Keller msg: 1613173594572 epoch sec; seqNo:6759; depth 9.27 2021-02-12T23:46:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173594.59 epoch sec; seqNo:6759; 4057816.13 north; 591497.65 east 2021-02-12T23:46:34Z- AHRS_M2 msg: 1613173594.97 epoch sec; seqNo:6706 2021-02-12T23:46:34Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-12T23:46:34Z- handleDepth()-Depth_Keller msg: 1613173594981 epoch sec; seqNo:6760; depth 9.41 2021-02-12T23:46:34Z- handleDvl() - RDI_Pathfinder msg: 1613173594.99 epoch sec; seqNo:3145 2021-02-12T23:46:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:46:34Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:46:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173595.00 epoch sec; seqNo:6760; 4057816.51 north; 591497.65 east 2021-02-12T23:46:35Z- AHRS_M2 msg: 1613173595.38 epoch sec; seqNo:6707 2021-02-12T23:46:35Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.17 psi 2021-02-12T23:46:35Z- handleDepth()-Depth_Keller msg: 1613173595379 epoch sec; seqNo:6761; depth 9.51 2021-02-12T23:46:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173595.40 epoch sec; seqNo:6761; 4057816.89 north; 591497.64 east 2021-02-12T23:46:35Z- AHRS_M2 msg: 1613173595.78 epoch sec; seqNo:6708 2021-02-12T23:46:35Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-12T23:46:35Z- handleDepth()-Depth_Keller msg: 1613173595785 epoch sec; seqNo:6762; depth 9.64 2021-02-12T23:46:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173595.79 epoch sec; seqNo:6762; 4057817.27 north; 591497.64 east 2021-02-12T23:46:36Z- AHRS_M2 msg: 1613173596.18 epoch sec; seqNo:6709 2021-02-12T23:46:36Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-12T23:46:36Z- handleDepth()-Depth_Keller msg: 1613173596189 epoch sec; seqNo:6763; depth 9.75 2021-02-12T23:46:36Z- handleDvl() - RDI_Pathfinder msg: 1613173596.20 epoch sec; seqNo:3146 2021-02-12T23:46:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 103.86 , 80.88 , 116.03 2021-02-12T23:46:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.10 , -0.03 2021-02-12T23:46:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173596.21 epoch sec; seqNo:6763; 4057817.27 north; 591497.64 east 2021-02-12T23:46:36Z- AHRS_M2 msg: 1613173596.58 epoch sec; seqNo:6710 2021-02-12T23:46:36Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.19 psi 2021-02-12T23:46:36Z- handleDepth()-Depth_Keller msg: 1613173596590 epoch sec; seqNo:6764; depth 9.87 2021-02-12T23:46:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173596.60 epoch sec; seqNo:6764; 4057817.66 north; 591498.86 east 2021-02-12T23:46:36Z- AHRS_M2 msg: 1613173596.99 epoch sec; seqNo:6711 2021-02-12T23:46:36Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.20 psi 2021-02-12T23:46:36Z- handleDepth()-Depth_Keller msg: 1613173596997 epoch sec; seqNo:6765; depth 9.98 2021-02-12T23:46:37Z- handleDvl() - RDI_Pathfinder msg: 1613173597.01 epoch sec; seqNo:3147 2021-02-12T23:46:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.84 , 105.41 , 81.08 , 117.58 2021-02-12T23:46:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , -0.10 2021-02-12T23:46:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173597.03 epoch sec; seqNo:6765; 4057818.04 north; 591498.85 east 2021-02-12T23:46:37Z- AHRS_M2 msg: 1613173597.40 epoch sec; seqNo:6712 2021-02-12T23:46:37Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.20 psi 2021-02-12T23:46:37Z- handleDepth()-Depth_Keller msg: 1613173597405 epoch sec; seqNo:6766; depth 10.11 2021-02-12T23:46:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173597.42 epoch sec; seqNo:6766; 4057818.04 north; 591498.85 east 2021-02-12T23:46:37Z- AHRS_M2 msg: 1613173597.80 epoch sec; seqNo:6713 2021-02-12T23:46:37Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.20 psi 2021-02-12T23:46:37Z- handleDepth()-Depth_Keller msg: 1613173597802 epoch sec; seqNo:6767; depth 10.23 2021-02-12T23:46:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173597.81 epoch sec; seqNo:6767; 4057818.42 north; 591498.85 east 2021-02-12T23:46:38Z- AHRS_M2 msg: 1613173598.20 epoch sec; seqNo:6714 2021-02-12T23:46:38Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.20 psi 2021-02-12T23:46:38Z- handleDepth()-Depth_Keller msg: 1613173598210 epoch sec; seqNo:6768; depth 10.35 2021-02-12T23:46:38Z- handleDvl() - RDI_Pathfinder msg: 1613173598.23 epoch sec; seqNo:3148 2021-02-12T23:46:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 106.63 , 80.95 , 117.44 2021-02-12T23:46:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , -0.08 2021-02-12T23:46:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173598.24 epoch sec; seqNo:6768; 4057818.80 north; 591498.85 east 2021-02-12T23:46:38Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-12T23:46:38Z- waiting for 1613173595.99, time is 1613173598.24 2021-02-12T23:46:38Z- LcmTrn::updateTrn() equating pose 1613173598.24 and meas times 1613173598.24 2021-02-12T23:46:38Z- LcmTrn::updateTrn() >>>> pose time 1613173598.24, meas time 1613173598.24 <<<< 2021-02-12T23:46:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173598.24 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:46:38Z- TerrainNav::motionUpdate() - currEstimate = (4057819, 591498.85, 10.35) 2021-02-12T23:46:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:46:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:46:38Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173598.24 sec.; No relevant navigation data available 2021-02-12T23:46:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:46:38Z- TNavBF::mmse selected from filter -1 2021-02-12T23:46:38Z- variances([0], [2], and psi_berg) = 737.803790 833.022527 0.000000 2021-02-12T23:46:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:46:38Z- LcmTrn::publishEstimates() - MLE : 4057809.30 591457.35 10.35 2021-02-12T23:46:38Z- LcmTrn::publishEstimates() - MMSE : 4057818.80 591498.85 10.35 2021-02-12T23:46:38Z- LcmTrn::publishEstimates() - COVAR: 737.80 833.02 1.35 0.00 2021-02-12T23:46:38Z- AHRS_M2 msg: 1613173598.60 epoch sec; seqNo:6715 2021-02-12T23:46:38Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.19 psi 2021-02-12T23:46:38Z- handleDepth()-Depth_Keller msg: 1613173598613 epoch sec; seqNo:6769; depth 10.48 2021-02-12T23:46:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173598.63 epoch sec; seqNo:6769; 4057819.17 north; 591498.84 east 2021-02-12T23:46:39Z- AHRS_M2 msg: 1613173599.01 epoch sec; seqNo:6716 2021-02-12T23:46:39Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.19 psi 2021-02-12T23:46:39Z- handleDepth()-Depth_Keller msg: 1613173599022 epoch sec; seqNo:6770; depth 10.57 2021-02-12T23:46:39Z- handleDvl() - RDI_Pathfinder msg: 1613173599.03 epoch sec; seqNo:3149 2021-02-12T23:46:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 106.90 , 80.14 , 115.82 2021-02-12T23:46:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , -0.05 2021-02-12T23:46:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173599.04 epoch sec; seqNo:6770; 4057819.19 north; 591500.06 east 2021-02-12T23:46:39Z- AHRS_M2 msg: 1613173599.42 epoch sec; seqNo:6717 2021-02-12T23:46:39Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.17 psi 2021-02-12T23:46:39Z- handleDepth()-Depth_Keller msg: 1613173599429 epoch sec; seqNo:6771; depth 10.70 2021-02-12T23:46:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173599.44 epoch sec; seqNo:6771; 4057819.57 north; 591500.06 east 2021-02-12T23:46:39Z- AHRS_M2 msg: 1613173599.82 epoch sec; seqNo:6718 2021-02-12T23:46:39Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.17 psi 2021-02-12T23:46:39Z- handleDepth()-Depth_Keller msg: 1613173599829 epoch sec; seqNo:6772; depth 10.81 2021-02-12T23:46:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173599.84 epoch sec; seqNo:6772; 4057819.94 north; 591500.05 east 2021-02-12T23:46:40Z- AHRS_M2 msg: 1613173600.22 epoch sec; seqNo:6719 2021-02-12T23:46:40Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.16 psi 2021-02-12T23:46:40Z- handleDepth()-Depth_Keller msg: 1613173600228 epoch sec; seqNo:6773; depth 10.94 2021-02-12T23:46:40Z- handleDvl() - RDI_Pathfinder msg: 1613173600.24 epoch sec; seqNo:3150 2021-02-12T23:46:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.15 , 105.07 , 79.39 , 115.89 2021-02-12T23:46:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.09 , -0.06 2021-02-12T23:46:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173600.25 epoch sec; seqNo:6773; 4057820.32 north; 591500.05 east 2021-02-12T23:46:40Z- AHRS_M2 msg: 1613173600.62 epoch sec; seqNo:6720 2021-02-12T23:46:40Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.17 psi 2021-02-12T23:46:40Z- handleDepth()-Depth_Keller msg: 1613173600628 epoch sec; seqNo:6774; depth 11.04 2021-02-12T23:46:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173600.64 epoch sec; seqNo:6774; 4057820.32 north; 591500.05 east 2021-02-12T23:46:41Z- AHRS_M2 msg: 1613173601.03 epoch sec; seqNo:6721 2021-02-12T23:46:41Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.17 psi 2021-02-12T23:46:41Z- handleDepth()-Depth_Keller msg: 1613173601032 epoch sec; seqNo:6775; depth 11.15 2021-02-12T23:46:41Z- handleDvl() - RDI_Pathfinder msg: 1613173601.04 epoch sec; seqNo:3151 2021-02-12T23:46:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 102.71 , 78.38 , 116.23 2021-02-12T23:46:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.11 , 0.00 2021-02-12T23:46:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173601.05 epoch sec; seqNo:6775; 4057820.70 north; 591500.05 east 2021-02-12T23:46:41Z- AHRS_M2 msg: 1613173601.43 epoch sec; seqNo:6722 2021-02-12T23:46:41Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.18 psi 2021-02-12T23:46:41Z- handleDepth()-Depth_Keller msg: 1613173601438 epoch sec; seqNo:6776; depth 11.28 2021-02-12T23:46:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173601.45 epoch sec; seqNo:6776; 4057821.08 north; 591500.04 east 2021-02-12T23:46:41Z- AHRS_M2 msg: 1613173601.85 epoch sec; seqNo:6723 2021-02-12T23:46:41Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-12T23:46:41Z- handleDepth()-Depth_Keller msg: 1613173601871 epoch sec; seqNo:6777; depth 11.40 2021-02-12T23:46:41Z- handleDvl() - RDI_Pathfinder msg: 1613173601.88 epoch sec; seqNo:3152 2021-02-12T23:46:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.13 , 78.44 , 116.29 2021-02-12T23:46:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , 0.09 2021-02-12T23:46:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173601.90 epoch sec; seqNo:6777; 4057821.09 north; 591501.26 east 2021-02-12T23:46:42Z- AHRS_M2 msg: 1613173602.24 epoch sec; seqNo:6724 2021-02-12T23:46:42Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-12T23:46:42Z- handleDepth()-Depth_Keller msg: 1613173602246 epoch sec; seqNo:6778; depth 11.54 2021-02-12T23:46:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173602.26 epoch sec; seqNo:6778; 4057821.47 north; 591501.26 east 2021-02-12T23:46:42Z- AHRS_M2 msg: 1613173602.64 epoch sec; seqNo:6725 2021-02-12T23:46:42Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.17 psi 2021-02-12T23:46:42Z- handleDepth()-Depth_Keller msg: 1613173602649 epoch sec; seqNo:6779; depth 11.65 2021-02-12T23:46:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173602.66 epoch sec; seqNo:6779; 4057821.85 north; 591501.25 east 2021-02-12T23:46:43Z- AHRS_M2 msg: 1613173603.05 epoch sec; seqNo:6726 2021-02-12T23:46:43Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.17 psi 2021-02-12T23:46:43Z- handleDepth()-Depth_Keller msg: 1613173603055 epoch sec; seqNo:6780; depth 11.77 2021-02-12T23:46:43Z- handleDvl() - RDI_Pathfinder msg: 1613173603.07 epoch sec; seqNo:3153 2021-02-12T23:46:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 103.79 , 79.46 , 117.31 2021-02-12T23:46:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.03 , 0.10 2021-02-12T23:46:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173603.08 epoch sec; seqNo:6780; 4057821.85 north; 591501.25 east 2021-02-12T23:46:43Z- LcmTrn::handleMessages() - handling 41 messages took 0.67 ms and 128 calls... 2021-02-12T23:46:43Z- waiting for 1613173601.24, time is 1613173603.08 2021-02-12T23:46:43Z- LcmTrn::updateTrn() equating pose 1613173603.08 and meas times 1613173603.08 2021-02-12T23:46:43Z- LcmTrn::updateTrn() >>>> pose time 1613173603.08, meas time 1613173603.08 <<<< 2021-02-12T23:46:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173603.08 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:46:43Z- TerrainNav::motionUpdate() - currEstimate = (4057822, 591501.25, 11.77) 2021-02-12T23:46:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:46:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:46:43Z- TNavBF::Projecting Measurements 2021-02-12T23:46:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:46:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:46:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:46:43Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:46:43Z- TNavFilter::3 good beams. 2021-02-12T23:46:43Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:46:43Z- TNavBF:: beamIndex[0] = 1 2021-02-12T23:46:43Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:46:43Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:46:43Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:46:43Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:46:43Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:46:43Z- TNavBF::mmse selected from filter -1 2021-02-12T23:46:43Z- TerrainMapOctree: (vehN, vehE ) = (4057821.85, 591501.25). 2021-02-12T23:46:43Z- TerrainMapOctree: Min Distance = 4394.06. 2021-02-12T23:46:43Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:46:43Z- There is no specific good beam that all particles have in common. 2021-02-12T23:46:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:46:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.55 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 17.24 2021-02-12T23:46:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:46:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.108 ExpectedMeasDiff: nan currMeas.covariance: 10.10 2021-02-12T23:46:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:46:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 22.02 2021-02-12T23:46:43Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:46:43Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:46:43Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:46:43Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:46:43Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:46:43Z- Filter 0 No beams used in subcloud update 2021-02-12T23:46:43Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:46:43Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:46:43Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 737.7 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:46:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173603.08 sec. 2021-02-12T23:46:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:46:43Z- TNavBF::mmse selected from filter -1 2021-02-12T23:46:43Z- variances([0], [2], and psi_berg) = 737.721647 833.022462 0.000000 2021-02-12T23:46:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:46:43Z- LcmTrn::publishEstimates() - MLE : 4057812.35 591459.75 11.77 2021-02-12T23:46:43Z- LcmTrn::publishEstimates() - MMSE : 4057821.85 591501.25 11.77 2021-02-12T23:46:43Z- LcmTrn::publishEstimates() - COVAR: 737.72 833.02 1.21 0.00 2021-02-12T23:46:43Z- AHRS_M2 msg: 1613173603.45 epoch sec; seqNo:6727 2021-02-12T23:46:43Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-12T23:46:43Z- handleDepth()-Depth_Keller msg: 1613173603455 epoch sec; seqNo:6781; depth 11.89 2021-02-12T23:46:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173603.47 epoch sec; seqNo:6781; 4057822.23 north; 591501.25 east 2021-02-12T23:46:43Z- AHRS_M2 msg: 1613173603.86 epoch sec; seqNo:6728 2021-02-12T23:46:43Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-12T23:46:43Z- handleDepth()-Depth_Keller msg: 1613173603860 epoch sec; seqNo:6782; depth 12.02 2021-02-12T23:46:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173603.87 epoch sec; seqNo:6782; 4057822.23 north; 591501.25 east 2021-02-12T23:46:44Z- AHRS_M2 msg: 1613173604.27 epoch sec; seqNo:6729 2021-02-12T23:46:44Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.19 psi 2021-02-12T23:46:44Z- handleDepth()-Depth_Keller msg: 1613173604277 epoch sec; seqNo:6783; depth 12.13 2021-02-12T23:46:44Z- handleDvl() - RDI_Pathfinder msg: 1613173604.29 epoch sec; seqNo:3154 2021-02-12T23:46:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 102.44 , 78.11 , 118.66 2021-02-12T23:46:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.07 , 0.02 2021-02-12T23:46:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173604.30 epoch sec; seqNo:6783; 4057822.61 north; 591501.25 east 2021-02-12T23:46:44Z- AHRS_M2 msg: 1613173604.67 epoch sec; seqNo:6730 2021-02-12T23:46:44Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.18 psi 2021-02-12T23:46:44Z- handleDepth()-Depth_Keller msg: 1613173604686 epoch sec; seqNo:6784; depth 12.28 2021-02-12T23:46:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173604.70 epoch sec; seqNo:6784; 4057823.00 north; 591502.46 east 2021-02-12T23:46:45Z- AHRS_M2 msg: 1613173605.07 epoch sec; seqNo:6731 2021-02-12T23:46:45Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.18 psi 2021-02-12T23:46:45Z- handleDepth()-Depth_Keller msg: 1613173605072 epoch sec; seqNo:6785; depth 12.41 2021-02-12T23:46:45Z- handleDvl() - RDI_Pathfinder msg: 1613173605.08 epoch sec; seqNo:3155 2021-02-12T23:46:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.20 , 104.13 , 77.64 , 117.65 2021-02-12T23:46:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , -0.09 2021-02-12T23:46:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173605.11 epoch sec; seqNo:6785; 4057823.38 north; 591502.46 east 2021-02-12T23:46:45Z- AHRS_M2 msg: 1613173605.47 epoch sec; seqNo:6732 2021-02-12T23:46:45Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-12T23:46:45Z- handleDepth()-Depth_Keller msg: 1613173605476 epoch sec; seqNo:6786; depth 12.53 2021-02-12T23:46:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173605.49 epoch sec; seqNo:6786; 4057823.38 north; 591502.46 east 2021-02-12T23:46:45Z- AHRS_M2 msg: 1613173605.88 epoch sec; seqNo:6733 2021-02-12T23:46:45Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.19 psi 2021-02-12T23:46:45Z- handleDepth()-Depth_Keller msg: 1613173605882 epoch sec; seqNo:6787; depth 12.67 2021-02-12T23:46:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173605.89 epoch sec; seqNo:6787; 4057823.76 north; 591502.46 east 2021-02-12T23:46:46Z- AHRS_M2 msg: 1613173606.28 epoch sec; seqNo:6734 2021-02-12T23:46:46Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.19 psi 2021-02-12T23:46:46Z- handleDepth()-Depth_Keller msg: 1613173606285 epoch sec; seqNo:6788; depth 12.80 2021-02-12T23:46:46Z- handleDvl() - RDI_Pathfinder msg: 1613173606.30 epoch sec; seqNo:3156 2021-02-12T23:46:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.20 , 104.13 , 78.44 , 114.94 2021-02-12T23:46:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.07 , -0.13 2021-02-12T23:46:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173606.31 epoch sec; seqNo:6788; 4057824.14 north; 591502.45 east 2021-02-12T23:46:46Z- AHRS_M2 msg: 1613173606.68 epoch sec; seqNo:6735 2021-02-12T23:46:46Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.20 psi 2021-02-12T23:46:46Z- handleDepth()-Depth_Keller msg: 1613173606688 epoch sec; seqNo:6789; depth 12.94 2021-02-12T23:46:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173606.72 epoch sec; seqNo:6789; 4057824.53 north; 591503.67 east 2021-02-12T23:46:47Z- AHRS_M2 msg: 1613173607.09 epoch sec; seqNo:6736 2021-02-12T23:46:47Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.20 psi 2021-02-12T23:46:47Z- handleDepth()-Depth_Keller msg: 1613173607097 epoch sec; seqNo:6790; depth 13.06 2021-02-12T23:46:47Z- handleDvl() - RDI_Pathfinder msg: 1613173607.11 epoch sec; seqNo:3157 2021-02-12T23:46:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.50 , 104.13 , 78.44 , 114.94 2021-02-12T23:46:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.14 , -0.11 2021-02-12T23:46:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173607.12 epoch sec; seqNo:6790; 4057824.91 north; 591503.66 east 2021-02-12T23:46:47Z- AHRS_M2 msg: 1613173607.49 epoch sec; seqNo:6737 2021-02-12T23:46:47Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.20 psi 2021-02-12T23:46:47Z- handleDepth()-Depth_Keller msg: 1613173607499 epoch sec; seqNo:6791; depth 13.21 2021-02-12T23:46:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173607.51 epoch sec; seqNo:6791; 4057824.91 north; 591503.66 east 2021-02-12T23:46:47Z- AHRS_M2 msg: 1613173607.90 epoch sec; seqNo:6738 2021-02-12T23:46:47Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.20 psi 2021-02-12T23:46:47Z- handleDepth()-Depth_Keller msg: 1613173607915 epoch sec; seqNo:6792; depth 13.32 2021-02-12T23:46:47Z- handleDvl() - RDI_Pathfinder msg: 1613173607.93 epoch sec; seqNo:3158 2021-02-12T23:46:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 104.20 , 78.51 , 115.01 2021-02-12T23:46:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.15 , -0.06 2021-02-12T23:46:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173607.94 epoch sec; seqNo:6792; 4057825.29 north; 591503.66 east 2021-02-12T23:46:48Z- AHRS_M2 msg: 1613173608.30 epoch sec; seqNo:6739 2021-02-12T23:46:48Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.19 psi 2021-02-12T23:46:48Z- handleDepth()-Depth_Keller msg: 1613173608313 epoch sec; seqNo:6793; depth 13.43 2021-02-12T23:46:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173608.32 epoch sec; seqNo:6793; 4057825.67 north; 591503.66 east 2021-02-12T23:46:48Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-12T23:46:48Z- waiting for 1613173606.08, time is 1613173608.32 2021-02-12T23:46:48Z- LcmTrn::updateTrn() >>>> pose time 1613173608.32, meas time 1613173607.93 <<<< 2021-02-12T23:46:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173607.93 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:46:48Z- TerrainNav::motionUpdate() - currEstimate = (4057826, 591503.66, 13.43) 2021-02-12T23:46:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:46:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:46:48Z- TNavBF::Projecting Measurements 2021-02-12T23:46:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:46:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:46:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:46:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:46:48Z- TNavFilter::4 good beams. 2021-02-12T23:46:48Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:46:48Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:46:48Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:46:48Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:46:48Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:46:48Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:46:48Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:46:48Z- TNavBF::mmse selected from filter -1 2021-02-12T23:46:48Z- TerrainMapOctree: (vehN, vehE ) = (4057825.38, 591503.47). 2021-02-12T23:46:48Z- TerrainMapOctree: Min Distance = 4389.92. 2021-02-12T23:46:48Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:46:48Z- There is no specific good beam that all particles have in common. 2021-02-12T23:46:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:46:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.42 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 11.27 2021-02-12T23:46:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:46:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 17.37 2021-02-12T23:46:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:46:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-12T23:46:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:46:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 21.16 2021-02-12T23:46:48Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:46:48Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:46:48Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:46:48Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:46:48Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:46:48Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:46:48Z- Filter 0 No beams used in subcloud update 2021-02-12T23:46:48Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:46:48Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:46:48Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 737.6 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:46:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173607.93 sec. 2021-02-12T23:46:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:46:48Z- TNavBF::mmse selected from filter -1 2021-02-12T23:46:48Z- variances([0], [2], and psi_berg) = 737.612493 833.022374 0.000000 2021-02-12T23:46:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:46:48Z- LcmTrn::publishEstimates() - MLE : 4057816.17 591462.16 13.43 2021-02-12T23:46:48Z- LcmTrn::publishEstimates() - MMSE : 4057825.67 591503.66 13.43 2021-02-12T23:46:48Z- LcmTrn::publishEstimates() - COVAR: 737.61 833.02 1.06 0.00 2021-02-12T23:46:48Z- AHRS_M2 msg: 1613173608.71 epoch sec; seqNo:6740 2021-02-12T23:46:48Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.18 psi 2021-02-12T23:46:48Z- handleDepth()-Depth_Keller msg: 1613173608718 epoch sec; seqNo:6794; depth 13.55 2021-02-12T23:46:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173608.73 epoch sec; seqNo:6794; 4057826.05 north; 591503.65 east 2021-02-12T23:46:49Z- AHRS_M2 msg: 1613173609.11 epoch sec; seqNo:6741 2021-02-12T23:46:49Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.18 psi 2021-02-12T23:46:49Z- handleDepth()-Depth_Keller msg: 1613173609118 epoch sec; seqNo:6795; depth 13.67 2021-02-12T23:46:49Z- handleDvl() - RDI_Pathfinder msg: 1613173609.14 epoch sec; seqNo:3159 2021-02-12T23:46:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 96.09 , 78.51 , 110.96 2021-02-12T23:46:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , -0.01 2021-02-12T23:46:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173609.15 epoch sec; seqNo:6795; 4057826.42 north; 591503.65 east 2021-02-12T23:46:49Z- AHRS_M2 msg: 1613173609.52 epoch sec; seqNo:6742 2021-02-12T23:46:49Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.18 psi 2021-02-12T23:46:49Z- handleDepth()-Depth_Keller msg: 1613173609525 epoch sec; seqNo:6796; depth 13.82 2021-02-12T23:46:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173609.54 epoch sec; seqNo:6796; 4057826.42 north; 591503.65 east 2021-02-12T23:46:49Z- handleDepth()-Depth_Keller msg: 1613173609941 epoch sec; seqNo:6797; depth 13.93 2021-02-12T23:46:49Z- handleDvl() - RDI_Pathfinder msg: 1613173609.95 epoch sec; seqNo:3160 2021-02-12T23:46:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.41 , 76.41 , 109.87 2021-02-12T23:46:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.00 2021-02-12T23:46:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173609.97 epoch sec; seqNo:6797; 4057826.82 north; 591504.86 east 2021-02-12T23:46:50Z- AHRS_M2 msg: 1613173610.32 epoch sec; seqNo:6743 2021-02-12T23:46:50Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.19 psi 2021-02-12T23:46:50Z- handleDepth()-Depth_Keller msg: 1613173610335 epoch sec; seqNo:6798; depth 14.08 2021-02-12T23:46:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173610.35 epoch sec; seqNo:6798; 4057827.19 north; 591504.86 east 2021-02-12T23:46:50Z- AHRS_M2 msg: 1613173610.73 epoch sec; seqNo:6744 2021-02-12T23:46:50Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.20 psi 2021-02-12T23:46:50Z- handleDepth()-Depth_Keller msg: 1613173610732 epoch sec; seqNo:6799; depth 14.18 2021-02-12T23:46:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173610.74 epoch sec; seqNo:6799; 4057827.57 north; 591504.86 east 2021-02-12T23:46:51Z- AHRS_M2 msg: 1613173611.13 epoch sec; seqNo:6745 2021-02-12T23:46:51Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.21 psi 2021-02-12T23:46:51Z- handleDepth()-Depth_Keller msg: 1613173611140 epoch sec; seqNo:6800; depth 14.32 2021-02-12T23:46:51Z- handleDvl() - RDI_Pathfinder msg: 1613173611.15 epoch sec; seqNo:3161 2021-02-12T23:46:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.22 , 100.14 , 75.18 , 110.96 2021-02-12T23:46:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.07 , -0.01 2021-02-12T23:46:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173611.18 epoch sec; seqNo:6800; 4057827.57 north; 591504.86 east 2021-02-12T23:46:51Z- AHRS_M2 msg: 1613173611.54 epoch sec; seqNo:6746 2021-02-12T23:46:51Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.21 psi 2021-02-12T23:46:51Z- handleDepth()-Depth_Keller msg: 1613173611545 epoch sec; seqNo:6801; depth 14.42 2021-02-12T23:46:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173611.56 epoch sec; seqNo:6801; 4057827.95 north; 591504.85 east 2021-02-12T23:46:51Z- AHRS_M2 msg: 1613173611.94 epoch sec; seqNo:6747 2021-02-12T23:46:51Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.21 psi 2021-02-12T23:46:51Z- handleDepth()-Depth_Keller msg: 1613173611944 epoch sec; seqNo:6802; depth 14.53 2021-02-12T23:46:51Z- handleDvl() - RDI_Pathfinder msg: 1613173611.96 epoch sec; seqNo:3162 2021-02-12T23:46:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.76 , 78.78 , 111.23 2021-02-12T23:46:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.08 , 0.03 2021-02-12T23:46:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173611.97 epoch sec; seqNo:6802; 4057828.33 north; 591504.85 east 2021-02-12T23:46:52Z- AHRS_M2 msg: 1613173612.34 epoch sec; seqNo:6748 2021-02-12T23:46:52Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.21 psi 2021-02-12T23:46:52Z- handleDepth()-Depth_Keller msg: 1613173612348 epoch sec; seqNo:6803; depth 14.66 2021-02-12T23:46:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173612.36 epoch sec; seqNo:6803; 4057828.33 north; 591504.85 east 2021-02-12T23:46:52Z- AHRS_M2 msg: 1613173612.75 epoch sec; seqNo:6749 2021-02-12T23:46:52Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.21 psi 2021-02-12T23:46:52Z- handleDepth()-Depth_Keller msg: 1613173612756 epoch sec; seqNo:6804; depth 14.77 2021-02-12T23:46:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173612.77 epoch sec; seqNo:6804; 4057828.71 north; 591504.84 east 2021-02-12T23:46:53Z- AHRS_M2 msg: 1613173613.15 epoch sec; seqNo:6750 2021-02-12T23:46:53Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.21 psi 2021-02-12T23:46:53Z- handleDepth()-Depth_Keller msg: 1613173613158 epoch sec; seqNo:6805; depth 14.89 2021-02-12T23:46:53Z- handleDvl() - RDI_Pathfinder msg: 1613173613.18 epoch sec; seqNo:3163 2021-02-12T23:46:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.01 , 101.29 , 0.00 , 109.40 2021-02-12T23:46:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.03 , 0.07 2021-02-12T23:46:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173613.19 epoch sec; seqNo:6805; 4057829.10 north; 591506.06 east 2021-02-12T23:46:53Z- LcmTrn::handleMessages() - handling 40 messages took 0.59 ms and 128 calls... 2021-02-12T23:46:53Z- waiting for 1613173611.32, time is 1613173613.19 2021-02-12T23:46:53Z- LcmTrn::updateTrn() equating pose 1613173613.19 and meas times 1613173613.19 2021-02-12T23:46:53Z- LcmTrn::updateTrn() >>>> pose time 1613173613.19, meas time 1613173613.19 <<<< 2021-02-12T23:46:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173613.19 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:46:53Z- TerrainNav::motionUpdate() - currEstimate = (4057829, 591506.06, 14.89) 2021-02-12T23:46:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:46:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:46:53Z- TNavBF::Projecting Measurements 2021-02-12T23:46:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:46:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:46:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:46:53Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:46:53Z- TNavFilter::3 good beams. 2021-02-12T23:46:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:46:53Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:46:53Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:46:53Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:46:53Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:46:53Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:46:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:46:53Z- TNavBF::mmse selected from filter -1 2021-02-12T23:46:53Z- TerrainMapOctree: (vehN, vehE ) = (4057829.10, 591506.06). 2021-02-12T23:46:53Z- TerrainMapOctree: Min Distance = 4385.40. 2021-02-12T23:46:53Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:46:53Z- There is no specific good beam that all particles have in common. 2021-02-12T23:46:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:46:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 10.50 2021-02-12T23:46:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:46:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 16.42 2021-02-12T23:46:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:46:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 19.15 2021-02-12T23:46:53Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:46:53Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:46:53Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:46:53Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:46:53Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:46:53Z- Filter 0 No beams used in subcloud update 2021-02-12T23:46:53Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:46:53Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:46:53Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 737.5 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:46:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173613.19 sec. 2021-02-12T23:46:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:46:53Z- TNavBF::mmse selected from filter -1 2021-02-12T23:46:53Z- variances([0], [2], and psi_berg) = 737.527426 833.022307 0.000000 2021-02-12T23:46:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:46:53Z- LcmTrn::publishEstimates() - MLE : 4057819.60 591464.56 14.89 2021-02-12T23:46:53Z- LcmTrn::publishEstimates() - MMSE : 4057829.10 591506.06 14.89 2021-02-12T23:46:53Z- LcmTrn::publishEstimates() - COVAR: 737.53 833.02 1.03 0.00 2021-02-12T23:46:53Z- AHRS_M2 msg: 1613173613.56 epoch sec; seqNo:6751 2021-02-12T23:46:53Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.21 psi 2021-02-12T23:46:53Z- handleDepth()-Depth_Keller msg: 1613173613593 epoch sec; seqNo:6806; depth 15.02 2021-02-12T23:46:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173613.60 epoch sec; seqNo:6806; 4057829.10 north; 591506.06 east 2021-02-12T23:46:53Z- AHRS_M2 msg: 1613173613.96 epoch sec; seqNo:6752 2021-02-12T23:46:53Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.21 psi 2021-02-12T23:46:53Z- handleDepth()-Depth_Keller msg: 1613173613967 epoch sec; seqNo:6807; depth 15.13 2021-02-12T23:46:53Z- handleDvl() - RDI_Pathfinder msg: 1613173613.98 epoch sec; seqNo:3164 2021-02-12T23:46:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.01 , 101.29 , 75.59 , 110.75 2021-02-12T23:46:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.02 , 0.07 2021-02-12T23:46:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173613.99 epoch sec; seqNo:6807; 4057829.48 north; 591506.06 east 2021-02-12T23:46:54Z- AHRS_M2 msg: 1613173614.36 epoch sec; seqNo:6753 2021-02-12T23:46:54Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.20 psi 2021-02-12T23:46:54Z- handleDepth()-Depth_Keller msg: 1613173614382 epoch sec; seqNo:6808; depth 15.24 2021-02-12T23:46:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173614.39 epoch sec; seqNo:6808; 4057829.86 north; 591506.05 east 2021-02-12T23:46:54Z- AHRS_M2 msg: 1613173614.77 epoch sec; seqNo:6754 2021-02-12T23:46:54Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.20 psi 2021-02-12T23:46:54Z- handleDepth()-Depth_Keller msg: 1613173614772 epoch sec; seqNo:6809; depth 15.36 2021-02-12T23:46:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173614.78 epoch sec; seqNo:6809; 4057829.86 north; 591506.05 east 2021-02-12T23:46:55Z- AHRS_M2 msg: 1613173615.17 epoch sec; seqNo:6755 2021-02-12T23:46:55Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.20 psi 2021-02-12T23:46:55Z- handleDepth()-Depth_Keller msg: 1613173615176 epoch sec; seqNo:6810; depth 15.46 2021-02-12T23:46:55Z- handleDvl() - RDI_Pathfinder msg: 1613173615.19 epoch sec; seqNo:3165 2021-02-12T23:46:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.49 , 100.41 , 78.78 , 108.52 2021-02-12T23:46:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , 0.02 2021-02-12T23:46:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173615.20 epoch sec; seqNo:6810; 4057830.24 north; 591506.05 east 2021-02-12T23:46:55Z- AHRS_M2 msg: 1613173615.58 epoch sec; seqNo:6756 2021-02-12T23:46:55Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.20 psi 2021-02-12T23:46:55Z- handleDepth()-Depth_Keller msg: 1613173615581 epoch sec; seqNo:6811; depth 15.58 2021-02-12T23:46:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173615.60 epoch sec; seqNo:6811; 4057830.63 north; 591507.27 east 2021-02-12T23:46:55Z- AHRS_M2 msg: 1613173615.98 epoch sec; seqNo:6757 2021-02-12T23:46:55Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.20 psi 2021-02-12T23:46:55Z- handleDepth()-Depth_Keller msg: 1613173615995 epoch sec; seqNo:6812; depth 15.68 2021-02-12T23:46:56Z- handleDvl() - RDI_Pathfinder msg: 1613173616.01 epoch sec; seqNo:3166 2021-02-12T23:46:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.49 , 100.41 , 78.78 , 111.23 2021-02-12T23:46:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , -0.03 2021-02-12T23:46:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173616.02 epoch sec; seqNo:6812; 4057830.63 north; 591507.27 east 2021-02-12T23:46:56Z- AHRS_M2 msg: 1613173616.39 epoch sec; seqNo:6758 2021-02-12T23:46:56Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.21 psi 2021-02-12T23:46:56Z- handleDepth()-Depth_Keller msg: 1613173616394 epoch sec; seqNo:6813; depth 15.82 2021-02-12T23:46:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173616.42 epoch sec; seqNo:6813; 4057831.01 north; 591507.26 east 2021-02-12T23:46:56Z- AHRS_M2 msg: 1613173616.79 epoch sec; seqNo:6759 2021-02-12T23:46:56Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.21 psi 2021-02-12T23:46:56Z- handleDepth()-Depth_Keller msg: 1613173616794 epoch sec; seqNo:6814; depth 15.91 2021-02-12T23:46:56Z- handleDvl() - RDI_Pathfinder msg: 1613173616.81 epoch sec; seqNo:3167 2021-02-12T23:46:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.35 , 73.96 , 111.17 2021-02-12T23:46:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.07 , -0.07 2021-02-12T23:46:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173616.82 epoch sec; seqNo:6814; 4057831.39 north; 591507.26 east 2021-02-12T23:46:57Z- AHRS_M2 msg: 1613173617.19 epoch sec; seqNo:6760 2021-02-12T23:46:57Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.20 psi 2021-02-12T23:46:57Z- handleDepth()-Depth_Keller msg: 1613173617198 epoch sec; seqNo:6815; depth 16.04 2021-02-12T23:46:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173617.21 epoch sec; seqNo:6815; 4057831.77 north; 591507.25 east 2021-02-12T23:46:57Z- AHRS_M2 msg: 1613173617.60 epoch sec; seqNo:6761 2021-02-12T23:46:57Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.20 psi 2021-02-12T23:46:57Z- handleDepth()-Depth_Keller msg: 1613173617618 epoch sec; seqNo:6816; depth 16.14 2021-02-12T23:46:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173617.63 epoch sec; seqNo:6816; 4057832.15 north; 591507.25 east 2021-02-12T23:46:58Z- AHRS_M2 msg: 1613173618.00 epoch sec; seqNo:6762 2021-02-12T23:46:58Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.19 psi 2021-02-12T23:46:58Z- handleDepth()-Depth_Keller msg: 1613173618009 epoch sec; seqNo:6817; depth 16.27 2021-02-12T23:46:58Z- handleDvl() - RDI_Pathfinder msg: 1613173618.02 epoch sec; seqNo:3168 2021-02-12T23:46:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.79 , 78.51 , 108.26 2021-02-12T23:46:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.13 , -0.10 2021-02-12T23:46:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173618.04 epoch sec; seqNo:6817; 4057832.52 north; 591507.25 east 2021-02-12T23:46:58Z- AHRS_M2 msg: 1613173618.40 epoch sec; seqNo:6763 2021-02-12T23:46:58Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.18 psi 2021-02-12T23:46:58Z- handleDepth()-Depth_Keller msg: 1613173618408 epoch sec; seqNo:6818; depth 16.40 2021-02-12T23:46:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173618.42 epoch sec; seqNo:6818; 4057832.92 north; 591508.46 east 2021-02-12T23:46:58Z- LcmTrn::handleMessages() - handling 44 messages took 0.60 ms and 132 calls... 2021-02-12T23:46:58Z- waiting for 1613173616.19, time is 1613173618.42 2021-02-12T23:46:58Z- LcmTrn::updateTrn() >>>> pose time 1613173618.42, meas time 1613173618.02 <<<< 2021-02-12T23:46:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173618.02 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:46:58Z- TerrainNav::motionUpdate() - currEstimate = (4057833, 591508.46, 16.40) 2021-02-12T23:46:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:46:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:46:58Z- TNavBF::Projecting Measurements 2021-02-12T23:46:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:46:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:46:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:46:58Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:46:58Z- TNavFilter::3 good beams. 2021-02-12T23:46:58Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:46:58Z- TNavBF:: beamIndex[0] = 1 2021-02-12T23:46:58Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:46:58Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:46:58Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:46:58Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:46:58Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:46:58Z- TNavBF::mmse selected from filter -1 2021-02-12T23:46:58Z- TerrainMapOctree: (vehN, vehE ) = (4057832.63, 591508.28). 2021-02-12T23:46:58Z- TerrainMapOctree: Min Distance = 4381.26. 2021-02-12T23:46:58Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:46:58Z- There is no specific good beam that all particles have in common. 2021-02-12T23:46:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:46:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 15.62 2021-02-12T23:46:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:46:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-12T23:46:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:46:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 18.75 2021-02-12T23:46:58Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:46:58Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:46:58Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:46:58Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:46:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:46:58Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:46:58Z- Filter 0 No beams used in subcloud update 2021-02-12T23:46:58Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:46:58Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:46:58Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 737.4 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:46:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173618.02 sec. 2021-02-12T23:46:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:46:58Z- TNavBF::mmse selected from filter -1 2021-02-12T23:46:58Z- variances([0], [2], and psi_berg) = 737.418464 833.022220 0.000000 2021-02-12T23:46:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:46:58Z- LcmTrn::publishEstimates() - MLE : 4057823.42 591466.96 16.40 2021-02-12T23:46:58Z- LcmTrn::publishEstimates() - MMSE : 4057832.92 591508.46 16.40 2021-02-12T23:46:58Z- LcmTrn::publishEstimates() - COVAR: 737.42 833.02 1.02 0.00 2021-02-12T23:46:58Z- AHRS_M2 msg: 1613173618.81 epoch sec; seqNo:6764 2021-02-12T23:46:58Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.18 psi 2021-02-12T23:46:58Z- handleDepth()-Depth_Keller msg: 1613173618822 epoch sec; seqNo:6819; depth 16.50 2021-02-12T23:46:58Z- handleDvl() - RDI_Pathfinder msg: 1613173618.84 epoch sec; seqNo:3169 2021-02-12T23:46:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 98.79 , 0.00 , 106.90 2021-02-12T23:46:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.18 , -0.07 2021-02-12T23:46:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173618.85 epoch sec; seqNo:6819; 4057833.30 north; 591508.46 east 2021-02-12T23:46:59Z- AHRS_M2 msg: 1613173619.21 epoch sec; seqNo:6765 2021-02-12T23:46:59Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.17 psi 2021-02-12T23:46:59Z- handleDepth()-Depth_Keller msg: 1613173619216 epoch sec; seqNo:6820; depth 16.62 2021-02-12T23:46:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173619.24 epoch sec; seqNo:6820; 4057833.30 north; 591508.46 east 2021-02-12T23:46:59Z- AHRS_M2 msg: 1613173619.62 epoch sec; seqNo:6766 2021-02-12T23:46:59Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.17 psi 2021-02-12T23:46:59Z- handleDepth()-Depth_Keller msg: 1613173619623 epoch sec; seqNo:6821; depth 16.73 2021-02-12T23:46:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173619.63 epoch sec; seqNo:6821; 4057833.67 north; 591508.45 east 2021-02-12T23:47:00Z- AHRS_M2 msg: 1613173620.02 epoch sec; seqNo:6767 2021-02-12T23:47:00Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.16 psi 2021-02-12T23:47:00Z- handleDepth()-Depth_Keller msg: 1613173620029 epoch sec; seqNo:6822; depth 16.83 2021-02-12T23:47:00Z- handleDvl() - RDI_Pathfinder msg: 1613173620.04 epoch sec; seqNo:3170 2021-02-12T23:47:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.93 , 96.15 , 73.14 , 105.62 2021-02-12T23:47:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.17 , -0.01 2021-02-12T23:47:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173620.05 epoch sec; seqNo:6822; 4057834.05 north; 591508.45 east 2021-02-12T23:47:00Z- AHRS_M2 msg: 1613173620.43 epoch sec; seqNo:6768 2021-02-12T23:47:00Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.16 psi 2021-02-12T23:47:00Z- handleDepth()-Depth_Keller msg: 1613173620434 epoch sec; seqNo:6823; depth 16.96 2021-02-12T23:47:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173620.44 epoch sec; seqNo:6823; 4057834.43 north; 591508.45 east 2021-02-12T23:47:00Z- AHRS_M2 msg: 1613173620.83 epoch sec; seqNo:6769 2021-02-12T23:47:00Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.16 psi 2021-02-12T23:47:00Z- handleDepth()-Depth_Keller msg: 1613173620832 epoch sec; seqNo:6824; depth 17.07 2021-02-12T23:47:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173620.84 epoch sec; seqNo:6824; 4057834.81 north; 591508.44 east 2021-02-12T23:47:01Z- AHRS_M2 msg: 1613173621.23 epoch sec; seqNo:6770 2021-02-12T23:47:01Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.16 psi 2021-02-12T23:47:01Z- handleDepth()-Depth_Keller msg: 1613173621241 epoch sec; seqNo:6825; depth 17.18 2021-02-12T23:47:01Z- handleDvl() - RDI_Pathfinder msg: 1613173621.25 epoch sec; seqNo:3171 2021-02-12T23:47:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.81 , 97.84 , 78.38 , 105.14 2021-02-12T23:47:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.10 , 0.04 2021-02-12T23:47:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173621.28 epoch sec; seqNo:6825; 4057834.81 north; 591508.44 east 2021-02-12T23:47:01Z- AHRS_M2 msg: 1613173621.64 epoch sec; seqNo:6771 2021-02-12T23:47:01Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.15 psi 2021-02-12T23:47:01Z- handleDepth()-Depth_Keller msg: 1613173621647 epoch sec; seqNo:6826; depth 17.30 2021-02-12T23:47:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173621.66 epoch sec; seqNo:6826; 4057835.20 north; 591509.66 east 2021-02-12T23:47:02Z- AHRS_M2 msg: 1613173622.04 epoch sec; seqNo:6772 2021-02-12T23:47:02Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.15 psi 2021-02-12T23:47:02Z- handleDepth()-Depth_Keller msg: 1613173622046 epoch sec; seqNo:6827; depth 17.41 2021-02-12T23:47:02Z- handleDvl() - RDI_Pathfinder msg: 1613173622.06 epoch sec; seqNo:3172 2021-02-12T23:47:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 97.71 , 78.24 , 103.79 2021-02-12T23:47:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , 0.05 2021-02-12T23:47:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173622.07 epoch sec; seqNo:6827; 4057835.58 north; 591509.65 east 2021-02-12T23:47:02Z- AHRS_M2 msg: 1613173622.44 epoch sec; seqNo:6773 2021-02-12T23:47:02Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.15 psi 2021-02-12T23:47:02Z- handleDepth()-Depth_Keller msg: 1613173622448 epoch sec; seqNo:6828; depth 17.53 2021-02-12T23:47:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173622.48 epoch sec; seqNo:6828; 4057835.96 north; 591509.65 east 2021-02-12T23:47:02Z- AHRS_M2 msg: 1613173622.86 epoch sec; seqNo:6774 2021-02-12T23:47:02Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.16 psi 2021-02-12T23:47:02Z- handleDepth()-Depth_Keller msg: 1613173622859 epoch sec; seqNo:6829; depth 17.63 2021-02-12T23:47:02Z- handleDvl() - RDI_Pathfinder msg: 1613173622.87 epoch sec; seqNo:3173 2021-02-12T23:47:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.78 , 78.31 , 106.29 2021-02-12T23:47:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , 0.00 2021-02-12T23:47:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173622.89 epoch sec; seqNo:6829; 4057835.96 north; 591509.65 east 2021-02-12T23:47:03Z- AHRS_M2 msg: 1613173623.25 epoch sec; seqNo:6775 2021-02-12T23:47:03Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.16 psi 2021-02-12T23:47:03Z- handleDepth()-Depth_Keller msg: 1613173623265 epoch sec; seqNo:6830; depth 17.76 2021-02-12T23:47:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173623.28 epoch sec; seqNo:6830; 4057836.34 north; 591509.65 east 2021-02-12T23:47:03Z- AHRS_M2 msg: 1613173623.65 epoch sec; seqNo:6776 2021-02-12T23:47:03Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.16 psi 2021-02-12T23:47:03Z- handleDepth()-Depth_Keller msg: 1613173623673 epoch sec; seqNo:6831; depth 17.88 2021-02-12T23:47:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173623.68 epoch sec; seqNo:6831; 4057836.72 north; 591509.64 east 2021-02-12T23:47:03Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 130 calls... 2021-02-12T23:47:03Z- waiting for 1613173621.42, time is 1613173623.68 2021-02-12T23:47:03Z- LcmTrn::updateTrn() >>>> pose time 1613173623.68, meas time 1613173622.87 <<<< 2021-02-12T23:47:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173622.87 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:47:03Z- TerrainNav::motionUpdate() - currEstimate = (4057837, 591509.64, 17.88) 2021-02-12T23:47:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:47:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:47:03Z- TNavBF::Projecting Measurements 2021-02-12T23:47:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:47:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:47:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:47:03Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:47:03Z- TNavFilter::3 good beams. 2021-02-12T23:47:03Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:47:03Z- TNavBF:: beamIndex[0] = 1 2021-02-12T23:47:03Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:47:03Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:47:03Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:47:03Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:47:03Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:47:03Z- TNavBF::mmse selected from filter -1 2021-02-12T23:47:03Z- TerrainMapOctree: (vehN, vehE ) = (4057836.13, 591509.46). 2021-02-12T23:47:03Z- TerrainMapOctree: Min Distance = 4377.79. 2021-02-12T23:47:03Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:47:03Z- There is no specific good beam that all particles have in common. 2021-02-12T23:47:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:47:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-12T23:47:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:47:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 9.81 2021-02-12T23:47:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:47:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 18.08 2021-02-12T23:47:03Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:47:03Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:47:03Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:47:03Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:47:03Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:47:03Z- Filter 0 No beams used in subcloud update 2021-02-12T23:47:03Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:47:03Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:47:03Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 737.3 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:47:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173622.87 sec. 2021-02-12T23:47:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:47:03Z- TNavBF::mmse selected from filter -1 2021-02-12T23:47:03Z- variances([0], [2], and psi_berg) = 737.309991 833.022134 0.000000 2021-02-12T23:47:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:47:03Z- LcmTrn::publishEstimates() - MLE : 4057827.22 591471.14 17.88 2021-02-12T23:47:03Z- LcmTrn::publishEstimates() - MMSE : 4057836.72 591509.64 17.88 2021-02-12T23:47:03Z- LcmTrn::publishEstimates() - COVAR: 737.31 833.02 1.00 0.00 2021-02-12T23:47:04Z- AHRS_M2 msg: 1613173624.06 epoch sec; seqNo:6777 2021-02-12T23:47:04Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.16 psi 2021-02-12T23:47:04Z- handleDepth()-Depth_Keller msg: 1613173624064 epoch sec; seqNo:6832; depth 18.00 2021-02-12T23:47:04Z- handleDvl() - RDI_Pathfinder msg: 1613173624.08 epoch sec; seqNo:3174 2021-02-12T23:47:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.50 , 72.32 , 106.97 2021-02-12T23:47:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.03 2021-02-12T23:47:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173624.09 epoch sec; seqNo:6832; 4057837.10 north; 591509.64 east 2021-02-12T23:47:04Z- AHRS_M2 msg: 1613173624.47 epoch sec; seqNo:6778 2021-02-12T23:47:04Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.16 psi 2021-02-12T23:47:04Z- handleDepth()-Depth_Keller msg: 1613173624474 epoch sec; seqNo:6833; depth 18.11 2021-02-12T23:47:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173624.50 epoch sec; seqNo:6833; 4057837.11 north; 591510.86 east 2021-02-12T23:47:04Z- handleDepth()-Depth_Keller msg: 1613173624889 epoch sec; seqNo:6834; depth 18.25 2021-02-12T23:47:04Z- handleDvl() - RDI_Pathfinder msg: 1613173624.90 epoch sec; seqNo:3175 2021-02-12T23:47:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.71 , 71.51 , 107.18 2021-02-12T23:47:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.11 , -0.05 2021-02-12T23:47:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173624.92 epoch sec; seqNo:6834; 4057837.49 north; 591510.86 east 2021-02-12T23:47:05Z- AHRS_M2 msg: 1613173625.27 epoch sec; seqNo:6779 2021-02-12T23:47:05Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.17 psi 2021-02-12T23:47:05Z- handleDepth()-Depth_Keller msg: 1613173625279 epoch sec; seqNo:6835; depth 18.38 2021-02-12T23:47:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173625.29 epoch sec; seqNo:6835; 4057837.87 north; 591510.85 east 2021-02-12T23:47:05Z- AHRS_M2 msg: 1613173625.68 epoch sec; seqNo:6780 2021-02-12T23:47:05Z- AHRS_M2 msg: 0.69 phi; -0.33 theta; -0.17 psi 2021-02-12T23:47:05Z- handleDepth()-Depth_Keller msg: 1613173625680 epoch sec; seqNo:6836; depth 18.48 2021-02-12T23:47:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173625.70 epoch sec; seqNo:6836; 4057838.25 north; 591510.85 east 2021-02-12T23:47:06Z- AHRS_M2 msg: 1613173626.08 epoch sec; seqNo:6781 2021-02-12T23:47:06Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.18 psi 2021-02-12T23:47:06Z- handleDepth()-Depth_Keller msg: 1613173626085 epoch sec; seqNo:6837; depth 18.61 2021-02-12T23:47:06Z- handleDvl() - RDI_Pathfinder msg: 1613173626.10 epoch sec; seqNo:3176 2021-02-12T23:47:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.14 , 97.71 , 78.79 , 108.53 2021-02-12T23:47:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.10 , -0.03 2021-02-12T23:47:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173626.12 epoch sec; seqNo:6837; 4057838.25 north; 591510.85 east 2021-02-12T23:47:06Z- AHRS_M2 msg: 1613173626.48 epoch sec; seqNo:6782 2021-02-12T23:47:06Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.18 psi 2021-02-12T23:47:06Z- handleDepth()-Depth_Keller msg: 1613173626488 epoch sec; seqNo:6838; depth 18.74 2021-02-12T23:47:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173626.50 epoch sec; seqNo:6838; 4057838.63 north; 591510.84 east 2021-02-12T23:47:06Z- AHRS_M2 msg: 1613173626.90 epoch sec; seqNo:6783 2021-02-12T23:47:06Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.18 psi 2021-02-12T23:47:06Z- handleDepth()-Depth_Keller msg: 1613173626913 epoch sec; seqNo:6839; depth 18.85 2021-02-12T23:47:06Z- handleDvl() - RDI_Pathfinder msg: 1613173626.93 epoch sec; seqNo:3177 2021-02-12T23:47:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.18 , 97.65 , 78.18 , 103.73 2021-02-12T23:47:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , 0.03 2021-02-12T23:47:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173626.95 epoch sec; seqNo:6839; 4057839.02 north; 591512.06 east 2021-02-12T23:47:07Z- AHRS_M2 msg: 1613173627.29 epoch sec; seqNo:6784 2021-02-12T23:47:07Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.19 psi 2021-02-12T23:47:07Z- handleDepth()-Depth_Keller msg: 1613173627301 epoch sec; seqNo:6840; depth 19.00 2021-02-12T23:47:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173627.33 epoch sec; seqNo:6840; 4057839.02 north; 591512.06 east 2021-02-12T23:47:07Z- AHRS_M2 msg: 1613173627.70 epoch sec; seqNo:6785 2021-02-12T23:47:07Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.19 psi 2021-02-12T23:47:07Z- handleDepth()-Depth_Keller msg: 1613173627705 epoch sec; seqNo:6841; depth 19.11 2021-02-12T23:47:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173627.71 epoch sec; seqNo:6841; 4057839.40 north; 591512.06 east 2021-02-12T23:47:08Z- AHRS_M2 msg: 1613173628.10 epoch sec; seqNo:6786 2021-02-12T23:47:08Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.19 psi 2021-02-12T23:47:08Z- handleDepth()-Depth_Keller msg: 1613173628105 epoch sec; seqNo:6842; depth 19.23 2021-02-12T23:47:08Z- handleDvl() - RDI_Pathfinder msg: 1613173628.12 epoch sec; seqNo:3178 2021-02-12T23:47:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:47:08Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:47:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173628.13 epoch sec; seqNo:6842; 4057839.77 north; 591512.05 east 2021-02-12T23:47:08Z- AHRS_M2 msg: 1613173628.50 epoch sec; seqNo:6787 2021-02-12T23:47:08Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.19 psi 2021-02-12T23:47:08Z- handleDepth()-Depth_Keller msg: 1613173628509 epoch sec; seqNo:6843; depth 19.37 2021-02-12T23:47:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173628.53 epoch sec; seqNo:6843; 4057839.77 north; 591512.05 east 2021-02-12T23:47:08Z- LcmTrn::handleMessages() - handling 40 messages took 0.62 ms and 128 calls... 2021-02-12T23:47:08Z- waiting for 1613173626.68, time is 1613173628.53 2021-02-12T23:47:08Z- LcmTrn::updateTrn() >>>> pose time 1613173628.53, meas time 1613173628.12 <<<< 2021-02-12T23:47:08Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173628.12, ping # 0. 2021-02-12T23:47:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173628.12 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:47:08Z- TerrainNav::motionUpdate() - currEstimate = (4057840, 591512.05, 19.37) 2021-02-12T23:47:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:47:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:47:08Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:47:08Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:47:08Z- TNavBF::Projecting Measurements 2021-02-12T23:47:08Z- TNavFilter::Measurement from time = 1613173628.12 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:47:08Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:47:08Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:47:08Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:47:08Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:47:08Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:47:08Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:47:08Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:47:08Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 737.2 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:47:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:47:08Z- TNavBF::mmse selected from filter -1 2021-02-12T23:47:08Z- variances([0], [2], and psi_berg) = 737.208191 833.022053 0.000000 2021-02-12T23:47:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:47:08Z- LcmTrn::publishEstimates() - MLE : 4057830.27 591473.55 19.37 2021-02-12T23:47:08Z- LcmTrn::publishEstimates() - MMSE : 4057839.77 591512.05 19.37 2021-02-12T23:47:08Z- LcmTrn::publishEstimates() - COVAR: 737.21 833.02 1.20 0.00 2021-02-12T23:47:08Z- AHRS_M2 msg: 1613173628.91 epoch sec; seqNo:6788 2021-02-12T23:47:08Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.19 psi 2021-02-12T23:47:08Z- handleDepth()-Depth_Keller msg: 1613173628912 epoch sec; seqNo:6844; depth 19.50 2021-02-12T23:47:08Z- handleDvl() - RDI_Pathfinder msg: 1613173628.92 epoch sec; seqNo:3179 2021-02-12T23:47:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.46 , 97.03 , 71.10 , 109.20 2021-02-12T23:47:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.11 , 0.04 2021-02-12T23:47:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173628.95 epoch sec; seqNo:6844; 4057840.15 north; 591512.05 east 2021-02-12T23:47:09Z- AHRS_M2 msg: 1613173629.31 epoch sec; seqNo:6789 2021-02-12T23:47:09Z- AHRS_M2 msg: 0.70 phi; -0.35 theta; -0.19 psi 2021-02-12T23:47:09Z- handleDepth()-Depth_Keller msg: 1613173629315 epoch sec; seqNo:6845; depth 19.62 2021-02-12T23:47:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173629.33 epoch sec; seqNo:6845; 4057840.53 north; 591512.04 east 2021-02-12T23:47:09Z- AHRS_M2 msg: 1613173629.72 epoch sec; seqNo:6790 2021-02-12T23:47:09Z- AHRS_M2 msg: 0.70 phi; -0.35 theta; -0.19 psi 2021-02-12T23:47:09Z- handleDepth()-Depth_Keller msg: 1613173629721 epoch sec; seqNo:6846; depth 19.76 2021-02-12T23:47:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173629.73 epoch sec; seqNo:6846; 4057840.55 north; 591513.26 east 2021-02-12T23:47:10Z- AHRS_M2 msg: 1613173630.12 epoch sec; seqNo:6791 2021-02-12T23:47:10Z- AHRS_M2 msg: 0.70 phi; -0.35 theta; -0.19 psi 2021-02-12T23:47:10Z- handleDepth()-Depth_Keller msg: 1613173630137 epoch sec; seqNo:6847; depth 19.89 2021-02-12T23:47:10Z- handleDvl() - RDI_Pathfinder msg: 1613173630.15 epoch sec; seqNo:3180 2021-02-12T23:47:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 95.41 , 78.38 , 107.58 2021-02-12T23:47:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.11 , -0.03 2021-02-12T23:47:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173630.16 epoch sec; seqNo:6847; 4057840.92 north; 591513.26 east 2021-02-12T23:47:10Z- AHRS_M2 msg: 1613173630.52 epoch sec; seqNo:6792 2021-02-12T23:47:10Z- AHRS_M2 msg: 0.70 phi; -0.35 theta; -0.19 psi 2021-02-12T23:47:10Z- handleDepth()-Depth_Keller msg: 1613173630528 epoch sec; seqNo:6848; depth 20.02 2021-02-12T23:47:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173630.54 epoch sec; seqNo:6848; 4057840.92 north; 591513.26 east 2021-02-12T23:47:10Z- AHRS_M2 msg: 1613173630.93 epoch sec; seqNo:6793 2021-02-12T23:47:10Z- AHRS_M2 msg: 0.70 phi; -0.35 theta; -0.19 psi 2021-02-12T23:47:10Z- handleDepth()-Depth_Keller msg: 1613173630937 epoch sec; seqNo:6849; depth 20.15 2021-02-12T23:47:10Z- handleDvl() - RDI_Pathfinder msg: 1613173630.96 epoch sec; seqNo:3181 2021-02-12T23:47:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.60 , 97.58 , 67.84 , 108.39 2021-02-12T23:47:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.07 , -0.09 2021-02-12T23:47:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173630.98 epoch sec; seqNo:6849; 4057841.30 north; 591513.26 east 2021-02-12T23:47:11Z- AHRS_M2 msg: 1613173631.33 epoch sec; seqNo:6794 2021-02-12T23:47:11Z- AHRS_M2 msg: 0.70 phi; -0.35 theta; -0.20 psi 2021-02-12T23:47:11Z- handleDepth()-Depth_Keller msg: 1613173631337 epoch sec; seqNo:6850; depth 20.28 2021-02-12T23:47:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173631.35 epoch sec; seqNo:6850; 4057841.70 north; 591514.47 east 2021-02-12T23:47:11Z- AHRS_M2 msg: 1613173631.73 epoch sec; seqNo:6795 2021-02-12T23:47:11Z- AHRS_M2 msg: 0.70 phi; -0.35 theta; -0.20 psi 2021-02-12T23:47:11Z- handleDepth()-Depth_Keller msg: 1613173631740 epoch sec; seqNo:6851; depth 20.41 2021-02-12T23:47:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173631.76 epoch sec; seqNo:6851; 4057841.70 north; 591514.47 east 2021-02-12T23:47:12Z- AHRS_M2 msg: 1613173632.14 epoch sec; seqNo:6796 2021-02-12T23:47:12Z- AHRS_M2 msg: 0.70 phi; -0.35 theta; -0.21 psi 2021-02-12T23:47:12Z- handleDepth()-Depth_Keller msg: 1613173632147 epoch sec; seqNo:6852; depth 20.55 2021-02-12T23:47:12Z- handleDvl() - RDI_Pathfinder msg: 1613173632.16 epoch sec; seqNo:3182 2021-02-12T23:47:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 70.29 , 108.93 2021-02-12T23:47:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , -0.10 2021-02-12T23:47:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173632.18 epoch sec; seqNo:6852; 4057842.07 north; 591514.47 east 2021-02-12T23:47:12Z- AHRS_M2 msg: 1613173632.54 epoch sec; seqNo:6797 2021-02-12T23:47:12Z- AHRS_M2 msg: 0.70 phi; -0.35 theta; -0.22 psi 2021-02-12T23:47:12Z- handleDepth()-Depth_Keller msg: 1613173632547 epoch sec; seqNo:6853; depth 20.69 2021-02-12T23:47:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173632.56 epoch sec; seqNo:6853; 4057842.45 north; 591514.47 east 2021-02-12T23:47:12Z- AHRS_M2 msg: 1613173632.95 epoch sec; seqNo:6798 2021-02-12T23:47:12Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.22 psi 2021-02-12T23:47:12Z- handleDepth()-Depth_Keller msg: 1613173632956 epoch sec; seqNo:6854; depth 20.81 2021-02-12T23:47:12Z- handleDvl() - RDI_Pathfinder msg: 1613173632.97 epoch sec; seqNo:3183 2021-02-12T23:47:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 97.85 , 69.47 , 108.80 2021-02-12T23:47:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , -0.07 2021-02-12T23:47:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173632.98 epoch sec; seqNo:6854; 4057842.83 north; 591514.46 east 2021-02-12T23:47:13Z- AHRS_M2 msg: 1613173633.36 epoch sec; seqNo:6799 2021-02-12T23:47:13Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.23 psi 2021-02-12T23:47:13Z- handleDepth()-Depth_Keller msg: 1613173633362 epoch sec; seqNo:6855; depth 20.95 2021-02-12T23:47:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173633.37 epoch sec; seqNo:6855; 4057843.21 north; 591514.46 east 2021-02-12T23:47:13Z- AHRS_M2 msg: 1613173633.76 epoch sec; seqNo:6800 2021-02-12T23:47:13Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.23 psi 2021-02-12T23:47:13Z- handleDepth()-Depth_Keller msg: 1613173633769 epoch sec; seqNo:6856; depth 21.08 2021-02-12T23:47:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173633.78 epoch sec; seqNo:6856; 4057843.22 north; 591515.68 east 2021-02-12T23:47:13Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-12T23:47:13Z- waiting for 1613173631.53, time is 1613173633.78 2021-02-12T23:47:13Z- LcmTrn::updateTrn() >>>> pose time 1613173633.78, meas time 1613173632.97 <<<< 2021-02-12T23:47:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173632.97 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:47:13Z- TerrainNav::motionUpdate() - currEstimate = (4057843, 591515.68, 21.08) 2021-02-12T23:47:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:47:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:47:13Z- TNavBF::Projecting Measurements 2021-02-12T23:47:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:47:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:47:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:47:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:47:13Z- TNavFilter::4 good beams. 2021-02-12T23:47:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:47:13Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:47:13Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:47:13Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:47:13Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:47:13Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:47:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:47:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:47:13Z- TerrainMapOctree: (vehN, vehE ) = (4057842.69, 591515.12). 2021-02-12T23:47:13Z- TerrainMapOctree: Min Distance = 4369.13. 2021-02-12T23:47:13Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:47:13Z- There is no specific good beam that all particles have in common. 2021-02-12T23:47:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:47:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.41 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 9.83 2021-02-12T23:47:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:47:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-12T23:47:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:47:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.110 ExpectedMeasDiff: nan currMeas.covariance: 7.72 2021-02-12T23:47:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:47:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 18.94 2021-02-12T23:47:13Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:47:13Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:47:13Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:47:13Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:47:13Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:47:13Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:47:13Z- Filter 0 No beams used in subcloud update 2021-02-12T23:47:13Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:47:13Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:47:13Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 737.1 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:47:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173632.97 sec. 2021-02-12T23:47:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:47:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:47:13Z- variances([0], [2], and psi_berg) = 737.086948 833.021957 0.000000 2021-02-12T23:47:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:47:13Z- LcmTrn::publishEstimates() - MLE : 4057834.72 591476.18 21.08 2021-02-12T23:47:13Z- LcmTrn::publishEstimates() - MMSE : 4057843.22 591515.68 21.08 2021-02-12T23:47:13Z- LcmTrn::publishEstimates() - COVAR: 737.09 833.02 1.03 0.00 2021-02-12T23:47:14Z- AHRS_M2 msg: 1613173634.16 epoch sec; seqNo:6801 2021-02-12T23:47:14Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.23 psi 2021-02-12T23:47:14Z- handleDepth()-Depth_Keller msg: 1613173634164 epoch sec; seqNo:6857; depth 21.20 2021-02-12T23:47:14Z- handleDvl() - RDI_Pathfinder msg: 1613173634.18 epoch sec; seqNo:3184 2021-02-12T23:47:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 97.85 , 67.43 , 108.80 2021-02-12T23:47:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.10 , 0.00 2021-02-12T23:47:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173634.19 epoch sec; seqNo:6857; 4057843.60 north; 591515.67 east 2021-02-12T23:47:14Z- AHRS_M2 msg: 1613173634.56 epoch sec; seqNo:6802 2021-02-12T23:47:14Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.23 psi 2021-02-12T23:47:14Z- handleDepth()-Depth_Keller msg: 1613173634573 epoch sec; seqNo:6858; depth 21.33 2021-02-12T23:47:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173634.58 epoch sec; seqNo:6858; 4057843.98 north; 591515.67 east 2021-02-12T23:47:14Z- AHRS_M2 msg: 1613173634.97 epoch sec; seqNo:6803 2021-02-12T23:47:14Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.23 psi 2021-02-12T23:47:14Z- handleDepth()-Depth_Keller msg: 1613173634973 epoch sec; seqNo:6859; depth 21.47 2021-02-12T23:47:14Z- handleDvl() - RDI_Pathfinder msg: 1613173634.98 epoch sec; seqNo:3185 2021-02-12T23:47:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.92 , 69.47 , 107.65 2021-02-12T23:47:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.03 2021-02-12T23:47:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173634.99 epoch sec; seqNo:6859; 4057843.98 north; 591515.67 east 2021-02-12T23:47:15Z- AHRS_M2 msg: 1613173635.37 epoch sec; seqNo:6804 2021-02-12T23:47:15Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.23 psi 2021-02-12T23:47:15Z- handleDepth()-Depth_Keller msg: 1613173635378 epoch sec; seqNo:6860; depth 21.59 2021-02-12T23:47:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173635.39 epoch sec; seqNo:6860; 4057844.36 north; 591515.67 east 2021-02-12T23:47:15Z- AHRS_M2 msg: 1613173635.78 epoch sec; seqNo:6805 2021-02-12T23:47:15Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.23 psi 2021-02-12T23:47:15Z- handleDepth()-Depth_Keller msg: 1613173635785 epoch sec; seqNo:6861; depth 21.72 2021-02-12T23:47:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173635.81 epoch sec; seqNo:6861; 4057844.74 north; 591515.66 east 2021-02-12T23:47:16Z- AHRS_M2 msg: 1613173636.18 epoch sec; seqNo:6806 2021-02-12T23:47:16Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.23 psi 2021-02-12T23:47:16Z- handleDepth()-Depth_Keller msg: 1613173636184 epoch sec; seqNo:6862; depth 21.85 2021-02-12T23:47:16Z- handleDvl() - RDI_Pathfinder msg: 1613173636.20 epoch sec; seqNo:3186 2021-02-12T23:47:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 97.65 , 78.18 , 111.03 2021-02-12T23:47:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , 0.01 2021-02-12T23:47:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173636.21 epoch sec; seqNo:6862; 4057844.74 north; 591515.66 east 2021-02-12T23:47:16Z- AHRS_M2 msg: 1613173636.58 epoch sec; seqNo:6807 2021-02-12T23:47:16Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.23 psi 2021-02-12T23:47:16Z- handleDepth()-Depth_Keller msg: 1613173636593 epoch sec; seqNo:6863; depth 21.98 2021-02-12T23:47:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173636.60 epoch sec; seqNo:6863; 4057845.13 north; 591516.88 east 2021-02-12T23:47:16Z- AHRS_M2 msg: 1613173636.99 epoch sec; seqNo:6808 2021-02-12T23:47:16Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.23 psi 2021-02-12T23:47:17Z- handleDepth()-Depth_Keller msg: 1613173637001 epoch sec; seqNo:6864; depth 22.11 2021-02-12T23:47:17Z- handleDvl() - RDI_Pathfinder msg: 1613173637.01 epoch sec; seqNo:3187 2021-02-12T23:47:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 97.71 , 97.71 , 106.22 2021-02-12T23:47:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , 0.02 , 0.01 2021-02-12T23:47:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173637.03 epoch sec; seqNo:6864; 4057845.51 north; 591516.88 east 2021-02-12T23:47:17Z- AHRS_M2 msg: 1613173637.39 epoch sec; seqNo:6809 2021-02-12T23:47:17Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.23 psi 2021-02-12T23:47:17Z- handleDepth()-Depth_Keller msg: 1613173637401 epoch sec; seqNo:6865; depth 22.24 2021-02-12T23:47:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173637.41 epoch sec; seqNo:6865; 4057845.51 north; 591516.88 east 2021-02-12T23:47:17Z- AHRS_M2 msg: 1613173637.80 epoch sec; seqNo:6810 2021-02-12T23:47:17Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.24 psi 2021-02-12T23:47:17Z- handleDepth()-Depth_Keller msg: 1613173637800 epoch sec; seqNo:6866; depth 22.37 2021-02-12T23:47:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173637.81 epoch sec; seqNo:6866; 4057845.89 north; 591516.87 east 2021-02-12T23:47:18Z- AHRS_M2 msg: 1613173638.20 epoch sec; seqNo:6811 2021-02-12T23:47:18Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.24 psi 2021-02-12T23:47:18Z- handleDepth()-Depth_Keller msg: 1613173638213 epoch sec; seqNo:6867; depth 22.49 2021-02-12T23:47:18Z- handleDvl() - RDI_Pathfinder msg: 1613173638.23 epoch sec; seqNo:3188 2021-02-12T23:47:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 97.57 , 78.65 , 104.33 2021-02-12T23:47:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , -0.08 2021-02-12T23:47:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173638.24 epoch sec; seqNo:6867; 4057846.27 north; 591516.87 east 2021-02-12T23:47:18Z- AHRS_M2 msg: 1613173638.60 epoch sec; seqNo:6812 2021-02-12T23:47:18Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.23 psi 2021-02-12T23:47:18Z- handleDepth()-Depth_Keller msg: 1613173638608 epoch sec; seqNo:6868; depth 22.64 2021-02-12T23:47:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173638.62 epoch sec; seqNo:6868; 4057846.27 north; 591516.87 east 2021-02-12T23:47:18Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-12T23:47:18Z- waiting for 1613173636.78, time is 1613173638.62 2021-02-12T23:47:18Z- LcmTrn::updateTrn() >>>> pose time 1613173638.62, meas time 1613173638.23 <<<< 2021-02-12T23:47:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173638.23 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:47:18Z- TerrainNav::motionUpdate() - currEstimate = (4057846, 591516.87, 22.64) 2021-02-12T23:47:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:47:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:47:18Z- TNavBF::Projecting Measurements 2021-02-12T23:47:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:47:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:47:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:47:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:47:18Z- TNavFilter::4 good beams. 2021-02-12T23:47:18Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:47:18Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:47:18Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:47:18Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:47:18Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:47:18Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:47:18Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:47:18Z- TNavBF::mmse selected from filter -1 2021-02-12T23:47:18Z- TerrainMapOctree: (vehN, vehE ) = (4057846.02, 591516.77). 2021-02-12T23:47:18Z- TerrainMapOctree: Min Distance = 4365.50. 2021-02-12T23:47:18Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:47:18Z- There is no specific good beam that all particles have in common. 2021-02-12T23:47:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:47:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 9.90 2021-02-12T23:47:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:47:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.23 2021-02-12T23:47:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:47:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 9.90 2021-02-12T23:47:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:47:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 17.42 2021-02-12T23:47:18Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:47:18Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:47:18Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:47:18Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:47:19Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:47:19Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:47:19Z- Filter 0 No beams used in subcloud update 2021-02-12T23:47:19Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:47:19Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:47:19Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 737.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:47:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173638.23 sec. 2021-02-12T23:47:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:47:19Z- TNavBF::mmse selected from filter -1 2021-02-12T23:47:19Z- variances([0], [2], and psi_berg) = 736.994338 833.021883 0.000000 2021-02-12T23:47:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:47:19Z- LcmTrn::publishEstimates() - MLE : 4057837.77 591477.37 22.64 2021-02-12T23:47:19Z- LcmTrn::publishEstimates() - MMSE : 4057846.27 591516.87 22.64 2021-02-12T23:47:19Z- LcmTrn::publishEstimates() - COVAR: 736.99 833.02 0.90 0.00 2021-02-12T23:47:19Z- AHRS_M2 msg: 1613173639.01 epoch sec; seqNo:6813 2021-02-12T23:47:19Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.23 psi 2021-02-12T23:47:19Z- handleDepth()-Depth_Keller msg: 1613173639011 epoch sec; seqNo:6869; depth 22.78 2021-02-12T23:47:19Z- handleDvl() - RDI_Pathfinder msg: 1613173639.02 epoch sec; seqNo:3189 2021-02-12T23:47:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 97.78 , 67.84 , 105.08 2021-02-12T23:47:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.14 , -0.05 2021-02-12T23:47:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173639.03 epoch sec; seqNo:6869; 4057846.65 north; 591516.86 east 2021-02-12T23:47:19Z- AHRS_M2 msg: 1613173639.41 epoch sec; seqNo:6814 2021-02-12T23:47:19Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.23 psi 2021-02-12T23:47:19Z- handleDepth()-Depth_Keller msg: 1613173639425 epoch sec; seqNo:6870; depth 22.92 2021-02-12T23:47:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173639.44 epoch sec; seqNo:6870; 4057847.04 north; 591518.08 east 2021-02-12T23:47:19Z- AHRS_M2 msg: 1613173639.84 epoch sec; seqNo:6815 2021-02-12T23:47:19Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.22 psi 2021-02-12T23:47:19Z- handleDepth()-Depth_Keller msg: 1613173639849 epoch sec; seqNo:6871; depth 23.03 2021-02-12T23:47:19Z- handleDvl() - RDI_Pathfinder msg: 1613173639.86 epoch sec; seqNo:3190 2021-02-12T23:47:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 94.33 , 68.24 , 102.85 2021-02-12T23:47:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.16 , 0.01 2021-02-12T23:47:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173639.87 epoch sec; seqNo:6871; 4057847.42 north; 591518.08 east 2021-02-12T23:47:20Z- AHRS_M2 msg: 1613173640.22 epoch sec; seqNo:6816 2021-02-12T23:47:20Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.22 psi 2021-02-12T23:47:20Z- handleDepth()-Depth_Keller msg: 1613173640225 epoch sec; seqNo:6872; depth 23.17 2021-02-12T23:47:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173640.23 epoch sec; seqNo:6872; 4057847.80 north; 591518.07 east 2021-02-12T23:47:20Z- AHRS_M2 msg: 1613173640.62 epoch sec; seqNo:6817 2021-02-12T23:47:20Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.21 psi 2021-02-12T23:47:20Z- handleDepth()-Depth_Keller msg: 1613173640632 epoch sec; seqNo:6873; depth 23.28 2021-02-12T23:47:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173640.64 epoch sec; seqNo:6873; 4057848.17 north; 591518.07 east 2021-02-12T23:47:21Z- AHRS_M2 msg: 1613173641.03 epoch sec; seqNo:6818 2021-02-12T23:47:21Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.21 psi 2021-02-12T23:47:21Z- handleDepth()-Depth_Keller msg: 1613173641032 epoch sec; seqNo:6874; depth 23.42 2021-02-12T23:47:21Z- handleDvl() - RDI_Pathfinder msg: 1613173641.05 epoch sec; seqNo:3191 2021-02-12T23:47:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.11 , 66.60 , 98.11 2021-02-12T23:47:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.15 , 0.07 2021-02-12T23:47:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173641.06 epoch sec; seqNo:6874; 4057848.17 north; 591518.07 east 2021-02-12T23:47:21Z- AHRS_M2 msg: 1613173641.43 epoch sec; seqNo:6819 2021-02-12T23:47:21Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.21 psi 2021-02-12T23:47:21Z- handleDepth()-Depth_Keller msg: 1613173641436 epoch sec; seqNo:6875; depth 23.54 2021-02-12T23:47:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173641.45 epoch sec; seqNo:6875; 4057848.55 north; 591518.06 east 2021-02-12T23:47:21Z- AHRS_M2 msg: 1613173641.84 epoch sec; seqNo:6820 2021-02-12T23:47:21Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.20 psi 2021-02-12T23:47:21Z- handleDepth()-Depth_Keller msg: 1613173641839 epoch sec; seqNo:6876; depth 23.66 2021-02-12T23:47:21Z- handleDvl() - RDI_Pathfinder msg: 1613173641.85 epoch sec; seqNo:3192 2021-02-12T23:47:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 91.90 , 66.60 , 102.85 2021-02-12T23:47:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.04 , 0.10 2021-02-12T23:47:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173641.86 epoch sec; seqNo:6876; 4057848.93 north; 591518.06 east 2021-02-12T23:47:22Z- AHRS_M2 msg: 1613173642.24 epoch sec; seqNo:6821 2021-02-12T23:47:22Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.20 psi 2021-02-12T23:47:22Z- handleDepth()-Depth_Keller msg: 1613173642244 epoch sec; seqNo:6877; depth 23.77 2021-02-12T23:47:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173642.26 epoch sec; seqNo:6877; 4057848.93 north; 591518.06 east 2021-02-12T23:47:22Z- AHRS_M2 msg: 1613173642.64 epoch sec; seqNo:6822 2021-02-12T23:47:22Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.19 psi 2021-02-12T23:47:22Z- handleDepth()-Depth_Keller msg: 1613173642648 epoch sec; seqNo:6878; depth 23.91 2021-02-12T23:47:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173642.66 epoch sec; seqNo:6878; 4057849.31 north; 591518.06 east 2021-02-12T23:47:23Z- AHRS_M2 msg: 1613173643.05 epoch sec; seqNo:6823 2021-02-12T23:47:23Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.19 psi 2021-02-12T23:47:23Z- handleDepth()-Depth_Keller msg: 1613173643057 epoch sec; seqNo:6879; depth 24.02 2021-02-12T23:47:23Z- handleDvl() - RDI_Pathfinder msg: 1613173643.08 epoch sec; seqNo:3193 2021-02-12T23:47:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:47:23Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:47:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173643.09 epoch sec; seqNo:6879; 4057849.70 north; 591519.27 east 2021-02-12T23:47:23Z- AHRS_M2 msg: 1613173643.45 epoch sec; seqNo:6824 2021-02-12T23:47:23Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.18 psi 2021-02-12T23:47:23Z- handleDepth()-Depth_Keller msg: 1613173643456 epoch sec; seqNo:6880; depth 24.16 2021-02-12T23:47:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173643.47 epoch sec; seqNo:6880; 4057849.70 north; 591519.27 east 2021-02-12T23:47:23Z- AHRS_M2 msg: 1613173643.86 epoch sec; seqNo:6825 2021-02-12T23:47:23Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-12T23:47:23Z- handleDepth()-Depth_Keller msg: 1613173643862 epoch sec; seqNo:6881; depth 24.28 2021-02-12T23:47:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173643.87 epoch sec; seqNo:6881; 4057850.08 north; 591519.27 east 2021-02-12T23:47:24Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-12T23:47:24Z- waiting for 1613173641.62, time is 1613173643.87 2021-02-12T23:47:24Z- LcmTrn::updateTrn() >>>> pose time 1613173643.87, meas time 1613173643.08 <<<< 2021-02-12T23:47:24Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173643.08, ping # 0. 2021-02-12T23:47:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173643.08 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:47:24Z- TerrainNav::motionUpdate() - currEstimate = (4057850, 591519.27, 24.28) 2021-02-12T23:47:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:47:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:47:24Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:47:24Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:47:24Z- TNavBF::Projecting Measurements 2021-02-12T23:47:24Z- TNavFilter::Measurement from time = 1613173643.08 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:47:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:47:24Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:47:24Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:47:24Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:47:24Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:47:24Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:47:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:47:24Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 736.9 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:47:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:47:24Z- TNavBF::mmse selected from filter -1 2021-02-12T23:47:24Z- variances([0], [2], and psi_berg) = 736.879728 833.021793 0.000000 2021-02-12T23:47:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:47:24Z- LcmTrn::publishEstimates() - MLE : 4057841.58 591479.77 24.28 2021-02-12T23:47:24Z- LcmTrn::publishEstimates() - MMSE : 4057850.08 591519.27 24.28 2021-02-12T23:47:24Z- LcmTrn::publishEstimates() - COVAR: 736.88 833.02 1.13 0.00 2021-02-12T23:47:24Z- AHRS_M2 msg: 1613173644.26 epoch sec; seqNo:6826 2021-02-12T23:47:24Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-12T23:47:24Z- handleDepth()-Depth_Keller msg: 1613173644264 epoch sec; seqNo:6882; depth 24.41 2021-02-12T23:47:24Z- handleDvl() - RDI_Pathfinder msg: 1613173644.28 epoch sec; seqNo:3194 2021-02-12T23:47:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 89.94 , 64.64 , 100.35 2021-02-12T23:47:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.07 , 0.05 2021-02-12T23:47:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173644.29 epoch sec; seqNo:6882; 4057850.46 north; 591519.26 east 2021-02-12T23:47:24Z- AHRS_M2 msg: 1613173644.66 epoch sec; seqNo:6827 2021-02-12T23:47:24Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.17 psi 2021-02-12T23:47:24Z- handleDepth()-Depth_Keller msg: 1613173644675 epoch sec; seqNo:6883; depth 24.51 2021-02-12T23:47:24Z- handleDvl() - RDI_Pathfinder msg: 1613173644.75 epoch sec; seqNo:3195 2021-02-12T23:47:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 0.00 , 66.19 , 100.75 2021-02-12T23:47:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.06 , 0.01 2021-02-12T23:47:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173644.76 epoch sec; seqNo:6883; 4057850.46 north; 591519.26 east 2021-02-12T23:47:25Z- AHRS_M2 msg: 1613173645.07 epoch sec; seqNo:6828 2021-02-12T23:47:25Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.16 psi 2021-02-12T23:47:25Z- handleDepth()-Depth_Keller msg: 1613173645074 epoch sec; seqNo:6884; depth 24.66 2021-02-12T23:47:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173645.09 epoch sec; seqNo:6884; 4057850.84 north; 591519.26 east 2021-02-12T23:47:25Z- AHRS_M2 msg: 1613173645.47 epoch sec; seqNo:6829 2021-02-12T23:47:25Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.16 psi 2021-02-12T23:47:25Z- handleDepth()-Depth_Keller msg: 1613173645476 epoch sec; seqNo:6885; depth 24.76 2021-02-12T23:47:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173645.50 epoch sec; seqNo:6885; 4057851.22 north; 591519.26 east 2021-02-12T23:47:25Z- AHRS_M2 msg: 1613173645.88 epoch sec; seqNo:6830 2021-02-12T23:47:25Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.16 psi 2021-02-12T23:47:25Z- handleDepth()-Depth_Keller msg: 1613173645885 epoch sec; seqNo:6886; depth 24.88 2021-02-12T23:47:25Z- handleDvl() - RDI_Pathfinder msg: 1613173645.91 epoch sec; seqNo:3196 2021-02-12T23:47:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.14 , 157.25 , 78.31 , 99.94 2021-02-12T23:47:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.49 , -0.50 , 0.26 2021-02-12T23:47:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173645.92 epoch sec; seqNo:6886; 4057851.22 north; 591519.26 east 2021-02-12T23:47:26Z- AHRS_M2 msg: 1613173646.28 epoch sec; seqNo:6831 2021-02-12T23:47:26Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.15 psi 2021-02-12T23:47:26Z- handleDepth()-Depth_Keller msg: 1613173646289 epoch sec; seqNo:6887; depth 25.01 2021-02-12T23:47:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173646.30 epoch sec; seqNo:6887; 4057851.60 north; 591519.25 east 2021-02-12T23:47:26Z- AHRS_M2 msg: 1613173646.68 epoch sec; seqNo:6832 2021-02-12T23:47:26Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.15 psi 2021-02-12T23:47:26Z- handleDepth()-Depth_Keller msg: 1613173646697 epoch sec; seqNo:6888; depth 25.14 2021-02-12T23:47:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173646.70 epoch sec; seqNo:6888; 4057851.60 north; 591519.25 east 2021-02-12T23:47:27Z- handleDepth()-Depth_Keller msg: 1613173647113 epoch sec; seqNo:6889; depth 25.24 2021-02-12T23:47:27Z- handleDvl() - RDI_Pathfinder msg: 1613173647.13 epoch sec; seqNo:3197 2021-02-12T23:47:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 0.00 , 64.97 , 99.40 2021-02-12T23:47:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , -0.03 2021-02-12T23:47:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173647.14 epoch sec; seqNo:6889; 4057851.99 north; 591520.47 east 2021-02-12T23:47:27Z- AHRS_M2 msg: 1613173647.49 epoch sec; seqNo:6833 2021-02-12T23:47:27Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.15 psi 2021-02-12T23:47:27Z- handleDepth()-Depth_Keller msg: 1613173647497 epoch sec; seqNo:6890; depth 25.39 2021-02-12T23:47:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173647.51 epoch sec; seqNo:6890; 4057852.37 north; 591520.46 east 2021-02-12T23:47:27Z- AHRS_M2 msg: 1613173647.89 epoch sec; seqNo:6834 2021-02-12T23:47:27Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.15 psi 2021-02-12T23:47:27Z- handleDepth()-Depth_Keller msg: 1613173647901 epoch sec; seqNo:6891; depth 25.49 2021-02-12T23:47:27Z- handleDvl() - RDI_Pathfinder msg: 1613173647.91 epoch sec; seqNo:3198 2021-02-12T23:47:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 0.00 , 64.57 , 98.93 2021-02-12T23:47:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , -0.04 2021-02-12T23:47:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173647.92 epoch sec; seqNo:6891; 4057852.75 north; 591520.46 east 2021-02-12T23:47:28Z- AHRS_M2 msg: 1613173648.30 epoch sec; seqNo:6835 2021-02-12T23:47:28Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.16 psi 2021-02-12T23:47:28Z- handleDepth()-Depth_Keller msg: 1613173648305 epoch sec; seqNo:6892; depth 25.63 2021-02-12T23:47:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173648.32 epoch sec; seqNo:6892; 4057852.75 north; 591520.46 east 2021-02-12T23:47:28Z- AHRS_M2 msg: 1613173648.72 epoch sec; seqNo:6836 2021-02-12T23:47:28Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.16 psi 2021-02-12T23:47:28Z- handleDepth()-Depth_Keller msg: 1613173648728 epoch sec; seqNo:6893; depth 25.74 2021-02-12T23:47:28Z- handleDvl() - RDI_Pathfinder msg: 1613173648.76 epoch sec; seqNo:3199 2021-02-12T23:47:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 87.17 , 63.75 , 99.07 2021-02-12T23:47:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.00 , -0.09 2021-02-12T23:47:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173648.78 epoch sec; seqNo:6893; 4057853.13 north; 591520.46 east 2021-02-12T23:47:29Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-12T23:47:29Z- waiting for 1613173646.87, time is 1613173648.78 2021-02-12T23:47:29Z- LcmTrn::updateTrn() equating pose 1613173648.78 and meas times 1613173648.78 2021-02-12T23:47:29Z- LcmTrn::updateTrn() >>>> pose time 1613173648.78, meas time 1613173648.78 <<<< 2021-02-12T23:47:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173648.78 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:47:29Z- TerrainNav::motionUpdate() - currEstimate = (4057853, 591520.46, 25.74) 2021-02-12T23:47:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:47:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:47:29Z- TNavBF::Projecting Measurements 2021-02-12T23:47:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:47:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:47:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:47:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:47:29Z- TNavFilter::4 good beams. 2021-02-12T23:47:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:47:29Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:47:29Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:47:29Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:47:29Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:47:29Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:47:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:47:29Z- TNavBF::mmse selected from filter -1 2021-02-12T23:47:29Z- TerrainMapOctree: (vehN, vehE ) = (4057853.13, 591520.46). 2021-02-12T23:47:29Z- TerrainMapOctree: Min Distance = 4357.66. 2021-02-12T23:47:29Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:47:29Z- There is no specific good beam that all particles have in common. 2021-02-12T23:47:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:47:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.117 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-12T23:47:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:47:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 12.16 2021-02-12T23:47:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:47:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.114 ExpectedMeasDiff: nan currMeas.covariance: 6.50 2021-02-12T23:47:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:47:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 15.70 2021-02-12T23:47:29Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:47:29Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:47:29Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:47:29Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:47:29Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:47:29Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:47:29Z- Filter 0 No beams used in subcloud update 2021-02-12T23:47:29Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:47:29Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:47:29Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 736.8 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:47:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173648.78 sec. 2021-02-12T23:47:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:47:29Z- TNavBF::mmse selected from filter -1 2021-02-12T23:47:29Z- variances([0], [2], and psi_berg) = 736.805316 833.021734 0.000000 2021-02-12T23:47:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:47:29Z- LcmTrn::publishEstimates() - MLE : 4057844.63 591480.96 25.74 2021-02-12T23:47:29Z- LcmTrn::publishEstimates() - MMSE : 4057853.13 591520.46 25.74 2021-02-12T23:47:29Z- LcmTrn::publishEstimates() - COVAR: 736.81 833.02 0.89 0.00 2021-02-12T23:47:29Z- AHRS_M2 msg: 1613173649.11 epoch sec; seqNo:6837 2021-02-12T23:47:29Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.17 psi 2021-02-12T23:47:29Z- handleDepth()-Depth_Keller msg: 1613173649113 epoch sec; seqNo:6894; depth 25.90 2021-02-12T23:47:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173649.13 epoch sec; seqNo:6894; 4057853.50 north; 591520.45 east 2021-02-12T23:47:29Z- AHRS_M2 msg: 1613173649.51 epoch sec; seqNo:6838 2021-02-12T23:47:29Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.17 psi 2021-02-12T23:47:29Z- handleDepth()-Depth_Keller msg: 1613173649521 epoch sec; seqNo:6895; depth 26.00 2021-02-12T23:47:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173649.53 epoch sec; seqNo:6895; 4057853.90 north; 591521.67 east 2021-02-12T23:47:29Z- AHRS_M2 msg: 1613173649.92 epoch sec; seqNo:6839 2021-02-12T23:47:29Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-12T23:47:29Z- handleDepth()-Depth_Keller msg: 1613173649921 epoch sec; seqNo:6896; depth 26.12 2021-02-12T23:47:29Z- handleDvl() - RDI_Pathfinder msg: 1613173649.94 epoch sec; seqNo:3200 2021-02-12T23:47:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:47:29Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:47:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173649.95 epoch sec; seqNo:6896; 4057854.28 north; 591521.67 east 2021-02-12T23:47:30Z- AHRS_M2 msg: 1613173650.32 epoch sec; seqNo:6840 2021-02-12T23:47:30Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.18 psi 2021-02-12T23:47:30Z- handleDepth()-Depth_Keller msg: 1613173650324 epoch sec; seqNo:6897; depth 26.26 2021-02-12T23:47:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173650.35 epoch sec; seqNo:6897; 4057854.28 north; 591521.67 east 2021-02-12T23:47:30Z- AHRS_M2 msg: 1613173650.73 epoch sec; seqNo:6841 2021-02-12T23:47:30Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.18 psi 2021-02-12T23:47:30Z- handleDepth()-Depth_Keller msg: 1613173650741 epoch sec; seqNo:6898; depth 26.38 2021-02-12T23:47:30Z- handleDvl() - RDI_Pathfinder msg: 1613173650.77 epoch sec; seqNo:3201 2021-02-12T23:47:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 0.00 , 64.16 , 97.65 2021-02-12T23:47:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.14 , -0.12 2021-02-12T23:47:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173650.78 epoch sec; seqNo:6898; 4057854.65 north; 591521.66 east 2021-02-12T23:47:31Z- AHRS_M2 msg: 1613173651.13 epoch sec; seqNo:6842 2021-02-12T23:47:31Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-12T23:47:31Z- handleDepth()-Depth_Keller msg: 1613173651141 epoch sec; seqNo:6899; depth 26.51 2021-02-12T23:47:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173651.15 epoch sec; seqNo:6899; 4057855.03 north; 591521.66 east 2021-02-12T23:47:31Z- AHRS_M2 msg: 1613173651.54 epoch sec; seqNo:6843 2021-02-12T23:47:31Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-12T23:47:31Z- handleDepth()-Depth_Keller msg: 1613173651554 epoch sec; seqNo:6900; depth 26.64 2021-02-12T23:47:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173651.57 epoch sec; seqNo:6900; 4057855.41 north; 591521.65 east 2021-02-12T23:47:31Z- AHRS_M2 msg: 1613173651.93 epoch sec; seqNo:6844 2021-02-12T23:47:31Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-12T23:47:31Z- handleDepth()-Depth_Keller msg: 1613173651941 epoch sec; seqNo:6901; depth 26.77 2021-02-12T23:47:31Z- handleDvl() - RDI_Pathfinder msg: 1613173651.95 epoch sec; seqNo:3202 2021-02-12T23:47:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 88.25 , 63.34 , 97.98 2021-02-12T23:47:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.14 , -0.06 2021-02-12T23:47:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173651.97 epoch sec; seqNo:6901; 4057855.80 north; 591522.87 east 2021-02-12T23:47:32Z- AHRS_M2 msg: 1613173652.34 epoch sec; seqNo:6845 2021-02-12T23:47:32Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-12T23:47:32Z- handleDepth()-Depth_Keller msg: 1613173652344 epoch sec; seqNo:6902; depth 26.90 2021-02-12T23:47:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173652.36 epoch sec; seqNo:6902; 4057856.18 north; 591522.87 east 2021-02-12T23:47:32Z- AHRS_M2 msg: 1613173652.76 epoch sec; seqNo:6846 2021-02-12T23:47:32Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.18 psi 2021-02-12T23:47:32Z- handleDepth()-Depth_Keller msg: 1613173652765 epoch sec; seqNo:6903; depth 27.02 2021-02-12T23:47:32Z- handleDvl() - RDI_Pathfinder msg: 1613173652.78 epoch sec; seqNo:3203 2021-02-12T23:47:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 86.09 , 63.34 , 97.98 2021-02-12T23:47:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.20 , 0.01 2021-02-12T23:47:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173652.79 epoch sec; seqNo:6903; 4057856.18 north; 591522.87 east 2021-02-12T23:47:33Z- AHRS_M2 msg: 1613173653.15 epoch sec; seqNo:6847 2021-02-12T23:47:33Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.18 psi 2021-02-12T23:47:33Z- handleDepth()-Depth_Keller msg: 1613173653154 epoch sec; seqNo:6904; depth 27.14 2021-02-12T23:47:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173653.18 epoch sec; seqNo:6904; 4057856.56 north; 591522.86 east 2021-02-12T23:47:33Z- AHRS_M2 msg: 1613173653.55 epoch sec; seqNo:6848 2021-02-12T23:47:33Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.18 psi 2021-02-12T23:47:33Z- handleDepth()-Depth_Keller msg: 1613173653556 epoch sec; seqNo:6905; depth 27.27 2021-02-12T23:47:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173653.56 epoch sec; seqNo:6905; 4057856.94 north; 591522.86 east 2021-02-12T23:47:33Z- AHRS_M2 msg: 1613173653.96 epoch sec; seqNo:6849 2021-02-12T23:47:33Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.18 psi 2021-02-12T23:47:33Z- handleDepth()-Depth_Keller msg: 1613173653961 epoch sec; seqNo:6906; depth 27.40 2021-02-12T23:47:33Z- handleDvl() - RDI_Pathfinder msg: 1613173653.98 epoch sec; seqNo:3204 2021-02-12T23:47:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 86.09 , 62.93 , 97.98 2021-02-12T23:47:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.11 , 0.12 2021-02-12T23:47:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173653.99 epoch sec; seqNo:6906; 4057857.32 north; 591522.85 east 2021-02-12T23:47:34Z- LcmTrn::handleMessages() - handling 44 messages took 0.62 ms and 132 calls... 2021-02-12T23:47:34Z- waiting for 1613173651.78, time is 1613173653.99 2021-02-12T23:47:34Z- LcmTrn::updateTrn() equating pose 1613173653.99 and meas times 1613173653.99 2021-02-12T23:47:34Z- LcmTrn::updateTrn() >>>> pose time 1613173653.99, meas time 1613173653.99 <<<< 2021-02-12T23:47:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173653.99 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:47:34Z- TerrainNav::motionUpdate() - currEstimate = (4057857, 591522.85, 27.40) 2021-02-12T23:47:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:47:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:47:34Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173653.99 sec.; No relevant navigation data available 2021-02-12T23:47:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:47:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:47:34Z- variances([0], [2], and psi_berg) = 736.714927 833.021662 0.000000 2021-02-12T23:47:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:47:34Z- LcmTrn::publishEstimates() - MLE : 4057847.82 591484.35 27.40 2021-02-12T23:47:34Z- LcmTrn::publishEstimates() - MMSE : 4057857.32 591522.85 27.40 2021-02-12T23:47:34Z- LcmTrn::publishEstimates() - COVAR: 736.71 833.02 1.14 0.00 2021-02-12T23:47:34Z- AHRS_M2 msg: 1613173654.36 epoch sec; seqNo:6850 2021-02-12T23:47:34Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.18 psi 2021-02-12T23:47:34Z- handleDepth()-Depth_Keller msg: 1613173654369 epoch sec; seqNo:6907; depth 27.54 2021-02-12T23:47:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173654.38 epoch sec; seqNo:6907; 4057857.32 north; 591522.85 east 2021-02-12T23:47:34Z- AHRS_M2 msg: 1613173654.78 epoch sec; seqNo:6851 2021-02-12T23:47:34Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.19 psi 2021-02-12T23:47:34Z- handleDepth()-Depth_Keller msg: 1613173654779 epoch sec; seqNo:6908; depth 27.67 2021-02-12T23:47:34Z- handleDvl() - RDI_Pathfinder msg: 1613173654.79 epoch sec; seqNo:3205 2021-02-12T23:47:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 0.00 , 63.34 , 97.98 2021-02-12T23:47:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.01 , 0.17 2021-02-12T23:47:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173654.80 epoch sec; seqNo:6908; 4057857.70 north; 591522.85 east 2021-02-12T23:47:35Z- AHRS_M2 msg: 1613173655.17 epoch sec; seqNo:6852 2021-02-12T23:47:35Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.19 psi 2021-02-12T23:47:35Z- handleDepth()-Depth_Keller msg: 1613173655193 epoch sec; seqNo:6909; depth 27.79 2021-02-12T23:47:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173655.20 epoch sec; seqNo:6909; 4057857.71 north; 591524.07 east 2021-02-12T23:47:35Z- AHRS_M2 msg: 1613173655.58 epoch sec; seqNo:6853 2021-02-12T23:47:35Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.20 psi 2021-02-12T23:47:35Z- handleDepth()-Depth_Keller msg: 1613173655584 epoch sec; seqNo:6910; depth 27.93 2021-02-12T23:47:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173655.59 epoch sec; seqNo:6910; 4057858.09 north; 591524.07 east 2021-02-12T23:47:35Z- AHRS_M2 msg: 1613173655.98 epoch sec; seqNo:6854 2021-02-12T23:47:35Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.20 psi 2021-02-12T23:47:35Z- handleDepth()-Depth_Keller msg: 1613173655988 epoch sec; seqNo:6911; depth 28.04 2021-02-12T23:47:36Z- handleDvl() - RDI_Pathfinder msg: 1613173656.00 epoch sec; seqNo:3206 2021-02-12T23:47:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 0.00 , 62.12 , 97.98 2021-02-12T23:47:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.12 , 0.16 2021-02-12T23:47:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173656.01 epoch sec; seqNo:6911; 4057858.09 north; 591524.07 east 2021-02-12T23:47:36Z- AHRS_M2 msg: 1613173656.39 epoch sec; seqNo:6855 2021-02-12T23:47:36Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.21 psi 2021-02-12T23:47:36Z- handleDepth()-Depth_Keller msg: 1613173656393 epoch sec; seqNo:6912; depth 28.18 2021-02-12T23:47:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173656.41 epoch sec; seqNo:6912; 4057858.47 north; 591524.06 east 2021-02-12T23:47:36Z- AHRS_M2 msg: 1613173656.82 epoch sec; seqNo:6856 2021-02-12T23:47:36Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.21 psi 2021-02-12T23:47:36Z- handleDepth()-Depth_Keller msg: 1613173656822 epoch sec; seqNo:6913; depth 28.31 2021-02-12T23:47:36Z- handleDvl() - RDI_Pathfinder msg: 1613173656.83 epoch sec; seqNo:3207 2021-02-12T23:47:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 87.17 , 62.12 , 99.07 2021-02-12T23:47:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.15 , 0.08 2021-02-12T23:47:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173656.86 epoch sec; seqNo:6913; 4057858.85 north; 591524.06 east 2021-02-12T23:47:37Z- AHRS_M2 msg: 1613173657.20 epoch sec; seqNo:6857 2021-02-12T23:47:37Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.22 psi 2021-02-12T23:47:37Z- handleDepth()-Depth_Keller msg: 1613173657218 epoch sec; seqNo:6914; depth 28.46 2021-02-12T23:47:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173657.23 epoch sec; seqNo:6914; 4057858.86 north; 591525.28 east 2021-02-12T23:47:37Z- AHRS_M2 msg: 1613173657.60 epoch sec; seqNo:6858 2021-02-12T23:47:37Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.22 psi 2021-02-12T23:47:37Z- handleDepth()-Depth_Keller msg: 1613173657604 epoch sec; seqNo:6915; depth 28.57 2021-02-12T23:47:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173657.61 epoch sec; seqNo:6915; 4057859.24 north; 591525.28 east 2021-02-12T23:47:38Z- AHRS_M2 msg: 1613173658.00 epoch sec; seqNo:6859 2021-02-12T23:47:38Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.22 psi 2021-02-12T23:47:38Z- handleDepth()-Depth_Keller msg: 1613173658007 epoch sec; seqNo:6916; depth 28.70 2021-02-12T23:47:38Z- handleDvl() - RDI_Pathfinder msg: 1613173658.02 epoch sec; seqNo:3208 2021-02-12T23:47:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 0.00 , 60.89 , 98.86 2021-02-12T23:47:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.14 , -0.01 2021-02-12T23:47:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173658.03 epoch sec; seqNo:6916; 4057859.24 north; 591525.28 east 2021-02-12T23:47:38Z- AHRS_M2 msg: 1613173658.41 epoch sec; seqNo:6860 2021-02-12T23:47:38Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.22 psi 2021-02-12T23:47:38Z- handleDepth()-Depth_Keller msg: 1613173658413 epoch sec; seqNo:6917; depth 28.83 2021-02-12T23:47:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173658.42 epoch sec; seqNo:6917; 4057859.62 north; 591525.27 east 2021-02-12T23:47:38Z- AHRS_M2 msg: 1613173658.81 epoch sec; seqNo:6861 2021-02-12T23:47:38Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.22 psi 2021-02-12T23:47:38Z- handleDepth()-Depth_Keller msg: 1613173658816 epoch sec; seqNo:6918; depth 28.96 2021-02-12T23:47:38Z- handleDvl() - RDI_Pathfinder msg: 1613173658.84 epoch sec; seqNo:3209 2021-02-12T23:47:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 0.00 , 62.93 , 97.98 2021-02-12T23:47:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.10 , -0.10 2021-02-12T23:47:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173658.85 epoch sec; seqNo:6918; 4057860.00 north; 591525.27 east 2021-02-12T23:47:39Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 129 calls... 2021-02-12T23:47:39Z- waiting for 1613173656.99, time is 1613173658.85 2021-02-12T23:47:39Z- LcmTrn::updateTrn() equating pose 1613173658.85 and meas times 1613173658.85 2021-02-12T23:47:39Z- LcmTrn::updateTrn() >>>> pose time 1613173658.85, meas time 1613173658.85 <<<< 2021-02-12T23:47:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173658.85 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:47:39Z- TerrainNav::motionUpdate() - currEstimate = (4057860, 591525.27, 28.96) 2021-02-12T23:47:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:47:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:47:39Z- TNavBF::Projecting Measurements 2021-02-12T23:47:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:47:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:47:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:47:39Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:47:39Z- TNavFilter::3 good beams. 2021-02-12T23:47:39Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:47:39Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:47:39Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:47:39Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:47:39Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:47:39Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:47:39Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:47:39Z- TNavBF::mmse selected from filter -1 2021-02-12T23:47:39Z- TerrainMapOctree: (vehN, vehE ) = (4057860.00, 591525.27). 2021-02-12T23:47:39Z- TerrainMapOctree: Min Distance = 4349.29. 2021-02-12T23:47:39Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:47:39Z- There is no specific good beam that all particles have in common. 2021-02-12T23:47:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:47:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.32 K: 0.128 ExpectedMeasDiff: nan currMeas.covariance: 7.27 2021-02-12T23:47:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:47:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.125 ExpectedMeasDiff: nan currMeas.covariance: 6.34 2021-02-12T23:47:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:47:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-12T23:47:39Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:47:39Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:47:39Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:47:39Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:47:39Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:47:39Z- Filter 0 No beams used in subcloud update 2021-02-12T23:47:39Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:47:39Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:47:39Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 736.6 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:47:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173658.85 sec. 2021-02-12T23:47:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:47:39Z- TNavBF::mmse selected from filter -1 2021-02-12T23:47:39Z- variances([0], [2], and psi_berg) = 736.636925 833.021601 0.000000 2021-02-12T23:47:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:47:39Z- LcmTrn::publishEstimates() - MLE : 4057850.50 591486.77 28.96 2021-02-12T23:47:39Z- LcmTrn::publishEstimates() - MMSE : 4057860.00 591525.27 28.96 2021-02-12T23:47:39Z- LcmTrn::publishEstimates() - COVAR: 736.64 833.02 0.95 0.00 2021-02-12T23:47:39Z- AHRS_M2 msg: 1613173659.22 epoch sec; seqNo:6862 2021-02-12T23:47:39Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.22 psi 2021-02-12T23:47:39Z- handleDepth()-Depth_Keller msg: 1613173659221 epoch sec; seqNo:6919; depth 29.10 2021-02-12T23:47:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173659.23 epoch sec; seqNo:6919; 4057860.39 north; 591526.48 east 2021-02-12T23:47:39Z- AHRS_M2 msg: 1613173659.62 epoch sec; seqNo:6863 2021-02-12T23:47:39Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.22 psi 2021-02-12T23:47:39Z- handleDepth()-Depth_Keller msg: 1613173659624 epoch sec; seqNo:6920; depth 29.22 2021-02-12T23:47:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173659.63 epoch sec; seqNo:6920; 4057860.39 north; 591526.48 east 2021-02-12T23:47:40Z- AHRS_M2 msg: 1613173660.02 epoch sec; seqNo:6864 2021-02-12T23:47:40Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.22 psi 2021-02-12T23:47:40Z- handleDepth()-Depth_Keller msg: 1613173660029 epoch sec; seqNo:6921; depth 29.34 2021-02-12T23:47:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173660.04 epoch sec; seqNo:6921; 4057860.77 north; 591526.48 east 2021-02-12T23:47:40Z- AHRS_M2 msg: 1613173660.43 epoch sec; seqNo:6865 2021-02-12T23:47:40Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.21 psi 2021-02-12T23:47:40Z- handleDepth()-Depth_Keller msg: 1613173660434 epoch sec; seqNo:6922; depth 29.49 2021-02-12T23:47:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173660.44 epoch sec; seqNo:6922; 4057861.15 north; 591526.48 east 2021-02-12T23:47:40Z- AHRS_M2 msg: 1613173660.83 epoch sec; seqNo:6866 2021-02-12T23:47:40Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.21 psi 2021-02-12T23:47:40Z- handleDepth()-Depth_Keller msg: 1613173660836 epoch sec; seqNo:6923; depth 29.61 2021-02-12T23:47:40Z- handleDvl() - RDI_Pathfinder msg: 1613173660.86 epoch sec; seqNo:3210 2021-02-12T23:47:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.20 , 0.00 , 61.30 , 97.98 2021-02-12T23:47:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.06 , -0.08 2021-02-12T23:47:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173660.87 epoch sec; seqNo:6923; 4057861.53 north; 591526.47 east 2021-02-12T23:47:41Z- AHRS_M2 msg: 1613173661.24 epoch sec; seqNo:6867 2021-02-12T23:47:41Z- AHRS_M2 msg: 0.60 phi; -0.27 theta; -0.20 psi 2021-02-12T23:47:41Z- handleDepth()-Depth_Keller msg: 1613173661241 epoch sec; seqNo:6924; depth 29.79 2021-02-12T23:47:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173661.25 epoch sec; seqNo:6924; 4057861.90 north; 591526.47 east 2021-02-12T23:47:41Z- AHRS_M2 msg: 1613173661.64 epoch sec; seqNo:6868 2021-02-12T23:47:41Z- AHRS_M2 msg: 0.59 phi; -0.23 theta; -0.19 psi 2021-02-12T23:47:41Z- handleDepth()-Depth_Keller msg: 1613173661644 epoch sec; seqNo:6925; depth 29.91 2021-02-12T23:47:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173661.65 epoch sec; seqNo:6925; 4057862.30 north; 591527.68 east 2021-02-12T23:47:42Z- AHRS_M2 msg: 1613173662.04 epoch sec; seqNo:6869 2021-02-12T23:47:42Z- AHRS_M2 msg: 0.57 phi; -0.18 theta; -0.17 psi 2021-02-12T23:47:42Z- handleDepth()-Depth_Keller msg: 1613173662048 epoch sec; seqNo:6926; depth 30.05 2021-02-12T23:47:42Z- handleDvl() - RDI_Pathfinder msg: 1613173662.07 epoch sec; seqNo:3211 2021-02-12T23:47:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.57 , 82.84 , 62.12 , 158.55 2021-02-12T23:47:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.10 , -0.09 2021-02-12T23:47:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173662.08 epoch sec; seqNo:6926; 4057862.30 north; 591527.68 east 2021-02-12T23:47:42Z- AHRS_M2 msg: 1613173662.45 epoch sec; seqNo:6870 2021-02-12T23:47:42Z- AHRS_M2 msg: 0.56 phi; -0.14 theta; -0.16 psi 2021-02-12T23:47:42Z- handleDepth()-Depth_Keller msg: 1613173662452 epoch sec; seqNo:6927; depth 30.17 2021-02-12T23:47:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173662.46 epoch sec; seqNo:6927; 4057862.67 north; 591527.68 east 2021-02-12T23:47:42Z- AHRS_M2 msg: 1613173662.85 epoch sec; seqNo:6871 2021-02-12T23:47:42Z- AHRS_M2 msg: 0.55 phi; -0.09 theta; -0.16 psi 2021-02-12T23:47:42Z- handleDepth()-Depth_Keller msg: 1613173662882 epoch sec; seqNo:6928; depth 30.27 2021-02-12T23:47:42Z- handleDvl() - RDI_Pathfinder msg: 1613173662.91 epoch sec; seqNo:3212 2021-02-12T23:47:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.34 , 78.79 , 63.34 , 78.79 2021-02-12T23:47:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.10 , -0.03 2021-02-12T23:47:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173662.92 epoch sec; seqNo:6928; 4057863.05 north; 591527.68 east 2021-02-12T23:47:43Z- AHRS_M2 msg: 1613173663.26 epoch sec; seqNo:6872 2021-02-12T23:47:43Z- AHRS_M2 msg: 0.54 phi; -0.05 theta; -0.15 psi 2021-02-12T23:47:43Z- handleDepth()-Depth_Keller msg: 1613173663260 epoch sec; seqNo:6929; depth 30.37 2021-02-12T23:47:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173663.29 epoch sec; seqNo:6929; 4057863.43 north; 591527.67 east 2021-02-12T23:47:43Z- AHRS_M2 msg: 1613173663.66 epoch sec; seqNo:6873 2021-02-12T23:47:43Z- AHRS_M2 msg: 0.54 phi; -0.01 theta; -0.14 psi 2021-02-12T23:47:43Z- handleDepth()-Depth_Keller msg: 1613173663664 epoch sec; seqNo:6930; depth 30.45 2021-02-12T23:47:43Z- handleDvl() - RDI_Pathfinder msg: 1613173663.67 epoch sec; seqNo:3213 2021-02-12T23:47:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.37 , 0.00 , 67.46 , 0.00 2021-02-12T23:47:43Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:47:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173663.70 epoch sec; seqNo:6930; 4057863.81 north; 591527.67 east 2021-02-12T23:47:44Z- AHRS_M2 msg: 1613173664.06 epoch sec; seqNo:6874 2021-02-12T23:47:44Z- AHRS_M2 msg: 0.53 phi; 0.03 theta; -0.13 psi 2021-02-12T23:47:44Z- handleDepth()-Depth_Keller msg: 1613173664073 epoch sec; seqNo:6931; depth 30.50 2021-02-12T23:47:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173664.08 epoch sec; seqNo:6931; 4057864.19 north; 591527.66 east 2021-02-12T23:47:44Z- LcmTrn::handleMessages() - handling 43 messages took 0.66 ms and 130 calls... 2021-02-12T23:47:44Z- waiting for 1613173661.85, time is 1613173664.08 2021-02-12T23:47:44Z- LcmTrn::updateTrn() >>>> pose time 1613173664.08, meas time 1613173663.67 <<<< 2021-02-12T23:47:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173663.67 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:47:44Z- TerrainNav::motionUpdate() - currEstimate = (4057864, 591527.66, 30.50) 2021-02-12T23:47:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:47:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:47:44Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:47:44Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:47:44Z- TNavBF::Projecting Measurements 2021-02-12T23:47:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:47:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:47:44Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-12T23:47:44Z- TNavFilter::2 good beams. 2021-02-12T23:47:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:47:44Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:47:44Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:47:44Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:47:44Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:47:44Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-12T23:47:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:47:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:47:44Z- TerrainMapOctree: (vehN, vehE ) = (4057863.86, 591527.48). 2021-02-12T23:47:44Z- TerrainMapOctree: Min Distance = 4344.89. 2021-02-12T23:47:44Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:47:44Z- There is no specific good beam that all particles have in common. 2021-02-12T23:47:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:47:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.126 ExpectedMeasDiff: nan currMeas.covariance: 6.42 2021-02-12T23:47:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:47:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 7.28 2021-02-12T23:47:44Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:47:44Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:47:44Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:47:44Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:47:44Z- Filter 0 No beams used in subcloud update 2021-02-12T23:47:44Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:47:44Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:47:44Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 736.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:47:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173663.67 sec. 2021-02-12T23:47:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:47:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:47:44Z- variances([0], [2], and psi_berg) = 736.525134 833.021513 0.000000 2021-02-12T23:47:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:47:44Z- LcmTrn::publishEstimates() - MLE : 4057854.69 591489.16 30.50 2021-02-12T23:47:44Z- LcmTrn::publishEstimates() - MMSE : 4057864.19 591527.66 30.50 2021-02-12T23:47:44Z- LcmTrn::publishEstimates() - COVAR: 736.53 833.02 0.94 0.00 2021-02-12T23:47:44Z- AHRS_M2 msg: 1613173664.47 epoch sec; seqNo:6875 2021-02-12T23:47:44Z- AHRS_M2 msg: 0.53 phi; 0.07 theta; -0.12 psi 2021-02-12T23:47:44Z- handleDepth()-Depth_Keller msg: 1613173664472 epoch sec; seqNo:6932; depth 30.55 2021-02-12T23:47:44Z- handleDvl() - RDI_Pathfinder msg: 1613173664.48 epoch sec; seqNo:3214 2021-02-12T23:47:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 64.22 , 0.00 , 0.00 , 65.99 2021-02-12T23:47:44Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:47:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173664.49 epoch sec; seqNo:6932; 4057864.57 north; 591527.66 east 2021-02-12T23:47:44Z- AHRS_M2 msg: 1613173664.87 epoch sec; seqNo:6876 2021-02-12T23:47:44Z- AHRS_M2 msg: 0.53 phi; 0.11 theta; -0.12 psi 2021-02-12T23:47:44Z- handleDepth()-Depth_Keller msg: 1613173664878 epoch sec; seqNo:6933; depth 30.59 2021-02-12T23:47:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173664.89 epoch sec; seqNo:6933; 4057864.58 north; 591528.88 east 2021-02-12T23:47:45Z- AHRS_M2 msg: 1613173665.28 epoch sec; seqNo:6877 2021-02-12T23:47:45Z- AHRS_M2 msg: 0.53 phi; 0.14 theta; -0.11 psi 2021-02-12T23:47:45Z- handleDepth()-Depth_Keller msg: 1613173665285 epoch sec; seqNo:6934; depth 30.60 2021-02-12T23:47:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173665.31 epoch sec; seqNo:6934; 4057864.96 north; 591528.88 east 2021-02-12T23:47:45Z- AHRS_M2 msg: 1613173665.68 epoch sec; seqNo:6878 2021-02-12T23:47:45Z- AHRS_M2 msg: 0.53 phi; 0.17 theta; -0.10 psi 2021-02-12T23:47:45Z- handleDepth()-Depth_Keller msg: 1613173665684 epoch sec; seqNo:6935; depth 30.62 2021-02-12T23:47:45Z- handleDvl() - RDI_Pathfinder msg: 1613173665.70 epoch sec; seqNo:3215 2021-02-12T23:47:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 65.55 , 0.00 , 0.00 , 62.89 2021-02-12T23:47:45Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:47:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173665.71 epoch sec; seqNo:6935; 4057865.34 north; 591528.87 east 2021-02-12T23:47:46Z- AHRS_M2 msg: 1613173666.08 epoch sec; seqNo:6879 2021-02-12T23:47:46Z- AHRS_M2 msg: 0.53 phi; 0.20 theta; -0.10 psi 2021-02-12T23:47:46Z- handleDepth()-Depth_Keller msg: 1613173666090 epoch sec; seqNo:6936; depth 30.59 2021-02-12T23:47:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173666.10 epoch sec; seqNo:6936; 4057865.72 north; 591528.87 east 2021-02-12T23:47:46Z- AHRS_M2 msg: 1613173666.49 epoch sec; seqNo:6880 2021-02-12T23:47:46Z- AHRS_M2 msg: 0.53 phi; 0.22 theta; -0.10 psi 2021-02-12T23:47:46Z- handleDepth()-Depth_Keller msg: 1613173666492 epoch sec; seqNo:6937; depth 30.58 2021-02-12T23:47:46Z- handleDvl() - RDI_Pathfinder msg: 1613173666.50 epoch sec; seqNo:3216 2021-02-12T23:47:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 66.43 , 0.00 , 0.00 , 61.56 2021-02-12T23:47:46Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:47:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173666.54 epoch sec; seqNo:6937; 4057866.10 north; 591528.87 east 2021-02-12T23:47:46Z- AHRS_M2 msg: 1613173666.89 epoch sec; seqNo:6881 2021-02-12T23:47:46Z- AHRS_M2 msg: 0.54 phi; 0.24 theta; -0.09 psi 2021-02-12T23:47:46Z- handleDepth()-Depth_Keller msg: 1613173666901 epoch sec; seqNo:6938; depth 30.54 2021-02-12T23:47:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173666.92 epoch sec; seqNo:6938; 4057866.48 north; 591528.86 east 2021-02-12T23:47:47Z- AHRS_M2 msg: 1613173667.30 epoch sec; seqNo:6882 2021-02-12T23:47:47Z- AHRS_M2 msg: 0.54 phi; 0.28 theta; -0.09 psi 2021-02-12T23:47:47Z- handleDepth()-Depth_Keller msg: 1613173667307 epoch sec; seqNo:6939; depth 30.50 2021-02-12T23:47:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173667.32 epoch sec; seqNo:6939; 4057866.86 north; 591528.86 east 2021-02-12T23:47:47Z- AHRS_M2 msg: 1613173667.70 epoch sec; seqNo:6883 2021-02-12T23:47:47Z- AHRS_M2 msg: 0.54 phi; 0.30 theta; -0.09 psi 2021-02-12T23:47:47Z- handleDepth()-Depth_Keller msg: 1613173667714 epoch sec; seqNo:6940; depth 30.44 2021-02-12T23:47:47Z- handleDvl() - RDI_Pathfinder msg: 1613173667.73 epoch sec; seqNo:3217 2021-02-12T23:47:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:47:47Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:47:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173667.74 epoch sec; seqNo:6940; 4057866.86 north; 591528.86 east 2021-02-12T23:47:48Z- AHRS_M2 msg: 1613173668.10 epoch sec; seqNo:6884 2021-02-12T23:47:48Z- AHRS_M2 msg: 0.54 phi; 0.31 theta; -0.09 psi 2021-02-12T23:47:48Z- handleDepth()-Depth_Keller msg: 1613173668109 epoch sec; seqNo:6941; depth 30.37 2021-02-12T23:47:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173668.12 epoch sec; seqNo:6941; 4057867.25 north; 591530.07 east 2021-02-12T23:47:48Z- AHRS_M2 msg: 1613173668.51 epoch sec; seqNo:6885 2021-02-12T23:47:48Z- AHRS_M2 msg: 0.54 phi; 0.32 theta; -0.09 psi 2021-02-12T23:47:48Z- handleDepth()-Depth_Keller msg: 1613173668517 epoch sec; seqNo:6942; depth 30.31 2021-02-12T23:47:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173668.53 epoch sec; seqNo:6942; 4057867.63 north; 591530.07 east 2021-02-12T23:47:48Z- AHRS_M2 msg: 1613173668.91 epoch sec; seqNo:6886 2021-02-12T23:47:48Z- AHRS_M2 msg: 0.54 phi; 0.33 theta; -0.09 psi 2021-02-12T23:47:48Z- handleDepth()-Depth_Keller msg: 1613173668917 epoch sec; seqNo:6943; depth 30.21 2021-02-12T23:47:48Z- handleDvl() - RDI_Pathfinder msg: 1613173668.93 epoch sec; seqNo:3218 2021-02-12T23:47:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 78.51 , 168.29 , 61.30 2021-02-12T23:47:48Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.05 , 0.05 2021-02-12T23:47:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173668.94 epoch sec; seqNo:6943; 4057868.00 north; 591530.07 east 2021-02-12T23:47:49Z- AHRS_M2 msg: 1613173669.32 epoch sec; seqNo:6887 2021-02-12T23:47:49Z- AHRS_M2 msg: 0.54 phi; 0.34 theta; -0.09 psi 2021-02-12T23:47:49Z- handleDepth()-Depth_Keller msg: 1613173669320 epoch sec; seqNo:6944; depth 30.13 2021-02-12T23:47:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173669.34 epoch sec; seqNo:6944; 4057868.38 north; 591530.06 east 2021-02-12T23:47:49Z- LcmTrn::handleMessages() - handling 44 messages took 0.74 ms and 129 calls... 2021-02-12T23:47:49Z- waiting for 1613173667.08, time is 1613173669.34 2021-02-12T23:47:49Z- LcmTrn::updateTrn() >>>> pose time 1613173669.34, meas time 1613173668.93 <<<< 2021-02-12T23:47:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173668.93 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:47:49Z- TerrainNav::motionUpdate() - currEstimate = (4057868, 591530.06, 30.13) 2021-02-12T23:47:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:47:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:47:49Z- TNavBF::Projecting Measurements 2021-02-12T23:47:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:47:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:47:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:47:49Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:47:49Z- TNavFilter::3 good beams. 2021-02-12T23:47:49Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:47:49Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:47:49Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:47:49Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:47:49Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:47:49Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:47:49Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:47:49Z- TNavBF::mmse selected from filter -1 2021-02-12T23:47:49Z- TerrainMapOctree: (vehN, vehE ) = (4057868.06, 591529.88). 2021-02-12T23:47:49Z- TerrainMapOctree: Min Distance = 4340.12. 2021-02-12T23:47:49Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:47:49Z- There is no specific good beam that all particles have in common. 2021-02-12T23:47:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:47:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-12T23:47:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:47:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-12T23:47:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:47:49Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.110 ExpectedMeasDiff: nan currMeas.covariance: 6.01 2021-02-12T23:47:49Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:47:49Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:47:49Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:47:49Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:47:49Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:47:49Z- Filter 0 No beams used in subcloud update 2021-02-12T23:47:49Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:47:49Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:47:49Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 736.4 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:47:49Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173668.93 sec. 2021-02-12T23:47:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:47:49Z- TNavBF::mmse selected from filter -1 2021-02-12T23:47:49Z- variances([0], [2], and psi_berg) = 736.413547 833.021425 0.000000 2021-02-12T23:47:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:47:49Z- LcmTrn::publishEstimates() - MLE : 4057858.88 591491.56 30.13 2021-02-12T23:47:49Z- LcmTrn::publishEstimates() - MMSE : 4057868.38 591530.06 30.13 2021-02-12T23:47:49Z- LcmTrn::publishEstimates() - COVAR: 736.41 833.02 0.87 0.00 2021-02-12T23:47:49Z- AHRS_M2 msg: 1613173669.76 epoch sec; seqNo:6888 2021-02-12T23:47:49Z- AHRS_M2 msg: 0.54 phi; 0.34 theta; -0.09 psi 2021-02-12T23:47:49Z- handleDepth()-Depth_Keller msg: 1613173669777 epoch sec; seqNo:6945; depth 30.05 2021-02-12T23:47:49Z- handleDvl() - RDI_Pathfinder msg: 1613173669.79 epoch sec; seqNo:3219 2021-02-12T23:47:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 78.58 , 97.51 , 59.66 2021-02-12T23:47:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , 0.12 2021-02-12T23:47:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173669.82 epoch sec; seqNo:6945; 4057868.76 north; 591530.06 east 2021-02-12T23:47:50Z- AHRS_M2 msg: 1613173670.13 epoch sec; seqNo:6889 2021-02-12T23:47:50Z- AHRS_M2 msg: 0.54 phi; 0.35 theta; -0.10 psi 2021-02-12T23:47:50Z- handleDepth()-Depth_Keller msg: 1613173670137 epoch sec; seqNo:6946; depth 29.94 2021-02-12T23:47:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173670.15 epoch sec; seqNo:6946; 4057868.76 north; 591530.06 east 2021-02-12T23:47:50Z- AHRS_M2 msg: 1613173670.53 epoch sec; seqNo:6890 2021-02-12T23:47:50Z- AHRS_M2 msg: 0.55 phi; 0.35 theta; -0.09 psi 2021-02-12T23:47:50Z- handleDepth()-Depth_Keller msg: 1613173670533 epoch sec; seqNo:6947; depth 29.85 2021-02-12T23:47:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173670.54 epoch sec; seqNo:6947; 4057869.14 north; 591530.05 east 2021-02-12T23:47:50Z- AHRS_M2 msg: 1613173670.93 epoch sec; seqNo:6891 2021-02-12T23:47:50Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.09 psi 2021-02-12T23:47:50Z- handleDepth()-Depth_Keller msg: 1613173670946 epoch sec; seqNo:6948; depth 29.76 2021-02-12T23:47:50Z- handleDvl() - RDI_Pathfinder msg: 1613173670.96 epoch sec; seqNo:3220 2021-02-12T23:47:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 78.51 , 97.98 , 61.30 2021-02-12T23:47:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.12 , 0.09 2021-02-12T23:47:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173670.97 epoch sec; seqNo:6948; 4057869.53 north; 591531.27 east 2021-02-12T23:47:51Z- AHRS_M2 msg: 1613173671.34 epoch sec; seqNo:6892 2021-02-12T23:47:51Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.09 psi 2021-02-12T23:47:51Z- handleDepth()-Depth_Keller msg: 1613173671340 epoch sec; seqNo:6949; depth 29.64 2021-02-12T23:47:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173671.35 epoch sec; seqNo:6949; 4057869.91 north; 591531.27 east 2021-02-12T23:47:51Z- AHRS_M2 msg: 1613173671.74 epoch sec; seqNo:6893 2021-02-12T23:47:51Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.09 psi 2021-02-12T23:47:51Z- handleDepth()-Depth_Keller msg: 1613173671744 epoch sec; seqNo:6950; depth 29.54 2021-02-12T23:47:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173671.75 epoch sec; seqNo:6950; 4057869.91 north; 591531.27 east 2021-02-12T23:47:52Z- AHRS_M2 msg: 1613173672.15 epoch sec; seqNo:6894 2021-02-12T23:47:52Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.09 psi 2021-02-12T23:47:52Z- handleDepth()-Depth_Keller msg: 1613173672157 epoch sec; seqNo:6951; depth 29.42 2021-02-12T23:47:52Z- handleDvl() - RDI_Pathfinder msg: 1613173672.17 epoch sec; seqNo:3221 2021-02-12T23:47:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 79.60 , 97.98 , 62.12 2021-02-12T23:47:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.19 , 0.00 2021-02-12T23:47:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173672.18 epoch sec; seqNo:6951; 4057870.29 north; 591531.26 east 2021-02-12T23:47:52Z- AHRS_M2 msg: 1613173672.55 epoch sec; seqNo:6895 2021-02-12T23:47:52Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.09 psi 2021-02-12T23:47:52Z- handleDepth()-Depth_Keller msg: 1613173672557 epoch sec; seqNo:6952; depth 29.31 2021-02-12T23:47:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173672.57 epoch sec; seqNo:6952; 4057870.67 north; 591531.26 east 2021-02-12T23:47:52Z- AHRS_M2 msg: 1613173672.95 epoch sec; seqNo:6896 2021-02-12T23:47:52Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.09 psi 2021-02-12T23:47:52Z- handleDepth()-Depth_Keller msg: 1613173672965 epoch sec; seqNo:6953; depth 29.20 2021-02-12T23:47:52Z- handleDvl() - RDI_Pathfinder msg: 1613173672.99 epoch sec; seqNo:3222 2021-02-12T23:47:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 80.68 , 97.98 , 61.71 2021-02-12T23:47:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.16 , -0.11 2021-02-12T23:47:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173673.01 epoch sec; seqNo:6953; 4057870.68 north; 591532.48 east 2021-02-12T23:47:53Z- AHRS_M2 msg: 1613173673.36 epoch sec; seqNo:6897 2021-02-12T23:47:53Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.09 psi 2021-02-12T23:47:53Z- handleDepth()-Depth_Keller msg: 1613173673365 epoch sec; seqNo:6954; depth 29.06 2021-02-12T23:47:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173673.38 epoch sec; seqNo:6954; 4057871.06 north; 591532.48 east 2021-02-12T23:47:53Z- AHRS_M2 msg: 1613173673.76 epoch sec; seqNo:6898 2021-02-12T23:47:53Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.09 psi 2021-02-12T23:47:53Z- handleDepth()-Depth_Keller msg: 1613173673768 epoch sec; seqNo:6955; depth 28.96 2021-02-12T23:47:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173673.81 epoch sec; seqNo:6955; 4057871.06 north; 591532.48 east 2021-02-12T23:47:54Z- AHRS_M2 msg: 1613173674.16 epoch sec; seqNo:6899 2021-02-12T23:47:54Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.10 psi 2021-02-12T23:47:54Z- handleDepth()-Depth_Keller msg: 1613173674201 epoch sec; seqNo:6956; depth 28.83 2021-02-12T23:47:54Z- handleDvl() - RDI_Pathfinder msg: 1613173674.21 epoch sec; seqNo:3223 2021-02-12T23:47:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 81.76 , 97.98 , 62.12 2021-02-12T23:47:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.08 , -0.18 2021-02-12T23:47:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173674.22 epoch sec; seqNo:6956; 4057871.44 north; 591532.47 east 2021-02-12T23:47:54Z- LcmTrn::handleMessages() - handling 41 messages took 0.67 ms and 128 calls... 2021-02-12T23:47:54Z- waiting for 1613173672.34, time is 1613173674.22 2021-02-12T23:47:54Z- LcmTrn::updateTrn() equating pose 1613173674.22 and meas times 1613173674.22 2021-02-12T23:47:54Z- LcmTrn::updateTrn() >>>> pose time 1613173674.22, meas time 1613173674.22 <<<< 2021-02-12T23:47:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173674.22 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:47:54Z- TerrainNav::motionUpdate() - currEstimate = (4057871, 591532.47, 28.83) 2021-02-12T23:47:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:47:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:47:54Z- TNavBF::Projecting Measurements 2021-02-12T23:47:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:47:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:47:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:47:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:47:54Z- TNavFilter::4 good beams. 2021-02-12T23:47:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:47:54Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:47:54Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:47:54Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:47:54Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:47:54Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:47:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:47:54Z- TNavBF::mmse selected from filter -1 2021-02-12T23:47:54Z- TerrainMapOctree: (vehN, vehE ) = (4057871.44, 591532.47). 2021-02-12T23:47:54Z- TerrainMapOctree: Min Distance = 4335.85. 2021-02-12T23:47:54Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:47:54Z- There is no specific good beam that all particles have in common. 2021-02-12T23:47:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:47:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-12T23:47:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:47:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 10.70 2021-02-12T23:47:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:47:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-12T23:47:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:47:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 6.17 2021-02-12T23:47:54Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:47:54Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:47:54Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:47:54Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:47:54Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:47:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:47:54Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:47:54Z- Filter 0 No beams used in subcloud update 2021-02-12T23:47:54Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:47:54Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:47:54Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 736.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:47:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173674.22 sec. 2021-02-12T23:47:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:47:54Z- TNavBF::mmse selected from filter -1 2021-02-12T23:47:54Z- variances([0], [2], and psi_berg) = 736.332797 833.021361 0.000000 2021-02-12T23:47:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:47:54Z- LcmTrn::publishEstimates() - MLE : 4057861.94 591493.97 28.83 2021-02-12T23:47:54Z- LcmTrn::publishEstimates() - MMSE : 4057871.44 591532.47 28.83 2021-02-12T23:47:54Z- LcmTrn::publishEstimates() - COVAR: 736.33 833.02 0.78 0.00 2021-02-12T23:47:54Z- AHRS_M2 msg: 1613173674.57 epoch sec; seqNo:6900 2021-02-12T23:47:54Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.10 psi 2021-02-12T23:47:54Z- handleDepth()-Depth_Keller msg: 1613173674606 epoch sec; seqNo:6957; depth 28.71 2021-02-12T23:47:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173674.62 epoch sec; seqNo:6957; 4057871.82 north; 591532.47 east 2021-02-12T23:47:54Z- AHRS_M2 msg: 1613173674.97 epoch sec; seqNo:6901 2021-02-12T23:47:54Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.11 psi 2021-02-12T23:47:54Z- handleDepth()-Depth_Keller msg: 1613173674978 epoch sec; seqNo:6958; depth 28.61 2021-02-12T23:47:54Z- handleDvl() - RDI_Pathfinder msg: 1613173674.99 epoch sec; seqNo:3224 2021-02-12T23:47:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 0.00 , 97.92 , 61.71 2021-02-12T23:47:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.01 , -0.19 2021-02-12T23:47:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173675.00 epoch sec; seqNo:6958; 4057871.82 north; 591532.47 east 2021-02-12T23:47:55Z- AHRS_M2 msg: 1613173675.38 epoch sec; seqNo:6902 2021-02-12T23:47:55Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.12 psi 2021-02-12T23:47:55Z- handleDepth()-Depth_Keller msg: 1613173675381 epoch sec; seqNo:6959; depth 28.48 2021-02-12T23:47:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173675.40 epoch sec; seqNo:6959; 4057872.21 north; 591533.68 east 2021-02-12T23:47:55Z- AHRS_M2 msg: 1613173675.78 epoch sec; seqNo:6903 2021-02-12T23:47:55Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.13 psi 2021-02-12T23:47:55Z- handleDepth()-Depth_Keller msg: 1613173675785 epoch sec; seqNo:6960; depth 28.36 2021-02-12T23:47:55Z- handleDvl() - RDI_Pathfinder msg: 1613173675.80 epoch sec; seqNo:3225 2021-02-12T23:47:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 0.00 , 97.98 , 61.30 2021-02-12T23:47:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.08 , -0.12 2021-02-12T23:47:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173675.82 epoch sec; seqNo:6960; 4057872.21 north; 591533.68 east 2021-02-12T23:47:56Z- AHRS_M2 msg: 1613173676.18 epoch sec; seqNo:6904 2021-02-12T23:47:56Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-12T23:47:56Z- handleDepth()-Depth_Keller msg: 1613173676189 epoch sec; seqNo:6961; depth 28.23 2021-02-12T23:47:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173676.20 epoch sec; seqNo:6961; 4057872.59 north; 591533.68 east 2021-02-12T23:47:56Z- AHRS_M2 msg: 1613173676.59 epoch sec; seqNo:6905 2021-02-12T23:47:56Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-12T23:47:56Z- handleDepth()-Depth_Keller msg: 1613173676595 epoch sec; seqNo:6962; depth 28.12 2021-02-12T23:47:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173676.60 epoch sec; seqNo:6962; 4057872.97 north; 591533.68 east 2021-02-12T23:47:56Z- AHRS_M2 msg: 1613173676.99 epoch sec; seqNo:6906 2021-02-12T23:47:56Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-12T23:47:56Z- handleDepth()-Depth_Keller msg: 1613173676997 epoch sec; seqNo:6963; depth 28.00 2021-02-12T23:47:57Z- handleDvl() - RDI_Pathfinder msg: 1613173677.02 epoch sec; seqNo:3226 2021-02-12T23:47:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 83.92 , 97.98 , 61.71 2021-02-12T23:47:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.16 , -0.03 2021-02-12T23:47:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173677.03 epoch sec; seqNo:6963; 4057872.97 north; 591533.68 east 2021-02-12T23:47:57Z- AHRS_M2 msg: 1613173677.40 epoch sec; seqNo:6907 2021-02-12T23:47:57Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-12T23:47:57Z- handleDepth()-Depth_Keller msg: 1613173677401 epoch sec; seqNo:6964; depth 27.88 2021-02-12T23:47:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173677.41 epoch sec; seqNo:6964; 4057873.35 north; 591533.67 east 2021-02-12T23:47:57Z- AHRS_M2 msg: 1613173677.80 epoch sec; seqNo:6908 2021-02-12T23:47:57Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-12T23:47:57Z- handleDepth()-Depth_Keller msg: 1613173677805 epoch sec; seqNo:6965; depth 27.77 2021-02-12T23:47:57Z- handleDvl() - RDI_Pathfinder msg: 1613173677.82 epoch sec; seqNo:3227 2021-02-12T23:47:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:47:57Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:47:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173677.83 epoch sec; seqNo:6965; 4057873.73 north; 591533.67 east 2021-02-12T23:47:58Z- AHRS_M2 msg: 1613173678.20 epoch sec; seqNo:6909 2021-02-12T23:47:58Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-12T23:47:58Z- handleDepth()-Depth_Keller msg: 1613173678213 epoch sec; seqNo:6966; depth 27.66 2021-02-12T23:47:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173678.23 epoch sec; seqNo:6966; 4057874.11 north; 591533.66 east 2021-02-12T23:47:58Z- AHRS_M2 msg: 1613173678.61 epoch sec; seqNo:6910 2021-02-12T23:47:58Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.19 psi 2021-02-12T23:47:58Z- handleDepth()-Depth_Keller msg: 1613173678621 epoch sec; seqNo:6967; depth 27.51 2021-02-12T23:47:58Z- handleDvl() - RDI_Pathfinder msg: 1613173678.64 epoch sec; seqNo:3228 2021-02-12T23:47:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 69.45 , 0.00 , 0.00 , 63.93 2021-02-12T23:47:58Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:47:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173678.65 epoch sec; seqNo:6967; 4057874.11 north; 591533.66 east 2021-02-12T23:47:59Z- AHRS_M2 msg: 1613173679.01 epoch sec; seqNo:6911 2021-02-12T23:47:59Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.19 psi 2021-02-12T23:47:59Z- handleDepth()-Depth_Keller msg: 1613173679017 epoch sec; seqNo:6968; depth 27.40 2021-02-12T23:47:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173679.03 epoch sec; seqNo:6968; 4057874.48 north; 591533.66 east 2021-02-12T23:47:59Z- AHRS_M2 msg: 1613173679.43 epoch sec; seqNo:6912 2021-02-12T23:47:59Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.19 psi 2021-02-12T23:47:59Z- handleDepth()-Depth_Keller msg: 1613173679439 epoch sec; seqNo:6969; depth 27.29 2021-02-12T23:47:59Z- handleDvl() - RDI_Pathfinder msg: 1613173679.45 epoch sec; seqNo:3229 2021-02-12T23:47:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 63.93 2021-02-12T23:47:59Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:47:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173679.48 epoch sec; seqNo:6969; 4057874.88 north; 591534.88 east 2021-02-12T23:47:59Z- LcmTrn::handleMessages() - handling 45 messages took 0.72 ms and 131 calls... 2021-02-12T23:47:59Z- waiting for 1613173677.22, time is 1613173679.48 2021-02-12T23:47:59Z- LcmTrn::updateTrn() equating pose 1613173679.48 and meas times 1613173679.48 2021-02-12T23:47:59Z- LcmTrn::updateTrn() >>>> pose time 1613173679.48, meas time 1613173679.48 <<<< 2021-02-12T23:47:59Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173679.48, ping # 0. 2021-02-12T23:47:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173679.48 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:47:59Z- TerrainNav::motionUpdate() - currEstimate = (4057875, 591534.88, 27.29) 2021-02-12T23:47:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:47:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:47:59Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:47:59Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:47:59Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173679.48 sec.; No relevant navigation data available 2021-02-12T23:47:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:47:59Z- TNavBF::mmse selected from filter -1 2021-02-12T23:47:59Z- variances([0], [2], and psi_berg) = 736.249058 833.021296 0.000000 2021-02-12T23:47:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:47:59Z- LcmTrn::publishEstimates() - MLE : 4057865.38 591496.38 27.29 2021-02-12T23:47:59Z- LcmTrn::publishEstimates() - MMSE : 4057874.88 591534.88 27.29 2021-02-12T23:47:59Z- LcmTrn::publishEstimates() - COVAR: 736.25 833.02 0.99 0.00 2021-02-12T23:47:59Z- AHRS_M2 msg: 1613173679.82 epoch sec; seqNo:6913 2021-02-12T23:47:59Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.19 psi 2021-02-12T23:47:59Z- handleDepth()-Depth_Keller msg: 1613173679827 epoch sec; seqNo:6970; depth 27.16 2021-02-12T23:47:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173679.84 epoch sec; seqNo:6970; 4057875.25 north; 591534.87 east 2021-02-12T23:48:00Z- AHRS_M2 msg: 1613173680.22 epoch sec; seqNo:6914 2021-02-12T23:48:00Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.19 psi 2021-02-12T23:48:00Z- handleDepth()-Depth_Keller msg: 1613173680266 epoch sec; seqNo:6971; depth 27.06 2021-02-12T23:48:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173680.28 epoch sec; seqNo:6971; 4057875.25 north; 591534.87 east 2021-02-12T23:48:00Z- AHRS_M2 msg: 1613173680.63 epoch sec; seqNo:6915 2021-02-12T23:48:00Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-12T23:48:00Z- handleDepth()-Depth_Keller msg: 1613173680647 epoch sec; seqNo:6972; depth 26.96 2021-02-12T23:48:00Z- handleDvl() - RDI_Pathfinder msg: 1613173680.68 epoch sec; seqNo:3230 2021-02-12T23:48:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 69.91 , 0.00 , 0.00 , 63.47 2021-02-12T23:48:00Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:48:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173680.71 epoch sec; seqNo:6972; 4057875.63 north; 591534.87 east 2021-02-12T23:48:01Z- AHRS_M2 msg: 1613173681.03 epoch sec; seqNo:6916 2021-02-12T23:48:01Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-12T23:48:01Z- handleDepth()-Depth_Keller msg: 1613173681069 epoch sec; seqNo:6973; depth 26.83 2021-02-12T23:48:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173681.08 epoch sec; seqNo:6973; 4057876.01 north; 591534.86 east 2021-02-12T23:48:01Z- AHRS_M2 msg: 1613173681.44 epoch sec; seqNo:6917 2021-02-12T23:48:01Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-12T23:48:01Z- handleDepth()-Depth_Keller msg: 1613173681485 epoch sec; seqNo:6974; depth 26.73 2021-02-12T23:48:01Z- handleDvl() - RDI_Pathfinder msg: 1613173681.50 epoch sec; seqNo:3231 2021-02-12T23:48:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:48:01Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:48:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173681.51 epoch sec; seqNo:6974; 4057876.39 north; 591534.86 east 2021-02-12T23:48:01Z- AHRS_M2 msg: 1613173681.84 epoch sec; seqNo:6918 2021-02-12T23:48:01Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-12T23:48:01Z- handleDepth()-Depth_Keller msg: 1613173681849 epoch sec; seqNo:6975; depth 26.60 2021-02-12T23:48:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173681.86 epoch sec; seqNo:6975; 4057876.39 north; 591534.86 east 2021-02-12T23:48:02Z- AHRS_M2 msg: 1613173682.24 epoch sec; seqNo:6919 2021-02-12T23:48:02Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-12T23:48:02Z- handleDepth()-Depth_Keller msg: 1613173682257 epoch sec; seqNo:6976; depth 26.51 2021-02-12T23:48:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173682.27 epoch sec; seqNo:6976; 4057876.77 north; 591534.86 east 2021-02-12T23:48:02Z- AHRS_M2 msg: 1613173682.67 epoch sec; seqNo:6920 2021-02-12T23:48:02Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-12T23:48:02Z- handleDepth()-Depth_Keller msg: 1613173682676 epoch sec; seqNo:6977; depth 26.40 2021-02-12T23:48:02Z- handleDvl() - RDI_Pathfinder msg: 1613173682.69 epoch sec; seqNo:3232 2021-02-12T23:48:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 0.00 , 97.98 , 63.75 2021-02-12T23:48:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.06 , 0.03 2021-02-12T23:48:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173682.72 epoch sec; seqNo:6977; 4057877.15 north; 591534.85 east 2021-02-12T23:48:03Z- AHRS_M2 msg: 1613173683.05 epoch sec; seqNo:6921 2021-02-12T23:48:03Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-12T23:48:03Z- handleDepth()-Depth_Keller msg: 1613173683056 epoch sec; seqNo:6978; depth 26.28 2021-02-12T23:48:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173683.08 epoch sec; seqNo:6978; 4057877.54 north; 591536.07 east 2021-02-12T23:48:03Z- AHRS_M2 msg: 1613173683.46 epoch sec; seqNo:6922 2021-02-12T23:48:03Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.17 psi 2021-02-12T23:48:03Z- handleDepth()-Depth_Keller msg: 1613173683461 epoch sec; seqNo:6979; depth 26.18 2021-02-12T23:48:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173683.47 epoch sec; seqNo:6979; 4057877.54 north; 591536.07 east 2021-02-12T23:48:03Z- AHRS_M2 msg: 1613173683.86 epoch sec; seqNo:6923 2021-02-12T23:48:03Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.17 psi 2021-02-12T23:48:03Z- handleDepth()-Depth_Keller msg: 1613173683873 epoch sec; seqNo:6980; depth 26.07 2021-02-12T23:48:03Z- handleDvl() - RDI_Pathfinder msg: 1613173683.89 epoch sec; seqNo:3233 2021-02-12T23:48:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:48:03Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:48:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173683.90 epoch sec; seqNo:6980; 4057877.92 north; 591536.07 east 2021-02-12T23:48:04Z- AHRS_M2 msg: 1613173684.27 epoch sec; seqNo:6924 2021-02-12T23:48:04Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.17 psi 2021-02-12T23:48:04Z- handleDepth()-Depth_Keller msg: 1613173684279 epoch sec; seqNo:6981; depth 25.96 2021-02-12T23:48:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173684.29 epoch sec; seqNo:6981; 4057878.30 north; 591536.06 east 2021-02-12T23:48:04Z- LcmTrn::handleMessages() - handling 40 messages took 0.79 ms and 124 calls... 2021-02-12T23:48:04Z- waiting for 1613173682.48, time is 1613173684.29 2021-02-12T23:48:04Z- LcmTrn::updateTrn() >>>> pose time 1613173684.29, meas time 1613173683.89 <<<< 2021-02-12T23:48:04Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173683.89, ping # 0. 2021-02-12T23:48:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173683.89 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:48:04Z- TerrainNav::motionUpdate() - currEstimate = (4057878, 591536.06, 25.96) 2021-02-12T23:48:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:48:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:48:04Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:48:04Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:48:04Z- TNavBF::Projecting Measurements 2021-02-12T23:48:04Z- TNavFilter::Measurement from time = 1613173683.89 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:48:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:48:04Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:48:04Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:48:04Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:48:04Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:48:04Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:48:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:48:04Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 736.2 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:48:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:48:04Z- TNavBF::mmse selected from filter -1 2021-02-12T23:48:04Z- variances([0], [2], and psi_berg) = 736.152204 833.021220 0.000000 2021-02-12T23:48:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:48:04Z- LcmTrn::publishEstimates() - MLE : 4057868.80 591497.56 25.96 2021-02-12T23:48:04Z- LcmTrn::publishEstimates() - MMSE : 4057878.30 591536.06 25.96 2021-02-12T23:48:04Z- LcmTrn::publishEstimates() - COVAR: 736.15 833.02 1.17 0.00 2021-02-12T23:48:04Z- AHRS_M2 msg: 1613173684.67 epoch sec; seqNo:6925 2021-02-12T23:48:04Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-12T23:48:04Z- handleDepth()-Depth_Keller msg: 1613173684678 epoch sec; seqNo:6982; depth 25.82 2021-02-12T23:48:04Z- handleDvl() - RDI_Pathfinder msg: 1613173684.70 epoch sec; seqNo:3234 2021-02-12T23:48:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 64.85 2021-02-12T23:48:04Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:48:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173684.73 epoch sec; seqNo:6982; 4057878.68 north; 591536.06 east 2021-02-12T23:48:05Z- AHRS_M2 msg: 1613173685.08 epoch sec; seqNo:6926 2021-02-12T23:48:05Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.19 psi 2021-02-12T23:48:05Z- handleDepth()-Depth_Keller msg: 1613173685081 epoch sec; seqNo:6983; depth 25.73 2021-02-12T23:48:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:48:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:48:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:48:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:48:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:48:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:48:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:48:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:48:05Z- handleDvl() - RDI_Pathfinder msg: 1613173685.08 epoch sec; seqNo:3235 2021-02-12T23:48:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:48:05Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:48:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173685.09 epoch sec; seqNo:6983; 4057878.69 north; 591537.28 east 2021-02-12T23:48:05Z- AHRS_M2 msg: 1613173685.48 epoch sec; seqNo:6927 2021-02-12T23:48:05Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.19 psi 2021-02-12T23:48:05Z- handleDepth()-Depth_Keller msg: 1613173685493 epoch sec; seqNo:6984; depth 25.61 2021-02-12T23:48:05Z- handleDvl() - RDI_Pathfinder msg: 1613173685.52 epoch sec; seqNo:3236 2021-02-12T23:48:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 65.31 2021-02-12T23:48:05Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:48:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173685.55 epoch sec; seqNo:6984; 4057879.07 north; 591537.27 east 2021-02-12T23:48:05Z- AHRS_M2 msg: 1613173685.88 epoch sec; seqNo:6928 2021-02-12T23:48:05Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-12T23:48:05Z- handleDepth()-Depth_Keller msg: 1613173685889 epoch sec; seqNo:6985; depth 25.49 2021-02-12T23:48:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173685.91 epoch sec; seqNo:6985; 4057879.45 north; 591537.27 east 2021-02-12T23:48:06Z- AHRS_M2 msg: 1613173686.28 epoch sec; seqNo:6929 2021-02-12T23:48:06Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-12T23:48:06Z- handleDepth()-Depth_Keller msg: 1613173686288 epoch sec; seqNo:6986; depth 25.38 2021-02-12T23:48:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:48:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:48:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:48:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:48:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:48:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:48:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:48:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:48:06Z- handleDvl() - RDI_Pathfinder msg: 1613173686.29 epoch sec; seqNo:3237 2021-02-12T23:48:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:48:06Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:48:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173686.30 epoch sec; seqNo:6986; 4057879.45 north; 591537.27 east 2021-02-12T23:48:06Z- AHRS_M2 msg: 1613173686.69 epoch sec; seqNo:6930 2021-02-12T23:48:06Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-12T23:48:06Z- handleDepth()-Depth_Keller msg: 1613173686701 epoch sec; seqNo:6987; depth 25.27 2021-02-12T23:48:06Z- handleDvl() - RDI_Pathfinder msg: 1613173686.71 epoch sec; seqNo:3238 2021-02-12T23:48:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 64.85 2021-02-12T23:48:06Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:48:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173686.72 epoch sec; seqNo:6987; 4057879.83 north; 591537.27 east 2021-02-12T23:48:07Z- AHRS_M2 msg: 1613173687.09 epoch sec; seqNo:6931 2021-02-12T23:48:07Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-12T23:48:07Z- handleDepth()-Depth_Keller msg: 1613173687106 epoch sec; seqNo:6988; depth 25.14 2021-02-12T23:48:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173687.12 epoch sec; seqNo:6988; 4057880.21 north; 591537.26 east 2021-02-12T23:48:07Z- AHRS_M2 msg: 1613173687.50 epoch sec; seqNo:6932 2021-02-12T23:48:07Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.18 psi 2021-02-12T23:48:07Z- handleDepth()-Depth_Keller msg: 1613173687505 epoch sec; seqNo:6989; depth 25.02 2021-02-12T23:48:07Z- handleDvl() - RDI_Pathfinder msg: 1613173687.53 epoch sec; seqNo:3239 2021-02-12T23:48:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:48:07Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:48:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173687.54 epoch sec; seqNo:6989; 4057880.60 north; 591538.48 east 2021-02-12T23:48:07Z- AHRS_M2 msg: 1613173687.91 epoch sec; seqNo:6933 2021-02-12T23:48:07Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-12T23:48:07Z- handleDepth()-Depth_Keller msg: 1613173687910 epoch sec; seqNo:6990; depth 24.91 2021-02-12T23:48:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173687.92 epoch sec; seqNo:6990; 4057880.60 north; 591538.48 east 2021-02-12T23:48:08Z- AHRS_M2 msg: 1613173688.30 epoch sec; seqNo:6934 2021-02-12T23:48:08Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-12T23:48:08Z- handleDepth()-Depth_Keller msg: 1613173688313 epoch sec; seqNo:6991; depth 24.79 2021-02-12T23:48:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173688.33 epoch sec; seqNo:6991; 4057880.98 north; 591538.47 east 2021-02-12T23:48:08Z- AHRS_M2 msg: 1613173688.71 epoch sec; seqNo:6935 2021-02-12T23:48:08Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-12T23:48:08Z- handleDepth()-Depth_Keller msg: 1613173688713 epoch sec; seqNo:6992; depth 24.66 2021-02-12T23:48:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:48:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:48:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:48:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:48:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:48:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:48:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:48:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:48:08Z- handleDvl() - RDI_Pathfinder msg: 1613173688.71 epoch sec; seqNo:3240 2021-02-12T23:48:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:48:08Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:48:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173688.72 epoch sec; seqNo:6992; 4057881.36 north; 591538.47 east 2021-02-12T23:48:09Z- AHRS_M2 msg: 1613173689.11 epoch sec; seqNo:6936 2021-02-12T23:48:09Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.14 psi 2021-02-12T23:48:09Z- handleDepth()-Depth_Keller msg: 1613173689125 epoch sec; seqNo:6993; depth 24.55 2021-02-12T23:48:09Z- handleDvl() - RDI_Pathfinder msg: 1613173689.14 epoch sec; seqNo:3241 2021-02-12T23:48:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 88.25 , 99.06 , 65.39 2021-02-12T23:48:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , -0.02 2021-02-12T23:48:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173689.17 epoch sec; seqNo:6993; 4057881.73 north; 591538.47 east 2021-02-12T23:48:09Z- AHRS_M2 msg: 1613173689.52 epoch sec; seqNo:6937 2021-02-12T23:48:09Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.14 psi 2021-02-12T23:48:09Z- handleDepth()-Depth_Keller msg: 1613173689523 epoch sec; seqNo:6994; depth 24.41 2021-02-12T23:48:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173689.53 epoch sec; seqNo:6994; 4057881.73 north; 591538.47 east 2021-02-12T23:48:09Z- LcmTrn::handleMessages() - handling 47 messages took 0.82 ms and 131 calls... 2021-02-12T23:48:09Z- waiting for 1613173687.29, time is 1613173689.53 2021-02-12T23:48:09Z- LcmTrn::updateTrn() >>>> pose time 1613173689.53, meas time 1613173689.14 <<<< 2021-02-12T23:48:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173689.14 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:48:09Z- TerrainNav::motionUpdate() - currEstimate = (4057882, 591538.47, 24.41) 2021-02-12T23:48:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:48:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:48:09Z- TNavBF::Projecting Measurements 2021-02-12T23:48:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:48:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:48:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:48:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:48:09Z- TNavFilter::4 good beams. 2021-02-12T23:48:09Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:48:09Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:48:09Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:48:09Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:48:09Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:48:09Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:48:09Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:48:09Z- TNavBF::mmse selected from filter -1 2021-02-12T23:48:09Z- TerrainMapOctree: (vehN, vehE ) = (4057881.48, 591538.29). 2021-02-12T23:48:09Z- TerrainMapOctree: Min Distance = 4324.39. 2021-02-12T23:48:09Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:48:09Z- There is no specific good beam that all particles have in common. 2021-02-12T23:48:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:48:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-12T23:48:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:48:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 12.46 2021-02-12T23:48:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:48:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 15.70 2021-02-12T23:48:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:48:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.110 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-12T23:48:09Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:48:09Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:48:09Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:48:09Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:48:09Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:48:09Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:48:09Z- Filter 0 No beams used in subcloud update 2021-02-12T23:48:09Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:48:09Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:48:09Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 736.1 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:48:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173689.14 sec. 2021-02-12T23:48:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:48:09Z- TNavBF::mmse selected from filter -1 2021-02-12T23:48:09Z- variances([0], [2], and psi_berg) = 736.048971 833.021139 0.000000 2021-02-12T23:48:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:48:09Z- LcmTrn::publishEstimates() - MLE : 4057872.23 591499.97 24.41 2021-02-12T23:48:09Z- LcmTrn::publishEstimates() - MMSE : 4057881.73 591538.47 24.41 2021-02-12T23:48:09Z- LcmTrn::publishEstimates() - COVAR: 736.05 833.02 0.96 0.00 2021-02-12T23:48:09Z- AHRS_M2 msg: 1613173689.92 epoch sec; seqNo:6938 2021-02-12T23:48:09Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-12T23:48:09Z- handleDepth()-Depth_Keller msg: 1613173689925 epoch sec; seqNo:6995; depth 24.30 2021-02-12T23:48:09Z- handleDvl() - RDI_Pathfinder msg: 1613173689.95 epoch sec; seqNo:3242 2021-02-12T23:48:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 87.43 , 99.60 , 65.80 2021-02-12T23:48:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.09 , 0.01 2021-02-12T23:48:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173689.96 epoch sec; seqNo:6995; 4057882.13 north; 591539.68 east 2021-02-12T23:48:10Z- AHRS_M2 msg: 1613173690.32 epoch sec; seqNo:6939 2021-02-12T23:48:10Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-12T23:48:10Z- handleDepth()-Depth_Keller msg: 1613173690333 epoch sec; seqNo:6996; depth 24.18 2021-02-12T23:48:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173690.35 epoch sec; seqNo:6996; 4057882.51 north; 591539.68 east 2021-02-12T23:48:10Z- AHRS_M2 msg: 1613173690.73 epoch sec; seqNo:6940 2021-02-12T23:48:10Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-12T23:48:10Z- handleDepth()-Depth_Keller msg: 1613173690741 epoch sec; seqNo:6997; depth 24.06 2021-02-12T23:48:10Z- handleDvl() - RDI_Pathfinder msg: 1613173690.75 epoch sec; seqNo:3243 2021-02-12T23:48:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 0.00 , 101.09 , 65.79 2021-02-12T23:48:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , 0.03 2021-02-12T23:48:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173690.76 epoch sec; seqNo:6997; 4057882.51 north; 591539.68 east 2021-02-12T23:48:11Z- AHRS_M2 msg: 1613173691.13 epoch sec; seqNo:6941 2021-02-12T23:48:11Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-12T23:48:11Z- handleDepth()-Depth_Keller msg: 1613173691147 epoch sec; seqNo:6998; depth 23.93 2021-02-12T23:48:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:48:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:48:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:48:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:48:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:48:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:48:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:48:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:48:11Z- handleDvl() - RDI_Pathfinder msg: 1613173691.15 epoch sec; seqNo:3244 2021-02-12T23:48:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:48:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , 0.03 2021-02-12T23:48:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173691.16 epoch sec; seqNo:6998; 4057882.88 north; 591539.68 east 2021-02-12T23:48:11Z- AHRS_M2 msg: 1613173691.54 epoch sec; seqNo:6942 2021-02-12T23:48:11Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-12T23:48:11Z- handleDepth()-Depth_Keller msg: 1613173691541 epoch sec; seqNo:6999; depth 23.82 2021-02-12T23:48:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173691.55 epoch sec; seqNo:6999; 4057883.26 north; 591539.67 east 2021-02-12T23:48:11Z- AHRS_M2 msg: 1613173691.94 epoch sec; seqNo:6943 2021-02-12T23:48:11Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-12T23:48:11Z- handleDepth()-Depth_Keller msg: 1613173691949 epoch sec; seqNo:7000; depth 23.69 2021-02-12T23:48:11Z- handleDvl() - RDI_Pathfinder msg: 1613173691.96 epoch sec; seqNo:3245 2021-02-12T23:48:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.16 , 0.00 , 99.00 , 65.79 2021-02-12T23:48:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.70 , 0.06 2021-02-12T23:48:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173691.98 epoch sec; seqNo:7000; 4057883.64 north; 591539.67 east 2021-02-12T23:48:12Z- AHRS_M2 msg: 1613173692.34 epoch sec; seqNo:6944 2021-02-12T23:48:12Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-12T23:48:12Z- handleDepth()-Depth_Keller msg: 1613173692351 epoch sec; seqNo:7001; depth 23.57 2021-02-12T23:48:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:48:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:48:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:48:12Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:48:12Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:48:12Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:48:12Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:48:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:48:12Z- handleDvl() - RDI_Pathfinder msg: 1613173692.35 epoch sec; seqNo:3246 2021-02-12T23:48:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:48:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.70 , 0.06 2021-02-12T23:48:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173692.37 epoch sec; seqNo:7001; 4057884.02 north; 591539.66 east 2021-02-12T23:48:12Z- AHRS_M2 msg: 1613173692.75 epoch sec; seqNo:6945 2021-02-12T23:48:12Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-12T23:48:12Z- handleDepth()-Depth_Keller msg: 1613173692755 epoch sec; seqNo:7002; depth 23.46 2021-02-12T23:48:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173692.77 epoch sec; seqNo:7002; 4057884.40 north; 591539.66 east 2021-02-12T23:48:13Z- AHRS_M2 msg: 1613173693.15 epoch sec; seqNo:6946 2021-02-12T23:48:13Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-12T23:48:13Z- handleDepth()-Depth_Keller msg: 1613173693159 epoch sec; seqNo:7003; depth 23.33 2021-02-12T23:48:13Z- handleDvl() - RDI_Pathfinder msg: 1613173693.17 epoch sec; seqNo:3247 2021-02-12T23:48:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:48:13Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:48:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173693.20 epoch sec; seqNo:7003; 4057885.16 north; 591539.65 east 2021-02-12T23:48:13Z- AHRS_M2 msg: 1613173693.56 epoch sec; seqNo:6947 2021-02-12T23:48:13Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-12T23:48:13Z- handleDepth()-Depth_Keller msg: 1613173693625 epoch sec; seqNo:7004; depth 23.20 2021-02-12T23:48:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:48:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:48:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:48:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:48:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:48:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:48:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:48:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:48:13Z- handleDvl() - RDI_Pathfinder msg: 1613173693.63 epoch sec; seqNo:3248 2021-02-12T23:48:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:48:13Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:48:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173693.64 epoch sec; seqNo:7004; 4057885.54 north; 591539.65 east 2021-02-12T23:48:13Z- AHRS_M2 msg: 1613173693.96 epoch sec; seqNo:6948 2021-02-12T23:48:13Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-12T23:48:14Z- handleDepth()-Depth_Keller msg: 1613173694041 epoch sec; seqNo:7005; depth 23.08 2021-02-12T23:48:14Z- handleDvl() - RDI_Pathfinder msg: 1613173694.05 epoch sec; seqNo:3249 2021-02-12T23:48:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 97.04 , 101.09 , 67.02 2021-02-12T23:48:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , 0.09 2021-02-12T23:48:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173694.06 epoch sec; seqNo:7005; 4057885.91 north; 591539.64 east 2021-02-12T23:48:14Z- AHRS_M2 msg: 1613173694.36 epoch sec; seqNo:6949 2021-02-12T23:48:14Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-12T23:48:14Z- handleDepth()-Depth_Keller msg: 1613173694397 epoch sec; seqNo:7006; depth 22.94 2021-02-12T23:48:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173694.43 epoch sec; seqNo:7006; 4057885.91 north; 591539.64 east 2021-02-12T23:48:14Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-12T23:48:14Z- waiting for 1613173692.53, time is 1613173694.43 2021-02-12T23:48:14Z- LcmTrn::updateTrn() >>>> pose time 1613173694.43, meas time 1613173694.05 <<<< 2021-02-12T23:48:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173694.05 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:48:14Z- TerrainNav::motionUpdate() - currEstimate = (4057886, 591539.64, 22.94) 2021-02-12T23:48:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:48:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:48:14Z- TNavBF::Projecting Measurements 2021-02-12T23:48:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:48:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:48:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:48:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:48:14Z- TNavFilter::4 good beams. 2021-02-12T23:48:14Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:48:14Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:48:14Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:48:14Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:48:14Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:48:14Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:48:14Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:48:14Z- TNavBF::mmse selected from filter -1 2021-02-12T23:48:14Z- TerrainMapOctree: (vehN, vehE ) = (4057885.60, 591539.55). 2021-02-12T23:48:14Z- TerrainMapOctree: Min Distance = 4320.39. 2021-02-12T23:48:14Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:48:14Z- There is no specific good beam that all particles have in common. 2021-02-12T23:48:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:48:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 8.75 2021-02-12T23:48:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:48:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 15.07 2021-02-12T23:48:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:48:14Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 16.35 2021-02-12T23:48:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:48:14Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 7.19 2021-02-12T23:48:14Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:48:14Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:48:14Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:48:14Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:48:14Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:48:14Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:48:14Z- Filter 0 No beams used in subcloud update 2021-02-12T23:48:14Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:48:14Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:48:14Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 736.0 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:48:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173694.05 sec. 2021-02-12T23:48:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:48:14Z- TNavBF::mmse selected from filter -1 2021-02-12T23:48:14Z- variances([0], [2], and psi_berg) = 735.943913 833.021057 0.000000 2021-02-12T23:48:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:48:14Z- LcmTrn::publishEstimates() - MLE : 4057876.41 591501.14 22.94 2021-02-12T23:48:14Z- LcmTrn::publishEstimates() - MMSE : 4057885.91 591539.64 22.94 2021-02-12T23:48:14Z- LcmTrn::publishEstimates() - COVAR: 735.94 833.02 0.86 0.00 2021-02-12T23:48:14Z- AHRS_M2 msg: 1613173694.79 epoch sec; seqNo:6950 2021-02-12T23:48:14Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-12T23:48:14Z- handleDepth()-Depth_Keller msg: 1613173694833 epoch sec; seqNo:7007; depth 22.82 2021-02-12T23:48:14Z- handleDvl() - RDI_Pathfinder msg: 1613173694.85 epoch sec; seqNo:3250 2021-02-12T23:48:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 0.00 , 101.84 , 128.61 2021-02-12T23:48:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.54 , 0.35 , -0.12 2021-02-12T23:48:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173694.86 epoch sec; seqNo:7007; 4057885.93 north; 591540.86 east 2021-02-12T23:48:15Z- AHRS_M2 msg: 1613173695.17 epoch sec; seqNo:6951 2021-02-12T23:48:15Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-12T23:48:15Z- handleDepth()-Depth_Keller msg: 1613173695197 epoch sec; seqNo:7008; depth 22.70 2021-02-12T23:48:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173695.21 epoch sec; seqNo:7008; 4057886.31 north; 591540.86 east 2021-02-12T23:48:15Z- AHRS_M2 msg: 1613173695.58 epoch sec; seqNo:6952 2021-02-12T23:48:15Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-12T23:48:15Z- handleDepth()-Depth_Keller msg: 1613173695582 epoch sec; seqNo:7009; depth 22.60 2021-02-12T23:48:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173695.59 epoch sec; seqNo:7009; 4057886.31 north; 591540.86 east 2021-02-12T23:48:15Z- AHRS_M2 msg: 1613173695.98 epoch sec; seqNo:6953 2021-02-12T23:48:15Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-12T23:48:15Z- handleDepth()-Depth_Keller msg: 1613173695994 epoch sec; seqNo:7010; depth 22.48 2021-02-12T23:48:16Z- handleDvl() - RDI_Pathfinder msg: 1613173696.01 epoch sec; seqNo:3251 2021-02-12T23:48:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.85 , 103.80 , 68.65 2021-02-12T23:48:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , 0.06 2021-02-12T23:48:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173696.02 epoch sec; seqNo:7010; 4057886.69 north; 591540.86 east 2021-02-12T23:48:16Z- AHRS_M2 msg: 1613173696.38 epoch sec; seqNo:6954 2021-02-12T23:48:16Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-12T23:48:16Z- handleDepth()-Depth_Keller msg: 1613173696393 epoch sec; seqNo:7011; depth 22.35 2021-02-12T23:48:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173696.40 epoch sec; seqNo:7011; 4057886.69 north; 591540.86 east 2021-02-12T23:48:16Z- AHRS_M2 msg: 1613173696.79 epoch sec; seqNo:6955 2021-02-12T23:48:16Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.14 psi 2021-02-12T23:48:16Z- handleDepth()-Depth_Keller msg: 1613173696797 epoch sec; seqNo:7012; depth 22.23 2021-02-12T23:48:16Z- handleDvl() - RDI_Pathfinder msg: 1613173696.81 epoch sec; seqNo:3252 2021-02-12T23:48:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 89.47 , 102.85 , 66.61 2021-02-12T23:48:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , 0.02 2021-02-12T23:48:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173696.82 epoch sec; seqNo:7012; 4057887.06 north; 591540.85 east 2021-02-12T23:48:17Z- AHRS_M2 msg: 1613173697.19 epoch sec; seqNo:6956 2021-02-12T23:48:17Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.14 psi 2021-02-12T23:48:17Z- handleDepth()-Depth_Keller msg: 1613173697204 epoch sec; seqNo:7013; depth 22.10 2021-02-12T23:48:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173697.21 epoch sec; seqNo:7013; 4057887.46 north; 591542.07 east 2021-02-12T23:48:17Z- AHRS_M2 msg: 1613173697.60 epoch sec; seqNo:6957 2021-02-12T23:48:17Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.14 psi 2021-02-12T23:48:17Z- handleDepth()-Depth_Keller msg: 1613173697601 epoch sec; seqNo:7014; depth 21.99 2021-02-12T23:48:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173697.61 epoch sec; seqNo:7014; 4057887.46 north; 591542.07 east 2021-02-12T23:48:18Z- AHRS_M2 msg: 1613173698.00 epoch sec; seqNo:6958 2021-02-12T23:48:18Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.14 psi 2021-02-12T23:48:18Z- handleDepth()-Depth_Keller msg: 1613173698008 epoch sec; seqNo:7015; depth 21.86 2021-02-12T23:48:18Z- handleDvl() - RDI_Pathfinder msg: 1613173698.02 epoch sec; seqNo:3253 2021-02-12T23:48:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 90.75 , 104.14 , 78.58 2021-02-12T23:48:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.06 , -0.02 2021-02-12T23:48:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173698.04 epoch sec; seqNo:7015; 4057887.83 north; 591542.06 east 2021-02-12T23:48:18Z- AHRS_M2 msg: 1613173698.40 epoch sec; seqNo:6959 2021-02-12T23:48:18Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.14 psi 2021-02-12T23:48:18Z- handleDepth()-Depth_Keller msg: 1613173698417 epoch sec; seqNo:7016; depth 21.74 2021-02-12T23:48:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173698.43 epoch sec; seqNo:7016; 4057888.21 north; 591542.06 east 2021-02-12T23:48:18Z- AHRS_M2 msg: 1613173698.81 epoch sec; seqNo:6960 2021-02-12T23:48:18Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.14 psi 2021-02-12T23:48:18Z- handleDepth()-Depth_Keller msg: 1613173698813 epoch sec; seqNo:7017; depth 21.62 2021-02-12T23:48:18Z- handleDvl() - RDI_Pathfinder msg: 1613173698.83 epoch sec; seqNo:3254 2021-02-12T23:48:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 97.92 , 104.00 , 69.47 2021-02-12T23:48:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.07 , -0.05 2021-02-12T23:48:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173698.84 epoch sec; seqNo:7017; 4057888.21 north; 591542.06 east 2021-02-12T23:48:19Z- AHRS_M2 msg: 1613173699.21 epoch sec; seqNo:6961 2021-02-12T23:48:19Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.14 psi 2021-02-12T23:48:19Z- handleDepth()-Depth_Keller msg: 1613173699217 epoch sec; seqNo:7018; depth 21.50 2021-02-12T23:48:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173699.23 epoch sec; seqNo:7018; 4057888.59 north; 591542.06 east 2021-02-12T23:48:19Z- AHRS_M2 msg: 1613173699.62 epoch sec; seqNo:6962 2021-02-12T23:48:19Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.15 psi 2021-02-12T23:48:19Z- handleDepth()-Depth_Keller msg: 1613173699621 epoch sec; seqNo:7019; depth 21.37 2021-02-12T23:48:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173699.63 epoch sec; seqNo:7019; 4057888.98 north; 591543.27 east 2021-02-12T23:48:19Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-12T23:48:19Z- waiting for 1613173697.43, time is 1613173699.63 2021-02-12T23:48:19Z- LcmTrn::updateTrn() >>>> pose time 1613173699.63, meas time 1613173698.83 <<<< 2021-02-12T23:48:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173698.83 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:48:19Z- TerrainNav::motionUpdate() - currEstimate = (4057889, 591543.27, 21.37) 2021-02-12T23:48:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:48:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:48:19Z- TNavBF::Projecting Measurements 2021-02-12T23:48:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:48:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:48:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:48:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:48:19Z- TNavFilter::4 good beams. 2021-02-12T23:48:19Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:48:19Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:48:19Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:48:19Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:48:19Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:48:19Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:48:19Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:48:19Z- TNavBF::mmse selected from filter -1 2021-02-12T23:48:19Z- TerrainMapOctree: (vehN, vehE ) = (4057888.51, 591542.72). 2021-02-12T23:48:19Z- TerrainMapOctree: Min Distance = 4316.13. 2021-02-12T23:48:19Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:48:19Z- There is no specific good beam that all particles have in common. 2021-02-12T23:48:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:48:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 9.44 2021-02-12T23:48:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:48:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-12T23:48:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:48:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 17.31 2021-02-12T23:48:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:48:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 7.72 2021-02-12T23:48:19Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:48:19Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:48:19Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:48:19Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:48:19Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:48:19Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:48:19Z- Filter 0 No beams used in subcloud update 2021-02-12T23:48:19Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:48:19Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:48:19Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 735.9 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:48:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173698.83 sec. 2021-02-12T23:48:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:48:19Z- TNavBF::mmse selected from filter -1 2021-02-12T23:48:19Z- variances([0], [2], and psi_berg) = 735.827551 833.020966 0.000000 2021-02-12T23:48:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:48:19Z- LcmTrn::publishEstimates() - MLE : 4057879.48 591504.77 21.37 2021-02-12T23:48:19Z- LcmTrn::publishEstimates() - MMSE : 4057888.98 591543.27 21.37 2021-02-12T23:48:19Z- LcmTrn::publishEstimates() - COVAR: 735.83 833.02 0.84 0.00 2021-02-12T23:48:20Z- AHRS_M2 msg: 1613173700.02 epoch sec; seqNo:6963 2021-02-12T23:48:20Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.15 psi 2021-02-12T23:48:20Z- handleDepth()-Depth_Keller msg: 1613173700033 epoch sec; seqNo:7020; depth 21.25 2021-02-12T23:48:20Z- handleDvl() - RDI_Pathfinder msg: 1613173700.05 epoch sec; seqNo:3255 2021-02-12T23:48:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 97.98 , 101.63 , 69.06 2021-02-12T23:48:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.05 , -0.07 2021-02-12T23:48:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173700.07 epoch sec; seqNo:7020; 4057888.98 north; 591543.27 east 2021-02-12T23:48:20Z- AHRS_M2 msg: 1613173700.42 epoch sec; seqNo:6964 2021-02-12T23:48:20Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.16 psi 2021-02-12T23:48:20Z- handleDepth()-Depth_Keller msg: 1613173700438 epoch sec; seqNo:7021; depth 21.13 2021-02-12T23:48:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173700.45 epoch sec; seqNo:7021; 4057889.36 north; 591543.27 east 2021-02-12T23:48:20Z- AHRS_M2 msg: 1613173700.85 epoch sec; seqNo:6965 2021-02-12T23:48:20Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.16 psi 2021-02-12T23:48:20Z- handleDepth()-Depth_Keller msg: 1613173700859 epoch sec; seqNo:7022; depth 20.99 2021-02-12T23:48:20Z- handleDvl() - RDI_Pathfinder msg: 1613173700.87 epoch sec; seqNo:3256 2021-02-12T23:48:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 102.92 , 67.84 2021-02-12T23:48:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.01 , -0.04 2021-02-12T23:48:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173700.88 epoch sec; seqNo:7022; 4057889.74 north; 591543.26 east 2021-02-12T23:48:21Z- AHRS_M2 msg: 1613173701.23 epoch sec; seqNo:6966 2021-02-12T23:48:21Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.17 psi 2021-02-12T23:48:21Z- handleDepth()-Depth_Keller msg: 1613173701249 epoch sec; seqNo:7023; depth 20.87 2021-02-12T23:48:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173701.28 epoch sec; seqNo:7023; 4057889.74 north; 591543.26 east 2021-02-12T23:48:21Z- AHRS_M2 msg: 1613173701.64 epoch sec; seqNo:6967 2021-02-12T23:48:21Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.18 psi 2021-02-12T23:48:21Z- handleDepth()-Depth_Keller msg: 1613173701647 epoch sec; seqNo:7024; depth 20.74 2021-02-12T23:48:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173701.65 epoch sec; seqNo:7024; 4057890.12 north; 591543.26 east 2021-02-12T23:48:22Z- AHRS_M2 msg: 1613173702.05 epoch sec; seqNo:6968 2021-02-12T23:48:22Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.21 psi 2021-02-12T23:48:22Z- handleDepth()-Depth_Keller msg: 1613173702052 epoch sec; seqNo:7025; depth 20.63 2021-02-12T23:48:22Z- handleDvl() - RDI_Pathfinder msg: 1613173702.06 epoch sec; seqNo:3257 2021-02-12T23:48:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 104.14 , 69.06 2021-02-12T23:48:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , 0.01 2021-02-12T23:48:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173702.07 epoch sec; seqNo:7025; 4057890.13 north; 591544.48 east 2021-02-12T23:48:22Z- AHRS_M2 msg: 1613173702.44 epoch sec; seqNo:6969 2021-02-12T23:48:22Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.22 psi 2021-02-12T23:48:22Z- handleDepth()-Depth_Keller msg: 1613173702457 epoch sec; seqNo:7026; depth 20.50 2021-02-12T23:48:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173702.47 epoch sec; seqNo:7026; 4057890.51 north; 591544.48 east 2021-02-12T23:48:22Z- AHRS_M2 msg: 1613173702.86 epoch sec; seqNo:6970 2021-02-12T23:48:22Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.23 psi 2021-02-12T23:48:22Z- handleDepth()-Depth_Keller msg: 1613173702861 epoch sec; seqNo:7027; depth 20.38 2021-02-12T23:48:22Z- handleDvl() - RDI_Pathfinder msg: 1613173702.87 epoch sec; seqNo:3258 2021-02-12T23:48:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 105.28 , 69.47 2021-02-12T23:48:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , 0.07 2021-02-12T23:48:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173702.89 epoch sec; seqNo:7027; 4057890.89 north; 591544.47 east 2021-02-12T23:48:23Z- AHRS_M2 msg: 1613173703.25 epoch sec; seqNo:6971 2021-02-12T23:48:23Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.23 psi 2021-02-12T23:48:23Z- handleDepth()-Depth_Keller msg: 1613173703258 epoch sec; seqNo:7028; depth 20.25 2021-02-12T23:48:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173703.27 epoch sec; seqNo:7028; 4057891.27 north; 591544.47 east 2021-02-12T23:48:23Z- AHRS_M2 msg: 1613173703.66 epoch sec; seqNo:6972 2021-02-12T23:48:23Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.23 psi 2021-02-12T23:48:23Z- handleDepth()-Depth_Keller msg: 1613173703661 epoch sec; seqNo:7029; depth 20.14 2021-02-12T23:48:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173703.68 epoch sec; seqNo:7029; 4057891.27 north; 591544.47 east 2021-02-12T23:48:24Z- AHRS_M2 msg: 1613173704.06 epoch sec; seqNo:6973 2021-02-12T23:48:24Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.23 psi 2021-02-12T23:48:24Z- handleDepth()-Depth_Keller msg: 1613173704065 epoch sec; seqNo:7030; depth 20.02 2021-02-12T23:48:24Z- handleDvl() - RDI_Pathfinder msg: 1613173704.08 epoch sec; seqNo:3259 2021-02-12T23:48:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 97.92 , 105.22 , 70.29 2021-02-12T23:48:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , 0.09 2021-02-12T23:48:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173704.11 epoch sec; seqNo:7030; 4057891.65 north; 591544.47 east 2021-02-12T23:48:24Z- AHRS_M2 msg: 1613173704.46 epoch sec; seqNo:6974 2021-02-12T23:48:24Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.24 psi 2021-02-12T23:48:24Z- handleDepth()-Depth_Keller msg: 1613173704469 epoch sec; seqNo:7031; depth 19.91 2021-02-12T23:48:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173704.48 epoch sec; seqNo:7031; 4057892.03 north; 591544.46 east 2021-02-12T23:48:24Z- AHRS_M2 msg: 1613173704.87 epoch sec; seqNo:6975 2021-02-12T23:48:24Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.23 psi 2021-02-12T23:48:24Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-12T23:48:24Z- waiting for 1613173702.63, time is 1613173704.87 2021-02-12T23:48:24Z- LcmTrn::updateTrn() >>>> pose time 1613173704.87, meas time 1613173704.08 <<<< 2021-02-12T23:48:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173704.08 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:48:24Z- TerrainNav::motionUpdate() - currEstimate = (4057892, 591544.46, 19.91) 2021-02-12T23:48:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:48:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:48:24Z- TNavBF::Projecting Measurements 2021-02-12T23:48:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:48:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:48:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:48:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:48:24Z- TNavFilter::4 good beams. 2021-02-12T23:48:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:48:24Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:48:24Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:48:24Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:48:24Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:48:24Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:48:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:48:24Z- TNavBF::mmse selected from filter -1 2021-02-12T23:48:24Z- TerrainMapOctree: (vehN, vehE ) = (4057891.57, 591544.28). 2021-02-12T23:48:24Z- TerrainMapOctree: Min Distance = 4312.77. 2021-02-12T23:48:24Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:48:24Z- There is no specific good beam that all particles have in common. 2021-02-12T23:48:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:48:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-12T23:48:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:48:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-12T23:48:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:48:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 17.71 2021-02-12T23:48:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:48:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 7.90 2021-02-12T23:48:24Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:48:24Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:48:24Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:48:24Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:48:24Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:48:24Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:48:24Z- Filter 0 No beams used in subcloud update 2021-02-12T23:48:24Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:48:24Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:48:24Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 735.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:48:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173704.08 sec. 2021-02-12T23:48:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:48:24Z- TNavBF::mmse selected from filter -1 2021-02-12T23:48:24Z- variances([0], [2], and psi_berg) = 735.731337 833.020891 0.000000 2021-02-12T23:48:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:48:24Z- LcmTrn::publishEstimates() - MLE : 4057883.53 591498.96 19.91 2021-02-12T23:48:24Z- LcmTrn::publishEstimates() - MMSE : 4057892.03 591544.46 19.91 2021-02-12T23:48:24Z- LcmTrn::publishEstimates() - COVAR: 735.73 833.02 0.78 0.00 2021-02-12T23:48:24Z- handleDepth()-Depth_Keller msg: 1613173704881 epoch sec; seqNo:7032; depth 19.80 2021-02-12T23:48:24Z- handleDvl() - RDI_Pathfinder msg: 1613173704.89 epoch sec; seqNo:3260 2021-02-12T23:48:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 97.92 , 106.43 , 71.92 2021-02-12T23:48:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , 0.05 2021-02-12T23:48:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173704.90 epoch sec; seqNo:7032; 4057892.04 north; 591545.68 east 2021-02-12T23:48:25Z- AHRS_M2 msg: 1613173705.27 epoch sec; seqNo:6976 2021-02-12T23:48:25Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.23 psi 2021-02-12T23:48:25Z- handleDepth()-Depth_Keller msg: 1613173705277 epoch sec; seqNo:7033; depth 19.69 2021-02-12T23:48:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173705.29 epoch sec; seqNo:7033; 4057892.42 north; 591545.68 east 2021-02-12T23:48:25Z- AHRS_M2 msg: 1613173705.68 epoch sec; seqNo:6977 2021-02-12T23:48:25Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.22 psi 2021-02-12T23:48:25Z- handleDepth()-Depth_Keller msg: 1613173705686 epoch sec; seqNo:7034; depth 19.57 2021-02-12T23:48:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173705.71 epoch sec; seqNo:7034; 4057892.80 north; 591545.67 east 2021-02-12T23:48:26Z- AHRS_M2 msg: 1613173706.08 epoch sec; seqNo:6978 2021-02-12T23:48:26Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.21 psi 2021-02-12T23:48:26Z- handleDepth()-Depth_Keller msg: 1613173706089 epoch sec; seqNo:7035; depth 19.47 2021-02-12T23:48:26Z- handleDvl() - RDI_Pathfinder msg: 1613173706.10 epoch sec; seqNo:3261 2021-02-12T23:48:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 99.07 , 107.58 , 78.38 2021-02-12T23:48:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.00 2021-02-12T23:48:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173706.12 epoch sec; seqNo:7035; 4057893.18 north; 591545.67 east 2021-02-12T23:48:26Z- AHRS_M2 msg: 1613173706.48 epoch sec; seqNo:6979 2021-02-12T23:48:26Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.20 psi 2021-02-12T23:48:26Z- handleDepth()-Depth_Keller msg: 1613173706497 epoch sec; seqNo:7036; depth 19.36 2021-02-12T23:48:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173706.51 epoch sec; seqNo:7036; 4057893.18 north; 591545.67 east 2021-02-12T23:48:26Z- AHRS_M2 msg: 1613173706.90 epoch sec; seqNo:6980 2021-02-12T23:48:26Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.19 psi 2021-02-12T23:48:26Z- handleDepth()-Depth_Keller msg: 1613173706921 epoch sec; seqNo:7037; depth 19.24 2021-02-12T23:48:26Z- handleDvl() - RDI_Pathfinder msg: 1613173706.93 epoch sec; seqNo:3262 2021-02-12T23:48:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.25 , 98.93 , 106.23 , 78.25 2021-02-12T23:48:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , -0.03 2021-02-12T23:48:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173706.95 epoch sec; seqNo:7037; 4057893.56 north; 591545.67 east 2021-02-12T23:48:27Z- AHRS_M2 msg: 1613173707.29 epoch sec; seqNo:6981 2021-02-12T23:48:27Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.18 psi 2021-02-12T23:48:27Z- handleDepth()-Depth_Keller msg: 1613173707419 epoch sec; seqNo:7038; depth 19.12 2021-02-12T23:48:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173707.43 epoch sec; seqNo:7038; 4057893.95 north; 591546.88 east 2021-02-12T23:48:27Z- AHRS_M2 msg: 1613173707.70 epoch sec; seqNo:6982 2021-02-12T23:48:27Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.18 psi 2021-02-12T23:48:27Z- handleDepth()-Depth_Keller msg: 1613173707716 epoch sec; seqNo:7039; depth 18.99 2021-02-12T23:48:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173707.72 epoch sec; seqNo:7039; 4057893.95 north; 591546.88 east 2021-02-12T23:48:28Z- AHRS_M2 msg: 1613173708.10 epoch sec; seqNo:6983 2021-02-12T23:48:28Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-12T23:48:28Z- handleDepth()-Depth_Keller msg: 1613173708133 epoch sec; seqNo:7040; depth 18.92 2021-02-12T23:48:28Z- handleDvl() - RDI_Pathfinder msg: 1613173708.15 epoch sec; seqNo:3263 2021-02-12T23:48:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.25 , 97.71 , 106.23 , 78.25 2021-02-12T23:48:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , -0.02 2021-02-12T23:48:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173708.17 epoch sec; seqNo:7040; 4057894.33 north; 591546.88 east 2021-02-12T23:48:28Z- AHRS_M2 msg: 1613173708.50 epoch sec; seqNo:6984 2021-02-12T23:48:28Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-12T23:48:28Z- handleDepth()-Depth_Keller msg: 1613173708545 epoch sec; seqNo:7041; depth 18.79 2021-02-12T23:48:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173708.56 epoch sec; seqNo:7041; 4057894.71 north; 591546.87 east 2021-02-12T23:48:28Z- AHRS_M2 msg: 1613173708.91 epoch sec; seqNo:6985 2021-02-12T23:48:28Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-12T23:48:28Z- handleDepth()-Depth_Keller msg: 1613173708913 epoch sec; seqNo:7042; depth 18.68 2021-02-12T23:48:28Z- handleDvl() - RDI_Pathfinder msg: 1613173708.92 epoch sec; seqNo:3264 2021-02-12T23:48:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 97.78 , 105.08 , 70.29 2021-02-12T23:48:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.10 , 0.02 2021-02-12T23:48:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173708.93 epoch sec; seqNo:7042; 4057895.09 north; 591546.87 east 2021-02-12T23:48:29Z- AHRS_M2 msg: 1613173709.31 epoch sec; seqNo:6986 2021-02-12T23:48:29Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.19 psi 2021-02-12T23:48:29Z- handleDepth()-Depth_Keller msg: 1613173709317 epoch sec; seqNo:7043; depth 18.58 2021-02-12T23:48:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173709.33 epoch sec; seqNo:7043; 4057895.09 north; 591546.87 east 2021-02-12T23:48:29Z- AHRS_M2 msg: 1613173709.72 epoch sec; seqNo:6987 2021-02-12T23:48:29Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.19 psi 2021-02-12T23:48:29Z- handleDepth()-Depth_Keller msg: 1613173709723 epoch sec; seqNo:7044; depth 18.47 2021-02-12T23:48:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173709.76 epoch sec; seqNo:7044; 4057895.46 north; 591546.87 east 2021-02-12T23:48:29Z- LcmTrn::handleMessages() - handling 43 messages took 0.75 ms and 128 calls... 2021-02-12T23:48:29Z- waiting for 1613173707.87, time is 1613173709.76 2021-02-12T23:48:29Z- LcmTrn::updateTrn() >>>> pose time 1613173709.76, meas time 1613173708.92 <<<< 2021-02-12T23:48:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173708.92 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:48:29Z- TerrainNav::motionUpdate() - currEstimate = (4057895, 591546.87, 18.47) 2021-02-12T23:48:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:48:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:48:29Z- TNavBF::Projecting Measurements 2021-02-12T23:48:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:48:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:48:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:48:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:48:29Z- TNavFilter::4 good beams. 2021-02-12T23:48:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:48:29Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:48:29Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:48:29Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:48:29Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:48:29Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:48:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:48:29Z- TNavBF::mmse selected from filter -1 2021-02-12T23:48:29Z- TerrainMapOctree: (vehN, vehE ) = (4057894.88, 591546.46). 2021-02-12T23:48:29Z- TerrainMapOctree: Min Distance = 4308.83. 2021-02-12T23:48:29Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:48:29Z- There is no specific good beam that all particles have in common. 2021-02-12T23:48:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:48:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 9.81 2021-02-12T23:48:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:48:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-12T23:48:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:48:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 17.67 2021-02-12T23:48:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:48:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 7.90 2021-02-12T23:48:30Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:48:30Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:48:30Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:48:30Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:48:30Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:48:30Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:48:30Z- Filter 0 No beams used in subcloud update 2021-02-12T23:48:30Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:48:30Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:48:30Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 735.7 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:48:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173708.92 sec. 2021-02-12T23:48:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:48:30Z- TNavBF::mmse selected from filter -1 2021-02-12T23:48:30Z- variances([0], [2], and psi_berg) = 735.621362 833.020805 0.000000 2021-02-12T23:48:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:48:30Z- LcmTrn::publishEstimates() - MLE : 4057886.96 591501.37 18.47 2021-02-12T23:48:30Z- LcmTrn::publishEstimates() - MMSE : 4057895.46 591546.87 18.47 2021-02-12T23:48:30Z- LcmTrn::publishEstimates() - COVAR: 735.62 833.02 0.78 0.00 2021-02-12T23:48:30Z- AHRS_M2 msg: 1613173710.12 epoch sec; seqNo:6988 2021-02-12T23:48:30Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.19 psi 2021-02-12T23:48:30Z- handleDepth()-Depth_Keller msg: 1613173710128 epoch sec; seqNo:7045; depth 18.37 2021-02-12T23:48:30Z- handleDvl() - RDI_Pathfinder msg: 1613173710.15 epoch sec; seqNo:3265 2021-02-12T23:48:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.67 , 97.92 , 106.43 , 78.45 2021-02-12T23:48:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , 0.05 2021-02-12T23:48:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173710.16 epoch sec; seqNo:7045; 4057895.84 north; 591546.86 east 2021-02-12T23:48:30Z- AHRS_M2 msg: 1613173710.53 epoch sec; seqNo:6989 2021-02-12T23:48:30Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-12T23:48:30Z- handleDepth()-Depth_Keller msg: 1613173710531 epoch sec; seqNo:7046; depth 18.26 2021-02-12T23:48:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173710.54 epoch sec; seqNo:7046; 4057896.23 north; 591548.08 east 2021-02-12T23:48:30Z- AHRS_M2 msg: 1613173710.93 epoch sec; seqNo:6990 2021-02-12T23:48:30Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-12T23:48:30Z- handleDepth()-Depth_Keller msg: 1613173710938 epoch sec; seqNo:7047; depth 18.15 2021-02-12T23:48:30Z- handleDvl() - RDI_Pathfinder msg: 1613173710.95 epoch sec; seqNo:3266 2021-02-12T23:48:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.46 , 97.71 , 106.23 , 78.25 2021-02-12T23:48:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.05 2021-02-12T23:48:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173710.96 epoch sec; seqNo:7047; 4057896.61 north; 591548.08 east 2021-02-12T23:48:31Z- AHRS_M2 msg: 1613173711.34 epoch sec; seqNo:6991 2021-02-12T23:48:31Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-12T23:48:31Z- handleDepth()-Depth_Keller msg: 1613173711349 epoch sec; seqNo:7048; depth 18.05 2021-02-12T23:48:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173711.36 epoch sec; seqNo:7048; 4057896.61 north; 591548.08 east 2021-02-12T23:48:31Z- AHRS_M2 msg: 1613173711.74 epoch sec; seqNo:6992 2021-02-12T23:48:31Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.17 psi 2021-02-12T23:48:31Z- handleDepth()-Depth_Keller msg: 1613173711741 epoch sec; seqNo:7049; depth 17.94 2021-02-12T23:48:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173711.76 epoch sec; seqNo:7049; 4057896.99 north; 591548.07 east 2021-02-12T23:48:32Z- AHRS_M2 msg: 1613173712.14 epoch sec; seqNo:6993 2021-02-12T23:48:32Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-12T23:48:32Z- handleDepth()-Depth_Keller msg: 1613173712153 epoch sec; seqNo:7050; depth 17.84 2021-02-12T23:48:32Z- handleDvl() - RDI_Pathfinder msg: 1613173712.17 epoch sec; seqNo:3267 2021-02-12T23:48:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 97.71 , 108.66 , 71.51 2021-02-12T23:48:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.00 , 0.00 2021-02-12T23:48:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173712.18 epoch sec; seqNo:7050; 4057897.37 north; 591548.07 east 2021-02-12T23:48:32Z- AHRS_M2 msg: 1613173712.54 epoch sec; seqNo:6994 2021-02-12T23:48:32Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-12T23:48:32Z- handleDepth()-Depth_Keller msg: 1613173712557 epoch sec; seqNo:7051; depth 17.73 2021-02-12T23:48:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173712.57 epoch sec; seqNo:7051; 4057897.37 north; 591548.07 east 2021-02-12T23:48:32Z- AHRS_M2 msg: 1613173712.95 epoch sec; seqNo:6995 2021-02-12T23:48:32Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-12T23:48:32Z- handleDepth()-Depth_Keller msg: 1613173712957 epoch sec; seqNo:7052; depth 17.61 2021-02-12T23:48:32Z- handleDvl() - RDI_Pathfinder msg: 1613173712.97 epoch sec; seqNo:3268 2021-02-12T23:48:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.75 , 97.78 , 107.51 , 71.92 2021-02-12T23:48:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , -0.03 2021-02-12T23:48:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173712.99 epoch sec; seqNo:7052; 4057897.75 north; 591548.06 east 2021-02-12T23:48:33Z- AHRS_M2 msg: 1613173713.35 epoch sec; seqNo:6996 2021-02-12T23:48:33Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-12T23:48:33Z- handleDepth()-Depth_Keller msg: 1613173713361 epoch sec; seqNo:7053; depth 17.49 2021-02-12T23:48:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173713.37 epoch sec; seqNo:7053; 4057898.14 north; 591549.28 east 2021-02-12T23:48:33Z- AHRS_M2 msg: 1613173713.76 epoch sec; seqNo:6997 2021-02-12T23:48:33Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-12T23:48:33Z- handleDepth()-Depth_Keller msg: 1613173713761 epoch sec; seqNo:7054; depth 17.37 2021-02-12T23:48:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173713.77 epoch sec; seqNo:7054; 4057898.14 north; 591549.28 east 2021-02-12T23:48:34Z- AHRS_M2 msg: 1613173714.16 epoch sec; seqNo:6998 2021-02-12T23:48:34Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-12T23:48:34Z- handleDepth()-Depth_Keller msg: 1613173714173 epoch sec; seqNo:7055; depth 17.26 2021-02-12T23:48:34Z- handleDvl() - RDI_Pathfinder msg: 1613173714.18 epoch sec; seqNo:3269 2021-02-12T23:48:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.75 , 97.78 , 107.51 , 78.31 2021-02-12T23:48:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , 0.01 2021-02-12T23:48:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173714.19 epoch sec; seqNo:7055; 4057898.52 north; 591549.28 east 2021-02-12T23:48:34Z- AHRS_M2 msg: 1613173714.56 epoch sec; seqNo:6999 2021-02-12T23:48:34Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-12T23:48:34Z- handleDepth()-Depth_Keller msg: 1613173714573 epoch sec; seqNo:7056; depth 17.14 2021-02-12T23:48:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173714.59 epoch sec; seqNo:7056; 4057898.90 north; 591549.27 east 2021-02-12T23:48:34Z- AHRS_M2 msg: 1613173714.97 epoch sec; seqNo:7000 2021-02-12T23:48:34Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-12T23:48:34Z- handleDepth()-Depth_Keller msg: 1613173714977 epoch sec; seqNo:7057; depth 17.02 2021-02-12T23:48:34Z- handleDvl() - RDI_Pathfinder msg: 1613173714.99 epoch sec; seqNo:3270 2021-02-12T23:48:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 97.65 , 108.60 , 71.92 2021-02-12T23:48:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , 0.07 2021-02-12T23:48:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173715.02 epoch sec; seqNo:7057; 4057899.28 north; 591549.27 east 2021-02-12T23:48:35Z- LcmTrn::handleMessages() - handling 45 messages took 0.73 ms and 131 calls... 2021-02-12T23:48:35Z- waiting for 1613173712.76, time is 1613173715.02 2021-02-12T23:48:35Z- LcmTrn::updateTrn() equating pose 1613173715.02 and meas times 1613173715.02 2021-02-12T23:48:35Z- LcmTrn::updateTrn() >>>> pose time 1613173715.02, meas time 1613173715.02 <<<< 2021-02-12T23:48:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173715.02 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:48:35Z- TerrainNav::motionUpdate() - currEstimate = (4057899, 591549.27, 17.02) 2021-02-12T23:48:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:48:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:48:35Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173715.02 sec.; No relevant navigation data available 2021-02-12T23:48:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:48:35Z- TNavBF::mmse selected from filter -1 2021-02-12T23:48:35Z- variances([0], [2], and psi_berg) = 735.535302 833.020738 0.000000 2021-02-12T23:48:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:48:35Z- LcmTrn::publishEstimates() - MLE : 4057890.78 591503.77 17.02 2021-02-12T23:48:35Z- LcmTrn::publishEstimates() - MMSE : 4057899.28 591549.27 17.02 2021-02-12T23:48:35Z- LcmTrn::publishEstimates() - COVAR: 735.54 833.02 1.01 0.00 2021-02-12T23:48:35Z- AHRS_M2 msg: 1613173715.37 epoch sec; seqNo:7001 2021-02-12T23:48:35Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-12T23:48:35Z- handleDepth()-Depth_Keller msg: 1613173715379 epoch sec; seqNo:7058; depth 16.90 2021-02-12T23:48:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173715.39 epoch sec; seqNo:7058; 4057899.28 north; 591549.27 east 2021-02-12T23:48:35Z- AHRS_M2 msg: 1613173715.78 epoch sec; seqNo:7002 2021-02-12T23:48:35Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-12T23:48:35Z- handleDepth()-Depth_Keller msg: 1613173715785 epoch sec; seqNo:7059; depth 16.77 2021-02-12T23:48:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173715.79 epoch sec; seqNo:7059; 4057899.66 north; 591549.26 east 2021-02-12T23:48:36Z- AHRS_M2 msg: 1613173716.18 epoch sec; seqNo:7003 2021-02-12T23:48:36Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-12T23:48:36Z- handleDepth()-Depth_Keller msg: 1613173716189 epoch sec; seqNo:7060; depth 16.68 2021-02-12T23:48:36Z- handleDvl() - RDI_Pathfinder msg: 1613173716.20 epoch sec; seqNo:3271 2021-02-12T23:48:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:48:36Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:48:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173716.21 epoch sec; seqNo:7060; 4057900.04 north; 591549.26 east 2021-02-12T23:48:36Z- AHRS_M2 msg: 1613173716.58 epoch sec; seqNo:7004 2021-02-12T23:48:36Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-12T23:48:36Z- handleDepth()-Depth_Keller msg: 1613173716589 epoch sec; seqNo:7061; depth 16.53 2021-02-12T23:48:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173716.61 epoch sec; seqNo:7061; 4057900.43 north; 591550.48 east 2021-02-12T23:48:36Z- AHRS_M2 msg: 1613173716.99 epoch sec; seqNo:7005 2021-02-12T23:48:36Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-12T23:48:36Z- handleDepth()-Depth_Keller msg: 1613173716997 epoch sec; seqNo:7062; depth 16.41 2021-02-12T23:48:37Z- handleDvl() - RDI_Pathfinder msg: 1613173717.03 epoch sec; seqNo:3272 2021-02-12T23:48:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.69 , 97.78 , 111.17 , 78.45 2021-02-12T23:48:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.10 2021-02-12T23:48:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173717.04 epoch sec; seqNo:7062; 4057900.43 north; 591550.48 east 2021-02-12T23:48:37Z- AHRS_M2 msg: 1613173717.39 epoch sec; seqNo:7006 2021-02-12T23:48:37Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-12T23:48:37Z- handleDepth()-Depth_Keller msg: 1613173717397 epoch sec; seqNo:7063; depth 16.29 2021-02-12T23:48:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173717.41 epoch sec; seqNo:7063; 4057900.81 north; 591550.47 east 2021-02-12T23:48:37Z- AHRS_M2 msg: 1613173717.80 epoch sec; seqNo:7007 2021-02-12T23:48:37Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-12T23:48:37Z- handleDepth()-Depth_Keller msg: 1613173717803 epoch sec; seqNo:7064; depth 16.19 2021-02-12T23:48:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173717.81 epoch sec; seqNo:7064; 4057901.19 north; 591550.47 east 2021-02-12T23:48:38Z- AHRS_M2 msg: 1613173718.20 epoch sec; seqNo:7008 2021-02-12T23:48:38Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-12T23:48:38Z- handleDepth()-Depth_Keller msg: 1613173718213 epoch sec; seqNo:7065; depth 16.06 2021-02-12T23:48:38Z- handleDvl() - RDI_Pathfinder msg: 1613173718.24 epoch sec; seqNo:3273 2021-02-12T23:48:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 99.07 , 111.23 , 78.79 2021-02-12T23:48:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.03 , 0.01 2021-02-12T23:48:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173718.25 epoch sec; seqNo:7065; 4057901.56 north; 591550.46 east 2021-02-12T23:48:38Z- AHRS_M2 msg: 1613173718.60 epoch sec; seqNo:7009 2021-02-12T23:48:38Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-12T23:48:38Z- handleDepth()-Depth_Keller msg: 1613173718609 epoch sec; seqNo:7066; depth 15.95 2021-02-12T23:48:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173718.62 epoch sec; seqNo:7066; 4057901.56 north; 591550.46 east 2021-02-12T23:48:39Z- AHRS_M2 msg: 1613173719.02 epoch sec; seqNo:7010 2021-02-12T23:48:39Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-12T23:48:39Z- handleDepth()-Depth_Keller msg: 1613173719022 epoch sec; seqNo:7067; depth 15.85 2021-02-12T23:48:39Z- handleDvl() - RDI_Pathfinder msg: 1613173719.03 epoch sec; seqNo:3274 2021-02-12T23:48:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 99.00 , 112.52 , 74.78 2021-02-12T23:48:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , -0.09 2021-02-12T23:48:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173719.06 epoch sec; seqNo:7067; 4057901.96 north; 591551.68 east 2021-02-12T23:48:39Z- AHRS_M2 msg: 1613173719.42 epoch sec; seqNo:7011 2021-02-12T23:48:39Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-12T23:48:39Z- handleDepth()-Depth_Keller msg: 1613173719423 epoch sec; seqNo:7068; depth 15.72 2021-02-12T23:48:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173719.43 epoch sec; seqNo:7068; 4057902.34 north; 591551.68 east 2021-02-12T23:48:39Z- AHRS_M2 msg: 1613173719.82 epoch sec; seqNo:7012 2021-02-12T23:48:39Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-12T23:48:39Z- handleDepth()-Depth_Keller msg: 1613173719834 epoch sec; seqNo:7069; depth 15.61 2021-02-12T23:48:39Z- handleDvl() - RDI_Pathfinder msg: 1613173719.85 epoch sec; seqNo:3275 2021-02-12T23:48:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.42 , 112.58 , 73.56 2021-02-12T23:48:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.12 , -0.12 2021-02-12T23:48:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173719.86 epoch sec; seqNo:7069; 4057902.71 north; 591551.67 east 2021-02-12T23:48:40Z- LcmTrn::handleMessages() - handling 41 messages took 0.72 ms and 127 calls... 2021-02-12T23:48:40Z- waiting for 1613173718.02, time is 1613173719.86 2021-02-12T23:48:40Z- LcmTrn::updateTrn() equating pose 1613173719.86 and meas times 1613173719.86 2021-02-12T23:48:40Z- LcmTrn::updateTrn() >>>> pose time 1613173719.86, meas time 1613173719.86 <<<< 2021-02-12T23:48:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173719.86 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:48:40Z- TerrainNav::motionUpdate() - currEstimate = (4057903, 591551.67, 15.61) 2021-02-12T23:48:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:48:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:48:40Z- TNavBF::Projecting Measurements 2021-02-12T23:48:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:48:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:48:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:48:40Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:48:40Z- TNavFilter::3 good beams. 2021-02-12T23:48:40Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:48:40Z- TNavBF:: beamIndex[0] = 1 2021-02-12T23:48:40Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:48:40Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:48:40Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:48:40Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:48:40Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:48:40Z- TNavBF::mmse selected from filter -1 2021-02-12T23:48:40Z- TerrainMapOctree: (vehN, vehE ) = (4057902.71, 591551.67). 2021-02-12T23:48:40Z- TerrainMapOctree: Min Distance = 4299.45. 2021-02-12T23:48:40Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:48:40Z- There is no specific good beam that all particles have in common. 2021-02-12T23:48:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:48:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 16.13 2021-02-12T23:48:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:48:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 20.28 2021-02-12T23:48:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:48:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 8.66 2021-02-12T23:48:40Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:48:40Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:48:40Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:48:40Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:48:40Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:48:40Z- Filter 0 No beams used in subcloud update 2021-02-12T23:48:40Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:48:40Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:48:40Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 735.5 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:48:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173719.86 sec. 2021-02-12T23:48:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:48:40Z- TNavBF::mmse selected from filter -1 2021-02-12T23:48:40Z- variances([0], [2], and psi_berg) = 735.452375 833.020673 0.000000 2021-02-12T23:48:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:48:40Z- LcmTrn::publishEstimates() - MLE : 4057894.21 591506.17 15.61 2021-02-12T23:48:40Z- LcmTrn::publishEstimates() - MMSE : 4057902.71 591551.67 15.61 2021-02-12T23:48:40Z- LcmTrn::publishEstimates() - COVAR: 735.45 833.02 0.99 0.00 2021-02-12T23:48:40Z- AHRS_M2 msg: 1613173720.22 epoch sec; seqNo:7013 2021-02-12T23:48:40Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:48:40Z- handleDepth()-Depth_Keller msg: 1613173720226 epoch sec; seqNo:7070; depth 15.50 2021-02-12T23:48:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173720.24 epoch sec; seqNo:7070; 4057903.09 north; 591551.67 east 2021-02-12T23:48:40Z- AHRS_M2 msg: 1613173720.62 epoch sec; seqNo:7014 2021-02-12T23:48:40Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-12T23:48:40Z- handleDepth()-Depth_Keller msg: 1613173720631 epoch sec; seqNo:7071; depth 15.39 2021-02-12T23:48:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173720.64 epoch sec; seqNo:7071; 4057903.09 north; 591551.67 east 2021-02-12T23:48:41Z- AHRS_M2 msg: 1613173721.03 epoch sec; seqNo:7015 2021-02-12T23:48:41Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-12T23:48:41Z- handleDepth()-Depth_Keller msg: 1613173721108 epoch sec; seqNo:7072; depth 15.28 2021-02-12T23:48:41Z- handleDvl() - RDI_Pathfinder msg: 1613173721.12 epoch sec; seqNo:3276 2021-02-12T23:48:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.57 , 100.15 , 110.96 , 78.51 2021-02-12T23:48:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.15 , -0.07 2021-02-12T23:48:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173721.14 epoch sec; seqNo:7072; 4057903.47 north; 591551.67 east 2021-02-12T23:48:41Z- AHRS_M2 msg: 1613173721.44 epoch sec; seqNo:7016 2021-02-12T23:48:41Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.14 psi 2021-02-12T23:48:41Z- handleDepth()-Depth_Keller msg: 1613173721523 epoch sec; seqNo:7073; depth 15.14 2021-02-12T23:48:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173721.53 epoch sec; seqNo:7073; 4057903.85 north; 591551.66 east 2021-02-12T23:48:41Z- AHRS_M2 msg: 1613173721.84 epoch sec; seqNo:7017 2021-02-12T23:48:41Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.13 psi 2021-02-12T23:48:41Z- handleDepth()-Depth_Keller msg: 1613173721882 epoch sec; seqNo:7074; depth 15.04 2021-02-12T23:48:41Z- handleDvl() - RDI_Pathfinder msg: 1613173721.91 epoch sec; seqNo:3277 2021-02-12T23:48:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 97.65 , 108.46 , 78.72 2021-02-12T23:48:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.13 , 0.04 2021-02-12T23:48:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173721.93 epoch sec; seqNo:7074; 4057904.23 north; 591551.66 east 2021-02-12T23:48:42Z- AHRS_M2 msg: 1613173722.24 epoch sec; seqNo:7018 2021-02-12T23:48:42Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.13 psi 2021-02-12T23:48:42Z- handleDepth()-Depth_Keller msg: 1613173722274 epoch sec; seqNo:7075; depth 14.92 2021-02-12T23:48:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173722.29 epoch sec; seqNo:7075; 4057904.23 north; 591551.66 east 2021-02-12T23:48:42Z- AHRS_M2 msg: 1613173722.65 epoch sec; seqNo:7019 2021-02-12T23:48:42Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.13 psi 2021-02-12T23:48:42Z- handleDepth()-Depth_Keller msg: 1613173722673 epoch sec; seqNo:7076; depth 14.81 2021-02-12T23:48:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173722.68 epoch sec; seqNo:7076; 4057904.62 north; 591552.87 east 2021-02-12T23:48:43Z- AHRS_M2 msg: 1613173723.05 epoch sec; seqNo:7020 2021-02-12T23:48:43Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.13 psi 2021-02-12T23:48:43Z- handleDepth()-Depth_Keller msg: 1613173723057 epoch sec; seqNo:7077; depth 14.69 2021-02-12T23:48:43Z- handleDvl() - RDI_Pathfinder msg: 1613173723.07 epoch sec; seqNo:3278 2021-02-12T23:48:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.71 , 108.53 , 78.79 2021-02-12T23:48:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.09 , 0.14 2021-02-12T23:48:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173723.10 epoch sec; seqNo:7077; 4057905.00 north; 591552.87 east 2021-02-12T23:48:43Z- AHRS_M2 msg: 1613173723.45 epoch sec; seqNo:7021 2021-02-12T23:48:43Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.13 psi 2021-02-12T23:48:43Z- handleDepth()-Depth_Keller msg: 1613173723465 epoch sec; seqNo:7078; depth 14.57 2021-02-12T23:48:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173723.48 epoch sec; seqNo:7078; 4057905.00 north; 591552.87 east 2021-02-12T23:48:43Z- AHRS_M2 msg: 1613173723.86 epoch sec; seqNo:7022 2021-02-12T23:48:43Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-12T23:48:43Z- handleDepth()-Depth_Keller msg: 1613173723866 epoch sec; seqNo:7079; depth 14.45 2021-02-12T23:48:43Z- handleDvl() - RDI_Pathfinder msg: 1613173723.88 epoch sec; seqNo:3279 2021-02-12T23:48:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.44 , 112.32 , 78.51 2021-02-12T23:48:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , 0.18 2021-02-12T23:48:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173723.89 epoch sec; seqNo:7079; 4057905.38 north; 591552.87 east 2021-02-12T23:48:44Z- AHRS_M2 msg: 1613173724.26 epoch sec; seqNo:7023 2021-02-12T23:48:44Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-12T23:48:44Z- handleDepth()-Depth_Keller msg: 1613173724269 epoch sec; seqNo:7080; depth 14.35 2021-02-12T23:48:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173724.30 epoch sec; seqNo:7080; 4057905.76 north; 591552.86 east 2021-02-12T23:48:44Z- AHRS_M2 msg: 1613173724.67 epoch sec; seqNo:7024 2021-02-12T23:48:44Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-12T23:48:44Z- handleDepth()-Depth_Keller msg: 1613173724677 epoch sec; seqNo:7081; depth 14.23 2021-02-12T23:48:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173724.68 epoch sec; seqNo:7081; 4057906.14 north; 591552.86 east 2021-02-12T23:48:45Z- AHRS_M2 msg: 1613173725.08 epoch sec; seqNo:7025 2021-02-12T23:48:45Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-12T23:48:45Z- handleDepth()-Depth_Keller msg: 1613173725082 epoch sec; seqNo:7082; depth 14.10 2021-02-12T23:48:45Z- handleDvl() - RDI_Pathfinder msg: 1613173725.09 epoch sec; seqNo:3280 2021-02-12T23:48:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.86 , 98.73 , 112.25 , 75.60 2021-02-12T23:48:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.03 , 0.12 2021-02-12T23:48:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173725.10 epoch sec; seqNo:7082; 4057906.14 north; 591552.86 east 2021-02-12T23:48:45Z- LcmTrn::handleMessages() - handling 44 messages took 0.82 ms and 128 calls... 2021-02-12T23:48:45Z- waiting for 1613173722.86, time is 1613173725.10 2021-02-12T23:48:45Z- LcmTrn::updateTrn() equating pose 1613173725.10 and meas times 1613173725.10 2021-02-12T23:48:45Z- LcmTrn::updateTrn() >>>> pose time 1613173725.10, meas time 1613173725.10 <<<< 2021-02-12T23:48:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173725.10 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:48:45Z- TerrainNav::motionUpdate() - currEstimate = (4057906, 591552.86, 14.10) 2021-02-12T23:48:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:48:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:48:45Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173725.10 sec.; No relevant navigation data available 2021-02-12T23:48:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:48:45Z- TNavBF::mmse selected from filter -1 2021-02-12T23:48:45Z- variances([0], [2], and psi_berg) = 735.375391 833.020613 0.000000 2021-02-12T23:48:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:48:45Z- LcmTrn::publishEstimates() - MLE : 4057897.64 591507.36 14.10 2021-02-12T23:48:45Z- LcmTrn::publishEstimates() - MMSE : 4057906.14 591552.86 14.10 2021-02-12T23:48:45Z- LcmTrn::publishEstimates() - COVAR: 735.38 833.02 1.17 0.00 2021-02-12T23:48:45Z- AHRS_M2 msg: 1613173725.48 epoch sec; seqNo:7026 2021-02-12T23:48:45Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.18 psi 2021-02-12T23:48:45Z- handleDepth()-Depth_Keller msg: 1613173725481 epoch sec; seqNo:7083; depth 13.98 2021-02-12T23:48:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173725.50 epoch sec; seqNo:7083; 4057906.53 north; 591554.07 east 2021-02-12T23:48:45Z- AHRS_M2 msg: 1613173725.88 epoch sec; seqNo:7027 2021-02-12T23:48:45Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-12T23:48:45Z- handleDepth()-Depth_Keller msg: 1613173725885 epoch sec; seqNo:7084; depth 13.88 2021-02-12T23:48:45Z- handleDvl() - RDI_Pathfinder msg: 1613173725.90 epoch sec; seqNo:3281 2021-02-12T23:48:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 103.05 , 113.87 , 78.72 2021-02-12T23:48:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.03 , 0.01 2021-02-12T23:48:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173725.93 epoch sec; seqNo:7084; 4057906.91 north; 591554.07 east 2021-02-12T23:48:46Z- AHRS_M2 msg: 1613173726.28 epoch sec; seqNo:7028 2021-02-12T23:48:46Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-12T23:48:46Z- handleDepth()-Depth_Keller msg: 1613173726289 epoch sec; seqNo:7085; depth 13.75 2021-02-12T23:48:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173726.30 epoch sec; seqNo:7085; 4057907.29 north; 591554.07 east 2021-02-12T23:48:46Z- AHRS_M2 msg: 1613173726.69 epoch sec; seqNo:7029 2021-02-12T23:48:46Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-12T23:48:46Z- handleDepth()-Depth_Keller msg: 1613173726695 epoch sec; seqNo:7086; depth 13.65 2021-02-12T23:48:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173726.70 epoch sec; seqNo:7086; 4057907.29 north; 591554.07 east 2021-02-12T23:48:47Z- AHRS_M2 msg: 1613173727.09 epoch sec; seqNo:7030 2021-02-12T23:48:47Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:48:47Z- handleDepth()-Depth_Keller msg: 1613173727101 epoch sec; seqNo:7087; depth 13.53 2021-02-12T23:48:47Z- handleDvl() - RDI_Pathfinder msg: 1613173727.12 epoch sec; seqNo:3282 2021-02-12T23:48:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 101.23 , 116.10 , 78.25 2021-02-12T23:48:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.00 , -0.10 2021-02-12T23:48:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173727.14 epoch sec; seqNo:7087; 4057907.67 north; 591554.06 east 2021-02-12T23:48:47Z- AHRS_M2 msg: 1613173727.49 epoch sec; seqNo:7031 2021-02-12T23:48:47Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.15 psi 2021-02-12T23:48:47Z- handleDepth()-Depth_Keller msg: 1613173727501 epoch sec; seqNo:7088; depth 13.41 2021-02-12T23:48:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173727.51 epoch sec; seqNo:7088; 4057908.06 north; 591555.28 east 2021-02-12T23:48:47Z- AHRS_M2 msg: 1613173727.90 epoch sec; seqNo:7032 2021-02-12T23:48:47Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.15 psi 2021-02-12T23:48:47Z- handleDepth()-Depth_Keller msg: 1613173727905 epoch sec; seqNo:7089; depth 13.32 2021-02-12T23:48:47Z- handleDvl() - RDI_Pathfinder msg: 1613173727.92 epoch sec; seqNo:3283 2021-02-12T23:48:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.28 , 100.15 , 116.37 , 75.60 2021-02-12T23:48:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , -0.15 2021-02-12T23:48:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173727.93 epoch sec; seqNo:7089; 4057908.44 north; 591555.28 east 2021-02-12T23:48:48Z- AHRS_M2 msg: 1613173728.30 epoch sec; seqNo:7033 2021-02-12T23:48:48Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-12T23:48:48Z- handleDepth()-Depth_Keller msg: 1613173728317 epoch sec; seqNo:7090; depth 13.19 2021-02-12T23:48:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173728.33 epoch sec; seqNo:7090; 4057908.44 north; 591555.28 east 2021-02-12T23:48:48Z- AHRS_M2 msg: 1613173728.71 epoch sec; seqNo:7034 2021-02-12T23:48:48Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.14 psi 2021-02-12T23:48:48Z- handleDepth()-Depth_Keller msg: 1613173728713 epoch sec; seqNo:7091; depth 13.09 2021-02-12T23:48:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173728.73 epoch sec; seqNo:7091; 4057908.81 north; 591555.27 east 2021-02-12T23:48:49Z- AHRS_M2 msg: 1613173729.11 epoch sec; seqNo:7035 2021-02-12T23:48:49Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.13 psi 2021-02-12T23:48:49Z- handleDepth()-Depth_Keller msg: 1613173729119 epoch sec; seqNo:7092; depth 12.98 2021-02-12T23:48:49Z- handleDvl() - RDI_Pathfinder msg: 1613173729.13 epoch sec; seqNo:3284 2021-02-12T23:48:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 100.21 , 115.09 , 78.58 2021-02-12T23:48:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.10 , -0.10 2021-02-12T23:48:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173729.16 epoch sec; seqNo:7092; 4057909.19 north; 591555.27 east 2021-02-12T23:48:49Z- AHRS_M2 msg: 1613173729.52 epoch sec; seqNo:7036 2021-02-12T23:48:49Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.14 psi 2021-02-12T23:48:49Z- handleDepth()-Depth_Keller msg: 1613173729521 epoch sec; seqNo:7093; depth 12.84 2021-02-12T23:48:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173729.53 epoch sec; seqNo:7093; 4057909.19 north; 591555.27 east 2021-02-12T23:48:49Z- AHRS_M2 msg: 1613173729.92 epoch sec; seqNo:7037 2021-02-12T23:48:49Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-12T23:48:49Z- handleDepth()-Depth_Keller msg: 1613173729933 epoch sec; seqNo:7094; depth 12.74 2021-02-12T23:48:49Z- handleDvl() - RDI_Pathfinder msg: 1613173729.94 epoch sec; seqNo:3285 2021-02-12T23:48:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.86 , 109.68 , 76.83 2021-02-12T23:48:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.14 , 0.01 2021-02-12T23:48:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173729.95 epoch sec; seqNo:7094; 4057909.57 north; 591555.26 east 2021-02-12T23:48:50Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-12T23:48:50Z- waiting for 1613173728.10, time is 1613173729.95 2021-02-12T23:48:50Z- LcmTrn::updateTrn() equating pose 1613173729.95 and meas times 1613173729.95 2021-02-12T23:48:50Z- LcmTrn::updateTrn() >>>> pose time 1613173729.95, meas time 1613173729.95 <<<< 2021-02-12T23:48:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173729.95 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:48:50Z- TerrainNav::motionUpdate() - currEstimate = (4057910, 591555.26, 12.74) 2021-02-12T23:48:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:48:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:48:50Z- TNavBF::Projecting Measurements 2021-02-12T23:48:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:48:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:48:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:48:50Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:48:50Z- TNavFilter::3 good beams. 2021-02-12T23:48:50Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:48:50Z- TNavBF:: beamIndex[0] = 1 2021-02-12T23:48:50Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:48:50Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:48:50Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:48:50Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:48:50Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:48:50Z- TNavBF::mmse selected from filter -1 2021-02-12T23:48:50Z- TerrainMapOctree: (vehN, vehE ) = (4057909.57, 591555.26). 2021-02-12T23:48:50Z- TerrainMapOctree: Min Distance = 4291.86. 2021-02-12T23:48:50Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:48:50Z- There is no specific good beam that all particles have in common. 2021-02-12T23:48:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:48:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.39 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 15.64 2021-02-12T23:48:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:48:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 19.25 2021-02-12T23:48:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:48:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 9.44 2021-02-12T23:48:50Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:48:50Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:48:50Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:48:50Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:48:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:48:50Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:48:50Z- Filter 0 No beams used in subcloud update 2021-02-12T23:48:50Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:48:50Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:48:50Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 735.3 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:48:50Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173729.95 sec. 2021-02-12T23:48:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:48:50Z- TNavBF::mmse selected from filter -1 2021-02-12T23:48:50Z- variances([0], [2], and psi_berg) = 735.292626 833.020549 0.000000 2021-02-12T23:48:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:48:50Z- LcmTrn::publishEstimates() - MLE : 4057901.07 591509.76 12.74 2021-02-12T23:48:50Z- LcmTrn::publishEstimates() - MMSE : 4057909.57 591555.26 12.74 2021-02-12T23:48:50Z- LcmTrn::publishEstimates() - COVAR: 735.29 833.02 1.09 0.00 2021-02-12T23:48:50Z- AHRS_M2 msg: 1613173730.32 epoch sec; seqNo:7038 2021-02-12T23:48:50Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-12T23:48:50Z- handleDepth()-Depth_Keller msg: 1613173730339 epoch sec; seqNo:7095; depth 12.61 2021-02-12T23:48:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173730.35 epoch sec; seqNo:7095; 4057909.95 north; 591555.26 east 2021-02-12T23:48:50Z- AHRS_M2 msg: 1613173730.73 epoch sec; seqNo:7039 2021-02-12T23:48:50Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-12T23:48:50Z- handleDepth()-Depth_Keller msg: 1613173730733 epoch sec; seqNo:7096; depth 12.49 2021-02-12T23:48:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173730.74 epoch sec; seqNo:7096; 4057909.95 north; 591555.26 east 2021-02-12T23:48:51Z- AHRS_M2 msg: 1613173731.13 epoch sec; seqNo:7040 2021-02-12T23:48:51Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.14 psi 2021-02-12T23:48:51Z- handleDepth()-Depth_Keller msg: 1613173731145 epoch sec; seqNo:7097; depth 12.38 2021-02-12T23:48:51Z- handleDvl() - RDI_Pathfinder msg: 1613173731.16 epoch sec; seqNo:3286 2021-02-12T23:48:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 100.15 , 115.02 , 78.51 2021-02-12T23:48:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.11 , 0.14 2021-02-12T23:48:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173731.18 epoch sec; seqNo:7097; 4057910.34 north; 591556.48 east 2021-02-12T23:48:51Z- AHRS_M2 msg: 1613173731.54 epoch sec; seqNo:7041 2021-02-12T23:48:51Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.15 psi 2021-02-12T23:48:51Z- handleDepth()-Depth_Keller msg: 1613173731542 epoch sec; seqNo:7098; depth 12.24 2021-02-12T23:48:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173731.55 epoch sec; seqNo:7098; 4057910.72 north; 591556.47 east 2021-02-12T23:48:51Z- AHRS_M2 msg: 1613173731.94 epoch sec; seqNo:7042 2021-02-12T23:48:51Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-12T23:48:51Z- handleDepth()-Depth_Keller msg: 1613173731949 epoch sec; seqNo:7099; depth 12.13 2021-02-12T23:48:51Z- handleDvl() - RDI_Pathfinder msg: 1613173731.97 epoch sec; seqNo:3287 2021-02-12T23:48:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 101.50 , 113.67 , 78.51 2021-02-12T23:48:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.09 , 0.16 2021-02-12T23:48:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173731.98 epoch sec; seqNo:7099; 4057910.72 north; 591556.47 east 2021-02-12T23:48:52Z- AHRS_M2 msg: 1613173732.34 epoch sec; seqNo:7043 2021-02-12T23:48:52Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-12T23:48:52Z- handleDepth()-Depth_Keller msg: 1613173732349 epoch sec; seqNo:7100; depth 11.99 2021-02-12T23:48:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173732.36 epoch sec; seqNo:7100; 4057911.10 north; 591556.47 east 2021-02-12T23:48:52Z- AHRS_M2 msg: 1613173732.75 epoch sec; seqNo:7044 2021-02-12T23:48:52Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-12T23:48:52Z- handleDepth()-Depth_Keller msg: 1613173732768 epoch sec; seqNo:7101; depth 11.88 2021-02-12T23:48:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173732.78 epoch sec; seqNo:7101; 4057911.48 north; 591556.46 east 2021-02-12T23:48:53Z- AHRS_M2 msg: 1613173733.15 epoch sec; seqNo:7045 2021-02-12T23:48:53Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-12T23:48:53Z- handleDepth()-Depth_Keller msg: 1613173733159 epoch sec; seqNo:7102; depth 11.76 2021-02-12T23:48:53Z- handleDvl() - RDI_Pathfinder msg: 1613173733.18 epoch sec; seqNo:3288 2021-02-12T23:48:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:48:53Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:48:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173733.19 epoch sec; seqNo:7102; 4057911.86 north; 591556.46 east 2021-02-12T23:48:53Z- AHRS_M2 msg: 1613173733.56 epoch sec; seqNo:7046 2021-02-12T23:48:53Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-12T23:48:53Z- handleDepth()-Depth_Keller msg: 1613173733569 epoch sec; seqNo:7103; depth 11.64 2021-02-12T23:48:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173733.58 epoch sec; seqNo:7103; 4057912.24 north; 591556.46 east 2021-02-12T23:48:53Z- AHRS_M2 msg: 1613173733.96 epoch sec; seqNo:7047 2021-02-12T23:48:53Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-12T23:48:53Z- handleDepth()-Depth_Keller msg: 1613173733973 epoch sec; seqNo:7104; depth 11.53 2021-02-12T23:48:53Z- handleDvl() - RDI_Pathfinder msg: 1613173733.98 epoch sec; seqNo:3289 2021-02-12T23:48:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 103.19 , 118.06 , 78.36 2021-02-12T23:48:53Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.07 , 0.04 2021-02-12T23:48:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173733.99 epoch sec; seqNo:7104; 4057912.25 north; 591557.68 east 2021-02-12T23:48:54Z- AHRS_M2 msg: 1613173734.36 epoch sec; seqNo:7048 2021-02-12T23:48:54Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-12T23:48:54Z- handleDepth()-Depth_Keller msg: 1613173734442 epoch sec; seqNo:7105; depth 11.40 2021-02-12T23:48:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173734.45 epoch sec; seqNo:7105; 4057912.63 north; 591557.67 east 2021-02-12T23:48:54Z- AHRS_M2 msg: 1613173734.77 epoch sec; seqNo:7049 2021-02-12T23:48:54Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.17 psi 2021-02-12T23:48:54Z- handleDepth()-Depth_Keller msg: 1613173734863 epoch sec; seqNo:7106; depth 11.27 2021-02-12T23:48:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173734.87 epoch sec; seqNo:7106; 4057913.01 north; 591557.67 east 2021-02-12T23:48:55Z- AHRS_M2 msg: 1613173735.17 epoch sec; seqNo:7050 2021-02-12T23:48:55Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.17 psi 2021-02-12T23:48:55Z- handleDepth()-Depth_Keller msg: 1613173735205 epoch sec; seqNo:7107; depth 11.15 2021-02-12T23:48:55Z- handleDvl() - RDI_Pathfinder msg: 1613173735.24 epoch sec; seqNo:3290 2021-02-12T23:48:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 103.80 , 118.67 , 79.46 2021-02-12T23:48:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.07 , 0.00 2021-02-12T23:48:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173735.25 epoch sec; seqNo:7107; 4057913.39 north; 591557.66 east 2021-02-12T23:48:55Z- LcmTrn::handleMessages() - handling 44 messages took 0.77 ms and 129 calls... 2021-02-12T23:48:55Z- waiting for 1613173732.95, time is 1613173735.25 2021-02-12T23:48:55Z- LcmTrn::updateTrn() equating pose 1613173735.25 and meas times 1613173735.25 2021-02-12T23:48:55Z- LcmTrn::updateTrn() >>>> pose time 1613173735.25, meas time 1613173735.25 <<<< 2021-02-12T23:48:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173735.25 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:48:55Z- TerrainNav::motionUpdate() - currEstimate = (4057913, 591557.66, 11.15) 2021-02-12T23:48:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:48:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:48:55Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173735.25 sec.; No relevant navigation data available 2021-02-12T23:48:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:48:55Z- TNavBF::mmse selected from filter -1 2021-02-12T23:48:55Z- variances([0], [2], and psi_berg) = 735.206910 833.020483 0.000000 2021-02-12T23:48:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:48:55Z- LcmTrn::publishEstimates() - MLE : 4057904.89 591512.16 11.15 2021-02-12T23:48:55Z- LcmTrn::publishEstimates() - MMSE : 4057913.39 591557.66 11.15 2021-02-12T23:48:55Z- LcmTrn::publishEstimates() - COVAR: 735.21 833.02 1.32 0.00 2021-02-12T23:48:55Z- AHRS_M2 msg: 1613173735.58 epoch sec; seqNo:7051 2021-02-12T23:48:55Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.17 psi 2021-02-12T23:48:55Z- handleDepth()-Depth_Keller msg: 1613173735613 epoch sec; seqNo:7108; depth 11.05 2021-02-12T23:48:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173735.62 epoch sec; seqNo:7108; 4057913.77 north; 591557.66 east 2021-02-12T23:48:55Z- AHRS_M2 msg: 1613173735.99 epoch sec; seqNo:7052 2021-02-12T23:48:55Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:48:56Z- handleDepth()-Depth_Keller msg: 1613173736009 epoch sec; seqNo:7109; depth 10.95 2021-02-12T23:48:56Z- handleDvl() - RDI_Pathfinder msg: 1613173736.02 epoch sec; seqNo:3291 2021-02-12T23:48:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.84 , 104.07 , 117.59 , 76.83 2021-02-12T23:48:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.02 2021-02-12T23:48:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173736.03 epoch sec; seqNo:7109; 4057914.14 north; 591557.66 east 2021-02-12T23:48:56Z- AHRS_M2 msg: 1613173736.39 epoch sec; seqNo:7053 2021-02-12T23:48:56Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:48:56Z- handleDepth()-Depth_Keller msg: 1613173736393 epoch sec; seqNo:7110; depth 10.82 2021-02-12T23:48:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173736.40 epoch sec; seqNo:7110; 4057914.16 north; 591558.88 east 2021-02-12T23:48:56Z- AHRS_M2 msg: 1613173736.79 epoch sec; seqNo:7054 2021-02-12T23:48:56Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:48:56Z- handleDepth()-Depth_Keller msg: 1613173736797 epoch sec; seqNo:7111; depth 10.73 2021-02-12T23:48:56Z- handleDvl() - RDI_Pathfinder msg: 1613173736.81 epoch sec; seqNo:3292 2021-02-12T23:48:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:48:56Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:48:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173736.82 epoch sec; seqNo:7111; 4057914.54 north; 591558.87 east 2021-02-12T23:48:57Z- AHRS_M2 msg: 1613173737.19 epoch sec; seqNo:7055 2021-02-12T23:48:57Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-12T23:48:57Z- handleDepth()-Depth_Keller msg: 1613173737201 epoch sec; seqNo:7112; depth 10.61 2021-02-12T23:48:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173737.21 epoch sec; seqNo:7112; 4057914.92 north; 591558.87 east 2021-02-12T23:48:57Z- AHRS_M2 msg: 1613173737.60 epoch sec; seqNo:7056 2021-02-12T23:48:57Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-12T23:48:57Z- handleDepth()-Depth_Keller msg: 1613173737601 epoch sec; seqNo:7113; depth 10.49 2021-02-12T23:48:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173737.62 epoch sec; seqNo:7113; 4057914.92 north; 591558.87 east 2021-02-12T23:48:58Z- AHRS_M2 msg: 1613173738.00 epoch sec; seqNo:7057 2021-02-12T23:48:58Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-12T23:48:58Z- handleDepth()-Depth_Keller msg: 1613173738009 epoch sec; seqNo:7114; depth 10.36 2021-02-12T23:48:58Z- handleDvl() - RDI_Pathfinder msg: 1613173738.02 epoch sec; seqNo:3293 2021-02-12T23:48:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.31 , 104.07 , 112.18 , 79.73 2021-02-12T23:48:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.09 , -0.00 2021-02-12T23:48:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173738.05 epoch sec; seqNo:7114; 4057915.29 north; 591558.87 east 2021-02-12T23:48:58Z- AHRS_M2 msg: 1613173738.41 epoch sec; seqNo:7058 2021-02-12T23:48:58Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.17 psi 2021-02-12T23:48:58Z- handleDepth()-Depth_Keller msg: 1613173738414 epoch sec; seqNo:7115; depth 10.24 2021-02-12T23:48:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173738.43 epoch sec; seqNo:7115; 4057915.67 north; 591558.86 east 2021-02-12T23:48:58Z- AHRS_M2 msg: 1613173738.81 epoch sec; seqNo:7059 2021-02-12T23:48:58Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.17 psi 2021-02-12T23:48:58Z- handleDepth()-Depth_Keller msg: 1613173738821 epoch sec; seqNo:7116; depth 10.14 2021-02-12T23:48:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173738.83 epoch sec; seqNo:7116; 4057915.69 north; 591560.08 east 2021-02-12T23:48:59Z- AHRS_M2 msg: 1613173739.21 epoch sec; seqNo:7060 2021-02-12T23:48:59Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.16 psi 2021-02-12T23:48:59Z- handleDepth()-Depth_Keller msg: 1613173739221 epoch sec; seqNo:7117; depth 10.04 2021-02-12T23:48:59Z- handleDvl() - RDI_Pathfinder msg: 1613173739.24 epoch sec; seqNo:3294 2021-02-12T23:48:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.28 , 118.80 , 168.83 2021-02-12T23:48:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , -0.05 2021-02-12T23:48:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173739.25 epoch sec; seqNo:7117; 4057916.06 north; 591560.08 east 2021-02-12T23:48:59Z- AHRS_M2 msg: 1613173739.62 epoch sec; seqNo:7061 2021-02-12T23:48:59Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.16 psi 2021-02-12T23:48:59Z- handleDepth()-Depth_Keller msg: 1613173739620 epoch sec; seqNo:7118; depth 9.91 2021-02-12T23:48:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173739.63 epoch sec; seqNo:7118; 4057916.44 north; 591560.07 east 2021-02-12T23:49:00Z- AHRS_M2 msg: 1613173740.02 epoch sec; seqNo:7062 2021-02-12T23:49:00Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.16 psi 2021-02-12T23:49:00Z- handleDepth()-Depth_Keller msg: 1613173740033 epoch sec; seqNo:7119; depth 9.82 2021-02-12T23:49:00Z- handleDvl() - RDI_Pathfinder msg: 1613173740.05 epoch sec; seqNo:3295 2021-02-12T23:49:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.32 , 106.16 , 119.55 , 77.47 2021-02-12T23:49:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , -0.05 2021-02-12T23:49:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173740.06 epoch sec; seqNo:7119; 4057916.44 north; 591560.07 east 2021-02-12T23:49:00Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 129 calls... 2021-02-12T23:49:00Z- waiting for 1613173738.25, time is 1613173740.06 2021-02-12T23:49:00Z- LcmTrn::updateTrn() equating pose 1613173740.06 and meas times 1613173740.06 2021-02-12T23:49:00Z- LcmTrn::updateTrn() >>>> pose time 1613173740.06, meas time 1613173740.06 <<<< 2021-02-12T23:49:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173740.06 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:49:00Z- TerrainNav::motionUpdate() - currEstimate = (4057916, 591560.07, 9.82) 2021-02-12T23:49:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:49:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:49:00Z- TNavBF::Projecting Measurements 2021-02-12T23:49:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:49:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:49:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:49:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:49:00Z- TNavFilter::4 good beams. 2021-02-12T23:49:00Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:49:00Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:49:00Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:49:00Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:49:00Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:49:00Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:49:00Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:49:00Z- TNavBF::mmse selected from filter -1 2021-02-12T23:49:00Z- TerrainMapOctree: (vehN, vehE ) = (4057916.44, 591560.07). 2021-02-12T23:49:00Z- TerrainMapOctree: Min Distance = 4283.49. 2021-02-12T23:49:00Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:49:00Z- There is no specific good beam that all particles have in common. 2021-02-12T23:49:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:49:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.52 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 12.48 2021-02-12T23:49:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:49:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 18.03 2021-02-12T23:49:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:49:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 22.87 2021-02-12T23:49:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:49:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 9.60 2021-02-12T23:49:00Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:49:00Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:49:00Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:49:00Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:49:00Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:49:00Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:49:00Z- Filter 0 No beams used in subcloud update 2021-02-12T23:49:00Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:49:00Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:49:00Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 735.1 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:49:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173740.06 sec. 2021-02-12T23:49:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:49:00Z- TNavBF::mmse selected from filter -1 2021-02-12T23:49:00Z- variances([0], [2], and psi_berg) = 735.127342 833.020421 0.000000 2021-02-12T23:49:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:49:00Z- LcmTrn::publishEstimates() - MLE : 4057907.94 591521.57 9.82 2021-02-12T23:49:00Z- LcmTrn::publishEstimates() - MMSE : 4057916.44 591560.07 9.82 2021-02-12T23:49:00Z- LcmTrn::publishEstimates() - COVAR: 735.13 833.02 1.10 0.00 2021-02-12T23:49:00Z- AHRS_M2 msg: 1613173740.42 epoch sec; seqNo:7063 2021-02-12T23:49:00Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.16 psi 2021-02-12T23:49:00Z- handleDepth()-Depth_Keller msg: 1613173740433 epoch sec; seqNo:7120; depth 9.69 2021-02-12T23:49:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173740.45 epoch sec; seqNo:7120; 4057916.82 north; 591560.07 east 2021-02-12T23:49:00Z- AHRS_M2 msg: 1613173740.83 epoch sec; seqNo:7064 2021-02-12T23:49:00Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.16 psi 2021-02-12T23:49:00Z- handleDepth()-Depth_Keller msg: 1613173740833 epoch sec; seqNo:7121; depth 9.57 2021-02-12T23:49:00Z- handleDvl() - RDI_Pathfinder msg: 1613173740.84 epoch sec; seqNo:3296 2021-02-12T23:49:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:49:00Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:49:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173740.87 epoch sec; seqNo:7121; 4057917.20 north; 591560.07 east 2021-02-12T23:49:01Z- AHRS_M2 msg: 1613173741.23 epoch sec; seqNo:7065 2021-02-12T23:49:01Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.17 psi 2021-02-12T23:49:01Z- handleDepth()-Depth_Keller msg: 1613173741245 epoch sec; seqNo:7122; depth 9.47 2021-02-12T23:49:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173741.26 epoch sec; seqNo:7122; 4057917.21 north; 591561.29 east 2021-02-12T23:49:01Z- AHRS_M2 msg: 1613173741.64 epoch sec; seqNo:7066 2021-02-12T23:49:01Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-12T23:49:01Z- handleDepth()-Depth_Keller msg: 1613173741643 epoch sec; seqNo:7123; depth 9.36 2021-02-12T23:49:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173741.65 epoch sec; seqNo:7123; 4057917.59 north; 591561.28 east 2021-02-12T23:49:02Z- AHRS_M2 msg: 1613173742.04 epoch sec; seqNo:7067 2021-02-12T23:49:02Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-12T23:49:02Z- handleDepth()-Depth_Keller msg: 1613173742045 epoch sec; seqNo:7124; depth 9.25 2021-02-12T23:49:02Z- handleDvl() - RDI_Pathfinder msg: 1613173742.06 epoch sec; seqNo:3297 2021-02-12T23:49:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 105.35 , 117.52 , 81.02 2021-02-12T23:49:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.03 2021-02-12T23:49:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173742.09 epoch sec; seqNo:7124; 4057917.97 north; 591561.28 east 2021-02-12T23:49:02Z- AHRS_M2 msg: 1613173742.44 epoch sec; seqNo:7068 2021-02-12T23:49:02Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-12T23:49:02Z- handleDepth()-Depth_Keller msg: 1613173742453 epoch sec; seqNo:7125; depth 9.13 2021-02-12T23:49:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173742.46 epoch sec; seqNo:7125; 4057917.97 north; 591561.28 east 2021-02-12T23:49:02Z- AHRS_M2 msg: 1613173742.85 epoch sec; seqNo:7069 2021-02-12T23:49:02Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-12T23:49:02Z- handleDepth()-Depth_Keller msg: 1613173742854 epoch sec; seqNo:7126; depth 9.02 2021-02-12T23:49:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173742.86 epoch sec; seqNo:7126; 4057918.35 north; 591561.27 east 2021-02-12T23:49:03Z- AHRS_M2 msg: 1613173743.25 epoch sec; seqNo:7070 2021-02-12T23:49:03Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.17 psi 2021-02-12T23:49:03Z- handleDepth()-Depth_Keller msg: 1613173743257 epoch sec; seqNo:7127; depth 8.89 2021-02-12T23:49:03Z- handleDvl() - RDI_Pathfinder msg: 1613173743.28 epoch sec; seqNo:3298 2021-02-12T23:49:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 106.50 , 121.37 , 82.17 2021-02-12T23:49:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.08 , -0.07 2021-02-12T23:49:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173743.29 epoch sec; seqNo:7127; 4057918.73 north; 591561.27 east 2021-02-12T23:49:03Z- AHRS_M2 msg: 1613173743.66 epoch sec; seqNo:7071 2021-02-12T23:49:03Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.17 psi 2021-02-12T23:49:03Z- handleDepth()-Depth_Keller msg: 1613173743661 epoch sec; seqNo:7128; depth 8.77 2021-02-12T23:49:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173743.67 epoch sec; seqNo:7128; 4057918.73 north; 591561.27 east 2021-02-12T23:49:04Z- AHRS_M2 msg: 1613173744.06 epoch sec; seqNo:7072 2021-02-12T23:49:04Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.17 psi 2021-02-12T23:49:04Z- handleDepth()-Depth_Keller msg: 1613173744069 epoch sec; seqNo:7129; depth 8.66 2021-02-12T23:49:04Z- handleDvl() - RDI_Pathfinder msg: 1613173744.08 epoch sec; seqNo:3299 2021-02-12T23:49:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 107.85 , 120.02 , 0.00 2021-02-12T23:49:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.00 , -0.12 2021-02-12T23:49:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173744.09 epoch sec; seqNo:7129; 4057919.12 north; 591562.49 east 2021-02-12T23:49:04Z- AHRS_M2 msg: 1613173744.47 epoch sec; seqNo:7073 2021-02-12T23:49:04Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.17 psi 2021-02-12T23:49:04Z- handleDepth()-Depth_Keller msg: 1613173744475 epoch sec; seqNo:7130; depth 8.55 2021-02-12T23:49:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173744.49 epoch sec; seqNo:7130; 4057919.50 north; 591562.48 east 2021-02-12T23:49:04Z- AHRS_M2 msg: 1613173744.87 epoch sec; seqNo:7074 2021-02-12T23:49:04Z- AHRS_M2 msg: 0.66 phi; 0.30 theta; -0.17 psi 2021-02-12T23:49:04Z- handleDepth()-Depth_Keller msg: 1613173744873 epoch sec; seqNo:7131; depth 8.44 2021-02-12T23:49:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173744.88 epoch sec; seqNo:7131; 4057919.50 north; 591562.48 east 2021-02-12T23:49:05Z- AHRS_M2 msg: 1613173745.27 epoch sec; seqNo:7075 2021-02-12T23:49:05Z- AHRS_M2 msg: 0.66 phi; 0.30 theta; -0.17 psi 2021-02-12T23:49:05Z- handleDepth()-Depth_Keller msg: 1613173745277 epoch sec; seqNo:7132; depth 8.33 2021-02-12T23:49:05Z- handleDvl() - RDI_Pathfinder msg: 1613173745.29 epoch sec; seqNo:3300 2021-02-12T23:49:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.45 , 106.30 , 118.20 , 82.50 2021-02-12T23:49:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.12 , 0.03 2021-02-12T23:49:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173745.30 epoch sec; seqNo:7132; 4057919.88 north; 591562.48 east 2021-02-12T23:49:05Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-12T23:49:05Z- waiting for 1613173743.06, time is 1613173745.30 2021-02-12T23:49:05Z- LcmTrn::updateTrn() equating pose 1613173745.30 and meas times 1613173745.30 2021-02-12T23:49:05Z- LcmTrn::updateTrn() >>>> pose time 1613173745.30, meas time 1613173745.30 <<<< 2021-02-12T23:49:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173745.30 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:49:05Z- TerrainNav::motionUpdate() - currEstimate = (4057920, 591562.48, 8.33) 2021-02-12T23:49:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:49:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:49:05Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173745.30 sec.; No relevant navigation data available 2021-02-12T23:49:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:49:05Z- TNavBF::mmse selected from filter -1 2021-02-12T23:49:05Z- variances([0], [2], and psi_berg) = 735.044826 833.020357 0.000000 2021-02-12T23:49:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:49:05Z- LcmTrn::publishEstimates() - MLE : 4057911.38 591523.98 8.33 2021-02-12T23:49:05Z- LcmTrn::publishEstimates() - MMSE : 4057919.88 591562.48 8.33 2021-02-12T23:49:05Z- LcmTrn::publishEstimates() - COVAR: 735.04 833.02 1.31 0.00 2021-02-12T23:49:05Z- AHRS_M2 msg: 1613173745.68 epoch sec; seqNo:7076 2021-02-12T23:49:05Z- AHRS_M2 msg: 0.65 phi; 0.30 theta; -0.16 psi 2021-02-12T23:49:05Z- handleDepth()-Depth_Keller msg: 1613173745681 epoch sec; seqNo:7133; depth 8.22 2021-02-12T23:49:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173745.69 epoch sec; seqNo:7133; 4057920.26 north; 591562.47 east 2021-02-12T23:49:06Z- AHRS_M2 msg: 1613173746.08 epoch sec; seqNo:7077 2021-02-12T23:49:06Z- AHRS_M2 msg: 0.65 phi; 0.30 theta; -0.16 psi 2021-02-12T23:49:06Z- handleDepth()-Depth_Keller msg: 1613173746093 epoch sec; seqNo:7134; depth 8.11 2021-02-12T23:49:06Z- handleDvl() - RDI_Pathfinder msg: 1613173746.11 epoch sec; seqNo:3301 2021-02-12T23:49:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 105.22 , 113.33 , 0.00 2021-02-12T23:49:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.14 , 0.17 2021-02-12T23:49:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173746.12 epoch sec; seqNo:7134; 4057920.26 north; 591562.47 east 2021-02-12T23:49:06Z- AHRS_M2 msg: 1613173746.48 epoch sec; seqNo:7078 2021-02-12T23:49:06Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.17 psi 2021-02-12T23:49:06Z- handleDepth()-Depth_Keller msg: 1613173746489 epoch sec; seqNo:7135; depth 8.00 2021-02-12T23:49:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173746.51 epoch sec; seqNo:7135; 4057920.64 north; 591562.47 east 2021-02-12T23:49:06Z- AHRS_M2 msg: 1613173746.89 epoch sec; seqNo:7079 2021-02-12T23:49:06Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.18 psi 2021-02-12T23:49:06Z- handleDepth()-Depth_Keller msg: 1613173746893 epoch sec; seqNo:7136; depth 7.90 2021-02-12T23:49:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173746.90 epoch sec; seqNo:7136; 4057921.03 north; 591563.69 east 2021-02-12T23:49:07Z- AHRS_M2 msg: 1613173747.29 epoch sec; seqNo:7080 2021-02-12T23:49:07Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.19 psi 2021-02-12T23:49:07Z- handleDepth()-Depth_Keller msg: 1613173747297 epoch sec; seqNo:7137; depth 7.80 2021-02-12T23:49:07Z- handleDvl() - RDI_Pathfinder msg: 1613173747.31 epoch sec; seqNo:3302 2021-02-12T23:49:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.72 , 106.57 , 115.49 , 82.77 2021-02-12T23:49:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.10 , 0.08 2021-02-12T23:49:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173747.32 epoch sec; seqNo:7137; 4057921.41 north; 591563.68 east 2021-02-12T23:49:07Z- AHRS_M2 msg: 1613173747.70 epoch sec; seqNo:7081 2021-02-12T23:49:07Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.18 psi 2021-02-12T23:49:07Z- handleDepth()-Depth_Keller msg: 1613173747702 epoch sec; seqNo:7138; depth 7.71 2021-02-12T23:49:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173747.71 epoch sec; seqNo:7138; 4057921.41 north; 591563.68 east 2021-02-12T23:49:08Z- AHRS_M2 msg: 1613173748.10 epoch sec; seqNo:7082 2021-02-12T23:49:08Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.18 psi 2021-02-12T23:49:08Z- handleDepth()-Depth_Keller msg: 1613173748250 epoch sec; seqNo:7139; depth 7.61 2021-02-12T23:49:08Z- handleDvl() - RDI_Pathfinder msg: 1613173748.26 epoch sec; seqNo:3303 2021-02-12T23:49:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:49:08Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:49:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173748.27 epoch sec; seqNo:7139; 4057922.17 north; 591563.68 east 2021-02-12T23:49:08Z- AHRS_M2 msg: 1613173748.50 epoch sec; seqNo:7083 2021-02-12T23:49:08Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.17 psi 2021-02-12T23:49:08Z- handleDepth()-Depth_Keller msg: 1613173748528 epoch sec; seqNo:7140; depth 7.48 2021-02-12T23:49:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173748.56 epoch sec; seqNo:7140; 4057922.17 north; 591563.68 east 2021-02-12T23:49:08Z- AHRS_M2 msg: 1613173748.91 epoch sec; seqNo:7084 2021-02-12T23:49:08Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-12T23:49:08Z- handleDepth()-Depth_Keller msg: 1613173748945 epoch sec; seqNo:7141; depth 7.42 2021-02-12T23:49:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173748.96 epoch sec; seqNo:7141; 4057922.54 north; 591563.67 east 2021-02-12T23:49:09Z- AHRS_M2 msg: 1613173749.31 epoch sec; seqNo:7085 2021-02-12T23:49:09Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-12T23:49:09Z- handleDepth()-Depth_Keller msg: 1613173749349 epoch sec; seqNo:7142; depth 7.32 2021-02-12T23:49:09Z- handleDvl() - RDI_Pathfinder msg: 1613173749.36 epoch sec; seqNo:3304 2021-02-12T23:49:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.80 , 106.16 , 119.55 , 83.86 2021-02-12T23:49:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.11 , 0.11 2021-02-12T23:49:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173749.38 epoch sec; seqNo:7142; 4057922.92 north; 591563.67 east 2021-02-12T23:49:09Z- AHRS_M2 msg: 1613173749.72 epoch sec; seqNo:7086 2021-02-12T23:49:09Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-12T23:49:09Z- handleDepth()-Depth_Keller msg: 1613173749720 epoch sec; seqNo:7143; depth 7.22 2021-02-12T23:49:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173749.73 epoch sec; seqNo:7143; 4057922.94 north; 591564.89 east 2021-02-12T23:49:10Z- AHRS_M2 msg: 1613173750.12 epoch sec; seqNo:7087 2021-02-12T23:49:10Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-12T23:49:10Z- handleDepth()-Depth_Keller msg: 1613173750135 epoch sec; seqNo:7144; depth 7.12 2021-02-12T23:49:10Z- handleDvl() - RDI_Pathfinder msg: 1613173750.16 epoch sec; seqNo:3305 2021-02-12T23:49:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.62 , 105.14 , 117.31 , 83.51 2021-02-12T23:49:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.10 , 0.20 2021-02-12T23:49:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173750.17 epoch sec; seqNo:7144; 4057923.32 north; 591564.88 east 2021-02-12T23:49:10Z- AHRS_M2 msg: 1613173750.52 epoch sec; seqNo:7088 2021-02-12T23:49:10Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-12T23:49:10Z- LcmTrn::handleMessages() - handling 42 messages took 0.84 ms and 125 calls... 2021-02-12T23:49:10Z- waiting for 1613173748.30, time is 1613173750.52 2021-02-12T23:49:10Z- LcmTrn::updateTrn() >>>> pose time 1613173750.52, meas time 1613173750.16 <<<< 2021-02-12T23:49:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173750.16 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:49:10Z- TerrainNav::motionUpdate() - currEstimate = (4057923, 591564.88, 7.12) 2021-02-12T23:49:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:49:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:49:10Z- TNavBF::Projecting Measurements 2021-02-12T23:49:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:49:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:49:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:49:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:49:10Z- TNavFilter::4 good beams. 2021-02-12T23:49:10Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:49:10Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:49:10Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:49:10Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:49:10Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:49:10Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:49:10Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:49:10Z- TNavBF::mmse selected from filter -1 2021-02-12T23:49:10Z- TerrainMapOctree: (vehN, vehE ) = (4057923.08, 591564.72). 2021-02-12T23:49:10Z- TerrainMapOctree: Min Distance = 4275.42. 2021-02-12T23:49:10Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:49:10Z- There is no specific good beam that all particles have in common. 2021-02-12T23:49:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:49:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.51 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 13.43 2021-02-12T23:49:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:49:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 17.69 2021-02-12T23:49:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:49:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 22.02 2021-02-12T23:49:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:49:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 11.16 2021-02-12T23:49:10Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:49:10Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:49:10Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:49:10Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:49:10Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:49:10Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:49:10Z- Filter 0 No beams used in subcloud update 2021-02-12T23:49:10Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:49:10Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:49:10Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 735.0 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:49:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173750.16 sec. 2021-02-12T23:49:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:49:10Z- TNavBF::mmse selected from filter -1 2021-02-12T23:49:10Z- variances([0], [2], and psi_berg) = 734.943560 833.020279 0.000000 2021-02-12T23:49:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:49:10Z- LcmTrn::publishEstimates() - MLE : 4057914.82 591518.38 7.12 2021-02-12T23:49:10Z- LcmTrn::publishEstimates() - MMSE : 4057923.32 591564.88 7.12 2021-02-12T23:49:10Z- LcmTrn::publishEstimates() - COVAR: 734.94 833.02 1.13 0.00 2021-02-12T23:49:10Z- handleDepth()-Depth_Keller msg: 1613173750530 epoch sec; seqNo:7145; depth 7.02 2021-02-12T23:49:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173750.54 epoch sec; seqNo:7145; 4057923.69 north; 591564.88 east 2021-02-12T23:49:10Z- AHRS_M2 msg: 1613173750.93 epoch sec; seqNo:7089 2021-02-12T23:49:10Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.18 psi 2021-02-12T23:49:10Z- handleDepth()-Depth_Keller msg: 1613173750933 epoch sec; seqNo:7146; depth 6.92 2021-02-12T23:49:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173750.95 epoch sec; seqNo:7146; 4057924.07 north; 591564.88 east 2021-02-12T23:49:11Z- AHRS_M2 msg: 1613173751.33 epoch sec; seqNo:7090 2021-02-12T23:49:11Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.19 psi 2021-02-12T23:49:11Z- handleDepth()-Depth_Keller msg: 1613173751341 epoch sec; seqNo:7147; depth 6.80 2021-02-12T23:49:11Z- handleDvl() - RDI_Pathfinder msg: 1613173751.35 epoch sec; seqNo:3306 2021-02-12T23:49:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.69 , 107.91 , 121.43 , 83.58 2021-02-12T23:49:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.28 , 0.10 2021-02-12T23:49:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173751.38 epoch sec; seqNo:7147; 4057924.07 north; 591564.88 east 2021-02-12T23:49:11Z- AHRS_M2 msg: 1613173751.74 epoch sec; seqNo:7091 2021-02-12T23:49:11Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.20 psi 2021-02-12T23:49:11Z- handleDepth()-Depth_Keller msg: 1613173751742 epoch sec; seqNo:7148; depth 6.70 2021-02-12T23:49:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173751.75 epoch sec; seqNo:7148; 4057924.46 north; 591566.09 east 2021-02-12T23:49:12Z- AHRS_M2 msg: 1613173752.14 epoch sec; seqNo:7092 2021-02-12T23:49:12Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.20 psi 2021-02-12T23:49:12Z- handleDepth()-Depth_Keller msg: 1613173752166 epoch sec; seqNo:7149; depth 6.60 2021-02-12T23:49:12Z- handleDvl() - RDI_Pathfinder msg: 1613173752.18 epoch sec; seqNo:3307 2021-02-12T23:49:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:49:12Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:49:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173752.19 epoch sec; seqNo:7149; 4057924.84 north; 591566.09 east 2021-02-12T23:49:12Z- AHRS_M2 msg: 1613173752.54 epoch sec; seqNo:7093 2021-02-12T23:49:12Z- AHRS_M2 msg: 0.61 phi; 0.29 theta; -0.21 psi 2021-02-12T23:49:12Z- handleDepth()-Depth_Keller msg: 1613173752553 epoch sec; seqNo:7150; depth 6.46 2021-02-12T23:49:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173752.58 epoch sec; seqNo:7150; 4057924.84 north; 591566.09 east 2021-02-12T23:49:12Z- AHRS_M2 msg: 1613173752.95 epoch sec; seqNo:7094 2021-02-12T23:49:12Z- AHRS_M2 msg: 0.63 phi; 0.25 theta; -0.21 psi 2021-02-12T23:49:12Z- handleDepth()-Depth_Keller msg: 1613173752956 epoch sec; seqNo:7151; depth 6.34 2021-02-12T23:49:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173752.97 epoch sec; seqNo:7151; 4057925.22 north; 591566.08 east 2021-02-12T23:49:13Z- AHRS_M2 msg: 1613173753.36 epoch sec; seqNo:7095 2021-02-12T23:49:13Z- AHRS_M2 msg: 0.67 phi; 0.16 theta; -0.18 psi 2021-02-12T23:49:13Z- handleDepth()-Depth_Keller msg: 1613173753364 epoch sec; seqNo:7152; depth 6.23 2021-02-12T23:49:13Z- handleDvl() - RDI_Pathfinder msg: 1613173753.38 epoch sec; seqNo:3308 2021-02-12T23:49:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 110.62 , 116.57 , 85.34 2021-02-12T23:49:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , -0.28 2021-02-12T23:49:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173753.39 epoch sec; seqNo:7152; 4057925.61 north; 591567.30 east 2021-02-12T23:49:13Z- AHRS_M2 msg: 1613173753.76 epoch sec; seqNo:7096 2021-02-12T23:49:13Z- AHRS_M2 msg: 0.67 phi; 0.11 theta; -0.17 psi 2021-02-12T23:49:13Z- handleDepth()-Depth_Keller msg: 1613173753763 epoch sec; seqNo:7153; depth 6.11 2021-02-12T23:49:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173753.78 epoch sec; seqNo:7153; 4057925.61 north; 591567.30 east 2021-02-12T23:49:14Z- AHRS_M2 msg: 1613173754.16 epoch sec; seqNo:7097 2021-02-12T23:49:14Z- AHRS_M2 msg: 0.67 phi; 0.07 theta; -0.16 psi 2021-02-12T23:49:14Z- handleDepth()-Depth_Keller msg: 1613173754168 epoch sec; seqNo:7154; depth 6.01 2021-02-12T23:49:14Z- handleDvl() - RDI_Pathfinder msg: 1613173754.20 epoch sec; seqNo:3309 2021-02-12T23:49:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.51 , 105.35 , 106.84 , 0.00 2021-02-12T23:49:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.21 , -0.07 2021-02-12T23:49:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173754.22 epoch sec; seqNo:7154; 4057925.99 north; 591567.30 east 2021-02-12T23:49:14Z- AHRS_M2 msg: 1613173754.56 epoch sec; seqNo:7098 2021-02-12T23:49:14Z- AHRS_M2 msg: 0.67 phi; 0.02 theta; -0.17 psi 2021-02-12T23:49:14Z- handleDepth()-Depth_Keller msg: 1613173754569 epoch sec; seqNo:7155; depth 5.92 2021-02-12T23:49:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173754.58 epoch sec; seqNo:7155; 4057926.37 north; 591567.29 east 2021-02-12T23:49:14Z- AHRS_M2 msg: 1613173754.97 epoch sec; seqNo:7099 2021-02-12T23:49:14Z- AHRS_M2 msg: 0.67 phi; -0.01 theta; -0.17 psi 2021-02-12T23:49:14Z- handleDepth()-Depth_Keller msg: 1613173754974 epoch sec; seqNo:7156; depth 5.85 2021-02-12T23:49:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173754.99 epoch sec; seqNo:7156; 4057926.75 north; 591567.29 east 2021-02-12T23:49:15Z- AHRS_M2 msg: 1613173755.37 epoch sec; seqNo:7100 2021-02-12T23:49:15Z- AHRS_M2 msg: 0.67 phi; -0.05 theta; -0.17 psi 2021-02-12T23:49:15Z- handleDepth()-Depth_Keller msg: 1613173755377 epoch sec; seqNo:7157; depth 5.78 2021-02-12T23:49:15Z- handleDvl() - RDI_Pathfinder msg: 1613173755.39 epoch sec; seqNo:3310 2021-02-12T23:49:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.44 , 97.04 , 97.04 2021-02-12T23:49:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.11 , 0.12 2021-02-12T23:49:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173755.40 epoch sec; seqNo:7157; 4057926.75 north; 591567.29 east 2021-02-12T23:49:15Z- LcmTrn::handleMessages() - handling 43 messages took 0.69 ms and 129 calls... 2021-02-12T23:49:15Z- waiting for 1613173753.52, time is 1613173755.40 2021-02-12T23:49:15Z- LcmTrn::updateTrn() equating pose 1613173755.40 and meas times 1613173755.40 2021-02-12T23:49:15Z- LcmTrn::updateTrn() >>>> pose time 1613173755.40, meas time 1613173755.40 <<<< 2021-02-12T23:49:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173755.40 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:49:15Z- TerrainNav::motionUpdate() - currEstimate = (4057927, 591567.29, 5.78) 2021-02-12T23:49:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:49:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:49:15Z- TNavBF::Projecting Measurements 2021-02-12T23:49:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:49:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:49:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:49:15Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:49:15Z- TNavFilter::3 good beams. 2021-02-12T23:49:15Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:49:15Z- TNavBF:: beamIndex[0] = 1 2021-02-12T23:49:15Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:49:15Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:49:15Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:49:15Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:49:15Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:49:15Z- TNavBF::mmse selected from filter -1 2021-02-12T23:49:15Z- TerrainMapOctree: (vehN, vehE ) = (4057926.75, 591567.29). 2021-02-12T23:49:15Z- TerrainMapOctree: Min Distance = 4270.94. 2021-02-12T23:49:15Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:49:15Z- There is no specific good beam that all particles have in common. 2021-02-12T23:49:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:49:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 16.79 2021-02-12T23:49:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:49:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 15.07 2021-02-12T23:49:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:49:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 15.07 2021-02-12T23:49:15Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:49:15Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:49:15Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:49:15Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:49:15Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:49:15Z- Filter 0 No beams used in subcloud update 2021-02-12T23:49:15Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:49:15Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:49:15Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 734.9 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:49:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173755.40 sec. 2021-02-12T23:49:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:49:15Z- TNavBF::mmse selected from filter -1 2021-02-12T23:49:15Z- variances([0], [2], and psi_berg) = 734.861229 833.020215 0.000000 2021-02-12T23:49:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:49:15Z- LcmTrn::publishEstimates() - MLE : 4057918.25 591520.79 5.78 2021-02-12T23:49:15Z- LcmTrn::publishEstimates() - MMSE : 4057926.75 591567.29 5.78 2021-02-12T23:49:15Z- LcmTrn::publishEstimates() - COVAR: 734.86 833.02 1.09 0.00 2021-02-12T23:49:15Z- AHRS_M2 msg: 1613173755.78 epoch sec; seqNo:7101 2021-02-12T23:49:15Z- AHRS_M2 msg: 0.67 phi; -0.08 theta; -0.17 psi 2021-02-12T23:49:15Z- handleDepth()-Depth_Keller msg: 1613173755783 epoch sec; seqNo:7158; depth 5.75 2021-02-12T23:49:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173755.80 epoch sec; seqNo:7158; 4057927.13 north; 591567.29 east 2021-02-12T23:49:16Z- AHRS_M2 msg: 1613173756.18 epoch sec; seqNo:7102 2021-02-12T23:49:16Z- AHRS_M2 msg: 0.67 phi; -0.12 theta; -0.17 psi 2021-02-12T23:49:16Z- handleDepth()-Depth_Keller msg: 1613173756190 epoch sec; seqNo:7159; depth 5.73 2021-02-12T23:49:16Z- handleDvl() - RDI_Pathfinder msg: 1613173756.20 epoch sec; seqNo:3311 2021-02-12T23:49:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 103.86 , 97.10 , 97.10 2021-02-12T23:49:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.18 , 0.07 2021-02-12T23:49:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173756.21 epoch sec; seqNo:7159; 4057927.52 north; 591568.50 east 2021-02-12T23:49:16Z- AHRS_M2 msg: 1613173756.59 epoch sec; seqNo:7103 2021-02-12T23:49:16Z- AHRS_M2 msg: 0.68 phi; -0.14 theta; -0.18 psi 2021-02-12T23:49:16Z- handleDepth()-Depth_Keller msg: 1613173756590 epoch sec; seqNo:7160; depth 5.73 2021-02-12T23:49:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173756.60 epoch sec; seqNo:7160; 4057927.52 north; 591568.50 east 2021-02-12T23:49:16Z- AHRS_M2 msg: 1613173756.99 epoch sec; seqNo:7104 2021-02-12T23:49:16Z- AHRS_M2 msg: 0.68 phi; -0.16 theta; -0.20 psi 2021-02-12T23:49:16Z- handleDepth()-Depth_Keller msg: 1613173756993 epoch sec; seqNo:7161; depth 5.73 2021-02-12T23:49:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173757.01 epoch sec; seqNo:7161; 4057927.90 north; 591568.50 east 2021-02-12T23:49:17Z- AHRS_M2 msg: 1613173757.39 epoch sec; seqNo:7105 2021-02-12T23:49:17Z- AHRS_M2 msg: 0.68 phi; -0.18 theta; -0.22 psi 2021-02-12T23:49:17Z- handleDepth()-Depth_Keller msg: 1613173757397 epoch sec; seqNo:7162; depth 5.72 2021-02-12T23:49:17Z- handleDvl() - RDI_Pathfinder msg: 1613173757.41 epoch sec; seqNo:3312 2021-02-12T23:49:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.84 , 104.06 , 97.30 , 102.71 2021-02-12T23:49:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.14 , -0.27 2021-02-12T23:49:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173757.43 epoch sec; seqNo:7162; 4057927.90 north; 591568.50 east 2021-02-12T23:49:17Z- AHRS_M2 msg: 1613173757.80 epoch sec; seqNo:7106 2021-02-12T23:49:17Z- AHRS_M2 msg: 0.69 phi; -0.19 theta; -0.25 psi 2021-02-12T23:49:17Z- handleDepth()-Depth_Keller msg: 1613173757801 epoch sec; seqNo:7163; depth 5.75 2021-02-12T23:49:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173757.81 epoch sec; seqNo:7163; 4057928.28 north; 591568.49 east 2021-02-12T23:49:18Z- AHRS_M2 msg: 1613173758.20 epoch sec; seqNo:7107 2021-02-12T23:49:18Z- AHRS_M2 msg: 0.71 phi; -0.21 theta; -0.26 psi 2021-02-12T23:49:18Z- handleDepth()-Depth_Keller msg: 1613173758213 epoch sec; seqNo:7164; depth 5.76 2021-02-12T23:49:18Z- handleDvl() - RDI_Pathfinder msg: 1613173758.23 epoch sec; seqNo:3313 2021-02-12T23:49:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 109.33 , 87.71 , 109.33 2021-02-12T23:49:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.30 , -0.29 2021-02-12T23:49:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173758.24 epoch sec; seqNo:7164; 4057928.66 north; 591568.49 east 2021-02-12T23:49:18Z- AHRS_M2 msg: 1613173758.60 epoch sec; seqNo:7108 2021-02-12T23:49:18Z- AHRS_M2 msg: 0.72 phi; -0.23 theta; -0.25 psi 2021-02-12T23:49:18Z- handleDepth()-Depth_Keller msg: 1613173758609 epoch sec; seqNo:7165; depth 5.81 2021-02-12T23:49:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173758.63 epoch sec; seqNo:7165; 4057929.05 north; 591569.71 east 2021-02-12T23:49:19Z- AHRS_M2 msg: 1613173759.01 epoch sec; seqNo:7109 2021-02-12T23:49:19Z- AHRS_M2 msg: 0.73 phi; -0.25 theta; -0.23 psi 2021-02-12T23:49:19Z- handleDepth()-Depth_Keller msg: 1613173759013 epoch sec; seqNo:7166; depth 5.87 2021-02-12T23:49:19Z- handleDvl() - RDI_Pathfinder msg: 1613173759.03 epoch sec; seqNo:3314 2021-02-12T23:49:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:49:19Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:49:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173759.06 epoch sec; seqNo:7166; 4057929.43 north; 591569.70 east 2021-02-12T23:49:19Z- AHRS_M2 msg: 1613173759.41 epoch sec; seqNo:7110 2021-02-12T23:49:19Z- AHRS_M2 msg: 0.73 phi; -0.28 theta; -0.21 psi 2021-02-12T23:49:19Z- handleDepth()-Depth_Keller msg: 1613173759417 epoch sec; seqNo:7167; depth 5.94 2021-02-12T23:49:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173759.43 epoch sec; seqNo:7167; 4057929.81 north; 591569.70 east 2021-02-12T23:49:19Z- AHRS_M2 msg: 1613173759.82 epoch sec; seqNo:7111 2021-02-12T23:49:19Z- AHRS_M2 msg: 0.73 phi; -0.30 theta; -0.18 psi 2021-02-12T23:49:19Z- handleDepth()-Depth_Keller msg: 1613173759830 epoch sec; seqNo:7168; depth 6.00 2021-02-12T23:49:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173759.84 epoch sec; seqNo:7168; 4057930.19 north; 591569.69 east 2021-02-12T23:49:20Z- AHRS_M2 msg: 1613173760.22 epoch sec; seqNo:7112 2021-02-12T23:49:20Z- AHRS_M2 msg: 0.73 phi; -0.31 theta; -0.15 psi 2021-02-12T23:49:20Z- handleDepth()-Depth_Keller msg: 1613173760234 epoch sec; seqNo:7169; depth 6.05 2021-02-12T23:49:20Z- handleDvl() - RDI_Pathfinder msg: 1613173760.25 epoch sec; seqNo:3315 2021-02-12T23:49:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 109.81 , 84.53 , 117.25 2021-02-12T23:49:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.54 , -0.21 , 0.34 2021-02-12T23:49:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173760.26 epoch sec; seqNo:7169; 4057930.19 north; 591569.69 east 2021-02-12T23:49:20Z- AHRS_M2 msg: 1613173760.62 epoch sec; seqNo:7113 2021-02-12T23:49:20Z- AHRS_M2 msg: 0.72 phi; -0.33 theta; -0.13 psi 2021-02-12T23:49:20Z- handleDepth()-Depth_Keller msg: 1613173760629 epoch sec; seqNo:7170; depth 6.12 2021-02-12T23:49:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173760.64 epoch sec; seqNo:7170; 4057930.57 north; 591569.69 east 2021-02-12T23:49:20Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-12T23:49:20Z- waiting for 1613173758.40, time is 1613173760.64 2021-02-12T23:49:20Z- LcmTrn::updateTrn() >>>> pose time 1613173760.64, meas time 1613173760.25 <<<< 2021-02-12T23:49:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173760.25 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:49:20Z- TerrainNav::motionUpdate() - currEstimate = (4057931, 591569.69, 6.12) 2021-02-12T23:49:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:49:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:49:20Z- TNavBF::Projecting Measurements 2021-02-12T23:49:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:49:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:49:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:49:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:49:20Z- TNavFilter::4 good beams. 2021-02-12T23:49:20Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:49:20Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:49:20Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:49:20Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:49:20Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:49:20Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:49:20Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:49:20Z- TNavBF::mmse selected from filter -1 2021-02-12T23:49:20Z- TerrainMapOctree: (vehN, vehE ) = (4057930.28, 591569.51). 2021-02-12T23:49:20Z- TerrainMapOctree: Min Distance = 4266.80. 2021-02-12T23:49:20Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:49:20Z- There is no specific good beam that all particles have in common. 2021-02-12T23:49:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:49:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 12.67 2021-02-12T23:49:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:49:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 19.29 2021-02-12T23:49:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:49:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 11.43 2021-02-12T23:49:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:49:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 22.00 2021-02-12T23:49:20Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:49:20Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:49:20Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:49:20Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:49:20Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:49:20Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:49:20Z- Filter 0 No beams used in subcloud update 2021-02-12T23:49:20Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:49:20Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:49:20Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 734.8 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:49:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173760.25 sec. 2021-02-12T23:49:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:49:20Z- TNavBF::mmse selected from filter -1 2021-02-12T23:49:20Z- variances([0], [2], and psi_berg) = 734.755866 833.020134 0.000000 2021-02-12T23:49:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:49:20Z- LcmTrn::publishEstimates() - MLE : 4057922.07 591523.19 6.12 2021-02-12T23:49:20Z- LcmTrn::publishEstimates() - MMSE : 4057930.57 591569.69 6.12 2021-02-12T23:49:20Z- LcmTrn::publishEstimates() - COVAR: 734.76 833.02 1.01 0.00 2021-02-12T23:49:21Z- handleDepth()-Depth_Keller msg: 1613173761049 epoch sec; seqNo:7171; depth 6.20 2021-02-12T23:49:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173761.06 epoch sec; seqNo:7171; 4057930.57 north; 591569.69 east 2021-02-12T23:49:21Z- AHRS_M2 msg: 1613173761.43 epoch sec; seqNo:7114 2021-02-12T23:49:21Z- AHRS_M2 msg: 0.71 phi; -0.34 theta; -0.14 psi 2021-02-12T23:49:21Z- handleDepth()-Depth_Keller msg: 1613173761502 epoch sec; seqNo:7172; depth 6.29 2021-02-12T23:49:21Z- handleDvl() - RDI_Pathfinder msg: 1613173761.51 epoch sec; seqNo:3316 2021-02-12T23:49:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.98 , 101.09 , 82.16 , 120.01 2021-02-12T23:49:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.62 , 0.09 , 0.24 2021-02-12T23:49:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173761.53 epoch sec; seqNo:7172; 4057930.57 north; 591569.69 east 2021-02-12T23:49:21Z- AHRS_M2 msg: 1613173761.84 epoch sec; seqNo:7115 2021-02-12T23:49:21Z- AHRS_M2 msg: 0.70 phi; -0.34 theta; -0.16 psi 2021-02-12T23:49:21Z- handleDepth()-Depth_Keller msg: 1613173761938 epoch sec; seqNo:7173; depth 6.39 2021-02-12T23:49:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173761.95 epoch sec; seqNo:7173; 4057930.94 north; 591569.69 east 2021-02-12T23:49:22Z- AHRS_M2 msg: 1613173762.24 epoch sec; seqNo:7116 2021-02-12T23:49:22Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.19 psi 2021-02-12T23:49:22Z- handleDepth()-Depth_Keller msg: 1613173762289 epoch sec; seqNo:7174; depth 6.50 2021-02-12T23:49:22Z- handleDvl() - RDI_Pathfinder msg: 1613173762.31 epoch sec; seqNo:3317 2021-02-12T23:49:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.10 , 106.56 , 82.23 , 125.49 2021-02-12T23:49:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.15 , 0.04 2021-02-12T23:49:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173762.32 epoch sec; seqNo:7174; 4057930.96 north; 591570.91 east 2021-02-12T23:49:22Z- AHRS_M2 msg: 1613173762.64 epoch sec; seqNo:7117 2021-02-12T23:49:22Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.22 psi 2021-02-12T23:49:22Z- handleDepth()-Depth_Keller msg: 1613173762686 epoch sec; seqNo:7175; depth 6.57 2021-02-12T23:49:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173762.70 epoch sec; seqNo:7175; 4057931.34 north; 591570.90 east 2021-02-12T23:49:23Z- AHRS_M2 msg: 1613173763.05 epoch sec; seqNo:7118 2021-02-12T23:49:23Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.24 psi 2021-02-12T23:49:23Z- handleDepth()-Depth_Keller msg: 1613173763061 epoch sec; seqNo:7176; depth 6.67 2021-02-12T23:49:23Z- handleDvl() - RDI_Pathfinder msg: 1613173763.07 epoch sec; seqNo:3318 2021-02-12T23:49:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 114.21 , 0.00 , 124.62 2021-02-12T23:49:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , 0.02 2021-02-12T23:49:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173763.08 epoch sec; seqNo:7176; 4057931.34 north; 591570.90 east 2021-02-12T23:49:23Z- AHRS_M2 msg: 1613173763.45 epoch sec; seqNo:7119 2021-02-12T23:49:23Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.25 psi 2021-02-12T23:49:23Z- handleDepth()-Depth_Keller msg: 1613173763477 epoch sec; seqNo:7177; depth 6.76 2021-02-12T23:49:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173763.49 epoch sec; seqNo:7177; 4057931.71 north; 591570.90 east 2021-02-12T23:49:23Z- AHRS_M2 msg: 1613173763.86 epoch sec; seqNo:7120 2021-02-12T23:49:23Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.26 psi 2021-02-12T23:49:23Z- handleDepth()-Depth_Keller msg: 1613173763861 epoch sec; seqNo:7178; depth 6.88 2021-02-12T23:49:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173763.87 epoch sec; seqNo:7178; 4057932.09 north; 591570.90 east 2021-02-12T23:49:24Z- AHRS_M2 msg: 1613173764.26 epoch sec; seqNo:7121 2021-02-12T23:49:24Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.26 psi 2021-02-12T23:49:24Z- handleDepth()-Depth_Keller msg: 1613173764273 epoch sec; seqNo:7179; depth 6.98 2021-02-12T23:49:24Z- handleDvl() - RDI_Pathfinder msg: 1613173764.29 epoch sec; seqNo:3319 2021-02-12T23:49:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.32 , 116.57 , 0.00 , 124.01 2021-02-12T23:49:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.18 2021-02-12T23:49:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173764.31 epoch sec; seqNo:7179; 4057932.09 north; 591570.90 east 2021-02-12T23:49:24Z- AHRS_M2 msg: 1613173764.66 epoch sec; seqNo:7122 2021-02-12T23:49:24Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.26 psi 2021-02-12T23:49:24Z- handleDepth()-Depth_Keller msg: 1613173764670 epoch sec; seqNo:7180; depth 7.10 2021-02-12T23:49:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173764.68 epoch sec; seqNo:7180; 4057932.47 north; 591570.89 east 2021-02-12T23:49:25Z- handleDepth()-Depth_Keller msg: 1613173765090 epoch sec; seqNo:7181; depth 7.21 2021-02-12T23:49:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173765.10 epoch sec; seqNo:7181; 4057932.86 north; 591572.11 east 2021-02-12T23:49:25Z- AHRS_M2 msg: 1613173765.47 epoch sec; seqNo:7123 2021-02-12T23:49:25Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.24 psi 2021-02-12T23:49:25Z- handleDepth()-Depth_Keller msg: 1613173765484 epoch sec; seqNo:7182; depth 7.34 2021-02-12T23:49:25Z- handleDvl() - RDI_Pathfinder msg: 1613173765.51 epoch sec; seqNo:3320 2021-02-12T23:49:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.31 , 116.64 , 83.92 , 127.05 2021-02-12T23:49:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.07 , 0.17 2021-02-12T23:49:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173765.52 epoch sec; seqNo:7182; 4057932.86 north; 591572.11 east 2021-02-12T23:49:25Z- LcmTrn::handleMessages() - handling 39 messages took 0.69 ms and 125 calls... 2021-02-12T23:49:25Z- waiting for 1613173763.64, time is 1613173765.52 2021-02-12T23:49:25Z- LcmTrn::updateTrn() equating pose 1613173765.52 and meas times 1613173765.52 2021-02-12T23:49:25Z- LcmTrn::updateTrn() >>>> pose time 1613173765.52, meas time 1613173765.52 <<<< 2021-02-12T23:49:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173765.52 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:49:25Z- TerrainNav::motionUpdate() - currEstimate = (4057933, 591572.11, 7.34) 2021-02-12T23:49:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:49:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:49:25Z- TNavBF::Projecting Measurements 2021-02-12T23:49:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:49:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:49:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:49:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:49:25Z- TNavFilter::4 good beams. 2021-02-12T23:49:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:49:25Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:49:25Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:49:25Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:49:25Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:49:25Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:49:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:49:25Z- TNavBF::mmse selected from filter -1 2021-02-12T23:49:25Z- TerrainMapOctree: (vehN, vehE ) = (4057932.86, 591572.11). 2021-02-12T23:49:25Z- TerrainMapOctree: Min Distance = 4263.16. 2021-02-12T23:49:25Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:49:25Z- There is no specific good beam that all particles have in common. 2021-02-12T23:49:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:49:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 15.15 2021-02-12T23:49:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:49:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 21.77 2021-02-12T23:49:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:49:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 11.27 2021-02-12T23:49:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:49:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.034 ExpectedMeasDiff: nan currMeas.covariance: 25.83 2021-02-12T23:49:25Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:49:25Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:49:25Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:49:25Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:49:25Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:49:25Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:49:25Z- Filter 0 No beams used in subcloud update 2021-02-12T23:49:25Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:49:25Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:49:25Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 734.7 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:49:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173765.52 sec. 2021-02-12T23:49:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:49:25Z- TNavBF::mmse selected from filter -1 2021-02-12T23:49:25Z- variances([0], [2], and psi_berg) = 734.682604 833.020077 0.000000 2021-02-12T23:49:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:49:25Z- LcmTrn::publishEstimates() - MLE : 4057924.36 591523.61 7.34 2021-02-12T23:49:25Z- LcmTrn::publishEstimates() - MMSE : 4057932.86 591572.11 7.34 2021-02-12T23:49:25Z- LcmTrn::publishEstimates() - COVAR: 734.68 833.02 0.94 0.00 2021-02-12T23:49:25Z- AHRS_M2 msg: 1613173765.88 epoch sec; seqNo:7124 2021-02-12T23:49:25Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.24 psi 2021-02-12T23:49:25Z- handleDepth()-Depth_Keller msg: 1613173765881 epoch sec; seqNo:7183; depth 7.45 2021-02-12T23:49:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173765.89 epoch sec; seqNo:7183; 4057933.24 north; 591572.10 east 2021-02-12T23:49:26Z- AHRS_M2 msg: 1613173766.28 epoch sec; seqNo:7125 2021-02-12T23:49:26Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.24 psi 2021-02-12T23:49:26Z- handleDepth()-Depth_Keller msg: 1613173766293 epoch sec; seqNo:7184; depth 7.54 2021-02-12T23:49:26Z- handleDvl() - RDI_Pathfinder msg: 1613173766.31 epoch sec; seqNo:3321 2021-02-12T23:49:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.65 , 115.42 , 82.70 , 127.31 2021-02-12T23:49:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.08 , 0.01 2021-02-12T23:49:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173766.32 epoch sec; seqNo:7184; 4057933.62 north; 591572.10 east 2021-02-12T23:49:26Z- AHRS_M2 msg: 1613173766.68 epoch sec; seqNo:7126 2021-02-12T23:49:26Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.24 psi 2021-02-12T23:49:26Z- handleDepth()-Depth_Keller msg: 1613173766689 epoch sec; seqNo:7185; depth 7.67 2021-02-12T23:49:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173766.71 epoch sec; seqNo:7185; 4057933.62 north; 591572.10 east 2021-02-12T23:49:27Z- AHRS_M2 msg: 1613173767.10 epoch sec; seqNo:7127 2021-02-12T23:49:27Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.24 psi 2021-02-12T23:49:27Z- handleDepth()-Depth_Keller msg: 1613173767107 epoch sec; seqNo:7186; depth 7.79 2021-02-12T23:49:27Z- handleDvl() - RDI_Pathfinder msg: 1613173767.13 epoch sec; seqNo:3322 2021-02-12T23:49:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 115.62 , 82.91 , 160.23 2021-02-12T23:49:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.32 , -0.28 2021-02-12T23:49:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173767.14 epoch sec; seqNo:7186; 4057934.00 north; 591572.10 east 2021-02-12T23:49:27Z- AHRS_M2 msg: 1613173767.49 epoch sec; seqNo:7128 2021-02-12T23:49:27Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.23 psi 2021-02-12T23:49:27Z- handleDepth()-Depth_Keller msg: 1613173767496 epoch sec; seqNo:7187; depth 7.92 2021-02-12T23:49:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173767.51 epoch sec; seqNo:7187; 4057934.38 north; 591572.09 east 2021-02-12T23:49:27Z- AHRS_M2 msg: 1613173767.90 epoch sec; seqNo:7129 2021-02-12T23:49:27Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.22 psi 2021-02-12T23:49:27Z- handleDepth()-Depth_Keller msg: 1613173767902 epoch sec; seqNo:7188; depth 8.02 2021-02-12T23:49:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173767.91 epoch sec; seqNo:7188; 4057934.77 north; 591573.31 east 2021-02-12T23:49:28Z- AHRS_M2 msg: 1613173768.30 epoch sec; seqNo:7130 2021-02-12T23:49:28Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-12T23:49:28Z- handleDepth()-Depth_Keller msg: 1613173768305 epoch sec; seqNo:7189; depth 8.17 2021-02-12T23:49:28Z- handleDvl() - RDI_Pathfinder msg: 1613173768.32 epoch sec; seqNo:3323 2021-02-12T23:49:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 112.11 , 82.36 , 122.51 2021-02-12T23:49:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , -0.10 2021-02-12T23:49:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173768.33 epoch sec; seqNo:7189; 4057935.15 north; 591573.30 east 2021-02-12T23:49:28Z- AHRS_M2 msg: 1613173768.71 epoch sec; seqNo:7131 2021-02-12T23:49:28Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-12T23:49:28Z- handleDepth()-Depth_Keller msg: 1613173768711 epoch sec; seqNo:7190; depth 8.28 2021-02-12T23:49:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173768.72 epoch sec; seqNo:7190; 4057935.15 north; 591573.30 east 2021-02-12T23:49:29Z- AHRS_M2 msg: 1613173769.11 epoch sec; seqNo:7132 2021-02-12T23:49:29Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-12T23:49:29Z- handleDepth()-Depth_Keller msg: 1613173769113 epoch sec; seqNo:7191; depth 8.40 2021-02-12T23:49:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173769.14 epoch sec; seqNo:7191; 4057935.53 north; 591573.30 east 2021-02-12T23:49:29Z- AHRS_M2 msg: 1613173769.51 epoch sec; seqNo:7133 2021-02-12T23:49:29Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.18 psi 2021-02-12T23:49:29Z- handleDepth()-Depth_Keller msg: 1613173769517 epoch sec; seqNo:7192; depth 8.53 2021-02-12T23:49:29Z- handleDvl() - RDI_Pathfinder msg: 1613173769.53 epoch sec; seqNo:3324 2021-02-12T23:49:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.86 , 109.68 , 81.42 , 118.60 2021-02-12T23:49:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.06 , -0.11 2021-02-12T23:49:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173769.56 epoch sec; seqNo:7192; 4057935.91 north; 591573.30 east 2021-02-12T23:49:29Z- AHRS_M2 msg: 1613173769.92 epoch sec; seqNo:7134 2021-02-12T23:49:29Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.18 psi 2021-02-12T23:49:29Z- handleDepth()-Depth_Keller msg: 1613173769922 epoch sec; seqNo:7193; depth 8.63 2021-02-12T23:49:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173769.94 epoch sec; seqNo:7193; 4057935.91 north; 591573.30 east 2021-02-12T23:49:30Z- AHRS_M2 msg: 1613173770.32 epoch sec; seqNo:7135 2021-02-12T23:49:30Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.18 psi 2021-02-12T23:49:30Z- handleDepth()-Depth_Keller msg: 1613173770335 epoch sec; seqNo:7194; depth 8.75 2021-02-12T23:49:30Z- handleDvl() - RDI_Pathfinder msg: 1613173770.35 epoch sec; seqNo:3325 2021-02-12T23:49:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 106.50 , 79.28 , 114.61 2021-02-12T23:49:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.01 , -0.16 2021-02-12T23:49:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173770.36 epoch sec; seqNo:7194; 4057936.29 north; 591573.29 east 2021-02-12T23:49:30Z- AHRS_M2 msg: 1613173770.73 epoch sec; seqNo:7136 2021-02-12T23:49:30Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.19 psi 2021-02-12T23:49:30Z- handleDepth()-Depth_Keller msg: 1613173770735 epoch sec; seqNo:7195; depth 8.85 2021-02-12T23:49:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173770.76 epoch sec; seqNo:7195; 4057936.68 north; 591574.51 east 2021-02-12T23:49:30Z- LcmTrn::handleMessages() - handling 44 messages took 0.76 ms and 129 calls... 2021-02-12T23:49:30Z- waiting for 1613173768.52, time is 1613173770.76 2021-02-12T23:49:30Z- LcmTrn::updateTrn() >>>> pose time 1613173770.76, meas time 1613173770.35 <<<< 2021-02-12T23:49:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173770.35 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:49:30Z- TerrainNav::motionUpdate() - currEstimate = (4057937, 591574.51, 8.85) 2021-02-12T23:49:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:49:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:49:30Z- TNavBF::Projecting Measurements 2021-02-12T23:49:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:49:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:49:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:49:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:49:30Z- TNavFilter::4 good beams. 2021-02-12T23:49:30Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:49:30Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:49:30Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:49:30Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:49:30Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:49:30Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:49:30Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:49:30Z- TNavBF::mmse selected from filter -1 2021-02-12T23:49:30Z- TerrainMapOctree: (vehN, vehE ) = (4057936.38, 591574.32). 2021-02-12T23:49:30Z- TerrainMapOctree: Min Distance = 4259.03. 2021-02-12T23:49:30Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:49:30Z- There is no specific good beam that all particles have in common. 2021-02-12T23:49:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:49:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 15.07 2021-02-12T23:49:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:49:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 18.15 2021-02-12T23:49:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:49:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 10.06 2021-02-12T23:49:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:49:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 21.02 2021-02-12T23:49:30Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:49:30Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:49:30Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:49:30Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:49:30Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:49:30Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:49:30Z- Filter 0 No beams used in subcloud update 2021-02-12T23:49:30Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:49:30Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:49:30Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 734.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:49:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173770.35 sec. 2021-02-12T23:49:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:49:30Z- TNavBF::mmse selected from filter -1 2021-02-12T23:49:30Z- variances([0], [2], and psi_berg) = 734.577146 833.019996 0.000000 2021-02-12T23:49:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:49:30Z- LcmTrn::publishEstimates() - MLE : 4057928.18 591526.01 8.85 2021-02-12T23:49:30Z- LcmTrn::publishEstimates() - MMSE : 4057936.68 591574.51 8.85 2021-02-12T23:49:30Z- LcmTrn::publishEstimates() - COVAR: 734.58 833.02 0.92 0.00 2021-02-12T23:49:31Z- AHRS_M2 msg: 1613173771.13 epoch sec; seqNo:7137 2021-02-12T23:49:31Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-12T23:49:31Z- handleDepth()-Depth_Keller msg: 1613173771142 epoch sec; seqNo:7196; depth 8.98 2021-02-12T23:49:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173771.15 epoch sec; seqNo:7196; 4057937.06 north; 591574.51 east 2021-02-12T23:49:31Z- AHRS_M2 msg: 1613173771.53 epoch sec; seqNo:7138 2021-02-12T23:49:31Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.20 psi 2021-02-12T23:49:31Z- handleDepth()-Depth_Keller msg: 1613173771537 epoch sec; seqNo:7197; depth 9.09 2021-02-12T23:49:31Z- handleDvl() - RDI_Pathfinder msg: 1613173771.56 epoch sec; seqNo:3326 2021-02-12T23:49:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 107.91 , 80.88 , 120.08 2021-02-12T23:49:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.21 , -0.15 2021-02-12T23:49:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173771.57 epoch sec; seqNo:7197; 4057937.44 north; 591574.50 east 2021-02-12T23:49:31Z- AHRS_M2 msg: 1613173771.94 epoch sec; seqNo:7139 2021-02-12T23:49:31Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.19 psi 2021-02-12T23:49:31Z- handleDepth()-Depth_Keller msg: 1613173771941 epoch sec; seqNo:7198; depth 9.21 2021-02-12T23:49:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173771.95 epoch sec; seqNo:7198; 4057937.44 north; 591574.50 east 2021-02-12T23:49:32Z- AHRS_M2 msg: 1613173772.34 epoch sec; seqNo:7140 2021-02-12T23:49:32Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.17 psi 2021-02-12T23:49:32Z- handleDepth()-Depth_Keller msg: 1613173772347 epoch sec; seqNo:7199; depth 9.34 2021-02-12T23:49:32Z- handleDvl() - RDI_Pathfinder msg: 1613173772.36 epoch sec; seqNo:3327 2021-02-12T23:49:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 109.27 , 80.88 , 121.43 2021-02-12T23:49:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.34 , 0.00 2021-02-12T23:49:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173772.37 epoch sec; seqNo:7199; 4057937.82 north; 591574.50 east 2021-02-12T23:49:32Z- AHRS_M2 msg: 1613173772.74 epoch sec; seqNo:7141 2021-02-12T23:49:32Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.14 psi 2021-02-12T23:49:32Z- handleDepth()-Depth_Keller msg: 1613173772749 epoch sec; seqNo:7200; depth 9.44 2021-02-12T23:49:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173772.76 epoch sec; seqNo:7200; 4057938.19 north; 591574.49 east 2021-02-12T23:49:33Z- AHRS_M2 msg: 1613173773.15 epoch sec; seqNo:7142 2021-02-12T23:49:33Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.11 psi 2021-02-12T23:49:33Z- handleDepth()-Depth_Keller msg: 1613173773162 epoch sec; seqNo:7201; depth 9.57 2021-02-12T23:49:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173773.21 epoch sec; seqNo:7201; 4057938.57 north; 591574.49 east 2021-02-12T23:49:33Z- AHRS_M2 msg: 1613173773.55 epoch sec; seqNo:7143 2021-02-12T23:49:33Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.10 psi 2021-02-12T23:49:33Z- handleDepth()-Depth_Keller msg: 1613173773559 epoch sec; seqNo:7202; depth 9.70 2021-02-12T23:49:33Z- handleDvl() - RDI_Pathfinder msg: 1613173773.57 epoch sec; seqNo:3328 2021-02-12T23:49:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.85 , 103.72 , 79.93 , 117.11 2021-02-12T23:49:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.28 , 0.09 2021-02-12T23:49:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173773.58 epoch sec; seqNo:7202; 4057938.95 north; 591574.49 east 2021-02-12T23:49:33Z- AHRS_M2 msg: 1613173773.96 epoch sec; seqNo:7144 2021-02-12T23:49:33Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.09 psi 2021-02-12T23:49:33Z- handleDepth()-Depth_Keller msg: 1613173773961 epoch sec; seqNo:7203; depth 9.82 2021-02-12T23:49:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173773.98 epoch sec; seqNo:7203; 4057939.33 north; 591574.48 east 2021-02-12T23:49:34Z- AHRS_M2 msg: 1613173774.36 epoch sec; seqNo:7145 2021-02-12T23:49:34Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.10 psi 2021-02-12T23:49:34Z- handleDepth()-Depth_Keller msg: 1613173774365 epoch sec; seqNo:7204; depth 9.95 2021-02-12T23:49:34Z- handleDvl() - RDI_Pathfinder msg: 1613173774.38 epoch sec; seqNo:3329 2021-02-12T23:49:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.48 , 99.94 , 79.67 , 120.22 2021-02-12T23:49:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.09 , 0.10 2021-02-12T23:49:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173774.40 epoch sec; seqNo:7204; 4057939.33 north; 591574.48 east 2021-02-12T23:49:34Z- AHRS_M2 msg: 1613173774.76 epoch sec; seqNo:7146 2021-02-12T23:49:34Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.11 psi 2021-02-12T23:49:34Z- handleDepth()-Depth_Keller msg: 1613173774769 epoch sec; seqNo:7205; depth 10.07 2021-02-12T23:49:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:49:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:49:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:49:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:49:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:49:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:49:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:49:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:49:34Z- handleDvl() - RDI_Pathfinder msg: 1613173774.77 epoch sec; seqNo:3330 2021-02-12T23:49:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:49:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.09 , 0.10 2021-02-12T23:49:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173774.82 epoch sec; seqNo:7205; 4057939.72 north; 591575.70 east 2021-02-12T23:49:35Z- AHRS_M2 msg: 1613173775.17 epoch sec; seqNo:7147 2021-02-12T23:49:35Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.13 psi 2021-02-12T23:49:35Z- handleDepth()-Depth_Keller msg: 1613173775248 epoch sec; seqNo:7206; depth 10.19 2021-02-12T23:49:35Z- handleDvl() - RDI_Pathfinder msg: 1613173775.26 epoch sec; seqNo:3331 2021-02-12T23:49:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 99.94 , 76.42 , 117.52 2021-02-12T23:49:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.10 , 0.11 2021-02-12T23:49:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173775.28 epoch sec; seqNo:7206; 4057940.10 north; 591575.69 east 2021-02-12T23:49:35Z- AHRS_M2 msg: 1613173775.57 epoch sec; seqNo:7148 2021-02-12T23:49:35Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.15 psi 2021-02-12T23:49:35Z- handleDepth()-Depth_Keller msg: 1613173775601 epoch sec; seqNo:7207; depth 10.33 2021-02-12T23:49:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173775.61 epoch sec; seqNo:7207; 4057940.10 north; 591575.69 east 2021-02-12T23:49:35Z- LcmTrn::handleMessages() - handling 42 messages took 0.72 ms and 128 calls... 2021-02-12T23:49:35Z- waiting for 1613173773.76, time is 1613173775.61 2021-02-12T23:49:35Z- LcmTrn::updateTrn() >>>> pose time 1613173775.61, meas time 1613173775.26 <<<< 2021-02-12T23:49:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173775.26 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:49:35Z- TerrainNav::motionUpdate() - currEstimate = (4057940, 591575.69, 10.33) 2021-02-12T23:49:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:49:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:49:35Z- TNavBF::Projecting Measurements 2021-02-12T23:49:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:49:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:49:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:49:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:49:35Z- TNavFilter::4 good beams. 2021-02-12T23:49:35Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:49:35Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:49:35Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:49:35Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:49:35Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:49:35Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:49:35Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:49:35Z- TNavBF::mmse selected from filter -1 2021-02-12T23:49:35Z- TerrainMapOctree: (vehN, vehE ) = (4057939.85, 591575.61). 2021-02-12T23:49:35Z- TerrainMapOctree: Min Distance = 4255.52. 2021-02-12T23:49:35Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:49:35Z- There is no specific good beam that all particles have in common. 2021-02-12T23:49:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:49:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-12T23:49:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:49:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.98 2021-02-12T23:49:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:49:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 9.34 2021-02-12T23:49:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:49:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 22.10 2021-02-12T23:49:36Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:49:36Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:49:36Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:49:36Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:49:36Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:49:36Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:49:36Z- Filter 0 No beams used in subcloud update 2021-02-12T23:49:36Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:49:36Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:49:36Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 734.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:49:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173775.26 sec. 2021-02-12T23:49:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:49:36Z- TNavBF::mmse selected from filter -1 2021-02-12T23:49:36Z- variances([0], [2], and psi_berg) = 734.483261 833.019924 0.000000 2021-02-12T23:49:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:49:36Z- LcmTrn::publishEstimates() - MLE : 4057931.60 591527.19 10.33 2021-02-12T23:49:36Z- LcmTrn::publishEstimates() - MMSE : 4057940.10 591575.69 10.33 2021-02-12T23:49:36Z- LcmTrn::publishEstimates() - COVAR: 734.48 833.02 0.87 0.00 2021-02-12T23:49:36Z- AHRS_M2 msg: 1613173775.98 epoch sec; seqNo:7149 2021-02-12T23:49:36Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.16 psi 2021-02-12T23:49:36Z- handleDepth()-Depth_Keller msg: 1613173776058 epoch sec; seqNo:7208; depth 10.45 2021-02-12T23:49:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:49:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:49:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:49:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:49:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:49:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:49:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:49:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:49:36Z- handleDvl() - RDI_Pathfinder msg: 1613173776.06 epoch sec; seqNo:3332 2021-02-12T23:49:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:49:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.10 , 0.11 2021-02-12T23:49:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173776.07 epoch sec; seqNo:7208; 4057940.48 north; 591575.69 east 2021-02-12T23:49:36Z- AHRS_M2 msg: 1613173776.38 epoch sec; seqNo:7150 2021-02-12T23:49:36Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.18 psi 2021-02-12T23:49:36Z- handleDepth()-Depth_Keller msg: 1613173776413 epoch sec; seqNo:7209; depth 10.59 2021-02-12T23:49:36Z- handleDvl() - RDI_Pathfinder msg: 1613173776.44 epoch sec; seqNo:3333 2021-02-12T23:49:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.12 , 102.64 , 78.31 , 120.21 2021-02-12T23:49:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.15 , 0.09 2021-02-12T23:49:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173776.46 epoch sec; seqNo:7209; 4057940.48 north; 591575.69 east 2021-02-12T23:49:36Z- AHRS_M2 msg: 1613173776.79 epoch sec; seqNo:7151 2021-02-12T23:49:36Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.19 psi 2021-02-12T23:49:36Z- handleDepth()-Depth_Keller msg: 1613173776813 epoch sec; seqNo:7210; depth 10.70 2021-02-12T23:49:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173776.82 epoch sec; seqNo:7210; 4057940.86 north; 591575.69 east 2021-02-12T23:49:37Z- AHRS_M2 msg: 1613173777.19 epoch sec; seqNo:7152 2021-02-12T23:49:37Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.20 psi 2021-02-12T23:49:37Z- handleDepth()-Depth_Keller msg: 1613173777217 epoch sec; seqNo:7211; depth 10.81 2021-02-12T23:49:37Z- handleDvl() - RDI_Pathfinder msg: 1613173777.23 epoch sec; seqNo:3334 2021-02-12T23:49:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 108.05 , 77.23 , 118.86 2021-02-12T23:49:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.13 , 0.04 2021-02-12T23:49:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173777.24 epoch sec; seqNo:7211; 4057940.87 north; 591576.91 east 2021-02-12T23:49:37Z- AHRS_M2 msg: 1613173777.59 epoch sec; seqNo:7153 2021-02-12T23:49:37Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.20 psi 2021-02-12T23:49:37Z- handleDepth()-Depth_Keller msg: 1613173777596 epoch sec; seqNo:7212; depth 10.96 2021-02-12T23:49:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173777.61 epoch sec; seqNo:7212; 4057941.25 north; 591576.90 east 2021-02-12T23:49:37Z- AHRS_M2 msg: 1613173778.00 epoch sec; seqNo:7154 2021-02-12T23:49:37Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.21 psi 2021-02-12T23:49:38Z- handleDepth()-Depth_Keller msg: 1613173778001 epoch sec; seqNo:7213; depth 11.07 2021-02-12T23:49:38Z- handleDvl() - RDI_Pathfinder msg: 1613173778.01 epoch sec; seqNo:3335 2021-02-12T23:49:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:49:38Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:49:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173778.02 epoch sec; seqNo:7213; 4057941.63 north; 591576.90 east 2021-02-12T23:49:38Z- AHRS_M2 msg: 1613173778.40 epoch sec; seqNo:7155 2021-02-12T23:49:38Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.21 psi 2021-02-12T23:49:38Z- handleDepth()-Depth_Keller msg: 1613173778413 epoch sec; seqNo:7214; depth 11.19 2021-02-12T23:49:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:49:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:49:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:49:38Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:49:38Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:49:38Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:49:38Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:49:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:49:38Z- handleDvl() - RDI_Pathfinder msg: 1613173778.41 epoch sec; seqNo:3336 2021-02-12T23:49:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:49:38Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:49:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173778.43 epoch sec; seqNo:7214; 4057941.63 north; 591576.90 east 2021-02-12T23:49:38Z- AHRS_M2 msg: 1613173778.80 epoch sec; seqNo:7156 2021-02-12T23:49:38Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.20 psi 2021-02-12T23:49:38Z- handleDepth()-Depth_Keller msg: 1613173778808 epoch sec; seqNo:7215; depth 11.33 2021-02-12T23:49:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173778.83 epoch sec; seqNo:7215; 4057942.01 north; 591576.89 east 2021-02-12T23:49:39Z- AHRS_M2 msg: 1613173779.21 epoch sec; seqNo:7157 2021-02-12T23:49:39Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.20 psi 2021-02-12T23:49:39Z- handleDepth()-Depth_Keller msg: 1613173779215 epoch sec; seqNo:7216; depth 11.45 2021-02-12T23:49:39Z- handleDvl() - RDI_Pathfinder msg: 1613173779.23 epoch sec; seqNo:3337 2021-02-12T23:49:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:49:39Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:49:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173779.27 epoch sec; seqNo:7216; 4057942.02 north; 591578.12 east 2021-02-12T23:49:39Z- AHRS_M2 msg: 1613173779.61 epoch sec; seqNo:7158 2021-02-12T23:49:39Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-12T23:49:39Z- handleDepth()-Depth_Keller msg: 1613173779616 epoch sec; seqNo:7217; depth 11.59 2021-02-12T23:49:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:49:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:49:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:49:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:49:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:49:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:49:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:49:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:49:39Z- handleDvl() - RDI_Pathfinder msg: 1613173779.62 epoch sec; seqNo:3338 2021-02-12T23:49:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:49:39Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:49:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173779.63 epoch sec; seqNo:7217; 4057942.40 north; 591578.11 east 2021-02-12T23:49:40Z- handleDepth()-Depth_Keller msg: 1613173780037 epoch sec; seqNo:7218; depth 11.71 2021-02-12T23:49:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173780.05 epoch sec; seqNo:7218; 4057942.78 north; 591578.11 east 2021-02-12T23:49:40Z- AHRS_M2 msg: 1613173780.42 epoch sec; seqNo:7159 2021-02-12T23:49:40Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-12T23:49:40Z- handleDepth()-Depth_Keller msg: 1613173780429 epoch sec; seqNo:7219; depth 11.85 2021-02-12T23:49:40Z- handleDvl() - RDI_Pathfinder msg: 1613173780.45 epoch sec; seqNo:3339 2021-02-12T23:49:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.09 , 105.42 , 78.65 , 117.32 2021-02-12T23:49:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , -0.02 2021-02-12T23:49:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173780.46 epoch sec; seqNo:7219; 4057942.78 north; 591578.11 east 2021-02-12T23:49:40Z- AHRS_M2 msg: 1613173780.83 epoch sec; seqNo:7160 2021-02-12T23:49:40Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-12T23:49:40Z- handleDepth()-Depth_Keller msg: 1613173780831 epoch sec; seqNo:7220; depth 11.96 2021-02-12T23:49:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:49:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:49:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:49:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:49:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:49:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:49:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:49:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:49:40Z- handleDvl() - RDI_Pathfinder msg: 1613173780.83 epoch sec; seqNo:3340 2021-02-12T23:49:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:49:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , -0.02 2021-02-12T23:49:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173780.84 epoch sec; seqNo:7220; 4057943.16 north; 591578.10 east 2021-02-12T23:49:41Z- LcmTrn::handleMessages() - handling 47 messages took 0.74 ms and 132 calls... 2021-02-12T23:49:41Z- waiting for 1613173778.61, time is 1613173780.84 2021-02-12T23:49:41Z- LcmTrn::updateTrn() equating pose 1613173780.84 and meas times 1613173780.84 2021-02-12T23:49:41Z- LcmTrn::updateTrn() >>>> pose time 1613173780.84, meas time 1613173780.84 <<<< 2021-02-12T23:49:41Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173780.84, ping # 0. 2021-02-12T23:49:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173780.84 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:49:41Z- TerrainNav::motionUpdate() - currEstimate = (4057943, 591578.10, 11.96) 2021-02-12T23:49:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:49:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:49:41Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:49:41Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:49:41Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173780.84 sec.; No relevant navigation data available 2021-02-12T23:49:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:49:41Z- TNavBF::mmse selected from filter -1 2021-02-12T23:49:41Z- variances([0], [2], and psi_berg) = 734.404390 833.019863 0.000000 2021-02-12T23:49:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:49:41Z- LcmTrn::publishEstimates() - MLE : 4057934.66 591532.60 11.96 2021-02-12T23:49:41Z- LcmTrn::publishEstimates() - MMSE : 4057943.16 591578.10 11.96 2021-02-12T23:49:41Z- LcmTrn::publishEstimates() - COVAR: 734.40 833.02 1.07 0.00 2021-02-12T23:49:41Z- AHRS_M2 msg: 1613173781.23 epoch sec; seqNo:7161 2021-02-12T23:49:41Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.17 psi 2021-02-12T23:49:41Z- handleDepth()-Depth_Keller msg: 1613173781236 epoch sec; seqNo:7221; depth 12.09 2021-02-12T23:49:41Z- handleDvl() - RDI_Pathfinder msg: 1613173781.25 epoch sec; seqNo:3341 2021-02-12T23:49:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 102.71 , 75.59 , 117.58 2021-02-12T23:49:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.03 2021-02-12T23:49:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173781.27 epoch sec; seqNo:7221; 4057943.54 north; 591578.10 east 2021-02-12T23:49:41Z- AHRS_M2 msg: 1613173781.63 epoch sec; seqNo:7162 2021-02-12T23:49:41Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-12T23:49:41Z- handleDepth()-Depth_Keller msg: 1613173781641 epoch sec; seqNo:7222; depth 12.22 2021-02-12T23:49:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173781.66 epoch sec; seqNo:7222; 4057943.93 north; 591579.32 east 2021-02-12T23:49:42Z- AHRS_M2 msg: 1613173782.04 epoch sec; seqNo:7163 2021-02-12T23:49:42Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.18 psi 2021-02-12T23:49:42Z- handleDepth()-Depth_Keller msg: 1613173782040 epoch sec; seqNo:7223; depth 12.34 2021-02-12T23:49:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:49:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:49:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:49:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:49:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:49:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:49:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:49:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:49:42Z- handleDvl() - RDI_Pathfinder msg: 1613173782.04 epoch sec; seqNo:3342 2021-02-12T23:49:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:49:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.03 2021-02-12T23:49:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173782.05 epoch sec; seqNo:7223; 4057943.93 north; 591579.32 east 2021-02-12T23:49:42Z- AHRS_M2 msg: 1613173782.44 epoch sec; seqNo:7164 2021-02-12T23:49:42Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.18 psi 2021-02-12T23:49:42Z- handleDepth()-Depth_Keller msg: 1613173782445 epoch sec; seqNo:7224; depth 12.47 2021-02-12T23:49:42Z- handleDvl() - RDI_Pathfinder msg: 1613173782.46 epoch sec; seqNo:3343 2021-02-12T23:49:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.85 , 78.51 , 113.66 2021-02-12T23:49:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.08 , -0.05 2021-02-12T23:49:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173782.47 epoch sec; seqNo:7224; 4057944.31 north; 591579.31 east 2021-02-12T23:49:42Z- AHRS_M2 msg: 1613173782.85 epoch sec; seqNo:7165 2021-02-12T23:49:42Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.18 psi 2021-02-12T23:49:42Z- handleDepth()-Depth_Keller msg: 1613173782857 epoch sec; seqNo:7225; depth 12.57 2021-02-12T23:49:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173782.87 epoch sec; seqNo:7225; 4057944.69 north; 591579.31 east 2021-02-12T23:49:43Z- AHRS_M2 msg: 1613173783.25 epoch sec; seqNo:7166 2021-02-12T23:49:43Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-12T23:49:43Z- handleDepth()-Depth_Keller msg: 1613173783253 epoch sec; seqNo:7226; depth 12.68 2021-02-12T23:49:43Z- handleDvl() - RDI_Pathfinder msg: 1613173783.27 epoch sec; seqNo:3344 2021-02-12T23:49:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.23 , 78.25 , 117.45 2021-02-12T23:49:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , -0.10 2021-02-12T23:49:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173783.30 epoch sec; seqNo:7226; 4057945.07 north; 591579.30 east 2021-02-12T23:49:43Z- AHRS_M2 msg: 1613173783.65 epoch sec; seqNo:7167 2021-02-12T23:49:43Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.19 psi 2021-02-12T23:49:43Z- handleDepth()-Depth_Keller msg: 1613173783659 epoch sec; seqNo:7227; depth 12.82 2021-02-12T23:49:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173783.67 epoch sec; seqNo:7227; 4057945.07 north; 591579.30 east 2021-02-12T23:49:44Z- AHRS_M2 msg: 1613173784.06 epoch sec; seqNo:7168 2021-02-12T23:49:44Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.20 psi 2021-02-12T23:49:44Z- handleDepth()-Depth_Keller msg: 1613173784065 epoch sec; seqNo:7228; depth 12.92 2021-02-12T23:49:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173784.07 epoch sec; seqNo:7228; 4057945.46 north; 591580.52 east 2021-02-12T23:49:44Z- AHRS_M2 msg: 1613173784.46 epoch sec; seqNo:7169 2021-02-12T23:49:44Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.20 psi 2021-02-12T23:49:44Z- handleDepth()-Depth_Keller msg: 1613173784465 epoch sec; seqNo:7229; depth 13.06 2021-02-12T23:49:44Z- handleDvl() - RDI_Pathfinder msg: 1613173784.48 epoch sec; seqNo:3345 2021-02-12T23:49:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.58 , 101.16 , 78.18 , 116.03 2021-02-12T23:49:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.10 2021-02-12T23:49:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173784.49 epoch sec; seqNo:7229; 4057945.84 north; 591580.52 east 2021-02-12T23:49:44Z- AHRS_M2 msg: 1613173784.87 epoch sec; seqNo:7170 2021-02-12T23:49:44Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.21 psi 2021-02-12T23:49:44Z- handleDepth()-Depth_Keller msg: 1613173784871 epoch sec; seqNo:7230; depth 13.17 2021-02-12T23:49:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173784.88 epoch sec; seqNo:7230; 4057946.22 north; 591580.51 east 2021-02-12T23:49:45Z- AHRS_M2 msg: 1613173785.27 epoch sec; seqNo:7171 2021-02-12T23:49:45Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.20 psi 2021-02-12T23:49:45Z- handleDepth()-Depth_Keller msg: 1613173785287 epoch sec; seqNo:7231; depth 13.28 2021-02-12T23:49:45Z- handleDvl() - RDI_Pathfinder msg: 1613173785.30 epoch sec; seqNo:3346 2021-02-12T23:49:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.64 , 102.92 , 78.58 , 115.09 2021-02-12T23:49:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.14 , -0.05 2021-02-12T23:49:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173785.31 epoch sec; seqNo:7231; 4057946.59 north; 591580.51 east 2021-02-12T23:49:45Z- AHRS_M2 msg: 1613173785.67 epoch sec; seqNo:7172 2021-02-12T23:49:45Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.20 psi 2021-02-12T23:49:45Z- handleDepth()-Depth_Keller msg: 1613173785677 epoch sec; seqNo:7232; depth 13.43 2021-02-12T23:49:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173785.69 epoch sec; seqNo:7232; 4057946.59 north; 591580.51 east 2021-02-12T23:49:46Z- LcmTrn::handleMessages() - handling 42 messages took 0.63 ms and 130 calls... 2021-02-12T23:49:46Z- waiting for 1613173783.84, time is 1613173785.69 2021-02-12T23:49:46Z- LcmTrn::updateTrn() >>>> pose time 1613173785.69, meas time 1613173785.30 <<<< 2021-02-12T23:49:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173785.30 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:49:46Z- TerrainNav::motionUpdate() - currEstimate = (4057947, 591580.51, 13.43) 2021-02-12T23:49:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:49:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:49:46Z- TNavBF::Projecting Measurements 2021-02-12T23:49:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:49:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:49:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:49:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:49:46Z- TNavFilter::4 good beams. 2021-02-12T23:49:46Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:49:46Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:49:46Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:49:46Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:49:46Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:49:46Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:49:46Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:49:46Z- TNavBF::mmse selected from filter -1 2021-02-12T23:49:46Z- TerrainMapOctree: (vehN, vehE ) = (4057946.32, 591580.32). 2021-02-12T23:49:46Z- TerrainMapOctree: Min Distance = 4247.53. 2021-02-12T23:49:46Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:49:46Z- There is no specific good beam that all particles have in common. 2021-02-12T23:49:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:49:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 10.93 2021-02-12T23:49:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:49:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 16.95 2021-02-12T23:49:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:49:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-12T23:49:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:49:46Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 21.19 2021-02-12T23:49:46Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:49:46Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:49:46Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:49:46Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:49:46Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:49:46Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:49:46Z- Filter 0 No beams used in subcloud update 2021-02-12T23:49:46Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:49:46Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:49:46Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 734.3 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:49:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173785.30 sec. 2021-02-12T23:49:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:49:46Z- TNavBF::mmse selected from filter -1 2021-02-12T23:49:46Z- variances([0], [2], and psi_berg) = 734.302794 833.019785 0.000000 2021-02-12T23:49:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:49:46Z- LcmTrn::publishEstimates() - MLE : 4057938.09 591535.01 13.43 2021-02-12T23:49:46Z- LcmTrn::publishEstimates() - MMSE : 4057946.59 591580.51 13.43 2021-02-12T23:49:46Z- LcmTrn::publishEstimates() - COVAR: 734.30 833.02 0.97 0.00 2021-02-12T23:49:46Z- AHRS_M2 msg: 1613173786.10 epoch sec; seqNo:7173 2021-02-12T23:49:46Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.20 psi 2021-02-12T23:49:46Z- handleDepth()-Depth_Keller msg: 1613173786122 epoch sec; seqNo:7233; depth 13.52 2021-02-12T23:49:46Z- handleDvl() - RDI_Pathfinder msg: 1613173786.14 epoch sec; seqNo:3347 2021-02-12T23:49:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.92 , 78.58 , 112.38 2021-02-12T23:49:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.15 , 0.01 2021-02-12T23:49:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173786.15 epoch sec; seqNo:7233; 4057946.97 north; 591580.50 east 2021-02-12T23:49:46Z- AHRS_M2 msg: 1613173786.48 epoch sec; seqNo:7174 2021-02-12T23:49:46Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.20 psi 2021-02-12T23:49:46Z- handleDepth()-Depth_Keller msg: 1613173786493 epoch sec; seqNo:7234; depth 13.66 2021-02-12T23:49:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173786.51 epoch sec; seqNo:7234; 4057947.35 north; 591580.50 east 2021-02-12T23:49:46Z- AHRS_M2 msg: 1613173786.88 epoch sec; seqNo:7175 2021-02-12T23:49:46Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.19 psi 2021-02-12T23:49:46Z- handleDepth()-Depth_Keller msg: 1613173786889 epoch sec; seqNo:7235; depth 13.76 2021-02-12T23:49:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173786.91 epoch sec; seqNo:7235; 4057947.74 north; 591581.72 east 2021-02-12T23:49:47Z- AHRS_M2 msg: 1613173787.29 epoch sec; seqNo:7176 2021-02-12T23:49:47Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.19 psi 2021-02-12T23:49:47Z- handleDepth()-Depth_Keller msg: 1613173787301 epoch sec; seqNo:7236; depth 13.88 2021-02-12T23:49:47Z- handleDvl() - RDI_Pathfinder msg: 1613173787.33 epoch sec; seqNo:3348 2021-02-12T23:49:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.30 , 102.57 , 78.24 , 107.98 2021-02-12T23:49:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.15 , 0.07 2021-02-12T23:49:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173787.34 epoch sec; seqNo:7236; 4057948.12 north; 591581.71 east 2021-02-12T23:49:47Z- AHRS_M2 msg: 1613173787.70 epoch sec; seqNo:7177 2021-02-12T23:49:47Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-12T23:49:47Z- handleDepth()-Depth_Keller msg: 1613173787703 epoch sec; seqNo:7237; depth 14.00 2021-02-12T23:49:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173787.71 epoch sec; seqNo:7237; 4057948.12 north; 591581.71 east 2021-02-12T23:49:48Z- AHRS_M2 msg: 1613173788.10 epoch sec; seqNo:7178 2021-02-12T23:49:48Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-12T23:49:48Z- handleDepth()-Depth_Keller msg: 1613173788206 epoch sec; seqNo:7238; depth 14.10 2021-02-12T23:49:48Z- handleDvl() - RDI_Pathfinder msg: 1613173788.22 epoch sec; seqNo:3349 2021-02-12T23:49:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 100.34 , 78.72 , 111.16 2021-02-12T23:49:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.07 , 0.09 2021-02-12T23:49:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173788.23 epoch sec; seqNo:7238; 4057948.50 north; 591581.71 east 2021-02-12T23:49:48Z- AHRS_M2 msg: 1613173788.50 epoch sec; seqNo:7179 2021-02-12T23:49:48Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.19 psi 2021-02-12T23:49:48Z- handleDepth()-Depth_Keller msg: 1613173788535 epoch sec; seqNo:7239; depth 14.24 2021-02-12T23:49:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173788.55 epoch sec; seqNo:7239; 4057948.88 north; 591581.71 east 2021-02-12T23:49:48Z- AHRS_M2 msg: 1613173788.90 epoch sec; seqNo:7180 2021-02-12T23:49:48Z- AHRS_M2 msg: 0.57 phi; -0.31 theta; -0.20 psi 2021-02-12T23:49:48Z- handleDepth()-Depth_Keller msg: 1613173788930 epoch sec; seqNo:7240; depth 14.33 2021-02-12T23:49:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173788.95 epoch sec; seqNo:7240; 4057949.26 north; 591581.70 east 2021-02-12T23:49:49Z- AHRS_M2 msg: 1613173789.31 epoch sec; seqNo:7181 2021-02-12T23:49:49Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.20 psi 2021-02-12T23:49:49Z- handleDepth()-Depth_Keller msg: 1613173789394 epoch sec; seqNo:7241; depth 14.43 2021-02-12T23:49:49Z- handleDvl() - RDI_Pathfinder msg: 1613173789.41 epoch sec; seqNo:3350 2021-02-12T23:49:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 100.34 , 74.37 , 112.51 2021-02-12T23:49:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.02 , 0.07 2021-02-12T23:49:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173789.42 epoch sec; seqNo:7241; 4057949.26 north; 591581.70 east 2021-02-12T23:49:49Z- AHRS_M2 msg: 1613173789.71 epoch sec; seqNo:7182 2021-02-12T23:49:49Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.20 psi 2021-02-12T23:49:49Z- handleDepth()-Depth_Keller msg: 1613173789725 epoch sec; seqNo:7242; depth 14.57 2021-02-12T23:49:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173789.74 epoch sec; seqNo:7242; 4057949.64 north; 591581.70 east 2021-02-12T23:49:50Z- AHRS_M2 msg: 1613173790.12 epoch sec; seqNo:7183 2021-02-12T23:49:50Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.19 psi 2021-02-12T23:49:50Z- handleDepth()-Depth_Keller msg: 1613173790145 epoch sec; seqNo:7243; depth 14.66 2021-02-12T23:49:50Z- handleDvl() - RDI_Pathfinder msg: 1613173790.16 epoch sec; seqNo:3351 2021-02-12T23:49:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.34 , 78.72 , 112.51 2021-02-12T23:49:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , 0.05 2021-02-12T23:49:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173790.17 epoch sec; seqNo:7243; 4057950.03 north; 591582.91 east 2021-02-12T23:49:50Z- AHRS_M2 msg: 1613173790.52 epoch sec; seqNo:7184 2021-02-12T23:49:50Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.19 psi 2021-02-12T23:49:50Z- handleDepth()-Depth_Keller msg: 1613173790550 epoch sec; seqNo:7244; depth 14.79 2021-02-12T23:49:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173790.56 epoch sec; seqNo:7244; 4057950.03 north; 591582.91 east 2021-02-12T23:49:50Z- AHRS_M2 msg: 1613173790.92 epoch sec; seqNo:7185 2021-02-12T23:49:50Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.19 psi 2021-02-12T23:49:50Z- handleDepth()-Depth_Keller msg: 1613173790930 epoch sec; seqNo:7245; depth 14.90 2021-02-12T23:49:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173790.94 epoch sec; seqNo:7245; 4057950.41 north; 591582.91 east 2021-02-12T23:49:51Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 131 calls... 2021-02-12T23:49:51Z- waiting for 1613173788.69, time is 1613173790.94 2021-02-12T23:49:51Z- LcmTrn::updateTrn() >>>> pose time 1613173790.94, meas time 1613173790.16 <<<< 2021-02-12T23:49:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173790.16 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:49:51Z- TerrainNav::motionUpdate() - currEstimate = (4057950, 591582.91, 14.90) 2021-02-12T23:49:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:49:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:49:51Z- TNavBF::Projecting Measurements 2021-02-12T23:49:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:49:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:49:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:49:51Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:49:51Z- TNavFilter::3 good beams. 2021-02-12T23:49:51Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:49:51Z- TNavBF:: beamIndex[0] = 1 2021-02-12T23:49:51Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:49:51Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:49:51Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:49:51Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:49:51Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:49:51Z- TNavBF::mmse selected from filter -1 2021-02-12T23:49:51Z- TerrainMapOctree: (vehN, vehE ) = (4057949.84, 591582.55). 2021-02-12T23:49:51Z- TerrainMapOctree: Min Distance = 4243.39. 2021-02-12T23:49:51Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:49:51Z- There is no specific good beam that all particles have in common. 2021-02-12T23:49:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:49:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 16.11 2021-02-12T23:49:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:49:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 9.91 2021-02-12T23:49:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:49:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 20.25 2021-02-12T23:49:51Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:49:51Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:49:51Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:49:51Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:49:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:49:51Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:49:51Z- Filter 0 No beams used in subcloud update 2021-02-12T23:49:51Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:49:51Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:49:51Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 734.2 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:49:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173790.16 sec. 2021-02-12T23:49:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:49:51Z- TNavBF::mmse selected from filter -1 2021-02-12T23:49:51Z- variances([0], [2], and psi_berg) = 734.192014 833.019700 0.000000 2021-02-12T23:49:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:49:51Z- LcmTrn::publishEstimates() - MLE : 4057942.91 591535.41 14.90 2021-02-12T23:49:51Z- LcmTrn::publishEstimates() - MMSE : 4057950.41 591582.91 14.90 2021-02-12T23:49:51Z- LcmTrn::publishEstimates() - COVAR: 734.19 833.02 0.99 0.00 2021-02-12T23:49:51Z- AHRS_M2 msg: 1613173791.33 epoch sec; seqNo:7186 2021-02-12T23:49:51Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.18 psi 2021-02-12T23:49:51Z- handleDepth()-Depth_Keller msg: 1613173791333 epoch sec; seqNo:7246; depth 15.02 2021-02-12T23:49:51Z- handleDvl() - RDI_Pathfinder msg: 1613173791.36 epoch sec; seqNo:3352 2021-02-12T23:49:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.29 , 78.31 , 113.46 2021-02-12T23:49:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , 0.04 2021-02-12T23:49:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173791.37 epoch sec; seqNo:7246; 4057950.79 north; 591582.91 east 2021-02-12T23:49:51Z- AHRS_M2 msg: 1613173791.73 epoch sec; seqNo:7187 2021-02-12T23:49:51Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.18 psi 2021-02-12T23:49:51Z- handleDepth()-Depth_Keller msg: 1613173791741 epoch sec; seqNo:7247; depth 15.13 2021-02-12T23:49:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173791.75 epoch sec; seqNo:7247; 4057951.17 north; 591582.90 east 2021-02-12T23:49:52Z- AHRS_M2 msg: 1613173792.16 epoch sec; seqNo:7188 2021-02-12T23:49:52Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-12T23:49:52Z- handleDepth()-Depth_Keller msg: 1613173792167 epoch sec; seqNo:7248; depth 15.24 2021-02-12T23:49:52Z- handleDvl() - RDI_Pathfinder msg: 1613173792.18 epoch sec; seqNo:3353 2021-02-12T23:49:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.37 , 101.30 , 74.37 , 112.11 2021-02-12T23:49:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , 0.01 2021-02-12T23:49:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173792.20 epoch sec; seqNo:7248; 4057951.17 north; 591582.90 east 2021-02-12T23:49:52Z- AHRS_M2 msg: 1613173792.54 epoch sec; seqNo:7189 2021-02-12T23:49:52Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-12T23:49:52Z- handleDepth()-Depth_Keller msg: 1613173792546 epoch sec; seqNo:7249; depth 15.38 2021-02-12T23:49:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173792.57 epoch sec; seqNo:7249; 4057951.56 north; 591584.12 east 2021-02-12T23:49:52Z- AHRS_M2 msg: 1613173792.94 epoch sec; seqNo:7190 2021-02-12T23:49:52Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.17 psi 2021-02-12T23:49:52Z- handleDepth()-Depth_Keller msg: 1613173792949 epoch sec; seqNo:7250; depth 15.49 2021-02-12T23:49:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173792.96 epoch sec; seqNo:7250; 4057951.94 north; 591584.11 east 2021-02-12T23:49:53Z- AHRS_M2 msg: 1613173793.35 epoch sec; seqNo:7191 2021-02-12T23:49:53Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.17 psi 2021-02-12T23:49:53Z- handleDepth()-Depth_Keller msg: 1613173793359 epoch sec; seqNo:7251; depth 15.61 2021-02-12T23:49:53Z- handleDvl() - RDI_Pathfinder msg: 1613173793.37 epoch sec; seqNo:3354 2021-02-12T23:49:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.42 , 99.00 , 78.72 , 111.17 2021-02-12T23:49:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , -0.01 2021-02-12T23:49:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173793.38 epoch sec; seqNo:7251; 4057952.32 north; 591584.11 east 2021-02-12T23:49:53Z- AHRS_M2 msg: 1613173793.75 epoch sec; seqNo:7192 2021-02-12T23:49:53Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.17 psi 2021-02-12T23:49:53Z- handleDepth()-Depth_Keller msg: 1613173793758 epoch sec; seqNo:7252; depth 15.72 2021-02-12T23:49:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173793.77 epoch sec; seqNo:7252; 4057952.69 north; 591584.11 east 2021-02-12T23:49:54Z- AHRS_M2 msg: 1613173794.16 epoch sec; seqNo:7193 2021-02-12T23:49:54Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.17 psi 2021-02-12T23:49:54Z- handleDepth()-Depth_Keller msg: 1613173794161 epoch sec; seqNo:7253; depth 15.83 2021-02-12T23:49:54Z- handleDvl() - RDI_Pathfinder msg: 1613173794.18 epoch sec; seqNo:3355 2021-02-12T23:49:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.42 , 97.64 , 74.37 , 109.81 2021-02-12T23:49:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.08 , 0.00 2021-02-12T23:49:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173794.19 epoch sec; seqNo:7253; 4057952.69 north; 591584.11 east 2021-02-12T23:49:54Z- AHRS_M2 msg: 1613173794.56 epoch sec; seqNo:7194 2021-02-12T23:49:54Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.17 psi 2021-02-12T23:49:54Z- handleDepth()-Depth_Keller msg: 1613173794569 epoch sec; seqNo:7254; depth 15.96 2021-02-12T23:49:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173794.58 epoch sec; seqNo:7254; 4057953.07 north; 591584.10 east 2021-02-12T23:49:54Z- AHRS_M2 msg: 1613173794.96 epoch sec; seqNo:7195 2021-02-12T23:49:54Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.17 psi 2021-02-12T23:49:54Z- handleDepth()-Depth_Keller msg: 1613173794970 epoch sec; seqNo:7255; depth 16.08 2021-02-12T23:49:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173794.98 epoch sec; seqNo:7255; 4057953.45 north; 591584.10 east 2021-02-12T23:49:55Z- AHRS_M2 msg: 1613173795.37 epoch sec; seqNo:7196 2021-02-12T23:49:55Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.18 psi 2021-02-12T23:49:55Z- handleDepth()-Depth_Keller msg: 1613173795373 epoch sec; seqNo:7256; depth 16.19 2021-02-12T23:49:55Z- handleDvl() - RDI_Pathfinder msg: 1613173795.39 epoch sec; seqNo:3356 2021-02-12T23:49:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 97.71 , 78.24 , 108.66 2021-02-12T23:49:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , -0.01 2021-02-12T23:49:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173795.40 epoch sec; seqNo:7256; 4057953.84 north; 591585.32 east 2021-02-12T23:49:55Z- AHRS_M2 msg: 1613173795.77 epoch sec; seqNo:7197 2021-02-12T23:49:55Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-12T23:49:55Z- handleDepth()-Depth_Keller msg: 1613173795777 epoch sec; seqNo:7257; depth 16.31 2021-02-12T23:49:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173795.79 epoch sec; seqNo:7257; 4057953.84 north; 591585.32 east 2021-02-12T23:49:56Z- AHRS_M2 msg: 1613173796.18 epoch sec; seqNo:7198 2021-02-12T23:49:56Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.16 psi 2021-02-12T23:49:56Z- handleDepth()-Depth_Keller msg: 1613173796181 epoch sec; seqNo:7258; depth 16.44 2021-02-12T23:49:56Z- handleDvl() - RDI_Pathfinder msg: 1613173796.19 epoch sec; seqNo:3357 2021-02-12T23:49:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 98.86 , 72.32 , 109.81 2021-02-12T23:49:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , -0.04 2021-02-12T23:49:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173796.20 epoch sec; seqNo:7258; 4057954.22 north; 591585.31 east 2021-02-12T23:49:56Z- LcmTrn::handleMessages() - handling 45 messages took 0.69 ms and 131 calls... 2021-02-12T23:49:56Z- waiting for 1613173793.94, time is 1613173796.20 2021-02-12T23:49:56Z- LcmTrn::updateTrn() equating pose 1613173796.20 and meas times 1613173796.20 2021-02-12T23:49:56Z- LcmTrn::updateTrn() >>>> pose time 1613173796.20, meas time 1613173796.20 <<<< 2021-02-12T23:49:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173796.20 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:49:56Z- TerrainNav::motionUpdate() - currEstimate = (4057954, 591585.31, 16.44) 2021-02-12T23:49:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:49:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:49:56Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173796.20 sec.; No relevant navigation data available 2021-02-12T23:49:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:49:56Z- TNavBF::mmse selected from filter -1 2021-02-12T23:49:56Z- variances([0], [2], and psi_berg) = 734.107437 833.019636 0.000000 2021-02-12T23:49:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:49:56Z- LcmTrn::publishEstimates() - MLE : 4057946.72 591537.81 16.44 2021-02-12T23:49:56Z- LcmTrn::publishEstimates() - MMSE : 4057954.22 591585.31 16.44 2021-02-12T23:49:56Z- LcmTrn::publishEstimates() - COVAR: 734.11 833.02 1.22 0.00 2021-02-12T23:49:56Z- AHRS_M2 msg: 1613173796.58 epoch sec; seqNo:7199 2021-02-12T23:49:56Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.16 psi 2021-02-12T23:49:56Z- handleDepth()-Depth_Keller msg: 1613173796590 epoch sec; seqNo:7259; depth 16.56 2021-02-12T23:49:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173796.60 epoch sec; seqNo:7259; 4057954.60 north; 591585.31 east 2021-02-12T23:49:56Z- AHRS_M2 msg: 1613173796.98 epoch sec; seqNo:7200 2021-02-12T23:49:56Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.15 psi 2021-02-12T23:49:56Z- handleDepth()-Depth_Keller msg: 1613173796989 epoch sec; seqNo:7260; depth 16.65 2021-02-12T23:49:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173797.00 epoch sec; seqNo:7260; 4057954.98 north; 591585.30 east 2021-02-12T23:49:57Z- AHRS_M2 msg: 1613173797.39 epoch sec; seqNo:7201 2021-02-12T23:49:57Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.15 psi 2021-02-12T23:49:57Z- handleDepth()-Depth_Keller msg: 1613173797393 epoch sec; seqNo:7261; depth 16.78 2021-02-12T23:49:57Z- handleDvl() - RDI_Pathfinder msg: 1613173797.40 epoch sec; seqNo:3358 2021-02-12T23:49:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 97.71 , 78.24 , 109.87 2021-02-12T23:49:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , -0.11 2021-02-12T23:49:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173797.41 epoch sec; seqNo:7261; 4057955.36 north; 591585.30 east 2021-02-12T23:49:57Z- AHRS_M2 msg: 1613173797.79 epoch sec; seqNo:7202 2021-02-12T23:49:57Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.15 psi 2021-02-12T23:49:57Z- handleDepth()-Depth_Keller msg: 1613173797805 epoch sec; seqNo:7262; depth 16.89 2021-02-12T23:49:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173797.82 epoch sec; seqNo:7262; 4057955.36 north; 591585.30 east 2021-02-12T23:49:58Z- AHRS_M2 msg: 1613173798.20 epoch sec; seqNo:7203 2021-02-12T23:49:58Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.16 psi 2021-02-12T23:49:58Z- handleDepth()-Depth_Keller msg: 1613173798208 epoch sec; seqNo:7263; depth 17.03 2021-02-12T23:49:58Z- handleDvl() - RDI_Pathfinder msg: 1613173798.23 epoch sec; seqNo:3359 2021-02-12T23:49:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 97.65 , 0.00 , 107.38 2021-02-12T23:49:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.14 , -0.11 2021-02-12T23:49:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173798.24 epoch sec; seqNo:7263; 4057955.75 north; 591586.52 east 2021-02-12T23:49:58Z- AHRS_M2 msg: 1613173798.60 epoch sec; seqNo:7204 2021-02-12T23:49:58Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.16 psi 2021-02-12T23:49:58Z- handleDepth()-Depth_Keller msg: 1613173798605 epoch sec; seqNo:7264; depth 17.15 2021-02-12T23:49:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173798.62 epoch sec; seqNo:7264; 4057956.13 north; 591586.51 east 2021-02-12T23:49:59Z- AHRS_M2 msg: 1613173799.03 epoch sec; seqNo:7205 2021-02-12T23:49:59Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.17 psi 2021-02-12T23:49:59Z- handleDepth()-Depth_Keller msg: 1613173799037 epoch sec; seqNo:7265; depth 17.26 2021-02-12T23:49:59Z- handleDvl() - RDI_Pathfinder msg: 1613173799.05 epoch sec; seqNo:3360 2021-02-12T23:49:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.44 , 69.88 , 106.91 2021-02-12T23:49:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.23 , -0.01 2021-02-12T23:49:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173799.07 epoch sec; seqNo:7265; 4057956.51 north; 591586.51 east 2021-02-12T23:49:59Z- AHRS_M2 msg: 1613173799.41 epoch sec; seqNo:7206 2021-02-12T23:49:59Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.17 psi 2021-02-12T23:49:59Z- handleDepth()-Depth_Keller msg: 1613173799413 epoch sec; seqNo:7266; depth 17.40 2021-02-12T23:49:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173799.43 epoch sec; seqNo:7266; 4057956.89 north; 591586.50 east 2021-02-12T23:49:59Z- AHRS_M2 msg: 1613173799.83 epoch sec; seqNo:7207 2021-02-12T23:49:59Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.17 psi 2021-02-12T23:49:59Z- handleDepth()-Depth_Keller msg: 1613173799841 epoch sec; seqNo:7267; depth 17.51 2021-02-12T23:49:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173799.86 epoch sec; seqNo:7267; 4057957.27 north; 591586.50 east 2021-02-12T23:50:00Z- AHRS_M2 msg: 1613173800.22 epoch sec; seqNo:7208 2021-02-12T23:50:00Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-12T23:50:00Z- handleDepth()-Depth_Keller msg: 1613173800223 epoch sec; seqNo:7268; depth 17.63 2021-02-12T23:50:00Z- handleDvl() - RDI_Pathfinder msg: 1613173800.23 epoch sec; seqNo:3361 2021-02-12T23:50:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 97.64 , 78.18 , 108.59 2021-02-12T23:50:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.22 , 0.10 2021-02-12T23:50:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173800.24 epoch sec; seqNo:7268; 4057957.27 north; 591586.50 east 2021-02-12T23:50:00Z- AHRS_M2 msg: 1613173800.62 epoch sec; seqNo:7209 2021-02-12T23:50:00Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-12T23:50:00Z- handleDepth()-Depth_Keller msg: 1613173800629 epoch sec; seqNo:7269; depth 17.74 2021-02-12T23:50:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173800.64 epoch sec; seqNo:7269; 4057957.65 north; 591586.50 east 2021-02-12T23:50:01Z- AHRS_M2 msg: 1613173801.03 epoch sec; seqNo:7210 2021-02-12T23:50:01Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-12T23:50:01Z- handleDepth()-Depth_Keller msg: 1613173801037 epoch sec; seqNo:7270; depth 17.87 2021-02-12T23:50:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173801.05 epoch sec; seqNo:7270; 4057958.02 north; 591586.49 east 2021-02-12T23:50:01Z- LcmTrn::handleMessages() - handling 40 messages took 0.62 ms and 128 calls... 2021-02-12T23:50:01Z- waiting for 1613173799.20, time is 1613173801.05 2021-02-12T23:50:01Z- LcmTrn::updateTrn() >>>> pose time 1613173801.05, meas time 1613173800.23 <<<< 2021-02-12T23:50:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173800.23 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:50:01Z- TerrainNav::motionUpdate() - currEstimate = (4057958, 591586.49, 17.87) 2021-02-12T23:50:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:50:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:50:01Z- TNavBF::Projecting Measurements 2021-02-12T23:50:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:50:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:50:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:50:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:50:01Z- TNavFilter::4 good beams. 2021-02-12T23:50:01Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:50:01Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:50:01Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:50:01Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:50:01Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:50:01Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:50:01Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:50:01Z- TNavBF::mmse selected from filter -1 2021-02-12T23:50:01Z- TerrainMapOctree: (vehN, vehE ) = (4057957.39, 591586.29). 2021-02-12T23:50:01Z- TerrainMapOctree: Min Distance = 4235.16. 2021-02-12T23:50:01Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:50:01Z- There is no specific good beam that all particles have in common. 2021-02-12T23:50:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:50:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.39 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 10.40 2021-02-12T23:50:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:50:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 15.25 2021-02-12T23:50:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:50:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 9.78 2021-02-12T23:50:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:50:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 18.87 2021-02-12T23:50:01Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:50:01Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:50:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:50:01Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:50:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:50:01Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:50:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:50:01Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:50:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:50:01Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:50:01Z- Filter 0 No beams used in subcloud update 2021-02-12T23:50:01Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:50:01Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:50:01Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 734.0 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:50:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173800.23 sec. 2021-02-12T23:50:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:50:01Z- TNavBF::mmse selected from filter -1 2021-02-12T23:50:01Z- variances([0], [2], and psi_berg) = 734.002541 833.019555 0.000000 2021-02-12T23:50:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:50:01Z- LcmTrn::publishEstimates() - MLE : 4057950.52 591538.99 17.87 2021-02-12T23:50:01Z- LcmTrn::publishEstimates() - MMSE : 4057958.02 591586.49 17.87 2021-02-12T23:50:01Z- LcmTrn::publishEstimates() - COVAR: 734.00 833.02 1.04 0.00 2021-02-12T23:50:01Z- AHRS_M2 msg: 1613173801.43 epoch sec; seqNo:7211 2021-02-12T23:50:01Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-12T23:50:01Z- handleDepth()-Depth_Keller msg: 1613173801433 epoch sec; seqNo:7271; depth 17.98 2021-02-12T23:50:01Z- handleDvl() - RDI_Pathfinder msg: 1613173801.45 epoch sec; seqNo:3362 2021-02-12T23:50:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.39 , 97.64 , 70.69 , 104.94 2021-02-12T23:50:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.12 , 0.18 2021-02-12T23:50:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173801.46 epoch sec; seqNo:7271; 4057958.02 north; 591586.49 east 2021-02-12T23:50:01Z- AHRS_M2 msg: 1613173801.83 epoch sec; seqNo:7212 2021-02-12T23:50:01Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-12T23:50:01Z- handleDepth()-Depth_Keller msg: 1613173801911 epoch sec; seqNo:7272; depth 18.10 2021-02-12T23:50:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173801.92 epoch sec; seqNo:7272; 4057958.42 north; 591587.71 east 2021-02-12T23:50:02Z- AHRS_M2 msg: 1613173802.24 epoch sec; seqNo:7213 2021-02-12T23:50:02Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-12T23:50:02Z- handleDepth()-Depth_Keller msg: 1613173802249 epoch sec; seqNo:7273; depth 18.24 2021-02-12T23:50:02Z- handleDvl() - RDI_Pathfinder msg: 1613173802.26 epoch sec; seqNo:3363 2021-02-12T23:50:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.66 , 97.91 , 0.00 , 107.64 2021-02-12T23:50:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , 0.13 2021-02-12T23:50:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173802.29 epoch sec; seqNo:7273; 4057958.80 north; 591587.70 east 2021-02-12T23:50:02Z- AHRS_M2 msg: 1613173802.64 epoch sec; seqNo:7214 2021-02-12T23:50:02Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-12T23:50:02Z- handleDepth()-Depth_Keller msg: 1613173802665 epoch sec; seqNo:7274; depth 18.33 2021-02-12T23:50:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173802.68 epoch sec; seqNo:7274; 4057958.80 north; 591587.70 east 2021-02-12T23:50:03Z- AHRS_M2 msg: 1613173803.04 epoch sec; seqNo:7215 2021-02-12T23:50:03Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-12T23:50:03Z- handleDepth()-Depth_Keller msg: 1613173803131 epoch sec; seqNo:7275; depth 18.45 2021-02-12T23:50:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173803.18 epoch sec; seqNo:7275; 4057959.17 north; 591587.70 east 2021-02-12T23:50:03Z- AHRS_M2 msg: 1613173803.45 epoch sec; seqNo:7216 2021-02-12T23:50:03Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.18 psi 2021-02-12T23:50:03Z- handleDepth()-Depth_Keller msg: 1613173803460 epoch sec; seqNo:7276; depth 18.59 2021-02-12T23:50:03Z- handleDvl() - RDI_Pathfinder msg: 1613173803.49 epoch sec; seqNo:3364 2021-02-12T23:50:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.44 , 78.51 , 109.60 2021-02-12T23:50:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.06 , 0.02 2021-02-12T23:50:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173803.51 epoch sec; seqNo:7276; 4057959.55 north; 591587.70 east 2021-02-12T23:50:03Z- AHRS_M2 msg: 1613173803.86 epoch sec; seqNo:7217 2021-02-12T23:50:03Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.18 psi 2021-02-12T23:50:03Z- handleDepth()-Depth_Keller msg: 1613173803881 epoch sec; seqNo:7277; depth 18.69 2021-02-12T23:50:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173803.90 epoch sec; seqNo:7277; 4057959.93 north; 591587.69 east 2021-02-12T23:50:04Z- AHRS_M2 msg: 1613173804.26 epoch sec; seqNo:7218 2021-02-12T23:50:04Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.17 psi 2021-02-12T23:50:04Z- handleDepth()-Depth_Keller msg: 1613173804281 epoch sec; seqNo:7278; depth 18.81 2021-02-12T23:50:04Z- handleDvl() - RDI_Pathfinder msg: 1613173804.29 epoch sec; seqNo:3365 2021-02-12T23:50:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.82 , 97.64 , 71.92 , 107.37 2021-02-12T23:50:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , -0.08 2021-02-12T23:50:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173804.32 epoch sec; seqNo:7278; 4057959.94 north; 591588.91 east 2021-02-12T23:50:04Z- AHRS_M2 msg: 1613173804.66 epoch sec; seqNo:7219 2021-02-12T23:50:04Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.17 psi 2021-02-12T23:50:04Z- handleDepth()-Depth_Keller msg: 1613173804673 epoch sec; seqNo:7279; depth 18.92 2021-02-12T23:50:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173804.68 epoch sec; seqNo:7279; 4057960.32 north; 591588.91 east 2021-02-12T23:50:05Z- AHRS_M2 msg: 1613173805.07 epoch sec; seqNo:7220 2021-02-12T23:50:05Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.17 psi 2021-02-12T23:50:05Z- handleDepth()-Depth_Keller msg: 1613173805078 epoch sec; seqNo:7280; depth 19.03 2021-02-12T23:50:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173805.11 epoch sec; seqNo:7280; 4057960.70 north; 591588.90 east 2021-02-12T23:50:05Z- AHRS_M2 msg: 1613173805.47 epoch sec; seqNo:7221 2021-02-12T23:50:05Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.17 psi 2021-02-12T23:50:05Z- handleDepth()-Depth_Keller msg: 1613173805473 epoch sec; seqNo:7281; depth 19.16 2021-02-12T23:50:05Z- handleDvl() - RDI_Pathfinder msg: 1613173805.48 epoch sec; seqNo:3366 2021-02-12T23:50:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 70.69 , 105.21 2021-02-12T23:50:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , -0.10 2021-02-12T23:50:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173805.49 epoch sec; seqNo:7281; 4057961.08 north; 591588.90 east 2021-02-12T23:50:05Z- AHRS_M2 msg: 1613173805.87 epoch sec; seqNo:7222 2021-02-12T23:50:05Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.18 psi 2021-02-12T23:50:05Z- handleDepth()-Depth_Keller msg: 1613173805877 epoch sec; seqNo:7282; depth 19.26 2021-02-12T23:50:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173805.89 epoch sec; seqNo:7282; 4057961.08 north; 591588.90 east 2021-02-12T23:50:06Z- AHRS_M2 msg: 1613173806.28 epoch sec; seqNo:7223 2021-02-12T23:50:06Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.18 psi 2021-02-12T23:50:06Z- handleDepth()-Depth_Keller msg: 1613173806282 epoch sec; seqNo:7283; depth 19.36 2021-02-12T23:50:06Z- handleDvl() - RDI_Pathfinder msg: 1613173806.29 epoch sec; seqNo:3367 2021-02-12T23:50:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 70.69 , 103.99 2021-02-12T23:50:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.12 , -0.06 2021-02-12T23:50:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173806.30 epoch sec; seqNo:7283; 4057961.46 north; 591588.90 east 2021-02-12T23:50:06Z- LcmTrn::handleMessages() - handling 45 messages took 0.82 ms and 129 calls... 2021-02-12T23:50:06Z- waiting for 1613173804.05, time is 1613173806.30 2021-02-12T23:50:06Z- LcmTrn::updateTrn() equating pose 1613173806.30 and meas times 1613173806.30 2021-02-12T23:50:06Z- LcmTrn::updateTrn() >>>> pose time 1613173806.30, meas time 1613173806.30 <<<< 2021-02-12T23:50:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173806.30 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:50:06Z- TerrainNav::motionUpdate() - currEstimate = (4057961, 591588.90, 19.36) 2021-02-12T23:50:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:50:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:50:06Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173806.30 sec.; No relevant navigation data available 2021-02-12T23:50:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:50:06Z- TNavBF::mmse selected from filter -1 2021-02-12T23:50:06Z- variances([0], [2], and psi_berg) = 733.921145 833.019493 0.000000 2021-02-12T23:50:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:50:06Z- LcmTrn::publishEstimates() - MLE : 4057952.96 591545.40 19.36 2021-02-12T23:50:06Z- LcmTrn::publishEstimates() - MMSE : 4057961.46 591588.90 19.36 2021-02-12T23:50:06Z- LcmTrn::publishEstimates() - COVAR: 733.92 833.02 1.25 0.00 2021-02-12T23:50:06Z- AHRS_M2 msg: 1613173806.68 epoch sec; seqNo:7224 2021-02-12T23:50:06Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.18 psi 2021-02-12T23:50:06Z- handleDepth()-Depth_Keller msg: 1613173806684 epoch sec; seqNo:7284; depth 19.49 2021-02-12T23:50:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173806.71 epoch sec; seqNo:7284; 4057961.85 north; 591590.11 east 2021-02-12T23:50:07Z- AHRS_M2 msg: 1613173807.08 epoch sec; seqNo:7225 2021-02-12T23:50:07Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.19 psi 2021-02-12T23:50:07Z- handleDepth()-Depth_Keller msg: 1613173807093 epoch sec; seqNo:7285; depth 19.61 2021-02-12T23:50:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173807.10 epoch sec; seqNo:7285; 4057962.23 north; 591590.11 east 2021-02-12T23:50:07Z- AHRS_M2 msg: 1613173807.49 epoch sec; seqNo:7226 2021-02-12T23:50:07Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.19 psi 2021-02-12T23:50:07Z- handleDepth()-Depth_Keller msg: 1613173807494 epoch sec; seqNo:7286; depth 19.72 2021-02-12T23:50:07Z- handleDvl() - RDI_Pathfinder msg: 1613173807.51 epoch sec; seqNo:3368 2021-02-12T23:50:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 78.51 , 106.50 2021-02-12T23:50:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.12 , 0.03 2021-02-12T23:50:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173807.52 epoch sec; seqNo:7286; 4057962.23 north; 591590.11 east 2021-02-12T23:50:07Z- AHRS_M2 msg: 1613173807.89 epoch sec; seqNo:7227 2021-02-12T23:50:07Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.19 psi 2021-02-12T23:50:07Z- handleDepth()-Depth_Keller msg: 1613173807905 epoch sec; seqNo:7287; depth 19.82 2021-02-12T23:50:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173807.92 epoch sec; seqNo:7287; 4057962.61 north; 591590.11 east 2021-02-12T23:50:08Z- AHRS_M2 msg: 1613173808.30 epoch sec; seqNo:7228 2021-02-12T23:50:08Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.20 psi 2021-02-12T23:50:08Z- handleDepth()-Depth_Keller msg: 1613173808310 epoch sec; seqNo:7288; depth 19.94 2021-02-12T23:50:08Z- handleDvl() - RDI_Pathfinder msg: 1613173808.32 epoch sec; seqNo:3369 2021-02-12T23:50:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:50:08Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:50:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173808.33 epoch sec; seqNo:7288; 4057962.99 north; 591590.10 east 2021-02-12T23:50:08Z- AHRS_M2 msg: 1613173808.70 epoch sec; seqNo:7229 2021-02-12T23:50:08Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.20 psi 2021-02-12T23:50:08Z- handleDepth()-Depth_Keller msg: 1613173808704 epoch sec; seqNo:7289; depth 20.07 2021-02-12T23:50:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173808.71 epoch sec; seqNo:7289; 4057963.37 north; 591590.10 east 2021-02-12T23:50:09Z- AHRS_M2 msg: 1613173809.12 epoch sec; seqNo:7230 2021-02-12T23:50:09Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.20 psi 2021-02-12T23:50:09Z- handleDepth()-Depth_Keller msg: 1613173809135 epoch sec; seqNo:7290; depth 20.17 2021-02-12T23:50:09Z- handleDvl() - RDI_Pathfinder msg: 1613173809.16 epoch sec; seqNo:3370 2021-02-12T23:50:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.78 , 0.00 , 106.70 2021-02-12T23:50:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , 0.07 2021-02-12T23:50:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173809.17 epoch sec; seqNo:7290; 4057963.37 north; 591590.10 east 2021-02-12T23:50:09Z- AHRS_M2 msg: 1613173809.51 epoch sec; seqNo:7231 2021-02-12T23:50:09Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.20 psi 2021-02-12T23:50:09Z- handleDepth()-Depth_Keller msg: 1613173809521 epoch sec; seqNo:7291; depth 20.29 2021-02-12T23:50:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173809.53 epoch sec; seqNo:7291; 4057963.76 north; 591591.31 east 2021-02-12T23:50:09Z- AHRS_M2 msg: 1613173809.91 epoch sec; seqNo:7232 2021-02-12T23:50:09Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.20 psi 2021-02-12T23:50:09Z- handleDepth()-Depth_Keller msg: 1613173809916 epoch sec; seqNo:7292; depth 20.41 2021-02-12T23:50:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173809.94 epoch sec; seqNo:7292; 4057964.14 north; 591591.31 east 2021-02-12T23:50:10Z- AHRS_M2 msg: 1613173810.32 epoch sec; seqNo:7233 2021-02-12T23:50:10Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.20 psi 2021-02-12T23:50:10Z- handleDepth()-Depth_Keller msg: 1613173810325 epoch sec; seqNo:7293; depth 20.51 2021-02-12T23:50:10Z- handleDvl() - RDI_Pathfinder msg: 1613173810.34 epoch sec; seqNo:3371 2021-02-12T23:50:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 97.51 , 78.58 , 104.27 2021-02-12T23:50:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.03 2021-02-12T23:50:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173810.36 epoch sec; seqNo:7293; 4057964.14 north; 591591.31 east 2021-02-12T23:50:10Z- AHRS_M2 msg: 1613173810.72 epoch sec; seqNo:7234 2021-02-12T23:50:10Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.20 psi 2021-02-12T23:50:10Z- handleDepth()-Depth_Keller msg: 1613173810726 epoch sec; seqNo:7294; depth 20.64 2021-02-12T23:50:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173810.74 epoch sec; seqNo:7294; 4057964.52 north; 591591.31 east 2021-02-12T23:50:11Z- AHRS_M2 msg: 1613173811.13 epoch sec; seqNo:7235 2021-02-12T23:50:11Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.20 psi 2021-02-12T23:50:11Z- handleDepth()-Depth_Keller msg: 1613173811140 epoch sec; seqNo:7295; depth 20.75 2021-02-12T23:50:11Z- handleDvl() - RDI_Pathfinder msg: 1613173811.15 epoch sec; seqNo:3372 2021-02-12T23:50:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 97.78 , 67.84 , 105.08 2021-02-12T23:50:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.09 , -0.01 2021-02-12T23:50:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173811.16 epoch sec; seqNo:7295; 4057964.90 north; 591591.30 east 2021-02-12T23:50:11Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-12T23:50:11Z- waiting for 1613173809.30, time is 1613173811.16 2021-02-12T23:50:11Z- LcmTrn::updateTrn() equating pose 1613173811.16 and meas times 1613173811.16 2021-02-12T23:50:11Z- LcmTrn::updateTrn() >>>> pose time 1613173811.16, meas time 1613173811.16 <<<< 2021-02-12T23:50:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173811.16 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:50:11Z- TerrainNav::motionUpdate() - currEstimate = (4057965, 591591.30, 20.75) 2021-02-12T23:50:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:50:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:50:11Z- TNavBF::Projecting Measurements 2021-02-12T23:50:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:50:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:50:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:50:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:50:11Z- TNavFilter::4 good beams. 2021-02-12T23:50:11Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:50:11Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:50:11Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:50:11Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:50:11Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:50:11Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:50:11Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:50:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:50:11Z- TerrainMapOctree: (vehN, vehE ) = (4057964.90, 591591.30). 2021-02-12T23:50:11Z- TerrainMapOctree: Min Distance = 4226.18. 2021-02-12T23:50:11Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:50:11Z- There is no specific good beam that all particles have in common. 2021-02-12T23:50:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:50:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.46 K: 0.123 ExpectedMeasDiff: nan currMeas.covariance: 8.75 2021-02-12T23:50:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:50:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-12T23:50:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:50:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.116 ExpectedMeasDiff: nan currMeas.covariance: 7.36 2021-02-12T23:50:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:50:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 17.67 2021-02-12T23:50:11Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:50:11Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:50:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:50:11Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:50:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:50:11Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:50:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:50:11Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:50:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:50:11Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:50:11Z- Filter 0 No beams used in subcloud update 2021-02-12T23:50:11Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:50:11Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:50:11Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 733.8 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:50:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173811.16 sec. 2021-02-12T23:50:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:50:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:50:11Z- variances([0], [2], and psi_berg) = 733.839829 833.019431 0.000000 2021-02-12T23:50:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:50:11Z- LcmTrn::publishEstimates() - MLE : 4057956.40 591547.80 20.75 2021-02-12T23:50:11Z- LcmTrn::publishEstimates() - MMSE : 4057964.90 591591.30 20.75 2021-02-12T23:50:11Z- LcmTrn::publishEstimates() - COVAR: 733.84 833.02 1.00 0.00 2021-02-12T23:50:11Z- AHRS_M2 msg: 1613173811.53 epoch sec; seqNo:7236 2021-02-12T23:50:11Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.20 psi 2021-02-12T23:50:11Z- handleDepth()-Depth_Keller msg: 1613173811537 epoch sec; seqNo:7296; depth 20.87 2021-02-12T23:50:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173811.56 epoch sec; seqNo:7296; 4057965.27 north; 591591.30 east 2021-02-12T23:50:11Z- AHRS_M2 msg: 1613173811.93 epoch sec; seqNo:7237 2021-02-12T23:50:11Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.20 psi 2021-02-12T23:50:11Z- handleDepth()-Depth_Keller msg: 1613173811941 epoch sec; seqNo:7297; depth 20.97 2021-02-12T23:50:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173811.95 epoch sec; seqNo:7297; 4057965.29 north; 591592.52 east 2021-02-12T23:50:12Z- AHRS_M2 msg: 1613173812.34 epoch sec; seqNo:7238 2021-02-12T23:50:12Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.20 psi 2021-02-12T23:50:12Z- handleDepth()-Depth_Keller msg: 1613173812342 epoch sec; seqNo:7298; depth 21.10 2021-02-12T23:50:12Z- handleDvl() - RDI_Pathfinder msg: 1613173812.35 epoch sec; seqNo:3373 2021-02-12T23:50:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 98.12 , 68.65 , 105.42 2021-02-12T23:50:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.07 , 0.00 2021-02-12T23:50:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173812.36 epoch sec; seqNo:7298; 4057965.67 north; 591592.51 east 2021-02-12T23:50:12Z- AHRS_M2 msg: 1613173812.74 epoch sec; seqNo:7239 2021-02-12T23:50:12Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.20 psi 2021-02-12T23:50:12Z- handleDepth()-Depth_Keller msg: 1613173812754 epoch sec; seqNo:7299; depth 21.21 2021-02-12T23:50:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173812.77 epoch sec; seqNo:7299; 4057966.05 north; 591592.51 east 2021-02-12T23:50:13Z- AHRS_M2 msg: 1613173813.15 epoch sec; seqNo:7240 2021-02-12T23:50:13Z- AHRS_M2 msg: 0.69 phi; -0.33 theta; -0.20 psi 2021-02-12T23:50:13Z- handleDepth()-Depth_Keller msg: 1613173813151 epoch sec; seqNo:7300; depth 21.33 2021-02-12T23:50:13Z- handleDvl() - RDI_Pathfinder msg: 1613173813.17 epoch sec; seqNo:3374 2021-02-12T23:50:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 95.55 , 69.06 , 105.28 2021-02-12T23:50:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.10 , 0.04 2021-02-12T23:50:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173813.18 epoch sec; seqNo:7300; 4057966.42 north; 591592.51 east 2021-02-12T23:50:13Z- AHRS_M2 msg: 1613173813.55 epoch sec; seqNo:7241 2021-02-12T23:50:13Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.20 psi 2021-02-12T23:50:13Z- handleDepth()-Depth_Keller msg: 1613173813555 epoch sec; seqNo:7301; depth 21.44 2021-02-12T23:50:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173813.57 epoch sec; seqNo:7301; 4057966.42 north; 591592.51 east 2021-02-12T23:50:13Z- AHRS_M2 msg: 1613173813.95 epoch sec; seqNo:7242 2021-02-12T23:50:13Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.19 psi 2021-02-12T23:50:13Z- handleDepth()-Depth_Keller msg: 1613173813961 epoch sec; seqNo:7302; depth 21.55 2021-02-12T23:50:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173813.97 epoch sec; seqNo:7302; 4057966.80 north; 591592.50 east 2021-02-12T23:50:14Z- AHRS_M2 msg: 1613173814.36 epoch sec; seqNo:7243 2021-02-12T23:50:14Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.19 psi 2021-02-12T23:50:14Z- handleDepth()-Depth_Keller msg: 1613173814365 epoch sec; seqNo:7303; depth 21.68 2021-02-12T23:50:14Z- handleDvl() - RDI_Pathfinder msg: 1613173814.38 epoch sec; seqNo:3375 2021-02-12T23:50:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 98.11 , 69.06 , 102.98 2021-02-12T23:50:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.07 , 0.06 2021-02-12T23:50:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173814.39 epoch sec; seqNo:7303; 4057967.18 north; 591592.50 east 2021-02-12T23:50:14Z- AHRS_M2 msg: 1613173814.76 epoch sec; seqNo:7244 2021-02-12T23:50:14Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.19 psi 2021-02-12T23:50:14Z- handleDepth()-Depth_Keller msg: 1613173814766 epoch sec; seqNo:7304; depth 21.79 2021-02-12T23:50:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173814.78 epoch sec; seqNo:7304; 4057967.57 north; 591593.72 east 2021-02-12T23:50:15Z- AHRS_M2 msg: 1613173815.16 epoch sec; seqNo:7245 2021-02-12T23:50:15Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.19 psi 2021-02-12T23:50:15Z- handleDepth()-Depth_Keller msg: 1613173815233 epoch sec; seqNo:7305; depth 21.92 2021-02-12T23:50:15Z- handleDvl() - RDI_Pathfinder msg: 1613173815.24 epoch sec; seqNo:3376 2021-02-12T23:50:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.98 , 78.51 , 104.06 2021-02-12T23:50:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.06 2021-02-12T23:50:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173815.26 epoch sec; seqNo:7305; 4057967.57 north; 591593.72 east 2021-02-12T23:50:15Z- AHRS_M2 msg: 1613173815.57 epoch sec; seqNo:7246 2021-02-12T23:50:15Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.19 psi 2021-02-12T23:50:15Z- handleDepth()-Depth_Keller msg: 1613173815580 epoch sec; seqNo:7306; depth 22.05 2021-02-12T23:50:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173815.60 epoch sec; seqNo:7306; 4057967.95 north; 591593.71 east 2021-02-12T23:50:15Z- AHRS_M2 msg: 1613173815.97 epoch sec; seqNo:7247 2021-02-12T23:50:15Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.20 psi 2021-02-12T23:50:16Z- handleDepth()-Depth_Keller msg: 1613173816002 epoch sec; seqNo:7307; depth 22.14 2021-02-12T23:50:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173816.04 epoch sec; seqNo:7307; 4057968.33 north; 591593.71 east 2021-02-12T23:50:16Z- AHRS_M2 msg: 1613173816.38 epoch sec; seqNo:7248 2021-02-12T23:50:16Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.20 psi 2021-02-12T23:50:16Z- handleDepth()-Depth_Keller msg: 1613173816466 epoch sec; seqNo:7308; depth 22.27 2021-02-12T23:50:16Z- handleDvl() - RDI_Pathfinder msg: 1613173816.48 epoch sec; seqNo:3377 2021-02-12T23:50:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.78 , 92.17 , 0.00 , 105.69 2021-02-12T23:50:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , 0.02 2021-02-12T23:50:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173816.49 epoch sec; seqNo:7308; 4057968.33 north; 591593.71 east 2021-02-12T23:50:16Z- LcmTrn::handleMessages() - handling 44 messages took 0.79 ms and 129 calls... 2021-02-12T23:50:16Z- waiting for 1613173814.16, time is 1613173816.49 2021-02-12T23:50:16Z- LcmTrn::updateTrn() equating pose 1613173816.49 and meas times 1613173816.49 2021-02-12T23:50:16Z- LcmTrn::updateTrn() >>>> pose time 1613173816.49, meas time 1613173816.49 <<<< 2021-02-12T23:50:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173816.49 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:50:16Z- TerrainNav::motionUpdate() - currEstimate = (4057968, 591593.71, 22.27) 2021-02-12T23:50:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:50:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:50:16Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173816.49 sec.; No relevant navigation data available 2021-02-12T23:50:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:50:16Z- TNavBF::mmse selected from filter -1 2021-02-12T23:50:16Z- variances([0], [2], and psi_berg) = 733.758593 833.019369 0.000000 2021-02-12T23:50:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:50:16Z- LcmTrn::publishEstimates() - MLE : 4057959.83 591550.21 22.27 2021-02-12T23:50:16Z- LcmTrn::publishEstimates() - MMSE : 4057968.33 591593.71 22.27 2021-02-12T23:50:16Z- LcmTrn::publishEstimates() - COVAR: 733.76 833.02 1.22 0.00 2021-02-12T23:50:16Z- AHRS_M2 msg: 1613173816.78 epoch sec; seqNo:7249 2021-02-12T23:50:16Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.21 psi 2021-02-12T23:50:16Z- handleDepth()-Depth_Keller msg: 1613173816791 epoch sec; seqNo:7309; depth 22.42 2021-02-12T23:50:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173816.80 epoch sec; seqNo:7309; 4057968.72 north; 591594.92 east 2021-02-12T23:50:17Z- AHRS_M2 msg: 1613173817.18 epoch sec; seqNo:7250 2021-02-12T23:50:17Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.21 psi 2021-02-12T23:50:17Z- handleDepth()-Depth_Keller msg: 1613173817209 epoch sec; seqNo:7310; depth 22.50 2021-02-12T23:50:17Z- handleDvl() - RDI_Pathfinder msg: 1613173817.23 epoch sec; seqNo:3378 2021-02-12T23:50:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.71 , 67.84 , 107.45 2021-02-12T23:50:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.06 , -0.05 2021-02-12T23:50:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173817.24 epoch sec; seqNo:7310; 4057969.10 north; 591594.92 east 2021-02-12T23:50:17Z- AHRS_M2 msg: 1613173817.59 epoch sec; seqNo:7251 2021-02-12T23:50:17Z- AHRS_M2 msg: 0.69 phi; -0.36 theta; -0.21 psi 2021-02-12T23:50:17Z- handleDepth()-Depth_Keller msg: 1613173817613 epoch sec; seqNo:7311; depth 22.63 2021-02-12T23:50:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173817.62 epoch sec; seqNo:7311; 4057969.10 north; 591594.92 east 2021-02-12T23:50:17Z- AHRS_M2 msg: 1613173817.99 epoch sec; seqNo:7252 2021-02-12T23:50:17Z- AHRS_M2 msg: 0.69 phi; -0.36 theta; -0.21 psi 2021-02-12T23:50:18Z- handleDepth()-Depth_Keller msg: 1613173818006 epoch sec; seqNo:7312; depth 22.78 2021-02-12T23:50:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173818.01 epoch sec; seqNo:7312; 4057969.48 north; 591594.92 east 2021-02-12T23:50:18Z- AHRS_M2 msg: 1613173818.40 epoch sec; seqNo:7253 2021-02-12T23:50:18Z- AHRS_M2 msg: 0.69 phi; -0.36 theta; -0.21 psi 2021-02-12T23:50:18Z- handleDepth()-Depth_Keller msg: 1613173818401 epoch sec; seqNo:7313; depth 22.88 2021-02-12T23:50:18Z- handleDvl() - RDI_Pathfinder msg: 1613173818.42 epoch sec; seqNo:3379 2021-02-12T23:50:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.18 , 93.99 , 67.01 , 104.94 2021-02-12T23:50:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , -0.09 2021-02-12T23:50:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173818.44 epoch sec; seqNo:7313; 4057969.86 north; 591594.91 east 2021-02-12T23:50:18Z- AHRS_M2 msg: 1613173818.80 epoch sec; seqNo:7254 2021-02-12T23:50:18Z- AHRS_M2 msg: 0.70 phi; -0.36 theta; -0.21 psi 2021-02-12T23:50:18Z- handleDepth()-Depth_Keller msg: 1613173818809 epoch sec; seqNo:7314; depth 23.03 2021-02-12T23:50:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173818.82 epoch sec; seqNo:7314; 4057969.86 north; 591594.91 east 2021-02-12T23:50:19Z- AHRS_M2 msg: 1613173819.21 epoch sec; seqNo:7255 2021-02-12T23:50:19Z- AHRS_M2 msg: 0.70 phi; -0.36 theta; -0.21 psi 2021-02-12T23:50:19Z- handleDepth()-Depth_Keller msg: 1613173819210 epoch sec; seqNo:7315; depth 23.15 2021-02-12T23:50:19Z- handleDvl() - RDI_Pathfinder msg: 1613173819.22 epoch sec; seqNo:3380 2021-02-12T23:50:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.73 , 98.11 , 67.42 , 105.41 2021-02-12T23:50:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , -0.11 2021-02-12T23:50:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173819.23 epoch sec; seqNo:7315; 4057970.25 north; 591596.13 east 2021-02-12T23:50:19Z- AHRS_M2 msg: 1613173819.61 epoch sec; seqNo:7256 2021-02-12T23:50:19Z- AHRS_M2 msg: 0.70 phi; -0.36 theta; -0.21 psi 2021-02-12T23:50:19Z- handleDepth()-Depth_Keller msg: 1613173819613 epoch sec; seqNo:7316; depth 23.29 2021-02-12T23:50:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173819.64 epoch sec; seqNo:7316; 4057970.63 north; 591596.12 east 2021-02-12T23:50:20Z- AHRS_M2 msg: 1613173820.02 epoch sec; seqNo:7257 2021-02-12T23:50:20Z- AHRS_M2 msg: 0.70 phi; -0.36 theta; -0.20 psi 2021-02-12T23:50:20Z- handleDepth()-Depth_Keller msg: 1613173820025 epoch sec; seqNo:7317; depth 23.42 2021-02-12T23:50:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173820.08 epoch sec; seqNo:7317; 4057971.01 north; 591596.12 east 2021-02-12T23:50:20Z- AHRS_M2 msg: 1613173820.42 epoch sec; seqNo:7258 2021-02-12T23:50:20Z- AHRS_M2 msg: 0.69 phi; -0.36 theta; -0.20 psi 2021-02-12T23:50:20Z- handleDepth()-Depth_Keller msg: 1613173820429 epoch sec; seqNo:7318; depth 23.55 2021-02-12T23:50:20Z- handleDvl() - RDI_Pathfinder msg: 1613173820.44 epoch sec; seqNo:3381 2021-02-12T23:50:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 98.12 , 65.79 , 104.20 2021-02-12T23:50:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , -0.05 2021-02-12T23:50:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173820.47 epoch sec; seqNo:7318; 4057971.01 north; 591596.12 east 2021-02-12T23:50:20Z- AHRS_M2 msg: 1613173820.82 epoch sec; seqNo:7259 2021-02-12T23:50:20Z- AHRS_M2 msg: 0.69 phi; -0.36 theta; -0.20 psi 2021-02-12T23:50:20Z- handleDepth()-Depth_Keller msg: 1613173820826 epoch sec; seqNo:7319; depth 23.67 2021-02-12T23:50:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173820.84 epoch sec; seqNo:7319; 4057971.39 north; 591596.12 east 2021-02-12T23:50:21Z- AHRS_M2 msg: 1613173821.23 epoch sec; seqNo:7260 2021-02-12T23:50:21Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.20 psi 2021-02-12T23:50:21Z- handleDepth()-Depth_Keller msg: 1613173821230 epoch sec; seqNo:7320; depth 23.78 2021-02-12T23:50:21Z- handleDvl() - RDI_Pathfinder msg: 1613173821.25 epoch sec; seqNo:3382 2021-02-12T23:50:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.74 , 90.75 , 65.38 , 105.35 2021-02-12T23:50:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.09 , 0.01 2021-02-12T23:50:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173821.26 epoch sec; seqNo:7320; 4057971.77 north; 591596.11 east 2021-02-12T23:50:21Z- LcmTrn::handleMessages() - handling 41 messages took 0.76 ms and 126 calls... 2021-02-12T23:50:21Z- waiting for 1613173819.49, time is 1613173821.26 2021-02-12T23:50:21Z- LcmTrn::updateTrn() equating pose 1613173821.26 and meas times 1613173821.26 2021-02-12T23:50:21Z- LcmTrn::updateTrn() >>>> pose time 1613173821.26, meas time 1613173821.26 <<<< 2021-02-12T23:50:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173821.26 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:50:21Z- TerrainNav::motionUpdate() - currEstimate = (4057972, 591596.11, 23.78) 2021-02-12T23:50:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:50:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:50:21Z- TNavBF::Projecting Measurements 2021-02-12T23:50:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:50:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:50:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:50:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:50:21Z- TNavFilter::4 good beams. 2021-02-12T23:50:21Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:50:21Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:50:21Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:50:21Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:50:21Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:50:21Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:50:21Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:50:21Z- TNavBF::mmse selected from filter -1 2021-02-12T23:50:21Z- TerrainMapOctree: (vehN, vehE ) = (4057971.77, 591596.11). 2021-02-12T23:50:21Z- TerrainMapOctree: Min Distance = 4217.81. 2021-02-12T23:50:21Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:50:21Z- There is no specific good beam that all particles have in common. 2021-02-12T23:50:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:50:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.123 ExpectedMeasDiff: nan currMeas.covariance: 8.47 2021-02-12T23:50:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:50:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 13.18 2021-02-12T23:50:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:50:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.119 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-12T23:50:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:50:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 17.76 2021-02-12T23:50:21Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:50:21Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:50:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:50:21Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:50:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:50:21Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:50:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:50:21Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:50:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:50:21Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:50:21Z- Filter 0 No beams used in subcloud update 2021-02-12T23:50:21Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:50:21Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:50:21Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 733.7 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:50:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173821.26 sec. 2021-02-12T23:50:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:50:21Z- TNavBF::mmse selected from filter -1 2021-02-12T23:50:21Z- variances([0], [2], and psi_berg) = 733.677437 833.019307 0.000000 2021-02-12T23:50:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:50:21Z- LcmTrn::publishEstimates() - MLE : 4057963.27 591552.61 23.78 2021-02-12T23:50:21Z- LcmTrn::publishEstimates() - MMSE : 4057971.77 591596.11 23.78 2021-02-12T23:50:21Z- LcmTrn::publishEstimates() - COVAR: 733.68 833.02 0.97 0.00 2021-02-12T23:50:21Z- AHRS_M2 msg: 1613173821.63 epoch sec; seqNo:7261 2021-02-12T23:50:21Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.20 psi 2021-02-12T23:50:21Z- handleDepth()-Depth_Keller msg: 1613173821633 epoch sec; seqNo:7321; depth 23.93 2021-02-12T23:50:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173821.65 epoch sec; seqNo:7321; 4057971.78 north; 591597.33 east 2021-02-12T23:50:22Z- AHRS_M2 msg: 1613173822.05 epoch sec; seqNo:7262 2021-02-12T23:50:22Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.21 psi 2021-02-12T23:50:22Z- handleDepth()-Depth_Keller msg: 1613173822076 epoch sec; seqNo:7322; depth 24.06 2021-02-12T23:50:22Z- handleDvl() - RDI_Pathfinder msg: 1613173822.09 epoch sec; seqNo:3383 2021-02-12T23:50:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 98.32 , 65.38 , 104.41 2021-02-12T23:50:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , 0.07 2021-02-12T23:50:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173822.10 epoch sec; seqNo:7322; 4057972.16 north; 591597.33 east 2021-02-12T23:50:22Z- AHRS_M2 msg: 1613173822.45 epoch sec; seqNo:7263 2021-02-12T23:50:22Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.22 psi 2021-02-12T23:50:22Z- handleDepth()-Depth_Keller msg: 1613173822457 epoch sec; seqNo:7323; depth 24.18 2021-02-12T23:50:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173822.47 epoch sec; seqNo:7323; 4057972.54 north; 591597.33 east 2021-02-12T23:50:22Z- AHRS_M2 msg: 1613173822.84 epoch sec; seqNo:7264 2021-02-12T23:50:22Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.23 psi 2021-02-12T23:50:22Z- handleDepth()-Depth_Keller msg: 1613173822849 epoch sec; seqNo:7324; depth 24.32 2021-02-12T23:50:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173822.86 epoch sec; seqNo:7324; 4057972.54 north; 591597.33 east 2021-02-12T23:50:23Z- AHRS_M2 msg: 1613173823.24 epoch sec; seqNo:7265 2021-02-12T23:50:23Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.23 psi 2021-02-12T23:50:23Z- handleDepth()-Depth_Keller msg: 1613173823249 epoch sec; seqNo:7325; depth 24.44 2021-02-12T23:50:23Z- handleDvl() - RDI_Pathfinder msg: 1613173823.26 epoch sec; seqNo:3384 2021-02-12T23:50:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 98.05 , 0.00 , 105.35 2021-02-12T23:50:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , 0.08 2021-02-12T23:50:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173823.27 epoch sec; seqNo:7325; 4057972.92 north; 591597.32 east 2021-02-12T23:50:23Z- AHRS_M2 msg: 1613173823.65 epoch sec; seqNo:7266 2021-02-12T23:50:23Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.24 psi 2021-02-12T23:50:23Z- handleDepth()-Depth_Keller msg: 1613173823654 epoch sec; seqNo:7326; depth 24.59 2021-02-12T23:50:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173823.66 epoch sec; seqNo:7326; 4057973.30 north; 591597.32 east 2021-02-12T23:50:24Z- AHRS_M2 msg: 1613173824.05 epoch sec; seqNo:7267 2021-02-12T23:50:24Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.24 psi 2021-02-12T23:50:24Z- handleDepth()-Depth_Keller msg: 1613173824061 epoch sec; seqNo:7327; depth 24.70 2021-02-12T23:50:24Z- handleDvl() - RDI_Pathfinder msg: 1613173824.08 epoch sec; seqNo:3385 2021-02-12T23:50:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 97.71 , 65.38 , 107.18 2021-02-12T23:50:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.02 , 0.05 2021-02-12T23:50:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173824.09 epoch sec; seqNo:7327; 4057973.30 north; 591597.32 east 2021-02-12T23:50:24Z- AHRS_M2 msg: 1613173824.46 epoch sec; seqNo:7268 2021-02-12T23:50:24Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.24 psi 2021-02-12T23:50:24Z- handleDepth()-Depth_Keller msg: 1613173824461 epoch sec; seqNo:7328; depth 24.85 2021-02-12T23:50:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173824.47 epoch sec; seqNo:7328; 4057973.69 north; 591598.53 east 2021-02-12T23:50:24Z- AHRS_M2 msg: 1613173824.87 epoch sec; seqNo:7269 2021-02-12T23:50:24Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.24 psi 2021-02-12T23:50:24Z- handleDepth()-Depth_Keller msg: 1613173824871 epoch sec; seqNo:7329; depth 24.97 2021-02-12T23:50:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173824.88 epoch sec; seqNo:7329; 4057974.07 north; 591598.53 east 2021-02-12T23:50:25Z- AHRS_M2 msg: 1613173825.27 epoch sec; seqNo:7270 2021-02-12T23:50:25Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.24 psi 2021-02-12T23:50:25Z- handleDepth()-Depth_Keller msg: 1613173825273 epoch sec; seqNo:7330; depth 25.10 2021-02-12T23:50:25Z- handleDvl() - RDI_Pathfinder msg: 1613173825.30 epoch sec; seqNo:3386 2021-02-12T23:50:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 98.19 , 66.20 , 105.49 2021-02-12T23:50:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.00 2021-02-12T23:50:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173825.31 epoch sec; seqNo:7330; 4057974.07 north; 591598.53 east 2021-02-12T23:50:25Z- AHRS_M2 msg: 1613173825.67 epoch sec; seqNo:7271 2021-02-12T23:50:25Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.23 psi 2021-02-12T23:50:25Z- handleDepth()-Depth_Keller msg: 1613173825681 epoch sec; seqNo:7331; depth 25.24 2021-02-12T23:50:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173825.69 epoch sec; seqNo:7331; 4057974.45 north; 591598.53 east 2021-02-12T23:50:26Z- AHRS_M2 msg: 1613173826.07 epoch sec; seqNo:7272 2021-02-12T23:50:26Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.22 psi 2021-02-12T23:50:26Z- handleDepth()-Depth_Keller msg: 1613173826077 epoch sec; seqNo:7332; depth 25.37 2021-02-12T23:50:26Z- handleDvl() - RDI_Pathfinder msg: 1613173826.09 epoch sec; seqNo:3387 2021-02-12T23:50:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 97.98 , 64.16 , 102.85 2021-02-12T23:50:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , -0.04 2021-02-12T23:50:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173826.10 epoch sec; seqNo:7332; 4057974.82 north; 591598.52 east 2021-02-12T23:50:26Z- AHRS_M2 msg: 1613173826.49 epoch sec; seqNo:7273 2021-02-12T23:50:26Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.22 psi 2021-02-12T23:50:26Z- handleDepth()-Depth_Keller msg: 1613173826491 epoch sec; seqNo:7333; depth 25.49 2021-02-12T23:50:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173826.50 epoch sec; seqNo:7333; 4057975.20 north; 591598.52 east 2021-02-12T23:50:26Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-12T23:50:26Z- waiting for 1613173824.26, time is 1613173826.50 2021-02-12T23:50:26Z- LcmTrn::updateTrn() >>>> pose time 1613173826.50, meas time 1613173826.09 <<<< 2021-02-12T23:50:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173826.09 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:50:26Z- TerrainNav::motionUpdate() - currEstimate = (4057975, 591598.52, 25.49) 2021-02-12T23:50:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:50:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:50:26Z- TNavBF::Projecting Measurements 2021-02-12T23:50:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:50:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:50:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:50:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:50:26Z- TNavFilter::4 good beams. 2021-02-12T23:50:26Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:50:26Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:50:26Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:50:26Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:50:26Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:50:26Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:50:26Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:50:26Z- TNavBF::mmse selected from filter -1 2021-02-12T23:50:26Z- TerrainMapOctree: (vehN, vehE ) = (4057974.93, 591598.33). 2021-02-12T23:50:26Z- TerrainMapOctree: Min Distance = 4213.96. 2021-02-12T23:50:26Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:50:26Z- There is no specific good beam that all particles have in common. 2021-02-12T23:50:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:50:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.108 ExpectedMeasDiff: nan currMeas.covariance: 7.90 2021-02-12T23:50:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:50:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-12T23:50:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:50:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 6.59 2021-02-12T23:50:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:50:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 16.93 2021-02-12T23:50:26Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:50:26Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:50:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:50:26Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:50:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:50:26Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:50:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:50:26Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:50:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:50:26Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:50:26Z- Filter 0 No beams used in subcloud update 2021-02-12T23:50:26Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:50:26Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:50:26Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 733.6 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:50:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173826.09 sec. 2021-02-12T23:50:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:50:26Z- TNavBF::mmse selected from filter -1 2021-02-12T23:50:26Z- variances([0], [2], and psi_berg) = 733.576880 833.019231 0.000000 2021-02-12T23:50:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:50:26Z- LcmTrn::publishEstimates() - MLE : 4057967.70 591554.02 25.49 2021-02-12T23:50:26Z- LcmTrn::publishEstimates() - MMSE : 4057975.20 591598.52 25.49 2021-02-12T23:50:26Z- LcmTrn::publishEstimates() - COVAR: 733.58 833.02 0.85 0.00 2021-02-12T23:50:26Z- AHRS_M2 msg: 1613173826.88 epoch sec; seqNo:7274 2021-02-12T23:50:26Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.21 psi 2021-02-12T23:50:26Z- handleDepth()-Depth_Keller msg: 1613173826885 epoch sec; seqNo:7334; depth 25.63 2021-02-12T23:50:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173826.89 epoch sec; seqNo:7334; 4057975.22 north; 591599.74 east 2021-02-12T23:50:27Z- AHRS_M2 msg: 1613173827.28 epoch sec; seqNo:7275 2021-02-12T23:50:27Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.21 psi 2021-02-12T23:50:27Z- handleDepth()-Depth_Keller msg: 1613173827293 epoch sec; seqNo:7335; depth 25.76 2021-02-12T23:50:27Z- handleDvl() - RDI_Pathfinder msg: 1613173827.31 epoch sec; seqNo:3388 2021-02-12T23:50:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 98.32 , 64.97 , 103.19 2021-02-12T23:50:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , -0.02 2021-02-12T23:50:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173827.32 epoch sec; seqNo:7335; 4057975.59 north; 591599.73 east 2021-02-12T23:50:27Z- AHRS_M2 msg: 1613173827.69 epoch sec; seqNo:7276 2021-02-12T23:50:27Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.20 psi 2021-02-12T23:50:27Z- handleDepth()-Depth_Keller msg: 1613173827697 epoch sec; seqNo:7336; depth 25.89 2021-02-12T23:50:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173827.71 epoch sec; seqNo:7336; 4057975.97 north; 591599.73 east 2021-02-12T23:50:28Z- AHRS_M2 msg: 1613173828.09 epoch sec; seqNo:7277 2021-02-12T23:50:28Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.20 psi 2021-02-12T23:50:28Z- handleDepth()-Depth_Keller msg: 1613173828097 epoch sec; seqNo:7337; depth 26.02 2021-02-12T23:50:28Z- handleDvl() - RDI_Pathfinder msg: 1613173828.12 epoch sec; seqNo:3389 2021-02-12T23:50:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.79 , 98.25 , 64.97 , 101.90 2021-02-12T23:50:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.10 , 0.01 2021-02-12T23:50:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173828.13 epoch sec; seqNo:7337; 4057976.35 north; 591599.73 east 2021-02-12T23:50:28Z- AHRS_M2 msg: 1613173828.50 epoch sec; seqNo:7278 2021-02-12T23:50:28Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.20 psi 2021-02-12T23:50:28Z- handleDepth()-Depth_Keller msg: 1613173828511 epoch sec; seqNo:7338; depth 26.16 2021-02-12T23:50:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173828.52 epoch sec; seqNo:7338; 4057976.73 north; 591599.72 east 2021-02-12T23:50:28Z- AHRS_M2 msg: 1613173828.92 epoch sec; seqNo:7279 2021-02-12T23:50:28Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.20 psi 2021-02-12T23:50:28Z- handleDepth()-Depth_Keller msg: 1613173828994 epoch sec; seqNo:7339; depth 26.29 2021-02-12T23:50:29Z- handleDvl() - RDI_Pathfinder msg: 1613173829.01 epoch sec; seqNo:3390 2021-02-12T23:50:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 0.00 , 64.97 , 100.55 2021-02-12T23:50:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , 0.02 2021-02-12T23:50:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173829.02 epoch sec; seqNo:7339; 4057976.73 north; 591599.72 east 2021-02-12T23:50:29Z- AHRS_M2 msg: 1613173829.30 epoch sec; seqNo:7280 2021-02-12T23:50:29Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.20 psi 2021-02-12T23:50:29Z- handleDepth()-Depth_Keller msg: 1613173829398 epoch sec; seqNo:7340; depth 26.43 2021-02-12T23:50:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173829.41 epoch sec; seqNo:7340; 4057977.11 north; 591599.72 east 2021-02-12T23:50:29Z- AHRS_M2 msg: 1613173829.71 epoch sec; seqNo:7281 2021-02-12T23:50:29Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.20 psi 2021-02-12T23:50:29Z- handleDepth()-Depth_Keller msg: 1613173829763 epoch sec; seqNo:7341; depth 26.56 2021-02-12T23:50:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173829.77 epoch sec; seqNo:7341; 4057977.49 north; 591599.71 east 2021-02-12T23:50:30Z- AHRS_M2 msg: 1613173830.11 epoch sec; seqNo:7282 2021-02-12T23:50:30Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.20 psi 2021-02-12T23:50:30Z- handleDepth()-Depth_Keller msg: 1613173830153 epoch sec; seqNo:7342; depth 26.68 2021-02-12T23:50:30Z- handleDvl() - RDI_Pathfinder msg: 1613173830.17 epoch sec; seqNo:3391 2021-02-12T23:50:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 88.25 , 63.34 , 101.23 2021-02-12T23:50:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , 0.02 2021-02-12T23:50:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173830.18 epoch sec; seqNo:7342; 4057977.88 north; 591600.93 east 2021-02-12T23:50:30Z- AHRS_M2 msg: 1613173830.52 epoch sec; seqNo:7283 2021-02-12T23:50:30Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.20 psi 2021-02-12T23:50:30Z- handleDepth()-Depth_Keller msg: 1613173830541 epoch sec; seqNo:7343; depth 26.80 2021-02-12T23:50:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173830.55 epoch sec; seqNo:7343; 4057977.88 north; 591600.93 east 2021-02-12T23:50:30Z- AHRS_M2 msg: 1613173830.94 epoch sec; seqNo:7284 2021-02-12T23:50:30Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.20 psi 2021-02-12T23:50:30Z- handleDepth()-Depth_Keller msg: 1613173830956 epoch sec; seqNo:7344; depth 26.92 2021-02-12T23:50:30Z- handleDvl() - RDI_Pathfinder msg: 1613173830.97 epoch sec; seqNo:3392 2021-02-12T23:50:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 0.00 , 62.93 , 100.08 2021-02-12T23:50:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.00 , -0.01 2021-02-12T23:50:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173830.98 epoch sec; seqNo:7344; 4057978.26 north; 591600.93 east 2021-02-12T23:50:31Z- AHRS_M2 msg: 1613173831.32 epoch sec; seqNo:7285 2021-02-12T23:50:31Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.19 psi 2021-02-12T23:50:31Z- handleDepth()-Depth_Keller msg: 1613173831328 epoch sec; seqNo:7345; depth 27.06 2021-02-12T23:50:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173831.34 epoch sec; seqNo:7345; 4057978.64 north; 591600.92 east 2021-02-12T23:50:31Z- AHRS_M2 msg: 1613173831.73 epoch sec; seqNo:7286 2021-02-12T23:50:31Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.19 psi 2021-02-12T23:50:31Z- handleDepth()-Depth_Keller msg: 1613173831738 epoch sec; seqNo:7346; depth 27.18 2021-02-12T23:50:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173831.75 epoch sec; seqNo:7346; 4057978.64 north; 591600.92 east 2021-02-12T23:50:31Z- LcmTrn::handleMessages() - handling 44 messages took 0.91 ms and 126 calls... 2021-02-12T23:50:31Z- waiting for 1613173829.50, time is 1613173831.75 2021-02-12T23:50:31Z- LcmTrn::updateTrn() >>>> pose time 1613173831.75, meas time 1613173830.97 <<<< 2021-02-12T23:50:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173830.97 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:50:31Z- TerrainNav::motionUpdate() - currEstimate = (4057979, 591600.92, 27.18) 2021-02-12T23:50:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:50:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:50:31Z- TNavBF::Projecting Measurements 2021-02-12T23:50:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:50:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:50:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:50:31Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:50:31Z- TNavFilter::3 good beams. 2021-02-12T23:50:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:50:31Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:50:31Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:50:31Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:50:31Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:50:31Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:50:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:50:31Z- TNavBF::mmse selected from filter -1 2021-02-12T23:50:31Z- TerrainMapOctree: (vehN, vehE ) = (4057978.13, 591600.57). 2021-02-12T23:50:31Z- TerrainMapOctree: Min Distance = 4210.06. 2021-02-12T23:50:31Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:50:31Z- There is no specific good beam that all particles have in common. 2021-02-12T23:50:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:50:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 7.72 2021-02-12T23:50:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:50:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 6.34 2021-02-12T23:50:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:50:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 16.03 2021-02-12T23:50:31Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:50:31Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:50:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:50:31Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:50:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:50:31Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:50:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:50:31Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:50:31Z- Filter 0 No beams used in subcloud update 2021-02-12T23:50:31Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:50:31Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:50:31Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 733.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:50:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173830.97 sec. 2021-02-12T23:50:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:50:31Z- TNavBF::mmse selected from filter -1 2021-02-12T23:50:31Z- variances([0], [2], and psi_berg) = 733.470979 833.019150 0.000000 2021-02-12T23:50:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:50:31Z- LcmTrn::publishEstimates() - MLE : 4057971.14 591556.42 27.18 2021-02-12T23:50:31Z- LcmTrn::publishEstimates() - MMSE : 4057978.64 591600.92 27.18 2021-02-12T23:50:31Z- LcmTrn::publishEstimates() - COVAR: 733.47 833.02 0.83 0.00 2021-02-12T23:50:32Z- handleDepth()-Depth_Keller msg: 1613173832162 epoch sec; seqNo:7347; depth 27.30 2021-02-12T23:50:32Z- handleDvl() - RDI_Pathfinder msg: 1613173832.17 epoch sec; seqNo:3393 2021-02-12T23:50:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 0.00 , 62.52 , 100.15 2021-02-12T23:50:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , -0.04 2021-02-12T23:50:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173832.18 epoch sec; seqNo:7347; 4057979.02 north; 591600.92 east 2021-02-12T23:50:32Z- AHRS_M2 msg: 1613173832.54 epoch sec; seqNo:7287 2021-02-12T23:50:32Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.18 psi 2021-02-12T23:50:32Z- handleDepth()-Depth_Keller msg: 1613173832541 epoch sec; seqNo:7348; depth 27.44 2021-02-12T23:50:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173832.55 epoch sec; seqNo:7348; 4057979.41 north; 591602.14 east 2021-02-12T23:50:32Z- AHRS_M2 msg: 1613173832.94 epoch sec; seqNo:7288 2021-02-12T23:50:32Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.18 psi 2021-02-12T23:50:32Z- handleDepth()-Depth_Keller msg: 1613173832947 epoch sec; seqNo:7349; depth 27.55 2021-02-12T23:50:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:50:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:50:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:50:32Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:50:32Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:50:32Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:50:32Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:50:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:50:32Z- handleDvl() - RDI_Pathfinder msg: 1613173832.95 epoch sec; seqNo:3394 2021-02-12T23:50:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:50:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , -0.04 2021-02-12T23:50:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173832.96 epoch sec; seqNo:7349; 4057979.79 north; 591602.13 east 2021-02-12T23:50:33Z- AHRS_M2 msg: 1613173833.34 epoch sec; seqNo:7289 2021-02-12T23:50:33Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.18 psi 2021-02-12T23:50:33Z- handleDepth()-Depth_Keller msg: 1613173833348 epoch sec; seqNo:7350; depth 27.69 2021-02-12T23:50:33Z- handleDvl() - RDI_Pathfinder msg: 1613173833.37 epoch sec; seqNo:3395 2021-02-12T23:50:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 86.09 , 60.48 , 97.98 2021-02-12T23:50:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , -0.08 2021-02-12T23:50:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173833.38 epoch sec; seqNo:7350; 4057980.17 north; 591602.13 east 2021-02-12T23:50:33Z- AHRS_M2 msg: 1613173833.75 epoch sec; seqNo:7290 2021-02-12T23:50:33Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-12T23:50:33Z- handleDepth()-Depth_Keller msg: 1613173833757 epoch sec; seqNo:7351; depth 27.82 2021-02-12T23:50:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173833.77 epoch sec; seqNo:7351; 4057980.17 north; 591602.13 east 2021-02-12T23:50:34Z- AHRS_M2 msg: 1613173834.15 epoch sec; seqNo:7291 2021-02-12T23:50:34Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-12T23:50:34Z- handleDepth()-Depth_Keller msg: 1613173834159 epoch sec; seqNo:7352; depth 27.95 2021-02-12T23:50:34Z- handleDvl() - RDI_Pathfinder msg: 1613173834.17 epoch sec; seqNo:3396 2021-02-12T23:50:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 84.94 , 60.48 , 97.92 2021-02-12T23:50:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , -0.07 2021-02-12T23:50:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173834.18 epoch sec; seqNo:7352; 4057980.55 north; 591602.12 east 2021-02-12T23:50:34Z- AHRS_M2 msg: 1613173834.56 epoch sec; seqNo:7292 2021-02-12T23:50:34Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-12T23:50:34Z- handleDepth()-Depth_Keller msg: 1613173834562 epoch sec; seqNo:7353; depth 28.09 2021-02-12T23:50:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173834.57 epoch sec; seqNo:7353; 4057980.92 north; 591602.12 east 2021-02-12T23:50:34Z- AHRS_M2 msg: 1613173834.96 epoch sec; seqNo:7293 2021-02-12T23:50:34Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.17 psi 2021-02-12T23:50:34Z- handleDepth()-Depth_Keller msg: 1613173834974 epoch sec; seqNo:7354; depth 28.19 2021-02-12T23:50:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173834.99 epoch sec; seqNo:7354; 4057981.30 north; 591602.12 east 2021-02-12T23:50:35Z- AHRS_M2 msg: 1613173835.36 epoch sec; seqNo:7294 2021-02-12T23:50:35Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.17 psi 2021-02-12T23:50:35Z- handleDepth()-Depth_Keller msg: 1613173835369 epoch sec; seqNo:7355; depth 28.34 2021-02-12T23:50:35Z- handleDvl() - RDI_Pathfinder msg: 1613173835.38 epoch sec; seqNo:3397 2021-02-12T23:50:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 85.00 , 60.48 , 97.98 2021-02-12T23:50:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.11 , -0.03 2021-02-12T23:50:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173835.40 epoch sec; seqNo:7355; 4057981.70 north; 591603.33 east 2021-02-12T23:50:35Z- AHRS_M2 msg: 1613173835.77 epoch sec; seqNo:7295 2021-02-12T23:50:35Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.17 psi 2021-02-12T23:50:35Z- handleDepth()-Depth_Keller msg: 1613173835773 epoch sec; seqNo:7356; depth 28.46 2021-02-12T23:50:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173835.79 epoch sec; seqNo:7356; 4057981.70 north; 591603.33 east 2021-02-12T23:50:36Z- AHRS_M2 msg: 1613173836.17 epoch sec; seqNo:7296 2021-02-12T23:50:36Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.16 psi 2021-02-12T23:50:36Z- handleDepth()-Depth_Keller msg: 1613173836179 epoch sec; seqNo:7357; depth 28.60 2021-02-12T23:50:36Z- handleDvl() - RDI_Pathfinder msg: 1613173836.19 epoch sec; seqNo:3398 2021-02-12T23:50:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 83.92 , 61.30 , 97.98 2021-02-12T23:50:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.10 , 0.03 2021-02-12T23:50:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173836.20 epoch sec; seqNo:7357; 4057982.07 north; 591603.33 east 2021-02-12T23:50:36Z- AHRS_M2 msg: 1613173836.58 epoch sec; seqNo:7297 2021-02-12T23:50:36Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.15 psi 2021-02-12T23:50:36Z- handleDepth()-Depth_Keller msg: 1613173836583 epoch sec; seqNo:7358; depth 28.71 2021-02-12T23:50:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:50:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:50:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:50:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:50:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:50:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:50:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:50:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:50:36Z- handleDvl() - RDI_Pathfinder msg: 1613173836.59 epoch sec; seqNo:3399 2021-02-12T23:50:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:50:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.10 , 0.03 2021-02-12T23:50:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173836.60 epoch sec; seqNo:7358; 4057982.45 north; 591603.32 east 2021-02-12T23:50:36Z- LcmTrn::handleMessages() - handling 42 messages took 0.63 ms and 130 calls... 2021-02-12T23:50:36Z- waiting for 1613173834.75, time is 1613173836.60 2021-02-12T23:50:36Z- LcmTrn::updateTrn() equating pose 1613173836.60 and meas times 1613173836.60 2021-02-12T23:50:36Z- LcmTrn::updateTrn() >>>> pose time 1613173836.60, meas time 1613173836.60 <<<< 2021-02-12T23:50:36Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173836.60, ping # 0. 2021-02-12T23:50:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173836.60 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:50:36Z- TerrainNav::motionUpdate() - currEstimate = (4057982, 591603.32, 28.71) 2021-02-12T23:50:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:50:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:50:36Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:50:36Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:50:36Z- TNavBF::Projecting Measurements 2021-02-12T23:50:36Z- TNavFilter::Measurement from time = 1613173836.60 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:50:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:50:36Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:50:36Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:50:36Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:50:36Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:50:36Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:50:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:50:36Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 733.4 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:50:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:50:36Z- TNavBF::mmse selected from filter -1 2021-02-12T23:50:36Z- variances([0], [2], and psi_berg) = 733.387137 833.019087 0.000000 2021-02-12T23:50:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:50:36Z- LcmTrn::publishEstimates() - MLE : 4057974.95 591558.82 28.71 2021-02-12T23:50:36Z- LcmTrn::publishEstimates() - MMSE : 4057982.45 591603.32 28.71 2021-02-12T23:50:36Z- LcmTrn::publishEstimates() - COVAR: 733.39 833.02 1.06 0.00 2021-02-12T23:50:36Z- AHRS_M2 msg: 1613173836.98 epoch sec; seqNo:7298 2021-02-12T23:50:36Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.15 psi 2021-02-12T23:50:36Z- handleDepth()-Depth_Keller msg: 1613173836993 epoch sec; seqNo:7359; depth 28.83 2021-02-12T23:50:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173837.03 epoch sec; seqNo:7359; 4057982.83 north; 591603.32 east 2021-02-12T23:50:37Z- AHRS_M2 msg: 1613173837.38 epoch sec; seqNo:7299 2021-02-12T23:50:37Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.14 psi 2021-02-12T23:50:37Z- handleDepth()-Depth_Keller msg: 1613173837389 epoch sec; seqNo:7360; depth 28.95 2021-02-12T23:50:37Z- handleDvl() - RDI_Pathfinder msg: 1613173837.40 epoch sec; seqNo:3400 2021-02-12T23:50:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 82.84 , 60.89 , 97.98 2021-02-12T23:50:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , 0.07 2021-02-12T23:50:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173837.41 epoch sec; seqNo:7360; 4057982.83 north; 591603.32 east 2021-02-12T23:50:37Z- AHRS_M2 msg: 1613173837.79 epoch sec; seqNo:7300 2021-02-12T23:50:37Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.14 psi 2021-02-12T23:50:37Z- handleDepth()-Depth_Keller msg: 1613173837801 epoch sec; seqNo:7361; depth 29.08 2021-02-12T23:50:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:50:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:50:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:50:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:50:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:50:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:50:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:50:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:50:37Z- handleDvl() - RDI_Pathfinder msg: 1613173837.80 epoch sec; seqNo:3401 2021-02-12T23:50:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:50:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , 0.07 2021-02-12T23:50:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173837.81 epoch sec; seqNo:7361; 4057983.21 north; 591603.32 east 2021-02-12T23:50:38Z- AHRS_M2 msg: 1613173838.20 epoch sec; seqNo:7301 2021-02-12T23:50:38Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.14 psi 2021-02-12T23:50:38Z- handleDepth()-Depth_Keller msg: 1613173838211 epoch sec; seqNo:7362; depth 29.20 2021-02-12T23:50:38Z- handleDvl() - RDI_Pathfinder msg: 1613173838.23 epoch sec; seqNo:3402 2021-02-12T23:50:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 81.69 , 60.07 , 97.92 2021-02-12T23:50:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , 0.07 2021-02-12T23:50:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173838.24 epoch sec; seqNo:7362; 4057983.59 north; 591603.31 east 2021-02-12T23:50:38Z- AHRS_M2 msg: 1613173838.60 epoch sec; seqNo:7302 2021-02-12T23:50:38Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.14 psi 2021-02-12T23:50:38Z- handleDepth()-Depth_Keller msg: 1613173838601 epoch sec; seqNo:7363; depth 29.34 2021-02-12T23:50:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173838.62 epoch sec; seqNo:7363; 4057983.60 north; 591604.53 east 2021-02-12T23:50:39Z- AHRS_M2 msg: 1613173839.00 epoch sec; seqNo:7303 2021-02-12T23:50:39Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.15 psi 2021-02-12T23:50:39Z- handleDepth()-Depth_Keller msg: 1613173839010 epoch sec; seqNo:7364; depth 29.47 2021-02-12T23:50:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:50:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:50:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:50:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:50:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:50:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:50:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:50:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:50:39Z- handleDvl() - RDI_Pathfinder msg: 1613173839.01 epoch sec; seqNo:3403 2021-02-12T23:50:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:50:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , 0.07 2021-02-12T23:50:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173839.02 epoch sec; seqNo:7364; 4057983.98 north; 591604.53 east 2021-02-12T23:50:39Z- AHRS_M2 msg: 1613173839.41 epoch sec; seqNo:7304 2021-02-12T23:50:39Z- AHRS_M2 msg: 0.64 phi; -0.24 theta; -0.16 psi 2021-02-12T23:50:39Z- handleDepth()-Depth_Keller msg: 1613173839416 epoch sec; seqNo:7365; depth 29.60 2021-02-12T23:50:39Z- handleDvl() - RDI_Pathfinder msg: 1613173839.43 epoch sec; seqNo:3404 2021-02-12T23:50:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 80.68 , 59.25 , 97.98 2021-02-12T23:50:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.14 , 0.02 2021-02-12T23:50:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173839.44 epoch sec; seqNo:7365; 4057984.36 north; 591604.52 east 2021-02-12T23:50:39Z- AHRS_M2 msg: 1613173839.81 epoch sec; seqNo:7305 2021-02-12T23:50:39Z- AHRS_M2 msg: 0.64 phi; -0.20 theta; -0.15 psi 2021-02-12T23:50:39Z- handleDepth()-Depth_Keller msg: 1613173839813 epoch sec; seqNo:7366; depth 29.75 2021-02-12T23:50:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173839.82 epoch sec; seqNo:7366; 4057984.36 north; 591604.52 east 2021-02-12T23:50:40Z- AHRS_M2 msg: 1613173840.21 epoch sec; seqNo:7306 2021-02-12T23:50:40Z- AHRS_M2 msg: 0.63 phi; -0.16 theta; -0.15 psi 2021-02-12T23:50:40Z- handleDepth()-Depth_Keller msg: 1613173840219 epoch sec; seqNo:7367; depth 29.87 2021-02-12T23:50:40Z- handleDvl() - RDI_Pathfinder msg: 1613173840.23 epoch sec; seqNo:3405 2021-02-12T23:50:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.61 , 0.00 , 62.12 , 108.80 2021-02-12T23:50:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.32 , 0.64 , -0.30 2021-02-12T23:50:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173840.26 epoch sec; seqNo:7367; 4057984.36 north; 591604.52 east 2021-02-12T23:50:40Z- AHRS_M2 msg: 1613173840.62 epoch sec; seqNo:7307 2021-02-12T23:50:40Z- AHRS_M2 msg: 0.63 phi; -0.12 theta; -0.15 psi 2021-02-12T23:50:40Z- handleDepth()-Depth_Keller msg: 1613173840621 epoch sec; seqNo:7368; depth 29.98 2021-02-12T23:50:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173840.63 epoch sec; seqNo:7368; 4057984.37 north; 591605.75 east 2021-02-12T23:50:41Z- AHRS_M2 msg: 1613173841.03 epoch sec; seqNo:7308 2021-02-12T23:50:41Z- AHRS_M2 msg: 0.62 phi; -0.07 theta; -0.15 psi 2021-02-12T23:50:41Z- handleDepth()-Depth_Keller msg: 1613173841037 epoch sec; seqNo:7369; depth 30.10 2021-02-12T23:50:41Z- handleDvl() - RDI_Pathfinder msg: 1613173841.05 epoch sec; seqNo:3406 2021-02-12T23:50:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 142.33 , 0.00 , 63.75 , 78.51 2021-02-12T23:50:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 1.07 , -0.01 2021-02-12T23:50:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173841.06 epoch sec; seqNo:7369; 4057984.75 north; 591605.74 east 2021-02-12T23:50:41Z- AHRS_M2 msg: 1613173841.42 epoch sec; seqNo:7309 2021-02-12T23:50:41Z- AHRS_M2 msg: 0.61 phi; -0.03 theta; -0.15 psi 2021-02-12T23:50:41Z- handleDepth()-Depth_Keller msg: 1613173841429 epoch sec; seqNo:7370; depth 30.19 2021-02-12T23:50:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:50:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:50:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:50:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:50:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:50:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:50:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:50:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:50:41Z- handleDvl() - RDI_Pathfinder msg: 1613173841.43 epoch sec; seqNo:3407 2021-02-12T23:50:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:50:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 1.07 , -0.01 2021-02-12T23:50:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173841.45 epoch sec; seqNo:7370; 4057984.75 north; 591605.74 east 2021-02-12T23:50:41Z- AHRS_M2 msg: 1613173841.83 epoch sec; seqNo:7310 2021-02-12T23:50:41Z- AHRS_M2 msg: 0.61 phi; 0.01 theta; -0.15 psi 2021-02-12T23:50:41Z- handleDepth()-Depth_Keller msg: 1613173841833 epoch sec; seqNo:7371; depth 30.26 2021-02-12T23:50:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173841.84 epoch sec; seqNo:7371; 4057984.77 north; 591606.96 east 2021-02-12T23:50:41Z- LcmTrn::handleMessages() - handling 47 messages took 0.72 ms and 133 calls... 2021-02-12T23:50:41Z- waiting for 1613173839.60, time is 1613173841.84 2021-02-12T23:50:41Z- LcmTrn::updateTrn() >>>> pose time 1613173841.84, meas time 1613173841.43 <<<< 2021-02-12T23:50:41Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173841.43, ping # 0. 2021-02-12T23:50:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173841.43 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:50:41Z- TerrainNav::motionUpdate() - currEstimate = (4057985, 591606.96, 30.26) 2021-02-12T23:50:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:50:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:50:41Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:50:41Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:50:41Z- TNavBF::Projecting Measurements 2021-02-12T23:50:41Z- TNavFilter::Measurement from time = 1613173841.43 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:50:41Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:50:41Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:50:41Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:50:41Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:50:41Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:50:41Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:50:41Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:50:41Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 733.3 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-12T23:50:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:50:41Z- TNavBF::mmse selected from filter -1 2021-02-12T23:50:41Z- variances([0], [2], and psi_berg) = 733.285586 833.019010 0.000000 2021-02-12T23:50:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:50:41Z- LcmTrn::publishEstimates() - MLE : 4057977.27 591562.46 30.26 2021-02-12T23:50:41Z- LcmTrn::publishEstimates() - MMSE : 4057984.77 591606.96 30.26 2021-02-12T23:50:41Z- LcmTrn::publishEstimates() - COVAR: 733.29 833.02 1.28 0.00 2021-02-12T23:50:42Z- AHRS_M2 msg: 1613173842.24 epoch sec; seqNo:7311 2021-02-12T23:50:42Z- AHRS_M2 msg: 0.60 phi; 0.05 theta; -0.16 psi 2021-02-12T23:50:42Z- handleDepth()-Depth_Keller msg: 1613173842244 epoch sec; seqNo:7372; depth 30.34 2021-02-12T23:50:42Z- handleDvl() - RDI_Pathfinder msg: 1613173842.26 epoch sec; seqNo:3408 2021-02-12T23:50:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.75 , 78.51 , 67.43 , 78.51 2021-02-12T23:50:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , -0.04 2021-02-12T23:50:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173842.27 epoch sec; seqNo:7372; 4057985.14 north; 591606.96 east 2021-02-12T23:50:42Z- AHRS_M2 msg: 1613173842.64 epoch sec; seqNo:7312 2021-02-12T23:50:42Z- AHRS_M2 msg: 0.59 phi; 0.08 theta; -0.16 psi 2021-02-12T23:50:42Z- handleDepth()-Depth_Keller msg: 1613173842705 epoch sec; seqNo:7373; depth 30.37 2021-02-12T23:50:42Z- handleDvl() - RDI_Pathfinder msg: 1613173842.72 epoch sec; seqNo:3409 2021-02-12T23:50:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.71 , 0.00 , 0.00 , 0.00 2021-02-12T23:50:42Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:50:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173842.73 epoch sec; seqNo:7373; 4057985.52 north; 591606.95 east 2021-02-12T23:50:43Z- AHRS_M2 msg: 1613173843.04 epoch sec; seqNo:7313 2021-02-12T23:50:43Z- AHRS_M2 msg: 0.58 phi; 0.12 theta; -0.16 psi 2021-02-12T23:50:43Z- handleDepth()-Depth_Keller msg: 1613173843146 epoch sec; seqNo:7374; depth 30.41 2021-02-12T23:50:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173843.16 epoch sec; seqNo:7374; 4057985.52 north; 591606.95 east 2021-02-12T23:50:43Z- AHRS_M2 msg: 1613173843.46 epoch sec; seqNo:7314 2021-02-12T23:50:43Z- AHRS_M2 msg: 0.58 phi; 0.15 theta; -0.17 psi 2021-02-12T23:50:43Z- handleDepth()-Depth_Keller msg: 1613173843491 epoch sec; seqNo:7375; depth 30.41 2021-02-12T23:50:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173843.50 epoch sec; seqNo:7375; 4057985.91 north; 591608.17 east 2021-02-12T23:50:43Z- AHRS_M2 msg: 1613173843.86 epoch sec; seqNo:7315 2021-02-12T23:50:43Z- AHRS_M2 msg: 0.57 phi; 0.17 theta; -0.17 psi 2021-02-12T23:50:43Z- handleDepth()-Depth_Keller msg: 1613173843898 epoch sec; seqNo:7376; depth 30.44 2021-02-12T23:50:43Z- handleDvl() - RDI_Pathfinder msg: 1613173843.91 epoch sec; seqNo:3410 2021-02-12T23:50:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.78 , 0.00 , 0.00 , 63.78 2021-02-12T23:50:43Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:50:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173843.92 epoch sec; seqNo:7376; 4057986.29 north; 591608.17 east 2021-02-12T23:50:44Z- AHRS_M2 msg: 1613173844.27 epoch sec; seqNo:7316 2021-02-12T23:50:44Z- AHRS_M2 msg: 0.57 phi; 0.20 theta; -0.17 psi 2021-02-12T23:50:44Z- handleDepth()-Depth_Keller msg: 1613173844289 epoch sec; seqNo:7377; depth 30.42 2021-02-12T23:50:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173844.30 epoch sec; seqNo:7377; 4057986.67 north; 591608.16 east 2021-02-12T23:50:44Z- AHRS_M2 msg: 1613173844.69 epoch sec; seqNo:7317 2021-02-12T23:50:44Z- AHRS_M2 msg: 0.56 phi; 0.22 theta; -0.17 psi 2021-02-12T23:50:44Z- handleDepth()-Depth_Keller msg: 1613173844703 epoch sec; seqNo:7378; depth 30.41 2021-02-12T23:50:44Z- handleDvl() - RDI_Pathfinder msg: 1613173844.71 epoch sec; seqNo:3411 2021-02-12T23:50:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 65.11 , 0.00 , 0.00 , 63.33 2021-02-12T23:50:44Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:50:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173844.73 epoch sec; seqNo:7378; 4057986.67 north; 591608.16 east 2021-02-12T23:50:45Z- AHRS_M2 msg: 1613173845.07 epoch sec; seqNo:7318 2021-02-12T23:50:45Z- AHRS_M2 msg: 0.56 phi; 0.24 theta; -0.17 psi 2021-02-12T23:50:45Z- handleDepth()-Depth_Keller msg: 1613173845077 epoch sec; seqNo:7379; depth 30.37 2021-02-12T23:50:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173845.09 epoch sec; seqNo:7379; 4057987.05 north; 591608.16 east 2021-02-12T23:50:45Z- AHRS_M2 msg: 1613173845.48 epoch sec; seqNo:7319 2021-02-12T23:50:45Z- AHRS_M2 msg: 0.55 phi; 0.26 theta; -0.17 psi 2021-02-12T23:50:45Z- handleDepth()-Depth_Keller msg: 1613173845481 epoch sec; seqNo:7380; depth 30.33 2021-02-12T23:50:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173845.49 epoch sec; seqNo:7380; 4057987.43 north; 591608.16 east 2021-02-12T23:50:45Z- AHRS_M2 msg: 1613173845.88 epoch sec; seqNo:7320 2021-02-12T23:50:45Z- AHRS_M2 msg: 0.55 phi; 0.28 theta; -0.16 psi 2021-02-12T23:50:45Z- handleDepth()-Depth_Keller msg: 1613173845886 epoch sec; seqNo:7381; depth 30.27 2021-02-12T23:50:45Z- handleDvl() - RDI_Pathfinder msg: 1613173845.90 epoch sec; seqNo:3412 2021-02-12T23:50:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 65.99 , 0.00 , 0.00 , 62.45 2021-02-12T23:50:45Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:50:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173845.91 epoch sec; seqNo:7381; 4057987.81 north; 591608.15 east 2021-02-12T23:50:46Z- AHRS_M2 msg: 1613173846.28 epoch sec; seqNo:7321 2021-02-12T23:50:46Z- AHRS_M2 msg: 0.55 phi; 0.29 theta; -0.16 psi 2021-02-12T23:50:46Z- handleDepth()-Depth_Keller msg: 1613173846289 epoch sec; seqNo:7382; depth 30.22 2021-02-12T23:50:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173846.31 epoch sec; seqNo:7382; 4057987.82 north; 591609.37 east 2021-02-12T23:50:46Z- AHRS_M2 msg: 1613173846.71 epoch sec; seqNo:7322 2021-02-12T23:50:46Z- AHRS_M2 msg: 0.55 phi; 0.31 theta; -0.16 psi 2021-02-12T23:50:46Z- handleDepth()-Depth_Keller msg: 1613173846719 epoch sec; seqNo:7383; depth 30.16 2021-02-12T23:50:46Z- handleDvl() - RDI_Pathfinder msg: 1613173846.73 epoch sec; seqNo:3413 2021-02-12T23:50:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:50:46Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:50:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173846.74 epoch sec; seqNo:7383; 4057988.20 north; 591609.37 east 2021-02-12T23:50:46Z- LcmTrn::handleMessages() - handling 42 messages took 0.78 ms and 127 calls... 2021-02-12T23:50:46Z- waiting for 1613173844.84, time is 1613173846.74 2021-02-12T23:50:46Z- LcmTrn::updateTrn() equating pose 1613173846.74 and meas times 1613173846.74 2021-02-12T23:50:46Z- LcmTrn::updateTrn() >>>> pose time 1613173846.74, meas time 1613173846.74 <<<< 2021-02-12T23:50:46Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173846.74, ping # 0. 2021-02-12T23:50:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173846.74 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:50:46Z- TerrainNav::motionUpdate() - currEstimate = (4057988, 591609.37, 30.16) 2021-02-12T23:50:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:50:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:50:46Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:50:46Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:50:47Z- TNavBF::Projecting Measurements 2021-02-12T23:50:47Z- TNavFilter::Measurement from time = 1613173846.74 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:50:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:50:47Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:50:47Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:50:47Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:50:47Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:50:47Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:50:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:50:47Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 733.2 EastVariance: 833.0 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-12T23:50:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:50:47Z- TNavBF::mmse selected from filter -1 2021-02-12T23:50:47Z- variances([0], [2], and psi_berg) = 733.204892 833.018949 0.000000 2021-02-12T23:50:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:50:47Z- LcmTrn::publishEstimates() - MLE : 4057980.70 591564.87 30.16 2021-02-12T23:50:47Z- LcmTrn::publishEstimates() - MMSE : 4057988.20 591609.37 30.16 2021-02-12T23:50:47Z- LcmTrn::publishEstimates() - COVAR: 733.20 833.02 1.49 0.00 2021-02-12T23:50:47Z- AHRS_M2 msg: 1613173847.09 epoch sec; seqNo:7323 2021-02-12T23:50:47Z- AHRS_M2 msg: 0.55 phi; 0.32 theta; -0.15 psi 2021-02-12T23:50:47Z- handleDepth()-Depth_Keller msg: 1613173847096 epoch sec; seqNo:7384; depth 30.07 2021-02-12T23:50:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173847.11 epoch sec; seqNo:7384; 4057988.58 north; 591609.36 east 2021-02-12T23:50:47Z- AHRS_M2 msg: 1613173847.50 epoch sec; seqNo:7324 2021-02-12T23:50:47Z- AHRS_M2 msg: 0.55 phi; 0.33 theta; -0.15 psi 2021-02-12T23:50:47Z- handleDepth()-Depth_Keller msg: 1613173847502 epoch sec; seqNo:7385; depth 30.00 2021-02-12T23:50:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173847.51 epoch sec; seqNo:7385; 4057988.96 north; 591609.36 east 2021-02-12T23:50:47Z- AHRS_M2 msg: 1613173847.90 epoch sec; seqNo:7325 2021-02-12T23:50:47Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.14 psi 2021-02-12T23:50:47Z- handleDepth()-Depth_Keller msg: 1613173847914 epoch sec; seqNo:7386; depth 29.90 2021-02-12T23:50:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173847.96 epoch sec; seqNo:7386; 4057988.96 north; 591609.36 east 2021-02-12T23:50:48Z- AHRS_M2 msg: 1613173848.31 epoch sec; seqNo:7326 2021-02-12T23:50:48Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.14 psi 2021-02-12T23:50:48Z- handleDepth()-Depth_Keller msg: 1613173848311 epoch sec; seqNo:7387; depth 29.81 2021-02-12T23:50:48Z- handleDvl() - RDI_Pathfinder msg: 1613173848.32 epoch sec; seqNo:3414 2021-02-12T23:50:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 0.00 , 97.98 , 60.48 2021-02-12T23:50:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.02 , -0.05 2021-02-12T23:50:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173848.33 epoch sec; seqNo:7387; 4057989.34 north; 591609.36 east 2021-02-12T23:50:48Z- AHRS_M2 msg: 1613173848.71 epoch sec; seqNo:7327 2021-02-12T23:50:48Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.14 psi 2021-02-12T23:50:48Z- handleDepth()-Depth_Keller msg: 1613173848715 epoch sec; seqNo:7388; depth 29.74 2021-02-12T23:50:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173848.73 epoch sec; seqNo:7388; 4057989.72 north; 591609.35 east 2021-02-12T23:50:49Z- AHRS_M2 msg: 1613173849.11 epoch sec; seqNo:7328 2021-02-12T23:50:49Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.14 psi 2021-02-12T23:50:49Z- handleDepth()-Depth_Keller msg: 1613173849132 epoch sec; seqNo:7389; depth 29.63 2021-02-12T23:50:49Z- handleDvl() - RDI_Pathfinder msg: 1613173849.15 epoch sec; seqNo:3415 2021-02-12T23:50:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:50:49Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:50:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173849.16 epoch sec; seqNo:7389; 4057989.73 north; 591610.57 east 2021-02-12T23:50:49Z- AHRS_M2 msg: 1613173849.52 epoch sec; seqNo:7329 2021-02-12T23:50:49Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.13 psi 2021-02-12T23:50:49Z- handleDepth()-Depth_Keller msg: 1613173849521 epoch sec; seqNo:7390; depth 29.51 2021-02-12T23:50:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173849.54 epoch sec; seqNo:7390; 4057990.11 north; 591610.57 east 2021-02-12T23:50:49Z- AHRS_M2 msg: 1613173849.92 epoch sec; seqNo:7330 2021-02-12T23:50:49Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.14 psi 2021-02-12T23:50:49Z- handleDepth()-Depth_Keller msg: 1613173849928 epoch sec; seqNo:7391; depth 29.42 2021-02-12T23:50:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173849.94 epoch sec; seqNo:7391; 4057990.49 north; 591610.56 east 2021-02-12T23:50:50Z- AHRS_M2 msg: 1613173850.33 epoch sec; seqNo:7331 2021-02-12T23:50:50Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.14 psi 2021-02-12T23:50:50Z- handleDepth()-Depth_Keller msg: 1613173850331 epoch sec; seqNo:7392; depth 29.31 2021-02-12T23:50:50Z- handleDvl() - RDI_Pathfinder msg: 1613173850.35 epoch sec; seqNo:3416 2021-02-12T23:50:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 83.38 , 97.98 , 61.71 2021-02-12T23:50:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.02 , 0.00 2021-02-12T23:50:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173850.36 epoch sec; seqNo:7392; 4057990.49 north; 591610.56 east 2021-02-12T23:50:50Z- AHRS_M2 msg: 1613173850.73 epoch sec; seqNo:7332 2021-02-12T23:50:50Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.13 psi 2021-02-12T23:50:50Z- handleDepth()-Depth_Keller msg: 1613173850737 epoch sec; seqNo:7393; depth 29.19 2021-02-12T23:50:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173850.75 epoch sec; seqNo:7393; 4057990.87 north; 591610.56 east 2021-02-12T23:50:51Z- AHRS_M2 msg: 1613173851.14 epoch sec; seqNo:7333 2021-02-12T23:50:51Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.13 psi 2021-02-12T23:50:51Z- handleDepth()-Depth_Keller msg: 1613173851142 epoch sec; seqNo:7394; depth 29.08 2021-02-12T23:50:51Z- handleDvl() - RDI_Pathfinder msg: 1613173851.15 epoch sec; seqNo:3417 2021-02-12T23:50:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 0.00 , 97.98 , 60.48 2021-02-12T23:50:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , 0.01 2021-02-12T23:50:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173851.16 epoch sec; seqNo:7394; 4057991.24 north; 591610.56 east 2021-02-12T23:50:51Z- AHRS_M2 msg: 1613173851.54 epoch sec; seqNo:7334 2021-02-12T23:50:51Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.13 psi 2021-02-12T23:50:51Z- handleDepth()-Depth_Keller msg: 1613173851554 epoch sec; seqNo:7395; depth 28.96 2021-02-12T23:50:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173851.57 epoch sec; seqNo:7395; 4057991.24 north; 591610.56 east 2021-02-12T23:50:51Z- AHRS_M2 msg: 1613173851.95 epoch sec; seqNo:7335 2021-02-12T23:50:51Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.13 psi 2021-02-12T23:50:51Z- handleDepth()-Depth_Keller msg: 1613173851960 epoch sec; seqNo:7396; depth 28.85 2021-02-12T23:50:51Z- handleDvl() - RDI_Pathfinder msg: 1613173851.98 epoch sec; seqNo:3418 2021-02-12T23:50:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 0.00 , 97.98 , 62.12 2021-02-12T23:50:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , 0.03 2021-02-12T23:50:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173851.99 epoch sec; seqNo:7396; 4057991.64 north; 591611.77 east 2021-02-12T23:50:52Z- LcmTrn::handleMessages() - handling 44 messages took 0.77 ms and 129 calls... 2021-02-12T23:50:52Z- waiting for 1613173849.74, time is 1613173851.99 2021-02-12T23:50:52Z- LcmTrn::updateTrn() equating pose 1613173851.99 and meas times 1613173851.99 2021-02-12T23:50:52Z- LcmTrn::updateTrn() >>>> pose time 1613173851.99, meas time 1613173851.99 <<<< 2021-02-12T23:50:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173851.99 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:50:52Z- TerrainNav::motionUpdate() - currEstimate = (4057992, 591611.77, 28.85) 2021-02-12T23:50:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:50:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:50:52Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173851.99 sec.; No relevant navigation data available 2021-02-12T23:50:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:50:52Z- TNavBF::mmse selected from filter -1 2021-02-12T23:50:52Z- variances([0], [2], and psi_berg) = 733.124277 833.018887 0.000000 2021-02-12T23:50:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:50:52Z- LcmTrn::publishEstimates() - MLE : 4057984.14 591567.27 28.85 2021-02-12T23:50:52Z- LcmTrn::publishEstimates() - MMSE : 4057991.64 591611.77 28.85 2021-02-12T23:50:52Z- LcmTrn::publishEstimates() - COVAR: 733.12 833.02 1.71 0.00 2021-02-12T23:50:52Z- AHRS_M2 msg: 1613173852.35 epoch sec; seqNo:7336 2021-02-12T23:50:52Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.13 psi 2021-02-12T23:50:52Z- handleDepth()-Depth_Keller msg: 1613173852360 epoch sec; seqNo:7397; depth 28.72 2021-02-12T23:50:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173852.37 epoch sec; seqNo:7397; 4057992.02 north; 591611.77 east 2021-02-12T23:50:52Z- AHRS_M2 msg: 1613173852.75 epoch sec; seqNo:7337 2021-02-12T23:50:52Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.13 psi 2021-02-12T23:50:52Z- handleDepth()-Depth_Keller msg: 1613173852766 epoch sec; seqNo:7398; depth 28.61 2021-02-12T23:50:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173852.77 epoch sec; seqNo:7398; 4057992.39 north; 591611.77 east 2021-02-12T23:50:53Z- AHRS_M2 msg: 1613173853.15 epoch sec; seqNo:7338 2021-02-12T23:50:53Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.12 psi 2021-02-12T23:50:53Z- handleDepth()-Depth_Keller msg: 1613173853158 epoch sec; seqNo:7399; depth 28.51 2021-02-12T23:50:53Z- handleDvl() - RDI_Pathfinder msg: 1613173853.17 epoch sec; seqNo:3419 2021-02-12T23:50:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 82.84 , 97.98 , 62.12 2021-02-12T23:50:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , 0.04 2021-02-12T23:50:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173853.19 epoch sec; seqNo:7399; 4057992.39 north; 591611.77 east 2021-02-12T23:50:53Z- AHRS_M2 msg: 1613173853.56 epoch sec; seqNo:7339 2021-02-12T23:50:53Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.12 psi 2021-02-12T23:50:53Z- handleDepth()-Depth_Keller msg: 1613173853564 epoch sec; seqNo:7400; depth 28.39 2021-02-12T23:50:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173853.58 epoch sec; seqNo:7400; 4057992.77 north; 591611.76 east 2021-02-12T23:50:53Z- AHRS_M2 msg: 1613173853.96 epoch sec; seqNo:7340 2021-02-12T23:50:53Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.11 psi 2021-02-12T23:50:53Z- handleDepth()-Depth_Keller msg: 1613173853966 epoch sec; seqNo:7401; depth 28.27 2021-02-12T23:50:53Z- handleDvl() - RDI_Pathfinder msg: 1613173853.98 epoch sec; seqNo:3420 2021-02-12T23:50:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:50:53Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:50:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173853.99 epoch sec; seqNo:7401; 4057993.15 north; 591611.76 east 2021-02-12T23:50:54Z- AHRS_M2 msg: 1613173854.36 epoch sec; seqNo:7341 2021-02-12T23:50:54Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.11 psi 2021-02-12T23:50:54Z- handleDepth()-Depth_Keller msg: 1613173854369 epoch sec; seqNo:7402; depth 28.15 2021-02-12T23:50:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173854.39 epoch sec; seqNo:7402; 4057993.53 north; 591611.75 east 2021-02-12T23:50:54Z- AHRS_M2 msg: 1613173854.77 epoch sec; seqNo:7342 2021-02-12T23:50:54Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.11 psi 2021-02-12T23:50:54Z- handleDepth()-Depth_Keller msg: 1613173854773 epoch sec; seqNo:7403; depth 28.03 2021-02-12T23:50:54Z- handleDvl() - RDI_Pathfinder msg: 1613173854.79 epoch sec; seqNo:3421 2021-02-12T23:50:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 71.75 , 0.00 , 0.00 , 62.09 2021-02-12T23:50:54Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:50:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173854.82 epoch sec; seqNo:7403; 4057993.54 north; 591612.97 east 2021-02-12T23:50:55Z- AHRS_M2 msg: 1613173855.17 epoch sec; seqNo:7343 2021-02-12T23:50:55Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.10 psi 2021-02-12T23:50:55Z- handleDepth()-Depth_Keller msg: 1613173855181 epoch sec; seqNo:7404; depth 27.91 2021-02-12T23:50:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173855.20 epoch sec; seqNo:7404; 4057993.92 north; 591612.97 east 2021-02-12T23:50:55Z- AHRS_M2 msg: 1613173855.59 epoch sec; seqNo:7344 2021-02-12T23:50:55Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.10 psi 2021-02-12T23:50:55Z- handleDepth()-Depth_Keller msg: 1613173855593 epoch sec; seqNo:7405; depth 27.78 2021-02-12T23:50:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173855.60 epoch sec; seqNo:7405; 4057994.30 north; 591612.97 east 2021-02-12T23:50:55Z- AHRS_M2 msg: 1613173855.98 epoch sec; seqNo:7345 2021-02-12T23:50:55Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.11 psi 2021-02-12T23:50:56Z- handleDepth()-Depth_Keller msg: 1613173856053 epoch sec; seqNo:7406; depth 27.68 2021-02-12T23:50:56Z- handleDvl() - RDI_Pathfinder msg: 1613173856.07 epoch sec; seqNo:3422 2021-02-12T23:50:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 70.83 , 0.00 , 0.00 , 63.47 2021-02-12T23:50:56Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:50:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173856.08 epoch sec; seqNo:7406; 4057994.68 north; 591612.96 east 2021-02-12T23:50:56Z- AHRS_M2 msg: 1613173856.39 epoch sec; seqNo:7346 2021-02-12T23:50:56Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.12 psi 2021-02-12T23:50:56Z- handleDepth()-Depth_Keller msg: 1613173856398 epoch sec; seqNo:7407; depth 27.52 2021-02-12T23:50:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173856.43 epoch sec; seqNo:7407; 4057994.68 north; 591612.96 east 2021-02-12T23:50:56Z- AHRS_M2 msg: 1613173856.79 epoch sec; seqNo:7347 2021-02-12T23:50:56Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.13 psi 2021-02-12T23:50:56Z- handleDepth()-Depth_Keller msg: 1613173856886 epoch sec; seqNo:7408; depth 27.41 2021-02-12T23:50:56Z- handleDvl() - RDI_Pathfinder msg: 1613173856.90 epoch sec; seqNo:3423 2021-02-12T23:50:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 71.75 , 0.00 , 0.00 , 63.47 2021-02-12T23:50:56Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:50:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173856.91 epoch sec; seqNo:7408; 4057995.06 north; 591612.96 east 2021-02-12T23:50:57Z- LcmTrn::handleMessages() - handling 41 messages took 0.65 ms and 128 calls... 2021-02-12T23:50:57Z- waiting for 1613173854.99, time is 1613173856.91 2021-02-12T23:50:57Z- LcmTrn::updateTrn() equating pose 1613173856.91 and meas times 1613173856.91 2021-02-12T23:50:57Z- LcmTrn::updateTrn() >>>> pose time 1613173856.91, meas time 1613173856.91 <<<< 2021-02-12T23:50:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173856.91 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:50:57Z- TerrainNav::motionUpdate() - currEstimate = (4057995, 591612.96, 27.41) 2021-02-12T23:50:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:50:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:50:57Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:50:57Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:50:57Z- TNavBF::Projecting Measurements 2021-02-12T23:50:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:50:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:50:57Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-12T23:50:57Z- TNavFilter::2 good beams. 2021-02-12T23:50:57Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:50:57Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:50:57Z- TNavBF:: beamIndex[1] = 3 2021-02-12T23:50:57Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:50:57Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:50:57Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-12T23:50:57Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:50:57Z- TNavBF::mmse selected from filter -1 2021-02-12T23:50:57Z- TerrainMapOctree: (vehN, vehE ) = (4057995.06, 591612.96). 2021-02-12T23:50:57Z- TerrainMapOctree: Min Distance = 4189.11. 2021-02-12T23:50:57Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:50:57Z- There is no specific good beam that all particles have in common. 2021-02-12T23:50:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:50:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.89 K: 0.160 ExpectedMeasDiff: nan currMeas.covariance: 8.24 2021-02-12T23:50:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:50:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.59 K: 0.163 ExpectedMeasDiff: nan currMeas.covariance: 6.45 2021-02-12T23:50:57Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:50:57Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:50:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:50:57Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:50:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:50:57Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:50:57Z- Filter 0 No beams used in subcloud update 2021-02-12T23:50:57Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:50:57Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:50:57Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 733.0 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-12T23:50:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173856.91 sec. 2021-02-12T23:50:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:50:57Z- TNavBF::mmse selected from filter -1 2021-02-12T23:50:57Z- variances([0], [2], and psi_berg) = 733.049435 833.018831 0.000000 2021-02-12T23:50:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:50:57Z- LcmTrn::publishEstimates() - MLE : 4057987.56 591568.46 27.41 2021-02-12T23:50:57Z- LcmTrn::publishEstimates() - MMSE : 4057995.06 591612.96 27.41 2021-02-12T23:50:57Z- LcmTrn::publishEstimates() - COVAR: 733.05 833.02 1.33 0.00 2021-02-12T23:50:57Z- AHRS_M2 msg: 1613173857.19 epoch sec; seqNo:7348 2021-02-12T23:50:57Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.14 psi 2021-02-12T23:50:57Z- handleDepth()-Depth_Keller msg: 1613173857222 epoch sec; seqNo:7409; depth 27.27 2021-02-12T23:50:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173857.23 epoch sec; seqNo:7409; 4057995.44 north; 591612.95 east 2021-02-12T23:50:57Z- AHRS_M2 msg: 1613173857.60 epoch sec; seqNo:7349 2021-02-12T23:50:57Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.16 psi 2021-02-12T23:50:57Z- handleDepth()-Depth_Keller msg: 1613173857626 epoch sec; seqNo:7410; depth 27.17 2021-02-12T23:50:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173857.63 epoch sec; seqNo:7410; 4057995.45 north; 591614.17 east 2021-02-12T23:50:58Z- AHRS_M2 msg: 1613173858.00 epoch sec; seqNo:7350 2021-02-12T23:50:58Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.17 psi 2021-02-12T23:50:58Z- handleDepth()-Depth_Keller msg: 1613173858025 epoch sec; seqNo:7411; depth 27.04 2021-02-12T23:50:58Z- handleDvl() - RDI_Pathfinder msg: 1613173858.04 epoch sec; seqNo:3424 2021-02-12T23:50:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:50:58Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:50:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173858.05 epoch sec; seqNo:7411; 4057995.83 north; 591614.17 east 2021-02-12T23:50:58Z- AHRS_M2 msg: 1613173858.41 epoch sec; seqNo:7351 2021-02-12T23:50:58Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.18 psi 2021-02-12T23:50:58Z- handleDepth()-Depth_Keller msg: 1613173858421 epoch sec; seqNo:7412; depth 26.92 2021-02-12T23:50:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173858.43 epoch sec; seqNo:7412; 4057996.21 north; 591614.17 east 2021-02-12T23:50:58Z- AHRS_M2 msg: 1613173858.81 epoch sec; seqNo:7352 2021-02-12T23:50:58Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.19 psi 2021-02-12T23:50:58Z- handleDepth()-Depth_Keller msg: 1613173858813 epoch sec; seqNo:7413; depth 26.81 2021-02-12T23:50:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173858.83 epoch sec; seqNo:7413; 4057996.21 north; 591614.17 east 2021-02-12T23:50:59Z- AHRS_M2 msg: 1613173859.21 epoch sec; seqNo:7353 2021-02-12T23:50:59Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.20 psi 2021-02-12T23:50:59Z- handleDepth()-Depth_Keller msg: 1613173859221 epoch sec; seqNo:7414; depth 26.68 2021-02-12T23:50:59Z- handleDvl() - RDI_Pathfinder msg: 1613173859.24 epoch sec; seqNo:3425 2021-02-12T23:50:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 89.33 , 97.98 , 62.93 2021-02-12T23:50:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , 0.03 2021-02-12T23:50:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173859.25 epoch sec; seqNo:7414; 4057996.59 north; 591614.16 east 2021-02-12T23:50:59Z- AHRS_M2 msg: 1613173859.62 epoch sec; seqNo:7354 2021-02-12T23:50:59Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.20 psi 2021-02-12T23:50:59Z- handleDepth()-Depth_Keller msg: 1613173859621 epoch sec; seqNo:7415; depth 26.56 2021-02-12T23:50:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173859.63 epoch sec; seqNo:7415; 4057996.97 north; 591614.16 east 2021-02-12T23:51:00Z- AHRS_M2 msg: 1613173860.02 epoch sec; seqNo:7355 2021-02-12T23:51:00Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.20 psi 2021-02-12T23:51:00Z- handleDepth()-Depth_Keller msg: 1613173860030 epoch sec; seqNo:7416; depth 26.44 2021-02-12T23:51:00Z- handleDvl() - RDI_Pathfinder msg: 1613173860.04 epoch sec; seqNo:3426 2021-02-12T23:51:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 0.00 , 101.16 , 63.75 2021-02-12T23:51:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , -0.02 2021-02-12T23:51:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173860.05 epoch sec; seqNo:7416; 4057997.36 north; 591615.37 east 2021-02-12T23:51:00Z- AHRS_M2 msg: 1613173860.42 epoch sec; seqNo:7356 2021-02-12T23:51:00Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.20 psi 2021-02-12T23:51:00Z- handleDepth()-Depth_Keller msg: 1613173860429 epoch sec; seqNo:7417; depth 26.32 2021-02-12T23:51:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173860.44 epoch sec; seqNo:7417; 4057997.36 north; 591615.37 east 2021-02-12T23:51:00Z- AHRS_M2 msg: 1613173860.83 epoch sec; seqNo:7357 2021-02-12T23:51:00Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.20 psi 2021-02-12T23:51:00Z- handleDepth()-Depth_Keller msg: 1613173860837 epoch sec; seqNo:7418; depth 26.21 2021-02-12T23:51:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173860.85 epoch sec; seqNo:7418; 4057997.74 north; 591615.37 east 2021-02-12T23:51:01Z- AHRS_M2 msg: 1613173861.23 epoch sec; seqNo:7358 2021-02-12T23:51:01Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.19 psi 2021-02-12T23:51:01Z- handleDepth()-Depth_Keller msg: 1613173861239 epoch sec; seqNo:7419; depth 26.07 2021-02-12T23:51:01Z- handleDvl() - RDI_Pathfinder msg: 1613173861.25 epoch sec; seqNo:3427 2021-02-12T23:51:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 90.41 , 101.23 , 65.38 2021-02-12T23:51:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.05 2021-02-12T23:51:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173861.26 epoch sec; seqNo:7419; 4057998.12 north; 591615.37 east 2021-02-12T23:51:01Z- AHRS_M2 msg: 1613173861.64 epoch sec; seqNo:7359 2021-02-12T23:51:01Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.19 psi 2021-02-12T23:51:01Z- handleDepth()-Depth_Keller msg: 1613173861645 epoch sec; seqNo:7420; depth 25.95 2021-02-12T23:51:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173861.67 epoch sec; seqNo:7420; 4057998.12 north; 591615.37 east 2021-02-12T23:51:02Z- AHRS_M2 msg: 1613173862.04 epoch sec; seqNo:7360 2021-02-12T23:51:02Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.18 psi 2021-02-12T23:51:02Z- handleDepth()-Depth_Keller msg: 1613173862046 epoch sec; seqNo:7421; depth 25.82 2021-02-12T23:51:02Z- handleDvl() - RDI_Pathfinder msg: 1613173862.06 epoch sec; seqNo:3428 2021-02-12T23:51:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:51:02Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:51:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173862.07 epoch sec; seqNo:7421; 4057998.49 north; 591615.36 east 2021-02-12T23:51:02Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-12T23:51:02Z- waiting for 1613173859.91, time is 1613173862.07 2021-02-12T23:51:02Z- LcmTrn::updateTrn() equating pose 1613173862.07 and meas times 1613173862.07 2021-02-12T23:51:02Z- LcmTrn::updateTrn() >>>> pose time 1613173862.07, meas time 1613173862.07 <<<< 2021-02-12T23:51:02Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173862.07, ping # 0. 2021-02-12T23:51:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173862.07 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:51:02Z- TerrainNav::motionUpdate() - currEstimate = (4057998, 591615.36, 25.82) 2021-02-12T23:51:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:51:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:51:02Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:51:02Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:51:02Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173862.07 sec.; No relevant navigation data available 2021-02-12T23:51:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:51:02Z- TNavBF::mmse selected from filter -1 2021-02-12T23:51:02Z- variances([0], [2], and psi_berg) = 732.968971 833.018770 0.000000 2021-02-12T23:51:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:51:02Z- LcmTrn::publishEstimates() - MLE : 4057990.99 591570.86 25.82 2021-02-12T23:51:02Z- LcmTrn::publishEstimates() - MMSE : 4057998.49 591615.36 25.82 2021-02-12T23:51:02Z- LcmTrn::publishEstimates() - COVAR: 732.97 833.02 1.55 0.00 2021-02-12T23:51:02Z- AHRS_M2 msg: 1613173862.44 epoch sec; seqNo:7361 2021-02-12T23:51:02Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.18 psi 2021-02-12T23:51:02Z- handleDepth()-Depth_Keller msg: 1613173862453 epoch sec; seqNo:7422; depth 25.69 2021-02-12T23:51:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173862.47 epoch sec; seqNo:7422; 4057998.89 north; 591616.58 east 2021-02-12T23:51:02Z- AHRS_M2 msg: 1613173862.85 epoch sec; seqNo:7362 2021-02-12T23:51:02Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.17 psi 2021-02-12T23:51:02Z- handleDepth()-Depth_Keller msg: 1613173862852 epoch sec; seqNo:7423; depth 25.58 2021-02-12T23:51:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173862.92 epoch sec; seqNo:7423; 4057999.27 north; 591616.58 east 2021-02-12T23:51:03Z- AHRS_M2 msg: 1613173863.25 epoch sec; seqNo:7363 2021-02-12T23:51:03Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.17 psi 2021-02-12T23:51:03Z- handleDepth()-Depth_Keller msg: 1613173863259 epoch sec; seqNo:7424; depth 25.44 2021-02-12T23:51:03Z- handleDvl() - RDI_Pathfinder msg: 1613173863.27 epoch sec; seqNo:3429 2021-02-12T23:51:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 97.11 , 101.16 , 64.97 2021-02-12T23:51:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , -0.03 2021-02-12T23:51:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173863.28 epoch sec; seqNo:7424; 4057999.27 north; 591616.58 east 2021-02-12T23:51:03Z- AHRS_M2 msg: 1613173863.66 epoch sec; seqNo:7364 2021-02-12T23:51:03Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.16 psi 2021-02-12T23:51:03Z- handleDepth()-Depth_Keller msg: 1613173863662 epoch sec; seqNo:7425; depth 25.32 2021-02-12T23:51:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173863.67 epoch sec; seqNo:7425; 4057999.64 north; 591616.57 east 2021-02-12T23:51:04Z- AHRS_M2 msg: 1613173864.06 epoch sec; seqNo:7365 2021-02-12T23:51:04Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.16 psi 2021-02-12T23:51:04Z- handleDepth()-Depth_Keller msg: 1613173864065 epoch sec; seqNo:7426; depth 25.19 2021-02-12T23:51:04Z- handleDvl() - RDI_Pathfinder msg: 1613173864.08 epoch sec; seqNo:3430 2021-02-12T23:51:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 88.59 , 99.54 , 65.38 2021-02-12T23:51:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , 0.01 2021-02-12T23:51:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173864.09 epoch sec; seqNo:7426; 4058000.02 north; 591616.57 east 2021-02-12T23:51:04Z- AHRS_M2 msg: 1613173864.46 epoch sec; seqNo:7366 2021-02-12T23:51:04Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.16 psi 2021-02-12T23:51:04Z- handleDepth()-Depth_Keller msg: 1613173864470 epoch sec; seqNo:7427; depth 25.04 2021-02-12T23:51:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173864.48 epoch sec; seqNo:7427; 4058000.02 north; 591616.57 east 2021-02-12T23:51:04Z- AHRS_M2 msg: 1613173864.89 epoch sec; seqNo:7367 2021-02-12T23:51:04Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.15 psi 2021-02-12T23:51:04Z- handleDepth()-Depth_Keller msg: 1613173864900 epoch sec; seqNo:7428; depth 24.93 2021-02-12T23:51:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173864.91 epoch sec; seqNo:7428; 4058000.40 north; 591616.56 east 2021-02-12T23:51:05Z- AHRS_M2 msg: 1613173865.27 epoch sec; seqNo:7368 2021-02-12T23:51:05Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.15 psi 2021-02-12T23:51:05Z- handleDepth()-Depth_Keller msg: 1613173865277 epoch sec; seqNo:7429; depth 24.79 2021-02-12T23:51:05Z- handleDvl() - RDI_Pathfinder msg: 1613173865.29 epoch sec; seqNo:3431 2021-02-12T23:51:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 0.00 , 102.18 , 66.20 2021-02-12T23:51:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.07 , 0.04 2021-02-12T23:51:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173865.30 epoch sec; seqNo:7429; 4058000.79 north; 591617.78 east 2021-02-12T23:51:05Z- AHRS_M2 msg: 1613173865.68 epoch sec; seqNo:7369 2021-02-12T23:51:05Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.15 psi 2021-02-12T23:51:05Z- handleDepth()-Depth_Keller msg: 1613173865681 epoch sec; seqNo:7430; depth 24.67 2021-02-12T23:51:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173865.69 epoch sec; seqNo:7430; 4058000.79 north; 591617.78 east 2021-02-12T23:51:06Z- AHRS_M2 msg: 1613173866.08 epoch sec; seqNo:7370 2021-02-12T23:51:06Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.14 psi 2021-02-12T23:51:06Z- handleDepth()-Depth_Keller msg: 1613173866089 epoch sec; seqNo:7431; depth 24.56 2021-02-12T23:51:06Z- handleDvl() - RDI_Pathfinder msg: 1613173866.10 epoch sec; seqNo:3432 2021-02-12T23:51:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:51:06Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:51:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173866.11 epoch sec; seqNo:7431; 4058001.17 north; 591617.78 east 2021-02-12T23:51:06Z- AHRS_M2 msg: 1613173866.49 epoch sec; seqNo:7371 2021-02-12T23:51:06Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.14 psi 2021-02-12T23:51:06Z- handleDepth()-Depth_Keller msg: 1613173866491 epoch sec; seqNo:7432; depth 24.41 2021-02-12T23:51:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173866.50 epoch sec; seqNo:7432; 4058001.55 north; 591617.77 east 2021-02-12T23:51:06Z- AHRS_M2 msg: 1613173866.89 epoch sec; seqNo:7372 2021-02-12T23:51:06Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.14 psi 2021-02-12T23:51:06Z- handleDepth()-Depth_Keller msg: 1613173866906 epoch sec; seqNo:7433; depth 24.28 2021-02-12T23:51:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173866.91 epoch sec; seqNo:7433; 4058001.93 north; 591617.77 east 2021-02-12T23:51:07Z- LcmTrn::handleMessages() - handling 40 messages took 0.58 ms and 129 calls... 2021-02-12T23:51:07Z- waiting for 1613173865.07, time is 1613173866.91 2021-02-12T23:51:07Z- LcmTrn::updateTrn() >>>> pose time 1613173866.91, meas time 1613173866.10 <<<< 2021-02-12T23:51:07Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173866.10, ping # 0. 2021-02-12T23:51:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173866.10 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:51:07Z- TerrainNav::motionUpdate() - currEstimate = (4058002, 591617.77, 24.28) 2021-02-12T23:51:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:51:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:51:07Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:51:07Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:51:07Z- TNavBF::Projecting Measurements 2021-02-12T23:51:07Z- TNavFilter::Measurement from time = 1613173866.10 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:51:07Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:51:07Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:51:07Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:51:07Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:51:07Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:51:07Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:51:07Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:51:07Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 732.9 EastVariance: 833.0 DepthVariance: 1.7 this->DepthBiasCov: 1.7 2021-02-12T23:51:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:51:07Z- TNavBF::mmse selected from filter -1 2021-02-12T23:51:07Z- variances([0], [2], and psi_berg) = 732.862721 833.018690 0.000000 2021-02-12T23:51:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:51:07Z- LcmTrn::publishEstimates() - MLE : 4057994.43 591573.27 24.28 2021-02-12T23:51:07Z- LcmTrn::publishEstimates() - MMSE : 4058001.93 591617.77 24.28 2021-02-12T23:51:07Z- LcmTrn::publishEstimates() - COVAR: 732.86 833.02 1.76 0.00 2021-02-12T23:51:07Z- AHRS_M2 msg: 1613173867.29 epoch sec; seqNo:7373 2021-02-12T23:51:07Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.14 psi 2021-02-12T23:51:07Z- handleDepth()-Depth_Keller msg: 1613173867297 epoch sec; seqNo:7434; depth 24.16 2021-02-12T23:51:07Z- handleDvl() - RDI_Pathfinder msg: 1613173867.31 epoch sec; seqNo:3433 2021-02-12T23:51:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 0.00 , 101.70 , 64.57 2021-02-12T23:51:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.00 , 0.03 2021-02-12T23:51:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173867.32 epoch sec; seqNo:7434; 4058001.93 north; 591617.77 east 2021-02-12T23:51:07Z- AHRS_M2 msg: 1613173867.70 epoch sec; seqNo:7374 2021-02-12T23:51:07Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.13 psi 2021-02-12T23:51:07Z- handleDepth()-Depth_Keller msg: 1613173867702 epoch sec; seqNo:7435; depth 24.02 2021-02-12T23:51:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173867.71 epoch sec; seqNo:7435; 4058002.31 north; 591617.76 east 2021-02-12T23:51:08Z- AHRS_M2 msg: 1613173868.10 epoch sec; seqNo:7375 2021-02-12T23:51:08Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.13 psi 2021-02-12T23:51:08Z- handleDepth()-Depth_Keller msg: 1613173868105 epoch sec; seqNo:7436; depth 23.89 2021-02-12T23:51:08Z- handleDvl() - RDI_Pathfinder msg: 1613173868.13 epoch sec; seqNo:3434 2021-02-12T23:51:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.78 , 0.00 , 102.24 , 65.38 2021-02-12T23:51:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , 0.03 2021-02-12T23:51:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173868.14 epoch sec; seqNo:7436; 4058002.70 north; 591618.98 east 2021-02-12T23:51:08Z- AHRS_M2 msg: 1613173868.50 epoch sec; seqNo:7376 2021-02-12T23:51:08Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.13 psi 2021-02-12T23:51:08Z- handleDepth()-Depth_Keller msg: 1613173868509 epoch sec; seqNo:7437; depth 23.77 2021-02-12T23:51:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173868.52 epoch sec; seqNo:7437; 4058002.70 north; 591618.98 east 2021-02-12T23:51:08Z- AHRS_M2 msg: 1613173868.91 epoch sec; seqNo:7377 2021-02-12T23:51:08Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.13 psi 2021-02-12T23:51:08Z- handleDepth()-Depth_Keller msg: 1613173868917 epoch sec; seqNo:7438; depth 23.65 2021-02-12T23:51:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173868.92 epoch sec; seqNo:7438; 4058003.08 north; 591618.98 east 2021-02-12T23:51:09Z- AHRS_M2 msg: 1613173869.31 epoch sec; seqNo:7378 2021-02-12T23:51:09Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.13 psi 2021-02-12T23:51:09Z- handleDepth()-Depth_Keller msg: 1613173869381 epoch sec; seqNo:7439; depth 23.52 2021-02-12T23:51:09Z- handleDvl() - RDI_Pathfinder msg: 1613173869.39 epoch sec; seqNo:3435 2021-02-12T23:51:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:51:09Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:51:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173869.41 epoch sec; seqNo:7439; 4058003.46 north; 591618.97 east 2021-02-12T23:51:09Z- AHRS_M2 msg: 1613173869.72 epoch sec; seqNo:7379 2021-02-12T23:51:09Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.13 psi 2021-02-12T23:51:09Z- handleDepth()-Depth_Keller msg: 1613173869766 epoch sec; seqNo:7440; depth 23.37 2021-02-12T23:51:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173869.78 epoch sec; seqNo:7440; 4058003.84 north; 591618.97 east 2021-02-12T23:51:10Z- AHRS_M2 msg: 1613173870.12 epoch sec; seqNo:7380 2021-02-12T23:51:10Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.13 psi 2021-02-12T23:51:10Z- handleDepth()-Depth_Keller msg: 1613173870141 epoch sec; seqNo:7441; depth 23.25 2021-02-12T23:51:10Z- handleDvl() - RDI_Pathfinder msg: 1613173870.16 epoch sec; seqNo:3436 2021-02-12T23:51:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.60 , 0.00 , 102.92 , 65.79 2021-02-12T23:51:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , 0.05 2021-02-12T23:51:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173870.17 epoch sec; seqNo:7441; 4058003.84 north; 591618.97 east 2021-02-12T23:51:10Z- AHRS_M2 msg: 1613173870.52 epoch sec; seqNo:7381 2021-02-12T23:51:10Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.13 psi 2021-02-12T23:51:10Z- handleDepth()-Depth_Keller msg: 1613173870610 epoch sec; seqNo:7442; depth 23.13 2021-02-12T23:51:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173870.63 epoch sec; seqNo:7442; 4058004.23 north; 591620.19 east 2021-02-12T23:51:10Z- AHRS_M2 msg: 1613173870.93 epoch sec; seqNo:7382 2021-02-12T23:51:10Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.13 psi 2021-02-12T23:51:10Z- handleDepth()-Depth_Keller msg: 1613173870945 epoch sec; seqNo:7443; depth 22.97 2021-02-12T23:51:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173870.95 epoch sec; seqNo:7443; 4058004.61 north; 591620.18 east 2021-02-12T23:51:11Z- AHRS_M2 msg: 1613173871.33 epoch sec; seqNo:7383 2021-02-12T23:51:11Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.13 psi 2021-02-12T23:51:11Z- handleDepth()-Depth_Keller msg: 1613173871357 epoch sec; seqNo:7444; depth 22.87 2021-02-12T23:51:11Z- handleDvl() - RDI_Pathfinder msg: 1613173871.37 epoch sec; seqNo:3437 2021-02-12T23:51:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 0.00 , 104.34 , 155.17 2021-02-12T23:51:11Z- handleDvl() - RDI_Pathfinder msg: velocities 1.35 , -0.52 , 0.34 2021-02-12T23:51:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173871.38 epoch sec; seqNo:7444; 4058004.99 north; 591620.18 east 2021-02-12T23:51:11Z- AHRS_M2 msg: 1613173871.74 epoch sec; seqNo:7384 2021-02-12T23:51:11Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.13 psi 2021-02-12T23:51:11Z- handleDepth()-Depth_Keller msg: 1613173871762 epoch sec; seqNo:7445; depth 22.73 2021-02-12T23:51:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173871.78 epoch sec; seqNo:7445; 4058005.75 north; 591620.17 east 2021-02-12T23:51:12Z- AHRS_M2 msg: 1613173872.14 epoch sec; seqNo:7385 2021-02-12T23:51:12Z- AHRS_M2 msg: 0.65 phi; 0.37 theta; -0.13 psi 2021-02-12T23:51:12Z- handleDepth()-Depth_Keller msg: 1613173872151 epoch sec; seqNo:7446; depth 22.61 2021-02-12T23:51:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173872.17 epoch sec; seqNo:7446; 4058006.12 north; 591620.17 east 2021-02-12T23:51:12Z- LcmTrn::handleMessages() - handling 44 messages took 0.75 ms and 129 calls... 2021-02-12T23:51:12Z- waiting for 1613173869.91, time is 1613173872.17 2021-02-12T23:51:12Z- LcmTrn::updateTrn() >>>> pose time 1613173872.17, meas time 1613173871.37 <<<< 2021-02-12T23:51:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173871.37 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:51:12Z- TerrainNav::motionUpdate() - currEstimate = (4058006, 591620.17, 22.61) 2021-02-12T23:51:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:51:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:51:12Z- TNavBF::Projecting Measurements 2021-02-12T23:51:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:51:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:51:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:51:12Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:51:12Z- TNavFilter::3 good beams. 2021-02-12T23:51:12Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:51:12Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:51:12Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:51:12Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:51:12Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:51:12Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:51:12Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:51:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:51:12Z- TerrainMapOctree: (vehN, vehE ) = (4058005.49, 591619.80). 2021-02-12T23:51:12Z- TerrainMapOctree: Min Distance = 4176.70. 2021-02-12T23:51:12Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:51:12Z- There is no specific good beam that all particles have in common. 2021-02-12T23:51:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:51:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.97 K: 0.150 ExpectedMeasDiff: nan currMeas.covariance: 9.44 2021-02-12T23:51:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:51:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.67 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 17.42 2021-02-12T23:51:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:51:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.54 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 38.53 2021-02-12T23:51:12Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:51:12Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:51:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:51:12Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:51:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:51:12Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:51:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:51:12Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:51:12Z- Filter 0 No beams used in subcloud update 2021-02-12T23:51:12Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:51:12Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:51:12Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 732.8 EastVariance: 833.0 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-12T23:51:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173871.37 sec. 2021-02-12T23:51:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:51:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:51:12Z- variances([0], [2], and psi_berg) = 732.749839 833.018605 0.000000 2021-02-12T23:51:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:51:12Z- LcmTrn::publishEstimates() - MLE : 4057998.62 591575.67 22.61 2021-02-12T23:51:12Z- LcmTrn::publishEstimates() - MMSE : 4058006.12 591620.17 22.61 2021-02-12T23:51:12Z- LcmTrn::publishEstimates() - COVAR: 732.75 833.02 1.52 0.00 2021-02-12T23:51:12Z- AHRS_M2 msg: 1613173872.54 epoch sec; seqNo:7386 2021-02-12T23:51:12Z- AHRS_M2 msg: 0.65 phi; 0.37 theta; -0.14 psi 2021-02-12T23:51:12Z- handleDepth()-Depth_Keller msg: 1613173872550 epoch sec; seqNo:7447; depth 22.49 2021-02-12T23:51:12Z- handleDvl() - RDI_Pathfinder msg: 1613173872.57 epoch sec; seqNo:3438 2021-02-12T23:51:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.14 , 97.24 , 104.68 , 77.91 2021-02-12T23:51:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , 0.06 2021-02-12T23:51:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173872.58 epoch sec; seqNo:7447; 4058006.50 north; 591620.16 east 2021-02-12T23:51:12Z- AHRS_M2 msg: 1613173872.95 epoch sec; seqNo:7387 2021-02-12T23:51:12Z- AHRS_M2 msg: 0.65 phi; 0.37 theta; -0.14 psi 2021-02-12T23:51:12Z- handleDepth()-Depth_Keller msg: 1613173872958 epoch sec; seqNo:7448; depth 22.35 2021-02-12T23:51:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173872.97 epoch sec; seqNo:7448; 4058006.88 north; 591620.16 east 2021-02-12T23:51:13Z- AHRS_M2 msg: 1613173873.36 epoch sec; seqNo:7388 2021-02-12T23:51:13Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.15 psi 2021-02-12T23:51:13Z- handleDepth()-Depth_Keller msg: 1613173873361 epoch sec; seqNo:7449; depth 22.21 2021-02-12T23:51:13Z- handleDvl() - RDI_Pathfinder msg: 1613173873.37 epoch sec; seqNo:3439 2021-02-12T23:51:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:51:13Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:51:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173873.40 epoch sec; seqNo:7449; 4058006.89 north; 591621.38 east 2021-02-12T23:51:13Z- AHRS_M2 msg: 1613173873.76 epoch sec; seqNo:7389 2021-02-12T23:51:13Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.15 psi 2021-02-12T23:51:13Z- handleDepth()-Depth_Keller msg: 1613173873761 epoch sec; seqNo:7450; depth 22.07 2021-02-12T23:51:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173873.78 epoch sec; seqNo:7450; 4058007.27 north; 591621.37 east 2021-02-12T23:51:14Z- AHRS_M2 msg: 1613173874.16 epoch sec; seqNo:7390 2021-02-12T23:51:14Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.16 psi 2021-02-12T23:51:14Z- handleDepth()-Depth_Keller msg: 1613173874173 epoch sec; seqNo:7451; depth 21.95 2021-02-12T23:51:14Z- handleDvl() - RDI_Pathfinder msg: 1613173874.18 epoch sec; seqNo:3440 2021-02-12T23:51:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 97.38 , 105.49 , 69.06 2021-02-12T23:51:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.07 , 0.00 2021-02-12T23:51:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173874.19 epoch sec; seqNo:7451; 4058007.65 north; 591621.37 east 2021-02-12T23:51:14Z- AHRS_M2 msg: 1613173874.56 epoch sec; seqNo:7391 2021-02-12T23:51:14Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.17 psi 2021-02-12T23:51:14Z- handleDepth()-Depth_Keller msg: 1613173874569 epoch sec; seqNo:7452; depth 21.83 2021-02-12T23:51:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173874.59 epoch sec; seqNo:7452; 4058007.65 north; 591621.37 east 2021-02-12T23:51:14Z- AHRS_M2 msg: 1613173874.97 epoch sec; seqNo:7392 2021-02-12T23:51:14Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.18 psi 2021-02-12T23:51:14Z- handleDepth()-Depth_Keller msg: 1613173874974 epoch sec; seqNo:7453; depth 21.71 2021-02-12T23:51:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173874.98 epoch sec; seqNo:7453; 4058008.03 north; 591621.37 east 2021-02-12T23:51:15Z- AHRS_M2 msg: 1613173875.37 epoch sec; seqNo:7393 2021-02-12T23:51:15Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.19 psi 2021-02-12T23:51:15Z- handleDepth()-Depth_Keller msg: 1613173875382 epoch sec; seqNo:7454; depth 21.58 2021-02-12T23:51:15Z- handleDvl() - RDI_Pathfinder msg: 1613173875.39 epoch sec; seqNo:3441 2021-02-12T23:51:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.60 , 97.98 , 106.50 , 69.47 2021-02-12T23:51:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , -0.03 2021-02-12T23:51:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173875.40 epoch sec; seqNo:7454; 4058008.42 north; 591622.58 east 2021-02-12T23:51:15Z- AHRS_M2 msg: 1613173875.78 epoch sec; seqNo:7394 2021-02-12T23:51:15Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.19 psi 2021-02-12T23:51:15Z- handleDepth()-Depth_Keller msg: 1613173875780 epoch sec; seqNo:7455; depth 21.45 2021-02-12T23:51:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173875.79 epoch sec; seqNo:7455; 4058008.42 north; 591622.58 east 2021-02-12T23:51:16Z- AHRS_M2 msg: 1613173876.18 epoch sec; seqNo:7395 2021-02-12T23:51:16Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.20 psi 2021-02-12T23:51:16Z- handleDepth()-Depth_Keller msg: 1613173876193 epoch sec; seqNo:7456; depth 21.33 2021-02-12T23:51:16Z- handleDvl() - RDI_Pathfinder msg: 1613173876.20 epoch sec; seqNo:3442 2021-02-12T23:51:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.01 , 94.33 , 107.72 , 78.51 2021-02-12T23:51:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , -0.04 2021-02-12T23:51:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173876.23 epoch sec; seqNo:7456; 4058008.80 north; 591622.58 east 2021-02-12T23:51:16Z- AHRS_M2 msg: 1613173876.58 epoch sec; seqNo:7396 2021-02-12T23:51:16Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.21 psi 2021-02-12T23:51:16Z- handleDepth()-Depth_Keller msg: 1613173876589 epoch sec; seqNo:7457; depth 21.19 2021-02-12T23:51:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173876.60 epoch sec; seqNo:7457; 4058009.18 north; 591622.57 east 2021-02-12T23:51:16Z- AHRS_M2 msg: 1613173876.99 epoch sec; seqNo:7397 2021-02-12T23:51:16Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.21 psi 2021-02-12T23:51:17Z- handleDepth()-Depth_Keller msg: 1613173877001 epoch sec; seqNo:7458; depth 21.07 2021-02-12T23:51:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173877.01 epoch sec; seqNo:7458; 4058009.18 north; 591622.57 east 2021-02-12T23:51:17Z- LcmTrn::handleMessages() - handling 41 messages took 0.65 ms and 128 calls... 2021-02-12T23:51:17Z- waiting for 1613173875.17, time is 1613173877.01 2021-02-12T23:51:17Z- LcmTrn::updateTrn() >>>> pose time 1613173877.01, meas time 1613173876.20 <<<< 2021-02-12T23:51:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173876.20 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:51:17Z- TerrainNav::motionUpdate() - currEstimate = (4058009, 591622.57, 21.07) 2021-02-12T23:51:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:51:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:51:17Z- TNavBF::Projecting Measurements 2021-02-12T23:51:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:51:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:51:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:51:17Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:51:17Z- TNavFilter::3 good beams. 2021-02-12T23:51:17Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:51:17Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:51:17Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:51:17Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:51:17Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:51:17Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:51:17Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:51:17Z- TNavBF::mmse selected from filter -1 2021-02-12T23:51:17Z- TerrainMapOctree: (vehN, vehE ) = (4058008.67, 591622.17). 2021-02-12T23:51:17Z- TerrainMapOctree: Min Distance = 4172.73. 2021-02-12T23:51:17Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:51:17Z- There is no specific good beam that all particles have in common. 2021-02-12T23:51:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:51:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.68 K: 0.133 ExpectedMeasDiff: nan currMeas.covariance: 9.24 2021-02-12T23:51:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:51:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.46 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 14.24 2021-02-12T23:51:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:51:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 18.57 2021-02-12T23:51:17Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:51:17Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:51:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:51:17Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:51:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:51:17Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:51:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:51:17Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:51:17Z- Filter 0 No beams used in subcloud update 2021-02-12T23:51:17Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:51:17Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:51:17Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 732.7 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-12T23:51:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173876.20 sec. 2021-02-12T23:51:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:51:17Z- TNavBF::mmse selected from filter -1 2021-02-12T23:51:17Z- variances([0], [2], and psi_berg) = 732.647842 833.018528 0.000000 2021-02-12T23:51:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:51:17Z- LcmTrn::publishEstimates() - MLE : 4058001.68 591578.07 21.07 2021-02-12T23:51:17Z- LcmTrn::publishEstimates() - MMSE : 4058009.18 591622.57 21.07 2021-02-12T23:51:17Z- LcmTrn::publishEstimates() - COVAR: 732.65 833.02 1.29 0.00 2021-02-12T23:51:17Z- AHRS_M2 msg: 1613173877.39 epoch sec; seqNo:7398 2021-02-12T23:51:17Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.22 psi 2021-02-12T23:51:17Z- handleDepth()-Depth_Keller msg: 1613173877401 epoch sec; seqNo:7459; depth 20.96 2021-02-12T23:51:17Z- handleDvl() - RDI_Pathfinder msg: 1613173877.42 epoch sec; seqNo:3443 2021-02-12T23:51:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 97.85 , 106.37 , 0.00 2021-02-12T23:51:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , -0.03 2021-02-12T23:51:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173877.43 epoch sec; seqNo:7459; 4058009.56 north; 591622.57 east 2021-02-12T23:51:17Z- AHRS_M2 msg: 1613173877.80 epoch sec; seqNo:7399 2021-02-12T23:51:17Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.22 psi 2021-02-12T23:51:17Z- handleDepth()-Depth_Keller msg: 1613173877805 epoch sec; seqNo:7460; depth 20.83 2021-02-12T23:51:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173877.82 epoch sec; seqNo:7460; 4058009.95 north; 591623.79 east 2021-02-12T23:51:18Z- AHRS_M2 msg: 1613173878.20 epoch sec; seqNo:7400 2021-02-12T23:51:18Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.22 psi 2021-02-12T23:51:18Z- handleDepth()-Depth_Keller msg: 1613173878213 epoch sec; seqNo:7461; depth 20.69 2021-02-12T23:51:18Z- handleDvl() - RDI_Pathfinder msg: 1613173878.22 epoch sec; seqNo:3444 2021-02-12T23:51:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.78 , 99.00 , 108.46 , 71.11 2021-02-12T23:51:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.02 , -0.00 2021-02-12T23:51:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173878.23 epoch sec; seqNo:7461; 4058009.95 north; 591623.79 east 2021-02-12T23:51:18Z- AHRS_M2 msg: 1613173878.60 epoch sec; seqNo:7401 2021-02-12T23:51:18Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.22 psi 2021-02-12T23:51:18Z- handleDepth()-Depth_Keller msg: 1613173878609 epoch sec; seqNo:7462; depth 20.59 2021-02-12T23:51:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173878.63 epoch sec; seqNo:7462; 4058010.33 north; 591623.78 east 2021-02-12T23:51:19Z- AHRS_M2 msg: 1613173879.01 epoch sec; seqNo:7402 2021-02-12T23:51:19Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.22 psi 2021-02-12T23:51:19Z- handleDepth()-Depth_Keller msg: 1613173879013 epoch sec; seqNo:7463; depth 20.46 2021-02-12T23:51:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173879.02 epoch sec; seqNo:7463; 4058010.71 north; 591623.78 east 2021-02-12T23:51:19Z- AHRS_M2 msg: 1613173879.42 epoch sec; seqNo:7403 2021-02-12T23:51:19Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.22 psi 2021-02-12T23:51:19Z- handleDepth()-Depth_Keller msg: 1613173879425 epoch sec; seqNo:7464; depth 20.34 2021-02-12T23:51:19Z- handleDvl() - RDI_Pathfinder msg: 1613173879.44 epoch sec; seqNo:3445 2021-02-12T23:51:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.42 , 97.92 , 107.65 , 78.45 2021-02-12T23:51:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.04 , 0.01 2021-02-12T23:51:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173879.45 epoch sec; seqNo:7464; 4058010.71 north; 591623.78 east 2021-02-12T23:51:19Z- AHRS_M2 msg: 1613173879.82 epoch sec; seqNo:7404 2021-02-12T23:51:19Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.21 psi 2021-02-12T23:51:19Z- handleDepth()-Depth_Keller msg: 1613173879821 epoch sec; seqNo:7465; depth 20.21 2021-02-12T23:51:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173879.84 epoch sec; seqNo:7465; 4058011.09 north; 591623.78 east 2021-02-12T23:51:20Z- AHRS_M2 msg: 1613173880.22 epoch sec; seqNo:7405 2021-02-12T23:51:20Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.22 psi 2021-02-12T23:51:20Z- handleDepth()-Depth_Keller msg: 1613173880225 epoch sec; seqNo:7466; depth 20.10 2021-02-12T23:51:20Z- handleDvl() - RDI_Pathfinder msg: 1613173880.24 epoch sec; seqNo:3446 2021-02-12T23:51:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 97.78 , 102.65 , 69.47 2021-02-12T23:51:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.07 , 0.03 2021-02-12T23:51:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173880.26 epoch sec; seqNo:7466; 4058011.47 north; 591623.77 east 2021-02-12T23:51:20Z- AHRS_M2 msg: 1613173880.62 epoch sec; seqNo:7406 2021-02-12T23:51:20Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.22 psi 2021-02-12T23:51:20Z- handleDepth()-Depth_Keller msg: 1613173880629 epoch sec; seqNo:7467; depth 19.99 2021-02-12T23:51:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173880.64 epoch sec; seqNo:7467; 4058011.47 north; 591623.77 east 2021-02-12T23:51:21Z- AHRS_M2 msg: 1613173881.05 epoch sec; seqNo:7407 2021-02-12T23:51:21Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.23 psi 2021-02-12T23:51:21Z- handleDepth()-Depth_Keller msg: 1613173881060 epoch sec; seqNo:7468; depth 19.85 2021-02-12T23:51:21Z- handleDvl() - RDI_Pathfinder msg: 1613173881.07 epoch sec; seqNo:3447 2021-02-12T23:51:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.42 , 99.20 , 108.93 , 69.47 2021-02-12T23:51:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , 0.02 2021-02-12T23:51:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173881.09 epoch sec; seqNo:7468; 4058011.86 north; 591624.99 east 2021-02-12T23:51:21Z- AHRS_M2 msg: 1613173881.43 epoch sec; seqNo:7408 2021-02-12T23:51:21Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.23 psi 2021-02-12T23:51:21Z- handleDepth()-Depth_Keller msg: 1613173881437 epoch sec; seqNo:7469; depth 19.72 2021-02-12T23:51:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173881.45 epoch sec; seqNo:7469; 4058012.24 north; 591624.98 east 2021-02-12T23:51:21Z- AHRS_M2 msg: 1613173881.84 epoch sec; seqNo:7409 2021-02-12T23:51:21Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.23 psi 2021-02-12T23:51:21Z- handleDepth()-Depth_Keller msg: 1613173881841 epoch sec; seqNo:7470; depth 19.62 2021-02-12T23:51:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173881.85 epoch sec; seqNo:7470; 4058012.62 north; 591624.98 east 2021-02-12T23:51:22Z- AHRS_M2 msg: 1613173882.24 epoch sec; seqNo:7410 2021-02-12T23:51:22Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.23 psi 2021-02-12T23:51:22Z- handleDepth()-Depth_Keller msg: 1613173882245 epoch sec; seqNo:7471; depth 19.49 2021-02-12T23:51:22Z- handleDvl() - RDI_Pathfinder msg: 1613173882.26 epoch sec; seqNo:3448 2021-02-12T23:51:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.01 , 100.35 , 106.43 , 71.51 2021-02-12T23:51:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.10 , 0.04 2021-02-12T23:51:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173882.27 epoch sec; seqNo:7471; 4058012.62 north; 591624.98 east 2021-02-12T23:51:22Z- LcmTrn::handleMessages() - handling 45 messages took 0.68 ms and 132 calls... 2021-02-12T23:51:22Z- waiting for 1613173880.01, time is 1613173882.27 2021-02-12T23:51:22Z- LcmTrn::updateTrn() equating pose 1613173882.27 and meas times 1613173882.27 2021-02-12T23:51:22Z- LcmTrn::updateTrn() >>>> pose time 1613173882.27, meas time 1613173882.27 <<<< 2021-02-12T23:51:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173882.27 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:51:22Z- TerrainNav::motionUpdate() - currEstimate = (4058013, 591624.98, 19.49) 2021-02-12T23:51:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:51:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:51:22Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173882.27 sec.; No relevant navigation data available 2021-02-12T23:51:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:51:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:51:22Z- variances([0], [2], and psi_berg) = 732.567766 833.018468 0.000000 2021-02-12T23:51:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:51:22Z- LcmTrn::publishEstimates() - MLE : 4058005.12 591580.48 19.49 2021-02-12T23:51:22Z- LcmTrn::publishEstimates() - MMSE : 4058012.62 591624.98 19.49 2021-02-12T23:51:22Z- LcmTrn::publishEstimates() - COVAR: 732.57 833.02 1.50 0.00 2021-02-12T23:51:22Z- AHRS_M2 msg: 1613173882.64 epoch sec; seqNo:7411 2021-02-12T23:51:22Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.22 psi 2021-02-12T23:51:22Z- handleDepth()-Depth_Keller msg: 1613173882722 epoch sec; seqNo:7472; depth 19.37 2021-02-12T23:51:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173882.73 epoch sec; seqNo:7472; 4058013.00 north; 591624.98 east 2021-02-12T23:51:23Z- AHRS_M2 msg: 1613173883.06 epoch sec; seqNo:7412 2021-02-12T23:51:23Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.22 psi 2021-02-12T23:51:23Z- handleDepth()-Depth_Keller msg: 1613173883075 epoch sec; seqNo:7473; depth 19.23 2021-02-12T23:51:23Z- handleDvl() - RDI_Pathfinder msg: 1613173883.09 epoch sec; seqNo:3449 2021-02-12T23:51:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.01 , 99.13 , 110.08 , 70.70 2021-02-12T23:51:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , 0.05 2021-02-12T23:51:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173883.10 epoch sec; seqNo:7473; 4058013.37 north; 591624.97 east 2021-02-12T23:51:23Z- AHRS_M2 msg: 1613173883.45 epoch sec; seqNo:7413 2021-02-12T23:51:23Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.21 psi 2021-02-12T23:51:23Z- handleDepth()-Depth_Keller msg: 1613173883482 epoch sec; seqNo:7474; depth 19.13 2021-02-12T23:51:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173883.49 epoch sec; seqNo:7474; 4058013.75 north; 591624.97 east 2021-02-12T23:51:23Z- AHRS_M2 msg: 1613173883.86 epoch sec; seqNo:7414 2021-02-12T23:51:23Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.19 psi 2021-02-12T23:51:23Z- handleDepth()-Depth_Keller msg: 1613173883881 epoch sec; seqNo:7475; depth 19.04 2021-02-12T23:51:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173883.89 epoch sec; seqNo:7475; 4058013.77 north; 591626.19 east 2021-02-12T23:51:24Z- AHRS_M2 msg: 1613173884.26 epoch sec; seqNo:7415 2021-02-12T23:51:24Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.18 psi 2021-02-12T23:51:24Z- handleDepth()-Depth_Keller msg: 1613173884329 epoch sec; seqNo:7476; depth 18.90 2021-02-12T23:51:24Z- handleDvl() - RDI_Pathfinder msg: 1613173884.34 epoch sec; seqNo:3450 2021-02-12T23:51:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.63 , 108.80 , 69.88 2021-02-12T23:51:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , 0.05 2021-02-12T23:51:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173884.35 epoch sec; seqNo:7476; 4058014.14 north; 591626.18 east 2021-02-12T23:51:24Z- AHRS_M2 msg: 1613173884.66 epoch sec; seqNo:7416 2021-02-12T23:51:24Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.17 psi 2021-02-12T23:51:24Z- handleDepth()-Depth_Keller msg: 1613173884692 epoch sec; seqNo:7477; depth 18.78 2021-02-12T23:51:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173884.70 epoch sec; seqNo:7477; 4058014.52 north; 591626.18 east 2021-02-12T23:51:25Z- AHRS_M2 msg: 1613173885.07 epoch sec; seqNo:7417 2021-02-12T23:51:25Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.16 psi 2021-02-12T23:51:25Z- handleDepth()-Depth_Keller msg: 1613173885101 epoch sec; seqNo:7478; depth 18.68 2021-02-12T23:51:25Z- handleDvl() - RDI_Pathfinder msg: 1613173885.11 epoch sec; seqNo:3451 2021-02-12T23:51:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.05 , 97.71 , 108.53 , 69.88 2021-02-12T23:51:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , 0.03 2021-02-12T23:51:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173885.13 epoch sec; seqNo:7478; 4058014.90 north; 591626.18 east 2021-02-12T23:51:25Z- AHRS_M2 msg: 1613173885.47 epoch sec; seqNo:7418 2021-02-12T23:51:25Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-12T23:51:25Z- handleDepth()-Depth_Keller msg: 1613173885497 epoch sec; seqNo:7479; depth 18.55 2021-02-12T23:51:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173885.52 epoch sec; seqNo:7479; 4058015.28 north; 591626.17 east 2021-02-12T23:51:25Z- AHRS_M2 msg: 1613173885.88 epoch sec; seqNo:7419 2021-02-12T23:51:25Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-12T23:51:25Z- handleDepth()-Depth_Keller msg: 1613173885889 epoch sec; seqNo:7480; depth 18.45 2021-02-12T23:51:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173885.90 epoch sec; seqNo:7480; 4058015.28 north; 591626.17 east 2021-02-12T23:51:26Z- AHRS_M2 msg: 1613173886.28 epoch sec; seqNo:7420 2021-02-12T23:51:26Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.14 psi 2021-02-12T23:51:26Z- handleDepth()-Depth_Keller msg: 1613173886294 epoch sec; seqNo:7481; depth 18.35 2021-02-12T23:51:26Z- handleDvl() - RDI_Pathfinder msg: 1613173886.31 epoch sec; seqNo:3452 2021-02-12T23:51:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.88 , 97.91 , 105.21 , 71.52 2021-02-12T23:51:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.11 , 0.01 2021-02-12T23:51:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173886.32 epoch sec; seqNo:7481; 4058015.66 north; 591626.17 east 2021-02-12T23:51:26Z- AHRS_M2 msg: 1613173886.69 epoch sec; seqNo:7421 2021-02-12T23:51:26Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-12T23:51:26Z- handleDepth()-Depth_Keller msg: 1613173886693 epoch sec; seqNo:7482; depth 18.21 2021-02-12T23:51:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173886.70 epoch sec; seqNo:7482; 4058016.04 north; 591626.16 east 2021-02-12T23:51:27Z- AHRS_M2 msg: 1613173887.09 epoch sec; seqNo:7422 2021-02-12T23:51:27Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-12T23:51:27Z- handleDepth()-Depth_Keller msg: 1613173887103 epoch sec; seqNo:7483; depth 18.10 2021-02-12T23:51:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173887.13 epoch sec; seqNo:7483; 4058016.43 north; 591627.38 east 2021-02-12T23:51:27Z- LcmTrn::handleMessages() - handling 40 messages took 0.71 ms and 126 calls... 2021-02-12T23:51:27Z- waiting for 1613173885.27, time is 1613173887.13 2021-02-12T23:51:27Z- LcmTrn::updateTrn() >>>> pose time 1613173887.13, meas time 1613173886.31 <<<< 2021-02-12T23:51:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173886.31 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:51:27Z- TerrainNav::motionUpdate() - currEstimate = (4058016, 591627.38, 18.10) 2021-02-12T23:51:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:51:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:51:27Z- TNavBF::Projecting Measurements 2021-02-12T23:51:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:51:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:51:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:51:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:51:27Z- TNavFilter::4 good beams. 2021-02-12T23:51:27Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:51:27Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:51:27Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:51:27Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:51:27Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:51:27Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:51:27Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:51:27Z- TNavBF::mmse selected from filter -1 2021-02-12T23:51:27Z- TerrainMapOctree: (vehN, vehE ) = (4058015.79, 591626.97). 2021-02-12T23:51:27Z- TerrainMapOctree: Min Distance = 4164.18. 2021-02-12T23:51:27Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:51:27Z- There is no specific good beam that all particles have in common. 2021-02-12T23:51:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:51:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.69 K: 0.122 ExpectedMeasDiff: nan currMeas.covariance: 10.47 2021-02-12T23:51:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:51:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.49 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-12T23:51:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:51:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 17.71 2021-02-12T23:51:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:51:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.114 ExpectedMeasDiff: nan currMeas.covariance: 8.18 2021-02-12T23:51:27Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:51:27Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:51:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:51:27Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:51:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:51:27Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:51:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:51:27Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:51:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:51:27Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:51:27Z- Filter 0 No beams used in subcloud update 2021-02-12T23:51:27Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:51:27Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:51:27Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 732.5 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:51:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173886.31 sec. 2021-02-12T23:51:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:51:27Z- TNavBF::mmse selected from filter -1 2021-02-12T23:51:27Z- variances([0], [2], and psi_berg) = 732.458096 833.018385 0.000000 2021-02-12T23:51:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:51:27Z- LcmTrn::publishEstimates() - MLE : 4058008.93 591582.88 18.10 2021-02-12T23:51:27Z- LcmTrn::publishEstimates() - MMSE : 4058016.43 591627.38 18.10 2021-02-12T23:51:27Z- LcmTrn::publishEstimates() - COVAR: 732.46 833.02 1.17 0.00 2021-02-12T23:51:27Z- AHRS_M2 msg: 1613173887.49 epoch sec; seqNo:7423 2021-02-12T23:51:27Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.16 psi 2021-02-12T23:51:27Z- handleDepth()-Depth_Keller msg: 1613173887499 epoch sec; seqNo:7484; depth 17.99 2021-02-12T23:51:27Z- handleDvl() - RDI_Pathfinder msg: 1613173887.51 epoch sec; seqNo:3453 2021-02-12T23:51:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.81 , 95.41 , 107.58 , 72.75 2021-02-12T23:51:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.12 , 0.02 2021-02-12T23:51:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173887.52 epoch sec; seqNo:7484; 4058016.81 north; 591627.38 east 2021-02-12T23:51:27Z- AHRS_M2 msg: 1613173887.89 epoch sec; seqNo:7424 2021-02-12T23:51:27Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.16 psi 2021-02-12T23:51:27Z- handleDepth()-Depth_Keller msg: 1613173887901 epoch sec; seqNo:7485; depth 17.88 2021-02-12T23:51:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173887.92 epoch sec; seqNo:7485; 4058016.81 north; 591627.38 east 2021-02-12T23:51:28Z- AHRS_M2 msg: 1613173888.30 epoch sec; seqNo:7425 2021-02-12T23:51:28Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.16 psi 2021-02-12T23:51:28Z- handleDepth()-Depth_Keller msg: 1613173888304 epoch sec; seqNo:7486; depth 17.75 2021-02-12T23:51:28Z- handleDvl() - RDI_Pathfinder msg: 1613173888.32 epoch sec; seqNo:3454 2021-02-12T23:51:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.81 , 97.85 , 108.80 , 78.38 2021-02-12T23:51:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.10 , 0.04 2021-02-12T23:51:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173888.34 epoch sec; seqNo:7486; 4058017.19 north; 591627.37 east 2021-02-12T23:51:28Z- AHRS_M2 msg: 1613173888.71 epoch sec; seqNo:7426 2021-02-12T23:51:28Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.16 psi 2021-02-12T23:51:28Z- handleDepth()-Depth_Keller msg: 1613173888715 epoch sec; seqNo:7487; depth 17.63 2021-02-12T23:51:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173888.73 epoch sec; seqNo:7487; 4058017.57 north; 591627.37 east 2021-02-12T23:51:29Z- AHRS_M2 msg: 1613173889.14 epoch sec; seqNo:7427 2021-02-12T23:51:29Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.15 psi 2021-02-12T23:51:29Z- handleDepth()-Depth_Keller msg: 1613173889143 epoch sec; seqNo:7488; depth 17.52 2021-02-12T23:51:29Z- handleDvl() - RDI_Pathfinder msg: 1613173889.16 epoch sec; seqNo:3455 2021-02-12T23:51:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.87 , 97.65 , 109.81 , 78.18 2021-02-12T23:51:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.06 2021-02-12T23:51:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173889.18 epoch sec; seqNo:7488; 4058017.95 north; 591627.36 east 2021-02-12T23:51:29Z- handleDepth()-Depth_Keller msg: 1613173889533 epoch sec; seqNo:7489; depth 17.38 2021-02-12T23:51:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173889.54 epoch sec; seqNo:7489; 4058018.33 north; 591627.36 east 2021-02-12T23:51:29Z- AHRS_M2 msg: 1613173889.92 epoch sec; seqNo:7428 2021-02-12T23:51:29Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.15 psi 2021-02-12T23:51:29Z- handleDepth()-Depth_Keller msg: 1613173889929 epoch sec; seqNo:7490; depth 17.28 2021-02-12T23:51:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173889.94 epoch sec; seqNo:7490; 4058018.34 north; 591628.58 east 2021-02-12T23:51:30Z- AHRS_M2 msg: 1613173890.33 epoch sec; seqNo:7429 2021-02-12T23:51:30Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.14 psi 2021-02-12T23:51:30Z- handleDepth()-Depth_Keller msg: 1613173890337 epoch sec; seqNo:7491; depth 17.18 2021-02-12T23:51:30Z- handleDvl() - RDI_Pathfinder msg: 1613173890.35 epoch sec; seqNo:3456 2021-02-12T23:51:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.83 , 97.65 , 112.25 , 78.18 2021-02-12T23:51:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , 0.05 2021-02-12T23:51:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173890.36 epoch sec; seqNo:7491; 4058018.72 north; 591628.58 east 2021-02-12T23:51:30Z- AHRS_M2 msg: 1613173890.73 epoch sec; seqNo:7430 2021-02-12T23:51:30Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.14 psi 2021-02-12T23:51:30Z- handleDepth()-Depth_Keller msg: 1613173890743 epoch sec; seqNo:7492; depth 17.04 2021-02-12T23:51:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173890.75 epoch sec; seqNo:7492; 4058019.10 north; 591628.57 east 2021-02-12T23:51:31Z- AHRS_M2 msg: 1613173891.13 epoch sec; seqNo:7431 2021-02-12T23:51:31Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.13 psi 2021-02-12T23:51:31Z- handleDepth()-Depth_Keller msg: 1613173891136 epoch sec; seqNo:7493; depth 16.95 2021-02-12T23:51:31Z- handleDvl() - RDI_Pathfinder msg: 1613173891.16 epoch sec; seqNo:3457 2021-02-12T23:51:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.49 , 97.71 , 109.88 , 73.56 2021-02-12T23:51:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.06 , 0.01 2021-02-12T23:51:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173891.17 epoch sec; seqNo:7493; 4058019.10 north; 591628.57 east 2021-02-12T23:51:31Z- AHRS_M2 msg: 1613173891.53 epoch sec; seqNo:7432 2021-02-12T23:51:31Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.12 psi 2021-02-12T23:51:31Z- handleDepth()-Depth_Keller msg: 1613173891541 epoch sec; seqNo:7494; depth 16.81 2021-02-12T23:51:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173891.55 epoch sec; seqNo:7494; 4058019.47 north; 591628.57 east 2021-02-12T23:51:31Z- AHRS_M2 msg: 1613173891.94 epoch sec; seqNo:7433 2021-02-12T23:51:31Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.11 psi 2021-02-12T23:51:31Z- handleDepth()-Depth_Keller msg: 1613173891942 epoch sec; seqNo:7495; depth 16.72 2021-02-12T23:51:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173891.95 epoch sec; seqNo:7495; 4058019.85 north; 591628.57 east 2021-02-12T23:51:32Z- AHRS_M2 msg: 1613173892.34 epoch sec; seqNo:7434 2021-02-12T23:51:32Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.11 psi 2021-02-12T23:51:32Z- handleDepth()-Depth_Keller msg: 1613173892344 epoch sec; seqNo:7496; depth 16.60 2021-02-12T23:51:32Z- handleDvl() - RDI_Pathfinder msg: 1613173892.36 epoch sec; seqNo:3458 2021-02-12T23:51:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 97.71 , 111.10 , 73.96 2021-02-12T23:51:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.05 , -0.04 2021-02-12T23:51:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173892.37 epoch sec; seqNo:7496; 4058019.87 north; 591629.79 east 2021-02-12T23:51:32Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 131 calls... 2021-02-12T23:51:32Z- waiting for 1613173890.13, time is 1613173892.37 2021-02-12T23:51:32Z- LcmTrn::updateTrn() equating pose 1613173892.37 and meas times 1613173892.37 2021-02-12T23:51:32Z- LcmTrn::updateTrn() >>>> pose time 1613173892.37, meas time 1613173892.37 <<<< 2021-02-12T23:51:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173892.37 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:51:32Z- TerrainNav::motionUpdate() - currEstimate = (4058020, 591629.79, 16.60) 2021-02-12T23:51:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:51:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:51:32Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173892.37 sec.; No relevant navigation data available 2021-02-12T23:51:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:51:32Z- TNavBF::mmse selected from filter -1 2021-02-12T23:51:32Z- variances([0], [2], and psi_berg) = 732.378202 833.018325 0.000000 2021-02-12T23:51:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:51:32Z- LcmTrn::publishEstimates() - MLE : 4058012.37 591586.29 16.60 2021-02-12T23:51:32Z- LcmTrn::publishEstimates() - MMSE : 4058019.87 591629.79 16.60 2021-02-12T23:51:32Z- LcmTrn::publishEstimates() - COVAR: 732.38 833.02 1.38 0.00 2021-02-12T23:51:32Z- AHRS_M2 msg: 1613173892.74 epoch sec; seqNo:7435 2021-02-12T23:51:32Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.11 psi 2021-02-12T23:51:32Z- handleDepth()-Depth_Keller msg: 1613173892749 epoch sec; seqNo:7497; depth 16.49 2021-02-12T23:51:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173892.76 epoch sec; seqNo:7497; 4058020.25 north; 591629.78 east 2021-02-12T23:51:33Z- AHRS_M2 msg: 1613173893.15 epoch sec; seqNo:7436 2021-02-12T23:51:33Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.12 psi 2021-02-12T23:51:33Z- handleDepth()-Depth_Keller msg: 1613173893160 epoch sec; seqNo:7498; depth 16.38 2021-02-12T23:51:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173893.20 epoch sec; seqNo:7498; 4058020.62 north; 591629.78 east 2021-02-12T23:51:33Z- AHRS_M2 msg: 1613173893.55 epoch sec; seqNo:7437 2021-02-12T23:51:33Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.13 psi 2021-02-12T23:51:33Z- handleDepth()-Depth_Keller msg: 1613173893558 epoch sec; seqNo:7499; depth 16.27 2021-02-12T23:51:33Z- handleDvl() - RDI_Pathfinder msg: 1613173893.57 epoch sec; seqNo:3459 2021-02-12T23:51:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.26 , 97.65 , 108.60 , 78.18 2021-02-12T23:51:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , -0.10 2021-02-12T23:51:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173893.58 epoch sec; seqNo:7499; 4058020.62 north; 591629.78 east 2021-02-12T23:51:33Z- AHRS_M2 msg: 1613173893.96 epoch sec; seqNo:7438 2021-02-12T23:51:33Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.14 psi 2021-02-12T23:51:33Z- handleDepth()-Depth_Keller msg: 1613173893961 epoch sec; seqNo:7500; depth 16.15 2021-02-12T23:51:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173893.98 epoch sec; seqNo:7500; 4058021.00 north; 591629.77 east 2021-02-12T23:51:34Z- AHRS_M2 msg: 1613173894.36 epoch sec; seqNo:7439 2021-02-12T23:51:34Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.15 psi 2021-02-12T23:51:34Z- handleDepth()-Depth_Keller msg: 1613173894364 epoch sec; seqNo:7501; depth 16.04 2021-02-12T23:51:34Z- handleDvl() - RDI_Pathfinder msg: 1613173894.38 epoch sec; seqNo:3460 2021-02-12T23:51:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 97.71 , 108.53 , 71.92 2021-02-12T23:51:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.11 , -0.10 2021-02-12T23:51:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173894.40 epoch sec; seqNo:7501; 4058021.38 north; 591629.77 east 2021-02-12T23:51:34Z- AHRS_M2 msg: 1613173894.77 epoch sec; seqNo:7440 2021-02-12T23:51:34Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-12T23:51:34Z- handleDepth()-Depth_Keller msg: 1613173894772 epoch sec; seqNo:7502; depth 15.93 2021-02-12T23:51:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:51:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:51:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:51:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:51:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:51:34Z- handleDvl() - RDI_Pathfinder msg: 1613173894.77 epoch sec; seqNo:3461 2021-02-12T23:51:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:51:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.11 , -0.10 2021-02-12T23:51:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173894.79 epoch sec; seqNo:7502; 4058021.38 north; 591629.77 east 2021-02-12T23:51:35Z- AHRS_M2 msg: 1613173895.17 epoch sec; seqNo:7441 2021-02-12T23:51:35Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-12T23:51:35Z- handleDepth()-Depth_Keller msg: 1613173895174 epoch sec; seqNo:7503; depth 15.83 2021-02-12T23:51:35Z- handleDvl() - RDI_Pathfinder msg: 1613173895.19 epoch sec; seqNo:3462 2021-02-12T23:51:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 98.93 , 109.88 , 71.92 2021-02-12T23:51:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.18 , -0.05 2021-02-12T23:51:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173895.20 epoch sec; seqNo:7503; 4058021.77 north; 591630.99 east 2021-02-12T23:51:35Z- AHRS_M2 msg: 1613173895.57 epoch sec; seqNo:7442 2021-02-12T23:51:35Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.18 psi 2021-02-12T23:51:35Z- handleDepth()-Depth_Keller msg: 1613173895577 epoch sec; seqNo:7504; depth 15.70 2021-02-12T23:51:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173895.59 epoch sec; seqNo:7504; 4058022.15 north; 591630.98 east 2021-02-12T23:51:35Z- AHRS_M2 msg: 1613173895.98 epoch sec; seqNo:7443 2021-02-12T23:51:35Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.18 psi 2021-02-12T23:51:35Z- handleDepth()-Depth_Keller msg: 1613173895984 epoch sec; seqNo:7505; depth 15.61 2021-02-12T23:51:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:35Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:51:35Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:51:35Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:51:35Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:51:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:51:35Z- handleDvl() - RDI_Pathfinder msg: 1613173895.99 epoch sec; seqNo:3463 2021-02-12T23:51:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:51:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.18 , -0.05 2021-02-12T23:51:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173895.99 epoch sec; seqNo:7505; 4058022.53 north; 591630.98 east 2021-02-12T23:51:36Z- AHRS_M2 msg: 1613173896.38 epoch sec; seqNo:7444 2021-02-12T23:51:36Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-12T23:51:36Z- handleDepth()-Depth_Keller msg: 1613173896385 epoch sec; seqNo:7506; depth 15.48 2021-02-12T23:51:36Z- handleDvl() - RDI_Pathfinder msg: 1613173896.40 epoch sec; seqNo:3464 2021-02-12T23:51:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 100.14 , 109.88 , 78.24 2021-02-12T23:51:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.16 , 0.01 2021-02-12T23:51:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173896.41 epoch sec; seqNo:7506; 4058022.91 north; 591630.97 east 2021-02-12T23:51:36Z- AHRS_M2 msg: 1613173896.79 epoch sec; seqNo:7445 2021-02-12T23:51:36Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.18 psi 2021-02-12T23:51:36Z- handleDepth()-Depth_Keller msg: 1613173896853 epoch sec; seqNo:7507; depth 15.38 2021-02-12T23:51:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173896.86 epoch sec; seqNo:7507; 4058022.91 north; 591630.97 east 2021-02-12T23:51:37Z- AHRS_M2 msg: 1613173897.19 epoch sec; seqNo:7446 2021-02-12T23:51:37Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-12T23:51:37Z- handleDepth()-Depth_Keller msg: 1613173897221 epoch sec; seqNo:7508; depth 15.25 2021-02-12T23:51:37Z- handleDvl() - RDI_Pathfinder msg: 1613173897.24 epoch sec; seqNo:3465 2021-02-12T23:51:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 99.00 , 113.87 , 78.71 2021-02-12T23:51:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.14 , 0.03 2021-02-12T23:51:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173897.26 epoch sec; seqNo:7508; 4058023.29 north; 591630.97 east 2021-02-12T23:51:37Z- LcmTrn::handleMessages() - handling 43 messages took 0.72 ms and 129 calls... 2021-02-12T23:51:37Z- waiting for 1613173895.37, time is 1613173897.26 2021-02-12T23:51:37Z- LcmTrn::updateTrn() equating pose 1613173897.26 and meas times 1613173897.26 2021-02-12T23:51:37Z- LcmTrn::updateTrn() >>>> pose time 1613173897.26, meas time 1613173897.26 <<<< 2021-02-12T23:51:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173897.26 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:51:37Z- TerrainNav::motionUpdate() - currEstimate = (4058023, 591630.97, 15.25) 2021-02-12T23:51:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:51:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:51:37Z- TNavBF::Projecting Measurements 2021-02-12T23:51:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:51:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:51:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:51:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:51:37Z- TNavFilter::4 good beams. 2021-02-12T23:51:37Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:51:37Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:51:37Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:51:37Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:51:37Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:51:37Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:51:37Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:51:37Z- TNavBF::mmse selected from filter -1 2021-02-12T23:51:37Z- TerrainMapOctree: (vehN, vehE ) = (4058023.29, 591630.97). 2021-02-12T23:51:37Z- TerrainMapOctree: Min Distance = 4155.84. 2021-02-12T23:51:37Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:51:37Z- There is no specific good beam that all particles have in common. 2021-02-12T23:51:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:51:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.57 K: 0.110 ExpectedMeasDiff: nan currMeas.covariance: 10.96 2021-02-12T23:51:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:51:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 15.68 2021-02-12T23:51:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:51:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 20.75 2021-02-12T23:51:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:51:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 9.91 2021-02-12T23:51:37Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:51:37Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:51:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:51:37Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:51:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:51:37Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:51:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:51:37Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:51:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:51:37Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:51:37Z- Filter 0 No beams used in subcloud update 2021-02-12T23:51:37Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:51:37Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:51:37Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 732.3 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:51:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173897.26 sec. 2021-02-12T23:51:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:51:37Z- TNavBF::mmse selected from filter -1 2021-02-12T23:51:37Z- variances([0], [2], and psi_berg) = 732.304029 833.018270 0.000000 2021-02-12T23:51:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:51:37Z- LcmTrn::publishEstimates() - MLE : 4058015.79 591587.47 15.25 2021-02-12T23:51:37Z- LcmTrn::publishEstimates() - MMSE : 4058023.29 591630.97 15.25 2021-02-12T23:51:37Z- LcmTrn::publishEstimates() - COVAR: 732.30 833.02 1.11 0.00 2021-02-12T23:51:37Z- AHRS_M2 msg: 1613173897.59 epoch sec; seqNo:7447 2021-02-12T23:51:37Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-12T23:51:37Z- handleDepth()-Depth_Keller msg: 1613173897674 epoch sec; seqNo:7509; depth 15.15 2021-02-12T23:51:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173897.69 epoch sec; seqNo:7509; 4058023.67 north; 591630.97 east 2021-02-12T23:51:37Z- AHRS_M2 msg: 1613173898.00 epoch sec; seqNo:7448 2021-02-12T23:51:37Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-12T23:51:38Z- handleDepth()-Depth_Keller msg: 1613173898053 epoch sec; seqNo:7510; depth 15.03 2021-02-12T23:51:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173898.07 epoch sec; seqNo:7510; 4058024.05 north; 591630.96 east 2021-02-12T23:51:38Z- AHRS_M2 msg: 1613173898.40 epoch sec; seqNo:7449 2021-02-12T23:51:38Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-12T23:51:38Z- handleDepth()-Depth_Keller msg: 1613173898425 epoch sec; seqNo:7511; depth 14.93 2021-02-12T23:51:38Z- handleDvl() - RDI_Pathfinder msg: 1613173898.44 epoch sec; seqNo:3466 2021-02-12T23:51:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.71 , 97.58 , 111.10 , 74.37 2021-02-12T23:51:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.14 , 0.04 2021-02-12T23:51:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173898.45 epoch sec; seqNo:7511; 4058024.05 north; 591630.96 east 2021-02-12T23:51:38Z- AHRS_M2 msg: 1613173898.80 epoch sec; seqNo:7450 2021-02-12T23:51:38Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-12T23:51:38Z- handleDepth()-Depth_Keller msg: 1613173898830 epoch sec; seqNo:7512; depth 14.83 2021-02-12T23:51:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173898.84 epoch sec; seqNo:7512; 4058024.44 north; 591632.18 east 2021-02-12T23:51:39Z- AHRS_M2 msg: 1613173899.21 epoch sec; seqNo:7451 2021-02-12T23:51:39Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-12T23:51:39Z- handleDepth()-Depth_Keller msg: 1613173899217 epoch sec; seqNo:7513; depth 14.72 2021-02-12T23:51:39Z- handleDvl() - RDI_Pathfinder msg: 1613173899.23 epoch sec; seqNo:3467 2021-02-12T23:51:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.86 , 111.03 , 75.19 2021-02-12T23:51:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.13 , 0.09 2021-02-12T23:51:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173899.25 epoch sec; seqNo:7513; 4058024.82 north; 591632.18 east 2021-02-12T23:51:39Z- AHRS_M2 msg: 1613173899.61 epoch sec; seqNo:7452 2021-02-12T23:51:39Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-12T23:51:39Z- handleDepth()-Depth_Keller msg: 1613173899618 epoch sec; seqNo:7514; depth 14.61 2021-02-12T23:51:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:51:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:51:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:51:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:51:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:51:39Z- handleDvl() - RDI_Pathfinder msg: 1613173899.62 epoch sec; seqNo:3468 2021-02-12T23:51:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:51:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.13 , 0.09 2021-02-12T23:51:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173899.63 epoch sec; seqNo:7514; 4058025.20 north; 591632.17 east 2021-02-12T23:51:40Z- AHRS_M2 msg: 1613173900.02 epoch sec; seqNo:7453 2021-02-12T23:51:40Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-12T23:51:40Z- handleDepth()-Depth_Keller msg: 1613173900026 epoch sec; seqNo:7515; depth 14.50 2021-02-12T23:51:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173900.04 epoch sec; seqNo:7515; 4058025.20 north; 591632.17 east 2021-02-12T23:51:40Z- AHRS_M2 msg: 1613173900.42 epoch sec; seqNo:7454 2021-02-12T23:51:40Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-12T23:51:40Z- handleDepth()-Depth_Keller msg: 1613173900424 epoch sec; seqNo:7516; depth 14.38 2021-02-12T23:51:40Z- handleDvl() - RDI_Pathfinder msg: 1613173900.43 epoch sec; seqNo:3469 2021-02-12T23:51:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.57 , 98.79 , 110.96 , 74.37 2021-02-12T23:51:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , 0.15 2021-02-12T23:51:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173900.46 epoch sec; seqNo:7516; 4058025.58 north; 591632.17 east 2021-02-12T23:51:40Z- AHRS_M2 msg: 1613173900.82 epoch sec; seqNo:7455 2021-02-12T23:51:40Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-12T23:51:40Z- handleDepth()-Depth_Keller msg: 1613173900829 epoch sec; seqNo:7517; depth 14.27 2021-02-12T23:51:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:51:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:51:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:51:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:51:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:51:40Z- handleDvl() - RDI_Pathfinder msg: 1613173900.83 epoch sec; seqNo:3470 2021-02-12T23:51:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:51:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , 0.15 2021-02-12T23:51:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173900.84 epoch sec; seqNo:7517; 4058025.95 north; 591632.16 east 2021-02-12T23:51:41Z- AHRS_M2 msg: 1613173901.23 epoch sec; seqNo:7456 2021-02-12T23:51:41Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-12T23:51:41Z- handleDepth()-Depth_Keller msg: 1613173901234 epoch sec; seqNo:7518; depth 14.15 2021-02-12T23:51:41Z- handleDvl() - RDI_Pathfinder msg: 1613173901.25 epoch sec; seqNo:3471 2021-02-12T23:51:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.26 , 97.65 , 113.46 , 78.18 2021-02-12T23:51:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , 0.15 2021-02-12T23:51:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173901.26 epoch sec; seqNo:7518; 4058026.33 north; 591632.16 east 2021-02-12T23:51:41Z- AHRS_M2 msg: 1613173901.63 epoch sec; seqNo:7457 2021-02-12T23:51:41Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-12T23:51:41Z- handleDepth()-Depth_Keller msg: 1613173901642 epoch sec; seqNo:7519; depth 14.05 2021-02-12T23:51:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173901.65 epoch sec; seqNo:7519; 4058026.73 north; 591633.38 east 2021-02-12T23:51:42Z- AHRS_M2 msg: 1613173902.04 epoch sec; seqNo:7458 2021-02-12T23:51:42Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-12T23:51:42Z- handleDepth()-Depth_Keller msg: 1613173902046 epoch sec; seqNo:7520; depth 13.91 2021-02-12T23:51:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:51:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:51:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:51:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:51:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:51:42Z- handleDvl() - RDI_Pathfinder msg: 1613173902.05 epoch sec; seqNo:3472 2021-02-12T23:51:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:51:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , 0.15 2021-02-12T23:51:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173902.07 epoch sec; seqNo:7520; 4058026.73 north; 591633.38 east 2021-02-12T23:51:42Z- AHRS_M2 msg: 1613173902.44 epoch sec; seqNo:7459 2021-02-12T23:51:42Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-12T23:51:42Z- handleDepth()-Depth_Keller msg: 1613173902445 epoch sec; seqNo:7521; depth 13.80 2021-02-12T23:51:42Z- handleDvl() - RDI_Pathfinder msg: 1613173902.46 epoch sec; seqNo:3473 2021-02-12T23:51:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 98.93 , 113.80 , 76.42 2021-02-12T23:51:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , 0.09 2021-02-12T23:51:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173902.47 epoch sec; seqNo:7521; 4058027.10 north; 591633.37 east 2021-02-12T23:51:42Z- LcmTrn::handleMessages() - handling 47 messages took 0.69 ms and 134 calls... 2021-02-12T23:51:42Z- waiting for 1613173900.26, time is 1613173902.47 2021-02-12T23:51:42Z- LcmTrn::updateTrn() equating pose 1613173902.47 and meas times 1613173902.47 2021-02-12T23:51:42Z- LcmTrn::updateTrn() >>>> pose time 1613173902.47, meas time 1613173902.47 <<<< 2021-02-12T23:51:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173902.47 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:51:42Z- TerrainNav::motionUpdate() - currEstimate = (4058027, 591633.37, 13.80) 2021-02-12T23:51:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:51:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:51:42Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173902.47 sec.; No relevant navigation data available 2021-02-12T23:51:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:51:42Z- TNavBF::mmse selected from filter -1 2021-02-12T23:51:42Z- variances([0], [2], and psi_berg) = 732.221357 833.018208 0.000000 2021-02-12T23:51:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:51:42Z- LcmTrn::publishEstimates() - MLE : 4058019.60 591589.87 13.80 2021-02-12T23:51:42Z- LcmTrn::publishEstimates() - MMSE : 4058027.10 591633.37 13.80 2021-02-12T23:51:42Z- LcmTrn::publishEstimates() - COVAR: 732.22 833.02 1.34 0.00 2021-02-12T23:51:42Z- AHRS_M2 msg: 1613173902.84 epoch sec; seqNo:7460 2021-02-12T23:51:42Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-12T23:51:42Z- handleDepth()-Depth_Keller msg: 1613173902849 epoch sec; seqNo:7522; depth 13.69 2021-02-12T23:51:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173902.86 epoch sec; seqNo:7522; 4058027.48 north; 591633.37 east 2021-02-12T23:51:43Z- AHRS_M2 msg: 1613173903.25 epoch sec; seqNo:7461 2021-02-12T23:51:43Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-12T23:51:43Z- handleDepth()-Depth_Keller msg: 1613173903254 epoch sec; seqNo:7523; depth 13.57 2021-02-12T23:51:43Z- handleDvl() - RDI_Pathfinder msg: 1613173903.27 epoch sec; seqNo:3474 2021-02-12T23:51:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 101.63 , 111.10 , 78.65 2021-02-12T23:51:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.05 , 0.04 2021-02-12T23:51:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173903.28 epoch sec; seqNo:7523; 4058027.86 north; 591633.36 east 2021-02-12T23:51:43Z- AHRS_M2 msg: 1613173903.65 epoch sec; seqNo:7462 2021-02-12T23:51:43Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-12T23:51:43Z- handleDepth()-Depth_Keller msg: 1613173903657 epoch sec; seqNo:7524; depth 13.46 2021-02-12T23:51:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173903.68 epoch sec; seqNo:7524; 4058027.86 north; 591633.36 east 2021-02-12T23:51:44Z- AHRS_M2 msg: 1613173904.06 epoch sec; seqNo:7463 2021-02-12T23:51:44Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-12T23:51:44Z- handleDepth()-Depth_Keller msg: 1613173904064 epoch sec; seqNo:7525; depth 13.35 2021-02-12T23:51:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173904.11 epoch sec; seqNo:7525; 4058028.25 north; 591634.58 east 2021-02-12T23:51:44Z- AHRS_M2 msg: 1613173904.46 epoch sec; seqNo:7464 2021-02-12T23:51:44Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-12T23:51:44Z- handleDepth()-Depth_Keller msg: 1613173904469 epoch sec; seqNo:7526; depth 13.22 2021-02-12T23:51:44Z- handleDvl() - RDI_Pathfinder msg: 1613173904.48 epoch sec; seqNo:3475 2021-02-12T23:51:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.21 , 115.09 , 78.58 2021-02-12T23:51:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.10 , 0.00 2021-02-12T23:51:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173904.49 epoch sec; seqNo:7526; 4058028.63 north; 591634.58 east 2021-02-12T23:51:44Z- AHRS_M2 msg: 1613173904.87 epoch sec; seqNo:7465 2021-02-12T23:51:44Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-12T23:51:44Z- handleDepth()-Depth_Keller msg: 1613173904870 epoch sec; seqNo:7527; depth 13.12 2021-02-12T23:51:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173904.88 epoch sec; seqNo:7527; 4058029.01 north; 591634.57 east 2021-02-12T23:51:45Z- AHRS_M2 msg: 1613173905.27 epoch sec; seqNo:7466 2021-02-12T23:51:45Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-12T23:51:45Z- handleDepth()-Depth_Keller msg: 1613173905283 epoch sec; seqNo:7528; depth 12.99 2021-02-12T23:51:45Z- handleDvl() - RDI_Pathfinder msg: 1613173905.30 epoch sec; seqNo:3476 2021-02-12T23:51:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.23 , 113.40 , 78.25 2021-02-12T23:51:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.10 , -0.08 2021-02-12T23:51:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173905.31 epoch sec; seqNo:7528; 4058029.01 north; 591634.57 east 2021-02-12T23:51:45Z- AHRS_M2 msg: 1613173905.67 epoch sec; seqNo:7467 2021-02-12T23:51:45Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-12T23:51:45Z- handleDepth()-Depth_Keller msg: 1613173905677 epoch sec; seqNo:7529; depth 12.89 2021-02-12T23:51:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:51:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:51:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:51:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:51:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:51:45Z- handleDvl() - RDI_Pathfinder msg: 1613173905.68 epoch sec; seqNo:3477 2021-02-12T23:51:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:51:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.10 , -0.08 2021-02-12T23:51:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173905.69 epoch sec; seqNo:7529; 4058029.39 north; 591634.57 east 2021-02-12T23:51:46Z- AHRS_M2 msg: 1613173906.08 epoch sec; seqNo:7468 2021-02-12T23:51:46Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-12T23:51:46Z- handleDepth()-Depth_Keller msg: 1613173906081 epoch sec; seqNo:7530; depth 12.78 2021-02-12T23:51:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173906.09 epoch sec; seqNo:7530; 4058029.78 north; 591635.79 east 2021-02-12T23:51:46Z- AHRS_M2 msg: 1613173906.48 epoch sec; seqNo:7469 2021-02-12T23:51:46Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.15 psi 2021-02-12T23:51:46Z- handleDepth()-Depth_Keller msg: 1613173906489 epoch sec; seqNo:7531; depth 12.67 2021-02-12T23:51:46Z- handleDvl() - RDI_Pathfinder msg: 1613173906.51 epoch sec; seqNo:3478 2021-02-12T23:51:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.42 , 101.30 , 120.22 , 76.83 2021-02-12T23:51:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.08 , -0.19 2021-02-12T23:51:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173906.52 epoch sec; seqNo:7531; 4058029.78 north; 591635.79 east 2021-02-12T23:51:46Z- AHRS_M2 msg: 1613173906.88 epoch sec; seqNo:7470 2021-02-12T23:51:46Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.15 psi 2021-02-12T23:51:46Z- handleDepth()-Depth_Keller msg: 1613173906903 epoch sec; seqNo:7532; depth 12.55 2021-02-12T23:51:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:51:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:51:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:51:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:51:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:51:46Z- handleDvl() - RDI_Pathfinder msg: 1613173906.90 epoch sec; seqNo:3479 2021-02-12T23:51:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:51:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.08 , -0.19 2021-02-12T23:51:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173906.92 epoch sec; seqNo:7532; 4058030.16 north; 591635.78 east 2021-02-12T23:51:47Z- AHRS_M2 msg: 1613173907.30 epoch sec; seqNo:7471 2021-02-12T23:51:47Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.15 psi 2021-02-12T23:51:47Z- handleDepth()-Depth_Keller msg: 1613173907318 epoch sec; seqNo:7533; depth 12.44 2021-02-12T23:51:47Z- handleDvl() - RDI_Pathfinder msg: 1613173907.33 epoch sec; seqNo:3480 2021-02-12T23:51:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 102.78 , 117.65 , 78.45 2021-02-12T23:51:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.01 , -0.25 2021-02-12T23:51:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173907.34 epoch sec; seqNo:7533; 4058030.16 north; 591635.78 east 2021-02-12T23:51:47Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-12T23:51:47Z- waiting for 1613173905.47, time is 1613173907.34 2021-02-12T23:51:47Z- LcmTrn::updateTrn() equating pose 1613173907.34 and meas times 1613173907.34 2021-02-12T23:51:47Z- LcmTrn::updateTrn() >>>> pose time 1613173907.34, meas time 1613173907.34 <<<< 2021-02-12T23:51:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173907.34 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:51:47Z- TerrainNav::motionUpdate() - currEstimate = (4058030, 591635.78, 12.44) 2021-02-12T23:51:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:51:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:51:47Z- TNavBF::Projecting Measurements 2021-02-12T23:51:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:51:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:51:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:51:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:51:47Z- TNavFilter::4 good beams. 2021-02-12T23:51:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:51:47Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:51:47Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:51:47Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:51:47Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:51:47Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:51:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:51:47Z- TNavBF::mmse selected from filter -1 2021-02-12T23:51:47Z- TerrainMapOctree: (vehN, vehE ) = (4058030.16, 591635.78). 2021-02-12T23:51:47Z- TerrainMapOctree: Min Distance = 4147.47. 2021-02-12T23:51:47Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:51:47Z- There is no specific good beam that all particles have in common. 2021-02-12T23:51:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:51:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.53 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 11.99 2021-02-12T23:51:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:51:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 16.90 2021-02-12T23:51:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:51:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 22.15 2021-02-12T23:51:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:51:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-12T23:51:47Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:51:47Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:51:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:51:47Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:51:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:51:47Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:51:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:51:47Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:51:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:51:47Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:51:47Z- Filter 0 No beams used in subcloud update 2021-02-12T23:51:47Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:51:47Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:51:47Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 732.1 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:51:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173907.34 sec. 2021-02-12T23:51:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:51:47Z- TNavBF::mmse selected from filter -1 2021-02-12T23:51:47Z- variances([0], [2], and psi_berg) = 732.144610 833.018150 0.000000 2021-02-12T23:51:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:51:47Z- LcmTrn::publishEstimates() - MLE : 4058022.66 591592.28 12.44 2021-02-12T23:51:47Z- LcmTrn::publishEstimates() - MMSE : 4058030.16 591635.78 12.44 2021-02-12T23:51:47Z- LcmTrn::publishEstimates() - COVAR: 732.14 833.02 1.10 0.00 2021-02-12T23:51:47Z- AHRS_M2 msg: 1613173907.70 epoch sec; seqNo:7472 2021-02-12T23:51:47Z- AHRS_M2 msg: 0.68 phi; 0.34 theta; -0.15 psi 2021-02-12T23:51:47Z- handleDepth()-Depth_Keller msg: 1613173907701 epoch sec; seqNo:7534; depth 12.29 2021-02-12T23:51:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173907.71 epoch sec; seqNo:7534; 4058030.54 north; 591635.78 east 2021-02-12T23:51:48Z- AHRS_M2 msg: 1613173908.10 epoch sec; seqNo:7473 2021-02-12T23:51:48Z- AHRS_M2 msg: 0.68 phi; 0.33 theta; -0.15 psi 2021-02-12T23:51:48Z- handleDepth()-Depth_Keller msg: 1613173908102 epoch sec; seqNo:7535; depth 12.19 2021-02-12T23:51:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:51:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:51:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:51:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:51:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:51:48Z- handleDvl() - RDI_Pathfinder msg: 1613173908.11 epoch sec; seqNo:3481 2021-02-12T23:51:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:51:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.01 , -0.25 2021-02-12T23:51:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173908.11 epoch sec; seqNo:7535; 4058030.54 north; 591635.78 east 2021-02-12T23:51:48Z- AHRS_M2 msg: 1613173908.50 epoch sec; seqNo:7474 2021-02-12T23:51:48Z- AHRS_M2 msg: 0.68 phi; 0.33 theta; -0.16 psi 2021-02-12T23:51:48Z- handleDepth()-Depth_Keller msg: 1613173908505 epoch sec; seqNo:7536; depth 12.06 2021-02-12T23:51:48Z- handleDvl() - RDI_Pathfinder msg: 1613173908.52 epoch sec; seqNo:3482 2021-02-12T23:51:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.21 , 101.43 , 117.65 , 78.45 2021-02-12T23:51:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.19 , -0.20 2021-02-12T23:51:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173908.53 epoch sec; seqNo:7536; 4058030.92 north; 591635.77 east 2021-02-12T23:51:48Z- AHRS_M2 msg: 1613173908.90 epoch sec; seqNo:7475 2021-02-12T23:51:48Z- AHRS_M2 msg: 0.68 phi; 0.33 theta; -0.16 psi 2021-02-12T23:51:48Z- handleDepth()-Depth_Keller msg: 1613173908913 epoch sec; seqNo:7537; depth 11.93 2021-02-12T23:51:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173908.92 epoch sec; seqNo:7537; 4058031.30 north; 591635.77 east 2021-02-12T23:51:49Z- AHRS_M2 msg: 1613173909.31 epoch sec; seqNo:7476 2021-02-12T23:51:49Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.16 psi 2021-02-12T23:51:49Z- handleDepth()-Depth_Keller msg: 1613173909313 epoch sec; seqNo:7538; depth 11.82 2021-02-12T23:51:49Z- handleDvl() - RDI_Pathfinder msg: 1613173909.33 epoch sec; seqNo:3483 2021-02-12T23:51:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.78 , 114.95 , 78.45 2021-02-12T23:51:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.32 , -0.02 2021-02-12T23:51:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173909.34 epoch sec; seqNo:7538; 4058031.68 north; 591635.76 east 2021-02-12T23:51:49Z- AHRS_M2 msg: 1613173909.71 epoch sec; seqNo:7477 2021-02-12T23:51:49Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.16 psi 2021-02-12T23:51:49Z- handleDepth()-Depth_Keller msg: 1613173909719 epoch sec; seqNo:7539; depth 11.69 2021-02-12T23:51:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173909.73 epoch sec; seqNo:7539; 4058031.69 north; 591636.99 east 2021-02-12T23:51:50Z- AHRS_M2 msg: 1613173910.12 epoch sec; seqNo:7478 2021-02-12T23:51:50Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.17 psi 2021-02-12T23:51:50Z- handleDepth()-Depth_Keller msg: 1613173910121 epoch sec; seqNo:7540; depth 11.58 2021-02-12T23:51:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173910.14 epoch sec; seqNo:7540; 4058032.07 north; 591636.98 east 2021-02-12T23:51:50Z- AHRS_M2 msg: 1613173910.52 epoch sec; seqNo:7479 2021-02-12T23:51:50Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.17 psi 2021-02-12T23:51:50Z- handleDepth()-Depth_Keller msg: 1613173910647 epoch sec; seqNo:7541; depth 11.46 2021-02-12T23:51:50Z- handleDvl() - RDI_Pathfinder msg: 1613173910.66 epoch sec; seqNo:3484 2021-02-12T23:51:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 102.58 , 116.10 , 78.25 2021-02-12T23:51:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.30 , 0.21 2021-02-12T23:51:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173910.68 epoch sec; seqNo:7541; 4058032.45 north; 591636.98 east 2021-02-12T23:51:50Z- AHRS_M2 msg: 1613173910.92 epoch sec; seqNo:7480 2021-02-12T23:51:50Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-12T23:51:50Z- handleDepth()-Depth_Keller msg: 1613173910945 epoch sec; seqNo:7542; depth 11.29 2021-02-12T23:51:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173910.96 epoch sec; seqNo:7542; 4058032.83 north; 591636.97 east 2021-02-12T23:51:51Z- AHRS_M2 msg: 1613173911.33 epoch sec; seqNo:7481 2021-02-12T23:51:51Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-12T23:51:51Z- handleDepth()-Depth_Keller msg: 1613173911369 epoch sec; seqNo:7543; depth 11.23 2021-02-12T23:51:51Z- handleDvl() - RDI_Pathfinder msg: 1613173911.38 epoch sec; seqNo:3485 2021-02-12T23:51:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.50 , 116.37 , 78.51 2021-02-12T23:51:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.16 , 0.33 2021-02-12T23:51:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173911.39 epoch sec; seqNo:7543; 4058033.20 north; 591636.97 east 2021-02-12T23:51:51Z- AHRS_M2 msg: 1613173911.73 epoch sec; seqNo:7482 2021-02-12T23:51:51Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-12T23:51:51Z- handleDepth()-Depth_Keller msg: 1613173911769 epoch sec; seqNo:7544; depth 11.11 2021-02-12T23:51:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:51:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:51:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:51:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:51:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:51:51Z- handleDvl() - RDI_Pathfinder msg: 1613173911.77 epoch sec; seqNo:3486 2021-02-12T23:51:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:51:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.16 , 0.33 2021-02-12T23:51:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173911.79 epoch sec; seqNo:7544; 4058033.58 north; 591636.97 east 2021-02-12T23:51:52Z- AHRS_M2 msg: 1613173912.14 epoch sec; seqNo:7483 2021-02-12T23:51:52Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-12T23:51:52Z- handleDepth()-Depth_Keller msg: 1613173912149 epoch sec; seqNo:7545; depth 10.99 2021-02-12T23:51:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173912.16 epoch sec; seqNo:7545; 4058033.96 north; 591636.96 east 2021-02-12T23:51:52Z- AHRS_M2 msg: 1613173912.54 epoch sec; seqNo:7484 2021-02-12T23:51:52Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-12T23:51:52Z- handleDepth()-Depth_Keller msg: 1613173912546 epoch sec; seqNo:7546; depth 10.89 2021-02-12T23:51:52Z- handleDvl() - RDI_Pathfinder msg: 1613173912.56 epoch sec; seqNo:3487 2021-02-12T23:51:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.29 , 103.86 , 117.39 , 79.53 2021-02-12T23:51:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.05 , 0.29 2021-02-12T23:51:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173912.57 epoch sec; seqNo:7546; 4058033.96 north; 591636.96 east 2021-02-12T23:51:52Z- LcmTrn::handleMessages() - handling 46 messages took 0.74 ms and 131 calls... 2021-02-12T23:51:52Z- waiting for 1613173910.34, time is 1613173912.57 2021-02-12T23:51:52Z- LcmTrn::updateTrn() equating pose 1613173912.57 and meas times 1613173912.57 2021-02-12T23:51:52Z- LcmTrn::updateTrn() >>>> pose time 1613173912.57, meas time 1613173912.57 <<<< 2021-02-12T23:51:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173912.57 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:51:52Z- TerrainNav::motionUpdate() - currEstimate = (4058034, 591636.96, 10.89) 2021-02-12T23:51:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:51:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:51:52Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173912.57 sec.; No relevant navigation data available 2021-02-12T23:51:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:51:52Z- TNavBF::mmse selected from filter -1 2021-02-12T23:51:52Z- variances([0], [2], and psi_berg) = 732.067068 833.018092 0.000000 2021-02-12T23:51:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:51:52Z- LcmTrn::publishEstimates() - MLE : 4058026.46 591599.46 10.89 2021-02-12T23:51:52Z- LcmTrn::publishEstimates() - MMSE : 4058033.96 591636.96 10.89 2021-02-12T23:51:52Z- LcmTrn::publishEstimates() - COVAR: 732.07 833.02 1.31 0.00 2021-02-12T23:51:52Z- AHRS_M2 msg: 1613173912.94 epoch sec; seqNo:7485 2021-02-12T23:51:52Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-12T23:51:52Z- handleDepth()-Depth_Keller msg: 1613173912948 epoch sec; seqNo:7547; depth 10.78 2021-02-12T23:51:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:52Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:51:52Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:51:52Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:51:52Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:51:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:51:52Z- handleDvl() - RDI_Pathfinder msg: 1613173912.95 epoch sec; seqNo:3488 2021-02-12T23:51:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:51:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.05 , 0.29 2021-02-12T23:51:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173912.97 epoch sec; seqNo:7547; 4058034.35 north; 591638.18 east 2021-02-12T23:51:53Z- AHRS_M2 msg: 1613173913.35 epoch sec; seqNo:7486 2021-02-12T23:51:53Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-12T23:51:53Z- handleDepth()-Depth_Keller msg: 1613173913352 epoch sec; seqNo:7548; depth 10.68 2021-02-12T23:51:53Z- handleDvl() - RDI_Pathfinder msg: 1613173913.37 epoch sec; seqNo:3489 2021-02-12T23:51:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.84 , 102.72 , 113.53 , 0.00 2021-02-12T23:51:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.24 , 0.14 2021-02-12T23:51:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173913.39 epoch sec; seqNo:7548; 4058034.73 north; 591638.17 east 2021-02-12T23:51:53Z- AHRS_M2 msg: 1613173913.75 epoch sec; seqNo:7487 2021-02-12T23:51:53Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-12T23:51:53Z- handleDepth()-Depth_Keller msg: 1613173913761 epoch sec; seqNo:7549; depth 10.58 2021-02-12T23:51:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173913.77 epoch sec; seqNo:7549; 4058035.11 north; 591638.17 east 2021-02-12T23:51:54Z- AHRS_M2 msg: 1613173914.16 epoch sec; seqNo:7488 2021-02-12T23:51:54Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-12T23:51:54Z- handleDepth()-Depth_Keller msg: 1613173914163 epoch sec; seqNo:7550; depth 10.49 2021-02-12T23:51:54Z- handleDvl() - RDI_Pathfinder msg: 1613173914.17 epoch sec; seqNo:3490 2021-02-12T23:51:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 103.93 , 117.32 , 79.28 2021-02-12T23:51:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.29 , -0.06 2021-02-12T23:51:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173914.18 epoch sec; seqNo:7550; 4058035.11 north; 591638.17 east 2021-02-12T23:51:54Z- AHRS_M2 msg: 1613173914.56 epoch sec; seqNo:7489 2021-02-12T23:51:54Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.18 psi 2021-02-12T23:51:54Z- handleDepth()-Depth_Keller msg: 1613173914574 epoch sec; seqNo:7551; depth 10.39 2021-02-12T23:51:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173914.59 epoch sec; seqNo:7551; 4058035.50 north; 591639.39 east 2021-02-12T23:51:54Z- AHRS_M2 msg: 1613173914.96 epoch sec; seqNo:7490 2021-02-12T23:51:54Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-12T23:51:54Z- handleDepth()-Depth_Keller msg: 1613173914973 epoch sec; seqNo:7552; depth 10.28 2021-02-12T23:51:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173914.98 epoch sec; seqNo:7552; 4058035.50 north; 591639.39 east 2021-02-12T23:51:55Z- AHRS_M2 msg: 1613173915.37 epoch sec; seqNo:7491 2021-02-12T23:51:55Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.19 psi 2021-02-12T23:51:55Z- handleDepth()-Depth_Keller msg: 1613173915372 epoch sec; seqNo:7553; depth 10.19 2021-02-12T23:51:55Z- handleDvl() - RDI_Pathfinder msg: 1613173915.38 epoch sec; seqNo:3491 2021-02-12T23:51:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.07 , 114.88 , 81.08 2021-02-12T23:51:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.15 , -0.20 2021-02-12T23:51:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173915.39 epoch sec; seqNo:7553; 4058035.88 north; 591639.38 east 2021-02-12T23:51:55Z- AHRS_M2 msg: 1613173915.77 epoch sec; seqNo:7492 2021-02-12T23:51:55Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.19 psi 2021-02-12T23:51:55Z- handleDepth()-Depth_Keller msg: 1613173915785 epoch sec; seqNo:7554; depth 10.07 2021-02-12T23:51:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173915.81 epoch sec; seqNo:7554; 4058035.88 north; 591639.38 east 2021-02-12T23:51:56Z- AHRS_M2 msg: 1613173916.18 epoch sec; seqNo:7493 2021-02-12T23:51:56Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.18 psi 2021-02-12T23:51:56Z- handleDepth()-Depth_Keller msg: 1613173916182 epoch sec; seqNo:7555; depth 9.97 2021-02-12T23:51:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173916.20 epoch sec; seqNo:7555; 4058036.26 north; 591639.38 east 2021-02-12T23:51:56Z- AHRS_M2 msg: 1613173916.58 epoch sec; seqNo:7494 2021-02-12T23:51:56Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.17 psi 2021-02-12T23:51:56Z- handleDepth()-Depth_Keller msg: 1613173916584 epoch sec; seqNo:7556; depth 9.87 2021-02-12T23:51:56Z- handleDvl() - RDI_Pathfinder msg: 1613173916.60 epoch sec; seqNo:3492 2021-02-12T23:51:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 106.50 , 117.32 , 80.81 2021-02-12T23:51:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.02 , -0.19 2021-02-12T23:51:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173916.62 epoch sec; seqNo:7556; 4058036.27 north; 591640.60 east 2021-02-12T23:51:56Z- AHRS_M2 msg: 1613173916.98 epoch sec; seqNo:7495 2021-02-12T23:51:56Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.16 psi 2021-02-12T23:51:56Z- handleDepth()-Depth_Keller msg: 1613173916993 epoch sec; seqNo:7557; depth 9.78 2021-02-12T23:51:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173917.01 epoch sec; seqNo:7557; 4058036.65 north; 591640.60 east 2021-02-12T23:51:57Z- AHRS_M2 msg: 1613173917.39 epoch sec; seqNo:7496 2021-02-12T23:51:57Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.16 psi 2021-02-12T23:51:57Z- handleDepth()-Depth_Keller msg: 1613173917401 epoch sec; seqNo:7558; depth 9.68 2021-02-12T23:51:57Z- handleDvl() - RDI_Pathfinder msg: 1613173917.41 epoch sec; seqNo:3493 2021-02-12T23:51:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.28 , 116.10 , 80.95 2021-02-12T23:51:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.16 , -0.11 2021-02-12T23:51:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173917.42 epoch sec; seqNo:7558; 4058037.03 north; 591640.59 east 2021-02-12T23:51:57Z- AHRS_M2 msg: 1613173917.80 epoch sec; seqNo:7497 2021-02-12T23:51:57Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.16 psi 2021-02-12T23:51:57Z- LcmTrn::handleMessages() - handling 43 messages took 0.66 ms and 130 calls... 2021-02-12T23:51:57Z- waiting for 1613173915.57, time is 1613173917.80 2021-02-12T23:51:57Z- LcmTrn::updateTrn() >>>> pose time 1613173917.80, meas time 1613173917.41 <<<< 2021-02-12T23:51:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173917.41 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:51:57Z- TerrainNav::motionUpdate() - currEstimate = (4058037, 591640.59, 9.68) 2021-02-12T23:51:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:51:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:51:57Z- TNavBF::Projecting Measurements 2021-02-12T23:51:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:51:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:51:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:51:57Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:51:57Z- TNavFilter::3 good beams. 2021-02-12T23:51:57Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:51:57Z- TNavBF:: beamIndex[0] = 1 2021-02-12T23:51:57Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:51:57Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:51:57Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:51:57Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:51:57Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:51:57Z- TNavBF::mmse selected from filter -1 2021-02-12T23:51:57Z- TerrainMapOctree: (vehN, vehE ) = (4058036.81, 591640.32). 2021-02-12T23:51:57Z- TerrainMapOctree: Min Distance = 4139.45. 2021-02-12T23:51:57Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:51:57Z- There is no specific good beam that all particles have in common. 2021-02-12T23:51:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:51:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.53 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 17.74 2021-02-12T23:51:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:51:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.42 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 21.57 2021-02-12T23:51:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:51:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 10.48 2021-02-12T23:51:57Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:51:57Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:51:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:51:57Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:51:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:51:57Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:51:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:51:57Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:51:57Z- Filter 0 No beams used in subcloud update 2021-02-12T23:51:57Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:51:57Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:51:57Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 732.0 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:51:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173917.41 sec. 2021-02-12T23:51:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:51:57Z- TNavBF::mmse selected from filter -1 2021-02-12T23:51:57Z- variances([0], [2], and psi_berg) = 731.962652 833.018014 0.000000 2021-02-12T23:51:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:51:57Z- LcmTrn::publishEstimates() - MLE : 4058029.53 591603.09 9.68 2021-02-12T23:51:57Z- LcmTrn::publishEstimates() - MMSE : 4058037.03 591640.59 9.68 2021-02-12T23:51:57Z- LcmTrn::publishEstimates() - COVAR: 731.96 833.02 1.22 0.00 2021-02-12T23:51:57Z- handleDepth()-Depth_Keller msg: 1613173917824 epoch sec; seqNo:7559; depth 9.57 2021-02-12T23:51:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:51:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:51:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:51:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:51:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:51:57Z- handleDvl() - RDI_Pathfinder msg: 1613173917.83 epoch sec; seqNo:3494 2021-02-12T23:51:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:51:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.16 , -0.11 2021-02-12T23:51:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173917.84 epoch sec; seqNo:7559; 4058037.41 north; 591640.59 east 2021-02-12T23:51:58Z- AHRS_M2 msg: 1613173918.20 epoch sec; seqNo:7498 2021-02-12T23:51:58Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-12T23:51:58Z- handleDepth()-Depth_Keller msg: 1613173918200 epoch sec; seqNo:7560; depth 9.47 2021-02-12T23:51:58Z- handleDvl() - RDI_Pathfinder msg: 1613173918.21 epoch sec; seqNo:3495 2021-02-12T23:51:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.51 , 103.73 , 117.25 , 0.00 2021-02-12T23:51:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.16 , -0.03 2021-02-12T23:51:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173918.23 epoch sec; seqNo:7560; 4058037.41 north; 591640.59 east 2021-02-12T23:51:58Z- AHRS_M2 msg: 1613173918.60 epoch sec; seqNo:7499 2021-02-12T23:51:58Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:51:58Z- handleDepth()-Depth_Keller msg: 1613173918609 epoch sec; seqNo:7561; depth 9.38 2021-02-12T23:51:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173918.62 epoch sec; seqNo:7561; 4058037.79 north; 591640.58 east 2021-02-12T23:51:59Z- AHRS_M2 msg: 1613173919.01 epoch sec; seqNo:7500 2021-02-12T23:51:59Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.18 psi 2021-02-12T23:51:59Z- handleDepth()-Depth_Keller msg: 1613173919010 epoch sec; seqNo:7562; depth 9.28 2021-02-12T23:51:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:51:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:51:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:51:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:51:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:51:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:51:59Z- handleDvl() - RDI_Pathfinder msg: 1613173919.01 epoch sec; seqNo:3496 2021-02-12T23:51:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:51:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.16 , -0.03 2021-02-12T23:51:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173919.02 epoch sec; seqNo:7562; 4058038.17 north; 591640.58 east 2021-02-12T23:51:59Z- AHRS_M2 msg: 1613173919.41 epoch sec; seqNo:7501 2021-02-12T23:51:59Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-12T23:51:59Z- handleDepth()-Depth_Keller msg: 1613173919413 epoch sec; seqNo:7563; depth 9.19 2021-02-12T23:51:59Z- handleDvl() - RDI_Pathfinder msg: 1613173919.43 epoch sec; seqNo:3497 2021-02-12T23:51:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.44 , 103.80 , 117.18 , 81.49 2021-02-12T23:51:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , 0.01 2021-02-12T23:51:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173919.44 epoch sec; seqNo:7563; 4058038.55 north; 591640.58 east 2021-02-12T23:51:59Z- AHRS_M2 msg: 1613173919.82 epoch sec; seqNo:7502 2021-02-12T23:51:59Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-12T23:51:59Z- handleDepth()-Depth_Keller msg: 1613173919820 epoch sec; seqNo:7564; depth 9.07 2021-02-12T23:51:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173919.83 epoch sec; seqNo:7564; 4058038.56 north; 591641.80 east 2021-02-12T23:52:00Z- AHRS_M2 msg: 1613173920.24 epoch sec; seqNo:7503 2021-02-12T23:52:00Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-12T23:52:00Z- handleDepth()-Depth_Keller msg: 1613173920243 epoch sec; seqNo:7565; depth 9.00 2021-02-12T23:52:00Z- handleDvl() - RDI_Pathfinder msg: 1613173920.26 epoch sec; seqNo:3498 2021-02-12T23:52:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 104.00 , 118.87 , 81.02 2021-02-12T23:52:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.11 , 0.01 2021-02-12T23:52:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173920.27 epoch sec; seqNo:7565; 4058038.94 north; 591641.79 east 2021-02-12T23:52:00Z- AHRS_M2 msg: 1613173920.62 epoch sec; seqNo:7504 2021-02-12T23:52:00Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-12T23:52:00Z- handleDepth()-Depth_Keller msg: 1613173920626 epoch sec; seqNo:7566; depth 8.89 2021-02-12T23:52:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173920.64 epoch sec; seqNo:7566; 4058039.32 north; 591641.79 east 2021-02-12T23:52:01Z- AHRS_M2 msg: 1613173921.02 epoch sec; seqNo:7505 2021-02-12T23:52:01Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-12T23:52:01Z- handleDepth()-Depth_Keller msg: 1613173921029 epoch sec; seqNo:7567; depth 8.81 2021-02-12T23:52:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173921.05 epoch sec; seqNo:7567; 4058039.32 north; 591641.79 east 2021-02-12T23:52:01Z- AHRS_M2 msg: 1613173921.43 epoch sec; seqNo:7506 2021-02-12T23:52:01Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-12T23:52:01Z- handleDepth()-Depth_Keller msg: 1613173921435 epoch sec; seqNo:7568; depth 8.70 2021-02-12T23:52:01Z- handleDvl() - RDI_Pathfinder msg: 1613173921.45 epoch sec; seqNo:3499 2021-02-12T23:52:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 102.44 , 122.72 , 0.00 2021-02-12T23:52:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.23 , -0.13 2021-02-12T23:52:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173921.47 epoch sec; seqNo:7568; 4058039.70 north; 591641.78 east 2021-02-12T23:52:01Z- AHRS_M2 msg: 1613173921.83 epoch sec; seqNo:7507 2021-02-12T23:52:01Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-12T23:52:01Z- handleDepth()-Depth_Keller msg: 1613173921838 epoch sec; seqNo:7569; depth 8.58 2021-02-12T23:52:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173921.85 epoch sec; seqNo:7569; 4058039.70 north; 591641.78 east 2021-02-12T23:52:02Z- AHRS_M2 msg: 1613173922.24 epoch sec; seqNo:7508 2021-02-12T23:52:02Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.18 psi 2021-02-12T23:52:02Z- handleDepth()-Depth_Keller msg: 1613173922242 epoch sec; seqNo:7570; depth 8.49 2021-02-12T23:52:02Z- handleDvl() - RDI_Pathfinder msg: 1613173922.25 epoch sec; seqNo:3500 2021-02-12T23:52:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 103.66 , 120.02 , 81.36 2021-02-12T23:52:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , 0.05 , -0.27 2021-02-12T23:52:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173922.27 epoch sec; seqNo:7570; 4058040.09 north; 591643.00 east 2021-02-12T23:52:02Z- AHRS_M2 msg: 1613173922.64 epoch sec; seqNo:7509 2021-02-12T23:52:02Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.19 psi 2021-02-12T23:52:02Z- handleDepth()-Depth_Keller msg: 1613173922649 epoch sec; seqNo:7571; depth 8.36 2021-02-12T23:52:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:02Z- handleDvl() - RDI_Pathfinder msg: 1613173922.65 epoch sec; seqNo:3501 2021-02-12T23:52:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , 0.05 , -0.27 2021-02-12T23:52:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173922.67 epoch sec; seqNo:7571; 4058040.09 north; 591643.00 east 2021-02-12T23:52:02Z- LcmTrn::handleMessages() - handling 46 messages took 0.70 ms and 132 calls... 2021-02-12T23:52:02Z- waiting for 1613173920.80, time is 1613173922.67 2021-02-12T23:52:02Z- LcmTrn::updateTrn() equating pose 1613173922.67 and meas times 1613173922.67 2021-02-12T23:52:02Z- LcmTrn::updateTrn() >>>> pose time 1613173922.67, meas time 1613173922.67 <<<< 2021-02-12T23:52:02Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173922.67, ping # 0. 2021-02-12T23:52:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173922.67 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:52:02Z- TerrainNav::motionUpdate() - currEstimate = (4058040, 591643.00, 8.36) 2021-02-12T23:52:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:52:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:52:02Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:52:02Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:52:02Z- TNavBF::Projecting Measurements 2021-02-12T23:52:02Z- TNavFilter::Measurement from time = 1613173922.67 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:52:02Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:52:02Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:52:02Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:52:02Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:52:02Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:52:02Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:52:02Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:52:02Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 731.9 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-12T23:52:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:52:02Z- TNavBF::mmse selected from filter -1 2021-02-12T23:52:02Z- variances([0], [2], and psi_berg) = 731.886142 833.017957 0.000000 2021-02-12T23:52:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:52:02Z- LcmTrn::publishEstimates() - MLE : 4058032.59 591605.50 8.36 2021-02-12T23:52:02Z- LcmTrn::publishEstimates() - MMSE : 4058040.09 591643.00 8.36 2021-02-12T23:52:02Z- LcmTrn::publishEstimates() - COVAR: 731.89 833.02 1.42 0.00 2021-02-12T23:52:03Z- AHRS_M2 msg: 1613173923.04 epoch sec; seqNo:7510 2021-02-12T23:52:03Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.21 psi 2021-02-12T23:52:03Z- handleDepth()-Depth_Keller msg: 1613173923049 epoch sec; seqNo:7572; depth 8.26 2021-02-12T23:52:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173923.06 epoch sec; seqNo:7572; 4058040.47 north; 591643.00 east 2021-02-12T23:52:03Z- AHRS_M2 msg: 1613173923.45 epoch sec; seqNo:7511 2021-02-12T23:52:03Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.21 psi 2021-02-12T23:52:03Z- handleDepth()-Depth_Keller msg: 1613173923456 epoch sec; seqNo:7573; depth 8.14 2021-02-12T23:52:03Z- handleDvl() - RDI_Pathfinder msg: 1613173923.47 epoch sec; seqNo:3502 2021-02-12T23:52:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 107.79 , 114.54 , 82.10 2021-02-12T23:52:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.31 , -0.12 2021-02-12T23:52:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173923.48 epoch sec; seqNo:7573; 4058040.47 north; 591643.00 east 2021-02-12T23:52:03Z- AHRS_M2 msg: 1613173923.85 epoch sec; seqNo:7512 2021-02-12T23:52:03Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.21 psi 2021-02-12T23:52:03Z- handleDepth()-Depth_Keller msg: 1613173923929 epoch sec; seqNo:7574; depth 8.01 2021-02-12T23:52:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:03Z- handleDvl() - RDI_Pathfinder msg: 1613173923.93 epoch sec; seqNo:3503 2021-02-12T23:52:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.31 , -0.12 2021-02-12T23:52:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173923.94 epoch sec; seqNo:7574; 4058040.85 north; 591642.99 east 2021-02-12T23:52:04Z- AHRS_M2 msg: 1613173924.26 epoch sec; seqNo:7513 2021-02-12T23:52:04Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.21 psi 2021-02-12T23:52:04Z- handleDepth()-Depth_Keller msg: 1613173924356 epoch sec; seqNo:7575; depth 7.88 2021-02-12T23:52:04Z- handleDvl() - RDI_Pathfinder msg: 1613173924.37 epoch sec; seqNo:3504 2021-02-12T23:52:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 110.49 , 115.90 , 0.00 2021-02-12T23:52:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.36 , 0.28 2021-02-12T23:52:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173924.39 epoch sec; seqNo:7575; 4058041.23 north; 591642.99 east 2021-02-12T23:52:04Z- AHRS_M2 msg: 1613173924.67 epoch sec; seqNo:7514 2021-02-12T23:52:04Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.20 psi 2021-02-12T23:52:04Z- handleDepth()-Depth_Keller msg: 1613173924693 epoch sec; seqNo:7576; depth 7.74 2021-02-12T23:52:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173924.72 epoch sec; seqNo:7576; 4058041.60 north; 591642.98 east 2021-02-12T23:52:05Z- AHRS_M2 msg: 1613173925.07 epoch sec; seqNo:7515 2021-02-12T23:52:05Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.19 psi 2021-02-12T23:52:05Z- handleDepth()-Depth_Keller msg: 1613173925105 epoch sec; seqNo:7577; depth 7.65 2021-02-12T23:52:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:05Z- handleDvl() - RDI_Pathfinder msg: 1613173925.11 epoch sec; seqNo:3505 2021-02-12T23:52:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.36 , 0.28 2021-02-12T23:52:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173925.13 epoch sec; seqNo:7577; 4058041.98 north; 591642.98 east 2021-02-12T23:52:05Z- AHRS_M2 msg: 1613173925.47 epoch sec; seqNo:7516 2021-02-12T23:52:05Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.19 psi 2021-02-12T23:52:05Z- handleDepth()-Depth_Keller msg: 1613173925493 epoch sec; seqNo:7578; depth 7.54 2021-02-12T23:52:05Z- handleDvl() - RDI_Pathfinder msg: 1613173925.51 epoch sec; seqNo:3506 2021-02-12T23:52:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.52 , 106.37 , 118.26 , 82.57 2021-02-12T23:52:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.19 , 0.52 2021-02-12T23:52:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173925.52 epoch sec; seqNo:7578; 4058042.36 north; 591642.98 east 2021-02-12T23:52:05Z- AHRS_M2 msg: 1613173925.87 epoch sec; seqNo:7517 2021-02-12T23:52:05Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.18 psi 2021-02-12T23:52:05Z- handleDepth()-Depth_Keller msg: 1613173925876 epoch sec; seqNo:7579; depth 7.42 2021-02-12T23:52:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173925.89 epoch sec; seqNo:7579; 4058042.74 north; 591642.97 east 2021-02-12T23:52:06Z- AHRS_M2 msg: 1613173926.29 epoch sec; seqNo:7518 2021-02-12T23:52:06Z- AHRS_M2 msg: 0.56 phi; 0.37 theta; -0.17 psi 2021-02-12T23:52:06Z- handleDepth()-Depth_Keller msg: 1613173926291 epoch sec; seqNo:7580; depth 7.30 2021-02-12T23:52:06Z- handleDvl() - RDI_Pathfinder msg: 1613173926.30 epoch sec; seqNo:3507 2021-02-12T23:52:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 107.92 , 122.79 , 82.23 2021-02-12T23:52:06Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , 0.22 , 0.44 2021-02-12T23:52:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173926.31 epoch sec; seqNo:7580; 4058043.13 north; 591644.19 east 2021-02-12T23:52:06Z- AHRS_M2 msg: 1613173926.68 epoch sec; seqNo:7519 2021-02-12T23:52:06Z- AHRS_M2 msg: 0.56 phi; 0.38 theta; -0.17 psi 2021-02-12T23:52:06Z- handleDepth()-Depth_Keller msg: 1613173926686 epoch sec; seqNo:7581; depth 7.19 2021-02-12T23:52:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173926.70 epoch sec; seqNo:7581; 4058043.51 north; 591644.19 east 2021-02-12T23:52:07Z- AHRS_M2 msg: 1613173927.08 epoch sec; seqNo:7520 2021-02-12T23:52:07Z- AHRS_M2 msg: 0.57 phi; 0.38 theta; -0.16 psi 2021-02-12T23:52:07Z- handleDepth()-Depth_Keller msg: 1613173927093 epoch sec; seqNo:7582; depth 7.08 2021-02-12T23:52:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173927.11 epoch sec; seqNo:7582; 4058043.89 north; 591644.18 east 2021-02-12T23:52:07Z- AHRS_M2 msg: 1613173927.49 epoch sec; seqNo:7521 2021-02-12T23:52:07Z- AHRS_M2 msg: 0.58 phi; 0.39 theta; -0.15 psi 2021-02-12T23:52:07Z- handleDepth()-Depth_Keller msg: 1613173927497 epoch sec; seqNo:7583; depth 6.97 2021-02-12T23:52:07Z- handleDvl() - RDI_Pathfinder msg: 1613173927.51 epoch sec; seqNo:3508 2021-02-12T23:52:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 108.33 , 127.66 , 83.04 2021-02-12T23:52:07Z- handleDvl() - RDI_Pathfinder msg: velocities 1.08 , 0.47 , 0.04 2021-02-12T23:52:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173927.53 epoch sec; seqNo:7583; 4058043.90 north; 591645.40 east 2021-02-12T23:52:07Z- LcmTrn::handleMessages() - handling 43 messages took 0.84 ms and 126 calls... 2021-02-12T23:52:07Z- waiting for 1613173925.67, time is 1613173927.53 2021-02-12T23:52:07Z- LcmTrn::updateTrn() equating pose 1613173927.53 and meas times 1613173927.53 2021-02-12T23:52:07Z- LcmTrn::updateTrn() >>>> pose time 1613173927.53, meas time 1613173927.53 <<<< 2021-02-12T23:52:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173927.53 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:52:07Z- TerrainNav::motionUpdate() - currEstimate = (4058044, 591645.40, 6.97) 2021-02-12T23:52:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:52:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:52:07Z- TNavBF::Projecting Measurements 2021-02-12T23:52:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:52:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:52:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:52:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:52:07Z- TNavFilter::4 good beams. 2021-02-12T23:52:07Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:52:07Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:52:07Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:52:07Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:52:07Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:52:07Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:52:07Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:52:07Z- TNavBF::mmse selected from filter -1 2021-02-12T23:52:07Z- TerrainMapOctree: (vehN, vehE ) = (4058043.90, 591645.40). 2021-02-12T23:52:07Z- TerrainMapOctree: Min Distance = 4130.74. 2021-02-12T23:52:07Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:52:07Z- There is no specific good beam that all particles have in common. 2021-02-12T23:52:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:52:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.65 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-12T23:52:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:52:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.51 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 18.78 2021-02-12T23:52:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:52:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 26.07 2021-02-12T23:52:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:52:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 11.03 2021-02-12T23:52:07Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:52:07Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:52:07Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:52:07Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:52:07Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:52:07Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:52:07Z- Filter 0 No beams used in subcloud update 2021-02-12T23:52:07Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:52:07Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:52:07Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 731.8 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:52:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173927.53 sec. 2021-02-12T23:52:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:52:07Z- TNavBF::mmse selected from filter -1 2021-02-12T23:52:07Z- variances([0], [2], and psi_berg) = 731.803883 833.017896 0.000000 2021-02-12T23:52:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:52:07Z- LcmTrn::publishEstimates() - MLE : 4058036.40 591607.90 6.97 2021-02-12T23:52:07Z- LcmTrn::publishEstimates() - MMSE : 4058043.90 591645.40 6.97 2021-02-12T23:52:07Z- LcmTrn::publishEstimates() - COVAR: 731.80 833.02 1.21 0.00 2021-02-12T23:52:07Z- AHRS_M2 msg: 1613173927.89 epoch sec; seqNo:7522 2021-02-12T23:52:07Z- AHRS_M2 msg: 0.59 phi; 0.39 theta; -0.15 psi 2021-02-12T23:52:07Z- handleDepth()-Depth_Keller msg: 1613173927898 epoch sec; seqNo:7584; depth 6.84 2021-02-12T23:52:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173927.91 epoch sec; seqNo:7584; 4058044.28 north; 591645.40 east 2021-02-12T23:52:08Z- AHRS_M2 msg: 1613173928.30 epoch sec; seqNo:7523 2021-02-12T23:52:08Z- AHRS_M2 msg: 0.60 phi; 0.38 theta; -0.16 psi 2021-02-12T23:52:08Z- handleDepth()-Depth_Keller msg: 1613173928302 epoch sec; seqNo:7585; depth 6.72 2021-02-12T23:52:08Z- handleDvl() - RDI_Pathfinder msg: 1613173928.32 epoch sec; seqNo:3509 2021-02-12T23:52:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.74 , 106.64 , 125.97 , 82.84 2021-02-12T23:52:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.37 , -0.34 2021-02-12T23:52:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173928.33 epoch sec; seqNo:7585; 4058044.28 north; 591645.40 east 2021-02-12T23:52:08Z- AHRS_M2 msg: 1613173928.70 epoch sec; seqNo:7524 2021-02-12T23:52:08Z- AHRS_M2 msg: 0.60 phi; 0.38 theta; -0.17 psi 2021-02-12T23:52:08Z- handleDepth()-Depth_Keller msg: 1613173928704 epoch sec; seqNo:7586; depth 6.60 2021-02-12T23:52:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:08Z- handleDvl() - RDI_Pathfinder msg: 1613173928.71 epoch sec; seqNo:3510 2021-02-12T23:52:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.37 , -0.34 2021-02-12T23:52:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173928.72 epoch sec; seqNo:7586; 4058044.66 north; 591645.39 east 2021-02-12T23:52:09Z- AHRS_M2 msg: 1613173929.10 epoch sec; seqNo:7525 2021-02-12T23:52:09Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.17 psi 2021-02-12T23:52:09Z- handleDepth()-Depth_Keller msg: 1613173929108 epoch sec; seqNo:7587; depth 6.46 2021-02-12T23:52:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173929.12 epoch sec; seqNo:7587; 4058044.67 north; 591646.61 east 2021-02-12T23:52:09Z- AHRS_M2 msg: 1613173929.51 epoch sec; seqNo:7526 2021-02-12T23:52:09Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.17 psi 2021-02-12T23:52:09Z- handleDepth()-Depth_Keller msg: 1613173929521 epoch sec; seqNo:7588; depth 6.32 2021-02-12T23:52:09Z- handleDvl() - RDI_Pathfinder msg: 1613173929.53 epoch sec; seqNo:3511 2021-02-12T23:52:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 108.33 , 129.15 , 83.04 2021-02-12T23:52:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.03 , -0.35 2021-02-12T23:52:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173929.56 epoch sec; seqNo:7588; 4058045.05 north; 591646.61 east 2021-02-12T23:52:09Z- AHRS_M2 msg: 1613173929.92 epoch sec; seqNo:7527 2021-02-12T23:52:09Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.17 psi 2021-02-12T23:52:09Z- handleDepth()-Depth_Keller msg: 1613173929940 epoch sec; seqNo:7589; depth 6.16 2021-02-12T23:52:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:09Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:09Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:09Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:09Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:09Z- handleDvl() - RDI_Pathfinder msg: 1613173929.94 epoch sec; seqNo:3512 2021-02-12T23:52:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.03 , -0.35 2021-02-12T23:52:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173929.95 epoch sec; seqNo:7589; 4058045.05 north; 591646.61 east 2021-02-12T23:52:10Z- AHRS_M2 msg: 1613173930.32 epoch sec; seqNo:7528 2021-02-12T23:52:10Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.15 psi 2021-02-12T23:52:10Z- handleDepth()-Depth_Keller msg: 1613173930320 epoch sec; seqNo:7590; depth 6.05 2021-02-12T23:52:10Z- handleDvl() - RDI_Pathfinder msg: 1613173930.34 epoch sec; seqNo:3513 2021-02-12T23:52:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 109.61 , 125.97 , 82.84 2021-02-12T23:52:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.09 , -0.04 2021-02-12T23:52:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173930.35 epoch sec; seqNo:7590; 4058045.43 north; 591646.61 east 2021-02-12T23:52:10Z- AHRS_M2 msg: 1613173930.72 epoch sec; seqNo:7529 2021-02-12T23:52:10Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.15 psi 2021-02-12T23:52:10Z- handleDepth()-Depth_Keller msg: 1613173930724 epoch sec; seqNo:7591; depth 5.90 2021-02-12T23:52:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173930.73 epoch sec; seqNo:7591; 4058045.43 north; 591646.61 east 2021-02-12T23:52:11Z- AHRS_M2 msg: 1613173931.13 epoch sec; seqNo:7530 2021-02-12T23:52:11Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.14 psi 2021-02-12T23:52:11Z- handleDepth()-Depth_Keller msg: 1613173931145 epoch sec; seqNo:7592; depth 5.76 2021-02-12T23:52:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:11Z- handleDvl() - RDI_Pathfinder msg: 1613173931.15 epoch sec; seqNo:3514 2021-02-12T23:52:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.09 , -0.04 2021-02-12T23:52:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173931.19 epoch sec; seqNo:7592; 4058045.81 north; 591646.60 east 2021-02-12T23:52:11Z- AHRS_M2 msg: 1613173931.53 epoch sec; seqNo:7531 2021-02-12T23:52:11Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.16 psi 2021-02-12T23:52:11Z- handleDepth()-Depth_Keller msg: 1613173931539 epoch sec; seqNo:7593; depth 5.60 2021-02-12T23:52:11Z- handleDvl() - RDI_Pathfinder msg: 1613173931.55 epoch sec; seqNo:3515 2021-02-12T23:52:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 109.68 , 127.52 , 0.00 2021-02-12T23:52:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , 0.08 2021-02-12T23:52:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173931.56 epoch sec; seqNo:7593; 4058046.19 north; 591646.60 east 2021-02-12T23:52:11Z- AHRS_M2 msg: 1613173931.93 epoch sec; seqNo:7532 2021-02-12T23:52:11Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.18 psi 2021-02-12T23:52:11Z- handleDepth()-Depth_Keller msg: 1613173931936 epoch sec; seqNo:7594; depth 5.47 2021-02-12T23:52:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173931.95 epoch sec; seqNo:7594; 4058046.19 north; 591646.60 east 2021-02-12T23:52:12Z- AHRS_M2 msg: 1613173932.34 epoch sec; seqNo:7533 2021-02-12T23:52:12Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.21 psi 2021-02-12T23:52:12Z- handleDepth()-Depth_Keller msg: 1613173932340 epoch sec; seqNo:7595; depth 5.32 2021-02-12T23:52:12Z- handleDvl() - RDI_Pathfinder msg: 1613173932.35 epoch sec; seqNo:3516 2021-02-12T23:52:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.78 , 109.14 , 128.47 , 0.00 2021-02-12T23:52:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.19 , -0.01 2021-02-12T23:52:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173932.36 epoch sec; seqNo:7595; 4058046.57 north; 591646.59 east 2021-02-12T23:52:12Z- AHRS_M2 msg: 1613173932.74 epoch sec; seqNo:7534 2021-02-12T23:52:12Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.23 psi 2021-02-12T23:52:12Z- handleDepth()-Depth_Keller msg: 1613173932753 epoch sec; seqNo:7596; depth 5.15 2021-02-12T23:52:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173932.77 epoch sec; seqNo:7596; 4058046.96 north; 591647.81 east 2021-02-12T23:52:12Z- LcmTrn::handleMessages() - handling 47 messages took 0.70 ms and 133 calls... 2021-02-12T23:52:12Z- waiting for 1613173930.53, time is 1613173932.77 2021-02-12T23:52:12Z- LcmTrn::updateTrn() >>>> pose time 1613173932.77, meas time 1613173932.35 <<<< 2021-02-12T23:52:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173932.35 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:52:12Z- TerrainNav::motionUpdate() - currEstimate = (4058047, 591647.81, 5.15) 2021-02-12T23:52:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:52:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:52:12Z- TNavBF::Projecting Measurements 2021-02-12T23:52:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:52:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:52:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:52:12Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:52:12Z- TNavFilter::3 good beams. 2021-02-12T23:52:12Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:52:12Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:52:12Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:52:12Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:52:12Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:52:12Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:52:12Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:52:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:52:12Z- TerrainMapOctree: (vehN, vehE ) = (4058046.72, 591647.62). 2021-02-12T23:52:12Z- TerrainMapOctree: Min Distance = 4127.15. 2021-02-12T23:52:12Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:52:12Z- There is no specific good beam that all particles have in common. 2021-02-12T23:52:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:52:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.39 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 13.77 2021-02-12T23:52:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:52:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 19.06 2021-02-12T23:52:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:52:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 26.41 2021-02-12T23:52:13Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:52:13Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:52:13Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:52:13Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:52:13Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:52:13Z- Filter 0 No beams used in subcloud update 2021-02-12T23:52:13Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:52:13Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:52:13Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 731.7 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:52:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173932.35 sec. 2021-02-12T23:52:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:52:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:52:13Z- variances([0], [2], and psi_berg) = 731.709180 833.017825 0.000000 2021-02-12T23:52:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:52:13Z- LcmTrn::publishEstimates() - MLE : 4058039.46 591600.31 5.15 2021-02-12T23:52:13Z- LcmTrn::publishEstimates() - MMSE : 4058046.96 591647.81 5.15 2021-02-12T23:52:13Z- LcmTrn::publishEstimates() - COVAR: 731.71 833.02 1.17 0.00 2021-02-12T23:52:13Z- AHRS_M2 msg: 1613173933.15 epoch sec; seqNo:7535 2021-02-12T23:52:13Z- AHRS_M2 msg: 0.65 phi; 0.29 theta; -0.23 psi 2021-02-12T23:52:13Z- handleDepth()-Depth_Keller msg: 1613173933156 epoch sec; seqNo:7597; depth 5.00 2021-02-12T23:52:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173933.16 epoch sec; seqNo:7597; 4058047.34 north; 591647.81 east 2021-02-12T23:52:13Z- AHRS_M2 msg: 1613173933.55 epoch sec; seqNo:7536 2021-02-12T23:52:13Z- AHRS_M2 msg: 0.65 phi; 0.24 theta; -0.22 psi 2021-02-12T23:52:13Z- handleDepth()-Depth_Keller msg: 1613173933553 epoch sec; seqNo:7598; depth 4.86 2021-02-12T23:52:13Z- handleDvl() - RDI_Pathfinder msg: 1613173933.58 epoch sec; seqNo:3517 2021-02-12T23:52:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.80 , 113.60 , 124.01 , 161.19 2021-02-12T23:52:13Z- handleDvl() - RDI_Pathfinder msg: velocities 1.10 , -0.22 , 0.06 2021-02-12T23:52:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173933.60 epoch sec; seqNo:7598; 4058047.72 north; 591647.80 east 2021-02-12T23:52:13Z- AHRS_M2 msg: 1613173933.95 epoch sec; seqNo:7537 2021-02-12T23:52:13Z- AHRS_M2 msg: 0.66 phi; 0.19 theta; -0.19 psi 2021-02-12T23:52:13Z- handleDepth()-Depth_Keller msg: 1613173933957 epoch sec; seqNo:7599; depth 4.71 2021-02-12T23:52:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173933.97 epoch sec; seqNo:7599; 4058048.10 north; 591647.80 east 2021-02-12T23:52:14Z- AHRS_M2 msg: 1613173934.37 epoch sec; seqNo:7538 2021-02-12T23:52:14Z- AHRS_M2 msg: 0.67 phi; 0.14 theta; -0.17 psi 2021-02-12T23:52:14Z- handleDepth()-Depth_Keller msg: 1613173934376 epoch sec; seqNo:7600; depth 4.59 2021-02-12T23:52:14Z- handleDvl() - RDI_Pathfinder msg: 1613173934.39 epoch sec; seqNo:3518 2021-02-12T23:52:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 110.62 , 112.11 , 97.24 2021-02-12T23:52:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.03 , 0.03 2021-02-12T23:52:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173934.41 epoch sec; seqNo:7600; 4058048.48 north; 591647.80 east 2021-02-12T23:52:14Z- AHRS_M2 msg: 1613173934.76 epoch sec; seqNo:7539 2021-02-12T23:52:14Z- AHRS_M2 msg: 0.68 phi; 0.09 theta; -0.16 psi 2021-02-12T23:52:14Z- handleDepth()-Depth_Keller msg: 1613173934767 epoch sec; seqNo:7601; depth 4.47 2021-02-12T23:52:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:14Z- handleDvl() - RDI_Pathfinder msg: 1613173934.77 epoch sec; seqNo:3519 2021-02-12T23:52:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.03 , 0.03 2021-02-12T23:52:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173934.78 epoch sec; seqNo:7601; 4058048.48 north; 591647.80 east 2021-02-12T23:52:15Z- AHRS_M2 msg: 1613173935.16 epoch sec; seqNo:7540 2021-02-12T23:52:15Z- AHRS_M2 msg: 0.69 phi; 0.05 theta; -0.16 psi 2021-02-12T23:52:15Z- handleDepth()-Depth_Keller msg: 1613173935173 epoch sec; seqNo:7602; depth 4.35 2021-02-12T23:52:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173935.19 epoch sec; seqNo:7602; 4058048.87 north; 591649.01 east 2021-02-12T23:52:15Z- AHRS_M2 msg: 1613173935.57 epoch sec; seqNo:7541 2021-02-12T23:52:15Z- AHRS_M2 msg: 0.71 phi; 0.00 theta; -0.17 psi 2021-02-12T23:52:15Z- handleDepth()-Depth_Keller msg: 1613173935572 epoch sec; seqNo:7603; depth 4.27 2021-02-12T23:52:15Z- handleDvl() - RDI_Pathfinder msg: 1613173935.58 epoch sec; seqNo:3520 2021-02-12T23:52:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 102.18 , 99.20 , 97.71 2021-02-12T23:52:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.00 , 0.02 2021-02-12T23:52:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173935.61 epoch sec; seqNo:7603; 4058049.25 north; 591649.01 east 2021-02-12T23:52:16Z- AHRS_M2 msg: 1613173936.00 epoch sec; seqNo:7542 2021-02-12T23:52:16Z- AHRS_M2 msg: 0.72 phi; -0.04 theta; -0.18 psi 2021-02-12T23:52:16Z- handleDepth()-Depth_Keller msg: 1613173936025 epoch sec; seqNo:7604; depth 4.21 2021-02-12T23:52:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:16Z- handleDvl() - RDI_Pathfinder msg: 1613173936.03 epoch sec; seqNo:3521 2021-02-12T23:52:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.00 , 0.02 2021-02-12T23:52:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173936.04 epoch sec; seqNo:7604; 4058049.25 north; 591649.01 east 2021-02-12T23:52:16Z- AHRS_M2 msg: 1613173936.38 epoch sec; seqNo:7543 2021-02-12T23:52:16Z- AHRS_M2 msg: 0.73 phi; -0.08 theta; -0.18 psi 2021-02-12T23:52:16Z- handleDepth()-Depth_Keller msg: 1613173936382 epoch sec; seqNo:7605; depth 4.15 2021-02-12T23:52:16Z- handleDvl() - RDI_Pathfinder msg: 1613173936.39 epoch sec; seqNo:3522 2021-02-12T23:52:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 103.86 , 97.11 , 97.11 2021-02-12T23:52:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.06 , 0.08 2021-02-12T23:52:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173936.41 epoch sec; seqNo:7605; 4058049.63 north; 591649.00 east 2021-02-12T23:52:16Z- AHRS_M2 msg: 1613173936.78 epoch sec; seqNo:7544 2021-02-12T23:52:16Z- AHRS_M2 msg: 0.74 phi; -0.12 theta; -0.19 psi 2021-02-12T23:52:16Z- handleDepth()-Depth_Keller msg: 1613173936785 epoch sec; seqNo:7606; depth 4.12 2021-02-12T23:52:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173936.80 epoch sec; seqNo:7606; 4058049.63 north; 591649.00 east 2021-02-12T23:52:17Z- AHRS_M2 msg: 1613173937.18 epoch sec; seqNo:7545 2021-02-12T23:52:17Z- AHRS_M2 msg: 0.74 phi; -0.16 theta; -0.18 psi 2021-02-12T23:52:17Z- handleDepth()-Depth_Keller msg: 1613173937193 epoch sec; seqNo:7607; depth 4.10 2021-02-12T23:52:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:17Z- handleDvl() - RDI_Pathfinder msg: 1613173937.19 epoch sec; seqNo:3523 2021-02-12T23:52:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.06 , 0.08 2021-02-12T23:52:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173937.20 epoch sec; seqNo:7607; 4058050.00 north; 591649.00 east 2021-02-12T23:52:17Z- AHRS_M2 msg: 1613173937.59 epoch sec; seqNo:7546 2021-02-12T23:52:17Z- AHRS_M2 msg: 0.75 phi; -0.19 theta; -0.18 psi 2021-02-12T23:52:17Z- handleDepth()-Depth_Keller msg: 1613173937720 epoch sec; seqNo:7608; depth 4.11 2021-02-12T23:52:17Z- handleDvl() - RDI_Pathfinder msg: 1613173937.73 epoch sec; seqNo:3524 2021-02-12T23:52:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 106.77 , 97.31 , 104.07 2021-02-12T23:52:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , -0.02 , 0.07 2021-02-12T23:52:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173937.75 epoch sec; seqNo:7608; 4058050.40 north; 591650.22 east 2021-02-12T23:52:17Z- AHRS_M2 msg: 1613173937.99 epoch sec; seqNo:7547 2021-02-12T23:52:17Z- AHRS_M2 msg: 0.74 phi; -0.22 theta; -0.19 psi 2021-02-12T23:52:18Z- handleDepth()-Depth_Keller msg: 1613173938013 epoch sec; seqNo:7609; depth 4.12 2021-02-12T23:52:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173938.02 epoch sec; seqNo:7609; 4058050.40 north; 591650.22 east 2021-02-12T23:52:18Z- LcmTrn::handleMessages() - handling 47 messages took 0.74 ms and 133 calls... 2021-02-12T23:52:18Z- waiting for 1613173935.77, time is 1613173938.02 2021-02-12T23:52:18Z- LcmTrn::updateTrn() >>>> pose time 1613173938.02, meas time 1613173937.73 <<<< 2021-02-12T23:52:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173937.73 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:52:18Z- TerrainNav::motionUpdate() - currEstimate = (4058050, 591650.22, 4.12) 2021-02-12T23:52:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:52:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:52:18Z- TNavBF::Projecting Measurements 2021-02-12T23:52:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:52:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:52:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:52:18Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:52:18Z- TNavFilter::3 good beams. 2021-02-12T23:52:18Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:52:18Z- TNavBF:: beamIndex[0] = 1 2021-02-12T23:52:18Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:52:18Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:52:18Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:52:18Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:52:18Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:52:18Z- TNavBF::mmse selected from filter -1 2021-02-12T23:52:18Z- TerrainMapOctree: (vehN, vehE ) = (4058050.21, 591650.08). 2021-02-12T23:52:18Z- TerrainMapOctree: Min Distance = 4122.89. 2021-02-12T23:52:18Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:52:18Z- There is no specific good beam that all particles have in common. 2021-02-12T23:52:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:52:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 18.24 2021-02-12T23:52:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:52:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 15.15 2021-02-12T23:52:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:52:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 17.33 2021-02-12T23:52:18Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:52:18Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:52:18Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:52:18Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:52:18Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:52:18Z- Filter 0 No beams used in subcloud update 2021-02-12T23:52:18Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:52:18Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:52:18Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 731.6 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:52:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173937.73 sec. 2021-02-12T23:52:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:52:18Z- TNavBF::mmse selected from filter -1 2021-02-12T23:52:18Z- variances([0], [2], and psi_berg) = 731.613591 833.017754 0.000000 2021-02-12T23:52:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:52:18Z- LcmTrn::publishEstimates() - MLE : 4058042.90 591602.72 4.12 2021-02-12T23:52:18Z- LcmTrn::publishEstimates() - MMSE : 4058050.40 591650.22 4.12 2021-02-12T23:52:18Z- LcmTrn::publishEstimates() - COVAR: 731.61 833.02 1.13 0.00 2021-02-12T23:52:18Z- AHRS_M2 msg: 1613173938.40 epoch sec; seqNo:7548 2021-02-12T23:52:18Z- AHRS_M2 msg: 0.74 phi; -0.25 theta; -0.21 psi 2021-02-12T23:52:18Z- handleDepth()-Depth_Keller msg: 1613173938433 epoch sec; seqNo:7610; depth 4.12 2021-02-12T23:52:18Z- handleDvl() - RDI_Pathfinder msg: 1613173938.44 epoch sec; seqNo:3525 2021-02-12T23:52:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 108.26 , 87.44 , 112.72 2021-02-12T23:52:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.04 , 0.02 2021-02-12T23:52:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173938.45 epoch sec; seqNo:7610; 4058050.77 north; 591650.21 east 2021-02-12T23:52:18Z- AHRS_M2 msg: 1613173938.80 epoch sec; seqNo:7549 2021-02-12T23:52:18Z- AHRS_M2 msg: 0.74 phi; -0.27 theta; -0.23 psi 2021-02-12T23:52:18Z- handleDepth()-Depth_Keller msg: 1613173938837 epoch sec; seqNo:7611; depth 4.15 2021-02-12T23:52:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173938.85 epoch sec; seqNo:7611; 4058050.77 north; 591650.21 east 2021-02-12T23:52:19Z- AHRS_M2 msg: 1613173939.21 epoch sec; seqNo:7550 2021-02-12T23:52:19Z- AHRS_M2 msg: 0.74 phi; -0.29 theta; -0.25 psi 2021-02-12T23:52:19Z- handleDepth()-Depth_Keller msg: 1613173939213 epoch sec; seqNo:7612; depth 4.20 2021-02-12T23:52:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173939.23 epoch sec; seqNo:7612; 4058051.15 north; 591650.21 east 2021-02-12T23:52:19Z- AHRS_M2 msg: 1613173939.61 epoch sec; seqNo:7551 2021-02-12T23:52:19Z- AHRS_M2 msg: 0.75 phi; -0.31 theta; -0.26 psi 2021-02-12T23:52:19Z- handleDepth()-Depth_Keller msg: 1613173939618 epoch sec; seqNo:7613; depth 4.26 2021-02-12T23:52:19Z- handleDvl() - RDI_Pathfinder msg: 1613173939.63 epoch sec; seqNo:3526 2021-02-12T23:52:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.98 , 115.83 , 86.09 , 121.78 2021-02-12T23:52:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.07 , 0.17 2021-02-12T23:52:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173939.64 epoch sec; seqNo:7613; 4058051.15 north; 591650.21 east 2021-02-12T23:52:20Z- AHRS_M2 msg: 1613173940.01 epoch sec; seqNo:7552 2021-02-12T23:52:20Z- AHRS_M2 msg: 0.74 phi; -0.33 theta; -0.25 psi 2021-02-12T23:52:20Z- handleDepth()-Depth_Keller msg: 1613173940016 epoch sec; seqNo:7614; depth 4.33 2021-02-12T23:52:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173940.04 epoch sec; seqNo:7614; 4058051.15 north; 591650.21 east 2021-02-12T23:52:20Z- AHRS_M2 msg: 1613173940.42 epoch sec; seqNo:7553 2021-02-12T23:52:20Z- AHRS_M2 msg: 0.73 phi; -0.34 theta; -0.24 psi 2021-02-12T23:52:20Z- handleDepth()-Depth_Keller msg: 1613173940422 epoch sec; seqNo:7615; depth 4.41 2021-02-12T23:52:20Z- handleDvl() - RDI_Pathfinder msg: 1613173940.43 epoch sec; seqNo:3527 2021-02-12T23:52:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.21 , 116.98 , 85.75 , 128.88 2021-02-12T23:52:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , 0.03 , 0.33 2021-02-12T23:52:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173940.46 epoch sec; seqNo:7615; 4058051.53 north; 591650.20 east 2021-02-12T23:52:20Z- AHRS_M2 msg: 1613173940.82 epoch sec; seqNo:7554 2021-02-12T23:52:20Z- AHRS_M2 msg: 0.73 phi; -0.36 theta; -0.23 psi 2021-02-12T23:52:20Z- handleDepth()-Depth_Keller msg: 1613173940829 epoch sec; seqNo:7616; depth 4.48 2021-02-12T23:52:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:20Z- handleDvl() - RDI_Pathfinder msg: 1613173940.83 epoch sec; seqNo:3528 2021-02-12T23:52:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , 0.03 , 0.33 2021-02-12T23:52:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173940.85 epoch sec; seqNo:7616; 4058051.55 north; 591651.43 east 2021-02-12T23:52:21Z- AHRS_M2 msg: 1613173941.22 epoch sec; seqNo:7555 2021-02-12T23:52:21Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.23 psi 2021-02-12T23:52:21Z- handleDepth()-Depth_Keller msg: 1613173941230 epoch sec; seqNo:7617; depth 4.58 2021-02-12T23:52:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173941.24 epoch sec; seqNo:7617; 4058051.55 north; 591651.43 east 2021-02-12T23:52:21Z- AHRS_M2 msg: 1613173941.63 epoch sec; seqNo:7556 2021-02-12T23:52:21Z- AHRS_M2 msg: 0.71 phi; -0.38 theta; -0.23 psi 2021-02-12T23:52:21Z- handleDepth()-Depth_Keller msg: 1613173941637 epoch sec; seqNo:7618; depth 4.67 2021-02-12T23:52:21Z- handleDvl() - RDI_Pathfinder msg: 1613173941.66 epoch sec; seqNo:3529 2021-02-12T23:52:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.65 , 118.47 , 0.00 , 134.82 2021-02-12T23:52:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.57 , 0.22 , 0.27 2021-02-12T23:52:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173941.67 epoch sec; seqNo:7618; 4058051.92 north; 591651.42 east 2021-02-12T23:52:22Z- AHRS_M2 msg: 1613173942.04 epoch sec; seqNo:7557 2021-02-12T23:52:22Z- AHRS_M2 msg: 0.67 phi; -0.38 theta; -0.23 psi 2021-02-12T23:52:22Z- handleDepth()-Depth_Keller msg: 1613173942042 epoch sec; seqNo:7619; depth 4.79 2021-02-12T23:52:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:22Z- handleDvl() - RDI_Pathfinder msg: 1613173942.04 epoch sec; seqNo:3530 2021-02-12T23:52:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.57 , 0.22 , 0.27 2021-02-12T23:52:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173942.06 epoch sec; seqNo:7619; 4058051.92 north; 591651.42 east 2021-02-12T23:52:22Z- AHRS_M2 msg: 1613173942.45 epoch sec; seqNo:7558 2021-02-12T23:52:22Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.24 psi 2021-02-12T23:52:22Z- handleDepth()-Depth_Keller msg: 1613173942455 epoch sec; seqNo:7620; depth 4.91 2021-02-12T23:52:22Z- handleDvl() - RDI_Pathfinder msg: 1613173942.47 epoch sec; seqNo:3531 2021-02-12T23:52:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 116.57 , 83.86 , 135.91 2021-02-12T23:52:22Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.30 , -0.07 2021-02-12T23:52:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173942.48 epoch sec; seqNo:7620; 4058052.30 north; 591651.42 east 2021-02-12T23:52:22Z- AHRS_M2 msg: 1613173942.84 epoch sec; seqNo:7559 2021-02-12T23:52:22Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.26 psi 2021-02-12T23:52:22Z- handleDepth()-Depth_Keller msg: 1613173942844 epoch sec; seqNo:7621; depth 5.04 2021-02-12T23:52:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173942.86 epoch sec; seqNo:7621; 4058052.32 north; 591652.64 east 2021-02-12T23:52:23Z- LcmTrn::handleMessages() - handling 43 messages took 0.67 ms and 130 calls... 2021-02-12T23:52:23Z- waiting for 1613173941.02, time is 1613173942.86 2021-02-12T23:52:23Z- LcmTrn::updateTrn() >>>> pose time 1613173942.86, meas time 1613173942.47 <<<< 2021-02-12T23:52:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173942.47 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:52:23Z- TerrainNav::motionUpdate() - currEstimate = (4058052, 591652.64, 5.04) 2021-02-12T23:52:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:52:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:52:23Z- TNavBF::Projecting Measurements 2021-02-12T23:52:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:52:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:52:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:52:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:52:23Z- TNavFilter::4 good beams. 2021-02-12T23:52:23Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:52:23Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:52:23Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:52:23Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:52:23Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:52:23Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:52:23Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:52:23Z- TNavBF::mmse selected from filter -1 2021-02-12T23:52:23Z- TerrainMapOctree: (vehN, vehE ) = (4058052.16, 591652.44). 2021-02-12T23:52:23Z- TerrainMapOctree: Min Distance = 4119.89. 2021-02-12T23:52:23Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:52:23Z- There is no specific good beam that all particles have in common. 2021-02-12T23:52:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:52:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-12T23:52:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:52:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 21.74 2021-02-12T23:52:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:52:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 11.25 2021-02-12T23:52:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:52:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.032 ExpectedMeasDiff: nan currMeas.covariance: 29.55 2021-02-12T23:52:23Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:52:23Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:52:23Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:52:23Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:52:23Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:52:23Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:52:23Z- Filter 0 No beams used in subcloud update 2021-02-12T23:52:23Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:52:23Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:52:23Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 731.5 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:52:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173942.47 sec. 2021-02-12T23:52:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:52:23Z- TNavBF::mmse selected from filter -1 2021-02-12T23:52:23Z- variances([0], [2], and psi_berg) = 731.529163 833.017692 0.000000 2021-02-12T23:52:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:52:23Z- LcmTrn::publishEstimates() - MLE : 4058044.82 591606.14 5.04 2021-02-12T23:52:23Z- LcmTrn::publishEstimates() - MMSE : 4058052.32 591652.64 5.04 2021-02-12T23:52:23Z- LcmTrn::publishEstimates() - COVAR: 731.53 833.02 1.02 0.00 2021-02-12T23:52:23Z- AHRS_M2 msg: 1613173943.24 epoch sec; seqNo:7560 2021-02-12T23:52:23Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.29 psi 2021-02-12T23:52:23Z- handleDepth()-Depth_Keller msg: 1613173943248 epoch sec; seqNo:7622; depth 5.15 2021-02-12T23:52:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:23Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:23Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:23Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:23Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:23Z- handleDvl() - RDI_Pathfinder msg: 1613173943.25 epoch sec; seqNo:3532 2021-02-12T23:52:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:23Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.30 , -0.07 2021-02-12T23:52:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173943.26 epoch sec; seqNo:7622; 4058052.70 north; 591652.63 east 2021-02-12T23:52:23Z- AHRS_M2 msg: 1613173943.65 epoch sec; seqNo:7561 2021-02-12T23:52:23Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.31 psi 2021-02-12T23:52:23Z- handleDepth()-Depth_Keller msg: 1613173943652 epoch sec; seqNo:7623; depth 5.26 2021-02-12T23:52:23Z- handleDvl() - RDI_Pathfinder msg: 1613173943.66 epoch sec; seqNo:3533 2021-02-12T23:52:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.38 , 116.71 , 85.48 , 130.09 2021-02-12T23:52:23Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , 0.02 , -0.37 2021-02-12T23:52:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173943.67 epoch sec; seqNo:7623; 4058053.07 north; 591652.63 east 2021-02-12T23:52:24Z- AHRS_M2 msg: 1613173944.05 epoch sec; seqNo:7562 2021-02-12T23:52:24Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.31 psi 2021-02-12T23:52:24Z- handleDepth()-Depth_Keller msg: 1613173944058 epoch sec; seqNo:7624; depth 5.41 2021-02-12T23:52:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173944.07 epoch sec; seqNo:7624; 4058053.45 north; 591652.63 east 2021-02-12T23:52:24Z- AHRS_M2 msg: 1613173944.46 epoch sec; seqNo:7563 2021-02-12T23:52:24Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.30 psi 2021-02-12T23:52:24Z- handleDepth()-Depth_Keller msg: 1613173944460 epoch sec; seqNo:7625; depth 5.53 2021-02-12T23:52:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:24Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:24Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:24Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:24Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:24Z- handleDvl() - RDI_Pathfinder msg: 1613173944.46 epoch sec; seqNo:3534 2021-02-12T23:52:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , 0.02 , -0.37 2021-02-12T23:52:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173944.48 epoch sec; seqNo:7625; 4058053.83 north; 591652.62 east 2021-02-12T23:52:24Z- AHRS_M2 msg: 1613173944.86 epoch sec; seqNo:7564 2021-02-12T23:52:24Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.28 psi 2021-02-12T23:52:24Z- handleDepth()-Depth_Keller msg: 1613173944870 epoch sec; seqNo:7626; depth 5.67 2021-02-12T23:52:24Z- handleDvl() - RDI_Pathfinder msg: 1613173944.89 epoch sec; seqNo:3535 2021-02-12T23:52:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.52 , 127.19 , 85.55 , 128.67 2021-02-12T23:52:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.27 , -0.15 2021-02-12T23:52:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173944.90 epoch sec; seqNo:7626; 4058054.22 north; 591653.84 east 2021-02-12T23:52:25Z- AHRS_M2 msg: 1613173945.26 epoch sec; seqNo:7565 2021-02-12T23:52:25Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.26 psi 2021-02-12T23:52:25Z- handleDepth()-Depth_Keller msg: 1613173945268 epoch sec; seqNo:7627; depth 5.78 2021-02-12T23:52:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173945.28 epoch sec; seqNo:7627; 4058054.60 north; 591653.83 east 2021-02-12T23:52:25Z- AHRS_M2 msg: 1613173945.67 epoch sec; seqNo:7566 2021-02-12T23:52:25Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.24 psi 2021-02-12T23:52:25Z- handleDepth()-Depth_Keller msg: 1613173945675 epoch sec; seqNo:7628; depth 5.88 2021-02-12T23:52:25Z- handleDvl() - RDI_Pathfinder msg: 1613173945.69 epoch sec; seqNo:3536 2021-02-12T23:52:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.52 , 118.26 , 84.06 , 125.70 2021-02-12T23:52:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.25 , 0.13 2021-02-12T23:52:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173945.70 epoch sec; seqNo:7628; 4058054.98 north; 591653.83 east 2021-02-12T23:52:26Z- AHRS_M2 msg: 1613173946.07 epoch sec; seqNo:7567 2021-02-12T23:52:26Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.22 psi 2021-02-12T23:52:26Z- handleDepth()-Depth_Keller msg: 1613173946077 epoch sec; seqNo:7629; depth 6.01 2021-02-12T23:52:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173946.10 epoch sec; seqNo:7629; 4058054.98 north; 591653.83 east 2021-02-12T23:52:26Z- AHRS_M2 msg: 1613173946.49 epoch sec; seqNo:7568 2021-02-12T23:52:26Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.21 psi 2021-02-12T23:52:26Z- handleDepth()-Depth_Keller msg: 1613173946510 epoch sec; seqNo:7630; depth 6.12 2021-02-12T23:52:26Z- handleDvl() - RDI_Pathfinder msg: 1613173946.52 epoch sec; seqNo:3537 2021-02-12T23:52:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.35 , 115.63 , 84.40 , 129.01 2021-02-12T23:52:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.08 , 0.19 2021-02-12T23:52:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173946.53 epoch sec; seqNo:7630; 4058055.36 north; 591653.83 east 2021-02-12T23:52:26Z- AHRS_M2 msg: 1613173946.88 epoch sec; seqNo:7569 2021-02-12T23:52:26Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-12T23:52:26Z- handleDepth()-Depth_Keller msg: 1613173946885 epoch sec; seqNo:7631; depth 6.25 2021-02-12T23:52:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:26Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:26Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:26Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:26Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:26Z- handleDvl() - RDI_Pathfinder msg: 1613173946.89 epoch sec; seqNo:3538 2021-02-12T23:52:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.08 , 0.19 2021-02-12T23:52:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173946.90 epoch sec; seqNo:7631; 4058055.36 north; 591653.83 east 2021-02-12T23:52:27Z- AHRS_M2 msg: 1613173947.29 epoch sec; seqNo:7570 2021-02-12T23:52:27Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-12T23:52:27Z- handleDepth()-Depth_Keller msg: 1613173947290 epoch sec; seqNo:7632; depth 6.35 2021-02-12T23:52:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173947.30 epoch sec; seqNo:7632; 4058055.74 north; 591653.82 east 2021-02-12T23:52:27Z- AHRS_M2 msg: 1613173947.69 epoch sec; seqNo:7571 2021-02-12T23:52:27Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-12T23:52:27Z- handleDepth()-Depth_Keller msg: 1613173947703 epoch sec; seqNo:7633; depth 6.47 2021-02-12T23:52:27Z- handleDvl() - RDI_Pathfinder msg: 1613173947.71 epoch sec; seqNo:3539 2021-02-12T23:52:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 111.10 , 82.84 , 124.48 2021-02-12T23:52:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.06 , 0.09 2021-02-12T23:52:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173947.72 epoch sec; seqNo:7633; 4058056.12 north; 591653.82 east 2021-02-12T23:52:28Z- AHRS_M2 msg: 1613173948.09 epoch sec; seqNo:7572 2021-02-12T23:52:28Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-12T23:52:28Z- handleDepth()-Depth_Keller msg: 1613173948097 epoch sec; seqNo:7634; depth 6.61 2021-02-12T23:52:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:28Z- handleDvl() - RDI_Pathfinder msg: 1613173948.10 epoch sec; seqNo:3540 2021-02-12T23:52:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.06 , 0.09 2021-02-12T23:52:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173948.11 epoch sec; seqNo:7634; 4058056.12 north; 591653.82 east 2021-02-12T23:52:28Z- LcmTrn::handleMessages() - handling 48 messages took 0.68 ms and 135 calls... 2021-02-12T23:52:28Z- waiting for 1613173945.86, time is 1613173948.11 2021-02-12T23:52:28Z- LcmTrn::updateTrn() equating pose 1613173948.11 and meas times 1613173948.11 2021-02-12T23:52:28Z- LcmTrn::updateTrn() >>>> pose time 1613173948.11, meas time 1613173948.11 <<<< 2021-02-12T23:52:28Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173948.11, ping # 0. 2021-02-12T23:52:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173948.11 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:52:28Z- TerrainNav::motionUpdate() - currEstimate = (4058056, 591653.82, 6.61) 2021-02-12T23:52:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:52:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:52:28Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:52:28Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:52:28Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173948.11 sec.; No relevant navigation data available 2021-02-12T23:52:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:52:28Z- TNavBF::mmse selected from filter -1 2021-02-12T23:52:28Z- variances([0], [2], and psi_berg) = 731.452191 833.017634 0.000000 2021-02-12T23:52:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:52:28Z- LcmTrn::publishEstimates() - MLE : 4058048.62 591607.32 6.61 2021-02-12T23:52:28Z- LcmTrn::publishEstimates() - MMSE : 4058056.12 591653.82 6.61 2021-02-12T23:52:28Z- LcmTrn::publishEstimates() - COVAR: 731.45 833.02 1.22 0.00 2021-02-12T23:52:28Z- AHRS_M2 msg: 1613173948.51 epoch sec; seqNo:7573 2021-02-12T23:52:28Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.21 psi 2021-02-12T23:52:28Z- handleDepth()-Depth_Keller msg: 1613173948509 epoch sec; seqNo:7635; depth 6.71 2021-02-12T23:52:28Z- handleDvl() - RDI_Pathfinder msg: 1613173948.52 epoch sec; seqNo:3541 2021-02-12T23:52:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.35 , 112.65 , 82.91 , 124.55 2021-02-12T23:52:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.07 , -0.09 2021-02-12T23:52:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173948.54 epoch sec; seqNo:7635; 4058056.51 north; 591655.04 east 2021-02-12T23:52:28Z- AHRS_M2 msg: 1613173948.90 epoch sec; seqNo:7574 2021-02-12T23:52:28Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.21 psi 2021-02-12T23:52:28Z- handleDepth()-Depth_Keller msg: 1613173948906 epoch sec; seqNo:7636; depth 6.83 2021-02-12T23:52:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173948.92 epoch sec; seqNo:7636; 4058056.89 north; 591655.03 east 2021-02-12T23:52:29Z- AHRS_M2 msg: 1613173949.30 epoch sec; seqNo:7575 2021-02-12T23:52:29Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.22 psi 2021-02-12T23:52:29Z- handleDepth()-Depth_Keller msg: 1613173949309 epoch sec; seqNo:7637; depth 6.94 2021-02-12T23:52:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:29Z- handleDvl() - RDI_Pathfinder msg: 1613173949.31 epoch sec; seqNo:3542 2021-02-12T23:52:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.07 , -0.09 2021-02-12T23:52:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173949.33 epoch sec; seqNo:7637; 4058056.89 north; 591655.03 east 2021-02-12T23:52:29Z- AHRS_M2 msg: 1613173949.71 epoch sec; seqNo:7576 2021-02-12T23:52:29Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.22 psi 2021-02-12T23:52:29Z- handleDepth()-Depth_Keller msg: 1613173949712 epoch sec; seqNo:7638; depth 7.08 2021-02-12T23:52:29Z- handleDvl() - RDI_Pathfinder msg: 1613173949.72 epoch sec; seqNo:3543 2021-02-12T23:52:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 112.79 , 0.00 , 126.17 2021-02-12T23:52:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.10 , -0.09 2021-02-12T23:52:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173949.75 epoch sec; seqNo:7638; 4058057.27 north; 591655.03 east 2021-02-12T23:52:30Z- AHRS_M2 msg: 1613173950.11 epoch sec; seqNo:7577 2021-02-12T23:52:30Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.21 psi 2021-02-12T23:52:30Z- handleDepth()-Depth_Keller msg: 1613173950118 epoch sec; seqNo:7639; depth 7.20 2021-02-12T23:52:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173950.13 epoch sec; seqNo:7639; 4058057.65 north; 591655.02 east 2021-02-12T23:52:30Z- AHRS_M2 msg: 1613173950.52 epoch sec; seqNo:7578 2021-02-12T23:52:30Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.20 psi 2021-02-12T23:52:30Z- handleDepth()-Depth_Keller msg: 1613173950528 epoch sec; seqNo:7640; depth 7.32 2021-02-12T23:52:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:30Z- handleDvl() - RDI_Pathfinder msg: 1613173950.53 epoch sec; seqNo:3544 2021-02-12T23:52:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.10 , -0.09 2021-02-12T23:52:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173950.55 epoch sec; seqNo:7640; 4058058.03 north; 591655.02 east 2021-02-12T23:52:30Z- AHRS_M2 msg: 1613173950.92 epoch sec; seqNo:7579 2021-02-12T23:52:30Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.18 psi 2021-02-12T23:52:30Z- handleDepth()-Depth_Keller msg: 1613173950998 epoch sec; seqNo:7641; depth 7.46 2021-02-12T23:52:31Z- handleDvl() - RDI_Pathfinder msg: 1613173951.01 epoch sec; seqNo:3545 2021-02-12T23:52:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.39 , 109.21 , 82.44 , 121.10 2021-02-12T23:52:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.13 , 0.08 2021-02-12T23:52:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173951.02 epoch sec; seqNo:7641; 4058058.42 north; 591656.24 east 2021-02-12T23:52:31Z- AHRS_M2 msg: 1613173951.32 epoch sec; seqNo:7580 2021-02-12T23:52:31Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.17 psi 2021-02-12T23:52:31Z- handleDepth()-Depth_Keller msg: 1613173951417 epoch sec; seqNo:7642; depth 7.60 2021-02-12T23:52:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173951.43 epoch sec; seqNo:7642; 4058058.42 north; 591656.24 east 2021-02-12T23:52:31Z- AHRS_M2 msg: 1613173951.73 epoch sec; seqNo:7581 2021-02-12T23:52:31Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.16 psi 2021-02-12T23:52:31Z- handleDepth()-Depth_Keller msg: 1613173951757 epoch sec; seqNo:7643; depth 7.71 2021-02-12T23:52:31Z- handleDvl() - RDI_Pathfinder msg: 1613173951.79 epoch sec; seqNo:3546 2021-02-12T23:52:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 107.85 , 82.17 , 118.67 2021-02-12T23:52:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.01 , 0.12 2021-02-12T23:52:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173951.81 epoch sec; seqNo:7643; 4058058.80 north; 591656.23 east 2021-02-12T23:52:32Z- AHRS_M2 msg: 1613173952.14 epoch sec; seqNo:7582 2021-02-12T23:52:32Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.16 psi 2021-02-12T23:52:32Z- handleDepth()-Depth_Keller msg: 1613173952161 epoch sec; seqNo:7644; depth 7.81 2021-02-12T23:52:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173952.17 epoch sec; seqNo:7644; 4058058.80 north; 591656.23 east 2021-02-12T23:52:32Z- AHRS_M2 msg: 1613173952.54 epoch sec; seqNo:7583 2021-02-12T23:52:32Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.17 psi 2021-02-12T23:52:32Z- handleDepth()-Depth_Keller msg: 1613173952562 epoch sec; seqNo:7645; depth 7.93 2021-02-12T23:52:32Z- handleDvl() - RDI_Pathfinder msg: 1613173952.57 epoch sec; seqNo:3547 2021-02-12T23:52:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.55 , 106.91 , 81.62 , 123.27 2021-02-12T23:52:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.06 , -0.05 2021-02-12T23:52:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173952.58 epoch sec; seqNo:7645; 4058059.17 north; 591656.23 east 2021-02-12T23:52:32Z- AHRS_M2 msg: 1613173952.94 epoch sec; seqNo:7584 2021-02-12T23:52:32Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.17 psi 2021-02-12T23:52:32Z- handleDepth()-Depth_Keller msg: 1613173952971 epoch sec; seqNo:7646; depth 8.06 2021-02-12T23:52:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:32Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:32Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:32Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:32Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:32Z- handleDvl() - RDI_Pathfinder msg: 1613173952.97 epoch sec; seqNo:3548 2021-02-12T23:52:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.06 , -0.05 2021-02-12T23:52:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173952.99 epoch sec; seqNo:7646; 4058059.55 north; 591656.22 east 2021-02-12T23:52:33Z- LcmTrn::handleMessages() - handling 44 messages took 0.80 ms and 128 calls... 2021-02-12T23:52:33Z- waiting for 1613173951.11, time is 1613173952.99 2021-02-12T23:52:33Z- LcmTrn::updateTrn() equating pose 1613173952.99 and meas times 1613173952.99 2021-02-12T23:52:33Z- LcmTrn::updateTrn() >>>> pose time 1613173952.99, meas time 1613173952.99 <<<< 2021-02-12T23:52:33Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173952.99, ping # 0. 2021-02-12T23:52:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173952.99 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:52:33Z- TerrainNav::motionUpdate() - currEstimate = (4058060, 591656.22, 8.06) 2021-02-12T23:52:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:52:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:52:33Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:52:33Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:52:33Z- TNavBF::Projecting Measurements 2021-02-12T23:52:33Z- TNavFilter::Measurement from time = 1613173952.99 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:52:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:52:33Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:52:33Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:52:33Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:52:33Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:52:33Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:52:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:52:33Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 731.4 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-12T23:52:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:52:33Z- TNavBF::mmse selected from filter -1 2021-02-12T23:52:33Z- variances([0], [2], and psi_berg) = 731.373255 833.017576 0.000000 2021-02-12T23:52:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:52:33Z- LcmTrn::publishEstimates() - MLE : 4058052.05 591609.72 8.06 2021-02-12T23:52:33Z- LcmTrn::publishEstimates() - MMSE : 4058059.55 591656.22 8.06 2021-02-12T23:52:33Z- LcmTrn::publishEstimates() - COVAR: 731.37 833.02 1.44 0.00 2021-02-12T23:52:33Z- AHRS_M2 msg: 1613173953.35 epoch sec; seqNo:7585 2021-02-12T23:52:33Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-12T23:52:33Z- handleDepth()-Depth_Keller msg: 1613173953353 epoch sec; seqNo:7647; depth 8.18 2021-02-12T23:52:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173953.37 epoch sec; seqNo:7647; 4058059.55 north; 591656.22 east 2021-02-12T23:52:33Z- AHRS_M2 msg: 1613173953.75 epoch sec; seqNo:7586 2021-02-12T23:52:33Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-12T23:52:33Z- handleDepth()-Depth_Keller msg: 1613173953761 epoch sec; seqNo:7648; depth 8.30 2021-02-12T23:52:33Z- handleDvl() - RDI_Pathfinder msg: 1613173953.77 epoch sec; seqNo:3549 2021-02-12T23:52:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 106.43 , 80.75 , 119.95 2021-02-12T23:52:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , -0.10 2021-02-12T23:52:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173953.78 epoch sec; seqNo:7648; 4058059.95 north; 591657.44 east 2021-02-12T23:52:34Z- AHRS_M2 msg: 1613173954.15 epoch sec; seqNo:7587 2021-02-12T23:52:34Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-12T23:52:34Z- handleDepth()-Depth_Keller msg: 1613173954158 epoch sec; seqNo:7649; depth 8.43 2021-02-12T23:52:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:34Z- handleDvl() - RDI_Pathfinder msg: 1613173954.16 epoch sec; seqNo:3550 2021-02-12T23:52:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , -0.10 2021-02-12T23:52:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173954.17 epoch sec; seqNo:7649; 4058060.32 north; 591657.44 east 2021-02-12T23:52:34Z- AHRS_M2 msg: 1613173954.57 epoch sec; seqNo:7588 2021-02-12T23:52:34Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.18 psi 2021-02-12T23:52:34Z- handleDepth()-Depth_Keller msg: 1613173954580 epoch sec; seqNo:7650; depth 8.55 2021-02-12T23:52:34Z- handleDvl() - RDI_Pathfinder msg: 1613173954.59 epoch sec; seqNo:3551 2021-02-12T23:52:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.86 , 107.79 , 80.75 , 119.95 2021-02-12T23:52:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , -0.06 2021-02-12T23:52:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173954.61 epoch sec; seqNo:7650; 4058060.70 north; 591657.43 east 2021-02-12T23:52:34Z- AHRS_M2 msg: 1613173954.99 epoch sec; seqNo:7589 2021-02-12T23:52:34Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.17 psi 2021-02-12T23:52:34Z- handleDepth()-Depth_Keller msg: 1613173954998 epoch sec; seqNo:7651; depth 8.71 2021-02-12T23:52:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173955.01 epoch sec; seqNo:7651; 4058061.08 north; 591657.43 east 2021-02-12T23:52:35Z- AHRS_M2 msg: 1613173955.37 epoch sec; seqNo:7590 2021-02-12T23:52:35Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.17 psi 2021-02-12T23:52:35Z- handleDepth()-Depth_Keller msg: 1613173955370 epoch sec; seqNo:7652; depth 8.85 2021-02-12T23:52:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:35Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:35Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:35Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:35Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:35Z- handleDvl() - RDI_Pathfinder msg: 1613173955.37 epoch sec; seqNo:3552 2021-02-12T23:52:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , -0.06 2021-02-12T23:52:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173955.38 epoch sec; seqNo:7652; 4058061.08 north; 591657.43 east 2021-02-12T23:52:35Z- AHRS_M2 msg: 1613173955.77 epoch sec; seqNo:7591 2021-02-12T23:52:35Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.16 psi 2021-02-12T23:52:35Z- handleDepth()-Depth_Keller msg: 1613173955781 epoch sec; seqNo:7653; depth 8.97 2021-02-12T23:52:35Z- handleDvl() - RDI_Pathfinder msg: 1613173955.79 epoch sec; seqNo:3553 2021-02-12T23:52:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.35 , 105.22 , 80.88 , 122.79 2021-02-12T23:52:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.03 , -0.07 2021-02-12T23:52:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173955.80 epoch sec; seqNo:7653; 4058061.46 north; 591657.42 east 2021-02-12T23:52:36Z- AHRS_M2 msg: 1613173956.17 epoch sec; seqNo:7592 2021-02-12T23:52:36Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.16 psi 2021-02-12T23:52:36Z- handleDepth()-Depth_Keller msg: 1613173956176 epoch sec; seqNo:7654; depth 9.10 2021-02-12T23:52:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173956.19 epoch sec; seqNo:7654; 4058061.85 north; 591658.64 east 2021-02-12T23:52:36Z- AHRS_M2 msg: 1613173956.58 epoch sec; seqNo:7593 2021-02-12T23:52:36Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.16 psi 2021-02-12T23:52:36Z- handleDepth()-Depth_Keller msg: 1613173956585 epoch sec; seqNo:7655; depth 9.24 2021-02-12T23:52:36Z- handleDvl() - RDI_Pathfinder msg: 1613173956.60 epoch sec; seqNo:3554 2021-02-12T23:52:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 106.50 , 0.00 , 121.37 2021-02-12T23:52:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , -0.10 2021-02-12T23:52:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173956.62 epoch sec; seqNo:7655; 4058061.85 north; 591658.64 east 2021-02-12T23:52:36Z- AHRS_M2 msg: 1613173956.98 epoch sec; seqNo:7594 2021-02-12T23:52:36Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.17 psi 2021-02-12T23:52:36Z- handleDepth()-Depth_Keller msg: 1613173956997 epoch sec; seqNo:7656; depth 9.39 2021-02-12T23:52:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173957.01 epoch sec; seqNo:7656; 4058062.23 north; 591658.64 east 2021-02-12T23:52:37Z- AHRS_M2 msg: 1613173957.39 epoch sec; seqNo:7595 2021-02-12T23:52:37Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.17 psi 2021-02-12T23:52:37Z- handleDepth()-Depth_Keller msg: 1613173957394 epoch sec; seqNo:7657; depth 9.52 2021-02-12T23:52:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173957.40 epoch sec; seqNo:7657; 4058062.61 north; 591658.63 east 2021-02-12T23:52:37Z- AHRS_M2 msg: 1613173957.79 epoch sec; seqNo:7596 2021-02-12T23:52:37Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.18 psi 2021-02-12T23:52:37Z- handleDepth()-Depth_Keller msg: 1613173957797 epoch sec; seqNo:7658; depth 9.66 2021-02-12T23:52:37Z- handleDvl() - RDI_Pathfinder msg: 1613173957.82 epoch sec; seqNo:3555 2021-02-12T23:52:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.72 , 106.57 , 76.01 , 121.44 2021-02-12T23:52:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.19 , -0.00 2021-02-12T23:52:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173957.83 epoch sec; seqNo:7658; 4058062.99 north; 591658.63 east 2021-02-12T23:52:38Z- AHRS_M2 msg: 1613173958.19 epoch sec; seqNo:7597 2021-02-12T23:52:38Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.18 psi 2021-02-12T23:52:38Z- handleDepth()-Depth_Keller msg: 1613173958201 epoch sec; seqNo:7659; depth 9.79 2021-02-12T23:52:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173958.23 epoch sec; seqNo:7659; 4058062.99 north; 591658.63 east 2021-02-12T23:52:38Z- LcmTrn::handleMessages() - handling 46 messages took 0.77 ms and 131 calls... 2021-02-12T23:52:38Z- waiting for 1613173955.99, time is 1613173958.23 2021-02-12T23:52:38Z- LcmTrn::updateTrn() >>>> pose time 1613173958.23, meas time 1613173957.82 <<<< 2021-02-12T23:52:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173957.82 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:52:38Z- TerrainNav::motionUpdate() - currEstimate = (4058063, 591658.63, 9.79) 2021-02-12T23:52:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:52:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:52:38Z- TNavBF::Projecting Measurements 2021-02-12T23:52:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:52:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:52:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:52:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:52:38Z- TNavFilter::4 good beams. 2021-02-12T23:52:38Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:52:38Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:52:38Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:52:38Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:52:38Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:52:38Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:52:38Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:52:38Z- TNavBF::mmse selected from filter -1 2021-02-12T23:52:38Z- TerrainMapOctree: (vehN, vehE ) = (4058062.72, 591658.44). 2021-02-12T23:52:38Z- TerrainMapOctree: Min Distance = 4107.91. 2021-02-12T23:52:38Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:52:38Z- There is no specific good beam that all particles have in common. 2021-02-12T23:52:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:52:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.63 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 12.59 2021-02-12T23:52:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:52:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.47 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 18.17 2021-02-12T23:52:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:52:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 9.24 2021-02-12T23:52:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:52:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 23.60 2021-02-12T23:52:38Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:52:38Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:52:38Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:52:38Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:52:38Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:52:38Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:52:38Z- Filter 0 No beams used in subcloud update 2021-02-12T23:52:38Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:52:38Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:52:38Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 731.3 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:52:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173957.82 sec. 2021-02-12T23:52:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:52:38Z- TNavBF::mmse selected from filter -1 2021-02-12T23:52:38Z- variances([0], [2], and psi_berg) = 731.275522 833.017503 0.000000 2021-02-12T23:52:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:52:38Z- LcmTrn::publishEstimates() - MLE : 4058055.49 591612.13 9.79 2021-02-12T23:52:38Z- LcmTrn::publishEstimates() - MMSE : 4058062.99 591658.63 9.79 2021-02-12T23:52:38Z- LcmTrn::publishEstimates() - COVAR: 731.28 833.02 1.18 0.00 2021-02-12T23:52:38Z- AHRS_M2 msg: 1613173958.60 epoch sec; seqNo:7598 2021-02-12T23:52:38Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.18 psi 2021-02-12T23:52:38Z- handleDepth()-Depth_Keller msg: 1613173958609 epoch sec; seqNo:7660; depth 9.93 2021-02-12T23:52:38Z- handleDvl() - RDI_Pathfinder msg: 1613173958.62 epoch sec; seqNo:3556 2021-02-12T23:52:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 106.64 , 79.28 , 121.51 2021-02-12T23:52:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.12 , 0.13 2021-02-12T23:52:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173958.63 epoch sec; seqNo:7660; 4058063.37 north; 591658.62 east 2021-02-12T23:52:39Z- AHRS_M2 msg: 1613173959.02 epoch sec; seqNo:7599 2021-02-12T23:52:39Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-12T23:52:39Z- handleDepth()-Depth_Keller msg: 1613173959021 epoch sec; seqNo:7661; depth 10.06 2021-02-12T23:52:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:39Z- handleDvl() - RDI_Pathfinder msg: 1613173959.02 epoch sec; seqNo:3557 2021-02-12T23:52:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.12 , 0.13 2021-02-12T23:52:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173959.03 epoch sec; seqNo:7661; 4058063.76 north; 591659.84 east 2021-02-12T23:52:39Z- AHRS_M2 msg: 1613173959.41 epoch sec; seqNo:7600 2021-02-12T23:52:39Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.18 psi 2021-02-12T23:52:39Z- handleDepth()-Depth_Keller msg: 1613173959413 epoch sec; seqNo:7662; depth 10.21 2021-02-12T23:52:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173959.43 epoch sec; seqNo:7662; 4058063.76 north; 591659.84 east 2021-02-12T23:52:39Z- AHRS_M2 msg: 1613173959.81 epoch sec; seqNo:7601 2021-02-12T23:52:39Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-12T23:52:39Z- handleDepth()-Depth_Keller msg: 1613173959816 epoch sec; seqNo:7663; depth 10.34 2021-02-12T23:52:39Z- handleDvl() - RDI_Pathfinder msg: 1613173959.83 epoch sec; seqNo:3558 2021-02-12T23:52:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 105.22 , 78.46 , 118.74 2021-02-12T23:52:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , 0.09 2021-02-12T23:52:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173959.85 epoch sec; seqNo:7663; 4058064.14 north; 591659.84 east 2021-02-12T23:52:40Z- AHRS_M2 msg: 1613173960.22 epoch sec; seqNo:7602 2021-02-12T23:52:40Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.20 psi 2021-02-12T23:52:40Z- handleDepth()-Depth_Keller msg: 1613173960221 epoch sec; seqNo:7664; depth 10.49 2021-02-12T23:52:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:40Z- handleDvl() - RDI_Pathfinder msg: 1613173960.22 epoch sec; seqNo:3559 2021-02-12T23:52:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , 0.09 2021-02-12T23:52:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173960.23 epoch sec; seqNo:7664; 4058064.52 north; 591659.83 east 2021-02-12T23:52:40Z- AHRS_M2 msg: 1613173960.62 epoch sec; seqNo:7603 2021-02-12T23:52:40Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.21 psi 2021-02-12T23:52:40Z- handleDepth()-Depth_Keller msg: 1613173960626 epoch sec; seqNo:7665; depth 10.60 2021-02-12T23:52:40Z- handleDvl() - RDI_Pathfinder msg: 1613173960.68 epoch sec; seqNo:3560 2021-02-12T23:52:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.78 , 106.70 , 79.67 , 121.58 2021-02-12T23:52:40Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.09 , -0.08 2021-02-12T23:52:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173960.69 epoch sec; seqNo:7665; 4058064.90 north; 591659.83 east 2021-02-12T23:52:41Z- AHRS_M2 msg: 1613173961.02 epoch sec; seqNo:7604 2021-02-12T23:52:41Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.22 psi 2021-02-12T23:52:41Z- handleDepth()-Depth_Keller msg: 1613173961029 epoch sec; seqNo:7666; depth 10.76 2021-02-12T23:52:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173961.05 epoch sec; seqNo:7666; 4058064.90 north; 591659.83 east 2021-02-12T23:52:41Z- AHRS_M2 msg: 1613173961.43 epoch sec; seqNo:7605 2021-02-12T23:52:41Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.23 psi 2021-02-12T23:52:41Z- handleDepth()-Depth_Keller msg: 1613173961433 epoch sec; seqNo:7667; depth 10.89 2021-02-12T23:52:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:41Z- handleDvl() - RDI_Pathfinder msg: 1613173961.43 epoch sec; seqNo:3561 2021-02-12T23:52:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:41Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.09 , -0.08 2021-02-12T23:52:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173961.45 epoch sec; seqNo:7667; 4058065.29 north; 591661.05 east 2021-02-12T23:52:41Z- AHRS_M2 msg: 1613173961.83 epoch sec; seqNo:7606 2021-02-12T23:52:41Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.22 psi 2021-02-12T23:52:41Z- handleDepth()-Depth_Keller msg: 1613173961837 epoch sec; seqNo:7668; depth 11.01 2021-02-12T23:52:41Z- handleDvl() - RDI_Pathfinder msg: 1613173961.85 epoch sec; seqNo:3562 2021-02-12T23:52:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.94 , 107.92 , 78.46 , 120.09 2021-02-12T23:52:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.08 , -0.14 2021-02-12T23:52:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173961.86 epoch sec; seqNo:7668; 4058065.67 north; 591661.04 east 2021-02-12T23:52:42Z- AHRS_M2 msg: 1613173962.24 epoch sec; seqNo:7607 2021-02-12T23:52:42Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.22 psi 2021-02-12T23:52:42Z- handleDepth()-Depth_Keller msg: 1613173962242 epoch sec; seqNo:7669; depth 11.16 2021-02-12T23:52:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173962.25 epoch sec; seqNo:7669; 4058066.05 north; 591661.04 east 2021-02-12T23:52:42Z- AHRS_M2 msg: 1613173962.64 epoch sec; seqNo:7608 2021-02-12T23:52:42Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.21 psi 2021-02-12T23:52:42Z- handleDepth()-Depth_Keller msg: 1613173962645 epoch sec; seqNo:7670; depth 11.31 2021-02-12T23:52:42Z- handleDvl() - RDI_Pathfinder msg: 1613173962.66 epoch sec; seqNo:3563 2021-02-12T23:52:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.72 , 105.35 , 79.67 , 118.87 2021-02-12T23:52:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.19 , -0.00 2021-02-12T23:52:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173962.67 epoch sec; seqNo:7670; 4058066.42 north; 591661.03 east 2021-02-12T23:52:43Z- AHRS_M2 msg: 1613173963.04 epoch sec; seqNo:7609 2021-02-12T23:52:43Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-12T23:52:43Z- handleDepth()-Depth_Keller msg: 1613173963053 epoch sec; seqNo:7671; depth 11.44 2021-02-12T23:52:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173963.07 epoch sec; seqNo:7671; 4058066.80 north; 591661.03 east 2021-02-12T23:52:43Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-12T23:52:43Z- waiting for 1613173961.23, time is 1613173963.07 2021-02-12T23:52:43Z- LcmTrn::updateTrn() >>>> pose time 1613173963.07, meas time 1613173962.66 <<<< 2021-02-12T23:52:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173962.66 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:52:43Z- TerrainNav::motionUpdate() - currEstimate = (4058067, 591661.03, 11.44) 2021-02-12T23:52:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:52:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:52:43Z- TNavBF::Projecting Measurements 2021-02-12T23:52:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:52:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:52:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:52:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:52:43Z- TNavFilter::4 good beams. 2021-02-12T23:52:43Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:52:43Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:52:43Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:52:43Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:52:43Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:52:43Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:52:43Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:52:43Z- TNavBF::mmse selected from filter -1 2021-02-12T23:52:43Z- TerrainMapOctree: (vehN, vehE ) = (4058066.49, 591660.83). 2021-02-12T23:52:43Z- TerrainMapOctree: Min Distance = 4103.48. 2021-02-12T23:52:43Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:52:43Z- There is no specific good beam that all particles have in common. 2021-02-12T23:52:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:52:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.39 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 11.21 2021-02-12T23:52:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:52:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 17.76 2021-02-12T23:52:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:52:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 10.15 2021-02-12T23:52:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:52:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 22.61 2021-02-12T23:52:43Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:52:43Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:52:43Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:52:43Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:52:43Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:52:43Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:52:43Z- Filter 0 No beams used in subcloud update 2021-02-12T23:52:43Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:52:43Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:52:43Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 731.2 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:52:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173962.66 sec. 2021-02-12T23:52:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:52:43Z- TNavBF::mmse selected from filter -1 2021-02-12T23:52:43Z- variances([0], [2], and psi_berg) = 731.173827 833.017428 0.000000 2021-02-12T23:52:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:52:43Z- LcmTrn::publishEstimates() - MLE : 4058059.30 591614.53 11.44 2021-02-12T23:52:43Z- LcmTrn::publishEstimates() - MMSE : 4058066.80 591661.03 11.44 2021-02-12T23:52:43Z- LcmTrn::publishEstimates() - COVAR: 731.17 833.02 1.04 0.00 2021-02-12T23:52:43Z- AHRS_M2 msg: 1613173963.45 epoch sec; seqNo:7610 2021-02-12T23:52:43Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.17 psi 2021-02-12T23:52:43Z- handleDepth()-Depth_Keller msg: 1613173963460 epoch sec; seqNo:7672; depth 11.57 2021-02-12T23:52:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173963.48 epoch sec; seqNo:7672; 4058067.18 north; 591661.03 east 2021-02-12T23:52:43Z- AHRS_M2 msg: 1613173963.85 epoch sec; seqNo:7611 2021-02-12T23:52:43Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.15 psi 2021-02-12T23:52:43Z- handleDepth()-Depth_Keller msg: 1613173963858 epoch sec; seqNo:7673; depth 11.70 2021-02-12T23:52:43Z- handleDvl() - RDI_Pathfinder msg: 1613173963.87 epoch sec; seqNo:3564 2021-02-12T23:52:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 104.07 , 77.24 , 117.59 2021-02-12T23:52:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.08 , 0.13 2021-02-12T23:52:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173963.88 epoch sec; seqNo:7673; 4058067.18 north; 591661.03 east 2021-02-12T23:52:44Z- AHRS_M2 msg: 1613173964.26 epoch sec; seqNo:7612 2021-02-12T23:52:44Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.13 psi 2021-02-12T23:52:44Z- handleDepth()-Depth_Keller msg: 1613173964265 epoch sec; seqNo:7674; depth 11.83 2021-02-12T23:52:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173964.29 epoch sec; seqNo:7674; 4058067.57 north; 591662.24 east 2021-02-12T23:52:44Z- AHRS_M2 msg: 1613173964.66 epoch sec; seqNo:7613 2021-02-12T23:52:44Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.13 psi 2021-02-12T23:52:44Z- handleDepth()-Depth_Keller msg: 1613173964783 epoch sec; seqNo:7675; depth 11.95 2021-02-12T23:52:44Z- handleDvl() - RDI_Pathfinder msg: 1613173964.80 epoch sec; seqNo:3565 2021-02-12T23:52:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 101.43 , 78.45 , 113.60 2021-02-12T23:52:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.13 , 0.10 2021-02-12T23:52:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173964.81 epoch sec; seqNo:7675; 4058067.57 north; 591662.24 east 2021-02-12T23:52:45Z- AHRS_M2 msg: 1613173965.07 epoch sec; seqNo:7614 2021-02-12T23:52:45Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.13 psi 2021-02-12T23:52:45Z- handleDepth()-Depth_Keller msg: 1613173965085 epoch sec; seqNo:7676; depth 12.12 2021-02-12T23:52:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173965.10 epoch sec; seqNo:7676; 4058067.95 north; 591662.24 east 2021-02-12T23:52:45Z- AHRS_M2 msg: 1613173965.47 epoch sec; seqNo:7615 2021-02-12T23:52:45Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.14 psi 2021-02-12T23:52:45Z- handleDepth()-Depth_Keller msg: 1613173965505 epoch sec; seqNo:7677; depth 12.20 2021-02-12T23:52:45Z- handleDvl() - RDI_Pathfinder msg: 1613173965.52 epoch sec; seqNo:3566 2021-02-12T23:52:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 98.79 , 76.01 , 116.37 2021-02-12T23:52:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.19 , -0.04 2021-02-12T23:52:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173965.53 epoch sec; seqNo:7677; 4058067.95 north; 591662.24 east 2021-02-12T23:52:45Z- AHRS_M2 msg: 1613173965.87 epoch sec; seqNo:7616 2021-02-12T23:52:45Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.16 psi 2021-02-12T23:52:45Z- handleDepth()-Depth_Keller msg: 1613173965909 epoch sec; seqNo:7678; depth 12.34 2021-02-12T23:52:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:45Z- handleDvl() - RDI_Pathfinder msg: 1613173965.91 epoch sec; seqNo:3567 2021-02-12T23:52:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.19 , -0.04 2021-02-12T23:52:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173965.92 epoch sec; seqNo:7678; 4058068.33 north; 591662.23 east 2021-02-12T23:52:46Z- AHRS_M2 msg: 1613173966.28 epoch sec; seqNo:7617 2021-02-12T23:52:46Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-12T23:52:46Z- handleDepth()-Depth_Keller msg: 1613173966283 epoch sec; seqNo:7679; depth 12.43 2021-02-12T23:52:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173966.30 epoch sec; seqNo:7679; 4058068.72 north; 591663.45 east 2021-02-12T23:52:46Z- AHRS_M2 msg: 1613173966.68 epoch sec; seqNo:7618 2021-02-12T23:52:46Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-12T23:52:46Z- handleDepth()-Depth_Keller msg: 1613173966686 epoch sec; seqNo:7680; depth 12.56 2021-02-12T23:52:46Z- handleDvl() - RDI_Pathfinder msg: 1613173966.70 epoch sec; seqNo:3568 2021-02-12T23:52:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 101.57 , 78.58 , 112.38 2021-02-12T23:52:46Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.09 , -0.14 2021-02-12T23:52:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173966.71 epoch sec; seqNo:7680; 4058069.10 north; 591663.45 east 2021-02-12T23:52:47Z- AHRS_M2 msg: 1613173967.08 epoch sec; seqNo:7619 2021-02-12T23:52:47Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.18 psi 2021-02-12T23:52:47Z- handleDepth()-Depth_Keller msg: 1613173967088 epoch sec; seqNo:7681; depth 12.70 2021-02-12T23:52:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:47Z- handleDvl() - RDI_Pathfinder msg: 1613173967.09 epoch sec; seqNo:3569 2021-02-12T23:52:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:47Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.09 , -0.14 2021-02-12T23:52:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173967.11 epoch sec; seqNo:7681; 4058069.10 north; 591663.45 east 2021-02-12T23:52:47Z- AHRS_M2 msg: 1613173967.49 epoch sec; seqNo:7620 2021-02-12T23:52:47Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.18 psi 2021-02-12T23:52:47Z- handleDepth()-Depth_Keller msg: 1613173967497 epoch sec; seqNo:7682; depth 12.81 2021-02-12T23:52:47Z- handleDvl() - RDI_Pathfinder msg: 1613173967.52 epoch sec; seqNo:3570 2021-02-12T23:52:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 104.27 , 74.37 , 116.44 2021-02-12T23:52:47Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.06 , -0.13 2021-02-12T23:52:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173967.53 epoch sec; seqNo:7682; 4058069.48 north; 591663.44 east 2021-02-12T23:52:47Z- AHRS_M2 msg: 1613173967.89 epoch sec; seqNo:7621 2021-02-12T23:52:47Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-12T23:52:47Z- handleDepth()-Depth_Keller msg: 1613173967896 epoch sec; seqNo:7683; depth 12.95 2021-02-12T23:52:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173967.91 epoch sec; seqNo:7683; 4058069.86 north; 591663.44 east 2021-02-12T23:52:48Z- AHRS_M2 msg: 1613173968.30 epoch sec; seqNo:7622 2021-02-12T23:52:48Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.17 psi 2021-02-12T23:52:48Z- handleDepth()-Depth_Keller msg: 1613173968302 epoch sec; seqNo:7684; depth 13.09 2021-02-12T23:52:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:48Z- handleDvl() - RDI_Pathfinder msg: 1613173968.30 epoch sec; seqNo:3571 2021-02-12T23:52:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:48Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.06 , -0.13 2021-02-12T23:52:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173968.31 epoch sec; seqNo:7684; 4058070.25 north; 591664.66 east 2021-02-12T23:52:48Z- LcmTrn::handleMessages() - handling 47 messages took 0.66 ms and 134 calls... 2021-02-12T23:52:48Z- waiting for 1613173966.07, time is 1613173968.31 2021-02-12T23:52:48Z- LcmTrn::updateTrn() equating pose 1613173968.31 and meas times 1613173968.31 2021-02-12T23:52:48Z- LcmTrn::updateTrn() >>>> pose time 1613173968.31, meas time 1613173968.31 <<<< 2021-02-12T23:52:48Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173968.31, ping # 0. 2021-02-12T23:52:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173968.31 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:52:48Z- TerrainNav::motionUpdate() - currEstimate = (4058070, 591664.66, 13.09) 2021-02-12T23:52:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:52:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:52:48Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:52:48Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:52:48Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173968.31 sec.; No relevant navigation data available 2021-02-12T23:52:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:52:48Z- TNavBF::mmse selected from filter -1 2021-02-12T23:52:48Z- variances([0], [2], and psi_berg) = 731.087888 833.017365 0.000000 2021-02-12T23:52:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:52:48Z- LcmTrn::publishEstimates() - MLE : 4058062.75 591616.16 13.09 2021-02-12T23:52:48Z- LcmTrn::publishEstimates() - MMSE : 4058070.25 591664.66 13.09 2021-02-12T23:52:48Z- LcmTrn::publishEstimates() - COVAR: 731.09 833.02 1.30 0.00 2021-02-12T23:52:48Z- AHRS_M2 msg: 1613173968.70 epoch sec; seqNo:7623 2021-02-12T23:52:48Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.17 psi 2021-02-12T23:52:48Z- handleDepth()-Depth_Keller msg: 1613173968704 epoch sec; seqNo:7685; depth 13.22 2021-02-12T23:52:48Z- handleDvl() - RDI_Pathfinder msg: 1613173968.72 epoch sec; seqNo:3572 2021-02-12T23:52:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 102.92 , 78.58 , 113.73 2021-02-12T23:52:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.10 , -0.10 2021-02-12T23:52:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173968.73 epoch sec; seqNo:7685; 4058070.63 north; 591664.65 east 2021-02-12T23:52:49Z- AHRS_M2 msg: 1613173969.10 epoch sec; seqNo:7624 2021-02-12T23:52:49Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.17 psi 2021-02-12T23:52:49Z- handleDepth()-Depth_Keller msg: 1613173969109 epoch sec; seqNo:7686; depth 13.36 2021-02-12T23:52:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173969.14 epoch sec; seqNo:7686; 4058071.01 north; 591664.65 east 2021-02-12T23:52:49Z- AHRS_M2 msg: 1613173969.51 epoch sec; seqNo:7625 2021-02-12T23:52:49Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.18 psi 2021-02-12T23:52:49Z- handleDepth()-Depth_Keller msg: 1613173969512 epoch sec; seqNo:7687; depth 13.49 2021-02-12T23:52:49Z- handleDvl() - RDI_Pathfinder msg: 1613173969.52 epoch sec; seqNo:3573 2021-02-12T23:52:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.21 , 75.19 , 112.38 2021-02-12T23:52:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.09 , -0.07 2021-02-12T23:52:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173969.53 epoch sec; seqNo:7687; 4058071.39 north; 591664.64 east 2021-02-12T23:52:49Z- AHRS_M2 msg: 1613173969.91 epoch sec; seqNo:7626 2021-02-12T23:52:49Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.18 psi 2021-02-12T23:52:49Z- handleDepth()-Depth_Keller msg: 1613173969918 epoch sec; seqNo:7688; depth 13.62 2021-02-12T23:52:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173969.93 epoch sec; seqNo:7688; 4058071.39 north; 591664.64 east 2021-02-12T23:52:50Z- AHRS_M2 msg: 1613173970.32 epoch sec; seqNo:7627 2021-02-12T23:52:50Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.19 psi 2021-02-12T23:52:50Z- handleDepth()-Depth_Keller msg: 1613173970321 epoch sec; seqNo:7689; depth 13.74 2021-02-12T23:52:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173970.34 epoch sec; seqNo:7689; 4058071.77 north; 591664.64 east 2021-02-12T23:52:50Z- AHRS_M2 msg: 1613173970.72 epoch sec; seqNo:7628 2021-02-12T23:52:50Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.19 psi 2021-02-12T23:52:50Z- handleDepth()-Depth_Keller msg: 1613173970729 epoch sec; seqNo:7690; depth 13.88 2021-02-12T23:52:50Z- handleDvl() - RDI_Pathfinder msg: 1613173970.74 epoch sec; seqNo:3574 2021-02-12T23:52:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.50 , 102.78 , 73.96 , 112.25 2021-02-12T23:52:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.10 , -0.05 2021-02-12T23:52:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173970.76 epoch sec; seqNo:7690; 4058072.15 north; 591664.64 east 2021-02-12T23:52:51Z- AHRS_M2 msg: 1613173971.13 epoch sec; seqNo:7629 2021-02-12T23:52:51Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.19 psi 2021-02-12T23:52:51Z- handleDepth()-Depth_Keller msg: 1613173971132 epoch sec; seqNo:7691; depth 14.00 2021-02-12T23:52:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173971.15 epoch sec; seqNo:7691; 4058072.54 north; 591665.85 east 2021-02-12T23:52:51Z- AHRS_M2 msg: 1613173971.53 epoch sec; seqNo:7630 2021-02-12T23:52:51Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.19 psi 2021-02-12T23:52:51Z- handleDepth()-Depth_Keller msg: 1613173971534 epoch sec; seqNo:7692; depth 14.12 2021-02-12T23:52:51Z- handleDvl() - RDI_Pathfinder msg: 1613173971.55 epoch sec; seqNo:3575 2021-02-12T23:52:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 101.70 , 78.72 , 113.87 2021-02-12T23:52:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.11 , 0.03 2021-02-12T23:52:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173971.56 epoch sec; seqNo:7692; 4058072.54 north; 591665.85 east 2021-02-12T23:52:51Z- AHRS_M2 msg: 1613173971.94 epoch sec; seqNo:7631 2021-02-12T23:52:51Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.20 psi 2021-02-12T23:52:51Z- handleDepth()-Depth_Keller msg: 1613173971952 epoch sec; seqNo:7693; depth 14.24 2021-02-12T23:52:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:51Z- handleDvl() - RDI_Pathfinder msg: 1613173971.96 epoch sec; seqNo:3576 2021-02-12T23:52:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.11 , 0.03 2021-02-12T23:52:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173971.97 epoch sec; seqNo:7693; 4058072.92 north; 591665.85 east 2021-02-12T23:52:52Z- AHRS_M2 msg: 1613173972.34 epoch sec; seqNo:7632 2021-02-12T23:52:52Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.21 psi 2021-02-12T23:52:52Z- handleDepth()-Depth_Keller msg: 1613173972340 epoch sec; seqNo:7694; depth 14.37 2021-02-12T23:52:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173972.35 epoch sec; seqNo:7694; 4058073.30 north; 591665.84 east 2021-02-12T23:52:52Z- AHRS_M2 msg: 1613173972.74 epoch sec; seqNo:7633 2021-02-12T23:52:52Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.21 psi 2021-02-12T23:52:52Z- handleDepth()-Depth_Keller msg: 1613173972749 epoch sec; seqNo:7695; depth 14.46 2021-02-12T23:52:52Z- handleDvl() - RDI_Pathfinder msg: 1613173972.76 epoch sec; seqNo:3577 2021-02-12T23:52:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.29 , 102.92 , 78.58 , 112.38 2021-02-12T23:52:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , 0.09 2021-02-12T23:52:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173972.77 epoch sec; seqNo:7695; 4058073.30 north; 591665.84 east 2021-02-12T23:52:53Z- AHRS_M2 msg: 1613173973.15 epoch sec; seqNo:7634 2021-02-12T23:52:53Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.21 psi 2021-02-12T23:52:53Z- handleDepth()-Depth_Keller msg: 1613173973153 epoch sec; seqNo:7696; depth 14.59 2021-02-12T23:52:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:53Z- handleDvl() - RDI_Pathfinder msg: 1613173973.15 epoch sec; seqNo:3578 2021-02-12T23:52:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , 0.09 2021-02-12T23:52:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173973.17 epoch sec; seqNo:7696; 4058073.68 north; 591665.84 east 2021-02-12T23:52:53Z- AHRS_M2 msg: 1613173973.55 epoch sec; seqNo:7635 2021-02-12T23:52:53Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.21 psi 2021-02-12T23:52:53Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-12T23:52:53Z- waiting for 1613173971.31, time is 1613173973.55 2021-02-12T23:52:53Z- LcmTrn::updateTrn() >>>> pose time 1613173973.55, meas time 1613173973.15 <<<< 2021-02-12T23:52:53Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613173973.15, ping # 0. 2021-02-12T23:52:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173973.15 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:52:53Z- TerrainNav::motionUpdate() - currEstimate = (4058074, 591665.84, 14.59) 2021-02-12T23:52:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:52:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:52:53Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:52:53Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:52:53Z- TNavBF::Projecting Measurements 2021-02-12T23:52:53Z- TNavFilter::Measurement from time = 1613173973.15 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:52:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:52:53Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:52:53Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:52:53Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:52:53Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:52:53Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:52:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:52:53Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 731.0 EastVariance: 833.0 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-12T23:52:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:52:53Z- TNavBF::mmse selected from filter -1 2021-02-12T23:52:53Z- variances([0], [2], and psi_berg) = 730.997656 833.017298 0.000000 2021-02-12T23:52:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:52:53Z- LcmTrn::publishEstimates() - MLE : 4058066.18 591617.34 14.59 2021-02-12T23:52:53Z- LcmTrn::publishEstimates() - MMSE : 4058073.68 591665.84 14.59 2021-02-12T23:52:53Z- LcmTrn::publishEstimates() - COVAR: 731.00 833.02 1.48 0.00 2021-02-12T23:52:53Z- handleDepth()-Depth_Keller msg: 1613173973553 epoch sec; seqNo:7697; depth 14.70 2021-02-12T23:52:53Z- handleDvl() - RDI_Pathfinder msg: 1613173973.57 epoch sec; seqNo:3579 2021-02-12T23:52:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.36 , 102.99 , 73.56 , 112.45 2021-02-12T23:52:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , 0.16 2021-02-12T23:52:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173973.58 epoch sec; seqNo:7697; 4058074.05 north; 591665.84 east 2021-02-12T23:52:53Z- AHRS_M2 msg: 1613173973.95 epoch sec; seqNo:7636 2021-02-12T23:52:53Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.21 psi 2021-02-12T23:52:53Z- handleDepth()-Depth_Keller msg: 1613173973957 epoch sec; seqNo:7698; depth 14.83 2021-02-12T23:52:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173973.97 epoch sec; seqNo:7698; 4058074.05 north; 591665.84 east 2021-02-12T23:52:54Z- AHRS_M2 msg: 1613173974.36 epoch sec; seqNo:7637 2021-02-12T23:52:54Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.21 psi 2021-02-12T23:52:54Z- handleDepth()-Depth_Keller msg: 1613173974360 epoch sec; seqNo:7699; depth 14.93 2021-02-12T23:52:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:54Z- handleDvl() - RDI_Pathfinder msg: 1613173974.36 epoch sec; seqNo:3580 2021-02-12T23:52:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , 0.16 2021-02-12T23:52:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173974.37 epoch sec; seqNo:7699; 4058074.45 north; 591667.05 east 2021-02-12T23:52:54Z- AHRS_M2 msg: 1613173974.76 epoch sec; seqNo:7638 2021-02-12T23:52:54Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.21 psi 2021-02-12T23:52:54Z- handleDepth()-Depth_Keller msg: 1613173974766 epoch sec; seqNo:7700; depth 15.06 2021-02-12T23:52:54Z- handleDvl() - RDI_Pathfinder msg: 1613173974.78 epoch sec; seqNo:3581 2021-02-12T23:52:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.07 , 101.70 , 78.72 , 111.17 2021-02-12T23:52:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.03 , 0.12 2021-02-12T23:52:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173974.79 epoch sec; seqNo:7700; 4058074.82 north; 591667.05 east 2021-02-12T23:52:55Z- AHRS_M2 msg: 1613173975.16 epoch sec; seqNo:7639 2021-02-12T23:52:55Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.22 psi 2021-02-12T23:52:55Z- handleDepth()-Depth_Keller msg: 1613173975169 epoch sec; seqNo:7701; depth 15.18 2021-02-12T23:52:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173975.19 epoch sec; seqNo:7701; 4058075.20 north; 591667.04 east 2021-02-12T23:52:55Z- AHRS_M2 msg: 1613173975.57 epoch sec; seqNo:7640 2021-02-12T23:52:55Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.23 psi 2021-02-12T23:52:55Z- handleDepth()-Depth_Keller msg: 1613173975573 epoch sec; seqNo:7702; depth 15.30 2021-02-12T23:52:55Z- handleDvl() - RDI_Pathfinder msg: 1613173975.60 epoch sec; seqNo:3582 2021-02-12T23:52:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.69 , 100.35 , 74.37 , 111.17 2021-02-12T23:52:55Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.06 , 0.02 2021-02-12T23:52:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173975.61 epoch sec; seqNo:7702; 4058075.58 north; 591667.04 east 2021-02-12T23:52:55Z- AHRS_M2 msg: 1613173975.98 epoch sec; seqNo:7641 2021-02-12T23:52:55Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.23 psi 2021-02-12T23:52:55Z- handleDepth()-Depth_Keller msg: 1613173975983 epoch sec; seqNo:7703; depth 15.43 2021-02-12T23:52:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173975.99 epoch sec; seqNo:7703; 4058075.58 north; 591667.04 east 2021-02-12T23:52:56Z- AHRS_M2 msg: 1613173976.39 epoch sec; seqNo:7642 2021-02-12T23:52:56Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.23 psi 2021-02-12T23:52:56Z- handleDepth()-Depth_Keller msg: 1613173976401 epoch sec; seqNo:7704; depth 15.56 2021-02-12T23:52:56Z- handleDvl() - RDI_Pathfinder msg: 1613173976.41 epoch sec; seqNo:3583 2021-02-12T23:52:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.05 , 102.51 , 73.56 , 112.25 2021-02-12T23:52:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.01 , -0.08 2021-02-12T23:52:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173976.42 epoch sec; seqNo:7704; 4058075.97 north; 591668.26 east 2021-02-12T23:52:56Z- AHRS_M2 msg: 1613173976.78 epoch sec; seqNo:7643 2021-02-12T23:52:56Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.23 psi 2021-02-12T23:52:56Z- handleDepth()-Depth_Keller msg: 1613173976785 epoch sec; seqNo:7705; depth 15.66 2021-02-12T23:52:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173976.80 epoch sec; seqNo:7705; 4058076.35 north; 591668.25 east 2021-02-12T23:52:57Z- AHRS_M2 msg: 1613173977.18 epoch sec; seqNo:7644 2021-02-12T23:52:57Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.22 psi 2021-02-12T23:52:57Z- handleDepth()-Depth_Keller msg: 1613173977193 epoch sec; seqNo:7706; depth 15.81 2021-02-12T23:52:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173977.20 epoch sec; seqNo:7706; 4058076.73 north; 591668.25 east 2021-02-12T23:52:57Z- AHRS_M2 msg: 1613173977.59 epoch sec; seqNo:7645 2021-02-12T23:52:57Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.22 psi 2021-02-12T23:52:57Z- handleDepth()-Depth_Keller msg: 1613173977597 epoch sec; seqNo:7707; depth 15.90 2021-02-12T23:52:57Z- handleDvl() - RDI_Pathfinder msg: 1613173977.61 epoch sec; seqNo:3584 2021-02-12T23:52:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.64 , 103.73 , 78.18 , 108.60 2021-02-12T23:52:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.10 , -0.09 2021-02-12T23:52:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173977.62 epoch sec; seqNo:7707; 4058077.11 north; 591668.25 east 2021-02-12T23:52:57Z- AHRS_M2 msg: 1613173977.99 epoch sec; seqNo:7646 2021-02-12T23:52:57Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.20 psi 2021-02-12T23:52:58Z- handleDepth()-Depth_Keller msg: 1613173978063 epoch sec; seqNo:7708; depth 16.02 2021-02-12T23:52:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:58Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:58Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:58Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:58Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:58Z- handleDvl() - RDI_Pathfinder msg: 1613173978.07 epoch sec; seqNo:3585 2021-02-12T23:52:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.10 , -0.09 2021-02-12T23:52:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173978.08 epoch sec; seqNo:7708; 4058077.49 north; 591668.24 east 2021-02-12T23:52:58Z- AHRS_M2 msg: 1613173978.40 epoch sec; seqNo:7647 2021-02-12T23:52:58Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.19 psi 2021-02-12T23:52:58Z- handleDepth()-Depth_Keller msg: 1613173978474 epoch sec; seqNo:7709; depth 16.16 2021-02-12T23:52:58Z- handleDvl() - RDI_Pathfinder msg: 1613173978.49 epoch sec; seqNo:3586 2021-02-12T23:52:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.05 , 100.42 , 74.37 , 107.18 2021-02-12T23:52:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.12 , -0.03 2021-02-12T23:52:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173978.50 epoch sec; seqNo:7709; 4058077.87 north; 591668.24 east 2021-02-12T23:52:58Z- LcmTrn::handleMessages() - handling 46 messages took 0.73 ms and 132 calls... 2021-02-12T23:52:58Z- waiting for 1613173976.55, time is 1613173978.50 2021-02-12T23:52:58Z- LcmTrn::updateTrn() equating pose 1613173978.50 and meas times 1613173978.50 2021-02-12T23:52:58Z- LcmTrn::updateTrn() >>>> pose time 1613173978.50, meas time 1613173978.50 <<<< 2021-02-12T23:52:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173978.50 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:52:58Z- TerrainNav::motionUpdate() - currEstimate = (4058078, 591668.24, 16.16) 2021-02-12T23:52:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:52:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:52:58Z- TNavBF::Projecting Measurements 2021-02-12T23:52:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:52:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:52:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:52:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:52:58Z- TNavFilter::4 good beams. 2021-02-12T23:52:58Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:52:58Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:52:58Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:52:58Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:52:58Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:52:58Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:52:58Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:52:58Z- TNavBF::mmse selected from filter -1 2021-02-12T23:52:58Z- TerrainMapOctree: (vehN, vehE ) = (4058077.87, 591668.24). 2021-02-12T23:52:58Z- TerrainMapOctree: Min Distance = 4089.98. 2021-02-12T23:52:58Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:52:58Z- There is no specific good beam that all particles have in common. 2021-02-12T23:52:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:52:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.73 K: 0.131 ExpectedMeasDiff: nan currMeas.covariance: 9.75 2021-02-12T23:52:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:52:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.50 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 16.13 2021-02-12T23:52:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:52:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.39 K: 0.116 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-12T23:52:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:52:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 18.38 2021-02-12T23:52:58Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:52:58Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:52:58Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:52:58Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:52:58Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:52:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:52:58Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:52:58Z- Filter 0 No beams used in subcloud update 2021-02-12T23:52:58Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:52:58Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:52:58Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 730.9 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:52:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173978.50 sec. 2021-02-12T23:52:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:52:58Z- TNavBF::mmse selected from filter -1 2021-02-12T23:52:58Z- variances([0], [2], and psi_berg) = 730.913401 833.017236 0.000000 2021-02-12T23:52:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:52:58Z- LcmTrn::publishEstimates() - MLE : 4058070.37 591620.74 16.16 2021-02-12T23:52:58Z- LcmTrn::publishEstimates() - MMSE : 4058077.87 591668.24 16.16 2021-02-12T23:52:58Z- LcmTrn::publishEstimates() - COVAR: 730.91 833.02 1.16 0.00 2021-02-12T23:52:58Z- AHRS_M2 msg: 1613173978.80 epoch sec; seqNo:7648 2021-02-12T23:52:58Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.18 psi 2021-02-12T23:52:58Z- handleDepth()-Depth_Keller msg: 1613173978845 epoch sec; seqNo:7710; depth 16.27 2021-02-12T23:52:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173978.87 epoch sec; seqNo:7710; 4058077.87 north; 591668.24 east 2021-02-12T23:52:59Z- AHRS_M2 msg: 1613173979.20 epoch sec; seqNo:7649 2021-02-12T23:52:59Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.17 psi 2021-02-12T23:52:59Z- handleDepth()-Depth_Keller msg: 1613173979241 epoch sec; seqNo:7711; depth 16.39 2021-02-12T23:52:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:52:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:52:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:52:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:52:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:52:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:52:59Z- handleDvl() - RDI_Pathfinder msg: 1613173979.24 epoch sec; seqNo:3587 2021-02-12T23:52:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:52:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.12 , -0.03 2021-02-12T23:52:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173979.26 epoch sec; seqNo:7711; 4058078.26 north; 591669.45 east 2021-02-12T23:52:59Z- AHRS_M2 msg: 1613173979.61 epoch sec; seqNo:7650 2021-02-12T23:52:59Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.16 psi 2021-02-12T23:52:59Z- handleDepth()-Depth_Keller msg: 1613173979614 epoch sec; seqNo:7712; depth 16.50 2021-02-12T23:52:59Z- handleDvl() - RDI_Pathfinder msg: 1613173979.63 epoch sec; seqNo:3588 2021-02-12T23:52:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.75 , 97.78 , 78.31 , 107.51 2021-02-12T23:52:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.11 , 0.00 2021-02-12T23:52:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173979.64 epoch sec; seqNo:7712; 4058078.64 north; 591669.45 east 2021-02-12T23:53:00Z- AHRS_M2 msg: 1613173980.02 epoch sec; seqNo:7651 2021-02-12T23:53:00Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.15 psi 2021-02-12T23:53:00Z- handleDepth()-Depth_Keller msg: 1613173980046 epoch sec; seqNo:7713; depth 16.60 2021-02-12T23:53:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173980.06 epoch sec; seqNo:7713; 4058079.02 north; 591669.45 east 2021-02-12T23:53:00Z- AHRS_M2 msg: 1613173980.42 epoch sec; seqNo:7652 2021-02-12T23:53:00Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.15 psi 2021-02-12T23:53:00Z- handleDepth()-Depth_Keller msg: 1613173980431 epoch sec; seqNo:7714; depth 16.74 2021-02-12T23:53:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:00Z- handleDvl() - RDI_Pathfinder msg: 1613173980.44 epoch sec; seqNo:3589 2021-02-12T23:53:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.11 , 0.00 2021-02-12T23:53:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173980.47 epoch sec; seqNo:7714; 4058079.40 north; 591669.44 east 2021-02-12T23:53:00Z- AHRS_M2 msg: 1613173980.82 epoch sec; seqNo:7653 2021-02-12T23:53:00Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.15 psi 2021-02-12T23:53:00Z- handleDepth()-Depth_Keller msg: 1613173980827 epoch sec; seqNo:7715; depth 16.82 2021-02-12T23:53:00Z- handleDvl() - RDI_Pathfinder msg: 1613173980.84 epoch sec; seqNo:3590 2021-02-12T23:53:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 97.65 , 73.15 , 107.38 2021-02-12T23:53:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.01 2021-02-12T23:53:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173980.85 epoch sec; seqNo:7715; 4058079.40 north; 591669.44 east 2021-02-12T23:53:01Z- AHRS_M2 msg: 1613173981.22 epoch sec; seqNo:7654 2021-02-12T23:53:01Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.15 psi 2021-02-12T23:53:01Z- handleDepth()-Depth_Keller msg: 1613173981233 epoch sec; seqNo:7716; depth 16.95 2021-02-12T23:53:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173981.25 epoch sec; seqNo:7716; 4058079.78 north; 591669.44 east 2021-02-12T23:53:01Z- AHRS_M2 msg: 1613173981.63 epoch sec; seqNo:7655 2021-02-12T23:53:01Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.16 psi 2021-02-12T23:53:01Z- handleDepth()-Depth_Keller msg: 1613173981632 epoch sec; seqNo:7717; depth 17.08 2021-02-12T23:53:01Z- handleDvl() - RDI_Pathfinder msg: 1613173981.64 epoch sec; seqNo:3591 2021-02-12T23:53:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.75 , 97.78 , 78.31 , 107.51 2021-02-12T23:53:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.09 , -0.04 2021-02-12T23:53:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173981.65 epoch sec; seqNo:7717; 4058080.15 north; 591669.43 east 2021-02-12T23:53:02Z- AHRS_M2 msg: 1613173982.03 epoch sec; seqNo:7656 2021-02-12T23:53:02Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.16 psi 2021-02-12T23:53:02Z- handleDepth()-Depth_Keller msg: 1613173982038 epoch sec; seqNo:7718; depth 17.18 2021-02-12T23:53:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173982.05 epoch sec; seqNo:7718; 4058080.53 north; 591669.43 east 2021-02-12T23:53:02Z- AHRS_M2 msg: 1613173982.46 epoch sec; seqNo:7657 2021-02-12T23:53:02Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.17 psi 2021-02-12T23:53:02Z- handleDepth()-Depth_Keller msg: 1613173982469 epoch sec; seqNo:7719; depth 17.30 2021-02-12T23:53:02Z- handleDvl() - RDI_Pathfinder msg: 1613173982.48 epoch sec; seqNo:3592 2021-02-12T23:53:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.39 , 97.65 , 71.11 , 107.38 2021-02-12T23:53:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.13 , -0.03 2021-02-12T23:53:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173982.51 epoch sec; seqNo:7719; 4058080.93 north; 591670.65 east 2021-02-12T23:53:02Z- AHRS_M2 msg: 1613173982.84 epoch sec; seqNo:7658 2021-02-12T23:53:02Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.17 psi 2021-02-12T23:53:02Z- handleDepth()-Depth_Keller msg: 1613173982844 epoch sec; seqNo:7720; depth 17.44 2021-02-12T23:53:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:02Z- handleDvl() - RDI_Pathfinder msg: 1613173982.85 epoch sec; seqNo:3593 2021-02-12T23:53:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.13 , -0.03 2021-02-12T23:53:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173982.87 epoch sec; seqNo:7720; 4058080.93 north; 591670.65 east 2021-02-12T23:53:03Z- AHRS_M2 msg: 1613173983.24 epoch sec; seqNo:7659 2021-02-12T23:53:03Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-12T23:53:03Z- handleDepth()-Depth_Keller msg: 1613173983248 epoch sec; seqNo:7721; depth 17.54 2021-02-12T23:53:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173983.26 epoch sec; seqNo:7721; 4058081.30 north; 591670.64 east 2021-02-12T23:53:03Z- AHRS_M2 msg: 1613173983.65 epoch sec; seqNo:7660 2021-02-12T23:53:03Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-12T23:53:03Z- handleDepth()-Depth_Keller msg: 1613173983653 epoch sec; seqNo:7722; depth 17.67 2021-02-12T23:53:03Z- handleDvl() - RDI_Pathfinder msg: 1613173983.66 epoch sec; seqNo:3594 2021-02-12T23:53:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.59 , 97.85 , 70.29 , 107.58 2021-02-12T23:53:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.13 , 0.01 2021-02-12T23:53:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173983.67 epoch sec; seqNo:7722; 4058081.68 north; 591670.64 east 2021-02-12T23:53:03Z- LcmTrn::handleMessages() - handling 47 messages took 0.79 ms and 131 calls... 2021-02-12T23:53:03Z- waiting for 1613173981.50, time is 1613173983.67 2021-02-12T23:53:03Z- LcmTrn::updateTrn() equating pose 1613173983.67 and meas times 1613173983.67 2021-02-12T23:53:03Z- LcmTrn::updateTrn() >>>> pose time 1613173983.67, meas time 1613173983.67 <<<< 2021-02-12T23:53:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173983.67 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:53:03Z- TerrainNav::motionUpdate() - currEstimate = (4058082, 591670.64, 17.67) 2021-02-12T23:53:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:53:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:53:03Z- TerrainNav::Measurement type 1 not incorporated from time = 1613173983.67 sec.; No relevant navigation data available 2021-02-12T23:53:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:53:03Z- TNavBF::mmse selected from filter -1 2021-02-12T23:53:03Z- variances([0], [2], and psi_berg) = 730.832093 833.017176 0.000000 2021-02-12T23:53:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:53:03Z- LcmTrn::publishEstimates() - MLE : 4058074.18 591623.14 17.67 2021-02-12T23:53:03Z- LcmTrn::publishEstimates() - MMSE : 4058081.68 591670.64 17.67 2021-02-12T23:53:03Z- LcmTrn::publishEstimates() - COVAR: 730.83 833.02 1.39 0.00 2021-02-12T23:53:04Z- AHRS_M2 msg: 1613173984.05 epoch sec; seqNo:7661 2021-02-12T23:53:04Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.17 psi 2021-02-12T23:53:04Z- handleDepth()-Depth_Keller msg: 1613173984065 epoch sec; seqNo:7723; depth 17.79 2021-02-12T23:53:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:04Z- handleDvl() - RDI_Pathfinder msg: 1613173984.07 epoch sec; seqNo:3595 2021-02-12T23:53:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.13 , 0.01 2021-02-12T23:53:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173984.08 epoch sec; seqNo:7723; 4058082.06 north; 591670.63 east 2021-02-12T23:53:04Z- AHRS_M2 msg: 1613173984.45 epoch sec; seqNo:7662 2021-02-12T23:53:04Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.17 psi 2021-02-12T23:53:04Z- handleDepth()-Depth_Keller msg: 1613173984464 epoch sec; seqNo:7724; depth 17.92 2021-02-12T23:53:04Z- handleDvl() - RDI_Pathfinder msg: 1613173984.48 epoch sec; seqNo:3596 2021-02-12T23:53:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.06 , 97.85 , 71.93 , 107.58 2021-02-12T23:53:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.12 , 0.06 2021-02-12T23:53:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173984.50 epoch sec; seqNo:7724; 4058082.44 north; 591670.63 east 2021-02-12T23:53:04Z- AHRS_M2 msg: 1613173984.86 epoch sec; seqNo:7663 2021-02-12T23:53:04Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.16 psi 2021-02-12T23:53:04Z- handleDepth()-Depth_Keller msg: 1613173984865 epoch sec; seqNo:7725; depth 18.04 2021-02-12T23:53:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173984.88 epoch sec; seqNo:7725; 4058082.44 north; 591670.63 east 2021-02-12T23:53:05Z- AHRS_M2 msg: 1613173985.27 epoch sec; seqNo:7664 2021-02-12T23:53:05Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.16 psi 2021-02-12T23:53:05Z- handleDepth()-Depth_Keller msg: 1613173985270 epoch sec; seqNo:7726; depth 18.16 2021-02-12T23:53:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:05Z- handleDvl() - RDI_Pathfinder msg: 1613173985.27 epoch sec; seqNo:3597 2021-02-12T23:53:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.12 , 0.06 2021-02-12T23:53:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173985.28 epoch sec; seqNo:7726; 4058082.83 north; 591671.85 east 2021-02-12T23:53:05Z- AHRS_M2 msg: 1613173985.67 epoch sec; seqNo:7665 2021-02-12T23:53:05Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.16 psi 2021-02-12T23:53:05Z- handleDepth()-Depth_Keller msg: 1613173985672 epoch sec; seqNo:7727; depth 18.29 2021-02-12T23:53:05Z- handleDvl() - RDI_Pathfinder msg: 1613173985.69 epoch sec; seqNo:3598 2021-02-12T23:53:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.46 , 94.20 , 71.51 , 106.37 2021-02-12T23:53:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.07 2021-02-12T23:53:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173985.70 epoch sec; seqNo:7727; 4058083.21 north; 591671.84 east 2021-02-12T23:53:06Z- AHRS_M2 msg: 1613173986.07 epoch sec; seqNo:7666 2021-02-12T23:53:06Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.16 psi 2021-02-12T23:53:06Z- handleDepth()-Depth_Keller msg: 1613173986076 epoch sec; seqNo:7728; depth 18.41 2021-02-12T23:53:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173986.09 epoch sec; seqNo:7728; 4058083.21 north; 591671.84 east 2021-02-12T23:53:06Z- AHRS_M2 msg: 1613173986.48 epoch sec; seqNo:7667 2021-02-12T23:53:06Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.16 psi 2021-02-12T23:53:06Z- handleDepth()-Depth_Keller msg: 1613173986482 epoch sec; seqNo:7729; depth 18.52 2021-02-12T23:53:06Z- handleDvl() - RDI_Pathfinder msg: 1613173986.49 epoch sec; seqNo:3599 2021-02-12T23:53:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.98 , 70.29 , 106.50 2021-02-12T23:53:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , 0.05 2021-02-12T23:53:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173986.51 epoch sec; seqNo:7729; 4058083.59 north; 591671.84 east 2021-02-12T23:53:06Z- AHRS_M2 msg: 1613173986.88 epoch sec; seqNo:7668 2021-02-12T23:53:06Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.16 psi 2021-02-12T23:53:06Z- handleDepth()-Depth_Keller msg: 1613173986884 epoch sec; seqNo:7730; depth 18.66 2021-02-12T23:53:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173986.90 epoch sec; seqNo:7730; 4058083.97 north; 591671.84 east 2021-02-12T23:53:07Z- AHRS_M2 msg: 1613173987.28 epoch sec; seqNo:7669 2021-02-12T23:53:07Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.17 psi 2021-02-12T23:53:07Z- handleDepth()-Depth_Keller msg: 1613173987289 epoch sec; seqNo:7731; depth 18.77 2021-02-12T23:53:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173987.30 epoch sec; seqNo:7731; 4058083.97 north; 591671.84 east 2021-02-12T23:53:07Z- AHRS_M2 msg: 1613173987.69 epoch sec; seqNo:7670 2021-02-12T23:53:07Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.17 psi 2021-02-12T23:53:07Z- handleDepth()-Depth_Keller msg: 1613173987697 epoch sec; seqNo:7732; depth 18.90 2021-02-12T23:53:07Z- handleDvl() - RDI_Pathfinder msg: 1613173987.71 epoch sec; seqNo:3600 2021-02-12T23:53:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.05 , 97.65 , 69.06 , 106.16 2021-02-12T23:53:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , -0.00 2021-02-12T23:53:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173987.75 epoch sec; seqNo:7732; 4058084.36 north; 591673.05 east 2021-02-12T23:53:08Z- AHRS_M2 msg: 1613173988.10 epoch sec; seqNo:7671 2021-02-12T23:53:08Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.18 psi 2021-02-12T23:53:08Z- handleDepth()-Depth_Keller msg: 1613173988110 epoch sec; seqNo:7733; depth 19.04 2021-02-12T23:53:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173988.13 epoch sec; seqNo:7733; 4058084.74 north; 591673.05 east 2021-02-12T23:53:08Z- AHRS_M2 msg: 1613173988.50 epoch sec; seqNo:7672 2021-02-12T23:53:08Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.18 psi 2021-02-12T23:53:08Z- handleDepth()-Depth_Keller msg: 1613173988501 epoch sec; seqNo:7734; depth 19.16 2021-02-12T23:53:08Z- handleDvl() - RDI_Pathfinder msg: 1613173988.52 epoch sec; seqNo:3601 2021-02-12T23:53:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.42 , 97.98 , 69.47 , 107.72 2021-02-12T23:53:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.03 , -0.05 2021-02-12T23:53:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173988.53 epoch sec; seqNo:7734; 4058085.12 north; 591673.04 east 2021-02-12T23:53:08Z- LcmTrn::handleMessages() - handling 43 messages took 0.69 ms and 129 calls... 2021-02-12T23:53:08Z- waiting for 1613173986.67, time is 1613173988.53 2021-02-12T23:53:08Z- LcmTrn::updateTrn() equating pose 1613173988.53 and meas times 1613173988.53 2021-02-12T23:53:08Z- LcmTrn::updateTrn() >>>> pose time 1613173988.53, meas time 1613173988.53 <<<< 2021-02-12T23:53:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173988.53 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:53:08Z- TerrainNav::motionUpdate() - currEstimate = (4058085, 591673.04, 19.16) 2021-02-12T23:53:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:53:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:53:08Z- TNavBF::Projecting Measurements 2021-02-12T23:53:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:53:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:53:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:53:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:53:08Z- TNavFilter::4 good beams. 2021-02-12T23:53:08Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:53:08Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:53:08Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:53:08Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:53:08Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:53:08Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:53:08Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:53:08Z- TNavBF::mmse selected from filter -1 2021-02-12T23:53:08Z- TerrainMapOctree: (vehN, vehE ) = (4058085.12, 591673.04). 2021-02-12T23:53:08Z- TerrainMapOctree: Min Distance = 4081.32. 2021-02-12T23:53:08Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:53:08Z- There is no specific good beam that all particles have in common. 2021-02-12T23:53:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:53:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.60 K: 0.126 ExpectedMeasDiff: nan currMeas.covariance: 9.34 2021-02-12T23:53:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:53:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-12T23:53:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:53:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.120 ExpectedMeasDiff: nan currMeas.covariance: 7.72 2021-02-12T23:53:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:53:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 18.57 2021-02-12T23:53:08Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:53:08Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:53:08Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:53:08Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:53:08Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:53:08Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:53:08Z- Filter 0 No beams used in subcloud update 2021-02-12T23:53:08Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:53:08Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:53:08Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 730.8 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:53:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173988.53 sec. 2021-02-12T23:53:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:53:08Z- TNavBF::mmse selected from filter -1 2021-02-12T23:53:08Z- variances([0], [2], and psi_berg) = 730.753739 833.017118 0.000000 2021-02-12T23:53:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:53:08Z- LcmTrn::publishEstimates() - MLE : 4058077.62 591625.54 19.16 2021-02-12T23:53:08Z- LcmTrn::publishEstimates() - MMSE : 4058085.12 591673.04 19.16 2021-02-12T23:53:08Z- LcmTrn::publishEstimates() - COVAR: 730.75 833.02 1.08 0.00 2021-02-12T23:53:08Z- AHRS_M2 msg: 1613173988.90 epoch sec; seqNo:7673 2021-02-12T23:53:08Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.18 psi 2021-02-12T23:53:08Z- handleDepth()-Depth_Keller msg: 1613173988905 epoch sec; seqNo:7735; depth 19.31 2021-02-12T23:53:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:08Z- handleDvl() - RDI_Pathfinder msg: 1613173988.91 epoch sec; seqNo:3602 2021-02-12T23:53:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.03 , -0.05 2021-02-12T23:53:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173988.92 epoch sec; seqNo:7735; 4058085.12 north; 591673.04 east 2021-02-12T23:53:09Z- AHRS_M2 msg: 1613173989.30 epoch sec; seqNo:7674 2021-02-12T23:53:09Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.18 psi 2021-02-12T23:53:09Z- handleDepth()-Depth_Keller msg: 1613173989313 epoch sec; seqNo:7736; depth 19.44 2021-02-12T23:53:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173989.32 epoch sec; seqNo:7736; 4058085.50 north; 591673.04 east 2021-02-12T23:53:09Z- AHRS_M2 msg: 1613173989.71 epoch sec; seqNo:7675 2021-02-12T23:53:09Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.18 psi 2021-02-12T23:53:09Z- handleDepth()-Depth_Keller msg: 1613173989715 epoch sec; seqNo:7737; depth 19.58 2021-02-12T23:53:09Z- handleDvl() - RDI_Pathfinder msg: 1613173989.73 epoch sec; seqNo:3603 2021-02-12T23:53:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 97.92 , 69.88 , 107.65 2021-02-12T23:53:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , -0.10 2021-02-12T23:53:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173989.74 epoch sec; seqNo:7737; 4058085.88 north; 591673.04 east 2021-02-12T23:53:10Z- AHRS_M2 msg: 1613173990.11 epoch sec; seqNo:7676 2021-02-12T23:53:10Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.18 psi 2021-02-12T23:53:10Z- handleDepth()-Depth_Keller msg: 1613173990119 epoch sec; seqNo:7738; depth 19.72 2021-02-12T23:53:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:10Z- handleDvl() - RDI_Pathfinder msg: 1613173990.12 epoch sec; seqNo:3604 2021-02-12T23:53:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , -0.10 2021-02-12T23:53:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173990.15 epoch sec; seqNo:7738; 4058086.27 north; 591674.25 east 2021-02-12T23:53:10Z- AHRS_M2 msg: 1613173990.52 epoch sec; seqNo:7677 2021-02-12T23:53:10Z- AHRS_M2 msg: 0.70 phi; -0.35 theta; -0.18 psi 2021-02-12T23:53:10Z- handleDepth()-Depth_Keller msg: 1613173990521 epoch sec; seqNo:7739; depth 19.86 2021-02-12T23:53:10Z- handleDvl() - RDI_Pathfinder msg: 1613173990.53 epoch sec; seqNo:3605 2021-02-12T23:53:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.01 , 97.92 , 70.29 , 107.65 2021-02-12T23:53:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , -0.11 2021-02-12T23:53:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173990.56 epoch sec; seqNo:7739; 4058086.65 north; 591674.25 east 2021-02-12T23:53:10Z- AHRS_M2 msg: 1613173990.92 epoch sec; seqNo:7678 2021-02-12T23:53:10Z- AHRS_M2 msg: 0.69 phi; -0.36 theta; -0.19 psi 2021-02-12T23:53:10Z- handleDepth()-Depth_Keller msg: 1613173990925 epoch sec; seqNo:7740; depth 20.01 2021-02-12T23:53:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173990.93 epoch sec; seqNo:7740; 4058086.65 north; 591674.25 east 2021-02-12T23:53:11Z- AHRS_M2 msg: 1613173991.33 epoch sec; seqNo:7679 2021-02-12T23:53:11Z- AHRS_M2 msg: 0.69 phi; -0.36 theta; -0.19 psi 2021-02-12T23:53:11Z- handleDepth()-Depth_Keller msg: 1613173991397 epoch sec; seqNo:7741; depth 20.12 2021-02-12T23:53:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:11Z- handleDvl() - RDI_Pathfinder msg: 1613173991.40 epoch sec; seqNo:3606 2021-02-12T23:53:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , -0.11 2021-02-12T23:53:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173991.41 epoch sec; seqNo:7741; 4058087.03 north; 591674.24 east 2021-02-12T23:53:11Z- AHRS_M2 msg: 1613173991.73 epoch sec; seqNo:7680 2021-02-12T23:53:11Z- AHRS_M2 msg: 0.69 phi; -0.36 theta; -0.20 psi 2021-02-12T23:53:11Z- handleDepth()-Depth_Keller msg: 1613173991748 epoch sec; seqNo:7742; depth 20.28 2021-02-12T23:53:11Z- handleDvl() - RDI_Pathfinder msg: 1613173991.78 epoch sec; seqNo:3607 2021-02-12T23:53:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 67.84 , 108.86 2021-02-12T23:53:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.15 , -0.05 2021-02-12T23:53:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173991.80 epoch sec; seqNo:7742; 4058087.40 north; 591674.24 east 2021-02-12T23:53:12Z- AHRS_M2 msg: 1613173992.13 epoch sec; seqNo:7681 2021-02-12T23:53:12Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.20 psi 2021-02-12T23:53:12Z- handleDepth()-Depth_Keller msg: 1613173992173 epoch sec; seqNo:7743; depth 20.41 2021-02-12T23:53:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173992.18 epoch sec; seqNo:7743; 4058087.78 north; 591674.24 east 2021-02-12T23:53:12Z- AHRS_M2 msg: 1613173992.54 epoch sec; seqNo:7682 2021-02-12T23:53:12Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.20 psi 2021-02-12T23:53:12Z- handleDepth()-Depth_Keller msg: 1613173992633 epoch sec; seqNo:7744; depth 20.55 2021-02-12T23:53:12Z- handleDvl() - RDI_Pathfinder msg: 1613173992.65 epoch sec; seqNo:3608 2021-02-12T23:53:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.91 , 78.44 , 107.65 2021-02-12T23:53:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.09 , 0.07 2021-02-12T23:53:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173992.66 epoch sec; seqNo:7744; 4058087.78 north; 591674.24 east 2021-02-12T23:53:12Z- AHRS_M2 msg: 1613173992.94 epoch sec; seqNo:7683 2021-02-12T23:53:12Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.21 psi 2021-02-12T23:53:12Z- handleDepth()-Depth_Keller msg: 1613173992959 epoch sec; seqNo:7745; depth 20.70 2021-02-12T23:53:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173992.97 epoch sec; seqNo:7745; 4058088.18 north; 591675.45 east 2021-02-12T23:53:13Z- AHRS_M2 msg: 1613173993.35 epoch sec; seqNo:7684 2021-02-12T23:53:13Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.21 psi 2021-02-12T23:53:13Z- handleDepth()-Depth_Keller msg: 1613173993366 epoch sec; seqNo:7746; depth 20.80 2021-02-12T23:53:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173993.38 epoch sec; seqNo:7746; 4058088.55 north; 591675.45 east 2021-02-12T23:53:13Z- AHRS_M2 msg: 1613173993.75 epoch sec; seqNo:7685 2021-02-12T23:53:13Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.22 psi 2021-02-12T23:53:13Z- handleDepth()-Depth_Keller msg: 1613173993778 epoch sec; seqNo:7747; depth 20.94 2021-02-12T23:53:13Z- LcmTrn::handleMessages() - handling 45 messages took 0.81 ms and 129 calls... 2021-02-12T23:53:13Z- waiting for 1613173991.53, time is 1613173993.75 2021-02-12T23:53:13Z- LcmTrn::updateTrn() >>>> pose time 1613173993.75, meas time 1613173992.65 <<<< 2021-02-12T23:53:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173992.65 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:53:13Z- TerrainNav::motionUpdate() - currEstimate = (4058089, 591675.45, 20.94) 2021-02-12T23:53:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:53:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:53:13Z- TNavBF::Projecting Measurements 2021-02-12T23:53:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:53:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:53:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:53:13Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:53:13Z- TNavFilter::3 good beams. 2021-02-12T23:53:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:53:13Z- TNavBF:: beamIndex[0] = 1 2021-02-12T23:53:13Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:53:13Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:53:13Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:53:13Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:53:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:53:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:53:13Z- TerrainMapOctree: (vehN, vehE ) = (4058087.83, 591674.94). 2021-02-12T23:53:13Z- TerrainMapOctree: Min Distance = 4078.02. 2021-02-12T23:53:13Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:53:13Z- There is no specific good beam that all particles have in common. 2021-02-12T23:53:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:53:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-12T23:53:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:53:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-12T23:53:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:53:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 18.54 2021-02-12T23:53:13Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:53:13Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:53:13Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:53:13Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:53:13Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:53:13Z- Filter 0 No beams used in subcloud update 2021-02-12T23:53:13Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:53:13Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:53:13Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 730.7 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:53:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173992.65 sec. 2021-02-12T23:53:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:53:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:53:13Z- variances([0], [2], and psi_berg) = 730.648265 833.017040 0.000000 2021-02-12T23:53:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:53:13Z- LcmTrn::publishEstimates() - MLE : 4058081.05 591712.95 20.94 2021-02-12T23:53:13Z- LcmTrn::publishEstimates() - MMSE : 4058088.55 591675.45 20.94 2021-02-12T23:53:13Z- LcmTrn::publishEstimates() - COVAR: 730.65 833.02 1.05 0.00 2021-02-12T23:53:13Z- handleDvl() - RDI_Pathfinder msg: 1613173993.79 epoch sec; seqNo:3609 2021-02-12T23:53:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 97.51 , 67.02 , 108.33 2021-02-12T23:53:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , 0.12 2021-02-12T23:53:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173993.80 epoch sec; seqNo:7747; 4058088.55 north; 591675.45 east 2021-02-12T23:53:14Z- AHRS_M2 msg: 1613173994.16 epoch sec; seqNo:7686 2021-02-12T23:53:14Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.22 psi 2021-02-12T23:53:14Z- handleDepth()-Depth_Keller msg: 1613173994161 epoch sec; seqNo:7748; depth 21.10 2021-02-12T23:53:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173994.17 epoch sec; seqNo:7748; 4058088.93 north; 591675.44 east 2021-02-12T23:53:14Z- AHRS_M2 msg: 1613173994.56 epoch sec; seqNo:7687 2021-02-12T23:53:14Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.22 psi 2021-02-12T23:53:14Z- handleDepth()-Depth_Keller msg: 1613173994560 epoch sec; seqNo:7749; depth 21.23 2021-02-12T23:53:14Z- handleDvl() - RDI_Pathfinder msg: 1613173994.58 epoch sec; seqNo:3610 2021-02-12T23:53:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 97.92 , 67.43 , 112.52 2021-02-12T23:53:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , 0.11 2021-02-12T23:53:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173994.59 epoch sec; seqNo:7749; 4058088.93 north; 591675.44 east 2021-02-12T23:53:14Z- AHRS_M2 msg: 1613173994.96 epoch sec; seqNo:7688 2021-02-12T23:53:14Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.22 psi 2021-02-12T23:53:14Z- handleDepth()-Depth_Keller msg: 1613173994969 epoch sec; seqNo:7750; depth 21.36 2021-02-12T23:53:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:14Z- handleDvl() - RDI_Pathfinder msg: 1613173994.97 epoch sec; seqNo:3611 2021-02-12T23:53:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , 0.11 2021-02-12T23:53:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173995.02 epoch sec; seqNo:7750; 4058089.31 north; 591675.44 east 2021-02-12T23:53:15Z- AHRS_M2 msg: 1613173995.38 epoch sec; seqNo:7689 2021-02-12T23:53:15Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.22 psi 2021-02-12T23:53:15Z- handleDepth()-Depth_Keller msg: 1613173995392 epoch sec; seqNo:7751; depth 21.51 2021-02-12T23:53:15Z- handleDvl() - RDI_Pathfinder msg: 1613173995.41 epoch sec; seqNo:3612 2021-02-12T23:53:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 97.85 , 66.20 , 107.58 2021-02-12T23:53:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , 0.03 2021-02-12T23:53:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173995.42 epoch sec; seqNo:7751; 4058089.70 north; 591676.66 east 2021-02-12T23:53:15Z- AHRS_M2 msg: 1613173995.77 epoch sec; seqNo:7690 2021-02-12T23:53:15Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.22 psi 2021-02-12T23:53:15Z- handleDepth()-Depth_Keller msg: 1613173995774 epoch sec; seqNo:7752; depth 21.66 2021-02-12T23:53:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173995.79 epoch sec; seqNo:7752; 4058089.70 north; 591676.66 east 2021-02-12T23:53:16Z- AHRS_M2 msg: 1613173996.17 epoch sec; seqNo:7691 2021-02-12T23:53:16Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.22 psi 2021-02-12T23:53:16Z- handleDepth()-Depth_Keller msg: 1613173996190 epoch sec; seqNo:7753; depth 21.79 2021-02-12T23:53:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:16Z- handleDvl() - RDI_Pathfinder msg: 1613173996.19 epoch sec; seqNo:3613 2021-02-12T23:53:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , 0.03 2021-02-12T23:53:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173996.20 epoch sec; seqNo:7753; 4058090.08 north; 591676.65 east 2021-02-12T23:53:16Z- AHRS_M2 msg: 1613173996.58 epoch sec; seqNo:7692 2021-02-12T23:53:16Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.21 psi 2021-02-12T23:53:16Z- handleDepth()-Depth_Keller msg: 1613173996580 epoch sec; seqNo:7754; depth 21.93 2021-02-12T23:53:16Z- handleDvl() - RDI_Pathfinder msg: 1613173996.59 epoch sec; seqNo:3614 2021-02-12T23:53:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 98.12 , 78.65 , 107.85 2021-02-12T23:53:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.03 , -0.06 2021-02-12T23:53:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173996.62 epoch sec; seqNo:7754; 4058090.46 north; 591676.65 east 2021-02-12T23:53:16Z- AHRS_M2 msg: 1613173996.98 epoch sec; seqNo:7693 2021-02-12T23:53:16Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.21 psi 2021-02-12T23:53:16Z- handleDepth()-Depth_Keller msg: 1613173996984 epoch sec; seqNo:7755; depth 22.08 2021-02-12T23:53:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173996.99 epoch sec; seqNo:7755; 4058090.84 north; 591676.65 east 2021-02-12T23:53:17Z- AHRS_M2 msg: 1613173997.40 epoch sec; seqNo:7694 2021-02-12T23:53:17Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.21 psi 2021-02-12T23:53:17Z- handleDepth()-Depth_Keller msg: 1613173997400 epoch sec; seqNo:7756; depth 22.20 2021-02-12T23:53:17Z- handleDvl() - RDI_Pathfinder msg: 1613173997.41 epoch sec; seqNo:3615 2021-02-12T23:53:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.38 , 95.28 , 66.21 , 106.23 2021-02-12T23:53:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , -0.09 2021-02-12T23:53:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173997.42 epoch sec; seqNo:7756; 4058090.84 north; 591676.65 east 2021-02-12T23:53:17Z- AHRS_M2 msg: 1613173997.79 epoch sec; seqNo:7695 2021-02-12T23:53:17Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.21 psi 2021-02-12T23:53:17Z- handleDepth()-Depth_Keller msg: 1613173997792 epoch sec; seqNo:7757; depth 22.34 2021-02-12T23:53:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173997.81 epoch sec; seqNo:7757; 4058091.23 north; 591677.86 east 2021-02-12T23:53:18Z- AHRS_M2 msg: 1613173998.19 epoch sec; seqNo:7696 2021-02-12T23:53:18Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.21 psi 2021-02-12T23:53:18Z- handleDepth()-Depth_Keller msg: 1613173998201 epoch sec; seqNo:7758; depth 22.49 2021-02-12T23:53:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173998.21 epoch sec; seqNo:7758; 4058091.61 north; 591677.86 east 2021-02-12T23:53:18Z- AHRS_M2 msg: 1613173998.60 epoch sec; seqNo:7697 2021-02-12T23:53:18Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.21 psi 2021-02-12T23:53:18Z- handleDepth()-Depth_Keller msg: 1613173998600 epoch sec; seqNo:7759; depth 22.62 2021-02-12T23:53:18Z- handleDvl() - RDI_Pathfinder msg: 1613173998.61 epoch sec; seqNo:3616 2021-02-12T23:53:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.97 , 94.46 , 66.62 , 107.85 2021-02-12T23:53:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , -0.08 2021-02-12T23:53:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173998.62 epoch sec; seqNo:7759; 4058091.99 north; 591677.85 east 2021-02-12T23:53:18Z- LcmTrn::handleMessages() - handling 45 messages took 0.68 ms and 132 calls... 2021-02-12T23:53:18Z- waiting for 1613173996.75, time is 1613173998.62 2021-02-12T23:53:18Z- LcmTrn::updateTrn() equating pose 1613173998.62 and meas times 1613173998.62 2021-02-12T23:53:18Z- LcmTrn::updateTrn() >>>> pose time 1613173998.62, meas time 1613173998.62 <<<< 2021-02-12T23:53:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613173998.62 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:53:18Z- TerrainNav::motionUpdate() - currEstimate = (4058092, 591677.85, 22.62) 2021-02-12T23:53:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:53:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:53:18Z- TNavBF::Projecting Measurements 2021-02-12T23:53:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:53:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:53:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:53:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:53:18Z- TNavFilter::4 good beams. 2021-02-12T23:53:18Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:53:18Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:53:18Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:53:18Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:53:18Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:53:18Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:53:18Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:53:18Z- TNavBF::mmse selected from filter -1 2021-02-12T23:53:18Z- TerrainMapOctree: (vehN, vehE ) = (4058091.99, 591677.85). 2021-02-12T23:53:18Z- TerrainMapOctree: Min Distance = 4072.95. 2021-02-12T23:53:18Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:53:18Z- There is no specific good beam that all particles have in common. 2021-02-12T23:53:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:53:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 8.76 2021-02-12T23:53:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:53:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 14.28 2021-02-12T23:53:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:53:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 7.10 2021-02-12T23:53:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:53:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 18.61 2021-02-12T23:53:18Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:53:18Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:53:18Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:53:18Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:53:18Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:53:18Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:53:18Z- Filter 0 No beams used in subcloud update 2021-02-12T23:53:18Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:53:18Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:53:18Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 730.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:53:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613173998.62 sec. 2021-02-12T23:53:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:53:18Z- TNavBF::mmse selected from filter -1 2021-02-12T23:53:18Z- variances([0], [2], and psi_berg) = 730.570083 833.016983 0.000000 2021-02-12T23:53:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:53:18Z- LcmTrn::publishEstimates() - MLE : 4058084.49 591715.35 22.62 2021-02-12T23:53:18Z- LcmTrn::publishEstimates() - MMSE : 4058091.99 591677.85 22.62 2021-02-12T23:53:18Z- LcmTrn::publishEstimates() - COVAR: 730.57 833.02 0.90 0.00 2021-02-12T23:53:19Z- AHRS_M2 msg: 1613173999.00 epoch sec; seqNo:7698 2021-02-12T23:53:19Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.20 psi 2021-02-12T23:53:19Z- handleDepth()-Depth_Keller msg: 1613173999010 epoch sec; seqNo:7760; depth 22.77 2021-02-12T23:53:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173999.03 epoch sec; seqNo:7760; 4058091.99 north; 591677.85 east 2021-02-12T23:53:19Z- AHRS_M2 msg: 1613173999.41 epoch sec; seqNo:7699 2021-02-12T23:53:19Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.20 psi 2021-02-12T23:53:19Z- handleDepth()-Depth_Keller msg: 1613173999413 epoch sec; seqNo:7761; depth 22.90 2021-02-12T23:53:19Z- handleDvl() - RDI_Pathfinder msg: 1613173999.42 epoch sec; seqNo:3617 2021-02-12T23:53:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 98.12 , 78.65 , 106.64 2021-02-12T23:53:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , -0.07 2021-02-12T23:53:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173999.45 epoch sec; seqNo:7761; 4058092.37 north; 591677.85 east 2021-02-12T23:53:19Z- AHRS_M2 msg: 1613173999.81 epoch sec; seqNo:7700 2021-02-12T23:53:19Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.19 psi 2021-02-12T23:53:19Z- handleDepth()-Depth_Keller msg: 1613173999813 epoch sec; seqNo:7762; depth 23.04 2021-02-12T23:53:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:19Z- handleDvl() - RDI_Pathfinder msg: 1613173999.82 epoch sec; seqNo:3618 2021-02-12T23:53:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , -0.07 2021-02-12T23:53:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613173999.83 epoch sec; seqNo:7762; 4058092.76 north; 591679.07 east 2021-02-12T23:53:20Z- AHRS_M2 msg: 1613174000.21 epoch sec; seqNo:7701 2021-02-12T23:53:20Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.19 psi 2021-02-12T23:53:20Z- handleDepth()-Depth_Keller msg: 1613174000221 epoch sec; seqNo:7763; depth 23.18 2021-02-12T23:53:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174000.24 epoch sec; seqNo:7763; 4058093.14 north; 591679.06 east 2021-02-12T23:53:20Z- AHRS_M2 msg: 1613174000.62 epoch sec; seqNo:7702 2021-02-12T23:53:20Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.18 psi 2021-02-12T23:53:20Z- handleDepth()-Depth_Keller msg: 1613174000629 epoch sec; seqNo:7764; depth 23.32 2021-02-12T23:53:20Z- handleDvl() - RDI_Pathfinder msg: 1613174000.64 epoch sec; seqNo:3619 2021-02-12T23:53:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.74 , 97.85 , 65.79 , 106.37 2021-02-12T23:53:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , -0.08 2021-02-12T23:53:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174000.65 epoch sec; seqNo:7764; 4058093.14 north; 591679.06 east 2021-02-12T23:53:21Z- AHRS_M2 msg: 1613174001.02 epoch sec; seqNo:7703 2021-02-12T23:53:21Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.18 psi 2021-02-12T23:53:21Z- handleDepth()-Depth_Keller msg: 1613174001026 epoch sec; seqNo:7765; depth 23.48 2021-02-12T23:53:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:21Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:21Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:21Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:21Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:21Z- handleDvl() - RDI_Pathfinder msg: 1613174001.03 epoch sec; seqNo:3620 2021-02-12T23:53:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , -0.08 2021-02-12T23:53:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174001.04 epoch sec; seqNo:7765; 4058093.52 north; 591679.06 east 2021-02-12T23:53:21Z- AHRS_M2 msg: 1613174001.42 epoch sec; seqNo:7704 2021-02-12T23:53:21Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.18 psi 2021-02-12T23:53:21Z- handleDepth()-Depth_Keller msg: 1613174001429 epoch sec; seqNo:7766; depth 23.62 2021-02-12T23:53:21Z- handleDvl() - RDI_Pathfinder msg: 1613174001.45 epoch sec; seqNo:3621 2021-02-12T23:53:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 90.82 , 66.61 , 104.20 2021-02-12T23:53:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , -0.08 2021-02-12T23:53:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174001.46 epoch sec; seqNo:7766; 4058093.90 north; 591679.05 east 2021-02-12T23:53:21Z- AHRS_M2 msg: 1613174001.83 epoch sec; seqNo:7705 2021-02-12T23:53:21Z- AHRS_M2 msg: 0.68 phi; -0.38 theta; -0.19 psi 2021-02-12T23:53:21Z- handleDepth()-Depth_Keller msg: 1613174001833 epoch sec; seqNo:7767; depth 23.78 2021-02-12T23:53:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174001.84 epoch sec; seqNo:7767; 4058094.28 north; 591679.05 east 2021-02-12T23:53:22Z- AHRS_M2 msg: 1613174002.23 epoch sec; seqNo:7706 2021-02-12T23:53:22Z- AHRS_M2 msg: 0.68 phi; -0.38 theta; -0.19 psi 2021-02-12T23:53:22Z- handleDepth()-Depth_Keller msg: 1613174002245 epoch sec; seqNo:7768; depth 23.91 2021-02-12T23:53:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:22Z- handleDvl() - RDI_Pathfinder msg: 1613174002.25 epoch sec; seqNo:3622 2021-02-12T23:53:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , -0.08 2021-02-12T23:53:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174002.26 epoch sec; seqNo:7768; 4058094.29 north; 591680.27 east 2021-02-12T23:53:22Z- AHRS_M2 msg: 1613174002.64 epoch sec; seqNo:7707 2021-02-12T23:53:22Z- AHRS_M2 msg: 0.67 phi; -0.38 theta; -0.20 psi 2021-02-12T23:53:22Z- handleDepth()-Depth_Keller msg: 1613174002648 epoch sec; seqNo:7769; depth 24.07 2021-02-12T23:53:22Z- handleDvl() - RDI_Pathfinder msg: 1613174002.66 epoch sec; seqNo:3623 2021-02-12T23:53:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.74 , 98.32 , 65.79 , 103.19 2021-02-12T23:53:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , -0.05 2021-02-12T23:53:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174002.67 epoch sec; seqNo:7769; 4058094.67 north; 591680.27 east 2021-02-12T23:53:23Z- AHRS_M2 msg: 1613174003.04 epoch sec; seqNo:7708 2021-02-12T23:53:23Z- AHRS_M2 msg: 0.67 phi; -0.38 theta; -0.21 psi 2021-02-12T23:53:23Z- handleDepth()-Depth_Keller msg: 1613174003045 epoch sec; seqNo:7770; depth 24.22 2021-02-12T23:53:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174003.06 epoch sec; seqNo:7770; 4058095.05 north; 591680.26 east 2021-02-12T23:53:23Z- AHRS_M2 msg: 1613174003.44 epoch sec; seqNo:7709 2021-02-12T23:53:23Z- AHRS_M2 msg: 0.66 phi; -0.38 theta; -0.21 psi 2021-02-12T23:53:23Z- handleDepth()-Depth_Keller msg: 1613174003453 epoch sec; seqNo:7771; depth 24.35 2021-02-12T23:53:23Z- handleDvl() - RDI_Pathfinder msg: 1613174003.46 epoch sec; seqNo:3624 2021-02-12T23:53:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 92.10 , 64.98 , 104.27 2021-02-12T23:53:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , -0.02 2021-02-12T23:53:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174003.47 epoch sec; seqNo:7771; 4058095.43 north; 591680.26 east 2021-02-12T23:53:23Z- AHRS_M2 msg: 1613174003.85 epoch sec; seqNo:7710 2021-02-12T23:53:23Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.22 psi 2021-02-12T23:53:23Z- handleDepth()-Depth_Keller msg: 1613174003854 epoch sec; seqNo:7772; depth 24.51 2021-02-12T23:53:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174003.87 epoch sec; seqNo:7772; 4058095.43 north; 591680.26 east 2021-02-12T23:53:23Z- LcmTrn::handleMessages() - handling 47 messages took 0.66 ms and 134 calls... 2021-02-12T23:53:23Z- waiting for 1613174001.62, time is 1613174003.87 2021-02-12T23:53:23Z- LcmTrn::updateTrn() >>>> pose time 1613174003.87, meas time 1613174003.46 <<<< 2021-02-12T23:53:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174003.46 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:53:23Z- TerrainNav::motionUpdate() - currEstimate = (4058095, 591680.26, 24.51) 2021-02-12T23:53:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:53:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:53:23Z- TNavBF::Projecting Measurements 2021-02-12T23:53:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:53:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:53:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:53:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:53:23Z- TNavFilter::4 good beams. 2021-02-12T23:53:23Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:53:23Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:53:23Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:53:23Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:53:23Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:53:23Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:53:23Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:53:23Z- TNavBF::mmse selected from filter -1 2021-02-12T23:53:23Z- TerrainMapOctree: (vehN, vehE ) = (4058095.16, 591680.08). 2021-02-12T23:53:23Z- TerrainMapOctree: Min Distance = 4069.09. 2021-02-12T23:53:23Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:53:23Z- There is no specific good beam that all particles have in common. 2021-02-12T23:53:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:53:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-12T23:53:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:53:23Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 13.57 2021-02-12T23:53:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:53:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 6.76 2021-02-12T23:53:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:53:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 17.39 2021-02-12T23:53:24Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:53:24Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:53:24Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:53:24Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:53:24Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:53:24Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:53:24Z- Filter 0 No beams used in subcloud update 2021-02-12T23:53:24Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:53:24Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:53:24Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 730.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:53:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174003.46 sec. 2021-02-12T23:53:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:53:24Z- TNavBF::mmse selected from filter -1 2021-02-12T23:53:24Z- variances([0], [2], and psi_berg) = 730.473421 833.016912 0.000000 2021-02-12T23:53:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:53:24Z- LcmTrn::publishEstimates() - MLE : 4058087.93 591717.76 24.51 2021-02-12T23:53:24Z- LcmTrn::publishEstimates() - MMSE : 4058095.43 591680.26 24.51 2021-02-12T23:53:24Z- LcmTrn::publishEstimates() - COVAR: 730.47 833.02 0.82 0.00 2021-02-12T23:53:24Z- AHRS_M2 msg: 1613174004.25 epoch sec; seqNo:7711 2021-02-12T23:53:24Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.22 psi 2021-02-12T23:53:24Z- handleDepth()-Depth_Keller msg: 1613174004257 epoch sec; seqNo:7773; depth 24.65 2021-02-12T23:53:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174004.28 epoch sec; seqNo:7773; 4058095.80 north; 591680.26 east 2021-02-12T23:53:24Z- AHRS_M2 msg: 1613174004.66 epoch sec; seqNo:7712 2021-02-12T23:53:24Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.22 psi 2021-02-12T23:53:24Z- handleDepth()-Depth_Keller msg: 1613174004667 epoch sec; seqNo:7774; depth 24.79 2021-02-12T23:53:24Z- handleDvl() - RDI_Pathfinder msg: 1613174004.68 epoch sec; seqNo:3625 2021-02-12T23:53:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.93 , 98.32 , 63.76 , 105.62 2021-02-12T23:53:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , 0.03 2021-02-12T23:53:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174004.70 epoch sec; seqNo:7774; 4058096.18 north; 591680.25 east 2021-02-12T23:53:25Z- AHRS_M2 msg: 1613174005.06 epoch sec; seqNo:7713 2021-02-12T23:53:25Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.23 psi 2021-02-12T23:53:25Z- handleDepth()-Depth_Keller msg: 1613174005136 epoch sec; seqNo:7775; depth 24.93 2021-02-12T23:53:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174005.17 epoch sec; seqNo:7775; 4058096.58 north; 591681.47 east 2021-02-12T23:53:25Z- AHRS_M2 msg: 1613174005.46 epoch sec; seqNo:7714 2021-02-12T23:53:25Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.23 psi 2021-02-12T23:53:25Z- handleDepth()-Depth_Keller msg: 1613174005477 epoch sec; seqNo:7776; depth 25.11 2021-02-12T23:53:25Z- handleDvl() - RDI_Pathfinder msg: 1613174005.49 epoch sec; seqNo:3626 2021-02-12T23:53:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 93.32 , 65.80 , 105.49 2021-02-12T23:53:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.00 , 0.05 2021-02-12T23:53:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174005.50 epoch sec; seqNo:7776; 4058096.58 north; 591681.47 east 2021-02-12T23:53:25Z- AHRS_M2 msg: 1613174005.87 epoch sec; seqNo:7715 2021-02-12T23:53:25Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.23 psi 2021-02-12T23:53:25Z- handleDepth()-Depth_Keller msg: 1613174005893 epoch sec; seqNo:7777; depth 25.24 2021-02-12T23:53:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:25Z- handleDvl() - RDI_Pathfinder msg: 1613174005.89 epoch sec; seqNo:3627 2021-02-12T23:53:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.00 , 0.05 2021-02-12T23:53:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174005.91 epoch sec; seqNo:7777; 4058096.95 north; 591681.46 east 2021-02-12T23:53:26Z- AHRS_M2 msg: 1613174006.28 epoch sec; seqNo:7716 2021-02-12T23:53:26Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.23 psi 2021-02-12T23:53:26Z- handleDepth()-Depth_Keller msg: 1613174006393 epoch sec; seqNo:7778; depth 25.38 2021-02-12T23:53:26Z- handleDvl() - RDI_Pathfinder msg: 1613174006.41 epoch sec; seqNo:3628 2021-02-12T23:53:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 98.32 , 64.98 , 105.62 2021-02-12T23:53:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.05 , 0.05 2021-02-12T23:53:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174006.42 epoch sec; seqNo:7778; 4058096.95 north; 591681.46 east 2021-02-12T23:53:26Z- AHRS_M2 msg: 1613174006.68 epoch sec; seqNo:7717 2021-02-12T23:53:26Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.23 psi 2021-02-12T23:53:26Z- handleDepth()-Depth_Keller msg: 1613174006690 epoch sec; seqNo:7779; depth 25.57 2021-02-12T23:53:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174006.70 epoch sec; seqNo:7779; 4058097.33 north; 591681.46 east 2021-02-12T23:53:27Z- AHRS_M2 msg: 1613174007.08 epoch sec; seqNo:7718 2021-02-12T23:53:27Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.23 psi 2021-02-12T23:53:27Z- handleDepth()-Depth_Keller msg: 1613174007105 epoch sec; seqNo:7780; depth 25.65 2021-02-12T23:53:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:27Z- handleDvl() - RDI_Pathfinder msg: 1613174007.11 epoch sec; seqNo:3629 2021-02-12T23:53:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.05 , 0.05 2021-02-12T23:53:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174007.12 epoch sec; seqNo:7780; 4058097.71 north; 591681.46 east 2021-02-12T23:53:27Z- AHRS_M2 msg: 1613174007.48 epoch sec; seqNo:7719 2021-02-12T23:53:27Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.23 psi 2021-02-12T23:53:27Z- handleDepth()-Depth_Keller msg: 1613174007509 epoch sec; seqNo:7781; depth 25.80 2021-02-12T23:53:27Z- handleDvl() - RDI_Pathfinder msg: 1613174007.52 epoch sec; seqNo:3630 2021-02-12T23:53:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 98.18 , 65.39 , 104.27 2021-02-12T23:53:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , 0.02 2021-02-12T23:53:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174007.53 epoch sec; seqNo:7781; 4058097.71 north; 591681.46 east 2021-02-12T23:53:27Z- AHRS_M2 msg: 1613174007.89 epoch sec; seqNo:7720 2021-02-12T23:53:27Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.23 psi 2021-02-12T23:53:27Z- handleDepth()-Depth_Keller msg: 1613174007892 epoch sec; seqNo:7782; depth 25.95 2021-02-12T23:53:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174007.90 epoch sec; seqNo:7782; 4058098.10 north; 591682.67 east 2021-02-12T23:53:28Z- AHRS_M2 msg: 1613174008.30 epoch sec; seqNo:7721 2021-02-12T23:53:28Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.22 psi 2021-02-12T23:53:28Z- handleDepth()-Depth_Keller msg: 1613174008316 epoch sec; seqNo:7783; depth 26.08 2021-02-12T23:53:28Z- handleDvl() - RDI_Pathfinder msg: 1613174008.35 epoch sec; seqNo:3631 2021-02-12T23:53:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 90.95 , 63.76 , 103.12 2021-02-12T23:53:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , -0.01 2021-02-12T23:53:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174008.36 epoch sec; seqNo:7783; 4058098.48 north; 591682.67 east 2021-02-12T23:53:28Z- AHRS_M2 msg: 1613174008.70 epoch sec; seqNo:7722 2021-02-12T23:53:28Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.22 psi 2021-02-12T23:53:28Z- handleDepth()-Depth_Keller msg: 1613174008702 epoch sec; seqNo:7784; depth 26.26 2021-02-12T23:53:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174008.71 epoch sec; seqNo:7784; 4058098.48 north; 591682.67 east 2021-02-12T23:53:29Z- LcmTrn::handleMessages() - handling 43 messages took 0.66 ms and 130 calls... 2021-02-12T23:53:29Z- waiting for 1613174006.87, time is 1613174008.71 2021-02-12T23:53:29Z- LcmTrn::updateTrn() >>>> pose time 1613174008.71, meas time 1613174008.35 <<<< 2021-02-12T23:53:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174008.35 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:53:29Z- TerrainNav::motionUpdate() - currEstimate = (4058098, 591682.67, 26.26) 2021-02-12T23:53:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:53:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:53:29Z- TNavBF::Projecting Measurements 2021-02-12T23:53:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:53:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:53:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:53:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:53:29Z- TNavFilter::4 good beams. 2021-02-12T23:53:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:53:29Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:53:29Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:53:29Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:53:29Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:53:29Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:53:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:53:29Z- TNavBF::mmse selected from filter -1 2021-02-12T23:53:29Z- TerrainMapOctree: (vehN, vehE ) = (4058098.25, 591682.49). 2021-02-12T23:53:29Z- TerrainMapOctree: Min Distance = 4065.17. 2021-02-12T23:53:29Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:53:29Z- There is no specific good beam that all particles have in common. 2021-02-12T23:53:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:53:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 7.91 2021-02-12T23:53:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:53:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 13.23 2021-02-12T23:53:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:53:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 6.50 2021-02-12T23:53:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:53:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.77 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 17.01 2021-02-12T23:53:29Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:53:29Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:53:29Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:53:29Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:53:29Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:53:29Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:53:29Z- Filter 0 No beams used in subcloud update 2021-02-12T23:53:29Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:53:29Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:53:29Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 730.4 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:53:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174008.35 sec. 2021-02-12T23:53:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:53:29Z- TNavBF::mmse selected from filter -1 2021-02-12T23:53:29Z- variances([0], [2], and psi_berg) = 730.380519 833.016844 0.000000 2021-02-12T23:53:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:53:29Z- LcmTrn::publishEstimates() - MLE : 4058091.98 591633.17 26.26 2021-02-12T23:53:29Z- LcmTrn::publishEstimates() - MMSE : 4058098.48 591682.67 26.26 2021-02-12T23:53:29Z- LcmTrn::publishEstimates() - COVAR: 730.38 833.02 0.76 0.00 2021-02-12T23:53:29Z- AHRS_M2 msg: 1613174009.10 epoch sec; seqNo:7723 2021-02-12T23:53:29Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.22 psi 2021-02-12T23:53:29Z- handleDepth()-Depth_Keller msg: 1613174009104 epoch sec; seqNo:7785; depth 26.37 2021-02-12T23:53:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174009.12 epoch sec; seqNo:7785; 4058098.86 north; 591682.66 east 2021-02-12T23:53:29Z- AHRS_M2 msg: 1613174009.50 epoch sec; seqNo:7724 2021-02-12T23:53:29Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.21 psi 2021-02-12T23:53:29Z- handleDepth()-Depth_Keller msg: 1613174009513 epoch sec; seqNo:7786; depth 26.51 2021-02-12T23:53:29Z- handleDvl() - RDI_Pathfinder msg: 1613174009.54 epoch sec; seqNo:3632 2021-02-12T23:53:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 90.21 , 64.16 , 102.11 2021-02-12T23:53:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.00 , -0.04 2021-02-12T23:53:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174009.55 epoch sec; seqNo:7786; 4058099.24 north; 591682.66 east 2021-02-12T23:53:29Z- AHRS_M2 msg: 1613174009.91 epoch sec; seqNo:7725 2021-02-12T23:53:29Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.21 psi 2021-02-12T23:53:29Z- handleDepth()-Depth_Keller msg: 1613174009912 epoch sec; seqNo:7787; depth 26.67 2021-02-12T23:53:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174009.93 epoch sec; seqNo:7787; 4058099.62 north; 591682.66 east 2021-02-12T23:53:30Z- AHRS_M2 msg: 1613174010.32 epoch sec; seqNo:7726 2021-02-12T23:53:30Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.20 psi 2021-02-12T23:53:30Z- handleDepth()-Depth_Keller msg: 1613174010326 epoch sec; seqNo:7788; depth 26.80 2021-02-12T23:53:30Z- handleDvl() - RDI_Pathfinder msg: 1613174010.34 epoch sec; seqNo:3633 2021-02-12T23:53:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 0.00 , 64.16 , 101.09 2021-02-12T23:53:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.02 , -0.06 2021-02-12T23:53:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174010.35 epoch sec; seqNo:7788; 4058100.01 north; 591683.87 east 2021-02-12T23:53:30Z- AHRS_M2 msg: 1613174010.73 epoch sec; seqNo:7727 2021-02-12T23:53:30Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.20 psi 2021-02-12T23:53:30Z- handleDepth()-Depth_Keller msg: 1613174010730 epoch sec; seqNo:7789; depth 26.95 2021-02-12T23:53:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:30Z- handleDvl() - RDI_Pathfinder msg: 1613174010.73 epoch sec; seqNo:3634 2021-02-12T23:53:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.02 , -0.06 2021-02-12T23:53:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174010.74 epoch sec; seqNo:7789; 4058100.01 north; 591683.87 east 2021-02-12T23:53:31Z- AHRS_M2 msg: 1613174011.12 epoch sec; seqNo:7728 2021-02-12T23:53:31Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.19 psi 2021-02-12T23:53:31Z- handleDepth()-Depth_Keller msg: 1613174011131 epoch sec; seqNo:7790; depth 27.08 2021-02-12T23:53:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174011.15 epoch sec; seqNo:7790; 4058100.39 north; 591683.87 east 2021-02-12T23:53:31Z- AHRS_M2 msg: 1613174011.52 epoch sec; seqNo:7729 2021-02-12T23:53:31Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.19 psi 2021-02-12T23:53:31Z- handleDepth()-Depth_Keller msg: 1613174011533 epoch sec; seqNo:7791; depth 27.22 2021-02-12T23:53:31Z- handleDvl() - RDI_Pathfinder msg: 1613174011.55 epoch sec; seqNo:3635 2021-02-12T23:53:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 88.25 , 62.12 , 99.06 2021-02-12T23:53:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.06 , -0.09 2021-02-12T23:53:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174011.57 epoch sec; seqNo:7791; 4058100.77 north; 591683.87 east 2021-02-12T23:53:31Z- AHRS_M2 msg: 1613174011.93 epoch sec; seqNo:7730 2021-02-12T23:53:31Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.19 psi 2021-02-12T23:53:31Z- handleDepth()-Depth_Keller msg: 1613174011941 epoch sec; seqNo:7792; depth 27.36 2021-02-12T23:53:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:31Z- handleDvl() - RDI_Pathfinder msg: 1613174011.94 epoch sec; seqNo:3636 2021-02-12T23:53:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.06 , -0.09 2021-02-12T23:53:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174011.96 epoch sec; seqNo:7792; 4058101.15 north; 591683.86 east 2021-02-12T23:53:32Z- AHRS_M2 msg: 1613174012.35 epoch sec; seqNo:7731 2021-02-12T23:53:32Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.19 psi 2021-02-12T23:53:32Z- handleDepth()-Depth_Keller msg: 1613174012358 epoch sec; seqNo:7793; depth 27.50 2021-02-12T23:53:32Z- handleDvl() - RDI_Pathfinder msg: 1613174012.37 epoch sec; seqNo:3637 2021-02-12T23:53:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 87.16 , 61.71 , 97.98 2021-02-12T23:53:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.11 2021-02-12T23:53:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174012.38 epoch sec; seqNo:7793; 4058101.53 north; 591683.86 east 2021-02-12T23:53:32Z- AHRS_M2 msg: 1613174012.74 epoch sec; seqNo:7732 2021-02-12T23:53:32Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.19 psi 2021-02-12T23:53:32Z- handleDepth()-Depth_Keller msg: 1613174012741 epoch sec; seqNo:7794; depth 27.66 2021-02-12T23:53:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174012.75 epoch sec; seqNo:7794; 4058101.92 north; 591685.07 east 2021-02-12T23:53:33Z- AHRS_M2 msg: 1613174013.14 epoch sec; seqNo:7733 2021-02-12T23:53:33Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.18 psi 2021-02-12T23:53:33Z- handleDepth()-Depth_Keller msg: 1613174013153 epoch sec; seqNo:7795; depth 27.79 2021-02-12T23:53:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:33Z- handleDvl() - RDI_Pathfinder msg: 1613174013.15 epoch sec; seqNo:3638 2021-02-12T23:53:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.11 2021-02-12T23:53:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174013.16 epoch sec; seqNo:7795; 4058101.92 north; 591685.07 east 2021-02-12T23:53:33Z- AHRS_M2 msg: 1613174013.54 epoch sec; seqNo:7734 2021-02-12T23:53:33Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.18 psi 2021-02-12T23:53:33Z- handleDepth()-Depth_Keller msg: 1613174013553 epoch sec; seqNo:7796; depth 27.92 2021-02-12T23:53:33Z- handleDvl() - RDI_Pathfinder msg: 1613174013.57 epoch sec; seqNo:3639 2021-02-12T23:53:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.66 , 86.08 , 61.71 , 97.98 2021-02-12T23:53:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , -0.12 2021-02-12T23:53:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174013.58 epoch sec; seqNo:7796; 4058102.30 north; 591685.07 east 2021-02-12T23:53:33Z- AHRS_M2 msg: 1613174013.95 epoch sec; seqNo:7735 2021-02-12T23:53:33Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-12T23:53:33Z- handleDepth()-Depth_Keller msg: 1613174013954 epoch sec; seqNo:7797; depth 28.06 2021-02-12T23:53:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174013.96 epoch sec; seqNo:7797; 4058102.68 north; 591685.07 east 2021-02-12T23:53:34Z- LcmTrn::handleMessages() - handling 47 messages took 0.61 ms and 135 calls... 2021-02-12T23:53:34Z- waiting for 1613174011.71, time is 1613174013.96 2021-02-12T23:53:34Z- LcmTrn::updateTrn() >>>> pose time 1613174013.96, meas time 1613174013.57 <<<< 2021-02-12T23:53:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174013.57 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:53:34Z- TerrainNav::motionUpdate() - currEstimate = (4058103, 591685.07, 28.06) 2021-02-12T23:53:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:53:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:53:34Z- TNavBF::Projecting Measurements 2021-02-12T23:53:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:53:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:53:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:53:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:53:34Z- TNavFilter::4 good beams. 2021-02-12T23:53:34Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:53:34Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:53:34Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:53:34Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:53:34Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:53:34Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:53:34Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:53:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:53:34Z- TerrainMapOctree: (vehN, vehE ) = (4058102.36, 591684.88). 2021-02-12T23:53:34Z- TerrainMapOctree: Min Distance = 4060.48. 2021-02-12T23:53:34Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:53:34Z- There is no specific good beam that all particles have in common. 2021-02-12T23:53:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:53:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 7.54 2021-02-12T23:53:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:53:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 11.86 2021-02-12T23:53:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:53:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 6.09 2021-02-12T23:53:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:53:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.76 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-12T23:53:34Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:53:34Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:53:34Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:53:34Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:53:34Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:53:34Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:53:34Z- Filter 0 No beams used in subcloud update 2021-02-12T23:53:34Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:53:34Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:53:34Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 730.3 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:53:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174013.57 sec. 2021-02-12T23:53:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:53:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:53:34Z- variances([0], [2], and psi_berg) = 730.277070 833.016768 0.000000 2021-02-12T23:53:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:53:34Z- LcmTrn::publishEstimates() - MLE : 4058095.18 591722.57 28.06 2021-02-12T23:53:34Z- LcmTrn::publishEstimates() - MMSE : 4058102.68 591685.07 28.06 2021-02-12T23:53:34Z- LcmTrn::publishEstimates() - COVAR: 730.28 833.02 0.74 0.00 2021-02-12T23:53:34Z- AHRS_M2 msg: 1613174014.36 epoch sec; seqNo:7736 2021-02-12T23:53:34Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-12T23:53:34Z- handleDepth()-Depth_Keller msg: 1613174014370 epoch sec; seqNo:7798; depth 28.20 2021-02-12T23:53:34Z- handleDvl() - RDI_Pathfinder msg: 1613174014.38 epoch sec; seqNo:3640 2021-02-12T23:53:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 86.08 , 61.71 , 97.98 2021-02-12T23:53:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , -0.09 2021-02-12T23:53:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174014.39 epoch sec; seqNo:7798; 4058103.05 north; 591685.06 east 2021-02-12T23:53:34Z- AHRS_M2 msg: 1613174014.76 epoch sec; seqNo:7737 2021-02-12T23:53:34Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.17 psi 2021-02-12T23:53:34Z- handleDepth()-Depth_Keller msg: 1613174014761 epoch sec; seqNo:7799; depth 28.34 2021-02-12T23:53:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174014.77 epoch sec; seqNo:7799; 4058103.43 north; 591685.06 east 2021-02-12T23:53:35Z- AHRS_M2 msg: 1613174015.16 epoch sec; seqNo:7738 2021-02-12T23:53:35Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.17 psi 2021-02-12T23:53:35Z- handleDepth()-Depth_Keller msg: 1613174015165 epoch sec; seqNo:7800; depth 28.48 2021-02-12T23:53:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174015.17 epoch sec; seqNo:7800; 4058103.43 north; 591685.06 east 2021-02-12T23:53:35Z- AHRS_M2 msg: 1613174015.56 epoch sec; seqNo:7739 2021-02-12T23:53:35Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.16 psi 2021-02-12T23:53:35Z- handleDepth()-Depth_Keller msg: 1613174015577 epoch sec; seqNo:7801; depth 28.61 2021-02-12T23:53:35Z- handleDvl() - RDI_Pathfinder msg: 1613174015.59 epoch sec; seqNo:3641 2021-02-12T23:53:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.25 , 83.92 , 61.30 , 97.98 2021-02-12T23:53:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.12 , -0.04 2021-02-12T23:53:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174015.60 epoch sec; seqNo:7801; 4058103.81 north; 591685.05 east 2021-02-12T23:53:35Z- AHRS_M2 msg: 1613174015.97 epoch sec; seqNo:7740 2021-02-12T23:53:35Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.16 psi 2021-02-12T23:53:35Z- handleDepth()-Depth_Keller msg: 1613174015973 epoch sec; seqNo:7802; depth 28.76 2021-02-12T23:53:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174015.99 epoch sec; seqNo:7802; 4058104.20 north; 591686.27 east 2021-02-12T23:53:36Z- AHRS_M2 msg: 1613174016.37 epoch sec; seqNo:7741 2021-02-12T23:53:36Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.16 psi 2021-02-12T23:53:36Z- handleDepth()-Depth_Keller msg: 1613174016377 epoch sec; seqNo:7803; depth 28.89 2021-02-12T23:53:36Z- handleDvl() - RDI_Pathfinder msg: 1613174016.40 epoch sec; seqNo:3642 2021-02-12T23:53:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.25 , 153.15 , 60.90 , 97.98 2021-02-12T23:53:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.14 , -0.04 2021-02-12T23:53:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174016.42 epoch sec; seqNo:7803; 4058104.58 north; 591686.27 east 2021-02-12T23:53:36Z- AHRS_M2 msg: 1613174016.78 epoch sec; seqNo:7742 2021-02-12T23:53:36Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.16 psi 2021-02-12T23:53:36Z- handleDepth()-Depth_Keller msg: 1613174016781 epoch sec; seqNo:7804; depth 29.02 2021-02-12T23:53:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174016.79 epoch sec; seqNo:7804; 4058104.58 north; 591686.27 east 2021-02-12T23:53:37Z- AHRS_M2 msg: 1613174017.18 epoch sec; seqNo:7743 2021-02-12T23:53:37Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.15 psi 2021-02-12T23:53:37Z- handleDepth()-Depth_Keller msg: 1613174017187 epoch sec; seqNo:7805; depth 29.15 2021-02-12T23:53:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174017.19 epoch sec; seqNo:7805; 4058104.96 north; 591686.26 east 2021-02-12T23:53:37Z- AHRS_M2 msg: 1613174017.58 epoch sec; seqNo:7744 2021-02-12T23:53:37Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.15 psi 2021-02-12T23:53:37Z- handleDepth()-Depth_Keller msg: 1613174017589 epoch sec; seqNo:7806; depth 29.28 2021-02-12T23:53:37Z- handleDvl() - RDI_Pathfinder msg: 1613174017.60 epoch sec; seqNo:3643 2021-02-12T23:53:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 0.00 , 61.30 , 97.44 2021-02-12T23:53:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.08 2021-02-12T23:53:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174017.62 epoch sec; seqNo:7806; 4058104.96 north; 591686.26 east 2021-02-12T23:53:37Z- AHRS_M2 msg: 1613174017.99 epoch sec; seqNo:7745 2021-02-12T23:53:37Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.16 psi 2021-02-12T23:53:37Z- handleDepth()-Depth_Keller msg: 1613174017993 epoch sec; seqNo:7807; depth 29.43 2021-02-12T23:53:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:37Z- handleDvl() - RDI_Pathfinder msg: 1613174017.99 epoch sec; seqNo:3644 2021-02-12T23:53:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.08 2021-02-12T23:53:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174018.01 epoch sec; seqNo:7807; 4058105.34 north; 591686.26 east 2021-02-12T23:53:38Z- AHRS_M2 msg: 1613174018.39 epoch sec; seqNo:7746 2021-02-12T23:53:38Z- AHRS_M2 msg: 0.64 phi; -0.28 theta; -0.16 psi 2021-02-12T23:53:38Z- handleDepth()-Depth_Keller msg: 1613174018466 epoch sec; seqNo:7808; depth 29.56 2021-02-12T23:53:38Z- handleDvl() - RDI_Pathfinder msg: 1613174018.48 epoch sec; seqNo:3645 2021-02-12T23:53:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 80.67 , 60.90 , 97.98 2021-02-12T23:53:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , 0.11 2021-02-12T23:53:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174018.50 epoch sec; seqNo:7808; 4058105.73 north; 591687.48 east 2021-02-12T23:53:38Z- AHRS_M2 msg: 1613174018.80 epoch sec; seqNo:7747 2021-02-12T23:53:38Z- AHRS_M2 msg: 0.64 phi; -0.25 theta; -0.15 psi 2021-02-12T23:53:38Z- handleDepth()-Depth_Keller msg: 1613174018810 epoch sec; seqNo:7809; depth 29.74 2021-02-12T23:53:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174018.82 epoch sec; seqNo:7809; 4058105.73 north; 591687.48 east 2021-02-12T23:53:39Z- AHRS_M2 msg: 1613174019.20 epoch sec; seqNo:7748 2021-02-12T23:53:39Z- AHRS_M2 msg: 0.63 phi; -0.21 theta; -0.15 psi 2021-02-12T23:53:39Z- handleDepth()-Depth_Keller msg: 1613174019229 epoch sec; seqNo:7810; depth 29.85 2021-02-12T23:53:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:39Z- handleDvl() - RDI_Pathfinder msg: 1613174019.23 epoch sec; seqNo:3646 2021-02-12T23:53:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , 0.11 2021-02-12T23:53:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174019.24 epoch sec; seqNo:7810; 4058106.11 north; 591687.47 east 2021-02-12T23:53:39Z- LcmTrn::handleMessages() - handling 46 messages took 0.79 ms and 130 calls... 2021-02-12T23:53:39Z- waiting for 1613174016.96, time is 1613174019.24 2021-02-12T23:53:39Z- LcmTrn::updateTrn() equating pose 1613174019.24 and meas times 1613174019.24 2021-02-12T23:53:39Z- LcmTrn::updateTrn() >>>> pose time 1613174019.24, meas time 1613174019.24 <<<< 2021-02-12T23:53:39Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174019.24, ping # 0. 2021-02-12T23:53:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174019.24 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:53:39Z- TerrainNav::motionUpdate() - currEstimate = (4058106, 591687.47, 29.85) 2021-02-12T23:53:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:53:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:53:39Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:53:39Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:53:39Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174019.24 sec.; No relevant navigation data available 2021-02-12T23:53:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:53:39Z- TNavBF::mmse selected from filter -1 2021-02-12T23:53:39Z- variances([0], [2], and psi_berg) = 730.199232 833.016711 0.000000 2021-02-12T23:53:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:53:39Z- LcmTrn::publishEstimates() - MLE : 4058099.61 591637.97 29.85 2021-02-12T23:53:39Z- LcmTrn::publishEstimates() - MMSE : 4058106.11 591687.47 29.85 2021-02-12T23:53:39Z- LcmTrn::publishEstimates() - COVAR: 730.20 833.02 0.96 0.00 2021-02-12T23:53:39Z- AHRS_M2 msg: 1613174019.60 epoch sec; seqNo:7749 2021-02-12T23:53:39Z- AHRS_M2 msg: 0.62 phi; -0.17 theta; -0.14 psi 2021-02-12T23:53:39Z- handleDepth()-Depth_Keller msg: 1613174019629 epoch sec; seqNo:7811; depth 29.98 2021-02-12T23:53:39Z- handleDvl() - RDI_Pathfinder msg: 1613174019.65 epoch sec; seqNo:3647 2021-02-12T23:53:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.80 , 0.00 , 62.53 , 85.00 2021-02-12T23:53:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , 0.09 2021-02-12T23:53:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174019.66 epoch sec; seqNo:7811; 4058106.49 north; 591687.47 east 2021-02-12T23:53:40Z- AHRS_M2 msg: 1613174020.04 epoch sec; seqNo:7750 2021-02-12T23:53:40Z- AHRS_M2 msg: 0.62 phi; -0.12 theta; -0.14 psi 2021-02-12T23:53:40Z- handleDepth()-Depth_Keller msg: 1613174020104 epoch sec; seqNo:7812; depth 30.12 2021-02-12T23:53:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174020.12 epoch sec; seqNo:7812; 4058106.87 north; 591687.46 east 2021-02-12T23:53:40Z- AHRS_M2 msg: 1613174020.41 epoch sec; seqNo:7751 2021-02-12T23:53:40Z- AHRS_M2 msg: 0.61 phi; -0.08 theta; -0.14 psi 2021-02-12T23:53:40Z- handleDepth()-Depth_Keller msg: 1613174020441 epoch sec; seqNo:7813; depth 30.24 2021-02-12T23:53:40Z- handleDvl() - RDI_Pathfinder msg: 1613174020.47 epoch sec; seqNo:3648 2021-02-12T23:53:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.57 , 78.51 , 64.16 , 0.00 2021-02-12T23:53:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.08 , 0.03 2021-02-12T23:53:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174020.48 epoch sec; seqNo:7813; 4058106.88 north; 591688.68 east 2021-02-12T23:53:40Z- AHRS_M2 msg: 1613174020.82 epoch sec; seqNo:7752 2021-02-12T23:53:40Z- AHRS_M2 msg: 0.61 phi; -0.04 theta; -0.14 psi 2021-02-12T23:53:40Z- handleDepth()-Depth_Keller msg: 1613174020842 epoch sec; seqNo:7814; depth 30.32 2021-02-12T23:53:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174020.86 epoch sec; seqNo:7814; 4058107.26 north; 591688.68 east 2021-02-12T23:53:41Z- AHRS_M2 msg: 1613174021.22 epoch sec; seqNo:7753 2021-02-12T23:53:41Z- AHRS_M2 msg: 0.60 phi; 0.01 theta; -0.14 psi 2021-02-12T23:53:41Z- handleDepth()-Depth_Keller msg: 1613174021245 epoch sec; seqNo:7815; depth 30.39 2021-02-12T23:53:41Z- handleDvl() - RDI_Pathfinder msg: 1613174021.27 epoch sec; seqNo:3649 2021-02-12T23:53:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 64.09 , 0.00 , 67.65 , 0.00 2021-02-12T23:53:41Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:53:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174021.28 epoch sec; seqNo:7815; 4058107.64 north; 591688.68 east 2021-02-12T23:53:41Z- AHRS_M2 msg: 1613174021.62 epoch sec; seqNo:7754 2021-02-12T23:53:41Z- AHRS_M2 msg: 0.60 phi; 0.05 theta; -0.14 psi 2021-02-12T23:53:41Z- handleDepth()-Depth_Keller msg: 1613174021630 epoch sec; seqNo:7816; depth 30.45 2021-02-12T23:53:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174021.64 epoch sec; seqNo:7816; 4058108.02 north; 591688.67 east 2021-02-12T23:53:42Z- AHRS_M2 msg: 1613174022.06 epoch sec; seqNo:7755 2021-02-12T23:53:42Z- AHRS_M2 msg: 0.59 phi; 0.08 theta; -0.15 psi 2021-02-12T23:53:42Z- handleDepth()-Depth_Keller msg: 1613174022059 epoch sec; seqNo:7817; depth 30.50 2021-02-12T23:53:42Z- handleDvl() - RDI_Pathfinder msg: 1613174022.07 epoch sec; seqNo:3650 2021-02-12T23:53:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.46 , 0.00 , 71.98 , 65.59 2021-02-12T23:53:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.09 , -0.10 2021-02-12T23:53:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174022.08 epoch sec; seqNo:7817; 4058108.02 north; 591688.67 east 2021-02-12T23:53:42Z- AHRS_M2 msg: 1613174022.43 epoch sec; seqNo:7756 2021-02-12T23:53:42Z- AHRS_M2 msg: 0.58 phi; 0.12 theta; -0.15 psi 2021-02-12T23:53:42Z- handleDepth()-Depth_Keller msg: 1613174022436 epoch sec; seqNo:7818; depth 30.53 2021-02-12T23:53:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174022.45 epoch sec; seqNo:7818; 4058108.40 north; 591688.67 east 2021-02-12T23:53:42Z- AHRS_M2 msg: 1613174022.84 epoch sec; seqNo:7757 2021-02-12T23:53:42Z- AHRS_M2 msg: 0.58 phi; 0.15 theta; -0.15 psi 2021-02-12T23:53:42Z- handleDepth()-Depth_Keller msg: 1613174022885 epoch sec; seqNo:7819; depth 30.53 2021-02-12T23:53:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174022.96 epoch sec; seqNo:7819; 4058108.79 north; 591689.88 east 2021-02-12T23:53:43Z- handleDepth()-Depth_Keller msg: 1613174023273 epoch sec; seqNo:7820; depth 30.55 2021-02-12T23:53:43Z- handleDvl() - RDI_Pathfinder msg: 1613174023.28 epoch sec; seqNo:3651 2021-02-12T23:53:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.40 , 0.00 , 76.36 , 63.78 2021-02-12T23:53:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , -0.13 2021-02-12T23:53:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174023.30 epoch sec; seqNo:7820; 4058109.17 north; 591689.88 east 2021-02-12T23:53:43Z- AHRS_M2 msg: 1613174023.64 epoch sec; seqNo:7758 2021-02-12T23:53:43Z- AHRS_M2 msg: 0.57 phi; 0.21 theta; -0.16 psi 2021-02-12T23:53:43Z- handleDepth()-Depth_Keller msg: 1613174023649 epoch sec; seqNo:7821; depth 30.53 2021-02-12T23:53:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174023.67 epoch sec; seqNo:7821; 4058109.17 north; 591689.88 east 2021-02-12T23:53:44Z- AHRS_M2 msg: 1613174024.05 epoch sec; seqNo:7759 2021-02-12T23:53:44Z- AHRS_M2 msg: 0.56 phi; 0.23 theta; -0.16 psi 2021-02-12T23:53:44Z- handleDepth()-Depth_Keller msg: 1613174024057 epoch sec; seqNo:7822; depth 30.51 2021-02-12T23:53:44Z- handleDvl() - RDI_Pathfinder msg: 1613174024.07 epoch sec; seqNo:3652 2021-02-12T23:53:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 65.06 , 0.00 , 0.00 , 62.15 2021-02-12T23:53:44Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:53:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174024.08 epoch sec; seqNo:7822; 4058109.55 north; 591689.88 east 2021-02-12T23:53:44Z- LcmTrn::handleMessages() - handling 41 messages took 0.77 ms and 126 calls... 2021-02-12T23:53:44Z- waiting for 1613174022.24, time is 1613174024.08 2021-02-12T23:53:44Z- LcmTrn::updateTrn() equating pose 1613174024.08 and meas times 1613174024.08 2021-02-12T23:53:44Z- LcmTrn::updateTrn() >>>> pose time 1613174024.08, meas time 1613174024.08 <<<< 2021-02-12T23:53:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174024.08 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:53:44Z- TerrainNav::motionUpdate() - currEstimate = (4058110, 591689.88, 30.51) 2021-02-12T23:53:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:53:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:53:44Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:53:44Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:53:44Z- TNavBF::Projecting Measurements 2021-02-12T23:53:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:53:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:53:44Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-12T23:53:44Z- TNavFilter::2 good beams. 2021-02-12T23:53:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:53:44Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:53:44Z- TNavBF:: beamIndex[1] = 3 2021-02-12T23:53:44Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:53:44Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:53:44Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-12T23:53:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:53:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:53:44Z- TerrainMapOctree: (vehN, vehE ) = (4058109.55, 591689.88). 2021-02-12T23:53:44Z- TerrainMapOctree: Min Distance = 4051.75. 2021-02-12T23:53:44Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:53:44Z- There is no specific good beam that all particles have in common. 2021-02-12T23:53:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:53:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.121 ExpectedMeasDiff: nan currMeas.covariance: 6.77 2021-02-12T23:53:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:53:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.115 ExpectedMeasDiff: nan currMeas.covariance: 6.18 2021-02-12T23:53:44Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:53:44Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:53:44Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:53:44Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:53:44Z- Filter 0 No beams used in subcloud update 2021-02-12T23:53:44Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:53:44Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:53:44Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 730.1 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:53:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174024.08 sec. 2021-02-12T23:53:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:53:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:53:44Z- variances([0], [2], and psi_berg) = 730.121467 833.016654 0.000000 2021-02-12T23:53:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:53:44Z- LcmTrn::publishEstimates() - MLE : 4058103.05 591640.38 30.51 2021-02-12T23:53:44Z- LcmTrn::publishEstimates() - MMSE : 4058109.55 591689.88 30.51 2021-02-12T23:53:44Z- LcmTrn::publishEstimates() - COVAR: 730.12 833.02 0.91 0.00 2021-02-12T23:53:44Z- AHRS_M2 msg: 1613174024.45 epoch sec; seqNo:7760 2021-02-12T23:53:44Z- AHRS_M2 msg: 0.56 phi; 0.25 theta; -0.16 psi 2021-02-12T23:53:44Z- handleDepth()-Depth_Keller msg: 1613174024456 epoch sec; seqNo:7823; depth 30.49 2021-02-12T23:53:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174024.47 epoch sec; seqNo:7823; 4058109.93 north; 591689.87 east 2021-02-12T23:53:44Z- AHRS_M2 msg: 1613174024.86 epoch sec; seqNo:7761 2021-02-12T23:53:44Z- AHRS_M2 msg: 0.56 phi; 0.27 theta; -0.16 psi 2021-02-12T23:53:44Z- handleDepth()-Depth_Keller msg: 1613174024862 epoch sec; seqNo:7824; depth 30.45 2021-02-12T23:53:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174024.87 epoch sec; seqNo:7824; 4058109.93 north; 591689.87 east 2021-02-12T23:53:45Z- AHRS_M2 msg: 1613174025.26 epoch sec; seqNo:7762 2021-02-12T23:53:45Z- AHRS_M2 msg: 0.56 phi; 0.29 theta; -0.15 psi 2021-02-12T23:53:45Z- handleDepth()-Depth_Keller msg: 1613174025269 epoch sec; seqNo:7825; depth 30.39 2021-02-12T23:53:45Z- handleDvl() - RDI_Pathfinder msg: 1613174025.29 epoch sec; seqNo:3653 2021-02-12T23:53:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 65.59 , 0.00 , 0.00 , 61.76 2021-02-12T23:53:45Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:53:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174025.30 epoch sec; seqNo:7825; 4058110.32 north; 591691.09 east 2021-02-12T23:53:45Z- AHRS_M2 msg: 1613174025.66 epoch sec; seqNo:7763 2021-02-12T23:53:45Z- AHRS_M2 msg: 0.56 phi; 0.30 theta; -0.15 psi 2021-02-12T23:53:45Z- handleDepth()-Depth_Keller msg: 1613174025668 epoch sec; seqNo:7826; depth 30.32 2021-02-12T23:53:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174025.68 epoch sec; seqNo:7826; 4058110.70 north; 591691.08 east 2021-02-12T23:53:46Z- AHRS_M2 msg: 1613174026.07 epoch sec; seqNo:7764 2021-02-12T23:53:46Z- AHRS_M2 msg: 0.56 phi; 0.32 theta; -0.15 psi 2021-02-12T23:53:46Z- handleDepth()-Depth_Keller msg: 1613174026078 epoch sec; seqNo:7827; depth 30.26 2021-02-12T23:53:46Z- handleDvl() - RDI_Pathfinder msg: 1613174026.09 epoch sec; seqNo:3654 2021-02-12T23:53:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 66.87 , 0.00 , 0.00 , 59.63 2021-02-12T23:53:46Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:53:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174026.11 epoch sec; seqNo:7827; 4058110.70 north; 591691.08 east 2021-02-12T23:53:46Z- AHRS_M2 msg: 1613174026.47 epoch sec; seqNo:7765 2021-02-12T23:53:46Z- AHRS_M2 msg: 0.56 phi; 0.33 theta; -0.15 psi 2021-02-12T23:53:46Z- handleDepth()-Depth_Keller msg: 1613174026481 epoch sec; seqNo:7828; depth 30.18 2021-02-12T23:53:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:46Z- handleDvl() - RDI_Pathfinder msg: 1613174026.48 epoch sec; seqNo:3655 2021-02-12T23:53:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:46Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:53:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174026.51 epoch sec; seqNo:7828; 4058111.08 north; 591691.08 east 2021-02-12T23:53:46Z- AHRS_M2 msg: 1613174026.88 epoch sec; seqNo:7766 2021-02-12T23:53:46Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.15 psi 2021-02-12T23:53:46Z- handleDepth()-Depth_Keller msg: 1613174026881 epoch sec; seqNo:7829; depth 30.11 2021-02-12T23:53:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174026.89 epoch sec; seqNo:7829; 4058111.45 north; 591691.08 east 2021-02-12T23:53:47Z- AHRS_M2 msg: 1613174027.29 epoch sec; seqNo:7767 2021-02-12T23:53:47Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.14 psi 2021-02-12T23:53:47Z- handleDepth()-Depth_Keller msg: 1613174027297 epoch sec; seqNo:7830; depth 30.02 2021-02-12T23:53:47Z- handleDvl() - RDI_Pathfinder msg: 1613174027.31 epoch sec; seqNo:3656 2021-02-12T23:53:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 67.72 , 0.00 , 0.00 , 61.33 2021-02-12T23:53:47Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:53:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174027.32 epoch sec; seqNo:7830; 4058111.83 north; 591691.07 east 2021-02-12T23:53:47Z- AHRS_M2 msg: 1613174027.69 epoch sec; seqNo:7768 2021-02-12T23:53:47Z- AHRS_M2 msg: 0.56 phi; 0.37 theta; -0.15 psi 2021-02-12T23:53:47Z- handleDepth()-Depth_Keller msg: 1613174027694 epoch sec; seqNo:7831; depth 29.92 2021-02-12T23:53:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:47Z- handleDvl() - RDI_Pathfinder msg: 1613174027.70 epoch sec; seqNo:3657 2021-02-12T23:53:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:47Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:53:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174027.71 epoch sec; seqNo:7831; 4058111.83 north; 591691.07 east 2021-02-12T23:53:48Z- AHRS_M2 msg: 1613174028.09 epoch sec; seqNo:7769 2021-02-12T23:53:48Z- AHRS_M2 msg: 0.56 phi; 0.37 theta; -0.15 psi 2021-02-12T23:53:48Z- handleDepth()-Depth_Keller msg: 1613174028106 epoch sec; seqNo:7832; depth 29.81 2021-02-12T23:53:48Z- handleDvl() - RDI_Pathfinder msg: 1613174028.12 epoch sec; seqNo:3658 2021-02-12T23:53:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:48Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:53:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174028.13 epoch sec; seqNo:7832; 4058112.23 north; 591692.29 east 2021-02-12T23:53:48Z- AHRS_M2 msg: 1613174028.49 epoch sec; seqNo:7770 2021-02-12T23:53:48Z- AHRS_M2 msg: 0.55 phi; 0.37 theta; -0.15 psi 2021-02-12T23:53:48Z- handleDepth()-Depth_Keller msg: 1613174028497 epoch sec; seqNo:7833; depth 29.70 2021-02-12T23:53:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174028.51 epoch sec; seqNo:7833; 4058112.60 north; 591692.29 east 2021-02-12T23:53:48Z- AHRS_M2 msg: 1613174028.90 epoch sec; seqNo:7771 2021-02-12T23:53:48Z- AHRS_M2 msg: 0.55 phi; 0.38 theta; -0.15 psi 2021-02-12T23:53:48Z- handleDepth()-Depth_Keller msg: 1613174028901 epoch sec; seqNo:7834; depth 29.61 2021-02-12T23:53:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:48Z- handleDvl() - RDI_Pathfinder msg: 1613174028.90 epoch sec; seqNo:3659 2021-02-12T23:53:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:48Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:53:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174028.91 epoch sec; seqNo:7834; 4058112.60 north; 591692.29 east 2021-02-12T23:53:49Z- AHRS_M2 msg: 1613174029.31 epoch sec; seqNo:7772 2021-02-12T23:53:49Z- AHRS_M2 msg: 0.55 phi; 0.38 theta; -0.15 psi 2021-02-12T23:53:49Z- handleDepth()-Depth_Keller msg: 1613174029326 epoch sec; seqNo:7835; depth 29.49 2021-02-12T23:53:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174029.36 epoch sec; seqNo:7835; 4058112.98 north; 591692.28 east 2021-02-12T23:53:49Z- LcmTrn::handleMessages() - handling 46 messages took 0.67 ms and 133 calls... 2021-02-12T23:53:49Z- waiting for 1613174027.08, time is 1613174029.36 2021-02-12T23:53:49Z- LcmTrn::updateTrn() >>>> pose time 1613174029.36, meas time 1613174028.90 <<<< 2021-02-12T23:53:49Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174028.90, ping # 0. 2021-02-12T23:53:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174028.90 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:53:49Z- TerrainNav::motionUpdate() - currEstimate = (4058113, 591692.28, 29.49) 2021-02-12T23:53:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:53:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:53:49Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:53:49Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:53:49Z- TNavBF::Projecting Measurements 2021-02-12T23:53:49Z- TNavFilter::Measurement from time = 1613174028.90 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:53:49Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:53:49Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:53:49Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:53:49Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:53:49Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:53:49Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:53:49Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:53:49Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 730.0 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:53:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:53:49Z- TNavBF::mmse selected from filter -1 2021-02-12T23:53:49Z- variances([0], [2], and psi_berg) = 730.024322 833.016583 0.000000 2021-02-12T23:53:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:53:49Z- LcmTrn::publishEstimates() - MLE : 4058106.48 591642.78 29.49 2021-02-12T23:53:49Z- LcmTrn::publishEstimates() - MMSE : 4058112.98 591692.28 29.49 2021-02-12T23:53:49Z- LcmTrn::publishEstimates() - COVAR: 730.02 833.02 1.12 0.00 2021-02-12T23:53:49Z- AHRS_M2 msg: 1613174029.70 epoch sec; seqNo:7773 2021-02-12T23:53:49Z- AHRS_M2 msg: 0.55 phi; 0.38 theta; -0.15 psi 2021-02-12T23:53:49Z- handleDepth()-Depth_Keller msg: 1613174029708 epoch sec; seqNo:7836; depth 29.37 2021-02-12T23:53:49Z- handleDvl() - RDI_Pathfinder msg: 1613174029.73 epoch sec; seqNo:3660 2021-02-12T23:53:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 82.84 , 99.06 , 60.08 2021-02-12T23:53:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.07 , 0.07 2021-02-12T23:53:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174029.74 epoch sec; seqNo:7836; 4058113.36 north; 591692.28 east 2021-02-12T23:53:50Z- AHRS_M2 msg: 1613174030.11 epoch sec; seqNo:7774 2021-02-12T23:53:50Z- AHRS_M2 msg: 0.55 phi; 0.38 theta; -0.15 psi 2021-02-12T23:53:50Z- handleDepth()-Depth_Keller msg: 1613174030121 epoch sec; seqNo:7837; depth 29.24 2021-02-12T23:53:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:50Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:50Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:50Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:50Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:50Z- handleDvl() - RDI_Pathfinder msg: 1613174030.12 epoch sec; seqNo:3661 2021-02-12T23:53:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.07 , 0.07 2021-02-12T23:53:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174030.15 epoch sec; seqNo:7837; 4058113.36 north; 591692.28 east 2021-02-12T23:53:50Z- AHRS_M2 msg: 1613174030.51 epoch sec; seqNo:7775 2021-02-12T23:53:50Z- AHRS_M2 msg: 0.55 phi; 0.38 theta; -0.15 psi 2021-02-12T23:53:50Z- handleDepth()-Depth_Keller msg: 1613174030517 epoch sec; seqNo:7838; depth 29.14 2021-02-12T23:53:50Z- handleDvl() - RDI_Pathfinder msg: 1613174030.53 epoch sec; seqNo:3662 2021-02-12T23:53:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 120.70 , 96.90 , 60.08 2021-02-12T23:53:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.09 , 0.07 2021-02-12T23:53:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174030.54 epoch sec; seqNo:7838; 4058113.74 north; 591692.27 east 2021-02-12T23:53:50Z- AHRS_M2 msg: 1613174030.92 epoch sec; seqNo:7776 2021-02-12T23:53:50Z- AHRS_M2 msg: 0.55 phi; 0.38 theta; -0.14 psi 2021-02-12T23:53:50Z- handleDepth()-Depth_Keller msg: 1613174030923 epoch sec; seqNo:7839; depth 29.01 2021-02-12T23:53:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174030.93 epoch sec; seqNo:7839; 4058114.13 north; 591693.49 east 2021-02-12T23:53:51Z- AHRS_M2 msg: 1613174031.32 epoch sec; seqNo:7777 2021-02-12T23:53:51Z- AHRS_M2 msg: 0.56 phi; 0.38 theta; -0.14 psi 2021-02-12T23:53:51Z- handleDepth()-Depth_Keller msg: 1613174031325 epoch sec; seqNo:7840; depth 28.90 2021-02-12T23:53:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:51Z- handleDvl() - RDI_Pathfinder msg: 1613174031.33 epoch sec; seqNo:3663 2021-02-12T23:53:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.09 , 0.07 2021-02-12T23:53:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174031.35 epoch sec; seqNo:7840; 4058114.51 north; 591693.49 east 2021-02-12T23:53:51Z- AHRS_M2 msg: 1613174031.73 epoch sec; seqNo:7778 2021-02-12T23:53:51Z- AHRS_M2 msg: 0.56 phi; 0.38 theta; -0.13 psi 2021-02-12T23:53:51Z- handleDepth()-Depth_Keller msg: 1613174031802 epoch sec; seqNo:7841; depth 28.78 2021-02-12T23:53:51Z- handleDvl() - RDI_Pathfinder msg: 1613174031.82 epoch sec; seqNo:3664 2021-02-12T23:53:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 82.23 , 98.05 , 62.12 2021-02-12T23:53:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , 0.09 2021-02-12T23:53:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174031.83 epoch sec; seqNo:7841; 4058114.51 north; 591693.49 east 2021-02-12T23:53:52Z- AHRS_M2 msg: 1613174032.13 epoch sec; seqNo:7779 2021-02-12T23:53:52Z- AHRS_M2 msg: 0.57 phi; 0.38 theta; -0.13 psi 2021-02-12T23:53:52Z- handleDepth()-Depth_Keller msg: 1613174032147 epoch sec; seqNo:7842; depth 28.64 2021-02-12T23:53:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174032.16 epoch sec; seqNo:7842; 4058114.89 north; 591693.48 east 2021-02-12T23:53:52Z- AHRS_M2 msg: 1613174032.53 epoch sec; seqNo:7780 2021-02-12T23:53:52Z- AHRS_M2 msg: 0.58 phi; 0.38 theta; -0.13 psi 2021-02-12T23:53:52Z- handleDepth()-Depth_Keller msg: 1613174032557 epoch sec; seqNo:7843; depth 28.53 2021-02-12T23:53:52Z- handleDvl() - RDI_Pathfinder msg: 1613174032.57 epoch sec; seqNo:3665 2021-02-12T23:53:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 82.84 , 96.90 , 61.30 2021-02-12T23:53:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , 0.04 2021-02-12T23:53:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174032.58 epoch sec; seqNo:7843; 4058115.27 north; 591693.48 east 2021-02-12T23:53:52Z- AHRS_M2 msg: 1613174032.94 epoch sec; seqNo:7781 2021-02-12T23:53:52Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.12 psi 2021-02-12T23:53:52Z- handleDepth()-Depth_Keller msg: 1613174032961 epoch sec; seqNo:7844; depth 28.40 2021-02-12T23:53:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174032.97 epoch sec; seqNo:7844; 4058115.27 north; 591693.48 east 2021-02-12T23:53:53Z- AHRS_M2 msg: 1613174033.34 epoch sec; seqNo:7782 2021-02-12T23:53:53Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.12 psi 2021-02-12T23:53:53Z- handleDepth()-Depth_Keller msg: 1613174033410 epoch sec; seqNo:7845; depth 28.29 2021-02-12T23:53:53Z- handleDvl() - RDI_Pathfinder msg: 1613174033.42 epoch sec; seqNo:3666 2021-02-12T23:53:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.52 , 0.00 , 99.00 , 61.71 2021-02-12T23:53:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.09 , -0.01 2021-02-12T23:53:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174033.43 epoch sec; seqNo:7845; 4058115.66 north; 591694.69 east 2021-02-12T23:53:53Z- AHRS_M2 msg: 1613174033.74 epoch sec; seqNo:7783 2021-02-12T23:53:53Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.13 psi 2021-02-12T23:53:53Z- handleDepth()-Depth_Keller msg: 1613174033777 epoch sec; seqNo:7846; depth 28.15 2021-02-12T23:53:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:53Z- handleDvl() - RDI_Pathfinder msg: 1613174033.78 epoch sec; seqNo:3667 2021-02-12T23:53:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.09 , -0.01 2021-02-12T23:53:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174033.79 epoch sec; seqNo:7846; 4058116.04 north; 591694.69 east 2021-02-12T23:53:54Z- AHRS_M2 msg: 1613174034.15 epoch sec; seqNo:7784 2021-02-12T23:53:54Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.13 psi 2021-02-12T23:53:54Z- handleDepth()-Depth_Keller msg: 1613174034174 epoch sec; seqNo:7847; depth 28.03 2021-02-12T23:53:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174034.19 epoch sec; seqNo:7847; 4058116.04 north; 591694.69 east 2021-02-12T23:53:54Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-12T23:53:54Z- waiting for 1613174032.36, time is 1613174034.19 2021-02-12T23:53:54Z- LcmTrn::updateTrn() >>>> pose time 1613174034.19, meas time 1613174033.78 <<<< 2021-02-12T23:53:54Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174033.78, ping # 0. 2021-02-12T23:53:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174033.78 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:53:54Z- TerrainNav::motionUpdate() - currEstimate = (4058116, 591694.69, 28.03) 2021-02-12T23:53:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:53:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:53:54Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:53:54Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:53:54Z- TNavBF::Projecting Measurements 2021-02-12T23:53:54Z- TNavFilter::Measurement from time = 1613174033.78 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:53:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:53:54Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:53:54Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:53:54Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:53:54Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:53:54Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:53:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:53:54Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 730.0 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-12T23:53:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:53:54Z- TNavBF::mmse selected from filter -1 2021-02-12T23:53:54Z- variances([0], [2], and psi_berg) = 729.931020 833.016515 0.000000 2021-02-12T23:53:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:53:54Z- LcmTrn::publishEstimates() - MLE : 4058109.54 591646.19 28.03 2021-02-12T23:53:54Z- LcmTrn::publishEstimates() - MMSE : 4058116.04 591694.69 28.03 2021-02-12T23:53:54Z- LcmTrn::publishEstimates() - COVAR: 729.93 833.02 1.32 0.00 2021-02-12T23:53:54Z- AHRS_M2 msg: 1613174034.55 epoch sec; seqNo:7785 2021-02-12T23:53:54Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.13 psi 2021-02-12T23:53:54Z- handleDepth()-Depth_Keller msg: 1613174034561 epoch sec; seqNo:7848; depth 27.91 2021-02-12T23:53:54Z- handleDvl() - RDI_Pathfinder msg: 1613174034.57 epoch sec; seqNo:3668 2021-02-12T23:53:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 82.84 , 97.98 , 61.30 2021-02-12T23:53:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.07 , -0.01 2021-02-12T23:53:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174034.58 epoch sec; seqNo:7848; 4058116.42 north; 591694.69 east 2021-02-12T23:53:54Z- AHRS_M2 msg: 1613174034.96 epoch sec; seqNo:7786 2021-02-12T23:53:54Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-12T23:53:54Z- handleDepth()-Depth_Keller msg: 1613174034981 epoch sec; seqNo:7849; depth 27.79 2021-02-12T23:53:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:54Z- handleDvl() - RDI_Pathfinder msg: 1613174034.98 epoch sec; seqNo:3669 2021-02-12T23:53:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.07 , -0.01 2021-02-12T23:53:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174034.99 epoch sec; seqNo:7849; 4058116.80 north; 591694.68 east 2021-02-12T23:53:55Z- AHRS_M2 msg: 1613174035.36 epoch sec; seqNo:7787 2021-02-12T23:53:55Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-12T23:53:55Z- handleDepth()-Depth_Keller msg: 1613174035367 epoch sec; seqNo:7850; depth 27.66 2021-02-12T23:53:55Z- handleDvl() - RDI_Pathfinder msg: 1613174035.38 epoch sec; seqNo:3670 2021-02-12T23:53:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 83.92 , 100.14 , 61.71 2021-02-12T23:53:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.04 , -0.02 2021-02-12T23:53:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174035.40 epoch sec; seqNo:7850; 4058116.80 north; 591694.68 east 2021-02-12T23:53:55Z- AHRS_M2 msg: 1613174035.76 epoch sec; seqNo:7788 2021-02-12T23:53:55Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-12T23:53:55Z- handleDepth()-Depth_Keller msg: 1613174035773 epoch sec; seqNo:7851; depth 27.53 2021-02-12T23:53:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174035.78 epoch sec; seqNo:7851; 4058117.19 north; 591695.90 east 2021-02-12T23:53:56Z- AHRS_M2 msg: 1613174036.17 epoch sec; seqNo:7789 2021-02-12T23:53:56Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-12T23:53:56Z- handleDepth()-Depth_Keller msg: 1613174036174 epoch sec; seqNo:7852; depth 27.42 2021-02-12T23:53:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:56Z- handleDvl() - RDI_Pathfinder msg: 1613174036.18 epoch sec; seqNo:3671 2021-02-12T23:53:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.04 , -0.02 2021-02-12T23:53:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174036.18 epoch sec; seqNo:7852; 4058117.57 north; 591695.90 east 2021-02-12T23:53:56Z- AHRS_M2 msg: 1613174036.57 epoch sec; seqNo:7790 2021-02-12T23:53:56Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-12T23:53:56Z- handleDepth()-Depth_Keller msg: 1613174036577 epoch sec; seqNo:7853; depth 27.30 2021-02-12T23:53:56Z- handleDvl() - RDI_Pathfinder msg: 1613174036.60 epoch sec; seqNo:3672 2021-02-12T23:53:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 0.00 , 100.14 , 63.76 2021-02-12T23:53:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.01 , -0.01 2021-02-12T23:53:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174036.61 epoch sec; seqNo:7853; 4058117.57 north; 591695.90 east 2021-02-12T23:53:56Z- AHRS_M2 msg: 1613174036.98 epoch sec; seqNo:7791 2021-02-12T23:53:56Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-12T23:53:56Z- handleDepth()-Depth_Keller msg: 1613174036980 epoch sec; seqNo:7854; depth 27.18 2021-02-12T23:53:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174036.99 epoch sec; seqNo:7854; 4058117.95 north; 591695.89 east 2021-02-12T23:53:57Z- AHRS_M2 msg: 1613174037.38 epoch sec; seqNo:7792 2021-02-12T23:53:57Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-12T23:53:57Z- handleDepth()-Depth_Keller msg: 1613174037389 epoch sec; seqNo:7855; depth 27.07 2021-02-12T23:53:57Z- handleDvl() - RDI_Pathfinder msg: 1613174037.40 epoch sec; seqNo:3673 2021-02-12T23:53:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 0.00 , 100.14 , 64.57 2021-02-12T23:53:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , -0.03 2021-02-12T23:53:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174037.43 epoch sec; seqNo:7855; 4058118.33 north; 591695.89 east 2021-02-12T23:53:57Z- AHRS_M2 msg: 1613174037.78 epoch sec; seqNo:7793 2021-02-12T23:53:57Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-12T23:53:57Z- handleDepth()-Depth_Keller msg: 1613174037791 epoch sec; seqNo:7856; depth 26.94 2021-02-12T23:53:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174037.80 epoch sec; seqNo:7856; 4058118.33 north; 591695.89 east 2021-02-12T23:53:58Z- AHRS_M2 msg: 1613174038.19 epoch sec; seqNo:7794 2021-02-12T23:53:58Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-12T23:53:58Z- handleDepth()-Depth_Keller msg: 1613174038192 epoch sec; seqNo:7857; depth 26.82 2021-02-12T23:53:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174038.21 epoch sec; seqNo:7857; 4058118.72 north; 591697.10 east 2021-02-12T23:53:58Z- handleDepth()-Depth_Keller msg: 1613174038613 epoch sec; seqNo:7858; depth 26.71 2021-02-12T23:53:58Z- handleDvl() - RDI_Pathfinder msg: 1613174038.63 epoch sec; seqNo:3674 2021-02-12T23:53:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 87.16 , 97.98 , 63.76 2021-02-12T23:53:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , -0.05 2021-02-12T23:53:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174038.64 epoch sec; seqNo:7858; 4058119.10 north; 591697.10 east 2021-02-12T23:53:59Z- AHRS_M2 msg: 1613174039.00 epoch sec; seqNo:7795 2021-02-12T23:53:59Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-12T23:53:59Z- handleDepth()-Depth_Keller msg: 1613174039002 epoch sec; seqNo:7859; depth 26.60 2021-02-12T23:53:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174039.01 epoch sec; seqNo:7859; 4058119.48 north; 591697.10 east 2021-02-12T23:53:59Z- AHRS_M2 msg: 1613174039.40 epoch sec; seqNo:7796 2021-02-12T23:53:59Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-12T23:53:59Z- handleDepth()-Depth_Keller msg: 1613174039417 epoch sec; seqNo:7860; depth 26.49 2021-02-12T23:53:59Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-12T23:53:59Z- waiting for 1613174037.19, time is 1613174039.40 2021-02-12T23:53:59Z- LcmTrn::updateTrn() >>>> pose time 1613174039.40, meas time 1613174038.63 <<<< 2021-02-12T23:53:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174038.63 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:53:59Z- TerrainNav::motionUpdate() - currEstimate = (4058119, 591697.10, 26.49) 2021-02-12T23:53:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:53:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:53:59Z- TNavBF::Projecting Measurements 2021-02-12T23:53:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:53:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:53:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:53:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:53:59Z- TNavFilter::4 good beams. 2021-02-12T23:53:59Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:53:59Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:53:59Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:53:59Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:53:59Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:53:59Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:53:59Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:53:59Z- TNavBF::mmse selected from filter -1 2021-02-12T23:53:59Z- TerrainMapOctree: (vehN, vehE ) = (4058118.97, 591696.74). 2021-02-12T23:53:59Z- TerrainMapOctree: Min Distance = 4040.11. 2021-02-12T23:53:59Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:53:59Z- There is no specific good beam that all particles have in common. 2021-02-12T23:53:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:53:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.50 K: 0.136 ExpectedMeasDiff: nan currMeas.covariance: 7.81 2021-02-12T23:53:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:53:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 12.16 2021-02-12T23:53:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:53:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-12T23:53:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:53:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.119 ExpectedMeasDiff: nan currMeas.covariance: 6.50 2021-02-12T23:53:59Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:53:59Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:53:59Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:53:59Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:53:59Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:53:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:53:59Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:53:59Z- Filter 0 No beams used in subcloud update 2021-02-12T23:53:59Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:53:59Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:53:59Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 729.9 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:53:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174038.63 sec. 2021-02-12T23:53:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:53:59Z- TNavBF::mmse selected from filter -1 2021-02-12T23:53:59Z- variances([0], [2], and psi_berg) = 729.829662 833.016441 0.000000 2021-02-12T23:53:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:53:59Z- LcmTrn::publishEstimates() - MLE : 4058112.98 591648.60 26.49 2021-02-12T23:53:59Z- LcmTrn::publishEstimates() - MMSE : 4058119.48 591697.10 26.49 2021-02-12T23:53:59Z- LcmTrn::publishEstimates() - COVAR: 729.83 833.02 1.01 0.00 2021-02-12T23:53:59Z- handleDvl() - RDI_Pathfinder msg: 1613174039.43 epoch sec; seqNo:3675 2021-02-12T23:53:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 87.16 , 99.06 , 63.35 2021-02-12T23:53:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , -0.07 2021-02-12T23:53:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174039.44 epoch sec; seqNo:7860; 4058119.48 north; 591697.10 east 2021-02-12T23:53:59Z- AHRS_M2 msg: 1613174039.80 epoch sec; seqNo:7797 2021-02-12T23:53:59Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-12T23:53:59Z- handleDepth()-Depth_Keller msg: 1613174039813 epoch sec; seqNo:7861; depth 26.36 2021-02-12T23:53:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:53:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:53:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:53:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:53:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:53:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:53:59Z- handleDvl() - RDI_Pathfinder msg: 1613174039.82 epoch sec; seqNo:3676 2021-02-12T23:53:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:53:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , -0.07 2021-02-12T23:53:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174039.83 epoch sec; seqNo:7861; 4058119.85 north; 591697.09 east 2021-02-12T23:54:00Z- AHRS_M2 msg: 1613174040.21 epoch sec; seqNo:7798 2021-02-12T23:54:00Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.15 psi 2021-02-12T23:54:00Z- handleDepth()-Depth_Keller msg: 1613174040217 epoch sec; seqNo:7862; depth 26.26 2021-02-12T23:54:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174040.23 epoch sec; seqNo:7862; 4058120.23 north; 591697.09 east 2021-02-12T23:54:00Z- AHRS_M2 msg: 1613174040.61 epoch sec; seqNo:7799 2021-02-12T23:54:00Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-12T23:54:00Z- handleDepth()-Depth_Keller msg: 1613174040616 epoch sec; seqNo:7863; depth 26.14 2021-02-12T23:54:00Z- handleDvl() - RDI_Pathfinder msg: 1613174040.63 epoch sec; seqNo:3677 2021-02-12T23:54:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 86.01 , 97.91 , 64.57 2021-02-12T23:54:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.10 , -0.07 2021-02-12T23:54:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174040.64 epoch sec; seqNo:7863; 4058120.23 north; 591697.09 east 2021-02-12T23:54:01Z- AHRS_M2 msg: 1613174041.02 epoch sec; seqNo:7800 2021-02-12T23:54:01Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-12T23:54:01Z- handleDepth()-Depth_Keller msg: 1613174041022 epoch sec; seqNo:7864; depth 26.04 2021-02-12T23:54:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:01Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:01Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:01Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:01Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:01Z- handleDvl() - RDI_Pathfinder msg: 1613174041.02 epoch sec; seqNo:3678 2021-02-12T23:54:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.10 , -0.07 2021-02-12T23:54:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174041.04 epoch sec; seqNo:7864; 4058120.61 north; 591697.08 east 2021-02-12T23:54:01Z- AHRS_M2 msg: 1613174041.42 epoch sec; seqNo:7801 2021-02-12T23:54:01Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.13 psi 2021-02-12T23:54:01Z- handleDepth()-Depth_Keller msg: 1613174041434 epoch sec; seqNo:7865; depth 25.93 2021-02-12T23:54:01Z- handleDvl() - RDI_Pathfinder msg: 1613174041.45 epoch sec; seqNo:3679 2021-02-12T23:54:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 85.00 , 97.98 , 64.16 2021-02-12T23:54:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.13 , -0.02 2021-02-12T23:54:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174041.46 epoch sec; seqNo:7865; 4058121.00 north; 591698.30 east 2021-02-12T23:54:01Z- AHRS_M2 msg: 1613174041.82 epoch sec; seqNo:7802 2021-02-12T23:54:01Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.13 psi 2021-02-12T23:54:01Z- handleDepth()-Depth_Keller msg: 1613174041829 epoch sec; seqNo:7866; depth 25.80 2021-02-12T23:54:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174041.84 epoch sec; seqNo:7866; 4058121.38 north; 591698.30 east 2021-02-12T23:54:02Z- AHRS_M2 msg: 1613174042.23 epoch sec; seqNo:7803 2021-02-12T23:54:02Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.13 psi 2021-02-12T23:54:02Z- handleDepth()-Depth_Keller msg: 1613174042233 epoch sec; seqNo:7867; depth 25.70 2021-02-12T23:54:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:02Z- handleDvl() - RDI_Pathfinder msg: 1613174042.23 epoch sec; seqNo:3680 2021-02-12T23:54:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.13 , -0.02 2021-02-12T23:54:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174042.24 epoch sec; seqNo:7867; 4058121.38 north; 591698.30 east 2021-02-12T23:54:02Z- AHRS_M2 msg: 1613174042.63 epoch sec; seqNo:7804 2021-02-12T23:54:02Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.14 psi 2021-02-12T23:54:02Z- handleDepth()-Depth_Keller msg: 1613174042638 epoch sec; seqNo:7868; depth 25.58 2021-02-12T23:54:02Z- handleDvl() - RDI_Pathfinder msg: 1613174042.65 epoch sec; seqNo:3681 2021-02-12T23:54:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.52 , 86.08 , 94.74 , 64.16 2021-02-12T23:54:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.15 , 0.07 2021-02-12T23:54:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174042.66 epoch sec; seqNo:7868; 4058121.76 north; 591698.29 east 2021-02-12T23:54:03Z- AHRS_M2 msg: 1613174043.04 epoch sec; seqNo:7805 2021-02-12T23:54:03Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.14 psi 2021-02-12T23:54:03Z- handleDepth()-Depth_Keller msg: 1613174043040 epoch sec; seqNo:7869; depth 25.46 2021-02-12T23:54:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174043.06 epoch sec; seqNo:7869; 4058122.14 north; 591698.29 east 2021-02-12T23:54:03Z- AHRS_M2 msg: 1613174043.44 epoch sec; seqNo:7806 2021-02-12T23:54:03Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.14 psi 2021-02-12T23:54:03Z- handleDepth()-Depth_Keller msg: 1613174043449 epoch sec; seqNo:7870; depth 25.37 2021-02-12T23:54:03Z- handleDvl() - RDI_Pathfinder msg: 1613174043.48 epoch sec; seqNo:3682 2021-02-12T23:54:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 85.00 , 97.98 , 65.80 2021-02-12T23:54:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , 0.13 2021-02-12T23:54:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174043.50 epoch sec; seqNo:7870; 4058122.52 north; 591698.28 east 2021-02-12T23:54:03Z- AHRS_M2 msg: 1613174043.84 epoch sec; seqNo:7807 2021-02-12T23:54:03Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.14 psi 2021-02-12T23:54:03Z- handleDepth()-Depth_Keller msg: 1613174043849 epoch sec; seqNo:7871; depth 25.24 2021-02-12T23:54:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174043.86 epoch sec; seqNo:7871; 4058122.90 north; 591698.28 east 2021-02-12T23:54:04Z- AHRS_M2 msg: 1613174044.25 epoch sec; seqNo:7808 2021-02-12T23:54:04Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-12T23:54:04Z- handleDepth()-Depth_Keller msg: 1613174044254 epoch sec; seqNo:7872; depth 25.15 2021-02-12T23:54:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174044.27 epoch sec; seqNo:7872; 4058122.91 north; 591699.50 east 2021-02-12T23:54:04Z- LcmTrn::handleMessages() - handling 45 messages took 0.67 ms and 132 calls... 2021-02-12T23:54:04Z- waiting for 1613174042.40, time is 1613174044.27 2021-02-12T23:54:04Z- LcmTrn::updateTrn() >>>> pose time 1613174044.27, meas time 1613174043.48 <<<< 2021-02-12T23:54:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174043.48 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:54:04Z- TerrainNav::motionUpdate() - currEstimate = (4058123, 591699.50, 25.15) 2021-02-12T23:54:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:54:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:54:04Z- TNavBF::Projecting Measurements 2021-02-12T23:54:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:54:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:54:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:54:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:54:04Z- TNavFilter::4 good beams. 2021-02-12T23:54:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:54:04Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:54:04Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:54:04Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:54:04Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:54:04Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:54:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:54:04Z- TNavBF::mmse selected from filter -1 2021-02-12T23:54:04Z- TerrainMapOctree: (vehN, vehE ) = (4058122.36, 591699.11). 2021-02-12T23:54:04Z- TerrainMapOctree: Min Distance = 4035.98. 2021-02-12T23:54:04Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:54:04Z- There is no specific good beam that all particles have in common. 2021-02-12T23:54:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:54:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.108 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-12T23:54:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:54:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 11.56 2021-02-12T23:54:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:54:04Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-12T23:54:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:54:04Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 6.93 2021-02-12T23:54:04Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:54:04Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:54:04Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:54:04Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:54:04Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:54:04Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:54:04Z- Filter 0 No beams used in subcloud update 2021-02-12T23:54:04Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:54:04Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:54:04Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 729.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:54:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174043.48 sec. 2021-02-12T23:54:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:54:04Z- TNavBF::mmse selected from filter -1 2021-02-12T23:54:04Z- variances([0], [2], and psi_berg) = 729.727710 833.016367 0.000000 2021-02-12T23:54:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:54:04Z- LcmTrn::publishEstimates() - MLE : 4058116.41 591652.00 25.15 2021-02-12T23:54:04Z- LcmTrn::publishEstimates() - MMSE : 4058122.91 591699.50 25.15 2021-02-12T23:54:04Z- LcmTrn::publishEstimates() - COVAR: 729.73 833.02 0.87 0.00 2021-02-12T23:54:04Z- AHRS_M2 msg: 1613174044.66 epoch sec; seqNo:7809 2021-02-12T23:54:04Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-12T23:54:04Z- handleDepth()-Depth_Keller msg: 1613174044664 epoch sec; seqNo:7873; depth 25.03 2021-02-12T23:54:04Z- handleDvl() - RDI_Pathfinder msg: 1613174044.68 epoch sec; seqNo:3683 2021-02-12T23:54:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.93 , 85.00 , 97.98 , 65.80 2021-02-12T23:54:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , 0.15 2021-02-12T23:54:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174044.69 epoch sec; seqNo:7873; 4058123.29 north; 591699.50 east 2021-02-12T23:54:05Z- AHRS_M2 msg: 1613174045.07 epoch sec; seqNo:7810 2021-02-12T23:54:05Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-12T23:54:05Z- handleDepth()-Depth_Keller msg: 1613174045142 epoch sec; seqNo:7874; depth 24.93 2021-02-12T23:54:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174045.16 epoch sec; seqNo:7874; 4058123.67 north; 591699.49 east 2021-02-12T23:54:05Z- AHRS_M2 msg: 1613174045.46 epoch sec; seqNo:7811 2021-02-12T23:54:05Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-12T23:54:05Z- handleDepth()-Depth_Keller msg: 1613174045475 epoch sec; seqNo:7875; depth 24.80 2021-02-12T23:54:05Z- handleDvl() - RDI_Pathfinder msg: 1613174045.49 epoch sec; seqNo:3684 2021-02-12T23:54:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 86.01 , 97.91 , 65.39 2021-02-12T23:54:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.11 , 0.08 2021-02-12T23:54:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174045.52 epoch sec; seqNo:7875; 4058124.05 north; 591699.49 east 2021-02-12T23:54:05Z- AHRS_M2 msg: 1613174045.86 epoch sec; seqNo:7812 2021-02-12T23:54:05Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.15 psi 2021-02-12T23:54:05Z- handleDepth()-Depth_Keller msg: 1613174045889 epoch sec; seqNo:7876; depth 24.71 2021-02-12T23:54:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:05Z- handleDvl() - RDI_Pathfinder msg: 1613174045.89 epoch sec; seqNo:3685 2021-02-12T23:54:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.11 , 0.08 2021-02-12T23:54:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174045.90 epoch sec; seqNo:7876; 4058124.05 north; 591699.49 east 2021-02-12T23:54:06Z- AHRS_M2 msg: 1613174046.27 epoch sec; seqNo:7813 2021-02-12T23:54:06Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.14 psi 2021-02-12T23:54:06Z- handleDepth()-Depth_Keller msg: 1613174046293 epoch sec; seqNo:7877; depth 24.60 2021-02-12T23:54:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174046.31 epoch sec; seqNo:7877; 4058124.44 north; 591700.71 east 2021-02-12T23:54:06Z- AHRS_M2 msg: 1613174046.67 epoch sec; seqNo:7814 2021-02-12T23:54:06Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.14 psi 2021-02-12T23:54:06Z- handleDepth()-Depth_Keller msg: 1613174046677 epoch sec; seqNo:7878; depth 24.48 2021-02-12T23:54:06Z- handleDvl() - RDI_Pathfinder msg: 1613174046.69 epoch sec; seqNo:3686 2021-02-12T23:54:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 0.00 , 96.83 , 65.80 2021-02-12T23:54:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.12 , -0.02 2021-02-12T23:54:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174046.70 epoch sec; seqNo:7878; 4058124.82 north; 591700.70 east 2021-02-12T23:54:07Z- AHRS_M2 msg: 1613174047.08 epoch sec; seqNo:7815 2021-02-12T23:54:07Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.14 psi 2021-02-12T23:54:07Z- handleDepth()-Depth_Keller msg: 1613174047145 epoch sec; seqNo:7879; depth 24.39 2021-02-12T23:54:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:07Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:07Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:07Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:07Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:07Z- handleDvl() - RDI_Pathfinder msg: 1613174047.15 epoch sec; seqNo:3687 2021-02-12T23:54:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.12 , -0.02 2021-02-12T23:54:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174047.17 epoch sec; seqNo:7879; 4058124.82 north; 591700.70 east 2021-02-12T23:54:07Z- AHRS_M2 msg: 1613174047.48 epoch sec; seqNo:7816 2021-02-12T23:54:07Z- AHRS_M2 msg: 0.67 phi; 0.30 theta; -0.14 psi 2021-02-12T23:54:07Z- handleDepth()-Depth_Keller msg: 1613174047493 epoch sec; seqNo:7880; depth 24.26 2021-02-12T23:54:07Z- handleDvl() - RDI_Pathfinder msg: 1613174047.51 epoch sec; seqNo:3688 2021-02-12T23:54:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 0.00 , 96.83 , 67.03 2021-02-12T23:54:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.07 , -0.09 2021-02-12T23:54:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174047.54 epoch sec; seqNo:7880; 4058125.20 north; 591700.70 east 2021-02-12T23:54:07Z- AHRS_M2 msg: 1613174047.88 epoch sec; seqNo:7817 2021-02-12T23:54:07Z- AHRS_M2 msg: 0.68 phi; 0.30 theta; -0.14 psi 2021-02-12T23:54:07Z- handleDepth()-Depth_Keller msg: 1613174047909 epoch sec; seqNo:7881; depth 24.16 2021-02-12T23:54:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174047.92 epoch sec; seqNo:7881; 4058125.20 north; 591700.70 east 2021-02-12T23:54:08Z- AHRS_M2 msg: 1613174048.29 epoch sec; seqNo:7818 2021-02-12T23:54:08Z- AHRS_M2 msg: 0.68 phi; 0.30 theta; -0.15 psi 2021-02-12T23:54:08Z- handleDepth()-Depth_Keller msg: 1613174048307 epoch sec; seqNo:7882; depth 24.06 2021-02-12T23:54:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:08Z- handleDvl() - RDI_Pathfinder msg: 1613174048.31 epoch sec; seqNo:3689 2021-02-12T23:54:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.07 , -0.09 2021-02-12T23:54:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174048.32 epoch sec; seqNo:7882; 4058125.59 north; 591701.91 east 2021-02-12T23:54:08Z- AHRS_M2 msg: 1613174048.69 epoch sec; seqNo:7819 2021-02-12T23:54:08Z- AHRS_M2 msg: 0.68 phi; 0.30 theta; -0.15 psi 2021-02-12T23:54:08Z- handleDepth()-Depth_Keller msg: 1613174048701 epoch sec; seqNo:7883; depth 23.96 2021-02-12T23:54:08Z- handleDvl() - RDI_Pathfinder msg: 1613174048.71 epoch sec; seqNo:3690 2021-02-12T23:54:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 87.10 , 96.83 , 67.03 2021-02-12T23:54:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.01 , -0.09 2021-02-12T23:54:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174048.73 epoch sec; seqNo:7883; 4058125.97 north; 591701.91 east 2021-02-12T23:54:09Z- AHRS_M2 msg: 1613174049.10 epoch sec; seqNo:7820 2021-02-12T23:54:09Z- AHRS_M2 msg: 0.67 phi; 0.30 theta; -0.15 psi 2021-02-12T23:54:09Z- handleDepth()-Depth_Keller msg: 1613174049105 epoch sec; seqNo:7884; depth 23.83 2021-02-12T23:54:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174049.12 epoch sec; seqNo:7884; 4058125.97 north; 591701.91 east 2021-02-12T23:54:09Z- AHRS_M2 msg: 1613174049.50 epoch sec; seqNo:7821 2021-02-12T23:54:09Z- AHRS_M2 msg: 0.67 phi; 0.30 theta; -0.15 psi 2021-02-12T23:54:09Z- handleDepth()-Depth_Keller msg: 1613174049511 epoch sec; seqNo:7885; depth 23.75 2021-02-12T23:54:09Z- handleDvl() - RDI_Pathfinder msg: 1613174049.52 epoch sec; seqNo:3691 2021-02-12T23:54:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:09Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:54:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174049.53 epoch sec; seqNo:7885; 4058126.35 north; 591701.91 east 2021-02-12T23:54:09Z- LcmTrn::handleMessages() - handling 48 messages took 0.83 ms and 132 calls... 2021-02-12T23:54:09Z- waiting for 1613174047.27, time is 1613174049.53 2021-02-12T23:54:09Z- LcmTrn::updateTrn() equating pose 1613174049.53 and meas times 1613174049.53 2021-02-12T23:54:09Z- LcmTrn::updateTrn() >>>> pose time 1613174049.53, meas time 1613174049.53 <<<< 2021-02-12T23:54:09Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174049.53, ping # 0. 2021-02-12T23:54:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174049.53 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:54:09Z- TerrainNav::motionUpdate() - currEstimate = (4058126, 591701.91, 23.75) 2021-02-12T23:54:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:54:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:54:09Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:54:09Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:54:09Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174049.53 sec.; No relevant navigation data available 2021-02-12T23:54:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:54:09Z- TNavBF::mmse selected from filter -1 2021-02-12T23:54:09Z- variances([0], [2], and psi_berg) = 729.650379 833.016311 0.000000 2021-02-12T23:54:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:54:09Z- LcmTrn::publishEstimates() - MLE : 4058119.85 591654.41 23.75 2021-02-12T23:54:09Z- LcmTrn::publishEstimates() - MMSE : 4058126.35 591701.91 23.75 2021-02-12T23:54:09Z- LcmTrn::publishEstimates() - COVAR: 729.65 833.02 1.09 0.00 2021-02-12T23:54:09Z- AHRS_M2 msg: 1613174049.90 epoch sec; seqNo:7822 2021-02-12T23:54:09Z- AHRS_M2 msg: 0.67 phi; 0.30 theta; -0.15 psi 2021-02-12T23:54:09Z- handleDepth()-Depth_Keller msg: 1613174049908 epoch sec; seqNo:7886; depth 23.62 2021-02-12T23:54:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174049.92 epoch sec; seqNo:7886; 4058126.73 north; 591701.90 east 2021-02-12T23:54:10Z- AHRS_M2 msg: 1613174050.31 epoch sec; seqNo:7823 2021-02-12T23:54:10Z- AHRS_M2 msg: 0.66 phi; 0.30 theta; -0.16 psi 2021-02-12T23:54:10Z- handleDepth()-Depth_Keller msg: 1613174050312 epoch sec; seqNo:7887; depth 23.53 2021-02-12T23:54:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174050.32 epoch sec; seqNo:7887; 4058126.73 north; 591701.90 east 2021-02-12T23:54:10Z- AHRS_M2 msg: 1613174050.71 epoch sec; seqNo:7824 2021-02-12T23:54:10Z- AHRS_M2 msg: 0.66 phi; 0.30 theta; -0.16 psi 2021-02-12T23:54:10Z- handleDepth()-Depth_Keller msg: 1613174050725 epoch sec; seqNo:7888; depth 23.42 2021-02-12T23:54:10Z- handleDvl() - RDI_Pathfinder msg: 1613174050.74 epoch sec; seqNo:3692 2021-02-12T23:54:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 88.18 , 97.91 , 66.62 2021-02-12T23:54:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.05 , 0.02 2021-02-12T23:54:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174050.75 epoch sec; seqNo:7888; 4058127.10 north; 591701.90 east 2021-02-12T23:54:11Z- AHRS_M2 msg: 1613174051.12 epoch sec; seqNo:7825 2021-02-12T23:54:11Z- AHRS_M2 msg: 0.66 phi; 0.30 theta; -0.17 psi 2021-02-12T23:54:11Z- handleDepth()-Depth_Keller msg: 1613174051125 epoch sec; seqNo:7889; depth 23.29 2021-02-12T23:54:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174051.14 epoch sec; seqNo:7889; 4058127.50 north; 591703.11 east 2021-02-12T23:54:11Z- AHRS_M2 msg: 1613174051.52 epoch sec; seqNo:7826 2021-02-12T23:54:11Z- AHRS_M2 msg: 0.65 phi; 0.30 theta; -0.18 psi 2021-02-12T23:54:11Z- handleDepth()-Depth_Keller msg: 1613174051525 epoch sec; seqNo:7890; depth 23.19 2021-02-12T23:54:11Z- handleDvl() - RDI_Pathfinder msg: 1613174051.54 epoch sec; seqNo:3693 2021-02-12T23:54:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 0.00 , 100.01 , 67.03 2021-02-12T23:54:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.00 , 0.03 2021-02-12T23:54:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174051.55 epoch sec; seqNo:7890; 4058127.50 north; 591703.11 east 2021-02-12T23:54:11Z- AHRS_M2 msg: 1613174051.92 epoch sec; seqNo:7827 2021-02-12T23:54:11Z- AHRS_M2 msg: 0.65 phi; 0.30 theta; -0.19 psi 2021-02-12T23:54:11Z- handleDepth()-Depth_Keller msg: 1613174051929 epoch sec; seqNo:7891; depth 23.08 2021-02-12T23:54:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:11Z- handleDvl() - RDI_Pathfinder msg: 1613174051.93 epoch sec; seqNo:3694 2021-02-12T23:54:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.00 , 0.03 2021-02-12T23:54:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174051.94 epoch sec; seqNo:7891; 4058127.88 north; 591703.11 east 2021-02-12T23:54:12Z- AHRS_M2 msg: 1613174052.36 epoch sec; seqNo:7828 2021-02-12T23:54:12Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.20 psi 2021-02-12T23:54:12Z- handleDepth()-Depth_Keller msg: 1613174052360 epoch sec; seqNo:7892; depth 22.98 2021-02-12T23:54:12Z- handleDvl() - RDI_Pathfinder msg: 1613174052.37 epoch sec; seqNo:3695 2021-02-12T23:54:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:12Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:54:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174052.40 epoch sec; seqNo:7892; 4058128.25 north; 591703.11 east 2021-02-12T23:54:12Z- AHRS_M2 msg: 1613174052.73 epoch sec; seqNo:7829 2021-02-12T23:54:12Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.21 psi 2021-02-12T23:54:12Z- handleDepth()-Depth_Keller msg: 1613174052736 epoch sec; seqNo:7893; depth 22.84 2021-02-12T23:54:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174052.75 epoch sec; seqNo:7893; 4058128.25 north; 591703.11 east 2021-02-12T23:54:13Z- AHRS_M2 msg: 1613174053.14 epoch sec; seqNo:7830 2021-02-12T23:54:13Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.22 psi 2021-02-12T23:54:13Z- handleDepth()-Depth_Keller msg: 1613174053149 epoch sec; seqNo:7894; depth 22.74 2021-02-12T23:54:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:13Z- handleDvl() - RDI_Pathfinder msg: 1613174053.15 epoch sec; seqNo:3696 2021-02-12T23:54:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:13Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:54:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174053.17 epoch sec; seqNo:7894; 4058128.63 north; 591703.10 east 2021-02-12T23:54:13Z- AHRS_M2 msg: 1613174053.54 epoch sec; seqNo:7831 2021-02-12T23:54:13Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.23 psi 2021-02-12T23:54:13Z- handleDepth()-Depth_Keller msg: 1613174053547 epoch sec; seqNo:7895; depth 22.65 2021-02-12T23:54:13Z- handleDvl() - RDI_Pathfinder msg: 1613174053.56 epoch sec; seqNo:3697 2021-02-12T23:54:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 97.51 , 101.56 , 78.58 2021-02-12T23:54:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.00 , 0.06 2021-02-12T23:54:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174053.57 epoch sec; seqNo:7895; 4058129.03 north; 591704.32 east 2021-02-12T23:54:13Z- AHRS_M2 msg: 1613174053.95 epoch sec; seqNo:7832 2021-02-12T23:54:13Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.24 psi 2021-02-12T23:54:13Z- handleDepth()-Depth_Keller msg: 1613174053950 epoch sec; seqNo:7896; depth 22.52 2021-02-12T23:54:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174053.96 epoch sec; seqNo:7896; 4058129.03 north; 591704.32 east 2021-02-12T23:54:14Z- AHRS_M2 msg: 1613174054.35 epoch sec; seqNo:7833 2021-02-12T23:54:14Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.24 psi 2021-02-12T23:54:14Z- handleDepth()-Depth_Keller msg: 1613174054389 epoch sec; seqNo:7897; depth 22.43 2021-02-12T23:54:14Z- handleDvl() - RDI_Pathfinder msg: 1613174054.41 epoch sec; seqNo:3698 2021-02-12T23:54:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.93 , 97.98 , 101.63 , 69.89 2021-02-12T23:54:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , 0.04 2021-02-12T23:54:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174054.42 epoch sec; seqNo:7897; 4058129.40 north; 591704.32 east 2021-02-12T23:54:14Z- LcmTrn::handleMessages() - handling 43 messages took 0.62 ms and 131 calls... 2021-02-12T23:54:14Z- waiting for 1613174052.53, time is 1613174054.42 2021-02-12T23:54:14Z- LcmTrn::updateTrn() equating pose 1613174054.42 and meas times 1613174054.42 2021-02-12T23:54:14Z- LcmTrn::updateTrn() >>>> pose time 1613174054.42, meas time 1613174054.42 <<<< 2021-02-12T23:54:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174054.42 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:54:14Z- TerrainNav::motionUpdate() - currEstimate = (4058129, 591704.32, 22.43) 2021-02-12T23:54:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:54:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:54:14Z- TNavBF::Projecting Measurements 2021-02-12T23:54:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:54:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:54:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:54:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:54:14Z- TNavFilter::4 good beams. 2021-02-12T23:54:14Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:54:14Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:54:14Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:54:14Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:54:14Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:54:14Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:54:14Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:54:14Z- TNavBF::mmse selected from filter -1 2021-02-12T23:54:14Z- TerrainMapOctree: (vehN, vehE ) = (4058129.40, 591704.32). 2021-02-12T23:54:14Z- TerrainMapOctree: Min Distance = 4027.24. 2021-02-12T23:54:14Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:54:14Z- There is no specific good beam that all particles have in common. 2021-02-12T23:54:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:54:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.114 ExpectedMeasDiff: nan currMeas.covariance: 8.28 2021-02-12T23:54:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:54:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-12T23:54:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:54:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 16.53 2021-02-12T23:54:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:54:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 7.81 2021-02-12T23:54:14Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:54:14Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:54:14Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:54:14Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:54:14Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:54:14Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:54:14Z- Filter 0 No beams used in subcloud update 2021-02-12T23:54:14Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:54:14Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:54:14Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 729.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:54:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174054.42 sec. 2021-02-12T23:54:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:54:14Z- TNavBF::mmse selected from filter -1 2021-02-12T23:54:14Z- variances([0], [2], and psi_berg) = 729.575949 833.016257 0.000000 2021-02-12T23:54:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:54:14Z- LcmTrn::publishEstimates() - MLE : 4058122.90 591656.82 22.43 2021-02-12T23:54:14Z- LcmTrn::publishEstimates() - MMSE : 4058129.40 591704.32 22.43 2021-02-12T23:54:14Z- LcmTrn::publishEstimates() - COVAR: 729.58 833.02 0.91 0.00 2021-02-12T23:54:14Z- AHRS_M2 msg: 1613174054.75 epoch sec; seqNo:7834 2021-02-12T23:54:14Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.24 psi 2021-02-12T23:54:14Z- handleDepth()-Depth_Keller msg: 1613174054757 epoch sec; seqNo:7898; depth 22.31 2021-02-12T23:54:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174054.77 epoch sec; seqNo:7898; 4058129.78 north; 591704.31 east 2021-02-12T23:54:15Z- AHRS_M2 msg: 1613174055.16 epoch sec; seqNo:7835 2021-02-12T23:54:15Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.24 psi 2021-02-12T23:54:15Z- handleDepth()-Depth_Keller msg: 1613174055163 epoch sec; seqNo:7899; depth 22.22 2021-02-12T23:54:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174055.17 epoch sec; seqNo:7899; 4058129.78 north; 591704.31 east 2021-02-12T23:54:15Z- AHRS_M2 msg: 1613174055.56 epoch sec; seqNo:7836 2021-02-12T23:54:15Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.23 psi 2021-02-12T23:54:15Z- handleDepth()-Depth_Keller msg: 1613174055567 epoch sec; seqNo:7900; depth 22.12 2021-02-12T23:54:15Z- handleDvl() - RDI_Pathfinder msg: 1613174055.59 epoch sec; seqNo:3699 2021-02-12T23:54:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 98.18 , 101.83 , 68.66 2021-02-12T23:54:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.05 2021-02-12T23:54:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174055.60 epoch sec; seqNo:7900; 4058130.16 north; 591704.31 east 2021-02-12T23:54:15Z- AHRS_M2 msg: 1613174055.96 epoch sec; seqNo:7837 2021-02-12T23:54:15Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.23 psi 2021-02-12T23:54:15Z- handleDepth()-Depth_Keller msg: 1613174055969 epoch sec; seqNo:7901; depth 22.00 2021-02-12T23:54:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174055.98 epoch sec; seqNo:7901; 4058130.54 north; 591704.30 east 2021-02-12T23:54:16Z- AHRS_M2 msg: 1613174056.37 epoch sec; seqNo:7838 2021-02-12T23:54:16Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.22 psi 2021-02-12T23:54:16Z- handleDepth()-Depth_Keller msg: 1613174056376 epoch sec; seqNo:7902; depth 21.91 2021-02-12T23:54:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174056.38 epoch sec; seqNo:7902; 4058130.93 north; 591705.52 east 2021-02-12T23:54:16Z- AHRS_M2 msg: 1613174056.77 epoch sec; seqNo:7839 2021-02-12T23:54:16Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.22 psi 2021-02-12T23:54:16Z- handleDepth()-Depth_Keller msg: 1613174056778 epoch sec; seqNo:7903; depth 21.80 2021-02-12T23:54:16Z- handleDvl() - RDI_Pathfinder msg: 1613174056.79 epoch sec; seqNo:3700 2021-02-12T23:54:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:16Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:54:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174056.80 epoch sec; seqNo:7903; 4058131.31 north; 591705.52 east 2021-02-12T23:54:17Z- AHRS_M2 msg: 1613174057.18 epoch sec; seqNo:7840 2021-02-12T23:54:17Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.21 psi 2021-02-12T23:54:17Z- handleDepth()-Depth_Keller msg: 1613174057181 epoch sec; seqNo:7904; depth 21.69 2021-02-12T23:54:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174057.20 epoch sec; seqNo:7904; 4058131.31 north; 591705.52 east 2021-02-12T23:54:17Z- AHRS_M2 msg: 1613174057.58 epoch sec; seqNo:7841 2021-02-12T23:54:17Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.20 psi 2021-02-12T23:54:17Z- handleDepth()-Depth_Keller msg: 1613174057644 epoch sec; seqNo:7905; depth 21.58 2021-02-12T23:54:17Z- handleDvl() - RDI_Pathfinder msg: 1613174057.66 epoch sec; seqNo:3701 2021-02-12T23:54:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 97.10 , 101.16 , 69.89 2021-02-12T23:54:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , 0.04 2021-02-12T23:54:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174057.67 epoch sec; seqNo:7905; 4058131.69 north; 591705.51 east 2021-02-12T23:54:17Z- AHRS_M2 msg: 1613174057.98 epoch sec; seqNo:7842 2021-02-12T23:54:17Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.19 psi 2021-02-12T23:54:17Z- handleDepth()-Depth_Keller msg: 1613174057996 epoch sec; seqNo:7906; depth 21.47 2021-02-12T23:54:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:18Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:18Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:18Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:18Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:18Z- handleDvl() - RDI_Pathfinder msg: 1613174058.00 epoch sec; seqNo:3702 2021-02-12T23:54:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , 0.04 2021-02-12T23:54:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174058.01 epoch sec; seqNo:7906; 4058132.07 north; 591705.51 east 2021-02-12T23:54:18Z- AHRS_M2 msg: 1613174058.39 epoch sec; seqNo:7843 2021-02-12T23:54:18Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.18 psi 2021-02-12T23:54:18Z- handleDepth()-Depth_Keller msg: 1613174058417 epoch sec; seqNo:7907; depth 21.39 2021-02-12T23:54:18Z- handleDvl() - RDI_Pathfinder msg: 1613174058.43 epoch sec; seqNo:3703 2021-02-12T23:54:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.38 , 98.18 , 100.62 , 69.48 2021-02-12T23:54:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.04 , 0.02 2021-02-12T23:54:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174058.45 epoch sec; seqNo:7907; 4058132.45 north; 591705.50 east 2021-02-12T23:54:18Z- AHRS_M2 msg: 1613174058.80 epoch sec; seqNo:7844 2021-02-12T23:54:18Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.18 psi 2021-02-12T23:54:18Z- handleDepth()-Depth_Keller msg: 1613174058826 epoch sec; seqNo:7908; depth 21.28 2021-02-12T23:54:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174058.84 epoch sec; seqNo:7908; 4058132.46 north; 591706.72 east 2021-02-12T23:54:19Z- AHRS_M2 msg: 1613174059.20 epoch sec; seqNo:7845 2021-02-12T23:54:19Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.17 psi 2021-02-12T23:54:19Z- handleDepth()-Depth_Keller msg: 1613174059201 epoch sec; seqNo:7909; depth 21.16 2021-02-12T23:54:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:19Z- handleDvl() - RDI_Pathfinder msg: 1613174059.20 epoch sec; seqNo:3704 2021-02-12T23:54:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.04 , 0.02 2021-02-12T23:54:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174059.22 epoch sec; seqNo:7909; 4058132.84 north; 591706.72 east 2021-02-12T23:54:19Z- AHRS_M2 msg: 1613174059.60 epoch sec; seqNo:7846 2021-02-12T23:54:19Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.16 psi 2021-02-12T23:54:19Z- handleDepth()-Depth_Keller msg: 1613174059610 epoch sec; seqNo:7910; depth 21.08 2021-02-12T23:54:19Z- handleDvl() - RDI_Pathfinder msg: 1613174059.62 epoch sec; seqNo:3705 2021-02-12T23:54:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.64 , 98.12 , 101.77 , 69.07 2021-02-12T23:54:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.08 , -0.03 2021-02-12T23:54:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174059.63 epoch sec; seqNo:7910; 4058133.22 north; 591706.72 east 2021-02-12T23:54:19Z- LcmTrn::handleMessages() - handling 46 messages took 0.73 ms and 132 calls... 2021-02-12T23:54:19Z- waiting for 1613174057.42, time is 1613174059.63 2021-02-12T23:54:19Z- LcmTrn::updateTrn() equating pose 1613174059.63 and meas times 1613174059.63 2021-02-12T23:54:19Z- LcmTrn::updateTrn() >>>> pose time 1613174059.63, meas time 1613174059.63 <<<< 2021-02-12T23:54:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174059.63 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:54:19Z- TerrainNav::motionUpdate() - currEstimate = (4058133, 591706.72, 21.08) 2021-02-12T23:54:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:54:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:54:19Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174059.63 sec.; No relevant navigation data available 2021-02-12T23:54:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:54:19Z- TNavBF::mmse selected from filter -1 2021-02-12T23:54:19Z- variances([0], [2], and psi_berg) = 729.495924 833.016199 0.000000 2021-02-12T23:54:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:54:19Z- LcmTrn::publishEstimates() - MLE : 4058126.72 591659.22 21.08 2021-02-12T23:54:19Z- LcmTrn::publishEstimates() - MMSE : 4058133.22 591706.72 21.08 2021-02-12T23:54:19Z- LcmTrn::publishEstimates() - COVAR: 729.50 833.02 1.14 0.00 2021-02-12T23:54:20Z- AHRS_M2 msg: 1613174060.00 epoch sec; seqNo:7847 2021-02-12T23:54:20Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.16 psi 2021-02-12T23:54:20Z- handleDepth()-Depth_Keller msg: 1613174060101 epoch sec; seqNo:7911; depth 20.96 2021-02-12T23:54:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174060.11 epoch sec; seqNo:7911; 4058133.22 north; 591706.72 east 2021-02-12T23:54:20Z- handleDepth()-Depth_Keller msg: 1613174060434 epoch sec; seqNo:7912; depth 20.83 2021-02-12T23:54:20Z- handleDvl() - RDI_Pathfinder msg: 1613174060.47 epoch sec; seqNo:3706 2021-02-12T23:54:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.79 , 91.96 , 99.26 , 69.48 2021-02-12T23:54:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.06 , -0.08 2021-02-12T23:54:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174060.50 epoch sec; seqNo:7912; 4058133.60 north; 591706.71 east 2021-02-12T23:54:20Z- AHRS_M2 msg: 1613174060.81 epoch sec; seqNo:7848 2021-02-12T23:54:20Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.17 psi 2021-02-12T23:54:20Z- handleDepth()-Depth_Keller msg: 1613174060819 epoch sec; seqNo:7913; depth 20.75 2021-02-12T23:54:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174060.83 epoch sec; seqNo:7913; 4058133.60 north; 591706.71 east 2021-02-12T23:54:21Z- AHRS_M2 msg: 1613174061.21 epoch sec; seqNo:7849 2021-02-12T23:54:21Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.17 psi 2021-02-12T23:54:21Z- handleDepth()-Depth_Keller msg: 1613174061222 epoch sec; seqNo:7914; depth 20.64 2021-02-12T23:54:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174061.24 epoch sec; seqNo:7914; 4058133.99 north; 591707.93 east 2021-02-12T23:54:21Z- AHRS_M2 msg: 1613174061.62 epoch sec; seqNo:7850 2021-02-12T23:54:21Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.18 psi 2021-02-12T23:54:21Z- handleDepth()-Depth_Keller msg: 1613174061629 epoch sec; seqNo:7915; depth 20.55 2021-02-12T23:54:21Z- handleDvl() - RDI_Pathfinder msg: 1613174061.64 epoch sec; seqNo:3707 2021-02-12T23:54:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.79 , 98.32 , 100.75 , 70.29 2021-02-12T23:54:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.03 , -0.11 2021-02-12T23:54:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174061.65 epoch sec; seqNo:7915; 4058134.37 north; 591707.93 east 2021-02-12T23:54:22Z- AHRS_M2 msg: 1613174062.03 epoch sec; seqNo:7851 2021-02-12T23:54:22Z- AHRS_M2 msg: 0.61 phi; 0.29 theta; -0.18 psi 2021-02-12T23:54:22Z- handleDepth()-Depth_Keller msg: 1613174062039 epoch sec; seqNo:7916; depth 20.43 2021-02-12T23:54:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174062.05 epoch sec; seqNo:7916; 4058134.37 north; 591707.93 east 2021-02-12T23:54:22Z- AHRS_M2 msg: 1613174062.43 epoch sec; seqNo:7852 2021-02-12T23:54:22Z- AHRS_M2 msg: 0.61 phi; 0.29 theta; -0.18 psi 2021-02-12T23:54:22Z- handleDepth()-Depth_Keller msg: 1613174062434 epoch sec; seqNo:7917; depth 20.34 2021-02-12T23:54:22Z- handleDvl() - RDI_Pathfinder msg: 1613174062.45 epoch sec; seqNo:3708 2021-02-12T23:54:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.64 , 98.12 , 99.33 , 70.29 2021-02-12T23:54:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.17 , -0.07 2021-02-12T23:54:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174062.46 epoch sec; seqNo:7917; 4058134.75 north; 591707.92 east 2021-02-12T23:54:22Z- AHRS_M2 msg: 1613174062.83 epoch sec; seqNo:7853 2021-02-12T23:54:22Z- AHRS_M2 msg: 0.61 phi; 0.29 theta; -0.19 psi 2021-02-12T23:54:22Z- handleDepth()-Depth_Keller msg: 1613174062836 epoch sec; seqNo:7918; depth 20.23 2021-02-12T23:54:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:22Z- handleDvl() - RDI_Pathfinder msg: 1613174062.84 epoch sec; seqNo:3709 2021-02-12T23:54:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.17 , -0.07 2021-02-12T23:54:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174062.85 epoch sec; seqNo:7918; 4058135.13 north; 591707.92 east 2021-02-12T23:54:23Z- AHRS_M2 msg: 1613174063.24 epoch sec; seqNo:7854 2021-02-12T23:54:23Z- AHRS_M2 msg: 0.60 phi; 0.29 theta; -0.19 psi 2021-02-12T23:54:23Z- handleDepth()-Depth_Keller msg: 1613174063242 epoch sec; seqNo:7919; depth 20.13 2021-02-12T23:54:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174063.25 epoch sec; seqNo:7919; 4058135.13 north; 591707.92 east 2021-02-12T23:54:23Z- AHRS_M2 msg: 1613174063.64 epoch sec; seqNo:7855 2021-02-12T23:54:23Z- AHRS_M2 msg: 0.59 phi; 0.29 theta; -0.19 psi 2021-02-12T23:54:23Z- handleDepth()-Depth_Keller msg: 1613174063645 epoch sec; seqNo:7920; depth 20.02 2021-02-12T23:54:23Z- handleDvl() - RDI_Pathfinder msg: 1613174063.67 epoch sec; seqNo:3710 2021-02-12T23:54:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 100.48 , 69.89 2021-02-12T23:54:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.21 , 0.05 2021-02-12T23:54:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174063.68 epoch sec; seqNo:7920; 4058135.50 north; 591707.91 east 2021-02-12T23:54:24Z- AHRS_M2 msg: 1613174064.04 epoch sec; seqNo:7856 2021-02-12T23:54:24Z- AHRS_M2 msg: 0.58 phi; 0.30 theta; -0.19 psi 2021-02-12T23:54:24Z- handleDepth()-Depth_Keller msg: 1613174064053 epoch sec; seqNo:7921; depth 19.92 2021-02-12T23:54:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:24Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:24Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:24Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:24Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:24Z- handleDvl() - RDI_Pathfinder msg: 1613174064.06 epoch sec; seqNo:3711 2021-02-12T23:54:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.21 , 0.05 2021-02-12T23:54:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174064.07 epoch sec; seqNo:7921; 4058135.88 north; 591707.91 east 2021-02-12T23:54:24Z- AHRS_M2 msg: 1613174064.47 epoch sec; seqNo:7857 2021-02-12T23:54:24Z- AHRS_M2 msg: 0.57 phi; 0.30 theta; -0.18 psi 2021-02-12T23:54:24Z- handleDepth()-Depth_Keller msg: 1613174064473 epoch sec; seqNo:7922; depth 19.81 2021-02-12T23:54:24Z- handleDvl() - RDI_Pathfinder msg: 1613174064.49 epoch sec; seqNo:3712 2021-02-12T23:54:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 100.48 , 69.89 2021-02-12T23:54:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.17 , 0.17 2021-02-12T23:54:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174064.50 epoch sec; seqNo:7922; 4058136.26 north; 591707.90 east 2021-02-12T23:54:24Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-12T23:54:24Z- waiting for 1613174062.63, time is 1613174064.50 2021-02-12T23:54:24Z- LcmTrn::updateTrn() equating pose 1613174064.50 and meas times 1613174064.50 2021-02-12T23:54:24Z- LcmTrn::updateTrn() >>>> pose time 1613174064.50, meas time 1613174064.50 <<<< 2021-02-12T23:54:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174064.50 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:54:24Z- TerrainNav::motionUpdate() - currEstimate = (4058136, 591707.90, 19.81) 2021-02-12T23:54:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:54:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:54:24Z- TNavBF::Projecting Measurements 2021-02-12T23:54:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:54:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:54:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:54:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:54:24Z- TNavFilter::4 good beams. 2021-02-12T23:54:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:54:24Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:54:24Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:54:24Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:54:24Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:54:24Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:54:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:54:24Z- TNavBF::mmse selected from filter -1 2021-02-12T23:54:24Z- TerrainMapOctree: (vehN, vehE ) = (4058136.26, 591707.90). 2021-02-12T23:54:24Z- TerrainMapOctree: Min Distance = 4019.65. 2021-02-12T23:54:24Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:54:24Z- There is no specific good beam that all particles have in common. 2021-02-12T23:54:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:54:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-12T23:54:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:54:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 15.38 2021-02-12T23:54:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:54:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 16.15 2021-02-12T23:54:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:54:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 7.81 2021-02-12T23:54:24Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:54:24Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:54:24Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:54:24Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:54:24Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:54:24Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:54:24Z- Filter 0 No beams used in subcloud update 2021-02-12T23:54:24Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:54:24Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:54:24Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 729.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:54:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174064.50 sec. 2021-02-12T23:54:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:54:24Z- TNavBF::mmse selected from filter -1 2021-02-12T23:54:24Z- variances([0], [2], and psi_berg) = 729.427853 833.016149 0.000000 2021-02-12T23:54:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:54:24Z- LcmTrn::publishEstimates() - MLE : 4058129.76 591660.40 19.81 2021-02-12T23:54:24Z- LcmTrn::publishEstimates() - MMSE : 4058136.26 591707.90 19.81 2021-02-12T23:54:24Z- LcmTrn::publishEstimates() - COVAR: 729.43 833.02 0.94 0.00 2021-02-12T23:54:24Z- AHRS_M2 msg: 1613174064.85 epoch sec; seqNo:7858 2021-02-12T23:54:24Z- AHRS_M2 msg: 0.57 phi; 0.30 theta; -0.18 psi 2021-02-12T23:54:24Z- handleDepth()-Depth_Keller msg: 1613174064862 epoch sec; seqNo:7923; depth 19.70 2021-02-12T23:54:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174064.87 epoch sec; seqNo:7923; 4058136.64 north; 591707.90 east 2021-02-12T23:54:25Z- AHRS_M2 msg: 1613174065.26 epoch sec; seqNo:7859 2021-02-12T23:54:25Z- AHRS_M2 msg: 0.56 phi; 0.31 theta; -0.18 psi 2021-02-12T23:54:25Z- handleDepth()-Depth_Keller msg: 1613174065265 epoch sec; seqNo:7924; depth 19.61 2021-02-12T23:54:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:25Z- handleDvl() - RDI_Pathfinder msg: 1613174065.27 epoch sec; seqNo:3713 2021-02-12T23:54:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.17 , 0.17 2021-02-12T23:54:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174065.29 epoch sec; seqNo:7924; 4058137.03 north; 591709.12 east 2021-02-12T23:54:25Z- AHRS_M2 msg: 1613174065.66 epoch sec; seqNo:7860 2021-02-12T23:54:25Z- AHRS_M2 msg: 0.56 phi; 0.31 theta; -0.17 psi 2021-02-12T23:54:25Z- handleDepth()-Depth_Keller msg: 1613174065669 epoch sec; seqNo:7925; depth 19.51 2021-02-12T23:54:25Z- handleDvl() - RDI_Pathfinder msg: 1613174065.68 epoch sec; seqNo:3714 2021-02-12T23:54:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 93.18 , 101.70 , 70.29 2021-02-12T23:54:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , 0.21 2021-02-12T23:54:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174065.71 epoch sec; seqNo:7925; 4058137.03 north; 591709.12 east 2021-02-12T23:54:26Z- AHRS_M2 msg: 1613174066.06 epoch sec; seqNo:7861 2021-02-12T23:54:26Z- AHRS_M2 msg: 0.56 phi; 0.32 theta; -0.17 psi 2021-02-12T23:54:26Z- handleDepth()-Depth_Keller msg: 1613174066073 epoch sec; seqNo:7926; depth 19.41 2021-02-12T23:54:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174066.08 epoch sec; seqNo:7926; 4058137.41 north; 591709.11 east 2021-02-12T23:54:26Z- AHRS_M2 msg: 1613174066.47 epoch sec; seqNo:7862 2021-02-12T23:54:26Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.16 psi 2021-02-12T23:54:26Z- handleDepth()-Depth_Keller msg: 1613174066481 epoch sec; seqNo:7927; depth 19.31 2021-02-12T23:54:26Z- handleDvl() - RDI_Pathfinder msg: 1613174066.49 epoch sec; seqNo:3715 2021-02-12T23:54:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 93.25 , 102.98 , 71.11 2021-02-12T23:54:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.07 , 0.13 2021-02-12T23:54:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174066.50 epoch sec; seqNo:7927; 4058137.79 north; 591709.11 east 2021-02-12T23:54:26Z- AHRS_M2 msg: 1613174066.87 epoch sec; seqNo:7863 2021-02-12T23:54:26Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.15 psi 2021-02-12T23:54:26Z- handleDepth()-Depth_Keller msg: 1613174066876 epoch sec; seqNo:7928; depth 19.20 2021-02-12T23:54:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174066.89 epoch sec; seqNo:7928; 4058138.17 north; 591709.11 east 2021-02-12T23:54:27Z- AHRS_M2 msg: 1613174067.28 epoch sec; seqNo:7864 2021-02-12T23:54:27Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-12T23:54:27Z- handleDepth()-Depth_Keller msg: 1613174067280 epoch sec; seqNo:7929; depth 19.10 2021-02-12T23:54:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174067.29 epoch sec; seqNo:7929; 4058138.17 north; 591709.11 east 2021-02-12T23:54:27Z- AHRS_M2 msg: 1613174067.68 epoch sec; seqNo:7865 2021-02-12T23:54:27Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.14 psi 2021-02-12T23:54:27Z- handleDepth()-Depth_Keller msg: 1613174067685 epoch sec; seqNo:7930; depth 18.98 2021-02-12T23:54:27Z- handleDvl() - RDI_Pathfinder msg: 1613174067.70 epoch sec; seqNo:3716 2021-02-12T23:54:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.86 , 98.12 , 104.20 , 70.70 2021-02-12T23:54:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.10 , -0.02 2021-02-12T23:54:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174067.71 epoch sec; seqNo:7930; 4058138.56 north; 591710.32 east 2021-02-12T23:54:28Z- AHRS_M2 msg: 1613174068.08 epoch sec; seqNo:7866 2021-02-12T23:54:28Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.14 psi 2021-02-12T23:54:28Z- handleDepth()-Depth_Keller msg: 1613174068087 epoch sec; seqNo:7931; depth 18.88 2021-02-12T23:54:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174068.11 epoch sec; seqNo:7931; 4058138.94 north; 591710.32 east 2021-02-12T23:54:28Z- AHRS_M2 msg: 1613174068.49 epoch sec; seqNo:7867 2021-02-12T23:54:28Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.14 psi 2021-02-12T23:54:28Z- handleDepth()-Depth_Keller msg: 1613174068492 epoch sec; seqNo:7932; depth 18.78 2021-02-12T23:54:28Z- handleDvl() - RDI_Pathfinder msg: 1613174068.51 epoch sec; seqNo:3717 2021-02-12T23:54:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.88 , 97.91 , 104.00 , 71.11 2021-02-12T23:54:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , -0.13 2021-02-12T23:54:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174068.53 epoch sec; seqNo:7932; 4058138.94 north; 591710.32 east 2021-02-12T23:54:28Z- AHRS_M2 msg: 1613174068.89 epoch sec; seqNo:7868 2021-02-12T23:54:28Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.14 psi 2021-02-12T23:54:28Z- handleDepth()-Depth_Keller msg: 1613174068901 epoch sec; seqNo:7933; depth 18.66 2021-02-12T23:54:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:28Z- handleDvl() - RDI_Pathfinder msg: 1613174068.90 epoch sec; seqNo:3718 2021-02-12T23:54:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , -0.13 2021-02-12T23:54:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174068.91 epoch sec; seqNo:7933; 4058139.32 north; 591710.31 east 2021-02-12T23:54:29Z- AHRS_M2 msg: 1613174069.30 epoch sec; seqNo:7869 2021-02-12T23:54:29Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-12T23:54:29Z- handleDepth()-Depth_Keller msg: 1613174069302 epoch sec; seqNo:7934; depth 18.56 2021-02-12T23:54:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174069.31 epoch sec; seqNo:7934; 4058139.70 north; 591710.31 east 2021-02-12T23:54:29Z- AHRS_M2 msg: 1613174069.70 epoch sec; seqNo:7870 2021-02-12T23:54:29Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:54:29Z- handleDepth()-Depth_Keller msg: 1613174069704 epoch sec; seqNo:7935; depth 18.46 2021-02-12T23:54:29Z- handleDvl() - RDI_Pathfinder msg: 1613174069.72 epoch sec; seqNo:3719 2021-02-12T23:54:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.88 , 94.26 , 104.00 , 72.75 2021-02-12T23:54:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.16 , -0.10 2021-02-12T23:54:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174069.73 epoch sec; seqNo:7935; 4058140.08 north; 591710.31 east 2021-02-12T23:54:29Z- LcmTrn::handleMessages() - handling 46 messages took 0.64 ms and 133 calls... 2021-02-12T23:54:29Z- waiting for 1613174067.50, time is 1613174069.73 2021-02-12T23:54:29Z- LcmTrn::updateTrn() equating pose 1613174069.73 and meas times 1613174069.73 2021-02-12T23:54:29Z- LcmTrn::updateTrn() >>>> pose time 1613174069.73, meas time 1613174069.73 <<<< 2021-02-12T23:54:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174069.73 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:54:29Z- TerrainNav::motionUpdate() - currEstimate = (4058140, 591710.31, 18.46) 2021-02-12T23:54:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:54:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:54:29Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174069.73 sec.; No relevant navigation data available 2021-02-12T23:54:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:54:29Z- TNavBF::mmse selected from filter -1 2021-02-12T23:54:29Z- variances([0], [2], and psi_berg) = 729.347968 833.016091 0.000000 2021-02-12T23:54:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:54:29Z- LcmTrn::publishEstimates() - MLE : 4058133.58 591662.81 18.46 2021-02-12T23:54:29Z- LcmTrn::publishEstimates() - MMSE : 4058140.08 591710.31 18.46 2021-02-12T23:54:29Z- LcmTrn::publishEstimates() - COVAR: 729.35 833.02 1.16 0.00 2021-02-12T23:54:30Z- AHRS_M2 msg: 1613174070.11 epoch sec; seqNo:7871 2021-02-12T23:54:30Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-12T23:54:30Z- handleDepth()-Depth_Keller msg: 1613174070117 epoch sec; seqNo:7936; depth 18.34 2021-02-12T23:54:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:30Z- handleDvl() - RDI_Pathfinder msg: 1613174070.12 epoch sec; seqNo:3720 2021-02-12T23:54:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.16 , -0.10 2021-02-12T23:54:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174070.14 epoch sec; seqNo:7936; 4058140.08 north; 591710.31 east 2021-02-12T23:54:30Z- AHRS_M2 msg: 1613174070.51 epoch sec; seqNo:7872 2021-02-12T23:54:30Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-12T23:54:30Z- handleDepth()-Depth_Keller msg: 1613174070517 epoch sec; seqNo:7937; depth 18.22 2021-02-12T23:54:30Z- handleDvl() - RDI_Pathfinder msg: 1613174070.53 epoch sec; seqNo:3721 2021-02-12T23:54:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 104.00 , 71.52 2021-02-12T23:54:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.22 , 0.04 2021-02-12T23:54:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174070.54 epoch sec; seqNo:7937; 4058140.46 north; 591710.30 east 2021-02-12T23:54:30Z- AHRS_M2 msg: 1613174070.91 epoch sec; seqNo:7873 2021-02-12T23:54:30Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-12T23:54:30Z- handleDepth()-Depth_Keller msg: 1613174070918 epoch sec; seqNo:7938; depth 18.12 2021-02-12T23:54:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174070.93 epoch sec; seqNo:7938; 4058140.85 north; 591711.52 east 2021-02-12T23:54:31Z- AHRS_M2 msg: 1613174071.32 epoch sec; seqNo:7874 2021-02-12T23:54:31Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.17 psi 2021-02-12T23:54:31Z- handleDepth()-Depth_Keller msg: 1613174071320 epoch sec; seqNo:7939; depth 18.02 2021-02-12T23:54:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:31Z- handleDvl() - RDI_Pathfinder msg: 1613174071.32 epoch sec; seqNo:3722 2021-02-12T23:54:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.22 , 0.04 2021-02-12T23:54:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174071.34 epoch sec; seqNo:7939; 4058141.23 north; 591711.51 east 2021-02-12T23:54:31Z- AHRS_M2 msg: 1613174071.72 epoch sec; seqNo:7875 2021-02-12T23:54:31Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.17 psi 2021-02-12T23:54:31Z- handleDepth()-Depth_Keller msg: 1613174071724 epoch sec; seqNo:7940; depth 17.91 2021-02-12T23:54:31Z- handleDvl() - RDI_Pathfinder msg: 1613174071.73 epoch sec; seqNo:3723 2021-02-12T23:54:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.66 , 97.91 , 102.78 , 71.11 2021-02-12T23:54:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.14 , 0.15 2021-02-12T23:54:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174071.76 epoch sec; seqNo:7940; 4058141.61 north; 591711.51 east 2021-02-12T23:54:32Z- AHRS_M2 msg: 1613174072.13 epoch sec; seqNo:7876 2021-02-12T23:54:32Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-12T23:54:32Z- handleDepth()-Depth_Keller msg: 1613174072131 epoch sec; seqNo:7941; depth 17.80 2021-02-12T23:54:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174072.14 epoch sec; seqNo:7941; 4058141.98 north; 591711.51 east 2021-02-12T23:54:32Z- AHRS_M2 msg: 1613174072.53 epoch sec; seqNo:7877 2021-02-12T23:54:32Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-12T23:54:32Z- handleDepth()-Depth_Keller msg: 1613174072533 epoch sec; seqNo:7942; depth 17.70 2021-02-12T23:54:32Z- handleDvl() - RDI_Pathfinder msg: 1613174072.55 epoch sec; seqNo:3724 2021-02-12T23:54:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.91 , 106.43 , 72.75 2021-02-12T23:54:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.14 2021-02-12T23:54:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174072.56 epoch sec; seqNo:7942; 4058141.98 north; 591711.51 east 2021-02-12T23:54:32Z- AHRS_M2 msg: 1613174072.93 epoch sec; seqNo:7878 2021-02-12T23:54:32Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.16 psi 2021-02-12T23:54:32Z- handleDepth()-Depth_Keller msg: 1613174072941 epoch sec; seqNo:7943; depth 17.58 2021-02-12T23:54:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174072.96 epoch sec; seqNo:7943; 4058142.36 north; 591711.50 east 2021-02-12T23:54:33Z- AHRS_M2 msg: 1613174073.34 epoch sec; seqNo:7879 2021-02-12T23:54:33Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-12T23:54:33Z- handleDepth()-Depth_Keller msg: 1613174073340 epoch sec; seqNo:7944; depth 17.47 2021-02-12T23:54:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174073.35 epoch sec; seqNo:7944; 4058142.74 north; 591711.50 east 2021-02-12T23:54:33Z- AHRS_M2 msg: 1613174073.74 epoch sec; seqNo:7880 2021-02-12T23:54:33Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.14 psi 2021-02-12T23:54:33Z- handleDepth()-Depth_Keller msg: 1613174073749 epoch sec; seqNo:7945; depth 17.37 2021-02-12T23:54:33Z- handleDvl() - RDI_Pathfinder msg: 1613174073.76 epoch sec; seqNo:3725 2021-02-12T23:54:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.48 , 97.71 , 107.18 , 73.15 2021-02-12T23:54:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.09 , 0.04 2021-02-12T23:54:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174073.77 epoch sec; seqNo:7945; 4058143.13 north; 591712.72 east 2021-02-12T23:54:34Z- AHRS_M2 msg: 1613174074.15 epoch sec; seqNo:7881 2021-02-12T23:54:34Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.12 psi 2021-02-12T23:54:34Z- handleDepth()-Depth_Keller msg: 1613174074150 epoch sec; seqNo:7946; depth 17.27 2021-02-12T23:54:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174074.17 epoch sec; seqNo:7946; 4058143.13 north; 591712.72 east 2021-02-12T23:54:34Z- AHRS_M2 msg: 1613174074.55 epoch sec; seqNo:7882 2021-02-12T23:54:34Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.12 psi 2021-02-12T23:54:34Z- handleDepth()-Depth_Keller msg: 1613174074557 epoch sec; seqNo:7947; depth 17.15 2021-02-12T23:54:34Z- handleDvl() - RDI_Pathfinder msg: 1613174074.58 epoch sec; seqNo:3726 2021-02-12T23:54:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 94.13 , 108.73 , 71.52 2021-02-12T23:54:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.06 , -0.10 2021-02-12T23:54:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174074.59 epoch sec; seqNo:7947; 4058143.51 north; 591712.71 east 2021-02-12T23:54:34Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-12T23:54:34Z- waiting for 1613174072.73, time is 1613174074.59 2021-02-12T23:54:34Z- LcmTrn::updateTrn() equating pose 1613174074.59 and meas times 1613174074.59 2021-02-12T23:54:34Z- LcmTrn::updateTrn() >>>> pose time 1613174074.59, meas time 1613174074.59 <<<< 2021-02-12T23:54:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174074.59 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:54:34Z- TerrainNav::motionUpdate() - currEstimate = (4058144, 591712.71, 17.15) 2021-02-12T23:54:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:54:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:54:34Z- TNavBF::Projecting Measurements 2021-02-12T23:54:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:54:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:54:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:54:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:54:34Z- TNavFilter::4 good beams. 2021-02-12T23:54:34Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:54:34Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:54:34Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:54:34Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:54:34Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:54:34Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:54:34Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:54:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:54:34Z- TerrainMapOctree: (vehN, vehE ) = (4058143.51, 591712.71). 2021-02-12T23:54:34Z- TerrainMapOctree: Min Distance = 4011.00. 2021-02-12T23:54:34Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:54:34Z- There is no specific good beam that all particles have in common. 2021-02-12T23:54:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:54:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 10.75 2021-02-12T23:54:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:54:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 14.18 2021-02-12T23:54:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:54:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 18.92 2021-02-12T23:54:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:54:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 8.18 2021-02-12T23:54:34Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:54:34Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:54:34Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:54:34Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:54:34Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:54:34Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:54:34Z- Filter 0 No beams used in subcloud update 2021-02-12T23:54:34Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:54:34Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:54:34Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 729.3 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:54:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174074.59 sec. 2021-02-12T23:54:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:54:34Z- TNavBF::mmse selected from filter -1 2021-02-12T23:54:34Z- variances([0], [2], and psi_berg) = 729.270983 833.016036 0.000000 2021-02-12T23:54:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:54:34Z- LcmTrn::publishEstimates() - MLE : 4058137.01 591665.21 17.15 2021-02-12T23:54:34Z- LcmTrn::publishEstimates() - MMSE : 4058143.51 591712.71 17.15 2021-02-12T23:54:34Z- LcmTrn::publishEstimates() - COVAR: 729.27 833.02 0.98 0.00 2021-02-12T23:54:34Z- AHRS_M2 msg: 1613174074.95 epoch sec; seqNo:7883 2021-02-12T23:54:34Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.12 psi 2021-02-12T23:54:34Z- handleDepth()-Depth_Keller msg: 1613174074959 epoch sec; seqNo:7948; depth 17.05 2021-02-12T23:54:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:34Z- handleDvl() - RDI_Pathfinder msg: 1613174074.96 epoch sec; seqNo:3727 2021-02-12T23:54:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.06 , -0.10 2021-02-12T23:54:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174074.97 epoch sec; seqNo:7948; 4058143.89 north; 591712.71 east 2021-02-12T23:54:35Z- AHRS_M2 msg: 1613174075.36 epoch sec; seqNo:7884 2021-02-12T23:54:35Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.11 psi 2021-02-12T23:54:35Z- handleDepth()-Depth_Keller msg: 1613174075363 epoch sec; seqNo:7949; depth 16.94 2021-02-12T23:54:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174075.37 epoch sec; seqNo:7949; 4058143.89 north; 591712.71 east 2021-02-12T23:54:35Z- AHRS_M2 msg: 1613174075.76 epoch sec; seqNo:7885 2021-02-12T23:54:35Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.11 psi 2021-02-12T23:54:35Z- handleDepth()-Depth_Keller msg: 1613174075765 epoch sec; seqNo:7950; depth 16.83 2021-02-12T23:54:35Z- handleDvl() - RDI_Pathfinder msg: 1613174075.79 epoch sec; seqNo:3728 2021-02-12T23:54:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.24 , 97.85 , 107.58 , 71.93 2021-02-12T23:54:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , -0.15 2021-02-12T23:54:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174075.80 epoch sec; seqNo:7950; 4058144.27 north; 591712.70 east 2021-02-12T23:54:36Z- AHRS_M2 msg: 1613174076.16 epoch sec; seqNo:7886 2021-02-12T23:54:36Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.12 psi 2021-02-12T23:54:36Z- handleDepth()-Depth_Keller msg: 1613174076168 epoch sec; seqNo:7951; depth 16.71 2021-02-12T23:54:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:36Z- handleDvl() - RDI_Pathfinder msg: 1613174076.17 epoch sec; seqNo:3729 2021-02-12T23:54:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , -0.15 2021-02-12T23:54:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174076.18 epoch sec; seqNo:7951; 4058144.66 north; 591713.92 east 2021-02-12T23:54:36Z- AHRS_M2 msg: 1613174076.57 epoch sec; seqNo:7887 2021-02-12T23:54:36Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.12 psi 2021-02-12T23:54:36Z- handleDepth()-Depth_Keller msg: 1613174076577 epoch sec; seqNo:7952; depth 16.61 2021-02-12T23:54:36Z- handleDvl() - RDI_Pathfinder msg: 1613174076.59 epoch sec; seqNo:3730 2021-02-12T23:54:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.18 , 92.91 , 108.73 , 71.52 2021-02-12T23:54:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.13 , -0.07 2021-02-12T23:54:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174076.60 epoch sec; seqNo:7952; 4058144.66 north; 591713.92 east 2021-02-12T23:54:36Z- AHRS_M2 msg: 1613174076.97 epoch sec; seqNo:7888 2021-02-12T23:54:36Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.12 psi 2021-02-12T23:54:36Z- handleDepth()-Depth_Keller msg: 1613174076976 epoch sec; seqNo:7953; depth 16.48 2021-02-12T23:54:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174076.99 epoch sec; seqNo:7953; 4058145.04 north; 591713.92 east 2021-02-12T23:54:37Z- AHRS_M2 msg: 1613174077.38 epoch sec; seqNo:7889 2021-02-12T23:54:37Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.12 psi 2021-02-12T23:54:37Z- handleDepth()-Depth_Keller msg: 1613174077385 epoch sec; seqNo:7954; depth 16.38 2021-02-12T23:54:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:37Z- handleDvl() - RDI_Pathfinder msg: 1613174077.39 epoch sec; seqNo:3731 2021-02-12T23:54:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.13 , -0.07 2021-02-12T23:54:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174077.41 epoch sec; seqNo:7954; 4058145.42 north; 591713.91 east 2021-02-12T23:54:37Z- AHRS_M2 msg: 1613174077.78 epoch sec; seqNo:7890 2021-02-12T23:54:37Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.13 psi 2021-02-12T23:54:37Z- handleDepth()-Depth_Keller msg: 1613174077784 epoch sec; seqNo:7955; depth 16.26 2021-02-12T23:54:37Z- handleDvl() - RDI_Pathfinder msg: 1613174077.80 epoch sec; seqNo:3732 2021-02-12T23:54:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.24 , 97.85 , 112.45 , 73.15 2021-02-12T23:54:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.14 , 0.04 2021-02-12T23:54:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174077.81 epoch sec; seqNo:7955; 4058145.80 north; 591713.91 east 2021-02-12T23:54:38Z- AHRS_M2 msg: 1613174078.18 epoch sec; seqNo:7891 2021-02-12T23:54:38Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.13 psi 2021-02-12T23:54:38Z- handleDepth()-Depth_Keller msg: 1613174078188 epoch sec; seqNo:7956; depth 16.13 2021-02-12T23:54:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174078.20 epoch sec; seqNo:7956; 4058145.80 north; 591713.91 east 2021-02-12T23:54:38Z- AHRS_M2 msg: 1613174078.59 epoch sec; seqNo:7892 2021-02-12T23:54:38Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.13 psi 2021-02-12T23:54:38Z- handleDepth()-Depth_Keller msg: 1613174078594 epoch sec; seqNo:7957; depth 16.02 2021-02-12T23:54:38Z- handleDvl() - RDI_Pathfinder msg: 1613174078.61 epoch sec; seqNo:3733 2021-02-12T23:54:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.26 , 97.64 , 111.03 , 71.93 2021-02-12T23:54:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.09 , 0.09 2021-02-12T23:54:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174078.62 epoch sec; seqNo:7957; 4058146.18 north; 591713.90 east 2021-02-12T23:54:38Z- AHRS_M2 msg: 1613174078.99 epoch sec; seqNo:7893 2021-02-12T23:54:38Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.14 psi 2021-02-12T23:54:39Z- handleDepth()-Depth_Keller msg: 1613174079001 epoch sec; seqNo:7958; depth 15.90 2021-02-12T23:54:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174079.03 epoch sec; seqNo:7958; 4058146.57 north; 591715.12 east 2021-02-12T23:54:39Z- AHRS_M2 msg: 1613174079.40 epoch sec; seqNo:7894 2021-02-12T23:54:39Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-12T23:54:39Z- handleDepth()-Depth_Keller msg: 1613174079407 epoch sec; seqNo:7959; depth 15.80 2021-02-12T23:54:39Z- handleDvl() - RDI_Pathfinder msg: 1613174079.43 epoch sec; seqNo:3734 2021-02-12T23:54:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.64 , 109.81 , 72.34 2021-02-12T23:54:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.01 , 0.09 2021-02-12T23:54:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174079.45 epoch sec; seqNo:7959; 4058146.95 north; 591715.12 east 2021-02-12T23:54:39Z- AHRS_M2 msg: 1613174079.80 epoch sec; seqNo:7895 2021-02-12T23:54:39Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-12T23:54:39Z- handleDepth()-Depth_Keller msg: 1613174079805 epoch sec; seqNo:7960; depth 15.66 2021-02-12T23:54:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:39Z- handleDvl() - RDI_Pathfinder msg: 1613174079.81 epoch sec; seqNo:3735 2021-02-12T23:54:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.01 , 0.09 2021-02-12T23:54:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174079.82 epoch sec; seqNo:7960; 4058146.95 north; 591715.12 east 2021-02-12T23:54:39Z- LcmTrn::handleMessages() - handling 48 messages took 0.69 ms and 134 calls... 2021-02-12T23:54:39Z- waiting for 1613174077.59, time is 1613174079.82 2021-02-12T23:54:39Z- LcmTrn::updateTrn() equating pose 1613174079.82 and meas times 1613174079.82 2021-02-12T23:54:39Z- LcmTrn::updateTrn() >>>> pose time 1613174079.82, meas time 1613174079.82 <<<< 2021-02-12T23:54:39Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174079.82, ping # 0. 2021-02-12T23:54:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174079.82 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:54:39Z- TerrainNav::motionUpdate() - currEstimate = (4058147, 591715.12, 15.66) 2021-02-12T23:54:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:54:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:54:39Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:54:39Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:54:39Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174079.82 sec.; No relevant navigation data available 2021-02-12T23:54:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:54:39Z- TNavBF::mmse selected from filter -1 2021-02-12T23:54:39Z- variances([0], [2], and psi_berg) = 729.194069 833.015980 0.000000 2021-02-12T23:54:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:54:39Z- LcmTrn::publishEstimates() - MLE : 4058140.45 591665.62 15.66 2021-02-12T23:54:39Z- LcmTrn::publishEstimates() - MMSE : 4058146.95 591715.12 15.66 2021-02-12T23:54:39Z- LcmTrn::publishEstimates() - COVAR: 729.19 833.02 1.20 0.00 2021-02-12T23:54:40Z- AHRS_M2 msg: 1613174080.20 epoch sec; seqNo:7896 2021-02-12T23:54:40Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-12T23:54:40Z- handleDepth()-Depth_Keller msg: 1613174080208 epoch sec; seqNo:7961; depth 15.55 2021-02-12T23:54:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174080.23 epoch sec; seqNo:7961; 4058147.33 north; 591715.11 east 2021-02-12T23:54:40Z- AHRS_M2 msg: 1613174080.61 epoch sec; seqNo:7897 2021-02-12T23:54:40Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.15 psi 2021-02-12T23:54:40Z- handleDepth()-Depth_Keller msg: 1613174080612 epoch sec; seqNo:7962; depth 15.44 2021-02-12T23:54:40Z- handleDvl() - RDI_Pathfinder msg: 1613174080.62 epoch sec; seqNo:3736 2021-02-12T23:54:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 97.64 , 109.81 , 73.15 2021-02-12T23:54:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.03 , 0.04 2021-02-12T23:54:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174080.64 epoch sec; seqNo:7962; 4058147.71 north; 591715.11 east 2021-02-12T23:54:41Z- AHRS_M2 msg: 1613174081.01 epoch sec; seqNo:7898 2021-02-12T23:54:41Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.15 psi 2021-02-12T23:54:41Z- handleDepth()-Depth_Keller msg: 1613174081019 epoch sec; seqNo:7963; depth 15.30 2021-02-12T23:54:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:41Z- handleDvl() - RDI_Pathfinder msg: 1613174081.02 epoch sec; seqNo:3737 2021-02-12T23:54:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.03 , 0.04 2021-02-12T23:54:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174081.03 epoch sec; seqNo:7963; 4058147.71 north; 591715.11 east 2021-02-12T23:54:41Z- AHRS_M2 msg: 1613174081.42 epoch sec; seqNo:7899 2021-02-12T23:54:41Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.15 psi 2021-02-12T23:54:41Z- handleDepth()-Depth_Keller msg: 1613174081422 epoch sec; seqNo:7964; depth 15.19 2021-02-12T23:54:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174081.43 epoch sec; seqNo:7964; 4058148.10 north; 591716.33 east 2021-02-12T23:54:41Z- AHRS_M2 msg: 1613174081.82 epoch sec; seqNo:7900 2021-02-12T23:54:41Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.15 psi 2021-02-12T23:54:41Z- handleDepth()-Depth_Keller msg: 1613174081824 epoch sec; seqNo:7965; depth 15.07 2021-02-12T23:54:41Z- handleDvl() - RDI_Pathfinder msg: 1613174081.85 epoch sec; seqNo:3738 2021-02-12T23:54:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.26 , 98.86 , 113.46 , 78.17 2021-02-12T23:54:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.03 , -0.01 2021-02-12T23:54:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174081.86 epoch sec; seqNo:7965; 4058148.10 north; 591716.33 east 2021-02-12T23:54:42Z- AHRS_M2 msg: 1613174082.22 epoch sec; seqNo:7901 2021-02-12T23:54:42Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.15 psi 2021-02-12T23:54:42Z- handleDepth()-Depth_Keller msg: 1613174082233 epoch sec; seqNo:7966; depth 14.94 2021-02-12T23:54:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:42Z- handleDvl() - RDI_Pathfinder msg: 1613174082.23 epoch sec; seqNo:3739 2021-02-12T23:54:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.03 , -0.01 2021-02-12T23:54:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174082.26 epoch sec; seqNo:7966; 4058148.48 north; 591716.32 east 2021-02-12T23:54:42Z- AHRS_M2 msg: 1613174082.63 epoch sec; seqNo:7902 2021-02-12T23:54:42Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.16 psi 2021-02-12T23:54:42Z- handleDepth()-Depth_Keller msg: 1613174082632 epoch sec; seqNo:7967; depth 14.82 2021-02-12T23:54:42Z- handleDvl() - RDI_Pathfinder msg: 1613174082.64 epoch sec; seqNo:3740 2021-02-12T23:54:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.70 , 98.93 , 113.80 , 73.15 2021-02-12T23:54:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.01 2021-02-12T23:54:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174082.65 epoch sec; seqNo:7967; 4058148.86 north; 591716.32 east 2021-02-12T23:54:43Z- AHRS_M2 msg: 1613174083.03 epoch sec; seqNo:7903 2021-02-12T23:54:43Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.16 psi 2021-02-12T23:54:43Z- handleDepth()-Depth_Keller msg: 1613174083038 epoch sec; seqNo:7968; depth 14.70 2021-02-12T23:54:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174083.05 epoch sec; seqNo:7968; 4058148.86 north; 591716.32 east 2021-02-12T23:54:43Z- AHRS_M2 msg: 1613174083.44 epoch sec; seqNo:7904 2021-02-12T23:54:43Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.16 psi 2021-02-12T23:54:43Z- handleDepth()-Depth_Keller msg: 1613174083440 epoch sec; seqNo:7969; depth 14.57 2021-02-12T23:54:43Z- handleDvl() - RDI_Pathfinder msg: 1613174083.46 epoch sec; seqNo:3741 2021-02-12T23:54:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.42 , 117.99 , 73.56 2021-02-12T23:54:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.09 , 0.07 2021-02-12T23:54:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174083.47 epoch sec; seqNo:7969; 4058149.23 north; 591716.31 east 2021-02-12T23:54:43Z- AHRS_M2 msg: 1613174083.84 epoch sec; seqNo:7905 2021-02-12T23:54:43Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.17 psi 2021-02-12T23:54:43Z- handleDepth()-Depth_Keller msg: 1613174083844 epoch sec; seqNo:7970; depth 14.43 2021-02-12T23:54:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174083.86 epoch sec; seqNo:7970; 4058149.61 north; 591716.31 east 2021-02-12T23:54:44Z- AHRS_M2 msg: 1613174084.24 epoch sec; seqNo:7906 2021-02-12T23:54:44Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.17 psi 2021-02-12T23:54:44Z- handleDepth()-Depth_Keller msg: 1613174084248 epoch sec; seqNo:7971; depth 14.31 2021-02-12T23:54:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174084.26 epoch sec; seqNo:7971; 4058150.01 north; 591717.53 east 2021-02-12T23:54:44Z- AHRS_M2 msg: 1613174084.65 epoch sec; seqNo:7907 2021-02-12T23:54:44Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.18 psi 2021-02-12T23:54:44Z- handleDepth()-Depth_Keller msg: 1613174084654 epoch sec; seqNo:7972; depth 14.19 2021-02-12T23:54:44Z- handleDvl() - RDI_Pathfinder msg: 1613174084.67 epoch sec; seqNo:3742 2021-02-12T23:54:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.71 , 101.29 , 114.82 , 74.79 2021-02-12T23:54:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.09 2021-02-12T23:54:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174084.68 epoch sec; seqNo:7972; 4058150.01 north; 591717.53 east 2021-02-12T23:54:44Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-12T23:54:44Z- waiting for 1613174082.82, time is 1613174084.68 2021-02-12T23:54:44Z- LcmTrn::updateTrn() equating pose 1613174084.68 and meas times 1613174084.68 2021-02-12T23:54:44Z- LcmTrn::updateTrn() >>>> pose time 1613174084.68, meas time 1613174084.68 <<<< 2021-02-12T23:54:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174084.68 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:54:44Z- TerrainNav::motionUpdate() - currEstimate = (4058150, 591717.53, 14.19) 2021-02-12T23:54:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:54:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:54:44Z- TNavBF::Projecting Measurements 2021-02-12T23:54:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:54:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:54:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:54:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:54:44Z- TNavFilter::4 good beams. 2021-02-12T23:54:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:54:44Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:54:44Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:54:44Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:54:44Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:54:44Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:54:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:54:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:54:44Z- TerrainMapOctree: (vehN, vehE ) = (4058150.01, 591717.53). 2021-02-12T23:54:44Z- TerrainMapOctree: Min Distance = 4002.92. 2021-02-12T23:54:44Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:54:45Z- There is no specific good beam that all particles have in common. 2021-02-12T23:54:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:54:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.39 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 11.21 2021-02-12T23:54:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:54:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 16.42 2021-02-12T23:54:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:54:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 21.09 2021-02-12T23:54:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:54:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 8.95 2021-02-12T23:54:45Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:54:45Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:54:45Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:54:45Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:54:45Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:54:45Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:54:45Z- Filter 0 No beams used in subcloud update 2021-02-12T23:54:45Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:54:45Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:54:45Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 729.1 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:54:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174084.68 sec. 2021-02-12T23:54:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:54:45Z- TNavBF::mmse selected from filter -1 2021-02-12T23:54:45Z- variances([0], [2], and psi_berg) = 729.120041 833.015926 0.000000 2021-02-12T23:54:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:54:45Z- LcmTrn::publishEstimates() - MLE : 4058143.51 591669.03 14.19 2021-02-12T23:54:45Z- LcmTrn::publishEstimates() - MMSE : 4058150.01 591717.53 14.19 2021-02-12T23:54:45Z- LcmTrn::publishEstimates() - COVAR: 729.12 833.02 1.01 0.00 2021-02-12T23:54:45Z- AHRS_M2 msg: 1613174085.05 epoch sec; seqNo:7908 2021-02-12T23:54:45Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.18 psi 2021-02-12T23:54:45Z- handleDepth()-Depth_Keller msg: 1613174085060 epoch sec; seqNo:7973; depth 14.05 2021-02-12T23:54:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174085.10 epoch sec; seqNo:7973; 4058150.38 north; 591717.52 east 2021-02-12T23:54:45Z- AHRS_M2 msg: 1613174085.46 epoch sec; seqNo:7909 2021-02-12T23:54:45Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.18 psi 2021-02-12T23:54:45Z- handleDepth()-Depth_Keller msg: 1613174085460 epoch sec; seqNo:7974; depth 13.92 2021-02-12T23:54:45Z- handleDvl() - RDI_Pathfinder msg: 1613174085.47 epoch sec; seqNo:3743 2021-02-12T23:54:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.05 , 101.63 , 115.15 , 74.79 2021-02-12T23:54:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , 0.05 2021-02-12T23:54:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174085.48 epoch sec; seqNo:7974; 4058150.76 north; 591717.52 east 2021-02-12T23:54:45Z- AHRS_M2 msg: 1613174085.86 epoch sec; seqNo:7910 2021-02-12T23:54:45Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.18 psi 2021-02-12T23:54:45Z- handleDepth()-Depth_Keller msg: 1613174085868 epoch sec; seqNo:7975; depth 13.81 2021-02-12T23:54:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:45Z- handleDvl() - RDI_Pathfinder msg: 1613174085.87 epoch sec; seqNo:3744 2021-02-12T23:54:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , 0.05 2021-02-12T23:54:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174085.89 epoch sec; seqNo:7975; 4058151.14 north; 591717.51 east 2021-02-12T23:54:46Z- AHRS_M2 msg: 1613174086.26 epoch sec; seqNo:7911 2021-02-12T23:54:46Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.18 psi 2021-02-12T23:54:46Z- handleDepth()-Depth_Keller msg: 1613174086268 epoch sec; seqNo:7976; depth 13.69 2021-02-12T23:54:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174086.28 epoch sec; seqNo:7976; 4058151.52 north; 591717.51 east 2021-02-12T23:54:46Z- AHRS_M2 msg: 1613174086.67 epoch sec; seqNo:7912 2021-02-12T23:54:46Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.18 psi 2021-02-12T23:54:46Z- handleDepth()-Depth_Keller msg: 1613174086672 epoch sec; seqNo:7977; depth 13.58 2021-02-12T23:54:46Z- handleDvl() - RDI_Pathfinder msg: 1613174086.68 epoch sec; seqNo:3745 2021-02-12T23:54:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.05 , 101.63 , 115.15 , 76.43 2021-02-12T23:54:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.00 , -0.05 2021-02-12T23:54:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174086.70 epoch sec; seqNo:7977; 4058151.53 north; 591718.73 east 2021-02-12T23:54:47Z- AHRS_M2 msg: 1613174087.07 epoch sec; seqNo:7913 2021-02-12T23:54:47Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.18 psi 2021-02-12T23:54:47Z- handleDepth()-Depth_Keller msg: 1613174087076 epoch sec; seqNo:7978; depth 13.45 2021-02-12T23:54:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:47Z- handleDvl() - RDI_Pathfinder msg: 1613174087.08 epoch sec; seqNo:3746 2021-02-12T23:54:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.00 , -0.05 2021-02-12T23:54:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174087.09 epoch sec; seqNo:7978; 4058151.91 north; 591718.73 east 2021-02-12T23:54:47Z- AHRS_M2 msg: 1613174087.48 epoch sec; seqNo:7914 2021-02-12T23:54:47Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.18 psi 2021-02-12T23:54:47Z- handleDepth()-Depth_Keller msg: 1613174087481 epoch sec; seqNo:7979; depth 13.36 2021-02-12T23:54:47Z- handleDvl() - RDI_Pathfinder msg: 1613174087.49 epoch sec; seqNo:3747 2021-02-12T23:54:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 102.92 , 115.08 , 74.38 2021-02-12T23:54:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , -0.10 2021-02-12T23:54:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174087.51 epoch sec; seqNo:7979; 4058152.29 north; 591718.72 east 2021-02-12T23:54:47Z- AHRS_M2 msg: 1613174087.88 epoch sec; seqNo:7915 2021-02-12T23:54:47Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.18 psi 2021-02-12T23:54:47Z- handleDepth()-Depth_Keller msg: 1613174087894 epoch sec; seqNo:7980; depth 13.23 2021-02-12T23:54:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174087.90 epoch sec; seqNo:7980; 4058152.29 north; 591718.72 east 2021-02-12T23:54:48Z- AHRS_M2 msg: 1613174088.28 epoch sec; seqNo:7916 2021-02-12T23:54:48Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.18 psi 2021-02-12T23:54:48Z- handleDepth()-Depth_Keller msg: 1613174088293 epoch sec; seqNo:7981; depth 13.13 2021-02-12T23:54:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:48Z- handleDvl() - RDI_Pathfinder msg: 1613174088.30 epoch sec; seqNo:3748 2021-02-12T23:54:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , -0.10 2021-02-12T23:54:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174088.30 epoch sec; seqNo:7981; 4058152.67 north; 591718.72 east 2021-02-12T23:54:48Z- AHRS_M2 msg: 1613174088.69 epoch sec; seqNo:7917 2021-02-12T23:54:48Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.18 psi 2021-02-12T23:54:48Z- handleDepth()-Depth_Keller msg: 1613174088692 epoch sec; seqNo:7982; depth 13.00 2021-02-12T23:54:48Z- handleDvl() - RDI_Pathfinder msg: 1613174088.70 epoch sec; seqNo:3749 2021-02-12T23:54:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 101.56 , 117.79 , 76.02 2021-02-12T23:54:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , -0.06 2021-02-12T23:54:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174088.71 epoch sec; seqNo:7982; 4058153.05 north; 591718.71 east 2021-02-12T23:54:49Z- AHRS_M2 msg: 1613174089.09 epoch sec; seqNo:7918 2021-02-12T23:54:49Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.18 psi 2021-02-12T23:54:49Z- handleDepth()-Depth_Keller msg: 1613174089097 epoch sec; seqNo:7983; depth 12.88 2021-02-12T23:54:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174089.11 epoch sec; seqNo:7983; 4058153.05 north; 591718.71 east 2021-02-12T23:54:49Z- AHRS_M2 msg: 1613174089.49 epoch sec; seqNo:7919 2021-02-12T23:54:49Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.19 psi 2021-02-12T23:54:49Z- handleDepth()-Depth_Keller msg: 1613174089499 epoch sec; seqNo:7984; depth 12.76 2021-02-12T23:54:49Z- handleDvl() - RDI_Pathfinder msg: 1613174089.52 epoch sec; seqNo:3750 2021-02-12T23:54:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 102.92 , 113.73 , 76.02 2021-02-12T23:54:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , 0.04 2021-02-12T23:54:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174089.53 epoch sec; seqNo:7984; 4058153.44 north; 591719.93 east 2021-02-12T23:54:49Z- AHRS_M2 msg: 1613174089.90 epoch sec; seqNo:7920 2021-02-12T23:54:49Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.19 psi 2021-02-12T23:54:49Z- handleDepth()-Depth_Keller msg: 1613174089904 epoch sec; seqNo:7985; depth 12.64 2021-02-12T23:54:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174089.92 epoch sec; seqNo:7985; 4058153.82 north; 591719.93 east 2021-02-12T23:54:50Z- LcmTrn::handleMessages() - handling 47 messages took 0.65 ms and 134 calls... 2021-02-12T23:54:50Z- waiting for 1613174087.68, time is 1613174089.92 2021-02-12T23:54:50Z- LcmTrn::updateTrn() >>>> pose time 1613174089.92, meas time 1613174089.52 <<<< 2021-02-12T23:54:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174089.52 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:54:50Z- TerrainNav::motionUpdate() - currEstimate = (4058154, 591719.93, 12.64) 2021-02-12T23:54:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:54:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:54:50Z- TNavBF::Projecting Measurements 2021-02-12T23:54:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:54:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:54:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:54:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:54:50Z- TNavFilter::4 good beams. 2021-02-12T23:54:50Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:54:50Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:54:50Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:54:50Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:54:50Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:54:50Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:54:50Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:54:50Z- TNavBF::mmse selected from filter -1 2021-02-12T23:54:50Z- TerrainMapOctree: (vehN, vehE ) = (4058153.53, 591719.75). 2021-02-12T23:54:50Z- TerrainMapOctree: Min Distance = 3998.79. 2021-02-12T23:54:50Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:54:50Z- There is no specific good beam that all particles have in common. 2021-02-12T23:54:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:54:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 11.65 2021-02-12T23:54:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:54:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 16.95 2021-02-12T23:54:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:54:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 20.70 2021-02-12T23:54:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:54:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 9.25 2021-02-12T23:54:50Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:54:50Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:54:50Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:54:50Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:54:50Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:54:50Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:54:50Z- Filter 0 No beams used in subcloud update 2021-02-12T23:54:50Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:54:50Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:54:50Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 729.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:54:50Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174089.52 sec. 2021-02-12T23:54:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:54:50Z- TNavBF::mmse selected from filter -1 2021-02-12T23:54:50Z- variances([0], [2], and psi_berg) = 729.021713 833.015855 0.000000 2021-02-12T23:54:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:54:50Z- LcmTrn::publishEstimates() - MLE : 4058147.32 591672.43 12.64 2021-02-12T23:54:50Z- LcmTrn::publishEstimates() - MMSE : 4058153.82 591719.93 12.64 2021-02-12T23:54:50Z- LcmTrn::publishEstimates() - COVAR: 729.02 833.02 0.94 0.00 2021-02-12T23:54:50Z- AHRS_M2 msg: 1613174090.30 epoch sec; seqNo:7921 2021-02-12T23:54:50Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-12T23:54:50Z- handleDepth()-Depth_Keller msg: 1613174090313 epoch sec; seqNo:7986; depth 12.54 2021-02-12T23:54:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174090.32 epoch sec; seqNo:7986; 4058154.20 north; 591719.92 east 2021-02-12T23:54:50Z- AHRS_M2 msg: 1613174090.71 epoch sec; seqNo:7922 2021-02-12T23:54:50Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.18 psi 2021-02-12T23:54:50Z- handleDepth()-Depth_Keller msg: 1613174090721 epoch sec; seqNo:7987; depth 12.41 2021-02-12T23:54:50Z- handleDvl() - RDI_Pathfinder msg: 1613174090.73 epoch sec; seqNo:3751 2021-02-12T23:54:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 104.27 , 115.08 , 76.43 2021-02-12T23:54:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , 0.09 2021-02-12T23:54:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174090.74 epoch sec; seqNo:7987; 4058154.20 north; 591719.92 east 2021-02-12T23:54:51Z- AHRS_M2 msg: 1613174091.11 epoch sec; seqNo:7923 2021-02-12T23:54:51Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.18 psi 2021-02-12T23:54:51Z- handleDepth()-Depth_Keller msg: 1613174091118 epoch sec; seqNo:7988; depth 12.29 2021-02-12T23:54:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174091.14 epoch sec; seqNo:7988; 4058154.58 north; 591719.92 east 2021-02-12T23:54:51Z- AHRS_M2 msg: 1613174091.53 epoch sec; seqNo:7924 2021-02-12T23:54:51Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.17 psi 2021-02-12T23:54:51Z- handleDepth()-Depth_Keller msg: 1613174091534 epoch sec; seqNo:7989; depth 12.16 2021-02-12T23:54:51Z- handleDvl() - RDI_Pathfinder msg: 1613174091.56 epoch sec; seqNo:3752 2021-02-12T23:54:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.27 , 102.85 , 117.72 , 78.51 2021-02-12T23:54:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.06 , 0.05 2021-02-12T23:54:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174091.57 epoch sec; seqNo:7989; 4058154.96 north; 591719.91 east 2021-02-12T23:54:51Z- AHRS_M2 msg: 1613174091.92 epoch sec; seqNo:7925 2021-02-12T23:54:51Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.16 psi 2021-02-12T23:54:51Z- handleDepth()-Depth_Keller msg: 1613174091924 epoch sec; seqNo:7990; depth 12.02 2021-02-12T23:54:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:51Z- handleDvl() - RDI_Pathfinder msg: 1613174091.93 epoch sec; seqNo:3753 2021-02-12T23:54:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.06 , 0.05 2021-02-12T23:54:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174091.94 epoch sec; seqNo:7990; 4058154.97 north; 591721.14 east 2021-02-12T23:54:52Z- AHRS_M2 msg: 1613174092.33 epoch sec; seqNo:7926 2021-02-12T23:54:52Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.16 psi 2021-02-12T23:54:52Z- handleDepth()-Depth_Keller msg: 1613174092329 epoch sec; seqNo:7991; depth 11.91 2021-02-12T23:54:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174092.34 epoch sec; seqNo:7991; 4058155.35 north; 591721.13 east 2021-02-12T23:54:52Z- AHRS_M2 msg: 1613174092.73 epoch sec; seqNo:7927 2021-02-12T23:54:52Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.16 psi 2021-02-12T23:54:52Z- handleDepth()-Depth_Keller msg: 1613174092732 epoch sec; seqNo:7992; depth 11.78 2021-02-12T23:54:52Z- handleDvl() - RDI_Pathfinder msg: 1613174092.75 epoch sec; seqNo:3754 2021-02-12T23:54:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 102.78 , 117.65 , 77.24 2021-02-12T23:54:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , -0.03 2021-02-12T23:54:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174092.76 epoch sec; seqNo:7992; 4058155.73 north; 591721.13 east 2021-02-12T23:54:53Z- AHRS_M2 msg: 1613174093.13 epoch sec; seqNo:7928 2021-02-12T23:54:53Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.16 psi 2021-02-12T23:54:53Z- handleDepth()-Depth_Keller msg: 1613174093145 epoch sec; seqNo:7993; depth 11.66 2021-02-12T23:54:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:53Z- handleDvl() - RDI_Pathfinder msg: 1613174093.15 epoch sec; seqNo:3755 2021-02-12T23:54:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , -0.03 2021-02-12T23:54:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174093.17 epoch sec; seqNo:7993; 4058156.11 north; 591721.12 east 2021-02-12T23:54:53Z- AHRS_M2 msg: 1613174093.54 epoch sec; seqNo:7929 2021-02-12T23:54:53Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.15 psi 2021-02-12T23:54:53Z- handleDepth()-Depth_Keller msg: 1613174093548 epoch sec; seqNo:7994; depth 11.54 2021-02-12T23:54:53Z- handleDvl() - RDI_Pathfinder msg: 1613174093.56 epoch sec; seqNo:3756 2021-02-12T23:54:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.13 , 120.36 , 76.02 2021-02-12T23:54:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , 0.01 2021-02-12T23:54:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174093.59 epoch sec; seqNo:7994; 4058156.11 north; 591721.12 east 2021-02-12T23:54:53Z- AHRS_M2 msg: 1613174093.94 epoch sec; seqNo:7930 2021-02-12T23:54:53Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.15 psi 2021-02-12T23:54:53Z- handleDepth()-Depth_Keller msg: 1613174093949 epoch sec; seqNo:7995; depth 11.41 2021-02-12T23:54:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174093.96 epoch sec; seqNo:7995; 4058156.48 north; 591721.12 east 2021-02-12T23:54:54Z- AHRS_M2 msg: 1613174094.34 epoch sec; seqNo:7931 2021-02-12T23:54:54Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.14 psi 2021-02-12T23:54:54Z- handleDepth()-Depth_Keller msg: 1613174094358 epoch sec; seqNo:7996; depth 11.29 2021-02-12T23:54:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:54Z- handleDvl() - RDI_Pathfinder msg: 1613174094.36 epoch sec; seqNo:3757 2021-02-12T23:54:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , 0.01 2021-02-12T23:54:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174094.37 epoch sec; seqNo:7996; 4058156.88 north; 591722.34 east 2021-02-12T23:54:54Z- AHRS_M2 msg: 1613174094.75 epoch sec; seqNo:7932 2021-02-12T23:54:54Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.14 psi 2021-02-12T23:54:54Z- handleDepth()-Depth_Keller msg: 1613174094752 epoch sec; seqNo:7997; depth 11.16 2021-02-12T23:54:54Z- handleDvl() - RDI_Pathfinder msg: 1613174094.76 epoch sec; seqNo:3758 2021-02-12T23:54:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.92 , 103.79 , 118.67 , 78.06 2021-02-12T23:54:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.09 2021-02-12T23:54:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174094.79 epoch sec; seqNo:7997; 4058157.26 north; 591722.33 east 2021-02-12T23:54:55Z- AHRS_M2 msg: 1613174095.15 epoch sec; seqNo:7933 2021-02-12T23:54:55Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.14 psi 2021-02-12T23:54:55Z- LcmTrn::handleMessages() - handling 45 messages took 0.65 ms and 132 calls... 2021-02-12T23:54:55Z- waiting for 1613174092.92, time is 1613174095.15 2021-02-12T23:54:55Z- LcmTrn::updateTrn() >>>> pose time 1613174095.15, meas time 1613174094.76 <<<< 2021-02-12T23:54:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174094.76 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:54:55Z- TerrainNav::motionUpdate() - currEstimate = (4058157, 591722.33, 11.16) 2021-02-12T23:54:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:54:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:54:55Z- TNavBF::Projecting Measurements 2021-02-12T23:54:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:54:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:54:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:54:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:54:55Z- TNavFilter::4 good beams. 2021-02-12T23:54:55Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:54:55Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:54:55Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:54:55Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:54:55Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:54:55Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:54:55Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:54:55Z- TNavBF::mmse selected from filter -1 2021-02-12T23:54:55Z- TerrainMapOctree: (vehN, vehE ) = (4058157.00, 591722.15). 2021-02-12T23:54:55Z- TerrainMapOctree: Min Distance = 3994.58. 2021-02-12T23:54:55Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:54:55Z- There is no specific good beam that all particles have in common. 2021-02-12T23:54:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:54:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 12.65 2021-02-12T23:54:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:54:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 17.24 2021-02-12T23:54:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:54:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 22.53 2021-02-12T23:54:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:54:55Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 9.75 2021-02-12T23:54:55Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:54:55Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:54:55Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:54:55Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:54:55Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:54:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:54:55Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:54:55Z- Filter 0 No beams used in subcloud update 2021-02-12T23:54:55Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:54:55Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:54:55Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 728.9 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:54:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174094.76 sec. 2021-02-12T23:54:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:54:55Z- TNavBF::mmse selected from filter -1 2021-02-12T23:54:55Z- variances([0], [2], and psi_berg) = 728.927029 833.015787 0.000000 2021-02-12T23:54:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:54:55Z- LcmTrn::publishEstimates() - MLE : 4058150.76 591674.83 11.16 2021-02-12T23:54:55Z- LcmTrn::publishEstimates() - MMSE : 4058157.26 591722.33 11.16 2021-02-12T23:54:55Z- LcmTrn::publishEstimates() - COVAR: 728.93 833.02 0.90 0.00 2021-02-12T23:54:55Z- handleDepth()-Depth_Keller msg: 1613174095169 epoch sec; seqNo:7998; depth 11.03 2021-02-12T23:54:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174095.19 epoch sec; seqNo:7998; 4058157.63 north; 591722.33 east 2021-02-12T23:54:55Z- AHRS_M2 msg: 1613174095.56 epoch sec; seqNo:7934 2021-02-12T23:54:55Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.14 psi 2021-02-12T23:54:55Z- handleDepth()-Depth_Keller msg: 1613174095579 epoch sec; seqNo:7999; depth 10.93 2021-02-12T23:54:55Z- handleDvl() - RDI_Pathfinder msg: 1613174095.59 epoch sec; seqNo:3759 2021-02-12T23:54:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.12 , 102.65 , 120.22 , 76.83 2021-02-12T23:54:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.12 , 0.09 2021-02-12T23:54:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174095.60 epoch sec; seqNo:7999; 4058157.63 north; 591722.33 east 2021-02-12T23:54:55Z- AHRS_M2 msg: 1613174095.96 epoch sec; seqNo:7935 2021-02-12T23:54:55Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.14 psi 2021-02-12T23:54:55Z- handleDepth()-Depth_Keller msg: 1613174095964 epoch sec; seqNo:8000; depth 10.79 2021-02-12T23:54:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174095.99 epoch sec; seqNo:8000; 4058158.01 north; 591722.32 east 2021-02-12T23:54:56Z- AHRS_M2 msg: 1613174096.37 epoch sec; seqNo:7936 2021-02-12T23:54:56Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-12T23:54:56Z- handleDepth()-Depth_Keller msg: 1613174096374 epoch sec; seqNo:8001; depth 10.67 2021-02-12T23:54:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174096.38 epoch sec; seqNo:8001; 4058158.01 north; 591722.32 east 2021-02-12T23:54:56Z- AHRS_M2 msg: 1613174096.77 epoch sec; seqNo:7937 2021-02-12T23:54:56Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.18 psi 2021-02-12T23:54:56Z- handleDepth()-Depth_Keller msg: 1613174096777 epoch sec; seqNo:8002; depth 10.54 2021-02-12T23:54:56Z- handleDvl() - RDI_Pathfinder msg: 1613174096.79 epoch sec; seqNo:3760 2021-02-12T23:54:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 104.00 , 121.58 , 76.83 2021-02-12T23:54:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.23 , -0.10 2021-02-12T23:54:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174096.80 epoch sec; seqNo:8002; 4058158.41 north; 591723.54 east 2021-02-12T23:54:57Z- AHRS_M2 msg: 1613174097.17 epoch sec; seqNo:7938 2021-02-12T23:54:57Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.19 psi 2021-02-12T23:54:57Z- handleDepth()-Depth_Keller msg: 1613174097180 epoch sec; seqNo:8003; depth 10.38 2021-02-12T23:54:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174097.19 epoch sec; seqNo:8003; 4058158.41 north; 591723.54 east 2021-02-12T23:54:57Z- AHRS_M2 msg: 1613174097.58 epoch sec; seqNo:7939 2021-02-12T23:54:57Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.21 psi 2021-02-12T23:54:57Z- handleDepth()-Depth_Keller msg: 1613174097581 epoch sec; seqNo:8004; depth 10.25 2021-02-12T23:54:57Z- handleDvl() - RDI_Pathfinder msg: 1613174097.59 epoch sec; seqNo:3761 2021-02-12T23:54:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 106.50 , 120.02 , 79.46 2021-02-12T23:54:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.04 , -0.22 2021-02-12T23:54:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174097.60 epoch sec; seqNo:8004; 4058158.78 north; 591723.54 east 2021-02-12T23:54:57Z- AHRS_M2 msg: 1613174097.98 epoch sec; seqNo:7940 2021-02-12T23:54:57Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.22 psi 2021-02-12T23:54:57Z- handleDepth()-Depth_Keller msg: 1613174097985 epoch sec; seqNo:8005; depth 10.11 2021-02-12T23:54:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:57Z- handleDvl() - RDI_Pathfinder msg: 1613174097.99 epoch sec; seqNo:3762 2021-02-12T23:54:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.04 , -0.22 2021-02-12T23:54:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174098.00 epoch sec; seqNo:8005; 4058158.78 north; 591723.54 east 2021-02-12T23:54:58Z- AHRS_M2 msg: 1613174098.38 epoch sec; seqNo:7941 2021-02-12T23:54:58Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.21 psi 2021-02-12T23:54:58Z- handleDepth()-Depth_Keller msg: 1613174098388 epoch sec; seqNo:8006; depth 9.97 2021-02-12T23:54:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174098.40 epoch sec; seqNo:8006; 4058159.16 north; 591723.53 east 2021-02-12T23:54:58Z- AHRS_M2 msg: 1613174098.79 epoch sec; seqNo:7942 2021-02-12T23:54:58Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.20 psi 2021-02-12T23:54:58Z- handleDepth()-Depth_Keller msg: 1613174098797 epoch sec; seqNo:8007; depth 9.84 2021-02-12T23:54:58Z- handleDvl() - RDI_Pathfinder msg: 1613174098.82 epoch sec; seqNo:3763 2021-02-12T23:54:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.01 , 108.32 , 121.71 , 80.07 2021-02-12T23:54:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.25 , -0.01 2021-02-12T23:54:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174098.83 epoch sec; seqNo:8007; 4058159.54 north; 591723.53 east 2021-02-12T23:54:59Z- AHRS_M2 msg: 1613174099.19 epoch sec; seqNo:7943 2021-02-12T23:54:59Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.19 psi 2021-02-12T23:54:59Z- handleDepth()-Depth_Keller msg: 1613174099196 epoch sec; seqNo:8008; depth 9.71 2021-02-12T23:54:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:54:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:54:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:54:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:54:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:54:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:54:59Z- handleDvl() - RDI_Pathfinder msg: 1613174099.20 epoch sec; seqNo:3764 2021-02-12T23:54:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:54:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.25 , -0.01 2021-02-12T23:54:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174099.21 epoch sec; seqNo:8008; 4058159.92 north; 591723.52 east 2021-02-12T23:54:59Z- AHRS_M2 msg: 1613174099.60 epoch sec; seqNo:7944 2021-02-12T23:54:59Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-12T23:54:59Z- handleDepth()-Depth_Keller msg: 1613174099607 epoch sec; seqNo:8009; depth 9.59 2021-02-12T23:54:59Z- handleDvl() - RDI_Pathfinder msg: 1613174099.62 epoch sec; seqNo:3765 2021-02-12T23:54:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 107.92 , 122.79 , 80.88 2021-02-12T23:54:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.17 , 0.22 2021-02-12T23:54:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174099.63 epoch sec; seqNo:8009; 4058160.31 north; 591724.74 east 2021-02-12T23:55:00Z- AHRS_M2 msg: 1613174100.00 epoch sec; seqNo:7945 2021-02-12T23:55:00Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-12T23:55:00Z- handleDepth()-Depth_Keller msg: 1613174100009 epoch sec; seqNo:8010; depth 9.46 2021-02-12T23:55:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174100.02 epoch sec; seqNo:8010; 4058160.31 north; 591724.74 east 2021-02-12T23:55:00Z- LcmTrn::handleMessages() - handling 45 messages took 0.62 ms and 133 calls... 2021-02-12T23:55:00Z- waiting for 1613174098.15, time is 1613174100.02 2021-02-12T23:55:00Z- LcmTrn::updateTrn() >>>> pose time 1613174100.02, meas time 1613174099.62 <<<< 2021-02-12T23:55:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174099.62 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:55:00Z- TerrainNav::motionUpdate() - currEstimate = (4058160, 591724.74, 9.46) 2021-02-12T23:55:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:55:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:55:00Z- TNavBF::Projecting Measurements 2021-02-12T23:55:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:55:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:55:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:55:00Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:55:00Z- TNavFilter::3 good beams. 2021-02-12T23:55:00Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:55:00Z- TNavBF:: beamIndex[0] = 1 2021-02-12T23:55:00Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:55:00Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:55:00Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:55:00Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:55:00Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:55:00Z- TNavBF::mmse selected from filter -1 2021-02-12T23:55:00Z- TerrainMapOctree: (vehN, vehE ) = (4058160.06, 591724.54). 2021-02-12T23:55:00Z- TerrainMapOctree: Min Distance = 3990.70. 2021-02-12T23:55:00Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:55:00Z- There is no specific good beam that all particles have in common. 2021-02-12T23:55:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:55:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 18.63 2021-02-12T23:55:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:55:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 24.12 2021-02-12T23:55:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:55:00Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 10.47 2021-02-12T23:55:00Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:55:00Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:55:00Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:55:00Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:55:00Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:55:00Z- Filter 0 No beams used in subcloud update 2021-02-12T23:55:00Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:55:00Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:55:00Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 728.9 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:55:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174099.62 sec. 2021-02-12T23:55:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:55:00Z- TNavBF::mmse selected from filter -1 2021-02-12T23:55:00Z- variances([0], [2], and psi_berg) = 728.835175 833.015721 0.000000 2021-02-12T23:55:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:55:00Z- LcmTrn::publishEstimates() - MLE : 4058153.81 591677.24 9.46 2021-02-12T23:55:00Z- LcmTrn::publishEstimates() - MMSE : 4058160.31 591724.74 9.46 2021-02-12T23:55:00Z- LcmTrn::publishEstimates() - COVAR: 728.84 833.02 0.93 0.00 2021-02-12T23:55:00Z- AHRS_M2 msg: 1613174100.40 epoch sec; seqNo:7946 2021-02-12T23:55:00Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-12T23:55:00Z- handleDepth()-Depth_Keller msg: 1613174100413 epoch sec; seqNo:8011; depth 9.34 2021-02-12T23:55:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:00Z- handleDvl() - RDI_Pathfinder msg: 1613174100.41 epoch sec; seqNo:3766 2021-02-12T23:55:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.17 , 0.22 2021-02-12T23:55:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174100.42 epoch sec; seqNo:8011; 4058160.69 north; 591724.74 east 2021-02-12T23:55:00Z- AHRS_M2 msg: 1613174100.81 epoch sec; seqNo:7947 2021-02-12T23:55:00Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-12T23:55:00Z- handleDepth()-Depth_Keller msg: 1613174100814 epoch sec; seqNo:8012; depth 9.21 2021-02-12T23:55:00Z- handleDvl() - RDI_Pathfinder msg: 1613174100.83 epoch sec; seqNo:3767 2021-02-12T23:55:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.85 , 103.73 , 118.60 , 79.93 2021-02-12T23:55:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , 0.20 2021-02-12T23:55:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174100.84 epoch sec; seqNo:8012; 4058161.07 north; 591724.73 east 2021-02-12T23:55:01Z- AHRS_M2 msg: 1613174101.21 epoch sec; seqNo:7948 2021-02-12T23:55:01Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-12T23:55:01Z- handleDepth()-Depth_Keller msg: 1613174101217 epoch sec; seqNo:8013; depth 9.09 2021-02-12T23:55:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174101.23 epoch sec; seqNo:8013; 4058161.45 north; 591724.73 east 2021-02-12T23:55:01Z- AHRS_M2 msg: 1613174101.62 epoch sec; seqNo:7949 2021-02-12T23:55:01Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-12T23:55:01Z- handleDepth()-Depth_Keller msg: 1613174101620 epoch sec; seqNo:8014; depth 8.97 2021-02-12T23:55:01Z- handleDvl() - RDI_Pathfinder msg: 1613174101.63 epoch sec; seqNo:3768 2021-02-12T23:55:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 104.00 , 117.52 , 79.69 2021-02-12T23:55:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.01 , 0.12 2021-02-12T23:55:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174101.64 epoch sec; seqNo:8014; 4058161.83 north; 591724.72 east 2021-02-12T23:55:02Z- AHRS_M2 msg: 1613174102.02 epoch sec; seqNo:7950 2021-02-12T23:55:02Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-12T23:55:02Z- handleDepth()-Depth_Keller msg: 1613174102025 epoch sec; seqNo:8015; depth 8.86 2021-02-12T23:55:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174102.04 epoch sec; seqNo:8015; 4058162.21 north; 591724.72 east 2021-02-12T23:55:02Z- AHRS_M2 msg: 1613174102.42 epoch sec; seqNo:7951 2021-02-12T23:55:02Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-12T23:55:02Z- handleDepth()-Depth_Keller msg: 1613174102428 epoch sec; seqNo:8016; depth 8.75 2021-02-12T23:55:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174102.44 epoch sec; seqNo:8016; 4058162.22 north; 591725.94 east 2021-02-12T23:55:02Z- AHRS_M2 msg: 1613174102.83 epoch sec; seqNo:7952 2021-02-12T23:55:02Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-12T23:55:02Z- handleDepth()-Depth_Keller msg: 1613174102833 epoch sec; seqNo:8017; depth 8.64 2021-02-12T23:55:02Z- handleDvl() - RDI_Pathfinder msg: 1613174102.85 epoch sec; seqNo:3769 2021-02-12T23:55:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.03 , 105.15 , 118.67 , 79.29 2021-02-12T23:55:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.09 , 0.06 2021-02-12T23:55:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174102.86 epoch sec; seqNo:8017; 4058162.60 north; 591725.94 east 2021-02-12T23:55:03Z- AHRS_M2 msg: 1613174103.23 epoch sec; seqNo:7953 2021-02-12T23:55:03Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-12T23:55:03Z- handleDepth()-Depth_Keller msg: 1613174103238 epoch sec; seqNo:8018; depth 8.53 2021-02-12T23:55:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174103.25 epoch sec; seqNo:8018; 4058162.98 north; 591725.93 east 2021-02-12T23:55:03Z- AHRS_M2 msg: 1613174103.64 epoch sec; seqNo:7954 2021-02-12T23:55:03Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.14 psi 2021-02-12T23:55:03Z- handleDepth()-Depth_Keller msg: 1613174103650 epoch sec; seqNo:8019; depth 8.42 2021-02-12T23:55:03Z- handleDvl() - RDI_Pathfinder msg: 1613174103.66 epoch sec; seqNo:3770 2021-02-12T23:55:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.27 , 105.15 , 118.67 , 80.81 2021-02-12T23:55:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.19 , -0.00 2021-02-12T23:55:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174103.67 epoch sec; seqNo:8019; 4058162.98 north; 591725.93 east 2021-02-12T23:55:04Z- AHRS_M2 msg: 1613174104.04 epoch sec; seqNo:7955 2021-02-12T23:55:04Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.15 psi 2021-02-12T23:55:04Z- handleDepth()-Depth_Keller msg: 1613174104044 epoch sec; seqNo:8020; depth 8.32 2021-02-12T23:55:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:04Z- handleDvl() - RDI_Pathfinder msg: 1613174104.05 epoch sec; seqNo:3771 2021-02-12T23:55:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.19 , -0.00 2021-02-12T23:55:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174104.06 epoch sec; seqNo:8020; 4058163.37 north; 591727.15 east 2021-02-12T23:55:04Z- AHRS_M2 msg: 1613174104.44 epoch sec; seqNo:7956 2021-02-12T23:55:04Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.15 psi 2021-02-12T23:55:04Z- handleDepth()-Depth_Keller msg: 1613174104448 epoch sec; seqNo:8021; depth 8.21 2021-02-12T23:55:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174104.46 epoch sec; seqNo:8021; 4058163.75 north; 591727.15 east 2021-02-12T23:55:04Z- AHRS_M2 msg: 1613174104.85 epoch sec; seqNo:7957 2021-02-12T23:55:04Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.16 psi 2021-02-12T23:55:04Z- handleDepth()-Depth_Keller msg: 1613174104854 epoch sec; seqNo:8022; depth 8.09 2021-02-12T23:55:04Z- handleDvl() - RDI_Pathfinder msg: 1613174104.87 epoch sec; seqNo:3772 2021-02-12T23:55:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.41 , 102.58 , 118.80 , 80.94 2021-02-12T23:55:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.15 , -0.08 2021-02-12T23:55:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174104.88 epoch sec; seqNo:8022; 4058163.75 north; 591727.15 east 2021-02-12T23:55:05Z- AHRS_M2 msg: 1613174105.25 epoch sec; seqNo:7958 2021-02-12T23:55:05Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.16 psi 2021-02-12T23:55:05Z- handleDepth()-Depth_Keller msg: 1613174105256 epoch sec; seqNo:8023; depth 7.97 2021-02-12T23:55:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:05Z- handleDvl() - RDI_Pathfinder msg: 1613174105.26 epoch sec; seqNo:3773 2021-02-12T23:55:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.15 , -0.08 2021-02-12T23:55:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174105.28 epoch sec; seqNo:8023; 4058164.13 north; 591727.14 east 2021-02-12T23:55:05Z- LcmTrn::handleMessages() - handling 47 messages took 0.76 ms and 132 calls... 2021-02-12T23:55:05Z- waiting for 1613174103.02, time is 1613174105.28 2021-02-12T23:55:05Z- LcmTrn::updateTrn() equating pose 1613174105.28 and meas times 1613174105.28 2021-02-12T23:55:05Z- LcmTrn::updateTrn() >>>> pose time 1613174105.28, meas time 1613174105.28 <<<< 2021-02-12T23:55:05Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174105.28, ping # 0. 2021-02-12T23:55:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174105.28 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:55:05Z- TerrainNav::motionUpdate() - currEstimate = (4058164, 591727.14, 7.97) 2021-02-12T23:55:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:55:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:55:05Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:55:05Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:55:05Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174105.28 sec.; No relevant navigation data available 2021-02-12T23:55:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:55:05Z- TNavBF::mmse selected from filter -1 2021-02-12T23:55:05Z- variances([0], [2], and psi_berg) = 728.755848 833.015664 0.000000 2021-02-12T23:55:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:55:05Z- LcmTrn::publishEstimates() - MLE : 4058157.63 591679.64 7.97 2021-02-12T23:55:05Z- LcmTrn::publishEstimates() - MMSE : 4058164.13 591727.14 7.97 2021-02-12T23:55:05Z- LcmTrn::publishEstimates() - COVAR: 728.76 833.02 1.16 0.00 2021-02-12T23:55:05Z- AHRS_M2 msg: 1613174105.66 epoch sec; seqNo:7959 2021-02-12T23:55:05Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.17 psi 2021-02-12T23:55:05Z- handleDepth()-Depth_Keller msg: 1613174105660 epoch sec; seqNo:8024; depth 7.86 2021-02-12T23:55:05Z- handleDvl() - RDI_Pathfinder msg: 1613174105.67 epoch sec; seqNo:3774 2021-02-12T23:55:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.17 , 107.99 , 121.51 , 82.30 2021-02-12T23:55:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , -0.10 2021-02-12T23:55:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174105.70 epoch sec; seqNo:8024; 4058164.51 north; 591727.14 east 2021-02-12T23:55:06Z- AHRS_M2 msg: 1613174106.06 epoch sec; seqNo:7960 2021-02-12T23:55:06Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.18 psi 2021-02-12T23:55:06Z- handleDepth()-Depth_Keller msg: 1613174106064 epoch sec; seqNo:8025; depth 7.72 2021-02-12T23:55:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174106.08 epoch sec; seqNo:8025; 4058164.51 north; 591727.14 east 2021-02-12T23:55:06Z- AHRS_M2 msg: 1613174106.46 epoch sec; seqNo:7961 2021-02-12T23:55:06Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.19 psi 2021-02-12T23:55:06Z- handleDepth()-Depth_Keller msg: 1613174106477 epoch sec; seqNo:8026; depth 7.63 2021-02-12T23:55:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:06Z- handleDvl() - RDI_Pathfinder msg: 1613174106.48 epoch sec; seqNo:3775 2021-02-12T23:55:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , -0.10 2021-02-12T23:55:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174106.49 epoch sec; seqNo:8026; 4058164.90 north; 591728.35 east 2021-02-12T23:55:06Z- AHRS_M2 msg: 1613174106.87 epoch sec; seqNo:7962 2021-02-12T23:55:06Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.20 psi 2021-02-12T23:55:06Z- handleDepth()-Depth_Keller msg: 1613174106877 epoch sec; seqNo:8027; depth 7.51 2021-02-12T23:55:06Z- handleDvl() - RDI_Pathfinder msg: 1613174106.89 epoch sec; seqNo:3776 2021-02-12T23:55:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.91 , 108.32 , 121.71 , 81.55 2021-02-12T23:55:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , -0.05 2021-02-12T23:55:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174106.91 epoch sec; seqNo:8027; 4058165.28 north; 591728.35 east 2021-02-12T23:55:07Z- AHRS_M2 msg: 1613174107.27 epoch sec; seqNo:7963 2021-02-12T23:55:07Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.20 psi 2021-02-12T23:55:07Z- handleDepth()-Depth_Keller msg: 1613174107276 epoch sec; seqNo:8028; depth 7.38 2021-02-12T23:55:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174107.29 epoch sec; seqNo:8028; 4058165.28 north; 591728.35 east 2021-02-12T23:55:07Z- AHRS_M2 msg: 1613174107.68 epoch sec; seqNo:7964 2021-02-12T23:55:07Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.19 psi 2021-02-12T23:55:07Z- handleDepth()-Depth_Keller msg: 1613174107685 epoch sec; seqNo:8029; depth 7.30 2021-02-12T23:55:07Z- handleDvl() - RDI_Pathfinder msg: 1613174107.70 epoch sec; seqNo:3777 2021-02-12T23:55:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.48 , 111.30 , 124.69 , 83.04 2021-02-12T23:55:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.00 , -0.00 2021-02-12T23:55:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174107.71 epoch sec; seqNo:8029; 4058165.66 north; 591728.35 east 2021-02-12T23:55:08Z- AHRS_M2 msg: 1613174108.08 epoch sec; seqNo:7965 2021-02-12T23:55:08Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.18 psi 2021-02-12T23:55:08Z- handleDepth()-Depth_Keller msg: 1613174108086 epoch sec; seqNo:8030; depth 7.17 2021-02-12T23:55:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174108.10 epoch sec; seqNo:8030; 4058166.03 north; 591728.34 east 2021-02-12T23:55:08Z- AHRS_M2 msg: 1613174108.49 epoch sec; seqNo:7966 2021-02-12T23:55:08Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.18 psi 2021-02-12T23:55:08Z- handleDepth()-Depth_Keller msg: 1613174108492 epoch sec; seqNo:8031; depth 7.05 2021-02-12T23:55:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174108.53 epoch sec; seqNo:8031; 4058166.03 north; 591728.34 east 2021-02-12T23:55:08Z- AHRS_M2 msg: 1613174108.89 epoch sec; seqNo:7967 2021-02-12T23:55:08Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-12T23:55:08Z- handleDepth()-Depth_Keller msg: 1613174108892 epoch sec; seqNo:8032; depth 6.93 2021-02-12T23:55:08Z- handleDvl() - RDI_Pathfinder msg: 1613174108.90 epoch sec; seqNo:3778 2021-02-12T23:55:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.48 , 109.81 , 120.22 , 83.04 2021-02-12T23:55:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.06 , 0.03 2021-02-12T23:55:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174108.91 epoch sec; seqNo:8032; 4058166.41 north; 591728.34 east 2021-02-12T23:55:09Z- AHRS_M2 msg: 1613174109.29 epoch sec; seqNo:7968 2021-02-12T23:55:09Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-12T23:55:09Z- handleDepth()-Depth_Keller msg: 1613174109305 epoch sec; seqNo:8033; depth 6.81 2021-02-12T23:55:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174109.32 epoch sec; seqNo:8033; 4058166.43 north; 591729.56 east 2021-02-12T23:55:09Z- AHRS_M2 msg: 1613174109.70 epoch sec; seqNo:7969 2021-02-12T23:55:09Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-12T23:55:09Z- handleDepth()-Depth_Keller msg: 1613174109705 epoch sec; seqNo:8034; depth 6.68 2021-02-12T23:55:09Z- handleDvl() - RDI_Pathfinder msg: 1613174109.72 epoch sec; seqNo:3779 2021-02-12T23:55:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 109.14 , 121.31 , 83.45 2021-02-12T23:55:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.03 , 0.04 2021-02-12T23:55:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174109.73 epoch sec; seqNo:8034; 4058166.80 north; 591729.56 east 2021-02-12T23:55:10Z- AHRS_M2 msg: 1613174110.12 epoch sec; seqNo:7970 2021-02-12T23:55:10Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.18 psi 2021-02-12T23:55:10Z- handleDepth()-Depth_Keller msg: 1613174110121 epoch sec; seqNo:8035; depth 6.55 2021-02-12T23:55:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:10Z- handleDvl() - RDI_Pathfinder msg: 1613174110.12 epoch sec; seqNo:3780 2021-02-12T23:55:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.03 , 0.04 2021-02-12T23:55:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174110.15 epoch sec; seqNo:8035; 4058167.18 north; 591729.55 east 2021-02-12T23:55:10Z- LcmTrn::handleMessages() - handling 43 messages took 0.73 ms and 129 calls... 2021-02-12T23:55:10Z- waiting for 1613174108.28, time is 1613174110.15 2021-02-12T23:55:10Z- LcmTrn::updateTrn() equating pose 1613174110.15 and meas times 1613174110.15 2021-02-12T23:55:10Z- LcmTrn::updateTrn() >>>> pose time 1613174110.15, meas time 1613174110.15 <<<< 2021-02-12T23:55:10Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174110.15, ping # 0. 2021-02-12T23:55:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174110.15 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:55:10Z- TerrainNav::motionUpdate() - currEstimate = (4058167, 591729.55, 6.55) 2021-02-12T23:55:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:55:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:55:10Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:55:10Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:55:10Z- TNavBF::Projecting Measurements 2021-02-12T23:55:10Z- TNavFilter::Measurement from time = 1613174110.15 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:55:10Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:55:10Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:55:10Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:55:10Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:55:10Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:55:10Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:55:10Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:55:10Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 728.7 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-12T23:55:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:55:10Z- TNavBF::mmse selected from filter -1 2021-02-12T23:55:10Z- variances([0], [2], and psi_berg) = 728.682203 833.015611 0.000000 2021-02-12T23:55:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:55:10Z- LcmTrn::publishEstimates() - MLE : 4058160.68 591682.05 6.55 2021-02-12T23:55:10Z- LcmTrn::publishEstimates() - MMSE : 4058167.18 591729.55 6.55 2021-02-12T23:55:10Z- LcmTrn::publishEstimates() - COVAR: 728.68 833.02 1.36 0.00 2021-02-12T23:55:10Z- AHRS_M2 msg: 1613174110.51 epoch sec; seqNo:7971 2021-02-12T23:55:10Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.18 psi 2021-02-12T23:55:10Z- handleDepth()-Depth_Keller msg: 1613174110521 epoch sec; seqNo:8036; depth 6.44 2021-02-12T23:55:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174110.55 epoch sec; seqNo:8036; 4058167.18 north; 591729.55 east 2021-02-12T23:55:10Z- AHRS_M2 msg: 1613174110.91 epoch sec; seqNo:7972 2021-02-12T23:55:10Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.17 psi 2021-02-12T23:55:10Z- handleDepth()-Depth_Keller msg: 1613174110913 epoch sec; seqNo:8037; depth 6.31 2021-02-12T23:55:10Z- handleDvl() - RDI_Pathfinder msg: 1613174110.93 epoch sec; seqNo:3781 2021-02-12T23:55:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.90 , 111.23 , 124.62 , 82.97 2021-02-12T23:55:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.07 2021-02-12T23:55:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174110.94 epoch sec; seqNo:8037; 4058167.56 north; 591729.55 east 2021-02-12T23:55:11Z- AHRS_M2 msg: 1613174111.31 epoch sec; seqNo:7973 2021-02-12T23:55:11Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.17 psi 2021-02-12T23:55:11Z- handleDepth()-Depth_Keller msg: 1613174111316 epoch sec; seqNo:8038; depth 6.19 2021-02-12T23:55:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:11Z- handleDvl() - RDI_Pathfinder msg: 1613174111.32 epoch sec; seqNo:3782 2021-02-12T23:55:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.07 2021-02-12T23:55:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174111.33 epoch sec; seqNo:8038; 4058167.94 north; 591729.54 east 2021-02-12T23:55:11Z- AHRS_M2 msg: 1613174111.72 epoch sec; seqNo:7974 2021-02-12T23:55:11Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.17 psi 2021-02-12T23:55:11Z- handleDepth()-Depth_Keller msg: 1613174111720 epoch sec; seqNo:8039; depth 6.08 2021-02-12T23:55:11Z- handleDvl() - RDI_Pathfinder msg: 1613174111.73 epoch sec; seqNo:3783 2021-02-12T23:55:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.38 , 109.74 , 126.11 , 0.00 2021-02-12T23:55:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.01 2021-02-12T23:55:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174111.74 epoch sec; seqNo:8039; 4058168.33 north; 591730.76 east 2021-02-12T23:55:12Z- AHRS_M2 msg: 1613174112.13 epoch sec; seqNo:7975 2021-02-12T23:55:12Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.17 psi 2021-02-12T23:55:12Z- handleDepth()-Depth_Keller msg: 1613174112131 epoch sec; seqNo:8040; depth 5.94 2021-02-12T23:55:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174112.15 epoch sec; seqNo:8040; 4058168.33 north; 591730.76 east 2021-02-12T23:55:12Z- AHRS_M2 msg: 1613174112.52 epoch sec; seqNo:7976 2021-02-12T23:55:12Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.17 psi 2021-02-12T23:55:12Z- handleDepth()-Depth_Keller msg: 1613174112528 epoch sec; seqNo:8041; depth 5.81 2021-02-12T23:55:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:12Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:12Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:12Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:12Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:12Z- handleDvl() - RDI_Pathfinder msg: 1613174112.53 epoch sec; seqNo:3784 2021-02-12T23:55:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.01 2021-02-12T23:55:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174112.54 epoch sec; seqNo:8041; 4058168.71 north; 591730.76 east 2021-02-12T23:55:12Z- AHRS_M2 msg: 1613174112.93 epoch sec; seqNo:7977 2021-02-12T23:55:12Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.18 psi 2021-02-12T23:55:12Z- handleDepth()-Depth_Keller msg: 1613174112937 epoch sec; seqNo:8042; depth 5.65 2021-02-12T23:55:12Z- handleDvl() - RDI_Pathfinder msg: 1613174112.96 epoch sec; seqNo:3785 2021-02-12T23:55:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 110.62 , 129.96 , 82.36 2021-02-12T23:55:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , -0.06 2021-02-12T23:55:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174112.98 epoch sec; seqNo:8042; 4058169.09 north; 591730.75 east 2021-02-12T23:55:13Z- AHRS_M2 msg: 1613174113.34 epoch sec; seqNo:7978 2021-02-12T23:55:13Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.20 psi 2021-02-12T23:55:13Z- handleDepth()-Depth_Keller msg: 1613174113343 epoch sec; seqNo:8043; depth 5.52 2021-02-12T23:55:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174113.35 epoch sec; seqNo:8043; 4058169.09 north; 591730.75 east 2021-02-12T23:55:13Z- AHRS_M2 msg: 1613174113.74 epoch sec; seqNo:7979 2021-02-12T23:55:13Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.20 psi 2021-02-12T23:55:13Z- handleDepth()-Depth_Keller msg: 1613174113750 epoch sec; seqNo:8044; depth 5.38 2021-02-12T23:55:13Z- handleDvl() - RDI_Pathfinder msg: 1613174113.76 epoch sec; seqNo:3786 2021-02-12T23:55:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.64 , 114.00 , 127.39 , 84.26 2021-02-12T23:55:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , 0.02 2021-02-12T23:55:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174113.77 epoch sec; seqNo:8044; 4058169.47 north; 591730.75 east 2021-02-12T23:55:14Z- AHRS_M2 msg: 1613174114.14 epoch sec; seqNo:7980 2021-02-12T23:55:14Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.19 psi 2021-02-12T23:55:14Z- handleDepth()-Depth_Keller msg: 1613174114149 epoch sec; seqNo:8045; depth 5.24 2021-02-12T23:55:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174114.17 epoch sec; seqNo:8045; 4058169.85 north; 591730.74 east 2021-02-12T23:55:14Z- AHRS_M2 msg: 1613174114.54 epoch sec; seqNo:7981 2021-02-12T23:55:14Z- AHRS_M2 msg: 0.63 phi; 0.25 theta; -0.17 psi 2021-02-12T23:55:14Z- handleDepth()-Depth_Keller msg: 1613174114548 epoch sec; seqNo:8046; depth 5.10 2021-02-12T23:55:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174114.56 epoch sec; seqNo:8046; 4058170.24 north; 591731.96 east 2021-02-12T23:55:14Z- AHRS_M2 msg: 1613174114.95 epoch sec; seqNo:7982 2021-02-12T23:55:14Z- AHRS_M2 msg: 0.65 phi; 0.20 theta; -0.16 psi 2021-02-12T23:55:14Z- handleDepth()-Depth_Keller msg: 1613174114957 epoch sec; seqNo:8047; depth 4.98 2021-02-12T23:55:14Z- handleDvl() - RDI_Pathfinder msg: 1613174114.97 epoch sec; seqNo:3787 2021-02-12T23:55:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.14 , 112.45 , 122.86 , 85.68 2021-02-12T23:55:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , 0.04 2021-02-12T23:55:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174114.98 epoch sec; seqNo:8047; 4058170.62 north; 591731.96 east 2021-02-12T23:55:15Z- AHRS_M2 msg: 1613174115.35 epoch sec; seqNo:7983 2021-02-12T23:55:15Z- AHRS_M2 msg: 0.67 phi; 0.15 theta; -0.15 psi 2021-02-12T23:55:15Z- handleDepth()-Depth_Keller msg: 1613174115356 epoch sec; seqNo:8048; depth 4.84 2021-02-12T23:55:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174115.37 epoch sec; seqNo:8048; 4058170.62 north; 591731.96 east 2021-02-12T23:55:15Z- LcmTrn::handleMessages() - handling 46 messages took 0.65 ms and 133 calls... 2021-02-12T23:55:15Z- waiting for 1613174113.15, time is 1613174115.37 2021-02-12T23:55:15Z- LcmTrn::updateTrn() >>>> pose time 1613174115.37, meas time 1613174114.97 <<<< 2021-02-12T23:55:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174114.97 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:55:15Z- TerrainNav::motionUpdate() - currEstimate = (4058171, 591731.96, 4.84) 2021-02-12T23:55:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:55:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:55:15Z- TNavBF::Projecting Measurements 2021-02-12T23:55:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:55:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:55:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:55:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:55:15Z- TNavFilter::4 good beams. 2021-02-12T23:55:15Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:55:15Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:55:15Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:55:15Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:55:15Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:55:15Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:55:15Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:55:15Z- TNavBF::mmse selected from filter -1 2021-02-12T23:55:15Z- TerrainMapOctree: (vehN, vehE ) = (4058170.35, 591731.77). 2021-02-12T23:55:15Z- TerrainMapOctree: Min Distance = 3978.15. 2021-02-12T23:55:15Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:55:15Z- There is no specific good beam that all particles have in common. 2021-02-12T23:55:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:55:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.56 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 13.00 2021-02-12T23:55:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:55:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.41 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 20.23 2021-02-12T23:55:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:55:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.32 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 24.15 2021-02-12T23:55:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:55:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 11.74 2021-02-12T23:55:15Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:55:15Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:55:15Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:55:15Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:55:15Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:55:15Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:55:15Z- Filter 0 No beams used in subcloud update 2021-02-12T23:55:15Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:55:15Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:55:15Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 728.6 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:55:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174114.97 sec. 2021-02-12T23:55:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:55:15Z- TNavBF::mmse selected from filter -1 2021-02-12T23:55:15Z- variances([0], [2], and psi_berg) = 728.587614 833.015543 0.000000 2021-02-12T23:55:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:55:15Z- LcmTrn::publishEstimates() - MLE : 4058164.12 591684.46 4.84 2021-02-12T23:55:15Z- LcmTrn::publishEstimates() - MMSE : 4058170.62 591731.96 4.84 2021-02-12T23:55:15Z- LcmTrn::publishEstimates() - COVAR: 728.59 833.02 1.17 0.00 2021-02-12T23:55:15Z- AHRS_M2 msg: 1613174115.76 epoch sec; seqNo:7984 2021-02-12T23:55:15Z- AHRS_M2 msg: 0.68 phi; 0.09 theta; -0.16 psi 2021-02-12T23:55:15Z- handleDepth()-Depth_Keller msg: 1613174115761 epoch sec; seqNo:8049; depth 4.72 2021-02-12T23:55:15Z- handleDvl() - RDI_Pathfinder msg: 1613174115.77 epoch sec; seqNo:3788 2021-02-12T23:55:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.92 , 105.28 , 109.74 , 87.43 2021-02-12T23:55:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.03 , -0.03 2021-02-12T23:55:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174115.79 epoch sec; seqNo:8049; 4058171.00 north; 591731.95 east 2021-02-12T23:55:16Z- AHRS_M2 msg: 1613174116.16 epoch sec; seqNo:7985 2021-02-12T23:55:16Z- AHRS_M2 msg: 0.69 phi; 0.04 theta; -0.18 psi 2021-02-12T23:55:16Z- handleDepth()-Depth_Keller msg: 1613174116164 epoch sec; seqNo:8050; depth 4.60 2021-02-12T23:55:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:16Z- handleDvl() - RDI_Pathfinder msg: 1613174116.17 epoch sec; seqNo:3789 2021-02-12T23:55:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.03 , -0.03 2021-02-12T23:55:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174116.18 epoch sec; seqNo:8050; 4058171.38 north; 591731.95 east 2021-02-12T23:55:16Z- AHRS_M2 msg: 1613174116.57 epoch sec; seqNo:7986 2021-02-12T23:55:16Z- AHRS_M2 msg: 0.69 phi; -0.00 theta; -0.19 psi 2021-02-12T23:55:16Z- handleDepth()-Depth_Keller msg: 1613174116594 epoch sec; seqNo:8051; depth 4.50 2021-02-12T23:55:16Z- handleDvl() - RDI_Pathfinder msg: 1613174116.62 epoch sec; seqNo:3790 2021-02-12T23:55:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.35 , 97.24 , 97.24 2021-02-12T23:55:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.13 , 0.09 2021-02-12T23:55:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174116.64 epoch sec; seqNo:8051; 4058171.38 north; 591731.95 east 2021-02-12T23:55:16Z- AHRS_M2 msg: 1613174116.97 epoch sec; seqNo:7987 2021-02-12T23:55:16Z- AHRS_M2 msg: 0.69 phi; -0.04 theta; -0.20 psi 2021-02-12T23:55:16Z- handleDepth()-Depth_Keller msg: 1613174116972 epoch sec; seqNo:8052; depth 4.41 2021-02-12T23:55:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174116.99 epoch sec; seqNo:8052; 4058171.77 north; 591733.17 east 2021-02-12T23:55:17Z- AHRS_M2 msg: 1613174117.37 epoch sec; seqNo:7988 2021-02-12T23:55:17Z- AHRS_M2 msg: 0.69 phi; -0.08 theta; -0.20 psi 2021-02-12T23:55:17Z- handleDepth()-Depth_Keller msg: 1613174117376 epoch sec; seqNo:8053; depth 4.35 2021-02-12T23:55:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174117.38 epoch sec; seqNo:8053; 4058172.15 north; 591733.16 east 2021-02-12T23:55:17Z- AHRS_M2 msg: 1613174117.78 epoch sec; seqNo:7989 2021-02-12T23:55:17Z- AHRS_M2 msg: 0.70 phi; -0.11 theta; -0.19 psi 2021-02-12T23:55:17Z- handleDepth()-Depth_Keller msg: 1613174117785 epoch sec; seqNo:8054; depth 4.31 2021-02-12T23:55:17Z- handleDvl() - RDI_Pathfinder msg: 1613174117.80 epoch sec; seqNo:3791 2021-02-12T23:55:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 111.91 , 97.03 , 99.74 2021-02-12T23:55:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.04 , 0.23 2021-02-12T23:55:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174117.81 epoch sec; seqNo:8054; 4058172.15 north; 591733.16 east 2021-02-12T23:55:18Z- AHRS_M2 msg: 1613174118.18 epoch sec; seqNo:7990 2021-02-12T23:55:18Z- AHRS_M2 msg: 0.70 phi; -0.14 theta; -0.18 psi 2021-02-12T23:55:18Z- handleDepth()-Depth_Keller msg: 1613174118184 epoch sec; seqNo:8055; depth 4.28 2021-02-12T23:55:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174118.19 epoch sec; seqNo:8055; 4058172.53 north; 591733.16 east 2021-02-12T23:55:18Z- AHRS_M2 msg: 1613174118.59 epoch sec; seqNo:7991 2021-02-12T23:55:18Z- AHRS_M2 msg: 0.70 phi; -0.17 theta; -0.18 psi 2021-02-12T23:55:18Z- handleDepth()-Depth_Keller msg: 1613174118599 epoch sec; seqNo:8056; depth 4.26 2021-02-12T23:55:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:18Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:18Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:18Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:18Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:18Z- handleDvl() - RDI_Pathfinder msg: 1613174118.60 epoch sec; seqNo:3792 2021-02-12T23:55:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.04 , 0.23 2021-02-12T23:55:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174118.63 epoch sec; seqNo:8056; 4058172.53 north; 591733.16 east 2021-02-12T23:55:18Z- AHRS_M2 msg: 1613174118.99 epoch sec; seqNo:7992 2021-02-12T23:55:18Z- AHRS_M2 msg: 0.70 phi; -0.19 theta; -0.18 psi 2021-02-12T23:55:18Z- handleDepth()-Depth_Keller msg: 1613174118994 epoch sec; seqNo:8057; depth 4.26 2021-02-12T23:55:19Z- handleDvl() - RDI_Pathfinder msg: 1613174119.01 epoch sec; seqNo:3793 2021-02-12T23:55:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.17 , 107.99 , 97.17 , 106.63 2021-02-12T23:55:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.06 , 0.19 2021-02-12T23:55:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174119.02 epoch sec; seqNo:8057; 4058172.91 north; 591733.15 east 2021-02-12T23:55:19Z- AHRS_M2 msg: 1613174119.39 epoch sec; seqNo:7993 2021-02-12T23:55:19Z- AHRS_M2 msg: 0.70 phi; -0.20 theta; -0.19 psi 2021-02-12T23:55:19Z- handleDepth()-Depth_Keller msg: 1613174119396 epoch sec; seqNo:8058; depth 4.28 2021-02-12T23:55:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174119.42 epoch sec; seqNo:8058; 4058173.30 north; 591734.37 east 2021-02-12T23:55:19Z- AHRS_M2 msg: 1613174119.80 epoch sec; seqNo:7994 2021-02-12T23:55:19Z- AHRS_M2 msg: 0.70 phi; -0.21 theta; -0.20 psi 2021-02-12T23:55:19Z- handleDepth()-Depth_Keller msg: 1613174119800 epoch sec; seqNo:8059; depth 4.27 2021-02-12T23:55:19Z- handleDvl() - RDI_Pathfinder msg: 1613174119.81 epoch sec; seqNo:3794 2021-02-12T23:55:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 109.81 , 88.99 , 111.30 2021-02-12T23:55:19Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.00 , 0.14 2021-02-12T23:55:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174119.84 epoch sec; seqNo:8059; 4058173.30 north; 591734.37 east 2021-02-12T23:55:20Z- AHRS_M2 msg: 1613174120.20 epoch sec; seqNo:7995 2021-02-12T23:55:20Z- AHRS_M2 msg: 0.71 phi; -0.21 theta; -0.20 psi 2021-02-12T23:55:20Z- handleDepth()-Depth_Keller msg: 1613174120209 epoch sec; seqNo:8060; depth 4.33 2021-02-12T23:55:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174120.22 epoch sec; seqNo:8060; 4058173.68 north; 591734.37 east 2021-02-12T23:55:20Z- LcmTrn::handleMessages() - handling 43 messages took 0.66 ms and 130 calls... 2021-02-12T23:55:20Z- waiting for 1613174118.37, time is 1613174120.22 2021-02-12T23:55:20Z- LcmTrn::updateTrn() >>>> pose time 1613174120.22, meas time 1613174119.81 <<<< 2021-02-12T23:55:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174119.81 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:55:20Z- TerrainNav::motionUpdate() - currEstimate = (4058174, 591734.37, 4.33) 2021-02-12T23:55:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:55:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:55:20Z- TNavBF::Projecting Measurements 2021-02-12T23:55:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:55:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:55:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:55:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:55:20Z- TNavFilter::4 good beams. 2021-02-12T23:55:20Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:55:20Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:55:20Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:55:20Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:55:20Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:55:20Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:55:20Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:55:20Z- TNavBF::mmse selected from filter -1 2021-02-12T23:55:20Z- TerrainMapOctree: (vehN, vehE ) = (4058173.42, 591734.16). 2021-02-12T23:55:20Z- TerrainMapOctree: Min Distance = 3974.27. 2021-02-12T23:55:20Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:55:20Z- There is no specific good beam that all particles have in common. 2021-02-12T23:55:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:55:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.35 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 12.67 2021-02-12T23:55:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:55:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 19.29 2021-02-12T23:55:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:55:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 12.67 2021-02-12T23:55:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:55:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 19.82 2021-02-12T23:55:20Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:55:20Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:55:20Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:55:20Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:55:20Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:55:20Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:55:20Z- Filter 0 No beams used in subcloud update 2021-02-12T23:55:20Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:55:20Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:55:20Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 728.5 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:55:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174119.81 sec. 2021-02-12T23:55:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:55:20Z- TNavBF::mmse selected from filter -1 2021-02-12T23:55:20Z- variances([0], [2], and psi_berg) = 728.495952 833.015477 0.000000 2021-02-12T23:55:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:55:20Z- LcmTrn::publishEstimates() - MLE : 4058167.18 591686.87 4.33 2021-02-12T23:55:20Z- LcmTrn::publishEstimates() - MMSE : 4058173.68 591734.37 4.33 2021-02-12T23:55:20Z- LcmTrn::publishEstimates() - COVAR: 728.50 833.02 1.04 0.00 2021-02-12T23:55:20Z- AHRS_M2 msg: 1613174120.61 epoch sec; seqNo:7996 2021-02-12T23:55:20Z- AHRS_M2 msg: 0.72 phi; -0.22 theta; -0.19 psi 2021-02-12T23:55:20Z- handleDepth()-Depth_Keller msg: 1613174120610 epoch sec; seqNo:8061; depth 4.38 2021-02-12T23:55:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174120.62 epoch sec; seqNo:8061; 4058174.06 north; 591734.36 east 2021-02-12T23:55:21Z- AHRS_M2 msg: 1613174121.01 epoch sec; seqNo:7997 2021-02-12T23:55:21Z- AHRS_M2 msg: 0.73 phi; -0.24 theta; -0.17 psi 2021-02-12T23:55:21Z- handleDepth()-Depth_Keller msg: 1613174121022 epoch sec; seqNo:8062; depth 4.44 2021-02-12T23:55:21Z- handleDvl() - RDI_Pathfinder msg: 1613174121.03 epoch sec; seqNo:3795 2021-02-12T23:55:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.92 , 110.56 , 87.57 , 113.26 2021-02-12T23:55:21Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.02 , 0.13 2021-02-12T23:55:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174121.05 epoch sec; seqNo:8062; 4058174.43 north; 591734.36 east 2021-02-12T23:55:21Z- AHRS_M2 msg: 1613174121.41 epoch sec; seqNo:7998 2021-02-12T23:55:21Z- AHRS_M2 msg: 0.73 phi; -0.26 theta; -0.15 psi 2021-02-12T23:55:21Z- handleDepth()-Depth_Keller msg: 1613174121416 epoch sec; seqNo:8063; depth 4.51 2021-02-12T23:55:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174121.43 epoch sec; seqNo:8063; 4058174.45 north; 591735.58 east 2021-02-12T23:55:21Z- AHRS_M2 msg: 1613174121.82 epoch sec; seqNo:7999 2021-02-12T23:55:21Z- AHRS_M2 msg: 0.74 phi; -0.29 theta; -0.14 psi 2021-02-12T23:55:21Z- handleDepth()-Depth_Keller msg: 1613174121821 epoch sec; seqNo:8064; depth 4.60 2021-02-12T23:55:21Z- handleDvl() - RDI_Pathfinder msg: 1613174121.83 epoch sec; seqNo:3796 2021-02-12T23:55:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.27 , 107.85 , 86.22 , 111.91 2021-02-12T23:55:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.16 , -0.02 2021-02-12T23:55:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174121.84 epoch sec; seqNo:8064; 4058174.83 north; 591735.57 east 2021-02-12T23:55:22Z- AHRS_M2 msg: 1613174122.22 epoch sec; seqNo:8000 2021-02-12T23:55:22Z- AHRS_M2 msg: 0.74 phi; -0.31 theta; -0.14 psi 2021-02-12T23:55:22Z- handleDepth()-Depth_Keller msg: 1613174122233 epoch sec; seqNo:8065; depth 4.67 2021-02-12T23:55:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174122.24 epoch sec; seqNo:8065; 4058174.83 north; 591735.57 east 2021-02-12T23:55:22Z- AHRS_M2 msg: 1613174122.62 epoch sec; seqNo:8001 2021-02-12T23:55:22Z- AHRS_M2 msg: 0.75 phi; -0.33 theta; -0.15 psi 2021-02-12T23:55:22Z- handleDepth()-Depth_Keller msg: 1613174122629 epoch sec; seqNo:8066; depth 4.74 2021-02-12T23:55:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174122.66 epoch sec; seqNo:8066; 4058175.20 north; 591735.57 east 2021-02-12T23:55:23Z- AHRS_M2 msg: 1613174123.03 epoch sec; seqNo:8002 2021-02-12T23:55:23Z- AHRS_M2 msg: 0.75 phi; -0.34 theta; -0.16 psi 2021-02-12T23:55:23Z- handleDepth()-Depth_Keller msg: 1613174123032 epoch sec; seqNo:8067; depth 4.82 2021-02-12T23:55:23Z- handleDvl() - RDI_Pathfinder msg: 1613174123.04 epoch sec; seqNo:3797 2021-02-12T23:55:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.10 , 106.57 , 83.58 , 118.74 2021-02-12T23:55:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , 0.22 , -0.28 2021-02-12T23:55:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174123.07 epoch sec; seqNo:8067; 4058175.22 north; 591736.79 east 2021-02-12T23:55:23Z- AHRS_M2 msg: 1613174123.43 epoch sec; seqNo:8003 2021-02-12T23:55:23Z- AHRS_M2 msg: 0.75 phi; -0.35 theta; -0.18 psi 2021-02-12T23:55:23Z- handleDepth()-Depth_Keller msg: 1613174123436 epoch sec; seqNo:8068; depth 4.92 2021-02-12T23:55:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174123.45 epoch sec; seqNo:8068; 4058175.60 north; 591736.79 east 2021-02-12T23:55:23Z- AHRS_M2 msg: 1613174123.84 epoch sec; seqNo:8004 2021-02-12T23:55:23Z- AHRS_M2 msg: 0.75 phi; -0.36 theta; -0.20 psi 2021-02-12T23:55:23Z- handleDepth()-Depth_Keller msg: 1613174123841 epoch sec; seqNo:8069; depth 5.00 2021-02-12T23:55:23Z- handleDvl() - RDI_Pathfinder msg: 1613174123.85 epoch sec; seqNo:3798 2021-02-12T23:55:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.52 , 112.85 , 83.11 , 126.24 2021-02-12T23:55:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , 0.06 , -0.28 2021-02-12T23:55:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174123.86 epoch sec; seqNo:8069; 4058175.60 north; 591736.79 east 2021-02-12T23:55:24Z- AHRS_M2 msg: 1613174124.24 epoch sec; seqNo:8005 2021-02-12T23:55:24Z- AHRS_M2 msg: 0.75 phi; -0.37 theta; -0.20 psi 2021-02-12T23:55:24Z- handleDepth()-Depth_Keller msg: 1613174124250 epoch sec; seqNo:8070; depth 5.12 2021-02-12T23:55:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174124.27 epoch sec; seqNo:8070; 4058175.98 north; 591736.78 east 2021-02-12T23:55:24Z- AHRS_M2 msg: 1613174124.65 epoch sec; seqNo:8006 2021-02-12T23:55:24Z- AHRS_M2 msg: 0.73 phi; -0.37 theta; -0.21 psi 2021-02-12T23:55:24Z- handleDepth()-Depth_Keller msg: 1613174124653 epoch sec; seqNo:8071; depth 5.23 2021-02-12T23:55:24Z- handleDvl() - RDI_Pathfinder msg: 1613174124.67 epoch sec; seqNo:3799 2021-02-12T23:55:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.34 , 114.14 , 0.00 , 133.48 2021-02-12T23:55:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.06 , -0.17 2021-02-12T23:55:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174124.68 epoch sec; seqNo:8071; 4058176.35 north; 591736.78 east 2021-02-12T23:55:25Z- AHRS_M2 msg: 1613174125.05 epoch sec; seqNo:8007 2021-02-12T23:55:25Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.22 psi 2021-02-12T23:55:25Z- handleDepth()-Depth_Keller msg: 1613174125053 epoch sec; seqNo:8072; depth 5.35 2021-02-12T23:55:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174125.07 epoch sec; seqNo:8072; 4058176.35 north; 591736.78 east 2021-02-12T23:55:25Z- AHRS_M2 msg: 1613174125.45 epoch sec; seqNo:8008 2021-02-12T23:55:25Z- AHRS_M2 msg: 0.71 phi; -0.36 theta; -0.24 psi 2021-02-12T23:55:25Z- handleDepth()-Depth_Keller msg: 1613174125456 epoch sec; seqNo:8073; depth 5.44 2021-02-12T23:55:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174125.46 epoch sec; seqNo:8073; 4058176.73 north; 591736.78 east 2021-02-12T23:55:25Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-12T23:55:25Z- waiting for 1613174123.22, time is 1613174125.46 2021-02-12T23:55:25Z- LcmTrn::updateTrn() >>>> pose time 1613174125.46, meas time 1613174124.67 <<<< 2021-02-12T23:55:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174124.67 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:55:25Z- TerrainNav::motionUpdate() - currEstimate = (4058177, 591736.78, 5.44) 2021-02-12T23:55:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:55:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:55:25Z- TNavBF::Projecting Measurements 2021-02-12T23:55:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:55:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:55:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:55:25Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:55:25Z- TNavFilter::3 good beams. 2021-02-12T23:55:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:55:25Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:55:25Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:55:25Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:55:25Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:55:25Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:55:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:55:25Z- TNavBF::mmse selected from filter -1 2021-02-12T23:55:25Z- TerrainMapOctree: (vehN, vehE ) = (4058176.27, 591736.41). 2021-02-12T23:55:25Z- TerrainMapOctree: Min Distance = 3970.64. 2021-02-12T23:55:25Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:55:25Z- There is no specific good beam that all particles have in common. 2021-02-12T23:55:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:55:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 13.06 2021-02-12T23:55:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:55:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 20.84 2021-02-12T23:55:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:55:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.034 ExpectedMeasDiff: nan currMeas.covariance: 28.50 2021-02-12T23:55:25Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:55:25Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:55:25Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:55:25Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:55:25Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:55:25Z- Filter 0 No beams used in subcloud update 2021-02-12T23:55:25Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:55:25Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:55:25Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 728.4 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:55:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174124.67 sec. 2021-02-12T23:55:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:55:25Z- TNavBF::mmse selected from filter -1 2021-02-12T23:55:25Z- variances([0], [2], and psi_berg) = 728.399762 833.015408 0.000000 2021-02-12T23:55:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:55:25Z- LcmTrn::publishEstimates() - MLE : 4058170.23 591690.28 5.44 2021-02-12T23:55:25Z- LcmTrn::publishEstimates() - MMSE : 4058176.73 591736.78 5.44 2021-02-12T23:55:25Z- LcmTrn::publishEstimates() - COVAR: 728.40 833.02 1.06 0.00 2021-02-12T23:55:25Z- AHRS_M2 msg: 1613174125.86 epoch sec; seqNo:8009 2021-02-12T23:55:25Z- AHRS_M2 msg: 0.69 phi; -0.36 theta; -0.25 psi 2021-02-12T23:55:25Z- handleDepth()-Depth_Keller msg: 1613174125865 epoch sec; seqNo:8074; depth 5.57 2021-02-12T23:55:25Z- handleDvl() - RDI_Pathfinder msg: 1613174125.88 epoch sec; seqNo:3800 2021-02-12T23:55:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 122.52 , 82.36 , 125.50 2021-02-12T23:55:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.20 , 0.01 2021-02-12T23:55:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174125.90 epoch sec; seqNo:8074; 4058177.13 north; 591737.99 east 2021-02-12T23:55:26Z- handleDepth()-Depth_Keller msg: 1613174126281 epoch sec; seqNo:8075; depth 5.70 2021-02-12T23:55:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174126.29 epoch sec; seqNo:8075; 4058177.13 north; 591737.99 east 2021-02-12T23:55:26Z- AHRS_M2 msg: 1613174126.68 epoch sec; seqNo:8010 2021-02-12T23:55:26Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.26 psi 2021-02-12T23:55:26Z- handleDepth()-Depth_Keller msg: 1613174126693 epoch sec; seqNo:8076; depth 5.83 2021-02-12T23:55:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174126.70 epoch sec; seqNo:8076; 4058177.50 north; 591737.99 east 2021-02-12T23:55:27Z- AHRS_M2 msg: 1613174127.07 epoch sec; seqNo:8011 2021-02-12T23:55:27Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.25 psi 2021-02-12T23:55:27Z- handleDepth()-Depth_Keller msg: 1613174127077 epoch sec; seqNo:8077; depth 5.96 2021-02-12T23:55:27Z- handleDvl() - RDI_Pathfinder msg: 1613174127.10 epoch sec; seqNo:3801 2021-02-12T23:55:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.51 , 122.79 , 84.12 , 134.69 2021-02-12T23:55:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.09 , 0.27 2021-02-12T23:55:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174127.11 epoch sec; seqNo:8077; 4058177.88 north; 591737.98 east 2021-02-12T23:55:27Z- AHRS_M2 msg: 1613174127.47 epoch sec; seqNo:8012 2021-02-12T23:55:27Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.24 psi 2021-02-12T23:55:27Z- handleDepth()-Depth_Keller msg: 1613174127476 epoch sec; seqNo:8078; depth 6.10 2021-02-12T23:55:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174127.49 epoch sec; seqNo:8078; 4058177.88 north; 591737.98 east 2021-02-12T23:55:27Z- AHRS_M2 msg: 1613174127.88 epoch sec; seqNo:8013 2021-02-12T23:55:27Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.23 psi 2021-02-12T23:55:27Z- handleDepth()-Depth_Keller msg: 1613174127885 epoch sec; seqNo:8079; depth 6.23 2021-02-12T23:55:27Z- handleDvl() - RDI_Pathfinder msg: 1613174127.90 epoch sec; seqNo:3802 2021-02-12T23:55:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.80 , 119.55 , 83.85 , 137.40 2021-02-12T23:55:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.08 , 0.22 2021-02-12T23:55:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174127.91 epoch sec; seqNo:8079; 4058178.26 north; 591737.98 east 2021-02-12T23:55:28Z- AHRS_M2 msg: 1613174128.28 epoch sec; seqNo:8014 2021-02-12T23:55:28Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.23 psi 2021-02-12T23:55:28Z- handleDepth()-Depth_Keller msg: 1613174128286 epoch sec; seqNo:8080; depth 6.36 2021-02-12T23:55:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:28Z- handleDvl() - RDI_Pathfinder msg: 1613174128.29 epoch sec; seqNo:3803 2021-02-12T23:55:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.08 , 0.22 2021-02-12T23:55:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174128.30 epoch sec; seqNo:8080; 4058178.26 north; 591737.98 east 2021-02-12T23:55:28Z- AHRS_M2 msg: 1613174128.69 epoch sec; seqNo:8015 2021-02-12T23:55:28Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.24 psi 2021-02-12T23:55:28Z- handleDepth()-Depth_Keller msg: 1613174128693 epoch sec; seqNo:8081; depth 6.49 2021-02-12T23:55:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174128.70 epoch sec; seqNo:8081; 4058178.65 north; 591739.20 east 2021-02-12T23:55:29Z- AHRS_M2 msg: 1613174129.09 epoch sec; seqNo:8016 2021-02-12T23:55:29Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.24 psi 2021-02-12T23:55:29Z- handleDepth()-Depth_Keller msg: 1613174129096 epoch sec; seqNo:8082; depth 6.63 2021-02-12T23:55:29Z- handleDvl() - RDI_Pathfinder msg: 1613174129.11 epoch sec; seqNo:3804 2021-02-12T23:55:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.00 , 116.78 , 82.57 , 133.14 2021-02-12T23:55:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.10 , 0.06 2021-02-12T23:55:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174129.14 epoch sec; seqNo:8082; 4058179.03 north; 591739.19 east 2021-02-12T23:55:29Z- AHRS_M2 msg: 1613174129.49 epoch sec; seqNo:8017 2021-02-12T23:55:29Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.25 psi 2021-02-12T23:55:29Z- handleDepth()-Depth_Keller msg: 1613174129505 epoch sec; seqNo:8083; depth 6.77 2021-02-12T23:55:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:29Z- handleDvl() - RDI_Pathfinder msg: 1613174129.51 epoch sec; seqNo:3805 2021-02-12T23:55:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.10 , 0.06 2021-02-12T23:55:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174129.52 epoch sec; seqNo:8083; 4058179.03 north; 591739.19 east 2021-02-12T23:55:29Z- AHRS_M2 msg: 1613174129.91 epoch sec; seqNo:8018 2021-02-12T23:55:29Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.25 psi 2021-02-12T23:55:29Z- handleDepth()-Depth_Keller msg: 1613174129918 epoch sec; seqNo:8084; depth 6.91 2021-02-12T23:55:29Z- handleDvl() - RDI_Pathfinder msg: 1613174129.93 epoch sec; seqNo:3806 2021-02-12T23:55:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.72 , 115.49 , 84.26 , 130.37 2021-02-12T23:55:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.04 , -0.05 2021-02-12T23:55:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174129.94 epoch sec; seqNo:8084; 4058179.41 north; 591739.19 east 2021-02-12T23:55:30Z- AHRS_M2 msg: 1613174130.30 epoch sec; seqNo:8019 2021-02-12T23:55:30Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.24 psi 2021-02-12T23:55:30Z- handleDepth()-Depth_Keller msg: 1613174130312 epoch sec; seqNo:8085; depth 7.06 2021-02-12T23:55:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174130.33 epoch sec; seqNo:8085; 4058179.79 north; 591739.18 east 2021-02-12T23:55:30Z- AHRS_M2 msg: 1613174130.71 epoch sec; seqNo:8020 2021-02-12T23:55:30Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.23 psi 2021-02-12T23:55:30Z- LcmTrn::handleMessages() - handling 43 messages took 0.67 ms and 130 calls... 2021-02-12T23:55:30Z- waiting for 1613174128.46, time is 1613174130.71 2021-02-12T23:55:30Z- LcmTrn::updateTrn() >>>> pose time 1613174130.71, meas time 1613174129.93 <<<< 2021-02-12T23:55:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174129.93 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:55:30Z- TerrainNav::motionUpdate() - currEstimate = (4058180, 591739.18, 7.06) 2021-02-12T23:55:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:55:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:55:30Z- TNavBF::Projecting Measurements 2021-02-12T23:55:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:55:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:55:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:55:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:55:30Z- TNavFilter::4 good beams. 2021-02-12T23:55:30Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:55:30Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:55:30Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:55:30Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:55:30Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:55:30Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:55:30Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:55:30Z- TNavBF::mmse selected from filter -1 2021-02-12T23:55:30Z- TerrainMapOctree: (vehN, vehE ) = (4058179.33, 591738.82). 2021-02-12T23:55:30Z- TerrainMapOctree: Min Distance = 3966.74. 2021-02-12T23:55:30Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:55:30Z- There is no specific good beam that all particles have in common. 2021-02-12T23:55:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:55:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 12.59 2021-02-12T23:55:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:55:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 21.34 2021-02-12T23:55:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:55:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 11.36 2021-02-12T23:55:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:55:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.033 ExpectedMeasDiff: nan currMeas.covariance: 27.19 2021-02-12T23:55:30Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:55:30Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:55:30Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:55:30Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:55:30Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:55:30Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:55:30Z- Filter 0 No beams used in subcloud update 2021-02-12T23:55:30Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:55:30Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:55:30Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 728.3 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:55:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174129.93 sec. 2021-02-12T23:55:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:55:30Z- TNavBF::mmse selected from filter -1 2021-02-12T23:55:30Z- variances([0], [2], and psi_berg) = 728.303806 833.015339 0.000000 2021-02-12T23:55:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:55:30Z- LcmTrn::publishEstimates() - MLE : 4058173.29 591692.68 7.06 2021-02-12T23:55:30Z- LcmTrn::publishEstimates() - MMSE : 4058179.79 591739.18 7.06 2021-02-12T23:55:30Z- LcmTrn::publishEstimates() - COVAR: 728.30 833.02 0.99 0.00 2021-02-12T23:55:30Z- handleDepth()-Depth_Keller msg: 1613174130718 epoch sec; seqNo:8086; depth 7.19 2021-02-12T23:55:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:30Z- handleDvl() - RDI_Pathfinder msg: 1613174130.72 epoch sec; seqNo:3807 2021-02-12T23:55:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.04 , -0.05 2021-02-12T23:55:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174130.73 epoch sec; seqNo:8086; 4058180.18 north; 591740.40 east 2021-02-12T23:55:31Z- AHRS_M2 msg: 1613174131.12 epoch sec; seqNo:8021 2021-02-12T23:55:31Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.23 psi 2021-02-12T23:55:31Z- handleDepth()-Depth_Keller msg: 1613174131124 epoch sec; seqNo:8087; depth 7.32 2021-02-12T23:55:31Z- handleDvl() - RDI_Pathfinder msg: 1613174131.14 epoch sec; seqNo:3808 2021-02-12T23:55:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.30 , 115.56 , 82.84 , 121.51 2021-02-12T23:55:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , -0.07 2021-02-12T23:55:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174131.15 epoch sec; seqNo:8087; 4058180.18 north; 591740.40 east 2021-02-12T23:55:31Z- AHRS_M2 msg: 1613174131.52 epoch sec; seqNo:8022 2021-02-12T23:55:31Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.22 psi 2021-02-12T23:55:31Z- handleDepth()-Depth_Keller msg: 1613174131520 epoch sec; seqNo:8088; depth 7.45 2021-02-12T23:55:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174131.54 epoch sec; seqNo:8088; 4058180.56 north; 591740.40 east 2021-02-12T23:55:31Z- AHRS_M2 msg: 1613174131.92 epoch sec; seqNo:8023 2021-02-12T23:55:31Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.21 psi 2021-02-12T23:55:31Z- handleDepth()-Depth_Keller msg: 1613174131923 epoch sec; seqNo:8089; depth 7.58 2021-02-12T23:55:31Z- handleDvl() - RDI_Pathfinder msg: 1613174131.93 epoch sec; seqNo:3809 2021-02-12T23:55:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 112.85 , 83.11 , 121.78 2021-02-12T23:55:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , -0.01 2021-02-12T23:55:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174131.96 epoch sec; seqNo:8089; 4058180.94 north; 591740.39 east 2021-02-12T23:55:32Z- AHRS_M2 msg: 1613174132.32 epoch sec; seqNo:8024 2021-02-12T23:55:32Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.20 psi 2021-02-12T23:55:32Z- handleDepth()-Depth_Keller msg: 1613174132333 epoch sec; seqNo:8090; depth 7.69 2021-02-12T23:55:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174132.35 epoch sec; seqNo:8090; 4058181.32 north; 591740.39 east 2021-02-12T23:55:32Z- AHRS_M2 msg: 1613174132.73 epoch sec; seqNo:8025 2021-02-12T23:55:32Z- AHRS_M2 msg: 0.57 phi; -0.30 theta; -0.19 psi 2021-02-12T23:55:32Z- handleDepth()-Depth_Keller msg: 1613174132739 epoch sec; seqNo:8091; depth 7.81 2021-02-12T23:55:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174132.75 epoch sec; seqNo:8091; 4058181.70 north; 591740.38 east 2021-02-12T23:55:33Z- AHRS_M2 msg: 1613174133.13 epoch sec; seqNo:8026 2021-02-12T23:55:33Z- AHRS_M2 msg: 0.57 phi; -0.29 theta; -0.19 psi 2021-02-12T23:55:33Z- handleDepth()-Depth_Keller msg: 1613174133140 epoch sec; seqNo:8092; depth 7.91 2021-02-12T23:55:33Z- handleDvl() - RDI_Pathfinder msg: 1613174133.16 epoch sec; seqNo:3810 2021-02-12T23:55:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 109.88 , 83.11 , 118.80 2021-02-12T23:55:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.01 , 0.02 2021-02-12T23:55:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174133.17 epoch sec; seqNo:8092; 4058181.70 north; 591740.38 east 2021-02-12T23:55:33Z- AHRS_M2 msg: 1613174133.54 epoch sec; seqNo:8027 2021-02-12T23:55:33Z- AHRS_M2 msg: 0.57 phi; -0.29 theta; -0.19 psi 2021-02-12T23:55:33Z- handleDepth()-Depth_Keller msg: 1613174133540 epoch sec; seqNo:8093; depth 8.03 2021-02-12T23:55:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174133.55 epoch sec; seqNo:8093; 4058182.09 north; 591741.60 east 2021-02-12T23:55:33Z- AHRS_M2 msg: 1613174133.94 epoch sec; seqNo:8028 2021-02-12T23:55:33Z- AHRS_M2 msg: 0.57 phi; -0.28 theta; -0.18 psi 2021-02-12T23:55:33Z- handleDepth()-Depth_Keller msg: 1613174133944 epoch sec; seqNo:8094; depth 8.14 2021-02-12T23:55:33Z- handleDvl() - RDI_Pathfinder msg: 1613174133.96 epoch sec; seqNo:3811 2021-02-12T23:55:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 105.15 , 82.16 , 117.32 2021-02-12T23:55:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.09 , -0.04 2021-02-12T23:55:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174133.97 epoch sec; seqNo:8094; 4058182.47 north; 591741.60 east 2021-02-12T23:55:34Z- AHRS_M2 msg: 1613174134.35 epoch sec; seqNo:8029 2021-02-12T23:55:34Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.17 psi 2021-02-12T23:55:34Z- handleDepth()-Depth_Keller msg: 1613174134349 epoch sec; seqNo:8095; depth 8.24 2021-02-12T23:55:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:34Z- handleDvl() - RDI_Pathfinder msg: 1613174134.35 epoch sec; seqNo:3812 2021-02-12T23:55:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.09 , -0.04 2021-02-12T23:55:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174134.36 epoch sec; seqNo:8095; 4058182.47 north; 591741.60 east 2021-02-12T23:55:34Z- AHRS_M2 msg: 1613174134.75 epoch sec; seqNo:8030 2021-02-12T23:55:34Z- AHRS_M2 msg: 0.59 phi; -0.27 theta; -0.16 psi 2021-02-12T23:55:34Z- handleDepth()-Depth_Keller msg: 1613174134752 epoch sec; seqNo:8096; depth 8.35 2021-02-12T23:55:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174134.77 epoch sec; seqNo:8096; 4058182.85 north; 591741.59 east 2021-02-12T23:55:35Z- AHRS_M2 msg: 1613174135.15 epoch sec; seqNo:8031 2021-02-12T23:55:35Z- AHRS_M2 msg: 0.60 phi; -0.27 theta; -0.15 psi 2021-02-12T23:55:35Z- handleDepth()-Depth_Keller msg: 1613174135158 epoch sec; seqNo:8097; depth 8.46 2021-02-12T23:55:35Z- handleDvl() - RDI_Pathfinder msg: 1613174135.17 epoch sec; seqNo:3813 2021-02-12T23:55:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.70 , 105.28 , 82.30 , 110.69 2021-02-12T23:55:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.10 , -0.15 2021-02-12T23:55:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174135.19 epoch sec; seqNo:8097; 4058183.23 north; 591741.59 east 2021-02-12T23:55:35Z- AHRS_M2 msg: 1613174135.56 epoch sec; seqNo:8032 2021-02-12T23:55:35Z- AHRS_M2 msg: 0.60 phi; -0.27 theta; -0.15 psi 2021-02-12T23:55:35Z- handleDepth()-Depth_Keller msg: 1613174135560 epoch sec; seqNo:8098; depth 8.56 2021-02-12T23:55:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:35Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:35Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:35Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:35Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:35Z- handleDvl() - RDI_Pathfinder msg: 1613174135.56 epoch sec; seqNo:3814 2021-02-12T23:55:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.10 , -0.15 2021-02-12T23:55:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174135.57 epoch sec; seqNo:8098; 4058183.60 north; 591741.59 east 2021-02-12T23:55:35Z- LcmTrn::handleMessages() - handling 46 messages took 0.62 ms and 134 calls... 2021-02-12T23:55:35Z- waiting for 1613174133.71, time is 1613174135.57 2021-02-12T23:55:35Z- LcmTrn::updateTrn() equating pose 1613174135.57 and meas times 1613174135.57 2021-02-12T23:55:35Z- LcmTrn::updateTrn() >>>> pose time 1613174135.57, meas time 1613174135.57 <<<< 2021-02-12T23:55:35Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174135.57, ping # 0. 2021-02-12T23:55:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174135.57 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:55:35Z- TerrainNav::motionUpdate() - currEstimate = (4058184, 591741.59, 8.56) 2021-02-12T23:55:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:55:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:55:35Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:55:35Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:55:35Z- TNavBF::Projecting Measurements 2021-02-12T23:55:35Z- TNavFilter::Measurement from time = 1613174135.57 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:55:35Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:55:35Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:55:35Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:55:35Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:55:35Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:55:35Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:55:35Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:55:35Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 728.2 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:55:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:55:35Z- TNavBF::mmse selected from filter -1 2021-02-12T23:55:35Z- variances([0], [2], and psi_berg) = 728.224975 833.015283 0.000000 2021-02-12T23:55:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:55:35Z- LcmTrn::publishEstimates() - MLE : 4058177.10 591695.09 8.56 2021-02-12T23:55:35Z- LcmTrn::publishEstimates() - MMSE : 4058183.60 591741.59 8.56 2021-02-12T23:55:35Z- LcmTrn::publishEstimates() - COVAR: 728.22 833.02 1.22 0.00 2021-02-12T23:55:35Z- AHRS_M2 msg: 1613174135.96 epoch sec; seqNo:8033 2021-02-12T23:55:35Z- AHRS_M2 msg: 0.61 phi; -0.27 theta; -0.16 psi 2021-02-12T23:55:35Z- handleDepth()-Depth_Keller msg: 1613174135966 epoch sec; seqNo:8099; depth 8.65 2021-02-12T23:55:35Z- handleDvl() - RDI_Pathfinder msg: 1613174135.98 epoch sec; seqNo:3815 2021-02-12T23:55:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.84 , 104.07 , 82.43 , 116.24 2021-02-12T23:55:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.20 2021-02-12T23:55:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174135.99 epoch sec; seqNo:8099; 4058183.62 north; 591742.81 east 2021-02-12T23:55:36Z- AHRS_M2 msg: 1613174136.36 epoch sec; seqNo:8034 2021-02-12T23:55:36Z- AHRS_M2 msg: 0.61 phi; -0.27 theta; -0.17 psi 2021-02-12T23:55:36Z- handleDepth()-Depth_Keller msg: 1613174136368 epoch sec; seqNo:8100; depth 8.74 2021-02-12T23:55:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174136.39 epoch sec; seqNo:8100; 4058184.00 north; 591742.80 east 2021-02-12T23:55:36Z- AHRS_M2 msg: 1613174136.77 epoch sec; seqNo:8035 2021-02-12T23:55:36Z- AHRS_M2 msg: 0.61 phi; -0.27 theta; -0.17 psi 2021-02-12T23:55:36Z- handleDepth()-Depth_Keller msg: 1613174136772 epoch sec; seqNo:8101; depth 8.83 2021-02-12T23:55:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:36Z- handleDvl() - RDI_Pathfinder msg: 1613174136.77 epoch sec; seqNo:3816 2021-02-12T23:55:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.20 2021-02-12T23:55:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174136.78 epoch sec; seqNo:8101; 4058184.38 north; 591742.80 east 2021-02-12T23:55:37Z- AHRS_M2 msg: 1613174137.17 epoch sec; seqNo:8036 2021-02-12T23:55:37Z- AHRS_M2 msg: 0.61 phi; -0.27 theta; -0.16 psi 2021-02-12T23:55:37Z- handleDepth()-Depth_Keller msg: 1613174137176 epoch sec; seqNo:8102; depth 8.93 2021-02-12T23:55:37Z- handleDvl() - RDI_Pathfinder msg: 1613174137.19 epoch sec; seqNo:3817 2021-02-12T23:55:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.07 , 105.35 , 81.01 , 113.46 2021-02-12T23:55:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.20 , -0.05 2021-02-12T23:55:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174137.20 epoch sec; seqNo:8102; 4058184.75 north; 591742.79 east 2021-02-12T23:55:37Z- AHRS_M2 msg: 1613174137.58 epoch sec; seqNo:8037 2021-02-12T23:55:37Z- AHRS_M2 msg: 0.61 phi; -0.28 theta; -0.16 psi 2021-02-12T23:55:37Z- handleDepth()-Depth_Keller msg: 1613174137581 epoch sec; seqNo:8103; depth 9.04 2021-02-12T23:55:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174137.59 epoch sec; seqNo:8103; 4058185.13 north; 591742.79 east 2021-02-12T23:55:37Z- AHRS_M2 msg: 1613174137.98 epoch sec; seqNo:8038 2021-02-12T23:55:37Z- AHRS_M2 msg: 0.61 phi; -0.28 theta; -0.16 psi 2021-02-12T23:55:37Z- handleDepth()-Depth_Keller msg: 1613174137994 epoch sec; seqNo:8104; depth 9.14 2021-02-12T23:55:38Z- handleDvl() - RDI_Pathfinder msg: 1613174138.01 epoch sec; seqNo:3818 2021-02-12T23:55:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.13 , 81.15 , 112.25 2021-02-12T23:55:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.18 , 0.17 2021-02-12T23:55:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174138.02 epoch sec; seqNo:8104; 4058185.51 north; 591742.79 east 2021-02-12T23:55:38Z- AHRS_M2 msg: 1613174138.38 epoch sec; seqNo:8039 2021-02-12T23:55:38Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.16 psi 2021-02-12T23:55:38Z- handleDepth()-Depth_Keller msg: 1613174138388 epoch sec; seqNo:8105; depth 9.22 2021-02-12T23:55:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174138.40 epoch sec; seqNo:8105; 4058185.51 north; 591742.79 east 2021-02-12T23:55:38Z- AHRS_M2 msg: 1613174138.79 epoch sec; seqNo:8040 2021-02-12T23:55:38Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.16 psi 2021-02-12T23:55:38Z- handleDepth()-Depth_Keller msg: 1613174138792 epoch sec; seqNo:8106; depth 9.33 2021-02-12T23:55:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174138.80 epoch sec; seqNo:8106; 4058185.89 north; 591742.78 east 2021-02-12T23:55:39Z- AHRS_M2 msg: 1613174139.19 epoch sec; seqNo:8041 2021-02-12T23:55:39Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.16 psi 2021-02-12T23:55:39Z- handleDepth()-Depth_Keller msg: 1613174139198 epoch sec; seqNo:8107; depth 9.43 2021-02-12T23:55:39Z- handleDvl() - RDI_Pathfinder msg: 1613174139.21 epoch sec; seqNo:3819 2021-02-12T23:55:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 102.51 , 80.88 , 113.33 2021-02-12T23:55:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.02 , 0.22 2021-02-12T23:55:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174139.22 epoch sec; seqNo:8107; 4058186.28 north; 591744.00 east 2021-02-12T23:55:39Z- AHRS_M2 msg: 1613174139.60 epoch sec; seqNo:8042 2021-02-12T23:55:39Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.15 psi 2021-02-12T23:55:39Z- handleDepth()-Depth_Keller msg: 1613174139600 epoch sec; seqNo:8108; depth 9.52 2021-02-12T23:55:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174139.62 epoch sec; seqNo:8108; 4058186.28 north; 591744.00 east 2021-02-12T23:55:40Z- AHRS_M2 msg: 1613174140.00 epoch sec; seqNo:8043 2021-02-12T23:55:40Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.14 psi 2021-02-12T23:55:40Z- handleDepth()-Depth_Keller msg: 1613174140004 epoch sec; seqNo:8109; depth 9.62 2021-02-12T23:55:40Z- handleDvl() - RDI_Pathfinder msg: 1613174140.02 epoch sec; seqNo:3820 2021-02-12T23:55:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.43 , 78.88 , 112.25 2021-02-12T23:55:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.13 , 0.14 2021-02-12T23:55:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174140.05 epoch sec; seqNo:8109; 4058186.66 north; 591743.99 east 2021-02-12T23:55:40Z- AHRS_M2 msg: 1613174140.40 epoch sec; seqNo:8044 2021-02-12T23:55:40Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.14 psi 2021-02-12T23:55:40Z- handleDepth()-Depth_Keller msg: 1613174140408 epoch sec; seqNo:8110; depth 9.72 2021-02-12T23:55:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:40Z- handleDvl() - RDI_Pathfinder msg: 1613174140.41 epoch sec; seqNo:3821 2021-02-12T23:55:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.13 , 0.14 2021-02-12T23:55:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174140.42 epoch sec; seqNo:8110; 4058186.66 north; 591743.99 east 2021-02-12T23:55:40Z- AHRS_M2 msg: 1613174140.81 epoch sec; seqNo:8045 2021-02-12T23:55:40Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.15 psi 2021-02-12T23:55:40Z- handleDepth()-Depth_Keller msg: 1613174140814 epoch sec; seqNo:8111; depth 9.83 2021-02-12T23:55:40Z- handleDvl() - RDI_Pathfinder msg: 1613174140.83 epoch sec; seqNo:3822 2021-02-12T23:55:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:40Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:55:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174140.84 epoch sec; seqNo:8111; 4058187.04 north; 591743.99 east 2021-02-12T23:55:40Z- LcmTrn::handleMessages() - handling 47 messages took 0.69 ms and 133 calls... 2021-02-12T23:55:40Z- waiting for 1613174138.57, time is 1613174140.84 2021-02-12T23:55:40Z- LcmTrn::updateTrn() equating pose 1613174140.84 and meas times 1613174140.84 2021-02-12T23:55:40Z- LcmTrn::updateTrn() >>>> pose time 1613174140.84, meas time 1613174140.84 <<<< 2021-02-12T23:55:40Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174140.84, ping # 0. 2021-02-12T23:55:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174140.84 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:55:40Z- TerrainNav::motionUpdate() - currEstimate = (4058187, 591743.99, 9.83) 2021-02-12T23:55:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:55:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:55:40Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:55:40Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:55:40Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174140.84 sec.; No relevant navigation data available 2021-02-12T23:55:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:55:40Z- TNavBF::mmse selected from filter -1 2021-02-12T23:55:40Z- variances([0], [2], and psi_berg) = 728.149005 833.015229 0.000000 2021-02-12T23:55:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:55:40Z- LcmTrn::publishEstimates() - MLE : 4058180.54 591697.49 9.83 2021-02-12T23:55:40Z- LcmTrn::publishEstimates() - MMSE : 4058187.04 591743.99 9.83 2021-02-12T23:55:40Z- LcmTrn::publishEstimates() - COVAR: 728.15 833.02 1.44 0.00 2021-02-12T23:55:41Z- AHRS_M2 msg: 1613174141.21 epoch sec; seqNo:8046 2021-02-12T23:55:41Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.15 psi 2021-02-12T23:55:41Z- handleDepth()-Depth_Keller msg: 1613174141223 epoch sec; seqNo:8112; depth 9.92 2021-02-12T23:55:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174141.23 epoch sec; seqNo:8112; 4058187.42 north; 591743.99 east 2021-02-12T23:55:41Z- AHRS_M2 msg: 1613174141.62 epoch sec; seqNo:8047 2021-02-12T23:55:41Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.14 psi 2021-02-12T23:55:41Z- handleDepth()-Depth_Keller msg: 1613174141620 epoch sec; seqNo:8113; depth 10.02 2021-02-12T23:55:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:41Z- handleDvl() - RDI_Pathfinder msg: 1613174141.62 epoch sec; seqNo:3823 2021-02-12T23:55:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:41Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:55:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174141.63 epoch sec; seqNo:8113; 4058187.43 north; 591745.21 east 2021-02-12T23:55:42Z- AHRS_M2 msg: 1613174142.02 epoch sec; seqNo:8048 2021-02-12T23:55:42Z- AHRS_M2 msg: 0.66 phi; -0.30 theta; -0.14 psi 2021-02-12T23:55:42Z- handleDepth()-Depth_Keller msg: 1613174142026 epoch sec; seqNo:8114; depth 10.13 2021-02-12T23:55:42Z- handleDvl() - RDI_Pathfinder msg: 1613174142.04 epoch sec; seqNo:3824 2021-02-12T23:55:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.77 , 102.65 , 79.66 , 116.17 2021-02-12T23:55:42Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.11 , -0.05 2021-02-12T23:55:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174142.05 epoch sec; seqNo:8114; 4058187.81 north; 591745.20 east 2021-02-12T23:55:42Z- AHRS_M2 msg: 1613174142.42 epoch sec; seqNo:8049 2021-02-12T23:55:42Z- AHRS_M2 msg: 0.66 phi; -0.30 theta; -0.14 psi 2021-02-12T23:55:42Z- handleDepth()-Depth_Keller msg: 1613174142429 epoch sec; seqNo:8115; depth 10.26 2021-02-12T23:55:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174142.45 epoch sec; seqNo:8115; 4058188.19 north; 591745.20 east 2021-02-12T23:55:42Z- AHRS_M2 msg: 1613174142.83 epoch sec; seqNo:8050 2021-02-12T23:55:42Z- AHRS_M2 msg: 0.67 phi; -0.30 theta; -0.14 psi 2021-02-12T23:55:42Z- handleDepth()-Depth_Keller msg: 1613174142832 epoch sec; seqNo:8116; depth 10.36 2021-02-12T23:55:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:42Z- handleDvl() - RDI_Pathfinder msg: 1613174142.83 epoch sec; seqNo:3825 2021-02-12T23:55:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:42Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.11 , -0.05 2021-02-12T23:55:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174142.85 epoch sec; seqNo:8116; 4058188.19 north; 591745.20 east 2021-02-12T23:55:43Z- AHRS_M2 msg: 1613174143.23 epoch sec; seqNo:8051 2021-02-12T23:55:43Z- AHRS_M2 msg: 0.68 phi; -0.30 theta; -0.15 psi 2021-02-12T23:55:43Z- handleDepth()-Depth_Keller msg: 1613174143236 epoch sec; seqNo:8117; depth 10.49 2021-02-12T23:55:43Z- handleDvl() - RDI_Pathfinder msg: 1613174143.25 epoch sec; seqNo:3826 2021-02-12T23:55:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 101.36 , 79.73 , 113.53 2021-02-12T23:55:43Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.01 , -0.09 2021-02-12T23:55:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174143.26 epoch sec; seqNo:8117; 4058188.58 north; 591746.42 east 2021-02-12T23:55:43Z- AHRS_M2 msg: 1613174143.64 epoch sec; seqNo:8052 2021-02-12T23:55:43Z- AHRS_M2 msg: 0.68 phi; -0.30 theta; -0.15 psi 2021-02-12T23:55:43Z- handleDepth()-Depth_Keller msg: 1613174143644 epoch sec; seqNo:8118; depth 10.58 2021-02-12T23:55:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174143.65 epoch sec; seqNo:8118; 4058188.96 north; 591746.41 east 2021-02-12T23:55:44Z- AHRS_M2 msg: 1613174144.04 epoch sec; seqNo:8053 2021-02-12T23:55:44Z- AHRS_M2 msg: 0.68 phi; -0.31 theta; -0.15 psi 2021-02-12T23:55:44Z- handleDepth()-Depth_Keller msg: 1613174144049 epoch sec; seqNo:8119; depth 10.70 2021-02-12T23:55:44Z- handleDvl() - RDI_Pathfinder msg: 1613174144.07 epoch sec; seqNo:3827 2021-02-12T23:55:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 101.50 , 0.00 , 113.67 2021-02-12T23:55:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , -0.03 2021-02-12T23:55:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174144.08 epoch sec; seqNo:8119; 4058189.34 north; 591746.41 east 2021-02-12T23:55:44Z- AHRS_M2 msg: 1613174144.44 epoch sec; seqNo:8054 2021-02-12T23:55:44Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.16 psi 2021-02-12T23:55:44Z- handleDepth()-Depth_Keller msg: 1613174144448 epoch sec; seqNo:8120; depth 10.81 2021-02-12T23:55:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174144.46 epoch sec; seqNo:8120; 4058189.72 north; 591746.40 east 2021-02-12T23:55:44Z- AHRS_M2 msg: 1613174144.85 epoch sec; seqNo:8055 2021-02-12T23:55:44Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.16 psi 2021-02-12T23:55:44Z- handleDepth()-Depth_Keller msg: 1613174144865 epoch sec; seqNo:8121; depth 10.93 2021-02-12T23:55:44Z- handleDvl() - RDI_Pathfinder msg: 1613174144.88 epoch sec; seqNo:3828 2021-02-12T23:55:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.38 , 78.04 , 115.90 2021-02-12T23:55:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.01 2021-02-12T23:55:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174144.89 epoch sec; seqNo:8121; 4058189.72 north; 591746.40 east 2021-02-12T23:55:45Z- AHRS_M2 msg: 1613174145.25 epoch sec; seqNo:8056 2021-02-12T23:55:45Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.16 psi 2021-02-12T23:55:45Z- handleDepth()-Depth_Keller msg: 1613174145256 epoch sec; seqNo:8122; depth 11.04 2021-02-12T23:55:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:45Z- handleDvl() - RDI_Pathfinder msg: 1613174145.26 epoch sec; seqNo:3829 2021-02-12T23:55:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.01 2021-02-12T23:55:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174145.28 epoch sec; seqNo:8122; 4058190.10 north; 591746.40 east 2021-02-12T23:55:45Z- AHRS_M2 msg: 1613174145.66 epoch sec; seqNo:8057 2021-02-12T23:55:45Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.17 psi 2021-02-12T23:55:45Z- handleDepth()-Depth_Keller msg: 1613174145664 epoch sec; seqNo:8123; depth 11.16 2021-02-12T23:55:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174145.68 epoch sec; seqNo:8123; 4058190.49 north; 591747.62 east 2021-02-12T23:55:45Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 131 calls... 2021-02-12T23:55:45Z- waiting for 1613174143.84, time is 1613174145.68 2021-02-12T23:55:45Z- LcmTrn::updateTrn() >>>> pose time 1613174145.68, meas time 1613174145.26 <<<< 2021-02-12T23:55:45Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174145.26, ping # 0. 2021-02-12T23:55:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174145.26 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:55:45Z- TerrainNav::motionUpdate() - currEstimate = (4058190, 591747.62, 11.16) 2021-02-12T23:55:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:55:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:55:45Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:55:45Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:55:45Z- TNavBF::Projecting Measurements 2021-02-12T23:55:45Z- TNavFilter::Measurement from time = 1613174145.26 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:55:45Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:55:45Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:55:45Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:55:45Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:55:45Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:55:45Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:55:45Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:55:45Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 728.1 EastVariance: 833.0 DepthVariance: 1.7 this->DepthBiasCov: 1.7 2021-02-12T23:55:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:55:45Z- TNavBF::mmse selected from filter -1 2021-02-12T23:55:45Z- variances([0], [2], and psi_berg) = 728.045410 833.015155 0.000000 2021-02-12T23:55:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:55:45Z- LcmTrn::publishEstimates() - MLE : 4058184.99 591700.12 11.16 2021-02-12T23:55:45Z- LcmTrn::publishEstimates() - MMSE : 4058190.49 591747.62 11.16 2021-02-12T23:55:45Z- LcmTrn::publishEstimates() - COVAR: 728.05 833.02 1.69 0.00 2021-02-12T23:55:46Z- AHRS_M2 msg: 1613174146.06 epoch sec; seqNo:8058 2021-02-12T23:55:46Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.17 psi 2021-02-12T23:55:46Z- handleDepth()-Depth_Keller msg: 1613174146064 epoch sec; seqNo:8124; depth 11.26 2021-02-12T23:55:46Z- handleDvl() - RDI_Pathfinder msg: 1613174146.08 epoch sec; seqNo:3830 2021-02-12T23:55:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.28 , 102.51 , 78.17 , 116.03 2021-02-12T23:55:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.07 , -0.08 2021-02-12T23:55:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174146.09 epoch sec; seqNo:8124; 4058190.87 north; 591747.61 east 2021-02-12T23:55:46Z- AHRS_M2 msg: 1613174146.46 epoch sec; seqNo:8059 2021-02-12T23:55:46Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.19 psi 2021-02-12T23:55:46Z- handleDepth()-Depth_Keller msg: 1613174146470 epoch sec; seqNo:8125; depth 11.37 2021-02-12T23:55:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:46Z- handleDvl() - RDI_Pathfinder msg: 1613174146.47 epoch sec; seqNo:3831 2021-02-12T23:55:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.07 , -0.08 2021-02-12T23:55:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174146.48 epoch sec; seqNo:8125; 4058190.87 north; 591747.61 east 2021-02-12T23:55:46Z- AHRS_M2 msg: 1613174146.87 epoch sec; seqNo:8060 2021-02-12T23:55:46Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.20 psi 2021-02-12T23:55:46Z- handleDepth()-Depth_Keller msg: 1613174146873 epoch sec; seqNo:8126; depth 11.49 2021-02-12T23:55:46Z- handleDvl() - RDI_Pathfinder msg: 1613174146.89 epoch sec; seqNo:3832 2021-02-12T23:55:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.13 , 78.44 , 116.30 2021-02-12T23:55:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , -0.21 2021-02-12T23:55:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174146.90 epoch sec; seqNo:8126; 4058191.25 north; 591747.61 east 2021-02-12T23:55:47Z- AHRS_M2 msg: 1613174147.27 epoch sec; seqNo:8061 2021-02-12T23:55:47Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.21 psi 2021-02-12T23:55:47Z- handleDepth()-Depth_Keller msg: 1613174147278 epoch sec; seqNo:8127; depth 11.61 2021-02-12T23:55:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174147.29 epoch sec; seqNo:8127; 4058191.63 north; 591747.60 east 2021-02-12T23:55:47Z- AHRS_M2 msg: 1613174147.68 epoch sec; seqNo:8062 2021-02-12T23:55:47Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.23 psi 2021-02-12T23:55:47Z- handleDepth()-Depth_Keller msg: 1613174147687 epoch sec; seqNo:8128; depth 11.75 2021-02-12T23:55:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:47Z- handleDvl() - RDI_Pathfinder msg: 1613174147.69 epoch sec; seqNo:3833 2021-02-12T23:55:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , -0.21 2021-02-12T23:55:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174147.70 epoch sec; seqNo:8128; 4058192.02 north; 591748.82 east 2021-02-12T23:55:48Z- AHRS_M2 msg: 1613174148.08 epoch sec; seqNo:8063 2021-02-12T23:55:48Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.23 psi 2021-02-12T23:55:48Z- handleDepth()-Depth_Keller msg: 1613174148088 epoch sec; seqNo:8129; depth 11.88 2021-02-12T23:55:48Z- handleDvl() - RDI_Pathfinder msg: 1613174148.11 epoch sec; seqNo:3834 2021-02-12T23:55:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.51 , 107.85 , 78.10 , 114.61 2021-02-12T23:55:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.17 , -0.19 2021-02-12T23:55:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174148.12 epoch sec; seqNo:8129; 4058192.40 north; 591748.82 east 2021-02-12T23:55:48Z- AHRS_M2 msg: 1613174148.48 epoch sec; seqNo:8064 2021-02-12T23:55:48Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.23 psi 2021-02-12T23:55:48Z- handleDepth()-Depth_Keller msg: 1613174148488 epoch sec; seqNo:8130; depth 12.01 2021-02-12T23:55:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174148.50 epoch sec; seqNo:8130; 4058192.78 north; 591748.81 east 2021-02-12T23:55:48Z- AHRS_M2 msg: 1613174148.89 epoch sec; seqNo:8065 2021-02-12T23:55:48Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.22 psi 2021-02-12T23:55:48Z- handleDepth()-Depth_Keller msg: 1613174148893 epoch sec; seqNo:8131; depth 12.13 2021-02-12T23:55:48Z- handleDvl() - RDI_Pathfinder msg: 1613174148.91 epoch sec; seqNo:3835 2021-02-12T23:55:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.85 , 108.19 , 75.61 , 113.60 2021-02-12T23:55:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.20 , 0.00 2021-02-12T23:55:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174148.92 epoch sec; seqNo:8131; 4058192.78 north; 591748.81 east 2021-02-12T23:55:49Z- AHRS_M2 msg: 1613174149.29 epoch sec; seqNo:8066 2021-02-12T23:55:49Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.22 psi 2021-02-12T23:55:49Z- handleDepth()-Depth_Keller msg: 1613174149298 epoch sec; seqNo:8132; depth 12.27 2021-02-12T23:55:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174149.31 epoch sec; seqNo:8132; 4058193.15 north; 591748.81 east 2021-02-12T23:55:49Z- AHRS_M2 msg: 1613174149.70 epoch sec; seqNo:8067 2021-02-12T23:55:49Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.21 psi 2021-02-12T23:55:49Z- handleDepth()-Depth_Keller msg: 1613174149708 epoch sec; seqNo:8133; depth 12.40 2021-02-12T23:55:49Z- handleDvl() - RDI_Pathfinder msg: 1613174149.72 epoch sec; seqNo:3836 2021-02-12T23:55:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 106.90 , 77.24 , 117.72 2021-02-12T23:55:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.09 , 0.19 2021-02-12T23:55:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174149.74 epoch sec; seqNo:8133; 4058193.53 north; 591748.80 east 2021-02-12T23:55:50Z- AHRS_M2 msg: 1613174150.10 epoch sec; seqNo:8068 2021-02-12T23:55:50Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.21 psi 2021-02-12T23:55:50Z- handleDepth()-Depth_Keller msg: 1613174150104 epoch sec; seqNo:8134; depth 12.53 2021-02-12T23:55:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:50Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:50Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:50Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:50Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:50Z- handleDvl() - RDI_Pathfinder msg: 1613174150.11 epoch sec; seqNo:3837 2021-02-12T23:55:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.09 , 0.19 2021-02-12T23:55:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174150.16 epoch sec; seqNo:8134; 4058193.53 north; 591748.80 east 2021-02-12T23:55:50Z- AHRS_M2 msg: 1613174150.50 epoch sec; seqNo:8069 2021-02-12T23:55:50Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.21 psi 2021-02-12T23:55:50Z- handleDepth()-Depth_Keller msg: 1613174150510 epoch sec; seqNo:8135; depth 12.66 2021-02-12T23:55:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174150.52 epoch sec; seqNo:8135; 4058193.91 north; 591748.80 east 2021-02-12T23:55:50Z- AHRS_M2 msg: 1613174150.91 epoch sec; seqNo:8070 2021-02-12T23:55:50Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.21 psi 2021-02-12T23:55:50Z- LcmTrn::handleMessages() - handling 45 messages took 0.69 ms and 131 calls... 2021-02-12T23:55:50Z- waiting for 1613174148.68, time is 1613174150.91 2021-02-12T23:55:50Z- LcmTrn::updateTrn() >>>> pose time 1613174150.91, meas time 1613174150.11 <<<< 2021-02-12T23:55:50Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174150.11, ping # 0. 2021-02-12T23:55:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174150.11 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:55:50Z- TerrainNav::motionUpdate() - currEstimate = (4058194, 591748.80, 12.66) 2021-02-12T23:55:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:55:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:55:50Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:55:50Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:55:50Z- TNavBF::Projecting Measurements 2021-02-12T23:55:50Z- TNavFilter::Measurement from time = 1613174150.11 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:55:50Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:55:50Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:55:50Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:55:50Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:55:50Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:55:50Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:55:50Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:55:50Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 728.0 EastVariance: 833.0 DepthVariance: 1.8 this->DepthBiasCov: 1.8 2021-02-12T23:55:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:55:50Z- TNavBF::mmse selected from filter -1 2021-02-12T23:55:50Z- variances([0], [2], and psi_berg) = 727.953025 833.015089 0.000000 2021-02-12T23:55:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:55:50Z- LcmTrn::publishEstimates() - MLE : 4058188.41 591701.30 12.66 2021-02-12T23:55:50Z- LcmTrn::publishEstimates() - MMSE : 4058193.91 591748.80 12.66 2021-02-12T23:55:50Z- LcmTrn::publishEstimates() - COVAR: 727.95 833.02 1.88 0.00 2021-02-12T23:55:50Z- handleDepth()-Depth_Keller msg: 1613174150912 epoch sec; seqNo:8136; depth 12.76 2021-02-12T23:55:50Z- handleDvl() - RDI_Pathfinder msg: 1613174150.93 epoch sec; seqNo:3838 2021-02-12T23:55:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.80 , 105.08 , 76.02 , 119.95 2021-02-12T23:55:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.05 , 0.27 2021-02-12T23:55:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174150.94 epoch sec; seqNo:8136; 4058193.91 north; 591748.80 east 2021-02-12T23:55:51Z- AHRS_M2 msg: 1613174151.32 epoch sec; seqNo:8071 2021-02-12T23:55:51Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.21 psi 2021-02-12T23:55:51Z- handleDepth()-Depth_Keller msg: 1613174151321 epoch sec; seqNo:8137; depth 12.93 2021-02-12T23:55:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:51Z- handleDvl() - RDI_Pathfinder msg: 1613174151.32 epoch sec; seqNo:3839 2021-02-12T23:55:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.05 , 0.27 2021-02-12T23:55:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174151.35 epoch sec; seqNo:8137; 4058194.30 north; 591750.02 east 2021-02-12T23:55:51Z- AHRS_M2 msg: 1613174151.72 epoch sec; seqNo:8072 2021-02-12T23:55:51Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-12T23:55:51Z- handleDepth()-Depth_Keller msg: 1613174151720 epoch sec; seqNo:8138; depth 13.05 2021-02-12T23:55:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174151.73 epoch sec; seqNo:8138; 4058194.30 north; 591750.02 east 2021-02-12T23:55:52Z- AHRS_M2 msg: 1613174152.12 epoch sec; seqNo:8073 2021-02-12T23:55:52Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-12T23:55:52Z- handleDepth()-Depth_Keller msg: 1613174152129 epoch sec; seqNo:8139; depth 13.19 2021-02-12T23:55:52Z- handleDvl() - RDI_Pathfinder msg: 1613174152.14 epoch sec; seqNo:3840 2021-02-12T23:55:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 104.07 , 76.83 , 117.59 2021-02-12T23:55:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.20 , 0.19 2021-02-12T23:55:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174152.15 epoch sec; seqNo:8139; 4058194.68 north; 591750.01 east 2021-02-12T23:55:52Z- AHRS_M2 msg: 1613174152.52 epoch sec; seqNo:8074 2021-02-12T23:55:52Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.21 psi 2021-02-12T23:55:52Z- handleDepth()-Depth_Keller msg: 1613174152528 epoch sec; seqNo:8140; depth 13.31 2021-02-12T23:55:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:52Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:52Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:52Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:52Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:52Z- handleDvl() - RDI_Pathfinder msg: 1613174152.53 epoch sec; seqNo:3841 2021-02-12T23:55:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.20 , 0.19 2021-02-12T23:55:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174152.55 epoch sec; seqNo:8140; 4058195.06 north; 591750.01 east 2021-02-12T23:55:52Z- AHRS_M2 msg: 1613174152.93 epoch sec; seqNo:8075 2021-02-12T23:55:52Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.21 psi 2021-02-12T23:55:52Z- handleDepth()-Depth_Keller msg: 1613174152937 epoch sec; seqNo:8141; depth 13.46 2021-02-12T23:55:52Z- handleDvl() - RDI_Pathfinder msg: 1613174152.95 epoch sec; seqNo:3842 2021-02-12T23:55:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.13 , 78.44 , 114.95 2021-02-12T23:55:52Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.28 , -0.01 2021-02-12T23:55:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174152.97 epoch sec; seqNo:8141; 4058195.06 north; 591750.01 east 2021-02-12T23:55:53Z- AHRS_M2 msg: 1613174153.33 epoch sec; seqNo:8076 2021-02-12T23:55:53Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.21 psi 2021-02-12T23:55:53Z- handleDepth()-Depth_Keller msg: 1613174153336 epoch sec; seqNo:8142; depth 13.60 2021-02-12T23:55:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174153.35 epoch sec; seqNo:8142; 4058195.45 north; 591751.23 east 2021-02-12T23:55:53Z- AHRS_M2 msg: 1613174153.74 epoch sec; seqNo:8077 2021-02-12T23:55:53Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.22 psi 2021-02-12T23:55:53Z- handleDepth()-Depth_Keller msg: 1613174153749 epoch sec; seqNo:8143; depth 13.73 2021-02-12T23:55:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:53Z- handleDvl() - RDI_Pathfinder msg: 1613174153.75 epoch sec; seqNo:3843 2021-02-12T23:55:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:53Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.28 , -0.01 2021-02-12T23:55:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174153.76 epoch sec; seqNo:8143; 4058195.83 north; 591751.22 east 2021-02-12T23:55:54Z- AHRS_M2 msg: 1613174154.14 epoch sec; seqNo:8078 2021-02-12T23:55:54Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.22 psi 2021-02-12T23:55:54Z- handleDepth()-Depth_Keller msg: 1613174154147 epoch sec; seqNo:8144; depth 13.87 2021-02-12T23:55:54Z- handleDvl() - RDI_Pathfinder msg: 1613174154.16 epoch sec; seqNo:3844 2021-02-12T23:55:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.58 , 105.21 , 78.17 , 113.33 2021-02-12T23:55:54Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.17 , -0.20 2021-02-12T23:55:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174154.18 epoch sec; seqNo:8144; 4058196.21 north; 591751.22 east 2021-02-12T23:55:54Z- AHRS_M2 msg: 1613174154.54 epoch sec; seqNo:8079 2021-02-12T23:55:54Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.22 psi 2021-02-12T23:55:54Z- handleDepth()-Depth_Keller msg: 1613174154548 epoch sec; seqNo:8145; depth 14.00 2021-02-12T23:55:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174154.56 epoch sec; seqNo:8145; 4058196.21 north; 591751.22 east 2021-02-12T23:55:54Z- AHRS_M2 msg: 1613174154.95 epoch sec; seqNo:8080 2021-02-12T23:55:54Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.22 psi 2021-02-12T23:55:54Z- handleDepth()-Depth_Keller msg: 1613174154965 epoch sec; seqNo:8146; depth 14.13 2021-02-12T23:55:54Z- handleDvl() - RDI_Pathfinder msg: 1613174154.98 epoch sec; seqNo:3845 2021-02-12T23:55:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.21 , 104.20 , 78.51 , 116.37 2021-02-12T23:55:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.03 , -0.24 2021-02-12T23:55:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174154.99 epoch sec; seqNo:8146; 4058196.60 north; 591752.43 east 2021-02-12T23:55:55Z- AHRS_M2 msg: 1613174155.35 epoch sec; seqNo:8081 2021-02-12T23:55:55Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.22 psi 2021-02-12T23:55:55Z- handleDepth()-Depth_Keller msg: 1613174155356 epoch sec; seqNo:8147; depth 14.27 2021-02-12T23:55:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174155.37 epoch sec; seqNo:8147; 4058196.98 north; 591752.43 east 2021-02-12T23:55:55Z- AHRS_M2 msg: 1613174155.77 epoch sec; seqNo:8082 2021-02-12T23:55:55Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.22 psi 2021-02-12T23:55:55Z- handleDepth()-Depth_Keller msg: 1613174155773 epoch sec; seqNo:8148; depth 14.39 2021-02-12T23:55:55Z- handleDvl() - RDI_Pathfinder msg: 1613174155.80 epoch sec; seqNo:3846 2021-02-12T23:55:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.21 , 104.20 , 75.20 , 112.31 2021-02-12T23:55:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.18 , -0.09 2021-02-12T23:55:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174155.81 epoch sec; seqNo:8148; 4058197.36 north; 591752.43 east 2021-02-12T23:55:55Z- LcmTrn::handleMessages() - handling 47 messages took 0.63 ms and 135 calls... 2021-02-12T23:55:55Z- waiting for 1613174153.91, time is 1613174155.81 2021-02-12T23:55:55Z- LcmTrn::updateTrn() equating pose 1613174155.81 and meas times 1613174155.81 2021-02-12T23:55:55Z- LcmTrn::updateTrn() >>>> pose time 1613174155.81, meas time 1613174155.81 <<<< 2021-02-12T23:55:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174155.81 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:55:55Z- TerrainNav::motionUpdate() - currEstimate = (4058197, 591752.43, 14.39) 2021-02-12T23:55:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:55:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:55:55Z- TNavBF::Projecting Measurements 2021-02-12T23:55:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:55:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:55:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:55:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:55:55Z- TNavFilter::4 good beams. 2021-02-12T23:55:55Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:55:55Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:55:55Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:55:55Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:55:55Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:55:55Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:55:55Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:55:55Z- TNavBF::mmse selected from filter -1 2021-02-12T23:55:55Z- TerrainMapOctree: (vehN, vehE ) = (4058197.36, 591752.43). 2021-02-12T23:55:55Z- TerrainMapOctree: Min Distance = 3944.17. 2021-02-12T23:55:55Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:55:55Z- There is no specific good beam that all particles have in common. 2021-02-12T23:55:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:55:55Z- Old Depth Bias: 0.00 DepthBiasCov: 2.13 K: 0.148 ExpectedMeasDiff: nan currMeas.covariance: 10.55 2021-02-12T23:55:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:55:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.82 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 17.37 2021-02-12T23:55:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:55:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.66 K: 0.133 ExpectedMeasDiff: nan currMeas.covariance: 9.05 2021-02-12T23:55:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:55:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.44 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 20.18 2021-02-12T23:55:56Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:55:56Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:55:56Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:55:56Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:55:56Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:55:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:55:56Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:55:56Z- Filter 0 No beams used in subcloud update 2021-02-12T23:55:56Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:55:56Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:55:56Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 727.9 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-12T23:55:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174155.81 sec. 2021-02-12T23:55:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:55:56Z- TNavBF::mmse selected from filter -1 2021-02-12T23:55:56Z- variances([0], [2], and psi_berg) = 727.870305 833.015030 0.000000 2021-02-12T23:55:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:55:56Z- LcmTrn::publishEstimates() - MLE : 4058191.86 591704.93 14.39 2021-02-12T23:55:56Z- LcmTrn::publishEstimates() - MMSE : 4058197.36 591752.43 14.39 2021-02-12T23:55:56Z- LcmTrn::publishEstimates() - COVAR: 727.87 833.02 1.35 0.00 2021-02-12T23:55:56Z- AHRS_M2 msg: 1613174156.16 epoch sec; seqNo:8083 2021-02-12T23:55:56Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.22 psi 2021-02-12T23:55:56Z- handleDepth()-Depth_Keller msg: 1613174156166 epoch sec; seqNo:8149; depth 14.52 2021-02-12T23:55:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:56Z- handleDvl() - RDI_Pathfinder msg: 1613174156.17 epoch sec; seqNo:3847 2021-02-12T23:55:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.18 , -0.09 2021-02-12T23:55:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174156.18 epoch sec; seqNo:8149; 4058197.74 north; 591752.42 east 2021-02-12T23:55:56Z- AHRS_M2 msg: 1613174156.56 epoch sec; seqNo:8084 2021-02-12T23:55:56Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.21 psi 2021-02-12T23:55:56Z- handleDepth()-Depth_Keller msg: 1613174156568 epoch sec; seqNo:8150; depth 14.65 2021-02-12T23:55:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174156.59 epoch sec; seqNo:8150; 4058198.12 north; 591752.42 east 2021-02-12T23:55:56Z- AHRS_M2 msg: 1613174156.97 epoch sec; seqNo:8085 2021-02-12T23:55:56Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.21 psi 2021-02-12T23:55:56Z- handleDepth()-Depth_Keller msg: 1613174156976 epoch sec; seqNo:8151; depth 14.76 2021-02-12T23:55:56Z- handleDvl() - RDI_Pathfinder msg: 1613174156.99 epoch sec; seqNo:3848 2021-02-12T23:55:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.88 , 103.05 , 78.71 , 109.81 2021-02-12T23:55:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.17 , 0.10 2021-02-12T23:55:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174157.01 epoch sec; seqNo:8151; 4058198.50 north; 591752.41 east 2021-02-12T23:55:57Z- AHRS_M2 msg: 1613174157.37 epoch sec; seqNo:8086 2021-02-12T23:55:57Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.20 psi 2021-02-12T23:55:57Z- handleDepth()-Depth_Keller msg: 1613174157376 epoch sec; seqNo:8152; depth 14.90 2021-02-12T23:55:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:57Z- handleDvl() - RDI_Pathfinder msg: 1613174157.38 epoch sec; seqNo:3849 2021-02-12T23:55:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.17 , 0.10 2021-02-12T23:55:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174157.39 epoch sec; seqNo:8152; 4058198.50 north; 591752.41 east 2021-02-12T23:55:57Z- AHRS_M2 msg: 1613174157.78 epoch sec; seqNo:8087 2021-02-12T23:55:57Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.19 psi 2021-02-12T23:55:57Z- handleDepth()-Depth_Keller msg: 1613174157785 epoch sec; seqNo:8153; depth 15.01 2021-02-12T23:55:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174157.81 epoch sec; seqNo:8153; 4058198.88 north; 591752.41 east 2021-02-12T23:55:58Z- AHRS_M2 msg: 1613174158.18 epoch sec; seqNo:8088 2021-02-12T23:55:58Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.19 psi 2021-02-12T23:55:58Z- handleDepth()-Depth_Keller msg: 1613174158185 epoch sec; seqNo:8154; depth 15.13 2021-02-12T23:55:58Z- handleDvl() - RDI_Pathfinder msg: 1613174158.20 epoch sec; seqNo:3850 2021-02-12T23:55:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.42 , 100.35 , 78.71 , 111.16 2021-02-12T23:55:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.15 2021-02-12T23:55:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174158.21 epoch sec; seqNo:8154; 4058199.27 north; 591753.63 east 2021-02-12T23:55:58Z- AHRS_M2 msg: 1613174158.58 epoch sec; seqNo:8089 2021-02-12T23:55:58Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.19 psi 2021-02-12T23:55:58Z- handleDepth()-Depth_Keller msg: 1613174158588 epoch sec; seqNo:8155; depth 15.26 2021-02-12T23:55:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:55:58Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:55:58Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:55:58Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:55:58Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:55:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:55:58Z- handleDvl() - RDI_Pathfinder msg: 1613174158.59 epoch sec; seqNo:3851 2021-02-12T23:55:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:55:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.15 2021-02-12T23:55:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174158.60 epoch sec; seqNo:8155; 4058199.65 north; 591753.62 east 2021-02-12T23:55:58Z- AHRS_M2 msg: 1613174158.99 epoch sec; seqNo:8090 2021-02-12T23:55:58Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.19 psi 2021-02-12T23:55:58Z- handleDepth()-Depth_Keller msg: 1613174158992 epoch sec; seqNo:8156; depth 15.38 2021-02-12T23:55:59Z- handleDvl() - RDI_Pathfinder msg: 1613174159.00 epoch sec; seqNo:3852 2021-02-12T23:55:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.42 , 99.00 , 78.71 , 111.16 2021-02-12T23:55:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.08 , 0.01 2021-02-12T23:55:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174159.02 epoch sec; seqNo:8156; 4058199.65 north; 591753.62 east 2021-02-12T23:55:59Z- AHRS_M2 msg: 1613174159.39 epoch sec; seqNo:8091 2021-02-12T23:55:59Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.19 psi 2021-02-12T23:55:59Z- handleDepth()-Depth_Keller msg: 1613174159397 epoch sec; seqNo:8157; depth 15.49 2021-02-12T23:55:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174159.41 epoch sec; seqNo:8157; 4058200.03 north; 591753.62 east 2021-02-12T23:55:59Z- AHRS_M2 msg: 1613174159.80 epoch sec; seqNo:8092 2021-02-12T23:55:59Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-12T23:55:59Z- handleDepth()-Depth_Keller msg: 1613174159809 epoch sec; seqNo:8158; depth 15.62 2021-02-12T23:55:59Z- handleDvl() - RDI_Pathfinder msg: 1613174159.83 epoch sec; seqNo:3853 2021-02-12T23:55:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.29 , 99.00 , 73.15 , 108.46 2021-02-12T23:55:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , -0.16 2021-02-12T23:55:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174159.84 epoch sec; seqNo:8158; 4058200.40 north; 591753.62 east 2021-02-12T23:56:00Z- AHRS_M2 msg: 1613174160.20 epoch sec; seqNo:8093 2021-02-12T23:56:00Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.19 psi 2021-02-12T23:56:00Z- handleDepth()-Depth_Keller msg: 1613174160205 epoch sec; seqNo:8159; depth 15.75 2021-02-12T23:56:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174160.22 epoch sec; seqNo:8159; 4058200.78 north; 591753.61 east 2021-02-12T23:56:00Z- AHRS_M2 msg: 1613174160.60 epoch sec; seqNo:8094 2021-02-12T23:56:00Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.19 psi 2021-02-12T23:56:00Z- handleDepth()-Depth_Keller msg: 1613174160608 epoch sec; seqNo:8160; depth 15.85 2021-02-12T23:56:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174160.62 epoch sec; seqNo:8160; 4058201.18 north; 591754.83 east 2021-02-12T23:56:01Z- AHRS_M2 msg: 1613174161.01 epoch sec; seqNo:8095 2021-02-12T23:56:01Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.20 psi 2021-02-12T23:56:01Z- handleDepth()-Depth_Keller msg: 1613174161015 epoch sec; seqNo:8161; depth 15.97 2021-02-12T23:56:01Z- handleDvl() - RDI_Pathfinder msg: 1613174161.03 epoch sec; seqNo:3854 2021-02-12T23:56:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.52 , 100.21 , 71.52 , 107.51 2021-02-12T23:56:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.13 , -0.19 2021-02-12T23:56:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174161.04 epoch sec; seqNo:8161; 4058201.55 north; 591754.82 east 2021-02-12T23:56:01Z- LcmTrn::handleMessages() - handling 47 messages took 0.69 ms and 134 calls... 2021-02-12T23:56:01Z- waiting for 1613174158.81, time is 1613174161.04 2021-02-12T23:56:01Z- LcmTrn::updateTrn() equating pose 1613174161.04 and meas times 1613174161.04 2021-02-12T23:56:01Z- LcmTrn::updateTrn() >>>> pose time 1613174161.04, meas time 1613174161.04 <<<< 2021-02-12T23:56:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174161.04 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:56:01Z- TerrainNav::motionUpdate() - currEstimate = (4058202, 591754.82, 15.97) 2021-02-12T23:56:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:56:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:56:01Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174161.04 sec.; No relevant navigation data available 2021-02-12T23:56:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:56:01Z- TNavBF::mmse selected from filter -1 2021-02-12T23:56:01Z- variances([0], [2], and psi_berg) = 727.789115 833.014972 0.000000 2021-02-12T23:56:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:56:01Z- LcmTrn::publishEstimates() - MLE : 4058196.05 591707.32 15.97 2021-02-12T23:56:01Z- LcmTrn::publishEstimates() - MMSE : 4058201.55 591754.82 15.97 2021-02-12T23:56:01Z- LcmTrn::publishEstimates() - COVAR: 727.79 833.01 1.60 0.00 2021-02-12T23:56:01Z- AHRS_M2 msg: 1613174161.41 epoch sec; seqNo:8096 2021-02-12T23:56:01Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.19 psi 2021-02-12T23:56:01Z- handleDepth()-Depth_Keller msg: 1613174161416 epoch sec; seqNo:8162; depth 16.10 2021-02-12T23:56:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174161.43 epoch sec; seqNo:8162; 4058201.93 north; 591754.82 east 2021-02-12T23:56:01Z- AHRS_M2 msg: 1613174161.82 epoch sec; seqNo:8097 2021-02-12T23:56:01Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.18 psi 2021-02-12T23:56:01Z- handleDepth()-Depth_Keller msg: 1613174161829 epoch sec; seqNo:8163; depth 16.21 2021-02-12T23:56:01Z- handleDvl() - RDI_Pathfinder msg: 1613174161.85 epoch sec; seqNo:3855 2021-02-12T23:56:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.06 , 99.00 , 0.00 , 107.51 2021-02-12T23:56:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.26 , -0.06 2021-02-12T23:56:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174161.86 epoch sec; seqNo:8163; 4058202.31 north; 591754.82 east 2021-02-12T23:56:02Z- AHRS_M2 msg: 1613174162.22 epoch sec; seqNo:8098 2021-02-12T23:56:02Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.17 psi 2021-02-12T23:56:02Z- handleDepth()-Depth_Keller msg: 1613174162233 epoch sec; seqNo:8164; depth 16.35 2021-02-12T23:56:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:02Z- handleDvl() - RDI_Pathfinder msg: 1613174162.23 epoch sec; seqNo:3856 2021-02-12T23:56:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.26 , -0.06 2021-02-12T23:56:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174162.25 epoch sec; seqNo:8164; 4058202.31 north; 591754.82 east 2021-02-12T23:56:02Z- AHRS_M2 msg: 1613174162.63 epoch sec; seqNo:8099 2021-02-12T23:56:02Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.15 psi 2021-02-12T23:56:02Z- handleDepth()-Depth_Keller msg: 1613174162638 epoch sec; seqNo:8165; depth 16.44 2021-02-12T23:56:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174162.65 epoch sec; seqNo:8165; 4058202.69 north; 591754.81 east 2021-02-12T23:56:03Z- AHRS_M2 msg: 1613174163.03 epoch sec; seqNo:8100 2021-02-12T23:56:03Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.14 psi 2021-02-12T23:56:03Z- handleDepth()-Depth_Keller msg: 1613174163037 epoch sec; seqNo:8166; depth 16.56 2021-02-12T23:56:03Z- handleDvl() - RDI_Pathfinder msg: 1613174163.06 epoch sec; seqNo:3857 2021-02-12T23:56:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.29 , 97.44 , 73.15 , 105.55 2021-02-12T23:56:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.21 , 0.09 2021-02-12T23:56:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174163.07 epoch sec; seqNo:8166; 4058203.07 north; 591754.81 east 2021-02-12T23:56:03Z- AHRS_M2 msg: 1613174163.44 epoch sec; seqNo:8101 2021-02-12T23:56:03Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.13 psi 2021-02-12T23:56:03Z- handleDepth()-Depth_Keller msg: 1613174163440 epoch sec; seqNo:8167; depth 16.68 2021-02-12T23:56:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:03Z- handleDvl() - RDI_Pathfinder msg: 1613174163.44 epoch sec; seqNo:3858 2021-02-12T23:56:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.21 , 0.09 2021-02-12T23:56:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174163.45 epoch sec; seqNo:8167; 4058203.45 north; 591754.80 east 2021-02-12T23:56:03Z- AHRS_M2 msg: 1613174163.84 epoch sec; seqNo:8102 2021-02-12T23:56:03Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.12 psi 2021-02-12T23:56:03Z- handleDepth()-Depth_Keller msg: 1613174163850 epoch sec; seqNo:8168; depth 16.79 2021-02-12T23:56:03Z- handleDvl() - RDI_Pathfinder msg: 1613174163.87 epoch sec; seqNo:3859 2021-02-12T23:56:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.03 , 92.98 , 73.15 , 106.36 2021-02-12T23:56:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.09 , 0.16 2021-02-12T23:56:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174163.89 epoch sec; seqNo:8168; 4058203.83 north; 591754.80 east 2021-02-12T23:56:04Z- AHRS_M2 msg: 1613174164.24 epoch sec; seqNo:8103 2021-02-12T23:56:04Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.12 psi 2021-02-12T23:56:04Z- handleDepth()-Depth_Keller msg: 1613174164250 epoch sec; seqNo:8169; depth 16.92 2021-02-12T23:56:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174164.27 epoch sec; seqNo:8169; 4058203.83 north; 591754.80 east 2021-02-12T23:56:04Z- AHRS_M2 msg: 1613174164.65 epoch sec; seqNo:8104 2021-02-12T23:56:04Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.12 psi 2021-02-12T23:56:04Z- handleDepth()-Depth_Keller msg: 1613174164659 epoch sec; seqNo:8170; depth 17.03 2021-02-12T23:56:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:04Z- handleDvl() - RDI_Pathfinder msg: 1613174164.67 epoch sec; seqNo:3860 2021-02-12T23:56:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.09 , 0.16 2021-02-12T23:56:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174164.68 epoch sec; seqNo:8170; 4058204.22 north; 591756.02 east 2021-02-12T23:56:05Z- AHRS_M2 msg: 1613174165.06 epoch sec; seqNo:8105 2021-02-12T23:56:05Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.13 psi 2021-02-12T23:56:05Z- handleDepth()-Depth_Keller msg: 1613174165060 epoch sec; seqNo:8171; depth 17.15 2021-02-12T23:56:05Z- handleDvl() - RDI_Pathfinder msg: 1613174165.07 epoch sec; seqNo:3861 2021-02-12T23:56:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.85 , 69.07 , 106.36 2021-02-12T23:56:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.15 2021-02-12T23:56:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174165.09 epoch sec; seqNo:8171; 4058204.60 north; 591756.01 east 2021-02-12T23:56:05Z- AHRS_M2 msg: 1613174165.46 epoch sec; seqNo:8106 2021-02-12T23:56:05Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.14 psi 2021-02-12T23:56:05Z- handleDepth()-Depth_Keller msg: 1613174165460 epoch sec; seqNo:8172; depth 17.25 2021-02-12T23:56:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174165.47 epoch sec; seqNo:8172; 4058204.60 north; 591756.01 east 2021-02-12T23:56:05Z- AHRS_M2 msg: 1613174165.86 epoch sec; seqNo:8107 2021-02-12T23:56:05Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.15 psi 2021-02-12T23:56:05Z- handleDepth()-Depth_Keller msg: 1613174165874 epoch sec; seqNo:8173; depth 17.36 2021-02-12T23:56:05Z- handleDvl() - RDI_Pathfinder msg: 1613174165.89 epoch sec; seqNo:3862 2021-02-12T23:56:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 97.64 , 72.34 , 107.38 2021-02-12T23:56:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , 0.12 2021-02-12T23:56:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174165.90 epoch sec; seqNo:8173; 4058204.98 north; 591756.01 east 2021-02-12T23:56:06Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-12T23:56:06Z- waiting for 1613174164.04, time is 1613174165.90 2021-02-12T23:56:06Z- LcmTrn::updateTrn() equating pose 1613174165.90 and meas times 1613174165.90 2021-02-12T23:56:06Z- LcmTrn::updateTrn() >>>> pose time 1613174165.90, meas time 1613174165.90 <<<< 2021-02-12T23:56:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174165.90 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:56:06Z- TerrainNav::motionUpdate() - currEstimate = (4058205, 591756.01, 17.36) 2021-02-12T23:56:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:56:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:56:06Z- TNavBF::Projecting Measurements 2021-02-12T23:56:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:56:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:56:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:56:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:56:06Z- TNavFilter::4 good beams. 2021-02-12T23:56:06Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:56:06Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:56:06Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:56:06Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:56:06Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:56:06Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:56:06Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:56:06Z- TNavBF::mmse selected from filter -1 2021-02-12T23:56:06Z- TerrainMapOctree: (vehN, vehE ) = (4058204.98, 591756.01). 2021-02-12T23:56:06Z- TerrainMapOctree: Min Distance = 3936.01. 2021-02-12T23:56:06Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:56:06Z- There is no specific good beam that all particles have in common. 2021-02-12T23:56:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:56:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.78 K: 0.125 ExpectedMeasDiff: nan currMeas.covariance: 10.71 2021-02-12T23:56:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:56:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.56 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 15.25 2021-02-12T23:56:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:56:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.124 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-12T23:56:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:56:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 18.45 2021-02-12T23:56:06Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:56:06Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:56:06Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:56:06Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:56:06Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:56:06Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:56:06Z- Filter 0 No beams used in subcloud update 2021-02-12T23:56:06Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:56:06Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:56:06Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 727.7 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:56:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174165.90 sec. 2021-02-12T23:56:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:56:06Z- TNavBF::mmse selected from filter -1 2021-02-12T23:56:06Z- variances([0], [2], and psi_berg) = 727.718878 833.014922 0.000000 2021-02-12T23:56:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:56:06Z- LcmTrn::publishEstimates() - MLE : 4058199.48 591708.51 17.36 2021-02-12T23:56:06Z- LcmTrn::publishEstimates() - MMSE : 4058204.98 591756.01 17.36 2021-02-12T23:56:06Z- LcmTrn::publishEstimates() - COVAR: 727.72 833.01 1.18 0.00 2021-02-12T23:56:06Z- AHRS_M2 msg: 1613174166.26 epoch sec; seqNo:8108 2021-02-12T23:56:06Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.16 psi 2021-02-12T23:56:06Z- handleDepth()-Depth_Keller msg: 1613174166273 epoch sec; seqNo:8174; depth 17.48 2021-02-12T23:56:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174166.30 epoch sec; seqNo:8174; 4058205.36 north; 591756.00 east 2021-02-12T23:56:06Z- AHRS_M2 msg: 1613174166.67 epoch sec; seqNo:8109 2021-02-12T23:56:06Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.17 psi 2021-02-12T23:56:06Z- handleDepth()-Depth_Keller msg: 1613174166677 epoch sec; seqNo:8175; depth 17.60 2021-02-12T23:56:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174166.68 epoch sec; seqNo:8175; 4058205.36 north; 591756.00 east 2021-02-12T23:56:07Z- AHRS_M2 msg: 1613174167.07 epoch sec; seqNo:8110 2021-02-12T23:56:07Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.17 psi 2021-02-12T23:56:07Z- handleDepth()-Depth_Keller msg: 1613174167077 epoch sec; seqNo:8176; depth 17.70 2021-02-12T23:56:07Z- handleDvl() - RDI_Pathfinder msg: 1613174167.09 epoch sec; seqNo:3863 2021-02-12T23:56:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.52 , 97.78 , 71.52 , 106.30 2021-02-12T23:56:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.09 , 0.05 2021-02-12T23:56:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174167.11 epoch sec; seqNo:8176; 4058205.75 north; 591757.22 east 2021-02-12T23:56:07Z- AHRS_M2 msg: 1613174167.47 epoch sec; seqNo:8111 2021-02-12T23:56:07Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.17 psi 2021-02-12T23:56:07Z- handleDepth()-Depth_Keller msg: 1613174167480 epoch sec; seqNo:8177; depth 17.83 2021-02-12T23:56:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174167.50 epoch sec; seqNo:8177; 4058206.13 north; 591757.22 east 2021-02-12T23:56:07Z- AHRS_M2 msg: 1613174167.88 epoch sec; seqNo:8112 2021-02-12T23:56:07Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.17 psi 2021-02-12T23:56:07Z- handleDepth()-Depth_Keller msg: 1613174167889 epoch sec; seqNo:8178; depth 17.95 2021-02-12T23:56:07Z- handleDvl() - RDI_Pathfinder msg: 1613174167.90 epoch sec; seqNo:3864 2021-02-12T23:56:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.47 , 97.85 , 78.37 , 106.36 2021-02-12T23:56:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.06 , -0.01 2021-02-12T23:56:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174167.92 epoch sec; seqNo:8178; 4058206.13 north; 591757.22 east 2021-02-12T23:56:08Z- AHRS_M2 msg: 1613174168.28 epoch sec; seqNo:8113 2021-02-12T23:56:08Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.17 psi 2021-02-12T23:56:08Z- handleDepth()-Depth_Keller msg: 1613174168288 epoch sec; seqNo:8179; depth 18.06 2021-02-12T23:56:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:08Z- handleDvl() - RDI_Pathfinder msg: 1613174168.29 epoch sec; seqNo:3865 2021-02-12T23:56:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.06 , -0.01 2021-02-12T23:56:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174168.30 epoch sec; seqNo:8179; 4058206.51 north; 591757.21 east 2021-02-12T23:56:08Z- AHRS_M2 msg: 1613174168.69 epoch sec; seqNo:8114 2021-02-12T23:56:08Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-12T23:56:08Z- handleDepth()-Depth_Keller msg: 1613174168695 epoch sec; seqNo:8180; depth 18.17 2021-02-12T23:56:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174168.70 epoch sec; seqNo:8180; 4058206.88 north; 591757.21 east 2021-02-12T23:56:09Z- AHRS_M2 msg: 1613174169.09 epoch sec; seqNo:8115 2021-02-12T23:56:09Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-12T23:56:09Z- handleDepth()-Depth_Keller msg: 1613174169101 epoch sec; seqNo:8181; depth 18.30 2021-02-12T23:56:09Z- handleDvl() - RDI_Pathfinder msg: 1613174169.12 epoch sec; seqNo:3866 2021-02-12T23:56:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.24 , 95.28 , 70.70 , 106.23 2021-02-12T23:56:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , -0.09 2021-02-12T23:56:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174169.13 epoch sec; seqNo:8181; 4058207.28 north; 591758.43 east 2021-02-12T23:56:09Z- AHRS_M2 msg: 1613174169.50 epoch sec; seqNo:8116 2021-02-12T23:56:09Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.18 psi 2021-02-12T23:56:09Z- handleDepth()-Depth_Keller msg: 1613174169500 epoch sec; seqNo:8182; depth 18.41 2021-02-12T23:56:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:09Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:09Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:09Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:09Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:09Z- handleDvl() - RDI_Pathfinder msg: 1613174169.50 epoch sec; seqNo:3867 2021-02-12T23:56:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , -0.09 2021-02-12T23:56:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174169.51 epoch sec; seqNo:8182; 4058207.28 north; 591758.43 east 2021-02-12T23:56:09Z- AHRS_M2 msg: 1613174169.90 epoch sec; seqNo:8117 2021-02-12T23:56:09Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-12T23:56:09Z- handleDepth()-Depth_Keller msg: 1613174169909 epoch sec; seqNo:8183; depth 18.53 2021-02-12T23:56:09Z- handleDvl() - RDI_Pathfinder msg: 1613174169.92 epoch sec; seqNo:3868 2021-02-12T23:56:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 70.29 , 106.50 2021-02-12T23:56:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.04 , -0.14 2021-02-12T23:56:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174169.93 epoch sec; seqNo:8183; 4058207.65 north; 591758.42 east 2021-02-12T23:56:10Z- AHRS_M2 msg: 1613174170.30 epoch sec; seqNo:8118 2021-02-12T23:56:10Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.17 psi 2021-02-12T23:56:10Z- handleDepth()-Depth_Keller msg: 1613174170314 epoch sec; seqNo:8184; depth 18.64 2021-02-12T23:56:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174170.33 epoch sec; seqNo:8184; 4058208.03 north; 591758.42 east 2021-02-12T23:56:10Z- AHRS_M2 msg: 1613174170.72 epoch sec; seqNo:8119 2021-02-12T23:56:10Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.17 psi 2021-02-12T23:56:10Z- handleDepth()-Depth_Keller msg: 1613174170731 epoch sec; seqNo:8185; depth 18.76 2021-02-12T23:56:10Z- handleDvl() - RDI_Pathfinder msg: 1613174170.74 epoch sec; seqNo:3869 2021-02-12T23:56:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.65 , 96.70 , 69.89 , 106.43 2021-02-12T23:56:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.13 , -0.09 2021-02-12T23:56:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174170.75 epoch sec; seqNo:8185; 4058208.41 north; 591758.41 east 2021-02-12T23:56:11Z- AHRS_M2 msg: 1613174171.11 epoch sec; seqNo:8120 2021-02-12T23:56:11Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.17 psi 2021-02-12T23:56:11Z- handleDepth()-Depth_Keller msg: 1613174171117 epoch sec; seqNo:8186; depth 18.89 2021-02-12T23:56:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174171.13 epoch sec; seqNo:8186; 4058208.79 north; 591758.41 east 2021-02-12T23:56:11Z- LcmTrn::handleMessages() - handling 46 messages took 0.70 ms and 132 calls... 2021-02-12T23:56:11Z- waiting for 1613174168.90, time is 1613174171.13 2021-02-12T23:56:11Z- LcmTrn::updateTrn() >>>> pose time 1613174171.13, meas time 1613174170.74 <<<< 2021-02-12T23:56:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174170.74 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:56:11Z- TerrainNav::motionUpdate() - currEstimate = (4058209, 591758.41, 18.89) 2021-02-12T23:56:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:56:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:56:11Z- TNavBF::Projecting Measurements 2021-02-12T23:56:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:56:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:56:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:56:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:56:11Z- TNavFilter::4 good beams. 2021-02-12T23:56:11Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:56:11Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:56:11Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:56:11Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:56:11Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:56:11Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:56:11Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:56:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:56:11Z- TerrainMapOctree: (vehN, vehE ) = (4058208.51, 591758.23). 2021-02-12T23:56:11Z- TerrainMapOctree: Min Distance = 3931.87. 2021-02-12T23:56:11Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:56:11Z- There is no specific good beam that all particles have in common. 2021-02-12T23:56:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:56:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.39 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 9.65 2021-02-12T23:56:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:56:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 14.96 2021-02-12T23:56:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:56:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.108 ExpectedMeasDiff: nan currMeas.covariance: 7.81 2021-02-12T23:56:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:56:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 18.12 2021-02-12T23:56:11Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:56:11Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:56:11Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:56:11Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:56:11Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:56:11Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:56:11Z- Filter 0 No beams used in subcloud update 2021-02-12T23:56:11Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:56:11Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:56:11Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 727.6 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:56:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174170.74 sec. 2021-02-12T23:56:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:56:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:56:11Z- variances([0], [2], and psi_berg) = 727.622234 833.014853 0.000000 2021-02-12T23:56:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:56:11Z- LcmTrn::publishEstimates() - MLE : 4058203.29 591710.91 18.89 2021-02-12T23:56:11Z- LcmTrn::publishEstimates() - MMSE : 4058208.79 591758.41 18.89 2021-02-12T23:56:11Z- LcmTrn::publishEstimates() - COVAR: 727.62 833.01 1.00 0.00 2021-02-12T23:56:11Z- AHRS_M2 msg: 1613174171.52 epoch sec; seqNo:8121 2021-02-12T23:56:11Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.17 psi 2021-02-12T23:56:11Z- handleDepth()-Depth_Keller msg: 1613174171521 epoch sec; seqNo:8187; depth 19.01 2021-02-12T23:56:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174171.53 epoch sec; seqNo:8187; 4058208.80 north; 591759.63 east 2021-02-12T23:56:11Z- AHRS_M2 msg: 1613174171.92 epoch sec; seqNo:8122 2021-02-12T23:56:11Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.17 psi 2021-02-12T23:56:11Z- handleDepth()-Depth_Keller msg: 1613174171924 epoch sec; seqNo:8188; depth 19.12 2021-02-12T23:56:11Z- handleDvl() - RDI_Pathfinder msg: 1613174171.94 epoch sec; seqNo:3870 2021-02-12T23:56:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 95.55 , 70.70 , 107.72 2021-02-12T23:56:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.14 , -0.01 2021-02-12T23:56:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174171.95 epoch sec; seqNo:8188; 4058209.18 north; 591759.63 east 2021-02-12T23:56:12Z- AHRS_M2 msg: 1613174172.32 epoch sec; seqNo:8123 2021-02-12T23:56:12Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.17 psi 2021-02-12T23:56:12Z- handleDepth()-Depth_Keller msg: 1613174172328 epoch sec; seqNo:8189; depth 19.27 2021-02-12T23:56:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174172.34 epoch sec; seqNo:8189; 4058209.56 north; 591759.62 east 2021-02-12T23:56:12Z- AHRS_M2 msg: 1613174172.73 epoch sec; seqNo:8124 2021-02-12T23:56:12Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.17 psi 2021-02-12T23:56:12Z- handleDepth()-Depth_Keller msg: 1613174172735 epoch sec; seqNo:8190; depth 19.39 2021-02-12T23:56:12Z- handleDvl() - RDI_Pathfinder msg: 1613174172.75 epoch sec; seqNo:3871 2021-02-12T23:56:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 94.33 , 69.89 , 108.93 2021-02-12T23:56:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.13 , 0.06 2021-02-12T23:56:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174172.78 epoch sec; seqNo:8190; 4058209.94 north; 591759.62 east 2021-02-12T23:56:13Z- AHRS_M2 msg: 1613174173.14 epoch sec; seqNo:8125 2021-02-12T23:56:13Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.17 psi 2021-02-12T23:56:13Z- handleDepth()-Depth_Keller msg: 1613174173142 epoch sec; seqNo:8191; depth 19.53 2021-02-12T23:56:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174173.15 epoch sec; seqNo:8191; 4058209.94 north; 591759.62 east 2021-02-12T23:56:13Z- AHRS_M2 msg: 1613174173.54 epoch sec; seqNo:8126 2021-02-12T23:56:13Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.17 psi 2021-02-12T23:56:13Z- handleDepth()-Depth_Keller msg: 1613174173540 epoch sec; seqNo:8192; depth 19.66 2021-02-12T23:56:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174173.55 epoch sec; seqNo:8192; 4058210.32 north; 591759.61 east 2021-02-12T23:56:13Z- AHRS_M2 msg: 1613174173.94 epoch sec; seqNo:8127 2021-02-12T23:56:13Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.17 psi 2021-02-12T23:56:13Z- handleDepth()-Depth_Keller msg: 1613174173949 epoch sec; seqNo:8193; depth 19.77 2021-02-12T23:56:13Z- handleDvl() - RDI_Pathfinder msg: 1613174173.96 epoch sec; seqNo:3872 2021-02-12T23:56:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.65 , 94.33 , 68.25 , 107.72 2021-02-12T23:56:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , 0.08 2021-02-12T23:56:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174173.97 epoch sec; seqNo:8193; 4058210.70 north; 591759.61 east 2021-02-12T23:56:14Z- AHRS_M2 msg: 1613174174.34 epoch sec; seqNo:8128 2021-02-12T23:56:14Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.18 psi 2021-02-12T23:56:14Z- handleDepth()-Depth_Keller msg: 1613174174353 epoch sec; seqNo:8194; depth 19.92 2021-02-12T23:56:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:14Z- handleDvl() - RDI_Pathfinder msg: 1613174174.36 epoch sec; seqNo:3873 2021-02-12T23:56:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , 0.08 2021-02-12T23:56:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174174.37 epoch sec; seqNo:8194; 4058211.09 north; 591760.83 east 2021-02-12T23:56:14Z- AHRS_M2 msg: 1613174174.75 epoch sec; seqNo:8129 2021-02-12T23:56:14Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.18 psi 2021-02-12T23:56:14Z- handleDepth()-Depth_Keller msg: 1613174174752 epoch sec; seqNo:8195; depth 20.04 2021-02-12T23:56:14Z- handleDvl() - RDI_Pathfinder msg: 1613174174.77 epoch sec; seqNo:3874 2021-02-12T23:56:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.02 , 98.05 , 69.89 , 107.78 2021-02-12T23:56:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.05 2021-02-12T23:56:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174174.78 epoch sec; seqNo:8195; 4058211.09 north; 591760.83 east 2021-02-12T23:56:15Z- AHRS_M2 msg: 1613174175.15 epoch sec; seqNo:8130 2021-02-12T23:56:15Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.19 psi 2021-02-12T23:56:15Z- handleDepth()-Depth_Keller msg: 1613174175169 epoch sec; seqNo:8196; depth 20.18 2021-02-12T23:56:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174175.19 epoch sec; seqNo:8196; 4058211.47 north; 591760.82 east 2021-02-12T23:56:15Z- AHRS_M2 msg: 1613174175.56 epoch sec; seqNo:8131 2021-02-12T23:56:15Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.19 psi 2021-02-12T23:56:15Z- handleDepth()-Depth_Keller msg: 1613174175560 epoch sec; seqNo:8197; depth 20.32 2021-02-12T23:56:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:15Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:15Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:15Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:15Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:15Z- handleDvl() - RDI_Pathfinder msg: 1613174175.56 epoch sec; seqNo:3875 2021-02-12T23:56:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.05 2021-02-12T23:56:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174175.57 epoch sec; seqNo:8197; 4058211.85 north; 591760.82 east 2021-02-12T23:56:15Z- AHRS_M2 msg: 1613174175.96 epoch sec; seqNo:8132 2021-02-12T23:56:15Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.19 psi 2021-02-12T23:56:15Z- handleDepth()-Depth_Keller msg: 1613174175966 epoch sec; seqNo:8198; depth 20.45 2021-02-12T23:56:15Z- handleDvl() - RDI_Pathfinder msg: 1613174175.98 epoch sec; seqNo:3876 2021-02-12T23:56:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 95.48 , 69.07 , 108.86 2021-02-12T23:56:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , -0.02 2021-02-12T23:56:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174175.99 epoch sec; seqNo:8198; 4058212.23 north; 591760.81 east 2021-02-12T23:56:16Z- LcmTrn::handleMessages() - handling 43 messages took 0.62 ms and 131 calls... 2021-02-12T23:56:16Z- waiting for 1613174174.13, time is 1613174175.99 2021-02-12T23:56:16Z- LcmTrn::updateTrn() equating pose 1613174175.99 and meas times 1613174175.99 2021-02-12T23:56:16Z- LcmTrn::updateTrn() >>>> pose time 1613174175.99, meas time 1613174175.99 <<<< 2021-02-12T23:56:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174175.99 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:56:16Z- TerrainNav::motionUpdate() - currEstimate = (4058212, 591760.81, 20.45) 2021-02-12T23:56:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:56:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:56:16Z- TNavBF::Projecting Measurements 2021-02-12T23:56:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:56:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:56:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:56:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:56:16Z- TNavFilter::4 good beams. 2021-02-12T23:56:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:56:16Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:56:16Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:56:16Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:56:16Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:56:16Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:56:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:56:16Z- TNavBF::mmse selected from filter -1 2021-02-12T23:56:16Z- TerrainMapOctree: (vehN, vehE ) = (4058212.23, 591760.81). 2021-02-12T23:56:16Z- TerrainMapOctree: Min Distance = 3927.35. 2021-02-12T23:56:16Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:56:16Z- There is no specific good beam that all particles have in common. 2021-02-12T23:56:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:56:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 9.15 2021-02-12T23:56:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:56:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 14.59 2021-02-12T23:56:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:56:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-12T23:56:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:56:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 18.96 2021-02-12T23:56:16Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:56:16Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:56:16Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:56:16Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:56:16Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:56:16Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:56:16Z- Filter 0 No beams used in subcloud update 2021-02-12T23:56:16Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:56:16Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:56:16Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 727.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:56:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174175.99 sec. 2021-02-12T23:56:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:56:16Z- TNavBF::mmse selected from filter -1 2021-02-12T23:56:16Z- variances([0], [2], and psi_berg) = 727.546801 833.014800 0.000000 2021-02-12T23:56:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:56:16Z- LcmTrn::publishEstimates() - MLE : 4058206.73 591713.31 20.45 2021-02-12T23:56:16Z- LcmTrn::publishEstimates() - MMSE : 4058212.23 591760.81 20.45 2021-02-12T23:56:16Z- LcmTrn::publishEstimates() - COVAR: 727.55 833.01 0.88 0.00 2021-02-12T23:56:16Z- AHRS_M2 msg: 1613174176.36 epoch sec; seqNo:8133 2021-02-12T23:56:16Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.19 psi 2021-02-12T23:56:16Z- handleDepth()-Depth_Keller msg: 1613174176372 epoch sec; seqNo:8199; depth 20.57 2021-02-12T23:56:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174176.38 epoch sec; seqNo:8199; 4058212.23 north; 591760.81 east 2021-02-12T23:56:16Z- AHRS_M2 msg: 1613174176.77 epoch sec; seqNo:8134 2021-02-12T23:56:16Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.20 psi 2021-02-12T23:56:16Z- handleDepth()-Depth_Keller msg: 1613174176777 epoch sec; seqNo:8200; depth 20.71 2021-02-12T23:56:16Z- handleDvl() - RDI_Pathfinder msg: 1613174176.79 epoch sec; seqNo:3877 2021-02-12T23:56:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 97.98 , 67.43 , 107.72 2021-02-12T23:56:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.04 , -0.07 2021-02-12T23:56:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174176.80 epoch sec; seqNo:8200; 4058212.62 north; 591762.03 east 2021-02-12T23:56:17Z- AHRS_M2 msg: 1613174177.17 epoch sec; seqNo:8135 2021-02-12T23:56:17Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.20 psi 2021-02-12T23:56:17Z- handleDepth()-Depth_Keller msg: 1613174177177 epoch sec; seqNo:8201; depth 20.84 2021-02-12T23:56:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174177.19 epoch sec; seqNo:8201; 4058213.00 north; 591762.03 east 2021-02-12T23:56:17Z- AHRS_M2 msg: 1613174177.58 epoch sec; seqNo:8136 2021-02-12T23:56:17Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.20 psi 2021-02-12T23:56:17Z- handleDepth()-Depth_Keller msg: 1613174177580 epoch sec; seqNo:8202; depth 20.98 2021-02-12T23:56:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174177.59 epoch sec; seqNo:8202; 4058213.38 north; 591762.02 east 2021-02-12T23:56:17Z- AHRS_M2 msg: 1613174177.98 epoch sec; seqNo:8137 2021-02-12T23:56:17Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.20 psi 2021-02-12T23:56:17Z- handleDepth()-Depth_Keller msg: 1613174177989 epoch sec; seqNo:8203; depth 21.12 2021-02-12T23:56:18Z- handleDvl() - RDI_Pathfinder msg: 1613174178.00 epoch sec; seqNo:3878 2021-02-12T23:56:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.20 , 97.98 , 68.66 , 107.72 2021-02-12T23:56:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , -0.07 2021-02-12T23:56:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174178.01 epoch sec; seqNo:8203; 4058213.38 north; 591762.02 east 2021-02-12T23:56:18Z- AHRS_M2 msg: 1613174178.39 epoch sec; seqNo:8138 2021-02-12T23:56:18Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.20 psi 2021-02-12T23:56:18Z- handleDepth()-Depth_Keller msg: 1613174178396 epoch sec; seqNo:8204; depth 21.25 2021-02-12T23:56:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174178.41 epoch sec; seqNo:8204; 4058213.76 north; 591762.02 east 2021-02-12T23:56:18Z- AHRS_M2 msg: 1613174178.80 epoch sec; seqNo:8139 2021-02-12T23:56:18Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.20 psi 2021-02-12T23:56:18Z- handleDepth()-Depth_Keller msg: 1613174178803 epoch sec; seqNo:8205; depth 21.39 2021-02-12T23:56:18Z- handleDvl() - RDI_Pathfinder msg: 1613174178.81 epoch sec; seqNo:3879 2021-02-12T23:56:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 67.84 , 105.28 2021-02-12T23:56:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.06 , -0.03 2021-02-12T23:56:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174178.83 epoch sec; seqNo:8205; 4058214.13 north; 591762.01 east 2021-02-12T23:56:19Z- AHRS_M2 msg: 1613174179.19 epoch sec; seqNo:8140 2021-02-12T23:56:19Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.20 psi 2021-02-12T23:56:19Z- handleDepth()-Depth_Keller msg: 1613174179201 epoch sec; seqNo:8206; depth 21.53 2021-02-12T23:56:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:19Z- handleDvl() - RDI_Pathfinder msg: 1613174179.20 epoch sec; seqNo:3880 2021-02-12T23:56:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.06 , -0.03 2021-02-12T23:56:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174179.22 epoch sec; seqNo:8206; 4058214.53 north; 591763.23 east 2021-02-12T23:56:19Z- AHRS_M2 msg: 1613174179.60 epoch sec; seqNo:8141 2021-02-12T23:56:19Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.20 psi 2021-02-12T23:56:19Z- handleDepth()-Depth_Keller msg: 1613174179600 epoch sec; seqNo:8207; depth 21.66 2021-02-12T23:56:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174179.61 epoch sec; seqNo:8207; 4058214.53 north; 591763.23 east 2021-02-12T23:56:20Z- AHRS_M2 msg: 1613174180.00 epoch sec; seqNo:8142 2021-02-12T23:56:20Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.20 psi 2021-02-12T23:56:20Z- handleDepth()-Depth_Keller msg: 1613174180004 epoch sec; seqNo:8208; depth 21.80 2021-02-12T23:56:20Z- handleDvl() - RDI_Pathfinder msg: 1613174180.02 epoch sec; seqNo:3881 2021-02-12T23:56:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 97.98 , 68.66 , 105.28 2021-02-12T23:56:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , 0.04 2021-02-12T23:56:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174180.03 epoch sec; seqNo:8208; 4058214.91 north; 591763.23 east 2021-02-12T23:56:20Z- AHRS_M2 msg: 1613174180.40 epoch sec; seqNo:8143 2021-02-12T23:56:20Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.19 psi 2021-02-12T23:56:20Z- handleDepth()-Depth_Keller msg: 1613174180414 epoch sec; seqNo:8209; depth 21.93 2021-02-12T23:56:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:20Z- handleDvl() - RDI_Pathfinder msg: 1613174180.42 epoch sec; seqNo:3882 2021-02-12T23:56:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , 0.04 2021-02-12T23:56:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174180.43 epoch sec; seqNo:8209; 4058215.28 north; 591763.22 east 2021-02-12T23:56:20Z- AHRS_M2 msg: 1613174180.81 epoch sec; seqNo:8144 2021-02-12T23:56:20Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.19 psi 2021-02-12T23:56:20Z- handleDepth()-Depth_Keller msg: 1613174180812 epoch sec; seqNo:8210; depth 22.06 2021-02-12T23:56:20Z- handleDvl() - RDI_Pathfinder msg: 1613174180.82 epoch sec; seqNo:3883 2021-02-12T23:56:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.97 , 98.05 , 78.58 , 106.57 2021-02-12T23:56:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , 0.04 , 0.10 2021-02-12T23:56:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174180.85 epoch sec; seqNo:8210; 4058215.28 north; 591763.22 east 2021-02-12T23:56:21Z- AHRS_M2 msg: 1613174181.21 epoch sec; seqNo:8145 2021-02-12T23:56:21Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.19 psi 2021-02-12T23:56:21Z- handleDepth()-Depth_Keller msg: 1613174181216 epoch sec; seqNo:8211; depth 22.21 2021-02-12T23:56:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174181.23 epoch sec; seqNo:8211; 4058215.66 north; 591763.22 east 2021-02-12T23:56:21Z- LcmTrn::handleMessages() - handling 46 messages took 0.66 ms and 133 calls... 2021-02-12T23:56:21Z- waiting for 1613174178.99, time is 1613174181.23 2021-02-12T23:56:21Z- LcmTrn::updateTrn() >>>> pose time 1613174181.23, meas time 1613174180.82 <<<< 2021-02-12T23:56:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174180.82 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:56:21Z- TerrainNav::motionUpdate() - currEstimate = (4058216, 591763.22, 22.21) 2021-02-12T23:56:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:56:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:56:21Z- TNavBF::Projecting Measurements 2021-02-12T23:56:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:56:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:56:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:56:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:56:21Z- TNavFilter::4 good beams. 2021-02-12T23:56:21Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:56:21Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:56:21Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:56:21Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:56:21Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:56:21Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:56:21Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:56:21Z- TNavBF::mmse selected from filter -1 2021-02-12T23:56:21Z- TerrainMapOctree: (vehN, vehE ) = (4058215.39, 591763.03). 2021-02-12T23:56:21Z- TerrainMapOctree: Min Distance = 3923.50. 2021-02-12T23:56:21Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:56:21Z- There is no specific good beam that all particles have in common. 2021-02-12T23:56:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:56:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 8.76 2021-02-12T23:56:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:56:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.38 2021-02-12T23:56:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:56:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-12T23:56:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:56:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 18.17 2021-02-12T23:56:21Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:56:21Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:56:21Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:56:21Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:56:21Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:56:21Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:56:21Z- Filter 0 No beams used in subcloud update 2021-02-12T23:56:21Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:56:21Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:56:21Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 727.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:56:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174180.82 sec. 2021-02-12T23:56:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:56:21Z- TNavBF::mmse selected from filter -1 2021-02-12T23:56:21Z- variances([0], [2], and psi_berg) = 727.453403 833.014734 0.000000 2021-02-12T23:56:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:56:21Z- LcmTrn::publishEstimates() - MLE : 4058210.16 591715.72 22.21 2021-02-12T23:56:21Z- LcmTrn::publishEstimates() - MMSE : 4058215.66 591763.22 22.21 2021-02-12T23:56:21Z- LcmTrn::publishEstimates() - COVAR: 727.45 833.01 0.84 0.00 2021-02-12T23:56:21Z- AHRS_M2 msg: 1613174181.62 epoch sec; seqNo:8146 2021-02-12T23:56:21Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.19 psi 2021-02-12T23:56:21Z- handleDepth()-Depth_Keller msg: 1613174181623 epoch sec; seqNo:8212; depth 22.34 2021-02-12T23:56:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:21Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:21Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:21Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:21Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:21Z- handleDvl() - RDI_Pathfinder msg: 1613174181.63 epoch sec; seqNo:3884 2021-02-12T23:56:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , 0.04 , 0.10 2021-02-12T23:56:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174181.63 epoch sec; seqNo:8212; 4058215.66 north; 591763.22 east 2021-02-12T23:56:22Z- AHRS_M2 msg: 1613174182.03 epoch sec; seqNo:8147 2021-02-12T23:56:22Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.19 psi 2021-02-12T23:56:22Z- handleDepth()-Depth_Keller msg: 1613174182031 epoch sec; seqNo:8213; depth 22.49 2021-02-12T23:56:22Z- handleDvl() - RDI_Pathfinder msg: 1613174182.04 epoch sec; seqNo:3885 2021-02-12T23:56:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.97 , 92.98 , 67.84 , 106.36 2021-02-12T23:56:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.10 , 0.07 2021-02-12T23:56:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174182.05 epoch sec; seqNo:8213; 4058216.05 north; 591764.44 east 2021-02-12T23:56:22Z- AHRS_M2 msg: 1613174182.43 epoch sec; seqNo:8148 2021-02-12T23:56:22Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.19 psi 2021-02-12T23:56:22Z- handleDepth()-Depth_Keller msg: 1613174182431 epoch sec; seqNo:8214; depth 22.60 2021-02-12T23:56:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174182.45 epoch sec; seqNo:8214; 4058216.05 north; 591764.44 east 2021-02-12T23:56:22Z- AHRS_M2 msg: 1613174182.83 epoch sec; seqNo:8149 2021-02-12T23:56:22Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.19 psi 2021-02-12T23:56:22Z- handleDepth()-Depth_Keller msg: 1613174182837 epoch sec; seqNo:8215; depth 22.74 2021-02-12T23:56:22Z- handleDvl() - RDI_Pathfinder msg: 1613174182.85 epoch sec; seqNo:3886 2021-02-12T23:56:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 98.18 , 66.62 , 107.92 2021-02-12T23:56:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.11 , 0.03 2021-02-12T23:56:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174182.87 epoch sec; seqNo:8215; 4058216.43 north; 591764.43 east 2021-02-12T23:56:23Z- AHRS_M2 msg: 1613174183.23 epoch sec; seqNo:8150 2021-02-12T23:56:23Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.19 psi 2021-02-12T23:56:23Z- handleDepth()-Depth_Keller msg: 1613174183241 epoch sec; seqNo:8216; depth 22.88 2021-02-12T23:56:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174183.25 epoch sec; seqNo:8216; 4058216.43 north; 591764.43 east 2021-02-12T23:56:23Z- AHRS_M2 msg: 1613174183.64 epoch sec; seqNo:8151 2021-02-12T23:56:23Z- AHRS_M2 msg: 0.71 phi; -0.37 theta; -0.19 psi 2021-02-12T23:56:23Z- handleDepth()-Depth_Keller msg: 1613174183641 epoch sec; seqNo:8217; depth 23.01 2021-02-12T23:56:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174183.65 epoch sec; seqNo:8217; 4058216.81 north; 591764.43 east 2021-02-12T23:56:24Z- AHRS_M2 msg: 1613174184.04 epoch sec; seqNo:8152 2021-02-12T23:56:24Z- AHRS_M2 msg: 0.71 phi; -0.37 theta; -0.19 psi 2021-02-12T23:56:24Z- handleDepth()-Depth_Keller msg: 1613174184055 epoch sec; seqNo:8218; depth 23.16 2021-02-12T23:56:24Z- handleDvl() - RDI_Pathfinder msg: 1613174184.07 epoch sec; seqNo:3887 2021-02-12T23:56:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.97 , 98.05 , 67.03 , 107.78 2021-02-12T23:56:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.10 , -0.05 2021-02-12T23:56:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174184.08 epoch sec; seqNo:8218; 4058217.20 north; 591765.64 east 2021-02-12T23:56:24Z- AHRS_M2 msg: 1613174184.44 epoch sec; seqNo:8153 2021-02-12T23:56:24Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.19 psi 2021-02-12T23:56:24Z- handleDepth()-Depth_Keller msg: 1613174184448 epoch sec; seqNo:8219; depth 23.29 2021-02-12T23:56:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174184.46 epoch sec; seqNo:8219; 4058217.20 north; 591765.64 east 2021-02-12T23:56:24Z- AHRS_M2 msg: 1613174184.85 epoch sec; seqNo:8154 2021-02-12T23:56:24Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.19 psi 2021-02-12T23:56:24Z- handleDepth()-Depth_Keller msg: 1613174184852 epoch sec; seqNo:8220; depth 23.43 2021-02-12T23:56:24Z- handleDvl() - RDI_Pathfinder msg: 1613174184.86 epoch sec; seqNo:3888 2021-02-12T23:56:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 98.05 , 66.62 , 106.57 2021-02-12T23:56:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.03 , -0.10 2021-02-12T23:56:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174184.87 epoch sec; seqNo:8220; 4058217.58 north; 591765.64 east 2021-02-12T23:56:25Z- handleDepth()-Depth_Keller msg: 1613174185273 epoch sec; seqNo:8221; depth 23.59 2021-02-12T23:56:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174185.28 epoch sec; seqNo:8221; 4058217.96 north; 591765.64 east 2021-02-12T23:56:25Z- AHRS_M2 msg: 1613174185.67 epoch sec; seqNo:8155 2021-02-12T23:56:25Z- AHRS_M2 msg: 0.71 phi; -0.37 theta; -0.19 psi 2021-02-12T23:56:25Z- handleDepth()-Depth_Keller msg: 1613174185674 epoch sec; seqNo:8222; depth 23.72 2021-02-12T23:56:25Z- handleDvl() - RDI_Pathfinder msg: 1613174185.68 epoch sec; seqNo:3889 2021-02-12T23:56:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 98.12 , 66.62 , 106.63 2021-02-12T23:56:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.07 , -0.11 2021-02-12T23:56:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174185.69 epoch sec; seqNo:8222; 4058218.34 north; 591765.63 east 2021-02-12T23:56:26Z- AHRS_M2 msg: 1613174186.06 epoch sec; seqNo:8156 2021-02-12T23:56:26Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.20 psi 2021-02-12T23:56:26Z- handleDepth()-Depth_Keller msg: 1613174186069 epoch sec; seqNo:8223; depth 23.87 2021-02-12T23:56:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174186.08 epoch sec; seqNo:8223; 4058218.73 north; 591766.85 east 2021-02-12T23:56:26Z- AHRS_M2 msg: 1613174186.46 epoch sec; seqNo:8157 2021-02-12T23:56:26Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.21 psi 2021-02-12T23:56:26Z- LcmTrn::handleMessages() - handling 42 messages took 0.64 ms and 129 calls... 2021-02-12T23:56:26Z- waiting for 1613174184.23, time is 1613174186.46 2021-02-12T23:56:26Z- LcmTrn::updateTrn() >>>> pose time 1613174186.46, meas time 1613174185.68 <<<< 2021-02-12T23:56:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174185.68 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:56:26Z- TerrainNav::motionUpdate() - currEstimate = (4058219, 591766.85, 23.87) 2021-02-12T23:56:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:56:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:56:26Z- TNavBF::Projecting Measurements 2021-02-12T23:56:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:56:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:56:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:56:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:56:26Z- TNavFilter::4 good beams. 2021-02-12T23:56:26Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:56:26Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:56:26Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:56:26Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:56:26Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:56:26Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:56:26Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:56:26Z- TNavBF::mmse selected from filter -1 2021-02-12T23:56:26Z- TerrainMapOctree: (vehN, vehE ) = (4058218.28, 591766.31). 2021-02-12T23:56:26Z- TerrainMapOctree: Min Distance = 3919.19. 2021-02-12T23:56:26Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:56:26Z- There is no specific good beam that all particles have in common. 2021-02-12T23:56:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:56:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-12T23:56:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:56:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 15.40 2021-02-12T23:56:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:56:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 7.10 2021-02-12T23:56:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:56:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 18.19 2021-02-12T23:56:26Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:56:26Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:56:26Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:56:26Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:56:26Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:56:26Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:56:26Z- Filter 0 No beams used in subcloud update 2021-02-12T23:56:26Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:56:26Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:56:26Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 727.4 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:56:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174185.68 sec. 2021-02-12T23:56:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:56:26Z- TNavBF::mmse selected from filter -1 2021-02-12T23:56:26Z- variances([0], [2], and psi_berg) = 727.348545 833.014659 0.000000 2021-02-12T23:56:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:56:26Z- LcmTrn::publishEstimates() - MLE : 4058213.23 591719.35 23.87 2021-02-12T23:56:26Z- LcmTrn::publishEstimates() - MMSE : 4058218.73 591766.85 23.87 2021-02-12T23:56:26Z- LcmTrn::publishEstimates() - COVAR: 727.35 833.01 0.82 0.00 2021-02-12T23:56:26Z- handleDepth()-Depth_Keller msg: 1613174186469 epoch sec; seqNo:8224; depth 24.00 2021-02-12T23:56:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:26Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:26Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:26Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:26Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:26Z- handleDvl() - RDI_Pathfinder msg: 1613174186.47 epoch sec; seqNo:3890 2021-02-12T23:56:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.07 , -0.11 2021-02-12T23:56:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174186.48 epoch sec; seqNo:8224; 4058218.73 north; 591766.85 east 2021-02-12T23:56:26Z- AHRS_M2 msg: 1613174186.87 epoch sec; seqNo:8158 2021-02-12T23:56:26Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.22 psi 2021-02-12T23:56:26Z- handleDepth()-Depth_Keller msg: 1613174186872 epoch sec; seqNo:8225; depth 24.14 2021-02-12T23:56:26Z- handleDvl() - RDI_Pathfinder msg: 1613174186.88 epoch sec; seqNo:3891 2021-02-12T23:56:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 98.12 , 66.21 , 105.42 2021-02-12T23:56:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , -0.10 2021-02-12T23:56:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174186.91 epoch sec; seqNo:8225; 4058219.11 north; 591766.85 east 2021-02-12T23:56:27Z- AHRS_M2 msg: 1613174187.27 epoch sec; seqNo:8159 2021-02-12T23:56:27Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.23 psi 2021-02-12T23:56:27Z- handleDepth()-Depth_Keller msg: 1613174187276 epoch sec; seqNo:8226; depth 24.30 2021-02-12T23:56:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174187.29 epoch sec; seqNo:8226; 4058219.49 north; 591766.84 east 2021-02-12T23:56:27Z- AHRS_M2 msg: 1613174187.68 epoch sec; seqNo:8160 2021-02-12T23:56:27Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.24 psi 2021-02-12T23:56:27Z- handleDepth()-Depth_Keller msg: 1613174187684 epoch sec; seqNo:8227; depth 24.41 2021-02-12T23:56:27Z- handleDvl() - RDI_Pathfinder msg: 1613174187.70 epoch sec; seqNo:3892 2021-02-12T23:56:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 98.12 , 66.21 , 105.42 2021-02-12T23:56:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , -0.05 2021-02-12T23:56:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174187.70 epoch sec; seqNo:8227; 4058219.87 north; 591766.84 east 2021-02-12T23:56:28Z- AHRS_M2 msg: 1613174188.08 epoch sec; seqNo:8161 2021-02-12T23:56:28Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.24 psi 2021-02-12T23:56:28Z- handleDepth()-Depth_Keller msg: 1613174188084 epoch sec; seqNo:8228; depth 24.56 2021-02-12T23:56:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174188.10 epoch sec; seqNo:8228; 4058220.25 north; 591766.83 east 2021-02-12T23:56:28Z- AHRS_M2 msg: 1613174188.48 epoch sec; seqNo:8162 2021-02-12T23:56:28Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.25 psi 2021-02-12T23:56:28Z- handleDepth()-Depth_Keller msg: 1613174188488 epoch sec; seqNo:8229; depth 24.71 2021-02-12T23:56:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174188.50 epoch sec; seqNo:8229; 4058220.26 north; 591768.05 east 2021-02-12T23:56:28Z- AHRS_M2 msg: 1613174188.89 epoch sec; seqNo:8163 2021-02-12T23:56:28Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.25 psi 2021-02-12T23:56:28Z- handleDepth()-Depth_Keller msg: 1613174188897 epoch sec; seqNo:8230; depth 24.83 2021-02-12T23:56:28Z- handleDvl() - RDI_Pathfinder msg: 1613174188.91 epoch sec; seqNo:3893 2021-02-12T23:56:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 95.68 , 66.21 , 102.98 2021-02-12T23:56:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.06 , 0.01 2021-02-12T23:56:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174188.92 epoch sec; seqNo:8230; 4058220.64 north; 591768.05 east 2021-02-12T23:56:29Z- AHRS_M2 msg: 1613174189.29 epoch sec; seqNo:8164 2021-02-12T23:56:29Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.26 psi 2021-02-12T23:56:29Z- handleDepth()-Depth_Keller msg: 1613174189297 epoch sec; seqNo:8231; depth 24.99 2021-02-12T23:56:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174189.31 epoch sec; seqNo:8231; 4058221.02 north; 591768.05 east 2021-02-12T23:56:29Z- AHRS_M2 msg: 1613174189.70 epoch sec; seqNo:8165 2021-02-12T23:56:29Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.26 psi 2021-02-12T23:56:29Z- handleDepth()-Depth_Keller msg: 1613174189704 epoch sec; seqNo:8232; depth 25.11 2021-02-12T23:56:29Z- handleDvl() - RDI_Pathfinder msg: 1613174189.73 epoch sec; seqNo:3894 2021-02-12T23:56:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 98.32 , 66.21 , 104.40 2021-02-12T23:56:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , 0.05 2021-02-12T23:56:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174189.74 epoch sec; seqNo:8232; 4058221.02 north; 591768.05 east 2021-02-12T23:56:30Z- AHRS_M2 msg: 1613174190.10 epoch sec; seqNo:8166 2021-02-12T23:56:30Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.26 psi 2021-02-12T23:56:30Z- handleDepth()-Depth_Keller msg: 1613174190105 epoch sec; seqNo:8233; depth 25.26 2021-02-12T23:56:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:30Z- handleDvl() - RDI_Pathfinder msg: 1613174190.11 epoch sec; seqNo:3895 2021-02-12T23:56:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , 0.05 2021-02-12T23:56:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174190.12 epoch sec; seqNo:8233; 4058221.40 north; 591768.04 east 2021-02-12T23:56:30Z- AHRS_M2 msg: 1613174190.51 epoch sec; seqNo:8167 2021-02-12T23:56:30Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.25 psi 2021-02-12T23:56:30Z- handleDepth()-Depth_Keller msg: 1613174190509 epoch sec; seqNo:8234; depth 25.38 2021-02-12T23:56:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174190.52 epoch sec; seqNo:8234; 4058221.78 north; 591768.04 east 2021-02-12T23:56:30Z- AHRS_M2 msg: 1613174190.91 epoch sec; seqNo:8168 2021-02-12T23:56:30Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.24 psi 2021-02-12T23:56:30Z- handleDepth()-Depth_Keller msg: 1613174190912 epoch sec; seqNo:8235; depth 25.52 2021-02-12T23:56:30Z- handleDvl() - RDI_Pathfinder msg: 1613174190.92 epoch sec; seqNo:3896 2021-02-12T23:56:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 98.25 , 65.80 , 103.12 2021-02-12T23:56:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.01 , 0.05 2021-02-12T23:56:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174190.94 epoch sec; seqNo:8235; 4058221.78 north; 591768.04 east 2021-02-12T23:56:31Z- AHRS_M2 msg: 1613174191.31 epoch sec; seqNo:8169 2021-02-12T23:56:31Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.24 psi 2021-02-12T23:56:31Z- handleDepth()-Depth_Keller msg: 1613174191316 epoch sec; seqNo:8236; depth 25.67 2021-02-12T23:56:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:31Z- handleDvl() - RDI_Pathfinder msg: 1613174191.32 epoch sec; seqNo:3897 2021-02-12T23:56:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.01 , 0.05 2021-02-12T23:56:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174191.33 epoch sec; seqNo:8236; 4058222.17 north; 591769.25 east 2021-02-12T23:56:31Z- LcmTrn::handleMessages() - handling 46 messages took 0.63 ms and 134 calls... 2021-02-12T23:56:31Z- waiting for 1613174189.46, time is 1613174191.33 2021-02-12T23:56:31Z- LcmTrn::updateTrn() equating pose 1613174191.33 and meas times 1613174191.33 2021-02-12T23:56:31Z- LcmTrn::updateTrn() >>>> pose time 1613174191.33, meas time 1613174191.33 <<<< 2021-02-12T23:56:31Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174191.33, ping # 0. 2021-02-12T23:56:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174191.33 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:56:31Z- TerrainNav::motionUpdate() - currEstimate = (4058222, 591769.25, 25.67) 2021-02-12T23:56:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:56:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:56:31Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:56:31Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:56:31Z- TNavBF::Projecting Measurements 2021-02-12T23:56:31Z- TNavFilter::Measurement from time = 1613174191.33 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:56:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:56:31Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:56:31Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:56:31Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:56:31Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:56:31Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:56:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:56:31Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 727.3 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:56:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:56:31Z- TNavBF::mmse selected from filter -1 2021-02-12T23:56:31Z- variances([0], [2], and psi_berg) = 727.273354 833.014606 0.000000 2021-02-12T23:56:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:56:31Z- LcmTrn::publishEstimates() - MLE : 4058216.67 591721.75 25.67 2021-02-12T23:56:31Z- LcmTrn::publishEstimates() - MMSE : 4058222.17 591769.25 25.67 2021-02-12T23:56:31Z- LcmTrn::publishEstimates() - COVAR: 727.27 833.01 1.03 0.00 2021-02-12T23:56:31Z- AHRS_M2 msg: 1613174191.72 epoch sec; seqNo:8170 2021-02-12T23:56:31Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.23 psi 2021-02-12T23:56:31Z- handleDepth()-Depth_Keller msg: 1613174191723 epoch sec; seqNo:8237; depth 25.80 2021-02-12T23:56:31Z- handleDvl() - RDI_Pathfinder msg: 1613174191.73 epoch sec; seqNo:3898 2021-02-12T23:56:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 98.25 , 64.98 , 103.12 2021-02-12T23:56:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , 0.02 2021-02-12T23:56:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174191.75 epoch sec; seqNo:8237; 4058222.55 north; 591769.25 east 2021-02-12T23:56:32Z- AHRS_M2 msg: 1613174192.12 epoch sec; seqNo:8171 2021-02-12T23:56:32Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.23 psi 2021-02-12T23:56:32Z- handleDepth()-Depth_Keller msg: 1613174192127 epoch sec; seqNo:8238; depth 25.92 2021-02-12T23:56:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174192.15 epoch sec; seqNo:8238; 4058222.55 north; 591769.25 east 2021-02-12T23:56:32Z- AHRS_M2 msg: 1613174192.52 epoch sec; seqNo:8172 2021-02-12T23:56:32Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.22 psi 2021-02-12T23:56:32Z- handleDepth()-Depth_Keller msg: 1613174192533 epoch sec; seqNo:8239; depth 26.06 2021-02-12T23:56:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:32Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:32Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:32Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:32Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:32Z- handleDvl() - RDI_Pathfinder msg: 1613174192.54 epoch sec; seqNo:3899 2021-02-12T23:56:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , 0.02 2021-02-12T23:56:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174192.54 epoch sec; seqNo:8239; 4058222.93 north; 591769.25 east 2021-02-12T23:56:32Z- AHRS_M2 msg: 1613174192.93 epoch sec; seqNo:8173 2021-02-12T23:56:32Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.22 psi 2021-02-12T23:56:32Z- handleDepth()-Depth_Keller msg: 1613174192932 epoch sec; seqNo:8240; depth 26.19 2021-02-12T23:56:32Z- handleDvl() - RDI_Pathfinder msg: 1613174192.95 epoch sec; seqNo:3900 2021-02-12T23:56:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 98.25 , 64.16 , 99.47 2021-02-12T23:56:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.05 2021-02-12T23:56:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174192.96 epoch sec; seqNo:8240; 4058223.30 north; 591769.24 east 2021-02-12T23:56:33Z- AHRS_M2 msg: 1613174193.33 epoch sec; seqNo:8174 2021-02-12T23:56:33Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.21 psi 2021-02-12T23:56:33Z- handleDepth()-Depth_Keller msg: 1613174193338 epoch sec; seqNo:8241; depth 26.33 2021-02-12T23:56:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174193.35 epoch sec; seqNo:8241; 4058223.68 north; 591769.24 east 2021-02-12T23:56:33Z- AHRS_M2 msg: 1613174193.74 epoch sec; seqNo:8175 2021-02-12T23:56:33Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.21 psi 2021-02-12T23:56:33Z- handleDepth()-Depth_Keller msg: 1613174193741 epoch sec; seqNo:8242; depth 26.46 2021-02-12T23:56:33Z- handleDvl() - RDI_Pathfinder msg: 1613174193.76 epoch sec; seqNo:3901 2021-02-12T23:56:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 89.87 , 62.94 , 99.60 2021-02-12T23:56:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , -0.06 2021-02-12T23:56:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174193.77 epoch sec; seqNo:8242; 4058223.68 north; 591769.24 east 2021-02-12T23:56:34Z- AHRS_M2 msg: 1613174194.14 epoch sec; seqNo:8176 2021-02-12T23:56:34Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.20 psi 2021-02-12T23:56:34Z- handleDepth()-Depth_Keller msg: 1613174194147 epoch sec; seqNo:8243; depth 26.59 2021-02-12T23:56:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174194.16 epoch sec; seqNo:8243; 4058224.08 north; 591770.46 east 2021-02-12T23:56:34Z- AHRS_M2 msg: 1613174194.54 epoch sec; seqNo:8177 2021-02-12T23:56:34Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-12T23:56:34Z- handleDepth()-Depth_Keller msg: 1613174194548 epoch sec; seqNo:8244; depth 26.72 2021-02-12T23:56:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174194.56 epoch sec; seqNo:8244; 4058224.45 north; 591770.45 east 2021-02-12T23:56:34Z- AHRS_M2 msg: 1613174194.95 epoch sec; seqNo:8178 2021-02-12T23:56:34Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.18 psi 2021-02-12T23:56:34Z- handleDepth()-Depth_Keller msg: 1613174194965 epoch sec; seqNo:8245; depth 26.84 2021-02-12T23:56:34Z- handleDvl() - RDI_Pathfinder msg: 1613174194.98 epoch sec; seqNo:3902 2021-02-12T23:56:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 0.00 , 62.94 , 97.92 2021-02-12T23:56:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.03 2021-02-12T23:56:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174194.99 epoch sec; seqNo:8245; 4058224.83 north; 591770.45 east 2021-02-12T23:56:35Z- AHRS_M2 msg: 1613174195.35 epoch sec; seqNo:8179 2021-02-12T23:56:35Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.18 psi 2021-02-12T23:56:35Z- handleDepth()-Depth_Keller msg: 1613174195356 epoch sec; seqNo:8246; depth 26.99 2021-02-12T23:56:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174195.37 epoch sec; seqNo:8246; 4058225.21 north; 591770.44 east 2021-02-12T23:56:35Z- AHRS_M2 msg: 1613174195.76 epoch sec; seqNo:8180 2021-02-12T23:56:35Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.17 psi 2021-02-12T23:56:35Z- handleDepth()-Depth_Keller msg: 1613174195769 epoch sec; seqNo:8247; depth 27.13 2021-02-12T23:56:35Z- handleDvl() - RDI_Pathfinder msg: 1613174195.78 epoch sec; seqNo:3903 2021-02-12T23:56:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 86.08 , 62.53 , 97.99 2021-02-12T23:56:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.00 2021-02-12T23:56:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174195.79 epoch sec; seqNo:8247; 4058225.21 north; 591770.44 east 2021-02-12T23:56:36Z- AHRS_M2 msg: 1613174196.16 epoch sec; seqNo:8181 2021-02-12T23:56:36Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.17 psi 2021-02-12T23:56:36Z- handleDepth()-Depth_Keller msg: 1613174196170 epoch sec; seqNo:8248; depth 27.24 2021-02-12T23:56:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:36Z- handleDvl() - RDI_Pathfinder msg: 1613174196.17 epoch sec; seqNo:3904 2021-02-12T23:56:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.00 2021-02-12T23:56:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174196.18 epoch sec; seqNo:8248; 4058225.59 north; 591770.44 east 2021-02-12T23:56:36Z- AHRS_M2 msg: 1613174196.56 epoch sec; seqNo:8182 2021-02-12T23:56:36Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.16 psi 2021-02-12T23:56:36Z- handleDepth()-Depth_Keller msg: 1613174196568 epoch sec; seqNo:8249; depth 27.38 2021-02-12T23:56:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174196.58 epoch sec; seqNo:8249; 4058225.98 north; 591771.66 east 2021-02-12T23:56:36Z- LcmTrn::handleMessages() - handling 46 messages took 0.68 ms and 133 calls... 2021-02-12T23:56:36Z- waiting for 1613174194.33, time is 1613174196.58 2021-02-12T23:56:36Z- LcmTrn::updateTrn() >>>> pose time 1613174196.58, meas time 1613174196.17 <<<< 2021-02-12T23:56:36Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174196.17, ping # 0. 2021-02-12T23:56:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174196.17 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:56:36Z- TerrainNav::motionUpdate() - currEstimate = (4058226, 591771.66, 27.38) 2021-02-12T23:56:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:56:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:56:36Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:56:36Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:56:36Z- TNavBF::Projecting Measurements 2021-02-12T23:56:36Z- TNavFilter::Measurement from time = 1613174196.17 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:56:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:56:36Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:56:36Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:56:36Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:56:36Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:56:36Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:56:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:56:36Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 727.2 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:56:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:56:36Z- TNavBF::mmse selected from filter -1 2021-02-12T23:56:36Z- variances([0], [2], and psi_berg) = 727.176922 833.014538 0.000000 2021-02-12T23:56:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:56:36Z- LcmTrn::publishEstimates() - MLE : 4058220.48 591724.16 27.38 2021-02-12T23:56:36Z- LcmTrn::publishEstimates() - MMSE : 4058225.98 591771.66 27.38 2021-02-12T23:56:36Z- LcmTrn::publishEstimates() - COVAR: 727.18 833.01 1.26 0.00 2021-02-12T23:56:36Z- AHRS_M2 msg: 1613174196.97 epoch sec; seqNo:8183 2021-02-12T23:56:36Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.16 psi 2021-02-12T23:56:36Z- handleDepth()-Depth_Keller msg: 1613174196975 epoch sec; seqNo:8250; depth 27.50 2021-02-12T23:56:36Z- handleDvl() - RDI_Pathfinder msg: 1613174196.99 epoch sec; seqNo:3905 2021-02-12T23:56:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 87.16 , 63.35 , 97.98 2021-02-12T23:56:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.00 2021-02-12T23:56:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174197.00 epoch sec; seqNo:8250; 4058225.98 north; 591771.66 east 2021-02-12T23:56:37Z- AHRS_M2 msg: 1613174197.37 epoch sec; seqNo:8184 2021-02-12T23:56:37Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.16 psi 2021-02-12T23:56:37Z- handleDepth()-Depth_Keller msg: 1613174197378 epoch sec; seqNo:8251; depth 27.63 2021-02-12T23:56:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:37Z- handleDvl() - RDI_Pathfinder msg: 1613174197.38 epoch sec; seqNo:3906 2021-02-12T23:56:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.00 2021-02-12T23:56:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174197.39 epoch sec; seqNo:8251; 4058226.36 north; 591771.65 east 2021-02-12T23:56:37Z- AHRS_M2 msg: 1613174197.78 epoch sec; seqNo:8185 2021-02-12T23:56:37Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.16 psi 2021-02-12T23:56:37Z- handleDepth()-Depth_Keller msg: 1613174197780 epoch sec; seqNo:8252; depth 27.75 2021-02-12T23:56:37Z- handleDvl() - RDI_Pathfinder msg: 1613174197.79 epoch sec; seqNo:3907 2021-02-12T23:56:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 85.00 , 62.53 , 97.98 2021-02-12T23:56:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.01 2021-02-12T23:56:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174197.82 epoch sec; seqNo:8252; 4058226.74 north; 591771.65 east 2021-02-12T23:56:38Z- AHRS_M2 msg: 1613174198.18 epoch sec; seqNo:8186 2021-02-12T23:56:38Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.15 psi 2021-02-12T23:56:38Z- handleDepth()-Depth_Keller msg: 1613174198184 epoch sec; seqNo:8253; depth 27.87 2021-02-12T23:56:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174198.19 epoch sec; seqNo:8253; 4058227.12 north; 591771.64 east 2021-02-12T23:56:38Z- AHRS_M2 msg: 1613174198.61 epoch sec; seqNo:8187 2021-02-12T23:56:38Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.15 psi 2021-02-12T23:56:38Z- handleDepth()-Depth_Keller msg: 1613174198617 epoch sec; seqNo:8254; depth 28.00 2021-02-12T23:56:38Z- handleDvl() - RDI_Pathfinder msg: 1613174198.63 epoch sec; seqNo:3908 2021-02-12T23:56:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 0.00 , 61.71 , 97.98 2021-02-12T23:56:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.03 2021-02-12T23:56:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174198.64 epoch sec; seqNo:8254; 4058227.12 north; 591771.64 east 2021-02-12T23:56:39Z- handleDepth()-Depth_Keller msg: 1613174199009 epoch sec; seqNo:8255; depth 28.13 2021-02-12T23:56:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174199.02 epoch sec; seqNo:8255; 4058227.50 north; 591771.64 east 2021-02-12T23:56:39Z- AHRS_M2 msg: 1613174199.40 epoch sec; seqNo:8188 2021-02-12T23:56:39Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.15 psi 2021-02-12T23:56:39Z- handleDepth()-Depth_Keller msg: 1613174199403 epoch sec; seqNo:8256; depth 28.27 2021-02-12T23:56:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174199.42 epoch sec; seqNo:8256; 4058227.89 north; 591772.86 east 2021-02-12T23:56:39Z- AHRS_M2 msg: 1613174199.80 epoch sec; seqNo:8189 2021-02-12T23:56:39Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.15 psi 2021-02-12T23:56:39Z- handleDepth()-Depth_Keller msg: 1613174199805 epoch sec; seqNo:8257; depth 28.39 2021-02-12T23:56:39Z- handleDvl() - RDI_Pathfinder msg: 1613174199.83 epoch sec; seqNo:3909 2021-02-12T23:56:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 83.92 , 62.12 , 95.82 2021-02-12T23:56:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , -0.03 2021-02-12T23:56:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174199.85 epoch sec; seqNo:8257; 4058228.27 north; 591772.85 east 2021-02-12T23:56:40Z- AHRS_M2 msg: 1613174200.20 epoch sec; seqNo:8190 2021-02-12T23:56:40Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.15 psi 2021-02-12T23:56:40Z- handleDepth()-Depth_Keller msg: 1613174200204 epoch sec; seqNo:8258; depth 28.52 2021-02-12T23:56:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174200.22 epoch sec; seqNo:8258; 4058228.27 north; 591772.85 east 2021-02-12T23:56:40Z- AHRS_M2 msg: 1613174200.60 epoch sec; seqNo:8191 2021-02-12T23:56:40Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.15 psi 2021-02-12T23:56:40Z- handleDepth()-Depth_Keller msg: 1613174200608 epoch sec; seqNo:8259; depth 28.63 2021-02-12T23:56:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174200.62 epoch sec; seqNo:8259; 4058228.65 north; 591772.85 east 2021-02-12T23:56:41Z- AHRS_M2 msg: 1613174201.01 epoch sec; seqNo:8192 2021-02-12T23:56:41Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.15 psi 2021-02-12T23:56:41Z- handleDepth()-Depth_Keller msg: 1613174201017 epoch sec; seqNo:8260; depth 28.77 2021-02-12T23:56:41Z- handleDvl() - RDI_Pathfinder msg: 1613174201.03 epoch sec; seqNo:3910 2021-02-12T23:56:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.25 , 83.92 , 60.90 , 97.98 2021-02-12T23:56:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , -0.02 2021-02-12T23:56:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174201.05 epoch sec; seqNo:8260; 4058229.03 north; 591772.84 east 2021-02-12T23:56:41Z- AHRS_M2 msg: 1613174201.41 epoch sec; seqNo:8193 2021-02-12T23:56:41Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.15 psi 2021-02-12T23:56:41Z- handleDepth()-Depth_Keller msg: 1613174201418 epoch sec; seqNo:8261; depth 28.90 2021-02-12T23:56:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174201.43 epoch sec; seqNo:8261; 4058229.41 north; 591772.84 east 2021-02-12T23:56:41Z- LcmTrn::handleMessages() - handling 41 messages took 0.58 ms and 130 calls... 2021-02-12T23:56:41Z- waiting for 1613174199.58, time is 1613174201.43 2021-02-12T23:56:41Z- LcmTrn::updateTrn() >>>> pose time 1613174201.43, meas time 1613174201.03 <<<< 2021-02-12T23:56:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174201.03 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:56:41Z- TerrainNav::motionUpdate() - currEstimate = (4058229, 591772.84, 28.90) 2021-02-12T23:56:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:56:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:56:41Z- TNavBF::Projecting Measurements 2021-02-12T23:56:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:56:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:56:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:56:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:56:41Z- TNavFilter::4 good beams. 2021-02-12T23:56:41Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:56:41Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:56:41Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:56:41Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:56:41Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:56:41Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:56:41Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:56:41Z- TNavBF::mmse selected from filter -1 2021-02-12T23:56:41Z- TerrainMapOctree: (vehN, vehE ) = (4058229.12, 591772.74). 2021-02-12T23:56:41Z- TerrainMapOctree: Min Distance = 3906.73. 2021-02-12T23:56:41Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:56:41Z- There is no specific good beam that all particles have in common. 2021-02-12T23:56:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:56:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.135 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-12T23:56:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:56:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 11.27 2021-02-12T23:56:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:56:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.129 ExpectedMeasDiff: nan currMeas.covariance: 5.93 2021-02-12T23:56:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:56:41Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-12T23:56:41Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:56:41Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:56:41Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:56:41Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:56:41Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:56:41Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:56:41Z- Filter 0 No beams used in subcloud update 2021-02-12T23:56:41Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:56:41Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:56:41Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 727.1 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:56:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174201.03 sec. 2021-02-12T23:56:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:56:41Z- TNavBF::mmse selected from filter -1 2021-02-12T23:56:41Z- variances([0], [2], and psi_berg) = 727.090103 833.014476 0.000000 2021-02-12T23:56:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:56:41Z- LcmTrn::publishEstimates() - MLE : 4058223.91 591725.34 28.90 2021-02-12T23:56:41Z- LcmTrn::publishEstimates() - MMSE : 4058229.41 591772.84 28.90 2021-02-12T23:56:41Z- LcmTrn::publishEstimates() - COVAR: 727.09 833.01 0.95 0.00 2021-02-12T23:56:41Z- AHRS_M2 msg: 1613174201.82 epoch sec; seqNo:8194 2021-02-12T23:56:41Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.15 psi 2021-02-12T23:56:41Z- handleDepth()-Depth_Keller msg: 1613174201822 epoch sec; seqNo:8262; depth 29.01 2021-02-12T23:56:41Z- handleDvl() - RDI_Pathfinder msg: 1613174201.83 epoch sec; seqNo:3911 2021-02-12T23:56:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.25 , 79.29 , 61.30 , 97.98 2021-02-12T23:56:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , -0.00 2021-02-12T23:56:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174201.84 epoch sec; seqNo:8262; 4058229.42 north; 591774.06 east 2021-02-12T23:56:42Z- AHRS_M2 msg: 1613174202.22 epoch sec; seqNo:8195 2021-02-12T23:56:42Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.16 psi 2021-02-12T23:56:42Z- handleDepth()-Depth_Keller msg: 1613174202229 epoch sec; seqNo:8263; depth 29.15 2021-02-12T23:56:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:42Z- handleDvl() - RDI_Pathfinder msg: 1613174202.23 epoch sec; seqNo:3912 2021-02-12T23:56:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , -0.00 2021-02-12T23:56:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174202.25 epoch sec; seqNo:8263; 4058229.80 north; 591774.06 east 2021-02-12T23:56:42Z- AHRS_M2 msg: 1613174202.62 epoch sec; seqNo:8196 2021-02-12T23:56:42Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.17 psi 2021-02-12T23:56:42Z- handleDepth()-Depth_Keller msg: 1613174202628 epoch sec; seqNo:8264; depth 29.28 2021-02-12T23:56:42Z- handleDvl() - RDI_Pathfinder msg: 1613174202.64 epoch sec; seqNo:3913 2021-02-12T23:56:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.03 , 79.29 , 60.90 , 92.57 2021-02-12T23:56:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.03 2021-02-12T23:56:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174202.66 epoch sec; seqNo:8264; 4058230.18 north; 591774.05 east 2021-02-12T23:56:43Z- AHRS_M2 msg: 1613174203.03 epoch sec; seqNo:8197 2021-02-12T23:56:43Z- AHRS_M2 msg: 0.67 phi; -0.28 theta; -0.17 psi 2021-02-12T23:56:43Z- handleDepth()-Depth_Keller msg: 1613174203033 epoch sec; seqNo:8265; depth 29.41 2021-02-12T23:56:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174203.04 epoch sec; seqNo:8265; 4058230.56 north; 591774.05 east 2021-02-12T23:56:43Z- AHRS_M2 msg: 1613174203.43 epoch sec; seqNo:8198 2021-02-12T23:56:43Z- AHRS_M2 msg: 0.66 phi; -0.24 theta; -0.17 psi 2021-02-12T23:56:43Z- handleDepth()-Depth_Keller msg: 1613174203437 epoch sec; seqNo:8266; depth 29.56 2021-02-12T23:56:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:43Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:43Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:43Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:43Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:43Z- handleDvl() - RDI_Pathfinder msg: 1613174203.44 epoch sec; seqNo:3914 2021-02-12T23:56:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.03 2021-02-12T23:56:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174203.45 epoch sec; seqNo:8266; 4058230.56 north; 591774.05 east 2021-02-12T23:56:43Z- AHRS_M2 msg: 1613174203.84 epoch sec; seqNo:8199 2021-02-12T23:56:43Z- AHRS_M2 msg: 0.65 phi; -0.20 theta; -0.17 psi 2021-02-12T23:56:43Z- handleDepth()-Depth_Keller msg: 1613174203840 epoch sec; seqNo:8267; depth 29.68 2021-02-12T23:56:43Z- handleDvl() - RDI_Pathfinder msg: 1613174203.86 epoch sec; seqNo:3915 2021-02-12T23:56:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.21 , 78.47 , 60.49 , 88.25 2021-02-12T23:56:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.04 2021-02-12T23:56:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174203.87 epoch sec; seqNo:8267; 4058230.93 north; 591774.04 east 2021-02-12T23:56:44Z- AHRS_M2 msg: 1613174204.24 epoch sec; seqNo:8200 2021-02-12T23:56:44Z- AHRS_M2 msg: 0.64 phi; -0.16 theta; -0.16 psi 2021-02-12T23:56:44Z- handleDepth()-Depth_Keller msg: 1613174204249 epoch sec; seqNo:8268; depth 29.82 2021-02-12T23:56:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174204.26 epoch sec; seqNo:8268; 4058231.33 north; 591775.26 east 2021-02-12T23:56:44Z- AHRS_M2 msg: 1613174204.64 epoch sec; seqNo:8201 2021-02-12T23:56:44Z- AHRS_M2 msg: 0.63 phi; -0.12 theta; -0.16 psi 2021-02-12T23:56:44Z- handleDepth()-Depth_Keller msg: 1613174204653 epoch sec; seqNo:8269; depth 29.92 2021-02-12T23:56:44Z- handleDvl() - RDI_Pathfinder msg: 1613174204.66 epoch sec; seqNo:3916 2021-02-12T23:56:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.76 , 80.67 , 63.35 , 76.02 2021-02-12T23:56:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , 0.02 2021-02-12T23:56:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174204.67 epoch sec; seqNo:8269; 4058231.70 north; 591775.26 east 2021-02-12T23:56:45Z- AHRS_M2 msg: 1613174205.05 epoch sec; seqNo:8202 2021-02-12T23:56:45Z- AHRS_M2 msg: 0.62 phi; -0.08 theta; -0.16 psi 2021-02-12T23:56:45Z- handleDepth()-Depth_Keller msg: 1613174205053 epoch sec; seqNo:8270; depth 30.02 2021-02-12T23:56:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174205.07 epoch sec; seqNo:8270; 4058231.70 north; 591775.26 east 2021-02-12T23:56:45Z- AHRS_M2 msg: 1613174205.45 epoch sec; seqNo:8203 2021-02-12T23:56:45Z- AHRS_M2 msg: 0.61 phi; -0.03 theta; -0.16 psi 2021-02-12T23:56:45Z- handleDepth()-Depth_Keller msg: 1613174205460 epoch sec; seqNo:8271; depth 30.12 2021-02-12T23:56:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174205.48 epoch sec; seqNo:8271; 4058232.08 north; 591775.25 east 2021-02-12T23:56:45Z- AHRS_M2 msg: 1613174205.86 epoch sec; seqNo:8204 2021-02-12T23:56:45Z- AHRS_M2 msg: 0.60 phi; 0.01 theta; -0.16 psi 2021-02-12T23:56:45Z- handleDepth()-Depth_Keller msg: 1613174205861 epoch sec; seqNo:8272; depth 30.18 2021-02-12T23:56:45Z- handleDvl() - RDI_Pathfinder msg: 1613174205.87 epoch sec; seqNo:3917 2021-02-12T23:56:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.35 , 78.51 , 66.21 , 70.70 2021-02-12T23:56:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.03 , -0.00 2021-02-12T23:56:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174205.90 epoch sec; seqNo:8272; 4058232.46 north; 591775.25 east 2021-02-12T23:56:46Z- AHRS_M2 msg: 1613174206.30 epoch sec; seqNo:8205 2021-02-12T23:56:46Z- AHRS_M2 msg: 0.59 phi; 0.05 theta; -0.16 psi 2021-02-12T23:56:46Z- handleDepth()-Depth_Keller msg: 1613174206313 epoch sec; seqNo:8273; depth 30.24 2021-02-12T23:56:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174206.33 epoch sec; seqNo:8273; 4058232.84 north; 591775.25 east 2021-02-12T23:56:46Z- AHRS_M2 msg: 1613174206.66 epoch sec; seqNo:8206 2021-02-12T23:56:46Z- AHRS_M2 msg: 0.59 phi; 0.09 theta; -0.16 psi 2021-02-12T23:56:46Z- handleDepth()-Depth_Keller msg: 1613174206669 epoch sec; seqNo:8274; depth 30.30 2021-02-12T23:56:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174206.68 epoch sec; seqNo:8274; 4058232.85 north; 591776.47 east 2021-02-12T23:56:46Z- LcmTrn::handleMessages() - handling 46 messages took 0.71 ms and 132 calls... 2021-02-12T23:56:46Z- waiting for 1613174204.43, time is 1613174206.68 2021-02-12T23:56:46Z- LcmTrn::updateTrn() >>>> pose time 1613174206.68, meas time 1613174205.87 <<<< 2021-02-12T23:56:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174205.87 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:56:46Z- TerrainNav::motionUpdate() - currEstimate = (4058233, 591776.47, 30.30) 2021-02-12T23:56:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:56:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:56:46Z- TNavBF::Projecting Measurements 2021-02-12T23:56:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:56:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:56:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:56:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:56:46Z- TNavFilter::4 good beams. 2021-02-12T23:56:46Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:56:46Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:56:46Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:56:46Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:56:46Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:56:46Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:56:46Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:56:46Z- TNavBF::mmse selected from filter -1 2021-02-12T23:56:46Z- TerrainMapOctree: (vehN, vehE ) = (4058232.33, 591775.91). 2021-02-12T23:56:46Z- TerrainMapOctree: Min Distance = 3902.24. 2021-02-12T23:56:46Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:56:46Z- There is no specific good beam that all particles have in common. 2021-02-12T23:56:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:56:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.125 ExpectedMeasDiff: nan currMeas.covariance: 6.42 2021-02-12T23:56:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:56:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-12T23:56:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:56:46Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 7.01 2021-02-12T23:56:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:56:46Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-12T23:56:46Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:56:46Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:56:46Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:56:46Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:56:46Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:56:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:56:46Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:56:46Z- Filter 0 No beams used in subcloud update 2021-02-12T23:56:46Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:56:46Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:56:46Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 727.0 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:56:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174205.87 sec. 2021-02-12T23:56:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:56:46Z- TNavBF::mmse selected from filter -1 2021-02-12T23:56:46Z- variances([0], [2], and psi_berg) = 726.982718 833.014401 0.000000 2021-02-12T23:56:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:56:46Z- LcmTrn::publishEstimates() - MLE : 4058227.35 591728.97 30.30 2021-02-12T23:56:46Z- LcmTrn::publishEstimates() - MMSE : 4058232.85 591776.47 30.30 2021-02-12T23:56:46Z- LcmTrn::publishEstimates() - COVAR: 726.98 833.01 0.82 0.00 2021-02-12T23:56:47Z- AHRS_M2 msg: 1613174207.07 epoch sec; seqNo:8207 2021-02-12T23:56:47Z- AHRS_M2 msg: 0.58 phi; 0.12 theta; -0.16 psi 2021-02-12T23:56:47Z- handleDepth()-Depth_Keller msg: 1613174207072 epoch sec; seqNo:8275; depth 30.32 2021-02-12T23:56:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:47Z- handleDvl() - RDI_Pathfinder msg: 1613174207.07 epoch sec; seqNo:3918 2021-02-12T23:56:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.03 , -0.00 2021-02-12T23:56:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174207.09 epoch sec; seqNo:8275; 4058233.23 north; 591776.46 east 2021-02-12T23:56:47Z- AHRS_M2 msg: 1613174207.47 epoch sec; seqNo:8208 2021-02-12T23:56:47Z- AHRS_M2 msg: 0.57 phi; 0.15 theta; -0.16 psi 2021-02-12T23:56:47Z- handleDepth()-Depth_Keller msg: 1613174207476 epoch sec; seqNo:8276; depth 30.33 2021-02-12T23:56:47Z- handleDvl() - RDI_Pathfinder msg: 1613174207.49 epoch sec; seqNo:3919 2021-02-12T23:56:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 64.22 , 0.00 , 0.00 , 64.22 2021-02-12T23:56:47Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:56:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174207.52 epoch sec; seqNo:8276; 4058233.61 north; 591776.46 east 2021-02-12T23:56:47Z- AHRS_M2 msg: 1613174207.88 epoch sec; seqNo:8209 2021-02-12T23:56:47Z- AHRS_M2 msg: 0.57 phi; 0.18 theta; -0.16 psi 2021-02-12T23:56:47Z- handleDepth()-Depth_Keller msg: 1613174207880 epoch sec; seqNo:8277; depth 30.34 2021-02-12T23:56:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174207.89 epoch sec; seqNo:8277; 4058233.61 north; 591776.46 east 2021-02-12T23:56:48Z- AHRS_M2 msg: 1613174208.28 epoch sec; seqNo:8210 2021-02-12T23:56:48Z- AHRS_M2 msg: 0.56 phi; 0.21 theta; -0.16 psi 2021-02-12T23:56:48Z- handleDepth()-Depth_Keller msg: 1613174208287 epoch sec; seqNo:8278; depth 30.32 2021-02-12T23:56:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:48Z- handleDvl() - RDI_Pathfinder msg: 1613174208.29 epoch sec; seqNo:3920 2021-02-12T23:56:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:48Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:56:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174208.30 epoch sec; seqNo:8278; 4058233.99 north; 591776.45 east 2021-02-12T23:56:48Z- AHRS_M2 msg: 1613174208.68 epoch sec; seqNo:8211 2021-02-12T23:56:48Z- AHRS_M2 msg: 0.56 phi; 0.24 theta; -0.16 psi 2021-02-12T23:56:48Z- handleDepth()-Depth_Keller msg: 1613174208693 epoch sec; seqNo:8279; depth 30.29 2021-02-12T23:56:48Z- handleDvl() - RDI_Pathfinder msg: 1613174208.71 epoch sec; seqNo:3921 2021-02-12T23:56:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 65.12 , 0.00 , 0.00 , 62.46 2021-02-12T23:56:48Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:56:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174208.73 epoch sec; seqNo:8279; 4058234.37 north; 591776.45 east 2021-02-12T23:56:49Z- AHRS_M2 msg: 1613174209.09 epoch sec; seqNo:8212 2021-02-12T23:56:49Z- AHRS_M2 msg: 0.56 phi; 0.26 theta; -0.16 psi 2021-02-12T23:56:49Z- handleDepth()-Depth_Keller msg: 1613174209097 epoch sec; seqNo:8280; depth 30.26 2021-02-12T23:56:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174209.11 epoch sec; seqNo:8280; 4058234.76 north; 591777.67 east 2021-02-12T23:56:49Z- AHRS_M2 msg: 1613174209.49 epoch sec; seqNo:8213 2021-02-12T23:56:49Z- AHRS_M2 msg: 0.55 phi; 0.28 theta; -0.16 psi 2021-02-12T23:56:49Z- handleDepth()-Depth_Keller msg: 1613174209497 epoch sec; seqNo:8281; depth 30.20 2021-02-12T23:56:49Z- handleDvl() - RDI_Pathfinder msg: 1613174209.51 epoch sec; seqNo:3922 2021-02-12T23:56:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 66.45 , 0.00 , 0.00 , 62.02 2021-02-12T23:56:49Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:56:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174209.52 epoch sec; seqNo:8281; 4058234.76 north; 591777.67 east 2021-02-12T23:56:49Z- AHRS_M2 msg: 1613174209.90 epoch sec; seqNo:8214 2021-02-12T23:56:49Z- AHRS_M2 msg: 0.55 phi; 0.29 theta; -0.16 psi 2021-02-12T23:56:49Z- handleDepth()-Depth_Keller msg: 1613174209909 epoch sec; seqNo:8282; depth 30.15 2021-02-12T23:56:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174209.92 epoch sec; seqNo:8282; 4058235.14 north; 591777.66 east 2021-02-12T23:56:50Z- AHRS_M2 msg: 1613174210.30 epoch sec; seqNo:8215 2021-02-12T23:56:50Z- AHRS_M2 msg: 0.55 phi; 0.31 theta; -0.15 psi 2021-02-12T23:56:50Z- handleDepth()-Depth_Keller msg: 1613174210308 epoch sec; seqNo:8283; depth 30.09 2021-02-12T23:56:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174210.34 epoch sec; seqNo:8283; 4058235.52 north; 591777.66 east 2021-02-12T23:56:50Z- AHRS_M2 msg: 1613174210.70 epoch sec; seqNo:8216 2021-02-12T23:56:50Z- AHRS_M2 msg: 0.55 phi; 0.32 theta; -0.15 psi 2021-02-12T23:56:50Z- handleDepth()-Depth_Keller msg: 1613174210708 epoch sec; seqNo:8284; depth 30.02 2021-02-12T23:56:50Z- handleDvl() - RDI_Pathfinder msg: 1613174210.72 epoch sec; seqNo:3923 2021-02-12T23:56:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:50Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:56:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174210.75 epoch sec; seqNo:8284; 4058235.52 north; 591777.66 east 2021-02-12T23:56:51Z- AHRS_M2 msg: 1613174211.11 epoch sec; seqNo:8217 2021-02-12T23:56:51Z- AHRS_M2 msg: 0.55 phi; 0.33 theta; -0.14 psi 2021-02-12T23:56:51Z- handleDepth()-Depth_Keller msg: 1613174211118 epoch sec; seqNo:8285; depth 29.92 2021-02-12T23:56:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174211.13 epoch sec; seqNo:8285; 4058235.90 north; 591777.65 east 2021-02-12T23:56:51Z- AHRS_M2 msg: 1613174211.51 epoch sec; seqNo:8218 2021-02-12T23:56:51Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.14 psi 2021-02-12T23:56:51Z- handleDepth()-Depth_Keller msg: 1613174211517 epoch sec; seqNo:8286; depth 29.84 2021-02-12T23:56:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174211.53 epoch sec; seqNo:8286; 4058236.28 north; 591777.65 east 2021-02-12T23:56:51Z- LcmTrn::handleMessages() - handling 42 messages took 0.61 ms and 130 calls... 2021-02-12T23:56:51Z- waiting for 1613174209.68, time is 1613174211.53 2021-02-12T23:56:51Z- LcmTrn::updateTrn() >>>> pose time 1613174211.53, meas time 1613174210.72 <<<< 2021-02-12T23:56:51Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174210.72, ping # 0. 2021-02-12T23:56:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174210.72 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:56:51Z- TerrainNav::motionUpdate() - currEstimate = (4058236, 591777.65, 29.84) 2021-02-12T23:56:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:56:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:56:51Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:56:51Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:56:51Z- TNavBF::Projecting Measurements 2021-02-12T23:56:51Z- TNavFilter::Measurement from time = 1613174210.72 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:56:51Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:56:51Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:56:51Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:56:51Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:56:51Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:56:51Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:56:51Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:56:51Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 726.9 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:56:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:56:51Z- TNavBF::mmse selected from filter -1 2021-02-12T23:56:51Z- variances([0], [2], and psi_berg) = 726.890843 833.014336 0.000000 2021-02-12T23:56:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:56:51Z- LcmTrn::publishEstimates() - MLE : 4058230.78 591730.15 29.84 2021-02-12T23:56:51Z- LcmTrn::publishEstimates() - MMSE : 4058236.28 591777.65 29.84 2021-02-12T23:56:51Z- LcmTrn::publishEstimates() - COVAR: 726.89 833.01 1.00 0.00 2021-02-12T23:56:51Z- AHRS_M2 msg: 1613174211.92 epoch sec; seqNo:8219 2021-02-12T23:56:51Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.14 psi 2021-02-12T23:56:51Z- handleDepth()-Depth_Keller msg: 1613174211925 epoch sec; seqNo:8287; depth 29.74 2021-02-12T23:56:51Z- handleDvl() - RDI_Pathfinder msg: 1613174211.94 epoch sec; seqNo:3924 2021-02-12T23:56:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.26 , 80.68 , 97.99 , 60.49 2021-02-12T23:56:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.02 , -0.03 2021-02-12T23:56:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174211.95 epoch sec; seqNo:8287; 4058236.67 north; 591778.87 east 2021-02-12T23:56:52Z- AHRS_M2 msg: 1613174212.32 epoch sec; seqNo:8220 2021-02-12T23:56:52Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.13 psi 2021-02-12T23:56:52Z- handleDepth()-Depth_Keller msg: 1613174212324 epoch sec; seqNo:8288; depth 29.64 2021-02-12T23:56:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174212.34 epoch sec; seqNo:8288; 4058236.67 north; 591778.87 east 2021-02-12T23:56:52Z- AHRS_M2 msg: 1613174212.72 epoch sec; seqNo:8221 2021-02-12T23:56:52Z- AHRS_M2 msg: 0.56 phi; 0.37 theta; -0.13 psi 2021-02-12T23:56:52Z- handleDepth()-Depth_Keller msg: 1613174212728 epoch sec; seqNo:8289; depth 29.54 2021-02-12T23:56:52Z- handleDvl() - RDI_Pathfinder msg: 1613174212.74 epoch sec; seqNo:3925 2021-02-12T23:56:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 81.75 , 97.98 , 61.30 2021-02-12T23:56:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , -0.01 2021-02-12T23:56:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174212.75 epoch sec; seqNo:8289; 4058237.05 north; 591778.86 east 2021-02-12T23:56:53Z- AHRS_M2 msg: 1613174213.13 epoch sec; seqNo:8222 2021-02-12T23:56:53Z- AHRS_M2 msg: 0.56 phi; 0.37 theta; -0.13 psi 2021-02-12T23:56:53Z- handleDepth()-Depth_Keller msg: 1613174213138 epoch sec; seqNo:8290; depth 29.42 2021-02-12T23:56:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:53Z- handleDvl() - RDI_Pathfinder msg: 1613174213.14 epoch sec; seqNo:3926 2021-02-12T23:56:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , -0.01 2021-02-12T23:56:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174213.15 epoch sec; seqNo:8290; 4058237.43 north; 591778.86 east 2021-02-12T23:56:53Z- AHRS_M2 msg: 1613174213.53 epoch sec; seqNo:8223 2021-02-12T23:56:53Z- AHRS_M2 msg: 0.56 phi; 0.38 theta; -0.13 psi 2021-02-12T23:56:53Z- handleDepth()-Depth_Keller msg: 1613174213536 epoch sec; seqNo:8291; depth 29.31 2021-02-12T23:56:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174213.55 epoch sec; seqNo:8291; 4058237.43 north; 591778.86 east 2021-02-12T23:56:53Z- AHRS_M2 msg: 1613174213.94 epoch sec; seqNo:8224 2021-02-12T23:56:53Z- AHRS_M2 msg: 0.56 phi; 0.38 theta; -0.13 psi 2021-02-12T23:56:53Z- handleDepth()-Depth_Keller msg: 1613174213940 epoch sec; seqNo:8292; depth 29.20 2021-02-12T23:56:53Z- handleDvl() - RDI_Pathfinder msg: 1613174213.95 epoch sec; seqNo:3927 2021-02-12T23:56:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 81.75 , 97.98 , 60.08 2021-02-12T23:56:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , 0.01 2021-02-12T23:56:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174213.97 epoch sec; seqNo:8292; 4058237.81 north; 591778.85 east 2021-02-12T23:56:54Z- AHRS_M2 msg: 1613174214.34 epoch sec; seqNo:8225 2021-02-12T23:56:54Z- AHRS_M2 msg: 0.57 phi; 0.38 theta; -0.13 psi 2021-02-12T23:56:54Z- handleDepth()-Depth_Keller msg: 1613174214345 epoch sec; seqNo:8293; depth 29.07 2021-02-12T23:56:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:54Z- handleDvl() - RDI_Pathfinder msg: 1613174214.35 epoch sec; seqNo:3928 2021-02-12T23:56:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , 0.01 2021-02-12T23:56:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174214.36 epoch sec; seqNo:8293; 4058238.18 north; 591778.85 east 2021-02-12T23:56:54Z- AHRS_M2 msg: 1613174214.74 epoch sec; seqNo:8226 2021-02-12T23:56:54Z- AHRS_M2 msg: 0.57 phi; 0.38 theta; -0.13 psi 2021-02-12T23:56:54Z- handleDepth()-Depth_Keller msg: 1613174214750 epoch sec; seqNo:8294; depth 28.96 2021-02-12T23:56:54Z- handleDvl() - RDI_Pathfinder msg: 1613174214.76 epoch sec; seqNo:3929 2021-02-12T23:56:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.71 , 0.00 , 99.07 , 62.13 2021-02-12T23:56:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , 0.01 2021-02-12T23:56:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174214.77 epoch sec; seqNo:8294; 4058238.20 north; 591780.07 east 2021-02-12T23:56:55Z- AHRS_M2 msg: 1613174215.15 epoch sec; seqNo:8227 2021-02-12T23:56:55Z- AHRS_M2 msg: 0.57 phi; 0.38 theta; -0.13 psi 2021-02-12T23:56:55Z- handleDepth()-Depth_Keller msg: 1613174215154 epoch sec; seqNo:8295; depth 28.82 2021-02-12T23:56:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174215.19 epoch sec; seqNo:8295; 4058238.58 north; 591780.07 east 2021-02-12T23:56:55Z- AHRS_M2 msg: 1613174215.55 epoch sec; seqNo:8228 2021-02-12T23:56:55Z- AHRS_M2 msg: 0.57 phi; 0.38 theta; -0.14 psi 2021-02-12T23:56:55Z- handleDepth()-Depth_Keller msg: 1613174215556 epoch sec; seqNo:8296; depth 28.71 2021-02-12T23:56:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:55Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:55Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:55Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:55Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:55Z- handleDvl() - RDI_Pathfinder msg: 1613174215.56 epoch sec; seqNo:3930 2021-02-12T23:56:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , 0.01 2021-02-12T23:56:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174215.57 epoch sec; seqNo:8296; 4058238.96 north; 591780.06 east 2021-02-12T23:56:55Z- AHRS_M2 msg: 1613174215.96 epoch sec; seqNo:8229 2021-02-12T23:56:55Z- AHRS_M2 msg: 0.58 phi; 0.38 theta; -0.14 psi 2021-02-12T23:56:55Z- handleDepth()-Depth_Keller msg: 1613174215970 epoch sec; seqNo:8297; depth 28.59 2021-02-12T23:56:55Z- handleDvl() - RDI_Pathfinder msg: 1613174215.98 epoch sec; seqNo:3931 2021-02-12T23:56:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.71 , 85.00 , 97.99 , 60.49 2021-02-12T23:56:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , 0.02 2021-02-12T23:56:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174215.99 epoch sec; seqNo:8297; 4058239.33 north; 591780.06 east 2021-02-12T23:56:56Z- AHRS_M2 msg: 1613174216.37 epoch sec; seqNo:8230 2021-02-12T23:56:56Z- AHRS_M2 msg: 0.58 phi; 0.38 theta; -0.15 psi 2021-02-12T23:56:56Z- handleDepth()-Depth_Keller msg: 1613174216371 epoch sec; seqNo:8298; depth 28.45 2021-02-12T23:56:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174216.38 epoch sec; seqNo:8298; 4058239.33 north; 591780.06 east 2021-02-12T23:56:56Z- AHRS_M2 msg: 1613174216.77 epoch sec; seqNo:8231 2021-02-12T23:56:56Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.15 psi 2021-02-12T23:56:56Z- handleDepth()-Depth_Keller msg: 1613174216777 epoch sec; seqNo:8299; depth 28.32 2021-02-12T23:56:56Z- handleDvl() - RDI_Pathfinder msg: 1613174216.80 epoch sec; seqNo:3932 2021-02-12T23:56:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.71 , 85.00 , 99.07 , 60.90 2021-02-12T23:56:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.03 2021-02-12T23:56:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174216.81 epoch sec; seqNo:8299; 4058239.71 north; 591780.06 east 2021-02-12T23:56:56Z- LcmTrn::handleMessages() - handling 48 messages took 0.71 ms and 134 calls... 2021-02-12T23:56:56Z- waiting for 1613174214.53, time is 1613174216.81 2021-02-12T23:56:56Z- LcmTrn::updateTrn() equating pose 1613174216.81 and meas times 1613174216.81 2021-02-12T23:56:56Z- LcmTrn::updateTrn() >>>> pose time 1613174216.81, meas time 1613174216.81 <<<< 2021-02-12T23:56:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174216.81 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:56:56Z- TerrainNav::motionUpdate() - currEstimate = (4058240, 591780.06, 28.32) 2021-02-12T23:56:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:56:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:56:56Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174216.81 sec.; No relevant navigation data available 2021-02-12T23:56:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:56:56Z- TNavBF::mmse selected from filter -1 2021-02-12T23:56:56Z- variances([0], [2], and psi_berg) = 726.816054 833.014283 0.000000 2021-02-12T23:56:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:56:56Z- LcmTrn::publishEstimates() - MLE : 4058234.21 591732.56 28.32 2021-02-12T23:56:56Z- LcmTrn::publishEstimates() - MMSE : 4058239.71 591780.06 28.32 2021-02-12T23:56:56Z- LcmTrn::publishEstimates() - COVAR: 726.82 833.01 1.22 0.00 2021-02-12T23:56:57Z- AHRS_M2 msg: 1613174217.17 epoch sec; seqNo:8232 2021-02-12T23:56:57Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.15 psi 2021-02-12T23:56:57Z- handleDepth()-Depth_Keller msg: 1613174217176 epoch sec; seqNo:8300; depth 28.20 2021-02-12T23:56:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174217.19 epoch sec; seqNo:8300; 4058240.09 north; 591780.05 east 2021-02-12T23:56:57Z- AHRS_M2 msg: 1613174217.57 epoch sec; seqNo:8233 2021-02-12T23:56:57Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.15 psi 2021-02-12T23:56:57Z- handleDepth()-Depth_Keller msg: 1613174217578 epoch sec; seqNo:8301; depth 28.08 2021-02-12T23:56:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174217.59 epoch sec; seqNo:8301; 4058240.10 north; 591781.27 east 2021-02-12T23:56:57Z- AHRS_M2 msg: 1613174217.97 epoch sec; seqNo:8234 2021-02-12T23:56:57Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.15 psi 2021-02-12T23:56:57Z- handleDepth()-Depth_Keller msg: 1613174217985 epoch sec; seqNo:8302; depth 27.96 2021-02-12T23:56:58Z- handleDvl() - RDI_Pathfinder msg: 1613174218.01 epoch sec; seqNo:3933 2021-02-12T23:56:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.71 , 85.00 , 97.99 , 62.94 2021-02-12T23:56:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.04 2021-02-12T23:56:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174218.02 epoch sec; seqNo:8302; 4058240.48 north; 591781.27 east 2021-02-12T23:56:58Z- AHRS_M2 msg: 1613174218.38 epoch sec; seqNo:8235 2021-02-12T23:56:58Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.15 psi 2021-02-12T23:56:58Z- handleDepth()-Depth_Keller msg: 1613174218384 epoch sec; seqNo:8303; depth 27.80 2021-02-12T23:56:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174218.40 epoch sec; seqNo:8303; 4058240.86 north; 591781.26 east 2021-02-12T23:56:58Z- AHRS_M2 msg: 1613174218.78 epoch sec; seqNo:8236 2021-02-12T23:56:58Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.15 psi 2021-02-12T23:56:58Z- handleDepth()-Depth_Keller msg: 1613174218788 epoch sec; seqNo:8304; depth 27.69 2021-02-12T23:56:58Z- handleDvl() - RDI_Pathfinder msg: 1613174218.80 epoch sec; seqNo:3934 2021-02-12T23:56:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 85.00 , 100.15 , 62.94 2021-02-12T23:56:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , 0.05 2021-02-12T23:56:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174218.81 epoch sec; seqNo:8304; 4058241.24 north; 591781.26 east 2021-02-12T23:56:59Z- AHRS_M2 msg: 1613174219.19 epoch sec; seqNo:8237 2021-02-12T23:56:59Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.15 psi 2021-02-12T23:56:59Z- handleDepth()-Depth_Keller msg: 1613174219193 epoch sec; seqNo:8305; depth 27.55 2021-02-12T23:56:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:56:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:56:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:56:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:56:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:56:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:56:59Z- handleDvl() - RDI_Pathfinder msg: 1613174219.19 epoch sec; seqNo:3935 2021-02-12T23:56:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:56:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , 0.05 2021-02-12T23:56:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174219.21 epoch sec; seqNo:8305; 4058241.24 north; 591781.26 east 2021-02-12T23:56:59Z- AHRS_M2 msg: 1613174219.59 epoch sec; seqNo:8238 2021-02-12T23:56:59Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.15 psi 2021-02-12T23:56:59Z- handleDepth()-Depth_Keller msg: 1613174219596 epoch sec; seqNo:8306; depth 27.43 2021-02-12T23:56:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174219.61 epoch sec; seqNo:8306; 4058241.62 north; 591781.26 east 2021-02-12T23:56:59Z- AHRS_M2 msg: 1613174220.00 epoch sec; seqNo:8239 2021-02-12T23:56:59Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.15 psi 2021-02-12T23:57:00Z- handleDepth()-Depth_Keller msg: 1613174220005 epoch sec; seqNo:8307; depth 27.30 2021-02-12T23:57:00Z- handleDvl() - RDI_Pathfinder msg: 1613174220.02 epoch sec; seqNo:3936 2021-02-12T23:57:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 86.08 , 97.99 , 63.76 2021-02-12T23:57:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , 0.04 2021-02-12T23:57:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174220.03 epoch sec; seqNo:8307; 4058242.00 north; 591781.25 east 2021-02-12T23:57:00Z- AHRS_M2 msg: 1613174220.40 epoch sec; seqNo:8240 2021-02-12T23:57:00Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.14 psi 2021-02-12T23:57:00Z- handleDepth()-Depth_Keller msg: 1613174220409 epoch sec; seqNo:8308; depth 27.17 2021-02-12T23:57:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:00Z- handleDvl() - RDI_Pathfinder msg: 1613174220.41 epoch sec; seqNo:3937 2021-02-12T23:57:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , 0.04 2021-02-12T23:57:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174220.42 epoch sec; seqNo:8308; 4058242.39 north; 591782.47 east 2021-02-12T23:57:00Z- AHRS_M2 msg: 1613174220.81 epoch sec; seqNo:8241 2021-02-12T23:57:00Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.14 psi 2021-02-12T23:57:00Z- handleDepth()-Depth_Keller msg: 1613174220809 epoch sec; seqNo:8309; depth 27.06 2021-02-12T23:57:00Z- handleDvl() - RDI_Pathfinder msg: 1613174220.82 epoch sec; seqNo:3938 2021-02-12T23:57:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.94 , 85.95 , 100.01 , 62.94 2021-02-12T23:57:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , 0.02 2021-02-12T23:57:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174220.83 epoch sec; seqNo:8309; 4058242.39 north; 591782.47 east 2021-02-12T23:57:01Z- AHRS_M2 msg: 1613174221.21 epoch sec; seqNo:8242 2021-02-12T23:57:01Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.13 psi 2021-02-12T23:57:01Z- handleDepth()-Depth_Keller msg: 1613174221217 epoch sec; seqNo:8310; depth 26.94 2021-02-12T23:57:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174221.23 epoch sec; seqNo:8310; 4058242.77 north; 591782.46 east 2021-02-12T23:57:01Z- AHRS_M2 msg: 1613174221.61 epoch sec; seqNo:8243 2021-02-12T23:57:01Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.13 psi 2021-02-12T23:57:01Z- handleDepth()-Depth_Keller msg: 1613174221617 epoch sec; seqNo:8311; depth 26.80 2021-02-12T23:57:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:01Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:01Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:01Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:01Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:01Z- handleDvl() - RDI_Pathfinder msg: 1613174221.62 epoch sec; seqNo:3939 2021-02-12T23:57:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , 0.02 2021-02-12T23:57:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174221.63 epoch sec; seqNo:8311; 4058243.15 north; 591782.46 east 2021-02-12T23:57:01Z- LcmTrn::handleMessages() - handling 43 messages took 0.61 ms and 131 calls... 2021-02-12T23:57:01Z- waiting for 1613174219.81, time is 1613174221.63 2021-02-12T23:57:01Z- LcmTrn::updateTrn() equating pose 1613174221.63 and meas times 1613174221.63 2021-02-12T23:57:01Z- LcmTrn::updateTrn() >>>> pose time 1613174221.63, meas time 1613174221.63 <<<< 2021-02-12T23:57:01Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174221.63, ping # 0. 2021-02-12T23:57:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174221.63 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:57:01Z- TerrainNav::motionUpdate() - currEstimate = (4058243, 591782.46, 26.80) 2021-02-12T23:57:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:57:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:57:01Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:57:01Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:57:01Z- TNavBF::Projecting Measurements 2021-02-12T23:57:01Z- TNavFilter::Measurement from time = 1613174221.63 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:57:01Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:57:01Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:57:01Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:57:01Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:57:01Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:57:01Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:57:01Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:57:01Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 726.7 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-12T23:57:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:57:01Z- TNavBF::mmse selected from filter -1 2021-02-12T23:57:01Z- variances([0], [2], and psi_berg) = 726.741330 833.014231 0.000000 2021-02-12T23:57:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:57:01Z- LcmTrn::publishEstimates() - MLE : 4058237.65 591734.96 26.80 2021-02-12T23:57:01Z- LcmTrn::publishEstimates() - MMSE : 4058243.15 591782.46 26.80 2021-02-12T23:57:01Z- LcmTrn::publishEstimates() - COVAR: 726.74 833.01 1.43 0.00 2021-02-12T23:57:02Z- AHRS_M2 msg: 1613174222.02 epoch sec; seqNo:8244 2021-02-12T23:57:02Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.12 psi 2021-02-12T23:57:02Z- handleDepth()-Depth_Keller msg: 1613174222023 epoch sec; seqNo:8312; depth 26.68 2021-02-12T23:57:02Z- handleDvl() - RDI_Pathfinder msg: 1613174222.04 epoch sec; seqNo:3940 2021-02-12T23:57:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.35 , 86.02 , 97.92 , 63.76 2021-02-12T23:57:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.07 , -0.02 2021-02-12T23:57:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174222.05 epoch sec; seqNo:8312; 4058243.15 north; 591782.46 east 2021-02-12T23:57:02Z- AHRS_M2 msg: 1613174222.42 epoch sec; seqNo:8245 2021-02-12T23:57:02Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.12 psi 2021-02-12T23:57:02Z- handleDepth()-Depth_Keller msg: 1613174222429 epoch sec; seqNo:8313; depth 26.54 2021-02-12T23:57:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174222.44 epoch sec; seqNo:8313; 4058243.53 north; 591782.46 east 2021-02-12T23:57:02Z- AHRS_M2 msg: 1613174222.82 epoch sec; seqNo:8246 2021-02-12T23:57:02Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.11 psi 2021-02-12T23:57:02Z- handleDepth()-Depth_Keller msg: 1613174222828 epoch sec; seqNo:8314; depth 26.43 2021-02-12T23:57:02Z- handleDvl() - RDI_Pathfinder msg: 1613174222.84 epoch sec; seqNo:3941 2021-02-12T23:57:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 0.00 , 100.14 , 63.76 2021-02-12T23:57:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.06 , -0.05 2021-02-12T23:57:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174222.87 epoch sec; seqNo:8314; 4058243.92 north; 591783.67 east 2021-02-12T23:57:03Z- AHRS_M2 msg: 1613174223.23 epoch sec; seqNo:8247 2021-02-12T23:57:03Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.11 psi 2021-02-12T23:57:03Z- handleDepth()-Depth_Keller msg: 1613174223233 epoch sec; seqNo:8315; depth 26.31 2021-02-12T23:57:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174223.24 epoch sec; seqNo:8315; 4058243.92 north; 591783.67 east 2021-02-12T23:57:03Z- AHRS_M2 msg: 1613174223.64 epoch sec; seqNo:8248 2021-02-12T23:57:03Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.11 psi 2021-02-12T23:57:03Z- handleDepth()-Depth_Keller msg: 1613174223642 epoch sec; seqNo:8316; depth 26.18 2021-02-12T23:57:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174223.67 epoch sec; seqNo:8316; 4058244.30 north; 591783.67 east 2021-02-12T23:57:04Z- AHRS_M2 msg: 1613174224.04 epoch sec; seqNo:8249 2021-02-12T23:57:04Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.11 psi 2021-02-12T23:57:04Z- handleDepth()-Depth_Keller msg: 1613174224040 epoch sec; seqNo:8317; depth 26.04 2021-02-12T23:57:04Z- handleDvl() - RDI_Pathfinder msg: 1613174224.06 epoch sec; seqNo:3942 2021-02-12T23:57:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 84.86 , 100.01 , 63.76 2021-02-12T23:57:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.02 , -0.07 2021-02-12T23:57:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174224.07 epoch sec; seqNo:8317; 4058244.30 north; 591783.67 east 2021-02-12T23:57:04Z- AHRS_M2 msg: 1613174224.44 epoch sec; seqNo:8250 2021-02-12T23:57:04Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.12 psi 2021-02-12T23:57:04Z- handleDepth()-Depth_Keller msg: 1613174224444 epoch sec; seqNo:8318; depth 25.92 2021-02-12T23:57:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174224.46 epoch sec; seqNo:8318; 4058244.68 north; 591783.67 east 2021-02-12T23:57:04Z- AHRS_M2 msg: 1613174224.84 epoch sec; seqNo:8251 2021-02-12T23:57:04Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.13 psi 2021-02-12T23:57:04Z- handleDepth()-Depth_Keller msg: 1613174224853 epoch sec; seqNo:8319; depth 25.80 2021-02-12T23:57:04Z- handleDvl() - RDI_Pathfinder msg: 1613174224.86 epoch sec; seqNo:3943 2021-02-12T23:57:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 84.94 , 99.00 , 64.99 2021-02-12T23:57:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.04 , -0.05 2021-02-12T23:57:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174224.87 epoch sec; seqNo:8319; 4058245.06 north; 591783.66 east 2021-02-12T23:57:05Z- AHRS_M2 msg: 1613174225.25 epoch sec; seqNo:8252 2021-02-12T23:57:05Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.14 psi 2021-02-12T23:57:05Z- handleDepth()-Depth_Keller msg: 1613174225261 epoch sec; seqNo:8320; depth 25.67 2021-02-12T23:57:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:05Z- handleDvl() - RDI_Pathfinder msg: 1613174225.26 epoch sec; seqNo:3944 2021-02-12T23:57:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.04 , -0.05 2021-02-12T23:57:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174225.27 epoch sec; seqNo:8320; 4058245.07 north; 591784.88 east 2021-02-12T23:57:05Z- AHRS_M2 msg: 1613174225.66 epoch sec; seqNo:8253 2021-02-12T23:57:05Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.15 psi 2021-02-12T23:57:05Z- handleDepth()-Depth_Keller msg: 1613174225683 epoch sec; seqNo:8321; depth 25.54 2021-02-12T23:57:05Z- handleDvl() - RDI_Pathfinder msg: 1613174225.70 epoch sec; seqNo:3945 2021-02-12T23:57:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 86.96 , 103.19 , 64.17 2021-02-12T23:57:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.07 , 0.02 2021-02-12T23:57:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174225.71 epoch sec; seqNo:8321; 4058245.45 north; 591784.88 east 2021-02-12T23:57:06Z- AHRS_M2 msg: 1613174226.06 epoch sec; seqNo:8254 2021-02-12T23:57:06Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.16 psi 2021-02-12T23:57:06Z- handleDepth()-Depth_Keller msg: 1613174226060 epoch sec; seqNo:8322; depth 25.41 2021-02-12T23:57:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174226.07 epoch sec; seqNo:8322; 4058245.45 north; 591784.88 east 2021-02-12T23:57:06Z- AHRS_M2 msg: 1613174226.46 epoch sec; seqNo:8255 2021-02-12T23:57:06Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.16 psi 2021-02-12T23:57:06Z- handleDepth()-Depth_Keller msg: 1613174226469 epoch sec; seqNo:8323; depth 25.29 2021-02-12T23:57:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:06Z- handleDvl() - RDI_Pathfinder msg: 1613174226.47 epoch sec; seqNo:3946 2021-02-12T23:57:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.07 , 0.02 2021-02-12T23:57:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174226.48 epoch sec; seqNo:8323; 4058245.83 north; 591784.87 east 2021-02-12T23:57:06Z- AHRS_M2 msg: 1613174226.86 epoch sec; seqNo:8256 2021-02-12T23:57:06Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.17 psi 2021-02-12T23:57:06Z- handleDepth()-Depth_Keller msg: 1613174226870 epoch sec; seqNo:8324; depth 25.17 2021-02-12T23:57:06Z- handleDvl() - RDI_Pathfinder msg: 1613174226.88 epoch sec; seqNo:3947 2021-02-12T23:57:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.35 , 0.00 , 99.41 , 64.99 2021-02-12T23:57:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.09 , 0.07 2021-02-12T23:57:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174226.90 epoch sec; seqNo:8324; 4058246.21 north; 591784.87 east 2021-02-12T23:57:06Z- LcmTrn::handleMessages() - handling 47 messages took 0.69 ms and 133 calls... 2021-02-12T23:57:06Z- waiting for 1613174224.63, time is 1613174226.90 2021-02-12T23:57:06Z- LcmTrn::updateTrn() equating pose 1613174226.90 and meas times 1613174226.90 2021-02-12T23:57:06Z- LcmTrn::updateTrn() >>>> pose time 1613174226.90, meas time 1613174226.90 <<<< 2021-02-12T23:57:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174226.90 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:57:06Z- TerrainNav::motionUpdate() - currEstimate = (4058246, 591784.87, 25.17) 2021-02-12T23:57:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:57:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:57:06Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174226.90 sec.; No relevant navigation data available 2021-02-12T23:57:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:57:06Z- TNavBF::mmse selected from filter -1 2021-02-12T23:57:06Z- variances([0], [2], and psi_berg) = 726.669408 833.014180 0.000000 2021-02-12T23:57:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:57:06Z- LcmTrn::publishEstimates() - MLE : 4058240.71 591737.37 25.17 2021-02-12T23:57:06Z- LcmTrn::publishEstimates() - MMSE : 4058246.21 591784.87 25.17 2021-02-12T23:57:06Z- LcmTrn::publishEstimates() - COVAR: 726.67 833.01 1.63 0.00 2021-02-12T23:57:07Z- AHRS_M2 msg: 1613174227.27 epoch sec; seqNo:8257 2021-02-12T23:57:07Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.17 psi 2021-02-12T23:57:07Z- handleDepth()-Depth_Keller msg: 1613174227277 epoch sec; seqNo:8325; depth 25.04 2021-02-12T23:57:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174227.29 epoch sec; seqNo:8325; 4058246.58 north; 591784.87 east 2021-02-12T23:57:07Z- AHRS_M2 msg: 1613174227.67 epoch sec; seqNo:8258 2021-02-12T23:57:07Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.16 psi 2021-02-12T23:57:07Z- handleDepth()-Depth_Keller msg: 1613174227681 epoch sec; seqNo:8326; depth 24.91 2021-02-12T23:57:07Z- handleDvl() - RDI_Pathfinder msg: 1613174227.69 epoch sec; seqNo:3948 2021-02-12T23:57:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.94 , 0.00 , 101.23 , 64.99 2021-02-12T23:57:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.00 , 0.10 2021-02-12T23:57:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174227.70 epoch sec; seqNo:8326; 4058246.58 north; 591784.87 east 2021-02-12T23:57:08Z- AHRS_M2 msg: 1613174228.08 epoch sec; seqNo:8259 2021-02-12T23:57:08Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.16 psi 2021-02-12T23:57:08Z- handleDepth()-Depth_Keller msg: 1613174228087 epoch sec; seqNo:8327; depth 24.78 2021-02-12T23:57:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174228.10 epoch sec; seqNo:8327; 4058246.98 north; 591786.08 east 2021-02-12T23:57:08Z- AHRS_M2 msg: 1613174228.48 epoch sec; seqNo:8260 2021-02-12T23:57:08Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-12T23:57:08Z- handleDepth()-Depth_Keller msg: 1613174228489 epoch sec; seqNo:8328; depth 24.66 2021-02-12T23:57:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174228.50 epoch sec; seqNo:8328; 4058247.36 north; 591786.08 east 2021-02-12T23:57:08Z- AHRS_M2 msg: 1613174228.88 epoch sec; seqNo:8261 2021-02-12T23:57:08Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-12T23:57:08Z- handleDepth()-Depth_Keller msg: 1613174228893 epoch sec; seqNo:8329; depth 24.54 2021-02-12T23:57:08Z- handleDvl() - RDI_Pathfinder msg: 1613174228.90 epoch sec; seqNo:3949 2021-02-12T23:57:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.98 , 89.33 , 100.15 , 64.58 2021-02-12T23:57:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , 0.09 2021-02-12T23:57:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174228.93 epoch sec; seqNo:8329; 4058247.73 north; 591786.07 east 2021-02-12T23:57:09Z- AHRS_M2 msg: 1613174229.29 epoch sec; seqNo:8262 2021-02-12T23:57:09Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-12T23:57:09Z- handleDepth()-Depth_Keller msg: 1613174229293 epoch sec; seqNo:8330; depth 24.41 2021-02-12T23:57:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174229.31 epoch sec; seqNo:8330; 4058247.73 north; 591786.07 east 2021-02-12T23:57:09Z- AHRS_M2 msg: 1613174229.69 epoch sec; seqNo:8263 2021-02-12T23:57:09Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-12T23:57:09Z- handleDepth()-Depth_Keller msg: 1613174229698 epoch sec; seqNo:8331; depth 24.28 2021-02-12T23:57:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174229.71 epoch sec; seqNo:8331; 4058248.11 north; 591786.07 east 2021-02-12T23:57:10Z- AHRS_M2 msg: 1613174230.10 epoch sec; seqNo:8264 2021-02-12T23:57:10Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-12T23:57:10Z- handleDepth()-Depth_Keller msg: 1613174230100 epoch sec; seqNo:8332; depth 24.16 2021-02-12T23:57:10Z- handleDvl() - RDI_Pathfinder msg: 1613174230.12 epoch sec; seqNo:3950 2021-02-12T23:57:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 139.98 , 101.03 , 65.40 2021-02-12T23:57:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.12 , 0.03 2021-02-12T23:57:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174230.14 epoch sec; seqNo:8332; 4058248.49 north; 591786.07 east 2021-02-12T23:57:10Z- handleDepth()-Depth_Keller msg: 1613174230521 epoch sec; seqNo:8333; depth 24.03 2021-02-12T23:57:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174230.54 epoch sec; seqNo:8333; 4058248.50 north; 591787.29 east 2021-02-12T23:57:10Z- AHRS_M2 msg: 1613174230.90 epoch sec; seqNo:8265 2021-02-12T23:57:10Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-12T23:57:10Z- handleDepth()-Depth_Keller msg: 1613174230910 epoch sec; seqNo:8334; depth 23.91 2021-02-12T23:57:10Z- handleDvl() - RDI_Pathfinder msg: 1613174230.92 epoch sec; seqNo:3951 2021-02-12T23:57:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 87.91 , 104.14 , 65.81 2021-02-12T23:57:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.10 , 0.01 2021-02-12T23:57:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174230.93 epoch sec; seqNo:8334; 4058248.88 north; 591787.28 east 2021-02-12T23:57:11Z- AHRS_M2 msg: 1613174231.31 epoch sec; seqNo:8266 2021-02-12T23:57:11Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.16 psi 2021-02-12T23:57:11Z- handleDepth()-Depth_Keller msg: 1613174231322 epoch sec; seqNo:8335; depth 23.80 2021-02-12T23:57:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:11Z- handleDvl() - RDI_Pathfinder msg: 1613174231.32 epoch sec; seqNo:3952 2021-02-12T23:57:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.10 , 0.01 2021-02-12T23:57:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174231.34 epoch sec; seqNo:8335; 4058249.26 north; 591787.28 east 2021-02-12T23:57:11Z- AHRS_M2 msg: 1613174231.71 epoch sec; seqNo:8267 2021-02-12T23:57:11Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.16 psi 2021-02-12T23:57:11Z- handleDepth()-Depth_Keller msg: 1613174231716 epoch sec; seqNo:8336; depth 23.67 2021-02-12T23:57:11Z- handleDvl() - RDI_Pathfinder msg: 1613174231.73 epoch sec; seqNo:3953 2021-02-12T23:57:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:11Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:57:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174231.76 epoch sec; seqNo:8336; 4058249.26 north; 591787.28 east 2021-02-12T23:57:11Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-12T23:57:11Z- waiting for 1613174229.90, time is 1613174231.76 2021-02-12T23:57:11Z- LcmTrn::updateTrn() equating pose 1613174231.76 and meas times 1613174231.76 2021-02-12T23:57:11Z- LcmTrn::updateTrn() >>>> pose time 1613174231.76, meas time 1613174231.76 <<<< 2021-02-12T23:57:11Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174231.76, ping # 0. 2021-02-12T23:57:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174231.76 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:57:11Z- TerrainNav::motionUpdate() - currEstimate = (4058249, 591787.28, 23.67) 2021-02-12T23:57:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:57:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:57:11Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:57:11Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:57:11Z- TNavBF::Projecting Measurements 2021-02-12T23:57:11Z- TNavFilter::Measurement from time = 1613174231.76 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:57:11Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:57:11Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:57:11Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:57:11Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:57:11Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:57:11Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:57:11Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:57:11Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 726.6 EastVariance: 833.0 DepthVariance: 1.8 this->DepthBiasCov: 1.8 2021-02-12T23:57:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:57:11Z- TNavBF::mmse selected from filter -1 2021-02-12T23:57:11Z- variances([0], [2], and psi_berg) = 726.597547 833.014130 0.000000 2021-02-12T23:57:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:57:11Z- LcmTrn::publishEstimates() - MLE : 4058243.76 591738.78 23.67 2021-02-12T23:57:11Z- LcmTrn::publishEstimates() - MMSE : 4058249.26 591787.28 23.67 2021-02-12T23:57:11Z- LcmTrn::publishEstimates() - COVAR: 726.60 833.01 1.83 0.00 2021-02-12T23:57:12Z- AHRS_M2 msg: 1613174232.12 epoch sec; seqNo:8268 2021-02-12T23:57:12Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.16 psi 2021-02-12T23:57:12Z- handleDepth()-Depth_Keller msg: 1613174232129 epoch sec; seqNo:8337; depth 23.55 2021-02-12T23:57:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174232.14 epoch sec; seqNo:8337; 4058249.64 north; 591787.27 east 2021-02-12T23:57:12Z- AHRS_M2 msg: 1613174232.52 epoch sec; seqNo:8269 2021-02-12T23:57:12Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.17 psi 2021-02-12T23:57:12Z- handleDepth()-Depth_Keller msg: 1613174232528 epoch sec; seqNo:8338; depth 23.43 2021-02-12T23:57:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:12Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:12Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:12Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:12Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:12Z- handleDvl() - RDI_Pathfinder msg: 1613174232.53 epoch sec; seqNo:3954 2021-02-12T23:57:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:12Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:57:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174232.55 epoch sec; seqNo:8338; 4058250.03 north; 591788.49 east 2021-02-12T23:57:12Z- AHRS_M2 msg: 1613174232.92 epoch sec; seqNo:8270 2021-02-12T23:57:12Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.18 psi 2021-02-12T23:57:12Z- handleDepth()-Depth_Keller msg: 1613174232933 epoch sec; seqNo:8339; depth 23.31 2021-02-12T23:57:12Z- handleDvl() - RDI_Pathfinder msg: 1613174232.96 epoch sec; seqNo:3955 2021-02-12T23:57:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 0.00 , 100.69 , 67.44 2021-02-12T23:57:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.13 , -0.04 2021-02-12T23:57:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174232.97 epoch sec; seqNo:8339; 4058250.03 north; 591788.49 east 2021-02-12T23:57:13Z- AHRS_M2 msg: 1613174233.33 epoch sec; seqNo:8271 2021-02-12T23:57:13Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.18 psi 2021-02-12T23:57:13Z- handleDepth()-Depth_Keller msg: 1613174233337 epoch sec; seqNo:8340; depth 23.20 2021-02-12T23:57:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174233.35 epoch sec; seqNo:8340; 4058250.41 north; 591788.49 east 2021-02-12T23:57:13Z- AHRS_M2 msg: 1613174233.76 epoch sec; seqNo:8272 2021-02-12T23:57:13Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.20 psi 2021-02-12T23:57:13Z- handleDepth()-Depth_Keller msg: 1613174233765 epoch sec; seqNo:8341; depth 23.09 2021-02-12T23:57:13Z- handleDvl() - RDI_Pathfinder msg: 1613174233.79 epoch sec; seqNo:3956 2021-02-12T23:57:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.92 , 99.00 , 67.03 2021-02-12T23:57:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.03 , 0.00 2021-02-12T23:57:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174233.80 epoch sec; seqNo:8341; 4058250.79 north; 591788.48 east 2021-02-12T23:57:14Z- AHRS_M2 msg: 1613174234.14 epoch sec; seqNo:8273 2021-02-12T23:57:14Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.20 psi 2021-02-12T23:57:14Z- handleDepth()-Depth_Keller msg: 1613174234145 epoch sec; seqNo:8342; depth 22.96 2021-02-12T23:57:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174234.16 epoch sec; seqNo:8342; 4058250.79 north; 591788.48 east 2021-02-12T23:57:14Z- AHRS_M2 msg: 1613174234.54 epoch sec; seqNo:8274 2021-02-12T23:57:14Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.20 psi 2021-02-12T23:57:14Z- handleDepth()-Depth_Keller msg: 1613174234544 epoch sec; seqNo:8343; depth 22.86 2021-02-12T23:57:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174234.55 epoch sec; seqNo:8343; 4058251.18 north; 591789.70 east 2021-02-12T23:57:14Z- AHRS_M2 msg: 1613174234.94 epoch sec; seqNo:8275 2021-02-12T23:57:14Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.21 psi 2021-02-12T23:57:14Z- handleDepth()-Depth_Keller msg: 1613174234953 epoch sec; seqNo:8344; depth 22.74 2021-02-12T23:57:14Z- handleDvl() - RDI_Pathfinder msg: 1613174234.96 epoch sec; seqNo:3957 2021-02-12T23:57:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 98.12 , 99.34 , 68.26 2021-02-12T23:57:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.02 , 0.04 2021-02-12T23:57:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174234.99 epoch sec; seqNo:8344; 4058251.18 north; 591789.70 east 2021-02-12T23:57:15Z- AHRS_M2 msg: 1613174235.35 epoch sec; seqNo:8276 2021-02-12T23:57:15Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.21 psi 2021-02-12T23:57:15Z- handleDepth()-Depth_Keller msg: 1613174235353 epoch sec; seqNo:8345; depth 22.63 2021-02-12T23:57:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174235.37 epoch sec; seqNo:8345; 4058251.56 north; 591789.70 east 2021-02-12T23:57:15Z- AHRS_M2 msg: 1613174235.75 epoch sec; seqNo:8277 2021-02-12T23:57:15Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.21 psi 2021-02-12T23:57:15Z- handleDepth()-Depth_Keller msg: 1613174235757 epoch sec; seqNo:8346; depth 22.51 2021-02-12T23:57:15Z- handleDvl() - RDI_Pathfinder msg: 1613174235.77 epoch sec; seqNo:3958 2021-02-12T23:57:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.35 , 93.39 , 99.47 , 67.85 2021-02-12T23:57:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.03 , 0.04 2021-02-12T23:57:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174235.78 epoch sec; seqNo:8346; 4058251.94 north; 591789.69 east 2021-02-12T23:57:16Z- AHRS_M2 msg: 1613174236.16 epoch sec; seqNo:8278 2021-02-12T23:57:16Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.21 psi 2021-02-12T23:57:16Z- handleDepth()-Depth_Keller msg: 1613174236175 epoch sec; seqNo:8347; depth 22.41 2021-02-12T23:57:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:16Z- handleDvl() - RDI_Pathfinder msg: 1613174236.18 epoch sec; seqNo:3959 2021-02-12T23:57:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.03 , 0.04 2021-02-12T23:57:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174236.19 epoch sec; seqNo:8347; 4058251.94 north; 591789.69 east 2021-02-12T23:57:16Z- AHRS_M2 msg: 1613174236.56 epoch sec; seqNo:8279 2021-02-12T23:57:16Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.21 psi 2021-02-12T23:57:16Z- handleDepth()-Depth_Keller msg: 1613174236564 epoch sec; seqNo:8348; depth 22.29 2021-02-12T23:57:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174236.57 epoch sec; seqNo:8348; 4058252.32 north; 591789.69 east 2021-02-12T23:57:16Z- AHRS_M2 msg: 1613174236.96 epoch sec; seqNo:8280 2021-02-12T23:57:16Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.21 psi 2021-02-12T23:57:16Z- handleDepth()-Depth_Keller msg: 1613174236968 epoch sec; seqNo:8349; depth 22.18 2021-02-12T23:57:16Z- handleDvl() - RDI_Pathfinder msg: 1613174236.98 epoch sec; seqNo:3960 2021-02-12T23:57:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 92.24 , 103.19 , 67.44 2021-02-12T23:57:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.04 , -0.01 2021-02-12T23:57:16Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-12T23:57:16Z- waiting for 1613174234.76, time is 1613174236.98 2021-02-12T23:57:16Z- LcmTrn::updateTrn() equating pose 1613174236.98 and meas times 1613174236.98 2021-02-12T23:57:16Z- LcmTrn::updateTrn() >>>> pose time 1613174236.98, meas time 1613174236.98 <<<< 2021-02-12T23:57:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174236.98 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:57:16Z- TerrainNav::motionUpdate() - currEstimate = (4058252, 591789.69, 22.18) 2021-02-12T23:57:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:57:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:57:16Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174236.98 sec.; No relevant navigation data available 2021-02-12T23:57:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:57:16Z- TNavBF::mmse selected from filter -1 2021-02-12T23:57:16Z- variances([0], [2], and psi_berg) = 726.525746 833.014079 0.000000 2021-02-12T23:57:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:57:16Z- LcmTrn::publishEstimates() - MLE : 4058246.82 591741.19 22.18 2021-02-12T23:57:16Z- LcmTrn::publishEstimates() - MMSE : 4058252.32 591789.69 22.18 2021-02-12T23:57:16Z- LcmTrn::publishEstimates() - COVAR: 726.53 833.01 2.02 0.00 2021-02-12T23:57:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174236.99 epoch sec; seqNo:8349; 4058252.70 north; 591789.68 east 2021-02-12T23:57:17Z- AHRS_M2 msg: 1613174237.37 epoch sec; seqNo:8281 2021-02-12T23:57:17Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.21 psi 2021-02-12T23:57:17Z- handleDepth()-Depth_Keller msg: 1613174237377 epoch sec; seqNo:8350; depth 22.06 2021-02-12T23:57:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:17Z- handleDvl() - RDI_Pathfinder msg: 1613174237.38 epoch sec; seqNo:3961 2021-02-12T23:57:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.04 , -0.01 2021-02-12T23:57:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174237.39 epoch sec; seqNo:8350; 4058252.71 north; 591790.90 east 2021-02-12T23:57:17Z- AHRS_M2 msg: 1613174237.77 epoch sec; seqNo:8282 2021-02-12T23:57:17Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.22 psi 2021-02-12T23:57:17Z- handleDepth()-Depth_Keller msg: 1613174237776 epoch sec; seqNo:8351; depth 21.96 2021-02-12T23:57:17Z- handleDvl() - RDI_Pathfinder msg: 1613174237.79 epoch sec; seqNo:3962 2021-02-12T23:57:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.98 , 97.99 , 102.85 , 68.26 2021-02-12T23:57:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.06 , -0.06 2021-02-12T23:57:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174237.81 epoch sec; seqNo:8351; 4058253.09 north; 591790.90 east 2021-02-12T23:57:18Z- AHRS_M2 msg: 1613174238.18 epoch sec; seqNo:8283 2021-02-12T23:57:18Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.22 psi 2021-02-12T23:57:18Z- handleDepth()-Depth_Keller msg: 1613174238185 epoch sec; seqNo:8352; depth 21.84 2021-02-12T23:57:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174238.21 epoch sec; seqNo:8352; 4058253.47 north; 591790.90 east 2021-02-12T23:57:18Z- AHRS_M2 msg: 1613174238.58 epoch sec; seqNo:8284 2021-02-12T23:57:18Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.22 psi 2021-02-12T23:57:18Z- handleDepth()-Depth_Keller msg: 1613174238583 epoch sec; seqNo:8353; depth 21.72 2021-02-12T23:57:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:18Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:18Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:18Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:18Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:18Z- handleDvl() - RDI_Pathfinder msg: 1613174238.59 epoch sec; seqNo:3963 2021-02-12T23:57:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.06 , -0.06 2021-02-12T23:57:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174238.59 epoch sec; seqNo:8353; 4058253.47 north; 591790.90 east 2021-02-12T23:57:18Z- AHRS_M2 msg: 1613174238.98 epoch sec; seqNo:8285 2021-02-12T23:57:18Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.22 psi 2021-02-12T23:57:18Z- handleDepth()-Depth_Keller msg: 1613174238993 epoch sec; seqNo:8354; depth 21.62 2021-02-12T23:57:19Z- handleDvl() - RDI_Pathfinder msg: 1613174239.01 epoch sec; seqNo:3964 2021-02-12T23:57:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.57 , 98.12 , 100.56 , 69.07 2021-02-12T23:57:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , -0.09 2021-02-12T23:57:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174239.02 epoch sec; seqNo:8354; 4058253.85 north; 591790.89 east 2021-02-12T23:57:19Z- AHRS_M2 msg: 1613174239.39 epoch sec; seqNo:8286 2021-02-12T23:57:19Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.22 psi 2021-02-12T23:57:19Z- handleDepth()-Depth_Keller msg: 1613174239400 epoch sec; seqNo:8355; depth 21.51 2021-02-12T23:57:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174239.41 epoch sec; seqNo:8355; 4058254.23 north; 591790.89 east 2021-02-12T23:57:19Z- AHRS_M2 msg: 1613174239.79 epoch sec; seqNo:8287 2021-02-12T23:57:19Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.22 psi 2021-02-12T23:57:19Z- handleDepth()-Depth_Keller msg: 1613174239796 epoch sec; seqNo:8356; depth 21.38 2021-02-12T23:57:19Z- handleDvl() - RDI_Pathfinder msg: 1613174239.81 epoch sec; seqNo:3965 2021-02-12T23:57:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.71 , 98.19 , 105.49 , 69.07 2021-02-12T23:57:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.12 , -0.03 2021-02-12T23:57:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174239.83 epoch sec; seqNo:8356; 4058254.61 north; 591790.88 east 2021-02-12T23:57:20Z- AHRS_M2 msg: 1613174240.22 epoch sec; seqNo:8288 2021-02-12T23:57:20Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.21 psi 2021-02-12T23:57:20Z- handleDepth()-Depth_Keller msg: 1613174240231 epoch sec; seqNo:8357; depth 21.26 2021-02-12T23:57:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174240.24 epoch sec; seqNo:8357; 4058254.62 north; 591792.11 east 2021-02-12T23:57:20Z- AHRS_M2 msg: 1613174240.60 epoch sec; seqNo:8289 2021-02-12T23:57:20Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.21 psi 2021-02-12T23:57:20Z- handleDepth()-Depth_Keller msg: 1613174240609 epoch sec; seqNo:8358; depth 21.16 2021-02-12T23:57:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174240.62 epoch sec; seqNo:8358; 4058255.00 north; 591792.10 east 2021-02-12T23:57:21Z- AHRS_M2 msg: 1613174241.00 epoch sec; seqNo:8290 2021-02-12T23:57:21Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.20 psi 2021-02-12T23:57:21Z- handleDepth()-Depth_Keller msg: 1613174241017 epoch sec; seqNo:8359; depth 21.05 2021-02-12T23:57:21Z- handleDvl() - RDI_Pathfinder msg: 1613174241.03 epoch sec; seqNo:3966 2021-02-12T23:57:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 95.48 , 105.22 , 69.07 2021-02-12T23:57:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.13 , 0.04 2021-02-12T23:57:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174241.04 epoch sec; seqNo:8359; 4058255.38 north; 591792.10 east 2021-02-12T23:57:21Z- AHRS_M2 msg: 1613174241.41 epoch sec; seqNo:8291 2021-02-12T23:57:21Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.19 psi 2021-02-12T23:57:21Z- handleDepth()-Depth_Keller msg: 1613174241417 epoch sec; seqNo:8360; depth 20.94 2021-02-12T23:57:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174241.43 epoch sec; seqNo:8360; 4058255.75 north; 591792.09 east 2021-02-12T23:57:21Z- AHRS_M2 msg: 1613174241.81 epoch sec; seqNo:8292 2021-02-12T23:57:21Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.18 psi 2021-02-12T23:57:21Z- handleDepth()-Depth_Keller msg: 1613174241816 epoch sec; seqNo:8361; depth 20.82 2021-02-12T23:57:21Z- handleDvl() - RDI_Pathfinder msg: 1613174241.83 epoch sec; seqNo:3967 2021-02-12T23:57:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 94.40 , 102.92 , 68.67 2021-02-12T23:57:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.10 , 0.07 2021-02-12T23:57:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174241.85 epoch sec; seqNo:8361; 4058255.75 north; 591792.09 east 2021-02-12T23:57:22Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-12T23:57:22Z- waiting for 1613174239.98, time is 1613174241.85 2021-02-12T23:57:22Z- LcmTrn::updateTrn() equating pose 1613174241.85 and meas times 1613174241.85 2021-02-12T23:57:22Z- LcmTrn::updateTrn() >>>> pose time 1613174241.85, meas time 1613174241.85 <<<< 2021-02-12T23:57:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174241.85 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:57:22Z- TerrainNav::motionUpdate() - currEstimate = (4058256, 591792.09, 20.82) 2021-02-12T23:57:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:57:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:57:22Z- TNavBF::Projecting Measurements 2021-02-12T23:57:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:57:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:57:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:57:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:57:22Z- TNavFilter::4 good beams. 2021-02-12T23:57:22Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:57:22Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:57:22Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:57:22Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:57:22Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:57:22Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:57:22Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:57:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:57:22Z- TerrainMapOctree: (vehN, vehE ) = (4058255.75, 591792.09). 2021-02-12T23:57:22Z- TerrainMapOctree: Min Distance = 3873.86. 2021-02-12T23:57:22Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:57:22Z- There is no specific good beam that all particles have in common. 2021-02-12T23:57:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:57:22Z- Old Depth Bias: 0.00 DepthBiasCov: 2.24 K: 0.162 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-12T23:57:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:57:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.88 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 14.26 2021-02-12T23:57:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:57:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.68 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 16.95 2021-02-12T23:57:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:57:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.54 K: 0.142 ExpectedMeasDiff: nan currMeas.covariance: 7.54 2021-02-12T23:57:22Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:57:22Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:57:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:57:22Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:57:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:57:22Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:57:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:57:22Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:57:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:57:22Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:57:22Z- Filter 0 No beams used in subcloud update 2021-02-12T23:57:22Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:57:22Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:57:22Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 726.5 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-12T23:57:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174241.85 sec. 2021-02-12T23:57:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:57:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:57:22Z- variances([0], [2], and psi_berg) = 726.451276 833.014027 0.000000 2021-02-12T23:57:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:57:22Z- LcmTrn::publishEstimates() - MLE : 4058250.25 591743.59 20.82 2021-02-12T23:57:22Z- LcmTrn::publishEstimates() - MMSE : 4058255.75 591792.09 20.82 2021-02-12T23:57:22Z- LcmTrn::publishEstimates() - COVAR: 726.45 833.01 1.32 0.00 2021-02-12T23:57:22Z- AHRS_M2 msg: 1613174242.22 epoch sec; seqNo:8293 2021-02-12T23:57:22Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.18 psi 2021-02-12T23:57:22Z- handleDepth()-Depth_Keller msg: 1613174242223 epoch sec; seqNo:8362; depth 20.72 2021-02-12T23:57:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:22Z- handleDvl() - RDI_Pathfinder msg: 1613174242.22 epoch sec; seqNo:3968 2021-02-12T23:57:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.10 , 0.07 2021-02-12T23:57:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174242.24 epoch sec; seqNo:8362; 4058256.13 north; 591792.09 east 2021-02-12T23:57:22Z- AHRS_M2 msg: 1613174242.62 epoch sec; seqNo:8294 2021-02-12T23:57:22Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.17 psi 2021-02-12T23:57:22Z- handleDepth()-Depth_Keller msg: 1613174242637 epoch sec; seqNo:8363; depth 20.60 2021-02-12T23:57:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174242.66 epoch sec; seqNo:8363; 4058256.51 north; 591792.09 east 2021-02-12T23:57:23Z- AHRS_M2 msg: 1613174243.02 epoch sec; seqNo:8295 2021-02-12T23:57:23Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.17 psi 2021-02-12T23:57:23Z- handleDepth()-Depth_Keller msg: 1613174243028 epoch sec; seqNo:8364; depth 20.48 2021-02-12T23:57:23Z- handleDvl() - RDI_Pathfinder msg: 1613174243.05 epoch sec; seqNo:3969 2021-02-12T23:57:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.12 , 102.98 , 69.07 2021-02-12T23:57:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.06 2021-02-12T23:57:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174243.06 epoch sec; seqNo:8364; 4058256.89 north; 591792.08 east 2021-02-12T23:57:23Z- AHRS_M2 msg: 1613174243.43 epoch sec; seqNo:8296 2021-02-12T23:57:23Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.17 psi 2021-02-12T23:57:23Z- handleDepth()-Depth_Keller msg: 1613174243437 epoch sec; seqNo:8365; depth 20.37 2021-02-12T23:57:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:23Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:23Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:23Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:23Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:23Z- handleDvl() - RDI_Pathfinder msg: 1613174243.44 epoch sec; seqNo:3970 2021-02-12T23:57:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.06 2021-02-12T23:57:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174243.46 epoch sec; seqNo:8365; 4058257.28 north; 591793.30 east 2021-02-12T23:57:23Z- AHRS_M2 msg: 1613174243.84 epoch sec; seqNo:8297 2021-02-12T23:57:23Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.16 psi 2021-02-12T23:57:23Z- handleDepth()-Depth_Keller msg: 1613174243841 epoch sec; seqNo:8366; depth 20.27 2021-02-12T23:57:23Z- handleDvl() - RDI_Pathfinder msg: 1613174243.85 epoch sec; seqNo:3971 2021-02-12T23:57:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 94.13 , 102.65 , 68.25 2021-02-12T23:57:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.05 2021-02-12T23:57:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174243.86 epoch sec; seqNo:8366; 4058257.28 north; 591793.30 east 2021-02-12T23:57:24Z- AHRS_M2 msg: 1613174244.24 epoch sec; seqNo:8298 2021-02-12T23:57:24Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.16 psi 2021-02-12T23:57:24Z- handleDepth()-Depth_Keller msg: 1613174244242 epoch sec; seqNo:8367; depth 20.15 2021-02-12T23:57:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174244.25 epoch sec; seqNo:8367; 4058257.66 north; 591793.29 east 2021-02-12T23:57:24Z- AHRS_M2 msg: 1613174244.64 epoch sec; seqNo:8299 2021-02-12T23:57:24Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.16 psi 2021-02-12T23:57:24Z- handleDepth()-Depth_Keller msg: 1613174244650 epoch sec; seqNo:8368; depth 20.05 2021-02-12T23:57:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:24Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:24Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:24Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:24Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:24Z- handleDvl() - RDI_Pathfinder msg: 1613174244.65 epoch sec; seqNo:3972 2021-02-12T23:57:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.05 2021-02-12T23:57:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174244.66 epoch sec; seqNo:8368; 4058258.04 north; 591793.29 east 2021-02-12T23:57:25Z- AHRS_M2 msg: 1613174245.04 epoch sec; seqNo:8300 2021-02-12T23:57:25Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.15 psi 2021-02-12T23:57:25Z- handleDepth()-Depth_Keller msg: 1613174245048 epoch sec; seqNo:8369; depth 19.93 2021-02-12T23:57:25Z- handleDvl() - RDI_Pathfinder msg: 1613174245.06 epoch sec; seqNo:3973 2021-02-12T23:57:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 93.25 , 105.42 , 69.07 2021-02-12T23:57:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , 0.07 2021-02-12T23:57:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174245.08 epoch sec; seqNo:8369; 4058258.42 north; 591793.29 east 2021-02-12T23:57:25Z- AHRS_M2 msg: 1613174245.45 epoch sec; seqNo:8301 2021-02-12T23:57:25Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-12T23:57:25Z- handleDepth()-Depth_Keller msg: 1613174245457 epoch sec; seqNo:8370; depth 19.83 2021-02-12T23:57:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174245.47 epoch sec; seqNo:8370; 4058258.42 north; 591793.29 east 2021-02-12T23:57:25Z- AHRS_M2 msg: 1613174245.85 epoch sec; seqNo:8302 2021-02-12T23:57:25Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-12T23:57:25Z- handleDepth()-Depth_Keller msg: 1613174245858 epoch sec; seqNo:8371; depth 19.72 2021-02-12T23:57:25Z- handleDvl() - RDI_Pathfinder msg: 1613174245.87 epoch sec; seqNo:3974 2021-02-12T23:57:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 91.97 , 105.36 , 69.07 2021-02-12T23:57:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , 0.05 2021-02-12T23:57:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174245.88 epoch sec; seqNo:8371; 4058258.81 north; 591794.50 east 2021-02-12T23:57:26Z- AHRS_M2 msg: 1613174246.26 epoch sec; seqNo:8303 2021-02-12T23:57:26Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-12T23:57:26Z- handleDepth()-Depth_Keller msg: 1613174246271 epoch sec; seqNo:8372; depth 19.60 2021-02-12T23:57:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174246.28 epoch sec; seqNo:8372; 4058259.19 north; 591794.50 east 2021-02-12T23:57:26Z- AHRS_M2 msg: 1613174246.67 epoch sec; seqNo:8304 2021-02-12T23:57:26Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.14 psi 2021-02-12T23:57:26Z- handleDepth()-Depth_Keller msg: 1613174246676 epoch sec; seqNo:8373; depth 19.50 2021-02-12T23:57:26Z- handleDvl() - RDI_Pathfinder msg: 1613174246.69 epoch sec; seqNo:3975 2021-02-12T23:57:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 92.04 , 105.42 , 70.30 2021-02-12T23:57:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.05 , -0.01 2021-02-12T23:57:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174246.71 epoch sec; seqNo:8373; 4058259.19 north; 591794.50 east 2021-02-12T23:57:27Z- AHRS_M2 msg: 1613174247.06 epoch sec; seqNo:8305 2021-02-12T23:57:27Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-12T23:57:27Z- handleDepth()-Depth_Keller msg: 1613174247068 epoch sec; seqNo:8374; depth 19.37 2021-02-12T23:57:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:27Z- handleDvl() - RDI_Pathfinder msg: 1613174247.07 epoch sec; seqNo:3976 2021-02-12T23:57:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.05 , -0.01 2021-02-12T23:57:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174247.08 epoch sec; seqNo:8374; 4058259.57 north; 591794.49 east 2021-02-12T23:57:27Z- LcmTrn::handleMessages() - handling 48 messages took 0.74 ms and 133 calls... 2021-02-12T23:57:27Z- waiting for 1613174244.85, time is 1613174247.08 2021-02-12T23:57:27Z- LcmTrn::updateTrn() equating pose 1613174247.08 and meas times 1613174247.08 2021-02-12T23:57:27Z- LcmTrn::updateTrn() >>>> pose time 1613174247.08, meas time 1613174247.08 <<<< 2021-02-12T23:57:27Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174247.08, ping # 0. 2021-02-12T23:57:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174247.08 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:57:27Z- TerrainNav::motionUpdate() - currEstimate = (4058260, 591794.49, 19.37) 2021-02-12T23:57:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:57:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:57:27Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:57:27Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:57:27Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174247.08 sec.; No relevant navigation data available 2021-02-12T23:57:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:57:27Z- TNavBF::mmse selected from filter -1 2021-02-12T23:57:27Z- variances([0], [2], and psi_berg) = 726.374140 833.013973 0.000000 2021-02-12T23:57:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:57:27Z- LcmTrn::publishEstimates() - MLE : 4058254.07 591745.99 19.37 2021-02-12T23:57:27Z- LcmTrn::publishEstimates() - MMSE : 4058259.57 591794.49 19.37 2021-02-12T23:57:27Z- LcmTrn::publishEstimates() - COVAR: 726.37 833.01 1.55 0.00 2021-02-12T23:57:27Z- AHRS_M2 msg: 1613174247.47 epoch sec; seqNo:8306 2021-02-12T23:57:27Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-12T23:57:27Z- handleDepth()-Depth_Keller msg: 1613174247474 epoch sec; seqNo:8375; depth 19.28 2021-02-12T23:57:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174247.48 epoch sec; seqNo:8375; 4058259.95 north; 591794.49 east 2021-02-12T23:57:27Z- AHRS_M2 msg: 1613174247.87 epoch sec; seqNo:8307 2021-02-12T23:57:27Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-12T23:57:27Z- handleDepth()-Depth_Keller msg: 1613174247876 epoch sec; seqNo:8376; depth 19.16 2021-02-12T23:57:27Z- handleDvl() - RDI_Pathfinder msg: 1613174247.89 epoch sec; seqNo:3977 2021-02-12T23:57:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.66 , 97.85 , 103.94 , 69.89 2021-02-12T23:57:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , -0.05 2021-02-12T23:57:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174247.90 epoch sec; seqNo:8376; 4058259.95 north; 591794.49 east 2021-02-12T23:57:28Z- AHRS_M2 msg: 1613174248.28 epoch sec; seqNo:8308 2021-02-12T23:57:28Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.14 psi 2021-02-12T23:57:28Z- handleDepth()-Depth_Keller msg: 1613174248285 epoch sec; seqNo:8377; depth 19.06 2021-02-12T23:57:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:28Z- handleDvl() - RDI_Pathfinder msg: 1613174248.29 epoch sec; seqNo:3978 2021-02-12T23:57:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , -0.05 2021-02-12T23:57:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174248.30 epoch sec; seqNo:8377; 4058260.33 north; 591794.49 east 2021-02-12T23:57:28Z- AHRS_M2 msg: 1613174248.68 epoch sec; seqNo:8309 2021-02-12T23:57:28Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-12T23:57:28Z- handleDepth()-Depth_Keller msg: 1613174248684 epoch sec; seqNo:8378; depth 18.94 2021-02-12T23:57:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174248.69 epoch sec; seqNo:8378; 4058260.72 north; 591795.70 east 2021-02-12T23:57:29Z- AHRS_M2 msg: 1613174249.08 epoch sec; seqNo:8310 2021-02-12T23:57:29Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-12T23:57:29Z- handleDepth()-Depth_Keller msg: 1613174249090 epoch sec; seqNo:8379; depth 18.84 2021-02-12T23:57:29Z- handleDvl() - RDI_Pathfinder msg: 1613174249.10 epoch sec; seqNo:3979 2021-02-12T23:57:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.73 , 94.33 , 104.07 , 69.89 2021-02-12T23:57:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.08 , -0.05 2021-02-12T23:57:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174249.11 epoch sec; seqNo:8379; 4058260.72 north; 591795.70 east 2021-02-12T23:57:29Z- AHRS_M2 msg: 1613174249.49 epoch sec; seqNo:8311 2021-02-12T23:57:29Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-12T23:57:29Z- handleDepth()-Depth_Keller msg: 1613174249493 epoch sec; seqNo:8380; depth 18.72 2021-02-12T23:57:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:29Z- handleDvl() - RDI_Pathfinder msg: 1613174249.49 epoch sec; seqNo:3980 2021-02-12T23:57:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.08 , -0.05 2021-02-12T23:57:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174249.52 epoch sec; seqNo:8380; 4058261.10 north; 591795.70 east 2021-02-12T23:57:29Z- AHRS_M2 msg: 1613174249.89 epoch sec; seqNo:8312 2021-02-12T23:57:29Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-12T23:57:29Z- handleDepth()-Depth_Keller msg: 1613174249896 epoch sec; seqNo:8381; depth 18.62 2021-02-12T23:57:29Z- handleDvl() - RDI_Pathfinder msg: 1613174249.91 epoch sec; seqNo:3981 2021-02-12T23:57:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 104.13 , 70.70 2021-02-12T23:57:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.12 , 0.00 2021-02-12T23:57:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174249.93 epoch sec; seqNo:8381; 4058261.48 north; 591795.69 east 2021-02-12T23:57:30Z- AHRS_M2 msg: 1613174250.30 epoch sec; seqNo:8313 2021-02-12T23:57:30Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-12T23:57:30Z- handleDepth()-Depth_Keller msg: 1613174250307 epoch sec; seqNo:8382; depth 18.48 2021-02-12T23:57:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174250.32 epoch sec; seqNo:8382; 4058261.86 north; 591795.69 east 2021-02-12T23:57:30Z- AHRS_M2 msg: 1613174250.71 epoch sec; seqNo:8314 2021-02-12T23:57:30Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.19 psi 2021-02-12T23:57:30Z- handleDepth()-Depth_Keller msg: 1613174250711 epoch sec; seqNo:8383; depth 18.39 2021-02-12T23:57:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:30Z- handleDvl() - RDI_Pathfinder msg: 1613174250.72 epoch sec; seqNo:3982 2021-02-12T23:57:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.12 , 0.00 2021-02-12T23:57:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174250.72 epoch sec; seqNo:8383; 4058261.86 north; 591795.69 east 2021-02-12T23:57:31Z- AHRS_M2 msg: 1613174251.11 epoch sec; seqNo:8315 2021-02-12T23:57:31Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.19 psi 2021-02-12T23:57:31Z- handleDepth()-Depth_Keller msg: 1613174251111 epoch sec; seqNo:8384; depth 18.28 2021-02-12T23:57:31Z- handleDvl() - RDI_Pathfinder msg: 1613174251.13 epoch sec; seqNo:3983 2021-02-12T23:57:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.92 , 106.44 , 70.71 2021-02-12T23:57:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.12 , 0.05 2021-02-12T23:57:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174251.14 epoch sec; seqNo:8384; 4058262.23 north; 591795.69 east 2021-02-12T23:57:31Z- AHRS_M2 msg: 1613174251.51 epoch sec; seqNo:8316 2021-02-12T23:57:31Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.19 psi 2021-02-12T23:57:31Z- handleDepth()-Depth_Keller msg: 1613174251512 epoch sec; seqNo:8385; depth 18.15 2021-02-12T23:57:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174251.52 epoch sec; seqNo:8385; 4058262.61 north; 591795.68 east 2021-02-12T23:57:31Z- AHRS_M2 msg: 1613174251.91 epoch sec; seqNo:8317 2021-02-12T23:57:31Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.19 psi 2021-02-12T23:57:31Z- handleDepth()-Depth_Keller msg: 1613174251916 epoch sec; seqNo:8386; depth 18.06 2021-02-12T23:57:31Z- handleDvl() - RDI_Pathfinder msg: 1613174251.93 epoch sec; seqNo:3984 2021-02-12T23:57:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.92 , 105.22 , 71.53 2021-02-12T23:57:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.05 2021-02-12T23:57:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174251.94 epoch sec; seqNo:8386; 4058263.01 north; 591796.90 east 2021-02-12T23:57:32Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-12T23:57:32Z- waiting for 1613174250.08, time is 1613174251.94 2021-02-12T23:57:32Z- LcmTrn::updateTrn() equating pose 1613174251.94 and meas times 1613174251.94 2021-02-12T23:57:32Z- LcmTrn::updateTrn() >>>> pose time 1613174251.94, meas time 1613174251.94 <<<< 2021-02-12T23:57:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174251.94 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:57:32Z- TerrainNav::motionUpdate() - currEstimate = (4058263, 591796.90, 18.06) 2021-02-12T23:57:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:57:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:57:32Z- TNavBF::Projecting Measurements 2021-02-12T23:57:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:57:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:57:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:57:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:57:32Z- TNavFilter::4 good beams. 2021-02-12T23:57:32Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:57:32Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:57:32Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:57:32Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:57:32Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:57:32Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:57:32Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:57:32Z- TNavBF::mmse selected from filter -1 2021-02-12T23:57:32Z- TerrainMapOctree: (vehN, vehE ) = (4058263.01, 591796.90). 2021-02-12T23:57:32Z- TerrainMapOctree: Min Distance = 3865.21. 2021-02-12T23:57:32Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:57:32Z- There is no specific good beam that all particles have in common. 2021-02-12T23:57:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:57:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.76 K: 0.132 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-12T23:57:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:57:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.53 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-12T23:57:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:57:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.41 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 17.71 2021-02-12T23:57:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:57:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.117 ExpectedMeasDiff: nan currMeas.covariance: 8.19 2021-02-12T23:57:32Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:57:32Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:57:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:57:32Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:57:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:57:32Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:57:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:57:32Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:57:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:57:32Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:57:32Z- Filter 0 No beams used in subcloud update 2021-02-12T23:57:32Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:57:32Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:57:32Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 726.3 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:57:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174251.94 sec. 2021-02-12T23:57:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:57:32Z- TNavBF::mmse selected from filter -1 2021-02-12T23:57:32Z- variances([0], [2], and psi_berg) = 726.299802 833.013921 0.000000 2021-02-12T23:57:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:57:32Z- LcmTrn::publishEstimates() - MLE : 4058257.51 591748.40 18.06 2021-02-12T23:57:32Z- LcmTrn::publishEstimates() - MMSE : 4058263.01 591796.90 18.06 2021-02-12T23:57:32Z- LcmTrn::publishEstimates() - COVAR: 726.30 833.01 1.16 0.00 2021-02-12T23:57:32Z- AHRS_M2 msg: 1613174252.32 epoch sec; seqNo:8318 2021-02-12T23:57:32Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-12T23:57:32Z- handleDepth()-Depth_Keller msg: 1613174252321 epoch sec; seqNo:8387; depth 17.96 2021-02-12T23:57:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174252.33 epoch sec; seqNo:8387; 4058263.38 north; 591796.90 east 2021-02-12T23:57:32Z- AHRS_M2 msg: 1613174252.72 epoch sec; seqNo:8319 2021-02-12T23:57:32Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-12T23:57:32Z- handleDepth()-Depth_Keller msg: 1613174252734 epoch sec; seqNo:8388; depth 17.85 2021-02-12T23:57:32Z- handleDvl() - RDI_Pathfinder msg: 1613174252.75 epoch sec; seqNo:3985 2021-02-12T23:57:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.70 , 106.44 , 71.94 2021-02-12T23:57:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.02 2021-02-12T23:57:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174252.76 epoch sec; seqNo:8388; 4058263.38 north; 591796.90 east 2021-02-12T23:57:33Z- AHRS_M2 msg: 1613174253.13 epoch sec; seqNo:8320 2021-02-12T23:57:33Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-12T23:57:33Z- handleDepth()-Depth_Keller msg: 1613174253133 epoch sec; seqNo:8389; depth 17.73 2021-02-12T23:57:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:33Z- handleDvl() - RDI_Pathfinder msg: 1613174253.14 epoch sec; seqNo:3986 2021-02-12T23:57:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.02 2021-02-12T23:57:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174253.16 epoch sec; seqNo:8389; 4058263.76 north; 591796.89 east 2021-02-12T23:57:33Z- AHRS_M2 msg: 1613174253.53 epoch sec; seqNo:8321 2021-02-12T23:57:33Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-12T23:57:33Z- handleDepth()-Depth_Keller msg: 1613174253532 epoch sec; seqNo:8390; depth 17.62 2021-02-12T23:57:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174253.54 epoch sec; seqNo:8390; 4058264.14 north; 591796.89 east 2021-02-12T23:57:33Z- AHRS_M2 msg: 1613174253.93 epoch sec; seqNo:8322 2021-02-12T23:57:33Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-12T23:57:33Z- handleDepth()-Depth_Keller msg: 1613174253936 epoch sec; seqNo:8391; depth 17.50 2021-02-12T23:57:33Z- handleDvl() - RDI_Pathfinder msg: 1613174253.95 epoch sec; seqNo:3987 2021-02-12T23:57:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 97.65 , 104.95 , 73.16 2021-02-12T23:57:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , -0.01 2021-02-12T23:57:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174253.97 epoch sec; seqNo:8391; 4058264.52 north; 591796.88 east 2021-02-12T23:57:34Z- AHRS_M2 msg: 1613174254.34 epoch sec; seqNo:8323 2021-02-12T23:57:34Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-12T23:57:34Z- handleDepth()-Depth_Keller msg: 1613174254341 epoch sec; seqNo:8392; depth 17.41 2021-02-12T23:57:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:34Z- handleDvl() - RDI_Pathfinder msg: 1613174254.34 epoch sec; seqNo:3988 2021-02-12T23:57:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , -0.01 2021-02-12T23:57:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174254.36 epoch sec; seqNo:8392; 4058264.53 north; 591798.10 east 2021-02-12T23:57:34Z- AHRS_M2 msg: 1613174254.74 epoch sec; seqNo:8324 2021-02-12T23:57:34Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.13 psi 2021-02-12T23:57:34Z- handleDepth()-Depth_Keller msg: 1613174254748 epoch sec; seqNo:8393; depth 17.28 2021-02-12T23:57:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174254.77 epoch sec; seqNo:8393; 4058264.91 north; 591798.10 east 2021-02-12T23:57:35Z- AHRS_M2 msg: 1613174255.15 epoch sec; seqNo:8325 2021-02-12T23:57:35Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.13 psi 2021-02-12T23:57:35Z- handleDepth()-Depth_Keller msg: 1613174255152 epoch sec; seqNo:8394; depth 17.18 2021-02-12T23:57:35Z- handleDvl() - RDI_Pathfinder msg: 1613174255.16 epoch sec; seqNo:3989 2021-02-12T23:57:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.81 , 97.85 , 106.37 , 73.16 2021-02-12T23:57:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , -0.04 2021-02-12T23:57:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174255.17 epoch sec; seqNo:8394; 4058265.29 north; 591798.10 east 2021-02-12T23:57:35Z- AHRS_M2 msg: 1613174255.55 epoch sec; seqNo:8326 2021-02-12T23:57:35Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.12 psi 2021-02-12T23:57:35Z- handleDepth()-Depth_Keller msg: 1613174255557 epoch sec; seqNo:8395; depth 17.07 2021-02-12T23:57:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:35Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:35Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:35Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:35Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:35Z- handleDvl() - RDI_Pathfinder msg: 1613174255.56 epoch sec; seqNo:3990 2021-02-12T23:57:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , -0.04 2021-02-12T23:57:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174255.58 epoch sec; seqNo:8395; 4058265.29 north; 591798.10 east 2021-02-12T23:57:35Z- AHRS_M2 msg: 1613174255.95 epoch sec; seqNo:8327 2021-02-12T23:57:35Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.12 psi 2021-02-12T23:57:35Z- handleDepth()-Depth_Keller msg: 1613174255961 epoch sec; seqNo:8396; depth 16.95 2021-02-12T23:57:35Z- handleDvl() - RDI_Pathfinder msg: 1613174255.97 epoch sec; seqNo:3991 2021-02-12T23:57:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 94.13 , 105.09 , 78.31 2021-02-12T23:57:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , -0.04 2021-02-12T23:57:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174256.00 epoch sec; seqNo:8396; 4058265.67 north; 591798.09 east 2021-02-12T23:57:36Z- AHRS_M2 msg: 1613174256.36 epoch sec; seqNo:8328 2021-02-12T23:57:36Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.12 psi 2021-02-12T23:57:36Z- handleDepth()-Depth_Keller msg: 1613174256360 epoch sec; seqNo:8397; depth 16.84 2021-02-12T23:57:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174256.37 epoch sec; seqNo:8397; 4058266.05 north; 591798.09 east 2021-02-12T23:57:36Z- AHRS_M2 msg: 1613174256.76 epoch sec; seqNo:8329 2021-02-12T23:57:36Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.12 psi 2021-02-12T23:57:36Z- handleDepth()-Depth_Keller msg: 1613174256769 epoch sec; seqNo:8398; depth 16.74 2021-02-12T23:57:36Z- handleDvl() - RDI_Pathfinder msg: 1613174256.78 epoch sec; seqNo:3992 2021-02-12T23:57:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.78 , 106.30 , 73.16 2021-02-12T23:57:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.10 , -0.02 2021-02-12T23:57:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174256.79 epoch sec; seqNo:8398; 4058266.43 north; 591798.08 east 2021-02-12T23:57:37Z- AHRS_M2 msg: 1613174257.16 epoch sec; seqNo:8330 2021-02-12T23:57:37Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.12 psi 2021-02-12T23:57:37Z- handleDepth()-Depth_Keller msg: 1613174257168 epoch sec; seqNo:8399; depth 16.62 2021-02-12T23:57:37Z- LcmTrn::handleMessages() - handling 46 messages took 0.64 ms and 133 calls... 2021-02-12T23:57:37Z- waiting for 1613174254.94, time is 1613174257.16 2021-02-12T23:57:37Z- LcmTrn::updateTrn() >>>> pose time 1613174257.16, meas time 1613174256.78 <<<< 2021-02-12T23:57:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174256.78 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:57:37Z- TerrainNav::motionUpdate() - currEstimate = (4058266, 591798.08, 16.62) 2021-02-12T23:57:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:57:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:57:37Z- TNavBF::Projecting Measurements 2021-02-12T23:57:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:57:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:57:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:57:37Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:57:37Z- TNavFilter::3 good beams. 2021-02-12T23:57:37Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:57:37Z- TNavBF:: beamIndex[0] = 1 2021-02-12T23:57:37Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:57:37Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:57:37Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:57:37Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:57:37Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:57:37Z- TNavBF::mmse selected from filter -1 2021-02-12T23:57:37Z- TerrainMapOctree: (vehN, vehE ) = (4058266.18, 591798.00). 2021-02-12T23:57:37Z- TerrainMapOctree: Min Distance = 3862.06. 2021-02-12T23:57:37Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:57:37Z- There is no specific good beam that all particles have in common. 2021-02-12T23:57:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:57:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-12T23:57:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:57:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 18.08 2021-02-12T23:57:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:57:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-12T23:57:37Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:57:37Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:57:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:57:37Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:57:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:57:37Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:57:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:57:37Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:57:37Z- Filter 0 No beams used in subcloud update 2021-02-12T23:57:37Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:57:37Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:57:37Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 726.2 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:57:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174256.78 sec. 2021-02-12T23:57:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:57:37Z- TNavBF::mmse selected from filter -1 2021-02-12T23:57:37Z- variances([0], [2], and psi_berg) = 726.214705 833.013861 0.000000 2021-02-12T23:57:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:57:37Z- LcmTrn::publishEstimates() - MLE : 4058260.93 591749.58 16.62 2021-02-12T23:57:37Z- LcmTrn::publishEstimates() - MMSE : 4058266.43 591798.08 16.62 2021-02-12T23:57:37Z- LcmTrn::publishEstimates() - COVAR: 726.21 833.01 1.06 0.00 2021-02-12T23:57:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174257.19 epoch sec; seqNo:8399; 4058266.44 north; 591799.30 east 2021-02-12T23:57:37Z- AHRS_M2 msg: 1613174257.57 epoch sec; seqNo:8331 2021-02-12T23:57:37Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.13 psi 2021-02-12T23:57:37Z- handleDepth()-Depth_Keller msg: 1613174257572 epoch sec; seqNo:8400; depth 16.52 2021-02-12T23:57:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174257.58 epoch sec; seqNo:8400; 4058266.82 north; 591799.30 east 2021-02-12T23:57:37Z- AHRS_M2 msg: 1613174257.97 epoch sec; seqNo:8332 2021-02-12T23:57:37Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-12T23:57:37Z- handleDepth()-Depth_Keller msg: 1613174257976 epoch sec; seqNo:8401; depth 16.40 2021-02-12T23:57:37Z- handleDvl() - RDI_Pathfinder msg: 1613174257.99 epoch sec; seqNo:3993 2021-02-12T23:57:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 93.66 , 105.83 , 73.57 2021-02-12T23:57:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.14 , 0.07 2021-02-12T23:57:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174258.00 epoch sec; seqNo:8401; 4058267.20 north; 591799.30 east 2021-02-12T23:57:38Z- AHRS_M2 msg: 1613174258.38 epoch sec; seqNo:8333 2021-02-12T23:57:38Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-12T23:57:38Z- handleDepth()-Depth_Keller msg: 1613174258382 epoch sec; seqNo:8402; depth 16.28 2021-02-12T23:57:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174258.39 epoch sec; seqNo:8402; 4058267.58 north; 591799.29 east 2021-02-12T23:57:38Z- AHRS_M2 msg: 1613174258.78 epoch sec; seqNo:8334 2021-02-12T23:57:38Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-12T23:57:38Z- handleDepth()-Depth_Keller msg: 1613174258802 epoch sec; seqNo:8403; depth 16.18 2021-02-12T23:57:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174258.82 epoch sec; seqNo:8403; 4058267.58 north; 591799.29 east 2021-02-12T23:57:39Z- AHRS_M2 msg: 1613174259.18 epoch sec; seqNo:8335 2021-02-12T23:57:39Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-12T23:57:39Z- handleDepth()-Depth_Keller msg: 1613174259188 epoch sec; seqNo:8404; depth 16.05 2021-02-12T23:57:39Z- handleDvl() - RDI_Pathfinder msg: 1613174259.20 epoch sec; seqNo:3994 2021-02-12T23:57:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.03 , 95.42 , 108.81 , 78.38 2021-02-12T23:57:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.09 , 0.15 2021-02-12T23:57:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174259.23 epoch sec; seqNo:8404; 4058267.96 north; 591799.29 east 2021-02-12T23:57:39Z- AHRS_M2 msg: 1613174259.59 epoch sec; seqNo:8336 2021-02-12T23:57:39Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-12T23:57:39Z- handleDepth()-Depth_Keller msg: 1613174259592 epoch sec; seqNo:8405; depth 15.95 2021-02-12T23:57:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174259.61 epoch sec; seqNo:8405; 4058268.34 north; 591799.28 east 2021-02-12T23:57:39Z- AHRS_M2 msg: 1613174259.99 epoch sec; seqNo:8337 2021-02-12T23:57:39Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.14 psi 2021-02-12T23:57:39Z- handleDepth()-Depth_Keller msg: 1613174259997 epoch sec; seqNo:8406; depth 15.83 2021-02-12T23:57:40Z- handleDvl() - RDI_Pathfinder msg: 1613174260.01 epoch sec; seqNo:3995 2021-02-12T23:57:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.65 , 111.04 , 73.57 2021-02-12T23:57:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.04 , 0.13 2021-02-12T23:57:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174260.02 epoch sec; seqNo:8406; 4058268.73 north; 591800.50 east 2021-02-12T23:57:40Z- AHRS_M2 msg: 1613174260.40 epoch sec; seqNo:8338 2021-02-12T23:57:40Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-12T23:57:40Z- handleDepth()-Depth_Keller msg: 1613174260401 epoch sec; seqNo:8407; depth 15.71 2021-02-12T23:57:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:40Z- handleDvl() - RDI_Pathfinder msg: 1613174260.40 epoch sec; seqNo:3996 2021-02-12T23:57:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.04 , 0.13 2021-02-12T23:57:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174260.42 epoch sec; seqNo:8407; 4058269.11 north; 591800.50 east 2021-02-12T23:57:40Z- AHRS_M2 msg: 1613174260.81 epoch sec; seqNo:8339 2021-02-12T23:57:40Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.13 psi 2021-02-12T23:57:40Z- handleDepth()-Depth_Keller msg: 1613174260839 epoch sec; seqNo:8408; depth 15.60 2021-02-12T23:57:40Z- handleDvl() - RDI_Pathfinder msg: 1613174260.85 epoch sec; seqNo:3997 2021-02-12T23:57:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 96.16 , 109.68 , 78.58 2021-02-12T23:57:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.11 , 0.02 2021-02-12T23:57:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174260.87 epoch sec; seqNo:8408; 4058269.11 north; 591800.50 east 2021-02-12T23:57:41Z- AHRS_M2 msg: 1613174261.20 epoch sec; seqNo:8340 2021-02-12T23:57:41Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.13 psi 2021-02-12T23:57:41Z- handleDepth()-Depth_Keller msg: 1613174261208 epoch sec; seqNo:8409; depth 15.46 2021-02-12T23:57:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174261.22 epoch sec; seqNo:8409; 4058269.48 north; 591800.49 east 2021-02-12T23:57:41Z- AHRS_M2 msg: 1613174261.61 epoch sec; seqNo:8341 2021-02-12T23:57:41Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.13 psi 2021-02-12T23:57:41Z- handleDepth()-Depth_Keller msg: 1613174261613 epoch sec; seqNo:8410; depth 15.38 2021-02-12T23:57:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:41Z- handleDvl() - RDI_Pathfinder msg: 1613174261.62 epoch sec; seqNo:3998 2021-02-12T23:57:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.11 , 0.02 2021-02-12T23:57:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174261.62 epoch sec; seqNo:8410; 4058269.86 north; 591800.49 east 2021-02-12T23:57:42Z- AHRS_M2 msg: 1613174262.01 epoch sec; seqNo:8342 2021-02-12T23:57:42Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.13 psi 2021-02-12T23:57:42Z- handleDepth()-Depth_Keller msg: 1613174262016 epoch sec; seqNo:8411; depth 15.26 2021-02-12T23:57:42Z- handleDvl() - RDI_Pathfinder msg: 1613174262.03 epoch sec; seqNo:3999 2021-02-12T23:57:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 97.71 , 111.24 , 73.16 2021-02-12T23:57:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.06 , -0.09 2021-02-12T23:57:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174262.05 epoch sec; seqNo:8411; 4058269.88 north; 591801.71 east 2021-02-12T23:57:42Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-12T23:57:42Z- waiting for 1613174260.16, time is 1613174262.05 2021-02-12T23:57:42Z- LcmTrn::updateTrn() equating pose 1613174262.05 and meas times 1613174262.05 2021-02-12T23:57:42Z- LcmTrn::updateTrn() >>>> pose time 1613174262.05, meas time 1613174262.05 <<<< 2021-02-12T23:57:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174262.05 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:57:42Z- TerrainNav::motionUpdate() - currEstimate = (4058270, 591801.71, 15.26) 2021-02-12T23:57:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:57:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:57:42Z- TNavBF::Projecting Measurements 2021-02-12T23:57:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:57:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:57:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:57:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:57:42Z- TNavFilter::4 good beams. 2021-02-12T23:57:42Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:57:42Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:57:42Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:57:42Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:57:42Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:57:42Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:57:42Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:57:42Z- TNavBF::mmse selected from filter -1 2021-02-12T23:57:42Z- TerrainMapOctree: (vehN, vehE ) = (4058269.88, 591801.71). 2021-02-12T23:57:42Z- TerrainMapOctree: Min Distance = 3856.85. 2021-02-12T23:57:42Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:57:42Z- There is no specific good beam that all particles have in common. 2021-02-12T23:57:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:57:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 11.34 2021-02-12T23:57:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:57:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-12T23:57:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:57:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 19.80 2021-02-12T23:57:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:57:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-12T23:57:42Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:57:42Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:57:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:57:42Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:57:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:57:42Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:57:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:57:42Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:57:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:57:42Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:57:42Z- Filter 0 No beams used in subcloud update 2021-02-12T23:57:42Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:57:42Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:57:42Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 726.1 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:57:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174262.05 sec. 2021-02-12T23:57:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:57:42Z- TNavBF::mmse selected from filter -1 2021-02-12T23:57:42Z- variances([0], [2], and psi_berg) = 726.133653 833.013805 0.000000 2021-02-12T23:57:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:57:42Z- LcmTrn::publishEstimates() - MLE : 4058264.38 591753.21 15.26 2021-02-12T23:57:42Z- LcmTrn::publishEstimates() - MMSE : 4058269.88 591801.71 15.26 2021-02-12T23:57:42Z- LcmTrn::publishEstimates() - COVAR: 726.13 833.01 0.96 0.00 2021-02-12T23:57:42Z- AHRS_M2 msg: 1613174262.42 epoch sec; seqNo:8343 2021-02-12T23:57:42Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.14 psi 2021-02-12T23:57:42Z- handleDepth()-Depth_Keller msg: 1613174262420 epoch sec; seqNo:8412; depth 15.13 2021-02-12T23:57:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174262.43 epoch sec; seqNo:8412; 4058270.26 north; 591801.71 east 2021-02-12T23:57:42Z- AHRS_M2 msg: 1613174262.82 epoch sec; seqNo:8344 2021-02-12T23:57:42Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.15 psi 2021-02-12T23:57:42Z- handleDepth()-Depth_Keller msg: 1613174262825 epoch sec; seqNo:8413; depth 15.02 2021-02-12T23:57:42Z- handleDvl() - RDI_Pathfinder msg: 1613174262.84 epoch sec; seqNo:4000 2021-02-12T23:57:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.64 , 109.81 , 75.20 2021-02-12T23:57:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.04 , -0.09 2021-02-12T23:57:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174262.85 epoch sec; seqNo:8413; 4058270.63 north; 591801.70 east 2021-02-12T23:57:43Z- AHRS_M2 msg: 1613174263.23 epoch sec; seqNo:8345 2021-02-12T23:57:43Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.16 psi 2021-02-12T23:57:43Z- handleDepth()-Depth_Keller msg: 1613174263230 epoch sec; seqNo:8414; depth 14.90 2021-02-12T23:57:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174263.24 epoch sec; seqNo:8414; 4058270.63 north; 591801.70 east 2021-02-12T23:57:43Z- AHRS_M2 msg: 1613174263.63 epoch sec; seqNo:8346 2021-02-12T23:57:43Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.17 psi 2021-02-12T23:57:43Z- handleDepth()-Depth_Keller msg: 1613174263632 epoch sec; seqNo:8415; depth 14.77 2021-02-12T23:57:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174263.65 epoch sec; seqNo:8415; 4058271.01 north; 591801.70 east 2021-02-12T23:57:44Z- AHRS_M2 msg: 1613174264.03 epoch sec; seqNo:8347 2021-02-12T23:57:44Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.17 psi 2021-02-12T23:57:44Z- handleDepth()-Depth_Keller msg: 1613174264036 epoch sec; seqNo:8416; depth 14.67 2021-02-12T23:57:44Z- handleDvl() - RDI_Pathfinder msg: 1613174264.05 epoch sec; seqNo:4001 2021-02-12T23:57:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.63 , 98.86 , 108.32 , 78.58 2021-02-12T23:57:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.07 , -0.02 2021-02-12T23:57:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174264.07 epoch sec; seqNo:8416; 4058271.39 north; 591801.69 east 2021-02-12T23:57:44Z- AHRS_M2 msg: 1613174264.43 epoch sec; seqNo:8348 2021-02-12T23:57:44Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:57:44Z- handleDepth()-Depth_Keller msg: 1613174264440 epoch sec; seqNo:8417; depth 14.53 2021-02-12T23:57:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174264.45 epoch sec; seqNo:8417; 4058271.39 north; 591801.69 east 2021-02-12T23:57:44Z- AHRS_M2 msg: 1613174264.84 epoch sec; seqNo:8349 2021-02-12T23:57:44Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:57:44Z- handleDepth()-Depth_Keller msg: 1613174264850 epoch sec; seqNo:8418; depth 14.43 2021-02-12T23:57:44Z- handleDvl() - RDI_Pathfinder msg: 1613174264.86 epoch sec; seqNo:4002 2021-02-12T23:57:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 99.07 , 111.24 , 73.57 2021-02-12T23:57:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.06 2021-02-12T23:57:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174264.87 epoch sec; seqNo:8418; 4058271.78 north; 591802.91 east 2021-02-12T23:57:45Z- AHRS_M2 msg: 1613174265.24 epoch sec; seqNo:8350 2021-02-12T23:57:45Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:57:45Z- handleDepth()-Depth_Keller msg: 1613174265263 epoch sec; seqNo:8419; depth 14.32 2021-02-12T23:57:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:45Z- handleDvl() - RDI_Pathfinder msg: 1613174265.27 epoch sec; seqNo:4003 2021-02-12T23:57:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.06 2021-02-12T23:57:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174265.30 epoch sec; seqNo:8419; 4058272.16 north; 591802.91 east 2021-02-12T23:57:45Z- AHRS_M2 msg: 1613174265.65 epoch sec; seqNo:8351 2021-02-12T23:57:45Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-12T23:57:45Z- handleDepth()-Depth_Keller msg: 1613174265654 epoch sec; seqNo:8420; depth 14.20 2021-02-12T23:57:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174265.66 epoch sec; seqNo:8420; 4058272.16 north; 591802.91 east 2021-02-12T23:57:46Z- AHRS_M2 msg: 1613174266.05 epoch sec; seqNo:8352 2021-02-12T23:57:46Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-12T23:57:46Z- handleDepth()-Depth_Keller msg: 1613174266056 epoch sec; seqNo:8421; depth 14.11 2021-02-12T23:57:46Z- handleDvl() - RDI_Pathfinder msg: 1613174266.07 epoch sec; seqNo:4004 2021-02-12T23:57:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 100.08 , 112.25 , 76.02 2021-02-12T23:57:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.05 , 0.05 2021-02-12T23:57:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174266.09 epoch sec; seqNo:8421; 4058272.54 north; 591802.90 east 2021-02-12T23:57:46Z- AHRS_M2 msg: 1613174266.46 epoch sec; seqNo:8353 2021-02-12T23:57:46Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-12T23:57:46Z- handleDepth()-Depth_Keller msg: 1613174266461 epoch sec; seqNo:8422; depth 13.97 2021-02-12T23:57:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:46Z- handleDvl() - RDI_Pathfinder msg: 1613174266.46 epoch sec; seqNo:4005 2021-02-12T23:57:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.05 , 0.05 2021-02-12T23:57:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174266.47 epoch sec; seqNo:8422; 4058272.92 north; 591802.90 east 2021-02-12T23:57:46Z- AHRS_M2 msg: 1613174266.86 epoch sec; seqNo:8354 2021-02-12T23:57:46Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.16 psi 2021-02-12T23:57:46Z- handleDepth()-Depth_Keller msg: 1613174266867 epoch sec; seqNo:8423; depth 13.86 2021-02-12T23:57:46Z- handleDvl() - RDI_Pathfinder msg: 1613174266.89 epoch sec; seqNo:4006 2021-02-12T23:57:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 99.00 , 111.17 , 128.75 2021-02-12T23:57:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , 0.05 , -0.05 2021-02-12T23:57:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174266.90 epoch sec; seqNo:8423; 4058272.92 north; 591802.90 east 2021-02-12T23:57:47Z- AHRS_M2 msg: 1613174267.26 epoch sec; seqNo:8355 2021-02-12T23:57:47Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.16 psi 2021-02-12T23:57:47Z- handleDepth()-Depth_Keller msg: 1613174267272 epoch sec; seqNo:8424; depth 13.74 2021-02-12T23:57:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174267.29 epoch sec; seqNo:8424; 4058273.31 north; 591804.11 east 2021-02-12T23:57:47Z- LcmTrn::handleMessages() - handling 46 messages took 0.78 ms and 131 calls... 2021-02-12T23:57:47Z- waiting for 1613174265.05, time is 1613174267.29 2021-02-12T23:57:47Z- LcmTrn::updateTrn() >>>> pose time 1613174267.29, meas time 1613174266.89 <<<< 2021-02-12T23:57:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174266.89 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:57:47Z- TerrainNav::motionUpdate() - currEstimate = (4058273, 591804.11, 13.74) 2021-02-12T23:57:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:57:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:57:47Z- TNavBF::Projecting Measurements 2021-02-12T23:57:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:57:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:57:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:57:47Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:57:47Z- TNavFilter::3 good beams. 2021-02-12T23:57:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:57:47Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:57:47Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:57:47Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:57:47Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:57:47Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:57:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:57:47Z- TNavBF::mmse selected from filter -1 2021-02-12T23:57:47Z- TerrainMapOctree: (vehN, vehE ) = (4058273.05, 591803.93). 2021-02-12T23:57:47Z- TerrainMapOctree: Min Distance = 3852.98. 2021-02-12T23:57:47Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:57:47Z- There is no specific good beam that all particles have in common. 2021-02-12T23:57:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:57:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 11.32 2021-02-12T23:57:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:57:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 15.68 2021-02-12T23:57:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:57:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 19.77 2021-02-12T23:57:47Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:57:47Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:57:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:57:47Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:57:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:57:47Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:57:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:57:47Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:57:47Z- Filter 0 No beams used in subcloud update 2021-02-12T23:57:47Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:57:47Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:57:47Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 726.1 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:57:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174266.89 sec. 2021-02-12T23:57:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:57:47Z- TNavBF::mmse selected from filter -1 2021-02-12T23:57:47Z- variances([0], [2], and psi_berg) = 726.042059 833.013741 0.000000 2021-02-12T23:57:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:57:47Z- LcmTrn::publishEstimates() - MLE : 4058267.81 591755.61 13.74 2021-02-12T23:57:47Z- LcmTrn::publishEstimates() - MMSE : 4058273.31 591804.11 13.74 2021-02-12T23:57:47Z- LcmTrn::publishEstimates() - COVAR: 726.04 833.01 0.97 0.00 2021-02-12T23:57:47Z- AHRS_M2 msg: 1613174267.67 epoch sec; seqNo:8356 2021-02-12T23:57:47Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.16 psi 2021-02-12T23:57:47Z- handleDepth()-Depth_Keller msg: 1613174267677 epoch sec; seqNo:8425; depth 13.65 2021-02-12T23:57:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:47Z- handleDvl() - RDI_Pathfinder msg: 1613174267.68 epoch sec; seqNo:4007 2021-02-12T23:57:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , 0.05 , -0.05 2021-02-12T23:57:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174267.69 epoch sec; seqNo:8425; 4058273.31 north; 591804.11 east 2021-02-12T23:57:48Z- AHRS_M2 msg: 1613174268.08 epoch sec; seqNo:8357 2021-02-12T23:57:48Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.17 psi 2021-02-12T23:57:48Z- handleDepth()-Depth_Keller msg: 1613174268083 epoch sec; seqNo:8426; depth 13.53 2021-02-12T23:57:48Z- handleDvl() - RDI_Pathfinder msg: 1613174268.10 epoch sec; seqNo:4008 2021-02-12T23:57:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.26 , 100.62 , 111.04 , 78.31 2021-02-12T23:57:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , -0.05 2021-02-12T23:57:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174268.11 epoch sec; seqNo:8426; 4058273.69 north; 591804.11 east 2021-02-12T23:57:48Z- AHRS_M2 msg: 1613174268.48 epoch sec; seqNo:8358 2021-02-12T23:57:48Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.17 psi 2021-02-12T23:57:48Z- handleDepth()-Depth_Keller msg: 1613174268485 epoch sec; seqNo:8427; depth 13.39 2021-02-12T23:57:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174268.50 epoch sec; seqNo:8427; 4058274.07 north; 591804.11 east 2021-02-12T23:57:48Z- AHRS_M2 msg: 1613174268.88 epoch sec; seqNo:8359 2021-02-12T23:57:48Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.18 psi 2021-02-12T23:57:48Z- handleDepth()-Depth_Keller msg: 1613174268889 epoch sec; seqNo:8428; depth 13.30 2021-02-12T23:57:48Z- handleDvl() - RDI_Pathfinder msg: 1613174268.90 epoch sec; seqNo:4009 2021-02-12T23:57:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.23 , 112.46 , 77.66 2021-02-12T23:57:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.14 , 0.01 2021-02-12T23:57:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174268.91 epoch sec; seqNo:8428; 4058274.07 north; 591804.11 east 2021-02-12T23:57:49Z- AHRS_M2 msg: 1613174269.29 epoch sec; seqNo:8360 2021-02-12T23:57:49Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.19 psi 2021-02-12T23:57:49Z- handleDepth()-Depth_Keller msg: 1613174269291 epoch sec; seqNo:8429; depth 13.16 2021-02-12T23:57:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174269.31 epoch sec; seqNo:8429; 4058274.45 north; 591804.10 east 2021-02-12T23:57:49Z- AHRS_M2 msg: 1613174269.69 epoch sec; seqNo:8361 2021-02-12T23:57:49Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.19 psi 2021-02-12T23:57:49Z- handleDepth()-Depth_Keller msg: 1613174269692 epoch sec; seqNo:8430; depth 13.06 2021-02-12T23:57:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174269.70 epoch sec; seqNo:8430; 4058274.83 north; 591804.10 east 2021-02-12T23:57:50Z- AHRS_M2 msg: 1613174270.09 epoch sec; seqNo:8362 2021-02-12T23:57:50Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.19 psi 2021-02-12T23:57:50Z- handleDepth()-Depth_Keller msg: 1613174270096 epoch sec; seqNo:8431; depth 12.94 2021-02-12T23:57:50Z- handleDvl() - RDI_Pathfinder msg: 1613174270.11 epoch sec; seqNo:4010 2021-02-12T23:57:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 104.21 , 112.32 , 78.51 2021-02-12T23:57:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.12 , 0.12 2021-02-12T23:57:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174270.12 epoch sec; seqNo:8431; 4058275.21 north; 591804.09 east 2021-02-12T23:57:50Z- AHRS_M2 msg: 1613174270.50 epoch sec; seqNo:8363 2021-02-12T23:57:50Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-12T23:57:50Z- handleDepth()-Depth_Keller msg: 1613174270502 epoch sec; seqNo:8432; depth 12.83 2021-02-12T23:57:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174270.51 epoch sec; seqNo:8432; 4058275.60 north; 591805.31 east 2021-02-12T23:57:50Z- AHRS_M2 msg: 1613174270.90 epoch sec; seqNo:8364 2021-02-12T23:57:50Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.18 psi 2021-02-12T23:57:50Z- handleDepth()-Depth_Keller msg: 1613174270910 epoch sec; seqNo:8433; depth 12.72 2021-02-12T23:57:50Z- handleDvl() - RDI_Pathfinder msg: 1613174270.93 epoch sec; seqNo:4011 2021-02-12T23:57:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 102.92 , 115.09 , 78.58 2021-02-12T23:57:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.01 , 0.14 2021-02-12T23:57:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174270.94 epoch sec; seqNo:8433; 4058275.98 north; 591805.31 east 2021-02-12T23:57:51Z- AHRS_M2 msg: 1613174271.31 epoch sec; seqNo:8365 2021-02-12T23:57:51Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.18 psi 2021-02-12T23:57:51Z- handleDepth()-Depth_Keller msg: 1613174271312 epoch sec; seqNo:8434; depth 12.59 2021-02-12T23:57:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:51Z- handleDvl() - RDI_Pathfinder msg: 1613174271.31 epoch sec; seqNo:4012 2021-02-12T23:57:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.01 , 0.14 2021-02-12T23:57:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174271.33 epoch sec; seqNo:8434; 4058275.98 north; 591805.31 east 2021-02-12T23:57:51Z- AHRS_M2 msg: 1613174271.71 epoch sec; seqNo:8366 2021-02-12T23:57:51Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.18 psi 2021-02-12T23:57:51Z- handleDepth()-Depth_Keller msg: 1613174271717 epoch sec; seqNo:8435; depth 12.47 2021-02-12T23:57:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174271.73 epoch sec; seqNo:8435; 4058276.36 north; 591805.30 east 2021-02-12T23:57:52Z- AHRS_M2 msg: 1613174272.11 epoch sec; seqNo:8367 2021-02-12T23:57:52Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.18 psi 2021-02-12T23:57:52Z- handleDepth()-Depth_Keller msg: 1613174272119 epoch sec; seqNo:8436; depth 12.37 2021-02-12T23:57:52Z- handleDvl() - RDI_Pathfinder msg: 1613174272.13 epoch sec; seqNo:4013 2021-02-12T23:57:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.27 , 102.85 , 116.38 , 0.00 2021-02-12T23:57:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , 0.05 2021-02-12T23:57:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174272.15 epoch sec; seqNo:8436; 4058276.74 north; 591805.30 east 2021-02-12T23:57:52Z- LcmTrn::handleMessages() - handling 43 messages took 0.62 ms and 131 calls... 2021-02-12T23:57:52Z- waiting for 1613174270.29, time is 1613174272.15 2021-02-12T23:57:52Z- LcmTrn::updateTrn() equating pose 1613174272.15 and meas times 1613174272.15 2021-02-12T23:57:52Z- LcmTrn::updateTrn() >>>> pose time 1613174272.15, meas time 1613174272.15 <<<< 2021-02-12T23:57:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174272.15 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:57:52Z- TerrainNav::motionUpdate() - currEstimate = (4058277, 591805.30, 12.37) 2021-02-12T23:57:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:57:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:57:52Z- TNavBF::Projecting Measurements 2021-02-12T23:57:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:57:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:57:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:57:52Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:57:52Z- TNavFilter::3 good beams. 2021-02-12T23:57:52Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:57:52Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:57:52Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:57:52Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:57:52Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:57:52Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:57:52Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:57:52Z- TNavBF::mmse selected from filter -1 2021-02-12T23:57:52Z- TerrainMapOctree: (vehN, vehE ) = (4058276.74, 591805.30). 2021-02-12T23:57:52Z- TerrainMapOctree: Min Distance = 3849.27. 2021-02-12T23:57:52Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:57:52Z- There is no specific good beam that all particles have in common. 2021-02-12T23:57:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:57:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 11.63 2021-02-12T23:57:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:57:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 16.93 2021-02-12T23:57:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:57:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 21.67 2021-02-12T23:57:52Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:57:52Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:57:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:57:52Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:57:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:57:52Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:57:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:57:52Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:57:52Z- Filter 0 No beams used in subcloud update 2021-02-12T23:57:52Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:57:52Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:57:52Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 726.0 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:57:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174272.15 sec. 2021-02-12T23:57:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:57:52Z- TNavBF::mmse selected from filter -1 2021-02-12T23:57:52Z- variances([0], [2], and psi_berg) = 725.973244 833.013693 0.000000 2021-02-12T23:57:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:57:52Z- LcmTrn::publishEstimates() - MLE : 4058271.24 591756.80 12.37 2021-02-12T23:57:52Z- LcmTrn::publishEstimates() - MMSE : 4058276.74 591805.30 12.37 2021-02-12T23:57:52Z- LcmTrn::publishEstimates() - COVAR: 725.97 833.01 0.97 0.00 2021-02-12T23:57:52Z- AHRS_M2 msg: 1613174272.52 epoch sec; seqNo:8368 2021-02-12T23:57:52Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-12T23:57:52Z- handleDepth()-Depth_Keller msg: 1613174272530 epoch sec; seqNo:8437; depth 12.25 2021-02-12T23:57:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:52Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:52Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:52Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:52Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:52Z- handleDvl() - RDI_Pathfinder msg: 1613174272.53 epoch sec; seqNo:4014 2021-02-12T23:57:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , 0.05 2021-02-12T23:57:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174272.55 epoch sec; seqNo:8437; 4058277.13 north; 591806.52 east 2021-02-12T23:57:52Z- AHRS_M2 msg: 1613174272.92 epoch sec; seqNo:8369 2021-02-12T23:57:52Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-12T23:57:52Z- handleDepth()-Depth_Keller msg: 1613174272924 epoch sec; seqNo:8438; depth 12.14 2021-02-12T23:57:52Z- handleDvl() - RDI_Pathfinder msg: 1613174272.93 epoch sec; seqNo:4015 2021-02-12T23:57:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:52Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:57:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174272.96 epoch sec; seqNo:8438; 4058277.13 north; 591806.52 east 2021-02-12T23:57:53Z- AHRS_M2 msg: 1613174273.32 epoch sec; seqNo:8370 2021-02-12T23:57:53Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:57:53Z- handleDepth()-Depth_Keller msg: 1613174273328 epoch sec; seqNo:8439; depth 12.01 2021-02-12T23:57:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174273.34 epoch sec; seqNo:8439; 4058277.51 north; 591806.51 east 2021-02-12T23:57:53Z- AHRS_M2 msg: 1613174273.73 epoch sec; seqNo:8371 2021-02-12T23:57:53Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-12T23:57:53Z- handleDepth()-Depth_Keller msg: 1613174273734 epoch sec; seqNo:8440; depth 11.92 2021-02-12T23:57:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:53Z- handleDvl() - RDI_Pathfinder msg: 1613174273.74 epoch sec; seqNo:4016 2021-02-12T23:57:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:53Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:57:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174273.74 epoch sec; seqNo:8440; 4058277.88 north; 591806.51 east 2021-02-12T23:57:54Z- AHRS_M2 msg: 1613174274.13 epoch sec; seqNo:8372 2021-02-12T23:57:54Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-12T23:57:54Z- handleDepth()-Depth_Keller msg: 1613174274141 epoch sec; seqNo:8441; depth 11.80 2021-02-12T23:57:54Z- handleDvl() - RDI_Pathfinder msg: 1613174274.15 epoch sec; seqNo:4017 2021-02-12T23:57:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.44 , 114.34 , 78.64 2021-02-12T23:57:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.03 , -0.06 2021-02-12T23:57:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174274.18 epoch sec; seqNo:8441; 4058277.88 north; 591806.51 east 2021-02-12T23:57:54Z- AHRS_M2 msg: 1613174274.54 epoch sec; seqNo:8373 2021-02-12T23:57:54Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-12T23:57:54Z- handleDepth()-Depth_Keller msg: 1613174274540 epoch sec; seqNo:8442; depth 11.69 2021-02-12T23:57:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174274.55 epoch sec; seqNo:8442; 4058278.26 north; 591806.50 east 2021-02-12T23:57:54Z- AHRS_M2 msg: 1613174274.94 epoch sec; seqNo:8374 2021-02-12T23:57:54Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-12T23:57:54Z- handleDepth()-Depth_Keller msg: 1613174274945 epoch sec; seqNo:8443; depth 11.58 2021-02-12T23:57:54Z- handleDvl() - RDI_Pathfinder msg: 1613174274.96 epoch sec; seqNo:4018 2021-02-12T23:57:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.36 , 102.58 , 114.75 , 0.00 2021-02-12T23:57:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.00 , -0.04 2021-02-12T23:57:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174274.97 epoch sec; seqNo:8443; 4058278.66 north; 591807.72 east 2021-02-12T23:57:55Z- AHRS_M2 msg: 1613174275.34 epoch sec; seqNo:8375 2021-02-12T23:57:55Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-12T23:57:55Z- handleDepth()-Depth_Keller msg: 1613174275348 epoch sec; seqNo:8444; depth 11.46 2021-02-12T23:57:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174275.37 epoch sec; seqNo:8444; 4058278.66 north; 591807.72 east 2021-02-12T23:57:55Z- AHRS_M2 msg: 1613174275.75 epoch sec; seqNo:8376 2021-02-12T23:57:55Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-12T23:57:55Z- handleDepth()-Depth_Keller msg: 1613174275752 epoch sec; seqNo:8445; depth 11.36 2021-02-12T23:57:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174275.76 epoch sec; seqNo:8445; 4058279.03 north; 591807.72 east 2021-02-12T23:57:56Z- AHRS_M2 msg: 1613174276.15 epoch sec; seqNo:8377 2021-02-12T23:57:56Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.15 psi 2021-02-12T23:57:56Z- handleDepth()-Depth_Keller msg: 1613174276158 epoch sec; seqNo:8446; depth 11.26 2021-02-12T23:57:56Z- handleDvl() - RDI_Pathfinder msg: 1613174276.17 epoch sec; seqNo:4019 2021-02-12T23:57:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.08 , 102.45 , 112.86 , 78.65 2021-02-12T23:57:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.00 , -0.01 2021-02-12T23:57:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174276.18 epoch sec; seqNo:8446; 4058279.03 north; 591807.72 east 2021-02-12T23:57:56Z- AHRS_M2 msg: 1613174276.56 epoch sec; seqNo:8378 2021-02-12T23:57:56Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.15 psi 2021-02-12T23:57:56Z- handleDepth()-Depth_Keller msg: 1613174276566 epoch sec; seqNo:8447; depth 11.14 2021-02-12T23:57:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174276.58 epoch sec; seqNo:8447; 4058279.41 north; 591807.71 east 2021-02-12T23:57:56Z- AHRS_M2 msg: 1613174276.96 epoch sec; seqNo:8379 2021-02-12T23:57:56Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.14 psi 2021-02-12T23:57:56Z- handleDepth()-Depth_Keller msg: 1613174276966 epoch sec; seqNo:8448; depth 11.03 2021-02-12T23:57:56Z- handleDvl() - RDI_Pathfinder msg: 1613174276.99 epoch sec; seqNo:4020 2021-02-12T23:57:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.62 , 101.50 , 110.96 , 76.83 2021-02-12T23:57:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.02 , 0.00 2021-02-12T23:57:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174277.00 epoch sec; seqNo:8448; 4058279.79 north; 591807.71 east 2021-02-12T23:57:57Z- AHRS_M2 msg: 1613174277.36 epoch sec; seqNo:8380 2021-02-12T23:57:57Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.14 psi 2021-02-12T23:57:57Z- handleDepth()-Depth_Keller msg: 1613174277368 epoch sec; seqNo:8449; depth 10.94 2021-02-12T23:57:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:57Z- handleDvl() - RDI_Pathfinder msg: 1613174277.37 epoch sec; seqNo:4021 2021-02-12T23:57:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.02 , 0.00 2021-02-12T23:57:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174277.38 epoch sec; seqNo:8449; 4058279.79 north; 591807.71 east 2021-02-12T23:57:57Z- LcmTrn::handleMessages() - handling 47 messages took 0.66 ms and 134 calls... 2021-02-12T23:57:57Z- waiting for 1613174275.15, time is 1613174277.38 2021-02-12T23:57:57Z- LcmTrn::updateTrn() equating pose 1613174277.38 and meas times 1613174277.38 2021-02-12T23:57:57Z- LcmTrn::updateTrn() >>>> pose time 1613174277.38, meas time 1613174277.38 <<<< 2021-02-12T23:57:57Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174277.38, ping # 0. 2021-02-12T23:57:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174277.38 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:57:57Z- TerrainNav::motionUpdate() - currEstimate = (4058280, 591807.71, 10.94) 2021-02-12T23:57:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:57:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:57:57Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:57:57Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:57:57Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174277.38 sec.; No relevant navigation data available 2021-02-12T23:57:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:57:57Z- TNavBF::mmse selected from filter -1 2021-02-12T23:57:57Z- variances([0], [2], and psi_berg) = 725.901965 833.013643 0.000000 2021-02-12T23:57:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:57:57Z- LcmTrn::publishEstimates() - MLE : 4058274.29 591759.21 10.94 2021-02-12T23:57:57Z- LcmTrn::publishEstimates() - MMSE : 4058279.79 591807.71 10.94 2021-02-12T23:57:57Z- LcmTrn::publishEstimates() - COVAR: 725.90 833.01 1.17 0.00 2021-02-12T23:57:57Z- AHRS_M2 msg: 1613174277.77 epoch sec; seqNo:8381 2021-02-12T23:57:57Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.15 psi 2021-02-12T23:57:57Z- handleDepth()-Depth_Keller msg: 1613174277772 epoch sec; seqNo:8450; depth 10.86 2021-02-12T23:57:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174277.78 epoch sec; seqNo:8450; 4058280.18 north; 591808.92 east 2021-02-12T23:57:58Z- AHRS_M2 msg: 1613174278.17 epoch sec; seqNo:8382 2021-02-12T23:57:58Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.15 psi 2021-02-12T23:57:58Z- handleDepth()-Depth_Keller msg: 1613174278177 epoch sec; seqNo:8451; depth 10.76 2021-02-12T23:57:58Z- handleDvl() - RDI_Pathfinder msg: 1613174278.19 epoch sec; seqNo:4022 2021-02-12T23:57:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.76 , 96.22 , 109.74 , 80.00 2021-02-12T23:57:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.03 2021-02-12T23:57:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174278.20 epoch sec; seqNo:8451; 4058280.56 north; 591808.92 east 2021-02-12T23:57:58Z- AHRS_M2 msg: 1613174278.58 epoch sec; seqNo:8383 2021-02-12T23:57:58Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-12T23:57:58Z- handleDepth()-Depth_Keller msg: 1613174278590 epoch sec; seqNo:8452; depth 10.65 2021-02-12T23:57:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:57:58Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:57:58Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:57:58Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:57:58Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:57:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:57:58Z- handleDvl() - RDI_Pathfinder msg: 1613174278.59 epoch sec; seqNo:4023 2021-02-12T23:57:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:57:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.03 2021-02-12T23:57:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174278.60 epoch sec; seqNo:8452; 4058280.56 north; 591808.92 east 2021-02-12T23:57:58Z- AHRS_M2 msg: 1613174278.98 epoch sec; seqNo:8384 2021-02-12T23:57:58Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-12T23:57:58Z- handleDepth()-Depth_Keller msg: 1613174278984 epoch sec; seqNo:8453; depth 10.55 2021-02-12T23:57:59Z- handleDvl() - RDI_Pathfinder msg: 1613174279.00 epoch sec; seqNo:4024 2021-02-12T23:57:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.83 , 104.41 , 112.52 , 78.07 2021-02-12T23:57:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , 0.04 2021-02-12T23:57:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174279.02 epoch sec; seqNo:8453; 4058280.94 north; 591808.92 east 2021-02-12T23:57:59Z- AHRS_M2 msg: 1613174279.38 epoch sec; seqNo:8385 2021-02-12T23:57:59Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:57:59Z- handleDepth()-Depth_Keller msg: 1613174279388 epoch sec; seqNo:8454; depth 10.45 2021-02-12T23:57:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174279.40 epoch sec; seqNo:8454; 4058281.32 north; 591808.91 east 2021-02-12T23:57:59Z- AHRS_M2 msg: 1613174279.79 epoch sec; seqNo:8386 2021-02-12T23:57:59Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-12T23:57:59Z- handleDepth()-Depth_Keller msg: 1613174279794 epoch sec; seqNo:8455; depth 10.37 2021-02-12T23:57:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174279.80 epoch sec; seqNo:8455; 4058281.70 north; 591808.91 east 2021-02-12T23:58:00Z- AHRS_M2 msg: 1613174280.19 epoch sec; seqNo:8387 2021-02-12T23:58:00Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-12T23:58:00Z- handleDepth()-Depth_Keller msg: 1613174280201 epoch sec; seqNo:8456; depth 10.27 2021-02-12T23:58:00Z- handleDvl() - RDI_Pathfinder msg: 1613174280.25 epoch sec; seqNo:4025 2021-02-12T23:58:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 102.85 , 117.73 , 79.86 2021-02-12T23:58:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.04 , -0.01 2021-02-12T23:58:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174280.27 epoch sec; seqNo:8456; 4058281.71 north; 591810.13 east 2021-02-12T23:58:00Z- AHRS_M2 msg: 1613174280.60 epoch sec; seqNo:8388 2021-02-12T23:58:00Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-12T23:58:00Z- handleDepth()-Depth_Keller msg: 1613174280605 epoch sec; seqNo:8457; depth 10.18 2021-02-12T23:58:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174280.62 epoch sec; seqNo:8457; 4058282.09 north; 591810.13 east 2021-02-12T23:58:01Z- AHRS_M2 msg: 1613174281.00 epoch sec; seqNo:8389 2021-02-12T23:58:01Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-12T23:58:01Z- handleDepth()-Depth_Keller msg: 1613174281006 epoch sec; seqNo:8458; depth 10.11 2021-02-12T23:58:01Z- handleDvl() - RDI_Pathfinder msg: 1613174281.02 epoch sec; seqNo:4026 2021-02-12T23:58:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.84 , 104.07 , 114.89 , 78.48 2021-02-12T23:58:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.01 , -0.10 2021-02-12T23:58:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174281.03 epoch sec; seqNo:8458; 4058282.47 north; 591810.12 east 2021-02-12T23:58:01Z- AHRS_M2 msg: 1613174281.40 epoch sec; seqNo:8390 2021-02-12T23:58:01Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.18 psi 2021-02-12T23:58:01Z- handleDepth()-Depth_Keller msg: 1613174281418 epoch sec; seqNo:8459; depth 9.99 2021-02-12T23:58:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174281.43 epoch sec; seqNo:8459; 4058282.47 north; 591810.12 east 2021-02-12T23:58:01Z- AHRS_M2 msg: 1613174281.81 epoch sec; seqNo:8391 2021-02-12T23:58:01Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.18 psi 2021-02-12T23:58:01Z- handleDepth()-Depth_Keller msg: 1613174281817 epoch sec; seqNo:8460; depth 9.89 2021-02-12T23:58:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174281.83 epoch sec; seqNo:8460; 4058282.85 north; 591810.12 east 2021-02-12T23:58:02Z- AHRS_M2 msg: 1613174282.21 epoch sec; seqNo:8392 2021-02-12T23:58:02Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.18 psi 2021-02-12T23:58:02Z- handleDepth()-Depth_Keller msg: 1613174282218 epoch sec; seqNo:8461; depth 9.80 2021-02-12T23:58:02Z- handleDvl() - RDI_Pathfinder msg: 1613174282.23 epoch sec; seqNo:4027 2021-02-12T23:58:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 105.49 , 116.31 , 79.80 2021-02-12T23:58:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.09 , -0.12 2021-02-12T23:58:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174282.24 epoch sec; seqNo:8461; 4058283.23 north; 591810.11 east 2021-02-12T23:58:02Z- LcmTrn::handleMessages() - handling 42 messages took 0.64 ms and 129 calls... 2021-02-12T23:58:02Z- waiting for 1613174280.38, time is 1613174282.24 2021-02-12T23:58:02Z- LcmTrn::updateTrn() equating pose 1613174282.24 and meas times 1613174282.24 2021-02-12T23:58:02Z- LcmTrn::updateTrn() >>>> pose time 1613174282.24, meas time 1613174282.24 <<<< 2021-02-12T23:58:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174282.24 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:58:02Z- TerrainNav::motionUpdate() - currEstimate = (4058283, 591810.11, 9.80) 2021-02-12T23:58:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:58:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:58:02Z- TNavBF::Projecting Measurements 2021-02-12T23:58:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:58:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:58:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:58:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:58:02Z- TNavFilter::4 good beams. 2021-02-12T23:58:02Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:58:02Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:58:02Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:58:02Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:58:02Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:58:02Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:58:02Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:58:02Z- TNavBF::mmse selected from filter -1 2021-02-12T23:58:02Z- TerrainMapOctree: (vehN, vehE ) = (4058283.23, 591810.11). 2021-02-12T23:58:02Z- TerrainMapOctree: Min Distance = 3841.20. 2021-02-12T23:58:02Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:58:02Z- There is no specific good beam that all particles have in common. 2021-02-12T23:58:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:58:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 11.99 2021-02-12T23:58:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:58:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 17.81 2021-02-12T23:58:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:58:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 21.64 2021-02-12T23:58:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:58:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 10.19 2021-02-12T23:58:02Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:58:02Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:58:02Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:58:02Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:58:02Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:58:02Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:58:02Z- Filter 0 No beams used in subcloud update 2021-02-12T23:58:02Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:58:02Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:58:02Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 725.8 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:58:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174282.24 sec. 2021-02-12T23:58:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:58:02Z- TNavBF::mmse selected from filter -1 2021-02-12T23:58:02Z- variances([0], [2], and psi_berg) = 725.828035 833.013592 0.000000 2021-02-12T23:58:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:58:02Z- LcmTrn::publishEstimates() - MLE : 4058277.73 591763.61 9.80 2021-02-12T23:58:02Z- LcmTrn::publishEstimates() - MMSE : 4058283.23 591810.11 9.80 2021-02-12T23:58:02Z- LcmTrn::publishEstimates() - COVAR: 725.83 833.01 1.02 0.00 2021-02-12T23:58:02Z- AHRS_M2 msg: 1613174282.62 epoch sec; seqNo:8393 2021-02-12T23:58:02Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.19 psi 2021-02-12T23:58:02Z- handleDepth()-Depth_Keller msg: 1613174282620 epoch sec; seqNo:8462; depth 9.70 2021-02-12T23:58:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174282.64 epoch sec; seqNo:8462; 4058283.23 north; 591810.11 east 2021-02-12T23:58:03Z- AHRS_M2 msg: 1613174283.02 epoch sec; seqNo:8394 2021-02-12T23:58:03Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.19 psi 2021-02-12T23:58:03Z- handleDepth()-Depth_Keller msg: 1613174283024 epoch sec; seqNo:8463; depth 9.59 2021-02-12T23:58:03Z- handleDvl() - RDI_Pathfinder msg: 1613174283.04 epoch sec; seqNo:4028 2021-02-12T23:58:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 106.77 , 117.59 , 81.08 2021-02-12T23:58:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.18 , -0.04 2021-02-12T23:58:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174283.06 epoch sec; seqNo:8463; 4058283.62 north; 591811.33 east 2021-02-12T23:58:03Z- AHRS_M2 msg: 1613174283.42 epoch sec; seqNo:8395 2021-02-12T23:58:03Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-12T23:58:03Z- handleDepth()-Depth_Keller msg: 1613174283428 epoch sec; seqNo:8464; depth 9.47 2021-02-12T23:58:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174283.44 epoch sec; seqNo:8464; 4058284.00 north; 591811.33 east 2021-02-12T23:58:03Z- AHRS_M2 msg: 1613174283.85 epoch sec; seqNo:8396 2021-02-12T23:58:03Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.19 psi 2021-02-12T23:58:03Z- handleDepth()-Depth_Keller msg: 1613174283861 epoch sec; seqNo:8465; depth 9.38 2021-02-12T23:58:03Z- handleDvl() - RDI_Pathfinder msg: 1613174283.87 epoch sec; seqNo:4029 2021-02-12T23:58:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.70 , 107.99 , 120.16 , 0.00 2021-02-12T23:58:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.15 , 0.07 2021-02-12T23:58:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174283.89 epoch sec; seqNo:8465; 4058284.38 north; 591811.32 east 2021-02-12T23:58:04Z- AHRS_M2 msg: 1613174284.23 epoch sec; seqNo:8397 2021-02-12T23:58:04Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.18 psi 2021-02-12T23:58:04Z- handleDepth()-Depth_Keller msg: 1613174284237 epoch sec; seqNo:8466; depth 9.25 2021-02-12T23:58:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174284.26 epoch sec; seqNo:8466; 4058284.76 north; 591811.32 east 2021-02-12T23:58:04Z- AHRS_M2 msg: 1613174284.63 epoch sec; seqNo:8398 2021-02-12T23:58:04Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-12T23:58:04Z- handleDepth()-Depth_Keller msg: 1613174284641 epoch sec; seqNo:8467; depth 9.15 2021-02-12T23:58:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:58:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:58:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:58:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:58:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:58:04Z- handleDvl() - RDI_Pathfinder msg: 1613174284.65 epoch sec; seqNo:4030 2021-02-12T23:58:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.15 , 0.07 2021-02-12T23:58:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174284.65 epoch sec; seqNo:8467; 4058285.14 north; 591811.31 east 2021-02-12T23:58:05Z- AHRS_M2 msg: 1613174285.05 epoch sec; seqNo:8399 2021-02-12T23:58:05Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.16 psi 2021-02-12T23:58:05Z- handleDepth()-Depth_Keller msg: 1613174285051 epoch sec; seqNo:8468; depth 9.03 2021-02-12T23:58:05Z- handleDvl() - RDI_Pathfinder msg: 1613174285.06 epoch sec; seqNo:4031 2021-02-12T23:58:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:05Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:58:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174285.07 epoch sec; seqNo:8468; 4058285.51 north; 591811.31 east 2021-02-12T23:58:05Z- AHRS_M2 msg: 1613174285.45 epoch sec; seqNo:8400 2021-02-12T23:58:05Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-12T23:58:05Z- handleDepth()-Depth_Keller msg: 1613174285451 epoch sec; seqNo:8469; depth 8.93 2021-02-12T23:58:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174285.47 epoch sec; seqNo:8469; 4058285.51 north; 591811.31 east 2021-02-12T23:58:05Z- AHRS_M2 msg: 1613174285.85 epoch sec; seqNo:8401 2021-02-12T23:58:05Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-12T23:58:05Z- handleDepth()-Depth_Keller msg: 1613174285853 epoch sec; seqNo:8470; depth 8.83 2021-02-12T23:58:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174285.86 epoch sec; seqNo:8470; 4058285.91 north; 591812.53 east 2021-02-12T23:58:06Z- AHRS_M2 msg: 1613174286.25 epoch sec; seqNo:8402 2021-02-12T23:58:06Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-12T23:58:06Z- handleDepth()-Depth_Keller msg: 1613174286261 epoch sec; seqNo:8471; depth 8.71 2021-02-12T23:58:06Z- handleDvl() - RDI_Pathfinder msg: 1613174286.27 epoch sec; seqNo:4032 2021-02-12T23:58:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.80 , 106.16 , 121.04 , 80.88 2021-02-12T23:58:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.04 , 0.04 2021-02-12T23:58:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174286.28 epoch sec; seqNo:8471; 4058286.28 north; 591812.52 east 2021-02-12T23:58:06Z- AHRS_M2 msg: 1613174286.66 epoch sec; seqNo:8403 2021-02-12T23:58:06Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-12T23:58:06Z- handleDepth()-Depth_Keller msg: 1613174286661 epoch sec; seqNo:8472; depth 8.58 2021-02-12T23:58:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174286.67 epoch sec; seqNo:8472; 4058286.66 north; 591812.52 east 2021-02-12T23:58:07Z- AHRS_M2 msg: 1613174287.06 epoch sec; seqNo:8404 2021-02-12T23:58:07Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-12T23:58:07Z- handleDepth()-Depth_Keller msg: 1613174287064 epoch sec; seqNo:8473; depth 8.47 2021-02-12T23:58:07Z- handleDvl() - RDI_Pathfinder msg: 1613174287.08 epoch sec; seqNo:4033 2021-02-12T23:58:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 105.22 , 121.45 , 80.88 2021-02-12T23:58:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.00 , 0.05 2021-02-12T23:58:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174287.09 epoch sec; seqNo:8473; 4058286.66 north; 591812.52 east 2021-02-12T23:58:07Z- AHRS_M2 msg: 1613174287.46 epoch sec; seqNo:8405 2021-02-12T23:58:07Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.16 psi 2021-02-12T23:58:07Z- handleDepth()-Depth_Keller msg: 1613174287473 epoch sec; seqNo:8474; depth 8.33 2021-02-12T23:58:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174287.48 epoch sec; seqNo:8474; 4058287.04 north; 591812.51 east 2021-02-12T23:58:07Z- LcmTrn::handleMessages() - handling 45 messages took 0.66 ms and 132 calls... 2021-02-12T23:58:07Z- waiting for 1613174285.24, time is 1613174287.48 2021-02-12T23:58:07Z- LcmTrn::updateTrn() >>>> pose time 1613174287.48, meas time 1613174287.08 <<<< 2021-02-12T23:58:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174287.08 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:58:07Z- TerrainNav::motionUpdate() - currEstimate = (4058287, 591812.51, 8.33) 2021-02-12T23:58:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:58:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:58:07Z- TNavBF::Projecting Measurements 2021-02-12T23:58:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:58:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:58:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:58:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:58:07Z- TNavFilter::4 good beams. 2021-02-12T23:58:07Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:58:07Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:58:07Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:58:07Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:58:07Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:58:07Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:58:07Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:58:07Z- TNavBF::mmse selected from filter -1 2021-02-12T23:58:07Z- TerrainMapOctree: (vehN, vehE ) = (4058286.75, 591812.33). 2021-02-12T23:58:07Z- TerrainMapOctree: Min Distance = 3837.07. 2021-02-12T23:58:07Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:58:07Z- There is no specific good beam that all particles have in common. 2021-02-12T23:58:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:58:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-12T23:58:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:58:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 17.71 2021-02-12T23:58:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:58:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 23.60 2021-02-12T23:58:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:58:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 10.47 2021-02-12T23:58:07Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:58:07Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:58:07Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:58:07Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:58:07Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:58:07Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:58:07Z- Filter 0 No beams used in subcloud update 2021-02-12T23:58:07Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:58:07Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:58:07Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 725.8 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-12T23:58:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174287.08 sec. 2021-02-12T23:58:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:58:07Z- TNavBF::mmse selected from filter -1 2021-02-12T23:58:07Z- variances([0], [2], and psi_berg) = 725.733307 833.013526 0.000000 2021-02-12T23:58:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:58:07Z- LcmTrn::publishEstimates() - MLE : 4058281.54 591767.01 8.33 2021-02-12T23:58:07Z- LcmTrn::publishEstimates() - MMSE : 4058287.04 591812.51 8.33 2021-02-12T23:58:07Z- LcmTrn::publishEstimates() - COVAR: 725.73 833.01 0.98 0.00 2021-02-12T23:58:07Z- AHRS_M2 msg: 1613174287.87 epoch sec; seqNo:8406 2021-02-12T23:58:07Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.16 psi 2021-02-12T23:58:07Z- handleDepth()-Depth_Keller msg: 1613174287872 epoch sec; seqNo:8475; depth 8.22 2021-02-12T23:58:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174287.88 epoch sec; seqNo:8475; 4058287.42 north; 591812.51 east 2021-02-12T23:58:08Z- AHRS_M2 msg: 1613174288.27 epoch sec; seqNo:8407 2021-02-12T23:58:08Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.16 psi 2021-02-12T23:58:08Z- handleDepth()-Depth_Keller msg: 1613174288285 epoch sec; seqNo:8476; depth 8.08 2021-02-12T23:58:08Z- handleDvl() - RDI_Pathfinder msg: 1613174288.30 epoch sec; seqNo:4034 2021-02-12T23:58:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.55 , 106.91 , 124.76 , 81.62 2021-02-12T23:58:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , 0.15 2021-02-12T23:58:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174288.31 epoch sec; seqNo:8476; 4058287.42 north; 591812.51 east 2021-02-12T23:58:08Z- AHRS_M2 msg: 1613174288.68 epoch sec; seqNo:8408 2021-02-12T23:58:08Z- AHRS_M2 msg: 0.59 phi; 0.38 theta; -0.15 psi 2021-02-12T23:58:08Z- handleDepth()-Depth_Keller msg: 1613174288680 epoch sec; seqNo:8477; depth 7.97 2021-02-12T23:58:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174288.70 epoch sec; seqNo:8477; 4058287.81 north; 591813.73 east 2021-02-12T23:58:09Z- AHRS_M2 msg: 1613174289.08 epoch sec; seqNo:8409 2021-02-12T23:58:09Z- AHRS_M2 msg: 0.60 phi; 0.39 theta; -0.13 psi 2021-02-12T23:58:09Z- handleDepth()-Depth_Keller msg: 1613174289085 epoch sec; seqNo:8478; depth 7.83 2021-02-12T23:58:09Z- handleDvl() - RDI_Pathfinder msg: 1613174289.10 epoch sec; seqNo:4035 2021-02-12T23:58:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 105.09 , 125.37 , 82.10 2021-02-12T23:58:09Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.14 , 0.14 2021-02-12T23:58:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174289.11 epoch sec; seqNo:8478; 4058288.19 north; 591813.72 east 2021-02-12T23:58:09Z- AHRS_M2 msg: 1613174289.48 epoch sec; seqNo:8410 2021-02-12T23:58:09Z- AHRS_M2 msg: 0.61 phi; 0.39 theta; -0.12 psi 2021-02-12T23:58:09Z- handleDepth()-Depth_Keller msg: 1613174289489 epoch sec; seqNo:8479; depth 7.71 2021-02-12T23:58:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:09Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:58:09Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:58:09Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:58:09Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:58:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:58:09Z- handleDvl() - RDI_Pathfinder msg: 1613174289.49 epoch sec; seqNo:4036 2021-02-12T23:58:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:09Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.14 , 0.14 2021-02-12T23:58:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174289.50 epoch sec; seqNo:8479; 4058288.57 north; 591813.72 east 2021-02-12T23:58:09Z- AHRS_M2 msg: 1613174289.89 epoch sec; seqNo:8411 2021-02-12T23:58:09Z- AHRS_M2 msg: 0.62 phi; 0.39 theta; -0.11 psi 2021-02-12T23:58:09Z- handleDepth()-Depth_Keller msg: 1613174289892 epoch sec; seqNo:8480; depth 7.59 2021-02-12T23:58:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174289.91 epoch sec; seqNo:8480; 4058288.57 north; 591813.72 east 2021-02-12T23:58:10Z- AHRS_M2 msg: 1613174290.29 epoch sec; seqNo:8412 2021-02-12T23:58:10Z- AHRS_M2 msg: 0.64 phi; 0.39 theta; -0.10 psi 2021-02-12T23:58:10Z- handleDepth()-Depth_Keller msg: 1613174290296 epoch sec; seqNo:8481; depth 7.46 2021-02-12T23:58:10Z- handleDvl() - RDI_Pathfinder msg: 1613174290.31 epoch sec; seqNo:4037 2021-02-12T23:58:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.91 , 106.84 , 126.17 , 83.04 2021-02-12T23:58:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.24 , -0.09 2021-02-12T23:58:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174290.33 epoch sec; seqNo:8481; 4058288.96 north; 591814.94 east 2021-02-12T23:58:10Z- AHRS_M2 msg: 1613174290.70 epoch sec; seqNo:8413 2021-02-12T23:58:10Z- AHRS_M2 msg: 0.65 phi; 0.38 theta; -0.11 psi 2021-02-12T23:58:10Z- handleDepth()-Depth_Keller msg: 1613174290700 epoch sec; seqNo:8482; depth 7.33 2021-02-12T23:58:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:58:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:58:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:58:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:58:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:58:10Z- handleDvl() - RDI_Pathfinder msg: 1613174290.70 epoch sec; seqNo:4038 2021-02-12T23:58:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.24 , -0.09 2021-02-12T23:58:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174290.71 epoch sec; seqNo:8482; 4058289.34 north; 591814.93 east 2021-02-12T23:58:11Z- AHRS_M2 msg: 1613174291.10 epoch sec; seqNo:8414 2021-02-12T23:58:11Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.12 psi 2021-02-12T23:58:11Z- handleDepth()-Depth_Keller msg: 1613174291106 epoch sec; seqNo:8483; depth 7.20 2021-02-12T23:58:11Z- handleDvl() - RDI_Pathfinder msg: 1613174291.12 epoch sec; seqNo:4039 2021-02-12T23:58:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 106.70 , 127.53 , 81.42 2021-02-12T23:58:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.08 , -0.26 2021-02-12T23:58:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174291.13 epoch sec; seqNo:8483; 4058289.34 north; 591814.93 east 2021-02-12T23:58:11Z- AHRS_M2 msg: 1613174291.50 epoch sec; seqNo:8415 2021-02-12T23:58:11Z- AHRS_M2 msg: 0.67 phi; 0.36 theta; -0.12 psi 2021-02-12T23:58:11Z- handleDepth()-Depth_Keller msg: 1613174291508 epoch sec; seqNo:8484; depth 7.06 2021-02-12T23:58:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174291.53 epoch sec; seqNo:8484; 4058289.72 north; 591814.93 east 2021-02-12T23:58:11Z- AHRS_M2 msg: 1613174291.91 epoch sec; seqNo:8416 2021-02-12T23:58:11Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.12 psi 2021-02-12T23:58:11Z- handleDepth()-Depth_Keller msg: 1613174291917 epoch sec; seqNo:8485; depth 6.92 2021-02-12T23:58:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174291.95 epoch sec; seqNo:8485; 4058289.72 north; 591814.93 east 2021-02-12T23:58:12Z- AHRS_M2 msg: 1613174292.31 epoch sec; seqNo:8417 2021-02-12T23:58:12Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.12 psi 2021-02-12T23:58:12Z- handleDepth()-Depth_Keller msg: 1613174292316 epoch sec; seqNo:8486; depth 6.81 2021-02-12T23:58:12Z- handleDvl() - RDI_Pathfinder msg: 1613174292.33 epoch sec; seqNo:4040 2021-02-12T23:58:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 103.80 , 127.60 , 81.49 2021-02-12T23:58:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.05 , -0.16 2021-02-12T23:58:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174292.36 epoch sec; seqNo:8486; 4058290.10 north; 591814.92 east 2021-02-12T23:58:12Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-12T23:58:12Z- waiting for 1613174290.48, time is 1613174292.36 2021-02-12T23:58:12Z- LcmTrn::updateTrn() equating pose 1613174292.36 and meas times 1613174292.36 2021-02-12T23:58:12Z- LcmTrn::updateTrn() >>>> pose time 1613174292.36, meas time 1613174292.36 <<<< 2021-02-12T23:58:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174292.36 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:58:12Z- TerrainNav::motionUpdate() - currEstimate = (4058290, 591814.92, 6.81) 2021-02-12T23:58:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:58:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:58:12Z- TNavBF::Projecting Measurements 2021-02-12T23:58:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:58:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:58:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:58:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:58:12Z- TNavFilter::4 good beams. 2021-02-12T23:58:12Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:58:12Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:58:12Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:58:12Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:58:12Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:58:12Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:58:12Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:58:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:58:12Z- TerrainMapOctree: (vehN, vehE ) = (4058290.10, 591814.92). 2021-02-12T23:58:12Z- TerrainMapOctree: Min Distance = 3832.83. 2021-02-12T23:58:12Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:58:12Z- There is no specific good beam that all particles have in common. 2021-02-12T23:58:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:58:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-12T23:58:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:58:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 17.24 2021-02-12T23:58:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:58:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 26.05 2021-02-12T23:58:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:58:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 10.62 2021-02-12T23:58:12Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:58:12Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:58:12Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:58:12Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:58:12Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:58:12Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:58:12Z- Filter 0 No beams used in subcloud update 2021-02-12T23:58:12Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:58:12Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:58:12Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 725.7 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:58:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174292.36 sec. 2021-02-12T23:58:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:58:12Z- TNavBF::mmse selected from filter -1 2021-02-12T23:58:12Z- variances([0], [2], and psi_berg) = 725.662227 833.013476 0.000000 2021-02-12T23:58:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:58:12Z- LcmTrn::publishEstimates() - MLE : 4058284.60 591769.42 6.81 2021-02-12T23:58:12Z- LcmTrn::publishEstimates() - MMSE : 4058290.10 591814.92 6.81 2021-02-12T23:58:12Z- LcmTrn::publishEstimates() - COVAR: 725.66 833.01 0.93 0.00 2021-02-12T23:58:12Z- AHRS_M2 msg: 1613174292.72 epoch sec; seqNo:8418 2021-02-12T23:58:12Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.12 psi 2021-02-12T23:58:12Z- handleDepth()-Depth_Keller msg: 1613174292721 epoch sec; seqNo:8487; depth 6.68 2021-02-12T23:58:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174292.73 epoch sec; seqNo:8487; 4058290.10 north; 591814.92 east 2021-02-12T23:58:13Z- AHRS_M2 msg: 1613174293.12 epoch sec; seqNo:8419 2021-02-12T23:58:13Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.14 psi 2021-02-12T23:58:13Z- handleDepth()-Depth_Keller msg: 1613174293127 epoch sec; seqNo:8488; depth 6.54 2021-02-12T23:58:13Z- handleDvl() - RDI_Pathfinder msg: 1613174293.15 epoch sec; seqNo:4041 2021-02-12T23:58:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 108.26 , 124.63 , 82.97 2021-02-12T23:58:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.10 , -0.14 2021-02-12T23:58:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174293.16 epoch sec; seqNo:8488; 4058290.49 north; 591816.14 east 2021-02-12T23:58:13Z- AHRS_M2 msg: 1613174293.52 epoch sec; seqNo:8420 2021-02-12T23:58:13Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.15 psi 2021-02-12T23:58:13Z- handleDepth()-Depth_Keller msg: 1613174293528 epoch sec; seqNo:8489; depth 6.42 2021-02-12T23:58:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174293.54 epoch sec; seqNo:8489; 4058290.87 north; 591816.14 east 2021-02-12T23:58:13Z- AHRS_M2 msg: 1613174293.93 epoch sec; seqNo:8421 2021-02-12T23:58:13Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.17 psi 2021-02-12T23:58:13Z- handleDepth()-Depth_Keller msg: 1613174293936 epoch sec; seqNo:8490; depth 6.29 2021-02-12T23:58:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174294.03 epoch sec; seqNo:8490; 4058290.87 north; 591816.14 east 2021-02-12T23:58:14Z- AHRS_M2 msg: 1613174294.33 epoch sec; seqNo:8422 2021-02-12T23:58:14Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.17 psi 2021-02-12T23:58:14Z- handleDepth()-Depth_Keller msg: 1613174294336 epoch sec; seqNo:8491; depth 6.13 2021-02-12T23:58:14Z- handleDvl() - RDI_Pathfinder msg: 1613174294.35 epoch sec; seqNo:4042 2021-02-12T23:58:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 108.26 , 123.13 , 82.97 2021-02-12T23:58:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.28 , -0.12 2021-02-12T23:58:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174294.36 epoch sec; seqNo:8491; 4058291.25 north; 591816.13 east 2021-02-12T23:58:14Z- AHRS_M2 msg: 1613174294.73 epoch sec; seqNo:8423 2021-02-12T23:58:14Z- AHRS_M2 msg: 0.68 phi; 0.34 theta; -0.17 psi 2021-02-12T23:58:14Z- handleDepth()-Depth_Keller msg: 1613174294740 epoch sec; seqNo:8492; depth 6.03 2021-02-12T23:58:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174294.75 epoch sec; seqNo:8492; 4058291.63 north; 591816.13 east 2021-02-12T23:58:15Z- AHRS_M2 msg: 1613174295.14 epoch sec; seqNo:8424 2021-02-12T23:58:15Z- AHRS_M2 msg: 0.68 phi; 0.34 theta; -0.16 psi 2021-02-12T23:58:15Z- handleDepth()-Depth_Keller msg: 1613174295149 epoch sec; seqNo:8493; depth 5.92 2021-02-12T23:58:15Z- handleDvl() - RDI_Pathfinder msg: 1613174295.16 epoch sec; seqNo:4043 2021-02-12T23:58:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.41 , 111.23 , 124.62 , 82.97 2021-02-12T23:58:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.31 , 0.14 2021-02-12T23:58:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174295.18 epoch sec; seqNo:8493; 4058292.01 north; 591816.12 east 2021-02-12T23:58:15Z- AHRS_M2 msg: 1613174295.54 epoch sec; seqNo:8425 2021-02-12T23:58:15Z- AHRS_M2 msg: 0.68 phi; 0.34 theta; -0.14 psi 2021-02-12T23:58:15Z- handleDepth()-Depth_Keller msg: 1613174295548 epoch sec; seqNo:8494; depth 5.79 2021-02-12T23:58:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:15Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:58:15Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:58:15Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:58:15Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:58:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:58:15Z- handleDvl() - RDI_Pathfinder msg: 1613174295.55 epoch sec; seqNo:4044 2021-02-12T23:58:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.31 , 0.14 2021-02-12T23:58:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174295.56 epoch sec; seqNo:8494; 4058292.39 north; 591816.12 east 2021-02-12T23:58:15Z- AHRS_M2 msg: 1613174295.98 epoch sec; seqNo:8426 2021-02-12T23:58:15Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.14 psi 2021-02-12T23:58:15Z- handleDepth()-Depth_Keller msg: 1613174295982 epoch sec; seqNo:8495; depth 5.68 2021-02-12T23:58:15Z- handleDvl() - RDI_Pathfinder msg: 1613174295.99 epoch sec; seqNo:4045 2021-02-12T23:58:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:15Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:58:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174296.02 epoch sec; seqNo:8495; 4058292.76 north; 591816.12 east 2021-02-12T23:58:16Z- AHRS_M2 msg: 1613174296.35 epoch sec; seqNo:8427 2021-02-12T23:58:16Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.13 psi 2021-02-12T23:58:16Z- handleDepth()-Depth_Keller msg: 1613174296356 epoch sec; seqNo:8496; depth 5.52 2021-02-12T23:58:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174296.38 epoch sec; seqNo:8496; 4058293.14 north; 591816.11 east 2021-02-12T23:58:16Z- AHRS_M2 msg: 1613174296.76 epoch sec; seqNo:8428 2021-02-12T23:58:16Z- AHRS_M2 msg: 0.66 phi; 0.30 theta; -0.13 psi 2021-02-12T23:58:16Z- handleDepth()-Depth_Keller msg: 1613174296760 epoch sec; seqNo:8497; depth 5.38 2021-02-12T23:58:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:58:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:58:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:58:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:58:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:58:16Z- handleDvl() - RDI_Pathfinder msg: 1613174296.76 epoch sec; seqNo:4046 2021-02-12T23:58:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:16Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:58:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174296.77 epoch sec; seqNo:8497; 4058293.14 north; 591816.11 east 2021-02-12T23:58:17Z- AHRS_M2 msg: 1613174297.16 epoch sec; seqNo:8429 2021-02-12T23:58:17Z- AHRS_M2 msg: 0.65 phi; 0.28 theta; -0.13 psi 2021-02-12T23:58:17Z- handleDepth()-Depth_Keller msg: 1613174297164 epoch sec; seqNo:8498; depth 5.26 2021-02-12T23:58:17Z- handleDvl() - RDI_Pathfinder msg: 1613174297.18 epoch sec; seqNo:4047 2021-02-12T23:58:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.78 , 106.16 , 121.04 , 83.85 2021-02-12T23:58:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.12 , 0.32 2021-02-12T23:58:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174297.19 epoch sec; seqNo:8498; 4058293.54 north; 591817.33 east 2021-02-12T23:58:17Z- AHRS_M2 msg: 1613174297.57 epoch sec; seqNo:8430 2021-02-12T23:58:17Z- AHRS_M2 msg: 0.64 phi; 0.25 theta; -0.14 psi 2021-02-12T23:58:17Z- handleDepth()-Depth_Keller msg: 1613174297569 epoch sec; seqNo:8499; depth 5.12 2021-02-12T23:58:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174297.58 epoch sec; seqNo:8499; 4058293.91 north; 591817.32 east 2021-02-12T23:58:17Z- LcmTrn::handleMessages() - handling 46 messages took 0.68 ms and 133 calls... 2021-02-12T23:58:17Z- waiting for 1613174295.36, time is 1613174297.58 2021-02-12T23:58:17Z- LcmTrn::updateTrn() >>>> pose time 1613174297.58, meas time 1613174297.18 <<<< 2021-02-12T23:58:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174297.18 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:58:17Z- TerrainNav::motionUpdate() - currEstimate = (4058294, 591817.32, 5.12) 2021-02-12T23:58:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:58:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:58:17Z- TNavBF::Projecting Measurements 2021-02-12T23:58:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:58:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:58:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:58:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:58:17Z- TNavFilter::4 good beams. 2021-02-12T23:58:17Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:58:17Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:58:17Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:58:17Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:58:17Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:58:17Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:58:17Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:58:17Z- TNavBF::mmse selected from filter -1 2021-02-12T23:58:17Z- TerrainMapOctree: (vehN, vehE ) = (4058293.62, 591817.14). 2021-02-12T23:58:17Z- TerrainMapOctree: Min Distance = 3828.71. 2021-02-12T23:58:17Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:58:17Z- There is no specific good beam that all particles have in common. 2021-02-12T23:58:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:58:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 13.77 2021-02-12T23:58:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:58:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 18.03 2021-02-12T23:58:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:58:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 23.44 2021-02-12T23:58:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:58:17Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 11.25 2021-02-12T23:58:17Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:58:17Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:58:17Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:58:17Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:58:17Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:58:17Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:58:17Z- Filter 0 No beams used in subcloud update 2021-02-12T23:58:17Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:58:17Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:58:17Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 725.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:58:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174297.18 sec. 2021-02-12T23:58:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:58:17Z- TNavBF::mmse selected from filter -1 2021-02-12T23:58:17Z- variances([0], [2], and psi_berg) = 725.567569 833.013411 0.000000 2021-02-12T23:58:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:58:17Z- LcmTrn::publishEstimates() - MLE : 4058288.41 591770.82 5.12 2021-02-12T23:58:17Z- LcmTrn::publishEstimates() - MMSE : 4058293.91 591817.32 5.12 2021-02-12T23:58:17Z- LcmTrn::publishEstimates() - COVAR: 725.57 833.01 0.92 0.00 2021-02-12T23:58:17Z- AHRS_M2 msg: 1613174297.97 epoch sec; seqNo:8431 2021-02-12T23:58:17Z- AHRS_M2 msg: 0.64 phi; 0.22 theta; -0.16 psi 2021-02-12T23:58:17Z- handleDepth()-Depth_Keller msg: 1613174297985 epoch sec; seqNo:8500; depth 4.98 2021-02-12T23:58:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174298.00 epoch sec; seqNo:8500; 4058294.29 north; 591817.32 east 2021-02-12T23:58:18Z- AHRS_M2 msg: 1613174298.37 epoch sec; seqNo:8432 2021-02-12T23:58:18Z- AHRS_M2 msg: 0.64 phi; 0.19 theta; -0.19 psi 2021-02-12T23:58:18Z- handleDepth()-Depth_Keller msg: 1613174298376 epoch sec; seqNo:8501; depth 4.83 2021-02-12T23:58:18Z- handleDvl() - RDI_Pathfinder msg: 1613174298.39 epoch sec; seqNo:4048 2021-02-12T23:58:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 105.09 , 113.20 , 86.15 2021-02-12T23:58:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.26 , 0.09 2021-02-12T23:58:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174298.42 epoch sec; seqNo:8501; 4058294.29 north; 591817.32 east 2021-02-12T23:58:18Z- AHRS_M2 msg: 1613174298.78 epoch sec; seqNo:8433 2021-02-12T23:58:18Z- AHRS_M2 msg: 0.65 phi; 0.15 theta; -0.22 psi 2021-02-12T23:58:18Z- handleDepth()-Depth_Keller msg: 1613174298780 epoch sec; seqNo:8502; depth 4.70 2021-02-12T23:58:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174298.79 epoch sec; seqNo:8502; 4058294.68 north; 591818.54 east 2021-02-12T23:58:19Z- AHRS_M2 msg: 1613174299.18 epoch sec; seqNo:8434 2021-02-12T23:58:19Z- AHRS_M2 msg: 0.66 phi; 0.11 theta; -0.26 psi 2021-02-12T23:58:19Z- handleDepth()-Depth_Keller msg: 1613174299185 epoch sec; seqNo:8503; depth 4.60 2021-02-12T23:58:19Z- handleDvl() - RDI_Pathfinder msg: 1613174299.20 epoch sec; seqNo:4049 2021-02-12T23:58:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 111.85 , 107.79 , 96.97 2021-02-12T23:58:19Z- handleDvl() - RDI_Pathfinder msg: velocities 1.10 , 0.14 , -0.05 2021-02-12T23:58:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174299.21 epoch sec; seqNo:8503; 4058295.06 north; 591818.53 east 2021-02-12T23:58:19Z- AHRS_M2 msg: 1613174299.58 epoch sec; seqNo:8435 2021-02-12T23:58:19Z- AHRS_M2 msg: 0.68 phi; 0.06 theta; -0.28 psi 2021-02-12T23:58:19Z- handleDepth()-Depth_Keller msg: 1613174299588 epoch sec; seqNo:8504; depth 4.51 2021-02-12T23:58:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174299.61 epoch sec; seqNo:8504; 4058295.06 north; 591818.53 east 2021-02-12T23:58:19Z- AHRS_M2 msg: 1613174299.99 epoch sec; seqNo:8436 2021-02-12T23:58:19Z- AHRS_M2 msg: 0.71 phi; 0.01 theta; -0.27 psi 2021-02-12T23:58:19Z- handleDepth()-Depth_Keller msg: 1613174299997 epoch sec; seqNo:8505; depth 4.45 2021-02-12T23:58:20Z- handleDvl() - RDI_Pathfinder msg: 1613174300.02 epoch sec; seqNo:4050 2021-02-12T23:58:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 115.69 , 102.31 , 97.85 2021-02-12T23:58:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.09 , -0.03 2021-02-12T23:58:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174300.03 epoch sec; seqNo:8505; 4058295.44 north; 591818.53 east 2021-02-12T23:58:20Z- AHRS_M2 msg: 1613174300.39 epoch sec; seqNo:8437 2021-02-12T23:58:20Z- AHRS_M2 msg: 0.74 phi; -0.04 theta; -0.24 psi 2021-02-12T23:58:20Z- handleDepth()-Depth_Keller msg: 1613174300401 epoch sec; seqNo:8506; depth 4.38 2021-02-12T23:58:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:58:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:58:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:58:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:58:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:58:20Z- handleDvl() - RDI_Pathfinder msg: 1613174300.40 epoch sec; seqNo:4051 2021-02-12T23:58:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.09 , -0.03 2021-02-12T23:58:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174300.42 epoch sec; seqNo:8506; 4058295.83 north; 591819.75 east 2021-02-12T23:58:20Z- AHRS_M2 msg: 1613174300.80 epoch sec; seqNo:8438 2021-02-12T23:58:20Z- AHRS_M2 msg: 0.76 phi; -0.10 theta; -0.20 psi 2021-02-12T23:58:20Z- handleDepth()-Depth_Keller msg: 1613174300802 epoch sec; seqNo:8507; depth 4.35 2021-02-12T23:58:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174300.81 epoch sec; seqNo:8507; 4058296.21 north; 591819.74 east 2021-02-12T23:58:21Z- AHRS_M2 msg: 1613174301.20 epoch sec; seqNo:8439 2021-02-12T23:58:21Z- AHRS_M2 msg: 0.78 phi; -0.15 theta; -0.15 psi 2021-02-12T23:58:21Z- handleDepth()-Depth_Keller msg: 1613174301204 epoch sec; seqNo:8508; depth 4.32 2021-02-12T23:58:21Z- handleDvl() - RDI_Pathfinder msg: 1613174301.23 epoch sec; seqNo:4052 2021-02-12T23:58:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 109.54 , 97.64 , 100.62 2021-02-12T23:58:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.00 , -0.07 2021-02-12T23:58:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174301.24 epoch sec; seqNo:8508; 4058296.21 north; 591819.74 east 2021-02-12T23:58:21Z- handleDepth()-Depth_Keller msg: 1613174301629 epoch sec; seqNo:8509; depth 4.32 2021-02-12T23:58:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:21Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:58:21Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:58:21Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:58:21Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:58:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:58:21Z- handleDvl() - RDI_Pathfinder msg: 1613174301.63 epoch sec; seqNo:4053 2021-02-12T23:58:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.00 , -0.07 2021-02-12T23:58:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174301.65 epoch sec; seqNo:8509; 4058296.59 north; 591819.74 east 2021-02-12T23:58:22Z- AHRS_M2 msg: 1613174302.03 epoch sec; seqNo:8440 2021-02-12T23:58:22Z- AHRS_M2 msg: 0.78 phi; -0.27 theta; -0.11 psi 2021-02-12T23:58:22Z- handleDepth()-Depth_Keller msg: 1613174302031 epoch sec; seqNo:8510; depth 4.32 2021-02-12T23:58:22Z- handleDvl() - RDI_Pathfinder msg: 1613174302.04 epoch sec; seqNo:4054 2021-02-12T23:58:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.91 , 103.86 , 87.50 , 108.32 2021-02-12T23:58:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.02 , -0.00 2021-02-12T23:58:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174302.07 epoch sec; seqNo:8510; 4058296.59 north; 591819.74 east 2021-02-12T23:58:22Z- AHRS_M2 msg: 1613174302.42 epoch sec; seqNo:8441 2021-02-12T23:58:22Z- AHRS_M2 msg: 0.77 phi; -0.31 theta; -0.13 psi 2021-02-12T23:58:22Z- handleDepth()-Depth_Keller msg: 1613174302430 epoch sec; seqNo:8511; depth 4.33 2021-02-12T23:58:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174302.44 epoch sec; seqNo:8511; 4058296.97 north; 591819.73 east 2021-02-12T23:58:22Z- AHRS_M2 msg: 1613174302.82 epoch sec; seqNo:8442 2021-02-12T23:58:22Z- AHRS_M2 msg: 0.76 phi; -0.34 theta; -0.18 psi 2021-02-12T23:58:22Z- handleDepth()-Depth_Keller msg: 1613174302821 epoch sec; seqNo:8512; depth 4.38 2021-02-12T23:58:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:58:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:58:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:58:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:58:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:58:22Z- handleDvl() - RDI_Pathfinder msg: 1613174302.82 epoch sec; seqNo:4055 2021-02-12T23:58:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.02 , -0.00 2021-02-12T23:58:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174302.84 epoch sec; seqNo:8512; 4058296.98 north; 591820.95 east 2021-02-12T23:58:22Z- LcmTrn::handleMessages() - handling 46 messages took 0.68 ms and 133 calls... 2021-02-12T23:58:22Z- waiting for 1613174300.58, time is 1613174302.84 2021-02-12T23:58:22Z- LcmTrn::updateTrn() equating pose 1613174302.84 and meas times 1613174302.84 2021-02-12T23:58:22Z- LcmTrn::updateTrn() >>>> pose time 1613174302.84, meas time 1613174302.84 <<<< 2021-02-12T23:58:22Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174302.84, ping # 0. 2021-02-12T23:58:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174302.84 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:58:22Z- TerrainNav::motionUpdate() - currEstimate = (4058297, 591820.95, 4.38) 2021-02-12T23:58:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:58:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:58:22Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:58:22Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:58:22Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174302.84 sec.; No relevant navigation data available 2021-02-12T23:58:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:58:22Z- TNavBF::mmse selected from filter -1 2021-02-12T23:58:22Z- variances([0], [2], and psi_berg) = 725.489163 833.013356 0.000000 2021-02-12T23:58:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:58:22Z- LcmTrn::publishEstimates() - MLE : 4058291.48 591774.45 4.38 2021-02-12T23:58:22Z- LcmTrn::publishEstimates() - MMSE : 4058296.98 591820.95 4.38 2021-02-12T23:58:22Z- LcmTrn::publishEstimates() - COVAR: 725.49 833.01 1.16 0.00 2021-02-12T23:58:23Z- AHRS_M2 msg: 1613174303.22 epoch sec; seqNo:8443 2021-02-12T23:58:23Z- AHRS_M2 msg: 0.74 phi; -0.36 theta; -0.23 psi 2021-02-12T23:58:23Z- handleDepth()-Depth_Keller msg: 1613174303224 epoch sec; seqNo:8513; depth 4.42 2021-02-12T23:58:23Z- handleDvl() - RDI_Pathfinder msg: 1613174303.24 epoch sec; seqNo:4056 2021-02-12T23:58:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.10 , 106.57 , 84.93 , 117.38 2021-02-12T23:58:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.61 , 0.10 , 0.05 2021-02-12T23:58:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174303.25 epoch sec; seqNo:8513; 4058297.36 north; 591820.95 east 2021-02-12T23:58:23Z- AHRS_M2 msg: 1613174303.63 epoch sec; seqNo:8444 2021-02-12T23:58:23Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.25 psi 2021-02-12T23:58:23Z- handleDepth()-Depth_Keller msg: 1613174303635 epoch sec; seqNo:8514; depth 4.48 2021-02-12T23:58:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174303.65 epoch sec; seqNo:8514; 4058297.36 north; 591820.95 east 2021-02-12T23:58:24Z- AHRS_M2 msg: 1613174304.03 epoch sec; seqNo:8445 2021-02-12T23:58:24Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.28 psi 2021-02-12T23:58:24Z- handleDepth()-Depth_Keller msg: 1613174304037 epoch sec; seqNo:8515; depth 4.58 2021-02-12T23:58:24Z- handleDvl() - RDI_Pathfinder msg: 1613174304.05 epoch sec; seqNo:4057 2021-02-12T23:58:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.98 , 118.80 , 84.59 , 135.16 2021-02-12T23:58:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.18 , -0.07 2021-02-12T23:58:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174304.06 epoch sec; seqNo:8515; 4058297.36 north; 591820.95 east 2021-02-12T23:58:24Z- AHRS_M2 msg: 1613174304.43 epoch sec; seqNo:8446 2021-02-12T23:58:24Z- AHRS_M2 msg: 0.69 phi; -0.36 theta; -0.30 psi 2021-02-12T23:58:24Z- handleDepth()-Depth_Keller msg: 1613174304436 epoch sec; seqNo:8516; depth 4.66 2021-02-12T23:58:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174304.46 epoch sec; seqNo:8516; 4058297.74 north; 591820.95 east 2021-02-12T23:58:24Z- AHRS_M2 msg: 1613174304.84 epoch sec; seqNo:8447 2021-02-12T23:58:24Z- AHRS_M2 msg: 0.69 phi; -0.36 theta; -0.31 psi 2021-02-12T23:58:24Z- handleDepth()-Depth_Keller msg: 1613174304845 epoch sec; seqNo:8517; depth 4.77 2021-02-12T23:58:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174304.85 epoch sec; seqNo:8517; 4058298.13 north; 591822.16 east 2021-02-12T23:58:25Z- AHRS_M2 msg: 1613174305.26 epoch sec; seqNo:8448 2021-02-12T23:58:25Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.31 psi 2021-02-12T23:58:25Z- handleDepth()-Depth_Keller msg: 1613174305267 epoch sec; seqNo:8518; depth 4.87 2021-02-12T23:58:25Z- handleDvl() - RDI_Pathfinder msg: 1613174305.28 epoch sec; seqNo:4058 2021-02-12T23:58:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.31 , 125.57 , 85.41 , 134.49 2021-02-12T23:58:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.01 , -0.10 2021-02-12T23:58:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174305.30 epoch sec; seqNo:8518; 4058298.13 north; 591822.16 east 2021-02-12T23:58:25Z- AHRS_M2 msg: 1613174305.64 epoch sec; seqNo:8449 2021-02-12T23:58:25Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.29 psi 2021-02-12T23:58:25Z- handleDepth()-Depth_Keller msg: 1613174305651 epoch sec; seqNo:8519; depth 5.01 2021-02-12T23:58:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174305.66 epoch sec; seqNo:8519; 4058298.51 north; 591822.16 east 2021-02-12T23:58:26Z- AHRS_M2 msg: 1613174306.05 epoch sec; seqNo:8450 2021-02-12T23:58:26Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.28 psi 2021-02-12T23:58:26Z- handleDepth()-Depth_Keller msg: 1613174306052 epoch sec; seqNo:8520; depth 5.12 2021-02-12T23:58:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174306.07 epoch sec; seqNo:8520; 4058298.89 north; 591822.15 east 2021-02-12T23:58:26Z- AHRS_M2 msg: 1613174306.45 epoch sec; seqNo:8451 2021-02-12T23:58:26Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.26 psi 2021-02-12T23:58:26Z- handleDepth()-Depth_Keller msg: 1613174306456 epoch sec; seqNo:8521; depth 5.24 2021-02-12T23:58:26Z- handleDvl() - RDI_Pathfinder msg: 1613174306.47 epoch sec; seqNo:4059 2021-02-12T23:58:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.31 , 126.98 , 85.34 , 135.91 2021-02-12T23:58:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.09 , 0.00 2021-02-12T23:58:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174306.49 epoch sec; seqNo:8521; 4058299.27 north; 591822.15 east 2021-02-12T23:58:26Z- AHRS_M2 msg: 1613174306.86 epoch sec; seqNo:8452 2021-02-12T23:58:26Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.25 psi 2021-02-12T23:58:26Z- handleDepth()-Depth_Keller msg: 1613174306860 epoch sec; seqNo:8522; depth 5.38 2021-02-12T23:58:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174306.87 epoch sec; seqNo:8522; 4058299.65 north; 591822.15 east 2021-02-12T23:58:27Z- AHRS_M2 msg: 1613174307.26 epoch sec; seqNo:8453 2021-02-12T23:58:27Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.24 psi 2021-02-12T23:58:27Z- handleDepth()-Depth_Keller msg: 1613174307266 epoch sec; seqNo:8523; depth 5.52 2021-02-12T23:58:27Z- handleDvl() - RDI_Pathfinder msg: 1613174307.28 epoch sec; seqNo:4060 2021-02-12T23:58:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.87 , 122.60 , 83.92 , 130.04 2021-02-12T23:58:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.14 , -0.07 2021-02-12T23:58:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174307.29 epoch sec; seqNo:8523; 4058299.66 north; 591823.37 east 2021-02-12T23:58:27Z- AHRS_M2 msg: 1613174307.66 epoch sec; seqNo:8454 2021-02-12T23:58:27Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.23 psi 2021-02-12T23:58:27Z- handleDepth()-Depth_Keller msg: 1613174307668 epoch sec; seqNo:8524; depth 5.66 2021-02-12T23:58:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:58:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:58:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:58:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:58:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:58:27Z- handleDvl() - RDI_Pathfinder msg: 1613174307.67 epoch sec; seqNo:4061 2021-02-12T23:58:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.14 , -0.07 2021-02-12T23:58:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174307.69 epoch sec; seqNo:8524; 4058300.04 north; 591823.36 east 2021-02-12T23:58:27Z- LcmTrn::handleMessages() - handling 42 messages took 0.63 ms and 130 calls... 2021-02-12T23:58:27Z- waiting for 1613174305.84, time is 1613174307.69 2021-02-12T23:58:27Z- LcmTrn::updateTrn() equating pose 1613174307.69 and meas times 1613174307.69 2021-02-12T23:58:27Z- LcmTrn::updateTrn() >>>> pose time 1613174307.69, meas time 1613174307.69 <<<< 2021-02-12T23:58:27Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174307.69, ping # 0. 2021-02-12T23:58:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174307.69 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:58:27Z- TerrainNav::motionUpdate() - currEstimate = (4058300, 591823.36, 5.66) 2021-02-12T23:58:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:58:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:58:27Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:58:27Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:58:27Z- TNavBF::Projecting Measurements 2021-02-12T23:58:27Z- TNavFilter::Measurement from time = 1613174307.69 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:58:27Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:58:27Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:58:27Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:58:27Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:58:27Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:58:27Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:58:27Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:58:28Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 725.4 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-12T23:58:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:58:28Z- TNavBF::mmse selected from filter -1 2021-02-12T23:58:28Z- variances([0], [2], and psi_berg) = 725.418284 833.013307 0.000000 2021-02-12T23:58:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:58:28Z- LcmTrn::publishEstimates() - MLE : 4058294.54 591776.86 5.66 2021-02-12T23:58:28Z- LcmTrn::publishEstimates() - MMSE : 4058300.04 591823.36 5.66 2021-02-12T23:58:28Z- LcmTrn::publishEstimates() - COVAR: 725.42 833.01 1.36 0.00 2021-02-12T23:58:28Z- AHRS_M2 msg: 1613174308.07 epoch sec; seqNo:8455 2021-02-12T23:58:28Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.22 psi 2021-02-12T23:58:28Z- handleDepth()-Depth_Keller msg: 1613174308072 epoch sec; seqNo:8525; depth 5.80 2021-02-12T23:58:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174308.08 epoch sec; seqNo:8525; 4058300.42 north; 591823.36 east 2021-02-12T23:58:28Z- AHRS_M2 msg: 1613174308.47 epoch sec; seqNo:8456 2021-02-12T23:58:28Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.22 psi 2021-02-12T23:58:28Z- handleDepth()-Depth_Keller msg: 1613174308478 epoch sec; seqNo:8526; depth 5.92 2021-02-12T23:58:28Z- handleDvl() - RDI_Pathfinder msg: 1613174308.49 epoch sec; seqNo:4062 2021-02-12T23:58:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.51 , 115.36 , 84.12 , 128.74 2021-02-12T23:58:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.19 , -0.15 2021-02-12T23:58:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174308.50 epoch sec; seqNo:8526; 4058300.80 north; 591823.36 east 2021-02-12T23:58:28Z- AHRS_M2 msg: 1613174308.88 epoch sec; seqNo:8457 2021-02-12T23:58:28Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.22 psi 2021-02-12T23:58:28Z- handleDepth()-Depth_Keller msg: 1613174308883 epoch sec; seqNo:8527; depth 6.06 2021-02-12T23:58:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:58:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:58:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:58:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:58:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:58:28Z- handleDvl() - RDI_Pathfinder msg: 1613174308.88 epoch sec; seqNo:4063 2021-02-12T23:58:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.19 , -0.15 2021-02-12T23:58:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174308.90 epoch sec; seqNo:8527; 4058300.80 north; 591823.36 east 2021-02-12T23:58:29Z- AHRS_M2 msg: 1613174309.28 epoch sec; seqNo:8458 2021-02-12T23:58:29Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.22 psi 2021-02-12T23:58:29Z- handleDepth()-Depth_Keller msg: 1613174309286 epoch sec; seqNo:8528; depth 6.17 2021-02-12T23:58:29Z- handleDvl() - RDI_Pathfinder msg: 1613174309.30 epoch sec; seqNo:4064 2021-02-12T23:58:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.58 , 113.94 , 84.19 , 122.87 2021-02-12T23:58:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.17 , 0.01 2021-02-12T23:58:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174309.32 epoch sec; seqNo:8528; 4058301.18 north; 591823.35 east 2021-02-12T23:58:29Z- AHRS_M2 msg: 1613174309.68 epoch sec; seqNo:8459 2021-02-12T23:58:29Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.22 psi 2021-02-12T23:58:29Z- handleDepth()-Depth_Keller msg: 1613174309688 epoch sec; seqNo:8529; depth 6.32 2021-02-12T23:58:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174309.70 epoch sec; seqNo:8529; 4058301.56 north; 591823.35 east 2021-02-12T23:58:30Z- AHRS_M2 msg: 1613174310.09 epoch sec; seqNo:8460 2021-02-12T23:58:30Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.22 psi 2021-02-12T23:58:30Z- handleDepth()-Depth_Keller msg: 1613174310092 epoch sec; seqNo:8530; depth 6.43 2021-02-12T23:58:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174310.10 epoch sec; seqNo:8530; 4058301.56 north; 591823.35 east 2021-02-12T23:58:30Z- AHRS_M2 msg: 1613174310.49 epoch sec; seqNo:8461 2021-02-12T23:58:30Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.21 psi 2021-02-12T23:58:30Z- handleDepth()-Depth_Keller msg: 1613174310498 epoch sec; seqNo:8531; depth 6.57 2021-02-12T23:58:30Z- handleDvl() - RDI_Pathfinder msg: 1613174310.51 epoch sec; seqNo:4065 2021-02-12T23:58:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.28 , 114.08 , 0.00 , 127.47 2021-02-12T23:58:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.04 , 0.19 2021-02-12T23:58:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174310.52 epoch sec; seqNo:8531; 4058301.93 north; 591823.34 east 2021-02-12T23:58:30Z- AHRS_M2 msg: 1613174310.90 epoch sec; seqNo:8462 2021-02-12T23:58:30Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.21 psi 2021-02-12T23:58:30Z- handleDepth()-Depth_Keller msg: 1613174310900 epoch sec; seqNo:8532; depth 6.70 2021-02-12T23:58:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174310.92 epoch sec; seqNo:8532; 4058301.93 north; 591823.34 east 2021-02-12T23:58:31Z- AHRS_M2 msg: 1613174311.31 epoch sec; seqNo:8463 2021-02-12T23:58:31Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.21 psi 2021-02-12T23:58:31Z- handleDepth()-Depth_Keller msg: 1613174311315 epoch sec; seqNo:8533; depth 6.82 2021-02-12T23:58:31Z- handleDvl() - RDI_Pathfinder msg: 1613174311.33 epoch sec; seqNo:4066 2021-02-12T23:58:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.80 , 112.12 , 82.37 , 126.99 2021-02-12T23:58:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.10 , 0.12 2021-02-12T23:58:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174311.34 epoch sec; seqNo:8533; 4058302.33 north; 591824.56 east 2021-02-12T23:58:31Z- AHRS_M2 msg: 1613174311.71 epoch sec; seqNo:8464 2021-02-12T23:58:31Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.21 psi 2021-02-12T23:58:31Z- handleDepth()-Depth_Keller msg: 1613174311724 epoch sec; seqNo:8534; depth 6.96 2021-02-12T23:58:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174311.73 epoch sec; seqNo:8534; 4058302.33 north; 591824.56 east 2021-02-12T23:58:32Z- AHRS_M2 msg: 1613174312.11 epoch sec; seqNo:8465 2021-02-12T23:58:32Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.21 psi 2021-02-12T23:58:32Z- handleDepth()-Depth_Keller msg: 1613174312112 epoch sec; seqNo:8535; depth 7.11 2021-02-12T23:58:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174312.13 epoch sec; seqNo:8535; 4058302.71 north; 591824.56 east 2021-02-12T23:58:32Z- AHRS_M2 msg: 1613174312.51 epoch sec; seqNo:8466 2021-02-12T23:58:32Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.21 psi 2021-02-12T23:58:32Z- handleDepth()-Depth_Keller msg: 1613174312516 epoch sec; seqNo:8536; depth 7.22 2021-02-12T23:58:32Z- handleDvl() - RDI_Pathfinder msg: 1613174312.53 epoch sec; seqNo:4067 2021-02-12T23:58:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 112.73 , 0.00 , 126.11 2021-02-12T23:58:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.09 , -0.01 2021-02-12T23:58:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174312.55 epoch sec; seqNo:8536; 4058303.08 north; 591824.55 east 2021-02-12T23:58:32Z- AHRS_M2 msg: 1613174312.92 epoch sec; seqNo:8467 2021-02-12T23:58:32Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.21 psi 2021-02-12T23:58:32Z- handleDepth()-Depth_Keller msg: 1613174312920 epoch sec; seqNo:8537; depth 7.35 2021-02-12T23:58:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174312.93 epoch sec; seqNo:8537; 4058303.08 north; 591824.55 east 2021-02-12T23:58:33Z- LcmTrn::handleMessages() - handling 45 messages took 0.67 ms and 132 calls... 2021-02-12T23:58:33Z- waiting for 1613174310.69, time is 1613174312.93 2021-02-12T23:58:33Z- LcmTrn::updateTrn() >>>> pose time 1613174312.93, meas time 1613174312.53 <<<< 2021-02-12T23:58:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174312.53 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:58:33Z- TerrainNav::motionUpdate() - currEstimate = (4058303, 591824.55, 7.35) 2021-02-12T23:58:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:58:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:58:33Z- TNavBF::Projecting Measurements 2021-02-12T23:58:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:58:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:58:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:58:33Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:58:33Z- TNavFilter::3 good beams. 2021-02-12T23:58:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:58:33Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:58:33Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:58:33Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:58:33Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:58:33Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:58:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:58:33Z- TNavBF::mmse selected from filter -1 2021-02-12T23:58:33Z- TerrainMapOctree: (vehN, vehE ) = (4058302.85, 591824.46). 2021-02-12T23:58:33Z- TerrainMapOctree: Min Distance = 3816.93. 2021-02-12T23:58:33Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:58:33Z- There is no specific good beam that all particles have in common. 2021-02-12T23:58:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:58:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.51 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 12.65 2021-02-12T23:58:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:58:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 20.33 2021-02-12T23:58:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:58:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 25.45 2021-02-12T23:58:33Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:58:33Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:58:33Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:58:33Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:58:33Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:58:33Z- Filter 0 No beams used in subcloud update 2021-02-12T23:58:33Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:58:33Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:58:33Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 725.4 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:58:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174312.53 sec. 2021-02-12T23:58:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:58:33Z- TNavBF::mmse selected from filter -1 2021-02-12T23:58:33Z- variances([0], [2], and psi_berg) = 725.337929 833.013251 0.000000 2021-02-12T23:58:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:58:33Z- LcmTrn::publishEstimates() - MLE : 4058297.58 591778.05 7.35 2021-02-12T23:58:33Z- LcmTrn::publishEstimates() - MMSE : 4058303.08 591824.55 7.35 2021-02-12T23:58:33Z- LcmTrn::publishEstimates() - COVAR: 725.34 833.01 1.24 0.00 2021-02-12T23:58:33Z- AHRS_M2 msg: 1613174313.32 epoch sec; seqNo:8468 2021-02-12T23:58:33Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.19 psi 2021-02-12T23:58:33Z- handleDepth()-Depth_Keller msg: 1613174313325 epoch sec; seqNo:8538; depth 7.47 2021-02-12T23:58:33Z- handleDvl() - RDI_Pathfinder msg: 1613174313.34 epoch sec; seqNo:4068 2021-02-12T23:58:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.38 , 112.18 , 0.00 , 119.62 2021-02-12T23:58:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.04 , 0.04 2021-02-12T23:58:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174313.35 epoch sec; seqNo:8538; 4058303.46 north; 591824.55 east 2021-02-12T23:58:33Z- AHRS_M2 msg: 1613174313.72 epoch sec; seqNo:8469 2021-02-12T23:58:33Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.17 psi 2021-02-12T23:58:33Z- handleDepth()-Depth_Keller msg: 1613174313728 epoch sec; seqNo:8539; depth 7.60 2021-02-12T23:58:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174313.75 epoch sec; seqNo:8539; 4058303.86 north; 591825.76 east 2021-02-12T23:58:34Z- AHRS_M2 msg: 1613174314.13 epoch sec; seqNo:8470 2021-02-12T23:58:34Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.16 psi 2021-02-12T23:58:34Z- handleDepth()-Depth_Keller msg: 1613174314137 epoch sec; seqNo:8540; depth 7.75 2021-02-12T23:58:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174314.15 epoch sec; seqNo:8540; 4058304.23 north; 591825.76 east 2021-02-12T23:58:34Z- AHRS_M2 msg: 1613174314.53 epoch sec; seqNo:8471 2021-02-12T23:58:34Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.14 psi 2021-02-12T23:58:34Z- handleDepth()-Depth_Keller msg: 1613174314536 epoch sec; seqNo:8541; depth 7.85 2021-02-12T23:58:34Z- handleDvl() - RDI_Pathfinder msg: 1613174314.55 epoch sec; seqNo:4069 2021-02-12T23:58:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.51 , 106.43 , 82.63 , 115.36 2021-02-12T23:58:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.13 , 0.08 2021-02-12T23:58:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174314.56 epoch sec; seqNo:8541; 4058304.23 north; 591825.76 east 2021-02-12T23:58:34Z- AHRS_M2 msg: 1613174314.94 epoch sec; seqNo:8472 2021-02-12T23:58:34Z- AHRS_M2 msg: 0.61 phi; -0.28 theta; -0.13 psi 2021-02-12T23:58:34Z- handleDepth()-Depth_Keller msg: 1613174314949 epoch sec; seqNo:8542; depth 7.96 2021-02-12T23:58:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174314.96 epoch sec; seqNo:8542; 4058304.61 north; 591825.76 east 2021-02-12T23:58:35Z- AHRS_M2 msg: 1613174315.35 epoch sec; seqNo:8473 2021-02-12T23:58:35Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.13 psi 2021-02-12T23:58:35Z- handleDepth()-Depth_Keller msg: 1613174315357 epoch sec; seqNo:8543; depth 8.07 2021-02-12T23:58:35Z- handleDvl() - RDI_Pathfinder msg: 1613174315.37 epoch sec; seqNo:4070 2021-02-12T23:58:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 102.38 , 82.09 , 119.95 2021-02-12T23:58:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.24 , -0.07 2021-02-12T23:58:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174315.38 epoch sec; seqNo:8543; 4058304.61 north; 591825.76 east 2021-02-12T23:58:35Z- AHRS_M2 msg: 1613174315.74 epoch sec; seqNo:8474 2021-02-12T23:58:35Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.14 psi 2021-02-12T23:58:35Z- handleDepth()-Depth_Keller msg: 1613174315754 epoch sec; seqNo:8544; depth 8.17 2021-02-12T23:58:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174315.77 epoch sec; seqNo:8544; 4058305.00 north; 591826.97 east 2021-02-12T23:58:36Z- AHRS_M2 msg: 1613174316.15 epoch sec; seqNo:8475 2021-02-12T23:58:36Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.15 psi 2021-02-12T23:58:36Z- handleDepth()-Depth_Keller msg: 1613174316161 epoch sec; seqNo:8545; depth 8.26 2021-02-12T23:58:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174316.17 epoch sec; seqNo:8545; 4058305.38 north; 591826.97 east 2021-02-12T23:58:36Z- AHRS_M2 msg: 1613174316.55 epoch sec; seqNo:8476 2021-02-12T23:58:36Z- AHRS_M2 msg: 0.60 phi; -0.27 theta; -0.17 psi 2021-02-12T23:58:36Z- handleDepth()-Depth_Keller msg: 1613174316559 epoch sec; seqNo:8546; depth 8.36 2021-02-12T23:58:36Z- handleDvl() - RDI_Pathfinder msg: 1613174316.57 epoch sec; seqNo:4071 2021-02-12T23:58:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 102.38 , 79.29 , 114.54 2021-02-12T23:58:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.09 , -0.19 2021-02-12T23:58:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174316.59 epoch sec; seqNo:8546; 4058305.38 north; 591826.97 east 2021-02-12T23:58:36Z- AHRS_M2 msg: 1613174316.96 epoch sec; seqNo:8477 2021-02-12T23:58:36Z- AHRS_M2 msg: 0.59 phi; -0.27 theta; -0.18 psi 2021-02-12T23:58:36Z- handleDepth()-Depth_Keller msg: 1613174316960 epoch sec; seqNo:8547; depth 8.48 2021-02-12T23:58:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174316.97 epoch sec; seqNo:8547; 4058305.76 north; 591826.96 east 2021-02-12T23:58:37Z- AHRS_M2 msg: 1613174317.36 epoch sec; seqNo:8478 2021-02-12T23:58:37Z- AHRS_M2 msg: 0.59 phi; -0.26 theta; -0.19 psi 2021-02-12T23:58:37Z- handleDepth()-Depth_Keller msg: 1613174317366 epoch sec; seqNo:8548; depth 8.56 2021-02-12T23:58:37Z- handleDvl() - RDI_Pathfinder msg: 1613174317.38 epoch sec; seqNo:4072 2021-02-12T23:58:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.28 , 82.30 , 113.39 2021-02-12T23:58:37Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.04 , -0.15 2021-02-12T23:58:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174317.39 epoch sec; seqNo:8548; 4058306.14 north; 591826.96 east 2021-02-12T23:58:37Z- AHRS_M2 msg: 1613174317.76 epoch sec; seqNo:8479 2021-02-12T23:58:37Z- AHRS_M2 msg: 0.59 phi; -0.26 theta; -0.19 psi 2021-02-12T23:58:37Z- handleDepth()-Depth_Keller msg: 1613174317770 epoch sec; seqNo:8549; depth 8.68 2021-02-12T23:58:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174317.79 epoch sec; seqNo:8549; 4058306.53 north; 591828.18 east 2021-02-12T23:58:38Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-12T23:58:38Z- waiting for 1613174315.93, time is 1613174317.79 2021-02-12T23:58:38Z- LcmTrn::updateTrn() >>>> pose time 1613174317.79, meas time 1613174317.38 <<<< 2021-02-12T23:58:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174317.38 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:58:38Z- TerrainNav::motionUpdate() - currEstimate = (4058307, 591828.18, 8.68) 2021-02-12T23:58:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:58:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:58:38Z- TNavBF::Projecting Measurements 2021-02-12T23:58:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:58:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:58:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:58:38Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:58:38Z- TNavFilter::3 good beams. 2021-02-12T23:58:38Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:58:38Z- TNavBF:: beamIndex[0] = 1 2021-02-12T23:58:38Z- TNavBF:: beamIndex[1] = 2 2021-02-12T23:58:38Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:58:38Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:58:38Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:58:38Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:58:38Z- TNavBF::mmse selected from filter -1 2021-02-12T23:58:38Z- TerrainMapOctree: (vehN, vehE ) = (4058306.24, 591827.87). 2021-02-12T23:58:38Z- TerrainMapOctree: Min Distance = 3812.14. 2021-02-12T23:58:38Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:58:38Z- There is no specific good beam that all particles have in common. 2021-02-12T23:58:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:58:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.48 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 17.73 2021-02-12T23:58:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:58:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 10.84 2021-02-12T23:58:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:58:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 20.57 2021-02-12T23:58:38Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:58:38Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:58:38Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:58:38Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:58:38Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:58:38Z- Filter 0 No beams used in subcloud update 2021-02-12T23:58:38Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:58:38Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:58:38Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 725.3 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:58:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174317.38 sec. 2021-02-12T23:58:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:58:38Z- TNavBF::mmse selected from filter -1 2021-02-12T23:58:38Z- variances([0], [2], and psi_berg) = 725.237854 833.013182 0.000000 2021-02-12T23:58:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:58:38Z- LcmTrn::publishEstimates() - MLE : 4058301.03 591781.68 8.68 2021-02-12T23:58:38Z- LcmTrn::publishEstimates() - MMSE : 4058306.53 591828.18 8.68 2021-02-12T23:58:38Z- LcmTrn::publishEstimates() - COVAR: 725.24 833.01 1.19 0.00 2021-02-12T23:58:38Z- AHRS_M2 msg: 1613174318.17 epoch sec; seqNo:8480 2021-02-12T23:58:38Z- AHRS_M2 msg: 0.60 phi; -0.26 theta; -0.19 psi 2021-02-12T23:58:38Z- handleDepth()-Depth_Keller msg: 1613174318177 epoch sec; seqNo:8550; depth 8.77 2021-02-12T23:58:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174318.18 epoch sec; seqNo:8550; 4058306.91 north; 591828.17 east 2021-02-12T23:58:38Z- AHRS_M2 msg: 1613174318.57 epoch sec; seqNo:8481 2021-02-12T23:58:38Z- AHRS_M2 msg: 0.60 phi; -0.26 theta; -0.18 psi 2021-02-12T23:58:38Z- handleDepth()-Depth_Keller msg: 1613174318578 epoch sec; seqNo:8551; depth 8.89 2021-02-12T23:58:38Z- handleDvl() - RDI_Pathfinder msg: 1613174318.60 epoch sec; seqNo:4073 2021-02-12T23:58:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.15 , 106.43 , 0.00 , 114.54 2021-02-12T23:58:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.08 , -0.14 2021-02-12T23:58:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174318.61 epoch sec; seqNo:8551; 4058307.29 north; 591828.17 east 2021-02-12T23:58:38Z- AHRS_M2 msg: 1613174318.98 epoch sec; seqNo:8482 2021-02-12T23:58:38Z- AHRS_M2 msg: 0.61 phi; -0.26 theta; -0.17 psi 2021-02-12T23:58:38Z- handleDepth()-Depth_Keller msg: 1613174318980 epoch sec; seqNo:8552; depth 9.00 2021-02-12T23:58:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174319.00 epoch sec; seqNo:8552; 4058307.67 north; 591828.17 east 2021-02-12T23:58:39Z- AHRS_M2 msg: 1613174319.39 epoch sec; seqNo:8483 2021-02-12T23:58:39Z- AHRS_M2 msg: 0.63 phi; -0.26 theta; -0.16 psi 2021-02-12T23:58:39Z- handleDepth()-Depth_Keller msg: 1613174319390 epoch sec; seqNo:8553; depth 9.10 2021-02-12T23:58:39Z- handleDvl() - RDI_Pathfinder msg: 1613174319.41 epoch sec; seqNo:4074 2021-02-12T23:58:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.02 , 103.87 , 81.56 , 109.82 2021-02-12T23:58:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.17 , -0.20 2021-02-12T23:58:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174319.43 epoch sec; seqNo:8553; 4058308.05 north; 591828.16 east 2021-02-12T23:58:39Z- AHRS_M2 msg: 1613174319.79 epoch sec; seqNo:8484 2021-02-12T23:58:39Z- AHRS_M2 msg: 0.63 phi; -0.26 theta; -0.16 psi 2021-02-12T23:58:39Z- handleDepth()-Depth_Keller msg: 1613174319795 epoch sec; seqNo:8554; depth 9.19 2021-02-12T23:58:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:58:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:58:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:58:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:58:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:58:39Z- handleDvl() - RDI_Pathfinder msg: 1613174319.80 epoch sec; seqNo:4075 2021-02-12T23:58:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.17 , -0.20 2021-02-12T23:58:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174319.81 epoch sec; seqNo:8554; 4058308.43 north; 591828.16 east 2021-02-12T23:58:40Z- AHRS_M2 msg: 1613174320.21 epoch sec; seqNo:8485 2021-02-12T23:58:40Z- AHRS_M2 msg: 0.64 phi; -0.27 theta; -0.16 psi 2021-02-12T23:58:40Z- handleDepth()-Depth_Keller msg: 1613174320214 epoch sec; seqNo:8555; depth 9.28 2021-02-12T23:58:40Z- handleDvl() - RDI_Pathfinder msg: 1613174320.23 epoch sec; seqNo:4076 2021-02-12T23:58:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.85 , 81.22 , 109.62 2021-02-12T23:58:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.30 , -0.08 2021-02-12T23:58:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174320.27 epoch sec; seqNo:8555; 4058308.81 north; 591828.15 east 2021-02-12T23:58:40Z- AHRS_M2 msg: 1613174320.59 epoch sec; seqNo:8486 2021-02-12T23:58:40Z- AHRS_M2 msg: 0.64 phi; -0.27 theta; -0.15 psi 2021-02-12T23:58:40Z- handleDepth()-Depth_Keller msg: 1613174320596 epoch sec; seqNo:8556; depth 9.38 2021-02-12T23:58:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174320.61 epoch sec; seqNo:8556; 4058308.81 north; 591828.15 east 2021-02-12T23:58:41Z- AHRS_M2 msg: 1613174321.01 epoch sec; seqNo:8487 2021-02-12T23:58:41Z- AHRS_M2 msg: 0.64 phi; -0.28 theta; -0.14 psi 2021-02-12T23:58:41Z- handleDepth()-Depth_Keller msg: 1613174321011 epoch sec; seqNo:8557; depth 9.47 2021-02-12T23:58:41Z- handleDvl() - RDI_Pathfinder msg: 1613174321.03 epoch sec; seqNo:4077 2021-02-12T23:58:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:41Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:58:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174321.04 epoch sec; seqNo:8557; 4058309.20 north; 591829.37 east 2021-02-12T23:58:41Z- AHRS_M2 msg: 1613174321.40 epoch sec; seqNo:8488 2021-02-12T23:58:41Z- AHRS_M2 msg: 0.64 phi; -0.29 theta; -0.13 psi 2021-02-12T23:58:41Z- handleDepth()-Depth_Keller msg: 1613174321404 epoch sec; seqNo:8558; depth 9.58 2021-02-12T23:58:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174321.42 epoch sec; seqNo:8558; 4058309.58 north; 591829.37 east 2021-02-12T23:58:41Z- AHRS_M2 msg: 1613174321.81 epoch sec; seqNo:8489 2021-02-12T23:58:41Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.13 psi 2021-02-12T23:58:41Z- handleDepth()-Depth_Keller msg: 1613174321816 epoch sec; seqNo:8559; depth 9.66 2021-02-12T23:58:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174321.83 epoch sec; seqNo:8559; 4058309.96 north; 591829.36 east 2021-02-12T23:58:42Z- AHRS_M2 msg: 1613174322.21 epoch sec; seqNo:8490 2021-02-12T23:58:42Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.12 psi 2021-02-12T23:58:42Z- handleDepth()-Depth_Keller msg: 1613174322218 epoch sec; seqNo:8560; depth 9.75 2021-02-12T23:58:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:58:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:58:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:58:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:58:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:58:42Z- handleDvl() - RDI_Pathfinder msg: 1613174322.23 epoch sec; seqNo:4078 2021-02-12T23:58:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:42Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:58:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174322.26 epoch sec; seqNo:8560; 4058310.33 north; 591829.36 east 2021-02-12T23:58:42Z- AHRS_M2 msg: 1613174322.61 epoch sec; seqNo:8491 2021-02-12T23:58:42Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.13 psi 2021-02-12T23:58:42Z- handleDepth()-Depth_Keller msg: 1613174322621 epoch sec; seqNo:8561; depth 9.86 2021-02-12T23:58:42Z- handleDvl() - RDI_Pathfinder msg: 1613174322.63 epoch sec; seqNo:4079 2021-02-12T23:58:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.77 , 101.29 , 0.00 , 109.41 2021-02-12T23:58:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , 0.01 , 0.35 2021-02-12T23:58:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174322.66 epoch sec; seqNo:8561; 4058310.33 north; 591829.36 east 2021-02-12T23:58:43Z- AHRS_M2 msg: 1613174323.02 epoch sec; seqNo:8492 2021-02-12T23:58:43Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.14 psi 2021-02-12T23:58:43Z- handleDepth()-Depth_Keller msg: 1613174323020 epoch sec; seqNo:8562; depth 9.96 2021-02-12T23:58:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174323.03 epoch sec; seqNo:8562; 4058310.71 north; 591829.35 east 2021-02-12T23:58:43Z- LcmTrn::handleMessages() - handling 46 messages took 0.67 ms and 133 calls... 2021-02-12T23:58:43Z- waiting for 1613174320.79, time is 1613174323.03 2021-02-12T23:58:43Z- LcmTrn::updateTrn() >>>> pose time 1613174323.03, meas time 1613174322.63 <<<< 2021-02-12T23:58:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174322.63 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:58:43Z- TerrainNav::motionUpdate() - currEstimate = (4058311, 591829.35, 9.96) 2021-02-12T23:58:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:58:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:58:43Z- TNavBF::Projecting Measurements 2021-02-12T23:58:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:58:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:58:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:58:43Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-12T23:58:43Z- TNavFilter::3 good beams. 2021-02-12T23:58:43Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:58:43Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:58:43Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:58:43Z- TNavBF:: beamIndex[2] = 3 2021-02-12T23:58:43Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:58:43Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-12T23:58:43Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:58:43Z- TNavBF::mmse selected from filter -1 2021-02-12T23:58:43Z- TerrainMapOctree: (vehN, vehE ) = (4058310.40, 591829.26). 2021-02-12T23:58:43Z- TerrainMapOctree: Min Distance = 3808.05. 2021-02-12T23:58:43Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:58:43Z- There is no specific good beam that all particles have in common. 2021-02-12T23:58:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:58:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 12.33 2021-02-12T23:58:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:58:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 16.42 2021-02-12T23:58:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:58:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 19.15 2021-02-12T23:58:43Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:58:43Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:58:43Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:58:43Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:58:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:58:43Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:58:43Z- Filter 0 No beams used in subcloud update 2021-02-12T23:58:43Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:58:43Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:58:43Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 725.2 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:58:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174322.63 sec. 2021-02-12T23:58:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:58:43Z- TNavBF::mmse selected from filter -1 2021-02-12T23:58:43Z- variances([0], [2], and psi_berg) = 725.145544 833.013118 0.000000 2021-02-12T23:58:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:58:43Z- LcmTrn::publishEstimates() - MLE : 4058305.21 591874.85 9.96 2021-02-12T23:58:43Z- LcmTrn::publishEstimates() - MMSE : 4058310.71 591829.35 9.96 2021-02-12T23:58:43Z- LcmTrn::publishEstimates() - COVAR: 725.15 833.01 1.14 0.00 2021-02-12T23:58:43Z- AHRS_M2 msg: 1613174323.42 epoch sec; seqNo:8493 2021-02-12T23:58:43Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.15 psi 2021-02-12T23:58:43Z- handleDepth()-Depth_Keller msg: 1613174323424 epoch sec; seqNo:8563; depth 10.04 2021-02-12T23:58:43Z- handleDvl() - RDI_Pathfinder msg: 1613174323.44 epoch sec; seqNo:4080 2021-02-12T23:58:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 101.16 , 79.53 , 116.04 2021-02-12T23:58:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.27 , 0.25 2021-02-12T23:58:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174323.45 epoch sec; seqNo:8563; 4058310.71 north; 591829.35 east 2021-02-12T23:58:43Z- AHRS_M2 msg: 1613174323.83 epoch sec; seqNo:8494 2021-02-12T23:58:43Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.17 psi 2021-02-12T23:58:43Z- handleDepth()-Depth_Keller msg: 1613174323829 epoch sec; seqNo:8564; depth 10.14 2021-02-12T23:58:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174323.85 epoch sec; seqNo:8564; 4058311.09 north; 591829.35 east 2021-02-12T23:58:44Z- AHRS_M2 msg: 1613174324.23 epoch sec; seqNo:8495 2021-02-12T23:58:44Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.19 psi 2021-02-12T23:58:44Z- handleDepth()-Depth_Keller msg: 1613174324232 epoch sec; seqNo:8565; depth 10.22 2021-02-12T23:58:44Z- handleDvl() - RDI_Pathfinder msg: 1613174324.24 epoch sec; seqNo:4081 2021-02-12T23:58:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:44Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:58:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174324.25 epoch sec; seqNo:8565; 4058311.11 north; 591830.57 east 2021-02-12T23:58:44Z- AHRS_M2 msg: 1613174324.63 epoch sec; seqNo:8496 2021-02-12T23:58:44Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.20 psi 2021-02-12T23:58:44Z- handleDepth()-Depth_Keller msg: 1613174324645 epoch sec; seqNo:8566; depth 10.33 2021-02-12T23:58:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:58:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:58:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:58:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:58:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:58:44Z- handleDvl() - RDI_Pathfinder msg: 1613174324.65 epoch sec; seqNo:4082 2021-02-12T23:58:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:44Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:58:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174324.66 epoch sec; seqNo:8566; 4058311.48 north; 591830.57 east 2021-02-12T23:58:45Z- AHRS_M2 msg: 1613174325.04 epoch sec; seqNo:8497 2021-02-12T23:58:45Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.21 psi 2021-02-12T23:58:45Z- handleDepth()-Depth_Keller msg: 1613174325050 epoch sec; seqNo:8567; depth 10.45 2021-02-12T23:58:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174325.06 epoch sec; seqNo:8567; 4058311.48 north; 591830.57 east 2021-02-12T23:58:45Z- AHRS_M2 msg: 1613174325.44 epoch sec; seqNo:8498 2021-02-12T23:58:45Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.21 psi 2021-02-12T23:58:45Z- handleDepth()-Depth_Keller msg: 1613174325449 epoch sec; seqNo:8568; depth 10.57 2021-02-12T23:58:45Z- handleDvl() - RDI_Pathfinder msg: 1613174325.47 epoch sec; seqNo:4083 2021-02-12T23:58:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 108.12 , 81.08 , 114.88 2021-02-12T23:58:45Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.08 , -0.14 2021-02-12T23:58:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174325.48 epoch sec; seqNo:8568; 4058311.86 north; 591830.56 east 2021-02-12T23:58:45Z- AHRS_M2 msg: 1613174325.84 epoch sec; seqNo:8499 2021-02-12T23:58:45Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.21 psi 2021-02-12T23:58:45Z- handleDepth()-Depth_Keller msg: 1613174325853 epoch sec; seqNo:8569; depth 10.68 2021-02-12T23:58:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:58:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:58:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:58:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:58:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:58:45Z- handleDvl() - RDI_Pathfinder msg: 1613174325.86 epoch sec; seqNo:4084 2021-02-12T23:58:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:45Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.08 , -0.14 2021-02-12T23:58:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174325.87 epoch sec; seqNo:8569; 4058312.26 north; 591831.78 east 2021-02-12T23:58:46Z- AHRS_M2 msg: 1613174326.25 epoch sec; seqNo:8500 2021-02-12T23:58:46Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.20 psi 2021-02-12T23:58:46Z- handleDepth()-Depth_Keller msg: 1613174326254 epoch sec; seqNo:8570; depth 10.79 2021-02-12T23:58:46Z- handleDvl() - RDI_Pathfinder msg: 1613174326.27 epoch sec; seqNo:4085 2021-02-12T23:58:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 108.39 , 80.13 , 114.34 2021-02-12T23:58:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.13 , -0.16 2021-02-12T23:58:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174326.28 epoch sec; seqNo:8570; 4058312.63 north; 591831.78 east 2021-02-12T23:58:46Z- AHRS_M2 msg: 1613174326.65 epoch sec; seqNo:8501 2021-02-12T23:58:46Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.19 psi 2021-02-12T23:58:46Z- handleDepth()-Depth_Keller msg: 1613174326658 epoch sec; seqNo:8571; depth 10.91 2021-02-12T23:58:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174326.67 epoch sec; seqNo:8571; 4058313.01 north; 591831.77 east 2021-02-12T23:58:47Z- AHRS_M2 msg: 1613174327.06 epoch sec; seqNo:8502 2021-02-12T23:58:47Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.18 psi 2021-02-12T23:58:47Z- handleDepth()-Depth_Keller msg: 1613174327065 epoch sec; seqNo:8572; depth 11.02 2021-02-12T23:58:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:58:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:58:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:58:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:58:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:58:47Z- handleDvl() - RDI_Pathfinder msg: 1613174327.07 epoch sec; seqNo:4086 2021-02-12T23:58:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.13 , -0.16 2021-02-12T23:58:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174327.08 epoch sec; seqNo:8572; 4058313.01 north; 591831.77 east 2021-02-12T23:58:47Z- AHRS_M2 msg: 1613174327.46 epoch sec; seqNo:8503 2021-02-12T23:58:47Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-12T23:58:47Z- handleDepth()-Depth_Keller msg: 1613174327469 epoch sec; seqNo:8573; depth 11.13 2021-02-12T23:58:47Z- handleDvl() - RDI_Pathfinder msg: 1613174327.48 epoch sec; seqNo:4087 2021-02-12T23:58:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.80 , 103.73 , 76.02 , 111.84 2021-02-12T23:58:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.14 , -0.10 2021-02-12T23:58:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174327.50 epoch sec; seqNo:8573; 4058313.39 north; 591831.77 east 2021-02-12T23:58:47Z- AHRS_M2 msg: 1613174327.87 epoch sec; seqNo:8504 2021-02-12T23:58:47Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-12T23:58:47Z- handleDepth()-Depth_Keller msg: 1613174327869 epoch sec; seqNo:8574; depth 11.24 2021-02-12T23:58:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174327.88 epoch sec; seqNo:8574; 4058313.77 north; 591831.76 east 2021-02-12T23:58:48Z- AHRS_M2 msg: 1613174328.28 epoch sec; seqNo:8505 2021-02-12T23:58:48Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.16 psi 2021-02-12T23:58:48Z- handleDepth()-Depth_Keller msg: 1613174328287 epoch sec; seqNo:8575; depth 11.34 2021-02-12T23:58:48Z- handleDvl() - RDI_Pathfinder msg: 1613174328.30 epoch sec; seqNo:4088 2021-02-12T23:58:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 102.92 , 78.58 , 115.09 2021-02-12T23:58:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.09 , 0.05 2021-02-12T23:58:48Z- LcmTrn::handleMessages() - handling 47 messages took 0.65 ms and 134 calls... 2021-02-12T23:58:48Z- waiting for 1613174326.03, time is 1613174328.30 2021-02-12T23:58:48Z- LcmTrn::updateTrn() equating pose 1613174328.30 and meas times 1613174328.30 2021-02-12T23:58:48Z- LcmTrn::updateTrn() >>>> pose time 1613174328.30, meas time 1613174328.30 <<<< 2021-02-12T23:58:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174328.30 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:58:48Z- TerrainNav::motionUpdate() - currEstimate = (4058314, 591831.76, 11.34) 2021-02-12T23:58:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:58:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:58:48Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174328.30 sec.; No relevant navigation data available 2021-02-12T23:58:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:58:48Z- TNavBF::mmse selected from filter -1 2021-02-12T23:58:48Z- variances([0], [2], and psi_berg) = 725.074948 833.013070 0.000000 2021-02-12T23:58:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:58:48Z- LcmTrn::publishEstimates() - MLE : 4058308.27 591877.26 11.34 2021-02-12T23:58:48Z- LcmTrn::publishEstimates() - MMSE : 4058313.77 591831.76 11.34 2021-02-12T23:58:48Z- LcmTrn::publishEstimates() - COVAR: 725.07 833.01 1.34 0.00 2021-02-12T23:58:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174328.31 epoch sec; seqNo:8575; 4058314.15 north; 591831.76 east 2021-02-12T23:58:48Z- AHRS_M2 msg: 1613174328.67 epoch sec; seqNo:8506 2021-02-12T23:58:48Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.15 psi 2021-02-12T23:58:48Z- handleDepth()-Depth_Keller msg: 1613174328676 epoch sec; seqNo:8576; depth 11.48 2021-02-12T23:58:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174328.69 epoch sec; seqNo:8576; 4058314.15 north; 591831.76 east 2021-02-12T23:58:49Z- AHRS_M2 msg: 1613174329.08 epoch sec; seqNo:8507 2021-02-12T23:58:49Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.16 psi 2021-02-12T23:58:49Z- handleDepth()-Depth_Keller msg: 1613174329080 epoch sec; seqNo:8577; depth 11.59 2021-02-12T23:58:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174329.09 epoch sec; seqNo:8577; 4058314.54 north; 591832.98 east 2021-02-12T23:58:49Z- AHRS_M2 msg: 1613174329.48 epoch sec; seqNo:8508 2021-02-12T23:58:49Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.15 psi 2021-02-12T23:58:49Z- handleDepth()-Depth_Keller msg: 1613174329489 epoch sec; seqNo:8578; depth 11.69 2021-02-12T23:58:49Z- handleDvl() - RDI_Pathfinder msg: 1613174329.50 epoch sec; seqNo:4089 2021-02-12T23:58:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 101.50 , 76.02 , 115.03 2021-02-12T23:58:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , 0.14 2021-02-12T23:58:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174329.51 epoch sec; seqNo:8578; 4058314.92 north; 591832.97 east 2021-02-12T23:58:49Z- AHRS_M2 msg: 1613174329.89 epoch sec; seqNo:8509 2021-02-12T23:58:49Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.16 psi 2021-02-12T23:58:49Z- handleDepth()-Depth_Keller msg: 1613174329895 epoch sec; seqNo:8579; depth 11.82 2021-02-12T23:58:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174329.91 epoch sec; seqNo:8579; 4058314.92 north; 591832.97 east 2021-02-12T23:58:50Z- AHRS_M2 msg: 1613174330.29 epoch sec; seqNo:8510 2021-02-12T23:58:50Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.17 psi 2021-02-12T23:58:50Z- handleDepth()-Depth_Keller msg: 1613174330298 epoch sec; seqNo:8580; depth 11.92 2021-02-12T23:58:50Z- handleDvl() - RDI_Pathfinder msg: 1613174330.31 epoch sec; seqNo:4090 2021-02-12T23:58:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.92 , 78.58 , 115.08 2021-02-12T23:58:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.12 , 0.02 2021-02-12T23:58:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174330.33 epoch sec; seqNo:8580; 4058315.30 north; 591832.97 east 2021-02-12T23:58:50Z- AHRS_M2 msg: 1613174330.70 epoch sec; seqNo:8511 2021-02-12T23:58:50Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.17 psi 2021-02-12T23:58:50Z- handleDepth()-Depth_Keller msg: 1613174330704 epoch sec; seqNo:8581; depth 12.04 2021-02-12T23:58:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:50Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:58:50Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:58:50Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:58:50Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:58:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:58:50Z- handleDvl() - RDI_Pathfinder msg: 1613174330.71 epoch sec; seqNo:4091 2021-02-12T23:58:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.12 , 0.02 2021-02-12T23:58:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174330.72 epoch sec; seqNo:8581; 4058315.68 north; 591832.96 east 2021-02-12T23:58:51Z- AHRS_M2 msg: 1613174331.10 epoch sec; seqNo:8512 2021-02-12T23:58:51Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-12T23:58:51Z- handleDepth()-Depth_Keller msg: 1613174331100 epoch sec; seqNo:8582; depth 12.17 2021-02-12T23:58:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174331.12 epoch sec; seqNo:8582; 4058316.07 north; 591834.18 east 2021-02-12T23:58:51Z- AHRS_M2 msg: 1613174331.50 epoch sec; seqNo:8513 2021-02-12T23:58:51Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-12T23:58:51Z- handleDepth()-Depth_Keller msg: 1613174331504 epoch sec; seqNo:8583; depth 12.27 2021-02-12T23:58:51Z- handleDvl() - RDI_Pathfinder msg: 1613174331.52 epoch sec; seqNo:4092 2021-02-12T23:58:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.28 , 103.86 , 73.97 , 116.03 2021-02-12T23:58:51Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.01 , -0.13 2021-02-12T23:58:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174331.55 epoch sec; seqNo:8583; 4058316.45 north; 591834.18 east 2021-02-12T23:58:51Z- AHRS_M2 msg: 1613174331.91 epoch sec; seqNo:8514 2021-02-12T23:58:51Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.20 psi 2021-02-12T23:58:51Z- handleDepth()-Depth_Keller msg: 1613174331917 epoch sec; seqNo:8584; depth 12.40 2021-02-12T23:58:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:58:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:58:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:58:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:58:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:58:51Z- handleDvl() - RDI_Pathfinder msg: 1613174331.92 epoch sec; seqNo:4093 2021-02-12T23:58:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:51Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.01 , -0.13 2021-02-12T23:58:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174331.93 epoch sec; seqNo:8584; 4058316.45 north; 591834.18 east 2021-02-12T23:58:52Z- AHRS_M2 msg: 1613174332.31 epoch sec; seqNo:8515 2021-02-12T23:58:52Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.20 psi 2021-02-12T23:58:52Z- handleDepth()-Depth_Keller msg: 1613174332314 epoch sec; seqNo:8585; depth 12.52 2021-02-12T23:58:52Z- handleDvl() - RDI_Pathfinder msg: 1613174332.33 epoch sec; seqNo:4094 2021-02-12T23:58:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 104.20 , 75.61 , 115.02 2021-02-12T23:58:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.17 , -0.12 2021-02-12T23:58:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174332.34 epoch sec; seqNo:8585; 4058316.83 north; 591834.17 east 2021-02-12T23:58:52Z- AHRS_M2 msg: 1613174332.71 epoch sec; seqNo:8516 2021-02-12T23:58:52Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.20 psi 2021-02-12T23:58:52Z- handleDepth()-Depth_Keller msg: 1613174332721 epoch sec; seqNo:8586; depth 12.64 2021-02-12T23:58:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174332.74 epoch sec; seqNo:8586; 4058317.21 north; 591834.17 east 2021-02-12T23:58:53Z- AHRS_M2 msg: 1613174333.12 epoch sec; seqNo:8517 2021-02-12T23:58:53Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.20 psi 2021-02-12T23:58:53Z- handleDepth()-Depth_Keller msg: 1613174333129 epoch sec; seqNo:8587; depth 12.76 2021-02-12T23:58:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:58:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:58:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:58:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:58:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:58:53Z- handleDvl() - RDI_Pathfinder msg: 1613174333.13 epoch sec; seqNo:4095 2021-02-12T23:58:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.17 , -0.12 2021-02-12T23:58:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174333.16 epoch sec; seqNo:8587; 4058317.59 north; 591834.16 east 2021-02-12T23:58:53Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-12T23:58:53Z- waiting for 1613174331.30, time is 1613174333.16 2021-02-12T23:58:53Z- LcmTrn::updateTrn() equating pose 1613174333.16 and meas times 1613174333.16 2021-02-12T23:58:53Z- LcmTrn::updateTrn() >>>> pose time 1613174333.16, meas time 1613174333.16 <<<< 2021-02-12T23:58:53Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174333.16, ping # 0. 2021-02-12T23:58:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174333.16 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:58:53Z- TerrainNav::motionUpdate() - currEstimate = (4058318, 591834.16, 12.76) 2021-02-12T23:58:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:58:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:58:53Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:58:53Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:58:53Z- TNavBF::Projecting Measurements 2021-02-12T23:58:53Z- TNavFilter::Measurement from time = 1613174333.16 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:58:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:58:53Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:58:53Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:58:53Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:58:53Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:58:53Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:58:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:58:53Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 725.0 EastVariance: 833.0 DepthVariance: 1.6 this->DepthBiasCov: 1.6 2021-02-12T23:58:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:58:53Z- TNavBF::mmse selected from filter -1 2021-02-12T23:58:53Z- variances([0], [2], and psi_berg) = 724.999039 833.013017 0.000000 2021-02-12T23:58:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:58:53Z- LcmTrn::publishEstimates() - MLE : 4058312.09 591879.66 12.76 2021-02-12T23:58:53Z- LcmTrn::publishEstimates() - MMSE : 4058317.59 591834.16 12.76 2021-02-12T23:58:53Z- LcmTrn::publishEstimates() - COVAR: 725.00 833.01 1.57 0.00 2021-02-12T23:58:53Z- AHRS_M2 msg: 1613174333.52 epoch sec; seqNo:8518 2021-02-12T23:58:53Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.19 psi 2021-02-12T23:58:53Z- handleDepth()-Depth_Keller msg: 1613174333525 epoch sec; seqNo:8588; depth 12.90 2021-02-12T23:58:53Z- handleDvl() - RDI_Pathfinder msg: 1613174333.54 epoch sec; seqNo:4096 2021-02-12T23:58:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 102.92 , 78.58 , 115.08 2021-02-12T23:58:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.22 , 0.01 2021-02-12T23:58:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174333.55 epoch sec; seqNo:8588; 4058317.96 north; 591834.16 east 2021-02-12T23:58:53Z- AHRS_M2 msg: 1613174333.92 epoch sec; seqNo:8519 2021-02-12T23:58:53Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.18 psi 2021-02-12T23:58:53Z- handleDepth()-Depth_Keller msg: 1613174333933 epoch sec; seqNo:8589; depth 13.02 2021-02-12T23:58:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174333.94 epoch sec; seqNo:8589; 4058317.96 north; 591834.16 east 2021-02-12T23:58:54Z- AHRS_M2 msg: 1613174334.33 epoch sec; seqNo:8520 2021-02-12T23:58:54Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.18 psi 2021-02-12T23:58:54Z- handleDepth()-Depth_Keller msg: 1613174334332 epoch sec; seqNo:8590; depth 13.11 2021-02-12T23:58:54Z- handleDvl() - RDI_Pathfinder msg: 1613174334.35 epoch sec; seqNo:4097 2021-02-12T23:58:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 104.20 , 74.79 , 116.37 2021-02-12T23:58:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.10 , 0.12 2021-02-12T23:58:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174334.36 epoch sec; seqNo:8590; 4058318.36 north; 591835.38 east 2021-02-12T23:58:54Z- AHRS_M2 msg: 1613174334.73 epoch sec; seqNo:8521 2021-02-12T23:58:54Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.16 psi 2021-02-12T23:58:54Z- handleDepth()-Depth_Keller msg: 1613174334736 epoch sec; seqNo:8591; depth 13.26 2021-02-12T23:58:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174334.75 epoch sec; seqNo:8591; 4058318.73 north; 591835.37 east 2021-02-12T23:58:55Z- AHRS_M2 msg: 1613174335.14 epoch sec; seqNo:8522 2021-02-12T23:58:55Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.16 psi 2021-02-12T23:58:55Z- handleDepth()-Depth_Keller msg: 1613174335144 epoch sec; seqNo:8592; depth 13.38 2021-02-12T23:58:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174335.15 epoch sec; seqNo:8592; 4058318.73 north; 591835.37 east 2021-02-12T23:58:55Z- AHRS_M2 msg: 1613174335.54 epoch sec; seqNo:8523 2021-02-12T23:58:55Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.16 psi 2021-02-12T23:58:55Z- handleDepth()-Depth_Keller msg: 1613174335551 epoch sec; seqNo:8593; depth 13.51 2021-02-12T23:58:55Z- handleDvl() - RDI_Pathfinder msg: 1613174335.57 epoch sec; seqNo:4098 2021-02-12T23:58:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 101.56 , 78.58 , 116.44 2021-02-12T23:58:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.01 , 0.14 2021-02-12T23:58:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174335.58 epoch sec; seqNo:8593; 4058319.11 north; 591835.37 east 2021-02-12T23:58:55Z- AHRS_M2 msg: 1613174335.94 epoch sec; seqNo:8524 2021-02-12T23:58:55Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.16 psi 2021-02-12T23:58:55Z- handleDepth()-Depth_Keller msg: 1613174335948 epoch sec; seqNo:8594; depth 13.63 2021-02-12T23:58:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174335.97 epoch sec; seqNo:8594; 4058319.11 north; 591835.37 east 2021-02-12T23:58:56Z- AHRS_M2 msg: 1613174336.36 epoch sec; seqNo:8525 2021-02-12T23:58:56Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.16 psi 2021-02-12T23:58:56Z- handleDepth()-Depth_Keller msg: 1613174336361 epoch sec; seqNo:8595; depth 13.74 2021-02-12T23:58:56Z- handleDvl() - RDI_Pathfinder msg: 1613174336.38 epoch sec; seqNo:4099 2021-02-12T23:58:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.27 , 100.14 , 78.51 , 120.43 2021-02-12T23:58:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.11 , 0.10 2021-02-12T23:58:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174336.39 epoch sec; seqNo:8595; 4058319.49 north; 591835.36 east 2021-02-12T23:58:56Z- AHRS_M2 msg: 1613174336.76 epoch sec; seqNo:8526 2021-02-12T23:58:56Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.17 psi 2021-02-12T23:58:56Z- handleDepth()-Depth_Keller msg: 1613174336761 epoch sec; seqNo:8596; depth 13.89 2021-02-12T23:58:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:58:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:58:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:58:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:58:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:58:56Z- handleDvl() - RDI_Pathfinder msg: 1613174336.76 epoch sec; seqNo:4100 2021-02-12T23:58:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.11 , 0.10 2021-02-12T23:58:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174336.77 epoch sec; seqNo:8596; 4058319.87 north; 591835.36 east 2021-02-12T23:58:57Z- AHRS_M2 msg: 1613174337.16 epoch sec; seqNo:8527 2021-02-12T23:58:57Z- AHRS_M2 msg: 0.66 phi; -0.38 theta; -0.17 psi 2021-02-12T23:58:57Z- handleDepth()-Depth_Keller msg: 1613174337167 epoch sec; seqNo:8597; depth 14.01 2021-02-12T23:58:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174337.18 epoch sec; seqNo:8597; 4058319.88 north; 591836.58 east 2021-02-12T23:58:57Z- AHRS_M2 msg: 1613174337.56 epoch sec; seqNo:8528 2021-02-12T23:58:57Z- AHRS_M2 msg: 0.66 phi; -0.38 theta; -0.18 psi 2021-02-12T23:58:57Z- handleDepth()-Depth_Keller msg: 1613174337567 epoch sec; seqNo:8598; depth 14.13 2021-02-12T23:58:57Z- handleDvl() - RDI_Pathfinder msg: 1613174337.58 epoch sec; seqNo:4101 2021-02-12T23:58:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.20 , 100.08 , 78.44 , 114.95 2021-02-12T23:58:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.15 , 0.00 2021-02-12T23:58:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174337.61 epoch sec; seqNo:8598; 4058320.26 north; 591836.58 east 2021-02-12T23:58:57Z- AHRS_M2 msg: 1613174337.97 epoch sec; seqNo:8529 2021-02-12T23:58:57Z- AHRS_M2 msg: 0.66 phi; -0.38 theta; -0.18 psi 2021-02-12T23:58:57Z- handleDepth()-Depth_Keller msg: 1613174337977 epoch sec; seqNo:8599; depth 14.29 2021-02-12T23:58:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:58:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:58:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:58:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:58:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:58:57Z- handleDvl() - RDI_Pathfinder msg: 1613174337.98 epoch sec; seqNo:4102 2021-02-12T23:58:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.15 , 0.00 2021-02-12T23:58:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174337.99 epoch sec; seqNo:8599; 4058320.26 north; 591836.58 east 2021-02-12T23:58:58Z- LcmTrn::handleMessages() - handling 43 messages took 0.61 ms and 131 calls... 2021-02-12T23:58:58Z- waiting for 1613174336.16, time is 1613174337.99 2021-02-12T23:58:58Z- LcmTrn::updateTrn() equating pose 1613174337.99 and meas times 1613174337.99 2021-02-12T23:58:58Z- LcmTrn::updateTrn() >>>> pose time 1613174337.99, meas time 1613174337.99 <<<< 2021-02-12T23:58:58Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174337.99, ping # 0. 2021-02-12T23:58:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174337.99 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:58:58Z- TerrainNav::motionUpdate() - currEstimate = (4058320, 591836.58, 14.29) 2021-02-12T23:58:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:58:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:58:58Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:58:58Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:58:58Z- TNavBF::Projecting Measurements 2021-02-12T23:58:58Z- TNavFilter::Measurement from time = 1613174337.99 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:58:58Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:58:58Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:58:58Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:58:58Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:58:58Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:58:58Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:58:58Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:58:58Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 724.9 EastVariance: 833.0 DepthVariance: 1.8 this->DepthBiasCov: 1.8 2021-02-12T23:58:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:58:58Z- TNavBF::mmse selected from filter -1 2021-02-12T23:58:58Z- variances([0], [2], and psi_berg) = 724.931214 833.012971 0.000000 2021-02-12T23:58:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:58:58Z- LcmTrn::publishEstimates() - MLE : 4058315.76 591802.08 14.29 2021-02-12T23:58:58Z- LcmTrn::publishEstimates() - MMSE : 4058320.26 591836.58 14.29 2021-02-12T23:58:58Z- LcmTrn::publishEstimates() - COVAR: 724.93 833.01 1.75 0.00 2021-02-12T23:58:58Z- AHRS_M2 msg: 1613174338.37 epoch sec; seqNo:8530 2021-02-12T23:58:58Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.19 psi 2021-02-12T23:58:58Z- handleDepth()-Depth_Keller msg: 1613174338374 epoch sec; seqNo:8600; depth 14.43 2021-02-12T23:58:58Z- handleDvl() - RDI_Pathfinder msg: 1613174338.39 epoch sec; seqNo:4103 2021-02-12T23:58:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 101.56 , 78.58 , 119.14 2021-02-12T23:58:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.08 , -0.10 2021-02-12T23:58:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174338.40 epoch sec; seqNo:8600; 4058320.64 north; 591836.57 east 2021-02-12T23:58:58Z- AHRS_M2 msg: 1613174338.77 epoch sec; seqNo:8531 2021-02-12T23:58:58Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.20 psi 2021-02-12T23:58:58Z- handleDepth()-Depth_Keller msg: 1613174338776 epoch sec; seqNo:8601; depth 14.55 2021-02-12T23:58:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174338.79 epoch sec; seqNo:8601; 4058321.02 north; 591836.57 east 2021-02-12T23:58:59Z- AHRS_M2 msg: 1613174339.17 epoch sec; seqNo:8532 2021-02-12T23:58:59Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.22 psi 2021-02-12T23:58:59Z- handleDepth()-Depth_Keller msg: 1613174339181 epoch sec; seqNo:8602; depth 14.67 2021-02-12T23:58:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:58:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:58:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:58:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:58:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:58:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:58:59Z- handleDvl() - RDI_Pathfinder msg: 1613174339.18 epoch sec; seqNo:4104 2021-02-12T23:58:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:58:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.08 , -0.10 2021-02-12T23:58:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174339.19 epoch sec; seqNo:8602; 4058321.41 north; 591837.79 east 2021-02-12T23:58:59Z- AHRS_M2 msg: 1613174339.59 epoch sec; seqNo:8533 2021-02-12T23:58:59Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.23 psi 2021-02-12T23:58:59Z- handleDepth()-Depth_Keller msg: 1613174339590 epoch sec; seqNo:8603; depth 14.83 2021-02-12T23:58:59Z- handleDvl() - RDI_Pathfinder msg: 1613174339.60 epoch sec; seqNo:4105 2021-02-12T23:58:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.50 , 104.13 , 72.75 , 117.65 2021-02-12T23:58:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.10 2021-02-12T23:58:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174339.61 epoch sec; seqNo:8603; 4058321.41 north; 591837.79 east 2021-02-12T23:58:59Z- AHRS_M2 msg: 1613174339.98 epoch sec; seqNo:8534 2021-02-12T23:58:59Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.23 psi 2021-02-12T23:58:59Z- handleDepth()-Depth_Keller msg: 1613174339988 epoch sec; seqNo:8604; depth 14.97 2021-02-12T23:59:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174340.01 epoch sec; seqNo:8604; 4058321.79 north; 591837.78 east 2021-02-12T23:59:00Z- AHRS_M2 msg: 1613174340.39 epoch sec; seqNo:8535 2021-02-12T23:59:00Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.22 psi 2021-02-12T23:59:00Z- handleDepth()-Depth_Keller msg: 1613174340392 epoch sec; seqNo:8605; depth 15.09 2021-02-12T23:59:00Z- handleDvl() - RDI_Pathfinder msg: 1613174340.40 epoch sec; seqNo:4106 2021-02-12T23:59:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.28 , 105.63 , 78.58 , 117.80 2021-02-12T23:59:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.11 , -0.02 2021-02-12T23:59:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174340.42 epoch sec; seqNo:8605; 4058322.17 north; 591837.78 east 2021-02-12T23:59:00Z- AHRS_M2 msg: 1613174340.79 epoch sec; seqNo:8536 2021-02-12T23:59:00Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.20 psi 2021-02-12T23:59:00Z- handleDepth()-Depth_Keller msg: 1613174340796 epoch sec; seqNo:8606; depth 15.24 2021-02-12T23:59:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174340.81 epoch sec; seqNo:8606; 4058322.55 north; 591837.77 east 2021-02-12T23:59:01Z- AHRS_M2 msg: 1613174341.20 epoch sec; seqNo:8537 2021-02-12T23:59:01Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.19 psi 2021-02-12T23:59:01Z- handleDepth()-Depth_Keller msg: 1613174341206 epoch sec; seqNo:8607; depth 15.36 2021-02-12T23:59:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174341.22 epoch sec; seqNo:8607; 4058322.55 north; 591837.77 east 2021-02-12T23:59:01Z- AHRS_M2 msg: 1613174341.60 epoch sec; seqNo:8538 2021-02-12T23:59:01Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.18 psi 2021-02-12T23:59:01Z- handleDepth()-Depth_Keller msg: 1613174341616 epoch sec; seqNo:8608; depth 15.49 2021-02-12T23:59:01Z- handleDvl() - RDI_Pathfinder msg: 1613174341.63 epoch sec; seqNo:4107 2021-02-12T23:59:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.50 , 102.79 , 0.00 , 117.66 2021-02-12T23:59:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , 0.07 2021-02-12T23:59:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174341.64 epoch sec; seqNo:8608; 4058322.94 north; 591838.99 east 2021-02-12T23:59:02Z- AHRS_M2 msg: 1613174342.01 epoch sec; seqNo:8539 2021-02-12T23:59:02Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.18 psi 2021-02-12T23:59:02Z- handleDepth()-Depth_Keller msg: 1613174342010 epoch sec; seqNo:8609; depth 15.64 2021-02-12T23:59:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174342.04 epoch sec; seqNo:8609; 4058323.32 north; 591838.99 east 2021-02-12T23:59:02Z- AHRS_M2 msg: 1613174342.41 epoch sec; seqNo:8540 2021-02-12T23:59:02Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.18 psi 2021-02-12T23:59:02Z- handleDepth()-Depth_Keller msg: 1613174342412 epoch sec; seqNo:8610; depth 15.77 2021-02-12T23:59:02Z- handleDvl() - RDI_Pathfinder msg: 1613174342.42 epoch sec; seqNo:4108 2021-02-12T23:59:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.45 , 99.68 , 78.04 , 115.90 2021-02-12T23:59:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.03 , 0.08 2021-02-12T23:59:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174342.43 epoch sec; seqNo:8610; 4058323.70 north; 591838.98 east 2021-02-12T23:59:02Z- AHRS_M2 msg: 1613174342.81 epoch sec; seqNo:8541 2021-02-12T23:59:02Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.19 psi 2021-02-12T23:59:02Z- handleDepth()-Depth_Keller msg: 1613174342821 epoch sec; seqNo:8611; depth 15.91 2021-02-12T23:59:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:02Z- handleDvl() - RDI_Pathfinder msg: 1613174342.82 epoch sec; seqNo:4109 2021-02-12T23:59:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.03 , 0.08 2021-02-12T23:59:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174342.83 epoch sec; seqNo:8611; 4058323.70 north; 591838.98 east 2021-02-12T23:59:03Z- AHRS_M2 msg: 1613174343.21 epoch sec; seqNo:8542 2021-02-12T23:59:03Z- AHRS_M2 msg: 0.61 phi; -0.38 theta; -0.19 psi 2021-02-12T23:59:03Z- handleDepth()-Depth_Keller msg: 1613174343223 epoch sec; seqNo:8612; depth 16.05 2021-02-12T23:59:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174343.23 epoch sec; seqNo:8612; 4058324.08 north; 591838.98 east 2021-02-12T23:59:03Z- LcmTrn::handleMessages() - handling 46 messages took 0.67 ms and 133 calls... 2021-02-12T23:59:03Z- waiting for 1613174340.99, time is 1613174343.23 2021-02-12T23:59:03Z- LcmTrn::updateTrn() >>>> pose time 1613174343.23, meas time 1613174342.82 <<<< 2021-02-12T23:59:03Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174342.82, ping # 0. 2021-02-12T23:59:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174342.82 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:59:03Z- TerrainNav::motionUpdate() - currEstimate = (4058324, 591838.98, 16.05) 2021-02-12T23:59:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:59:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:59:03Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:59:03Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:59:03Z- TNavBF::Projecting Measurements 2021-02-12T23:59:03Z- TNavFilter::Measurement from time = 1613174342.82 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:59:03Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:59:03Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:59:03Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:59:03Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:59:03Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:59:03Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:59:03Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:59:03Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 724.9 EastVariance: 833.0 DepthVariance: 2.0 this->DepthBiasCov: 2.0 2021-02-12T23:59:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:59:03Z- TNavBF::mmse selected from filter -1 2021-02-12T23:59:03Z- variances([0], [2], and psi_berg) = 724.837249 833.012906 0.000000 2021-02-12T23:59:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:59:03Z- LcmTrn::publishEstimates() - MLE : 4058319.58 591804.48 16.05 2021-02-12T23:59:03Z- LcmTrn::publishEstimates() - MMSE : 4058324.08 591838.98 16.05 2021-02-12T23:59:03Z- LcmTrn::publishEstimates() - COVAR: 724.84 833.01 1.98 0.00 2021-02-12T23:59:03Z- AHRS_M2 msg: 1613174343.62 epoch sec; seqNo:8543 2021-02-12T23:59:03Z- AHRS_M2 msg: 0.61 phi; -0.38 theta; -0.20 psi 2021-02-12T23:59:03Z- handleDepth()-Depth_Keller msg: 1613174343624 epoch sec; seqNo:8613; depth 16.19 2021-02-12T23:59:03Z- handleDvl() - RDI_Pathfinder msg: 1613174343.63 epoch sec; seqNo:4110 2021-02-12T23:59:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.64 , 100.22 , 78.58 , 115.09 2021-02-12T23:59:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.09 , -0.01 2021-02-12T23:59:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174343.65 epoch sec; seqNo:8613; 4058324.08 north; 591838.98 east 2021-02-12T23:59:04Z- AHRS_M2 msg: 1613174344.02 epoch sec; seqNo:8544 2021-02-12T23:59:04Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.21 psi 2021-02-12T23:59:04Z- handleDepth()-Depth_Keller msg: 1613174344028 epoch sec; seqNo:8614; depth 16.32 2021-02-12T23:59:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:04Z- handleDvl() - RDI_Pathfinder msg: 1613174344.03 epoch sec; seqNo:4111 2021-02-12T23:59:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.09 , -0.01 2021-02-12T23:59:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174344.04 epoch sec; seqNo:8614; 4058324.47 north; 591840.19 east 2021-02-12T23:59:04Z- AHRS_M2 msg: 1613174344.43 epoch sec; seqNo:8545 2021-02-12T23:59:04Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.21 psi 2021-02-12T23:59:04Z- handleDepth()-Depth_Keller msg: 1613174344433 epoch sec; seqNo:8615; depth 16.43 2021-02-12T23:59:04Z- handleDvl() - RDI_Pathfinder msg: 1613174344.44 epoch sec; seqNo:4112 2021-02-12T23:59:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.28 , 101.57 , 71.94 , 116.44 2021-02-12T23:59:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.06 , -0.10 2021-02-12T23:59:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174344.46 epoch sec; seqNo:8615; 4058324.85 north; 591840.19 east 2021-02-12T23:59:04Z- AHRS_M2 msg: 1613174344.83 epoch sec; seqNo:8546 2021-02-12T23:59:04Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.21 psi 2021-02-12T23:59:04Z- handleDepth()-Depth_Keller msg: 1613174344838 epoch sec; seqNo:8616; depth 16.59 2021-02-12T23:59:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174344.85 epoch sec; seqNo:8616; 4058325.23 north; 591840.19 east 2021-02-12T23:59:05Z- AHRS_M2 msg: 1613174345.24 epoch sec; seqNo:8547 2021-02-12T23:59:05Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.21 psi 2021-02-12T23:59:05Z- handleDepth()-Depth_Keller msg: 1613174345245 epoch sec; seqNo:8617; depth 16.72 2021-02-12T23:59:05Z- handleDvl() - RDI_Pathfinder msg: 1613174345.26 epoch sec; seqNo:4113 2021-02-12T23:59:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.07 , 101.71 , 71.53 , 116.58 2021-02-12T23:59:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.10 , -0.12 2021-02-12T23:59:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174345.30 epoch sec; seqNo:8617; 4058325.61 north; 591840.18 east 2021-02-12T23:59:05Z- AHRS_M2 msg: 1613174345.64 epoch sec; seqNo:8548 2021-02-12T23:59:05Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.21 psi 2021-02-12T23:59:05Z- handleDepth()-Depth_Keller msg: 1613174345644 epoch sec; seqNo:8618; depth 16.88 2021-02-12T23:59:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174345.66 epoch sec; seqNo:8618; 4058325.61 north; 591840.18 east 2021-02-12T23:59:06Z- AHRS_M2 msg: 1613174346.05 epoch sec; seqNo:8549 2021-02-12T23:59:06Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.21 psi 2021-02-12T23:59:06Z- handleDepth()-Depth_Keller msg: 1613174346049 epoch sec; seqNo:8619; depth 17.01 2021-02-12T23:59:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174346.06 epoch sec; seqNo:8619; 4058325.99 north; 591840.18 east 2021-02-12T23:59:06Z- AHRS_M2 msg: 1613174346.45 epoch sec; seqNo:8550 2021-02-12T23:59:06Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.21 psi 2021-02-12T23:59:06Z- handleDepth()-Depth_Keller msg: 1613174346454 epoch sec; seqNo:8620; depth 17.14 2021-02-12T23:59:06Z- handleDvl() - RDI_Pathfinder msg: 1613174346.47 epoch sec; seqNo:4114 2021-02-12T23:59:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.78 , 100.42 , 72.35 , 112.59 2021-02-12T23:59:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.19 , -0.07 2021-02-12T23:59:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174346.48 epoch sec; seqNo:8620; 4058326.36 north; 591840.17 east 2021-02-12T23:59:06Z- AHRS_M2 msg: 1613174346.85 epoch sec; seqNo:8551 2021-02-12T23:59:06Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.21 psi 2021-02-12T23:59:06Z- handleDepth()-Depth_Keller msg: 1613174346861 epoch sec; seqNo:8621; depth 17.27 2021-02-12T23:59:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174346.88 epoch sec; seqNo:8621; 4058326.76 north; 591841.39 east 2021-02-12T23:59:07Z- AHRS_M2 msg: 1613174347.26 epoch sec; seqNo:8552 2021-02-12T23:59:07Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-12T23:59:07Z- handleDepth()-Depth_Keller msg: 1613174347260 epoch sec; seqNo:8622; depth 17.42 2021-02-12T23:59:07Z- handleDvl() - RDI_Pathfinder msg: 1613174347.27 epoch sec; seqNo:4115 2021-02-12T23:59:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 98.86 , 71.52 , 117.11 2021-02-12T23:59:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.22 , 0.04 2021-02-12T23:59:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174347.30 epoch sec; seqNo:8622; 4058327.13 north; 591841.39 east 2021-02-12T23:59:07Z- AHRS_M2 msg: 1613174347.66 epoch sec; seqNo:8553 2021-02-12T23:59:07Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-12T23:59:07Z- handleDepth()-Depth_Keller msg: 1613174347665 epoch sec; seqNo:8623; depth 17.56 2021-02-12T23:59:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:07Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:07Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:07Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:07Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:07Z- handleDvl() - RDI_Pathfinder msg: 1613174347.67 epoch sec; seqNo:4116 2021-02-12T23:59:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.22 , 0.04 2021-02-12T23:59:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174347.68 epoch sec; seqNo:8623; 4058327.51 north; 591841.38 east 2021-02-12T23:59:08Z- AHRS_M2 msg: 1613174348.06 epoch sec; seqNo:8554 2021-02-12T23:59:08Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.18 psi 2021-02-12T23:59:08Z- handleDepth()-Depth_Keller msg: 1613174348068 epoch sec; seqNo:8624; depth 17.69 2021-02-12T23:59:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174348.08 epoch sec; seqNo:8624; 4058327.89 north; 591841.38 east 2021-02-12T23:59:08Z- LcmTrn::handleMessages() - handling 43 messages took 0.67 ms and 130 calls... 2021-02-12T23:59:08Z- waiting for 1613174346.23, time is 1613174348.08 2021-02-12T23:59:08Z- LcmTrn::updateTrn() >>>> pose time 1613174348.08, meas time 1613174347.67 <<<< 2021-02-12T23:59:08Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174347.67, ping # 0. 2021-02-12T23:59:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174347.67 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:59:08Z- TerrainNav::motionUpdate() - currEstimate = (4058328, 591841.38, 17.69) 2021-02-12T23:59:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:59:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:59:08Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:59:08Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:59:08Z- TNavBF::Projecting Measurements 2021-02-12T23:59:08Z- TNavFilter::Measurement from time = 1613174347.67 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:59:08Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:59:08Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:59:08Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:59:08Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:59:08Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:59:08Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:59:08Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:59:08Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 724.8 EastVariance: 833.0 DepthVariance: 2.2 this->DepthBiasCov: 2.2 2021-02-12T23:59:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:59:08Z- TNavBF::mmse selected from filter -1 2021-02-12T23:59:08Z- variances([0], [2], and psi_berg) = 724.742699 833.012841 0.000000 2021-02-12T23:59:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:59:08Z- LcmTrn::publishEstimates() - MLE : 4058323.39 591806.88 17.69 2021-02-12T23:59:08Z- LcmTrn::publishEstimates() - MMSE : 4058327.89 591841.38 17.69 2021-02-12T23:59:08Z- LcmTrn::publishEstimates() - COVAR: 724.74 833.01 2.21 0.00 2021-02-12T23:59:08Z- AHRS_M2 msg: 1613174348.47 epoch sec; seqNo:8555 2021-02-12T23:59:08Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.17 psi 2021-02-12T23:59:08Z- handleDepth()-Depth_Keller msg: 1613174348472 epoch sec; seqNo:8625; depth 17.82 2021-02-12T23:59:08Z- handleDvl() - RDI_Pathfinder msg: 1613174348.48 epoch sec; seqNo:4117 2021-02-12T23:59:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.78 , 97.71 , 70.29 , 111.23 2021-02-12T23:59:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.16 , 0.10 2021-02-12T23:59:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174348.51 epoch sec; seqNo:8625; 4058327.89 north; 591841.38 east 2021-02-12T23:59:08Z- AHRS_M2 msg: 1613174348.87 epoch sec; seqNo:8556 2021-02-12T23:59:08Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.17 psi 2021-02-12T23:59:08Z- handleDepth()-Depth_Keller msg: 1613174348875 epoch sec; seqNo:8626; depth 17.96 2021-02-12T23:59:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:08Z- handleDvl() - RDI_Pathfinder msg: 1613174348.88 epoch sec; seqNo:4118 2021-02-12T23:59:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.16 , 0.10 2021-02-12T23:59:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174348.89 epoch sec; seqNo:8626; 4058328.27 north; 591841.38 east 2021-02-12T23:59:09Z- AHRS_M2 msg: 1613174349.28 epoch sec; seqNo:8557 2021-02-12T23:59:09Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.18 psi 2021-02-12T23:59:09Z- handleDepth()-Depth_Keller msg: 1613174349282 epoch sec; seqNo:8627; depth 18.09 2021-02-12T23:59:09Z- handleDvl() - RDI_Pathfinder msg: 1613174349.29 epoch sec; seqNo:4119 2021-02-12T23:59:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 95.34 , 71.93 , 111.16 2021-02-12T23:59:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , 0.11 2021-02-12T23:59:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174349.31 epoch sec; seqNo:8627; 4058328.65 north; 591841.37 east 2021-02-12T23:59:09Z- AHRS_M2 msg: 1613174349.68 epoch sec; seqNo:8558 2021-02-12T23:59:09Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.18 psi 2021-02-12T23:59:09Z- handleDepth()-Depth_Keller msg: 1613174349684 epoch sec; seqNo:8628; depth 18.22 2021-02-12T23:59:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174349.70 epoch sec; seqNo:8628; 4058328.65 north; 591841.37 east 2021-02-12T23:59:10Z- AHRS_M2 msg: 1613174350.08 epoch sec; seqNo:8559 2021-02-12T23:59:10Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.18 psi 2021-02-12T23:59:10Z- handleDepth()-Depth_Keller msg: 1613174350088 epoch sec; seqNo:8629; depth 18.34 2021-02-12T23:59:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:10Z- handleDvl() - RDI_Pathfinder msg: 1613174350.09 epoch sec; seqNo:4120 2021-02-12T23:59:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , 0.11 2021-02-12T23:59:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174350.10 epoch sec; seqNo:8629; 4058329.03 north; 591841.37 east 2021-02-12T23:59:10Z- AHRS_M2 msg: 1613174350.49 epoch sec; seqNo:8560 2021-02-12T23:59:10Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.18 psi 2021-02-12T23:59:10Z- handleDepth()-Depth_Keller msg: 1613174350497 epoch sec; seqNo:8630; depth 18.48 2021-02-12T23:59:10Z- handleDvl() - RDI_Pathfinder msg: 1613174350.51 epoch sec; seqNo:4121 2021-02-12T23:59:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.07 , 96.29 , 69.07 , 111.16 2021-02-12T23:59:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.04 , 0.06 2021-02-12T23:59:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174350.52 epoch sec; seqNo:8630; 4058329.42 north; 591842.58 east 2021-02-12T23:59:10Z- AHRS_M2 msg: 1613174350.89 epoch sec; seqNo:8561 2021-02-12T23:59:10Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.18 psi 2021-02-12T23:59:10Z- handleDepth()-Depth_Keller msg: 1613174350905 epoch sec; seqNo:8631; depth 18.61 2021-02-12T23:59:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174350.92 epoch sec; seqNo:8631; 4058329.42 north; 591842.58 east 2021-02-12T23:59:11Z- AHRS_M2 msg: 1613174351.30 epoch sec; seqNo:8562 2021-02-12T23:59:11Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.18 psi 2021-02-12T23:59:11Z- handleDepth()-Depth_Keller msg: 1613174351310 epoch sec; seqNo:8632; depth 18.74 2021-02-12T23:59:11Z- handleDvl() - RDI_Pathfinder msg: 1613174351.32 epoch sec; seqNo:4122 2021-02-12T23:59:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.66 , 97.85 , 70.71 , 111.24 2021-02-12T23:59:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.08 , 0.01 2021-02-12T23:59:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174351.33 epoch sec; seqNo:8632; 4058329.80 north; 591842.58 east 2021-02-12T23:59:11Z- AHRS_M2 msg: 1613174351.70 epoch sec; seqNo:8563 2021-02-12T23:59:11Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.18 psi 2021-02-12T23:59:11Z- handleDepth()-Depth_Keller msg: 1613174351704 epoch sec; seqNo:8633; depth 18.88 2021-02-12T23:59:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174351.72 epoch sec; seqNo:8633; 4058330.18 north; 591842.58 east 2021-02-12T23:59:12Z- AHRS_M2 msg: 1613174352.10 epoch sec; seqNo:8564 2021-02-12T23:59:12Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.18 psi 2021-02-12T23:59:12Z- handleDepth()-Depth_Keller msg: 1613174352108 epoch sec; seqNo:8634; depth 19.00 2021-02-12T23:59:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174352.12 epoch sec; seqNo:8634; 4058330.18 north; 591842.58 east 2021-02-12T23:59:12Z- AHRS_M2 msg: 1613174352.51 epoch sec; seqNo:8565 2021-02-12T23:59:12Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.18 psi 2021-02-12T23:59:12Z- handleDepth()-Depth_Keller msg: 1613174352513 epoch sec; seqNo:8635; depth 19.12 2021-02-12T23:59:12Z- handleDvl() - RDI_Pathfinder msg: 1613174352.53 epoch sec; seqNo:4123 2021-02-12T23:59:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.59 , 97.85 , 68.67 , 110.02 2021-02-12T23:59:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , -0.02 2021-02-12T23:59:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174352.54 epoch sec; seqNo:8635; 4058330.56 north; 591842.57 east 2021-02-12T23:59:12Z- AHRS_M2 msg: 1613174352.91 epoch sec; seqNo:8566 2021-02-12T23:59:12Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.18 psi 2021-02-12T23:59:12Z- handleDepth()-Depth_Keller msg: 1613174352926 epoch sec; seqNo:8636; depth 19.27 2021-02-12T23:59:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174352.94 epoch sec; seqNo:8636; 4058330.95 north; 591843.79 east 2021-02-12T23:59:13Z- AHRS_M2 msg: 1613174353.32 epoch sec; seqNo:8567 2021-02-12T23:59:13Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.18 psi 2021-02-12T23:59:13Z- handleDepth()-Depth_Keller msg: 1613174353320 epoch sec; seqNo:8637; depth 19.40 2021-02-12T23:59:13Z- handleDvl() - RDI_Pathfinder msg: 1613174353.34 epoch sec; seqNo:4124 2021-02-12T23:59:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 95.42 , 70.71 , 108.81 2021-02-12T23:59:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , -0.04 2021-02-12T23:59:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174353.35 epoch sec; seqNo:8637; 4058331.33 north; 591843.78 east 2021-02-12T23:59:13Z- LcmTrn::handleMessages() - handling 47 messages took 0.70 ms and 133 calls... 2021-02-12T23:59:13Z- waiting for 1613174351.08, time is 1613174353.35 2021-02-12T23:59:13Z- LcmTrn::updateTrn() equating pose 1613174353.35 and meas times 1613174353.35 2021-02-12T23:59:13Z- LcmTrn::updateTrn() >>>> pose time 1613174353.35, meas time 1613174353.35 <<<< 2021-02-12T23:59:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174353.35 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:59:13Z- TerrainNav::motionUpdate() - currEstimate = (4058331, 591843.78, 19.40) 2021-02-12T23:59:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:59:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:59:13Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174353.35 sec.; No relevant navigation data available 2021-02-12T23:59:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:59:13Z- TNavBF::mmse selected from filter -1 2021-02-12T23:59:13Z- variances([0], [2], and psi_berg) = 724.669759 833.012791 0.000000 2021-02-12T23:59:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:59:13Z- LcmTrn::publishEstimates() - MLE : 4058326.83 591809.28 19.40 2021-02-12T23:59:13Z- LcmTrn::publishEstimates() - MMSE : 4058331.33 591843.78 19.40 2021-02-12T23:59:13Z- LcmTrn::publishEstimates() - COVAR: 724.67 833.01 2.43 0.00 2021-02-12T23:59:13Z- AHRS_M2 msg: 1613174353.73 epoch sec; seqNo:8568 2021-02-12T23:59:13Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.18 psi 2021-02-12T23:59:13Z- handleDepth()-Depth_Keller msg: 1613174353730 epoch sec; seqNo:8638; depth 19.53 2021-02-12T23:59:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:13Z- handleDvl() - RDI_Pathfinder msg: 1613174353.73 epoch sec; seqNo:4125 2021-02-12T23:59:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , -0.04 2021-02-12T23:59:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174353.74 epoch sec; seqNo:8638; 4058331.33 north; 591843.78 east 2021-02-12T23:59:14Z- AHRS_M2 msg: 1613174354.13 epoch sec; seqNo:8569 2021-02-12T23:59:14Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-12T23:59:14Z- handleDepth()-Depth_Keller msg: 1613174354135 epoch sec; seqNo:8639; depth 19.67 2021-02-12T23:59:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174354.14 epoch sec; seqNo:8639; 4058331.71 north; 591843.78 east 2021-02-12T23:59:14Z- AHRS_M2 msg: 1613174354.53 epoch sec; seqNo:8570 2021-02-12T23:59:14Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-12T23:59:14Z- handleDepth()-Depth_Keller msg: 1613174354542 epoch sec; seqNo:8640; depth 19.79 2021-02-12T23:59:14Z- handleDvl() - RDI_Pathfinder msg: 1613174354.55 epoch sec; seqNo:4126 2021-02-12T23:59:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.17 , 95.21 , 69.89 , 108.60 2021-02-12T23:59:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , -0.05 2021-02-12T23:59:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174354.57 epoch sec; seqNo:8640; 4058332.09 north; 591843.78 east 2021-02-12T23:59:14Z- AHRS_M2 msg: 1613174354.93 epoch sec; seqNo:8571 2021-02-12T23:59:14Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.18 psi 2021-02-12T23:59:14Z- handleDepth()-Depth_Keller msg: 1613174354941 epoch sec; seqNo:8641; depth 19.92 2021-02-12T23:59:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:14Z- handleDvl() - RDI_Pathfinder msg: 1613174354.94 epoch sec; seqNo:4127 2021-02-12T23:59:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , -0.05 2021-02-12T23:59:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174354.96 epoch sec; seqNo:8641; 4058332.48 north; 591844.99 east 2021-02-12T23:59:15Z- AHRS_M2 msg: 1613174355.34 epoch sec; seqNo:8572 2021-02-12T23:59:15Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.17 psi 2021-02-12T23:59:15Z- handleDepth()-Depth_Keller msg: 1613174355345 epoch sec; seqNo:8642; depth 20.05 2021-02-12T23:59:15Z- handleDvl() - RDI_Pathfinder msg: 1613174355.36 epoch sec; seqNo:4128 2021-02-12T23:59:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 97.92 , 78.44 , 106.44 2021-02-12T23:59:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.06 2021-02-12T23:59:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174355.37 epoch sec; seqNo:8642; 4058332.48 north; 591844.99 east 2021-02-12T23:59:15Z- AHRS_M2 msg: 1613174355.74 epoch sec; seqNo:8573 2021-02-12T23:59:15Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.17 psi 2021-02-12T23:59:15Z- handleDepth()-Depth_Keller msg: 1613174355745 epoch sec; seqNo:8643; depth 20.18 2021-02-12T23:59:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174355.76 epoch sec; seqNo:8643; 4058332.86 north; 591844.99 east 2021-02-12T23:59:16Z- AHRS_M2 msg: 1613174356.17 epoch sec; seqNo:8574 2021-02-12T23:59:16Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.16 psi 2021-02-12T23:59:16Z- handleDepth()-Depth_Keller msg: 1613174356170 epoch sec; seqNo:8644; depth 20.30 2021-02-12T23:59:16Z- handleDvl() - RDI_Pathfinder msg: 1613174356.18 epoch sec; seqNo:4129 2021-02-12T23:59:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.02 , 94.13 , 68.26 , 106.30 2021-02-12T23:59:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , -0.04 2021-02-12T23:59:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174356.19 epoch sec; seqNo:8644; 4058333.24 north; 591844.98 east 2021-02-12T23:59:16Z- AHRS_M2 msg: 1613174356.55 epoch sec; seqNo:8575 2021-02-12T23:59:16Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.16 psi 2021-02-12T23:59:16Z- handleDepth()-Depth_Keller msg: 1613174356552 epoch sec; seqNo:8645; depth 20.44 2021-02-12T23:59:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174356.56 epoch sec; seqNo:8645; 4058333.61 north; 591844.98 east 2021-02-12T23:59:16Z- AHRS_M2 msg: 1613174356.95 epoch sec; seqNo:8576 2021-02-12T23:59:16Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.16 psi 2021-02-12T23:59:16Z- handleDepth()-Depth_Keller msg: 1613174356956 epoch sec; seqNo:8646; depth 20.56 2021-02-12T23:59:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174356.96 epoch sec; seqNo:8646; 4058333.99 north; 591844.98 east 2021-02-12T23:59:17Z- AHRS_M2 msg: 1613174357.36 epoch sec; seqNo:8577 2021-02-12T23:59:17Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.16 psi 2021-02-12T23:59:17Z- handleDepth()-Depth_Keller msg: 1613174357369 epoch sec; seqNo:8647; depth 20.67 2021-02-12T23:59:17Z- handleDvl() - RDI_Pathfinder msg: 1613174357.38 epoch sec; seqNo:4130 2021-02-12T23:59:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.64 , 98.12 , 69.07 , 106.63 2021-02-12T23:59:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.11 , -0.02 2021-02-12T23:59:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174357.39 epoch sec; seqNo:8647; 4058333.99 north; 591844.98 east 2021-02-12T23:59:17Z- AHRS_M2 msg: 1613174357.76 epoch sec; seqNo:8578 2021-02-12T23:59:17Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.17 psi 2021-02-12T23:59:17Z- handleDepth()-Depth_Keller msg: 1613174357768 epoch sec; seqNo:8648; depth 20.81 2021-02-12T23:59:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174357.79 epoch sec; seqNo:8648; 4058334.39 north; 591846.19 east 2021-02-12T23:59:18Z- AHRS_M2 msg: 1613174358.17 epoch sec; seqNo:8579 2021-02-12T23:59:18Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.17 psi 2021-02-12T23:59:18Z- handleDepth()-Depth_Keller msg: 1613174358173 epoch sec; seqNo:8649; depth 20.93 2021-02-12T23:59:18Z- handleDvl() - RDI_Pathfinder msg: 1613174358.19 epoch sec; seqNo:4131 2021-02-12T23:59:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 91.83 , 69.07 , 107.65 2021-02-12T23:59:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.12 , 0.01 2021-02-12T23:59:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174358.20 epoch sec; seqNo:8649; 4058334.76 north; 591846.19 east 2021-02-12T23:59:18Z- LcmTrn::handleMessages() - handling 43 messages took 0.58 ms and 132 calls... 2021-02-12T23:59:18Z- waiting for 1613174356.35, time is 1613174358.20 2021-02-12T23:59:18Z- LcmTrn::updateTrn() equating pose 1613174358.20 and meas times 1613174358.20 2021-02-12T23:59:18Z- LcmTrn::updateTrn() >>>> pose time 1613174358.20, meas time 1613174358.20 <<<< 2021-02-12T23:59:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174358.20 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:59:18Z- TerrainNav::motionUpdate() - currEstimate = (4058335, 591846.19, 20.93) 2021-02-12T23:59:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:59:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:59:18Z- TNavBF::Projecting Measurements 2021-02-12T23:59:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:59:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:59:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:59:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:59:18Z- TNavFilter::4 good beams. 2021-02-12T23:59:18Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:59:18Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:59:18Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:59:18Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:59:18Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:59:18Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:59:18Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:59:18Z- TNavBF::mmse selected from filter -1 2021-02-12T23:59:18Z- TerrainMapOctree: (vehN, vehE ) = (4058334.76, 591846.19). 2021-02-12T23:59:18Z- TerrainMapOctree: Min Distance = 3778.48. 2021-02-12T23:59:18Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:59:18Z- There is no specific good beam that all particles have in common. 2021-02-12T23:59:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:59:18Z- Old Depth Bias: 0.00 DepthBiasCov: 2.64 K: 0.192 ExpectedMeasDiff: nan currMeas.covariance: 9.35 2021-02-12T23:59:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:59:18Z- Old Depth Bias: 0.00 DepthBiasCov: 2.13 K: 0.123 ExpectedMeasDiff: nan currMeas.covariance: 13.49 2021-02-12T23:59:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:59:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.87 K: 0.166 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-12T23:59:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:59:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.56 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 18.54 2021-02-12T23:59:18Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:59:18Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:59:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:59:18Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:59:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:59:18Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:59:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:59:18Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:59:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:59:18Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:59:18Z- Filter 0 No beams used in subcloud update 2021-02-12T23:59:18Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:59:18Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:59:18Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 724.6 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-12T23:59:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174358.20 sec. 2021-02-12T23:59:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:59:18Z- TNavBF::mmse selected from filter -1 2021-02-12T23:59:18Z- variances([0], [2], and psi_berg) = 724.596881 833.012741 0.000000 2021-02-12T23:59:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:59:18Z- LcmTrn::publishEstimates() - MLE : 4058330.26 591811.69 20.93 2021-02-12T23:59:18Z- LcmTrn::publishEstimates() - MMSE : 4058334.76 591846.19 20.93 2021-02-12T23:59:18Z- LcmTrn::publishEstimates() - COVAR: 724.60 833.01 1.45 0.00 2021-02-12T23:59:18Z- AHRS_M2 msg: 1613174358.57 epoch sec; seqNo:8580 2021-02-12T23:59:18Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.18 psi 2021-02-12T23:59:18Z- handleDepth()-Depth_Keller msg: 1613174358577 epoch sec; seqNo:8650; depth 21.07 2021-02-12T23:59:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:18Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:18Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:18Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:18Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:18Z- handleDvl() - RDI_Pathfinder msg: 1613174358.58 epoch sec; seqNo:4132 2021-02-12T23:59:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.12 , 0.01 2021-02-12T23:59:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174358.59 epoch sec; seqNo:8650; 4058335.14 north; 591846.19 east 2021-02-12T23:59:18Z- AHRS_M2 msg: 1613174358.97 epoch sec; seqNo:8581 2021-02-12T23:59:18Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.18 psi 2021-02-12T23:59:18Z- handleDepth()-Depth_Keller msg: 1613174358982 epoch sec; seqNo:8651; depth 21.17 2021-02-12T23:59:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174358.99 epoch sec; seqNo:8651; 4058335.52 north; 591846.18 east 2021-02-12T23:59:19Z- AHRS_M2 msg: 1613174359.38 epoch sec; seqNo:8582 2021-02-12T23:59:19Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.19 psi 2021-02-12T23:59:19Z- handleDepth()-Depth_Keller msg: 1613174359385 epoch sec; seqNo:8652; depth 21.30 2021-02-12T23:59:19Z- handleDvl() - RDI_Pathfinder msg: 1613174359.40 epoch sec; seqNo:4133 2021-02-12T23:59:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 98.12 , 67.44 , 105.42 2021-02-12T23:59:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.10 , 0.03 2021-02-12T23:59:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174359.41 epoch sec; seqNo:8652; 4058335.52 north; 591846.18 east 2021-02-12T23:59:19Z- AHRS_M2 msg: 1613174359.78 epoch sec; seqNo:8583 2021-02-12T23:59:19Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.19 psi 2021-02-12T23:59:19Z- handleDepth()-Depth_Keller msg: 1613174359785 epoch sec; seqNo:8653; depth 21.43 2021-02-12T23:59:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:19Z- handleDvl() - RDI_Pathfinder msg: 1613174359.79 epoch sec; seqNo:4134 2021-02-12T23:59:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.10 , 0.03 2021-02-12T23:59:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174359.80 epoch sec; seqNo:8653; 4058335.90 north; 591846.18 east 2021-02-12T23:59:20Z- AHRS_M2 msg: 1613174360.18 epoch sec; seqNo:8584 2021-02-12T23:59:20Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.19 psi 2021-02-12T23:59:20Z- handleDepth()-Depth_Keller msg: 1613174360193 epoch sec; seqNo:8654; depth 21.56 2021-02-12T23:59:20Z- handleDvl() - RDI_Pathfinder msg: 1613174360.20 epoch sec; seqNo:4135 2021-02-12T23:59:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.21 , 98.05 , 68.26 , 105.36 2021-02-12T23:59:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.09 , 0.03 2021-02-12T23:59:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174360.23 epoch sec; seqNo:8654; 4058336.28 north; 591846.17 east 2021-02-12T23:59:20Z- AHRS_M2 msg: 1613174360.59 epoch sec; seqNo:8585 2021-02-12T23:59:20Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.19 psi 2021-02-12T23:59:20Z- handleDepth()-Depth_Keller msg: 1613174360592 epoch sec; seqNo:8655; depth 21.70 2021-02-12T23:59:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174360.60 epoch sec; seqNo:8655; 4058336.67 north; 591847.39 east 2021-02-12T23:59:20Z- AHRS_M2 msg: 1613174360.99 epoch sec; seqNo:8586 2021-02-12T23:59:20Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.19 psi 2021-02-12T23:59:20Z- handleDepth()-Depth_Keller msg: 1613174360998 epoch sec; seqNo:8656; depth 21.82 2021-02-12T23:59:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:21Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:21Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:21Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:21Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:21Z- handleDvl() - RDI_Pathfinder msg: 1613174361.00 epoch sec; seqNo:4136 2021-02-12T23:59:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.09 , 0.03 2021-02-12T23:59:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174361.01 epoch sec; seqNo:8656; 4058337.05 north; 591847.39 east 2021-02-12T23:59:21Z- AHRS_M2 msg: 1613174361.40 epoch sec; seqNo:8587 2021-02-12T23:59:21Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.19 psi 2021-02-12T23:59:21Z- handleDepth()-Depth_Keller msg: 1613174361405 epoch sec; seqNo:8657; depth 21.95 2021-02-12T23:59:21Z- handleDvl() - RDI_Pathfinder msg: 1613174361.43 epoch sec; seqNo:4137 2021-02-12T23:59:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 68.26 , 102.92 2021-02-12T23:59:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , 0.01 2021-02-12T23:59:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174361.44 epoch sec; seqNo:8657; 4058337.05 north; 591847.39 east 2021-02-12T23:59:21Z- AHRS_M2 msg: 1613174361.80 epoch sec; seqNo:8588 2021-02-12T23:59:21Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.19 psi 2021-02-12T23:59:21Z- handleDepth()-Depth_Keller msg: 1613174361804 epoch sec; seqNo:8658; depth 22.09 2021-02-12T23:59:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174361.82 epoch sec; seqNo:8658; 4058337.43 north; 591847.38 east 2021-02-12T23:59:22Z- AHRS_M2 msg: 1613174362.20 epoch sec; seqNo:8589 2021-02-12T23:59:22Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.18 psi 2021-02-12T23:59:22Z- handleDepth()-Depth_Keller msg: 1613174362213 epoch sec; seqNo:8659; depth 22.21 2021-02-12T23:59:22Z- handleDvl() - RDI_Pathfinder msg: 1613174362.23 epoch sec; seqNo:4138 2021-02-12T23:59:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.57 , 98.05 , 67.44 , 105.36 2021-02-12T23:59:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.02 2021-02-12T23:59:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174362.24 epoch sec; seqNo:8659; 4058337.81 north; 591847.38 east 2021-02-12T23:59:22Z- AHRS_M2 msg: 1613174362.61 epoch sec; seqNo:8590 2021-02-12T23:59:22Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.18 psi 2021-02-12T23:59:22Z- handleDepth()-Depth_Keller msg: 1613174362614 epoch sec; seqNo:8660; depth 22.34 2021-02-12T23:59:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174362.63 epoch sec; seqNo:8660; 4058337.81 north; 591847.38 east 2021-02-12T23:59:23Z- AHRS_M2 msg: 1613174363.01 epoch sec; seqNo:8591 2021-02-12T23:59:23Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.18 psi 2021-02-12T23:59:23Z- handleDepth()-Depth_Keller msg: 1613174363016 epoch sec; seqNo:8661; depth 22.47 2021-02-12T23:59:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174363.02 epoch sec; seqNo:8661; 4058338.19 north; 591847.37 east 2021-02-12T23:59:23Z- AHRS_M2 msg: 1613174363.42 epoch sec; seqNo:8592 2021-02-12T23:59:23Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.18 psi 2021-02-12T23:59:23Z- handleDepth()-Depth_Keller msg: 1613174363426 epoch sec; seqNo:8662; depth 22.58 2021-02-12T23:59:23Z- handleDvl() - RDI_Pathfinder msg: 1613174363.44 epoch sec; seqNo:4139 2021-02-12T23:59:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:23Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:59:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174363.45 epoch sec; seqNo:8662; 4058338.58 north; 591848.59 east 2021-02-12T23:59:23Z- LcmTrn::handleMessages() - handling 47 messages took 0.66 ms and 134 calls... 2021-02-12T23:59:23Z- waiting for 1613174361.20, time is 1613174363.45 2021-02-12T23:59:23Z- LcmTrn::updateTrn() equating pose 1613174363.45 and meas times 1613174363.45 2021-02-12T23:59:23Z- LcmTrn::updateTrn() >>>> pose time 1613174363.45, meas time 1613174363.45 <<<< 2021-02-12T23:59:23Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174363.45, ping # 0. 2021-02-12T23:59:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174363.45 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:59:23Z- TerrainNav::motionUpdate() - currEstimate = (4058339, 591848.59, 22.58) 2021-02-12T23:59:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:59:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:59:23Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:59:23Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:59:23Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174363.45 sec.; No relevant navigation data available 2021-02-12T23:59:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:59:23Z- TNavBF::mmse selected from filter -1 2021-02-12T23:59:23Z- variances([0], [2], and psi_berg) = 724.521393 833.012689 0.000000 2021-02-12T23:59:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:59:23Z- LcmTrn::publishEstimates() - MLE : 4058334.08 591814.09 22.58 2021-02-12T23:59:23Z- LcmTrn::publishEstimates() - MMSE : 4058338.58 591848.59 22.58 2021-02-12T23:59:23Z- LcmTrn::publishEstimates() - COVAR: 724.52 833.01 1.68 0.00 2021-02-12T23:59:23Z- AHRS_M2 msg: 1613174363.82 epoch sec; seqNo:8593 2021-02-12T23:59:23Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.18 psi 2021-02-12T23:59:23Z- handleDepth()-Depth_Keller msg: 1613174363833 epoch sec; seqNo:8663; depth 22.73 2021-02-12T23:59:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174363.85 epoch sec; seqNo:8663; 4058338.58 north; 591848.59 east 2021-02-12T23:59:24Z- AHRS_M2 msg: 1613174364.22 epoch sec; seqNo:8594 2021-02-12T23:59:24Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.17 psi 2021-02-12T23:59:24Z- handleDepth()-Depth_Keller msg: 1613174364229 epoch sec; seqNo:8664; depth 22.86 2021-02-12T23:59:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174364.24 epoch sec; seqNo:8664; 4058338.96 north; 591848.59 east 2021-02-12T23:59:24Z- AHRS_M2 msg: 1613174364.63 epoch sec; seqNo:8595 2021-02-12T23:59:24Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.17 psi 2021-02-12T23:59:24Z- handleDepth()-Depth_Keller msg: 1613174364632 epoch sec; seqNo:8665; depth 22.99 2021-02-12T23:59:24Z- handleDvl() - RDI_Pathfinder msg: 1613174364.65 epoch sec; seqNo:4140 2021-02-12T23:59:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 97.37 , 67.30 , 106.30 2021-02-12T23:59:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.02 , 0.01 2021-02-12T23:59:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174364.68 epoch sec; seqNo:8665; 4058339.34 north; 591848.58 east 2021-02-12T23:59:25Z- AHRS_M2 msg: 1613174365.03 epoch sec; seqNo:8596 2021-02-12T23:59:25Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.17 psi 2021-02-12T23:59:25Z- handleDepth()-Depth_Keller msg: 1613174365036 epoch sec; seqNo:8666; depth 23.12 2021-02-12T23:59:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174365.05 epoch sec; seqNo:8666; 4058339.34 north; 591848.58 east 2021-02-12T23:59:25Z- AHRS_M2 msg: 1613174365.44 epoch sec; seqNo:8597 2021-02-12T23:59:25Z- AHRS_M2 msg: 0.69 phi; -0.36 theta; -0.17 psi 2021-02-12T23:59:25Z- handleDepth()-Depth_Keller msg: 1613174365442 epoch sec; seqNo:8667; depth 23.25 2021-02-12T23:59:25Z- handleDvl() - RDI_Pathfinder msg: 1613174365.45 epoch sec; seqNo:4141 2021-02-12T23:59:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.57 , 90.68 , 65.81 , 104.07 2021-02-12T23:59:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , 0.04 2021-02-12T23:59:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174365.46 epoch sec; seqNo:8667; 4058339.72 north; 591848.58 east 2021-02-12T23:59:25Z- AHRS_M2 msg: 1613174365.84 epoch sec; seqNo:8598 2021-02-12T23:59:25Z- AHRS_M2 msg: 0.69 phi; -0.36 theta; -0.17 psi 2021-02-12T23:59:25Z- handleDepth()-Depth_Keller msg: 1613174365844 epoch sec; seqNo:8668; depth 23.38 2021-02-12T23:59:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:25Z- handleDvl() - RDI_Pathfinder msg: 1613174365.85 epoch sec; seqNo:4142 2021-02-12T23:59:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , 0.04 2021-02-12T23:59:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174365.87 epoch sec; seqNo:8668; 4058340.11 north; 591849.79 east 2021-02-12T23:59:26Z- AHRS_M2 msg: 1613174366.24 epoch sec; seqNo:8599 2021-02-12T23:59:26Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.17 psi 2021-02-12T23:59:26Z- handleDepth()-Depth_Keller msg: 1613174366248 epoch sec; seqNo:8669; depth 23.49 2021-02-12T23:59:26Z- handleDvl() - RDI_Pathfinder msg: 1613174366.26 epoch sec; seqNo:4143 2021-02-12T23:59:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.98 , 0.00 , 65.81 , 109.28 2021-02-12T23:59:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , 0.04 2021-02-12T23:59:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174366.27 epoch sec; seqNo:8669; 4058340.11 north; 591849.79 east 2021-02-12T23:59:26Z- AHRS_M2 msg: 1613174366.65 epoch sec; seqNo:8600 2021-02-12T23:59:26Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.16 psi 2021-02-12T23:59:26Z- handleDepth()-Depth_Keller msg: 1613174366661 epoch sec; seqNo:8670; depth 23.63 2021-02-12T23:59:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174366.68 epoch sec; seqNo:8670; 4058340.49 north; 591849.79 east 2021-02-12T23:59:27Z- AHRS_M2 msg: 1613174367.05 epoch sec; seqNo:8601 2021-02-12T23:59:27Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.16 psi 2021-02-12T23:59:27Z- handleDepth()-Depth_Keller msg: 1613174367058 epoch sec; seqNo:8671; depth 23.77 2021-02-12T23:59:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:27Z- handleDvl() - RDI_Pathfinder msg: 1613174367.06 epoch sec; seqNo:4144 2021-02-12T23:59:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , 0.04 2021-02-12T23:59:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174367.07 epoch sec; seqNo:8671; 4058340.49 north; 591849.79 east 2021-02-12T23:59:27Z- AHRS_M2 msg: 1613174367.45 epoch sec; seqNo:8602 2021-02-12T23:59:27Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.16 psi 2021-02-12T23:59:27Z- handleDepth()-Depth_Keller msg: 1613174367460 epoch sec; seqNo:8672; depth 23.90 2021-02-12T23:59:27Z- handleDvl() - RDI_Pathfinder msg: 1613174367.48 epoch sec; seqNo:4145 2021-02-12T23:59:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 88.65 , 65.40 , 104.48 2021-02-12T23:59:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.09 , 0.03 2021-02-12T23:59:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174367.49 epoch sec; seqNo:8672; 4058340.86 north; 591849.79 east 2021-02-12T23:59:27Z- AHRS_M2 msg: 1613174367.86 epoch sec; seqNo:8603 2021-02-12T23:59:27Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.16 psi 2021-02-12T23:59:27Z- handleDepth()-Depth_Keller msg: 1613174367864 epoch sec; seqNo:8673; depth 24.04 2021-02-12T23:59:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174367.88 epoch sec; seqNo:8673; 4058341.26 north; 591851.00 east 2021-02-12T23:59:28Z- AHRS_M2 msg: 1613174368.26 epoch sec; seqNo:8604 2021-02-12T23:59:28Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.17 psi 2021-02-12T23:59:28Z- handleDepth()-Depth_Keller msg: 1613174368277 epoch sec; seqNo:8674; depth 24.14 2021-02-12T23:59:28Z- handleDvl() - RDI_Pathfinder msg: 1613174368.29 epoch sec; seqNo:4146 2021-02-12T23:59:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.78 , 89.74 , 64.99 , 106.78 2021-02-12T23:59:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.12 , -0.02 2021-02-12T23:59:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174368.30 epoch sec; seqNo:8674; 4058341.26 north; 591851.00 east 2021-02-12T23:59:28Z- LcmTrn::handleMessages() - handling 43 messages took 0.68 ms and 130 calls... 2021-02-12T23:59:28Z- waiting for 1613174366.45, time is 1613174368.30 2021-02-12T23:59:28Z- LcmTrn::updateTrn() equating pose 1613174368.30 and meas times 1613174368.30 2021-02-12T23:59:28Z- LcmTrn::updateTrn() >>>> pose time 1613174368.30, meas time 1613174368.30 <<<< 2021-02-12T23:59:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174368.30 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:59:28Z- TerrainNav::motionUpdate() - currEstimate = (4058341, 591851.00, 24.14) 2021-02-12T23:59:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:59:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:59:28Z- TNavBF::Projecting Measurements 2021-02-12T23:59:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:59:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:59:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:59:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:59:28Z- TNavFilter::4 good beams. 2021-02-12T23:59:28Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:59:28Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:59:28Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:59:28Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:59:28Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:59:28Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:59:28Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:59:28Z- TNavBF::mmse selected from filter -1 2021-02-12T23:59:28Z- TerrainMapOctree: (vehN, vehE ) = (4058341.26, 591851.00). 2021-02-12T23:59:28Z- TerrainMapOctree: Min Distance = 3770.41. 2021-02-12T23:59:28Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:59:28Z- There is no specific good beam that all particles have in common. 2021-02-12T23:59:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:59:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.86 K: 0.138 ExpectedMeasDiff: nan currMeas.covariance: 9.93 2021-02-12T23:59:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:59:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.60 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 12.88 2021-02-12T23:59:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:59:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.45 K: 0.146 ExpectedMeasDiff: nan currMeas.covariance: 6.76 2021-02-12T23:59:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:59:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 18.24 2021-02-12T23:59:28Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:59:28Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:59:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:59:28Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:59:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:59:28Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:59:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:59:28Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:59:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:59:28Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:59:28Z- Filter 0 No beams used in subcloud update 2021-02-12T23:59:28Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:59:28Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:59:28Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 724.5 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-12T23:59:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174368.30 sec. 2021-02-12T23:59:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:59:28Z- TNavBF::mmse selected from filter -1 2021-02-12T23:59:28Z- variances([0], [2], and psi_berg) = 724.453942 833.012643 0.000000 2021-02-12T23:59:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:59:28Z- LcmTrn::publishEstimates() - MLE : 4058336.76 591816.50 24.14 2021-02-12T23:59:28Z- LcmTrn::publishEstimates() - MMSE : 4058341.26 591851.00 24.14 2021-02-12T23:59:28Z- LcmTrn::publishEstimates() - COVAR: 724.45 833.01 1.16 0.00 2021-02-12T23:59:28Z- AHRS_M2 msg: 1613174368.67 epoch sec; seqNo:8605 2021-02-12T23:59:28Z- AHRS_M2 msg: 0.70 phi; -0.38 theta; -0.17 psi 2021-02-12T23:59:28Z- handleDepth()-Depth_Keller msg: 1613174368672 epoch sec; seqNo:8675; depth 24.29 2021-02-12T23:59:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174368.69 epoch sec; seqNo:8675; 4058341.64 north; 591851.00 east 2021-02-12T23:59:29Z- AHRS_M2 msg: 1613174369.09 epoch sec; seqNo:8606 2021-02-12T23:59:29Z- AHRS_M2 msg: 0.70 phi; -0.38 theta; -0.18 psi 2021-02-12T23:59:29Z- handleDepth()-Depth_Keller msg: 1613174369101 epoch sec; seqNo:8676; depth 24.43 2021-02-12T23:59:29Z- handleDvl() - RDI_Pathfinder msg: 1613174369.13 epoch sec; seqNo:4147 2021-02-12T23:59:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 90.89 , 64.17 , 105.49 2021-02-12T23:59:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.10 , -0.08 2021-02-12T23:59:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174369.14 epoch sec; seqNo:8676; 4058342.01 north; 591851.00 east 2021-02-12T23:59:29Z- AHRS_M2 msg: 1613174369.48 epoch sec; seqNo:8607 2021-02-12T23:59:29Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.19 psi 2021-02-12T23:59:29Z- handleDepth()-Depth_Keller msg: 1613174369482 epoch sec; seqNo:8677; depth 24.58 2021-02-12T23:59:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:29Z- handleDvl() - RDI_Pathfinder msg: 1613174369.48 epoch sec; seqNo:4148 2021-02-12T23:59:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.10 , -0.08 2021-02-12T23:59:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174369.49 epoch sec; seqNo:8677; 4058342.01 north; 591851.00 east 2021-02-12T23:59:29Z- AHRS_M2 msg: 1613174369.88 epoch sec; seqNo:8608 2021-02-12T23:59:29Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.20 psi 2021-02-12T23:59:29Z- handleDepth()-Depth_Keller msg: 1613174369884 epoch sec; seqNo:8678; depth 24.68 2021-02-12T23:59:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174369.90 epoch sec; seqNo:8678; 4058342.41 north; 591852.21 east 2021-02-12T23:59:30Z- AHRS_M2 msg: 1613174370.28 epoch sec; seqNo:8609 2021-02-12T23:59:30Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.21 psi 2021-02-12T23:59:30Z- handleDepth()-Depth_Keller msg: 1613174370288 epoch sec; seqNo:8679; depth 24.81 2021-02-12T23:59:30Z- handleDvl() - RDI_Pathfinder msg: 1613174370.34 epoch sec; seqNo:4149 2021-02-12T23:59:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.94 , 98.32 , 65.81 , 104.41 2021-02-12T23:59:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , -0.13 2021-02-12T23:59:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174370.35 epoch sec; seqNo:8679; 4058342.79 north; 591852.21 east 2021-02-12T23:59:30Z- AHRS_M2 msg: 1613174370.70 epoch sec; seqNo:8610 2021-02-12T23:59:30Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.23 psi 2021-02-12T23:59:30Z- handleDepth()-Depth_Keller msg: 1613174370705 epoch sec; seqNo:8680; depth 24.96 2021-02-12T23:59:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:30Z- handleDvl() - RDI_Pathfinder msg: 1613174370.71 epoch sec; seqNo:4150 2021-02-12T23:59:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , -0.13 2021-02-12T23:59:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174370.72 epoch sec; seqNo:8680; 4058343.16 north; 591852.20 east 2021-02-12T23:59:31Z- AHRS_M2 msg: 1613174371.11 epoch sec; seqNo:8611 2021-02-12T23:59:31Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.24 psi 2021-02-12T23:59:31Z- handleDepth()-Depth_Keller msg: 1613174371110 epoch sec; seqNo:8681; depth 25.09 2021-02-12T23:59:31Z- handleDvl() - RDI_Pathfinder msg: 1613174371.12 epoch sec; seqNo:4151 2021-02-12T23:59:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.71 , 98.19 , 64.58 , 104.27 2021-02-12T23:59:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.09 , -0.12 2021-02-12T23:59:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174371.13 epoch sec; seqNo:8681; 4058343.16 north; 591852.20 east 2021-02-12T23:59:31Z- AHRS_M2 msg: 1613174371.50 epoch sec; seqNo:8612 2021-02-12T23:59:31Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.24 psi 2021-02-12T23:59:31Z- handleDepth()-Depth_Keller msg: 1613174371502 epoch sec; seqNo:8682; depth 25.22 2021-02-12T23:59:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174371.52 epoch sec; seqNo:8682; 4058343.54 north; 591852.20 east 2021-02-12T23:59:31Z- AHRS_M2 msg: 1613174371.90 epoch sec; seqNo:8613 2021-02-12T23:59:31Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.25 psi 2021-02-12T23:59:31Z- handleDepth()-Depth_Keller msg: 1613174371909 epoch sec; seqNo:8683; depth 25.34 2021-02-12T23:59:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:31Z- handleDvl() - RDI_Pathfinder msg: 1613174371.91 epoch sec; seqNo:4152 2021-02-12T23:59:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.09 , -0.12 2021-02-12T23:59:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174371.92 epoch sec; seqNo:8683; 4058343.92 north; 591852.20 east 2021-02-12T23:59:32Z- AHRS_M2 msg: 1613174372.30 epoch sec; seqNo:8614 2021-02-12T23:59:32Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.25 psi 2021-02-12T23:59:32Z- handleDepth()-Depth_Keller msg: 1613174372308 epoch sec; seqNo:8684; depth 25.47 2021-02-12T23:59:32Z- handleDvl() - RDI_Pathfinder msg: 1613174372.32 epoch sec; seqNo:4153 2021-02-12T23:59:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 98.26 , 64.99 , 101.91 2021-02-12T23:59:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.17 , -0.04 2021-02-12T23:59:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174372.33 epoch sec; seqNo:8684; 4058344.30 north; 591852.19 east 2021-02-12T23:59:32Z- AHRS_M2 msg: 1613174372.71 epoch sec; seqNo:8615 2021-02-12T23:59:32Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.25 psi 2021-02-12T23:59:32Z- handleDepth()-Depth_Keller msg: 1613174372718 epoch sec; seqNo:8685; depth 25.60 2021-02-12T23:59:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174372.73 epoch sec; seqNo:8685; 4058344.69 north; 591853.41 east 2021-02-12T23:59:33Z- AHRS_M2 msg: 1613174373.11 epoch sec; seqNo:8616 2021-02-12T23:59:33Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.25 psi 2021-02-12T23:59:33Z- handleDepth()-Depth_Keller msg: 1613174373128 epoch sec; seqNo:8686; depth 25.72 2021-02-12T23:59:33Z- handleDvl() - RDI_Pathfinder msg: 1613174373.14 epoch sec; seqNo:4154 2021-02-12T23:59:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 98.32 , 64.99 , 101.97 2021-02-12T23:59:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.19 , 0.04 2021-02-12T23:59:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174373.15 epoch sec; seqNo:8686; 4058345.07 north; 591853.40 east 2021-02-12T23:59:33Z- AHRS_M2 msg: 1613174373.52 epoch sec; seqNo:8617 2021-02-12T23:59:33Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.24 psi 2021-02-12T23:59:33Z- handleDepth()-Depth_Keller msg: 1613174373520 epoch sec; seqNo:8687; depth 25.85 2021-02-12T23:59:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174373.53 epoch sec; seqNo:8687; 4058345.07 north; 591853.40 east 2021-02-12T23:59:33Z- LcmTrn::handleMessages() - handling 47 messages took 0.65 ms and 134 calls... 2021-02-12T23:59:33Z- waiting for 1613174371.30, time is 1613174373.53 2021-02-12T23:59:33Z- LcmTrn::updateTrn() >>>> pose time 1613174373.53, meas time 1613174373.14 <<<< 2021-02-12T23:59:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174373.14 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:59:33Z- TerrainNav::motionUpdate() - currEstimate = (4058345, 591853.40, 25.85) 2021-02-12T23:59:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:59:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:59:33Z- TNavBF::Projecting Measurements 2021-02-12T23:59:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:59:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:59:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:59:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:59:33Z- TNavFilter::4 good beams. 2021-02-12T23:59:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:59:33Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:59:33Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:59:33Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:59:33Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:59:33Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:59:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:59:33Z- TNavBF::mmse selected from filter -1 2021-02-12T23:59:33Z- TerrainMapOctree: (vehN, vehE ) = (4058344.79, 591853.23). 2021-02-12T23:59:33Z- TerrainMapOctree: Min Distance = 3766.27. 2021-02-12T23:59:33Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:59:33Z- There is no specific good beam that all particles have in common. 2021-02-12T23:59:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:59:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.128 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-12T23:59:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:59:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 15.47 2021-02-12T23:59:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:59:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.117 ExpectedMeasDiff: nan currMeas.covariance: 6.76 2021-02-12T23:59:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:59:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 16.64 2021-02-12T23:59:33Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:59:33Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:59:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:59:33Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:59:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:59:33Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:59:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:59:33Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:59:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:59:33Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:59:33Z- Filter 0 No beams used in subcloud update 2021-02-12T23:59:33Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:59:33Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:59:33Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 724.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-12T23:59:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174373.14 sec. 2021-02-12T23:59:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:59:33Z- TNavBF::mmse selected from filter -1 2021-02-12T23:59:33Z- variances([0], [2], and psi_berg) = 724.360887 833.012579 0.000000 2021-02-12T23:59:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:59:33Z- LcmTrn::publishEstimates() - MLE : 4058340.57 591818.90 25.85 2021-02-12T23:59:33Z- LcmTrn::publishEstimates() - MMSE : 4058345.07 591853.40 25.85 2021-02-12T23:59:33Z- LcmTrn::publishEstimates() - COVAR: 724.36 833.01 0.96 0.00 2021-02-12T23:59:33Z- AHRS_M2 msg: 1613174373.92 epoch sec; seqNo:8618 2021-02-12T23:59:33Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.24 psi 2021-02-12T23:59:33Z- handleDepth()-Depth_Keller msg: 1613174373924 epoch sec; seqNo:8688; depth 25.98 2021-02-12T23:59:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174373.94 epoch sec; seqNo:8688; 4058345.45 north; 591853.40 east 2021-02-12T23:59:34Z- AHRS_M2 msg: 1613174374.33 epoch sec; seqNo:8619 2021-02-12T23:59:34Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.23 psi 2021-02-12T23:59:34Z- handleDepth()-Depth_Keller msg: 1613174374329 epoch sec; seqNo:8689; depth 26.11 2021-02-12T23:59:34Z- handleDvl() - RDI_Pathfinder msg: 1613174374.34 epoch sec; seqNo:4155 2021-02-12T23:59:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 98.32 , 0.00 , 99.54 2021-02-12T23:59:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.09 , 0.09 2021-02-12T23:59:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174374.35 epoch sec; seqNo:8689; 4058345.83 north; 591853.40 east 2021-02-12T23:59:34Z- AHRS_M2 msg: 1613174374.73 epoch sec; seqNo:8620 2021-02-12T23:59:34Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.22 psi 2021-02-12T23:59:34Z- handleDepth()-Depth_Keller msg: 1613174374732 epoch sec; seqNo:8690; depth 26.24 2021-02-12T23:59:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174374.75 epoch sec; seqNo:8690; 4058346.21 north; 591853.39 east 2021-02-12T23:59:35Z- AHRS_M2 msg: 1613174375.15 epoch sec; seqNo:8621 2021-02-12T23:59:35Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.21 psi 2021-02-12T23:59:35Z- handleDepth()-Depth_Keller msg: 1613174375165 epoch sec; seqNo:8691; depth 26.34 2021-02-12T23:59:35Z- handleDvl() - RDI_Pathfinder msg: 1613174375.18 epoch sec; seqNo:4156 2021-02-12T23:59:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 89.33 , 62.53 , 101.50 2021-02-12T23:59:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , 0.11 2021-02-12T23:59:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174375.20 epoch sec; seqNo:8691; 4058346.21 north; 591853.39 east 2021-02-12T23:59:35Z- AHRS_M2 msg: 1613174375.54 epoch sec; seqNo:8622 2021-02-12T23:59:35Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.20 psi 2021-02-12T23:59:35Z- handleDepth()-Depth_Keller msg: 1613174375539 epoch sec; seqNo:8692; depth 26.48 2021-02-12T23:59:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:35Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:35Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:35Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:35Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:35Z- handleDvl() - RDI_Pathfinder msg: 1613174375.54 epoch sec; seqNo:4157 2021-02-12T23:59:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , 0.11 2021-02-12T23:59:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174375.56 epoch sec; seqNo:8692; 4058346.59 north; 591853.39 east 2021-02-12T23:59:35Z- AHRS_M2 msg: 1613174375.94 epoch sec; seqNo:8623 2021-02-12T23:59:35Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.20 psi 2021-02-12T23:59:35Z- handleDepth()-Depth_Keller msg: 1613174375945 epoch sec; seqNo:8693; depth 26.59 2021-02-12T23:59:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174375.95 epoch sec; seqNo:8693; 4058346.60 north; 591854.61 east 2021-02-12T23:59:36Z- AHRS_M2 msg: 1613174376.34 epoch sec; seqNo:8624 2021-02-12T23:59:36Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-12T23:59:36Z- handleDepth()-Depth_Keller msg: 1613174376348 epoch sec; seqNo:8694; depth 26.71 2021-02-12T23:59:36Z- handleDvl() - RDI_Pathfinder msg: 1613174376.36 epoch sec; seqNo:4158 2021-02-12T23:59:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 0.00 , 63.35 , 97.92 2021-02-12T23:59:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.05 , 0.09 2021-02-12T23:59:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174376.37 epoch sec; seqNo:8694; 4058346.98 north; 591854.61 east 2021-02-12T23:59:36Z- AHRS_M2 msg: 1613174376.75 epoch sec; seqNo:8625 2021-02-12T23:59:36Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.19 psi 2021-02-12T23:59:36Z- handleDepth()-Depth_Keller msg: 1613174376759 epoch sec; seqNo:8695; depth 26.83 2021-02-12T23:59:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:36Z- handleDvl() - RDI_Pathfinder msg: 1613174376.76 epoch sec; seqNo:4159 2021-02-12T23:59:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.05 , 0.09 2021-02-12T23:59:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174376.77 epoch sec; seqNo:8695; 4058347.36 north; 591854.60 east 2021-02-12T23:59:37Z- AHRS_M2 msg: 1613174377.15 epoch sec; seqNo:8626 2021-02-12T23:59:37Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.19 psi 2021-02-12T23:59:37Z- handleDepth()-Depth_Keller msg: 1613174377158 epoch sec; seqNo:8696; depth 26.95 2021-02-12T23:59:37Z- handleDvl() - RDI_Pathfinder msg: 1613174377.17 epoch sec; seqNo:4160 2021-02-12T23:59:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 0.00 , 62.13 , 97.99 2021-02-12T23:59:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.08 , 0.06 2021-02-12T23:59:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174377.20 epoch sec; seqNo:8696; 4058347.36 north; 591854.60 east 2021-02-12T23:59:37Z- AHRS_M2 msg: 1613174377.56 epoch sec; seqNo:8627 2021-02-12T23:59:37Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.19 psi 2021-02-12T23:59:37Z- handleDepth()-Depth_Keller msg: 1613174377565 epoch sec; seqNo:8697; depth 27.06 2021-02-12T23:59:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174377.58 epoch sec; seqNo:8697; 4058347.74 north; 591854.60 east 2021-02-12T23:59:37Z- AHRS_M2 msg: 1613174377.96 epoch sec; seqNo:8628 2021-02-12T23:59:37Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.19 psi 2021-02-12T23:59:37Z- handleDepth()-Depth_Keller msg: 1613174377965 epoch sec; seqNo:8698; depth 27.18 2021-02-12T23:59:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:37Z- handleDvl() - RDI_Pathfinder msg: 1613174377.97 epoch sec; seqNo:4161 2021-02-12T23:59:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.08 , 0.06 2021-02-12T23:59:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174377.97 epoch sec; seqNo:8698; 4058347.74 north; 591854.60 east 2021-02-12T23:59:38Z- AHRS_M2 msg: 1613174378.36 epoch sec; seqNo:8629 2021-02-12T23:59:38Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.19 psi 2021-02-12T23:59:38Z- handleDepth()-Depth_Keller msg: 1613174378368 epoch sec; seqNo:8699; depth 27.28 2021-02-12T23:59:38Z- handleDvl() - RDI_Pathfinder msg: 1613174378.39 epoch sec; seqNo:4162 2021-02-12T23:59:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.67 , 86.08 , 62.13 , 97.99 2021-02-12T23:59:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , 0.03 2021-02-12T23:59:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174378.40 epoch sec; seqNo:8699; 4058348.13 north; 591855.81 east 2021-02-12T23:59:38Z- AHRS_M2 msg: 1613174378.77 epoch sec; seqNo:8630 2021-02-12T23:59:38Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-12T23:59:38Z- handleDepth()-Depth_Keller msg: 1613174378773 epoch sec; seqNo:8700; depth 27.41 2021-02-12T23:59:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174378.78 epoch sec; seqNo:8700; 4058348.51 north; 591855.81 east 2021-02-12T23:59:38Z- LcmTrn::handleMessages() - handling 47 messages took 0.67 ms and 134 calls... 2021-02-12T23:59:38Z- waiting for 1613174376.53, time is 1613174378.78 2021-02-12T23:59:38Z- LcmTrn::updateTrn() >>>> pose time 1613174378.78, meas time 1613174378.39 <<<< 2021-02-12T23:59:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174378.39 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:59:38Z- TerrainNav::motionUpdate() - currEstimate = (4058349, 591855.81, 27.41) 2021-02-12T23:59:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:59:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:59:38Z- TNavBF::Projecting Measurements 2021-02-12T23:59:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:59:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:59:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:59:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:59:38Z- TNavFilter::4 good beams. 2021-02-12T23:59:38Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:59:38Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:59:38Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:59:38Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:59:38Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:59:38Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:59:38Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:59:38Z- TNavBF::mmse selected from filter -1 2021-02-12T23:59:38Z- TerrainMapOctree: (vehN, vehE ) = (4058348.25, 591855.63). 2021-02-12T23:59:38Z- TerrainMapOctree: Min Distance = 3762.07. 2021-02-12T23:59:38Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:59:38Z- There is no specific good beam that all particles have in common. 2021-02-12T23:59:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:59:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 7.54 2021-02-12T23:59:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:59:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 11.86 2021-02-12T23:59:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:59:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.108 ExpectedMeasDiff: nan currMeas.covariance: 6.18 2021-02-12T23:59:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:59:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-12T23:59:38Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:59:38Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:59:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:59:38Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:59:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:59:38Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:59:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:59:38Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:59:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:59:38Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:59:38Z- Filter 0 No beams used in subcloud update 2021-02-12T23:59:38Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:59:38Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:59:38Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 724.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-12T23:59:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174378.39 sec. 2021-02-12T23:59:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:59:38Z- TNavBF::mmse selected from filter -1 2021-02-12T23:59:38Z- variances([0], [2], and psi_berg) = 724.271102 833.012517 0.000000 2021-02-12T23:59:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:59:38Z- LcmTrn::publishEstimates() - MLE : 4058344.01 591821.31 27.41 2021-02-12T23:59:38Z- LcmTrn::publishEstimates() - MMSE : 4058348.51 591855.81 27.41 2021-02-12T23:59:38Z- LcmTrn::publishEstimates() - COVAR: 724.27 833.01 0.83 0.00 2021-02-12T23:59:39Z- AHRS_M2 msg: 1613174379.17 epoch sec; seqNo:8631 2021-02-12T23:59:39Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-12T23:59:39Z- handleDepth()-Depth_Keller msg: 1613174379181 epoch sec; seqNo:8701; depth 27.53 2021-02-12T23:59:39Z- handleDvl() - RDI_Pathfinder msg: 1613174379.20 epoch sec; seqNo:4163 2021-02-12T23:59:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:39Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:59:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174379.24 epoch sec; seqNo:8701; 4058348.89 north; 591855.81 east 2021-02-12T23:59:39Z- AHRS_M2 msg: 1613174379.58 epoch sec; seqNo:8632 2021-02-12T23:59:39Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-12T23:59:39Z- handleDepth()-Depth_Keller msg: 1613174379586 epoch sec; seqNo:8702; depth 27.67 2021-02-12T23:59:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174379.60 epoch sec; seqNo:8702; 4058348.89 north; 591855.81 east 2021-02-12T23:59:39Z- AHRS_M2 msg: 1613174379.98 epoch sec; seqNo:8633 2021-02-12T23:59:39Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-12T23:59:39Z- handleDepth()-Depth_Keller msg: 1613174379984 epoch sec; seqNo:8703; depth 27.78 2021-02-12T23:59:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174379.99 epoch sec; seqNo:8703; 4058349.26 north; 591855.80 east 2021-02-12T23:59:40Z- AHRS_M2 msg: 1613174380.38 epoch sec; seqNo:8634 2021-02-12T23:59:40Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-12T23:59:40Z- handleDepth()-Depth_Keller msg: 1613174380393 epoch sec; seqNo:8704; depth 27.89 2021-02-12T23:59:40Z- handleDvl() - RDI_Pathfinder msg: 1613174380.40 epoch sec; seqNo:4164 2021-02-12T23:59:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.44 , 86.96 , 61.31 , 97.92 2021-02-12T23:59:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.02 , -0.01 2021-02-12T23:59:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174380.43 epoch sec; seqNo:8704; 4058349.64 north; 591855.80 east 2021-02-12T23:59:40Z- AHRS_M2 msg: 1613174380.79 epoch sec; seqNo:8635 2021-02-12T23:59:40Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-12T23:59:40Z- handleDepth()-Depth_Keller msg: 1613174380795 epoch sec; seqNo:8705; depth 28.03 2021-02-12T23:59:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174380.81 epoch sec; seqNo:8705; 4058349.66 north; 591857.02 east 2021-02-12T23:59:41Z- AHRS_M2 msg: 1613174381.19 epoch sec; seqNo:8636 2021-02-12T23:59:41Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-12T23:59:41Z- handleDepth()-Depth_Keller msg: 1613174381205 epoch sec; seqNo:8706; depth 28.13 2021-02-12T23:59:41Z- handleDvl() - RDI_Pathfinder msg: 1613174381.22 epoch sec; seqNo:4165 2021-02-12T23:59:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.67 , 86.08 , 61.72 , 97.99 2021-02-12T23:59:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , -0.02 2021-02-12T23:59:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174381.23 epoch sec; seqNo:8706; 4058350.04 north; 591857.01 east 2021-02-12T23:59:41Z- AHRS_M2 msg: 1613174381.60 epoch sec; seqNo:8637 2021-02-12T23:59:41Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-12T23:59:41Z- handleDepth()-Depth_Keller msg: 1613174381600 epoch sec; seqNo:8707; depth 28.26 2021-02-12T23:59:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:41Z- handleDvl() - RDI_Pathfinder msg: 1613174381.60 epoch sec; seqNo:4166 2021-02-12T23:59:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , -0.02 2021-02-12T23:59:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174381.63 epoch sec; seqNo:8707; 4058350.41 north; 591857.01 east 2021-02-12T23:59:42Z- AHRS_M2 msg: 1613174382.00 epoch sec; seqNo:8638 2021-02-12T23:59:42Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.18 psi 2021-02-12T23:59:42Z- handleDepth()-Depth_Keller msg: 1613174382004 epoch sec; seqNo:8708; depth 28.38 2021-02-12T23:59:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174382.01 epoch sec; seqNo:8708; 4058350.79 north; 591857.01 east 2021-02-12T23:59:42Z- AHRS_M2 msg: 1613174382.40 epoch sec; seqNo:8639 2021-02-12T23:59:42Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.18 psi 2021-02-12T23:59:42Z- handleDepth()-Depth_Keller msg: 1613174382409 epoch sec; seqNo:8709; depth 28.49 2021-02-12T23:59:42Z- handleDvl() - RDI_Pathfinder msg: 1613174382.42 epoch sec; seqNo:4167 2021-02-12T23:59:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:42Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:59:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174382.43 epoch sec; seqNo:8709; 4058350.79 north; 591857.01 east 2021-02-12T23:59:42Z- AHRS_M2 msg: 1613174382.82 epoch sec; seqNo:8640 2021-02-12T23:59:42Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.18 psi 2021-02-12T23:59:42Z- handleDepth()-Depth_Keller msg: 1613174382825 epoch sec; seqNo:8710; depth 28.61 2021-02-12T23:59:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:42Z- handleDvl() - RDI_Pathfinder msg: 1613174382.83 epoch sec; seqNo:4168 2021-02-12T23:59:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:42Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:59:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174382.84 epoch sec; seqNo:8710; 4058351.17 north; 591857.00 east 2021-02-12T23:59:43Z- AHRS_M2 msg: 1613174383.21 epoch sec; seqNo:8641 2021-02-12T23:59:43Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-12T23:59:43Z- handleDepth()-Depth_Keller msg: 1613174383216 epoch sec; seqNo:8711; depth 28.74 2021-02-12T23:59:43Z- handleDvl() - RDI_Pathfinder msg: 1613174383.23 epoch sec; seqNo:4169 2021-02-12T23:59:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.85 , 84.94 , 61.72 , 97.11 2021-02-12T23:59:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.07 , -0.02 2021-02-12T23:59:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174383.26 epoch sec; seqNo:8711; 4058351.56 north; 591858.22 east 2021-02-12T23:59:43Z- AHRS_M2 msg: 1613174383.62 epoch sec; seqNo:8642 2021-02-12T23:59:43Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.17 psi 2021-02-12T23:59:43Z- handleDepth()-Depth_Keller msg: 1613174383625 epoch sec; seqNo:8712; depth 28.86 2021-02-12T23:59:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174383.63 epoch sec; seqNo:8712; 4058351.94 north; 591858.21 east 2021-02-12T23:59:43Z- LcmTrn::handleMessages() - handling 43 messages took 0.67 ms and 130 calls... 2021-02-12T23:59:43Z- waiting for 1613174381.78, time is 1613174383.63 2021-02-12T23:59:43Z- LcmTrn::updateTrn() >>>> pose time 1613174383.63, meas time 1613174383.23 <<<< 2021-02-12T23:59:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174383.23 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:59:43Z- TerrainNav::motionUpdate() - currEstimate = (4058352, 591858.21, 28.86) 2021-02-12T23:59:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:59:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:59:43Z- TNavBF::Projecting Measurements 2021-02-12T23:59:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-12T23:59:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-12T23:59:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-12T23:59:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-12T23:59:43Z- TNavFilter::4 good beams. 2021-02-12T23:59:43Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:59:43Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:59:43Z- TNavBF:: beamIndex[1] = 1 2021-02-12T23:59:43Z- TNavBF:: beamIndex[2] = 2 2021-02-12T23:59:43Z- TNavBF:: beamIndex[3] = 3 2021-02-12T23:59:43Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:59:43Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:59:43Z- TNavBF::mmse selected from filter -1 2021-02-12T23:59:43Z- TerrainMapOctree: (vehN, vehE ) = (4058351.66, 591858.01). 2021-02-12T23:59:43Z- TerrainMapOctree: Min Distance = 3757.93. 2021-02-12T23:59:43Z- TerrainMapOctree: Using tile 1. 2021-02-12T23:59:43Z- There is no specific good beam that all particles have in common. 2021-02-12T23:59:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-12T23:59:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 7.37 2021-02-12T23:59:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-12T23:59:43Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 11.54 2021-02-12T23:59:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-12T23:59:43Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 6.09 2021-02-12T23:59:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-12T23:59:43Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-12T23:59:43Z- Filter 0 Weighting particles with subcloud comparison 2021-02-12T23:59:43Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:59:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-12T23:59:43Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:59:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-12T23:59:44Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:59:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-12T23:59:44Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-12T23:59:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-12T23:59:44Z- Filter 0 SubcloudNIS: 0.000000 2021-02-12T23:59:44Z- Filter 0 No beams used in subcloud update 2021-02-12T23:59:44Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-12T23:59:44Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-12T23:59:44Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 724.2 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-12T23:59:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174383.23 sec. 2021-02-12T23:59:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:59:44Z- TNavBF::mmse selected from filter -1 2021-02-12T23:59:44Z- variances([0], [2], and psi_berg) = 724.180694 833.012456 0.000000 2021-02-12T23:59:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:59:44Z- LcmTrn::publishEstimates() - MLE : 4058347.44 591823.71 28.86 2021-02-12T23:59:44Z- LcmTrn::publishEstimates() - MMSE : 4058351.94 591858.21 28.86 2021-02-12T23:59:44Z- LcmTrn::publishEstimates() - COVAR: 724.18 833.01 0.76 0.00 2021-02-12T23:59:44Z- AHRS_M2 msg: 1613174384.02 epoch sec; seqNo:8643 2021-02-12T23:59:44Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.17 psi 2021-02-12T23:59:44Z- handleDepth()-Depth_Keller msg: 1613174384026 epoch sec; seqNo:8713; depth 28.98 2021-02-12T23:59:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:44Z- handleDvl() - RDI_Pathfinder msg: 1613174384.03 epoch sec; seqNo:4170 2021-02-12T23:59:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.07 , -0.02 2021-02-12T23:59:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174384.04 epoch sec; seqNo:8713; 4058352.32 north; 591858.21 east 2021-02-12T23:59:44Z- AHRS_M2 msg: 1613174384.42 epoch sec; seqNo:8644 2021-02-12T23:59:44Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.17 psi 2021-02-12T23:59:44Z- handleDepth()-Depth_Keller msg: 1613174384428 epoch sec; seqNo:8714; depth 29.09 2021-02-12T23:59:44Z- handleDvl() - RDI_Pathfinder msg: 1613174384.45 epoch sec; seqNo:4171 2021-02-12T23:59:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.85 , 83.92 , 61.72 , 97.99 2021-02-12T23:59:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.02 2021-02-12T23:59:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174384.46 epoch sec; seqNo:8714; 4058352.32 north; 591858.21 east 2021-02-12T23:59:44Z- AHRS_M2 msg: 1613174384.83 epoch sec; seqNo:8645 2021-02-12T23:59:44Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.17 psi 2021-02-12T23:59:44Z- handleDepth()-Depth_Keller msg: 1613174384833 epoch sec; seqNo:8715; depth 29.20 2021-02-12T23:59:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174384.85 epoch sec; seqNo:8715; 4058352.70 north; 591858.21 east 2021-02-12T23:59:45Z- AHRS_M2 msg: 1613174385.23 epoch sec; seqNo:8646 2021-02-12T23:59:45Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.17 psi 2021-02-12T23:59:45Z- handleDepth()-Depth_Keller msg: 1613174385241 epoch sec; seqNo:8716; depth 29.32 2021-02-12T23:59:45Z- handleDvl() - RDI_Pathfinder msg: 1613174385.25 epoch sec; seqNo:4172 2021-02-12T23:59:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.44 , 83.92 , 60.08 , 97.99 2021-02-12T23:59:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.03 2021-02-12T23:59:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174385.26 epoch sec; seqNo:8716; 4058353.08 north; 591858.20 east 2021-02-12T23:59:45Z- AHRS_M2 msg: 1613174385.63 epoch sec; seqNo:8647 2021-02-12T23:59:45Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.17 psi 2021-02-12T23:59:45Z- handleDepth()-Depth_Keller msg: 1613174385642 epoch sec; seqNo:8717; depth 29.44 2021-02-12T23:59:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174385.65 epoch sec; seqNo:8717; 4058353.46 north; 591858.20 east 2021-02-12T23:59:46Z- AHRS_M2 msg: 1613174386.04 epoch sec; seqNo:8648 2021-02-12T23:59:46Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.17 psi 2021-02-12T23:59:46Z- handleDepth()-Depth_Keller msg: 1613174386044 epoch sec; seqNo:8718; depth 29.57 2021-02-12T23:59:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174386.05 epoch sec; seqNo:8718; 4058353.47 north; 591859.42 east 2021-02-12T23:59:46Z- AHRS_M2 msg: 1613174386.44 epoch sec; seqNo:8649 2021-02-12T23:59:46Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.17 psi 2021-02-12T23:59:46Z- handleDepth()-Depth_Keller msg: 1613174386448 epoch sec; seqNo:8719; depth 29.71 2021-02-12T23:59:46Z- handleDvl() - RDI_Pathfinder msg: 1613174386.46 epoch sec; seqNo:4173 2021-02-12T23:59:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:46Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:59:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174386.47 epoch sec; seqNo:8719; 4058353.85 north; 591859.42 east 2021-02-12T23:59:46Z- AHRS_M2 msg: 1613174386.85 epoch sec; seqNo:8650 2021-02-12T23:59:46Z- AHRS_M2 msg: 0.64 phi; -0.27 theta; -0.17 psi 2021-02-12T23:59:46Z- handleDepth()-Depth_Keller msg: 1613174386854 epoch sec; seqNo:8720; depth 29.83 2021-02-12T23:59:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174386.87 epoch sec; seqNo:8720; 4058354.23 north; 591859.41 east 2021-02-12T23:59:47Z- AHRS_M2 msg: 1613174387.25 epoch sec; seqNo:8651 2021-02-12T23:59:47Z- AHRS_M2 msg: 0.64 phi; -0.23 theta; -0.16 psi 2021-02-12T23:59:47Z- handleDepth()-Depth_Keller msg: 1613174387256 epoch sec; seqNo:8721; depth 29.95 2021-02-12T23:59:47Z- handleDvl() - RDI_Pathfinder msg: 1613174387.28 epoch sec; seqNo:4174 2021-02-12T23:59:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:47Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:59:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174387.29 epoch sec; seqNo:8721; 4058354.61 north; 591859.41 east 2021-02-12T23:59:47Z- AHRS_M2 msg: 1613174387.65 epoch sec; seqNo:8652 2021-02-12T23:59:47Z- AHRS_M2 msg: 0.63 phi; -0.19 theta; -0.16 psi 2021-02-12T23:59:47Z- handleDepth()-Depth_Keller msg: 1613174387667 epoch sec; seqNo:8722; depth 30.09 2021-02-12T23:59:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:47Z- handleDvl() - RDI_Pathfinder msg: 1613174387.67 epoch sec; seqNo:4175 2021-02-12T23:59:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:47Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:59:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174387.69 epoch sec; seqNo:8722; 4058354.99 north; 591859.40 east 2021-02-12T23:59:48Z- AHRS_M2 msg: 1613174388.07 epoch sec; seqNo:8653 2021-02-12T23:59:48Z- AHRS_M2 msg: 0.61 phi; -0.10 theta; -0.15 psi 2021-02-12T23:59:48Z- handleDepth()-Depth_Keller msg: 1613174388084 epoch sec; seqNo:8723; depth 30.19 2021-02-12T23:59:48Z- handleDvl() - RDI_Pathfinder msg: 1613174388.11 epoch sec; seqNo:4176 2021-02-12T23:59:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.35 , 78.92 , 62.94 , 78.92 2021-02-12T23:59:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , 0.02 2021-02-12T23:59:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174388.12 epoch sec; seqNo:8723; 4058354.99 north; 591859.40 east 2021-02-12T23:59:48Z- AHRS_M2 msg: 1613174388.47 epoch sec; seqNo:8654 2021-02-12T23:59:48Z- AHRS_M2 msg: 0.60 phi; -0.05 theta; -0.15 psi 2021-02-12T23:59:48Z- handleDepth()-Depth_Keller msg: 1613174388474 epoch sec; seqNo:8724; depth 30.30 2021-02-12T23:59:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174388.48 epoch sec; seqNo:8724; 4058355.38 north; 591860.62 east 2021-02-12T23:59:48Z- AHRS_M2 msg: 1613174388.87 epoch sec; seqNo:8655 2021-02-12T23:59:48Z- AHRS_M2 msg: 0.60 phi; -0.01 theta; -0.15 psi 2021-02-12T23:59:48Z- handleDepth()-Depth_Keller msg: 1613174388873 epoch sec; seqNo:8725; depth 30.38 2021-02-12T23:59:48Z- handleDvl() - RDI_Pathfinder msg: 1613174388.89 epoch sec; seqNo:4177 2021-02-12T23:59:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.38 , 0.00 , 66.45 , 0.00 2021-02-12T23:59:48Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:59:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174388.90 epoch sec; seqNo:8725; 4058355.76 north; 591860.62 east 2021-02-12T23:59:49Z- LcmTrn::handleMessages() - handling 47 messages took 0.74 ms and 133 calls... 2021-02-12T23:59:49Z- waiting for 1613174386.63, time is 1613174388.90 2021-02-12T23:59:49Z- LcmTrn::updateTrn() equating pose 1613174388.90 and meas times 1613174388.90 2021-02-12T23:59:49Z- LcmTrn::updateTrn() >>>> pose time 1613174388.90, meas time 1613174388.90 <<<< 2021-02-12T23:59:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174388.90 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:59:49Z- TerrainNav::motionUpdate() - currEstimate = (4058356, 591860.62, 30.38) 2021-02-12T23:59:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:59:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:59:49Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:59:49Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:59:49Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174388.90 sec.; No relevant navigation data available 2021-02-12T23:59:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:59:49Z- TNavBF::mmse selected from filter -1 2021-02-12T23:59:49Z- variances([0], [2], and psi_berg) = 724.105569 833.012404 0.000000 2021-02-12T23:59:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:59:49Z- LcmTrn::publishEstimates() - MLE : 4058351.26 591826.12 30.38 2021-02-12T23:59:49Z- LcmTrn::publishEstimates() - MMSE : 4058355.76 591860.62 30.38 2021-02-12T23:59:49Z- LcmTrn::publishEstimates() - COVAR: 724.11 833.01 0.99 0.00 2021-02-12T23:59:49Z- AHRS_M2 msg: 1613174389.27 epoch sec; seqNo:8656 2021-02-12T23:59:49Z- AHRS_M2 msg: 0.59 phi; 0.04 theta; -0.15 psi 2021-02-12T23:59:49Z- handleDepth()-Depth_Keller msg: 1613174389276 epoch sec; seqNo:8726; depth 30.45 2021-02-12T23:59:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174389.29 epoch sec; seqNo:8726; 4058356.14 north; 591860.61 east 2021-02-12T23:59:49Z- AHRS_M2 msg: 1613174389.68 epoch sec; seqNo:8657 2021-02-12T23:59:49Z- AHRS_M2 msg: 0.58 phi; 0.08 theta; -0.15 psi 2021-02-12T23:59:49Z- handleDepth()-Depth_Keller msg: 1613174389680 epoch sec; seqNo:8727; depth 30.49 2021-02-12T23:59:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174389.69 epoch sec; seqNo:8727; 4058356.14 north; 591860.61 east 2021-02-12T23:59:50Z- AHRS_M2 msg: 1613174390.08 epoch sec; seqNo:8658 2021-02-12T23:59:50Z- AHRS_M2 msg: 0.57 phi; 0.11 theta; -0.16 psi 2021-02-12T23:59:50Z- handleDepth()-Depth_Keller msg: 1613174390086 epoch sec; seqNo:8728; depth 30.53 2021-02-12T23:59:50Z- handleDvl() - RDI_Pathfinder msg: 1613174390.10 epoch sec; seqNo:4178 2021-02-12T23:59:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.35 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:50Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:59:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174390.11 epoch sec; seqNo:8728; 4058356.52 north; 591860.61 east 2021-02-12T23:59:50Z- AHRS_M2 msg: 1613174390.49 epoch sec; seqNo:8659 2021-02-12T23:59:50Z- AHRS_M2 msg: 0.56 phi; 0.15 theta; -0.16 psi 2021-02-12T23:59:50Z- handleDepth()-Depth_Keller msg: 1613174390496 epoch sec; seqNo:8729; depth 30.55 2021-02-12T23:59:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174390.52 epoch sec; seqNo:8729; 4058356.89 north; 591860.60 east 2021-02-12T23:59:50Z- AHRS_M2 msg: 1613174390.89 epoch sec; seqNo:8660 2021-02-12T23:59:50Z- AHRS_M2 msg: 0.56 phi; 0.18 theta; -0.16 psi 2021-02-12T23:59:50Z- handleDepth()-Depth_Keller msg: 1613174390897 epoch sec; seqNo:8730; depth 30.54 2021-02-12T23:59:50Z- handleDvl() - RDI_Pathfinder msg: 1613174390.91 epoch sec; seqNo:4179 2021-02-12T23:59:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.79 , 0.00 , 0.00 , 63.35 2021-02-12T23:59:50Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:59:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174390.94 epoch sec; seqNo:8730; 4058357.27 north; 591860.60 east 2021-02-12T23:59:51Z- AHRS_M2 msg: 1613174391.29 epoch sec; seqNo:8661 2021-02-12T23:59:51Z- AHRS_M2 msg: 0.55 phi; 0.21 theta; -0.16 psi 2021-02-12T23:59:51Z- handleDepth()-Depth_Keller msg: 1613174391297 epoch sec; seqNo:8731; depth 30.54 2021-02-12T23:59:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:51Z- handleDvl() - RDI_Pathfinder msg: 1613174391.30 epoch sec; seqNo:4180 2021-02-12T23:59:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:51Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:59:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174391.31 epoch sec; seqNo:8731; 4058357.27 north; 591860.60 east 2021-02-12T23:59:51Z- AHRS_M2 msg: 1613174391.70 epoch sec; seqNo:8662 2021-02-12T23:59:51Z- AHRS_M2 msg: 0.55 phi; 0.23 theta; -0.16 psi 2021-02-12T23:59:51Z- handleDepth()-Depth_Keller msg: 1613174391702 epoch sec; seqNo:8732; depth 30.51 2021-02-12T23:59:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174391.71 epoch sec; seqNo:8732; 4058357.66 north; 591861.82 east 2021-02-12T23:59:52Z- AHRS_M2 msg: 1613174392.10 epoch sec; seqNo:8663 2021-02-12T23:59:52Z- AHRS_M2 msg: 0.54 phi; 0.25 theta; -0.16 psi 2021-02-12T23:59:52Z- handleDepth()-Depth_Keller msg: 1613174392109 epoch sec; seqNo:8733; depth 30.47 2021-02-12T23:59:52Z- handleDvl() - RDI_Pathfinder msg: 1613174392.13 epoch sec; seqNo:4181 2021-02-12T23:59:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 64.68 , 0.00 , 0.00 , 62.02 2021-02-12T23:59:52Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:59:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174392.15 epoch sec; seqNo:8733; 4058358.04 north; 591861.81 east 2021-02-12T23:59:52Z- AHRS_M2 msg: 1613174392.50 epoch sec; seqNo:8664 2021-02-12T23:59:52Z- AHRS_M2 msg: 0.54 phi; 0.27 theta; -0.16 psi 2021-02-12T23:59:52Z- handleDepth()-Depth_Keller msg: 1613174392508 epoch sec; seqNo:8734; depth 30.43 2021-02-12T23:59:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174392.52 epoch sec; seqNo:8734; 4058358.42 north; 591861.81 east 2021-02-12T23:59:52Z- AHRS_M2 msg: 1613174392.91 epoch sec; seqNo:8665 2021-02-12T23:59:52Z- AHRS_M2 msg: 0.54 phi; 0.29 theta; -0.16 psi 2021-02-12T23:59:52Z- handleDepth()-Depth_Keller msg: 1613174392913 epoch sec; seqNo:8735; depth 30.37 2021-02-12T23:59:52Z- handleDvl() - RDI_Pathfinder msg: 1613174392.92 epoch sec; seqNo:4182 2021-02-12T23:59:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:52Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-12T23:59:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174392.93 epoch sec; seqNo:8735; 4058358.80 north; 591861.80 east 2021-02-12T23:59:53Z- AHRS_M2 msg: 1613174393.31 epoch sec; seqNo:8666 2021-02-12T23:59:53Z- AHRS_M2 msg: 0.54 phi; 0.31 theta; -0.16 psi 2021-02-12T23:59:53Z- handleDepth()-Depth_Keller msg: 1613174393317 epoch sec; seqNo:8736; depth 30.30 2021-02-12T23:59:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174393.33 epoch sec; seqNo:8736; 4058358.80 north; 591861.80 east 2021-02-12T23:59:53Z- AHRS_M2 msg: 1613174393.71 epoch sec; seqNo:8667 2021-02-12T23:59:53Z- AHRS_M2 msg: 0.53 phi; 0.32 theta; -0.16 psi 2021-02-12T23:59:53Z- handleDepth()-Depth_Keller msg: 1613174393730 epoch sec; seqNo:8737; depth 30.23 2021-02-12T23:59:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174393.74 epoch sec; seqNo:8737; 4058359.18 north; 591861.80 east 2021-02-12T23:59:54Z- LcmTrn::handleMessages() - handling 41 messages took 0.66 ms and 128 calls... 2021-02-12T23:59:54Z- waiting for 1613174391.90, time is 1613174393.74 2021-02-12T23:59:54Z- LcmTrn::updateTrn() >>>> pose time 1613174393.74, meas time 1613174392.92 <<<< 2021-02-12T23:59:54Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174392.92, ping # 0. 2021-02-12T23:59:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174392.92 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:59:54Z- TerrainNav::motionUpdate() - currEstimate = (4058359, 591861.80, 30.23) 2021-02-12T23:59:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:59:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:59:54Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:59:54Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:59:54Z- TNavBF::Projecting Measurements 2021-02-12T23:59:54Z- TNavFilter::Measurement from time = 1613174392.92 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:59:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:59:54Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:59:54Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:59:54Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:59:54Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:59:54Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:59:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:59:54Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 724.0 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-12T23:59:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:59:54Z- TNavBF::mmse selected from filter -1 2021-02-12T23:59:54Z- variances([0], [2], and psi_berg) = 724.016633 833.012344 0.000000 2021-02-12T23:59:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:59:54Z- LcmTrn::publishEstimates() - MLE : 4058354.68 591827.30 30.23 2021-02-12T23:59:54Z- LcmTrn::publishEstimates() - MMSE : 4058359.18 591861.80 30.23 2021-02-12T23:59:54Z- LcmTrn::publishEstimates() - COVAR: 724.02 833.01 1.17 0.00 2021-02-12T23:59:54Z- AHRS_M2 msg: 1613174394.13 epoch sec; seqNo:8668 2021-02-12T23:59:54Z- AHRS_M2 msg: 0.54 phi; 0.33 theta; -0.15 psi 2021-02-12T23:59:54Z- handleDepth()-Depth_Keller msg: 1613174394137 epoch sec; seqNo:8738; depth 30.14 2021-02-12T23:59:54Z- handleDvl() - RDI_Pathfinder msg: 1613174394.16 epoch sec; seqNo:4183 2021-02-12T23:59:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.62 , 80.68 , 90.41 , 60.08 2021-02-12T23:59:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.05 , -0.01 2021-02-12T23:59:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174394.17 epoch sec; seqNo:8738; 4058359.56 north; 591861.80 east 2021-02-12T23:59:54Z- AHRS_M2 msg: 1613174394.52 epoch sec; seqNo:8669 2021-02-12T23:59:54Z- AHRS_M2 msg: 0.54 phi; 0.34 theta; -0.15 psi 2021-02-12T23:59:54Z- handleDepth()-Depth_Keller msg: 1613174394528 epoch sec; seqNo:8739; depth 30.04 2021-02-12T23:59:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174394.54 epoch sec; seqNo:8739; 4058359.95 north; 591863.01 east 2021-02-12T23:59:54Z- AHRS_M2 msg: 1613174394.93 epoch sec; seqNo:8670 2021-02-12T23:59:54Z- AHRS_M2 msg: 0.54 phi; 0.35 theta; -0.14 psi 2021-02-12T23:59:54Z- handleDepth()-Depth_Keller msg: 1613174394941 epoch sec; seqNo:8740; depth 29.94 2021-02-12T23:59:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:54Z- handleDvl() - RDI_Pathfinder msg: 1613174394.94 epoch sec; seqNo:4184 2021-02-12T23:59:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.05 , -0.01 2021-02-12T23:59:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174394.95 epoch sec; seqNo:8740; 4058359.95 north; 591863.01 east 2021-02-12T23:59:55Z- AHRS_M2 msg: 1613174395.34 epoch sec; seqNo:8671 2021-02-12T23:59:55Z- AHRS_M2 msg: 0.55 phi; 0.36 theta; -0.14 psi 2021-02-12T23:59:55Z- handleDepth()-Depth_Keller msg: 1613174395351 epoch sec; seqNo:8741; depth 29.83 2021-02-12T23:59:55Z- handleDvl() - RDI_Pathfinder msg: 1613174395.36 epoch sec; seqNo:4185 2021-02-12T23:59:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.26 , 81.76 , 97.99 , 61.31 2021-02-12T23:59:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.05 , -0.03 2021-02-12T23:59:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174395.37 epoch sec; seqNo:8741; 4058360.33 north; 591863.01 east 2021-02-12T23:59:55Z- AHRS_M2 msg: 1613174395.74 epoch sec; seqNo:8672 2021-02-12T23:59:55Z- AHRS_M2 msg: 0.55 phi; 0.37 theta; -0.13 psi 2021-02-12T23:59:55Z- handleDepth()-Depth_Keller msg: 1613174395740 epoch sec; seqNo:8742; depth 29.71 2021-02-12T23:59:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174395.75 epoch sec; seqNo:8742; 4058360.71 north; 591863.00 east 2021-02-12T23:59:56Z- AHRS_M2 msg: 1613174396.14 epoch sec; seqNo:8673 2021-02-12T23:59:56Z- AHRS_M2 msg: 0.55 phi; 0.37 theta; -0.13 psi 2021-02-12T23:59:56Z- handleDepth()-Depth_Keller msg: 1613174396150 epoch sec; seqNo:8743; depth 29.62 2021-02-12T23:59:56Z- handleDvl() - RDI_Pathfinder msg: 1613174396.16 epoch sec; seqNo:4186 2021-02-12T23:59:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 81.76 , 97.99 , 60.90 2021-02-12T23:59:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.03 , -0.02 2021-02-12T23:59:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174396.19 epoch sec; seqNo:8743; 4058360.71 north; 591863.00 east 2021-02-12T23:59:56Z- AHRS_M2 msg: 1613174396.55 epoch sec; seqNo:8674 2021-02-12T23:59:56Z- AHRS_M2 msg: 0.56 phi; 0.37 theta; -0.13 psi 2021-02-12T23:59:56Z- handleDepth()-Depth_Keller msg: 1613174396549 epoch sec; seqNo:8744; depth 29.49 2021-02-12T23:59:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174396.56 epoch sec; seqNo:8744; 4058361.09 north; 591863.00 east 2021-02-12T23:59:56Z- AHRS_M2 msg: 1613174396.95 epoch sec; seqNo:8675 2021-02-12T23:59:56Z- AHRS_M2 msg: 0.56 phi; 0.38 theta; -0.13 psi 2021-02-12T23:59:56Z- handleDepth()-Depth_Keller msg: 1613174396962 epoch sec; seqNo:8745; depth 29.38 2021-02-12T23:59:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174396.97 epoch sec; seqNo:8745; 4058361.47 north; 591863.00 east 2021-02-12T23:59:57Z- AHRS_M2 msg: 1613174397.35 epoch sec; seqNo:8676 2021-02-12T23:59:57Z- AHRS_M2 msg: 0.57 phi; 0.38 theta; -0.13 psi 2021-02-12T23:59:57Z- handleDepth()-Depth_Keller msg: 1613174397361 epoch sec; seqNo:8746; depth 29.25 2021-02-12T23:59:57Z- handleDvl() - RDI_Pathfinder msg: 1613174397.38 epoch sec; seqNo:4187 2021-02-12T23:59:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 82.84 , 97.99 , 60.08 2021-02-12T23:59:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , -0.04 2021-02-12T23:59:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174397.39 epoch sec; seqNo:8746; 4058361.48 north; 591864.22 east 2021-02-12T23:59:57Z- AHRS_M2 msg: 1613174397.76 epoch sec; seqNo:8677 2021-02-12T23:59:57Z- AHRS_M2 msg: 0.57 phi; 0.38 theta; -0.13 psi 2021-02-12T23:59:57Z- handleDepth()-Depth_Keller msg: 1613174397760 epoch sec; seqNo:8747; depth 29.10 2021-02-12T23:59:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174397.77 epoch sec; seqNo:8747; 4058361.86 north; 591864.21 east 2021-02-12T23:59:58Z- AHRS_M2 msg: 1613174398.16 epoch sec; seqNo:8678 2021-02-12T23:59:58Z- AHRS_M2 msg: 0.57 phi; 0.38 theta; -0.13 psi 2021-02-12T23:59:58Z- handleDepth()-Depth_Keller msg: 1613174398165 epoch sec; seqNo:8748; depth 29.00 2021-02-12T23:59:58Z- handleDvl() - RDI_Pathfinder msg: 1613174398.18 epoch sec; seqNo:4188 2021-02-12T23:59:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.30 , 83.92 , 97.99 , 61.72 2021-02-12T23:59:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , -0.02 2021-02-12T23:59:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174398.19 epoch sec; seqNo:8748; 4058362.24 north; 591864.21 east 2021-02-12T23:59:58Z- AHRS_M2 msg: 1613174398.56 epoch sec; seqNo:8679 2021-02-12T23:59:58Z- AHRS_M2 msg: 0.58 phi; 0.38 theta; -0.13 psi 2021-02-12T23:59:58Z- handleDepth()-Depth_Keller msg: 1613174398568 epoch sec; seqNo:8749; depth 28.85 2021-02-12T23:59:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:58Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:58Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:58Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:58Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:58Z- handleDvl() - RDI_Pathfinder msg: 1613174398.57 epoch sec; seqNo:4189 2021-02-12T23:59:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , -0.02 2021-02-12T23:59:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174398.59 epoch sec; seqNo:8749; 4058362.24 north; 591864.21 east 2021-02-12T23:59:58Z- AHRS_M2 msg: 1613174398.97 epoch sec; seqNo:8680 2021-02-12T23:59:58Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.13 psi 2021-02-12T23:59:58Z- handleDepth()-Depth_Keller msg: 1613174398981 epoch sec; seqNo:8750; depth 28.73 2021-02-12T23:59:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174399.00 epoch sec; seqNo:8750; 4058362.62 north; 591864.20 east 2021-02-12T23:59:59Z- LcmTrn::handleMessages() - handling 46 messages took 0.70 ms and 132 calls... 2021-02-12T23:59:59Z- waiting for 1613174396.74, time is 1613174399.00 2021-02-12T23:59:59Z- LcmTrn::updateTrn() >>>> pose time 1613174399.00, meas time 1613174398.57 <<<< 2021-02-12T23:59:59Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174398.57, ping # 0. 2021-02-12T23:59:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174398.57 sec, ping # 0; waiting for more recent INS data... 2021-02-12T23:59:59Z- TerrainNav::motionUpdate() - currEstimate = (4058363, 591864.20, 28.73) 2021-02-12T23:59:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-12T23:59:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-12T23:59:59Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-12T23:59:59Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-12T23:59:59Z- TNavBF::Projecting Measurements 2021-02-12T23:59:59Z- TNavFilter::Measurement from time = 1613174398.57 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-12T23:59:59Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-12T23:59:59Z- TNavBF:: beamIndex[0] = 0 2021-02-12T23:59:59Z- TNavBF:: beamIndex[1] = 0 2021-02-12T23:59:59Z- TNavBF:: beamIndex[2] = 0 2021-02-12T23:59:59Z- TNavBF:: beamIndex[3] = 0 2021-02-12T23:59:59Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-12T23:59:59Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-12T23:59:59Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 723.9 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-12T23:59:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-12T23:59:59Z- TNavBF::mmse selected from filter -1 2021-02-12T23:59:59Z- variances([0], [2], and psi_berg) = 723.926634 833.012282 0.000000 2021-02-12T23:59:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-12T23:59:59Z- LcmTrn::publishEstimates() - MLE : 4058358.12 591829.70 28.73 2021-02-12T23:59:59Z- LcmTrn::publishEstimates() - MMSE : 4058362.62 591864.20 28.73 2021-02-12T23:59:59Z- LcmTrn::publishEstimates() - COVAR: 723.93 833.01 1.39 0.00 2021-02-12T23:59:59Z- AHRS_M2 msg: 1613174399.37 epoch sec; seqNo:8681 2021-02-12T23:59:59Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.13 psi 2021-02-12T23:59:59Z- handleDepth()-Depth_Keller msg: 1613174399376 epoch sec; seqNo:8751; depth 28.59 2021-02-12T23:59:59Z- handleDvl() - RDI_Pathfinder msg: 1613174399.39 epoch sec; seqNo:4190 2021-02-12T23:59:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.71 , 83.92 , 97.99 , 61.72 2021-02-12T23:59:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.01 2021-02-12T23:59:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174399.40 epoch sec; seqNo:8751; 4058362.99 north; 591864.20 east 2021-02-12T23:59:59Z- AHRS_M2 msg: 1613174399.78 epoch sec; seqNo:8682 2021-02-12T23:59:59Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.13 psi 2021-02-12T23:59:59Z- handleDepth()-Depth_Keller msg: 1613174399782 epoch sec; seqNo:8752; depth 28.45 2021-02-12T23:59:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-12T23:59:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-12T23:59:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-12T23:59:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-12T23:59:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-12T23:59:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-12T23:59:59Z- handleDvl() - RDI_Pathfinder msg: 1613174399.78 epoch sec; seqNo:4191 2021-02-12T23:59:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-12T23:59:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.01 2021-02-12T23:59:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174399.80 epoch sec; seqNo:8752; 4058362.99 north; 591864.20 east 2021-02-13T00:00:00Z- AHRS_M2 msg: 1613174400.18 epoch sec; seqNo:8683 2021-02-13T00:00:00Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.13 psi 2021-02-13T00:00:00Z- handleDepth()-Depth_Keller msg: 1613174400185 epoch sec; seqNo:8753; depth 28.33 2021-02-13T00:00:00Z- handleDvl() - RDI_Pathfinder msg: 1613174400.20 epoch sec; seqNo:4192 2021-02-13T00:00:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.71 , 81.76 , 99.07 , 61.72 2021-02-13T00:00:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , 0.01 2021-02-13T00:00:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174400.21 epoch sec; seqNo:8753; 4058363.37 north; 591864.20 east 2021-02-13T00:00:00Z- AHRS_M2 msg: 1613174400.59 epoch sec; seqNo:8684 2021-02-13T00:00:00Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.13 psi 2021-02-13T00:00:00Z- handleDepth()-Depth_Keller msg: 1613174400589 epoch sec; seqNo:8754; depth 28.19 2021-02-13T00:00:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174400.60 epoch sec; seqNo:8754; 4058363.77 north; 591865.41 east 2021-02-13T00:00:00Z- AHRS_M2 msg: 1613174400.99 epoch sec; seqNo:8685 2021-02-13T00:00:00Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.13 psi 2021-02-13T00:00:00Z- handleDepth()-Depth_Keller msg: 1613174400997 epoch sec; seqNo:8755; depth 28.04 2021-02-13T00:00:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:01Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:01Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:01Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:01Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:01Z- handleDvl() - RDI_Pathfinder msg: 1613174401.00 epoch sec; seqNo:4193 2021-02-13T00:00:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , 0.01 2021-02-13T00:00:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174401.01 epoch sec; seqNo:8755; 4058364.14 north; 591865.41 east 2021-02-13T00:00:01Z- AHRS_M2 msg: 1613174401.39 epoch sec; seqNo:8686 2021-02-13T00:00:01Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.12 psi 2021-02-13T00:00:01Z- handleDepth()-Depth_Keller msg: 1613174401401 epoch sec; seqNo:8756; depth 27.91 2021-02-13T00:00:01Z- handleDvl() - RDI_Pathfinder msg: 1613174401.42 epoch sec; seqNo:4194 2021-02-13T00:00:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 82.84 , 97.99 , 62.53 2021-02-13T00:00:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , 0.04 2021-02-13T00:00:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174401.43 epoch sec; seqNo:8756; 4058364.14 north; 591865.41 east 2021-02-13T00:00:01Z- AHRS_M2 msg: 1613174401.80 epoch sec; seqNo:8687 2021-02-13T00:00:01Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.12 psi 2021-02-13T00:00:01Z- handleDepth()-Depth_Keller msg: 1613174401801 epoch sec; seqNo:8757; depth 27.77 2021-02-13T00:00:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174401.81 epoch sec; seqNo:8757; 4058364.52 north; 591865.41 east 2021-02-13T00:00:02Z- AHRS_M2 msg: 1613174402.21 epoch sec; seqNo:8688 2021-02-13T00:00:02Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.12 psi 2021-02-13T00:00:02Z- handleDepth()-Depth_Keller msg: 1613174402211 epoch sec; seqNo:8758; depth 27.63 2021-02-13T00:00:02Z- handleDvl() - RDI_Pathfinder msg: 1613174402.22 epoch sec; seqNo:4195 2021-02-13T00:00:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 82.77 , 97.92 , 62.94 2021-02-13T00:00:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , 0.04 2021-02-13T00:00:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174402.23 epoch sec; seqNo:8758; 4058364.90 north; 591865.40 east 2021-02-13T00:00:02Z- AHRS_M2 msg: 1613174402.60 epoch sec; seqNo:8689 2021-02-13T00:00:02Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.12 psi 2021-02-13T00:00:02Z- handleDepth()-Depth_Keller msg: 1613174402608 epoch sec; seqNo:8759; depth 27.50 2021-02-13T00:00:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174402.63 epoch sec; seqNo:8759; 4058365.28 north; 591865.40 east 2021-02-13T00:00:03Z- AHRS_M2 msg: 1613174403.01 epoch sec; seqNo:8690 2021-02-13T00:00:03Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.12 psi 2021-02-13T00:00:03Z- handleDepth()-Depth_Keller msg: 1613174403017 epoch sec; seqNo:8760; depth 27.36 2021-02-13T00:00:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174403.03 epoch sec; seqNo:8760; 4058365.28 north; 591865.40 east 2021-02-13T00:00:03Z- AHRS_M2 msg: 1613174403.41 epoch sec; seqNo:8691 2021-02-13T00:00:03Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.11 psi 2021-02-13T00:00:03Z- handleDepth()-Depth_Keller msg: 1613174403421 epoch sec; seqNo:8761; depth 27.23 2021-02-13T00:00:03Z- handleDvl() - RDI_Pathfinder msg: 1613174403.43 epoch sec; seqNo:4196 2021-02-13T00:00:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.94 , 83.85 , 99.00 , 61.31 2021-02-13T00:00:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.05 2021-02-13T00:00:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174403.44 epoch sec; seqNo:8761; 4058365.66 north; 591865.39 east 2021-02-13T00:00:03Z- AHRS_M2 msg: 1613174403.82 epoch sec; seqNo:8692 2021-02-13T00:00:03Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.11 psi 2021-02-13T00:00:03Z- handleDepth()-Depth_Keller msg: 1613174403822 epoch sec; seqNo:8762; depth 27.09 2021-02-13T00:00:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174403.84 epoch sec; seqNo:8762; 4058366.05 north; 591866.61 east 2021-02-13T00:00:04Z- LcmTrn::handleMessages() - handling 43 messages took 0.61 ms and 131 calls... 2021-02-13T00:00:04Z- waiting for 1613174402.00, time is 1613174403.84 2021-02-13T00:00:04Z- LcmTrn::updateTrn() >>>> pose time 1613174403.84, meas time 1613174403.43 <<<< 2021-02-13T00:00:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174403.43 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:00:04Z- TerrainNav::motionUpdate() - currEstimate = (4058366, 591866.61, 27.09) 2021-02-13T00:00:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:00:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:00:04Z- TNavBF::Projecting Measurements 2021-02-13T00:00:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:00:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:00:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:00:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:00:04Z- TNavFilter::4 good beams. 2021-02-13T00:00:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:00:04Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:00:04Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:00:04Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:00:04Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:00:04Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:00:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:00:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:00:04Z- TerrainMapOctree: (vehN, vehE ) = (4058365.76, 591866.41). 2021-02-13T00:00:04Z- TerrainMapOctree: Min Distance = 3741.70. 2021-02-13T00:00:04Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:00:04Z- There is no specific good beam that all particles have in common. 2021-02-13T00:00:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:00:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.58 K: 0.137 ExpectedMeasDiff: nan currMeas.covariance: 8.28 2021-02-13T00:00:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:00:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 11.25 2021-02-13T00:00:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:00:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 15.68 2021-02-13T00:00:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:00:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.130 ExpectedMeasDiff: nan currMeas.covariance: 6.01 2021-02-13T00:00:04Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:00:04Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:00:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:00:04Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:00:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:00:04Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:00:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:00:04Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:00:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:00:04Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:00:04Z- Filter 0 No beams used in subcloud update 2021-02-13T00:00:04Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:00:04Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:00:04Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 723.9 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:00:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174403.43 sec. 2021-02-13T00:00:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:00:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:00:04Z- variances([0], [2], and psi_berg) = 723.836580 833.012221 0.000000 2021-02-13T00:00:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:00:04Z- LcmTrn::publishEstimates() - MLE : 4058361.55 591832.11 27.09 2021-02-13T00:00:04Z- LcmTrn::publishEstimates() - MMSE : 4058366.05 591866.61 27.09 2021-02-13T00:00:04Z- LcmTrn::publishEstimates() - COVAR: 723.84 833.01 1.02 0.00 2021-02-13T00:00:04Z- AHRS_M2 msg: 1613174404.22 epoch sec; seqNo:8693 2021-02-13T00:00:04Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.11 psi 2021-02-13T00:00:04Z- handleDepth()-Depth_Keller msg: 1613174404225 epoch sec; seqNo:8763; depth 26.96 2021-02-13T00:00:04Z- handleDvl() - RDI_Pathfinder msg: 1613174404.24 epoch sec; seqNo:4197 2021-02-13T00:00:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 82.84 , 100.15 , 63.76 2021-02-13T00:00:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.02 , 0.05 2021-02-13T00:00:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174404.26 epoch sec; seqNo:8763; 4058366.43 north; 591866.61 east 2021-02-13T00:00:04Z- AHRS_M2 msg: 1613174404.62 epoch sec; seqNo:8694 2021-02-13T00:00:04Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.11 psi 2021-02-13T00:00:04Z- handleDepth()-Depth_Keller msg: 1613174404628 epoch sec; seqNo:8764; depth 26.82 2021-02-13T00:00:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:04Z- handleDvl() - RDI_Pathfinder msg: 1613174404.63 epoch sec; seqNo:4198 2021-02-13T00:00:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.02 , 0.05 2021-02-13T00:00:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174404.65 epoch sec; seqNo:8764; 4058366.43 north; 591866.61 east 2021-02-13T00:00:05Z- AHRS_M2 msg: 1613174405.03 epoch sec; seqNo:8695 2021-02-13T00:00:05Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.11 psi 2021-02-13T00:00:05Z- handleDepth()-Depth_Keller msg: 1613174405032 epoch sec; seqNo:8765; depth 26.68 2021-02-13T00:00:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174405.05 epoch sec; seqNo:8765; 4058366.81 north; 591866.60 east 2021-02-13T00:00:05Z- AHRS_M2 msg: 1613174405.44 epoch sec; seqNo:8696 2021-02-13T00:00:05Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.12 psi 2021-02-13T00:00:05Z- handleDepth()-Depth_Keller msg: 1613174405443 epoch sec; seqNo:8766; depth 26.55 2021-02-13T00:00:05Z- handleDvl() - RDI_Pathfinder msg: 1613174405.45 epoch sec; seqNo:4199 2021-02-13T00:00:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.16 , 82.84 , 100.15 , 64.17 2021-02-13T00:00:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.06 2021-02-13T00:00:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174405.46 epoch sec; seqNo:8766; 4058367.19 north; 591866.60 east 2021-02-13T00:00:05Z- AHRS_M2 msg: 1613174405.84 epoch sec; seqNo:8697 2021-02-13T00:00:05Z- AHRS_M2 msg: 0.65 phi; 0.37 theta; -0.12 psi 2021-02-13T00:00:05Z- handleDepth()-Depth_Keller msg: 1613174405847 epoch sec; seqNo:8767; depth 26.41 2021-02-13T00:00:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:05Z- handleDvl() - RDI_Pathfinder msg: 1613174405.85 epoch sec; seqNo:4200 2021-02-13T00:00:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.06 2021-02-13T00:00:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174405.87 epoch sec; seqNo:8767; 4058367.19 north; 591866.60 east 2021-02-13T00:00:06Z- AHRS_M2 msg: 1613174406.24 epoch sec; seqNo:8698 2021-02-13T00:00:06Z- AHRS_M2 msg: 0.65 phi; 0.37 theta; -0.12 psi 2021-02-13T00:00:06Z- handleDepth()-Depth_Keller msg: 1613174406249 epoch sec; seqNo:8768; depth 26.27 2021-02-13T00:00:06Z- handleDvl() - RDI_Pathfinder msg: 1613174406.26 epoch sec; seqNo:4201 2021-02-13T00:00:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.16 , 84.94 , 100.08 , 62.53 2021-02-13T00:00:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.00 , 0.05 2021-02-13T00:00:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174406.28 epoch sec; seqNo:8768; 4058367.58 north; 591867.81 east 2021-02-13T00:00:06Z- AHRS_M2 msg: 1613174406.64 epoch sec; seqNo:8699 2021-02-13T00:00:06Z- AHRS_M2 msg: 0.66 phi; 0.38 theta; -0.12 psi 2021-02-13T00:00:06Z- handleDepth()-Depth_Keller msg: 1613174406650 epoch sec; seqNo:8769; depth 26.14 2021-02-13T00:00:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174406.66 epoch sec; seqNo:8769; 4058367.96 north; 591867.81 east 2021-02-13T00:00:07Z- AHRS_M2 msg: 1613174407.05 epoch sec; seqNo:8700 2021-02-13T00:00:07Z- AHRS_M2 msg: 0.66 phi; 0.38 theta; -0.11 psi 2021-02-13T00:00:07Z- handleDepth()-Depth_Keller msg: 1613174407052 epoch sec; seqNo:8770; depth 26.00 2021-02-13T00:00:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:07Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:07Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:07Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:07Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:07Z- handleDvl() - RDI_Pathfinder msg: 1613174407.05 epoch sec; seqNo:4202 2021-02-13T00:00:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.00 , 0.05 2021-02-13T00:00:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174407.07 epoch sec; seqNo:8770; 4058367.96 north; 591867.81 east 2021-02-13T00:00:07Z- AHRS_M2 msg: 1613174407.45 epoch sec; seqNo:8701 2021-02-13T00:00:07Z- AHRS_M2 msg: 0.67 phi; 0.38 theta; -0.11 psi 2021-02-13T00:00:07Z- handleDepth()-Depth_Keller msg: 1613174407456 epoch sec; seqNo:8771; depth 25.85 2021-02-13T00:00:07Z- handleDvl() - RDI_Pathfinder msg: 1613174407.47 epoch sec; seqNo:4203 2021-02-13T00:00:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 86.02 , 101.16 , 64.99 2021-02-13T00:00:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.00 , 0.05 2021-02-13T00:00:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174407.49 epoch sec; seqNo:8771; 4058368.34 north; 591867.81 east 2021-02-13T00:00:07Z- AHRS_M2 msg: 1613174407.86 epoch sec; seqNo:8702 2021-02-13T00:00:07Z- AHRS_M2 msg: 0.67 phi; 0.38 theta; -0.11 psi 2021-02-13T00:00:07Z- handleDepth()-Depth_Keller msg: 1613174407860 epoch sec; seqNo:8772; depth 25.71 2021-02-13T00:00:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174407.87 epoch sec; seqNo:8772; 4058368.72 north; 591867.80 east 2021-02-13T00:00:08Z- AHRS_M2 msg: 1613174408.26 epoch sec; seqNo:8703 2021-02-13T00:00:08Z- AHRS_M2 msg: 0.67 phi; 0.37 theta; -0.12 psi 2021-02-13T00:00:08Z- handleDepth()-Depth_Keller msg: 1613174408266 epoch sec; seqNo:8773; depth 25.57 2021-02-13T00:00:08Z- handleDvl() - RDI_Pathfinder msg: 1613174408.28 epoch sec; seqNo:4204 2021-02-13T00:00:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.57 , 85.00 , 100.83 , 62.94 2021-02-13T00:00:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , 0.07 2021-02-13T00:00:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174408.29 epoch sec; seqNo:8773; 4058368.72 north; 591867.80 east 2021-02-13T00:00:08Z- AHRS_M2 msg: 1613174408.66 epoch sec; seqNo:8704 2021-02-13T00:00:08Z- AHRS_M2 msg: 0.67 phi; 0.37 theta; -0.12 psi 2021-02-13T00:00:08Z- handleDepth()-Depth_Keller msg: 1613174408668 epoch sec; seqNo:8774; depth 25.40 2021-02-13T00:00:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174408.69 epoch sec; seqNo:8774; 4058369.11 north; 591869.02 east 2021-02-13T00:00:09Z- AHRS_M2 msg: 1613174409.07 epoch sec; seqNo:8705 2021-02-13T00:00:09Z- AHRS_M2 msg: 0.67 phi; 0.37 theta; -0.12 psi 2021-02-13T00:00:09Z- handleDepth()-Depth_Keller msg: 1613174409077 epoch sec; seqNo:8775; depth 25.29 2021-02-13T00:00:09Z- handleDvl() - RDI_Pathfinder msg: 1613174409.09 epoch sec; seqNo:4205 2021-02-13T00:00:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 0.00 , 102.25 , 64.99 2021-02-13T00:00:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , 0.09 2021-02-13T00:00:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174409.11 epoch sec; seqNo:8775; 4058369.49 north; 591869.02 east 2021-02-13T00:00:09Z- LcmTrn::handleMessages() - handling 48 messages took 0.69 ms and 134 calls... 2021-02-13T00:00:09Z- waiting for 1613174406.84, time is 1613174409.11 2021-02-13T00:00:09Z- LcmTrn::updateTrn() equating pose 1613174409.11 and meas times 1613174409.11 2021-02-13T00:00:09Z- LcmTrn::updateTrn() >>>> pose time 1613174409.11, meas time 1613174409.11 <<<< 2021-02-13T00:00:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174409.11 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:00:09Z- TerrainNav::motionUpdate() - currEstimate = (4058369, 591869.02, 25.29) 2021-02-13T00:00:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:00:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:00:09Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174409.11 sec.; No relevant navigation data available 2021-02-13T00:00:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:00:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:00:09Z- variances([0], [2], and psi_berg) = 723.764402 833.012172 0.000000 2021-02-13T00:00:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:00:09Z- LcmTrn::publishEstimates() - MLE : 4058363.99 591912.52 25.29 2021-02-13T00:00:09Z- LcmTrn::publishEstimates() - MMSE : 4058369.49 591869.02 25.29 2021-02-13T00:00:09Z- LcmTrn::publishEstimates() - COVAR: 723.76 833.01 1.24 0.00 2021-02-13T00:00:09Z- AHRS_M2 msg: 1613174409.47 epoch sec; seqNo:8706 2021-02-13T00:00:09Z- AHRS_M2 msg: 0.67 phi; 0.37 theta; -0.13 psi 2021-02-13T00:00:09Z- handleDepth()-Depth_Keller msg: 1613174409481 epoch sec; seqNo:8776; depth 25.12 2021-02-13T00:00:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:09Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:09Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:09Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:09Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:09Z- handleDvl() - RDI_Pathfinder msg: 1613174409.48 epoch sec; seqNo:4206 2021-02-13T00:00:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , 0.09 2021-02-13T00:00:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174409.49 epoch sec; seqNo:8776; 4058369.49 north; 591869.02 east 2021-02-13T00:00:09Z- AHRS_M2 msg: 1613174409.88 epoch sec; seqNo:8707 2021-02-13T00:00:09Z- AHRS_M2 msg: 0.67 phi; 0.37 theta; -0.13 psi 2021-02-13T00:00:09Z- handleDepth()-Depth_Keller msg: 1613174409886 epoch sec; seqNo:8777; depth 24.98 2021-02-13T00:00:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174409.89 epoch sec; seqNo:8777; 4058369.87 north; 591869.01 east 2021-02-13T00:00:10Z- AHRS_M2 msg: 1613174410.28 epoch sec; seqNo:8708 2021-02-13T00:00:10Z- AHRS_M2 msg: 0.67 phi; 0.37 theta; -0.13 psi 2021-02-13T00:00:10Z- handleDepth()-Depth_Keller msg: 1613174410289 epoch sec; seqNo:8778; depth 24.84 2021-02-13T00:00:10Z- handleDvl() - RDI_Pathfinder msg: 1613174410.30 epoch sec; seqNo:4207 2021-02-13T00:00:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 0.00 , 102.25 , 64.58 2021-02-13T00:00:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.09 , 0.09 2021-02-13T00:00:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174410.31 epoch sec; seqNo:8778; 4058370.24 north; 591869.01 east 2021-02-13T00:00:10Z- AHRS_M2 msg: 1613174410.68 epoch sec; seqNo:8709 2021-02-13T00:00:10Z- AHRS_M2 msg: 0.67 phi; 0.37 theta; -0.13 psi 2021-02-13T00:00:10Z- handleDepth()-Depth_Keller msg: 1613174410689 epoch sec; seqNo:8779; depth 24.69 2021-02-13T00:00:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:10Z- handleDvl() - RDI_Pathfinder msg: 1613174410.69 epoch sec; seqNo:4208 2021-02-13T00:00:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.09 , 0.09 2021-02-13T00:00:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174410.70 epoch sec; seqNo:8779; 4058370.26 north; 591870.23 east 2021-02-13T00:00:11Z- AHRS_M2 msg: 1613174411.11 epoch sec; seqNo:8710 2021-02-13T00:00:11Z- AHRS_M2 msg: 0.67 phi; 0.37 theta; -0.14 psi 2021-02-13T00:00:11Z- handleDepth()-Depth_Keller msg: 1613174411117 epoch sec; seqNo:8780; depth 24.56 2021-02-13T00:00:11Z- handleDvl() - RDI_Pathfinder msg: 1613174411.14 epoch sec; seqNo:4209 2021-02-13T00:00:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 0.00 , 102.25 , 64.17 2021-02-13T00:00:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.08 , 0.06 2021-02-13T00:00:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174411.16 epoch sec; seqNo:8780; 4058370.64 north; 591870.22 east 2021-02-13T00:00:11Z- AHRS_M2 msg: 1613174411.50 epoch sec; seqNo:8711 2021-02-13T00:00:11Z- AHRS_M2 msg: 0.67 phi; 0.37 theta; -0.15 psi 2021-02-13T00:00:11Z- handleDepth()-Depth_Keller msg: 1613174411500 epoch sec; seqNo:8781; depth 24.40 2021-02-13T00:00:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174411.51 epoch sec; seqNo:8781; 4058371.02 north; 591870.22 east 2021-02-13T00:00:11Z- AHRS_M2 msg: 1613174411.90 epoch sec; seqNo:8712 2021-02-13T00:00:11Z- AHRS_M2 msg: 0.67 phi; 0.37 theta; -0.17 psi 2021-02-13T00:00:11Z- handleDepth()-Depth_Keller msg: 1613174411900 epoch sec; seqNo:8782; depth 24.27 2021-02-13T00:00:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:11Z- handleDvl() - RDI_Pathfinder msg: 1613174411.90 epoch sec; seqNo:4210 2021-02-13T00:00:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.08 , 0.06 2021-02-13T00:00:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174411.93 epoch sec; seqNo:8782; 4058371.02 north; 591870.22 east 2021-02-13T00:00:12Z- AHRS_M2 msg: 1613174412.30 epoch sec; seqNo:8713 2021-02-13T00:00:12Z- AHRS_M2 msg: 0.67 phi; 0.37 theta; -0.18 psi 2021-02-13T00:00:12Z- handleDepth()-Depth_Keller msg: 1613174412309 epoch sec; seqNo:8783; depth 24.12 2021-02-13T00:00:12Z- handleDvl() - RDI_Pathfinder msg: 1613174412.32 epoch sec; seqNo:4211 2021-02-13T00:00:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.39 , 90.21 , 104.27 , 0.00 2021-02-13T00:00:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.10 , -0.02 2021-02-13T00:00:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174412.34 epoch sec; seqNo:8783; 4058371.39 north; 591870.22 east 2021-02-13T00:00:12Z- AHRS_M2 msg: 1613174412.70 epoch sec; seqNo:8714 2021-02-13T00:00:12Z- AHRS_M2 msg: 0.67 phi; 0.37 theta; -0.19 psi 2021-02-13T00:00:12Z- handleDepth()-Depth_Keller msg: 1613174412708 epoch sec; seqNo:8784; depth 23.97 2021-02-13T00:00:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174412.72 epoch sec; seqNo:8784; 4058371.79 north; 591871.43 east 2021-02-13T00:00:13Z- AHRS_M2 msg: 1613174413.11 epoch sec; seqNo:8715 2021-02-13T00:00:13Z- AHRS_M2 msg: 0.67 phi; 0.36 theta; -0.20 psi 2021-02-13T00:00:13Z- handleDepth()-Depth_Keller msg: 1613174413112 epoch sec; seqNo:8785; depth 23.84 2021-02-13T00:00:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:13Z- handleDvl() - RDI_Pathfinder msg: 1613174413.13 epoch sec; seqNo:4212 2021-02-13T00:00:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.10 , -0.02 2021-02-13T00:00:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174413.14 epoch sec; seqNo:8785; 4058371.79 north; 591871.43 east 2021-02-13T00:00:13Z- AHRS_M2 msg: 1613174413.51 epoch sec; seqNo:8716 2021-02-13T00:00:13Z- AHRS_M2 msg: 0.67 phi; 0.36 theta; -0.20 psi 2021-02-13T00:00:13Z- handleDepth()-Depth_Keller msg: 1613174413516 epoch sec; seqNo:8786; depth 23.70 2021-02-13T00:00:13Z- handleDvl() - RDI_Pathfinder msg: 1613174413.53 epoch sec; seqNo:4213 2021-02-13T00:00:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 97.78 , 106.30 , 64.99 2021-02-13T00:00:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , -0.08 2021-02-13T00:00:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174413.54 epoch sec; seqNo:8786; 4058372.17 north; 591871.43 east 2021-02-13T00:00:13Z- AHRS_M2 msg: 1613174413.92 epoch sec; seqNo:8717 2021-02-13T00:00:13Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.21 psi 2021-02-13T00:00:13Z- handleDepth()-Depth_Keller msg: 1613174413926 epoch sec; seqNo:8787; depth 23.56 2021-02-13T00:00:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174413.94 epoch sec; seqNo:8787; 4058372.17 north; 591871.43 east 2021-02-13T00:00:14Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-13T00:00:14Z- waiting for 1613174412.11, time is 1613174413.94 2021-02-13T00:00:14Z- LcmTrn::updateTrn() >>>> pose time 1613174413.94, meas time 1613174413.53 <<<< 2021-02-13T00:00:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174413.53 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:00:14Z- TerrainNav::motionUpdate() - currEstimate = (4058372, 591871.43, 23.56) 2021-02-13T00:00:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:00:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:00:14Z- TNavBF::Projecting Measurements 2021-02-13T00:00:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:00:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:00:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:00:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:00:14Z- TNavFilter::4 good beams. 2021-02-13T00:00:14Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:00:14Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:00:14Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:00:14Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:00:14Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:00:14Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:00:14Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:00:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:00:14Z- TerrainMapOctree: (vehN, vehE ) = (4058371.94, 591871.22). 2021-02-13T00:00:14Z- TerrainMapOctree: Min Distance = 3733.87. 2021-02-13T00:00:14Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:00:14Z- There is no specific good beam that all particles have in common. 2021-02-13T00:00:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:00:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.108 ExpectedMeasDiff: nan currMeas.covariance: 9.81 2021-02-13T00:00:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:00:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-13T00:00:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:00:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 18.08 2021-02-13T00:00:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:00:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.115 ExpectedMeasDiff: nan currMeas.covariance: 6.76 2021-02-13T00:00:14Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:00:14Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:00:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:00:14Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:00:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:00:14Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:00:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:00:14Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:00:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:00:14Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:00:14Z- Filter 0 No beams used in subcloud update 2021-02-13T00:00:14Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:00:14Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:00:14Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 723.7 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:00:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174413.53 sec. 2021-02-13T00:00:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:00:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:00:14Z- variances([0], [2], and psi_berg) = 723.680981 833.012115 0.000000 2021-02-13T00:00:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:00:14Z- LcmTrn::publishEstimates() - MLE : 4058366.67 591914.93 23.56 2021-02-13T00:00:14Z- LcmTrn::publishEstimates() - MMSE : 4058372.17 591871.43 23.56 2021-02-13T00:00:14Z- LcmTrn::publishEstimates() - COVAR: 723.68 833.01 0.99 0.00 2021-02-13T00:00:14Z- AHRS_M2 msg: 1613174414.32 epoch sec; seqNo:8718 2021-02-13T00:00:14Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.22 psi 2021-02-13T00:00:14Z- handleDepth()-Depth_Keller msg: 1613174414333 epoch sec; seqNo:8788; depth 23.42 2021-02-13T00:00:14Z- handleDvl() - RDI_Pathfinder msg: 1613174414.34 epoch sec; seqNo:4214 2021-02-13T00:00:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:14Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:00:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174414.36 epoch sec; seqNo:8788; 4058372.54 north; 591871.42 east 2021-02-13T00:00:14Z- AHRS_M2 msg: 1613174414.72 epoch sec; seqNo:8719 2021-02-13T00:00:14Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.23 psi 2021-02-13T00:00:14Z- handleDepth()-Depth_Keller msg: 1613174414728 epoch sec; seqNo:8789; depth 23.27 2021-02-13T00:00:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174414.75 epoch sec; seqNo:8789; 4058372.92 north; 591871.42 east 2021-02-13T00:00:15Z- AHRS_M2 msg: 1613174415.13 epoch sec; seqNo:8720 2021-02-13T00:00:15Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.23 psi 2021-02-13T00:00:15Z- handleDepth()-Depth_Keller msg: 1613174415135 epoch sec; seqNo:8790; depth 23.13 2021-02-13T00:00:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174415.15 epoch sec; seqNo:8790; 4058372.94 north; 591872.64 east 2021-02-13T00:00:15Z- AHRS_M2 msg: 1613174415.53 epoch sec; seqNo:8721 2021-02-13T00:00:15Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.23 psi 2021-02-13T00:00:15Z- handleDepth()-Depth_Keller msg: 1613174415541 epoch sec; seqNo:8791; depth 23.01 2021-02-13T00:00:15Z- handleDvl() - RDI_Pathfinder msg: 1613174415.55 epoch sec; seqNo:4215 2021-02-13T00:00:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.94 , 97.17 , 103.94 , 67.85 2021-02-13T00:00:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.09 , -0.01 2021-02-13T00:00:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174415.56 epoch sec; seqNo:8791; 4058373.31 north; 591872.64 east 2021-02-13T00:00:15Z- AHRS_M2 msg: 1613174415.94 epoch sec; seqNo:8722 2021-02-13T00:00:15Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.23 psi 2021-02-13T00:00:15Z- handleDepth()-Depth_Keller msg: 1613174415942 epoch sec; seqNo:8792; depth 22.87 2021-02-13T00:00:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174415.98 epoch sec; seqNo:8792; 4058373.69 north; 591872.63 east 2021-02-13T00:00:16Z- AHRS_M2 msg: 1613174416.34 epoch sec; seqNo:8723 2021-02-13T00:00:16Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.22 psi 2021-02-13T00:00:16Z- handleDepth()-Depth_Keller msg: 1613174416344 epoch sec; seqNo:8793; depth 22.74 2021-02-13T00:00:16Z- handleDvl() - RDI_Pathfinder msg: 1613174416.36 epoch sec; seqNo:4216 2021-02-13T00:00:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.71 , 98.19 , 105.49 , 66.62 2021-02-13T00:00:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , 0.06 2021-02-13T00:00:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174416.37 epoch sec; seqNo:8793; 4058374.07 north; 591872.63 east 2021-02-13T00:00:16Z- AHRS_M2 msg: 1613174416.74 epoch sec; seqNo:8724 2021-02-13T00:00:16Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.21 psi 2021-02-13T00:00:16Z- handleDepth()-Depth_Keller msg: 1613174416748 epoch sec; seqNo:8794; depth 22.60 2021-02-13T00:00:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:16Z- handleDvl() - RDI_Pathfinder msg: 1613174416.75 epoch sec; seqNo:4217 2021-02-13T00:00:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , 0.06 2021-02-13T00:00:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174416.76 epoch sec; seqNo:8794; 4058374.07 north; 591872.63 east 2021-02-13T00:00:17Z- AHRS_M2 msg: 1613174417.15 epoch sec; seqNo:8725 2021-02-13T00:00:17Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.21 psi 2021-02-13T00:00:17Z- handleDepth()-Depth_Keller msg: 1613174417154 epoch sec; seqNo:8795; depth 22.47 2021-02-13T00:00:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174417.16 epoch sec; seqNo:8795; 4058374.45 north; 591872.62 east 2021-02-13T00:00:17Z- AHRS_M2 msg: 1613174417.56 epoch sec; seqNo:8726 2021-02-13T00:00:17Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.20 psi 2021-02-13T00:00:17Z- handleDepth()-Depth_Keller msg: 1613174417562 epoch sec; seqNo:8796; depth 22.36 2021-02-13T00:00:17Z- handleDvl() - RDI_Pathfinder msg: 1613174417.58 epoch sec; seqNo:4218 2021-02-13T00:00:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.99 , 102.86 , 66.63 2021-02-13T00:00:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.10 2021-02-13T00:00:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174417.59 epoch sec; seqNo:8796; 4058374.83 north; 591872.62 east 2021-02-13T00:00:17Z- AHRS_M2 msg: 1613174417.96 epoch sec; seqNo:8727 2021-02-13T00:00:17Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.20 psi 2021-02-13T00:00:17Z- handleDepth()-Depth_Keller msg: 1613174417970 epoch sec; seqNo:8797; depth 22.20 2021-02-13T00:00:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:17Z- handleDvl() - RDI_Pathfinder msg: 1613174417.97 epoch sec; seqNo:4219 2021-02-13T00:00:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.10 2021-02-13T00:00:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174417.98 epoch sec; seqNo:8797; 4058375.22 north; 591873.84 east 2021-02-13T00:00:18Z- AHRS_M2 msg: 1613174418.36 epoch sec; seqNo:8728 2021-02-13T00:00:18Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.20 psi 2021-02-13T00:00:18Z- handleDepth()-Depth_Keller msg: 1613174418364 epoch sec; seqNo:8798; depth 22.07 2021-02-13T00:00:18Z- handleDvl() - RDI_Pathfinder msg: 1613174418.38 epoch sec; seqNo:4220 2021-02-13T00:00:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.58 , 93.19 , 105.36 , 68.67 2021-02-13T00:00:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.00 , 0.09 2021-02-13T00:00:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174418.40 epoch sec; seqNo:8798; 4058375.22 north; 591873.84 east 2021-02-13T00:00:18Z- AHRS_M2 msg: 1613174418.76 epoch sec; seqNo:8729 2021-02-13T00:00:18Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.20 psi 2021-02-13T00:00:18Z- handleDepth()-Depth_Keller msg: 1613174418768 epoch sec; seqNo:8799; depth 21.97 2021-02-13T00:00:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174418.78 epoch sec; seqNo:8799; 4058375.60 north; 591873.83 east 2021-02-13T00:00:19Z- AHRS_M2 msg: 1613174419.17 epoch sec; seqNo:8730 2021-02-13T00:00:19Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.20 psi 2021-02-13T00:00:19Z- handleDepth()-Depth_Keller msg: 1613174419172 epoch sec; seqNo:8800; depth 21.84 2021-02-13T00:00:19Z- handleDvl() - RDI_Pathfinder msg: 1613174419.19 epoch sec; seqNo:4221 2021-02-13T00:00:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.21 , 98.19 , 104.27 , 68.26 2021-02-13T00:00:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , 0.07 2021-02-13T00:00:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174419.20 epoch sec; seqNo:8800; 4058375.98 north; 591873.83 east 2021-02-13T00:00:19Z- LcmTrn::handleMessages() - handling 47 messages took 0.68 ms and 134 calls... 2021-02-13T00:00:19Z- waiting for 1613174416.94, time is 1613174419.20 2021-02-13T00:00:19Z- LcmTrn::updateTrn() equating pose 1613174419.20 and meas times 1613174419.20 2021-02-13T00:00:19Z- LcmTrn::updateTrn() >>>> pose time 1613174419.20, meas time 1613174419.20 <<<< 2021-02-13T00:00:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174419.20 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:00:19Z- TerrainNav::motionUpdate() - currEstimate = (4058376, 591873.83, 21.84) 2021-02-13T00:00:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:00:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:00:19Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174419.20 sec.; No relevant navigation data available 2021-02-13T00:00:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:00:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:00:19Z- variances([0], [2], and psi_berg) = 723.606288 833.012064 0.000000 2021-02-13T00:00:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:00:19Z- LcmTrn::publishEstimates() - MLE : 4058370.48 591917.33 21.84 2021-02-13T00:00:19Z- LcmTrn::publishEstimates() - MMSE : 4058375.98 591873.83 21.84 2021-02-13T00:00:19Z- LcmTrn::publishEstimates() - COVAR: 723.61 833.01 1.22 0.00 2021-02-13T00:00:19Z- AHRS_M2 msg: 1613174419.57 epoch sec; seqNo:8731 2021-02-13T00:00:19Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.20 psi 2021-02-13T00:00:19Z- handleDepth()-Depth_Keller msg: 1613174419576 epoch sec; seqNo:8801; depth 21.71 2021-02-13T00:00:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174419.59 epoch sec; seqNo:8801; 4058375.98 north; 591873.83 east 2021-02-13T00:00:19Z- AHRS_M2 msg: 1613174419.98 epoch sec; seqNo:8732 2021-02-13T00:00:19Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.19 psi 2021-02-13T00:00:19Z- handleDepth()-Depth_Keller msg: 1613174419980 epoch sec; seqNo:8802; depth 21.59 2021-02-13T00:00:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174419.99 epoch sec; seqNo:8802; 4058376.36 north; 591873.83 east 2021-02-13T00:00:20Z- AHRS_M2 msg: 1613174420.38 epoch sec; seqNo:8733 2021-02-13T00:00:20Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T00:00:20Z- handleDepth()-Depth_Keller msg: 1613174420388 epoch sec; seqNo:8803; depth 21.48 2021-02-13T00:00:20Z- handleDvl() - RDI_Pathfinder msg: 1613174420.40 epoch sec; seqNo:4222 2021-02-13T00:00:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 93.18 , 105.36 , 69.07 2021-02-13T00:00:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.00 , 0.06 2021-02-13T00:00:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174420.41 epoch sec; seqNo:8803; 4058376.74 north; 591873.82 east 2021-02-13T00:00:20Z- AHRS_M2 msg: 1613174420.78 epoch sec; seqNo:8734 2021-02-13T00:00:20Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.18 psi 2021-02-13T00:00:20Z- handleDepth()-Depth_Keller msg: 1613174420790 epoch sec; seqNo:8804; depth 21.34 2021-02-13T00:00:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174420.80 epoch sec; seqNo:8804; 4058377.13 north; 591875.04 east 2021-02-13T00:00:21Z- AHRS_M2 msg: 1613174421.19 epoch sec; seqNo:8735 2021-02-13T00:00:21Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-13T00:00:21Z- handleDepth()-Depth_Keller msg: 1613174421198 epoch sec; seqNo:8805; depth 21.23 2021-02-13T00:00:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174421.23 epoch sec; seqNo:8805; 4058377.13 north; 591875.04 east 2021-02-13T00:00:21Z- AHRS_M2 msg: 1613174421.59 epoch sec; seqNo:8736 2021-02-13T00:00:21Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T00:00:21Z- handleDepth()-Depth_Keller msg: 1613174421596 epoch sec; seqNo:8806; depth 21.11 2021-02-13T00:00:21Z- handleDvl() - RDI_Pathfinder msg: 1613174421.61 epoch sec; seqNo:4223 2021-02-13T00:00:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 93.18 , 104.14 , 67.44 2021-02-13T00:00:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.00 , 0.05 2021-02-13T00:00:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174421.63 epoch sec; seqNo:8806; 4058377.51 north; 591875.03 east 2021-02-13T00:00:22Z- AHRS_M2 msg: 1613174422.00 epoch sec; seqNo:8737 2021-02-13T00:00:22Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-13T00:00:22Z- handleDepth()-Depth_Keller msg: 1613174422011 epoch sec; seqNo:8807; depth 20.99 2021-02-13T00:00:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174422.02 epoch sec; seqNo:8807; 4058377.89 north; 591875.03 east 2021-02-13T00:00:22Z- AHRS_M2 msg: 1613174422.40 epoch sec; seqNo:8738 2021-02-13T00:00:22Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-13T00:00:22Z- handleDepth()-Depth_Keller msg: 1613174422414 epoch sec; seqNo:8808; depth 20.86 2021-02-13T00:00:22Z- handleDvl() - RDI_Pathfinder msg: 1613174422.42 epoch sec; seqNo:4224 2021-02-13T00:00:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 98.12 , 102.99 , 69.89 2021-02-13T00:00:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , 0.05 2021-02-13T00:00:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174422.44 epoch sec; seqNo:8808; 4058377.89 north; 591875.03 east 2021-02-13T00:00:22Z- AHRS_M2 msg: 1613174422.81 epoch sec; seqNo:8739 2021-02-13T00:00:22Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-13T00:00:22Z- handleDepth()-Depth_Keller msg: 1613174422813 epoch sec; seqNo:8809; depth 20.73 2021-02-13T00:00:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:22Z- handleDvl() - RDI_Pathfinder msg: 1613174422.82 epoch sec; seqNo:4225 2021-02-13T00:00:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , 0.05 2021-02-13T00:00:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174422.83 epoch sec; seqNo:8809; 4058378.27 north; 591875.03 east 2021-02-13T00:00:23Z- AHRS_M2 msg: 1613174423.23 epoch sec; seqNo:8740 2021-02-13T00:00:23Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-13T00:00:23Z- handleDepth()-Depth_Keller msg: 1613174423238 epoch sec; seqNo:8810; depth 20.62 2021-02-13T00:00:23Z- handleDvl() - RDI_Pathfinder msg: 1613174423.25 epoch sec; seqNo:4226 2021-02-13T00:00:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 90.68 , 102.85 , 69.89 2021-02-13T00:00:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.06 2021-02-13T00:00:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174423.27 epoch sec; seqNo:8810; 4058378.66 north; 591876.24 east 2021-02-13T00:00:23Z- AHRS_M2 msg: 1613174423.61 epoch sec; seqNo:8741 2021-02-13T00:00:23Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T00:00:23Z- handleDepth()-Depth_Keller msg: 1613174423616 epoch sec; seqNo:8811; depth 20.50 2021-02-13T00:00:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174423.63 epoch sec; seqNo:8811; 4058378.66 north; 591876.24 east 2021-02-13T00:00:24Z- AHRS_M2 msg: 1613174424.02 epoch sec; seqNo:8742 2021-02-13T00:00:24Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-13T00:00:24Z- handleDepth()-Depth_Keller msg: 1613174424021 epoch sec; seqNo:8812; depth 20.39 2021-02-13T00:00:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:24Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:24Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:24Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:24Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:24Z- handleDvl() - RDI_Pathfinder msg: 1613174424.02 epoch sec; seqNo:4227 2021-02-13T00:00:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.06 2021-02-13T00:00:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174424.03 epoch sec; seqNo:8812; 4058379.04 north; 591876.24 east 2021-02-13T00:00:24Z- LcmTrn::handleMessages() - handling 42 messages took 0.66 ms and 129 calls... 2021-02-13T00:00:24Z- waiting for 1613174422.20, time is 1613174424.03 2021-02-13T00:00:24Z- LcmTrn::updateTrn() equating pose 1613174424.03 and meas times 1613174424.03 2021-02-13T00:00:24Z- LcmTrn::updateTrn() >>>> pose time 1613174424.03, meas time 1613174424.03 <<<< 2021-02-13T00:00:24Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174424.03, ping # 0. 2021-02-13T00:00:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174424.03 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:00:24Z- TerrainNav::motionUpdate() - currEstimate = (4058379, 591876.24, 20.39) 2021-02-13T00:00:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:00:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:00:24Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:00:24Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:00:24Z- TNavBF::Projecting Measurements 2021-02-13T00:00:24Z- TNavFilter::Measurement from time = 1613174424.03 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:00:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:00:24Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:00:24Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:00:24Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:00:24Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:00:24Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:00:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:00:24Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 723.5 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T00:00:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:00:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:00:24Z- variances([0], [2], and psi_berg) = 723.536940 833.012017 0.000000 2021-02-13T00:00:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:00:24Z- LcmTrn::publishEstimates() - MLE : 4058373.54 591919.74 20.39 2021-02-13T00:00:24Z- LcmTrn::publishEstimates() - MMSE : 4058379.04 591876.24 20.39 2021-02-13T00:00:24Z- LcmTrn::publishEstimates() - COVAR: 723.54 833.01 1.42 0.00 2021-02-13T00:00:24Z- AHRS_M2 msg: 1613174424.42 epoch sec; seqNo:8743 2021-02-13T00:00:24Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T00:00:24Z- handleDepth()-Depth_Keller msg: 1613174424426 epoch sec; seqNo:8813; depth 20.28 2021-02-13T00:00:24Z- handleDvl() - RDI_Pathfinder msg: 1613174424.44 epoch sec; seqNo:4228 2021-02-13T00:00:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.84 , 94.54 , 107.93 , 69.48 2021-02-13T00:00:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.02 , 0.09 2021-02-13T00:00:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174424.45 epoch sec; seqNo:8813; 4058379.42 north; 591876.23 east 2021-02-13T00:00:24Z- AHRS_M2 msg: 1613174424.82 epoch sec; seqNo:8744 2021-02-13T00:00:24Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T00:00:24Z- handleDepth()-Depth_Keller msg: 1613174424833 epoch sec; seqNo:8814; depth 20.14 2021-02-13T00:00:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174424.84 epoch sec; seqNo:8814; 4058379.79 north; 591876.23 east 2021-02-13T00:00:25Z- AHRS_M2 msg: 1613174425.24 epoch sec; seqNo:8745 2021-02-13T00:00:25Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-13T00:00:25Z- handleDepth()-Depth_Keller msg: 1613174425240 epoch sec; seqNo:8815; depth 20.03 2021-02-13T00:00:25Z- handleDvl() - RDI_Pathfinder msg: 1613174425.25 epoch sec; seqNo:4229 2021-02-13T00:00:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.99 , 106.51 , 78.51 2021-02-13T00:00:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.01 , 0.07 2021-02-13T00:00:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174425.28 epoch sec; seqNo:8815; 4058380.17 north; 591876.23 east 2021-02-13T00:00:25Z- AHRS_M2 msg: 1613174425.63 epoch sec; seqNo:8746 2021-02-13T00:00:25Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-13T00:00:25Z- handleDepth()-Depth_Keller msg: 1613174425638 epoch sec; seqNo:8816; depth 19.91 2021-02-13T00:00:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174425.65 epoch sec; seqNo:8816; 4058380.19 north; 591877.45 east 2021-02-13T00:00:26Z- AHRS_M2 msg: 1613174426.04 epoch sec; seqNo:8747 2021-02-13T00:00:26Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-13T00:00:26Z- handleDepth()-Depth_Keller msg: 1613174426041 epoch sec; seqNo:8817; depth 19.79 2021-02-13T00:00:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174426.06 epoch sec; seqNo:8817; 4058380.57 north; 591877.44 east 2021-02-13T00:00:26Z- AHRS_M2 msg: 1613174426.44 epoch sec; seqNo:8748 2021-02-13T00:00:26Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-13T00:00:26Z- handleDepth()-Depth_Keller msg: 1613174426445 epoch sec; seqNo:8818; depth 19.67 2021-02-13T00:00:26Z- handleDvl() - RDI_Pathfinder msg: 1613174426.46 epoch sec; seqNo:4230 2021-02-13T00:00:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 97.51 , 109.68 , 68.26 2021-02-13T00:00:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , -0.03 2021-02-13T00:00:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174426.47 epoch sec; seqNo:8818; 4058380.94 north; 591877.44 east 2021-02-13T00:00:26Z- AHRS_M2 msg: 1613174426.84 epoch sec; seqNo:8749 2021-02-13T00:00:26Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-13T00:00:26Z- handleDepth()-Depth_Keller msg: 1613174426849 epoch sec; seqNo:8819; depth 19.57 2021-02-13T00:00:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174426.86 epoch sec; seqNo:8819; 4058381.32 north; 591877.43 east 2021-02-13T00:00:27Z- AHRS_M2 msg: 1613174427.25 epoch sec; seqNo:8750 2021-02-13T00:00:27Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-13T00:00:27Z- handleDepth()-Depth_Keller msg: 1613174427257 epoch sec; seqNo:8820; depth 19.44 2021-02-13T00:00:27Z- handleDvl() - RDI_Pathfinder msg: 1613174427.28 epoch sec; seqNo:4231 2021-02-13T00:00:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.84 , 97.92 , 106.44 , 69.48 2021-02-13T00:00:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , -0.13 2021-02-13T00:00:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174427.29 epoch sec; seqNo:8820; 4058381.32 north; 591877.43 east 2021-02-13T00:00:27Z- AHRS_M2 msg: 1613174427.65 epoch sec; seqNo:8751 2021-02-13T00:00:27Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-13T00:00:27Z- handleDepth()-Depth_Keller msg: 1613174427661 epoch sec; seqNo:8821; depth 19.31 2021-02-13T00:00:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:27Z- handleDvl() - RDI_Pathfinder msg: 1613174427.66 epoch sec; seqNo:4232 2021-02-13T00:00:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , -0.13 2021-02-13T00:00:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174427.67 epoch sec; seqNo:8821; 4058381.70 north; 591877.43 east 2021-02-13T00:00:28Z- AHRS_M2 msg: 1613174428.06 epoch sec; seqNo:8752 2021-02-13T00:00:28Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-13T00:00:28Z- handleDepth()-Depth_Keller msg: 1613174428060 epoch sec; seqNo:8822; depth 19.20 2021-02-13T00:00:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174428.07 epoch sec; seqNo:8822; 4058382.09 north; 591878.65 east 2021-02-13T00:00:28Z- AHRS_M2 msg: 1613174428.46 epoch sec; seqNo:8753 2021-02-13T00:00:28Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T00:00:28Z- handleDepth()-Depth_Keller msg: 1613174428465 epoch sec; seqNo:8823; depth 19.06 2021-02-13T00:00:28Z- handleDvl() - RDI_Pathfinder msg: 1613174428.48 epoch sec; seqNo:4233 2021-02-13T00:00:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.73 , 97.99 , 106.51 , 118.68 2021-02-13T00:00:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.10 , -0.16 2021-02-13T00:00:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174428.49 epoch sec; seqNo:8823; 4058382.09 north; 591878.65 east 2021-02-13T00:00:28Z- AHRS_M2 msg: 1613174428.87 epoch sec; seqNo:8754 2021-02-13T00:00:28Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T00:00:28Z- handleDepth()-Depth_Keller msg: 1613174428871 epoch sec; seqNo:8824; depth 18.93 2021-02-13T00:00:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:28Z- handleDvl() - RDI_Pathfinder msg: 1613174428.87 epoch sec; seqNo:4234 2021-02-13T00:00:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.10 , -0.16 2021-02-13T00:00:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174428.88 epoch sec; seqNo:8824; 4058382.47 north; 591878.64 east 2021-02-13T00:00:29Z- AHRS_M2 msg: 1613174429.28 epoch sec; seqNo:8755 2021-02-13T00:00:29Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T00:00:29Z- handleDepth()-Depth_Keller msg: 1613174429286 epoch sec; seqNo:8825; depth 18.82 2021-02-13T00:00:29Z- handleDvl() - RDI_Pathfinder msg: 1613174429.30 epoch sec; seqNo:4235 2021-02-13T00:00:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.25 , 96.97 , 106.44 , 78.04 2021-02-13T00:00:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.18 , -0.06 2021-02-13T00:00:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174429.32 epoch sec; seqNo:8825; 4058382.85 north; 591878.64 east 2021-02-13T00:00:29Z- LcmTrn::handleMessages() - handling 47 messages took 0.71 ms and 133 calls... 2021-02-13T00:00:29Z- waiting for 1613174427.03, time is 1613174429.32 2021-02-13T00:00:29Z- LcmTrn::updateTrn() equating pose 1613174429.32 and meas times 1613174429.32 2021-02-13T00:00:29Z- LcmTrn::updateTrn() >>>> pose time 1613174429.32, meas time 1613174429.32 <<<< 2021-02-13T00:00:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174429.32 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:00:29Z- TerrainNav::motionUpdate() - currEstimate = (4058383, 591878.64, 18.82) 2021-02-13T00:00:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:00:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:00:29Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174429.32 sec.; No relevant navigation data available 2021-02-13T00:00:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:00:29Z- TNavBF::mmse selected from filter -1 2021-02-13T00:00:29Z- variances([0], [2], and psi_berg) = 723.462372 833.011967 0.000000 2021-02-13T00:00:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:00:29Z- LcmTrn::publishEstimates() - MLE : 4058377.35 591918.14 18.82 2021-02-13T00:00:29Z- LcmTrn::publishEstimates() - MMSE : 4058382.85 591878.64 18.82 2021-02-13T00:00:29Z- LcmTrn::publishEstimates() - COVAR: 723.46 833.01 1.65 0.00 2021-02-13T00:00:29Z- AHRS_M2 msg: 1613174429.67 epoch sec; seqNo:8756 2021-02-13T00:00:29Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-13T00:00:29Z- handleDepth()-Depth_Keller msg: 1613174429677 epoch sec; seqNo:8826; depth 18.67 2021-02-13T00:00:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174429.69 epoch sec; seqNo:8826; 4058382.85 north; 591878.64 east 2021-02-13T00:00:30Z- AHRS_M2 msg: 1613174430.08 epoch sec; seqNo:8757 2021-02-13T00:00:30Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-13T00:00:30Z- handleDepth()-Depth_Keller msg: 1613174430090 epoch sec; seqNo:8827; depth 18.55 2021-02-13T00:00:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:30Z- handleDvl() - RDI_Pathfinder msg: 1613174430.09 epoch sec; seqNo:4236 2021-02-13T00:00:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.18 , -0.06 2021-02-13T00:00:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174430.10 epoch sec; seqNo:8827; 4058383.23 north; 591878.64 east 2021-02-13T00:00:30Z- AHRS_M2 msg: 1613174430.48 epoch sec; seqNo:8758 2021-02-13T00:00:30Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-13T00:00:30Z- handleDepth()-Depth_Keller msg: 1613174430484 epoch sec; seqNo:8828; depth 18.41 2021-02-13T00:00:30Z- handleDvl() - RDI_Pathfinder msg: 1613174430.49 epoch sec; seqNo:4237 2021-02-13T00:00:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 97.78 , 106.30 , 71.53 2021-02-13T00:00:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.18 , 0.09 2021-02-13T00:00:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174430.52 epoch sec; seqNo:8828; 4058383.61 north; 591878.63 east 2021-02-13T00:00:30Z- AHRS_M2 msg: 1613174430.88 epoch sec; seqNo:8759 2021-02-13T00:00:30Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-13T00:00:30Z- handleDepth()-Depth_Keller msg: 1613174430888 epoch sec; seqNo:8829; depth 18.29 2021-02-13T00:00:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174430.90 epoch sec; seqNo:8829; 4058383.99 north; 591878.63 east 2021-02-13T00:00:31Z- AHRS_M2 msg: 1613174431.29 epoch sec; seqNo:8760 2021-02-13T00:00:31Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-13T00:00:31Z- handleDepth()-Depth_Keller msg: 1613174431294 epoch sec; seqNo:8830; depth 18.17 2021-02-13T00:00:31Z- handleDvl() - RDI_Pathfinder msg: 1613174431.31 epoch sec; seqNo:4238 2021-02-13T00:00:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.66 , 97.92 , 105.22 , 70.71 2021-02-13T00:00:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.09 , 0.15 2021-02-13T00:00:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174431.32 epoch sec; seqNo:8830; 4058384.37 north; 591878.62 east 2021-02-13T00:00:31Z- AHRS_M2 msg: 1613174431.69 epoch sec; seqNo:8761 2021-02-13T00:00:31Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.15 psi 2021-02-13T00:00:31Z- handleDepth()-Depth_Keller msg: 1613174431696 epoch sec; seqNo:8831; depth 18.06 2021-02-13T00:00:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174431.71 epoch sec; seqNo:8831; 4058384.75 north; 591878.62 east 2021-02-13T00:00:32Z- AHRS_M2 msg: 1613174432.10 epoch sec; seqNo:8762 2021-02-13T00:00:32Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.14 psi 2021-02-13T00:00:32Z- handleDepth()-Depth_Keller msg: 1613174432104 epoch sec; seqNo:8832; depth 17.94 2021-02-13T00:00:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174432.11 epoch sec; seqNo:8832; 4058384.76 north; 591879.84 east 2021-02-13T00:00:32Z- AHRS_M2 msg: 1613174432.50 epoch sec; seqNo:8763 2021-02-13T00:00:32Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.14 psi 2021-02-13T00:00:32Z- handleDepth()-Depth_Keller msg: 1613174432504 epoch sec; seqNo:8833; depth 17.83 2021-02-13T00:00:32Z- handleDvl() - RDI_Pathfinder msg: 1613174432.52 epoch sec; seqNo:4239 2021-02-13T00:00:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.92 , 106.44 , 71.53 2021-02-13T00:00:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , 0.10 2021-02-13T00:00:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174432.53 epoch sec; seqNo:8833; 4058385.14 north; 591879.84 east 2021-02-13T00:00:32Z- AHRS_M2 msg: 1613174432.90 epoch sec; seqNo:8764 2021-02-13T00:00:32Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.13 psi 2021-02-13T00:00:32Z- handleDepth()-Depth_Keller msg: 1613174432909 epoch sec; seqNo:8834; depth 17.70 2021-02-13T00:00:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174432.92 epoch sec; seqNo:8834; 4058385.52 north; 591879.83 east 2021-02-13T00:00:33Z- AHRS_M2 msg: 1613174433.31 epoch sec; seqNo:8765 2021-02-13T00:00:33Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.12 psi 2021-02-13T00:00:33Z- handleDepth()-Depth_Keller msg: 1613174433312 epoch sec; seqNo:8835; depth 17.59 2021-02-13T00:00:33Z- handleDvl() - RDI_Pathfinder msg: 1613174433.33 epoch sec; seqNo:4240 2021-02-13T00:00:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.49 , 97.71 , 105.83 , 71.53 2021-02-13T00:00:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , 0.02 2021-02-13T00:00:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174433.34 epoch sec; seqNo:8835; 4058385.90 north; 591879.83 east 2021-02-13T00:00:33Z- AHRS_M2 msg: 1613174433.71 epoch sec; seqNo:8766 2021-02-13T00:00:33Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.12 psi 2021-02-13T00:00:33Z- handleDepth()-Depth_Keller msg: 1613174433721 epoch sec; seqNo:8836; depth 17.47 2021-02-13T00:00:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:33Z- handleDvl() - RDI_Pathfinder msg: 1613174433.72 epoch sec; seqNo:4241 2021-02-13T00:00:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , 0.02 2021-02-13T00:00:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174433.73 epoch sec; seqNo:8836; 4058385.90 north; 591879.83 east 2021-02-13T00:00:34Z- AHRS_M2 msg: 1613174434.12 epoch sec; seqNo:8767 2021-02-13T00:00:34Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.12 psi 2021-02-13T00:00:34Z- handleDepth()-Depth_Keller msg: 1613174434123 epoch sec; seqNo:8837; depth 17.36 2021-02-13T00:00:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174434.13 epoch sec; seqNo:8837; 4058386.29 north; 591881.04 east 2021-02-13T00:00:34Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-13T00:00:34Z- waiting for 1613174432.32, time is 1613174434.13 2021-02-13T00:00:34Z- LcmTrn::updateTrn() >>>> pose time 1613174434.13, meas time 1613174433.72 <<<< 2021-02-13T00:00:34Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174433.72, ping # 0. 2021-02-13T00:00:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174433.72 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:00:34Z- TerrainNav::motionUpdate() - currEstimate = (4058386, 591881.04, 17.36) 2021-02-13T00:00:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:00:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:00:34Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:00:34Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:00:34Z- TNavBF::Projecting Measurements 2021-02-13T00:00:34Z- TNavFilter::Measurement from time = 1613174433.72 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:00:34Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:00:34Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:00:34Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:00:34Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:00:34Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:00:34Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:00:34Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:00:34Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 723.4 EastVariance: 833.0 DepthVariance: 1.8 this->DepthBiasCov: 1.8 2021-02-13T00:00:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:00:34Z- TNavBF::mmse selected from filter -1 2021-02-13T00:00:34Z- variances([0], [2], and psi_berg) = 723.372647 833.011906 0.000000 2021-02-13T00:00:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:00:34Z- LcmTrn::publishEstimates() - MLE : 4058380.79 591920.54 17.36 2021-02-13T00:00:34Z- LcmTrn::publishEstimates() - MMSE : 4058386.29 591881.04 17.36 2021-02-13T00:00:34Z- LcmTrn::publishEstimates() - COVAR: 723.37 833.01 1.86 0.00 2021-02-13T00:00:34Z- AHRS_M2 msg: 1613174434.52 epoch sec; seqNo:8768 2021-02-13T00:00:34Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.11 psi 2021-02-13T00:00:34Z- handleDepth()-Depth_Keller msg: 1613174434526 epoch sec; seqNo:8838; depth 17.24 2021-02-13T00:00:34Z- handleDvl() - RDI_Pathfinder msg: 1613174434.54 epoch sec; seqNo:4242 2021-02-13T00:00:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.03 , 97.85 , 107.59 , 69.89 2021-02-13T00:00:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.01 2021-02-13T00:00:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174434.55 epoch sec; seqNo:8838; 4058386.67 north; 591881.04 east 2021-02-13T00:00:34Z- AHRS_M2 msg: 1613174434.92 epoch sec; seqNo:8769 2021-02-13T00:00:34Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.11 psi 2021-02-13T00:00:34Z- handleDepth()-Depth_Keller msg: 1613174434933 epoch sec; seqNo:8839; depth 17.13 2021-02-13T00:00:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:34Z- handleDvl() - RDI_Pathfinder msg: 1613174434.93 epoch sec; seqNo:4243 2021-02-13T00:00:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.01 2021-02-13T00:00:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174434.96 epoch sec; seqNo:8839; 4058386.67 north; 591881.04 east 2021-02-13T00:00:35Z- AHRS_M2 msg: 1613174435.33 epoch sec; seqNo:8770 2021-02-13T00:00:35Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.12 psi 2021-02-13T00:00:35Z- handleDepth()-Depth_Keller msg: 1613174435332 epoch sec; seqNo:8840; depth 16.99 2021-02-13T00:00:35Z- handleDvl() - RDI_Pathfinder msg: 1613174435.37 epoch sec; seqNo:4244 2021-02-13T00:00:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.03 , 97.85 , 107.59 , 78.38 2021-02-13T00:00:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.07 , 0.04 2021-02-13T00:00:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174435.38 epoch sec; seqNo:8840; 4058387.04 north; 591881.04 east 2021-02-13T00:00:35Z- AHRS_M2 msg: 1613174435.73 epoch sec; seqNo:8771 2021-02-13T00:00:35Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.13 psi 2021-02-13T00:00:35Z- handleDepth()-Depth_Keller msg: 1613174435736 epoch sec; seqNo:8841; depth 16.87 2021-02-13T00:00:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174435.75 epoch sec; seqNo:8841; 4058387.42 north; 591881.03 east 2021-02-13T00:00:36Z- AHRS_M2 msg: 1613174436.14 epoch sec; seqNo:8772 2021-02-13T00:00:36Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.13 psi 2021-02-13T00:00:36Z- handleDepth()-Depth_Keller msg: 1613174436146 epoch sec; seqNo:8842; depth 16.75 2021-02-13T00:00:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:36Z- handleDvl() - RDI_Pathfinder msg: 1613174436.15 epoch sec; seqNo:4245 2021-02-13T00:00:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.07 , 0.04 2021-02-13T00:00:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174436.16 epoch sec; seqNo:8842; 4058387.42 north; 591881.03 east 2021-02-13T00:00:36Z- AHRS_M2 msg: 1613174436.54 epoch sec; seqNo:8773 2021-02-13T00:00:36Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-13T00:00:36Z- handleDepth()-Depth_Keller msg: 1613174436544 epoch sec; seqNo:8843; depth 16.63 2021-02-13T00:00:36Z- handleDvl() - RDI_Pathfinder msg: 1613174436.56 epoch sec; seqNo:4246 2021-02-13T00:00:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 94.00 , 107.39 , 78.17 2021-02-13T00:00:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.04 , 0.10 2021-02-13T00:00:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174436.57 epoch sec; seqNo:8843; 4058387.80 north; 591881.03 east 2021-02-13T00:00:36Z- AHRS_M2 msg: 1613174436.94 epoch sec; seqNo:8774 2021-02-13T00:00:36Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-13T00:00:36Z- handleDepth()-Depth_Keller msg: 1613174436953 epoch sec; seqNo:8844; depth 16.48 2021-02-13T00:00:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174436.98 epoch sec; seqNo:8844; 4058388.19 north; 591882.24 east 2021-02-13T00:00:37Z- AHRS_M2 msg: 1613174437.35 epoch sec; seqNo:8775 2021-02-13T00:00:37Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-13T00:00:37Z- handleDepth()-Depth_Keller msg: 1613174437352 epoch sec; seqNo:8845; depth 16.37 2021-02-13T00:00:37Z- handleDvl() - RDI_Pathfinder msg: 1613174437.36 epoch sec; seqNo:4247 2021-02-13T00:00:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 97.71 , 107.45 , 78.24 2021-02-13T00:00:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , 0.13 2021-02-13T00:00:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174437.37 epoch sec; seqNo:8845; 4058388.57 north; 591882.24 east 2021-02-13T00:00:37Z- AHRS_M2 msg: 1613174437.75 epoch sec; seqNo:8776 2021-02-13T00:00:37Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T00:00:37Z- handleDepth()-Depth_Keller msg: 1613174437758 epoch sec; seqNo:8846; depth 16.25 2021-02-13T00:00:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174437.77 epoch sec; seqNo:8846; 4058388.57 north; 591882.24 east 2021-02-13T00:00:38Z- AHRS_M2 msg: 1613174438.16 epoch sec; seqNo:8777 2021-02-13T00:00:38Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-13T00:00:38Z- handleDepth()-Depth_Keller msg: 1613174438161 epoch sec; seqNo:8847; depth 16.14 2021-02-13T00:00:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174438.17 epoch sec; seqNo:8847; 4058388.95 north; 591882.24 east 2021-02-13T00:00:38Z- AHRS_M2 msg: 1613174438.56 epoch sec; seqNo:8778 2021-02-13T00:00:38Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.18 psi 2021-02-13T00:00:38Z- handleDepth()-Depth_Keller msg: 1613174438564 epoch sec; seqNo:8848; depth 16.03 2021-02-13T00:00:38Z- handleDvl() - RDI_Pathfinder msg: 1613174438.58 epoch sec; seqNo:4248 2021-02-13T00:00:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.65 , 109.82 , 78.17 2021-02-13T00:00:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.06 , 0.10 2021-02-13T00:00:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174438.59 epoch sec; seqNo:8848; 4058389.33 north; 591882.23 east 2021-02-13T00:00:38Z- AHRS_M2 msg: 1613174438.97 epoch sec; seqNo:8779 2021-02-13T00:00:38Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.18 psi 2021-02-13T00:00:38Z- handleDepth()-Depth_Keller msg: 1613174438973 epoch sec; seqNo:8849; depth 15.89 2021-02-13T00:00:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174438.99 epoch sec; seqNo:8849; 4058389.71 north; 591882.23 east 2021-02-13T00:00:39Z- AHRS_M2 msg: 1613174439.37 epoch sec; seqNo:8780 2021-02-13T00:00:39Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-13T00:00:39Z- handleDepth()-Depth_Keller msg: 1613174439372 epoch sec; seqNo:8850; depth 15.78 2021-02-13T00:00:39Z- handleDvl() - RDI_Pathfinder msg: 1613174439.39 epoch sec; seqNo:4249 2021-02-13T00:00:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.15 , 110.97 , 78.51 2021-02-13T00:00:39Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.10 , 0.02 2021-02-13T00:00:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174439.40 epoch sec; seqNo:8850; 4058389.72 north; 591883.45 east 2021-02-13T00:00:39Z- LcmTrn::handleMessages() - handling 47 messages took 0.75 ms and 132 calls... 2021-02-13T00:00:39Z- waiting for 1613174437.13, time is 1613174439.40 2021-02-13T00:00:39Z- LcmTrn::updateTrn() equating pose 1613174439.40 and meas times 1613174439.40 2021-02-13T00:00:39Z- LcmTrn::updateTrn() >>>> pose time 1613174439.40, meas time 1613174439.40 <<<< 2021-02-13T00:00:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174439.40 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:00:39Z- TerrainNav::motionUpdate() - currEstimate = (4058390, 591883.45, 15.78) 2021-02-13T00:00:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:00:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:00:39Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174439.40 sec.; No relevant navigation data available 2021-02-13T00:00:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:00:39Z- TNavBF::mmse selected from filter -1 2021-02-13T00:00:39Z- variances([0], [2], and psi_berg) = 723.300854 833.011857 0.000000 2021-02-13T00:00:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:00:39Z- LcmTrn::publishEstimates() - MLE : 4058384.22 591922.95 15.78 2021-02-13T00:00:39Z- LcmTrn::publishEstimates() - MMSE : 4058389.72 591883.45 15.78 2021-02-13T00:00:39Z- LcmTrn::publishEstimates() - COVAR: 723.30 833.01 2.08 0.00 2021-02-13T00:00:39Z- AHRS_M2 msg: 1613174439.78 epoch sec; seqNo:8781 2021-02-13T00:00:39Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-13T00:00:39Z- handleDepth()-Depth_Keller msg: 1613174439781 epoch sec; seqNo:8851; depth 15.66 2021-02-13T00:00:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:39Z- handleDvl() - RDI_Pathfinder msg: 1613174439.78 epoch sec; seqNo:4250 2021-02-13T00:00:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:39Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.10 , 0.02 2021-02-13T00:00:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174439.80 epoch sec; seqNo:8851; 4058390.10 north; 591883.45 east 2021-02-13T00:00:40Z- AHRS_M2 msg: 1613174440.18 epoch sec; seqNo:8782 2021-02-13T00:00:40Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-13T00:00:40Z- handleDepth()-Depth_Keller msg: 1613174440183 epoch sec; seqNo:8852; depth 15.55 2021-02-13T00:00:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174440.19 epoch sec; seqNo:8852; 4058390.48 north; 591883.44 east 2021-02-13T00:00:40Z- AHRS_M2 msg: 1613174440.58 epoch sec; seqNo:8783 2021-02-13T00:00:40Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.15 psi 2021-02-13T00:00:40Z- handleDepth()-Depth_Keller msg: 1613174440590 epoch sec; seqNo:8853; depth 15.43 2021-02-13T00:00:40Z- handleDvl() - RDI_Pathfinder msg: 1613174440.60 epoch sec; seqNo:4251 2021-02-13T00:00:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.43 , 98.66 , 112.18 , 74.80 2021-02-13T00:00:40Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.09 , -0.10 2021-02-13T00:00:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174440.61 epoch sec; seqNo:8853; 4058390.86 north; 591883.44 east 2021-02-13T00:00:40Z- AHRS_M2 msg: 1613174440.99 epoch sec; seqNo:8784 2021-02-13T00:00:40Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.14 psi 2021-02-13T00:00:40Z- handleDepth()-Depth_Keller msg: 1613174440992 epoch sec; seqNo:8854; depth 15.32 2021-02-13T00:00:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:40Z- handleDvl() - RDI_Pathfinder msg: 1613174440.99 epoch sec; seqNo:4252 2021-02-13T00:00:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:40Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.09 , -0.10 2021-02-13T00:00:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174441.03 epoch sec; seqNo:8854; 4058390.86 north; 591883.44 east 2021-02-13T00:00:41Z- AHRS_M2 msg: 1613174441.39 epoch sec; seqNo:8785 2021-02-13T00:00:41Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.14 psi 2021-02-13T00:00:41Z- handleDepth()-Depth_Keller msg: 1613174441397 epoch sec; seqNo:8855; depth 15.21 2021-02-13T00:00:41Z- handleDvl() - RDI_Pathfinder msg: 1613174441.41 epoch sec; seqNo:4253 2021-02-13T00:00:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 96.36 , 112.59 , 73.98 2021-02-13T00:00:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , -0.16 2021-02-13T00:00:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174441.42 epoch sec; seqNo:8855; 4058391.25 north; 591884.65 east 2021-02-13T00:00:41Z- AHRS_M2 msg: 1613174441.79 epoch sec; seqNo:8786 2021-02-13T00:00:41Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.13 psi 2021-02-13T00:00:41Z- handleDepth()-Depth_Keller msg: 1613174441797 epoch sec; seqNo:8856; depth 15.08 2021-02-13T00:00:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174441.81 epoch sec; seqNo:8856; 4058391.63 north; 591884.65 east 2021-02-13T00:00:42Z- AHRS_M2 msg: 1613174442.21 epoch sec; seqNo:8787 2021-02-13T00:00:42Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.14 psi 2021-02-13T00:00:42Z- handleDepth()-Depth_Keller msg: 1613174442225 epoch sec; seqNo:8857; depth 14.97 2021-02-13T00:00:42Z- handleDvl() - RDI_Pathfinder msg: 1613174442.24 epoch sec; seqNo:4254 2021-02-13T00:00:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.65 , 111.04 , 78.17 2021-02-13T00:00:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.04 , -0.18 2021-02-13T00:00:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174442.25 epoch sec; seqNo:8857; 4058391.63 north; 591884.65 east 2021-02-13T00:00:42Z- AHRS_M2 msg: 1613174442.60 epoch sec; seqNo:8788 2021-02-13T00:00:42Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.14 psi 2021-02-13T00:00:42Z- handleDepth()-Depth_Keller msg: 1613174442604 epoch sec; seqNo:8858; depth 14.85 2021-02-13T00:00:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174442.62 epoch sec; seqNo:8858; 4058392.01 north; 591884.65 east 2021-02-13T00:00:43Z- AHRS_M2 msg: 1613174443.00 epoch sec; seqNo:8789 2021-02-13T00:00:43Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.14 psi 2021-02-13T00:00:43Z- handleDepth()-Depth_Keller msg: 1613174443010 epoch sec; seqNo:8859; depth 14.75 2021-02-13T00:00:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174443.02 epoch sec; seqNo:8859; 4058392.39 north; 591884.64 east 2021-02-13T00:00:43Z- AHRS_M2 msg: 1613174443.41 epoch sec; seqNo:8790 2021-02-13T00:00:43Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.15 psi 2021-02-13T00:00:43Z- handleDepth()-Depth_Keller msg: 1613174443421 epoch sec; seqNo:8860; depth 14.61 2021-02-13T00:00:43Z- handleDvl() - RDI_Pathfinder msg: 1613174443.43 epoch sec; seqNo:4255 2021-02-13T00:00:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.44 , 109.62 , 74.80 2021-02-13T00:00:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.14 , -0.13 2021-02-13T00:00:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174443.44 epoch sec; seqNo:8860; 4058392.39 north; 591884.64 east 2021-02-13T00:00:43Z- AHRS_M2 msg: 1613174443.81 epoch sec; seqNo:8791 2021-02-13T00:00:43Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.15 psi 2021-02-13T00:00:43Z- handleDepth()-Depth_Keller msg: 1613174443816 epoch sec; seqNo:8861; depth 14.49 2021-02-13T00:00:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174443.84 epoch sec; seqNo:8861; 4058392.77 north; 591884.64 east 2021-02-13T00:00:44Z- AHRS_M2 msg: 1613174444.22 epoch sec; seqNo:8792 2021-02-13T00:00:44Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.16 psi 2021-02-13T00:00:44Z- handleDepth()-Depth_Keller msg: 1613174444229 epoch sec; seqNo:8862; depth 14.38 2021-02-13T00:00:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174444.26 epoch sec; seqNo:8862; 4058393.15 north; 591884.63 east 2021-02-13T00:00:44Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-13T00:00:44Z- waiting for 1613174442.40, time is 1613174444.26 2021-02-13T00:00:44Z- LcmTrn::updateTrn() >>>> pose time 1613174444.26, meas time 1613174443.43 <<<< 2021-02-13T00:00:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174443.43 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:00:44Z- TerrainNav::motionUpdate() - currEstimate = (4058393, 591884.63, 14.38) 2021-02-13T00:00:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:00:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:00:44Z- TNavBF::Projecting Measurements 2021-02-13T00:00:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:00:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:00:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:00:44Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:00:44Z- TNavFilter::3 good beams. 2021-02-13T00:00:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:00:44Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:00:44Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:00:44Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:00:44Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:00:44Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:00:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:00:44Z- TNavBF::mmse selected from filter -1 2021-02-13T00:00:44Z- TerrainMapOctree: (vehN, vehE ) = (4058392.56, 591884.43). 2021-02-13T00:00:44Z- TerrainMapOctree: Min Distance = 3709.56. 2021-02-13T00:00:44Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:00:44Z- There is no specific good beam that all particles have in common. 2021-02-13T00:00:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:00:44Z- Old Depth Bias: 0.00 DepthBiasCov: 2.23 K: 0.116 ExpectedMeasDiff: nan currMeas.covariance: 15.19 2021-02-13T00:00:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:00:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.97 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 19.23 2021-02-13T00:00:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:00:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.80 K: 0.144 ExpectedMeasDiff: nan currMeas.covariance: 8.95 2021-02-13T00:00:44Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:00:44Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:00:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:00:44Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:00:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:00:44Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:00:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:00:44Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:00:44Z- Filter 0 No beams used in subcloud update 2021-02-13T00:00:44Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:00:44Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:00:44Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 723.2 EastVariance: 833.0 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-13T00:00:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174443.43 sec. 2021-02-13T00:00:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:00:44Z- TNavBF::mmse selected from filter -1 2021-02-13T00:00:44Z- variances([0], [2], and psi_berg) = 723.212661 833.011798 0.000000 2021-02-13T00:00:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:00:44Z- LcmTrn::publishEstimates() - MLE : 4058387.65 591924.13 14.38 2021-02-13T00:00:44Z- LcmTrn::publishEstimates() - MMSE : 4058393.15 591884.63 14.38 2021-02-13T00:00:44Z- LcmTrn::publishEstimates() - COVAR: 723.21 833.01 1.57 0.00 2021-02-13T00:00:44Z- AHRS_M2 msg: 1613174444.62 epoch sec; seqNo:8793 2021-02-13T00:00:44Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.16 psi 2021-02-13T00:00:44Z- handleDepth()-Depth_Keller msg: 1613174444624 epoch sec; seqNo:8863; depth 14.27 2021-02-13T00:00:44Z- handleDvl() - RDI_Pathfinder msg: 1613174444.64 epoch sec; seqNo:4256 2021-02-13T00:00:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.70 , 98.93 , 112.46 , 78.65 2021-02-13T00:00:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.19 , -0.02 2021-02-13T00:00:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174444.65 epoch sec; seqNo:8863; 4058393.16 north; 591885.85 east 2021-02-13T00:00:45Z- AHRS_M2 msg: 1613174445.02 epoch sec; seqNo:8794 2021-02-13T00:00:45Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.17 psi 2021-02-13T00:00:45Z- handleDepth()-Depth_Keller msg: 1613174445035 epoch sec; seqNo:8864; depth 14.14 2021-02-13T00:00:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174445.05 epoch sec; seqNo:8864; 4058393.54 north; 591885.85 east 2021-02-13T00:00:45Z- AHRS_M2 msg: 1613174445.43 epoch sec; seqNo:8795 2021-02-13T00:00:45Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.18 psi 2021-02-13T00:00:45Z- handleDepth()-Depth_Keller msg: 1613174445432 epoch sec; seqNo:8865; depth 14.02 2021-02-13T00:00:45Z- handleDvl() - RDI_Pathfinder msg: 1613174445.45 epoch sec; seqNo:4257 2021-02-13T00:00:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 99.00 , 109.82 , 75.61 2021-02-13T00:00:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.20 , 0.14 2021-02-13T00:00:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174445.46 epoch sec; seqNo:8865; 4058393.92 north; 591885.85 east 2021-02-13T00:00:45Z- AHRS_M2 msg: 1613174445.83 epoch sec; seqNo:8796 2021-02-13T00:00:45Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-13T00:00:45Z- handleDepth()-Depth_Keller msg: 1613174445845 epoch sec; seqNo:8866; depth 13.90 2021-02-13T00:00:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:45Z- handleDvl() - RDI_Pathfinder msg: 1613174445.85 epoch sec; seqNo:4258 2021-02-13T00:00:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.20 , 0.14 2021-02-13T00:00:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174445.86 epoch sec; seqNo:8866; 4058394.30 north; 591885.84 east 2021-02-13T00:00:46Z- AHRS_M2 msg: 1613174446.24 epoch sec; seqNo:8797 2021-02-13T00:00:46Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.19 psi 2021-02-13T00:00:46Z- handleDepth()-Depth_Keller msg: 1613174446242 epoch sec; seqNo:8867; depth 13.79 2021-02-13T00:00:46Z- handleDvl() - RDI_Pathfinder msg: 1613174446.25 epoch sec; seqNo:4259 2021-02-13T00:00:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 100.42 , 112.59 , 74.39 2021-02-13T00:00:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.11 , 0.28 2021-02-13T00:00:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174446.26 epoch sec; seqNo:8867; 4058394.67 north; 591885.84 east 2021-02-13T00:00:46Z- AHRS_M2 msg: 1613174446.64 epoch sec; seqNo:8798 2021-02-13T00:00:46Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.19 psi 2021-02-13T00:00:46Z- handleDepth()-Depth_Keller msg: 1613174446646 epoch sec; seqNo:8868; depth 13.67 2021-02-13T00:00:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174446.66 epoch sec; seqNo:8868; 4058395.05 north; 591885.83 east 2021-02-13T00:00:47Z- AHRS_M2 msg: 1613174447.04 epoch sec; seqNo:8799 2021-02-13T00:00:47Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T00:00:47Z- handleDepth()-Depth_Keller msg: 1613174447048 epoch sec; seqNo:8869; depth 13.56 2021-02-13T00:00:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:47Z- handleDvl() - RDI_Pathfinder msg: 1613174447.05 epoch sec; seqNo:4260 2021-02-13T00:00:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.11 , 0.28 2021-02-13T00:00:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174447.07 epoch sec; seqNo:8869; 4058395.05 north; 591885.83 east 2021-02-13T00:00:47Z- AHRS_M2 msg: 1613174447.45 epoch sec; seqNo:8800 2021-02-13T00:00:47Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.18 psi 2021-02-13T00:00:47Z- handleDepth()-Depth_Keller msg: 1613174447452 epoch sec; seqNo:8870; depth 13.44 2021-02-13T00:00:47Z- handleDvl() - RDI_Pathfinder msg: 1613174447.46 epoch sec; seqNo:4261 2021-02-13T00:00:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 101.71 , 112.52 , 76.43 2021-02-13T00:00:47Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.04 , 0.26 2021-02-13T00:00:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174447.49 epoch sec; seqNo:8870; 4058395.44 north; 591887.05 east 2021-02-13T00:00:47Z- AHRS_M2 msg: 1613174447.85 epoch sec; seqNo:8801 2021-02-13T00:00:47Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-13T00:00:47Z- handleDepth()-Depth_Keller msg: 1613174447857 epoch sec; seqNo:8871; depth 13.32 2021-02-13T00:00:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174447.87 epoch sec; seqNo:8871; 4058395.82 north; 591887.05 east 2021-02-13T00:00:48Z- AHRS_M2 msg: 1613174448.28 epoch sec; seqNo:8802 2021-02-13T00:00:48Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.16 psi 2021-02-13T00:00:48Z- handleDepth()-Depth_Keller msg: 1613174448283 epoch sec; seqNo:8872; depth 13.21 2021-02-13T00:00:48Z- handleDvl() - RDI_Pathfinder msg: 1613174448.30 epoch sec; seqNo:4262 2021-02-13T00:00:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 101.64 , 115.16 , 78.65 2021-02-13T00:00:48Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.13 , 0.13 2021-02-13T00:00:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174448.32 epoch sec; seqNo:8872; 4058396.20 north; 591887.04 east 2021-02-13T00:00:48Z- AHRS_M2 msg: 1613174448.66 epoch sec; seqNo:8803 2021-02-13T00:00:48Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.15 psi 2021-02-13T00:00:48Z- handleDepth()-Depth_Keller msg: 1613174448664 epoch sec; seqNo:8873; depth 13.07 2021-02-13T00:00:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174448.68 epoch sec; seqNo:8873; 4058396.58 north; 591887.04 east 2021-02-13T00:00:49Z- AHRS_M2 msg: 1613174449.06 epoch sec; seqNo:8804 2021-02-13T00:00:49Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.14 psi 2021-02-13T00:00:49Z- handleDepth()-Depth_Keller msg: 1613174449073 epoch sec; seqNo:8874; depth 12.98 2021-02-13T00:00:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174449.08 epoch sec; seqNo:8874; 4058396.97 north; 591888.26 east 2021-02-13T00:00:49Z- AHRS_M2 msg: 1613174449.47 epoch sec; seqNo:8805 2021-02-13T00:00:49Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.12 psi 2021-02-13T00:00:49Z- handleDepth()-Depth_Keller msg: 1613174449472 epoch sec; seqNo:8875; depth 12.85 2021-02-13T00:00:49Z- handleDvl() - RDI_Pathfinder msg: 1613174449.48 epoch sec; seqNo:4263 2021-02-13T00:00:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.98 , 100.15 , 116.38 , 78.51 2021-02-13T00:00:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.17 , -0.08 2021-02-13T00:00:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174449.49 epoch sec; seqNo:8875; 4058396.97 north; 591888.26 east 2021-02-13T00:00:49Z- LcmTrn::handleMessages() - handling 47 messages took 0.70 ms and 133 calls... 2021-02-13T00:00:49Z- waiting for 1613174447.26, time is 1613174449.49 2021-02-13T00:00:49Z- LcmTrn::updateTrn() equating pose 1613174449.49 and meas times 1613174449.49 2021-02-13T00:00:49Z- LcmTrn::updateTrn() >>>> pose time 1613174449.49, meas time 1613174449.49 <<<< 2021-02-13T00:00:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174449.49 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:00:49Z- TerrainNav::motionUpdate() - currEstimate = (4058397, 591888.26, 12.85) 2021-02-13T00:00:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:00:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:00:49Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174449.49 sec.; No relevant navigation data available 2021-02-13T00:00:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:00:49Z- TNavBF::mmse selected from filter -1 2021-02-13T00:00:49Z- variances([0], [2], and psi_berg) = 723.132334 833.011743 0.000000 2021-02-13T00:00:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:00:49Z- LcmTrn::publishEstimates() - MLE : 4058391.47 591927.76 12.85 2021-02-13T00:00:49Z- LcmTrn::publishEstimates() - MMSE : 4058396.97 591888.26 12.85 2021-02-13T00:00:49Z- LcmTrn::publishEstimates() - COVAR: 723.13 833.01 1.84 0.00 2021-02-13T00:00:49Z- AHRS_M2 msg: 1613174449.87 epoch sec; seqNo:8806 2021-02-13T00:00:49Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.11 psi 2021-02-13T00:00:49Z- handleDepth()-Depth_Keller msg: 1613174449878 epoch sec; seqNo:8876; depth 12.72 2021-02-13T00:00:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174449.89 epoch sec; seqNo:8876; 4058397.35 north; 591888.25 east 2021-02-13T00:00:50Z- AHRS_M2 msg: 1613174450.29 epoch sec; seqNo:8807 2021-02-13T00:00:50Z- AHRS_M2 msg: 0.68 phi; 0.34 theta; -0.10 psi 2021-02-13T00:00:50Z- handleDepth()-Depth_Keller msg: 1613174450291 epoch sec; seqNo:8877; depth 12.60 2021-02-13T00:00:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174450.31 epoch sec; seqNo:8877; 4058397.35 north; 591888.25 east 2021-02-13T00:00:50Z- AHRS_M2 msg: 1613174450.68 epoch sec; seqNo:8808 2021-02-13T00:00:50Z- AHRS_M2 msg: 0.69 phi; 0.33 theta; -0.10 psi 2021-02-13T00:00:50Z- handleDepth()-Depth_Keller msg: 1613174450684 epoch sec; seqNo:8878; depth 12.46 2021-02-13T00:00:50Z- handleDvl() - RDI_Pathfinder msg: 1613174450.69 epoch sec; seqNo:4264 2021-02-13T00:00:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.91 , 97.38 , 116.31 , 78.44 2021-02-13T00:00:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.08 , -0.17 2021-02-13T00:00:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174450.72 epoch sec; seqNo:8878; 4058397.73 north; 591888.25 east 2021-02-13T00:00:51Z- AHRS_M2 msg: 1613174451.08 epoch sec; seqNo:8809 2021-02-13T00:00:51Z- AHRS_M2 msg: 0.69 phi; 0.33 theta; -0.11 psi 2021-02-13T00:00:51Z- handleDepth()-Depth_Keller msg: 1613174451088 epoch sec; seqNo:8879; depth 12.34 2021-02-13T00:00:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174451.10 epoch sec; seqNo:8879; 4058397.73 north; 591888.25 east 2021-02-13T00:00:51Z- AHRS_M2 msg: 1613174451.49 epoch sec; seqNo:8810 2021-02-13T00:00:51Z- AHRS_M2 msg: 0.69 phi; 0.33 theta; -0.12 psi 2021-02-13T00:00:51Z- handleDepth()-Depth_Keller msg: 1613174451494 epoch sec; seqNo:8880; depth 12.20 2021-02-13T00:00:51Z- handleDvl() - RDI_Pathfinder msg: 1613174451.51 epoch sec; seqNo:4265 2021-02-13T00:00:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.98 , 98.80 , 115.03 , 78.51 2021-02-13T00:00:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.03 , -0.11 2021-02-13T00:00:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174451.52 epoch sec; seqNo:8880; 4058398.12 north; 591889.46 east 2021-02-13T00:00:51Z- AHRS_M2 msg: 1613174451.89 epoch sec; seqNo:8811 2021-02-13T00:00:51Z- AHRS_M2 msg: 0.68 phi; 0.33 theta; -0.14 psi 2021-02-13T00:00:51Z- handleDepth()-Depth_Keller msg: 1613174451898 epoch sec; seqNo:8881; depth 12.05 2021-02-13T00:00:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174451.92 epoch sec; seqNo:8881; 4058398.12 north; 591889.46 east 2021-02-13T00:00:52Z- AHRS_M2 msg: 1613174452.30 epoch sec; seqNo:8812 2021-02-13T00:00:52Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.15 psi 2021-02-13T00:00:52Z- handleDepth()-Depth_Keller msg: 1613174452300 epoch sec; seqNo:8882; depth 11.94 2021-02-13T00:00:52Z- handleDvl() - RDI_Pathfinder msg: 1613174452.31 epoch sec; seqNo:4266 2021-02-13T00:00:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 100.15 , 113.67 , 78.51 2021-02-13T00:00:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.06 2021-02-13T00:00:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174452.33 epoch sec; seqNo:8882; 4058398.50 north; 591889.46 east 2021-02-13T00:00:52Z- AHRS_M2 msg: 1613174452.70 epoch sec; seqNo:8813 2021-02-13T00:00:52Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.17 psi 2021-02-13T00:00:52Z- handleDepth()-Depth_Keller msg: 1613174452704 epoch sec; seqNo:8883; depth 11.81 2021-02-13T00:00:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174452.71 epoch sec; seqNo:8883; 4058398.88 north; 591889.46 east 2021-02-13T00:00:53Z- AHRS_M2 msg: 1613174453.10 epoch sec; seqNo:8814 2021-02-13T00:00:53Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.17 psi 2021-02-13T00:00:53Z- handleDepth()-Depth_Keller msg: 1613174453111 epoch sec; seqNo:8884; depth 11.69 2021-02-13T00:00:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:53Z- handleDvl() - RDI_Pathfinder msg: 1613174453.11 epoch sec; seqNo:4267 2021-02-13T00:00:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.06 2021-02-13T00:00:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174453.12 epoch sec; seqNo:8884; 4058399.26 north; 591889.45 east 2021-02-13T00:00:53Z- AHRS_M2 msg: 1613174453.51 epoch sec; seqNo:8815 2021-02-13T00:00:53Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.18 psi 2021-02-13T00:00:53Z- handleDepth()-Depth_Keller msg: 1613174453517 epoch sec; seqNo:8885; depth 11.56 2021-02-13T00:00:53Z- handleDvl() - RDI_Pathfinder msg: 1613174453.54 epoch sec; seqNo:4268 2021-02-13T00:00:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.92 , 115.09 , 78.58 2021-02-13T00:00:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.08 , 0.13 2021-02-13T00:00:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174453.55 epoch sec; seqNo:8885; 4058399.26 north; 591889.45 east 2021-02-13T00:00:53Z- AHRS_M2 msg: 1613174453.91 epoch sec; seqNo:8816 2021-02-13T00:00:53Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.18 psi 2021-02-13T00:00:53Z- handleDepth()-Depth_Keller msg: 1613174453916 epoch sec; seqNo:8886; depth 11.44 2021-02-13T00:00:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174453.93 epoch sec; seqNo:8886; 4058399.65 north; 591890.67 east 2021-02-13T00:00:54Z- AHRS_M2 msg: 1613174454.32 epoch sec; seqNo:8817 2021-02-13T00:00:54Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.19 psi 2021-02-13T00:00:54Z- handleDepth()-Depth_Keller msg: 1613174454320 epoch sec; seqNo:8887; depth 11.30 2021-02-13T00:00:54Z- handleDvl() - RDI_Pathfinder msg: 1613174454.33 epoch sec; seqNo:4269 2021-02-13T00:00:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.22 , 116.04 , 79.53 2021-02-13T00:00:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.14 , 0.02 2021-02-13T00:00:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174454.34 epoch sec; seqNo:8887; 4058400.03 north; 591890.66 east 2021-02-13T00:00:54Z- LcmTrn::handleMessages() - handling 42 messages took 0.63 ms and 130 calls... 2021-02-13T00:00:54Z- waiting for 1613174452.49, time is 1613174454.34 2021-02-13T00:00:54Z- LcmTrn::updateTrn() equating pose 1613174454.34 and meas times 1613174454.34 2021-02-13T00:00:54Z- LcmTrn::updateTrn() >>>> pose time 1613174454.34, meas time 1613174454.34 <<<< 2021-02-13T00:00:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174454.34 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:00:54Z- TerrainNav::motionUpdate() - currEstimate = (4058400, 591890.66, 11.30) 2021-02-13T00:00:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:00:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:00:54Z- TNavBF::Projecting Measurements 2021-02-13T00:00:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:00:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:00:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:00:54Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:00:54Z- TNavFilter::3 good beams. 2021-02-13T00:00:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:00:54Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:00:54Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:00:54Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:00:54Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:00:54Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:00:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:00:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:00:54Z- TerrainMapOctree: (vehN, vehE ) = (4058400.03, 591890.66). 2021-02-13T00:00:54Z- TerrainMapOctree: Min Distance = 3699.84. 2021-02-13T00:00:54Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:00:54Z- There is no specific good beam that all particles have in common. 2021-02-13T00:00:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:00:54Z- Old Depth Bias: 0.00 DepthBiasCov: 2.04 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 17.71 2021-02-13T00:00:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:00:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.85 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 21.54 2021-02-13T00:00:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:00:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.71 K: 0.126 ExpectedMeasDiff: nan currMeas.covariance: 10.12 2021-02-13T00:00:54Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:00:54Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:00:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:00:54Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:00:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:00:54Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:00:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:00:54Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:00:54Z- Filter 0 No beams used in subcloud update 2021-02-13T00:00:54Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:00:54Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:00:54Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 723.1 EastVariance: 833.0 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-13T00:00:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174454.34 sec. 2021-02-13T00:00:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:00:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:00:54Z- variances([0], [2], and psi_berg) = 723.063364 833.011697 0.000000 2021-02-13T00:00:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:00:54Z- LcmTrn::publishEstimates() - MLE : 4058394.53 591930.16 11.30 2021-02-13T00:00:54Z- LcmTrn::publishEstimates() - MMSE : 4058400.03 591890.66 11.30 2021-02-13T00:00:54Z- LcmTrn::publishEstimates() - COVAR: 723.06 833.01 1.49 0.00 2021-02-13T00:00:54Z- AHRS_M2 msg: 1613174454.72 epoch sec; seqNo:8818 2021-02-13T00:00:54Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.19 psi 2021-02-13T00:00:54Z- handleDepth()-Depth_Keller msg: 1613174454725 epoch sec; seqNo:8888; depth 11.19 2021-02-13T00:00:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174454.74 epoch sec; seqNo:8888; 4058400.03 north; 591890.66 east 2021-02-13T00:00:55Z- AHRS_M2 msg: 1613174455.13 epoch sec; seqNo:8819 2021-02-13T00:00:55Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.19 psi 2021-02-13T00:00:55Z- handleDepth()-Depth_Keller msg: 1613174455131 epoch sec; seqNo:8889; depth 11.07 2021-02-13T00:00:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174455.15 epoch sec; seqNo:8889; 4058400.41 north; 591890.66 east 2021-02-13T00:00:55Z- AHRS_M2 msg: 1613174455.53 epoch sec; seqNo:8820 2021-02-13T00:00:55Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.20 psi 2021-02-13T00:00:55Z- handleDepth()-Depth_Keller msg: 1613174455532 epoch sec; seqNo:8890; depth 10.95 2021-02-13T00:00:55Z- handleDvl() - RDI_Pathfinder msg: 1613174455.54 epoch sec; seqNo:4270 2021-02-13T00:00:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 105.15 , 120.03 , 79.46 2021-02-13T00:00:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.04 , -0.12 2021-02-13T00:00:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174455.57 epoch sec; seqNo:8890; 4058400.79 north; 591890.66 east 2021-02-13T00:00:55Z- AHRS_M2 msg: 1613174455.94 epoch sec; seqNo:8821 2021-02-13T00:00:55Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.21 psi 2021-02-13T00:00:55Z- handleDepth()-Depth_Keller msg: 1613174455942 epoch sec; seqNo:8891; depth 10.82 2021-02-13T00:00:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174455.96 epoch sec; seqNo:8891; 4058400.80 north; 591891.88 east 2021-02-13T00:00:56Z- AHRS_M2 msg: 1613174456.34 epoch sec; seqNo:8822 2021-02-13T00:00:56Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.22 psi 2021-02-13T00:00:56Z- handleDepth()-Depth_Keller msg: 1613174456346 epoch sec; seqNo:8892; depth 10.70 2021-02-13T00:00:56Z- handleDvl() - RDI_Pathfinder msg: 1613174456.36 epoch sec; seqNo:4271 2021-02-13T00:00:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 108.06 , 120.23 , 79.66 2021-02-13T00:00:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.07 , -0.14 2021-02-13T00:00:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174456.37 epoch sec; seqNo:8892; 4058401.18 north; 591891.87 east 2021-02-13T00:00:56Z- AHRS_M2 msg: 1613174456.74 epoch sec; seqNo:8823 2021-02-13T00:00:56Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.23 psi 2021-02-13T00:00:56Z- handleDepth()-Depth_Keller msg: 1613174456751 epoch sec; seqNo:8893; depth 10.58 2021-02-13T00:00:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:56Z- handleDvl() - RDI_Pathfinder msg: 1613174456.75 epoch sec; seqNo:4272 2021-02-13T00:00:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.07 , -0.14 2021-02-13T00:00:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174456.78 epoch sec; seqNo:8893; 4058401.56 north; 591891.87 east 2021-02-13T00:00:57Z- AHRS_M2 msg: 1613174457.15 epoch sec; seqNo:8824 2021-02-13T00:00:57Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.23 psi 2021-02-13T00:00:57Z- handleDepth()-Depth_Keller msg: 1613174457155 epoch sec; seqNo:8894; depth 10.44 2021-02-13T00:00:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174457.16 epoch sec; seqNo:8894; 4058401.56 north; 591891.87 east 2021-02-13T00:00:57Z- AHRS_M2 msg: 1613174457.55 epoch sec; seqNo:8825 2021-02-13T00:00:57Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.23 psi 2021-02-13T00:00:57Z- handleDepth()-Depth_Keller msg: 1613174457553 epoch sec; seqNo:8895; depth 10.32 2021-02-13T00:00:57Z- handleDvl() - RDI_Pathfinder msg: 1613174457.57 epoch sec; seqNo:4273 2021-02-13T00:00:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:57Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:00:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174457.58 epoch sec; seqNo:8895; 4058401.94 north; 591891.86 east 2021-02-13T00:00:57Z- AHRS_M2 msg: 1613174457.96 epoch sec; seqNo:8826 2021-02-13T00:00:57Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.22 psi 2021-02-13T00:00:57Z- handleDepth()-Depth_Keller msg: 1613174457961 epoch sec; seqNo:8896; depth 10.20 2021-02-13T00:00:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:57Z- handleDvl() - RDI_Pathfinder msg: 1613174457.96 epoch sec; seqNo:4274 2021-02-13T00:00:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:57Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:00:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174457.97 epoch sec; seqNo:8896; 4058402.32 north; 591891.86 east 2021-02-13T00:00:58Z- AHRS_M2 msg: 1613174458.38 epoch sec; seqNo:8827 2021-02-13T00:00:58Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.21 psi 2021-02-13T00:00:58Z- handleDepth()-Depth_Keller msg: 1613174458386 epoch sec; seqNo:8897; depth 10.07 2021-02-13T00:00:58Z- handleDvl() - RDI_Pathfinder msg: 1613174458.40 epoch sec; seqNo:4275 2021-02-13T00:00:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 109.14 , 121.04 , 80.88 2021-02-13T00:00:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.09 , 0.03 2021-02-13T00:00:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174458.42 epoch sec; seqNo:8897; 4058402.70 north; 591891.86 east 2021-02-13T00:00:58Z- AHRS_M2 msg: 1613174458.76 epoch sec; seqNo:8828 2021-02-13T00:00:58Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.21 psi 2021-02-13T00:00:58Z- handleDepth()-Depth_Keller msg: 1613174458766 epoch sec; seqNo:8898; depth 9.95 2021-02-13T00:00:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174458.78 epoch sec; seqNo:8898; 4058402.70 north; 591891.86 east 2021-02-13T00:00:59Z- AHRS_M2 msg: 1613174459.16 epoch sec; seqNo:8829 2021-02-13T00:00:59Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.20 psi 2021-02-13T00:00:59Z- handleDepth()-Depth_Keller msg: 1613174459173 epoch sec; seqNo:8899; depth 9.84 2021-02-13T00:00:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:00:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:00:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:00:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:00:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:00:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:00:59Z- handleDvl() - RDI_Pathfinder msg: 1613174459.17 epoch sec; seqNo:4276 2021-02-13T00:00:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:00:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.09 , 0.03 2021-02-13T00:00:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174459.19 epoch sec; seqNo:8899; 4058403.09 north; 591893.07 east 2021-02-13T00:00:59Z- AHRS_M2 msg: 1613174459.57 epoch sec; seqNo:8830 2021-02-13T00:00:59Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.19 psi 2021-02-13T00:00:59Z- handleDepth()-Depth_Keller msg: 1613174459572 epoch sec; seqNo:8900; depth 9.71 2021-02-13T00:00:59Z- handleDvl() - RDI_Pathfinder msg: 1613174459.58 epoch sec; seqNo:4277 2021-02-13T00:00:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 156.61 , 110.63 , 117.39 , 0.00 2021-02-13T00:00:59Z- handleDvl() - RDI_Pathfinder msg: velocities -0.30 , -1.19 , -0.49 2021-02-13T00:00:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174459.61 epoch sec; seqNo:8900; 4058403.07 north; 591891.85 east 2021-02-13T00:00:59Z- LcmTrn::handleMessages() - handling 47 messages took 0.69 ms and 133 calls... 2021-02-13T00:00:59Z- waiting for 1613174457.34, time is 1613174459.61 2021-02-13T00:00:59Z- LcmTrn::updateTrn() equating pose 1613174459.61 and meas times 1613174459.61 2021-02-13T00:00:59Z- LcmTrn::updateTrn() >>>> pose time 1613174459.61, meas time 1613174459.61 <<<< 2021-02-13T00:00:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174459.61 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:00:59Z- TerrainNav::motionUpdate() - currEstimate = (4058403, 591891.85, 9.71) 2021-02-13T00:00:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:00:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:00:59Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174459.61 sec.; No relevant navigation data available 2021-02-13T00:00:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:00:59Z- TNavBF::mmse selected from filter -1 2021-02-13T00:00:59Z- variances([0], [2], and psi_berg) = 723.000220 833.011654 0.000000 2021-02-13T00:00:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:00:59Z- LcmTrn::publishEstimates() - MLE : 4058397.57 591931.35 9.71 2021-02-13T00:00:59Z- LcmTrn::publishEstimates() - MMSE : 4058403.07 591891.85 9.71 2021-02-13T00:00:59Z- LcmTrn::publishEstimates() - COVAR: 723.00 833.01 1.66 0.00 2021-02-13T00:00:59Z- AHRS_M2 msg: 1613174459.97 epoch sec; seqNo:8831 2021-02-13T00:00:59Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-13T00:00:59Z- handleDepth()-Depth_Keller msg: 1613174459976 epoch sec; seqNo:8901; depth 9.58 2021-02-13T00:00:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174459.99 epoch sec; seqNo:8901; 4058403.45 north; 591891.85 east 2021-02-13T00:01:00Z- AHRS_M2 msg: 1613174460.44 epoch sec; seqNo:8832 2021-02-13T00:01:00Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.18 psi 2021-02-13T00:01:00Z- handleDepth()-Depth_Keller msg: 1613174460446 epoch sec; seqNo:8902; depth 9.46 2021-02-13T00:01:00Z- handleDvl() - RDI_Pathfinder msg: 1613174460.46 epoch sec; seqNo:4278 2021-02-13T00:01:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 106.17 , 119.56 , 80.88 2021-02-13T00:01:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.08 , 0.20 2021-02-13T00:01:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174460.47 epoch sec; seqNo:8902; 4058403.83 north; 591891.84 east 2021-02-13T00:01:00Z- AHRS_M2 msg: 1613174460.78 epoch sec; seqNo:8833 2021-02-13T00:01:00Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.18 psi 2021-02-13T00:01:00Z- handleDepth()-Depth_Keller msg: 1613174460784 epoch sec; seqNo:8903; depth 9.30 2021-02-13T00:01:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174460.80 epoch sec; seqNo:8903; 4058404.21 north; 591891.84 east 2021-02-13T00:01:01Z- AHRS_M2 msg: 1613174461.18 epoch sec; seqNo:8834 2021-02-13T00:01:01Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.18 psi 2021-02-13T00:01:01Z- handleDepth()-Depth_Keller msg: 1613174461188 epoch sec; seqNo:8904; depth 9.21 2021-02-13T00:01:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174461.20 epoch sec; seqNo:8904; 4058404.21 north; 591891.84 east 2021-02-13T00:01:01Z- AHRS_M2 msg: 1613174461.59 epoch sec; seqNo:8835 2021-02-13T00:01:01Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.17 psi 2021-02-13T00:01:01Z- handleDepth()-Depth_Keller msg: 1613174461594 epoch sec; seqNo:8905; depth 9.08 2021-02-13T00:01:01Z- handleDvl() - RDI_Pathfinder msg: 1613174461.61 epoch sec; seqNo:4279 2021-02-13T00:01:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.86 , 106.44 , 121.31 , 80.74 2021-02-13T00:01:01Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.03 , 0.21 2021-02-13T00:01:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174461.62 epoch sec; seqNo:8905; 4058404.59 north; 591891.84 east 2021-02-13T00:01:02Z- AHRS_M2 msg: 1613174461.99 epoch sec; seqNo:8836 2021-02-13T00:01:02Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.17 psi 2021-02-13T00:01:02Z- handleDepth()-Depth_Keller msg: 1613174462002 epoch sec; seqNo:8906; depth 8.96 2021-02-13T00:01:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174462.06 epoch sec; seqNo:8906; 4058404.98 north; 591893.05 east 2021-02-13T00:01:02Z- AHRS_M2 msg: 1613174462.39 epoch sec; seqNo:8837 2021-02-13T00:01:02Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.16 psi 2021-02-13T00:01:02Z- handleDepth()-Depth_Keller msg: 1613174462405 epoch sec; seqNo:8907; depth 8.84 2021-02-13T00:01:02Z- handleDvl() - RDI_Pathfinder msg: 1613174462.42 epoch sec; seqNo:4280 2021-02-13T00:01:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 106.57 , 121.45 , 79.29 2021-02-13T00:01:02Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.14 , 0.10 2021-02-13T00:01:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174462.43 epoch sec; seqNo:8907; 4058405.36 north; 591893.05 east 2021-02-13T00:01:02Z- AHRS_M2 msg: 1613174462.80 epoch sec; seqNo:8838 2021-02-13T00:01:02Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.15 psi 2021-02-13T00:01:02Z- handleDepth()-Depth_Keller msg: 1613174462804 epoch sec; seqNo:8908; depth 8.72 2021-02-13T00:01:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:02Z- handleDvl() - RDI_Pathfinder msg: 1613174462.81 epoch sec; seqNo:4281 2021-02-13T00:01:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:02Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.14 , 0.10 2021-02-13T00:01:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174462.82 epoch sec; seqNo:8908; 4058405.74 north; 591893.05 east 2021-02-13T00:01:03Z- AHRS_M2 msg: 1613174463.20 epoch sec; seqNo:8839 2021-02-13T00:01:03Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-13T00:01:03Z- handleDepth()-Depth_Keller msg: 1613174463208 epoch sec; seqNo:8909; depth 8.60 2021-02-13T00:01:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174463.22 epoch sec; seqNo:8909; 4058405.74 north; 591893.05 east 2021-02-13T00:01:03Z- AHRS_M2 msg: 1613174463.61 epoch sec; seqNo:8840 2021-02-13T00:01:03Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.13 psi 2021-02-13T00:01:03Z- handleDepth()-Depth_Keller msg: 1613174463614 epoch sec; seqNo:8910; depth 8.48 2021-02-13T00:01:03Z- handleDvl() - RDI_Pathfinder msg: 1613174463.62 epoch sec; seqNo:4282 2021-02-13T00:01:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 105.09 , 122.67 , 78.88 2021-02-13T00:01:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.14 , -0.02 2021-02-13T00:01:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174463.63 epoch sec; seqNo:8910; 4058406.12 north; 591893.04 east 2021-02-13T00:01:04Z- AHRS_M2 msg: 1613174464.01 epoch sec; seqNo:8841 2021-02-13T00:01:04Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.13 psi 2021-02-13T00:01:04Z- handleDepth()-Depth_Keller msg: 1613174464015 epoch sec; seqNo:8911; depth 8.35 2021-02-13T00:01:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:04Z- handleDvl() - RDI_Pathfinder msg: 1613174464.02 epoch sec; seqNo:4283 2021-02-13T00:01:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.14 , -0.02 2021-02-13T00:01:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174464.04 epoch sec; seqNo:8911; 4058406.51 north; 591894.26 east 2021-02-13T00:01:04Z- AHRS_M2 msg: 1613174464.43 epoch sec; seqNo:8842 2021-02-13T00:01:04Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.13 psi 2021-02-13T00:01:04Z- handleDepth()-Depth_Keller msg: 1613174464436 epoch sec; seqNo:8912; depth 8.23 2021-02-13T00:01:04Z- handleDvl() - RDI_Pathfinder msg: 1613174464.45 epoch sec; seqNo:4284 2021-02-13T00:01:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.56 , 103.73 , 122.67 , 82.10 2021-02-13T00:01:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.09 , -0.00 2021-02-13T00:01:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174464.46 epoch sec; seqNo:8912; 4058406.51 north; 591894.26 east 2021-02-13T00:01:04Z- LcmTrn::handleMessages() - handling 43 messages took 0.59 ms and 131 calls... 2021-02-13T00:01:04Z- waiting for 1613174462.61, time is 1613174464.46 2021-02-13T00:01:04Z- LcmTrn::updateTrn() equating pose 1613174464.46 and meas times 1613174464.46 2021-02-13T00:01:04Z- LcmTrn::updateTrn() >>>> pose time 1613174464.46, meas time 1613174464.46 <<<< 2021-02-13T00:01:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174464.46 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:01:04Z- TerrainNav::motionUpdate() - currEstimate = (4058407, 591894.26, 8.23) 2021-02-13T00:01:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:01:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:01:04Z- TNavBF::Projecting Measurements 2021-02-13T00:01:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:01:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:01:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:01:04Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:01:04Z- TNavFilter::3 good beams. 2021-02-13T00:01:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:01:04Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:01:04Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:01:04Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:01:04Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:01:04Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:01:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:01:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:01:04Z- TerrainMapOctree: (vehN, vehE ) = (4058406.51, 591894.26). 2021-02-13T00:01:04Z- TerrainMapOctree: Min Distance = 3692.55. 2021-02-13T00:01:04Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:01:04Z- There is no specific good beam that all particles have in common. 2021-02-13T00:01:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:01:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.87 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 17.22 2021-02-13T00:01:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:01:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.71 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 24.08 2021-02-13T00:01:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:01:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.60 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 10.78 2021-02-13T00:01:04Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:01:04Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:01:04Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:01:04Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:01:04Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:01:04Z- Filter 0 No beams used in subcloud update 2021-02-13T00:01:04Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:01:04Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:01:04Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 722.9 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T00:01:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174464.46 sec. 2021-02-13T00:01:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:01:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:01:04Z- variances([0], [2], and psi_berg) = 722.928735 833.011606 0.000000 2021-02-13T00:01:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:01:04Z- LcmTrn::publishEstimates() - MLE : 4058401.01 591933.76 8.23 2021-02-13T00:01:04Z- LcmTrn::publishEstimates() - MMSE : 4058406.51 591894.26 8.23 2021-02-13T00:01:04Z- LcmTrn::publishEstimates() - COVAR: 722.93 833.01 1.42 0.00 2021-02-13T00:01:04Z- AHRS_M2 msg: 1613174464.82 epoch sec; seqNo:8843 2021-02-13T00:01:04Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.13 psi 2021-02-13T00:01:04Z- handleDepth()-Depth_Keller msg: 1613174464824 epoch sec; seqNo:8913; depth 8.09 2021-02-13T00:01:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174464.84 epoch sec; seqNo:8913; 4058406.89 north; 591894.25 east 2021-02-13T00:01:05Z- AHRS_M2 msg: 1613174465.22 epoch sec; seqNo:8844 2021-02-13T00:01:05Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.13 psi 2021-02-13T00:01:05Z- handleDepth()-Depth_Keller msg: 1613174465237 epoch sec; seqNo:8914; depth 7.97 2021-02-13T00:01:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:05Z- handleDvl() - RDI_Pathfinder msg: 1613174465.24 epoch sec; seqNo:4285 2021-02-13T00:01:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.09 , -0.00 2021-02-13T00:01:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174465.25 epoch sec; seqNo:8914; 4058406.89 north; 591894.25 east 2021-02-13T00:01:05Z- handleDepth()-Depth_Keller msg: 1613174465646 epoch sec; seqNo:8915; depth 7.84 2021-02-13T00:01:05Z- handleDvl() - RDI_Pathfinder msg: 1613174465.66 epoch sec; seqNo:4286 2021-02-13T00:01:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 105.15 , 120.03 , 82.16 2021-02-13T00:01:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.10 , 0.03 2021-02-13T00:01:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174465.67 epoch sec; seqNo:8915; 4058407.27 north; 591894.25 east 2021-02-13T00:01:06Z- AHRS_M2 msg: 1613174466.03 epoch sec; seqNo:8845 2021-02-13T00:01:06Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-13T00:01:06Z- handleDepth()-Depth_Keller msg: 1613174466037 epoch sec; seqNo:8916; depth 7.69 2021-02-13T00:01:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174466.05 epoch sec; seqNo:8916; 4058407.66 north; 591895.47 east 2021-02-13T00:01:06Z- AHRS_M2 msg: 1613174466.44 epoch sec; seqNo:8846 2021-02-13T00:01:06Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-13T00:01:06Z- handleDepth()-Depth_Keller msg: 1613174466445 epoch sec; seqNo:8917; depth 7.57 2021-02-13T00:01:06Z- handleDvl() - RDI_Pathfinder msg: 1613174466.46 epoch sec; seqNo:4287 2021-02-13T00:01:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 106.44 , 122.67 , 82.10 2021-02-13T00:01:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.09 , -0.02 2021-02-13T00:01:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174466.47 epoch sec; seqNo:8917; 4058407.66 north; 591895.47 east 2021-02-13T00:01:06Z- AHRS_M2 msg: 1613174466.84 epoch sec; seqNo:8847 2021-02-13T00:01:06Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.18 psi 2021-02-13T00:01:06Z- handleDepth()-Depth_Keller msg: 1613174466844 epoch sec; seqNo:8918; depth 7.44 2021-02-13T00:01:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174466.86 epoch sec; seqNo:8918; 4058408.04 north; 591895.46 east 2021-02-13T00:01:07Z- AHRS_M2 msg: 1613174467.24 epoch sec; seqNo:8848 2021-02-13T00:01:07Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.18 psi 2021-02-13T00:01:07Z- handleDepth()-Depth_Keller msg: 1613174467248 epoch sec; seqNo:8919; depth 7.32 2021-02-13T00:01:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174467.26 epoch sec; seqNo:8919; 4058408.42 north; 591895.46 east 2021-02-13T00:01:07Z- AHRS_M2 msg: 1613174467.65 epoch sec; seqNo:8849 2021-02-13T00:01:07Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.17 psi 2021-02-13T00:01:07Z- handleDepth()-Depth_Keller msg: 1613174467652 epoch sec; seqNo:8920; depth 7.22 2021-02-13T00:01:07Z- handleDvl() - RDI_Pathfinder msg: 1613174467.66 epoch sec; seqNo:4288 2021-02-13T00:01:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 108.33 , 121.72 , 0.00 2021-02-13T00:01:07Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , 0.09 , -0.11 2021-02-13T00:01:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174467.67 epoch sec; seqNo:8920; 4058408.42 north; 591895.46 east 2021-02-13T00:01:08Z- AHRS_M2 msg: 1613174468.05 epoch sec; seqNo:8850 2021-02-13T00:01:08Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.16 psi 2021-02-13T00:01:08Z- handleDepth()-Depth_Keller msg: 1613174468056 epoch sec; seqNo:8921; depth 7.10 2021-02-13T00:01:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174468.08 epoch sec; seqNo:8921; 4058408.80 north; 591895.45 east 2021-02-13T00:01:08Z- AHRS_M2 msg: 1613174468.46 epoch sec; seqNo:8851 2021-02-13T00:01:08Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.15 psi 2021-02-13T00:01:08Z- handleDepth()-Depth_Keller msg: 1613174468460 epoch sec; seqNo:8922; depth 6.99 2021-02-13T00:01:08Z- handleDvl() - RDI_Pathfinder msg: 1613174468.47 epoch sec; seqNo:4289 2021-02-13T00:01:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.31 , 109.21 , 128.55 , 0.00 2021-02-13T00:01:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , -0.24 2021-02-13T00:01:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174468.49 epoch sec; seqNo:8922; 4058409.19 north; 591896.67 east 2021-02-13T00:01:08Z- AHRS_M2 msg: 1613174468.86 epoch sec; seqNo:8852 2021-02-13T00:01:08Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.14 psi 2021-02-13T00:01:08Z- handleDepth()-Depth_Keller msg: 1613174468864 epoch sec; seqNo:8923; depth 6.87 2021-02-13T00:01:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:08Z- handleDvl() - RDI_Pathfinder msg: 1613174468.87 epoch sec; seqNo:4290 2021-02-13T00:01:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , -0.24 2021-02-13T00:01:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174468.88 epoch sec; seqNo:8923; 4058409.19 north; 591896.67 east 2021-02-13T00:01:09Z- AHRS_M2 msg: 1613174469.26 epoch sec; seqNo:8853 2021-02-13T00:01:09Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.13 psi 2021-02-13T00:01:09Z- handleDepth()-Depth_Keller msg: 1613174469268 epoch sec; seqNo:8924; depth 6.78 2021-02-13T00:01:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174469.28 epoch sec; seqNo:8924; 4058409.57 north; 591896.67 east 2021-02-13T00:01:09Z- AHRS_M2 msg: 1613174469.67 epoch sec; seqNo:8854 2021-02-13T00:01:09Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.12 psi 2021-02-13T00:01:09Z- handleDepth()-Depth_Keller msg: 1613174469674 epoch sec; seqNo:8925; depth 6.67 2021-02-13T00:01:09Z- handleDvl() - RDI_Pathfinder msg: 1613174469.69 epoch sec; seqNo:4291 2021-02-13T00:01:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 106.17 , 124.02 , 82.37 2021-02-13T00:01:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.16 , -0.27 2021-02-13T00:01:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174469.70 epoch sec; seqNo:8925; 4058409.95 north; 591896.66 east 2021-02-13T00:01:09Z- LcmTrn::handleMessages() - handling 45 messages took 0.71 ms and 131 calls... 2021-02-13T00:01:09Z- waiting for 1613174467.46, time is 1613174469.70 2021-02-13T00:01:09Z- LcmTrn::updateTrn() equating pose 1613174469.70 and meas times 1613174469.70 2021-02-13T00:01:09Z- LcmTrn::updateTrn() >>>> pose time 1613174469.70, meas time 1613174469.70 <<<< 2021-02-13T00:01:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174469.70 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:01:09Z- TerrainNav::motionUpdate() - currEstimate = (4058410, 591896.66, 6.67) 2021-02-13T00:01:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:01:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:01:09Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174469.70 sec.; No relevant navigation data available 2021-02-13T00:01:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:01:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:01:09Z- variances([0], [2], and psi_berg) = 722.857309 833.011558 0.000000 2021-02-13T00:01:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:01:09Z- LcmTrn::publishEstimates() - MLE : 4058404.45 591945.16 6.67 2021-02-13T00:01:09Z- LcmTrn::publishEstimates() - MMSE : 4058409.95 591896.66 6.67 2021-02-13T00:01:09Z- LcmTrn::publishEstimates() - COVAR: 722.86 833.01 1.63 0.00 2021-02-13T00:01:10Z- AHRS_M2 msg: 1613174470.07 epoch sec; seqNo:8855 2021-02-13T00:01:10Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.12 psi 2021-02-13T00:01:10Z- handleDepth()-Depth_Keller msg: 1613174470075 epoch sec; seqNo:8926; depth 6.55 2021-02-13T00:01:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:10Z- handleDvl() - RDI_Pathfinder msg: 1613174470.08 epoch sec; seqNo:4292 2021-02-13T00:01:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.16 , -0.27 2021-02-13T00:01:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174470.10 epoch sec; seqNo:8926; 4058409.95 north; 591896.66 east 2021-02-13T00:01:10Z- AHRS_M2 msg: 1613174470.47 epoch sec; seqNo:8856 2021-02-13T00:01:10Z- AHRS_M2 msg: 0.68 phi; 0.31 theta; -0.13 psi 2021-02-13T00:01:10Z- handleDepth()-Depth_Keller msg: 1613174470485 epoch sec; seqNo:8927; depth 6.44 2021-02-13T00:01:10Z- handleDvl() - RDI_Pathfinder msg: 1613174470.50 epoch sec; seqNo:4293 2021-02-13T00:01:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 103.87 , 117.26 , 83.04 2021-02-13T00:01:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.61 , -0.31 , -0.08 2021-02-13T00:01:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174470.52 epoch sec; seqNo:8927; 4058410.32 north; 591896.66 east 2021-02-13T00:01:10Z- AHRS_M2 msg: 1613174470.88 epoch sec; seqNo:8857 2021-02-13T00:01:10Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.13 psi 2021-02-13T00:01:10Z- handleDepth()-Depth_Keller msg: 1613174470884 epoch sec; seqNo:8928; depth 6.31 2021-02-13T00:01:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174470.89 epoch sec; seqNo:8928; 4058410.70 north; 591896.65 east 2021-02-13T00:01:11Z- AHRS_M2 msg: 1613174471.28 epoch sec; seqNo:8858 2021-02-13T00:01:11Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.14 psi 2021-02-13T00:01:11Z- handleDepth()-Depth_Keller msg: 1613174471290 epoch sec; seqNo:8929; depth 6.21 2021-02-13T00:01:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:11Z- handleDvl() - RDI_Pathfinder msg: 1613174471.29 epoch sec; seqNo:4294 2021-02-13T00:01:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.61 , -0.31 , -0.08 2021-02-13T00:01:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174471.30 epoch sec; seqNo:8929; 4058410.70 north; 591896.65 east 2021-02-13T00:01:11Z- AHRS_M2 msg: 1613174471.69 epoch sec; seqNo:8859 2021-02-13T00:01:11Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.15 psi 2021-02-13T00:01:11Z- handleDepth()-Depth_Keller msg: 1613174471692 epoch sec; seqNo:8930; depth 6.09 2021-02-13T00:01:11Z- handleDvl() - RDI_Pathfinder msg: 1613174471.71 epoch sec; seqNo:4295 2021-02-13T00:01:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 106.44 , 119.96 , 83.45 2021-02-13T00:01:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.19 , 0.17 2021-02-13T00:01:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174471.73 epoch sec; seqNo:8930; 4058411.08 north; 591896.65 east 2021-02-13T00:01:12Z- AHRS_M2 msg: 1613174472.09 epoch sec; seqNo:8860 2021-02-13T00:01:12Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-13T00:01:12Z- handleDepth()-Depth_Keller msg: 1613174472096 epoch sec; seqNo:8931; depth 5.96 2021-02-13T00:01:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174472.11 epoch sec; seqNo:8931; 4058411.46 north; 591896.65 east 2021-02-13T00:01:12Z- AHRS_M2 msg: 1613174472.50 epoch sec; seqNo:8861 2021-02-13T00:01:12Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-13T00:01:12Z- handleDepth()-Depth_Keller msg: 1613174472509 epoch sec; seqNo:8932; depth 5.85 2021-02-13T00:01:12Z- handleDvl() - RDI_Pathfinder msg: 1613174472.52 epoch sec; seqNo:4296 2021-02-13T00:01:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 106.91 , 120.30 , 84.60 2021-02-13T00:01:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.21 2021-02-13T00:01:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174472.53 epoch sec; seqNo:8932; 4058411.85 north; 591897.86 east 2021-02-13T00:01:12Z- AHRS_M2 msg: 1613174472.90 epoch sec; seqNo:8862 2021-02-13T00:01:12Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-13T00:01:12Z- handleDepth()-Depth_Keller msg: 1613174472904 epoch sec; seqNo:8933; depth 5.75 2021-02-13T00:01:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174472.92 epoch sec; seqNo:8933; 4058411.85 north; 591897.86 east 2021-02-13T00:01:13Z- AHRS_M2 msg: 1613174473.30 epoch sec; seqNo:8863 2021-02-13T00:01:13Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.20 psi 2021-02-13T00:01:13Z- handleDepth()-Depth_Keller msg: 1613174473313 epoch sec; seqNo:8934; depth 5.63 2021-02-13T00:01:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174473.34 epoch sec; seqNo:8934; 4058412.23 north; 591897.86 east 2021-02-13T00:01:13Z- AHRS_M2 msg: 1613174473.71 epoch sec; seqNo:8864 2021-02-13T00:01:13Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.20 psi 2021-02-13T00:01:13Z- handleDepth()-Depth_Keller msg: 1613174473713 epoch sec; seqNo:8935; depth 5.53 2021-02-13T00:01:13Z- handleDvl() - RDI_Pathfinder msg: 1613174473.73 epoch sec; seqNo:4297 2021-02-13T00:01:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.48 , 108.33 , 120.23 , 84.53 2021-02-13T00:01:13Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.02 , 0.17 2021-02-13T00:01:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174473.74 epoch sec; seqNo:8935; 4058412.61 north; 591897.86 east 2021-02-13T00:01:14Z- AHRS_M2 msg: 1613174474.13 epoch sec; seqNo:8865 2021-02-13T00:01:14Z- AHRS_M2 msg: 0.63 phi; 0.28 theta; -0.21 psi 2021-02-13T00:01:14Z- handleDepth()-Depth_Keller msg: 1613174474150 epoch sec; seqNo:8936; depth 5.42 2021-02-13T00:01:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174474.17 epoch sec; seqNo:8936; 4058412.99 north; 591897.85 east 2021-02-13T00:01:14Z- AHRS_M2 msg: 1613174474.52 epoch sec; seqNo:8866 2021-02-13T00:01:14Z- AHRS_M2 msg: 0.63 phi; 0.26 theta; -0.21 psi 2021-02-13T00:01:14Z- handleDepth()-Depth_Keller msg: 1613174474524 epoch sec; seqNo:8937; depth 5.30 2021-02-13T00:01:14Z- handleDvl() - RDI_Pathfinder msg: 1613174474.54 epoch sec; seqNo:4298 2021-02-13T00:01:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.21 , 111.85 , 118.61 , 84.80 2021-02-13T00:01:14Z- handleDvl() - RDI_Pathfinder msg: velocities 1.08 , 0.03 , 0.14 2021-02-13T00:01:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174474.55 epoch sec; seqNo:8937; 4058413.38 north; 591899.07 east 2021-02-13T00:01:14Z- LcmTrn::handleMessages() - handling 43 messages took 0.69 ms and 130 calls... 2021-02-13T00:01:14Z- waiting for 1613174472.70, time is 1613174474.55 2021-02-13T00:01:14Z- LcmTrn::updateTrn() equating pose 1613174474.55 and meas times 1613174474.55 2021-02-13T00:01:14Z- LcmTrn::updateTrn() >>>> pose time 1613174474.55, meas time 1613174474.55 <<<< 2021-02-13T00:01:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174474.55 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:01:14Z- TerrainNav::motionUpdate() - currEstimate = (4058413, 591899.07, 5.30) 2021-02-13T00:01:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:01:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:01:14Z- TNavBF::Projecting Measurements 2021-02-13T00:01:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:01:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:01:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:01:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:01:14Z- TNavFilter::4 good beams. 2021-02-13T00:01:14Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:01:14Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:01:14Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:01:14Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:01:14Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:01:14Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:01:14Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:01:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:01:14Z- TerrainMapOctree: (vehN, vehE ) = (4058413.38, 591899.07). 2021-02-13T00:01:14Z- TerrainMapOctree: Min Distance = 3684.19. 2021-02-13T00:01:14Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:01:14Z- There is no specific good beam that all particles have in common. 2021-02-13T00:01:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:01:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.85 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 13.02 2021-02-13T00:01:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:01:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.64 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 20.02 2021-02-13T00:01:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:01:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.53 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 22.51 2021-02-13T00:01:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:01:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.44 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 11.51 2021-02-13T00:01:14Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:01:14Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:01:14Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:01:14Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:01:14Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:01:14Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:01:14Z- Filter 0 No beams used in subcloud update 2021-02-13T00:01:14Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:01:14Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:01:14Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 722.8 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:01:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174474.55 sec. 2021-02-13T00:01:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:01:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:01:14Z- variances([0], [2], and psi_berg) = 722.785942 833.011510 0.000000 2021-02-13T00:01:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:01:14Z- LcmTrn::publishEstimates() - MLE : 4058407.88 591947.57 5.30 2021-02-13T00:01:14Z- LcmTrn::publishEstimates() - MMSE : 4058413.38 591899.07 5.30 2021-02-13T00:01:14Z- LcmTrn::publishEstimates() - COVAR: 722.79 833.01 1.29 0.00 2021-02-13T00:01:14Z- AHRS_M2 msg: 1613174474.92 epoch sec; seqNo:8867 2021-02-13T00:01:14Z- AHRS_M2 msg: 0.63 phi; 0.23 theta; -0.21 psi 2021-02-13T00:01:14Z- handleDepth()-Depth_Keller msg: 1613174474924 epoch sec; seqNo:8938; depth 5.18 2021-02-13T00:01:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:14Z- handleDvl() - RDI_Pathfinder msg: 1613174474.93 epoch sec; seqNo:4299 2021-02-13T00:01:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:14Z- handleDvl() - RDI_Pathfinder msg: velocities 1.08 , 0.03 , 0.14 2021-02-13T00:01:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174474.94 epoch sec; seqNo:8938; 4058413.38 north; 591899.07 east 2021-02-13T00:01:15Z- AHRS_M2 msg: 1613174475.32 epoch sec; seqNo:8868 2021-02-13T00:01:15Z- AHRS_M2 msg: 0.65 phi; 0.20 theta; -0.20 psi 2021-02-13T00:01:15Z- handleDepth()-Depth_Keller msg: 1613174475333 epoch sec; seqNo:8939; depth 5.09 2021-02-13T00:01:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174475.34 epoch sec; seqNo:8939; 4058413.76 north; 591899.06 east 2021-02-13T00:01:15Z- AHRS_M2 msg: 1613174475.73 epoch sec; seqNo:8869 2021-02-13T00:01:15Z- AHRS_M2 msg: 0.66 phi; 0.16 theta; -0.21 psi 2021-02-13T00:01:15Z- handleDepth()-Depth_Keller msg: 1613174475734 epoch sec; seqNo:8940; depth 4.99 2021-02-13T00:01:15Z- handleDvl() - RDI_Pathfinder msg: 1613174475.74 epoch sec; seqNo:4300 2021-02-13T00:01:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:15Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:01:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174475.76 epoch sec; seqNo:8940; 4058414.14 north; 591899.06 east 2021-02-13T00:01:16Z- AHRS_M2 msg: 1613174476.13 epoch sec; seqNo:8870 2021-02-13T00:01:16Z- AHRS_M2 msg: 0.68 phi; 0.12 theta; -0.20 psi 2021-02-13T00:01:16Z- handleDepth()-Depth_Keller msg: 1613174476152 epoch sec; seqNo:8941; depth 4.91 2021-02-13T00:01:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:16Z- handleDvl() - RDI_Pathfinder msg: 1613174476.15 epoch sec; seqNo:4301 2021-02-13T00:01:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:16Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:01:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174476.17 epoch sec; seqNo:8941; 4058414.52 north; 591899.06 east 2021-02-13T00:01:16Z- AHRS_M2 msg: 1613174476.54 epoch sec; seqNo:8871 2021-02-13T00:01:16Z- AHRS_M2 msg: 0.69 phi; 0.07 theta; -0.16 psi 2021-02-13T00:01:16Z- handleDepth()-Depth_Keller msg: 1613174476545 epoch sec; seqNo:8942; depth 4.85 2021-02-13T00:01:16Z- handleDvl() - RDI_Pathfinder msg: 1613174476.56 epoch sec; seqNo:4302 2021-02-13T00:01:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 110.63 , 107.65 , 97.24 2021-02-13T00:01:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.22 , 0.10 2021-02-13T00:01:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174476.57 epoch sec; seqNo:8942; 4058414.91 north; 591900.27 east 2021-02-13T00:01:16Z- AHRS_M2 msg: 1613174476.94 epoch sec; seqNo:8872 2021-02-13T00:01:16Z- AHRS_M2 msg: 0.71 phi; 0.02 theta; -0.12 psi 2021-02-13T00:01:16Z- handleDepth()-Depth_Keller msg: 1613174476945 epoch sec; seqNo:8943; depth 4.78 2021-02-13T00:01:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174476.96 epoch sec; seqNo:8943; 4058414.91 north; 591900.27 east 2021-02-13T00:01:17Z- AHRS_M2 msg: 1613174477.35 epoch sec; seqNo:8873 2021-02-13T00:01:17Z- AHRS_M2 msg: 0.72 phi; -0.03 theta; -0.09 psi 2021-02-13T00:01:17Z- handleDepth()-Depth_Keller msg: 1613174477352 epoch sec; seqNo:8944; depth 4.72 2021-02-13T00:01:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:17Z- handleDvl() - RDI_Pathfinder msg: 1613174477.37 epoch sec; seqNo:4303 2021-02-13T00:01:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.22 , 0.10 2021-02-13T00:01:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174477.38 epoch sec; seqNo:8944; 4058415.29 north; 591900.27 east 2021-02-13T00:01:17Z- AHRS_M2 msg: 1613174477.76 epoch sec; seqNo:8874 2021-02-13T00:01:17Z- AHRS_M2 msg: 0.72 phi; -0.07 theta; -0.09 psi 2021-02-13T00:01:17Z- handleDepth()-Depth_Keller msg: 1613174477792 epoch sec; seqNo:8945; depth 4.66 2021-02-13T00:01:17Z- handleDvl() - RDI_Pathfinder msg: 1613174477.89 epoch sec; seqNo:4304 2021-02-13T00:01:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 100.83 , 97.85 , 97.85 2021-02-13T00:01:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , 0.32 , -0.05 2021-02-13T00:01:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174477.95 epoch sec; seqNo:8945; 4058415.30 north; 591901.49 east 2021-02-13T00:01:18Z- AHRS_M2 msg: 1613174478.16 epoch sec; seqNo:8875 2021-02-13T00:01:18Z- AHRS_M2 msg: 0.72 phi; -0.10 theta; -0.10 psi 2021-02-13T00:01:18Z- handleDepth()-Depth_Keller msg: 1613174478166 epoch sec; seqNo:8946; depth 4.60 2021-02-13T00:01:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174478.18 epoch sec; seqNo:8946; 4058415.30 north; 591901.49 east 2021-02-13T00:01:18Z- AHRS_M2 msg: 1613174478.56 epoch sec; seqNo:8876 2021-02-13T00:01:18Z- AHRS_M2 msg: 0.71 phi; -0.12 theta; -0.13 psi 2021-02-13T00:01:18Z- handleDepth()-Depth_Keller msg: 1613174478565 epoch sec; seqNo:8947; depth 4.58 2021-02-13T00:01:18Z- handleDvl() - RDI_Pathfinder msg: 1613174478.58 epoch sec; seqNo:4305 2021-02-13T00:01:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.76 , 99.88 , 97.17 , 99.88 2021-02-13T00:01:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.19 , -0.16 2021-02-13T00:01:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174478.59 epoch sec; seqNo:8947; 4058415.68 north; 591901.49 east 2021-02-13T00:01:18Z- AHRS_M2 msg: 1613174478.96 epoch sec; seqNo:8877 2021-02-13T00:01:18Z- AHRS_M2 msg: 0.71 phi; -0.14 theta; -0.17 psi 2021-02-13T00:01:18Z- handleDepth()-Depth_Keller msg: 1613174478967 epoch sec; seqNo:8948; depth 4.55 2021-02-13T00:01:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174478.99 epoch sec; seqNo:8948; 4058416.06 north; 591901.48 east 2021-02-13T00:01:19Z- AHRS_M2 msg: 1613174479.37 epoch sec; seqNo:8878 2021-02-13T00:01:19Z- AHRS_M2 msg: 0.70 phi; -0.15 theta; -0.21 psi 2021-02-13T00:01:19Z- handleDepth()-Depth_Keller msg: 1613174479372 epoch sec; seqNo:8949; depth 4.55 2021-02-13T00:01:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174479.38 epoch sec; seqNo:8949; 4058416.06 north; 591901.48 east 2021-02-13T00:01:19Z- AHRS_M2 msg: 1613174479.77 epoch sec; seqNo:8879 2021-02-13T00:01:19Z- AHRS_M2 msg: 0.69 phi; -0.17 theta; -0.24 psi 2021-02-13T00:01:19Z- handleDepth()-Depth_Keller msg: 1613174479781 epoch sec; seqNo:8950; depth 4.54 2021-02-13T00:01:19Z- handleDvl() - RDI_Pathfinder msg: 1613174479.79 epoch sec; seqNo:4306 2021-02-13T00:01:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.31 , 109.48 , 97.31 , 105.42 2021-02-13T00:01:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , -0.14 2021-02-13T00:01:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174479.80 epoch sec; seqNo:8950; 4058416.45 north; 591902.70 east 2021-02-13T00:01:19Z- LcmTrn::handleMessages() - handling 47 messages took 0.78 ms and 132 calls... 2021-02-13T00:01:19Z- waiting for 1613174477.55, time is 1613174479.80 2021-02-13T00:01:19Z- LcmTrn::updateTrn() equating pose 1613174479.80 and meas times 1613174479.80 2021-02-13T00:01:19Z- LcmTrn::updateTrn() >>>> pose time 1613174479.80, meas time 1613174479.80 <<<< 2021-02-13T00:01:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174479.80 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:01:19Z- TerrainNav::motionUpdate() - currEstimate = (4058416, 591902.70, 4.54) 2021-02-13T00:01:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:01:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:01:19Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174479.80 sec.; No relevant navigation data available 2021-02-13T00:01:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:01:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:01:19Z- variances([0], [2], and psi_berg) = 722.710020 833.011459 0.000000 2021-02-13T00:01:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:01:19Z- LcmTrn::publishEstimates() - MLE : 4058410.95 591951.20 4.54 2021-02-13T00:01:19Z- LcmTrn::publishEstimates() - MMSE : 4058416.45 591902.70 4.54 2021-02-13T00:01:19Z- LcmTrn::publishEstimates() - COVAR: 722.71 833.01 1.54 0.00 2021-02-13T00:01:20Z- AHRS_M2 msg: 1613174480.18 epoch sec; seqNo:8880 2021-02-13T00:01:20Z- AHRS_M2 msg: 0.68 phi; -0.18 theta; -0.27 psi 2021-02-13T00:01:20Z- handleDepth()-Depth_Keller msg: 1613174480186 epoch sec; seqNo:8951; depth 4.56 2021-02-13T00:01:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174480.20 epoch sec; seqNo:8951; 4058416.83 north; 591902.69 east 2021-02-13T00:01:20Z- AHRS_M2 msg: 1613174480.58 epoch sec; seqNo:8881 2021-02-13T00:01:20Z- AHRS_M2 msg: 0.67 phi; -0.19 theta; -0.28 psi 2021-02-13T00:01:20Z- handleDepth()-Depth_Keller msg: 1613174480594 epoch sec; seqNo:8952; depth 4.60 2021-02-13T00:01:20Z- handleDvl() - RDI_Pathfinder msg: 1613174480.61 epoch sec; seqNo:4307 2021-02-13T00:01:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 115.77 , 88.99 , 112.79 2021-02-13T00:01:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.12 , -0.17 2021-02-13T00:01:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174480.62 epoch sec; seqNo:8952; 4058417.21 north; 591902.69 east 2021-02-13T00:01:20Z- AHRS_M2 msg: 1613174480.98 epoch sec; seqNo:8882 2021-02-13T00:01:20Z- AHRS_M2 msg: 0.68 phi; -0.20 theta; -0.30 psi 2021-02-13T00:01:20Z- handleDepth()-Depth_Keller msg: 1613174480988 epoch sec; seqNo:8953; depth 4.63 2021-02-13T00:01:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:20Z- handleDvl() - RDI_Pathfinder msg: 1613174480.99 epoch sec; seqNo:4308 2021-02-13T00:01:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.12 , -0.17 2021-02-13T00:01:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174481.00 epoch sec; seqNo:8953; 4058417.21 north; 591902.69 east 2021-02-13T00:01:21Z- AHRS_M2 msg: 1613174481.39 epoch sec; seqNo:8883 2021-02-13T00:01:21Z- AHRS_M2 msg: 0.68 phi; -0.21 theta; -0.31 psi 2021-02-13T00:01:21Z- handleDepth()-Depth_Keller msg: 1613174481392 epoch sec; seqNo:8954; depth 4.66 2021-02-13T00:01:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174481.40 epoch sec; seqNo:8954; 4058417.59 north; 591902.69 east 2021-02-13T00:01:21Z- AHRS_M2 msg: 1613174481.80 epoch sec; seqNo:8884 2021-02-13T00:01:21Z- AHRS_M2 msg: 0.69 phi; -0.24 theta; -0.30 psi 2021-02-13T00:01:21Z- handleDepth()-Depth_Keller msg: 1613174481800 epoch sec; seqNo:8955; depth 4.72 2021-02-13T00:01:21Z- handleDvl() - RDI_Pathfinder msg: 1613174481.81 epoch sec; seqNo:4309 2021-02-13T00:01:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.17 , 122.87 , 88.65 , 113.94 2021-02-13T00:01:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.19 , -0.08 2021-02-13T00:01:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174481.82 epoch sec; seqNo:8955; 4058417.97 north; 591902.68 east 2021-02-13T00:01:22Z- AHRS_M2 msg: 1613174482.20 epoch sec; seqNo:8885 2021-02-13T00:01:22Z- AHRS_M2 msg: 0.70 phi; -0.26 theta; -0.28 psi 2021-02-13T00:01:22Z- handleDepth()-Depth_Keller msg: 1613174482200 epoch sec; seqNo:8956; depth 4.78 2021-02-13T00:01:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174482.22 epoch sec; seqNo:8956; 4058418.35 north; 591902.68 east 2021-02-13T00:01:22Z- AHRS_M2 msg: 1613174482.60 epoch sec; seqNo:8886 2021-02-13T00:01:22Z- AHRS_M2 msg: 0.69 phi; -0.29 theta; -0.25 psi 2021-02-13T00:01:22Z- handleDepth()-Depth_Keller msg: 1613174482603 epoch sec; seqNo:8957; depth 4.84 2021-02-13T00:01:22Z- handleDvl() - RDI_Pathfinder msg: 1613174482.62 epoch sec; seqNo:4310 2021-02-13T00:01:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.95 , 123.14 , 0.00 , 117.19 2021-02-13T00:01:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.16 , 0.07 2021-02-13T00:01:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174482.64 epoch sec; seqNo:8957; 4058418.74 north; 591903.89 east 2021-02-13T00:01:23Z- AHRS_M2 msg: 1613174483.01 epoch sec; seqNo:8887 2021-02-13T00:01:23Z- AHRS_M2 msg: 0.68 phi; -0.31 theta; -0.23 psi 2021-02-13T00:01:23Z- handleDepth()-Depth_Keller msg: 1613174483014 epoch sec; seqNo:8958; depth 4.91 2021-02-13T00:01:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174483.03 epoch sec; seqNo:8958; 4058418.74 north; 591903.89 east 2021-02-13T00:01:23Z- AHRS_M2 msg: 1613174483.41 epoch sec; seqNo:8888 2021-02-13T00:01:23Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.23 psi 2021-02-13T00:01:23Z- handleDepth()-Depth_Keller msg: 1613174483421 epoch sec; seqNo:8959; depth 4.98 2021-02-13T00:01:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174483.43 epoch sec; seqNo:8959; 4058419.12 north; 591903.89 east 2021-02-13T00:01:23Z- AHRS_M2 msg: 1613174483.81 epoch sec; seqNo:8889 2021-02-13T00:01:23Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.24 psi 2021-02-13T00:01:23Z- handleDepth()-Depth_Keller msg: 1613174483816 epoch sec; seqNo:8960; depth 5.05 2021-02-13T00:01:23Z- handleDvl() - RDI_Pathfinder msg: 1613174483.83 epoch sec; seqNo:4311 2021-02-13T00:01:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.45 , 112.25 , 85.48 , 125.64 2021-02-13T00:01:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.08 , 0.15 2021-02-13T00:01:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174483.84 epoch sec; seqNo:8960; 4058419.50 north; 591903.89 east 2021-02-13T00:01:24Z- AHRS_M2 msg: 1613174484.22 epoch sec; seqNo:8890 2021-02-13T00:01:24Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.25 psi 2021-02-13T00:01:24Z- handleDepth()-Depth_Keller msg: 1613174484221 epoch sec; seqNo:8961; depth 5.15 2021-02-13T00:01:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174484.24 epoch sec; seqNo:8961; 4058419.50 north; 591903.89 east 2021-02-13T00:01:24Z- AHRS_M2 msg: 1613174484.62 epoch sec; seqNo:8891 2021-02-13T00:01:24Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.26 psi 2021-02-13T00:01:24Z- handleDepth()-Depth_Keller msg: 1613174484624 epoch sec; seqNo:8962; depth 5.21 2021-02-13T00:01:24Z- handleDvl() - RDI_Pathfinder msg: 1613174484.64 epoch sec; seqNo:4312 2021-02-13T00:01:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.41 , 117.19 , 84.46 , 129.09 2021-02-13T00:01:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.01 , 0.10 2021-02-13T00:01:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174484.66 epoch sec; seqNo:8962; 4058419.87 north; 591903.88 east 2021-02-13T00:01:24Z- LcmTrn::handleMessages() - handling 42 messages took 0.62 ms and 130 calls... 2021-02-13T00:01:24Z- waiting for 1613174482.80, time is 1613174484.66 2021-02-13T00:01:24Z- LcmTrn::updateTrn() equating pose 1613174484.66 and meas times 1613174484.66 2021-02-13T00:01:24Z- LcmTrn::updateTrn() >>>> pose time 1613174484.66, meas time 1613174484.66 <<<< 2021-02-13T00:01:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174484.66 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:01:24Z- TerrainNav::motionUpdate() - currEstimate = (4058420, 591903.88, 5.21) 2021-02-13T00:01:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:01:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:01:24Z- TNavBF::Projecting Measurements 2021-02-13T00:01:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:01:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:01:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:01:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:01:24Z- TNavFilter::4 good beams. 2021-02-13T00:01:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:01:24Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:01:24Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:01:24Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:01:24Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:01:24Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:01:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:01:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:01:24Z- TerrainMapOctree: (vehN, vehE ) = (4058419.87, 591903.88). 2021-02-13T00:01:24Z- TerrainMapOctree: Min Distance = 3676.12. 2021-02-13T00:01:24Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:01:24Z- There is no specific good beam that all particles have in common. 2021-02-13T00:01:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:01:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.72 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 13.08 2021-02-13T00:01:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:01:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.54 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 21.97 2021-02-13T00:01:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:01:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.45 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 11.41 2021-02-13T00:01:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:01:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 26.66 2021-02-13T00:01:25Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:01:25Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:01:25Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:01:25Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:01:25Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:01:25Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:01:25Z- Filter 0 No beams used in subcloud update 2021-02-13T00:01:25Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:01:25Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:01:25Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 722.6 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:01:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174484.66 sec. 2021-02-13T00:01:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:01:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:01:25Z- variances([0], [2], and psi_berg) = 722.643811 833.011414 0.000000 2021-02-13T00:01:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:01:25Z- LcmTrn::publishEstimates() - MLE : 4058414.37 591943.38 5.21 2021-02-13T00:01:25Z- LcmTrn::publishEstimates() - MMSE : 4058419.87 591903.88 5.21 2021-02-13T00:01:25Z- LcmTrn::publishEstimates() - COVAR: 722.64 833.01 1.25 0.00 2021-02-13T00:01:25Z- AHRS_M2 msg: 1613174485.02 epoch sec; seqNo:8892 2021-02-13T00:01:25Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.26 psi 2021-02-13T00:01:25Z- handleDepth()-Depth_Keller msg: 1613174485027 epoch sec; seqNo:8963; depth 5.32 2021-02-13T00:01:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174485.04 epoch sec; seqNo:8963; 4058419.87 north; 591903.88 east 2021-02-13T00:01:25Z- AHRS_M2 msg: 1613174485.43 epoch sec; seqNo:8893 2021-02-13T00:01:25Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.25 psi 2021-02-13T00:01:25Z- handleDepth()-Depth_Keller msg: 1613174485436 epoch sec; seqNo:8964; depth 5.43 2021-02-13T00:01:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174485.44 epoch sec; seqNo:8964; 4058420.25 north; 591903.88 east 2021-02-13T00:01:25Z- AHRS_M2 msg: 1613174485.83 epoch sec; seqNo:8894 2021-02-13T00:01:25Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.23 psi 2021-02-13T00:01:25Z- handleDepth()-Depth_Keller msg: 1613174485836 epoch sec; seqNo:8965; depth 5.53 2021-02-13T00:01:25Z- handleDvl() - RDI_Pathfinder msg: 1613174485.86 epoch sec; seqNo:4313 2021-02-13T00:01:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.65 , 118.47 , 84.26 , 128.89 2021-02-13T00:01:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , -0.02 2021-02-13T00:01:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174485.87 epoch sec; seqNo:8965; 4058420.64 north; 591905.09 east 2021-02-13T00:01:26Z- AHRS_M2 msg: 1613174486.24 epoch sec; seqNo:8895 2021-02-13T00:01:26Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.22 psi 2021-02-13T00:01:26Z- handleDepth()-Depth_Keller msg: 1613174486240 epoch sec; seqNo:8966; depth 5.66 2021-02-13T00:01:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174486.25 epoch sec; seqNo:8966; 4058420.64 north; 591905.09 east 2021-02-13T00:01:26Z- AHRS_M2 msg: 1613174486.66 epoch sec; seqNo:8896 2021-02-13T00:01:26Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.21 psi 2021-02-13T00:01:26Z- handleDepth()-Depth_Keller msg: 1613174486665 epoch sec; seqNo:8967; depth 5.78 2021-02-13T00:01:26Z- handleDvl() - RDI_Pathfinder msg: 1613174486.69 epoch sec; seqNo:4314 2021-02-13T00:01:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.07 , 113.88 , 84.12 , 122.80 2021-02-13T00:01:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.11 , -0.12 2021-02-13T00:01:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174486.70 epoch sec; seqNo:8967; 4058421.02 north; 591905.09 east 2021-02-13T00:01:27Z- AHRS_M2 msg: 1613174487.05 epoch sec; seqNo:8897 2021-02-13T00:01:27Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.21 psi 2021-02-13T00:01:27Z- handleDepth()-Depth_Keller msg: 1613174487050 epoch sec; seqNo:8968; depth 5.88 2021-02-13T00:01:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:27Z- handleDvl() - RDI_Pathfinder msg: 1613174487.05 epoch sec; seqNo:4315 2021-02-13T00:01:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.11 , -0.12 2021-02-13T00:01:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174487.06 epoch sec; seqNo:8968; 4058421.40 north; 591905.09 east 2021-02-13T00:01:27Z- AHRS_M2 msg: 1613174487.45 epoch sec; seqNo:8898 2021-02-13T00:01:27Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.21 psi 2021-02-13T00:01:27Z- handleDepth()-Depth_Keller msg: 1613174487452 epoch sec; seqNo:8969; depth 6.00 2021-02-13T00:01:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174487.47 epoch sec; seqNo:8969; 4058421.78 north; 591905.08 east 2021-02-13T00:01:27Z- AHRS_M2 msg: 1613174487.85 epoch sec; seqNo:8899 2021-02-13T00:01:27Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.20 psi 2021-02-13T00:01:27Z- handleDepth()-Depth_Keller msg: 1613174487856 epoch sec; seqNo:8970; depth 6.11 2021-02-13T00:01:27Z- handleDvl() - RDI_Pathfinder msg: 1613174487.87 epoch sec; seqNo:4316 2021-02-13T00:01:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.41 , 115.70 , 84.46 , 126.11 2021-02-13T00:01:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.29 , 0.00 2021-02-13T00:01:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174487.89 epoch sec; seqNo:8970; 4058421.78 north; 591905.08 east 2021-02-13T00:01:28Z- AHRS_M2 msg: 1613174488.26 epoch sec; seqNo:8900 2021-02-13T00:01:28Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.19 psi 2021-02-13T00:01:28Z- handleDepth()-Depth_Keller msg: 1613174488260 epoch sec; seqNo:8971; depth 6.23 2021-02-13T00:01:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:28Z- handleDvl() - RDI_Pathfinder msg: 1613174488.26 epoch sec; seqNo:4317 2021-02-13T00:01:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.29 , 0.00 2021-02-13T00:01:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174488.27 epoch sec; seqNo:8971; 4058422.16 north; 591905.08 east 2021-02-13T00:01:28Z- AHRS_M2 msg: 1613174488.66 epoch sec; seqNo:8901 2021-02-13T00:01:28Z- AHRS_M2 msg: 0.69 phi; -0.36 theta; -0.17 psi 2021-02-13T00:01:28Z- handleDepth()-Depth_Keller msg: 1613174488668 epoch sec; seqNo:8972; depth 6.33 2021-02-13T00:01:28Z- handleDvl() - RDI_Pathfinder msg: 1613174488.68 epoch sec; seqNo:4318 2021-02-13T00:01:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.35 , 111.17 , 82.91 , 124.56 2021-02-13T00:01:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.24 , 0.30 2021-02-13T00:01:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174488.69 epoch sec; seqNo:8972; 4058422.54 north; 591905.07 east 2021-02-13T00:01:29Z- AHRS_M2 msg: 1613174489.06 epoch sec; seqNo:8902 2021-02-13T00:01:29Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.15 psi 2021-02-13T00:01:29Z- handleDepth()-Depth_Keller msg: 1613174489074 epoch sec; seqNo:8973; depth 6.47 2021-02-13T00:01:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174489.09 epoch sec; seqNo:8973; 4058422.55 north; 591906.30 east 2021-02-13T00:01:29Z- AHRS_M2 msg: 1613174489.47 epoch sec; seqNo:8903 2021-02-13T00:01:29Z- AHRS_M2 msg: 0.69 phi; -0.38 theta; -0.14 psi 2021-02-13T00:01:29Z- handleDepth()-Depth_Keller msg: 1613174489473 epoch sec; seqNo:8974; depth 6.58 2021-02-13T00:01:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:29Z- handleDvl() - RDI_Pathfinder msg: 1613174489.47 epoch sec; seqNo:4319 2021-02-13T00:01:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.24 , 0.30 2021-02-13T00:01:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174489.51 epoch sec; seqNo:8974; 4058422.93 north; 591906.29 east 2021-02-13T00:01:29Z- AHRS_M2 msg: 1613174489.87 epoch sec; seqNo:8904 2021-02-13T00:01:29Z- AHRS_M2 msg: 0.69 phi; -0.39 theta; -0.12 psi 2021-02-13T00:01:29Z- handleDepth()-Depth_Keller msg: 1613174489876 epoch sec; seqNo:8975; depth 6.69 2021-02-13T00:01:29Z- handleDvl() - RDI_Pathfinder msg: 1613174489.89 epoch sec; seqNo:4320 2021-02-13T00:01:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.46 , 108.33 , 78.48 , 129.16 2021-02-13T00:01:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , 0.07 , 0.46 2021-02-13T00:01:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174489.90 epoch sec; seqNo:8975; 4058422.93 north; 591906.29 east 2021-02-13T00:01:30Z- LcmTrn::handleMessages() - handling 47 messages took 0.67 ms and 134 calls... 2021-02-13T00:01:30Z- waiting for 1613174487.66, time is 1613174489.90 2021-02-13T00:01:30Z- LcmTrn::updateTrn() equating pose 1613174489.90 and meas times 1613174489.90 2021-02-13T00:01:30Z- LcmTrn::updateTrn() >>>> pose time 1613174489.90, meas time 1613174489.90 <<<< 2021-02-13T00:01:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174489.90 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:01:30Z- TerrainNav::motionUpdate() - currEstimate = (4058423, 591906.29, 6.69) 2021-02-13T00:01:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:01:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:01:30Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174489.90 sec.; No relevant navigation data available 2021-02-13T00:01:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:01:30Z- TNavBF::mmse selected from filter -1 2021-02-13T00:01:30Z- variances([0], [2], and psi_berg) = 722.575228 833.011368 0.000000 2021-02-13T00:01:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:01:30Z- LcmTrn::publishEstimates() - MLE : 4058417.43 591945.79 6.69 2021-02-13T00:01:30Z- LcmTrn::publishEstimates() - MMSE : 4058422.93 591906.29 6.69 2021-02-13T00:01:30Z- LcmTrn::publishEstimates() - COVAR: 722.58 833.01 1.45 0.00 2021-02-13T00:01:30Z- AHRS_M2 msg: 1613174490.28 epoch sec; seqNo:8905 2021-02-13T00:01:30Z- AHRS_M2 msg: 0.67 phi; -0.40 theta; -0.12 psi 2021-02-13T00:01:30Z- handleDepth()-Depth_Keller msg: 1613174490281 epoch sec; seqNo:8976; depth 6.81 2021-02-13T00:01:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174490.29 epoch sec; seqNo:8976; 4058423.31 north; 591906.29 east 2021-02-13T00:01:30Z- AHRS_M2 msg: 1613174490.68 epoch sec; seqNo:8906 2021-02-13T00:01:30Z- AHRS_M2 msg: 0.66 phi; -0.40 theta; -0.13 psi 2021-02-13T00:01:30Z- handleDepth()-Depth_Keller msg: 1613174490685 epoch sec; seqNo:8977; depth 6.95 2021-02-13T00:01:30Z- handleDvl() - RDI_Pathfinder msg: 1613174490.70 epoch sec; seqNo:4321 2021-02-13T00:01:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.43 , 106.84 , 81.56 , 139.57 2021-02-13T00:01:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.45 , 0.38 2021-02-13T00:01:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174490.72 epoch sec; seqNo:8977; 4058423.31 north; 591906.29 east 2021-02-13T00:01:31Z- AHRS_M2 msg: 1613174491.09 epoch sec; seqNo:8907 2021-02-13T00:01:31Z- AHRS_M2 msg: 0.64 phi; -0.39 theta; -0.16 psi 2021-02-13T00:01:31Z- handleDepth()-Depth_Keller msg: 1613174491094 epoch sec; seqNo:8978; depth 7.07 2021-02-13T00:01:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174491.11 epoch sec; seqNo:8978; 4058423.31 north; 591906.29 east 2021-02-13T00:01:31Z- AHRS_M2 msg: 1613174491.49 epoch sec; seqNo:8908 2021-02-13T00:01:31Z- AHRS_M2 msg: 0.64 phi; -0.39 theta; -0.19 psi 2021-02-13T00:01:31Z- handleDepth()-Depth_Keller msg: 1613174491498 epoch sec; seqNo:8979; depth 7.20 2021-02-13T00:01:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174491.51 epoch sec; seqNo:8979; 4058423.32 north; 591907.51 east 2021-02-13T00:01:31Z- AHRS_M2 msg: 1613174491.89 epoch sec; seqNo:8909 2021-02-13T00:01:31Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.22 psi 2021-02-13T00:01:31Z- handleDepth()-Depth_Keller msg: 1613174491896 epoch sec; seqNo:8980; depth 7.31 2021-02-13T00:01:31Z- handleDvl() - RDI_Pathfinder msg: 1613174491.92 epoch sec; seqNo:4322 2021-02-13T00:01:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.58 , 109.48 , 0.00 , 128.82 2021-02-13T00:01:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.57 , -0.01 2021-02-13T00:01:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174491.93 epoch sec; seqNo:8980; 4058423.70 north; 591907.50 east 2021-02-13T00:01:32Z- AHRS_M2 msg: 1613174492.30 epoch sec; seqNo:8910 2021-02-13T00:01:32Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.24 psi 2021-02-13T00:01:32Z- handleDepth()-Depth_Keller msg: 1613174492305 epoch sec; seqNo:8981; depth 7.45 2021-02-13T00:01:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174492.32 epoch sec; seqNo:8981; 4058423.70 north; 591907.50 east 2021-02-13T00:01:32Z- AHRS_M2 msg: 1613174492.71 epoch sec; seqNo:8911 2021-02-13T00:01:32Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.26 psi 2021-02-13T00:01:32Z- handleDepth()-Depth_Keller msg: 1613174492717 epoch sec; seqNo:8982; depth 7.57 2021-02-13T00:01:32Z- handleDvl() - RDI_Pathfinder msg: 1613174492.73 epoch sec; seqNo:4323 2021-02-13T00:01:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 115.09 , 82.37 , 131.46 2021-02-13T00:01:32Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.29 , -0.39 2021-02-13T00:01:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174492.74 epoch sec; seqNo:8982; 4058424.09 north; 591908.72 east 2021-02-13T00:01:33Z- AHRS_M2 msg: 1613174493.10 epoch sec; seqNo:8912 2021-02-13T00:01:33Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.26 psi 2021-02-13T00:01:33Z- handleDepth()-Depth_Keller msg: 1613174493120 epoch sec; seqNo:8983; depth 7.71 2021-02-13T00:01:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:33Z- handleDvl() - RDI_Pathfinder msg: 1613174493.12 epoch sec; seqNo:4324 2021-02-13T00:01:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:33Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.29 , -0.39 2021-02-13T00:01:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174493.14 epoch sec; seqNo:8983; 4058424.47 north; 591908.72 east 2021-02-13T00:01:33Z- AHRS_M2 msg: 1613174493.51 epoch sec; seqNo:8913 2021-02-13T00:01:33Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.26 psi 2021-02-13T00:01:33Z- handleDepth()-Depth_Keller msg: 1613174493512 epoch sec; seqNo:8984; depth 7.84 2021-02-13T00:01:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174493.52 epoch sec; seqNo:8984; 4058424.85 north; 591908.71 east 2021-02-13T00:01:33Z- AHRS_M2 msg: 1613174493.91 epoch sec; seqNo:8914 2021-02-13T00:01:33Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.25 psi 2021-02-13T00:01:33Z- handleDepth()-Depth_Keller msg: 1613174493916 epoch sec; seqNo:8985; depth 7.95 2021-02-13T00:01:33Z- handleDvl() - RDI_Pathfinder msg: 1613174493.93 epoch sec; seqNo:4325 2021-02-13T00:01:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.65 , 119.89 , 82.70 , 128.82 2021-02-13T00:01:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.15 , -0.41 2021-02-13T00:01:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174493.94 epoch sec; seqNo:8985; 4058425.23 north; 591908.71 east 2021-02-13T00:01:34Z- AHRS_M2 msg: 1613174494.32 epoch sec; seqNo:8915 2021-02-13T00:01:34Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.23 psi 2021-02-13T00:01:34Z- handleDepth()-Depth_Keller msg: 1613174494321 epoch sec; seqNo:8986; depth 8.10 2021-02-13T00:01:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:34Z- handleDvl() - RDI_Pathfinder msg: 1613174494.32 epoch sec; seqNo:4326 2021-02-13T00:01:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.15 , -0.41 2021-02-13T00:01:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174494.34 epoch sec; seqNo:8986; 4058425.61 north; 591908.70 east 2021-02-13T00:01:34Z- AHRS_M2 msg: 1613174494.74 epoch sec; seqNo:8916 2021-02-13T00:01:34Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.21 psi 2021-02-13T00:01:34Z- handleDepth()-Depth_Keller msg: 1613174494755 epoch sec; seqNo:8987; depth 8.22 2021-02-13T00:01:34Z- handleDvl() - RDI_Pathfinder msg: 1613174494.77 epoch sec; seqNo:4327 2021-02-13T00:01:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.24 , 115.50 , 81.28 , 125.91 2021-02-13T00:01:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.32 , -0.19 2021-02-13T00:01:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174494.78 epoch sec; seqNo:8987; 4058425.99 north; 591908.70 east 2021-02-13T00:01:35Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-13T00:01:35Z- waiting for 1613174492.90, time is 1613174494.78 2021-02-13T00:01:35Z- LcmTrn::updateTrn() equating pose 1613174494.78 and meas times 1613174494.78 2021-02-13T00:01:35Z- LcmTrn::updateTrn() >>>> pose time 1613174494.78, meas time 1613174494.78 <<<< 2021-02-13T00:01:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174494.78 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:01:35Z- TerrainNav::motionUpdate() - currEstimate = (4058426, 591908.70, 8.22) 2021-02-13T00:01:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:01:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:01:35Z- TNavBF::Projecting Measurements 2021-02-13T00:01:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:01:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:01:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:01:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:01:35Z- TNavFilter::4 good beams. 2021-02-13T00:01:35Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:01:35Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:01:35Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:01:35Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:01:35Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:01:35Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:01:35Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:01:35Z- TNavBF::mmse selected from filter -1 2021-02-13T00:01:35Z- TerrainMapOctree: (vehN, vehE ) = (4058425.99, 591908.70). 2021-02-13T00:01:35Z- TerrainMapOctree: Min Distance = 3668.34. 2021-02-13T00:01:35Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:01:35Z- There is no specific good beam that all particles have in common. 2021-02-13T00:01:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:01:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.64 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 12.18 2021-02-13T00:01:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:01:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.47 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 21.34 2021-02-13T00:01:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:01:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 10.57 2021-02-13T00:01:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:01:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 25.37 2021-02-13T00:01:35Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:01:35Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:01:35Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:01:35Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:01:35Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:01:35Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:01:35Z- Filter 0 No beams used in subcloud update 2021-02-13T00:01:35Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:01:35Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:01:35Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 722.5 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:01:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174494.78 sec. 2021-02-13T00:01:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:01:35Z- TNavBF::mmse selected from filter -1 2021-02-13T00:01:35Z- variances([0], [2], and psi_berg) = 722.506700 833.011322 0.000000 2021-02-13T00:01:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:01:35Z- LcmTrn::publishEstimates() - MLE : 4058420.49 591948.20 8.22 2021-02-13T00:01:35Z- LcmTrn::publishEstimates() - MMSE : 4058425.99 591908.70 8.22 2021-02-13T00:01:35Z- LcmTrn::publishEstimates() - COVAR: 722.51 833.01 1.19 0.00 2021-02-13T00:01:35Z- AHRS_M2 msg: 1613174495.13 epoch sec; seqNo:8917 2021-02-13T00:01:35Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.19 psi 2021-02-13T00:01:35Z- handleDepth()-Depth_Keller msg: 1613174495132 epoch sec; seqNo:8988; depth 8.37 2021-02-13T00:01:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174495.15 epoch sec; seqNo:8988; 4058426.38 north; 591909.92 east 2021-02-13T00:01:35Z- AHRS_M2 msg: 1613174495.53 epoch sec; seqNo:8918 2021-02-13T00:01:35Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.18 psi 2021-02-13T00:01:35Z- handleDepth()-Depth_Keller msg: 1613174495541 epoch sec; seqNo:8989; depth 8.48 2021-02-13T00:01:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:35Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:35Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:35Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:35Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:35Z- handleDvl() - RDI_Pathfinder msg: 1613174495.54 epoch sec; seqNo:4328 2021-02-13T00:01:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.32 , -0.19 2021-02-13T00:01:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174495.55 epoch sec; seqNo:8989; 4058426.76 north; 591909.91 east 2021-02-13T00:01:35Z- AHRS_M2 msg: 1613174495.93 epoch sec; seqNo:8919 2021-02-13T00:01:35Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.18 psi 2021-02-13T00:01:35Z- handleDepth()-Depth_Keller msg: 1613174495936 epoch sec; seqNo:8990; depth 8.63 2021-02-13T00:01:35Z- handleDvl() - RDI_Pathfinder msg: 1613174495.95 epoch sec; seqNo:4329 2021-02-13T00:01:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 108.33 , 77.66 , 123.21 2021-02-13T00:01:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.20 , 0.01 2021-02-13T00:01:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174495.97 epoch sec; seqNo:8990; 4058426.76 north; 591909.91 east 2021-02-13T00:01:36Z- AHRS_M2 msg: 1613174496.34 epoch sec; seqNo:8920 2021-02-13T00:01:36Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.18 psi 2021-02-13T00:01:36Z- handleDepth()-Depth_Keller msg: 1613174496340 epoch sec; seqNo:8991; depth 8.76 2021-02-13T00:01:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174496.35 epoch sec; seqNo:8991; 4058427.14 north; 591909.91 east 2021-02-13T00:01:36Z- AHRS_M2 msg: 1613174496.76 epoch sec; seqNo:8921 2021-02-13T00:01:36Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.18 psi 2021-02-13T00:01:36Z- handleDepth()-Depth_Keller msg: 1613174496763 epoch sec; seqNo:8992; depth 8.89 2021-02-13T00:01:36Z- handleDvl() - RDI_Pathfinder msg: 1613174496.79 epoch sec; seqNo:4330 2021-02-13T00:01:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 107.93 , 79.53 , 122.80 2021-02-13T00:01:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.06 , 0.05 2021-02-13T00:01:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174496.80 epoch sec; seqNo:8992; 4058427.52 north; 591909.90 east 2021-02-13T00:01:37Z- AHRS_M2 msg: 1613174497.15 epoch sec; seqNo:8922 2021-02-13T00:01:37Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.18 psi 2021-02-13T00:01:37Z- handleDepth()-Depth_Keller msg: 1613174497156 epoch sec; seqNo:8993; depth 9.03 2021-02-13T00:01:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174497.17 epoch sec; seqNo:8993; 4058427.90 north; 591909.90 east 2021-02-13T00:01:37Z- AHRS_M2 msg: 1613174497.55 epoch sec; seqNo:8923 2021-02-13T00:01:37Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.19 psi 2021-02-13T00:01:37Z- handleDepth()-Depth_Keller msg: 1613174497557 epoch sec; seqNo:8994; depth 9.16 2021-02-13T00:01:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174497.57 epoch sec; seqNo:8994; 4058427.90 north; 591909.90 east 2021-02-13T00:01:37Z- AHRS_M2 msg: 1613174497.95 epoch sec; seqNo:8924 2021-02-13T00:01:37Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.19 psi 2021-02-13T00:01:37Z- handleDepth()-Depth_Keller msg: 1613174497961 epoch sec; seqNo:8995; depth 9.29 2021-02-13T00:01:37Z- handleDvl() - RDI_Pathfinder msg: 1613174497.97 epoch sec; seqNo:4331 2021-02-13T00:01:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 106.51 , 78.48 , 122.73 2021-02-13T00:01:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , -0.03 2021-02-13T00:01:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174497.98 epoch sec; seqNo:8995; 4058428.27 north; 591909.90 east 2021-02-13T00:01:38Z- AHRS_M2 msg: 1613174498.36 epoch sec; seqNo:8925 2021-02-13T00:01:38Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.20 psi 2021-02-13T00:01:38Z- handleDepth()-Depth_Keller msg: 1613174498362 epoch sec; seqNo:8996; depth 9.41 2021-02-13T00:01:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174498.37 epoch sec; seqNo:8996; 4058428.67 north; 591911.11 east 2021-02-13T00:01:38Z- AHRS_M2 msg: 1613174498.76 epoch sec; seqNo:8926 2021-02-13T00:01:38Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.20 psi 2021-02-13T00:01:38Z- handleDepth()-Depth_Keller msg: 1613174498764 epoch sec; seqNo:8997; depth 9.55 2021-02-13T00:01:38Z- handleDvl() - RDI_Pathfinder msg: 1613174498.79 epoch sec; seqNo:4332 2021-02-13T00:01:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 106.57 , 79.53 , 120.10 2021-02-13T00:01:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.13 , -0.07 2021-02-13T00:01:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174498.80 epoch sec; seqNo:8997; 4058429.04 north; 591911.11 east 2021-02-13T00:01:39Z- AHRS_M2 msg: 1613174499.16 epoch sec; seqNo:8927 2021-02-13T00:01:39Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.20 psi 2021-02-13T00:01:39Z- handleDepth()-Depth_Keller msg: 1613174499173 epoch sec; seqNo:8998; depth 9.68 2021-02-13T00:01:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:39Z- handleDvl() - RDI_Pathfinder msg: 1613174499.17 epoch sec; seqNo:4333 2021-02-13T00:01:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.13 , -0.07 2021-02-13T00:01:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174499.18 epoch sec; seqNo:8998; 4058429.42 north; 591911.11 east 2021-02-13T00:01:39Z- AHRS_M2 msg: 1613174499.57 epoch sec; seqNo:8928 2021-02-13T00:01:39Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-13T00:01:39Z- handleDepth()-Depth_Keller msg: 1613174499572 epoch sec; seqNo:8999; depth 9.84 2021-02-13T00:01:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174499.58 epoch sec; seqNo:8999; 4058429.42 north; 591911.11 east 2021-02-13T00:01:39Z- AHRS_M2 msg: 1613174499.97 epoch sec; seqNo:8929 2021-02-13T00:01:39Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-13T00:01:39Z- handleDepth()-Depth_Keller msg: 1613174499977 epoch sec; seqNo:9000; depth 9.97 2021-02-13T00:01:39Z- handleDvl() - RDI_Pathfinder msg: 1613174499.99 epoch sec; seqNo:4334 2021-02-13T00:01:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.02 , 106.84 , 80.07 , 117.26 2021-02-13T00:01:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.19 , -0.01 2021-02-13T00:01:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174500.00 epoch sec; seqNo:9000; 4058429.80 north; 591911.10 east 2021-02-13T00:01:40Z- LcmTrn::handleMessages() - handling 46 messages took 0.58 ms and 135 calls... 2021-02-13T00:01:40Z- waiting for 1613174497.78, time is 1613174500.00 2021-02-13T00:01:40Z- LcmTrn::updateTrn() equating pose 1613174500.00 and meas times 1613174500.00 2021-02-13T00:01:40Z- LcmTrn::updateTrn() >>>> pose time 1613174500.00, meas time 1613174500.00 <<<< 2021-02-13T00:01:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174500.00 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:01:40Z- TerrainNav::motionUpdate() - currEstimate = (4058430, 591911.10, 9.97) 2021-02-13T00:01:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:01:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:01:40Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174500.00 sec.; No relevant navigation data available 2021-02-13T00:01:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:01:40Z- TNavBF::mmse selected from filter -1 2021-02-13T00:01:40Z- variances([0], [2], and psi_berg) = 722.433011 833.011273 0.000000 2021-02-13T00:01:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:01:40Z- LcmTrn::publishEstimates() - MLE : 4058425.30 591862.60 9.97 2021-02-13T00:01:40Z- LcmTrn::publishEstimates() - MMSE : 4058429.80 591911.10 9.97 2021-02-13T00:01:40Z- LcmTrn::publishEstimates() - COVAR: 722.43 833.01 1.42 0.00 2021-02-13T00:01:40Z- AHRS_M2 msg: 1613174500.38 epoch sec; seqNo:8930 2021-02-13T00:01:40Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-13T00:01:40Z- handleDepth()-Depth_Keller msg: 1613174500380 epoch sec; seqNo:9001; depth 10.10 2021-02-13T00:01:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:40Z- handleDvl() - RDI_Pathfinder msg: 1613174500.38 epoch sec; seqNo:4335 2021-02-13T00:01:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.19 , -0.01 2021-02-13T00:01:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174500.42 epoch sec; seqNo:9001; 4058430.18 north; 591911.10 east 2021-02-13T00:01:40Z- AHRS_M2 msg: 1613174500.78 epoch sec; seqNo:8931 2021-02-13T00:01:40Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.20 psi 2021-02-13T00:01:40Z- handleDepth()-Depth_Keller msg: 1613174500784 epoch sec; seqNo:9002; depth 10.24 2021-02-13T00:01:40Z- handleDvl() - RDI_Pathfinder msg: 1613174500.80 epoch sec; seqNo:4336 2021-02-13T00:01:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.07 , 105.36 , 78.88 , 120.23 2021-02-13T00:01:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.16 , 0.09 2021-02-13T00:01:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174500.81 epoch sec; seqNo:9002; 4058430.56 north; 591911.09 east 2021-02-13T00:01:41Z- AHRS_M2 msg: 1613174501.18 epoch sec; seqNo:8932 2021-02-13T00:01:41Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.21 psi 2021-02-13T00:01:41Z- handleDepth()-Depth_Keller msg: 1613174501190 epoch sec; seqNo:9003; depth 10.36 2021-02-13T00:01:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174501.20 epoch sec; seqNo:9003; 4058430.56 north; 591911.09 east 2021-02-13T00:01:41Z- AHRS_M2 msg: 1613174501.59 epoch sec; seqNo:8933 2021-02-13T00:01:41Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.22 psi 2021-02-13T00:01:41Z- handleDepth()-Depth_Keller msg: 1613174501601 epoch sec; seqNo:9004; depth 10.50 2021-02-13T00:01:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:41Z- handleDvl() - RDI_Pathfinder msg: 1613174501.60 epoch sec; seqNo:4337 2021-02-13T00:01:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.16 , 0.09 2021-02-13T00:01:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174501.61 epoch sec; seqNo:9004; 4058430.94 north; 591911.09 east 2021-02-13T00:01:41Z- AHRS_M2 msg: 1613174501.99 epoch sec; seqNo:8934 2021-02-13T00:01:41Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.21 psi 2021-02-13T00:01:41Z- handleDepth()-Depth_Keller msg: 1613174501996 epoch sec; seqNo:9005; depth 10.64 2021-02-13T00:01:42Z- handleDvl() - RDI_Pathfinder msg: 1613174502.01 epoch sec; seqNo:4338 2021-02-13T00:01:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.07 , 106.71 , 79.66 , 118.88 2021-02-13T00:01:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.03 , 0.21 2021-02-13T00:01:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174502.03 epoch sec; seqNo:9005; 4058431.33 north; 591912.31 east 2021-02-13T00:01:42Z- AHRS_M2 msg: 1613174502.40 epoch sec; seqNo:8935 2021-02-13T00:01:42Z- AHRS_M2 msg: 0.56 phi; -0.31 theta; -0.21 psi 2021-02-13T00:01:42Z- handleDepth()-Depth_Keller msg: 1613174502400 epoch sec; seqNo:9006; depth 10.75 2021-02-13T00:01:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174502.41 epoch sec; seqNo:9006; 4058431.33 north; 591912.31 east 2021-02-13T00:01:42Z- AHRS_M2 msg: 1613174502.80 epoch sec; seqNo:8936 2021-02-13T00:01:42Z- AHRS_M2 msg: 0.56 phi; -0.31 theta; -0.21 psi 2021-02-13T00:01:42Z- handleDepth()-Depth_Keller msg: 1613174502804 epoch sec; seqNo:9007; depth 10.89 2021-02-13T00:01:42Z- handleDvl() - RDI_Pathfinder msg: 1613174502.82 epoch sec; seqNo:4339 2021-02-13T00:01:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.07 , 105.36 , 79.66 , 113.47 2021-02-13T00:01:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , 0.16 , 0.25 2021-02-13T00:01:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174502.83 epoch sec; seqNo:9007; 4058431.71 north; 591912.30 east 2021-02-13T00:01:43Z- AHRS_M2 msg: 1613174503.20 epoch sec; seqNo:8937 2021-02-13T00:01:43Z- AHRS_M2 msg: 0.55 phi; -0.30 theta; -0.20 psi 2021-02-13T00:01:43Z- handleDepth()-Depth_Keller msg: 1613174503210 epoch sec; seqNo:9008; depth 11.00 2021-02-13T00:01:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174503.23 epoch sec; seqNo:9008; 4058431.71 north; 591912.30 east 2021-02-13T00:01:43Z- AHRS_M2 msg: 1613174503.61 epoch sec; seqNo:8938 2021-02-13T00:01:43Z- AHRS_M2 msg: 0.55 phi; -0.29 theta; -0.19 psi 2021-02-13T00:01:43Z- handleDepth()-Depth_Keller msg: 1613174503612 epoch sec; seqNo:9009; depth 11.13 2021-02-13T00:01:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174503.62 epoch sec; seqNo:9009; 4058432.09 north; 591912.30 east 2021-02-13T00:01:44Z- AHRS_M2 msg: 1613174504.01 epoch sec; seqNo:8939 2021-02-13T00:01:44Z- AHRS_M2 msg: 0.55 phi; -0.29 theta; -0.18 psi 2021-02-13T00:01:44Z- handleDepth()-Depth_Keller msg: 1613174504016 epoch sec; seqNo:9010; depth 11.23 2021-02-13T00:01:44Z- handleDvl() - RDI_Pathfinder msg: 1613174504.03 epoch sec; seqNo:4340 2021-02-13T00:01:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.21 , 104.14 , 79.80 , 114.96 2021-02-13T00:01:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.30 , 0.17 2021-02-13T00:01:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174504.04 epoch sec; seqNo:9010; 4058432.09 north; 591912.30 east 2021-02-13T00:01:44Z- AHRS_M2 msg: 1613174504.42 epoch sec; seqNo:8940 2021-02-13T00:01:44Z- AHRS_M2 msg: 0.55 phi; -0.28 theta; -0.17 psi 2021-02-13T00:01:44Z- handleDepth()-Depth_Keller msg: 1613174504424 epoch sec; seqNo:9011; depth 11.34 2021-02-13T00:01:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174504.44 epoch sec; seqNo:9011; 4058432.48 north; 591913.51 east 2021-02-13T00:01:44Z- AHRS_M2 msg: 1613174504.82 epoch sec; seqNo:8941 2021-02-13T00:01:44Z- AHRS_M2 msg: 0.56 phi; -0.28 theta; -0.16 psi 2021-02-13T00:01:44Z- handleDepth()-Depth_Keller msg: 1613174504827 epoch sec; seqNo:9012; depth 11.45 2021-02-13T00:01:44Z- handleDvl() - RDI_Pathfinder msg: 1613174504.85 epoch sec; seqNo:4341 2021-02-13T00:01:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.21 , 101.43 , 0.00 , 112.25 2021-02-13T00:01:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.33 , -0.01 2021-02-13T00:01:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174504.86 epoch sec; seqNo:9012; 4058432.48 north; 591913.51 east 2021-02-13T00:01:45Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-13T00:01:45Z- waiting for 1613174503.00, time is 1613174504.86 2021-02-13T00:01:45Z- LcmTrn::updateTrn() equating pose 1613174504.86 and meas times 1613174504.86 2021-02-13T00:01:45Z- LcmTrn::updateTrn() >>>> pose time 1613174504.86, meas time 1613174504.86 <<<< 2021-02-13T00:01:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174504.86 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:01:45Z- TerrainNav::motionUpdate() - currEstimate = (4058432, 591913.51, 11.45) 2021-02-13T00:01:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:01:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:01:45Z- TNavBF::Projecting Measurements 2021-02-13T00:01:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:01:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:01:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:01:45Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:01:45Z- TNavFilter::3 good beams. 2021-02-13T00:01:45Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:01:45Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:01:45Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:01:45Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:01:45Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:01:45Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:01:45Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:01:45Z- TNavBF::mmse selected from filter -1 2021-02-13T00:01:45Z- TerrainMapOctree: (vehN, vehE ) = (4058432.48, 591913.51). 2021-02-13T00:01:45Z- TerrainMapOctree: Min Distance = 3660.27. 2021-02-13T00:01:45Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:01:45Z- There is no specific good beam that all particles have in common. 2021-02-13T00:01:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:01:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.60 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 11.62 2021-02-13T00:01:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:01:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 16.46 2021-02-13T00:01:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:01:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 20.16 2021-02-13T00:01:45Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:01:45Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:01:45Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:01:45Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:01:45Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:01:45Z- Filter 0 No beams used in subcloud update 2021-02-13T00:01:45Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:01:45Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:01:45Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 722.4 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:01:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174504.86 sec. 2021-02-13T00:01:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:01:45Z- TNavBF::mmse selected from filter -1 2021-02-13T00:01:45Z- variances([0], [2], and psi_berg) = 722.367167 833.011229 0.000000 2021-02-13T00:01:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:01:45Z- LcmTrn::publishEstimates() - MLE : 4058427.98 591865.01 11.45 2021-02-13T00:01:45Z- LcmTrn::publishEstimates() - MMSE : 4058432.48 591913.51 11.45 2021-02-13T00:01:45Z- LcmTrn::publishEstimates() - COVAR: 722.37 833.01 1.25 0.00 2021-02-13T00:01:45Z- AHRS_M2 msg: 1613174505.22 epoch sec; seqNo:8942 2021-02-13T00:01:45Z- AHRS_M2 msg: 0.56 phi; -0.27 theta; -0.16 psi 2021-02-13T00:01:45Z- handleDepth()-Depth_Keller msg: 1613174505233 epoch sec; seqNo:9013; depth 11.57 2021-02-13T00:01:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:45Z- handleDvl() - RDI_Pathfinder msg: 1613174505.23 epoch sec; seqNo:4342 2021-02-13T00:01:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.33 , -0.01 2021-02-13T00:01:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174505.27 epoch sec; seqNo:9013; 4058432.86 north; 591913.51 east 2021-02-13T00:01:45Z- AHRS_M2 msg: 1613174505.63 epoch sec; seqNo:8943 2021-02-13T00:01:45Z- AHRS_M2 msg: 0.56 phi; -0.27 theta; -0.17 psi 2021-02-13T00:01:45Z- handleDepth()-Depth_Keller msg: 1613174505633 epoch sec; seqNo:9014; depth 11.66 2021-02-13T00:01:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174505.64 epoch sec; seqNo:9014; 4058432.86 north; 591913.51 east 2021-02-13T00:01:46Z- AHRS_M2 msg: 1613174506.03 epoch sec; seqNo:8944 2021-02-13T00:01:46Z- AHRS_M2 msg: 0.57 phi; -0.26 theta; -0.18 psi 2021-02-13T00:01:46Z- handleDepth()-Depth_Keller msg: 1613174506038 epoch sec; seqNo:9015; depth 11.75 2021-02-13T00:01:46Z- handleDvl() - RDI_Pathfinder msg: 1613174506.05 epoch sec; seqNo:4343 2021-02-13T00:01:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 102.45 , 79.46 , 107.86 2021-02-13T00:01:46Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.21 , -0.21 2021-02-13T00:01:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174506.06 epoch sec; seqNo:9015; 4058433.25 north; 591914.73 east 2021-02-13T00:01:46Z- AHRS_M2 msg: 1613174506.44 epoch sec; seqNo:8945 2021-02-13T00:01:46Z- AHRS_M2 msg: 0.58 phi; -0.26 theta; -0.18 psi 2021-02-13T00:01:46Z- handleDepth()-Depth_Keller msg: 1613174506445 epoch sec; seqNo:9016; depth 11.86 2021-02-13T00:01:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:46Z- handleDvl() - RDI_Pathfinder msg: 1613174506.45 epoch sec; seqNo:4344 2021-02-13T00:01:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:46Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.21 , -0.21 2021-02-13T00:01:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174506.47 epoch sec; seqNo:9016; 4058433.63 north; 591914.72 east 2021-02-13T00:01:46Z- AHRS_M2 msg: 1613174506.84 epoch sec; seqNo:8946 2021-02-13T00:01:46Z- AHRS_M2 msg: 0.59 phi; -0.26 theta; -0.18 psi 2021-02-13T00:01:46Z- handleDepth()-Depth_Keller msg: 1613174506846 epoch sec; seqNo:9017; depth 11.95 2021-02-13T00:01:46Z- handleDvl() - RDI_Pathfinder msg: 1613174506.86 epoch sec; seqNo:4345 2021-02-13T00:01:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 101.64 , 80.00 , 109.75 2021-02-13T00:01:46Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.02 , -0.31 2021-02-13T00:01:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174506.87 epoch sec; seqNo:9017; 4058434.01 north; 591914.72 east 2021-02-13T00:01:47Z- AHRS_M2 msg: 1613174507.25 epoch sec; seqNo:8947 2021-02-13T00:01:47Z- AHRS_M2 msg: 0.60 phi; -0.26 theta; -0.19 psi 2021-02-13T00:01:47Z- handleDepth()-Depth_Keller msg: 1613174507265 epoch sec; seqNo:9018; depth 12.06 2021-02-13T00:01:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174507.28 epoch sec; seqNo:9018; 4058434.39 north; 591914.71 east 2021-02-13T00:01:47Z- AHRS_M2 msg: 1613174507.65 epoch sec; seqNo:8948 2021-02-13T00:01:47Z- AHRS_M2 msg: 0.61 phi; -0.26 theta; -0.18 psi 2021-02-13T00:01:47Z- handleDepth()-Depth_Keller msg: 1613174507685 epoch sec; seqNo:9019; depth 12.15 2021-02-13T00:01:47Z- handleDvl() - RDI_Pathfinder msg: 1613174507.70 epoch sec; seqNo:4346 2021-02-13T00:01:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.70 , 101.64 , 78.65 , 105.69 2021-02-13T00:01:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.21 , -0.22 2021-02-13T00:01:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174507.71 epoch sec; seqNo:9019; 4058434.77 north; 591914.71 east 2021-02-13T00:01:48Z- AHRS_M2 msg: 1613174508.05 epoch sec; seqNo:8949 2021-02-13T00:01:48Z- AHRS_M2 msg: 0.62 phi; -0.26 theta; -0.17 psi 2021-02-13T00:01:48Z- handleDepth()-Depth_Keller msg: 1613174508059 epoch sec; seqNo:9020; depth 12.28 2021-02-13T00:01:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174508.07 epoch sec; seqNo:9020; 4058435.15 north; 591914.71 east 2021-02-13T00:01:48Z- AHRS_M2 msg: 1613174508.46 epoch sec; seqNo:8950 2021-02-13T00:01:48Z- AHRS_M2 msg: 0.63 phi; -0.26 theta; -0.14 psi 2021-02-13T00:01:48Z- handleDepth()-Depth_Keller msg: 1613174508466 epoch sec; seqNo:9021; depth 12.35 2021-02-13T00:01:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174508.47 epoch sec; seqNo:9021; 4058435.54 north; 591915.92 east 2021-02-13T00:01:48Z- AHRS_M2 msg: 1613174508.86 epoch sec; seqNo:8951 2021-02-13T00:01:48Z- AHRS_M2 msg: 0.63 phi; -0.27 theta; -0.13 psi 2021-02-13T00:01:48Z- handleDepth()-Depth_Keller msg: 1613174508873 epoch sec; seqNo:9022; depth 12.45 2021-02-13T00:01:48Z- handleDvl() - RDI_Pathfinder msg: 1613174508.88 epoch sec; seqNo:4347 2021-02-13T00:01:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 96.36 , 0.00 , 104.48 2021-02-13T00:01:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.24 , -0.08 2021-02-13T00:01:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174508.89 epoch sec; seqNo:9022; 4058435.92 north; 591915.92 east 2021-02-13T00:01:49Z- AHRS_M2 msg: 1613174509.26 epoch sec; seqNo:8952 2021-02-13T00:01:49Z- AHRS_M2 msg: 0.63 phi; -0.27 theta; -0.12 psi 2021-02-13T00:01:49Z- handleDepth()-Depth_Keller msg: 1613174509268 epoch sec; seqNo:9023; depth 12.55 2021-02-13T00:01:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174509.29 epoch sec; seqNo:9023; 4058436.30 north; 591915.92 east 2021-02-13T00:01:49Z- AHRS_M2 msg: 1613174509.67 epoch sec; seqNo:8953 2021-02-13T00:01:49Z- AHRS_M2 msg: 0.64 phi; -0.28 theta; -0.11 psi 2021-02-13T00:01:49Z- handleDepth()-Depth_Keller msg: 1613174509673 epoch sec; seqNo:9024; depth 12.62 2021-02-13T00:01:49Z- handleDvl() - RDI_Pathfinder msg: 1613174509.68 epoch sec; seqNo:4348 2021-02-13T00:01:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 97.51 , 76.02 , 105.63 2021-02-13T00:01:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.16 , -0.01 2021-02-13T00:01:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174509.71 epoch sec; seqNo:9024; 4058436.67 north; 591915.91 east 2021-02-13T00:01:50Z- AHRS_M2 msg: 1613174510.08 epoch sec; seqNo:8954 2021-02-13T00:01:50Z- AHRS_M2 msg: 0.64 phi; -0.28 theta; -0.10 psi 2021-02-13T00:01:50Z- handleDepth()-Depth_Keller msg: 1613174510081 epoch sec; seqNo:9025; depth 12.73 2021-02-13T00:01:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174510.10 epoch sec; seqNo:9025; 4058436.67 north; 591915.91 east 2021-02-13T00:01:50Z- LcmTrn::handleMessages() - handling 46 messages took 0.66 ms and 133 calls... 2021-02-13T00:01:50Z- waiting for 1613174507.86, time is 1613174510.10 2021-02-13T00:01:50Z- LcmTrn::updateTrn() >>>> pose time 1613174510.10, meas time 1613174509.68 <<<< 2021-02-13T00:01:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174509.68 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:01:50Z- TerrainNav::motionUpdate() - currEstimate = (4058437, 591915.91, 12.73) 2021-02-13T00:01:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:01:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:01:50Z- TNavBF::Projecting Measurements 2021-02-13T00:01:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:01:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:01:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:01:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:01:50Z- TNavFilter::4 good beams. 2021-02-13T00:01:50Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:01:50Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:01:50Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:01:50Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:01:50Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:01:50Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:01:50Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:01:50Z- TNavBF::mmse selected from filter -1 2021-02-13T00:01:50Z- TerrainMapOctree: (vehN, vehE ) = (4058436.35, 591915.72). 2021-02-13T00:01:50Z- TerrainMapOctree: Min Distance = 3655.89. 2021-02-13T00:01:50Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:01:50Z- There is no specific good beam that all particles have in common. 2021-02-13T00:01:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:01:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.48 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 11.29 2021-02-13T00:01:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:01:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 15.21 2021-02-13T00:01:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:01:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 9.25 2021-02-13T00:01:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:01:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 17.85 2021-02-13T00:01:50Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:01:50Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:01:50Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:01:50Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:01:50Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:01:50Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:01:50Z- Filter 0 No beams used in subcloud update 2021-02-13T00:01:50Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:01:50Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:01:50Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 722.3 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:01:50Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174509.68 sec. 2021-02-13T00:01:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:01:50Z- TNavBF::mmse selected from filter -1 2021-02-13T00:01:50Z- variances([0], [2], and psi_berg) = 722.272757 833.011166 0.000000 2021-02-13T00:01:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:01:50Z- LcmTrn::publishEstimates() - MLE : 4058432.17 591868.41 12.73 2021-02-13T00:01:50Z- LcmTrn::publishEstimates() - MMSE : 4058436.67 591915.91 12.73 2021-02-13T00:01:50Z- LcmTrn::publishEstimates() - COVAR: 722.27 833.01 1.07 0.00 2021-02-13T00:01:50Z- AHRS_M2 msg: 1613174510.48 epoch sec; seqNo:8955 2021-02-13T00:01:50Z- AHRS_M2 msg: 0.65 phi; -0.29 theta; -0.11 psi 2021-02-13T00:01:50Z- handleDepth()-Depth_Keller msg: 1613174510489 epoch sec; seqNo:9026; depth 12.83 2021-02-13T00:01:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174510.50 epoch sec; seqNo:9026; 4058437.05 north; 591915.91 east 2021-02-13T00:01:50Z- AHRS_M2 msg: 1613174510.88 epoch sec; seqNo:8956 2021-02-13T00:01:50Z- AHRS_M2 msg: 0.66 phi; -0.30 theta; -0.11 psi 2021-02-13T00:01:50Z- handleDepth()-Depth_Keller msg: 1613174510884 epoch sec; seqNo:9027; depth 12.92 2021-02-13T00:01:50Z- handleDvl() - RDI_Pathfinder msg: 1613174510.90 epoch sec; seqNo:4349 2021-02-13T00:01:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.71 , 74.80 , 107.45 2021-02-13T00:01:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.16 , -0.08 2021-02-13T00:01:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174510.91 epoch sec; seqNo:9027; 4058437.43 north; 591915.90 east 2021-02-13T00:01:51Z- AHRS_M2 msg: 1613174511.28 epoch sec; seqNo:8957 2021-02-13T00:01:51Z- AHRS_M2 msg: 0.67 phi; -0.30 theta; -0.11 psi 2021-02-13T00:01:51Z- handleDepth()-Depth_Keller msg: 1613174511293 epoch sec; seqNo:9028; depth 13.02 2021-02-13T00:01:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:51Z- handleDvl() - RDI_Pathfinder msg: 1613174511.30 epoch sec; seqNo:4350 2021-02-13T00:01:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.16 , -0.08 2021-02-13T00:01:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174511.31 epoch sec; seqNo:9028; 4058437.81 north; 591915.90 east 2021-02-13T00:01:51Z- AHRS_M2 msg: 1613174511.69 epoch sec; seqNo:8958 2021-02-13T00:01:51Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.12 psi 2021-02-13T00:01:51Z- handleDepth()-Depth_Keller msg: 1613174511693 epoch sec; seqNo:9029; depth 13.12 2021-02-13T00:01:51Z- handleDvl() - RDI_Pathfinder msg: 1613174511.70 epoch sec; seqNo:4351 2021-02-13T00:01:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 153.03 , 94.88 , 0.00 , 109.75 2021-02-13T00:01:51Z- handleDvl() - RDI_Pathfinder msg: velocities 1.29 , -0.54 , -0.29 2021-02-13T00:01:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174511.72 epoch sec; seqNo:9029; 4058438.19 north; 591915.89 east 2021-02-13T00:01:52Z- AHRS_M2 msg: 1613174512.09 epoch sec; seqNo:8959 2021-02-13T00:01:52Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.13 psi 2021-02-13T00:01:52Z- handleDepth()-Depth_Keller msg: 1613174512097 epoch sec; seqNo:9030; depth 13.23 2021-02-13T00:01:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174512.12 epoch sec; seqNo:9030; 4058438.96 north; 591917.11 east 2021-02-13T00:01:52Z- AHRS_M2 msg: 1613174512.50 epoch sec; seqNo:8960 2021-02-13T00:01:52Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.15 psi 2021-02-13T00:01:52Z- handleDepth()-Depth_Keller msg: 1613174512500 epoch sec; seqNo:9031; depth 13.36 2021-02-13T00:01:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:52Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:52Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:52Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:52Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:52Z- handleDvl() - RDI_Pathfinder msg: 1613174512.50 epoch sec; seqNo:4352 2021-02-13T00:01:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:52Z- handleDvl() - RDI_Pathfinder msg: velocities 1.29 , -0.54 , -0.29 2021-02-13T00:01:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174512.51 epoch sec; seqNo:9031; 4058439.34 north; 591917.10 east 2021-02-13T00:01:52Z- AHRS_M2 msg: 1613174512.90 epoch sec; seqNo:8961 2021-02-13T00:01:52Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.16 psi 2021-02-13T00:01:52Z- handleDepth()-Depth_Keller msg: 1613174512906 epoch sec; seqNo:9032; depth 13.46 2021-02-13T00:01:52Z- handleDvl() - RDI_Pathfinder msg: 1613174512.92 epoch sec; seqNo:4353 2021-02-13T00:01:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.85 , 97.24 , 75.72 , 110.63 2021-02-13T00:01:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.24 , 0.00 2021-02-13T00:01:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174512.93 epoch sec; seqNo:9032; 4058439.72 north; 591917.10 east 2021-02-13T00:01:53Z- AHRS_M2 msg: 1613174513.30 epoch sec; seqNo:8962 2021-02-13T00:01:53Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.17 psi 2021-02-13T00:01:53Z- handleDepth()-Depth_Keller msg: 1613174513308 epoch sec; seqNo:9033; depth 13.59 2021-02-13T00:01:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174513.33 epoch sec; seqNo:9033; 4058440.10 north; 591917.10 east 2021-02-13T00:01:53Z- AHRS_M2 msg: 1613174513.71 epoch sec; seqNo:8963 2021-02-13T00:01:53Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.17 psi 2021-02-13T00:01:53Z- handleDepth()-Depth_Keller msg: 1613174513717 epoch sec; seqNo:9034; depth 13.69 2021-02-13T00:01:53Z- handleDvl() - RDI_Pathfinder msg: 1613174513.74 epoch sec; seqNo:4354 2021-02-13T00:01:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 99.07 , 78.78 , 111.24 2021-02-13T00:01:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.13 , 0.16 2021-02-13T00:01:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174513.74 epoch sec; seqNo:9034; 4058440.10 north; 591917.10 east 2021-02-13T00:01:54Z- AHRS_M2 msg: 1613174514.11 epoch sec; seqNo:8964 2021-02-13T00:01:54Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.18 psi 2021-02-13T00:01:54Z- handleDepth()-Depth_Keller msg: 1613174514117 epoch sec; seqNo:9035; depth 13.83 2021-02-13T00:01:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174514.14 epoch sec; seqNo:9035; 4058440.48 north; 591917.09 east 2021-02-13T00:01:54Z- AHRS_M2 msg: 1613174514.52 epoch sec; seqNo:8965 2021-02-13T00:01:54Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.18 psi 2021-02-13T00:01:54Z- handleDepth()-Depth_Keller msg: 1613174514521 epoch sec; seqNo:9036; depth 13.95 2021-02-13T00:01:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174514.53 epoch sec; seqNo:9036; 4058440.85 north; 591917.09 east 2021-02-13T00:01:54Z- AHRS_M2 msg: 1613174514.92 epoch sec; seqNo:8966 2021-02-13T00:01:54Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.18 psi 2021-02-13T00:01:54Z- handleDepth()-Depth_Keller msg: 1613174514926 epoch sec; seqNo:9037; depth 14.06 2021-02-13T00:01:54Z- handleDvl() - RDI_Pathfinder msg: 1613174514.94 epoch sec; seqNo:4355 2021-02-13T00:01:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.85 , 98.73 , 78.44 , 113.61 2021-02-13T00:01:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , 0.03 , 0.22 2021-02-13T00:01:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174514.96 epoch sec; seqNo:9037; 4058440.85 north; 591917.09 east 2021-02-13T00:01:55Z- AHRS_M2 msg: 1613174515.32 epoch sec; seqNo:8967 2021-02-13T00:01:55Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.19 psi 2021-02-13T00:01:55Z- handleDepth()-Depth_Keller msg: 1613174515328 epoch sec; seqNo:9038; depth 14.19 2021-02-13T00:01:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174515.34 epoch sec; seqNo:9038; 4058441.23 north; 591917.08 east 2021-02-13T00:01:55Z- LcmTrn::handleMessages() - handling 46 messages took 0.66 ms and 133 calls... 2021-02-13T00:01:55Z- waiting for 1613174513.10, time is 1613174515.34 2021-02-13T00:01:55Z- LcmTrn::updateTrn() >>>> pose time 1613174515.34, meas time 1613174514.94 <<<< 2021-02-13T00:01:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174514.94 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:01:55Z- TerrainNav::motionUpdate() - currEstimate = (4058441, 591917.08, 14.19) 2021-02-13T00:01:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:01:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:01:55Z- TNavBF::Projecting Measurements 2021-02-13T00:01:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:01:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:01:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:01:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:01:55Z- TNavFilter::4 good beams. 2021-02-13T00:01:55Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:01:55Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:01:55Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:01:55Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:01:55Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:01:55Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:01:55Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:01:55Z- TNavBF::mmse selected from filter -1 2021-02-13T00:01:55Z- TerrainMapOctree: (vehN, vehE ) = (4058440.88, 591916.99). 2021-02-13T00:01:55Z- TerrainMapOctree: Min Distance = 3651.58. 2021-02-13T00:01:55Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:01:55Z- There is no specific good beam that all particles have in common. 2021-02-13T00:01:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:01:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 11.25 2021-02-13T00:01:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:01:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 15.60 2021-02-13T00:01:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:01:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T00:01:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:01:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 20.65 2021-02-13T00:01:55Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:01:55Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:01:55Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:01:55Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:01:55Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:01:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:01:55Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:01:55Z- Filter 0 No beams used in subcloud update 2021-02-13T00:01:55Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:01:55Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:01:55Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 722.2 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:01:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174514.94 sec. 2021-02-13T00:01:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:01:55Z- TNavBF::mmse selected from filter -1 2021-02-13T00:01:55Z- variances([0], [2], and psi_berg) = 722.179814 833.011103 0.000000 2021-02-13T00:01:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:01:55Z- LcmTrn::publishEstimates() - MLE : 4058436.73 591872.58 14.19 2021-02-13T00:01:55Z- LcmTrn::publishEstimates() - MMSE : 4058441.23 591917.08 14.19 2021-02-13T00:01:55Z- LcmTrn::publishEstimates() - COVAR: 722.18 833.01 0.98 0.00 2021-02-13T00:01:55Z- AHRS_M2 msg: 1613174515.73 epoch sec; seqNo:8968 2021-02-13T00:01:55Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.19 psi 2021-02-13T00:01:55Z- handleDepth()-Depth_Keller msg: 1613174515738 epoch sec; seqNo:9039; depth 14.30 2021-02-13T00:01:55Z- handleDvl() - RDI_Pathfinder msg: 1613174515.75 epoch sec; seqNo:4356 2021-02-13T00:01:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 101.64 , 78.65 , 112.46 2021-02-13T00:01:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.17 , 0.19 2021-02-13T00:01:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174515.76 epoch sec; seqNo:9039; 4058441.25 north; 591918.30 east 2021-02-13T00:01:56Z- AHRS_M2 msg: 1613174516.13 epoch sec; seqNo:8969 2021-02-13T00:01:56Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.19 psi 2021-02-13T00:01:56Z- handleDepth()-Depth_Keller msg: 1613174516145 epoch sec; seqNo:9040; depth 14.42 2021-02-13T00:01:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:56Z- handleDvl() - RDI_Pathfinder msg: 1613174516.15 epoch sec; seqNo:4357 2021-02-13T00:01:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.17 , 0.19 2021-02-13T00:01:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174516.16 epoch sec; seqNo:9040; 4058441.25 north; 591918.30 east 2021-02-13T00:01:56Z- AHRS_M2 msg: 1613174516.55 epoch sec; seqNo:8970 2021-02-13T00:01:56Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.19 psi 2021-02-13T00:01:56Z- handleDepth()-Depth_Keller msg: 1613174516554 epoch sec; seqNo:9041; depth 14.54 2021-02-13T00:01:56Z- handleDvl() - RDI_Pathfinder msg: 1613174516.57 epoch sec; seqNo:4358 2021-02-13T00:01:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.92 , 74.80 , 119.15 2021-02-13T00:01:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.23 , 0.11 2021-02-13T00:01:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174516.58 epoch sec; seqNo:9041; 4058441.63 north; 591918.30 east 2021-02-13T00:01:56Z- AHRS_M2 msg: 1613174516.94 epoch sec; seqNo:8971 2021-02-13T00:01:56Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.20 psi 2021-02-13T00:01:56Z- handleDepth()-Depth_Keller msg: 1613174516946 epoch sec; seqNo:9042; depth 14.69 2021-02-13T00:01:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174516.96 epoch sec; seqNo:9042; 4058441.63 north; 591918.30 east 2021-02-13T00:01:57Z- AHRS_M2 msg: 1613174517.34 epoch sec; seqNo:8972 2021-02-13T00:01:57Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.20 psi 2021-02-13T00:01:57Z- handleDepth()-Depth_Keller msg: 1613174517350 epoch sec; seqNo:9043; depth 14.80 2021-02-13T00:01:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:57Z- handleDvl() - RDI_Pathfinder msg: 1613174517.35 epoch sec; seqNo:4359 2021-02-13T00:01:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.23 , 0.11 2021-02-13T00:01:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174517.36 epoch sec; seqNo:9043; 4058442.02 north; 591919.52 east 2021-02-13T00:01:57Z- AHRS_M2 msg: 1613174517.77 epoch sec; seqNo:8973 2021-02-13T00:01:57Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.21 psi 2021-02-13T00:01:57Z- handleDepth()-Depth_Keller msg: 1613174517778 epoch sec; seqNo:9044; depth 14.95 2021-02-13T00:01:57Z- handleDvl() - RDI_Pathfinder msg: 1613174517.79 epoch sec; seqNo:4360 2021-02-13T00:01:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.58 , 102.52 , 78.17 , 121.45 2021-02-13T00:01:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.17 , -0.02 2021-02-13T00:01:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174517.80 epoch sec; seqNo:9044; 4058442.40 north; 591919.51 east 2021-02-13T00:01:58Z- AHRS_M2 msg: 1613174518.15 epoch sec; seqNo:8974 2021-02-13T00:01:58Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.22 psi 2021-02-13T00:01:58Z- handleDepth()-Depth_Keller msg: 1613174518158 epoch sec; seqNo:9045; depth 15.08 2021-02-13T00:01:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174518.17 epoch sec; seqNo:9045; 4058442.40 north; 591919.51 east 2021-02-13T00:01:58Z- AHRS_M2 msg: 1613174518.56 epoch sec; seqNo:8975 2021-02-13T00:01:58Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.22 psi 2021-02-13T00:01:58Z- handleDepth()-Depth_Keller msg: 1613174518561 epoch sec; seqNo:9046; depth 15.21 2021-02-13T00:01:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:01:58Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:01:58Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:01:58Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:01:58Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:01:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:01:58Z- handleDvl() - RDI_Pathfinder msg: 1613174518.56 epoch sec; seqNo:4361 2021-02-13T00:01:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:01:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.17 , -0.02 2021-02-13T00:01:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174518.58 epoch sec; seqNo:9046; 4058442.77 north; 591919.51 east 2021-02-13T00:01:58Z- AHRS_M2 msg: 1613174518.96 epoch sec; seqNo:8976 2021-02-13T00:01:58Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.23 psi 2021-02-13T00:01:58Z- handleDepth()-Depth_Keller msg: 1613174518964 epoch sec; seqNo:9047; depth 15.36 2021-02-13T00:01:58Z- handleDvl() - RDI_Pathfinder msg: 1613174518.98 epoch sec; seqNo:4362 2021-02-13T00:01:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.08 , 104.07 , 73.57 , 117.60 2021-02-13T00:01:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.06 , -0.12 2021-02-13T00:01:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174518.99 epoch sec; seqNo:9047; 4058443.15 north; 591919.50 east 2021-02-13T00:01:59Z- AHRS_M2 msg: 1613174519.36 epoch sec; seqNo:8977 2021-02-13T00:01:59Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.23 psi 2021-02-13T00:01:59Z- handleDepth()-Depth_Keller msg: 1613174519368 epoch sec; seqNo:9048; depth 15.48 2021-02-13T00:01:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174519.38 epoch sec; seqNo:9048; 4058443.17 north; 591920.73 east 2021-02-13T00:01:59Z- AHRS_M2 msg: 1613174519.77 epoch sec; seqNo:8978 2021-02-13T00:01:59Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.22 psi 2021-02-13T00:01:59Z- handleDepth()-Depth_Keller msg: 1613174519782 epoch sec; seqNo:9049; depth 15.62 2021-02-13T00:01:59Z- handleDvl() - RDI_Pathfinder msg: 1613174519.79 epoch sec; seqNo:4363 2021-02-13T00:01:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.35 , 105.69 , 73.98 , 115.16 2021-02-13T00:01:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.05 , -0.13 2021-02-13T00:01:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174519.80 epoch sec; seqNo:9049; 4058443.55 north; 591920.72 east 2021-02-13T00:02:00Z- AHRS_M2 msg: 1613174520.17 epoch sec; seqNo:8979 2021-02-13T00:02:00Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.21 psi 2021-02-13T00:02:00Z- handleDepth()-Depth_Keller msg: 1613174520176 epoch sec; seqNo:9050; depth 15.77 2021-02-13T00:02:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174520.19 epoch sec; seqNo:9050; 4058443.92 north; 591920.72 east 2021-02-13T00:02:00Z- LcmTrn::handleMessages() - handling 44 messages took 0.56 ms and 133 calls... 2021-02-13T00:02:00Z- waiting for 1613174518.34, time is 1613174520.19 2021-02-13T00:02:00Z- LcmTrn::updateTrn() >>>> pose time 1613174520.19, meas time 1613174519.79 <<<< 2021-02-13T00:02:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174519.79 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:02:00Z- TerrainNav::motionUpdate() - currEstimate = (4058444, 591920.72, 15.77) 2021-02-13T00:02:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:02:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:02:00Z- TNavBF::Projecting Measurements 2021-02-13T00:02:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:02:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:02:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:02:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:02:00Z- TNavFilter::4 good beams. 2021-02-13T00:02:00Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:02:00Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:02:00Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:02:00Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:02:00Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:02:00Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:02:00Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:02:00Z- TNavBF::mmse selected from filter -1 2021-02-13T00:02:00Z- TerrainMapOctree: (vehN, vehE ) = (4058443.71, 591920.42). 2021-02-13T00:02:00Z- TerrainMapOctree: Min Distance = 3647.22. 2021-02-13T00:02:00Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:02:00Z- There is no specific good beam that all particles have in common. 2021-02-13T00:02:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:02:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 10.59 2021-02-13T00:02:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:02:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 17.87 2021-02-13T00:02:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:02:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 8.76 2021-02-13T00:02:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:02:00Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 21.22 2021-02-13T00:02:00Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:02:00Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:02:00Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:02:00Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:02:00Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:02:00Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:02:00Z- Filter 0 No beams used in subcloud update 2021-02-13T00:02:00Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:02:00Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:02:00Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 722.1 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:02:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174519.79 sec. 2021-02-13T00:02:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:02:00Z- TNavBF::mmse selected from filter -1 2021-02-13T00:02:00Z- variances([0], [2], and psi_berg) = 722.088407 833.011042 0.000000 2021-02-13T00:02:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:02:00Z- LcmTrn::publishEstimates() - MLE : 4058439.42 591876.22 15.77 2021-02-13T00:02:00Z- LcmTrn::publishEstimates() - MMSE : 4058443.92 591920.72 15.77 2021-02-13T00:02:00Z- LcmTrn::publishEstimates() - COVAR: 722.09 833.01 0.92 0.00 2021-02-13T00:02:00Z- AHRS_M2 msg: 1613174520.58 epoch sec; seqNo:8980 2021-02-13T00:02:00Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.21 psi 2021-02-13T00:02:00Z- handleDepth()-Depth_Keller msg: 1613174520581 epoch sec; seqNo:9051; depth 15.88 2021-02-13T00:02:00Z- handleDvl() - RDI_Pathfinder msg: 1613174520.59 epoch sec; seqNo:4364 2021-02-13T00:02:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.48 , 101.64 , 73.98 , 116.51 2021-02-13T00:02:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.08 , -0.11 2021-02-13T00:02:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174520.60 epoch sec; seqNo:9051; 4058444.30 north; 591920.71 east 2021-02-13T00:02:00Z- AHRS_M2 msg: 1613174520.98 epoch sec; seqNo:8981 2021-02-13T00:02:00Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.20 psi 2021-02-13T00:02:00Z- handleDepth()-Depth_Keller msg: 1613174520989 epoch sec; seqNo:9052; depth 16.04 2021-02-13T00:02:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:00Z- handleDvl() - RDI_Pathfinder msg: 1613174520.99 epoch sec; seqNo:4365 2021-02-13T00:02:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.08 , -0.11 2021-02-13T00:02:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174521.00 epoch sec; seqNo:9052; 4058444.68 north; 591920.71 east 2021-02-13T00:02:01Z- AHRS_M2 msg: 1613174521.38 epoch sec; seqNo:8982 2021-02-13T00:02:01Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.20 psi 2021-02-13T00:02:01Z- handleDepth()-Depth_Keller msg: 1613174521389 epoch sec; seqNo:9053; depth 16.16 2021-02-13T00:02:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174521.40 epoch sec; seqNo:9053; 4058445.06 north; 591920.70 east 2021-02-13T00:02:01Z- AHRS_M2 msg: 1613174521.79 epoch sec; seqNo:8983 2021-02-13T00:02:01Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.19 psi 2021-02-13T00:02:01Z- handleDepth()-Depth_Keller msg: 1613174521792 epoch sec; seqNo:9054; depth 16.29 2021-02-13T00:02:01Z- handleDvl() - RDI_Pathfinder msg: 1613174521.80 epoch sec; seqNo:4366 2021-02-13T00:02:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.42 , 100.35 , 78.71 , 113.88 2021-02-13T00:02:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.10 , -0.09 2021-02-13T00:02:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174521.81 epoch sec; seqNo:9054; 4058445.06 north; 591920.70 east 2021-02-13T00:02:02Z- AHRS_M2 msg: 1613174522.19 epoch sec; seqNo:8984 2021-02-13T00:02:02Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.19 psi 2021-02-13T00:02:02Z- handleDepth()-Depth_Keller msg: 1613174522196 epoch sec; seqNo:9055; depth 16.44 2021-02-13T00:02:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:02Z- handleDvl() - RDI_Pathfinder msg: 1613174522.20 epoch sec; seqNo:4367 2021-02-13T00:02:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.10 , -0.09 2021-02-13T00:02:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174522.21 epoch sec; seqNo:9055; 4058445.45 north; 591921.92 east 2021-02-13T00:02:02Z- AHRS_M2 msg: 1613174522.62 epoch sec; seqNo:8985 2021-02-13T00:02:02Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.18 psi 2021-02-13T00:02:02Z- handleDepth()-Depth_Keller msg: 1613174522621 epoch sec; seqNo:9056; depth 16.57 2021-02-13T00:02:02Z- handleDvl() - RDI_Pathfinder msg: 1613174522.63 epoch sec; seqNo:4368 2021-02-13T00:02:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.35 , 98.93 , 70.30 , 111.10 2021-02-13T00:02:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.12 , -0.08 2021-02-13T00:02:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174522.64 epoch sec; seqNo:9056; 4058445.83 north; 591921.92 east 2021-02-13T00:02:03Z- AHRS_M2 msg: 1613174523.00 epoch sec; seqNo:8986 2021-02-13T00:02:03Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.18 psi 2021-02-13T00:02:03Z- handleDepth()-Depth_Keller msg: 1613174523009 epoch sec; seqNo:9057; depth 16.71 2021-02-13T00:02:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174523.02 epoch sec; seqNo:9057; 4058446.21 north; 591921.91 east 2021-02-13T00:02:03Z- AHRS_M2 msg: 1613174523.40 epoch sec; seqNo:8987 2021-02-13T00:02:03Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.18 psi 2021-02-13T00:02:03Z- handleDepth()-Depth_Keller msg: 1613174523408 epoch sec; seqNo:9058; depth 16.84 2021-02-13T00:02:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:03Z- handleDvl() - RDI_Pathfinder msg: 1613174523.41 epoch sec; seqNo:4369 2021-02-13T00:02:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.12 , -0.08 2021-02-13T00:02:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174523.42 epoch sec; seqNo:9058; 4058446.59 north; 591921.91 east 2021-02-13T00:02:03Z- AHRS_M2 msg: 1613174523.81 epoch sec; seqNo:8988 2021-02-13T00:02:03Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.19 psi 2021-02-13T00:02:03Z- handleDepth()-Depth_Keller msg: 1613174523814 epoch sec; seqNo:9059; depth 16.97 2021-02-13T00:02:03Z- handleDvl() - RDI_Pathfinder msg: 1613174523.83 epoch sec; seqNo:4370 2021-02-13T00:02:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.29 , 98.93 , 78.24 , 114.75 2021-02-13T00:02:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.15 , -0.05 2021-02-13T00:02:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174523.84 epoch sec; seqNo:9059; 4058446.59 north; 591921.91 east 2021-02-13T00:02:04Z- AHRS_M2 msg: 1613174524.21 epoch sec; seqNo:8989 2021-02-13T00:02:04Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.19 psi 2021-02-13T00:02:04Z- handleDepth()-Depth_Keller msg: 1613174524216 epoch sec; seqNo:9060; depth 17.12 2021-02-13T00:02:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174524.24 epoch sec; seqNo:9060; 4058446.97 north; 591921.91 east 2021-02-13T00:02:04Z- AHRS_M2 msg: 1613174524.62 epoch sec; seqNo:8990 2021-02-13T00:02:04Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.19 psi 2021-02-13T00:02:04Z- handleDepth()-Depth_Keller msg: 1613174524627 epoch sec; seqNo:9061; depth 17.26 2021-02-13T00:02:04Z- handleDvl() - RDI_Pathfinder msg: 1613174524.65 epoch sec; seqNo:4371 2021-02-13T00:02:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.07 , 99.00 , 70.30 , 113.88 2021-02-13T00:02:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.18 , -0.01 2021-02-13T00:02:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174524.66 epoch sec; seqNo:9061; 4058447.35 north; 591921.90 east 2021-02-13T00:02:05Z- AHRS_M2 msg: 1613174525.02 epoch sec; seqNo:8991 2021-02-13T00:02:05Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.19 psi 2021-02-13T00:02:05Z- handleDepth()-Depth_Keller msg: 1613174525024 epoch sec; seqNo:9062; depth 17.41 2021-02-13T00:02:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174525.04 epoch sec; seqNo:9062; 4058447.73 north; 591921.90 east 2021-02-13T00:02:05Z- handleDepth()-Depth_Keller msg: 1613174525473 epoch sec; seqNo:9063; depth 17.52 2021-02-13T00:02:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174525.49 epoch sec; seqNo:9063; 4058448.12 north; 591923.11 east 2021-02-13T00:02:05Z- LcmTrn::handleMessages() - handling 46 messages took 0.62 ms and 134 calls... 2021-02-13T00:02:05Z- waiting for 1613174523.19, time is 1613174525.49 2021-02-13T00:02:05Z- LcmTrn::updateTrn() >>>> pose time 1613174525.49, meas time 1613174524.65 <<<< 2021-02-13T00:02:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174524.65 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:02:05Z- TerrainNav::motionUpdate() - currEstimate = (4058448, 591923.11, 17.52) 2021-02-13T00:02:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:02:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:02:05Z- TNavBF::Projecting Measurements 2021-02-13T00:02:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:02:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:02:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:02:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:02:05Z- TNavFilter::4 good beams. 2021-02-13T00:02:05Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:02:05Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:02:05Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:02:05Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:02:05Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:02:05Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:02:05Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:02:05Z- TNavBF::mmse selected from filter -1 2021-02-13T00:02:05Z- TerrainMapOctree: (vehN, vehE ) = (4058447.45, 591922.73). 2021-02-13T00:02:05Z- TerrainMapOctree: Min Distance = 3642.86. 2021-02-13T00:02:05Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:02:05Z- There is no specific good beam that all particles have in common. 2021-02-13T00:02:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:02:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 10.26 2021-02-13T00:02:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:02:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 15.68 2021-02-13T00:02:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:02:05Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 7.91 2021-02-13T00:02:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:02:05Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 20.75 2021-02-13T00:02:05Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:02:05Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:02:05Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:02:05Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:02:05Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:02:05Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:02:05Z- Filter 0 No beams used in subcloud update 2021-02-13T00:02:05Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:02:05Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:02:05Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 722.0 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:02:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174524.65 sec. 2021-02-13T00:02:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:02:05Z- TNavBF::mmse selected from filter -1 2021-02-13T00:02:05Z- variances([0], [2], and psi_berg) = 721.988575 833.010976 0.000000 2021-02-13T00:02:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:02:05Z- LcmTrn::publishEstimates() - MLE : 4058443.62 591878.61 17.52 2021-02-13T00:02:05Z- LcmTrn::publishEstimates() - MMSE : 4058448.12 591923.11 17.52 2021-02-13T00:02:05Z- LcmTrn::publishEstimates() - COVAR: 721.99 833.01 0.89 0.00 2021-02-13T00:02:05Z- AHRS_M2 msg: 1613174525.83 epoch sec; seqNo:8992 2021-02-13T00:02:05Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-13T00:02:05Z- handleDepth()-Depth_Keller msg: 1613174525841 epoch sec; seqNo:9064; depth 17.69 2021-02-13T00:02:05Z- handleDvl() - RDI_Pathfinder msg: 1613174525.86 epoch sec; seqNo:4372 2021-02-13T00:02:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 97.71 , 70.71 , 112.32 2021-02-13T00:02:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.17 , 0.03 2021-02-13T00:02:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174525.88 epoch sec; seqNo:9064; 4058448.50 north; 591923.11 east 2021-02-13T00:02:06Z- AHRS_M2 msg: 1613174526.23 epoch sec; seqNo:8993 2021-02-13T00:02:06Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-13T00:02:06Z- handleDepth()-Depth_Keller msg: 1613174526237 epoch sec; seqNo:9065; depth 17.82 2021-02-13T00:02:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174526.25 epoch sec; seqNo:9065; 4058448.50 north; 591923.11 east 2021-02-13T00:02:06Z- AHRS_M2 msg: 1613174526.65 epoch sec; seqNo:8994 2021-02-13T00:02:06Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.21 psi 2021-02-13T00:02:06Z- handleDepth()-Depth_Keller msg: 1613174526657 epoch sec; seqNo:9066; depth 17.95 2021-02-13T00:02:06Z- handleDvl() - RDI_Pathfinder msg: 1613174526.67 epoch sec; seqNo:4373 2021-02-13T00:02:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.48 , 98.93 , 78.24 , 112.32 2021-02-13T00:02:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.15 , 0.04 2021-02-13T00:02:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174526.69 epoch sec; seqNo:9066; 4058448.88 north; 591923.11 east 2021-02-13T00:02:07Z- AHRS_M2 msg: 1613174527.04 epoch sec; seqNo:8995 2021-02-13T00:02:07Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.21 psi 2021-02-13T00:02:07Z- handleDepth()-Depth_Keller msg: 1613174527049 epoch sec; seqNo:9067; depth 18.09 2021-02-13T00:02:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:07Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:07Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:07Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:07Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:07Z- handleDvl() - RDI_Pathfinder msg: 1613174527.05 epoch sec; seqNo:4374 2021-02-13T00:02:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.15 , 0.04 2021-02-13T00:02:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174527.06 epoch sec; seqNo:9067; 4058449.25 north; 591923.10 east 2021-02-13T00:02:07Z- AHRS_M2 msg: 1613174527.44 epoch sec; seqNo:8996 2021-02-13T00:02:07Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.22 psi 2021-02-13T00:02:07Z- handleDepth()-Depth_Keller msg: 1613174527448 epoch sec; seqNo:9068; depth 18.21 2021-02-13T00:02:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174527.46 epoch sec; seqNo:9068; 4058449.63 north; 591923.10 east 2021-02-13T00:02:07Z- AHRS_M2 msg: 1613174527.85 epoch sec; seqNo:8997 2021-02-13T00:02:07Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.21 psi 2021-02-13T00:02:07Z- handleDepth()-Depth_Keller msg: 1613174527852 epoch sec; seqNo:9069; depth 18.34 2021-02-13T00:02:07Z- handleDvl() - RDI_Pathfinder msg: 1613174527.86 epoch sec; seqNo:4375 2021-02-13T00:02:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.25 , 99.07 , 70.71 , 118.00 2021-02-13T00:02:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.11 , 0.06 2021-02-13T00:02:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174527.87 epoch sec; seqNo:9069; 4058450.01 north; 591923.09 east 2021-02-13T00:02:08Z- AHRS_M2 msg: 1613174528.25 epoch sec; seqNo:8998 2021-02-13T00:02:08Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.21 psi 2021-02-13T00:02:08Z- handleDepth()-Depth_Keller msg: 1613174528265 epoch sec; seqNo:9070; depth 18.48 2021-02-13T00:02:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:08Z- handleDvl() - RDI_Pathfinder msg: 1613174528.27 epoch sec; seqNo:4376 2021-02-13T00:02:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.11 , 0.06 2021-02-13T00:02:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174528.28 epoch sec; seqNo:9070; 4058450.01 north; 591923.09 east 2021-02-13T00:02:08Z- AHRS_M2 msg: 1613174528.66 epoch sec; seqNo:8999 2021-02-13T00:02:08Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.21 psi 2021-02-13T00:02:08Z- handleDepth()-Depth_Keller msg: 1613174528666 epoch sec; seqNo:9071; depth 18.63 2021-02-13T00:02:08Z- handleDvl() - RDI_Pathfinder msg: 1613174528.68 epoch sec; seqNo:4377 2021-02-13T00:02:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.84 , 98.86 , 78.17 , 112.25 2021-02-13T00:02:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , 0.09 2021-02-13T00:02:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174528.69 epoch sec; seqNo:9071; 4058450.40 north; 591924.31 east 2021-02-13T00:02:09Z- AHRS_M2 msg: 1613174529.06 epoch sec; seqNo:9000 2021-02-13T00:02:09Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.20 psi 2021-02-13T00:02:09Z- handleDepth()-Depth_Keller msg: 1613174529069 epoch sec; seqNo:9072; depth 18.77 2021-02-13T00:02:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174529.08 epoch sec; seqNo:9072; 4058450.78 north; 591924.31 east 2021-02-13T00:02:09Z- AHRS_M2 msg: 1613174529.46 epoch sec; seqNo:9001 2021-02-13T00:02:09Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.19 psi 2021-02-13T00:02:09Z- handleDepth()-Depth_Keller msg: 1613174529473 epoch sec; seqNo:9073; depth 18.90 2021-02-13T00:02:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:09Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:09Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:09Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:09Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:09Z- handleDvl() - RDI_Pathfinder msg: 1613174529.47 epoch sec; seqNo:4378 2021-02-13T00:02:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , 0.09 2021-02-13T00:02:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174529.48 epoch sec; seqNo:9073; 4058450.78 north; 591924.31 east 2021-02-13T00:02:09Z- AHRS_M2 msg: 1613174529.87 epoch sec; seqNo:9002 2021-02-13T00:02:09Z- AHRS_M2 msg: 0.60 phi; -0.38 theta; -0.18 psi 2021-02-13T00:02:09Z- handleDepth()-Depth_Keller msg: 1613174529874 epoch sec; seqNo:9074; depth 19.04 2021-02-13T00:02:09Z- handleDvl() - RDI_Pathfinder msg: 1613174529.89 epoch sec; seqNo:4379 2021-02-13T00:02:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.17 , 97.65 , 71.12 , 111.04 2021-02-13T00:02:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.03 , 0.11 2021-02-13T00:02:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174529.90 epoch sec; seqNo:9074; 4058451.16 north; 591924.30 east 2021-02-13T00:02:10Z- AHRS_M2 msg: 1613174530.27 epoch sec; seqNo:9003 2021-02-13T00:02:10Z- AHRS_M2 msg: 0.61 phi; -0.38 theta; -0.18 psi 2021-02-13T00:02:10Z- handleDepth()-Depth_Keller msg: 1613174530281 epoch sec; seqNo:9075; depth 19.16 2021-02-13T00:02:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174530.30 epoch sec; seqNo:9075; 4058451.16 north; 591924.30 east 2021-02-13T00:02:10Z- AHRS_M2 msg: 1613174530.68 epoch sec; seqNo:9004 2021-02-13T00:02:10Z- AHRS_M2 msg: 0.61 phi; -0.38 theta; -0.18 psi 2021-02-13T00:02:10Z- LcmTrn::handleMessages() - handling 45 messages took 0.64 ms and 132 calls... 2021-02-13T00:02:10Z- waiting for 1613174528.49, time is 1613174530.68 2021-02-13T00:02:10Z- LcmTrn::updateTrn() >>>> pose time 1613174530.68, meas time 1613174529.89 <<<< 2021-02-13T00:02:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174529.89 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:02:10Z- TerrainNav::motionUpdate() - currEstimate = (4058451, 591924.30, 19.16) 2021-02-13T00:02:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:02:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:02:10Z- TNavBF::Projecting Measurements 2021-02-13T00:02:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:02:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:02:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:02:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:02:10Z- TNavFilter::4 good beams. 2021-02-13T00:02:10Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:02:10Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:02:10Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:02:10Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:02:10Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:02:10Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:02:10Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:02:10Z- TNavBF::mmse selected from filter -1 2021-02-13T00:02:10Z- TerrainMapOctree: (vehN, vehE ) = (4058450.70, 591924.12). 2021-02-13T00:02:10Z- TerrainMapOctree: Min Distance = 3639.48. 2021-02-13T00:02:10Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:02:10Z- There is no specific good beam that all particles have in common. 2021-02-13T00:02:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:02:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 9.78 2021-02-13T00:02:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:02:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 15.26 2021-02-13T00:02:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:02:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-13T00:02:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:02:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 19.73 2021-02-13T00:02:10Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:02:10Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:02:10Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:02:10Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:02:10Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:02:10Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:02:10Z- Filter 0 No beams used in subcloud update 2021-02-13T00:02:10Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:02:10Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:02:10Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 721.9 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:02:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174529.89 sec. 2021-02-13T00:02:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:02:10Z- TNavBF::mmse selected from filter -1 2021-02-13T00:02:10Z- variances([0], [2], and psi_berg) = 721.906827 833.010921 0.000000 2021-02-13T00:02:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:02:10Z- LcmTrn::publishEstimates() - MLE : 4058446.66 591879.80 19.16 2021-02-13T00:02:10Z- LcmTrn::publishEstimates() - MMSE : 4058451.16 591924.30 19.16 2021-02-13T00:02:10Z- LcmTrn::publishEstimates() - COVAR: 721.91 833.01 0.82 0.00 2021-02-13T00:02:10Z- handleDepth()-Depth_Keller msg: 1613174530680 epoch sec; seqNo:9076; depth 19.29 2021-02-13T00:02:10Z- handleDvl() - RDI_Pathfinder msg: 1613174530.69 epoch sec; seqNo:4380 2021-02-13T00:02:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 96.57 , 69.48 , 106.30 2021-02-13T00:02:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.09 , 0.05 2021-02-13T00:02:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174530.70 epoch sec; seqNo:9076; 4058451.54 north; 591924.30 east 2021-02-13T00:02:11Z- AHRS_M2 msg: 1613174531.08 epoch sec; seqNo:9005 2021-02-13T00:02:11Z- AHRS_M2 msg: 0.61 phi; -0.38 theta; -0.17 psi 2021-02-13T00:02:11Z- handleDepth()-Depth_Keller msg: 1613174531085 epoch sec; seqNo:9077; depth 19.44 2021-02-13T00:02:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174531.10 epoch sec; seqNo:9077; 4058451.92 north; 591924.29 east 2021-02-13T00:02:11Z- AHRS_M2 msg: 1613174531.48 epoch sec; seqNo:9006 2021-02-13T00:02:11Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.17 psi 2021-02-13T00:02:11Z- handleDepth()-Depth_Keller msg: 1613174531500 epoch sec; seqNo:9078; depth 19.58 2021-02-13T00:02:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174531.51 epoch sec; seqNo:9078; 4058451.93 north; 591925.51 east 2021-02-13T00:02:11Z- AHRS_M2 msg: 1613174531.89 epoch sec; seqNo:9007 2021-02-13T00:02:11Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.17 psi 2021-02-13T00:02:11Z- handleDepth()-Depth_Keller msg: 1613174531897 epoch sec; seqNo:9079; depth 19.72 2021-02-13T00:02:11Z- handleDvl() - RDI_Pathfinder msg: 1613174531.91 epoch sec; seqNo:4381 2021-02-13T00:02:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.66 , 97.92 , 78.44 , 112.52 2021-02-13T00:02:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.07 , -0.05 2021-02-13T00:02:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174531.92 epoch sec; seqNo:9079; 4058452.31 north; 591925.51 east 2021-02-13T00:02:12Z- AHRS_M2 msg: 1613174532.29 epoch sec; seqNo:9008 2021-02-13T00:02:12Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.18 psi 2021-02-13T00:02:12Z- handleDepth()-Depth_Keller msg: 1613174532305 epoch sec; seqNo:9080; depth 19.86 2021-02-13T00:02:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174532.32 epoch sec; seqNo:9080; 4058452.69 north; 591925.51 east 2021-02-13T00:02:12Z- AHRS_M2 msg: 1613174532.70 epoch sec; seqNo:9009 2021-02-13T00:02:12Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.18 psi 2021-02-13T00:02:12Z- handleDepth()-Depth_Keller msg: 1613174532700 epoch sec; seqNo:9081; depth 20.00 2021-02-13T00:02:12Z- handleDvl() - RDI_Pathfinder msg: 1613174532.71 epoch sec; seqNo:4382 2021-02-13T00:02:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.78 , 96.36 , 69.48 , 111.24 2021-02-13T00:02:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.02 , -0.10 2021-02-13T00:02:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174532.73 epoch sec; seqNo:9081; 4058453.07 north; 591925.50 east 2021-02-13T00:02:13Z- AHRS_M2 msg: 1613174533.10 epoch sec; seqNo:9010 2021-02-13T00:02:13Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.18 psi 2021-02-13T00:02:13Z- handleDepth()-Depth_Keller msg: 1613174533104 epoch sec; seqNo:9082; depth 20.15 2021-02-13T00:02:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:13Z- handleDvl() - RDI_Pathfinder msg: 1613174533.11 epoch sec; seqNo:4383 2021-02-13T00:02:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.02 , -0.10 2021-02-13T00:02:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174533.12 epoch sec; seqNo:9082; 4058453.07 north; 591925.50 east 2021-02-13T00:02:13Z- AHRS_M2 msg: 1613174533.50 epoch sec; seqNo:9011 2021-02-13T00:02:13Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.18 psi 2021-02-13T00:02:13Z- handleDepth()-Depth_Keller msg: 1613174533510 epoch sec; seqNo:9083; depth 20.29 2021-02-13T00:02:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174533.52 epoch sec; seqNo:9083; 4058453.46 north; 591926.72 east 2021-02-13T00:02:13Z- AHRS_M2 msg: 1613174533.91 epoch sec; seqNo:9012 2021-02-13T00:02:13Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.17 psi 2021-02-13T00:02:13Z- handleDepth()-Depth_Keller msg: 1613174533912 epoch sec; seqNo:9084; depth 20.43 2021-02-13T00:02:13Z- handleDvl() - RDI_Pathfinder msg: 1613174533.93 epoch sec; seqNo:4384 2021-02-13T00:02:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 97.85 , 67.85 , 111.24 2021-02-13T00:02:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.11 , -0.08 2021-02-13T00:02:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174533.94 epoch sec; seqNo:9084; 4058453.84 north; 591926.72 east 2021-02-13T00:02:14Z- AHRS_M2 msg: 1613174534.31 epoch sec; seqNo:9013 2021-02-13T00:02:14Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.17 psi 2021-02-13T00:02:14Z- handleDepth()-Depth_Keller msg: 1613174534316 epoch sec; seqNo:9085; depth 20.57 2021-02-13T00:02:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:14Z- handleDvl() - RDI_Pathfinder msg: 1613174534.32 epoch sec; seqNo:4385 2021-02-13T00:02:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.11 , -0.08 2021-02-13T00:02:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174534.33 epoch sec; seqNo:9085; 4058454.22 north; 591926.71 east 2021-02-13T00:02:14Z- AHRS_M2 msg: 1613174534.72 epoch sec; seqNo:9014 2021-02-13T00:02:14Z- AHRS_M2 msg: 0.66 phi; -0.38 theta; -0.17 psi 2021-02-13T00:02:14Z- handleDepth()-Depth_Keller msg: 1613174534729 epoch sec; seqNo:9086; depth 20.71 2021-02-13T00:02:14Z- handleDvl() - RDI_Pathfinder msg: 1613174534.74 epoch sec; seqNo:4386 2021-02-13T00:02:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.02 , 93.05 , 67.44 , 108.87 2021-02-13T00:02:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.13 , -0.02 2021-02-13T00:02:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174534.75 epoch sec; seqNo:9086; 4058454.60 north; 591926.71 east 2021-02-13T00:02:15Z- AHRS_M2 msg: 1613174535.12 epoch sec; seqNo:9015 2021-02-13T00:02:15Z- AHRS_M2 msg: 0.66 phi; -0.38 theta; -0.16 psi 2021-02-13T00:02:15Z- handleDepth()-Depth_Keller msg: 1613174535137 epoch sec; seqNo:9087; depth 20.87 2021-02-13T00:02:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174535.15 epoch sec; seqNo:9087; 4058454.60 north; 591926.71 east 2021-02-13T00:02:15Z- AHRS_M2 msg: 1613174535.52 epoch sec; seqNo:9016 2021-02-13T00:02:15Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.16 psi 2021-02-13T00:02:15Z- handleDepth()-Depth_Keller msg: 1613174535533 epoch sec; seqNo:9088; depth 21.00 2021-02-13T00:02:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:15Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:15Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:15Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:15Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:15Z- handleDvl() - RDI_Pathfinder msg: 1613174535.54 epoch sec; seqNo:4387 2021-02-13T00:02:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.13 , -0.02 2021-02-13T00:02:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174535.54 epoch sec; seqNo:9088; 4058454.98 north; 591926.70 east 2021-02-13T00:02:15Z- LcmTrn::handleMessages() - handling 46 messages took 0.65 ms and 133 calls... 2021-02-13T00:02:15Z- waiting for 1613174533.68, time is 1613174535.54 2021-02-13T00:02:15Z- LcmTrn::updateTrn() equating pose 1613174535.54 and meas times 1613174535.54 2021-02-13T00:02:15Z- LcmTrn::updateTrn() >>>> pose time 1613174535.54, meas time 1613174535.54 <<<< 2021-02-13T00:02:15Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174535.54, ping # 0. 2021-02-13T00:02:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174535.54 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:02:15Z- TerrainNav::motionUpdate() - currEstimate = (4058455, 591926.70, 21.00) 2021-02-13T00:02:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:02:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:02:15Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:02:15Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:02:15Z- TNavBF::Projecting Measurements 2021-02-13T00:02:15Z- TNavFilter::Measurement from time = 1613174535.54 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:02:15Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:02:15Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:02:15Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:02:15Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:02:15Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:02:15Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:02:15Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:02:15Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 721.8 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:02:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:02:15Z- TNavBF::mmse selected from filter -1 2021-02-13T00:02:15Z- variances([0], [2], and psi_berg) = 721.833644 833.010873 0.000000 2021-02-13T00:02:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:02:15Z- LcmTrn::publishEstimates() - MLE : 4058450.48 591882.20 21.00 2021-02-13T00:02:15Z- LcmTrn::publishEstimates() - MMSE : 4058454.98 591926.70 21.00 2021-02-13T00:02:15Z- LcmTrn::publishEstimates() - COVAR: 721.83 833.01 1.05 0.00 2021-02-13T00:02:15Z- AHRS_M2 msg: 1613174535.93 epoch sec; seqNo:9017 2021-02-13T00:02:15Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.17 psi 2021-02-13T00:02:15Z- handleDepth()-Depth_Keller msg: 1613174535932 epoch sec; seqNo:9089; depth 21.15 2021-02-13T00:02:15Z- handleDvl() - RDI_Pathfinder msg: 1613174535.94 epoch sec; seqNo:4388 2021-02-13T00:02:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 93.18 , 68.26 , 107.79 2021-02-13T00:02:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , 0.01 2021-02-13T00:02:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174535.95 epoch sec; seqNo:9089; 4058455.36 north; 591926.70 east 2021-02-13T00:02:16Z- AHRS_M2 msg: 1613174536.33 epoch sec; seqNo:9018 2021-02-13T00:02:16Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.17 psi 2021-02-13T00:02:16Z- handleDepth()-Depth_Keller msg: 1613174536337 epoch sec; seqNo:9090; depth 21.28 2021-02-13T00:02:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174536.35 epoch sec; seqNo:9090; 4058455.73 north; 591926.70 east 2021-02-13T00:02:16Z- AHRS_M2 msg: 1613174536.74 epoch sec; seqNo:9019 2021-02-13T00:02:16Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.17 psi 2021-02-13T00:02:16Z- handleDepth()-Depth_Keller msg: 1613174536741 epoch sec; seqNo:9091; depth 21.43 2021-02-13T00:02:16Z- handleDvl() - RDI_Pathfinder msg: 1613174536.76 epoch sec; seqNo:4389 2021-02-13T00:02:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 66.21 , 107.79 2021-02-13T00:02:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.00 2021-02-13T00:02:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174536.77 epoch sec; seqNo:9091; 4058455.75 north; 591927.92 east 2021-02-13T00:02:17Z- AHRS_M2 msg: 1613174537.14 epoch sec; seqNo:9020 2021-02-13T00:02:17Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.18 psi 2021-02-13T00:02:17Z- handleDepth()-Depth_Keller msg: 1613174537149 epoch sec; seqNo:9092; depth 21.56 2021-02-13T00:02:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174537.17 epoch sec; seqNo:9092; 4058456.13 north; 591927.91 east 2021-02-13T00:02:17Z- AHRS_M2 msg: 1613174537.54 epoch sec; seqNo:9021 2021-02-13T00:02:17Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.19 psi 2021-02-13T00:02:17Z- handleDepth()-Depth_Keller msg: 1613174537548 epoch sec; seqNo:9093; depth 21.71 2021-02-13T00:02:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174537.56 epoch sec; seqNo:9093; 4058456.50 north; 591927.91 east 2021-02-13T00:02:17Z- AHRS_M2 msg: 1613174537.95 epoch sec; seqNo:9022 2021-02-13T00:02:17Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.19 psi 2021-02-13T00:02:17Z- handleDepth()-Depth_Keller msg: 1613174537969 epoch sec; seqNo:9094; depth 21.84 2021-02-13T00:02:17Z- handleDvl() - RDI_Pathfinder msg: 1613174537.99 epoch sec; seqNo:4390 2021-02-13T00:02:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.99 , 67.03 , 107.72 2021-02-13T00:02:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , -0.03 2021-02-13T00:02:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174538.04 epoch sec; seqNo:9094; 4058456.88 north; 591927.90 east 2021-02-13T00:02:18Z- AHRS_M2 msg: 1613174538.35 epoch sec; seqNo:9023 2021-02-13T00:02:18Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.19 psi 2021-02-13T00:02:18Z- handleDepth()-Depth_Keller msg: 1613174538357 epoch sec; seqNo:9095; depth 22.02 2021-02-13T00:02:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174538.39 epoch sec; seqNo:9095; 4058456.88 north; 591927.90 east 2021-02-13T00:02:18Z- AHRS_M2 msg: 1613174538.76 epoch sec; seqNo:9024 2021-02-13T00:02:18Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.20 psi 2021-02-13T00:02:18Z- handleDepth()-Depth_Keller msg: 1613174538761 epoch sec; seqNo:9096; depth 22.13 2021-02-13T00:02:18Z- handleDvl() - RDI_Pathfinder msg: 1613174538.77 epoch sec; seqNo:4391 2021-02-13T00:02:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.39 , 97.99 , 0.00 , 108.94 2021-02-13T00:02:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , -0.04 2021-02-13T00:02:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174538.79 epoch sec; seqNo:9096; 4058457.26 north; 591927.90 east 2021-02-13T00:02:19Z- AHRS_M2 msg: 1613174539.17 epoch sec; seqNo:9025 2021-02-13T00:02:19Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.20 psi 2021-02-13T00:02:19Z- handleDepth()-Depth_Keller msg: 1613174539173 epoch sec; seqNo:9097; depth 22.27 2021-02-13T00:02:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:19Z- handleDvl() - RDI_Pathfinder msg: 1613174539.17 epoch sec; seqNo:4392 2021-02-13T00:02:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , -0.04 2021-02-13T00:02:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174539.19 epoch sec; seqNo:9097; 4058457.65 north; 591929.12 east 2021-02-13T00:02:19Z- AHRS_M2 msg: 1613174539.57 epoch sec; seqNo:9026 2021-02-13T00:02:19Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.19 psi 2021-02-13T00:02:19Z- handleDepth()-Depth_Keller msg: 1613174539573 epoch sec; seqNo:9098; depth 22.42 2021-02-13T00:02:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174539.61 epoch sec; seqNo:9098; 4058458.03 north; 591929.11 east 2021-02-13T00:02:19Z- AHRS_M2 msg: 1613174539.97 epoch sec; seqNo:9027 2021-02-13T00:02:19Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.19 psi 2021-02-13T00:02:19Z- handleDepth()-Depth_Keller msg: 1613174539973 epoch sec; seqNo:9099; depth 22.57 2021-02-13T00:02:19Z- handleDvl() - RDI_Pathfinder msg: 1613174539.98 epoch sec; seqNo:4393 2021-02-13T00:02:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 97.58 , 65.40 , 105.69 2021-02-13T00:02:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.06 , -0.07 2021-02-13T00:02:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174540.02 epoch sec; seqNo:9099; 4058458.03 north; 591929.11 east 2021-02-13T00:02:20Z- AHRS_M2 msg: 1613174540.37 epoch sec; seqNo:9028 2021-02-13T00:02:20Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.18 psi 2021-02-13T00:02:20Z- handleDepth()-Depth_Keller msg: 1613174540381 epoch sec; seqNo:9100; depth 22.72 2021-02-13T00:02:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:20Z- handleDvl() - RDI_Pathfinder msg: 1613174540.38 epoch sec; seqNo:4394 2021-02-13T00:02:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.06 , -0.07 2021-02-13T00:02:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174540.40 epoch sec; seqNo:9100; 4058458.41 north; 591929.11 east 2021-02-13T00:02:20Z- AHRS_M2 msg: 1613174540.78 epoch sec; seqNo:9029 2021-02-13T00:02:20Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.18 psi 2021-02-13T00:02:20Z- LcmTrn::handleMessages() - handling 44 messages took 0.74 ms and 129 calls... 2021-02-13T00:02:20Z- waiting for 1613174538.54, time is 1613174540.78 2021-02-13T00:02:20Z- LcmTrn::updateTrn() >>>> pose time 1613174540.78, meas time 1613174540.38 <<<< 2021-02-13T00:02:20Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174540.38, ping # 0. 2021-02-13T00:02:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174540.38 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:02:20Z- TerrainNav::motionUpdate() - currEstimate = (4058458, 591929.11, 22.72) 2021-02-13T00:02:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:02:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:02:20Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:02:20Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:02:20Z- TNavBF::Projecting Measurements 2021-02-13T00:02:20Z- TNavFilter::Measurement from time = 1613174540.38 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:02:20Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:02:20Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:02:20Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:02:20Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:02:20Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:02:20Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:02:20Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:02:20Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 721.8 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:02:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:02:20Z- TNavBF::mmse selected from filter -1 2021-02-13T00:02:20Z- variances([0], [2], and psi_berg) = 721.746516 833.010815 0.000000 2021-02-13T00:02:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:02:20Z- LcmTrn::publishEstimates() - MLE : 4058454.91 591884.61 22.72 2021-02-13T00:02:20Z- LcmTrn::publishEstimates() - MMSE : 4058458.41 591929.11 22.72 2021-02-13T00:02:20Z- LcmTrn::publishEstimates() - COVAR: 721.75 833.01 1.26 0.00 2021-02-13T00:02:20Z- handleDepth()-Depth_Keller msg: 1613174540786 epoch sec; seqNo:9101; depth 22.87 2021-02-13T00:02:20Z- handleDvl() - RDI_Pathfinder msg: 1613174540.81 epoch sec; seqNo:4395 2021-02-13T00:02:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.98 , 90.75 , 78.58 , 104.14 2021-02-13T00:02:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , -0.04 2021-02-13T00:02:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174540.85 epoch sec; seqNo:9101; 4058458.79 north; 591929.10 east 2021-02-13T00:02:21Z- AHRS_M2 msg: 1613174541.18 epoch sec; seqNo:9030 2021-02-13T00:02:21Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.17 psi 2021-02-13T00:02:21Z- handleDepth()-Depth_Keller msg: 1613174541189 epoch sec; seqNo:9102; depth 23.02 2021-02-13T00:02:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174541.22 epoch sec; seqNo:9102; 4058458.79 north; 591929.10 east 2021-02-13T00:02:21Z- AHRS_M2 msg: 1613174541.58 epoch sec; seqNo:9031 2021-02-13T00:02:21Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.16 psi 2021-02-13T00:02:21Z- handleDepth()-Depth_Keller msg: 1613174541593 epoch sec; seqNo:9103; depth 23.15 2021-02-13T00:02:21Z- handleDvl() - RDI_Pathfinder msg: 1613174541.63 epoch sec; seqNo:4396 2021-02-13T00:02:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.57 , 90.75 , 66.62 , 104.14 2021-02-13T00:02:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , -0.06 2021-02-13T00:02:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174541.65 epoch sec; seqNo:9103; 4058459.18 north; 591930.32 east 2021-02-13T00:02:21Z- AHRS_M2 msg: 1613174541.99 epoch sec; seqNo:9032 2021-02-13T00:02:21Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.16 psi 2021-02-13T00:02:21Z- handleDepth()-Depth_Keller msg: 1613174541997 epoch sec; seqNo:9104; depth 23.31 2021-02-13T00:02:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174542.02 epoch sec; seqNo:9104; 4058459.56 north; 591930.32 east 2021-02-13T00:02:22Z- AHRS_M2 msg: 1613174542.43 epoch sec; seqNo:9033 2021-02-13T00:02:22Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.17 psi 2021-02-13T00:02:22Z- handleDepth()-Depth_Keller msg: 1613174542458 epoch sec; seqNo:9105; depth 23.46 2021-02-13T00:02:22Z- handleDvl() - RDI_Pathfinder msg: 1613174542.48 epoch sec; seqNo:4397 2021-02-13T00:02:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.16 , 0.00 , 66.21 , 103.19 2021-02-13T00:02:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.12 , -0.01 2021-02-13T00:02:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174542.52 epoch sec; seqNo:9105; 4058459.94 north; 591930.31 east 2021-02-13T00:02:22Z- AHRS_M2 msg: 1613174542.80 epoch sec; seqNo:9034 2021-02-13T00:02:22Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.18 psi 2021-02-13T00:02:22Z- handleDepth()-Depth_Keller msg: 1613174542825 epoch sec; seqNo:9106; depth 23.62 2021-02-13T00:02:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:22Z- handleDvl() - RDI_Pathfinder msg: 1613174542.83 epoch sec; seqNo:4398 2021-02-13T00:02:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.12 , -0.01 2021-02-13T00:02:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174542.85 epoch sec; seqNo:9106; 4058459.94 north; 591930.31 east 2021-02-13T00:02:23Z- AHRS_M2 msg: 1613174543.20 epoch sec; seqNo:9035 2021-02-13T00:02:23Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.19 psi 2021-02-13T00:02:23Z- handleDepth()-Depth_Keller msg: 1613174543209 epoch sec; seqNo:9107; depth 23.74 2021-02-13T00:02:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174543.22 epoch sec; seqNo:9107; 4058460.32 north; 591930.31 east 2021-02-13T00:02:23Z- AHRS_M2 msg: 1613174543.60 epoch sec; seqNo:9036 2021-02-13T00:02:23Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.20 psi 2021-02-13T00:02:23Z- handleDepth()-Depth_Keller msg: 1613174543616 epoch sec; seqNo:9108; depth 23.87 2021-02-13T00:02:23Z- handleDvl() - RDI_Pathfinder msg: 1613174543.63 epoch sec; seqNo:4399 2021-02-13T00:02:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.16 , 90.28 , 64.58 , 103.26 2021-02-13T00:02:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , 0.05 2021-02-13T00:02:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174543.64 epoch sec; seqNo:9108; 4058460.70 north; 591930.30 east 2021-02-13T00:02:24Z- AHRS_M2 msg: 1613174544.01 epoch sec; seqNo:9037 2021-02-13T00:02:24Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.21 psi 2021-02-13T00:02:24Z- handleDepth()-Depth_Keller msg: 1613174544017 epoch sec; seqNo:9109; depth 24.02 2021-02-13T00:02:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:24Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:24Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:24Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:24Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:24Z- handleDvl() - RDI_Pathfinder msg: 1613174544.02 epoch sec; seqNo:4400 2021-02-13T00:02:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , 0.05 2021-02-13T00:02:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174544.04 epoch sec; seqNo:9109; 4058460.70 north; 591930.30 east 2021-02-13T00:02:24Z- AHRS_M2 msg: 1613174544.41 epoch sec; seqNo:9038 2021-02-13T00:02:24Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.22 psi 2021-02-13T00:02:24Z- handleDepth()-Depth_Keller msg: 1613174544416 epoch sec; seqNo:9110; depth 24.14 2021-02-13T00:02:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174544.42 epoch sec; seqNo:9110; 4058461.09 north; 591931.52 east 2021-02-13T00:02:24Z- AHRS_M2 msg: 1613174544.82 epoch sec; seqNo:9039 2021-02-13T00:02:24Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.22 psi 2021-02-13T00:02:24Z- handleDepth()-Depth_Keller msg: 1613174544825 epoch sec; seqNo:9111; depth 24.29 2021-02-13T00:02:24Z- handleDvl() - RDI_Pathfinder msg: 1613174544.84 epoch sec; seqNo:4401 2021-02-13T00:02:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 92.31 , 66.62 , 103.26 2021-02-13T00:02:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.07 2021-02-13T00:02:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174544.85 epoch sec; seqNo:9111; 4058461.47 north; 591931.52 east 2021-02-13T00:02:25Z- AHRS_M2 msg: 1613174545.22 epoch sec; seqNo:9040 2021-02-13T00:02:25Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.22 psi 2021-02-13T00:02:25Z- handleDepth()-Depth_Keller msg: 1613174545229 epoch sec; seqNo:9112; depth 24.41 2021-02-13T00:02:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:25Z- handleDvl() - RDI_Pathfinder msg: 1613174545.23 epoch sec; seqNo:4402 2021-02-13T00:02:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.07 2021-02-13T00:02:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174545.24 epoch sec; seqNo:9112; 4058461.47 north; 591931.52 east 2021-02-13T00:02:25Z- AHRS_M2 msg: 1613174545.62 epoch sec; seqNo:9041 2021-02-13T00:02:25Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.22 psi 2021-02-13T00:02:25Z- handleDepth()-Depth_Keller msg: 1613174545628 epoch sec; seqNo:9113; depth 24.56 2021-02-13T00:02:25Z- handleDvl() - RDI_Pathfinder msg: 1613174545.65 epoch sec; seqNo:4403 2021-02-13T00:02:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.94 , 92.24 , 64.99 , 101.97 2021-02-13T00:02:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.07 , 0.05 2021-02-13T00:02:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174545.66 epoch sec; seqNo:9113; 4058461.85 north; 591931.51 east 2021-02-13T00:02:25Z- LcmTrn::handleMessages() - handling 47 messages took 0.86 ms and 130 calls... 2021-02-13T00:02:25Z- waiting for 1613174543.78, time is 1613174545.66 2021-02-13T00:02:25Z- LcmTrn::updateTrn() equating pose 1613174545.66 and meas times 1613174545.66 2021-02-13T00:02:25Z- LcmTrn::updateTrn() >>>> pose time 1613174545.66, meas time 1613174545.66 <<<< 2021-02-13T00:02:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174545.66 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:02:25Z- TerrainNav::motionUpdate() - currEstimate = (4058462, 591931.51, 24.56) 2021-02-13T00:02:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:02:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:02:25Z- TNavBF::Projecting Measurements 2021-02-13T00:02:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:02:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:02:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:02:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:02:25Z- TNavFilter::4 good beams. 2021-02-13T00:02:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:02:25Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:02:25Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:02:25Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:02:25Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:02:25Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:02:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:02:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:02:25Z- TerrainMapOctree: (vehN, vehE ) = (4058461.85, 591931.51). 2021-02-13T00:02:25Z- TerrainMapOctree: Min Distance = 3626.18. 2021-02-13T00:02:25Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:02:25Z- There is no specific good beam that all particles have in common. 2021-02-13T00:02:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:02:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.47 K: 0.128 ExpectedMeasDiff: nan currMeas.covariance: 8.28 2021-02-13T00:02:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:02:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 13.61 2021-02-13T00:02:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:02:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.122 ExpectedMeasDiff: nan currMeas.covariance: 6.76 2021-02-13T00:02:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:02:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 16.64 2021-02-13T00:02:25Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:02:25Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:02:25Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:02:25Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:02:25Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:02:25Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:02:25Z- Filter 0 No beams used in subcloud update 2021-02-13T00:02:25Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:02:25Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:02:25Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 721.7 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:02:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174545.66 sec. 2021-02-13T00:02:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:02:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:02:25Z- variances([0], [2], and psi_berg) = 721.676053 833.010768 0.000000 2021-02-13T00:02:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:02:25Z- LcmTrn::publishEstimates() - MLE : 4058458.35 591887.01 24.56 2021-02-13T00:02:25Z- LcmTrn::publishEstimates() - MMSE : 4058461.85 591931.51 24.56 2021-02-13T00:02:25Z- LcmTrn::publishEstimates() - COVAR: 721.68 833.01 0.98 0.00 2021-02-13T00:02:26Z- AHRS_M2 msg: 1613174546.03 epoch sec; seqNo:9042 2021-02-13T00:02:26Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.22 psi 2021-02-13T00:02:26Z- handleDepth()-Depth_Keller msg: 1613174546032 epoch sec; seqNo:9114; depth 24.68 2021-02-13T00:02:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174546.04 epoch sec; seqNo:9114; 4058462.23 north; 591931.51 east 2021-02-13T00:02:26Z- AHRS_M2 msg: 1613174546.43 epoch sec; seqNo:9043 2021-02-13T00:02:26Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.22 psi 2021-02-13T00:02:26Z- handleDepth()-Depth_Keller msg: 1613174546436 epoch sec; seqNo:9115; depth 24.81 2021-02-13T00:02:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:26Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:26Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:26Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:26Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:26Z- handleDvl() - RDI_Pathfinder msg: 1613174546.44 epoch sec; seqNo:4404 2021-02-13T00:02:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.07 , 0.05 2021-02-13T00:02:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174546.45 epoch sec; seqNo:9115; 4058462.23 north; 591931.51 east 2021-02-13T00:02:26Z- AHRS_M2 msg: 1613174546.84 epoch sec; seqNo:9044 2021-02-13T00:02:26Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.22 psi 2021-02-13T00:02:26Z- handleDepth()-Depth_Keller msg: 1613174546842 epoch sec; seqNo:9116; depth 24.94 2021-02-13T00:02:26Z- handleDvl() - RDI_Pathfinder msg: 1613174546.85 epoch sec; seqNo:4405 2021-02-13T00:02:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 91.09 , 64.58 , 100.83 2021-02-13T00:02:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.08 , -0.01 2021-02-13T00:02:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174546.87 epoch sec; seqNo:9116; 4058462.62 north; 591932.73 east 2021-02-13T00:02:27Z- AHRS_M2 msg: 1613174547.24 epoch sec; seqNo:9045 2021-02-13T00:02:27Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.22 psi 2021-02-13T00:02:27Z- handleDepth()-Depth_Keller msg: 1613174547249 epoch sec; seqNo:9117; depth 25.07 2021-02-13T00:02:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174547.27 epoch sec; seqNo:9117; 4058463.00 north; 591932.72 east 2021-02-13T00:02:27Z- AHRS_M2 msg: 1613174547.64 epoch sec; seqNo:9046 2021-02-13T00:02:27Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.21 psi 2021-02-13T00:02:27Z- handleDepth()-Depth_Keller msg: 1613174547648 epoch sec; seqNo:9118; depth 25.21 2021-02-13T00:02:27Z- handleDvl() - RDI_Pathfinder msg: 1613174547.66 epoch sec; seqNo:4406 2021-02-13T00:02:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.71 , 91.02 , 65.40 , 99.54 2021-02-13T00:02:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.07 , -0.06 2021-02-13T00:02:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174547.69 epoch sec; seqNo:9118; 4058463.38 north; 591932.72 east 2021-02-13T00:02:28Z- AHRS_M2 msg: 1613174548.05 epoch sec; seqNo:9047 2021-02-13T00:02:28Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.21 psi 2021-02-13T00:02:28Z- handleDepth()-Depth_Keller msg: 1613174548052 epoch sec; seqNo:9119; depth 25.34 2021-02-13T00:02:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174548.06 epoch sec; seqNo:9119; 4058463.38 north; 591932.72 east 2021-02-13T00:02:28Z- AHRS_M2 msg: 1613174548.46 epoch sec; seqNo:9048 2021-02-13T00:02:28Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.21 psi 2021-02-13T00:02:28Z- handleDepth()-Depth_Keller msg: 1613174548465 epoch sec; seqNo:9120; depth 25.46 2021-02-13T00:02:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174548.47 epoch sec; seqNo:9120; 4058463.76 north; 591932.71 east 2021-02-13T00:02:28Z- AHRS_M2 msg: 1613174548.86 epoch sec; seqNo:9049 2021-02-13T00:02:28Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.21 psi 2021-02-13T00:02:28Z- handleDepth()-Depth_Keller msg: 1613174548861 epoch sec; seqNo:9121; depth 25.59 2021-02-13T00:02:28Z- handleDvl() - RDI_Pathfinder msg: 1613174548.88 epoch sec; seqNo:4407 2021-02-13T00:02:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 89.13 , 0.00 , 97.78 2021-02-13T00:02:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , -0.10 2021-02-13T00:02:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174548.89 epoch sec; seqNo:9121; 4058464.15 north; 591933.93 east 2021-02-13T00:02:29Z- AHRS_M2 msg: 1613174549.26 epoch sec; seqNo:9050 2021-02-13T00:02:29Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.20 psi 2021-02-13T00:02:29Z- handleDepth()-Depth_Keller msg: 1613174549264 epoch sec; seqNo:9122; depth 25.72 2021-02-13T00:02:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174549.28 epoch sec; seqNo:9122; 4058464.53 north; 591933.93 east 2021-02-13T00:02:29Z- AHRS_M2 msg: 1613174549.66 epoch sec; seqNo:9051 2021-02-13T00:02:29Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.20 psi 2021-02-13T00:02:29Z- handleDepth()-Depth_Keller msg: 1613174549668 epoch sec; seqNo:9123; depth 25.83 2021-02-13T00:02:29Z- handleDvl() - RDI_Pathfinder msg: 1613174549.68 epoch sec; seqNo:4408 2021-02-13T00:02:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 89.60 , 0.00 , 98.12 2021-02-13T00:02:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , -0.10 2021-02-13T00:02:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174549.69 epoch sec; seqNo:9123; 4058464.53 north; 591933.93 east 2021-02-13T00:02:30Z- AHRS_M2 msg: 1613174550.07 epoch sec; seqNo:9052 2021-02-13T00:02:30Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.20 psi 2021-02-13T00:02:30Z- handleDepth()-Depth_Keller msg: 1613174550074 epoch sec; seqNo:9124; depth 25.94 2021-02-13T00:02:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:30Z- handleDvl() - RDI_Pathfinder msg: 1613174550.08 epoch sec; seqNo:4409 2021-02-13T00:02:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , -0.10 2021-02-13T00:02:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174550.09 epoch sec; seqNo:9124; 4058464.90 north; 591933.92 east 2021-02-13T00:02:30Z- AHRS_M2 msg: 1613174550.48 epoch sec; seqNo:9053 2021-02-13T00:02:30Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.20 psi 2021-02-13T00:02:30Z- handleDepth()-Depth_Keller msg: 1613174550488 epoch sec; seqNo:9125; depth 26.05 2021-02-13T00:02:30Z- handleDvl() - RDI_Pathfinder msg: 1613174550.50 epoch sec; seqNo:4410 2021-02-13T00:02:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:30Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174550.52 epoch sec; seqNo:9125; 4058465.28 north; 591933.92 east 2021-02-13T00:02:30Z- AHRS_M2 msg: 1613174550.88 epoch sec; seqNo:9054 2021-02-13T00:02:30Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.20 psi 2021-02-13T00:02:30Z- handleDepth()-Depth_Keller msg: 1613174550881 epoch sec; seqNo:9126; depth 26.19 2021-02-13T00:02:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174550.89 epoch sec; seqNo:9126; 4058465.66 north; 591933.91 east 2021-02-13T00:02:30Z- LcmTrn::handleMessages() - handling 46 messages took 0.70 ms and 132 calls... 2021-02-13T00:02:30Z- waiting for 1613174548.66, time is 1613174550.89 2021-02-13T00:02:30Z- LcmTrn::updateTrn() >>>> pose time 1613174550.89, meas time 1613174550.50 <<<< 2021-02-13T00:02:30Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174550.50, ping # 0. 2021-02-13T00:02:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174550.50 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:02:30Z- TerrainNav::motionUpdate() - currEstimate = (4058466, 591933.91, 26.19) 2021-02-13T00:02:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:02:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:02:30Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:02:30Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:02:30Z- TNavBF::Projecting Measurements 2021-02-13T00:02:30Z- TNavFilter::Measurement from time = 1613174550.50 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:02:30Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:02:30Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:02:30Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:02:30Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:02:30Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:02:30Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:02:30Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:02:30Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 721.6 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:02:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:02:30Z- TNavBF::mmse selected from filter -1 2021-02-13T00:02:30Z- variances([0], [2], and psi_berg) = 721.585948 833.010708 0.000000 2021-02-13T00:02:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:02:30Z- LcmTrn::publishEstimates() - MLE : 4058462.16 591889.41 26.19 2021-02-13T00:02:30Z- LcmTrn::publishEstimates() - MMSE : 4058465.66 591933.91 26.19 2021-02-13T00:02:30Z- LcmTrn::publishEstimates() - COVAR: 721.59 833.01 1.21 0.00 2021-02-13T00:02:31Z- AHRS_M2 msg: 1613174551.28 epoch sec; seqNo:9055 2021-02-13T00:02:31Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.20 psi 2021-02-13T00:02:31Z- handleDepth()-Depth_Keller msg: 1613174551284 epoch sec; seqNo:9127; depth 26.31 2021-02-13T00:02:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:31Z- handleDvl() - RDI_Pathfinder msg: 1613174551.29 epoch sec; seqNo:4411 2021-02-13T00:02:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:31Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174551.30 epoch sec; seqNo:9127; 4058466.04 north; 591933.91 east 2021-02-13T00:02:31Z- AHRS_M2 msg: 1613174551.69 epoch sec; seqNo:9056 2021-02-13T00:02:31Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.20 psi 2021-02-13T00:02:31Z- handleDepth()-Depth_Keller msg: 1613174551691 epoch sec; seqNo:9128; depth 26.42 2021-02-13T00:02:31Z- handleDvl() - RDI_Pathfinder msg: 1613174551.70 epoch sec; seqNo:4412 2021-02-13T00:02:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.67 , 0.00 , 63.76 , 98.12 2021-02-13T00:02:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.13 , -0.03 2021-02-13T00:02:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174551.73 epoch sec; seqNo:9128; 4058466.43 north; 591935.13 east 2021-02-13T00:02:32Z- AHRS_M2 msg: 1613174552.09 epoch sec; seqNo:9057 2021-02-13T00:02:32Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.20 psi 2021-02-13T00:02:32Z- handleDepth()-Depth_Keller msg: 1613174552093 epoch sec; seqNo:9129; depth 26.55 2021-02-13T00:02:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174552.10 epoch sec; seqNo:9129; 4058466.43 north; 591935.13 east 2021-02-13T00:02:32Z- AHRS_M2 msg: 1613174552.50 epoch sec; seqNo:9058 2021-02-13T00:02:32Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.20 psi 2021-02-13T00:02:32Z- handleDepth()-Depth_Keller msg: 1613174552507 epoch sec; seqNo:9130; depth 26.66 2021-02-13T00:02:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:32Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:32Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:32Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:32Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:32Z- handleDvl() - RDI_Pathfinder msg: 1613174552.51 epoch sec; seqNo:4413 2021-02-13T00:02:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.13 , -0.03 2021-02-13T00:02:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174552.53 epoch sec; seqNo:9130; 4058466.81 north; 591935.12 east 2021-02-13T00:02:32Z- AHRS_M2 msg: 1613174552.89 epoch sec; seqNo:9059 2021-02-13T00:02:32Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.20 psi 2021-02-13T00:02:32Z- handleDepth()-Depth_Keller msg: 1613174552900 epoch sec; seqNo:9131; depth 26.79 2021-02-13T00:02:32Z- handleDvl() - RDI_Pathfinder msg: 1613174552.92 epoch sec; seqNo:4414 2021-02-13T00:02:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.67 , 87.17 , 63.35 , 97.99 2021-02-13T00:02:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.11 , 0.00 2021-02-13T00:02:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174552.93 epoch sec; seqNo:9131; 4058467.19 north; 591935.12 east 2021-02-13T00:02:33Z- AHRS_M2 msg: 1613174553.30 epoch sec; seqNo:9060 2021-02-13T00:02:33Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.20 psi 2021-02-13T00:02:33Z- handleDepth()-Depth_Keller msg: 1613174553304 epoch sec; seqNo:9132; depth 26.90 2021-02-13T00:02:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174553.32 epoch sec; seqNo:9132; 4058467.57 north; 591935.11 east 2021-02-13T00:02:33Z- AHRS_M2 msg: 1613174553.70 epoch sec; seqNo:9061 2021-02-13T00:02:33Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.20 psi 2021-02-13T00:02:33Z- handleDepth()-Depth_Keller msg: 1613174553707 epoch sec; seqNo:9133; depth 27.02 2021-02-13T00:02:33Z- handleDvl() - RDI_Pathfinder msg: 1613174553.72 epoch sec; seqNo:4415 2021-02-13T00:02:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 0.00 , 63.76 , 97.99 2021-02-13T00:02:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.11 , 0.02 2021-02-13T00:02:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174553.73 epoch sec; seqNo:9133; 4058467.95 north; 591935.11 east 2021-02-13T00:02:34Z- AHRS_M2 msg: 1613174554.11 epoch sec; seqNo:9062 2021-02-13T00:02:34Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.20 psi 2021-02-13T00:02:34Z- handleDepth()-Depth_Keller msg: 1613174554115 epoch sec; seqNo:9134; depth 27.15 2021-02-13T00:02:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174554.13 epoch sec; seqNo:9134; 4058467.95 north; 591935.11 east 2021-02-13T00:02:34Z- AHRS_M2 msg: 1613174554.54 epoch sec; seqNo:9063 2021-02-13T00:02:34Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.19 psi 2021-02-13T00:02:34Z- handleDepth()-Depth_Keller msg: 1613174554540 epoch sec; seqNo:9135; depth 27.25 2021-02-13T00:02:34Z- handleDvl() - RDI_Pathfinder msg: 1613174554.55 epoch sec; seqNo:4416 2021-02-13T00:02:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.26 , 86.02 , 62.94 , 97.92 2021-02-13T00:02:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , 0.05 2021-02-13T00:02:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174554.56 epoch sec; seqNo:9135; 4058468.33 north; 591935.11 east 2021-02-13T00:02:34Z- AHRS_M2 msg: 1613174554.92 epoch sec; seqNo:9064 2021-02-13T00:02:34Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.19 psi 2021-02-13T00:02:34Z- handleDepth()-Depth_Keller msg: 1613174554920 epoch sec; seqNo:9136; depth 27.37 2021-02-13T00:02:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:34Z- handleDvl() - RDI_Pathfinder msg: 1613174554.92 epoch sec; seqNo:4417 2021-02-13T00:02:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , 0.05 2021-02-13T00:02:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174554.93 epoch sec; seqNo:9136; 4058468.72 north; 591936.32 east 2021-02-13T00:02:35Z- AHRS_M2 msg: 1613174555.32 epoch sec; seqNo:9065 2021-02-13T00:02:35Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.19 psi 2021-02-13T00:02:35Z- handleDepth()-Depth_Keller msg: 1613174555325 epoch sec; seqNo:9137; depth 27.48 2021-02-13T00:02:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174555.33 epoch sec; seqNo:9137; 4058469.10 north; 591936.32 east 2021-02-13T00:02:35Z- AHRS_M2 msg: 1613174555.72 epoch sec; seqNo:9066 2021-02-13T00:02:35Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.19 psi 2021-02-13T00:02:35Z- handleDepth()-Depth_Keller msg: 1613174555729 epoch sec; seqNo:9138; depth 27.61 2021-02-13T00:02:35Z- handleDvl() - RDI_Pathfinder msg: 1613174555.74 epoch sec; seqNo:4418 2021-02-13T00:02:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 85.00 , 62.13 , 97.99 2021-02-13T00:02:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , 0.05 2021-02-13T00:02:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174555.75 epoch sec; seqNo:9138; 4058469.10 north; 591936.32 east 2021-02-13T00:02:35Z- LcmTrn::handleMessages() - handling 44 messages took 0.59 ms and 133 calls... 2021-02-13T00:02:35Z- waiting for 1613174553.89, time is 1613174555.75 2021-02-13T00:02:35Z- LcmTrn::updateTrn() equating pose 1613174555.75 and meas times 1613174555.75 2021-02-13T00:02:35Z- LcmTrn::updateTrn() >>>> pose time 1613174555.75, meas time 1613174555.75 <<<< 2021-02-13T00:02:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174555.75 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:02:35Z- TerrainNav::motionUpdate() - currEstimate = (4058469, 591936.32, 27.61) 2021-02-13T00:02:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:02:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:02:35Z- TNavBF::Projecting Measurements 2021-02-13T00:02:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:02:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:02:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:02:35Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:02:35Z- TNavFilter::3 good beams. 2021-02-13T00:02:35Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:02:35Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:02:35Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:02:35Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:02:35Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:02:35Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:02:35Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:02:35Z- TNavBF::mmse selected from filter -1 2021-02-13T00:02:35Z- TerrainMapOctree: (vehN, vehE ) = (4058469.10, 591936.32). 2021-02-13T00:02:35Z- TerrainMapOctree: Min Distance = 3617.53. 2021-02-13T00:02:35Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:02:35Z- There is no specific good beam that all particles have in common. 2021-02-13T00:02:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:02:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 11.56 2021-02-13T00:02:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:02:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.140 ExpectedMeasDiff: nan currMeas.covariance: 6.18 2021-02-13T00:02:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:02:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:02:35Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:02:35Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:02:36Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:02:36Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:02:36Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:02:36Z- Filter 0 No beams used in subcloud update 2021-02-13T00:02:36Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:02:36Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:02:36Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 721.5 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:02:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174555.75 sec. 2021-02-13T00:02:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:02:36Z- TNavBF::mmse selected from filter -1 2021-02-13T00:02:36Z- variances([0], [2], and psi_berg) = 721.515615 833.010661 0.000000 2021-02-13T00:02:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:02:36Z- LcmTrn::publishEstimates() - MLE : 4058465.60 591891.82 27.61 2021-02-13T00:02:36Z- LcmTrn::publishEstimates() - MMSE : 4058469.10 591936.32 27.61 2021-02-13T00:02:36Z- LcmTrn::publishEstimates() - COVAR: 721.52 833.01 1.04 0.00 2021-02-13T00:02:36Z- AHRS_M2 msg: 1613174556.13 epoch sec; seqNo:9067 2021-02-13T00:02:36Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.19 psi 2021-02-13T00:02:36Z- handleDepth()-Depth_Keller msg: 1613174556143 epoch sec; seqNo:9139; depth 27.73 2021-02-13T00:02:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:36Z- handleDvl() - RDI_Pathfinder msg: 1613174556.15 epoch sec; seqNo:4419 2021-02-13T00:02:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , 0.05 2021-02-13T00:02:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174556.16 epoch sec; seqNo:9139; 4058469.48 north; 591936.31 east 2021-02-13T00:02:36Z- AHRS_M2 msg: 1613174556.54 epoch sec; seqNo:9068 2021-02-13T00:02:36Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-13T00:02:36Z- handleDepth()-Depth_Keller msg: 1613174556548 epoch sec; seqNo:9140; depth 27.86 2021-02-13T00:02:36Z- handleDvl() - RDI_Pathfinder msg: 1613174556.57 epoch sec; seqNo:4420 2021-02-13T00:02:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.85 , 84.80 , 61.72 , 97.78 2021-02-13T00:02:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.05 2021-02-13T00:02:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174556.58 epoch sec; seqNo:9140; 4058469.86 north; 591936.31 east 2021-02-13T00:02:36Z- AHRS_M2 msg: 1613174556.94 epoch sec; seqNo:9069 2021-02-13T00:02:36Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-13T00:02:36Z- handleDepth()-Depth_Keller msg: 1613174556940 epoch sec; seqNo:9141; depth 27.98 2021-02-13T00:02:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174556.95 epoch sec; seqNo:9141; 4058470.23 north; 591936.31 east 2021-02-13T00:02:37Z- AHRS_M2 msg: 1613174557.34 epoch sec; seqNo:9070 2021-02-13T00:02:37Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-13T00:02:37Z- handleDepth()-Depth_Keller msg: 1613174557345 epoch sec; seqNo:9142; depth 28.10 2021-02-13T00:02:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:37Z- handleDvl() - RDI_Pathfinder msg: 1613174557.35 epoch sec; seqNo:4421 2021-02-13T00:02:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.05 2021-02-13T00:02:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174557.36 epoch sec; seqNo:9142; 4058470.25 north; 591937.53 east 2021-02-13T00:02:37Z- AHRS_M2 msg: 1613174557.77 epoch sec; seqNo:9071 2021-02-13T00:02:37Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:02:37Z- handleDepth()-Depth_Keller msg: 1613174557815 epoch sec; seqNo:9143; depth 28.21 2021-02-13T00:02:37Z- handleDvl() - RDI_Pathfinder msg: 1613174557.85 epoch sec; seqNo:4422 2021-02-13T00:02:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.26 , 0.00 , 61.72 , 97.99 2021-02-13T00:02:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.02 , 0.04 2021-02-13T00:02:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174557.89 epoch sec; seqNo:9143; 4058470.63 north; 591937.52 east 2021-02-13T00:02:38Z- AHRS_M2 msg: 1613174558.15 epoch sec; seqNo:9072 2021-02-13T00:02:38Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:02:38Z- handleDepth()-Depth_Keller msg: 1613174558161 epoch sec; seqNo:9144; depth 28.36 2021-02-13T00:02:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174558.17 epoch sec; seqNo:9144; 4058471.01 north; 591937.52 east 2021-02-13T00:02:38Z- AHRS_M2 msg: 1613174558.55 epoch sec; seqNo:9073 2021-02-13T00:02:38Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:02:38Z- handleDepth()-Depth_Keller msg: 1613174558556 epoch sec; seqNo:9145; depth 28.46 2021-02-13T00:02:38Z- handleDvl() - RDI_Pathfinder msg: 1613174558.57 epoch sec; seqNo:4423 2021-02-13T00:02:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.03 , 0.00 , 62.13 , 97.99 2021-02-13T00:02:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.02 2021-02-13T00:02:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174558.59 epoch sec; seqNo:9145; 4058471.38 north; 591937.52 east 2021-02-13T00:02:38Z- AHRS_M2 msg: 1613174558.95 epoch sec; seqNo:9074 2021-02-13T00:02:38Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:02:38Z- handleDepth()-Depth_Keller msg: 1613174558960 epoch sec; seqNo:9146; depth 28.56 2021-02-13T00:02:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174558.98 epoch sec; seqNo:9146; 4058471.38 north; 591937.52 east 2021-02-13T00:02:39Z- AHRS_M2 msg: 1613174559.36 epoch sec; seqNo:9075 2021-02-13T00:02:39Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.17 psi 2021-02-13T00:02:39Z- handleDepth()-Depth_Keller msg: 1613174559364 epoch sec; seqNo:9147; depth 28.70 2021-02-13T00:02:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174559.39 epoch sec; seqNo:9147; 4058471.76 north; 591937.51 east 2021-02-13T00:02:39Z- AHRS_M2 msg: 1613174559.77 epoch sec; seqNo:9076 2021-02-13T00:02:39Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.17 psi 2021-02-13T00:02:39Z- handleDepth()-Depth_Keller msg: 1613174559773 epoch sec; seqNo:9148; depth 28.81 2021-02-13T00:02:39Z- handleDvl() - RDI_Pathfinder msg: 1613174559.79 epoch sec; seqNo:4424 2021-02-13T00:02:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.44 , 83.92 , 62.13 , 97.99 2021-02-13T00:02:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.07 , 0.01 2021-02-13T00:02:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174559.81 epoch sec; seqNo:9148; 4058472.16 north; 591938.73 east 2021-02-13T00:02:40Z- AHRS_M2 msg: 1613174560.17 epoch sec; seqNo:9077 2021-02-13T00:02:40Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.17 psi 2021-02-13T00:02:40Z- handleDepth()-Depth_Keller msg: 1613174560177 epoch sec; seqNo:9149; depth 28.94 2021-02-13T00:02:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174560.19 epoch sec; seqNo:9149; 4058472.16 north; 591938.73 east 2021-02-13T00:02:40Z- AHRS_M2 msg: 1613174560.57 epoch sec; seqNo:9078 2021-02-13T00:02:40Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.17 psi 2021-02-13T00:02:40Z- handleDepth()-Depth_Keller msg: 1613174560578 epoch sec; seqNo:9150; depth 29.05 2021-02-13T00:02:40Z- handleDvl() - RDI_Pathfinder msg: 1613174560.59 epoch sec; seqNo:4425 2021-02-13T00:02:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.03 , 83.92 , 62.13 , 91.50 2021-02-13T00:02:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , -0.03 2021-02-13T00:02:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174560.60 epoch sec; seqNo:9150; 4058472.53 north; 591938.72 east 2021-02-13T00:02:40Z- AHRS_M2 msg: 1613174560.98 epoch sec; seqNo:9079 2021-02-13T00:02:40Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.17 psi 2021-02-13T00:02:40Z- handleDepth()-Depth_Keller msg: 1613174560980 epoch sec; seqNo:9151; depth 29.18 2021-02-13T00:02:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:40Z- handleDvl() - RDI_Pathfinder msg: 1613174560.98 epoch sec; seqNo:4426 2021-02-13T00:02:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , -0.03 2021-02-13T00:02:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174561.00 epoch sec; seqNo:9151; 4058472.91 north; 591938.72 east 2021-02-13T00:02:41Z- LcmTrn::handleMessages() - handling 47 messages took 0.63 ms and 135 calls... 2021-02-13T00:02:41Z- waiting for 1613174558.75, time is 1613174561.00 2021-02-13T00:02:41Z- LcmTrn::updateTrn() equating pose 1613174561.00 and meas times 1613174561.00 2021-02-13T00:02:41Z- LcmTrn::updateTrn() >>>> pose time 1613174561.00, meas time 1613174561.00 <<<< 2021-02-13T00:02:41Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174561.00, ping # 0. 2021-02-13T00:02:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174561.00 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:02:41Z- TerrainNav::motionUpdate() - currEstimate = (4058473, 591938.72, 29.18) 2021-02-13T00:02:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:02:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:02:41Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:02:41Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:02:41Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174561.00 sec.; No relevant navigation data available 2021-02-13T00:02:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:02:41Z- TNavBF::mmse selected from filter -1 2021-02-13T00:02:41Z- variances([0], [2], and psi_berg) = 721.442760 833.010613 0.000000 2021-02-13T00:02:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:02:41Z- LcmTrn::publishEstimates() - MLE : 4058469.41 591894.22 29.18 2021-02-13T00:02:41Z- LcmTrn::publishEstimates() - MMSE : 4058472.91 591938.72 29.18 2021-02-13T00:02:41Z- LcmTrn::publishEstimates() - COVAR: 721.44 833.01 1.27 0.00 2021-02-13T00:02:41Z- AHRS_M2 msg: 1613174561.38 epoch sec; seqNo:9080 2021-02-13T00:02:41Z- AHRS_M2 msg: 0.64 phi; -0.29 theta; -0.17 psi 2021-02-13T00:02:41Z- handleDepth()-Depth_Keller msg: 1613174561384 epoch sec; seqNo:9152; depth 29.30 2021-02-13T00:02:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174561.39 epoch sec; seqNo:9152; 4058473.29 north; 591938.72 east 2021-02-13T00:02:41Z- AHRS_M2 msg: 1613174561.78 epoch sec; seqNo:9081 2021-02-13T00:02:41Z- AHRS_M2 msg: 0.64 phi; -0.26 theta; -0.17 psi 2021-02-13T00:02:41Z- handleDepth()-Depth_Keller msg: 1613174561788 epoch sec; seqNo:9153; depth 29.43 2021-02-13T00:02:41Z- handleDvl() - RDI_Pathfinder msg: 1613174561.80 epoch sec; seqNo:4427 2021-02-13T00:02:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.21 , 83.92 , 60.49 , 0.00 2021-02-13T00:02:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , -0.05 2021-02-13T00:02:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174561.81 epoch sec; seqNo:9153; 4058473.30 north; 591939.94 east 2021-02-13T00:02:42Z- AHRS_M2 msg: 1613174562.19 epoch sec; seqNo:9082 2021-02-13T00:02:42Z- AHRS_M2 msg: 0.64 phi; -0.22 theta; -0.16 psi 2021-02-13T00:02:42Z- handleDepth()-Depth_Keller msg: 1613174562197 epoch sec; seqNo:9154; depth 29.58 2021-02-13T00:02:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:42Z- handleDvl() - RDI_Pathfinder msg: 1613174562.20 epoch sec; seqNo:4428 2021-02-13T00:02:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , -0.05 2021-02-13T00:02:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174562.22 epoch sec; seqNo:9154; 4058473.68 north; 591939.93 east 2021-02-13T00:02:42Z- AHRS_M2 msg: 1613174562.59 epoch sec; seqNo:9083 2021-02-13T00:02:42Z- AHRS_M2 msg: 0.64 phi; -0.18 theta; -0.16 psi 2021-02-13T00:02:42Z- handleDepth()-Depth_Keller msg: 1613174562596 epoch sec; seqNo:9155; depth 29.70 2021-02-13T00:02:42Z- handleDvl() - RDI_Pathfinder msg: 1613174562.61 epoch sec; seqNo:4429 2021-02-13T00:02:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:42Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174562.63 epoch sec; seqNo:9155; 4058474.06 north; 591939.93 east 2021-02-13T00:02:42Z- AHRS_M2 msg: 1613174563.00 epoch sec; seqNo:9084 2021-02-13T00:02:42Z- AHRS_M2 msg: 0.63 phi; -0.14 theta; -0.15 psi 2021-02-13T00:02:43Z- handleDepth()-Depth_Keller msg: 1613174563000 epoch sec; seqNo:9156; depth 29.83 2021-02-13T00:02:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174563.01 epoch sec; seqNo:9156; 4058474.44 north; 591939.92 east 2021-02-13T00:02:43Z- AHRS_M2 msg: 1613174563.40 epoch sec; seqNo:9085 2021-02-13T00:02:43Z- AHRS_M2 msg: 0.62 phi; -0.09 theta; -0.15 psi 2021-02-13T00:02:43Z- handleDepth()-Depth_Keller msg: 1613174563413 epoch sec; seqNo:9157; depth 29.94 2021-02-13T00:02:43Z- handleDvl() - RDI_Pathfinder msg: 1613174563.43 epoch sec; seqNo:4430 2021-02-13T00:02:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.79 , 0.00 , 64.23 , 0.00 2021-02-13T00:02:43Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174563.44 epoch sec; seqNo:9157; 4058474.44 north; 591939.92 east 2021-02-13T00:02:43Z- AHRS_M2 msg: 1613174563.80 epoch sec; seqNo:9086 2021-02-13T00:02:43Z- AHRS_M2 msg: 0.62 phi; -0.05 theta; -0.15 psi 2021-02-13T00:02:43Z- handleDepth()-Depth_Keller msg: 1613174563813 epoch sec; seqNo:9158; depth 30.04 2021-02-13T00:02:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174563.83 epoch sec; seqNo:9158; 4058474.82 north; 591939.92 east 2021-02-13T00:02:44Z- AHRS_M2 msg: 1613174564.23 epoch sec; seqNo:9087 2021-02-13T00:02:44Z- AHRS_M2 msg: 0.61 phi; -0.01 theta; -0.15 psi 2021-02-13T00:02:44Z- handleDepth()-Depth_Keller msg: 1613174564243 epoch sec; seqNo:9159; depth 30.12 2021-02-13T00:02:44Z- handleDvl() - RDI_Pathfinder msg: 1613174564.26 epoch sec; seqNo:4431 2021-02-13T00:02:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.35 , 0.00 , 67.33 , 0.00 2021-02-13T00:02:44Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174564.27 epoch sec; seqNo:9159; 4058475.21 north; 591941.14 east 2021-02-13T00:02:44Z- AHRS_M2 msg: 1613174564.62 epoch sec; seqNo:9088 2021-02-13T00:02:44Z- AHRS_M2 msg: 0.60 phi; 0.03 theta; -0.15 psi 2021-02-13T00:02:44Z- handleDepth()-Depth_Keller msg: 1613174564624 epoch sec; seqNo:9160; depth 30.18 2021-02-13T00:02:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:44Z- handleDvl() - RDI_Pathfinder msg: 1613174564.63 epoch sec; seqNo:4432 2021-02-13T00:02:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:44Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174564.64 epoch sec; seqNo:9160; 4058475.59 north; 591941.13 east 2021-02-13T00:02:45Z- AHRS_M2 msg: 1613174565.03 epoch sec; seqNo:9089 2021-02-13T00:02:45Z- AHRS_M2 msg: 0.59 phi; 0.07 theta; -0.15 psi 2021-02-13T00:02:45Z- handleDepth()-Depth_Keller msg: 1613174565037 epoch sec; seqNo:9161; depth 30.24 2021-02-13T00:02:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174565.05 epoch sec; seqNo:9161; 4058475.97 north; 591941.13 east 2021-02-13T00:02:45Z- AHRS_M2 msg: 1613174565.43 epoch sec; seqNo:9090 2021-02-13T00:02:45Z- AHRS_M2 msg: 0.58 phi; 0.11 theta; -0.15 psi 2021-02-13T00:02:45Z- handleDepth()-Depth_Keller msg: 1613174565437 epoch sec; seqNo:9162; depth 30.26 2021-02-13T00:02:45Z- handleDvl() - RDI_Pathfinder msg: 1613174565.45 epoch sec; seqNo:4433 2021-02-13T00:02:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.35 , 0.00 , 71.76 , 0.00 2021-02-13T00:02:45Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174565.50 epoch sec; seqNo:9162; 4058476.35 north; 591941.12 east 2021-02-13T00:02:45Z- AHRS_M2 msg: 1613174565.83 epoch sec; seqNo:9091 2021-02-13T00:02:45Z- AHRS_M2 msg: 0.57 phi; 0.14 theta; -0.16 psi 2021-02-13T00:02:45Z- handleDepth()-Depth_Keller msg: 1613174565836 epoch sec; seqNo:9163; depth 30.29 2021-02-13T00:02:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:45Z- handleDvl() - RDI_Pathfinder msg: 1613174565.84 epoch sec; seqNo:4434 2021-02-13T00:02:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:45Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174565.86 epoch sec; seqNo:9163; 4058476.35 north; 591941.12 east 2021-02-13T00:02:46Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 131 calls... 2021-02-13T00:02:46Z- waiting for 1613174564.00, time is 1613174565.86 2021-02-13T00:02:46Z- LcmTrn::updateTrn() equating pose 1613174565.86 and meas times 1613174565.86 2021-02-13T00:02:46Z- LcmTrn::updateTrn() >>>> pose time 1613174565.86, meas time 1613174565.86 <<<< 2021-02-13T00:02:46Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174565.86, ping # 0. 2021-02-13T00:02:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174565.86 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:02:46Z- TerrainNav::motionUpdate() - currEstimate = (4058476, 591941.12, 30.29) 2021-02-13T00:02:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:02:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:02:46Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:02:46Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:02:46Z- TNavBF::Projecting Measurements 2021-02-13T00:02:46Z- TNavFilter::Measurement from time = 1613174565.86 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:02:46Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:02:46Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:02:46Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:02:46Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:02:46Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:02:46Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:02:46Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:02:46Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 721.4 EastVariance: 833.0 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-13T00:02:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:02:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:02:46Z- variances([0], [2], and psi_berg) = 721.372542 833.010566 0.000000 2021-02-13T00:02:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:02:46Z- LcmTrn::publishEstimates() - MLE : 4058472.85 591896.62 30.29 2021-02-13T00:02:46Z- LcmTrn::publishEstimates() - MMSE : 4058476.35 591941.12 30.29 2021-02-13T00:02:46Z- LcmTrn::publishEstimates() - COVAR: 721.37 833.01 1.48 0.00 2021-02-13T00:02:46Z- AHRS_M2 msg: 1613174566.24 epoch sec; seqNo:9092 2021-02-13T00:02:46Z- AHRS_M2 msg: 0.56 phi; 0.17 theta; -0.16 psi 2021-02-13T00:02:46Z- handleDepth()-Depth_Keller msg: 1613174566240 epoch sec; seqNo:9164; depth 30.29 2021-02-13T00:02:46Z- handleDvl() - RDI_Pathfinder msg: 1613174566.25 epoch sec; seqNo:4435 2021-02-13T00:02:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:46Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174566.28 epoch sec; seqNo:9164; 4058476.73 north; 591941.12 east 2021-02-13T00:02:46Z- AHRS_M2 msg: 1613174566.64 epoch sec; seqNo:9093 2021-02-13T00:02:46Z- AHRS_M2 msg: 0.56 phi; 0.19 theta; -0.16 psi 2021-02-13T00:02:46Z- handleDepth()-Depth_Keller msg: 1613174566649 epoch sec; seqNo:9165; depth 30.31 2021-02-13T00:02:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174566.66 epoch sec; seqNo:9165; 4058477.12 north; 591942.34 east 2021-02-13T00:02:47Z- AHRS_M2 msg: 1613174567.05 epoch sec; seqNo:9094 2021-02-13T00:02:47Z- AHRS_M2 msg: 0.55 phi; 0.22 theta; -0.16 psi 2021-02-13T00:02:47Z- handleDepth()-Depth_Keller msg: 1613174567049 epoch sec; seqNo:9166; depth 30.28 2021-02-13T00:02:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:47Z- handleDvl() - RDI_Pathfinder msg: 1613174567.05 epoch sec; seqNo:4436 2021-02-13T00:02:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:47Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174567.06 epoch sec; seqNo:9166; 4058477.50 north; 591942.33 east 2021-02-13T00:02:47Z- AHRS_M2 msg: 1613174567.45 epoch sec; seqNo:9095 2021-02-13T00:02:47Z- AHRS_M2 msg: 0.54 phi; 0.24 theta; -0.16 psi 2021-02-13T00:02:47Z- handleDepth()-Depth_Keller msg: 1613174567452 epoch sec; seqNo:9167; depth 30.25 2021-02-13T00:02:47Z- handleDvl() - RDI_Pathfinder msg: 1613174567.47 epoch sec; seqNo:4437 2021-02-13T00:02:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:47Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174567.48 epoch sec; seqNo:9167; 4058477.50 north; 591942.33 east 2021-02-13T00:02:47Z- AHRS_M2 msg: 1613174567.85 epoch sec; seqNo:9096 2021-02-13T00:02:47Z- AHRS_M2 msg: 0.54 phi; 0.26 theta; -0.16 psi 2021-02-13T00:02:47Z- handleDepth()-Depth_Keller msg: 1613174567856 epoch sec; seqNo:9168; depth 30.22 2021-02-13T00:02:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174567.87 epoch sec; seqNo:9168; 4058477.88 north; 591942.33 east 2021-02-13T00:02:48Z- AHRS_M2 msg: 1613174568.26 epoch sec; seqNo:9097 2021-02-13T00:02:48Z- AHRS_M2 msg: 0.53 phi; 0.28 theta; -0.15 psi 2021-02-13T00:02:48Z- handleDepth()-Depth_Keller msg: 1613174568261 epoch sec; seqNo:9169; depth 30.15 2021-02-13T00:02:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:48Z- handleDvl() - RDI_Pathfinder msg: 1613174568.26 epoch sec; seqNo:4438 2021-02-13T00:02:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:48Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174568.28 epoch sec; seqNo:9169; 4058478.26 north; 591942.33 east 2021-02-13T00:02:48Z- AHRS_M2 msg: 1613174568.66 epoch sec; seqNo:9098 2021-02-13T00:02:48Z- AHRS_M2 msg: 0.53 phi; 0.29 theta; -0.15 psi 2021-02-13T00:02:48Z- handleDepth()-Depth_Keller msg: 1613174568673 epoch sec; seqNo:9170; depth 30.10 2021-02-13T00:02:48Z- handleDvl() - RDI_Pathfinder msg: 1613174568.68 epoch sec; seqNo:4439 2021-02-13T00:02:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.53 , 79.39 , 0.00 , 60.66 2021-02-13T00:02:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , 0.07 2021-02-13T00:02:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174568.70 epoch sec; seqNo:9170; 4058478.63 north; 591942.32 east 2021-02-13T00:02:49Z- AHRS_M2 msg: 1613174569.07 epoch sec; seqNo:9099 2021-02-13T00:02:49Z- AHRS_M2 msg: 0.53 phi; 0.31 theta; -0.15 psi 2021-02-13T00:02:49Z- handleDepth()-Depth_Keller msg: 1613174569072 epoch sec; seqNo:9171; depth 30.05 2021-02-13T00:02:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174569.12 epoch sec; seqNo:9171; 4058479.03 north; 591943.54 east 2021-02-13T00:02:49Z- AHRS_M2 msg: 1613174569.47 epoch sec; seqNo:9100 2021-02-13T00:02:49Z- AHRS_M2 msg: 0.53 phi; 0.32 theta; -0.15 psi 2021-02-13T00:02:49Z- handleDepth()-Depth_Keller msg: 1613174569472 epoch sec; seqNo:9172; depth 29.95 2021-02-13T00:02:49Z- handleDvl() - RDI_Pathfinder msg: 1613174569.48 epoch sec; seqNo:4440 2021-02-13T00:02:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 67.66 , 0.00 , 0.00 , 61.51 2021-02-13T00:02:49Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174569.52 epoch sec; seqNo:9172; 4058479.41 north; 591943.53 east 2021-02-13T00:02:49Z- AHRS_M2 msg: 1613174569.88 epoch sec; seqNo:9101 2021-02-13T00:02:49Z- AHRS_M2 msg: 0.54 phi; 0.33 theta; -0.14 psi 2021-02-13T00:02:49Z- handleDepth()-Depth_Keller msg: 1613174569882 epoch sec; seqNo:9173; depth 29.87 2021-02-13T00:02:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174569.90 epoch sec; seqNo:9173; 4058479.41 north; 591943.53 east 2021-02-13T00:02:50Z- AHRS_M2 msg: 1613174570.28 epoch sec; seqNo:9102 2021-02-13T00:02:50Z- AHRS_M2 msg: 0.54 phi; 0.34 theta; -0.13 psi 2021-02-13T00:02:50Z- handleDepth()-Depth_Keller msg: 1613174570289 epoch sec; seqNo:9174; depth 29.79 2021-02-13T00:02:50Z- handleDvl() - RDI_Pathfinder msg: 1613174570.30 epoch sec; seqNo:4441 2021-02-13T00:02:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 69.00 , 0.00 , 0.00 , 61.33 2021-02-13T00:02:50Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174570.31 epoch sec; seqNo:9174; 4058479.78 north; 591943.53 east 2021-02-13T00:02:50Z- AHRS_M2 msg: 1613174570.68 epoch sec; seqNo:9103 2021-02-13T00:02:50Z- AHRS_M2 msg: 0.55 phi; 0.35 theta; -0.12 psi 2021-02-13T00:02:50Z- handleDepth()-Depth_Keller msg: 1613174570684 epoch sec; seqNo:9175; depth 29.69 2021-02-13T00:02:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:50Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:50Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:50Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:50Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:50Z- handleDvl() - RDI_Pathfinder msg: 1613174570.69 epoch sec; seqNo:4442 2021-02-13T00:02:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:50Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174570.70 epoch sec; seqNo:9175; 4058480.16 north; 591943.53 east 2021-02-13T00:02:51Z- AHRS_M2 msg: 1613174571.08 epoch sec; seqNo:9104 2021-02-13T00:02:51Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.12 psi 2021-02-13T00:02:51Z- handleDepth()-Depth_Keller msg: 1613174571088 epoch sec; seqNo:9176; depth 29.62 2021-02-13T00:02:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174571.10 epoch sec; seqNo:9176; 4058480.54 north; 591943.52 east 2021-02-13T00:02:51Z- LcmTrn::handleMessages() - handling 47 messages took 0.72 ms and 133 calls... 2021-02-13T00:02:51Z- waiting for 1613174568.86, time is 1613174571.10 2021-02-13T00:02:51Z- LcmTrn::updateTrn() >>>> pose time 1613174571.10, meas time 1613174570.69 <<<< 2021-02-13T00:02:51Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174570.69, ping # 0. 2021-02-13T00:02:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174570.69 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:02:51Z- TerrainNav::motionUpdate() - currEstimate = (4058481, 591943.52, 29.62) 2021-02-13T00:02:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:02:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:02:51Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:02:51Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:02:51Z- TNavBF::Projecting Measurements 2021-02-13T00:02:51Z- TNavFilter::Measurement from time = 1613174570.69 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:02:51Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:02:51Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:02:51Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:02:51Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:02:51Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:02:51Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:02:51Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:02:51Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 721.3 EastVariance: 833.0 DepthVariance: 1.7 this->DepthBiasCov: 1.7 2021-02-13T00:02:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:02:51Z- TNavBF::mmse selected from filter -1 2021-02-13T00:02:51Z- variances([0], [2], and psi_berg) = 721.279207 833.010505 0.000000 2021-02-13T00:02:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:02:51Z- LcmTrn::publishEstimates() - MLE : 4058477.04 591899.02 29.62 2021-02-13T00:02:51Z- LcmTrn::publishEstimates() - MMSE : 4058480.54 591943.52 29.62 2021-02-13T00:02:51Z- LcmTrn::publishEstimates() - COVAR: 721.28 833.01 1.73 0.00 2021-02-13T00:02:51Z- AHRS_M2 msg: 1613174571.49 epoch sec; seqNo:9105 2021-02-13T00:02:51Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.11 psi 2021-02-13T00:02:51Z- handleDepth()-Depth_Keller msg: 1613174571492 epoch sec; seqNo:9177; depth 29.51 2021-02-13T00:02:51Z- handleDvl() - RDI_Pathfinder msg: 1613174571.51 epoch sec; seqNo:4443 2021-02-13T00:02:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 69.85 , 0.00 , 0.00 , 60.48 2021-02-13T00:02:51Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174571.52 epoch sec; seqNo:9177; 4058480.54 north; 591943.52 east 2021-02-13T00:02:51Z- AHRS_M2 msg: 1613174571.89 epoch sec; seqNo:9106 2021-02-13T00:02:51Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.11 psi 2021-02-13T00:02:51Z- handleDepth()-Depth_Keller msg: 1613174571898 epoch sec; seqNo:9178; depth 29.38 2021-02-13T00:02:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:51Z- handleDvl() - RDI_Pathfinder msg: 1613174571.90 epoch sec; seqNo:4444 2021-02-13T00:02:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:51Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174571.91 epoch sec; seqNo:9178; 4058480.93 north; 591944.74 east 2021-02-13T00:02:52Z- AHRS_M2 msg: 1613174572.30 epoch sec; seqNo:9107 2021-02-13T00:02:52Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.11 psi 2021-02-13T00:02:52Z- handleDepth()-Depth_Keller msg: 1613174572300 epoch sec; seqNo:9179; depth 29.28 2021-02-13T00:02:52Z- handleDvl() - RDI_Pathfinder msg: 1613174572.32 epoch sec; seqNo:4445 2021-02-13T00:02:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 70.71 , 0.00 , 0.00 , 60.92 2021-02-13T00:02:52Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174572.33 epoch sec; seqNo:9179; 4058481.31 north; 591944.73 east 2021-02-13T00:02:52Z- AHRS_M2 msg: 1613174572.70 epoch sec; seqNo:9108 2021-02-13T00:02:52Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.10 psi 2021-02-13T00:02:52Z- handleDepth()-Depth_Keller msg: 1613174572704 epoch sec; seqNo:9180; depth 29.18 2021-02-13T00:02:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174572.71 epoch sec; seqNo:9180; 4058481.69 north; 591944.73 east 2021-02-13T00:02:53Z- AHRS_M2 msg: 1613174573.10 epoch sec; seqNo:9109 2021-02-13T00:02:53Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.10 psi 2021-02-13T00:02:53Z- handleDepth()-Depth_Keller msg: 1613174573108 epoch sec; seqNo:9181; depth 29.07 2021-02-13T00:02:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:53Z- handleDvl() - RDI_Pathfinder msg: 1613174573.11 epoch sec; seqNo:4446 2021-02-13T00:02:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:53Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174573.12 epoch sec; seqNo:9181; 4058481.69 north; 591944.73 east 2021-02-13T00:02:53Z- AHRS_M2 msg: 1613174573.51 epoch sec; seqNo:9110 2021-02-13T00:02:53Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.10 psi 2021-02-13T00:02:53Z- handleDepth()-Depth_Keller msg: 1613174573523 epoch sec; seqNo:9182; depth 28.96 2021-02-13T00:02:53Z- handleDvl() - RDI_Pathfinder msg: 1613174573.54 epoch sec; seqNo:4447 2021-02-13T00:02:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:53Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174573.55 epoch sec; seqNo:9182; 4058482.07 north; 591944.73 east 2021-02-13T00:02:53Z- AHRS_M2 msg: 1613174573.91 epoch sec; seqNo:9111 2021-02-13T00:02:53Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.10 psi 2021-02-13T00:02:53Z- handleDepth()-Depth_Keller msg: 1613174573916 epoch sec; seqNo:9183; depth 28.80 2021-02-13T00:02:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174573.93 epoch sec; seqNo:9183; 4058482.45 north; 591944.72 east 2021-02-13T00:02:54Z- AHRS_M2 msg: 1613174574.32 epoch sec; seqNo:9112 2021-02-13T00:02:54Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.11 psi 2021-02-13T00:02:54Z- handleDepth()-Depth_Keller msg: 1613174574320 epoch sec; seqNo:9184; depth 28.70 2021-02-13T00:02:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:54Z- handleDvl() - RDI_Pathfinder msg: 1613174574.32 epoch sec; seqNo:4448 2021-02-13T00:02:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:54Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174574.33 epoch sec; seqNo:9184; 4058482.84 north; 591945.94 east 2021-02-13T00:02:54Z- AHRS_M2 msg: 1613174574.72 epoch sec; seqNo:9113 2021-02-13T00:02:54Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.11 psi 2021-02-13T00:02:54Z- handleDepth()-Depth_Keller msg: 1613174574725 epoch sec; seqNo:9185; depth 28.58 2021-02-13T00:02:54Z- handleDvl() - RDI_Pathfinder msg: 1613174574.74 epoch sec; seqNo:4449 2021-02-13T00:02:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 0.00 , 97.99 , 60.09 2021-02-13T00:02:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.04 , -0.09 2021-02-13T00:02:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174574.75 epoch sec; seqNo:9185; 4058482.84 north; 591945.94 east 2021-02-13T00:02:55Z- AHRS_M2 msg: 1613174575.13 epoch sec; seqNo:9114 2021-02-13T00:02:55Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.12 psi 2021-02-13T00:02:55Z- handleDepth()-Depth_Keller msg: 1613174575134 epoch sec; seqNo:9186; depth 28.45 2021-02-13T00:02:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174575.15 epoch sec; seqNo:9186; 4058483.22 north; 591945.93 east 2021-02-13T00:02:55Z- AHRS_M2 msg: 1613174575.55 epoch sec; seqNo:9115 2021-02-13T00:02:55Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.12 psi 2021-02-13T00:02:55Z- handleDepth()-Depth_Keller msg: 1613174575561 epoch sec; seqNo:9187; depth 28.33 2021-02-13T00:02:55Z- handleDvl() - RDI_Pathfinder msg: 1613174575.57 epoch sec; seqNo:4450 2021-02-13T00:02:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.90 , 82.84 , 97.99 , 60.91 2021-02-13T00:02:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.05 , -0.06 2021-02-13T00:02:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174575.58 epoch sec; seqNo:9187; 4058483.60 north; 591945.93 east 2021-02-13T00:02:55Z- AHRS_M2 msg: 1613174575.93 epoch sec; seqNo:9116 2021-02-13T00:02:55Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.13 psi 2021-02-13T00:02:55Z- handleDepth()-Depth_Keller msg: 1613174575936 epoch sec; seqNo:9188; depth 28.19 2021-02-13T00:02:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174575.97 epoch sec; seqNo:9188; 4058483.60 north; 591945.93 east 2021-02-13T00:02:56Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-13T00:02:56Z- waiting for 1613174574.10, time is 1613174575.97 2021-02-13T00:02:56Z- LcmTrn::updateTrn() >>>> pose time 1613174575.97, meas time 1613174575.57 <<<< 2021-02-13T00:02:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174575.57 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:02:56Z- TerrainNav::motionUpdate() - currEstimate = (4058484, 591945.93, 28.19) 2021-02-13T00:02:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:02:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:02:56Z- TNavBF::Projecting Measurements 2021-02-13T00:02:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:02:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:02:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:02:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:02:56Z- TNavFilter::4 good beams. 2021-02-13T00:02:56Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:02:56Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:02:56Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:02:56Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:02:56Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:02:56Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:02:56Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:02:56Z- TNavBF::mmse selected from filter -1 2021-02-13T00:02:56Z- TerrainMapOctree: (vehN, vehE ) = (4058483.35, 591945.74). 2021-02-13T00:02:56Z- TerrainMapOctree: Min Distance = 3600.55. 2021-02-13T00:02:56Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:02:56Z- There is no specific good beam that all particles have in common. 2021-02-13T00:02:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:02:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.91 K: 0.167 ExpectedMeasDiff: nan currMeas.covariance: 7.82 2021-02-13T00:02:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:02:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.59 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 10.98 2021-02-13T00:02:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:02:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.42 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:02:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:02:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.146 ExpectedMeasDiff: nan currMeas.covariance: 5.94 2021-02-13T00:02:56Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:02:56Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:02:56Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:02:56Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:02:56Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:02:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:02:56Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:02:56Z- Filter 0 No beams used in subcloud update 2021-02-13T00:02:56Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:02:56Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:02:56Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 721.2 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:02:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174575.57 sec. 2021-02-13T00:02:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:02:56Z- TNavBF::mmse selected from filter -1 2021-02-13T00:02:56Z- variances([0], [2], and psi_berg) = 721.195275 833.010449 0.000000 2021-02-13T00:02:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:02:56Z- LcmTrn::publishEstimates() - MLE : 4058480.10 591901.43 28.19 2021-02-13T00:02:56Z- LcmTrn::publishEstimates() - MMSE : 4058483.60 591945.93 28.19 2021-02-13T00:02:56Z- LcmTrn::publishEstimates() - COVAR: 721.20 833.01 1.13 0.00 2021-02-13T00:02:56Z- AHRS_M2 msg: 1613174576.34 epoch sec; seqNo:9117 2021-02-13T00:02:56Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.13 psi 2021-02-13T00:02:56Z- handleDepth()-Depth_Keller msg: 1613174576340 epoch sec; seqNo:9189; depth 28.07 2021-02-13T00:02:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174576.35 epoch sec; seqNo:9189; 4058483.98 north; 591945.93 east 2021-02-13T00:02:56Z- AHRS_M2 msg: 1613174576.75 epoch sec; seqNo:9118 2021-02-13T00:02:56Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.14 psi 2021-02-13T00:02:56Z- handleDepth()-Depth_Keller msg: 1613174576752 epoch sec; seqNo:9190; depth 27.95 2021-02-13T00:02:56Z- handleDvl() - RDI_Pathfinder msg: 1613174576.76 epoch sec; seqNo:4451 2021-02-13T00:02:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:56Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:02:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174576.78 epoch sec; seqNo:9190; 4058484.36 north; 591945.92 east 2021-02-13T00:02:57Z- AHRS_M2 msg: 1613174577.15 epoch sec; seqNo:9119 2021-02-13T00:02:57Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.14 psi 2021-02-13T00:02:57Z- handleDepth()-Depth_Keller msg: 1613174577154 epoch sec; seqNo:9191; depth 27.83 2021-02-13T00:02:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174577.17 epoch sec; seqNo:9191; 4058484.36 north; 591945.92 east 2021-02-13T00:02:57Z- AHRS_M2 msg: 1613174577.55 epoch sec; seqNo:9120 2021-02-13T00:02:57Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.14 psi 2021-02-13T00:02:57Z- handleDepth()-Depth_Keller msg: 1613174577552 epoch sec; seqNo:9192; depth 27.70 2021-02-13T00:02:57Z- handleDvl() - RDI_Pathfinder msg: 1613174577.56 epoch sec; seqNo:4452 2021-02-13T00:02:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 83.52 , 98.13 , 62.13 2021-02-13T00:02:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.11 , 0.02 2021-02-13T00:02:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174577.57 epoch sec; seqNo:9192; 4058484.75 north; 591947.14 east 2021-02-13T00:02:57Z- AHRS_M2 msg: 1613174577.95 epoch sec; seqNo:9121 2021-02-13T00:02:57Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.14 psi 2021-02-13T00:02:57Z- handleDepth()-Depth_Keller msg: 1613174577961 epoch sec; seqNo:9193; depth 27.57 2021-02-13T00:02:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:57Z- handleDvl() - RDI_Pathfinder msg: 1613174577.96 epoch sec; seqNo:4453 2021-02-13T00:02:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.11 , 0.02 2021-02-13T00:02:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174577.98 epoch sec; seqNo:9193; 4058485.13 north; 591947.14 east 2021-02-13T00:02:58Z- AHRS_M2 msg: 1613174578.36 epoch sec; seqNo:9122 2021-02-13T00:02:58Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T00:02:58Z- handleDepth()-Depth_Keller msg: 1613174578362 epoch sec; seqNo:9194; depth 27.45 2021-02-13T00:02:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174578.37 epoch sec; seqNo:9194; 4058485.51 north; 591947.13 east 2021-02-13T00:02:58Z- AHRS_M2 msg: 1613174578.76 epoch sec; seqNo:9123 2021-02-13T00:02:58Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T00:02:58Z- handleDepth()-Depth_Keller msg: 1613174578769 epoch sec; seqNo:9195; depth 27.35 2021-02-13T00:02:58Z- handleDvl() - RDI_Pathfinder msg: 1613174578.79 epoch sec; seqNo:4454 2021-02-13T00:02:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 83.93 , 97.99 , 61.72 2021-02-13T00:02:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.09 , 0.06 2021-02-13T00:02:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174578.80 epoch sec; seqNo:9195; 4058485.51 north; 591947.13 east 2021-02-13T00:02:59Z- AHRS_M2 msg: 1613174579.16 epoch sec; seqNo:9124 2021-02-13T00:02:59Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T00:02:59Z- handleDepth()-Depth_Keller msg: 1613174579168 epoch sec; seqNo:9196; depth 27.20 2021-02-13T00:02:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:02:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:02:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:02:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:02:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:02:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:02:59Z- handleDvl() - RDI_Pathfinder msg: 1613174579.17 epoch sec; seqNo:4455 2021-02-13T00:02:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:02:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.09 , 0.06 2021-02-13T00:02:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174579.18 epoch sec; seqNo:9196; 4058485.89 north; 591947.13 east 2021-02-13T00:02:59Z- AHRS_M2 msg: 1613174579.57 epoch sec; seqNo:9125 2021-02-13T00:02:59Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T00:02:59Z- handleDepth()-Depth_Keller msg: 1613174579574 epoch sec; seqNo:9197; depth 27.09 2021-02-13T00:02:59Z- handleDvl() - RDI_Pathfinder msg: 1613174579.59 epoch sec; seqNo:4456 2021-02-13T00:02:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.53 , 83.86 , 99.00 , 62.95 2021-02-13T00:02:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , 0.09 2021-02-13T00:02:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174579.60 epoch sec; seqNo:9197; 4058486.26 north; 591947.12 east 2021-02-13T00:02:59Z- AHRS_M2 msg: 1613174579.97 epoch sec; seqNo:9126 2021-02-13T00:02:59Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T00:02:59Z- handleDepth()-Depth_Keller msg: 1613174579978 epoch sec; seqNo:9198; depth 26.95 2021-02-13T00:02:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174579.99 epoch sec; seqNo:9198; 4058486.64 north; 591947.12 east 2021-02-13T00:03:00Z- AHRS_M2 msg: 1613174580.38 epoch sec; seqNo:9127 2021-02-13T00:03:00Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T00:03:00Z- handleDepth()-Depth_Keller msg: 1613174580390 epoch sec; seqNo:9199; depth 26.84 2021-02-13T00:03:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:00Z- handleDvl() - RDI_Pathfinder msg: 1613174580.39 epoch sec; seqNo:4457 2021-02-13T00:03:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , 0.09 2021-02-13T00:03:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174580.40 epoch sec; seqNo:9199; 4058486.64 north; 591947.12 east 2021-02-13T00:03:00Z- AHRS_M2 msg: 1613174580.78 epoch sec; seqNo:9128 2021-02-13T00:03:00Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T00:03:00Z- handleDepth()-Depth_Keller msg: 1613174580784 epoch sec; seqNo:9200; depth 26.71 2021-02-13T00:03:00Z- handleDvl() - RDI_Pathfinder msg: 1613174580.80 epoch sec; seqNo:4458 2021-02-13T00:03:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 85.00 , 97.99 , 63.76 2021-02-13T00:03:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , 0.09 2021-02-13T00:03:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174580.82 epoch sec; seqNo:9200; 4058487.04 north; 591948.34 east 2021-02-13T00:03:01Z- AHRS_M2 msg: 1613174581.18 epoch sec; seqNo:9129 2021-02-13T00:03:01Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-13T00:03:01Z- handleDepth()-Depth_Keller msg: 1613174581188 epoch sec; seqNo:9201; depth 26.60 2021-02-13T00:03:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174581.20 epoch sec; seqNo:9201; 4058487.41 north; 591948.33 east 2021-02-13T00:03:01Z- LcmTrn::handleMessages() - handling 47 messages took 0.69 ms and 133 calls... 2021-02-13T00:03:01Z- waiting for 1613174578.97, time is 1613174581.20 2021-02-13T00:03:01Z- LcmTrn::updateTrn() >>>> pose time 1613174581.20, meas time 1613174580.80 <<<< 2021-02-13T00:03:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174580.80 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:03:01Z- TerrainNav::motionUpdate() - currEstimate = (4058487, 591948.33, 26.60) 2021-02-13T00:03:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:03:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:03:01Z- TNavBF::Projecting Measurements 2021-02-13T00:03:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:03:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:03:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:03:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:03:01Z- TNavFilter::4 good beams. 2021-02-13T00:03:01Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:03:01Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:03:01Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:03:01Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:03:01Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:03:01Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:03:01Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:03:01Z- TNavBF::mmse selected from filter -1 2021-02-13T00:03:01Z- TerrainMapOctree: (vehN, vehE ) = (4058487.12, 591948.15). 2021-02-13T00:03:01Z- TerrainMapOctree: Min Distance = 3596.11. 2021-02-13T00:03:01Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:03:01Z- There is no specific good beam that all particles have in common. 2021-02-13T00:03:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:03:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.35 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:03:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:03:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 11.56 2021-02-13T00:03:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:03:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:03:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:03:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 6.50 2021-02-13T00:03:01Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:03:01Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:03:01Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:03:01Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:03:01Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:03:01Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:03:01Z- Filter 0 No beams used in subcloud update 2021-02-13T00:03:01Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:03:01Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:03:01Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 721.1 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:03:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174580.80 sec. 2021-02-13T00:03:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:03:01Z- TNavBF::mmse selected from filter -1 2021-02-13T00:03:01Z- variances([0], [2], and psi_berg) = 721.105407 833.010390 0.000000 2021-02-13T00:03:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:03:01Z- LcmTrn::publishEstimates() - MLE : 4058482.91 591913.83 26.60 2021-02-13T00:03:01Z- LcmTrn::publishEstimates() - MMSE : 4058487.41 591948.33 26.60 2021-02-13T00:03:01Z- LcmTrn::publishEstimates() - COVAR: 721.11 833.01 0.94 0.00 2021-02-13T00:03:01Z- AHRS_M2 msg: 1613174581.62 epoch sec; seqNo:9130 2021-02-13T00:03:01Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-13T00:03:01Z- handleDepth()-Depth_Keller msg: 1613174581621 epoch sec; seqNo:9202; depth 26.48 2021-02-13T00:03:01Z- handleDvl() - RDI_Pathfinder msg: 1613174581.63 epoch sec; seqNo:4459 2021-02-13T00:03:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 85.95 , 98.93 , 64.17 2021-02-13T00:03:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.08 2021-02-13T00:03:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174581.66 epoch sec; seqNo:9202; 4058487.79 north; 591948.33 east 2021-02-13T00:03:01Z- AHRS_M2 msg: 1613174581.99 epoch sec; seqNo:9131 2021-02-13T00:03:01Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-13T00:03:02Z- handleDepth()-Depth_Keller msg: 1613174582006 epoch sec; seqNo:9203; depth 26.34 2021-02-13T00:03:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174582.02 epoch sec; seqNo:9203; 4058487.79 north; 591948.33 east 2021-02-13T00:03:02Z- AHRS_M2 msg: 1613174582.40 epoch sec; seqNo:9132 2021-02-13T00:03:02Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-13T00:03:02Z- handleDepth()-Depth_Keller msg: 1613174582400 epoch sec; seqNo:9204; depth 26.25 2021-02-13T00:03:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174582.41 epoch sec; seqNo:9204; 4058488.17 north; 591948.32 east 2021-02-13T00:03:02Z- AHRS_M2 msg: 1613174582.80 epoch sec; seqNo:9133 2021-02-13T00:03:02Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-13T00:03:02Z- handleDepth()-Depth_Keller msg: 1613174582804 epoch sec; seqNo:9205; depth 26.12 2021-02-13T00:03:02Z- handleDvl() - RDI_Pathfinder msg: 1613174582.82 epoch sec; seqNo:4460 2021-02-13T00:03:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 0.00 , 98.87 , 64.58 2021-02-13T00:03:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , 0.07 2021-02-13T00:03:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174582.83 epoch sec; seqNo:9205; 4058488.55 north; 591948.32 east 2021-02-13T00:03:03Z- AHRS_M2 msg: 1613174583.20 epoch sec; seqNo:9134 2021-02-13T00:03:03Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-13T00:03:03Z- handleDepth()-Depth_Keller msg: 1613174583217 epoch sec; seqNo:9206; depth 26.02 2021-02-13T00:03:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174583.23 epoch sec; seqNo:9206; 4058488.94 north; 591949.54 east 2021-02-13T00:03:03Z- AHRS_M2 msg: 1613174583.61 epoch sec; seqNo:9135 2021-02-13T00:03:03Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-13T00:03:03Z- handleDepth()-Depth_Keller msg: 1613174583622 epoch sec; seqNo:9207; depth 25.88 2021-02-13T00:03:03Z- handleDvl() - RDI_Pathfinder msg: 1613174583.63 epoch sec; seqNo:4461 2021-02-13T00:03:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.94 , 86.09 , 97.99 , 64.58 2021-02-13T00:03:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.06 , 0.04 2021-02-13T00:03:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174583.64 epoch sec; seqNo:9207; 4058488.94 north; 591949.54 east 2021-02-13T00:03:04Z- AHRS_M2 msg: 1613174584.01 epoch sec; seqNo:9136 2021-02-13T00:03:04Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-13T00:03:04Z- handleDepth()-Depth_Keller msg: 1613174584017 epoch sec; seqNo:9208; depth 25.78 2021-02-13T00:03:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:04Z- handleDvl() - RDI_Pathfinder msg: 1613174584.02 epoch sec; seqNo:4462 2021-02-13T00:03:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.06 , 0.04 2021-02-13T00:03:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174584.05 epoch sec; seqNo:9208; 4058489.32 north; 591949.53 east 2021-02-13T00:03:04Z- AHRS_M2 msg: 1613174584.42 epoch sec; seqNo:9137 2021-02-13T00:03:04Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.15 psi 2021-02-13T00:03:04Z- handleDepth()-Depth_Keller msg: 1613174584420 epoch sec; seqNo:9209; depth 25.64 2021-02-13T00:03:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174584.43 epoch sec; seqNo:9209; 4058489.70 north; 591949.53 east 2021-02-13T00:03:04Z- AHRS_M2 msg: 1613174584.82 epoch sec; seqNo:9138 2021-02-13T00:03:04Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.15 psi 2021-02-13T00:03:04Z- handleDepth()-Depth_Keller msg: 1613174584829 epoch sec; seqNo:9210; depth 25.52 2021-02-13T00:03:04Z- handleDvl() - RDI_Pathfinder msg: 1613174584.84 epoch sec; seqNo:4463 2021-02-13T00:03:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 87.10 , 97.92 , 64.58 2021-02-13T00:03:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.08 , 0.00 2021-02-13T00:03:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174584.85 epoch sec; seqNo:9210; 4058490.08 north; 591949.52 east 2021-02-13T00:03:05Z- AHRS_M2 msg: 1613174585.22 epoch sec; seqNo:9139 2021-02-13T00:03:05Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.15 psi 2021-02-13T00:03:05Z- handleDepth()-Depth_Keller msg: 1613174585233 epoch sec; seqNo:9211; depth 25.42 2021-02-13T00:03:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:05Z- handleDvl() - RDI_Pathfinder msg: 1613174585.23 epoch sec; seqNo:4464 2021-02-13T00:03:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.08 , 0.00 2021-02-13T00:03:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174585.24 epoch sec; seqNo:9211; 4058490.09 north; 591950.74 east 2021-02-13T00:03:05Z- AHRS_M2 msg: 1613174585.63 epoch sec; seqNo:9140 2021-02-13T00:03:05Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.15 psi 2021-02-13T00:03:05Z- handleDepth()-Depth_Keller msg: 1613174585632 epoch sec; seqNo:9212; depth 25.29 2021-02-13T00:03:05Z- handleDvl() - RDI_Pathfinder msg: 1613174585.64 epoch sec; seqNo:4465 2021-02-13T00:03:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.17 , 86.97 , 98.87 , 65.81 2021-02-13T00:03:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , -0.05 2021-02-13T00:03:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174585.66 epoch sec; seqNo:9212; 4058490.47 north; 591950.74 east 2021-02-13T00:03:06Z- AHRS_M2 msg: 1613174586.03 epoch sec; seqNo:9141 2021-02-13T00:03:06Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.15 psi 2021-02-13T00:03:06Z- handleDepth()-Depth_Keller msg: 1613174586038 epoch sec; seqNo:9213; depth 25.17 2021-02-13T00:03:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174586.05 epoch sec; seqNo:9213; 4058490.85 north; 591950.74 east 2021-02-13T00:03:06Z- LcmTrn::handleMessages() - handling 43 messages took 0.67 ms and 130 calls... 2021-02-13T00:03:06Z- waiting for 1613174584.20, time is 1613174586.05 2021-02-13T00:03:06Z- LcmTrn::updateTrn() >>>> pose time 1613174586.05, meas time 1613174585.64 <<<< 2021-02-13T00:03:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174585.64 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:03:06Z- TerrainNav::motionUpdate() - currEstimate = (4058491, 591950.74, 25.17) 2021-02-13T00:03:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:03:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:03:06Z- TNavBF::Projecting Measurements 2021-02-13T00:03:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:03:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:03:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:03:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:03:06Z- TNavFilter::4 good beams. 2021-02-13T00:03:06Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:03:06Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:03:06Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:03:06Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:03:06Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:03:06Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:03:06Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:03:06Z- TNavBF::mmse selected from filter -1 2021-02-13T00:03:06Z- TerrainMapOctree: (vehN, vehE ) = (4058490.56, 591950.53). 2021-02-13T00:03:06Z- TerrainMapOctree: Min Distance = 3591.94. 2021-02-13T00:03:06Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:03:06Z- There is no specific good beam that all particles have in common. 2021-02-13T00:03:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:03:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 8.57 2021-02-13T00:03:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:03:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 12.10 2021-02-13T00:03:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:03:06Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 15.64 2021-02-13T00:03:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:03:06Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 6.93 2021-02-13T00:03:06Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:03:06Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:03:06Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:03:06Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:03:06Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:03:06Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:03:06Z- Filter 0 No beams used in subcloud update 2021-02-13T00:03:06Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:03:06Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:03:06Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 721.0 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:03:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174585.64 sec. 2021-02-13T00:03:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:03:06Z- TNavBF::mmse selected from filter -1 2021-02-13T00:03:06Z- variances([0], [2], and psi_berg) = 721.018200 833.010332 0.000000 2021-02-13T00:03:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:03:06Z- LcmTrn::publishEstimates() - MLE : 4058486.35 591916.24 25.17 2021-02-13T00:03:06Z- LcmTrn::publishEstimates() - MMSE : 4058490.85 591950.74 25.17 2021-02-13T00:03:06Z- LcmTrn::publishEstimates() - COVAR: 721.02 833.01 0.84 0.00 2021-02-13T00:03:06Z- AHRS_M2 msg: 1613174586.43 epoch sec; seqNo:9142 2021-02-13T00:03:06Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.15 psi 2021-02-13T00:03:06Z- handleDepth()-Depth_Keller msg: 1613174586445 epoch sec; seqNo:9214; depth 25.04 2021-02-13T00:03:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:06Z- handleDvl() - RDI_Pathfinder msg: 1613174586.45 epoch sec; seqNo:4466 2021-02-13T00:03:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , -0.05 2021-02-13T00:03:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174586.46 epoch sec; seqNo:9214; 4058490.85 north; 591950.74 east 2021-02-13T00:03:06Z- AHRS_M2 msg: 1613174586.84 epoch sec; seqNo:9143 2021-02-13T00:03:06Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.15 psi 2021-02-13T00:03:06Z- handleDepth()-Depth_Keller msg: 1613174586844 epoch sec; seqNo:9215; depth 24.92 2021-02-13T00:03:06Z- handleDvl() - RDI_Pathfinder msg: 1613174586.86 epoch sec; seqNo:4467 2021-02-13T00:03:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:06Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:03:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174586.87 epoch sec; seqNo:9215; 4058491.23 north; 591950.73 east 2021-02-13T00:03:07Z- AHRS_M2 msg: 1613174587.24 epoch sec; seqNo:9144 2021-02-13T00:03:07Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.15 psi 2021-02-13T00:03:07Z- handleDepth()-Depth_Keller msg: 1613174587258 epoch sec; seqNo:9216; depth 24.81 2021-02-13T00:03:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174587.27 epoch sec; seqNo:9216; 4058491.61 north; 591950.73 east 2021-02-13T00:03:07Z- AHRS_M2 msg: 1613174587.65 epoch sec; seqNo:9145 2021-02-13T00:03:07Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.15 psi 2021-02-13T00:03:07Z- handleDepth()-Depth_Keller msg: 1613174587657 epoch sec; seqNo:9217; depth 24.69 2021-02-13T00:03:07Z- handleDvl() - RDI_Pathfinder msg: 1613174587.67 epoch sec; seqNo:4468 2021-02-13T00:03:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 88.05 , 99.00 , 65.81 2021-02-13T00:03:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.07 2021-02-13T00:03:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174587.68 epoch sec; seqNo:9217; 4058491.62 north; 591951.95 east 2021-02-13T00:03:08Z- AHRS_M2 msg: 1613174588.05 epoch sec; seqNo:9146 2021-02-13T00:03:08Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.15 psi 2021-02-13T00:03:08Z- handleDepth()-Depth_Keller msg: 1613174588058 epoch sec; seqNo:9218; depth 24.55 2021-02-13T00:03:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174588.07 epoch sec; seqNo:9218; 4058492.00 north; 591951.95 east 2021-02-13T00:03:08Z- AHRS_M2 msg: 1613174588.46 epoch sec; seqNo:9147 2021-02-13T00:03:08Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.15 psi 2021-02-13T00:03:08Z- handleDepth()-Depth_Keller msg: 1613174588465 epoch sec; seqNo:9219; depth 24.43 2021-02-13T00:03:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174588.47 epoch sec; seqNo:9219; 4058492.38 north; 591951.94 east 2021-02-13T00:03:08Z- AHRS_M2 msg: 1613174588.86 epoch sec; seqNo:9148 2021-02-13T00:03:08Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.16 psi 2021-02-13T00:03:08Z- handleDepth()-Depth_Keller msg: 1613174588864 epoch sec; seqNo:9220; depth 24.31 2021-02-13T00:03:08Z- handleDvl() - RDI_Pathfinder msg: 1613174588.88 epoch sec; seqNo:4469 2021-02-13T00:03:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.79 , 87.31 , 97.04 , 66.22 2021-02-13T00:03:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , -0.06 2021-02-13T00:03:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174588.88 epoch sec; seqNo:9220; 4058492.38 north; 591951.94 east 2021-02-13T00:03:09Z- AHRS_M2 msg: 1613174589.27 epoch sec; seqNo:9149 2021-02-13T00:03:09Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.16 psi 2021-02-13T00:03:09Z- handleDepth()-Depth_Keller msg: 1613174589271 epoch sec; seqNo:9221; depth 24.20 2021-02-13T00:03:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174589.28 epoch sec; seqNo:9221; 4058492.76 north; 591951.94 east 2021-02-13T00:03:09Z- AHRS_M2 msg: 1613174589.67 epoch sec; seqNo:9150 2021-02-13T00:03:09Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.16 psi 2021-02-13T00:03:09Z- handleDepth()-Depth_Keller msg: 1613174589672 epoch sec; seqNo:9222; depth 24.07 2021-02-13T00:03:09Z- handleDvl() - RDI_Pathfinder msg: 1613174589.68 epoch sec; seqNo:4470 2021-02-13T00:03:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.17 , 88.05 , 99.95 , 64.99 2021-02-13T00:03:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , -0.06 2021-02-13T00:03:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174589.70 epoch sec; seqNo:9222; 4058493.14 north; 591951.93 east 2021-02-13T00:03:10Z- AHRS_M2 msg: 1613174590.07 epoch sec; seqNo:9151 2021-02-13T00:03:10Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.16 psi 2021-02-13T00:03:10Z- handleDepth()-Depth_Keller msg: 1613174590081 epoch sec; seqNo:9223; depth 23.97 2021-02-13T00:03:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:10Z- handleDvl() - RDI_Pathfinder msg: 1613174590.08 epoch sec; seqNo:4471 2021-02-13T00:03:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , -0.06 2021-02-13T00:03:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174590.09 epoch sec; seqNo:9223; 4058493.14 north; 591951.93 east 2021-02-13T00:03:10Z- AHRS_M2 msg: 1613174590.50 epoch sec; seqNo:9152 2021-02-13T00:03:10Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.17 psi 2021-02-13T00:03:10Z- handleDepth()-Depth_Keller msg: 1613174590507 epoch sec; seqNo:9224; depth 23.85 2021-02-13T00:03:10Z- handleDvl() - RDI_Pathfinder msg: 1613174590.52 epoch sec; seqNo:4472 2021-02-13T00:03:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 0.00 , 100.01 , 67.03 2021-02-13T00:03:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , -0.05 2021-02-13T00:03:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174590.55 epoch sec; seqNo:9224; 4058493.53 north; 591953.15 east 2021-02-13T00:03:10Z- AHRS_M2 msg: 1613174590.88 epoch sec; seqNo:9153 2021-02-13T00:03:10Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.17 psi 2021-02-13T00:03:10Z- handleDepth()-Depth_Keller msg: 1613174590889 epoch sec; seqNo:9225; depth 23.72 2021-02-13T00:03:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174590.91 epoch sec; seqNo:9225; 4058493.91 north; 591953.15 east 2021-02-13T00:03:11Z- AHRS_M2 msg: 1613174591.28 epoch sec; seqNo:9154 2021-02-13T00:03:11Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.17 psi 2021-02-13T00:03:11Z- handleDepth()-Depth_Keller msg: 1613174591288 epoch sec; seqNo:9226; depth 23.60 2021-02-13T00:03:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:11Z- handleDvl() - RDI_Pathfinder msg: 1613174591.29 epoch sec; seqNo:4473 2021-02-13T00:03:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , -0.05 2021-02-13T00:03:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174591.30 epoch sec; seqNo:9226; 4058494.29 north; 591953.14 east 2021-02-13T00:03:11Z- LcmTrn::handleMessages() - handling 47 messages took 0.67 ms and 134 calls... 2021-02-13T00:03:11Z- waiting for 1613174589.05, time is 1613174591.30 2021-02-13T00:03:11Z- LcmTrn::updateTrn() equating pose 1613174591.30 and meas times 1613174591.30 2021-02-13T00:03:11Z- LcmTrn::updateTrn() >>>> pose time 1613174591.30, meas time 1613174591.30 <<<< 2021-02-13T00:03:11Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174591.30, ping # 0. 2021-02-13T00:03:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174591.30 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:03:11Z- TerrainNav::motionUpdate() - currEstimate = (4058494, 591953.14, 23.60) 2021-02-13T00:03:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:03:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:03:11Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:03:11Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:03:11Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174591.30 sec.; No relevant navigation data available 2021-02-13T00:03:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:03:11Z- TNavBF::mmse selected from filter -1 2021-02-13T00:03:11Z- variances([0], [2], and psi_berg) = 720.948322 833.010286 0.000000 2021-02-13T00:03:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:03:11Z- LcmTrn::publishEstimates() - MLE : 4058489.79 591918.64 23.60 2021-02-13T00:03:11Z- LcmTrn::publishEstimates() - MMSE : 4058494.29 591953.14 23.60 2021-02-13T00:03:11Z- LcmTrn::publishEstimates() - COVAR: 720.95 833.01 1.05 0.00 2021-02-13T00:03:11Z- AHRS_M2 msg: 1613174591.69 epoch sec; seqNo:9155 2021-02-13T00:03:11Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.17 psi 2021-02-13T00:03:11Z- handleDepth()-Depth_Keller msg: 1613174591692 epoch sec; seqNo:9227; depth 23.49 2021-02-13T00:03:11Z- handleDvl() - RDI_Pathfinder msg: 1613174591.70 epoch sec; seqNo:4474 2021-02-13T00:03:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.16 , 89.33 , 100.15 , 67.44 2021-02-13T00:03:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.12 , -0.01 2021-02-13T00:03:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174591.71 epoch sec; seqNo:9227; 4058494.29 north; 591953.14 east 2021-02-13T00:03:12Z- AHRS_M2 msg: 1613174592.09 epoch sec; seqNo:9156 2021-02-13T00:03:12Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.17 psi 2021-02-13T00:03:12Z- handleDepth()-Depth_Keller msg: 1613174592105 epoch sec; seqNo:9228; depth 23.37 2021-02-13T00:03:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174592.12 epoch sec; seqNo:9228; 4058494.66 north; 591953.14 east 2021-02-13T00:03:12Z- AHRS_M2 msg: 1613174592.50 epoch sec; seqNo:9157 2021-02-13T00:03:12Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.17 psi 2021-02-13T00:03:12Z- handleDepth()-Depth_Keller msg: 1613174592500 epoch sec; seqNo:9229; depth 23.25 2021-02-13T00:03:12Z- handleDvl() - RDI_Pathfinder msg: 1613174592.52 epoch sec; seqNo:4475 2021-02-13T00:03:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 0.00 , 99.61 , 65.81 2021-02-13T00:03:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , 0.05 2021-02-13T00:03:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174592.53 epoch sec; seqNo:9229; 4058495.04 north; 591953.13 east 2021-02-13T00:03:12Z- AHRS_M2 msg: 1613174592.90 epoch sec; seqNo:9158 2021-02-13T00:03:12Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.17 psi 2021-02-13T00:03:12Z- handleDepth()-Depth_Keller msg: 1613174592904 epoch sec; seqNo:9230; depth 23.13 2021-02-13T00:03:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174592.92 epoch sec; seqNo:9230; 4058495.42 north; 591953.13 east 2021-02-13T00:03:13Z- AHRS_M2 msg: 1613174593.30 epoch sec; seqNo:9159 2021-02-13T00:03:13Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.17 psi 2021-02-13T00:03:13Z- handleDepth()-Depth_Keller msg: 1613174593308 epoch sec; seqNo:9231; depth 23.02 2021-02-13T00:03:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174593.32 epoch sec; seqNo:9231; 4058495.44 north; 591954.35 east 2021-02-13T00:03:13Z- AHRS_M2 msg: 1613174593.71 epoch sec; seqNo:9160 2021-02-13T00:03:13Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.17 psi 2021-02-13T00:03:13Z- handleDepth()-Depth_Keller msg: 1613174593719 epoch sec; seqNo:9232; depth 22.90 2021-02-13T00:03:13Z- handleDvl() - RDI_Pathfinder msg: 1613174593.73 epoch sec; seqNo:4476 2021-02-13T00:03:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.57 , 0.00 , 101.23 , 65.81 2021-02-13T00:03:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.11 2021-02-13T00:03:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174593.74 epoch sec; seqNo:9232; 4058495.81 north; 591954.35 east 2021-02-13T00:03:14Z- AHRS_M2 msg: 1613174594.12 epoch sec; seqNo:9161 2021-02-13T00:03:14Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.16 psi 2021-02-13T00:03:14Z- handleDepth()-Depth_Keller msg: 1613174594133 epoch sec; seqNo:9233; depth 22.77 2021-02-13T00:03:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174594.15 epoch sec; seqNo:9233; 4058496.19 north; 591954.34 east 2021-02-13T00:03:14Z- AHRS_M2 msg: 1613174594.52 epoch sec; seqNo:9162 2021-02-13T00:03:14Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.16 psi 2021-02-13T00:03:14Z- handleDepth()-Depth_Keller msg: 1613174594520 epoch sec; seqNo:9234; depth 22.66 2021-02-13T00:03:14Z- handleDvl() - RDI_Pathfinder msg: 1613174594.53 epoch sec; seqNo:4477 2021-02-13T00:03:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.99 , 0.00 , 102.25 , 67.86 2021-02-13T00:03:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.04 , 0.13 2021-02-13T00:03:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174594.56 epoch sec; seqNo:9234; 4058496.19 north; 591954.34 east 2021-02-13T00:03:14Z- AHRS_M2 msg: 1613174594.92 epoch sec; seqNo:9163 2021-02-13T00:03:14Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.16 psi 2021-02-13T00:03:14Z- handleDepth()-Depth_Keller msg: 1613174594929 epoch sec; seqNo:9235; depth 22.54 2021-02-13T00:03:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:14Z- handleDvl() - RDI_Pathfinder msg: 1613174594.93 epoch sec; seqNo:4478 2021-02-13T00:03:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.04 , 0.13 2021-02-13T00:03:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174594.94 epoch sec; seqNo:9235; 4058496.57 north; 591954.34 east 2021-02-13T00:03:15Z- AHRS_M2 msg: 1613174595.32 epoch sec; seqNo:9164 2021-02-13T00:03:15Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.16 psi 2021-02-13T00:03:15Z- handleDepth()-Depth_Keller msg: 1613174595337 epoch sec; seqNo:9236; depth 22.41 2021-02-13T00:03:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174595.35 epoch sec; seqNo:9236; 4058496.95 north; 591954.33 east 2021-02-13T00:03:15Z- AHRS_M2 msg: 1613174595.73 epoch sec; seqNo:9165 2021-02-13T00:03:15Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.16 psi 2021-02-13T00:03:15Z- handleDepth()-Depth_Keller msg: 1613174595737 epoch sec; seqNo:9237; depth 22.28 2021-02-13T00:03:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174595.77 epoch sec; seqNo:9237; 4058496.96 north; 591955.55 east 2021-02-13T00:03:16Z- AHRS_M2 msg: 1613174596.13 epoch sec; seqNo:9166 2021-02-13T00:03:16Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.16 psi 2021-02-13T00:03:16Z- handleDepth()-Depth_Keller msg: 1613174596149 epoch sec; seqNo:9238; depth 22.16 2021-02-13T00:03:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:16Z- handleDvl() - RDI_Pathfinder msg: 1613174596.15 epoch sec; seqNo:4479 2021-02-13T00:03:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.04 , 0.13 2021-02-13T00:03:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174596.16 epoch sec; seqNo:9238; 4058497.34 north; 591955.55 east 2021-02-13T00:03:16Z- LcmTrn::handleMessages() - handling 42 messages took 0.64 ms and 130 calls... 2021-02-13T00:03:16Z- waiting for 1613174594.30, time is 1613174596.16 2021-02-13T00:03:16Z- LcmTrn::updateTrn() equating pose 1613174596.16 and meas times 1613174596.16 2021-02-13T00:03:16Z- LcmTrn::updateTrn() >>>> pose time 1613174596.16, meas time 1613174596.16 <<<< 2021-02-13T00:03:16Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174596.16, ping # 0. 2021-02-13T00:03:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174596.16 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:03:16Z- TerrainNav::motionUpdate() - currEstimate = (4058497, 591955.55, 22.16) 2021-02-13T00:03:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:03:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:03:16Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:03:16Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:03:16Z- TNavBF::Projecting Measurements 2021-02-13T00:03:16Z- TNavFilter::Measurement from time = 1613174596.16 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:03:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:03:16Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:03:16Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:03:16Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:03:16Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:03:16Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:03:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:03:16Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 720.9 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:03:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:03:16Z- TNavBF::mmse selected from filter -1 2021-02-13T00:03:16Z- variances([0], [2], and psi_berg) = 720.881059 833.010242 0.000000 2021-02-13T00:03:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:03:16Z- LcmTrn::publishEstimates() - MLE : 4058492.84 591995.05 22.16 2021-02-13T00:03:16Z- LcmTrn::publishEstimates() - MMSE : 4058497.34 591955.55 22.16 2021-02-13T00:03:16Z- LcmTrn::publishEstimates() - COVAR: 720.88 833.01 1.25 0.00 2021-02-13T00:03:16Z- AHRS_M2 msg: 1613174596.54 epoch sec; seqNo:9167 2021-02-13T00:03:16Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.16 psi 2021-02-13T00:03:16Z- handleDepth()-Depth_Keller msg: 1613174596550 epoch sec; seqNo:9239; depth 22.05 2021-02-13T00:03:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174596.57 epoch sec; seqNo:9239; 4058497.72 north; 591955.55 east 2021-02-13T00:03:16Z- AHRS_M2 msg: 1613174596.94 epoch sec; seqNo:9168 2021-02-13T00:03:16Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.15 psi 2021-02-13T00:03:16Z- handleDepth()-Depth_Keller msg: 1613174596957 epoch sec; seqNo:9240; depth 21.92 2021-02-13T00:03:16Z- handleDvl() - RDI_Pathfinder msg: 1613174596.97 epoch sec; seqNo:4480 2021-02-13T00:03:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.79 , 90.96 , 103.13 , 67.04 2021-02-13T00:03:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , 0.10 2021-02-13T00:03:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174596.98 epoch sec; seqNo:9240; 4058497.72 north; 591955.55 east 2021-02-13T00:03:17Z- AHRS_M2 msg: 1613174597.34 epoch sec; seqNo:9169 2021-02-13T00:03:17Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.15 psi 2021-02-13T00:03:17Z- handleDepth()-Depth_Keller msg: 1613174597348 epoch sec; seqNo:9241; depth 21.78 2021-02-13T00:03:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:17Z- handleDvl() - RDI_Pathfinder msg: 1613174597.35 epoch sec; seqNo:4481 2021-02-13T00:03:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , 0.10 2021-02-13T00:03:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174597.36 epoch sec; seqNo:9241; 4058498.10 north; 591955.54 east 2021-02-13T00:03:17Z- AHRS_M2 msg: 1613174597.75 epoch sec; seqNo:9170 2021-02-13T00:03:17Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.16 psi 2021-02-13T00:03:17Z- handleDepth()-Depth_Keller msg: 1613174597753 epoch sec; seqNo:9242; depth 21.67 2021-02-13T00:03:17Z- handleDvl() - RDI_Pathfinder msg: 1613174597.76 epoch sec; seqNo:4482 2021-02-13T00:03:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 90.89 , 104.28 , 68.67 2021-02-13T00:03:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , 0.07 2021-02-13T00:03:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174597.78 epoch sec; seqNo:9242; 4058498.49 north; 591956.76 east 2021-02-13T00:03:18Z- AHRS_M2 msg: 1613174598.15 epoch sec; seqNo:9171 2021-02-13T00:03:18Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.18 psi 2021-02-13T00:03:18Z- handleDepth()-Depth_Keller msg: 1613174598163 epoch sec; seqNo:9243; depth 21.54 2021-02-13T00:03:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174598.17 epoch sec; seqNo:9243; 4058498.87 north; 591956.76 east 2021-02-13T00:03:18Z- AHRS_M2 msg: 1613174598.58 epoch sec; seqNo:9172 2021-02-13T00:03:18Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.19 psi 2021-02-13T00:03:18Z- handleDepth()-Depth_Keller msg: 1613174598585 epoch sec; seqNo:9244; depth 21.42 2021-02-13T00:03:18Z- handleDvl() - RDI_Pathfinder msg: 1613174598.60 epoch sec; seqNo:4483 2021-02-13T00:03:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 98.13 , 107.86 , 69.49 2021-02-13T00:03:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.08 , 0.03 2021-02-13T00:03:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174598.61 epoch sec; seqNo:9244; 4058498.87 north; 591956.76 east 2021-02-13T00:03:18Z- AHRS_M2 msg: 1613174598.96 epoch sec; seqNo:9173 2021-02-13T00:03:18Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.20 psi 2021-02-13T00:03:18Z- handleDepth()-Depth_Keller msg: 1613174598969 epoch sec; seqNo:9245; depth 21.29 2021-02-13T00:03:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174598.99 epoch sec; seqNo:9245; 4058499.25 north; 591956.75 east 2021-02-13T00:03:19Z- AHRS_M2 msg: 1613174599.36 epoch sec; seqNo:9174 2021-02-13T00:03:19Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.22 psi 2021-02-13T00:03:19Z- handleDepth()-Depth_Keller msg: 1613174599368 epoch sec; seqNo:9246; depth 21.19 2021-02-13T00:03:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174599.38 epoch sec; seqNo:9246; 4058499.63 north; 591956.75 east 2021-02-13T00:03:19Z- AHRS_M2 msg: 1613174599.77 epoch sec; seqNo:9175 2021-02-13T00:03:19Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.22 psi 2021-02-13T00:03:19Z- handleDepth()-Depth_Keller msg: 1613174599777 epoch sec; seqNo:9247; depth 21.05 2021-02-13T00:03:19Z- handleDvl() - RDI_Pathfinder msg: 1613174599.79 epoch sec; seqNo:4484 2021-02-13T00:03:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 97.92 , 106.44 , 68.67 2021-02-13T00:03:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.07 , -0.04 2021-02-13T00:03:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174599.80 epoch sec; seqNo:9247; 4058499.64 north; 591957.97 east 2021-02-13T00:03:20Z- AHRS_M2 msg: 1613174600.17 epoch sec; seqNo:9176 2021-02-13T00:03:20Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.22 psi 2021-02-13T00:03:20Z- handleDepth()-Depth_Keller msg: 1613174600176 epoch sec; seqNo:9248; depth 20.92 2021-02-13T00:03:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174600.19 epoch sec; seqNo:9248; 4058500.02 north; 591957.96 east 2021-02-13T00:03:20Z- AHRS_M2 msg: 1613174600.58 epoch sec; seqNo:9177 2021-02-13T00:03:20Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.22 psi 2021-02-13T00:03:20Z- handleDepth()-Depth_Keller msg: 1613174600580 epoch sec; seqNo:9249; depth 20.82 2021-02-13T00:03:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174600.59 epoch sec; seqNo:9249; 4058500.40 north; 591957.96 east 2021-02-13T00:03:20Z- AHRS_M2 msg: 1613174600.98 epoch sec; seqNo:9178 2021-02-13T00:03:20Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.22 psi 2021-02-13T00:03:20Z- handleDepth()-Depth_Keller msg: 1613174600989 epoch sec; seqNo:9250; depth 20.69 2021-02-13T00:03:21Z- handleDvl() - RDI_Pathfinder msg: 1613174601.00 epoch sec; seqNo:4485 2021-02-13T00:03:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 97.92 , 106.44 , 69.49 2021-02-13T00:03:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.05 , -0.11 2021-02-13T00:03:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174601.01 epoch sec; seqNo:9250; 4058500.40 north; 591957.96 east 2021-02-13T00:03:21Z- AHRS_M2 msg: 1613174601.38 epoch sec; seqNo:9179 2021-02-13T00:03:21Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.21 psi 2021-02-13T00:03:21Z- handleDepth()-Depth_Keller msg: 1613174601390 epoch sec; seqNo:9251; depth 20.58 2021-02-13T00:03:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174601.40 epoch sec; seqNo:9251; 4058500.78 north; 591957.96 east 2021-02-13T00:03:21Z- LcmTrn::handleMessages() - handling 45 messages took 0.62 ms and 133 calls... 2021-02-13T00:03:21Z- waiting for 1613174599.16, time is 1613174601.40 2021-02-13T00:03:21Z- LcmTrn::updateTrn() >>>> pose time 1613174601.40, meas time 1613174601.00 <<<< 2021-02-13T00:03:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174601.00 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:03:21Z- TerrainNav::motionUpdate() - currEstimate = (4058501, 591957.96, 20.58) 2021-02-13T00:03:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:03:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:03:21Z- TNavBF::Projecting Measurements 2021-02-13T00:03:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:03:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:03:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:03:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:03:21Z- TNavFilter::4 good beams. 2021-02-13T00:03:21Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:03:21Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:03:21Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:03:21Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:03:21Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:03:21Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:03:21Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:03:21Z- TNavBF::mmse selected from filter -1 2021-02-13T00:03:21Z- TerrainMapOctree: (vehN, vehE ) = (4058500.52, 591957.77). 2021-02-13T00:03:21Z- TerrainMapOctree: Min Distance = 3579.66. 2021-02-13T00:03:21Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:03:21Z- There is no specific good beam that all particles have in common. 2021-02-13T00:03:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:03:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.44 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T00:03:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:03:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:03:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:03:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 18.13 2021-02-13T00:03:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:03:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 7.73 2021-02-13T00:03:21Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:03:21Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:03:21Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:03:21Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:03:21Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:03:21Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:03:21Z- Filter 0 No beams used in subcloud update 2021-02-13T00:03:21Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:03:21Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:03:21Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 720.8 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:03:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174601.00 sec. 2021-02-13T00:03:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:03:21Z- TNavBF::mmse selected from filter -1 2021-02-13T00:03:21Z- variances([0], [2], and psi_berg) = 720.794733 833.010185 0.000000 2021-02-13T00:03:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:03:21Z- LcmTrn::publishEstimates() - MLE : 4058496.28 592003.46 20.58 2021-02-13T00:03:21Z- LcmTrn::publishEstimates() - MMSE : 4058500.78 591957.96 20.58 2021-02-13T00:03:21Z- LcmTrn::publishEstimates() - COVAR: 720.79 833.01 1.02 0.00 2021-02-13T00:03:21Z- AHRS_M2 msg: 1613174601.79 epoch sec; seqNo:9180 2021-02-13T00:03:21Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.21 psi 2021-02-13T00:03:21Z- handleDepth()-Depth_Keller msg: 1613174601797 epoch sec; seqNo:9252; depth 20.46 2021-02-13T00:03:21Z- handleDvl() - RDI_Pathfinder msg: 1613174601.81 epoch sec; seqNo:4486 2021-02-13T00:03:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 96.71 , 104.01 , 78.45 2021-02-13T00:03:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.02 , -0.11 2021-02-13T00:03:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174601.83 epoch sec; seqNo:9252; 4058501.16 north; 591957.95 east 2021-02-13T00:03:22Z- AHRS_M2 msg: 1613174602.19 epoch sec; seqNo:9181 2021-02-13T00:03:22Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.20 psi 2021-02-13T00:03:22Z- handleDepth()-Depth_Keller msg: 1613174602201 epoch sec; seqNo:9253; depth 20.34 2021-02-13T00:03:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:22Z- handleDvl() - RDI_Pathfinder msg: 1613174602.20 epoch sec; seqNo:4487 2021-02-13T00:03:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.02 , -0.11 2021-02-13T00:03:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174602.21 epoch sec; seqNo:9253; 4058501.17 north; 591959.17 east 2021-02-13T00:03:22Z- AHRS_M2 msg: 1613174602.60 epoch sec; seqNo:9182 2021-02-13T00:03:22Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.20 psi 2021-02-13T00:03:22Z- handleDepth()-Depth_Keller msg: 1613174602611 epoch sec; seqNo:9254; depth 20.23 2021-02-13T00:03:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174602.62 epoch sec; seqNo:9254; 4058501.55 north; 591959.17 east 2021-02-13T00:03:23Z- AHRS_M2 msg: 1613174603.00 epoch sec; seqNo:9183 2021-02-13T00:03:23Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.19 psi 2021-02-13T00:03:23Z- handleDepth()-Depth_Keller msg: 1613174603005 epoch sec; seqNo:9255; depth 20.12 2021-02-13T00:03:23Z- handleDvl() - RDI_Pathfinder msg: 1613174603.02 epoch sec; seqNo:4488 2021-02-13T00:03:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 97.86 , 103.94 , 78.38 2021-02-13T00:03:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.08 , -0.06 2021-02-13T00:03:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174603.04 epoch sec; seqNo:9255; 4058501.93 north; 591959.16 east 2021-02-13T00:03:23Z- AHRS_M2 msg: 1613174603.40 epoch sec; seqNo:9184 2021-02-13T00:03:23Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.19 psi 2021-02-13T00:03:23Z- handleDepth()-Depth_Keller msg: 1613174603408 epoch sec; seqNo:9256; depth 20.00 2021-02-13T00:03:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:23Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:23Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:23Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:23Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:23Z- handleDvl() - RDI_Pathfinder msg: 1613174603.41 epoch sec; seqNo:4489 2021-02-13T00:03:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.08 , -0.06 2021-02-13T00:03:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174603.42 epoch sec; seqNo:9256; 4058501.93 north; 591959.16 east 2021-02-13T00:03:23Z- AHRS_M2 msg: 1613174603.81 epoch sec; seqNo:9185 2021-02-13T00:03:23Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.19 psi 2021-02-13T00:03:23Z- handleDepth()-Depth_Keller msg: 1613174603814 epoch sec; seqNo:9257; depth 19.89 2021-02-13T00:03:23Z- handleDvl() - RDI_Pathfinder msg: 1613174603.83 epoch sec; seqNo:4490 2021-02-13T00:03:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.62 , 97.79 , 100.22 , 69.08 2021-02-13T00:03:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.10 , 0.00 2021-02-13T00:03:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174603.84 epoch sec; seqNo:9257; 4058502.31 north; 591959.16 east 2021-02-13T00:03:24Z- AHRS_M2 msg: 1613174604.21 epoch sec; seqNo:9186 2021-02-13T00:03:24Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-13T00:03:24Z- handleDepth()-Depth_Keller msg: 1613174604216 epoch sec; seqNo:9258; depth 19.77 2021-02-13T00:03:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174604.23 epoch sec; seqNo:9258; 4058502.69 north; 591959.16 east 2021-02-13T00:03:24Z- AHRS_M2 msg: 1613174604.62 epoch sec; seqNo:9187 2021-02-13T00:03:24Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-13T00:03:24Z- handleDepth()-Depth_Keller msg: 1613174604636 epoch sec; seqNo:9259; depth 19.67 2021-02-13T00:03:24Z- handleDvl() - RDI_Pathfinder msg: 1613174604.67 epoch sec; seqNo:4491 2021-02-13T00:03:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 98.13 , 104.21 , 78.65 2021-02-13T00:03:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.09 , 0.03 2021-02-13T00:03:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174604.69 epoch sec; seqNo:9259; 4058503.06 north; 591959.15 east 2021-02-13T00:03:25Z- AHRS_M2 msg: 1613174605.02 epoch sec; seqNo:9188 2021-02-13T00:03:25Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-13T00:03:25Z- handleDepth()-Depth_Keller msg: 1613174605026 epoch sec; seqNo:9260; depth 19.52 2021-02-13T00:03:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174605.04 epoch sec; seqNo:9260; 4058503.06 north; 591959.15 east 2021-02-13T00:03:25Z- AHRS_M2 msg: 1613174605.42 epoch sec; seqNo:9189 2021-02-13T00:03:25Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-13T00:03:25Z- handleDepth()-Depth_Keller msg: 1613174605428 epoch sec; seqNo:9261; depth 19.43 2021-02-13T00:03:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174605.44 epoch sec; seqNo:9261; 4058503.46 north; 591960.37 east 2021-02-13T00:03:25Z- AHRS_M2 msg: 1613174605.83 epoch sec; seqNo:9190 2021-02-13T00:03:25Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-13T00:03:25Z- handleDepth()-Depth_Keller msg: 1613174605837 epoch sec; seqNo:9262; depth 19.32 2021-02-13T00:03:25Z- handleDvl() - RDI_Pathfinder msg: 1613174605.85 epoch sec; seqNo:4492 2021-02-13T00:03:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 97.79 , 105.09 , 69.08 2021-02-13T00:03:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.11 , 0.03 2021-02-13T00:03:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174605.87 epoch sec; seqNo:9262; 4058503.84 north; 591960.36 east 2021-02-13T00:03:26Z- AHRS_M2 msg: 1613174606.23 epoch sec; seqNo:9191 2021-02-13T00:03:26Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-13T00:03:26Z- handleDepth()-Depth_Keller msg: 1613174606237 epoch sec; seqNo:9263; depth 19.20 2021-02-13T00:03:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174606.25 epoch sec; seqNo:9263; 4058504.21 north; 591960.36 east 2021-02-13T00:03:26Z- AHRS_M2 msg: 1613174606.64 epoch sec; seqNo:9192 2021-02-13T00:03:26Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-13T00:03:26Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-13T00:03:26Z- waiting for 1613174604.40, time is 1613174606.64 2021-02-13T00:03:26Z- LcmTrn::updateTrn() >>>> pose time 1613174606.64, meas time 1613174605.85 <<<< 2021-02-13T00:03:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174605.85 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:03:26Z- TerrainNav::motionUpdate() - currEstimate = (4058504, 591960.36, 19.20) 2021-02-13T00:03:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:03:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:03:26Z- TNavBF::Projecting Measurements 2021-02-13T00:03:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:03:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:03:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:03:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:03:26Z- TNavFilter::4 good beams. 2021-02-13T00:03:26Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:03:26Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:03:26Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:03:26Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:03:26Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:03:26Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:03:26Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:03:26Z- TNavBF::mmse selected from filter -1 2021-02-13T00:03:26Z- TerrainMapOctree: (vehN, vehE ) = (4058503.70, 591960.00). 2021-02-13T00:03:26Z- TerrainMapOctree: Min Distance = 3575.79. 2021-02-13T00:03:26Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:03:26Z- There is no specific good beam that all particles have in common. 2021-02-13T00:03:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:03:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 9.81 2021-02-13T00:03:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:03:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-13T00:03:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:03:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 17.67 2021-02-13T00:03:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:03:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 7.64 2021-02-13T00:03:26Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:03:26Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:03:26Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:03:26Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:03:26Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:03:26Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:03:26Z- Filter 0 No beams used in subcloud update 2021-02-13T00:03:26Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:03:26Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:03:26Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 720.7 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:03:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174605.85 sec. 2021-02-13T00:03:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:03:26Z- TNavBF::mmse selected from filter -1 2021-02-13T00:03:26Z- variances([0], [2], and psi_berg) = 720.703481 833.010125 0.000000 2021-02-13T00:03:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:03:26Z- LcmTrn::publishEstimates() - MLE : 4058499.71 592005.86 19.20 2021-02-13T00:03:26Z- LcmTrn::publishEstimates() - MMSE : 4058504.21 591960.36 19.20 2021-02-13T00:03:26Z- LcmTrn::publishEstimates() - COVAR: 720.70 833.01 0.91 0.00 2021-02-13T00:03:26Z- handleDepth()-Depth_Keller msg: 1613174606649 epoch sec; seqNo:9264; depth 19.10 2021-02-13T00:03:26Z- handleDvl() - RDI_Pathfinder msg: 1613174606.66 epoch sec; seqNo:4493 2021-02-13T00:03:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.26 , 95.56 , 102.86 , 71.12 2021-02-13T00:03:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.11 , 0.03 2021-02-13T00:03:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174606.67 epoch sec; seqNo:9264; 4058504.21 north; 591960.36 east 2021-02-13T00:03:27Z- AHRS_M2 msg: 1613174607.04 epoch sec; seqNo:9193 2021-02-13T00:03:27Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-13T00:03:27Z- handleDepth()-Depth_Keller msg: 1613174607049 epoch sec; seqNo:9265; depth 18.97 2021-02-13T00:03:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:27Z- handleDvl() - RDI_Pathfinder msg: 1613174607.05 epoch sec; seqNo:4494 2021-02-13T00:03:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.11 , 0.03 2021-02-13T00:03:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174607.07 epoch sec; seqNo:9265; 4058504.59 north; 591960.36 east 2021-02-13T00:03:27Z- AHRS_M2 msg: 1613174607.44 epoch sec; seqNo:9194 2021-02-13T00:03:27Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-13T00:03:27Z- handleDepth()-Depth_Keller msg: 1613174607448 epoch sec; seqNo:9266; depth 18.85 2021-02-13T00:03:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174607.46 epoch sec; seqNo:9266; 4058504.97 north; 591960.35 east 2021-02-13T00:03:27Z- AHRS_M2 msg: 1613174607.85 epoch sec; seqNo:9195 2021-02-13T00:03:27Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.18 psi 2021-02-13T00:03:27Z- handleDepth()-Depth_Keller msg: 1613174607852 epoch sec; seqNo:9267; depth 18.75 2021-02-13T00:03:27Z- handleDvl() - RDI_Pathfinder msg: 1613174607.86 epoch sec; seqNo:4495 2021-02-13T00:03:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.06 , 105.36 , 78.58 2021-02-13T00:03:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.10 , 0.06 2021-02-13T00:03:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174607.87 epoch sec; seqNo:9267; 4058505.35 north; 591960.35 east 2021-02-13T00:03:28Z- AHRS_M2 msg: 1613174608.25 epoch sec; seqNo:9196 2021-02-13T00:03:28Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.18 psi 2021-02-13T00:03:28Z- handleDepth()-Depth_Keller msg: 1613174608258 epoch sec; seqNo:9268; depth 18.64 2021-02-13T00:03:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:28Z- handleDvl() - RDI_Pathfinder msg: 1613174608.26 epoch sec; seqNo:4496 2021-02-13T00:03:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.10 , 0.06 2021-02-13T00:03:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174608.27 epoch sec; seqNo:9268; 4058505.35 north; 591960.35 east 2021-02-13T00:03:28Z- AHRS_M2 msg: 1613174608.66 epoch sec; seqNo:9197 2021-02-13T00:03:28Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:03:28Z- handleDepth()-Depth_Keller msg: 1613174608667 epoch sec; seqNo:9269; depth 18.53 2021-02-13T00:03:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174608.71 epoch sec; seqNo:9269; 4058505.74 north; 591961.57 east 2021-02-13T00:03:29Z- AHRS_M2 msg: 1613174609.06 epoch sec; seqNo:9198 2021-02-13T00:03:29Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:03:29Z- handleDepth()-Depth_Keller msg: 1613174609064 epoch sec; seqNo:9270; depth 18.41 2021-02-13T00:03:29Z- handleDvl() - RDI_Pathfinder msg: 1613174609.08 epoch sec; seqNo:4497 2021-02-13T00:03:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 95.35 , 105.09 , 78.31 2021-02-13T00:03:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.09 , 0.10 2021-02-13T00:03:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174609.10 epoch sec; seqNo:9270; 4058506.12 north; 591961.56 east 2021-02-13T00:03:29Z- AHRS_M2 msg: 1613174609.46 epoch sec; seqNo:9199 2021-02-13T00:03:29Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T00:03:29Z- handleDepth()-Depth_Keller msg: 1613174609477 epoch sec; seqNo:9271; depth 18.32 2021-02-13T00:03:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:29Z- handleDvl() - RDI_Pathfinder msg: 1613174609.48 epoch sec; seqNo:4498 2021-02-13T00:03:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.09 , 0.10 2021-02-13T00:03:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174609.49 epoch sec; seqNo:9271; 4058506.50 north; 591961.56 east 2021-02-13T00:03:29Z- AHRS_M2 msg: 1613174609.87 epoch sec; seqNo:9200 2021-02-13T00:03:29Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.19 psi 2021-02-13T00:03:29Z- handleDepth()-Depth_Keller msg: 1613174609872 epoch sec; seqNo:9272; depth 18.20 2021-02-13T00:03:29Z- handleDvl() - RDI_Pathfinder msg: 1613174609.89 epoch sec; seqNo:4499 2021-02-13T00:03:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 97.86 , 106.38 , 78.38 2021-02-13T00:03:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , 0.14 2021-02-13T00:03:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174609.91 epoch sec; seqNo:9272; 4058506.88 north; 591961.55 east 2021-02-13T00:03:30Z- AHRS_M2 msg: 1613174610.27 epoch sec; seqNo:9201 2021-02-13T00:03:30Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.19 psi 2021-02-13T00:03:30Z- handleDepth()-Depth_Keller msg: 1613174610276 epoch sec; seqNo:9273; depth 18.08 2021-02-13T00:03:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174610.29 epoch sec; seqNo:9273; 4058506.88 north; 591961.55 east 2021-02-13T00:03:30Z- AHRS_M2 msg: 1613174610.68 epoch sec; seqNo:9202 2021-02-13T00:03:30Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.20 psi 2021-02-13T00:03:30Z- handleDepth()-Depth_Keller msg: 1613174610690 epoch sec; seqNo:9274; depth 17.98 2021-02-13T00:03:30Z- handleDvl() - RDI_Pathfinder msg: 1613174610.71 epoch sec; seqNo:4500 2021-02-13T00:03:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.25 , 97.71 , 107.45 , 78.24 2021-02-13T00:03:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , 0.12 2021-02-13T00:03:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174610.72 epoch sec; seqNo:9274; 4058507.26 north; 591961.55 east 2021-02-13T00:03:31Z- AHRS_M2 msg: 1613174611.09 epoch sec; seqNo:9203 2021-02-13T00:03:31Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.19 psi 2021-02-13T00:03:31Z- handleDepth()-Depth_Keller msg: 1613174611091 epoch sec; seqNo:9275; depth 17.86 2021-02-13T00:03:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174611.10 epoch sec; seqNo:9275; 4058507.64 north; 591961.54 east 2021-02-13T00:03:31Z- AHRS_M2 msg: 1613174611.49 epoch sec; seqNo:9204 2021-02-13T00:03:31Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.19 psi 2021-02-13T00:03:31Z- handleDepth()-Depth_Keller msg: 1613174611498 epoch sec; seqNo:9276; depth 17.75 2021-02-13T00:03:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174611.51 epoch sec; seqNo:9276; 4058508.03 north; 591962.76 east 2021-02-13T00:03:31Z- LcmTrn::handleMessages() - handling 46 messages took 0.63 ms and 134 calls... 2021-02-13T00:03:31Z- waiting for 1613174609.64, time is 1613174611.51 2021-02-13T00:03:31Z- LcmTrn::updateTrn() >>>> pose time 1613174611.51, meas time 1613174610.71 <<<< 2021-02-13T00:03:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174610.71 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:03:31Z- TerrainNav::motionUpdate() - currEstimate = (4058508, 591962.76, 17.75) 2021-02-13T00:03:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:03:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:03:31Z- TNavBF::Projecting Measurements 2021-02-13T00:03:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:03:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:03:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:03:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:03:31Z- TNavFilter::4 good beams. 2021-02-13T00:03:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:03:31Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:03:31Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:03:31Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:03:31Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:03:31Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:03:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:03:31Z- TNavBF::mmse selected from filter -1 2021-02-13T00:03:31Z- TerrainMapOctree: (vehN, vehE ) = (4058507.40, 591962.37). 2021-02-13T00:03:31Z- TerrainMapOctree: Min Distance = 3571.43. 2021-02-13T00:03:31Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:03:31Z- There is no specific good beam that all particles have in common. 2021-02-13T00:03:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:03:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 9.55 2021-02-13T00:03:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:03:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:03:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:03:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 18.47 2021-02-13T00:03:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:03:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 9.79 2021-02-13T00:03:31Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:03:31Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:03:31Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:03:31Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:03:31Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:03:31Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:03:31Z- Filter 0 No beams used in subcloud update 2021-02-13T00:03:31Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:03:31Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:03:31Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 720.6 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:03:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174610.71 sec. 2021-02-13T00:03:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:03:31Z- TNavBF::mmse selected from filter -1 2021-02-13T00:03:31Z- variances([0], [2], and psi_berg) = 720.608273 833.010062 0.000000 2021-02-13T00:03:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:03:31Z- LcmTrn::publishEstimates() - MLE : 4058503.53 592008.26 17.75 2021-02-13T00:03:31Z- LcmTrn::publishEstimates() - MMSE : 4058508.03 591962.76 17.75 2021-02-13T00:03:31Z- LcmTrn::publishEstimates() - COVAR: 720.61 833.01 0.88 0.00 2021-02-13T00:03:31Z- AHRS_M2 msg: 1613174611.89 epoch sec; seqNo:9205 2021-02-13T00:03:31Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.18 psi 2021-02-13T00:03:31Z- handleDepth()-Depth_Keller msg: 1613174611892 epoch sec; seqNo:9277; depth 17.65 2021-02-13T00:03:31Z- handleDvl() - RDI_Pathfinder msg: 1613174611.90 epoch sec; seqNo:4501 2021-02-13T00:03:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 97.78 , 107.52 , 78.31 2021-02-13T00:03:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , 0.06 2021-02-13T00:03:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174611.93 epoch sec; seqNo:9277; 4058508.03 north; 591962.76 east 2021-02-13T00:03:32Z- AHRS_M2 msg: 1613174612.29 epoch sec; seqNo:9206 2021-02-13T00:03:32Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-13T00:03:32Z- handleDepth()-Depth_Keller msg: 1613174612296 epoch sec; seqNo:9278; depth 17.52 2021-02-13T00:03:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174612.31 epoch sec; seqNo:9278; 4058508.41 north; 591962.76 east 2021-02-13T00:03:32Z- AHRS_M2 msg: 1613174612.70 epoch sec; seqNo:9207 2021-02-13T00:03:32Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-13T00:03:32Z- handleDepth()-Depth_Keller msg: 1613174612711 epoch sec; seqNo:9279; depth 17.41 2021-02-13T00:03:32Z- handleDvl() - RDI_Pathfinder msg: 1613174612.72 epoch sec; seqNo:4502 2021-02-13T00:03:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 97.65 , 109.83 , 73.58 2021-02-13T00:03:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.03 , -0.04 2021-02-13T00:03:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174612.74 epoch sec; seqNo:9279; 4058508.79 north; 591962.75 east 2021-02-13T00:03:33Z- AHRS_M2 msg: 1613174613.10 epoch sec; seqNo:9208 2021-02-13T00:03:33Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-13T00:03:33Z- handleDepth()-Depth_Keller msg: 1613174613109 epoch sec; seqNo:9280; depth 17.30 2021-02-13T00:03:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:33Z- handleDvl() - RDI_Pathfinder msg: 1613174613.11 epoch sec; seqNo:4503 2021-02-13T00:03:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.03 , -0.04 2021-02-13T00:03:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174613.13 epoch sec; seqNo:9280; 4058509.17 north; 591962.75 east 2021-02-13T00:03:33Z- AHRS_M2 msg: 1613174613.50 epoch sec; seqNo:9209 2021-02-13T00:03:33Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-13T00:03:33Z- handleDepth()-Depth_Keller msg: 1613174613508 epoch sec; seqNo:9281; depth 17.19 2021-02-13T00:03:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174613.52 epoch sec; seqNo:9281; 4058509.17 north; 591962.75 east 2021-02-13T00:03:33Z- AHRS_M2 msg: 1613174613.91 epoch sec; seqNo:9210 2021-02-13T00:03:33Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-13T00:03:33Z- handleDepth()-Depth_Keller msg: 1613174613912 epoch sec; seqNo:9282; depth 17.08 2021-02-13T00:03:33Z- handleDvl() - RDI_Pathfinder msg: 1613174613.92 epoch sec; seqNo:4504 2021-02-13T00:03:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 97.72 , 107.46 , 71.53 2021-02-13T00:03:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.08 2021-02-13T00:03:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174613.93 epoch sec; seqNo:9282; 4058509.56 north; 591963.97 east 2021-02-13T00:03:34Z- AHRS_M2 msg: 1613174614.31 epoch sec; seqNo:9211 2021-02-13T00:03:34Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-13T00:03:34Z- handleDepth()-Depth_Keller msg: 1613174614330 epoch sec; seqNo:9283; depth 16.96 2021-02-13T00:03:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:34Z- handleDvl() - RDI_Pathfinder msg: 1613174614.33 epoch sec; seqNo:4505 2021-02-13T00:03:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.08 2021-02-13T00:03:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174614.34 epoch sec; seqNo:9283; 4058509.94 north; 591963.96 east 2021-02-13T00:03:34Z- AHRS_M2 msg: 1613174614.72 epoch sec; seqNo:9212 2021-02-13T00:03:34Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-13T00:03:34Z- handleDepth()-Depth_Keller msg: 1613174614721 epoch sec; seqNo:9284; depth 16.84 2021-02-13T00:03:34Z- handleDvl() - RDI_Pathfinder msg: 1613174614.73 epoch sec; seqNo:4506 2021-02-13T00:03:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.03 , 97.86 , 108.81 , 73.99 2021-02-13T00:03:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , -0.09 2021-02-13T00:03:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174614.76 epoch sec; seqNo:9284; 4058510.31 north; 591963.96 east 2021-02-13T00:03:35Z- AHRS_M2 msg: 1613174615.12 epoch sec; seqNo:9213 2021-02-13T00:03:35Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-13T00:03:35Z- handleDepth()-Depth_Keller msg: 1613174615127 epoch sec; seqNo:9285; depth 16.74 2021-02-13T00:03:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174615.14 epoch sec; seqNo:9285; 4058510.31 north; 591963.96 east 2021-02-13T00:03:35Z- AHRS_M2 msg: 1613174615.53 epoch sec; seqNo:9214 2021-02-13T00:03:35Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.13 psi 2021-02-13T00:03:35Z- handleDepth()-Depth_Keller msg: 1613174615537 epoch sec; seqNo:9286; depth 16.61 2021-02-13T00:03:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:35Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:35Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:35Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:35Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:35Z- handleDvl() - RDI_Pathfinder msg: 1613174615.54 epoch sec; seqNo:4507 2021-02-13T00:03:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , -0.09 2021-02-13T00:03:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174615.55 epoch sec; seqNo:9286; 4058510.69 north; 591963.95 east 2021-02-13T00:03:35Z- AHRS_M2 msg: 1613174615.93 epoch sec; seqNo:9215 2021-02-13T00:03:35Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.13 psi 2021-02-13T00:03:35Z- handleDepth()-Depth_Keller msg: 1613174615931 epoch sec; seqNo:9287; depth 16.50 2021-02-13T00:03:35Z- handleDvl() - RDI_Pathfinder msg: 1613174615.95 epoch sec; seqNo:4508 2021-02-13T00:03:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.90 , 97.72 , 107.46 , 78.24 2021-02-13T00:03:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.09 , -0.04 2021-02-13T00:03:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174615.96 epoch sec; seqNo:9287; 4058511.07 north; 591963.95 east 2021-02-13T00:03:36Z- AHRS_M2 msg: 1613174616.33 epoch sec; seqNo:9216 2021-02-13T00:03:36Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.13 psi 2021-02-13T00:03:36Z- handleDepth()-Depth_Keller msg: 1613174616345 epoch sec; seqNo:9288; depth 16.39 2021-02-13T00:03:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174616.36 epoch sec; seqNo:9288; 4058511.07 north; 591963.95 east 2021-02-13T00:03:36Z- AHRS_M2 msg: 1613174616.74 epoch sec; seqNo:9217 2021-02-13T00:03:36Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.13 psi 2021-02-13T00:03:36Z- handleDepth()-Depth_Keller msg: 1613174616744 epoch sec; seqNo:9289; depth 16.28 2021-02-13T00:03:36Z- handleDvl() - RDI_Pathfinder msg: 1613174616.76 epoch sec; seqNo:4509 2021-02-13T00:03:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.83 , 97.65 , 108.61 , 78.18 2021-02-13T00:03:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.11 , 0.02 2021-02-13T00:03:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174616.77 epoch sec; seqNo:9289; 4058511.46 north; 591965.17 east 2021-02-13T00:03:36Z- LcmTrn::handleMessages() - handling 48 messages took 0.68 ms and 135 calls... 2021-02-13T00:03:36Z- waiting for 1613174614.51, time is 1613174616.77 2021-02-13T00:03:36Z- LcmTrn::updateTrn() equating pose 1613174616.77 and meas times 1613174616.77 2021-02-13T00:03:36Z- LcmTrn::updateTrn() >>>> pose time 1613174616.77, meas time 1613174616.77 <<<< 2021-02-13T00:03:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174616.77 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:03:36Z- TerrainNav::motionUpdate() - currEstimate = (4058511, 591965.17, 16.28) 2021-02-13T00:03:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:03:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:03:36Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174616.77 sec.; No relevant navigation data available 2021-02-13T00:03:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:03:36Z- TNavBF::mmse selected from filter -1 2021-02-13T00:03:36Z- variances([0], [2], and psi_berg) = 720.538722 833.010017 0.000000 2021-02-13T00:03:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:03:36Z- LcmTrn::publishEstimates() - MLE : 4058506.96 592010.67 16.28 2021-02-13T00:03:36Z- LcmTrn::publishEstimates() - MMSE : 4058511.46 591965.17 16.28 2021-02-13T00:03:36Z- LcmTrn::publishEstimates() - COVAR: 720.54 833.01 1.09 0.00 2021-02-13T00:03:37Z- AHRS_M2 msg: 1613174617.14 epoch sec; seqNo:9218 2021-02-13T00:03:37Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-13T00:03:37Z- handleDepth()-Depth_Keller msg: 1613174617149 epoch sec; seqNo:9290; depth 16.15 2021-02-13T00:03:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174617.16 epoch sec; seqNo:9290; 4058511.84 north; 591965.16 east 2021-02-13T00:03:37Z- AHRS_M2 msg: 1613174617.54 epoch sec; seqNo:9219 2021-02-13T00:03:37Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-13T00:03:37Z- handleDepth()-Depth_Keller msg: 1613174617548 epoch sec; seqNo:9291; depth 16.04 2021-02-13T00:03:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174617.56 epoch sec; seqNo:9291; 4058512.22 north; 591965.16 east 2021-02-13T00:03:37Z- AHRS_M2 msg: 1613174617.95 epoch sec; seqNo:9220 2021-02-13T00:03:37Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-13T00:03:37Z- handleDepth()-Depth_Keller msg: 1613174617957 epoch sec; seqNo:9292; depth 15.93 2021-02-13T00:03:37Z- handleDvl() - RDI_Pathfinder msg: 1613174617.98 epoch sec; seqNo:4510 2021-02-13T00:03:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.83 , 97.65 , 108.61 , 78.18 2021-02-13T00:03:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.12 , 0.05 2021-02-13T00:03:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174617.98 epoch sec; seqNo:9292; 4058512.22 north; 591965.16 east 2021-02-13T00:03:38Z- AHRS_M2 msg: 1613174618.38 epoch sec; seqNo:9221 2021-02-13T00:03:38Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-13T00:03:38Z- handleDepth()-Depth_Keller msg: 1613174618382 epoch sec; seqNo:9293; depth 15.81 2021-02-13T00:03:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174618.41 epoch sec; seqNo:9293; 4058512.60 north; 591965.15 east 2021-02-13T00:03:38Z- AHRS_M2 msg: 1613174618.76 epoch sec; seqNo:9222 2021-02-13T00:03:38Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-13T00:03:38Z- handleDepth()-Depth_Keller msg: 1613174618762 epoch sec; seqNo:9294; depth 15.69 2021-02-13T00:03:38Z- handleDvl() - RDI_Pathfinder msg: 1613174618.77 epoch sec; seqNo:4511 2021-02-13T00:03:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 96.43 , 109.82 , 73.98 2021-02-13T00:03:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.10 , 0.04 2021-02-13T00:03:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174618.78 epoch sec; seqNo:9294; 4058512.98 north; 591965.15 east 2021-02-13T00:03:39Z- AHRS_M2 msg: 1613174619.16 epoch sec; seqNo:9223 2021-02-13T00:03:39Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-13T00:03:39Z- handleDepth()-Depth_Keller msg: 1613174619165 epoch sec; seqNo:9295; depth 15.57 2021-02-13T00:03:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:39Z- handleDvl() - RDI_Pathfinder msg: 1613174619.17 epoch sec; seqNo:4512 2021-02-13T00:03:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.10 , 0.04 2021-02-13T00:03:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174619.20 epoch sec; seqNo:9295; 4058513.36 north; 591965.15 east 2021-02-13T00:03:39Z- AHRS_M2 msg: 1613174619.56 epoch sec; seqNo:9224 2021-02-13T00:03:39Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-13T00:03:39Z- handleDepth()-Depth_Keller msg: 1613174619568 epoch sec; seqNo:9296; depth 15.48 2021-02-13T00:03:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174619.58 epoch sec; seqNo:9296; 4058513.37 north; 591966.37 east 2021-02-13T00:03:39Z- AHRS_M2 msg: 1613174619.97 epoch sec; seqNo:9225 2021-02-13T00:03:39Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-13T00:03:39Z- handleDepth()-Depth_Keller msg: 1613174619973 epoch sec; seqNo:9297; depth 15.36 2021-02-13T00:03:39Z- handleDvl() - RDI_Pathfinder msg: 1613174619.98 epoch sec; seqNo:4513 2021-02-13T00:03:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 97.71 , 109.89 , 73.57 2021-02-13T00:03:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , -0.01 2021-02-13T00:03:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174619.99 epoch sec; seqNo:9297; 4058513.75 north; 591966.36 east 2021-02-13T00:03:40Z- AHRS_M2 msg: 1613174620.37 epoch sec; seqNo:9226 2021-02-13T00:03:40Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-13T00:03:40Z- handleDepth()-Depth_Keller msg: 1613174620387 epoch sec; seqNo:9298; depth 15.24 2021-02-13T00:03:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:40Z- handleDvl() - RDI_Pathfinder msg: 1613174620.39 epoch sec; seqNo:4514 2021-02-13T00:03:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , -0.01 2021-02-13T00:03:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174620.40 epoch sec; seqNo:9298; 4058514.13 north; 591966.36 east 2021-02-13T00:03:40Z- AHRS_M2 msg: 1613174620.78 epoch sec; seqNo:9227 2021-02-13T00:03:40Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-13T00:03:40Z- handleDepth()-Depth_Keller msg: 1613174620785 epoch sec; seqNo:9299; depth 15.15 2021-02-13T00:03:40Z- handleDvl() - RDI_Pathfinder msg: 1613174620.80 epoch sec; seqNo:4515 2021-02-13T00:03:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 97.71 , 111.10 , 78.24 2021-02-13T00:03:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.10 , -0.06 2021-02-13T00:03:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174620.81 epoch sec; seqNo:9299; 4058514.51 north; 591966.35 east 2021-02-13T00:03:41Z- AHRS_M2 msg: 1613174621.18 epoch sec; seqNo:9228 2021-02-13T00:03:41Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.15 psi 2021-02-13T00:03:41Z- handleDepth()-Depth_Keller msg: 1613174621186 epoch sec; seqNo:9300; depth 15.04 2021-02-13T00:03:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174621.20 epoch sec; seqNo:9300; 4058514.51 north; 591966.35 east 2021-02-13T00:03:41Z- AHRS_M2 msg: 1613174621.61 epoch sec; seqNo:9229 2021-02-13T00:03:41Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-13T00:03:41Z- handleDepth()-Depth_Keller msg: 1613174621617 epoch sec; seqNo:9301; depth 14.93 2021-02-13T00:03:41Z- handleDvl() - RDI_Pathfinder msg: 1613174621.63 epoch sec; seqNo:4516 2021-02-13T00:03:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 96.29 , 109.82 , 74.80 2021-02-13T00:03:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.13 , -0.05 2021-02-13T00:03:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174621.64 epoch sec; seqNo:9301; 4058514.89 north; 591966.35 east 2021-02-13T00:03:41Z- LcmTrn::handleMessages() - handling 43 messages took 0.60 ms and 131 calls... 2021-02-13T00:03:41Z- waiting for 1613174619.77, time is 1613174621.64 2021-02-13T00:03:41Z- LcmTrn::updateTrn() equating pose 1613174621.64 and meas times 1613174621.64 2021-02-13T00:03:41Z- LcmTrn::updateTrn() >>>> pose time 1613174621.64, meas time 1613174621.64 <<<< 2021-02-13T00:03:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174621.64 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:03:41Z- TerrainNav::motionUpdate() - currEstimate = (4058515, 591966.35, 14.93) 2021-02-13T00:03:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:03:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:03:41Z- TNavBF::Projecting Measurements 2021-02-13T00:03:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:03:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:03:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:03:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:03:41Z- TNavFilter::4 good beams. 2021-02-13T00:03:41Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:03:41Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:03:41Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:03:41Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:03:41Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:03:41Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:03:41Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:03:41Z- TNavBF::mmse selected from filter -1 2021-02-13T00:03:41Z- TerrainMapOctree: (vehN, vehE ) = (4058514.89, 591966.35). 2021-02-13T00:03:41Z- TerrainMapOctree: Min Distance = 3563.13. 2021-02-13T00:03:41Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:03:41Z- There is no specific good beam that all particles have in common. 2021-02-13T00:03:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:03:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 11.32 2021-02-13T00:03:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:03:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 14.84 2021-02-13T00:03:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:03:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 19.30 2021-02-13T00:03:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:03:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 8.95 2021-02-13T00:03:41Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:03:41Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:03:41Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:03:41Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:03:41Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:03:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:03:41Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:03:41Z- Filter 0 No beams used in subcloud update 2021-02-13T00:03:41Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:03:41Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:03:41Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 720.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:03:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174621.64 sec. 2021-02-13T00:03:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:03:41Z- TNavBF::mmse selected from filter -1 2021-02-13T00:03:41Z- variances([0], [2], and psi_berg) = 720.474140 833.009974 0.000000 2021-02-13T00:03:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:03:41Z- LcmTrn::publishEstimates() - MLE : 4058510.39 592011.85 14.93 2021-02-13T00:03:41Z- LcmTrn::publishEstimates() - MMSE : 4058514.89 591966.35 14.93 2021-02-13T00:03:41Z- LcmTrn::publishEstimates() - COVAR: 720.47 833.01 0.94 0.00 2021-02-13T00:03:41Z- AHRS_M2 msg: 1613174621.99 epoch sec; seqNo:9230 2021-02-13T00:03:41Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-13T00:03:41Z- handleDepth()-Depth_Keller msg: 1613174621997 epoch sec; seqNo:9302; depth 14.81 2021-02-13T00:03:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174622.01 epoch sec; seqNo:9302; 4058515.27 north; 591966.35 east 2021-02-13T00:03:42Z- AHRS_M2 msg: 1613174622.39 epoch sec; seqNo:9231 2021-02-13T00:03:42Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.14 psi 2021-02-13T00:03:42Z- handleDepth()-Depth_Keller msg: 1613174622397 epoch sec; seqNo:9303; depth 14.72 2021-02-13T00:03:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174622.41 epoch sec; seqNo:9303; 4058515.64 north; 591966.34 east 2021-02-13T00:03:42Z- AHRS_M2 msg: 1613174622.80 epoch sec; seqNo:9232 2021-02-13T00:03:42Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.13 psi 2021-02-13T00:03:42Z- handleDepth()-Depth_Keller msg: 1613174622812 epoch sec; seqNo:9304; depth 14.59 2021-02-13T00:03:42Z- handleDvl() - RDI_Pathfinder msg: 1613174622.82 epoch sec; seqNo:4517 2021-02-13T00:03:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.43 , 108.60 , 75.21 2021-02-13T00:03:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.14 , 0.03 2021-02-13T00:03:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174622.83 epoch sec; seqNo:9304; 4058515.66 north; 591967.56 east 2021-02-13T00:03:43Z- AHRS_M2 msg: 1613174623.20 epoch sec; seqNo:9233 2021-02-13T00:03:43Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.13 psi 2021-02-13T00:03:43Z- handleDepth()-Depth_Keller msg: 1613174623204 epoch sec; seqNo:9305; depth 14.48 2021-02-13T00:03:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174623.22 epoch sec; seqNo:9305; 4058516.04 north; 591967.56 east 2021-02-13T00:03:43Z- AHRS_M2 msg: 1613174623.62 epoch sec; seqNo:9234 2021-02-13T00:03:43Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.12 psi 2021-02-13T00:03:43Z- handleDepth()-Depth_Keller msg: 1613174623628 epoch sec; seqNo:9306; depth 14.37 2021-02-13T00:03:43Z- handleDvl() - RDI_Pathfinder msg: 1613174623.65 epoch sec; seqNo:4518 2021-02-13T00:03:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 96.23 , 108.40 , 0.00 2021-02-13T00:03:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.09 , 0.12 2021-02-13T00:03:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174623.66 epoch sec; seqNo:9306; 4058516.42 north; 591967.56 east 2021-02-13T00:03:44Z- AHRS_M2 msg: 1613174624.01 epoch sec; seqNo:9235 2021-02-13T00:03:44Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.12 psi 2021-02-13T00:03:44Z- handleDepth()-Depth_Keller msg: 1613174624016 epoch sec; seqNo:9307; depth 14.28 2021-02-13T00:03:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:44Z- handleDvl() - RDI_Pathfinder msg: 1613174624.02 epoch sec; seqNo:4519 2021-02-13T00:03:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.09 , 0.12 2021-02-13T00:03:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174624.04 epoch sec; seqNo:9307; 4058516.79 north; 591967.55 east 2021-02-13T00:03:44Z- AHRS_M2 msg: 1613174624.42 epoch sec; seqNo:9236 2021-02-13T00:03:44Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.13 psi 2021-02-13T00:03:44Z- handleDepth()-Depth_Keller msg: 1613174624427 epoch sec; seqNo:9308; depth 14.15 2021-02-13T00:03:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174624.44 epoch sec; seqNo:9308; 4058516.79 north; 591967.55 east 2021-02-13T00:03:44Z- AHRS_M2 msg: 1613174624.82 epoch sec; seqNo:9237 2021-02-13T00:03:44Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.13 psi 2021-02-13T00:03:44Z- handleDepth()-Depth_Keller msg: 1613174624820 epoch sec; seqNo:9309; depth 14.05 2021-02-13T00:03:44Z- handleDvl() - RDI_Pathfinder msg: 1613174624.84 epoch sec; seqNo:4520 2021-02-13T00:03:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.85 , 97.38 , 109.55 , 73.98 2021-02-13T00:03:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.15 2021-02-13T00:03:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174624.85 epoch sec; seqNo:9309; 4058517.17 north; 591967.55 east 2021-02-13T00:03:45Z- AHRS_M2 msg: 1613174625.22 epoch sec; seqNo:9238 2021-02-13T00:03:45Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.13 psi 2021-02-13T00:03:45Z- handleDepth()-Depth_Keller msg: 1613174625225 epoch sec; seqNo:9310; depth 13.94 2021-02-13T00:03:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:45Z- handleDvl() - RDI_Pathfinder msg: 1613174625.23 epoch sec; seqNo:4521 2021-02-13T00:03:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.15 2021-02-13T00:03:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174625.24 epoch sec; seqNo:9310; 4058517.55 north; 591967.54 east 2021-02-13T00:03:45Z- AHRS_M2 msg: 1613174625.64 epoch sec; seqNo:9239 2021-02-13T00:03:45Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-13T00:03:45Z- handleDepth()-Depth_Keller msg: 1613174625649 epoch sec; seqNo:9311; depth 13.81 2021-02-13T00:03:45Z- handleDvl() - RDI_Pathfinder msg: 1613174625.66 epoch sec; seqNo:4522 2021-02-13T00:03:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:45Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:03:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174625.68 epoch sec; seqNo:9311; 4058517.94 north; 591968.76 east 2021-02-13T00:03:46Z- AHRS_M2 msg: 1613174626.03 epoch sec; seqNo:9240 2021-02-13T00:03:46Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.15 psi 2021-02-13T00:03:46Z- handleDepth()-Depth_Keller msg: 1613174626038 epoch sec; seqNo:9312; depth 13.70 2021-02-13T00:03:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174626.05 epoch sec; seqNo:9312; 4058517.94 north; 591968.76 east 2021-02-13T00:03:46Z- AHRS_M2 msg: 1613174626.43 epoch sec; seqNo:9241 2021-02-13T00:03:46Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.16 psi 2021-02-13T00:03:46Z- handleDepth()-Depth_Keller msg: 1613174626445 epoch sec; seqNo:9313; depth 13.60 2021-02-13T00:03:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:46Z- handleDvl() - RDI_Pathfinder msg: 1613174626.45 epoch sec; seqNo:4523 2021-02-13T00:03:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:46Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:03:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174626.46 epoch sec; seqNo:9313; 4058518.32 north; 591968.76 east 2021-02-13T00:03:46Z- AHRS_M2 msg: 1613174626.84 epoch sec; seqNo:9242 2021-02-13T00:03:46Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.17 psi 2021-02-13T00:03:46Z- handleDepth()-Depth_Keller msg: 1613174626841 epoch sec; seqNo:9314; depth 13.50 2021-02-13T00:03:46Z- handleDvl() - RDI_Pathfinder msg: 1613174626.85 epoch sec; seqNo:4524 2021-02-13T00:03:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 98.80 , 110.70 , 77.97 2021-02-13T00:03:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.03 , 0.06 2021-02-13T00:03:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174626.88 epoch sec; seqNo:9314; 4058518.70 north; 591968.75 east 2021-02-13T00:03:46Z- LcmTrn::handleMessages() - handling 47 messages took 0.65 ms and 134 calls... 2021-02-13T00:03:46Z- waiting for 1613174624.64, time is 1613174626.88 2021-02-13T00:03:46Z- LcmTrn::updateTrn() equating pose 1613174626.88 and meas times 1613174626.88 2021-02-13T00:03:46Z- LcmTrn::updateTrn() >>>> pose time 1613174626.88, meas time 1613174626.88 <<<< 2021-02-13T00:03:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174626.88 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:03:46Z- TerrainNav::motionUpdate() - currEstimate = (4058519, 591968.75, 13.50) 2021-02-13T00:03:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:03:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:03:46Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174626.88 sec.; No relevant navigation data available 2021-02-13T00:03:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:03:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:03:46Z- variances([0], [2], and psi_berg) = 720.402146 833.009927 0.000000 2021-02-13T00:03:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:03:46Z- LcmTrn::publishEstimates() - MLE : 4058515.20 591920.25 13.50 2021-02-13T00:03:46Z- LcmTrn::publishEstimates() - MMSE : 4058518.70 591968.75 13.50 2021-02-13T00:03:46Z- LcmTrn::publishEstimates() - COVAR: 720.40 833.01 1.17 0.00 2021-02-13T00:03:47Z- AHRS_M2 msg: 1613174627.24 epoch sec; seqNo:9243 2021-02-13T00:03:47Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.17 psi 2021-02-13T00:03:47Z- handleDepth()-Depth_Keller msg: 1613174627245 epoch sec; seqNo:9315; depth 13.38 2021-02-13T00:03:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174627.26 epoch sec; seqNo:9315; 4058519.08 north; 591968.75 east 2021-02-13T00:03:47Z- AHRS_M2 msg: 1613174627.67 epoch sec; seqNo:9244 2021-02-13T00:03:47Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.16 psi 2021-02-13T00:03:47Z- handleDepth()-Depth_Keller msg: 1613174627672 epoch sec; seqNo:9316; depth 13.28 2021-02-13T00:03:47Z- handleDvl() - RDI_Pathfinder msg: 1613174627.68 epoch sec; seqNo:4525 2021-02-13T00:03:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.35 , 109.82 , 113.88 2021-02-13T00:03:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.46 , -0.43 , -0.27 2021-02-13T00:03:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174627.69 epoch sec; seqNo:9316; 4058519.08 north; 591968.75 east 2021-02-13T00:03:48Z- handleDepth()-Depth_Keller msg: 1613174628065 epoch sec; seqNo:9317; depth 13.16 2021-02-13T00:03:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174628.08 epoch sec; seqNo:9317; 4058519.46 north; 591968.74 east 2021-02-13T00:03:48Z- AHRS_M2 msg: 1613174628.46 epoch sec; seqNo:9245 2021-02-13T00:03:48Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.15 psi 2021-02-13T00:03:48Z- handleDepth()-Depth_Keller msg: 1613174628466 epoch sec; seqNo:9318; depth 13.06 2021-02-13T00:03:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174628.47 epoch sec; seqNo:9318; 4058519.46 north; 591968.74 east 2021-02-13T00:03:48Z- AHRS_M2 msg: 1613174628.86 epoch sec; seqNo:9246 2021-02-13T00:03:48Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.16 psi 2021-02-13T00:03:48Z- handleDepth()-Depth_Keller msg: 1613174628861 epoch sec; seqNo:9319; depth 12.95 2021-02-13T00:03:48Z- handleDvl() - RDI_Pathfinder msg: 1613174628.88 epoch sec; seqNo:4526 2021-02-13T00:03:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.54 , 98.93 , 110.83 , 78.11 2021-02-13T00:03:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.05 , -0.03 2021-02-13T00:03:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174628.89 epoch sec; seqNo:9319; 4058519.84 north; 591968.74 east 2021-02-13T00:03:49Z- AHRS_M2 msg: 1613174629.26 epoch sec; seqNo:9247 2021-02-13T00:03:49Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.16 psi 2021-02-13T00:03:49Z- handleDepth()-Depth_Keller msg: 1613174629264 epoch sec; seqNo:9320; depth 12.83 2021-02-13T00:03:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174629.28 epoch sec; seqNo:9320; 4058520.23 north; 591969.96 east 2021-02-13T00:03:49Z- AHRS_M2 msg: 1613174629.68 epoch sec; seqNo:9248 2021-02-13T00:03:49Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.15 psi 2021-02-13T00:03:49Z- handleDepth()-Depth_Keller msg: 1613174629694 epoch sec; seqNo:9321; depth 12.74 2021-02-13T00:03:49Z- handleDvl() - RDI_Pathfinder msg: 1613174629.72 epoch sec; seqNo:4527 2021-02-13T00:03:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 100.29 , 113.82 , 0.00 2021-02-13T00:03:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.07 , -0.04 2021-02-13T00:03:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174629.73 epoch sec; seqNo:9321; 4058520.23 north; 591969.96 east 2021-02-13T00:03:50Z- AHRS_M2 msg: 1613174630.07 epoch sec; seqNo:9249 2021-02-13T00:03:50Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.15 psi 2021-02-13T00:03:50Z- handleDepth()-Depth_Keller msg: 1613174630074 epoch sec; seqNo:9322; depth 12.63 2021-02-13T00:03:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:50Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:50Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:50Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:50Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:50Z- handleDvl() - RDI_Pathfinder msg: 1613174630.08 epoch sec; seqNo:4528 2021-02-13T00:03:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.07 , -0.04 2021-02-13T00:03:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174630.09 epoch sec; seqNo:9322; 4058520.61 north; 591969.95 east 2021-02-13T00:03:50Z- AHRS_M2 msg: 1613174630.47 epoch sec; seqNo:9250 2021-02-13T00:03:50Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.14 psi 2021-02-13T00:03:50Z- handleDepth()-Depth_Keller msg: 1613174630476 epoch sec; seqNo:9323; depth 12.54 2021-02-13T00:03:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174630.48 epoch sec; seqNo:9323; 4058520.99 north; 591969.95 east 2021-02-13T00:03:50Z- AHRS_M2 msg: 1613174630.88 epoch sec; seqNo:9251 2021-02-13T00:03:50Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.15 psi 2021-02-13T00:03:50Z- handleDepth()-Depth_Keller msg: 1613174630880 epoch sec; seqNo:9324; depth 12.42 2021-02-13T00:03:50Z- handleDvl() - RDI_Pathfinder msg: 1613174630.89 epoch sec; seqNo:4529 2021-02-13T00:03:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 99.75 , 114.63 , 78.11 2021-02-13T00:03:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.05 , -0.07 2021-02-13T00:03:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174630.90 epoch sec; seqNo:9324; 4058520.99 north; 591969.95 east 2021-02-13T00:03:51Z- AHRS_M2 msg: 1613174631.28 epoch sec; seqNo:9252 2021-02-13T00:03:51Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.17 psi 2021-02-13T00:03:51Z- handleDepth()-Depth_Keller msg: 1613174631285 epoch sec; seqNo:9325; depth 12.30 2021-02-13T00:03:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:51Z- handleDvl() - RDI_Pathfinder msg: 1613174631.29 epoch sec; seqNo:4530 2021-02-13T00:03:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.05 , -0.07 2021-02-13T00:03:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174631.31 epoch sec; seqNo:9325; 4058521.37 north; 591969.94 east 2021-02-13T00:03:51Z- AHRS_M2 msg: 1613174631.68 epoch sec; seqNo:9253 2021-02-13T00:03:51Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.18 psi 2021-02-13T00:03:51Z- handleDepth()-Depth_Keller msg: 1613174631690 epoch sec; seqNo:9326; depth 12.19 2021-02-13T00:03:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174631.70 epoch sec; seqNo:9326; 4058521.38 north; 591971.16 east 2021-02-13T00:03:52Z- LcmTrn::handleMessages() - handling 41 messages took 0.55 ms and 130 calls... 2021-02-13T00:03:52Z- waiting for 1613174629.88, time is 1613174631.70 2021-02-13T00:03:52Z- LcmTrn::updateTrn() >>>> pose time 1613174631.70, meas time 1613174631.29 <<<< 2021-02-13T00:03:52Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174631.29, ping # 0. 2021-02-13T00:03:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174631.29 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:03:52Z- TerrainNav::motionUpdate() - currEstimate = (4058521, 591971.16, 12.19) 2021-02-13T00:03:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:03:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:03:52Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:03:52Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:03:52Z- TNavBF::Projecting Measurements 2021-02-13T00:03:52Z- TNavFilter::Measurement from time = 1613174631.29 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:03:52Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:03:52Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:03:52Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:03:52Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:03:52Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:03:52Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:03:52Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:03:52Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 720.3 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:03:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:03:52Z- TNavBF::mmse selected from filter -1 2021-02-13T00:03:52Z- variances([0], [2], and psi_berg) = 720.321831 833.009875 0.000000 2021-02-13T00:03:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:03:52Z- LcmTrn::publishEstimates() - MLE : 4058517.88 591922.66 12.19 2021-02-13T00:03:52Z- LcmTrn::publishEstimates() - MMSE : 4058521.38 591971.16 12.19 2021-02-13T00:03:52Z- LcmTrn::publishEstimates() - COVAR: 720.32 833.01 1.36 0.00 2021-02-13T00:03:52Z- AHRS_M2 msg: 1613174632.09 epoch sec; seqNo:9254 2021-02-13T00:03:52Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.19 psi 2021-02-13T00:03:52Z- handleDepth()-Depth_Keller msg: 1613174632097 epoch sec; seqNo:9327; depth 12.08 2021-02-13T00:03:52Z- handleDvl() - RDI_Pathfinder msg: 1613174632.12 epoch sec; seqNo:4531 2021-02-13T00:03:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 100.22 , 116.44 , 78.58 2021-02-13T00:03:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , -0.13 2021-02-13T00:03:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174632.13 epoch sec; seqNo:9327; 4058521.76 north; 591971.16 east 2021-02-13T00:03:52Z- AHRS_M2 msg: 1613174632.49 epoch sec; seqNo:9255 2021-02-13T00:03:52Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.20 psi 2021-02-13T00:03:52Z- handleDepth()-Depth_Keller msg: 1613174632499 epoch sec; seqNo:9328; depth 11.97 2021-02-13T00:03:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:52Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:52Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:52Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:52Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:52Z- handleDvl() - RDI_Pathfinder msg: 1613174632.50 epoch sec; seqNo:4532 2021-02-13T00:03:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , -0.13 2021-02-13T00:03:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174632.51 epoch sec; seqNo:9328; 4058522.14 north; 591971.16 east 2021-02-13T00:03:52Z- AHRS_M2 msg: 1613174632.90 epoch sec; seqNo:9256 2021-02-13T00:03:52Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.22 psi 2021-02-13T00:03:52Z- handleDepth()-Depth_Keller msg: 1613174632901 epoch sec; seqNo:9329; depth 11.88 2021-02-13T00:03:52Z- handleDvl() - RDI_Pathfinder msg: 1613174632.91 epoch sec; seqNo:4533 2021-02-13T00:03:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 104.21 , 115.03 , 78.51 2021-02-13T00:03:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , -0.15 2021-02-13T00:03:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174632.94 epoch sec; seqNo:9329; 4058522.52 north; 591971.15 east 2021-02-13T00:03:53Z- AHRS_M2 msg: 1613174633.30 epoch sec; seqNo:9257 2021-02-13T00:03:53Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.23 psi 2021-02-13T00:03:53Z- handleDepth()-Depth_Keller msg: 1613174633305 epoch sec; seqNo:9330; depth 11.76 2021-02-13T00:03:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174633.32 epoch sec; seqNo:9330; 4058522.52 north; 591971.15 east 2021-02-13T00:03:53Z- AHRS_M2 msg: 1613174633.71 epoch sec; seqNo:9258 2021-02-13T00:03:53Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.24 psi 2021-02-13T00:03:53Z- handleDepth()-Depth_Keller msg: 1613174633719 epoch sec; seqNo:9331; depth 11.66 2021-02-13T00:03:53Z- handleDvl() - RDI_Pathfinder msg: 1613174633.73 epoch sec; seqNo:4534 2021-02-13T00:03:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 106.91 , 115.03 , 78.51 2021-02-13T00:03:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.17 , -0.13 2021-02-13T00:03:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174633.74 epoch sec; seqNo:9331; 4058522.90 north; 591971.15 east 2021-02-13T00:03:54Z- AHRS_M2 msg: 1613174634.11 epoch sec; seqNo:9259 2021-02-13T00:03:54Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.24 psi 2021-02-13T00:03:54Z- handleDepth()-Depth_Keller msg: 1613174634113 epoch sec; seqNo:9332; depth 11.57 2021-02-13T00:03:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174634.13 epoch sec; seqNo:9332; 4058523.27 north; 591971.14 east 2021-02-13T00:03:54Z- AHRS_M2 msg: 1613174634.52 epoch sec; seqNo:9260 2021-02-13T00:03:54Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.23 psi 2021-02-13T00:03:54Z- handleDepth()-Depth_Keller msg: 1613174634520 epoch sec; seqNo:9333; depth 11.46 2021-02-13T00:03:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174634.53 epoch sec; seqNo:9333; 4058523.65 north; 591971.14 east 2021-02-13T00:03:54Z- AHRS_M2 msg: 1613174634.92 epoch sec; seqNo:9261 2021-02-13T00:03:54Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.22 psi 2021-02-13T00:03:54Z- handleDepth()-Depth_Keller msg: 1613174634926 epoch sec; seqNo:9334; depth 11.38 2021-02-13T00:03:54Z- handleDvl() - RDI_Pathfinder msg: 1613174634.94 epoch sec; seqNo:4535 2021-02-13T00:03:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 110.56 , 117.32 , 79.46 2021-02-13T00:03:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.24 , -0.08 2021-02-13T00:03:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174634.95 epoch sec; seqNo:9334; 4058523.67 north; 591972.36 east 2021-02-13T00:03:55Z- AHRS_M2 msg: 1613174635.32 epoch sec; seqNo:9262 2021-02-13T00:03:55Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.20 psi 2021-02-13T00:03:55Z- handleDepth()-Depth_Keller msg: 1613174635328 epoch sec; seqNo:9335; depth 11.28 2021-02-13T00:03:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174635.35 epoch sec; seqNo:9335; 4058524.04 north; 591972.36 east 2021-02-13T00:03:55Z- AHRS_M2 msg: 1613174635.72 epoch sec; seqNo:9263 2021-02-13T00:03:55Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.19 psi 2021-02-13T00:03:55Z- handleDepth()-Depth_Keller msg: 1613174635729 epoch sec; seqNo:9336; depth 11.17 2021-02-13T00:03:55Z- handleDvl() - RDI_Pathfinder msg: 1613174635.74 epoch sec; seqNo:4536 2021-02-13T00:03:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.36 , 112.79 , 77.25 2021-02-13T00:03:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.31 , 0.07 2021-02-13T00:03:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174635.78 epoch sec; seqNo:9336; 4058524.42 north; 591972.35 east 2021-02-13T00:03:56Z- AHRS_M2 msg: 1613174636.13 epoch sec; seqNo:9264 2021-02-13T00:03:56Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.18 psi 2021-02-13T00:03:56Z- handleDepth()-Depth_Keller msg: 1613174636136 epoch sec; seqNo:9337; depth 11.06 2021-02-13T00:03:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:56Z- handleDvl() - RDI_Pathfinder msg: 1613174636.14 epoch sec; seqNo:4537 2021-02-13T00:03:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.31 , 0.07 2021-02-13T00:03:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174636.15 epoch sec; seqNo:9337; 4058524.80 north; 591972.35 east 2021-02-13T00:03:56Z- AHRS_M2 msg: 1613174636.53 epoch sec; seqNo:9265 2021-02-13T00:03:56Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.18 psi 2021-02-13T00:03:56Z- handleDepth()-Depth_Keller msg: 1613174636542 epoch sec; seqNo:9338; depth 10.95 2021-02-13T00:03:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174636.55 epoch sec; seqNo:9338; 4058525.18 north; 591972.34 east 2021-02-13T00:03:56Z- AHRS_M2 msg: 1613174636.93 epoch sec; seqNo:9266 2021-02-13T00:03:56Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-13T00:03:56Z- handleDepth()-Depth_Keller msg: 1613174636941 epoch sec; seqNo:9339; depth 10.84 2021-02-13T00:03:56Z- handleDvl() - RDI_Pathfinder msg: 1613174636.95 epoch sec; seqNo:4538 2021-02-13T00:03:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.36 , 106.64 , 117.46 , 79.59 2021-02-13T00:03:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.20 , 0.32 2021-02-13T00:03:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174636.97 epoch sec; seqNo:9339; 4058525.18 north; 591972.34 east 2021-02-13T00:03:57Z- LcmTrn::handleMessages() - handling 47 messages took 0.70 ms and 133 calls... 2021-02-13T00:03:57Z- waiting for 1613174634.70, time is 1613174636.97 2021-02-13T00:03:57Z- LcmTrn::updateTrn() equating pose 1613174636.97 and meas times 1613174636.97 2021-02-13T00:03:57Z- LcmTrn::updateTrn() >>>> pose time 1613174636.97, meas time 1613174636.97 <<<< 2021-02-13T00:03:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174636.97 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:03:57Z- TerrainNav::motionUpdate() - currEstimate = (4058525, 591972.34, 10.84) 2021-02-13T00:03:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:03:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:03:57Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174636.97 sec.; No relevant navigation data available 2021-02-13T00:03:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:03:57Z- TNavBF::mmse selected from filter -1 2021-02-13T00:03:57Z- variances([0], [2], and psi_berg) = 720.254293 833.009830 0.000000 2021-02-13T00:03:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:03:57Z- LcmTrn::publishEstimates() - MLE : 4058521.68 591924.84 10.84 2021-02-13T00:03:57Z- LcmTrn::publishEstimates() - MMSE : 4058525.18 591972.34 10.84 2021-02-13T00:03:57Z- LcmTrn::publishEstimates() - COVAR: 720.25 833.01 1.56 0.00 2021-02-13T00:03:57Z- AHRS_M2 msg: 1613174637.34 epoch sec; seqNo:9267 2021-02-13T00:03:57Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.18 psi 2021-02-13T00:03:57Z- handleDepth()-Depth_Keller msg: 1613174637344 epoch sec; seqNo:9340; depth 10.72 2021-02-13T00:03:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:57Z- handleDvl() - RDI_Pathfinder msg: 1613174637.35 epoch sec; seqNo:4539 2021-02-13T00:03:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.20 , 0.32 2021-02-13T00:03:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174637.36 epoch sec; seqNo:9340; 4058525.56 north; 591972.34 east 2021-02-13T00:03:57Z- AHRS_M2 msg: 1613174637.75 epoch sec; seqNo:9268 2021-02-13T00:03:57Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-13T00:03:57Z- handleDepth()-Depth_Keller msg: 1613174637750 epoch sec; seqNo:9341; depth 10.60 2021-02-13T00:03:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174637.76 epoch sec; seqNo:9341; 4058525.94 north; 591972.34 east 2021-02-13T00:03:58Z- AHRS_M2 msg: 1613174638.15 epoch sec; seqNo:9269 2021-02-13T00:03:58Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.18 psi 2021-02-13T00:03:58Z- handleDepth()-Depth_Keller msg: 1613174638154 epoch sec; seqNo:9342; depth 10.48 2021-02-13T00:03:58Z- handleDvl() - RDI_Pathfinder msg: 1613174638.17 epoch sec; seqNo:4540 2021-02-13T00:03:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.78 , 105.36 , 118.88 , 79.66 2021-02-13T00:03:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.06 , 0.42 2021-02-13T00:03:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174638.18 epoch sec; seqNo:9342; 4058526.32 north; 591972.33 east 2021-02-13T00:03:58Z- AHRS_M2 msg: 1613174638.55 epoch sec; seqNo:9270 2021-02-13T00:03:58Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.18 psi 2021-02-13T00:03:58Z- handleDepth()-Depth_Keller msg: 1613174638556 epoch sec; seqNo:9343; depth 10.36 2021-02-13T00:03:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:58Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:58Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:58Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:58Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:58Z- handleDvl() - RDI_Pathfinder msg: 1613174638.56 epoch sec; seqNo:4541 2021-02-13T00:03:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.06 , 0.42 2021-02-13T00:03:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174638.57 epoch sec; seqNo:9343; 4058526.71 north; 591973.55 east 2021-02-13T00:03:58Z- AHRS_M2 msg: 1613174638.96 epoch sec; seqNo:9271 2021-02-13T00:03:58Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.18 psi 2021-02-13T00:03:58Z- handleDepth()-Depth_Keller msg: 1613174638960 epoch sec; seqNo:9344; depth 10.24 2021-02-13T00:03:58Z- handleDvl() - RDI_Pathfinder msg: 1613174638.97 epoch sec; seqNo:4542 2021-02-13T00:03:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.13 , 104.00 , 125.64 , 79.66 2021-02-13T00:03:58Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.31 , 0.23 2021-02-13T00:03:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174638.98 epoch sec; seqNo:9344; 4058526.71 north; 591973.55 east 2021-02-13T00:03:59Z- AHRS_M2 msg: 1613174639.36 epoch sec; seqNo:9272 2021-02-13T00:03:59Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.18 psi 2021-02-13T00:03:59Z- handleDepth()-Depth_Keller msg: 1613174639365 epoch sec; seqNo:9345; depth 10.13 2021-02-13T00:03:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174639.38 epoch sec; seqNo:9345; 4058527.09 north; 591973.55 east 2021-02-13T00:03:59Z- AHRS_M2 msg: 1613174639.76 epoch sec; seqNo:9273 2021-02-13T00:03:59Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.18 psi 2021-02-13T00:03:59Z- handleDepth()-Depth_Keller msg: 1613174639773 epoch sec; seqNo:9346; depth 10.01 2021-02-13T00:03:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:03:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:03:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:03:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:03:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:03:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:03:59Z- handleDvl() - RDI_Pathfinder msg: 1613174639.78 epoch sec; seqNo:4543 2021-02-13T00:03:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:03:59Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.31 , 0.23 2021-02-13T00:03:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174639.83 epoch sec; seqNo:9346; 4058527.48 north; 591974.76 east 2021-02-13T00:04:00Z- AHRS_M2 msg: 1613174640.17 epoch sec; seqNo:9274 2021-02-13T00:04:00Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.18 psi 2021-02-13T00:04:00Z- handleDepth()-Depth_Keller msg: 1613174640172 epoch sec; seqNo:9347; depth 9.90 2021-02-13T00:04:00Z- handleDvl() - RDI_Pathfinder msg: 1613174640.18 epoch sec; seqNo:4544 2021-02-13T00:04:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 106.51 , 122.73 , 80.81 2021-02-13T00:04:00Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.34 , -0.11 2021-02-13T00:04:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174640.19 epoch sec; seqNo:9347; 4058527.48 north; 591974.76 east 2021-02-13T00:04:00Z- AHRS_M2 msg: 1613174640.58 epoch sec; seqNo:9275 2021-02-13T00:04:00Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.18 psi 2021-02-13T00:04:00Z- handleDepth()-Depth_Keller msg: 1613174640581 epoch sec; seqNo:9348; depth 9.80 2021-02-13T00:04:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174640.59 epoch sec; seqNo:9348; 4058527.86 north; 591974.76 east 2021-02-13T00:04:00Z- AHRS_M2 msg: 1613174640.98 epoch sec; seqNo:9276 2021-02-13T00:04:00Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.17 psi 2021-02-13T00:04:00Z- handleDepth()-Depth_Keller msg: 1613174640990 epoch sec; seqNo:9349; depth 9.69 2021-02-13T00:04:01Z- handleDvl() - RDI_Pathfinder msg: 1613174641.00 epoch sec; seqNo:4545 2021-02-13T00:04:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 107.86 , 122.73 , 80.81 2021-02-13T00:04:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.13 , -0.34 2021-02-13T00:04:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174641.01 epoch sec; seqNo:9349; 4058528.24 north; 591974.75 east 2021-02-13T00:04:01Z- AHRS_M2 msg: 1613174641.38 epoch sec; seqNo:9277 2021-02-13T00:04:01Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.16 psi 2021-02-13T00:04:01Z- handleDepth()-Depth_Keller msg: 1613174641389 epoch sec; seqNo:9350; depth 9.59 2021-02-13T00:04:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174641.41 epoch sec; seqNo:9350; 4058528.24 north; 591974.75 east 2021-02-13T00:04:01Z- AHRS_M2 msg: 1613174641.81 epoch sec; seqNo:9278 2021-02-13T00:04:01Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.15 psi 2021-02-13T00:04:01Z- handleDepth()-Depth_Keller msg: 1613174641816 epoch sec; seqNo:9351; depth 9.47 2021-02-13T00:04:01Z- handleDvl() - RDI_Pathfinder msg: 1613174641.84 epoch sec; seqNo:4546 2021-02-13T00:04:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 106.84 , 123.21 , 81.56 2021-02-13T00:04:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.11 , -0.31 2021-02-13T00:04:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174641.85 epoch sec; seqNo:9351; 4058528.62 north; 591974.75 east 2021-02-13T00:04:02Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:04:02Z- waiting for 1613174639.97, time is 1613174641.85 2021-02-13T00:04:02Z- LcmTrn::updateTrn() equating pose 1613174641.85 and meas times 1613174641.85 2021-02-13T00:04:02Z- LcmTrn::updateTrn() >>>> pose time 1613174641.85, meas time 1613174641.85 <<<< 2021-02-13T00:04:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174641.85 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:04:02Z- TerrainNav::motionUpdate() - currEstimate = (4058529, 591974.75, 9.47) 2021-02-13T00:04:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:04:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:04:02Z- TNavBF::Projecting Measurements 2021-02-13T00:04:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:04:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:04:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:04:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:04:02Z- TNavFilter::4 good beams. 2021-02-13T00:04:02Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:04:02Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:04:02Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:04:02Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:04:02Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:04:02Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:04:02Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:04:02Z- TNavBF::mmse selected from filter -1 2021-02-13T00:04:02Z- TerrainMapOctree: (vehN, vehE ) = (4058528.62, 591974.75). 2021-02-13T00:04:02Z- TerrainMapOctree: Min Distance = 3547.21. 2021-02-13T00:04:02Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:04:02Z- There is no specific good beam that all particles have in common. 2021-02-13T00:04:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:04:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.77 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:04:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:04:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.61 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 18.27 2021-02-13T00:04:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:04:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.49 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 24.29 2021-02-13T00:04:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:04:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.41 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 10.64 2021-02-13T00:04:02Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:04:02Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:04:02Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:04:02Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:04:02Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:04:02Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:04:02Z- Filter 0 No beams used in subcloud update 2021-02-13T00:04:02Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:04:02Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:04:02Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 720.2 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:04:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174641.85 sec. 2021-02-13T00:04:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:04:02Z- TNavBF::mmse selected from filter -1 2021-02-13T00:04:02Z- variances([0], [2], and psi_berg) = 720.185021 833.009785 0.000000 2021-02-13T00:04:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:04:02Z- LcmTrn::publishEstimates() - MLE : 4058525.12 591927.25 9.47 2021-02-13T00:04:02Z- LcmTrn::publishEstimates() - MMSE : 4058528.62 591974.75 9.47 2021-02-13T00:04:02Z- LcmTrn::publishEstimates() - COVAR: 720.19 833.01 1.26 0.00 2021-02-13T00:04:02Z- AHRS_M2 msg: 1613174642.19 epoch sec; seqNo:9279 2021-02-13T00:04:02Z- AHRS_M2 msg: 0.68 phi; 0.33 theta; -0.14 psi 2021-02-13T00:04:02Z- handleDepth()-Depth_Keller msg: 1613174642194 epoch sec; seqNo:9352; depth 9.35 2021-02-13T00:04:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:04:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:04:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:04:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:04:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:04:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:04:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:04:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:04:02Z- handleDvl() - RDI_Pathfinder msg: 1613174642.20 epoch sec; seqNo:4547 2021-02-13T00:04:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:04:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.11 , -0.31 2021-02-13T00:04:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174642.21 epoch sec; seqNo:9352; 4058528.63 north; 591975.97 east 2021-02-13T00:04:02Z- AHRS_M2 msg: 1613174642.59 epoch sec; seqNo:9280 2021-02-13T00:04:02Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.13 psi 2021-02-13T00:04:02Z- handleDepth()-Depth_Keller msg: 1613174642610 epoch sec; seqNo:9353; depth 9.25 2021-02-13T00:04:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174642.62 epoch sec; seqNo:9353; 4058529.01 north; 591975.97 east 2021-02-13T00:04:02Z- AHRS_M2 msg: 1613174643.00 epoch sec; seqNo:9281 2021-02-13T00:04:02Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.13 psi 2021-02-13T00:04:03Z- handleDepth()-Depth_Keller msg: 1613174643000 epoch sec; seqNo:9354; depth 9.12 2021-02-13T00:04:03Z- handleDvl() - RDI_Pathfinder msg: 1613174643.01 epoch sec; seqNo:4548 2021-02-13T00:04:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 103.87 , 120.23 , 81.56 2021-02-13T00:04:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.60 , -0.26 , -0.09 2021-02-13T00:04:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174643.03 epoch sec; seqNo:9354; 4058529.39 north; 591975.96 east 2021-02-13T00:04:03Z- AHRS_M2 msg: 1613174643.40 epoch sec; seqNo:9282 2021-02-13T00:04:03Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.14 psi 2021-02-13T00:04:03Z- handleDepth()-Depth_Keller msg: 1613174643404 epoch sec; seqNo:9355; depth 9.00 2021-02-13T00:04:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:04:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:04:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:04:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:04:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:04:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:04:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:04:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:04:03Z- handleDvl() - RDI_Pathfinder msg: 1613174643.41 epoch sec; seqNo:4549 2021-02-13T00:04:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:04:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.60 , -0.26 , -0.09 2021-02-13T00:04:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174643.42 epoch sec; seqNo:9355; 4058529.39 north; 591975.96 east 2021-02-13T00:04:03Z- AHRS_M2 msg: 1613174643.85 epoch sec; seqNo:9283 2021-02-13T00:04:03Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-13T00:04:03Z- handleDepth()-Depth_Keller msg: 1613174643851 epoch sec; seqNo:9356; depth 8.88 2021-02-13T00:04:03Z- handleDvl() - RDI_Pathfinder msg: 1613174643.87 epoch sec; seqNo:4550 2021-02-13T00:04:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.86 , 103.73 , 115.90 , 80.74 2021-02-13T00:04:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , -0.18 , 0.16 2021-02-13T00:04:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174643.88 epoch sec; seqNo:9356; 4058529.77 north; 591975.96 east 2021-02-13T00:04:04Z- handleDepth()-Depth_Keller msg: 1613174644229 epoch sec; seqNo:9357; depth 8.74 2021-02-13T00:04:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174644.24 epoch sec; seqNo:9357; 4058530.15 north; 591975.95 east 2021-02-13T00:04:04Z- AHRS_M2 msg: 1613174644.61 epoch sec; seqNo:9284 2021-02-13T00:04:04Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-13T00:04:04Z- handleDepth()-Depth_Keller msg: 1613174644617 epoch sec; seqNo:9358; depth 8.62 2021-02-13T00:04:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:04:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:04:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:04:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:04:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:04:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:04:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:04:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:04:04Z- handleDvl() - RDI_Pathfinder msg: 1613174644.62 epoch sec; seqNo:4551 2021-02-13T00:04:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:04:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , -0.18 , 0.16 2021-02-13T00:04:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174644.63 epoch sec; seqNo:9358; 4058530.15 north; 591975.95 east 2021-02-13T00:04:05Z- AHRS_M2 msg: 1613174645.02 epoch sec; seqNo:9285 2021-02-13T00:04:05Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-13T00:04:05Z- handleDepth()-Depth_Keller msg: 1613174645020 epoch sec; seqNo:9359; depth 8.50 2021-02-13T00:04:05Z- handleDvl() - RDI_Pathfinder msg: 1613174645.04 epoch sec; seqNo:4552 2021-02-13T00:04:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 105.36 , 120.23 , 81.01 2021-02-13T00:04:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , 0.19 2021-02-13T00:04:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174645.05 epoch sec; seqNo:9359; 4058530.52 north; 591975.95 east 2021-02-13T00:04:05Z- AHRS_M2 msg: 1613174645.43 epoch sec; seqNo:9286 2021-02-13T00:04:05Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.18 psi 2021-02-13T00:04:05Z- handleDepth()-Depth_Keller msg: 1613174645431 epoch sec; seqNo:9360; depth 8.35 2021-02-13T00:04:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174645.44 epoch sec; seqNo:9360; 4058530.90 north; 591975.95 east 2021-02-13T00:04:05Z- handleDepth()-Depth_Keller msg: 1613174645869 epoch sec; seqNo:9361; depth 8.24 2021-02-13T00:04:05Z- handleDvl() - RDI_Pathfinder msg: 1613174645.88 epoch sec; seqNo:4553 2021-02-13T00:04:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.21 , 107.79 , 122.67 , 82.10 2021-02-13T00:04:05Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.01 , 0.02 2021-02-13T00:04:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174645.90 epoch sec; seqNo:9361; 4058531.30 north; 591977.16 east 2021-02-13T00:04:06Z- AHRS_M2 msg: 1613174646.23 epoch sec; seqNo:9287 2021-02-13T00:04:06Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.18 psi 2021-02-13T00:04:06Z- handleDepth()-Depth_Keller msg: 1613174646233 epoch sec; seqNo:9362; depth 8.10 2021-02-13T00:04:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174646.25 epoch sec; seqNo:9362; 4058531.67 north; 591977.16 east 2021-02-13T00:04:06Z- AHRS_M2 msg: 1613174646.63 epoch sec; seqNo:9288 2021-02-13T00:04:06Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.18 psi 2021-02-13T00:04:06Z- handleDepth()-Depth_Keller msg: 1613174646636 epoch sec; seqNo:9363; depth 7.99 2021-02-13T00:04:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174646.64 epoch sec; seqNo:9363; 4058531.67 north; 591977.16 east 2021-02-13T00:04:07Z- AHRS_M2 msg: 1613174647.04 epoch sec; seqNo:9289 2021-02-13T00:04:07Z- AHRS_M2 msg: 0.65 phi; 0.37 theta; -0.17 psi 2021-02-13T00:04:07Z- handleDepth()-Depth_Keller msg: 1613174647049 epoch sec; seqNo:9364; depth 7.89 2021-02-13T00:04:07Z- handleDvl() - RDI_Pathfinder msg: 1613174647.06 epoch sec; seqNo:4554 2021-02-13T00:04:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.56 , 110.50 , 126.72 , 82.10 2021-02-13T00:04:07Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.12 , -0.12 2021-02-13T00:04:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174647.07 epoch sec; seqNo:9364; 4058532.05 north; 591977.15 east 2021-02-13T00:04:07Z- LcmTrn::handleMessages() - handling 45 messages took 0.68 ms and 132 calls... 2021-02-13T00:04:07Z- waiting for 1613174644.85, time is 1613174647.07 2021-02-13T00:04:07Z- LcmTrn::updateTrn() equating pose 1613174647.07 and meas times 1613174647.07 2021-02-13T00:04:07Z- LcmTrn::updateTrn() >>>> pose time 1613174647.07, meas time 1613174647.07 <<<< 2021-02-13T00:04:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174647.07 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:04:07Z- TerrainNav::motionUpdate() - currEstimate = (4058532, 591977.15, 7.89) 2021-02-13T00:04:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:04:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:04:07Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174647.07 sec.; No relevant navigation data available 2021-02-13T00:04:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:04:07Z- TNavBF::mmse selected from filter -1 2021-02-13T00:04:07Z- variances([0], [2], and psi_berg) = 720.115805 833.009740 0.000000 2021-02-13T00:04:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:04:07Z- LcmTrn::publishEstimates() - MLE : 4058528.55 591934.65 7.89 2021-02-13T00:04:07Z- LcmTrn::publishEstimates() - MMSE : 4058532.05 591977.15 7.89 2021-02-13T00:04:07Z- LcmTrn::publishEstimates() - COVAR: 720.12 833.01 1.48 0.00 2021-02-13T00:04:07Z- AHRS_M2 msg: 1613174647.44 epoch sec; seqNo:9290 2021-02-13T00:04:07Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.17 psi 2021-02-13T00:04:07Z- handleDepth()-Depth_Keller msg: 1613174647444 epoch sec; seqNo:9365; depth 7.75 2021-02-13T00:04:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174647.46 epoch sec; seqNo:9365; 4058532.43 north; 591977.15 east 2021-02-13T00:04:07Z- AHRS_M2 msg: 1613174647.84 epoch sec; seqNo:9291 2021-02-13T00:04:07Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.16 psi 2021-02-13T00:04:07Z- handleDepth()-Depth_Keller msg: 1613174647848 epoch sec; seqNo:9366; depth 7.64 2021-02-13T00:04:07Z- handleDvl() - RDI_Pathfinder msg: 1613174647.86 epoch sec; seqNo:4555 2021-02-13T00:04:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.41 , 109.75 , 123.14 , 81.49 2021-02-13T00:04:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.19 , -0.04 2021-02-13T00:04:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174647.88 epoch sec; seqNo:9366; 4058532.81 north; 591977.15 east 2021-02-13T00:04:08Z- AHRS_M2 msg: 1613174648.25 epoch sec; seqNo:9292 2021-02-13T00:04:08Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.14 psi 2021-02-13T00:04:08Z- handleDepth()-Depth_Keller msg: 1613174648252 epoch sec; seqNo:9367; depth 7.52 2021-02-13T00:04:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174648.26 epoch sec; seqNo:9367; 4058533.19 north; 591977.14 east 2021-02-13T00:04:08Z- AHRS_M2 msg: 1613174648.65 epoch sec; seqNo:9293 2021-02-13T00:04:08Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.13 psi 2021-02-13T00:04:08Z- handleDepth()-Depth_Keller msg: 1613174648659 epoch sec; seqNo:9368; depth 7.39 2021-02-13T00:04:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174648.67 epoch sec; seqNo:9368; 4058533.58 north; 591978.36 east 2021-02-13T00:04:09Z- AHRS_M2 msg: 1613174649.06 epoch sec; seqNo:9294 2021-02-13T00:04:09Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.13 psi 2021-02-13T00:04:09Z- handleDepth()-Depth_Keller msg: 1613174649065 epoch sec; seqNo:9369; depth 7.28 2021-02-13T00:04:09Z- handleDvl() - RDI_Pathfinder msg: 1613174649.08 epoch sec; seqNo:4556 2021-02-13T00:04:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.04 , 106.91 , 130.71 , 0.00 2021-02-13T00:04:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.08 , 0.10 2021-02-13T00:04:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174649.10 epoch sec; seqNo:9369; 4058533.58 north; 591978.36 east 2021-02-13T00:04:09Z- AHRS_M2 msg: 1613174649.46 epoch sec; seqNo:9295 2021-02-13T00:04:09Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.13 psi 2021-02-13T00:04:09Z- handleDepth()-Depth_Keller msg: 1613174649464 epoch sec; seqNo:9370; depth 7.14 2021-02-13T00:04:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174649.47 epoch sec; seqNo:9370; 4058533.96 north; 591978.36 east 2021-02-13T00:04:09Z- AHRS_M2 msg: 1613174649.86 epoch sec; seqNo:9296 2021-02-13T00:04:09Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.13 psi 2021-02-13T00:04:09Z- handleDepth()-Depth_Keller msg: 1613174649881 epoch sec; seqNo:9371; depth 7.04 2021-02-13T00:04:09Z- handleDvl() - RDI_Pathfinder msg: 1613174649.89 epoch sec; seqNo:4557 2021-02-13T00:04:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 106.17 , 125.51 , 0.00 2021-02-13T00:04:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , 0.07 , 0.08 2021-02-13T00:04:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174649.90 epoch sec; seqNo:9371; 4058533.96 north; 591978.36 east 2021-02-13T00:04:10Z- AHRS_M2 msg: 1613174650.27 epoch sec; seqNo:9297 2021-02-13T00:04:10Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.14 psi 2021-02-13T00:04:10Z- handleDepth()-Depth_Keller msg: 1613174650272 epoch sec; seqNo:9372; depth 6.90 2021-02-13T00:04:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174650.28 epoch sec; seqNo:9372; 4058534.34 north; 591978.35 east 2021-02-13T00:04:10Z- AHRS_M2 msg: 1613174650.67 epoch sec; seqNo:9298 2021-02-13T00:04:10Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.15 psi 2021-02-13T00:04:10Z- handleDepth()-Depth_Keller msg: 1613174650681 epoch sec; seqNo:9373; depth 6.76 2021-02-13T00:04:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174650.69 epoch sec; seqNo:9373; 4058534.72 north; 591978.35 east 2021-02-13T00:04:11Z- AHRS_M2 msg: 1613174651.08 epoch sec; seqNo:9299 2021-02-13T00:04:11Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.17 psi 2021-02-13T00:04:11Z- handleDepth()-Depth_Keller msg: 1613174651080 epoch sec; seqNo:9374; depth 6.65 2021-02-13T00:04:11Z- handleDvl() - RDI_Pathfinder msg: 1613174651.10 epoch sec; seqNo:4558 2021-02-13T00:04:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.31 , 106.24 , 125.57 , 0.00 2021-02-13T00:04:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.06 , 0.02 2021-02-13T00:04:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174651.11 epoch sec; seqNo:9374; 4058534.72 north; 591978.35 east 2021-02-13T00:04:11Z- AHRS_M2 msg: 1613174651.48 epoch sec; seqNo:9300 2021-02-13T00:04:11Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.17 psi 2021-02-13T00:04:11Z- handleDepth()-Depth_Keller msg: 1613174651486 epoch sec; seqNo:9375; depth 6.50 2021-02-13T00:04:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174651.50 epoch sec; seqNo:9375; 4058535.11 north; 591979.56 east 2021-02-13T00:04:11Z- AHRS_M2 msg: 1613174651.88 epoch sec; seqNo:9301 2021-02-13T00:04:11Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.18 psi 2021-02-13T00:04:11Z- handleDepth()-Depth_Keller msg: 1613174651893 epoch sec; seqNo:9376; depth 6.37 2021-02-13T00:04:11Z- handleDvl() - RDI_Pathfinder msg: 1613174651.91 epoch sec; seqNo:4559 2021-02-13T00:04:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.78 , 110.63 , 128.48 , 0.00 2021-02-13T00:04:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , 0.13 2021-02-13T00:04:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174651.92 epoch sec; seqNo:9376; 4058535.11 north; 591979.56 east 2021-02-13T00:04:12Z- LcmTrn::handleMessages() - handling 41 messages took 0.68 ms and 128 calls... 2021-02-13T00:04:12Z- waiting for 1613174650.07, time is 1613174651.92 2021-02-13T00:04:12Z- LcmTrn::updateTrn() equating pose 1613174651.92 and meas times 1613174651.92 2021-02-13T00:04:12Z- LcmTrn::updateTrn() >>>> pose time 1613174651.92, meas time 1613174651.92 <<<< 2021-02-13T00:04:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174651.92 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:04:12Z- TerrainNav::motionUpdate() - currEstimate = (4058535, 591979.56, 6.37) 2021-02-13T00:04:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:04:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:04:12Z- TNavBF::Projecting Measurements 2021-02-13T00:04:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:04:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:04:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:04:12Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:04:12Z- TNavFilter::3 good beams. 2021-02-13T00:04:12Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:04:12Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:04:12Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:04:12Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:04:12Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:04:12Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:04:12Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:04:12Z- TNavBF::mmse selected from filter -1 2021-02-13T00:04:12Z- TerrainMapOctree: (vehN, vehE ) = (4058535.11, 591979.56). 2021-02-13T00:04:12Z- TerrainMapOctree: Min Distance = 3539.14. 2021-02-13T00:04:12Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:04:12Z- There is no specific good beam that all particles have in common. 2021-02-13T00:04:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:04:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.67 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 13.77 2021-02-13T00:04:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:04:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.51 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 19.58 2021-02-13T00:04:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:04:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.41 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 26.41 2021-02-13T00:04:12Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:04:12Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:04:12Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:04:12Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:04:12Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:04:12Z- Filter 0 No beams used in subcloud update 2021-02-13T00:04:12Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:04:12Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:04:12Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 720.0 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:04:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174651.92 sec. 2021-02-13T00:04:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:04:12Z- TNavBF::mmse selected from filter -1 2021-02-13T00:04:12Z- variances([0], [2], and psi_berg) = 720.049178 833.009696 0.000000 2021-02-13T00:04:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:04:12Z- LcmTrn::publishEstimates() - MLE : 4058531.61 591937.06 6.37 2021-02-13T00:04:12Z- LcmTrn::publishEstimates() - MMSE : 4058535.11 591979.56 6.37 2021-02-13T00:04:12Z- LcmTrn::publishEstimates() - COVAR: 720.05 833.01 1.34 0.00 2021-02-13T00:04:12Z- AHRS_M2 msg: 1613174652.29 epoch sec; seqNo:9302 2021-02-13T00:04:12Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.18 psi 2021-02-13T00:04:12Z- handleDepth()-Depth_Keller msg: 1613174652292 epoch sec; seqNo:9377; depth 6.23 2021-02-13T00:04:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174652.30 epoch sec; seqNo:9377; 4058535.49 north; 591979.56 east 2021-02-13T00:04:12Z- AHRS_M2 msg: 1613174652.69 epoch sec; seqNo:9303 2021-02-13T00:04:12Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.18 psi 2021-02-13T00:04:12Z- handleDepth()-Depth_Keller msg: 1613174652696 epoch sec; seqNo:9378; depth 6.11 2021-02-13T00:04:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174652.70 epoch sec; seqNo:9378; 4058535.87 north; 591979.56 east 2021-02-13T00:04:13Z- AHRS_M2 msg: 1613174653.10 epoch sec; seqNo:9304 2021-02-13T00:04:13Z- AHRS_M2 msg: 0.61 phi; 0.38 theta; -0.18 psi 2021-02-13T00:04:13Z- handleDepth()-Depth_Keller msg: 1613174653110 epoch sec; seqNo:9379; depth 5.98 2021-02-13T00:04:13Z- handleDvl() - RDI_Pathfinder msg: 1613174653.12 epoch sec; seqNo:4560 2021-02-13T00:04:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.29 , 110.63 , 128.48 , 82.37 2021-02-13T00:04:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.06 , 0.26 2021-02-13T00:04:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174653.14 epoch sec; seqNo:9379; 4058536.25 north; 591979.55 east 2021-02-13T00:04:13Z- AHRS_M2 msg: 1613174653.50 epoch sec; seqNo:9305 2021-02-13T00:04:13Z- AHRS_M2 msg: 0.61 phi; 0.39 theta; -0.17 psi 2021-02-13T00:04:13Z- handleDepth()-Depth_Keller msg: 1613174653504 epoch sec; seqNo:9380; depth 5.83 2021-02-13T00:04:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174653.52 epoch sec; seqNo:9380; 4058536.25 north; 591979.55 east 2021-02-13T00:04:13Z- AHRS_M2 msg: 1613174653.90 epoch sec; seqNo:9306 2021-02-13T00:04:13Z- AHRS_M2 msg: 0.61 phi; 0.39 theta; -0.17 psi 2021-02-13T00:04:13Z- handleDepth()-Depth_Keller msg: 1613174653913 epoch sec; seqNo:9381; depth 5.71 2021-02-13T00:04:13Z- handleDvl() - RDI_Pathfinder msg: 1613174653.92 epoch sec; seqNo:4561 2021-02-13T00:04:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.39 , 112.73 , 133.55 , 0.00 2021-02-13T00:04:13Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.15 , 0.10 2021-02-13T00:04:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174653.93 epoch sec; seqNo:9381; 4058536.64 north; 591980.77 east 2021-02-13T00:04:14Z- AHRS_M2 msg: 1613174654.31 epoch sec; seqNo:9307 2021-02-13T00:04:14Z- AHRS_M2 msg: 0.62 phi; 0.40 theta; -0.17 psi 2021-02-13T00:04:14Z- handleDepth()-Depth_Keller msg: 1613174654321 epoch sec; seqNo:9382; depth 5.57 2021-02-13T00:04:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174654.33 epoch sec; seqNo:9382; 4058537.02 north; 591980.76 east 2021-02-13T00:04:14Z- AHRS_M2 msg: 1613174654.71 epoch sec; seqNo:9308 2021-02-13T00:04:14Z- AHRS_M2 msg: 0.63 phi; 0.40 theta; -0.17 psi 2021-02-13T00:04:14Z- handleDepth()-Depth_Keller msg: 1613174654716 epoch sec; seqNo:9383; depth 5.43 2021-02-13T00:04:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174654.73 epoch sec; seqNo:9383; 4058537.02 north; 591980.76 east 2021-02-13T00:04:15Z- AHRS_M2 msg: 1613174655.12 epoch sec; seqNo:9309 2021-02-13T00:04:15Z- AHRS_M2 msg: 0.63 phi; 0.39 theta; -0.18 psi 2021-02-13T00:04:15Z- handleDepth()-Depth_Keller msg: 1613174655123 epoch sec; seqNo:9384; depth 5.30 2021-02-13T00:04:15Z- handleDvl() - RDI_Pathfinder msg: 1613174655.14 epoch sec; seqNo:4562 2021-02-13T00:04:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 112.12 , 134.43 , 0.00 2021-02-13T00:04:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.04 , -0.15 2021-02-13T00:04:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174655.16 epoch sec; seqNo:9384; 4058537.40 north; 591980.76 east 2021-02-13T00:04:15Z- AHRS_M2 msg: 1613174655.52 epoch sec; seqNo:9310 2021-02-13T00:04:15Z- AHRS_M2 msg: 0.64 phi; 0.40 theta; -0.18 psi 2021-02-13T00:04:15Z- handleDepth()-Depth_Keller msg: 1613174655544 epoch sec; seqNo:9385; depth 5.15 2021-02-13T00:04:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174655.57 epoch sec; seqNo:9385; 4058537.79 north; 591981.98 east 2021-02-13T00:04:15Z- AHRS_M2 msg: 1613174655.93 epoch sec; seqNo:9311 2021-02-13T00:04:15Z- AHRS_M2 msg: 0.64 phi; 0.39 theta; -0.17 psi 2021-02-13T00:04:15Z- handleDepth()-Depth_Keller msg: 1613174655933 epoch sec; seqNo:9386; depth 5.01 2021-02-13T00:04:15Z- handleDvl() - RDI_Pathfinder msg: 1613174655.94 epoch sec; seqNo:4563 2021-02-13T00:04:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 115.09 , 134.43 , 116.58 2021-02-13T00:04:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.06 , -0.12 2021-02-13T00:04:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174655.95 epoch sec; seqNo:9386; 4058538.17 north; 591981.97 east 2021-02-13T00:04:16Z- AHRS_M2 msg: 1613174656.33 epoch sec; seqNo:9312 2021-02-13T00:04:16Z- AHRS_M2 msg: 0.65 phi; 0.39 theta; -0.16 psi 2021-02-13T00:04:16Z- handleDepth()-Depth_Keller msg: 1613174656332 epoch sec; seqNo:9387; depth 4.89 2021-02-13T00:04:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174656.35 epoch sec; seqNo:9387; 4058538.17 north; 591981.97 east 2021-02-13T00:04:16Z- AHRS_M2 msg: 1613174656.73 epoch sec; seqNo:9313 2021-02-13T00:04:16Z- AHRS_M2 msg: 0.66 phi; 0.38 theta; -0.15 psi 2021-02-13T00:04:16Z- handleDepth()-Depth_Keller msg: 1613174656745 epoch sec; seqNo:9388; depth 4.77 2021-02-13T00:04:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174656.77 epoch sec; seqNo:9388; 4058538.55 north; 591981.97 east 2021-02-13T00:04:17Z- AHRS_M2 msg: 1613174657.14 epoch sec; seqNo:9314 2021-02-13T00:04:17Z- AHRS_M2 msg: 0.67 phi; 0.36 theta; -0.14 psi 2021-02-13T00:04:17Z- handleDepth()-Depth_Keller msg: 1613174657145 epoch sec; seqNo:9389; depth 4.62 2021-02-13T00:04:17Z- handleDvl() - RDI_Pathfinder msg: 1613174657.16 epoch sec; seqNo:4564 2021-02-13T00:04:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 113.61 , 137.41 , 173.11 2021-02-13T00:04:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.56 , -0.03 , -0.20 2021-02-13T00:04:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174657.17 epoch sec; seqNo:9389; 4058538.92 north; 591981.96 east 2021-02-13T00:04:17Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-13T00:04:17Z- waiting for 1613174654.92, time is 1613174657.17 2021-02-13T00:04:17Z- LcmTrn::updateTrn() equating pose 1613174657.17 and meas times 1613174657.17 2021-02-13T00:04:17Z- LcmTrn::updateTrn() >>>> pose time 1613174657.17, meas time 1613174657.17 <<<< 2021-02-13T00:04:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174657.17 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:04:17Z- TerrainNav::motionUpdate() - currEstimate = (4058539, 591981.96, 4.62) 2021-02-13T00:04:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:04:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:04:17Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174657.17 sec.; No relevant navigation data available 2021-02-13T00:04:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:04:17Z- TNavBF::mmse selected from filter -1 2021-02-13T00:04:17Z- variances([0], [2], and psi_berg) = 719.977532 833.009650 0.000000 2021-02-13T00:04:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:04:17Z- LcmTrn::publishEstimates() - MLE : 4058535.42 591939.46 4.62 2021-02-13T00:04:17Z- LcmTrn::publishEstimates() - MMSE : 4058538.92 591981.96 4.62 2021-02-13T00:04:17Z- LcmTrn::publishEstimates() - COVAR: 719.98 833.01 1.57 0.00 2021-02-13T00:04:17Z- AHRS_M2 msg: 1613174657.54 epoch sec; seqNo:9315 2021-02-13T00:04:17Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.15 psi 2021-02-13T00:04:17Z- handleDepth()-Depth_Keller msg: 1613174657546 epoch sec; seqNo:9390; depth 4.48 2021-02-13T00:04:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174657.56 epoch sec; seqNo:9390; 4058538.92 north; 591981.96 east 2021-02-13T00:04:17Z- AHRS_M2 msg: 1613174657.94 epoch sec; seqNo:9316 2021-02-13T00:04:17Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.18 psi 2021-02-13T00:04:17Z- handleDepth()-Depth_Keller msg: 1613174657952 epoch sec; seqNo:9391; depth 4.34 2021-02-13T00:04:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174657.98 epoch sec; seqNo:9391; 4058538.92 north; 591981.96 east 2021-02-13T00:04:18Z- AHRS_M2 msg: 1613174658.35 epoch sec; seqNo:9317 2021-02-13T00:04:18Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.21 psi 2021-02-13T00:04:18Z- handleDepth()-Depth_Keller msg: 1613174658357 epoch sec; seqNo:9392; depth 4.18 2021-02-13T00:04:18Z- handleDvl() - RDI_Pathfinder msg: 1613174658.37 epoch sec; seqNo:4565 2021-02-13T00:04:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 107.65 , 128.48 , 85.34 2021-02-13T00:04:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.61 , -0.20 , -0.18 2021-02-13T00:04:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174658.39 epoch sec; seqNo:9392; 4058539.30 north; 591981.96 east 2021-02-13T00:04:18Z- AHRS_M2 msg: 1613174658.75 epoch sec; seqNo:9318 2021-02-13T00:04:18Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.23 psi 2021-02-13T00:04:18Z- handleDepth()-Depth_Keller msg: 1613174658756 epoch sec; seqNo:9393; depth 4.03 2021-02-13T00:04:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174658.77 epoch sec; seqNo:9393; 4058539.68 north; 591981.96 east 2021-02-13T00:04:19Z- AHRS_M2 msg: 1613174659.16 epoch sec; seqNo:9319 2021-02-13T00:04:19Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.23 psi 2021-02-13T00:04:19Z- handleDepth()-Depth_Keller msg: 1613174659163 epoch sec; seqNo:9394; depth 3.90 2021-02-13T00:04:19Z- handleDvl() - RDI_Pathfinder msg: 1613174659.17 epoch sec; seqNo:4566 2021-02-13T00:04:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 115.57 , 125.98 , 0.00 2021-02-13T00:04:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.24 , 0.19 2021-02-13T00:04:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174659.18 epoch sec; seqNo:9394; 4058540.06 north; 591981.95 east 2021-02-13T00:04:19Z- AHRS_M2 msg: 1613174659.56 epoch sec; seqNo:9320 2021-02-13T00:04:19Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.21 psi 2021-02-13T00:04:19Z- handleDepth()-Depth_Keller msg: 1613174659564 epoch sec; seqNo:9395; depth 3.79 2021-02-13T00:04:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174659.58 epoch sec; seqNo:9395; 4058540.45 north; 591983.17 east 2021-02-13T00:04:19Z- AHRS_M2 msg: 1613174659.96 epoch sec; seqNo:9321 2021-02-13T00:04:19Z- AHRS_M2 msg: 0.64 phi; 0.38 theta; -0.18 psi 2021-02-13T00:04:19Z- handleDepth()-Depth_Keller msg: 1613174659968 epoch sec; seqNo:9396; depth 3.66 2021-02-13T00:04:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174659.98 epoch sec; seqNo:9396; 4058540.45 north; 591983.17 east 2021-02-13T00:04:20Z- AHRS_M2 msg: 1613174660.37 epoch sec; seqNo:9322 2021-02-13T00:04:20Z- AHRS_M2 msg: 0.65 phi; 0.38 theta; -0.15 psi 2021-02-13T00:04:20Z- handleDepth()-Depth_Keller msg: 1613174660372 epoch sec; seqNo:9397; depth 3.54 2021-02-13T00:04:20Z- handleDvl() - RDI_Pathfinder msg: 1613174660.38 epoch sec; seqNo:4567 2021-02-13T00:04:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 119.56 , 134.43 , 85.34 2021-02-13T00:04:20Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.10 , 0.32 2021-02-13T00:04:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174660.39 epoch sec; seqNo:9397; 4058540.83 north; 591983.17 east 2021-02-13T00:04:20Z- AHRS_M2 msg: 1613174660.77 epoch sec; seqNo:9323 2021-02-13T00:04:20Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.14 psi 2021-02-13T00:04:20Z- handleDepth()-Depth_Keller msg: 1613174660779 epoch sec; seqNo:9398; depth 3.40 2021-02-13T00:04:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174660.79 epoch sec; seqNo:9398; 4058541.21 north; 591983.16 east 2021-02-13T00:04:21Z- AHRS_M2 msg: 1613174661.18 epoch sec; seqNo:9324 2021-02-13T00:04:21Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.14 psi 2021-02-13T00:04:21Z- handleDepth()-Depth_Keller msg: 1613174661183 epoch sec; seqNo:9399; depth 3.27 2021-02-13T00:04:21Z- handleDvl() - RDI_Pathfinder msg: 1613174661.19 epoch sec; seqNo:4568 2021-02-13T00:04:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.31 , 113.67 , 135.99 , 85.41 2021-02-13T00:04:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.10 , 0.12 2021-02-13T00:04:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174661.22 epoch sec; seqNo:9399; 4058541.59 north; 591983.16 east 2021-02-13T00:04:21Z- AHRS_M2 msg: 1613174661.58 epoch sec; seqNo:9325 2021-02-13T00:04:21Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.16 psi 2021-02-13T00:04:21Z- handleDepth()-Depth_Keller msg: 1613174661584 epoch sec; seqNo:9400; depth 3.10 2021-02-13T00:04:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174661.60 epoch sec; seqNo:9400; 4058541.60 north; 591984.38 east 2021-02-13T00:04:21Z- AHRS_M2 msg: 1613174661.99 epoch sec; seqNo:9326 2021-02-13T00:04:21Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.18 psi 2021-02-13T00:04:21Z- handleDepth()-Depth_Keller msg: 1613174661993 epoch sec; seqNo:9401; depth 2.98 2021-02-13T00:04:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174662.01 epoch sec; seqNo:9401; 4058541.98 north; 591984.37 east 2021-02-13T00:04:22Z- LcmTrn::handleMessages() - handling 40 messages took 0.64 ms and 128 calls... 2021-02-13T00:04:22Z- waiting for 1613174660.17, time is 1613174662.01 2021-02-13T00:04:22Z- LcmTrn::updateTrn() >>>> pose time 1613174662.01, meas time 1613174661.19 <<<< 2021-02-13T00:04:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174661.19 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:04:22Z- TerrainNav::motionUpdate() - currEstimate = (4058542, 591984.37, 2.98) 2021-02-13T00:04:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:04:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:04:22Z- TNavBF::Projecting Measurements 2021-02-13T00:04:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:04:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:04:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:04:22Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:04:22Z- TNavFilter::3 good beams. 2021-02-13T00:04:22Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:04:22Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:04:22Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:04:22Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:04:22Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:04:22Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:04:22Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:04:22Z- TNavBF::mmse selected from filter -1 2021-02-13T00:04:22Z- TerrainMapOctree: (vehN, vehE ) = (4058541.46, 591983.97). 2021-02-13T00:04:22Z- TerrainMapOctree: Min Distance = 3531.44. 2021-02-13T00:04:22Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:04:22Z- There is no specific good beam that all particles have in common. 2021-02-13T00:04:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:04:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.74 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 15.15 2021-02-13T00:04:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:04:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.58 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 20.67 2021-02-13T00:04:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:04:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.47 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 29.59 2021-02-13T00:04:22Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:04:22Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:04:22Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:04:22Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:04:22Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:04:22Z- Filter 0 No beams used in subcloud update 2021-02-13T00:04:22Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:04:22Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:04:22Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 719.9 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T00:04:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174661.19 sec. 2021-02-13T00:04:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:04:22Z- TNavBF::mmse selected from filter -1 2021-02-13T00:04:22Z- variances([0], [2], and psi_berg) = 719.889556 833.009592 0.000000 2021-02-13T00:04:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:04:22Z- LcmTrn::publishEstimates() - MLE : 4058538.48 591941.87 2.98 2021-02-13T00:04:22Z- LcmTrn::publishEstimates() - MMSE : 4058541.98 591984.37 2.98 2021-02-13T00:04:22Z- LcmTrn::publishEstimates() - COVAR: 719.89 833.01 1.44 0.00 2021-02-13T00:04:22Z- AHRS_M2 msg: 1613174662.40 epoch sec; seqNo:9327 2021-02-13T00:04:22Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.19 psi 2021-02-13T00:04:22Z- handleDepth()-Depth_Keller msg: 1613174662402 epoch sec; seqNo:9402; depth 2.84 2021-02-13T00:04:22Z- handleDvl() - RDI_Pathfinder msg: 1613174662.42 epoch sec; seqNo:4569 2021-02-13T00:04:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.44 , 109.35 , 133.15 , 0.00 2021-02-13T00:04:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.05 , 0.05 2021-02-13T00:04:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174662.43 epoch sec; seqNo:9402; 4058542.36 north; 591984.37 east 2021-02-13T00:04:22Z- AHRS_M2 msg: 1613174662.80 epoch sec; seqNo:9328 2021-02-13T00:04:22Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.18 psi 2021-02-13T00:04:22Z- handleDepth()-Depth_Keller msg: 1613174662805 epoch sec; seqNo:9403; depth 2.69 2021-02-13T00:04:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174662.82 epoch sec; seqNo:9403; 4058542.36 north; 591984.37 east 2021-02-13T00:04:23Z- AHRS_M2 msg: 1613174663.20 epoch sec; seqNo:9329 2021-02-13T00:04:23Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.18 psi 2021-02-13T00:04:23Z- handleDepth()-Depth_Keller msg: 1613174663203 epoch sec; seqNo:9404; depth 2.57 2021-02-13T00:04:23Z- handleDvl() - RDI_Pathfinder msg: 1613174663.21 epoch sec; seqNo:4570 2021-02-13T00:04:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 115.09 , 132.94 , 0.00 2021-02-13T00:04:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.16 , -0.06 2021-02-13T00:04:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174663.23 epoch sec; seqNo:9404; 4058542.74 north; 591984.37 east 2021-02-13T00:04:23Z- AHRS_M2 msg: 1613174663.60 epoch sec; seqNo:9330 2021-02-13T00:04:23Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.19 psi 2021-02-13T00:04:23Z- handleDepth()-Depth_Keller msg: 1613174663604 epoch sec; seqNo:9405; depth 2.47 2021-02-13T00:04:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174663.62 epoch sec; seqNo:9405; 4058543.12 north; 591984.36 east 2021-02-13T00:04:24Z- AHRS_M2 msg: 1613174664.00 epoch sec; seqNo:9331 2021-02-13T00:04:24Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.20 psi 2021-02-13T00:04:24Z- handleDepth()-Depth_Keller msg: 1613174664013 epoch sec; seqNo:9406; depth 2.34 2021-02-13T00:04:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174664.02 epoch sec; seqNo:9406; 4058543.13 north; 591985.58 east 2021-02-13T00:04:24Z- AHRS_M2 msg: 1613174664.41 epoch sec; seqNo:9332 2021-02-13T00:04:24Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.17 psi 2021-02-13T00:04:24Z- handleDepth()-Depth_Keller msg: 1613174664412 epoch sec; seqNo:9407; depth 2.24 2021-02-13T00:04:24Z- handleDvl() - RDI_Pathfinder msg: 1613174664.42 epoch sec; seqNo:4571 2021-02-13T00:04:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 115.09 , 131.46 , 86.83 2021-02-13T00:04:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , -0.20 2021-02-13T00:04:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174664.43 epoch sec; seqNo:9407; 4058543.51 north; 591985.58 east 2021-02-13T00:04:24Z- AHRS_M2 msg: 1613174664.81 epoch sec; seqNo:9333 2021-02-13T00:04:24Z- AHRS_M2 msg: 0.68 phi; 0.34 theta; -0.15 psi 2021-02-13T00:04:24Z- handleDepth()-Depth_Keller msg: 1613174664818 epoch sec; seqNo:9408; depth 2.12 2021-02-13T00:04:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174664.83 epoch sec; seqNo:9408; 4058543.51 north; 591985.58 east 2021-02-13T00:04:25Z- AHRS_M2 msg: 1613174665.21 epoch sec; seqNo:9334 2021-02-13T00:04:25Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.14 psi 2021-02-13T00:04:25Z- handleDepth()-Depth_Keller msg: 1613174665221 epoch sec; seqNo:9409; depth 2.01 2021-02-13T00:04:25Z- handleDvl() - RDI_Pathfinder msg: 1613174665.24 epoch sec; seqNo:4572 2021-02-13T00:04:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.44 , 115.29 , 133.14 , 0.00 2021-02-13T00:04:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.08 , -0.15 2021-02-13T00:04:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174665.25 epoch sec; seqNo:9409; 4058543.89 north; 591985.57 east 2021-02-13T00:04:25Z- handleDepth()-Depth_Keller msg: 1613174665637 epoch sec; seqNo:9410; depth 1.88 2021-02-13T00:04:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174665.65 epoch sec; seqNo:9410; 4058544.27 north; 591985.57 east 2021-02-13T00:04:26Z- AHRS_M2 msg: 1613174666.02 epoch sec; seqNo:9335 2021-02-13T00:04:26Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-13T00:04:26Z- handleDepth()-Depth_Keller msg: 1613174666029 epoch sec; seqNo:9411; depth 1.74 2021-02-13T00:04:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174666.04 epoch sec; seqNo:9411; 4058544.27 north; 591985.57 east 2021-02-13T00:04:26Z- AHRS_M2 msg: 1613174666.43 epoch sec; seqNo:9336 2021-02-13T00:04:26Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.18 psi 2021-02-13T00:04:26Z- handleDepth()-Depth_Keller msg: 1613174666441 epoch sec; seqNo:9412; depth 1.61 2021-02-13T00:04:26Z- handleDvl() - RDI_Pathfinder msg: 1613174666.45 epoch sec; seqNo:4573 2021-02-13T00:04:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 112.11 , 128.47 , 97.24 2021-02-13T00:04:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.26 , 0.10 2021-02-13T00:04:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174666.46 epoch sec; seqNo:9412; 4058544.65 north; 591985.57 east 2021-02-13T00:04:26Z- AHRS_M2 msg: 1613174666.83 epoch sec; seqNo:9337 2021-02-13T00:04:26Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.16 psi 2021-02-13T00:04:26Z- handleDepth()-Depth_Keller msg: 1613174666839 epoch sec; seqNo:9413; depth 1.48 2021-02-13T00:04:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174666.86 epoch sec; seqNo:9413; 4058545.03 north; 591985.56 east 2021-02-13T00:04:27Z- AHRS_M2 msg: 1613174667.24 epoch sec; seqNo:9338 2021-02-13T00:04:27Z- AHRS_M2 msg: 0.65 phi; 0.40 theta; -0.09 psi 2021-02-13T00:04:27Z- handleDepth()-Depth_Keller msg: 1613174667241 epoch sec; seqNo:9414; depth 1.38 2021-02-13T00:04:27Z- handleDvl() - RDI_Pathfinder msg: 1613174667.26 epoch sec; seqNo:4574 2021-02-13T00:04:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.38 , 118.20 , 133.08 , 86.96 2021-02-13T00:04:27Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , 0.07 , 0.46 2021-02-13T00:04:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174667.28 epoch sec; seqNo:9414; 4058545.42 north; 591986.78 east 2021-02-13T00:04:27Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 132 calls... 2021-02-13T00:04:27Z- waiting for 1613174665.01, time is 1613174667.28 2021-02-13T00:04:27Z- LcmTrn::updateTrn() equating pose 1613174667.28 and meas times 1613174667.28 2021-02-13T00:04:27Z- LcmTrn::updateTrn() >>>> pose time 1613174667.28, meas time 1613174667.28 <<<< 2021-02-13T00:04:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174667.28 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:04:27Z- TerrainNav::motionUpdate() - currEstimate = (4058545, 591986.78, 1.38) 2021-02-13T00:04:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:04:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:04:27Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174667.28 sec.; No relevant navigation data available 2021-02-13T00:04:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:04:27Z- TNavBF::mmse selected from filter -1 2021-02-13T00:04:27Z- variances([0], [2], and psi_berg) = 719.820572 833.009547 0.000000 2021-02-13T00:04:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:04:27Z- LcmTrn::publishEstimates() - MLE : 4058541.92 591944.28 1.38 2021-02-13T00:04:27Z- LcmTrn::publishEstimates() - MMSE : 4058545.42 591986.78 1.38 2021-02-13T00:04:27Z- LcmTrn::publishEstimates() - COVAR: 719.82 833.01 1.65 0.00 2021-02-13T00:04:27Z- AHRS_M2 msg: 1613174667.64 epoch sec; seqNo:9339 2021-02-13T00:04:27Z- AHRS_M2 msg: 0.70 phi; 0.41 theta; -0.02 psi 2021-02-13T00:04:27Z- handleDepth()-Depth_Keller msg: 1613174667644 epoch sec; seqNo:9415; depth 1.29 2021-02-13T00:04:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174667.65 epoch sec; seqNo:9415; 4058545.80 north; 591986.77 east 2021-02-13T00:04:28Z- AHRS_M2 msg: 1613174668.05 epoch sec; seqNo:9340 2021-02-13T00:04:28Z- AHRS_M2 msg: 0.75 phi; 0.38 theta; 0.02 psi 2021-02-13T00:04:28Z- handleDepth()-Depth_Keller msg: 1613174668052 epoch sec; seqNo:9416; depth 1.20 2021-02-13T00:04:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174668.06 epoch sec; seqNo:9416; 4058545.80 north; 591986.77 east 2021-02-13T00:04:28Z- AHRS_M2 msg: 1613174668.45 epoch sec; seqNo:9341 2021-02-13T00:04:28Z- AHRS_M2 msg: 0.75 phi; 0.33 theta; -0.00 psi 2021-02-13T00:04:28Z- handleDepth()-Depth_Keller msg: 1613174668453 epoch sec; seqNo:9417; depth 1.08 2021-02-13T00:04:28Z- handleDvl() - RDI_Pathfinder msg: 1613174668.47 epoch sec; seqNo:4575 2021-02-13T00:04:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 104.82 , 109.28 , 144.98 , 0.00 2021-02-13T00:04:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.74 , -0.33 2021-02-13T00:04:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174668.48 epoch sec; seqNo:9417; 4058545.81 north; 591988.00 east 2021-02-13T00:04:28Z- AHRS_M2 msg: 1613174668.85 epoch sec; seqNo:9342 2021-02-13T00:04:28Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.11 psi 2021-02-13T00:04:28Z- handleDepth()-Depth_Keller msg: 1613174668861 epoch sec; seqNo:9418; depth 0.91 2021-02-13T00:04:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174668.87 epoch sec; seqNo:9418; 4058545.81 north; 591988.00 east 2021-02-13T00:04:29Z- AHRS_M2 msg: 1613174669.26 epoch sec; seqNo:9343 2021-02-13T00:04:29Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.22 psi 2021-02-13T00:04:29Z- handleDepth()-Depth_Keller msg: 1613174669260 epoch sec; seqNo:9419; depth 0.74 2021-02-13T00:04:29Z- handleDvl() - RDI_Pathfinder msg: 1613174669.28 epoch sec; seqNo:4576 2021-02-13T00:04:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.71 , 104.68 , 129.97 , 0.00 2021-02-13T00:04:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.55 , -0.55 , -0.78 2021-02-13T00:04:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174669.29 epoch sec; seqNo:9419; 4058546.19 north; 591987.99 east 2021-02-13T00:04:29Z- AHRS_M2 msg: 1613174669.66 epoch sec; seqNo:9344 2021-02-13T00:04:29Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.25 psi 2021-02-13T00:04:29Z- handleDepth()-Depth_Keller msg: 1613174669666 epoch sec; seqNo:9420; depth 0.55 2021-02-13T00:04:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174669.68 epoch sec; seqNo:9420; 4058546.57 north; 591987.99 east 2021-02-13T00:04:30Z- AHRS_M2 msg: 1613174670.06 epoch sec; seqNo:9345 2021-02-13T00:04:30Z- AHRS_M2 msg: 0.67 phi; 0.30 theta; -0.22 psi 2021-02-13T00:04:30Z- handleDepth()-Depth_Keller msg: 1613174670068 epoch sec; seqNo:9421; depth 0.43 2021-02-13T00:04:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174670.09 epoch sec; seqNo:9421; 4058546.95 north; 591987.98 east 2021-02-13T00:04:30Z- AHRS_M2 msg: 1613174670.47 epoch sec; seqNo:9346 2021-02-13T00:04:30Z- AHRS_M2 msg: 0.69 phi; 0.25 theta; -0.13 psi 2021-02-13T00:04:30Z- handleDepth()-Depth_Keller msg: 1613174670477 epoch sec; seqNo:9422; depth 0.32 2021-02-13T00:04:30Z- handleDvl() - RDI_Pathfinder msg: 1613174670.51 epoch sec; seqNo:4577 2021-02-13T00:04:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 125.51 , 134.43 , 89.80 2021-02-13T00:04:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.57 , 0.59 2021-02-13T00:04:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174670.52 epoch sec; seqNo:9422; 4058547.32 north; 591987.98 east 2021-02-13T00:04:30Z- AHRS_M2 msg: 1613174670.87 epoch sec; seqNo:9347 2021-02-13T00:04:30Z- AHRS_M2 msg: 0.67 phi; 0.17 theta; -0.08 psi 2021-02-13T00:04:30Z- handleDepth()-Depth_Keller msg: 1613174670876 epoch sec; seqNo:9423; depth 0.26 2021-02-13T00:04:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174670.91 epoch sec; seqNo:9423; 4058547.70 north; 591987.98 east 2021-02-13T00:04:31Z- AHRS_M2 msg: 1613174671.30 epoch sec; seqNo:9348 2021-02-13T00:04:31Z- AHRS_M2 msg: 0.63 phi; 0.12 theta; 0.01 psi 2021-02-13T00:04:31Z- handleDepth()-Depth_Keller msg: 1613174671301 epoch sec; seqNo:9424; depth 0.21 2021-02-13T00:04:31Z- handleDvl() - RDI_Pathfinder msg: 1613174671.33 epoch sec; seqNo:4578 2021-02-13T00:04:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 109.14 , 115.09 , 97.24 2021-02-13T00:04:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.38 , 0.95 2021-02-13T00:04:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174671.34 epoch sec; seqNo:9424; 4058548.08 north; 591987.97 east 2021-02-13T00:04:31Z- AHRS_M2 msg: 1613174671.68 epoch sec; seqNo:9349 2021-02-13T00:04:31Z- AHRS_M2 msg: 0.60 phi; 0.04 theta; 0.18 psi 2021-02-13T00:04:31Z- handleDepth()-Depth_Keller msg: 1613174671684 epoch sec; seqNo:9425; depth 0.20 2021-02-13T00:04:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174671.70 epoch sec; seqNo:9425; 4058548.08 north; 591987.97 east 2021-02-13T00:04:32Z- AHRS_M2 msg: 1613174672.08 epoch sec; seqNo:9350 2021-02-13T00:04:32Z- AHRS_M2 msg: 0.54 phi; -0.06 theta; 0.27 psi 2021-02-13T00:04:32Z- handleDepth()-Depth_Keller msg: 1613174672088 epoch sec; seqNo:9426; depth 0.20 2021-02-13T00:04:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174672.10 epoch sec; seqNo:9426; 4058548.46 north; 591987.97 east 2021-02-13T00:04:32Z- AHRS_M2 msg: 1613174672.49 epoch sec; seqNo:9351 2021-02-13T00:04:32Z- AHRS_M2 msg: 0.46 phi; -0.03 theta; 0.27 psi 2021-02-13T00:04:32Z- handleDepth()-Depth_Keller msg: 1613174672492 epoch sec; seqNo:9427; depth 0.16 2021-02-13T00:04:32Z- LcmTrn::handleMessages() - handling 42 messages took 0.64 ms and 129 calls... 2021-02-13T00:04:32Z- waiting for 1613174670.28, time is 1613174672.49 2021-02-13T00:04:32Z- LcmTrn::updateTrn() >>>> pose time 1613174672.49, meas time 1613174671.33 <<<< 2021-02-13T00:04:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174671.33 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:04:32Z- TerrainNav::motionUpdate() - currEstimate = (4058548, 591987.97, 0.16) 2021-02-13T00:04:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:04:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:04:32Z- TNavBF::Projecting Measurements 2021-02-13T00:04:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:04:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:04:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:04:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:04:32Z- TNavFilter::4 good beams. 2021-02-13T00:04:32Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:04:32Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:04:32Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:04:32Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:04:32Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:04:32Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:04:32Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:04:32Z- TNavBF::mmse selected from filter -1 2021-02-13T00:04:32Z- TerrainMapOctree: (vehN, vehE ) = (4058547.79, 591987.70). 2021-02-13T00:04:32Z- TerrainMapOctree: Min Distance = 3524.19. 2021-02-13T00:04:32Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:04:32Z- There is no specific good beam that all particles have in common. 2021-02-13T00:04:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:04:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.78 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-13T00:04:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:04:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.61 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 19.06 2021-02-13T00:04:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:04:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.50 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 21.19 2021-02-13T00:04:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:04:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-13T00:04:32Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:04:32Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:04:32Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:04:32Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:04:32Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:04:32Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:04:32Z- Filter 0 No beams used in subcloud update 2021-02-13T00:04:32Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:04:32Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:04:32Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 719.8 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:04:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174671.33 sec. 2021-02-13T00:04:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:04:32Z- TNavBF::mmse selected from filter -1 2021-02-13T00:04:32Z- variances([0], [2], and psi_berg) = 719.738264 833.009494 0.000000 2021-02-13T00:04:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:04:32Z- LcmTrn::publishEstimates() - MLE : 4058545.96 591945.47 0.16 2021-02-13T00:04:32Z- LcmTrn::publishEstimates() - MMSE : 4058548.46 591987.97 0.16 2021-02-13T00:04:32Z- LcmTrn::publishEstimates() - COVAR: 719.74 833.01 1.33 0.00 2021-02-13T00:04:32Z- handleDvl() - RDI_Pathfinder msg: 1613174672.51 epoch sec; seqNo:4579 2021-02-13T00:04:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.71 , 105.15 , 97.71 2021-02-13T00:04:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.63 , 1.50 , -0.16 2021-02-13T00:04:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174672.52 epoch sec; seqNo:9427; 4058548.47 north; 591989.19 east 2021-02-13T00:04:32Z- AHRS_M2 msg: 1613174672.89 epoch sec; seqNo:9352 2021-02-13T00:04:32Z- AHRS_M2 msg: 0.47 phi; -0.01 theta; 0.08 psi 2021-02-13T00:04:32Z- handleDepth()-Depth_Keller msg: 1613174672898 epoch sec; seqNo:9428; depth 0.11 2021-02-13T00:04:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174672.92 epoch sec; seqNo:9428; 4058548.49 north; 591990.41 east 2021-02-13T00:04:33Z- AHRS_M2 msg: 1613174673.30 epoch sec; seqNo:9353 2021-02-13T00:04:33Z- AHRS_M2 msg: 0.49 phi; -0.00 theta; -0.09 psi 2021-02-13T00:04:33Z- handleDepth()-Depth_Keller msg: 1613174673301 epoch sec; seqNo:9429; depth 0.06 2021-02-13T00:04:33Z- handleDvl() - RDI_Pathfinder msg: 1613174673.33 epoch sec; seqNo:4580 2021-02-13T00:04:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 107.86 , 97.04 , 101.10 , 98.39 2021-02-13T00:04:33Z- handleDvl() - RDI_Pathfinder msg: velocities 1.14 , 0.26 , -0.85 2021-02-13T00:04:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174673.34 epoch sec; seqNo:9429; 4058548.87 north; 591990.40 east 2021-02-13T00:04:33Z- AHRS_M2 msg: 1613174673.70 epoch sec; seqNo:9354 2021-02-13T00:04:33Z- AHRS_M2 msg: 0.48 phi; 0.05 theta; -0.03 psi 2021-02-13T00:04:33Z- handleDepth()-Depth_Keller msg: 1613174673704 epoch sec; seqNo:9430; depth 0.05 2021-02-13T00:04:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:04:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:04:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:04:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:04:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:04:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:04:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:04:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:04:33Z- handleDvl() - RDI_Pathfinder msg: 1613174673.73 epoch sec; seqNo:4581 2021-02-13T00:04:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:04:33Z- handleDvl() - RDI_Pathfinder msg: velocities 1.14 , 0.26 , -0.85 2021-02-13T00:04:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174673.75 epoch sec; seqNo:9430; 4058549.24 north; 591990.40 east 2021-02-13T00:04:34Z- AHRS_M2 msg: 1613174674.10 epoch sec; seqNo:9355 2021-02-13T00:04:34Z- AHRS_M2 msg: 0.46 phi; 0.02 theta; 0.17 psi 2021-02-13T00:04:34Z- handleDepth()-Depth_Keller msg: 1613174674113 epoch sec; seqNo:9431; depth 0.11 2021-02-13T00:04:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174674.15 epoch sec; seqNo:9431; 4058549.62 north; 591990.40 east 2021-02-13T00:04:34Z- AHRS_M2 msg: 1613174674.51 epoch sec; seqNo:9356 2021-02-13T00:04:34Z- AHRS_M2 msg: 0.46 phi; -0.08 theta; 0.27 psi 2021-02-13T00:04:34Z- handleDepth()-Depth_Keller msg: 1613174674514 epoch sec; seqNo:9432; depth 0.11 2021-02-13T00:04:34Z- handleDvl() - RDI_Pathfinder msg: 1613174674.53 epoch sec; seqNo:4582 2021-02-13T00:04:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.68 , 99.68 , 98.32 , 96.97 2021-02-13T00:04:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.53 , 0.01 , 0.14 2021-02-13T00:04:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174674.54 epoch sec; seqNo:9432; 4058549.62 north; 591990.40 east 2021-02-13T00:04:34Z- AHRS_M2 msg: 1613174674.91 epoch sec; seqNo:9357 2021-02-13T00:04:34Z- AHRS_M2 msg: 0.41 phi; -0.18 theta; 0.22 psi 2021-02-13T00:04:34Z- handleDepth()-Depth_Keller msg: 1613174674916 epoch sec; seqNo:9433; depth 0.08 2021-02-13T00:04:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:04:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:04:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:04:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:04:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:04:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:04:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:04:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:04:34Z- handleDvl() - RDI_Pathfinder msg: 1613174674.92 epoch sec; seqNo:4583 2021-02-13T00:04:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:04:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.53 , 0.01 , 0.14 2021-02-13T00:04:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174674.94 epoch sec; seqNo:9433; 4058550.00 north; 591990.39 east 2021-02-13T00:04:35Z- AHRS_M2 msg: 1613174675.32 epoch sec; seqNo:9358 2021-02-13T00:04:35Z- AHRS_M2 msg: 0.39 phi; -0.12 theta; 0.07 psi 2021-02-13T00:04:35Z- handleDepth()-Depth_Keller msg: 1613174675320 epoch sec; seqNo:9434; depth 0.07 2021-02-13T00:04:35Z- handleDvl() - RDI_Pathfinder msg: 1613174675.33 epoch sec; seqNo:4584 2021-02-13T00:04:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.17 , 97.17 , 116.11 2021-02-13T00:04:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.31 , -0.10 , -0.67 2021-02-13T00:04:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174675.36 epoch sec; seqNo:9434; 4058550.00 north; 591990.39 east 2021-02-13T00:04:35Z- AHRS_M2 msg: 1613174675.72 epoch sec; seqNo:9359 2021-02-13T00:04:35Z- AHRS_M2 msg: 0.41 phi; -0.04 theta; 0.02 psi 2021-02-13T00:04:35Z- handleDepth()-Depth_Keller msg: 1613174675724 epoch sec; seqNo:9435; depth 0.05 2021-02-13T00:04:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174675.74 epoch sec; seqNo:9435; 4058550.00 north; 591990.39 east 2021-02-13T00:04:36Z- AHRS_M2 msg: 1613174676.13 epoch sec; seqNo:9360 2021-02-13T00:04:36Z- AHRS_M2 msg: 0.36 phi; -0.07 theta; 0.11 psi 2021-02-13T00:04:36Z- handleDepth()-Depth_Keller msg: 1613174676137 epoch sec; seqNo:9436; depth 0.10 2021-02-13T00:04:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174676.15 epoch sec; seqNo:9436; 4058550.38 north; 591990.39 east 2021-02-13T00:04:36Z- AHRS_M2 msg: 1613174676.53 epoch sec; seqNo:9361 2021-02-13T00:04:36Z- AHRS_M2 msg: 0.30 phi; -0.12 theta; 0.18 psi 2021-02-13T00:04:36Z- handleDepth()-Depth_Keller msg: 1613174676532 epoch sec; seqNo:9437; depth 0.10 2021-02-13T00:04:36Z- handleDvl() - RDI_Pathfinder msg: 1613174676.55 epoch sec; seqNo:4585 2021-02-13T00:04:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 100.76 , 97.78 , 97.78 , 100.76 2021-02-13T00:04:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.28 , -0.05 , 0.75 2021-02-13T00:04:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174676.56 epoch sec; seqNo:9437; 4058550.38 north; 591990.39 east 2021-02-13T00:04:36Z- AHRS_M2 msg: 1613174676.93 epoch sec; seqNo:9362 2021-02-13T00:04:36Z- AHRS_M2 msg: 0.25 phi; -0.10 theta; 0.23 psi 2021-02-13T00:04:36Z- handleDepth()-Depth_Keller msg: 1613174676936 epoch sec; seqNo:9438; depth 0.12 2021-02-13T00:04:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174676.95 epoch sec; seqNo:9438; 4058550.38 north; 591990.39 east 2021-02-13T00:04:37Z- AHRS_M2 msg: 1613174677.34 epoch sec; seqNo:9363 2021-02-13T00:04:37Z- AHRS_M2 msg: 0.22 phi; -0.01 theta; 0.20 psi 2021-02-13T00:04:37Z- handleDepth()-Depth_Keller msg: 1613174677343 epoch sec; seqNo:9439; depth 0.09 2021-02-13T00:04:37Z- handleDvl() - RDI_Pathfinder msg: 1613174677.36 epoch sec; seqNo:4586 2021-02-13T00:04:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 120.10 , 97.11 , 97.11 , 109.28 2021-02-13T00:04:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.65 , -0.13 2021-02-13T00:04:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174677.37 epoch sec; seqNo:9439; 4058550.77 north; 591991.60 east 2021-02-13T00:04:37Z- LcmTrn::handleMessages() - handling 45 messages took 0.64 ms and 133 calls... 2021-02-13T00:04:37Z- waiting for 1613174675.49, time is 1613174677.37 2021-02-13T00:04:37Z- LcmTrn::updateTrn() equating pose 1613174677.37 and meas times 1613174677.37 2021-02-13T00:04:37Z- LcmTrn::updateTrn() >>>> pose time 1613174677.37, meas time 1613174677.37 <<<< 2021-02-13T00:04:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174677.37 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:04:37Z- TerrainNav::motionUpdate() - currEstimate = (4058551, 591991.60, 0.09) 2021-02-13T00:04:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:04:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:04:37Z- TNavBF::Projecting Measurements 2021-02-13T00:04:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:04:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:04:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:04:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:04:37Z- TNavFilter::4 good beams. 2021-02-13T00:04:37Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:04:37Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:04:37Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:04:37Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:04:37Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:04:37Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:04:37Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:04:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:04:37Z- TerrainMapOctree: (vehN, vehE ) = (4058550.77, 591991.60). 2021-02-13T00:04:37Z- TerrainMapOctree: Min Distance = 3519.40. 2021-02-13T00:04:37Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:04:37Z- There is no specific good beam that all particles have in common. 2021-02-13T00:04:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:04:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.55 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 23.08 2021-02-13T00:04:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:04:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.46 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-13T00:04:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:04:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.35 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-13T00:04:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:04:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 19.11 2021-02-13T00:04:37Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:04:37Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:04:37Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:04:37Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:04:37Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:04:37Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:04:37Z- Filter 0 No beams used in subcloud update 2021-02-13T00:04:37Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:04:37Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:04:37Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 719.7 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:04:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174677.37 sec. 2021-02-13T00:04:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:04:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:04:37Z- variances([0], [2], and psi_berg) = 719.668447 833.009449 0.000000 2021-02-13T00:04:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:04:37Z- LcmTrn::publishEstimates() - MLE : 4058548.27 591949.10 0.09 2021-02-13T00:04:37Z- LcmTrn::publishEstimates() - MMSE : 4058550.77 591991.60 0.09 2021-02-13T00:04:37Z- LcmTrn::publishEstimates() - COVAR: 719.67 833.01 1.18 0.00 2021-02-13T00:04:37Z- AHRS_M2 msg: 1613174677.75 epoch sec; seqNo:9364 2021-02-13T00:04:37Z- AHRS_M2 msg: 0.20 phi; 0.03 theta; 0.02 psi 2021-02-13T00:04:37Z- handleDepth()-Depth_Keller msg: 1613174677754 epoch sec; seqNo:9440; depth 0.06 2021-02-13T00:04:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174677.78 epoch sec; seqNo:9440; 4058551.15 north; 591991.60 east 2021-02-13T00:04:38Z- AHRS_M2 msg: 1613174678.14 epoch sec; seqNo:9365 2021-02-13T00:04:38Z- AHRS_M2 msg: 0.23 phi; 0.01 theta; -0.17 psi 2021-02-13T00:04:38Z- handleDepth()-Depth_Keller msg: 1613174678151 epoch sec; seqNo:9441; depth 0.06 2021-02-13T00:04:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174678.19 epoch sec; seqNo:9441; 4058551.53 north; 591991.60 east 2021-02-13T00:04:38Z- AHRS_M2 msg: 1613174678.55 epoch sec; seqNo:9366 2021-02-13T00:04:38Z- AHRS_M2 msg: 0.23 phi; -0.01 theta; -0.18 psi 2021-02-13T00:04:38Z- handleDepth()-Depth_Keller msg: 1613174678553 epoch sec; seqNo:9442; depth 0.05 2021-02-13T00:04:38Z- handleDvl() - RDI_Pathfinder msg: 1613174678.57 epoch sec; seqNo:4587 2021-02-13T00:04:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.58 , 102.04 , 103.53 , 97.58 2021-02-13T00:04:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.48 , -0.08 , -0.85 2021-02-13T00:04:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174678.58 epoch sec; seqNo:9442; 4058551.53 north; 591991.60 east 2021-02-13T00:04:38Z- AHRS_M2 msg: 1613174678.95 epoch sec; seqNo:9367 2021-02-13T00:04:38Z- AHRS_M2 msg: 0.24 phi; -0.01 theta; -0.05 psi 2021-02-13T00:04:38Z- handleDepth()-Depth_Keller msg: 1613174678960 epoch sec; seqNo:9443; depth 0.04 2021-02-13T00:04:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174678.98 epoch sec; seqNo:9443; 4058551.91 north; 591991.59 east 2021-02-13T00:04:39Z- AHRS_M2 msg: 1613174679.38 epoch sec; seqNo:9368 2021-02-13T00:04:39Z- AHRS_M2 msg: 0.19 phi; -0.06 theta; 0.29 psi 2021-02-13T00:04:39Z- handleDepth()-Depth_Keller msg: 1613174679379 epoch sec; seqNo:9444; depth 0.08 2021-02-13T00:04:39Z- handleDvl() - RDI_Pathfinder msg: 1613174679.40 epoch sec; seqNo:4588 2021-02-13T00:04:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 105.15 , 97.04 , 101.10 2021-02-13T00:04:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.34 , -0.93 , 0.22 2021-02-13T00:04:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174679.41 epoch sec; seqNo:9444; 4058551.91 north; 591991.59 east 2021-02-13T00:04:39Z- handleDepth()-Depth_Keller msg: 1613174679782 epoch sec; seqNo:9445; depth 0.09 2021-02-13T00:04:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174679.81 epoch sec; seqNo:9445; 4058552.29 north; 591991.59 east 2021-02-13T00:04:40Z- AHRS_M2 msg: 1613174680.17 epoch sec; seqNo:9369 2021-02-13T00:04:40Z- AHRS_M2 msg: 0.25 phi; 0.02 theta; 0.29 psi 2021-02-13T00:04:40Z- handleDepth()-Depth_Keller msg: 1613174680175 epoch sec; seqNo:9446; depth 0.08 2021-02-13T00:04:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174680.21 epoch sec; seqNo:9446; 4058552.28 north; 591990.37 east 2021-02-13T00:04:40Z- handleDepth()-Depth_Keller msg: 1613174680601 epoch sec; seqNo:9447; depth 0.06 2021-02-13T00:04:40Z- handleDvl() - RDI_Pathfinder msg: 1613174680.63 epoch sec; seqNo:4589 2021-02-13T00:04:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 121.38 , 97.04 , 107.86 , 103.80 2021-02-13T00:04:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.37 , -0.34 , 0.98 2021-02-13T00:04:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174680.64 epoch sec; seqNo:9447; 4058552.65 north; 591990.36 east 2021-02-13T00:04:40Z- AHRS_M2 msg: 1613174680.97 epoch sec; seqNo:9370 2021-02-13T00:04:40Z- AHRS_M2 msg: 0.24 phi; -0.07 theta; 0.04 psi 2021-02-13T00:04:40Z- handleDepth()-Depth_Keller msg: 1613174680990 epoch sec; seqNo:9448; depth 0.07 2021-02-13T00:04:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174681.00 epoch sec; seqNo:9448; 4058552.65 north; 591990.36 east 2021-02-13T00:04:41Z- AHRS_M2 msg: 1613174681.38 epoch sec; seqNo:9371 2021-02-13T00:04:41Z- AHRS_M2 msg: 0.24 phi; -0.03 theta; -0.09 psi 2021-02-13T00:04:41Z- handleDepth()-Depth_Keller msg: 1613174681381 epoch sec; seqNo:9449; depth 0.04 2021-02-13T00:04:41Z- handleDvl() - RDI_Pathfinder msg: 1613174681.39 epoch sec; seqNo:4590 2021-02-13T00:04:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 115.30 , 97.44 , 98.93 , 104.88 2021-02-13T00:04:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.08 , 0.62 , -0.04 2021-02-13T00:04:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174681.42 epoch sec; seqNo:9449; 4058552.65 north; 591990.36 east 2021-02-13T00:04:41Z- AHRS_M2 msg: 1613174681.78 epoch sec; seqNo:9372 2021-02-13T00:04:41Z- AHRS_M2 msg: 0.24 phi; -0.03 theta; -0.12 psi 2021-02-13T00:04:41Z- handleDepth()-Depth_Keller msg: 1613174681784 epoch sec; seqNo:9450; depth 0.03 2021-02-13T00:04:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174681.80 epoch sec; seqNo:9450; 4058552.65 north; 591990.36 east 2021-02-13T00:04:42Z- AHRS_M2 msg: 1613174682.18 epoch sec; seqNo:9373 2021-02-13T00:04:42Z- AHRS_M2 msg: 0.22 phi; -0.03 theta; -0.01 psi 2021-02-13T00:04:42Z- handleDepth()-Depth_Keller msg: 1613174682197 epoch sec; seqNo:9451; depth 0.06 2021-02-13T00:04:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174682.21 epoch sec; seqNo:9451; 4058552.67 north; 591991.58 east 2021-02-13T00:04:42Z- AHRS_M2 msg: 1613174682.59 epoch sec; seqNo:9374 2021-02-13T00:04:42Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 0.09 psi 2021-02-13T00:04:42Z- handleDepth()-Depth_Keller msg: 1613174682592 epoch sec; seqNo:9452; depth 0.10 2021-02-13T00:04:42Z- handleDvl() - RDI_Pathfinder msg: 1613174682.61 epoch sec; seqNo:4591 2021-02-13T00:04:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.32 , 106.71 , 99.27 , 100.76 2021-02-13T00:04:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.02 , 0.03 , 0.27 2021-02-13T00:04:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174682.62 epoch sec; seqNo:9452; 4058552.67 north; 591991.58 east 2021-02-13T00:04:42Z- LcmTrn::handleMessages() - handling 42 messages took 0.72 ms and 128 calls... 2021-02-13T00:04:42Z- waiting for 1613174680.37, time is 1613174682.62 2021-02-13T00:04:42Z- LcmTrn::updateTrn() equating pose 1613174682.62 and meas times 1613174682.62 2021-02-13T00:04:42Z- LcmTrn::updateTrn() >>>> pose time 1613174682.62, meas time 1613174682.62 <<<< 2021-02-13T00:04:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174682.62 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:04:42Z- TerrainNav::motionUpdate() - currEstimate = (4058553, 591991.58, 0.10) 2021-02-13T00:04:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:04:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:04:42Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174682.62 sec.; No relevant navigation data available 2021-02-13T00:04:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:04:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:04:42Z- variances([0], [2], and psi_berg) = 719.622201 833.009418 0.000000 2021-02-13T00:04:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:04:42Z- LcmTrn::publishEstimates() - MLE : 4058550.17 591949.08 0.10 2021-02-13T00:04:42Z- LcmTrn::publishEstimates() - MMSE : 4058552.67 591991.58 0.10 2021-02-13T00:04:42Z- LcmTrn::publishEstimates() - COVAR: 719.62 833.01 1.27 0.00 2021-02-13T00:04:42Z- AHRS_M2 msg: 1613174682.99 epoch sec; seqNo:9375 2021-02-13T00:04:42Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 0.12 psi 2021-02-13T00:04:42Z- handleDepth()-Depth_Keller msg: 1613174682996 epoch sec; seqNo:9453; depth 0.09 2021-02-13T00:04:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174683.01 epoch sec; seqNo:9453; 4058552.67 north; 591991.58 east 2021-02-13T00:04:43Z- AHRS_M2 msg: 1613174683.40 epoch sec; seqNo:9376 2021-02-13T00:04:43Z- AHRS_M2 msg: 0.16 phi; 0.07 theta; 0.19 psi 2021-02-13T00:04:43Z- handleDepth()-Depth_Keller msg: 1613174683402 epoch sec; seqNo:9454; depth 0.09 2021-02-13T00:04:43Z- handleDvl() - RDI_Pathfinder msg: 1613174683.41 epoch sec; seqNo:4592 2021-02-13T00:04:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 107.86 , 97.04 , 98.39 , 102.45 2021-02-13T00:04:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.44 , -0.12 , 0.42 2021-02-13T00:04:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174683.42 epoch sec; seqNo:9454; 4058553.05 north; 591991.58 east 2021-02-13T00:04:43Z- AHRS_M2 msg: 1613174683.80 epoch sec; seqNo:9377 2021-02-13T00:04:43Z- AHRS_M2 msg: 0.24 phi; 0.14 theta; 0.25 psi 2021-02-13T00:04:43Z- handleDepth()-Depth_Keller msg: 1613174683815 epoch sec; seqNo:9455; depth 0.10 2021-02-13T00:04:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174683.83 epoch sec; seqNo:9455; 4058553.05 north; 591991.58 east 2021-02-13T00:04:44Z- AHRS_M2 msg: 1613174684.20 epoch sec; seqNo:9378 2021-02-13T00:04:44Z- AHRS_M2 msg: 0.30 phi; 0.06 theta; 0.18 psi 2021-02-13T00:04:44Z- handleDepth()-Depth_Keller msg: 1613174684208 epoch sec; seqNo:9456; depth 0.10 2021-02-13T00:04:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174684.24 epoch sec; seqNo:9456; 4058553.43 north; 591991.58 east 2021-02-13T00:04:44Z- AHRS_M2 msg: 1613174684.61 epoch sec; seqNo:9379 2021-02-13T00:04:44Z- AHRS_M2 msg: 0.30 phi; -0.12 theta; 0.03 psi 2021-02-13T00:04:44Z- handleDepth()-Depth_Keller msg: 1613174684612 epoch sec; seqNo:9457; depth 0.07 2021-02-13T00:04:44Z- handleDvl() - RDI_Pathfinder msg: 1613174684.62 epoch sec; seqNo:4593 2021-02-13T00:04:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 123.27 , 90.54 , 111.37 , 97.98 2021-02-13T00:04:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.55 , 1.01 , 0.40 2021-02-13T00:04:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174684.63 epoch sec; seqNo:9457; 4058553.43 north; 591991.58 east 2021-02-13T00:04:45Z- AHRS_M2 msg: 1613174685.02 epoch sec; seqNo:9380 2021-02-13T00:04:45Z- AHRS_M2 msg: 0.25 phi; -0.16 theta; -0.09 psi 2021-02-13T00:04:45Z- handleDepth()-Depth_Keller msg: 1613174685021 epoch sec; seqNo:9458; depth 0.03 2021-02-13T00:04:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174685.03 epoch sec; seqNo:9458; 4058553.43 north; 591991.58 east 2021-02-13T00:04:45Z- AHRS_M2 msg: 1613174685.42 epoch sec; seqNo:9381 2021-02-13T00:04:45Z- AHRS_M2 msg: 0.20 phi; -0.05 theta; -0.06 psi 2021-02-13T00:04:45Z- handleDepth()-Depth_Keller msg: 1613174685422 epoch sec; seqNo:9459; depth 0.03 2021-02-13T00:04:45Z- handleDvl() - RDI_Pathfinder msg: 1613174685.45 epoch sec; seqNo:4594 2021-02-13T00:04:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.06 , 103.52 , 97.57 , 0.00 2021-02-13T00:04:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.56 , 0.09 , -1.10 2021-02-13T00:04:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174685.46 epoch sec; seqNo:9459; 4058553.80 north; 591991.57 east 2021-02-13T00:04:45Z- AHRS_M2 msg: 1613174685.82 epoch sec; seqNo:9382 2021-02-13T00:04:45Z- AHRS_M2 msg: 0.21 phi; 0.03 theta; 0.01 psi 2021-02-13T00:04:45Z- handleDepth()-Depth_Keller msg: 1613174685824 epoch sec; seqNo:9460; depth 0.08 2021-02-13T00:04:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174685.84 epoch sec; seqNo:9460; 4058553.80 north; 591991.57 east 2021-02-13T00:04:46Z- AHRS_M2 msg: 1613174686.22 epoch sec; seqNo:9383 2021-02-13T00:04:46Z- AHRS_M2 msg: 0.27 phi; 0.02 theta; 0.06 psi 2021-02-13T00:04:46Z- handleDepth()-Depth_Keller msg: 1613174686233 epoch sec; seqNo:9461; depth 0.16 2021-02-13T00:04:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174686.24 epoch sec; seqNo:9461; 4058554.20 north; 591992.79 east 2021-02-13T00:04:46Z- AHRS_M2 msg: 1613174686.63 epoch sec; seqNo:9384 2021-02-13T00:04:46Z- AHRS_M2 msg: 0.30 phi; -0.04 theta; 0.04 psi 2021-02-13T00:04:46Z- handleDepth()-Depth_Keller msg: 1613174686634 epoch sec; seqNo:9462; depth 0.21 2021-02-13T00:04:46Z- handleDvl() - RDI_Pathfinder msg: 1613174686.65 epoch sec; seqNo:4595 2021-02-13T00:04:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 98.39 , 99.74 , 97.04 2021-02-13T00:04:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.06 , -0.32 2021-02-13T00:04:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174686.66 epoch sec; seqNo:9462; 4058554.57 north; 591992.78 east 2021-02-13T00:04:47Z- AHRS_M2 msg: 1613174687.03 epoch sec; seqNo:9385 2021-02-13T00:04:47Z- AHRS_M2 msg: 0.31 phi; -0.05 theta; 0.01 psi 2021-02-13T00:04:47Z- handleDepth()-Depth_Keller msg: 1613174687036 epoch sec; seqNo:9463; depth 0.24 2021-02-13T00:04:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174687.06 epoch sec; seqNo:9463; 4058554.95 north; 591992.78 east 2021-02-13T00:04:47Z- AHRS_M2 msg: 1613174687.44 epoch sec; seqNo:9386 2021-02-13T00:04:47Z- AHRS_M2 msg: 0.33 phi; -0.03 theta; -0.02 psi 2021-02-13T00:04:47Z- handleDepth()-Depth_Keller msg: 1613174687440 epoch sec; seqNo:9464; depth 0.27 2021-02-13T00:04:47Z- handleDvl() - RDI_Pathfinder msg: 1613174687.45 epoch sec; seqNo:4596 2021-02-13T00:04:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.23 , 95.82 , 97.17 , 101.23 2021-02-13T00:04:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.05 , -0.24 , -0.25 2021-02-13T00:04:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174687.48 epoch sec; seqNo:9464; 4058554.95 north; 591992.78 east 2021-02-13T00:04:47Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-13T00:04:47Z- waiting for 1613174685.62, time is 1613174687.48 2021-02-13T00:04:47Z- LcmTrn::updateTrn() equating pose 1613174687.48 and meas times 1613174687.48 2021-02-13T00:04:47Z- LcmTrn::updateTrn() >>>> pose time 1613174687.48, meas time 1613174687.48 <<<< 2021-02-13T00:04:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174687.48 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:04:47Z- TerrainNav::motionUpdate() - currEstimate = (4058555, 591992.78, 0.27) 2021-02-13T00:04:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:04:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:04:47Z- TNavBF::Projecting Measurements 2021-02-13T00:04:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:04:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:04:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:04:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:04:47Z- TNavFilter::4 good beams. 2021-02-13T00:04:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:04:47Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:04:47Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:04:47Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:04:47Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:04:47Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:04:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:04:47Z- TNavBF::mmse selected from filter -1 2021-02-13T00:04:47Z- TerrainMapOctree: (vehN, vehE ) = (4058554.95, 591992.78). 2021-02-13T00:04:47Z- TerrainMapOctree: Min Distance = 3515.44. 2021-02-13T00:04:47Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:04:47Z- There is no specific good beam that all particles have in common. 2021-02-13T00:04:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:04:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 16.40 2021-02-13T00:04:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:04:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 14.69 2021-02-13T00:04:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:04:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 15.11 2021-02-13T00:04:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:04:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 16.40 2021-02-13T00:04:47Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:04:47Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:04:47Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:04:47Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:04:47Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:04:47Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:04:47Z- Filter 0 No beams used in subcloud update 2021-02-13T00:04:47Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:04:47Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:04:47Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 719.6 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:04:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174687.48 sec. 2021-02-13T00:04:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:04:47Z- TNavBF::mmse selected from filter -1 2021-02-13T00:04:47Z- variances([0], [2], and psi_berg) = 719.568256 833.009383 0.000000 2021-02-13T00:04:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:04:47Z- LcmTrn::publishEstimates() - MLE : 4058552.45 591950.28 0.27 2021-02-13T00:04:47Z- LcmTrn::publishEstimates() - MMSE : 4058554.95 591992.78 0.27 2021-02-13T00:04:47Z- LcmTrn::publishEstimates() - COVAR: 719.57 833.01 1.06 0.00 2021-02-13T00:04:47Z- AHRS_M2 msg: 1613174687.84 epoch sec; seqNo:9387 2021-02-13T00:04:47Z- AHRS_M2 msg: 0.34 phi; -0.09 theta; -0.04 psi 2021-02-13T00:04:47Z- handleDepth()-Depth_Keller msg: 1613174687844 epoch sec; seqNo:9465; depth 0.30 2021-02-13T00:04:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174687.86 epoch sec; seqNo:9465; 4058554.95 north; 591992.78 east 2021-02-13T00:04:48Z- AHRS_M2 msg: 1613174688.24 epoch sec; seqNo:9388 2021-02-13T00:04:48Z- AHRS_M2 msg: 0.33 phi; -0.17 theta; -0.00 psi 2021-02-13T00:04:48Z- handleDepth()-Depth_Keller msg: 1613174688257 epoch sec; seqNo:9466; depth 0.31 2021-02-13T00:04:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174688.27 epoch sec; seqNo:9466; 4058554.94 north; 591991.56 east 2021-02-13T00:04:48Z- AHRS_M2 msg: 1613174688.65 epoch sec; seqNo:9389 2021-02-13T00:04:48Z- AHRS_M2 msg: 0.30 phi; -0.21 theta; 0.07 psi 2021-02-13T00:04:48Z- handleDepth()-Depth_Keller msg: 1613174688652 epoch sec; seqNo:9467; depth 0.33 2021-02-13T00:04:48Z- handleDvl() - RDI_Pathfinder msg: 1613174688.66 epoch sec; seqNo:4597 2021-02-13T00:04:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 98.46 , 97.11 , 102.52 2021-02-13T00:04:48Z- handleDvl() - RDI_Pathfinder msg: velocities -0.44 , -0.33 , 0.20 2021-02-13T00:04:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174688.68 epoch sec; seqNo:9467; 4058554.94 north; 591991.56 east 2021-02-13T00:04:49Z- AHRS_M2 msg: 1613174689.05 epoch sec; seqNo:9390 2021-02-13T00:04:49Z- AHRS_M2 msg: 0.27 phi; -0.21 theta; 0.15 psi 2021-02-13T00:04:49Z- handleDepth()-Depth_Keller msg: 1613174689058 epoch sec; seqNo:9468; depth 0.37 2021-02-13T00:04:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174689.07 epoch sec; seqNo:9468; 4058554.56 north; 591991.56 east 2021-02-13T00:04:49Z- AHRS_M2 msg: 1613174689.46 epoch sec; seqNo:9391 2021-02-13T00:04:49Z- AHRS_M2 msg: 0.26 phi; -0.18 theta; 0.13 psi 2021-02-13T00:04:49Z- handleDepth()-Depth_Keller msg: 1613174689469 epoch sec; seqNo:9469; depth 0.37 2021-02-13T00:04:49Z- handleDvl() - RDI_Pathfinder msg: 1613174689.48 epoch sec; seqNo:4598 2021-02-13T00:04:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 107.93 , 97.11 , 97.11 , 114.69 2021-02-13T00:04:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.09 , 0.23 , 0.21 2021-02-13T00:04:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174689.49 epoch sec; seqNo:9469; 4058554.56 north; 591991.56 east 2021-02-13T00:04:49Z- AHRS_M2 msg: 1613174689.86 epoch sec; seqNo:9392 2021-02-13T00:04:49Z- AHRS_M2 msg: 0.26 phi; -0.12 theta; 0.05 psi 2021-02-13T00:04:49Z- handleDepth()-Depth_Keller msg: 1613174689874 epoch sec; seqNo:9470; depth 0.38 2021-02-13T00:04:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174689.88 epoch sec; seqNo:9470; 4058554.56 north; 591991.56 east 2021-02-13T00:04:50Z- AHRS_M2 msg: 1613174690.26 epoch sec; seqNo:9393 2021-02-13T00:04:50Z- AHRS_M2 msg: 0.27 phi; -0.05 theta; 0.00 psi 2021-02-13T00:04:50Z- handleDepth()-Depth_Keller msg: 1613174690274 epoch sec; seqNo:9471; depth 0.40 2021-02-13T00:04:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174690.28 epoch sec; seqNo:9471; 4058554.56 north; 591991.56 east 2021-02-13T00:04:50Z- AHRS_M2 msg: 1613174690.67 epoch sec; seqNo:9394 2021-02-13T00:04:50Z- AHRS_M2 msg: 0.32 phi; -0.05 theta; -0.05 psi 2021-02-13T00:04:50Z- handleDepth()-Depth_Keller msg: 1613174690677 epoch sec; seqNo:9472; depth 0.42 2021-02-13T00:04:50Z- handleDvl() - RDI_Pathfinder msg: 1613174690.69 epoch sec; seqNo:4599 2021-02-13T00:04:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 105.22 , 97.78 , 97.78 , 105.22 2021-02-13T00:04:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.31 , -0.13 2021-02-13T00:04:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174690.70 epoch sec; seqNo:9472; 4058554.95 north; 591992.78 east 2021-02-13T00:04:51Z- AHRS_M2 msg: 1613174691.07 epoch sec; seqNo:9395 2021-02-13T00:04:51Z- AHRS_M2 msg: 0.37 phi; -0.14 theta; -0.05 psi 2021-02-13T00:04:51Z- handleDepth()-Depth_Keller msg: 1613174691085 epoch sec; seqNo:9473; depth 0.46 2021-02-13T00:04:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174691.10 epoch sec; seqNo:9473; 4058555.33 north; 591992.78 east 2021-02-13T00:04:51Z- AHRS_M2 msg: 1613174691.48 epoch sec; seqNo:9396 2021-02-13T00:04:51Z- AHRS_M2 msg: 0.36 phi; -0.22 theta; -0.02 psi 2021-02-13T00:04:51Z- handleDepth()-Depth_Keller msg: 1613174691480 epoch sec; seqNo:9474; depth 0.50 2021-02-13T00:04:51Z- handleDvl() - RDI_Pathfinder msg: 1613174691.50 epoch sec; seqNo:4600 2021-02-13T00:04:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 99.47 , 97.99 , 102.45 2021-02-13T00:04:51Z- handleDvl() - RDI_Pathfinder msg: velocities -0.03 , 0.29 , -0.63 2021-02-13T00:04:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174691.51 epoch sec; seqNo:9474; 4058555.33 north; 591992.78 east 2021-02-13T00:04:51Z- AHRS_M2 msg: 1613174691.88 epoch sec; seqNo:9397 2021-02-13T00:04:51Z- AHRS_M2 msg: 0.31 phi; -0.20 theta; -0.02 psi 2021-02-13T00:04:51Z- handleDepth()-Depth_Keller msg: 1613174691884 epoch sec; seqNo:9475; depth 0.49 2021-02-13T00:04:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174691.89 epoch sec; seqNo:9475; 4058555.33 north; 591992.78 east 2021-02-13T00:04:52Z- AHRS_M2 msg: 1613174692.29 epoch sec; seqNo:9398 2021-02-13T00:04:52Z- AHRS_M2 msg: 0.31 phi; -0.15 theta; -0.08 psi 2021-02-13T00:04:52Z- handleDepth()-Depth_Keller msg: 1613174692290 epoch sec; seqNo:9476; depth 0.45 2021-02-13T00:04:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174692.30 epoch sec; seqNo:9476; 4058555.33 north; 591992.78 east 2021-02-13T00:04:52Z- AHRS_M2 msg: 1613174692.69 epoch sec; seqNo:9399 2021-02-13T00:04:52Z- AHRS_M2 msg: 0.35 phi; -0.15 theta; -0.08 psi 2021-02-13T00:04:52Z- handleDepth()-Depth_Keller msg: 1613174692692 epoch sec; seqNo:9477; depth 0.42 2021-02-13T00:04:52Z- handleDvl() - RDI_Pathfinder msg: 1613174692.71 epoch sec; seqNo:4601 2021-02-13T00:04:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.46 , 98.46 , 97.11 , 111.98 2021-02-13T00:04:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.38 , -0.91 , -0.37 2021-02-13T00:04:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174692.72 epoch sec; seqNo:9477; 4058555.33 north; 591992.78 east 2021-02-13T00:04:52Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-13T00:04:52Z- waiting for 1613174690.48, time is 1613174692.72 2021-02-13T00:04:52Z- LcmTrn::updateTrn() equating pose 1613174692.72 and meas times 1613174692.72 2021-02-13T00:04:52Z- LcmTrn::updateTrn() >>>> pose time 1613174692.72, meas time 1613174692.72 <<<< 2021-02-13T00:04:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174692.72 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:04:52Z- TerrainNav::motionUpdate() - currEstimate = (4058555, 591992.78, 0.42) 2021-02-13T00:04:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:04:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:04:52Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174692.72 sec.; No relevant navigation data available 2021-02-13T00:04:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:04:52Z- TNavBF::mmse selected from filter -1 2021-02-13T00:04:52Z- variances([0], [2], and psi_berg) = 719.547599 833.009370 0.000000 2021-02-13T00:04:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:04:52Z- LcmTrn::publishEstimates() - MLE : 4058552.83 591950.28 0.42 2021-02-13T00:04:52Z- LcmTrn::publishEstimates() - MMSE : 4058555.33 591992.78 0.42 2021-02-13T00:04:52Z- LcmTrn::publishEstimates() - COVAR: 719.55 833.01 1.08 0.00 2021-02-13T00:04:53Z- AHRS_M2 msg: 1613174693.09 epoch sec; seqNo:9400 2021-02-13T00:04:53Z- AHRS_M2 msg: 0.35 phi; -0.23 theta; 0.04 psi 2021-02-13T00:04:53Z- handleDepth()-Depth_Keller msg: 1613174693096 epoch sec; seqNo:9478; depth 0.45 2021-02-13T00:04:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174693.11 epoch sec; seqNo:9478; 4058555.71 north; 591992.77 east 2021-02-13T00:04:53Z- AHRS_M2 msg: 1613174693.50 epoch sec; seqNo:9401 2021-02-13T00:04:53Z- AHRS_M2 msg: 0.32 phi; -0.31 theta; 0.19 psi 2021-02-13T00:04:53Z- handleDepth()-Depth_Keller msg: 1613174693500 epoch sec; seqNo:9479; depth 0.49 2021-02-13T00:04:53Z- handleDvl() - RDI_Pathfinder msg: 1613174693.51 epoch sec; seqNo:4602 2021-02-13T00:04:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 100.96 , 97.99 , 0.00 2021-02-13T00:04:53Z- handleDvl() - RDI_Pathfinder msg: velocities -0.17 , -0.33 , 1.06 2021-02-13T00:04:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174693.52 epoch sec; seqNo:9479; 4058555.70 north; 591991.55 east 2021-02-13T00:04:53Z- AHRS_M2 msg: 1613174693.90 epoch sec; seqNo:9402 2021-02-13T00:04:53Z- AHRS_M2 msg: 0.29 phi; -0.30 theta; 0.26 psi 2021-02-13T00:04:53Z- handleDepth()-Depth_Keller msg: 1613174693904 epoch sec; seqNo:9480; depth 0.49 2021-02-13T00:04:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174693.93 epoch sec; seqNo:9480; 4058555.70 north; 591991.55 east 2021-02-13T00:04:54Z- AHRS_M2 msg: 1613174694.31 epoch sec; seqNo:9403 2021-02-13T00:04:54Z- AHRS_M2 msg: 0.28 phi; -0.26 theta; 0.16 psi 2021-02-13T00:04:54Z- handleDepth()-Depth_Keller msg: 1613174694321 epoch sec; seqNo:9481; depth 0.45 2021-02-13T00:04:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174694.35 epoch sec; seqNo:9481; 4058555.32 north; 591991.56 east 2021-02-13T00:04:54Z- AHRS_M2 msg: 1613174694.71 epoch sec; seqNo:9404 2021-02-13T00:04:54Z- AHRS_M2 msg: 0.29 phi; -0.25 theta; -0.00 psi 2021-02-13T00:04:54Z- handleDepth()-Depth_Keller msg: 1613174694712 epoch sec; seqNo:9482; depth 0.41 2021-02-13T00:04:54Z- handleDvl() - RDI_Pathfinder msg: 1613174694.73 epoch sec; seqNo:4603 2021-02-13T00:04:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 124.15 , 91.70 , 97.11 , 129.56 2021-02-13T00:04:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.07 , 0.12 , 0.54 2021-02-13T00:04:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174694.74 epoch sec; seqNo:9482; 4058555.32 north; 591991.56 east 2021-02-13T00:04:55Z- AHRS_M2 msg: 1613174695.12 epoch sec; seqNo:9405 2021-02-13T00:04:55Z- AHRS_M2 msg: 0.28 phi; -0.30 theta; -0.09 psi 2021-02-13T00:04:55Z- handleDepth()-Depth_Keller msg: 1613174695120 epoch sec; seqNo:9483; depth 0.42 2021-02-13T00:04:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174695.13 epoch sec; seqNo:9483; 4058555.32 north; 591991.56 east 2021-02-13T00:04:55Z- AHRS_M2 msg: 1613174695.52 epoch sec; seqNo:9406 2021-02-13T00:04:55Z- AHRS_M2 msg: 0.24 phi; -0.36 theta; -0.09 psi 2021-02-13T00:04:55Z- handleDepth()-Depth_Keller msg: 1613174695526 epoch sec; seqNo:9484; depth 0.44 2021-02-13T00:04:55Z- handleDvl() - RDI_Pathfinder msg: 1613174695.54 epoch sec; seqNo:4604 2021-02-13T00:04:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 100.22 , 106.17 , 97.24 , 112.12 2021-02-13T00:04:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.02 , 0.58 , 0.26 2021-02-13T00:04:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174695.55 epoch sec; seqNo:9484; 4058555.32 north; 591991.56 east 2021-02-13T00:04:55Z- AHRS_M2 msg: 1613174695.92 epoch sec; seqNo:9407 2021-02-13T00:04:55Z- AHRS_M2 msg: 0.19 phi; -0.36 theta; -0.08 psi 2021-02-13T00:04:55Z- handleDepth()-Depth_Keller msg: 1613174695926 epoch sec; seqNo:9485; depth 0.43 2021-02-13T00:04:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174695.94 epoch sec; seqNo:9485; 4058555.32 north; 591991.56 east 2021-02-13T00:04:56Z- AHRS_M2 msg: 1613174696.33 epoch sec; seqNo:9408 2021-02-13T00:04:56Z- AHRS_M2 msg: 0.17 phi; -0.28 theta; -0.09 psi 2021-02-13T00:04:56Z- handleDepth()-Depth_Keller msg: 1613174696332 epoch sec; seqNo:9486; depth 0.44 2021-02-13T00:04:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174696.37 epoch sec; seqNo:9486; 4058555.33 north; 591992.78 east 2021-02-13T00:04:56Z- AHRS_M2 msg: 1613174696.73 epoch sec; seqNo:9409 2021-02-13T00:04:56Z- AHRS_M2 msg: 0.20 phi; -0.24 theta; -0.10 psi 2021-02-13T00:04:56Z- handleDepth()-Depth_Keller msg: 1613174696733 epoch sec; seqNo:9487; depth 0.44 2021-02-13T00:04:56Z- handleDvl() - RDI_Pathfinder msg: 1613174696.75 epoch sec; seqNo:4605 2021-02-13T00:04:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 100.75 , 109.68 , 87.37 , 129.01 2021-02-13T00:04:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.37 , 0.45 , -0.14 2021-02-13T00:04:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174696.76 epoch sec; seqNo:9487; 4058555.33 north; 591992.78 east 2021-02-13T00:04:57Z- AHRS_M2 msg: 1613174697.14 epoch sec; seqNo:9410 2021-02-13T00:04:57Z- AHRS_M2 msg: 0.27 phi; -0.36 theta; -0.11 psi 2021-02-13T00:04:57Z- handleDepth()-Depth_Keller msg: 1613174697148 epoch sec; seqNo:9488; depth 0.44 2021-02-13T00:04:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174697.16 epoch sec; seqNo:9488; 4058555.33 north; 591992.78 east 2021-02-13T00:04:57Z- AHRS_M2 msg: 1613174697.54 epoch sec; seqNo:9411 2021-02-13T00:04:57Z- AHRS_M2 msg: 0.21 phi; -0.41 theta; -0.07 psi 2021-02-13T00:04:57Z- handleDepth()-Depth_Keller msg: 1613174697542 epoch sec; seqNo:9489; depth 0.48 2021-02-13T00:04:57Z- handleDvl() - RDI_Pathfinder msg: 1613174697.57 epoch sec; seqNo:4606 2021-02-13T00:04:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.51 , 106.43 , 97.51 , 116.84 2021-02-13T00:04:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.03 , 0.40 , -0.27 2021-02-13T00:04:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174697.58 epoch sec; seqNo:9489; 4058555.33 north; 591992.78 east 2021-02-13T00:04:57Z- LcmTrn::handleMessages() - handling 41 messages took 0.69 ms and 127 calls... 2021-02-13T00:04:57Z- waiting for 1613174695.72, time is 1613174697.58 2021-02-13T00:04:57Z- LcmTrn::updateTrn() equating pose 1613174697.58 and meas times 1613174697.58 2021-02-13T00:04:57Z- LcmTrn::updateTrn() >>>> pose time 1613174697.58, meas time 1613174697.58 <<<< 2021-02-13T00:04:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174697.58 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:04:57Z- TerrainNav::motionUpdate() - currEstimate = (4058555, 591992.78, 0.48) 2021-02-13T00:04:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:04:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:04:57Z- TNavBF::Projecting Measurements 2021-02-13T00:04:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:04:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:04:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:04:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:04:57Z- TNavFilter::4 good beams. 2021-02-13T00:04:57Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:04:57Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:04:57Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:04:57Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:04:57Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:04:57Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:04:57Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:04:57Z- TNavBF::mmse selected from filter -1 2021-02-13T00:04:57Z- TerrainMapOctree: (vehN, vehE ) = (4058555.33, 591992.78). 2021-02-13T00:04:57Z- TerrainMapOctree: Min Distance = 3515.15. 2021-02-13T00:04:57Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:04:57Z- There is no specific good beam that all particles have in common. 2021-02-13T00:04:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:04:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.21 2021-02-13T00:04:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:04:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 18.12 2021-02-13T00:04:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:04:57Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.21 2021-02-13T00:04:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:04:57Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 21.84 2021-02-13T00:04:57Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:04:57Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:04:57Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:04:57Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:04:57Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:04:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:04:57Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:04:57Z- Filter 0 No beams used in subcloud update 2021-02-13T00:04:57Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:04:57Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:04:57Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 719.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:04:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174697.58 sec. 2021-02-13T00:04:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:04:57Z- TNavBF::mmse selected from filter -1 2021-02-13T00:04:57Z- variances([0], [2], and psi_berg) = 719.547599 833.009370 0.000000 2021-02-13T00:04:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:04:57Z- LcmTrn::publishEstimates() - MLE : 4058552.83 591950.28 0.48 2021-02-13T00:04:57Z- LcmTrn::publishEstimates() - MMSE : 4058555.33 591992.78 0.48 2021-02-13T00:04:57Z- LcmTrn::publishEstimates() - COVAR: 719.55 833.01 0.88 0.00 2021-02-13T00:04:57Z- AHRS_M2 msg: 1613174697.94 epoch sec; seqNo:9412 2021-02-13T00:04:57Z- AHRS_M2 msg: 0.17 phi; -0.36 theta; -0.07 psi 2021-02-13T00:04:57Z- handleDepth()-Depth_Keller msg: 1613174697949 epoch sec; seqNo:9490; depth 0.49 2021-02-13T00:04:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174697.98 epoch sec; seqNo:9490; 4058555.33 north; 591992.78 east 2021-02-13T00:04:58Z- AHRS_M2 msg: 1613174698.35 epoch sec; seqNo:9413 2021-02-13T00:04:58Z- AHRS_M2 msg: 0.14 phi; -0.31 theta; -0.08 psi 2021-02-13T00:04:58Z- handleDepth()-Depth_Keller msg: 1613174698354 epoch sec; seqNo:9491; depth 0.48 2021-02-13T00:04:58Z- handleDvl() - RDI_Pathfinder msg: 1613174698.38 epoch sec; seqNo:4607 2021-02-13T00:04:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 102.18 , 112.59 , 87.30 , 0.00 2021-02-13T00:04:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.33 , 0.04 , -0.46 2021-02-13T00:04:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174698.39 epoch sec; seqNo:9491; 4058555.71 north; 591992.77 east 2021-02-13T00:04:58Z- AHRS_M2 msg: 1613174698.75 epoch sec; seqNo:9414 2021-02-13T00:04:58Z- AHRS_M2 msg: 0.14 phi; -0.29 theta; -0.12 psi 2021-02-13T00:04:58Z- handleDepth()-Depth_Keller msg: 1613174698757 epoch sec; seqNo:9492; depth 0.49 2021-02-13T00:04:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174698.79 epoch sec; seqNo:9492; 4058555.71 north; 591992.77 east 2021-02-13T00:04:59Z- AHRS_M2 msg: 1613174699.15 epoch sec; seqNo:9415 2021-02-13T00:04:59Z- AHRS_M2 msg: 0.16 phi; -0.32 theta; -0.21 psi 2021-02-13T00:04:59Z- handleDepth()-Depth_Keller msg: 1613174699158 epoch sec; seqNo:9493; depth 0.45 2021-02-13T00:04:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:04:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:04:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:04:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:04:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:04:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:04:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:04:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:04:59Z- handleDvl() - RDI_Pathfinder msg: 1613174699.17 epoch sec; seqNo:4608 2021-02-13T00:04:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:04:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.33 , 0.04 , -0.46 2021-02-13T00:04:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174699.19 epoch sec; seqNo:9493; 4058556.09 north; 591992.77 east 2021-02-13T00:04:59Z- AHRS_M2 msg: 1613174699.56 epoch sec; seqNo:9416 2021-02-13T00:04:59Z- AHRS_M2 msg: 0.17 phi; -0.34 theta; -0.22 psi 2021-02-13T00:04:59Z- handleDepth()-Depth_Keller msg: 1613174699562 epoch sec; seqNo:9494; depth 0.45 2021-02-13T00:04:59Z- handleDvl() - RDI_Pathfinder msg: 1613174699.57 epoch sec; seqNo:4609 2021-02-13T00:04:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.62 , 109.14 , 0.00 , 124.02 2021-02-13T00:04:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.47 , -0.52 , -0.34 2021-02-13T00:04:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174699.59 epoch sec; seqNo:9494; 4058556.09 north; 591992.77 east 2021-02-13T00:04:59Z- AHRS_M2 msg: 1613174699.96 epoch sec; seqNo:9417 2021-02-13T00:04:59Z- AHRS_M2 msg: 0.20 phi; -0.31 theta; -0.15 psi 2021-02-13T00:04:59Z- handleDepth()-Depth_Keller msg: 1613174699964 epoch sec; seqNo:9495; depth 0.47 2021-02-13T00:04:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174699.97 epoch sec; seqNo:9495; 4058556.47 north; 591992.76 east 2021-02-13T00:05:00Z- AHRS_M2 msg: 1613174700.36 epoch sec; seqNo:9418 2021-02-13T00:05:00Z- AHRS_M2 msg: 0.24 phi; -0.33 theta; -0.01 psi 2021-02-13T00:05:00Z- handleDepth()-Depth_Keller msg: 1613174700368 epoch sec; seqNo:9496; depth 0.45 2021-02-13T00:05:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174700.38 epoch sec; seqNo:9496; 4058556.85 north; 591992.76 east 2021-02-13T00:05:00Z- AHRS_M2 msg: 1613174700.77 epoch sec; seqNo:9419 2021-02-13T00:05:00Z- AHRS_M2 msg: 0.23 phi; -0.40 theta; 0.20 psi 2021-02-13T00:05:00Z- handleDepth()-Depth_Keller msg: 1613174700773 epoch sec; seqNo:9497; depth 0.50 2021-02-13T00:05:00Z- handleDvl() - RDI_Pathfinder msg: 1613174700.79 epoch sec; seqNo:4610 2021-02-13T00:05:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.31 , 118.14 , 97.31 , 133.01 2021-02-13T00:05:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.20 , -0.62 , 0.71 2021-02-13T00:05:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174700.80 epoch sec; seqNo:9497; 4058556.85 north; 591992.76 east 2021-02-13T00:05:01Z- AHRS_M2 msg: 1613174701.18 epoch sec; seqNo:9420 2021-02-13T00:05:01Z- AHRS_M2 msg: 0.17 phi; -0.40 theta; 0.35 psi 2021-02-13T00:05:01Z- handleDepth()-Depth_Keller msg: 1613174701182 epoch sec; seqNo:9498; depth 0.54 2021-02-13T00:05:01Z- handleDvl() - RDI_Pathfinder msg: 1613174701.19 epoch sec; seqNo:4611 2021-02-13T00:05:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:05:01Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:05:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174701.20 epoch sec; seqNo:9498; 4058556.83 north; 591991.54 east 2021-02-13T00:05:01Z- AHRS_M2 msg: 1613174701.58 epoch sec; seqNo:9421 2021-02-13T00:05:01Z- AHRS_M2 msg: 0.12 phi; -0.36 theta; 0.36 psi 2021-02-13T00:05:01Z- handleDepth()-Depth_Keller msg: 1613174701580 epoch sec; seqNo:9499; depth 0.56 2021-02-13T00:05:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174701.60 epoch sec; seqNo:9499; 4058556.83 north; 591991.54 east 2021-02-13T00:05:01Z- AHRS_M2 msg: 1613174701.98 epoch sec; seqNo:9422 2021-02-13T00:05:01Z- AHRS_M2 msg: 0.12 phi; -0.36 theta; 0.16 psi 2021-02-13T00:05:01Z- handleDepth()-Depth_Keller msg: 1613174701984 epoch sec; seqNo:9500; depth 0.54 2021-02-13T00:05:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174701.99 epoch sec; seqNo:9500; 4058556.83 north; 591991.54 east 2021-02-13T00:05:02Z- AHRS_M2 msg: 1613174702.38 epoch sec; seqNo:9423 2021-02-13T00:05:02Z- AHRS_M2 msg: 0.12 phi; -0.38 theta; -0.09 psi 2021-02-13T00:05:02Z- handleDepth()-Depth_Keller msg: 1613174702393 epoch sec; seqNo:9501; depth 0.50 2021-02-13T00:05:02Z- handleDvl() - RDI_Pathfinder msg: 1613174702.40 epoch sec; seqNo:4612 2021-02-13T00:05:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:05:02Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:05:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174702.42 epoch sec; seqNo:9501; 4058556.83 north; 591991.54 east 2021-02-13T00:05:02Z- AHRS_M2 msg: 1613174702.79 epoch sec; seqNo:9424 2021-02-13T00:05:02Z- AHRS_M2 msg: 0.11 phi; -0.36 theta; -0.24 psi 2021-02-13T00:05:02Z- handleDepth()-Depth_Keller msg: 1613174702794 epoch sec; seqNo:9502; depth 0.48 2021-02-13T00:05:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174702.81 epoch sec; seqNo:9502; 4058556.82 north; 591990.32 east 2021-02-13T00:05:02Z- LcmTrn::handleMessages() - handling 45 messages took 0.69 ms and 131 calls... 2021-02-13T00:05:02Z- waiting for 1613174700.58, time is 1613174702.81 2021-02-13T00:05:02Z- LcmTrn::updateTrn() >>>> pose time 1613174702.81, meas time 1613174702.40 <<<< 2021-02-13T00:05:02Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174702.40, ping # 0. 2021-02-13T00:05:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174702.40 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:05:02Z- TerrainNav::motionUpdate() - currEstimate = (4058557, 591990.32, 0.48) 2021-02-13T00:05:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:05:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:05:02Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:05:02Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:05:02Z- TNavBF::Projecting Measurements 2021-02-13T00:05:02Z- TNavFilter::Measurement from time = 1613174702.40 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:05:02Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:05:02Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:05:02Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:05:02Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:05:02Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:05:02Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:05:02Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:05:02Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 719.5 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:05:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:05:02Z- TNavBF::mmse selected from filter -1 2021-02-13T00:05:02Z- variances([0], [2], and psi_berg) = 719.477213 833.009324 0.000000 2021-02-13T00:05:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:05:02Z- LcmTrn::publishEstimates() - MLE : 4058554.32 591947.82 0.48 2021-02-13T00:05:02Z- LcmTrn::publishEstimates() - MMSE : 4058556.82 591990.32 0.48 2021-02-13T00:05:02Z- LcmTrn::publishEstimates() - COVAR: 719.48 833.01 1.03 0.00 2021-02-13T00:05:03Z- AHRS_M2 msg: 1613174703.19 epoch sec; seqNo:9425 2021-02-13T00:05:03Z- AHRS_M2 msg: 0.12 phi; -0.34 theta; -0.26 psi 2021-02-13T00:05:03Z- handleDepth()-Depth_Keller msg: 1613174703196 epoch sec; seqNo:9503; depth 0.46 2021-02-13T00:05:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174703.23 epoch sec; seqNo:9503; 4058556.82 north; 591990.32 east 2021-02-13T00:05:03Z- AHRS_M2 msg: 1613174703.60 epoch sec; seqNo:9426 2021-02-13T00:05:03Z- AHRS_M2 msg: 0.13 phi; -0.34 theta; -0.20 psi 2021-02-13T00:05:03Z- handleDepth()-Depth_Keller msg: 1613174703601 epoch sec; seqNo:9504; depth 0.45 2021-02-13T00:05:03Z- handleDvl() - RDI_Pathfinder msg: 1613174703.63 epoch sec; seqNo:4613 2021-02-13T00:05:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 125.51 , 97.24 , 135.92 2021-02-13T00:05:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.23 , -0.30 , -0.25 2021-02-13T00:05:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174703.64 epoch sec; seqNo:9504; 4058556.82 north; 591990.32 east 2021-02-13T00:05:04Z- AHRS_M2 msg: 1613174704.00 epoch sec; seqNo:9427 2021-02-13T00:05:04Z- AHRS_M2 msg: 0.15 phi; -0.35 theta; -0.11 psi 2021-02-13T00:05:04Z- handleDepth()-Depth_Keller msg: 1613174704004 epoch sec; seqNo:9505; depth 0.48 2021-02-13T00:05:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174704.03 epoch sec; seqNo:9505; 4058557.20 north; 591990.31 east 2021-02-13T00:05:04Z- AHRS_M2 msg: 1613174704.41 epoch sec; seqNo:9428 2021-02-13T00:05:04Z- AHRS_M2 msg: 0.16 phi; -0.35 theta; -0.01 psi 2021-02-13T00:05:04Z- handleDepth()-Depth_Keller msg: 1613174704411 epoch sec; seqNo:9506; depth 0.47 2021-02-13T00:05:04Z- handleDvl() - RDI_Pathfinder msg: 1613174704.43 epoch sec; seqNo:4614 2021-02-13T00:05:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:05:04Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:05:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174704.44 epoch sec; seqNo:9506; 4058557.20 north; 591990.31 east 2021-02-13T00:05:04Z- AHRS_M2 msg: 1613174704.81 epoch sec; seqNo:9429 2021-02-13T00:05:04Z- AHRS_M2 msg: 0.15 phi; -0.36 theta; 0.05 psi 2021-02-13T00:05:04Z- handleDepth()-Depth_Keller msg: 1613174704812 epoch sec; seqNo:9507; depth 0.48 2021-02-13T00:05:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174704.83 epoch sec; seqNo:9507; 4058557.58 north; 591990.31 east 2021-02-13T00:05:05Z- AHRS_M2 msg: 1613174705.21 epoch sec; seqNo:9430 2021-02-13T00:05:05Z- AHRS_M2 msg: 0.14 phi; -0.38 theta; 0.05 psi 2021-02-13T00:05:05Z- handleDepth()-Depth_Keller msg: 1613174705216 epoch sec; seqNo:9508; depth 0.46 2021-02-13T00:05:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:05:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:05:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:05:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:05:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:05:05Z- handleDvl() - RDI_Pathfinder msg: 1613174705.22 epoch sec; seqNo:4615 2021-02-13T00:05:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:05:05Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:05:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174705.23 epoch sec; seqNo:9508; 4058557.58 north; 591990.31 east 2021-02-13T00:05:05Z- AHRS_M2 msg: 1613174705.63 epoch sec; seqNo:9431 2021-02-13T00:05:05Z- AHRS_M2 msg: 0.14 phi; -0.36 theta; 0.07 psi 2021-02-13T00:05:05Z- handleDepth()-Depth_Keller msg: 1613174705637 epoch sec; seqNo:9509; depth 0.46 2021-02-13T00:05:05Z- handleDvl() - RDI_Pathfinder msg: 1613174705.65 epoch sec; seqNo:4616 2021-02-13T00:05:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 107.65 , 101.71 , 86.83 , 134.43 2021-02-13T00:05:05Z- handleDvl() - RDI_Pathfinder msg: velocities -0.11 , -0.30 , 0.46 2021-02-13T00:05:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174705.68 epoch sec; seqNo:9509; 4058557.58 north; 591990.31 east 2021-02-13T00:05:06Z- AHRS_M2 msg: 1613174706.03 epoch sec; seqNo:9432 2021-02-13T00:05:06Z- AHRS_M2 msg: 0.15 phi; -0.35 theta; 0.07 psi 2021-02-13T00:05:06Z- handleDepth()-Depth_Keller msg: 1613174706034 epoch sec; seqNo:9510; depth 0.47 2021-02-13T00:05:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174706.05 epoch sec; seqNo:9510; 4058557.58 north; 591990.31 east 2021-02-13T00:05:06Z- AHRS_M2 msg: 1613174706.43 epoch sec; seqNo:9433 2021-02-13T00:05:06Z- AHRS_M2 msg: 0.17 phi; -0.39 theta; 0.02 psi 2021-02-13T00:05:06Z- handleDepth()-Depth_Keller msg: 1613174706436 epoch sec; seqNo:9511; depth 0.46 2021-02-13T00:05:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:05:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:05:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:05:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:05:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:05:06Z- handleDvl() - RDI_Pathfinder msg: 1613174706.44 epoch sec; seqNo:4617 2021-02-13T00:05:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:05:06Z- handleDvl() - RDI_Pathfinder msg: velocities -0.11 , -0.30 , 0.46 2021-02-13T00:05:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174706.46 epoch sec; seqNo:9511; 4058557.19 north; 591989.09 east 2021-02-13T00:05:06Z- AHRS_M2 msg: 1613174706.84 epoch sec; seqNo:9434 2021-02-13T00:05:06Z- AHRS_M2 msg: 0.18 phi; -0.43 theta; -0.01 psi 2021-02-13T00:05:06Z- handleDepth()-Depth_Keller msg: 1613174706840 epoch sec; seqNo:9512; depth 0.46 2021-02-13T00:05:06Z- handleDvl() - RDI_Pathfinder msg: 1613174706.85 epoch sec; seqNo:4618 2021-02-13T00:05:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 110.83 , 100.42 , 87.03 , 139.10 2021-02-13T00:05:06Z- handleDvl() - RDI_Pathfinder msg: velocities -0.44 , 0.35 , 0.37 2021-02-13T00:05:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174706.87 epoch sec; seqNo:9512; 4058557.20 north; 591990.31 east 2021-02-13T00:05:07Z- AHRS_M2 msg: 1613174707.24 epoch sec; seqNo:9435 2021-02-13T00:05:07Z- AHRS_M2 msg: 0.18 phi; -0.42 theta; -0.06 psi 2021-02-13T00:05:07Z- handleDepth()-Depth_Keller msg: 1613174707249 epoch sec; seqNo:9513; depth 0.47 2021-02-13T00:05:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174707.26 epoch sec; seqNo:9513; 4058556.82 north; 591990.32 east 2021-02-13T00:05:07Z- AHRS_M2 msg: 1613174707.64 epoch sec; seqNo:9436 2021-02-13T00:05:07Z- AHRS_M2 msg: 0.18 phi; -0.40 theta; -0.11 psi 2021-02-13T00:05:07Z- handleDepth()-Depth_Keller msg: 1613174707650 epoch sec; seqNo:9514; depth 0.45 2021-02-13T00:05:07Z- handleDvl() - RDI_Pathfinder msg: 1613174707.66 epoch sec; seqNo:4619 2021-02-13T00:05:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 106.24 , 109.21 , 97.31 , 146.40 2021-02-13T00:05:07Z- handleDvl() - RDI_Pathfinder msg: velocities -0.05 , 0.16 , -0.19 2021-02-13T00:05:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174707.67 epoch sec; seqNo:9514; 4058556.82 north; 591990.32 east 2021-02-13T00:05:07Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-13T00:05:07Z- waiting for 1613174705.81, time is 1613174707.67 2021-02-13T00:05:07Z- LcmTrn::updateTrn() equating pose 1613174707.67 and meas times 1613174707.67 2021-02-13T00:05:07Z- LcmTrn::updateTrn() >>>> pose time 1613174707.67, meas time 1613174707.67 <<<< 2021-02-13T00:05:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174707.67 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:05:07Z- TerrainNav::motionUpdate() - currEstimate = (4058557, 591990.32, 0.45) 2021-02-13T00:05:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:05:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:05:07Z- TNavBF::Projecting Measurements 2021-02-13T00:05:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:05:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:05:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:05:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:05:07Z- TNavFilter::4 good beams. 2021-02-13T00:05:07Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:05:07Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:05:07Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:05:07Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:05:07Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:05:07Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:05:07Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:05:07Z- TNavBF::mmse selected from filter -1 2021-02-13T00:05:07Z- TerrainMapOctree: (vehN, vehE ) = (4058556.82, 591990.32). 2021-02-13T00:05:07Z- TerrainMapOctree: Min Distance = 3515.59. 2021-02-13T00:05:07Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:05:07Z- There is no specific good beam that all particles have in common. 2021-02-13T00:05:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:05:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 18.06 2021-02-13T00:05:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:05:07Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 19.08 2021-02-13T00:05:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:05:07Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 15.15 2021-02-13T00:05:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:05:07Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.024 ExpectedMeasDiff: nan currMeas.covariance: 34.29 2021-02-13T00:05:07Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:05:07Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:05:07Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:05:07Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:05:07Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:05:07Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:05:07Z- Filter 0 No beams used in subcloud update 2021-02-13T00:05:07Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:05:07Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:05:07Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 719.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:05:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174707.67 sec. 2021-02-13T00:05:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:05:07Z- TNavBF::mmse selected from filter -1 2021-02-13T00:05:07Z- variances([0], [2], and psi_berg) = 719.477213 833.009324 0.000000 2021-02-13T00:05:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:05:07Z- LcmTrn::publishEstimates() - MLE : 4058554.32 591947.82 0.45 2021-02-13T00:05:07Z- LcmTrn::publishEstimates() - MMSE : 4058556.82 591990.32 0.45 2021-02-13T00:05:07Z- LcmTrn::publishEstimates() - COVAR: 719.48 833.01 0.86 0.00 2021-02-13T00:05:08Z- AHRS_M2 msg: 1613174708.05 epoch sec; seqNo:9437 2021-02-13T00:05:08Z- AHRS_M2 msg: 0.16 phi; -0.40 theta; -0.10 psi 2021-02-13T00:05:08Z- handleDepth()-Depth_Keller msg: 1613174708052 epoch sec; seqNo:9515; depth 0.45 2021-02-13T00:05:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174708.07 epoch sec; seqNo:9515; 4058556.82 north; 591990.32 east 2021-02-13T00:05:08Z- AHRS_M2 msg: 1613174708.45 epoch sec; seqNo:9438 2021-02-13T00:05:08Z- AHRS_M2 msg: 0.13 phi; -0.33 theta; -0.03 psi 2021-02-13T00:05:08Z- handleDepth()-Depth_Keller msg: 1613174708456 epoch sec; seqNo:9516; depth 0.44 2021-02-13T00:05:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174708.50 epoch sec; seqNo:9516; 4058556.82 north; 591990.32 east 2021-02-13T00:05:08Z- AHRS_M2 msg: 1613174708.86 epoch sec; seqNo:9439 2021-02-13T00:05:08Z- AHRS_M2 msg: 0.14 phi; -0.27 theta; -0.00 psi 2021-02-13T00:05:08Z- handleDepth()-Depth_Keller msg: 1613174708861 epoch sec; seqNo:9517; depth 0.39 2021-02-13T00:05:08Z- handleDvl() - RDI_Pathfinder msg: 1613174708.89 epoch sec; seqNo:4620 2021-02-13T00:05:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.71 , 112.12 , 88.32 , 138.89 2021-02-13T00:05:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.51 , -0.13 , 0.06 2021-02-13T00:05:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174708.91 epoch sec; seqNo:9517; 4058557.20 north; 591990.31 east 2021-02-13T00:05:09Z- AHRS_M2 msg: 1613174709.26 epoch sec; seqNo:9440 2021-02-13T00:05:09Z- AHRS_M2 msg: 0.18 phi; -0.30 theta; 0.01 psi 2021-02-13T00:05:09Z- handleDepth()-Depth_Keller msg: 1613174709273 epoch sec; seqNo:9518; depth 0.37 2021-02-13T00:05:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174709.31 epoch sec; seqNo:9518; 4058557.20 north; 591990.31 east 2021-02-13T00:05:09Z- AHRS_M2 msg: 1613174709.66 epoch sec; seqNo:9441 2021-02-13T00:05:09Z- AHRS_M2 msg: 0.19 phi; -0.40 theta; 0.02 psi 2021-02-13T00:05:09Z- handleDepth()-Depth_Keller msg: 1613174709675 epoch sec; seqNo:9519; depth 0.38 2021-02-13T00:05:09Z- handleDvl() - RDI_Pathfinder msg: 1613174709.72 epoch sec; seqNo:4621 2021-02-13T00:05:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.19 , 101.71 , 97.24 , 118.07 2021-02-13T00:05:09Z- handleDvl() - RDI_Pathfinder msg: velocities -0.10 , -0.14 , 0.21 2021-02-13T00:05:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174709.74 epoch sec; seqNo:9519; 4058557.20 north; 591990.31 east 2021-02-13T00:05:10Z- AHRS_M2 msg: 1613174710.07 epoch sec; seqNo:9442 2021-02-13T00:05:10Z- AHRS_M2 msg: 0.18 phi; -0.47 theta; 0.01 psi 2021-02-13T00:05:10Z- handleDepth()-Depth_Keller msg: 1613174710073 epoch sec; seqNo:9520; depth 0.37 2021-02-13T00:05:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174710.09 epoch sec; seqNo:9520; 4058557.20 north; 591990.31 east 2021-02-13T00:05:10Z- AHRS_M2 msg: 1613174710.47 epoch sec; seqNo:9443 2021-02-13T00:05:10Z- AHRS_M2 msg: 0.13 phi; -0.46 theta; 0.08 psi 2021-02-13T00:05:10Z- handleDepth()-Depth_Keller msg: 1613174710478 epoch sec; seqNo:9521; depth 0.38 2021-02-13T00:05:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174710.49 epoch sec; seqNo:9521; 4058557.20 north; 591990.31 east 2021-02-13T00:05:10Z- AHRS_M2 msg: 1613174710.88 epoch sec; seqNo:9444 2021-02-13T00:05:10Z- AHRS_M2 msg: 0.13 phi; -0.39 theta; 0.11 psi 2021-02-13T00:05:10Z- handleDepth()-Depth_Keller msg: 1613174710882 epoch sec; seqNo:9522; depth 0.36 2021-02-13T00:05:10Z- handleDvl() - RDI_Pathfinder msg: 1613174710.91 epoch sec; seqNo:4622 2021-02-13T00:05:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 114.01 , 109.55 , 85.75 , 0.00 2021-02-13T00:05:10Z- handleDvl() - RDI_Pathfinder msg: velocities -0.25 , 0.23 , -0.15 2021-02-13T00:05:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174710.92 epoch sec; seqNo:9522; 4058556.82 north; 591990.32 east 2021-02-13T00:05:11Z- AHRS_M2 msg: 1613174711.28 epoch sec; seqNo:9445 2021-02-13T00:05:11Z- AHRS_M2 msg: 0.16 phi; -0.39 theta; 0.00 psi 2021-02-13T00:05:11Z- handleDepth()-Depth_Keller msg: 1613174711289 epoch sec; seqNo:9523; depth 0.30 2021-02-13T00:05:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:05:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:05:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:05:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:05:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:05:11Z- handleDvl() - RDI_Pathfinder msg: 1613174711.29 epoch sec; seqNo:4623 2021-02-13T00:05:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:05:11Z- handleDvl() - RDI_Pathfinder msg: velocities -0.25 , 0.23 , -0.15 2021-02-13T00:05:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174711.30 epoch sec; seqNo:9523; 4058556.82 north; 591990.32 east 2021-02-13T00:05:11Z- AHRS_M2 msg: 1613174711.68 epoch sec; seqNo:9446 2021-02-13T00:05:11Z- AHRS_M2 msg: 0.17 phi; -0.44 theta; -0.07 psi 2021-02-13T00:05:11Z- handleDepth()-Depth_Keller msg: 1613174711688 epoch sec; seqNo:9524; depth 0.26 2021-02-13T00:05:11Z- handleDvl() - RDI_Pathfinder msg: 1613174711.71 epoch sec; seqNo:4624 2021-02-13T00:05:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:05:11Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:05:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174711.72 epoch sec; seqNo:9524; 4058556.82 north; 591990.32 east 2021-02-13T00:05:12Z- AHRS_M2 msg: 1613174712.09 epoch sec; seqNo:9447 2021-02-13T00:05:12Z- AHRS_M2 msg: 0.18 phi; -0.49 theta; -0.03 psi 2021-02-13T00:05:12Z- handleDepth()-Depth_Keller msg: 1613174712105 epoch sec; seqNo:9525; depth 0.26 2021-02-13T00:05:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174712.14 epoch sec; seqNo:9525; 4058556.82 north; 591990.32 east 2021-02-13T00:05:12Z- AHRS_M2 msg: 1613174712.49 epoch sec; seqNo:9448 2021-02-13T00:05:12Z- AHRS_M2 msg: 0.12 phi; -0.50 theta; 0.10 psi 2021-02-13T00:05:12Z- handleDepth()-Depth_Keller msg: 1613174712497 epoch sec; seqNo:9526; depth 0.31 2021-02-13T00:05:12Z- handleDvl() - RDI_Pathfinder msg: 1613174712.51 epoch sec; seqNo:4625 2021-02-13T00:05:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:05:12Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:05:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174712.52 epoch sec; seqNo:9526; 4058556.82 north; 591990.32 east 2021-02-13T00:05:12Z- AHRS_M2 msg: 1613174712.90 epoch sec; seqNo:9449 2021-02-13T00:05:12Z- AHRS_M2 msg: 0.07 phi; -0.42 theta; 0.10 psi 2021-02-13T00:05:12Z- handleDepth()-Depth_Keller msg: 1613174712909 epoch sec; seqNo:9527; depth 0.26 2021-02-13T00:05:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174712.92 epoch sec; seqNo:9527; 4058556.44 north; 591990.32 east 2021-02-13T00:05:13Z- LcmTrn::handleMessages() - handling 45 messages took 0.82 ms and 129 calls... 2021-02-13T00:05:13Z- waiting for 1613174710.67, time is 1613174712.92 2021-02-13T00:05:13Z- LcmTrn::updateTrn() >>>> pose time 1613174712.92, meas time 1613174712.51 <<<< 2021-02-13T00:05:13Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174712.51, ping # 0. 2021-02-13T00:05:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174712.51 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:05:13Z- TerrainNav::motionUpdate() - currEstimate = (4058556, 591990.32, 0.26) 2021-02-13T00:05:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:05:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:05:13Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:05:13Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:05:13Z- TNavBF::Projecting Measurements 2021-02-13T00:05:13Z- TNavFilter::Measurement from time = 1613174712.51 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:05:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:05:13Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:05:13Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:05:13Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:05:13Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:05:13Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:05:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:05:13Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 719.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:05:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:05:13Z- TNavBF::mmse selected from filter -1 2021-02-13T00:05:13Z- variances([0], [2], and psi_berg) = 719.451605 833.009308 0.000000 2021-02-13T00:05:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:05:13Z- LcmTrn::publishEstimates() - MLE : 4058553.94 591947.82 0.26 2021-02-13T00:05:13Z- LcmTrn::publishEstimates() - MMSE : 4058556.44 591990.32 0.26 2021-02-13T00:05:13Z- LcmTrn::publishEstimates() - COVAR: 719.45 833.01 0.88 0.00 2021-02-13T00:05:13Z- AHRS_M2 msg: 1613174713.30 epoch sec; seqNo:9450 2021-02-13T00:05:13Z- AHRS_M2 msg: 0.05 phi; -0.38 theta; -0.05 psi 2021-02-13T00:05:13Z- handleDepth()-Depth_Keller msg: 1613174713309 epoch sec; seqNo:9528; depth 0.22 2021-02-13T00:05:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174713.33 epoch sec; seqNo:9528; 4058556.44 north; 591990.32 east 2021-02-13T00:05:13Z- AHRS_M2 msg: 1613174713.70 epoch sec; seqNo:9451 2021-02-13T00:05:13Z- AHRS_M2 msg: 0.04 phi; -0.35 theta; -0.15 psi 2021-02-13T00:05:13Z- handleDepth()-Depth_Keller msg: 1613174713713 epoch sec; seqNo:9529; depth 0.19 2021-02-13T00:05:13Z- handleDvl() - RDI_Pathfinder msg: 1613174713.74 epoch sec; seqNo:4626 2021-02-13T00:05:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 102.31 , 112.73 , 0.00 , 118.68 2021-02-13T00:05:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.57 , -0.18 2021-02-13T00:05:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174713.76 epoch sec; seqNo:9529; 4058556.82 north; 591990.32 east 2021-02-13T00:05:14Z- AHRS_M2 msg: 1613174714.12 epoch sec; seqNo:9452 2021-02-13T00:05:14Z- AHRS_M2 msg: 0.05 phi; -0.26 theta; 0.03 psi 2021-02-13T00:05:14Z- handleDepth()-Depth_Keller msg: 1613174714134 epoch sec; seqNo:9530; depth 0.17 2021-02-13T00:05:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174714.17 epoch sec; seqNo:9530; 4058557.20 north; 591990.31 east 2021-02-13T00:05:14Z- AHRS_M2 msg: 1613174714.52 epoch sec; seqNo:9453 2021-02-13T00:05:14Z- AHRS_M2 msg: 0.03 phi; -0.26 theta; 0.21 psi 2021-02-13T00:05:14Z- handleDepth()-Depth_Keller msg: 1613174714530 epoch sec; seqNo:9531; depth 0.18 2021-02-13T00:05:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174714.56 epoch sec; seqNo:9531; 4058557.58 north; 591990.31 east 2021-02-13T00:05:14Z- AHRS_M2 msg: 1613174714.92 epoch sec; seqNo:9454 2021-02-13T00:05:14Z- AHRS_M2 msg: 0.04 phi; -0.25 theta; 0.22 psi 2021-02-13T00:05:14Z- handleDepth()-Depth_Keller msg: 1613174714924 epoch sec; seqNo:9532; depth 0.20 2021-02-13T00:05:14Z- handleDvl() - RDI_Pathfinder msg: 1613174714.96 epoch sec; seqNo:4627 2021-02-13T00:05:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 107.65 , 97.24 , 125.51 2021-02-13T00:05:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.20 , -0.30 , 0.49 2021-02-13T00:05:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174714.97 epoch sec; seqNo:9532; 4058557.57 north; 591989.09 east 2021-02-13T00:05:15Z- AHRS_M2 msg: 1613174715.32 epoch sec; seqNo:9455 2021-02-13T00:05:15Z- AHRS_M2 msg: 0.05 phi; -0.25 theta; 0.14 psi 2021-02-13T00:05:15Z- handleDepth()-Depth_Keller msg: 1613174715333 epoch sec; seqNo:9533; depth 0.18 2021-02-13T00:05:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174715.34 epoch sec; seqNo:9533; 4058557.57 north; 591989.09 east 2021-02-13T00:05:15Z- AHRS_M2 msg: 1613174715.73 epoch sec; seqNo:9456 2021-02-13T00:05:15Z- AHRS_M2 msg: 0.03 phi; -0.22 theta; 0.02 psi 2021-02-13T00:05:15Z- handleDepth()-Depth_Keller msg: 1613174715737 epoch sec; seqNo:9534; depth 0.19 2021-02-13T00:05:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174715.75 epoch sec; seqNo:9534; 4058557.57 north; 591989.09 east 2021-02-13T00:05:16Z- AHRS_M2 msg: 1613174716.13 epoch sec; seqNo:9457 2021-02-13T00:05:16Z- AHRS_M2 msg: 0.04 phi; -0.18 theta; -0.07 psi 2021-02-13T00:05:16Z- handleDepth()-Depth_Keller msg: 1613174716153 epoch sec; seqNo:9535; depth 0.14 2021-02-13T00:05:16Z- handleDvl() - RDI_Pathfinder msg: 1613174716.17 epoch sec; seqNo:4628 2021-02-13T00:05:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 118.34 , 97.51 , 97.51 , 119.83 2021-02-13T00:05:16Z- handleDvl() - RDI_Pathfinder msg: velocities -0.16 , -0.08 , 0.09 2021-02-13T00:05:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174716.18 epoch sec; seqNo:9535; 4058557.57 north; 591989.09 east 2021-02-13T00:05:16Z- AHRS_M2 msg: 1613174716.54 epoch sec; seqNo:9458 2021-02-13T00:05:16Z- AHRS_M2 msg: 0.04 phi; -0.18 theta; -0.07 psi 2021-02-13T00:05:16Z- handleDepth()-Depth_Keller msg: 1613174716546 epoch sec; seqNo:9536; depth 0.09 2021-02-13T00:05:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174716.57 epoch sec; seqNo:9536; 4058557.57 north; 591989.09 east 2021-02-13T00:05:16Z- AHRS_M2 msg: 1613174716.94 epoch sec; seqNo:9459 2021-02-13T00:05:16Z- AHRS_M2 msg: 0.02 phi; -0.22 theta; -0.00 psi 2021-02-13T00:05:16Z- handleDepth()-Depth_Keller msg: 1613174716944 epoch sec; seqNo:9537; depth 0.10 2021-02-13T00:05:16Z- handleDvl() - RDI_Pathfinder msg: 1613174716.96 epoch sec; seqNo:4629 2021-02-13T00:05:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.38 , 101.84 , 97.38 , 113.74 2021-02-13T00:05:16Z- handleDvl() - RDI_Pathfinder msg: velocities -0.19 , 0.07 , 0.50 2021-02-13T00:05:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174716.97 epoch sec; seqNo:9537; 4058557.57 north; 591989.09 east 2021-02-13T00:05:17Z- AHRS_M2 msg: 1613174717.35 epoch sec; seqNo:9460 2021-02-13T00:05:17Z- AHRS_M2 msg: 0.01 phi; -0.28 theta; 0.05 psi 2021-02-13T00:05:17Z- handleDepth()-Depth_Keller msg: 1613174717350 epoch sec; seqNo:9538; depth 0.13 2021-02-13T00:05:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:05:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:05:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:05:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:05:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:05:17Z- handleDvl() - RDI_Pathfinder msg: 1613174717.35 epoch sec; seqNo:4630 2021-02-13T00:05:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:05:17Z- handleDvl() - RDI_Pathfinder msg: velocities -0.19 , 0.07 , 0.50 2021-02-13T00:05:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174717.36 epoch sec; seqNo:9538; 4058557.19 north; 591989.09 east 2021-02-13T00:05:17Z- AHRS_M2 msg: 1613174717.77 epoch sec; seqNo:9461 2021-02-13T00:05:17Z- AHRS_M2 msg: 6.25 phi; -0.24 theta; 0.08 psi 2021-02-13T00:05:17Z- handleDepth()-Depth_Keller msg: 1613174717773 epoch sec; seqNo:9539; depth 0.13 2021-02-13T00:05:17Z- handleDvl() - RDI_Pathfinder msg: 1613174717.79 epoch sec; seqNo:4631 2021-02-13T00:05:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.87 , 100.89 , 97.92 , 117.26 2021-02-13T00:05:17Z- handleDvl() - RDI_Pathfinder msg: velocities -0.42 , -0.10 , -0.45 2021-02-13T00:05:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174717.80 epoch sec; seqNo:9539; 4058557.19 north; 591989.09 east 2021-02-13T00:05:18Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-13T00:05:18Z- waiting for 1613174715.92, time is 1613174717.80 2021-02-13T00:05:18Z- LcmTrn::updateTrn() equating pose 1613174717.80 and meas times 1613174717.80 2021-02-13T00:05:18Z- LcmTrn::updateTrn() >>>> pose time 1613174717.80, meas time 1613174717.80 <<<< 2021-02-13T00:05:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174717.80 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:05:18Z- TerrainNav::motionUpdate() - currEstimate = (4058557, 591989.09, 0.13) 2021-02-13T00:05:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:05:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:05:18Z- TNavBF::Projecting Measurements 2021-02-13T00:05:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:05:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:05:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:05:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:05:18Z- TNavFilter::4 good beams. 2021-02-13T00:05:18Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:05:18Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:05:18Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:05:18Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:05:18Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:05:18Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:05:18Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:05:18Z- TNavBF::mmse selected from filter -1 2021-02-13T00:05:18Z- TerrainMapOctree: (vehN, vehE ) = (4058557.19, 591989.09). 2021-02-13T00:05:18Z- TerrainMapOctree: Min Distance = 3516.10. 2021-02-13T00:05:18Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:05:18Z- There is no specific good beam that all particles have in common. 2021-02-13T00:05:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:05:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 17.26 2021-02-13T00:05:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:05:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 16.29 2021-02-13T00:05:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:05:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:05:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:05:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.034 ExpectedMeasDiff: nan currMeas.covariance: 22.00 2021-02-13T00:05:18Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:05:18Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:05:18Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:05:18Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:05:18Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:05:18Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:05:18Z- Filter 0 No beams used in subcloud update 2021-02-13T00:05:18Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:05:18Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:05:18Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 719.4 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:05:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174717.80 sec. 2021-02-13T00:05:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:05:18Z- TNavBF::mmse selected from filter -1 2021-02-13T00:05:18Z- variances([0], [2], and psi_berg) = 719.411428 833.009282 0.000000 2021-02-13T00:05:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:05:18Z- LcmTrn::publishEstimates() - MLE : 4058554.69 591946.59 0.13 2021-02-13T00:05:18Z- LcmTrn::publishEstimates() - MMSE : 4058557.19 591989.09 0.13 2021-02-13T00:05:18Z- LcmTrn::publishEstimates() - COVAR: 719.41 833.01 0.80 0.00 2021-02-13T00:05:18Z- AHRS_M2 msg: 1613174718.16 epoch sec; seqNo:9462 2021-02-13T00:05:18Z- AHRS_M2 msg: 6.24 phi; -0.14 theta; 0.09 psi 2021-02-13T00:05:18Z- handleDepth()-Depth_Keller msg: 1613174718160 epoch sec; seqNo:9540; depth 0.09 2021-02-13T00:05:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174718.17 epoch sec; seqNo:9540; 4058557.19 north; 591989.09 east 2021-02-13T00:05:18Z- AHRS_M2 msg: 1613174718.56 epoch sec; seqNo:9463 2021-02-13T00:05:18Z- AHRS_M2 msg: 6.25 phi; -0.09 theta; 0.07 psi 2021-02-13T00:05:18Z- handleDepth()-Depth_Keller msg: 1613174718560 epoch sec; seqNo:9541; depth 0.08 2021-02-13T00:05:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:18Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:05:18Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:05:18Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:05:18Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:05:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:05:18Z- handleDvl() - RDI_Pathfinder msg: 1613174718.58 epoch sec; seqNo:4632 2021-02-13T00:05:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:05:18Z- handleDvl() - RDI_Pathfinder msg: velocities -0.42 , -0.10 , -0.45 2021-02-13T00:05:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174718.59 epoch sec; seqNo:9541; 4058556.81 north; 591989.10 east 2021-02-13T00:05:18Z- AHRS_M2 msg: 1613174718.96 epoch sec; seqNo:9464 2021-02-13T00:05:18Z- AHRS_M2 msg: 6.23 phi; -0.10 theta; 0.07 psi 2021-02-13T00:05:18Z- handleDepth()-Depth_Keller msg: 1613174718967 epoch sec; seqNo:9542; depth 0.07 2021-02-13T00:05:18Z- handleDvl() - RDI_Pathfinder msg: 1613174718.99 epoch sec; seqNo:4633 2021-02-13T00:05:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 105.15 , 97.71 , 97.71 , 108.13 2021-02-13T00:05:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.28 , -0.15 , -0.05 2021-02-13T00:05:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174719.00 epoch sec; seqNo:9542; 4058557.19 north; 591989.09 east 2021-02-13T00:05:19Z- AHRS_M2 msg: 1613174719.36 epoch sec; seqNo:9465 2021-02-13T00:05:19Z- AHRS_M2 msg: 6.18 phi; -0.09 theta; 0.11 psi 2021-02-13T00:05:19Z- handleDepth()-Depth_Keller msg: 1613174719368 epoch sec; seqNo:9543; depth 0.06 2021-02-13T00:05:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174719.39 epoch sec; seqNo:9543; 4058557.19 north; 591989.09 east 2021-02-13T00:05:19Z- AHRS_M2 msg: 1613174719.77 epoch sec; seqNo:9466 2021-02-13T00:05:19Z- AHRS_M2 msg: 6.17 phi; -0.03 theta; 0.15 psi 2021-02-13T00:05:19Z- handleDepth()-Depth_Keller msg: 1613174719775 epoch sec; seqNo:9544; depth 0.08 2021-02-13T00:05:19Z- handleDvl() - RDI_Pathfinder msg: 1613174719.79 epoch sec; seqNo:4634 2021-02-13T00:05:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.87 , 94.94 , 97.92 , 105.36 2021-02-13T00:05:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.06 , -0.20 , 0.63 2021-02-13T00:05:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174719.81 epoch sec; seqNo:9544; 4058557.19 north; 591989.09 east 2021-02-13T00:05:20Z- AHRS_M2 msg: 1613174720.18 epoch sec; seqNo:9467 2021-02-13T00:05:20Z- AHRS_M2 msg: 6.19 phi; 0.00 theta; 0.10 psi 2021-02-13T00:05:20Z- handleDepth()-Depth_Keller msg: 1613174720183 epoch sec; seqNo:9545; depth 0.08 2021-02-13T00:05:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174720.20 epoch sec; seqNo:9545; 4058557.19 north; 591989.09 east 2021-02-13T00:05:20Z- AHRS_M2 msg: 1613174720.60 epoch sec; seqNo:9468 2021-02-13T00:05:20Z- AHRS_M2 msg: 6.23 phi; 0.03 theta; 0.05 psi 2021-02-13T00:05:20Z- handleDepth()-Depth_Keller msg: 1613174720605 epoch sec; seqNo:9546; depth 0.05 2021-02-13T00:05:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174720.61 epoch sec; seqNo:9546; 4058557.19 north; 591989.09 east 2021-02-13T00:05:20Z- AHRS_M2 msg: 1613174720.98 epoch sec; seqNo:9469 2021-02-13T00:05:20Z- AHRS_M2 msg: 6.22 phi; -0.00 theta; 0.03 psi 2021-02-13T00:05:20Z- handleDepth()-Depth_Keller msg: 1613174720984 epoch sec; seqNo:9547; depth 0.03 2021-02-13T00:05:21Z- handleDvl() - RDI_Pathfinder msg: 1613174721.00 epoch sec; seqNo:4635 2021-02-13T00:05:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 110.50 , 96.97 , 99.68 , 99.68 2021-02-13T00:05:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.05 , 0.27 , 0.61 2021-02-13T00:05:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174721.01 epoch sec; seqNo:9547; 4058557.19 north; 591989.09 east 2021-02-13T00:05:21Z- AHRS_M2 msg: 1613174721.38 epoch sec; seqNo:9470 2021-02-13T00:05:21Z- AHRS_M2 msg: 6.20 phi; 0.01 theta; 0.01 psi 2021-02-13T00:05:21Z- handleDepth()-Depth_Keller msg: 1613174721393 epoch sec; seqNo:9548; depth 0.02 2021-02-13T00:05:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174721.40 epoch sec; seqNo:9548; 4058557.19 north; 591989.09 east 2021-02-13T00:05:21Z- AHRS_M2 msg: 1613174721.79 epoch sec; seqNo:9471 2021-02-13T00:05:21Z- AHRS_M2 msg: 6.26 phi; -0.01 theta; 0.14 psi 2021-02-13T00:05:21Z- handleDepth()-Depth_Keller msg: 1613174721792 epoch sec; seqNo:9549; depth 0.03 2021-02-13T00:05:21Z- handleDvl() - RDI_Pathfinder msg: 1613174721.80 epoch sec; seqNo:4636 2021-02-13T00:05:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 100.96 , 100.96 , 100.96 , 99.47 2021-02-13T00:05:21Z- handleDvl() - RDI_Pathfinder msg: velocities -0.36 , -0.28 , 0.50 2021-02-13T00:05:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174721.82 epoch sec; seqNo:9549; 4058557.19 north; 591989.09 east 2021-02-13T00:05:22Z- AHRS_M2 msg: 1613174722.20 epoch sec; seqNo:9472 2021-02-13T00:05:22Z- AHRS_M2 msg: 6.26 phi; -0.10 theta; 0.29 psi 2021-02-13T00:05:22Z- handleDepth()-Depth_Keller msg: 1613174722210 epoch sec; seqNo:9550; depth 0.03 2021-02-13T00:05:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174722.26 epoch sec; seqNo:9550; 4058556.81 north; 591989.10 east 2021-02-13T00:05:22Z- AHRS_M2 msg: 1613174722.60 epoch sec; seqNo:9473 2021-02-13T00:05:22Z- AHRS_M2 msg: 6.14 phi; -0.17 theta; 0.33 psi 2021-02-13T00:05:22Z- handleDepth()-Depth_Keller msg: 1613174722600 epoch sec; seqNo:9551; depth 0.06 2021-02-13T00:05:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174722.63 epoch sec; seqNo:9551; 4058556.81 north; 591989.10 east 2021-02-13T00:05:23Z- AHRS_M2 msg: 1613174723.00 epoch sec; seqNo:9474 2021-02-13T00:05:23Z- AHRS_M2 msg: 6.02 phi; -0.14 theta; 0.17 psi 2021-02-13T00:05:23Z- handleDepth()-Depth_Keller msg: 1613174723004 epoch sec; seqNo:9552; depth 0.05 2021-02-13T00:05:23Z- handleDvl() - RDI_Pathfinder msg: 1613174723.03 epoch sec; seqNo:4637 2021-02-13T00:05:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.97 , 101.03 , 117.26 2021-02-13T00:05:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.23 , 1.57 , 0.36 2021-02-13T00:05:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174723.05 epoch sec; seqNo:9552; 4058556.81 north; 591989.10 east 2021-02-13T00:05:23Z- LcmTrn::handleMessages() - handling 45 messages took 0.67 ms and 132 calls... 2021-02-13T00:05:23Z- waiting for 1613174720.80, time is 1613174723.05 2021-02-13T00:05:23Z- LcmTrn::updateTrn() equating pose 1613174723.05 and meas times 1613174723.05 2021-02-13T00:05:23Z- LcmTrn::updateTrn() >>>> pose time 1613174723.05, meas time 1613174723.05 <<<< 2021-02-13T00:05:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174723.05 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:05:23Z- TerrainNav::motionUpdate() - currEstimate = (4058557, 591989.10, 0.05) 2021-02-13T00:05:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:05:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:05:23Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174723.05 sec.; No relevant navigation data available 2021-02-13T00:05:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:05:23Z- TNavBF::mmse selected from filter -1 2021-02-13T00:05:23Z- variances([0], [2], and psi_berg) = 719.390807 833.009268 0.000000 2021-02-13T00:05:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:05:23Z- LcmTrn::publishEstimates() - MLE : 4058554.31 591946.60 0.05 2021-02-13T00:05:23Z- LcmTrn::publishEstimates() - MMSE : 4058556.81 591989.10 0.05 2021-02-13T00:05:23Z- LcmTrn::publishEstimates() - COVAR: 719.39 833.01 0.82 0.00 2021-02-13T00:05:23Z- AHRS_M2 msg: 1613174723.41 epoch sec; seqNo:9475 2021-02-13T00:05:23Z- AHRS_M2 msg: 6.00 phi; -0.11 theta; -0.05 psi 2021-02-13T00:05:23Z- handleDepth()-Depth_Keller msg: 1613174723410 epoch sec; seqNo:9553; depth 0.03 2021-02-13T00:05:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174723.42 epoch sec; seqNo:9553; 4058557.19 north; 591989.09 east 2021-02-13T00:05:23Z- AHRS_M2 msg: 1613174723.83 epoch sec; seqNo:9476 2021-02-13T00:05:23Z- AHRS_M2 msg: 6.02 phi; -0.06 theta; -0.21 psi 2021-02-13T00:05:23Z- handleDepth()-Depth_Keller msg: 1613174723836 epoch sec; seqNo:9554; depth 0.02 2021-02-13T00:05:23Z- handleDvl() - RDI_Pathfinder msg: 1613174723.85 epoch sec; seqNo:4638 2021-02-13T00:05:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 106.57 , 97.65 , 97.65 , 102.11 2021-02-13T00:05:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.35 , 0.54 , -0.99 2021-02-13T00:05:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174723.86 epoch sec; seqNo:9554; 4058557.58 north; 591990.31 east 2021-02-13T00:05:24Z- AHRS_M2 msg: 1613174724.21 epoch sec; seqNo:9477 2021-02-13T00:05:24Z- AHRS_M2 msg: 6.01 phi; -0.03 theta; -0.24 psi 2021-02-13T00:05:24Z- handleDepth()-Depth_Keller msg: 1613174724216 epoch sec; seqNo:9555; depth 0.04 2021-02-13T00:05:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174724.23 epoch sec; seqNo:9555; 4058557.58 north; 591990.31 east 2021-02-13T00:05:24Z- AHRS_M2 msg: 1613174724.62 epoch sec; seqNo:9478 2021-02-13T00:05:24Z- AHRS_M2 msg: 5.98 phi; 0.04 theta; -0.05 psi 2021-02-13T00:05:24Z- handleDepth()-Depth_Keller msg: 1613174724620 epoch sec; seqNo:9556; depth 0.09 2021-02-13T00:05:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174724.63 epoch sec; seqNo:9556; 4058557.96 north; 591990.31 east 2021-02-13T00:05:25Z- AHRS_M2 msg: 1613174725.02 epoch sec; seqNo:9479 2021-02-13T00:05:25Z- AHRS_M2 msg: 5.97 phi; 0.09 theta; 0.14 psi 2021-02-13T00:05:25Z- handleDepth()-Depth_Keller msg: 1613174725034 epoch sec; seqNo:9557; depth 0.12 2021-02-13T00:05:25Z- handleDvl() - RDI_Pathfinder msg: 1613174725.05 epoch sec; seqNo:4639 2021-02-13T00:05:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 109.14 , 101.03 , 101.03 2021-02-13T00:05:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.60 , -0.46 , -0.22 2021-02-13T00:05:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174725.06 epoch sec; seqNo:9557; 4058557.95 north; 591989.09 east 2021-02-13T00:05:25Z- AHRS_M2 msg: 1613174725.42 epoch sec; seqNo:9480 2021-02-13T00:05:25Z- AHRS_M2 msg: 5.97 phi; 0.06 theta; 0.17 psi 2021-02-13T00:05:25Z- handleDepth()-Depth_Keller msg: 1613174725433 epoch sec; seqNo:9558; depth 0.12 2021-02-13T00:05:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174725.44 epoch sec; seqNo:9558; 4058557.95 north; 591989.09 east 2021-02-13T00:05:25Z- AHRS_M2 msg: 1613174725.83 epoch sec; seqNo:9481 2021-02-13T00:05:25Z- AHRS_M2 msg: 5.95 phi; 0.04 theta; 0.09 psi 2021-02-13T00:05:25Z- handleDepth()-Depth_Keller msg: 1613174725833 epoch sec; seqNo:9559; depth 0.12 2021-02-13T00:05:25Z- handleDvl() - RDI_Pathfinder msg: 1613174725.85 epoch sec; seqNo:4640 2021-02-13T00:05:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 105.36 , 91.97 , 105.36 , 97.92 2021-02-13T00:05:25Z- handleDvl() - RDI_Pathfinder msg: velocities -0.12 , -0.64 , -0.08 2021-02-13T00:05:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174725.86 epoch sec; seqNo:9559; 4058557.95 north; 591989.09 east 2021-02-13T00:05:26Z- AHRS_M2 msg: 1613174726.23 epoch sec; seqNo:9482 2021-02-13T00:05:26Z- AHRS_M2 msg: 5.94 phi; 0.02 theta; -0.04 psi 2021-02-13T00:05:26Z- handleDepth()-Depth_Keller msg: 1613174726239 epoch sec; seqNo:9560; depth 0.07 2021-02-13T00:05:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174726.25 epoch sec; seqNo:9560; 4058557.95 north; 591989.09 east 2021-02-13T00:05:26Z- AHRS_M2 msg: 1613174726.64 epoch sec; seqNo:9483 2021-02-13T00:05:26Z- AHRS_M2 msg: 5.93 phi; -0.06 theta; -0.06 psi 2021-02-13T00:05:26Z- handleDepth()-Depth_Keller msg: 1613174726640 epoch sec; seqNo:9561; depth 0.05 2021-02-13T00:05:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174726.66 epoch sec; seqNo:9561; 4058557.57 north; 591989.09 east 2021-02-13T00:05:27Z- AHRS_M2 msg: 1613174727.04 epoch sec; seqNo:9484 2021-02-13T00:05:27Z- AHRS_M2 msg: 5.92 phi; -0.13 theta; -0.00 psi 2021-02-13T00:05:27Z- handleDepth()-Depth_Keller msg: 1613174727044 epoch sec; seqNo:9562; depth 0.06 2021-02-13T00:05:27Z- handleDvl() - RDI_Pathfinder msg: 1613174727.06 epoch sec; seqNo:4641 2021-02-13T00:05:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 101.10 , 99.74 , 97.04 2021-02-13T00:05:27Z- handleDvl() - RDI_Pathfinder msg: velocities -0.66 , -0.47 , 0.27 2021-02-13T00:05:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174727.07 epoch sec; seqNo:9562; 4058557.19 north; 591989.09 east 2021-02-13T00:05:27Z- AHRS_M2 msg: 1613174727.44 epoch sec; seqNo:9485 2021-02-13T00:05:27Z- AHRS_M2 msg: 5.91 phi; -0.08 theta; 0.08 psi 2021-02-13T00:05:27Z- handleDepth()-Depth_Keller msg: 1613174727450 epoch sec; seqNo:9563; depth 0.07 2021-02-13T00:05:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174727.46 epoch sec; seqNo:9563; 4058557.19 north; 591989.09 east 2021-02-13T00:05:27Z- AHRS_M2 msg: 1613174727.85 epoch sec; seqNo:9486 2021-02-13T00:05:27Z- AHRS_M2 msg: 5.90 phi; -0.01 theta; 0.15 psi 2021-02-13T00:05:27Z- handleDepth()-Depth_Keller msg: 1613174727852 epoch sec; seqNo:9564; depth 0.09 2021-02-13T00:05:27Z- handleDvl() - RDI_Pathfinder msg: 1613174727.87 epoch sec; seqNo:4642 2021-02-13T00:05:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 102.58 , 97.17 , 97.17 , 101.23 2021-02-13T00:05:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.22 , -0.21 , -0.27 2021-02-13T00:05:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174727.88 epoch sec; seqNo:9564; 4058557.19 north; 591989.09 east 2021-02-13T00:05:28Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-13T00:05:28Z- waiting for 1613174726.05, time is 1613174727.88 2021-02-13T00:05:28Z- LcmTrn::updateTrn() equating pose 1613174727.88 and meas times 1613174727.88 2021-02-13T00:05:28Z- LcmTrn::updateTrn() >>>> pose time 1613174727.88, meas time 1613174727.88 <<<< 2021-02-13T00:05:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174727.88 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:05:28Z- TerrainNav::motionUpdate() - currEstimate = (4058557, 591989.09, 0.09) 2021-02-13T00:05:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:05:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:05:28Z- TNavBF::Projecting Measurements 2021-02-13T00:05:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:05:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:05:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:05:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:05:28Z- TNavFilter::4 good beams. 2021-02-13T00:05:28Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:05:28Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:05:28Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:05:28Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:05:28Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:05:28Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:05:28Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:05:28Z- TNavBF::mmse selected from filter -1 2021-02-13T00:05:28Z- TerrainMapOctree: (vehN, vehE ) = (4058557.19, 591989.09). 2021-02-13T00:05:28Z- TerrainMapOctree: Min Distance = 3516.10. 2021-02-13T00:05:28Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:05:28Z- There is no specific good beam that all particles have in common. 2021-02-13T00:05:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:05:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 16.84 2021-02-13T00:05:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:05:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 15.11 2021-02-13T00:05:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:05:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.76 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 15.11 2021-02-13T00:05:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:05:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.73 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 16.40 2021-02-13T00:05:28Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:05:28Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:05:28Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:05:28Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:05:28Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:05:28Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:05:28Z- Filter 0 No beams used in subcloud update 2021-02-13T00:05:28Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:05:28Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:05:28Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 719.4 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:05:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174727.88 sec. 2021-02-13T00:05:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:05:28Z- TNavBF::mmse selected from filter -1 2021-02-13T00:05:28Z- variances([0], [2], and psi_berg) = 719.370191 833.009255 0.000000 2021-02-13T00:05:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:05:28Z- LcmTrn::publishEstimates() - MLE : 4058554.69 591946.59 0.09 2021-02-13T00:05:28Z- LcmTrn::publishEstimates() - MMSE : 4058557.19 591989.09 0.09 2021-02-13T00:05:28Z- LcmTrn::publishEstimates() - COVAR: 719.37 833.01 0.70 0.00 2021-02-13T00:05:28Z- AHRS_M2 msg: 1613174728.25 epoch sec; seqNo:9487 2021-02-13T00:05:28Z- AHRS_M2 msg: 5.86 phi; 0.03 theta; 0.16 psi 2021-02-13T00:05:28Z- handleDepth()-Depth_Keller msg: 1613174728259 epoch sec; seqNo:9565; depth 0.09 2021-02-13T00:05:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174728.27 epoch sec; seqNo:9565; 4058557.17 north; 591987.87 east 2021-02-13T00:05:28Z- AHRS_M2 msg: 1613174728.66 epoch sec; seqNo:9488 2021-02-13T00:05:28Z- AHRS_M2 msg: 5.83 phi; -0.08 theta; 0.13 psi 2021-02-13T00:05:28Z- handleDepth()-Depth_Keller msg: 1613174728660 epoch sec; seqNo:9566; depth 0.08 2021-02-13T00:05:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174728.67 epoch sec; seqNo:9566; 4058557.17 north; 591987.87 east 2021-02-13T00:05:29Z- AHRS_M2 msg: 1613174729.06 epoch sec; seqNo:9489 2021-02-13T00:05:29Z- AHRS_M2 msg: 5.86 phi; -0.17 theta; 0.05 psi 2021-02-13T00:05:29Z- handleDepth()-Depth_Keller msg: 1613174729066 epoch sec; seqNo:9567; depth 0.10 2021-02-13T00:05:29Z- handleDvl() - RDI_Pathfinder msg: 1613174729.08 epoch sec; seqNo:4643 2021-02-13T00:05:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 110.63 , 97.11 , 99.81 , 98.46 2021-02-13T00:05:29Z- handleDvl() - RDI_Pathfinder msg: velocities -0.14 , -0.00 , 0.56 2021-02-13T00:05:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174729.09 epoch sec; seqNo:9567; 4058557.17 north; 591987.87 east 2021-02-13T00:05:29Z- AHRS_M2 msg: 1613174729.46 epoch sec; seqNo:9490 2021-02-13T00:05:29Z- AHRS_M2 msg: 5.85 phi; -0.11 theta; 0.00 psi 2021-02-13T00:05:29Z- handleDepth()-Depth_Keller msg: 1613174729473 epoch sec; seqNo:9568; depth 0.09 2021-02-13T00:05:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174729.49 epoch sec; seqNo:9568; 4058557.17 north; 591987.87 east 2021-02-13T00:05:29Z- AHRS_M2 msg: 1613174729.87 epoch sec; seqNo:9491 2021-02-13T00:05:29Z- AHRS_M2 msg: 5.79 phi; -0.00 theta; -0.02 psi 2021-02-13T00:05:29Z- handleDepth()-Depth_Keller msg: 1613174729872 epoch sec; seqNo:9569; depth 0.05 2021-02-13T00:05:29Z- handleDvl() - RDI_Pathfinder msg: 1613174729.88 epoch sec; seqNo:4644 2021-02-13T00:05:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.47 , 99.47 , 97.99 , 108.40 2021-02-13T00:05:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.35 , 0.34 , 0.28 2021-02-13T00:05:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174729.91 epoch sec; seqNo:9569; 4058557.17 north; 591987.87 east 2021-02-13T00:05:30Z- AHRS_M2 msg: 1613174730.28 epoch sec; seqNo:9492 2021-02-13T00:05:30Z- AHRS_M2 msg: 5.76 phi; 0.03 theta; -0.10 psi 2021-02-13T00:05:30Z- handleDepth()-Depth_Keller msg: 1613174730279 epoch sec; seqNo:9570; depth 0.02 2021-02-13T00:05:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174730.30 epoch sec; seqNo:9570; 4058557.57 north; 591989.09 east 2021-02-13T00:05:30Z- AHRS_M2 msg: 1613174730.68 epoch sec; seqNo:9493 2021-02-13T00:05:30Z- AHRS_M2 msg: 5.73 phi; -0.01 theta; -0.21 psi 2021-02-13T00:05:30Z- handleDepth()-Depth_Keller msg: 1613174730693 epoch sec; seqNo:9571; depth 0.03 2021-02-13T00:05:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174730.70 epoch sec; seqNo:9571; 4058557.57 north; 591989.09 east 2021-02-13T00:05:31Z- AHRS_M2 msg: 1613174731.09 epoch sec; seqNo:9494 2021-02-13T00:05:31Z- AHRS_M2 msg: 5.78 phi; -0.08 theta; -0.13 psi 2021-02-13T00:05:31Z- handleDepth()-Depth_Keller msg: 1613174731095 epoch sec; seqNo:9572; depth 0.06 2021-02-13T00:05:31Z- handleDvl() - RDI_Pathfinder msg: 1613174731.11 epoch sec; seqNo:4645 2021-02-13T00:05:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 114.35 , 103.94 , 99.47 2021-02-13T00:05:31Z- handleDvl() - RDI_Pathfinder msg: velocities -0.55 , -0.27 , 0.03 2021-02-13T00:05:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174731.12 epoch sec; seqNo:9572; 4058557.57 north; 591989.09 east 2021-02-13T00:05:31Z- AHRS_M2 msg: 1613174731.48 epoch sec; seqNo:9495 2021-02-13T00:05:31Z- AHRS_M2 msg: 5.81 phi; -0.01 theta; 0.06 psi 2021-02-13T00:05:31Z- handleDepth()-Depth_Keller msg: 1613174731489 epoch sec; seqNo:9573; depth 0.06 2021-02-13T00:05:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174731.53 epoch sec; seqNo:9573; 4058557.19 north; 591989.09 east 2021-02-13T00:05:31Z- AHRS_M2 msg: 1613174731.89 epoch sec; seqNo:9496 2021-02-13T00:05:31Z- AHRS_M2 msg: 5.82 phi; -0.05 theta; 0.19 psi 2021-02-13T00:05:31Z- handleDepth()-Depth_Keller msg: 1613174731893 epoch sec; seqNo:9574; depth 0.07 2021-02-13T00:05:31Z- handleDvl() - RDI_Pathfinder msg: 1613174731.92 epoch sec; seqNo:4646 2021-02-13T00:05:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 105.22 , 97.78 , 105.22 2021-02-13T00:05:31Z- handleDvl() - RDI_Pathfinder msg: velocities -0.28 , -0.38 , -0.24 2021-02-13T00:05:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174731.94 epoch sec; seqNo:9574; 4058556.81 north; 591989.10 east 2021-02-13T00:05:32Z- AHRS_M2 msg: 1613174732.30 epoch sec; seqNo:9497 2021-02-13T00:05:32Z- AHRS_M2 msg: 5.85 phi; -0.10 theta; 0.21 psi 2021-02-13T00:05:32Z- handleDepth()-Depth_Keller msg: 1613174732299 epoch sec; seqNo:9575; depth 0.04 2021-02-13T00:05:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174732.31 epoch sec; seqNo:9575; 4058556.80 north; 591987.88 east 2021-02-13T00:05:32Z- AHRS_M2 msg: 1613174732.70 epoch sec; seqNo:9498 2021-02-13T00:05:32Z- AHRS_M2 msg: 5.86 phi; -0.06 theta; 0.13 psi 2021-02-13T00:05:32Z- handleDepth()-Depth_Keller msg: 1613174732700 epoch sec; seqNo:9576; depth 0.05 2021-02-13T00:05:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174732.72 epoch sec; seqNo:9576; 4058556.80 north; 591987.88 east 2021-02-13T00:05:33Z- AHRS_M2 msg: 1613174733.10 epoch sec; seqNo:9499 2021-02-13T00:05:33Z- AHRS_M2 msg: 5.83 phi; -0.01 theta; -0.08 psi 2021-02-13T00:05:33Z- handleDepth()-Depth_Keller msg: 1613174733109 epoch sec; seqNo:9577; depth 0.06 2021-02-13T00:05:33Z- handleDvl() - RDI_Pathfinder msg: 1613174733.13 epoch sec; seqNo:4647 2021-02-13T00:05:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 114.35 , 97.99 , 97.99 , 105.42 2021-02-13T00:05:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.42 , 0.16 , -0.56 2021-02-13T00:05:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174733.14 epoch sec; seqNo:9577; 4058556.80 north; 591987.88 east 2021-02-13T00:05:33Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-13T00:05:33Z- waiting for 1613174730.88, time is 1613174733.14 2021-02-13T00:05:33Z- LcmTrn::updateTrn() equating pose 1613174733.14 and meas times 1613174733.14 2021-02-13T00:05:33Z- LcmTrn::updateTrn() >>>> pose time 1613174733.14, meas time 1613174733.14 <<<< 2021-02-13T00:05:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174733.14 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:05:33Z- TerrainNav::motionUpdate() - currEstimate = (4058557, 591987.88, 0.06) 2021-02-13T00:05:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:05:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:05:33Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174733.14 sec.; No relevant navigation data available 2021-02-13T00:05:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:05:33Z- TNavBF::mmse selected from filter -1 2021-02-13T00:05:33Z- variances([0], [2], and psi_berg) = 719.332331 833.009231 0.000000 2021-02-13T00:05:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:05:33Z- LcmTrn::publishEstimates() - MLE : 4058554.30 591945.38 0.06 2021-02-13T00:05:33Z- LcmTrn::publishEstimates() - MMSE : 4058556.80 591987.88 0.06 2021-02-13T00:05:33Z- LcmTrn::publishEstimates() - COVAR: 719.33 833.01 0.77 0.00 2021-02-13T00:05:33Z- AHRS_M2 msg: 1613174733.50 epoch sec; seqNo:9500 2021-02-13T00:05:33Z- AHRS_M2 msg: 5.84 phi; -0.01 theta; -0.23 psi 2021-02-13T00:05:33Z- handleDepth()-Depth_Keller msg: 1613174733509 epoch sec; seqNo:9578; depth 0.07 2021-02-13T00:05:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174733.52 epoch sec; seqNo:9578; 4058557.17 north; 591987.87 east 2021-02-13T00:05:33Z- AHRS_M2 msg: 1613174733.91 epoch sec; seqNo:9501 2021-02-13T00:05:33Z- AHRS_M2 msg: 5.84 phi; 0.01 theta; -0.19 psi 2021-02-13T00:05:33Z- handleDepth()-Depth_Keller msg: 1613174733914 epoch sec; seqNo:9579; depth 0.08 2021-02-13T00:05:33Z- handleDvl() - RDI_Pathfinder msg: 1613174733.93 epoch sec; seqNo:4648 2021-02-13T00:05:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 105.22 , 99.27 , 97.78 2021-02-13T00:05:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.55 , -0.31 , 0.42 2021-02-13T00:05:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174733.94 epoch sec; seqNo:9579; 4058557.17 north; 591987.87 east 2021-02-13T00:05:34Z- AHRS_M2 msg: 1613174734.31 epoch sec; seqNo:9502 2021-02-13T00:05:34Z- AHRS_M2 msg: 5.86 phi; -0.02 theta; 0.01 psi 2021-02-13T00:05:34Z- handleDepth()-Depth_Keller msg: 1613174734326 epoch sec; seqNo:9580; depth 0.09 2021-02-13T00:05:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174734.34 epoch sec; seqNo:9580; 4058557.17 north; 591987.87 east 2021-02-13T00:05:34Z- AHRS_M2 msg: 1613174734.72 epoch sec; seqNo:9503 2021-02-13T00:05:34Z- AHRS_M2 msg: 5.88 phi; -0.15 theta; 0.24 psi 2021-02-13T00:05:34Z- handleDepth()-Depth_Keller msg: 1613174734721 epoch sec; seqNo:9581; depth 0.10 2021-02-13T00:05:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174734.73 epoch sec; seqNo:9581; 4058557.55 north; 591987.87 east 2021-02-13T00:05:35Z- AHRS_M2 msg: 1613174735.12 epoch sec; seqNo:9504 2021-02-13T00:05:35Z- AHRS_M2 msg: 5.92 phi; -0.23 theta; 0.31 psi 2021-02-13T00:05:35Z- handleDepth()-Depth_Keller msg: 1613174735133 epoch sec; seqNo:9582; depth 0.14 2021-02-13T00:05:35Z- handleDvl() - RDI_Pathfinder msg: 1613174735.15 epoch sec; seqNo:4649 2021-02-13T00:05:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 106.51 , 99.74 , 98.39 2021-02-13T00:05:35Z- handleDvl() - RDI_Pathfinder msg: velocities -0.42 , -0.10 , 1.20 2021-02-13T00:05:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174735.17 epoch sec; seqNo:9582; 4058557.17 north; 591987.87 east 2021-02-13T00:05:35Z- AHRS_M2 msg: 1613174735.53 epoch sec; seqNo:9505 2021-02-13T00:05:35Z- AHRS_M2 msg: 5.97 phi; -0.18 theta; 0.25 psi 2021-02-13T00:05:35Z- handleDepth()-Depth_Keller msg: 1613174735529 epoch sec; seqNo:9583; depth 0.15 2021-02-13T00:05:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174735.56 epoch sec; seqNo:9583; 4058556.80 north; 591987.88 east 2021-02-13T00:05:35Z- AHRS_M2 msg: 1613174735.93 epoch sec; seqNo:9506 2021-02-13T00:05:35Z- AHRS_M2 msg: 5.97 phi; -0.15 theta; 0.10 psi 2021-02-13T00:05:35Z- handleDepth()-Depth_Keller msg: 1613174735937 epoch sec; seqNo:9584; depth 0.09 2021-02-13T00:05:35Z- handleDvl() - RDI_Pathfinder msg: 1613174735.96 epoch sec; seqNo:4650 2021-02-13T00:05:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 130.78 , 90.21 , 94.27 , 119.96 2021-02-13T00:05:35Z- handleDvl() - RDI_Pathfinder msg: velocities -0.55 , 0.70 , -0.05 2021-02-13T00:05:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174735.97 epoch sec; seqNo:9584; 4058556.80 north; 591987.88 east 2021-02-13T00:05:36Z- AHRS_M2 msg: 1613174736.33 epoch sec; seqNo:9507 2021-02-13T00:05:36Z- AHRS_M2 msg: 5.90 phi; -0.27 theta; -0.05 psi 2021-02-13T00:05:36Z- handleDepth()-Depth_Keller msg: 1613174736341 epoch sec; seqNo:9585; depth 0.04 2021-02-13T00:05:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174736.35 epoch sec; seqNo:9585; 4058556.80 north; 591987.88 east 2021-02-13T00:05:36Z- AHRS_M2 msg: 1613174736.74 epoch sec; seqNo:9508 2021-02-13T00:05:36Z- AHRS_M2 msg: 5.79 phi; -0.30 theta; -0.20 psi 2021-02-13T00:05:36Z- handleDepth()-Depth_Keller msg: 1613174736741 epoch sec; seqNo:9586; depth 0.09 2021-02-13T00:05:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174736.75 epoch sec; seqNo:9586; 4058556.43 north; 591989.10 east 2021-02-13T00:05:37Z- AHRS_M2 msg: 1613174737.15 epoch sec; seqNo:9509 2021-02-13T00:05:37Z- AHRS_M2 msg: 5.76 phi; -0.15 theta; -0.31 psi 2021-02-13T00:05:37Z- handleDepth()-Depth_Keller msg: 1613174737152 epoch sec; seqNo:9587; depth 0.07 2021-02-13T00:05:37Z- handleDvl() - RDI_Pathfinder msg: 1613174737.18 epoch sec; seqNo:4651 2021-02-13T00:05:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:05:37Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:05:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174737.19 epoch sec; seqNo:9587; 4058556.43 north; 591989.10 east 2021-02-13T00:05:37Z- AHRS_M2 msg: 1613174737.54 epoch sec; seqNo:9510 2021-02-13T00:05:37Z- AHRS_M2 msg: 5.78 phi; -0.03 theta; -0.23 psi 2021-02-13T00:05:37Z- handleDepth()-Depth_Keller msg: 1613174737553 epoch sec; seqNo:9588; depth 0.03 2021-02-13T00:05:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174737.58 epoch sec; seqNo:9588; 4058556.43 north; 591989.10 east 2021-02-13T00:05:37Z- AHRS_M2 msg: 1613174737.95 epoch sec; seqNo:9511 2021-02-13T00:05:37Z- AHRS_M2 msg: 5.77 phi; -0.03 theta; -0.06 psi 2021-02-13T00:05:37Z- handleDepth()-Depth_Keller msg: 1613174737953 epoch sec; seqNo:9589; depth 0.04 2021-02-13T00:05:37Z- handleDvl() - RDI_Pathfinder msg: 1613174737.97 epoch sec; seqNo:4652 2021-02-13T00:05:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.65 , 127.40 , 97.65 , 114.01 2021-02-13T00:05:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.51 , -0.28 , -0.18 2021-02-13T00:05:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174738.00 epoch sec; seqNo:9589; 4058556.43 north; 591989.10 east 2021-02-13T00:05:38Z- LcmTrn::handleMessages() - handling 41 messages took 0.69 ms and 128 calls... 2021-02-13T00:05:38Z- waiting for 1613174736.14, time is 1613174738.00 2021-02-13T00:05:38Z- LcmTrn::updateTrn() equating pose 1613174738.00 and meas times 1613174738.00 2021-02-13T00:05:38Z- LcmTrn::updateTrn() >>>> pose time 1613174738.00, meas time 1613174738.00 <<<< 2021-02-13T00:05:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174738.00 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:05:38Z- TerrainNav::motionUpdate() - currEstimate = (4058556, 591989.10, 0.04) 2021-02-13T00:05:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:05:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:05:38Z- TNavBF::Projecting Measurements 2021-02-13T00:05:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:05:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:05:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:05:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:05:38Z- TNavFilter::4 good beams. 2021-02-13T00:05:38Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:05:38Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:05:38Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:05:38Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:05:38Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:05:38Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:05:38Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:05:38Z- TNavBF::mmse selected from filter -1 2021-02-13T00:05:38Z- TerrainMapOctree: (vehN, vehE ) = (4058556.43, 591989.10). 2021-02-13T00:05:38Z- TerrainMapOctree: Min Distance = 3516.67. 2021-02-13T00:05:38Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:05:38Z- There is no specific good beam that all particles have in common. 2021-02-13T00:05:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:05:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 15.26 2021-02-13T00:05:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:05:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.79 K: 0.028 ExpectedMeasDiff: nan currMeas.covariance: 25.97 2021-02-13T00:05:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:05:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.77 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 15.26 2021-02-13T00:05:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:05:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.74 K: 0.032 ExpectedMeasDiff: nan currMeas.covariance: 20.80 2021-02-13T00:05:38Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:05:38Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:05:38Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:05:38Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:05:38Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:05:38Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:05:38Z- Filter 0 No beams used in subcloud update 2021-02-13T00:05:38Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:05:38Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:05:38Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 719.3 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:05:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174738.00 sec. 2021-02-13T00:05:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:05:38Z- TNavBF::mmse selected from filter -1 2021-02-13T00:05:38Z- variances([0], [2], and psi_berg) = 719.294487 833.009206 0.000000 2021-02-13T00:05:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:05:38Z- LcmTrn::publishEstimates() - MLE : 4058553.93 591946.60 0.04 2021-02-13T00:05:38Z- LcmTrn::publishEstimates() - MMSE : 4058556.43 591989.10 0.04 2021-02-13T00:05:38Z- LcmTrn::publishEstimates() - COVAR: 719.29 833.01 0.71 0.00 2021-02-13T00:05:38Z- AHRS_M2 msg: 1613174738.35 epoch sec; seqNo:9512 2021-02-13T00:05:38Z- AHRS_M2 msg: 5.77 phi; -0.04 theta; 0.06 psi 2021-02-13T00:05:38Z- handleDepth()-Depth_Keller msg: 1613174738359 epoch sec; seqNo:9590; depth 0.06 2021-02-13T00:05:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174738.37 epoch sec; seqNo:9590; 4058556.81 north; 591989.10 east 2021-02-13T00:05:38Z- AHRS_M2 msg: 1613174738.76 epoch sec; seqNo:9513 2021-02-13T00:05:38Z- AHRS_M2 msg: 5.78 phi; 0.07 theta; 0.11 psi 2021-02-13T00:05:38Z- handleDepth()-Depth_Keller msg: 1613174738762 epoch sec; seqNo:9591; depth 0.09 2021-02-13T00:05:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174738.77 epoch sec; seqNo:9591; 4058556.81 north; 591989.10 east 2021-02-13T00:05:39Z- AHRS_M2 msg: 1613174739.16 epoch sec; seqNo:9514 2021-02-13T00:05:39Z- AHRS_M2 msg: 5.77 phi; 0.14 theta; 0.11 psi 2021-02-13T00:05:39Z- handleDepth()-Depth_Keller msg: 1613174739169 epoch sec; seqNo:9592; depth 0.08 2021-02-13T00:05:39Z- handleDvl() - RDI_Pathfinder msg: 1613174739.19 epoch sec; seqNo:4653 2021-02-13T00:05:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.80 , 97.31 , 97.31 , 101.77 2021-02-13T00:05:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.14 , -0.64 , -0.15 2021-02-13T00:05:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174739.20 epoch sec; seqNo:9592; 4058556.80 north; 591987.88 east 2021-02-13T00:05:39Z- AHRS_M2 msg: 1613174739.56 epoch sec; seqNo:9515 2021-02-13T00:05:39Z- AHRS_M2 msg: 5.74 phi; 0.05 theta; 0.08 psi 2021-02-13T00:05:39Z- handleDepth()-Depth_Keller msg: 1613174739573 epoch sec; seqNo:9593; depth 0.06 2021-02-13T00:05:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174739.58 epoch sec; seqNo:9593; 4058556.80 north; 591987.88 east 2021-02-13T00:05:39Z- AHRS_M2 msg: 1613174739.97 epoch sec; seqNo:9516 2021-02-13T00:05:39Z- AHRS_M2 msg: 5.71 phi; -0.10 theta; 0.01 psi 2021-02-13T00:05:39Z- handleDepth()-Depth_Keller msg: 1613174739979 epoch sec; seqNo:9594; depth 0.07 2021-02-13T00:05:39Z- handleDvl() - RDI_Pathfinder msg: 1613174739.99 epoch sec; seqNo:4654 2021-02-13T00:05:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 105.22 , 97.11 , 105.22 , 97.11 2021-02-13T00:05:39Z- handleDvl() - RDI_Pathfinder msg: velocities -0.43 , -0.48 , 0.15 2021-02-13T00:05:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174740.00 epoch sec; seqNo:9594; 4058556.42 north; 591987.88 east 2021-02-13T00:05:40Z- AHRS_M2 msg: 1613174740.37 epoch sec; seqNo:9517 2021-02-13T00:05:40Z- AHRS_M2 msg: 5.75 phi; -0.13 theta; -0.00 psi 2021-02-13T00:05:40Z- handleDepth()-Depth_Keller msg: 1613174740388 epoch sec; seqNo:9595; depth 0.09 2021-02-13T00:05:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174740.40 epoch sec; seqNo:9595; 4058556.04 north; 591987.89 east 2021-02-13T00:05:40Z- AHRS_M2 msg: 1613174740.78 epoch sec; seqNo:9518 2021-02-13T00:05:40Z- AHRS_M2 msg: 5.80 phi; -0.08 theta; 0.07 psi 2021-02-13T00:05:40Z- handleDepth()-Depth_Keller msg: 1613174740780 epoch sec; seqNo:9596; depth 0.05 2021-02-13T00:05:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174740.79 epoch sec; seqNo:9596; 4058556.04 north; 591987.89 east 2021-02-13T00:05:41Z- AHRS_M2 msg: 1613174741.18 epoch sec; seqNo:9519 2021-02-13T00:05:41Z- AHRS_M2 msg: 5.78 phi; -0.07 theta; 0.20 psi 2021-02-13T00:05:41Z- handleDepth()-Depth_Keller msg: 1613174741189 epoch sec; seqNo:9597; depth 0.09 2021-02-13T00:05:41Z- handleDvl() - RDI_Pathfinder msg: 1613174741.20 epoch sec; seqNo:4655 2021-02-13T00:05:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.47 , 99.47 , 97.99 , 108.40 2021-02-13T00:05:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.37 , -0.59 , -0.19 2021-02-13T00:05:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174741.22 epoch sec; seqNo:9597; 4058556.04 north; 591987.89 east 2021-02-13T00:05:41Z- AHRS_M2 msg: 1613174741.58 epoch sec; seqNo:9520 2021-02-13T00:05:41Z- AHRS_M2 msg: 5.76 phi; -0.08 theta; 0.19 psi 2021-02-13T00:05:41Z- handleDepth()-Depth_Keller msg: 1613174741589 epoch sec; seqNo:9598; depth 0.13 2021-02-13T00:05:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:05:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:05:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:05:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:05:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:05:41Z- handleDvl() - RDI_Pathfinder msg: 1613174741.59 epoch sec; seqNo:4656 2021-02-13T00:05:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:05:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.37 , -0.59 , -0.19 2021-02-13T00:05:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174741.61 epoch sec; seqNo:9598; 4058556.04 north; 591987.89 east 2021-02-13T00:05:41Z- AHRS_M2 msg: 1613174741.99 epoch sec; seqNo:9521 2021-02-13T00:05:41Z- AHRS_M2 msg: 5.77 phi; -0.15 theta; 0.06 psi 2021-02-13T00:05:41Z- handleDepth()-Depth_Keller msg: 1613174741994 epoch sec; seqNo:9599; depth 0.08 2021-02-13T00:05:42Z- handleDvl() - RDI_Pathfinder msg: 1613174742.01 epoch sec; seqNo:4657 2021-02-13T00:05:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 115.84 , 89.06 , 97.99 , 103.94 2021-02-13T00:05:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.02 , 0.24 , 0.68 2021-02-13T00:05:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174742.02 epoch sec; seqNo:9599; 4058556.04 north; 591987.89 east 2021-02-13T00:05:42Z- AHRS_M2 msg: 1613174742.39 epoch sec; seqNo:9522 2021-02-13T00:05:42Z- AHRS_M2 msg: 5.74 phi; -0.23 theta; -0.03 psi 2021-02-13T00:05:42Z- handleDepth()-Depth_Keller msg: 1613174742397 epoch sec; seqNo:9600; depth 0.09 2021-02-13T00:05:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174742.42 epoch sec; seqNo:9600; 4058556.04 north; 591987.89 east 2021-02-13T00:05:42Z- AHRS_M2 msg: 1613174742.80 epoch sec; seqNo:9523 2021-02-13T00:05:42Z- AHRS_M2 msg: 5.70 phi; -0.23 theta; -0.09 psi 2021-02-13T00:05:42Z- handleDepth()-Depth_Keller msg: 1613174742800 epoch sec; seqNo:9601; depth 0.10 2021-02-13T00:05:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:05:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:05:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:05:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:05:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:05:42Z- handleDvl() - RDI_Pathfinder msg: 1613174742.80 epoch sec; seqNo:4658 2021-02-13T00:05:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:05:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.02 , 0.24 , 0.68 2021-02-13T00:05:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174742.81 epoch sec; seqNo:9601; 4058556.04 north; 591987.89 east 2021-02-13T00:05:43Z- AHRS_M2 msg: 1613174743.20 epoch sec; seqNo:9524 2021-02-13T00:05:43Z- AHRS_M2 msg: 5.66 phi; -0.18 theta; -0.17 psi 2021-02-13T00:05:43Z- handleDepth()-Depth_Keller msg: 1613174743204 epoch sec; seqNo:9602; depth 0.09 2021-02-13T00:05:43Z- handleDvl() - RDI_Pathfinder msg: 1613174743.22 epoch sec; seqNo:4659 2021-02-13T00:05:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.47 , 102.45 , 97.99 , 114.35 2021-02-13T00:05:43Z- handleDvl() - RDI_Pathfinder msg: velocities -0.40 , 0.53 , -0.00 2021-02-13T00:05:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174743.23 epoch sec; seqNo:9602; 4058556.04 north; 591987.89 east 2021-02-13T00:05:43Z- LcmTrn::handleMessages() - handling 46 messages took 0.64 ms and 134 calls... 2021-02-13T00:05:43Z- waiting for 1613174741.00, time is 1613174743.23 2021-02-13T00:05:43Z- LcmTrn::updateTrn() equating pose 1613174743.23 and meas times 1613174743.23 2021-02-13T00:05:43Z- LcmTrn::updateTrn() >>>> pose time 1613174743.23, meas time 1613174743.23 <<<< 2021-02-13T00:05:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174743.23 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:05:43Z- TerrainNav::motionUpdate() - currEstimate = (4058556, 591987.89, 0.09) 2021-02-13T00:05:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:05:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:05:43Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174743.23 sec.; No relevant navigation data available 2021-02-13T00:05:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:05:43Z- TNavBF::mmse selected from filter -1 2021-02-13T00:05:43Z- variances([0], [2], and psi_berg) = 719.256660 833.009182 0.000000 2021-02-13T00:05:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:05:43Z- LcmTrn::publishEstimates() - MLE : 4058553.54 591945.39 0.09 2021-02-13T00:05:43Z- LcmTrn::publishEstimates() - MMSE : 4058556.04 591987.89 0.09 2021-02-13T00:05:43Z- LcmTrn::publishEstimates() - COVAR: 719.26 833.01 0.78 0.00 2021-02-13T00:05:43Z- AHRS_M2 msg: 1613174743.61 epoch sec; seqNo:9525 2021-02-13T00:05:43Z- AHRS_M2 msg: 5.65 phi; -0.23 theta; -0.22 psi 2021-02-13T00:05:43Z- handleDepth()-Depth_Keller msg: 1613174743615 epoch sec; seqNo:9603; depth 0.07 2021-02-13T00:05:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174743.63 epoch sec; seqNo:9603; 4058556.04 north; 591987.89 east 2021-02-13T00:05:44Z- AHRS_M2 msg: 1613174744.01 epoch sec; seqNo:9526 2021-02-13T00:05:44Z- AHRS_M2 msg: 5.66 phi; -0.30 theta; -0.15 psi 2021-02-13T00:05:44Z- handleDepth()-Depth_Keller msg: 1613174744013 epoch sec; seqNo:9604; depth 0.11 2021-02-13T00:05:44Z- handleDvl() - RDI_Pathfinder msg: 1613174744.03 epoch sec; seqNo:4660 2021-02-13T00:05:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 118.61 , 97.78 , 117.12 2021-02-13T00:05:44Z- handleDvl() - RDI_Pathfinder msg: velocities -0.60 , -0.30 , -0.21 2021-02-13T00:05:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174744.04 epoch sec; seqNo:9604; 4058555.66 north; 591987.89 east 2021-02-13T00:05:44Z- AHRS_M2 msg: 1613174744.41 epoch sec; seqNo:9527 2021-02-13T00:05:44Z- AHRS_M2 msg: 5.69 phi; -0.26 theta; -0.04 psi 2021-02-13T00:05:44Z- handleDepth()-Depth_Keller msg: 1613174744416 epoch sec; seqNo:9605; depth 0.18 2021-02-13T00:05:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174744.43 epoch sec; seqNo:9605; 4058555.66 north; 591987.89 east 2021-02-13T00:05:44Z- AHRS_M2 msg: 1613174744.82 epoch sec; seqNo:9528 2021-02-13T00:05:44Z- AHRS_M2 msg: 5.72 phi; -0.15 theta; 0.08 psi 2021-02-13T00:05:44Z- handleDepth()-Depth_Keller msg: 1613174744820 epoch sec; seqNo:9606; depth 0.16 2021-02-13T00:05:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174744.87 epoch sec; seqNo:9606; 4058555.28 north; 591987.89 east 2021-02-13T00:05:45Z- AHRS_M2 msg: 1613174745.22 epoch sec; seqNo:9529 2021-02-13T00:05:45Z- AHRS_M2 msg: 5.75 phi; -0.11 theta; 0.13 psi 2021-02-13T00:05:45Z- handleDepth()-Depth_Keller msg: 1613174745233 epoch sec; seqNo:9607; depth 0.15 2021-02-13T00:05:45Z- handleDvl() - RDI_Pathfinder msg: 1613174745.26 epoch sec; seqNo:4661 2021-02-13T00:05:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.93 , 98.93 , 97.44 , 0.00 2021-02-13T00:05:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.19 , -0.04 , -0.65 2021-02-13T00:05:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174745.29 epoch sec; seqNo:9607; 4058555.28 north; 591987.89 east 2021-02-13T00:05:45Z- AHRS_M2 msg: 1613174745.62 epoch sec; seqNo:9530 2021-02-13T00:05:45Z- AHRS_M2 msg: 5.71 phi; -0.15 theta; 0.05 psi 2021-02-13T00:05:45Z- handleDepth()-Depth_Keller msg: 1613174745641 epoch sec; seqNo:9608; depth 0.11 2021-02-13T00:05:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174745.65 epoch sec; seqNo:9608; 4058555.28 north; 591987.89 east 2021-02-13T00:05:46Z- AHRS_M2 msg: 1613174746.03 epoch sec; seqNo:9531 2021-02-13T00:05:46Z- AHRS_M2 msg: 5.69 phi; -0.14 theta; -0.11 psi 2021-02-13T00:05:46Z- handleDepth()-Depth_Keller msg: 1613174746033 epoch sec; seqNo:9609; depth 0.06 2021-02-13T00:05:46Z- handleDvl() - RDI_Pathfinder msg: 1613174746.05 epoch sec; seqNo:4662 2021-02-13T00:05:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.23 , 97.17 , 97.17 , 109.35 2021-02-13T00:05:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.41 , -0.51 , -0.66 2021-02-13T00:05:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174746.07 epoch sec; seqNo:9609; 4058555.66 north; 591987.89 east 2021-02-13T00:05:46Z- AHRS_M2 msg: 1613174746.43 epoch sec; seqNo:9532 2021-02-13T00:05:46Z- AHRS_M2 msg: 5.74 phi; -0.06 theta; -0.14 psi 2021-02-13T00:05:46Z- handleDepth()-Depth_Keller msg: 1613174746437 epoch sec; seqNo:9610; depth 0.01 2021-02-13T00:05:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:05:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:05:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:05:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:05:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:05:46Z- handleDvl() - RDI_Pathfinder msg: 1613174746.44 epoch sec; seqNo:4663 2021-02-13T00:05:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:05:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.41 , -0.51 , -0.66 2021-02-13T00:05:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174746.45 epoch sec; seqNo:9610; 4058555.66 north; 591987.89 east 2021-02-13T00:05:46Z- AHRS_M2 msg: 1613174746.84 epoch sec; seqNo:9533 2021-02-13T00:05:46Z- AHRS_M2 msg: 5.75 phi; 0.02 theta; 0.02 psi 2021-02-13T00:05:46Z- handleDepth()-Depth_Keller msg: 1613174746842 epoch sec; seqNo:9611; depth 0.04 2021-02-13T00:05:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174746.85 epoch sec; seqNo:9611; 4058555.66 north; 591987.89 east 2021-02-13T00:05:47Z- AHRS_M2 msg: 1613174747.24 epoch sec; seqNo:9534 2021-02-13T00:05:47Z- AHRS_M2 msg: 5.74 phi; 0.09 theta; 0.13 psi 2021-02-13T00:05:47Z- handleDepth()-Depth_Keller msg: 1613174747269 epoch sec; seqNo:9612; depth 0.05 2021-02-13T00:05:47Z- handleDvl() - RDI_Pathfinder msg: 1613174747.28 epoch sec; seqNo:4664 2021-02-13T00:05:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 103.94 , 97.99 , 97.99 2021-02-13T00:05:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.51 , -0.34 , 1.00 2021-02-13T00:05:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174747.29 epoch sec; seqNo:9612; 4058555.65 north; 591986.67 east 2021-02-13T00:05:47Z- AHRS_M2 msg: 1613174747.64 epoch sec; seqNo:9535 2021-02-13T00:05:47Z- AHRS_M2 msg: 5.74 phi; 0.11 theta; 0.17 psi 2021-02-13T00:05:47Z- handleDepth()-Depth_Keller msg: 1613174747654 epoch sec; seqNo:9613; depth 0.05 2021-02-13T00:05:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:05:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:05:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:05:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:05:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:05:47Z- handleDvl() - RDI_Pathfinder msg: 1613174747.66 epoch sec; seqNo:4665 2021-02-13T00:05:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:05:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.51 , -0.34 , 1.00 2021-02-13T00:05:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174747.68 epoch sec; seqNo:9613; 4058555.65 north; 591986.67 east 2021-02-13T00:05:48Z- AHRS_M2 msg: 1613174748.05 epoch sec; seqNo:9536 2021-02-13T00:05:48Z- AHRS_M2 msg: 5.74 phi; 0.05 theta; 0.02 psi 2021-02-13T00:05:48Z- handleDepth()-Depth_Keller msg: 1613174748065 epoch sec; seqNo:9614; depth 0.06 2021-02-13T00:05:48Z- handleDvl() - RDI_Pathfinder msg: 1613174748.08 epoch sec; seqNo:4666 2021-02-13T00:05:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 113.33 , 97.11 , 107.93 , 97.11 2021-02-13T00:05:48Z- handleDvl() - RDI_Pathfinder msg: velocities -0.39 , 0.01 , 1.34 2021-02-13T00:05:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174748.10 epoch sec; seqNo:9614; 4058555.65 north; 591986.67 east 2021-02-13T00:05:48Z- AHRS_M2 msg: 1613174748.46 epoch sec; seqNo:9537 2021-02-13T00:05:48Z- AHRS_M2 msg: 5.75 phi; 0.02 theta; -0.12 psi 2021-02-13T00:05:48Z- handleDepth()-Depth_Keller msg: 1613174748469 epoch sec; seqNo:9615; depth 0.04 2021-02-13T00:05:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174748.48 epoch sec; seqNo:9615; 4058555.65 north; 591986.67 east 2021-02-13T00:05:48Z- LcmTrn::handleMessages() - handling 46 messages took 0.74 ms and 131 calls... 2021-02-13T00:05:48Z- waiting for 1613174746.23, time is 1613174748.48 2021-02-13T00:05:48Z- LcmTrn::updateTrn() >>>> pose time 1613174748.48, meas time 1613174748.08 <<<< 2021-02-13T00:05:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174748.08 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:05:48Z- TerrainNav::motionUpdate() - currEstimate = (4058556, 591986.67, 0.04) 2021-02-13T00:05:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:05:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:05:48Z- TNavBF::Projecting Measurements 2021-02-13T00:05:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:05:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:05:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:05:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:05:48Z- TNavFilter::4 good beams. 2021-02-13T00:05:48Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:05:48Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:05:48Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:05:48Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:05:48Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:05:48Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:05:48Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:05:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:05:48Z- TerrainMapOctree: (vehN, vehE ) = (4058555.68, 591986.76). 2021-02-13T00:05:48Z- TerrainMapOctree: Min Distance = 3518.75. 2021-02-13T00:05:48Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:05:48Z- There is no specific good beam that all particles have in common. 2021-02-13T00:05:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:05:48Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 20.55 2021-02-13T00:05:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:05:48Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-13T00:05:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:05:48Z- Old Depth Bias: 0.00 DepthBiasCov: 0.77 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 18.64 2021-02-13T00:05:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:05:48Z- Old Depth Bias: 0.00 DepthBiasCov: 0.74 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-13T00:05:48Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:05:48Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:05:48Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:05:48Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:05:48Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:05:48Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:05:48Z- Filter 0 No beams used in subcloud update 2021-02-13T00:05:48Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:05:48Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:05:48Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 719.2 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:05:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174748.08 sec. 2021-02-13T00:05:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:05:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:05:48Z- variances([0], [2], and psi_berg) = 719.209825 833.009151 0.000000 2021-02-13T00:05:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:05:48Z- LcmTrn::publishEstimates() - MLE : 4058553.15 591944.17 0.04 2021-02-13T00:05:48Z- LcmTrn::publishEstimates() - MMSE : 4058555.65 591986.67 0.04 2021-02-13T00:05:48Z- LcmTrn::publishEstimates() - COVAR: 719.21 833.01 0.72 0.00 2021-02-13T00:05:48Z- AHRS_M2 msg: 1613174748.86 epoch sec; seqNo:9538 2021-02-13T00:05:48Z- AHRS_M2 msg: 5.76 phi; -0.09 theta; -0.12 psi 2021-02-13T00:05:48Z- handleDepth()-Depth_Keller msg: 1613174748860 epoch sec; seqNo:9616; depth -0.00 2021-02-13T00:05:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:05:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:05:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:05:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:05:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:05:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:05:48Z- handleDvl() - RDI_Pathfinder msg: 1613174748.86 epoch sec; seqNo:4667 2021-02-13T00:05:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:05:48Z- handleDvl() - RDI_Pathfinder msg: velocities -0.39 , 0.01 , 1.34 2021-02-13T00:05:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174748.88 epoch sec; seqNo:9616; 4058555.27 north; 591986.67 east 2021-02-13T00:05:49Z- AHRS_M2 msg: 1613174749.27 epoch sec; seqNo:9539 2021-02-13T00:05:49Z- AHRS_M2 msg: 5.69 phi; -0.26 theta; 0.00 psi 2021-02-13T00:05:49Z- handleDepth()-Depth_Keller msg: 1613174749272 epoch sec; seqNo:9617; depth -0.00 2021-02-13T00:05:49Z- handleDvl() - RDI_Pathfinder msg: 1613174749.30 epoch sec; seqNo:4668 2021-02-13T00:05:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.28 , 105.23 , 106.71 , 97.79 2021-02-13T00:05:49Z- handleDvl() - RDI_Pathfinder msg: velocities -1.17 , 0.29 , 0.22 2021-02-13T00:05:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174749.31 epoch sec; seqNo:9617; 4058554.90 north; 591987.90 east 2021-02-13T00:05:49Z- AHRS_M2 msg: 1613174749.66 epoch sec; seqNo:9540 2021-02-13T00:05:49Z- AHRS_M2 msg: 5.68 phi; -0.24 theta; 0.12 psi 2021-02-13T00:05:49Z- handleDepth()-Depth_Keller msg: 1613174749673 epoch sec; seqNo:9618; depth 0.04 2021-02-13T00:05:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174749.71 epoch sec; seqNo:9618; 4058554.90 north; 591987.90 east 2021-02-13T00:05:50Z- AHRS_M2 msg: 1613174750.07 epoch sec; seqNo:9541 2021-02-13T00:05:50Z- AHRS_M2 msg: 5.70 phi; -0.07 theta; 0.14 psi 2021-02-13T00:05:50Z- handleDepth()-Depth_Keller msg: 1613174750074 epoch sec; seqNo:9619; depth 0.06 2021-02-13T00:05:50Z- handleDvl() - RDI_Pathfinder msg: 1613174750.09 epoch sec; seqNo:4669 2021-02-13T00:05:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:05:50Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:05:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174750.10 epoch sec; seqNo:9619; 4058554.52 north; 591987.90 east 2021-02-13T00:05:50Z- AHRS_M2 msg: 1613174750.47 epoch sec; seqNo:9542 2021-02-13T00:05:50Z- AHRS_M2 msg: 5.69 phi; -0.03 theta; 0.04 psi 2021-02-13T00:05:50Z- handleDepth()-Depth_Keller msg: 1613174750477 epoch sec; seqNo:9620; depth 0.11 2021-02-13T00:05:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174750.50 epoch sec; seqNo:9620; 4058554.14 north; 591987.91 east 2021-02-13T00:05:50Z- AHRS_M2 msg: 1613174750.88 epoch sec; seqNo:9543 2021-02-13T00:05:50Z- AHRS_M2 msg: 5.66 phi; -0.10 theta; -0.09 psi 2021-02-13T00:05:50Z- handleDepth()-Depth_Keller msg: 1613174750884 epoch sec; seqNo:9621; depth 0.17 2021-02-13T00:05:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174750.92 epoch sec; seqNo:9621; 4058553.78 north; 591989.13 east 2021-02-13T00:05:51Z- AHRS_M2 msg: 1613174751.28 epoch sec; seqNo:9544 2021-02-13T00:05:51Z- AHRS_M2 msg: 5.64 phi; -0.19 theta; -0.20 psi 2021-02-13T00:05:51Z- handleDepth()-Depth_Keller msg: 1613174751289 epoch sec; seqNo:9622; depth 0.20 2021-02-13T00:05:51Z- handleDvl() - RDI_Pathfinder msg: 1613174751.30 epoch sec; seqNo:4670 2021-02-13T00:05:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 105.22 , 97.78 , 97.78 , 99.27 2021-02-13T00:05:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.57 , 0.21 , -0.06 2021-02-13T00:05:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174751.31 epoch sec; seqNo:9622; 4058554.16 north; 591989.13 east 2021-02-13T00:05:51Z- AHRS_M2 msg: 1613174751.68 epoch sec; seqNo:9545 2021-02-13T00:05:51Z- AHRS_M2 msg: 5.66 phi; -0.24 theta; -0.23 psi 2021-02-13T00:05:51Z- handleDepth()-Depth_Keller msg: 1613174751691 epoch sec; seqNo:9623; depth 0.24 2021-02-13T00:05:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174751.70 epoch sec; seqNo:9623; 4058554.16 north; 591989.13 east 2021-02-13T00:05:52Z- AHRS_M2 msg: 1613174752.09 epoch sec; seqNo:9546 2021-02-13T00:05:52Z- AHRS_M2 msg: 5.70 phi; -0.24 theta; -0.16 psi 2021-02-13T00:05:52Z- handleDepth()-Depth_Keller msg: 1613174752097 epoch sec; seqNo:9624; depth 0.22 2021-02-13T00:05:52Z- handleDvl() - RDI_Pathfinder msg: 1613174752.11 epoch sec; seqNo:4671 2021-02-13T00:05:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 115.97 , 97.04 , 109.21 2021-02-13T00:05:52Z- handleDvl() - RDI_Pathfinder msg: velocities -0.07 , -0.66 , -0.51 2021-02-13T00:05:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174752.13 epoch sec; seqNo:9624; 4058554.14 north; 591987.91 east 2021-02-13T00:05:52Z- AHRS_M2 msg: 1613174752.49 epoch sec; seqNo:9547 2021-02-13T00:05:52Z- AHRS_M2 msg: 5.73 phi; -0.23 theta; 0.02 psi 2021-02-13T00:05:52Z- handleDepth()-Depth_Keller msg: 1613174752497 epoch sec; seqNo:9625; depth 0.26 2021-02-13T00:05:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174752.51 epoch sec; seqNo:9625; 4058553.76 north; 591987.91 east 2021-02-13T00:05:52Z- AHRS_M2 msg: 1613174752.91 epoch sec; seqNo:9548 2021-02-13T00:05:52Z- AHRS_M2 msg: 5.76 phi; -0.21 theta; 0.15 psi 2021-02-13T00:05:52Z- handleDepth()-Depth_Keller msg: 1613174752934 epoch sec; seqNo:9626; depth 0.27 2021-02-13T00:05:52Z- handleDvl() - RDI_Pathfinder msg: 1613174752.95 epoch sec; seqNo:4672 2021-02-13T00:05:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 100.69 , 97.71 , 117.05 2021-02-13T00:05:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.42 , -0.59 , 0.08 2021-02-13T00:05:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174752.96 epoch sec; seqNo:9626; 4058554.14 north; 591987.91 east 2021-02-13T00:05:53Z- AHRS_M2 msg: 1613174753.30 epoch sec; seqNo:9549 2021-02-13T00:05:53Z- AHRS_M2 msg: 5.79 phi; -0.22 theta; 0.16 psi 2021-02-13T00:05:53Z- handleDepth()-Depth_Keller msg: 1613174753304 epoch sec; seqNo:9627; depth 0.25 2021-02-13T00:05:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174753.32 epoch sec; seqNo:9627; 4058554.14 north; 591987.91 east 2021-02-13T00:05:53Z- LcmTrn::handleMessages() - handling 42 messages took 0.69 ms and 128 calls... 2021-02-13T00:05:53Z- waiting for 1613174751.48, time is 1613174753.32 2021-02-13T00:05:53Z- LcmTrn::updateTrn() >>>> pose time 1613174753.32, meas time 1613174752.95 <<<< 2021-02-13T00:05:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174752.95 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:05:53Z- TerrainNav::motionUpdate() - currEstimate = (4058554, 591987.91, 0.25) 2021-02-13T00:05:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:05:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:05:53Z- TNavBF::Projecting Measurements 2021-02-13T00:05:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:05:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:05:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:05:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:05:53Z- TNavFilter::4 good beams. 2021-02-13T00:05:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:05:53Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:05:53Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:05:53Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:05:53Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:05:53Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:05:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:05:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:05:53Z- TerrainMapOctree: (vehN, vehE ) = (4058554.26, 591987.81). 2021-02-13T00:05:53Z- TerrainMapOctree: Min Distance = 3519.16. 2021-02-13T00:05:53Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:05:53Z- There is no specific good beam that all particles have in common. 2021-02-13T00:05:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:05:53Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:05:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:05:53Z- Old Depth Bias: 0.00 DepthBiasCov: 0.77 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 16.22 2021-02-13T00:05:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:05:53Z- Old Depth Bias: 0.00 DepthBiasCov: 0.74 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:05:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:05:53Z- Old Depth Bias: 0.00 DepthBiasCov: 0.71 K: 0.029 ExpectedMeasDiff: nan currMeas.covariance: 21.92 2021-02-13T00:05:53Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:05:53Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:05:53Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:05:53Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:05:53Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:05:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:05:53Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:05:53Z- Filter 0 No beams used in subcloud update 2021-02-13T00:05:53Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:05:53Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:05:53Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 719.2 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:05:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174752.95 sec. 2021-02-13T00:05:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:05:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:05:53Z- variances([0], [2], and psi_berg) = 719.152132 833.009114 0.000000 2021-02-13T00:05:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:05:53Z- LcmTrn::publishEstimates() - MLE : 4058551.64 591945.41 0.25 2021-02-13T00:05:53Z- LcmTrn::publishEstimates() - MMSE : 4058554.14 591987.91 0.25 2021-02-13T00:05:53Z- LcmTrn::publishEstimates() - COVAR: 719.15 833.01 0.70 0.00 2021-02-13T00:05:53Z- AHRS_M2 msg: 1613174753.70 epoch sec; seqNo:9550 2021-02-13T00:05:53Z- AHRS_M2 msg: 5.81 phi; -0.26 theta; 0.12 psi 2021-02-13T00:05:53Z- handleDepth()-Depth_Keller msg: 1613174753708 epoch sec; seqNo:9628; depth 0.23 2021-02-13T00:05:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174753.72 epoch sec; seqNo:9628; 4058554.14 north; 591987.91 east 2021-02-13T00:05:54Z- AHRS_M2 msg: 1613174754.11 epoch sec; seqNo:9551 2021-02-13T00:05:54Z- AHRS_M2 msg: 5.81 phi; -0.31 theta; 0.07 psi 2021-02-13T00:05:54Z- handleDepth()-Depth_Keller msg: 1613174754112 epoch sec; seqNo:9629; depth 0.22 2021-02-13T00:05:54Z- handleDvl() - RDI_Pathfinder msg: 1613174754.13 epoch sec; seqNo:4673 2021-02-13T00:05:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 112.73 , 97.85 , 97.85 , 117.19 2021-02-13T00:05:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.04 , -0.37 , 0.71 2021-02-13T00:05:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174754.14 epoch sec; seqNo:9629; 4058553.75 north; 591986.69 east 2021-02-13T00:05:54Z- AHRS_M2 msg: 1613174754.51 epoch sec; seqNo:9552 2021-02-13T00:05:54Z- AHRS_M2 msg: 5.83 phi; -0.33 theta; 0.01 psi 2021-02-13T00:05:54Z- handleDepth()-Depth_Keller msg: 1613174754519 epoch sec; seqNo:9630; depth 0.23 2021-02-13T00:05:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174754.53 epoch sec; seqNo:9630; 4058553.75 north; 591986.69 east 2021-02-13T00:05:54Z- AHRS_M2 msg: 1613174754.92 epoch sec; seqNo:9553 2021-02-13T00:05:54Z- AHRS_M2 msg: 5.86 phi; -0.36 theta; -0.01 psi 2021-02-13T00:05:54Z- handleDepth()-Depth_Keller msg: 1613174754925 epoch sec; seqNo:9631; depth 0.19 2021-02-13T00:05:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174754.94 epoch sec; seqNo:9631; 4058553.75 north; 591986.69 east 2021-02-13T00:05:55Z- AHRS_M2 msg: 1613174755.32 epoch sec; seqNo:9554 2021-02-13T00:05:55Z- AHRS_M2 msg: 5.85 phi; -0.43 theta; -0.00 psi 2021-02-13T00:05:55Z- handleDepth()-Depth_Keller msg: 1613174755329 epoch sec; seqNo:9632; depth 0.20 2021-02-13T00:05:55Z- handleDvl() - RDI_Pathfinder msg: 1613174755.34 epoch sec; seqNo:4674 2021-02-13T00:05:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 106.51 , 102.04 , 88.65 , 130.31 2021-02-13T00:05:55Z- handleDvl() - RDI_Pathfinder msg: velocities -0.65 , 0.53 , 0.69 2021-02-13T00:05:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174755.36 epoch sec; seqNo:9632; 4058553.39 north; 591987.91 east 2021-02-13T00:05:55Z- AHRS_M2 msg: 1613174755.72 epoch sec; seqNo:9555 2021-02-13T00:05:55Z- AHRS_M2 msg: 5.78 phi; -0.47 theta; -0.05 psi 2021-02-13T00:05:55Z- handleDepth()-Depth_Keller msg: 1613174755728 epoch sec; seqNo:9633; depth 0.21 2021-02-13T00:05:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174755.74 epoch sec; seqNo:9633; 4058553.01 north; 591987.92 east 2021-02-13T00:05:56Z- AHRS_M2 msg: 1613174756.13 epoch sec; seqNo:9556 2021-02-13T00:05:56Z- AHRS_M2 msg: 5.75 phi; -0.46 theta; -0.17 psi 2021-02-13T00:05:56Z- handleDepth()-Depth_Keller msg: 1613174756137 epoch sec; seqNo:9634; depth 0.21 2021-02-13T00:05:56Z- handleDvl() - RDI_Pathfinder msg: 1613174756.15 epoch sec; seqNo:4675 2021-02-13T00:05:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 110.90 , 107.93 , 0.00 , 99.00 2021-02-13T00:05:56Z- handleDvl() - RDI_Pathfinder msg: velocities 1.33 , 0.59 , -0.55 2021-02-13T00:05:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174756.16 epoch sec; seqNo:9634; 4058553.76 north; 591987.91 east 2021-02-13T00:05:56Z- AHRS_M2 msg: 1613174756.53 epoch sec; seqNo:9557 2021-02-13T00:05:56Z- AHRS_M2 msg: 5.75 phi; -0.44 theta; -0.20 psi 2021-02-13T00:05:56Z- handleDepth()-Depth_Keller msg: 1613174756551 epoch sec; seqNo:9635; depth 0.20 2021-02-13T00:05:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174756.56 epoch sec; seqNo:9635; 4058554.52 north; 591987.90 east 2021-02-13T00:05:56Z- AHRS_M2 msg: 1613174756.94 epoch sec; seqNo:9558 2021-02-13T00:05:56Z- AHRS_M2 msg: 5.77 phi; -0.37 theta; -0.16 psi 2021-02-13T00:05:56Z- handleDepth()-Depth_Keller msg: 1613174756941 epoch sec; seqNo:9636; depth 0.20 2021-02-13T00:05:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174756.95 epoch sec; seqNo:9636; 4058554.90 north; 591987.90 east 2021-02-13T00:05:57Z- AHRS_M2 msg: 1613174757.35 epoch sec; seqNo:9559 2021-02-13T00:05:57Z- AHRS_M2 msg: 5.79 phi; -0.27 theta; -0.08 psi 2021-02-13T00:05:57Z- handleDepth()-Depth_Keller msg: 1613174757353 epoch sec; seqNo:9637; depth 0.19 2021-02-13T00:05:57Z- handleDvl() - RDI_Pathfinder msg: 1613174757.37 epoch sec; seqNo:4676 2021-02-13T00:05:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.09 , 122.87 , 88.65 , 155.60 2021-02-13T00:05:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.37 , -0.11 , -0.69 2021-02-13T00:05:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174757.38 epoch sec; seqNo:9637; 4058555.27 north; 591986.67 east 2021-02-13T00:05:57Z- AHRS_M2 msg: 1613174757.76 epoch sec; seqNo:9560 2021-02-13T00:05:57Z- AHRS_M2 msg: 5.80 phi; -0.16 theta; 0.02 psi 2021-02-13T00:05:57Z- handleDepth()-Depth_Keller msg: 1613174757805 epoch sec; seqNo:9638; depth 0.17 2021-02-13T00:05:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174757.83 epoch sec; seqNo:9638; 4058555.27 north; 591986.67 east 2021-02-13T00:05:58Z- AHRS_M2 msg: 1613174758.15 epoch sec; seqNo:9561 2021-02-13T00:05:58Z- AHRS_M2 msg: 5.82 phi; -0.13 theta; 0.06 psi 2021-02-13T00:05:58Z- handleDepth()-Depth_Keller msg: 1613174758162 epoch sec; seqNo:9639; depth 0.12 2021-02-13T00:05:58Z- handleDvl() - RDI_Pathfinder msg: 1613174758.19 epoch sec; seqNo:4677 2021-02-13T00:05:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:05:58Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:05:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174758.20 epoch sec; seqNo:9639; 4058555.65 north; 591986.67 east 2021-02-13T00:05:58Z- AHRS_M2 msg: 1613174758.55 epoch sec; seqNo:9562 2021-02-13T00:05:58Z- AHRS_M2 msg: 5.82 phi; -0.15 theta; 0.08 psi 2021-02-13T00:05:58Z- handleDepth()-Depth_Keller msg: 1613174758556 epoch sec; seqNo:9640; depth 0.11 2021-02-13T00:05:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174758.57 epoch sec; seqNo:9640; 4058555.65 north; 591986.67 east 2021-02-13T00:05:58Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-13T00:05:58Z- waiting for 1613174756.32, time is 1613174758.57 2021-02-13T00:05:58Z- LcmTrn::updateTrn() >>>> pose time 1613174758.57, meas time 1613174758.19 <<<< 2021-02-13T00:05:58Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174758.19, ping # 0. 2021-02-13T00:05:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174758.19 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:05:58Z- TerrainNav::motionUpdate() - currEstimate = (4058556, 591986.67, 0.11) 2021-02-13T00:05:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:05:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:05:58Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:05:58Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:05:58Z- TNavBF::Projecting Measurements 2021-02-13T00:05:58Z- TNavFilter::Measurement from time = 1613174758.19 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:05:58Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:05:58Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:05:58Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:05:58Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:05:58Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:05:58Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:05:58Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:05:58Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 719.1 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:05:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:05:58Z- TNavBF::mmse selected from filter -1 2021-02-13T00:05:58Z- variances([0], [2], and psi_berg) = 719.094712 833.009077 0.000000 2021-02-13T00:05:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:05:58Z- LcmTrn::publishEstimates() - MLE : 4058553.15 591944.17 0.11 2021-02-13T00:05:58Z- LcmTrn::publishEstimates() - MMSE : 4058555.65 591986.67 0.11 2021-02-13T00:05:58Z- LcmTrn::publishEstimates() - COVAR: 719.09 833.01 0.80 0.00 2021-02-13T00:05:58Z- AHRS_M2 msg: 1613174758.96 epoch sec; seqNo:9563 2021-02-13T00:05:58Z- AHRS_M2 msg: 5.80 phi; -0.19 theta; 0.04 psi 2021-02-13T00:05:58Z- handleDepth()-Depth_Keller msg: 1613174758966 epoch sec; seqNo:9641; depth 0.16 2021-02-13T00:05:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174758.97 epoch sec; seqNo:9641; 4058555.65 north; 591986.67 east 2021-02-13T00:05:59Z- AHRS_M2 msg: 1613174759.36 epoch sec; seqNo:9564 2021-02-13T00:05:59Z- AHRS_M2 msg: 5.79 phi; -0.15 theta; -0.03 psi 2021-02-13T00:05:59Z- handleDepth()-Depth_Keller msg: 1613174759369 epoch sec; seqNo:9642; depth 0.16 2021-02-13T00:05:59Z- handleDvl() - RDI_Pathfinder msg: 1613174759.38 epoch sec; seqNo:4678 2021-02-13T00:05:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.80 , 97.04 , 97.04 , 109.21 2021-02-13T00:05:59Z- handleDvl() - RDI_Pathfinder msg: velocities -0.34 , -0.27 , 0.22 2021-02-13T00:05:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174759.39 epoch sec; seqNo:9642; 4058555.65 north; 591986.67 east 2021-02-13T00:05:59Z- AHRS_M2 msg: 1613174759.76 epoch sec; seqNo:9565 2021-02-13T00:05:59Z- AHRS_M2 msg: 5.80 phi; -0.10 theta; -0.08 psi 2021-02-13T00:05:59Z- handleDepth()-Depth_Keller msg: 1613174759773 epoch sec; seqNo:9643; depth 0.12 2021-02-13T00:05:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174759.79 epoch sec; seqNo:9643; 4058555.27 north; 591986.67 east 2021-02-13T00:06:00Z- AHRS_M2 msg: 1613174760.17 epoch sec; seqNo:9566 2021-02-13T00:06:00Z- AHRS_M2 msg: 5.81 phi; -0.10 theta; -0.07 psi 2021-02-13T00:06:00Z- handleDepth()-Depth_Keller msg: 1613174760177 epoch sec; seqNo:9644; depth 0.09 2021-02-13T00:06:00Z- handleDvl() - RDI_Pathfinder msg: 1613174760.19 epoch sec; seqNo:4679 2021-02-13T00:06:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 102.45 , 97.99 , 106.91 2021-02-13T00:06:00Z- handleDvl() - RDI_Pathfinder msg: velocities -0.28 , -0.04 , 0.14 2021-02-13T00:06:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174760.21 epoch sec; seqNo:9644; 4058555.27 north; 591986.67 east 2021-02-13T00:06:00Z- AHRS_M2 msg: 1613174760.57 epoch sec; seqNo:9567 2021-02-13T00:06:00Z- AHRS_M2 msg: 5.79 phi; -0.08 theta; -0.03 psi 2021-02-13T00:06:00Z- handleDepth()-Depth_Keller msg: 1613174760576 epoch sec; seqNo:9645; depth 0.11 2021-02-13T00:06:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174760.59 epoch sec; seqNo:9645; 4058555.27 north; 591986.67 east 2021-02-13T00:06:00Z- AHRS_M2 msg: 1613174760.98 epoch sec; seqNo:9568 2021-02-13T00:06:00Z- AHRS_M2 msg: 5.77 phi; -0.02 theta; -0.02 psi 2021-02-13T00:06:00Z- handleDepth()-Depth_Keller msg: 1613174760985 epoch sec; seqNo:9646; depth 0.08 2021-02-13T00:06:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174761.00 epoch sec; seqNo:9646; 4058555.27 north; 591986.67 east 2021-02-13T00:06:01Z- AHRS_M2 msg: 1613174761.38 epoch sec; seqNo:9569 2021-02-13T00:06:01Z- AHRS_M2 msg: 5.74 phi; 0.02 theta; -0.03 psi 2021-02-13T00:06:01Z- handleDepth()-Depth_Keller msg: 1613174761384 epoch sec; seqNo:9647; depth 0.04 2021-02-13T00:06:01Z- handleDvl() - RDI_Pathfinder msg: 1613174761.40 epoch sec; seqNo:4680 2021-02-13T00:06:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 99.48 , 97.99 , 103.94 2021-02-13T00:06:01Z- handleDvl() - RDI_Pathfinder msg: velocities -0.02 , -0.20 , -0.09 2021-02-13T00:06:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174761.42 epoch sec; seqNo:9647; 4058554.89 north; 591986.68 east 2021-02-13T00:06:01Z- AHRS_M2 msg: 1613174761.78 epoch sec; seqNo:9570 2021-02-13T00:06:01Z- AHRS_M2 msg: 5.71 phi; -0.03 theta; -0.01 psi 2021-02-13T00:06:01Z- handleDepth()-Depth_Keller msg: 1613174761788 epoch sec; seqNo:9648; depth -0.01 2021-02-13T00:06:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174761.80 epoch sec; seqNo:9648; 4058554.89 north; 591986.68 east 2021-02-13T00:06:02Z- AHRS_M2 msg: 1613174762.19 epoch sec; seqNo:9571 2021-02-13T00:06:02Z- AHRS_M2 msg: 5.74 phi; -0.11 theta; 0.01 psi 2021-02-13T00:06:02Z- handleDepth()-Depth_Keller msg: 1613174762201 epoch sec; seqNo:9649; depth 0.00 2021-02-13T00:06:02Z- handleDvl() - RDI_Pathfinder msg: 1613174762.21 epoch sec; seqNo:4681 2021-02-13T00:06:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.40 , 95.69 , 98.40 , 98.40 2021-02-13T00:06:02Z- handleDvl() - RDI_Pathfinder msg: velocities -0.13 , -0.20 , 0.01 2021-02-13T00:06:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174762.22 epoch sec; seqNo:9649; 4058554.89 north; 591986.68 east 2021-02-13T00:06:02Z- AHRS_M2 msg: 1613174762.59 epoch sec; seqNo:9572 2021-02-13T00:06:02Z- AHRS_M2 msg: 5.77 phi; -0.13 theta; 0.10 psi 2021-02-13T00:06:02Z- handleDepth()-Depth_Keller msg: 1613174762607 epoch sec; seqNo:9650; depth 0.05 2021-02-13T00:06:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174762.62 epoch sec; seqNo:9650; 4058554.89 north; 591986.68 east 2021-02-13T00:06:02Z- AHRS_M2 msg: 1613174763.00 epoch sec; seqNo:9573 2021-02-13T00:06:02Z- AHRS_M2 msg: 5.72 phi; -0.07 theta; 0.13 psi 2021-02-13T00:06:03Z- handleDepth()-Depth_Keller msg: 1613174763000 epoch sec; seqNo:9651; depth 0.12 2021-02-13T00:06:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174763.01 epoch sec; seqNo:9651; 4058554.89 north; 591986.68 east 2021-02-13T00:06:03Z- AHRS_M2 msg: 1613174763.40 epoch sec; seqNo:9574 2021-02-13T00:06:03Z- AHRS_M2 msg: 5.73 phi; 0.04 theta; 0.06 psi 2021-02-13T00:06:03Z- handleDepth()-Depth_Keller msg: 1613174763407 epoch sec; seqNo:9652; depth 0.14 2021-02-13T00:06:03Z- handleDvl() - RDI_Pathfinder msg: 1613174763.42 epoch sec; seqNo:4682 2021-02-13T00:06:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 106.71 , 97.25 , 97.25 , 113.48 2021-02-13T00:06:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.47 , -0.42 2021-02-13T00:06:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174763.43 epoch sec; seqNo:9652; 4058554.89 north; 591986.68 east 2021-02-13T00:06:03Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-13T00:06:03Z- waiting for 1613174761.57, time is 1613174763.43 2021-02-13T00:06:03Z- LcmTrn::updateTrn() equating pose 1613174763.43 and meas times 1613174763.43 2021-02-13T00:06:03Z- LcmTrn::updateTrn() >>>> pose time 1613174763.43, meas time 1613174763.43 <<<< 2021-02-13T00:06:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174763.43 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:06:03Z- TerrainNav::motionUpdate() - currEstimate = (4058555, 591986.68, 0.14) 2021-02-13T00:06:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:06:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:06:03Z- TNavBF::Projecting Measurements 2021-02-13T00:06:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:06:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:06:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:06:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:06:03Z- TNavFilter::4 good beams. 2021-02-13T00:06:03Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:06:03Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:06:03Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:06:03Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:06:03Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:06:03Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:06:03Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:06:03Z- TNavBF::mmse selected from filter -1 2021-02-13T00:06:03Z- TerrainMapOctree: (vehN, vehE ) = (4058554.89, 591986.68). 2021-02-13T00:06:03Z- TerrainMapOctree: Min Distance = 3519.41. 2021-02-13T00:06:03Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:06:03Z- There is no specific good beam that all particles have in common. 2021-02-13T00:06:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:06:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 18.22 2021-02-13T00:06:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:06:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-13T00:06:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:06:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.76 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-13T00:06:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:06:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.73 K: 0.032 ExpectedMeasDiff: nan currMeas.covariance: 20.60 2021-02-13T00:06:03Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:06:03Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:06:03Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:06:03Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:06:03Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:06:03Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:06:03Z- Filter 0 No beams used in subcloud update 2021-02-13T00:06:03Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:06:03Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:06:03Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 719.1 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:06:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174763.43 sec. 2021-02-13T00:06:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:06:03Z- TNavBF::mmse selected from filter -1 2021-02-13T00:06:03Z- variances([0], [2], and psi_berg) = 719.065654 833.009058 0.000000 2021-02-13T00:06:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:06:03Z- LcmTrn::publishEstimates() - MLE : 4058552.39 591944.18 0.14 2021-02-13T00:06:03Z- LcmTrn::publishEstimates() - MMSE : 4058554.89 591986.68 0.14 2021-02-13T00:06:03Z- LcmTrn::publishEstimates() - COVAR: 719.07 833.01 0.71 0.00 2021-02-13T00:06:03Z- AHRS_M2 msg: 1613174763.80 epoch sec; seqNo:9575 2021-02-13T00:06:03Z- AHRS_M2 msg: 5.78 phi; 0.04 theta; 0.00 psi 2021-02-13T00:06:03Z- handleDepth()-Depth_Keller msg: 1613174763808 epoch sec; seqNo:9653; depth 0.14 2021-02-13T00:06:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174763.83 epoch sec; seqNo:9653; 4058554.88 north; 591985.46 east 2021-02-13T00:06:04Z- AHRS_M2 msg: 1613174764.21 epoch sec; seqNo:9576 2021-02-13T00:06:04Z- AHRS_M2 msg: 5.75 phi; -0.09 theta; 0.03 psi 2021-02-13T00:06:04Z- handleDepth()-Depth_Keller msg: 1613174764212 epoch sec; seqNo:9654; depth 0.17 2021-02-13T00:06:04Z- handleDvl() - RDI_Pathfinder msg: 1613174764.22 epoch sec; seqNo:4683 2021-02-13T00:06:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.28 , 97.79 , 102.25 , 97.79 2021-02-13T00:06:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.13 , -0.11 , 1.01 2021-02-13T00:06:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174764.26 epoch sec; seqNo:9654; 4058554.88 north; 591985.46 east 2021-02-13T00:06:04Z- AHRS_M2 msg: 1613174764.61 epoch sec; seqNo:9577 2021-02-13T00:06:04Z- AHRS_M2 msg: 5.69 phi; -0.20 theta; 0.08 psi 2021-02-13T00:06:04Z- handleDepth()-Depth_Keller msg: 1613174764621 epoch sec; seqNo:9655; depth 0.22 2021-02-13T00:06:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174764.64 epoch sec; seqNo:9655; 4058554.88 north; 591985.46 east 2021-02-13T00:06:05Z- AHRS_M2 msg: 1613174765.02 epoch sec; seqNo:9578 2021-02-13T00:06:05Z- AHRS_M2 msg: 5.66 phi; -0.23 theta; 0.00 psi 2021-02-13T00:06:05Z- handleDepth()-Depth_Keller msg: 1613174765027 epoch sec; seqNo:9656; depth 0.20 2021-02-13T00:06:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174765.04 epoch sec; seqNo:9656; 4058554.88 north; 591985.46 east 2021-02-13T00:06:05Z- AHRS_M2 msg: 1613174765.42 epoch sec; seqNo:9579 2021-02-13T00:06:05Z- AHRS_M2 msg: 5.64 phi; -0.29 theta; -0.15 psi 2021-02-13T00:06:05Z- handleDepth()-Depth_Keller msg: 1613174765430 epoch sec; seqNo:9657; depth 0.16 2021-02-13T00:06:05Z- handleDvl() - RDI_Pathfinder msg: 1613174765.45 epoch sec; seqNo:4684 2021-02-13T00:06:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 106.58 , 97.11 , 97.11 , 114.69 2021-02-13T00:06:05Z- handleDvl() - RDI_Pathfinder msg: velocities -0.88 , 0.12 , 0.02 2021-02-13T00:06:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174765.46 epoch sec; seqNo:9657; 4058554.51 north; 591986.68 east 2021-02-13T00:06:05Z- AHRS_M2 msg: 1613174765.83 epoch sec; seqNo:9580 2021-02-13T00:06:05Z- AHRS_M2 msg: 5.66 phi; -0.40 theta; -0.12 psi 2021-02-13T00:06:05Z- handleDepth()-Depth_Keller msg: 1613174765829 epoch sec; seqNo:9658; depth 0.14 2021-02-13T00:06:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174765.84 epoch sec; seqNo:9658; 4058554.51 north; 591986.68 east 2021-02-13T00:06:06Z- AHRS_M2 msg: 1613174766.23 epoch sec; seqNo:9581 2021-02-13T00:06:06Z- AHRS_M2 msg: 5.60 phi; -0.40 theta; -0.00 psi 2021-02-13T00:06:06Z- handleDepth()-Depth_Keller msg: 1613174766233 epoch sec; seqNo:9659; depth 0.18 2021-02-13T00:06:06Z- handleDvl() - RDI_Pathfinder msg: 1613174766.26 epoch sec; seqNo:4685 2021-02-13T00:06:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.79 , 112.67 , 97.79 , 123.08 2021-02-13T00:06:06Z- handleDvl() - RDI_Pathfinder msg: velocities -0.70 , 0.10 , -0.62 2021-02-13T00:06:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174766.28 epoch sec; seqNo:9659; 4058554.51 north; 591986.68 east 2021-02-13T00:06:06Z- AHRS_M2 msg: 1613174766.63 epoch sec; seqNo:9582 2021-02-13T00:06:06Z- AHRS_M2 msg: 5.54 phi; -0.33 theta; 0.04 psi 2021-02-13T00:06:06Z- handleDepth()-Depth_Keller msg: 1613174766649 epoch sec; seqNo:9660; depth 0.19 2021-02-13T00:06:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174766.68 epoch sec; seqNo:9660; 4058554.13 north; 591986.69 east 2021-02-13T00:06:07Z- AHRS_M2 msg: 1613174767.04 epoch sec; seqNo:9583 2021-02-13T00:06:07Z- AHRS_M2 msg: 5.56 phi; -0.28 theta; -0.04 psi 2021-02-13T00:06:07Z- handleDepth()-Depth_Keller msg: 1613174767040 epoch sec; seqNo:9661; depth 0.15 2021-02-13T00:06:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174767.06 epoch sec; seqNo:9661; 4058554.13 north; 591986.69 east 2021-02-13T00:06:07Z- AHRS_M2 msg: 1613174767.44 epoch sec; seqNo:9584 2021-02-13T00:06:07Z- AHRS_M2 msg: 5.57 phi; -0.25 theta; -0.04 psi 2021-02-13T00:06:07Z- handleDepth()-Depth_Keller msg: 1613174767449 epoch sec; seqNo:9662; depth 0.13 2021-02-13T00:06:07Z- handleDvl() - RDI_Pathfinder msg: 1613174767.46 epoch sec; seqNo:4686 2021-02-13T00:06:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:06:07Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:06:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174767.49 epoch sec; seqNo:9662; 4058554.13 north; 591986.69 east 2021-02-13T00:06:07Z- AHRS_M2 msg: 1613174767.84 epoch sec; seqNo:9585 2021-02-13T00:06:07Z- AHRS_M2 msg: 5.54 phi; -0.20 theta; -0.07 psi 2021-02-13T00:06:07Z- handleDepth()-Depth_Keller msg: 1613174767850 epoch sec; seqNo:9663; depth 0.12 2021-02-13T00:06:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174767.86 epoch sec; seqNo:9663; 4058553.75 north; 591986.69 east 2021-02-13T00:06:08Z- AHRS_M2 msg: 1613174768.25 epoch sec; seqNo:9586 2021-02-13T00:06:08Z- AHRS_M2 msg: 5.49 phi; -0.15 theta; -0.11 psi 2021-02-13T00:06:08Z- handleDepth()-Depth_Keller msg: 1613174768257 epoch sec; seqNo:9664; depth 0.19 2021-02-13T00:06:08Z- handleDvl() - RDI_Pathfinder msg: 1613174768.27 epoch sec; seqNo:4687 2021-02-13T00:06:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.74 , 104.69 , 97.25 , 119.56 2021-02-13T00:06:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.59 , -0.43 , 0.02 2021-02-13T00:06:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174768.28 epoch sec; seqNo:9664; 4058553.75 north; 591986.69 east 2021-02-13T00:06:08Z- AHRS_M2 msg: 1613174768.65 epoch sec; seqNo:9587 2021-02-13T00:06:08Z- AHRS_M2 msg: 5.45 phi; -0.11 theta; -0.16 psi 2021-02-13T00:06:08Z- handleDepth()-Depth_Keller msg: 1613174768657 epoch sec; seqNo:9665; depth 0.19 2021-02-13T00:06:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174768.69 epoch sec; seqNo:9665; 4058553.75 north; 591986.69 east 2021-02-13T00:06:08Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-13T00:06:08Z- waiting for 1613174766.43, time is 1613174768.69 2021-02-13T00:06:08Z- LcmTrn::updateTrn() >>>> pose time 1613174768.69, meas time 1613174768.27 <<<< 2021-02-13T00:06:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174768.27 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:06:08Z- TerrainNav::motionUpdate() - currEstimate = (4058554, 591986.69, 0.19) 2021-02-13T00:06:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:06:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:06:08Z- TNavBF::Projecting Measurements 2021-02-13T00:06:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:06:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:06:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:06:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:06:08Z- TNavFilter::4 good beams. 2021-02-13T00:06:08Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:06:08Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:06:08Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:06:08Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:06:08Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:06:08Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:06:08Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:06:08Z- TNavBF::mmse selected from filter -1 2021-02-13T00:06:08Z- TerrainMapOctree: (vehN, vehE ) = (4058553.84, 591986.69). 2021-02-13T00:06:08Z- TerrainMapOctree: Min Distance = 3520.21. 2021-02-13T00:06:08Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:06:08Z- There is no specific good beam that all particles have in common. 2021-02-13T00:06:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:06:08Z- Old Depth Bias: 0.00 DepthBiasCov: 0.76 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 15.60 2021-02-13T00:06:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:06:08Z- Old Depth Bias: 0.00 DepthBiasCov: 0.73 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 17.53 2021-02-13T00:06:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:06:08Z- Old Depth Bias: 0.00 DepthBiasCov: 0.70 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-13T00:06:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:06:08Z- Old Depth Bias: 0.00 DepthBiasCov: 0.67 K: 0.027 ExpectedMeasDiff: nan currMeas.covariance: 22.87 2021-02-13T00:06:08Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:06:08Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:06:08Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:06:08Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:06:08Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:06:08Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:06:08Z- Filter 0 No beams used in subcloud update 2021-02-13T00:06:08Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:06:08Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:06:08Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 719.0 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:06:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174768.27 sec. 2021-02-13T00:06:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:06:08Z- TNavBF::mmse selected from filter -1 2021-02-13T00:06:08Z- variances([0], [2], and psi_berg) = 719.021478 833.009030 0.000000 2021-02-13T00:06:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:06:08Z- LcmTrn::publishEstimates() - MLE : 4058551.25 591944.19 0.19 2021-02-13T00:06:08Z- LcmTrn::publishEstimates() - MMSE : 4058553.75 591986.69 0.19 2021-02-13T00:06:08Z- LcmTrn::publishEstimates() - COVAR: 719.02 833.01 0.66 0.00 2021-02-13T00:06:09Z- AHRS_M2 msg: 1613174769.06 epoch sec; seqNo:9588 2021-02-13T00:06:09Z- AHRS_M2 msg: 5.50 phi; -0.10 theta; -0.15 psi 2021-02-13T00:06:09Z- handleDepth()-Depth_Keller msg: 1613174769065 epoch sec; seqNo:9666; depth 0.17 2021-02-13T00:06:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174769.10 epoch sec; seqNo:9666; 4058553.75 north; 591986.69 east 2021-02-13T00:06:09Z- AHRS_M2 msg: 1613174769.46 epoch sec; seqNo:9589 2021-02-13T00:06:09Z- AHRS_M2 msg: 5.56 phi; -0.13 theta; -0.06 psi 2021-02-13T00:06:09Z- handleDepth()-Depth_Keller msg: 1613174769475 epoch sec; seqNo:9667; depth 0.13 2021-02-13T00:06:09Z- handleDvl() - RDI_Pathfinder msg: 1613174769.49 epoch sec; seqNo:4688 2021-02-13T00:06:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.19 , 108.07 , 97.65 , 109.56 2021-02-13T00:06:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.41 , -0.41 , 0.87 2021-02-13T00:06:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174769.50 epoch sec; seqNo:9667; 4058553.74 north; 591985.47 east 2021-02-13T00:06:09Z- AHRS_M2 msg: 1613174769.86 epoch sec; seqNo:9590 2021-02-13T00:06:09Z- AHRS_M2 msg: 5.55 phi; -0.17 theta; 0.09 psi 2021-02-13T00:06:09Z- handleDepth()-Depth_Keller msg: 1613174769869 epoch sec; seqNo:9668; depth 0.16 2021-02-13T00:06:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174769.89 epoch sec; seqNo:9668; 4058553.74 north; 591985.47 east 2021-02-13T00:06:10Z- AHRS_M2 msg: 1613174770.27 epoch sec; seqNo:9591 2021-02-13T00:06:10Z- AHRS_M2 msg: 5.49 phi; -0.11 theta; 0.15 psi 2021-02-13T00:06:10Z- handleDepth()-Depth_Keller msg: 1613174770277 epoch sec; seqNo:9669; depth 0.21 2021-02-13T00:06:10Z- handleDvl() - RDI_Pathfinder msg: 1613174770.30 epoch sec; seqNo:4689 2021-02-13T00:06:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 100.90 , 97.92 , 106.85 2021-02-13T00:06:10Z- handleDvl() - RDI_Pathfinder msg: velocities -0.18 , 0.25 , 0.52 2021-02-13T00:06:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174770.31 epoch sec; seqNo:9669; 4058553.74 north; 591985.47 east 2021-02-13T00:06:10Z- AHRS_M2 msg: 1613174770.67 epoch sec; seqNo:9592 2021-02-13T00:06:10Z- AHRS_M2 msg: 5.46 phi; -0.06 theta; 0.00 psi 2021-02-13T00:06:10Z- handleDepth()-Depth_Keller msg: 1613174770685 epoch sec; seqNo:9670; depth 0.15 2021-02-13T00:06:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174770.70 epoch sec; seqNo:9670; 4058553.75 north; 591986.69 east 2021-02-13T00:06:11Z- AHRS_M2 msg: 1613174771.08 epoch sec; seqNo:9593 2021-02-13T00:06:11Z- AHRS_M2 msg: 5.42 phi; -0.13 theta; -0.20 psi 2021-02-13T00:06:11Z- handleDepth()-Depth_Keller msg: 1613174771082 epoch sec; seqNo:9671; depth 0.07 2021-02-13T00:06:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174771.09 epoch sec; seqNo:9671; 4058553.75 north; 591986.69 east 2021-02-13T00:06:11Z- AHRS_M2 msg: 1613174771.48 epoch sec; seqNo:9594 2021-02-13T00:06:11Z- AHRS_M2 msg: 5.40 phi; -0.17 theta; -0.35 psi 2021-02-13T00:06:11Z- handleDepth()-Depth_Keller msg: 1613174771489 epoch sec; seqNo:9672; depth 0.06 2021-02-13T00:06:11Z- handleDvl() - RDI_Pathfinder msg: 1613174771.51 epoch sec; seqNo:4690 2021-02-13T00:06:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.74 , 99.68 , 96.98 , 96.98 2021-02-13T00:06:11Z- handleDvl() - RDI_Pathfinder msg: velocities -0.30 , -0.10 , 0.65 2021-02-13T00:06:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174771.52 epoch sec; seqNo:9672; 4058553.75 north; 591986.69 east 2021-02-13T00:06:11Z- AHRS_M2 msg: 1613174771.88 epoch sec; seqNo:9595 2021-02-13T00:06:11Z- AHRS_M2 msg: 5.43 phi; -0.16 theta; -0.36 psi 2021-02-13T00:06:11Z- handleDepth()-Depth_Keller msg: 1613174771893 epoch sec; seqNo:9673; depth 0.01 2021-02-13T00:06:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174771.90 epoch sec; seqNo:9673; 4058553.75 north; 591986.69 east 2021-02-13T00:06:12Z- AHRS_M2 msg: 1613174772.29 epoch sec; seqNo:9596 2021-02-13T00:06:12Z- AHRS_M2 msg: 5.47 phi; -0.21 theta; -0.20 psi 2021-02-13T00:06:12Z- handleDepth()-Depth_Keller msg: 1613174772297 epoch sec; seqNo:9674; depth 0.01 2021-02-13T00:06:12Z- handleDvl() - RDI_Pathfinder msg: 1613174772.31 epoch sec; seqNo:4691 2021-02-13T00:06:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.00 , 130.93 , 98.46 , 121.46 2021-02-13T00:06:12Z- handleDvl() - RDI_Pathfinder msg: velocities -0.87 , -0.57 , -0.04 2021-02-13T00:06:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174772.32 epoch sec; seqNo:9674; 4058553.37 north; 591986.69 east 2021-02-13T00:06:12Z- AHRS_M2 msg: 1613174772.69 epoch sec; seqNo:9597 2021-02-13T00:06:12Z- AHRS_M2 msg: 5.48 phi; -0.22 theta; 0.04 psi 2021-02-13T00:06:12Z- handleDepth()-Depth_Keller msg: 1613174772696 epoch sec; seqNo:9675; depth 0.03 2021-02-13T00:06:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174772.71 epoch sec; seqNo:9675; 4058552.99 north; 591986.70 east 2021-02-13T00:06:13Z- AHRS_M2 msg: 1613174773.12 epoch sec; seqNo:9598 2021-02-13T00:06:13Z- AHRS_M2 msg: 5.45 phi; -0.10 theta; 0.16 psi 2021-02-13T00:06:13Z- handleDepth()-Depth_Keller msg: 1613174773131 epoch sec; seqNo:9676; depth 0.07 2021-02-13T00:06:13Z- handleDvl() - RDI_Pathfinder msg: 1613174773.15 epoch sec; seqNo:4692 2021-02-13T00:06:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 100.22 , 98.74 , 97.25 , 0.00 2021-02-13T00:06:13Z- handleDvl() - RDI_Pathfinder msg: velocities -0.08 , 0.03 , -0.99 2021-02-13T00:06:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174773.16 epoch sec; seqNo:9676; 4058552.99 north; 591986.70 east 2021-02-13T00:06:13Z- AHRS_M2 msg: 1613174773.50 epoch sec; seqNo:9599 2021-02-13T00:06:13Z- AHRS_M2 msg: 5.49 phi; 0.05 theta; 0.09 psi 2021-02-13T00:06:13Z- handleDepth()-Depth_Keller msg: 1613174773504 epoch sec; seqNo:9677; depth 0.05 2021-02-13T00:06:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174773.52 epoch sec; seqNo:9677; 4058552.99 north; 591986.70 east 2021-02-13T00:06:13Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-13T00:06:13Z- waiting for 1613174771.69, time is 1613174773.52 2021-02-13T00:06:13Z- LcmTrn::updateTrn() >>>> pose time 1613174773.52, meas time 1613174773.15 <<<< 2021-02-13T00:06:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174773.15 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:06:13Z- TerrainNav::motionUpdate() - currEstimate = (4058553, 591986.70, 0.05) 2021-02-13T00:06:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:06:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:06:13Z- TNavBF::Projecting Measurements 2021-02-13T00:06:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:06:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:06:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:06:13Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:06:13Z- TNavFilter::3 good beams. 2021-02-13T00:06:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:06:13Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:06:13Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:06:13Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:06:13Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:06:13Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:06:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:06:13Z- TNavBF::mmse selected from filter -1 2021-02-13T00:06:13Z- TerrainMapOctree: (vehN, vehE ) = (4058553.05, 591986.70). 2021-02-13T00:06:13Z- TerrainMapOctree: Min Distance = 3520.81. 2021-02-13T00:06:13Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:06:13Z- There is no specific good beam that all particles have in common. 2021-02-13T00:06:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:06:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.70 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 16.07 2021-02-13T00:06:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:06:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.67 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 15.60 2021-02-13T00:06:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:06:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.65 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-13T00:06:13Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:06:13Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:06:13Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:06:13Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:06:13Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:06:13Z- Filter 0 No beams used in subcloud update 2021-02-13T00:06:13Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:06:13Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:06:13Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 719.0 EastVariance: 833.0 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-13T00:06:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174773.15 sec. 2021-02-13T00:06:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:06:13Z- TNavBF::mmse selected from filter -1 2021-02-13T00:06:13Z- variances([0], [2], and psi_berg) = 718.985573 833.009006 0.000000 2021-02-13T00:06:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:06:13Z- LcmTrn::publishEstimates() - MLE : 4058550.49 591944.20 0.05 2021-02-13T00:06:13Z- LcmTrn::publishEstimates() - MMSE : 4058552.99 591986.70 0.05 2021-02-13T00:06:13Z- LcmTrn::publishEstimates() - COVAR: 718.99 833.01 0.62 0.00 2021-02-13T00:06:13Z- AHRS_M2 msg: 1613174773.90 epoch sec; seqNo:9600 2021-02-13T00:06:13Z- AHRS_M2 msg: 5.50 phi; 0.10 theta; -0.06 psi 2021-02-13T00:06:13Z- handleDepth()-Depth_Keller msg: 1613174773909 epoch sec; seqNo:9678; depth 0.08 2021-02-13T00:06:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174773.92 epoch sec; seqNo:9678; 4058552.99 north; 591986.70 east 2021-02-13T00:06:14Z- AHRS_M2 msg: 1613174774.31 epoch sec; seqNo:9601 2021-02-13T00:06:14Z- AHRS_M2 msg: 5.45 phi; 0.02 theta; -0.20 psi 2021-02-13T00:06:14Z- handleDepth()-Depth_Keller msg: 1613174774322 epoch sec; seqNo:9679; depth 0.11 2021-02-13T00:06:14Z- handleDvl() - RDI_Pathfinder msg: 1613174774.33 epoch sec; seqNo:4693 2021-02-13T00:06:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.94 , 97.18 , 0.00 , 97.18 2021-02-13T00:06:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.39 , 0.31 , -0.04 2021-02-13T00:06:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174774.34 epoch sec; seqNo:9679; 4058553.37 north; 591986.69 east 2021-02-13T00:06:14Z- AHRS_M2 msg: 1613174774.71 epoch sec; seqNo:9602 2021-02-13T00:06:14Z- AHRS_M2 msg: 5.44 phi; 0.00 theta; -0.30 psi 2021-02-13T00:06:14Z- handleDepth()-Depth_Keller msg: 1613174774716 epoch sec; seqNo:9680; depth 0.09 2021-02-13T00:06:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174774.73 epoch sec; seqNo:9680; 4058553.37 north; 591986.69 east 2021-02-13T00:06:15Z- AHRS_M2 msg: 1613174775.12 epoch sec; seqNo:9603 2021-02-13T00:06:15Z- AHRS_M2 msg: 5.47 phi; 0.06 theta; -0.23 psi 2021-02-13T00:06:15Z- handleDepth()-Depth_Keller msg: 1613174775125 epoch sec; seqNo:9681; depth 0.08 2021-02-13T00:06:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174775.18 epoch sec; seqNo:9681; 4058553.75 north; 591986.69 east 2021-02-13T00:06:15Z- AHRS_M2 msg: 1613174775.52 epoch sec; seqNo:9604 2021-02-13T00:06:15Z- AHRS_M2 msg: 5.47 phi; 0.02 theta; -0.09 psi 2021-02-13T00:06:15Z- handleDepth()-Depth_Keller msg: 1613174775527 epoch sec; seqNo:9682; depth 0.05 2021-02-13T00:06:15Z- handleDvl() - RDI_Pathfinder msg: 1613174775.55 epoch sec; seqNo:4694 2021-02-13T00:06:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 121.46 , 101.17 , 101.17 2021-02-13T00:06:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.38 , -0.54 , -0.47 2021-02-13T00:06:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174775.56 epoch sec; seqNo:9682; 4058553.37 north; 591986.69 east 2021-02-13T00:06:15Z- AHRS_M2 msg: 1613174775.92 epoch sec; seqNo:9605 2021-02-13T00:06:15Z- AHRS_M2 msg: 5.41 phi; -0.00 theta; -0.00 psi 2021-02-13T00:06:15Z- handleDepth()-Depth_Keller msg: 1613174775928 epoch sec; seqNo:9683; depth 0.09 2021-02-13T00:06:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174775.94 epoch sec; seqNo:9683; 4058553.37 north; 591986.69 east 2021-02-13T00:06:16Z- AHRS_M2 msg: 1613174776.33 epoch sec; seqNo:9606 2021-02-13T00:06:16Z- AHRS_M2 msg: 5.40 phi; 0.02 theta; -0.04 psi 2021-02-13T00:06:16Z- handleDepth()-Depth_Keller msg: 1613174776336 epoch sec; seqNo:9684; depth 0.05 2021-02-13T00:06:16Z- handleDvl() - RDI_Pathfinder msg: 1613174776.35 epoch sec; seqNo:4695 2021-02-13T00:06:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.65 , 99.14 , 97.65 , 99.14 2021-02-13T00:06:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.56 , -0.55 , 0.14 2021-02-13T00:06:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174776.38 epoch sec; seqNo:9684; 4058553.36 north; 591985.47 east 2021-02-13T00:06:16Z- AHRS_M2 msg: 1613174776.73 epoch sec; seqNo:9607 2021-02-13T00:06:16Z- AHRS_M2 msg: 5.48 phi; -0.00 theta; -0.05 psi 2021-02-13T00:06:16Z- handleDepth()-Depth_Keller msg: 1613174776737 epoch sec; seqNo:9685; depth 0.00 2021-02-13T00:06:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174776.75 epoch sec; seqNo:9685; 4058553.36 north; 591985.47 east 2021-02-13T00:06:17Z- AHRS_M2 msg: 1613174777.14 epoch sec; seqNo:9608 2021-02-13T00:06:17Z- AHRS_M2 msg: 5.54 phi; -0.06 theta; 0.10 psi 2021-02-13T00:06:17Z- handleDepth()-Depth_Keller msg: 1613174777143 epoch sec; seqNo:9686; depth 0.03 2021-02-13T00:06:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174777.20 epoch sec; seqNo:9686; 4058553.36 north; 591985.47 east 2021-02-13T00:06:17Z- AHRS_M2 msg: 1613174777.54 epoch sec; seqNo:9609 2021-02-13T00:06:17Z- AHRS_M2 msg: 5.50 phi; -0.07 theta; 0.19 psi 2021-02-13T00:06:17Z- handleDepth()-Depth_Keller msg: 1613174777549 epoch sec; seqNo:9687; depth 0.06 2021-02-13T00:06:17Z- handleDvl() - RDI_Pathfinder msg: 1613174777.59 epoch sec; seqNo:4696 2021-02-13T00:06:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.18 , 102.59 , 99.89 , 98.53 2021-02-13T00:06:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.02 , -0.23 , 0.68 2021-02-13T00:06:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174777.66 epoch sec; seqNo:9687; 4058553.36 north; 591985.47 east 2021-02-13T00:06:17Z- AHRS_M2 msg: 1613174777.94 epoch sec; seqNo:9610 2021-02-13T00:06:17Z- AHRS_M2 msg: 5.41 phi; -0.02 theta; 0.07 psi 2021-02-13T00:06:17Z- handleDepth()-Depth_Keller msg: 1613174777948 epoch sec; seqNo:9688; depth 0.04 2021-02-13T00:06:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174777.96 epoch sec; seqNo:9688; 4058553.36 north; 591985.47 east 2021-02-13T00:06:18Z- AHRS_M2 msg: 1613174778.35 epoch sec; seqNo:9611 2021-02-13T00:06:18Z- AHRS_M2 msg: 5.41 phi; 0.00 theta; -0.17 psi 2021-02-13T00:06:18Z- handleDepth()-Depth_Keller msg: 1613174778357 epoch sec; seqNo:9689; depth 0.00 2021-02-13T00:06:18Z- handleDvl() - RDI_Pathfinder msg: 1613174778.37 epoch sec; seqNo:4697 2021-02-13T00:06:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 110.64 , 97.11 , 97.11 , 99.82 2021-02-13T00:06:18Z- handleDvl() - RDI_Pathfinder msg: velocities -0.12 , -0.20 , 0.77 2021-02-13T00:06:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174778.38 epoch sec; seqNo:9689; 4058553.36 north; 591985.47 east 2021-02-13T00:06:18Z- AHRS_M2 msg: 1613174778.75 epoch sec; seqNo:9612 2021-02-13T00:06:18Z- AHRS_M2 msg: 5.43 phi; 0.01 theta; -0.30 psi 2021-02-13T00:06:18Z- handleDepth()-Depth_Keller msg: 1613174778761 epoch sec; seqNo:9690; depth -0.03 2021-02-13T00:06:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174778.78 epoch sec; seqNo:9690; 4058553.36 north; 591985.47 east 2021-02-13T00:06:18Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 132 calls... 2021-02-13T00:06:18Z- waiting for 1613174776.52, time is 1613174778.78 2021-02-13T00:06:18Z- LcmTrn::updateTrn() >>>> pose time 1613174778.78, meas time 1613174778.37 <<<< 2021-02-13T00:06:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174778.37 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:06:18Z- TerrainNav::motionUpdate() - currEstimate = (4058553, 591985.47, -0.03) 2021-02-13T00:06:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:06:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:06:18Z- TNavBF::Projecting Measurements 2021-02-13T00:06:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:06:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:06:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:06:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:06:18Z- TNavFilter::4 good beams. 2021-02-13T00:06:18Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:06:18Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:06:18Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:06:18Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:06:18Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:06:18Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:06:18Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:06:18Z- TNavBF::mmse selected from filter -1 2021-02-13T00:06:18Z- TerrainMapOctree: (vehN, vehE ) = (4058553.33, 591985.57). 2021-02-13T00:06:18Z- TerrainMapOctree: Min Distance = 3521.32. 2021-02-13T00:06:18Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:06:19Z- There is no specific good beam that all particles have in common. 2021-02-13T00:06:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:06:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.68 K: 0.031 ExpectedMeasDiff: nan currMeas.covariance: 19.58 2021-02-13T00:06:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:06:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.66 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-13T00:06:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:06:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.64 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-13T00:06:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:06:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.61 K: 0.034 ExpectedMeasDiff: nan currMeas.covariance: 15.94 2021-02-13T00:06:19Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:06:19Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:06:19Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:06:19Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:06:19Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:06:19Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:06:19Z- Filter 0 No beams used in subcloud update 2021-02-13T00:06:19Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:06:19Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:06:19Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 718.9 EastVariance: 833.0 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-13T00:06:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174778.37 sec. 2021-02-13T00:06:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:06:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:06:19Z- variances([0], [2], and psi_berg) = 718.938869 833.008976 0.000000 2021-02-13T00:06:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:06:19Z- LcmTrn::publishEstimates() - MLE : 4058550.86 591942.97 -0.03 2021-02-13T00:06:19Z- LcmTrn::publishEstimates() - MMSE : 4058553.36 591985.47 -0.03 2021-02-13T00:06:19Z- LcmTrn::publishEstimates() - COVAR: 718.94 833.01 0.60 0.00 2021-02-13T00:06:19Z- AHRS_M2 msg: 1613174779.18 epoch sec; seqNo:9613 2021-02-13T00:06:19Z- AHRS_M2 msg: 5.43 phi; 0.02 theta; -0.29 psi 2021-02-13T00:06:19Z- handleDepth()-Depth_Keller msg: 1613174779190 epoch sec; seqNo:9691; depth -0.00 2021-02-13T00:06:19Z- handleDvl() - RDI_Pathfinder msg: 1613174779.20 epoch sec; seqNo:4698 2021-02-13T00:06:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 126.26 , 103.94 , 103.94 2021-02-13T00:06:19Z- handleDvl() - RDI_Pathfinder msg: velocities -0.33 , -0.11 , 0.18 2021-02-13T00:06:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174779.21 epoch sec; seqNo:9691; 4058552.98 north; 591985.48 east 2021-02-13T00:06:19Z- AHRS_M2 msg: 1613174779.56 epoch sec; seqNo:9614 2021-02-13T00:06:19Z- AHRS_M2 msg: 5.40 phi; 0.01 theta; -0.18 psi 2021-02-13T00:06:19Z- handleDepth()-Depth_Keller msg: 1613174779569 epoch sec; seqNo:9692; depth 0.03 2021-02-13T00:06:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174779.58 epoch sec; seqNo:9692; 4058552.98 north; 591985.48 east 2021-02-13T00:06:19Z- AHRS_M2 msg: 1613174779.96 epoch sec; seqNo:9615 2021-02-13T00:06:19Z- AHRS_M2 msg: 5.39 phi; -0.03 theta; -0.06 psi 2021-02-13T00:06:19Z- handleDepth()-Depth_Keller msg: 1613174779970 epoch sec; seqNo:9693; depth 0.03 2021-02-13T00:06:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174779.98 epoch sec; seqNo:9693; 4058552.98 north; 591985.48 east 2021-02-13T00:06:20Z- AHRS_M2 msg: 1613174780.37 epoch sec; seqNo:9616 2021-02-13T00:06:20Z- AHRS_M2 msg: 5.38 phi; -0.14 theta; 0.03 psi 2021-02-13T00:06:20Z- handleDepth()-Depth_Keller msg: 1613174780373 epoch sec; seqNo:9694; depth 0.06 2021-02-13T00:06:20Z- handleDvl() - RDI_Pathfinder msg: 1613174780.39 epoch sec; seqNo:4699 2021-02-13T00:06:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.45 , 106.38 , 100.43 , 98.94 2021-02-13T00:06:20Z- handleDvl() - RDI_Pathfinder msg: velocities -0.67 , -0.14 , -0.17 2021-02-13T00:06:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174780.40 epoch sec; seqNo:9694; 4058552.60 north; 591985.48 east 2021-02-13T00:06:20Z- AHRS_M2 msg: 1613174780.77 epoch sec; seqNo:9617 2021-02-13T00:06:20Z- AHRS_M2 msg: 5.39 phi; -0.18 theta; -0.01 psi 2021-02-13T00:06:20Z- handleDepth()-Depth_Keller msg: 1613174780781 epoch sec; seqNo:9695; depth 0.05 2021-02-13T00:06:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174780.79 epoch sec; seqNo:9695; 4058552.60 north; 591985.48 east 2021-02-13T00:06:21Z- AHRS_M2 msg: 1613174781.18 epoch sec; seqNo:9618 2021-02-13T00:06:21Z- AHRS_M2 msg: 5.42 phi; -0.11 theta; -0.08 psi 2021-02-13T00:06:21Z- handleDepth()-Depth_Keller msg: 1613174781185 epoch sec; seqNo:9696; depth 0.04 2021-02-13T00:06:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174781.19 epoch sec; seqNo:9696; 4058552.22 north; 591985.48 east 2021-02-13T00:06:21Z- AHRS_M2 msg: 1613174781.58 epoch sec; seqNo:9619 2021-02-13T00:06:21Z- AHRS_M2 msg: 5.47 phi; -0.02 theta; -0.05 psi 2021-02-13T00:06:21Z- handleDepth()-Depth_Keller msg: 1613174781593 epoch sec; seqNo:9697; depth 0.04 2021-02-13T00:06:21Z- handleDvl() - RDI_Pathfinder msg: 1613174781.61 epoch sec; seqNo:4700 2021-02-13T00:06:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.33 , 102.39 , 96.98 , 109.15 2021-02-13T00:06:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.42 , -0.42 , -0.70 2021-02-13T00:06:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174781.62 epoch sec; seqNo:9697; 4058552.22 north; 591985.48 east 2021-02-13T00:06:21Z- AHRS_M2 msg: 1613174781.99 epoch sec; seqNo:9620 2021-02-13T00:06:21Z- AHRS_M2 msg: 5.54 phi; 0.02 theta; 0.02 psi 2021-02-13T00:06:22Z- handleDepth()-Depth_Keller msg: 1613174782003 epoch sec; seqNo:9698; depth 0.07 2021-02-13T00:06:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174782.02 epoch sec; seqNo:9698; 4058552.22 north; 591985.48 east 2021-02-13T00:06:22Z- AHRS_M2 msg: 1613174782.39 epoch sec; seqNo:9621 2021-02-13T00:06:22Z- AHRS_M2 msg: 5.48 phi; 0.02 theta; -0.02 psi 2021-02-13T00:06:22Z- handleDepth()-Depth_Keller msg: 1613174782398 epoch sec; seqNo:9699; depth 0.09 2021-02-13T00:06:22Z- handleDvl() - RDI_Pathfinder msg: 1613174782.41 epoch sec; seqNo:4701 2021-02-13T00:06:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.48 , 97.99 , 100.97 , 97.99 2021-02-13T00:06:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.53 , 0.19 , 0.36 2021-02-13T00:06:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174782.45 epoch sec; seqNo:9699; 4058552.60 north; 591985.48 east 2021-02-13T00:06:22Z- AHRS_M2 msg: 1613174782.80 epoch sec; seqNo:9622 2021-02-13T00:06:22Z- AHRS_M2 msg: 5.48 phi; -0.01 theta; -0.14 psi 2021-02-13T00:06:22Z- handleDepth()-Depth_Keller msg: 1613174782800 epoch sec; seqNo:9700; depth 0.05 2021-02-13T00:06:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174782.81 epoch sec; seqNo:9700; 4058552.60 north; 591985.48 east 2021-02-13T00:06:23Z- AHRS_M2 msg: 1613174783.20 epoch sec; seqNo:9623 2021-02-13T00:06:23Z- AHRS_M2 msg: 5.53 phi; -0.09 theta; -0.18 psi 2021-02-13T00:06:23Z- handleDepth()-Depth_Keller msg: 1613174783205 epoch sec; seqNo:9701; depth 0.02 2021-02-13T00:06:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174783.23 epoch sec; seqNo:9701; 4058552.98 north; 591985.48 east 2021-02-13T00:06:23Z- AHRS_M2 msg: 1613174783.60 epoch sec; seqNo:9624 2021-02-13T00:06:23Z- AHRS_M2 msg: 5.55 phi; -0.13 theta; -0.13 psi 2021-02-13T00:06:23Z- handleDepth()-Depth_Keller msg: 1613174783604 epoch sec; seqNo:9702; depth 0.02 2021-02-13T00:06:23Z- handleDvl() - RDI_Pathfinder msg: 1613174783.62 epoch sec; seqNo:4702 2021-02-13T00:06:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 102.52 , 102.52 , 97.11 2021-02-13T00:06:23Z- handleDvl() - RDI_Pathfinder msg: velocities -0.25 , -0.12 , 0.78 2021-02-13T00:06:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174783.63 epoch sec; seqNo:9702; 4058552.60 north; 591985.48 east 2021-02-13T00:06:24Z- AHRS_M2 msg: 1613174784.00 epoch sec; seqNo:9625 2021-02-13T00:06:24Z- AHRS_M2 msg: 5.53 phi; -0.17 theta; -0.07 psi 2021-02-13T00:06:24Z- handleDepth()-Depth_Keller msg: 1613174784008 epoch sec; seqNo:9703; depth 0.02 2021-02-13T00:06:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174784.02 epoch sec; seqNo:9703; 4058552.60 north; 591985.48 east 2021-02-13T00:06:24Z- LcmTrn::handleMessages() - handling 44 messages took 0.75 ms and 129 calls... 2021-02-13T00:06:24Z- waiting for 1613174781.78, time is 1613174784.02 2021-02-13T00:06:24Z- LcmTrn::updateTrn() >>>> pose time 1613174784.02, meas time 1613174783.62 <<<< 2021-02-13T00:06:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174783.62 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:06:24Z- TerrainNav::motionUpdate() - currEstimate = (4058553, 591985.48, 0.02) 2021-02-13T00:06:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:06:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:06:24Z- TNavBF::Projecting Measurements 2021-02-13T00:06:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:06:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:06:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:06:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:06:24Z- TNavFilter::4 good beams. 2021-02-13T00:06:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:06:24Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:06:24Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:06:24Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:06:24Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:06:24Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:06:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:06:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:06:24Z- TerrainMapOctree: (vehN, vehE ) = (4058552.66, 591985.48). 2021-02-13T00:06:24Z- TerrainMapOctree: Min Distance = 3521.89. 2021-02-13T00:06:24Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:06:24Z- There is no specific good beam that all particles have in common. 2021-02-13T00:06:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:06:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.63 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-13T00:06:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:06:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.61 K: 0.032 ExpectedMeasDiff: nan currMeas.covariance: 16.82 2021-02-13T00:06:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:06:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.59 K: 0.031 ExpectedMeasDiff: nan currMeas.covariance: 16.82 2021-02-13T00:06:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:06:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.57 K: 0.033 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-13T00:06:24Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:06:24Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:06:24Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:06:24Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:06:24Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:06:24Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:06:24Z- Filter 0 No beams used in subcloud update 2021-02-13T00:06:24Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:06:24Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:06:24Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 718.9 EastVariance: 833.0 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-13T00:06:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174783.62 sec. 2021-02-13T00:06:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:06:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:06:24Z- variances([0], [2], and psi_berg) = 718.903002 833.008953 0.000000 2021-02-13T00:06:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:06:24Z- LcmTrn::publishEstimates() - MLE : 4058550.10 591942.98 0.02 2021-02-13T00:06:24Z- LcmTrn::publishEstimates() - MMSE : 4058552.60 591985.48 0.02 2021-02-13T00:06:24Z- LcmTrn::publishEstimates() - COVAR: 718.90 833.01 0.56 0.00 2021-02-13T00:06:24Z- AHRS_M2 msg: 1613174784.41 epoch sec; seqNo:9626 2021-02-13T00:06:24Z- AHRS_M2 msg: 5.55 phi; -0.18 theta; -0.02 psi 2021-02-13T00:06:24Z- handleDepth()-Depth_Keller msg: 1613174784417 epoch sec; seqNo:9704; depth 0.02 2021-02-13T00:06:24Z- handleDvl() - RDI_Pathfinder msg: 1613174784.43 epoch sec; seqNo:4703 2021-02-13T00:06:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 109.22 , 97.04 , 107.86 2021-02-13T00:06:24Z- handleDvl() - RDI_Pathfinder msg: velocities -0.70 , -0.12 , -0.38 2021-02-13T00:06:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174784.44 epoch sec; seqNo:9704; 4058552.22 north; 591985.48 east 2021-02-13T00:06:24Z- AHRS_M2 msg: 1613174784.81 epoch sec; seqNo:9627 2021-02-13T00:06:24Z- AHRS_M2 msg: 5.58 phi; -0.07 theta; 0.01 psi 2021-02-13T00:06:24Z- handleDepth()-Depth_Keller msg: 1613174784816 epoch sec; seqNo:9705; depth 0.03 2021-02-13T00:06:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174784.83 epoch sec; seqNo:9705; 4058552.22 north; 591985.48 east 2021-02-13T00:06:25Z- AHRS_M2 msg: 1613174785.23 epoch sec; seqNo:9628 2021-02-13T00:06:25Z- AHRS_M2 msg: 5.64 phi; 0.04 theta; 0.07 psi 2021-02-13T00:06:25Z- handleDepth()-Depth_Keller msg: 1613174785239 epoch sec; seqNo:9706; depth 0.04 2021-02-13T00:06:25Z- handleDvl() - RDI_Pathfinder msg: 1613174785.25 epoch sec; seqNo:4704 2021-02-13T00:06:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.34 , 100.83 , 97.86 , 108.27 2021-02-13T00:06:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.05 , -0.42 , -0.28 2021-02-13T00:06:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174785.26 epoch sec; seqNo:9706; 4058552.22 north; 591985.48 east 2021-02-13T00:06:25Z- AHRS_M2 msg: 1613174785.62 epoch sec; seqNo:9629 2021-02-13T00:06:25Z- AHRS_M2 msg: 5.66 phi; 0.02 theta; 0.12 psi 2021-02-13T00:06:25Z- handleDepth()-Depth_Keller msg: 1613174785624 epoch sec; seqNo:9707; depth 0.08 2021-02-13T00:06:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174785.67 epoch sec; seqNo:9707; 4058551.84 north; 591985.49 east 2021-02-13T00:06:26Z- AHRS_M2 msg: 1613174786.03 epoch sec; seqNo:9630 2021-02-13T00:06:26Z- AHRS_M2 msg: 5.64 phi; -0.02 theta; 0.03 psi 2021-02-13T00:06:26Z- handleDepth()-Depth_Keller msg: 1613174786030 epoch sec; seqNo:9708; depth 0.09 2021-02-13T00:06:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174786.04 epoch sec; seqNo:9708; 4058551.84 north; 591985.49 east 2021-02-13T00:06:26Z- AHRS_M2 msg: 1613174786.43 epoch sec; seqNo:9631 2021-02-13T00:06:26Z- AHRS_M2 msg: 5.59 phi; 0.01 theta; -0.12 psi 2021-02-13T00:06:26Z- handleDepth()-Depth_Keller msg: 1613174786437 epoch sec; seqNo:9709; depth 0.09 2021-02-13T00:06:26Z- handleDvl() - RDI_Pathfinder msg: 1613174786.46 epoch sec; seqNo:4705 2021-02-13T00:06:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 106.92 , 97.99 , 102.45 , 97.99 2021-02-13T00:06:26Z- handleDvl() - RDI_Pathfinder msg: velocities -0.01 , 0.27 , -0.17 2021-02-13T00:06:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174786.47 epoch sec; seqNo:9709; 4058551.84 north; 591985.49 east 2021-02-13T00:06:26Z- AHRS_M2 msg: 1613174786.83 epoch sec; seqNo:9632 2021-02-13T00:06:26Z- AHRS_M2 msg: 5.59 phi; 0.03 theta; -0.16 psi 2021-02-13T00:06:26Z- handleDepth()-Depth_Keller msg: 1613174786836 epoch sec; seqNo:9710; depth 0.05 2021-02-13T00:06:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174786.85 epoch sec; seqNo:9710; 4058552.22 north; 591985.48 east 2021-02-13T00:06:27Z- AHRS_M2 msg: 1613174787.24 epoch sec; seqNo:9633 2021-02-13T00:06:27Z- AHRS_M2 msg: 5.59 phi; 0.05 theta; -0.08 psi 2021-02-13T00:06:27Z- handleDepth()-Depth_Keller msg: 1613174787240 epoch sec; seqNo:9711; depth 0.04 2021-02-13T00:06:27Z- handleDvl() - RDI_Pathfinder msg: 1613174787.25 epoch sec; seqNo:4706 2021-02-13T00:06:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 108.41 , 102.45 , 97.99 2021-02-13T00:06:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.22 , -0.42 , -0.00 2021-02-13T00:06:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174787.26 epoch sec; seqNo:9711; 4058551.84 north; 591985.49 east 2021-02-13T00:06:27Z- AHRS_M2 msg: 1613174787.64 epoch sec; seqNo:9634 2021-02-13T00:06:27Z- AHRS_M2 msg: 5.59 phi; 0.02 theta; -0.02 psi 2021-02-13T00:06:27Z- handleDepth()-Depth_Keller msg: 1613174787646 epoch sec; seqNo:9712; depth 0.04 2021-02-13T00:06:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174787.66 epoch sec; seqNo:9712; 4058551.84 north; 591985.49 east 2021-02-13T00:06:28Z- AHRS_M2 msg: 1613174788.04 epoch sec; seqNo:9635 2021-02-13T00:06:28Z- AHRS_M2 msg: 5.57 phi; -0.10 theta; 0.05 psi 2021-02-13T00:06:28Z- handleDepth()-Depth_Keller msg: 1613174788058 epoch sec; seqNo:9713; depth 0.01 2021-02-13T00:06:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174788.07 epoch sec; seqNo:9713; 4058551.83 north; 591984.27 east 2021-02-13T00:06:28Z- AHRS_M2 msg: 1613174788.45 epoch sec; seqNo:9636 2021-02-13T00:06:28Z- AHRS_M2 msg: 5.54 phi; -0.25 theta; 0.11 psi 2021-02-13T00:06:28Z- handleDepth()-Depth_Keller msg: 1613174788456 epoch sec; seqNo:9714; depth 0.01 2021-02-13T00:06:28Z- handleDvl() - RDI_Pathfinder msg: 1613174788.49 epoch sec; seqNo:4707 2021-02-13T00:06:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 99.48 , 100.97 , 97.99 2021-02-13T00:06:28Z- handleDvl() - RDI_Pathfinder msg: velocities -0.47 , -0.57 , 0.28 2021-02-13T00:06:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174788.50 epoch sec; seqNo:9714; 4058551.45 north; 591984.27 east 2021-02-13T00:06:28Z- AHRS_M2 msg: 1613174788.85 epoch sec; seqNo:9637 2021-02-13T00:06:28Z- AHRS_M2 msg: 5.57 phi; -0.24 theta; 0.14 psi 2021-02-13T00:06:28Z- handleDepth()-Depth_Keller msg: 1613174788857 epoch sec; seqNo:9715; depth 0.04 2021-02-13T00:06:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174788.88 epoch sec; seqNo:9715; 4058511.20 north; 592011.56 east 2021-02-13T00:06:29Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-13T00:06:29Z- waiting for 1613174787.02, time is 1613174788.88 2021-02-13T00:06:29Z- LcmTrn::updateTrn() >>>> pose time 1613174788.88, meas time 1613174788.49 <<<< 2021-02-13T00:06:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174788.49 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:06:29Z- TerrainNav::motionUpdate() - currEstimate = (4058511, 592011.56, 0.04) 2021-02-13T00:06:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:06:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:06:29Z- TNavBF::Projecting Measurements 2021-02-13T00:06:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:06:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:06:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:06:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:06:29Z- TNavFilter::4 good beams. 2021-02-13T00:06:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:06:29Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:06:29Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:06:29Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:06:29Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:06:29Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:06:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:06:29Z- TNavBF::mmse selected from filter -1 2021-02-13T00:06:29Z- TerrainMapOctree: (vehN, vehE ) = (4058514.51, 592009.48). 2021-02-13T00:06:29Z- TerrainMapOctree: Min Distance = 3535.93. 2021-02-13T00:06:29Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:06:29Z- There is no specific good beam that all particles have in common. 2021-02-13T00:06:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:06:29Z- Old Depth Bias: 0.00 DepthBiasCov: 2.85 K: 0.143 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:06:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:06:29Z- Old Depth Bias: 0.00 DepthBiasCov: 2.44 K: 0.122 ExpectedMeasDiff: nan currMeas.covariance: 15.83 2021-02-13T00:06:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:06:29Z- Old Depth Bias: 0.00 DepthBiasCov: 2.15 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 16.31 2021-02-13T00:06:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:06:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.92 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:06:29Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:06:29Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:06:29Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:06:29Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:06:29Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:06:29Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:06:29Z- Filter 0 No beams used in subcloud update 2021-02-13T00:06:29Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:06:29Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:06:29Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 718.7 EastVariance: 833.0 DepthVariance: 1.7 this->DepthBiasCov: 1.7 2021-02-13T00:06:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174788.49 sec. 2021-02-13T00:06:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:06:29Z- TNavBF::mmse selected from filter -1 2021-02-13T00:06:29Z- variances([0], [2], and psi_berg) = 718.613790 833.008767 0.000000 2021-02-13T00:06:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:06:29Z- LcmTrn::publishEstimates() - MLE : 4058508.70 591969.06 0.04 2021-02-13T00:06:29Z- LcmTrn::publishEstimates() - MMSE : 4058511.20 592011.56 0.04 2021-02-13T00:06:29Z- LcmTrn::publishEstimates() - COVAR: 718.61 833.01 1.92 0.00 2021-02-13T00:06:29Z- AHRS_M2 msg: 1613174789.27 epoch sec; seqNo:9638 2021-02-13T00:06:29Z- AHRS_M2 msg: 5.64 phi; -0.05 theta; 0.07 psi 2021-02-13T00:06:29Z- handleDepth()-Depth_Keller msg: 1613174789270 epoch sec; seqNo:9716; depth 0.05 2021-02-13T00:06:29Z- handleDvl() - RDI_Pathfinder msg: 1613174789.29 epoch sec; seqNo:4708 2021-02-13T00:06:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 111.99 , 95.76 , 97.11 , 118.75 2021-02-13T00:06:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.34 , -0.05 , -0.88 2021-02-13T00:06:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174789.32 epoch sec; seqNo:9716; 4058511.58 north; 592011.56 east 2021-02-13T00:06:29Z- AHRS_M2 msg: 1613174789.70 epoch sec; seqNo:9639 2021-02-13T00:06:29Z- AHRS_M2 msg: 5.67 phi; 0.11 theta; -0.04 psi 2021-02-13T00:06:29Z- handleDepth()-Depth_Keller msg: 1613174789700 epoch sec; seqNo:9717; depth 0.12 2021-02-13T00:06:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174789.71 epoch sec; seqNo:9717; 4058511.58 north; 592011.56 east 2021-02-13T00:06:30Z- AHRS_M2 msg: 1613174790.10 epoch sec; seqNo:9640 2021-02-13T00:06:30Z- AHRS_M2 msg: 5.65 phi; 0.08 theta; -0.14 psi 2021-02-13T00:06:30Z- handleDepth()-Depth_Keller msg: 1613174790109 epoch sec; seqNo:9718; depth 0.16 2021-02-13T00:06:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174790.13 epoch sec; seqNo:9718; 4058511.58 north; 592011.56 east 2021-02-13T00:06:30Z- AHRS_M2 msg: 1613174790.50 epoch sec; seqNo:9641 2021-02-13T00:06:30Z- AHRS_M2 msg: 5.64 phi; 0.01 theta; -0.19 psi 2021-02-13T00:06:30Z- handleDepth()-Depth_Keller msg: 1613174790515 epoch sec; seqNo:9719; depth 0.18 2021-02-13T00:06:30Z- handleDvl() - RDI_Pathfinder msg: 1613174790.55 epoch sec; seqNo:4709 2021-02-13T00:06:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.58 , 100.56 , 109.49 , 0.00 2021-02-13T00:06:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.63 , 0.14 , 0.59 2021-02-13T00:06:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174790.57 epoch sec; seqNo:9719; 4058511.96 north; 592011.56 east 2021-02-13T00:06:30Z- AHRS_M2 msg: 1613174790.91 epoch sec; seqNo:9642 2021-02-13T00:06:30Z- AHRS_M2 msg: 5.65 phi; -0.02 theta; -0.19 psi 2021-02-13T00:06:30Z- handleDepth()-Depth_Keller msg: 1613174790917 epoch sec; seqNo:9720; depth 0.20 2021-02-13T00:06:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174790.93 epoch sec; seqNo:9720; 4058511.96 north; 592011.56 east 2021-02-13T00:06:31Z- AHRS_M2 msg: 1613174791.31 epoch sec; seqNo:9643 2021-02-13T00:06:31Z- AHRS_M2 msg: 5.66 phi; -0.02 theta; -0.13 psi 2021-02-13T00:06:31Z- handleDepth()-Depth_Keller msg: 1613174791321 epoch sec; seqNo:9721; depth 0.21 2021-02-13T00:06:31Z- handleDvl() - RDI_Pathfinder msg: 1613174791.33 epoch sec; seqNo:4710 2021-02-13T00:06:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 112.80 , 102.39 , 99.41 2021-02-13T00:06:31Z- handleDvl() - RDI_Pathfinder msg: velocities -0.16 , -0.10 , 0.14 2021-02-13T00:06:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174791.36 epoch sec; seqNo:9721; 4058511.96 north; 592011.56 east 2021-02-13T00:06:31Z- AHRS_M2 msg: 1613174791.72 epoch sec; seqNo:9644 2021-02-13T00:06:31Z- AHRS_M2 msg: 5.64 phi; 0.00 theta; -0.05 psi 2021-02-13T00:06:31Z- handleDepth()-Depth_Keller msg: 1613174791725 epoch sec; seqNo:9722; depth 0.23 2021-02-13T00:06:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174791.74 epoch sec; seqNo:9722; 4058511.96 north; 592011.56 east 2021-02-13T00:06:32Z- AHRS_M2 msg: 1613174792.13 epoch sec; seqNo:9645 2021-02-13T00:06:32Z- AHRS_M2 msg: 5.65 phi; 0.01 theta; 0.02 psi 2021-02-13T00:06:32Z- handleDepth()-Depth_Keller msg: 1613174792131 epoch sec; seqNo:9723; depth 0.24 2021-02-13T00:06:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174792.17 epoch sec; seqNo:9723; 4058511.96 north; 592011.56 east 2021-02-13T00:06:32Z- AHRS_M2 msg: 1613174792.52 epoch sec; seqNo:9646 2021-02-13T00:06:32Z- AHRS_M2 msg: 5.72 phi; -0.03 theta; 0.05 psi 2021-02-13T00:06:32Z- handleDepth()-Depth_Keller msg: 1613174792528 epoch sec; seqNo:9724; depth 0.23 2021-02-13T00:06:32Z- handleDvl() - RDI_Pathfinder msg: 1613174792.91 epoch sec; seqNo:4711 2021-02-13T00:06:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.86 , 100.83 , 99.34 , 99.34 2021-02-13T00:06:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.01 , -0.44 , 0.16 2021-02-13T00:06:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174792.92 epoch sec; seqNo:9724; 4058511.58 north; 592011.56 east 2021-02-13T00:06:33Z- AHRS_M2 msg: 1613174793.02 epoch sec; seqNo:9647 2021-02-13T00:06:33Z- AHRS_M2 msg: 5.76 phi; -0.06 theta; 0.05 psi 2021-02-13T00:06:33Z- handleDepth()-Depth_Keller msg: 1613174793022 epoch sec; seqNo:9725; depth 0.20 2021-02-13T00:06:34Z- LcmTrn::handleMessages() - handling 33 messages took 0.61 ms and 121 calls... 2021-02-13T00:06:34Z- waiting for 1613174791.88, time is 1613174793.02 2021-02-13T00:06:34Z- LcmTrn::updateTrn() >>>> pose time 1613174793.02, meas time 1613174792.91 <<<< 2021-02-13T00:06:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174792.91 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:06:34Z- TerrainNav::motionUpdate() - currEstimate = (4058512, 592011.56, 0.20) 2021-02-13T00:06:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:06:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:06:34Z- TNavBF::Projecting Measurements 2021-02-13T00:06:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:06:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:06:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:06:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:06:34Z- TNavFilter::4 good beams. 2021-02-13T00:06:34Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:06:34Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:06:34Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:06:34Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:06:34Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:06:34Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:06:34Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:06:34Z- TNavBF::mmse selected from filter -1 2021-02-13T00:06:34Z- TerrainMapOctree: (vehN, vehE ) = (4058511.57, 592011.56). 2021-02-13T00:06:34Z- TerrainMapOctree: Min Distance = 3536.89. 2021-02-13T00:06:34Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:06:34Z- There is no specific good beam that all particles have in common. 2021-02-13T00:06:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:06:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.94 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-13T00:06:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:06:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.74 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 16.27 2021-02-13T00:06:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:06:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.59 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 15.79 2021-02-13T00:06:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:06:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.46 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 15.79 2021-02-13T00:06:34Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:06:34Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:06:34Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:06:34Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:06:34Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:06:34Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:06:34Z- Filter 0 No beams used in subcloud update 2021-02-13T00:06:34Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:06:34Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:06:34Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 718.6 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:06:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174792.91 sec. 2021-02-13T00:06:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:06:34Z- TNavBF::mmse selected from filter -1 2021-02-13T00:06:34Z- variances([0], [2], and psi_berg) = 718.590285 833.008752 0.000000 2021-02-13T00:06:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:06:34Z- LcmTrn::publishEstimates() - MLE : 4058509.08 591969.06 0.20 2021-02-13T00:06:34Z- LcmTrn::publishEstimates() - MMSE : 4058511.58 592011.56 0.20 2021-02-13T00:06:34Z- LcmTrn::publishEstimates() - COVAR: 718.59 833.01 1.35 0.00 2021-02-13T00:06:35Z- handleDvl() - RDI_Pathfinder msg: 1613174795.06 epoch sec; seqNo:4712 2021-02-13T00:06:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.17 , 95.76 , 97.11 , 101.17 2021-02-13T00:06:35Z- handleDvl() - RDI_Pathfinder msg: velocities -0.24 , -0.04 , 0.01 2021-02-13T00:06:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174795.07 epoch sec; seqNo:9725; 4058511.20 north; 592011.56 east 2021-02-13T00:06:35Z- AHRS_M2 msg: 1613174795.17 epoch sec; seqNo:9648 2021-02-13T00:06:35Z- AHRS_M2 msg: 5.72 phi; -0.14 theta; -0.10 psi 2021-02-13T00:06:35Z- handleDepth()-Depth_Keller msg: 1613174795180 epoch sec; seqNo:9726; depth 0.09 2021-02-13T00:06:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174795.19 epoch sec; seqNo:9726; 4058511.20 north; 592011.56 east 2021-02-13T00:06:35Z- AHRS_M2 msg: 1613174795.56 epoch sec; seqNo:9649 2021-02-13T00:06:35Z- AHRS_M2 msg: 5.67 phi; -0.31 theta; -0.01 psi 2021-02-13T00:06:35Z- handleDepth()-Depth_Keller msg: 1613174795562 epoch sec; seqNo:9727; depth 0.10 2021-02-13T00:06:35Z- handleDvl() - RDI_Pathfinder msg: 1613174795.58 epoch sec; seqNo:4713 2021-02-13T00:06:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 105.16 , 97.04 , 106.51 2021-02-13T00:06:35Z- handleDvl() - RDI_Pathfinder msg: velocities -0.65 , 0.14 , -0.13 2021-02-13T00:06:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174795.59 epoch sec; seqNo:9727; 4058510.83 north; 592011.57 east 2021-02-13T00:06:35Z- AHRS_M2 msg: 1613174795.96 epoch sec; seqNo:9650 2021-02-13T00:06:35Z- AHRS_M2 msg: 5.70 phi; -0.31 theta; -0.01 psi 2021-02-13T00:06:35Z- handleDepth()-Depth_Keller msg: 1613174795969 epoch sec; seqNo:9728; depth 0.11 2021-02-13T00:06:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174795.98 epoch sec; seqNo:9728; 4058510.83 north; 592011.57 east 2021-02-13T00:06:36Z- AHRS_M2 msg: 1613174796.37 epoch sec; seqNo:9651 2021-02-13T00:06:36Z- AHRS_M2 msg: 5.70 phi; -0.23 theta; -0.03 psi 2021-02-13T00:06:36Z- handleDepth()-Depth_Keller msg: 1613174796369 epoch sec; seqNo:9729; depth 0.09 2021-02-13T00:06:36Z- handleDvl() - RDI_Pathfinder msg: 1613174796.38 epoch sec; seqNo:4714 2021-02-13T00:06:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 102.45 , 103.94 , 0.00 , 124.77 2021-02-13T00:06:36Z- handleDvl() - RDI_Pathfinder msg: velocities -0.21 , -0.07 , -1.27 2021-02-13T00:06:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174796.39 epoch sec; seqNo:9729; 4058510.83 north; 592011.57 east 2021-02-13T00:06:36Z- AHRS_M2 msg: 1613174796.77 epoch sec; seqNo:9652 2021-02-13T00:06:36Z- AHRS_M2 msg: 5.71 phi; -0.13 theta; -0.04 psi 2021-02-13T00:06:36Z- handleDepth()-Depth_Keller msg: 1613174796774 epoch sec; seqNo:9730; depth 0.07 2021-02-13T00:06:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174796.79 epoch sec; seqNo:9730; 4058510.83 north; 592011.57 east 2021-02-13T00:06:37Z- AHRS_M2 msg: 1613174797.17 epoch sec; seqNo:9653 2021-02-13T00:06:37Z- AHRS_M2 msg: 5.74 phi; -0.06 theta; -0.04 psi 2021-02-13T00:06:37Z- handleDepth()-Depth_Keller msg: 1613174797179 epoch sec; seqNo:9731; depth 0.04 2021-02-13T00:06:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174797.19 epoch sec; seqNo:9731; 4058510.83 north; 592011.57 east 2021-02-13T00:06:37Z- AHRS_M2 msg: 1613174797.58 epoch sec; seqNo:9654 2021-02-13T00:06:37Z- AHRS_M2 msg: 5.75 phi; -0.04 theta; 0.00 psi 2021-02-13T00:06:37Z- handleDepth()-Depth_Keller msg: 1613174797583 epoch sec; seqNo:9732; depth 0.07 2021-02-13T00:06:37Z- handleDvl() - RDI_Pathfinder msg: 1613174797.60 epoch sec; seqNo:4715 2021-02-13T00:06:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.25 , 101.71 , 97.25 , 107.66 2021-02-13T00:06:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.50 , -0.35 , 0.24 2021-02-13T00:06:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174797.61 epoch sec; seqNo:9732; 4058510.83 north; 592011.57 east 2021-02-13T00:06:37Z- AHRS_M2 msg: 1613174797.98 epoch sec; seqNo:9655 2021-02-13T00:06:37Z- AHRS_M2 msg: 5.73 phi; -0.01 theta; 0.04 psi 2021-02-13T00:06:37Z- handleDepth()-Depth_Keller msg: 1613174797989 epoch sec; seqNo:9733; depth 0.11 2021-02-13T00:06:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174798.03 epoch sec; seqNo:9733; 4058510.83 north; 592011.57 east 2021-02-13T00:06:38Z- AHRS_M2 msg: 1613174798.38 epoch sec; seqNo:9656 2021-02-13T00:06:38Z- AHRS_M2 msg: 5.73 phi; 0.01 theta; 0.07 psi 2021-02-13T00:06:38Z- handleDepth()-Depth_Keller msg: 1613174798389 epoch sec; seqNo:9734; depth 0.10 2021-02-13T00:06:38Z- handleDvl() - RDI_Pathfinder msg: 1613174798.40 epoch sec; seqNo:4716 2021-02-13T00:06:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.68 , 98.33 , 99.68 , 96.98 2021-02-13T00:06:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.05 , -0.20 , 0.46 2021-02-13T00:06:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174798.42 epoch sec; seqNo:9734; 4058510.83 north; 592011.57 east 2021-02-13T00:06:38Z- AHRS_M2 msg: 1613174798.79 epoch sec; seqNo:9657 2021-02-13T00:06:38Z- AHRS_M2 msg: 5.74 phi; -0.04 theta; 0.04 psi 2021-02-13T00:06:38Z- handleDepth()-Depth_Keller msg: 1613174798793 epoch sec; seqNo:9735; depth 0.08 2021-02-13T00:06:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174798.87 epoch sec; seqNo:9735; 4058510.83 north; 592011.57 east 2021-02-13T00:06:39Z- AHRS_M2 msg: 1613174799.19 epoch sec; seqNo:9658 2021-02-13T00:06:39Z- AHRS_M2 msg: 5.73 phi; -0.09 theta; -0.07 psi 2021-02-13T00:06:39Z- handleDepth()-Depth_Keller msg: 1613174799198 epoch sec; seqNo:9736; depth 0.07 2021-02-13T00:06:39Z- LcmTrn::handleMessages() - handling 38 messages took 0.58 ms and 127 calls... 2021-02-13T00:06:39Z- waiting for 1613174796.02, time is 1613174799.19 2021-02-13T00:06:39Z- LcmTrn::updateTrn() >>>> pose time 1613174799.19, meas time 1613174798.40 <<<< 2021-02-13T00:06:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174798.40 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:06:39Z- TerrainNav::motionUpdate() - currEstimate = (4058511, 592011.57, 0.07) 2021-02-13T00:06:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:06:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:06:39Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174798.40 sec.; No relevant navigation data available 2021-02-13T00:06:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:06:39Z- TNavBF::mmse selected from filter -1 2021-02-13T00:06:39Z- variances([0], [2], and psi_berg) = 718.561393 833.008733 0.000000 2021-02-13T00:06:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:06:39Z- LcmTrn::publishEstimates() - MLE : 4058508.33 591969.07 0.07 2021-02-13T00:06:39Z- LcmTrn::publishEstimates() - MMSE : 4058510.83 592011.57 0.07 2021-02-13T00:06:39Z- LcmTrn::publishEstimates() - COVAR: 718.56 833.01 1.38 0.00 2021-02-13T00:06:39Z- handleDvl() - RDI_Pathfinder msg: 1613174799.90 epoch sec; seqNo:4717 2021-02-13T00:06:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.24 , 97.18 , 99.89 , 98.53 2021-02-13T00:06:39Z- handleDvl() - RDI_Pathfinder msg: velocities -0.24 , -0.08 , -0.32 2021-02-13T00:06:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174799.91 epoch sec; seqNo:9736; 4058510.45 north; 592011.57 east 2021-02-13T00:06:39Z- AHRS_M2 msg: 1613174799.99 epoch sec; seqNo:9659 2021-02-13T00:06:39Z- AHRS_M2 msg: 5.75 phi; 0.05 theta; -0.11 psi 2021-02-13T00:06:39Z- handleDepth()-Depth_Keller msg: 1613174799994 epoch sec; seqNo:9737; depth 0.05 2021-02-13T00:06:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174800.01 epoch sec; seqNo:9737; 4058510.45 north; 592011.57 east 2021-02-13T00:06:40Z- AHRS_M2 msg: 1613174800.38 epoch sec; seqNo:9660 2021-02-13T00:06:40Z- AHRS_M2 msg: 5.73 phi; 0.03 theta; -0.02 psi 2021-02-13T00:06:40Z- handleDepth()-Depth_Keller msg: 1613174800398 epoch sec; seqNo:9738; depth 0.04 2021-02-13T00:06:42Z- handleDvl() - RDI_Pathfinder msg: 1613174802.45 epoch sec; seqNo:4718 2021-02-13T00:06:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 105.22 , 99.81 , 97.11 2021-02-13T00:06:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.22 , -0.37 , 0.31 2021-02-13T00:06:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174802.46 epoch sec; seqNo:9738; 4058510.43 north; 592010.35 east 2021-02-13T00:06:42Z- AHRS_M2 msg: 1613174802.57 epoch sec; seqNo:9661 2021-02-13T00:06:42Z- AHRS_M2 msg: 5.70 phi; -0.13 theta; -0.11 psi 2021-02-13T00:06:42Z- handleDepth()-Depth_Keller msg: 1613174802576 epoch sec; seqNo:9739; depth 0.07 2021-02-13T00:06:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174802.59 epoch sec; seqNo:9739; 4058510.43 north; 592010.35 east 2021-02-13T00:06:43Z- AHRS_M2 msg: 1613174803.03 epoch sec; seqNo:9662 2021-02-13T00:06:43Z- AHRS_M2 msg: 5.71 phi; 0.01 theta; -0.07 psi 2021-02-13T00:06:43Z- handleDepth()-Depth_Keller msg: 1613174803034 epoch sec; seqNo:9740; depth 0.09 2021-02-13T00:06:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174803.12 epoch sec; seqNo:9740; 4058510.43 north; 592010.35 east 2021-02-13T00:06:43Z- AHRS_M2 msg: 1613174803.36 epoch sec; seqNo:9663 2021-02-13T00:06:43Z- AHRS_M2 msg: 5.68 phi; 0.08 theta; 0.03 psi 2021-02-13T00:06:43Z- handleDepth()-Depth_Keller msg: 1613174803373 epoch sec; seqNo:9741; depth 0.18 2021-02-13T00:06:43Z- handleDvl() - RDI_Pathfinder msg: 1613174803.39 epoch sec; seqNo:4719 2021-02-13T00:06:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 107.79 , 96.97 , 99.68 2021-02-13T00:06:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.44 , -0.23 2021-02-13T00:06:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174803.40 epoch sec; seqNo:9741; 4058510.80 north; 592009.13 east 2021-02-13T00:06:43Z- AHRS_M2 msg: 1613174803.77 epoch sec; seqNo:9664 2021-02-13T00:06:43Z- AHRS_M2 msg: 5.68 phi; 0.02 theta; 0.05 psi 2021-02-13T00:06:43Z- handleDepth()-Depth_Keller msg: 1613174803777 epoch sec; seqNo:9742; depth 0.20 2021-02-13T00:06:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174803.80 epoch sec; seqNo:9742; 4058510.80 north; 592009.13 east 2021-02-13T00:06:44Z- AHRS_M2 msg: 1613174804.17 epoch sec; seqNo:9665 2021-02-13T00:06:44Z- AHRS_M2 msg: 5.70 phi; -0.04 theta; 0.01 psi 2021-02-13T00:06:44Z- handleDepth()-Depth_Keller msg: 1613174804176 epoch sec; seqNo:9743; depth 0.23 2021-02-13T00:06:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174804.18 epoch sec; seqNo:9743; 4058510.80 north; 592009.13 east 2021-02-13T00:06:44Z- LcmTrn::handleMessages() - handling 25 messages took 0.42 ms and 117 calls... 2021-02-13T00:06:44Z- waiting for 1613174802.19, time is 1613174804.18 2021-02-13T00:06:44Z- LcmTrn::updateTrn() >>>> pose time 1613174804.18, meas time 1613174803.39 <<<< 2021-02-13T00:06:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174803.39 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:06:44Z- TerrainNav::motionUpdate() - currEstimate = (4058511, 592009.13, 0.23) 2021-02-13T00:06:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:06:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:06:44Z- TNavBF::Projecting Measurements 2021-02-13T00:06:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:06:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:06:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:06:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:06:44Z- TNavFilter::4 good beams. 2021-02-13T00:06:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:06:44Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:06:44Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:06:44Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:06:44Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:06:44Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:06:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:06:44Z- TNavBF::mmse selected from filter -1 2021-02-13T00:06:44Z- TerrainMapOctree: (vehN, vehE ) = (4058510.80, 592009.52). 2021-02-13T00:06:44Z- TerrainMapOctree: Min Distance = 3538.78. 2021-02-13T00:06:44Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:06:44Z- There is no specific good beam that all particles have in common. 2021-02-13T00:06:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:06:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.49 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 15.05 2021-02-13T00:06:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:06:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 18.59 2021-02-13T00:06:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:06:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 15.05 2021-02-13T00:06:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:06:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 15.90 2021-02-13T00:06:44Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:06:44Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:06:44Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:06:44Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:06:44Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:06:44Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:06:44Z- Filter 0 No beams used in subcloud update 2021-02-13T00:06:44Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:06:44Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:06:44Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 718.5 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:06:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174803.39 sec. 2021-02-13T00:06:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:06:44Z- TNavBF::mmse selected from filter -1 2021-02-13T00:06:44Z- variances([0], [2], and psi_berg) = 718.493157 833.008690 0.000000 2021-02-13T00:06:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:06:44Z- LcmTrn::publishEstimates() - MLE : 4058508.30 591966.63 0.23 2021-02-13T00:06:44Z- LcmTrn::publishEstimates() - MMSE : 4058510.80 592009.13 0.23 2021-02-13T00:06:44Z- LcmTrn::publishEstimates() - COVAR: 718.49 833.01 1.14 0.00 2021-02-13T00:06:44Z- AHRS_M2 msg: 1613174804.58 epoch sec; seqNo:9666 2021-02-13T00:06:44Z- AHRS_M2 msg: 5.69 phi; -0.03 theta; -0.02 psi 2021-02-13T00:06:44Z- handleDepth()-Depth_Keller msg: 1613174804585 epoch sec; seqNo:9744; depth 0.22 2021-02-13T00:06:44Z- handleDvl() - RDI_Pathfinder msg: 1613174804.60 epoch sec; seqNo:4720 2021-02-13T00:06:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.03 , 96.98 , 103.74 , 96.98 2021-02-13T00:06:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.16 , -0.46 , 0.22 2021-02-13T00:06:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174804.61 epoch sec; seqNo:9744; 4058510.80 north; 592009.13 east 2021-02-13T00:06:44Z- AHRS_M2 msg: 1613174804.98 epoch sec; seqNo:9667 2021-02-13T00:06:44Z- AHRS_M2 msg: 5.71 phi; -0.07 theta; -0.02 psi 2021-02-13T00:06:44Z- handleDepth()-Depth_Keller msg: 1613174804993 epoch sec; seqNo:9745; depth 0.24 2021-02-13T00:06:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174805.01 epoch sec; seqNo:9745; 4058510.80 north; 592009.13 east 2021-02-13T00:06:45Z- AHRS_M2 msg: 1613174805.38 epoch sec; seqNo:9668 2021-02-13T00:06:45Z- AHRS_M2 msg: 5.75 phi; -0.13 theta; 0.01 psi 2021-02-13T00:06:45Z- handleDepth()-Depth_Keller msg: 1613174805393 epoch sec; seqNo:9746; depth 0.24 2021-02-13T00:06:45Z- handleDvl() - RDI_Pathfinder msg: 1613174805.41 epoch sec; seqNo:4721 2021-02-13T00:06:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.40 , 98.40 , 98.40 , 99.75 2021-02-13T00:06:45Z- handleDvl() - RDI_Pathfinder msg: velocities -0.26 , -0.27 , 0.57 2021-02-13T00:06:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174805.42 epoch sec; seqNo:9746; 4058510.80 north; 592009.13 east 2021-02-13T00:06:45Z- AHRS_M2 msg: 1613174805.79 epoch sec; seqNo:9669 2021-02-13T00:06:45Z- AHRS_M2 msg: 5.73 phi; -0.17 theta; 0.09 psi 2021-02-13T00:06:45Z- handleDepth()-Depth_Keller msg: 1613174805797 epoch sec; seqNo:9747; depth 0.25 2021-02-13T00:06:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174805.81 epoch sec; seqNo:9747; 4058510.41 north; 592007.91 east 2021-02-13T00:06:46Z- AHRS_M2 msg: 1613174806.19 epoch sec; seqNo:9670 2021-02-13T00:06:46Z- AHRS_M2 msg: 5.70 phi; -0.16 theta; 0.09 psi 2021-02-13T00:06:46Z- handleDepth()-Depth_Keller msg: 1613174806197 epoch sec; seqNo:9748; depth 0.27 2021-02-13T00:06:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174806.21 epoch sec; seqNo:9748; 4058510.41 north; 592007.91 east 2021-02-13T00:06:46Z- AHRS_M2 msg: 1613174806.60 epoch sec; seqNo:9671 2021-02-13T00:06:46Z- AHRS_M2 msg: 5.70 phi; -0.17 theta; -0.04 psi 2021-02-13T00:06:46Z- handleDepth()-Depth_Keller msg: 1613174806602 epoch sec; seqNo:9749; depth 0.24 2021-02-13T00:06:46Z- handleDvl() - RDI_Pathfinder msg: 1613174806.61 epoch sec; seqNo:4722 2021-02-13T00:06:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.94 , 97.18 , 97.18 , 106.65 2021-02-13T00:06:46Z- handleDvl() - RDI_Pathfinder msg: velocities -0.36 , 0.42 , 0.15 2021-02-13T00:06:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174806.62 epoch sec; seqNo:9749; 4058510.42 north; 592009.13 east 2021-02-13T00:06:47Z- AHRS_M2 msg: 1613174807.00 epoch sec; seqNo:9672 2021-02-13T00:06:47Z- AHRS_M2 msg: 5.71 phi; -0.20 theta; -0.17 psi 2021-02-13T00:06:47Z- handleDepth()-Depth_Keller msg: 1613174807009 epoch sec; seqNo:9750; depth 0.18 2021-02-13T00:06:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174807.03 epoch sec; seqNo:9750; 4058510.04 north; 592009.13 east 2021-02-13T00:06:47Z- AHRS_M2 msg: 1613174807.40 epoch sec; seqNo:9673 2021-02-13T00:06:47Z- AHRS_M2 msg: 5.71 phi; -0.26 theta; -0.21 psi 2021-02-13T00:06:47Z- handleDepth()-Depth_Keller msg: 1613174807413 epoch sec; seqNo:9751; depth 0.16 2021-02-13T00:06:47Z- handleDvl() - RDI_Pathfinder msg: 1613174807.43 epoch sec; seqNo:4723 2021-02-13T00:06:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 102.45 , 103.94 , 97.99 , 109.89 2021-02-13T00:06:47Z- handleDvl() - RDI_Pathfinder msg: velocities -0.20 , -0.06 , -0.22 2021-02-13T00:06:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174807.45 epoch sec; seqNo:9751; 4058510.04 north; 592009.13 east 2021-02-13T00:06:47Z- AHRS_M2 msg: 1613174807.81 epoch sec; seqNo:9674 2021-02-13T00:06:47Z- AHRS_M2 msg: 5.71 phi; -0.29 theta; -0.11 psi 2021-02-13T00:06:47Z- handleDepth()-Depth_Keller msg: 1613174807813 epoch sec; seqNo:9752; depth 0.15 2021-02-13T00:06:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174807.82 epoch sec; seqNo:9752; 4058510.04 north; 592009.13 east 2021-02-13T00:06:48Z- AHRS_M2 msg: 1613174808.22 epoch sec; seqNo:9675 2021-02-13T00:06:48Z- AHRS_M2 msg: 5.70 phi; -0.33 theta; 0.04 psi 2021-02-13T00:06:48Z- handleDepth()-Depth_Keller msg: 1613174808228 epoch sec; seqNo:9753; depth 0.14 2021-02-13T00:06:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174808.25 epoch sec; seqNo:9753; 4058510.04 north; 592009.13 east 2021-02-13T00:06:48Z- AHRS_M2 msg: 1613174808.62 epoch sec; seqNo:9676 2021-02-13T00:06:48Z- AHRS_M2 msg: 5.69 phi; -0.35 theta; 0.12 psi 2021-02-13T00:06:48Z- handleDepth()-Depth_Keller msg: 1613174808620 epoch sec; seqNo:9754; depth 0.15 2021-02-13T00:06:48Z- handleDvl() - RDI_Pathfinder msg: 1613174808.64 epoch sec; seqNo:4724 2021-02-13T00:06:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.79 , 114.15 , 97.79 , 97.79 2021-02-13T00:06:48Z- handleDvl() - RDI_Pathfinder msg: velocities -0.05 , -0.19 , -0.12 2021-02-13T00:06:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174808.65 epoch sec; seqNo:9754; 4058510.04 north; 592009.13 east 2021-02-13T00:06:49Z- AHRS_M2 msg: 1613174809.02 epoch sec; seqNo:9677 2021-02-13T00:06:49Z- AHRS_M2 msg: 5.69 phi; -0.30 theta; 0.11 psi 2021-02-13T00:06:49Z- handleDepth()-Depth_Keller msg: 1613174809026 epoch sec; seqNo:9755; depth 0.17 2021-02-13T00:06:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174809.04 epoch sec; seqNo:9755; 4058510.04 north; 592009.13 east 2021-02-13T00:06:49Z- AHRS_M2 msg: 1613174809.42 epoch sec; seqNo:9678 2021-02-13T00:06:49Z- AHRS_M2 msg: 5.71 phi; -0.27 theta; 0.02 psi 2021-02-13T00:06:49Z- handleDepth()-Depth_Keller msg: 1613174809429 epoch sec; seqNo:9756; depth 0.15 2021-02-13T00:06:49Z- handleDvl() - RDI_Pathfinder msg: 1613174809.44 epoch sec; seqNo:4725 2021-02-13T00:06:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 112.87 , 97.99 , 87.57 , 132.21 2021-02-13T00:06:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.01 , -0.25 , -0.33 2021-02-13T00:06:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174809.45 epoch sec; seqNo:9756; 4058510.04 north; 592009.13 east 2021-02-13T00:06:49Z- LcmTrn::handleMessages() - handling 45 messages took 0.68 ms and 132 calls... 2021-02-13T00:06:49Z- waiting for 1613174807.18, time is 1613174809.45 2021-02-13T00:06:49Z- LcmTrn::updateTrn() equating pose 1613174809.45 and meas times 1613174809.45 2021-02-13T00:06:49Z- LcmTrn::updateTrn() >>>> pose time 1613174809.45, meas time 1613174809.45 <<<< 2021-02-13T00:06:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174809.45 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:06:49Z- TerrainNav::motionUpdate() - currEstimate = (4058510, 592009.13, 0.15) 2021-02-13T00:06:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:06:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:06:49Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174809.45 sec.; No relevant navigation data available 2021-02-13T00:06:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:06:49Z- TNavBF::mmse selected from filter -1 2021-02-13T00:06:49Z- variances([0], [2], and psi_berg) = 718.464297 833.008671 0.000000 2021-02-13T00:06:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:06:49Z- LcmTrn::publishEstimates() - MLE : 4058507.54 591966.63 0.15 2021-02-13T00:06:49Z- LcmTrn::publishEstimates() - MMSE : 4058510.04 592009.13 0.15 2021-02-13T00:06:49Z- LcmTrn::publishEstimates() - COVAR: 718.46 833.01 1.17 0.00 2021-02-13T00:06:49Z- AHRS_M2 msg: 1613174809.83 epoch sec; seqNo:9679 2021-02-13T00:06:49Z- AHRS_M2 msg: 5.73 phi; -0.29 theta; -0.04 psi 2021-02-13T00:06:49Z- handleDepth()-Depth_Keller msg: 1613174809841 epoch sec; seqNo:9757; depth 0.14 2021-02-13T00:06:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174809.86 epoch sec; seqNo:9757; 4058510.04 north; 592009.13 east 2021-02-13T00:06:50Z- AHRS_M2 msg: 1613174810.23 epoch sec; seqNo:9680 2021-02-13T00:06:50Z- AHRS_M2 msg: 5.73 phi; -0.27 theta; -0.05 psi 2021-02-13T00:06:50Z- handleDepth()-Depth_Keller msg: 1613174810241 epoch sec; seqNo:9758; depth 0.14 2021-02-13T00:06:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174810.26 epoch sec; seqNo:9758; 4058510.03 north; 592007.91 east 2021-02-13T00:06:50Z- AHRS_M2 msg: 1613174810.64 epoch sec; seqNo:9681 2021-02-13T00:06:50Z- AHRS_M2 msg: 5.73 phi; -0.25 theta; -0.05 psi 2021-02-13T00:06:50Z- handleDepth()-Depth_Keller msg: 1613174810640 epoch sec; seqNo:9759; depth 0.12 2021-02-13T00:06:50Z- handleDvl() - RDI_Pathfinder msg: 1613174810.70 epoch sec; seqNo:4726 2021-02-13T00:06:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.91 , 103.40 , 88.52 , 121.25 2021-02-13T00:06:50Z- handleDvl() - RDI_Pathfinder msg: velocities -0.01 , -0.02 , 0.09 2021-02-13T00:06:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174810.71 epoch sec; seqNo:9759; 4058510.03 north; 592007.91 east 2021-02-13T00:06:51Z- AHRS_M2 msg: 1613174811.04 epoch sec; seqNo:9682 2021-02-13T00:06:51Z- AHRS_M2 msg: 5.71 phi; -0.29 theta; -0.01 psi 2021-02-13T00:06:51Z- handleDepth()-Depth_Keller msg: 1613174811049 epoch sec; seqNo:9760; depth 0.13 2021-02-13T00:06:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174811.06 epoch sec; seqNo:9760; 4058509.65 north; 592007.92 east 2021-02-13T00:06:51Z- AHRS_M2 msg: 1613174811.44 epoch sec; seqNo:9683 2021-02-13T00:06:51Z- AHRS_M2 msg: 5.68 phi; -0.30 theta; 0.02 psi 2021-02-13T00:06:51Z- handleDepth()-Depth_Keller msg: 1613174811455 epoch sec; seqNo:9761; depth 0.17 2021-02-13T00:06:51Z- handleDvl() - RDI_Pathfinder msg: 1613174811.47 epoch sec; seqNo:4727 2021-02-13T00:06:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.27 , 103.74 , 97.79 , 118.62 2021-02-13T00:06:51Z- handleDvl() - RDI_Pathfinder msg: velocities -0.25 , 0.08 , 0.06 2021-02-13T00:06:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174811.48 epoch sec; seqNo:9761; 4058509.66 north; 592009.14 east 2021-02-13T00:06:51Z- AHRS_M2 msg: 1613174811.85 epoch sec; seqNo:9684 2021-02-13T00:06:51Z- AHRS_M2 msg: 5.66 phi; -0.25 theta; -0.01 psi 2021-02-13T00:06:51Z- handleDepth()-Depth_Keller msg: 1613174811857 epoch sec; seqNo:9762; depth 0.18 2021-02-13T00:06:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174811.87 epoch sec; seqNo:9762; 4058509.66 north; 592009.14 east 2021-02-13T00:06:52Z- AHRS_M2 msg: 1613174812.26 epoch sec; seqNo:9685 2021-02-13T00:06:52Z- AHRS_M2 msg: 5.64 phi; -0.18 theta; -0.07 psi 2021-02-13T00:06:52Z- handleDepth()-Depth_Keller msg: 1613174812262 epoch sec; seqNo:9763; depth 0.18 2021-02-13T00:06:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174812.31 epoch sec; seqNo:9763; 4058509.66 north; 592009.14 east 2021-02-13T00:06:52Z- AHRS_M2 msg: 1613174812.66 epoch sec; seqNo:9686 2021-02-13T00:06:52Z- AHRS_M2 msg: 5.63 phi; -0.09 theta; -0.14 psi 2021-02-13T00:06:52Z- handleDepth()-Depth_Keller msg: 1613174812660 epoch sec; seqNo:9764; depth 0.17 2021-02-13T00:06:52Z- handleDvl() - RDI_Pathfinder msg: 1613174812.67 epoch sec; seqNo:4728 2021-02-13T00:06:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 102.18 , 100.69 , 97.72 , 120.04 2021-02-13T00:06:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.21 , -0.04 , -0.56 2021-02-13T00:06:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174812.68 epoch sec; seqNo:9764; 4058509.66 north; 592009.14 east 2021-02-13T00:06:53Z- AHRS_M2 msg: 1613174813.06 epoch sec; seqNo:9687 2021-02-13T00:06:53Z- AHRS_M2 msg: 5.63 phi; -0.05 theta; -0.17 psi 2021-02-13T00:06:53Z- handleDepth()-Depth_Keller msg: 1613174813065 epoch sec; seqNo:9765; depth 0.14 2021-02-13T00:06:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174813.15 epoch sec; seqNo:9765; 4058509.66 north; 592009.14 east 2021-02-13T00:06:53Z- AHRS_M2 msg: 1613174813.46 epoch sec; seqNo:9688 2021-02-13T00:06:53Z- AHRS_M2 msg: 5.59 phi; -0.09 theta; -0.14 psi 2021-02-13T00:06:53Z- handleDepth()-Depth_Keller msg: 1613174813467 epoch sec; seqNo:9766; depth 0.10 2021-02-13T00:06:54Z- handleDvl() - RDI_Pathfinder msg: 1613174814.12 epoch sec; seqNo:4729 2021-02-13T00:06:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.70 , 108.07 , 97.65 , 102.11 2021-02-13T00:06:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.26 , -0.47 , 0.38 2021-02-13T00:06:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174814.13 epoch sec; seqNo:9766; 4058509.65 north; 592007.92 east 2021-02-13T00:06:54Z- AHRS_M2 msg: 1613174814.24 epoch sec; seqNo:9689 2021-02-13T00:06:54Z- AHRS_M2 msg: 5.68 phi; -0.14 theta; 0.05 psi 2021-02-13T00:06:54Z- handleDepth()-Depth_Keller msg: 1613174814243 epoch sec; seqNo:9767; depth 0.07 2021-02-13T00:06:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174814.25 epoch sec; seqNo:9767; 4058509.65 north; 592007.92 east 2021-02-13T00:06:54Z- LcmTrn::handleMessages() - handling 37 messages took 0.59 ms and 125 calls... 2021-02-13T00:06:54Z- waiting for 1613174812.45, time is 1613174814.25 2021-02-13T00:06:54Z- LcmTrn::updateTrn() >>>> pose time 1613174814.25, meas time 1613174814.12 <<<< 2021-02-13T00:06:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174814.12 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:06:54Z- TerrainNav::motionUpdate() - currEstimate = (4058510, 592007.92, 0.07) 2021-02-13T00:06:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:06:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:06:54Z- TNavBF::Projecting Measurements 2021-02-13T00:06:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:06:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:06:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:06:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:06:54Z- TNavFilter::4 good beams. 2021-02-13T00:06:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:06:54Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:06:54Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:06:54Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:06:54Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:06:54Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:06:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:06:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:06:54Z- TerrainMapOctree: (vehN, vehE ) = (4058509.66, 592007.95). 2021-02-13T00:06:54Z- TerrainMapOctree: Min Distance = 3540.65. 2021-02-13T00:06:54Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:06:54Z- There is no specific good beam that all particles have in common. 2021-02-13T00:06:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:06:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 13.45 2021-02-13T00:06:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:06:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 18.69 2021-02-13T00:06:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:06:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.26 2021-02-13T00:06:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:06:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 16.68 2021-02-13T00:06:54Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:06:54Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:06:54Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:06:54Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:06:54Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:06:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:06:54Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:06:54Z- Filter 0 No beams used in subcloud update 2021-02-13T00:06:54Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:06:54Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:06:54Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 718.4 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:06:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174814.12 sec. 2021-02-13T00:06:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:06:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:06:54Z- variances([0], [2], and psi_berg) = 718.420997 833.008643 0.000000 2021-02-13T00:06:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:06:54Z- LcmTrn::publishEstimates() - MLE : 4058507.15 592050.42 0.07 2021-02-13T00:06:54Z- LcmTrn::publishEstimates() - MMSE : 4058509.65 592007.92 0.07 2021-02-13T00:06:54Z- LcmTrn::publishEstimates() - COVAR: 718.42 833.01 0.97 0.00 2021-02-13T00:06:54Z- AHRS_M2 msg: 1613174814.61 epoch sec; seqNo:9690 2021-02-13T00:06:54Z- AHRS_M2 msg: 5.69 phi; -0.09 theta; 0.17 psi 2021-02-13T00:06:54Z- handleDepth()-Depth_Keller msg: 1613174814612 epoch sec; seqNo:9768; depth 0.06 2021-02-13T00:06:56Z- handleDvl() - RDI_Pathfinder msg: 1613174816.66 epoch sec; seqNo:4730 2021-02-13T00:06:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 102.45 , 97.04 , 109.21 2021-02-13T00:06:56Z- handleDvl() - RDI_Pathfinder msg: velocities -0.12 , -0.40 , 0.15 2021-02-13T00:06:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174816.67 epoch sec; seqNo:9768; 4058508.89 north; 592007.93 east 2021-02-13T00:06:56Z- AHRS_M2 msg: 1613174816.78 epoch sec; seqNo:9691 2021-02-13T00:06:56Z- AHRS_M2 msg: 5.69 phi; -0.04 theta; -0.13 psi 2021-02-13T00:06:56Z- handleDepth()-Depth_Keller msg: 1613174816783 epoch sec; seqNo:9769; depth 0.03 2021-02-13T00:06:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174816.80 epoch sec; seqNo:9769; 4058508.89 north; 592007.93 east 2021-02-13T00:06:57Z- AHRS_M2 msg: 1613174817.17 epoch sec; seqNo:9692 2021-02-13T00:06:57Z- AHRS_M2 msg: 5.68 phi; -0.07 theta; -0.10 psi 2021-02-13T00:06:57Z- handleDepth()-Depth_Keller msg: 1613174817177 epoch sec; seqNo:9770; depth 0.04 2021-02-13T00:06:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174817.20 epoch sec; seqNo:9770; 4058508.50 north; 592006.71 east 2021-02-13T00:06:57Z- AHRS_M2 msg: 1613174817.58 epoch sec; seqNo:9693 2021-02-13T00:06:57Z- AHRS_M2 msg: 5.71 phi; -0.03 theta; 0.03 psi 2021-02-13T00:06:57Z- handleDepth()-Depth_Keller msg: 1613174817585 epoch sec; seqNo:9771; depth 0.05 2021-02-13T00:06:57Z- handleDvl() - RDI_Pathfinder msg: 1613174817.60 epoch sec; seqNo:4731 2021-02-13T00:06:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 107.86 , 97.04 , 102.45 2021-02-13T00:06:57Z- handleDvl() - RDI_Pathfinder msg: velocities -0.14 , -0.45 , -0.71 2021-02-13T00:06:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174817.62 epoch sec; seqNo:9771; 4058508.50 north; 592006.71 east 2021-02-13T00:06:57Z- AHRS_M2 msg: 1613174817.98 epoch sec; seqNo:9694 2021-02-13T00:06:57Z- AHRS_M2 msg: 5.74 phi; 0.02 theta; 0.16 psi 2021-02-13T00:06:57Z- handleDepth()-Depth_Keller msg: 1613174817985 epoch sec; seqNo:9772; depth 0.09 2021-02-13T00:06:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174818.00 epoch sec; seqNo:9772; 4058508.50 north; 592006.71 east 2021-02-13T00:06:58Z- AHRS_M2 msg: 1613174818.39 epoch sec; seqNo:9695 2021-02-13T00:06:58Z- AHRS_M2 msg: 5.74 phi; -0.01 theta; 0.16 psi 2021-02-13T00:06:58Z- handleDepth()-Depth_Keller msg: 1613174818397 epoch sec; seqNo:9773; depth 0.12 2021-02-13T00:06:58Z- handleDvl() - RDI_Pathfinder msg: 1613174818.41 epoch sec; seqNo:4732 2021-02-13T00:06:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.03 , 96.97 , 99.68 , 101.03 2021-02-13T00:06:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.17 , -0.14 , 0.32 2021-02-13T00:06:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174818.42 epoch sec; seqNo:9773; 4058508.50 north; 592006.71 east 2021-02-13T00:06:58Z- AHRS_M2 msg: 1613174818.79 epoch sec; seqNo:9696 2021-02-13T00:06:58Z- AHRS_M2 msg: 5.77 phi; -0.16 theta; 0.07 psi 2021-02-13T00:06:58Z- handleDepth()-Depth_Keller msg: 1613174818797 epoch sec; seqNo:9774; depth 0.06 2021-02-13T00:06:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:06:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:06:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:06:58Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:06:58Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:06:58Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:06:58Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:06:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:06:58Z- handleDvl() - RDI_Pathfinder msg: 1613174818.80 epoch sec; seqNo:4733 2021-02-13T00:06:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:06:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.17 , -0.14 , 0.32 2021-02-13T00:06:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174818.82 epoch sec; seqNo:9774; 4058508.50 north; 592006.71 east 2021-02-13T00:06:59Z- AHRS_M2 msg: 1613174819.19 epoch sec; seqNo:9697 2021-02-13T00:06:59Z- AHRS_M2 msg: 5.77 phi; -0.22 theta; -0.02 psi 2021-02-13T00:06:59Z- handleDepth()-Depth_Keller msg: 1613174819205 epoch sec; seqNo:9775; depth 0.06 2021-02-13T00:06:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174819.22 epoch sec; seqNo:9775; 4058508.50 north; 592006.71 east 2021-02-13T00:06:59Z- AHRS_M2 msg: 1613174819.60 epoch sec; seqNo:9698 2021-02-13T00:06:59Z- AHRS_M2 msg: 5.73 phi; -0.16 theta; -0.06 psi 2021-02-13T00:06:59Z- handleDepth()-Depth_Keller msg: 1613174819607 epoch sec; seqNo:9776; depth 0.02 2021-02-13T00:06:59Z- handleDvl() - RDI_Pathfinder msg: 1613174819.62 epoch sec; seqNo:4734 2021-02-13T00:06:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.81 , 97.04 , 97.04 , 101.10 2021-02-13T00:06:59Z- handleDvl() - RDI_Pathfinder msg: velocities -0.64 , -0.05 , -0.33 2021-02-13T00:06:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174819.63 epoch sec; seqNo:9776; 4058508.12 north; 592006.71 east 2021-02-13T00:06:59Z- LcmTrn::handleMessages() - handling 32 messages took 0.43 ms and 124 calls... 2021-02-13T00:06:59Z- waiting for 1613174817.25, time is 1613174819.63 2021-02-13T00:06:59Z- LcmTrn::updateTrn() equating pose 1613174819.63 and meas times 1613174819.63 2021-02-13T00:06:59Z- LcmTrn::updateTrn() >>>> pose time 1613174819.63, meas time 1613174819.63 <<<< 2021-02-13T00:06:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174819.63 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:06:59Z- TerrainNav::motionUpdate() - currEstimate = (4058508, 592006.71, 0.02) 2021-02-13T00:06:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:06:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:06:59Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174819.63 sec.; No relevant navigation data available 2021-02-13T00:06:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:06:59Z- TNavBF::mmse selected from filter -1 2021-02-13T00:06:59Z- variances([0], [2], and psi_berg) = 718.374783 833.008614 0.000000 2021-02-13T00:06:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:06:59Z- LcmTrn::publishEstimates() - MLE : 4058505.62 592049.21 0.02 2021-02-13T00:06:59Z- LcmTrn::publishEstimates() - MMSE : 4058508.12 592006.71 0.02 2021-02-13T00:06:59Z- LcmTrn::publishEstimates() - COVAR: 718.37 833.01 1.07 0.00 2021-02-13T00:07:00Z- AHRS_M2 msg: 1613174820.00 epoch sec; seqNo:9699 2021-02-13T00:07:00Z- AHRS_M2 msg: 5.74 phi; -0.04 theta; -0.07 psi 2021-02-13T00:07:00Z- handleDepth()-Depth_Keller msg: 1613174820006 epoch sec; seqNo:9777; depth 0.02 2021-02-13T00:07:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174820.02 epoch sec; seqNo:9777; 4058508.12 north; 592006.71 east 2021-02-13T00:07:00Z- AHRS_M2 msg: 1613174820.40 epoch sec; seqNo:9700 2021-02-13T00:07:00Z- AHRS_M2 msg: 5.80 phi; 0.07 theta; 0.03 psi 2021-02-13T00:07:00Z- handleDepth()-Depth_Keller msg: 1613174820408 epoch sec; seqNo:9778; depth 0.07 2021-02-13T00:07:00Z- handleDvl() - RDI_Pathfinder msg: 1613174820.42 epoch sec; seqNo:4735 2021-02-13T00:07:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 101.10 , 97.04 , 103.81 2021-02-13T00:07:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.26 , 0.01 , 0.22 2021-02-13T00:07:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174820.44 epoch sec; seqNo:9778; 4058508.12 north; 592006.71 east 2021-02-13T00:07:00Z- AHRS_M2 msg: 1613174820.81 epoch sec; seqNo:9701 2021-02-13T00:07:00Z- AHRS_M2 msg: 5.79 phi; 0.12 theta; 0.11 psi 2021-02-13T00:07:00Z- handleDepth()-Depth_Keller msg: 1613174820817 epoch sec; seqNo:9779; depth 0.10 2021-02-13T00:07:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174820.83 epoch sec; seqNo:9779; 4058508.12 north; 592006.71 east 2021-02-13T00:07:01Z- AHRS_M2 msg: 1613174821.21 epoch sec; seqNo:9702 2021-02-13T00:07:01Z- AHRS_M2 msg: 5.76 phi; 0.09 theta; 0.12 psi 2021-02-13T00:07:01Z- handleDepth()-Depth_Keller msg: 1613174821217 epoch sec; seqNo:9780; depth 0.12 2021-02-13T00:07:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174821.23 epoch sec; seqNo:9780; 4058508.12 north; 592006.71 east 2021-02-13T00:07:01Z- AHRS_M2 msg: 1613174821.62 epoch sec; seqNo:9703 2021-02-13T00:07:01Z- AHRS_M2 msg: 5.80 phi; 0.02 theta; 0.06 psi 2021-02-13T00:07:01Z- handleDepth()-Depth_Keller msg: 1613174821620 epoch sec; seqNo:9781; depth 0.11 2021-02-13T00:07:01Z- handleDvl() - RDI_Pathfinder msg: 1613174821.64 epoch sec; seqNo:4736 2021-02-13T00:07:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 107.86 , 97.04 , 107.86 , 97.04 2021-02-13T00:07:01Z- handleDvl() - RDI_Pathfinder msg: velocities -0.05 , -0.16 , 0.51 2021-02-13T00:07:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174821.65 epoch sec; seqNo:9781; 4058508.12 north; 592006.71 east 2021-02-13T00:07:02Z- AHRS_M2 msg: 1613174822.02 epoch sec; seqNo:9704 2021-02-13T00:07:02Z- AHRS_M2 msg: 5.87 phi; 0.02 theta; 0.02 psi 2021-02-13T00:07:02Z- handleDepth()-Depth_Keller msg: 1613174822029 epoch sec; seqNo:9782; depth 0.10 2021-02-13T00:07:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174822.04 epoch sec; seqNo:9782; 4058508.12 north; 592006.71 east 2021-02-13T00:07:02Z- AHRS_M2 msg: 1613174822.42 epoch sec; seqNo:9705 2021-02-13T00:07:02Z- AHRS_M2 msg: 5.91 phi; 0.00 theta; 0.01 psi 2021-02-13T00:07:02Z- handleDepth()-Depth_Keller msg: 1613174822428 epoch sec; seqNo:9783; depth 0.06 2021-02-13T00:07:02Z- handleDvl() - RDI_Pathfinder msg: 1613174822.44 epoch sec; seqNo:4737 2021-02-13T00:07:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 102.38 , 97.92 , 102.38 , 97.92 2021-02-13T00:07:02Z- handleDvl() - RDI_Pathfinder msg: velocities -0.34 , 0.28 , -0.34 2021-02-13T00:07:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174822.45 epoch sec; seqNo:9783; 4058508.12 north; 592006.71 east 2021-02-13T00:07:02Z- AHRS_M2 msg: 1613174822.83 epoch sec; seqNo:9706 2021-02-13T00:07:02Z- AHRS_M2 msg: 5.83 phi; -0.10 theta; -0.06 psi 2021-02-13T00:07:02Z- handleDepth()-Depth_Keller msg: 1613174822837 epoch sec; seqNo:9784; depth 0.04 2021-02-13T00:07:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174822.85 epoch sec; seqNo:9784; 4058508.12 north; 592006.71 east 2021-02-13T00:07:03Z- AHRS_M2 msg: 1613174823.23 epoch sec; seqNo:9707 2021-02-13T00:07:03Z- AHRS_M2 msg: 5.82 phi; -0.16 theta; -0.14 psi 2021-02-13T00:07:03Z- handleDepth()-Depth_Keller msg: 1613174823246 epoch sec; seqNo:9785; depth 0.03 2021-02-13T00:07:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174823.26 epoch sec; seqNo:9785; 4058508.12 north; 592006.71 east 2021-02-13T00:07:03Z- AHRS_M2 msg: 1613174823.64 epoch sec; seqNo:9708 2021-02-13T00:07:03Z- AHRS_M2 msg: 5.87 phi; -0.18 theta; -0.10 psi 2021-02-13T00:07:03Z- handleDepth()-Depth_Keller msg: 1613174823640 epoch sec; seqNo:9786; depth -0.01 2021-02-13T00:07:03Z- handleDvl() - RDI_Pathfinder msg: 1613174823.65 epoch sec; seqNo:4738 2021-02-13T00:07:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.39 , 101.10 , 97.04 , 106.51 2021-02-13T00:07:03Z- handleDvl() - RDI_Pathfinder msg: velocities -0.41 , -0.48 , -0.54 2021-02-13T00:07:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174823.68 epoch sec; seqNo:9786; 4058507.74 north; 592006.72 east 2021-02-13T00:07:04Z- AHRS_M2 msg: 1613174824.04 epoch sec; seqNo:9709 2021-02-13T00:07:04Z- AHRS_M2 msg: 5.90 phi; -0.14 theta; 0.09 psi 2021-02-13T00:07:04Z- handleDepth()-Depth_Keller msg: 1613174824049 epoch sec; seqNo:9787; depth 0.03 2021-02-13T00:07:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174824.06 epoch sec; seqNo:9787; 4058507.74 north; 592006.72 east 2021-02-13T00:07:04Z- AHRS_M2 msg: 1613174824.44 epoch sec; seqNo:9710 2021-02-13T00:07:04Z- AHRS_M2 msg: 5.91 phi; -0.03 theta; 0.25 psi 2021-02-13T00:07:04Z- handleDepth()-Depth_Keller msg: 1613174824456 epoch sec; seqNo:9788; depth 0.11 2021-02-13T00:07:04Z- handleDvl() - RDI_Pathfinder msg: 1613174824.47 epoch sec; seqNo:4739 2021-02-13T00:07:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.68 , 96.97 , 96.97 , 98.33 2021-02-13T00:07:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.53 , -0.26 , -0.22 2021-02-13T00:07:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174824.48 epoch sec; seqNo:9788; 4058507.74 north; 592006.72 east 2021-02-13T00:07:04Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-13T00:07:04Z- waiting for 1613174822.63, time is 1613174824.48 2021-02-13T00:07:04Z- LcmTrn::updateTrn() equating pose 1613174824.48 and meas times 1613174824.48 2021-02-13T00:07:04Z- LcmTrn::updateTrn() >>>> pose time 1613174824.48, meas time 1613174824.48 <<<< 2021-02-13T00:07:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174824.48 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:07:04Z- TerrainNav::motionUpdate() - currEstimate = (4058508, 592006.72, 0.11) 2021-02-13T00:07:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:07:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:07:04Z- TNavBF::Projecting Measurements 2021-02-13T00:07:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:07:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:07:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:07:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:07:04Z- TNavFilter::4 good beams. 2021-02-13T00:07:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:07:04Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:07:04Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:07:04Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:07:04Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:07:04Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:07:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:07:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:07:04Z- TerrainMapOctree: (vehN, vehE ) = (4058507.74, 592006.72). 2021-02-13T00:07:04Z- TerrainMapOctree: Min Distance = 3542.92. 2021-02-13T00:07:04Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:07:04Z- There is no specific good beam that all particles have in common. 2021-02-13T00:07:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:07:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 15.90 2021-02-13T00:07:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:07:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 15.05 2021-02-13T00:07:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:07:04Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.05 2021-02-13T00:07:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:07:04Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 15.47 2021-02-13T00:07:04Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:07:04Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:07:04Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:07:04Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:07:04Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:07:04Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:07:04Z- Filter 0 No beams used in subcloud update 2021-02-13T00:07:04Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:07:04Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:07:04Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 718.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:07:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174824.48 sec. 2021-02-13T00:07:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:07:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:07:04Z- variances([0], [2], and psi_berg) = 718.354403 833.008601 0.000000 2021-02-13T00:07:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:07:04Z- LcmTrn::publishEstimates() - MLE : 4058505.24 592049.22 0.11 2021-02-13T00:07:04Z- LcmTrn::publishEstimates() - MMSE : 4058507.74 592006.72 0.11 2021-02-13T00:07:04Z- LcmTrn::publishEstimates() - COVAR: 718.35 833.01 0.87 0.00 2021-02-13T00:07:04Z- AHRS_M2 msg: 1613174824.85 epoch sec; seqNo:9711 2021-02-13T00:07:04Z- AHRS_M2 msg: 5.94 phi; 0.11 theta; 0.29 psi 2021-02-13T00:07:04Z- handleDepth()-Depth_Keller msg: 1613174824861 epoch sec; seqNo:9789; depth 0.12 2021-02-13T00:07:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174824.88 epoch sec; seqNo:9789; 4058507.74 north; 592006.72 east 2021-02-13T00:07:05Z- AHRS_M2 msg: 1613174825.25 epoch sec; seqNo:9712 2021-02-13T00:07:05Z- AHRS_M2 msg: 5.96 phi; 0.13 theta; 0.21 psi 2021-02-13T00:07:05Z- handleDepth()-Depth_Keller msg: 1613174825261 epoch sec; seqNo:9790; depth 0.08 2021-02-13T00:07:05Z- handleDvl() - RDI_Pathfinder msg: 1613174825.27 epoch sec; seqNo:4740 2021-02-13T00:07:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:07:05Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:07:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174825.30 epoch sec; seqNo:9790; 4058508.12 north; 592006.71 east 2021-02-13T00:07:05Z- AHRS_M2 msg: 1613174825.66 epoch sec; seqNo:9713 2021-02-13T00:07:05Z- AHRS_M2 msg: 5.98 phi; 0.00 theta; 0.07 psi 2021-02-13T00:07:05Z- handleDepth()-Depth_Keller msg: 1613174825683 epoch sec; seqNo:9791; depth 0.04 2021-02-13T00:07:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174825.69 epoch sec; seqNo:9791; 4058508.12 north; 592006.71 east 2021-02-13T00:07:06Z- AHRS_M2 msg: 1613174826.06 epoch sec; seqNo:9714 2021-02-13T00:07:06Z- AHRS_M2 msg: 5.97 phi; -0.11 theta; -0.07 psi 2021-02-13T00:07:06Z- handleDepth()-Depth_Keller msg: 1613174826064 epoch sec; seqNo:9792; depth 0.05 2021-02-13T00:07:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174826.09 epoch sec; seqNo:9792; 4058508.50 north; 592006.71 east 2021-02-13T00:07:06Z- AHRS_M2 msg: 1613174826.46 epoch sec; seqNo:9715 2021-02-13T00:07:06Z- AHRS_M2 msg: 5.92 phi; -0.05 theta; -0.14 psi 2021-02-13T00:07:06Z- handleDepth()-Depth_Keller msg: 1613174826469 epoch sec; seqNo:9793; depth 0.03 2021-02-13T00:07:06Z- handleDvl() - RDI_Pathfinder msg: 1613174826.48 epoch sec; seqNo:4741 2021-02-13T00:07:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 97.24 , 97.24 , 99.95 2021-02-13T00:07:06Z- handleDvl() - RDI_Pathfinder msg: velocities -0.44 , 0.08 , -0.60 2021-02-13T00:07:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174826.51 epoch sec; seqNo:9793; 4058508.12 north; 592006.71 east 2021-02-13T00:07:06Z- AHRS_M2 msg: 1613174826.87 epoch sec; seqNo:9716 2021-02-13T00:07:06Z- AHRS_M2 msg: 5.92 phi; -0.02 theta; -0.06 psi 2021-02-13T00:07:06Z- handleDepth()-Depth_Keller msg: 1613174826877 epoch sec; seqNo:9794; depth 0.03 2021-02-13T00:07:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174826.89 epoch sec; seqNo:9794; 4058508.12 north; 592006.71 east 2021-02-13T00:07:07Z- AHRS_M2 msg: 1613174827.27 epoch sec; seqNo:9717 2021-02-13T00:07:07Z- AHRS_M2 msg: 5.93 phi; -0.07 theta; 0.05 psi 2021-02-13T00:07:07Z- handleDepth()-Depth_Keller msg: 1613174827278 epoch sec; seqNo:9795; depth 0.05 2021-02-13T00:07:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174827.37 epoch sec; seqNo:9795; 4058508.12 north; 592006.71 east 2021-02-13T00:07:07Z- AHRS_M2 msg: 1613174827.68 epoch sec; seqNo:9718 2021-02-13T00:07:07Z- AHRS_M2 msg: 5.91 phi; -0.04 theta; 0.08 psi 2021-02-13T00:07:07Z- handleDepth()-Depth_Keller msg: 1613174827681 epoch sec; seqNo:9796; depth 0.10 2021-02-13T00:07:08Z- handleDvl() - RDI_Pathfinder msg: 1613174828.33 epoch sec; seqNo:4742 2021-02-13T00:07:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 99.95 , 97.24 , 99.95 2021-02-13T00:07:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.00 , -0.11 , -0.29 2021-02-13T00:07:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174828.35 epoch sec; seqNo:9796; 4058508.12 north; 592006.71 east 2021-02-13T00:07:08Z- AHRS_M2 msg: 1613174828.47 epoch sec; seqNo:9719 2021-02-13T00:07:08Z- AHRS_M2 msg: 5.89 phi; 0.01 theta; 0.04 psi 2021-02-13T00:07:08Z- handleDepth()-Depth_Keller msg: 1613174828489 epoch sec; seqNo:9797; depth 0.06 2021-02-13T00:07:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174828.50 epoch sec; seqNo:9797; 4058508.12 north; 592006.71 east 2021-02-13T00:07:08Z- AHRS_M2 msg: 1613174828.87 epoch sec; seqNo:9720 2021-02-13T00:07:08Z- AHRS_M2 msg: 5.87 phi; -0.19 theta; -0.08 psi 2021-02-13T00:07:08Z- handleDepth()-Depth_Keller msg: 1613174828881 epoch sec; seqNo:9798; depth 0.06 2021-02-13T00:07:09Z- LcmTrn::handleMessages() - handling 32 messages took 0.57 ms and 121 calls... 2021-02-13T00:07:09Z- waiting for 1613174827.48, time is 1613174828.87 2021-02-13T00:07:09Z- LcmTrn::updateTrn() >>>> pose time 1613174828.87, meas time 1613174828.33 <<<< 2021-02-13T00:07:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174828.33 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:07:09Z- TerrainNav::motionUpdate() - currEstimate = (4058508, 592006.71, 0.06) 2021-02-13T00:07:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:07:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:07:09Z- TNavBF::Projecting Measurements 2021-02-13T00:07:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:07:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:07:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:07:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:07:09Z- TNavFilter::4 good beams. 2021-02-13T00:07:09Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:07:09Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:07:09Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:07:09Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:07:09Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:07:09Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:07:09Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:07:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:07:09Z- TerrainMapOctree: (vehN, vehE ) = (4058508.07, 592006.71). 2021-02-13T00:07:09Z- TerrainMapOctree: Min Distance = 3542.67. 2021-02-13T00:07:09Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:07:09Z- There is no specific good beam that all particles have in common. 2021-02-13T00:07:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:07:09Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-13T00:07:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:07:09Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 15.98 2021-02-13T00:07:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:07:09Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-13T00:07:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:07:09Z- Old Depth Bias: 0.00 DepthBiasCov: 0.76 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 15.98 2021-02-13T00:07:09Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:07:09Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:07:09Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:07:09Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:07:09Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:07:09Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:07:09Z- Filter 0 No beams used in subcloud update 2021-02-13T00:07:09Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:07:09Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:07:09Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 718.3 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:07:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174828.33 sec. 2021-02-13T00:07:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:07:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:07:09Z- variances([0], [2], and psi_berg) = 718.328162 833.008584 0.000000 2021-02-13T00:07:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:07:09Z- LcmTrn::publishEstimates() - MLE : 4058505.62 592049.21 0.06 2021-02-13T00:07:09Z- LcmTrn::publishEstimates() - MMSE : 4058508.12 592006.71 0.06 2021-02-13T00:07:09Z- LcmTrn::publishEstimates() - COVAR: 718.33 833.01 0.74 0.00 2021-02-13T00:07:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174830.93 epoch sec; seqNo:9798; 4058508.11 north; 592005.49 east 2021-02-13T00:07:11Z- AHRS_M2 msg: 1613174831.02 epoch sec; seqNo:9721 2021-02-13T00:07:11Z- AHRS_M2 msg: 5.94 phi; -0.03 theta; 0.06 psi 2021-02-13T00:07:11Z- handleDepth()-Depth_Keller msg: 1613174831028 epoch sec; seqNo:9799; depth 0.14 2021-02-13T00:07:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174831.04 epoch sec; seqNo:9799; 4058508.11 north; 592005.49 east 2021-02-13T00:07:11Z- AHRS_M2 msg: 1613174831.42 epoch sec; seqNo:9722 2021-02-13T00:07:11Z- AHRS_M2 msg: 5.98 phi; 0.07 theta; -0.00 psi 2021-02-13T00:07:11Z- handleDepth()-Depth_Keller msg: 1613174831421 epoch sec; seqNo:9800; depth 0.14 2021-02-13T00:07:11Z- handleDvl() - RDI_Pathfinder msg: 1613174831.43 epoch sec; seqNo:4743 2021-02-13T00:07:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.81 , 97.04 , 98.39 , 99.75 2021-02-13T00:07:11Z- handleDvl() - RDI_Pathfinder msg: velocities -0.10 , 0.14 , 0.05 2021-02-13T00:07:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174831.45 epoch sec; seqNo:9800; 4058507.73 north; 592005.50 east 2021-02-13T00:07:11Z- AHRS_M2 msg: 1613174831.82 epoch sec; seqNo:9723 2021-02-13T00:07:11Z- AHRS_M2 msg: 6.01 phi; -0.00 theta; -0.03 psi 2021-02-13T00:07:11Z- handleDepth()-Depth_Keller msg: 1613174831829 epoch sec; seqNo:9801; depth 0.09 2021-02-13T00:07:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174831.84 epoch sec; seqNo:9801; 4058507.73 north; 592005.50 east 2021-02-13T00:07:12Z- AHRS_M2 msg: 1613174832.22 epoch sec; seqNo:9724 2021-02-13T00:07:12Z- AHRS_M2 msg: 6.00 phi; -0.02 theta; 0.02 psi 2021-02-13T00:07:12Z- handleDepth()-Depth_Keller msg: 1613174832230 epoch sec; seqNo:9802; depth 0.09 2021-02-13T00:07:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174832.24 epoch sec; seqNo:9802; 4058507.74 north; 592006.72 east 2021-02-13T00:07:12Z- AHRS_M2 msg: 1613174832.63 epoch sec; seqNo:9725 2021-02-13T00:07:12Z- AHRS_M2 msg: 6.01 phi; -0.04 theta; 0.06 psi 2021-02-13T00:07:12Z- handleDepth()-Depth_Keller msg: 1613174832641 epoch sec; seqNo:9803; depth 0.10 2021-02-13T00:07:12Z- handleDvl() - RDI_Pathfinder msg: 1613174832.67 epoch sec; seqNo:4744 2021-02-13T00:07:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.39 , 99.75 , 101.10 , 97.04 2021-02-13T00:07:12Z- handleDvl() - RDI_Pathfinder msg: velocities -0.51 , 0.29 , 0.18 2021-02-13T00:07:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174832.68 epoch sec; seqNo:9803; 4058507.74 north; 592006.72 east 2021-02-13T00:07:13Z- AHRS_M2 msg: 1613174833.04 epoch sec; seqNo:9726 2021-02-13T00:07:13Z- AHRS_M2 msg: 6.04 phi; -0.09 theta; 0.01 psi 2021-02-13T00:07:13Z- handleDepth()-Depth_Keller msg: 1613174833045 epoch sec; seqNo:9804; depth 0.10 2021-02-13T00:07:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:07:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:07:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:07:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:07:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:07:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:07:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:07:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:07:13Z- handleDvl() - RDI_Pathfinder msg: 1613174833.05 epoch sec; seqNo:4745 2021-02-13T00:07:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:07:13Z- handleDvl() - RDI_Pathfinder msg: velocities -0.51 , 0.29 , 0.18 2021-02-13T00:07:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174833.06 epoch sec; seqNo:9804; 4058507.36 north; 592006.72 east 2021-02-13T00:07:13Z- AHRS_M2 msg: 1613174833.44 epoch sec; seqNo:9727 2021-02-13T00:07:13Z- AHRS_M2 msg: 6.01 phi; -0.08 theta; -0.10 psi 2021-02-13T00:07:13Z- handleDepth()-Depth_Keller msg: 1613174833449 epoch sec; seqNo:9805; depth 0.07 2021-02-13T00:07:13Z- handleDvl() - RDI_Pathfinder msg: 1613174833.46 epoch sec; seqNo:4746 2021-02-13T00:07:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.87 , 97.11 , 97.11 , 101.17 2021-02-13T00:07:13Z- handleDvl() - RDI_Pathfinder msg: velocities -0.40 , 0.60 , -0.56 2021-02-13T00:07:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174833.47 epoch sec; seqNo:9805; 4058507.36 north; 592006.72 east 2021-02-13T00:07:13Z- AHRS_M2 msg: 1613174833.84 epoch sec; seqNo:9728 2021-02-13T00:07:13Z- AHRS_M2 msg: 5.98 phi; -0.03 theta; -0.17 psi 2021-02-13T00:07:13Z- handleDepth()-Depth_Keller msg: 1613174833852 epoch sec; seqNo:9806; depth 0.04 2021-02-13T00:07:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174833.87 epoch sec; seqNo:9806; 4058507.36 north; 592006.72 east 2021-02-13T00:07:14Z- AHRS_M2 msg: 1613174834.25 epoch sec; seqNo:9729 2021-02-13T00:07:14Z- AHRS_M2 msg: 5.95 phi; -0.03 theta; -0.13 psi 2021-02-13T00:07:14Z- handleDepth()-Depth_Keller msg: 1613174834257 epoch sec; seqNo:9807; depth 0.04 2021-02-13T00:07:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174834.27 epoch sec; seqNo:9807; 4058507.36 north; 592006.72 east 2021-02-13T00:07:14Z- AHRS_M2 msg: 1613174834.65 epoch sec; seqNo:9730 2021-02-13T00:07:14Z- AHRS_M2 msg: 5.93 phi; -0.00 theta; 0.07 psi 2021-02-13T00:07:14Z- handleDepth()-Depth_Keller msg: 1613174834657 epoch sec; seqNo:9808; depth 0.05 2021-02-13T00:07:14Z- handleDvl() - RDI_Pathfinder msg: 1613174834.67 epoch sec; seqNo:4747 2021-02-13T00:07:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 110.57 , 98.39 , 101.10 2021-02-13T00:07:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.18 , -0.83 , -0.54 2021-02-13T00:07:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174834.69 epoch sec; seqNo:9808; 4058507.36 north; 592006.72 east 2021-02-13T00:07:14Z- LcmTrn::handleMessages() - handling 36 messages took 0.59 ms and 125 calls... 2021-02-13T00:07:14Z- waiting for 1613174831.87, time is 1613174834.69 2021-02-13T00:07:14Z- LcmTrn::updateTrn() equating pose 1613174834.69 and meas times 1613174834.69 2021-02-13T00:07:14Z- LcmTrn::updateTrn() >>>> pose time 1613174834.69, meas time 1613174834.69 <<<< 2021-02-13T00:07:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174834.69 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:07:14Z- TerrainNav::motionUpdate() - currEstimate = (4058507, 592006.72, 0.05) 2021-02-13T00:07:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:07:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:07:14Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174834.69 sec.; No relevant navigation data available 2021-02-13T00:07:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:07:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:07:14Z- variances([0], [2], and psi_berg) = 718.299356 833.008565 0.000000 2021-02-13T00:07:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:07:14Z- LcmTrn::publishEstimates() - MLE : 4058504.86 592049.22 0.05 2021-02-13T00:07:14Z- LcmTrn::publishEstimates() - MMSE : 4058507.36 592006.72 0.05 2021-02-13T00:07:14Z- LcmTrn::publishEstimates() - COVAR: 718.30 833.01 0.77 0.00 2021-02-13T00:07:15Z- AHRS_M2 msg: 1613174835.06 epoch sec; seqNo:9731 2021-02-13T00:07:15Z- AHRS_M2 msg: 5.96 phi; -0.01 theta; 0.28 psi 2021-02-13T00:07:15Z- handleDepth()-Depth_Keller msg: 1613174835060 epoch sec; seqNo:9809; depth 0.06 2021-02-13T00:07:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174835.07 epoch sec; seqNo:9809; 4058507.36 north; 592006.72 east 2021-02-13T00:07:15Z- AHRS_M2 msg: 1613174835.46 epoch sec; seqNo:9732 2021-02-13T00:07:15Z- AHRS_M2 msg: 5.98 phi; -0.01 theta; 0.34 psi 2021-02-13T00:07:15Z- handleDepth()-Depth_Keller msg: 1613174835469 epoch sec; seqNo:9810; depth 0.07 2021-02-13T00:07:15Z- handleDvl() - RDI_Pathfinder msg: 1613174835.48 epoch sec; seqNo:4748 2021-02-13T00:07:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 106.92 , 97.99 , 105.43 , 97.99 2021-02-13T00:07:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.02 , -0.34 , 0.55 2021-02-13T00:07:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174835.49 epoch sec; seqNo:9810; 4058507.36 north; 592006.72 east 2021-02-13T00:07:15Z- AHRS_M2 msg: 1613174835.86 epoch sec; seqNo:9733 2021-02-13T00:07:15Z- AHRS_M2 msg: 5.96 phi; -0.01 theta; 0.23 psi 2021-02-13T00:07:15Z- handleDepth()-Depth_Keller msg: 1613174835869 epoch sec; seqNo:9811; depth 0.06 2021-02-13T00:07:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174835.89 epoch sec; seqNo:9811; 4058506.98 north; 592006.73 east 2021-02-13T00:07:16Z- AHRS_M2 msg: 1613174836.28 epoch sec; seqNo:9734 2021-02-13T00:07:16Z- AHRS_M2 msg: 6.02 phi; -0.02 theta; 0.05 psi 2021-02-13T00:07:16Z- handleDepth()-Depth_Keller msg: 1613174836281 epoch sec; seqNo:9812; depth 0.02 2021-02-13T00:07:16Z- handleDvl() - RDI_Pathfinder msg: 1613174836.30 epoch sec; seqNo:4749 2021-02-13T00:07:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.92 , 102.38 , 97.92 2021-02-13T00:07:16Z- handleDvl() - RDI_Pathfinder msg: velocities -0.22 , 0.04 , 0.46 2021-02-13T00:07:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174836.31 epoch sec; seqNo:9812; 4058506.98 north; 592006.73 east 2021-02-13T00:07:16Z- AHRS_M2 msg: 1613174836.68 epoch sec; seqNo:9735 2021-02-13T00:07:16Z- AHRS_M2 msg: 6.04 phi; -0.08 theta; -0.03 psi 2021-02-13T00:07:16Z- handleDepth()-Depth_Keller msg: 1613174836683 epoch sec; seqNo:9813; depth -0.00 2021-02-13T00:07:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174836.69 epoch sec; seqNo:9813; 4058506.98 north; 592006.73 east 2021-02-13T00:07:17Z- AHRS_M2 msg: 1613174837.08 epoch sec; seqNo:9736 2021-02-13T00:07:17Z- AHRS_M2 msg: 5.95 phi; -0.07 theta; -0.05 psi 2021-02-13T00:07:17Z- handleDepth()-Depth_Keller msg: 1613174837082 epoch sec; seqNo:9814; depth -0.00 2021-02-13T00:07:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174837.09 epoch sec; seqNo:9814; 4058506.98 north; 592006.73 east 2021-02-13T00:07:17Z- AHRS_M2 msg: 1613174837.48 epoch sec; seqNo:9737 2021-02-13T00:07:17Z- AHRS_M2 msg: 5.88 phi; -0.10 theta; -0.05 psi 2021-02-13T00:07:17Z- handleDepth()-Depth_Keller msg: 1613174837489 epoch sec; seqNo:9815; depth 0.02 2021-02-13T00:07:17Z- handleDvl() - RDI_Pathfinder msg: 1613174837.51 epoch sec; seqNo:4750 2021-02-13T00:07:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.46 , 99.81 , 97.11 , 103.87 2021-02-13T00:07:17Z- handleDvl() - RDI_Pathfinder msg: velocities -0.32 , 0.27 , -0.28 2021-02-13T00:07:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174837.52 epoch sec; seqNo:9815; 4058506.61 north; 592006.73 east 2021-02-13T00:07:17Z- AHRS_M2 msg: 1613174837.88 epoch sec; seqNo:9738 2021-02-13T00:07:17Z- AHRS_M2 msg: 5.90 phi; -0.10 theta; -0.09 psi 2021-02-13T00:07:17Z- handleDepth()-Depth_Keller msg: 1613174837889 epoch sec; seqNo:9816; depth 0.04 2021-02-13T00:07:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174837.97 epoch sec; seqNo:9816; 4058506.61 north; 592006.73 east 2021-02-13T00:07:18Z- AHRS_M2 msg: 1613174838.29 epoch sec; seqNo:9739 2021-02-13T00:07:18Z- AHRS_M2 msg: 5.94 phi; -0.03 theta; -0.06 psi 2021-02-13T00:07:18Z- handleDepth()-Depth_Keller msg: 1613174838297 epoch sec; seqNo:9817; depth 0.07 2021-02-13T00:07:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174838.31 epoch sec; seqNo:9817; 4058506.61 north; 592006.73 east 2021-02-13T00:07:18Z- AHRS_M2 msg: 1613174838.69 epoch sec; seqNo:9740 2021-02-13T00:07:18Z- AHRS_M2 msg: 5.94 phi; -0.02 theta; -0.01 psi 2021-02-13T00:07:18Z- handleDepth()-Depth_Keller msg: 1613174838712 epoch sec; seqNo:9818; depth 0.06 2021-02-13T00:07:18Z- handleDvl() - RDI_Pathfinder msg: 1613174838.72 epoch sec; seqNo:4751 2021-02-13T00:07:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 103.81 , 97.04 , 103.81 2021-02-13T00:07:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.19 , -0.22 , -0.77 2021-02-13T00:07:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174838.73 epoch sec; seqNo:9818; 4058506.61 north; 592006.73 east 2021-02-13T00:07:19Z- AHRS_M2 msg: 1613174839.10 epoch sec; seqNo:9741 2021-02-13T00:07:19Z- AHRS_M2 msg: 5.90 phi; -0.03 theta; -0.00 psi 2021-02-13T00:07:19Z- handleDepth()-Depth_Keller msg: 1613174839101 epoch sec; seqNo:9819; depth 0.07 2021-02-13T00:07:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174839.12 epoch sec; seqNo:9819; 4058506.61 north; 592006.73 east 2021-02-13T00:07:19Z- AHRS_M2 msg: 1613174839.50 epoch sec; seqNo:9742 2021-02-13T00:07:19Z- AHRS_M2 msg: 5.90 phi; 0.03 theta; -0.00 psi 2021-02-13T00:07:19Z- handleDepth()-Depth_Keller msg: 1613174839509 epoch sec; seqNo:9820; depth 0.09 2021-02-13T00:07:19Z- handleDvl() - RDI_Pathfinder msg: 1613174839.52 epoch sec; seqNo:4752 2021-02-13T00:07:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.33 , 98.33 , 98.33 , 98.33 2021-02-13T00:07:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.28 , -0.74 , -0.00 2021-02-13T00:07:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174839.54 epoch sec; seqNo:9820; 4058506.61 north; 592006.73 east 2021-02-13T00:07:19Z- AHRS_M2 msg: 1613174839.90 epoch sec; seqNo:9743 2021-02-13T00:07:19Z- AHRS_M2 msg: 5.91 phi; 0.06 theta; 0.03 psi 2021-02-13T00:07:19Z- handleDepth()-Depth_Keller msg: 1613174839913 epoch sec; seqNo:9821; depth 0.05 2021-02-13T00:07:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174839.92 epoch sec; seqNo:9821; 4058506.59 north; 592005.51 east 2021-02-13T00:07:19Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-13T00:07:19Z- waiting for 1613174837.69, time is 1613174839.92 2021-02-13T00:07:19Z- LcmTrn::updateTrn() >>>> pose time 1613174839.92, meas time 1613174839.52 <<<< 2021-02-13T00:07:19Z- TerrainNav:: Throwing out all beams because more than two are equal to 98.326 at t=1613174839.52, ping # 0. 2021-02-13T00:07:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174839.52 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:07:19Z- TerrainNav::motionUpdate() - currEstimate = (4058507, 592005.51, 0.05) 2021-02-13T00:07:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:07:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:07:19Z- TNavBF::Projecting Measurements 2021-02-13T00:07:19Z- TNavFilter::Measurement from time = 1613174839.52 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:07:19Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:07:19Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:07:19Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:07:19Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:07:19Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:07:19Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:07:19Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:07:19Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 718.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:07:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:07:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:07:19Z- variances([0], [2], and psi_berg) = 718.250275 833.008534 0.000000 2021-02-13T00:07:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:07:19Z- LcmTrn::publishEstimates() - MLE : 4058504.09 591963.01 0.05 2021-02-13T00:07:19Z- LcmTrn::publishEstimates() - MMSE : 4058506.59 592005.51 0.05 2021-02-13T00:07:19Z- LcmTrn::publishEstimates() - COVAR: 718.25 833.01 0.85 0.00 2021-02-13T00:07:20Z- AHRS_M2 msg: 1613174840.31 epoch sec; seqNo:9744 2021-02-13T00:07:20Z- AHRS_M2 msg: 5.91 phi; 0.00 theta; 0.17 psi 2021-02-13T00:07:20Z- handleDepth()-Depth_Keller msg: 1613174840313 epoch sec; seqNo:9822; depth 0.04 2021-02-13T00:07:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174840.33 epoch sec; seqNo:9822; 4058506.59 north; 592005.51 east 2021-02-13T00:07:20Z- AHRS_M2 msg: 1613174840.72 epoch sec; seqNo:9745 2021-02-13T00:07:20Z- AHRS_M2 msg: 5.90 phi; -0.03 theta; 0.29 psi 2021-02-13T00:07:20Z- handleDepth()-Depth_Keller msg: 1613174840729 epoch sec; seqNo:9823; depth 0.07 2021-02-13T00:07:20Z- handleDvl() - RDI_Pathfinder msg: 1613174840.74 epoch sec; seqNo:4753 2021-02-13T00:07:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.23 , 97.18 , 103.94 , 97.18 2021-02-13T00:07:20Z- handleDvl() - RDI_Pathfinder msg: velocities -0.15 , -0.30 , 1.44 2021-02-13T00:07:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174840.75 epoch sec; seqNo:9823; 4058506.59 north; 592005.51 east 2021-02-13T00:07:21Z- AHRS_M2 msg: 1613174841.12 epoch sec; seqNo:9746 2021-02-13T00:07:21Z- AHRS_M2 msg: 5.89 phi; -0.04 theta; 0.29 psi 2021-02-13T00:07:21Z- handleDepth()-Depth_Keller msg: 1613174841121 epoch sec; seqNo:9824; depth 0.04 2021-02-13T00:07:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174841.14 epoch sec; seqNo:9824; 4058506.21 north; 592005.51 east 2021-02-13T00:07:21Z- AHRS_M2 msg: 1613174841.52 epoch sec; seqNo:9747 2021-02-13T00:07:21Z- AHRS_M2 msg: 5.86 phi; -0.04 theta; 0.16 psi 2021-02-13T00:07:21Z- handleDepth()-Depth_Keller msg: 1613174841529 epoch sec; seqNo:9825; depth 0.02 2021-02-13T00:07:21Z- handleDvl() - RDI_Pathfinder msg: 1613174841.55 epoch sec; seqNo:4754 2021-02-13T00:07:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 126.80 , 88.93 , 105.16 , 106.51 2021-02-13T00:07:21Z- handleDvl() - RDI_Pathfinder msg: velocities -0.74 , 0.44 , 0.66 2021-02-13T00:07:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174841.56 epoch sec; seqNo:9825; 4058506.21 north; 592005.51 east 2021-02-13T00:07:21Z- AHRS_M2 msg: 1613174841.92 epoch sec; seqNo:9748 2021-02-13T00:07:21Z- AHRS_M2 msg: 5.85 phi; -0.06 theta; -0.04 psi 2021-02-13T00:07:21Z- handleDepth()-Depth_Keller msg: 1613174841933 epoch sec; seqNo:9826; depth -0.02 2021-02-13T00:07:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174841.94 epoch sec; seqNo:9826; 4058505.84 north; 592005.52 east 2021-02-13T00:07:22Z- AHRS_M2 msg: 1613174842.33 epoch sec; seqNo:9749 2021-02-13T00:07:22Z- AHRS_M2 msg: 5.83 phi; -0.10 theta; -0.18 psi 2021-02-13T00:07:22Z- handleDepth()-Depth_Keller msg: 1613174842339 epoch sec; seqNo:9827; depth 0.01 2021-02-13T00:07:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174842.35 epoch sec; seqNo:9827; 4058505.84 north; 592005.52 east 2021-02-13T00:07:22Z- AHRS_M2 msg: 1613174842.73 epoch sec; seqNo:9750 2021-02-13T00:07:22Z- AHRS_M2 msg: 5.83 phi; -0.08 theta; -0.16 psi 2021-02-13T00:07:22Z- handleDepth()-Depth_Keller msg: 1613174842742 epoch sec; seqNo:9828; depth 0.05 2021-02-13T00:07:22Z- handleDvl() - RDI_Pathfinder msg: 1613174842.84 epoch sec; seqNo:4755 2021-02-13T00:07:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.47 , 99.00 , 101.98 , 100.49 2021-02-13T00:07:22Z- handleDvl() - RDI_Pathfinder msg: velocities -0.85 , 0.40 , -0.43 2021-02-13T00:07:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174842.95 epoch sec; seqNo:9828; 4058505.47 north; 592006.74 east 2021-02-13T00:07:23Z- AHRS_M2 msg: 1613174843.22 epoch sec; seqNo:9751 2021-02-13T00:07:23Z- AHRS_M2 msg: 5.81 phi; -0.07 theta; -0.07 psi 2021-02-13T00:07:23Z- handleDepth()-Depth_Keller msg: 1613174843242 epoch sec; seqNo:9829; depth 0.01 2021-02-13T00:07:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174843.27 epoch sec; seqNo:9829; 4058505.09 north; 592006.75 east 2021-02-13T00:07:23Z- AHRS_M2 msg: 1613174843.59 epoch sec; seqNo:9752 2021-02-13T00:07:23Z- AHRS_M2 msg: 5.79 phi; -0.06 theta; 0.01 psi 2021-02-13T00:07:23Z- handleDepth()-Depth_Keller msg: 1613174843603 epoch sec; seqNo:9830; depth 0.02 2021-02-13T00:07:23Z- handleDvl() - RDI_Pathfinder msg: 1613174843.62 epoch sec; seqNo:4756 2021-02-13T00:07:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 106.92 , 97.99 , 100.96 2021-02-13T00:07:23Z- handleDvl() - RDI_Pathfinder msg: velocities -0.21 , 0.12 , -0.74 2021-02-13T00:07:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174843.63 epoch sec; seqNo:9830; 4058505.09 north; 592006.75 east 2021-02-13T00:07:23Z- AHRS_M2 msg: 1613174843.99 epoch sec; seqNo:9753 2021-02-13T00:07:23Z- AHRS_M2 msg: 5.78 phi; -0.04 theta; 0.09 psi 2021-02-13T00:07:24Z- handleDepth()-Depth_Keller msg: 1613174844005 epoch sec; seqNo:9831; depth 0.06 2021-02-13T00:07:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174844.03 epoch sec; seqNo:9831; 4058505.09 north; 592006.75 east 2021-02-13T00:07:24Z- AHRS_M2 msg: 1613174844.42 epoch sec; seqNo:9754 2021-02-13T00:07:24Z- AHRS_M2 msg: 5.76 phi; -0.07 theta; 0.02 psi 2021-02-13T00:07:24Z- handleDepth()-Depth_Keller msg: 1613174844427 epoch sec; seqNo:9832; depth 0.06 2021-02-13T00:07:24Z- handleDvl() - RDI_Pathfinder msg: 1613174844.44 epoch sec; seqNo:4757 2021-02-13T00:07:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.68 , 96.97 , 96.97 , 102.38 2021-02-13T00:07:24Z- handleDvl() - RDI_Pathfinder msg: velocities -0.07 , -0.18 , -0.32 2021-02-13T00:07:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174844.46 epoch sec; seqNo:9832; 4058505.09 north; 592006.75 east 2021-02-13T00:07:24Z- AHRS_M2 msg: 1613174844.80 epoch sec; seqNo:9755 2021-02-13T00:07:24Z- AHRS_M2 msg: 5.76 phi; -0.09 theta; -0.05 psi 2021-02-13T00:07:24Z- handleDepth()-Depth_Keller msg: 1613174844808 epoch sec; seqNo:9833; depth 0.04 2021-02-13T00:07:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174844.83 epoch sec; seqNo:9833; 4058505.09 north; 592006.75 east 2021-02-13T00:07:25Z- LcmTrn::handleMessages() - handling 41 messages took 0.73 ms and 127 calls... 2021-02-13T00:07:25Z- waiting for 1613174842.92, time is 1613174844.83 2021-02-13T00:07:25Z- LcmTrn::updateTrn() >>>> pose time 1613174844.83, meas time 1613174844.44 <<<< 2021-02-13T00:07:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174844.44 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:07:25Z- TerrainNav::motionUpdate() - currEstimate = (4058505, 592006.75, 0.04) 2021-02-13T00:07:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:07:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:07:25Z- TNavBF::Projecting Measurements 2021-02-13T00:07:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:07:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:07:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:07:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:07:25Z- TNavFilter::4 good beams. 2021-02-13T00:07:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:07:25Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:07:25Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:07:25Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:07:25Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:07:25Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:07:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:07:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:07:25Z- TerrainMapOctree: (vehN, vehE ) = (4058505.21, 592006.65). 2021-02-13T00:07:25Z- TerrainMapOctree: Min Distance = 3544.93. 2021-02-13T00:07:25Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:07:25Z- There is no specific good beam that all particles have in common. 2021-02-13T00:07:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:07:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 15.90 2021-02-13T00:07:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:07:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 15.05 2021-02-13T00:07:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:07:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 15.05 2021-02-13T00:07:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:07:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 16.77 2021-02-13T00:07:25Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:07:25Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:07:25Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:07:25Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:07:25Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:07:25Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:07:25Z- Filter 0 No beams used in subcloud update 2021-02-13T00:07:25Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:07:25Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:07:25Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 718.2 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:07:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174844.44 sec. 2021-02-13T00:07:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:07:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:07:25Z- variances([0], [2], and psi_berg) = 718.193066 833.008497 0.000000 2021-02-13T00:07:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:07:25Z- LcmTrn::publishEstimates() - MLE : 4058502.59 591964.25 0.04 2021-02-13T00:07:25Z- LcmTrn::publishEstimates() - MMSE : 4058505.09 592006.75 0.04 2021-02-13T00:07:25Z- LcmTrn::publishEstimates() - COVAR: 718.19 833.01 0.78 0.00 2021-02-13T00:07:25Z- AHRS_M2 msg: 1613174845.21 epoch sec; seqNo:9756 2021-02-13T00:07:25Z- AHRS_M2 msg: 5.81 phi; -0.05 theta; 0.01 psi 2021-02-13T00:07:25Z- handleDepth()-Depth_Keller msg: 1613174845212 epoch sec; seqNo:9834; depth 0.04 2021-02-13T00:07:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174845.22 epoch sec; seqNo:9834; 4058505.09 north; 592006.75 east 2021-02-13T00:07:25Z- AHRS_M2 msg: 1613174845.61 epoch sec; seqNo:9757 2021-02-13T00:07:25Z- AHRS_M2 msg: 5.81 phi; -0.01 theta; 0.12 psi 2021-02-13T00:07:25Z- handleDepth()-Depth_Keller msg: 1613174845615 epoch sec; seqNo:9835; depth 0.02 2021-02-13T00:07:25Z- handleDvl() - RDI_Pathfinder msg: 1613174845.63 epoch sec; seqNo:4758 2021-02-13T00:07:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 99.81 , 97.11 , 102.52 2021-02-13T00:07:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.06 , -0.48 , -0.09 2021-02-13T00:07:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174845.64 epoch sec; seqNo:9835; 4058505.09 north; 592006.75 east 2021-02-13T00:07:26Z- AHRS_M2 msg: 1613174846.02 epoch sec; seqNo:9758 2021-02-13T00:07:26Z- AHRS_M2 msg: 5.79 phi; 0.01 theta; 0.15 psi 2021-02-13T00:07:26Z- handleDepth()-Depth_Keller msg: 1613174846025 epoch sec; seqNo:9836; depth 0.07 2021-02-13T00:07:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174846.04 epoch sec; seqNo:9836; 4058504.71 north; 592006.75 east 2021-02-13T00:07:26Z- AHRS_M2 msg: 1613174846.42 epoch sec; seqNo:9759 2021-02-13T00:07:26Z- AHRS_M2 msg: 5.80 phi; 0.02 theta; 0.08 psi 2021-02-13T00:07:26Z- handleDepth()-Depth_Keller msg: 1613174846429 epoch sec; seqNo:9837; depth 0.06 2021-02-13T00:07:26Z- handleDvl() - RDI_Pathfinder msg: 1613174846.45 epoch sec; seqNo:4759 2021-02-13T00:07:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 107.93 , 97.11 , 99.81 , 101.17 2021-02-13T00:07:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.35 , 0.01 , -0.05 2021-02-13T00:07:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174846.46 epoch sec; seqNo:9837; 4058505.09 north; 592006.75 east 2021-02-13T00:07:26Z- AHRS_M2 msg: 1613174846.82 epoch sec; seqNo:9760 2021-02-13T00:07:26Z- AHRS_M2 msg: 5.79 phi; 0.06 theta; -0.01 psi 2021-02-13T00:07:26Z- handleDepth()-Depth_Keller msg: 1613174846829 epoch sec; seqNo:9838; depth 0.06 2021-02-13T00:07:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174846.85 epoch sec; seqNo:9838; 4058505.09 north; 592006.75 east 2021-02-13T00:07:27Z- AHRS_M2 msg: 1613174847.23 epoch sec; seqNo:9761 2021-02-13T00:07:27Z- AHRS_M2 msg: 5.79 phi; 0.04 theta; -0.09 psi 2021-02-13T00:07:27Z- handleDepth()-Depth_Keller msg: 1613174847233 epoch sec; seqNo:9839; depth 0.06 2021-02-13T00:07:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174847.24 epoch sec; seqNo:9839; 4058505.08 north; 592005.53 east 2021-02-13T00:07:27Z- AHRS_M2 msg: 1613174847.63 epoch sec; seqNo:9762 2021-02-13T00:07:27Z- AHRS_M2 msg: 5.76 phi; 0.02 theta; -0.07 psi 2021-02-13T00:07:27Z- handleDepth()-Depth_Keller msg: 1613174847642 epoch sec; seqNo:9840; depth 0.03 2021-02-13T00:07:27Z- handleDvl() - RDI_Pathfinder msg: 1613174847.65 epoch sec; seqNo:4760 2021-02-13T00:07:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 99.48 , 100.96 , 97.99 2021-02-13T00:07:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.15 , -0.22 , 0.28 2021-02-13T00:07:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174847.66 epoch sec; seqNo:9840; 4058505.08 north; 592005.53 east 2021-02-13T00:07:28Z- AHRS_M2 msg: 1613174848.04 epoch sec; seqNo:9763 2021-02-13T00:07:28Z- AHRS_M2 msg: 5.78 phi; -0.04 theta; -0.00 psi 2021-02-13T00:07:28Z- handleDepth()-Depth_Keller msg: 1613174848045 epoch sec; seqNo:9841; depth 0.04 2021-02-13T00:07:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174848.06 epoch sec; seqNo:9841; 4058505.08 north; 592005.53 east 2021-02-13T00:07:28Z- AHRS_M2 msg: 1613174848.44 epoch sec; seqNo:9764 2021-02-13T00:07:28Z- AHRS_M2 msg: 5.82 phi; -0.11 theta; 0.06 psi 2021-02-13T00:07:28Z- handleDepth()-Depth_Keller msg: 1613174848444 epoch sec; seqNo:9842; depth 0.05 2021-02-13T00:07:28Z- handleDvl() - RDI_Pathfinder msg: 1613174848.46 epoch sec; seqNo:4761 2021-02-13T00:07:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 102.52 , 99.81 , 98.46 2021-02-13T00:07:28Z- handleDvl() - RDI_Pathfinder msg: velocities -0.53 , -0.32 , 0.42 2021-02-13T00:07:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174848.47 epoch sec; seqNo:9842; 4058505.08 north; 592005.53 east 2021-02-13T00:07:28Z- AHRS_M2 msg: 1613174848.85 epoch sec; seqNo:9765 2021-02-13T00:07:28Z- AHRS_M2 msg: 5.81 phi; -0.11 theta; 0.16 psi 2021-02-13T00:07:28Z- handleDepth()-Depth_Keller msg: 1613174848857 epoch sec; seqNo:9843; depth 0.04 2021-02-13T00:07:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174848.87 epoch sec; seqNo:9843; 4058504.70 north; 592005.53 east 2021-02-13T00:07:29Z- AHRS_M2 msg: 1613174849.25 epoch sec; seqNo:9766 2021-02-13T00:07:29Z- AHRS_M2 msg: 5.81 phi; -0.11 theta; 0.21 psi 2021-02-13T00:07:29Z- handleDepth()-Depth_Keller msg: 1613174849262 epoch sec; seqNo:9844; depth 0.06 2021-02-13T00:07:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174849.27 epoch sec; seqNo:9844; 4058504.32 north; 592005.53 east 2021-02-13T00:07:29Z- AHRS_M2 msg: 1613174849.65 epoch sec; seqNo:9767 2021-02-13T00:07:29Z- AHRS_M2 msg: 5.81 phi; -0.11 theta; 0.13 psi 2021-02-13T00:07:29Z- handleDepth()-Depth_Keller msg: 1613174849661 epoch sec; seqNo:9845; depth 0.03 2021-02-13T00:07:29Z- handleDvl() - RDI_Pathfinder msg: 1613174849.75 epoch sec; seqNo:4762 2021-02-13T00:07:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 110.64 , 97.11 , 97.11 , 106.58 2021-02-13T00:07:29Z- handleDvl() - RDI_Pathfinder msg: velocities -0.45 , 0.08 , 0.38 2021-02-13T00:07:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174849.76 epoch sec; seqNo:9845; 4058504.32 north; 592005.53 east 2021-02-13T00:07:30Z- AHRS_M2 msg: 1613174850.06 epoch sec; seqNo:9768 2021-02-13T00:07:30Z- AHRS_M2 msg: 5.80 phi; -0.10 theta; 0.01 psi 2021-02-13T00:07:30Z- handleDepth()-Depth_Keller msg: 1613174850065 epoch sec; seqNo:9846; depth 0.04 2021-02-13T00:07:30Z- LcmTrn::handleMessages() - handling 43 messages took 0.70 ms and 129 calls... 2021-02-13T00:07:30Z- waiting for 1613174847.83, time is 1613174850.06 2021-02-13T00:07:30Z- LcmTrn::updateTrn() >>>> pose time 1613174850.06, meas time 1613174849.75 <<<< 2021-02-13T00:07:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174849.75 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:07:30Z- TerrainNav::motionUpdate() - currEstimate = (4058504, 592005.53, 0.04) 2021-02-13T00:07:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:07:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:07:30Z- TNavBF::Projecting Measurements 2021-02-13T00:07:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:07:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:07:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:07:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:07:30Z- TNavFilter::4 good beams. 2021-02-13T00:07:30Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:07:30Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:07:30Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:07:30Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:07:30Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:07:30Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:07:30Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:07:30Z- TNavBF::mmse selected from filter -1 2021-02-13T00:07:30Z- TerrainMapOctree: (vehN, vehE ) = (4058504.37, 592005.61). 2021-02-13T00:07:30Z- TerrainMapOctree: Min Distance = 3546.24. 2021-02-13T00:07:30Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:07:30Z- There is no specific good beam that all particles have in common. 2021-02-13T00:07:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:07:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 19.58 2021-02-13T00:07:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:07:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-13T00:07:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:07:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-13T00:07:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:07:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.74 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 18.17 2021-02-13T00:07:30Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:07:30Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:07:30Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:07:30Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:07:30Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:07:30Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:07:30Z- Filter 0 No beams used in subcloud update 2021-02-13T00:07:30Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:07:30Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:07:30Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 718.2 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:07:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174849.75 sec. 2021-02-13T00:07:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:07:30Z- TNavBF::mmse selected from filter -1 2021-02-13T00:07:30Z- variances([0], [2], and psi_berg) = 718.145022 833.008467 0.000000 2021-02-13T00:07:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:07:30Z- LcmTrn::publishEstimates() - MLE : 4058501.82 591963.03 0.04 2021-02-13T00:07:30Z- LcmTrn::publishEstimates() - MMSE : 4058504.32 592005.53 0.04 2021-02-13T00:07:30Z- LcmTrn::publishEstimates() - COVAR: 718.15 833.01 0.72 0.00 2021-02-13T00:07:30Z- handleDvl() - RDI_Pathfinder msg: 1613174850.72 epoch sec; seqNo:4763 2021-02-13T00:07:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 105.29 , 97.85 , 97.85 , 105.29 2021-02-13T00:07:30Z- handleDvl() - RDI_Pathfinder msg: velocities -0.31 , 0.08 , -0.27 2021-02-13T00:07:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174850.74 epoch sec; seqNo:9846; 4058503.95 north; 592006.76 east 2021-02-13T00:07:30Z- handleDepth()-Depth_Keller msg: 1613174850865 epoch sec; seqNo:9847; depth 0.08 2021-02-13T00:07:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174850.88 epoch sec; seqNo:9847; 4058503.95 north; 592006.76 east 2021-02-13T00:07:31Z- AHRS_M2 msg: 1613174851.23 epoch sec; seqNo:9769 2021-02-13T00:07:31Z- AHRS_M2 msg: 5.81 phi; 0.03 theta; 0.03 psi 2021-02-13T00:07:31Z- handleDepth()-Depth_Keller msg: 1613174851237 epoch sec; seqNo:9848; depth 0.07 2021-02-13T00:07:33Z- handleDvl() - RDI_Pathfinder msg: 1613174853.27 epoch sec; seqNo:4764 2021-02-13T00:07:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 102.45 , 97.99 , 103.94 2021-02-13T00:07:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.15 , 0.17 , -0.20 2021-02-13T00:07:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174853.29 epoch sec; seqNo:9848; 4058504.33 north; 592006.75 east 2021-02-13T00:07:33Z- AHRS_M2 msg: 1613174853.45 epoch sec; seqNo:9770 2021-02-13T00:07:33Z- AHRS_M2 msg: 5.76 phi; 0.01 theta; -0.04 psi 2021-02-13T00:07:33Z- handleDepth()-Depth_Keller msg: 1613174853455 epoch sec; seqNo:9849; depth 0.05 2021-02-13T00:07:33Z- handleDvl() - RDI_Pathfinder msg: 1613174853.47 epoch sec; seqNo:4765 2021-02-13T00:07:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.53 , 97.18 , 97.18 , 98.53 2021-02-13T00:07:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.19 , -0.73 , 0.00 2021-02-13T00:07:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174853.48 epoch sec; seqNo:9849; 4058504.33 north; 592006.75 east 2021-02-13T00:07:33Z- AHRS_M2 msg: 1613174853.79 epoch sec; seqNo:9771 2021-02-13T00:07:33Z- AHRS_M2 msg: 5.81 phi; 0.01 theta; 0.02 psi 2021-02-13T00:07:33Z- handleDepth()-Depth_Keller msg: 1613174853793 epoch sec; seqNo:9850; depth 0.07 2021-02-13T00:07:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174853.81 epoch sec; seqNo:9850; 4058504.32 north; 592005.53 east 2021-02-13T00:07:34Z- AHRS_M2 msg: 1613174854.19 epoch sec; seqNo:9772 2021-02-13T00:07:34Z- AHRS_M2 msg: 5.84 phi; 0.01 theta; 0.12 psi 2021-02-13T00:07:34Z- handleDepth()-Depth_Keller msg: 1613174854198 epoch sec; seqNo:9851; depth 0.08 2021-02-13T00:07:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174854.21 epoch sec; seqNo:9851; 4058504.32 north; 592005.53 east 2021-02-13T00:07:34Z- AHRS_M2 msg: 1613174854.60 epoch sec; seqNo:9773 2021-02-13T00:07:34Z- AHRS_M2 msg: 5.82 phi; -0.02 theta; 0.21 psi 2021-02-13T00:07:34Z- handleDepth()-Depth_Keller msg: 1613174854609 epoch sec; seqNo:9852; depth 0.08 2021-02-13T00:07:34Z- handleDvl() - RDI_Pathfinder msg: 1613174854.64 epoch sec; seqNo:4766 2021-02-13T00:07:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 102.65 , 97.24 , 99.95 , 98.60 2021-02-13T00:07:34Z- handleDvl() - RDI_Pathfinder msg: velocities -0.08 , -0.21 , 0.62 2021-02-13T00:07:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174854.65 epoch sec; seqNo:9852; 4058504.32 north; 592005.53 east 2021-02-13T00:07:35Z- AHRS_M2 msg: 1613174855.00 epoch sec; seqNo:9774 2021-02-13T00:07:35Z- AHRS_M2 msg: 5.82 phi; -0.06 theta; 0.18 psi 2021-02-13T00:07:35Z- handleDepth()-Depth_Keller msg: 1613174855006 epoch sec; seqNo:9853; depth 0.08 2021-02-13T00:07:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174855.02 epoch sec; seqNo:9853; 4058504.32 north; 592005.53 east 2021-02-13T00:07:35Z- LcmTrn::handleMessages() - handling 25 messages took 0.42 ms and 117 calls... 2021-02-13T00:07:35Z- waiting for 1613174853.06, time is 1613174855.02 2021-02-13T00:07:35Z- LcmTrn::updateTrn() >>>> pose time 1613174855.02, meas time 1613174854.64 <<<< 2021-02-13T00:07:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174854.64 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:07:35Z- TerrainNav::motionUpdate() - currEstimate = (4058504, 592005.53, 0.08) 2021-02-13T00:07:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:07:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:07:35Z- TNavBF::Projecting Measurements 2021-02-13T00:07:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:07:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:07:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:07:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:07:35Z- TNavFilter::4 good beams. 2021-02-13T00:07:35Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:07:35Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:07:35Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:07:35Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:07:35Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:07:35Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:07:35Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:07:35Z- TNavBF::mmse selected from filter -1 2021-02-13T00:07:35Z- TerrainMapOctree: (vehN, vehE ) = (4058504.32, 592005.53). 2021-02-13T00:07:35Z- TerrainMapOctree: Min Distance = 3546.32. 2021-02-13T00:07:35Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:07:35Z- There is no specific good beam that all particles have in common. 2021-02-13T00:07:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:07:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.72 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 16.86 2021-02-13T00:07:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:07:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.69 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-13T00:07:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:07:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.67 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 15.98 2021-02-13T00:07:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:07:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.64 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 15.55 2021-02-13T00:07:35Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:07:35Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:07:35Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:07:35Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:07:35Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:07:35Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:07:35Z- Filter 0 No beams used in subcloud update 2021-02-13T00:07:35Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:07:35Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:07:35Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 718.1 EastVariance: 833.0 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-13T00:07:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174854.64 sec. 2021-02-13T00:07:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:07:35Z- TNavBF::mmse selected from filter -1 2021-02-13T00:07:35Z- variances([0], [2], and psi_berg) = 718.145022 833.008467 0.000000 2021-02-13T00:07:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:07:35Z- LcmTrn::publishEstimates() - MLE : 4058501.82 591963.03 0.08 2021-02-13T00:07:35Z- LcmTrn::publishEstimates() - MMSE : 4058504.32 592005.53 0.08 2021-02-13T00:07:35Z- LcmTrn::publishEstimates() - COVAR: 718.15 833.01 0.62 0.00 2021-02-13T00:07:35Z- AHRS_M2 msg: 1613174855.42 epoch sec; seqNo:9775 2021-02-13T00:07:35Z- AHRS_M2 msg: 5.85 phi; 0.00 theta; -0.08 psi 2021-02-13T00:07:35Z- handleDepth()-Depth_Keller msg: 1613174855427 epoch sec; seqNo:9854; depth 0.05 2021-02-13T00:07:35Z- handleDvl() - RDI_Pathfinder msg: 1613174855.44 epoch sec; seqNo:4767 2021-02-13T00:07:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 111.92 , 97.04 , 98.39 , 103.81 2021-02-13T00:07:35Z- handleDvl() - RDI_Pathfinder msg: velocities -0.36 , 0.37 , 0.27 2021-02-13T00:07:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174855.45 epoch sec; seqNo:9854; 4058503.94 north; 592005.54 east 2021-02-13T00:07:35Z- AHRS_M2 msg: 1613174855.81 epoch sec; seqNo:9776 2021-02-13T00:07:35Z- AHRS_M2 msg: 5.84 phi; -0.01 theta; -0.11 psi 2021-02-13T00:07:35Z- handleDepth()-Depth_Keller msg: 1613174855817 epoch sec; seqNo:9855; depth 0.02 2021-02-13T00:07:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174855.85 epoch sec; seqNo:9855; 4058503.94 north; 592005.54 east 2021-02-13T00:07:36Z- AHRS_M2 msg: 1613174856.22 epoch sec; seqNo:9777 2021-02-13T00:07:36Z- AHRS_M2 msg: 5.80 phi; -0.05 theta; -0.09 psi 2021-02-13T00:07:36Z- handleDepth()-Depth_Keller msg: 1613174856222 epoch sec; seqNo:9856; depth 0.00 2021-02-13T00:07:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174856.23 epoch sec; seqNo:9856; 4058503.94 north; 592005.54 east 2021-02-13T00:07:36Z- AHRS_M2 msg: 1613174856.62 epoch sec; seqNo:9778 2021-02-13T00:07:36Z- AHRS_M2 msg: 5.82 phi; -0.09 theta; -0.04 psi 2021-02-13T00:07:36Z- handleDepth()-Depth_Keller msg: 1613174856622 epoch sec; seqNo:9857; depth 0.01 2021-02-13T00:07:36Z- handleDvl() - RDI_Pathfinder msg: 1613174856.63 epoch sec; seqNo:4768 2021-02-13T00:07:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 106.78 , 100.83 , 99.34 2021-02-13T00:07:36Z- handleDvl() - RDI_Pathfinder msg: velocities -0.71 , 0.35 , 0.54 2021-02-13T00:07:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174856.65 epoch sec; seqNo:9857; 4058503.95 north; 592006.76 east 2021-02-13T00:07:37Z- AHRS_M2 msg: 1613174857.02 epoch sec; seqNo:9779 2021-02-13T00:07:37Z- AHRS_M2 msg: 5.88 phi; -0.17 theta; 0.12 psi 2021-02-13T00:07:37Z- handleDepth()-Depth_Keller msg: 1613174857024 epoch sec; seqNo:9858; depth -0.00 2021-02-13T00:07:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174857.04 epoch sec; seqNo:9858; 4058503.58 north; 592006.76 east 2021-02-13T00:07:37Z- AHRS_M2 msg: 1613174857.42 epoch sec; seqNo:9780 2021-02-13T00:07:37Z- AHRS_M2 msg: 5.84 phi; -0.21 theta; 0.28 psi 2021-02-13T00:07:37Z- handleDepth()-Depth_Keller msg: 1613174857428 epoch sec; seqNo:9859; depth 0.05 2021-02-13T00:07:37Z- handleDvl() - RDI_Pathfinder msg: 1613174857.44 epoch sec; seqNo:4769 2021-02-13T00:07:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 102.45 , 97.99 , 103.94 2021-02-13T00:07:37Z- handleDvl() - RDI_Pathfinder msg: velocities -1.01 , 0.32 , -0.35 2021-02-13T00:07:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174857.47 epoch sec; seqNo:9859; 4058503.20 north; 592006.77 east 2021-02-13T00:07:37Z- AHRS_M2 msg: 1613174857.83 epoch sec; seqNo:9781 2021-02-13T00:07:37Z- AHRS_M2 msg: 5.76 phi; -0.14 theta; 0.22 psi 2021-02-13T00:07:37Z- handleDepth()-Depth_Keller msg: 1613174857835 epoch sec; seqNo:9860; depth 0.06 2021-02-13T00:07:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174857.85 epoch sec; seqNo:9860; 4058502.82 north; 592006.77 east 2021-02-13T00:07:38Z- AHRS_M2 msg: 1613174858.23 epoch sec; seqNo:9782 2021-02-13T00:07:38Z- AHRS_M2 msg: 5.76 phi; -0.03 theta; 0.06 psi 2021-02-13T00:07:38Z- handleDepth()-Depth_Keller msg: 1613174858249 epoch sec; seqNo:9861; depth 0.08 2021-02-13T00:07:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174858.26 epoch sec; seqNo:9861; 4058502.83 north; 592007.99 east 2021-02-13T00:07:38Z- AHRS_M2 msg: 1613174858.64 epoch sec; seqNo:9783 2021-02-13T00:07:38Z- AHRS_M2 msg: 5.82 phi; 0.05 theta; -0.04 psi 2021-02-13T00:07:38Z- handleDepth()-Depth_Keller msg: 1613174858640 epoch sec; seqNo:9862; depth 0.09 2021-02-13T00:07:38Z- handleDvl() - RDI_Pathfinder msg: 1613174858.66 epoch sec; seqNo:4770 2021-02-13T00:07:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 121.32 , 90.21 , 96.97 , 0.00 2021-02-13T00:07:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.29 , -0.16 , -1.09 2021-02-13T00:07:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174858.67 epoch sec; seqNo:9862; 4058502.83 north; 592007.99 east 2021-02-13T00:07:39Z- AHRS_M2 msg: 1613174859.04 epoch sec; seqNo:9784 2021-02-13T00:07:39Z- AHRS_M2 msg: 5.84 phi; 0.07 theta; -0.06 psi 2021-02-13T00:07:39Z- handleDepth()-Depth_Keller msg: 1613174859049 epoch sec; seqNo:9863; depth 0.10 2021-02-13T00:07:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174859.06 epoch sec; seqNo:9863; 4058502.83 north; 592007.99 east 2021-02-13T00:07:39Z- AHRS_M2 msg: 1613174859.44 epoch sec; seqNo:9785 2021-02-13T00:07:39Z- AHRS_M2 msg: 5.85 phi; 0.05 theta; 0.01 psi 2021-02-13T00:07:39Z- handleDepth()-Depth_Keller msg: 1613174859453 epoch sec; seqNo:9864; depth 0.13 2021-02-13T00:07:39Z- handleDvl() - RDI_Pathfinder msg: 1613174859.46 epoch sec; seqNo:4771 2021-02-13T00:07:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 100.83 , 102.32 , 97.85 2021-02-13T00:07:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.43 , -0.10 , -0.05 2021-02-13T00:07:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174859.47 epoch sec; seqNo:9864; 4058502.82 north; 592006.77 east 2021-02-13T00:07:39Z- AHRS_M2 msg: 1613174859.86 epoch sec; seqNo:9786 2021-02-13T00:07:39Z- AHRS_M2 msg: 5.89 phi; -0.01 theta; 0.04 psi 2021-02-13T00:07:39Z- handleDepth()-Depth_Keller msg: 1613174859866 epoch sec; seqNo:9865; depth 0.17 2021-02-13T00:07:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174859.88 epoch sec; seqNo:9865; 4058503.20 north; 592006.77 east 2021-02-13T00:07:40Z- AHRS_M2 msg: 1613174860.25 epoch sec; seqNo:9787 2021-02-13T00:07:40Z- AHRS_M2 msg: 5.83 phi; -0.02 theta; -0.01 psi 2021-02-13T00:07:40Z- handleDepth()-Depth_Keller msg: 1613174860256 epoch sec; seqNo:9866; depth 0.18 2021-02-13T00:07:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174860.28 epoch sec; seqNo:9866; 4058503.20 north; 592006.77 east 2021-02-13T00:07:40Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-13T00:07:40Z- waiting for 1613174858.02, time is 1613174860.28 2021-02-13T00:07:40Z- LcmTrn::updateTrn() >>>> pose time 1613174860.28, meas time 1613174859.46 <<<< 2021-02-13T00:07:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174859.46 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:07:40Z- TerrainNav::motionUpdate() - currEstimate = (4058503, 592006.77, 0.18) 2021-02-13T00:07:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:07:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:07:40Z- TNavBF::Projecting Measurements 2021-02-13T00:07:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:07:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:07:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:07:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:07:40Z- TNavFilter::4 good beams. 2021-02-13T00:07:40Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:07:40Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:07:40Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:07:40Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:07:40Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:07:40Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:07:40Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:07:40Z- TNavBF::mmse selected from filter -1 2021-02-13T00:07:40Z- TerrainMapOctree: (vehN, vehE ) = (4058503.37, 592006.58). 2021-02-13T00:07:40Z- TerrainMapOctree: Min Distance = 3546.40. 2021-02-13T00:07:40Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:07:40Z- There is no specific good beam that all particles have in common. 2021-02-13T00:07:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:07:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.69 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-13T00:07:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:07:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.66 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 16.27 2021-02-13T00:07:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:07:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.64 K: 0.034 ExpectedMeasDiff: nan currMeas.covariance: 16.75 2021-02-13T00:07:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:07:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.62 K: 0.035 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-13T00:07:40Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:07:40Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:07:40Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:07:40Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:07:40Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:07:40Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:07:40Z- Filter 0 No beams used in subcloud update 2021-02-13T00:07:40Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:07:40Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:07:40Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 718.1 EastVariance: 833.0 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-13T00:07:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174859.46 sec. 2021-02-13T00:07:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:07:40Z- TNavBF::mmse selected from filter -1 2021-02-13T00:07:40Z- variances([0], [2], and psi_berg) = 718.089046 833.008431 0.000000 2021-02-13T00:07:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:07:40Z- LcmTrn::publishEstimates() - MLE : 4058500.70 591964.27 0.18 2021-02-13T00:07:40Z- LcmTrn::publishEstimates() - MMSE : 4058503.20 592006.77 0.18 2021-02-13T00:07:40Z- LcmTrn::publishEstimates() - COVAR: 718.09 833.01 0.61 0.00 2021-02-13T00:07:40Z- AHRS_M2 msg: 1613174860.66 epoch sec; seqNo:9788 2021-02-13T00:07:40Z- AHRS_M2 msg: 5.78 phi; 0.06 theta; -0.09 psi 2021-02-13T00:07:40Z- handleDepth()-Depth_Keller msg: 1613174860665 epoch sec; seqNo:9867; depth 0.18 2021-02-13T00:07:40Z- handleDvl() - RDI_Pathfinder msg: 1613174860.68 epoch sec; seqNo:4772 2021-02-13T00:07:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 102.45 , 97.04 , 101.10 , 97.04 2021-02-13T00:07:40Z- handleDvl() - RDI_Pathfinder msg: velocities -0.05 , 0.09 , -0.37 2021-02-13T00:07:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174860.75 epoch sec; seqNo:9867; 4058503.20 north; 592006.77 east 2021-02-13T00:07:41Z- AHRS_M2 msg: 1613174861.06 epoch sec; seqNo:9789 2021-02-13T00:07:41Z- AHRS_M2 msg: 5.80 phi; 0.09 theta; -0.06 psi 2021-02-13T00:07:41Z- handleDepth()-Depth_Keller msg: 1613174861064 epoch sec; seqNo:9868; depth 0.13 2021-02-13T00:07:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174861.08 epoch sec; seqNo:9868; 4058503.20 north; 592006.77 east 2021-02-13T00:07:41Z- AHRS_M2 msg: 1613174861.46 epoch sec; seqNo:9790 2021-02-13T00:07:41Z- AHRS_M2 msg: 5.85 phi; 0.05 theta; 0.03 psi 2021-02-13T00:07:41Z- handleDepth()-Depth_Keller msg: 1613174861470 epoch sec; seqNo:9869; depth 0.12 2021-02-13T00:07:41Z- handleDvl() - RDI_Pathfinder msg: 1613174861.48 epoch sec; seqNo:4773 2021-02-13T00:07:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 105.22 , 102.52 , 97.11 2021-02-13T00:07:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.02 , -0.01 , 0.91 2021-02-13T00:07:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174861.49 epoch sec; seqNo:9869; 4058503.20 north; 592006.77 east 2021-02-13T00:07:41Z- AHRS_M2 msg: 1613174861.87 epoch sec; seqNo:9791 2021-02-13T00:07:41Z- AHRS_M2 msg: 5.81 phi; -0.01 theta; 0.08 psi 2021-02-13T00:07:41Z- handleDepth()-Depth_Keller msg: 1613174861873 epoch sec; seqNo:9870; depth 0.11 2021-02-13T00:07:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174861.89 epoch sec; seqNo:9870; 4058503.20 north; 592006.77 east 2021-02-13T00:07:42Z- AHRS_M2 msg: 1613174862.27 epoch sec; seqNo:9792 2021-02-13T00:07:42Z- AHRS_M2 msg: 5.74 phi; -0.14 theta; 0.04 psi 2021-02-13T00:07:42Z- handleDepth()-Depth_Keller msg: 1613174862276 epoch sec; seqNo:9871; depth 0.08 2021-02-13T00:07:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174862.29 epoch sec; seqNo:9871; 4058503.20 north; 592006.77 east 2021-02-13T00:07:42Z- AHRS_M2 msg: 1613174862.68 epoch sec; seqNo:9793 2021-02-13T00:07:42Z- AHRS_M2 msg: 5.77 phi; -0.20 theta; -0.06 psi 2021-02-13T00:07:42Z- handleDepth()-Depth_Keller msg: 1613174862685 epoch sec; seqNo:9872; depth 0.07 2021-02-13T00:07:42Z- handleDvl() - RDI_Pathfinder msg: 1613174862.70 epoch sec; seqNo:4774 2021-02-13T00:07:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 106.44 , 96.97 , 99.68 , 96.97 2021-02-13T00:07:42Z- handleDvl() - RDI_Pathfinder msg: velocities -0.98 , 0.16 , -0.15 2021-02-13T00:07:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174862.71 epoch sec; seqNo:9872; 4058502.82 north; 592006.77 east 2021-02-13T00:07:43Z- AHRS_M2 msg: 1613174863.08 epoch sec; seqNo:9794 2021-02-13T00:07:43Z- AHRS_M2 msg: 5.85 phi; -0.10 theta; -0.13 psi 2021-02-13T00:07:43Z- handleDepth()-Depth_Keller msg: 1613174863086 epoch sec; seqNo:9873; depth 0.03 2021-02-13T00:07:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174863.10 epoch sec; seqNo:9873; 4058502.45 north; 592008.00 east 2021-02-13T00:07:43Z- AHRS_M2 msg: 1613174863.48 epoch sec; seqNo:9795 2021-02-13T00:07:43Z- AHRS_M2 msg: 5.81 phi; -0.03 theta; -0.08 psi 2021-02-13T00:07:43Z- handleDepth()-Depth_Keller msg: 1613174863498 epoch sec; seqNo:9874; depth 0.05 2021-02-13T00:07:43Z- handleDvl() - RDI_Pathfinder msg: 1613174863.51 epoch sec; seqNo:4775 2021-02-13T00:07:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 106.51 , 97.04 , 110.57 2021-02-13T00:07:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.17 , -0.26 , -0.91 2021-02-13T00:07:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174863.53 epoch sec; seqNo:9874; 4058502.44 north; 592006.77 east 2021-02-13T00:07:43Z- AHRS_M2 msg: 1613174863.89 epoch sec; seqNo:9796 2021-02-13T00:07:43Z- AHRS_M2 msg: 5.77 phi; 0.01 theta; -0.05 psi 2021-02-13T00:07:43Z- handleDepth()-Depth_Keller msg: 1613174863892 epoch sec; seqNo:9875; depth 0.09 2021-02-13T00:07:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174863.90 epoch sec; seqNo:9875; 4058502.44 north; 592006.77 east 2021-02-13T00:07:44Z- AHRS_M2 msg: 1613174864.29 epoch sec; seqNo:9797 2021-02-13T00:07:44Z- AHRS_M2 msg: 5.81 phi; 0.05 theta; -0.03 psi 2021-02-13T00:07:44Z- handleDepth()-Depth_Keller msg: 1613174864296 epoch sec; seqNo:9876; depth 0.10 2021-02-13T00:07:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174864.31 epoch sec; seqNo:9876; 4058502.44 north; 592006.77 east 2021-02-13T00:07:44Z- AHRS_M2 msg: 1613174864.70 epoch sec; seqNo:9798 2021-02-13T00:07:44Z- AHRS_M2 msg: 5.84 phi; 0.04 theta; 0.11 psi 2021-02-13T00:07:44Z- handleDepth()-Depth_Keller msg: 1613174864702 epoch sec; seqNo:9877; depth 0.12 2021-02-13T00:07:44Z- handleDvl() - RDI_Pathfinder msg: 1613174864.71 epoch sec; seqNo:4776 2021-02-13T00:07:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 103.81 , 99.34 , 97.85 2021-02-13T00:07:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.57 , -0.83 , 0.33 2021-02-13T00:07:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174864.72 epoch sec; seqNo:9877; 4058502.44 north; 592006.77 east 2021-02-13T00:07:45Z- AHRS_M2 msg: 1613174865.10 epoch sec; seqNo:9799 2021-02-13T00:07:45Z- AHRS_M2 msg: 5.82 phi; -0.02 theta; 0.22 psi 2021-02-13T00:07:45Z- handleDepth()-Depth_Keller msg: 1613174865109 epoch sec; seqNo:9878; depth 0.14 2021-02-13T00:07:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174865.13 epoch sec; seqNo:9878; 4058502.82 north; 592006.77 east 2021-02-13T00:07:45Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-13T00:07:45Z- waiting for 1613174863.28, time is 1613174865.13 2021-02-13T00:07:45Z- LcmTrn::updateTrn() >>>> pose time 1613174865.13, meas time 1613174864.71 <<<< 2021-02-13T00:07:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174864.71 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:07:45Z- TerrainNav::motionUpdate() - currEstimate = (4058503, 592006.77, 0.14) 2021-02-13T00:07:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:07:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:07:45Z- TNavBF::Projecting Measurements 2021-02-13T00:07:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:07:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:07:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:07:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:07:45Z- TNavFilter::4 good beams. 2021-02-13T00:07:45Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:07:45Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:07:45Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:07:45Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:07:45Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:07:45Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:07:45Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:07:45Z- TNavBF::mmse selected from filter -1 2021-02-13T00:07:45Z- TerrainMapOctree: (vehN, vehE ) = (4058502.85, 592006.77). 2021-02-13T00:07:45Z- TerrainMapOctree: Min Distance = 3546.68. 2021-02-13T00:07:45Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:07:45Z- There is no specific good beam that all particles have in common. 2021-02-13T00:07:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:07:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.63 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-13T00:07:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:07:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.61 K: 0.031 ExpectedMeasDiff: nan currMeas.covariance: 17.24 2021-02-13T00:07:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:07:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.59 K: 0.032 ExpectedMeasDiff: nan currMeas.covariance: 15.79 2021-02-13T00:07:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:07:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.57 K: 0.032 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-13T00:07:45Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:07:45Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:07:45Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:07:45Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:07:45Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:07:45Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:07:45Z- Filter 0 No beams used in subcloud update 2021-02-13T00:07:45Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:07:45Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:07:45Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 718.1 EastVariance: 833.0 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-13T00:07:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174864.71 sec. 2021-02-13T00:07:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:07:45Z- TNavBF::mmse selected from filter -1 2021-02-13T00:07:45Z- variances([0], [2], and psi_berg) = 718.063676 833.008415 0.000000 2021-02-13T00:07:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:07:45Z- LcmTrn::publishEstimates() - MLE : 4058500.32 591964.27 0.14 2021-02-13T00:07:45Z- LcmTrn::publishEstimates() - MMSE : 4058502.82 592006.77 0.14 2021-02-13T00:07:45Z- LcmTrn::publishEstimates() - COVAR: 718.06 833.01 0.55 0.00 2021-02-13T00:07:45Z- AHRS_M2 msg: 1613174865.50 epoch sec; seqNo:9800 2021-02-13T00:07:45Z- AHRS_M2 msg: 5.82 phi; -0.12 theta; 0.18 psi 2021-02-13T00:07:45Z- handleDepth()-Depth_Keller msg: 1613174865513 epoch sec; seqNo:9879; depth 0.14 2021-02-13T00:07:45Z- handleDvl() - RDI_Pathfinder msg: 1613174865.53 epoch sec; seqNo:4777 2021-02-13T00:07:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 111.92 , 97.04 , 106.51 , 98.39 2021-02-13T00:07:45Z- handleDvl() - RDI_Pathfinder msg: velocities -0.24 , -0.23 , 0.80 2021-02-13T00:07:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174865.55 epoch sec; seqNo:9879; 4058502.44 north; 592006.77 east 2021-02-13T00:07:45Z- AHRS_M2 msg: 1613174865.91 epoch sec; seqNo:9801 2021-02-13T00:07:45Z- AHRS_M2 msg: 5.84 phi; -0.17 theta; 0.07 psi 2021-02-13T00:07:45Z- handleDepth()-Depth_Keller msg: 1613174865914 epoch sec; seqNo:9880; depth 0.11 2021-02-13T00:07:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174865.93 epoch sec; seqNo:9880; 4058502.43 north; 592005.55 east 2021-02-13T00:07:46Z- AHRS_M2 msg: 1613174866.31 epoch sec; seqNo:9802 2021-02-13T00:07:46Z- AHRS_M2 msg: 5.83 phi; -0.13 theta; -0.02 psi 2021-02-13T00:07:46Z- handleDepth()-Depth_Keller msg: 1613174866318 epoch sec; seqNo:9881; depth 0.10 2021-02-13T00:07:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174866.33 epoch sec; seqNo:9881; 4058502.05 north; 592005.56 east 2021-02-13T00:07:46Z- AHRS_M2 msg: 1613174866.71 epoch sec; seqNo:9803 2021-02-13T00:07:46Z- AHRS_M2 msg: 5.80 phi; -0.09 theta; -0.06 psi 2021-02-13T00:07:46Z- handleDepth()-Depth_Keller msg: 1613174866725 epoch sec; seqNo:9882; depth 0.08 2021-02-13T00:07:46Z- handleDvl() - RDI_Pathfinder msg: 1613174866.74 epoch sec; seqNo:4778 2021-02-13T00:07:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 105.29 , 97.85 , 97.85 , 114.22 2021-02-13T00:07:46Z- handleDvl() - RDI_Pathfinder msg: velocities -0.44 , 0.27 , 0.25 2021-02-13T00:07:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174866.76 epoch sec; seqNo:9882; 4058502.06 north; 592006.78 east 2021-02-13T00:07:47Z- AHRS_M2 msg: 1613174867.12 epoch sec; seqNo:9804 2021-02-13T00:07:47Z- AHRS_M2 msg: 5.78 phi; -0.16 theta; -0.09 psi 2021-02-13T00:07:47Z- handleDepth()-Depth_Keller msg: 1613174867133 epoch sec; seqNo:9883; depth 0.05 2021-02-13T00:07:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174867.16 epoch sec; seqNo:9883; 4058502.06 north; 592006.78 east 2021-02-13T00:07:47Z- AHRS_M2 msg: 1613174867.52 epoch sec; seqNo:9805 2021-02-13T00:07:47Z- AHRS_M2 msg: 5.75 phi; -0.22 theta; -0.13 psi 2021-02-13T00:07:47Z- handleDepth()-Depth_Keller msg: 1613174867528 epoch sec; seqNo:9884; depth 0.04 2021-02-13T00:07:47Z- handleDvl() - RDI_Pathfinder msg: 1613174867.54 epoch sec; seqNo:4779 2021-02-13T00:07:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.79 , 102.25 , 97.79 , 105.22 2021-02-13T00:07:47Z- handleDvl() - RDI_Pathfinder msg: velocities -0.53 , 0.38 , 0.17 2021-02-13T00:07:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174867.55 epoch sec; seqNo:9884; 4058501.68 north; 592006.78 east 2021-02-13T00:07:47Z- AHRS_M2 msg: 1613174867.93 epoch sec; seqNo:9806 2021-02-13T00:07:47Z- AHRS_M2 msg: 5.76 phi; -0.17 theta; -0.14 psi 2021-02-13T00:07:47Z- handleDepth()-Depth_Keller msg: 1613174867941 epoch sec; seqNo:9885; depth 0.05 2021-02-13T00:07:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174867.96 epoch sec; seqNo:9885; 4058501.68 north; 592006.78 east 2021-02-13T00:07:48Z- AHRS_M2 msg: 1613174868.33 epoch sec; seqNo:9807 2021-02-13T00:07:48Z- AHRS_M2 msg: 5.81 phi; -0.14 theta; -0.05 psi 2021-02-13T00:07:48Z- handleDepth()-Depth_Keller msg: 1613174868336 epoch sec; seqNo:9886; depth 0.05 2021-02-13T00:07:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174868.35 epoch sec; seqNo:9886; 4058501.68 north; 592006.78 east 2021-02-13T00:07:48Z- AHRS_M2 msg: 1613174868.74 epoch sec; seqNo:9808 2021-02-13T00:07:48Z- AHRS_M2 msg: 5.81 phi; -0.13 theta; 0.05 psi 2021-02-13T00:07:48Z- handleDepth()-Depth_Keller msg: 1613174868740 epoch sec; seqNo:9887; depth 0.06 2021-02-13T00:07:48Z- handleDvl() - RDI_Pathfinder msg: 1613174868.75 epoch sec; seqNo:4780 2021-02-13T00:07:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 109.15 , 96.97 , 111.85 2021-02-13T00:07:48Z- handleDvl() - RDI_Pathfinder msg: velocities -0.30 , -0.07 , -0.47 2021-02-13T00:07:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174868.79 epoch sec; seqNo:9887; 4058501.30 north; 592006.79 east 2021-02-13T00:07:49Z- AHRS_M2 msg: 1613174869.14 epoch sec; seqNo:9809 2021-02-13T00:07:49Z- AHRS_M2 msg: 5.82 phi; -0.09 theta; 0.11 psi 2021-02-13T00:07:49Z- handleDepth()-Depth_Keller msg: 1613174869147 epoch sec; seqNo:9888; depth 0.05 2021-02-13T00:07:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174869.16 epoch sec; seqNo:9888; 4058501.30 north; 592006.79 east 2021-02-13T00:07:49Z- AHRS_M2 msg: 1613174869.55 epoch sec; seqNo:9810 2021-02-13T00:07:49Z- AHRS_M2 msg: 5.80 phi; -0.09 theta; 0.12 psi 2021-02-13T00:07:49Z- handleDepth()-Depth_Keller msg: 1613174869550 epoch sec; seqNo:9889; depth 0.09 2021-02-13T00:07:49Z- handleDvl() - RDI_Pathfinder msg: 1613174869.56 epoch sec; seqNo:4781 2021-02-13T00:07:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 105.22 , 97.79 , 97.79 , 109.69 2021-02-13T00:07:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.01 , 0.14 , -0.40 2021-02-13T00:07:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174869.57 epoch sec; seqNo:9889; 4058501.32 north; 592008.01 east 2021-02-13T00:07:49Z- AHRS_M2 msg: 1613174869.95 epoch sec; seqNo:9811 2021-02-13T00:07:49Z- AHRS_M2 msg: 5.76 phi; -0.03 theta; 0.01 psi 2021-02-13T00:07:49Z- handleDepth()-Depth_Keller msg: 1613174869964 epoch sec; seqNo:9890; depth 0.06 2021-02-13T00:07:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174869.98 epoch sec; seqNo:9890; 4058501.32 north; 592008.01 east 2021-02-13T00:07:50Z- AHRS_M2 msg: 1613174870.38 epoch sec; seqNo:9812 2021-02-13T00:07:50Z- AHRS_M2 msg: 5.82 phi; 0.05 theta; -0.07 psi 2021-02-13T00:07:50Z- handleDepth()-Depth_Keller msg: 1613174870386 epoch sec; seqNo:9891; depth 0.03 2021-02-13T00:07:50Z- handleDvl() - RDI_Pathfinder msg: 1613174870.46 epoch sec; seqNo:4782 2021-02-13T00:07:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 100.90 , 97.92 , 97.92 , 100.90 2021-02-13T00:07:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.09 , -0.66 , -0.05 2021-02-13T00:07:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174870.47 epoch sec; seqNo:9891; 4058501.30 north; 592006.79 east 2021-02-13T00:07:50Z- LcmTrn::handleMessages() - handling 45 messages took 0.65 ms and 132 calls... 2021-02-13T00:07:50Z- waiting for 1613174868.13, time is 1613174870.47 2021-02-13T00:07:50Z- LcmTrn::updateTrn() equating pose 1613174870.47 and meas times 1613174870.47 2021-02-13T00:07:50Z- LcmTrn::updateTrn() >>>> pose time 1613174870.47, meas time 1613174870.47 <<<< 2021-02-13T00:07:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174870.47 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:07:50Z- TerrainNav::motionUpdate() - currEstimate = (4058501, 592006.79, 0.03) 2021-02-13T00:07:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:07:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:07:50Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174870.47 sec.; No relevant navigation data available 2021-02-13T00:07:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:07:50Z- TNavBF::mmse selected from filter -1 2021-02-13T00:07:50Z- variances([0], [2], and psi_berg) = 718.023058 833.008389 0.000000 2021-02-13T00:07:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:07:50Z- LcmTrn::publishEstimates() - MLE : 4058498.80 591964.29 0.03 2021-02-13T00:07:50Z- LcmTrn::publishEstimates() - MMSE : 4058501.30 592006.79 0.03 2021-02-13T00:07:50Z- LcmTrn::publishEstimates() - COVAR: 718.02 833.01 0.63 0.00 2021-02-13T00:07:50Z- AHRS_M2 msg: 1613174870.76 epoch sec; seqNo:9813 2021-02-13T00:07:50Z- AHRS_M2 msg: 5.84 phi; 0.00 theta; 0.07 psi 2021-02-13T00:07:50Z- handleDepth()-Depth_Keller msg: 1613174870760 epoch sec; seqNo:9892; depth 0.08 2021-02-13T00:07:51Z- handleDvl() - RDI_Pathfinder msg: 1613174871.42 epoch sec; seqNo:4783 2021-02-13T00:07:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 107.93 , 97.11 , 102.52 , 101.17 2021-02-13T00:07:51Z- handleDvl() - RDI_Pathfinder msg: velocities -0.27 , 0.11 , 0.27 2021-02-13T00:07:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174871.43 epoch sec; seqNo:9892; 4058501.30 north; 592006.79 east 2021-02-13T00:07:51Z- AHRS_M2 msg: 1613174871.53 epoch sec; seqNo:9814 2021-02-13T00:07:51Z- AHRS_M2 msg: 5.76 phi; -0.04 theta; 0.21 psi 2021-02-13T00:07:51Z- handleDepth()-Depth_Keller msg: 1613174871539 epoch sec; seqNo:9893; depth 0.06 2021-02-13T00:07:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174871.55 epoch sec; seqNo:9893; 4058501.30 north; 592006.79 east 2021-02-13T00:07:51Z- AHRS_M2 msg: 1613174871.93 epoch sec; seqNo:9815 2021-02-13T00:07:51Z- AHRS_M2 msg: 5.71 phi; 0.01 theta; 0.05 psi 2021-02-13T00:07:51Z- handleDepth()-Depth_Keller msg: 1613174871933 epoch sec; seqNo:9894; depth 0.06 2021-02-13T00:07:53Z- handleDvl() - RDI_Pathfinder msg: 1613174873.97 epoch sec; seqNo:4784 2021-02-13T00:07:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.87 , 97.92 , 100.90 , 99.41 2021-02-13T00:07:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.19 , -0.26 , 0.44 2021-02-13T00:07:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174874.00 epoch sec; seqNo:9894; 4058501.30 north; 592006.79 east 2021-02-13T00:07:54Z- AHRS_M2 msg: 1613174874.14 epoch sec; seqNo:9816 2021-02-13T00:07:54Z- AHRS_M2 msg: 5.75 phi; 0.02 theta; 0.08 psi 2021-02-13T00:07:54Z- handleDepth()-Depth_Keller msg: 1613174874150 epoch sec; seqNo:9895; depth 0.09 2021-02-13T00:07:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174874.17 epoch sec; seqNo:9895; 4058501.30 north; 592006.79 east 2021-02-13T00:07:54Z- AHRS_M2 msg: 1613174874.51 epoch sec; seqNo:9817 2021-02-13T00:07:54Z- AHRS_M2 msg: 5.71 phi; 0.03 theta; 0.14 psi 2021-02-13T00:07:54Z- handleDepth()-Depth_Keller msg: 1613174874517 epoch sec; seqNo:9896; depth 0.09 2021-02-13T00:07:54Z- handleDvl() - RDI_Pathfinder msg: 1613174874.54 epoch sec; seqNo:4785 2021-02-13T00:07:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 107.87 , 97.05 , 102.46 , 99.75 2021-02-13T00:07:54Z- handleDvl() - RDI_Pathfinder msg: velocities -0.07 , 0.20 , 0.35 2021-02-13T00:07:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174874.57 epoch sec; seqNo:9896; 4058501.30 north; 592006.79 east 2021-02-13T00:07:54Z- AHRS_M2 msg: 1613174874.91 epoch sec; seqNo:9818 2021-02-13T00:07:54Z- AHRS_M2 msg: 5.71 phi; -0.05 theta; 0.01 psi 2021-02-13T00:07:54Z- handleDepth()-Depth_Keller msg: 1613174874925 epoch sec; seqNo:9897; depth 0.09 2021-02-13T00:07:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174874.94 epoch sec; seqNo:9897; 4058501.30 north; 592006.79 east 2021-02-13T00:07:55Z- AHRS_M2 msg: 1613174875.32 epoch sec; seqNo:9819 2021-02-13T00:07:55Z- AHRS_M2 msg: 5.69 phi; -0.10 theta; -0.16 psi 2021-02-13T00:07:55Z- handleDepth()-Depth_Keller msg: 1613174875321 epoch sec; seqNo:9898; depth 0.07 2021-02-13T00:07:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174875.37 epoch sec; seqNo:9898; 4058501.30 north; 592006.79 east 2021-02-13T00:07:55Z- LcmTrn::handleMessages() - handling 24 messages took 0.45 ms and 115 calls... 2021-02-13T00:07:55Z- waiting for 1613174873.47, time is 1613174875.37 2021-02-13T00:07:55Z- LcmTrn::updateTrn() >>>> pose time 1613174875.37, meas time 1613174874.54 <<<< 2021-02-13T00:07:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174874.54 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:07:55Z- TerrainNav::motionUpdate() - currEstimate = (4058501, 592006.79, 0.07) 2021-02-13T00:07:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:07:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:07:55Z- TNavBF::Projecting Measurements 2021-02-13T00:07:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:07:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:07:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:07:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:07:55Z- TNavFilter::4 good beams. 2021-02-13T00:07:55Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:07:55Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:07:55Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:07:55Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:07:55Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:07:55Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:07:55Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:07:55Z- TNavBF::mmse selected from filter -1 2021-02-13T00:07:55Z- TerrainMapOctree: (vehN, vehE ) = (4058501.30, 592006.79). 2021-02-13T00:07:55Z- TerrainMapOctree: Min Distance = 3547.87. 2021-02-13T00:07:55Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:07:55Z- There is no specific good beam that all particles have in common. 2021-02-13T00:07:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:07:55Z- Old Depth Bias: 0.00 DepthBiasCov: 0.63 K: 0.030 ExpectedMeasDiff: nan currMeas.covariance: 18.62 2021-02-13T00:07:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:07:55Z- Old Depth Bias: 0.00 DepthBiasCov: 0.61 K: 0.035 ExpectedMeasDiff: nan currMeas.covariance: 15.07 2021-02-13T00:07:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:07:55Z- Old Depth Bias: 0.00 DepthBiasCov: 0.59 K: 0.031 ExpectedMeasDiff: nan currMeas.covariance: 16.80 2021-02-13T00:07:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:07:55Z- Old Depth Bias: 0.00 DepthBiasCov: 0.57 K: 0.031 ExpectedMeasDiff: nan currMeas.covariance: 15.92 2021-02-13T00:07:55Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:07:55Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:07:55Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:07:55Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:07:55Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:07:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:07:55Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:07:55Z- Filter 0 No beams used in subcloud update 2021-02-13T00:07:55Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:07:55Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:07:55Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 718.0 EastVariance: 833.0 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-13T00:07:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174874.54 sec. 2021-02-13T00:07:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:07:55Z- TNavBF::mmse selected from filter -1 2021-02-13T00:07:55Z- variances([0], [2], and psi_berg) = 718.023058 833.008389 0.000000 2021-02-13T00:07:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:07:55Z- LcmTrn::publishEstimates() - MLE : 4058498.80 591964.29 0.07 2021-02-13T00:07:55Z- LcmTrn::publishEstimates() - MMSE : 4058501.30 592006.79 0.07 2021-02-13T00:07:55Z- LcmTrn::publishEstimates() - COVAR: 718.02 833.01 0.55 0.00 2021-02-13T00:07:55Z- AHRS_M2 msg: 1613174875.72 epoch sec; seqNo:9820 2021-02-13T00:07:55Z- AHRS_M2 msg: 5.66 phi; -0.07 theta; -0.24 psi 2021-02-13T00:07:55Z- handleDepth()-Depth_Keller msg: 1613174875725 epoch sec; seqNo:9899; depth 0.08 2021-02-13T00:07:55Z- handleDvl() - RDI_Pathfinder msg: 1613174875.74 epoch sec; seqNo:4786 2021-02-13T00:07:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.95 , 97.99 , 99.48 , 97.99 2021-02-13T00:07:55Z- handleDvl() - RDI_Pathfinder msg: velocities -0.64 , 0.40 , -0.67 2021-02-13T00:07:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174875.78 epoch sec; seqNo:9899; 4058500.92 north; 592006.79 east 2021-02-13T00:07:56Z- AHRS_M2 msg: 1613174876.13 epoch sec; seqNo:9821 2021-02-13T00:07:56Z- AHRS_M2 msg: 5.66 phi; -0.03 theta; -0.20 psi 2021-02-13T00:07:56Z- handleDepth()-Depth_Keller msg: 1613174876133 epoch sec; seqNo:9900; depth 0.05 2021-02-13T00:07:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174876.15 epoch sec; seqNo:9900; 4058500.92 north; 592006.79 east 2021-02-13T00:07:56Z- AHRS_M2 msg: 1613174876.53 epoch sec; seqNo:9822 2021-02-13T00:07:56Z- AHRS_M2 msg: 5.65 phi; 0.04 theta; -0.02 psi 2021-02-13T00:07:56Z- handleDepth()-Depth_Keller msg: 1613174876539 epoch sec; seqNo:9901; depth 0.06 2021-02-13T00:07:56Z- handleDvl() - RDI_Pathfinder msg: 1613174876.56 epoch sec; seqNo:4787 2021-02-13T00:07:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.05 , 115.99 , 97.05 , 106.52 2021-02-13T00:07:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.22 , -0.69 , -0.39 2021-02-13T00:07:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174876.58 epoch sec; seqNo:9901; 4058500.92 north; 592006.79 east 2021-02-13T00:07:56Z- AHRS_M2 msg: 1613174876.93 epoch sec; seqNo:9823 2021-02-13T00:07:56Z- AHRS_M2 msg: 5.71 phi; 0.09 theta; 0.18 psi 2021-02-13T00:07:56Z- handleDepth()-Depth_Keller msg: 1613174876941 epoch sec; seqNo:9902; depth 0.05 2021-02-13T00:07:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174876.97 epoch sec; seqNo:9902; 4058500.92 north; 592006.79 east 2021-02-13T00:07:57Z- AHRS_M2 msg: 1613174877.34 epoch sec; seqNo:9824 2021-02-13T00:07:57Z- AHRS_M2 msg: 5.72 phi; 0.03 theta; 0.30 psi 2021-02-13T00:07:57Z- handleDepth()-Depth_Keller msg: 1613174877350 epoch sec; seqNo:9903; depth 0.07 2021-02-13T00:07:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174877.40 epoch sec; seqNo:9903; 4058500.54 north; 592006.79 east 2021-02-13T00:07:57Z- AHRS_M2 msg: 1613174877.77 epoch sec; seqNo:9825 2021-02-13T00:07:57Z- AHRS_M2 msg: 5.69 phi; -0.09 theta; 0.25 psi 2021-02-13T00:07:57Z- handleDepth()-Depth_Keller msg: 1613174877778 epoch sec; seqNo:9904; depth 0.05 2021-02-13T00:07:57Z- handleDvl() - RDI_Pathfinder msg: 1613174877.90 epoch sec; seqNo:4788 2021-02-13T00:07:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.74 , 97.79 , 105.23 , 97.79 2021-02-13T00:07:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.11 , -0.47 , 0.45 2021-02-13T00:07:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174877.96 epoch sec; seqNo:9904; 4058500.53 north; 592005.57 east 2021-02-13T00:07:58Z- AHRS_M2 msg: 1613174878.15 epoch sec; seqNo:9826 2021-02-13T00:07:58Z- AHRS_M2 msg: 5.66 phi; -0.11 theta; 0.10 psi 2021-02-13T00:07:58Z- handleDepth()-Depth_Keller msg: 1613174878151 epoch sec; seqNo:9905; depth 0.02 2021-02-13T00:07:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174878.16 epoch sec; seqNo:9905; 4058500.53 north; 592005.57 east 2021-02-13T00:07:58Z- AHRS_M2 msg: 1613174878.54 epoch sec; seqNo:9827 2021-02-13T00:07:58Z- AHRS_M2 msg: 5.60 phi; -0.04 theta; -0.05 psi 2021-02-13T00:07:58Z- handleDepth()-Depth_Keller msg: 1613174878548 epoch sec; seqNo:9906; depth 0.05 2021-02-13T00:07:58Z- handleDvl() - RDI_Pathfinder msg: 1613174878.56 epoch sec; seqNo:4789 2021-02-13T00:07:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 118.62 , 96.97 , 98.33 , 106.44 2021-02-13T00:07:58Z- handleDvl() - RDI_Pathfinder msg: velocities -0.32 , -0.18 , -0.28 2021-02-13T00:07:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174878.57 epoch sec; seqNo:9906; 4058500.15 north; 592005.58 east 2021-02-13T00:07:58Z- AHRS_M2 msg: 1613174878.95 epoch sec; seqNo:9828 2021-02-13T00:07:58Z- AHRS_M2 msg: 5.61 phi; 0.02 theta; -0.11 psi 2021-02-13T00:07:58Z- handleDepth()-Depth_Keller msg: 1613174878961 epoch sec; seqNo:9907; depth 0.03 2021-02-13T00:07:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174878.97 epoch sec; seqNo:9907; 4058500.15 north; 592005.58 east 2021-02-13T00:07:59Z- AHRS_M2 msg: 1613174879.35 epoch sec; seqNo:9829 2021-02-13T00:07:59Z- AHRS_M2 msg: 5.64 phi; 0.02 theta; -0.00 psi 2021-02-13T00:07:59Z- handleDepth()-Depth_Keller msg: 1613174879361 epoch sec; seqNo:9908; depth 0.05 2021-02-13T00:07:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174879.38 epoch sec; seqNo:9908; 4058500.15 north; 592005.58 east 2021-02-13T00:07:59Z- AHRS_M2 msg: 1613174879.76 epoch sec; seqNo:9830 2021-02-13T00:07:59Z- AHRS_M2 msg: 5.59 phi; 0.05 theta; 0.14 psi 2021-02-13T00:07:59Z- handleDepth()-Depth_Keller msg: 1613174879765 epoch sec; seqNo:9909; depth 0.10 2021-02-13T00:07:59Z- handleDvl() - RDI_Pathfinder msg: 1613174879.78 epoch sec; seqNo:4790 2021-02-13T00:07:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.65 , 105.09 , 97.65 , 99.14 2021-02-13T00:07:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.21 , 0.09 , 0.62 2021-02-13T00:07:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174879.80 epoch sec; seqNo:9909; 4058500.15 north; 592005.58 east 2021-02-13T00:08:00Z- AHRS_M2 msg: 1613174880.16 epoch sec; seqNo:9831 2021-02-13T00:08:00Z- AHRS_M2 msg: 5.59 phi; 0.04 theta; 0.09 psi 2021-02-13T00:08:00Z- handleDepth()-Depth_Keller msg: 1613174880169 epoch sec; seqNo:9910; depth 0.06 2021-02-13T00:08:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174880.18 epoch sec; seqNo:9910; 4058500.15 north; 592005.58 east 2021-02-13T00:08:00Z- AHRS_M2 msg: 1613174880.57 epoch sec; seqNo:9832 2021-02-13T00:08:00Z- AHRS_M2 msg: 5.61 phi; -0.07 theta; -0.06 psi 2021-02-13T00:08:00Z- handleDepth()-Depth_Keller msg: 1613174880589 epoch sec; seqNo:9911; depth -0.02 2021-02-13T00:08:00Z- handleDvl() - RDI_Pathfinder msg: 1613174880.62 epoch sec; seqNo:4791 2021-02-13T00:08:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 109.22 , 97.05 , 105.16 , 97.05 2021-02-13T00:08:00Z- handleDvl() - RDI_Pathfinder msg: velocities -0.30 , 0.11 , 0.06 2021-02-13T00:08:00Z- LcmTrn::handleMessages() - handling 44 messages took 0.80 ms and 128 calls... 2021-02-13T00:08:00Z- waiting for 1613174878.37, time is 1613174880.62 2021-02-13T00:08:00Z- LcmTrn::updateTrn() equating pose 1613174880.62 and meas times 1613174880.62 2021-02-13T00:08:00Z- LcmTrn::updateTrn() >>>> pose time 1613174880.62, meas time 1613174880.62 <<<< 2021-02-13T00:08:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174880.62 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:08:00Z- TerrainNav::motionUpdate() - currEstimate = (4058500, 592005.58, -0.02) 2021-02-13T00:08:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:08:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:08:00Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174880.62 sec.; No relevant navigation data available 2021-02-13T00:08:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:08:00Z- TNavBF::mmse selected from filter -1 2021-02-13T00:08:00Z- variances([0], [2], and psi_berg) = 717.980465 833.008362 0.000000 2021-02-13T00:08:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:08:00Z- LcmTrn::publishEstimates() - MLE : 4058497.65 591963.08 -0.02 2021-02-13T00:08:00Z- LcmTrn::publishEstimates() - MMSE : 4058500.15 592005.58 -0.02 2021-02-13T00:08:00Z- LcmTrn::publishEstimates() - COVAR: 717.98 833.01 0.64 0.00 2021-02-13T00:08:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174880.64 epoch sec; seqNo:9911; 4058500.15 north; 592005.58 east 2021-02-13T00:08:00Z- AHRS_M2 msg: 1613174880.97 epoch sec; seqNo:9833 2021-02-13T00:08:00Z- AHRS_M2 msg: 5.56 phi; -0.16 theta; -0.08 psi 2021-02-13T00:08:00Z- handleDepth()-Depth_Keller msg: 1613174880977 epoch sec; seqNo:9912; depth 0.03 2021-02-13T00:08:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:00Z- handleDvl() - RDI_Pathfinder msg: 1613174880.98 epoch sec; seqNo:4792 2021-02-13T00:08:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:00Z- handleDvl() - RDI_Pathfinder msg: velocities -0.30 , 0.11 , 0.06 2021-02-13T00:08:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174881.01 epoch sec; seqNo:9912; 4058500.15 north; 592005.58 east 2021-02-13T00:08:01Z- AHRS_M2 msg: 1613174881.38 epoch sec; seqNo:9834 2021-02-13T00:08:01Z- AHRS_M2 msg: 5.50 phi; -0.10 theta; -0.05 psi 2021-02-13T00:08:01Z- handleDepth()-Depth_Keller msg: 1613174881382 epoch sec; seqNo:9913; depth 0.05 2021-02-13T00:08:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174881.42 epoch sec; seqNo:9913; 4058500.17 north; 592006.80 east 2021-02-13T00:08:01Z- AHRS_M2 msg: 1613174881.78 epoch sec; seqNo:9835 2021-02-13T00:08:01Z- AHRS_M2 msg: 5.48 phi; -0.01 theta; -0.01 psi 2021-02-13T00:08:01Z- handleDepth()-Depth_Keller msg: 1613174881782 epoch sec; seqNo:9914; depth 0.05 2021-02-13T00:08:01Z- handleDvl() - RDI_Pathfinder msg: 1613174881.79 epoch sec; seqNo:4793 2021-02-13T00:08:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.93 , 103.88 , 97.93 , 111.32 2021-02-13T00:08:01Z- handleDvl() - RDI_Pathfinder msg: velocities -0.06 , -0.19 , -0.68 2021-02-13T00:08:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174881.80 epoch sec; seqNo:9914; 4058499.79 north; 592006.80 east 2021-02-13T00:08:02Z- AHRS_M2 msg: 1613174882.18 epoch sec; seqNo:9836 2021-02-13T00:08:02Z- AHRS_M2 msg: 5.49 phi; 0.03 theta; 0.05 psi 2021-02-13T00:08:02Z- handleDepth()-Depth_Keller msg: 1613174882185 epoch sec; seqNo:9915; depth 0.09 2021-02-13T00:08:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:02Z- handleDvl() - RDI_Pathfinder msg: 1613174882.19 epoch sec; seqNo:4794 2021-02-13T00:08:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:02Z- handleDvl() - RDI_Pathfinder msg: velocities -0.06 , -0.19 , -0.68 2021-02-13T00:08:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174882.21 epoch sec; seqNo:9915; 4058499.79 north; 592006.80 east 2021-02-13T00:08:02Z- AHRS_M2 msg: 1613174882.58 epoch sec; seqNo:9837 2021-02-13T00:08:02Z- AHRS_M2 msg: 5.46 phi; 0.03 theta; 0.06 psi 2021-02-13T00:08:02Z- handleDepth()-Depth_Keller msg: 1613174882589 epoch sec; seqNo:9916; depth 0.10 2021-02-13T00:08:02Z- handleDvl() - RDI_Pathfinder msg: 1613174882.62 epoch sec; seqNo:4795 2021-02-13T00:08:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 102.32 , 97.85 , 99.34 , 97.85 2021-02-13T00:08:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.27 , -0.26 , 0.41 2021-02-13T00:08:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174882.64 epoch sec; seqNo:9916; 4058499.77 north; 592005.58 east 2021-02-13T00:08:02Z- AHRS_M2 msg: 1613174882.99 epoch sec; seqNo:9838 2021-02-13T00:08:02Z- AHRS_M2 msg: 5.40 phi; -0.07 theta; -0.02 psi 2021-02-13T00:08:02Z- handleDepth()-Depth_Keller msg: 1613174882997 epoch sec; seqNo:9917; depth 0.13 2021-02-13T00:08:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174883.02 epoch sec; seqNo:9917; 4058499.77 north; 592005.58 east 2021-02-13T00:08:03Z- handleDepth()-Depth_Keller msg: 1613174883409 epoch sec; seqNo:9918; depth 0.12 2021-02-13T00:08:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:03Z- handleDvl() - RDI_Pathfinder msg: 1613174883.41 epoch sec; seqNo:4796 2021-02-13T00:08:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.27 , -0.26 , 0.41 2021-02-13T00:08:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174883.46 epoch sec; seqNo:9918; 4058499.77 north; 592005.58 east 2021-02-13T00:08:03Z- AHRS_M2 msg: 1613174883.80 epoch sec; seqNo:9839 2021-02-13T00:08:03Z- AHRS_M2 msg: 5.40 phi; -0.21 theta; -0.23 psi 2021-02-13T00:08:03Z- handleDepth()-Depth_Keller msg: 1613174883801 epoch sec; seqNo:9919; depth 0.11 2021-02-13T00:08:03Z- handleDvl() - RDI_Pathfinder msg: 1613174883.83 epoch sec; seqNo:4797 2021-02-13T00:08:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.81 , 103.87 , 97.11 , 99.81 2021-02-13T00:08:03Z- handleDvl() - RDI_Pathfinder msg: velocities -0.71 , -0.53 , -0.09 2021-02-13T00:08:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174883.85 epoch sec; seqNo:9919; 4058499.39 north; 592005.59 east 2021-02-13T00:08:04Z- AHRS_M2 msg: 1613174884.20 epoch sec; seqNo:9840 2021-02-13T00:08:04Z- AHRS_M2 msg: 5.43 phi; -0.25 theta; -0.11 psi 2021-02-13T00:08:04Z- handleDepth()-Depth_Keller msg: 1613174884209 epoch sec; seqNo:9920; depth 0.13 2021-02-13T00:08:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174884.25 epoch sec; seqNo:9920; 4058499.02 north; 592005.59 east 2021-02-13T00:08:04Z- AHRS_M2 msg: 1613174884.60 epoch sec; seqNo:9841 2021-02-13T00:08:04Z- AHRS_M2 msg: 5.49 phi; -0.23 theta; 0.05 psi 2021-02-13T00:08:04Z- handleDepth()-Depth_Keller msg: 1613174884613 epoch sec; seqNo:9921; depth 0.17 2021-02-13T00:08:04Z- handleDvl() - RDI_Pathfinder msg: 1613174884.66 epoch sec; seqNo:4798 2021-02-13T00:08:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.05 , 111.93 , 97.05 , 0.00 2021-02-13T00:08:04Z- handleDvl() - RDI_Pathfinder msg: velocities -0.23 , -0.38 , -0.31 2021-02-13T00:08:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174884.67 epoch sec; seqNo:9921; 4058499.02 north; 592005.59 east 2021-02-13T00:08:05Z- AHRS_M2 msg: 1613174885.01 epoch sec; seqNo:9842 2021-02-13T00:08:05Z- AHRS_M2 msg: 5.51 phi; -0.20 theta; 0.19 psi 2021-02-13T00:08:05Z- handleDepth()-Depth_Keller msg: 1613174885022 epoch sec; seqNo:9922; depth 0.17 2021-02-13T00:08:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174885.04 epoch sec; seqNo:9922; 4058499.02 north; 592005.59 east 2021-02-13T00:08:05Z- AHRS_M2 msg: 1613174885.43 epoch sec; seqNo:9843 2021-02-13T00:08:05Z- AHRS_M2 msg: 5.39 phi; -0.29 theta; -0.06 psi 2021-02-13T00:08:05Z- handleDepth()-Depth_Keller msg: 1613174885441 epoch sec; seqNo:9923; depth 0.16 2021-02-13T00:08:05Z- handleDvl() - RDI_Pathfinder msg: 1613174885.47 epoch sec; seqNo:4799 2021-02-13T00:08:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 117.27 , 97.93 , 97.93 , 118.76 2021-02-13T00:08:05Z- handleDvl() - RDI_Pathfinder msg: velocities -0.23 , 0.22 , 0.47 2021-02-13T00:08:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174885.50 epoch sec; seqNo:9923; 4058499.02 north; 592005.59 east 2021-02-13T00:08:05Z- LcmTrn::handleMessages() - handling 44 messages took 0.82 ms and 128 calls... 2021-02-13T00:08:05Z- waiting for 1613174883.62, time is 1613174885.50 2021-02-13T00:08:05Z- LcmTrn::updateTrn() equating pose 1613174885.50 and meas times 1613174885.50 2021-02-13T00:08:05Z- LcmTrn::updateTrn() >>>> pose time 1613174885.50, meas time 1613174885.50 <<<< 2021-02-13T00:08:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174885.50 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:08:05Z- TerrainNav::motionUpdate() - currEstimate = (4058499, 592005.59, 0.16) 2021-02-13T00:08:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:08:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:08:05Z- TNavBF::Projecting Measurements 2021-02-13T00:08:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:08:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:08:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:08:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:08:05Z- TNavFilter::4 good beams. 2021-02-13T00:08:05Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:08:05Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:08:05Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:08:05Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:08:05Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:08:05Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:08:05Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:08:05Z- TNavBF::mmse selected from filter -1 2021-02-13T00:08:05Z- TerrainMapOctree: (vehN, vehE ) = (4058499.02, 592005.59). 2021-02-13T00:08:05Z- TerrainMapOctree: Min Distance = 3550.39. 2021-02-13T00:08:05Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:08:05Z- There is no specific good beam that all particles have in common. 2021-02-13T00:08:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:08:05Z- Old Depth Bias: 0.00 DepthBiasCov: 0.70 K: 0.028 ExpectedMeasDiff: nan currMeas.covariance: 22.00 2021-02-13T00:08:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:08:05Z- Old Depth Bias: 0.00 DepthBiasCov: 0.68 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:08:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:08:05Z- Old Depth Bias: 0.00 DepthBiasCov: 0.65 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:08:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:08:05Z- Old Depth Bias: 0.00 DepthBiasCov: 0.63 K: 0.025 ExpectedMeasDiff: nan currMeas.covariance: 22.57 2021-02-13T00:08:05Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:08:05Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:08:05Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:08:05Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:08:05Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:08:05Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:08:05Z- Filter 0 No beams used in subcloud update 2021-02-13T00:08:05Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:08:05Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:08:05Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 717.9 EastVariance: 833.0 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-13T00:08:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174885.50 sec. 2021-02-13T00:08:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:08:05Z- TNavBF::mmse selected from filter -1 2021-02-13T00:08:05Z- variances([0], [2], and psi_berg) = 717.945322 833.008339 0.000000 2021-02-13T00:08:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:08:05Z- LcmTrn::publishEstimates() - MLE : 4058496.52 592049.09 0.16 2021-02-13T00:08:05Z- LcmTrn::publishEstimates() - MMSE : 4058499.02 592005.59 0.16 2021-02-13T00:08:05Z- LcmTrn::publishEstimates() - COVAR: 717.95 833.01 0.61 0.00 2021-02-13T00:08:05Z- AHRS_M2 msg: 1613174885.82 epoch sec; seqNo:9844 2021-02-13T00:08:05Z- AHRS_M2 msg: 5.41 phi; -0.35 theta; -0.30 psi 2021-02-13T00:08:05Z- handleDepth()-Depth_Keller msg: 1613174885825 epoch sec; seqNo:9924; depth 0.13 2021-02-13T00:08:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:05Z- handleDvl() - RDI_Pathfinder msg: 1613174885.83 epoch sec; seqNo:4800 2021-02-13T00:08:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:05Z- handleDvl() - RDI_Pathfinder msg: velocities -0.23 , 0.22 , 0.47 2021-02-13T00:08:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174885.86 epoch sec; seqNo:9924; 4058499.02 north; 592005.59 east 2021-02-13T00:08:06Z- AHRS_M2 msg: 1613174886.22 epoch sec; seqNo:9845 2021-02-13T00:08:06Z- AHRS_M2 msg: 5.50 phi; -0.35 theta; -0.37 psi 2021-02-13T00:08:06Z- handleDepth()-Depth_Keller msg: 1613174886229 epoch sec; seqNo:9925; depth 0.20 2021-02-13T00:08:06Z- handleDvl() - RDI_Pathfinder msg: 1613174886.25 epoch sec; seqNo:4801 2021-02-13T00:08:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:06Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:08:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174886.29 epoch sec; seqNo:9925; 4058499.03 north; 592006.81 east 2021-02-13T00:08:06Z- AHRS_M2 msg: 1613174886.62 epoch sec; seqNo:9846 2021-02-13T00:08:06Z- AHRS_M2 msg: 5.58 phi; -0.32 theta; -0.24 psi 2021-02-13T00:08:06Z- handleDepth()-Depth_Keller msg: 1613174886637 epoch sec; seqNo:9926; depth 0.24 2021-02-13T00:08:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174886.66 epoch sec; seqNo:9926; 4058499.03 north; 592006.81 east 2021-02-13T00:08:07Z- AHRS_M2 msg: 1613174887.03 epoch sec; seqNo:9847 2021-02-13T00:08:07Z- AHRS_M2 msg: 5.62 phi; -0.22 theta; -0.02 psi 2021-02-13T00:08:07Z- handleDepth()-Depth_Keller msg: 1613174887033 epoch sec; seqNo:9927; depth 0.28 2021-02-13T00:08:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:07Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:07Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:07Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:07Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:07Z- handleDvl() - RDI_Pathfinder msg: 1613174887.04 epoch sec; seqNo:4802 2021-02-13T00:08:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:07Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:08:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174887.06 epoch sec; seqNo:9927; 4058498.65 north; 592006.82 east 2021-02-13T00:08:07Z- AHRS_M2 msg: 1613174887.43 epoch sec; seqNo:9848 2021-02-13T00:08:07Z- AHRS_M2 msg: 5.65 phi; -0.07 theta; 0.12 psi 2021-02-13T00:08:07Z- handleDepth()-Depth_Keller msg: 1613174887441 epoch sec; seqNo:9928; depth 0.31 2021-02-13T00:08:07Z- handleDvl() - RDI_Pathfinder msg: 1613174887.45 epoch sec; seqNo:4803 2021-02-13T00:08:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:07Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:08:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174887.46 epoch sec; seqNo:9928; 4058498.65 north; 592006.82 east 2021-02-13T00:08:07Z- AHRS_M2 msg: 1613174887.84 epoch sec; seqNo:9849 2021-02-13T00:08:07Z- AHRS_M2 msg: 5.68 phi; 0.02 theta; 0.15 psi 2021-02-13T00:08:07Z- handleDepth()-Depth_Keller msg: 1613174887853 epoch sec; seqNo:9929; depth 0.33 2021-02-13T00:08:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174887.86 epoch sec; seqNo:9929; 4058498.65 north; 592006.82 east 2021-02-13T00:08:08Z- AHRS_M2 msg: 1613174888.24 epoch sec; seqNo:9850 2021-02-13T00:08:08Z- AHRS_M2 msg: 5.69 phi; 0.02 theta; 0.12 psi 2021-02-13T00:08:08Z- handleDepth()-Depth_Keller msg: 1613174888245 epoch sec; seqNo:9930; depth 0.29 2021-02-13T00:08:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:08Z- handleDvl() - RDI_Pathfinder msg: 1613174888.25 epoch sec; seqNo:4804 2021-02-13T00:08:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:08Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:08:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174888.27 epoch sec; seqNo:9930; 4058498.65 north; 592006.82 east 2021-02-13T00:08:08Z- AHRS_M2 msg: 1613174888.64 epoch sec; seqNo:9851 2021-02-13T00:08:08Z- AHRS_M2 msg: 5.70 phi; -0.03 theta; 0.01 psi 2021-02-13T00:08:08Z- handleDepth()-Depth_Keller msg: 1613174888653 epoch sec; seqNo:9931; depth 0.25 2021-02-13T00:08:08Z- handleDvl() - RDI_Pathfinder msg: 1613174888.66 epoch sec; seqNo:4805 2021-02-13T00:08:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 109.36 , 97.45 , 100.43 , 103.40 2021-02-13T00:08:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.21 , 0.75 2021-02-13T00:08:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174888.69 epoch sec; seqNo:9931; 4058499.03 north; 592006.81 east 2021-02-13T00:08:09Z- AHRS_M2 msg: 1613174889.05 epoch sec; seqNo:9852 2021-02-13T00:08:09Z- AHRS_M2 msg: 5.70 phi; -0.07 theta; -0.12 psi 2021-02-13T00:08:09Z- handleDepth()-Depth_Keller msg: 1613174889057 epoch sec; seqNo:9932; depth 0.21 2021-02-13T00:08:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174889.07 epoch sec; seqNo:9932; 4058499.03 north; 592006.81 east 2021-02-13T00:08:09Z- AHRS_M2 msg: 1613174889.48 epoch sec; seqNo:9853 2021-02-13T00:08:09Z- AHRS_M2 msg: 5.69 phi; -0.07 theta; -0.18 psi 2021-02-13T00:08:09Z- handleDepth()-Depth_Keller msg: 1613174889486 epoch sec; seqNo:9933; depth 0.20 2021-02-13T00:08:09Z- handleDvl() - RDI_Pathfinder msg: 1613174889.51 epoch sec; seqNo:4806 2021-02-13T00:08:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.93 , 99.41 , 97.93 , 102.39 2021-02-13T00:08:09Z- handleDvl() - RDI_Pathfinder msg: velocities -0.43 , -0.22 , 0.42 2021-02-13T00:08:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174889.54 epoch sec; seqNo:9933; 4058499.03 north; 592006.81 east 2021-02-13T00:08:09Z- AHRS_M2 msg: 1613174889.86 epoch sec; seqNo:9854 2021-02-13T00:08:09Z- AHRS_M2 msg: 5.71 phi; -0.09 theta; -0.14 psi 2021-02-13T00:08:09Z- handleDepth()-Depth_Keller msg: 1613174889865 epoch sec; seqNo:9934; depth 0.16 2021-02-13T00:08:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174889.89 epoch sec; seqNo:9934; 4058498.65 north; 592006.82 east 2021-02-13T00:08:10Z- AHRS_M2 msg: 1613174890.26 epoch sec; seqNo:9855 2021-02-13T00:08:10Z- AHRS_M2 msg: 5.76 phi; -0.19 theta; -0.02 psi 2021-02-13T00:08:10Z- handleDepth()-Depth_Keller msg: 1613174890273 epoch sec; seqNo:9935; depth 0.12 2021-02-13T00:08:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174890.37 epoch sec; seqNo:9935; 4058498.27 north; 592006.82 east 2021-02-13T00:08:10Z- AHRS_M2 msg: 1613174890.67 epoch sec; seqNo:9856 2021-02-13T00:08:10Z- AHRS_M2 msg: 5.79 phi; -0.29 theta; 0.14 psi 2021-02-13T00:08:10Z- handleDepth()-Depth_Keller msg: 1613174890677 epoch sec; seqNo:9936; depth 0.13 2021-02-13T00:08:10Z- handleDvl() - RDI_Pathfinder msg: 1613174890.69 epoch sec; seqNo:4807 2021-02-13T00:08:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.06 , 106.58 , 97.11 , 106.58 2021-02-13T00:08:10Z- handleDvl() - RDI_Pathfinder msg: velocities -1.04 , -0.24 , 0.28 2021-02-13T00:08:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174890.70 epoch sec; seqNo:9936; 4058498.27 north; 592006.82 east 2021-02-13T00:08:10Z- LcmTrn::handleMessages() - handling 47 messages took 0.81 ms and 131 calls... 2021-02-13T00:08:10Z- waiting for 1613174888.50, time is 1613174890.70 2021-02-13T00:08:10Z- LcmTrn::updateTrn() equating pose 1613174890.70 and meas times 1613174890.70 2021-02-13T00:08:10Z- LcmTrn::updateTrn() >>>> pose time 1613174890.70, meas time 1613174890.70 <<<< 2021-02-13T00:08:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174890.70 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:08:10Z- TerrainNav::motionUpdate() - currEstimate = (4058498, 592006.82, 0.13) 2021-02-13T00:08:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:08:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:08:10Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174890.70 sec.; No relevant navigation data available 2021-02-13T00:08:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:08:10Z- TNavBF::mmse selected from filter -1 2021-02-13T00:08:10Z- variances([0], [2], and psi_berg) = 717.905825 833.008314 0.000000 2021-02-13T00:08:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:08:10Z- LcmTrn::publishEstimates() - MLE : 4058495.77 592050.32 0.13 2021-02-13T00:08:10Z- LcmTrn::publishEstimates() - MMSE : 4058498.27 592006.82 0.13 2021-02-13T00:08:10Z- LcmTrn::publishEstimates() - COVAR: 717.91 833.01 0.68 0.00 2021-02-13T00:08:11Z- AHRS_M2 msg: 1613174891.07 epoch sec; seqNo:9857 2021-02-13T00:08:11Z- AHRS_M2 msg: 5.76 phi; -0.31 theta; 0.21 psi 2021-02-13T00:08:11Z- handleDepth()-Depth_Keller msg: 1613174891073 epoch sec; seqNo:9937; depth 0.15 2021-02-13T00:08:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174891.09 epoch sec; seqNo:9937; 4058497.51 north; 592006.83 east 2021-02-13T00:08:11Z- AHRS_M2 msg: 1613174891.50 epoch sec; seqNo:9858 2021-02-13T00:08:11Z- AHRS_M2 msg: 5.71 phi; -0.25 theta; 0.13 psi 2021-02-13T00:08:11Z- handleDepth()-Depth_Keller msg: 1613174891506 epoch sec; seqNo:9938; depth 0.10 2021-02-13T00:08:11Z- handleDvl() - RDI_Pathfinder msg: 1613174891.52 epoch sec; seqNo:4808 2021-02-13T00:08:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 117.27 , 96.98 , 96.98 , 128.10 2021-02-13T00:08:11Z- handleDvl() - RDI_Pathfinder msg: velocities -0.35 , 0.46 , -0.84 2021-02-13T00:08:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174891.53 epoch sec; seqNo:9938; 4058497.89 north; 592006.82 east 2021-02-13T00:08:11Z- AHRS_M2 msg: 1613174891.88 epoch sec; seqNo:9859 2021-02-13T00:08:11Z- AHRS_M2 msg: 5.64 phi; -0.17 theta; -0.04 psi 2021-02-13T00:08:11Z- handleDepth()-Depth_Keller msg: 1613174891883 epoch sec; seqNo:9939; depth 0.06 2021-02-13T00:08:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:11Z- handleDvl() - RDI_Pathfinder msg: 1613174891.89 epoch sec; seqNo:4809 2021-02-13T00:08:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:11Z- handleDvl() - RDI_Pathfinder msg: velocities -0.35 , 0.46 , -0.84 2021-02-13T00:08:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174891.90 epoch sec; seqNo:9939; 4058497.89 north; 592006.82 east 2021-02-13T00:08:12Z- AHRS_M2 msg: 1613174892.28 epoch sec; seqNo:9860 2021-02-13T00:08:12Z- AHRS_M2 msg: 5.60 phi; -0.06 theta; -0.21 psi 2021-02-13T00:08:12Z- handleDepth()-Depth_Keller msg: 1613174892289 epoch sec; seqNo:9940; depth 0.04 2021-02-13T00:08:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174892.32 epoch sec; seqNo:9940; 4058497.91 north; 592008.04 east 2021-02-13T00:08:12Z- AHRS_M2 msg: 1613174892.68 epoch sec; seqNo:9861 2021-02-13T00:08:12Z- AHRS_M2 msg: 5.68 phi; 0.11 theta; -0.16 psi 2021-02-13T00:08:12Z- handleDepth()-Depth_Keller msg: 1613174892701 epoch sec; seqNo:9941; depth 0.05 2021-02-13T00:08:12Z- handleDvl() - RDI_Pathfinder msg: 1613174892.73 epoch sec; seqNo:4810 2021-02-13T00:08:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.25 , 97.25 , 97.25 , 0.00 2021-02-13T00:08:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.54 , -0.33 2021-02-13T00:08:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174892.74 epoch sec; seqNo:9941; 4058491.87 north; 592010.55 east 2021-02-13T00:08:13Z- AHRS_M2 msg: 1613174893.09 epoch sec; seqNo:9862 2021-02-13T00:08:13Z- AHRS_M2 msg: 5.74 phi; 0.14 theta; 0.11 psi 2021-02-13T00:08:13Z- handleDepth()-Depth_Keller msg: 1613174893097 epoch sec; seqNo:9942; depth 0.06 2021-02-13T00:08:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:13Z- handleDvl() - RDI_Pathfinder msg: 1613174893.12 epoch sec; seqNo:4811 2021-02-13T00:08:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.54 , -0.33 2021-02-13T00:08:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174893.14 epoch sec; seqNo:9942; 4058491.87 north; 592010.55 east 2021-02-13T00:08:13Z- AHRS_M2 msg: 1613174893.49 epoch sec; seqNo:9863 2021-02-13T00:08:13Z- AHRS_M2 msg: 5.71 phi; 0.08 theta; 0.24 psi 2021-02-13T00:08:13Z- handleDepth()-Depth_Keller msg: 1613174893497 epoch sec; seqNo:9943; depth 0.09 2021-02-13T00:08:13Z- handleDvl() - RDI_Pathfinder msg: 1613174893.52 epoch sec; seqNo:4812 2021-02-13T00:08:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.25 , 104.01 , 0.00 , 97.25 2021-02-13T00:08:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.23 , 0.25 , 0.95 2021-02-13T00:08:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174893.56 epoch sec; seqNo:9943; 4058491.87 north; 592010.55 east 2021-02-13T00:08:13Z- AHRS_M2 msg: 1613174893.90 epoch sec; seqNo:9864 2021-02-13T00:08:13Z- AHRS_M2 msg: 5.68 phi; 0.02 theta; 0.19 psi 2021-02-13T00:08:13Z- handleDepth()-Depth_Keller msg: 1613174893909 epoch sec; seqNo:9944; depth 0.05 2021-02-13T00:08:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174893.96 epoch sec; seqNo:9944; 4058492.25 north; 592010.55 east 2021-02-13T00:08:14Z- AHRS_M2 msg: 1613174894.30 epoch sec; seqNo:9865 2021-02-13T00:08:14Z- AHRS_M2 msg: 5.62 phi; -0.11 theta; 0.08 psi 2021-02-13T00:08:14Z- handleDepth()-Depth_Keller msg: 1613174894313 epoch sec; seqNo:9945; depth 0.01 2021-02-13T00:08:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:14Z- handleDvl() - RDI_Pathfinder msg: 1613174894.32 epoch sec; seqNo:4813 2021-02-13T00:08:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.23 , 0.25 , 0.95 2021-02-13T00:08:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174894.34 epoch sec; seqNo:9945; 4058492.25 north; 592010.55 east 2021-02-13T00:08:14Z- AHRS_M2 msg: 1613174894.70 epoch sec; seqNo:9866 2021-02-13T00:08:14Z- AHRS_M2 msg: 5.56 phi; -0.18 theta; -0.02 psi 2021-02-13T00:08:14Z- handleDepth()-Depth_Keller msg: 1613174894713 epoch sec; seqNo:9946; depth 0.04 2021-02-13T00:08:14Z- handleDvl() - RDI_Pathfinder msg: 1613174894.73 epoch sec; seqNo:4814 2021-02-13T00:08:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 114.70 , 97.11 , 102.53 , 97.11 2021-02-13T00:08:14Z- handleDvl() - RDI_Pathfinder msg: velocities -0.64 , 0.06 , 0.13 2021-02-13T00:08:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174894.76 epoch sec; seqNo:9946; 4058491.87 north; 592010.55 east 2021-02-13T00:08:15Z- AHRS_M2 msg: 1613174895.11 epoch sec; seqNo:9867 2021-02-13T00:08:15Z- AHRS_M2 msg: 5.56 phi; -0.11 theta; -0.07 psi 2021-02-13T00:08:15Z- handleDepth()-Depth_Keller msg: 1613174895117 epoch sec; seqNo:9947; depth 0.04 2021-02-13T00:08:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174895.14 epoch sec; seqNo:9947; 4058491.87 north; 592010.55 east 2021-02-13T00:08:15Z- AHRS_M2 msg: 1613174895.51 epoch sec; seqNo:9868 2021-02-13T00:08:15Z- AHRS_M2 msg: 5.55 phi; -0.09 theta; -0.07 psi 2021-02-13T00:08:15Z- handleDepth()-Depth_Keller msg: 1613174895521 epoch sec; seqNo:9948; depth 0.03 2021-02-13T00:08:15Z- handleDvl() - RDI_Pathfinder msg: 1613174895.55 epoch sec; seqNo:4815 2021-02-13T00:08:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.86 , 103.81 , 97.86 , 108.28 2021-02-13T00:08:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.19 , -0.25 , -0.08 2021-02-13T00:08:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174895.58 epoch sec; seqNo:9948; 4058491.87 north; 592010.55 east 2021-02-13T00:08:15Z- LcmTrn::handleMessages() - handling 44 messages took 0.87 ms and 127 calls... 2021-02-13T00:08:15Z- waiting for 1613174893.70, time is 1613174895.58 2021-02-13T00:08:15Z- LcmTrn::updateTrn() equating pose 1613174895.58 and meas times 1613174895.58 2021-02-13T00:08:15Z- LcmTrn::updateTrn() >>>> pose time 1613174895.58, meas time 1613174895.58 <<<< 2021-02-13T00:08:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174895.58 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:08:15Z- TerrainNav::motionUpdate() - currEstimate = (4058492, 592010.55, 0.03) 2021-02-13T00:08:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:08:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:08:15Z- TNavBF::Projecting Measurements 2021-02-13T00:08:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:08:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:08:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:08:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:08:15Z- TNavFilter::4 good beams. 2021-02-13T00:08:15Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:08:15Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:08:15Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:08:15Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:08:15Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:08:15Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:08:15Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:08:15Z- TNavBF::mmse selected from filter -1 2021-02-13T00:08:15Z- TerrainMapOctree: (vehN, vehE ) = (4058491.87, 592010.55). 2021-02-13T00:08:15Z- TerrainMapOctree: Min Distance = 3552.80. 2021-02-13T00:08:15Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:08:15Z- There is no specific good beam that all particles have in common. 2021-02-13T00:08:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:08:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-13T00:08:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:08:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 17.24 2021-02-13T00:08:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:08:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-13T00:08:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:08:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 18.76 2021-02-13T00:08:15Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:08:15Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:08:15Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:08:15Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:08:15Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:08:15Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:08:15Z- Filter 0 No beams used in subcloud update 2021-02-13T00:08:15Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:08:15Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:08:15Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 717.8 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:08:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174895.58 sec. 2021-02-13T00:08:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:08:15Z- TNavBF::mmse selected from filter -1 2021-02-13T00:08:15Z- variances([0], [2], and psi_berg) = 717.816170 833.008257 0.000000 2021-02-13T00:08:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:08:15Z- LcmTrn::publishEstimates() - MLE : 4058490.37 591962.05 0.03 2021-02-13T00:08:15Z- LcmTrn::publishEstimates() - MMSE : 4058491.87 592010.55 0.03 2021-02-13T00:08:15Z- LcmTrn::publishEstimates() - COVAR: 717.82 833.01 0.86 0.00 2021-02-13T00:08:15Z- AHRS_M2 msg: 1613174895.93 epoch sec; seqNo:9869 2021-02-13T00:08:15Z- AHRS_M2 msg: 5.52 phi; -0.12 theta; -0.01 psi 2021-02-13T00:08:15Z- handleDepth()-Depth_Keller msg: 1613174895941 epoch sec; seqNo:9949; depth 0.09 2021-02-13T00:08:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174895.97 epoch sec; seqNo:9949; 4058491.87 north; 592010.55 east 2021-02-13T00:08:16Z- AHRS_M2 msg: 1613174896.32 epoch sec; seqNo:9870 2021-02-13T00:08:16Z- AHRS_M2 msg: 5.51 phi; -0.11 theta; 0.01 psi 2021-02-13T00:08:16Z- handleDepth()-Depth_Keller msg: 1613174896325 epoch sec; seqNo:9950; depth 0.12 2021-02-13T00:08:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174896.35 epoch sec; seqNo:9950; 4058491.87 north; 592010.55 east 2021-02-13T00:08:16Z- AHRS_M2 msg: 1613174896.72 epoch sec; seqNo:9871 2021-02-13T00:08:16Z- AHRS_M2 msg: 5.54 phi; -0.12 theta; -0.01 psi 2021-02-13T00:08:16Z- handleDepth()-Depth_Keller msg: 1613174896728 epoch sec; seqNo:9951; depth 0.14 2021-02-13T00:08:16Z- handleDvl() - RDI_Pathfinder msg: 1613174896.74 epoch sec; seqNo:4816 2021-02-13T00:08:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.41 , 99.41 , 97.93 , 105.37 2021-02-13T00:08:16Z- handleDvl() - RDI_Pathfinder msg: velocities -0.08 , -0.27 , 0.09 2021-02-13T00:08:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174896.76 epoch sec; seqNo:9951; 4058491.87 north; 592010.55 east 2021-02-13T00:08:17Z- AHRS_M2 msg: 1613174897.13 epoch sec; seqNo:9872 2021-02-13T00:08:17Z- AHRS_M2 msg: 5.56 phi; -0.17 theta; 0.00 psi 2021-02-13T00:08:17Z- handleDepth()-Depth_Keller msg: 1613174897145 epoch sec; seqNo:9952; depth 0.13 2021-02-13T00:08:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174897.18 epoch sec; seqNo:9952; 4058491.49 north; 592010.55 east 2021-02-13T00:08:17Z- AHRS_M2 msg: 1613174897.53 epoch sec; seqNo:9873 2021-02-13T00:08:17Z- AHRS_M2 msg: 5.54 phi; -0.22 theta; 0.02 psi 2021-02-13T00:08:17Z- handleDepth()-Depth_Keller msg: 1613174897545 epoch sec; seqNo:9953; depth 0.17 2021-02-13T00:08:17Z- handleDvl() - RDI_Pathfinder msg: 1613174897.57 epoch sec; seqNo:4817 2021-02-13T00:08:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.04 , 98.33 , 96.98 , 106.45 2021-02-13T00:08:17Z- handleDvl() - RDI_Pathfinder msg: velocities -0.50 , 0.24 , 0.26 2021-02-13T00:08:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174897.60 epoch sec; seqNo:9953; 4058491.49 north; 592010.55 east 2021-02-13T00:08:17Z- AHRS_M2 msg: 1613174897.94 epoch sec; seqNo:9874 2021-02-13T00:08:17Z- AHRS_M2 msg: 5.51 phi; -0.27 theta; -0.03 psi 2021-02-13T00:08:17Z- handleDepth()-Depth_Keller msg: 1613174897961 epoch sec; seqNo:9954; depth 0.18 2021-02-13T00:08:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:17Z- handleDvl() - RDI_Pathfinder msg: 1613174897.96 epoch sec; seqNo:4818 2021-02-13T00:08:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:17Z- handleDvl() - RDI_Pathfinder msg: velocities -0.50 , 0.24 , 0.26 2021-02-13T00:08:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174897.99 epoch sec; seqNo:9954; 4058491.49 north; 592010.55 east 2021-02-13T00:08:18Z- AHRS_M2 msg: 1613174898.34 epoch sec; seqNo:9875 2021-02-13T00:08:18Z- AHRS_M2 msg: 5.49 phi; -0.31 theta; -0.14 psi 2021-02-13T00:08:18Z- handleDepth()-Depth_Keller msg: 1613174898353 epoch sec; seqNo:9955; depth 0.17 2021-02-13T00:08:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174898.41 epoch sec; seqNo:9955; 4058491.49 north; 592010.55 east 2021-02-13T00:08:18Z- AHRS_M2 msg: 1613174898.74 epoch sec; seqNo:9876 2021-02-13T00:08:18Z- AHRS_M2 msg: 5.50 phi; -0.31 theta; -0.23 psi 2021-02-13T00:08:18Z- handleDepth()-Depth_Keller msg: 1613174898765 epoch sec; seqNo:9956; depth 0.17 2021-02-13T00:08:18Z- handleDvl() - RDI_Pathfinder msg: 1613174898.78 epoch sec; seqNo:4819 2021-02-13T00:08:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:18Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:08:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174898.80 epoch sec; seqNo:9956; 4058491.13 north; 592011.78 east 2021-02-13T00:08:19Z- AHRS_M2 msg: 1613174899.15 epoch sec; seqNo:9877 2021-02-13T00:08:19Z- AHRS_M2 msg: 5.53 phi; -0.29 theta; -0.25 psi 2021-02-13T00:08:19Z- handleDepth()-Depth_Keller msg: 1613174899156 epoch sec; seqNo:9957; depth 0.18 2021-02-13T00:08:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:19Z- handleDvl() - RDI_Pathfinder msg: 1613174899.16 epoch sec; seqNo:4820 2021-02-13T00:08:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:19Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:08:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174899.18 epoch sec; seqNo:9957; 4058491.13 north; 592011.78 east 2021-02-13T00:08:19Z- AHRS_M2 msg: 1613174899.56 epoch sec; seqNo:9878 2021-02-13T00:08:19Z- AHRS_M2 msg: 5.54 phi; -0.29 theta; -0.17 psi 2021-02-13T00:08:19Z- handleDepth()-Depth_Keller msg: 1613174899566 epoch sec; seqNo:9958; depth 0.19 2021-02-13T00:08:19Z- handleDvl() - RDI_Pathfinder msg: 1613174899.58 epoch sec; seqNo:4821 2021-02-13T00:08:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.19 , 124.44 , 97.65 , 125.93 2021-02-13T00:08:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.15 , -0.32 , -0.54 2021-02-13T00:08:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174899.60 epoch sec; seqNo:9958; 4058491.13 north; 592011.78 east 2021-02-13T00:08:19Z- AHRS_M2 msg: 1613174899.96 epoch sec; seqNo:9879 2021-02-13T00:08:19Z- AHRS_M2 msg: 5.55 phi; -0.34 theta; -0.07 psi 2021-02-13T00:08:19Z- handleDepth()-Depth_Keller msg: 1613174899964 epoch sec; seqNo:9959; depth 0.20 2021-02-13T00:08:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174899.99 epoch sec; seqNo:9959; 4058491.11 north; 592010.56 east 2021-02-13T00:08:20Z- AHRS_M2 msg: 1613174900.36 epoch sec; seqNo:9880 2021-02-13T00:08:20Z- AHRS_M2 msg: 5.57 phi; -0.37 theta; 0.04 psi 2021-02-13T00:08:20Z- handleDepth()-Depth_Keller msg: 1613174900368 epoch sec; seqNo:9960; depth 0.23 2021-02-13T00:08:20Z- handleDvl() - RDI_Pathfinder msg: 1613174900.39 epoch sec; seqNo:4822 2021-02-13T00:08:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:20Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:08:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174900.41 epoch sec; seqNo:9960; 4058491.11 north; 592010.56 east 2021-02-13T00:08:20Z- AHRS_M2 msg: 1613174900.78 epoch sec; seqNo:9881 2021-02-13T00:08:20Z- AHRS_M2 msg: 5.55 phi; -0.36 theta; 0.10 psi 2021-02-13T00:08:20Z- handleDepth()-Depth_Keller msg: 1613174900781 epoch sec; seqNo:9961; depth 0.25 2021-02-13T00:08:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174900.80 epoch sec; seqNo:9961; 4058491.11 north; 592010.56 east 2021-02-13T00:08:20Z- LcmTrn::handleMessages() - handling 46 messages took 0.96 ms and 127 calls... 2021-02-13T00:08:20Z- waiting for 1613174898.58, time is 1613174900.80 2021-02-13T00:08:20Z- LcmTrn::updateTrn() >>>> pose time 1613174900.80, meas time 1613174900.39 <<<< 2021-02-13T00:08:20Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174900.39, ping # 0. 2021-02-13T00:08:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174900.39 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:08:20Z- TerrainNav::motionUpdate() - currEstimate = (4058491, 592010.56, 0.25) 2021-02-13T00:08:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:08:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:08:20Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:08:20Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:08:20Z- TNavBF::Projecting Measurements 2021-02-13T00:08:20Z- TNavFilter::Measurement from time = 1613174900.39 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:08:20Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:08:20Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:08:20Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:08:20Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:08:20Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:08:20Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:08:20Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:08:20Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 717.8 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:08:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:08:20Z- TNavBF::mmse selected from filter -1 2021-02-13T00:08:20Z- variances([0], [2], and psi_berg) = 717.780675 833.008234 0.000000 2021-02-13T00:08:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:08:20Z- LcmTrn::publishEstimates() - MLE : 4058489.61 591962.06 0.25 2021-02-13T00:08:20Z- LcmTrn::publishEstimates() - MMSE : 4058491.11 592010.56 0.25 2021-02-13T00:08:20Z- LcmTrn::publishEstimates() - COVAR: 717.78 833.01 0.90 0.00 2021-02-13T00:08:21Z- AHRS_M2 msg: 1613174901.18 epoch sec; seqNo:9882 2021-02-13T00:08:21Z- AHRS_M2 msg: 5.54 phi; -0.36 theta; 0.05 psi 2021-02-13T00:08:21Z- handleDepth()-Depth_Keller msg: 1613174901195 epoch sec; seqNo:9962; depth 0.24 2021-02-13T00:08:21Z- handleDvl() - RDI_Pathfinder msg: 1613174901.24 epoch sec; seqNo:4823 2021-02-13T00:08:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:21Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:08:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174901.25 epoch sec; seqNo:9962; 4058491.11 north; 592010.56 east 2021-02-13T00:08:21Z- AHRS_M2 msg: 1613174901.58 epoch sec; seqNo:9883 2021-02-13T00:08:21Z- AHRS_M2 msg: 5.55 phi; -0.38 theta; -0.08 psi 2021-02-13T00:08:21Z- handleDepth()-Depth_Keller msg: 1613174901583 epoch sec; seqNo:9963; depth 0.22 2021-02-13T00:08:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:21Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:21Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:21Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:21Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:21Z- handleDvl() - RDI_Pathfinder msg: 1613174901.59 epoch sec; seqNo:4824 2021-02-13T00:08:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:21Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:08:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174901.62 epoch sec; seqNo:9963; 4058491.11 north; 592010.56 east 2021-02-13T00:08:21Z- AHRS_M2 msg: 1613174901.98 epoch sec; seqNo:9884 2021-02-13T00:08:21Z- AHRS_M2 msg: 5.56 phi; -0.41 theta; -0.15 psi 2021-02-13T00:08:21Z- handleDepth()-Depth_Keller msg: 1613174901984 epoch sec; seqNo:9964; depth 0.24 2021-02-13T00:08:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174902.00 epoch sec; seqNo:9964; 4058491.11 north; 592010.56 east 2021-02-13T00:08:22Z- AHRS_M2 msg: 1613174902.39 epoch sec; seqNo:9885 2021-02-13T00:08:22Z- AHRS_M2 msg: 5.47 phi; -0.50 theta; -0.11 psi 2021-02-13T00:08:22Z- handleDepth()-Depth_Keller msg: 1613174902396 epoch sec; seqNo:9965; depth 0.28 2021-02-13T00:08:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174902.41 epoch sec; seqNo:9965; 4058491.10 north; 592009.34 east 2021-02-13T00:08:22Z- AHRS_M2 msg: 1613174902.79 epoch sec; seqNo:9886 2021-02-13T00:08:22Z- AHRS_M2 msg: 5.47 phi; -0.48 theta; -0.15 psi 2021-02-13T00:08:22Z- handleDepth()-Depth_Keller msg: 1613174902797 epoch sec; seqNo:9966; depth 0.33 2021-02-13T00:08:22Z- handleDvl() - RDI_Pathfinder msg: 1613174902.83 epoch sec; seqNo:4825 2021-02-13T00:08:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 100.22 , 116.60 , 0.00 , 140.41 2021-02-13T00:08:22Z- handleDvl() - RDI_Pathfinder msg: velocities -0.21 , -0.07 , -0.05 2021-02-13T00:08:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174902.84 epoch sec; seqNo:9966; 4058491.11 north; 592010.56 east 2021-02-13T00:08:23Z- AHRS_M2 msg: 1613174903.19 epoch sec; seqNo:9887 2021-02-13T00:08:23Z- AHRS_M2 msg: 5.48 phi; -0.42 theta; -0.16 psi 2021-02-13T00:08:23Z- handleDepth()-Depth_Keller msg: 1613174903196 epoch sec; seqNo:9967; depth 0.34 2021-02-13T00:08:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174903.21 epoch sec; seqNo:9967; 4058491.11 north; 592010.56 east 2021-02-13T00:08:23Z- AHRS_M2 msg: 1613174903.60 epoch sec; seqNo:9888 2021-02-13T00:08:23Z- AHRS_M2 msg: 5.50 phi; -0.33 theta; -0.14 psi 2021-02-13T00:08:23Z- handleDepth()-Depth_Keller msg: 1613174903611 epoch sec; seqNo:9968; depth 0.36 2021-02-13T00:08:23Z- handleDvl() - RDI_Pathfinder msg: 1613174903.63 epoch sec; seqNo:4826 2021-02-13T00:08:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:23Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:08:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174903.65 epoch sec; seqNo:9968; 4058491.11 north; 592010.56 east 2021-02-13T00:08:24Z- AHRS_M2 msg: 1613174904.00 epoch sec; seqNo:9889 2021-02-13T00:08:24Z- AHRS_M2 msg: 5.50 phi; -0.31 theta; -0.11 psi 2021-02-13T00:08:24Z- handleDepth()-Depth_Keller msg: 1613174904005 epoch sec; seqNo:9969; depth 0.34 2021-02-13T00:08:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:24Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:24Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:24Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:24Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:24Z- handleDvl() - RDI_Pathfinder msg: 1613174904.01 epoch sec; seqNo:4827 2021-02-13T00:08:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:24Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:08:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174904.04 epoch sec; seqNo:9969; 4058490.73 north; 592010.56 east 2021-02-13T00:08:24Z- AHRS_M2 msg: 1613174904.40 epoch sec; seqNo:9890 2021-02-13T00:08:24Z- AHRS_M2 msg: 5.50 phi; -0.36 theta; -0.09 psi 2021-02-13T00:08:24Z- handleDepth()-Depth_Keller msg: 1613174904409 epoch sec; seqNo:9970; depth 0.35 2021-02-13T00:08:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174904.42 epoch sec; seqNo:9970; 4058490.73 north; 592010.56 east 2021-02-13T00:08:24Z- AHRS_M2 msg: 1613174904.81 epoch sec; seqNo:9891 2021-02-13T00:08:24Z- AHRS_M2 msg: 5.50 phi; -0.42 theta; -0.09 psi 2021-02-13T00:08:24Z- handleDepth()-Depth_Keller msg: 1613174904812 epoch sec; seqNo:9971; depth 0.37 2021-02-13T00:08:24Z- handleDvl() - RDI_Pathfinder msg: 1613174904.84 epoch sec; seqNo:4828 2021-02-13T00:08:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.32 , 110.71 , 86.90 , 125.59 2021-02-13T00:08:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.62 , -0.12 , 0.42 2021-02-13T00:08:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174904.85 epoch sec; seqNo:9971; 4058491.10 north; 592009.34 east 2021-02-13T00:08:25Z- AHRS_M2 msg: 1613174905.21 epoch sec; seqNo:9892 2021-02-13T00:08:25Z- AHRS_M2 msg: 5.49 phi; -0.48 theta; -0.07 psi 2021-02-13T00:08:25Z- handleDepth()-Depth_Keller msg: 1613174905216 epoch sec; seqNo:9972; depth 0.38 2021-02-13T00:08:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:25Z- handleDvl() - RDI_Pathfinder msg: 1613174905.22 epoch sec; seqNo:4829 2021-02-13T00:08:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.62 , -0.12 , 0.42 2021-02-13T00:08:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174905.23 epoch sec; seqNo:9972; 4058491.10 north; 592009.34 east 2021-02-13T00:08:25Z- AHRS_M2 msg: 1613174905.62 epoch sec; seqNo:9893 2021-02-13T00:08:25Z- AHRS_M2 msg: 5.48 phi; -0.52 theta; -0.09 psi 2021-02-13T00:08:25Z- handleDepth()-Depth_Keller msg: 1613174905622 epoch sec; seqNo:9973; depth 0.44 2021-02-13T00:08:25Z- handleDvl() - RDI_Pathfinder msg: 1613174905.64 epoch sec; seqNo:4830 2021-02-13T00:08:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.61 , 114.03 , 85.75 , 0.00 2021-02-13T00:08:25Z- handleDvl() - RDI_Pathfinder msg: velocities -0.38 , 0.08 , 0.33 2021-02-13T00:08:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174905.66 epoch sec; seqNo:9973; 4058490.73 north; 592010.56 east 2021-02-13T00:08:25Z- LcmTrn::handleMessages() - handling 44 messages took 0.75 ms and 129 calls... 2021-02-13T00:08:25Z- waiting for 1613174903.80, time is 1613174905.66 2021-02-13T00:08:25Z- LcmTrn::updateTrn() equating pose 1613174905.66 and meas times 1613174905.66 2021-02-13T00:08:25Z- LcmTrn::updateTrn() >>>> pose time 1613174905.66, meas time 1613174905.66 <<<< 2021-02-13T00:08:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174905.66 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:08:25Z- TerrainNav::motionUpdate() - currEstimate = (4058491, 592010.56, 0.44) 2021-02-13T00:08:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:08:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:08:25Z- TNavBF::Projecting Measurements 2021-02-13T00:08:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:08:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:08:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:08:25Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:08:25Z- TNavFilter::3 good beams. 2021-02-13T00:08:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:08:25Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:08:25Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:08:25Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:08:25Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:08:25Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:08:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:08:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:08:25Z- TerrainMapOctree: (vehN, vehE ) = (4058490.73, 592010.56). 2021-02-13T00:08:25Z- TerrainMapOctree: Min Distance = 3553.68. 2021-02-13T00:08:25Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:08:25Z- There is no specific good beam that all particles have in common. 2021-02-13T00:08:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:08:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 17.18 2021-02-13T00:08:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:08:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 20.80 2021-02-13T00:08:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:08:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 11.76 2021-02-13T00:08:25Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:08:25Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:08:25Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:08:25Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:08:25Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:08:25Z- Filter 0 No beams used in subcloud update 2021-02-13T00:08:25Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:08:25Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:08:25Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 717.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:08:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174905.66 sec. 2021-02-13T00:08:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:08:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:08:25Z- variances([0], [2], and psi_berg) = 717.760432 833.008222 0.000000 2021-02-13T00:08:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:08:25Z- LcmTrn::publishEstimates() - MLE : 4058489.23 591962.06 0.44 2021-02-13T00:08:25Z- LcmTrn::publishEstimates() - MMSE : 4058490.73 592010.56 0.44 2021-02-13T00:08:25Z- LcmTrn::publishEstimates() - COVAR: 717.76 833.01 0.79 0.00 2021-02-13T00:08:26Z- AHRS_M2 msg: 1613174906.02 epoch sec; seqNo:9894 2021-02-13T00:08:26Z- AHRS_M2 msg: 5.50 phi; -0.55 theta; -0.18 psi 2021-02-13T00:08:26Z- handleDepth()-Depth_Keller msg: 1613174906030 epoch sec; seqNo:9974; depth 0.44 2021-02-13T00:08:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174906.05 epoch sec; seqNo:9974; 4058490.73 north; 592010.56 east 2021-02-13T00:08:26Z- AHRS_M2 msg: 1613174906.42 epoch sec; seqNo:9895 2021-02-13T00:08:26Z- AHRS_M2 msg: 5.51 phi; -0.57 theta; -0.18 psi 2021-02-13T00:08:26Z- handleDepth()-Depth_Keller msg: 1613174906429 epoch sec; seqNo:9975; depth 0.48 2021-02-13T00:08:26Z- handleDvl() - RDI_Pathfinder msg: 1613174906.45 epoch sec; seqNo:4831 2021-02-13T00:08:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:26Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:08:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174906.47 epoch sec; seqNo:9975; 4058490.73 north; 592010.56 east 2021-02-13T00:08:26Z- AHRS_M2 msg: 1613174906.83 epoch sec; seqNo:9896 2021-02-13T00:08:26Z- AHRS_M2 msg: 5.55 phi; -0.50 theta; -0.17 psi 2021-02-13T00:08:26Z- handleDepth()-Depth_Keller msg: 1613174906832 epoch sec; seqNo:9976; depth 0.53 2021-02-13T00:08:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174906.85 epoch sec; seqNo:9976; 4058490.73 north; 592010.56 east 2021-02-13T00:08:27Z- AHRS_M2 msg: 1613174907.24 epoch sec; seqNo:9897 2021-02-13T00:08:27Z- AHRS_M2 msg: 5.56 phi; -0.37 theta; -0.14 psi 2021-02-13T00:08:27Z- handleDepth()-Depth_Keller msg: 1613174907254 epoch sec; seqNo:9977; depth 0.56 2021-02-13T00:08:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174907.31 epoch sec; seqNo:9977; 4058490.35 north; 592010.57 east 2021-02-13T00:08:27Z- AHRS_M2 msg: 1613174907.64 epoch sec; seqNo:9898 2021-02-13T00:08:27Z- AHRS_M2 msg: 5.54 phi; -0.34 theta; -0.08 psi 2021-02-13T00:08:27Z- handleDepth()-Depth_Keller msg: 1613174907640 epoch sec; seqNo:9978; depth 0.56 2021-02-13T00:08:27Z- handleDvl() - RDI_Pathfinder msg: 1613174907.67 epoch sec; seqNo:4832 2021-02-13T00:08:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:27Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:08:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174907.68 epoch sec; seqNo:9978; 4058490.35 north; 592010.57 east 2021-02-13T00:08:28Z- AHRS_M2 msg: 1613174908.04 epoch sec; seqNo:9899 2021-02-13T00:08:28Z- AHRS_M2 msg: 5.53 phi; -0.38 theta; -0.11 psi 2021-02-13T00:08:28Z- handleDepth()-Depth_Keller msg: 1613174908045 epoch sec; seqNo:9979; depth 0.58 2021-02-13T00:08:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174908.06 epoch sec; seqNo:9979; 4058490.35 north; 592010.57 east 2021-02-13T00:08:28Z- AHRS_M2 msg: 1613174908.45 epoch sec; seqNo:9900 2021-02-13T00:08:28Z- AHRS_M2 msg: 5.57 phi; -0.37 theta; -0.16 psi 2021-02-13T00:08:28Z- handleDepth()-Depth_Keller msg: 1613174908450 epoch sec; seqNo:9980; depth 0.62 2021-02-13T00:08:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174908.46 epoch sec; seqNo:9980; 4058489.98 north; 592010.57 east 2021-02-13T00:08:28Z- AHRS_M2 msg: 1613174908.85 epoch sec; seqNo:9901 2021-02-13T00:08:28Z- AHRS_M2 msg: 5.62 phi; -0.39 theta; -0.15 psi 2021-02-13T00:08:28Z- handleDepth()-Depth_Keller msg: 1613174908852 epoch sec; seqNo:9981; depth 0.64 2021-02-13T00:08:28Z- handleDvl() - RDI_Pathfinder msg: 1613174908.88 epoch sec; seqNo:4833 2021-02-13T00:08:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.33 , 110.51 , 86.15 , 132.15 2021-02-13T00:08:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.56 , -0.50 , 0.39 2021-02-13T00:08:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174908.89 epoch sec; seqNo:9981; 4058489.98 north; 592010.57 east 2021-02-13T00:08:29Z- AHRS_M2 msg: 1613174909.25 epoch sec; seqNo:9902 2021-02-13T00:08:29Z- AHRS_M2 msg: 5.67 phi; -0.48 theta; -0.08 psi 2021-02-13T00:08:29Z- handleDepth()-Depth_Keller msg: 1613174909295 epoch sec; seqNo:9982; depth 0.73 2021-02-13T00:08:29Z- handleDvl() - RDI_Pathfinder msg: 1613174909.32 epoch sec; seqNo:4834 2021-02-13T00:08:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:29Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:08:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174909.35 epoch sec; seqNo:9982; 4058489.98 north; 592010.57 east 2021-02-13T00:08:29Z- AHRS_M2 msg: 1613174909.66 epoch sec; seqNo:9903 2021-02-13T00:08:29Z- AHRS_M2 msg: 5.69 phi; -0.53 theta; 0.04 psi 2021-02-13T00:08:29Z- handleDepth()-Depth_Keller msg: 1613174909664 epoch sec; seqNo:9983; depth 0.88 2021-02-13T00:08:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174909.68 epoch sec; seqNo:9983; 4058489.98 north; 592010.57 east 2021-02-13T00:08:30Z- AHRS_M2 msg: 1613174910.06 epoch sec; seqNo:9904 2021-02-13T00:08:30Z- AHRS_M2 msg: 5.69 phi; -0.48 theta; 0.17 psi 2021-02-13T00:08:30Z- handleDepth()-Depth_Keller msg: 1613174910065 epoch sec; seqNo:9984; depth 0.91 2021-02-13T00:08:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:30Z- handleDvl() - RDI_Pathfinder msg: 1613174910.07 epoch sec; seqNo:4835 2021-02-13T00:08:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:30Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:08:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174910.08 epoch sec; seqNo:9984; 4058489.98 north; 592010.57 east 2021-02-13T00:08:30Z- AHRS_M2 msg: 1613174910.46 epoch sec; seqNo:9905 2021-02-13T00:08:30Z- AHRS_M2 msg: 5.70 phi; -0.42 theta; 0.19 psi 2021-02-13T00:08:30Z- handleDepth()-Depth_Keller msg: 1613174910470 epoch sec; seqNo:9985; depth 0.91 2021-02-13T00:08:30Z- handleDvl() - RDI_Pathfinder msg: 1613174910.49 epoch sec; seqNo:4836 2021-02-13T00:08:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:30Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:08:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174910.51 epoch sec; seqNo:9985; 4058489.96 north; 592009.35 east 2021-02-13T00:08:30Z- AHRS_M2 msg: 1613174910.87 epoch sec; seqNo:9906 2021-02-13T00:08:30Z- AHRS_M2 msg: 5.71 phi; -0.41 theta; 0.09 psi 2021-02-13T00:08:30Z- handleDepth()-Depth_Keller msg: 1613174910878 epoch sec; seqNo:9986; depth 0.91 2021-02-13T00:08:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174910.90 epoch sec; seqNo:9986; 4058489.96 north; 592009.35 east 2021-02-13T00:08:31Z- LcmTrn::handleMessages() - handling 45 messages took 0.86 ms and 128 calls... 2021-02-13T00:08:31Z- waiting for 1613174908.66, time is 1613174910.90 2021-02-13T00:08:31Z- LcmTrn::updateTrn() >>>> pose time 1613174910.90, meas time 1613174910.49 <<<< 2021-02-13T00:08:31Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174910.49, ping # 0. 2021-02-13T00:08:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174910.49 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:08:31Z- TerrainNav::motionUpdate() - currEstimate = (4058490, 592009.35, 0.91) 2021-02-13T00:08:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:08:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:08:31Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:08:31Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:08:31Z- TNavBF::Projecting Measurements 2021-02-13T00:08:31Z- TNavFilter::Measurement from time = 1613174910.49 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:08:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:08:31Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:08:31Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:08:31Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:08:31Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:08:31Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:08:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:08:31Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 717.7 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:08:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:08:31Z- TNavBF::mmse selected from filter -1 2021-02-13T00:08:31Z- variances([0], [2], and psi_berg) = 717.711560 833.008190 0.000000 2021-02-13T00:08:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:08:31Z- LcmTrn::publishEstimates() - MLE : 4058488.46 591960.85 0.91 2021-02-13T00:08:31Z- LcmTrn::publishEstimates() - MMSE : 4058489.96 592009.35 0.91 2021-02-13T00:08:31Z- LcmTrn::publishEstimates() - COVAR: 717.71 833.01 0.87 0.00 2021-02-13T00:08:31Z- AHRS_M2 msg: 1613174911.30 epoch sec; seqNo:9907 2021-02-13T00:08:31Z- AHRS_M2 msg: 5.72 phi; -0.46 theta; -0.05 psi 2021-02-13T00:08:31Z- handleDepth()-Depth_Keller msg: 1613174911303 epoch sec; seqNo:9987; depth 0.93 2021-02-13T00:08:31Z- handleDvl() - RDI_Pathfinder msg: 1613174911.32 epoch sec; seqNo:4837 2021-02-13T00:08:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:31Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:08:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174911.34 epoch sec; seqNo:9987; 4058489.96 north; 592009.35 east 2021-02-13T00:08:31Z- AHRS_M2 msg: 1613174911.68 epoch sec; seqNo:9908 2021-02-13T00:08:31Z- AHRS_M2 msg: 5.75 phi; -0.51 theta; -0.12 psi 2021-02-13T00:08:31Z- handleDepth()-Depth_Keller msg: 1613174911680 epoch sec; seqNo:9988; depth 1.00 2021-02-13T00:08:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174911.70 epoch sec; seqNo:9988; 4058489.96 north; 592009.35 east 2021-02-13T00:08:32Z- AHRS_M2 msg: 1613174912.08 epoch sec; seqNo:9909 2021-02-13T00:08:32Z- AHRS_M2 msg: 5.78 phi; -0.53 theta; -0.11 psi 2021-02-13T00:08:32Z- handleDepth()-Depth_Keller msg: 1613174912084 epoch sec; seqNo:9989; depth 1.05 2021-02-13T00:08:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174912.11 epoch sec; seqNo:9989; 4058489.96 north; 592009.35 east 2021-02-13T00:08:32Z- AHRS_M2 msg: 1613174912.48 epoch sec; seqNo:9910 2021-02-13T00:08:32Z- AHRS_M2 msg: 5.81 phi; -0.52 theta; -0.04 psi 2021-02-13T00:08:32Z- handleDepth()-Depth_Keller msg: 1613174912488 epoch sec; seqNo:9990; depth 1.13 2021-02-13T00:08:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174912.50 epoch sec; seqNo:9990; 4058489.96 north; 592009.35 east 2021-02-13T00:08:32Z- handleDepth()-Depth_Keller msg: 1613174912909 epoch sec; seqNo:9991; depth 1.22 2021-02-13T00:08:32Z- handleDvl() - RDI_Pathfinder msg: 1613174912.92 epoch sec; seqNo:4838 2021-02-13T00:08:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 106.45 , 118.35 , 0.00 , 149.61 2021-02-13T00:08:32Z- handleDvl() - RDI_Pathfinder msg: velocities 1.25 , 0.13 , -0.31 2021-02-13T00:08:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174912.95 epoch sec; seqNo:9991; 4058490.71 north; 592008.12 east 2021-02-13T00:08:33Z- AHRS_M2 msg: 1613174913.29 epoch sec; seqNo:9911 2021-02-13T00:08:33Z- AHRS_M2 msg: 5.85 phi; -0.47 theta; 0.06 psi 2021-02-13T00:08:33Z- handleDepth()-Depth_Keller msg: 1613174913298 epoch sec; seqNo:9992; depth 1.27 2021-02-13T00:08:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174913.32 epoch sec; seqNo:9992; 4058491.09 north; 592008.12 east 2021-02-13T00:08:33Z- AHRS_M2 msg: 1613174913.70 epoch sec; seqNo:9912 2021-02-13T00:08:33Z- AHRS_M2 msg: 5.88 phi; -0.45 theta; 0.03 psi 2021-02-13T00:08:33Z- handleDepth()-Depth_Keller msg: 1613174913701 epoch sec; seqNo:9993; depth 1.35 2021-02-13T00:08:33Z- handleDvl() - RDI_Pathfinder msg: 1613174913.73 epoch sec; seqNo:4839 2021-02-13T00:08:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 113.62 , 106.18 , 85.34 , 0.00 2021-02-13T00:08:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.17 , -0.52 2021-02-13T00:08:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174913.74 epoch sec; seqNo:9993; 4058491.47 north; 592008.11 east 2021-02-13T00:08:34Z- AHRS_M2 msg: 1613174914.10 epoch sec; seqNo:9913 2021-02-13T00:08:34Z- AHRS_M2 msg: 5.92 phi; -0.45 theta; -0.03 psi 2021-02-13T00:08:34Z- handleDepth()-Depth_Keller msg: 1613174914105 epoch sec; seqNo:9994; depth 1.43 2021-02-13T00:08:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174914.12 epoch sec; seqNo:9994; 4058491.84 north; 592008.11 east 2021-02-13T00:08:34Z- AHRS_M2 msg: 1613174914.52 epoch sec; seqNo:9914 2021-02-13T00:08:34Z- AHRS_M2 msg: 5.97 phi; -0.45 theta; -0.04 psi 2021-02-13T00:08:34Z- handleDepth()-Depth_Keller msg: 1613174914530 epoch sec; seqNo:9995; depth 1.52 2021-02-13T00:08:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174914.59 epoch sec; seqNo:9995; 4058492.22 north; 592008.11 east 2021-02-13T00:08:34Z- AHRS_M2 msg: 1613174914.91 epoch sec; seqNo:9915 2021-02-13T00:08:34Z- AHRS_M2 msg: 6.01 phi; -0.45 theta; 0.02 psi 2021-02-13T00:08:34Z- handleDepth()-Depth_Keller msg: 1613174914912 epoch sec; seqNo:9996; depth 1.63 2021-02-13T00:08:34Z- handleDvl() - RDI_Pathfinder msg: 1613174914.94 epoch sec; seqNo:4840 2021-02-13T00:08:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:34Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:08:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174914.97 epoch sec; seqNo:9996; 4058492.60 north; 592008.10 east 2021-02-13T00:08:35Z- AHRS_M2 msg: 1613174915.31 epoch sec; seqNo:9916 2021-02-13T00:08:35Z- AHRS_M2 msg: 6.03 phi; -0.44 theta; 0.10 psi 2021-02-13T00:08:35Z- handleDepth()-Depth_Keller msg: 1613174915316 epoch sec; seqNo:9997; depth 1.75 2021-02-13T00:08:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174915.33 epoch sec; seqNo:9997; 4058492.59 north; 592006.88 east 2021-02-13T00:08:35Z- AHRS_M2 msg: 1613174915.72 epoch sec; seqNo:9917 2021-02-13T00:08:35Z- AHRS_M2 msg: 6.05 phi; -0.44 theta; 0.13 psi 2021-02-13T00:08:35Z- handleDepth()-Depth_Keller msg: 1613174915722 epoch sec; seqNo:9998; depth 1.86 2021-02-13T00:08:35Z- handleDvl() - RDI_Pathfinder msg: 1613174915.78 epoch sec; seqNo:4841 2021-02-13T00:08:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 112.74 , 102.32 , 85.95 , 0.00 2021-02-13T00:08:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.39 , -0.24 , 0.15 2021-02-13T00:08:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174915.80 epoch sec; seqNo:9998; 4058492.97 north; 592006.88 east 2021-02-13T00:08:36Z- LcmTrn::handleMessages() - handling 40 messages took 0.72 ms and 126 calls... 2021-02-13T00:08:36Z- waiting for 1613174913.90, time is 1613174915.80 2021-02-13T00:08:36Z- LcmTrn::updateTrn() equating pose 1613174915.80 and meas times 1613174915.80 2021-02-13T00:08:36Z- LcmTrn::updateTrn() >>>> pose time 1613174915.80, meas time 1613174915.80 <<<< 2021-02-13T00:08:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174915.80 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:08:36Z- TerrainNav::motionUpdate() - currEstimate = (4058493, 592006.88, 1.86) 2021-02-13T00:08:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:08:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:08:36Z- TNavBF::Projecting Measurements 2021-02-13T00:08:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:08:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:08:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:08:36Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:08:36Z- TNavFilter::3 good beams. 2021-02-13T00:08:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:08:36Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:08:36Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:08:36Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:08:36Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:08:36Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:08:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:08:36Z- TNavBF::mmse selected from filter -1 2021-02-13T00:08:36Z- TerrainMapOctree: (vehN, vehE ) = (4058492.97, 592006.88). 2021-02-13T00:08:36Z- TerrainMapOctree: Min Distance = 3554.27. 2021-02-13T00:08:36Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:08:36Z- There is no specific good beam that all particles have in common. 2021-02-13T00:08:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:08:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 20.34 2021-02-13T00:08:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:08:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 16.75 2021-02-13T00:08:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:08:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 11.82 2021-02-13T00:08:36Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:08:36Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:08:36Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:08:36Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:08:36Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:08:36Z- Filter 0 No beams used in subcloud update 2021-02-13T00:08:36Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:08:36Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:08:36Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 717.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:08:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174915.80 sec. 2021-02-13T00:08:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:08:36Z- TNavBF::mmse selected from filter -1 2021-02-13T00:08:36Z- variances([0], [2], and psi_berg) = 717.646724 833.008149 0.000000 2021-02-13T00:08:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:08:36Z- LcmTrn::publishEstimates() - MLE : 4058491.47 591958.38 1.86 2021-02-13T00:08:36Z- LcmTrn::publishEstimates() - MMSE : 4058492.97 592006.88 1.86 2021-02-13T00:08:36Z- LcmTrn::publishEstimates() - COVAR: 717.65 833.01 0.90 0.00 2021-02-13T00:08:36Z- AHRS_M2 msg: 1613174916.12 epoch sec; seqNo:9918 2021-02-13T00:08:36Z- AHRS_M2 msg: 6.08 phi; -0.45 theta; 0.15 psi 2021-02-13T00:08:36Z- handleDepth()-Depth_Keller msg: 1613174916137 epoch sec; seqNo:9999; depth 1.98 2021-02-13T00:08:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:36Z- handleDvl() - RDI_Pathfinder msg: 1613174916.14 epoch sec; seqNo:4842 2021-02-13T00:08:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.39 , -0.24 , 0.15 2021-02-13T00:08:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174916.16 epoch sec; seqNo:9999; 4058492.97 north; 592006.88 east 2021-02-13T00:08:36Z- AHRS_M2 msg: 1613174916.52 epoch sec; seqNo:9919 2021-02-13T00:08:36Z- AHRS_M2 msg: 6.09 phi; -0.45 theta; 0.13 psi 2021-02-13T00:08:36Z- handleDepth()-Depth_Keller msg: 1613174916528 epoch sec; seqNo:10000; depth 2.07 2021-02-13T00:08:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174916.54 epoch sec; seqNo:10000; 4058492.97 north; 592006.88 east 2021-02-13T00:08:36Z- AHRS_M2 msg: 1613174916.94 epoch sec; seqNo:9920 2021-02-13T00:08:36Z- AHRS_M2 msg: 6.12 phi; -0.44 theta; 0.09 psi 2021-02-13T00:08:36Z- handleDepth()-Depth_Keller msg: 1613174916945 epoch sec; seqNo:10001; depth 2.16 2021-02-13T00:08:37Z- handleDvl() - RDI_Pathfinder msg: 1613174917.00 epoch sec; seqNo:4843 2021-02-13T00:08:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 118.69 , 98.40 , 86.22 , 140.34 2021-02-13T00:08:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.29 , -0.05 , 0.04 2021-02-13T00:08:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174917.02 epoch sec; seqNo:10001; 4058492.97 north; 592006.88 east 2021-02-13T00:08:37Z- AHRS_M2 msg: 1613174917.33 epoch sec; seqNo:9921 2021-02-13T00:08:37Z- AHRS_M2 msg: 6.15 phi; -0.41 theta; 0.04 psi 2021-02-13T00:08:37Z- handleDepth()-Depth_Keller msg: 1613174917347 epoch sec; seqNo:10002; depth 2.27 2021-02-13T00:08:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174917.40 epoch sec; seqNo:10002; 4058493.35 north; 592006.87 east 2021-02-13T00:08:37Z- AHRS_M2 msg: 1613174917.76 epoch sec; seqNo:9922 2021-02-13T00:08:37Z- AHRS_M2 msg: 6.19 phi; -0.37 theta; 0.00 psi 2021-02-13T00:08:37Z- handleDepth()-Depth_Keller msg: 1613174917833 epoch sec; seqNo:10003; depth 2.36 2021-02-13T00:08:37Z- handleDvl() - RDI_Pathfinder msg: 1613174917.98 epoch sec; seqNo:4844 2021-02-13T00:08:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 110.64 , 101.71 , 85.34 , 0.00 2021-02-13T00:08:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.63 , -0.18 , -0.09 2021-02-13T00:08:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174918.02 epoch sec; seqNo:10003; 4058493.73 north; 592006.87 east 2021-02-13T00:08:38Z- AHRS_M2 msg: 1613174918.35 epoch sec; seqNo:9923 2021-02-13T00:08:38Z- AHRS_M2 msg: 6.26 phi; -0.32 theta; 0.03 psi 2021-02-13T00:08:38Z- handleDepth()-Depth_Keller msg: 1613174918386 epoch sec; seqNo:10004; depth 2.53 2021-02-13T00:08:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174918.47 epoch sec; seqNo:10004; 4058493.73 north; 592006.87 east 2021-02-13T00:08:38Z- AHRS_M2 msg: 1613174918.73 epoch sec; seqNo:9924 2021-02-13T00:08:38Z- AHRS_M2 msg: 0.01 phi; -0.31 theta; 0.06 psi 2021-02-13T00:08:38Z- handleDepth()-Depth_Keller msg: 1613174918738 epoch sec; seqNo:10005; depth 2.65 2021-02-13T00:08:38Z- handleDvl() - RDI_Pathfinder msg: 1613174918.76 epoch sec; seqNo:4845 2021-02-13T00:08:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:38Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:08:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174918.80 epoch sec; seqNo:10005; 4058494.10 north; 592006.86 east 2021-02-13T00:08:39Z- AHRS_M2 msg: 1613174919.14 epoch sec; seqNo:9925 2021-02-13T00:08:39Z- AHRS_M2 msg: 0.04 phi; -0.31 theta; 0.09 psi 2021-02-13T00:08:39Z- handleDepth()-Depth_Keller msg: 1613174919145 epoch sec; seqNo:10006; depth 2.73 2021-02-13T00:08:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174919.17 epoch sec; seqNo:10006; 4058494.48 north; 592006.86 east 2021-02-13T00:08:39Z- AHRS_M2 msg: 1613174919.54 epoch sec; seqNo:9926 2021-02-13T00:08:39Z- AHRS_M2 msg: 0.08 phi; -0.31 theta; 0.11 psi 2021-02-13T00:08:39Z- handleDepth()-Depth_Keller msg: 1613174919549 epoch sec; seqNo:10007; depth 2.85 2021-02-13T00:08:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:39Z- handleDvl() - RDI_Pathfinder msg: 1613174919.56 epoch sec; seqNo:4846 2021-02-13T00:08:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:39Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:08:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174919.59 epoch sec; seqNo:10007; 4058494.48 north; 592006.86 east 2021-02-13T00:08:39Z- AHRS_M2 msg: 1613174919.94 epoch sec; seqNo:9927 2021-02-13T00:08:39Z- AHRS_M2 msg: 0.12 phi; -0.30 theta; 0.13 psi 2021-02-13T00:08:39Z- handleDepth()-Depth_Keller msg: 1613174919947 epoch sec; seqNo:10008; depth 2.94 2021-02-13T00:08:39Z- handleDvl() - RDI_Pathfinder msg: 1613174919.96 epoch sec; seqNo:4847 2021-02-13T00:08:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 105.23 , 97.79 , 87.37 , 121.60 2021-02-13T00:08:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.51 , 0.04 , 0.30 2021-02-13T00:08:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174919.97 epoch sec; seqNo:10008; 4058494.86 north; 592006.86 east 2021-02-13T00:08:40Z- AHRS_M2 msg: 1613174920.34 epoch sec; seqNo:9928 2021-02-13T00:08:40Z- AHRS_M2 msg: 0.15 phi; -0.29 theta; 0.15 psi 2021-02-13T00:08:40Z- handleDepth()-Depth_Keller msg: 1613174920350 epoch sec; seqNo:10009; depth 3.03 2021-02-13T00:08:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174920.36 epoch sec; seqNo:10009; 4058494.86 north; 592006.86 east 2021-02-13T00:08:40Z- AHRS_M2 msg: 1613174920.78 epoch sec; seqNo:9929 2021-02-13T00:08:40Z- AHRS_M2 msg: 0.18 phi; -0.27 theta; 0.15 psi 2021-02-13T00:08:40Z- handleDepth()-Depth_Keller msg: 1613174920781 epoch sec; seqNo:10010; depth 3.11 2021-02-13T00:08:40Z- handleDvl() - RDI_Pathfinder msg: 1613174920.79 epoch sec; seqNo:4848 2021-02-13T00:08:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 109.76 , 91.90 , 87.44 , 115.72 2021-02-13T00:08:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.51 , 0.16 , -0.18 2021-02-13T00:08:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174920.80 epoch sec; seqNo:10010; 4058495.24 north; 592006.85 east 2021-02-13T00:08:41Z- AHRS_M2 msg: 1613174921.15 epoch sec; seqNo:9930 2021-02-13T00:08:41Z- AHRS_M2 msg: 0.21 phi; -0.25 theta; 0.12 psi 2021-02-13T00:08:41Z- LcmTrn::handleMessages() - handling 44 messages took 0.84 ms and 128 calls... 2021-02-13T00:08:41Z- waiting for 1613174918.80, time is 1613174921.15 2021-02-13T00:08:41Z- LcmTrn::updateTrn() >>>> pose time 1613174921.15, meas time 1613174920.79 <<<< 2021-02-13T00:08:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174920.79 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:08:41Z- TerrainNav::motionUpdate() - currEstimate = (4058495, 592006.85, 3.11) 2021-02-13T00:08:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:08:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:08:41Z- TNavBF::Projecting Measurements 2021-02-13T00:08:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:08:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:08:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:08:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:08:41Z- TNavFilter::4 good beams. 2021-02-13T00:08:41Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:08:41Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:08:41Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:08:41Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:08:41Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:08:41Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:08:41Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:08:41Z- TNavBF::mmse selected from filter -1 2021-02-13T00:08:41Z- TerrainMapOctree: (vehN, vehE ) = (4058495.09, 592006.85). 2021-02-13T00:08:41Z- TerrainMapOctree: Min Distance = 3552.64. 2021-02-13T00:08:41Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:08:41Z- There is no specific good beam that all particles have in common. 2021-02-13T00:08:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:08:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 19.28 2021-02-13T00:08:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:08:41Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 13.51 2021-02-13T00:08:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:08:41Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 12.23 2021-02-13T00:08:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:08:41Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.035 ExpectedMeasDiff: nan currMeas.covariance: 21.42 2021-02-13T00:08:41Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:08:41Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:08:41Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:08:41Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:08:41Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:08:41Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:08:41Z- Filter 0 No beams used in subcloud update 2021-02-13T00:08:41Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:08:41Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:08:41Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 717.6 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:08:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174920.79 sec. 2021-02-13T00:08:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:08:41Z- TNavBF::mmse selected from filter -1 2021-02-13T00:08:41Z- variances([0], [2], and psi_berg) = 717.586078 833.008111 0.000000 2021-02-13T00:08:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:08:41Z- LcmTrn::publishEstimates() - MLE : 4058493.74 591958.35 3.11 2021-02-13T00:08:41Z- LcmTrn::publishEstimates() - MMSE : 4058495.24 592006.85 3.11 2021-02-13T00:08:41Z- LcmTrn::publishEstimates() - COVAR: 717.59 833.01 0.83 0.00 2021-02-13T00:08:41Z- handleDepth()-Depth_Keller msg: 1613174921161 epoch sec; seqNo:10011; depth 3.18 2021-02-13T00:08:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174921.18 epoch sec; seqNo:10011; 4058495.24 north; 592006.85 east 2021-02-13T00:08:41Z- AHRS_M2 msg: 1613174921.58 epoch sec; seqNo:9931 2021-02-13T00:08:41Z- AHRS_M2 msg: 0.25 phi; -0.23 theta; 0.08 psi 2021-02-13T00:08:41Z- handleDepth()-Depth_Keller msg: 1613174921586 epoch sec; seqNo:10012; depth 3.25 2021-02-13T00:08:41Z- handleDvl() - RDI_Pathfinder msg: 1613174921.60 epoch sec; seqNo:4849 2021-02-13T00:08:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 109.90 , 97.99 , 87.58 , 117.34 2021-02-13T00:08:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.03 , -0.32 2021-02-13T00:08:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174921.61 epoch sec; seqNo:10012; 4058495.62 north; 592006.85 east 2021-02-13T00:08:41Z- AHRS_M2 msg: 1613174921.96 epoch sec; seqNo:9932 2021-02-13T00:08:41Z- AHRS_M2 msg: 0.28 phi; -0.22 theta; 0.04 psi 2021-02-13T00:08:41Z- handleDepth()-Depth_Keller msg: 1613174921968 epoch sec; seqNo:10013; depth 3.33 2021-02-13T00:08:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:41Z- handleDvl() - RDI_Pathfinder msg: 1613174921.97 epoch sec; seqNo:4850 2021-02-13T00:08:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.03 , -0.32 2021-02-13T00:08:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174921.98 epoch sec; seqNo:10013; 4058496.00 north; 592006.84 east 2021-02-13T00:08:42Z- AHRS_M2 msg: 1613174922.36 epoch sec; seqNo:9933 2021-02-13T00:08:42Z- AHRS_M2 msg: 0.32 phi; -0.22 theta; 0.03 psi 2021-02-13T00:08:42Z- handleDepth()-Depth_Keller msg: 1613174922368 epoch sec; seqNo:10014; depth 3.38 2021-02-13T00:08:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174922.38 epoch sec; seqNo:10014; 4058496.38 north; 592006.84 east 2021-02-13T00:08:42Z- AHRS_M2 msg: 1613174922.77 epoch sec; seqNo:9934 2021-02-13T00:08:42Z- AHRS_M2 msg: 0.36 phi; -0.23 theta; 0.06 psi 2021-02-13T00:08:42Z- handleDepth()-Depth_Keller msg: 1613174922805 epoch sec; seqNo:10015; depth 3.47 2021-02-13T00:08:42Z- handleDvl() - RDI_Pathfinder msg: 1613174922.86 epoch sec; seqNo:4851 2021-02-13T00:08:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 100.83 , 97.86 , 87.44 , 112.74 2021-02-13T00:08:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.62 , -0.22 , 0.05 2021-02-13T00:08:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174922.90 epoch sec; seqNo:10015; 4058496.38 north; 592006.84 east 2021-02-13T00:08:43Z- AHRS_M2 msg: 1613174923.24 epoch sec; seqNo:9935 2021-02-13T00:08:43Z- AHRS_M2 msg: 0.39 phi; -0.24 theta; 0.11 psi 2021-02-13T00:08:43Z- handleDepth()-Depth_Keller msg: 1613174923302 epoch sec; seqNo:10016; depth 3.55 2021-02-13T00:08:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:43Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:43Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:43Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:43Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:43Z- handleDvl() - RDI_Pathfinder msg: 1613174923.33 epoch sec; seqNo:4852 2021-02-13T00:08:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.62 , -0.22 , 0.05 2021-02-13T00:08:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174923.43 epoch sec; seqNo:10016; 4058496.76 north; 592006.84 east 2021-02-13T00:08:43Z- AHRS_M2 msg: 1613174923.89 epoch sec; seqNo:9936 2021-02-13T00:08:43Z- AHRS_M2 msg: 0.46 phi; -0.24 theta; 0.19 psi 2021-02-13T00:08:43Z- handleDepth()-Depth_Keller msg: 1613174923899 epoch sec; seqNo:10017; depth 3.64 2021-02-13T00:08:43Z- handleDvl() - RDI_Pathfinder msg: 1613174923.91 epoch sec; seqNo:4853 2021-02-13T00:08:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 107.87 , 91.63 , 0.00 , 115.99 2021-02-13T00:08:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.52 , 0.02 , 0.26 2021-02-13T00:08:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174923.92 epoch sec; seqNo:10017; 4058497.13 north; 592006.83 east 2021-02-13T00:08:44Z- AHRS_M2 msg: 1613174924.29 epoch sec; seqNo:9937 2021-02-13T00:08:44Z- AHRS_M2 msg: 0.49 phi; -0.24 theta; 0.20 psi 2021-02-13T00:08:44Z- handleDepth()-Depth_Keller msg: 1613174924301 epoch sec; seqNo:10018; depth 3.75 2021-02-13T00:08:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174924.32 epoch sec; seqNo:10018; 4058497.13 north; 592006.83 east 2021-02-13T00:08:44Z- AHRS_M2 msg: 1613174924.70 epoch sec; seqNo:9938 2021-02-13T00:08:44Z- AHRS_M2 msg: 0.51 phi; -0.24 theta; 0.18 psi 2021-02-13T00:08:44Z- handleDepth()-Depth_Keller msg: 1613174924700 epoch sec; seqNo:10019; depth 3.80 2021-02-13T00:08:44Z- handleDvl() - RDI_Pathfinder msg: 1613174924.71 epoch sec; seqNo:4854 2021-02-13T00:08:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 114.03 , 90.21 , 0.00 , 118.49 2021-02-13T00:08:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , 0.24 , 0.04 2021-02-13T00:08:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174924.74 epoch sec; seqNo:10019; 4058497.53 north; 592008.05 east 2021-02-13T00:08:45Z- AHRS_M2 msg: 1613174925.10 epoch sec; seqNo:9939 2021-02-13T00:08:45Z- AHRS_M2 msg: 0.54 phi; -0.23 theta; 0.14 psi 2021-02-13T00:08:45Z- handleDepth()-Depth_Keller msg: 1613174925104 epoch sec; seqNo:10020; depth 3.86 2021-02-13T00:08:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174925.12 epoch sec; seqNo:10020; 4058497.53 north; 592008.05 east 2021-02-13T00:08:45Z- AHRS_M2 msg: 1613174925.51 epoch sec; seqNo:9940 2021-02-13T00:08:45Z- AHRS_M2 msg: 0.58 phi; -0.23 theta; 0.09 psi 2021-02-13T00:08:45Z- handleDepth()-Depth_Keller msg: 1613174925514 epoch sec; seqNo:10021; depth 3.91 2021-02-13T00:08:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174925.53 epoch sec; seqNo:10021; 4058497.91 north; 592008.04 east 2021-02-13T00:08:45Z- AHRS_M2 msg: 1613174925.91 epoch sec; seqNo:9941 2021-02-13T00:08:45Z- AHRS_M2 msg: 0.61 phi; -0.23 theta; 0.07 psi 2021-02-13T00:08:45Z- handleDepth()-Depth_Keller msg: 1613174925917 epoch sec; seqNo:10022; depth 3.96 2021-02-13T00:08:45Z- handleDvl() - RDI_Pathfinder msg: 1613174925.93 epoch sec; seqNo:4855 2021-02-13T00:08:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 110.85 , 97.45 , 0.00 , 116.80 2021-02-13T00:08:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.08 , -0.19 2021-02-13T00:08:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174925.94 epoch sec; seqNo:10022; 4058497.91 north; 592008.04 east 2021-02-13T00:08:46Z- LcmTrn::handleMessages() - handling 42 messages took 0.69 ms and 128 calls... 2021-02-13T00:08:46Z- waiting for 1613174924.15, time is 1613174925.94 2021-02-13T00:08:46Z- LcmTrn::updateTrn() equating pose 1613174925.94 and meas times 1613174925.94 2021-02-13T00:08:46Z- LcmTrn::updateTrn() >>>> pose time 1613174925.94, meas time 1613174925.94 <<<< 2021-02-13T00:08:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174925.94 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:08:46Z- TerrainNav::motionUpdate() - currEstimate = (4058498, 592008.04, 3.96) 2021-02-13T00:08:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:08:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:08:46Z- TNavBF::Projecting Measurements 2021-02-13T00:08:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:08:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:08:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:08:46Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:08:46Z- TNavFilter::3 good beams. 2021-02-13T00:08:46Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:08:46Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:08:46Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:08:46Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:08:46Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:08:46Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:08:46Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:08:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:08:46Z- TerrainMapOctree: (vehN, vehE ) = (4058497.91, 592008.04). 2021-02-13T00:08:46Z- TerrainMapOctree: Min Distance = 3549.70. 2021-02-13T00:08:46Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:08:46Z- There is no specific good beam that all particles have in common. 2021-02-13T00:08:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:08:46Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 19.66 2021-02-13T00:08:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:08:46Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 15.20 2021-02-13T00:08:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:08:46Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 21.83 2021-02-13T00:08:46Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:08:46Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:08:46Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:08:46Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:08:46Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:08:46Z- Filter 0 No beams used in subcloud update 2021-02-13T00:08:46Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:08:46Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:08:46Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 717.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:08:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174925.94 sec. 2021-02-13T00:08:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:08:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:08:46Z- variances([0], [2], and psi_berg) = 717.530003 833.008075 0.000000 2021-02-13T00:08:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:08:46Z- LcmTrn::publishEstimates() - MLE : 4058496.41 591959.54 3.96 2021-02-13T00:08:46Z- LcmTrn::publishEstimates() - MMSE : 4058497.91 592008.04 3.96 2021-02-13T00:08:46Z- LcmTrn::publishEstimates() - COVAR: 717.53 833.01 0.85 0.00 2021-02-13T00:08:46Z- AHRS_M2 msg: 1613174926.31 epoch sec; seqNo:9942 2021-02-13T00:08:46Z- AHRS_M2 msg: 0.65 phi; -0.24 theta; 0.06 psi 2021-02-13T00:08:46Z- handleDepth()-Depth_Keller msg: 1613174926316 epoch sec; seqNo:10023; depth 4.04 2021-02-13T00:08:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174926.33 epoch sec; seqNo:10023; 4058498.28 north; 592008.04 east 2021-02-13T00:08:46Z- AHRS_M2 msg: 1613174926.72 epoch sec; seqNo:9943 2021-02-13T00:08:46Z- AHRS_M2 msg: 0.68 phi; -0.26 theta; 0.08 psi 2021-02-13T00:08:46Z- handleDepth()-Depth_Keller msg: 1613174926720 epoch sec; seqNo:10024; depth 4.10 2021-02-13T00:08:46Z- handleDvl() - RDI_Pathfinder msg: 1613174926.73 epoch sec; seqNo:4856 2021-02-13T00:08:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.34 , 94.41 , 86.97 , 115.24 2021-02-13T00:08:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , -0.09 , -0.06 2021-02-13T00:08:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174926.74 epoch sec; seqNo:10024; 4058498.28 north; 592008.04 east 2021-02-13T00:08:47Z- AHRS_M2 msg: 1613174927.12 epoch sec; seqNo:9944 2021-02-13T00:08:47Z- AHRS_M2 msg: 0.70 phi; -0.26 theta; 0.10 psi 2021-02-13T00:08:47Z- handleDepth()-Depth_Keller msg: 1613174927134 epoch sec; seqNo:10025; depth 4.18 2021-02-13T00:08:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:47Z- handleDvl() - RDI_Pathfinder msg: 1613174927.14 epoch sec; seqNo:4857 2021-02-13T00:08:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , -0.09 , -0.06 2021-02-13T00:08:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174927.15 epoch sec; seqNo:10025; 4058498.66 north; 592008.04 east 2021-02-13T00:08:47Z- AHRS_M2 msg: 1613174927.53 epoch sec; seqNo:9945 2021-02-13T00:08:47Z- AHRS_M2 msg: 0.73 phi; -0.27 theta; 0.11 psi 2021-02-13T00:08:47Z- handleDepth()-Depth_Keller msg: 1613174927539 epoch sec; seqNo:10026; depth 4.25 2021-02-13T00:08:47Z- handleDvl() - RDI_Pathfinder msg: 1613174927.58 epoch sec; seqNo:4858 2021-02-13T00:08:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 106.52 , 97.05 , 0.00 , 115.99 2021-02-13T00:08:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.59 , 0.00 , -0.05 2021-02-13T00:08:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174927.59 epoch sec; seqNo:10026; 4058498.68 north; 592009.26 east 2021-02-13T00:08:47Z- AHRS_M2 msg: 1613174927.93 epoch sec; seqNo:9946 2021-02-13T00:08:47Z- AHRS_M2 msg: 0.75 phi; -0.27 theta; 0.11 psi 2021-02-13T00:08:47Z- handleDepth()-Depth_Keller msg: 1613174927932 epoch sec; seqNo:10027; depth 4.31 2021-02-13T00:08:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174927.94 epoch sec; seqNo:10027; 4058499.06 north; 592009.25 east 2021-02-13T00:08:48Z- AHRS_M2 msg: 1613174928.33 epoch sec; seqNo:9947 2021-02-13T00:08:48Z- AHRS_M2 msg: 0.77 phi; -0.27 theta; 0.10 psi 2021-02-13T00:08:48Z- handleDepth()-Depth_Keller msg: 1613174928337 epoch sec; seqNo:10028; depth 4.38 2021-02-13T00:08:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:48Z- handleDvl() - RDI_Pathfinder msg: 1613174928.34 epoch sec; seqNo:4859 2021-02-13T00:08:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.59 , 0.00 , -0.05 2021-02-13T00:08:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174928.35 epoch sec; seqNo:10028; 4058499.06 north; 592009.25 east 2021-02-13T00:08:48Z- AHRS_M2 msg: 1613174928.74 epoch sec; seqNo:9948 2021-02-13T00:08:48Z- AHRS_M2 msg: 0.79 phi; -0.27 theta; 0.09 psi 2021-02-13T00:08:48Z- handleDepth()-Depth_Keller msg: 1613174928745 epoch sec; seqNo:10029; depth 4.45 2021-02-13T00:08:48Z- handleDvl() - RDI_Pathfinder msg: 1613174928.76 epoch sec; seqNo:4860 2021-02-13T00:08:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 105.03 , 91.63 , 85.68 , 115.45 2021-02-13T00:08:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.56 , 0.10 , -0.11 2021-02-13T00:08:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174928.78 epoch sec; seqNo:10029; 4058499.43 north; 592009.25 east 2021-02-13T00:08:49Z- AHRS_M2 msg: 1613174929.14 epoch sec; seqNo:9949 2021-02-13T00:08:49Z- AHRS_M2 msg: 0.80 phi; -0.28 theta; 0.07 psi 2021-02-13T00:08:49Z- handleDepth()-Depth_Keller msg: 1613174929149 epoch sec; seqNo:10030; depth 4.52 2021-02-13T00:08:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174929.16 epoch sec; seqNo:10030; 4058499.43 north; 592009.25 east 2021-02-13T00:08:49Z- AHRS_M2 msg: 1613174929.54 epoch sec; seqNo:9950 2021-02-13T00:08:49Z- AHRS_M2 msg: 0.81 phi; -0.29 theta; 0.05 psi 2021-02-13T00:08:49Z- handleDepth()-Depth_Keller msg: 1613174929548 epoch sec; seqNo:10031; depth 4.58 2021-02-13T00:08:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:49Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:49Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:49Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:49Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:49Z- handleDvl() - RDI_Pathfinder msg: 1613174929.55 epoch sec; seqNo:4861 2021-02-13T00:08:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.56 , 0.10 , -0.11 2021-02-13T00:08:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174929.56 epoch sec; seqNo:10031; 4058499.43 north; 592009.25 east 2021-02-13T00:08:49Z- AHRS_M2 msg: 1613174929.95 epoch sec; seqNo:9951 2021-02-13T00:08:49Z- AHRS_M2 msg: 0.82 phi; -0.29 theta; 0.03 psi 2021-02-13T00:08:49Z- handleDepth()-Depth_Keller msg: 1613174929954 epoch sec; seqNo:10032; depth 4.65 2021-02-13T00:08:49Z- handleDvl() - RDI_Pathfinder msg: 1613174929.96 epoch sec; seqNo:4862 2021-02-13T00:08:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.88 , 97.11 , 0.00 , 118.76 2021-02-13T00:08:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.57 , 0.06 , -0.22 2021-02-13T00:08:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174929.98 epoch sec; seqNo:10032; 4058499.81 north; 592009.24 east 2021-02-13T00:08:50Z- AHRS_M2 msg: 1613174930.35 epoch sec; seqNo:9952 2021-02-13T00:08:50Z- AHRS_M2 msg: 0.84 phi; -0.30 theta; 0.01 psi 2021-02-13T00:08:50Z- handleDepth()-Depth_Keller msg: 1613174930357 epoch sec; seqNo:10033; depth 4.73 2021-02-13T00:08:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174930.38 epoch sec; seqNo:10033; 4058499.83 north; 592010.47 east 2021-02-13T00:08:50Z- AHRS_M2 msg: 1613174930.78 epoch sec; seqNo:9953 2021-02-13T00:08:50Z- AHRS_M2 msg: 0.85 phi; -0.30 theta; -0.02 psi 2021-02-13T00:08:50Z- handleDepth()-Depth_Keller msg: 1613174930788 epoch sec; seqNo:10034; depth 4.78 2021-02-13T00:08:50Z- handleDvl() - RDI_Pathfinder msg: 1613174930.80 epoch sec; seqNo:4863 2021-02-13T00:08:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.07 , 97.59 , 84.19 , 119.91 2021-02-13T00:08:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , -0.20 , -0.14 2021-02-13T00:08:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174930.82 epoch sec; seqNo:10034; 4058500.20 north; 592010.46 east 2021-02-13T00:08:51Z- AHRS_M2 msg: 1613174931.16 epoch sec; seqNo:9954 2021-02-13T00:08:51Z- AHRS_M2 msg: 0.87 phi; -0.31 theta; -0.04 psi 2021-02-13T00:08:51Z- handleDepth()-Depth_Keller msg: 1613174931164 epoch sec; seqNo:10035; depth 4.86 2021-02-13T00:08:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174931.18 epoch sec; seqNo:10035; 4058500.20 north; 592010.46 east 2021-02-13T00:08:51Z- LcmTrn::handleMessages() - handling 47 messages took 0.72 ms and 133 calls... 2021-02-13T00:08:51Z- waiting for 1613174928.94, time is 1613174931.18 2021-02-13T00:08:51Z- LcmTrn::updateTrn() >>>> pose time 1613174931.18, meas time 1613174930.80 <<<< 2021-02-13T00:08:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174930.80 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:08:51Z- TerrainNav::motionUpdate() - currEstimate = (4058500, 592010.46, 4.86) 2021-02-13T00:08:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:08:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:08:51Z- TNavBF::Projecting Measurements 2021-02-13T00:08:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:08:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:08:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:08:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:08:51Z- TNavFilter::4 good beams. 2021-02-13T00:08:51Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:08:51Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:08:51Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:08:51Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:08:51Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:08:51Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:08:51Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:08:51Z- TNavBF::mmse selected from filter -1 2021-02-13T00:08:51Z- TerrainMapOctree: (vehN, vehE ) = (4058500.04, 592010.29). 2021-02-13T00:08:51Z- TerrainMapOctree: Min Distance = 3546.63. 2021-02-13T00:08:51Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:08:51Z- There is no specific good beam that all particles have in common. 2021-02-13T00:08:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:08:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.71 2021-02-13T00:08:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:08:51Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 15.24 2021-02-13T00:08:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:08:51Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 11.34 2021-02-13T00:08:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:08:51Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.033 ExpectedMeasDiff: nan currMeas.covariance: 23.01 2021-02-13T00:08:51Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:08:51Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:08:51Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:08:51Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:08:51Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:08:51Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:08:51Z- Filter 0 No beams used in subcloud update 2021-02-13T00:08:51Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:08:51Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:08:51Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 717.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:08:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174930.80 sec. 2021-02-13T00:08:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:08:51Z- TNavBF::mmse selected from filter -1 2021-02-13T00:08:51Z- variances([0], [2], and psi_berg) = 717.456236 833.008028 0.000000 2021-02-13T00:08:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:08:51Z- LcmTrn::publishEstimates() - MLE : 4058498.70 591961.96 4.86 2021-02-13T00:08:51Z- LcmTrn::publishEstimates() - MMSE : 4058500.20 592010.46 4.86 2021-02-13T00:08:51Z- LcmTrn::publishEstimates() - COVAR: 717.46 833.01 0.83 0.00 2021-02-13T00:08:51Z- AHRS_M2 msg: 1613174931.57 epoch sec; seqNo:9955 2021-02-13T00:08:51Z- AHRS_M2 msg: 0.88 phi; -0.32 theta; -0.05 psi 2021-02-13T00:08:51Z- handleDepth()-Depth_Keller msg: 1613174931569 epoch sec; seqNo:10036; depth 4.93 2021-02-13T00:08:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174931.58 epoch sec; seqNo:10036; 4058500.58 north; 592010.46 east 2021-02-13T00:08:51Z- AHRS_M2 msg: 1613174931.97 epoch sec; seqNo:9956 2021-02-13T00:08:51Z- AHRS_M2 msg: 0.88 phi; -0.34 theta; -0.05 psi 2021-02-13T00:08:51Z- handleDepth()-Depth_Keller msg: 1613174931972 epoch sec; seqNo:10037; depth 4.99 2021-02-13T00:08:52Z- handleDvl() - RDI_Pathfinder msg: 1613174932.00 epoch sec; seqNo:4864 2021-02-13T00:08:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.69 , 99.75 , 83.52 , 121.40 2021-02-13T00:08:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.61 , -0.32 , 0.23 2021-02-13T00:08:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174932.01 epoch sec; seqNo:10037; 4058500.96 north; 592010.45 east 2021-02-13T00:08:52Z- AHRS_M2 msg: 1613174932.37 epoch sec; seqNo:9957 2021-02-13T00:08:52Z- AHRS_M2 msg: 0.88 phi; -0.35 theta; -0.04 psi 2021-02-13T00:08:52Z- handleDepth()-Depth_Keller msg: 1613174932376 epoch sec; seqNo:10038; depth 5.08 2021-02-13T00:08:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174932.40 epoch sec; seqNo:10038; 4058500.96 north; 592010.45 east 2021-02-13T00:08:52Z- AHRS_M2 msg: 1613174932.78 epoch sec; seqNo:9958 2021-02-13T00:08:52Z- AHRS_M2 msg: 0.86 phi; -0.36 theta; -0.06 psi 2021-02-13T00:08:52Z- handleDepth()-Depth_Keller msg: 1613174932788 epoch sec; seqNo:10039; depth 5.15 2021-02-13T00:08:52Z- handleDvl() - RDI_Pathfinder msg: 1613174932.80 epoch sec; seqNo:4865 2021-02-13T00:08:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.40 , 101.11 , 83.52 , 124.10 2021-02-13T00:08:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.08 , 0.44 2021-02-13T00:08:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174932.81 epoch sec; seqNo:10039; 4058500.96 north; 592010.45 east 2021-02-13T00:08:53Z- AHRS_M2 msg: 1613174933.18 epoch sec; seqNo:9959 2021-02-13T00:08:53Z- AHRS_M2 msg: 0.86 phi; -0.36 theta; -0.09 psi 2021-02-13T00:08:53Z- handleDepth()-Depth_Keller msg: 1613174933194 epoch sec; seqNo:10040; depth 5.25 2021-02-13T00:08:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:53Z- handleDvl() - RDI_Pathfinder msg: 1613174933.20 epoch sec; seqNo:4866 2021-02-13T00:08:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.08 , 0.44 2021-02-13T00:08:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174933.21 epoch sec; seqNo:10040; 4058501.34 north; 592010.45 east 2021-02-13T00:08:53Z- AHRS_M2 msg: 1613174933.58 epoch sec; seqNo:9960 2021-02-13T00:08:53Z- AHRS_M2 msg: 0.86 phi; -0.36 theta; -0.11 psi 2021-02-13T00:08:53Z- handleDepth()-Depth_Keller msg: 1613174933588 epoch sec; seqNo:10041; depth 5.33 2021-02-13T00:08:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174933.60 epoch sec; seqNo:10041; 4058501.34 north; 592010.45 east 2021-02-13T00:08:53Z- AHRS_M2 msg: 1613174933.99 epoch sec; seqNo:9961 2021-02-13T00:08:53Z- AHRS_M2 msg: 0.86 phi; -0.36 theta; -0.12 psi 2021-02-13T00:08:53Z- handleDepth()-Depth_Keller msg: 1613174933992 epoch sec; seqNo:10042; depth 5.43 2021-02-13T00:08:54Z- handleDvl() - RDI_Pathfinder msg: 1613174934.00 epoch sec; seqNo:4867 2021-02-13T00:08:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 103.95 , 83.11 , 127.76 2021-02-13T00:08:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.28 , 0.33 2021-02-13T00:08:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174934.01 epoch sec; seqNo:10042; 4058501.35 north; 592011.67 east 2021-02-13T00:08:54Z- AHRS_M2 msg: 1613174934.39 epoch sec; seqNo:9962 2021-02-13T00:08:54Z- AHRS_M2 msg: 0.87 phi; -0.36 theta; -0.13 psi 2021-02-13T00:08:54Z- handleDepth()-Depth_Keller msg: 1613174934398 epoch sec; seqNo:10043; depth 5.55 2021-02-13T00:08:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:54Z- handleDvl() - RDI_Pathfinder msg: 1613174934.40 epoch sec; seqNo:4868 2021-02-13T00:08:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.28 , 0.33 2021-02-13T00:08:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174934.41 epoch sec; seqNo:10043; 4058501.73 north; 592011.67 east 2021-02-13T00:08:54Z- AHRS_M2 msg: 1613174934.80 epoch sec; seqNo:9963 2021-02-13T00:08:54Z- AHRS_M2 msg: 0.87 phi; -0.35 theta; -0.14 psi 2021-02-13T00:08:54Z- handleDepth()-Depth_Keller msg: 1613174934810 epoch sec; seqNo:10044; depth 5.66 2021-02-13T00:08:54Z- handleDvl() - RDI_Pathfinder msg: 1613174934.82 epoch sec; seqNo:4869 2021-02-13T00:08:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.86 , 106.79 , 82.98 , 129.11 2021-02-13T00:08:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.40 , -0.08 2021-02-13T00:08:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174934.83 epoch sec; seqNo:10044; 4058501.73 north; 592011.67 east 2021-02-13T00:08:55Z- AHRS_M2 msg: 1613174935.20 epoch sec; seqNo:9964 2021-02-13T00:08:55Z- AHRS_M2 msg: 0.88 phi; -0.35 theta; -0.15 psi 2021-02-13T00:08:55Z- handleDepth()-Depth_Keller msg: 1613174935204 epoch sec; seqNo:10045; depth 5.77 2021-02-13T00:08:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174935.21 epoch sec; seqNo:10045; 4058501.75 north; 592012.89 east 2021-02-13T00:08:55Z- AHRS_M2 msg: 1613174935.60 epoch sec; seqNo:9965 2021-02-13T00:08:55Z- AHRS_M2 msg: 0.90 phi; -0.35 theta; -0.16 psi 2021-02-13T00:08:55Z- handleDepth()-Depth_Keller msg: 1613174935608 epoch sec; seqNo:10046; depth 5.88 2021-02-13T00:08:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:55Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:55Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:55Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:55Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:55Z- handleDvl() - RDI_Pathfinder msg: 1613174935.61 epoch sec; seqNo:4870 2021-02-13T00:08:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.40 , -0.08 2021-02-13T00:08:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174935.62 epoch sec; seqNo:10046; 4058502.13 north; 592012.88 east 2021-02-13T00:08:56Z- AHRS_M2 msg: 1613174936.01 epoch sec; seqNo:9966 2021-02-13T00:08:56Z- AHRS_M2 msg: 0.91 phi; -0.35 theta; -0.17 psi 2021-02-13T00:08:56Z- handleDepth()-Depth_Keller msg: 1613174936014 epoch sec; seqNo:10047; depth 5.99 2021-02-13T00:08:56Z- handleDvl() - RDI_Pathfinder msg: 1613174936.03 epoch sec; seqNo:4871 2021-02-13T00:08:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.79 , 108.21 , 82.91 , 124.58 2021-02-13T00:08:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.16 , -0.38 2021-02-13T00:08:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174936.04 epoch sec; seqNo:10047; 4058502.13 north; 592012.88 east 2021-02-13T00:08:56Z- AHRS_M2 msg: 1613174936.41 epoch sec; seqNo:9967 2021-02-13T00:08:56Z- AHRS_M2 msg: 0.92 phi; -0.34 theta; -0.17 psi 2021-02-13T00:08:56Z- LcmTrn::handleMessages() - handling 45 messages took 0.64 ms and 132 calls... 2021-02-13T00:08:56Z- waiting for 1613174934.18, time is 1613174936.41 2021-02-13T00:08:56Z- LcmTrn::updateTrn() >>>> pose time 1613174936.41, meas time 1613174936.03 <<<< 2021-02-13T00:08:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174936.03 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:08:56Z- TerrainNav::motionUpdate() - currEstimate = (4058502, 592012.88, 5.99) 2021-02-13T00:08:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:08:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:08:56Z- TNavBF::Projecting Measurements 2021-02-13T00:08:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:08:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:08:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:08:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:08:56Z- TNavFilter::4 good beams. 2021-02-13T00:08:56Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:08:56Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:08:56Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:08:56Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:08:56Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:08:56Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:08:56Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:08:56Z- TNavBF::mmse selected from filter -1 2021-02-13T00:08:56Z- TerrainMapOctree: (vehN, vehE ) = (4058501.98, 592012.70). 2021-02-13T00:08:56Z- TerrainMapOctree: Min Distance = 3543.60. 2021-02-13T00:08:56Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:08:56Z- There is no specific good beam that all particles have in common. 2021-02-13T00:08:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:08:56Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-13T00:08:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:08:56Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 18.73 2021-02-13T00:08:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:08:56Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 11.00 2021-02-13T00:08:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:08:56Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.030 ExpectedMeasDiff: nan currMeas.covariance: 24.83 2021-02-13T00:08:56Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:08:56Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:08:56Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:08:56Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:08:56Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:08:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:08:56Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:08:56Z- Filter 0 No beams used in subcloud update 2021-02-13T00:08:56Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:08:56Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:08:56Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 717.4 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:08:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174936.03 sec. 2021-02-13T00:08:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:08:56Z- TNavBF::mmse selected from filter -1 2021-02-13T00:08:56Z- variances([0], [2], and psi_berg) = 717.385147 833.007983 0.000000 2021-02-13T00:08:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:08:56Z- LcmTrn::publishEstimates() - MLE : 4058500.63 591964.38 5.99 2021-02-13T00:08:56Z- LcmTrn::publishEstimates() - MMSE : 4058502.13 592012.88 5.99 2021-02-13T00:08:56Z- LcmTrn::publishEstimates() - COVAR: 717.39 833.01 0.81 0.00 2021-02-13T00:08:56Z- handleDepth()-Depth_Keller msg: 1613174936416 epoch sec; seqNo:10048; depth 6.10 2021-02-13T00:08:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174936.43 epoch sec; seqNo:10048; 4058502.50 north; 592012.88 east 2021-02-13T00:08:56Z- AHRS_M2 msg: 1613174936.82 epoch sec; seqNo:9968 2021-02-13T00:08:56Z- AHRS_M2 msg: 0.93 phi; -0.34 theta; -0.16 psi 2021-02-13T00:08:56Z- handleDepth()-Depth_Keller msg: 1613174936821 epoch sec; seqNo:10049; depth 6.22 2021-02-13T00:08:56Z- handleDvl() - RDI_Pathfinder msg: 1613174936.83 epoch sec; seqNo:4872 2021-02-13T00:08:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.90 , 109.76 , 82.98 , 121.67 2021-02-13T00:08:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.20 , -0.31 2021-02-13T00:08:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174936.86 epoch sec; seqNo:10049; 4058502.52 north; 592014.10 east 2021-02-13T00:08:57Z- AHRS_M2 msg: 1613174937.22 epoch sec; seqNo:9969 2021-02-13T00:08:57Z- AHRS_M2 msg: 0.94 phi; -0.34 theta; -0.15 psi 2021-02-13T00:08:57Z- handleDepth()-Depth_Keller msg: 1613174937225 epoch sec; seqNo:10050; depth 6.32 2021-02-13T00:08:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174937.24 epoch sec; seqNo:10050; 4058502.90 north; 592014.10 east 2021-02-13T00:08:57Z- AHRS_M2 msg: 1613174937.64 epoch sec; seqNo:9970 2021-02-13T00:08:57Z- AHRS_M2 msg: 0.95 phi; -0.35 theta; -0.13 psi 2021-02-13T00:08:57Z- handleDepth()-Depth_Keller msg: 1613174937641 epoch sec; seqNo:10051; depth 6.44 2021-02-13T00:08:57Z- handleDvl() - RDI_Pathfinder msg: 1613174937.66 epoch sec; seqNo:4873 2021-02-13T00:08:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 106.92 , 78.91 , 124.78 2021-02-13T00:08:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.63 , -0.37 , 0.04 2021-02-13T00:08:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174937.69 epoch sec; seqNo:10051; 4058503.27 north; 592014.09 east 2021-02-13T00:08:58Z- AHRS_M2 msg: 1613174938.03 epoch sec; seqNo:9971 2021-02-13T00:08:58Z- AHRS_M2 msg: 0.95 phi; -0.35 theta; -0.12 psi 2021-02-13T00:08:58Z- handleDepth()-Depth_Keller msg: 1613174938032 epoch sec; seqNo:10052; depth 6.57 2021-02-13T00:08:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:58Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:58Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:58Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:58Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:58Z- handleDvl() - RDI_Pathfinder msg: 1613174938.03 epoch sec; seqNo:4874 2021-02-13T00:08:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.63 , -0.37 , 0.04 2021-02-13T00:08:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174938.05 epoch sec; seqNo:10052; 4058503.27 north; 592014.09 east 2021-02-13T00:08:58Z- AHRS_M2 msg: 1613174938.43 epoch sec; seqNo:9972 2021-02-13T00:08:58Z- AHRS_M2 msg: 0.95 phi; -0.36 theta; -0.11 psi 2021-02-13T00:08:58Z- handleDepth()-Depth_Keller msg: 1613174938436 epoch sec; seqNo:10053; depth 6.66 2021-02-13T00:08:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174938.44 epoch sec; seqNo:10053; 4058503.65 north; 592014.09 east 2021-02-13T00:08:58Z- AHRS_M2 msg: 1613174938.84 epoch sec; seqNo:9973 2021-02-13T00:08:58Z- AHRS_M2 msg: 0.94 phi; -0.36 theta; -0.10 psi 2021-02-13T00:08:58Z- handleDepth()-Depth_Keller msg: 1613174938840 epoch sec; seqNo:10054; depth 6.76 2021-02-13T00:08:58Z- handleDvl() - RDI_Pathfinder msg: 1613174938.85 epoch sec; seqNo:4875 2021-02-13T00:08:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.93 , 105.37 , 81.56 , 124.71 2021-02-13T00:08:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , -0.19 , 0.35 2021-02-13T00:08:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174938.86 epoch sec; seqNo:10054; 4058503.65 north; 592014.09 east 2021-02-13T00:08:59Z- AHRS_M2 msg: 1613174939.24 epoch sec; seqNo:9974 2021-02-13T00:08:59Z- AHRS_M2 msg: 0.93 phi; -0.36 theta; -0.11 psi 2021-02-13T00:08:59Z- handleDepth()-Depth_Keller msg: 1613174939246 epoch sec; seqNo:10055; depth 6.86 2021-02-13T00:08:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:08:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:08:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:08:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:08:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:08:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:08:59Z- handleDvl() - RDI_Pathfinder msg: 1613174939.25 epoch sec; seqNo:4876 2021-02-13T00:08:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:08:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , -0.19 , 0.35 2021-02-13T00:08:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174939.26 epoch sec; seqNo:10055; 4058504.03 north; 592014.08 east 2021-02-13T00:08:59Z- AHRS_M2 msg: 1613174939.65 epoch sec; seqNo:9975 2021-02-13T00:08:59Z- AHRS_M2 msg: 0.93 phi; -0.36 theta; -0.12 psi 2021-02-13T00:08:59Z- handleDepth()-Depth_Keller msg: 1613174939652 epoch sec; seqNo:10056; depth 6.97 2021-02-13T00:08:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174939.68 epoch sec; seqNo:10056; 4058504.03 north; 592014.08 east 2021-02-13T00:09:00Z- AHRS_M2 msg: 1613174940.05 epoch sec; seqNo:9976 2021-02-13T00:09:00Z- AHRS_M2 msg: 0.94 phi; -0.36 theta; -0.12 psi 2021-02-13T00:09:00Z- handleDepth()-Depth_Keller msg: 1613174940052 epoch sec; seqNo:10057; depth 7.08 2021-02-13T00:09:00Z- handleDvl() - RDI_Pathfinder msg: 1613174940.06 epoch sec; seqNo:4877 2021-02-13T00:09:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.95 , 103.88 , 81.56 , 130.67 2021-02-13T00:09:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.14 , 0.36 2021-02-13T00:09:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174940.10 epoch sec; seqNo:10057; 4058504.42 north; 592015.30 east 2021-02-13T00:09:00Z- AHRS_M2 msg: 1613174940.45 epoch sec; seqNo:9977 2021-02-13T00:09:00Z- AHRS_M2 msg: 0.94 phi; -0.35 theta; -0.12 psi 2021-02-13T00:09:00Z- handleDepth()-Depth_Keller msg: 1613174940456 epoch sec; seqNo:10058; depth 7.20 2021-02-13T00:09:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:00Z- handleDvl() - RDI_Pathfinder msg: 1613174940.46 epoch sec; seqNo:4878 2021-02-13T00:09:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.14 , 0.36 2021-02-13T00:09:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174940.47 epoch sec; seqNo:10058; 4058504.42 north; 592015.30 east 2021-02-13T00:09:00Z- AHRS_M2 msg: 1613174940.86 epoch sec; seqNo:9978 2021-02-13T00:09:00Z- AHRS_M2 msg: 0.95 phi; -0.35 theta; -0.11 psi 2021-02-13T00:09:00Z- handleDepth()-Depth_Keller msg: 1613174940862 epoch sec; seqNo:10059; depth 7.32 2021-02-13T00:09:00Z- handleDvl() - RDI_Pathfinder msg: 1613174940.87 epoch sec; seqNo:4879 2021-02-13T00:09:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.86 , 105.30 , 81.49 , 127.62 2021-02-13T00:09:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.28 , 0.10 2021-02-13T00:09:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174940.88 epoch sec; seqNo:10059; 4058504.42 north; 592015.30 east 2021-02-13T00:09:01Z- AHRS_M2 msg: 1613174941.26 epoch sec; seqNo:9979 2021-02-13T00:09:01Z- AHRS_M2 msg: 0.96 phi; -0.35 theta; -0.11 psi 2021-02-13T00:09:01Z- handleDepth()-Depth_Keller msg: 1613174941264 epoch sec; seqNo:10060; depth 7.45 2021-02-13T00:09:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174941.28 epoch sec; seqNo:10060; 4058504.42 north; 592015.30 east 2021-02-13T00:09:01Z- LcmTrn::handleMessages() - handling 46 messages took 0.68 ms and 133 calls... 2021-02-13T00:09:01Z- waiting for 1613174939.41, time is 1613174941.28 2021-02-13T00:09:01Z- LcmTrn::updateTrn() >>>> pose time 1613174941.28, meas time 1613174940.87 <<<< 2021-02-13T00:09:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174940.87 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:09:01Z- TerrainNav::motionUpdate() - currEstimate = (4058504, 592015.30, 7.45) 2021-02-13T00:09:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:09:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:09:01Z- TNavBF::Projecting Measurements 2021-02-13T00:09:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:09:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:09:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:09:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:09:01Z- TNavFilter::4 good beams. 2021-02-13T00:09:01Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:09:01Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:09:01Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:09:01Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:09:01Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:09:01Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:09:01Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:09:01Z- TNavBF::mmse selected from filter -1 2021-02-13T00:09:01Z- TerrainMapOctree: (vehN, vehE ) = (4058504.23, 592015.10). 2021-02-13T00:09:01Z- TerrainMapOctree: Min Distance = 3540.34. 2021-02-13T00:09:01Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:09:01Z- There is no specific good beam that all particles have in common. 2021-02-13T00:09:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:09:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-13T00:09:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:09:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 17.74 2021-02-13T00:09:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:09:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 10.62 2021-02-13T00:09:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:09:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.029 ExpectedMeasDiff: nan currMeas.covariance: 26.06 2021-02-13T00:09:01Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:09:01Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:09:01Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:09:01Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:09:01Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:09:01Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:09:01Z- Filter 0 No beams used in subcloud update 2021-02-13T00:09:01Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:09:01Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:09:01Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 717.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:09:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174940.87 sec. 2021-02-13T00:09:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:09:01Z- TNavBF::mmse selected from filter -1 2021-02-13T00:09:01Z- variances([0], [2], and psi_berg) = 717.310602 833.007936 0.000000 2021-02-13T00:09:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:09:01Z- LcmTrn::publishEstimates() - MLE : 4058502.92 591966.80 7.45 2021-02-13T00:09:01Z- LcmTrn::publishEstimates() - MMSE : 4058504.42 592015.30 7.45 2021-02-13T00:09:01Z- LcmTrn::publishEstimates() - COVAR: 717.31 833.01 0.81 0.00 2021-02-13T00:09:01Z- AHRS_M2 msg: 1613174941.66 epoch sec; seqNo:9980 2021-02-13T00:09:01Z- AHRS_M2 msg: 0.97 phi; -0.35 theta; -0.12 psi 2021-02-13T00:09:01Z- handleDepth()-Depth_Keller msg: 1613174941673 epoch sec; seqNo:10061; depth 7.57 2021-02-13T00:09:01Z- handleDvl() - RDI_Pathfinder msg: 1613174941.68 epoch sec; seqNo:4880 2021-02-13T00:09:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.39 , 103.81 , 79.72 , 123.16 2021-02-13T00:09:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.21 , -0.13 2021-02-13T00:09:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174941.71 epoch sec; seqNo:10061; 4058504.82 north; 592016.52 east 2021-02-13T00:09:02Z- AHRS_M2 msg: 1613174942.07 epoch sec; seqNo:9981 2021-02-13T00:09:02Z- AHRS_M2 msg: 0.97 phi; -0.35 theta; -0.13 psi 2021-02-13T00:09:02Z- handleDepth()-Depth_Keller msg: 1613174942072 epoch sec; seqNo:10062; depth 7.70 2021-02-13T00:09:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174942.08 epoch sec; seqNo:10062; 4058504.82 north; 592016.52 east 2021-02-13T00:09:02Z- AHRS_M2 msg: 1613174942.47 epoch sec; seqNo:9982 2021-02-13T00:09:02Z- AHRS_M2 msg: 0.98 phi; -0.35 theta; -0.13 psi 2021-02-13T00:09:02Z- handleDepth()-Depth_Keller msg: 1613174942478 epoch sec; seqNo:10063; depth 7.83 2021-02-13T00:09:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174942.49 epoch sec; seqNo:10063; 4058505.20 north; 592016.51 east 2021-02-13T00:09:02Z- AHRS_M2 msg: 1613174942.88 epoch sec; seqNo:9983 2021-02-13T00:09:02Z- AHRS_M2 msg: 0.98 phi; -0.35 theta; -0.14 psi 2021-02-13T00:09:02Z- handleDepth()-Depth_Keller msg: 1613174942881 epoch sec; seqNo:10064; depth 7.97 2021-02-13T00:09:02Z- handleDvl() - RDI_Pathfinder msg: 1613174942.90 epoch sec; seqNo:4881 2021-02-13T00:09:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.05 , 105.16 , 80.81 , 122.75 2021-02-13T00:09:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.12 , -0.30 2021-02-13T00:09:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174942.91 epoch sec; seqNo:10064; 4058505.21 north; 592017.73 east 2021-02-13T00:09:03Z- AHRS_M2 msg: 1613174943.28 epoch sec; seqNo:9984 2021-02-13T00:09:03Z- AHRS_M2 msg: 0.98 phi; -0.35 theta; -0.14 psi 2021-02-13T00:09:03Z- handleDepth()-Depth_Keller msg: 1613174943287 epoch sec; seqNo:10065; depth 8.09 2021-02-13T00:09:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174943.32 epoch sec; seqNo:10065; 4058505.59 north; 592017.73 east 2021-02-13T00:09:03Z- AHRS_M2 msg: 1613174943.68 epoch sec; seqNo:9985 2021-02-13T00:09:03Z- AHRS_M2 msg: 0.98 phi; -0.35 theta; -0.15 psi 2021-02-13T00:09:03Z- handleDepth()-Depth_Keller msg: 1613174943688 epoch sec; seqNo:10066; depth 8.21 2021-02-13T00:09:03Z- handleDvl() - RDI_Pathfinder msg: 1613174943.70 epoch sec; seqNo:4882 2021-02-13T00:09:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.55 , 105.43 , 0.00 , 123.29 2021-02-13T00:09:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.03 , -0.38 2021-02-13T00:09:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174943.71 epoch sec; seqNo:10066; 4058505.59 north; 592017.73 east 2021-02-13T00:09:04Z- AHRS_M2 msg: 1613174944.09 epoch sec; seqNo:9986 2021-02-13T00:09:04Z- AHRS_M2 msg: 0.98 phi; -0.34 theta; -0.16 psi 2021-02-13T00:09:04Z- handleDepth()-Depth_Keller msg: 1613174944097 epoch sec; seqNo:10067; depth 8.35 2021-02-13T00:09:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:04Z- handleDvl() - RDI_Pathfinder msg: 1613174944.10 epoch sec; seqNo:4883 2021-02-13T00:09:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.03 , -0.38 2021-02-13T00:09:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174944.11 epoch sec; seqNo:10067; 4058505.97 north; 592017.72 east 2021-02-13T00:09:04Z- AHRS_M2 msg: 1613174944.49 epoch sec; seqNo:9987 2021-02-13T00:09:04Z- AHRS_M2 msg: 0.98 phi; -0.34 theta; -0.18 psi 2021-02-13T00:09:04Z- handleDepth()-Depth_Keller msg: 1613174944496 epoch sec; seqNo:10068; depth 8.48 2021-02-13T00:09:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174944.51 epoch sec; seqNo:10068; 4058505.97 north; 592017.72 east 2021-02-13T00:09:04Z- AHRS_M2 msg: 1613174944.90 epoch sec; seqNo:9988 2021-02-13T00:09:04Z- AHRS_M2 msg: 0.98 phi; -0.34 theta; -0.19 psi 2021-02-13T00:09:04Z- handleDepth()-Depth_Keller msg: 1613174944900 epoch sec; seqNo:10069; depth 8.59 2021-02-13T00:09:04Z- handleDvl() - RDI_Pathfinder msg: 1613174944.91 epoch sec; seqNo:4884 2021-02-13T00:09:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.59 , 108.00 , 77.27 , 118.42 2021-02-13T00:09:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.26 , -0.26 2021-02-13T00:09:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174944.93 epoch sec; seqNo:10069; 4058506.36 north; 592018.94 east 2021-02-13T00:09:05Z- AHRS_M2 msg: 1613174945.30 epoch sec; seqNo:9989 2021-02-13T00:09:05Z- AHRS_M2 msg: 0.97 phi; -0.33 theta; -0.19 psi 2021-02-13T00:09:05Z- handleDepth()-Depth_Keller msg: 1613174945304 epoch sec; seqNo:10070; depth 8.72 2021-02-13T00:09:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:05Z- handleDvl() - RDI_Pathfinder msg: 1613174945.31 epoch sec; seqNo:4885 2021-02-13T00:09:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.26 , -0.26 2021-02-13T00:09:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174945.32 epoch sec; seqNo:10070; 4058506.74 north; 592018.94 east 2021-02-13T00:09:05Z- AHRS_M2 msg: 1613174945.70 epoch sec; seqNo:9990 2021-02-13T00:09:05Z- AHRS_M2 msg: 0.97 phi; -0.33 theta; -0.18 psi 2021-02-13T00:09:05Z- handleDepth()-Depth_Keller msg: 1613174945717 epoch sec; seqNo:10071; depth 8.84 2021-02-13T00:09:05Z- handleDvl() - RDI_Pathfinder msg: 1613174945.73 epoch sec; seqNo:4886 2021-02-13T00:09:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 107.87 , 80.81 , 120.04 2021-02-13T00:09:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.29 , 0.08 2021-02-13T00:09:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174945.74 epoch sec; seqNo:10071; 4058506.74 north; 592018.94 east 2021-02-13T00:09:06Z- AHRS_M2 msg: 1613174946.11 epoch sec; seqNo:9991 2021-02-13T00:09:06Z- AHRS_M2 msg: 0.96 phi; -0.33 theta; -0.17 psi 2021-02-13T00:09:06Z- handleDepth()-Depth_Keller msg: 1613174946134 epoch sec; seqNo:10072; depth 8.97 2021-02-13T00:09:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174946.15 epoch sec; seqNo:10072; 4058507.12 north; 592018.93 east 2021-02-13T00:09:06Z- AHRS_M2 msg: 1613174946.51 epoch sec; seqNo:9992 2021-02-13T00:09:06Z- AHRS_M2 msg: 0.94 phi; -0.32 theta; -0.17 psi 2021-02-13T00:09:06Z- handleDepth()-Depth_Keller msg: 1613174946516 epoch sec; seqNo:10073; depth 9.08 2021-02-13T00:09:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:06Z- handleDvl() - RDI_Pathfinder msg: 1613174946.52 epoch sec; seqNo:4887 2021-02-13T00:09:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.29 , 0.08 2021-02-13T00:09:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174946.53 epoch sec; seqNo:10073; 4058507.49 north; 592018.93 east 2021-02-13T00:09:06Z- LcmTrn::handleMessages() - handling 47 messages took 0.66 ms and 134 calls... 2021-02-13T00:09:06Z- waiting for 1613174944.28, time is 1613174946.53 2021-02-13T00:09:06Z- LcmTrn::updateTrn() equating pose 1613174946.53 and meas times 1613174946.53 2021-02-13T00:09:06Z- LcmTrn::updateTrn() >>>> pose time 1613174946.53, meas time 1613174946.53 <<<< 2021-02-13T00:09:06Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174946.53, ping # 0. 2021-02-13T00:09:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174946.53 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:09:06Z- TerrainNav::motionUpdate() - currEstimate = (4058507, 592018.93, 9.08) 2021-02-13T00:09:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:09:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:09:06Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:09:06Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:09:06Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174946.53 sec.; No relevant navigation data available 2021-02-13T00:09:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:09:06Z- TNavBF::mmse selected from filter -1 2021-02-13T00:09:06Z- variances([0], [2], and psi_berg) = 717.239269 833.007891 0.000000 2021-02-13T00:09:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:09:06Z- LcmTrn::publishEstimates() - MLE : 4058505.99 591970.43 9.08 2021-02-13T00:09:06Z- LcmTrn::publishEstimates() - MMSE : 4058507.49 592018.93 9.08 2021-02-13T00:09:06Z- LcmTrn::publishEstimates() - COVAR: 717.24 833.01 1.05 0.00 2021-02-13T00:09:06Z- AHRS_M2 msg: 1613174946.92 epoch sec; seqNo:9993 2021-02-13T00:09:06Z- AHRS_M2 msg: 0.93 phi; -0.32 theta; -0.16 psi 2021-02-13T00:09:06Z- handleDepth()-Depth_Keller msg: 1613174946920 epoch sec; seqNo:10074; depth 9.18 2021-02-13T00:09:06Z- handleDvl() - RDI_Pathfinder msg: 1613174946.93 epoch sec; seqNo:4888 2021-02-13T00:09:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.44 , 106.79 , 81.49 , 117.20 2021-02-13T00:09:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.11 , 0.34 2021-02-13T00:09:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174946.94 epoch sec; seqNo:10074; 4058507.49 north; 592018.93 east 2021-02-13T00:09:07Z- AHRS_M2 msg: 1613174947.32 epoch sec; seqNo:9994 2021-02-13T00:09:07Z- AHRS_M2 msg: 0.92 phi; -0.31 theta; -0.16 psi 2021-02-13T00:09:07Z- handleDepth()-Depth_Keller msg: 1613174947326 epoch sec; seqNo:10075; depth 9.32 2021-02-13T00:09:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174947.34 epoch sec; seqNo:10075; 4058507.87 north; 592018.93 east 2021-02-13T00:09:07Z- AHRS_M2 msg: 1613174947.74 epoch sec; seqNo:9995 2021-02-13T00:09:07Z- AHRS_M2 msg: 0.91 phi; -0.31 theta; -0.16 psi 2021-02-13T00:09:07Z- handleDepth()-Depth_Keller msg: 1613174947746 epoch sec; seqNo:10076; depth 9.41 2021-02-13T00:09:07Z- handleDvl() - RDI_Pathfinder msg: 1613174947.76 epoch sec; seqNo:4889 2021-02-13T00:09:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.71 , 103.95 , 80.95 , 117.47 2021-02-13T00:09:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.15 , 0.31 2021-02-13T00:09:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174947.77 epoch sec; seqNo:10076; 4058507.89 north; 592020.15 east 2021-02-13T00:09:08Z- AHRS_M2 msg: 1613174948.13 epoch sec; seqNo:9996 2021-02-13T00:09:08Z- AHRS_M2 msg: 0.90 phi; -0.30 theta; -0.17 psi 2021-02-13T00:09:08Z- handleDepth()-Depth_Keller msg: 1613174948137 epoch sec; seqNo:10077; depth 9.54 2021-02-13T00:09:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174948.18 epoch sec; seqNo:10077; 4058507.89 north; 592020.15 east 2021-02-13T00:09:08Z- AHRS_M2 msg: 1613174948.53 epoch sec; seqNo:9997 2021-02-13T00:09:08Z- AHRS_M2 msg: 0.89 phi; -0.29 theta; -0.18 psi 2021-02-13T00:09:08Z- handleDepth()-Depth_Keller msg: 1613174948546 epoch sec; seqNo:10078; depth 9.64 2021-02-13T00:09:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174948.59 epoch sec; seqNo:10078; 4058508.27 north; 592020.14 east 2021-02-13T00:09:08Z- AHRS_M2 msg: 1613174948.94 epoch sec; seqNo:9998 2021-02-13T00:09:08Z- AHRS_M2 msg: 0.90 phi; -0.29 theta; -0.19 psi 2021-02-13T00:09:08Z- handleDepth()-Depth_Keller msg: 1613174948952 epoch sec; seqNo:10079; depth 9.74 2021-02-13T00:09:08Z- handleDvl() - RDI_Pathfinder msg: 1613174948.97 epoch sec; seqNo:4890 2021-02-13T00:09:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.01 , 79.32 , 114.84 2021-02-13T00:09:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.26 , 0.04 2021-02-13T00:09:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174948.98 epoch sec; seqNo:10079; 4058508.27 north; 592020.14 east 2021-02-13T00:09:09Z- AHRS_M2 msg: 1613174949.34 epoch sec; seqNo:9999 2021-02-13T00:09:09Z- AHRS_M2 msg: 0.90 phi; -0.29 theta; -0.19 psi 2021-02-13T00:09:09Z- handleDepth()-Depth_Keller msg: 1613174949344 epoch sec; seqNo:10080; depth 9.85 2021-02-13T00:09:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174949.37 epoch sec; seqNo:10080; 4058508.28 north; 592021.36 east 2021-02-13T00:09:09Z- AHRS_M2 msg: 1613174949.75 epoch sec; seqNo:10000 2021-02-13T00:09:09Z- AHRS_M2 msg: 0.91 phi; -0.28 theta; -0.20 psi 2021-02-13T00:09:09Z- handleDepth()-Depth_Keller msg: 1613174949755 epoch sec; seqNo:10081; depth 9.96 2021-02-13T00:09:09Z- handleDvl() - RDI_Pathfinder msg: 1613174949.77 epoch sec; seqNo:4891 2021-02-13T00:09:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.94 , 106.58 , 80.88 , 114.70 2021-02-13T00:09:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.14 , -0.16 2021-02-13T00:09:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174949.79 epoch sec; seqNo:10081; 4058508.66 north; 592021.36 east 2021-02-13T00:09:10Z- AHRS_M2 msg: 1613174950.16 epoch sec; seqNo:10001 2021-02-13T00:09:10Z- AHRS_M2 msg: 0.91 phi; -0.28 theta; -0.18 psi 2021-02-13T00:09:10Z- handleDepth()-Depth_Keller msg: 1613174950166 epoch sec; seqNo:10082; depth 10.08 2021-02-13T00:09:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174950.18 epoch sec; seqNo:10082; 4058508.66 north; 592021.36 east 2021-02-13T00:09:10Z- AHRS_M2 msg: 1613174950.56 epoch sec; seqNo:10002 2021-02-13T00:09:10Z- AHRS_M2 msg: 0.91 phi; -0.28 theta; -0.17 psi 2021-02-13T00:09:10Z- handleDepth()-Depth_Keller msg: 1613174950561 epoch sec; seqNo:10083; depth 10.18 2021-02-13T00:09:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174950.57 epoch sec; seqNo:10083; 4058509.05 north; 592022.58 east 2021-02-13T00:09:10Z- AHRS_M2 msg: 1613174950.96 epoch sec; seqNo:10003 2021-02-13T00:09:10Z- AHRS_M2 msg: 0.91 phi; -0.28 theta; -0.17 psi 2021-02-13T00:09:10Z- handleDepth()-Depth_Keller msg: 1613174950965 epoch sec; seqNo:10084; depth 10.29 2021-02-13T00:09:10Z- handleDvl() - RDI_Pathfinder msg: 1613174950.98 epoch sec; seqNo:4892 2021-02-13T00:09:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.08 , 81.08 , 112.20 2021-02-13T00:09:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , -0.17 2021-02-13T00:09:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174950.99 epoch sec; seqNo:10084; 4058509.43 north; 592022.57 east 2021-02-13T00:09:11Z- AHRS_M2 msg: 1613174951.36 epoch sec; seqNo:10004 2021-02-13T00:09:11Z- AHRS_M2 msg: 0.91 phi; -0.28 theta; -0.16 psi 2021-02-13T00:09:11Z- handleDepth()-Depth_Keller msg: 1613174951369 epoch sec; seqNo:10085; depth 10.41 2021-02-13T00:09:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:11Z- handleDvl() - RDI_Pathfinder msg: 1613174951.37 epoch sec; seqNo:4893 2021-02-13T00:09:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , -0.17 2021-02-13T00:09:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174951.38 epoch sec; seqNo:10085; 4058509.43 north; 592022.57 east 2021-02-13T00:09:11Z- LcmTrn::handleMessages() - handling 42 messages took 0.59 ms and 131 calls... 2021-02-13T00:09:11Z- waiting for 1613174949.53, time is 1613174951.38 2021-02-13T00:09:11Z- LcmTrn::updateTrn() equating pose 1613174951.38 and meas times 1613174951.38 2021-02-13T00:09:11Z- LcmTrn::updateTrn() >>>> pose time 1613174951.38, meas time 1613174951.38 <<<< 2021-02-13T00:09:11Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174951.38, ping # 0. 2021-02-13T00:09:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174951.38 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:09:11Z- TerrainNav::motionUpdate() - currEstimate = (4058509, 592022.57, 10.41) 2021-02-13T00:09:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:09:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:09:11Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:09:11Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:09:11Z- TNavBF::Projecting Measurements 2021-02-13T00:09:11Z- TNavFilter::Measurement from time = 1613174951.38 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:09:11Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:09:11Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:09:11Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:09:11Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:09:11Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:09:11Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:09:11Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:09:11Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 717.2 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:09:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:09:11Z- TNavBF::mmse selected from filter -1 2021-02-13T00:09:11Z- variances([0], [2], and psi_berg) = 717.172888 833.007849 0.000000 2021-02-13T00:09:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:09:11Z- LcmTrn::publishEstimates() - MLE : 4058507.93 591974.07 10.41 2021-02-13T00:09:11Z- LcmTrn::publishEstimates() - MMSE : 4058509.43 592022.57 10.41 2021-02-13T00:09:11Z- LcmTrn::publishEstimates() - COVAR: 717.17 833.01 1.26 0.00 2021-02-13T00:09:11Z- AHRS_M2 msg: 1613174951.76 epoch sec; seqNo:10005 2021-02-13T00:09:11Z- AHRS_M2 msg: 0.91 phi; -0.28 theta; -0.16 psi 2021-02-13T00:09:11Z- handleDepth()-Depth_Keller msg: 1613174951773 epoch sec; seqNo:10086; depth 10.50 2021-02-13T00:09:11Z- handleDvl() - RDI_Pathfinder msg: 1613174951.78 epoch sec; seqNo:4894 2021-02-13T00:09:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.94 , 103.88 , 79.53 , 112.00 2021-02-13T00:09:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , -0.07 2021-02-13T00:09:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174951.80 epoch sec; seqNo:10086; 4058509.81 north; 592022.57 east 2021-02-13T00:09:12Z- AHRS_M2 msg: 1613174952.17 epoch sec; seqNo:10006 2021-02-13T00:09:12Z- AHRS_M2 msg: 0.91 phi; -0.27 theta; -0.16 psi 2021-02-13T00:09:12Z- handleDepth()-Depth_Keller msg: 1613174952179 epoch sec; seqNo:10087; depth 10.61 2021-02-13T00:09:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174952.19 epoch sec; seqNo:10087; 4058510.19 north; 592022.56 east 2021-02-13T00:09:12Z- AHRS_M2 msg: 1613174952.57 epoch sec; seqNo:10007 2021-02-13T00:09:12Z- AHRS_M2 msg: 0.90 phi; -0.27 theta; -0.16 psi 2021-02-13T00:09:12Z- handleDepth()-Depth_Keller msg: 1613174952576 epoch sec; seqNo:10088; depth 10.73 2021-02-13T00:09:12Z- handleDvl() - RDI_Pathfinder msg: 1613174952.60 epoch sec; seqNo:4895 2021-02-13T00:09:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.16 , 77.27 , 109.63 2021-02-13T00:09:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , 0.01 2021-02-13T00:09:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174952.61 epoch sec; seqNo:10088; 4058510.20 north; 592023.78 east 2021-02-13T00:09:12Z- AHRS_M2 msg: 1613174952.98 epoch sec; seqNo:10008 2021-02-13T00:09:12Z- AHRS_M2 msg: 0.90 phi; -0.27 theta; -0.15 psi 2021-02-13T00:09:12Z- handleDepth()-Depth_Keller msg: 1613174952980 epoch sec; seqNo:10089; depth 10.84 2021-02-13T00:09:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174952.99 epoch sec; seqNo:10089; 4058510.58 north; 592023.78 east 2021-02-13T00:09:13Z- AHRS_M2 msg: 1613174953.38 epoch sec; seqNo:10009 2021-02-13T00:09:13Z- AHRS_M2 msg: 0.90 phi; -0.26 theta; -0.15 psi 2021-02-13T00:09:13Z- handleDepth()-Depth_Keller msg: 1613174953384 epoch sec; seqNo:10090; depth 10.94 2021-02-13T00:09:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174953.39 epoch sec; seqNo:10090; 4058510.58 north; 592023.78 east 2021-02-13T00:09:13Z- AHRS_M2 msg: 1613174953.78 epoch sec; seqNo:10010 2021-02-13T00:09:13Z- AHRS_M2 msg: 0.90 phi; -0.26 theta; -0.15 psi 2021-02-13T00:09:13Z- handleDepth()-Depth_Keller msg: 1613174953790 epoch sec; seqNo:10091; depth 11.05 2021-02-13T00:09:13Z- handleDvl() - RDI_Pathfinder msg: 1613174953.80 epoch sec; seqNo:4896 2021-02-13T00:09:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.21 , 100.09 , 79.80 , 106.86 2021-02-13T00:09:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.00 , 0.01 2021-02-13T00:09:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174953.81 epoch sec; seqNo:10091; 4058510.96 north; 592023.78 east 2021-02-13T00:09:14Z- AHRS_M2 msg: 1613174954.19 epoch sec; seqNo:10011 2021-02-13T00:09:14Z- AHRS_M2 msg: 0.90 phi; -0.26 theta; -0.14 psi 2021-02-13T00:09:14Z- handleDepth()-Depth_Keller msg: 1613174954193 epoch sec; seqNo:10092; depth 11.15 2021-02-13T00:09:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174954.22 epoch sec; seqNo:10092; 4058510.96 north; 592023.78 east 2021-02-13T00:09:14Z- AHRS_M2 msg: 1613174954.61 epoch sec; seqNo:10012 2021-02-13T00:09:14Z- AHRS_M2 msg: 0.90 phi; -0.26 theta; -0.14 psi 2021-02-13T00:09:14Z- handleDepth()-Depth_Keller msg: 1613174954611 epoch sec; seqNo:10093; depth 11.24 2021-02-13T00:09:14Z- handleDvl() - RDI_Pathfinder msg: 1613174954.63 epoch sec; seqNo:4897 2021-02-13T00:09:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.65 , 80.07 , 108.48 2021-02-13T00:09:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.05 , 0.00 2021-02-13T00:09:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174954.64 epoch sec; seqNo:10093; 4058511.35 north; 592024.99 east 2021-02-13T00:09:14Z- AHRS_M2 msg: 1613174955.00 epoch sec; seqNo:10013 2021-02-13T00:09:14Z- AHRS_M2 msg: 0.89 phi; -0.25 theta; -0.15 psi 2021-02-13T00:09:15Z- handleDepth()-Depth_Keller msg: 1613174955000 epoch sec; seqNo:10094; depth 11.36 2021-02-13T00:09:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:15Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:15Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:15Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:15Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:15Z- handleDvl() - RDI_Pathfinder msg: 1613174955.00 epoch sec; seqNo:4898 2021-02-13T00:09:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.05 , 0.00 2021-02-13T00:09:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174955.01 epoch sec; seqNo:10094; 4058511.35 north; 592024.99 east 2021-02-13T00:09:15Z- AHRS_M2 msg: 1613174955.40 epoch sec; seqNo:10014 2021-02-13T00:09:15Z- AHRS_M2 msg: 0.89 phi; -0.25 theta; -0.16 psi 2021-02-13T00:09:15Z- handleDepth()-Depth_Keller msg: 1613174955405 epoch sec; seqNo:10095; depth 11.44 2021-02-13T00:09:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174955.41 epoch sec; seqNo:10095; 4058511.73 north; 592024.99 east 2021-02-13T00:09:15Z- AHRS_M2 msg: 1613174955.80 epoch sec; seqNo:10015 2021-02-13T00:09:15Z- AHRS_M2 msg: 0.89 phi; -0.25 theta; -0.16 psi 2021-02-13T00:09:15Z- handleDepth()-Depth_Keller msg: 1613174955808 epoch sec; seqNo:10096; depth 11.52 2021-02-13T00:09:15Z- handleDvl() - RDI_Pathfinder msg: 1613174955.83 epoch sec; seqNo:4899 2021-02-13T00:09:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 100.16 , 0.00 , 106.92 2021-02-13T00:09:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.06 , -0.02 2021-02-13T00:09:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174955.84 epoch sec; seqNo:10096; 4058511.73 north; 592024.99 east 2021-02-13T00:09:16Z- AHRS_M2 msg: 1613174956.21 epoch sec; seqNo:10016 2021-02-13T00:09:16Z- AHRS_M2 msg: 0.89 phi; -0.25 theta; -0.17 psi 2021-02-13T00:09:16Z- handleDepth()-Depth_Keller msg: 1613174956213 epoch sec; seqNo:10097; depth 11.63 2021-02-13T00:09:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:16Z- handleDvl() - RDI_Pathfinder msg: 1613174956.21 epoch sec; seqNo:4900 2021-02-13T00:09:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.06 , -0.02 2021-02-13T00:09:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174956.23 epoch sec; seqNo:10097; 4058512.12 north; 592026.21 east 2021-02-13T00:09:16Z- AHRS_M2 msg: 1613174956.61 epoch sec; seqNo:10017 2021-02-13T00:09:16Z- AHRS_M2 msg: 0.89 phi; -0.25 theta; -0.17 psi 2021-02-13T00:09:16Z- handleDepth()-Depth_Keller msg: 1613174956622 epoch sec; seqNo:10098; depth 11.71 2021-02-13T00:09:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174956.63 epoch sec; seqNo:10098; 4058512.12 north; 592026.21 east 2021-02-13T00:09:16Z- LcmTrn::handleMessages() - handling 46 messages took 0.68 ms and 133 calls... 2021-02-13T00:09:16Z- waiting for 1613174954.38, time is 1613174956.63 2021-02-13T00:09:16Z- LcmTrn::updateTrn() >>>> pose time 1613174956.63, meas time 1613174956.21 <<<< 2021-02-13T00:09:16Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613174956.21, ping # 0. 2021-02-13T00:09:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174956.21 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:09:16Z- TerrainNav::motionUpdate() - currEstimate = (4058512, 592026.21, 11.71) 2021-02-13T00:09:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:09:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:09:16Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:09:16Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:09:16Z- TNavBF::Projecting Measurements 2021-02-13T00:09:16Z- TNavFilter::Measurement from time = 1613174956.21 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:09:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:09:16Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:09:16Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:09:16Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:09:16Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:09:16Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:09:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:09:16Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 717.1 EastVariance: 833.0 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-13T00:09:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:09:16Z- TNavBF::mmse selected from filter -1 2021-02-13T00:09:16Z- variances([0], [2], and psi_berg) = 717.086696 833.007794 0.000000 2021-02-13T00:09:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:09:16Z- LcmTrn::publishEstimates() - MLE : 4058511.62 591977.71 11.71 2021-02-13T00:09:16Z- LcmTrn::publishEstimates() - MMSE : 4058512.12 592026.21 11.71 2021-02-13T00:09:16Z- LcmTrn::publishEstimates() - COVAR: 717.09 833.01 1.49 0.00 2021-02-13T00:09:17Z- AHRS_M2 msg: 1613174957.02 epoch sec; seqNo:10018 2021-02-13T00:09:17Z- AHRS_M2 msg: 0.90 phi; -0.25 theta; -0.17 psi 2021-02-13T00:09:17Z- handleDepth()-Depth_Keller msg: 1613174957022 epoch sec; seqNo:10099; depth 11.81 2021-02-13T00:09:17Z- handleDvl() - RDI_Pathfinder msg: 1613174957.03 epoch sec; seqNo:4901 2021-02-13T00:09:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.79 , 100.02 , 79.73 , 105.43 2021-02-13T00:09:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , -0.07 2021-02-13T00:09:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174957.04 epoch sec; seqNo:10099; 4058512.50 north; 592026.20 east 2021-02-13T00:09:17Z- AHRS_M2 msg: 1613174957.42 epoch sec; seqNo:10019 2021-02-13T00:09:17Z- AHRS_M2 msg: 0.90 phi; -0.25 theta; -0.17 psi 2021-02-13T00:09:17Z- handleDepth()-Depth_Keller msg: 1613174957428 epoch sec; seqNo:10100; depth 11.92 2021-02-13T00:09:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:17Z- handleDvl() - RDI_Pathfinder msg: 1613174957.43 epoch sec; seqNo:4902 2021-02-13T00:09:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , -0.07 2021-02-13T00:09:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174957.45 epoch sec; seqNo:10100; 4058512.50 north; 592026.20 east 2021-02-13T00:09:17Z- AHRS_M2 msg: 1613174957.82 epoch sec; seqNo:10020 2021-02-13T00:09:17Z- AHRS_M2 msg: 0.91 phi; -0.25 theta; -0.17 psi 2021-02-13T00:09:17Z- handleDepth()-Depth_Keller msg: 1613174957833 epoch sec; seqNo:10101; depth 12.02 2021-02-13T00:09:17Z- handleDvl() - RDI_Pathfinder msg: 1613174957.85 epoch sec; seqNo:4903 2021-02-13T00:09:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 100.43 , 0.00 , 105.84 2021-02-13T00:09:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , -0.13 2021-02-13T00:09:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174957.87 epoch sec; seqNo:10101; 4058512.88 north; 592026.20 east 2021-02-13T00:09:18Z- AHRS_M2 msg: 1613174958.25 epoch sec; seqNo:10021 2021-02-13T00:09:18Z- AHRS_M2 msg: 0.92 phi; -0.25 theta; -0.16 psi 2021-02-13T00:09:18Z- handleDepth()-Depth_Keller msg: 1613174958255 epoch sec; seqNo:10102; depth 12.12 2021-02-13T00:09:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174958.27 epoch sec; seqNo:10102; 4058513.27 north; 592027.41 east 2021-02-13T00:09:18Z- AHRS_M2 msg: 1613174958.67 epoch sec; seqNo:10022 2021-02-13T00:09:18Z- AHRS_M2 msg: 0.92 phi; -0.25 theta; -0.16 psi 2021-02-13T00:09:18Z- handleDepth()-Depth_Keller msg: 1613174958670 epoch sec; seqNo:10103; depth 12.21 2021-02-13T00:09:18Z- handleDvl() - RDI_Pathfinder msg: 1613174958.68 epoch sec; seqNo:4904 2021-02-13T00:09:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.50 , 100.09 , 77.27 , 105.50 2021-02-13T00:09:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.10 , -0.14 2021-02-13T00:09:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174958.70 epoch sec; seqNo:10103; 4058513.27 north; 592027.41 east 2021-02-13T00:09:19Z- AHRS_M2 msg: 1613174959.04 epoch sec; seqNo:10023 2021-02-13T00:09:19Z- AHRS_M2 msg: 0.92 phi; -0.25 theta; -0.16 psi 2021-02-13T00:09:19Z- handleDepth()-Depth_Keller msg: 1613174959041 epoch sec; seqNo:10104; depth 12.34 2021-02-13T00:09:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174959.05 epoch sec; seqNo:10104; 4058513.65 north; 592027.41 east 2021-02-13T00:09:19Z- AHRS_M2 msg: 1613174959.44 epoch sec; seqNo:10024 2021-02-13T00:09:19Z- AHRS_M2 msg: 0.92 phi; -0.25 theta; -0.15 psi 2021-02-13T00:09:19Z- handleDepth()-Depth_Keller msg: 1613174959444 epoch sec; seqNo:10105; depth 12.41 2021-02-13T00:09:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174959.45 epoch sec; seqNo:10105; 4058514.03 north; 592027.41 east 2021-02-13T00:09:19Z- AHRS_M2 msg: 1613174959.85 epoch sec; seqNo:10025 2021-02-13T00:09:19Z- AHRS_M2 msg: 0.92 phi; -0.25 theta; -0.15 psi 2021-02-13T00:09:19Z- handleDepth()-Depth_Keller msg: 1613174959849 epoch sec; seqNo:10106; depth 12.50 2021-02-13T00:09:19Z- handleDvl() - RDI_Pathfinder msg: 1613174959.86 epoch sec; seqNo:4905 2021-02-13T00:09:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 97.65 , 77.27 , 106.18 2021-02-13T00:09:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.15 , -0.06 2021-02-13T00:09:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174959.87 epoch sec; seqNo:10106; 4058514.04 north; 592028.63 east 2021-02-13T00:09:20Z- AHRS_M2 msg: 1613174960.25 epoch sec; seqNo:10026 2021-02-13T00:09:20Z- AHRS_M2 msg: 0.91 phi; -0.25 theta; -0.15 psi 2021-02-13T00:09:20Z- handleDepth()-Depth_Keller msg: 1613174960267 epoch sec; seqNo:10107; depth 12.61 2021-02-13T00:09:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174960.29 epoch sec; seqNo:10107; 4058514.42 north; 592028.62 east 2021-02-13T00:09:20Z- AHRS_M2 msg: 1613174960.65 epoch sec; seqNo:10027 2021-02-13T00:09:20Z- AHRS_M2 msg: 0.90 phi; -0.25 theta; -0.16 psi 2021-02-13T00:09:20Z- handleDepth()-Depth_Keller msg: 1613174960664 epoch sec; seqNo:10108; depth 12.71 2021-02-13T00:09:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174960.70 epoch sec; seqNo:10108; 4058514.80 north; 592028.62 east 2021-02-13T00:09:21Z- AHRS_M2 msg: 1613174961.06 epoch sec; seqNo:10028 2021-02-13T00:09:21Z- AHRS_M2 msg: 0.90 phi; -0.25 theta; -0.16 psi 2021-02-13T00:09:21Z- handleDepth()-Depth_Keller msg: 1613174961060 epoch sec; seqNo:10109; depth 12.81 2021-02-13T00:09:21Z- handleDvl() - RDI_Pathfinder msg: 1613174961.07 epoch sec; seqNo:4906 2021-02-13T00:09:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 98.87 , 78.17 , 104.96 2021-02-13T00:09:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.10 , 0.07 2021-02-13T00:09:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174961.08 epoch sec; seqNo:10109; 4058514.80 north; 592028.62 east 2021-02-13T00:09:21Z- AHRS_M2 msg: 1613174961.46 epoch sec; seqNo:10029 2021-02-13T00:09:21Z- AHRS_M2 msg: 0.89 phi; -0.25 theta; -0.17 psi 2021-02-13T00:09:21Z- handleDepth()-Depth_Keller msg: 1613174961466 epoch sec; seqNo:10110; depth 12.89 2021-02-13T00:09:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174961.48 epoch sec; seqNo:10110; 4058515.18 north; 592028.61 east 2021-02-13T00:09:21Z- LcmTrn::handleMessages() - handling 42 messages took 0.67 ms and 129 calls... 2021-02-13T00:09:21Z- waiting for 1613174959.63, time is 1613174961.48 2021-02-13T00:09:21Z- LcmTrn::updateTrn() >>>> pose time 1613174961.48, meas time 1613174961.07 <<<< 2021-02-13T00:09:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174961.07 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:09:21Z- TerrainNav::motionUpdate() - currEstimate = (4058515, 592028.61, 12.89) 2021-02-13T00:09:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:09:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:09:21Z- TNavBF::Projecting Measurements 2021-02-13T00:09:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:09:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:09:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:09:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:09:21Z- TNavFilter::4 good beams. 2021-02-13T00:09:21Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:09:21Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:09:21Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:09:21Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:09:21Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:09:21Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:09:21Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:09:21Z- TNavBF::mmse selected from filter -1 2021-02-13T00:09:21Z- TerrainMapOctree: (vehN, vehE ) = (4058514.92, 592028.41). 2021-02-13T00:09:21Z- TerrainMapOctree: Min Distance = 3523.65. 2021-02-13T00:09:21Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:09:21Z- There is no specific good beam that all particles have in common. 2021-02-13T00:09:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:09:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.67 K: 0.116 ExpectedMeasDiff: nan currMeas.covariance: 11.03 2021-02-13T00:09:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:09:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.48 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 15.64 2021-02-13T00:09:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:09:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.36 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 9.78 2021-02-13T00:09:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:09:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 17.63 2021-02-13T00:09:21Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:09:21Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:09:21Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:09:21Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:09:21Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:09:21Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:09:21Z- Filter 0 No beams used in subcloud update 2021-02-13T00:09:21Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:09:21Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:09:21Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 717.0 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:09:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174961.07 sec. 2021-02-13T00:09:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:09:21Z- TNavBF::mmse selected from filter -1 2021-02-13T00:09:21Z- variances([0], [2], and psi_berg) = 717.006452 833.007744 0.000000 2021-02-13T00:09:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:09:21Z- LcmTrn::publishEstimates() - MLE : 4058514.68 591981.11 12.89 2021-02-13T00:09:21Z- LcmTrn::publishEstimates() - MMSE : 4058515.18 592028.61 12.89 2021-02-13T00:09:21Z- LcmTrn::publishEstimates() - COVAR: 717.01 833.01 1.16 0.00 2021-02-13T00:09:21Z- AHRS_M2 msg: 1613174961.86 epoch sec; seqNo:10030 2021-02-13T00:09:21Z- AHRS_M2 msg: 0.88 phi; -0.25 theta; -0.17 psi 2021-02-13T00:09:21Z- handleDepth()-Depth_Keller msg: 1613174961869 epoch sec; seqNo:10111; depth 12.99 2021-02-13T00:09:21Z- handleDvl() - RDI_Pathfinder msg: 1613174961.89 epoch sec; seqNo:4907 2021-02-13T00:09:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 97.65 , 78.17 , 106.18 2021-02-13T00:09:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.00 , 0.17 2021-02-13T00:09:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174961.90 epoch sec; seqNo:10111; 4058515.18 north; 592028.61 east 2021-02-13T00:09:22Z- AHRS_M2 msg: 1613174962.27 epoch sec; seqNo:10031 2021-02-13T00:09:22Z- AHRS_M2 msg: 0.88 phi; -0.25 theta; -0.18 psi 2021-02-13T00:09:22Z- handleDepth()-Depth_Keller msg: 1613174962277 epoch sec; seqNo:10112; depth 13.07 2021-02-13T00:09:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:22Z- handleDvl() - RDI_Pathfinder msg: 1613174962.28 epoch sec; seqNo:4908 2021-02-13T00:09:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.00 , 0.17 2021-02-13T00:09:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174962.30 epoch sec; seqNo:10112; 4058515.57 north; 592029.83 east 2021-02-13T00:09:22Z- AHRS_M2 msg: 1613174962.67 epoch sec; seqNo:10032 2021-02-13T00:09:22Z- AHRS_M2 msg: 0.87 phi; -0.25 theta; -0.18 psi 2021-02-13T00:09:22Z- handleDepth()-Depth_Keller msg: 1613174962676 epoch sec; seqNo:10113; depth 13.17 2021-02-13T00:09:22Z- handleDvl() - RDI_Pathfinder msg: 1613174962.69 epoch sec; seqNo:4909 2021-02-13T00:09:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 98.87 , 77.27 , 106.18 2021-02-13T00:09:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.12 , 0.16 2021-02-13T00:09:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174962.70 epoch sec; seqNo:10113; 4058515.57 north; 592029.83 east 2021-02-13T00:09:23Z- AHRS_M2 msg: 1613174963.08 epoch sec; seqNo:10033 2021-02-13T00:09:23Z- AHRS_M2 msg: 0.88 phi; -0.25 theta; -0.18 psi 2021-02-13T00:09:23Z- handleDepth()-Depth_Keller msg: 1613174963082 epoch sec; seqNo:10114; depth 13.26 2021-02-13T00:09:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174963.09 epoch sec; seqNo:10114; 4058515.95 north; 592029.83 east 2021-02-13T00:09:23Z- AHRS_M2 msg: 1613174963.48 epoch sec; seqNo:10034 2021-02-13T00:09:23Z- AHRS_M2 msg: 0.88 phi; -0.25 theta; -0.18 psi 2021-02-13T00:09:23Z- handleDepth()-Depth_Keller msg: 1613174963484 epoch sec; seqNo:10115; depth 13.36 2021-02-13T00:09:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:23Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:23Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:23Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:23Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:23Z- handleDvl() - RDI_Pathfinder msg: 1613174963.49 epoch sec; seqNo:4910 2021-02-13T00:09:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.12 , 0.16 2021-02-13T00:09:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174963.51 epoch sec; seqNo:10115; 4058515.95 north; 592029.83 east 2021-02-13T00:09:23Z- AHRS_M2 msg: 1613174963.88 epoch sec; seqNo:10035 2021-02-13T00:09:23Z- AHRS_M2 msg: 0.89 phi; -0.25 theta; -0.18 psi 2021-02-13T00:09:23Z- handleDepth()-Depth_Keller msg: 1613174963893 epoch sec; seqNo:10116; depth 13.45 2021-02-13T00:09:23Z- handleDvl() - RDI_Pathfinder msg: 1613174963.90 epoch sec; seqNo:4911 2021-02-13T00:09:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.83 , 98.87 , 75.64 , 103.74 2021-02-13T00:09:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.15 , 0.05 2021-02-13T00:09:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174963.93 epoch sec; seqNo:10116; 4058516.34 north; 592031.04 east 2021-02-13T00:09:24Z- AHRS_M2 msg: 1613174964.29 epoch sec; seqNo:10036 2021-02-13T00:09:24Z- AHRS_M2 msg: 0.89 phi; -0.25 theta; -0.18 psi 2021-02-13T00:09:24Z- handleDepth()-Depth_Keller msg: 1613174964294 epoch sec; seqNo:10117; depth 13.55 2021-02-13T00:09:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174964.31 epoch sec; seqNo:10117; 4058516.34 north; 592031.04 east 2021-02-13T00:09:24Z- AHRS_M2 msg: 1613174964.71 epoch sec; seqNo:10037 2021-02-13T00:09:24Z- AHRS_M2 msg: 0.90 phi; -0.26 theta; -0.17 psi 2021-02-13T00:09:24Z- handleDepth()-Depth_Keller msg: 1613174964710 epoch sec; seqNo:10118; depth 13.64 2021-02-13T00:09:24Z- handleDvl() - RDI_Pathfinder msg: 1613174964.72 epoch sec; seqNo:4912 2021-02-13T00:09:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 99.01 , 78.31 , 106.31 2021-02-13T00:09:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.12 , -0.07 2021-02-13T00:09:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174964.74 epoch sec; seqNo:10118; 4058516.72 north; 592031.04 east 2021-02-13T00:09:25Z- AHRS_M2 msg: 1613174965.10 epoch sec; seqNo:10038 2021-02-13T00:09:25Z- AHRS_M2 msg: 0.91 phi; -0.26 theta; -0.16 psi 2021-02-13T00:09:25Z- handleDepth()-Depth_Keller msg: 1613174965100 epoch sec; seqNo:10119; depth 13.75 2021-02-13T00:09:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174965.11 epoch sec; seqNo:10119; 4058516.72 north; 592031.04 east 2021-02-13T00:09:25Z- AHRS_M2 msg: 1613174965.50 epoch sec; seqNo:10039 2021-02-13T00:09:25Z- AHRS_M2 msg: 0.92 phi; -0.26 theta; -0.16 psi 2021-02-13T00:09:25Z- handleDepth()-Depth_Keller msg: 1613174965509 epoch sec; seqNo:10120; depth 13.85 2021-02-13T00:09:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174965.52 epoch sec; seqNo:10120; 4058517.11 north; 592032.26 east 2021-02-13T00:09:25Z- AHRS_M2 msg: 1613174965.90 epoch sec; seqNo:10040 2021-02-13T00:09:25Z- AHRS_M2 msg: 0.92 phi; -0.26 theta; -0.15 psi 2021-02-13T00:09:25Z- handleDepth()-Depth_Keller msg: 1613174965919 epoch sec; seqNo:10121; depth 13.94 2021-02-13T00:09:25Z- handleDvl() - RDI_Pathfinder msg: 1613174965.93 epoch sec; seqNo:4913 2021-02-13T00:09:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.83 , 97.65 , 78.17 , 104.96 2021-02-13T00:09:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , -0.14 2021-02-13T00:09:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174965.95 epoch sec; seqNo:10121; 4058517.11 north; 592032.26 east 2021-02-13T00:09:26Z- AHRS_M2 msg: 1613174966.31 epoch sec; seqNo:10041 2021-02-13T00:09:26Z- AHRS_M2 msg: 0.92 phi; -0.27 theta; -0.15 psi 2021-02-13T00:09:26Z- handleDepth()-Depth_Keller msg: 1613174966315 epoch sec; seqNo:10122; depth 14.04 2021-02-13T00:09:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174966.33 epoch sec; seqNo:10122; 4058517.49 north; 592032.25 east 2021-02-13T00:09:26Z- AHRS_M2 msg: 1613174966.71 epoch sec; seqNo:10042 2021-02-13T00:09:26Z- AHRS_M2 msg: 0.91 phi; -0.27 theta; -0.16 psi 2021-02-13T00:09:26Z- handleDepth()-Depth_Keller msg: 1613174966727 epoch sec; seqNo:10123; depth 14.12 2021-02-13T00:09:26Z- handleDvl() - RDI_Pathfinder msg: 1613174966.74 epoch sec; seqNo:4914 2021-02-13T00:09:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 98.94 , 78.24 , 105.03 2021-02-13T00:09:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , -0.09 2021-02-13T00:09:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174966.75 epoch sec; seqNo:10123; 4058517.87 north; 592032.25 east 2021-02-13T00:09:26Z- LcmTrn::handleMessages() - handling 47 messages took 0.74 ms and 133 calls... 2021-02-13T00:09:26Z- waiting for 1613174964.48, time is 1613174966.75 2021-02-13T00:09:26Z- LcmTrn::updateTrn() equating pose 1613174966.75 and meas times 1613174966.75 2021-02-13T00:09:26Z- LcmTrn::updateTrn() >>>> pose time 1613174966.75, meas time 1613174966.75 <<<< 2021-02-13T00:09:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174966.75 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:09:26Z- TerrainNav::motionUpdate() - currEstimate = (4058518, 592032.25, 14.12) 2021-02-13T00:09:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:09:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:09:26Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174966.75 sec.; No relevant navigation data available 2021-02-13T00:09:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:09:26Z- TNavBF::mmse selected from filter -1 2021-02-13T00:09:26Z- variances([0], [2], and psi_berg) = 716.937120 833.007700 0.000000 2021-02-13T00:09:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:09:26Z- LcmTrn::publishEstimates() - MLE : 4058517.37 591984.75 14.12 2021-02-13T00:09:26Z- LcmTrn::publishEstimates() - MMSE : 4058517.87 592032.25 14.12 2021-02-13T00:09:26Z- LcmTrn::publishEstimates() - COVAR: 716.94 833.01 1.39 0.00 2021-02-13T00:09:27Z- AHRS_M2 msg: 1613174967.12 epoch sec; seqNo:10043 2021-02-13T00:09:27Z- AHRS_M2 msg: 0.90 phi; -0.28 theta; -0.18 psi 2021-02-13T00:09:27Z- handleDepth()-Depth_Keller msg: 1613174967133 epoch sec; seqNo:10124; depth 14.23 2021-02-13T00:09:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:27Z- handleDvl() - RDI_Pathfinder msg: 1613174967.13 epoch sec; seqNo:4915 2021-02-13T00:09:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , -0.09 2021-02-13T00:09:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174967.15 epoch sec; seqNo:10124; 4058517.88 north; 592033.47 east 2021-02-13T00:09:27Z- AHRS_M2 msg: 1613174967.52 epoch sec; seqNo:10044 2021-02-13T00:09:27Z- AHRS_M2 msg: 0.89 phi; -0.28 theta; -0.19 psi 2021-02-13T00:09:27Z- handleDepth()-Depth_Keller msg: 1613174967528 epoch sec; seqNo:10125; depth 14.31 2021-02-13T00:09:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174967.54 epoch sec; seqNo:10125; 4058518.26 north; 592033.46 east 2021-02-13T00:09:27Z- AHRS_M2 msg: 1613174967.92 epoch sec; seqNo:10045 2021-02-13T00:09:27Z- AHRS_M2 msg: 0.89 phi; -0.28 theta; -0.19 psi 2021-02-13T00:09:27Z- handleDepth()-Depth_Keller msg: 1613174967933 epoch sec; seqNo:10126; depth 14.42 2021-02-13T00:09:27Z- handleDvl() - RDI_Pathfinder msg: 1613174967.94 epoch sec; seqNo:4916 2021-02-13T00:09:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.83 , 97.65 , 78.17 , 104.96 2021-02-13T00:09:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.08 , 0.01 2021-02-13T00:09:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174967.95 epoch sec; seqNo:10126; 4058518.26 north; 592033.46 east 2021-02-13T00:09:28Z- AHRS_M2 msg: 1613174968.33 epoch sec; seqNo:10046 2021-02-13T00:09:28Z- AHRS_M2 msg: 0.88 phi; -0.28 theta; -0.19 psi 2021-02-13T00:09:28Z- handleDepth()-Depth_Keller msg: 1613174968338 epoch sec; seqNo:10127; depth 14.52 2021-02-13T00:09:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:28Z- handleDvl() - RDI_Pathfinder msg: 1613174968.34 epoch sec; seqNo:4917 2021-02-13T00:09:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.08 , 0.01 2021-02-13T00:09:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174968.35 epoch sec; seqNo:10127; 4058518.64 north; 592033.46 east 2021-02-13T00:09:28Z- AHRS_M2 msg: 1613174968.73 epoch sec; seqNo:10047 2021-02-13T00:09:28Z- AHRS_M2 msg: 0.88 phi; -0.29 theta; -0.19 psi 2021-02-13T00:09:28Z- handleDepth()-Depth_Keller msg: 1613174968741 epoch sec; seqNo:10128; depth 14.62 2021-02-13T00:09:28Z- handleDvl() - RDI_Pathfinder msg: 1613174968.76 epoch sec; seqNo:4918 2021-02-13T00:09:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.91 , 100.16 , 78.24 , 107.46 2021-02-13T00:09:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.05 2021-02-13T00:09:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174968.77 epoch sec; seqNo:10128; 4058519.02 north; 592033.46 east 2021-02-13T00:09:29Z- AHRS_M2 msg: 1613174969.14 epoch sec; seqNo:10048 2021-02-13T00:09:29Z- AHRS_M2 msg: 0.88 phi; -0.29 theta; -0.19 psi 2021-02-13T00:09:29Z- handleDepth()-Depth_Keller msg: 1613174969145 epoch sec; seqNo:10129; depth 14.71 2021-02-13T00:09:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174969.16 epoch sec; seqNo:10129; 4058519.03 north; 592034.68 east 2021-02-13T00:09:29Z- AHRS_M2 msg: 1613174969.54 epoch sec; seqNo:10049 2021-02-13T00:09:29Z- AHRS_M2 msg: 0.89 phi; -0.29 theta; -0.18 psi 2021-02-13T00:09:29Z- handleDepth()-Depth_Keller msg: 1613174969546 epoch sec; seqNo:10130; depth 14.83 2021-02-13T00:09:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:29Z- handleDvl() - RDI_Pathfinder msg: 1613174969.55 epoch sec; seqNo:4919 2021-02-13T00:09:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.05 2021-02-13T00:09:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174969.56 epoch sec; seqNo:10130; 4058519.41 north; 592034.67 east 2021-02-13T00:09:29Z- AHRS_M2 msg: 1613174969.94 epoch sec; seqNo:10050 2021-02-13T00:09:29Z- AHRS_M2 msg: 0.89 phi; -0.29 theta; -0.18 psi 2021-02-13T00:09:29Z- handleDepth()-Depth_Keller msg: 1613174969953 epoch sec; seqNo:10131; depth 14.93 2021-02-13T00:09:29Z- handleDvl() - RDI_Pathfinder msg: 1613174969.97 epoch sec; seqNo:4920 2021-02-13T00:09:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 98.87 , 73.59 , 112.27 2021-02-13T00:09:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.04 2021-02-13T00:09:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174969.98 epoch sec; seqNo:10131; 4058519.41 north; 592034.67 east 2021-02-13T00:09:30Z- AHRS_M2 msg: 1613174970.35 epoch sec; seqNo:10051 2021-02-13T00:09:30Z- AHRS_M2 msg: 0.90 phi; -0.29 theta; -0.18 psi 2021-02-13T00:09:30Z- handleDepth()-Depth_Keller msg: 1613174970352 epoch sec; seqNo:10132; depth 15.03 2021-02-13T00:09:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174970.37 epoch sec; seqNo:10132; 4058519.79 north; 592034.67 east 2021-02-13T00:09:30Z- AHRS_M2 msg: 1613174970.75 epoch sec; seqNo:10052 2021-02-13T00:09:30Z- AHRS_M2 msg: 0.90 phi; -0.29 theta; -0.18 psi 2021-02-13T00:09:30Z- handleDepth()-Depth_Keller msg: 1613174970756 epoch sec; seqNo:10133; depth 15.12 2021-02-13T00:09:30Z- handleDvl() - RDI_Pathfinder msg: 1613174970.77 epoch sec; seqNo:4921 2021-02-13T00:09:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 97.65 , 78.17 , 106.18 2021-02-13T00:09:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , -0.13 2021-02-13T00:09:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174970.81 epoch sec; seqNo:10133; 4058520.17 north; 592034.67 east 2021-02-13T00:09:31Z- AHRS_M2 msg: 1613174971.16 epoch sec; seqNo:10053 2021-02-13T00:09:31Z- AHRS_M2 msg: 0.90 phi; -0.29 theta; -0.18 psi 2021-02-13T00:09:31Z- handleDepth()-Depth_Keller msg: 1613174971169 epoch sec; seqNo:10134; depth 15.24 2021-02-13T00:09:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174971.18 epoch sec; seqNo:10134; 4058520.18 north; 592035.89 east 2021-02-13T00:09:31Z- AHRS_M2 msg: 1613174971.56 epoch sec; seqNo:10054 2021-02-13T00:09:31Z- AHRS_M2 msg: 0.90 phi; -0.30 theta; -0.18 psi 2021-02-13T00:09:31Z- handleDepth()-Depth_Keller msg: 1613174971566 epoch sec; seqNo:10135; depth 15.34 2021-02-13T00:09:31Z- handleDvl() - RDI_Pathfinder msg: 1613174971.59 epoch sec; seqNo:4922 2021-02-13T00:09:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.50 , 74.82 , 107.46 2021-02-13T00:09:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.15 , -0.11 2021-02-13T00:09:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174971.60 epoch sec; seqNo:10135; 4058520.56 north; 592035.88 east 2021-02-13T00:09:31Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-13T00:09:31Z- waiting for 1613174969.75, time is 1613174971.60 2021-02-13T00:09:31Z- LcmTrn::updateTrn() equating pose 1613174971.60 and meas times 1613174971.60 2021-02-13T00:09:31Z- LcmTrn::updateTrn() >>>> pose time 1613174971.60, meas time 1613174971.60 <<<< 2021-02-13T00:09:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174971.60 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:09:31Z- TerrainNav::motionUpdate() - currEstimate = (4058521, 592035.88, 15.34) 2021-02-13T00:09:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:09:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:09:31Z- TNavBF::Projecting Measurements 2021-02-13T00:09:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:09:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:09:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:09:31Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:09:31Z- TNavFilter::3 good beams. 2021-02-13T00:09:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:09:31Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:09:31Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:09:31Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:09:31Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:09:31Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:09:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:09:31Z- TNavBF::mmse selected from filter -1 2021-02-13T00:09:31Z- TerrainMapOctree: (vehN, vehE ) = (4058520.56, 592035.88). 2021-02-13T00:09:31Z- TerrainMapOctree: Min Distance = 3514.57. 2021-02-13T00:09:31Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:09:31Z- There is no specific good beam that all particles have in common. 2021-02-13T00:09:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:09:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.62 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 14.90 2021-02-13T00:09:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:09:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.48 K: 0.122 ExpectedMeasDiff: nan currMeas.covariance: 8.96 2021-02-13T00:09:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:09:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 18.48 2021-02-13T00:09:31Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:09:31Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:09:31Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:09:31Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:09:32Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:09:32Z- Filter 0 No beams used in subcloud update 2021-02-13T00:09:32Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:09:32Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:09:32Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 716.9 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:09:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174971.60 sec. 2021-02-13T00:09:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:09:32Z- TNavBF::mmse selected from filter -1 2021-02-13T00:09:32Z- variances([0], [2], and psi_berg) = 716.867842 833.007656 0.000000 2021-02-13T00:09:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:09:32Z- LcmTrn::publishEstimates() - MLE : 4058520.06 591988.38 15.34 2021-02-13T00:09:32Z- LcmTrn::publishEstimates() - MMSE : 4058520.56 592035.88 15.34 2021-02-13T00:09:32Z- LcmTrn::publishEstimates() - COVAR: 716.87 833.01 1.22 0.00 2021-02-13T00:09:32Z- AHRS_M2 msg: 1613174971.96 epoch sec; seqNo:10055 2021-02-13T00:09:32Z- AHRS_M2 msg: 0.91 phi; -0.30 theta; -0.18 psi 2021-02-13T00:09:32Z- handleDepth()-Depth_Keller msg: 1613174971968 epoch sec; seqNo:10136; depth 15.47 2021-02-13T00:09:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:32Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:32Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:32Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:32Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:32Z- handleDvl() - RDI_Pathfinder msg: 1613174971.97 epoch sec; seqNo:4923 2021-02-13T00:09:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.15 , -0.11 2021-02-13T00:09:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174971.98 epoch sec; seqNo:10136; 4058520.94 north; 592035.88 east 2021-02-13T00:09:32Z- AHRS_M2 msg: 1613174972.37 epoch sec; seqNo:10056 2021-02-13T00:09:32Z- AHRS_M2 msg: 0.91 phi; -0.30 theta; -0.18 psi 2021-02-13T00:09:32Z- handleDepth()-Depth_Keller msg: 1613174972374 epoch sec; seqNo:10137; depth 15.56 2021-02-13T00:09:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174972.38 epoch sec; seqNo:10137; 4058520.94 north; 592035.88 east 2021-02-13T00:09:32Z- AHRS_M2 msg: 1613174972.77 epoch sec; seqNo:10057 2021-02-13T00:09:32Z- AHRS_M2 msg: 0.90 phi; -0.30 theta; -0.17 psi 2021-02-13T00:09:32Z- handleDepth()-Depth_Keller msg: 1613174972777 epoch sec; seqNo:10138; depth 15.68 2021-02-13T00:09:32Z- handleDvl() - RDI_Pathfinder msg: 1613174972.80 epoch sec; seqNo:4924 2021-02-13T00:09:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.09 , 97.65 , 78.71 , 105.77 2021-02-13T00:09:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.19 , -0.00 2021-02-13T00:09:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174972.81 epoch sec; seqNo:10138; 4058521.32 north; 592035.87 east 2021-02-13T00:09:33Z- AHRS_M2 msg: 1613174973.18 epoch sec; seqNo:10058 2021-02-13T00:09:33Z- AHRS_M2 msg: 0.90 phi; -0.31 theta; -0.16 psi 2021-02-13T00:09:33Z- handleDepth()-Depth_Keller msg: 1613174973185 epoch sec; seqNo:10139; depth 15.78 2021-02-13T00:09:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:33Z- handleDvl() - RDI_Pathfinder msg: 1613174973.19 epoch sec; seqNo:4925 2021-02-13T00:09:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.19 , -0.00 2021-02-13T00:09:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174973.20 epoch sec; seqNo:10139; 4058521.33 north; 592037.09 east 2021-02-13T00:09:33Z- AHRS_M2 msg: 1613174973.59 epoch sec; seqNo:10059 2021-02-13T00:09:33Z- AHRS_M2 msg: 0.90 phi; -0.31 theta; -0.16 psi 2021-02-13T00:09:33Z- handleDepth()-Depth_Keller msg: 1613174973595 epoch sec; seqNo:10140; depth 15.89 2021-02-13T00:09:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174973.62 epoch sec; seqNo:10140; 4058521.71 north; 592037.09 east 2021-02-13T00:09:33Z- AHRS_M2 msg: 1613174973.98 epoch sec; seqNo:10060 2021-02-13T00:09:33Z- AHRS_M2 msg: 0.90 phi; -0.31 theta; -0.15 psi 2021-02-13T00:09:33Z- handleDepth()-Depth_Keller msg: 1613174973988 epoch sec; seqNo:10141; depth 16.01 2021-02-13T00:09:34Z- handleDvl() - RDI_Pathfinder msg: 1613174974.01 epoch sec; seqNo:4926 2021-02-13T00:09:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.72 , 97.79 , 78.31 , 108.75 2021-02-13T00:09:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.14 , 0.09 2021-02-13T00:09:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174974.02 epoch sec; seqNo:10141; 4058522.09 north; 592037.09 east 2021-02-13T00:09:34Z- AHRS_M2 msg: 1613174974.39 epoch sec; seqNo:10061 2021-02-13T00:09:34Z- AHRS_M2 msg: 0.89 phi; -0.32 theta; -0.15 psi 2021-02-13T00:09:34Z- handleDepth()-Depth_Keller msg: 1613174974392 epoch sec; seqNo:10142; depth 16.12 2021-02-13T00:09:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:34Z- handleDvl() - RDI_Pathfinder msg: 1613174974.39 epoch sec; seqNo:4927 2021-02-13T00:09:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.14 , 0.09 2021-02-13T00:09:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174974.40 epoch sec; seqNo:10142; 4058522.09 north; 592037.09 east 2021-02-13T00:09:34Z- AHRS_M2 msg: 1613174974.79 epoch sec; seqNo:10062 2021-02-13T00:09:34Z- AHRS_M2 msg: 0.89 phi; -0.32 theta; -0.15 psi 2021-02-13T00:09:34Z- handleDepth()-Depth_Keller msg: 1613174974798 epoch sec; seqNo:10143; depth 16.22 2021-02-13T00:09:34Z- handleDvl() - RDI_Pathfinder msg: 1613174974.81 epoch sec; seqNo:4928 2021-02-13T00:09:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.83 , 97.65 , 71.14 , 108.61 2021-02-13T00:09:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.06 , 0.13 2021-02-13T00:09:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174974.82 epoch sec; seqNo:10143; 4058522.47 north; 592037.08 east 2021-02-13T00:09:35Z- AHRS_M2 msg: 1613174975.20 epoch sec; seqNo:10063 2021-02-13T00:09:35Z- AHRS_M2 msg: 0.89 phi; -0.32 theta; -0.15 psi 2021-02-13T00:09:35Z- handleDepth()-Depth_Keller msg: 1613174975202 epoch sec; seqNo:10144; depth 16.34 2021-02-13T00:09:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174975.21 epoch sec; seqNo:10144; 4058522.47 north; 592037.08 east 2021-02-13T00:09:35Z- AHRS_M2 msg: 1613174975.62 epoch sec; seqNo:10064 2021-02-13T00:09:35Z- AHRS_M2 msg: 0.89 phi; -0.33 theta; -0.16 psi 2021-02-13T00:09:35Z- handleDepth()-Depth_Keller msg: 1613174975634 epoch sec; seqNo:10145; depth 16.45 2021-02-13T00:09:35Z- handleDvl() - RDI_Pathfinder msg: 1613174975.65 epoch sec; seqNo:4929 2021-02-13T00:09:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.18 , 97.79 , 72.37 , 108.75 2021-02-13T00:09:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , 0.11 2021-02-13T00:09:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174975.66 epoch sec; seqNo:10145; 4058522.86 north; 592038.30 east 2021-02-13T00:09:36Z- handleDepth()-Depth_Keller msg: 1613174976029 epoch sec; seqNo:10146; depth 16.59 2021-02-13T00:09:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174976.05 epoch sec; seqNo:10146; 4058522.86 north; 592038.30 east 2021-02-13T00:09:36Z- AHRS_M2 msg: 1613174976.41 epoch sec; seqNo:10065 2021-02-13T00:09:36Z- AHRS_M2 msg: 0.90 phi; -0.33 theta; -0.17 psi 2021-02-13T00:09:36Z- handleDepth()-Depth_Keller msg: 1613174976419 epoch sec; seqNo:10147; depth 16.70 2021-02-13T00:09:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174976.43 epoch sec; seqNo:10147; 4058523.24 north; 592038.29 east 2021-02-13T00:09:36Z- AHRS_M2 msg: 1613174976.81 epoch sec; seqNo:10066 2021-02-13T00:09:36Z- AHRS_M2 msg: 0.90 phi; -0.33 theta; -0.18 psi 2021-02-13T00:09:36Z- handleDepth()-Depth_Keller msg: 1613174976816 epoch sec; seqNo:10148; depth 16.83 2021-02-13T00:09:36Z- handleDvl() - RDI_Pathfinder msg: 1613174976.84 epoch sec; seqNo:4930 2021-02-13T00:09:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.72 , 78.24 , 108.68 2021-02-13T00:09:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.02 , 0.04 2021-02-13T00:09:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174976.85 epoch sec; seqNo:10148; 4058523.62 north; 592038.29 east 2021-02-13T00:09:37Z- LcmTrn::handleMessages() - handling 46 messages took 0.58 ms and 135 calls... 2021-02-13T00:09:37Z- waiting for 1613174974.60, time is 1613174976.85 2021-02-13T00:09:37Z- LcmTrn::updateTrn() equating pose 1613174976.85 and meas times 1613174976.85 2021-02-13T00:09:37Z- LcmTrn::updateTrn() >>>> pose time 1613174976.85, meas time 1613174976.85 <<<< 2021-02-13T00:09:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174976.85 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:09:37Z- TerrainNav::motionUpdate() - currEstimate = (4058524, 592038.29, 16.83) 2021-02-13T00:09:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:09:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:09:37Z- TerrainNav::Measurement type 1 not incorporated from time = 1613174976.85 sec.; No relevant navigation data available 2021-02-13T00:09:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:09:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:09:37Z- variances([0], [2], and psi_berg) = 716.803619 833.007616 0.000000 2021-02-13T00:09:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:09:37Z- LcmTrn::publishEstimates() - MLE : 4058523.12 591990.79 16.83 2021-02-13T00:09:37Z- LcmTrn::publishEstimates() - MMSE : 4058523.62 592038.29 16.83 2021-02-13T00:09:37Z- LcmTrn::publishEstimates() - COVAR: 716.80 833.01 1.42 0.00 2021-02-13T00:09:37Z- AHRS_M2 msg: 1613174977.21 epoch sec; seqNo:10067 2021-02-13T00:09:37Z- AHRS_M2 msg: 0.91 phi; -0.33 theta; -0.18 psi 2021-02-13T00:09:37Z- handleDepth()-Depth_Keller msg: 1613174977221 epoch sec; seqNo:10149; depth 16.94 2021-02-13T00:09:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174977.24 epoch sec; seqNo:10149; 4058523.63 north; 592039.51 east 2021-02-13T00:09:37Z- AHRS_M2 msg: 1613174977.62 epoch sec; seqNo:10068 2021-02-13T00:09:37Z- AHRS_M2 msg: 0.91 phi; -0.33 theta; -0.17 psi 2021-02-13T00:09:37Z- handleDepth()-Depth_Keller msg: 1613174977625 epoch sec; seqNo:10150; depth 17.06 2021-02-13T00:09:37Z- handleDvl() - RDI_Pathfinder msg: 1613174977.64 epoch sec; seqNo:4931 2021-02-13T00:09:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.93 , 97.52 , 71.14 , 108.34 2021-02-13T00:09:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.02 , -0.04 2021-02-13T00:09:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174977.65 epoch sec; seqNo:10150; 4058524.01 north; 592039.51 east 2021-02-13T00:09:38Z- AHRS_M2 msg: 1613174978.03 epoch sec; seqNo:10069 2021-02-13T00:09:38Z- AHRS_M2 msg: 0.92 phi; -0.33 theta; -0.17 psi 2021-02-13T00:09:38Z- handleDepth()-Depth_Keller msg: 1613174978029 epoch sec; seqNo:10151; depth 17.21 2021-02-13T00:09:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:38Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:38Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:38Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:38Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:38Z- handleDvl() - RDI_Pathfinder msg: 1613174978.03 epoch sec; seqNo:4932 2021-02-13T00:09:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.02 , -0.04 2021-02-13T00:09:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174978.04 epoch sec; seqNo:10151; 4058524.01 north; 592039.51 east 2021-02-13T00:09:38Z- AHRS_M2 msg: 1613174978.43 epoch sec; seqNo:10070 2021-02-13T00:09:38Z- AHRS_M2 msg: 0.92 phi; -0.33 theta; -0.16 psi 2021-02-13T00:09:38Z- handleDepth()-Depth_Keller msg: 1613174978432 epoch sec; seqNo:10152; depth 17.34 2021-02-13T00:09:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174978.44 epoch sec; seqNo:10152; 4058524.39 north; 592039.50 east 2021-02-13T00:09:38Z- AHRS_M2 msg: 1613174978.83 epoch sec; seqNo:10071 2021-02-13T00:09:38Z- AHRS_M2 msg: 0.92 phi; -0.33 theta; -0.16 psi 2021-02-13T00:09:38Z- handleDepth()-Depth_Keller msg: 1613174978836 epoch sec; seqNo:10153; depth 17.47 2021-02-13T00:09:38Z- handleDvl() - RDI_Pathfinder msg: 1613174978.85 epoch sec; seqNo:4933 2021-02-13T00:09:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.59 , 97.86 , 71.96 , 107.60 2021-02-13T00:09:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.05 , -0.08 2021-02-13T00:09:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174978.86 epoch sec; seqNo:10153; 4058524.78 north; 592040.72 east 2021-02-13T00:09:39Z- AHRS_M2 msg: 1613174979.25 epoch sec; seqNo:10072 2021-02-13T00:09:39Z- AHRS_M2 msg: 0.93 phi; -0.33 theta; -0.16 psi 2021-02-13T00:09:39Z- handleDepth()-Depth_Keller msg: 1613174979257 epoch sec; seqNo:10154; depth 17.59 2021-02-13T00:09:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:39Z- handleDvl() - RDI_Pathfinder msg: 1613174979.26 epoch sec; seqNo:4934 2021-02-13T00:09:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.05 , -0.08 2021-02-13T00:09:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174979.27 epoch sec; seqNo:10154; 4058524.78 north; 592040.72 east 2021-02-13T00:09:39Z- AHRS_M2 msg: 1613174979.64 epoch sec; seqNo:10073 2021-02-13T00:09:39Z- AHRS_M2 msg: 0.92 phi; -0.33 theta; -0.17 psi 2021-02-13T00:09:39Z- handleDepth()-Depth_Keller msg: 1613174979649 epoch sec; seqNo:10155; depth 17.72 2021-02-13T00:09:39Z- handleDvl() - RDI_Pathfinder msg: 1613174979.67 epoch sec; seqNo:4935 2021-02-13T00:09:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.59 , 95.42 , 72.37 , 108.82 2021-02-13T00:09:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , -0.07 2021-02-13T00:09:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174979.68 epoch sec; seqNo:10155; 4058525.16 north; 592040.72 east 2021-02-13T00:09:40Z- AHRS_M2 msg: 1613174980.04 epoch sec; seqNo:10074 2021-02-13T00:09:40Z- AHRS_M2 msg: 0.92 phi; -0.33 theta; -0.17 psi 2021-02-13T00:09:40Z- handleDepth()-Depth_Keller msg: 1613174980048 epoch sec; seqNo:10156; depth 17.85 2021-02-13T00:09:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174980.06 epoch sec; seqNo:10156; 4058525.16 north; 592040.72 east 2021-02-13T00:09:40Z- AHRS_M2 msg: 1613174980.45 epoch sec; seqNo:10075 2021-02-13T00:09:40Z- AHRS_M2 msg: 0.92 phi; -0.33 theta; -0.17 psi 2021-02-13T00:09:40Z- handleDepth()-Depth_Keller msg: 1613174980457 epoch sec; seqNo:10157; depth 17.97 2021-02-13T00:09:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:40Z- handleDvl() - RDI_Pathfinder msg: 1613174980.46 epoch sec; seqNo:4936 2021-02-13T00:09:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , -0.07 2021-02-13T00:09:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174980.47 epoch sec; seqNo:10157; 4058525.54 north; 592040.71 east 2021-02-13T00:09:40Z- AHRS_M2 msg: 1613174980.85 epoch sec; seqNo:10076 2021-02-13T00:09:40Z- AHRS_M2 msg: 0.92 phi; -0.33 theta; -0.17 psi 2021-02-13T00:09:40Z- handleDepth()-Depth_Keller msg: 1613174980866 epoch sec; seqNo:10158; depth 18.08 2021-02-13T00:09:40Z- handleDvl() - RDI_Pathfinder msg: 1613174980.88 epoch sec; seqNo:4937 2021-02-13T00:09:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.59 , 97.86 , 69.91 , 106.38 2021-02-13T00:09:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , -0.03 2021-02-13T00:09:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174980.89 epoch sec; seqNo:10158; 4058525.93 north; 592041.93 east 2021-02-13T00:09:41Z- AHRS_M2 msg: 1613174981.26 epoch sec; seqNo:10077 2021-02-13T00:09:41Z- AHRS_M2 msg: 0.92 phi; -0.33 theta; -0.17 psi 2021-02-13T00:09:41Z- handleDepth()-Depth_Keller msg: 1613174981265 epoch sec; seqNo:10159; depth 18.20 2021-02-13T00:09:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174981.28 epoch sec; seqNo:10159; 4058525.93 north; 592041.93 east 2021-02-13T00:09:41Z- AHRS_M2 msg: 1613174981.66 epoch sec; seqNo:10078 2021-02-13T00:09:41Z- AHRS_M2 msg: 0.92 phi; -0.33 theta; -0.17 psi 2021-02-13T00:09:41Z- handleDepth()-Depth_Keller msg: 1613174981671 epoch sec; seqNo:10160; depth 18.31 2021-02-13T00:09:41Z- handleDvl() - RDI_Pathfinder msg: 1613174981.68 epoch sec; seqNo:4938 2021-02-13T00:09:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.93 , 71.55 , 106.45 2021-02-13T00:09:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , -0.01 2021-02-13T00:09:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174981.69 epoch sec; seqNo:10160; 4058526.31 north; 592041.92 east 2021-02-13T00:09:42Z- AHRS_M2 msg: 1613174982.06 epoch sec; seqNo:10079 2021-02-13T00:09:42Z- AHRS_M2 msg: 0.92 phi; -0.33 theta; -0.17 psi 2021-02-13T00:09:42Z- LcmTrn::handleMessages() - handling 45 messages took 0.65 ms and 132 calls... 2021-02-13T00:09:42Z- waiting for 1613174979.85, time is 1613174982.06 2021-02-13T00:09:42Z- LcmTrn::updateTrn() >>>> pose time 1613174982.06, meas time 1613174981.68 <<<< 2021-02-13T00:09:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174981.68 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:09:42Z- TerrainNav::motionUpdate() - currEstimate = (4058526, 592041.92, 18.31) 2021-02-13T00:09:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:09:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:09:42Z- TNavBF::Projecting Measurements 2021-02-13T00:09:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:09:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:09:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:09:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:09:42Z- TNavFilter::4 good beams. 2021-02-13T00:09:42Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:09:42Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:09:42Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:09:42Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:09:42Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:09:42Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:09:42Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:09:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:09:42Z- TerrainMapOctree: (vehN, vehE ) = (4058526.11, 592041.66). 2021-02-13T00:09:42Z- TerrainMapOctree: Min Distance = 3506.61. 2021-02-13T00:09:42Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:09:42Z- There is no specific good beam that all particles have in common. 2021-02-13T00:09:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:09:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.63 K: 0.124 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T00:09:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:09:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:09:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:09:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.32 K: 0.117 ExpectedMeasDiff: nan currMeas.covariance: 8.19 2021-02-13T00:09:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:09:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 18.13 2021-02-13T00:09:42Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:09:42Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:09:42Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:09:42Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:09:42Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:09:42Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:09:42Z- Filter 0 No beams used in subcloud update 2021-02-13T00:09:42Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:09:42Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:09:42Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 716.7 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:09:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174981.68 sec. 2021-02-13T00:09:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:09:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:09:42Z- variances([0], [2], and psi_berg) = 716.718376 833.007562 0.000000 2021-02-13T00:09:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:09:42Z- LcmTrn::publishEstimates() - MLE : 4058525.81 591994.42 18.31 2021-02-13T00:09:42Z- LcmTrn::publishEstimates() - MMSE : 4058526.31 592041.92 18.31 2021-02-13T00:09:42Z- LcmTrn::publishEstimates() - COVAR: 716.72 833.01 1.12 0.00 2021-02-13T00:09:42Z- handleDepth()-Depth_Keller msg: 1613174982074 epoch sec; seqNo:10161; depth 18.43 2021-02-13T00:09:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174982.09 epoch sec; seqNo:10161; 4058526.31 north; 592041.92 east 2021-02-13T00:09:42Z- AHRS_M2 msg: 1613174982.47 epoch sec; seqNo:10080 2021-02-13T00:09:42Z- AHRS_M2 msg: 0.92 phi; -0.33 theta; -0.17 psi 2021-02-13T00:09:42Z- handleDepth()-Depth_Keller msg: 1613174982472 epoch sec; seqNo:10162; depth 18.56 2021-02-13T00:09:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174982.48 epoch sec; seqNo:10162; 4058526.69 north; 592041.92 east 2021-02-13T00:09:42Z- AHRS_M2 msg: 1613174982.87 epoch sec; seqNo:10081 2021-02-13T00:09:42Z- AHRS_M2 msg: 0.92 phi; -0.33 theta; -0.17 psi 2021-02-13T00:09:42Z- handleDepth()-Depth_Keller msg: 1613174982881 epoch sec; seqNo:10163; depth 18.67 2021-02-13T00:09:42Z- handleDvl() - RDI_Pathfinder msg: 1613174982.89 epoch sec; seqNo:4939 2021-02-13T00:09:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.93 , 70.32 , 106.45 2021-02-13T00:09:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.02 2021-02-13T00:09:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174982.92 epoch sec; seqNo:10163; 4058526.70 north; 592043.14 east 2021-02-13T00:09:43Z- AHRS_M2 msg: 1613174983.28 epoch sec; seqNo:10082 2021-02-13T00:09:43Z- AHRS_M2 msg: 0.92 phi; -0.33 theta; -0.17 psi 2021-02-13T00:09:43Z- handleDepth()-Depth_Keller msg: 1613174983289 epoch sec; seqNo:10164; depth 18.80 2021-02-13T00:09:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174983.30 epoch sec; seqNo:10164; 4058527.08 north; 592043.14 east 2021-02-13T00:09:43Z- AHRS_M2 msg: 1613174983.68 epoch sec; seqNo:10083 2021-02-13T00:09:43Z- AHRS_M2 msg: 0.93 phi; -0.32 theta; -0.17 psi 2021-02-13T00:09:43Z- handleDepth()-Depth_Keller msg: 1613174983686 epoch sec; seqNo:10165; depth 18.90 2021-02-13T00:09:43Z- handleDvl() - RDI_Pathfinder msg: 1613174983.70 epoch sec; seqNo:4940 2021-02-13T00:09:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.93 , 69.50 , 106.45 2021-02-13T00:09:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.06 2021-02-13T00:09:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174983.71 epoch sec; seqNo:10165; 4058527.08 north; 592043.14 east 2021-02-13T00:09:44Z- AHRS_M2 msg: 1613174984.08 epoch sec; seqNo:10084 2021-02-13T00:09:44Z- AHRS_M2 msg: 0.93 phi; -0.32 theta; -0.17 psi 2021-02-13T00:09:44Z- handleDepth()-Depth_Keller msg: 1613174984095 epoch sec; seqNo:10166; depth 19.03 2021-02-13T00:09:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:44Z- handleDvl() - RDI_Pathfinder msg: 1613174984.10 epoch sec; seqNo:4941 2021-02-13T00:09:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.06 2021-02-13T00:09:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174984.12 epoch sec; seqNo:10166; 4058527.46 north; 592043.13 east 2021-02-13T00:09:44Z- AHRS_M2 msg: 1613174984.50 epoch sec; seqNo:10085 2021-02-13T00:09:44Z- AHRS_M2 msg: 0.93 phi; -0.33 theta; -0.17 psi 2021-02-13T00:09:44Z- handleDepth()-Depth_Keller msg: 1613174984501 epoch sec; seqNo:10167; depth 19.15 2021-02-13T00:09:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174984.51 epoch sec; seqNo:10167; 4058527.47 north; 592044.35 east 2021-02-13T00:09:44Z- AHRS_M2 msg: 1613174984.90 epoch sec; seqNo:10086 2021-02-13T00:09:44Z- AHRS_M2 msg: 0.93 phi; -0.33 theta; -0.17 psi 2021-02-13T00:09:44Z- handleDepth()-Depth_Keller msg: 1613174984901 epoch sec; seqNo:10168; depth 19.27 2021-02-13T00:09:44Z- handleDvl() - RDI_Pathfinder msg: 1613174984.91 epoch sec; seqNo:4942 2021-02-13T00:09:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.45 , 97.93 , 70.32 , 104.01 2021-02-13T00:09:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , -0.08 2021-02-13T00:09:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174984.92 epoch sec; seqNo:10168; 4058527.85 north; 592044.35 east 2021-02-13T00:09:45Z- AHRS_M2 msg: 1613174985.30 epoch sec; seqNo:10087 2021-02-13T00:09:45Z- AHRS_M2 msg: 0.93 phi; -0.33 theta; -0.17 psi 2021-02-13T00:09:45Z- handleDepth()-Depth_Keller msg: 1613174985301 epoch sec; seqNo:10169; depth 19.39 2021-02-13T00:09:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:45Z- handleDvl() - RDI_Pathfinder msg: 1613174985.30 epoch sec; seqNo:4943 2021-02-13T00:09:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , -0.08 2021-02-13T00:09:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174985.33 epoch sec; seqNo:10169; 4058528.23 north; 592044.35 east 2021-02-13T00:09:45Z- AHRS_M2 msg: 1613174985.70 epoch sec; seqNo:10088 2021-02-13T00:09:45Z- AHRS_M2 msg: 0.93 phi; -0.33 theta; -0.17 psi 2021-02-13T00:09:45Z- handleDepth()-Depth_Keller msg: 1613174985705 epoch sec; seqNo:10170; depth 19.51 2021-02-13T00:09:45Z- handleDvl() - RDI_Pathfinder msg: 1613174985.72 epoch sec; seqNo:4944 2021-02-13T00:09:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.64 , 94.34 , 70.73 , 105.30 2021-02-13T00:09:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.11 , -0.05 2021-02-13T00:09:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174985.74 epoch sec; seqNo:10170; 4058528.23 north; 592044.35 east 2021-02-13T00:09:46Z- AHRS_M2 msg: 1613174986.10 epoch sec; seqNo:10089 2021-02-13T00:09:46Z- AHRS_M2 msg: 0.93 phi; -0.33 theta; -0.17 psi 2021-02-13T00:09:46Z- handleDepth()-Depth_Keller msg: 1613174986108 epoch sec; seqNo:10171; depth 19.64 2021-02-13T00:09:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174986.12 epoch sec; seqNo:10171; 4058528.61 north; 592044.34 east 2021-02-13T00:09:46Z- AHRS_M2 msg: 1613174986.52 epoch sec; seqNo:10090 2021-02-13T00:09:46Z- AHRS_M2 msg: 0.93 phi; -0.33 theta; -0.17 psi 2021-02-13T00:09:46Z- handleDepth()-Depth_Keller msg: 1613174986533 epoch sec; seqNo:10172; depth 19.76 2021-02-13T00:09:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:46Z- handleDvl() - RDI_Pathfinder msg: 1613174986.54 epoch sec; seqNo:4945 2021-02-13T00:09:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.11 , -0.05 2021-02-13T00:09:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174986.56 epoch sec; seqNo:10172; 4058529.00 north; 592045.56 east 2021-02-13T00:09:46Z- AHRS_M2 msg: 1613174986.91 epoch sec; seqNo:10091 2021-02-13T00:09:46Z- AHRS_M2 msg: 0.93 phi; -0.34 theta; -0.18 psi 2021-02-13T00:09:46Z- handleDepth()-Depth_Keller msg: 1613174986922 epoch sec; seqNo:10173; depth 19.89 2021-02-13T00:09:46Z- handleDvl() - RDI_Pathfinder msg: 1613174986.93 epoch sec; seqNo:4946 2021-02-13T00:09:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.04 , 93.19 , 69.10 , 104.15 2021-02-13T00:09:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.11 , -0.01 2021-02-13T00:09:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174986.94 epoch sec; seqNo:10173; 4058529.00 north; 592045.56 east 2021-02-13T00:09:47Z- LcmTrn::handleMessages() - handling 46 messages took 0.64 ms and 133 calls... 2021-02-13T00:09:47Z- waiting for 1613174985.06, time is 1613174986.94 2021-02-13T00:09:47Z- LcmTrn::updateTrn() equating pose 1613174986.94 and meas times 1613174986.94 2021-02-13T00:09:47Z- LcmTrn::updateTrn() >>>> pose time 1613174986.94, meas time 1613174986.94 <<<< 2021-02-13T00:09:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174986.94 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:09:47Z- TerrainNav::motionUpdate() - currEstimate = (4058529, 592045.56, 19.89) 2021-02-13T00:09:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:09:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:09:47Z- TNavBF::Projecting Measurements 2021-02-13T00:09:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:09:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:09:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:09:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:09:47Z- TNavFilter::4 good beams. 2021-02-13T00:09:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:09:47Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:09:47Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:09:47Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:09:47Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:09:47Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:09:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:09:47Z- TNavBF::mmse selected from filter -1 2021-02-13T00:09:47Z- TerrainMapOctree: (vehN, vehE ) = (4058529.00, 592045.56). 2021-02-13T00:09:47Z- TerrainMapOctree: Min Distance = 3501.92. 2021-02-13T00:09:47Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:09:47Z- There is no specific good beam that all particles have in common. 2021-02-13T00:09:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:09:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.35 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 9.25 2021-02-13T00:09:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:09:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 13.90 2021-02-13T00:09:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:09:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 7.64 2021-02-13T00:09:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:09:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 17.36 2021-02-13T00:09:47Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:09:47Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:09:47Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:09:47Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:09:47Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:09:47Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:09:47Z- Filter 0 No beams used in subcloud update 2021-02-13T00:09:47Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:09:47Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:09:47Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 716.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:09:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174986.94 sec. 2021-02-13T00:09:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:09:47Z- TNavBF::mmse selected from filter -1 2021-02-13T00:09:47Z- variances([0], [2], and psi_berg) = 716.649268 833.007519 0.000000 2021-02-13T00:09:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:09:47Z- LcmTrn::publishEstimates() - MLE : 4058528.50 591998.06 19.89 2021-02-13T00:09:47Z- LcmTrn::publishEstimates() - MMSE : 4058529.00 592045.56 19.89 2021-02-13T00:09:47Z- LcmTrn::publishEstimates() - COVAR: 716.65 833.01 0.95 0.00 2021-02-13T00:09:47Z- AHRS_M2 msg: 1613174987.32 epoch sec; seqNo:10092 2021-02-13T00:09:47Z- AHRS_M2 msg: 0.93 phi; -0.34 theta; -0.18 psi 2021-02-13T00:09:47Z- handleDepth()-Depth_Keller msg: 1613174987322 epoch sec; seqNo:10174; depth 20.01 2021-02-13T00:09:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174987.33 epoch sec; seqNo:10174; 4058529.38 north; 592045.55 east 2021-02-13T00:09:47Z- AHRS_M2 msg: 1613174987.72 epoch sec; seqNo:10093 2021-02-13T00:09:47Z- AHRS_M2 msg: 0.94 phi; -0.34 theta; -0.19 psi 2021-02-13T00:09:47Z- handleDepth()-Depth_Keller msg: 1613174987729 epoch sec; seqNo:10175; depth 20.12 2021-02-13T00:09:47Z- handleDvl() - RDI_Pathfinder msg: 1613174987.74 epoch sec; seqNo:4947 2021-02-13T00:09:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 98.13 , 68.28 , 106.65 2021-02-13T00:09:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.10 , 0.02 2021-02-13T00:09:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174987.75 epoch sec; seqNo:10175; 4058529.38 north; 592045.55 east 2021-02-13T00:09:48Z- AHRS_M2 msg: 1613174988.13 epoch sec; seqNo:10094 2021-02-13T00:09:48Z- AHRS_M2 msg: 0.94 phi; -0.34 theta; -0.19 psi 2021-02-13T00:09:48Z- handleDepth()-Depth_Keller msg: 1613174988141 epoch sec; seqNo:10176; depth 20.25 2021-02-13T00:09:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174988.15 epoch sec; seqNo:10176; 4058529.76 north; 592045.55 east 2021-02-13T00:09:48Z- AHRS_M2 msg: 1613174988.53 epoch sec; seqNo:10095 2021-02-13T00:09:48Z- AHRS_M2 msg: 0.94 phi; -0.34 theta; -0.19 psi 2021-02-13T00:09:48Z- handleDepth()-Depth_Keller msg: 1613174988532 epoch sec; seqNo:10177; depth 20.38 2021-02-13T00:09:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174988.55 epoch sec; seqNo:10177; 4058530.13 north; 592045.55 east 2021-02-13T00:09:48Z- AHRS_M2 msg: 1613174988.93 epoch sec; seqNo:10096 2021-02-13T00:09:48Z- AHRS_M2 msg: 0.95 phi; -0.34 theta; -0.18 psi 2021-02-13T00:09:48Z- handleDepth()-Depth_Keller msg: 1613174988941 epoch sec; seqNo:10178; depth 20.50 2021-02-13T00:09:48Z- handleDvl() - RDI_Pathfinder msg: 1613174988.95 epoch sec; seqNo:4948 2021-02-13T00:09:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 94.27 , 78.44 , 106.45 2021-02-13T00:09:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , 0.03 2021-02-13T00:09:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174988.96 epoch sec; seqNo:10178; 4058530.15 north; 592046.77 east 2021-02-13T00:09:49Z- AHRS_M2 msg: 1613174989.34 epoch sec; seqNo:10097 2021-02-13T00:09:49Z- AHRS_M2 msg: 0.95 phi; -0.34 theta; -0.17 psi 2021-02-13T00:09:49Z- handleDepth()-Depth_Keller msg: 1613174989349 epoch sec; seqNo:10179; depth 20.64 2021-02-13T00:09:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174989.36 epoch sec; seqNo:10179; 4058530.53 north; 592046.76 east 2021-02-13T00:09:49Z- AHRS_M2 msg: 1613174989.74 epoch sec; seqNo:10098 2021-02-13T00:09:49Z- AHRS_M2 msg: 0.95 phi; -0.34 theta; -0.17 psi 2021-02-13T00:09:49Z- handleDepth()-Depth_Keller msg: 1613174989744 epoch sec; seqNo:10180; depth 20.77 2021-02-13T00:09:49Z- handleDvl() - RDI_Pathfinder msg: 1613174989.76 epoch sec; seqNo:4949 2021-02-13T00:09:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 92.99 , 66.64 , 105.16 2021-02-13T00:09:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , 0.04 2021-02-13T00:09:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174989.78 epoch sec; seqNo:10180; 4058530.53 north; 592046.76 east 2021-02-13T00:09:50Z- AHRS_M2 msg: 1613174990.15 epoch sec; seqNo:10099 2021-02-13T00:09:50Z- AHRS_M2 msg: 0.95 phi; -0.35 theta; -0.16 psi 2021-02-13T00:09:50Z- handleDepth()-Depth_Keller msg: 1613174990157 epoch sec; seqNo:10181; depth 20.91 2021-02-13T00:09:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:50Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:50Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:50Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:50Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:50Z- handleDvl() - RDI_Pathfinder msg: 1613174990.16 epoch sec; seqNo:4950 2021-02-13T00:09:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , 0.04 2021-02-13T00:09:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174990.17 epoch sec; seqNo:10181; 4058530.91 north; 592046.76 east 2021-02-13T00:09:50Z- AHRS_M2 msg: 1613174990.57 epoch sec; seqNo:10100 2021-02-13T00:09:50Z- AHRS_M2 msg: 0.96 phi; -0.35 theta; -0.16 psi 2021-02-13T00:09:50Z- handleDepth()-Depth_Keller msg: 1613174990571 epoch sec; seqNo:10182; depth 21.04 2021-02-13T00:09:50Z- handleDvl() - RDI_Pathfinder msg: 1613174990.58 epoch sec; seqNo:4951 2021-02-13T00:09:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 98.13 , 67.46 , 105.43 2021-02-13T00:09:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.04 , 0.04 2021-02-13T00:09:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174990.59 epoch sec; seqNo:10182; 4058530.92 north; 592047.98 east 2021-02-13T00:09:50Z- handleDepth()-Depth_Keller msg: 1613174990977 epoch sec; seqNo:10183; depth 21.17 2021-02-13T00:09:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174990.99 epoch sec; seqNo:10183; 4058530.92 north; 592047.98 east 2021-02-13T00:09:51Z- AHRS_M2 msg: 1613174991.36 epoch sec; seqNo:10101 2021-02-13T00:09:51Z- AHRS_M2 msg: 0.96 phi; -0.35 theta; -0.16 psi 2021-02-13T00:09:51Z- handleDepth()-Depth_Keller msg: 1613174991365 epoch sec; seqNo:10184; depth 21.30 2021-02-13T00:09:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:51Z- handleDvl() - RDI_Pathfinder msg: 1613174991.37 epoch sec; seqNo:4952 2021-02-13T00:09:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.04 , 0.04 2021-02-13T00:09:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174991.37 epoch sec; seqNo:10184; 4058531.30 north; 592047.98 east 2021-02-13T00:09:51Z- AHRS_M2 msg: 1613174991.76 epoch sec; seqNo:10102 2021-02-13T00:09:51Z- AHRS_M2 msg: 0.96 phi; -0.35 theta; -0.16 psi 2021-02-13T00:09:51Z- handleDepth()-Depth_Keller msg: 1613174991769 epoch sec; seqNo:10185; depth 21.40 2021-02-13T00:09:51Z- handleDvl() - RDI_Pathfinder msg: 1613174991.78 epoch sec; seqNo:4953 2021-02-13T00:09:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.99 , 66.64 , 105.30 2021-02-13T00:09:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.07 , 0.06 2021-02-13T00:09:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174991.79 epoch sec; seqNo:10185; 4058531.30 north; 592047.98 east 2021-02-13T00:09:52Z- AHRS_M2 msg: 1613174992.16 epoch sec; seqNo:10103 2021-02-13T00:09:52Z- AHRS_M2 msg: 0.96 phi; -0.35 theta; -0.16 psi 2021-02-13T00:09:52Z- handleDepth()-Depth_Keller msg: 1613174992170 epoch sec; seqNo:10186; depth 21.56 2021-02-13T00:09:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174992.18 epoch sec; seqNo:10186; 4058531.68 north; 592047.97 east 2021-02-13T00:09:52Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-13T00:09:52Z- waiting for 1613174989.94, time is 1613174992.18 2021-02-13T00:09:52Z- LcmTrn::updateTrn() >>>> pose time 1613174992.18, meas time 1613174991.78 <<<< 2021-02-13T00:09:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174991.78 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:09:52Z- TerrainNav::motionUpdate() - currEstimate = (4058532, 592047.97, 21.56) 2021-02-13T00:09:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:09:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:09:52Z- TNavBF::Projecting Measurements 2021-02-13T00:09:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:09:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:09:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:09:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:09:52Z- TNavFilter::4 good beams. 2021-02-13T00:09:52Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:09:52Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:09:52Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:09:52Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:09:52Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:09:52Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:09:52Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:09:52Z- TNavBF::mmse selected from filter -1 2021-02-13T00:09:52Z- TerrainMapOctree: (vehN, vehE ) = (4058531.47, 592047.79). 2021-02-13T00:09:52Z- TerrainMapOctree: Min Distance = 3498.59. 2021-02-13T00:09:52Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:09:52Z- There is no specific good beam that all particles have in common. 2021-02-13T00:09:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:09:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:09:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:09:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:09:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:09:52Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 7.11 2021-02-13T00:09:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:09:52Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 17.74 2021-02-13T00:09:52Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:09:52Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:09:52Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:09:52Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:09:52Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:09:52Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:09:52Z- Filter 0 No beams used in subcloud update 2021-02-13T00:09:52Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:09:52Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:09:52Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 716.6 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:09:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174991.78 sec. 2021-02-13T00:09:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:09:52Z- TNavBF::mmse selected from filter -1 2021-02-13T00:09:52Z- variances([0], [2], and psi_berg) = 716.572970 833.007471 0.000000 2021-02-13T00:09:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:09:52Z- LcmTrn::publishEstimates() - MLE : 4058531.18 592000.47 21.56 2021-02-13T00:09:52Z- LcmTrn::publishEstimates() - MMSE : 4058531.68 592047.97 21.56 2021-02-13T00:09:52Z- LcmTrn::publishEstimates() - COVAR: 716.57 833.01 0.84 0.00 2021-02-13T00:09:52Z- AHRS_M2 msg: 1613174992.57 epoch sec; seqNo:10104 2021-02-13T00:09:52Z- AHRS_M2 msg: 0.96 phi; -0.35 theta; -0.17 psi 2021-02-13T00:09:52Z- handleDepth()-Depth_Keller msg: 1613174992572 epoch sec; seqNo:10187; depth 21.68 2021-02-13T00:09:52Z- handleDvl() - RDI_Pathfinder msg: 1613174992.59 epoch sec; seqNo:4954 2021-02-13T00:09:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.23 , 91.90 , 67.46 , 106.52 2021-02-13T00:09:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.09 , 0.06 2021-02-13T00:09:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174992.60 epoch sec; seqNo:10187; 4058531.69 north; 592049.19 east 2021-02-13T00:09:52Z- AHRS_M2 msg: 1613174992.97 epoch sec; seqNo:10105 2021-02-13T00:09:52Z- AHRS_M2 msg: 0.96 phi; -0.35 theta; -0.18 psi 2021-02-13T00:09:52Z- handleDepth()-Depth_Keller msg: 1613174992981 epoch sec; seqNo:10188; depth 21.80 2021-02-13T00:09:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174992.99 epoch sec; seqNo:10188; 4058532.07 north; 592049.19 east 2021-02-13T00:09:53Z- AHRS_M2 msg: 1613174993.38 epoch sec; seqNo:10106 2021-02-13T00:09:53Z- AHRS_M2 msg: 0.95 phi; -0.36 theta; -0.19 psi 2021-02-13T00:09:53Z- handleDepth()-Depth_Keller msg: 1613174993385 epoch sec; seqNo:10189; depth 21.94 2021-02-13T00:09:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174993.40 epoch sec; seqNo:10189; 4058532.07 north; 592049.19 east 2021-02-13T00:09:53Z- AHRS_M2 msg: 1613174993.78 epoch sec; seqNo:10107 2021-02-13T00:09:53Z- AHRS_M2 msg: 0.95 phi; -0.36 theta; -0.20 psi 2021-02-13T00:09:53Z- handleDepth()-Depth_Keller msg: 1613174993786 epoch sec; seqNo:10190; depth 22.07 2021-02-13T00:09:53Z- handleDvl() - RDI_Pathfinder msg: 1613174993.80 epoch sec; seqNo:4955 2021-02-13T00:09:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.20 , 68.28 , 106.72 2021-02-13T00:09:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.09 , 0.01 2021-02-13T00:09:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174993.81 epoch sec; seqNo:10190; 4058532.45 north; 592049.18 east 2021-02-13T00:09:54Z- AHRS_M2 msg: 1613174994.18 epoch sec; seqNo:10108 2021-02-13T00:09:54Z- AHRS_M2 msg: 0.95 phi; -0.36 theta; -0.20 psi 2021-02-13T00:09:54Z- handleDepth()-Depth_Keller msg: 1613174994193 epoch sec; seqNo:10191; depth 22.21 2021-02-13T00:09:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174994.22 epoch sec; seqNo:10191; 4058532.46 north; 592050.41 east 2021-02-13T00:09:54Z- AHRS_M2 msg: 1613174994.59 epoch sec; seqNo:10109 2021-02-13T00:09:54Z- AHRS_M2 msg: 0.95 phi; -0.36 theta; -0.20 psi 2021-02-13T00:09:54Z- handleDepth()-Depth_Keller msg: 1613174994592 epoch sec; seqNo:10192; depth 22.35 2021-02-13T00:09:54Z- handleDvl() - RDI_Pathfinder msg: 1613174994.60 epoch sec; seqNo:4956 2021-02-13T00:09:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.41 , 97.65 , 66.64 , 106.18 2021-02-13T00:09:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.07 , -0.04 2021-02-13T00:09:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174994.61 epoch sec; seqNo:10192; 4058532.84 north; 592050.40 east 2021-02-13T00:09:54Z- AHRS_M2 msg: 1613174994.99 epoch sec; seqNo:10110 2021-02-13T00:09:54Z- AHRS_M2 msg: 0.96 phi; -0.36 theta; -0.20 psi 2021-02-13T00:09:54Z- handleDepth()-Depth_Keller msg: 1613174994997 epoch sec; seqNo:10193; depth 22.48 2021-02-13T00:09:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:55Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:55Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:55Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:55Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:55Z- handleDvl() - RDI_Pathfinder msg: 1613174995.00 epoch sec; seqNo:4957 2021-02-13T00:09:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.07 , -0.04 2021-02-13T00:09:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174995.01 epoch sec; seqNo:10193; 4058532.84 north; 592050.40 east 2021-02-13T00:09:55Z- AHRS_M2 msg: 1613174995.39 epoch sec; seqNo:10111 2021-02-13T00:09:55Z- AHRS_M2 msg: 0.96 phi; -0.36 theta; -0.21 psi 2021-02-13T00:09:55Z- handleDepth()-Depth_Keller msg: 1613174995400 epoch sec; seqNo:10194; depth 22.62 2021-02-13T00:09:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174995.41 epoch sec; seqNo:10194; 4058533.22 north; 592050.40 east 2021-02-13T00:09:55Z- AHRS_M2 msg: 1613174995.82 epoch sec; seqNo:10112 2021-02-13T00:09:55Z- AHRS_M2 msg: 0.96 phi; -0.36 theta; -0.21 psi 2021-02-13T00:09:55Z- handleDepth()-Depth_Keller msg: 1613174995827 epoch sec; seqNo:10195; depth 22.75 2021-02-13T00:09:55Z- handleDvl() - RDI_Pathfinder msg: 1613174995.85 epoch sec; seqNo:4958 2021-02-13T00:09:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 98.13 , 67.46 , 104.22 2021-02-13T00:09:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.02 , -0.11 2021-02-13T00:09:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174995.86 epoch sec; seqNo:10195; 4058533.23 north; 592051.62 east 2021-02-13T00:09:56Z- AHRS_M2 msg: 1613174996.20 epoch sec; seqNo:10113 2021-02-13T00:09:56Z- AHRS_M2 msg: 0.95 phi; -0.37 theta; -0.21 psi 2021-02-13T00:09:56Z- handleDepth()-Depth_Keller msg: 1613174996217 epoch sec; seqNo:10196; depth 22.91 2021-02-13T00:09:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:56Z- handleDvl() - RDI_Pathfinder msg: 1613174996.22 epoch sec; seqNo:4959 2021-02-13T00:09:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.02 , -0.11 2021-02-13T00:09:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174996.23 epoch sec; seqNo:10196; 4058533.61 north; 592051.61 east 2021-02-13T00:09:56Z- AHRS_M2 msg: 1613174996.63 epoch sec; seqNo:10114 2021-02-13T00:09:56Z- AHRS_M2 msg: 0.95 phi; -0.37 theta; -0.21 psi 2021-02-13T00:09:56Z- handleDepth()-Depth_Keller msg: 1613174996632 epoch sec; seqNo:10197; depth 23.03 2021-02-13T00:09:56Z- handleDvl() - RDI_Pathfinder msg: 1613174996.64 epoch sec; seqNo:4960 2021-02-13T00:09:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.99 , 67.05 , 106.52 2021-02-13T00:09:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , -0.15 2021-02-13T00:09:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174996.65 epoch sec; seqNo:10197; 4058533.61 north; 592051.61 east 2021-02-13T00:09:57Z- AHRS_M2 msg: 1613174997.01 epoch sec; seqNo:10115 2021-02-13T00:09:57Z- AHRS_M2 msg: 0.95 phi; -0.37 theta; -0.21 psi 2021-02-13T00:09:57Z- handleDepth()-Depth_Keller msg: 1613174997016 epoch sec; seqNo:10198; depth 23.19 2021-02-13T00:09:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174997.04 epoch sec; seqNo:10198; 4058533.99 north; 592051.61 east 2021-02-13T00:09:57Z- LcmTrn::handleMessages() - handling 43 messages took 0.62 ms and 131 calls... 2021-02-13T00:09:57Z- waiting for 1613174995.18, time is 1613174997.04 2021-02-13T00:09:57Z- LcmTrn::updateTrn() >>>> pose time 1613174997.04, meas time 1613174996.64 <<<< 2021-02-13T00:09:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613174996.64 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:09:57Z- TerrainNav::motionUpdate() - currEstimate = (4058534, 592051.61, 23.19) 2021-02-13T00:09:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:09:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:09:57Z- TNavBF::Projecting Measurements 2021-02-13T00:09:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:09:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:09:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:09:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:09:57Z- TNavFilter::4 good beams. 2021-02-13T00:09:57Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:09:57Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:09:57Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:09:57Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:09:57Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:09:57Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:09:57Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:09:57Z- TNavBF::mmse selected from filter -1 2021-02-13T00:09:57Z- TerrainMapOctree: (vehN, vehE ) = (4058533.80, 592051.32). 2021-02-13T00:09:57Z- TerrainMapOctree: Min Distance = 3494.56. 2021-02-13T00:09:57Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:09:57Z- There is no specific good beam that all particles have in common. 2021-02-13T00:09:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:09:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:09:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:09:57Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:09:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:09:57Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 7.19 2021-02-13T00:09:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:09:57Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 18.15 2021-02-13T00:09:57Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:09:57Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:09:57Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:09:57Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:09:57Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:09:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:09:57Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:09:57Z- Filter 0 No beams used in subcloud update 2021-02-13T00:09:57Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:09:57Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:09:57Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 716.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:09:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613174996.64 sec. 2021-02-13T00:09:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:09:57Z- TNavBF::mmse selected from filter -1 2021-02-13T00:09:57Z- variances([0], [2], and psi_berg) = 716.489239 833.007418 0.000000 2021-02-13T00:09:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:09:57Z- LcmTrn::publishEstimates() - MLE : 4058533.49 592004.11 23.19 2021-02-13T00:09:57Z- LcmTrn::publishEstimates() - MMSE : 4058533.99 592051.61 23.19 2021-02-13T00:09:57Z- LcmTrn::publishEstimates() - COVAR: 716.49 833.01 0.81 0.00 2021-02-13T00:09:57Z- AHRS_M2 msg: 1613174997.42 epoch sec; seqNo:10116 2021-02-13T00:09:57Z- AHRS_M2 msg: 0.94 phi; -0.37 theta; -0.21 psi 2021-02-13T00:09:57Z- handleDepth()-Depth_Keller msg: 1613174997420 epoch sec; seqNo:10199; depth 23.33 2021-02-13T00:09:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:09:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:09:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:09:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:09:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:09:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:09:57Z- handleDvl() - RDI_Pathfinder msg: 1613174997.42 epoch sec; seqNo:4961 2021-02-13T00:09:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:09:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , -0.15 2021-02-13T00:09:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174997.43 epoch sec; seqNo:10199; 4058534.38 north; 592052.83 east 2021-02-13T00:09:57Z- AHRS_M2 msg: 1613174997.82 epoch sec; seqNo:10117 2021-02-13T00:09:57Z- AHRS_M2 msg: 0.93 phi; -0.37 theta; -0.21 psi 2021-02-13T00:09:57Z- handleDepth()-Depth_Keller msg: 1613174997829 epoch sec; seqNo:10200; depth 23.44 2021-02-13T00:09:57Z- handleDvl() - RDI_Pathfinder msg: 1613174997.84 epoch sec; seqNo:4962 2021-02-13T00:09:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.99 , 0.00 , 105.30 2021-02-13T00:09:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , -0.15 2021-02-13T00:09:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174997.85 epoch sec; seqNo:10200; 4058534.38 north; 592052.83 east 2021-02-13T00:09:58Z- AHRS_M2 msg: 1613174998.23 epoch sec; seqNo:10118 2021-02-13T00:09:58Z- AHRS_M2 msg: 0.92 phi; -0.37 theta; -0.21 psi 2021-02-13T00:09:58Z- handleDepth()-Depth_Keller msg: 1613174998237 epoch sec; seqNo:10201; depth 23.60 2021-02-13T00:09:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174998.25 epoch sec; seqNo:10201; 4058534.76 north; 592052.82 east 2021-02-13T00:09:58Z- AHRS_M2 msg: 1613174998.63 epoch sec; seqNo:10119 2021-02-13T00:09:58Z- AHRS_M2 msg: 0.91 phi; -0.36 theta; -0.21 psi 2021-02-13T00:09:58Z- handleDepth()-Depth_Keller msg: 1613174998637 epoch sec; seqNo:10202; depth 23.73 2021-02-13T00:09:58Z- handleDvl() - RDI_Pathfinder msg: 1613174998.66 epoch sec; seqNo:4963 2021-02-13T00:09:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 97.59 , 64.19 , 104.35 2021-02-13T00:09:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , -0.13 2021-02-13T00:09:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174998.67 epoch sec; seqNo:10202; 4058534.76 north; 592052.82 east 2021-02-13T00:09:59Z- AHRS_M2 msg: 1613174999.03 epoch sec; seqNo:10120 2021-02-13T00:09:59Z- AHRS_M2 msg: 0.91 phi; -0.36 theta; -0.22 psi 2021-02-13T00:09:59Z- handleDepth()-Depth_Keller msg: 1613174999036 epoch sec; seqNo:10203; depth 23.89 2021-02-13T00:09:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174999.05 epoch sec; seqNo:10203; 4058535.15 north; 592054.04 east 2021-02-13T00:09:59Z- AHRS_M2 msg: 1613174999.44 epoch sec; seqNo:10121 2021-02-13T00:09:59Z- AHRS_M2 msg: 0.90 phi; -0.36 theta; -0.22 psi 2021-02-13T00:09:59Z- handleDepth()-Depth_Keller msg: 1613174999444 epoch sec; seqNo:10204; depth 24.01 2021-02-13T00:09:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174999.46 epoch sec; seqNo:10204; 4058535.53 north; 592054.03 east 2021-02-13T00:09:59Z- AHRS_M2 msg: 1613174999.84 epoch sec; seqNo:10122 2021-02-13T00:09:59Z- AHRS_M2 msg: 0.89 phi; -0.35 theta; -0.23 psi 2021-02-13T00:09:59Z- handleDepth()-Depth_Keller msg: 1613174999846 epoch sec; seqNo:10205; depth 24.15 2021-02-13T00:09:59Z- handleDvl() - RDI_Pathfinder msg: 1613174999.86 epoch sec; seqNo:4964 2021-02-13T00:09:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.14 , 98.13 , 65.01 , 104.22 2021-02-13T00:09:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.05 , -0.09 2021-02-13T00:09:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613174999.87 epoch sec; seqNo:10205; 4058535.53 north; 592054.03 east 2021-02-13T00:10:00Z- AHRS_M2 msg: 1613175000.24 epoch sec; seqNo:10123 2021-02-13T00:10:00Z- AHRS_M2 msg: 0.88 phi; -0.35 theta; -0.23 psi 2021-02-13T00:10:00Z- handleDepth()-Depth_Keller msg: 1613175000258 epoch sec; seqNo:10206; depth 24.29 2021-02-13T00:10:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175000.27 epoch sec; seqNo:10206; 4058535.91 north; 592054.03 east 2021-02-13T00:10:00Z- AHRS_M2 msg: 1613175000.65 epoch sec; seqNo:10124 2021-02-13T00:10:00Z- AHRS_M2 msg: 0.87 phi; -0.35 theta; -0.23 psi 2021-02-13T00:10:00Z- handleDepth()-Depth_Keller msg: 1613175000652 epoch sec; seqNo:10207; depth 24.44 2021-02-13T00:10:00Z- handleDvl() - RDI_Pathfinder msg: 1613175000.66 epoch sec; seqNo:4965 2021-02-13T00:10:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.91 , 98.26 , 65.01 , 101.92 2021-02-13T00:10:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.07 , -0.03 2021-02-13T00:10:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175000.69 epoch sec; seqNo:10207; 4058535.91 north; 592054.03 east 2021-02-13T00:10:01Z- AHRS_M2 msg: 1613175001.05 epoch sec; seqNo:10125 2021-02-13T00:10:01Z- AHRS_M2 msg: 0.86 phi; -0.35 theta; -0.23 psi 2021-02-13T00:10:01Z- handleDepth()-Depth_Keller msg: 1613175001056 epoch sec; seqNo:10208; depth 24.57 2021-02-13T00:10:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:01Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:01Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:01Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:01Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:01Z- handleDvl() - RDI_Pathfinder msg: 1613175001.06 epoch sec; seqNo:4966 2021-02-13T00:10:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.07 , -0.03 2021-02-13T00:10:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175001.07 epoch sec; seqNo:10208; 4058536.29 north; 592054.03 east 2021-02-13T00:10:01Z- AHRS_M2 msg: 1613175001.47 epoch sec; seqNo:10126 2021-02-13T00:10:01Z- AHRS_M2 msg: 0.85 phi; -0.34 theta; -0.22 psi 2021-02-13T00:10:01Z- handleDepth()-Depth_Keller msg: 1613175001477 epoch sec; seqNo:10209; depth 24.70 2021-02-13T00:10:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175001.50 epoch sec; seqNo:10209; 4058536.30 north; 592055.25 east 2021-02-13T00:10:01Z- AHRS_M2 msg: 1613175001.87 epoch sec; seqNo:10127 2021-02-13T00:10:01Z- AHRS_M2 msg: 0.84 phi; -0.34 theta; -0.22 psi 2021-02-13T00:10:01Z- handleDepth()-Depth_Keller msg: 1613175001870 epoch sec; seqNo:10210; depth 24.85 2021-02-13T00:10:01Z- handleDvl() - RDI_Pathfinder msg: 1613175001.88 epoch sec; seqNo:4967 2021-02-13T00:10:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 93.46 , 65.42 , 101.98 2021-02-13T00:10:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.06 , 0.03 2021-02-13T00:10:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175001.89 epoch sec; seqNo:10210; 4058536.68 north; 592055.24 east 2021-02-13T00:10:02Z- AHRS_M2 msg: 1613175002.26 epoch sec; seqNo:10128 2021-02-13T00:10:02Z- AHRS_M2 msg: 0.84 phi; -0.34 theta; -0.22 psi 2021-02-13T00:10:02Z- handleDepth()-Depth_Keller msg: 1613175002269 epoch sec; seqNo:10211; depth 24.95 2021-02-13T00:10:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:02Z- handleDvl() - RDI_Pathfinder msg: 1613175002.27 epoch sec; seqNo:4968 2021-02-13T00:10:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.06 , 0.03 2021-02-13T00:10:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175002.30 epoch sec; seqNo:10211; 4058536.68 north; 592055.24 east 2021-02-13T00:10:02Z- LcmTrn::handleMessages() - handling 47 messages took 0.68 ms and 134 calls... 2021-02-13T00:10:02Z- waiting for 1613175000.04, time is 1613175002.30 2021-02-13T00:10:02Z- LcmTrn::updateTrn() equating pose 1613175002.30 and meas times 1613175002.30 2021-02-13T00:10:02Z- LcmTrn::updateTrn() >>>> pose time 1613175002.30, meas time 1613175002.30 <<<< 2021-02-13T00:10:02Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175002.30, ping # 0. 2021-02-13T00:10:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175002.30 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:10:02Z- TerrainNav::motionUpdate() - currEstimate = (4058537, 592055.24, 24.95) 2021-02-13T00:10:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:10:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:10:02Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:10:02Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:10:02Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175002.30 sec.; No relevant navigation data available 2021-02-13T00:10:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:10:02Z- TNavBF::mmse selected from filter -1 2021-02-13T00:10:02Z- variances([0], [2], and psi_berg) = 716.420309 833.007375 0.000000 2021-02-13T00:10:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:10:02Z- LcmTrn::publishEstimates() - MLE : 4058535.18 592008.74 24.95 2021-02-13T00:10:02Z- LcmTrn::publishEstimates() - MMSE : 4058536.68 592055.24 24.95 2021-02-13T00:10:02Z- LcmTrn::publishEstimates() - COVAR: 716.42 833.01 1.04 0.00 2021-02-13T00:10:02Z- AHRS_M2 msg: 1613175002.67 epoch sec; seqNo:10129 2021-02-13T00:10:02Z- AHRS_M2 msg: 0.83 phi; -0.34 theta; -0.21 psi 2021-02-13T00:10:02Z- handleDepth()-Depth_Keller msg: 1613175002672 epoch sec; seqNo:10212; depth 25.10 2021-02-13T00:10:02Z- handleDvl() - RDI_Pathfinder msg: 1613175002.68 epoch sec; seqNo:4969 2021-02-13T00:10:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.50 , 98.06 , 65.42 , 100.50 2021-02-13T00:10:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.01 , 0.08 2021-02-13T00:10:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175002.69 epoch sec; seqNo:10212; 4058537.06 north; 592055.24 east 2021-02-13T00:10:03Z- AHRS_M2 msg: 1613175003.07 epoch sec; seqNo:10130 2021-02-13T00:10:03Z- AHRS_M2 msg: 0.83 phi; -0.33 theta; -0.21 psi 2021-02-13T00:10:03Z- handleDepth()-Depth_Keller msg: 1613175003081 epoch sec; seqNo:10213; depth 25.23 2021-02-13T00:10:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175003.10 epoch sec; seqNo:10213; 4058537.06 north; 592055.24 east 2021-02-13T00:10:03Z- AHRS_M2 msg: 1613175003.48 epoch sec; seqNo:10131 2021-02-13T00:10:03Z- AHRS_M2 msg: 0.82 phi; -0.33 theta; -0.21 psi 2021-02-13T00:10:03Z- handleDepth()-Depth_Keller msg: 1613175003480 epoch sec; seqNo:10214; depth 25.34 2021-02-13T00:10:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:03Z- handleDvl() - RDI_Pathfinder msg: 1613175003.48 epoch sec; seqNo:4970 2021-02-13T00:10:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.01 , 0.08 2021-02-13T00:10:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175003.50 epoch sec; seqNo:10214; 4058537.44 north; 592055.24 east 2021-02-13T00:10:03Z- AHRS_M2 msg: 1613175003.88 epoch sec; seqNo:10132 2021-02-13T00:10:03Z- AHRS_M2 msg: 0.82 phi; -0.33 theta; -0.20 psi 2021-02-13T00:10:03Z- handleDepth()-Depth_Keller msg: 1613175003889 epoch sec; seqNo:10215; depth 25.48 2021-02-13T00:10:03Z- handleDvl() - RDI_Pathfinder msg: 1613175003.91 epoch sec; seqNo:4971 2021-02-13T00:10:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 89.81 , 63.78 , 99.55 2021-02-13T00:10:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.02 , 0.10 2021-02-13T00:10:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175003.92 epoch sec; seqNo:10215; 4058537.83 north; 592056.45 east 2021-02-13T00:10:04Z- AHRS_M2 msg: 1613175004.28 epoch sec; seqNo:10133 2021-02-13T00:10:04Z- AHRS_M2 msg: 0.82 phi; -0.33 theta; -0.19 psi 2021-02-13T00:10:04Z- handleDepth()-Depth_Keller msg: 1613175004288 epoch sec; seqNo:10216; depth 25.61 2021-02-13T00:10:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175004.30 epoch sec; seqNo:10216; 4058537.83 north; 592056.45 east 2021-02-13T00:10:04Z- AHRS_M2 msg: 1613175004.69 epoch sec; seqNo:10134 2021-02-13T00:10:04Z- AHRS_M2 msg: 0.83 phi; -0.33 theta; -0.19 psi 2021-02-13T00:10:04Z- handleDepth()-Depth_Keller msg: 1613175004701 epoch sec; seqNo:10217; depth 25.73 2021-02-13T00:10:04Z- handleDvl() - RDI_Pathfinder msg: 1613175004.71 epoch sec; seqNo:4972 2021-02-13T00:10:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:04Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:10:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175004.72 epoch sec; seqNo:10217; 4058538.21 north; 592056.45 east 2021-02-13T00:10:05Z- AHRS_M2 msg: 1613175005.09 epoch sec; seqNo:10135 2021-02-13T00:10:05Z- AHRS_M2 msg: 0.83 phi; -0.33 theta; -0.18 psi 2021-02-13T00:10:05Z- handleDepth()-Depth_Keller msg: 1613175005106 epoch sec; seqNo:10218; depth 25.89 2021-02-13T00:10:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175005.12 epoch sec; seqNo:10218; 4058538.21 north; 592056.45 east 2021-02-13T00:10:05Z- AHRS_M2 msg: 1613175005.50 epoch sec; seqNo:10136 2021-02-13T00:10:05Z- AHRS_M2 msg: 0.83 phi; -0.33 theta; -0.17 psi 2021-02-13T00:10:05Z- handleDepth()-Depth_Keller msg: 1613175005500 epoch sec; seqNo:10219; depth 26.01 2021-02-13T00:10:05Z- handleDvl() - RDI_Pathfinder msg: 1613175005.51 epoch sec; seqNo:4973 2021-02-13T00:10:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.32 , 0.00 , 63.37 , 97.72 2021-02-13T00:10:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , 0.01 2021-02-13T00:10:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175005.54 epoch sec; seqNo:10219; 4058538.59 north; 592056.44 east 2021-02-13T00:10:05Z- AHRS_M2 msg: 1613175005.90 epoch sec; seqNo:10137 2021-02-13T00:10:05Z- AHRS_M2 msg: 0.83 phi; -0.32 theta; -0.17 psi 2021-02-13T00:10:05Z- handleDepth()-Depth_Keller msg: 1613175005909 epoch sec; seqNo:10220; depth 26.12 2021-02-13T00:10:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:05Z- handleDvl() - RDI_Pathfinder msg: 1613175005.91 epoch sec; seqNo:4974 2021-02-13T00:10:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , 0.01 2021-02-13T00:10:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175005.92 epoch sec; seqNo:10220; 4058538.60 north; 592057.66 east 2021-02-13T00:10:06Z- AHRS_M2 msg: 1613175006.31 epoch sec; seqNo:10138 2021-02-13T00:10:06Z- AHRS_M2 msg: 0.84 phi; -0.32 theta; -0.17 psi 2021-02-13T00:10:06Z- handleDepth()-Depth_Keller msg: 1613175006317 epoch sec; seqNo:10221; depth 26.25 2021-02-13T00:10:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175006.33 epoch sec; seqNo:10221; 4058538.98 north; 592057.66 east 2021-02-13T00:10:06Z- AHRS_M2 msg: 1613175006.71 epoch sec; seqNo:10139 2021-02-13T00:10:06Z- AHRS_M2 msg: 0.84 phi; -0.32 theta; -0.17 psi 2021-02-13T00:10:06Z- handleDepth()-Depth_Keller msg: 1613175006717 epoch sec; seqNo:10222; depth 26.37 2021-02-13T00:10:06Z- handleDvl() - RDI_Pathfinder msg: 1613175006.73 epoch sec; seqNo:4975 2021-02-13T00:10:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 86.09 , 62.55 , 97.99 2021-02-13T00:10:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , -0.04 2021-02-13T00:10:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175006.74 epoch sec; seqNo:10222; 4058539.36 north; 592057.66 east 2021-02-13T00:10:07Z- AHRS_M2 msg: 1613175007.11 epoch sec; seqNo:10140 2021-02-13T00:10:07Z- AHRS_M2 msg: 0.84 phi; -0.32 theta; -0.17 psi 2021-02-13T00:10:07Z- handleDepth()-Depth_Keller msg: 1613175007117 epoch sec; seqNo:10223; depth 26.51 2021-02-13T00:10:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:07Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:07Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:07Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:07Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:07Z- handleDvl() - RDI_Pathfinder msg: 1613175007.12 epoch sec; seqNo:4976 2021-02-13T00:10:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , -0.04 2021-02-13T00:10:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175007.13 epoch sec; seqNo:10223; 4058539.36 north; 592057.66 east 2021-02-13T00:10:07Z- LcmTrn::handleMessages() - handling 44 messages took 0.61 ms and 132 calls... 2021-02-13T00:10:07Z- waiting for 1613175005.30, time is 1613175007.13 2021-02-13T00:10:07Z- LcmTrn::updateTrn() equating pose 1613175007.13 and meas times 1613175007.13 2021-02-13T00:10:07Z- LcmTrn::updateTrn() >>>> pose time 1613175007.13, meas time 1613175007.13 <<<< 2021-02-13T00:10:07Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175007.13, ping # 0. 2021-02-13T00:10:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175007.13 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:10:07Z- TerrainNav::motionUpdate() - currEstimate = (4058539, 592057.66, 26.51) 2021-02-13T00:10:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:10:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:10:07Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:10:07Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:10:07Z- TNavBF::Projecting Measurements 2021-02-13T00:10:07Z- TNavFilter::Measurement from time = 1613175007.13 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:10:07Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:10:07Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:10:07Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:10:07Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:10:07Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:10:07Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:10:07Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:10:07Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 716.4 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:10:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:10:07Z- TNavBF::mmse selected from filter -1 2021-02-13T00:10:07Z- variances([0], [2], and psi_berg) = 716.358812 833.007336 0.000000 2021-02-13T00:10:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:10:07Z- LcmTrn::publishEstimates() - MLE : 4058537.86 592011.16 26.51 2021-02-13T00:10:07Z- LcmTrn::publishEstimates() - MMSE : 4058539.36 592057.66 26.51 2021-02-13T00:10:07Z- LcmTrn::publishEstimates() - COVAR: 716.36 833.01 1.22 0.00 2021-02-13T00:10:07Z- AHRS_M2 msg: 1613175007.53 epoch sec; seqNo:10141 2021-02-13T00:10:07Z- AHRS_M2 msg: 0.84 phi; -0.32 theta; -0.16 psi 2021-02-13T00:10:07Z- handleDepth()-Depth_Keller msg: 1613175007531 epoch sec; seqNo:10224; depth 26.62 2021-02-13T00:10:07Z- handleDvl() - RDI_Pathfinder msg: 1613175007.54 epoch sec; seqNo:4977 2021-02-13T00:10:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 84.87 , 63.38 , 97.86 2021-02-13T00:10:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , -0.07 2021-02-13T00:10:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175007.55 epoch sec; seqNo:10224; 4058539.75 north; 592058.87 east 2021-02-13T00:10:07Z- AHRS_M2 msg: 1613175007.92 epoch sec; seqNo:10142 2021-02-13T00:10:07Z- AHRS_M2 msg: 0.84 phi; -0.32 theta; -0.16 psi 2021-02-13T00:10:07Z- handleDepth()-Depth_Keller msg: 1613175007924 epoch sec; seqNo:10225; depth 26.74 2021-02-13T00:10:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175007.94 epoch sec; seqNo:10225; 4058539.75 north; 592058.87 east 2021-02-13T00:10:08Z- AHRS_M2 msg: 1613175008.32 epoch sec; seqNo:10143 2021-02-13T00:10:08Z- AHRS_M2 msg: 0.84 phi; -0.32 theta; -0.16 psi 2021-02-13T00:10:08Z- handleDepth()-Depth_Keller msg: 1613175008333 epoch sec; seqNo:10226; depth 26.88 2021-02-13T00:10:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:08Z- handleDvl() - RDI_Pathfinder msg: 1613175008.34 epoch sec; seqNo:4978 2021-02-13T00:10:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , -0.07 2021-02-13T00:10:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175008.34 epoch sec; seqNo:10226; 4058540.13 north; 592058.87 east 2021-02-13T00:10:08Z- AHRS_M2 msg: 1613175008.73 epoch sec; seqNo:10144 2021-02-13T00:10:08Z- AHRS_M2 msg: 0.85 phi; -0.32 theta; -0.16 psi 2021-02-13T00:10:08Z- handleDepth()-Depth_Keller msg: 1613175008738 epoch sec; seqNo:10227; depth 26.99 2021-02-13T00:10:08Z- handleDvl() - RDI_Pathfinder msg: 1613175008.75 epoch sec; seqNo:4979 2021-02-13T00:10:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.10 , 84.81 , 62.15 , 97.79 2021-02-13T00:10:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , -0.09 2021-02-13T00:10:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175008.76 epoch sec; seqNo:10227; 4058540.51 north; 592058.86 east 2021-02-13T00:10:09Z- AHRS_M2 msg: 1613175009.13 epoch sec; seqNo:10145 2021-02-13T00:10:09Z- AHRS_M2 msg: 0.85 phi; -0.32 theta; -0.16 psi 2021-02-13T00:10:09Z- handleDepth()-Depth_Keller msg: 1613175009147 epoch sec; seqNo:10228; depth 27.12 2021-02-13T00:10:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175009.16 epoch sec; seqNo:10228; 4058540.51 north; 592058.86 east 2021-02-13T00:10:09Z- AHRS_M2 msg: 1613175009.54 epoch sec; seqNo:10146 2021-02-13T00:10:09Z- AHRS_M2 msg: 0.85 phi; -0.32 theta; -0.16 psi 2021-02-13T00:10:09Z- handleDepth()-Depth_Keller msg: 1613175009545 epoch sec; seqNo:10229; depth 27.23 2021-02-13T00:10:09Z- handleDvl() - RDI_Pathfinder msg: 1613175009.56 epoch sec; seqNo:4980 2021-02-13T00:10:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.69 , 85.00 , 62.14 , 97.99 2021-02-13T00:10:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , -0.07 2021-02-13T00:10:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175009.57 epoch sec; seqNo:10229; 4058540.90 north; 592060.08 east 2021-02-13T00:10:09Z- AHRS_M2 msg: 1613175009.94 epoch sec; seqNo:10147 2021-02-13T00:10:09Z- AHRS_M2 msg: 0.86 phi; -0.32 theta; -0.16 psi 2021-02-13T00:10:09Z- handleDepth()-Depth_Keller msg: 1613175009949 epoch sec; seqNo:10230; depth 27.35 2021-02-13T00:10:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175009.96 epoch sec; seqNo:10230; 4058541.28 north; 592060.08 east 2021-02-13T00:10:10Z- AHRS_M2 msg: 1613175010.34 epoch sec; seqNo:10148 2021-02-13T00:10:10Z- AHRS_M2 msg: 0.86 phi; -0.32 theta; -0.16 psi 2021-02-13T00:10:10Z- handleDepth()-Depth_Keller msg: 1613175010350 epoch sec; seqNo:10231; depth 27.47 2021-02-13T00:10:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175010.36 epoch sec; seqNo:10231; 4058541.28 north; 592060.08 east 2021-02-13T00:10:10Z- AHRS_M2 msg: 1613175010.75 epoch sec; seqNo:10149 2021-02-13T00:10:10Z- AHRS_M2 msg: 0.86 phi; -0.32 theta; -0.16 psi 2021-02-13T00:10:10Z- handleDepth()-Depth_Keller msg: 1613175010757 epoch sec; seqNo:10232; depth 27.58 2021-02-13T00:10:10Z- handleDvl() - RDI_Pathfinder msg: 1613175010.78 epoch sec; seqNo:4981 2021-02-13T00:10:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.69 , 82.84 , 62.96 , 97.99 2021-02-13T00:10:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , -0.06 2021-02-13T00:10:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175010.79 epoch sec; seqNo:10232; 4058541.66 north; 592060.07 east 2021-02-13T00:10:11Z- AHRS_M2 msg: 1613175011.15 epoch sec; seqNo:10150 2021-02-13T00:10:11Z- AHRS_M2 msg: 0.87 phi; -0.32 theta; -0.15 psi 2021-02-13T00:10:11Z- handleDepth()-Depth_Keller msg: 1613175011165 epoch sec; seqNo:10233; depth 27.71 2021-02-13T00:10:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175011.18 epoch sec; seqNo:10233; 4058542.04 north; 592060.07 east 2021-02-13T00:10:11Z- AHRS_M2 msg: 1613175011.56 epoch sec; seqNo:10151 2021-02-13T00:10:11Z- AHRS_M2 msg: 0.87 phi; -0.32 theta; -0.15 psi 2021-02-13T00:10:11Z- handleDepth()-Depth_Keller msg: 1613175011562 epoch sec; seqNo:10234; depth 27.83 2021-02-13T00:10:11Z- handleDvl() - RDI_Pathfinder msg: 1613175011.57 epoch sec; seqNo:4982 2021-02-13T00:10:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.28 , 83.92 , 62.14 , 97.99 2021-02-13T00:10:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , -0.05 2021-02-13T00:10:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175011.58 epoch sec; seqNo:10234; 4058542.05 north; 592061.29 east 2021-02-13T00:10:11Z- AHRS_M2 msg: 1613175011.96 epoch sec; seqNo:10152 2021-02-13T00:10:11Z- AHRS_M2 msg: 0.88 phi; -0.31 theta; -0.15 psi 2021-02-13T00:10:11Z- handleDepth()-Depth_Keller msg: 1613175011964 epoch sec; seqNo:10235; depth 27.96 2021-02-13T00:10:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:11Z- handleDvl() - RDI_Pathfinder msg: 1613175011.97 epoch sec; seqNo:4983 2021-02-13T00:10:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , -0.05 2021-02-13T00:10:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175011.98 epoch sec; seqNo:10235; 4058542.43 north; 592061.29 east 2021-02-13T00:10:12Z- AHRS_M2 msg: 1613175012.36 epoch sec; seqNo:10153 2021-02-13T00:10:12Z- AHRS_M2 msg: 0.88 phi; -0.31 theta; -0.15 psi 2021-02-13T00:10:12Z- handleDepth()-Depth_Keller msg: 1613175012369 epoch sec; seqNo:10236; depth 28.09 2021-02-13T00:10:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175012.38 epoch sec; seqNo:10236; 4058542.43 north; 592061.29 east 2021-02-13T00:10:12Z- LcmTrn::handleMessages() - handling 46 messages took 0.67 ms and 133 calls... 2021-02-13T00:10:12Z- waiting for 1613175010.13, time is 1613175012.38 2021-02-13T00:10:12Z- LcmTrn::updateTrn() >>>> pose time 1613175012.38, meas time 1613175011.97 <<<< 2021-02-13T00:10:12Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175011.97, ping # 0. 2021-02-13T00:10:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175011.97 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:10:12Z- TerrainNav::motionUpdate() - currEstimate = (4058542, 592061.29, 28.09) 2021-02-13T00:10:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:10:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:10:12Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:10:12Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:10:12Z- TNavBF::Projecting Measurements 2021-02-13T00:10:12Z- TNavFilter::Measurement from time = 1613175011.97 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:10:12Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:10:12Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:10:12Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:10:12Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:10:12Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:10:12Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:10:12Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:10:12Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 716.3 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T00:10:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:10:12Z- TNavBF::mmse selected from filter -1 2021-02-13T00:10:12Z- variances([0], [2], and psi_berg) = 716.271317 833.007282 0.000000 2021-02-13T00:10:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:10:12Z- LcmTrn::publishEstimates() - MLE : 4058540.93 592012.79 28.09 2021-02-13T00:10:12Z- LcmTrn::publishEstimates() - MMSE : 4058542.43 592061.29 28.09 2021-02-13T00:10:12Z- LcmTrn::publishEstimates() - COVAR: 716.27 833.01 1.47 0.00 2021-02-13T00:10:12Z- AHRS_M2 msg: 1613175012.77 epoch sec; seqNo:10154 2021-02-13T00:10:12Z- AHRS_M2 msg: 0.88 phi; -0.31 theta; -0.14 psi 2021-02-13T00:10:12Z- handleDepth()-Depth_Keller msg: 1613175012777 epoch sec; seqNo:10237; depth 28.19 2021-02-13T00:10:12Z- handleDvl() - RDI_Pathfinder msg: 1613175012.79 epoch sec; seqNo:4984 2021-02-13T00:10:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 82.84 , 61.33 , 91.50 2021-02-13T00:10:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , -0.00 2021-02-13T00:10:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175012.80 epoch sec; seqNo:10237; 4058542.81 north; 592061.28 east 2021-02-13T00:10:13Z- AHRS_M2 msg: 1613175013.17 epoch sec; seqNo:10155 2021-02-13T00:10:13Z- AHRS_M2 msg: 0.88 phi; -0.31 theta; -0.14 psi 2021-02-13T00:10:13Z- handleDepth()-Depth_Keller msg: 1613175013178 epoch sec; seqNo:10238; depth 28.32 2021-02-13T00:10:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:13Z- handleDvl() - RDI_Pathfinder msg: 1613175013.18 epoch sec; seqNo:4985 2021-02-13T00:10:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , -0.00 2021-02-13T00:10:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175013.19 epoch sec; seqNo:10238; 4058543.19 north; 592061.28 east 2021-02-13T00:10:13Z- AHRS_M2 msg: 1613175013.58 epoch sec; seqNo:10156 2021-02-13T00:10:13Z- AHRS_M2 msg: 0.89 phi; -0.31 theta; -0.14 psi 2021-02-13T00:10:13Z- handleDepth()-Depth_Keller msg: 1613175013585 epoch sec; seqNo:10239; depth 28.44 2021-02-13T00:10:13Z- handleDvl() - RDI_Pathfinder msg: 1613175013.60 epoch sec; seqNo:4986 2021-02-13T00:10:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.87 , 81.76 , 61.74 , 92.58 2021-02-13T00:10:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.08 , 0.01 2021-02-13T00:10:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175013.61 epoch sec; seqNo:10239; 4058543.20 north; 592062.50 east 2021-02-13T00:10:13Z- AHRS_M2 msg: 1613175013.98 epoch sec; seqNo:10157 2021-02-13T00:10:13Z- AHRS_M2 msg: 0.89 phi; -0.31 theta; -0.14 psi 2021-02-13T00:10:13Z- handleDepth()-Depth_Keller msg: 1613175013989 epoch sec; seqNo:10240; depth 28.57 2021-02-13T00:10:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175014.00 epoch sec; seqNo:10240; 4058543.58 north; 592062.49 east 2021-02-13T00:10:14Z- AHRS_M2 msg: 1613175014.39 epoch sec; seqNo:10158 2021-02-13T00:10:14Z- AHRS_M2 msg: 0.89 phi; -0.31 theta; -0.14 psi 2021-02-13T00:10:14Z- handleDepth()-Depth_Keller msg: 1613175014399 epoch sec; seqNo:10241; depth 28.67 2021-02-13T00:10:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:14Z- handleDvl() - RDI_Pathfinder msg: 1613175014.40 epoch sec; seqNo:4987 2021-02-13T00:10:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.08 , 0.01 2021-02-13T00:10:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175014.41 epoch sec; seqNo:10241; 4058543.58 north; 592062.49 east 2021-02-13T00:10:14Z- AHRS_M2 msg: 1613175014.79 epoch sec; seqNo:10159 2021-02-13T00:10:14Z- AHRS_M2 msg: 0.89 phi; -0.31 theta; -0.14 psi 2021-02-13T00:10:14Z- handleDepth()-Depth_Keller msg: 1613175014792 epoch sec; seqNo:10242; depth 28.79 2021-02-13T00:10:14Z- handleDvl() - RDI_Pathfinder msg: 1613175014.82 epoch sec; seqNo:4988 2021-02-13T00:10:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.87 , 81.76 , 61.74 , 97.99 2021-02-13T00:10:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , 0.03 2021-02-13T00:10:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175014.83 epoch sec; seqNo:10242; 4058543.96 north; 592062.49 east 2021-02-13T00:10:15Z- AHRS_M2 msg: 1613175015.19 epoch sec; seqNo:10160 2021-02-13T00:10:15Z- AHRS_M2 msg: 0.90 phi; -0.31 theta; -0.15 psi 2021-02-13T00:10:15Z- handleDepth()-Depth_Keller msg: 1613175015196 epoch sec; seqNo:10243; depth 28.91 2021-02-13T00:10:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175015.21 epoch sec; seqNo:10243; 4058543.96 north; 592062.49 east 2021-02-13T00:10:15Z- AHRS_M2 msg: 1613175015.60 epoch sec; seqNo:10161 2021-02-13T00:10:15Z- AHRS_M2 msg: 0.90 phi; -0.31 theta; -0.15 psi 2021-02-13T00:10:15Z- handleDepth()-Depth_Keller msg: 1613175015600 epoch sec; seqNo:10244; depth 29.01 2021-02-13T00:10:15Z- handleDvl() - RDI_Pathfinder msg: 1613175015.61 epoch sec; seqNo:4989 2021-02-13T00:10:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 78.51 , 60.92 , 98.00 2021-02-13T00:10:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.09 , 0.06 2021-02-13T00:10:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175015.62 epoch sec; seqNo:10244; 4058544.35 north; 592063.71 east 2021-02-13T00:10:16Z- handleDepth()-Depth_Keller msg: 1613175016021 epoch sec; seqNo:10245; depth 29.11 2021-02-13T00:10:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175016.03 epoch sec; seqNo:10245; 4058544.35 north; 592063.71 east 2021-02-13T00:10:16Z- AHRS_M2 msg: 1613175016.41 epoch sec; seqNo:10162 2021-02-13T00:10:16Z- AHRS_M2 msg: 0.91 phi; -0.31 theta; -0.15 psi 2021-02-13T00:10:16Z- handleDepth()-Depth_Keller msg: 1613175016413 epoch sec; seqNo:10246; depth 29.24 2021-02-13T00:10:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175016.43 epoch sec; seqNo:10246; 4058544.73 north; 592063.70 east 2021-02-13T00:10:16Z- AHRS_M2 msg: 1613175016.81 epoch sec; seqNo:10163 2021-02-13T00:10:16Z- AHRS_M2 msg: 0.91 phi; -0.31 theta; -0.15 psi 2021-02-13T00:10:16Z- handleDepth()-Depth_Keller msg: 1613175016812 epoch sec; seqNo:10247; depth 29.36 2021-02-13T00:10:16Z- handleDvl() - RDI_Pathfinder msg: 1613175016.82 epoch sec; seqNo:4990 2021-02-13T00:10:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.88 , 82.84 , 60.11 , 90.42 2021-02-13T00:10:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.03 , 0.03 2021-02-13T00:10:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175016.86 epoch sec; seqNo:10247; 4058544.73 north; 592063.70 east 2021-02-13T00:10:17Z- AHRS_M2 msg: 1613175017.21 epoch sec; seqNo:10164 2021-02-13T00:10:17Z- AHRS_M2 msg: 0.91 phi; -0.29 theta; -0.15 psi 2021-02-13T00:10:17Z- handleDepth()-Depth_Keller msg: 1613175017216 epoch sec; seqNo:10248; depth 29.49 2021-02-13T00:10:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175017.23 epoch sec; seqNo:10248; 4058545.12 north; 592064.92 east 2021-02-13T00:10:17Z- LcmTrn::handleMessages() - handling 42 messages took 0.68 ms and 129 calls... 2021-02-13T00:10:17Z- waiting for 1613175015.38, time is 1613175017.23 2021-02-13T00:10:17Z- LcmTrn::updateTrn() >>>> pose time 1613175017.23, meas time 1613175016.82 <<<< 2021-02-13T00:10:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175016.82 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:10:17Z- TerrainNav::motionUpdate() - currEstimate = (4058545, 592064.92, 29.49) 2021-02-13T00:10:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:10:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:10:17Z- TNavBF::Projecting Measurements 2021-02-13T00:10:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:10:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:10:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:10:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:10:17Z- TNavFilter::4 good beams. 2021-02-13T00:10:17Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:10:17Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:10:17Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:10:17Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:10:17Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:10:17Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:10:17Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:10:17Z- TNavBF::mmse selected from filter -1 2021-02-13T00:10:17Z- TerrainMapOctree: (vehN, vehE ) = (4058544.89, 592064.62). 2021-02-13T00:10:17Z- TerrainMapOctree: Min Distance = 3477.57. 2021-02-13T00:10:17Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:10:17Z- There is no specific good beam that all particles have in common. 2021-02-13T00:10:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:10:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.68 K: 0.156 ExpectedMeasDiff: nan currMeas.covariance: 7.37 2021-02-13T00:10:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:10:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.42 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 10.98 2021-02-13T00:10:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:10:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.145 ExpectedMeasDiff: nan currMeas.covariance: 5.78 2021-02-13T00:10:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:10:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 13.08 2021-02-13T00:10:17Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:10:17Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:10:17Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:10:17Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:10:17Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:10:17Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:10:17Z- Filter 0 No beams used in subcloud update 2021-02-13T00:10:17Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:10:17Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:10:17Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 716.2 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:10:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175016.82 sec. 2021-02-13T00:10:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:10:17Z- TNavBF::mmse selected from filter -1 2021-02-13T00:10:17Z- variances([0], [2], and psi_berg) = 716.185622 833.007228 0.000000 2021-02-13T00:10:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:10:17Z- LcmTrn::publishEstimates() - MLE : 4058543.62 592016.42 29.49 2021-02-13T00:10:17Z- LcmTrn::publishEstimates() - MMSE : 4058545.12 592064.92 29.49 2021-02-13T00:10:17Z- LcmTrn::publishEstimates() - COVAR: 716.19 833.01 1.03 0.00 2021-02-13T00:10:17Z- AHRS_M2 msg: 1613175017.62 epoch sec; seqNo:10165 2021-02-13T00:10:17Z- AHRS_M2 msg: 0.91 phi; -0.27 theta; -0.16 psi 2021-02-13T00:10:17Z- handleDepth()-Depth_Keller msg: 1613175017629 epoch sec; seqNo:10249; depth 29.61 2021-02-13T00:10:17Z- handleDvl() - RDI_Pathfinder msg: 1613175017.64 epoch sec; seqNo:4991 2021-02-13T00:10:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.82 , 80.68 , 61.74 , 0.00 2021-02-13T00:10:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.03 , -0.00 2021-02-13T00:10:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175017.65 epoch sec; seqNo:10249; 4058545.12 north; 592064.92 east 2021-02-13T00:10:18Z- AHRS_M2 msg: 1613175018.02 epoch sec; seqNo:10166 2021-02-13T00:10:18Z- AHRS_M2 msg: 0.90 phi; -0.24 theta; -0.16 psi 2021-02-13T00:10:18Z- handleDepth()-Depth_Keller msg: 1613175018024 epoch sec; seqNo:10250; depth 29.74 2021-02-13T00:10:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:18Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:18Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:18Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:18Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:18Z- handleDvl() - RDI_Pathfinder msg: 1613175018.03 epoch sec; seqNo:4992 2021-02-13T00:10:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.03 , -0.00 2021-02-13T00:10:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175018.05 epoch sec; seqNo:10250; 4058545.50 north; 592064.92 east 2021-02-13T00:10:18Z- AHRS_M2 msg: 1613175018.42 epoch sec; seqNo:10167 2021-02-13T00:10:18Z- AHRS_M2 msg: 0.90 phi; -0.20 theta; -0.16 psi 2021-02-13T00:10:18Z- handleDepth()-Depth_Keller msg: 1613175018428 epoch sec; seqNo:10251; depth 29.88 2021-02-13T00:10:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175018.44 epoch sec; seqNo:10251; 4058545.50 north; 592064.92 east 2021-02-13T00:10:18Z- AHRS_M2 msg: 1613175018.84 epoch sec; seqNo:10168 2021-02-13T00:10:18Z- AHRS_M2 msg: 0.88 phi; -0.12 theta; -0.14 psi 2021-02-13T00:10:18Z- handleDepth()-Depth_Keller msg: 1613175018840 epoch sec; seqNo:10252; depth 29.96 2021-02-13T00:10:18Z- handleDvl() - RDI_Pathfinder msg: 1613175018.85 epoch sec; seqNo:4993 2021-02-13T00:10:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 78.51 , 61.74 , 158.60 2021-02-13T00:10:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.04 , -0.04 2021-02-13T00:10:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175018.86 epoch sec; seqNo:10252; 4058545.88 north; 592064.91 east 2021-02-13T00:10:19Z- AHRS_M2 msg: 1613175019.24 epoch sec; seqNo:10169 2021-02-13T00:10:19Z- AHRS_M2 msg: 0.88 phi; -0.07 theta; -0.15 psi 2021-02-13T00:10:19Z- handleDepth()-Depth_Keller msg: 1613175019255 epoch sec; seqNo:10253; depth 30.08 2021-02-13T00:10:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:19Z- handleDvl() - RDI_Pathfinder msg: 1613175019.26 epoch sec; seqNo:4994 2021-02-13T00:10:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.04 , -0.04 2021-02-13T00:10:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175019.27 epoch sec; seqNo:10253; 4058546.27 north; 592066.13 east 2021-02-13T00:10:19Z- AHRS_M2 msg: 1613175019.64 epoch sec; seqNo:10170 2021-02-13T00:10:19Z- AHRS_M2 msg: 0.87 phi; -0.03 theta; -0.15 psi 2021-02-13T00:10:19Z- handleDepth()-Depth_Keller msg: 1613175019649 epoch sec; seqNo:10254; depth 30.19 2021-02-13T00:10:19Z- handleDvl() - RDI_Pathfinder msg: 1613175019.67 epoch sec; seqNo:4995 2021-02-13T00:10:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.96 , 78.11 , 64.19 , 0.00 2021-02-13T00:10:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , -0.03 2021-02-13T00:10:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175019.68 epoch sec; seqNo:10254; 4058546.27 north; 592066.13 east 2021-02-13T00:10:20Z- AHRS_M2 msg: 1613175020.05 epoch sec; seqNo:10171 2021-02-13T00:10:20Z- AHRS_M2 msg: 0.86 phi; 0.01 theta; -0.15 psi 2021-02-13T00:10:20Z- handleDepth()-Depth_Keller msg: 1613175020052 epoch sec; seqNo:10255; depth 30.24 2021-02-13T00:10:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175020.09 epoch sec; seqNo:10255; 4058546.65 north; 592066.12 east 2021-02-13T00:10:20Z- AHRS_M2 msg: 1613175020.45 epoch sec; seqNo:10172 2021-02-13T00:10:20Z- AHRS_M2 msg: 0.85 phi; 0.05 theta; -0.16 psi 2021-02-13T00:10:20Z- handleDepth()-Depth_Keller msg: 1613175020458 epoch sec; seqNo:10256; depth 30.30 2021-02-13T00:10:20Z- handleDvl() - RDI_Pathfinder msg: 1613175020.47 epoch sec; seqNo:4996 2021-02-13T00:10:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.14 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:20Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:10:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175020.48 epoch sec; seqNo:10256; 4058546.65 north; 592066.12 east 2021-02-13T00:10:20Z- AHRS_M2 msg: 1613175020.86 epoch sec; seqNo:10173 2021-02-13T00:10:20Z- AHRS_M2 msg: 0.84 phi; 0.09 theta; -0.16 psi 2021-02-13T00:10:20Z- handleDepth()-Depth_Keller msg: 1613175020863 epoch sec; seqNo:10257; depth 30.37 2021-02-13T00:10:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175020.88 epoch sec; seqNo:10257; 4058547.04 north; 592067.34 east 2021-02-13T00:10:21Z- AHRS_M2 msg: 1613175021.26 epoch sec; seqNo:10174 2021-02-13T00:10:21Z- AHRS_M2 msg: 0.83 phi; 0.12 theta; -0.16 psi 2021-02-13T00:10:21Z- handleDepth()-Depth_Keller msg: 1613175021260 epoch sec; seqNo:10258; depth 30.39 2021-02-13T00:10:21Z- handleDvl() - RDI_Pathfinder msg: 1613175021.29 epoch sec; seqNo:4997 2021-02-13T00:10:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.06 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:21Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:10:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175021.30 epoch sec; seqNo:10258; 4058547.42 north; 592067.34 east 2021-02-13T00:10:21Z- AHRS_M2 msg: 1613175021.66 epoch sec; seqNo:10175 2021-02-13T00:10:21Z- AHRS_M2 msg: 0.82 phi; 0.16 theta; -0.16 psi 2021-02-13T00:10:21Z- handleDepth()-Depth_Keller msg: 1613175021664 epoch sec; seqNo:10259; depth 30.40 2021-02-13T00:10:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:21Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:21Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:21Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:21Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:21Z- handleDvl() - RDI_Pathfinder msg: 1613175021.67 epoch sec; seqNo:4998 2021-02-13T00:10:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:21Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:10:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175021.68 epoch sec; seqNo:10259; 4058547.42 north; 592067.34 east 2021-02-13T00:10:22Z- AHRS_M2 msg: 1613175022.06 epoch sec; seqNo:10176 2021-02-13T00:10:22Z- AHRS_M2 msg: 0.82 phi; 0.18 theta; -0.16 psi 2021-02-13T00:10:22Z- handleDepth()-Depth_Keller msg: 1613175022073 epoch sec; seqNo:10260; depth 30.42 2021-02-13T00:10:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175022.08 epoch sec; seqNo:10260; 4058547.80 north; 592067.33 east 2021-02-13T00:10:22Z- AHRS_M2 msg: 1613175022.47 epoch sec; seqNo:10177 2021-02-13T00:10:22Z- AHRS_M2 msg: 0.81 phi; 0.21 theta; -0.16 psi 2021-02-13T00:10:22Z- handleDepth()-Depth_Keller msg: 1613175022478 epoch sec; seqNo:10261; depth 30.41 2021-02-13T00:10:22Z- handleDvl() - RDI_Pathfinder msg: 1613175022.49 epoch sec; seqNo:4999 2021-02-13T00:10:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.91 , 0.00 , 0.00 , 63.06 2021-02-13T00:10:22Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:10:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175022.50 epoch sec; seqNo:10261; 4058547.80 north; 592067.33 east 2021-02-13T00:10:22Z- LcmTrn::handleMessages() - handling 48 messages took 0.63 ms and 136 calls... 2021-02-13T00:10:22Z- waiting for 1613175020.23, time is 1613175022.50 2021-02-13T00:10:22Z- LcmTrn::updateTrn() equating pose 1613175022.50 and meas times 1613175022.50 2021-02-13T00:10:22Z- LcmTrn::updateTrn() >>>> pose time 1613175022.50, meas time 1613175022.50 <<<< 2021-02-13T00:10:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175022.50 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:10:22Z- TerrainNav::motionUpdate() - currEstimate = (4058548, 592067.33, 30.41) 2021-02-13T00:10:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:10:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:10:22Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:10:22Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:10:22Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175022.50 sec.; No relevant navigation data available 2021-02-13T00:10:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:10:22Z- TNavBF::mmse selected from filter -1 2021-02-13T00:10:22Z- variances([0], [2], and psi_berg) = 716.124287 833.007190 0.000000 2021-02-13T00:10:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:10:22Z- LcmTrn::publishEstimates() - MLE : 4058546.30 592018.83 30.41 2021-02-13T00:10:22Z- LcmTrn::publishEstimates() - MMSE : 4058547.80 592067.33 30.41 2021-02-13T00:10:22Z- LcmTrn::publishEstimates() - COVAR: 716.12 833.01 1.22 0.00 2021-02-13T00:10:22Z- AHRS_M2 msg: 1613175022.87 epoch sec; seqNo:10178 2021-02-13T00:10:22Z- AHRS_M2 msg: 0.81 phi; 0.23 theta; -0.16 psi 2021-02-13T00:10:22Z- handleDepth()-Depth_Keller msg: 1613175022876 epoch sec; seqNo:10262; depth 30.38 2021-02-13T00:10:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:22Z- handleDvl() - RDI_Pathfinder msg: 1613175022.88 epoch sec; seqNo:5000 2021-02-13T00:10:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:22Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:10:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175022.90 epoch sec; seqNo:10262; 4058548.19 north; 592068.55 east 2021-02-13T00:10:23Z- AHRS_M2 msg: 1613175023.28 epoch sec; seqNo:10179 2021-02-13T00:10:23Z- AHRS_M2 msg: 0.80 phi; 0.26 theta; -0.16 psi 2021-02-13T00:10:23Z- handleDepth()-Depth_Keller msg: 1613175023285 epoch sec; seqNo:10263; depth 30.35 2021-02-13T00:10:23Z- handleDvl() - RDI_Pathfinder msg: 1613175023.30 epoch sec; seqNo:5001 2021-02-13T00:10:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 65.19 , 0.00 , 0.00 , 62.21 2021-02-13T00:10:23Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:10:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175023.33 epoch sec; seqNo:10263; 4058548.57 north; 592068.55 east 2021-02-13T00:10:23Z- AHRS_M2 msg: 1613175023.68 epoch sec; seqNo:10180 2021-02-13T00:10:23Z- AHRS_M2 msg: 0.80 phi; 0.28 theta; -0.15 psi 2021-02-13T00:10:23Z- handleDepth()-Depth_Keller msg: 1613175023684 epoch sec; seqNo:10264; depth 30.32 2021-02-13T00:10:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175023.69 epoch sec; seqNo:10264; 4058548.57 north; 592068.55 east 2021-02-13T00:10:24Z- AHRS_M2 msg: 1613175024.08 epoch sec; seqNo:10181 2021-02-13T00:10:24Z- AHRS_M2 msg: 0.80 phi; 0.29 theta; -0.15 psi 2021-02-13T00:10:24Z- handleDepth()-Depth_Keller msg: 1613175024090 epoch sec; seqNo:10265; depth 30.26 2021-02-13T00:10:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:24Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:24Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:24Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:24Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:24Z- handleDvl() - RDI_Pathfinder msg: 1613175024.09 epoch sec; seqNo:5002 2021-02-13T00:10:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:24Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:10:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175024.10 epoch sec; seqNo:10265; 4058548.95 north; 592068.54 east 2021-02-13T00:10:24Z- AHRS_M2 msg: 1613175024.49 epoch sec; seqNo:10182 2021-02-13T00:10:24Z- AHRS_M2 msg: 0.80 phi; 0.31 theta; -0.15 psi 2021-02-13T00:10:24Z- handleDepth()-Depth_Keller msg: 1613175024492 epoch sec; seqNo:10266; depth 30.19 2021-02-13T00:10:24Z- handleDvl() - RDI_Pathfinder msg: 1613175024.50 epoch sec; seqNo:5003 2021-02-13T00:10:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:24Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:10:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175024.54 epoch sec; seqNo:10266; 4058548.95 north; 592068.54 east 2021-02-13T00:10:24Z- AHRS_M2 msg: 1613175024.89 epoch sec; seqNo:10183 2021-02-13T00:10:24Z- AHRS_M2 msg: 0.80 phi; 0.32 theta; -0.16 psi 2021-02-13T00:10:24Z- handleDepth()-Depth_Keller msg: 1613175024897 epoch sec; seqNo:10267; depth 30.13 2021-02-13T00:10:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175024.91 epoch sec; seqNo:10267; 4058549.34 north; 592069.76 east 2021-02-13T00:10:25Z- AHRS_M2 msg: 1613175025.30 epoch sec; seqNo:10184 2021-02-13T00:10:25Z- AHRS_M2 msg: 0.79 phi; 0.33 theta; -0.16 psi 2021-02-13T00:10:25Z- handleDepth()-Depth_Keller msg: 1613175025302 epoch sec; seqNo:10268; depth 30.04 2021-02-13T00:10:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:25Z- handleDvl() - RDI_Pathfinder msg: 1613175025.30 epoch sec; seqNo:5004 2021-02-13T00:10:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:25Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:10:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175025.31 epoch sec; seqNo:10268; 4058549.72 north; 592069.75 east 2021-02-13T00:10:25Z- AHRS_M2 msg: 1613175025.70 epoch sec; seqNo:10185 2021-02-13T00:10:25Z- AHRS_M2 msg: 0.79 phi; 0.34 theta; -0.16 psi 2021-02-13T00:10:25Z- handleDepth()-Depth_Keller msg: 1613175025704 epoch sec; seqNo:10269; depth 29.97 2021-02-13T00:10:25Z- handleDvl() - RDI_Pathfinder msg: 1613175025.72 epoch sec; seqNo:5005 2021-02-13T00:10:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.05 , 80.68 , 97.99 , 61.74 2021-02-13T00:10:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , -0.02 2021-02-13T00:10:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175025.73 epoch sec; seqNo:10269; 4058549.72 north; 592069.75 east 2021-02-13T00:10:26Z- AHRS_M2 msg: 1613175026.10 epoch sec; seqNo:10186 2021-02-13T00:10:26Z- AHRS_M2 msg: 0.79 phi; 0.35 theta; -0.16 psi 2021-02-13T00:10:26Z- handleDepth()-Depth_Keller msg: 1613175026113 epoch sec; seqNo:10270; depth 29.86 2021-02-13T00:10:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175026.13 epoch sec; seqNo:10270; 4058550.10 north; 592069.75 east 2021-02-13T00:10:26Z- AHRS_M2 msg: 1613175026.51 epoch sec; seqNo:10187 2021-02-13T00:10:26Z- AHRS_M2 msg: 0.79 phi; 0.35 theta; -0.16 psi 2021-02-13T00:10:26Z- handleDepth()-Depth_Keller msg: 1613175026512 epoch sec; seqNo:10271; depth 29.75 2021-02-13T00:10:26Z- handleDvl() - RDI_Pathfinder msg: 1613175026.53 epoch sec; seqNo:5006 2021-02-13T00:10:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.05 , 79.72 , 97.99 , 60.51 2021-02-13T00:10:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.04 , 0.01 2021-02-13T00:10:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175026.54 epoch sec; seqNo:10271; 4058550.10 north; 592069.75 east 2021-02-13T00:10:26Z- AHRS_M2 msg: 1613175026.91 epoch sec; seqNo:10188 2021-02-13T00:10:26Z- AHRS_M2 msg: 0.79 phi; 0.36 theta; -0.15 psi 2021-02-13T00:10:26Z- handleDepth()-Depth_Keller msg: 1613175026925 epoch sec; seqNo:10272; depth 29.66 2021-02-13T00:10:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175026.94 epoch sec; seqNo:10272; 4058550.49 north; 592070.97 east 2021-02-13T00:10:27Z- AHRS_M2 msg: 1613175027.32 epoch sec; seqNo:10189 2021-02-13T00:10:27Z- AHRS_M2 msg: 0.79 phi; 0.37 theta; -0.15 psi 2021-02-13T00:10:27Z- handleDepth()-Depth_Keller msg: 1613175027323 epoch sec; seqNo:10273; depth 29.54 2021-02-13T00:10:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175027.33 epoch sec; seqNo:10273; 4058550.49 north; 592070.97 east 2021-02-13T00:10:27Z- AHRS_M2 msg: 1613175027.72 epoch sec; seqNo:10190 2021-02-13T00:10:27Z- AHRS_M2 msg: 0.79 phi; 0.37 theta; -0.15 psi 2021-02-13T00:10:27Z- LcmTrn::handleMessages() - handling 44 messages took 0.75 ms and 129 calls... 2021-02-13T00:10:27Z- waiting for 1613175025.50, time is 1613175027.72 2021-02-13T00:10:27Z- LcmTrn::updateTrn() >>>> pose time 1613175027.72, meas time 1613175026.53 <<<< 2021-02-13T00:10:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175026.53 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:10:27Z- TerrainNav::motionUpdate() - currEstimate = (4058550, 592070.97, 29.54) 2021-02-13T00:10:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:10:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:10:27Z- TNavBF::Projecting Measurements 2021-02-13T00:10:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:10:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:10:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:10:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:10:27Z- TNavFilter::4 good beams. 2021-02-13T00:10:27Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:10:27Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:10:27Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:10:27Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:10:27Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:10:27Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:10:27Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:10:27Z- TNavBF::mmse selected from filter -1 2021-02-13T00:10:27Z- TerrainMapOctree: (vehN, vehE ) = (4058549.87, 592070.13). 2021-02-13T00:10:27Z- TerrainMapOctree: Min Distance = 3470.23. 2021-02-13T00:10:27Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:10:27Z- There is no specific good beam that all particles have in common. 2021-02-13T00:10:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:10:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.135 ExpectedMeasDiff: nan currMeas.covariance: 7.19 2021-02-13T00:10:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:10:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 10.17 2021-02-13T00:10:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:10:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:10:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:10:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.119 ExpectedMeasDiff: nan currMeas.covariance: 5.86 2021-02-13T00:10:27Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:10:27Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:10:27Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:10:27Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:10:27Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:10:27Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:10:27Z- Filter 0 No beams used in subcloud update 2021-02-13T00:10:27Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:10:27Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:10:27Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 716.1 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:10:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175026.53 sec. 2021-02-13T00:10:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:10:27Z- TNavBF::mmse selected from filter -1 2021-02-13T00:10:27Z- variances([0], [2], and psi_berg) = 716.031183 833.007132 0.000000 2021-02-13T00:10:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:10:27Z- LcmTrn::publishEstimates() - MLE : 4058548.99 592021.47 29.54 2021-02-13T00:10:27Z- LcmTrn::publishEstimates() - MMSE : 4058550.49 592070.97 29.54 2021-02-13T00:10:27Z- LcmTrn::publishEstimates() - COVAR: 716.03 833.01 0.96 0.00 2021-02-13T00:10:27Z- handleDepth()-Depth_Keller msg: 1613175027729 epoch sec; seqNo:10274; depth 29.43 2021-02-13T00:10:27Z- handleDvl() - RDI_Pathfinder msg: 1613175027.74 epoch sec; seqNo:5007 2021-02-13T00:10:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.10 , 0.00 , 98.00 , 61.33 2021-02-13T00:10:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , 0.05 2021-02-13T00:10:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175027.77 epoch sec; seqNo:10274; 4058550.87 north; 592070.96 east 2021-02-13T00:10:28Z- AHRS_M2 msg: 1613175028.13 epoch sec; seqNo:10191 2021-02-13T00:10:28Z- AHRS_M2 msg: 0.79 phi; 0.38 theta; -0.14 psi 2021-02-13T00:10:28Z- handleDepth()-Depth_Keller msg: 1613175028134 epoch sec; seqNo:10275; depth 29.33 2021-02-13T00:10:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175028.15 epoch sec; seqNo:10275; 4058551.25 north; 592070.96 east 2021-02-13T00:10:28Z- AHRS_M2 msg: 1613175028.53 epoch sec; seqNo:10192 2021-02-13T00:10:28Z- AHRS_M2 msg: 0.79 phi; 0.38 theta; -0.14 psi 2021-02-13T00:10:28Z- handleDepth()-Depth_Keller msg: 1613175028541 epoch sec; seqNo:10276; depth 29.21 2021-02-13T00:10:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175028.55 epoch sec; seqNo:10276; 4058551.25 north; 592070.96 east 2021-02-13T00:10:28Z- AHRS_M2 msg: 1613175028.93 epoch sec; seqNo:10193 2021-02-13T00:10:28Z- AHRS_M2 msg: 0.79 phi; 0.38 theta; -0.15 psi 2021-02-13T00:10:28Z- handleDepth()-Depth_Keller msg: 1613175028936 epoch sec; seqNo:10277; depth 29.08 2021-02-13T00:10:28Z- handleDvl() - RDI_Pathfinder msg: 1613175028.95 epoch sec; seqNo:5008 2021-02-13T00:10:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 82.84 , 98.00 , 61.33 2021-02-13T00:10:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , 0.06 2021-02-13T00:10:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175028.97 epoch sec; seqNo:10277; 4058551.63 north; 592070.95 east 2021-02-13T00:10:29Z- AHRS_M2 msg: 1613175029.34 epoch sec; seqNo:10194 2021-02-13T00:10:29Z- AHRS_M2 msg: 0.79 phi; 0.38 theta; -0.15 psi 2021-02-13T00:10:29Z- handleDepth()-Depth_Keller msg: 1613175029349 epoch sec; seqNo:10278; depth 28.96 2021-02-13T00:10:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175029.36 epoch sec; seqNo:10278; 4058551.64 north; 592072.18 east 2021-02-13T00:10:29Z- AHRS_M2 msg: 1613175029.74 epoch sec; seqNo:10195 2021-02-13T00:10:29Z- AHRS_M2 msg: 0.79 phi; 0.38 theta; -0.15 psi 2021-02-13T00:10:29Z- handleDepth()-Depth_Keller msg: 1613175029749 epoch sec; seqNo:10279; depth 28.84 2021-02-13T00:10:29Z- handleDvl() - RDI_Pathfinder msg: 1613175029.76 epoch sec; seqNo:5009 2021-02-13T00:10:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.92 , 83.86 , 99.01 , 62.15 2021-02-13T00:10:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.06 , 0.03 2021-02-13T00:10:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175029.77 epoch sec; seqNo:10279; 4058552.02 north; 592072.17 east 2021-02-13T00:10:30Z- AHRS_M2 msg: 1613175030.15 epoch sec; seqNo:10196 2021-02-13T00:10:30Z- AHRS_M2 msg: 0.80 phi; 0.38 theta; -0.16 psi 2021-02-13T00:10:30Z- handleDepth()-Depth_Keller msg: 1613175030160 epoch sec; seqNo:10280; depth 28.70 2021-02-13T00:10:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:30Z- handleDvl() - RDI_Pathfinder msg: 1613175030.16 epoch sec; seqNo:5010 2021-02-13T00:10:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.06 , 0.03 2021-02-13T00:10:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175030.18 epoch sec; seqNo:10280; 4058552.40 north; 592072.17 east 2021-02-13T00:10:30Z- AHRS_M2 msg: 1613175030.55 epoch sec; seqNo:10197 2021-02-13T00:10:30Z- AHRS_M2 msg: 0.80 phi; 0.38 theta; -0.16 psi 2021-02-13T00:10:30Z- handleDepth()-Depth_Keller msg: 1613175030557 epoch sec; seqNo:10281; depth 28.58 2021-02-13T00:10:30Z- handleDvl() - RDI_Pathfinder msg: 1613175030.57 epoch sec; seqNo:5011 2021-02-13T00:10:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.51 , 0.00 , 99.08 , 61.74 2021-02-13T00:10:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.09 , -0.02 2021-02-13T00:10:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175030.59 epoch sec; seqNo:10281; 4058552.40 north; 592072.17 east 2021-02-13T00:10:30Z- AHRS_M2 msg: 1613175030.95 epoch sec; seqNo:10198 2021-02-13T00:10:30Z- AHRS_M2 msg: 0.80 phi; 0.37 theta; -0.16 psi 2021-02-13T00:10:30Z- handleDepth()-Depth_Keller msg: 1613175030961 epoch sec; seqNo:10282; depth 28.45 2021-02-13T00:10:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175030.97 epoch sec; seqNo:10282; 4058552.79 north; 592073.38 east 2021-02-13T00:10:31Z- AHRS_M2 msg: 1613175031.36 epoch sec; seqNo:10199 2021-02-13T00:10:31Z- AHRS_M2 msg: 0.81 phi; 0.37 theta; -0.16 psi 2021-02-13T00:10:31Z- handleDepth()-Depth_Keller msg: 1613175031361 epoch sec; seqNo:10283; depth 28.34 2021-02-13T00:10:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:31Z- handleDvl() - RDI_Pathfinder msg: 1613175031.36 epoch sec; seqNo:5012 2021-02-13T00:10:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.09 , -0.02 2021-02-13T00:10:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175031.38 epoch sec; seqNo:10283; 4058552.79 north; 592073.38 east 2021-02-13T00:10:31Z- AHRS_M2 msg: 1613175031.76 epoch sec; seqNo:10200 2021-02-13T00:10:31Z- AHRS_M2 msg: 0.82 phi; 0.37 theta; -0.16 psi 2021-02-13T00:10:31Z- handleDepth()-Depth_Keller msg: 1613175031769 epoch sec; seqNo:10284; depth 28.20 2021-02-13T00:10:31Z- handleDvl() - RDI_Pathfinder msg: 1613175031.79 epoch sec; seqNo:5013 2021-02-13T00:10:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.33 , 83.93 , 99.08 , 60.92 2021-02-13T00:10:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.05 , -0.07 2021-02-13T00:10:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175031.80 epoch sec; seqNo:10284; 4058553.17 north; 592073.38 east 2021-02-13T00:10:32Z- AHRS_M2 msg: 1613175032.16 epoch sec; seqNo:10201 2021-02-13T00:10:32Z- AHRS_M2 msg: 0.82 phi; 0.37 theta; -0.16 psi 2021-02-13T00:10:32Z- handleDepth()-Depth_Keller msg: 1613175032168 epoch sec; seqNo:10285; depth 28.07 2021-02-13T00:10:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175032.18 epoch sec; seqNo:10285; 4058553.17 north; 592073.38 east 2021-02-13T00:10:32Z- AHRS_M2 msg: 1613175032.57 epoch sec; seqNo:10202 2021-02-13T00:10:32Z- AHRS_M2 msg: 0.82 phi; 0.36 theta; -0.16 psi 2021-02-13T00:10:32Z- handleDepth()-Depth_Keller msg: 1613175032574 epoch sec; seqNo:10286; depth 27.95 2021-02-13T00:10:32Z- handleDvl() - RDI_Pathfinder msg: 1613175032.59 epoch sec; seqNo:5014 2021-02-13T00:10:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.33 , 0.00 , 98.00 , 62.97 2021-02-13T00:10:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.01 , -0.08 2021-02-13T00:10:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175032.60 epoch sec; seqNo:10286; 4058553.55 north; 592073.38 east 2021-02-13T00:10:32Z- LcmTrn::handleMessages() - handling 46 messages took 0.64 ms and 133 calls... 2021-02-13T00:10:32Z- waiting for 1613175030.72, time is 1613175032.60 2021-02-13T00:10:32Z- LcmTrn::updateTrn() equating pose 1613175032.60 and meas times 1613175032.60 2021-02-13T00:10:32Z- LcmTrn::updateTrn() >>>> pose time 1613175032.60, meas time 1613175032.60 <<<< 2021-02-13T00:10:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175032.60 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:10:32Z- TerrainNav::motionUpdate() - currEstimate = (4058554, 592073.38, 27.95) 2021-02-13T00:10:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:10:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:10:32Z- TNavBF::Projecting Measurements 2021-02-13T00:10:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:10:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:10:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:10:32Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:10:32Z- TNavFilter::3 good beams. 2021-02-13T00:10:32Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:10:32Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:10:32Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:10:32Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:10:32Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:10:32Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:10:32Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:10:32Z- TNavBF::mmse selected from filter -1 2021-02-13T00:10:32Z- TerrainMapOctree: (vehN, vehE ) = (4058553.55, 592073.38). 2021-02-13T00:10:32Z- TerrainMapOctree: Min Distance = 3465.34. 2021-02-13T00:10:32Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:10:32Z- There is no specific good beam that all particles have in common. 2021-02-13T00:10:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:10:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 7.91 2021-02-13T00:10:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:10:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 15.37 2021-02-13T00:10:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:10:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.108 ExpectedMeasDiff: nan currMeas.covariance: 6.34 2021-02-13T00:10:32Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:10:32Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:10:32Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:10:32Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:10:32Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:10:32Z- Filter 0 No beams used in subcloud update 2021-02-13T00:10:32Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:10:32Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:10:32Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 716.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:10:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175032.60 sec. 2021-02-13T00:10:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:10:32Z- TNavBF::mmse selected from filter -1 2021-02-13T00:10:32Z- variances([0], [2], and psi_berg) = 715.967562 833.007092 0.000000 2021-02-13T00:10:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:10:32Z- LcmTrn::publishEstimates() - MLE : 4058552.05 592023.88 27.95 2021-02-13T00:10:32Z- LcmTrn::publishEstimates() - MMSE : 4058553.55 592073.38 27.95 2021-02-13T00:10:32Z- LcmTrn::publishEstimates() - COVAR: 715.97 833.01 0.87 0.00 2021-02-13T00:10:32Z- handleDepth()-Depth_Keller msg: 1613175032993 epoch sec; seqNo:10287; depth 27.80 2021-02-13T00:10:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175033.01 epoch sec; seqNo:10287; 4058553.55 north; 592073.38 east 2021-02-13T00:10:33Z- AHRS_M2 msg: 1613175033.38 epoch sec; seqNo:10203 2021-02-13T00:10:33Z- AHRS_M2 msg: 0.83 phi; 0.36 theta; -0.16 psi 2021-02-13T00:10:33Z- handleDepth()-Depth_Keller msg: 1613175033381 epoch sec; seqNo:10288; depth 27.68 2021-02-13T00:10:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175033.39 epoch sec; seqNo:10288; 4058553.94 north; 592074.59 east 2021-02-13T00:10:33Z- AHRS_M2 msg: 1613175033.78 epoch sec; seqNo:10204 2021-02-13T00:10:33Z- AHRS_M2 msg: 0.83 phi; 0.36 theta; -0.16 psi 2021-02-13T00:10:33Z- handleDepth()-Depth_Keller msg: 1613175033787 epoch sec; seqNo:10289; depth 27.54 2021-02-13T00:10:33Z- handleDvl() - RDI_Pathfinder msg: 1613175033.80 epoch sec; seqNo:5015 2021-02-13T00:10:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.91 , 86.09 , 97.99 , 61.74 2021-02-13T00:10:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.08 , -0.06 2021-02-13T00:10:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175033.81 epoch sec; seqNo:10289; 4058553.94 north; 592074.59 east 2021-02-13T00:10:34Z- AHRS_M2 msg: 1613175034.18 epoch sec; seqNo:10205 2021-02-13T00:10:34Z- AHRS_M2 msg: 0.83 phi; 0.36 theta; -0.16 psi 2021-02-13T00:10:34Z- handleDepth()-Depth_Keller msg: 1613175034188 epoch sec; seqNo:10290; depth 27.40 2021-02-13T00:10:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175034.21 epoch sec; seqNo:10290; 4058554.32 north; 592074.59 east 2021-02-13T00:10:34Z- AHRS_M2 msg: 1613175034.60 epoch sec; seqNo:10206 2021-02-13T00:10:34Z- AHRS_M2 msg: 0.83 phi; 0.36 theta; -0.16 psi 2021-02-13T00:10:34Z- handleDepth()-Depth_Keller msg: 1613175034601 epoch sec; seqNo:10291; depth 27.27 2021-02-13T00:10:34Z- handleDvl() - RDI_Pathfinder msg: 1613175034.62 epoch sec; seqNo:5016 2021-02-13T00:10:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 85.00 , 97.99 , 61.33 2021-02-13T00:10:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.15 , 0.00 2021-02-13T00:10:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175034.63 epoch sec; seqNo:10291; 4058554.70 north; 592074.58 east 2021-02-13T00:10:34Z- AHRS_M2 msg: 1613175034.99 epoch sec; seqNo:10207 2021-02-13T00:10:34Z- AHRS_M2 msg: 0.83 phi; 0.35 theta; -0.15 psi 2021-02-13T00:10:34Z- handleDepth()-Depth_Keller msg: 1613175034996 epoch sec; seqNo:10292; depth 27.14 2021-02-13T00:10:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:35Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:35Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:35Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:35Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:35Z- handleDvl() - RDI_Pathfinder msg: 1613175035.00 epoch sec; seqNo:5017 2021-02-13T00:10:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.15 , 0.00 2021-02-13T00:10:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175035.01 epoch sec; seqNo:10292; 4058554.70 north; 592074.58 east 2021-02-13T00:10:35Z- AHRS_M2 msg: 1613175035.40 epoch sec; seqNo:10208 2021-02-13T00:10:35Z- AHRS_M2 msg: 0.83 phi; 0.35 theta; -0.15 psi 2021-02-13T00:10:35Z- handleDepth()-Depth_Keller msg: 1613175035402 epoch sec; seqNo:10293; depth 27.03 2021-02-13T00:10:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175035.41 epoch sec; seqNo:10293; 4058555.07 north; 592074.58 east 2021-02-13T00:10:35Z- AHRS_M2 msg: 1613175035.81 epoch sec; seqNo:10209 2021-02-13T00:10:35Z- AHRS_M2 msg: 0.83 phi; 0.35 theta; -0.15 psi 2021-02-13T00:10:35Z- handleDepth()-Depth_Keller msg: 1613175035822 epoch sec; seqNo:10294; depth 26.89 2021-02-13T00:10:35Z- handleDvl() - RDI_Pathfinder msg: 1613175035.86 epoch sec; seqNo:5018 2021-02-13T00:10:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.73 , 86.02 , 100.09 , 62.96 2021-02-13T00:10:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.10 , 0.07 2021-02-13T00:10:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175035.88 epoch sec; seqNo:10294; 4058555.47 north; 592075.80 east 2021-02-13T00:10:36Z- AHRS_M2 msg: 1613175036.21 epoch sec; seqNo:10210 2021-02-13T00:10:36Z- AHRS_M2 msg: 0.83 phi; 0.35 theta; -0.14 psi 2021-02-13T00:10:36Z- handleDepth()-Depth_Keller msg: 1613175036221 epoch sec; seqNo:10295; depth 26.76 2021-02-13T00:10:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:36Z- handleDvl() - RDI_Pathfinder msg: 1613175036.22 epoch sec; seqNo:5019 2021-02-13T00:10:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.10 , 0.07 2021-02-13T00:10:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175036.23 epoch sec; seqNo:10295; 4058555.47 north; 592075.80 east 2021-02-13T00:10:36Z- AHRS_M2 msg: 1613175036.61 epoch sec; seqNo:10211 2021-02-13T00:10:36Z- AHRS_M2 msg: 0.84 phi; 0.35 theta; -0.14 psi 2021-02-13T00:10:36Z- handleDepth()-Depth_Keller msg: 1613175036612 epoch sec; seqNo:10296; depth 26.65 2021-02-13T00:10:36Z- handleDvl() - RDI_Pathfinder msg: 1613175036.62 epoch sec; seqNo:5020 2021-02-13T00:10:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 83.92 , 97.99 , 62.96 2021-02-13T00:10:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.09 , 0.09 2021-02-13T00:10:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175036.64 epoch sec; seqNo:10296; 4058555.84 north; 592075.79 east 2021-02-13T00:10:37Z- AHRS_M2 msg: 1613175037.01 epoch sec; seqNo:10212 2021-02-13T00:10:37Z- AHRS_M2 msg: 0.84 phi; 0.35 theta; -0.13 psi 2021-02-13T00:10:37Z- handleDepth()-Depth_Keller msg: 1613175037021 epoch sec; seqNo:10297; depth 26.52 2021-02-13T00:10:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175037.03 epoch sec; seqNo:10297; 4058556.22 north; 592075.79 east 2021-02-13T00:10:37Z- AHRS_M2 msg: 1613175037.42 epoch sec; seqNo:10213 2021-02-13T00:10:37Z- AHRS_M2 msg: 0.84 phi; 0.35 theta; -0.13 psi 2021-02-13T00:10:37Z- handleDepth()-Depth_Keller msg: 1613175037427 epoch sec; seqNo:10298; depth 26.40 2021-02-13T00:10:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:37Z- handleDvl() - RDI_Pathfinder msg: 1613175037.43 epoch sec; seqNo:5021 2021-02-13T00:10:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.09 , 0.09 2021-02-13T00:10:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175037.44 epoch sec; seqNo:10298; 4058556.22 north; 592075.79 east 2021-02-13T00:10:37Z- AHRS_M2 msg: 1613175037.82 epoch sec; seqNo:10214 2021-02-13T00:10:37Z- AHRS_M2 msg: 0.85 phi; 0.35 theta; -0.13 psi 2021-02-13T00:10:37Z- handleDepth()-Depth_Keller msg: 1613175037829 epoch sec; seqNo:10299; depth 26.27 2021-02-13T00:10:37Z- handleDvl() - RDI_Pathfinder msg: 1613175037.85 epoch sec; seqNo:5022 2021-02-13T00:10:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 84.94 , 97.93 , 64.19 2021-02-13T00:10:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , 0.07 2021-02-13T00:10:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175037.86 epoch sec; seqNo:10299; 4058556.62 north; 592077.01 east 2021-02-13T00:10:37Z- LcmTrn::handleMessages() - handling 46 messages took 0.69 ms and 132 calls... 2021-02-13T00:10:37Z- waiting for 1613175035.60, time is 1613175037.86 2021-02-13T00:10:37Z- LcmTrn::updateTrn() equating pose 1613175037.86 and meas times 1613175037.86 2021-02-13T00:10:37Z- LcmTrn::updateTrn() >>>> pose time 1613175037.86, meas time 1613175037.86 <<<< 2021-02-13T00:10:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175037.86 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:10:37Z- TerrainNav::motionUpdate() - currEstimate = (4058557, 592077.01, 26.27) 2021-02-13T00:10:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:10:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:10:37Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175037.86 sec.; No relevant navigation data available 2021-02-13T00:10:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:10:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:10:37Z- variances([0], [2], and psi_berg) = 715.897297 833.007048 0.000000 2021-02-13T00:10:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:10:37Z- LcmTrn::publishEstimates() - MLE : 4058555.12 592028.51 26.27 2021-02-13T00:10:37Z- LcmTrn::publishEstimates() - MMSE : 4058556.62 592077.01 26.27 2021-02-13T00:10:37Z- LcmTrn::publishEstimates() - COVAR: 715.90 833.01 1.11 0.00 2021-02-13T00:10:38Z- AHRS_M2 msg: 1613175038.22 epoch sec; seqNo:10215 2021-02-13T00:10:38Z- AHRS_M2 msg: 0.85 phi; 0.35 theta; -0.13 psi 2021-02-13T00:10:38Z- handleDepth()-Depth_Keller msg: 1613175038229 epoch sec; seqNo:10300; depth 26.15 2021-02-13T00:10:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175038.24 epoch sec; seqNo:10300; 4058556.62 north; 592077.01 east 2021-02-13T00:10:38Z- AHRS_M2 msg: 1613175038.63 epoch sec; seqNo:10216 2021-02-13T00:10:38Z- AHRS_M2 msg: 0.85 phi; 0.35 theta; -0.13 psi 2021-02-13T00:10:38Z- handleDepth()-Depth_Keller msg: 1613175038637 epoch sec; seqNo:10301; depth 26.03 2021-02-13T00:10:38Z- handleDvl() - RDI_Pathfinder msg: 1613175038.65 epoch sec; seqNo:5023 2021-02-13T00:10:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.37 , 84.87 , 97.86 , 63.79 2021-02-13T00:10:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , 0.05 2021-02-13T00:10:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175038.66 epoch sec; seqNo:10301; 4058556.99 north; 592077.00 east 2021-02-13T00:10:39Z- AHRS_M2 msg: 1613175039.03 epoch sec; seqNo:10217 2021-02-13T00:10:39Z- AHRS_M2 msg: 0.86 phi; 0.35 theta; -0.13 psi 2021-02-13T00:10:39Z- handleDepth()-Depth_Keller msg: 1613175039038 epoch sec; seqNo:10302; depth 25.90 2021-02-13T00:10:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175039.05 epoch sec; seqNo:10302; 4058557.37 north; 592077.00 east 2021-02-13T00:10:39Z- AHRS_M2 msg: 1613175039.46 epoch sec; seqNo:10218 2021-02-13T00:10:39Z- AHRS_M2 msg: 0.86 phi; 0.34 theta; -0.13 psi 2021-02-13T00:10:39Z- handleDepth()-Depth_Keller msg: 1613175039460 epoch sec; seqNo:10303; depth 25.78 2021-02-13T00:10:39Z- handleDvl() - RDI_Pathfinder msg: 1613175039.47 epoch sec; seqNo:5024 2021-02-13T00:10:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.96 , 0.00 , 98.00 , 64.19 2021-02-13T00:10:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , 0.04 2021-02-13T00:10:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175039.48 epoch sec; seqNo:10303; 4058557.37 north; 592077.00 east 2021-02-13T00:10:39Z- AHRS_M2 msg: 1613175039.84 epoch sec; seqNo:10219 2021-02-13T00:10:39Z- AHRS_M2 msg: 0.87 phi; 0.34 theta; -0.13 psi 2021-02-13T00:10:39Z- handleDepth()-Depth_Keller msg: 1613175039845 epoch sec; seqNo:10304; depth 25.64 2021-02-13T00:10:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:39Z- handleDvl() - RDI_Pathfinder msg: 1613175039.85 epoch sec; seqNo:5025 2021-02-13T00:10:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , 0.04 2021-02-13T00:10:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175039.86 epoch sec; seqNo:10304; 4058557.77 north; 592078.21 east 2021-02-13T00:10:40Z- AHRS_M2 msg: 1613175040.24 epoch sec; seqNo:10220 2021-02-13T00:10:40Z- AHRS_M2 msg: 0.87 phi; 0.34 theta; -0.13 psi 2021-02-13T00:10:40Z- handleDepth()-Depth_Keller msg: 1613175040248 epoch sec; seqNo:10305; depth 25.54 2021-02-13T00:10:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175040.26 epoch sec; seqNo:10305; 4058557.77 north; 592078.21 east 2021-02-13T00:10:40Z- AHRS_M2 msg: 1613175040.65 epoch sec; seqNo:10221 2021-02-13T00:10:40Z- AHRS_M2 msg: 0.87 phi; 0.34 theta; -0.13 psi 2021-02-13T00:10:40Z- handleDepth()-Depth_Keller msg: 1613175040661 epoch sec; seqNo:10306; depth 25.40 2021-02-13T00:10:40Z- handleDvl() - RDI_Pathfinder msg: 1613175040.67 epoch sec; seqNo:5026 2021-02-13T00:10:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.78 , 83.93 , 96.92 , 63.79 2021-02-13T00:10:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , 0.05 2021-02-13T00:10:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175040.68 epoch sec; seqNo:10306; 4058558.14 north; 592078.21 east 2021-02-13T00:10:41Z- AHRS_M2 msg: 1613175041.05 epoch sec; seqNo:10222 2021-02-13T00:10:41Z- AHRS_M2 msg: 0.87 phi; 0.34 theta; -0.13 psi 2021-02-13T00:10:41Z- handleDepth()-Depth_Keller msg: 1613175041066 epoch sec; seqNo:10307; depth 25.28 2021-02-13T00:10:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:41Z- handleDvl() - RDI_Pathfinder msg: 1613175041.07 epoch sec; seqNo:5027 2021-02-13T00:10:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , 0.05 2021-02-13T00:10:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175041.08 epoch sec; seqNo:10307; 4058558.52 north; 592078.21 east 2021-02-13T00:10:41Z- AHRS_M2 msg: 1613175041.46 epoch sec; seqNo:10223 2021-02-13T00:10:41Z- AHRS_M2 msg: 0.87 phi; 0.34 theta; -0.13 psi 2021-02-13T00:10:41Z- handleDepth()-Depth_Keller msg: 1613175041466 epoch sec; seqNo:10308; depth 25.15 2021-02-13T00:10:41Z- handleDvl() - RDI_Pathfinder msg: 1613175041.48 epoch sec; seqNo:5028 2021-02-13T00:10:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.59 , 0.00 , 99.07 , 65.01 2021-02-13T00:10:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.04 , 0.05 2021-02-13T00:10:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175041.49 epoch sec; seqNo:10308; 4058558.52 north; 592078.21 east 2021-02-13T00:10:41Z- AHRS_M2 msg: 1613175041.86 epoch sec; seqNo:10224 2021-02-13T00:10:41Z- AHRS_M2 msg: 0.88 phi; 0.34 theta; -0.12 psi 2021-02-13T00:10:41Z- handleDepth()-Depth_Keller msg: 1613175041864 epoch sec; seqNo:10309; depth 25.05 2021-02-13T00:10:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175041.88 epoch sec; seqNo:10309; 4058558.92 north; 592079.42 east 2021-02-13T00:10:42Z- AHRS_M2 msg: 1613175042.26 epoch sec; seqNo:10225 2021-02-13T00:10:42Z- AHRS_M2 msg: 0.88 phi; 0.34 theta; -0.12 psi 2021-02-13T00:10:42Z- handleDepth()-Depth_Keller msg: 1613175042268 epoch sec; seqNo:10310; depth 24.92 2021-02-13T00:10:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:42Z- handleDvl() - RDI_Pathfinder msg: 1613175042.27 epoch sec; seqNo:5029 2021-02-13T00:10:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.04 , 0.05 2021-02-13T00:10:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175042.28 epoch sec; seqNo:10310; 4058558.92 north; 592079.42 east 2021-02-13T00:10:42Z- AHRS_M2 msg: 1613175042.67 epoch sec; seqNo:10226 2021-02-13T00:10:42Z- AHRS_M2 msg: 0.89 phi; 0.34 theta; -0.11 psi 2021-02-13T00:10:42Z- handleDepth()-Depth_Keller msg: 1613175042677 epoch sec; seqNo:10311; depth 24.80 2021-02-13T00:10:42Z- handleDvl() - RDI_Pathfinder msg: 1613175042.69 epoch sec; seqNo:5030 2021-02-13T00:10:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 0.00 , 97.99 , 66.23 2021-02-13T00:10:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.07 , 0.03 2021-02-13T00:10:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175042.71 epoch sec; seqNo:10311; 4058559.29 north; 592079.42 east 2021-02-13T00:10:42Z- LcmTrn::handleMessages() - handling 44 messages took 0.59 ms and 133 calls... 2021-02-13T00:10:42Z- waiting for 1613175040.86, time is 1613175042.71 2021-02-13T00:10:42Z- LcmTrn::updateTrn() equating pose 1613175042.71 and meas times 1613175042.71 2021-02-13T00:10:42Z- LcmTrn::updateTrn() >>>> pose time 1613175042.71, meas time 1613175042.71 <<<< 2021-02-13T00:10:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175042.71 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:10:42Z- TerrainNav::motionUpdate() - currEstimate = (4058559, 592079.42, 24.80) 2021-02-13T00:10:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:10:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:10:42Z- TNavBF::Projecting Measurements 2021-02-13T00:10:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:10:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:10:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:10:42Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:10:42Z- TNavFilter::3 good beams. 2021-02-13T00:10:42Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:10:42Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:10:42Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:10:42Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:10:42Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:10:42Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:10:42Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:10:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:10:42Z- TerrainMapOctree: (vehN, vehE ) = (4058559.29, 592079.42). 2021-02-13T00:10:42Z- TerrainMapOctree: Min Distance = 3457.07. 2021-02-13T00:10:42Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:10:42Z- There is no specific good beam that all particles have in common. 2021-02-13T00:10:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:10:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:10:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:10:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:10:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:10:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 7.02 2021-02-13T00:10:42Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:10:42Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:10:42Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:10:42Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:10:42Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:10:42Z- Filter 0 No beams used in subcloud update 2021-02-13T00:10:42Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:10:42Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:10:42Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 715.8 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:10:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175042.71 sec. 2021-02-13T00:10:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:10:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:10:42Z- variances([0], [2], and psi_berg) = 715.836160 833.007010 0.000000 2021-02-13T00:10:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:10:42Z- LcmTrn::publishEstimates() - MLE : 4058557.79 592030.92 24.80 2021-02-13T00:10:42Z- LcmTrn::publishEstimates() - MMSE : 4058559.29 592079.42 24.80 2021-02-13T00:10:42Z- LcmTrn::publishEstimates() - COVAR: 715.84 833.01 0.97 0.00 2021-02-13T00:10:43Z- AHRS_M2 msg: 1613175043.07 epoch sec; seqNo:10227 2021-02-13T00:10:43Z- AHRS_M2 msg: 0.89 phi; 0.34 theta; -0.11 psi 2021-02-13T00:10:43Z- handleDepth()-Depth_Keller msg: 1613175043076 epoch sec; seqNo:10312; depth 24.67 2021-02-13T00:10:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175043.09 epoch sec; seqNo:10312; 4058559.29 north; 592079.42 east 2021-02-13T00:10:43Z- AHRS_M2 msg: 1613175043.47 epoch sec; seqNo:10228 2021-02-13T00:10:43Z- AHRS_M2 msg: 0.89 phi; 0.34 theta; -0.11 psi 2021-02-13T00:10:43Z- handleDepth()-Depth_Keller msg: 1613175043483 epoch sec; seqNo:10313; depth 24.55 2021-02-13T00:10:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:43Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:43Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:43Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:43Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:43Z- handleDvl() - RDI_Pathfinder msg: 1613175043.48 epoch sec; seqNo:5031 2021-02-13T00:10:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.07 , 0.03 2021-02-13T00:10:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175043.49 epoch sec; seqNo:10313; 4058559.69 north; 592080.64 east 2021-02-13T00:10:43Z- AHRS_M2 msg: 1613175043.88 epoch sec; seqNo:10229 2021-02-13T00:10:43Z- AHRS_M2 msg: 0.90 phi; 0.34 theta; -0.11 psi 2021-02-13T00:10:43Z- handleDepth()-Depth_Keller msg: 1613175043889 epoch sec; seqNo:10314; depth 24.43 2021-02-13T00:10:43Z- handleDvl() - RDI_Pathfinder msg: 1613175043.91 epoch sec; seqNo:5032 2021-02-13T00:10:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 84.87 , 97.86 , 65.01 2021-02-13T00:10:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.07 , -0.01 2021-02-13T00:10:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175043.92 epoch sec; seqNo:10314; 4058559.69 north; 592080.64 east 2021-02-13T00:10:44Z- AHRS_M2 msg: 1613175044.29 epoch sec; seqNo:10230 2021-02-13T00:10:44Z- AHRS_M2 msg: 0.90 phi; 0.34 theta; -0.12 psi 2021-02-13T00:10:44Z- handleDepth()-Depth_Keller msg: 1613175044297 epoch sec; seqNo:10315; depth 24.28 2021-02-13T00:10:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175044.31 epoch sec; seqNo:10315; 4058560.06 north; 592080.63 east 2021-02-13T00:10:44Z- AHRS_M2 msg: 1613175044.69 epoch sec; seqNo:10231 2021-02-13T00:10:44Z- AHRS_M2 msg: 0.91 phi; 0.34 theta; -0.12 psi 2021-02-13T00:10:44Z- handleDepth()-Depth_Keller msg: 1613175044701 epoch sec; seqNo:10316; depth 24.18 2021-02-13T00:10:44Z- handleDvl() - RDI_Pathfinder msg: 1613175044.71 epoch sec; seqNo:5033 2021-02-13T00:10:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 85.96 , 98.94 , 65.42 2021-02-13T00:10:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , -0.05 2021-02-13T00:10:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175044.72 epoch sec; seqNo:10316; 4058560.06 north; 592080.63 east 2021-02-13T00:10:45Z- AHRS_M2 msg: 1613175045.09 epoch sec; seqNo:10232 2021-02-13T00:10:45Z- AHRS_M2 msg: 0.91 phi; 0.34 theta; -0.12 psi 2021-02-13T00:10:45Z- handleDepth()-Depth_Keller msg: 1613175045101 epoch sec; seqNo:10317; depth 24.07 2021-02-13T00:10:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175045.11 epoch sec; seqNo:10317; 4058560.44 north; 592080.63 east 2021-02-13T00:10:45Z- AHRS_M2 msg: 1613175045.50 epoch sec; seqNo:10233 2021-02-13T00:10:45Z- AHRS_M2 msg: 0.91 phi; 0.33 theta; -0.13 psi 2021-02-13T00:10:45Z- handleDepth()-Depth_Keller msg: 1613175045502 epoch sec; seqNo:10318; depth 23.93 2021-02-13T00:10:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175045.51 epoch sec; seqNo:10318; 4058560.46 north; 592081.85 east 2021-02-13T00:10:45Z- AHRS_M2 msg: 1613175045.90 epoch sec; seqNo:10234 2021-02-13T00:10:45Z- AHRS_M2 msg: 0.91 phi; 0.33 theta; -0.13 psi 2021-02-13T00:10:45Z- handleDepth()-Depth_Keller msg: 1613175045915 epoch sec; seqNo:10319; depth 23.82 2021-02-13T00:10:45Z- handleDvl() - RDI_Pathfinder msg: 1613175045.93 epoch sec; seqNo:5034 2021-02-13T00:10:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 86.97 , 99.96 , 65.82 2021-02-13T00:10:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , -0.07 2021-02-13T00:10:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175045.94 epoch sec; seqNo:10319; 4058560.84 north; 592081.84 east 2021-02-13T00:10:46Z- AHRS_M2 msg: 1613175046.30 epoch sec; seqNo:10235 2021-02-13T00:10:46Z- AHRS_M2 msg: 0.91 phi; 0.33 theta; -0.14 psi 2021-02-13T00:10:46Z- handleDepth()-Depth_Keller msg: 1613175046308 epoch sec; seqNo:10320; depth 23.66 2021-02-13T00:10:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175046.32 epoch sec; seqNo:10320; 4058560.84 north; 592081.84 east 2021-02-13T00:10:46Z- AHRS_M2 msg: 1613175046.71 epoch sec; seqNo:10236 2021-02-13T00:10:46Z- AHRS_M2 msg: 0.91 phi; 0.33 theta; -0.14 psi 2021-02-13T00:10:46Z- handleDepth()-Depth_Keller msg: 1613175046712 epoch sec; seqNo:10321; depth 23.56 2021-02-13T00:10:46Z- handleDvl() - RDI_Pathfinder msg: 1613175046.72 epoch sec; seqNo:5035 2021-02-13T00:10:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.59 , 0.00 , 98.40 , 67.05 2021-02-13T00:10:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.01 , -0.05 2021-02-13T00:10:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175046.73 epoch sec; seqNo:10321; 4058561.21 north; 592081.84 east 2021-02-13T00:10:47Z- AHRS_M2 msg: 1613175047.12 epoch sec; seqNo:10237 2021-02-13T00:10:47Z- AHRS_M2 msg: 0.91 phi; 0.33 theta; -0.14 psi 2021-02-13T00:10:47Z- handleDepth()-Depth_Keller msg: 1613175047121 epoch sec; seqNo:10322; depth 23.44 2021-02-13T00:10:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:47Z- handleDvl() - RDI_Pathfinder msg: 1613175047.12 epoch sec; seqNo:5036 2021-02-13T00:10:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.01 , -0.05 2021-02-13T00:10:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175047.14 epoch sec; seqNo:10322; 4058561.21 north; 592081.84 east 2021-02-13T00:10:47Z- AHRS_M2 msg: 1613175047.52 epoch sec; seqNo:10238 2021-02-13T00:10:47Z- AHRS_M2 msg: 0.91 phi; 0.33 theta; -0.14 psi 2021-02-13T00:10:47Z- handleDepth()-Depth_Keller msg: 1613175047520 epoch sec; seqNo:10323; depth 23.32 2021-02-13T00:10:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175047.54 epoch sec; seqNo:10323; 4058561.61 north; 592083.06 east 2021-02-13T00:10:47Z- AHRS_M2 msg: 1613175047.92 epoch sec; seqNo:10239 2021-02-13T00:10:47Z- AHRS_M2 msg: 0.91 phi; 0.33 theta; -0.15 psi 2021-02-13T00:10:47Z- handleDepth()-Depth_Keller msg: 1613175047924 epoch sec; seqNo:10324; depth 23.19 2021-02-13T00:10:47Z- handleDvl() - RDI_Pathfinder msg: 1613175047.94 epoch sec; seqNo:5037 2021-02-13T00:10:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 88.25 , 100.16 , 66.23 2021-02-13T00:10:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , 0.00 2021-02-13T00:10:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175047.96 epoch sec; seqNo:10324; 4058561.61 north; 592083.06 east 2021-02-13T00:10:48Z- LcmTrn::handleMessages() - handling 46 messages took 0.72 ms and 132 calls... 2021-02-13T00:10:48Z- waiting for 1613175045.71, time is 1613175047.96 2021-02-13T00:10:48Z- LcmTrn::updateTrn() equating pose 1613175047.96 and meas times 1613175047.96 2021-02-13T00:10:48Z- LcmTrn::updateTrn() >>>> pose time 1613175047.96, meas time 1613175047.96 <<<< 2021-02-13T00:10:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175047.96 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:10:48Z- TerrainNav::motionUpdate() - currEstimate = (4058562, 592083.06, 23.19) 2021-02-13T00:10:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:10:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:10:48Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175047.96 sec.; No relevant navigation data available 2021-02-13T00:10:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:10:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:10:48Z- variances([0], [2], and psi_berg) = 715.769351 833.006968 0.000000 2021-02-13T00:10:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:10:48Z- LcmTrn::publishEstimates() - MLE : 4058560.11 592034.56 23.19 2021-02-13T00:10:48Z- LcmTrn::publishEstimates() - MMSE : 4058561.61 592083.06 23.19 2021-02-13T00:10:48Z- LcmTrn::publishEstimates() - COVAR: 715.77 833.01 1.19 0.00 2021-02-13T00:10:48Z- AHRS_M2 msg: 1613175048.32 epoch sec; seqNo:10240 2021-02-13T00:10:48Z- AHRS_M2 msg: 0.90 phi; 0.32 theta; -0.17 psi 2021-02-13T00:10:48Z- handleDepth()-Depth_Keller msg: 1613175048328 epoch sec; seqNo:10325; depth 23.07 2021-02-13T00:10:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:48Z- handleDvl() - RDI_Pathfinder msg: 1613175048.33 epoch sec; seqNo:5038 2021-02-13T00:10:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , 0.00 2021-02-13T00:10:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175048.34 epoch sec; seqNo:10325; 4058561.99 north; 592083.05 east 2021-02-13T00:10:48Z- AHRS_M2 msg: 1613175048.73 epoch sec; seqNo:10241 2021-02-13T00:10:48Z- AHRS_M2 msg: 0.89 phi; 0.32 theta; -0.18 psi 2021-02-13T00:10:48Z- handleDepth()-Depth_Keller msg: 1613175048734 epoch sec; seqNo:10326; depth 22.97 2021-02-13T00:10:48Z- handleDvl() - RDI_Pathfinder msg: 1613175048.74 epoch sec; seqNo:5039 2021-02-13T00:10:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 89.81 , 100.77 , 66.64 2021-02-13T00:10:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.04 , 0.04 2021-02-13T00:10:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175048.76 epoch sec; seqNo:10326; 4058561.99 north; 592083.05 east 2021-02-13T00:10:49Z- AHRS_M2 msg: 1613175049.13 epoch sec; seqNo:10242 2021-02-13T00:10:49Z- AHRS_M2 msg: 0.89 phi; 0.32 theta; -0.20 psi 2021-02-13T00:10:49Z- handleDepth()-Depth_Keller msg: 1613175049147 epoch sec; seqNo:10327; depth 22.82 2021-02-13T00:10:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175049.16 epoch sec; seqNo:10327; 4058562.36 north; 592083.05 east 2021-02-13T00:10:49Z- AHRS_M2 msg: 1613175049.56 epoch sec; seqNo:10243 2021-02-13T00:10:49Z- AHRS_M2 msg: 0.88 phi; 0.32 theta; -0.21 psi 2021-02-13T00:10:49Z- handleDepth()-Depth_Keller msg: 1613175049572 epoch sec; seqNo:10328; depth 22.69 2021-02-13T00:10:49Z- handleDvl() - RDI_Pathfinder msg: 1613175049.59 epoch sec; seqNo:5040 2021-02-13T00:10:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.78 , 92.18 , 100.70 , 162.81 2021-02-13T00:10:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.58 , 0.00 , 0.01 2021-02-13T00:10:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175049.60 epoch sec; seqNo:10328; 4058562.38 north; 592084.27 east 2021-02-13T00:10:49Z- AHRS_M2 msg: 1613175049.94 epoch sec; seqNo:10244 2021-02-13T00:10:49Z- AHRS_M2 msg: 0.88 phi; 0.32 theta; -0.22 psi 2021-02-13T00:10:49Z- handleDepth()-Depth_Keller msg: 1613175049943 epoch sec; seqNo:10329; depth 22.57 2021-02-13T00:10:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175049.96 epoch sec; seqNo:10329; 4058562.76 north; 592084.27 east 2021-02-13T00:10:50Z- AHRS_M2 msg: 1613175050.34 epoch sec; seqNo:10245 2021-02-13T00:10:50Z- AHRS_M2 msg: 0.87 phi; 0.32 theta; -0.22 psi 2021-02-13T00:10:50Z- handleDepth()-Depth_Keller msg: 1613175050350 epoch sec; seqNo:10330; depth 22.48 2021-02-13T00:10:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175050.36 epoch sec; seqNo:10330; 4058562.76 north; 592084.27 east 2021-02-13T00:10:50Z- AHRS_M2 msg: 1613175050.75 epoch sec; seqNo:10246 2021-02-13T00:10:50Z- AHRS_M2 msg: 0.87 phi; 0.31 theta; -0.22 psi 2021-02-13T00:10:50Z- handleDepth()-Depth_Keller msg: 1613175050753 epoch sec; seqNo:10331; depth 22.37 2021-02-13T00:10:50Z- handleDvl() - RDI_Pathfinder msg: 1613175050.77 epoch sec; seqNo:5041 2021-02-13T00:10:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.56 , 97.53 , 101.99 , 78.18 2021-02-13T00:10:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.02 , 0.07 2021-02-13T00:10:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175050.78 epoch sec; seqNo:10331; 4058562.76 north; 592084.27 east 2021-02-13T00:10:51Z- AHRS_M2 msg: 1613175051.15 epoch sec; seqNo:10247 2021-02-13T00:10:51Z- AHRS_M2 msg: 0.86 phi; 0.31 theta; -0.22 psi 2021-02-13T00:10:51Z- handleDepth()-Depth_Keller msg: 1613175051158 epoch sec; seqNo:10332; depth 22.24 2021-02-13T00:10:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175051.17 epoch sec; seqNo:10332; 4058563.13 north; 592084.26 east 2021-02-13T00:10:51Z- AHRS_M2 msg: 1613175051.56 epoch sec; seqNo:10248 2021-02-13T00:10:51Z- AHRS_M2 msg: 0.86 phi; 0.31 theta; -0.22 psi 2021-02-13T00:10:51Z- handleDepth()-Depth_Keller msg: 1613175051570 epoch sec; seqNo:10333; depth 22.13 2021-02-13T00:10:51Z- handleDvl() - RDI_Pathfinder msg: 1613175051.58 epoch sec; seqNo:5042 2021-02-13T00:10:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.56 , 98.00 , 100.43 , 68.28 2021-02-13T00:10:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.02 , 0.08 2021-02-13T00:10:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175051.62 epoch sec; seqNo:10333; 4058563.15 north; 592085.48 east 2021-02-13T00:10:51Z- AHRS_M2 msg: 1613175051.96 epoch sec; seqNo:10249 2021-02-13T00:10:51Z- AHRS_M2 msg: 0.85 phi; 0.31 theta; -0.22 psi 2021-02-13T00:10:51Z- handleDepth()-Depth_Keller msg: 1613175051965 epoch sec; seqNo:10334; depth 22.02 2021-02-13T00:10:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:51Z- handleDvl() - RDI_Pathfinder msg: 1613175051.97 epoch sec; seqNo:5043 2021-02-13T00:10:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.02 , 0.08 2021-02-13T00:10:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175051.98 epoch sec; seqNo:10334; 4058563.53 north; 592085.48 east 2021-02-13T00:10:52Z- AHRS_M2 msg: 1613175052.36 epoch sec; seqNo:10250 2021-02-13T00:10:52Z- AHRS_M2 msg: 0.85 phi; 0.31 theta; -0.22 psi 2021-02-13T00:10:52Z- handleDepth()-Depth_Keller msg: 1613175052368 epoch sec; seqNo:10335; depth 21.90 2021-02-13T00:10:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175052.39 epoch sec; seqNo:10335; 4058563.53 north; 592085.48 east 2021-02-13T00:10:52Z- AHRS_M2 msg: 1613175052.77 epoch sec; seqNo:10251 2021-02-13T00:10:52Z- AHRS_M2 msg: 0.84 phi; 0.32 theta; -0.22 psi 2021-02-13T00:10:52Z- handleDepth()-Depth_Keller msg: 1613175052772 epoch sec; seqNo:10336; depth 21.79 2021-02-13T00:10:52Z- handleDvl() - RDI_Pathfinder msg: 1613175052.79 epoch sec; seqNo:5044 2021-02-13T00:10:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.78 , 98.27 , 100.70 , 69.10 2021-02-13T00:10:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , 0.09 2021-02-13T00:10:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175052.81 epoch sec; seqNo:10336; 4058563.91 north; 592085.47 east 2021-02-13T00:10:53Z- LcmTrn::handleMessages() - handling 43 messages took 0.68 ms and 130 calls... 2021-02-13T00:10:53Z- waiting for 1613175050.96, time is 1613175052.81 2021-02-13T00:10:53Z- LcmTrn::updateTrn() equating pose 1613175052.81 and meas times 1613175052.81 2021-02-13T00:10:53Z- LcmTrn::updateTrn() >>>> pose time 1613175052.81, meas time 1613175052.81 <<<< 2021-02-13T00:10:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175052.81 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:10:53Z- TerrainNav::motionUpdate() - currEstimate = (4058564, 592085.47, 21.79) 2021-02-13T00:10:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:10:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:10:53Z- TNavBF::Projecting Measurements 2021-02-13T00:10:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:10:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:10:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:10:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:10:53Z- TNavFilter::4 good beams. 2021-02-13T00:10:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:10:53Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:10:53Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:10:53Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:10:53Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:10:53Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:10:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:10:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:10:53Z- TerrainMapOctree: (vehN, vehE ) = (4058563.91, 592085.47). 2021-02-13T00:10:53Z- TerrainMapOctree: Min Distance = 3449.68. 2021-02-13T00:10:53Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:10:53Z- There is no specific good beam that all particles have in common. 2021-02-13T00:10:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:10:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.36 K: 0.118 ExpectedMeasDiff: nan currMeas.covariance: 8.48 2021-02-13T00:10:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:10:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 15.45 2021-02-13T00:10:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:10:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 16.23 2021-02-13T00:10:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:10:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 7.64 2021-02-13T00:10:53Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:10:53Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:10:53Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:10:53Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:10:53Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:10:53Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:10:53Z- Filter 0 No beams used in subcloud update 2021-02-13T00:10:53Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:10:53Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:10:53Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 715.7 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:10:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175052.81 sec. 2021-02-13T00:10:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:10:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:10:53Z- variances([0], [2], and psi_berg) = 715.710622 833.006932 0.000000 2021-02-13T00:10:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:10:53Z- LcmTrn::publishEstimates() - MLE : 4058562.41 592036.97 21.79 2021-02-13T00:10:53Z- LcmTrn::publishEstimates() - MMSE : 4058563.91 592085.47 21.79 2021-02-13T00:10:53Z- LcmTrn::publishEstimates() - COVAR: 715.71 833.01 0.95 0.00 2021-02-13T00:10:53Z- AHRS_M2 msg: 1613175053.18 epoch sec; seqNo:10252 2021-02-13T00:10:53Z- AHRS_M2 msg: 0.83 phi; 0.32 theta; -0.22 psi 2021-02-13T00:10:53Z- handleDepth()-Depth_Keller msg: 1613175053184 epoch sec; seqNo:10337; depth 21.66 2021-02-13T00:10:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:53Z- handleDvl() - RDI_Pathfinder msg: 1613175053.19 epoch sec; seqNo:5045 2021-02-13T00:10:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , 0.09 2021-02-13T00:10:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175053.20 epoch sec; seqNo:10337; 4058564.28 north; 592085.47 east 2021-02-13T00:10:53Z- AHRS_M2 msg: 1613175053.58 epoch sec; seqNo:10253 2021-02-13T00:10:53Z- AHRS_M2 msg: 0.82 phi; 0.32 theta; -0.22 psi 2021-02-13T00:10:53Z- handleDepth()-Depth_Keller msg: 1613175053588 epoch sec; seqNo:10338; depth 21.56 2021-02-13T00:10:53Z- handleDvl() - RDI_Pathfinder msg: 1613175053.60 epoch sec; seqNo:5046 2021-02-13T00:10:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.60 , 98.20 , 101.85 , 68.69 2021-02-13T00:10:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , 0.10 2021-02-13T00:10:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175053.61 epoch sec; seqNo:10338; 4058564.30 north; 592086.69 east 2021-02-13T00:10:53Z- AHRS_M2 msg: 1613175053.98 epoch sec; seqNo:10254 2021-02-13T00:10:53Z- AHRS_M2 msg: 0.81 phi; 0.32 theta; -0.22 psi 2021-02-13T00:10:53Z- handleDepth()-Depth_Keller msg: 1613175053984 epoch sec; seqNo:10339; depth 21.44 2021-02-13T00:10:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175054.00 epoch sec; seqNo:10339; 4058564.68 north; 592086.69 east 2021-02-13T00:10:54Z- AHRS_M2 msg: 1613175054.38 epoch sec; seqNo:10255 2021-02-13T00:10:54Z- AHRS_M2 msg: 0.81 phi; 0.32 theta; -0.22 psi 2021-02-13T00:10:54Z- handleDepth()-Depth_Keller msg: 1613175054388 epoch sec; seqNo:10340; depth 21.33 2021-02-13T00:10:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:10:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:10:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:10:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:10:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:10:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:10:54Z- handleDvl() - RDI_Pathfinder msg: 1613175054.39 epoch sec; seqNo:5047 2021-02-13T00:10:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:10:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , 0.10 2021-02-13T00:10:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175054.40 epoch sec; seqNo:10340; 4058564.68 north; 592086.69 east 2021-02-13T00:10:54Z- AHRS_M2 msg: 1613175054.79 epoch sec; seqNo:10256 2021-02-13T00:10:54Z- AHRS_M2 msg: 0.80 phi; 0.32 theta; -0.22 psi 2021-02-13T00:10:54Z- handleDepth()-Depth_Keller msg: 1613175054797 epoch sec; seqNo:10341; depth 21.22 2021-02-13T00:10:54Z- handleDvl() - RDI_Pathfinder msg: 1613175054.81 epoch sec; seqNo:5048 2021-02-13T00:10:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.60 , 98.13 , 103.00 , 69.51 2021-02-13T00:10:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.05 , 0.08 2021-02-13T00:10:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175054.82 epoch sec; seqNo:10341; 4058565.06 north; 592086.68 east 2021-02-13T00:10:55Z- AHRS_M2 msg: 1613175055.19 epoch sec; seqNo:10257 2021-02-13T00:10:55Z- AHRS_M2 msg: 0.80 phi; 0.32 theta; -0.21 psi 2021-02-13T00:10:55Z- handleDepth()-Depth_Keller msg: 1613175055198 epoch sec; seqNo:10342; depth 21.09 2021-02-13T00:10:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175055.21 epoch sec; seqNo:10342; 4058565.43 north; 592086.68 east 2021-02-13T00:10:55Z- AHRS_M2 msg: 1613175055.60 epoch sec; seqNo:10258 2021-02-13T00:10:55Z- AHRS_M2 msg: 0.80 phi; 0.32 theta; -0.21 psi 2021-02-13T00:10:55Z- handleDepth()-Depth_Keller msg: 1613175055615 epoch sec; seqNo:10343; depth 20.99 2021-02-13T00:10:55Z- handleDvl() - RDI_Pathfinder msg: 1613175055.63 epoch sec; seqNo:5049 2021-02-13T00:10:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.82 , 98.13 , 103.00 , 69.50 2021-02-13T00:10:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.07 , 0.04 2021-02-13T00:10:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175055.64 epoch sec; seqNo:10343; 4058565.45 north; 592087.90 east 2021-02-13T00:10:56Z- AHRS_M2 msg: 1613175056.00 epoch sec; seqNo:10259 2021-02-13T00:10:56Z- AHRS_M2 msg: 0.80 phi; 0.32 theta; -0.21 psi 2021-02-13T00:10:56Z- handleDepth()-Depth_Keller msg: 1613175056009 epoch sec; seqNo:10344; depth 20.86 2021-02-13T00:10:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175056.03 epoch sec; seqNo:10344; 4058565.83 north; 592087.89 east 2021-02-13T00:10:56Z- AHRS_M2 msg: 1613175056.40 epoch sec; seqNo:10260 2021-02-13T00:10:56Z- AHRS_M2 msg: 0.80 phi; 0.32 theta; -0.21 psi 2021-02-13T00:10:56Z- handleDepth()-Depth_Keller msg: 1613175056413 epoch sec; seqNo:10345; depth 20.77 2021-02-13T00:10:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175056.43 epoch sec; seqNo:10345; 4058566.20 north; 592087.89 east 2021-02-13T00:10:56Z- AHRS_M2 msg: 1613175056.81 epoch sec; seqNo:10261 2021-02-13T00:10:56Z- AHRS_M2 msg: 0.80 phi; 0.32 theta; -0.20 psi 2021-02-13T00:10:56Z- handleDepth()-Depth_Keller msg: 1613175056821 epoch sec; seqNo:10346; depth 20.64 2021-02-13T00:10:56Z- handleDvl() - RDI_Pathfinder msg: 1613175056.83 epoch sec; seqNo:5050 2021-02-13T00:10:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.23 , 98.06 , 102.93 , 68.28 2021-02-13T00:10:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.10 , -0.04 2021-02-13T00:10:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175056.84 epoch sec; seqNo:10346; 4058566.20 north; 592087.89 east 2021-02-13T00:10:57Z- AHRS_M2 msg: 1613175057.21 epoch sec; seqNo:10262 2021-02-13T00:10:57Z- AHRS_M2 msg: 0.80 phi; 0.32 theta; -0.20 psi 2021-02-13T00:10:57Z- handleDepth()-Depth_Keller msg: 1613175057216 epoch sec; seqNo:10347; depth 20.54 2021-02-13T00:10:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175057.24 epoch sec; seqNo:10347; 4058566.60 north; 592089.11 east 2021-02-13T00:10:57Z- AHRS_M2 msg: 1613175057.62 epoch sec; seqNo:10263 2021-02-13T00:10:57Z- AHRS_M2 msg: 0.81 phi; 0.31 theta; -0.19 psi 2021-02-13T00:10:57Z- handleDepth()-Depth_Keller msg: 1613175057625 epoch sec; seqNo:10348; depth 20.43 2021-02-13T00:10:57Z- handleDvl() - RDI_Pathfinder msg: 1613175057.64 epoch sec; seqNo:5051 2021-02-13T00:10:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.82 , 98.13 , 96.91 , 69.50 2021-02-13T00:10:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.10 , -0.10 2021-02-13T00:10:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175057.65 epoch sec; seqNo:10348; 4058566.60 north; 592089.11 east 2021-02-13T00:10:58Z- AHRS_M2 msg: 1613175058.02 epoch sec; seqNo:10264 2021-02-13T00:10:58Z- AHRS_M2 msg: 0.81 phi; 0.31 theta; -0.19 psi 2021-02-13T00:10:58Z- handleDepth()-Depth_Keller msg: 1613175058024 epoch sec; seqNo:10349; depth 20.32 2021-02-13T00:10:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175058.04 epoch sec; seqNo:10349; 4058566.98 north; 592089.10 east 2021-02-13T00:10:58Z- LcmTrn::handleMessages() - handling 46 messages took 0.69 ms and 132 calls... 2021-02-13T00:10:58Z- waiting for 1613175055.81, time is 1613175058.04 2021-02-13T00:10:58Z- LcmTrn::updateTrn() >>>> pose time 1613175058.04, meas time 1613175057.64 <<<< 2021-02-13T00:10:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175057.64 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:10:58Z- TerrainNav::motionUpdate() - currEstimate = (4058567, 592089.10, 20.32) 2021-02-13T00:10:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:10:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:10:58Z- TNavBF::Projecting Measurements 2021-02-13T00:10:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:10:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:10:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:10:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:10:58Z- TNavFilter::4 good beams. 2021-02-13T00:10:58Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:10:58Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:10:58Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:10:58Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:10:58Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:10:58Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:10:58Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:10:58Z- TNavBF::mmse selected from filter -1 2021-02-13T00:10:58Z- TerrainMapOctree: (vehN, vehE ) = (4058566.75, 592088.83). 2021-02-13T00:10:58Z- TerrainMapOctree: Min Distance = 3445.36. 2021-02-13T00:10:58Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:10:58Z- There is no specific good beam that all particles have in common. 2021-02-13T00:10:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:10:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 8.96 2021-02-13T00:10:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:10:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 15.41 2021-02-13T00:10:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:10:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 15.03 2021-02-13T00:10:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:10:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 7.73 2021-02-13T00:10:58Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:10:58Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:10:58Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:10:58Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:10:58Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:10:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:10:58Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:10:58Z- Filter 0 No beams used in subcloud update 2021-02-13T00:10:58Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:10:58Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:10:58Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 715.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:10:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175057.64 sec. 2021-02-13T00:10:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:10:58Z- TNavBF::mmse selected from filter -1 2021-02-13T00:10:58Z- variances([0], [2], and psi_berg) = 715.624071 833.006878 0.000000 2021-02-13T00:10:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:10:58Z- LcmTrn::publishEstimates() - MLE : 4058565.48 592040.60 20.32 2021-02-13T00:10:58Z- LcmTrn::publishEstimates() - MMSE : 4058566.98 592089.10 20.32 2021-02-13T00:10:58Z- LcmTrn::publishEstimates() - COVAR: 715.62 833.01 0.87 0.00 2021-02-13T00:10:58Z- AHRS_M2 msg: 1613175058.42 epoch sec; seqNo:10265 2021-02-13T00:10:58Z- AHRS_M2 msg: 0.82 phi; 0.31 theta; -0.18 psi 2021-02-13T00:10:58Z- handleDepth()-Depth_Keller msg: 1613175058430 epoch sec; seqNo:10350; depth 20.21 2021-02-13T00:10:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175058.44 epoch sec; seqNo:10350; 4058566.98 north; 592089.10 east 2021-02-13T00:10:58Z- AHRS_M2 msg: 1613175058.83 epoch sec; seqNo:10266 2021-02-13T00:10:58Z- AHRS_M2 msg: 0.82 phi; 0.31 theta; -0.18 psi 2021-02-13T00:10:58Z- handleDepth()-Depth_Keller msg: 1613175058832 epoch sec; seqNo:10351; depth 20.10 2021-02-13T00:10:58Z- handleDvl() - RDI_Pathfinder msg: 1613175058.84 epoch sec; seqNo:5052 2021-02-13T00:10:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.23 , 98.20 , 103.07 , 78.71 2021-02-13T00:10:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.00 , -0.14 2021-02-13T00:10:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175058.88 epoch sec; seqNo:10351; 4058567.35 north; 592089.10 east 2021-02-13T00:10:59Z- AHRS_M2 msg: 1613175059.23 epoch sec; seqNo:10267 2021-02-13T00:10:59Z- AHRS_M2 msg: 0.82 phi; 0.30 theta; -0.17 psi 2021-02-13T00:10:59Z- handleDepth()-Depth_Keller msg: 1613175059241 epoch sec; seqNo:10352; depth 19.99 2021-02-13T00:10:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175059.25 epoch sec; seqNo:10352; 4058567.35 north; 592089.10 east 2021-02-13T00:10:59Z- AHRS_M2 msg: 1613175059.65 epoch sec; seqNo:10268 2021-02-13T00:10:59Z- AHRS_M2 msg: 0.82 phi; 0.30 theta; -0.17 psi 2021-02-13T00:10:59Z- handleDepth()-Depth_Keller msg: 1613175059659 epoch sec; seqNo:10353; depth 19.87 2021-02-13T00:10:59Z- handleDvl() - RDI_Pathfinder msg: 1613175059.67 epoch sec; seqNo:5053 2021-02-13T00:10:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 97.86 , 100.29 , 70.73 2021-02-13T00:10:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.09 , -0.10 2021-02-13T00:10:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175059.68 epoch sec; seqNo:10353; 4058567.75 north; 592090.32 east 2021-02-13T00:11:00Z- AHRS_M2 msg: 1613175060.04 epoch sec; seqNo:10269 2021-02-13T00:11:00Z- AHRS_M2 msg: 0.82 phi; 0.30 theta; -0.17 psi 2021-02-13T00:11:00Z- handleDepth()-Depth_Keller msg: 1613175060045 epoch sec; seqNo:10354; depth 19.75 2021-02-13T00:11:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175060.06 epoch sec; seqNo:10354; 4058567.75 north; 592090.32 east 2021-02-13T00:11:00Z- AHRS_M2 msg: 1613175060.44 epoch sec; seqNo:10270 2021-02-13T00:11:00Z- AHRS_M2 msg: 0.82 phi; 0.30 theta; -0.17 psi 2021-02-13T00:11:00Z- handleDepth()-Depth_Keller msg: 1613175060448 epoch sec; seqNo:10355; depth 19.63 2021-02-13T00:11:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:00Z- handleDvl() - RDI_Pathfinder msg: 1613175060.45 epoch sec; seqNo:5054 2021-02-13T00:11:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.09 , -0.10 2021-02-13T00:11:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175060.46 epoch sec; seqNo:10355; 4058568.13 north; 592090.31 east 2021-02-13T00:11:00Z- AHRS_M2 msg: 1613175060.85 epoch sec; seqNo:10271 2021-02-13T00:11:00Z- AHRS_M2 msg: 0.82 phi; 0.30 theta; -0.16 psi 2021-02-13T00:11:00Z- handleDepth()-Depth_Keller msg: 1613175060874 epoch sec; seqNo:10356; depth 19.52 2021-02-13T00:11:00Z- handleDvl() - RDI_Pathfinder msg: 1613175060.90 epoch sec; seqNo:5055 2021-02-13T00:11:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.64 , 93.19 , 100.50 , 78.58 2021-02-13T00:11:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.15 , 0.01 2021-02-13T00:11:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175060.92 epoch sec; seqNo:10356; 4058568.13 north; 592090.31 east 2021-02-13T00:11:01Z- AHRS_M2 msg: 1613175061.25 epoch sec; seqNo:10272 2021-02-13T00:11:01Z- AHRS_M2 msg: 0.81 phi; 0.30 theta; -0.16 psi 2021-02-13T00:11:01Z- handleDepth()-Depth_Keller msg: 1613175061258 epoch sec; seqNo:10357; depth 19.39 2021-02-13T00:11:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175061.27 epoch sec; seqNo:10357; 4058568.50 north; 592090.31 east 2021-02-13T00:11:01Z- AHRS_M2 msg: 1613175061.66 epoch sec; seqNo:10273 2021-02-13T00:11:01Z- AHRS_M2 msg: 0.81 phi; 0.30 theta; -0.16 psi 2021-02-13T00:11:01Z- handleDepth()-Depth_Keller msg: 1613175061665 epoch sec; seqNo:10358; depth 19.30 2021-02-13T00:11:01Z- handleDvl() - RDI_Pathfinder msg: 1613175061.68 epoch sec; seqNo:5056 2021-02-13T00:11:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.64 , 93.19 , 101.71 , 78.58 2021-02-13T00:11:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.14 , 0.09 2021-02-13T00:11:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175061.69 epoch sec; seqNo:10358; 4058568.88 north; 592090.30 east 2021-02-13T00:11:02Z- AHRS_M2 msg: 1613175062.06 epoch sec; seqNo:10274 2021-02-13T00:11:02Z- AHRS_M2 msg: 0.81 phi; 0.30 theta; -0.16 psi 2021-02-13T00:11:02Z- handleDepth()-Depth_Keller msg: 1613175062069 epoch sec; seqNo:10359; depth 19.18 2021-02-13T00:11:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175062.08 epoch sec; seqNo:10359; 4058568.88 north; 592090.30 east 2021-02-13T00:11:02Z- AHRS_M2 msg: 1613175062.46 epoch sec; seqNo:10275 2021-02-13T00:11:02Z- AHRS_M2 msg: 0.81 phi; 0.30 theta; -0.16 psi 2021-02-13T00:11:02Z- handleDepth()-Depth_Keller msg: 1613175062468 epoch sec; seqNo:10360; depth 19.08 2021-02-13T00:11:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175062.48 epoch sec; seqNo:10360; 4058569.27 north; 592091.52 east 2021-02-13T00:11:02Z- AHRS_M2 msg: 1613175062.87 epoch sec; seqNo:10276 2021-02-13T00:11:02Z- AHRS_M2 msg: 0.81 phi; 0.31 theta; -0.15 psi 2021-02-13T00:11:02Z- handleDepth()-Depth_Keller msg: 1613175062875 epoch sec; seqNo:10361; depth 18.98 2021-02-13T00:11:02Z- handleDvl() - RDI_Pathfinder msg: 1613175062.90 epoch sec; seqNo:5057 2021-02-13T00:11:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.86 , 91.90 , 101.65 , 70.32 2021-02-13T00:11:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.11 , 0.14 2021-02-13T00:11:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175062.91 epoch sec; seqNo:10361; 4058569.65 north; 592091.52 east 2021-02-13T00:11:03Z- LcmTrn::handleMessages() - handling 42 messages took 0.69 ms and 128 calls... 2021-02-13T00:11:03Z- waiting for 1613175061.04, time is 1613175062.91 2021-02-13T00:11:03Z- LcmTrn::updateTrn() equating pose 1613175062.91 and meas times 1613175062.91 2021-02-13T00:11:03Z- LcmTrn::updateTrn() >>>> pose time 1613175062.91, meas time 1613175062.91 <<<< 2021-02-13T00:11:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175062.91 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:11:03Z- TerrainNav::motionUpdate() - currEstimate = (4058570, 592091.52, 18.98) 2021-02-13T00:11:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:11:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:11:03Z- TNavBF::Projecting Measurements 2021-02-13T00:11:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:11:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:11:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:11:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:11:03Z- TNavFilter::4 good beams. 2021-02-13T00:11:03Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:11:03Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:11:03Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:11:03Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:11:03Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:11:03Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:11:03Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:11:03Z- TNavBF::mmse selected from filter -1 2021-02-13T00:11:03Z- TerrainMapOctree: (vehN, vehE ) = (4058569.65, 592091.52). 2021-02-13T00:11:03Z- TerrainMapOctree: Min Distance = 3441.41. 2021-02-13T00:11:03Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:11:03Z- There is no specific good beam that all particles have in common. 2021-02-13T00:11:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:11:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 9.45 2021-02-13T00:11:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:11:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 13.51 2021-02-13T00:11:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:11:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 16.53 2021-02-13T00:11:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:11:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 7.91 2021-02-13T00:11:03Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:11:03Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:11:03Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:11:03Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:11:03Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:11:03Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:11:03Z- Filter 0 No beams used in subcloud update 2021-02-13T00:11:03Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:11:03Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:11:03Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 715.6 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:11:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175062.91 sec. 2021-02-13T00:11:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:11:03Z- TNavBF::mmse selected from filter -1 2021-02-13T00:11:03Z- variances([0], [2], and psi_berg) = 715.563120 833.006840 0.000000 2021-02-13T00:11:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:11:03Z- LcmTrn::publishEstimates() - MLE : 4058568.15 592043.02 18.98 2021-02-13T00:11:03Z- LcmTrn::publishEstimates() - MMSE : 4058569.65 592091.52 18.98 2021-02-13T00:11:03Z- LcmTrn::publishEstimates() - COVAR: 715.56 833.01 0.79 0.00 2021-02-13T00:11:03Z- AHRS_M2 msg: 1613175063.27 epoch sec; seqNo:10277 2021-02-13T00:11:03Z- AHRS_M2 msg: 0.81 phi; 0.31 theta; -0.15 psi 2021-02-13T00:11:03Z- handleDepth()-Depth_Keller msg: 1613175063276 epoch sec; seqNo:10362; depth 18.86 2021-02-13T00:11:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175063.29 epoch sec; seqNo:10362; 4058569.65 north; 592091.52 east 2021-02-13T00:11:03Z- AHRS_M2 msg: 1613175063.67 epoch sec; seqNo:10278 2021-02-13T00:11:03Z- AHRS_M2 msg: 0.81 phi; 0.31 theta; -0.15 psi 2021-02-13T00:11:03Z- handleDepth()-Depth_Keller msg: 1613175063684 epoch sec; seqNo:10363; depth 18.77 2021-02-13T00:11:03Z- handleDvl() - RDI_Pathfinder msg: 1613175063.69 epoch sec; seqNo:5058 2021-02-13T00:11:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.20 , 103.07 , 78.71 2021-02-13T00:11:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , 0.12 2021-02-13T00:11:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175063.70 epoch sec; seqNo:10363; 4058570.03 north; 592091.51 east 2021-02-13T00:11:04Z- AHRS_M2 msg: 1613175064.08 epoch sec; seqNo:10279 2021-02-13T00:11:04Z- AHRS_M2 msg: 0.82 phi; 0.31 theta; -0.14 psi 2021-02-13T00:11:04Z- handleDepth()-Depth_Keller msg: 1613175064093 epoch sec; seqNo:10364; depth 18.64 2021-02-13T00:11:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:04Z- handleDvl() - RDI_Pathfinder msg: 1613175064.10 epoch sec; seqNo:5059 2021-02-13T00:11:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , 0.12 2021-02-13T00:11:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175064.11 epoch sec; seqNo:10364; 4058570.41 north; 592091.51 east 2021-02-13T00:11:04Z- AHRS_M2 msg: 1613175064.49 epoch sec; seqNo:10280 2021-02-13T00:11:04Z- AHRS_M2 msg: 0.82 phi; 0.31 theta; -0.14 psi 2021-02-13T00:11:04Z- handleDepth()-Depth_Keller msg: 1613175064518 epoch sec; seqNo:10365; depth 18.54 2021-02-13T00:11:04Z- handleDvl() - RDI_Pathfinder msg: 1613175064.53 epoch sec; seqNo:5060 2021-02-13T00:11:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.50 , 97.65 , 103.07 , 71.55 2021-02-13T00:11:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , 0.08 2021-02-13T00:11:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175064.55 epoch sec; seqNo:10365; 4058570.80 north; 592092.72 east 2021-02-13T00:11:04Z- AHRS_M2 msg: 1613175064.89 epoch sec; seqNo:10281 2021-02-13T00:11:04Z- AHRS_M2 msg: 0.83 phi; 0.31 theta; -0.14 psi 2021-02-13T00:11:04Z- handleDepth()-Depth_Keller msg: 1613175064897 epoch sec; seqNo:10366; depth 18.42 2021-02-13T00:11:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175064.91 epoch sec; seqNo:10366; 4058570.80 north; 592092.72 east 2021-02-13T00:11:05Z- AHRS_M2 msg: 1613175065.29 epoch sec; seqNo:10282 2021-02-13T00:11:05Z- AHRS_M2 msg: 0.83 phi; 0.31 theta; -0.13 psi 2021-02-13T00:11:05Z- handleDepth()-Depth_Keller msg: 1613175065296 epoch sec; seqNo:10367; depth 18.33 2021-02-13T00:11:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:05Z- handleDvl() - RDI_Pathfinder msg: 1613175065.30 epoch sec; seqNo:5061 2021-02-13T00:11:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , 0.08 2021-02-13T00:11:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175065.31 epoch sec; seqNo:10367; 4058571.18 north; 592092.72 east 2021-02-13T00:11:05Z- AHRS_M2 msg: 1613175065.70 epoch sec; seqNo:10283 2021-02-13T00:11:05Z- AHRS_M2 msg: 0.84 phi; 0.31 theta; -0.13 psi 2021-02-13T00:11:05Z- handleDepth()-Depth_Keller msg: 1613175065709 epoch sec; seqNo:10368; depth 18.23 2021-02-13T00:11:05Z- handleDvl() - RDI_Pathfinder msg: 1613175065.72 epoch sec; seqNo:5062 2021-02-13T00:11:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.73 , 91.90 , 104.08 , 78.51 2021-02-13T00:11:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , 0.05 2021-02-13T00:11:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175065.73 epoch sec; seqNo:10368; 4058571.18 north; 592092.72 east 2021-02-13T00:11:06Z- AHRS_M2 msg: 1613175066.10 epoch sec; seqNo:10284 2021-02-13T00:11:06Z- AHRS_M2 msg: 0.84 phi; 0.31 theta; -0.13 psi 2021-02-13T00:11:06Z- handleDepth()-Depth_Keller msg: 1613175066109 epoch sec; seqNo:10369; depth 18.12 2021-02-13T00:11:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175066.14 epoch sec; seqNo:10369; 4058571.56 north; 592092.72 east 2021-02-13T00:11:06Z- AHRS_M2 msg: 1613175066.50 epoch sec; seqNo:10285 2021-02-13T00:11:06Z- AHRS_M2 msg: 0.84 phi; 0.31 theta; -0.13 psi 2021-02-13T00:11:06Z- handleDepth()-Depth_Keller msg: 1613175066508 epoch sec; seqNo:10370; depth 18.01 2021-02-13T00:11:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:06Z- handleDvl() - RDI_Pathfinder msg: 1613175066.51 epoch sec; seqNo:5063 2021-02-13T00:11:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , 0.05 2021-02-13T00:11:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175066.52 epoch sec; seqNo:10370; 4058571.95 north; 592093.93 east 2021-02-13T00:11:06Z- AHRS_M2 msg: 1613175066.91 epoch sec; seqNo:10286 2021-02-13T00:11:06Z- AHRS_M2 msg: 0.83 phi; 0.31 theta; -0.14 psi 2021-02-13T00:11:06Z- handleDepth()-Depth_Keller msg: 1613175066918 epoch sec; seqNo:10371; depth 17.89 2021-02-13T00:11:06Z- handleDvl() - RDI_Pathfinder msg: 1613175066.93 epoch sec; seqNo:5064 2021-02-13T00:11:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.66 , 97.93 , 102.80 , 78.44 2021-02-13T00:11:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.06 2021-02-13T00:11:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175066.94 epoch sec; seqNo:10371; 4058571.95 north; 592093.93 east 2021-02-13T00:11:07Z- AHRS_M2 msg: 1613175067.31 epoch sec; seqNo:10287 2021-02-13T00:11:07Z- AHRS_M2 msg: 0.83 phi; 0.31 theta; -0.14 psi 2021-02-13T00:11:07Z- handleDepth()-Depth_Keller msg: 1613175067316 epoch sec; seqNo:10372; depth 17.79 2021-02-13T00:11:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175067.33 epoch sec; seqNo:10372; 4058572.33 north; 592093.93 east 2021-02-13T00:11:07Z- AHRS_M2 msg: 1613175067.72 epoch sec; seqNo:10288 2021-02-13T00:11:07Z- AHRS_M2 msg: 0.82 phi; 0.32 theta; -0.15 psi 2021-02-13T00:11:07Z- handleDepth()-Depth_Keller msg: 1613175067725 epoch sec; seqNo:10373; depth 17.67 2021-02-13T00:11:07Z- handleDvl() - RDI_Pathfinder msg: 1613175067.74 epoch sec; seqNo:5065 2021-02-13T00:11:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.50 , 97.79 , 102.66 , 73.18 2021-02-13T00:11:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , 0.08 2021-02-13T00:11:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175067.77 epoch sec; seqNo:10373; 4058572.33 north; 592093.93 east 2021-02-13T00:11:08Z- AHRS_M2 msg: 1613175068.12 epoch sec; seqNo:10289 2021-02-13T00:11:08Z- AHRS_M2 msg: 0.82 phi; 0.32 theta; -0.15 psi 2021-02-13T00:11:08Z- handleDepth()-Depth_Keller msg: 1613175068129 epoch sec; seqNo:10374; depth 17.56 2021-02-13T00:11:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175068.14 epoch sec; seqNo:10374; 4058572.71 north; 592093.93 east 2021-02-13T00:11:08Z- LcmTrn::handleMessages() - handling 47 messages took 0.70 ms and 133 calls... 2021-02-13T00:11:08Z- waiting for 1613175065.91, time is 1613175068.14 2021-02-13T00:11:08Z- LcmTrn::updateTrn() >>>> pose time 1613175068.14, meas time 1613175067.74 <<<< 2021-02-13T00:11:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175067.74 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:11:08Z- TerrainNav::motionUpdate() - currEstimate = (4058573, 592093.93, 17.56) 2021-02-13T00:11:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:11:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:11:08Z- TNavBF::Projecting Measurements 2021-02-13T00:11:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:11:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:11:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:11:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:11:08Z- TNavFilter::4 good beams. 2021-02-13T00:11:08Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:11:08Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:11:08Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:11:08Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:11:08Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:11:08Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:11:08Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:11:08Z- TNavBF::mmse selected from filter -1 2021-02-13T00:11:08Z- TerrainMapOctree: (vehN, vehE ) = (4058572.47, 592093.74). 2021-02-13T00:11:08Z- TerrainMapOctree: Min Distance = 3437.82. 2021-02-13T00:11:08Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:11:08Z- There is no specific good beam that all particles have in common. 2021-02-13T00:11:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:11:08Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:11:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:11:08Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-13T00:11:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:11:08Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 16.86 2021-02-13T00:11:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:11:08Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 8.57 2021-02-13T00:11:08Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:11:08Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:11:08Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:11:08Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:11:08Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:11:08Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:11:08Z- Filter 0 No beams used in subcloud update 2021-02-13T00:11:08Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:11:08Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:11:08Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 715.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:11:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175067.74 sec. 2021-02-13T00:11:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:11:08Z- TNavBF::mmse selected from filter -1 2021-02-13T00:11:08Z- variances([0], [2], and psi_berg) = 715.484722 833.006791 0.000000 2021-02-13T00:11:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:11:08Z- LcmTrn::publishEstimates() - MLE : 4058571.21 592045.43 17.56 2021-02-13T00:11:08Z- LcmTrn::publishEstimates() - MMSE : 4058572.71 592093.93 17.56 2021-02-13T00:11:08Z- LcmTrn::publishEstimates() - COVAR: 715.48 833.01 0.77 0.00 2021-02-13T00:11:08Z- AHRS_M2 msg: 1613175068.53 epoch sec; seqNo:10290 2021-02-13T00:11:08Z- AHRS_M2 msg: 0.82 phi; 0.32 theta; -0.15 psi 2021-02-13T00:11:08Z- handleDepth()-Depth_Keller msg: 1613175068537 epoch sec; seqNo:10375; depth 17.44 2021-02-13T00:11:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175068.55 epoch sec; seqNo:10375; 4058573.10 north; 592095.14 east 2021-02-13T00:11:08Z- AHRS_M2 msg: 1613175068.93 epoch sec; seqNo:10291 2021-02-13T00:11:08Z- AHRS_M2 msg: 0.82 phi; 0.33 theta; -0.15 psi 2021-02-13T00:11:08Z- handleDepth()-Depth_Keller msg: 1613175068932 epoch sec; seqNo:10376; depth 17.31 2021-02-13T00:11:08Z- handleDvl() - RDI_Pathfinder msg: 1613175068.94 epoch sec; seqNo:5066 2021-02-13T00:11:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.88 , 97.93 , 106.45 , 78.44 2021-02-13T00:11:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.01 , 0.06 2021-02-13T00:11:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175068.95 epoch sec; seqNo:10376; 4058573.10 north; 592095.14 east 2021-02-13T00:11:09Z- AHRS_M2 msg: 1613175069.33 epoch sec; seqNo:10292 2021-02-13T00:11:09Z- AHRS_M2 msg: 0.82 phi; 0.33 theta; -0.15 psi 2021-02-13T00:11:09Z- handleDepth()-Depth_Keller msg: 1613175069341 epoch sec; seqNo:10377; depth 17.22 2021-02-13T00:11:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175069.36 epoch sec; seqNo:10377; 4058573.48 north; 592095.14 east 2021-02-13T00:11:09Z- AHRS_M2 msg: 1613175069.73 epoch sec; seqNo:10293 2021-02-13T00:11:09Z- AHRS_M2 msg: 0.83 phi; 0.33 theta; -0.14 psi 2021-02-13T00:11:09Z- handleDepth()-Depth_Keller msg: 1613175069743 epoch sec; seqNo:10378; depth 17.11 2021-02-13T00:11:09Z- handleDvl() - RDI_Pathfinder msg: 1613175069.76 epoch sec; seqNo:5067 2021-02-13T00:11:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 97.72 , 107.46 , 70.73 2021-02-13T00:11:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.07 , -0.03 2021-02-13T00:11:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175069.77 epoch sec; seqNo:10378; 4058573.86 north; 592095.13 east 2021-02-13T00:11:10Z- AHRS_M2 msg: 1613175070.15 epoch sec; seqNo:10294 2021-02-13T00:11:10Z- AHRS_M2 msg: 0.85 phi; 0.33 theta; -0.13 psi 2021-02-13T00:11:10Z- handleDepth()-Depth_Keller msg: 1613175070158 epoch sec; seqNo:10379; depth 17.00 2021-02-13T00:11:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:10Z- handleDvl() - RDI_Pathfinder msg: 1613175070.16 epoch sec; seqNo:5068 2021-02-13T00:11:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.07 , -0.03 2021-02-13T00:11:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175070.17 epoch sec; seqNo:10379; 4058573.87 north; 592096.35 east 2021-02-13T00:11:10Z- AHRS_M2 msg: 1613175070.56 epoch sec; seqNo:10295 2021-02-13T00:11:10Z- AHRS_M2 msg: 0.86 phi; 0.33 theta; -0.13 psi 2021-02-13T00:11:10Z- handleDepth()-Depth_Keller msg: 1613175070564 epoch sec; seqNo:10380; depth 16.88 2021-02-13T00:11:10Z- handleDvl() - RDI_Pathfinder msg: 1613175070.58 epoch sec; seqNo:5069 2021-02-13T00:11:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.91 , 94.14 , 108.75 , 72.37 2021-02-13T00:11:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , -0.14 2021-02-13T00:11:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175070.59 epoch sec; seqNo:10380; 4058574.25 north; 592096.35 east 2021-02-13T00:11:10Z- AHRS_M2 msg: 1613175070.95 epoch sec; seqNo:10296 2021-02-13T00:11:10Z- AHRS_M2 msg: 0.87 phi; 0.33 theta; -0.13 psi 2021-02-13T00:11:10Z- handleDepth()-Depth_Keller msg: 1613175070954 epoch sec; seqNo:10381; depth 16.76 2021-02-13T00:11:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175070.96 epoch sec; seqNo:10381; 4058574.25 north; 592096.35 east 2021-02-13T00:11:11Z- AHRS_M2 msg: 1613175071.35 epoch sec; seqNo:10297 2021-02-13T00:11:11Z- AHRS_M2 msg: 0.88 phi; 0.32 theta; -0.12 psi 2021-02-13T00:11:11Z- handleDepth()-Depth_Keller msg: 1613175071356 epoch sec; seqNo:10382; depth 16.66 2021-02-13T00:11:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:11Z- handleDvl() - RDI_Pathfinder msg: 1613175071.36 epoch sec; seqNo:5070 2021-02-13T00:11:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , -0.14 2021-02-13T00:11:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175071.37 epoch sec; seqNo:10382; 4058574.63 north; 592096.35 east 2021-02-13T00:11:11Z- AHRS_M2 msg: 1613175071.76 epoch sec; seqNo:10298 2021-02-13T00:11:11Z- AHRS_M2 msg: 0.89 phi; 0.32 theta; -0.11 psi 2021-02-13T00:11:11Z- handleDepth()-Depth_Keller msg: 1613175071760 epoch sec; seqNo:10383; depth 16.54 2021-02-13T00:11:11Z- handleDvl() - RDI_Pathfinder msg: 1613175071.77 epoch sec; seqNo:5071 2021-02-13T00:11:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.10 , 95.49 , 107.67 , 0.00 2021-02-13T00:11:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.00 , -0.17 2021-02-13T00:11:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175071.80 epoch sec; seqNo:10383; 4058574.63 north; 592096.35 east 2021-02-13T00:11:12Z- AHRS_M2 msg: 1613175072.16 epoch sec; seqNo:10299 2021-02-13T00:11:12Z- AHRS_M2 msg: 0.90 phi; 0.31 theta; -0.10 psi 2021-02-13T00:11:12Z- handleDepth()-Depth_Keller msg: 1613175072165 epoch sec; seqNo:10384; depth 16.43 2021-02-13T00:11:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175072.18 epoch sec; seqNo:10384; 4058575.02 north; 592097.56 east 2021-02-13T00:11:12Z- AHRS_M2 msg: 1613175072.56 epoch sec; seqNo:10300 2021-02-13T00:11:12Z- AHRS_M2 msg: 0.90 phi; 0.31 theta; -0.11 psi 2021-02-13T00:11:12Z- handleDepth()-Depth_Keller msg: 1613175072570 epoch sec; seqNo:10385; depth 16.33 2021-02-13T00:11:12Z- handleDvl() - RDI_Pathfinder msg: 1613175072.59 epoch sec; seqNo:5072 2021-02-13T00:11:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 92.04 , 106.92 , 71.55 2021-02-13T00:11:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.08 , -0.09 2021-02-13T00:11:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175072.60 epoch sec; seqNo:10385; 4058575.02 north; 592097.56 east 2021-02-13T00:11:12Z- AHRS_M2 msg: 1613175072.97 epoch sec; seqNo:10301 2021-02-13T00:11:12Z- AHRS_M2 msg: 0.90 phi; 0.31 theta; -0.11 psi 2021-02-13T00:11:12Z- handleDepth()-Depth_Keller msg: 1613175072972 epoch sec; seqNo:10386; depth 16.21 2021-02-13T00:11:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175072.99 epoch sec; seqNo:10386; 4058575.40 north; 592097.56 east 2021-02-13T00:11:13Z- AHRS_M2 msg: 1613175073.37 epoch sec; seqNo:10302 2021-02-13T00:11:13Z- AHRS_M2 msg: 0.89 phi; 0.31 theta; -0.13 psi 2021-02-13T00:11:13Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-13T00:11:13Z- waiting for 1613175071.14, time is 1613175073.37 2021-02-13T00:11:13Z- LcmTrn::updateTrn() >>>> pose time 1613175073.37, meas time 1613175072.59 <<<< 2021-02-13T00:11:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175072.59 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:11:13Z- TerrainNav::motionUpdate() - currEstimate = (4058575, 592097.56, 16.21) 2021-02-13T00:11:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:11:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:11:13Z- TNavBF::Projecting Measurements 2021-02-13T00:11:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:11:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:11:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:11:13Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:11:13Z- TNavFilter::3 good beams. 2021-02-13T00:11:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:11:13Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:11:13Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:11:13Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:11:13Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:11:13Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:11:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:11:13Z- TNavBF::mmse selected from filter -1 2021-02-13T00:11:13Z- TerrainMapOctree: (vehN, vehE ) = (4058575.00, 592097.01). 2021-02-13T00:11:13Z- TerrainMapOctree: Min Distance = 3433.80. 2021-02-13T00:11:13Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:11:13Z- There is no specific good beam that all particles have in common. 2021-02-13T00:11:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:11:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 13.55 2021-02-13T00:11:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:11:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 18.29 2021-02-13T00:11:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:11:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 8.19 2021-02-13T00:11:13Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:11:13Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:11:13Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:11:13Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:11:13Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:11:13Z- Filter 0 No beams used in subcloud update 2021-02-13T00:11:13Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:11:13Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:11:13Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 715.4 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:11:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175072.59 sec. 2021-02-13T00:11:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:11:13Z- TNavBF::mmse selected from filter -1 2021-02-13T00:11:13Z- variances([0], [2], and psi_berg) = 715.395479 833.006736 0.000000 2021-02-13T00:11:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:11:13Z- LcmTrn::publishEstimates() - MLE : 4058572.90 592139.06 16.21 2021-02-13T00:11:13Z- LcmTrn::publishEstimates() - MMSE : 4058575.40 592097.56 16.21 2021-02-13T00:11:13Z- LcmTrn::publishEstimates() - COVAR: 715.40 833.01 0.84 0.00 2021-02-13T00:11:13Z- handleDepth()-Depth_Keller msg: 1613175073376 epoch sec; seqNo:10387; depth 16.10 2021-02-13T00:11:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175073.38 epoch sec; seqNo:10387; 4058575.40 north; 592097.56 east 2021-02-13T00:11:13Z- AHRS_M2 msg: 1613175073.78 epoch sec; seqNo:10303 2021-02-13T00:11:13Z- AHRS_M2 msg: 0.88 phi; 0.31 theta; -0.14 psi 2021-02-13T00:11:13Z- handleDepth()-Depth_Keller msg: 1613175073780 epoch sec; seqNo:10388; depth 15.98 2021-02-13T00:11:13Z- handleDvl() - RDI_Pathfinder msg: 1613175073.79 epoch sec; seqNo:5073 2021-02-13T00:11:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 94.14 , 106.31 , 73.18 2021-02-13T00:11:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.11 , 0.06 2021-02-13T00:11:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175073.80 epoch sec; seqNo:10388; 4058575.78 north; 592097.56 east 2021-02-13T00:11:14Z- AHRS_M2 msg: 1613175074.18 epoch sec; seqNo:10304 2021-02-13T00:11:14Z- AHRS_M2 msg: 0.86 phi; 0.31 theta; -0.15 psi 2021-02-13T00:11:14Z- handleDepth()-Depth_Keller msg: 1613175074193 epoch sec; seqNo:10389; depth 15.84 2021-02-13T00:11:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175074.21 epoch sec; seqNo:10389; 4058575.78 north; 592097.56 east 2021-02-13T00:11:14Z- AHRS_M2 msg: 1613175074.58 epoch sec; seqNo:10305 2021-02-13T00:11:14Z- AHRS_M2 msg: 0.85 phi; 0.31 theta; -0.17 psi 2021-02-13T00:11:14Z- handleDepth()-Depth_Keller msg: 1613175074588 epoch sec; seqNo:10390; depth 15.73 2021-02-13T00:11:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175074.60 epoch sec; seqNo:10390; 4058576.16 north; 592097.55 east 2021-02-13T00:11:14Z- AHRS_M2 msg: 1613175074.99 epoch sec; seqNo:10306 2021-02-13T00:11:14Z- AHRS_M2 msg: 0.84 phi; 0.32 theta; -0.17 psi 2021-02-13T00:11:14Z- handleDepth()-Depth_Keller msg: 1613175074992 epoch sec; seqNo:10391; depth 15.63 2021-02-13T00:11:15Z- handleDvl() - RDI_Pathfinder msg: 1613175075.00 epoch sec; seqNo:5074 2021-02-13T00:11:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.81 , 97.86 , 106.38 , 78.38 2021-02-13T00:11:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.05 , 0.17 2021-02-13T00:11:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175075.03 epoch sec; seqNo:10391; 4058576.55 north; 592098.77 east 2021-02-13T00:11:15Z- AHRS_M2 msg: 1613175075.39 epoch sec; seqNo:10307 2021-02-13T00:11:15Z- AHRS_M2 msg: 0.83 phi; 0.32 theta; -0.18 psi 2021-02-13T00:11:15Z- handleDepth()-Depth_Keller msg: 1613175075396 epoch sec; seqNo:10392; depth 15.50 2021-02-13T00:11:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175075.41 epoch sec; seqNo:10392; 4058576.55 north; 592098.77 east 2021-02-13T00:11:15Z- AHRS_M2 msg: 1613175075.80 epoch sec; seqNo:10308 2021-02-13T00:11:15Z- AHRS_M2 msg: 0.83 phi; 0.33 theta; -0.18 psi 2021-02-13T00:11:15Z- handleDepth()-Depth_Keller msg: 1613175075805 epoch sec; seqNo:10393; depth 15.40 2021-02-13T00:11:15Z- handleDvl() - RDI_Pathfinder msg: 1613175075.82 epoch sec; seqNo:5075 2021-02-13T00:11:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 97.72 , 107.46 , 78.24 2021-02-13T00:11:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.06 , 0.14 2021-02-13T00:11:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175075.83 epoch sec; seqNo:10393; 4058576.93 north; 592098.76 east 2021-02-13T00:11:16Z- AHRS_M2 msg: 1613175076.20 epoch sec; seqNo:10309 2021-02-13T00:11:16Z- AHRS_M2 msg: 0.83 phi; 0.33 theta; -0.18 psi 2021-02-13T00:11:16Z- handleDepth()-Depth_Keller msg: 1613175076209 epoch sec; seqNo:10394; depth 15.29 2021-02-13T00:11:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:16Z- handleDvl() - RDI_Pathfinder msg: 1613175076.21 epoch sec; seqNo:5076 2021-02-13T00:11:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.06 , 0.14 2021-02-13T00:11:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175076.23 epoch sec; seqNo:10394; 4058576.93 north; 592098.76 east 2021-02-13T00:11:16Z- AHRS_M2 msg: 1613175076.62 epoch sec; seqNo:10310 2021-02-13T00:11:16Z- AHRS_M2 msg: 0.84 phi; 0.33 theta; -0.18 psi 2021-02-13T00:11:16Z- handleDepth()-Depth_Keller msg: 1613175076631 epoch sec; seqNo:10395; depth 15.19 2021-02-13T00:11:16Z- handleDvl() - RDI_Pathfinder msg: 1613175076.65 epoch sec; seqNo:5077 2021-02-13T00:11:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.83 , 98.87 , 109.83 , 74.82 2021-02-13T00:11:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.09 , -0.03 2021-02-13T00:11:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175076.66 epoch sec; seqNo:10395; 4058577.32 north; 592099.98 east 2021-02-13T00:11:17Z- AHRS_M2 msg: 1613175077.01 epoch sec; seqNo:10311 2021-02-13T00:11:17Z- AHRS_M2 msg: 0.84 phi; 0.33 theta; -0.18 psi 2021-02-13T00:11:17Z- handleDepth()-Depth_Keller msg: 1613175077017 epoch sec; seqNo:10396; depth 15.07 2021-02-13T00:11:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175077.03 epoch sec; seqNo:10396; 4058577.32 north; 592099.98 east 2021-02-13T00:11:17Z- AHRS_M2 msg: 1613175077.41 epoch sec; seqNo:10312 2021-02-13T00:11:17Z- AHRS_M2 msg: 0.85 phi; 0.33 theta; -0.18 psi 2021-02-13T00:11:17Z- handleDepth()-Depth_Keller msg: 1613175077418 epoch sec; seqNo:10397; depth 14.98 2021-02-13T00:11:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:17Z- handleDvl() - RDI_Pathfinder msg: 1613175077.42 epoch sec; seqNo:5078 2021-02-13T00:11:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.09 , -0.03 2021-02-13T00:11:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175077.43 epoch sec; seqNo:10397; 4058577.70 north; 592099.98 east 2021-02-13T00:11:17Z- AHRS_M2 msg: 1613175077.82 epoch sec; seqNo:10313 2021-02-13T00:11:17Z- AHRS_M2 msg: 0.86 phi; 0.32 theta; -0.18 psi 2021-02-13T00:11:17Z- handleDepth()-Depth_Keller msg: 1613175077821 epoch sec; seqNo:10398; depth 14.87 2021-02-13T00:11:17Z- handleDvl() - RDI_Pathfinder msg: 1613175077.84 epoch sec; seqNo:5079 2021-02-13T00:11:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.83 , 98.87 , 112.27 , 74.00 2021-02-13T00:11:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.02 , -0.15 2021-02-13T00:11:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175077.85 epoch sec; seqNo:10398; 4058578.08 north; 592099.97 east 2021-02-13T00:11:18Z- AHRS_M2 msg: 1613175078.22 epoch sec; seqNo:10314 2021-02-13T00:11:18Z- AHRS_M2 msg: 0.86 phi; 0.32 theta; -0.18 psi 2021-02-13T00:11:18Z- handleDepth()-Depth_Keller msg: 1613175078225 epoch sec; seqNo:10399; depth 14.75 2021-02-13T00:11:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175078.24 epoch sec; seqNo:10399; 4058578.08 north; 592099.97 east 2021-02-13T00:11:18Z- LcmTrn::handleMessages() - handling 45 messages took 0.69 ms and 131 calls... 2021-02-13T00:11:18Z- waiting for 1613175076.37, time is 1613175078.24 2021-02-13T00:11:18Z- LcmTrn::updateTrn() >>>> pose time 1613175078.24, meas time 1613175077.84 <<<< 2021-02-13T00:11:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175077.84 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:11:18Z- TerrainNav::motionUpdate() - currEstimate = (4058578, 592099.97, 14.75) 2021-02-13T00:11:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:11:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:11:18Z- TNavBF::Projecting Measurements 2021-02-13T00:11:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:11:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:11:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:11:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:11:18Z- TNavFilter::4 good beams. 2021-02-13T00:11:18Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:11:18Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:11:18Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:11:18Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:11:18Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:11:18Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:11:18Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:11:18Z- TNavBF::mmse selected from filter -1 2021-02-13T00:11:18Z- TerrainMapOctree: (vehN, vehE ) = (4058577.86, 592099.78). 2021-02-13T00:11:18Z- TerrainMapOctree: Min Distance = 3429.84. 2021-02-13T00:11:18Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:11:18Z- There is no specific good beam that all particles have in common. 2021-02-13T00:11:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:11:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 10.71 2021-02-13T00:11:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:11:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 15.64 2021-02-13T00:11:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:11:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 20.17 2021-02-13T00:11:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:11:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 8.76 2021-02-13T00:11:18Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:11:18Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:11:18Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:11:18Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:11:18Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:11:18Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:11:18Z- Filter 0 No beams used in subcloud update 2021-02-13T00:11:18Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:11:18Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:11:18Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 715.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:11:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175077.84 sec. 2021-02-13T00:11:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:11:18Z- TNavBF::mmse selected from filter -1 2021-02-13T00:11:18Z- variances([0], [2], and psi_berg) = 715.319862 833.006689 0.000000 2021-02-13T00:11:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:11:18Z- LcmTrn::publishEstimates() - MLE : 4058575.58 592141.47 14.75 2021-02-13T00:11:18Z- LcmTrn::publishEstimates() - MMSE : 4058578.08 592099.97 14.75 2021-02-13T00:11:18Z- LcmTrn::publishEstimates() - COVAR: 715.32 833.01 0.80 0.00 2021-02-13T00:11:18Z- AHRS_M2 msg: 1613175078.62 epoch sec; seqNo:10315 2021-02-13T00:11:18Z- AHRS_M2 msg: 0.86 phi; 0.31 theta; -0.18 psi 2021-02-13T00:11:18Z- handleDepth()-Depth_Keller msg: 1613175078629 epoch sec; seqNo:10400; depth 14.67 2021-02-13T00:11:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:18Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:18Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:18Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:18Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:18Z- handleDvl() - RDI_Pathfinder msg: 1613175078.63 epoch sec; seqNo:5080 2021-02-13T00:11:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.02 , -0.15 2021-02-13T00:11:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175078.64 epoch sec; seqNo:10400; 4058578.47 north; 592101.19 east 2021-02-13T00:11:19Z- AHRS_M2 msg: 1613175079.03 epoch sec; seqNo:10316 2021-02-13T00:11:19Z- AHRS_M2 msg: 0.86 phi; 0.31 theta; -0.19 psi 2021-02-13T00:11:19Z- handleDepth()-Depth_Keller msg: 1613175079113 epoch sec; seqNo:10401; depth 14.53 2021-02-13T00:11:19Z- handleDvl() - RDI_Pathfinder msg: 1613175079.13 epoch sec; seqNo:5081 2021-02-13T00:11:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.83 , 101.31 , 109.83 , 78.17 2021-02-13T00:11:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.17 , -0.08 2021-02-13T00:11:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175079.15 epoch sec; seqNo:10401; 4058578.47 north; 592101.19 east 2021-02-13T00:11:19Z- AHRS_M2 msg: 1613175079.43 epoch sec; seqNo:10317 2021-02-13T00:11:19Z- AHRS_M2 msg: 0.85 phi; 0.30 theta; -0.19 psi 2021-02-13T00:11:19Z- handleDepth()-Depth_Keller msg: 1613175079436 epoch sec; seqNo:10402; depth 14.38 2021-02-13T00:11:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175079.45 epoch sec; seqNo:10402; 4058578.85 north; 592101.18 east 2021-02-13T00:11:19Z- AHRS_M2 msg: 1613175079.84 epoch sec; seqNo:10318 2021-02-13T00:11:19Z- AHRS_M2 msg: 0.84 phi; 0.30 theta; -0.19 psi 2021-02-13T00:11:19Z- handleDepth()-Depth_Keller msg: 1613175079845 epoch sec; seqNo:10403; depth 14.30 2021-02-13T00:11:19Z- handleDvl() - RDI_Pathfinder msg: 1613175079.86 epoch sec; seqNo:5082 2021-02-13T00:11:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.72 , 100.36 , 109.83 , 78.71 2021-02-13T00:11:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.19 , 0.15 2021-02-13T00:11:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175079.87 epoch sec; seqNo:10403; 4058579.23 north; 592101.18 east 2021-02-13T00:11:20Z- AHRS_M2 msg: 1613175080.24 epoch sec; seqNo:10319 2021-02-13T00:11:20Z- AHRS_M2 msg: 0.83 phi; 0.31 theta; -0.19 psi 2021-02-13T00:11:20Z- handleDepth()-Depth_Keller msg: 1613175080246 epoch sec; seqNo:10404; depth 14.20 2021-02-13T00:11:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175080.26 epoch sec; seqNo:10404; 4058579.23 north; 592101.18 east 2021-02-13T00:11:20Z- AHRS_M2 msg: 1613175080.64 epoch sec; seqNo:10320 2021-02-13T00:11:20Z- AHRS_M2 msg: 0.82 phi; 0.31 theta; -0.18 psi 2021-02-13T00:11:20Z- handleDepth()-Depth_Keller msg: 1613175080648 epoch sec; seqNo:10405; depth 14.10 2021-02-13T00:11:20Z- handleDvl() - RDI_Pathfinder msg: 1613175080.67 epoch sec; seqNo:5083 2021-02-13T00:11:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.43 , 109.90 , 78.78 2021-02-13T00:11:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , 0.32 2021-02-13T00:11:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175080.68 epoch sec; seqNo:10405; 4058579.61 north; 592101.18 east 2021-02-13T00:11:21Z- AHRS_M2 msg: 1613175081.05 epoch sec; seqNo:10321 2021-02-13T00:11:21Z- AHRS_M2 msg: 0.81 phi; 0.32 theta; -0.18 psi 2021-02-13T00:11:21Z- handleDepth()-Depth_Keller msg: 1613175081057 epoch sec; seqNo:10406; depth 13.98 2021-02-13T00:11:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:21Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:21Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:21Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:21Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:21Z- handleDvl() - RDI_Pathfinder msg: 1613175081.06 epoch sec; seqNo:5084 2021-02-13T00:11:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , 0.32 2021-02-13T00:11:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175081.07 epoch sec; seqNo:10406; 4058580.00 north; 592102.39 east 2021-02-13T00:11:21Z- AHRS_M2 msg: 1613175081.45 epoch sec; seqNo:10322 2021-02-13T00:11:21Z- AHRS_M2 msg: 0.80 phi; 0.32 theta; -0.17 psi 2021-02-13T00:11:21Z- handleDepth()-Depth_Keller msg: 1613175081458 epoch sec; seqNo:10407; depth 13.88 2021-02-13T00:11:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175081.47 epoch sec; seqNo:10407; 4058580.00 north; 592102.39 east 2021-02-13T00:11:21Z- AHRS_M2 msg: 1613175081.86 epoch sec; seqNo:10323 2021-02-13T00:11:21Z- AHRS_M2 msg: 0.80 phi; 0.33 theta; -0.18 psi 2021-02-13T00:11:21Z- handleDepth()-Depth_Keller msg: 1613175081860 epoch sec; seqNo:10408; depth 13.76 2021-02-13T00:11:21Z- handleDvl() - RDI_Pathfinder msg: 1613175081.88 epoch sec; seqNo:5085 2021-02-13T00:11:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.86 , 101.44 , 110.91 , 78.44 2021-02-13T00:11:21Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.21 , 0.22 2021-02-13T00:11:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175081.89 epoch sec; seqNo:10408; 4058580.38 north; 592102.39 east 2021-02-13T00:11:22Z- AHRS_M2 msg: 1613175082.26 epoch sec; seqNo:10324 2021-02-13T00:11:22Z- AHRS_M2 msg: 0.80 phi; 0.34 theta; -0.18 psi 2021-02-13T00:11:22Z- handleDepth()-Depth_Keller msg: 1613175082264 epoch sec; seqNo:10409; depth 13.63 2021-02-13T00:11:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:22Z- handleDvl() - RDI_Pathfinder msg: 1613175082.27 epoch sec; seqNo:5086 2021-02-13T00:11:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:22Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.21 , 0.22 2021-02-13T00:11:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175082.28 epoch sec; seqNo:10409; 4058580.39 north; 592103.61 east 2021-02-13T00:11:22Z- AHRS_M2 msg: 1613175082.66 epoch sec; seqNo:10325 2021-02-13T00:11:22Z- AHRS_M2 msg: 0.80 phi; 0.34 theta; -0.18 psi 2021-02-13T00:11:22Z- handleDepth()-Depth_Keller msg: 1613175082668 epoch sec; seqNo:10410; depth 13.53 2021-02-13T00:11:22Z- handleDvl() - RDI_Pathfinder msg: 1613175082.68 epoch sec; seqNo:5087 2021-02-13T00:11:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.29 , 115.18 , 78.65 2021-02-13T00:11:22Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , 0.24 , -0.06 2021-02-13T00:11:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175082.69 epoch sec; seqNo:10410; 4058580.77 north; 592103.61 east 2021-02-13T00:11:23Z- AHRS_M2 msg: 1613175083.07 epoch sec; seqNo:10326 2021-02-13T00:11:23Z- AHRS_M2 msg: 0.81 phi; 0.34 theta; -0.18 psi 2021-02-13T00:11:23Z- handleDepth()-Depth_Keller msg: 1613175083074 epoch sec; seqNo:10411; depth 13.41 2021-02-13T00:11:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175083.09 epoch sec; seqNo:10411; 4058580.77 north; 592103.61 east 2021-02-13T00:11:23Z- AHRS_M2 msg: 1613175083.47 epoch sec; seqNo:10327 2021-02-13T00:11:23Z- AHRS_M2 msg: 0.82 phi; 0.34 theta; -0.18 psi 2021-02-13T00:11:23Z- handleDepth()-Depth_Keller msg: 1613175083476 epoch sec; seqNo:10412; depth 13.32 2021-02-13T00:11:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:23Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:23Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:23Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:23Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:23Z- handleDvl() - RDI_Pathfinder msg: 1613175083.48 epoch sec; seqNo:5088 2021-02-13T00:11:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:23Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , 0.24 , -0.06 2021-02-13T00:11:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175083.49 epoch sec; seqNo:10412; 4058581.15 north; 592103.60 east 2021-02-13T00:11:23Z- LcmTrn::handleMessages() - handling 48 messages took 0.72 ms and 134 calls... 2021-02-13T00:11:23Z- waiting for 1613175081.24, time is 1613175083.49 2021-02-13T00:11:23Z- LcmTrn::updateTrn() equating pose 1613175083.49 and meas times 1613175083.49 2021-02-13T00:11:23Z- LcmTrn::updateTrn() >>>> pose time 1613175083.49, meas time 1613175083.49 <<<< 2021-02-13T00:11:23Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175083.49, ping # 0. 2021-02-13T00:11:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175083.49 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:11:23Z- TerrainNav::motionUpdate() - currEstimate = (4058581, 592103.60, 13.32) 2021-02-13T00:11:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:11:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:11:23Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:11:23Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:11:23Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175083.49 sec.; No relevant navigation data available 2021-02-13T00:11:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:11:23Z- TNavBF::mmse selected from filter -1 2021-02-13T00:11:23Z- variances([0], [2], and psi_berg) = 715.250105 833.006646 0.000000 2021-02-13T00:11:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:11:23Z- LcmTrn::publishEstimates() - MLE : 4058578.65 592145.10 13.32 2021-02-13T00:11:23Z- LcmTrn::publishEstimates() - MMSE : 4058581.15 592103.60 13.32 2021-02-13T00:11:23Z- LcmTrn::publishEstimates() - COVAR: 715.25 833.01 1.04 0.00 2021-02-13T00:11:23Z- AHRS_M2 msg: 1613175083.88 epoch sec; seqNo:10328 2021-02-13T00:11:23Z- AHRS_M2 msg: 0.83 phi; 0.34 theta; -0.17 psi 2021-02-13T00:11:23Z- handleDepth()-Depth_Keller msg: 1613175083881 epoch sec; seqNo:10413; depth 13.22 2021-02-13T00:11:23Z- handleDvl() - RDI_Pathfinder msg: 1613175083.89 epoch sec; seqNo:5089 2021-02-13T00:11:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 101.24 , 116.12 , 78.24 2021-02-13T00:11:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.10 , -0.26 2021-02-13T00:11:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175083.92 epoch sec; seqNo:10413; 4058581.54 north; 592104.82 east 2021-02-13T00:11:24Z- AHRS_M2 msg: 1613175084.28 epoch sec; seqNo:10329 2021-02-13T00:11:24Z- AHRS_M2 msg: 0.84 phi; 0.33 theta; -0.16 psi 2021-02-13T00:11:24Z- handleDepth()-Depth_Keller msg: 1613175084284 epoch sec; seqNo:10414; depth 13.10 2021-02-13T00:11:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175084.30 epoch sec; seqNo:10414; 4058581.54 north; 592104.82 east 2021-02-13T00:11:24Z- handleDepth()-Depth_Keller msg: 1613175084713 epoch sec; seqNo:10415; depth 12.98 2021-02-13T00:11:24Z- handleDvl() - RDI_Pathfinder msg: 1613175084.72 epoch sec; seqNo:5090 2021-02-13T00:11:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.28 , 98.81 , 116.39 , 78.51 2021-02-13T00:11:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.08 , -0.26 2021-02-13T00:11:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175084.76 epoch sec; seqNo:10415; 4058581.92 north; 592104.81 east 2021-02-13T00:11:25Z- AHRS_M2 msg: 1613175085.09 epoch sec; seqNo:10330 2021-02-13T00:11:25Z- AHRS_M2 msg: 0.85 phi; 0.32 theta; -0.15 psi 2021-02-13T00:11:25Z- handleDepth()-Depth_Keller msg: 1613175085102 epoch sec; seqNo:10416; depth 12.85 2021-02-13T00:11:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175085.12 epoch sec; seqNo:10416; 4058581.92 north; 592104.81 east 2021-02-13T00:11:25Z- AHRS_M2 msg: 1613175085.49 epoch sec; seqNo:10331 2021-02-13T00:11:25Z- AHRS_M2 msg: 0.85 phi; 0.32 theta; -0.15 psi 2021-02-13T00:11:25Z- handleDepth()-Depth_Keller msg: 1613175085496 epoch sec; seqNo:10417; depth 12.74 2021-02-13T00:11:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175085.51 epoch sec; seqNo:10417; 4058582.30 north; 592104.81 east 2021-02-13T00:11:25Z- AHRS_M2 msg: 1613175085.90 epoch sec; seqNo:10332 2021-02-13T00:11:25Z- AHRS_M2 msg: 0.84 phi; 0.32 theta; -0.15 psi 2021-02-13T00:11:25Z- handleDepth()-Depth_Keller msg: 1613175085908 epoch sec; seqNo:10418; depth 12.64 2021-02-13T00:11:25Z- handleDvl() - RDI_Pathfinder msg: 1613175085.92 epoch sec; seqNo:5091 2021-02-13T00:11:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 100.22 , 112.40 , 78.58 2021-02-13T00:11:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.14 , -0.11 2021-02-13T00:11:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175085.95 epoch sec; seqNo:10418; 4058582.30 north; 592104.81 east 2021-02-13T00:11:26Z- AHRS_M2 msg: 1613175086.30 epoch sec; seqNo:10333 2021-02-13T00:11:26Z- AHRS_M2 msg: 0.83 phi; 0.32 theta; -0.16 psi 2021-02-13T00:11:26Z- handleDepth()-Depth_Keller msg: 1613175086307 epoch sec; seqNo:10419; depth 12.49 2021-02-13T00:11:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175086.32 epoch sec; seqNo:10419; 4058582.68 north; 592104.81 east 2021-02-13T00:11:26Z- AHRS_M2 msg: 1613175086.70 epoch sec; seqNo:10334 2021-02-13T00:11:26Z- AHRS_M2 msg: 0.82 phi; 0.33 theta; -0.17 psi 2021-02-13T00:11:26Z- handleDepth()-Depth_Keller msg: 1613175086708 epoch sec; seqNo:10420; depth 12.37 2021-02-13T00:11:26Z- handleDvl() - RDI_Pathfinder msg: 1613175086.74 epoch sec; seqNo:5092 2021-02-13T00:11:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 101.57 , 112.39 , 78.58 2021-02-13T00:11:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.12 , 0.09 2021-02-13T00:11:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175086.75 epoch sec; seqNo:10420; 4058582.69 north; 592106.03 east 2021-02-13T00:11:27Z- AHRS_M2 msg: 1613175087.11 epoch sec; seqNo:10335 2021-02-13T00:11:27Z- AHRS_M2 msg: 0.81 phi; 0.34 theta; -0.17 psi 2021-02-13T00:11:27Z- handleDepth()-Depth_Keller msg: 1613175087112 epoch sec; seqNo:10421; depth 12.25 2021-02-13T00:11:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:27Z- handleDvl() - RDI_Pathfinder msg: 1613175087.12 epoch sec; seqNo:5093 2021-02-13T00:11:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.12 , 0.09 2021-02-13T00:11:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175087.15 epoch sec; seqNo:10421; 4058583.07 north; 592106.02 east 2021-02-13T00:11:27Z- AHRS_M2 msg: 1613175087.52 epoch sec; seqNo:10336 2021-02-13T00:11:27Z- AHRS_M2 msg: 0.79 phi; 0.35 theta; -0.17 psi 2021-02-13T00:11:27Z- handleDepth()-Depth_Keller msg: 1613175087522 epoch sec; seqNo:10422; depth 12.12 2021-02-13T00:11:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175087.53 epoch sec; seqNo:10422; 4058583.45 north; 592106.02 east 2021-02-13T00:11:27Z- AHRS_M2 msg: 1613175087.92 epoch sec; seqNo:10337 2021-02-13T00:11:27Z- AHRS_M2 msg: 0.78 phi; 0.36 theta; -0.17 psi 2021-02-13T00:11:27Z- handleDepth()-Depth_Keller msg: 1613175087925 epoch sec; seqNo:10423; depth 11.98 2021-02-13T00:11:27Z- handleDvl() - RDI_Pathfinder msg: 1613175087.94 epoch sec; seqNo:5094 2021-02-13T00:11:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 101.57 , 117.81 , 77.67 2021-02-13T00:11:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.02 , 0.21 2021-02-13T00:11:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175087.95 epoch sec; seqNo:10423; 4058583.45 north; 592106.02 east 2021-02-13T00:11:28Z- AHRS_M2 msg: 1613175088.32 epoch sec; seqNo:10338 2021-02-13T00:11:28Z- AHRS_M2 msg: 0.78 phi; 0.36 theta; -0.17 psi 2021-02-13T00:11:28Z- handleDepth()-Depth_Keller msg: 1613175088325 epoch sec; seqNo:10424; depth 11.85 2021-02-13T00:11:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:28Z- handleDvl() - RDI_Pathfinder msg: 1613175088.33 epoch sec; seqNo:5095 2021-02-13T00:11:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.02 , 0.21 2021-02-13T00:11:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175088.34 epoch sec; seqNo:10424; 4058583.83 north; 592106.01 east 2021-02-13T00:11:28Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-13T00:11:28Z- waiting for 1613175086.49, time is 1613175088.34 2021-02-13T00:11:28Z- LcmTrn::updateTrn() equating pose 1613175088.34 and meas times 1613175088.34 2021-02-13T00:11:28Z- LcmTrn::updateTrn() >>>> pose time 1613175088.34, meas time 1613175088.34 <<<< 2021-02-13T00:11:28Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175088.34, ping # 0. 2021-02-13T00:11:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175088.34 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:11:28Z- TerrainNav::motionUpdate() - currEstimate = (4058584, 592106.01, 11.85) 2021-02-13T00:11:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:11:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:11:28Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:11:28Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:11:28Z- TNavBF::Projecting Measurements 2021-02-13T00:11:28Z- TNavFilter::Measurement from time = 1613175088.34 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:11:28Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:11:28Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:11:28Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:11:28Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:11:28Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:11:28Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:11:28Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:11:28Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 715.2 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:11:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:11:28Z- TNavBF::mmse selected from filter -1 2021-02-13T00:11:28Z- variances([0], [2], and psi_berg) = 715.189409 833.006608 0.000000 2021-02-13T00:11:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:11:28Z- LcmTrn::publishEstimates() - MLE : 4058582.33 592064.51 11.85 2021-02-13T00:11:28Z- LcmTrn::publishEstimates() - MMSE : 4058583.83 592106.01 11.85 2021-02-13T00:11:28Z- LcmTrn::publishEstimates() - COVAR: 715.19 833.01 1.23 0.00 2021-02-13T00:11:28Z- AHRS_M2 msg: 1613175088.73 epoch sec; seqNo:10339 2021-02-13T00:11:28Z- AHRS_M2 msg: 0.79 phi; 0.36 theta; -0.16 psi 2021-02-13T00:11:28Z- handleDepth()-Depth_Keller msg: 1613175088731 epoch sec; seqNo:10425; depth 11.72 2021-02-13T00:11:28Z- handleDvl() - RDI_Pathfinder msg: 1613175088.74 epoch sec; seqNo:5096 2021-02-13T00:11:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.62 , 101.50 , 117.74 , 74.81 2021-02-13T00:11:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.08 , 0.19 2021-02-13T00:11:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175088.75 epoch sec; seqNo:10425; 4058583.83 north; 592106.01 east 2021-02-13T00:11:29Z- AHRS_M2 msg: 1613175089.13 epoch sec; seqNo:10340 2021-02-13T00:11:29Z- AHRS_M2 msg: 0.79 phi; 0.36 theta; -0.16 psi 2021-02-13T00:11:29Z- handleDepth()-Depth_Keller msg: 1613175089138 epoch sec; seqNo:10426; depth 11.58 2021-02-13T00:11:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175089.15 epoch sec; seqNo:10426; 4058584.22 north; 592107.23 east 2021-02-13T00:11:29Z- AHRS_M2 msg: 1613175089.53 epoch sec; seqNo:10341 2021-02-13T00:11:29Z- AHRS_M2 msg: 0.79 phi; 0.36 theta; -0.16 psi 2021-02-13T00:11:29Z- handleDepth()-Depth_Keller msg: 1613175089536 epoch sec; seqNo:10427; depth 11.46 2021-02-13T00:11:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:29Z- handleDvl() - RDI_Pathfinder msg: 1613175089.54 epoch sec; seqNo:5097 2021-02-13T00:11:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.08 , 0.19 2021-02-13T00:11:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175089.56 epoch sec; seqNo:10427; 4058584.22 north; 592107.23 east 2021-02-13T00:11:29Z- AHRS_M2 msg: 1613175089.93 epoch sec; seqNo:10342 2021-02-13T00:11:29Z- AHRS_M2 msg: 0.80 phi; 0.36 theta; -0.15 psi 2021-02-13T00:11:29Z- handleDepth()-Depth_Keller msg: 1613175089941 epoch sec; seqNo:10428; depth 11.33 2021-02-13T00:11:29Z- handleDvl() - RDI_Pathfinder msg: 1613175089.96 epoch sec; seqNo:5098 2021-02-13T00:11:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.86 , 117.74 , 78.51 2021-02-13T00:11:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.11 , 0.06 2021-02-13T00:11:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175089.98 epoch sec; seqNo:10428; 4058584.60 north; 592107.23 east 2021-02-13T00:11:30Z- AHRS_M2 msg: 1613175090.34 epoch sec; seqNo:10343 2021-02-13T00:11:30Z- AHRS_M2 msg: 0.81 phi; 0.36 theta; -0.15 psi 2021-02-13T00:11:30Z- handleDepth()-Depth_Keller msg: 1613175090347 epoch sec; seqNo:10429; depth 11.19 2021-02-13T00:11:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175090.36 epoch sec; seqNo:10429; 4058584.98 north; 592107.22 east 2021-02-13T00:11:30Z- AHRS_M2 msg: 1613175090.75 epoch sec; seqNo:10344 2021-02-13T00:11:30Z- AHRS_M2 msg: 0.82 phi; 0.35 theta; -0.14 psi 2021-02-13T00:11:30Z- handleDepth()-Depth_Keller msg: 1613175090751 epoch sec; seqNo:10430; depth 11.09 2021-02-13T00:11:30Z- handleDvl() - RDI_Pathfinder msg: 1613175090.77 epoch sec; seqNo:5099 2021-02-13T00:11:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 103.81 , 117.33 , 78.10 2021-02-13T00:11:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.07 , -0.09 2021-02-13T00:11:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175090.79 epoch sec; seqNo:10430; 4058584.99 north; 592108.44 east 2021-02-13T00:11:31Z- AHRS_M2 msg: 1613175091.15 epoch sec; seqNo:10345 2021-02-13T00:11:31Z- AHRS_M2 msg: 0.84 phi; 0.35 theta; -0.14 psi 2021-02-13T00:11:31Z- handleDepth()-Depth_Keller msg: 1613175091157 epoch sec; seqNo:10431; depth 10.96 2021-02-13T00:11:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175091.17 epoch sec; seqNo:10431; 4058585.37 north; 592108.44 east 2021-02-13T00:11:31Z- AHRS_M2 msg: 1613175091.55 epoch sec; seqNo:10346 2021-02-13T00:11:31Z- AHRS_M2 msg: 0.85 phi; 0.34 theta; -0.14 psi 2021-02-13T00:11:31Z- handleDepth()-Depth_Keller msg: 1613175091557 epoch sec; seqNo:10432; depth 10.84 2021-02-13T00:11:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175091.57 epoch sec; seqNo:10432; 4058585.37 north; 592108.44 east 2021-02-13T00:11:31Z- AHRS_M2 msg: 1613175091.96 epoch sec; seqNo:10347 2021-02-13T00:11:31Z- AHRS_M2 msg: 0.85 phi; 0.33 theta; -0.14 psi 2021-02-13T00:11:31Z- handleDepth()-Depth_Keller msg: 1613175091970 epoch sec; seqNo:10433; depth 10.71 2021-02-13T00:11:31Z- handleDvl() - RDI_Pathfinder msg: 1613175091.98 epoch sec; seqNo:5100 2021-02-13T00:11:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.84 , 101.37 , 118.96 , 78.37 2021-02-13T00:11:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , -0.17 2021-02-13T00:11:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175091.99 epoch sec; seqNo:10433; 4058585.75 north; 592108.44 east 2021-02-13T00:11:32Z- AHRS_M2 msg: 1613175092.36 epoch sec; seqNo:10348 2021-02-13T00:11:32Z- AHRS_M2 msg: 0.86 phi; 0.32 theta; -0.14 psi 2021-02-13T00:11:32Z- handleDepth()-Depth_Keller msg: 1613175092366 epoch sec; seqNo:10434; depth 10.58 2021-02-13T00:11:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175092.38 epoch sec; seqNo:10434; 4058585.75 north; 592108.44 east 2021-02-13T00:11:32Z- AHRS_M2 msg: 1613175092.76 epoch sec; seqNo:10349 2021-02-13T00:11:32Z- AHRS_M2 msg: 0.86 phi; 0.31 theta; -0.14 psi 2021-02-13T00:11:32Z- handleDepth()-Depth_Keller msg: 1613175092773 epoch sec; seqNo:10435; depth 10.45 2021-02-13T00:11:32Z- handleDvl() - RDI_Pathfinder msg: 1613175092.78 epoch sec; seqNo:5101 2021-02-13T00:11:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 101.37 , 117.60 , 0.00 2021-02-13T00:11:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.19 , -0.07 2021-02-13T00:11:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175092.79 epoch sec; seqNo:10435; 4058586.13 north; 592108.43 east 2021-02-13T00:11:33Z- AHRS_M2 msg: 1613175093.17 epoch sec; seqNo:10350 2021-02-13T00:11:33Z- AHRS_M2 msg: 0.85 phi; 0.31 theta; -0.15 psi 2021-02-13T00:11:33Z- handleDepth()-Depth_Keller msg: 1613175093174 epoch sec; seqNo:10436; depth 10.33 2021-02-13T00:11:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175093.18 epoch sec; seqNo:10436; 4058586.52 north; 592109.65 east 2021-02-13T00:11:33Z- AHRS_M2 msg: 1613175093.57 epoch sec; seqNo:10351 2021-02-13T00:11:33Z- AHRS_M2 msg: 0.84 phi; 0.30 theta; -0.15 psi 2021-02-13T00:11:33Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-13T00:11:33Z- waiting for 1613175091.34, time is 1613175093.57 2021-02-13T00:11:33Z- LcmTrn::updateTrn() >>>> pose time 1613175093.57, meas time 1613175092.78 <<<< 2021-02-13T00:11:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175092.78 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:11:33Z- TerrainNav::motionUpdate() - currEstimate = (4058587, 592109.65, 10.33) 2021-02-13T00:11:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:11:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:11:33Z- TNavBF::Projecting Measurements 2021-02-13T00:11:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:11:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:11:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:11:33Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:11:33Z- TNavFilter::3 good beams. 2021-02-13T00:11:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:11:33Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:11:33Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:11:33Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:11:33Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:11:33Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:11:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:11:33Z- TNavBF::mmse selected from filter -1 2021-02-13T00:11:33Z- TerrainMapOctree: (vehN, vehE ) = (4058586.11, 592109.10). 2021-02-13T00:11:33Z- TerrainMapOctree: Min Distance = 3417.56. 2021-02-13T00:11:33Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:11:33Z- There is no specific good beam that all particles have in common. 2021-02-13T00:11:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:11:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.42 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 11.97 2021-02-13T00:11:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:11:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 16.44 2021-02-13T00:11:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:11:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 22.13 2021-02-13T00:11:33Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:11:33Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:11:33Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:11:33Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:11:33Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:11:33Z- Filter 0 No beams used in subcloud update 2021-02-13T00:11:33Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:11:33Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:11:33Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 715.1 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:11:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175092.78 sec. 2021-02-13T00:11:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:11:33Z- TNavBF::mmse selected from filter -1 2021-02-13T00:11:33Z- variances([0], [2], and psi_berg) = 715.100454 833.006553 0.000000 2021-02-13T00:11:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:11:33Z- LcmTrn::publishEstimates() - MLE : 4058585.02 592068.15 10.33 2021-02-13T00:11:33Z- LcmTrn::publishEstimates() - MMSE : 4058586.52 592109.65 10.33 2021-02-13T00:11:33Z- LcmTrn::publishEstimates() - COVAR: 715.10 833.01 1.18 0.00 2021-02-13T00:11:33Z- handleDepth()-Depth_Keller msg: 1613175093586 epoch sec; seqNo:10437; depth 10.23 2021-02-13T00:11:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175093.59 epoch sec; seqNo:10437; 4058586.52 north; 592109.65 east 2021-02-13T00:11:33Z- AHRS_M2 msg: 1613175093.98 epoch sec; seqNo:10352 2021-02-13T00:11:33Z- AHRS_M2 msg: 0.83 phi; 0.30 theta; -0.15 psi 2021-02-13T00:11:33Z- handleDepth()-Depth_Keller msg: 1613175093980 epoch sec; seqNo:10438; depth 10.11 2021-02-13T00:11:33Z- handleDvl() - RDI_Pathfinder msg: 1613175093.99 epoch sec; seqNo:5102 2021-02-13T00:11:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 102.45 , 115.84 , 80.13 2021-02-13T00:11:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.20 , 0.20 2021-02-13T00:11:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175094.01 epoch sec; seqNo:10438; 4058586.90 north; 592109.64 east 2021-02-13T00:11:34Z- AHRS_M2 msg: 1613175094.38 epoch sec; seqNo:10353 2021-02-13T00:11:34Z- AHRS_M2 msg: 0.82 phi; 0.31 theta; -0.15 psi 2021-02-13T00:11:34Z- handleDepth()-Depth_Keller msg: 1613175094384 epoch sec; seqNo:10439; depth 9.96 2021-02-13T00:11:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175094.39 epoch sec; seqNo:10439; 4058587.28 north; 592109.64 east 2021-02-13T00:11:34Z- AHRS_M2 msg: 1613175094.79 epoch sec; seqNo:10354 2021-02-13T00:11:34Z- AHRS_M2 msg: 0.82 phi; 0.32 theta; -0.15 psi 2021-02-13T00:11:34Z- handleDepth()-Depth_Keller msg: 1613175094797 epoch sec; seqNo:10440; depth 9.88 2021-02-13T00:11:34Z- handleDvl() - RDI_Pathfinder msg: 1613175094.81 epoch sec; seqNo:5103 2021-02-13T00:11:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.72 , 110.84 , 81.08 2021-02-13T00:11:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.00 , 0.38 2021-02-13T00:11:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175094.82 epoch sec; seqNo:10440; 4058587.28 north; 592109.64 east 2021-02-13T00:11:35Z- AHRS_M2 msg: 1613175095.19 epoch sec; seqNo:10355 2021-02-13T00:11:35Z- AHRS_M2 msg: 0.81 phi; 0.32 theta; -0.14 psi 2021-02-13T00:11:35Z- handleDepth()-Depth_Keller msg: 1613175095193 epoch sec; seqNo:10441; depth 9.76 2021-02-13T00:11:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175095.21 epoch sec; seqNo:10441; 4058587.67 north; 592110.86 east 2021-02-13T00:11:35Z- AHRS_M2 msg: 1613175095.60 epoch sec; seqNo:10356 2021-02-13T00:11:35Z- AHRS_M2 msg: 0.80 phi; 0.33 theta; -0.14 psi 2021-02-13T00:11:35Z- handleDepth()-Depth_Keller msg: 1613175095603 epoch sec; seqNo:10442; depth 9.65 2021-02-13T00:11:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:35Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:35Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:35Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:35Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:35Z- handleDvl() - RDI_Pathfinder msg: 1613175095.61 epoch sec; seqNo:5104 2021-02-13T00:11:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.00 , 0.38 2021-02-13T00:11:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175095.62 epoch sec; seqNo:10442; 4058588.05 north; 592110.85 east 2021-02-13T00:11:35Z- AHRS_M2 msg: 1613175095.99 epoch sec; seqNo:10357 2021-02-13T00:11:35Z- AHRS_M2 msg: 0.80 phi; 0.34 theta; -0.14 psi 2021-02-13T00:11:36Z- handleDepth()-Depth_Keller msg: 1613175096001 epoch sec; seqNo:10443; depth 9.56 2021-02-13T00:11:36Z- handleDvl() - RDI_Pathfinder msg: 1613175096.01 epoch sec; seqNo:5105 2021-02-13T00:11:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.18 , 101.23 , 117.47 , 80.94 2021-02-13T00:11:36Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , 0.20 , 0.25 2021-02-13T00:11:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175096.02 epoch sec; seqNo:10443; 4058588.05 north; 592110.85 east 2021-02-13T00:11:36Z- AHRS_M2 msg: 1613175096.40 epoch sec; seqNo:10358 2021-02-13T00:11:36Z- AHRS_M2 msg: 0.81 phi; 0.35 theta; -0.14 psi 2021-02-13T00:11:36Z- handleDepth()-Depth_Keller msg: 1613175096406 epoch sec; seqNo:10444; depth 9.44 2021-02-13T00:11:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175096.42 epoch sec; seqNo:10444; 4058588.43 north; 592110.85 east 2021-02-13T00:11:36Z- AHRS_M2 msg: 1613175096.80 epoch sec; seqNo:10359 2021-02-13T00:11:36Z- AHRS_M2 msg: 0.81 phi; 0.35 theta; -0.14 psi 2021-02-13T00:11:36Z- handleDepth()-Depth_Keller msg: 1613175096808 epoch sec; seqNo:10445; depth 9.34 2021-02-13T00:11:36Z- handleDvl() - RDI_Pathfinder msg: 1613175096.83 epoch sec; seqNo:5106 2021-02-13T00:11:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.70 , 102.52 , 118.75 , 77.67 2021-02-13T00:11:36Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.29 , -0.05 2021-02-13T00:11:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175096.84 epoch sec; seqNo:10445; 4058588.82 north; 592112.07 east 2021-02-13T00:11:37Z- AHRS_M2 msg: 1613175097.21 epoch sec; seqNo:10360 2021-02-13T00:11:37Z- AHRS_M2 msg: 0.82 phi; 0.35 theta; -0.13 psi 2021-02-13T00:11:37Z- handleDepth()-Depth_Keller msg: 1613175097217 epoch sec; seqNo:10446; depth 9.23 2021-02-13T00:11:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175097.23 epoch sec; seqNo:10446; 4058588.82 north; 592112.07 east 2021-02-13T00:11:37Z- AHRS_M2 msg: 1613175097.62 epoch sec; seqNo:10361 2021-02-13T00:11:37Z- AHRS_M2 msg: 0.83 phi; 0.34 theta; -0.14 psi 2021-02-13T00:11:37Z- handleDepth()-Depth_Keller msg: 1613175097619 epoch sec; seqNo:10447; depth 9.13 2021-02-13T00:11:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175097.63 epoch sec; seqNo:10447; 4058588.82 north; 592112.07 east 2021-02-13T00:11:38Z- AHRS_M2 msg: 1613175098.02 epoch sec; seqNo:10362 2021-02-13T00:11:38Z- AHRS_M2 msg: 0.83 phi; 0.34 theta; -0.14 psi 2021-02-13T00:11:38Z- handleDepth()-Depth_Keller msg: 1613175098026 epoch sec; seqNo:10448; depth 9.02 2021-02-13T00:11:38Z- handleDvl() - RDI_Pathfinder msg: 1613175098.04 epoch sec; seqNo:5107 2021-02-13T00:11:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:38Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:11:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175098.05 epoch sec; seqNo:10448; 4058589.21 north; 592113.28 east 2021-02-13T00:11:38Z- AHRS_M2 msg: 1613175098.42 epoch sec; seqNo:10363 2021-02-13T00:11:38Z- AHRS_M2 msg: 0.84 phi; 0.33 theta; -0.16 psi 2021-02-13T00:11:38Z- handleDepth()-Depth_Keller msg: 1613175098424 epoch sec; seqNo:10449; depth 8.88 2021-02-13T00:11:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175098.44 epoch sec; seqNo:10449; 4058589.21 north; 592113.28 east 2021-02-13T00:11:38Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-13T00:11:38Z- waiting for 1613175096.57, time is 1613175098.44 2021-02-13T00:11:38Z- LcmTrn::updateTrn() >>>> pose time 1613175098.44, meas time 1613175098.04 <<<< 2021-02-13T00:11:38Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175098.04, ping # 0. 2021-02-13T00:11:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175098.04 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:11:38Z- TerrainNav::motionUpdate() - currEstimate = (4058589, 592113.28, 8.88) 2021-02-13T00:11:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:11:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:11:38Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:11:38Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:11:38Z- TNavBF::Projecting Measurements 2021-02-13T00:11:38Z- TNavFilter::Measurement from time = 1613175098.04 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:11:38Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:11:38Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:11:38Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:11:38Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:11:38Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:11:38Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:11:38Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:11:38Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 715.0 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T00:11:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:11:38Z- TNavBF::mmse selected from filter -1 2021-02-13T00:11:38Z- variances([0], [2], and psi_berg) = 715.016026 833.006501 0.000000 2021-02-13T00:11:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:11:38Z- LcmTrn::publishEstimates() - MLE : 4058587.71 592071.78 8.88 2021-02-13T00:11:38Z- LcmTrn::publishEstimates() - MMSE : 4058589.21 592113.28 8.88 2021-02-13T00:11:38Z- LcmTrn::publishEstimates() - COVAR: 715.02 833.01 1.41 0.00 2021-02-13T00:11:38Z- AHRS_M2 msg: 1613175098.82 epoch sec; seqNo:10364 2021-02-13T00:11:38Z- AHRS_M2 msg: 0.84 phi; 0.33 theta; -0.17 psi 2021-02-13T00:11:38Z- handleDepth()-Depth_Keller msg: 1613175098828 epoch sec; seqNo:10450; depth 8.77 2021-02-13T00:11:38Z- handleDvl() - RDI_Pathfinder msg: 1613175098.84 epoch sec; seqNo:5108 2021-02-13T00:11:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 104.68 , 121.05 , 80.88 2021-02-13T00:11:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , 0.01 , -0.36 2021-02-13T00:11:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175098.85 epoch sec; seqNo:10450; 4058589.59 north; 592113.28 east 2021-02-13T00:11:39Z- AHRS_M2 msg: 1613175099.23 epoch sec; seqNo:10365 2021-02-13T00:11:39Z- AHRS_M2 msg: 0.84 phi; 0.33 theta; -0.18 psi 2021-02-13T00:11:39Z- handleDepth()-Depth_Keller msg: 1613175099233 epoch sec; seqNo:10451; depth 8.64 2021-02-13T00:11:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:39Z- handleDvl() - RDI_Pathfinder msg: 1613175099.23 epoch sec; seqNo:5109 2021-02-13T00:11:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , 0.01 , -0.36 2021-02-13T00:11:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175099.25 epoch sec; seqNo:10451; 4058589.59 north; 592113.28 east 2021-02-13T00:11:39Z- AHRS_M2 msg: 1613175099.63 epoch sec; seqNo:10366 2021-02-13T00:11:39Z- AHRS_M2 msg: 0.84 phi; 0.32 theta; -0.18 psi 2021-02-13T00:11:39Z- handleDepth()-Depth_Keller msg: 1613175099645 epoch sec; seqNo:10452; depth 8.51 2021-02-13T00:11:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175099.65 epoch sec; seqNo:10452; 4058589.97 north; 592113.27 east 2021-02-13T00:11:40Z- AHRS_M2 msg: 1613175100.04 epoch sec; seqNo:10367 2021-02-13T00:11:40Z- AHRS_M2 msg: 0.84 phi; 0.32 theta; -0.18 psi 2021-02-13T00:11:40Z- handleDepth()-Depth_Keller msg: 1613175100040 epoch sec; seqNo:10453; depth 8.38 2021-02-13T00:11:40Z- handleDvl() - RDI_Pathfinder msg: 1613175100.05 epoch sec; seqNo:5110 2021-02-13T00:11:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 106.44 , 119.97 , 82.09 2021-02-13T00:11:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.23 , -0.13 2021-02-13T00:11:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175100.07 epoch sec; seqNo:10453; 4058589.97 north; 592113.27 east 2021-02-13T00:11:40Z- AHRS_M2 msg: 1613175100.44 epoch sec; seqNo:10368 2021-02-13T00:11:40Z- AHRS_M2 msg: 0.83 phi; 0.33 theta; -0.17 psi 2021-02-13T00:11:40Z- handleDepth()-Depth_Keller msg: 1613175100449 epoch sec; seqNo:10454; depth 8.25 2021-02-13T00:11:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:40Z- handleDvl() - RDI_Pathfinder msg: 1613175100.45 epoch sec; seqNo:5111 2021-02-13T00:11:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.23 , -0.13 2021-02-13T00:11:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175100.46 epoch sec; seqNo:10454; 4058590.35 north; 592113.27 east 2021-02-13T00:11:40Z- AHRS_M2 msg: 1613175100.85 epoch sec; seqNo:10369 2021-02-13T00:11:40Z- AHRS_M2 msg: 0.82 phi; 0.33 theta; -0.16 psi 2021-02-13T00:11:40Z- handleDepth()-Depth_Keller msg: 1613175100850 epoch sec; seqNo:10455; depth 8.14 2021-02-13T00:11:40Z- handleDvl() - RDI_Pathfinder msg: 1613175100.86 epoch sec; seqNo:5112 2021-02-13T00:11:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 106.92 , 120.31 , 81.62 2021-02-13T00:11:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.17 , 0.17 2021-02-13T00:11:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175100.87 epoch sec; seqNo:10455; 4058590.74 north; 592114.49 east 2021-02-13T00:11:41Z- AHRS_M2 msg: 1613175101.25 epoch sec; seqNo:10370 2021-02-13T00:11:41Z- AHRS_M2 msg: 0.81 phi; 0.33 theta; -0.16 psi 2021-02-13T00:11:41Z- handleDepth()-Depth_Keller msg: 1613175101263 epoch sec; seqNo:10456; depth 8.01 2021-02-13T00:11:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175101.27 epoch sec; seqNo:10456; 4058590.74 north; 592114.49 east 2021-02-13T00:11:41Z- AHRS_M2 msg: 1613175101.65 epoch sec; seqNo:10371 2021-02-13T00:11:41Z- AHRS_M2 msg: 0.81 phi; 0.33 theta; -0.16 psi 2021-02-13T00:11:41Z- handleDepth()-Depth_Keller msg: 1613175101656 epoch sec; seqNo:10457; depth 7.91 2021-02-13T00:11:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:41Z- handleDvl() - RDI_Pathfinder msg: 1613175101.66 epoch sec; seqNo:5113 2021-02-13T00:11:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.17 , 0.17 2021-02-13T00:11:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175101.67 epoch sec; seqNo:10457; 4058591.12 north; 592114.48 east 2021-02-13T00:11:42Z- AHRS_M2 msg: 1613175102.06 epoch sec; seqNo:10372 2021-02-13T00:11:42Z- AHRS_M2 msg: 0.80 phi; 0.34 theta; -0.15 psi 2021-02-13T00:11:42Z- handleDepth()-Depth_Keller msg: 1613175102061 epoch sec; seqNo:10458; depth 7.77 2021-02-13T00:11:42Z- handleDvl() - RDI_Pathfinder msg: 1613175102.07 epoch sec; seqNo:5114 2021-02-13T00:11:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 105.22 , 120.11 , 80.88 2021-02-13T00:11:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.01 , 0.25 2021-02-13T00:11:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175102.08 epoch sec; seqNo:10458; 4058591.12 north; 592114.48 east 2021-02-13T00:11:42Z- AHRS_M2 msg: 1613175102.46 epoch sec; seqNo:10373 2021-02-13T00:11:42Z- AHRS_M2 msg: 0.80 phi; 0.34 theta; -0.15 psi 2021-02-13T00:11:42Z- handleDepth()-Depth_Keller msg: 1613175102469 epoch sec; seqNo:10459; depth 7.64 2021-02-13T00:11:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175102.48 epoch sec; seqNo:10459; 4058591.50 north; 592114.48 east 2021-02-13T00:11:42Z- AHRS_M2 msg: 1613175102.86 epoch sec; seqNo:10374 2021-02-13T00:11:42Z- AHRS_M2 msg: 0.80 phi; 0.34 theta; -0.16 psi 2021-02-13T00:11:42Z- handleDepth()-Depth_Keller msg: 1613175102868 epoch sec; seqNo:10460; depth 7.52 2021-02-13T00:11:42Z- handleDvl() - RDI_Pathfinder msg: 1613175102.89 epoch sec; seqNo:5115 2021-02-13T00:11:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 105.09 , 118.62 , 82.09 2021-02-13T00:11:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , 0.18 2021-02-13T00:11:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175102.90 epoch sec; seqNo:10460; 4058591.89 north; 592115.70 east 2021-02-13T00:11:43Z- AHRS_M2 msg: 1613175103.27 epoch sec; seqNo:10375 2021-02-13T00:11:43Z- AHRS_M2 msg: 0.79 phi; 0.35 theta; -0.17 psi 2021-02-13T00:11:43Z- handleDepth()-Depth_Keller msg: 1613175103277 epoch sec; seqNo:10461; depth 7.38 2021-02-13T00:11:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175103.29 epoch sec; seqNo:10461; 4058591.89 north; 592115.70 east 2021-02-13T00:11:43Z- AHRS_M2 msg: 1613175103.67 epoch sec; seqNo:10376 2021-02-13T00:11:43Z- AHRS_M2 msg: 0.79 phi; 0.35 theta; -0.18 psi 2021-02-13T00:11:43Z- LcmTrn::handleMessages() - handling 45 messages took 0.65 ms and 132 calls... 2021-02-13T00:11:43Z- waiting for 1613175101.44, time is 1613175103.67 2021-02-13T00:11:43Z- LcmTrn::updateTrn() >>>> pose time 1613175103.67, meas time 1613175102.89 <<<< 2021-02-13T00:11:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175102.89 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:11:43Z- TerrainNav::motionUpdate() - currEstimate = (4058592, 592115.70, 7.38) 2021-02-13T00:11:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:11:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:11:43Z- TNavBF::Projecting Measurements 2021-02-13T00:11:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:11:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:11:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:11:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:11:43Z- TNavFilter::4 good beams. 2021-02-13T00:11:43Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:11:43Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:11:43Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:11:43Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:11:43Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:11:43Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:11:43Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:11:43Z- TNavBF::mmse selected from filter -1 2021-02-13T00:11:43Z- TerrainMapOctree: (vehN, vehE ) = (4058591.49, 592115.33). 2021-02-13T00:11:43Z- TerrainMapOctree: Min Distance = 3409.47. 2021-02-13T00:11:43Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:11:43Z- There is no specific good beam that all particles have in common. 2021-02-13T00:11:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:11:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.57 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 15.05 2021-02-13T00:11:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:11:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 17.67 2021-02-13T00:11:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:11:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 22.51 2021-02-13T00:11:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:11:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 10.78 2021-02-13T00:11:43Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:11:43Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:11:43Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:11:43Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:11:43Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:11:43Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:11:43Z- Filter 0 No beams used in subcloud update 2021-02-13T00:11:43Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:11:43Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:11:43Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 715.0 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:11:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175102.89 sec. 2021-02-13T00:11:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:11:43Z- TNavBF::mmse selected from filter -1 2021-02-13T00:11:43Z- variances([0], [2], and psi_berg) = 714.936780 833.006452 0.000000 2021-02-13T00:11:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:11:43Z- LcmTrn::publishEstimates() - MLE : 4058590.39 592074.20 7.38 2021-02-13T00:11:43Z- LcmTrn::publishEstimates() - MMSE : 4058591.89 592115.70 7.38 2021-02-13T00:11:43Z- LcmTrn::publishEstimates() - COVAR: 714.94 833.01 1.18 0.00 2021-02-13T00:11:43Z- handleDepth()-Depth_Keller msg: 1613175103677 epoch sec; seqNo:10462; depth 7.27 2021-02-13T00:11:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175103.69 epoch sec; seqNo:10462; 4058592.27 north; 592115.69 east 2021-02-13T00:11:44Z- AHRS_M2 msg: 1613175104.08 epoch sec; seqNo:10377 2021-02-13T00:11:44Z- AHRS_M2 msg: 0.79 phi; 0.36 theta; -0.18 psi 2021-02-13T00:11:44Z- handleDepth()-Depth_Keller msg: 1613175104087 epoch sec; seqNo:10463; depth 7.16 2021-02-13T00:11:44Z- handleDvl() - RDI_Pathfinder msg: 1613175104.10 epoch sec; seqNo:5116 2021-02-13T00:11:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.21 , 107.79 , 121.32 , 82.09 2021-02-13T00:11:44Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.09 , 0.09 2021-02-13T00:11:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175104.11 epoch sec; seqNo:10463; 4058592.65 north; 592115.69 east 2021-02-13T00:11:44Z- AHRS_M2 msg: 1613175104.48 epoch sec; seqNo:10378 2021-02-13T00:11:44Z- AHRS_M2 msg: 0.80 phi; 0.36 theta; -0.18 psi 2021-02-13T00:11:44Z- handleDepth()-Depth_Keller msg: 1613175104489 epoch sec; seqNo:10464; depth 7.04 2021-02-13T00:11:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175104.51 epoch sec; seqNo:10464; 4058592.66 north; 592116.91 east 2021-02-13T00:11:44Z- AHRS_M2 msg: 1613175104.89 epoch sec; seqNo:10379 2021-02-13T00:11:44Z- AHRS_M2 msg: 0.81 phi; 0.36 theta; -0.18 psi 2021-02-13T00:11:44Z- handleDepth()-Depth_Keller msg: 1613175104894 epoch sec; seqNo:10465; depth 6.91 2021-02-13T00:11:44Z- handleDvl() - RDI_Pathfinder msg: 1613175104.90 epoch sec; seqNo:5117 2021-02-13T00:11:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 110.57 , 122.74 , 141.68 2021-02-13T00:11:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.06 , -0.08 2021-02-13T00:11:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175104.93 epoch sec; seqNo:10465; 4058593.04 north; 592116.90 east 2021-02-13T00:11:45Z- AHRS_M2 msg: 1613175105.29 epoch sec; seqNo:10380 2021-02-13T00:11:45Z- AHRS_M2 msg: 0.82 phi; 0.35 theta; -0.17 psi 2021-02-13T00:11:45Z- handleDepth()-Depth_Keller msg: 1613175105293 epoch sec; seqNo:10466; depth 6.78 2021-02-13T00:11:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:45Z- handleDvl() - RDI_Pathfinder msg: 1613175105.30 epoch sec; seqNo:5118 2021-02-13T00:11:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.06 , -0.08 2021-02-13T00:11:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175105.31 epoch sec; seqNo:10466; 4058593.04 north; 592116.90 east 2021-02-13T00:11:45Z- AHRS_M2 msg: 1613175105.70 epoch sec; seqNo:10381 2021-02-13T00:11:45Z- AHRS_M2 msg: 0.83 phi; 0.34 theta; -0.16 psi 2021-02-13T00:11:45Z- handleDepth()-Depth_Keller msg: 1613175105701 epoch sec; seqNo:10467; depth 6.70 2021-02-13T00:11:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175105.72 epoch sec; seqNo:10467; 4058593.42 north; 592116.90 east 2021-02-13T00:11:46Z- AHRS_M2 msg: 1613175106.10 epoch sec; seqNo:10382 2021-02-13T00:11:46Z- AHRS_M2 msg: 0.83 phi; 0.33 theta; -0.16 psi 2021-02-13T00:11:46Z- handleDepth()-Depth_Keller msg: 1613175106105 epoch sec; seqNo:10468; depth 6.58 2021-02-13T00:11:46Z- handleDvl() - RDI_Pathfinder msg: 1613175106.13 epoch sec; seqNo:5119 2021-02-13T00:11:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 109.15 , 125.52 , 79.72 2021-02-13T00:11:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.06 , -0.11 2021-02-13T00:11:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175106.14 epoch sec; seqNo:10468; 4058593.42 north; 592116.90 east 2021-02-13T00:11:46Z- AHRS_M2 msg: 1613175106.50 epoch sec; seqNo:10383 2021-02-13T00:11:46Z- AHRS_M2 msg: 0.83 phi; 0.33 theta; -0.16 psi 2021-02-13T00:11:46Z- handleDepth()-Depth_Keller msg: 1613175106504 epoch sec; seqNo:10469; depth 6.45 2021-02-13T00:11:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:46Z- handleDvl() - RDI_Pathfinder msg: 1613175106.51 epoch sec; seqNo:5120 2021-02-13T00:11:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.06 , -0.11 2021-02-13T00:11:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175106.52 epoch sec; seqNo:10469; 4058593.81 north; 592118.12 east 2021-02-13T00:11:46Z- AHRS_M2 msg: 1613175106.91 epoch sec; seqNo:10384 2021-02-13T00:11:46Z- AHRS_M2 msg: 0.82 phi; 0.33 theta; -0.16 psi 2021-02-13T00:11:46Z- handleDepth()-Depth_Keller msg: 1613175106910 epoch sec; seqNo:10470; depth 6.33 2021-02-13T00:11:46Z- handleDvl() - RDI_Pathfinder msg: 1613175106.92 epoch sec; seqNo:5121 2021-02-13T00:11:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 106.78 , 121.66 , 82.97 2021-02-13T00:11:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.01 , -0.05 2021-02-13T00:11:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175106.93 epoch sec; seqNo:10470; 4058594.19 north; 592118.11 east 2021-02-13T00:11:47Z- AHRS_M2 msg: 1613175107.31 epoch sec; seqNo:10385 2021-02-13T00:11:47Z- AHRS_M2 msg: 0.82 phi; 0.33 theta; -0.16 psi 2021-02-13T00:11:47Z- handleDepth()-Depth_Keller msg: 1613175107312 epoch sec; seqNo:10471; depth 6.21 2021-02-13T00:11:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175107.33 epoch sec; seqNo:10471; 4058594.19 north; 592118.11 east 2021-02-13T00:11:47Z- AHRS_M2 msg: 1613175107.71 epoch sec; seqNo:10386 2021-02-13T00:11:47Z- AHRS_M2 msg: 0.82 phi; 0.34 theta; -0.15 psi 2021-02-13T00:11:47Z- handleDepth()-Depth_Keller msg: 1613175107721 epoch sec; seqNo:10472; depth 6.08 2021-02-13T00:11:47Z- handleDvl() - RDI_Pathfinder msg: 1613175107.73 epoch sec; seqNo:5122 2021-02-13T00:11:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 106.85 , 123.22 , 0.00 2021-02-13T00:11:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , 0.02 2021-02-13T00:11:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175107.76 epoch sec; seqNo:10472; 4058594.57 north; 592118.11 east 2021-02-13T00:11:48Z- AHRS_M2 msg: 1613175108.12 epoch sec; seqNo:10387 2021-02-13T00:11:48Z- AHRS_M2 msg: 0.82 phi; 0.34 theta; -0.15 psi 2021-02-13T00:11:48Z- handleDepth()-Depth_Keller msg: 1613175108129 epoch sec; seqNo:10473; depth 5.96 2021-02-13T00:11:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175108.14 epoch sec; seqNo:10473; 4058594.57 north; 592118.11 east 2021-02-13T00:11:48Z- AHRS_M2 msg: 1613175108.53 epoch sec; seqNo:10388 2021-02-13T00:11:48Z- AHRS_M2 msg: 0.83 phi; 0.34 theta; -0.15 psi 2021-02-13T00:11:48Z- handleDepth()-Depth_Keller msg: 1613175108529 epoch sec; seqNo:10474; depth 5.85 2021-02-13T00:11:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175108.54 epoch sec; seqNo:10474; 4058594.96 north; 592119.33 east 2021-02-13T00:11:48Z- LcmTrn::handleMessages() - handling 45 messages took 0.63 ms and 133 calls... 2021-02-13T00:11:48Z- waiting for 1613175106.67, time is 1613175108.54 2021-02-13T00:11:48Z- LcmTrn::updateTrn() >>>> pose time 1613175108.54, meas time 1613175107.73 <<<< 2021-02-13T00:11:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175107.73 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:11:48Z- TerrainNav::motionUpdate() - currEstimate = (4058595, 592119.33, 5.85) 2021-02-13T00:11:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:11:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:11:48Z- TNavBF::Projecting Measurements 2021-02-13T00:11:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:11:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:11:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:11:48Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:11:48Z- TNavFilter::3 good beams. 2021-02-13T00:11:48Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:11:48Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:11:48Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:11:48Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:11:48Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:11:48Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:11:48Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:11:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:11:48Z- TerrainMapOctree: (vehN, vehE ) = (4058594.45, 592118.73). 2021-02-13T00:11:48Z- TerrainMapOctree: Min Distance = 3405.04. 2021-02-13T00:11:48Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:11:48Z- There is no specific good beam that all particles have in common. 2021-02-13T00:11:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:11:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:11:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:11:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 18.27 2021-02-13T00:11:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:11:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 24.29 2021-02-13T00:11:48Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:11:48Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:11:48Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:11:48Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:11:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:11:48Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:11:48Z- Filter 0 No beams used in subcloud update 2021-02-13T00:11:48Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:11:48Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:11:48Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 714.9 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:11:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175107.73 sec. 2021-02-13T00:11:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:11:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:11:48Z- variances([0], [2], and psi_berg) = 714.845103 833.006396 0.000000 2021-02-13T00:11:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:11:48Z- LcmTrn::publishEstimates() - MLE : 4058593.46 592077.83 5.85 2021-02-13T00:11:48Z- LcmTrn::publishEstimates() - MMSE : 4058594.96 592119.33 5.85 2021-02-13T00:11:48Z- LcmTrn::publishEstimates() - COVAR: 714.85 833.01 1.19 0.00 2021-02-13T00:11:48Z- AHRS_M2 msg: 1613175108.92 epoch sec; seqNo:10389 2021-02-13T00:11:48Z- AHRS_M2 msg: 0.84 phi; 0.33 theta; -0.15 psi 2021-02-13T00:11:48Z- handleDepth()-Depth_Keller msg: 1613175108928 epoch sec; seqNo:10475; depth 5.73 2021-02-13T00:11:48Z- handleDvl() - RDI_Pathfinder msg: 1613175108.95 epoch sec; seqNo:5123 2021-02-13T00:11:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.31 , 109.21 , 115.17 , 83.92 2021-02-13T00:11:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.05 , -0.11 2021-02-13T00:11:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175108.96 epoch sec; seqNo:10475; 4058594.96 north; 592119.33 east 2021-02-13T00:11:49Z- AHRS_M2 msg: 1613175109.33 epoch sec; seqNo:10390 2021-02-13T00:11:49Z- AHRS_M2 msg: 0.85 phi; 0.32 theta; -0.16 psi 2021-02-13T00:11:49Z- handleDepth()-Depth_Keller msg: 1613175109337 epoch sec; seqNo:10476; depth 5.61 2021-02-13T00:11:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175109.35 epoch sec; seqNo:10476; 4058595.34 north; 592119.32 east 2021-02-13T00:11:49Z- AHRS_M2 msg: 1613175109.74 epoch sec; seqNo:10391 2021-02-13T00:11:49Z- AHRS_M2 msg: 0.86 phi; 0.29 theta; -0.16 psi 2021-02-13T00:11:49Z- handleDepth()-Depth_Keller msg: 1613175109740 epoch sec; seqNo:10477; depth 5.50 2021-02-13T00:11:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175109.76 epoch sec; seqNo:10477; 4058595.34 north; 592119.32 east 2021-02-13T00:11:50Z- AHRS_M2 msg: 1613175110.14 epoch sec; seqNo:10392 2021-02-13T00:11:50Z- AHRS_M2 msg: 0.87 phi; 0.25 theta; -0.16 psi 2021-02-13T00:11:50Z- handleDepth()-Depth_Keller msg: 1613175110147 epoch sec; seqNo:10478; depth 5.37 2021-02-13T00:11:50Z- handleDvl() - RDI_Pathfinder msg: 1613175110.16 epoch sec; seqNo:5124 2021-02-13T00:11:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.04 , 106.92 , 121.80 , 84.60 2021-02-13T00:11:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.10 , -0.17 2021-02-13T00:11:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175110.17 epoch sec; seqNo:10478; 4058595.72 north; 592119.32 east 2021-02-13T00:11:50Z- AHRS_M2 msg: 1613175110.54 epoch sec; seqNo:10393 2021-02-13T00:11:50Z- AHRS_M2 msg: 0.87 phi; 0.21 theta; -0.16 psi 2021-02-13T00:11:50Z- handleDepth()-Depth_Keller msg: 1613175110544 epoch sec; seqNo:10479; depth 5.23 2021-02-13T00:11:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175110.56 epoch sec; seqNo:10479; 4058595.72 north; 592119.32 east 2021-02-13T00:11:50Z- AHRS_M2 msg: 1613175110.94 epoch sec; seqNo:10394 2021-02-13T00:11:50Z- AHRS_M2 msg: 0.87 phi; 0.17 theta; -0.15 psi 2021-02-13T00:11:50Z- handleDepth()-Depth_Keller msg: 1613175110948 epoch sec; seqNo:10480; depth 5.12 2021-02-13T00:11:50Z- handleDvl() - RDI_Pathfinder msg: 1613175110.96 epoch sec; seqNo:5125 2021-02-13T00:11:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 106.92 , 114.36 , 0.00 2021-02-13T00:11:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.14 , 0.09 2021-02-13T00:11:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175110.98 epoch sec; seqNo:10480; 4058596.11 north; 592120.53 east 2021-02-13T00:11:51Z- AHRS_M2 msg: 1613175111.35 epoch sec; seqNo:10395 2021-02-13T00:11:51Z- AHRS_M2 msg: 0.87 phi; 0.12 theta; -0.15 psi 2021-02-13T00:11:51Z- handleDepth()-Depth_Keller msg: 1613175111352 epoch sec; seqNo:10481; depth 5.00 2021-02-13T00:11:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:51Z- handleDvl() - RDI_Pathfinder msg: 1613175111.35 epoch sec; seqNo:5126 2021-02-13T00:11:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.14 , 0.09 2021-02-13T00:11:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175111.37 epoch sec; seqNo:10481; 4058596.49 north; 592120.53 east 2021-02-13T00:11:51Z- AHRS_M2 msg: 1613175111.75 epoch sec; seqNo:10396 2021-02-13T00:11:51Z- AHRS_M2 msg: 0.86 phi; 0.08 theta; -0.15 psi 2021-02-13T00:11:51Z- handleDepth()-Depth_Keller msg: 1613175111758 epoch sec; seqNo:10482; depth 4.90 2021-02-13T00:11:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175111.77 epoch sec; seqNo:10482; 4058596.49 north; 592120.53 east 2021-02-13T00:11:52Z- AHRS_M2 msg: 1613175112.16 epoch sec; seqNo:10397 2021-02-13T00:11:52Z- AHRS_M2 msg: 0.86 phi; 0.04 theta; -0.17 psi 2021-02-13T00:11:52Z- handleDepth()-Depth_Keller msg: 1613175112162 epoch sec; seqNo:10483; depth 4.81 2021-02-13T00:11:52Z- handleDvl() - RDI_Pathfinder msg: 1613175112.18 epoch sec; seqNo:5127 2021-02-13T00:11:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.65 , 102.05 , 103.53 , 97.58 2021-02-13T00:11:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.04 , 0.29 2021-02-13T00:11:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175112.19 epoch sec; seqNo:10483; 4058596.87 north; 592120.53 east 2021-02-13T00:11:52Z- AHRS_M2 msg: 1613175112.56 epoch sec; seqNo:10398 2021-02-13T00:11:52Z- AHRS_M2 msg: 0.85 phi; 0.00 theta; -0.19 psi 2021-02-13T00:11:52Z- handleDepth()-Depth_Keller msg: 1613175112569 epoch sec; seqNo:10484; depth 4.74 2021-02-13T00:11:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:52Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:52Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:52Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:52Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:52Z- handleDvl() - RDI_Pathfinder msg: 1613175112.57 epoch sec; seqNo:5128 2021-02-13T00:11:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.04 , 0.29 2021-02-13T00:11:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175112.58 epoch sec; seqNo:10484; 4058596.87 north; 592120.53 east 2021-02-13T00:11:52Z- AHRS_M2 msg: 1613175112.96 epoch sec; seqNo:10399 2021-02-13T00:11:52Z- AHRS_M2 msg: 0.85 phi; -0.03 theta; -0.22 psi 2021-02-13T00:11:52Z- handleDepth()-Depth_Keller msg: 1613175112968 epoch sec; seqNo:10485; depth 4.68 2021-02-13T00:11:52Z- handleDvl() - RDI_Pathfinder msg: 1613175112.98 epoch sec; seqNo:5129 2021-02-13T00:11:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.78 , 105.36 , 97.24 , 97.24 2021-02-13T00:11:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.18 , 0.23 2021-02-13T00:11:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175112.99 epoch sec; seqNo:10485; 4058596.88 north; 592121.75 east 2021-02-13T00:11:53Z- AHRS_M2 msg: 1613175113.37 epoch sec; seqNo:10400 2021-02-13T00:11:53Z- AHRS_M2 msg: 0.85 phi; -0.07 theta; -0.26 psi 2021-02-13T00:11:53Z- handleDepth()-Depth_Keller msg: 1613175113374 epoch sec; seqNo:10486; depth 4.64 2021-02-13T00:11:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175113.38 epoch sec; seqNo:10486; 4058597.26 north; 592121.74 east 2021-02-13T00:11:53Z- AHRS_M2 msg: 1613175113.78 epoch sec; seqNo:10401 2021-02-13T00:11:53Z- AHRS_M2 msg: 0.85 phi; -0.09 theta; -0.27 psi 2021-02-13T00:11:53Z- handleDepth()-Depth_Keller msg: 1613175113782 epoch sec; seqNo:10487; depth 4.62 2021-02-13T00:11:53Z- handleDvl() - RDI_Pathfinder msg: 1613175113.80 epoch sec; seqNo:5130 2021-02-13T00:11:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 110.84 , 97.31 , 101.37 2021-02-13T00:11:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.21 , 0.08 2021-02-13T00:11:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175113.81 epoch sec; seqNo:10487; 4058597.26 north; 592121.74 east 2021-02-13T00:11:53Z- LcmTrn::handleMessages() - handling 47 messages took 0.70 ms and 133 calls... 2021-02-13T00:11:53Z- waiting for 1613175111.54, time is 1613175113.81 2021-02-13T00:11:53Z- LcmTrn::updateTrn() equating pose 1613175113.81 and meas times 1613175113.81 2021-02-13T00:11:53Z- LcmTrn::updateTrn() >>>> pose time 1613175113.81, meas time 1613175113.81 <<<< 2021-02-13T00:11:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175113.81 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:11:53Z- TerrainNav::motionUpdate() - currEstimate = (4058597, 592121.74, 4.62) 2021-02-13T00:11:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:11:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:11:53Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175113.81 sec.; No relevant navigation data available 2021-02-13T00:11:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:11:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:11:53Z- variances([0], [2], and psi_berg) = 714.786978 833.006360 0.000000 2021-02-13T00:11:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:11:53Z- LcmTrn::publishEstimates() - MLE : 4058595.76 592080.24 4.62 2021-02-13T00:11:53Z- LcmTrn::publishEstimates() - MMSE : 4058597.26 592121.74 4.62 2021-02-13T00:11:53Z- LcmTrn::publishEstimates() - COVAR: 714.79 833.01 1.36 0.00 2021-02-13T00:11:54Z- AHRS_M2 msg: 1613175114.17 epoch sec; seqNo:10402 2021-02-13T00:11:54Z- AHRS_M2 msg: 0.85 phi; -0.11 theta; -0.27 psi 2021-02-13T00:11:54Z- handleDepth()-Depth_Keller msg: 1613175114185 epoch sec; seqNo:10488; depth 4.63 2021-02-13T00:11:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175114.20 epoch sec; seqNo:10488; 4058597.64 north; 592121.74 east 2021-02-13T00:11:54Z- AHRS_M2 msg: 1613175114.58 epoch sec; seqNo:10403 2021-02-13T00:11:54Z- AHRS_M2 msg: 0.86 phi; -0.13 theta; -0.26 psi 2021-02-13T00:11:54Z- handleDepth()-Depth_Keller msg: 1613175114586 epoch sec; seqNo:10489; depth 4.66 2021-02-13T00:11:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175114.59 epoch sec; seqNo:10489; 4058597.65 north; 592122.96 east 2021-02-13T00:11:54Z- AHRS_M2 msg: 1613175114.98 epoch sec; seqNo:10404 2021-02-13T00:11:54Z- AHRS_M2 msg: 0.86 phi; -0.14 theta; -0.25 psi 2021-02-13T00:11:54Z- handleDepth()-Depth_Keller msg: 1613175114990 epoch sec; seqNo:10490; depth 4.68 2021-02-13T00:11:55Z- handleDvl() - RDI_Pathfinder msg: 1613175115.00 epoch sec; seqNo:5131 2021-02-13T00:11:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.95 , 112.73 , 97.85 , 106.78 2021-02-13T00:11:55Z- handleDvl() - RDI_Pathfinder msg: velocities 1.08 , 0.24 , -0.07 2021-02-13T00:11:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175115.01 epoch sec; seqNo:10490; 4058598.03 north; 592122.95 east 2021-02-13T00:11:55Z- AHRS_M2 msg: 1613175115.39 epoch sec; seqNo:10405 2021-02-13T00:11:55Z- AHRS_M2 msg: 0.87 phi; -0.15 theta; -0.24 psi 2021-02-13T00:11:55Z- handleDepth()-Depth_Keller msg: 1613175115393 epoch sec; seqNo:10491; depth 4.74 2021-02-13T00:11:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175115.41 epoch sec; seqNo:10491; 4058598.03 north; 592122.95 east 2021-02-13T00:11:55Z- AHRS_M2 msg: 1613175115.79 epoch sec; seqNo:10406 2021-02-13T00:11:55Z- AHRS_M2 msg: 0.88 phi; -0.16 theta; -0.22 psi 2021-02-13T00:11:55Z- handleDepth()-Depth_Keller msg: 1613175115796 epoch sec; seqNo:10492; depth 4.79 2021-02-13T00:11:55Z- handleDvl() - RDI_Pathfinder msg: 1613175115.81 epoch sec; seqNo:5132 2021-02-13T00:11:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 111.85 , 96.97 , 107.79 2021-02-13T00:11:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.15 , -0.37 2021-02-13T00:11:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175115.83 epoch sec; seqNo:10492; 4058598.42 north; 592124.17 east 2021-02-13T00:11:56Z- handleDepth()-Depth_Keller msg: 1613175116212 epoch sec; seqNo:10493; depth 4.84 2021-02-13T00:11:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:56Z- handleDvl() - RDI_Pathfinder msg: 1613175116.21 epoch sec; seqNo:5133 2021-02-13T00:11:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.15 , -0.37 2021-02-13T00:11:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175116.22 epoch sec; seqNo:10493; 4058598.80 north; 592124.17 east 2021-02-13T00:11:56Z- AHRS_M2 msg: 1613175116.60 epoch sec; seqNo:10407 2021-02-13T00:11:56Z- AHRS_M2 msg: 0.89 phi; -0.20 theta; -0.20 psi 2021-02-13T00:11:56Z- handleDepth()-Depth_Keller msg: 1613175116605 epoch sec; seqNo:10494; depth 4.90 2021-02-13T00:11:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175116.62 epoch sec; seqNo:10494; 4058598.80 north; 592124.17 east 2021-02-13T00:11:57Z- AHRS_M2 msg: 1613175117.00 epoch sec; seqNo:10408 2021-02-13T00:11:57Z- AHRS_M2 msg: 0.89 phi; -0.22 theta; -0.22 psi 2021-02-13T00:11:57Z- handleDepth()-Depth_Keller msg: 1613175117013 epoch sec; seqNo:10495; depth 4.96 2021-02-13T00:11:57Z- handleDvl() - RDI_Pathfinder msg: 1613175117.03 epoch sec; seqNo:5134 2021-02-13T00:11:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.23 , 106.58 , 87.23 , 109.55 2021-02-13T00:11:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.15 , -0.48 2021-02-13T00:11:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175117.05 epoch sec; seqNo:10495; 4058599.18 north; 592124.16 east 2021-02-13T00:11:57Z- AHRS_M2 msg: 1613175117.41 epoch sec; seqNo:10409 2021-02-13T00:11:57Z- AHRS_M2 msg: 0.89 phi; -0.24 theta; -0.24 psi 2021-02-13T00:11:57Z- handleDepth()-Depth_Keller msg: 1613175117412 epoch sec; seqNo:10496; depth 5.00 2021-02-13T00:11:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:11:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:11:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:11:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:11:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:11:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:11:57Z- handleDvl() - RDI_Pathfinder msg: 1613175117.41 epoch sec; seqNo:5135 2021-02-13T00:11:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.15 , -0.48 2021-02-13T00:11:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175117.42 epoch sec; seqNo:10496; 4058599.57 north; 592125.38 east 2021-02-13T00:11:57Z- AHRS_M2 msg: 1613175117.81 epoch sec; seqNo:10410 2021-02-13T00:11:57Z- AHRS_M2 msg: 0.90 phi; -0.27 theta; -0.25 psi 2021-02-13T00:11:57Z- handleDepth()-Depth_Keller msg: 1613175117818 epoch sec; seqNo:10497; depth 5.06 2021-02-13T00:11:57Z- handleDvl() - RDI_Pathfinder msg: 1613175117.83 epoch sec; seqNo:5136 2021-02-13T00:11:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:11:57Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:11:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175117.85 epoch sec; seqNo:10497; 4058599.95 north; 592125.38 east 2021-02-13T00:11:58Z- AHRS_M2 msg: 1613175118.22 epoch sec; seqNo:10411 2021-02-13T00:11:58Z- AHRS_M2 msg: 0.89 phi; -0.29 theta; -0.24 psi 2021-02-13T00:11:58Z- handleDepth()-Depth_Keller msg: 1613175118220 epoch sec; seqNo:10498; depth 5.14 2021-02-13T00:11:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175118.24 epoch sec; seqNo:10498; 4058600.33 north; 592125.37 east 2021-02-13T00:11:58Z- AHRS_M2 msg: 1613175118.62 epoch sec; seqNo:10412 2021-02-13T00:11:58Z- AHRS_M2 msg: 0.89 phi; -0.31 theta; -0.22 psi 2021-02-13T00:11:58Z- handleDepth()-Depth_Keller msg: 1613175118624 epoch sec; seqNo:10499; depth 5.18 2021-02-13T00:11:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175118.63 epoch sec; seqNo:10499; 4058600.33 north; 592125.37 east 2021-02-13T00:11:58Z- LcmTrn::handleMessages() - handling 41 messages took 0.60 ms and 129 calls... 2021-02-13T00:11:58Z- waiting for 1613175116.81, time is 1613175118.63 2021-02-13T00:11:58Z- LcmTrn::updateTrn() >>>> pose time 1613175118.63, meas time 1613175117.83 <<<< 2021-02-13T00:11:58Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175117.83, ping # 0. 2021-02-13T00:11:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175117.83 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:11:58Z- TerrainNav::motionUpdate() - currEstimate = (4058600, 592125.37, 5.18) 2021-02-13T00:11:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:11:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:11:58Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:11:58Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:11:58Z- TNavBF::Projecting Measurements 2021-02-13T00:11:58Z- TNavFilter::Measurement from time = 1613175117.83 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:11:58Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:11:58Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:11:58Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:11:58Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:11:58Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:11:58Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:11:58Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:11:58Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 714.7 EastVariance: 833.0 DepthVariance: 1.6 this->DepthBiasCov: 1.6 2021-02-13T00:11:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:11:58Z- TNavBF::mmse selected from filter -1 2021-02-13T00:11:58Z- variances([0], [2], and psi_berg) = 714.695395 833.006304 0.000000 2021-02-13T00:11:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:11:58Z- LcmTrn::publishEstimates() - MLE : 4058598.83 592083.87 5.18 2021-02-13T00:11:58Z- LcmTrn::publishEstimates() - MMSE : 4058600.33 592125.37 5.18 2021-02-13T00:11:58Z- LcmTrn::publishEstimates() - COVAR: 714.70 833.01 1.60 0.00 2021-02-13T00:11:59Z- AHRS_M2 msg: 1613175119.02 epoch sec; seqNo:10413 2021-02-13T00:11:59Z- AHRS_M2 msg: 0.87 phi; -0.32 theta; -0.20 psi 2021-02-13T00:11:59Z- handleDepth()-Depth_Keller msg: 1613175119028 epoch sec; seqNo:10500; depth 5.25 2021-02-13T00:11:59Z- handleDvl() - RDI_Pathfinder msg: 1613175119.04 epoch sec; seqNo:5137 2021-02-13T00:11:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.13 , 113.48 , 85.07 , 124.30 2021-02-13T00:11:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.62 , -0.06 , 0.39 2021-02-13T00:11:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175119.05 epoch sec; seqNo:10500; 4058600.71 north; 592125.37 east 2021-02-13T00:11:59Z- AHRS_M2 msg: 1613175119.43 epoch sec; seqNo:10414 2021-02-13T00:11:59Z- AHRS_M2 msg: 0.86 phi; -0.32 theta; -0.20 psi 2021-02-13T00:11:59Z- handleDepth()-Depth_Keller msg: 1613175119434 epoch sec; seqNo:10501; depth 5.33 2021-02-13T00:11:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175119.44 epoch sec; seqNo:10501; 4058600.71 north; 592125.37 east 2021-02-13T00:11:59Z- AHRS_M2 msg: 1613175119.83 epoch sec; seqNo:10415 2021-02-13T00:11:59Z- AHRS_M2 msg: 0.85 phi; -0.32 theta; -0.19 psi 2021-02-13T00:11:59Z- handleDepth()-Depth_Keller msg: 1613175119837 epoch sec; seqNo:10502; depth 5.41 2021-02-13T00:11:59Z- handleDvl() - RDI_Pathfinder msg: 1613175119.87 epoch sec; seqNo:5138 2021-02-13T00:11:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.86 , 109.76 , 0.00 , 124.64 2021-02-13T00:11:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.15 , 0.25 2021-02-13T00:11:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175119.88 epoch sec; seqNo:10502; 4058600.72 north; 592126.59 east 2021-02-13T00:12:00Z- AHRS_M2 msg: 1613175120.24 epoch sec; seqNo:10416 2021-02-13T00:12:00Z- AHRS_M2 msg: 0.86 phi; -0.31 theta; -0.20 psi 2021-02-13T00:12:00Z- handleDepth()-Depth_Keller msg: 1613175120245 epoch sec; seqNo:10503; depth 5.53 2021-02-13T00:12:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175120.26 epoch sec; seqNo:10503; 4058601.10 north; 592126.58 east 2021-02-13T00:12:00Z- AHRS_M2 msg: 1613175120.64 epoch sec; seqNo:10417 2021-02-13T00:12:00Z- AHRS_M2 msg: 0.86 phi; -0.31 theta; -0.21 psi 2021-02-13T00:12:00Z- handleDepth()-Depth_Keller msg: 1613175120646 epoch sec; seqNo:10504; depth 5.61 2021-02-13T00:12:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175120.65 epoch sec; seqNo:10504; 4058601.10 north; 592126.58 east 2021-02-13T00:12:01Z- AHRS_M2 msg: 1613175121.04 epoch sec; seqNo:10418 2021-02-13T00:12:01Z- AHRS_M2 msg: 0.86 phi; -0.30 theta; -0.23 psi 2021-02-13T00:12:01Z- handleDepth()-Depth_Keller msg: 1613175121057 epoch sec; seqNo:10505; depth 5.71 2021-02-13T00:12:01Z- handleDvl() - RDI_Pathfinder msg: 1613175121.07 epoch sec; seqNo:5139 2021-02-13T00:12:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.30 , 112.12 , 83.86 , 128.49 2021-02-13T00:12:01Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.11 , -0.01 2021-02-13T00:12:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175121.08 epoch sec; seqNo:10505; 4058601.48 north; 592126.58 east 2021-02-13T00:12:01Z- AHRS_M2 msg: 1613175121.45 epoch sec; seqNo:10419 2021-02-13T00:12:01Z- AHRS_M2 msg: 0.87 phi; -0.30 theta; -0.23 psi 2021-02-13T00:12:01Z- handleDepth()-Depth_Keller msg: 1613175121453 epoch sec; seqNo:10506; depth 5.82 2021-02-13T00:12:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175121.48 epoch sec; seqNo:10506; 4058601.49 north; 592127.80 east 2021-02-13T00:12:01Z- AHRS_M2 msg: 1613175121.86 epoch sec; seqNo:10420 2021-02-13T00:12:01Z- AHRS_M2 msg: 0.87 phi; -0.31 theta; -0.23 psi 2021-02-13T00:12:01Z- handleDepth()-Depth_Keller msg: 1613175121866 epoch sec; seqNo:10507; depth 5.92 2021-02-13T00:12:01Z- handleDvl() - RDI_Pathfinder msg: 1613175121.89 epoch sec; seqNo:5140 2021-02-13T00:12:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.86 , 114.15 , 0.00 , 123.08 2021-02-13T00:12:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.00 , -0.07 2021-02-13T00:12:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175121.90 epoch sec; seqNo:10507; 4058601.87 north; 592127.80 east 2021-02-13T00:12:02Z- AHRS_M2 msg: 1613175122.26 epoch sec; seqNo:10421 2021-02-13T00:12:02Z- AHRS_M2 msg: 0.86 phi; -0.32 theta; -0.22 psi 2021-02-13T00:12:02Z- handleDepth()-Depth_Keller msg: 1613175122263 epoch sec; seqNo:10508; depth 6.03 2021-02-13T00:12:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:02Z- handleDvl() - RDI_Pathfinder msg: 1613175122.27 epoch sec; seqNo:5141 2021-02-13T00:12:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.00 , -0.07 2021-02-13T00:12:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175122.28 epoch sec; seqNo:10508; 4058601.87 north; 592127.80 east 2021-02-13T00:12:02Z- AHRS_M2 msg: 1613175122.66 epoch sec; seqNo:10422 2021-02-13T00:12:02Z- AHRS_M2 msg: 0.85 phi; -0.33 theta; -0.22 psi 2021-02-13T00:12:02Z- handleDepth()-Depth_Keller msg: 1613175122670 epoch sec; seqNo:10509; depth 6.14 2021-02-13T00:12:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175122.68 epoch sec; seqNo:10509; 4058602.25 north; 592127.79 east 2021-02-13T00:12:03Z- AHRS_M2 msg: 1613175123.06 epoch sec; seqNo:10423 2021-02-13T00:12:03Z- AHRS_M2 msg: 0.84 phi; -0.33 theta; -0.22 psi 2021-02-13T00:12:03Z- handleDepth()-Depth_Keller msg: 1613175123073 epoch sec; seqNo:10510; depth 6.24 2021-02-13T00:12:03Z- handleDvl() - RDI_Pathfinder msg: 1613175123.09 epoch sec; seqNo:5142 2021-02-13T00:12:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.46 , 113.75 , 83.99 , 125.65 2021-02-13T00:12:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.03 , -0.00 2021-02-13T00:12:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175123.11 epoch sec; seqNo:10510; 4058602.25 north; 592127.79 east 2021-02-13T00:12:03Z- AHRS_M2 msg: 1613175123.47 epoch sec; seqNo:10424 2021-02-13T00:12:03Z- AHRS_M2 msg: 0.83 phi; -0.33 theta; -0.22 psi 2021-02-13T00:12:03Z- handleDepth()-Depth_Keller msg: 1613175123473 epoch sec; seqNo:10511; depth 6.34 2021-02-13T00:12:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:03Z- handleDvl() - RDI_Pathfinder msg: 1613175123.47 epoch sec; seqNo:5143 2021-02-13T00:12:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.03 , -0.00 2021-02-13T00:12:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175123.49 epoch sec; seqNo:10511; 4058602.64 north; 592129.01 east 2021-02-13T00:12:03Z- AHRS_M2 msg: 1613175123.87 epoch sec; seqNo:10425 2021-02-13T00:12:03Z- AHRS_M2 msg: 0.83 phi; -0.34 theta; -0.23 psi 2021-02-13T00:12:03Z- handleDepth()-Depth_Keller msg: 1613175123889 epoch sec; seqNo:10512; depth 6.44 2021-02-13T00:12:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175123.90 epoch sec; seqNo:10512; 4058602.64 north; 592129.01 east 2021-02-13T00:12:03Z- LcmTrn::handleMessages() - handling 46 messages took 0.70 ms and 132 calls... 2021-02-13T00:12:03Z- waiting for 1613175121.63, time is 1613175123.90 2021-02-13T00:12:03Z- LcmTrn::updateTrn() >>>> pose time 1613175123.90, meas time 1613175123.47 <<<< 2021-02-13T00:12:03Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175123.47, ping # 0. 2021-02-13T00:12:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175123.47 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:12:03Z- TerrainNav::motionUpdate() - currEstimate = (4058603, 592129.01, 6.44) 2021-02-13T00:12:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:12:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:12:03Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:12:03Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:12:03Z- TNavBF::Projecting Measurements 2021-02-13T00:12:03Z- TNavFilter::Measurement from time = 1613175123.47 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:12:03Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:12:03Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:12:03Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:12:03Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:12:03Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:12:03Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:12:03Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:12:03Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 714.6 EastVariance: 833.0 DepthVariance: 1.8 this->DepthBiasCov: 1.8 2021-02-13T00:12:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:12:03Z- TNavBF::mmse selected from filter -1 2021-02-13T00:12:03Z- variances([0], [2], and psi_berg) = 714.613458 833.006253 0.000000 2021-02-13T00:12:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:12:03Z- LcmTrn::publishEstimates() - MLE : 4058601.14 592087.51 6.44 2021-02-13T00:12:03Z- LcmTrn::publishEstimates() - MMSE : 4058602.64 592129.01 6.44 2021-02-13T00:12:03Z- LcmTrn::publishEstimates() - COVAR: 714.61 833.01 1.82 0.00 2021-02-13T00:12:04Z- AHRS_M2 msg: 1613175124.28 epoch sec; seqNo:10426 2021-02-13T00:12:04Z- AHRS_M2 msg: 0.82 phi; -0.34 theta; -0.23 psi 2021-02-13T00:12:04Z- handleDepth()-Depth_Keller msg: 1613175124285 epoch sec; seqNo:10513; depth 6.56 2021-02-13T00:12:04Z- handleDvl() - RDI_Pathfinder msg: 1613175124.30 epoch sec; seqNo:5144 2021-02-13T00:12:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.86 , 112.73 , 82.98 , 121.66 2021-02-13T00:12:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.06 , 0.01 2021-02-13T00:12:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175124.31 epoch sec; seqNo:10513; 4058603.02 north; 592129.01 east 2021-02-13T00:12:04Z- AHRS_M2 msg: 1613175124.68 epoch sec; seqNo:10427 2021-02-13T00:12:04Z- AHRS_M2 msg: 0.82 phi; -0.34 theta; -0.23 psi 2021-02-13T00:12:04Z- handleDepth()-Depth_Keller msg: 1613175124684 epoch sec; seqNo:10514; depth 6.67 2021-02-13T00:12:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:04Z- handleDvl() - RDI_Pathfinder msg: 1613175124.69 epoch sec; seqNo:5145 2021-02-13T00:12:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.06 , 0.01 2021-02-13T00:12:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175124.70 epoch sec; seqNo:10514; 4058603.02 north; 592129.01 east 2021-02-13T00:12:05Z- AHRS_M2 msg: 1613175125.08 epoch sec; seqNo:10428 2021-02-13T00:12:05Z- AHRS_M2 msg: 0.82 phi; -0.35 theta; -0.24 psi 2021-02-13T00:12:05Z- handleDepth()-Depth_Keller msg: 1613175125089 epoch sec; seqNo:10515; depth 6.78 2021-02-13T00:12:05Z- handleDvl() - RDI_Pathfinder msg: 1613175125.10 epoch sec; seqNo:5146 2021-02-13T00:12:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.86 , 114.15 , 82.91 , 127.54 2021-02-13T00:12:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.06 , -0.01 2021-02-13T00:12:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175125.11 epoch sec; seqNo:10515; 4058603.40 north; 592129.00 east 2021-02-13T00:12:05Z- AHRS_M2 msg: 1613175125.49 epoch sec; seqNo:10429 2021-02-13T00:12:05Z- AHRS_M2 msg: 0.81 phi; -0.35 theta; -0.24 psi 2021-02-13T00:12:05Z- handleDepth()-Depth_Keller msg: 1613175125494 epoch sec; seqNo:10516; depth 6.88 2021-02-13T00:12:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175125.51 epoch sec; seqNo:10516; 4058603.40 north; 592129.00 east 2021-02-13T00:12:05Z- AHRS_M2 msg: 1613175125.89 epoch sec; seqNo:10430 2021-02-13T00:12:05Z- AHRS_M2 msg: 0.81 phi; -0.35 theta; -0.24 psi 2021-02-13T00:12:05Z- handleDepth()-Depth_Keller msg: 1613175125900 epoch sec; seqNo:10517; depth 7.00 2021-02-13T00:12:05Z- handleDvl() - RDI_Pathfinder msg: 1613175125.96 epoch sec; seqNo:5147 2021-02-13T00:12:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 115.71 , 82.98 , 126.12 2021-02-13T00:12:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.00 2021-02-13T00:12:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175125.98 epoch sec; seqNo:10517; 4058603.79 north; 592130.22 east 2021-02-13T00:12:06Z- AHRS_M2 msg: 1613175126.30 epoch sec; seqNo:10431 2021-02-13T00:12:06Z- AHRS_M2 msg: 0.80 phi; -0.35 theta; -0.25 psi 2021-02-13T00:12:06Z- handleDepth()-Depth_Keller msg: 1613175126303 epoch sec; seqNo:10518; depth 7.14 2021-02-13T00:12:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175126.32 epoch sec; seqNo:10518; 4058604.17 north; 592130.21 east 2021-02-13T00:12:06Z- AHRS_M2 msg: 1613175126.70 epoch sec; seqNo:10432 2021-02-13T00:12:06Z- AHRS_M2 msg: 0.79 phi; -0.35 theta; -0.26 psi 2021-02-13T00:12:06Z- handleDepth()-Depth_Keller msg: 1613175126704 epoch sec; seqNo:10519; depth 7.24 2021-02-13T00:12:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175126.72 epoch sec; seqNo:10519; 4058604.17 north; 592130.21 east 2021-02-13T00:12:07Z- AHRS_M2 msg: 1613175127.10 epoch sec; seqNo:10433 2021-02-13T00:12:07Z- AHRS_M2 msg: 0.78 phi; -0.35 theta; -0.26 psi 2021-02-13T00:12:07Z- handleDepth()-Depth_Keller msg: 1613175127109 epoch sec; seqNo:10520; depth 7.34 2021-02-13T00:12:07Z- handleDvl() - RDI_Pathfinder msg: 1613175127.12 epoch sec; seqNo:5148 2021-02-13T00:12:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 115.71 , 82.98 , 133.56 2021-02-13T00:12:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , 0.07 2021-02-13T00:12:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175127.15 epoch sec; seqNo:10520; 4058604.55 north; 592130.21 east 2021-02-13T00:12:07Z- AHRS_M2 msg: 1613175127.51 epoch sec; seqNo:10434 2021-02-13T00:12:07Z- AHRS_M2 msg: 0.77 phi; -0.35 theta; -0.26 psi 2021-02-13T00:12:07Z- handleDepth()-Depth_Keller msg: 1613175127512 epoch sec; seqNo:10521; depth 7.49 2021-02-13T00:12:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175127.52 epoch sec; seqNo:10521; 4058604.93 north; 592130.21 east 2021-02-13T00:12:07Z- AHRS_M2 msg: 1613175127.93 epoch sec; seqNo:10435 2021-02-13T00:12:07Z- AHRS_M2 msg: 0.76 phi; -0.35 theta; -0.25 psi 2021-02-13T00:12:07Z- handleDepth()-Depth_Keller msg: 1613175127938 epoch sec; seqNo:10522; depth 7.59 2021-02-13T00:12:07Z- handleDvl() - RDI_Pathfinder msg: 1613175127.95 epoch sec; seqNo:5149 2021-02-13T00:12:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.23 , 115.51 , 0.00 , 125.92 2021-02-13T00:12:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , 0.10 2021-02-13T00:12:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175127.96 epoch sec; seqNo:10522; 4058604.94 north; 592131.43 east 2021-02-13T00:12:08Z- AHRS_M2 msg: 1613175128.32 epoch sec; seqNo:10436 2021-02-13T00:12:08Z- AHRS_M2 msg: 0.76 phi; -0.34 theta; -0.24 psi 2021-02-13T00:12:08Z- handleDepth()-Depth_Keller msg: 1613175128334 epoch sec; seqNo:10523; depth 7.73 2021-02-13T00:12:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:08Z- handleDvl() - RDI_Pathfinder msg: 1613175128.34 epoch sec; seqNo:5150 2021-02-13T00:12:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , 0.10 2021-02-13T00:12:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175128.35 epoch sec; seqNo:10523; 4058605.32 north; 592131.42 east 2021-02-13T00:12:08Z- AHRS_M2 msg: 1613175128.72 epoch sec; seqNo:10437 2021-02-13T00:12:08Z- AHRS_M2 msg: 0.76 phi; -0.34 theta; -0.22 psi 2021-02-13T00:12:08Z- handleDepth()-Depth_Keller msg: 1613175128724 epoch sec; seqNo:10524; depth 7.84 2021-02-13T00:12:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175128.73 epoch sec; seqNo:10524; 4058605.32 north; 592131.42 east 2021-02-13T00:12:08Z- LcmTrn::handleMessages() - handling 43 messages took 0.61 ms and 131 calls... 2021-02-13T00:12:08Z- waiting for 1613175126.90, time is 1613175128.73 2021-02-13T00:12:08Z- LcmTrn::updateTrn() >>>> pose time 1613175128.73, meas time 1613175128.34 <<<< 2021-02-13T00:12:08Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175128.34, ping # 0. 2021-02-13T00:12:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175128.34 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:12:08Z- TerrainNav::motionUpdate() - currEstimate = (4058605, 592131.42, 7.84) 2021-02-13T00:12:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:12:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:12:08Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:12:08Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:12:08Z- TNavBF::Projecting Measurements 2021-02-13T00:12:08Z- TNavFilter::Measurement from time = 1613175128.34 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:12:08Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:12:08Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:12:08Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:12:08Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:12:08Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:12:08Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:12:08Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:12:08Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 714.6 EastVariance: 833.0 DepthVariance: 2.0 this->DepthBiasCov: 2.0 2021-02-13T00:12:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:12:08Z- TNavBF::mmse selected from filter -1 2021-02-13T00:12:08Z- variances([0], [2], and psi_berg) = 714.538462 833.006207 0.000000 2021-02-13T00:12:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:12:08Z- LcmTrn::publishEstimates() - MLE : 4058603.82 592087.92 7.84 2021-02-13T00:12:08Z- LcmTrn::publishEstimates() - MMSE : 4058605.32 592131.42 7.84 2021-02-13T00:12:08Z- LcmTrn::publishEstimates() - COVAR: 714.54 833.01 2.00 0.00 2021-02-13T00:12:09Z- AHRS_M2 msg: 1613175129.13 epoch sec; seqNo:10438 2021-02-13T00:12:09Z- AHRS_M2 msg: 0.76 phi; -0.34 theta; -0.20 psi 2021-02-13T00:12:09Z- handleDepth()-Depth_Keller msg: 1613175129131 epoch sec; seqNo:10525; depth 7.98 2021-02-13T00:12:09Z- handleDvl() - RDI_Pathfinder msg: 1613175129.15 epoch sec; seqNo:5151 2021-02-13T00:12:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 165.76 , 112.19 , 0.00 , 122.61 2021-02-13T00:12:09Z- handleDvl() - RDI_Pathfinder msg: velocities 2.40 , -1.42 , -0.81 2021-02-13T00:12:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175129.17 epoch sec; seqNo:10525; 4058606.83 north; 592131.41 east 2021-02-13T00:12:09Z- AHRS_M2 msg: 1613175129.53 epoch sec; seqNo:10439 2021-02-13T00:12:09Z- AHRS_M2 msg: 0.76 phi; -0.34 theta; -0.19 psi 2021-02-13T00:12:09Z- handleDepth()-Depth_Keller msg: 1613175129534 epoch sec; seqNo:10526; depth 8.10 2021-02-13T00:12:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:09Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:09Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:09Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:09Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:09Z- handleDvl() - RDI_Pathfinder msg: 1613175129.54 epoch sec; seqNo:5152 2021-02-13T00:12:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:09Z- handleDvl() - RDI_Pathfinder msg: velocities 2.40 , -1.42 , -0.81 2021-02-13T00:12:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175129.55 epoch sec; seqNo:10526; 4058607.59 north; 592131.40 east 2021-02-13T00:12:09Z- AHRS_M2 msg: 1613175129.93 epoch sec; seqNo:10440 2021-02-13T00:12:09Z- AHRS_M2 msg: 0.77 phi; -0.34 theta; -0.19 psi 2021-02-13T00:12:09Z- handleDepth()-Depth_Keller msg: 1613175129950 epoch sec; seqNo:10527; depth 8.22 2021-02-13T00:12:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175129.96 epoch sec; seqNo:10527; 4058608.74 north; 592132.61 east 2021-02-13T00:12:10Z- AHRS_M2 msg: 1613175130.34 epoch sec; seqNo:10441 2021-02-13T00:12:10Z- AHRS_M2 msg: 0.77 phi; -0.34 theta; -0.18 psi 2021-02-13T00:12:10Z- handleDepth()-Depth_Keller msg: 1613175130340 epoch sec; seqNo:10528; depth 8.34 2021-02-13T00:12:10Z- handleDvl() - RDI_Pathfinder msg: 1613175130.35 epoch sec; seqNo:5153 2021-02-13T00:12:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.32 , 107.66 , 80.88 , 118.08 2021-02-13T00:12:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.13 , -0.06 2021-02-13T00:12:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175130.37 epoch sec; seqNo:10528; 4058609.12 north; 592132.60 east 2021-02-13T00:12:10Z- AHRS_M2 msg: 1613175130.74 epoch sec; seqNo:10442 2021-02-13T00:12:10Z- AHRS_M2 msg: 0.78 phi; -0.34 theta; -0.18 psi 2021-02-13T00:12:10Z- handleDepth()-Depth_Keller msg: 1613175130744 epoch sec; seqNo:10529; depth 8.46 2021-02-13T00:12:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:10Z- handleDvl() - RDI_Pathfinder msg: 1613175130.75 epoch sec; seqNo:5154 2021-02-13T00:12:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.13 , -0.06 2021-02-13T00:12:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175130.76 epoch sec; seqNo:10529; 4058609.12 north; 592132.60 east 2021-02-13T00:12:11Z- handleDepth()-Depth_Keller msg: 1613175131165 epoch sec; seqNo:10530; depth 8.58 2021-02-13T00:12:11Z- handleDvl() - RDI_Pathfinder msg: 1613175131.18 epoch sec; seqNo:5155 2021-02-13T00:12:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 108.00 , 82.30 , 122.88 2021-02-13T00:12:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.05 , -0.14 2021-02-13T00:12:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175131.19 epoch sec; seqNo:10530; 4058609.50 north; 592132.60 east 2021-02-13T00:12:11Z- AHRS_M2 msg: 1613175131.55 epoch sec; seqNo:10443 2021-02-13T00:12:11Z- AHRS_M2 msg: 0.78 phi; -0.34 theta; -0.18 psi 2021-02-13T00:12:11Z- handleDepth()-Depth_Keller msg: 1613175131553 epoch sec; seqNo:10531; depth 8.70 2021-02-13T00:12:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175131.57 epoch sec; seqNo:10531; 4058609.88 north; 592132.59 east 2021-02-13T00:12:11Z- AHRS_M2 msg: 1613175131.95 epoch sec; seqNo:10444 2021-02-13T00:12:11Z- AHRS_M2 msg: 0.78 phi; -0.34 theta; -0.18 psi 2021-02-13T00:12:11Z- handleDepth()-Depth_Keller msg: 1613175131956 epoch sec; seqNo:10532; depth 8.81 2021-02-13T00:12:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:11Z- handleDvl() - RDI_Pathfinder msg: 1613175131.96 epoch sec; seqNo:5156 2021-02-13T00:12:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.05 , -0.14 2021-02-13T00:12:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175131.97 epoch sec; seqNo:10532; 4058609.89 north; 592133.82 east 2021-02-13T00:12:12Z- AHRS_M2 msg: 1613175132.36 epoch sec; seqNo:10445 2021-02-13T00:12:12Z- AHRS_M2 msg: 0.78 phi; -0.33 theta; -0.18 psi 2021-02-13T00:12:12Z- handleDepth()-Depth_Keller msg: 1613175132365 epoch sec; seqNo:10533; depth 8.94 2021-02-13T00:12:12Z- handleDvl() - RDI_Pathfinder msg: 1613175132.38 epoch sec; seqNo:5157 2021-02-13T00:12:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.86 , 107.80 , 80.75 , 124.03 2021-02-13T00:12:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.13 , -0.13 2021-02-13T00:12:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175132.39 epoch sec; seqNo:10533; 4058610.27 north; 592133.81 east 2021-02-13T00:12:12Z- AHRS_M2 msg: 1613175132.76 epoch sec; seqNo:10446 2021-02-13T00:12:12Z- AHRS_M2 msg: 0.77 phi; -0.33 theta; -0.18 psi 2021-02-13T00:12:12Z- handleDepth()-Depth_Keller msg: 1613175132767 epoch sec; seqNo:10534; depth 9.05 2021-02-13T00:12:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175132.78 epoch sec; seqNo:10534; 4058610.65 north; 592133.81 east 2021-02-13T00:12:13Z- AHRS_M2 msg: 1613175133.16 epoch sec; seqNo:10447 2021-02-13T00:12:13Z- AHRS_M2 msg: 0.78 phi; -0.32 theta; -0.18 psi 2021-02-13T00:12:13Z- handleDepth()-Depth_Keller msg: 1613175133168 epoch sec; seqNo:10535; depth 9.17 2021-02-13T00:12:13Z- handleDvl() - RDI_Pathfinder msg: 1613175133.18 epoch sec; seqNo:5158 2021-02-13T00:12:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.51 , 106.44 , 79.39 , 121.32 2021-02-13T00:12:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.22 , -0.02 2021-02-13T00:12:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175133.20 epoch sec; seqNo:10535; 4058611.03 north; 592133.80 east 2021-02-13T00:12:13Z- AHRS_M2 msg: 1613175133.57 epoch sec; seqNo:10448 2021-02-13T00:12:13Z- AHRS_M2 msg: 0.78 phi; -0.32 theta; -0.18 psi 2021-02-13T00:12:13Z- handleDepth()-Depth_Keller msg: 1613175133577 epoch sec; seqNo:10536; depth 9.31 2021-02-13T00:12:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175133.59 epoch sec; seqNo:10536; 4058611.41 north; 592133.80 east 2021-02-13T00:12:13Z- AHRS_M2 msg: 1613175133.98 epoch sec; seqNo:10449 2021-02-13T00:12:13Z- AHRS_M2 msg: 0.78 phi; -0.32 theta; -0.17 psi 2021-02-13T00:12:13Z- LcmTrn::handleMessages() - handling 44 messages took 0.62 ms and 132 calls... 2021-02-13T00:12:13Z- waiting for 1613175131.73, time is 1613175133.98 2021-02-13T00:12:13Z- LcmTrn::updateTrn() >>>> pose time 1613175133.98, meas time 1613175133.18 <<<< 2021-02-13T00:12:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175133.18 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:12:13Z- TerrainNav::motionUpdate() - currEstimate = (4058611, 592133.80, 9.31) 2021-02-13T00:12:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:12:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:12:13Z- TNavBF::Projecting Measurements 2021-02-13T00:12:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:12:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:12:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:12:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:12:13Z- TNavFilter::4 good beams. 2021-02-13T00:12:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:12:13Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:12:13Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:12:13Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:12:13Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:12:13Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:12:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:12:13Z- TNavBF::mmse selected from filter -1 2021-02-13T00:12:13Z- TerrainMapOctree: (vehN, vehE ) = (4058610.48, 592133.44). 2021-02-13T00:12:13Z- TerrainMapOctree: Min Distance = 3383.33. 2021-02-13T00:12:13Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:12:14Z- There is no specific good beam that all particles have in common. 2021-02-13T00:12:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:12:14Z- Old Depth Bias: 0.00 DepthBiasCov: 2.28 K: 0.140 ExpectedMeasDiff: nan currMeas.covariance: 12.25 2021-02-13T00:12:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:12:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.96 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 18.13 2021-02-13T00:12:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:12:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.79 K: 0.131 ExpectedMeasDiff: nan currMeas.covariance: 10.09 2021-02-13T00:12:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:12:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.55 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 23.55 2021-02-13T00:12:14Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:12:14Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:12:14Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:12:14Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:12:14Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:12:14Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:12:14Z- Filter 0 No beams used in subcloud update 2021-02-13T00:12:14Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:12:14Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:12:14Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 714.5 EastVariance: 833.0 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-13T00:12:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175133.18 sec. 2021-02-13T00:12:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:12:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:12:14Z- variances([0], [2], and psi_berg) = 714.432221 833.006142 0.000000 2021-02-13T00:12:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:12:14Z- LcmTrn::publishEstimates() - MLE : 4058609.91 592090.30 9.31 2021-02-13T00:12:14Z- LcmTrn::publishEstimates() - MMSE : 4058611.41 592133.80 9.31 2021-02-13T00:12:14Z- LcmTrn::publishEstimates() - COVAR: 714.43 833.01 1.51 0.00 2021-02-13T00:12:14Z- handleDepth()-Depth_Keller msg: 1613175133981 epoch sec; seqNo:10537; depth 9.41 2021-02-13T00:12:14Z- handleDvl() - RDI_Pathfinder msg: 1613175133.99 epoch sec; seqNo:5159 2021-02-13T00:12:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.29 , 79.60 , 117.47 2021-02-13T00:12:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.20 , 0.06 2021-02-13T00:12:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175134.00 epoch sec; seqNo:10537; 4058611.79 north; 592133.80 east 2021-02-13T00:12:14Z- AHRS_M2 msg: 1613175134.38 epoch sec; seqNo:10450 2021-02-13T00:12:14Z- AHRS_M2 msg: 0.79 phi; -0.32 theta; -0.16 psi 2021-02-13T00:12:14Z- handleDepth()-Depth_Keller msg: 1613175134386 epoch sec; seqNo:10538; depth 9.55 2021-02-13T00:12:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:14Z- handleDvl() - RDI_Pathfinder msg: 1613175134.39 epoch sec; seqNo:5160 2021-02-13T00:12:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.20 , 0.06 2021-02-13T00:12:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175134.40 epoch sec; seqNo:10538; 4058611.80 north; 592135.02 east 2021-02-13T00:12:14Z- AHRS_M2 msg: 1613175134.78 epoch sec; seqNo:10451 2021-02-13T00:12:14Z- AHRS_M2 msg: 0.79 phi; -0.33 theta; -0.16 psi 2021-02-13T00:12:14Z- handleDepth()-Depth_Keller msg: 1613175134784 epoch sec; seqNo:10539; depth 9.66 2021-02-13T00:12:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175134.79 epoch sec; seqNo:10539; 4058612.18 north; 592135.01 east 2021-02-13T00:12:15Z- AHRS_M2 msg: 1613175135.18 epoch sec; seqNo:10452 2021-02-13T00:12:15Z- AHRS_M2 msg: 0.79 phi; -0.33 theta; -0.15 psi 2021-02-13T00:12:15Z- handleDepth()-Depth_Keller msg: 1613175135193 epoch sec; seqNo:10540; depth 9.78 2021-02-13T00:12:15Z- handleDvl() - RDI_Pathfinder msg: 1613175135.20 epoch sec; seqNo:5161 2021-02-13T00:12:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.51 , 102.39 , 79.39 , 114.56 2021-02-13T00:12:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.06 , 0.05 2021-02-13T00:12:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175135.21 epoch sec; seqNo:10540; 4058612.56 north; 592135.01 east 2021-02-13T00:12:15Z- AHRS_M2 msg: 1613175135.59 epoch sec; seqNo:10453 2021-02-13T00:12:15Z- AHRS_M2 msg: 0.80 phi; -0.33 theta; -0.14 psi 2021-02-13T00:12:15Z- handleDepth()-Depth_Keller msg: 1613175135594 epoch sec; seqNo:10541; depth 9.90 2021-02-13T00:12:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175135.61 epoch sec; seqNo:10541; 4058612.56 north; 592135.01 east 2021-02-13T00:12:15Z- AHRS_M2 msg: 1613175135.99 epoch sec; seqNo:10454 2021-02-13T00:12:15Z- AHRS_M2 msg: 0.80 phi; -0.33 theta; -0.13 psi 2021-02-13T00:12:15Z- handleDepth()-Depth_Keller msg: 1613175135997 epoch sec; seqNo:10542; depth 9.99 2021-02-13T00:12:16Z- handleDvl() - RDI_Pathfinder msg: 1613175136.02 epoch sec; seqNo:5162 2021-02-13T00:12:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.79 , 76.44 , 114.96 2021-02-13T00:12:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.00 2021-02-13T00:12:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175136.03 epoch sec; seqNo:10542; 4058612.94 north; 592135.00 east 2021-02-13T00:12:16Z- AHRS_M2 msg: 1613175136.40 epoch sec; seqNo:10455 2021-02-13T00:12:16Z- AHRS_M2 msg: 0.81 phi; -0.33 theta; -0.13 psi 2021-02-13T00:12:16Z- handleDepth()-Depth_Keller msg: 1613175136400 epoch sec; seqNo:10543; depth 10.14 2021-02-13T00:12:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175136.41 epoch sec; seqNo:10543; 4058613.31 north; 592135.00 east 2021-02-13T00:12:16Z- AHRS_M2 msg: 1613175136.80 epoch sec; seqNo:10456 2021-02-13T00:12:16Z- AHRS_M2 msg: 0.82 phi; -0.33 theta; -0.13 psi 2021-02-13T00:12:16Z- handleDepth()-Depth_Keller msg: 1613175136804 epoch sec; seqNo:10544; depth 10.25 2021-02-13T00:12:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175136.81 epoch sec; seqNo:10544; 4058613.33 north; 592136.22 east 2021-02-13T00:12:17Z- AHRS_M2 msg: 1613175137.20 epoch sec; seqNo:10457 2021-02-13T00:12:17Z- AHRS_M2 msg: 0.82 phi; -0.33 theta; -0.13 psi 2021-02-13T00:12:17Z- handleDepth()-Depth_Keller msg: 1613175137209 epoch sec; seqNo:10545; depth 10.38 2021-02-13T00:12:17Z- handleDvl() - RDI_Pathfinder msg: 1613175137.22 epoch sec; seqNo:5163 2021-02-13T00:12:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.71 , 99.89 , 78.24 , 114.76 2021-02-13T00:12:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , -0.01 2021-02-13T00:12:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175137.23 epoch sec; seqNo:10545; 4058613.71 north; 592136.22 east 2021-02-13T00:12:17Z- AHRS_M2 msg: 1613175137.61 epoch sec; seqNo:10458 2021-02-13T00:12:17Z- AHRS_M2 msg: 0.83 phi; -0.32 theta; -0.12 psi 2021-02-13T00:12:17Z- handleDepth()-Depth_Keller msg: 1613175137612 epoch sec; seqNo:10546; depth 10.50 2021-02-13T00:12:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175137.63 epoch sec; seqNo:10546; 4058613.71 north; 592136.22 east 2021-02-13T00:12:18Z- AHRS_M2 msg: 1613175138.01 epoch sec; seqNo:10459 2021-02-13T00:12:18Z- AHRS_M2 msg: 0.84 phi; -0.32 theta; -0.11 psi 2021-02-13T00:12:18Z- handleDepth()-Depth_Keller msg: 1613175138047 epoch sec; seqNo:10547; depth 10.63 2021-02-13T00:12:18Z- handleDvl() - RDI_Pathfinder msg: 1613175138.06 epoch sec; seqNo:5164 2021-02-13T00:12:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.80 , 78.51 , 112.33 2021-02-13T00:12:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.03 , 0.01 2021-02-13T00:12:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175138.07 epoch sec; seqNo:10547; 4058614.09 north; 592136.21 east 2021-02-13T00:12:18Z- AHRS_M2 msg: 1613175138.42 epoch sec; seqNo:10460 2021-02-13T00:12:18Z- AHRS_M2 msg: 0.85 phi; -0.32 theta; -0.09 psi 2021-02-13T00:12:18Z- handleDepth()-Depth_Keller msg: 1613175138425 epoch sec; seqNo:10548; depth 10.75 2021-02-13T00:12:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175138.44 epoch sec; seqNo:10548; 4058614.09 north; 592136.21 east 2021-02-13T00:12:18Z- AHRS_M2 msg: 1613175138.82 epoch sec; seqNo:10461 2021-02-13T00:12:18Z- AHRS_M2 msg: 0.85 phi; -0.32 theta; -0.08 psi 2021-02-13T00:12:18Z- handleDepth()-Depth_Keller msg: 1613175138833 epoch sec; seqNo:10549; depth 10.88 2021-02-13T00:12:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175138.84 epoch sec; seqNo:10549; 4058614.48 north; 592137.43 east 2021-02-13T00:12:19Z- LcmTrn::handleMessages() - handling 44 messages took 0.63 ms and 132 calls... 2021-02-13T00:12:19Z- waiting for 1613175136.98, time is 1613175138.84 2021-02-13T00:12:19Z- LcmTrn::updateTrn() >>>> pose time 1613175138.84, meas time 1613175138.06 <<<< 2021-02-13T00:12:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175138.06 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:12:19Z- TerrainNav::motionUpdate() - currEstimate = (4058614, 592137.43, 10.88) 2021-02-13T00:12:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:12:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:12:19Z- TNavBF::Projecting Measurements 2021-02-13T00:12:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:12:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:12:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:12:19Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:12:19Z- TNavFilter::3 good beams. 2021-02-13T00:12:19Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:12:19Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:12:19Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:12:19Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:12:19Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:12:19Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:12:19Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:12:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:12:19Z- TerrainMapOctree: (vehN, vehE ) = (4058613.98, 592136.85). 2021-02-13T00:12:19Z- TerrainMapOctree: Min Distance = 3378.46. 2021-02-13T00:12:19Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:12:19Z- There is no specific good beam that all particles have in common. 2021-02-13T00:12:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:12:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.72 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 15.62 2021-02-13T00:12:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:12:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.56 K: 0.119 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:12:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:12:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 20.19 2021-02-13T00:12:19Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:12:19Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:12:19Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:12:19Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:12:19Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:12:19Z- Filter 0 No beams used in subcloud update 2021-02-13T00:12:19Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:12:19Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:12:19Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 714.4 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:12:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175138.06 sec. 2021-02-13T00:12:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:12:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:12:19Z- variances([0], [2], and psi_berg) = 714.341285 833.006086 0.000000 2021-02-13T00:12:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:12:19Z- LcmTrn::publishEstimates() - MLE : 4058612.98 592093.93 10.88 2021-02-13T00:12:19Z- LcmTrn::publishEstimates() - MMSE : 4058614.48 592137.43 10.88 2021-02-13T00:12:19Z- LcmTrn::publishEstimates() - COVAR: 714.34 833.01 1.33 0.00 2021-02-13T00:12:19Z- AHRS_M2 msg: 1613175139.23 epoch sec; seqNo:10462 2021-02-13T00:12:19Z- AHRS_M2 msg: 0.85 phi; -0.33 theta; -0.08 psi 2021-02-13T00:12:19Z- handleDepth()-Depth_Keller msg: 1613175139235 epoch sec; seqNo:10550; depth 10.97 2021-02-13T00:12:19Z- handleDvl() - RDI_Pathfinder msg: 1613175139.25 epoch sec; seqNo:5165 2021-02-13T00:12:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.20 , 97.32 , 78.38 , 114.90 2021-02-13T00:12:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , 0.06 , 0.02 2021-02-13T00:12:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175139.26 epoch sec; seqNo:10550; 4058614.48 north; 592137.43 east 2021-02-13T00:12:19Z- AHRS_M2 msg: 1613175139.63 epoch sec; seqNo:10463 2021-02-13T00:12:19Z- AHRS_M2 msg: 0.85 phi; -0.33 theta; -0.10 psi 2021-02-13T00:12:19Z- handleDepth()-Depth_Keller msg: 1613175139633 epoch sec; seqNo:10551; depth 11.10 2021-02-13T00:12:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175139.64 epoch sec; seqNo:10551; 4058614.86 north; 592137.42 east 2021-02-13T00:12:20Z- AHRS_M2 msg: 1613175140.03 epoch sec; seqNo:10464 2021-02-13T00:12:20Z- AHRS_M2 msg: 0.84 phi; -0.33 theta; -0.13 psi 2021-02-13T00:12:20Z- handleDepth()-Depth_Keller msg: 1613175140041 epoch sec; seqNo:10552; depth 11.19 2021-02-13T00:12:20Z- handleDvl() - RDI_Pathfinder msg: 1613175140.06 epoch sec; seqNo:5166 2021-02-13T00:12:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.33 , 97.45 , 78.51 , 116.39 2021-02-13T00:12:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.05 , 0.07 2021-02-13T00:12:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175140.07 epoch sec; seqNo:10552; 4058614.86 north; 592137.42 east 2021-02-13T00:12:20Z- AHRS_M2 msg: 1613175140.43 epoch sec; seqNo:10465 2021-02-13T00:12:20Z- AHRS_M2 msg: 0.82 phi; -0.33 theta; -0.16 psi 2021-02-13T00:12:20Z- handleDepth()-Depth_Keller msg: 1613175140445 epoch sec; seqNo:10553; depth 11.30 2021-02-13T00:12:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:20Z- handleDvl() - RDI_Pathfinder msg: 1613175140.45 epoch sec; seqNo:5167 2021-02-13T00:12:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.05 , 0.07 2021-02-13T00:12:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175140.46 epoch sec; seqNo:10553; 4058615.23 north; 592137.42 east 2021-02-13T00:12:20Z- AHRS_M2 msg: 1613175140.84 epoch sec; seqNo:10466 2021-02-13T00:12:20Z- AHRS_M2 msg: 0.81 phi; -0.33 theta; -0.19 psi 2021-02-13T00:12:20Z- handleDepth()-Depth_Keller msg: 1613175140847 epoch sec; seqNo:10554; depth 11.42 2021-02-13T00:12:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175140.86 epoch sec; seqNo:10554; 4058615.25 north; 592138.64 east 2021-02-13T00:12:21Z- AHRS_M2 msg: 1613175141.24 epoch sec; seqNo:10467 2021-02-13T00:12:21Z- AHRS_M2 msg: 0.80 phi; -0.33 theta; -0.20 psi 2021-02-13T00:12:21Z- handleDepth()-Depth_Keller msg: 1613175141253 epoch sec; seqNo:10555; depth 11.51 2021-02-13T00:12:21Z- handleDvl() - RDI_Pathfinder msg: 1613175141.27 epoch sec; seqNo:5168 2021-02-13T00:12:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 101.51 , 78.51 , 116.39 2021-02-13T00:12:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , 0.19 2021-02-13T00:12:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175141.28 epoch sec; seqNo:10555; 4058615.63 north; 592138.64 east 2021-02-13T00:12:21Z- AHRS_M2 msg: 1613175141.65 epoch sec; seqNo:10468 2021-02-13T00:12:21Z- AHRS_M2 msg: 0.80 phi; -0.32 theta; -0.21 psi 2021-02-13T00:12:21Z- handleDepth()-Depth_Keller msg: 1613175141653 epoch sec; seqNo:10556; depth 11.63 2021-02-13T00:12:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:21Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:21Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:21Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:21Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:21Z- handleDvl() - RDI_Pathfinder msg: 1613175141.65 epoch sec; seqNo:5169 2021-02-13T00:12:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , 0.19 2021-02-13T00:12:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175141.67 epoch sec; seqNo:10556; 4058615.63 north; 592138.64 east 2021-02-13T00:12:22Z- AHRS_M2 msg: 1613175142.05 epoch sec; seqNo:10469 2021-02-13T00:12:22Z- AHRS_M2 msg: 0.79 phi; -0.32 theta; -0.22 psi 2021-02-13T00:12:22Z- handleDepth()-Depth_Keller msg: 1613175142066 epoch sec; seqNo:10557; depth 11.75 2021-02-13T00:12:22Z- handleDvl() - RDI_Pathfinder msg: 1613175142.08 epoch sec; seqNo:5170 2021-02-13T00:12:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.21 , 105.50 , 77.67 , 117.67 2021-02-13T00:12:22Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.16 , 0.20 2021-02-13T00:12:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175142.09 epoch sec; seqNo:10557; 4058616.01 north; 592138.63 east 2021-02-13T00:12:22Z- AHRS_M2 msg: 1613175142.46 epoch sec; seqNo:10470 2021-02-13T00:12:22Z- AHRS_M2 msg: 0.78 phi; -0.32 theta; -0.23 psi 2021-02-13T00:12:22Z- handleDepth()-Depth_Keller msg: 1613175142460 epoch sec; seqNo:10558; depth 11.86 2021-02-13T00:12:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175142.47 epoch sec; seqNo:10558; 4058616.01 north; 592138.63 east 2021-02-13T00:12:22Z- AHRS_M2 msg: 1613175142.86 epoch sec; seqNo:10471 2021-02-13T00:12:22Z- AHRS_M2 msg: 0.78 phi; -0.31 theta; -0.23 psi 2021-02-13T00:12:22Z- handleDepth()-Depth_Keller msg: 1613175142864 epoch sec; seqNo:10559; depth 11.98 2021-02-13T00:12:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:22Z- handleDvl() - RDI_Pathfinder msg: 1613175142.87 epoch sec; seqNo:5171 2021-02-13T00:12:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:22Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.16 , 0.20 2021-02-13T00:12:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175142.87 epoch sec; seqNo:10559; 4058616.40 north; 592139.85 east 2021-02-13T00:12:23Z- AHRS_M2 msg: 1613175143.27 epoch sec; seqNo:10472 2021-02-13T00:12:23Z- AHRS_M2 msg: 0.78 phi; -0.31 theta; -0.24 psi 2021-02-13T00:12:23Z- handleDepth()-Depth_Keller msg: 1613175143269 epoch sec; seqNo:10560; depth 12.10 2021-02-13T00:12:23Z- handleDvl() - RDI_Pathfinder msg: 1613175143.28 epoch sec; seqNo:5172 2021-02-13T00:12:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.79 , 106.78 , 78.38 , 116.25 2021-02-13T00:12:23Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , 0.25 , 0.01 2021-02-13T00:12:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175143.29 epoch sec; seqNo:10560; 4058616.40 north; 592139.85 east 2021-02-13T00:12:23Z- AHRS_M2 msg: 1613175143.67 epoch sec; seqNo:10473 2021-02-13T00:12:23Z- AHRS_M2 msg: 0.78 phi; -0.31 theta; -0.24 psi 2021-02-13T00:12:23Z- handleDepth()-Depth_Keller msg: 1613175143672 epoch sec; seqNo:10561; depth 12.21 2021-02-13T00:12:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175143.69 epoch sec; seqNo:10561; 4058616.78 north; 592139.85 east 2021-02-13T00:12:24Z- AHRS_M2 msg: 1613175144.07 epoch sec; seqNo:10474 2021-02-13T00:12:24Z- AHRS_M2 msg: 0.79 phi; -0.31 theta; -0.24 psi 2021-02-13T00:12:24Z- handleDepth()-Depth_Keller msg: 1613175144077 epoch sec; seqNo:10562; depth 12.33 2021-02-13T00:12:24Z- handleDvl() - RDI_Pathfinder msg: 1613175144.10 epoch sec; seqNo:5173 2021-02-13T00:12:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.50 , 105.50 , 78.45 , 117.67 2021-02-13T00:12:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , 0.11 , -0.24 2021-02-13T00:12:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175144.11 epoch sec; seqNo:10562; 4058617.17 north; 592141.06 east 2021-02-13T00:12:24Z- LcmTrn::handleMessages() - handling 48 messages took 0.70 ms and 134 calls... 2021-02-13T00:12:24Z- waiting for 1613175141.84, time is 1613175144.11 2021-02-13T00:12:24Z- LcmTrn::updateTrn() equating pose 1613175144.11 and meas times 1613175144.11 2021-02-13T00:12:24Z- LcmTrn::updateTrn() >>>> pose time 1613175144.11, meas time 1613175144.11 <<<< 2021-02-13T00:12:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175144.11 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:12:24Z- TerrainNav::motionUpdate() - currEstimate = (4058617, 592141.06, 12.33) 2021-02-13T00:12:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:12:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:12:24Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175144.11 sec.; No relevant navigation data available 2021-02-13T00:12:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:12:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:12:24Z- variances([0], [2], and psi_berg) = 714.273993 833.006045 0.000000 2021-02-13T00:12:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:12:24Z- LcmTrn::publishEstimates() - MLE : 4058615.67 592097.56 12.33 2021-02-13T00:12:24Z- LcmTrn::publishEstimates() - MMSE : 4058617.17 592141.06 12.33 2021-02-13T00:12:24Z- LcmTrn::publishEstimates() - COVAR: 714.27 833.01 1.56 0.00 2021-02-13T00:12:24Z- AHRS_M2 msg: 1613175144.47 epoch sec; seqNo:10475 2021-02-13T00:12:24Z- AHRS_M2 msg: 0.80 phi; -0.31 theta; -0.23 psi 2021-02-13T00:12:24Z- handleDepth()-Depth_Keller msg: 1613175144485 epoch sec; seqNo:10563; depth 12.46 2021-02-13T00:12:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175144.50 epoch sec; seqNo:10563; 4058617.17 north; 592141.06 east 2021-02-13T00:12:24Z- AHRS_M2 msg: 1613175144.88 epoch sec; seqNo:10476 2021-02-13T00:12:24Z- AHRS_M2 msg: 0.80 phi; -0.31 theta; -0.21 psi 2021-02-13T00:12:24Z- handleDepth()-Depth_Keller msg: 1613175144884 epoch sec; seqNo:10564; depth 12.57 2021-02-13T00:12:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175144.89 epoch sec; seqNo:10564; 4058617.55 north; 592141.06 east 2021-02-13T00:12:25Z- AHRS_M2 msg: 1613175145.28 epoch sec; seqNo:10477 2021-02-13T00:12:25Z- AHRS_M2 msg: 0.81 phi; -0.31 theta; -0.20 psi 2021-02-13T00:12:25Z- handleDepth()-Depth_Keller msg: 1613175145288 epoch sec; seqNo:10565; depth 12.69 2021-02-13T00:12:25Z- handleDvl() - RDI_Pathfinder msg: 1613175145.31 epoch sec; seqNo:5174 2021-02-13T00:12:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.50 , 105.50 , 78.45 , 113.61 2021-02-13T00:12:25Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.09 , -0.31 2021-02-13T00:12:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175145.32 epoch sec; seqNo:10565; 4058617.93 north; 592141.05 east 2021-02-13T00:12:25Z- AHRS_M2 msg: 1613175145.69 epoch sec; seqNo:10478 2021-02-13T00:12:25Z- AHRS_M2 msg: 0.81 phi; -0.31 theta; -0.19 psi 2021-02-13T00:12:25Z- handleDepth()-Depth_Keller msg: 1613175145693 epoch sec; seqNo:10566; depth 12.80 2021-02-13T00:12:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175145.70 epoch sec; seqNo:10566; 4058618.32 north; 592142.27 east 2021-02-13T00:12:26Z- handleDepth()-Depth_Keller msg: 1613175146113 epoch sec; seqNo:10567; depth 12.92 2021-02-13T00:12:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175146.13 epoch sec; seqNo:10567; 4058618.70 north; 592142.27 east 2021-02-13T00:12:26Z- AHRS_M2 msg: 1613175146.49 epoch sec; seqNo:10479 2021-02-13T00:12:26Z- AHRS_M2 msg: 0.81 phi; -0.31 theta; -0.18 psi 2021-02-13T00:12:26Z- handleDepth()-Depth_Keller msg: 1613175146505 epoch sec; seqNo:10568; depth 13.03 2021-02-13T00:12:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:26Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:26Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:26Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:26Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:26Z- handleDvl() - RDI_Pathfinder msg: 1613175146.51 epoch sec; seqNo:5175 2021-02-13T00:12:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:26Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.09 , -0.31 2021-02-13T00:12:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175146.52 epoch sec; seqNo:10568; 4058619.08 north; 592142.26 east 2021-02-13T00:12:26Z- AHRS_M2 msg: 1613175146.90 epoch sec; seqNo:10480 2021-02-13T00:12:26Z- AHRS_M2 msg: 0.80 phi; -0.30 theta; -0.17 psi 2021-02-13T00:12:26Z- handleDepth()-Depth_Keller msg: 1613175146908 epoch sec; seqNo:10569; depth 13.15 2021-02-13T00:12:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175146.96 epoch sec; seqNo:10569; 4058619.45 north; 592142.26 east 2021-02-13T00:12:27Z- AHRS_M2 msg: 1613175147.30 epoch sec; seqNo:10481 2021-02-13T00:12:27Z- AHRS_M2 msg: 0.80 phi; -0.30 theta; -0.17 psi 2021-02-13T00:12:27Z- handleDepth()-Depth_Keller msg: 1613175147313 epoch sec; seqNo:10570; depth 13.25 2021-02-13T00:12:27Z- handleDvl() - RDI_Pathfinder msg: 1613175147.33 epoch sec; seqNo:5176 2021-02-13T00:12:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.36 , 101.64 , 78.65 , 109.76 2021-02-13T00:12:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.22 , -0.05 2021-02-13T00:12:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175147.34 epoch sec; seqNo:10570; 4058619.45 north; 592142.26 east 2021-02-13T00:12:27Z- AHRS_M2 msg: 1613175147.71 epoch sec; seqNo:10482 2021-02-13T00:12:27Z- AHRS_M2 msg: 0.79 phi; -0.30 theta; -0.17 psi 2021-02-13T00:12:27Z- handleDepth()-Depth_Keller msg: 1613175147714 epoch sec; seqNo:10571; depth 13.34 2021-02-13T00:12:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:27Z- handleDvl() - RDI_Pathfinder msg: 1613175147.72 epoch sec; seqNo:5177 2021-02-13T00:12:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.22 , -0.05 2021-02-13T00:12:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175147.73 epoch sec; seqNo:10571; 4058619.83 north; 592142.26 east 2021-02-13T00:12:28Z- AHRS_M2 msg: 1613175148.11 epoch sec; seqNo:10483 2021-02-13T00:12:28Z- AHRS_M2 msg: 0.79 phi; -0.30 theta; -0.18 psi 2021-02-13T00:12:28Z- handleDepth()-Depth_Keller msg: 1613175148117 epoch sec; seqNo:10572; depth 13.45 2021-02-13T00:12:28Z- handleDvl() - RDI_Pathfinder msg: 1613175148.13 epoch sec; seqNo:5178 2021-02-13T00:12:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.78 , 97.65 , 78.72 , 108.47 2021-02-13T00:12:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.17 , 0.07 2021-02-13T00:12:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175148.16 epoch sec; seqNo:10572; 4058620.23 north; 592143.47 east 2021-02-13T00:12:28Z- AHRS_M2 msg: 1613175148.52 epoch sec; seqNo:10484 2021-02-13T00:12:28Z- AHRS_M2 msg: 0.78 phi; -0.30 theta; -0.18 psi 2021-02-13T00:12:28Z- handleDepth()-Depth_Keller msg: 1613175148520 epoch sec; seqNo:10573; depth 13.57 2021-02-13T00:12:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175148.53 epoch sec; seqNo:10573; 4058620.23 north; 592143.47 east 2021-02-13T00:12:28Z- AHRS_M2 msg: 1613175148.94 epoch sec; seqNo:10485 2021-02-13T00:12:28Z- AHRS_M2 msg: 0.78 phi; -0.30 theta; -0.19 psi 2021-02-13T00:12:28Z- handleDepth()-Depth_Keller msg: 1613175148950 epoch sec; seqNo:10574; depth 13.66 2021-02-13T00:12:28Z- handleDvl() - RDI_Pathfinder msg: 1613175148.97 epoch sec; seqNo:5179 2021-02-13T00:12:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 99.07 , 78.79 , 108.54 2021-02-13T00:12:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.10 , 0.16 2021-02-13T00:12:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175148.99 epoch sec; seqNo:10574; 4058620.60 north; 592143.47 east 2021-02-13T00:12:29Z- LcmTrn::handleMessages() - handling 41 messages took 0.56 ms and 130 calls... 2021-02-13T00:12:29Z- waiting for 1613175147.11, time is 1613175148.99 2021-02-13T00:12:29Z- LcmTrn::updateTrn() equating pose 1613175148.99 and meas times 1613175148.99 2021-02-13T00:12:29Z- LcmTrn::updateTrn() >>>> pose time 1613175148.99, meas time 1613175148.99 <<<< 2021-02-13T00:12:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175148.99 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:12:29Z- TerrainNav::motionUpdate() - currEstimate = (4058621, 592143.47, 13.66) 2021-02-13T00:12:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:12:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:12:29Z- TNavBF::Projecting Measurements 2021-02-13T00:12:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:12:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:12:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:12:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:12:29Z- TNavFilter::4 good beams. 2021-02-13T00:12:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:12:29Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:12:29Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:12:29Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:12:29Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:12:29Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:12:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:12:29Z- TNavBF::mmse selected from filter -1 2021-02-13T00:12:29Z- TerrainMapOctree: (vehN, vehE ) = (4058620.60, 592143.47). 2021-02-13T00:12:29Z- TerrainMapOctree: Min Distance = 3369.15. 2021-02-13T00:12:29Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:12:29Z- There is no specific good beam that all particles have in common. 2021-02-13T00:12:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:12:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.78 K: 0.123 ExpectedMeasDiff: nan currMeas.covariance: 10.98 2021-02-13T00:12:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:12:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.56 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 15.70 2021-02-13T00:12:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:12:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 9.93 2021-02-13T00:12:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:12:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 18.85 2021-02-13T00:12:29Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:12:29Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:12:29Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:12:29Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:12:29Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:12:29Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:12:29Z- Filter 0 No beams used in subcloud update 2021-02-13T00:12:29Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:12:29Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:12:29Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 714.2 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:12:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175148.99 sec. 2021-02-13T00:12:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:12:29Z- TNavBF::mmse selected from filter -1 2021-02-13T00:12:29Z- variances([0], [2], and psi_berg) = 714.209236 833.006005 0.000000 2021-02-13T00:12:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:12:29Z- LcmTrn::publishEstimates() - MLE : 4058619.10 592099.97 13.66 2021-02-13T00:12:29Z- LcmTrn::publishEstimates() - MMSE : 4058620.60 592143.47 13.66 2021-02-13T00:12:29Z- LcmTrn::publishEstimates() - COVAR: 714.21 833.01 1.20 0.00 2021-02-13T00:12:29Z- AHRS_M2 msg: 1613175149.32 epoch sec; seqNo:10486 2021-02-13T00:12:29Z- AHRS_M2 msg: 0.77 phi; -0.30 theta; -0.19 psi 2021-02-13T00:12:29Z- handleDepth()-Depth_Keller msg: 1613175149338 epoch sec; seqNo:10575; depth 13.77 2021-02-13T00:12:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175149.35 epoch sec; seqNo:10575; 4058620.98 north; 592143.46 east 2021-02-13T00:12:29Z- AHRS_M2 msg: 1613175149.73 epoch sec; seqNo:10487 2021-02-13T00:12:29Z- AHRS_M2 msg: 0.77 phi; -0.30 theta; -0.19 psi 2021-02-13T00:12:29Z- handleDepth()-Depth_Keller msg: 1613175149732 epoch sec; seqNo:10576; depth 13.87 2021-02-13T00:12:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175149.74 epoch sec; seqNo:10576; 4058620.98 north; 592143.46 east 2021-02-13T00:12:30Z- AHRS_M2 msg: 1613175150.13 epoch sec; seqNo:10488 2021-02-13T00:12:30Z- AHRS_M2 msg: 0.77 phi; -0.30 theta; -0.20 psi 2021-02-13T00:12:30Z- handleDepth()-Depth_Keller msg: 1613175150145 epoch sec; seqNo:10577; depth 13.95 2021-02-13T00:12:30Z- handleDvl() - RDI_Pathfinder msg: 1613175150.16 epoch sec; seqNo:5180 2021-02-13T00:12:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.49 , 100.43 , 74.80 , 108.54 2021-02-13T00:12:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.01 , 0.19 2021-02-13T00:12:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175150.18 epoch sec; seqNo:10577; 4058621.36 north; 592143.46 east 2021-02-13T00:12:30Z- AHRS_M2 msg: 1613175150.54 epoch sec; seqNo:10489 2021-02-13T00:12:30Z- AHRS_M2 msg: 0.77 phi; -0.30 theta; -0.20 psi 2021-02-13T00:12:30Z- handleDepth()-Depth_Keller msg: 1613175150542 epoch sec; seqNo:10578; depth 14.06 2021-02-13T00:12:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175150.55 epoch sec; seqNo:10578; 4058621.37 north; 592144.68 east 2021-02-13T00:12:30Z- AHRS_M2 msg: 1613175150.96 epoch sec; seqNo:10490 2021-02-13T00:12:30Z- AHRS_M2 msg: 0.77 phi; -0.30 theta; -0.20 psi 2021-02-13T00:12:30Z- handleDepth()-Depth_Keller msg: 1613175150969 epoch sec; seqNo:10579; depth 14.16 2021-02-13T00:12:30Z- handleDvl() - RDI_Pathfinder msg: 1613175150.98 epoch sec; seqNo:5181 2021-02-13T00:12:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 100.09 , 74.40 , 109.83 2021-02-13T00:12:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.08 , 0.16 2021-02-13T00:12:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175150.99 epoch sec; seqNo:10579; 4058621.75 north; 592144.68 east 2021-02-13T00:12:31Z- AHRS_M2 msg: 1613175151.35 epoch sec; seqNo:10491 2021-02-13T00:12:31Z- AHRS_M2 msg: 0.77 phi; -0.31 theta; -0.20 psi 2021-02-13T00:12:31Z- handleDepth()-Depth_Keller msg: 1613175151349 epoch sec; seqNo:10580; depth 14.27 2021-02-13T00:12:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:31Z- handleDvl() - RDI_Pathfinder msg: 1613175151.35 epoch sec; seqNo:5182 2021-02-13T00:12:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.08 , 0.16 2021-02-13T00:12:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175151.36 epoch sec; seqNo:10580; 4058621.75 north; 592144.68 east 2021-02-13T00:12:31Z- AHRS_M2 msg: 1613175151.75 epoch sec; seqNo:10492 2021-02-13T00:12:31Z- AHRS_M2 msg: 0.76 phi; -0.31 theta; -0.20 psi 2021-02-13T00:12:31Z- handleDepth()-Depth_Keller msg: 1613175151753 epoch sec; seqNo:10581; depth 14.36 2021-02-13T00:12:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175151.77 epoch sec; seqNo:10581; 4058622.13 north; 592144.67 east 2021-02-13T00:12:32Z- AHRS_M2 msg: 1613175152.15 epoch sec; seqNo:10493 2021-02-13T00:12:32Z- AHRS_M2 msg: 0.76 phi; -0.31 theta; -0.20 psi 2021-02-13T00:12:32Z- handleDepth()-Depth_Keller msg: 1613175152157 epoch sec; seqNo:10582; depth 14.46 2021-02-13T00:12:32Z- handleDvl() - RDI_Pathfinder msg: 1613175152.17 epoch sec; seqNo:5183 2021-02-13T00:12:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.83 , 101.31 , 75.21 , 111.04 2021-02-13T00:12:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.13 , 0.08 2021-02-13T00:12:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175152.19 epoch sec; seqNo:10582; 4058622.13 north; 592144.67 east 2021-02-13T00:12:32Z- AHRS_M2 msg: 1613175152.56 epoch sec; seqNo:10494 2021-02-13T00:12:32Z- AHRS_M2 msg: 0.76 phi; -0.31 theta; -0.20 psi 2021-02-13T00:12:32Z- handleDepth()-Depth_Keller msg: 1613175152565 epoch sec; seqNo:10583; depth 14.57 2021-02-13T00:12:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:32Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:32Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:32Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:32Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:32Z- handleDvl() - RDI_Pathfinder msg: 1613175152.57 epoch sec; seqNo:5184 2021-02-13T00:12:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.13 , 0.08 2021-02-13T00:12:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175152.58 epoch sec; seqNo:10583; 4058622.52 north; 592145.89 east 2021-02-13T00:12:32Z- AHRS_M2 msg: 1613175152.96 epoch sec; seqNo:10495 2021-02-13T00:12:32Z- AHRS_M2 msg: 0.76 phi; -0.31 theta; -0.20 psi 2021-02-13T00:12:32Z- handleDepth()-Depth_Keller msg: 1613175152965 epoch sec; seqNo:10584; depth 14.68 2021-02-13T00:12:32Z- handleDvl() - RDI_Pathfinder msg: 1613175152.98 epoch sec; seqNo:5185 2021-02-13T00:12:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.49 , 100.43 , 75.62 , 111.25 2021-02-13T00:12:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.10 , 0.02 2021-02-13T00:12:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175152.99 epoch sec; seqNo:10584; 4058622.52 north; 592145.89 east 2021-02-13T00:12:33Z- AHRS_M2 msg: 1613175153.36 epoch sec; seqNo:10496 2021-02-13T00:12:33Z- AHRS_M2 msg: 0.76 phi; -0.31 theta; -0.21 psi 2021-02-13T00:12:33Z- handleDepth()-Depth_Keller msg: 1613175153368 epoch sec; seqNo:10585; depth 14.78 2021-02-13T00:12:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175153.38 epoch sec; seqNo:10585; 4058622.90 north; 592145.88 east 2021-02-13T00:12:33Z- AHRS_M2 msg: 1613175153.77 epoch sec; seqNo:10497 2021-02-13T00:12:33Z- AHRS_M2 msg: 0.76 phi; -0.31 theta; -0.21 psi 2021-02-13T00:12:33Z- handleDepth()-Depth_Keller msg: 1613175153777 epoch sec; seqNo:10586; depth 14.88 2021-02-13T00:12:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:33Z- handleDvl() - RDI_Pathfinder msg: 1613175153.78 epoch sec; seqNo:5186 2021-02-13T00:12:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.10 , 0.02 2021-02-13T00:12:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175153.79 epoch sec; seqNo:10586; 4058622.90 north; 592145.88 east 2021-02-13T00:12:34Z- AHRS_M2 msg: 1613175154.17 epoch sec; seqNo:10498 2021-02-13T00:12:34Z- AHRS_M2 msg: 0.76 phi; -0.32 theta; -0.21 psi 2021-02-13T00:12:34Z- handleDepth()-Depth_Keller msg: 1613175154176 epoch sec; seqNo:10587; depth 14.98 2021-02-13T00:12:34Z- handleDvl() - RDI_Pathfinder msg: 1613175154.19 epoch sec; seqNo:5187 2021-02-13T00:12:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.14 , 100.43 , 78.79 , 109.89 2021-02-13T00:12:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , 0.04 2021-02-13T00:12:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175154.20 epoch sec; seqNo:10587; 4058623.28 north; 592145.88 east 2021-02-13T00:12:34Z- LcmTrn::handleMessages() - handling 47 messages took 0.63 ms and 135 calls... 2021-02-13T00:12:34Z- waiting for 1613175151.99, time is 1613175154.20 2021-02-13T00:12:34Z- LcmTrn::updateTrn() equating pose 1613175154.20 and meas times 1613175154.20 2021-02-13T00:12:34Z- LcmTrn::updateTrn() >>>> pose time 1613175154.20, meas time 1613175154.20 <<<< 2021-02-13T00:12:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175154.20 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:12:34Z- TerrainNav::motionUpdate() - currEstimate = (4058623, 592145.88, 14.98) 2021-02-13T00:12:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:12:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:12:34Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175154.20 sec.; No relevant navigation data available 2021-02-13T00:12:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:12:34Z- TNavBF::mmse selected from filter -1 2021-02-13T00:12:34Z- variances([0], [2], and psi_berg) = 714.149241 833.005968 0.000000 2021-02-13T00:12:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:12:34Z- LcmTrn::publishEstimates() - MLE : 4058621.78 592102.38 14.98 2021-02-13T00:12:34Z- LcmTrn::publishEstimates() - MMSE : 4058623.28 592145.88 14.98 2021-02-13T00:12:34Z- LcmTrn::publishEstimates() - COVAR: 714.15 833.01 1.38 0.00 2021-02-13T00:12:34Z- AHRS_M2 msg: 1613175154.58 epoch sec; seqNo:10499 2021-02-13T00:12:34Z- AHRS_M2 msg: 0.76 phi; -0.32 theta; -0.20 psi 2021-02-13T00:12:34Z- handleDepth()-Depth_Keller msg: 1613175154583 epoch sec; seqNo:10588; depth 15.11 2021-02-13T00:12:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175154.59 epoch sec; seqNo:10588; 4058623.30 north; 592147.10 east 2021-02-13T00:12:34Z- AHRS_M2 msg: 1613175154.98 epoch sec; seqNo:10500 2021-02-13T00:12:34Z- AHRS_M2 msg: 0.76 phi; -0.32 theta; -0.20 psi 2021-02-13T00:12:35Z- handleDepth()-Depth_Keller msg: 1613175155010 epoch sec; seqNo:10589; depth 15.21 2021-02-13T00:12:35Z- handleDvl() - RDI_Pathfinder msg: 1613175155.02 epoch sec; seqNo:5188 2021-02-13T00:12:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.71 , 101.78 , 74.40 , 111.25 2021-02-13T00:12:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.01 , 0.07 2021-02-13T00:12:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175155.03 epoch sec; seqNo:10589; 4058623.67 north; 592147.10 east 2021-02-13T00:12:35Z- AHRS_M2 msg: 1613175155.39 epoch sec; seqNo:10501 2021-02-13T00:12:35Z- AHRS_M2 msg: 0.76 phi; -0.33 theta; -0.19 psi 2021-02-13T00:12:35Z- handleDepth()-Depth_Keller msg: 1613175155394 epoch sec; seqNo:10590; depth 15.35 2021-02-13T00:12:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175155.43 epoch sec; seqNo:10590; 4058624.05 north; 592147.09 east 2021-02-13T00:12:35Z- AHRS_M2 msg: 1613175155.79 epoch sec; seqNo:10502 2021-02-13T00:12:35Z- AHRS_M2 msg: 0.76 phi; -0.33 theta; -0.20 psi 2021-02-13T00:12:35Z- handleDepth()-Depth_Keller msg: 1613175155799 epoch sec; seqNo:10591; depth 15.44 2021-02-13T00:12:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175155.81 epoch sec; seqNo:10591; 4058624.43 north; 592147.09 east 2021-02-13T00:12:36Z- AHRS_M2 msg: 1613175156.20 epoch sec; seqNo:10503 2021-02-13T00:12:36Z- AHRS_M2 msg: 0.76 phi; -0.34 theta; -0.19 psi 2021-02-13T00:12:36Z- handleDepth()-Depth_Keller msg: 1613175156209 epoch sec; seqNo:10592; depth 15.55 2021-02-13T00:12:36Z- handleDvl() - RDI_Pathfinder msg: 1613175156.22 epoch sec; seqNo:5189 2021-02-13T00:12:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.49 , 100.43 , 78.79 , 112.60 2021-02-13T00:12:36Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.08 , 0.02 2021-02-13T00:12:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175156.23 epoch sec; seqNo:10592; 4058624.43 north; 592147.09 east 2021-02-13T00:12:36Z- AHRS_M2 msg: 1613175156.60 epoch sec; seqNo:10504 2021-02-13T00:12:36Z- AHRS_M2 msg: 0.76 phi; -0.34 theta; -0.18 psi 2021-02-13T00:12:36Z- handleDepth()-Depth_Keller msg: 1613175156604 epoch sec; seqNo:10593; depth 15.68 2021-02-13T00:12:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175156.62 epoch sec; seqNo:10593; 4058624.82 north; 592148.31 east 2021-02-13T00:12:37Z- AHRS_M2 msg: 1613175157.00 epoch sec; seqNo:10505 2021-02-13T00:12:37Z- AHRS_M2 msg: 0.76 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:37Z- handleDepth()-Depth_Keller msg: 1613175157008 epoch sec; seqNo:10594; depth 15.82 2021-02-13T00:12:37Z- handleDvl() - RDI_Pathfinder msg: 1613175157.02 epoch sec; seqNo:5190 2021-02-13T00:12:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.36 , 98.94 , 78.65 , 113.82 2021-02-13T00:12:37Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.05 , -0.11 2021-02-13T00:12:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175157.05 epoch sec; seqNo:10594; 4058625.20 north; 592148.30 east 2021-02-13T00:12:37Z- AHRS_M2 msg: 1613175157.40 epoch sec; seqNo:10506 2021-02-13T00:12:37Z- AHRS_M2 msg: 0.77 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:37Z- handleDepth()-Depth_Keller msg: 1613175157408 epoch sec; seqNo:10595; depth 15.95 2021-02-13T00:12:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:37Z- handleDvl() - RDI_Pathfinder msg: 1613175157.41 epoch sec; seqNo:5191 2021-02-13T00:12:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:37Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.05 , -0.11 2021-02-13T00:12:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175157.42 epoch sec; seqNo:10595; 4058625.20 north; 592148.30 east 2021-02-13T00:12:37Z- AHRS_M2 msg: 1613175157.81 epoch sec; seqNo:10507 2021-02-13T00:12:37Z- AHRS_M2 msg: 0.77 phi; -0.34 theta; -0.18 psi 2021-02-13T00:12:37Z- handleDepth()-Depth_Keller msg: 1613175157821 epoch sec; seqNo:10596; depth 16.06 2021-02-13T00:12:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175157.83 epoch sec; seqNo:10596; 4058625.58 north; 592148.30 east 2021-02-13T00:12:38Z- AHRS_M2 msg: 1613175158.21 epoch sec; seqNo:10508 2021-02-13T00:12:38Z- AHRS_M2 msg: 0.77 phi; -0.34 theta; -0.18 psi 2021-02-13T00:12:38Z- handleDepth()-Depth_Keller msg: 1613175158216 epoch sec; seqNo:10597; depth 16.19 2021-02-13T00:12:38Z- handleDvl() - RDI_Pathfinder msg: 1613175158.23 epoch sec; seqNo:5192 2021-02-13T00:12:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.42 , 99.00 , 78.72 , 109.83 2021-02-13T00:12:38Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.01 , -0.23 2021-02-13T00:12:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175158.24 epoch sec; seqNo:10597; 4058625.97 north; 592149.51 east 2021-02-13T00:12:38Z- AHRS_M2 msg: 1613175158.62 epoch sec; seqNo:10509 2021-02-13T00:12:38Z- AHRS_M2 msg: 0.78 phi; -0.34 theta; -0.18 psi 2021-02-13T00:12:38Z- handleDepth()-Depth_Keller msg: 1613175158625 epoch sec; seqNo:10598; depth 16.32 2021-02-13T00:12:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:38Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:38Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:38Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:38Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:38Z- handleDvl() - RDI_Pathfinder msg: 1613175158.63 epoch sec; seqNo:5193 2021-02-13T00:12:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:38Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.01 , -0.23 2021-02-13T00:12:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175158.64 epoch sec; seqNo:10598; 4058626.35 north; 592149.51 east 2021-02-13T00:12:39Z- AHRS_M2 msg: 1613175159.02 epoch sec; seqNo:10510 2021-02-13T00:12:39Z- AHRS_M2 msg: 0.78 phi; -0.34 theta; -0.18 psi 2021-02-13T00:12:39Z- handleDepth()-Depth_Keller msg: 1613175159025 epoch sec; seqNo:10599; depth 16.45 2021-02-13T00:12:39Z- handleDvl() - RDI_Pathfinder msg: 1613175159.04 epoch sec; seqNo:5194 2021-02-13T00:12:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.14 , 97.72 , 71.94 , 111.25 2021-02-13T00:12:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.15 , -0.25 2021-02-13T00:12:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175159.05 epoch sec; seqNo:10599; 4058626.73 north; 592149.51 east 2021-02-13T00:12:39Z- LcmTrn::handleMessages() - handling 43 messages took 0.63 ms and 131 calls... 2021-02-13T00:12:39Z- waiting for 1613175157.20, time is 1613175159.05 2021-02-13T00:12:39Z- LcmTrn::updateTrn() equating pose 1613175159.05 and meas times 1613175159.05 2021-02-13T00:12:39Z- LcmTrn::updateTrn() >>>> pose time 1613175159.05, meas time 1613175159.05 <<<< 2021-02-13T00:12:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175159.05 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:12:39Z- TerrainNav::motionUpdate() - currEstimate = (4058627, 592149.51, 16.45) 2021-02-13T00:12:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:12:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:12:39Z- TNavBF::Projecting Measurements 2021-02-13T00:12:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:12:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:12:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:12:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:12:39Z- TNavFilter::4 good beams. 2021-02-13T00:12:39Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:12:39Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:12:39Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:12:39Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:12:39Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:12:39Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:12:39Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:12:39Z- TNavBF::mmse selected from filter -1 2021-02-13T00:12:39Z- TerrainMapOctree: (vehN, vehE ) = (4058626.73, 592149.51). 2021-02-13T00:12:39Z- TerrainMapOctree: Min Distance = 3360.60. 2021-02-13T00:12:39Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:12:39Z- There is no specific good beam that all particles have in common. 2021-02-13T00:12:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:12:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.64 K: 0.120 ExpectedMeasDiff: nan currMeas.covariance: 10.28 2021-02-13T00:12:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:12:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.44 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:12:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:12:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.117 ExpectedMeasDiff: nan currMeas.covariance: 8.28 2021-02-13T00:12:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:12:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 19.80 2021-02-13T00:12:39Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:12:39Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:12:39Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:12:39Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:12:39Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:12:39Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:12:39Z- Filter 0 No beams used in subcloud update 2021-02-13T00:12:39Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:12:39Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:12:39Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 714.1 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:12:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175159.05 sec. 2021-02-13T00:12:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:12:39Z- TNavBF::mmse selected from filter -1 2021-02-13T00:12:39Z- variances([0], [2], and psi_berg) = 714.078603 833.005925 0.000000 2021-02-13T00:12:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:12:39Z- LcmTrn::publishEstimates() - MLE : 4058625.23 592106.01 16.45 2021-02-13T00:12:39Z- LcmTrn::publishEstimates() - MMSE : 4058626.73 592149.51 16.45 2021-02-13T00:12:39Z- LcmTrn::publishEstimates() - COVAR: 714.08 833.01 1.11 0.00 2021-02-13T00:12:39Z- AHRS_M2 msg: 1613175159.42 epoch sec; seqNo:10511 2021-02-13T00:12:39Z- AHRS_M2 msg: 0.78 phi; -0.34 theta; -0.18 psi 2021-02-13T00:12:39Z- handleDepth()-Depth_Keller msg: 1613175159430 epoch sec; seqNo:10600; depth 16.59 2021-02-13T00:12:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175159.44 epoch sec; seqNo:10600; 4058627.11 north; 592149.50 east 2021-02-13T00:12:39Z- AHRS_M2 msg: 1613175159.83 epoch sec; seqNo:10512 2021-02-13T00:12:39Z- AHRS_M2 msg: 0.78 phi; -0.34 theta; -0.18 psi 2021-02-13T00:12:39Z- handleDepth()-Depth_Keller msg: 1613175159837 epoch sec; seqNo:10601; depth 16.71 2021-02-13T00:12:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:39Z- handleDvl() - RDI_Pathfinder msg: 1613175159.84 epoch sec; seqNo:5195 2021-02-13T00:12:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.15 , -0.25 2021-02-13T00:12:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175159.85 epoch sec; seqNo:10601; 4058627.11 north; 592149.50 east 2021-02-13T00:12:40Z- AHRS_M2 msg: 1613175160.23 epoch sec; seqNo:10513 2021-02-13T00:12:40Z- AHRS_M2 msg: 0.78 phi; -0.34 theta; -0.18 psi 2021-02-13T00:12:40Z- handleDepth()-Depth_Keller msg: 1613175160241 epoch sec; seqNo:10602; depth 16.83 2021-02-13T00:12:40Z- handleDvl() - RDI_Pathfinder msg: 1613175160.25 epoch sec; seqNo:5196 2021-02-13T00:12:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.46 , 97.72 , 72.76 , 108.68 2021-02-13T00:12:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.24 , -0.15 2021-02-13T00:12:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175160.26 epoch sec; seqNo:10602; 4058627.50 north; 592150.72 east 2021-02-13T00:12:40Z- AHRS_M2 msg: 1613175160.64 epoch sec; seqNo:10514 2021-02-13T00:12:40Z- AHRS_M2 msg: 0.78 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:40Z- handleDepth()-Depth_Keller msg: 1613175160642 epoch sec; seqNo:10603; depth 16.95 2021-02-13T00:12:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175160.65 epoch sec; seqNo:10603; 4058627.88 north; 592150.71 east 2021-02-13T00:12:41Z- handleDepth()-Depth_Keller msg: 1613175161069 epoch sec; seqNo:10604; depth 17.09 2021-02-13T00:12:41Z- handleDvl() - RDI_Pathfinder msg: 1613175161.08 epoch sec; seqNo:5197 2021-02-13T00:12:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.48 , 97.79 , 70.31 , 108.74 2021-02-13T00:12:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.26 , -0.01 2021-02-13T00:12:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175161.09 epoch sec; seqNo:10604; 4058628.26 north; 592150.71 east 2021-02-13T00:12:41Z- AHRS_M2 msg: 1613175161.44 epoch sec; seqNo:10515 2021-02-13T00:12:41Z- AHRS_M2 msg: 0.78 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:41Z- handleDepth()-Depth_Keller msg: 1613175161457 epoch sec; seqNo:10605; depth 17.22 2021-02-13T00:12:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175161.48 epoch sec; seqNo:10605; 4058628.64 north; 592150.71 east 2021-02-13T00:12:41Z- AHRS_M2 msg: 1613175161.85 epoch sec; seqNo:10516 2021-02-13T00:12:41Z- AHRS_M2 msg: 0.78 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:41Z- handleDepth()-Depth_Keller msg: 1613175161853 epoch sec; seqNo:10606; depth 17.34 2021-02-13T00:12:41Z- handleDvl() - RDI_Pathfinder msg: 1613175161.86 epoch sec; seqNo:5198 2021-02-13T00:12:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.67 , 95.42 , 71.53 , 108.81 2021-02-13T00:12:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.24 , 0.11 2021-02-13T00:12:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175161.87 epoch sec; seqNo:10606; 4058628.64 north; 592150.71 east 2021-02-13T00:12:42Z- AHRS_M2 msg: 1613175162.25 epoch sec; seqNo:10517 2021-02-13T00:12:42Z- AHRS_M2 msg: 0.77 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:42Z- handleDepth()-Depth_Keller msg: 1613175162265 epoch sec; seqNo:10607; depth 17.47 2021-02-13T00:12:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:42Z- handleDvl() - RDI_Pathfinder msg: 1613175162.27 epoch sec; seqNo:5199 2021-02-13T00:12:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.24 , 0.11 2021-02-13T00:12:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175162.28 epoch sec; seqNo:10607; 4058629.02 north; 592150.70 east 2021-02-13T00:12:42Z- AHRS_M2 msg: 1613175162.66 epoch sec; seqNo:10518 2021-02-13T00:12:42Z- AHRS_M2 msg: 0.77 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:42Z- handleDepth()-Depth_Keller msg: 1613175162665 epoch sec; seqNo:10608; depth 17.60 2021-02-13T00:12:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175162.68 epoch sec; seqNo:10608; 4058629.02 north; 592150.70 east 2021-02-13T00:12:43Z- AHRS_M2 msg: 1613175163.06 epoch sec; seqNo:10519 2021-02-13T00:12:43Z- AHRS_M2 msg: 0.77 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:43Z- handleDepth()-Depth_Keller msg: 1613175163069 epoch sec; seqNo:10609; depth 17.72 2021-02-13T00:12:43Z- handleDvl() - RDI_Pathfinder msg: 1613175163.08 epoch sec; seqNo:5200 2021-02-13T00:12:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.66 , 94.27 , 70.72 , 107.66 2021-02-13T00:12:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.15 , 0.17 2021-02-13T00:12:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175163.10 epoch sec; seqNo:10609; 4058629.40 north; 592150.70 east 2021-02-13T00:12:43Z- AHRS_M2 msg: 1613175163.46 epoch sec; seqNo:10520 2021-02-13T00:12:43Z- AHRS_M2 msg: 0.77 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:43Z- handleDepth()-Depth_Keller msg: 1613175163468 epoch sec; seqNo:10610; depth 17.85 2021-02-13T00:12:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:43Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:43Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:43Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:43Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:43Z- handleDvl() - RDI_Pathfinder msg: 1613175163.47 epoch sec; seqNo:5201 2021-02-13T00:12:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.15 , 0.17 2021-02-13T00:12:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175163.48 epoch sec; seqNo:10610; 4058629.78 north; 592150.69 east 2021-02-13T00:12:43Z- AHRS_M2 msg: 1613175163.88 epoch sec; seqNo:10521 2021-02-13T00:12:43Z- AHRS_M2 msg: 0.77 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:43Z- handleDepth()-Depth_Keller msg: 1613175163896 epoch sec; seqNo:10611; depth 17.96 2021-02-13T00:12:43Z- handleDvl() - RDI_Pathfinder msg: 1613175163.91 epoch sec; seqNo:5202 2021-02-13T00:12:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.66 , 97.92 , 0.00 , 106.44 2021-02-13T00:12:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.03 , 0.18 2021-02-13T00:12:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175163.92 epoch sec; seqNo:10611; 4058629.79 north; 592151.92 east 2021-02-13T00:12:44Z- AHRS_M2 msg: 1613175164.27 epoch sec; seqNo:10522 2021-02-13T00:12:44Z- AHRS_M2 msg: 0.77 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:44Z- handleDepth()-Depth_Keller msg: 1613175164281 epoch sec; seqNo:10612; depth 18.10 2021-02-13T00:12:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175164.29 epoch sec; seqNo:10612; 4058630.17 north; 592151.91 east 2021-02-13T00:12:44Z- LcmTrn::handleMessages() - handling 46 messages took 0.61 ms and 134 calls... 2021-02-13T00:12:44Z- waiting for 1613175162.05, time is 1613175164.29 2021-02-13T00:12:44Z- LcmTrn::updateTrn() >>>> pose time 1613175164.29, meas time 1613175163.91 <<<< 2021-02-13T00:12:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175163.91 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:12:44Z- TerrainNav::motionUpdate() - currEstimate = (4058630, 592151.91, 18.10) 2021-02-13T00:12:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:12:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:12:44Z- TNavBF::Projecting Measurements 2021-02-13T00:12:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:12:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:12:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:12:44Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:12:44Z- TNavFilter::3 good beams. 2021-02-13T00:12:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:12:44Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:12:44Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:12:44Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:12:44Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:12:44Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:12:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:12:44Z- TNavBF::mmse selected from filter -1 2021-02-13T00:12:44Z- TerrainMapOctree: (vehN, vehE ) = (4058629.92, 592151.74). 2021-02-13T00:12:44Z- TerrainMapOctree: Min Distance = 3356.72. 2021-02-13T00:12:44Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:12:44Z- There is no specific good beam that all particles have in common. 2021-02-13T00:12:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:12:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 10.15 2021-02-13T00:12:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:12:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:12:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:12:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 18.13 2021-02-13T00:12:44Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:12:44Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:12:44Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:12:44Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:12:44Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:12:44Z- Filter 0 No beams used in subcloud update 2021-02-13T00:12:44Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:12:44Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:12:44Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 714.0 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:12:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175163.91 sec. 2021-02-13T00:12:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:12:44Z- TNavBF::mmse selected from filter -1 2021-02-13T00:12:44Z- variances([0], [2], and psi_berg) = 713.998973 833.005877 0.000000 2021-02-13T00:12:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:12:44Z- LcmTrn::publishEstimates() - MLE : 4058628.67 592201.41 18.10 2021-02-13T00:12:44Z- LcmTrn::publishEstimates() - MMSE : 4058630.17 592151.91 18.10 2021-02-13T00:12:44Z- LcmTrn::publishEstimates() - COVAR: 714.00 833.01 1.06 0.00 2021-02-13T00:12:44Z- AHRS_M2 msg: 1613175164.68 epoch sec; seqNo:10523 2021-02-13T00:12:44Z- AHRS_M2 msg: 0.77 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:44Z- handleDepth()-Depth_Keller msg: 1613175164680 epoch sec; seqNo:10613; depth 18.23 2021-02-13T00:12:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:44Z- handleDvl() - RDI_Pathfinder msg: 1613175164.68 epoch sec; seqNo:5203 2021-02-13T00:12:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.03 , 0.18 2021-02-13T00:12:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175164.69 epoch sec; seqNo:10613; 4058630.17 north; 592151.91 east 2021-02-13T00:12:45Z- AHRS_M2 msg: 1613175165.08 epoch sec; seqNo:10524 2021-02-13T00:12:45Z- AHRS_M2 msg: 0.78 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:45Z- handleDepth()-Depth_Keller msg: 1613175165093 epoch sec; seqNo:10614; depth 18.34 2021-02-13T00:12:45Z- handleDvl() - RDI_Pathfinder msg: 1613175165.10 epoch sec; seqNo:5204 2021-02-13T00:12:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.52 , 78.58 , 108.34 2021-02-13T00:12:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.10 , 0.16 2021-02-13T00:12:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175165.12 epoch sec; seqNo:10614; 4058630.55 north; 592151.91 east 2021-02-13T00:12:45Z- AHRS_M2 msg: 1613175165.48 epoch sec; seqNo:10525 2021-02-13T00:12:45Z- AHRS_M2 msg: 0.78 phi; -0.34 theta; -0.18 psi 2021-02-13T00:12:45Z- handleDepth()-Depth_Keller msg: 1613175165488 epoch sec; seqNo:10615; depth 18.47 2021-02-13T00:12:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175165.51 epoch sec; seqNo:10615; 4058630.55 north; 592151.91 east 2021-02-13T00:12:45Z- AHRS_M2 msg: 1613175165.89 epoch sec; seqNo:10526 2021-02-13T00:12:45Z- AHRS_M2 msg: 0.78 phi; -0.34 theta; -0.18 psi 2021-02-13T00:12:45Z- handleDepth()-Depth_Keller msg: 1613175165892 epoch sec; seqNo:10616; depth 18.58 2021-02-13T00:12:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:45Z- handleDvl() - RDI_Pathfinder msg: 1613175165.90 epoch sec; seqNo:5205 2021-02-13T00:12:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.10 , 0.16 2021-02-13T00:12:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175165.91 epoch sec; seqNo:10616; 4058630.92 north; 592151.90 east 2021-02-13T00:12:46Z- AHRS_M2 msg: 1613175166.29 epoch sec; seqNo:10527 2021-02-13T00:12:46Z- AHRS_M2 msg: 0.79 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:46Z- handleDepth()-Depth_Keller msg: 1613175166301 epoch sec; seqNo:10617; depth 18.72 2021-02-13T00:12:46Z- handleDvl() - RDI_Pathfinder msg: 1613175166.31 epoch sec; seqNo:5206 2021-02-13T00:12:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 97.52 , 78.58 , 109.69 2021-02-13T00:12:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.16 , 0.08 2021-02-13T00:12:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175166.32 epoch sec; seqNo:10617; 4058630.94 north; 592153.12 east 2021-02-13T00:12:46Z- AHRS_M2 msg: 1613175166.70 epoch sec; seqNo:10528 2021-02-13T00:12:46Z- AHRS_M2 msg: 0.79 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:46Z- handleDepth()-Depth_Keller msg: 1613175166703 epoch sec; seqNo:10618; depth 18.83 2021-02-13T00:12:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175166.71 epoch sec; seqNo:10618; 4058631.32 north; 592153.12 east 2021-02-13T00:12:47Z- AHRS_M2 msg: 1613175167.10 epoch sec; seqNo:10529 2021-02-13T00:12:47Z- AHRS_M2 msg: 0.80 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:47Z- handleDepth()-Depth_Keller msg: 1613175167104 epoch sec; seqNo:10619; depth 18.96 2021-02-13T00:12:47Z- handleDvl() - RDI_Pathfinder msg: 1613175167.13 epoch sec; seqNo:5207 2021-02-13T00:12:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.48 , 97.65 , 78.18 , 108.61 2021-02-13T00:12:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.15 , -0.04 2021-02-13T00:12:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175167.14 epoch sec; seqNo:10619; 4058631.32 north; 592153.12 east 2021-02-13T00:12:47Z- AHRS_M2 msg: 1613175167.51 epoch sec; seqNo:10530 2021-02-13T00:12:47Z- AHRS_M2 msg: 0.80 phi; -0.34 theta; -0.18 psi 2021-02-13T00:12:47Z- handleDepth()-Depth_Keller msg: 1613175167511 epoch sec; seqNo:10620; depth 19.08 2021-02-13T00:12:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175167.52 epoch sec; seqNo:10620; 4058631.70 north; 592153.12 east 2021-02-13T00:12:47Z- AHRS_M2 msg: 1613175167.91 epoch sec; seqNo:10531 2021-02-13T00:12:47Z- AHRS_M2 msg: 0.81 phi; -0.34 theta; -0.18 psi 2021-02-13T00:12:47Z- handleDepth()-Depth_Keller msg: 1613175167913 epoch sec; seqNo:10621; depth 19.21 2021-02-13T00:12:47Z- handleDvl() - RDI_Pathfinder msg: 1613175167.92 epoch sec; seqNo:5208 2021-02-13T00:12:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.85 , 97.72 , 69.49 , 108.68 2021-02-13T00:12:47Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.04 , -0.11 2021-02-13T00:12:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175167.93 epoch sec; seqNo:10621; 4058631.71 north; 592154.34 east 2021-02-13T00:12:48Z- AHRS_M2 msg: 1613175168.31 epoch sec; seqNo:10532 2021-02-13T00:12:48Z- AHRS_M2 msg: 0.81 phi; -0.34 theta; -0.18 psi 2021-02-13T00:12:48Z- handleDepth()-Depth_Keller msg: 1613175168318 epoch sec; seqNo:10622; depth 19.35 2021-02-13T00:12:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:48Z- handleDvl() - RDI_Pathfinder msg: 1613175168.32 epoch sec; seqNo:5209 2021-02-13T00:12:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:48Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.04 , -0.11 2021-02-13T00:12:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175168.33 epoch sec; seqNo:10622; 4058632.09 north; 592154.33 east 2021-02-13T00:12:48Z- AHRS_M2 msg: 1613175168.71 epoch sec; seqNo:10533 2021-02-13T00:12:48Z- AHRS_M2 msg: 0.82 phi; -0.34 theta; -0.18 psi 2021-02-13T00:12:48Z- handleDepth()-Depth_Keller msg: 1613175168725 epoch sec; seqNo:10623; depth 19.48 2021-02-13T00:12:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175168.74 epoch sec; seqNo:10623; 4058632.47 north; 592154.33 east 2021-02-13T00:12:49Z- AHRS_M2 msg: 1613175169.12 epoch sec; seqNo:10534 2021-02-13T00:12:49Z- AHRS_M2 msg: 0.82 phi; -0.34 theta; -0.18 psi 2021-02-13T00:12:49Z- handleDepth()-Depth_Keller msg: 1613175169127 epoch sec; seqNo:10624; depth 19.61 2021-02-13T00:12:49Z- handleDvl() - RDI_Pathfinder msg: 1613175169.14 epoch sec; seqNo:5210 2021-02-13T00:12:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 97.92 , 70.31 , 105.23 2021-02-13T00:12:49Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.06 , -0.13 2021-02-13T00:12:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175169.16 epoch sec; seqNo:10624; 4058632.85 north; 592154.32 east 2021-02-13T00:12:49Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-13T00:12:49Z- waiting for 1613175167.29, time is 1613175169.16 2021-02-13T00:12:49Z- LcmTrn::updateTrn() equating pose 1613175169.16 and meas times 1613175169.16 2021-02-13T00:12:49Z- LcmTrn::updateTrn() >>>> pose time 1613175169.16, meas time 1613175169.16 <<<< 2021-02-13T00:12:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175169.16 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:12:49Z- TerrainNav::motionUpdate() - currEstimate = (4058633, 592154.32, 19.61) 2021-02-13T00:12:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:12:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:12:49Z- TNavBF::Projecting Measurements 2021-02-13T00:12:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:12:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:12:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:12:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:12:49Z- TNavFilter::4 good beams. 2021-02-13T00:12:49Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:12:49Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:12:49Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:12:49Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:12:49Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:12:49Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:12:49Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:12:49Z- TNavBF::mmse selected from filter -1 2021-02-13T00:12:49Z- TerrainMapOctree: (vehN, vehE ) = (4058632.85, 592154.32). 2021-02-13T00:12:49Z- TerrainMapOctree: Min Distance = 3352.81. 2021-02-13T00:12:49Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:12:49Z- There is no specific good beam that all particles have in common. 2021-02-13T00:12:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:12:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T00:12:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:12:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:12:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:12:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 7.91 2021-02-13T00:12:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:12:49Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 17.72 2021-02-13T00:12:49Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:12:49Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:12:49Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:12:49Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:12:49Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:12:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:12:49Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:12:49Z- Filter 0 No beams used in subcloud update 2021-02-13T00:12:49Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:12:49Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:12:49Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 713.9 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:12:49Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175169.16 sec. 2021-02-13T00:12:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:12:49Z- TNavBF::mmse selected from filter -1 2021-02-13T00:12:49Z- variances([0], [2], and psi_berg) = 713.939119 833.005840 0.000000 2021-02-13T00:12:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:12:49Z- LcmTrn::publishEstimates() - MLE : 4058631.35 592203.82 19.61 2021-02-13T00:12:49Z- LcmTrn::publishEstimates() - MMSE : 4058632.85 592154.32 19.61 2021-02-13T00:12:49Z- LcmTrn::publishEstimates() - COVAR: 713.94 833.01 0.91 0.00 2021-02-13T00:12:49Z- AHRS_M2 msg: 1613175169.52 epoch sec; seqNo:10535 2021-02-13T00:12:49Z- AHRS_M2 msg: 0.83 phi; -0.34 theta; -0.18 psi 2021-02-13T00:12:49Z- handleDepth()-Depth_Keller msg: 1613175169528 epoch sec; seqNo:10625; depth 19.75 2021-02-13T00:12:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:49Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:49Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:49Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:49Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:49Z- handleDvl() - RDI_Pathfinder msg: 1613175169.53 epoch sec; seqNo:5211 2021-02-13T00:12:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:49Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.06 , -0.13 2021-02-13T00:12:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175169.54 epoch sec; seqNo:10625; 4058632.86 north; 592155.54 east 2021-02-13T00:12:49Z- AHRS_M2 msg: 1613175169.93 epoch sec; seqNo:10536 2021-02-13T00:12:49Z- AHRS_M2 msg: 0.83 phi; -0.33 theta; -0.17 psi 2021-02-13T00:12:49Z- handleDepth()-Depth_Keller msg: 1613175169941 epoch sec; seqNo:10626; depth 19.86 2021-02-13T00:12:49Z- handleDvl() - RDI_Pathfinder msg: 1613175169.95 epoch sec; seqNo:5212 2021-02-13T00:12:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.03 , 97.92 , 78.45 , 106.44 2021-02-13T00:12:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.15 , -0.12 2021-02-13T00:12:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175169.96 epoch sec; seqNo:10626; 4058633.24 north; 592155.54 east 2021-02-13T00:12:50Z- AHRS_M2 msg: 1613175170.33 epoch sec; seqNo:10537 2021-02-13T00:12:50Z- AHRS_M2 msg: 0.84 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:50Z- handleDepth()-Depth_Keller msg: 1613175170337 epoch sec; seqNo:10627; depth 20.00 2021-02-13T00:12:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175170.36 epoch sec; seqNo:10627; 4058633.62 north; 592155.54 east 2021-02-13T00:12:50Z- AHRS_M2 msg: 1613175170.74 epoch sec; seqNo:10538 2021-02-13T00:12:50Z- AHRS_M2 msg: 0.84 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:50Z- handleDepth()-Depth_Keller msg: 1613175170740 epoch sec; seqNo:10628; depth 20.12 2021-02-13T00:12:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:50Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:50Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:50Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:50Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:50Z- handleDvl() - RDI_Pathfinder msg: 1613175170.74 epoch sec; seqNo:5213 2021-02-13T00:12:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.15 , -0.12 2021-02-13T00:12:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175170.75 epoch sec; seqNo:10628; 4058633.99 north; 592155.53 east 2021-02-13T00:12:51Z- AHRS_M2 msg: 1613175171.14 epoch sec; seqNo:10539 2021-02-13T00:12:51Z- AHRS_M2 msg: 0.84 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:51Z- handleDepth()-Depth_Keller msg: 1613175171149 epoch sec; seqNo:10629; depth 20.25 2021-02-13T00:12:51Z- handleDvl() - RDI_Pathfinder msg: 1613175171.16 epoch sec; seqNo:5214 2021-02-13T00:12:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.62 , 94.14 , 69.49 , 102.66 2021-02-13T00:12:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.16 , -0.09 2021-02-13T00:12:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175171.17 epoch sec; seqNo:10629; 4058634.37 north; 592155.53 east 2021-02-13T00:12:51Z- AHRS_M2 msg: 1613175171.54 epoch sec; seqNo:10540 2021-02-13T00:12:51Z- AHRS_M2 msg: 0.84 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:51Z- handleDepth()-Depth_Keller msg: 1613175171557 epoch sec; seqNo:10630; depth 20.37 2021-02-13T00:12:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175171.57 epoch sec; seqNo:10630; 4058634.39 north; 592156.75 east 2021-02-13T00:12:51Z- AHRS_M2 msg: 1613175171.95 epoch sec; seqNo:10541 2021-02-13T00:12:51Z- AHRS_M2 msg: 0.84 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:51Z- handleDepth()-Depth_Keller msg: 1613175171953 epoch sec; seqNo:10631; depth 20.49 2021-02-13T00:12:51Z- handleDvl() - RDI_Pathfinder msg: 1613175171.96 epoch sec; seqNo:5215 2021-02-13T00:12:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 98.19 , 68.26 , 104.28 2021-02-13T00:12:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.13 , -0.06 2021-02-13T00:12:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175171.97 epoch sec; seqNo:10631; 4058634.77 north; 592156.75 east 2021-02-13T00:12:52Z- AHRS_M2 msg: 1613175172.35 epoch sec; seqNo:10542 2021-02-13T00:12:52Z- AHRS_M2 msg: 0.84 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:52Z- handleDepth()-Depth_Keller msg: 1613175172361 epoch sec; seqNo:10632; depth 20.62 2021-02-13T00:12:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175172.37 epoch sec; seqNo:10632; 4058635.14 north; 592156.74 east 2021-02-13T00:12:52Z- AHRS_M2 msg: 1613175172.76 epoch sec; seqNo:10543 2021-02-13T00:12:52Z- AHRS_M2 msg: 0.84 phi; -0.34 theta; -0.17 psi 2021-02-13T00:12:52Z- handleDepth()-Depth_Keller msg: 1613175172761 epoch sec; seqNo:10633; depth 20.75 2021-02-13T00:12:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175172.78 epoch sec; seqNo:10633; 4058635.14 north; 592156.74 east 2021-02-13T00:12:53Z- AHRS_M2 msg: 1613175173.18 epoch sec; seqNo:10544 2021-02-13T00:12:53Z- AHRS_M2 msg: 0.83 phi; -0.34 theta; -0.18 psi 2021-02-13T00:12:53Z- handleDepth()-Depth_Keller msg: 1613175173190 epoch sec; seqNo:10634; depth 20.85 2021-02-13T00:12:53Z- handleDvl() - RDI_Pathfinder msg: 1613175173.20 epoch sec; seqNo:5216 2021-02-13T00:12:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.99 , 67.04 , 105.29 2021-02-13T00:12:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.09 , 0.00 2021-02-13T00:12:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175173.21 epoch sec; seqNo:10634; 4058635.52 north; 592156.74 east 2021-02-13T00:12:53Z- AHRS_M2 msg: 1613175173.57 epoch sec; seqNo:10545 2021-02-13T00:12:53Z- AHRS_M2 msg: 0.83 phi; -0.34 theta; -0.19 psi 2021-02-13T00:12:53Z- handleDepth()-Depth_Keller msg: 1613175173585 epoch sec; seqNo:10635; depth 21.00 2021-02-13T00:12:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175173.60 epoch sec; seqNo:10635; 4058635.90 north; 592156.73 east 2021-02-13T00:12:53Z- AHRS_M2 msg: 1613175173.98 epoch sec; seqNo:10546 2021-02-13T00:12:53Z- AHRS_M2 msg: 0.83 phi; -0.34 theta; -0.19 psi 2021-02-13T00:12:53Z- handleDepth()-Depth_Keller msg: 1613175173988 epoch sec; seqNo:10636; depth 21.12 2021-02-13T00:12:54Z- handleDvl() - RDI_Pathfinder msg: 1613175174.00 epoch sec; seqNo:5217 2021-02-13T00:12:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 93.12 , 78.51 , 105.29 2021-02-13T00:12:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.02 , 0.05 2021-02-13T00:12:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175174.01 epoch sec; seqNo:10636; 4058635.92 north; 592157.95 east 2021-02-13T00:12:54Z- AHRS_M2 msg: 1613175174.37 epoch sec; seqNo:10547 2021-02-13T00:12:54Z- AHRS_M2 msg: 0.83 phi; -0.35 theta; -0.19 psi 2021-02-13T00:12:54Z- handleDepth()-Depth_Keller msg: 1613175174381 epoch sec; seqNo:10637; depth 21.26 2021-02-13T00:12:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:54Z- handleDvl() - RDI_Pathfinder msg: 1613175174.38 epoch sec; seqNo:5218 2021-02-13T00:12:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.02 , 0.05 2021-02-13T00:12:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175174.39 epoch sec; seqNo:10637; 4058636.29 north; 592157.95 east 2021-02-13T00:12:54Z- LcmTrn::handleMessages() - handling 47 messages took 0.66 ms and 134 calls... 2021-02-13T00:12:54Z- waiting for 1613175172.16, time is 1613175174.39 2021-02-13T00:12:54Z- LcmTrn::updateTrn() equating pose 1613175174.39 and meas times 1613175174.39 2021-02-13T00:12:54Z- LcmTrn::updateTrn() >>>> pose time 1613175174.39, meas time 1613175174.39 <<<< 2021-02-13T00:12:54Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175174.39, ping # 0. 2021-02-13T00:12:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175174.39 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:12:54Z- TerrainNav::motionUpdate() - currEstimate = (4058636, 592157.95, 21.26) 2021-02-13T00:12:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:12:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:12:54Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:12:54Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:12:54Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175174.39 sec.; No relevant navigation data available 2021-02-13T00:12:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:12:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:12:54Z- variances([0], [2], and psi_berg) = 713.868646 833.005797 0.000000 2021-02-13T00:12:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:12:54Z- LcmTrn::publishEstimates() - MLE : 4058634.79 592207.45 21.26 2021-02-13T00:12:54Z- LcmTrn::publishEstimates() - MMSE : 4058636.29 592157.95 21.26 2021-02-13T00:12:54Z- LcmTrn::publishEstimates() - COVAR: 713.87 833.01 1.16 0.00 2021-02-13T00:12:54Z- AHRS_M2 msg: 1613175174.78 epoch sec; seqNo:10548 2021-02-13T00:12:54Z- AHRS_M2 msg: 0.83 phi; -0.35 theta; -0.19 psi 2021-02-13T00:12:54Z- handleDepth()-Depth_Keller msg: 1613175174779 epoch sec; seqNo:10638; depth 21.38 2021-02-13T00:12:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175174.79 epoch sec; seqNo:10638; 4058636.29 north; 592157.95 east 2021-02-13T00:12:55Z- AHRS_M2 msg: 1613175175.18 epoch sec; seqNo:10549 2021-02-13T00:12:55Z- AHRS_M2 msg: 0.83 phi; -0.35 theta; -0.20 psi 2021-02-13T00:12:55Z- handleDepth()-Depth_Keller msg: 1613175175190 epoch sec; seqNo:10639; depth 21.49 2021-02-13T00:12:55Z- handleDvl() - RDI_Pathfinder msg: 1613175175.20 epoch sec; seqNo:5219 2021-02-13T00:12:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 97.86 , 78.38 , 105.16 2021-02-13T00:12:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.02 , 0.07 2021-02-13T00:12:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175175.21 epoch sec; seqNo:10639; 4058636.67 north; 592157.95 east 2021-02-13T00:12:55Z- AHRS_M2 msg: 1613175175.59 epoch sec; seqNo:10550 2021-02-13T00:12:55Z- AHRS_M2 msg: 0.84 phi; -0.35 theta; -0.20 psi 2021-02-13T00:12:55Z- handleDepth()-Depth_Keller msg: 1613175175589 epoch sec; seqNo:10640; depth 21.62 2021-02-13T00:12:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:55Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:55Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:55Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:55Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:55Z- handleDvl() - RDI_Pathfinder msg: 1613175175.59 epoch sec; seqNo:5220 2021-02-13T00:12:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.02 , 0.07 2021-02-13T00:12:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175175.60 epoch sec; seqNo:10640; 4058636.67 north; 592157.95 east 2021-02-13T00:12:55Z- AHRS_M2 msg: 1613175175.99 epoch sec; seqNo:10551 2021-02-13T00:12:55Z- AHRS_M2 msg: 0.84 phi; -0.35 theta; -0.20 psi 2021-02-13T00:12:56Z- handleDepth()-Depth_Keller msg: 1613175176001 epoch sec; seqNo:10641; depth 21.75 2021-02-13T00:12:56Z- handleDvl() - RDI_Pathfinder msg: 1613175176.01 epoch sec; seqNo:5221 2021-02-13T00:12:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.40 , 97.79 , 68.26 , 103.88 2021-02-13T00:12:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.06 , 0.03 2021-02-13T00:12:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175176.02 epoch sec; seqNo:10641; 4058637.05 north; 592157.94 east 2021-02-13T00:12:56Z- AHRS_M2 msg: 1613175176.39 epoch sec; seqNo:10552 2021-02-13T00:12:56Z- AHRS_M2 msg: 0.85 phi; -0.35 theta; -0.20 psi 2021-02-13T00:12:56Z- handleDepth()-Depth_Keller msg: 1613175176398 epoch sec; seqNo:10642; depth 21.88 2021-02-13T00:12:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175176.41 epoch sec; seqNo:10642; 4058637.07 north; 592159.16 east 2021-02-13T00:12:56Z- AHRS_M2 msg: 1613175176.80 epoch sec; seqNo:10553 2021-02-13T00:12:56Z- AHRS_M2 msg: 0.85 phi; -0.35 theta; -0.20 psi 2021-02-13T00:12:56Z- handleDepth()-Depth_Keller msg: 1613175176805 epoch sec; seqNo:10643; depth 22.01 2021-02-13T00:12:56Z- handleDvl() - RDI_Pathfinder msg: 1613175176.83 epoch sec; seqNo:5222 2021-02-13T00:12:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.99 , 98.13 , 67.86 , 104.21 2021-02-13T00:12:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , -0.02 2021-02-13T00:12:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175176.84 epoch sec; seqNo:10643; 4058637.44 north; 592159.16 east 2021-02-13T00:12:57Z- AHRS_M2 msg: 1613175177.20 epoch sec; seqNo:10554 2021-02-13T00:12:57Z- AHRS_M2 msg: 0.85 phi; -0.35 theta; -0.20 psi 2021-02-13T00:12:57Z- handleDepth()-Depth_Keller msg: 1613175177204 epoch sec; seqNo:10644; depth 22.14 2021-02-13T00:12:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175177.22 epoch sec; seqNo:10644; 4058637.44 north; 592159.16 east 2021-02-13T00:12:57Z- AHRS_M2 msg: 1613175177.61 epoch sec; seqNo:10555 2021-02-13T00:12:57Z- AHRS_M2 msg: 0.86 phi; -0.35 theta; -0.19 psi 2021-02-13T00:12:57Z- handleDepth()-Depth_Keller msg: 1613175177617 epoch sec; seqNo:10645; depth 22.26 2021-02-13T00:12:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175177.62 epoch sec; seqNo:10645; 4058637.82 north; 592159.15 east 2021-02-13T00:12:58Z- AHRS_M2 msg: 1613175178.01 epoch sec; seqNo:10556 2021-02-13T00:12:58Z- AHRS_M2 msg: 0.86 phi; -0.34 theta; -0.19 psi 2021-02-13T00:12:58Z- handleDepth()-Depth_Keller msg: 1613175178012 epoch sec; seqNo:10646; depth 22.41 2021-02-13T00:12:58Z- handleDvl() - RDI_Pathfinder msg: 1613175178.03 epoch sec; seqNo:5223 2021-02-13T00:12:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.58 , 98.13 , 67.45 , 103.00 2021-02-13T00:12:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.00 , -0.05 2021-02-13T00:12:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175178.04 epoch sec; seqNo:10646; 4058637.84 north; 592160.37 east 2021-02-13T00:12:58Z- AHRS_M2 msg: 1613175178.41 epoch sec; seqNo:10557 2021-02-13T00:12:58Z- AHRS_M2 msg: 0.86 phi; -0.34 theta; -0.19 psi 2021-02-13T00:12:58Z- handleDepth()-Depth_Keller msg: 1613175178416 epoch sec; seqNo:10647; depth 22.53 2021-02-13T00:12:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175178.43 epoch sec; seqNo:10647; 4058638.21 north; 592160.37 east 2021-02-13T00:12:58Z- AHRS_M2 msg: 1613175178.84 epoch sec; seqNo:10558 2021-02-13T00:12:58Z- AHRS_M2 msg: 0.85 phi; -0.34 theta; -0.19 psi 2021-02-13T00:12:58Z- handleDepth()-Depth_Keller msg: 1613175178845 epoch sec; seqNo:10648; depth 22.67 2021-02-13T00:12:58Z- handleDvl() - RDI_Pathfinder msg: 1613175178.86 epoch sec; seqNo:5224 2021-02-13T00:12:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 93.26 , 67.45 , 104.21 2021-02-13T00:12:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , -0.02 2021-02-13T00:12:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175178.87 epoch sec; seqNo:10648; 4058638.59 north; 592160.37 east 2021-02-13T00:12:59Z- AHRS_M2 msg: 1613175179.22 epoch sec; seqNo:10559 2021-02-13T00:12:59Z- AHRS_M2 msg: 0.84 phi; -0.35 theta; -0.19 psi 2021-02-13T00:12:59Z- handleDepth()-Depth_Keller msg: 1613175179227 epoch sec; seqNo:10649; depth 22.80 2021-02-13T00:12:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:12:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:12:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:12:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:12:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:12:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:12:59Z- handleDvl() - RDI_Pathfinder msg: 1613175179.23 epoch sec; seqNo:5225 2021-02-13T00:12:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:12:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , -0.02 2021-02-13T00:12:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175179.24 epoch sec; seqNo:10649; 4058638.59 north; 592160.37 east 2021-02-13T00:12:59Z- AHRS_M2 msg: 1613175179.62 epoch sec; seqNo:10560 2021-02-13T00:12:59Z- AHRS_M2 msg: 0.83 phi; -0.35 theta; -0.19 psi 2021-02-13T00:12:59Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-13T00:12:59Z- waiting for 1613175177.39, time is 1613175179.62 2021-02-13T00:12:59Z- LcmTrn::updateTrn() >>>> pose time 1613175179.62, meas time 1613175179.23 <<<< 2021-02-13T00:12:59Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175179.23, ping # 0. 2021-02-13T00:12:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175179.23 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:12:59Z- TerrainNav::motionUpdate() - currEstimate = (4058639, 592160.37, 22.80) 2021-02-13T00:12:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:12:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:12:59Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:12:59Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:12:59Z- TNavBF::Projecting Measurements 2021-02-13T00:12:59Z- TNavFilter::Measurement from time = 1613175179.23 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:12:59Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:12:59Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:12:59Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:12:59Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:12:59Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:12:59Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:12:59Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:12:59Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 713.8 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:12:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:12:59Z- TNavBF::mmse selected from filter -1 2021-02-13T00:12:59Z- variances([0], [2], and psi_berg) = 713.797574 833.005754 0.000000 2021-02-13T00:12:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:12:59Z- LcmTrn::publishEstimates() - MLE : 4058637.09 592209.87 22.80 2021-02-13T00:12:59Z- LcmTrn::publishEstimates() - MMSE : 4058638.59 592160.37 22.80 2021-02-13T00:12:59Z- LcmTrn::publishEstimates() - COVAR: 713.80 833.01 1.33 0.00 2021-02-13T00:12:59Z- handleDepth()-Depth_Keller msg: 1613175179633 epoch sec; seqNo:10650; depth 22.91 2021-02-13T00:12:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175179.65 epoch sec; seqNo:10650; 4058638.97 north; 592160.36 east 2021-02-13T00:13:00Z- AHRS_M2 msg: 1613175180.03 epoch sec; seqNo:10561 2021-02-13T00:13:00Z- AHRS_M2 msg: 0.82 phi; -0.35 theta; -0.20 psi 2021-02-13T00:13:00Z- handleDepth()-Depth_Keller msg: 1613175180034 epoch sec; seqNo:10651; depth 23.02 2021-02-13T00:13:00Z- handleDvl() - RDI_Pathfinder msg: 1613175180.05 epoch sec; seqNo:5226 2021-02-13T00:13:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.18 , 98.12 , 66.64 , 102.99 2021-02-13T00:13:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , 0.03 2021-02-13T00:13:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175180.07 epoch sec; seqNo:10651; 4058638.99 north; 592161.58 east 2021-02-13T00:13:00Z- AHRS_M2 msg: 1613175180.43 epoch sec; seqNo:10562 2021-02-13T00:13:00Z- AHRS_M2 msg: 0.81 phi; -0.35 theta; -0.21 psi 2021-02-13T00:13:00Z- handleDepth()-Depth_Keller msg: 1613175180436 epoch sec; seqNo:10652; depth 23.17 2021-02-13T00:13:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:00Z- handleDvl() - RDI_Pathfinder msg: 1613175180.44 epoch sec; seqNo:5227 2021-02-13T00:13:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , 0.03 2021-02-13T00:13:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175180.45 epoch sec; seqNo:10652; 4058639.36 north; 592161.58 east 2021-02-13T00:13:00Z- AHRS_M2 msg: 1613175180.85 epoch sec; seqNo:10563 2021-02-13T00:13:00Z- AHRS_M2 msg: 0.80 phi; -0.35 theta; -0.21 psi 2021-02-13T00:13:00Z- handleDepth()-Depth_Keller msg: 1613175180855 epoch sec; seqNo:10653; depth 23.27 2021-02-13T00:13:00Z- handleDvl() - RDI_Pathfinder msg: 1613175180.87 epoch sec; seqNo:5228 2021-02-13T00:13:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.71 , 98.19 , 67.04 , 104.28 2021-02-13T00:13:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.05 , 0.05 2021-02-13T00:13:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175180.88 epoch sec; seqNo:10653; 4058639.36 north; 592161.58 east 2021-02-13T00:13:01Z- AHRS_M2 msg: 1613175181.24 epoch sec; seqNo:10564 2021-02-13T00:13:01Z- AHRS_M2 msg: 0.79 phi; -0.35 theta; -0.22 psi 2021-02-13T00:13:01Z- handleDepth()-Depth_Keller msg: 1613175181254 epoch sec; seqNo:10654; depth 23.41 2021-02-13T00:13:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175181.27 epoch sec; seqNo:10654; 4058639.74 north; 592161.58 east 2021-02-13T00:13:01Z- AHRS_M2 msg: 1613175181.65 epoch sec; seqNo:10565 2021-02-13T00:13:01Z- AHRS_M2 msg: 0.79 phi; -0.35 theta; -0.23 psi 2021-02-13T00:13:01Z- handleDepth()-Depth_Keller msg: 1613175181651 epoch sec; seqNo:10655; depth 23.53 2021-02-13T00:13:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:01Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:01Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:01Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:01Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:01Z- handleDvl() - RDI_Pathfinder msg: 1613175181.65 epoch sec; seqNo:5229 2021-02-13T00:13:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.05 , 0.05 2021-02-13T00:13:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175181.66 epoch sec; seqNo:10655; 4058639.74 north; 592161.58 east 2021-02-13T00:13:02Z- AHRS_M2 msg: 1613175182.05 epoch sec; seqNo:10566 2021-02-13T00:13:02Z- AHRS_M2 msg: 0.78 phi; -0.34 theta; -0.23 psi 2021-02-13T00:13:02Z- handleDepth()-Depth_Keller msg: 1613175182057 epoch sec; seqNo:10656; depth 23.64 2021-02-13T00:13:02Z- handleDvl() - RDI_Pathfinder msg: 1613175182.07 epoch sec; seqNo:5230 2021-02-13T00:13:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.94 , 98.06 , 66.63 , 102.93 2021-02-13T00:13:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.10 , -0.01 2021-02-13T00:13:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175182.08 epoch sec; seqNo:10656; 4058640.14 north; 592162.79 east 2021-02-13T00:13:02Z- AHRS_M2 msg: 1613175182.45 epoch sec; seqNo:10567 2021-02-13T00:13:02Z- AHRS_M2 msg: 0.78 phi; -0.34 theta; -0.24 psi 2021-02-13T00:13:02Z- handleDepth()-Depth_Keller msg: 1613175182467 epoch sec; seqNo:10657; depth 23.77 2021-02-13T00:13:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175182.48 epoch sec; seqNo:10657; 4058640.51 north; 592162.79 east 2021-02-13T00:13:02Z- AHRS_M2 msg: 1613175182.86 epoch sec; seqNo:10568 2021-02-13T00:13:02Z- AHRS_M2 msg: 0.78 phi; -0.34 theta; -0.24 psi 2021-02-13T00:13:02Z- handleDepth()-Depth_Keller msg: 1613175182861 epoch sec; seqNo:10658; depth 23.90 2021-02-13T00:13:02Z- handleDvl() - RDI_Pathfinder msg: 1613175182.87 epoch sec; seqNo:5231 2021-02-13T00:13:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.72 , 98.19 , 67.04 , 103.06 2021-02-13T00:13:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , -0.08 2021-02-13T00:13:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175182.89 epoch sec; seqNo:10658; 4058640.51 north; 592162.79 east 2021-02-13T00:13:03Z- AHRS_M2 msg: 1613175183.26 epoch sec; seqNo:10569 2021-02-13T00:13:03Z- AHRS_M2 msg: 0.78 phi; -0.34 theta; -0.24 psi 2021-02-13T00:13:03Z- handleDepth()-Depth_Keller msg: 1613175183265 epoch sec; seqNo:10659; depth 24.02 2021-02-13T00:13:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175183.28 epoch sec; seqNo:10659; 4058640.89 north; 592162.78 east 2021-02-13T00:13:03Z- AHRS_M2 msg: 1613175183.67 epoch sec; seqNo:10570 2021-02-13T00:13:03Z- AHRS_M2 msg: 0.78 phi; -0.33 theta; -0.23 psi 2021-02-13T00:13:03Z- handleDepth()-Depth_Keller msg: 1613175183675 epoch sec; seqNo:10660; depth 24.13 2021-02-13T00:13:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175183.68 epoch sec; seqNo:10660; 4058641.27 north; 592162.78 east 2021-02-13T00:13:04Z- AHRS_M2 msg: 1613175184.07 epoch sec; seqNo:10571 2021-02-13T00:13:04Z- AHRS_M2 msg: 0.78 phi; -0.33 theta; -0.23 psi 2021-02-13T00:13:04Z- handleDepth()-Depth_Keller msg: 1613175184082 epoch sec; seqNo:10661; depth 24.25 2021-02-13T00:13:04Z- handleDvl() - RDI_Pathfinder msg: 1613175184.09 epoch sec; seqNo:5232 2021-02-13T00:13:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.72 , 98.19 , 65.40 , 100.63 2021-02-13T00:13:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , -0.12 2021-02-13T00:13:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175184.11 epoch sec; seqNo:10661; 4058641.28 north; 592164.00 east 2021-02-13T00:13:04Z- AHRS_M2 msg: 1613175184.47 epoch sec; seqNo:10572 2021-02-13T00:13:04Z- AHRS_M2 msg: 0.78 phi; -0.33 theta; -0.23 psi 2021-02-13T00:13:04Z- handleDepth()-Depth_Keller msg: 1613175184481 epoch sec; seqNo:10662; depth 24.38 2021-02-13T00:13:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175184.49 epoch sec; seqNo:10662; 4058641.66 north; 592164.00 east 2021-02-13T00:13:04Z- LcmTrn::handleMessages() - handling 45 messages took 0.60 ms and 133 calls... 2021-02-13T00:13:04Z- waiting for 1613175182.62, time is 1613175184.49 2021-02-13T00:13:04Z- LcmTrn::updateTrn() >>>> pose time 1613175184.49, meas time 1613175184.09 <<<< 2021-02-13T00:13:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175184.09 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:13:04Z- TerrainNav::motionUpdate() - currEstimate = (4058642, 592164.00, 24.38) 2021-02-13T00:13:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:13:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:13:04Z- TNavBF::Projecting Measurements 2021-02-13T00:13:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:13:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:13:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:13:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:13:04Z- TNavFilter::4 good beams. 2021-02-13T00:13:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:13:04Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:13:04Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:13:04Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:13:04Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:13:04Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:13:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:13:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:13:04Z- TerrainMapOctree: (vehN, vehE ) = (4058641.41, 592163.70). 2021-02-13T00:13:04Z- TerrainMapOctree: Min Distance = 3340.27. 2021-02-13T00:13:04Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:13:04Z- There is no specific good beam that all particles have in common. 2021-02-13T00:13:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:13:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.55 K: 0.138 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-13T00:13:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:13:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 15.43 2021-02-13T00:13:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:13:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.127 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-13T00:13:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:13:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 16.20 2021-02-13T00:13:04Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:13:04Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:13:04Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:13:04Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:13:04Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:13:04Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:13:04Z- Filter 0 No beams used in subcloud update 2021-02-13T00:13:04Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:13:04Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:13:04Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 713.7 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:13:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175184.09 sec. 2021-02-13T00:13:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:13:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:13:04Z- variances([0], [2], and psi_berg) = 713.712246 833.005702 0.000000 2021-02-13T00:13:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:13:04Z- LcmTrn::publishEstimates() - MLE : 4058641.16 592213.50 24.38 2021-02-13T00:13:04Z- LcmTrn::publishEstimates() - MMSE : 4058641.66 592164.00 24.38 2021-02-13T00:13:04Z- LcmTrn::publishEstimates() - COVAR: 713.71 833.01 1.04 0.00 2021-02-13T00:13:04Z- AHRS_M2 msg: 1613175184.88 epoch sec; seqNo:10573 2021-02-13T00:13:04Z- AHRS_M2 msg: 0.77 phi; -0.33 theta; -0.22 psi 2021-02-13T00:13:04Z- handleDepth()-Depth_Keller msg: 1613175184885 epoch sec; seqNo:10663; depth 24.49 2021-02-13T00:13:04Z- handleDvl() - RDI_Pathfinder msg: 1613175184.90 epoch sec; seqNo:5233 2021-02-13T00:13:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 92.24 , 64.99 , 98.33 2021-02-13T00:13:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.12 , -0.10 2021-02-13T00:13:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175184.91 epoch sec; seqNo:10663; 4058642.04 north; 592163.99 east 2021-02-13T00:13:05Z- AHRS_M2 msg: 1613175185.28 epoch sec; seqNo:10574 2021-02-13T00:13:05Z- AHRS_M2 msg: 0.77 phi; -0.33 theta; -0.22 psi 2021-02-13T00:13:05Z- handleDepth()-Depth_Keller msg: 1613175185293 epoch sec; seqNo:10664; depth 24.63 2021-02-13T00:13:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:05Z- handleDvl() - RDI_Pathfinder msg: 1613175185.30 epoch sec; seqNo:5234 2021-02-13T00:13:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.12 , -0.10 2021-02-13T00:13:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175185.31 epoch sec; seqNo:10664; 4058642.42 north; 592163.99 east 2021-02-13T00:13:05Z- AHRS_M2 msg: 1613175185.68 epoch sec; seqNo:10575 2021-02-13T00:13:05Z- AHRS_M2 msg: 0.76 phi; -0.33 theta; -0.21 psi 2021-02-13T00:13:05Z- handleDepth()-Depth_Keller msg: 1613175185693 epoch sec; seqNo:10665; depth 24.74 2021-02-13T00:13:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175185.71 epoch sec; seqNo:10665; 4058642.42 north; 592163.99 east 2021-02-13T00:13:06Z- AHRS_M2 msg: 1613175186.09 epoch sec; seqNo:10576 2021-02-13T00:13:06Z- AHRS_M2 msg: 0.75 phi; -0.33 theta; -0.21 psi 2021-02-13T00:13:06Z- handleDepth()-Depth_Keller msg: 1613175186097 epoch sec; seqNo:10666; depth 24.86 2021-02-13T00:13:06Z- handleDvl() - RDI_Pathfinder msg: 1613175186.11 epoch sec; seqNo:5235 2021-02-13T00:13:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.90 , 147.02 , 64.58 , 98.33 2021-02-13T00:13:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.05 , -0.08 2021-02-13T00:13:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175186.15 epoch sec; seqNo:10666; 4058642.80 north; 592163.98 east 2021-02-13T00:13:06Z- AHRS_M2 msg: 1613175186.49 epoch sec; seqNo:10577 2021-02-13T00:13:06Z- AHRS_M2 msg: 0.74 phi; -0.33 theta; -0.20 psi 2021-02-13T00:13:06Z- handleDepth()-Depth_Keller msg: 1613175186497 epoch sec; seqNo:10667; depth 24.98 2021-02-13T00:13:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:06Z- handleDvl() - RDI_Pathfinder msg: 1613175186.50 epoch sec; seqNo:5236 2021-02-13T00:13:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.05 , -0.08 2021-02-13T00:13:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175186.51 epoch sec; seqNo:10667; 4058642.81 north; 592165.21 east 2021-02-13T00:13:06Z- AHRS_M2 msg: 1613175186.89 epoch sec; seqNo:10578 2021-02-13T00:13:06Z- AHRS_M2 msg: 0.73 phi; -0.33 theta; -0.20 psi 2021-02-13T00:13:06Z- handleDepth()-Depth_Keller msg: 1613175186900 epoch sec; seqNo:10668; depth 25.09 2021-02-13T00:13:06Z- handleDvl() - RDI_Pathfinder msg: 1613175186.92 epoch sec; seqNo:5237 2021-02-13T00:13:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.49 , 90.69 , 64.18 , 97.45 2021-02-13T00:13:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.08 , 0.04 2021-02-13T00:13:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175186.93 epoch sec; seqNo:10668; 4058643.19 north; 592165.20 east 2021-02-13T00:13:07Z- AHRS_M2 msg: 1613175187.30 epoch sec; seqNo:10579 2021-02-13T00:13:07Z- AHRS_M2 msg: 0.73 phi; -0.33 theta; -0.20 psi 2021-02-13T00:13:07Z- handleDepth()-Depth_Keller msg: 1613175187304 epoch sec; seqNo:10669; depth 25.20 2021-02-13T00:13:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175187.32 epoch sec; seqNo:10669; 4058643.57 north; 592165.20 east 2021-02-13T00:13:07Z- AHRS_M2 msg: 1613175187.70 epoch sec; seqNo:10580 2021-02-13T00:13:07Z- AHRS_M2 msg: 0.72 phi; -0.33 theta; -0.20 psi 2021-02-13T00:13:07Z- handleDepth()-Depth_Keller msg: 1613175187708 epoch sec; seqNo:10670; depth 25.30 2021-02-13T00:13:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:07Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:07Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:07Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:07Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:07Z- handleDvl() - RDI_Pathfinder msg: 1613175187.71 epoch sec; seqNo:5238 2021-02-13T00:13:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.08 , 0.04 2021-02-13T00:13:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175187.72 epoch sec; seqNo:10670; 4058643.57 north; 592165.20 east 2021-02-13T00:13:08Z- AHRS_M2 msg: 1613175188.11 epoch sec; seqNo:10581 2021-02-13T00:13:08Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.20 psi 2021-02-13T00:13:08Z- handleDepth()-Depth_Keller msg: 1613175188122 epoch sec; seqNo:10671; depth 25.42 2021-02-13T00:13:08Z- handleDvl() - RDI_Pathfinder msg: 1613175188.14 epoch sec; seqNo:5239 2021-02-13T00:13:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.31 , 90.35 , 65.40 , 99.00 2021-02-13T00:13:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.03 , 0.08 2021-02-13T00:13:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175188.15 epoch sec; seqNo:10671; 4058643.95 north; 592165.19 east 2021-02-13T00:13:08Z- AHRS_M2 msg: 1613175188.51 epoch sec; seqNo:10582 2021-02-13T00:13:08Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.20 psi 2021-02-13T00:13:08Z- handleDepth()-Depth_Keller msg: 1613175188521 epoch sec; seqNo:10672; depth 25.54 2021-02-13T00:13:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175188.53 epoch sec; seqNo:10672; 4058643.95 north; 592165.19 east 2021-02-13T00:13:08Z- AHRS_M2 msg: 1613175188.92 epoch sec; seqNo:10583 2021-02-13T00:13:08Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.20 psi 2021-02-13T00:13:08Z- handleDepth()-Depth_Keller msg: 1613175188925 epoch sec; seqNo:10673; depth 25.66 2021-02-13T00:13:08Z- handleDvl() - RDI_Pathfinder msg: 1613175188.94 epoch sec; seqNo:5240 2021-02-13T00:13:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.49 , 0.00 , 63.77 , 97.86 2021-02-13T00:13:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.00 , 0.08 2021-02-13T00:13:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175188.95 epoch sec; seqNo:10673; 4058644.34 north; 592166.41 east 2021-02-13T00:13:09Z- AHRS_M2 msg: 1613175189.32 epoch sec; seqNo:10584 2021-02-13T00:13:09Z- AHRS_M2 msg: 0.71 phi; -0.32 theta; -0.19 psi 2021-02-13T00:13:09Z- handleDepth()-Depth_Keller msg: 1613175189326 epoch sec; seqNo:10674; depth 25.78 2021-02-13T00:13:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175189.34 epoch sec; seqNo:10674; 4058644.72 north; 592166.41 east 2021-02-13T00:13:09Z- AHRS_M2 msg: 1613175189.72 epoch sec; seqNo:10585 2021-02-13T00:13:09Z- AHRS_M2 msg: 0.71 phi; -0.32 theta; -0.19 psi 2021-02-13T00:13:09Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-13T00:13:09Z- waiting for 1613175187.49, time is 1613175189.72 2021-02-13T00:13:09Z- LcmTrn::updateTrn() >>>> pose time 1613175189.72, meas time 1613175188.94 <<<< 2021-02-13T00:13:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175188.94 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:13:09Z- TerrainNav::motionUpdate() - currEstimate = (4058645, 592166.41, 25.78) 2021-02-13T00:13:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:13:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:13:09Z- TNavBF::Projecting Measurements 2021-02-13T00:13:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:13:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:13:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:13:09Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:13:09Z- TNavFilter::3 good beams. 2021-02-13T00:13:09Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:13:09Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:13:09Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:13:09Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:13:09Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:13:09Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:13:09Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:13:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:13:09Z- TerrainMapOctree: (vehN, vehE ) = (4058644.26, 592166.04). 2021-02-13T00:13:09Z- TerrainMapOctree: Min Distance = 3336.58. 2021-02-13T00:13:09Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:13:09Z- There is no specific good beam that all particles have in common. 2021-02-13T00:13:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:13:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.114 ExpectedMeasDiff: nan currMeas.covariance: 7.73 2021-02-13T00:13:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:13:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.115 ExpectedMeasDiff: nan currMeas.covariance: 6.51 2021-02-13T00:13:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:13:09Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-13T00:13:09Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:13:09Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:13:09Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:13:09Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:13:09Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:13:09Z- Filter 0 No beams used in subcloud update 2021-02-13T00:13:09Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:13:09Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:13:09Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 713.7 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:13:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175188.94 sec. 2021-02-13T00:13:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:13:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:13:09Z- variances([0], [2], and psi_berg) = 713.631075 833.005652 0.000000 2021-02-13T00:13:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:13:09Z- LcmTrn::publishEstimates() - MLE : 4058644.22 592215.91 25.78 2021-02-13T00:13:09Z- LcmTrn::publishEstimates() - MMSE : 4058644.72 592166.41 25.78 2021-02-13T00:13:09Z- LcmTrn::publishEstimates() - COVAR: 713.63 833.01 0.93 0.00 2021-02-13T00:13:09Z- handleDepth()-Depth_Keller msg: 1613175189728 epoch sec; seqNo:10675; depth 25.88 2021-02-13T00:13:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175189.75 epoch sec; seqNo:10675; 4058644.72 north; 592166.41 east 2021-02-13T00:13:10Z- AHRS_M2 msg: 1613175190.13 epoch sec; seqNo:10586 2021-02-13T00:13:10Z- AHRS_M2 msg: 0.71 phi; -0.32 theta; -0.19 psi 2021-02-13T00:13:10Z- handleDepth()-Depth_Keller msg: 1613175190133 epoch sec; seqNo:10676; depth 26.00 2021-02-13T00:13:10Z- handleDvl() - RDI_Pathfinder msg: 1613175190.15 epoch sec; seqNo:5241 2021-02-13T00:13:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.49 , 88.25 , 62.95 , 97.99 2021-02-13T00:13:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.05 2021-02-13T00:13:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175190.18 epoch sec; seqNo:10676; 4058645.10 north; 592166.40 east 2021-02-13T00:13:10Z- AHRS_M2 msg: 1613175190.54 epoch sec; seqNo:10587 2021-02-13T00:13:10Z- AHRS_M2 msg: 0.71 phi; -0.32 theta; -0.18 psi 2021-02-13T00:13:10Z- handleDepth()-Depth_Keller msg: 1613175190549 epoch sec; seqNo:10677; depth 26.13 2021-02-13T00:13:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175190.56 epoch sec; seqNo:10677; 4058645.48 north; 592166.40 east 2021-02-13T00:13:10Z- AHRS_M2 msg: 1613175190.94 epoch sec; seqNo:10588 2021-02-13T00:13:10Z- AHRS_M2 msg: 0.72 phi; -0.32 theta; -0.18 psi 2021-02-13T00:13:10Z- handleDepth()-Depth_Keller msg: 1613175190946 epoch sec; seqNo:10678; depth 26.24 2021-02-13T00:13:10Z- handleDvl() - RDI_Pathfinder msg: 1613175190.98 epoch sec; seqNo:5242 2021-02-13T00:13:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.49 , 0.00 , 64.18 , 97.99 2021-02-13T00:13:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.04 2021-02-13T00:13:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175191.01 epoch sec; seqNo:10678; 4058645.48 north; 592166.40 east 2021-02-13T00:13:11Z- AHRS_M2 msg: 1613175191.34 epoch sec; seqNo:10589 2021-02-13T00:13:11Z- AHRS_M2 msg: 0.72 phi; -0.32 theta; -0.17 psi 2021-02-13T00:13:11Z- handleDepth()-Depth_Keller msg: 1613175191349 epoch sec; seqNo:10679; depth 26.37 2021-02-13T00:13:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:11Z- handleDvl() - RDI_Pathfinder msg: 1613175191.35 epoch sec; seqNo:5243 2021-02-13T00:13:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.04 2021-02-13T00:13:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175191.37 epoch sec; seqNo:10679; 4058645.87 north; 592167.61 east 2021-02-13T00:13:11Z- AHRS_M2 msg: 1613175191.75 epoch sec; seqNo:10590 2021-02-13T00:13:11Z- AHRS_M2 msg: 0.72 phi; -0.32 theta; -0.17 psi 2021-02-13T00:13:11Z- handleDepth()-Depth_Keller msg: 1613175191758 epoch sec; seqNo:10680; depth 26.47 2021-02-13T00:13:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175191.81 epoch sec; seqNo:10680; 4058646.25 north; 592167.61 east 2021-02-13T00:13:12Z- AHRS_M2 msg: 1613175192.15 epoch sec; seqNo:10591 2021-02-13T00:13:12Z- AHRS_M2 msg: 0.72 phi; -0.32 theta; -0.17 psi 2021-02-13T00:13:12Z- handleDepth()-Depth_Keller msg: 1613175192157 epoch sec; seqNo:10681; depth 26.58 2021-02-13T00:13:12Z- handleDvl() - RDI_Pathfinder msg: 1613175192.17 epoch sec; seqNo:5244 2021-02-13T00:13:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.08 , 86.09 , 64.18 , 95.83 2021-02-13T00:13:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.09 , 0.04 2021-02-13T00:13:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175192.18 epoch sec; seqNo:10681; 4058646.25 north; 592167.61 east 2021-02-13T00:13:12Z- AHRS_M2 msg: 1613175192.55 epoch sec; seqNo:10592 2021-02-13T00:13:12Z- AHRS_M2 msg: 0.73 phi; -0.32 theta; -0.16 psi 2021-02-13T00:13:12Z- handleDepth()-Depth_Keller msg: 1613175192558 epoch sec; seqNo:10682; depth 26.71 2021-02-13T00:13:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:12Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:12Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:12Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:12Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:12Z- handleDvl() - RDI_Pathfinder msg: 1613175192.56 epoch sec; seqNo:5245 2021-02-13T00:13:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.09 , 0.04 2021-02-13T00:13:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175192.57 epoch sec; seqNo:10682; 4058646.63 north; 592167.61 east 2021-02-13T00:13:12Z- AHRS_M2 msg: 1613175192.95 epoch sec; seqNo:10593 2021-02-13T00:13:12Z- AHRS_M2 msg: 0.73 phi; -0.32 theta; -0.16 psi 2021-02-13T00:13:12Z- handleDepth()-Depth_Keller msg: 1613175192961 epoch sec; seqNo:10683; depth 26.80 2021-02-13T00:13:12Z- handleDvl() - RDI_Pathfinder msg: 1613175192.98 epoch sec; seqNo:5246 2021-02-13T00:13:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.67 , 85.01 , 63.36 , 97.99 2021-02-13T00:13:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , 0.06 2021-02-13T00:13:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175192.99 epoch sec; seqNo:10683; 4058647.01 north; 592167.60 east 2021-02-13T00:13:13Z- AHRS_M2 msg: 1613175193.36 epoch sec; seqNo:10594 2021-02-13T00:13:13Z- AHRS_M2 msg: 0.74 phi; -0.32 theta; -0.16 psi 2021-02-13T00:13:13Z- handleDepth()-Depth_Keller msg: 1613175193364 epoch sec; seqNo:10684; depth 26.92 2021-02-13T00:13:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175193.38 epoch sec; seqNo:10684; 4058647.39 north; 592167.60 east 2021-02-13T00:13:13Z- AHRS_M2 msg: 1613175193.77 epoch sec; seqNo:10595 2021-02-13T00:13:13Z- AHRS_M2 msg: 0.74 phi; -0.32 theta; -0.15 psi 2021-02-13T00:13:13Z- handleDepth()-Depth_Keller msg: 1613175193774 epoch sec; seqNo:10685; depth 27.04 2021-02-13T00:13:13Z- handleDvl() - RDI_Pathfinder msg: 1613175193.79 epoch sec; seqNo:5247 2021-02-13T00:13:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.08 , 0.00 , 62.95 , 97.99 2021-02-13T00:13:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , 0.06 2021-02-13T00:13:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175193.80 epoch sec; seqNo:10685; 4058647.40 north; 592168.82 east 2021-02-13T00:13:14Z- AHRS_M2 msg: 1613175194.17 epoch sec; seqNo:10596 2021-02-13T00:13:14Z- AHRS_M2 msg: 0.75 phi; -0.32 theta; -0.15 psi 2021-02-13T00:13:14Z- handleDepth()-Depth_Keller msg: 1613175194185 epoch sec; seqNo:10686; depth 27.14 2021-02-13T00:13:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175194.20 epoch sec; seqNo:10686; 4058647.78 north; 592168.81 east 2021-02-13T00:13:14Z- AHRS_M2 msg: 1613175194.57 epoch sec; seqNo:10597 2021-02-13T00:13:14Z- AHRS_M2 msg: 0.75 phi; -0.32 theta; -0.15 psi 2021-02-13T00:13:14Z- handleDepth()-Depth_Keller msg: 1613175194581 epoch sec; seqNo:10687; depth 27.26 2021-02-13T00:13:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175194.60 epoch sec; seqNo:10687; 4058648.16 north; 592168.81 east 2021-02-13T00:13:14Z- LcmTrn::handleMessages() - handling 45 messages took 0.75 ms and 130 calls... 2021-02-13T00:13:14Z- waiting for 1613175192.72, time is 1613175194.60 2021-02-13T00:13:14Z- LcmTrn::updateTrn() >>>> pose time 1613175194.60, meas time 1613175193.79 <<<< 2021-02-13T00:13:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175193.79 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:13:14Z- TerrainNav::motionUpdate() - currEstimate = (4058648, 592168.81, 27.26) 2021-02-13T00:13:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:13:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:13:14Z- TNavBF::Projecting Measurements 2021-02-13T00:13:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:13:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:13:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:13:14Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:13:14Z- TNavFilter::3 good beams. 2021-02-13T00:13:14Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:13:14Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:13:14Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:13:14Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:13:14Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:13:14Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:13:14Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:13:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:13:14Z- TerrainMapOctree: (vehN, vehE ) = (4058647.58, 592168.41). 2021-02-13T00:13:14Z- TerrainMapOctree: Min Distance = 3332.50. 2021-02-13T00:13:14Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:13:14Z- There is no specific good beam that all particles have in common. 2021-02-13T00:13:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:13:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 7.64 2021-02-13T00:13:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:13:14Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 6.34 2021-02-13T00:13:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:13:14Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:13:14Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:13:14Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:13:14Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:13:14Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:13:14Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:13:14Z- Filter 0 No beams used in subcloud update 2021-02-13T00:13:14Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:13:14Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:13:14Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 713.6 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:13:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175193.79 sec. 2021-02-13T00:13:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:13:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:13:14Z- variances([0], [2], and psi_berg) = 713.546162 833.005601 0.000000 2021-02-13T00:13:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:13:14Z- LcmTrn::publishEstimates() - MLE : 4058647.66 592218.31 27.26 2021-02-13T00:13:14Z- LcmTrn::publishEstimates() - MMSE : 4058648.16 592168.81 27.26 2021-02-13T00:13:14Z- LcmTrn::publishEstimates() - COVAR: 713.55 833.01 0.87 0.00 2021-02-13T00:13:14Z- AHRS_M2 msg: 1613175194.97 epoch sec; seqNo:10598 2021-02-13T00:13:14Z- AHRS_M2 msg: 0.76 phi; -0.32 theta; -0.14 psi 2021-02-13T00:13:14Z- handleDepth()-Depth_Keller msg: 1613175194985 epoch sec; seqNo:10688; depth 27.37 2021-02-13T00:13:15Z- handleDvl() - RDI_Pathfinder msg: 1613175195.00 epoch sec; seqNo:5248 2021-02-13T00:13:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.08 , 83.93 , 62.54 , 97.99 2021-02-13T00:13:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , 0.04 2021-02-13T00:13:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175195.02 epoch sec; seqNo:10688; 4058648.16 north; 592168.81 east 2021-02-13T00:13:15Z- AHRS_M2 msg: 1613175195.38 epoch sec; seqNo:10599 2021-02-13T00:13:15Z- AHRS_M2 msg: 0.77 phi; -0.32 theta; -0.14 psi 2021-02-13T00:13:15Z- handleDepth()-Depth_Keller msg: 1613175195389 epoch sec; seqNo:10689; depth 27.48 2021-02-13T00:13:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175195.40 epoch sec; seqNo:10689; 4058648.54 north; 592168.81 east 2021-02-13T00:13:15Z- AHRS_M2 msg: 1613175195.80 epoch sec; seqNo:10600 2021-02-13T00:13:15Z- AHRS_M2 msg: 0.77 phi; -0.32 theta; -0.14 psi 2021-02-13T00:13:15Z- handleDepth()-Depth_Keller msg: 1613175195807 epoch sec; seqNo:10690; depth 27.60 2021-02-13T00:13:15Z- handleDvl() - RDI_Pathfinder msg: 1613175195.82 epoch sec; seqNo:5249 2021-02-13T00:13:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.08 , 82.84 , 61.72 , 97.99 2021-02-13T00:13:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.02 , -0.01 2021-02-13T00:13:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175195.83 epoch sec; seqNo:10690; 4058648.55 north; 592170.03 east 2021-02-13T00:13:16Z- AHRS_M2 msg: 1613175196.19 epoch sec; seqNo:10601 2021-02-13T00:13:16Z- AHRS_M2 msg: 0.78 phi; -0.32 theta; -0.13 psi 2021-02-13T00:13:16Z- handleDepth()-Depth_Keller msg: 1613175196192 epoch sec; seqNo:10691; depth 27.71 2021-02-13T00:13:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:16Z- handleDvl() - RDI_Pathfinder msg: 1613175196.19 epoch sec; seqNo:5250 2021-02-13T00:13:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.02 , -0.01 2021-02-13T00:13:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175196.23 epoch sec; seqNo:10691; 4058648.93 north; 592170.02 east 2021-02-13T00:13:16Z- AHRS_M2 msg: 1613175196.59 epoch sec; seqNo:10602 2021-02-13T00:13:16Z- AHRS_M2 msg: 0.79 phi; -0.32 theta; -0.13 psi 2021-02-13T00:13:16Z- handleDepth()-Depth_Keller msg: 1613175196601 epoch sec; seqNo:10692; depth 27.82 2021-02-13T00:13:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175196.61 epoch sec; seqNo:10692; 4058649.31 north; 592170.02 east 2021-02-13T00:13:16Z- AHRS_M2 msg: 1613175197.00 epoch sec; seqNo:10603 2021-02-13T00:13:16Z- AHRS_M2 msg: 0.79 phi; -0.32 theta; -0.14 psi 2021-02-13T00:13:17Z- handleDepth()-Depth_Keller msg: 1613175197001 epoch sec; seqNo:10693; depth 27.93 2021-02-13T00:13:17Z- handleDvl() - RDI_Pathfinder msg: 1613175197.01 epoch sec; seqNo:5251 2021-02-13T00:13:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.67 , 82.84 , 61.31 , 97.99 2021-02-13T00:13:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.00 , -0.07 2021-02-13T00:13:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175197.02 epoch sec; seqNo:10693; 4058649.31 north; 592170.02 east 2021-02-13T00:13:17Z- AHRS_M2 msg: 1613175197.40 epoch sec; seqNo:10604 2021-02-13T00:13:17Z- AHRS_M2 msg: 0.80 phi; -0.32 theta; -0.14 psi 2021-02-13T00:13:17Z- handleDepth()-Depth_Keller msg: 1613175197409 epoch sec; seqNo:10694; depth 28.04 2021-02-13T00:13:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:17Z- handleDvl() - RDI_Pathfinder msg: 1613175197.41 epoch sec; seqNo:5252 2021-02-13T00:13:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.00 , -0.07 2021-02-13T00:13:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175197.42 epoch sec; seqNo:10694; 4058649.69 north; 592170.01 east 2021-02-13T00:13:17Z- AHRS_M2 msg: 1613175197.81 epoch sec; seqNo:10605 2021-02-13T00:13:17Z- AHRS_M2 msg: 0.80 phi; -0.32 theta; -0.14 psi 2021-02-13T00:13:17Z- handleDepth()-Depth_Keller msg: 1613175197813 epoch sec; seqNo:10695; depth 28.14 2021-02-13T00:13:17Z- handleDvl() - RDI_Pathfinder msg: 1613175197.83 epoch sec; seqNo:5253 2021-02-13T00:13:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.67 , 82.84 , 61.31 , 97.99 2021-02-13T00:13:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , -0.11 2021-02-13T00:13:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175197.84 epoch sec; seqNo:10695; 4058650.08 north; 592171.23 east 2021-02-13T00:13:18Z- AHRS_M2 msg: 1613175198.21 epoch sec; seqNo:10606 2021-02-13T00:13:18Z- AHRS_M2 msg: 0.80 phi; -0.32 theta; -0.15 psi 2021-02-13T00:13:18Z- handleDepth()-Depth_Keller msg: 1613175198221 epoch sec; seqNo:10696; depth 28.27 2021-02-13T00:13:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175198.23 epoch sec; seqNo:10696; 4058650.46 north; 592171.23 east 2021-02-13T00:13:18Z- AHRS_M2 msg: 1613175198.61 epoch sec; seqNo:10607 2021-02-13T00:13:18Z- AHRS_M2 msg: 0.81 phi; -0.32 theta; -0.15 psi 2021-02-13T00:13:18Z- handleDepth()-Depth_Keller msg: 1613175198618 epoch sec; seqNo:10697; depth 28.38 2021-02-13T00:13:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:18Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:18Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:18Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:18Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:18Z- handleDvl() - RDI_Pathfinder msg: 1613175198.62 epoch sec; seqNo:5254 2021-02-13T00:13:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , -0.11 2021-02-13T00:13:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175198.63 epoch sec; seqNo:10697; 4058650.46 north; 592171.23 east 2021-02-13T00:13:19Z- AHRS_M2 msg: 1613175199.02 epoch sec; seqNo:10608 2021-02-13T00:13:19Z- AHRS_M2 msg: 0.81 phi; -0.32 theta; -0.15 psi 2021-02-13T00:13:19Z- handleDepth()-Depth_Keller msg: 1613175199021 epoch sec; seqNo:10698; depth 28.48 2021-02-13T00:13:19Z- handleDvl() - RDI_Pathfinder msg: 1613175199.04 epoch sec; seqNo:5255 2021-02-13T00:13:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.45 , 0.00 , 62.13 , 97.99 2021-02-13T00:13:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , -0.11 2021-02-13T00:13:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175199.05 epoch sec; seqNo:10698; 4058650.83 north; 592171.22 east 2021-02-13T00:13:19Z- AHRS_M2 msg: 1613175199.42 epoch sec; seqNo:10609 2021-02-13T00:13:19Z- AHRS_M2 msg: 0.81 phi; -0.32 theta; -0.16 psi 2021-02-13T00:13:19Z- handleDepth()-Depth_Keller msg: 1613175199424 epoch sec; seqNo:10699; depth 28.60 2021-02-13T00:13:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175199.43 epoch sec; seqNo:10699; 4058651.21 north; 592171.22 east 2021-02-13T00:13:19Z- AHRS_M2 msg: 1613175199.82 epoch sec; seqNo:10610 2021-02-13T00:13:19Z- AHRS_M2 msg: 0.81 phi; -0.32 theta; -0.16 psi 2021-02-13T00:13:19Z- handleDepth()-Depth_Keller msg: 1613175199828 epoch sec; seqNo:10700; depth 28.70 2021-02-13T00:13:19Z- handleDvl() - RDI_Pathfinder msg: 1613175199.84 epoch sec; seqNo:5256 2021-02-13T00:13:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.64 , 83.92 , 60.91 , 97.99 2021-02-13T00:13:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.12 , -0.08 2021-02-13T00:13:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175199.85 epoch sec; seqNo:10700; 4058651.23 north; 592172.44 east 2021-02-13T00:13:19Z- LcmTrn::handleMessages() - handling 48 messages took 0.63 ms and 136 calls... 2021-02-13T00:13:19Z- waiting for 1613175197.60, time is 1613175199.85 2021-02-13T00:13:19Z- LcmTrn::updateTrn() equating pose 1613175199.85 and meas times 1613175199.85 2021-02-13T00:13:19Z- LcmTrn::updateTrn() >>>> pose time 1613175199.85, meas time 1613175199.85 <<<< 2021-02-13T00:13:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175199.85 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:13:19Z- TerrainNav::motionUpdate() - currEstimate = (4058651, 592172.44, 28.70) 2021-02-13T00:13:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:13:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:13:19Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175199.85 sec.; No relevant navigation data available 2021-02-13T00:13:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:13:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:13:19Z- variances([0], [2], and psi_berg) = 713.477772 833.005559 0.000000 2021-02-13T00:13:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:13:19Z- LcmTrn::publishEstimates() - MLE : 4058650.73 592221.94 28.70 2021-02-13T00:13:19Z- LcmTrn::publishEstimates() - MMSE : 4058651.23 592172.44 28.70 2021-02-13T00:13:19Z- LcmTrn::publishEstimates() - COVAR: 713.48 833.01 1.12 0.00 2021-02-13T00:13:20Z- AHRS_M2 msg: 1613175200.23 epoch sec; seqNo:10611 2021-02-13T00:13:20Z- AHRS_M2 msg: 0.82 phi; -0.32 theta; -0.16 psi 2021-02-13T00:13:20Z- handleDepth()-Depth_Keller msg: 1613175200234 epoch sec; seqNo:10701; depth 28.81 2021-02-13T00:13:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175200.25 epoch sec; seqNo:10701; 4058651.61 north; 592172.44 east 2021-02-13T00:13:20Z- AHRS_M2 msg: 1613175200.63 epoch sec; seqNo:10612 2021-02-13T00:13:20Z- AHRS_M2 msg: 0.82 phi; -0.32 theta; -0.16 psi 2021-02-13T00:13:20Z- handleDepth()-Depth_Keller msg: 1613175200642 epoch sec; seqNo:10702; depth 28.94 2021-02-13T00:13:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175200.65 epoch sec; seqNo:10702; 4058651.98 north; 592172.43 east 2021-02-13T00:13:21Z- AHRS_M2 msg: 1613175201.04 epoch sec; seqNo:10613 2021-02-13T00:13:21Z- AHRS_M2 msg: 0.81 phi; -0.31 theta; -0.17 psi 2021-02-13T00:13:21Z- handleDepth()-Depth_Keller msg: 1613175201039 epoch sec; seqNo:10703; depth 29.05 2021-02-13T00:13:21Z- handleDvl() - RDI_Pathfinder msg: 1613175201.05 epoch sec; seqNo:5257 2021-02-13T00:13:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.04 , 0.00 , 61.32 , 90.41 2021-02-13T00:13:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.11 , -0.02 2021-02-13T00:13:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175201.06 epoch sec; seqNo:10703; 4058651.98 north; 592172.43 east 2021-02-13T00:13:21Z- AHRS_M2 msg: 1613175201.44 epoch sec; seqNo:10614 2021-02-13T00:13:21Z- AHRS_M2 msg: 0.81 phi; -0.30 theta; -0.17 psi 2021-02-13T00:13:21Z- handleDepth()-Depth_Keller msg: 1613175201445 epoch sec; seqNo:10704; depth 29.17 2021-02-13T00:13:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175201.46 epoch sec; seqNo:10704; 4058652.36 north; 592172.43 east 2021-02-13T00:13:21Z- AHRS_M2 msg: 1613175201.84 epoch sec; seqNo:10615 2021-02-13T00:13:21Z- AHRS_M2 msg: 0.81 phi; -0.27 theta; -0.17 psi 2021-02-13T00:13:21Z- handleDepth()-Depth_Keller msg: 1613175201848 epoch sec; seqNo:10705; depth 29.29 2021-02-13T00:13:21Z- handleDvl() - RDI_Pathfinder msg: 1613175201.87 epoch sec; seqNo:5258 2021-02-13T00:13:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.81 , 82.84 , 60.91 , 147.76 2021-02-13T00:13:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.62 , -0.10 , -0.00 2021-02-13T00:13:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175201.88 epoch sec; seqNo:10705; 4058652.74 north; 592172.42 east 2021-02-13T00:13:22Z- AHRS_M2 msg: 1613175202.25 epoch sec; seqNo:10616 2021-02-13T00:13:22Z- AHRS_M2 msg: 0.80 phi; -0.24 theta; -0.17 psi 2021-02-13T00:13:22Z- handleDepth()-Depth_Keller msg: 1613175202257 epoch sec; seqNo:10706; depth 29.42 2021-02-13T00:13:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175202.27 epoch sec; seqNo:10706; 4058652.74 north; 592172.42 east 2021-02-13T00:13:22Z- AHRS_M2 msg: 1613175202.65 epoch sec; seqNo:10617 2021-02-13T00:13:22Z- AHRS_M2 msg: 0.79 phi; -0.21 theta; -0.17 psi 2021-02-13T00:13:22Z- handleDepth()-Depth_Keller msg: 1613175202657 epoch sec; seqNo:10707; depth 29.54 2021-02-13T00:13:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175202.67 epoch sec; seqNo:10707; 4058653.13 north; 592173.64 east 2021-02-13T00:13:23Z- AHRS_M2 msg: 1613175203.06 epoch sec; seqNo:10618 2021-02-13T00:13:23Z- AHRS_M2 msg: 0.78 phi; -0.17 theta; -0.17 psi 2021-02-13T00:13:23Z- handleDepth()-Depth_Keller msg: 1613175203066 epoch sec; seqNo:10708; depth 29.67 2021-02-13T00:13:23Z- handleDvl() - RDI_Pathfinder msg: 1613175203.08 epoch sec; seqNo:5259 2021-02-13T00:13:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:23Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:13:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175203.09 epoch sec; seqNo:10708; 4058653.13 north; 592173.64 east 2021-02-13T00:13:23Z- AHRS_M2 msg: 1613175203.48 epoch sec; seqNo:10619 2021-02-13T00:13:23Z- AHRS_M2 msg: 0.78 phi; -0.13 theta; -0.17 psi 2021-02-13T00:13:23Z- handleDepth()-Depth_Keller msg: 1613175203491 epoch sec; seqNo:10709; depth 29.76 2021-02-13T00:13:23Z- handleDvl() - RDI_Pathfinder msg: 1613175203.50 epoch sec; seqNo:5260 2021-02-13T00:13:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.49 , 0.00 , 63.95 , 0.00 2021-02-13T00:13:23Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:13:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175203.51 epoch sec; seqNo:10709; 4058653.51 north; 592173.64 east 2021-02-13T00:13:23Z- AHRS_M2 msg: 1613175203.86 epoch sec; seqNo:10620 2021-02-13T00:13:23Z- AHRS_M2 msg: 0.77 phi; -0.09 theta; -0.17 psi 2021-02-13T00:13:23Z- handleDepth()-Depth_Keller msg: 1613175203869 epoch sec; seqNo:10710; depth 29.86 2021-02-13T00:13:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175203.88 epoch sec; seqNo:10710; 4058653.51 north; 592173.64 east 2021-02-13T00:13:24Z- AHRS_M2 msg: 1613175204.27 epoch sec; seqNo:10621 2021-02-13T00:13:24Z- AHRS_M2 msg: 0.76 phi; -0.05 theta; -0.17 psi 2021-02-13T00:13:24Z- handleDepth()-Depth_Keller msg: 1613175204272 epoch sec; seqNo:10711; depth 29.95 2021-02-13T00:13:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175204.28 epoch sec; seqNo:10711; 4058653.89 north; 592173.63 east 2021-02-13T00:13:24Z- AHRS_M2 msg: 1613175204.67 epoch sec; seqNo:10622 2021-02-13T00:13:24Z- AHRS_M2 msg: 0.75 phi; -0.01 theta; -0.17 psi 2021-02-13T00:13:24Z- handleDepth()-Depth_Keller msg: 1613175204678 epoch sec; seqNo:10712; depth 30.02 2021-02-13T00:13:24Z- handleDvl() - RDI_Pathfinder msg: 1613175204.69 epoch sec; seqNo:5261 2021-02-13T00:13:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.03 , 0.00 , 67.18 , 0.00 2021-02-13T00:13:24Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:13:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175204.70 epoch sec; seqNo:10712; 4058653.89 north; 592173.63 east 2021-02-13T00:13:24Z- LcmTrn::handleMessages() - handling 41 messages took 0.68 ms and 128 calls... 2021-02-13T00:13:24Z- waiting for 1613175202.85, time is 1613175204.70 2021-02-13T00:13:24Z- LcmTrn::updateTrn() equating pose 1613175204.70 and meas times 1613175204.70 2021-02-13T00:13:24Z- LcmTrn::updateTrn() >>>> pose time 1613175204.70, meas time 1613175204.70 <<<< 2021-02-13T00:13:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175204.70 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:13:24Z- TerrainNav::motionUpdate() - currEstimate = (4058654, 592173.63, 30.02) 2021-02-13T00:13:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:13:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:13:24Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:13:24Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:13:24Z- TNavBF::Projecting Measurements 2021-02-13T00:13:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:13:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:13:24Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-13T00:13:24Z- TNavFilter::2 good beams. 2021-02-13T00:13:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:13:24Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:13:24Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:13:24Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:13:24Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:13:24Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-13T00:13:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:13:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:13:24Z- TerrainMapOctree: (vehN, vehE ) = (4058653.89, 592173.63). 2021-02-13T00:13:24Z- TerrainMapOctree: Min Distance = 3324.32. 2021-02-13T00:13:24Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:13:24Z- There is no specific good beam that all particles have in common. 2021-02-13T00:13:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:13:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.135 ExpectedMeasDiff: nan currMeas.covariance: 6.36 2021-02-13T00:13:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:13:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 7.22 2021-02-13T00:13:24Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:13:24Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:13:24Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:13:24Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:13:24Z- Filter 0 No beams used in subcloud update 2021-02-13T00:13:24Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:13:24Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:13:24Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 713.4 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:13:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175204.70 sec. 2021-02-13T00:13:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:13:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:13:25Z- variances([0], [2], and psi_berg) = 713.424219 833.005527 0.000000 2021-02-13T00:13:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:13:25Z- LcmTrn::publishEstimates() - MLE : 4058653.39 592223.13 30.02 2021-02-13T00:13:25Z- LcmTrn::publishEstimates() - MMSE : 4058653.89 592173.63 30.02 2021-02-13T00:13:25Z- LcmTrn::publishEstimates() - COVAR: 713.42 833.01 0.98 0.00 2021-02-13T00:13:25Z- AHRS_M2 msg: 1613175205.08 epoch sec; seqNo:10623 2021-02-13T00:13:25Z- AHRS_M2 msg: 0.74 phi; 0.03 theta; -0.17 psi 2021-02-13T00:13:25Z- handleDepth()-Depth_Keller msg: 1613175205080 epoch sec; seqNo:10713; depth 30.08 2021-02-13T00:13:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175205.10 epoch sec; seqNo:10713; 4058654.27 north; 592173.63 east 2021-02-13T00:13:25Z- AHRS_M2 msg: 1613175205.49 epoch sec; seqNo:10624 2021-02-13T00:13:25Z- AHRS_M2 msg: 0.73 phi; 0.06 theta; -0.18 psi 2021-02-13T00:13:25Z- handleDepth()-Depth_Keller msg: 1613175205499 epoch sec; seqNo:10714; depth 30.13 2021-02-13T00:13:25Z- handleDvl() - RDI_Pathfinder msg: 1613175205.52 epoch sec; seqNo:5262 2021-02-13T00:13:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.03 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:25Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:13:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175205.53 epoch sec; seqNo:10714; 4058654.27 north; 592173.63 east 2021-02-13T00:13:25Z- AHRS_M2 msg: 1613175205.88 epoch sec; seqNo:10625 2021-02-13T00:13:25Z- AHRS_M2 msg: 0.72 phi; 0.10 theta; -0.18 psi 2021-02-13T00:13:25Z- handleDepth()-Depth_Keller msg: 1613175205888 epoch sec; seqNo:10715; depth 30.15 2021-02-13T00:13:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:25Z- handleDvl() - RDI_Pathfinder msg: 1613175205.89 epoch sec; seqNo:5263 2021-02-13T00:13:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:25Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:13:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175205.90 epoch sec; seqNo:10715; 4058654.65 north; 592173.62 east 2021-02-13T00:13:26Z- AHRS_M2 msg: 1613175206.29 epoch sec; seqNo:10626 2021-02-13T00:13:26Z- AHRS_M2 msg: 0.71 phi; 0.13 theta; -0.18 psi 2021-02-13T00:13:26Z- handleDepth()-Depth_Keller msg: 1613175206301 epoch sec; seqNo:10716; depth 30.19 2021-02-13T00:13:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175206.31 epoch sec; seqNo:10716; 4058655.04 north; 592174.84 east 2021-02-13T00:13:26Z- AHRS_M2 msg: 1613175206.69 epoch sec; seqNo:10627 2021-02-13T00:13:26Z- AHRS_M2 msg: 0.71 phi; 0.16 theta; -0.18 psi 2021-02-13T00:13:26Z- handleDepth()-Depth_Keller msg: 1613175206696 epoch sec; seqNo:10717; depth 30.18 2021-02-13T00:13:26Z- handleDvl() - RDI_Pathfinder msg: 1613175206.72 epoch sec; seqNo:5264 2021-02-13T00:13:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:26Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:13:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175206.73 epoch sec; seqNo:10717; 4058655.04 north; 592174.84 east 2021-02-13T00:13:27Z- AHRS_M2 msg: 1613175207.10 epoch sec; seqNo:10628 2021-02-13T00:13:27Z- AHRS_M2 msg: 0.70 phi; 0.18 theta; -0.18 psi 2021-02-13T00:13:27Z- handleDepth()-Depth_Keller msg: 1613175207100 epoch sec; seqNo:10718; depth 30.16 2021-02-13T00:13:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:27Z- handleDvl() - RDI_Pathfinder msg: 1613175207.10 epoch sec; seqNo:5265 2021-02-13T00:13:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:27Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:13:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175207.12 epoch sec; seqNo:10718; 4058655.42 north; 592174.84 east 2021-02-13T00:13:27Z- AHRS_M2 msg: 1613175207.50 epoch sec; seqNo:10629 2021-02-13T00:13:27Z- AHRS_M2 msg: 0.69 phi; 0.21 theta; -0.18 psi 2021-02-13T00:13:27Z- handleDepth()-Depth_Keller msg: 1613175207506 epoch sec; seqNo:10719; depth 30.15 2021-02-13T00:13:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175207.52 epoch sec; seqNo:10719; 4058655.42 north; 592174.84 east 2021-02-13T00:13:27Z- AHRS_M2 msg: 1613175207.91 epoch sec; seqNo:10630 2021-02-13T00:13:27Z- AHRS_M2 msg: 0.68 phi; 0.25 theta; -0.18 psi 2021-02-13T00:13:27Z- handleDepth()-Depth_Keller msg: 1613175207915 epoch sec; seqNo:10720; depth 30.11 2021-02-13T00:13:27Z- handleDvl() - RDI_Pathfinder msg: 1613175207.93 epoch sec; seqNo:5266 2021-02-13T00:13:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.18 , 80.68 , 83.93 , 63.36 2021-02-13T00:13:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , -0.03 2021-02-13T00:13:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175207.94 epoch sec; seqNo:10720; 4058655.80 north; 592174.83 east 2021-02-13T00:13:28Z- AHRS_M2 msg: 1613175208.31 epoch sec; seqNo:10631 2021-02-13T00:13:28Z- AHRS_M2 msg: 0.68 phi; 0.27 theta; -0.18 psi 2021-02-13T00:13:28Z- handleDepth()-Depth_Keller msg: 1613175208314 epoch sec; seqNo:10721; depth 30.07 2021-02-13T00:13:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:28Z- handleDvl() - RDI_Pathfinder msg: 1613175208.32 epoch sec; seqNo:5267 2021-02-13T00:13:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , -0.03 2021-02-13T00:13:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175208.33 epoch sec; seqNo:10721; 4058656.18 north; 592174.83 east 2021-02-13T00:13:28Z- AHRS_M2 msg: 1613175208.73 epoch sec; seqNo:10632 2021-02-13T00:13:28Z- AHRS_M2 msg: 0.67 phi; 0.28 theta; -0.18 psi 2021-02-13T00:13:28Z- handleDepth()-Depth_Keller msg: 1613175208738 epoch sec; seqNo:10722; depth 30.01 2021-02-13T00:13:28Z- handleDvl() - RDI_Pathfinder msg: 1613175208.75 epoch sec; seqNo:5268 2021-02-13T00:13:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.99 , 82.84 , 0.00 , 62.95 2021-02-13T00:13:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.10 , -0.04 2021-02-13T00:13:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175208.76 epoch sec; seqNo:10722; 4058656.18 north; 592174.83 east 2021-02-13T00:13:29Z- AHRS_M2 msg: 1613175209.12 epoch sec; seqNo:10633 2021-02-13T00:13:29Z- AHRS_M2 msg: 0.67 phi; 0.30 theta; -0.17 psi 2021-02-13T00:13:29Z- handleDepth()-Depth_Keller msg: 1613175209122 epoch sec; seqNo:10723; depth 29.96 2021-02-13T00:13:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175209.14 epoch sec; seqNo:10723; 4058656.57 north; 592176.05 east 2021-02-13T00:13:29Z- AHRS_M2 msg: 1613175209.52 epoch sec; seqNo:10634 2021-02-13T00:13:29Z- AHRS_M2 msg: 0.68 phi; 0.31 theta; -0.16 psi 2021-02-13T00:13:29Z- handleDepth()-Depth_Keller msg: 1613175209526 epoch sec; seqNo:10724; depth 29.88 2021-02-13T00:13:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:29Z- handleDvl() - RDI_Pathfinder msg: 1613175209.53 epoch sec; seqNo:5269 2021-02-13T00:13:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.10 , -0.04 2021-02-13T00:13:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175209.54 epoch sec; seqNo:10724; 4058656.95 north; 592176.04 east 2021-02-13T00:13:29Z- AHRS_M2 msg: 1613175209.92 epoch sec; seqNo:10635 2021-02-13T00:13:29Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.16 psi 2021-02-13T00:13:29Z- handleDepth()-Depth_Keller msg: 1613175209928 epoch sec; seqNo:10725; depth 29.80 2021-02-13T00:13:29Z- handleDvl() - RDI_Pathfinder msg: 1613175209.95 epoch sec; seqNo:5270 2021-02-13T00:13:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.63 , 81.76 , 0.00 , 60.09 2021-02-13T00:13:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.08 , -0.09 2021-02-13T00:13:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175209.96 epoch sec; seqNo:10725; 4058656.95 north; 592176.04 east 2021-02-13T00:13:30Z- LcmTrn::handleMessages() - handling 48 messages took 0.75 ms and 133 calls... 2021-02-13T00:13:30Z- waiting for 1613175207.70, time is 1613175209.96 2021-02-13T00:13:30Z- LcmTrn::updateTrn() equating pose 1613175209.96 and meas times 1613175209.96 2021-02-13T00:13:30Z- LcmTrn::updateTrn() >>>> pose time 1613175209.96, meas time 1613175209.96 <<<< 2021-02-13T00:13:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175209.96 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:13:30Z- TerrainNav::motionUpdate() - currEstimate = (4058657, 592176.04, 29.80) 2021-02-13T00:13:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:13:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:13:30Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175209.96 sec.; No relevant navigation data available 2021-02-13T00:13:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:13:30Z- TNavBF::mmse selected from filter -1 2021-02-13T00:13:30Z- variances([0], [2], and psi_berg) = 713.362423 833.005489 0.000000 2021-02-13T00:13:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:13:30Z- LcmTrn::publishEstimates() - MLE : 4058656.45 592225.54 29.80 2021-02-13T00:13:30Z- LcmTrn::publishEstimates() - MMSE : 4058656.95 592176.04 29.80 2021-02-13T00:13:30Z- LcmTrn::publishEstimates() - COVAR: 713.36 833.01 1.17 0.00 2021-02-13T00:13:30Z- AHRS_M2 msg: 1613175210.33 epoch sec; seqNo:10636 2021-02-13T00:13:30Z- AHRS_M2 msg: 0.69 phi; 0.32 theta; -0.15 psi 2021-02-13T00:13:30Z- handleDepth()-Depth_Keller msg: 1613175210332 epoch sec; seqNo:10726; depth 29.72 2021-02-13T00:13:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175210.34 epoch sec; seqNo:10726; 4058657.33 north; 592176.04 east 2021-02-13T00:13:30Z- AHRS_M2 msg: 1613175210.73 epoch sec; seqNo:10637 2021-02-13T00:13:30Z- AHRS_M2 msg: 0.69 phi; 0.33 theta; -0.15 psi 2021-02-13T00:13:30Z- handleDepth()-Depth_Keller msg: 1613175210740 epoch sec; seqNo:10727; depth 29.62 2021-02-13T00:13:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:30Z- handleDvl() - RDI_Pathfinder msg: 1613175210.74 epoch sec; seqNo:5271 2021-02-13T00:13:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.08 , -0.09 2021-02-13T00:13:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175210.75 epoch sec; seqNo:10727; 4058657.33 north; 592176.04 east 2021-02-13T00:13:31Z- AHRS_M2 msg: 1613175211.14 epoch sec; seqNo:10638 2021-02-13T00:13:31Z- AHRS_M2 msg: 0.70 phi; 0.33 theta; -0.14 psi 2021-02-13T00:13:31Z- handleDepth()-Depth_Keller msg: 1613175211144 epoch sec; seqNo:10728; depth 29.53 2021-02-13T00:13:31Z- handleDvl() - RDI_Pathfinder msg: 1613175211.16 epoch sec; seqNo:5272 2021-02-13T00:13:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.45 , 82.84 , 97.99 , 60.91 2021-02-13T00:13:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.06 , -0.13 2021-02-13T00:13:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175211.17 epoch sec; seqNo:10728; 4058657.72 north; 592177.25 east 2021-02-13T00:13:31Z- AHRS_M2 msg: 1613175211.54 epoch sec; seqNo:10639 2021-02-13T00:13:31Z- AHRS_M2 msg: 0.71 phi; 0.34 theta; -0.14 psi 2021-02-13T00:13:31Z- handleDepth()-Depth_Keller msg: 1613175211544 epoch sec; seqNo:10729; depth 29.42 2021-02-13T00:13:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175211.56 epoch sec; seqNo:10729; 4058657.72 north; 592177.25 east 2021-02-13T00:13:31Z- AHRS_M2 msg: 1613175211.94 epoch sec; seqNo:10640 2021-02-13T00:13:31Z- AHRS_M2 msg: 0.71 phi; 0.34 theta; -0.14 psi 2021-02-13T00:13:31Z- handleDepth()-Depth_Keller msg: 1613175211954 epoch sec; seqNo:10730; depth 29.34 2021-02-13T00:13:31Z- handleDvl() - RDI_Pathfinder msg: 1613175211.97 epoch sec; seqNo:5273 2021-02-13T00:13:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.86 , 0.00 , 97.99 , 62.13 2021-02-13T00:13:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.00 , -0.16 2021-02-13T00:13:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175211.98 epoch sec; seqNo:10730; 4058658.10 north; 592177.25 east 2021-02-13T00:13:32Z- AHRS_M2 msg: 1613175212.35 epoch sec; seqNo:10641 2021-02-13T00:13:32Z- AHRS_M2 msg: 0.72 phi; 0.34 theta; -0.13 psi 2021-02-13T00:13:32Z- handleDepth()-Depth_Keller msg: 1613175212355 epoch sec; seqNo:10731; depth 29.22 2021-02-13T00:13:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175212.37 epoch sec; seqNo:10731; 4058658.48 north; 592177.25 east 2021-02-13T00:13:32Z- AHRS_M2 msg: 1613175212.76 epoch sec; seqNo:10642 2021-02-13T00:13:32Z- AHRS_M2 msg: 0.72 phi; 0.34 theta; -0.13 psi 2021-02-13T00:13:32Z- handleDepth()-Depth_Keller msg: 1613175212782 epoch sec; seqNo:10732; depth 29.10 2021-02-13T00:13:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175212.80 epoch sec; seqNo:10732; 4058658.48 north; 592177.25 east 2021-02-13T00:13:33Z- AHRS_M2 msg: 1613175213.16 epoch sec; seqNo:10643 2021-02-13T00:13:33Z- AHRS_M2 msg: 0.72 phi; 0.34 theta; -0.13 psi 2021-02-13T00:13:33Z- handleDepth()-Depth_Keller msg: 1613175213162 epoch sec; seqNo:10733; depth 28.99 2021-02-13T00:13:33Z- handleDvl() - RDI_Pathfinder msg: 1613175213.17 epoch sec; seqNo:5274 2021-02-13T00:13:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.67 , 82.84 , 94.75 , 61.31 2021-02-13T00:13:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.08 , -0.14 2021-02-13T00:13:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175213.20 epoch sec; seqNo:10733; 4058658.86 north; 592177.24 east 2021-02-13T00:13:33Z- AHRS_M2 msg: 1613175213.56 epoch sec; seqNo:10644 2021-02-13T00:13:33Z- AHRS_M2 msg: 0.72 phi; 0.34 theta; -0.13 psi 2021-02-13T00:13:33Z- handleDepth()-Depth_Keller msg: 1613175213564 epoch sec; seqNo:10734; depth 28.88 2021-02-13T00:13:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175213.58 epoch sec; seqNo:10734; 4058658.86 north; 592177.24 east 2021-02-13T00:13:33Z- AHRS_M2 msg: 1613175213.96 epoch sec; seqNo:10645 2021-02-13T00:13:33Z- AHRS_M2 msg: 0.73 phi; 0.34 theta; -0.13 psi 2021-02-13T00:13:33Z- handleDepth()-Depth_Keller msg: 1613175213970 epoch sec; seqNo:10735; depth 28.78 2021-02-13T00:13:33Z- handleDvl() - RDI_Pathfinder msg: 1613175213.98 epoch sec; seqNo:5275 2021-02-13T00:13:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.26 , 0.00 , 97.99 , 61.31 2021-02-13T00:13:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.17 , -0.06 2021-02-13T00:13:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175213.99 epoch sec; seqNo:10735; 4058659.25 north; 592178.46 east 2021-02-13T00:13:34Z- AHRS_M2 msg: 1613175214.37 epoch sec; seqNo:10646 2021-02-13T00:13:34Z- AHRS_M2 msg: 0.73 phi; 0.34 theta; -0.14 psi 2021-02-13T00:13:34Z- handleDepth()-Depth_Keller msg: 1613175214372 epoch sec; seqNo:10736; depth 28.64 2021-02-13T00:13:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:34Z- handleDvl() - RDI_Pathfinder msg: 1613175214.37 epoch sec; seqNo:5276 2021-02-13T00:13:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.17 , -0.06 2021-02-13T00:13:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175214.39 epoch sec; seqNo:10736; 4058659.63 north; 592178.45 east 2021-02-13T00:13:34Z- AHRS_M2 msg: 1613175214.79 epoch sec; seqNo:10647 2021-02-13T00:13:34Z- AHRS_M2 msg: 0.72 phi; 0.34 theta; -0.14 psi 2021-02-13T00:13:34Z- handleDepth()-Depth_Keller msg: 1613175214800 epoch sec; seqNo:10737; depth 28.54 2021-02-13T00:13:34Z- handleDvl() - RDI_Pathfinder msg: 1613175214.82 epoch sec; seqNo:5277 2021-02-13T00:13:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.26 , 82.84 , 97.99 , 62.54 2021-02-13T00:13:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.17 , 0.04 2021-02-13T00:13:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175214.83 epoch sec; seqNo:10737; 4058659.63 north; 592178.45 east 2021-02-13T00:13:35Z- LcmTrn::handleMessages() - handling 43 messages took 0.60 ms and 131 calls... 2021-02-13T00:13:35Z- waiting for 1613175212.96, time is 1613175214.83 2021-02-13T00:13:35Z- LcmTrn::updateTrn() equating pose 1613175214.83 and meas times 1613175214.83 2021-02-13T00:13:35Z- LcmTrn::updateTrn() >>>> pose time 1613175214.83, meas time 1613175214.83 <<<< 2021-02-13T00:13:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175214.83 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:13:35Z- TerrainNav::motionUpdate() - currEstimate = (4058660, 592178.45, 28.54) 2021-02-13T00:13:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:13:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:13:35Z- TNavBF::Projecting Measurements 2021-02-13T00:13:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:13:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:13:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:13:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:13:35Z- TNavFilter::4 good beams. 2021-02-13T00:13:35Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:13:35Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:13:35Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:13:35Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:13:35Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:13:35Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:13:35Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:13:35Z- TNavBF::mmse selected from filter -1 2021-02-13T00:13:35Z- TerrainMapOctree: (vehN, vehE ) = (4058659.63, 592178.45). 2021-02-13T00:13:35Z- TerrainMapOctree: Min Distance = 3316.83. 2021-02-13T00:13:35Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:13:35Z- There is no specific good beam that all particles have in common. 2021-02-13T00:13:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:13:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.36 K: 0.129 ExpectedMeasDiff: nan currMeas.covariance: 7.46 2021-02-13T00:13:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:13:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 10.98 2021-02-13T00:13:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:13:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:13:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:13:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 6.26 2021-02-13T00:13:35Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:13:35Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:13:35Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:13:35Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:13:35Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:13:35Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:13:35Z- Filter 0 No beams used in subcloud update 2021-02-13T00:13:35Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:13:35Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:13:35Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 713.3 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:13:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175214.83 sec. 2021-02-13T00:13:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:13:35Z- TNavBF::mmse selected from filter -1 2021-02-13T00:13:35Z- variances([0], [2], and psi_berg) = 713.302991 833.005453 0.000000 2021-02-13T00:13:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:13:35Z- LcmTrn::publishEstimates() - MLE : 4058659.13 592227.95 28.54 2021-02-13T00:13:35Z- LcmTrn::publishEstimates() - MMSE : 4058659.63 592178.45 28.54 2021-02-13T00:13:35Z- LcmTrn::publishEstimates() - COVAR: 713.30 833.01 0.90 0.00 2021-02-13T00:13:35Z- AHRS_M2 msg: 1613175215.18 epoch sec; seqNo:10648 2021-02-13T00:13:35Z- AHRS_M2 msg: 0.72 phi; 0.34 theta; -0.14 psi 2021-02-13T00:13:35Z- handleDepth()-Depth_Keller msg: 1613175215187 epoch sec; seqNo:10738; depth 28.42 2021-02-13T00:13:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175215.20 epoch sec; seqNo:10738; 4058660.01 north; 592178.45 east 2021-02-13T00:13:35Z- AHRS_M2 msg: 1613175215.58 epoch sec; seqNo:10649 2021-02-13T00:13:35Z- AHRS_M2 msg: 0.72 phi; 0.34 theta; -0.14 psi 2021-02-13T00:13:35Z- handleDepth()-Depth_Keller msg: 1613175215584 epoch sec; seqNo:10739; depth 28.31 2021-02-13T00:13:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:35Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:35Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:35Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:35Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:35Z- handleDvl() - RDI_Pathfinder msg: 1613175215.59 epoch sec; seqNo:5278 2021-02-13T00:13:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.17 , 0.04 2021-02-13T00:13:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175215.60 epoch sec; seqNo:10739; 4058660.38 north; 592178.45 east 2021-02-13T00:13:35Z- AHRS_M2 msg: 1613175215.98 epoch sec; seqNo:10650 2021-02-13T00:13:35Z- AHRS_M2 msg: 0.72 phi; 0.34 theta; -0.14 psi 2021-02-13T00:13:36Z- handleDepth()-Depth_Keller msg: 1613175216009 epoch sec; seqNo:10740; depth 28.20 2021-02-13T00:13:36Z- handleDvl() - RDI_Pathfinder msg: 1613175216.03 epoch sec; seqNo:5279 2021-02-13T00:13:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.27 , 83.92 , 97.99 , 60.91 2021-02-13T00:13:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.14 , 0.12 2021-02-13T00:13:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175216.04 epoch sec; seqNo:10740; 4058660.38 north; 592178.45 east 2021-02-13T00:13:36Z- AHRS_M2 msg: 1613175216.39 epoch sec; seqNo:10651 2021-02-13T00:13:36Z- AHRS_M2 msg: 0.72 phi; 0.34 theta; -0.14 psi 2021-02-13T00:13:36Z- handleDepth()-Depth_Keller msg: 1613175216394 epoch sec; seqNo:10741; depth 28.07 2021-02-13T00:13:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175216.40 epoch sec; seqNo:10741; 4058660.76 north; 592178.44 east 2021-02-13T00:13:36Z- AHRS_M2 msg: 1613175216.80 epoch sec; seqNo:10652 2021-02-13T00:13:36Z- AHRS_M2 msg: 0.72 phi; 0.34 theta; -0.14 psi 2021-02-13T00:13:36Z- handleDepth()-Depth_Keller msg: 1613175216811 epoch sec; seqNo:10742; depth 27.97 2021-02-13T00:13:36Z- handleDvl() - RDI_Pathfinder msg: 1613175216.82 epoch sec; seqNo:5280 2021-02-13T00:13:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.68 , 83.92 , 97.99 , 61.73 2021-02-13T00:13:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.09 , 0.16 2021-02-13T00:13:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175216.83 epoch sec; seqNo:10742; 4058661.14 north; 592178.44 east 2021-02-13T00:13:37Z- AHRS_M2 msg: 1613175217.19 epoch sec; seqNo:10653 2021-02-13T00:13:37Z- AHRS_M2 msg: 0.72 phi; 0.34 theta; -0.14 psi 2021-02-13T00:13:37Z- handleDepth()-Depth_Keller msg: 1613175217199 epoch sec; seqNo:10743; depth 27.84 2021-02-13T00:13:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175217.21 epoch sec; seqNo:10743; 4058661.53 north; 592179.65 east 2021-02-13T00:13:37Z- AHRS_M2 msg: 1613175217.61 epoch sec; seqNo:10654 2021-02-13T00:13:37Z- AHRS_M2 msg: 0.72 phi; 0.34 theta; -0.14 psi 2021-02-13T00:13:37Z- handleDepth()-Depth_Keller msg: 1613175217613 epoch sec; seqNo:10744; depth 27.72 2021-02-13T00:13:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175217.64 epoch sec; seqNo:10744; 4058661.53 north; 592179.65 east 2021-02-13T00:13:38Z- AHRS_M2 msg: 1613175218.00 epoch sec; seqNo:10655 2021-02-13T00:13:38Z- AHRS_M2 msg: 0.72 phi; 0.34 theta; -0.14 psi 2021-02-13T00:13:38Z- handleDepth()-Depth_Keller msg: 1613175218010 epoch sec; seqNo:10745; depth 27.61 2021-02-13T00:13:38Z- handleDvl() - RDI_Pathfinder msg: 1613175218.02 epoch sec; seqNo:5281 2021-02-13T00:13:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:38Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:13:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175218.03 epoch sec; seqNo:10745; 4058661.91 north; 592179.65 east 2021-02-13T00:13:38Z- AHRS_M2 msg: 1613175218.41 epoch sec; seqNo:10656 2021-02-13T00:13:38Z- AHRS_M2 msg: 0.72 phi; 0.34 theta; -0.14 psi 2021-02-13T00:13:38Z- handleDepth()-Depth_Keller msg: 1613175218413 epoch sec; seqNo:10746; depth 27.50 2021-02-13T00:13:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175218.44 epoch sec; seqNo:10746; 4058662.29 north; 592179.65 east 2021-02-13T00:13:38Z- AHRS_M2 msg: 1613175218.81 epoch sec; seqNo:10657 2021-02-13T00:13:38Z- AHRS_M2 msg: 0.73 phi; 0.34 theta; -0.14 psi 2021-02-13T00:13:38Z- handleDepth()-Depth_Keller msg: 1613175218816 epoch sec; seqNo:10747; depth 27.39 2021-02-13T00:13:38Z- handleDvl() - RDI_Pathfinder msg: 1613175218.83 epoch sec; seqNo:5282 2021-02-13T00:13:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 70.39 , 0.00 , 0.00 , 62.11 2021-02-13T00:13:38Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:13:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175218.86 epoch sec; seqNo:10747; 4058662.67 north; 592179.64 east 2021-02-13T00:13:39Z- AHRS_M2 msg: 1613175219.21 epoch sec; seqNo:10658 2021-02-13T00:13:39Z- AHRS_M2 msg: 0.73 phi; 0.34 theta; -0.14 psi 2021-02-13T00:13:39Z- handleDepth()-Depth_Keller msg: 1613175219221 epoch sec; seqNo:10748; depth 27.27 2021-02-13T00:13:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:39Z- handleDvl() - RDI_Pathfinder msg: 1613175219.22 epoch sec; seqNo:5283 2021-02-13T00:13:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:39Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:13:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175219.23 epoch sec; seqNo:10748; 4058662.67 north; 592179.64 east 2021-02-13T00:13:39Z- AHRS_M2 msg: 1613175219.62 epoch sec; seqNo:10659 2021-02-13T00:13:39Z- AHRS_M2 msg: 0.73 phi; 0.34 theta; -0.14 psi 2021-02-13T00:13:39Z- handleDepth()-Depth_Keller msg: 1613175219625 epoch sec; seqNo:10749; depth 27.14 2021-02-13T00:13:39Z- handleDvl() - RDI_Pathfinder msg: 1613175219.64 epoch sec; seqNo:5284 2021-02-13T00:13:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 70.86 , 0.00 , 0.00 , 63.49 2021-02-13T00:13:39Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:13:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175219.65 epoch sec; seqNo:10749; 4058663.06 north; 592180.86 east 2021-02-13T00:13:40Z- AHRS_M2 msg: 1613175220.02 epoch sec; seqNo:10660 2021-02-13T00:13:40Z- AHRS_M2 msg: 0.74 phi; 0.34 theta; -0.14 psi 2021-02-13T00:13:40Z- handleDepth()-Depth_Keller msg: 1613175220028 epoch sec; seqNo:10750; depth 27.03 2021-02-13T00:13:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175220.05 epoch sec; seqNo:10750; 4058663.44 north; 592180.85 east 2021-02-13T00:13:40Z- LcmTrn::handleMessages() - handling 46 messages took 0.78 ms and 131 calls... 2021-02-13T00:13:40Z- waiting for 1613175217.83, time is 1613175220.05 2021-02-13T00:13:40Z- LcmTrn::updateTrn() >>>> pose time 1613175220.05, meas time 1613175219.64 <<<< 2021-02-13T00:13:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175219.64 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:13:40Z- TerrainNav::motionUpdate() - currEstimate = (4058663, 592180.85, 27.03) 2021-02-13T00:13:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:13:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:13:40Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:13:40Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:13:40Z- TNavBF::Projecting Measurements 2021-02-13T00:13:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:13:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:13:40Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-13T00:13:40Z- TNavFilter::2 good beams. 2021-02-13T00:13:40Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:13:40Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:13:40Z- TNavBF:: beamIndex[1] = 3 2021-02-13T00:13:40Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:13:40Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:13:40Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-13T00:13:40Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:13:40Z- TNavBF::mmse selected from filter -1 2021-02-13T00:13:40Z- TerrainMapOctree: (vehN, vehE ) = (4058663.14, 592180.67). 2021-02-13T00:13:40Z- TerrainMapOctree: Min Distance = 3312.70. 2021-02-13T00:13:40Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:13:40Z- There is no specific good beam that all particles have in common. 2021-02-13T00:13:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:13:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 8.03 2021-02-13T00:13:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:13:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 6.45 2021-02-13T00:13:40Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:13:40Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:13:40Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:13:40Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:13:40Z- Filter 0 No beams used in subcloud update 2021-02-13T00:13:40Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:13:40Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:13:40Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 713.2 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:13:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175219.64 sec. 2021-02-13T00:13:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:13:40Z- TNavBF::mmse selected from filter -1 2021-02-13T00:13:40Z- variances([0], [2], and psi_berg) = 713.220686 833.005403 0.000000 2021-02-13T00:13:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:13:40Z- LcmTrn::publishEstimates() - MLE : 4058662.94 592230.35 27.03 2021-02-13T00:13:40Z- LcmTrn::publishEstimates() - MMSE : 4058663.44 592180.85 27.03 2021-02-13T00:13:40Z- LcmTrn::publishEstimates() - COVAR: 713.22 833.01 0.91 0.00 2021-02-13T00:13:40Z- AHRS_M2 msg: 1613175220.43 epoch sec; seqNo:10661 2021-02-13T00:13:40Z- AHRS_M2 msg: 0.74 phi; 0.34 theta; -0.14 psi 2021-02-13T00:13:40Z- handleDepth()-Depth_Keller msg: 1613175220441 epoch sec; seqNo:10751; depth 26.92 2021-02-13T00:13:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:40Z- handleDvl() - RDI_Pathfinder msg: 1613175220.44 epoch sec; seqNo:5285 2021-02-13T00:13:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:40Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:13:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175220.48 epoch sec; seqNo:10751; 4058663.82 north; 592180.85 east 2021-02-13T00:13:40Z- AHRS_M2 msg: 1613175220.84 epoch sec; seqNo:10662 2021-02-13T00:13:40Z- AHRS_M2 msg: 0.75 phi; 0.34 theta; -0.15 psi 2021-02-13T00:13:40Z- handleDepth()-Depth_Keller msg: 1613175220845 epoch sec; seqNo:10752; depth 26.80 2021-02-13T00:13:40Z- handleDvl() - RDI_Pathfinder msg: 1613175220.86 epoch sec; seqNo:5286 2021-02-13T00:13:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 70.86 , 0.00 , 0.00 , 62.57 2021-02-13T00:13:40Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:13:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175220.88 epoch sec; seqNo:10752; 4058663.82 north; 592180.85 east 2021-02-13T00:13:41Z- AHRS_M2 msg: 1613175221.24 epoch sec; seqNo:10663 2021-02-13T00:13:41Z- AHRS_M2 msg: 0.75 phi; 0.34 theta; -0.14 psi 2021-02-13T00:13:41Z- handleDepth()-Depth_Keller msg: 1613175221242 epoch sec; seqNo:10753; depth 26.68 2021-02-13T00:13:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175221.26 epoch sec; seqNo:10753; 4058664.20 north; 592180.85 east 2021-02-13T00:13:41Z- AHRS_M2 msg: 1613175221.64 epoch sec; seqNo:10664 2021-02-13T00:13:41Z- AHRS_M2 msg: 0.76 phi; 0.34 theta; -0.14 psi 2021-02-13T00:13:41Z- handleDepth()-Depth_Keller msg: 1613175221644 epoch sec; seqNo:10754; depth 26.57 2021-02-13T00:13:41Z- handleDvl() - RDI_Pathfinder msg: 1613175221.66 epoch sec; seqNo:5287 2021-02-13T00:13:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:41Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:13:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175221.67 epoch sec; seqNo:10754; 4058664.59 north; 592182.06 east 2021-02-13T00:13:42Z- AHRS_M2 msg: 1613175222.04 epoch sec; seqNo:10665 2021-02-13T00:13:42Z- AHRS_M2 msg: 0.76 phi; 0.34 theta; -0.14 psi 2021-02-13T00:13:42Z- handleDepth()-Depth_Keller msg: 1613175222053 epoch sec; seqNo:10755; depth 26.45 2021-02-13T00:13:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175222.07 epoch sec; seqNo:10755; 4058664.59 north; 592182.06 east 2021-02-13T00:13:42Z- AHRS_M2 msg: 1613175222.45 epoch sec; seqNo:10666 2021-02-13T00:13:42Z- AHRS_M2 msg: 0.77 phi; 0.34 theta; -0.13 psi 2021-02-13T00:13:42Z- handleDepth()-Depth_Keller msg: 1613175222457 epoch sec; seqNo:10756; depth 26.34 2021-02-13T00:13:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175222.46 epoch sec; seqNo:10756; 4058664.97 north; 592182.06 east 2021-02-13T00:13:42Z- AHRS_M2 msg: 1613175222.85 epoch sec; seqNo:10667 2021-02-13T00:13:42Z- AHRS_M2 msg: 0.77 phi; 0.34 theta; -0.12 psi 2021-02-13T00:13:42Z- handleDepth()-Depth_Keller msg: 1613175222856 epoch sec; seqNo:10757; depth 26.23 2021-02-13T00:13:42Z- handleDvl() - RDI_Pathfinder msg: 1613175222.88 epoch sec; seqNo:5288 2021-02-13T00:13:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.94 , 83.93 , 96.91 , 62.54 2021-02-13T00:13:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , -0.04 2021-02-13T00:13:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175222.88 epoch sec; seqNo:10757; 4058665.35 north; 592182.06 east 2021-02-13T00:13:43Z- AHRS_M2 msg: 1613175223.26 epoch sec; seqNo:10668 2021-02-13T00:13:43Z- AHRS_M2 msg: 0.78 phi; 0.34 theta; -0.11 psi 2021-02-13T00:13:43Z- handleDepth()-Depth_Keller msg: 1613175223264 epoch sec; seqNo:10758; depth 26.11 2021-02-13T00:13:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175223.29 epoch sec; seqNo:10758; 4058665.35 north; 592182.06 east 2021-02-13T00:13:43Z- AHRS_M2 msg: 1613175223.66 epoch sec; seqNo:10669 2021-02-13T00:13:43Z- AHRS_M2 msg: 0.78 phi; 0.33 theta; -0.11 psi 2021-02-13T00:13:43Z- handleDepth()-Depth_Keller msg: 1613175223664 epoch sec; seqNo:10759; depth 25.98 2021-02-13T00:13:43Z- handleDvl() - RDI_Pathfinder msg: 1613175223.67 epoch sec; seqNo:5289 2021-02-13T00:13:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:43Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:13:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175223.69 epoch sec; seqNo:10759; 4058665.73 north; 592182.05 east 2021-02-13T00:13:44Z- AHRS_M2 msg: 1613175224.07 epoch sec; seqNo:10670 2021-02-13T00:13:44Z- AHRS_M2 msg: 0.79 phi; 0.33 theta; -0.10 psi 2021-02-13T00:13:44Z- handleDepth()-Depth_Keller msg: 1613175224069 epoch sec; seqNo:10760; depth 25.86 2021-02-13T00:13:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:44Z- handleDvl() - RDI_Pathfinder msg: 1613175224.07 epoch sec; seqNo:5290 2021-02-13T00:13:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:44Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:13:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175224.08 epoch sec; seqNo:10760; 4058665.74 north; 592183.27 east 2021-02-13T00:13:44Z- AHRS_M2 msg: 1613175224.50 epoch sec; seqNo:10671 2021-02-13T00:13:44Z- AHRS_M2 msg: 0.79 phi; 0.34 theta; -0.11 psi 2021-02-13T00:13:44Z- handleDepth()-Depth_Keller msg: 1613175224500 epoch sec; seqNo:10761; depth 25.74 2021-02-13T00:13:44Z- handleDvl() - RDI_Pathfinder msg: 1613175224.51 epoch sec; seqNo:5291 2021-02-13T00:13:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 73.16 , 0.00 , 0.00 , 64.41 2021-02-13T00:13:44Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:13:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175224.52 epoch sec; seqNo:10761; 4058666.12 north; 592183.27 east 2021-02-13T00:13:44Z- handleDepth()-Depth_Keller msg: 1613175224890 epoch sec; seqNo:10762; depth 25.60 2021-02-13T00:13:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175224.91 epoch sec; seqNo:10762; 4058666.50 north; 592183.26 east 2021-02-13T00:13:45Z- LcmTrn::handleMessages() - handling 42 messages took 0.70 ms and 128 calls... 2021-02-13T00:13:45Z- waiting for 1613175223.05, time is 1613175224.91 2021-02-13T00:13:45Z- LcmTrn::updateTrn() >>>> pose time 1613175224.91, meas time 1613175224.51 <<<< 2021-02-13T00:13:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175224.51 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:13:45Z- TerrainNav::motionUpdate() - currEstimate = (4058666, 592183.26, 25.60) 2021-02-13T00:13:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:13:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:13:45Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:13:45Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:13:45Z- TNavBF::Projecting Measurements 2021-02-13T00:13:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:13:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:13:45Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-13T00:13:45Z- TNavFilter::2 good beams. 2021-02-13T00:13:45Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:13:45Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:13:45Z- TNavBF:: beamIndex[1] = 3 2021-02-13T00:13:45Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:13:45Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:13:45Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-13T00:13:45Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:13:45Z- TNavBF::mmse selected from filter -1 2021-02-13T00:13:45Z- TerrainMapOctree: (vehN, vehE ) = (4058666.25, 592183.07). 2021-02-13T00:13:45Z- TerrainMapOctree: Min Distance = 3308.77. 2021-02-13T00:13:45Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:13:45Z- There is no specific good beam that all particles have in common. 2021-02-13T00:13:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:13:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-13T00:13:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:13:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 6.64 2021-02-13T00:13:45Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:13:45Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:13:45Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:13:45Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:13:45Z- Filter 0 No beams used in subcloud update 2021-02-13T00:13:45Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:13:45Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:13:45Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 713.2 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:13:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175224.51 sec. 2021-02-13T00:13:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:13:45Z- TNavBF::mmse selected from filter -1 2021-02-13T00:13:45Z- variances([0], [2], and psi_berg) = 713.144040 833.005357 0.000000 2021-02-13T00:13:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:13:45Z- LcmTrn::publishEstimates() - MLE : 4058666.00 592232.76 25.60 2021-02-13T00:13:45Z- LcmTrn::publishEstimates() - MMSE : 4058666.50 592183.26 25.60 2021-02-13T00:13:45Z- LcmTrn::publishEstimates() - COVAR: 713.14 833.01 0.90 0.00 2021-02-13T00:13:45Z- AHRS_M2 msg: 1613175225.29 epoch sec; seqNo:10672 2021-02-13T00:13:45Z- AHRS_M2 msg: 0.80 phi; 0.33 theta; -0.11 psi 2021-02-13T00:13:45Z- handleDepth()-Depth_Keller msg: 1613175225291 epoch sec; seqNo:10763; depth 25.50 2021-02-13T00:13:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:45Z- handleDvl() - RDI_Pathfinder msg: 1613175225.29 epoch sec; seqNo:5292 2021-02-13T00:13:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:45Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:13:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175225.30 epoch sec; seqNo:10763; 4058666.50 north; 592183.26 east 2021-02-13T00:13:45Z- AHRS_M2 msg: 1613175225.68 epoch sec; seqNo:10673 2021-02-13T00:13:45Z- AHRS_M2 msg: 0.80 phi; 0.33 theta; -0.12 psi 2021-02-13T00:13:45Z- handleDepth()-Depth_Keller msg: 1613175225684 epoch sec; seqNo:10764; depth 25.38 2021-02-13T00:13:45Z- handleDvl() - RDI_Pathfinder msg: 1613175225.70 epoch sec; seqNo:5293 2021-02-13T00:13:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 73.16 , 0.00 , 0.00 , 64.87 2021-02-13T00:13:45Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:13:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175225.71 epoch sec; seqNo:10764; 4058666.89 north; 592184.48 east 2021-02-13T00:13:46Z- AHRS_M2 msg: 1613175226.08 epoch sec; seqNo:10674 2021-02-13T00:13:46Z- AHRS_M2 msg: 0.81 phi; 0.33 theta; -0.12 psi 2021-02-13T00:13:46Z- handleDepth()-Depth_Keller msg: 1613175226097 epoch sec; seqNo:10765; depth 25.25 2021-02-13T00:13:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175226.11 epoch sec; seqNo:10765; 4058666.89 north; 592184.48 east 2021-02-13T00:13:46Z- handleDepth()-Depth_Keller msg: 1613175226513 epoch sec; seqNo:10766; depth 25.13 2021-02-13T00:13:46Z- handleDvl() - RDI_Pathfinder msg: 1613175226.53 epoch sec; seqNo:5294 2021-02-13T00:13:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 73.16 , 0.00 , 0.00 , 64.41 2021-02-13T00:13:46Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:13:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175226.54 epoch sec; seqNo:10766; 4058667.27 north; 592184.48 east 2021-02-13T00:13:46Z- AHRS_M2 msg: 1613175226.89 epoch sec; seqNo:10675 2021-02-13T00:13:46Z- AHRS_M2 msg: 0.82 phi; 0.33 theta; -0.12 psi 2021-02-13T00:13:46Z- handleDepth()-Depth_Keller msg: 1613175226901 epoch sec; seqNo:10767; depth 25.00 2021-02-13T00:13:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175226.91 epoch sec; seqNo:10767; 4058667.65 north; 592184.47 east 2021-02-13T00:13:47Z- AHRS_M2 msg: 1613175227.30 epoch sec; seqNo:10676 2021-02-13T00:13:47Z- AHRS_M2 msg: 0.82 phi; 0.33 theta; -0.13 psi 2021-02-13T00:13:47Z- handleDepth()-Depth_Keller msg: 1613175227306 epoch sec; seqNo:10768; depth 24.87 2021-02-13T00:13:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175227.31 epoch sec; seqNo:10768; 4058667.65 north; 592184.47 east 2021-02-13T00:13:47Z- AHRS_M2 msg: 1613175227.70 epoch sec; seqNo:10677 2021-02-13T00:13:47Z- AHRS_M2 msg: 0.82 phi; 0.33 theta; -0.13 psi 2021-02-13T00:13:47Z- handleDepth()-Depth_Keller msg: 1613175227706 epoch sec; seqNo:10769; depth 24.78 2021-02-13T00:13:47Z- handleDvl() - RDI_Pathfinder msg: 1613175227.72 epoch sec; seqNo:5295 2021-02-13T00:13:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:47Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:13:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175227.73 epoch sec; seqNo:10769; 4058668.04 north; 592185.69 east 2021-02-13T00:13:48Z- AHRS_M2 msg: 1613175228.10 epoch sec; seqNo:10678 2021-02-13T00:13:48Z- AHRS_M2 msg: 0.82 phi; 0.33 theta; -0.14 psi 2021-02-13T00:13:48Z- handleDepth()-Depth_Keller msg: 1613175228113 epoch sec; seqNo:10770; depth 24.64 2021-02-13T00:13:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175228.14 epoch sec; seqNo:10770; 4058668.04 north; 592185.69 east 2021-02-13T00:13:48Z- AHRS_M2 msg: 1613175228.51 epoch sec; seqNo:10679 2021-02-13T00:13:48Z- AHRS_M2 msg: 0.82 phi; 0.33 theta; -0.14 psi 2021-02-13T00:13:48Z- handleDepth()-Depth_Keller msg: 1613175228512 epoch sec; seqNo:10771; depth 24.52 2021-02-13T00:13:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175228.52 epoch sec; seqNo:10771; 4058668.42 north; 592185.68 east 2021-02-13T00:13:48Z- AHRS_M2 msg: 1613175228.91 epoch sec; seqNo:10680 2021-02-13T00:13:48Z- AHRS_M2 msg: 0.81 phi; 0.33 theta; -0.15 psi 2021-02-13T00:13:48Z- handleDepth()-Depth_Keller msg: 1613175228921 epoch sec; seqNo:10772; depth 24.40 2021-02-13T00:13:48Z- handleDvl() - RDI_Pathfinder msg: 1613175228.93 epoch sec; seqNo:5296 2021-02-13T00:13:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.77 , 87.17 , 99.07 , 64.18 2021-02-13T00:13:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.16 , 0.02 2021-02-13T00:13:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175228.94 epoch sec; seqNo:10772; 4058668.80 north; 592185.68 east 2021-02-13T00:13:49Z- AHRS_M2 msg: 1613175229.32 epoch sec; seqNo:10681 2021-02-13T00:13:49Z- AHRS_M2 msg: 0.80 phi; 0.33 theta; -0.15 psi 2021-02-13T00:13:49Z- handleDepth()-Depth_Keller msg: 1613175229326 epoch sec; seqNo:10773; depth 24.26 2021-02-13T00:13:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175229.34 epoch sec; seqNo:10773; 4058668.80 north; 592185.68 east 2021-02-13T00:13:49Z- AHRS_M2 msg: 1613175229.73 epoch sec; seqNo:10682 2021-02-13T00:13:49Z- AHRS_M2 msg: 0.80 phi; 0.33 theta; -0.16 psi 2021-02-13T00:13:49Z- handleDepth()-Depth_Keller msg: 1613175229739 epoch sec; seqNo:10774; depth 24.16 2021-02-13T00:13:49Z- handleDvl() - RDI_Pathfinder msg: 1613175229.75 epoch sec; seqNo:5297 2021-02-13T00:13:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.18 , 0.00 , 102.25 , 65.82 2021-02-13T00:13:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.15 , 0.14 2021-02-13T00:13:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175229.76 epoch sec; seqNo:10774; 4058669.18 north; 592185.68 east 2021-02-13T00:13:50Z- AHRS_M2 msg: 1613175230.13 epoch sec; seqNo:10683 2021-02-13T00:13:50Z- AHRS_M2 msg: 0.79 phi; 0.33 theta; -0.17 psi 2021-02-13T00:13:50Z- handleDepth()-Depth_Keller msg: 1613175230131 epoch sec; seqNo:10775; depth 24.02 2021-02-13T00:13:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:50Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:50Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:50Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:50Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:50Z- handleDvl() - RDI_Pathfinder msg: 1613175230.13 epoch sec; seqNo:5298 2021-02-13T00:13:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.15 , 0.14 2021-02-13T00:13:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175230.15 epoch sec; seqNo:10775; 4058669.57 north; 592186.89 east 2021-02-13T00:13:50Z- LcmTrn::handleMessages() - handling 45 messages took 0.63 ms and 133 calls... 2021-02-13T00:13:50Z- waiting for 1613175227.91, time is 1613175230.15 2021-02-13T00:13:50Z- LcmTrn::updateTrn() equating pose 1613175230.15 and meas times 1613175230.15 2021-02-13T00:13:50Z- LcmTrn::updateTrn() >>>> pose time 1613175230.15, meas time 1613175230.15 <<<< 2021-02-13T00:13:50Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175230.15, ping # 0. 2021-02-13T00:13:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175230.15 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:13:50Z- TerrainNav::motionUpdate() - currEstimate = (4058670, 592186.89, 24.02) 2021-02-13T00:13:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:13:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:13:50Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:13:50Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:13:50Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175230.15 sec.; No relevant navigation data available 2021-02-13T00:13:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:13:50Z- TNavBF::mmse selected from filter -1 2021-02-13T00:13:50Z- variances([0], [2], and psi_berg) = 713.075955 833.005316 0.000000 2021-02-13T00:13:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:13:50Z- LcmTrn::publishEstimates() - MLE : 4058669.07 592236.39 24.02 2021-02-13T00:13:50Z- LcmTrn::publishEstimates() - MMSE : 4058669.57 592186.89 24.02 2021-02-13T00:13:50Z- LcmTrn::publishEstimates() - COVAR: 713.08 833.01 1.14 0.00 2021-02-13T00:13:50Z- AHRS_M2 msg: 1613175230.53 epoch sec; seqNo:10684 2021-02-13T00:13:50Z- AHRS_M2 msg: 0.78 phi; 0.33 theta; -0.18 psi 2021-02-13T00:13:50Z- handleDepth()-Depth_Keller msg: 1613175230539 epoch sec; seqNo:10776; depth 23.91 2021-02-13T00:13:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175230.55 epoch sec; seqNo:10776; 4058669.57 north; 592186.89 east 2021-02-13T00:13:50Z- AHRS_M2 msg: 1613175230.93 epoch sec; seqNo:10685 2021-02-13T00:13:50Z- AHRS_M2 msg: 0.77 phi; 0.33 theta; -0.18 psi 2021-02-13T00:13:50Z- handleDepth()-Depth_Keller msg: 1613175230936 epoch sec; seqNo:10777; depth 23.79 2021-02-13T00:13:50Z- handleDvl() - RDI_Pathfinder msg: 1613175230.95 epoch sec; seqNo:5299 2021-02-13T00:13:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.36 , 89.26 , 100.08 , 66.23 2021-02-13T00:13:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.07 , 0.22 2021-02-13T00:13:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175230.97 epoch sec; seqNo:10777; 4058669.95 north; 592186.89 east 2021-02-13T00:13:51Z- AHRS_M2 msg: 1613175231.34 epoch sec; seqNo:10686 2021-02-13T00:13:51Z- AHRS_M2 msg: 0.76 phi; 0.34 theta; -0.19 psi 2021-02-13T00:13:51Z- handleDepth()-Depth_Keller msg: 1613175231340 epoch sec; seqNo:10778; depth 23.66 2021-02-13T00:13:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:51Z- handleDvl() - RDI_Pathfinder msg: 1613175231.34 epoch sec; seqNo:5300 2021-02-13T00:13:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.07 , 0.22 2021-02-13T00:13:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175231.35 epoch sec; seqNo:10778; 4058670.33 north; 592186.89 east 2021-02-13T00:13:51Z- AHRS_M2 msg: 1613175231.74 epoch sec; seqNo:10687 2021-02-13T00:13:51Z- AHRS_M2 msg: 0.76 phi; 0.34 theta; -0.19 psi 2021-02-13T00:13:51Z- handleDepth()-Depth_Keller msg: 1613175231748 epoch sec; seqNo:10779; depth 23.54 2021-02-13T00:13:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175231.76 epoch sec; seqNo:10779; 4058670.33 north; 592186.89 east 2021-02-13T00:13:52Z- AHRS_M2 msg: 1613175232.15 epoch sec; seqNo:10688 2021-02-13T00:13:52Z- AHRS_M2 msg: 0.75 phi; 0.34 theta; -0.19 psi 2021-02-13T00:13:52Z- handleDepth()-Depth_Keller msg: 1613175232153 epoch sec; seqNo:10780; depth 23.40 2021-02-13T00:13:52Z- handleDvl() - RDI_Pathfinder msg: 1613175232.16 epoch sec; seqNo:5301 2021-02-13T00:13:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.35 , 97.79 , 99.95 , 65.81 2021-02-13T00:13:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , 0.21 2021-02-13T00:13:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175232.17 epoch sec; seqNo:10780; 4058670.72 north; 592188.10 east 2021-02-13T00:13:52Z- AHRS_M2 msg: 1613175232.55 epoch sec; seqNo:10689 2021-02-13T00:13:52Z- AHRS_M2 msg: 0.75 phi; 0.34 theta; -0.19 psi 2021-02-13T00:13:52Z- handleDepth()-Depth_Keller msg: 1613175232551 epoch sec; seqNo:10781; depth 23.30 2021-02-13T00:13:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:52Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:52Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:52Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:52Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:52Z- handleDvl() - RDI_Pathfinder msg: 1613175232.56 epoch sec; seqNo:5302 2021-02-13T00:13:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , 0.21 2021-02-13T00:13:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175232.58 epoch sec; seqNo:10781; 4058671.10 north; 592188.10 east 2021-02-13T00:13:52Z- AHRS_M2 msg: 1613175232.95 epoch sec; seqNo:10690 2021-02-13T00:13:52Z- AHRS_M2 msg: 0.75 phi; 0.34 theta; -0.18 psi 2021-02-13T00:13:52Z- handleDepth()-Depth_Keller msg: 1613175232956 epoch sec; seqNo:10782; depth 23.17 2021-02-13T00:13:52Z- handleDvl() - RDI_Pathfinder msg: 1613175232.97 epoch sec; seqNo:5303 2021-02-13T00:13:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.17 , 90.96 , 101.91 , 66.63 2021-02-13T00:13:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.12 , 0.14 2021-02-13T00:13:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175232.99 epoch sec; seqNo:10782; 4058671.10 north; 592188.10 east 2021-02-13T00:13:53Z- AHRS_M2 msg: 1613175233.36 epoch sec; seqNo:10691 2021-02-13T00:13:53Z- AHRS_M2 msg: 0.76 phi; 0.34 theta; -0.18 psi 2021-02-13T00:13:53Z- handleDepth()-Depth_Keller msg: 1613175233369 epoch sec; seqNo:10783; depth 23.05 2021-02-13T00:13:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175233.38 epoch sec; seqNo:10783; 4058671.48 north; 592188.09 east 2021-02-13T00:13:53Z- AHRS_M2 msg: 1613175233.78 epoch sec; seqNo:10692 2021-02-13T00:13:53Z- AHRS_M2 msg: 0.77 phi; 0.34 theta; -0.17 psi 2021-02-13T00:13:53Z- handleDepth()-Depth_Keller msg: 1613175233782 epoch sec; seqNo:10784; depth 22.96 2021-02-13T00:13:53Z- handleDvl() - RDI_Pathfinder msg: 1613175233.79 epoch sec; seqNo:5304 2021-02-13T00:13:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.17 , 98.33 , 101.98 , 66.22 2021-02-13T00:13:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.23 , 0.01 2021-02-13T00:13:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175233.80 epoch sec; seqNo:10784; 4058671.49 north; 592189.31 east 2021-02-13T00:13:54Z- AHRS_M2 msg: 1613175234.16 epoch sec; seqNo:10693 2021-02-13T00:13:54Z- AHRS_M2 msg: 0.77 phi; 0.34 theta; -0.16 psi 2021-02-13T00:13:54Z- handleDepth()-Depth_Keller msg: 1613175234178 epoch sec; seqNo:10785; depth 22.82 2021-02-13T00:13:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175234.19 epoch sec; seqNo:10785; 4058671.87 north; 592189.31 east 2021-02-13T00:13:54Z- AHRS_M2 msg: 1613175234.57 epoch sec; seqNo:10694 2021-02-13T00:13:54Z- AHRS_M2 msg: 0.78 phi; 0.34 theta; -0.16 psi 2021-02-13T00:13:54Z- handleDepth()-Depth_Keller msg: 1613175234574 epoch sec; seqNo:10786; depth 22.71 2021-02-13T00:13:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175234.58 epoch sec; seqNo:10786; 4058671.87 north; 592189.31 east 2021-02-13T00:13:54Z- AHRS_M2 msg: 1613175234.97 epoch sec; seqNo:10695 2021-02-13T00:13:54Z- AHRS_M2 msg: 0.78 phi; 0.34 theta; -0.16 psi 2021-02-13T00:13:54Z- handleDepth()-Depth_Keller msg: 1613175234981 epoch sec; seqNo:10787; depth 22.60 2021-02-13T00:13:54Z- handleDvl() - RDI_Pathfinder msg: 1613175234.99 epoch sec; seqNo:5305 2021-02-13T00:13:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.21 , 90.89 , 104.28 , 65.81 2021-02-13T00:13:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.12 , -0.05 2021-02-13T00:13:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175235.00 epoch sec; seqNo:10787; 4058672.25 north; 592189.31 east 2021-02-13T00:13:55Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 131 calls... 2021-02-13T00:13:55Z- waiting for 1613175233.15, time is 1613175235.00 2021-02-13T00:13:55Z- LcmTrn::updateTrn() equating pose 1613175235.00 and meas times 1613175235.00 2021-02-13T00:13:55Z- LcmTrn::updateTrn() >>>> pose time 1613175235.00, meas time 1613175235.00 <<<< 2021-02-13T00:13:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175235.00 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:13:55Z- TerrainNav::motionUpdate() - currEstimate = (4058672, 592189.31, 22.60) 2021-02-13T00:13:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:13:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:13:55Z- TNavBF::Projecting Measurements 2021-02-13T00:13:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:13:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:13:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:13:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:13:55Z- TNavFilter::4 good beams. 2021-02-13T00:13:55Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:13:55Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:13:55Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:13:55Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:13:55Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:13:55Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:13:55Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:13:55Z- TNavBF::mmse selected from filter -1 2021-02-13T00:13:55Z- TerrainMapOctree: (vehN, vehE ) = (4058672.25, 592189.31). 2021-02-13T00:13:55Z- TerrainMapOctree: Min Distance = 3300.19. 2021-02-13T00:13:55Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:13:55Z- There is no specific good beam that all particles have in common. 2021-02-13T00:13:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:13:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.32 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 9.05 2021-02-13T00:13:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:13:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 13.22 2021-02-13T00:13:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:13:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 17.40 2021-02-13T00:13:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:13:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 6.93 2021-02-13T00:13:55Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:13:55Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:13:55Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:13:55Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:13:55Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:13:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:13:55Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:13:55Z- Filter 0 No beams used in subcloud update 2021-02-13T00:13:55Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:13:55Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:13:55Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 713.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:13:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175235.00 sec. 2021-02-13T00:13:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:13:55Z- TNavBF::mmse selected from filter -1 2021-02-13T00:13:55Z- variances([0], [2], and psi_berg) = 713.016712 833.005280 0.000000 2021-02-13T00:13:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:13:55Z- LcmTrn::publishEstimates() - MLE : 4058671.75 592238.81 22.60 2021-02-13T00:13:55Z- LcmTrn::publishEstimates() - MMSE : 4058672.25 592189.31 22.60 2021-02-13T00:13:55Z- LcmTrn::publishEstimates() - COVAR: 713.02 833.01 0.92 0.00 2021-02-13T00:13:55Z- AHRS_M2 msg: 1613175235.38 epoch sec; seqNo:10696 2021-02-13T00:13:55Z- AHRS_M2 msg: 0.79 phi; 0.34 theta; -0.17 psi 2021-02-13T00:13:55Z- handleDepth()-Depth_Keller msg: 1613175235381 epoch sec; seqNo:10788; depth 22.46 2021-02-13T00:13:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175235.39 epoch sec; seqNo:10788; 4058672.25 north; 592189.31 east 2021-02-13T00:13:55Z- AHRS_M2 msg: 1613175235.78 epoch sec; seqNo:10697 2021-02-13T00:13:55Z- AHRS_M2 msg: 0.78 phi; 0.33 theta; -0.17 psi 2021-02-13T00:13:55Z- handleDepth()-Depth_Keller msg: 1613175235795 epoch sec; seqNo:10789; depth 22.36 2021-02-13T00:13:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175235.81 epoch sec; seqNo:10789; 4058672.64 north; 592190.52 east 2021-02-13T00:13:56Z- AHRS_M2 msg: 1613175236.18 epoch sec; seqNo:10698 2021-02-13T00:13:56Z- AHRS_M2 msg: 0.78 phi; 0.33 theta; -0.19 psi 2021-02-13T00:13:56Z- handleDepth()-Depth_Keller msg: 1613175236193 epoch sec; seqNo:10790; depth 22.22 2021-02-13T00:13:56Z- handleDvl() - RDI_Pathfinder msg: 1613175236.20 epoch sec; seqNo:5306 2021-02-13T00:13:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.79 , 98.26 , 101.91 , 66.63 2021-02-13T00:13:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.07 , -0.07 2021-02-13T00:13:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175236.23 epoch sec; seqNo:10790; 4058672.64 north; 592190.52 east 2021-02-13T00:13:56Z- AHRS_M2 msg: 1613175236.59 epoch sec; seqNo:10699 2021-02-13T00:13:56Z- AHRS_M2 msg: 0.77 phi; 0.33 theta; -0.20 psi 2021-02-13T00:13:56Z- handleDepth()-Depth_Keller msg: 1613175236593 epoch sec; seqNo:10791; depth 22.11 2021-02-13T00:13:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175236.61 epoch sec; seqNo:10791; 4058673.02 north; 592190.52 east 2021-02-13T00:13:56Z- AHRS_M2 msg: 1613175236.99 epoch sec; seqNo:10700 2021-02-13T00:13:56Z- AHRS_M2 msg: 0.76 phi; 0.33 theta; -0.21 psi 2021-02-13T00:13:56Z- handleDepth()-Depth_Keller msg: 1613175236998 epoch sec; seqNo:10792; depth 21.98 2021-02-13T00:13:57Z- handleDvl() - RDI_Pathfinder msg: 1613175237.01 epoch sec; seqNo:5307 2021-02-13T00:13:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 94.41 , 101.71 , 69.08 2021-02-13T00:13:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.00 , -0.02 2021-02-13T00:13:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175237.02 epoch sec; seqNo:10792; 4058673.40 north; 592190.51 east 2021-02-13T00:13:57Z- AHRS_M2 msg: 1613175237.40 epoch sec; seqNo:10701 2021-02-13T00:13:57Z- AHRS_M2 msg: 0.75 phi; 0.33 theta; -0.22 psi 2021-02-13T00:13:57Z- handleDepth()-Depth_Keller msg: 1613175237401 epoch sec; seqNo:10793; depth 21.86 2021-02-13T00:13:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:57Z- handleDvl() - RDI_Pathfinder msg: 1613175237.40 epoch sec; seqNo:5308 2021-02-13T00:13:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.00 , -0.02 2021-02-13T00:13:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175237.41 epoch sec; seqNo:10793; 4058673.40 north; 592190.51 east 2021-02-13T00:13:57Z- AHRS_M2 msg: 1613175237.80 epoch sec; seqNo:10702 2021-02-13T00:13:57Z- AHRS_M2 msg: 0.74 phi; 0.33 theta; -0.22 psi 2021-02-13T00:13:57Z- handleDepth()-Depth_Keller msg: 1613175237804 epoch sec; seqNo:10794; depth 21.74 2021-02-13T00:13:57Z- handleDvl() - RDI_Pathfinder msg: 1613175237.82 epoch sec; seqNo:5309 2021-02-13T00:13:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.12 , 105.43 , 69.91 2021-02-13T00:13:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , 0.02 2021-02-13T00:13:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175237.83 epoch sec; seqNo:10794; 4058673.79 north; 592191.73 east 2021-02-13T00:13:58Z- AHRS_M2 msg: 1613175238.20 epoch sec; seqNo:10703 2021-02-13T00:13:58Z- AHRS_M2 msg: 0.73 phi; 0.33 theta; -0.22 psi 2021-02-13T00:13:58Z- handleDepth()-Depth_Keller msg: 1613175238209 epoch sec; seqNo:10795; depth 21.63 2021-02-13T00:13:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175238.22 epoch sec; seqNo:10795; 4058673.79 north; 592191.73 east 2021-02-13T00:13:58Z- AHRS_M2 msg: 1613175238.61 epoch sec; seqNo:10704 2021-02-13T00:13:58Z- AHRS_M2 msg: 0.73 phi; 0.33 theta; -0.22 psi 2021-02-13T00:13:58Z- handleDepth()-Depth_Keller msg: 1613175238617 epoch sec; seqNo:10796; depth 21.50 2021-02-13T00:13:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:58Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:58Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:58Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:58Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:58Z- handleDvl() - RDI_Pathfinder msg: 1613175238.62 epoch sec; seqNo:5310 2021-02-13T00:13:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , 0.02 2021-02-13T00:13:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175238.64 epoch sec; seqNo:10796; 4058674.17 north; 592191.73 east 2021-02-13T00:13:59Z- AHRS_M2 msg: 1613175239.01 epoch sec; seqNo:10705 2021-02-13T00:13:59Z- AHRS_M2 msg: 0.72 phi; 0.33 theta; -0.22 psi 2021-02-13T00:13:59Z- handleDepth()-Depth_Keller msg: 1613175239016 epoch sec; seqNo:10797; depth 21.39 2021-02-13T00:13:59Z- handleDvl() - RDI_Pathfinder msg: 1613175239.03 epoch sec; seqNo:5311 2021-02-13T00:13:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.17 , 97.65 , 96.43 , 69.09 2021-02-13T00:13:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.06 , 0.02 2021-02-13T00:13:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175239.04 epoch sec; seqNo:10797; 4058674.55 north; 592191.72 east 2021-02-13T00:13:59Z- AHRS_M2 msg: 1613175239.42 epoch sec; seqNo:10706 2021-02-13T00:13:59Z- AHRS_M2 msg: 0.72 phi; 0.34 theta; -0.21 psi 2021-02-13T00:13:59Z- handleDepth()-Depth_Keller msg: 1613175239420 epoch sec; seqNo:10798; depth 21.27 2021-02-13T00:13:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175239.43 epoch sec; seqNo:10798; 4058674.55 north; 592191.72 east 2021-02-13T00:13:59Z- AHRS_M2 msg: 1613175239.82 epoch sec; seqNo:10707 2021-02-13T00:13:59Z- AHRS_M2 msg: 0.73 phi; 0.34 theta; -0.20 psi 2021-02-13T00:13:59Z- handleDepth()-Depth_Keller msg: 1613175239826 epoch sec; seqNo:10799; depth 21.16 2021-02-13T00:13:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:13:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:13:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:13:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:13:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:13:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:13:59Z- handleDvl() - RDI_Pathfinder msg: 1613175239.83 epoch sec; seqNo:5312 2021-02-13T00:13:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:13:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.06 , 0.02 2021-02-13T00:13:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175239.84 epoch sec; seqNo:10799; 4058674.94 north; 592192.94 east 2021-02-13T00:14:00Z- AHRS_M2 msg: 1613175240.22 epoch sec; seqNo:10708 2021-02-13T00:14:00Z- AHRS_M2 msg: 0.73 phi; 0.33 theta; -0.20 psi 2021-02-13T00:14:00Z- handleDepth()-Depth_Keller msg: 1613175240229 epoch sec; seqNo:10800; depth 21.05 2021-02-13T00:14:00Z- handleDvl() - RDI_Pathfinder msg: 1613175240.24 epoch sec; seqNo:5313 2021-02-13T00:14:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:00Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:14:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175240.26 epoch sec; seqNo:10800; 4058675.32 north; 592192.94 east 2021-02-13T00:14:00Z- LcmTrn::handleMessages() - handling 47 messages took 0.62 ms and 135 calls... 2021-02-13T00:14:00Z- waiting for 1613175238.00, time is 1613175240.26 2021-02-13T00:14:00Z- LcmTrn::updateTrn() equating pose 1613175240.26 and meas times 1613175240.26 2021-02-13T00:14:00Z- LcmTrn::updateTrn() >>>> pose time 1613175240.26, meas time 1613175240.26 <<<< 2021-02-13T00:14:00Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175240.26, ping # 0. 2021-02-13T00:14:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175240.26 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:14:00Z- TerrainNav::motionUpdate() - currEstimate = (4058675, 592192.94, 21.05) 2021-02-13T00:14:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:14:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:14:00Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:14:00Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:14:00Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175240.26 sec.; No relevant navigation data available 2021-02-13T00:14:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:14:00Z- TNavBF::mmse selected from filter -1 2021-02-13T00:14:00Z- variances([0], [2], and psi_berg) = 712.948723 833.005239 0.000000 2021-02-13T00:14:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:14:00Z- LcmTrn::publishEstimates() - MLE : 4058674.82 592242.44 21.05 2021-02-13T00:14:00Z- LcmTrn::publishEstimates() - MMSE : 4058675.32 592192.94 21.05 2021-02-13T00:14:00Z- LcmTrn::publishEstimates() - COVAR: 712.95 833.01 1.17 0.00 2021-02-13T00:14:00Z- AHRS_M2 msg: 1613175240.63 epoch sec; seqNo:10709 2021-02-13T00:14:00Z- AHRS_M2 msg: 0.74 phi; 0.33 theta; -0.19 psi 2021-02-13T00:14:00Z- handleDepth()-Depth_Keller msg: 1613175240633 epoch sec; seqNo:10801; depth 20.93 2021-02-13T00:14:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175240.64 epoch sec; seqNo:10801; 4058675.32 north; 592192.94 east 2021-02-13T00:14:01Z- AHRS_M2 msg: 1613175241.05 epoch sec; seqNo:10710 2021-02-13T00:14:01Z- AHRS_M2 msg: 0.75 phi; 0.33 theta; -0.19 psi 2021-02-13T00:14:01Z- handleDepth()-Depth_Keller msg: 1613175241061 epoch sec; seqNo:10802; depth 20.83 2021-02-13T00:14:01Z- handleDvl() - RDI_Pathfinder msg: 1613175241.07 epoch sec; seqNo:5314 2021-02-13T00:14:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:01Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:14:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175241.10 epoch sec; seqNo:10802; 4058675.70 north; 592192.93 east 2021-02-13T00:14:01Z- AHRS_M2 msg: 1613175241.44 epoch sec; seqNo:10711 2021-02-13T00:14:01Z- AHRS_M2 msg: 0.75 phi; 0.33 theta; -0.18 psi 2021-02-13T00:14:01Z- handleDepth()-Depth_Keller msg: 1613175241440 epoch sec; seqNo:10803; depth 20.70 2021-02-13T00:14:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175241.46 epoch sec; seqNo:10803; 4058675.70 north; 592192.93 east 2021-02-13T00:14:01Z- AHRS_M2 msg: 1613175241.85 epoch sec; seqNo:10712 2021-02-13T00:14:01Z- AHRS_M2 msg: 0.76 phi; 0.32 theta; -0.18 psi 2021-02-13T00:14:01Z- handleDepth()-Depth_Keller msg: 1613175241877 epoch sec; seqNo:10804; depth 20.61 2021-02-13T00:14:01Z- handleDvl() - RDI_Pathfinder msg: 1613175241.89 epoch sec; seqNo:5315 2021-02-13T00:14:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 97.51 , 104.28 , 68.68 2021-02-13T00:14:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.11 , -0.17 2021-02-13T00:14:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175241.90 epoch sec; seqNo:10804; 4058676.07 north; 592192.93 east 2021-02-13T00:14:02Z- AHRS_M2 msg: 1613175242.25 epoch sec; seqNo:10713 2021-02-13T00:14:02Z- AHRS_M2 msg: 0.76 phi; 0.32 theta; -0.18 psi 2021-02-13T00:14:02Z- handleDepth()-Depth_Keller msg: 1613175242255 epoch sec; seqNo:10805; depth 20.47 2021-02-13T00:14:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:02Z- handleDvl() - RDI_Pathfinder msg: 1613175242.26 epoch sec; seqNo:5316 2021-02-13T00:14:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.11 , -0.17 2021-02-13T00:14:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175242.29 epoch sec; seqNo:10805; 4058676.47 north; 592194.14 east 2021-02-13T00:14:02Z- AHRS_M2 msg: 1613175242.66 epoch sec; seqNo:10714 2021-02-13T00:14:02Z- AHRS_M2 msg: 0.75 phi; 0.31 theta; -0.19 psi 2021-02-13T00:14:02Z- handleDepth()-Depth_Keller msg: 1613175242662 epoch sec; seqNo:10806; depth 20.36 2021-02-13T00:14:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175242.67 epoch sec; seqNo:10806; 4058676.47 north; 592194.14 east 2021-02-13T00:14:03Z- AHRS_M2 msg: 1613175243.05 epoch sec; seqNo:10715 2021-02-13T00:14:03Z- AHRS_M2 msg: 0.75 phi; 0.31 theta; -0.19 psi 2021-02-13T00:14:03Z- handleDepth()-Depth_Keller msg: 1613175243056 epoch sec; seqNo:10807; depth 20.25 2021-02-13T00:14:03Z- handleDvl() - RDI_Pathfinder msg: 1613175243.08 epoch sec; seqNo:5317 2021-02-13T00:14:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.40 , 97.99 , 104.07 , 0.00 2021-02-13T00:14:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.23 , -0.05 2021-02-13T00:14:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175243.09 epoch sec; seqNo:10807; 4058676.85 north; 592194.14 east 2021-02-13T00:14:03Z- AHRS_M2 msg: 1613175243.46 epoch sec; seqNo:10716 2021-02-13T00:14:03Z- AHRS_M2 msg: 0.74 phi; 0.31 theta; -0.20 psi 2021-02-13T00:14:03Z- handleDepth()-Depth_Keller msg: 1613175243460 epoch sec; seqNo:10808; depth 20.12 2021-02-13T00:14:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:03Z- handleDvl() - RDI_Pathfinder msg: 1613175243.46 epoch sec; seqNo:5318 2021-02-13T00:14:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.23 , -0.05 2021-02-13T00:14:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175243.47 epoch sec; seqNo:10808; 4058677.22 north; 592194.14 east 2021-02-13T00:14:03Z- AHRS_M2 msg: 1613175243.88 epoch sec; seqNo:10717 2021-02-13T00:14:03Z- AHRS_M2 msg: 0.73 phi; 0.31 theta; -0.20 psi 2021-02-13T00:14:03Z- handleDepth()-Depth_Keller msg: 1613175243881 epoch sec; seqNo:10809; depth 20.03 2021-02-13T00:14:03Z- handleDvl() - RDI_Pathfinder msg: 1613175243.89 epoch sec; seqNo:5319 2021-02-13T00:14:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.78 , 97.72 , 103.13 , 69.50 2021-02-13T00:14:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.25 , 0.13 2021-02-13T00:14:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175243.91 epoch sec; seqNo:10809; 4058677.22 north; 592194.14 east 2021-02-13T00:14:04Z- AHRS_M2 msg: 1613175244.26 epoch sec; seqNo:10718 2021-02-13T00:14:04Z- AHRS_M2 msg: 0.72 phi; 0.31 theta; -0.20 psi 2021-02-13T00:14:04Z- handleDepth()-Depth_Keller msg: 1613175244270 epoch sec; seqNo:10810; depth 19.90 2021-02-13T00:14:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175244.28 epoch sec; seqNo:10810; 4058677.60 north; 592194.13 east 2021-02-13T00:14:04Z- AHRS_M2 msg: 1613175244.67 epoch sec; seqNo:10719 2021-02-13T00:14:04Z- AHRS_M2 msg: 0.71 phi; 0.32 theta; -0.20 psi 2021-02-13T00:14:04Z- handleDepth()-Depth_Keller msg: 1613175244677 epoch sec; seqNo:10811; depth 19.81 2021-02-13T00:14:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:04Z- handleDvl() - RDI_Pathfinder msg: 1613175244.68 epoch sec; seqNo:5320 2021-02-13T00:14:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.25 , 0.13 2021-02-13T00:14:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175244.70 epoch sec; seqNo:10811; 4058677.98 north; 592194.13 east 2021-02-13T00:14:05Z- AHRS_M2 msg: 1613175245.07 epoch sec; seqNo:10720 2021-02-13T00:14:05Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.19 psi 2021-02-13T00:14:05Z- handleDepth()-Depth_Keller msg: 1613175245081 epoch sec; seqNo:10812; depth 19.68 2021-02-13T00:14:05Z- handleDvl() - RDI_Pathfinder msg: 1613175245.09 epoch sec; seqNo:5321 2021-02-13T00:14:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.22 , 97.99 , 104.07 , 69.91 2021-02-13T00:14:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.13 , 0.25 2021-02-13T00:14:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175245.12 epoch sec; seqNo:10812; 4058678.36 north; 592194.12 east 2021-02-13T00:14:05Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-13T00:14:05Z- waiting for 1613175243.26, time is 1613175245.12 2021-02-13T00:14:05Z- LcmTrn::updateTrn() equating pose 1613175245.12 and meas times 1613175245.12 2021-02-13T00:14:05Z- LcmTrn::updateTrn() >>>> pose time 1613175245.12, meas time 1613175245.12 <<<< 2021-02-13T00:14:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175245.12 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:14:05Z- TerrainNav::motionUpdate() - currEstimate = (4058678, 592194.12, 19.68) 2021-02-13T00:14:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:14:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:14:05Z- TNavBF::Projecting Measurements 2021-02-13T00:14:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:14:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:14:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:14:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:14:05Z- TNavFilter::4 good beams. 2021-02-13T00:14:05Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:14:05Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:14:05Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:14:05Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:14:05Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:14:05Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:14:05Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:14:05Z- TNavBF::mmse selected from filter -1 2021-02-13T00:14:05Z- TerrainMapOctree: (vehN, vehE ) = (4058678.36, 592194.12). 2021-02-13T00:14:05Z- TerrainMapOctree: Min Distance = 3292.41. 2021-02-13T00:14:05Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:14:05Z- There is no specific good beam that all particles have in common. 2021-02-13T00:14:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:14:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.110 ExpectedMeasDiff: nan currMeas.covariance: 9.05 2021-02-13T00:14:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:14:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:14:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:14:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 17.33 2021-02-13T00:14:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:14:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 7.82 2021-02-13T00:14:05Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:14:05Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:14:05Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:14:05Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:14:05Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:14:05Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:14:05Z- Filter 0 No beams used in subcloud update 2021-02-13T00:14:05Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:14:05Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:14:05Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 712.9 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:14:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175245.12 sec. 2021-02-13T00:14:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:14:05Z- TNavBF::mmse selected from filter -1 2021-02-13T00:14:05Z- variances([0], [2], and psi_berg) = 712.892400 833.005205 0.000000 2021-02-13T00:14:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:14:05Z- LcmTrn::publishEstimates() - MLE : 4058677.86 592243.62 19.68 2021-02-13T00:14:05Z- LcmTrn::publishEstimates() - MMSE : 4058678.36 592194.12 19.68 2021-02-13T00:14:05Z- LcmTrn::publishEstimates() - COVAR: 712.89 833.01 0.94 0.00 2021-02-13T00:14:05Z- AHRS_M2 msg: 1613175245.48 epoch sec; seqNo:10721 2021-02-13T00:14:05Z- AHRS_M2 msg: 0.69 phi; 0.32 theta; -0.19 psi 2021-02-13T00:14:05Z- handleDepth()-Depth_Keller msg: 1613175245480 epoch sec; seqNo:10813; depth 19.56 2021-02-13T00:14:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175245.49 epoch sec; seqNo:10813; 4058678.37 north; 592195.34 east 2021-02-13T00:14:05Z- AHRS_M2 msg: 1613175245.90 epoch sec; seqNo:10722 2021-02-13T00:14:05Z- AHRS_M2 msg: 0.69 phi; 0.33 theta; -0.18 psi 2021-02-13T00:14:05Z- handleDepth()-Depth_Keller msg: 1613175245910 epoch sec; seqNo:10814; depth 19.46 2021-02-13T00:14:05Z- handleDvl() - RDI_Pathfinder msg: 1613175245.92 epoch sec; seqNo:5322 2021-02-13T00:14:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.85 , 98.06 , 105.36 , 78.58 2021-02-13T00:14:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.04 , 0.22 2021-02-13T00:14:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175245.93 epoch sec; seqNo:10814; 4058678.75 north; 592195.34 east 2021-02-13T00:14:06Z- AHRS_M2 msg: 1613175246.28 epoch sec; seqNo:10723 2021-02-13T00:14:06Z- AHRS_M2 msg: 0.69 phi; 0.33 theta; -0.18 psi 2021-02-13T00:14:06Z- handleDepth()-Depth_Keller msg: 1613175246289 epoch sec; seqNo:10815; depth 19.34 2021-02-13T00:14:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175246.31 epoch sec; seqNo:10815; 4058679.13 north; 592195.34 east 2021-02-13T00:14:06Z- AHRS_M2 msg: 1613175246.69 epoch sec; seqNo:10724 2021-02-13T00:14:06Z- AHRS_M2 msg: 0.70 phi; 0.33 theta; -0.17 psi 2021-02-13T00:14:06Z- handleDepth()-Depth_Keller msg: 1613175246692 epoch sec; seqNo:10816; depth 19.24 2021-02-13T00:14:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175246.70 epoch sec; seqNo:10816; 4058679.51 north; 592195.33 east 2021-02-13T00:14:07Z- AHRS_M2 msg: 1613175247.09 epoch sec; seqNo:10725 2021-02-13T00:14:07Z- AHRS_M2 msg: 0.71 phi; 0.33 theta; -0.17 psi 2021-02-13T00:14:07Z- handleDepth()-Depth_Keller msg: 1613175247101 epoch sec; seqNo:10817; depth 19.14 2021-02-13T00:14:07Z- handleDvl() - RDI_Pathfinder msg: 1613175247.11 epoch sec; seqNo:5323 2021-02-13T00:14:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.85 , 97.79 , 105.09 , 69.08 2021-02-13T00:14:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.12 , 0.10 2021-02-13T00:14:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175247.13 epoch sec; seqNo:10817; 4058679.51 north; 592195.33 east 2021-02-13T00:14:07Z- AHRS_M2 msg: 1613175247.50 epoch sec; seqNo:10726 2021-02-13T00:14:07Z- AHRS_M2 msg: 0.72 phi; 0.32 theta; -0.16 psi 2021-02-13T00:14:07Z- handleDepth()-Depth_Keller msg: 1613175247509 epoch sec; seqNo:10818; depth 19.01 2021-02-13T00:14:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175247.52 epoch sec; seqNo:10818; 4058679.90 north; 592196.55 east 2021-02-13T00:14:07Z- AHRS_M2 msg: 1613175247.90 epoch sec; seqNo:10727 2021-02-13T00:14:07Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.15 psi 2021-02-13T00:14:07Z- handleDepth()-Depth_Keller msg: 1613175247910 epoch sec; seqNo:10819; depth 18.91 2021-02-13T00:14:07Z- handleDvl() - RDI_Pathfinder msg: 1613175247.93 epoch sec; seqNo:5324 2021-02-13T00:14:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 93.12 , 105.29 , 69.90 2021-02-13T00:14:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.10 , -0.02 2021-02-13T00:14:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175247.94 epoch sec; seqNo:10819; 4058679.90 north; 592196.55 east 2021-02-13T00:14:08Z- AHRS_M2 msg: 1613175248.30 epoch sec; seqNo:10728 2021-02-13T00:14:08Z- AHRS_M2 msg: 0.74 phi; 0.32 theta; -0.15 psi 2021-02-13T00:14:08Z- handleDepth()-Depth_Keller msg: 1613175248313 epoch sec; seqNo:10820; depth 18.80 2021-02-13T00:14:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:08Z- handleDvl() - RDI_Pathfinder msg: 1613175248.32 epoch sec; seqNo:5325 2021-02-13T00:14:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.10 , -0.02 2021-02-13T00:14:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175248.33 epoch sec; seqNo:10820; 4058680.28 north; 592196.55 east 2021-02-13T00:14:08Z- AHRS_M2 msg: 1613175248.71 epoch sec; seqNo:10729 2021-02-13T00:14:08Z- AHRS_M2 msg: 0.74 phi; 0.32 theta; -0.15 psi 2021-02-13T00:14:08Z- handleDepth()-Depth_Keller msg: 1613175248713 epoch sec; seqNo:10821; depth 18.68 2021-02-13T00:14:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175248.72 epoch sec; seqNo:10821; 4058680.66 north; 592196.54 east 2021-02-13T00:14:09Z- AHRS_M2 msg: 1613175249.11 epoch sec; seqNo:10730 2021-02-13T00:14:09Z- AHRS_M2 msg: 0.75 phi; 0.32 theta; -0.15 psi 2021-02-13T00:14:09Z- handleDepth()-Depth_Keller msg: 1613175249121 epoch sec; seqNo:10822; depth 18.59 2021-02-13T00:14:09Z- handleDvl() - RDI_Pathfinder msg: 1613175249.14 epoch sec; seqNo:5326 2021-02-13T00:14:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.79 , 106.31 , 71.94 2021-02-13T00:14:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , -0.05 2021-02-13T00:14:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175249.16 epoch sec; seqNo:10822; 4058680.66 north; 592196.54 east 2021-02-13T00:14:09Z- AHRS_M2 msg: 1613175249.52 epoch sec; seqNo:10731 2021-02-13T00:14:09Z- AHRS_M2 msg: 0.75 phi; 0.32 theta; -0.14 psi 2021-02-13T00:14:09Z- handleDepth()-Depth_Keller msg: 1613175249520 epoch sec; seqNo:10823; depth 18.49 2021-02-13T00:14:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:09Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:09Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:09Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:09Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:09Z- handleDvl() - RDI_Pathfinder msg: 1613175249.52 epoch sec; seqNo:5327 2021-02-13T00:14:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , -0.05 2021-02-13T00:14:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175249.53 epoch sec; seqNo:10823; 4058681.05 north; 592197.76 east 2021-02-13T00:14:09Z- AHRS_M2 msg: 1613175249.92 epoch sec; seqNo:10732 2021-02-13T00:14:09Z- AHRS_M2 msg: 0.74 phi; 0.32 theta; -0.14 psi 2021-02-13T00:14:09Z- handleDepth()-Depth_Keller msg: 1613175249933 epoch sec; seqNo:10824; depth 18.37 2021-02-13T00:14:09Z- handleDvl() - RDI_Pathfinder msg: 1613175249.94 epoch sec; seqNo:5328 2021-02-13T00:14:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.89 , 92.31 , 105.84 , 70.31 2021-02-13T00:14:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , 0.00 2021-02-13T00:14:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175249.96 epoch sec; seqNo:10824; 4058681.43 north; 592197.75 east 2021-02-13T00:14:10Z- AHRS_M2 msg: 1613175250.32 epoch sec; seqNo:10733 2021-02-13T00:14:10Z- AHRS_M2 msg: 0.74 phi; 0.32 theta; -0.14 psi 2021-02-13T00:14:10Z- handleDepth()-Depth_Keller msg: 1613175250328 epoch sec; seqNo:10825; depth 18.25 2021-02-13T00:14:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175250.34 epoch sec; seqNo:10825; 4058681.43 north; 592197.75 east 2021-02-13T00:14:10Z- LcmTrn::handleMessages() - handling 46 messages took 0.70 ms and 132 calls... 2021-02-13T00:14:10Z- waiting for 1613175248.12, time is 1613175250.34 2021-02-13T00:14:10Z- LcmTrn::updateTrn() >>>> pose time 1613175250.34, meas time 1613175249.94 <<<< 2021-02-13T00:14:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175249.94 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:14:10Z- TerrainNav::motionUpdate() - currEstimate = (4058681, 592197.75, 18.25) 2021-02-13T00:14:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:14:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:14:10Z- TNavBF::Projecting Measurements 2021-02-13T00:14:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:14:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:14:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:14:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:14:10Z- TNavFilter::4 good beams. 2021-02-13T00:14:10Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:14:10Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:14:10Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:14:10Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:14:10Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:14:10Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:14:10Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:14:10Z- TNavBF::mmse selected from filter -1 2021-02-13T00:14:10Z- TerrainMapOctree: (vehN, vehE ) = (4058681.20, 592197.48). 2021-02-13T00:14:10Z- TerrainMapOctree: Min Distance = 3288.10. 2021-02-13T00:14:10Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:14:10Z- There is no specific good beam that all particles have in common. 2021-02-13T00:14:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:14:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 9.96 2021-02-13T00:14:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:14:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 13.63 2021-02-13T00:14:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:14:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 17.92 2021-02-13T00:14:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:14:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 7.91 2021-02-13T00:14:10Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:14:10Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:14:10Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:14:10Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:14:10Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:14:10Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:14:10Z- Filter 0 No beams used in subcloud update 2021-02-13T00:14:10Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:14:10Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:14:10Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 712.8 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:14:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175249.94 sec. 2021-02-13T00:14:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:14:10Z- TNavBF::mmse selected from filter -1 2021-02-13T00:14:10Z- variances([0], [2], and psi_berg) = 712.808476 833.005154 0.000000 2021-02-13T00:14:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:14:10Z- LcmTrn::publishEstimates() - MLE : 4058680.93 592247.25 18.25 2021-02-13T00:14:10Z- LcmTrn::publishEstimates() - MMSE : 4058681.43 592197.75 18.25 2021-02-13T00:14:10Z- LcmTrn::publishEstimates() - COVAR: 712.81 833.01 0.88 0.00 2021-02-13T00:14:10Z- AHRS_M2 msg: 1613175250.73 epoch sec; seqNo:10734 2021-02-13T00:14:10Z- AHRS_M2 msg: 0.74 phi; 0.32 theta; -0.14 psi 2021-02-13T00:14:10Z- handleDepth()-Depth_Keller msg: 1613175250732 epoch sec; seqNo:10826; depth 18.14 2021-02-13T00:14:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:10Z- handleDvl() - RDI_Pathfinder msg: 1613175250.73 epoch sec; seqNo:5329 2021-02-13T00:14:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , 0.00 2021-02-13T00:14:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175250.74 epoch sec; seqNo:10826; 4058681.81 north; 592197.75 east 2021-02-13T00:14:11Z- AHRS_M2 msg: 1613175251.14 epoch sec; seqNo:10735 2021-02-13T00:14:11Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.15 psi 2021-02-13T00:14:11Z- handleDepth()-Depth_Keller msg: 1613175251149 epoch sec; seqNo:10827; depth 18.04 2021-02-13T00:14:11Z- handleDvl() - RDI_Pathfinder msg: 1613175251.16 epoch sec; seqNo:5330 2021-02-13T00:14:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.80 , 93.19 , 107.80 , 116.32 2021-02-13T00:14:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.61 , -0.06 , -0.00 2021-02-13T00:14:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175251.17 epoch sec; seqNo:10827; 4058681.81 north; 592197.75 east 2021-02-13T00:14:11Z- AHRS_M2 msg: 1613175251.54 epoch sec; seqNo:10736 2021-02-13T00:14:11Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.15 psi 2021-02-13T00:14:11Z- handleDepth()-Depth_Keller msg: 1613175251540 epoch sec; seqNo:10828; depth 17.92 2021-02-13T00:14:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175251.56 epoch sec; seqNo:10828; 4058682.19 north; 592197.75 east 2021-02-13T00:14:11Z- AHRS_M2 msg: 1613175251.94 epoch sec; seqNo:10737 2021-02-13T00:14:11Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.16 psi 2021-02-13T00:14:11Z- handleDepth()-Depth_Keller msg: 1613175251944 epoch sec; seqNo:10829; depth 17.81 2021-02-13T00:14:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:11Z- handleDvl() - RDI_Pathfinder msg: 1613175251.95 epoch sec; seqNo:5331 2021-02-13T00:14:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.61 , -0.06 , -0.00 2021-02-13T00:14:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175251.95 epoch sec; seqNo:10829; 4058682.19 north; 592197.75 east 2021-02-13T00:14:12Z- AHRS_M2 msg: 1613175252.34 epoch sec; seqNo:10738 2021-02-13T00:14:12Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.16 psi 2021-02-13T00:14:12Z- handleDepth()-Depth_Keller msg: 1613175252353 epoch sec; seqNo:10830; depth 17.70 2021-02-13T00:14:12Z- handleDvl() - RDI_Pathfinder msg: 1613175252.36 epoch sec; seqNo:5332 2021-02-13T00:14:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.88 , 97.11 , 106.58 , 78.17 2021-02-13T00:14:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , 0.06 2021-02-13T00:14:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175252.38 epoch sec; seqNo:10830; 4058682.58 north; 592198.96 east 2021-02-13T00:14:12Z- AHRS_M2 msg: 1613175252.75 epoch sec; seqNo:10739 2021-02-13T00:14:12Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.17 psi 2021-02-13T00:14:12Z- handleDepth()-Depth_Keller msg: 1613175252757 epoch sec; seqNo:10831; depth 17.60 2021-02-13T00:14:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175252.77 epoch sec; seqNo:10831; 4058682.96 north; 592198.96 east 2021-02-13T00:14:13Z- AHRS_M2 msg: 1613175253.15 epoch sec; seqNo:10740 2021-02-13T00:14:13Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.17 psi 2021-02-13T00:14:13Z- handleDepth()-Depth_Keller msg: 1613175253158 epoch sec; seqNo:10832; depth 17.47 2021-02-13T00:14:13Z- handleDvl() - RDI_Pathfinder msg: 1613175253.17 epoch sec; seqNo:5333 2021-02-13T00:14:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.67 , 97.72 , 105.02 , 78.24 2021-02-13T00:14:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , 0.01 2021-02-13T00:14:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175253.19 epoch sec; seqNo:10832; 4058682.96 north; 592198.96 east 2021-02-13T00:14:13Z- AHRS_M2 msg: 1613175253.56 epoch sec; seqNo:10741 2021-02-13T00:14:13Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.17 psi 2021-02-13T00:14:13Z- handleDepth()-Depth_Keller msg: 1613175253564 epoch sec; seqNo:10833; depth 17.37 2021-02-13T00:14:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175253.58 epoch sec; seqNo:10833; 4058683.34 north; 592198.95 east 2021-02-13T00:14:13Z- AHRS_M2 msg: 1613175253.96 epoch sec; seqNo:10742 2021-02-13T00:14:13Z- AHRS_M2 msg: 0.74 phi; 0.32 theta; -0.17 psi 2021-02-13T00:14:13Z- handleDepth()-Depth_Keller msg: 1613175253967 epoch sec; seqNo:10834; depth 17.27 2021-02-13T00:14:13Z- handleDvl() - RDI_Pathfinder msg: 1613175253.98 epoch sec; seqNo:5334 2021-02-13T00:14:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.49 , 97.72 , 106.24 , 72.77 2021-02-13T00:14:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.04 2021-02-13T00:14:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175253.99 epoch sec; seqNo:10834; 4058683.72 north; 592198.95 east 2021-02-13T00:14:14Z- AHRS_M2 msg: 1613175254.36 epoch sec; seqNo:10743 2021-02-13T00:14:14Z- AHRS_M2 msg: 0.74 phi; 0.32 theta; -0.16 psi 2021-02-13T00:14:14Z- handleDepth()-Depth_Keller msg: 1613175254373 epoch sec; seqNo:10835; depth 17.16 2021-02-13T00:14:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:14Z- handleDvl() - RDI_Pathfinder msg: 1613175254.38 epoch sec; seqNo:5335 2021-02-13T00:14:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.04 2021-02-13T00:14:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175254.40 epoch sec; seqNo:10835; 4058683.73 north; 592200.17 east 2021-02-13T00:14:14Z- AHRS_M2 msg: 1613175254.77 epoch sec; seqNo:10744 2021-02-13T00:14:14Z- AHRS_M2 msg: 0.75 phi; 0.32 theta; -0.15 psi 2021-02-13T00:14:14Z- handleDepth()-Depth_Keller msg: 1613175254772 epoch sec; seqNo:10836; depth 17.06 2021-02-13T00:14:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175254.79 epoch sec; seqNo:10836; 4058684.11 north; 592200.17 east 2021-02-13T00:14:15Z- AHRS_M2 msg: 1613175255.17 epoch sec; seqNo:10745 2021-02-13T00:14:15Z- AHRS_M2 msg: 0.76 phi; 0.32 theta; -0.15 psi 2021-02-13T00:14:15Z- handleDepth()-Depth_Keller msg: 1613175255178 epoch sec; seqNo:10837; depth 16.96 2021-02-13T00:14:15Z- handleDvl() - RDI_Pathfinder msg: 1613175255.19 epoch sec; seqNo:5336 2021-02-13T00:14:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.72 , 95.42 , 107.59 , 71.54 2021-02-13T00:14:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , -0.11 2021-02-13T00:14:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175255.20 epoch sec; seqNo:10837; 4058684.49 north; 592200.16 east 2021-02-13T00:14:15Z- AHRS_M2 msg: 1613175255.58 epoch sec; seqNo:10746 2021-02-13T00:14:15Z- AHRS_M2 msg: 0.77 phi; 0.32 theta; -0.14 psi 2021-02-13T00:14:15Z- handleDepth()-Depth_Keller msg: 1613175255582 epoch sec; seqNo:10838; depth 16.83 2021-02-13T00:14:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:15Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:15Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:15Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:15Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:15Z- handleDvl() - RDI_Pathfinder msg: 1613175255.59 epoch sec; seqNo:5337 2021-02-13T00:14:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , -0.11 2021-02-13T00:14:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175255.60 epoch sec; seqNo:10838; 4058684.49 north; 592200.16 east 2021-02-13T00:14:15Z- LcmTrn::handleMessages() - handling 48 messages took 0.71 ms and 134 calls... 2021-02-13T00:14:15Z- waiting for 1613175253.34, time is 1613175255.60 2021-02-13T00:14:15Z- LcmTrn::updateTrn() equating pose 1613175255.60 and meas times 1613175255.60 2021-02-13T00:14:15Z- LcmTrn::updateTrn() >>>> pose time 1613175255.60, meas time 1613175255.60 <<<< 2021-02-13T00:14:15Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175255.60, ping # 0. 2021-02-13T00:14:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175255.60 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:14:15Z- TerrainNav::motionUpdate() - currEstimate = (4058684, 592200.16, 16.83) 2021-02-13T00:14:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:14:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:14:15Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:14:15Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:14:15Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175255.60 sec.; No relevant navigation data available 2021-02-13T00:14:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:14:15Z- TNavBF::mmse selected from filter -1 2021-02-13T00:14:15Z- variances([0], [2], and psi_berg) = 712.747100 833.005117 0.000000 2021-02-13T00:14:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:14:15Z- LcmTrn::publishEstimates() - MLE : 4058683.99 592249.66 16.83 2021-02-13T00:14:15Z- LcmTrn::publishEstimates() - MMSE : 4058684.49 592200.16 16.83 2021-02-13T00:14:15Z- LcmTrn::publishEstimates() - COVAR: 712.75 833.01 1.08 0.00 2021-02-13T00:14:15Z- AHRS_M2 msg: 1613175255.98 epoch sec; seqNo:10747 2021-02-13T00:14:15Z- AHRS_M2 msg: 0.78 phi; 0.32 theta; -0.13 psi 2021-02-13T00:14:15Z- handleDepth()-Depth_Keller msg: 1613175255984 epoch sec; seqNo:10839; depth 16.74 2021-02-13T00:14:16Z- handleDvl() - RDI_Pathfinder msg: 1613175256.00 epoch sec; seqNo:5338 2021-02-13T00:14:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 95.49 , 106.44 , 71.94 2021-02-13T00:14:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.10 , -0.12 2021-02-13T00:14:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175256.01 epoch sec; seqNo:10839; 4058684.87 north; 592200.16 east 2021-02-13T00:14:16Z- AHRS_M2 msg: 1613175256.38 epoch sec; seqNo:10748 2021-02-13T00:14:16Z- AHRS_M2 msg: 0.78 phi; 0.31 theta; -0.12 psi 2021-02-13T00:14:16Z- handleDepth()-Depth_Keller msg: 1613175256388 epoch sec; seqNo:10840; depth 16.63 2021-02-13T00:14:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175256.40 epoch sec; seqNo:10840; 4058684.87 north; 592200.16 east 2021-02-13T00:14:16Z- AHRS_M2 msg: 1613175256.79 epoch sec; seqNo:10749 2021-02-13T00:14:16Z- AHRS_M2 msg: 0.79 phi; 0.31 theta; -0.12 psi 2021-02-13T00:14:16Z- handleDepth()-Depth_Keller msg: 1613175256797 epoch sec; seqNo:10841; depth 16.52 2021-02-13T00:14:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:16Z- handleDvl() - RDI_Pathfinder msg: 1613175256.80 epoch sec; seqNo:5339 2021-02-13T00:14:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.10 , -0.12 2021-02-13T00:14:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175256.81 epoch sec; seqNo:10841; 4058685.26 north; 592201.38 east 2021-02-13T00:14:17Z- AHRS_M2 msg: 1613175257.19 epoch sec; seqNo:10750 2021-02-13T00:14:17Z- AHRS_M2 msg: 0.79 phi; 0.31 theta; -0.12 psi 2021-02-13T00:14:17Z- handleDepth()-Depth_Keller msg: 1613175257199 epoch sec; seqNo:10842; depth 16.41 2021-02-13T00:14:17Z- handleDvl() - RDI_Pathfinder msg: 1613175257.21 epoch sec; seqNo:5340 2021-02-13T00:14:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.83 , 97.65 , 106.17 , 72.76 2021-02-13T00:14:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.18 , -0.07 2021-02-13T00:14:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175257.22 epoch sec; seqNo:10842; 4058685.64 north; 592201.37 east 2021-02-13T00:14:17Z- AHRS_M2 msg: 1613175257.60 epoch sec; seqNo:10751 2021-02-13T00:14:17Z- AHRS_M2 msg: 0.78 phi; 0.31 theta; -0.12 psi 2021-02-13T00:14:17Z- handleDepth()-Depth_Keller msg: 1613175257605 epoch sec; seqNo:10843; depth 16.30 2021-02-13T00:14:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175257.63 epoch sec; seqNo:10843; 4058686.02 north; 592201.37 east 2021-02-13T00:14:18Z- AHRS_M2 msg: 1613175258.00 epoch sec; seqNo:10752 2021-02-13T00:14:18Z- AHRS_M2 msg: 0.77 phi; 0.31 theta; -0.14 psi 2021-02-13T00:14:18Z- handleDepth()-Depth_Keller msg: 1613175258004 epoch sec; seqNo:10844; depth 16.18 2021-02-13T00:14:18Z- handleDvl() - RDI_Pathfinder msg: 1613175258.02 epoch sec; seqNo:5341 2021-02-13T00:14:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 97.79 , 106.31 , 71.53 2021-02-13T00:14:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.18 , 0.03 2021-02-13T00:14:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175258.05 epoch sec; seqNo:10844; 4058686.02 north; 592201.37 east 2021-02-13T00:14:18Z- AHRS_M2 msg: 1613175258.40 epoch sec; seqNo:10753 2021-02-13T00:14:18Z- AHRS_M2 msg: 0.76 phi; 0.31 theta; -0.15 psi 2021-02-13T00:14:18Z- handleDepth()-Depth_Keller msg: 1613175258412 epoch sec; seqNo:10845; depth 16.08 2021-02-13T00:14:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175258.43 epoch sec; seqNo:10845; 4058686.40 north; 592201.36 east 2021-02-13T00:14:18Z- AHRS_M2 msg: 1613175258.81 epoch sec; seqNo:10754 2021-02-13T00:14:18Z- AHRS_M2 msg: 0.75 phi; 0.31 theta; -0.17 psi 2021-02-13T00:14:18Z- handleDepth()-Depth_Keller msg: 1613175258814 epoch sec; seqNo:10846; depth 15.96 2021-02-13T00:14:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175258.83 epoch sec; seqNo:10846; 4058686.77 north; 592201.36 east 2021-02-13T00:14:19Z- AHRS_M2 msg: 1613175259.21 epoch sec; seqNo:10755 2021-02-13T00:14:19Z- AHRS_M2 msg: 0.74 phi; 0.32 theta; -0.18 psi 2021-02-13T00:14:19Z- handleDepth()-Depth_Keller msg: 1613175259221 epoch sec; seqNo:10847; depth 15.86 2021-02-13T00:14:19Z- handleDvl() - RDI_Pathfinder msg: 1613175259.24 epoch sec; seqNo:5342 2021-02-13T00:14:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.03 , 97.86 , 106.38 , 78.38 2021-02-13T00:14:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.14 , 0.13 2021-02-13T00:14:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175259.25 epoch sec; seqNo:10847; 4058687.15 north; 592201.35 east 2021-02-13T00:14:19Z- AHRS_M2 msg: 1613175259.62 epoch sec; seqNo:10756 2021-02-13T00:14:19Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.18 psi 2021-02-13T00:14:19Z- handleDepth()-Depth_Keller msg: 1613175259626 epoch sec; seqNo:10848; depth 15.74 2021-02-13T00:14:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175259.64 epoch sec; seqNo:10848; 4058687.17 north; 592202.58 east 2021-02-13T00:14:20Z- AHRS_M2 msg: 1613175260.02 epoch sec; seqNo:10757 2021-02-13T00:14:20Z- AHRS_M2 msg: 0.73 phi; 0.33 theta; -0.18 psi 2021-02-13T00:14:20Z- handleDepth()-Depth_Keller msg: 1613175260029 epoch sec; seqNo:10849; depth 15.63 2021-02-13T00:14:20Z- handleDvl() - RDI_Pathfinder msg: 1613175260.04 epoch sec; seqNo:5343 2021-02-13T00:14:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 97.65 , 109.83 , 0.00 2021-02-13T00:14:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , 0.16 2021-02-13T00:14:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175260.05 epoch sec; seqNo:10849; 4058687.55 north; 592202.57 east 2021-02-13T00:14:20Z- AHRS_M2 msg: 1613175260.43 epoch sec; seqNo:10758 2021-02-13T00:14:20Z- AHRS_M2 msg: 0.73 phi; 0.33 theta; -0.18 psi 2021-02-13T00:14:20Z- handleDepth()-Depth_Keller msg: 1613175260438 epoch sec; seqNo:10850; depth 15.52 2021-02-13T00:14:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:20Z- handleDvl() - RDI_Pathfinder msg: 1613175260.44 epoch sec; seqNo:5344 2021-02-13T00:14:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , 0.16 2021-02-13T00:14:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175260.45 epoch sec; seqNo:10850; 4058687.92 north; 592202.57 east 2021-02-13T00:14:20Z- LcmTrn::handleMessages() - handling 43 messages took 0.69 ms and 130 calls... 2021-02-13T00:14:20Z- waiting for 1613175258.60, time is 1613175260.45 2021-02-13T00:14:20Z- LcmTrn::updateTrn() equating pose 1613175260.45 and meas times 1613175260.45 2021-02-13T00:14:20Z- LcmTrn::updateTrn() >>>> pose time 1613175260.45, meas time 1613175260.45 <<<< 2021-02-13T00:14:20Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175260.45, ping # 0. 2021-02-13T00:14:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175260.45 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:14:20Z- TerrainNav::motionUpdate() - currEstimate = (4058688, 592202.57, 15.52) 2021-02-13T00:14:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:14:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:14:20Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:14:20Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:14:20Z- TNavBF::Projecting Measurements 2021-02-13T00:14:20Z- TNavFilter::Measurement from time = 1613175260.45 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:14:20Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:14:20Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:14:20Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:14:20Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:14:20Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:14:20Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:14:20Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:14:20Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 712.7 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:14:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:14:20Z- TNavBF::mmse selected from filter -1 2021-02-13T00:14:20Z- variances([0], [2], and psi_berg) = 712.683434 833.005079 0.000000 2021-02-13T00:14:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:14:20Z- LcmTrn::publishEstimates() - MLE : 4058687.42 592252.07 15.52 2021-02-13T00:14:20Z- LcmTrn::publishEstimates() - MMSE : 4058687.92 592202.57 15.52 2021-02-13T00:14:20Z- LcmTrn::publishEstimates() - COVAR: 712.68 833.01 1.29 0.00 2021-02-13T00:14:20Z- AHRS_M2 msg: 1613175260.83 epoch sec; seqNo:10759 2021-02-13T00:14:20Z- AHRS_M2 msg: 0.73 phi; 0.33 theta; -0.18 psi 2021-02-13T00:14:20Z- handleDepth()-Depth_Keller msg: 1613175260832 epoch sec; seqNo:10851; depth 15.42 2021-02-13T00:14:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175260.86 epoch sec; seqNo:10851; 4058687.92 north; 592202.57 east 2021-02-13T00:14:21Z- AHRS_M2 msg: 1613175261.23 epoch sec; seqNo:10760 2021-02-13T00:14:21Z- AHRS_M2 msg: 0.74 phi; 0.33 theta; -0.18 psi 2021-02-13T00:14:21Z- handleDepth()-Depth_Keller msg: 1613175261236 epoch sec; seqNo:10852; depth 15.31 2021-02-13T00:14:21Z- handleDvl() - RDI_Pathfinder msg: 1613175261.25 epoch sec; seqNo:5345 2021-02-13T00:14:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.50 , 96.03 , 109.56 , 74.80 2021-02-13T00:14:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.06 , 0.10 2021-02-13T00:14:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175261.26 epoch sec; seqNo:10852; 4058688.30 north; 592202.56 east 2021-02-13T00:14:21Z- AHRS_M2 msg: 1613175261.64 epoch sec; seqNo:10761 2021-02-13T00:14:21Z- AHRS_M2 msg: 0.74 phi; 0.33 theta; -0.17 psi 2021-02-13T00:14:21Z- handleDepth()-Depth_Keller msg: 1613175261642 epoch sec; seqNo:10853; depth 15.19 2021-02-13T00:14:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:21Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:21Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:21Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:21Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:21Z- handleDvl() - RDI_Pathfinder msg: 1613175261.64 epoch sec; seqNo:5346 2021-02-13T00:14:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.06 , 0.10 2021-02-13T00:14:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175261.65 epoch sec; seqNo:10853; 4058688.69 north; 592203.78 east 2021-02-13T00:14:22Z- AHRS_M2 msg: 1613175262.04 epoch sec; seqNo:10762 2021-02-13T00:14:22Z- AHRS_M2 msg: 0.75 phi; 0.32 theta; -0.17 psi 2021-02-13T00:14:22Z- handleDepth()-Depth_Keller msg: 1613175262045 epoch sec; seqNo:10854; depth 15.09 2021-02-13T00:14:22Z- handleDvl() - RDI_Pathfinder msg: 1613175262.06 epoch sec; seqNo:5347 2021-02-13T00:14:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 97.72 , 112.33 , 78.24 2021-02-13T00:14:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.11 , -0.01 2021-02-13T00:14:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175262.07 epoch sec; seqNo:10854; 4058688.69 north; 592203.78 east 2021-02-13T00:14:22Z- AHRS_M2 msg: 1613175262.44 epoch sec; seqNo:10763 2021-02-13T00:14:22Z- AHRS_M2 msg: 0.76 phi; 0.32 theta; -0.17 psi 2021-02-13T00:14:22Z- handleDepth()-Depth_Keller msg: 1613175262448 epoch sec; seqNo:10855; depth 14.96 2021-02-13T00:14:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175262.46 epoch sec; seqNo:10855; 4058689.07 north; 592203.78 east 2021-02-13T00:14:22Z- AHRS_M2 msg: 1613175262.86 epoch sec; seqNo:10764 2021-02-13T00:14:22Z- AHRS_M2 msg: 0.77 phi; 0.31 theta; -0.16 psi 2021-02-13T00:14:22Z- handleDepth()-Depth_Keller msg: 1613175262875 epoch sec; seqNo:10856; depth 14.86 2021-02-13T00:14:22Z- handleDvl() - RDI_Pathfinder msg: 1613175262.90 epoch sec; seqNo:5348 2021-02-13T00:14:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.30 , 109.83 , 78.72 2021-02-13T00:14:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.06 , -0.10 2021-02-13T00:14:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175262.91 epoch sec; seqNo:10856; 4058689.07 north; 592203.78 east 2021-02-13T00:14:23Z- AHRS_M2 msg: 1613175263.25 epoch sec; seqNo:10765 2021-02-13T00:14:23Z- AHRS_M2 msg: 0.77 phi; 0.31 theta; -0.16 psi 2021-02-13T00:14:23Z- handleDepth()-Depth_Keller msg: 1613175263265 epoch sec; seqNo:10857; depth 14.74 2021-02-13T00:14:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175263.28 epoch sec; seqNo:10857; 4058689.45 north; 592203.77 east 2021-02-13T00:14:23Z- AHRS_M2 msg: 1613175263.66 epoch sec; seqNo:10766 2021-02-13T00:14:23Z- AHRS_M2 msg: 0.78 phi; 0.31 theta; -0.16 psi 2021-02-13T00:14:23Z- handleDepth()-Depth_Keller msg: 1613175263660 epoch sec; seqNo:10858; depth 14.63 2021-02-13T00:14:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175263.68 epoch sec; seqNo:10858; 4058689.47 north; 592204.99 east 2021-02-13T00:14:24Z- AHRS_M2 msg: 1613175264.06 epoch sec; seqNo:10767 2021-02-13T00:14:24Z- AHRS_M2 msg: 0.78 phi; 0.31 theta; -0.16 psi 2021-02-13T00:14:24Z- handleDepth()-Depth_Keller msg: 1613175264069 epoch sec; seqNo:10859; depth 14.52 2021-02-13T00:14:24Z- handleDvl() - RDI_Pathfinder msg: 1613175264.08 epoch sec; seqNo:5349 2021-02-13T00:14:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.80 , 109.62 , 104.21 2021-02-13T00:14:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.11 , -0.72 , -0.48 2021-02-13T00:14:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175264.10 epoch sec; seqNo:10859; 4058689.83 north; 592203.77 east 2021-02-13T00:14:24Z- AHRS_M2 msg: 1613175264.47 epoch sec; seqNo:10768 2021-02-13T00:14:24Z- AHRS_M2 msg: 0.78 phi; 0.31 theta; -0.16 psi 2021-02-13T00:14:24Z- handleDepth()-Depth_Keller msg: 1613175264477 epoch sec; seqNo:10860; depth 14.40 2021-02-13T00:14:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175264.49 epoch sec; seqNo:10860; 4058690.21 north; 592203.76 east 2021-02-13T00:14:24Z- AHRS_M2 msg: 1613175264.87 epoch sec; seqNo:10769 2021-02-13T00:14:24Z- AHRS_M2 msg: 0.78 phi; 0.31 theta; -0.16 psi 2021-02-13T00:14:24Z- handleDepth()-Depth_Keller msg: 1613175264877 epoch sec; seqNo:10861; depth 14.28 2021-02-13T00:14:24Z- handleDvl() - RDI_Pathfinder msg: 1613175264.89 epoch sec; seqNo:5350 2021-02-13T00:14:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:24Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:14:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175264.90 epoch sec; seqNo:10861; 4058690.21 north; 592203.76 east 2021-02-13T00:14:25Z- AHRS_M2 msg: 1613175265.27 epoch sec; seqNo:10770 2021-02-13T00:14:25Z- AHRS_M2 msg: 0.77 phi; 0.31 theta; -0.16 psi 2021-02-13T00:14:25Z- handleDepth()-Depth_Keller msg: 1613175265276 epoch sec; seqNo:10862; depth 14.18 2021-02-13T00:14:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:25Z- handleDvl() - RDI_Pathfinder msg: 1613175265.28 epoch sec; seqNo:5351 2021-02-13T00:14:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:25Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:14:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175265.30 epoch sec; seqNo:10862; 4058690.59 north; 592203.76 east 2021-02-13T00:14:25Z- AHRS_M2 msg: 1613175265.68 epoch sec; seqNo:10771 2021-02-13T00:14:25Z- AHRS_M2 msg: 0.76 phi; 0.31 theta; -0.16 psi 2021-02-13T00:14:25Z- handleDepth()-Depth_Keller msg: 1613175265680 epoch sec; seqNo:10863; depth 14.07 2021-02-13T00:14:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175265.69 epoch sec; seqNo:10863; 4058690.59 north; 592203.76 east 2021-02-13T00:14:25Z- LcmTrn::handleMessages() - handling 46 messages took 0.66 ms and 133 calls... 2021-02-13T00:14:25Z- waiting for 1613175263.45, time is 1613175265.69 2021-02-13T00:14:25Z- LcmTrn::updateTrn() >>>> pose time 1613175265.69, meas time 1613175265.28 <<<< 2021-02-13T00:14:25Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175265.28, ping # 0. 2021-02-13T00:14:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175265.28 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:14:25Z- TerrainNav::motionUpdate() - currEstimate = (4058691, 592203.76, 14.07) 2021-02-13T00:14:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:14:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:14:25Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:14:25Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:14:25Z- TNavBF::Projecting Measurements 2021-02-13T00:14:25Z- TNavFilter::Measurement from time = 1613175265.28 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:14:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:14:25Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:14:25Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:14:25Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:14:25Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:14:25Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:14:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:14:25Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 712.6 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T00:14:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:14:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:14:25Z- variances([0], [2], and psi_berg) = 712.617615 833.005039 0.000000 2021-02-13T00:14:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:14:25Z- LcmTrn::publishEstimates() - MLE : 4058690.09 592253.26 14.07 2021-02-13T00:14:25Z- LcmTrn::publishEstimates() - MMSE : 4058690.59 592203.76 14.07 2021-02-13T00:14:25Z- LcmTrn::publishEstimates() - COVAR: 712.62 833.01 1.44 0.00 2021-02-13T00:14:26Z- AHRS_M2 msg: 1613175266.08 epoch sec; seqNo:10772 2021-02-13T00:14:26Z- AHRS_M2 msg: 0.76 phi; 0.31 theta; -0.16 psi 2021-02-13T00:14:26Z- handleDepth()-Depth_Keller msg: 1613175266084 epoch sec; seqNo:10864; depth 13.97 2021-02-13T00:14:26Z- handleDvl() - RDI_Pathfinder msg: 1613175266.10 epoch sec; seqNo:5352 2021-02-13T00:14:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.91 , 99.28 , 108.20 , 0.00 2021-02-13T00:14:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.17 , 0.11 2021-02-13T00:14:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175266.11 epoch sec; seqNo:10864; 4058690.97 north; 592203.76 east 2021-02-13T00:14:26Z- AHRS_M2 msg: 1613175266.48 epoch sec; seqNo:10773 2021-02-13T00:14:26Z- AHRS_M2 msg: 0.75 phi; 0.31 theta; -0.16 psi 2021-02-13T00:14:26Z- handleDepth()-Depth_Keller msg: 1613175266499 epoch sec; seqNo:10865; depth 13.84 2021-02-13T00:14:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:26Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:26Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:26Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:26Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:26Z- handleDvl() - RDI_Pathfinder msg: 1613175266.50 epoch sec; seqNo:5353 2021-02-13T00:14:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.17 , 0.11 2021-02-13T00:14:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175266.51 epoch sec; seqNo:10865; 4058691.35 north; 592203.75 east 2021-02-13T00:14:26Z- AHRS_M2 msg: 1613175266.89 epoch sec; seqNo:10774 2021-02-13T00:14:26Z- AHRS_M2 msg: 0.74 phi; 0.31 theta; -0.16 psi 2021-02-13T00:14:26Z- handleDepth()-Depth_Keller msg: 1613175266918 epoch sec; seqNo:10866; depth 13.73 2021-02-13T00:14:26Z- handleDvl() - RDI_Pathfinder msg: 1613175266.94 epoch sec; seqNo:5354 2021-02-13T00:14:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.67 , 108.14 , 78.38 2021-02-13T00:14:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , 0.21 2021-02-13T00:14:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175266.95 epoch sec; seqNo:10866; 4058691.73 north; 592203.75 east 2021-02-13T00:14:27Z- AHRS_M2 msg: 1613175267.29 epoch sec; seqNo:10775 2021-02-13T00:14:27Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.16 psi 2021-02-13T00:14:27Z- handleDepth()-Depth_Keller msg: 1613175267301 epoch sec; seqNo:10867; depth 13.62 2021-02-13T00:14:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175267.31 epoch sec; seqNo:10867; 4058691.73 north; 592203.75 east 2021-02-13T00:14:27Z- AHRS_M2 msg: 1613175267.70 epoch sec; seqNo:10776 2021-02-13T00:14:27Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.16 psi 2021-02-13T00:14:27Z- handleDepth()-Depth_Keller msg: 1613175267702 epoch sec; seqNo:10868; depth 13.52 2021-02-13T00:14:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:27Z- handleDvl() - RDI_Pathfinder msg: 1613175267.70 epoch sec; seqNo:5355 2021-02-13T00:14:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , 0.21 2021-02-13T00:14:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175267.71 epoch sec; seqNo:10868; 4058692.12 north; 592204.96 east 2021-02-13T00:14:28Z- AHRS_M2 msg: 1613175268.10 epoch sec; seqNo:10777 2021-02-13T00:14:28Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.16 psi 2021-02-13T00:14:28Z- handleDepth()-Depth_Keller msg: 1613175268104 epoch sec; seqNo:10869; depth 13.42 2021-02-13T00:14:28Z- handleDvl() - RDI_Pathfinder msg: 1613175268.12 epoch sec; seqNo:5356 2021-02-13T00:14:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.93 , 98.80 , 112.33 , 78.51 2021-02-13T00:14:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.08 , 0.17 2021-02-13T00:14:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175268.14 epoch sec; seqNo:10869; 4058692.50 north; 592204.96 east 2021-02-13T00:14:28Z- AHRS_M2 msg: 1613175268.50 epoch sec; seqNo:10778 2021-02-13T00:14:28Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.15 psi 2021-02-13T00:14:28Z- handleDepth()-Depth_Keller msg: 1613175268508 epoch sec; seqNo:10870; depth 13.30 2021-02-13T00:14:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175268.52 epoch sec; seqNo:10870; 4058692.50 north; 592204.96 east 2021-02-13T00:14:28Z- AHRS_M2 msg: 1613175268.91 epoch sec; seqNo:10779 2021-02-13T00:14:28Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.15 psi 2021-02-13T00:14:28Z- handleDepth()-Depth_Keller msg: 1613175268920 epoch sec; seqNo:10871; depth 13.21 2021-02-13T00:14:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:28Z- handleDvl() - RDI_Pathfinder msg: 1613175268.93 epoch sec; seqNo:5357 2021-02-13T00:14:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.08 , 0.17 2021-02-13T00:14:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175268.96 epoch sec; seqNo:10871; 4058692.88 north; 592204.96 east 2021-02-13T00:14:29Z- AHRS_M2 msg: 1613175269.31 epoch sec; seqNo:10780 2021-02-13T00:14:29Z- AHRS_M2 msg: 0.74 phi; 0.32 theta; -0.15 psi 2021-02-13T00:14:29Z- handleDepth()-Depth_Keller msg: 1613175269325 epoch sec; seqNo:10872; depth 13.09 2021-02-13T00:14:29Z- handleDvl() - RDI_Pathfinder msg: 1613175269.34 epoch sec; seqNo:5358 2021-02-13T00:14:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 98.94 , 111.11 , 78.65 2021-02-13T00:14:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.18 , 0.03 2021-02-13T00:14:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175269.36 epoch sec; seqNo:10872; 4058693.25 north; 592204.95 east 2021-02-13T00:14:29Z- AHRS_M2 msg: 1613175269.72 epoch sec; seqNo:10781 2021-02-13T00:14:29Z- AHRS_M2 msg: 0.75 phi; 0.32 theta; -0.15 psi 2021-02-13T00:14:29Z- handleDepth()-Depth_Keller msg: 1613175269725 epoch sec; seqNo:10873; depth 12.97 2021-02-13T00:14:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175269.74 epoch sec; seqNo:10873; 4058693.27 north; 592206.17 east 2021-02-13T00:14:30Z- AHRS_M2 msg: 1613175270.12 epoch sec; seqNo:10782 2021-02-13T00:14:30Z- AHRS_M2 msg: 0.75 phi; 0.32 theta; -0.15 psi 2021-02-13T00:14:30Z- handleDepth()-Depth_Keller msg: 1613175270127 epoch sec; seqNo:10874; depth 12.85 2021-02-13T00:14:30Z- handleDvl() - RDI_Pathfinder msg: 1613175270.16 epoch sec; seqNo:5359 2021-02-13T00:14:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.48 , 99.00 , 113.88 , 78.72 2021-02-13T00:14:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.13 , -0.13 2021-02-13T00:14:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175270.17 epoch sec; seqNo:10874; 4058693.65 north; 592206.17 east 2021-02-13T00:14:30Z- AHRS_M2 msg: 1613175270.52 epoch sec; seqNo:10783 2021-02-13T00:14:30Z- AHRS_M2 msg: 0.76 phi; 0.32 theta; -0.15 psi 2021-02-13T00:14:30Z- handleDepth()-Depth_Keller msg: 1613175270531 epoch sec; seqNo:10875; depth 12.73 2021-02-13T00:14:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175270.54 epoch sec; seqNo:10875; 4058693.65 north; 592206.17 east 2021-02-13T00:14:30Z- LcmTrn::handleMessages() - handling 44 messages took 0.73 ms and 130 calls... 2021-02-13T00:14:30Z- waiting for 1613175268.69, time is 1613175270.54 2021-02-13T00:14:30Z- LcmTrn::updateTrn() >>>> pose time 1613175270.54, meas time 1613175270.16 <<<< 2021-02-13T00:14:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175270.16 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:14:30Z- TerrainNav::motionUpdate() - currEstimate = (4058694, 592206.17, 12.73) 2021-02-13T00:14:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:14:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:14:30Z- TNavBF::Projecting Measurements 2021-02-13T00:14:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:14:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:14:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:14:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:14:30Z- TNavFilter::4 good beams. 2021-02-13T00:14:30Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:14:30Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:14:30Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:14:30Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:14:30Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:14:30Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:14:30Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:14:30Z- TNavBF::mmse selected from filter -1 2021-02-13T00:14:30Z- TerrainMapOctree: (vehN, vehE ) = (4058693.40, 592205.98). 2021-02-13T00:14:30Z- TerrainMapOctree: Min Distance = 3273.26. 2021-02-13T00:14:30Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:14:30Z- There is no specific good beam that all particles have in common. 2021-02-13T00:14:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:14:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.62 K: 0.108 ExpectedMeasDiff: nan currMeas.covariance: 11.69 2021-02-13T00:14:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:14:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.45 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 15.68 2021-02-13T00:14:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:14:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 20.75 2021-02-13T00:14:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:14:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 9.91 2021-02-13T00:14:30Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:14:30Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:14:30Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:14:30Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:14:30Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:14:30Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:14:30Z- Filter 0 No beams used in subcloud update 2021-02-13T00:14:30Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:14:30Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:14:30Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 712.6 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:14:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175270.16 sec. 2021-02-13T00:14:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:14:30Z- TNavBF::mmse selected from filter -1 2021-02-13T00:14:30Z- variances([0], [2], and psi_berg) = 712.541597 833.004994 0.000000 2021-02-13T00:14:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:14:30Z- LcmTrn::publishEstimates() - MLE : 4058693.15 592255.67 12.73 2021-02-13T00:14:30Z- LcmTrn::publishEstimates() - MMSE : 4058693.65 592206.17 12.73 2021-02-13T00:14:30Z- LcmTrn::publishEstimates() - COVAR: 712.54 833.00 1.15 0.00 2021-02-13T00:14:30Z- AHRS_M2 msg: 1613175270.93 epoch sec; seqNo:10784 2021-02-13T00:14:30Z- AHRS_M2 msg: 0.77 phi; 0.32 theta; -0.15 psi 2021-02-13T00:14:30Z- handleDepth()-Depth_Keller msg: 1613175270932 epoch sec; seqNo:10876; depth 12.64 2021-02-13T00:14:30Z- handleDvl() - RDI_Pathfinder msg: 1613175270.94 epoch sec; seqNo:5360 2021-02-13T00:14:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.70 , 100.22 , 113.75 , 74.80 2021-02-13T00:14:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.17 2021-02-13T00:14:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175270.97 epoch sec; seqNo:10876; 4058694.02 north; 592206.16 east 2021-02-13T00:14:31Z- AHRS_M2 msg: 1613175271.33 epoch sec; seqNo:10785 2021-02-13T00:14:31Z- AHRS_M2 msg: 0.77 phi; 0.32 theta; -0.16 psi 2021-02-13T00:14:31Z- handleDepth()-Depth_Keller msg: 1613175271341 epoch sec; seqNo:10877; depth 12.51 2021-02-13T00:14:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:31Z- handleDvl() - RDI_Pathfinder msg: 1613175271.34 epoch sec; seqNo:5361 2021-02-13T00:14:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.17 2021-02-13T00:14:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175271.35 epoch sec; seqNo:10877; 4058694.40 north; 592206.16 east 2021-02-13T00:14:31Z- AHRS_M2 msg: 1613175271.73 epoch sec; seqNo:10786 2021-02-13T00:14:31Z- AHRS_M2 msg: 0.77 phi; 0.32 theta; -0.16 psi 2021-02-13T00:14:31Z- handleDepth()-Depth_Keller msg: 1613175271743 epoch sec; seqNo:10878; depth 12.40 2021-02-13T00:14:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175271.75 epoch sec; seqNo:10878; 4058694.42 north; 592207.38 east 2021-02-13T00:14:32Z- AHRS_M2 msg: 1613175272.14 epoch sec; seqNo:10787 2021-02-13T00:14:32Z- AHRS_M2 msg: 0.77 phi; 0.32 theta; -0.17 psi 2021-02-13T00:14:32Z- handleDepth()-Depth_Keller msg: 1613175272149 epoch sec; seqNo:10879; depth 12.28 2021-02-13T00:14:32Z- handleDvl() - RDI_Pathfinder msg: 1613175272.17 epoch sec; seqNo:5362 2021-02-13T00:14:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 101.64 , 115.17 , 77.26 2021-02-13T00:14:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.15 , -0.12 2021-02-13T00:14:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175272.18 epoch sec; seqNo:10879; 4058694.80 north; 592207.38 east 2021-02-13T00:14:32Z- AHRS_M2 msg: 1613175272.54 epoch sec; seqNo:10788 2021-02-13T00:14:32Z- AHRS_M2 msg: 0.76 phi; 0.32 theta; -0.17 psi 2021-02-13T00:14:32Z- handleDepth()-Depth_Keller msg: 1613175272553 epoch sec; seqNo:10880; depth 12.15 2021-02-13T00:14:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:32Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:32Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:32Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:32Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:32Z- handleDvl() - RDI_Pathfinder msg: 1613175272.56 epoch sec; seqNo:5363 2021-02-13T00:14:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.15 , -0.12 2021-02-13T00:14:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175272.57 epoch sec; seqNo:10880; 4058695.17 north; 592207.37 east 2021-02-13T00:14:32Z- AHRS_M2 msg: 1613175272.95 epoch sec; seqNo:10789 2021-02-13T00:14:32Z- AHRS_M2 msg: 0.75 phi; 0.32 theta; -0.18 psi 2021-02-13T00:14:32Z- handleDepth()-Depth_Keller msg: 1613175272952 epoch sec; seqNo:10881; depth 12.04 2021-02-13T00:14:32Z- handleDvl() - RDI_Pathfinder msg: 1613175272.96 epoch sec; seqNo:5364 2021-02-13T00:14:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.93 , 115.10 , 78.58 2021-02-13T00:14:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.14 , -0.01 2021-02-13T00:14:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175272.97 epoch sec; seqNo:10881; 4058695.17 north; 592207.37 east 2021-02-13T00:14:33Z- AHRS_M2 msg: 1613175273.35 epoch sec; seqNo:10790 2021-02-13T00:14:33Z- AHRS_M2 msg: 0.74 phi; 0.31 theta; -0.17 psi 2021-02-13T00:14:33Z- handleDepth()-Depth_Keller msg: 1613175273358 epoch sec; seqNo:10882; depth 11.92 2021-02-13T00:14:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175273.37 epoch sec; seqNo:10882; 4058695.55 north; 592207.37 east 2021-02-13T00:14:33Z- AHRS_M2 msg: 1613175273.76 epoch sec; seqNo:10791 2021-02-13T00:14:33Z- AHRS_M2 msg: 0.74 phi; 0.31 theta; -0.17 psi 2021-02-13T00:14:33Z- handleDepth()-Depth_Keller msg: 1613175273765 epoch sec; seqNo:10883; depth 11.80 2021-02-13T00:14:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:33Z- handleDvl() - RDI_Pathfinder msg: 1613175273.77 epoch sec; seqNo:5365 2021-02-13T00:14:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.14 , -0.01 2021-02-13T00:14:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175273.78 epoch sec; seqNo:10883; 4058695.93 north; 592207.36 east 2021-02-13T00:14:34Z- AHRS_M2 msg: 1613175274.16 epoch sec; seqNo:10792 2021-02-13T00:14:34Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.17 psi 2021-02-13T00:14:34Z- handleDepth()-Depth_Keller msg: 1613175274169 epoch sec; seqNo:10884; depth 11.69 2021-02-13T00:14:34Z- handleDvl() - RDI_Pathfinder msg: 1613175274.18 epoch sec; seqNo:5366 2021-02-13T00:14:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.94 , 101.17 , 113.34 , 78.18 2021-02-13T00:14:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.07 2021-02-13T00:14:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175274.20 epoch sec; seqNo:10884; 4058695.93 north; 592207.36 east 2021-02-13T00:14:34Z- AHRS_M2 msg: 1613175274.56 epoch sec; seqNo:10793 2021-02-13T00:14:34Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.16 psi 2021-02-13T00:14:34Z- handleDepth()-Depth_Keller msg: 1613175274573 epoch sec; seqNo:10885; depth 11.58 2021-02-13T00:14:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175274.58 epoch sec; seqNo:10885; 4058696.32 north; 592208.58 east 2021-02-13T00:14:34Z- AHRS_M2 msg: 1613175274.97 epoch sec; seqNo:10794 2021-02-13T00:14:34Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.16 psi 2021-02-13T00:14:34Z- handleDepth()-Depth_Keller msg: 1613175274980 epoch sec; seqNo:10886; depth 11.47 2021-02-13T00:14:35Z- handleDvl() - RDI_Pathfinder msg: 1613175275.00 epoch sec; seqNo:5367 2021-02-13T00:14:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.70 , 101.57 , 113.75 , 77.67 2021-02-13T00:14:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.09 , 0.08 2021-02-13T00:14:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175275.01 epoch sec; seqNo:10886; 4058696.70 north; 592208.58 east 2021-02-13T00:14:35Z- AHRS_M2 msg: 1613175275.37 epoch sec; seqNo:10795 2021-02-13T00:14:35Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.16 psi 2021-02-13T00:14:35Z- handleDepth()-Depth_Keller msg: 1613175275376 epoch sec; seqNo:10887; depth 11.35 2021-02-13T00:14:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175275.39 epoch sec; seqNo:10887; 4058696.70 north; 592208.58 east 2021-02-13T00:14:35Z- AHRS_M2 msg: 1613175275.78 epoch sec; seqNo:10796 2021-02-13T00:14:35Z- AHRS_M2 msg: 0.74 phi; 0.32 theta; -0.17 psi 2021-02-13T00:14:35Z- handleDepth()-Depth_Keller msg: 1613175275780 epoch sec; seqNo:10888; depth 11.22 2021-02-13T00:14:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175275.79 epoch sec; seqNo:10888; 4058697.08 north; 592208.57 east 2021-02-13T00:14:35Z- LcmTrn::handleMessages() - handling 47 messages took 0.67 ms and 134 calls... 2021-02-13T00:14:35Z- waiting for 1613175273.54, time is 1613175275.79 2021-02-13T00:14:35Z- LcmTrn::updateTrn() >>>> pose time 1613175275.79, meas time 1613175275.00 <<<< 2021-02-13T00:14:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175275.00 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:14:35Z- TerrainNav::motionUpdate() - currEstimate = (4058697, 592208.57, 11.22) 2021-02-13T00:14:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:14:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:14:35Z- TNavBF::Projecting Measurements 2021-02-13T00:14:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:14:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:14:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:14:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:14:35Z- TNavFilter::4 good beams. 2021-02-13T00:14:35Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:14:35Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:14:35Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:14:35Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:14:35Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:14:35Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:14:35Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:14:35Z- TNavBF::mmse selected from filter -1 2021-02-13T00:14:35Z- TerrainMapOctree: (vehN, vehE ) = (4058696.56, 592208.21). 2021-02-13T00:14:35Z- TerrainMapOctree: Min Distance = 3269.40. 2021-02-13T00:14:35Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:14:35Z- There is no specific good beam that all particles have in common. 2021-02-13T00:14:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:14:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 12.03 2021-02-13T00:14:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:14:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 16.51 2021-02-13T00:14:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:14:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 20.70 2021-02-13T00:14:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:14:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 9.65 2021-02-13T00:14:35Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:14:35Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:14:35Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:14:35Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:14:35Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:14:35Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:14:35Z- Filter 0 No beams used in subcloud update 2021-02-13T00:14:35Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:14:35Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:14:35Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 712.5 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:14:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175275.00 sec. 2021-02-13T00:14:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:14:35Z- TNavBF::mmse selected from filter -1 2021-02-13T00:14:35Z- variances([0], [2], and psi_berg) = 712.458401 833.004944 0.000000 2021-02-13T00:14:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:14:35Z- LcmTrn::publishEstimates() - MLE : 4058696.58 592258.07 11.22 2021-02-13T00:14:35Z- LcmTrn::publishEstimates() - MMSE : 4058697.08 592208.57 11.22 2021-02-13T00:14:35Z- LcmTrn::publishEstimates() - COVAR: 712.46 833.00 1.02 0.00 2021-02-13T00:14:36Z- AHRS_M2 msg: 1613175276.18 epoch sec; seqNo:10797 2021-02-13T00:14:36Z- AHRS_M2 msg: 0.75 phi; 0.32 theta; -0.17 psi 2021-02-13T00:14:36Z- handleDepth()-Depth_Keller msg: 1613175276185 epoch sec; seqNo:10889; depth 11.12 2021-02-13T00:14:36Z- handleDvl() - RDI_Pathfinder msg: 1613175276.20 epoch sec; seqNo:5368 2021-02-13T00:14:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 101.51 , 116.39 , 0.00 2021-02-13T00:14:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.10 , 0.04 2021-02-13T00:14:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175276.21 epoch sec; seqNo:10889; 4058697.08 north; 592208.57 east 2021-02-13T00:14:36Z- AHRS_M2 msg: 1613175276.59 epoch sec; seqNo:10798 2021-02-13T00:14:36Z- AHRS_M2 msg: 0.76 phi; 0.32 theta; -0.17 psi 2021-02-13T00:14:36Z- handleDepth()-Depth_Keller msg: 1613175276598 epoch sec; seqNo:10890; depth 11.00 2021-02-13T00:14:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175276.61 epoch sec; seqNo:10890; 4058697.47 north; 592209.79 east 2021-02-13T00:14:36Z- AHRS_M2 msg: 1613175276.99 epoch sec; seqNo:10799 2021-02-13T00:14:36Z- AHRS_M2 msg: 0.76 phi; 0.31 theta; -0.16 psi 2021-02-13T00:14:36Z- handleDepth()-Depth_Keller msg: 1613175276998 epoch sec; seqNo:10891; depth 10.89 2021-02-13T00:14:37Z- handleDvl() - RDI_Pathfinder msg: 1613175277.02 epoch sec; seqNo:5369 2021-02-13T00:14:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.58 , 102.45 , 114.36 , 78.65 2021-02-13T00:14:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.04 , 0.01 2021-02-13T00:14:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175277.03 epoch sec; seqNo:10891; 4058697.47 north; 592209.79 east 2021-02-13T00:14:37Z- AHRS_M2 msg: 1613175277.39 epoch sec; seqNo:10800 2021-02-13T00:14:37Z- AHRS_M2 msg: 0.76 phi; 0.31 theta; -0.17 psi 2021-02-13T00:14:37Z- handleDepth()-Depth_Keller msg: 1613175277401 epoch sec; seqNo:10892; depth 10.77 2021-02-13T00:14:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:37Z- handleDvl() - RDI_Pathfinder msg: 1613175277.40 epoch sec; seqNo:5370 2021-02-13T00:14:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.04 , 0.01 2021-02-13T00:14:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175277.41 epoch sec; seqNo:10892; 4058697.85 north; 592209.79 east 2021-02-13T00:14:37Z- AHRS_M2 msg: 1613175277.80 epoch sec; seqNo:10801 2021-02-13T00:14:37Z- AHRS_M2 msg: 0.75 phi; 0.31 theta; -0.17 psi 2021-02-13T00:14:37Z- handleDepth()-Depth_Keller msg: 1613175277806 epoch sec; seqNo:10893; depth 10.65 2021-02-13T00:14:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175277.82 epoch sec; seqNo:10893; 4058698.23 north; 592209.78 east 2021-02-13T00:14:38Z- AHRS_M2 msg: 1613175278.20 epoch sec; seqNo:10802 2021-02-13T00:14:38Z- AHRS_M2 msg: 0.74 phi; 0.31 theta; -0.18 psi 2021-02-13T00:14:38Z- handleDepth()-Depth_Keller msg: 1613175278206 epoch sec; seqNo:10894; depth 10.54 2021-02-13T00:14:38Z- handleDvl() - RDI_Pathfinder msg: 1613175278.22 epoch sec; seqNo:5371 2021-02-13T00:14:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 104.14 , 110.91 , 79.80 2021-02-13T00:14:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.13 , 0.06 2021-02-13T00:14:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175278.23 epoch sec; seqNo:10894; 4058698.23 north; 592209.78 east 2021-02-13T00:14:38Z- AHRS_M2 msg: 1613175278.60 epoch sec; seqNo:10803 2021-02-13T00:14:38Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.18 psi 2021-02-13T00:14:38Z- handleDepth()-Depth_Keller msg: 1613175278613 epoch sec; seqNo:10895; depth 10.44 2021-02-13T00:14:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:38Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:38Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:38Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:38Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:38Z- handleDvl() - RDI_Pathfinder msg: 1613175278.62 epoch sec; seqNo:5372 2021-02-13T00:14:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.13 , 0.06 2021-02-13T00:14:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175278.63 epoch sec; seqNo:10895; 4058698.61 north; 592209.78 east 2021-02-13T00:14:39Z- AHRS_M2 msg: 1613175279.01 epoch sec; seqNo:10804 2021-02-13T00:14:39Z- AHRS_M2 msg: 0.72 phi; 0.32 theta; -0.18 psi 2021-02-13T00:14:39Z- handleDepth()-Depth_Keller msg: 1613175279017 epoch sec; seqNo:10896; depth 10.29 2021-02-13T00:14:39Z- handleDvl() - RDI_Pathfinder msg: 1613175279.03 epoch sec; seqNo:5373 2021-02-13T00:14:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.28 , 105.57 , 115.03 , 79.87 2021-02-13T00:14:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.11 , 0.14 2021-02-13T00:14:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175279.05 epoch sec; seqNo:10896; 4058699.00 north; 592210.99 east 2021-02-13T00:14:39Z- AHRS_M2 msg: 1613175279.41 epoch sec; seqNo:10805 2021-02-13T00:14:39Z- AHRS_M2 msg: 0.72 phi; 0.32 theta; -0.18 psi 2021-02-13T00:14:39Z- handleDepth()-Depth_Keller msg: 1613175279416 epoch sec; seqNo:10897; depth 10.17 2021-02-13T00:14:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175279.43 epoch sec; seqNo:10897; 4058699.38 north; 592210.99 east 2021-02-13T00:14:39Z- AHRS_M2 msg: 1613175279.82 epoch sec; seqNo:10806 2021-02-13T00:14:39Z- AHRS_M2 msg: 0.72 phi; 0.32 theta; -0.18 psi 2021-02-13T00:14:39Z- handleDepth()-Depth_Keller msg: 1613175279829 epoch sec; seqNo:10898; depth 10.09 2021-02-13T00:14:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:39Z- handleDvl() - RDI_Pathfinder msg: 1613175279.83 epoch sec; seqNo:5374 2021-02-13T00:14:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.11 , 0.14 2021-02-13T00:14:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175279.84 epoch sec; seqNo:10898; 4058699.76 north; 592210.99 east 2021-02-13T00:14:40Z- AHRS_M2 msg: 1613175280.22 epoch sec; seqNo:10807 2021-02-13T00:14:40Z- AHRS_M2 msg: 0.72 phi; 0.33 theta; -0.19 psi 2021-02-13T00:14:40Z- handleDepth()-Depth_Keller msg: 1613175280234 epoch sec; seqNo:10899; depth 9.97 2021-02-13T00:14:40Z- handleDvl() - RDI_Pathfinder msg: 1613175280.25 epoch sec; seqNo:5375 2021-02-13T00:14:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 104.14 , 112.26 , 79.80 2021-02-13T00:14:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.02 , 0.16 2021-02-13T00:14:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175280.26 epoch sec; seqNo:10899; 4058699.76 north; 592210.99 east 2021-02-13T00:14:40Z- AHRS_M2 msg: 1613175280.63 epoch sec; seqNo:10808 2021-02-13T00:14:40Z- AHRS_M2 msg: 0.72 phi; 0.33 theta; -0.19 psi 2021-02-13T00:14:40Z- handleDepth()-Depth_Keller msg: 1613175280635 epoch sec; seqNo:10900; depth 9.85 2021-02-13T00:14:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175280.65 epoch sec; seqNo:10900; 4058700.14 north; 592210.98 east 2021-02-13T00:14:41Z- LcmTrn::handleMessages() - handling 44 messages took 0.62 ms and 132 calls... 2021-02-13T00:14:41Z- waiting for 1613175278.79, time is 1613175280.65 2021-02-13T00:14:41Z- LcmTrn::updateTrn() >>>> pose time 1613175280.65, meas time 1613175280.25 <<<< 2021-02-13T00:14:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175280.25 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:14:41Z- TerrainNav::motionUpdate() - currEstimate = (4058700, 592210.98, 9.85) 2021-02-13T00:14:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:14:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:14:41Z- TNavBF::Projecting Measurements 2021-02-13T00:14:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:14:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:14:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:14:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:14:41Z- TNavFilter::4 good beams. 2021-02-13T00:14:41Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:14:41Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:14:41Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:14:41Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:14:41Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:14:41Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:14:41Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:14:41Z- TNavBF::mmse selected from filter -1 2021-02-13T00:14:41Z- TerrainMapOctree: (vehN, vehE ) = (4058699.89, 592210.78). 2021-02-13T00:14:41Z- TerrainMapOctree: Min Distance = 3265.19. 2021-02-13T00:14:41Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:14:41Z- There is no specific good beam that all particles have in common. 2021-02-13T00:14:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:14:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 11.99 2021-02-13T00:14:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:14:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 17.35 2021-02-13T00:14:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:14:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 20.16 2021-02-13T00:14:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:14:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 10.19 2021-02-13T00:14:41Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:14:41Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:14:41Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:14:41Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:14:41Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:14:41Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:14:41Z- Filter 0 No beams used in subcloud update 2021-02-13T00:14:41Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:14:41Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:14:41Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 712.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:14:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175280.25 sec. 2021-02-13T00:14:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:14:41Z- TNavBF::mmse selected from filter -1 2021-02-13T00:14:41Z- variances([0], [2], and psi_berg) = 712.382303 833.004898 0.000000 2021-02-13T00:14:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:14:41Z- LcmTrn::publishEstimates() - MLE : 4058699.64 592260.48 9.85 2021-02-13T00:14:41Z- LcmTrn::publishEstimates() - MMSE : 4058700.14 592210.98 9.85 2021-02-13T00:14:41Z- LcmTrn::publishEstimates() - COVAR: 712.38 833.00 0.94 0.00 2021-02-13T00:14:41Z- AHRS_M2 msg: 1613175281.03 epoch sec; seqNo:10809 2021-02-13T00:14:41Z- AHRS_M2 msg: 0.73 phi; 0.33 theta; -0.19 psi 2021-02-13T00:14:41Z- handleDepth()-Depth_Keller msg: 1613175281033 epoch sec; seqNo:10901; depth 9.74 2021-02-13T00:14:41Z- handleDvl() - RDI_Pathfinder msg: 1613175281.07 epoch sec; seqNo:5376 2021-02-13T00:14:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 105.50 , 117.67 , 79.30 2021-02-13T00:14:41Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.06 , 0.11 2021-02-13T00:14:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175281.10 epoch sec; seqNo:10901; 4058700.53 north; 592212.20 east 2021-02-13T00:14:41Z- AHRS_M2 msg: 1613175281.43 epoch sec; seqNo:10810 2021-02-13T00:14:41Z- AHRS_M2 msg: 0.74 phi; 0.33 theta; -0.18 psi 2021-02-13T00:14:41Z- handleDepth()-Depth_Keller msg: 1613175281436 epoch sec; seqNo:10902; depth 9.63 2021-02-13T00:14:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175281.45 epoch sec; seqNo:10902; 4058700.91 north; 592212.19 east 2021-02-13T00:14:41Z- AHRS_M2 msg: 1613175281.84 epoch sec; seqNo:10811 2021-02-13T00:14:41Z- AHRS_M2 msg: 0.74 phi; 0.33 theta; -0.18 psi 2021-02-13T00:14:41Z- handleDepth()-Depth_Keller msg: 1613175281840 epoch sec; seqNo:10903; depth 9.52 2021-02-13T00:14:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175281.85 epoch sec; seqNo:10903; 4058700.91 north; 592212.19 east 2021-02-13T00:14:42Z- AHRS_M2 msg: 1613175282.24 epoch sec; seqNo:10812 2021-02-13T00:14:42Z- AHRS_M2 msg: 0.75 phi; 0.33 theta; -0.17 psi 2021-02-13T00:14:42Z- handleDepth()-Depth_Keller msg: 1613175282249 epoch sec; seqNo:10904; depth 9.41 2021-02-13T00:14:42Z- handleDvl() - RDI_Pathfinder msg: 1613175282.26 epoch sec; seqNo:5377 2021-02-13T00:14:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.78 , 105.36 , 118.89 , 79.71 2021-02-13T00:14:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.10 , 0.02 2021-02-13T00:14:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175282.27 epoch sec; seqNo:10904; 4058701.29 north; 592212.19 east 2021-02-13T00:14:42Z- AHRS_M2 msg: 1613175282.64 epoch sec; seqNo:10813 2021-02-13T00:14:42Z- AHRS_M2 msg: 0.75 phi; 0.33 theta; -0.16 psi 2021-02-13T00:14:42Z- handleDepth()-Depth_Keller msg: 1613175282661 epoch sec; seqNo:10905; depth 9.31 2021-02-13T00:14:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175282.67 epoch sec; seqNo:10905; 4058701.68 north; 592213.41 east 2021-02-13T00:14:43Z- AHRS_M2 msg: 1613175283.05 epoch sec; seqNo:10814 2021-02-13T00:14:43Z- AHRS_M2 msg: 0.75 phi; 0.33 theta; -0.16 psi 2021-02-13T00:14:43Z- handleDepth()-Depth_Keller msg: 1613175283057 epoch sec; seqNo:10906; depth 9.18 2021-02-13T00:14:43Z- handleDvl() - RDI_Pathfinder msg: 1613175283.08 epoch sec; seqNo:5378 2021-02-13T00:14:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.13 , 105.36 , 118.89 , 0.00 2021-02-13T00:14:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.09 , -0.07 2021-02-13T00:14:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175283.09 epoch sec; seqNo:10906; 4058701.68 north; 592213.41 east 2021-02-13T00:14:43Z- AHRS_M2 msg: 1613175283.45 epoch sec; seqNo:10815 2021-02-13T00:14:43Z- AHRS_M2 msg: 0.75 phi; 0.33 theta; -0.15 psi 2021-02-13T00:14:43Z- handleDepth()-Depth_Keller msg: 1613175283461 epoch sec; seqNo:10907; depth 9.06 2021-02-13T00:14:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:43Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:43Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:43Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:43Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:43Z- handleDvl() - RDI_Pathfinder msg: 1613175283.46 epoch sec; seqNo:5379 2021-02-13T00:14:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.09 , -0.07 2021-02-13T00:14:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175283.47 epoch sec; seqNo:10907; 4058702.06 north; 592213.40 east 2021-02-13T00:14:43Z- AHRS_M2 msg: 1613175283.86 epoch sec; seqNo:10816 2021-02-13T00:14:43Z- AHRS_M2 msg: 0.75 phi; 0.33 theta; -0.15 psi 2021-02-13T00:14:43Z- handleDepth()-Depth_Keller msg: 1613175283861 epoch sec; seqNo:10908; depth 8.94 2021-02-13T00:14:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175283.87 epoch sec; seqNo:10908; 4058702.06 north; 592213.40 east 2021-02-13T00:14:44Z- AHRS_M2 msg: 1613175284.26 epoch sec; seqNo:10817 2021-02-13T00:14:44Z- AHRS_M2 msg: 0.76 phi; 0.33 theta; -0.15 psi 2021-02-13T00:14:44Z- handleDepth()-Depth_Keller msg: 1613175284264 epoch sec; seqNo:10909; depth 8.84 2021-02-13T00:14:44Z- handleDvl() - RDI_Pathfinder msg: 1613175284.28 epoch sec; seqNo:5380 2021-02-13T00:14:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.79 , 105.16 , 117.06 , 81.36 2021-02-13T00:14:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.13 , -0.12 2021-02-13T00:14:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175284.29 epoch sec; seqNo:10909; 4058702.44 north; 592213.40 east 2021-02-13T00:14:44Z- AHRS_M2 msg: 1613175284.66 epoch sec; seqNo:10818 2021-02-13T00:14:44Z- AHRS_M2 msg: 0.76 phi; 0.32 theta; -0.15 psi 2021-02-13T00:14:44Z- handleDepth()-Depth_Keller msg: 1613175284669 epoch sec; seqNo:10910; depth 8.71 2021-02-13T00:14:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:44Z- handleDvl() - RDI_Pathfinder msg: 1613175284.67 epoch sec; seqNo:5381 2021-02-13T00:14:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.13 , -0.12 2021-02-13T00:14:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175284.69 epoch sec; seqNo:10910; 4058702.83 north; 592214.62 east 2021-02-13T00:14:45Z- AHRS_M2 msg: 1613175285.07 epoch sec; seqNo:10819 2021-02-13T00:14:45Z- AHRS_M2 msg: 0.77 phi; 0.32 theta; -0.15 psi 2021-02-13T00:14:45Z- handleDepth()-Depth_Keller msg: 1613175285072 epoch sec; seqNo:10911; depth 8.60 2021-02-13T00:14:45Z- handleDvl() - RDI_Pathfinder msg: 1613175285.08 epoch sec; seqNo:5382 2021-02-13T00:14:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.18 , 105.29 , 118.82 , 80.95 2021-02-13T00:14:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.14 , -0.16 2021-02-13T00:14:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175285.10 epoch sec; seqNo:10911; 4058702.83 north; 592214.62 east 2021-02-13T00:14:45Z- AHRS_M2 msg: 1613175285.47 epoch sec; seqNo:10820 2021-02-13T00:14:45Z- AHRS_M2 msg: 0.78 phi; 0.32 theta; -0.16 psi 2021-02-13T00:14:45Z- handleDepth()-Depth_Keller msg: 1613175285476 epoch sec; seqNo:10912; depth 8.47 2021-02-13T00:14:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175285.49 epoch sec; seqNo:10912; 4058702.83 north; 592214.62 east 2021-02-13T00:14:45Z- AHRS_M2 msg: 1613175285.88 epoch sec; seqNo:10821 2021-02-13T00:14:45Z- AHRS_M2 msg: 0.78 phi; 0.32 theta; -0.16 psi 2021-02-13T00:14:45Z- handleDepth()-Depth_Keller msg: 1613175285889 epoch sec; seqNo:10913; depth 8.37 2021-02-13T00:14:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:45Z- handleDvl() - RDI_Pathfinder msg: 1613175285.89 epoch sec; seqNo:5383 2021-02-13T00:14:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.14 , -0.16 2021-02-13T00:14:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175285.90 epoch sec; seqNo:10913; 4058703.21 north; 592214.61 east 2021-02-13T00:14:46Z- LcmTrn::handleMessages() - handling 47 messages took 0.67 ms and 134 calls... 2021-02-13T00:14:46Z- waiting for 1613175283.65, time is 1613175285.90 2021-02-13T00:14:46Z- LcmTrn::updateTrn() equating pose 1613175285.90 and meas times 1613175285.90 2021-02-13T00:14:46Z- LcmTrn::updateTrn() >>>> pose time 1613175285.90, meas time 1613175285.90 <<<< 2021-02-13T00:14:46Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175285.90, ping # 0. 2021-02-13T00:14:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175285.90 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:14:46Z- TerrainNav::motionUpdate() - currEstimate = (4058703, 592214.61, 8.37) 2021-02-13T00:14:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:14:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:14:46Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:14:46Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:14:46Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175285.90 sec.; No relevant navigation data available 2021-02-13T00:14:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:14:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:14:46Z- variances([0], [2], and psi_berg) = 712.314791 833.004857 0.000000 2021-02-13T00:14:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:14:46Z- LcmTrn::publishEstimates() - MLE : 4058702.71 592264.11 8.37 2021-02-13T00:14:46Z- LcmTrn::publishEstimates() - MMSE : 4058703.21 592214.61 8.37 2021-02-13T00:14:46Z- LcmTrn::publishEstimates() - COVAR: 712.31 833.00 1.18 0.00 2021-02-13T00:14:46Z- AHRS_M2 msg: 1613175286.28 epoch sec; seqNo:10822 2021-02-13T00:14:46Z- AHRS_M2 msg: 0.79 phi; 0.32 theta; -0.16 psi 2021-02-13T00:14:46Z- handleDepth()-Depth_Keller msg: 1613175286287 epoch sec; seqNo:10914; depth 8.24 2021-02-13T00:14:46Z- handleDvl() - RDI_Pathfinder msg: 1613175286.31 epoch sec; seqNo:5384 2021-02-13T00:14:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.86 , 106.44 , 118.62 , 80.75 2021-02-13T00:14:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.03 , -0.15 2021-02-13T00:14:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175286.32 epoch sec; seqNo:10914; 4058703.21 north; 592214.61 east 2021-02-13T00:14:46Z- AHRS_M2 msg: 1613175286.68 epoch sec; seqNo:10823 2021-02-13T00:14:46Z- AHRS_M2 msg: 0.79 phi; 0.32 theta; -0.16 psi 2021-02-13T00:14:46Z- handleDepth()-Depth_Keller msg: 1613175286688 epoch sec; seqNo:10915; depth 8.10 2021-02-13T00:14:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175286.70 epoch sec; seqNo:10915; 4058703.60 north; 592215.83 east 2021-02-13T00:14:47Z- AHRS_M2 msg: 1613175287.09 epoch sec; seqNo:10824 2021-02-13T00:14:47Z- AHRS_M2 msg: 0.78 phi; 0.32 theta; -0.16 psi 2021-02-13T00:14:47Z- handleDepth()-Depth_Keller msg: 1613175287094 epoch sec; seqNo:10916; depth 7.99 2021-02-13T00:14:47Z- handleDvl() - RDI_Pathfinder msg: 1613175287.11 epoch sec; seqNo:5385 2021-02-13T00:14:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.17 , 106.64 , 118.81 , 82.30 2021-02-13T00:14:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.14 , -0.05 2021-02-13T00:14:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175287.12 epoch sec; seqNo:10916; 4058703.98 north; 592215.82 east 2021-02-13T00:14:47Z- AHRS_M2 msg: 1613175287.49 epoch sec; seqNo:10825 2021-02-13T00:14:47Z- AHRS_M2 msg: 0.78 phi; 0.32 theta; -0.17 psi 2021-02-13T00:14:47Z- handleDepth()-Depth_Keller msg: 1613175287498 epoch sec; seqNo:10917; depth 7.87 2021-02-13T00:14:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175287.51 epoch sec; seqNo:10917; 4058703.98 north; 592215.82 east 2021-02-13T00:14:47Z- AHRS_M2 msg: 1613175287.90 epoch sec; seqNo:10826 2021-02-13T00:14:47Z- AHRS_M2 msg: 0.77 phi; 0.32 theta; -0.17 psi 2021-02-13T00:14:47Z- handleDepth()-Depth_Keller msg: 1613175287900 epoch sec; seqNo:10918; depth 7.74 2021-02-13T00:14:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175287.91 epoch sec; seqNo:10918; 4058704.36 north; 592215.82 east 2021-02-13T00:14:48Z- AHRS_M2 msg: 1613175288.30 epoch sec; seqNo:10827 2021-02-13T00:14:48Z- AHRS_M2 msg: 0.76 phi; 0.32 theta; -0.17 psi 2021-02-13T00:14:48Z- handleDepth()-Depth_Keller msg: 1613175288304 epoch sec; seqNo:10919; depth 7.64 2021-02-13T00:14:48Z- handleDvl() - RDI_Pathfinder msg: 1613175288.32 epoch sec; seqNo:5386 2021-02-13T00:14:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 106.91 , 117.32 , 81.62 2021-02-13T00:14:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.17 , 0.02 2021-02-13T00:14:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175288.33 epoch sec; seqNo:10919; 4058704.74 north; 592215.82 east 2021-02-13T00:14:48Z- AHRS_M2 msg: 1613175288.70 epoch sec; seqNo:10828 2021-02-13T00:14:48Z- AHRS_M2 msg: 0.76 phi; 0.32 theta; -0.18 psi 2021-02-13T00:14:48Z- handleDepth()-Depth_Keller msg: 1613175288710 epoch sec; seqNo:10920; depth 7.51 2021-02-13T00:14:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175288.72 epoch sec; seqNo:10920; 4058704.74 north; 592215.82 east 2021-02-13T00:14:49Z- AHRS_M2 msg: 1613175289.11 epoch sec; seqNo:10829 2021-02-13T00:14:49Z- AHRS_M2 msg: 0.75 phi; 0.32 theta; -0.18 psi 2021-02-13T00:14:49Z- handleDepth()-Depth_Keller msg: 1613175289117 epoch sec; seqNo:10921; depth 7.41 2021-02-13T00:14:49Z- handleDvl() - RDI_Pathfinder msg: 1613175289.14 epoch sec; seqNo:5387 2021-02-13T00:14:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.86 , 106.44 , 115.91 , 82.10 2021-02-13T00:14:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.11 , 0.08 2021-02-13T00:14:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175289.15 epoch sec; seqNo:10921; 4058705.12 north; 592215.81 east 2021-02-13T00:14:49Z- AHRS_M2 msg: 1613175289.51 epoch sec; seqNo:10830 2021-02-13T00:14:49Z- AHRS_M2 msg: 0.74 phi; 0.32 theta; -0.18 psi 2021-02-13T00:14:49Z- handleDepth()-Depth_Keller msg: 1613175289521 epoch sec; seqNo:10922; depth 7.28 2021-02-13T00:14:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:49Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:49Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:49Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:49Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:49Z- handleDvl() - RDI_Pathfinder msg: 1613175289.52 epoch sec; seqNo:5388 2021-02-13T00:14:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.11 , 0.08 2021-02-13T00:14:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175289.54 epoch sec; seqNo:10922; 4058705.51 north; 592217.03 east 2021-02-13T00:14:49Z- AHRS_M2 msg: 1613175289.92 epoch sec; seqNo:10831 2021-02-13T00:14:49Z- AHRS_M2 msg: 0.74 phi; 0.32 theta; -0.18 psi 2021-02-13T00:14:49Z- handleDepth()-Depth_Keller msg: 1613175289923 epoch sec; seqNo:10923; depth 7.16 2021-02-13T00:14:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175289.93 epoch sec; seqNo:10923; 4058705.51 north; 592217.03 east 2021-02-13T00:14:50Z- AHRS_M2 msg: 1613175290.32 epoch sec; seqNo:10832 2021-02-13T00:14:50Z- AHRS_M2 msg: 0.74 phi; 0.32 theta; -0.19 psi 2021-02-13T00:14:50Z- handleDepth()-Depth_Keller msg: 1613175290324 epoch sec; seqNo:10924; depth 7.05 2021-02-13T00:14:50Z- handleDvl() - RDI_Pathfinder msg: 1613175290.34 epoch sec; seqNo:5389 2021-02-13T00:14:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.41 , 106.64 , 122.87 , 82.30 2021-02-13T00:14:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.13 2021-02-13T00:14:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175290.35 epoch sec; seqNo:10924; 4058705.89 north; 592217.02 east 2021-02-13T00:14:50Z- AHRS_M2 msg: 1613175290.72 epoch sec; seqNo:10833 2021-02-13T00:14:50Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.19 psi 2021-02-13T00:14:50Z- handleDepth()-Depth_Keller msg: 1613175290733 epoch sec; seqNo:10925; depth 6.93 2021-02-13T00:14:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:50Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:50Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:50Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:50Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:50Z- handleDvl() - RDI_Pathfinder msg: 1613175290.73 epoch sec; seqNo:5390 2021-02-13T00:14:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.13 2021-02-13T00:14:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175290.75 epoch sec; seqNo:10925; 4058706.27 north; 592217.02 east 2021-02-13T00:14:51Z- AHRS_M2 msg: 1613175291.13 epoch sec; seqNo:10834 2021-02-13T00:14:51Z- AHRS_M2 msg: 0.73 phi; 0.31 theta; -0.20 psi 2021-02-13T00:14:51Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-13T00:14:51Z- waiting for 1613175288.90, time is 1613175291.13 2021-02-13T00:14:51Z- LcmTrn::updateTrn() >>>> pose time 1613175291.13, meas time 1613175290.73 <<<< 2021-02-13T00:14:51Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175290.73, ping # 0. 2021-02-13T00:14:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175290.73 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:14:51Z- TerrainNav::motionUpdate() - currEstimate = (4058706, 592217.02, 6.93) 2021-02-13T00:14:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:14:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:14:51Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:14:51Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:14:51Z- TNavBF::Projecting Measurements 2021-02-13T00:14:51Z- TNavFilter::Measurement from time = 1613175290.73 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:14:51Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:14:51Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:14:51Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:14:51Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:14:51Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:14:51Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:14:51Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:14:51Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 712.3 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T00:14:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:14:51Z- TNavBF::mmse selected from filter -1 2021-02-13T00:14:51Z- variances([0], [2], and psi_berg) = 712.239297 833.004812 0.000000 2021-02-13T00:14:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:14:51Z- LcmTrn::publishEstimates() - MLE : 4058705.77 592266.52 6.93 2021-02-13T00:14:51Z- LcmTrn::publishEstimates() - MMSE : 4058706.27 592217.02 6.93 2021-02-13T00:14:51Z- LcmTrn::publishEstimates() - COVAR: 712.24 833.00 1.38 0.00 2021-02-13T00:14:51Z- handleDepth()-Depth_Keller msg: 1613175291138 epoch sec; seqNo:10926; depth 6.83 2021-02-13T00:14:51Z- handleDvl() - RDI_Pathfinder msg: 1613175291.15 epoch sec; seqNo:5391 2021-02-13T00:14:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 109.14 , 119.96 , 83.45 2021-02-13T00:14:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , 0.12 2021-02-13T00:14:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175291.16 epoch sec; seqNo:10926; 4058706.64 north; 592217.02 east 2021-02-13T00:14:51Z- AHRS_M2 msg: 1613175291.53 epoch sec; seqNo:10835 2021-02-13T00:14:51Z- AHRS_M2 msg: 0.73 phi; 0.31 theta; -0.20 psi 2021-02-13T00:14:51Z- handleDepth()-Depth_Keller msg: 1613175291536 epoch sec; seqNo:10927; depth 6.69 2021-02-13T00:14:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175291.57 epoch sec; seqNo:10927; 4058706.66 north; 592218.24 east 2021-02-13T00:14:51Z- AHRS_M2 msg: 1613175291.94 epoch sec; seqNo:10836 2021-02-13T00:14:51Z- AHRS_M2 msg: 0.73 phi; 0.31 theta; -0.20 psi 2021-02-13T00:14:51Z- handleDepth()-Depth_Keller msg: 1613175291949 epoch sec; seqNo:10928; depth 6.59 2021-02-13T00:14:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:51Z- handleDvl() - RDI_Pathfinder msg: 1613175291.95 epoch sec; seqNo:5392 2021-02-13T00:14:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , 0.12 2021-02-13T00:14:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175292.00 epoch sec; seqNo:10928; 4058707.04 north; 592218.23 east 2021-02-13T00:14:52Z- AHRS_M2 msg: 1613175292.34 epoch sec; seqNo:10837 2021-02-13T00:14:52Z- AHRS_M2 msg: 0.72 phi; 0.31 theta; -0.20 psi 2021-02-13T00:14:52Z- handleDepth()-Depth_Keller msg: 1613175292344 epoch sec; seqNo:10929; depth 6.48 2021-02-13T00:14:52Z- handleDvl() - RDI_Pathfinder msg: 1613175292.36 epoch sec; seqNo:5393 2021-02-13T00:14:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.00 , 111.31 , 121.73 , 84.53 2021-02-13T00:14:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.05 , 0.10 2021-02-13T00:14:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175292.37 epoch sec; seqNo:10929; 4058707.42 north; 592218.23 east 2021-02-13T00:14:52Z- AHRS_M2 msg: 1613175292.74 epoch sec; seqNo:10838 2021-02-13T00:14:52Z- AHRS_M2 msg: 0.72 phi; 0.31 theta; -0.20 psi 2021-02-13T00:14:52Z- handleDepth()-Depth_Keller msg: 1613175292750 epoch sec; seqNo:10930; depth 6.38 2021-02-13T00:14:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175292.76 epoch sec; seqNo:10930; 4058707.42 north; 592218.23 east 2021-02-13T00:14:53Z- AHRS_M2 msg: 1613175293.15 epoch sec; seqNo:10839 2021-02-13T00:14:53Z- AHRS_M2 msg: 0.71 phi; 0.31 theta; -0.19 psi 2021-02-13T00:14:53Z- handleDepth()-Depth_Keller msg: 1613175293155 epoch sec; seqNo:10931; depth 6.28 2021-02-13T00:14:53Z- handleDvl() - RDI_Pathfinder msg: 1613175293.18 epoch sec; seqNo:5394 2021-02-13T00:14:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 109.89 , 123.28 , 121.80 2021-02-13T00:14:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , 0.11 , -0.02 2021-02-13T00:14:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175293.19 epoch sec; seqNo:10931; 4058707.79 north; 592218.23 east 2021-02-13T00:14:53Z- AHRS_M2 msg: 1613175293.56 epoch sec; seqNo:10840 2021-02-13T00:14:53Z- AHRS_M2 msg: 0.71 phi; 0.31 theta; -0.18 psi 2021-02-13T00:14:53Z- handleDepth()-Depth_Keller msg: 1613175293565 epoch sec; seqNo:10932; depth 6.17 2021-02-13T00:14:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175293.58 epoch sec; seqNo:10932; 4058707.81 north; 592219.45 east 2021-02-13T00:14:53Z- AHRS_M2 msg: 1613175293.96 epoch sec; seqNo:10841 2021-02-13T00:14:53Z- AHRS_M2 msg: 0.72 phi; 0.31 theta; -0.18 psi 2021-02-13T00:14:53Z- handleDepth()-Depth_Keller msg: 1613175293966 epoch sec; seqNo:10933; depth 6.09 2021-02-13T00:14:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175293.97 epoch sec; seqNo:10933; 4058708.19 north; 592219.44 east 2021-02-13T00:14:54Z- AHRS_M2 msg: 1613175294.36 epoch sec; seqNo:10842 2021-02-13T00:14:54Z- AHRS_M2 msg: 0.72 phi; 0.31 theta; -0.18 psi 2021-02-13T00:14:54Z- handleDepth()-Depth_Keller msg: 1613175294370 epoch sec; seqNo:10934; depth 5.99 2021-02-13T00:14:54Z- handleDvl() - RDI_Pathfinder msg: 1613175294.38 epoch sec; seqNo:5395 2021-02-13T00:14:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 109.14 , 119.56 , 83.85 2021-02-13T00:14:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.17 , -0.06 2021-02-13T00:14:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175294.39 epoch sec; seqNo:10934; 4058708.19 north; 592219.44 east 2021-02-13T00:14:54Z- AHRS_M2 msg: 1613175294.76 epoch sec; seqNo:10843 2021-02-13T00:14:54Z- AHRS_M2 msg: 0.72 phi; 0.31 theta; -0.19 psi 2021-02-13T00:14:54Z- handleDepth()-Depth_Keller msg: 1613175294773 epoch sec; seqNo:10935; depth 5.90 2021-02-13T00:14:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175294.79 epoch sec; seqNo:10935; 4058708.57 north; 592219.44 east 2021-02-13T00:14:55Z- AHRS_M2 msg: 1613175295.17 epoch sec; seqNo:10844 2021-02-13T00:14:55Z- AHRS_M2 msg: 0.72 phi; 0.31 theta; -0.21 psi 2021-02-13T00:14:55Z- handleDepth()-Depth_Keller msg: 1613175295181 epoch sec; seqNo:10936; depth 5.78 2021-02-13T00:14:55Z- handleDvl() - RDI_Pathfinder msg: 1613175295.19 epoch sec; seqNo:5396 2021-02-13T00:14:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 111.31 , 123.21 , 0.00 2021-02-13T00:14:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.00 , -0.22 2021-02-13T00:14:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175295.20 epoch sec; seqNo:10936; 4058708.57 north; 592219.44 east 2021-02-13T00:14:55Z- AHRS_M2 msg: 1613175295.57 epoch sec; seqNo:10845 2021-02-13T00:14:55Z- AHRS_M2 msg: 0.72 phi; 0.31 theta; -0.22 psi 2021-02-13T00:14:55Z- handleDepth()-Depth_Keller msg: 1613175295582 epoch sec; seqNo:10937; depth 5.68 2021-02-13T00:14:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:55Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:55Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:55Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:55Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:55Z- handleDvl() - RDI_Pathfinder msg: 1613175295.58 epoch sec; seqNo:5397 2021-02-13T00:14:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.00 , -0.22 2021-02-13T00:14:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175295.60 epoch sec; seqNo:10937; 4058708.94 north; 592219.43 east 2021-02-13T00:14:56Z- AHRS_M2 msg: 1613175295.99 epoch sec; seqNo:10846 2021-02-13T00:14:56Z- AHRS_M2 msg: 0.72 phi; 0.31 theta; -0.22 psi 2021-02-13T00:14:56Z- handleDepth()-Depth_Keller msg: 1613175296006 epoch sec; seqNo:10938; depth 5.57 2021-02-13T00:14:56Z- handleDvl() - RDI_Pathfinder msg: 1613175296.02 epoch sec; seqNo:5398 2021-02-13T00:14:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.80 , 113.61 , 122.53 , 0.00 2021-02-13T00:14:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.26 , -0.04 2021-02-13T00:14:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175296.03 epoch sec; seqNo:10938; 4058709.34 north; 592220.65 east 2021-02-13T00:14:56Z- LcmTrn::handleMessages() - handling 46 messages took 0.73 ms and 132 calls... 2021-02-13T00:14:56Z- waiting for 1613175294.13, time is 1613175296.03 2021-02-13T00:14:56Z- LcmTrn::updateTrn() equating pose 1613175296.03 and meas times 1613175296.03 2021-02-13T00:14:56Z- LcmTrn::updateTrn() >>>> pose time 1613175296.03, meas time 1613175296.03 <<<< 2021-02-13T00:14:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175296.03 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:14:56Z- TerrainNav::motionUpdate() - currEstimate = (4058709, 592220.65, 5.57) 2021-02-13T00:14:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:14:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:14:56Z- TNavBF::Projecting Measurements 2021-02-13T00:14:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:14:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:14:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:14:56Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:14:56Z- TNavFilter::3 good beams. 2021-02-13T00:14:56Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:14:56Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:14:56Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:14:56Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:14:56Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:14:56Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:14:56Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:14:56Z- TNavBF::mmse selected from filter -1 2021-02-13T00:14:56Z- TerrainMapOctree: (vehN, vehE ) = (4058709.34, 592220.65). 2021-02-13T00:14:56Z- TerrainMapOctree: Min Distance = 3251.65. 2021-02-13T00:14:56Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:14:56Z- There is no specific good beam that all particles have in common. 2021-02-13T00:14:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:14:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.62 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 12.90 2021-02-13T00:14:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:14:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.46 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 20.65 2021-02-13T00:14:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:14:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 24.02 2021-02-13T00:14:56Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:14:56Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:14:56Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:14:56Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:14:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:14:56Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:14:56Z- Filter 0 No beams used in subcloud update 2021-02-13T00:14:56Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:14:56Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:14:56Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 712.2 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:14:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175296.03 sec. 2021-02-13T00:14:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:14:56Z- TNavBF::mmse selected from filter -1 2021-02-13T00:14:56Z- variances([0], [2], and psi_berg) = 712.171892 833.004772 0.000000 2021-02-13T00:14:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:14:56Z- LcmTrn::publishEstimates() - MLE : 4058708.84 592270.15 5.57 2021-02-13T00:14:56Z- LcmTrn::publishEstimates() - MMSE : 4058709.34 592220.65 5.57 2021-02-13T00:14:56Z- LcmTrn::publishEstimates() - COVAR: 712.17 833.00 1.30 0.00 2021-02-13T00:14:56Z- AHRS_M2 msg: 1613175296.38 epoch sec; seqNo:10847 2021-02-13T00:14:56Z- AHRS_M2 msg: 0.71 phi; 0.30 theta; -0.21 psi 2021-02-13T00:14:56Z- handleDepth()-Depth_Keller msg: 1613175296389 epoch sec; seqNo:10939; depth 5.42 2021-02-13T00:14:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175296.40 epoch sec; seqNo:10939; 4058709.71 north; 592220.65 east 2021-02-13T00:14:56Z- AHRS_M2 msg: 1613175296.79 epoch sec; seqNo:10848 2021-02-13T00:14:56Z- AHRS_M2 msg: 0.71 phi; 0.29 theta; -0.20 psi 2021-02-13T00:14:56Z- handleDepth()-Depth_Keller msg: 1613175296790 epoch sec; seqNo:10940; depth 5.33 2021-02-13T00:14:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:56Z- handleDvl() - RDI_Pathfinder msg: 1613175296.79 epoch sec; seqNo:5399 2021-02-13T00:14:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.26 , -0.04 2021-02-13T00:14:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175296.80 epoch sec; seqNo:10940; 4058709.71 north; 592220.65 east 2021-02-13T00:14:57Z- AHRS_M2 msg: 1613175297.19 epoch sec; seqNo:10849 2021-02-13T00:14:57Z- AHRS_M2 msg: 0.70 phi; 0.27 theta; -0.18 psi 2021-02-13T00:14:57Z- handleDepth()-Depth_Keller msg: 1613175297193 epoch sec; seqNo:10941; depth 5.19 2021-02-13T00:14:57Z- handleDvl() - RDI_Pathfinder msg: 1613175297.21 epoch sec; seqNo:5400 2021-02-13T00:14:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:57Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:14:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175297.22 epoch sec; seqNo:10941; 4058710.09 north; 592220.64 east 2021-02-13T00:14:57Z- AHRS_M2 msg: 1613175297.59 epoch sec; seqNo:10850 2021-02-13T00:14:57Z- AHRS_M2 msg: 0.70 phi; 0.24 theta; -0.16 psi 2021-02-13T00:14:57Z- handleDepth()-Depth_Keller msg: 1613175297596 epoch sec; seqNo:10942; depth 5.07 2021-02-13T00:14:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175297.61 epoch sec; seqNo:10942; 4058710.47 north; 592220.64 east 2021-02-13T00:14:57Z- AHRS_M2 msg: 1613175297.99 epoch sec; seqNo:10851 2021-02-13T00:14:57Z- AHRS_M2 msg: 0.71 phi; 0.21 theta; -0.15 psi 2021-02-13T00:14:58Z- handleDepth()-Depth_Keller msg: 1613175298005 epoch sec; seqNo:10943; depth 4.94 2021-02-13T00:14:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:14:58Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:14:58Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:14:58Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:14:58Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:14:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:14:58Z- handleDvl() - RDI_Pathfinder msg: 1613175298.01 epoch sec; seqNo:5401 2021-02-13T00:14:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:14:58Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:14:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175298.02 epoch sec; seqNo:10943; 4058710.85 north; 592220.63 east 2021-02-13T00:14:58Z- AHRS_M2 msg: 1613175298.40 epoch sec; seqNo:10852 2021-02-13T00:14:58Z- AHRS_M2 msg: 0.72 phi; 0.17 theta; -0.15 psi 2021-02-13T00:14:58Z- handleDepth()-Depth_Keller msg: 1613175298406 epoch sec; seqNo:10944; depth 4.82 2021-02-13T00:14:58Z- handleDvl() - RDI_Pathfinder msg: 1613175298.42 epoch sec; seqNo:5402 2021-02-13T00:14:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 107.65 , 115.09 , 86.83 2021-02-13T00:14:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.32 , 0.31 2021-02-13T00:14:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175298.43 epoch sec; seqNo:10944; 4058710.85 north; 592220.63 east 2021-02-13T00:14:58Z- AHRS_M2 msg: 1613175298.80 epoch sec; seqNo:10853 2021-02-13T00:14:58Z- AHRS_M2 msg: 0.73 phi; 0.13 theta; -0.15 psi 2021-02-13T00:14:58Z- handleDepth()-Depth_Keller msg: 1613175298818 epoch sec; seqNo:10945; depth 4.72 2021-02-13T00:14:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175298.83 epoch sec; seqNo:10945; 4058711.24 north; 592221.85 east 2021-02-13T00:14:59Z- AHRS_M2 msg: 1613175299.21 epoch sec; seqNo:10854 2021-02-13T00:14:59Z- AHRS_M2 msg: 0.75 phi; 0.09 theta; -0.15 psi 2021-02-13T00:14:59Z- handleDepth()-Depth_Keller msg: 1613175299217 epoch sec; seqNo:10946; depth 4.61 2021-02-13T00:14:59Z- handleDvl() - RDI_Pathfinder msg: 1613175299.24 epoch sec; seqNo:5403 2021-02-13T00:14:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.55 , 103.94 , 108.41 , 97.99 2021-02-13T00:14:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.52 , -0.04 2021-02-13T00:14:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175299.25 epoch sec; seqNo:10946; 4058711.24 north; 592221.85 east 2021-02-13T00:14:59Z- AHRS_M2 msg: 1613175299.61 epoch sec; seqNo:10855 2021-02-13T00:14:59Z- AHRS_M2 msg: 0.76 phi; 0.04 theta; -0.16 psi 2021-02-13T00:14:59Z- handleDepth()-Depth_Keller msg: 1613175299616 epoch sec; seqNo:10947; depth 4.52 2021-02-13T00:14:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175299.63 epoch sec; seqNo:10947; 4058711.62 north; 592221.85 east 2021-02-13T00:15:00Z- AHRS_M2 msg: 1613175300.02 epoch sec; seqNo:10856 2021-02-13T00:15:00Z- AHRS_M2 msg: 0.77 phi; -0.00 theta; -0.17 psi 2021-02-13T00:15:00Z- handleDepth()-Depth_Keller msg: 1613175300022 epoch sec; seqNo:10948; depth 4.43 2021-02-13T00:15:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175300.03 epoch sec; seqNo:10948; 4058711.64 north; 592223.07 east 2021-02-13T00:15:00Z- AHRS_M2 msg: 1613175300.42 epoch sec; seqNo:10857 2021-02-13T00:15:00Z- AHRS_M2 msg: 0.79 phi; -0.04 theta; -0.18 psi 2021-02-13T00:15:00Z- handleDepth()-Depth_Keller msg: 1613175300424 epoch sec; seqNo:10949; depth 4.39 2021-02-13T00:15:00Z- handleDvl() - RDI_Pathfinder msg: 1613175300.44 epoch sec; seqNo:5404 2021-02-13T00:15:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 103.81 , 99.75 , 92.99 2021-02-13T00:15:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.26 , -0.37 2021-02-13T00:15:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175300.45 epoch sec; seqNo:10949; 4058712.01 north; 592223.06 east 2021-02-13T00:15:00Z- AHRS_M2 msg: 1613175300.82 epoch sec; seqNo:10858 2021-02-13T00:15:00Z- AHRS_M2 msg: 0.80 phi; -0.08 theta; -0.20 psi 2021-02-13T00:15:00Z- handleDepth()-Depth_Keller msg: 1613175300828 epoch sec; seqNo:10950; depth 4.33 2021-02-13T00:15:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175300.84 epoch sec; seqNo:10950; 4058712.01 north; 592223.06 east 2021-02-13T00:15:01Z- handleDepth()-Depth_Keller msg: 1613175301249 epoch sec; seqNo:10951; depth 4.29 2021-02-13T00:15:01Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-13T00:15:01Z- waiting for 1613175299.03, time is 1613175300.84 2021-02-13T00:15:01Z- LcmTrn::updateTrn() >>>> pose time 1613175300.84, meas time 1613175300.44 <<<< 2021-02-13T00:15:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175300.44 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:15:01Z- TerrainNav::motionUpdate() - currEstimate = (4058712, 592223.06, 4.29) 2021-02-13T00:15:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:15:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:15:01Z- TNavBF::Projecting Measurements 2021-02-13T00:15:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:15:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:15:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:15:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:15:01Z- TNavFilter::4 good beams. 2021-02-13T00:15:01Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:15:01Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:15:01Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:15:01Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:15:01Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:15:01Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:15:01Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:15:01Z- TNavBF::mmse selected from filter -1 2021-02-13T00:15:01Z- TerrainMapOctree: (vehN, vehE ) = (4058711.79, 592222.86). 2021-02-13T00:15:01Z- TerrainMapOctree: Min Distance = 3248.35. 2021-02-13T00:15:01Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:15:01Z- There is no specific good beam that all particles have in common. 2021-02-13T00:15:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:15:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.47 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 12.65 2021-02-13T00:15:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:15:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 17.24 2021-02-13T00:15:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:15:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 15.92 2021-02-13T00:15:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:15:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 13.83 2021-02-13T00:15:01Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:15:01Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:15:01Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:15:01Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:15:01Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:15:01Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:15:01Z- Filter 0 No beams used in subcloud update 2021-02-13T00:15:01Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:15:01Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:15:01Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 712.1 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:15:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175300.44 sec. 2021-02-13T00:15:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:15:01Z- TNavBF::mmse selected from filter -1 2021-02-13T00:15:01Z- variances([0], [2], and psi_berg) = 712.098870 833.004728 0.000000 2021-02-13T00:15:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:15:01Z- LcmTrn::publishEstimates() - MLE : 4058711.51 592272.56 4.29 2021-02-13T00:15:01Z- LcmTrn::publishEstimates() - MMSE : 4058712.01 592223.06 4.29 2021-02-13T00:15:01Z- LcmTrn::publishEstimates() - COVAR: 712.10 833.00 1.10 0.00 2021-02-13T00:15:01Z- handleDvl() - RDI_Pathfinder msg: 1613175301.26 epoch sec; seqNo:5405 2021-02-13T00:15:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 106.84 , 93.32 , 100.08 2021-02-13T00:15:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.16 , -0.43 2021-02-13T00:15:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175301.28 epoch sec; seqNo:10951; 4058712.39 north; 592223.06 east 2021-02-13T00:15:01Z- AHRS_M2 msg: 1613175301.64 epoch sec; seqNo:10859 2021-02-13T00:15:01Z- AHRS_M2 msg: 0.82 phi; -0.15 theta; -0.22 psi 2021-02-13T00:15:01Z- handleDepth()-Depth_Keller msg: 1613175301647 epoch sec; seqNo:10952; depth 4.28 2021-02-13T00:15:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:01Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:01Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:01Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:01Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:01Z- handleDvl() - RDI_Pathfinder msg: 1613175301.65 epoch sec; seqNo:5406 2021-02-13T00:15:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.16 , -0.43 2021-02-13T00:15:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175301.66 epoch sec; seqNo:10952; 4058712.79 north; 592224.28 east 2021-02-13T00:15:02Z- AHRS_M2 msg: 1613175302.04 epoch sec; seqNo:10860 2021-02-13T00:15:02Z- AHRS_M2 msg: 0.83 phi; -0.17 theta; -0.24 psi 2021-02-13T00:15:02Z- handleDepth()-Depth_Keller msg: 1613175302043 epoch sec; seqNo:10953; depth 4.29 2021-02-13T00:15:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175302.07 epoch sec; seqNo:10953; 4058713.16 north; 592224.27 east 2021-02-13T00:15:02Z- AHRS_M2 msg: 1613175302.44 epoch sec; seqNo:10861 2021-02-13T00:15:02Z- AHRS_M2 msg: 0.84 phi; -0.19 theta; -0.25 psi 2021-02-13T00:15:02Z- handleDepth()-Depth_Keller msg: 1613175302444 epoch sec; seqNo:10954; depth 4.30 2021-02-13T00:15:02Z- handleDvl() - RDI_Pathfinder msg: 1613175302.46 epoch sec; seqNo:5407 2021-02-13T00:15:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 112.18 , 90.55 , 105.42 2021-02-13T00:15:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.50 , -0.29 2021-02-13T00:15:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175302.48 epoch sec; seqNo:10954; 4058713.54 north; 592224.27 east 2021-02-13T00:15:02Z- AHRS_M2 msg: 1613175302.84 epoch sec; seqNo:10862 2021-02-13T00:15:02Z- AHRS_M2 msg: 0.85 phi; -0.21 theta; -0.24 psi 2021-02-13T00:15:02Z- handleDepth()-Depth_Keller msg: 1613175302848 epoch sec; seqNo:10955; depth 4.32 2021-02-13T00:15:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:02Z- handleDvl() - RDI_Pathfinder msg: 1613175302.85 epoch sec; seqNo:5408 2021-02-13T00:15:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.50 , -0.29 2021-02-13T00:15:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175302.86 epoch sec; seqNo:10955; 4058713.92 north; 592224.26 east 2021-02-13T00:15:03Z- AHRS_M2 msg: 1613175303.25 epoch sec; seqNo:10863 2021-02-13T00:15:03Z- AHRS_M2 msg: 0.85 phi; -0.24 theta; -0.22 psi 2021-02-13T00:15:03Z- handleDepth()-Depth_Keller msg: 1613175303254 epoch sec; seqNo:10956; depth 4.35 2021-02-13T00:15:03Z- handleDvl() - RDI_Pathfinder msg: 1613175303.27 epoch sec; seqNo:5409 2021-02-13T00:15:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.36 , 114.76 , 87.71 , 110.71 2021-02-13T00:15:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , -0.56 , 0.09 2021-02-13T00:15:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175303.28 epoch sec; seqNo:10956; 4058714.30 north; 592224.26 east 2021-02-13T00:15:03Z- AHRS_M2 msg: 1613175303.65 epoch sec; seqNo:10864 2021-02-13T00:15:03Z- AHRS_M2 msg: 0.86 phi; -0.27 theta; -0.19 psi 2021-02-13T00:15:03Z- handleDepth()-Depth_Keller msg: 1613175303659 epoch sec; seqNo:10957; depth 4.41 2021-02-13T00:15:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175303.68 epoch sec; seqNo:10957; 4058714.30 north; 592224.26 east 2021-02-13T00:15:04Z- AHRS_M2 msg: 1613175304.06 epoch sec; seqNo:10865 2021-02-13T00:15:04Z- AHRS_M2 msg: 0.85 phi; -0.30 theta; -0.16 psi 2021-02-13T00:15:04Z- handleDepth()-Depth_Keller msg: 1613175304061 epoch sec; seqNo:10958; depth 4.46 2021-02-13T00:15:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:04Z- handleDvl() - RDI_Pathfinder msg: 1613175304.06 epoch sec; seqNo:5410 2021-02-13T00:15:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , -0.56 , 0.09 2021-02-13T00:15:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175304.07 epoch sec; seqNo:10958; 4058714.68 north; 592224.26 east 2021-02-13T00:15:04Z- AHRS_M2 msg: 1613175304.46 epoch sec; seqNo:10866 2021-02-13T00:15:04Z- AHRS_M2 msg: 0.84 phi; -0.33 theta; -0.14 psi 2021-02-13T00:15:04Z- handleDepth()-Depth_Keller msg: 1613175304464 epoch sec; seqNo:10959; depth 4.52 2021-02-13T00:15:04Z- handleDvl() - RDI_Pathfinder msg: 1613175304.48 epoch sec; seqNo:5411 2021-02-13T00:15:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 107.86 , 86.22 , 115.98 2021-02-13T00:15:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.49 , -0.22 , 0.43 2021-02-13T00:15:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175304.49 epoch sec; seqNo:10959; 4058715.06 north; 592224.25 east 2021-02-13T00:15:04Z- AHRS_M2 msg: 1613175304.86 epoch sec; seqNo:10867 2021-02-13T00:15:04Z- AHRS_M2 msg: 0.83 phi; -0.35 theta; -0.14 psi 2021-02-13T00:15:04Z- handleDepth()-Depth_Keller msg: 1613175304871 epoch sec; seqNo:10960; depth 4.58 2021-02-13T00:15:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175304.88 epoch sec; seqNo:10960; 4058715.06 north; 592224.25 east 2021-02-13T00:15:05Z- AHRS_M2 msg: 1613175305.27 epoch sec; seqNo:10868 2021-02-13T00:15:05Z- AHRS_M2 msg: 0.81 phi; -0.36 theta; -0.15 psi 2021-02-13T00:15:05Z- handleDepth()-Depth_Keller msg: 1613175305277 epoch sec; seqNo:10961; depth 4.64 2021-02-13T00:15:05Z- handleDvl() - RDI_Pathfinder msg: 1613175305.29 epoch sec; seqNo:5412 2021-02-13T00:15:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 109.21 , 83.52 , 124.09 2021-02-13T00:15:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.58 , 0.22 , 0.46 2021-02-13T00:15:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175305.31 epoch sec; seqNo:10961; 4058715.06 north; 592224.25 east 2021-02-13T00:15:05Z- AHRS_M2 msg: 1613175305.67 epoch sec; seqNo:10869 2021-02-13T00:15:05Z- AHRS_M2 msg: 0.80 phi; -0.37 theta; -0.17 psi 2021-02-13T00:15:05Z- handleDepth()-Depth_Keller msg: 1613175305679 epoch sec; seqNo:10962; depth 4.73 2021-02-13T00:15:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175305.69 epoch sec; seqNo:10962; 4058715.06 north; 592224.25 east 2021-02-13T00:15:06Z- AHRS_M2 msg: 1613175306.09 epoch sec; seqNo:10870 2021-02-13T00:15:06Z- AHRS_M2 msg: 0.79 phi; -0.36 theta; -0.19 psi 2021-02-13T00:15:06Z- handleDepth()-Depth_Keller msg: 1613175306097 epoch sec; seqNo:10963; depth 4.79 2021-02-13T00:15:06Z- handleDvl() - RDI_Pathfinder msg: 1613175306.11 epoch sec; seqNo:5413 2021-02-13T00:15:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 112.66 , 0.00 , 130.51 2021-02-13T00:15:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.34 , 0.14 2021-02-13T00:15:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175306.12 epoch sec; seqNo:10963; 4058715.07 north; 592225.47 east 2021-02-13T00:15:06Z- LcmTrn::handleMessages() - handling 46 messages took 0.59 ms and 134 calls... 2021-02-13T00:15:06Z- waiting for 1613175303.84, time is 1613175306.12 2021-02-13T00:15:06Z- LcmTrn::updateTrn() equating pose 1613175306.12 and meas times 1613175306.12 2021-02-13T00:15:06Z- LcmTrn::updateTrn() >>>> pose time 1613175306.12, meas time 1613175306.12 <<<< 2021-02-13T00:15:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175306.12 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:15:06Z- TerrainNav::motionUpdate() - currEstimate = (4058715, 592225.47, 4.79) 2021-02-13T00:15:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:15:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:15:06Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175306.12 sec.; No relevant navigation data available 2021-02-13T00:15:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:15:06Z- TNavBF::mmse selected from filter -1 2021-02-13T00:15:06Z- variances([0], [2], and psi_berg) = 712.037976 833.004692 0.000000 2021-02-13T00:15:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:15:06Z- LcmTrn::publishEstimates() - MLE : 4058714.57 592274.97 4.79 2021-02-13T00:15:06Z- LcmTrn::publishEstimates() - MMSE : 4058715.07 592225.47 4.79 2021-02-13T00:15:06Z- LcmTrn::publishEstimates() - COVAR: 712.04 833.00 1.30 0.00 2021-02-13T00:15:06Z- AHRS_M2 msg: 1613175306.48 epoch sec; seqNo:10871 2021-02-13T00:15:06Z- AHRS_M2 msg: 0.78 phi; -0.36 theta; -0.23 psi 2021-02-13T00:15:06Z- handleDepth()-Depth_Keller msg: 1613175306484 epoch sec; seqNo:10964; depth 4.91 2021-02-13T00:15:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:06Z- handleDvl() - RDI_Pathfinder msg: 1613175306.49 epoch sec; seqNo:5414 2021-02-13T00:15:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.34 , 0.14 2021-02-13T00:15:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175306.50 epoch sec; seqNo:10964; 4058715.45 north; 592225.47 east 2021-02-13T00:15:06Z- AHRS_M2 msg: 1613175306.88 epoch sec; seqNo:10872 2021-02-13T00:15:06Z- AHRS_M2 msg: 0.78 phi; -0.35 theta; -0.26 psi 2021-02-13T00:15:06Z- handleDepth()-Depth_Keller msg: 1613175306888 epoch sec; seqNo:10965; depth 5.00 2021-02-13T00:15:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175306.90 epoch sec; seqNo:10965; 4058715.45 north; 592225.47 east 2021-02-13T00:15:07Z- AHRS_M2 msg: 1613175307.29 epoch sec; seqNo:10873 2021-02-13T00:15:07Z- AHRS_M2 msg: 0.78 phi; -0.34 theta; -0.28 psi 2021-02-13T00:15:07Z- handleDepth()-Depth_Keller msg: 1613175307301 epoch sec; seqNo:10966; depth 5.09 2021-02-13T00:15:07Z- handleDvl() - RDI_Pathfinder msg: 1613175307.31 epoch sec; seqNo:5415 2021-02-13T00:15:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.80 , 119.56 , 85.34 , 128.48 2021-02-13T00:15:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.09 , -0.10 2021-02-13T00:15:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175307.32 epoch sec; seqNo:10966; 4058715.83 north; 592225.46 east 2021-02-13T00:15:07Z- AHRS_M2 msg: 1613175307.71 epoch sec; seqNo:10874 2021-02-13T00:15:07Z- AHRS_M2 msg: 0.77 phi; -0.34 theta; -0.28 psi 2021-02-13T00:15:07Z- handleDepth()-Depth_Keller msg: 1613175307714 epoch sec; seqNo:10967; depth 5.21 2021-02-13T00:15:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:07Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:07Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:07Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:07Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:07Z- handleDvl() - RDI_Pathfinder msg: 1613175307.72 epoch sec; seqNo:5416 2021-02-13T00:15:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.09 , -0.10 2021-02-13T00:15:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175307.73 epoch sec; seqNo:10967; 4058715.84 north; 592226.68 east 2021-02-13T00:15:08Z- AHRS_M2 msg: 1613175308.10 epoch sec; seqNo:10875 2021-02-13T00:15:08Z- AHRS_M2 msg: 0.76 phi; -0.35 theta; -0.26 psi 2021-02-13T00:15:08Z- handleDepth()-Depth_Keller msg: 1613175308105 epoch sec; seqNo:10968; depth 5.33 2021-02-13T00:15:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175308.11 epoch sec; seqNo:10968; 4058716.22 north; 592226.68 east 2021-02-13T00:15:08Z- AHRS_M2 msg: 1613175308.50 epoch sec; seqNo:10876 2021-02-13T00:15:08Z- AHRS_M2 msg: 0.76 phi; -0.35 theta; -0.24 psi 2021-02-13T00:15:08Z- handleDepth()-Depth_Keller msg: 1613175308504 epoch sec; seqNo:10969; depth 5.45 2021-02-13T00:15:08Z- handleDvl() - RDI_Pathfinder msg: 1613175308.52 epoch sec; seqNo:5417 2021-02-13T00:15:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.65 , 124.36 , 85.68 , 130.31 2021-02-13T00:15:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , 0.01 2021-02-13T00:15:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175308.53 epoch sec; seqNo:10969; 4058716.60 north; 592226.68 east 2021-02-13T00:15:08Z- AHRS_M2 msg: 1613175308.90 epoch sec; seqNo:10877 2021-02-13T00:15:08Z- AHRS_M2 msg: 0.75 phi; -0.36 theta; -0.23 psi 2021-02-13T00:15:08Z- handleDepth()-Depth_Keller msg: 1613175308913 epoch sec; seqNo:10970; depth 5.57 2021-02-13T00:15:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:08Z- handleDvl() - RDI_Pathfinder msg: 1613175308.91 epoch sec; seqNo:5418 2021-02-13T00:15:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , 0.01 2021-02-13T00:15:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175308.92 epoch sec; seqNo:10970; 4058716.60 north; 592226.68 east 2021-02-13T00:15:09Z- AHRS_M2 msg: 1613175309.31 epoch sec; seqNo:10878 2021-02-13T00:15:09Z- AHRS_M2 msg: 0.75 phi; -0.37 theta; -0.22 psi 2021-02-13T00:15:09Z- handleDepth()-Depth_Keller msg: 1613175309313 epoch sec; seqNo:10971; depth 5.68 2021-02-13T00:15:09Z- handleDvl() - RDI_Pathfinder msg: 1613175309.33 epoch sec; seqNo:5419 2021-02-13T00:15:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.07 , 116.85 , 84.12 , 130.24 2021-02-13T00:15:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , 0.05 , 0.03 2021-02-13T00:15:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175309.34 epoch sec; seqNo:10971; 4058716.98 north; 592226.67 east 2021-02-13T00:15:09Z- AHRS_M2 msg: 1613175309.71 epoch sec; seqNo:10879 2021-02-13T00:15:09Z- AHRS_M2 msg: 0.74 phi; -0.37 theta; -0.22 psi 2021-02-13T00:15:09Z- handleDepth()-Depth_Keller msg: 1613175309716 epoch sec; seqNo:10972; depth 5.82 2021-02-13T00:15:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175309.73 epoch sec; seqNo:10972; 4058716.99 north; 592227.89 east 2021-02-13T00:15:10Z- AHRS_M2 msg: 1613175310.12 epoch sec; seqNo:10880 2021-02-13T00:15:10Z- AHRS_M2 msg: 0.73 phi; -0.38 theta; -0.23 psi 2021-02-13T00:15:10Z- handleDepth()-Depth_Keller msg: 1613175310122 epoch sec; seqNo:10973; depth 5.91 2021-02-13T00:15:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:10Z- handleDvl() - RDI_Pathfinder msg: 1613175310.12 epoch sec; seqNo:5420 2021-02-13T00:15:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , 0.05 , 0.03 2021-02-13T00:15:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175310.14 epoch sec; seqNo:10973; 4058717.37 north; 592227.89 east 2021-02-13T00:15:10Z- AHRS_M2 msg: 1613175310.52 epoch sec; seqNo:10881 2021-02-13T00:15:10Z- AHRS_M2 msg: 0.73 phi; -0.38 theta; -0.23 psi 2021-02-13T00:15:10Z- handleDepth()-Depth_Keller msg: 1613175310525 epoch sec; seqNo:10974; depth 6.04 2021-02-13T00:15:10Z- handleDvl() - RDI_Pathfinder msg: 1613175310.54 epoch sec; seqNo:5421 2021-02-13T00:15:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.70 , 116.99 , 84.26 , 137.81 2021-02-13T00:15:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , 0.03 , -0.01 2021-02-13T00:15:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175310.55 epoch sec; seqNo:10974; 4058717.37 north; 592227.89 east 2021-02-13T00:15:10Z- AHRS_M2 msg: 1613175310.93 epoch sec; seqNo:10882 2021-02-13T00:15:10Z- AHRS_M2 msg: 0.71 phi; -0.39 theta; -0.24 psi 2021-02-13T00:15:10Z- handleDepth()-Depth_Keller msg: 1613175310943 epoch sec; seqNo:10975; depth 6.16 2021-02-13T00:15:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175310.95 epoch sec; seqNo:10975; 4058717.75 north; 592227.89 east 2021-02-13T00:15:11Z- AHRS_M2 msg: 1613175311.34 epoch sec; seqNo:10883 2021-02-13T00:15:11Z- AHRS_M2 msg: 0.70 phi; -0.39 theta; -0.24 psi 2021-02-13T00:15:11Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-13T00:15:11Z- waiting for 1613175309.12, time is 1613175311.34 2021-02-13T00:15:11Z- LcmTrn::updateTrn() >>>> pose time 1613175311.34, meas time 1613175310.54 <<<< 2021-02-13T00:15:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175310.54 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:15:11Z- TerrainNav::motionUpdate() - currEstimate = (4058718, 592227.89, 6.16) 2021-02-13T00:15:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:15:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:15:11Z- TNavBF::Projecting Measurements 2021-02-13T00:15:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:15:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:15:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:15:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:15:11Z- TNavFilter::4 good beams. 2021-02-13T00:15:11Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:15:11Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:15:11Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:15:11Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:15:11Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:15:11Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:15:11Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:15:11Z- TNavBF::mmse selected from filter -1 2021-02-13T00:15:11Z- TerrainMapOctree: (vehN, vehE ) = (4058717.34, 592227.51). 2021-02-13T00:15:11Z- TerrainMapOctree: Min Distance = 3241.12. 2021-02-13T00:15:11Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:15:11Z- There is no specific good beam that all particles have in common. 2021-02-13T00:15:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:15:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.45 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 13.45 2021-02-13T00:15:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:15:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 21.90 2021-02-13T00:15:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:15:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 11.36 2021-02-13T00:15:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:15:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.034 ExpectedMeasDiff: nan currMeas.covariance: 30.39 2021-02-13T00:15:11Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:15:11Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:15:11Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:15:11Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:15:11Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:15:11Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:15:11Z- Filter 0 No beams used in subcloud update 2021-02-13T00:15:11Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:15:11Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:15:11Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 712.0 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:15:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175310.54 sec. 2021-02-13T00:15:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:15:11Z- TNavBF::mmse selected from filter -1 2021-02-13T00:15:11Z- variances([0], [2], and psi_berg) = 711.961150 833.004646 0.000000 2021-02-13T00:15:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:15:11Z- LcmTrn::publishEstimates() - MLE : 4058717.25 592277.39 6.16 2021-02-13T00:15:11Z- LcmTrn::publishEstimates() - MMSE : 4058717.75 592227.89 6.16 2021-02-13T00:15:11Z- LcmTrn::publishEstimates() - COVAR: 711.96 833.00 1.13 0.00 2021-02-13T00:15:11Z- handleDepth()-Depth_Keller msg: 1613175311354 epoch sec; seqNo:10976; depth 6.28 2021-02-13T00:15:11Z- handleDvl() - RDI_Pathfinder msg: 1613175311.37 epoch sec; seqNo:5422 2021-02-13T00:15:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.50 , 115.30 , 84.06 , 137.62 2021-02-13T00:15:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.09 , 0.06 2021-02-13T00:15:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175311.38 epoch sec; seqNo:10976; 4058718.13 north; 592227.88 east 2021-02-13T00:15:11Z- AHRS_M2 msg: 1613175311.73 epoch sec; seqNo:10884 2021-02-13T00:15:11Z- AHRS_M2 msg: 0.69 phi; -0.39 theta; -0.23 psi 2021-02-13T00:15:11Z- handleDepth()-Depth_Keller msg: 1613175311736 epoch sec; seqNo:10977; depth 6.42 2021-02-13T00:15:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175311.75 epoch sec; seqNo:10977; 4058718.13 north; 592227.88 east 2021-02-13T00:15:12Z- AHRS_M2 msg: 1613175312.14 epoch sec; seqNo:10885 2021-02-13T00:15:12Z- AHRS_M2 msg: 0.69 phi; -0.39 theta; -0.23 psi 2021-02-13T00:15:12Z- handleDepth()-Depth_Keller msg: 1613175312147 epoch sec; seqNo:10978; depth 6.55 2021-02-13T00:15:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175312.16 epoch sec; seqNo:10978; 4058718.52 north; 592229.10 east 2021-02-13T00:15:12Z- AHRS_M2 msg: 1613175312.54 epoch sec; seqNo:10886 2021-02-13T00:15:12Z- AHRS_M2 msg: 0.69 phi; -0.38 theta; -0.23 psi 2021-02-13T00:15:12Z- handleDepth()-Depth_Keller msg: 1613175312544 epoch sec; seqNo:10979; depth 6.69 2021-02-13T00:15:12Z- handleDvl() - RDI_Pathfinder msg: 1613175312.56 epoch sec; seqNo:5423 2021-02-13T00:15:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.52 , 115.37 , 82.64 , 136.20 2021-02-13T00:15:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.19 , -0.06 2021-02-13T00:15:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175312.57 epoch sec; seqNo:10979; 4058718.52 north; 592229.10 east 2021-02-13T00:15:12Z- AHRS_M2 msg: 1613175312.94 epoch sec; seqNo:10887 2021-02-13T00:15:12Z- AHRS_M2 msg: 0.70 phi; -0.38 theta; -0.22 psi 2021-02-13T00:15:12Z- handleDepth()-Depth_Keller msg: 1613175312948 epoch sec; seqNo:10980; depth 6.81 2021-02-13T00:15:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175312.96 epoch sec; seqNo:10980; 4058718.90 north; 592229.09 east 2021-02-13T00:15:13Z- AHRS_M2 msg: 1613175313.35 epoch sec; seqNo:10888 2021-02-13T00:15:13Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.22 psi 2021-02-13T00:15:13Z- handleDepth()-Depth_Keller msg: 1613175313357 epoch sec; seqNo:10981; depth 6.94 2021-02-13T00:15:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175313.37 epoch sec; seqNo:10981; 4058719.28 north; 592229.09 east 2021-02-13T00:15:13Z- AHRS_M2 msg: 1613175313.76 epoch sec; seqNo:10889 2021-02-13T00:15:13Z- AHRS_M2 msg: 0.71 phi; -0.36 theta; -0.20 psi 2021-02-13T00:15:13Z- handleDepth()-Depth_Keller msg: 1613175313763 epoch sec; seqNo:10982; depth 7.10 2021-02-13T00:15:13Z- handleDvl() - RDI_Pathfinder msg: 1613175313.78 epoch sec; seqNo:5424 2021-02-13T00:15:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.45 , 113.82 , 82.57 , 136.13 2021-02-13T00:15:13Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.07 , -0.26 2021-02-13T00:15:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175313.79 epoch sec; seqNo:10982; 4058719.29 north; 592230.31 east 2021-02-13T00:15:14Z- AHRS_M2 msg: 1613175314.16 epoch sec; seqNo:10890 2021-02-13T00:15:14Z- AHRS_M2 msg: 0.71 phi; -0.35 theta; -0.19 psi 2021-02-13T00:15:14Z- handleDepth()-Depth_Keller msg: 1613175314161 epoch sec; seqNo:10983; depth 7.25 2021-02-13T00:15:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175314.17 epoch sec; seqNo:10983; 4058719.67 north; 592230.31 east 2021-02-13T00:15:14Z- AHRS_M2 msg: 1613175314.56 epoch sec; seqNo:10891 2021-02-13T00:15:14Z- AHRS_M2 msg: 0.72 phi; -0.35 theta; -0.18 psi 2021-02-13T00:15:14Z- handleDepth()-Depth_Keller msg: 1613175314573 epoch sec; seqNo:10984; depth 7.37 2021-02-13T00:15:14Z- handleDvl() - RDI_Pathfinder msg: 1613175314.58 epoch sec; seqNo:5425 2021-02-13T00:15:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.52 , 113.82 , 81.08 , 127.21 2021-02-13T00:15:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.16 , -0.24 2021-02-13T00:15:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175314.59 epoch sec; seqNo:10984; 4058720.05 north; 592230.30 east 2021-02-13T00:15:14Z- AHRS_M2 msg: 1613175314.96 epoch sec; seqNo:10892 2021-02-13T00:15:14Z- AHRS_M2 msg: 0.72 phi; -0.35 theta; -0.17 psi 2021-02-13T00:15:14Z- handleDepth()-Depth_Keller msg: 1613175314977 epoch sec; seqNo:10985; depth 7.52 2021-02-13T00:15:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:14Z- handleDvl() - RDI_Pathfinder msg: 1613175314.98 epoch sec; seqNo:5426 2021-02-13T00:15:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.16 , -0.24 2021-02-13T00:15:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175314.99 epoch sec; seqNo:10985; 4058720.43 north; 592230.30 east 2021-02-13T00:15:15Z- AHRS_M2 msg: 1613175315.37 epoch sec; seqNo:10893 2021-02-13T00:15:15Z- AHRS_M2 msg: 0.72 phi; -0.34 theta; -0.16 psi 2021-02-13T00:15:15Z- handleDepth()-Depth_Keller msg: 1613175315374 epoch sec; seqNo:10986; depth 7.65 2021-02-13T00:15:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175315.40 epoch sec; seqNo:10986; 4058720.81 north; 592230.29 east 2021-02-13T00:15:15Z- AHRS_M2 msg: 1613175315.77 epoch sec; seqNo:10894 2021-02-13T00:15:15Z- AHRS_M2 msg: 0.72 phi; -0.34 theta; -0.15 psi 2021-02-13T00:15:15Z- handleDepth()-Depth_Keller msg: 1613175315781 epoch sec; seqNo:10987; depth 7.80 2021-02-13T00:15:15Z- handleDvl() - RDI_Pathfinder msg: 1613175315.79 epoch sec; seqNo:5427 2021-02-13T00:15:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 108.33 , 81.56 , 123.21 2021-02-13T00:15:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.19 , -0.11 2021-02-13T00:15:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175315.82 epoch sec; seqNo:10987; 4058721.19 north; 592230.29 east 2021-02-13T00:15:16Z- AHRS_M2 msg: 1613175316.18 epoch sec; seqNo:10895 2021-02-13T00:15:16Z- AHRS_M2 msg: 0.72 phi; -0.34 theta; -0.15 psi 2021-02-13T00:15:16Z- handleDepth()-Depth_Keller msg: 1613175316180 epoch sec; seqNo:10988; depth 7.94 2021-02-13T00:15:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:16Z- handleDvl() - RDI_Pathfinder msg: 1613175316.18 epoch sec; seqNo:5428 2021-02-13T00:15:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.19 , -0.11 2021-02-13T00:15:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175316.19 epoch sec; seqNo:10988; 4058721.58 north; 592231.51 east 2021-02-13T00:15:16Z- LcmTrn::handleMessages() - handling 45 messages took 0.61 ms and 133 calls... 2021-02-13T00:15:16Z- waiting for 1613175314.34, time is 1613175316.19 2021-02-13T00:15:16Z- LcmTrn::updateTrn() equating pose 1613175316.19 and meas times 1613175316.19 2021-02-13T00:15:16Z- LcmTrn::updateTrn() >>>> pose time 1613175316.19, meas time 1613175316.19 <<<< 2021-02-13T00:15:16Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175316.19, ping # 0. 2021-02-13T00:15:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175316.19 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:15:16Z- TerrainNav::motionUpdate() - currEstimate = (4058722, 592231.51, 7.94) 2021-02-13T00:15:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:15:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:15:16Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:15:16Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:15:16Z- TNavBF::Projecting Measurements 2021-02-13T00:15:16Z- TNavFilter::Measurement from time = 1613175316.19 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:15:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:15:16Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:15:16Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:15:16Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:15:16Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:15:16Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:15:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:15:16Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 711.9 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T00:15:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:15:16Z- TNavBF::mmse selected from filter -1 2021-02-13T00:15:16Z- variances([0], [2], and psi_berg) = 711.890403 833.004604 0.000000 2021-02-13T00:15:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:15:16Z- LcmTrn::publishEstimates() - MLE : 4058721.08 592281.01 7.94 2021-02-13T00:15:16Z- LcmTrn::publishEstimates() - MMSE : 4058721.58 592231.51 7.94 2021-02-13T00:15:16Z- LcmTrn::publishEstimates() - COVAR: 711.89 833.00 1.40 0.00 2021-02-13T00:15:16Z- AHRS_M2 msg: 1613175316.58 epoch sec; seqNo:10896 2021-02-13T00:15:16Z- AHRS_M2 msg: 0.72 phi; -0.35 theta; -0.14 psi 2021-02-13T00:15:16Z- handleDepth()-Depth_Keller msg: 1613175316586 epoch sec; seqNo:10989; depth 8.05 2021-02-13T00:15:16Z- handleDvl() - RDI_Pathfinder msg: 1613175316.60 epoch sec; seqNo:5429 2021-02-13T00:15:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 105.29 , 81.49 , 118.68 2021-02-13T00:15:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.17 , -0.05 2021-02-13T00:15:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175316.61 epoch sec; seqNo:10989; 4058721.58 north; 592231.51 east 2021-02-13T00:15:16Z- AHRS_M2 msg: 1613175316.98 epoch sec; seqNo:10897 2021-02-13T00:15:16Z- AHRS_M2 msg: 0.72 phi; -0.35 theta; -0.14 psi 2021-02-13T00:15:17Z- handleDepth()-Depth_Keller msg: 1613175316998 epoch sec; seqNo:10990; depth 8.19 2021-02-13T00:15:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175317.01 epoch sec; seqNo:10990; 4058721.96 north; 592231.50 east 2021-02-13T00:15:17Z- AHRS_M2 msg: 1613175317.39 epoch sec; seqNo:10898 2021-02-13T00:15:17Z- AHRS_M2 msg: 0.72 phi; -0.34 theta; -0.14 psi 2021-02-13T00:15:17Z- handleDepth()-Depth_Keller msg: 1613175317392 epoch sec; seqNo:10991; depth 8.32 2021-02-13T00:15:17Z- handleDvl() - RDI_Pathfinder msg: 1613175317.40 epoch sec; seqNo:5430 2021-02-13T00:15:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 103.94 , 0.00 , 120.16 2021-02-13T00:15:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.12 , 0.07 2021-02-13T00:15:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175317.43 epoch sec; seqNo:10991; 4058722.34 north; 592231.50 east 2021-02-13T00:15:17Z- AHRS_M2 msg: 1613175317.80 epoch sec; seqNo:10899 2021-02-13T00:15:17Z- AHRS_M2 msg: 0.72 phi; -0.34 theta; -0.14 psi 2021-02-13T00:15:17Z- handleDepth()-Depth_Keller msg: 1613175317894 epoch sec; seqNo:10992; depth 8.46 2021-02-13T00:15:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175317.91 epoch sec; seqNo:10992; 4058722.34 north; 592231.50 east 2021-02-13T00:15:18Z- AHRS_M2 msg: 1613175318.20 epoch sec; seqNo:10900 2021-02-13T00:15:18Z- AHRS_M2 msg: 0.71 phi; -0.34 theta; -0.14 psi 2021-02-13T00:15:18Z- handleDepth()-Depth_Keller msg: 1613175318202 epoch sec; seqNo:10993; depth 8.62 2021-02-13T00:15:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175318.21 epoch sec; seqNo:10993; 4058722.71 north; 592231.49 east 2021-02-13T00:15:18Z- AHRS_M2 msg: 1613175318.60 epoch sec; seqNo:10901 2021-02-13T00:15:18Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.14 psi 2021-02-13T00:15:18Z- handleDepth()-Depth_Keller msg: 1613175318610 epoch sec; seqNo:10994; depth 8.71 2021-02-13T00:15:18Z- handleDvl() - RDI_Pathfinder msg: 1613175318.62 epoch sec; seqNo:5431 2021-02-13T00:15:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 103.67 , 79.70 , 120.03 2021-02-13T00:15:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.03 , 0.17 2021-02-13T00:15:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175318.63 epoch sec; seqNo:10994; 4058722.71 north; 592231.49 east 2021-02-13T00:15:19Z- AHRS_M2 msg: 1613175319.00 epoch sec; seqNo:10902 2021-02-13T00:15:19Z- AHRS_M2 msg: 0.71 phi; -0.32 theta; -0.14 psi 2021-02-13T00:15:19Z- handleDepth()-Depth_Keller msg: 1613175319013 epoch sec; seqNo:10995; depth 8.86 2021-02-13T00:15:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175319.03 epoch sec; seqNo:10995; 4058723.09 north; 592231.49 east 2021-02-13T00:15:19Z- AHRS_M2 msg: 1613175319.41 epoch sec; seqNo:10903 2021-02-13T00:15:19Z- AHRS_M2 msg: 0.71 phi; -0.31 theta; -0.15 psi 2021-02-13T00:15:19Z- handleDepth()-Depth_Keller msg: 1613175319412 epoch sec; seqNo:10996; depth 8.98 2021-02-13T00:15:19Z- handleDvl() - RDI_Pathfinder msg: 1613175319.42 epoch sec; seqNo:5432 2021-02-13T00:15:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 102.45 , 77.66 , 121.38 2021-02-13T00:15:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.07 , 0.16 2021-02-13T00:15:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175319.43 epoch sec; seqNo:10996; 4058723.11 north; 592232.71 east 2021-02-13T00:15:19Z- AHRS_M2 msg: 1613175319.81 epoch sec; seqNo:10904 2021-02-13T00:15:19Z- AHRS_M2 msg: 0.71 phi; -0.31 theta; -0.15 psi 2021-02-13T00:15:19Z- handleDepth()-Depth_Keller msg: 1613175319818 epoch sec; seqNo:10997; depth 9.10 2021-02-13T00:15:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:19Z- handleDvl() - RDI_Pathfinder msg: 1613175319.82 epoch sec; seqNo:5433 2021-02-13T00:15:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.07 , 0.16 2021-02-13T00:15:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175319.83 epoch sec; seqNo:10997; 4058723.48 north; 592232.71 east 2021-02-13T00:15:20Z- AHRS_M2 msg: 1613175320.22 epoch sec; seqNo:10905 2021-02-13T00:15:20Z- AHRS_M2 msg: 0.72 phi; -0.30 theta; -0.16 psi 2021-02-13T00:15:20Z- handleDepth()-Depth_Keller msg: 1613175320221 epoch sec; seqNo:10998; depth 9.23 2021-02-13T00:15:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175320.24 epoch sec; seqNo:10998; 4058723.48 north; 592232.71 east 2021-02-13T00:15:20Z- AHRS_M2 msg: 1613175320.62 epoch sec; seqNo:10906 2021-02-13T00:15:20Z- AHRS_M2 msg: 0.72 phi; -0.30 theta; -0.16 psi 2021-02-13T00:15:20Z- handleDepth()-Depth_Keller msg: 1613175320629 epoch sec; seqNo:10999; depth 9.34 2021-02-13T00:15:20Z- handleDvl() - RDI_Pathfinder msg: 1613175320.65 epoch sec; seqNo:5434 2021-02-13T00:15:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 104.00 , 81.01 , 116.18 2021-02-13T00:15:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.13 , 0.04 2021-02-13T00:15:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175320.66 epoch sec; seqNo:10999; 4058723.86 north; 592232.70 east 2021-02-13T00:15:21Z- AHRS_M2 msg: 1613175321.02 epoch sec; seqNo:10907 2021-02-13T00:15:21Z- AHRS_M2 msg: 0.72 phi; -0.29 theta; -0.16 psi 2021-02-13T00:15:21Z- handleDepth()-Depth_Keller msg: 1613175321033 epoch sec; seqNo:11000; depth 9.47 2021-02-13T00:15:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:21Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:21Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:21Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:21Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:21Z- handleDvl() - RDI_Pathfinder msg: 1613175321.04 epoch sec; seqNo:5435 2021-02-13T00:15:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.13 , 0.04 2021-02-13T00:15:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175321.05 epoch sec; seqNo:11000; 4058724.24 north; 592232.70 east 2021-02-13T00:15:21Z- AHRS_M2 msg: 1613175321.43 epoch sec; seqNo:10908 2021-02-13T00:15:21Z- AHRS_M2 msg: 0.73 phi; -0.29 theta; -0.16 psi 2021-02-13T00:15:21Z- handleDepth()-Depth_Keller msg: 1613175321433 epoch sec; seqNo:11001; depth 9.59 2021-02-13T00:15:21Z- handleDvl() - RDI_Pathfinder msg: 1613175321.44 epoch sec; seqNo:5436 2021-02-13T00:15:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 103.80 , 0.00 , 113.27 2021-02-13T00:15:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.06 , -0.07 2021-02-13T00:15:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175321.46 epoch sec; seqNo:11001; 4058724.26 north; 592233.92 east 2021-02-13T00:15:21Z- LcmTrn::handleMessages() - handling 47 messages took 0.69 ms and 133 calls... 2021-02-13T00:15:21Z- waiting for 1613175319.19, time is 1613175321.46 2021-02-13T00:15:21Z- LcmTrn::updateTrn() equating pose 1613175321.46 and meas times 1613175321.46 2021-02-13T00:15:21Z- LcmTrn::updateTrn() >>>> pose time 1613175321.46, meas time 1613175321.46 <<<< 2021-02-13T00:15:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175321.46 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:15:21Z- TerrainNav::motionUpdate() - currEstimate = (4058724, 592233.92, 9.59) 2021-02-13T00:15:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:15:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:15:21Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175321.46 sec.; No relevant navigation data available 2021-02-13T00:15:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:15:21Z- TNavBF::mmse selected from filter -1 2021-02-13T00:15:21Z- variances([0], [2], and psi_berg) = 711.831933 833.004569 0.000000 2021-02-13T00:15:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:15:21Z- LcmTrn::publishEstimates() - MLE : 4058723.76 592283.42 9.59 2021-02-13T00:15:21Z- LcmTrn::publishEstimates() - MMSE : 4058724.26 592233.92 9.59 2021-02-13T00:15:21Z- LcmTrn::publishEstimates() - COVAR: 711.83 833.00 1.59 0.00 2021-02-13T00:15:21Z- AHRS_M2 msg: 1613175321.83 epoch sec; seqNo:10909 2021-02-13T00:15:21Z- AHRS_M2 msg: 0.73 phi; -0.29 theta; -0.16 psi 2021-02-13T00:15:21Z- handleDepth()-Depth_Keller msg: 1613175321836 epoch sec; seqNo:11002; depth 9.70 2021-02-13T00:15:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175321.85 epoch sec; seqNo:11002; 4058724.63 north; 592233.92 east 2021-02-13T00:15:22Z- AHRS_M2 msg: 1613175322.24 epoch sec; seqNo:10910 2021-02-13T00:15:22Z- AHRS_M2 msg: 0.74 phi; -0.29 theta; -0.17 psi 2021-02-13T00:15:22Z- handleDepth()-Depth_Keller msg: 1613175322240 epoch sec; seqNo:11003; depth 9.82 2021-02-13T00:15:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:22Z- handleDvl() - RDI_Pathfinder msg: 1613175322.24 epoch sec; seqNo:5437 2021-02-13T00:15:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.06 , -0.07 2021-02-13T00:15:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175322.25 epoch sec; seqNo:11003; 4058725.01 north; 592233.91 east 2021-02-13T00:15:22Z- AHRS_M2 msg: 1613175322.64 epoch sec; seqNo:10911 2021-02-13T00:15:22Z- AHRS_M2 msg: 0.74 phi; -0.29 theta; -0.17 psi 2021-02-13T00:15:22Z- handleDepth()-Depth_Keller msg: 1613175322644 epoch sec; seqNo:11004; depth 9.95 2021-02-13T00:15:22Z- handleDvl() - RDI_Pathfinder msg: 1613175322.65 epoch sec; seqNo:5438 2021-02-13T00:15:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.15 , 101.03 , 79.39 , 115.90 2021-02-13T00:15:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.08 2021-02-13T00:15:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175322.67 epoch sec; seqNo:11004; 4058725.39 north; 592233.91 east 2021-02-13T00:15:23Z- AHRS_M2 msg: 1613175323.05 epoch sec; seqNo:10912 2021-02-13T00:15:23Z- AHRS_M2 msg: 0.74 phi; -0.29 theta; -0.17 psi 2021-02-13T00:15:23Z- handleDepth()-Depth_Keller msg: 1613175323049 epoch sec; seqNo:11005; depth 10.05 2021-02-13T00:15:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175323.06 epoch sec; seqNo:11005; 4058725.41 north; 592235.13 east 2021-02-13T00:15:23Z- AHRS_M2 msg: 1613175323.45 epoch sec; seqNo:10913 2021-02-13T00:15:23Z- AHRS_M2 msg: 0.74 phi; -0.29 theta; -0.16 psi 2021-02-13T00:15:23Z- handleDepth()-Depth_Keller msg: 1613175323452 epoch sec; seqNo:11006; depth 10.18 2021-02-13T00:15:23Z- handleDvl() - RDI_Pathfinder msg: 1613175323.46 epoch sec; seqNo:5439 2021-02-13T00:15:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 102.85 , 78.07 , 110.97 2021-02-13T00:15:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.02 , -0.03 2021-02-13T00:15:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175323.49 epoch sec; seqNo:11006; 4058725.78 north; 592235.12 east 2021-02-13T00:15:23Z- AHRS_M2 msg: 1613175323.85 epoch sec; seqNo:10914 2021-02-13T00:15:23Z- AHRS_M2 msg: 0.74 phi; -0.29 theta; -0.16 psi 2021-02-13T00:15:23Z- handleDepth()-Depth_Keller msg: 1613175323857 epoch sec; seqNo:11007; depth 10.29 2021-02-13T00:15:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175323.87 epoch sec; seqNo:11007; 4058725.78 north; 592235.12 east 2021-02-13T00:15:24Z- AHRS_M2 msg: 1613175324.26 epoch sec; seqNo:10915 2021-02-13T00:15:24Z- AHRS_M2 msg: 0.73 phi; -0.29 theta; -0.17 psi 2021-02-13T00:15:24Z- handleDepth()-Depth_Keller msg: 1613175324262 epoch sec; seqNo:11008; depth 10.40 2021-02-13T00:15:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175324.27 epoch sec; seqNo:11008; 4058726.16 north; 592235.12 east 2021-02-13T00:15:24Z- AHRS_M2 msg: 1613175324.66 epoch sec; seqNo:10916 2021-02-13T00:15:24Z- AHRS_M2 msg: 0.73 phi; -0.30 theta; -0.17 psi 2021-02-13T00:15:24Z- handleDepth()-Depth_Keller msg: 1613175324669 epoch sec; seqNo:11009; depth 10.50 2021-02-13T00:15:24Z- handleDvl() - RDI_Pathfinder msg: 1613175324.69 epoch sec; seqNo:5440 2021-02-13T00:15:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 101.57 , 0.00 , 108.33 2021-02-13T00:15:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.10 , -0.00 2021-02-13T00:15:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175324.70 epoch sec; seqNo:11009; 4058726.54 north; 592235.12 east 2021-02-13T00:15:25Z- AHRS_M2 msg: 1613175325.06 epoch sec; seqNo:10917 2021-02-13T00:15:25Z- AHRS_M2 msg: 0.73 phi; -0.30 theta; -0.18 psi 2021-02-13T00:15:25Z- handleDepth()-Depth_Keller msg: 1613175325069 epoch sec; seqNo:11010; depth 10.62 2021-02-13T00:15:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175325.08 epoch sec; seqNo:11010; 4058726.54 north; 592235.12 east 2021-02-13T00:15:25Z- AHRS_M2 msg: 1613175325.47 epoch sec; seqNo:10918 2021-02-13T00:15:25Z- AHRS_M2 msg: 0.72 phi; -0.30 theta; -0.18 psi 2021-02-13T00:15:25Z- handleDepth()-Depth_Keller msg: 1613175325472 epoch sec; seqNo:11011; depth 10.71 2021-02-13T00:15:25Z- handleDvl() - RDI_Pathfinder msg: 1613175325.48 epoch sec; seqNo:5441 2021-02-13T00:15:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.29 , 79.59 , 113.40 2021-02-13T00:15:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.14 , -0.00 2021-02-13T00:15:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175325.49 epoch sec; seqNo:11011; 4058726.93 north; 592236.33 east 2021-02-13T00:15:25Z- AHRS_M2 msg: 1613175325.88 epoch sec; seqNo:10919 2021-02-13T00:15:25Z- AHRS_M2 msg: 0.72 phi; -0.30 theta; -0.19 psi 2021-02-13T00:15:25Z- handleDepth()-Depth_Keller msg: 1613175325882 epoch sec; seqNo:11012; depth 10.83 2021-02-13T00:15:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:25Z- handleDvl() - RDI_Pathfinder msg: 1613175325.88 epoch sec; seqNo:5442 2021-02-13T00:15:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.14 , -0.00 2021-02-13T00:15:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175325.89 epoch sec; seqNo:11012; 4058726.93 north; 592236.33 east 2021-02-13T00:15:26Z- AHRS_M2 msg: 1613175326.28 epoch sec; seqNo:10920 2021-02-13T00:15:26Z- AHRS_M2 msg: 0.73 phi; -0.30 theta; -0.19 psi 2021-02-13T00:15:26Z- handleDepth()-Depth_Keller msg: 1613175326280 epoch sec; seqNo:11013; depth 10.94 2021-02-13T00:15:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175326.30 epoch sec; seqNo:11013; 4058727.31 north; 592236.33 east 2021-02-13T00:15:26Z- LcmTrn::handleMessages() - handling 42 messages took 0.67 ms and 129 calls... 2021-02-13T00:15:26Z- waiting for 1613175324.46, time is 1613175326.30 2021-02-13T00:15:26Z- LcmTrn::updateTrn() >>>> pose time 1613175326.30, meas time 1613175325.88 <<<< 2021-02-13T00:15:26Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175325.88, ping # 0. 2021-02-13T00:15:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175325.88 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:15:26Z- TerrainNav::motionUpdate() - currEstimate = (4058727, 592236.33, 10.94) 2021-02-13T00:15:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:15:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:15:26Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:15:26Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:15:26Z- TNavBF::Projecting Measurements 2021-02-13T00:15:26Z- TNavFilter::Measurement from time = 1613175325.88 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:15:26Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:15:26Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:15:26Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:15:26Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:15:26Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:15:26Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:15:26Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:15:26Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 711.8 EastVariance: 833.0 DepthVariance: 1.8 this->DepthBiasCov: 1.8 2021-02-13T00:15:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:15:26Z- TNavBF::mmse selected from filter -1 2021-02-13T00:15:26Z- variances([0], [2], and psi_berg) = 711.756157 833.004524 0.000000 2021-02-13T00:15:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:15:26Z- LcmTrn::publishEstimates() - MLE : 4058726.81 592285.83 10.94 2021-02-13T00:15:26Z- LcmTrn::publishEstimates() - MMSE : 4058727.31 592236.33 10.94 2021-02-13T00:15:26Z- LcmTrn::publishEstimates() - COVAR: 711.76 833.00 1.78 0.00 2021-02-13T00:15:26Z- AHRS_M2 msg: 1613175326.68 epoch sec; seqNo:10921 2021-02-13T00:15:26Z- AHRS_M2 msg: 0.73 phi; -0.29 theta; -0.19 psi 2021-02-13T00:15:26Z- handleDepth()-Depth_Keller msg: 1613175326684 epoch sec; seqNo:11014; depth 11.05 2021-02-13T00:15:26Z- handleDvl() - RDI_Pathfinder msg: 1613175326.70 epoch sec; seqNo:5443 2021-02-13T00:15:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.94 , 105.22 , 79.53 , 116.04 2021-02-13T00:15:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.13 , -0.04 2021-02-13T00:15:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175326.73 epoch sec; seqNo:11014; 4058727.69 north; 592236.32 east 2021-02-13T00:15:27Z- AHRS_M2 msg: 1613175327.08 epoch sec; seqNo:10922 2021-02-13T00:15:27Z- AHRS_M2 msg: 0.73 phi; -0.29 theta; -0.19 psi 2021-02-13T00:15:27Z- handleDepth()-Depth_Keller msg: 1613175327089 epoch sec; seqNo:11015; depth 11.18 2021-02-13T00:15:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:27Z- handleDvl() - RDI_Pathfinder msg: 1613175327.09 epoch sec; seqNo:5444 2021-02-13T00:15:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.13 , -0.04 2021-02-13T00:15:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175327.10 epoch sec; seqNo:11015; 4058727.70 north; 592237.55 east 2021-02-13T00:15:27Z- AHRS_M2 msg: 1613175327.49 epoch sec; seqNo:10923 2021-02-13T00:15:27Z- AHRS_M2 msg: 0.73 phi; -0.29 theta; -0.18 psi 2021-02-13T00:15:27Z- handleDepth()-Depth_Keller msg: 1613175327494 epoch sec; seqNo:11016; depth 11.26 2021-02-13T00:15:27Z- handleDvl() - RDI_Pathfinder msg: 1613175327.51 epoch sec; seqNo:5445 2021-02-13T00:15:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.07 , 79.73 , 113.54 2021-02-13T00:15:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.03 , -0.08 2021-02-13T00:15:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175327.52 epoch sec; seqNo:11016; 4058728.08 north; 592237.54 east 2021-02-13T00:15:27Z- AHRS_M2 msg: 1613175327.89 epoch sec; seqNo:10924 2021-02-13T00:15:27Z- AHRS_M2 msg: 0.73 phi; -0.29 theta; -0.18 psi 2021-02-13T00:15:27Z- handleDepth()-Depth_Keller msg: 1613175327905 epoch sec; seqNo:11017; depth 11.38 2021-02-13T00:15:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175327.92 epoch sec; seqNo:11017; 4058728.46 north; 592237.54 east 2021-02-13T00:15:28Z- AHRS_M2 msg: 1613175328.30 epoch sec; seqNo:10925 2021-02-13T00:15:28Z- AHRS_M2 msg: 0.71 phi; -0.28 theta; -0.21 psi 2021-02-13T00:15:28Z- handleDepth()-Depth_Keller msg: 1613175328306 epoch sec; seqNo:11018; depth 11.50 2021-02-13T00:15:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:28Z- handleDvl() - RDI_Pathfinder msg: 1613175328.31 epoch sec; seqNo:5446 2021-02-13T00:15:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.03 , -0.08 2021-02-13T00:15:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175328.32 epoch sec; seqNo:11018; 4058728.46 north; 592237.54 east 2021-02-13T00:15:28Z- AHRS_M2 msg: 1613175328.71 epoch sec; seqNo:10926 2021-02-13T00:15:28Z- AHRS_M2 msg: 0.71 phi; -0.28 theta; -0.22 psi 2021-02-13T00:15:28Z- handleDepth()-Depth_Keller msg: 1613175328715 epoch sec; seqNo:11019; depth 11.63 2021-02-13T00:15:28Z- handleDvl() - RDI_Pathfinder msg: 1613175328.73 epoch sec; seqNo:5447 2021-02-13T00:15:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.58 , 105.22 , 78.48 , 111.98 2021-02-13T00:15:28Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.09 , -0.09 2021-02-13T00:15:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175328.74 epoch sec; seqNo:11019; 4058728.84 north; 592237.53 east 2021-02-13T00:15:29Z- AHRS_M2 msg: 1613175329.10 epoch sec; seqNo:10927 2021-02-13T00:15:29Z- AHRS_M2 msg: 0.70 phi; -0.28 theta; -0.22 psi 2021-02-13T00:15:29Z- handleDepth()-Depth_Keller msg: 1613175329108 epoch sec; seqNo:11020; depth 11.74 2021-02-13T00:15:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175329.13 epoch sec; seqNo:11020; 4058729.23 north; 592238.75 east 2021-02-13T00:15:29Z- AHRS_M2 msg: 1613175329.51 epoch sec; seqNo:10928 2021-02-13T00:15:29Z- AHRS_M2 msg: 0.70 phi; -0.28 theta; -0.22 psi 2021-02-13T00:15:29Z- handleDepth()-Depth_Keller msg: 1613175329512 epoch sec; seqNo:11021; depth 11.84 2021-02-13T00:15:29Z- handleDvl() - RDI_Pathfinder msg: 1613175329.52 epoch sec; seqNo:5448 2021-02-13T00:15:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.64 , 106.98 , 78.07 , 111.04 2021-02-13T00:15:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.11 , -0.06 2021-02-13T00:15:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175329.55 epoch sec; seqNo:11021; 4058729.61 north; 592238.75 east 2021-02-13T00:15:29Z- AHRS_M2 msg: 1613175329.91 epoch sec; seqNo:10929 2021-02-13T00:15:29Z- AHRS_M2 msg: 0.70 phi; -0.28 theta; -0.21 psi 2021-02-13T00:15:29Z- handleDepth()-Depth_Keller msg: 1613175329916 epoch sec; seqNo:11022; depth 11.95 2021-02-13T00:15:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175329.93 epoch sec; seqNo:11022; 4058729.99 north; 592238.74 east 2021-02-13T00:15:30Z- AHRS_M2 msg: 1613175330.32 epoch sec; seqNo:10930 2021-02-13T00:15:30Z- AHRS_M2 msg: 0.70 phi; -0.28 theta; -0.20 psi 2021-02-13T00:15:30Z- handleDepth()-Depth_Keller msg: 1613175330322 epoch sec; seqNo:11023; depth 12.06 2021-02-13T00:15:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175330.33 epoch sec; seqNo:11023; 4058730.37 north; 592238.74 east 2021-02-13T00:15:30Z- AHRS_M2 msg: 1613175330.72 epoch sec; seqNo:10931 2021-02-13T00:15:30Z- AHRS_M2 msg: 0.70 phi; -0.28 theta; -0.19 psi 2021-02-13T00:15:30Z- handleDepth()-Depth_Keller msg: 1613175330739 epoch sec; seqNo:11024; depth 12.18 2021-02-13T00:15:30Z- handleDvl() - RDI_Pathfinder msg: 1613175330.75 epoch sec; seqNo:5449 2021-02-13T00:15:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.64 , 104.27 , 78.58 , 108.33 2021-02-13T00:15:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.12 , -0.06 2021-02-13T00:15:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175330.76 epoch sec; seqNo:11024; 4058730.75 north; 592238.73 east 2021-02-13T00:15:31Z- AHRS_M2 msg: 1613175331.13 epoch sec; seqNo:10932 2021-02-13T00:15:31Z- AHRS_M2 msg: 0.70 phi; -0.28 theta; -0.19 psi 2021-02-13T00:15:31Z- handleDepth()-Depth_Keller msg: 1613175331137 epoch sec; seqNo:11025; depth 12.28 2021-02-13T00:15:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175331.15 epoch sec; seqNo:11025; 4058730.75 north; 592238.73 east 2021-02-13T00:15:31Z- AHRS_M2 msg: 1613175331.53 epoch sec; seqNo:10933 2021-02-13T00:15:31Z- AHRS_M2 msg: 0.70 phi; -0.28 theta; -0.19 psi 2021-02-13T00:15:31Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-13T00:15:31Z- waiting for 1613175329.30, time is 1613175331.53 2021-02-13T00:15:31Z- LcmTrn::updateTrn() >>>> pose time 1613175331.53, meas time 1613175330.75 <<<< 2021-02-13T00:15:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175330.75 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:15:31Z- TerrainNav::motionUpdate() - currEstimate = (4058731, 592238.73, 12.28) 2021-02-13T00:15:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:15:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:15:31Z- TNavBF::Projecting Measurements 2021-02-13T00:15:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:15:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:15:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:15:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:15:31Z- TNavFilter::4 good beams. 2021-02-13T00:15:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:15:31Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:15:31Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:15:31Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:15:31Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:15:31Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:15:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:15:31Z- TNavBF::mmse selected from filter -1 2021-02-13T00:15:31Z- TerrainMapOctree: (vehN, vehE ) = (4058730.24, 592238.38). 2021-02-13T00:15:31Z- TerrainMapOctree: Min Distance = 3224.26. 2021-02-13T00:15:31Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:15:31Z- There is no specific good beam that all particles have in common. 2021-02-13T00:15:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:15:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.97 K: 0.134 ExpectedMeasDiff: nan currMeas.covariance: 10.93 2021-02-13T00:15:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:15:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.70 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 17.40 2021-02-13T00:15:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:15:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.56 K: 0.119 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-13T00:15:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:15:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 18.78 2021-02-13T00:15:31Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:15:31Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:15:31Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:15:31Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:15:31Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:15:31Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:15:31Z- Filter 0 No beams used in subcloud update 2021-02-13T00:15:31Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:15:31Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:15:31Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 711.7 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:15:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175330.75 sec. 2021-02-13T00:15:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:15:31Z- TNavBF::mmse selected from filter -1 2021-02-13T00:15:31Z- variances([0], [2], and psi_berg) = 711.673782 833.004474 0.000000 2021-02-13T00:15:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:15:31Z- LcmTrn::publishEstimates() - MLE : 4058730.25 592288.23 12.28 2021-02-13T00:15:31Z- LcmTrn::publishEstimates() - MMSE : 4058730.75 592238.73 12.28 2021-02-13T00:15:31Z- LcmTrn::publishEstimates() - COVAR: 711.67 833.00 1.32 0.00 2021-02-13T00:15:31Z- handleDepth()-Depth_Keller msg: 1613175331534 epoch sec; seqNo:11026; depth 12.38 2021-02-13T00:15:31Z- handleDvl() - RDI_Pathfinder msg: 1613175331.55 epoch sec; seqNo:5450 2021-02-13T00:15:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.70 , 100.29 , 78.65 , 108.40 2021-02-13T00:15:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , -0.06 2021-02-13T00:15:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175331.56 epoch sec; seqNo:11026; 4058731.14 north; 592239.95 east 2021-02-13T00:15:31Z- AHRS_M2 msg: 1613175331.93 epoch sec; seqNo:10934 2021-02-13T00:15:31Z- AHRS_M2 msg: 0.71 phi; -0.28 theta; -0.18 psi 2021-02-13T00:15:31Z- handleDepth()-Depth_Keller msg: 1613175331936 epoch sec; seqNo:11027; depth 12.49 2021-02-13T00:15:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:31Z- handleDvl() - RDI_Pathfinder msg: 1613175331.94 epoch sec; seqNo:5451 2021-02-13T00:15:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , -0.06 2021-02-13T00:15:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175331.96 epoch sec; seqNo:11027; 4058731.52 north; 592239.95 east 2021-02-13T00:15:32Z- AHRS_M2 msg: 1613175332.34 epoch sec; seqNo:10935 2021-02-13T00:15:32Z- AHRS_M2 msg: 0.71 phi; -0.29 theta; -0.17 psi 2021-02-13T00:15:32Z- handleDepth()-Depth_Keller msg: 1613175332345 epoch sec; seqNo:11028; depth 12.62 2021-02-13T00:15:32Z- handleDvl() - RDI_Pathfinder msg: 1613175332.36 epoch sec; seqNo:5452 2021-02-13T00:15:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.49 , 101.77 , 76.43 , 108.53 2021-02-13T00:15:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.09 , -0.04 2021-02-13T00:15:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175332.37 epoch sec; seqNo:11028; 4058731.90 north; 592239.94 east 2021-02-13T00:15:32Z- AHRS_M2 msg: 1613175332.74 epoch sec; seqNo:10936 2021-02-13T00:15:32Z- AHRS_M2 msg: 0.71 phi; -0.29 theta; -0.16 psi 2021-02-13T00:15:32Z- handleDepth()-Depth_Keller msg: 1613175332749 epoch sec; seqNo:11029; depth 12.71 2021-02-13T00:15:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175332.76 epoch sec; seqNo:11029; 4058731.90 north; 592239.94 east 2021-02-13T00:15:33Z- AHRS_M2 msg: 1613175333.15 epoch sec; seqNo:10937 2021-02-13T00:15:33Z- AHRS_M2 msg: 0.71 phi; -0.29 theta; -0.16 psi 2021-02-13T00:15:33Z- handleDepth()-Depth_Keller msg: 1613175333157 epoch sec; seqNo:11030; depth 12.81 2021-02-13T00:15:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:33Z- handleDvl() - RDI_Pathfinder msg: 1613175333.16 epoch sec; seqNo:5453 2021-02-13T00:15:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.09 , -0.04 2021-02-13T00:15:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175333.17 epoch sec; seqNo:11030; 4058732.28 north; 592239.94 east 2021-02-13T00:15:33Z- AHRS_M2 msg: 1613175333.55 epoch sec; seqNo:10938 2021-02-13T00:15:33Z- AHRS_M2 msg: 0.71 phi; -0.29 theta; -0.16 psi 2021-02-13T00:15:33Z- handleDepth()-Depth_Keller msg: 1613175333558 epoch sec; seqNo:11031; depth 12.93 2021-02-13T00:15:33Z- handleDvl() - RDI_Pathfinder msg: 1613175333.57 epoch sec; seqNo:5454 2021-02-13T00:15:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 96.36 , 78.78 , 107.18 2021-02-13T00:15:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.09 , 0.00 2021-02-13T00:15:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175333.58 epoch sec; seqNo:11031; 4058732.66 north; 592239.93 east 2021-02-13T00:15:33Z- AHRS_M2 msg: 1613175333.95 epoch sec; seqNo:10939 2021-02-13T00:15:33Z- AHRS_M2 msg: 0.70 phi; -0.29 theta; -0.17 psi 2021-02-13T00:15:33Z- handleDepth()-Depth_Keller msg: 1613175333957 epoch sec; seqNo:11032; depth 13.03 2021-02-13T00:15:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175333.97 epoch sec; seqNo:11032; 4058733.03 north; 592239.93 east 2021-02-13T00:15:34Z- AHRS_M2 msg: 1613175334.36 epoch sec; seqNo:10940 2021-02-13T00:15:34Z- AHRS_M2 msg: 0.70 phi; -0.29 theta; -0.18 psi 2021-02-13T00:15:34Z- handleDepth()-Depth_Keller msg: 1613175334366 epoch sec; seqNo:11033; depth 13.13 2021-02-13T00:15:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:34Z- handleDvl() - RDI_Pathfinder msg: 1613175334.37 epoch sec; seqNo:5455 2021-02-13T00:15:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.09 , 0.00 2021-02-13T00:15:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175334.38 epoch sec; seqNo:11033; 4058733.05 north; 592241.15 east 2021-02-13T00:15:34Z- AHRS_M2 msg: 1613175334.77 epoch sec; seqNo:10941 2021-02-13T00:15:34Z- AHRS_M2 msg: 0.70 phi; -0.29 theta; -0.19 psi 2021-02-13T00:15:34Z- handleDepth()-Depth_Keller msg: 1613175334777 epoch sec; seqNo:11034; depth 13.25 2021-02-13T00:15:34Z- handleDvl() - RDI_Pathfinder msg: 1613175334.79 epoch sec; seqNo:5456 2021-02-13T00:15:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 100.08 , 78.17 , 107.39 2021-02-13T00:15:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.08 , 0.04 2021-02-13T00:15:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175334.80 epoch sec; seqNo:11034; 4058733.43 north; 592241.15 east 2021-02-13T00:15:35Z- AHRS_M2 msg: 1613175335.16 epoch sec; seqNo:10942 2021-02-13T00:15:35Z- AHRS_M2 msg: 0.70 phi; -0.29 theta; -0.19 psi 2021-02-13T00:15:35Z- handleDepth()-Depth_Keller msg: 1613175335173 epoch sec; seqNo:11035; depth 13.34 2021-02-13T00:15:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175335.19 epoch sec; seqNo:11035; 4058733.81 north; 592241.14 east 2021-02-13T00:15:35Z- AHRS_M2 msg: 1613175335.57 epoch sec; seqNo:10943 2021-02-13T00:15:35Z- AHRS_M2 msg: 0.69 phi; -0.29 theta; -0.19 psi 2021-02-13T00:15:35Z- handleDepth()-Depth_Keller msg: 1613175335572 epoch sec; seqNo:11036; depth 13.46 2021-02-13T00:15:35Z- handleDvl() - RDI_Pathfinder msg: 1613175335.58 epoch sec; seqNo:5457 2021-02-13T00:15:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 100.42 , 76.43 , 109.89 2021-02-13T00:15:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.09 , 0.08 2021-02-13T00:15:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175335.61 epoch sec; seqNo:11036; 4058733.81 north; 592241.14 east 2021-02-13T00:15:35Z- AHRS_M2 msg: 1613175335.97 epoch sec; seqNo:10944 2021-02-13T00:15:35Z- AHRS_M2 msg: 0.69 phi; -0.30 theta; -0.19 psi 2021-02-13T00:15:35Z- handleDepth()-Depth_Keller msg: 1613175335979 epoch sec; seqNo:11037; depth 13.56 2021-02-13T00:15:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175335.99 epoch sec; seqNo:11037; 4058734.18 north; 592241.14 east 2021-02-13T00:15:36Z- AHRS_M2 msg: 1613175336.39 epoch sec; seqNo:10945 2021-02-13T00:15:36Z- AHRS_M2 msg: 0.69 phi; -0.30 theta; -0.19 psi 2021-02-13T00:15:36Z- handleDepth()-Depth_Keller msg: 1613175336397 epoch sec; seqNo:11038; depth 13.66 2021-02-13T00:15:36Z- handleDvl() - RDI_Pathfinder msg: 1613175336.41 epoch sec; seqNo:5458 2021-02-13T00:15:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 100.42 , 78.78 , 112.59 2021-02-13T00:15:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.02 , 0.12 2021-02-13T00:15:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175336.42 epoch sec; seqNo:11038; 4058734.56 north; 592241.13 east 2021-02-13T00:15:36Z- LcmTrn::handleMessages() - handling 47 messages took 0.60 ms and 135 calls... 2021-02-13T00:15:36Z- waiting for 1613175334.53, time is 1613175336.42 2021-02-13T00:15:36Z- LcmTrn::updateTrn() equating pose 1613175336.42 and meas times 1613175336.42 2021-02-13T00:15:36Z- LcmTrn::updateTrn() >>>> pose time 1613175336.42, meas time 1613175336.42 <<<< 2021-02-13T00:15:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175336.42 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:15:36Z- TerrainNav::motionUpdate() - currEstimate = (4058735, 592241.13, 13.66) 2021-02-13T00:15:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:15:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:15:36Z- TNavBF::Projecting Measurements 2021-02-13T00:15:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:15:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:15:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:15:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:15:36Z- TNavFilter::4 good beams. 2021-02-13T00:15:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:15:36Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:15:36Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:15:36Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:15:36Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:15:36Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:15:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:15:36Z- TNavBF::mmse selected from filter -1 2021-02-13T00:15:36Z- TerrainMapOctree: (vehN, vehE ) = (4058734.56, 592241.13). 2021-02-13T00:15:36Z- TerrainMapOctree: Min Distance = 3219.16. 2021-02-13T00:15:36Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:15:36Z- There is no specific good beam that all particles have in common. 2021-02-13T00:15:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:15:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.55 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 10.98 2021-02-13T00:15:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:15:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 16.13 2021-02-13T00:15:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:15:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 9.93 2021-02-13T00:15:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:15:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 20.28 2021-02-13T00:15:36Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:15:36Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:15:36Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:15:36Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:15:36Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:15:36Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:15:36Z- Filter 0 No beams used in subcloud update 2021-02-13T00:15:36Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:15:36Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:15:36Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 711.6 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:15:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175336.42 sec. 2021-02-13T00:15:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:15:36Z- TNavBF::mmse selected from filter -1 2021-02-13T00:15:36Z- variances([0], [2], and psi_berg) = 711.608559 833.004436 0.000000 2021-02-13T00:15:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:15:36Z- LcmTrn::publishEstimates() - MLE : 4058734.06 592290.63 13.66 2021-02-13T00:15:36Z- LcmTrn::publishEstimates() - MMSE : 4058734.56 592241.13 13.66 2021-02-13T00:15:36Z- LcmTrn::publishEstimates() - COVAR: 711.61 833.00 1.10 0.00 2021-02-13T00:15:36Z- AHRS_M2 msg: 1613175336.78 epoch sec; seqNo:10946 2021-02-13T00:15:36Z- AHRS_M2 msg: 0.69 phi; -0.30 theta; -0.19 psi 2021-02-13T00:15:36Z- handleDepth()-Depth_Keller msg: 1613175336783 epoch sec; seqNo:11039; depth 13.80 2021-02-13T00:15:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:36Z- handleDvl() - RDI_Pathfinder msg: 1613175336.79 epoch sec; seqNo:5459 2021-02-13T00:15:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.02 , 0.12 2021-02-13T00:15:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175336.81 epoch sec; seqNo:11039; 4058734.58 north; 592242.35 east 2021-02-13T00:15:37Z- AHRS_M2 msg: 1613175337.18 epoch sec; seqNo:10947 2021-02-13T00:15:37Z- AHRS_M2 msg: 0.69 phi; -0.30 theta; -0.19 psi 2021-02-13T00:15:37Z- handleDepth()-Depth_Keller msg: 1613175337189 epoch sec; seqNo:11040; depth 13.89 2021-02-13T00:15:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175337.20 epoch sec; seqNo:11040; 4058734.96 north; 592242.35 east 2021-02-13T00:15:37Z- AHRS_M2 msg: 1613175337.59 epoch sec; seqNo:10948 2021-02-13T00:15:37Z- AHRS_M2 msg: 0.69 phi; -0.31 theta; -0.19 psi 2021-02-13T00:15:37Z- handleDepth()-Depth_Keller msg: 1613175337594 epoch sec; seqNo:11041; depth 14.00 2021-02-13T00:15:37Z- handleDvl() - RDI_Pathfinder msg: 1613175337.61 epoch sec; seqNo:5460 2021-02-13T00:15:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.29 , 78.65 , 109.75 2021-02-13T00:15:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.06 , 0.12 2021-02-13T00:15:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175337.62 epoch sec; seqNo:11041; 4058734.96 north; 592242.35 east 2021-02-13T00:15:37Z- AHRS_M2 msg: 1613175337.99 epoch sec; seqNo:10949 2021-02-13T00:15:37Z- AHRS_M2 msg: 0.69 phi; -0.31 theta; -0.19 psi 2021-02-13T00:15:38Z- handleDepth()-Depth_Keller msg: 1613175338006 epoch sec; seqNo:11042; depth 14.11 2021-02-13T00:15:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:38Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:38Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:38Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:38Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:38Z- handleDvl() - RDI_Pathfinder msg: 1613175338.01 epoch sec; seqNo:5461 2021-02-13T00:15:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.06 , 0.12 2021-02-13T00:15:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175338.02 epoch sec; seqNo:11042; 4058735.33 north; 592242.35 east 2021-02-13T00:15:38Z- AHRS_M2 msg: 1613175338.40 epoch sec; seqNo:10950 2021-02-13T00:15:38Z- AHRS_M2 msg: 0.69 phi; -0.31 theta; -0.18 psi 2021-02-13T00:15:38Z- handleDepth()-Depth_Keller msg: 1613175338404 epoch sec; seqNo:11043; depth 14.23 2021-02-13T00:15:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175338.41 epoch sec; seqNo:11043; 4058735.33 north; 592242.35 east 2021-02-13T00:15:38Z- AHRS_M2 msg: 1613175338.80 epoch sec; seqNo:10951 2021-02-13T00:15:38Z- AHRS_M2 msg: 0.70 phi; -0.32 theta; -0.18 psi 2021-02-13T00:15:38Z- handleDepth()-Depth_Keller msg: 1613175338804 epoch sec; seqNo:11044; depth 14.33 2021-02-13T00:15:38Z- handleDvl() - RDI_Pathfinder msg: 1613175338.82 epoch sec; seqNo:5462 2021-02-13T00:15:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.50 , 100.08 , 78.44 , 109.55 2021-02-13T00:15:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.14 , 0.05 2021-02-13T00:15:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175338.83 epoch sec; seqNo:11044; 4058735.71 north; 592242.34 east 2021-02-13T00:15:39Z- AHRS_M2 msg: 1613175339.20 epoch sec; seqNo:10952 2021-02-13T00:15:39Z- AHRS_M2 msg: 0.70 phi; -0.32 theta; -0.18 psi 2021-02-13T00:15:39Z- handleDepth()-Depth_Keller msg: 1613175339217 epoch sec; seqNo:11045; depth 14.45 2021-02-13T00:15:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:39Z- handleDvl() - RDI_Pathfinder msg: 1613175339.22 epoch sec; seqNo:5463 2021-02-13T00:15:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.14 , 0.05 2021-02-13T00:15:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175339.23 epoch sec; seqNo:11045; 4058736.10 north; 592243.56 east 2021-02-13T00:15:39Z- AHRS_M2 msg: 1613175339.61 epoch sec; seqNo:10953 2021-02-13T00:15:39Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.18 psi 2021-02-13T00:15:39Z- handleDepth()-Depth_Keller msg: 1613175339612 epoch sec; seqNo:11046; depth 14.57 2021-02-13T00:15:39Z- handleDvl() - RDI_Pathfinder msg: 1613175339.63 epoch sec; seqNo:5464 2021-02-13T00:15:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 99.00 , 74.39 , 111.17 2021-02-13T00:15:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.11 , -0.04 2021-02-13T00:15:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175339.64 epoch sec; seqNo:11046; 4058736.10 north; 592243.56 east 2021-02-13T00:15:40Z- AHRS_M2 msg: 1613175340.01 epoch sec; seqNo:10954 2021-02-13T00:15:40Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.18 psi 2021-02-13T00:15:40Z- handleDepth()-Depth_Keller msg: 1613175340016 epoch sec; seqNo:11047; depth 14.68 2021-02-13T00:15:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175340.03 epoch sec; seqNo:11047; 4058736.48 north; 592243.56 east 2021-02-13T00:15:40Z- AHRS_M2 msg: 1613175340.42 epoch sec; seqNo:10955 2021-02-13T00:15:40Z- AHRS_M2 msg: 0.72 phi; -0.33 theta; -0.17 psi 2021-02-13T00:15:40Z- handleDepth()-Depth_Keller msg: 1613175340433 epoch sec; seqNo:11048; depth 14.80 2021-02-13T00:15:40Z- handleDvl() - RDI_Pathfinder msg: 1613175340.44 epoch sec; seqNo:5465 2021-02-13T00:15:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.49 , 100.42 , 75.21 , 111.24 2021-02-13T00:15:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.05 , -0.09 2021-02-13T00:15:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175340.45 epoch sec; seqNo:11048; 4058736.86 north; 592243.55 east 2021-02-13T00:15:40Z- AHRS_M2 msg: 1613175340.82 epoch sec; seqNo:10956 2021-02-13T00:15:40Z- AHRS_M2 msg: 0.72 phi; -0.33 theta; -0.17 psi 2021-02-13T00:15:40Z- handleDepth()-Depth_Keller msg: 1613175340830 epoch sec; seqNo:11049; depth 14.95 2021-02-13T00:15:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175340.85 epoch sec; seqNo:11049; 4058737.25 north; 592244.77 east 2021-02-13T00:15:41Z- AHRS_M2 msg: 1613175341.22 epoch sec; seqNo:10957 2021-02-13T00:15:41Z- AHRS_M2 msg: 0.72 phi; -0.33 theta; -0.17 psi 2021-02-13T00:15:41Z- handleDepth()-Depth_Keller msg: 1613175341233 epoch sec; seqNo:11050; depth 15.07 2021-02-13T00:15:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175341.25 epoch sec; seqNo:11050; 4058737.25 north; 592244.77 east 2021-02-13T00:15:41Z- AHRS_M2 msg: 1613175341.63 epoch sec; seqNo:10958 2021-02-13T00:15:41Z- AHRS_M2 msg: 0.72 phi; -0.33 theta; -0.17 psi 2021-02-13T00:15:41Z- handleDepth()-Depth_Keller msg: 1613175341632 epoch sec; seqNo:11051; depth 15.18 2021-02-13T00:15:41Z- handleDvl() - RDI_Pathfinder msg: 1613175341.64 epoch sec; seqNo:5466 2021-02-13T00:15:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 99.07 , 72.35 , 112.59 2021-02-13T00:15:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.02 , -0.09 2021-02-13T00:15:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175341.65 epoch sec; seqNo:11051; 4058737.63 north; 592244.76 east 2021-02-13T00:15:41Z- LcmTrn::handleMessages() - handling 47 messages took 0.82 ms and 131 calls... 2021-02-13T00:15:41Z- waiting for 1613175339.42, time is 1613175341.65 2021-02-13T00:15:41Z- LcmTrn::updateTrn() equating pose 1613175341.65 and meas times 1613175341.65 2021-02-13T00:15:41Z- LcmTrn::updateTrn() >>>> pose time 1613175341.65, meas time 1613175341.65 <<<< 2021-02-13T00:15:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175341.65 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:15:41Z- TerrainNav::motionUpdate() - currEstimate = (4058738, 592244.76, 15.18) 2021-02-13T00:15:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:15:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:15:41Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175341.65 sec.; No relevant navigation data available 2021-02-13T00:15:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:15:41Z- TNavBF::mmse selected from filter -1 2021-02-13T00:15:41Z- variances([0], [2], and psi_berg) = 711.541622 833.004396 0.000000 2021-02-13T00:15:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:15:41Z- LcmTrn::publishEstimates() - MLE : 4058737.13 592294.26 15.18 2021-02-13T00:15:41Z- LcmTrn::publishEstimates() - MMSE : 4058737.63 592244.76 15.18 2021-02-13T00:15:41Z- LcmTrn::publishEstimates() - COVAR: 711.54 833.00 1.34 0.00 2021-02-13T00:15:42Z- AHRS_M2 msg: 1613175342.04 epoch sec; seqNo:10959 2021-02-13T00:15:42Z- AHRS_M2 msg: 0.72 phi; -0.33 theta; -0.17 psi 2021-02-13T00:15:42Z- handleDepth()-Depth_Keller msg: 1613175342043 epoch sec; seqNo:11052; depth 15.33 2021-02-13T00:15:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175342.06 epoch sec; seqNo:11052; 4058738.01 north; 592244.76 east 2021-02-13T00:15:42Z- AHRS_M2 msg: 1613175342.44 epoch sec; seqNo:10960 2021-02-13T00:15:42Z- AHRS_M2 msg: 0.72 phi; -0.33 theta; -0.17 psi 2021-02-13T00:15:42Z- handleDepth()-Depth_Keller msg: 1613175342443 epoch sec; seqNo:11053; depth 15.44 2021-02-13T00:15:42Z- handleDvl() - RDI_Pathfinder msg: 1613175342.45 epoch sec; seqNo:5467 2021-02-13T00:15:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.29 , 98.86 , 78.17 , 111.04 2021-02-13T00:15:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.07 , -0.09 2021-02-13T00:15:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175342.46 epoch sec; seqNo:11053; 4058738.39 north; 592244.76 east 2021-02-13T00:15:42Z- AHRS_M2 msg: 1613175342.84 epoch sec; seqNo:10961 2021-02-13T00:15:42Z- AHRS_M2 msg: 0.72 phi; -0.33 theta; -0.18 psi 2021-02-13T00:15:42Z- handleDepth()-Depth_Keller msg: 1613175342844 epoch sec; seqNo:11054; depth 15.57 2021-02-13T00:15:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:42Z- handleDvl() - RDI_Pathfinder msg: 1613175342.85 epoch sec; seqNo:5468 2021-02-13T00:15:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.07 , -0.09 2021-02-13T00:15:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175342.87 epoch sec; seqNo:11054; 4058738.40 north; 592245.98 east 2021-02-13T00:15:43Z- AHRS_M2 msg: 1613175343.24 epoch sec; seqNo:10962 2021-02-13T00:15:43Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.18 psi 2021-02-13T00:15:43Z- handleDepth()-Depth_Keller msg: 1613175343248 epoch sec; seqNo:11055; depth 15.71 2021-02-13T00:15:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175343.26 epoch sec; seqNo:11055; 4058738.78 north; 592245.97 east 2021-02-13T00:15:43Z- AHRS_M2 msg: 1613175343.65 epoch sec; seqNo:10963 2021-02-13T00:15:43Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.19 psi 2021-02-13T00:15:43Z- handleDepth()-Depth_Keller msg: 1613175343652 epoch sec; seqNo:11056; depth 15.82 2021-02-13T00:15:43Z- handleDvl() - RDI_Pathfinder msg: 1613175343.66 epoch sec; seqNo:5469 2021-02-13T00:15:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.49 , 99.07 , 73.16 , 111.24 2021-02-13T00:15:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.13 , -0.09 2021-02-13T00:15:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175343.67 epoch sec; seqNo:11056; 4058739.16 north; 592245.97 east 2021-02-13T00:15:44Z- AHRS_M2 msg: 1613175344.06 epoch sec; seqNo:10964 2021-02-13T00:15:44Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.20 psi 2021-02-13T00:15:44Z- handleDepth()-Depth_Keller msg: 1613175344067 epoch sec; seqNo:11057; depth 15.95 2021-02-13T00:15:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:44Z- handleDvl() - RDI_Pathfinder msg: 1613175344.07 epoch sec; seqNo:5470 2021-02-13T00:15:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.13 , -0.09 2021-02-13T00:15:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175344.09 epoch sec; seqNo:11057; 4058739.54 north; 592245.96 east 2021-02-13T00:15:44Z- AHRS_M2 msg: 1613175344.45 epoch sec; seqNo:10965 2021-02-13T00:15:44Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.20 psi 2021-02-13T00:15:44Z- handleDepth()-Depth_Keller msg: 1613175344470 epoch sec; seqNo:11058; depth 16.07 2021-02-13T00:15:44Z- handleDvl() - RDI_Pathfinder msg: 1613175344.49 epoch sec; seqNo:5471 2021-02-13T00:15:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.29 , 100.08 , 71.94 , 108.60 2021-02-13T00:15:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.15 , -0.06 2021-02-13T00:15:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175344.50 epoch sec; seqNo:11058; 4058739.92 north; 592245.96 east 2021-02-13T00:15:44Z- AHRS_M2 msg: 1613175344.86 epoch sec; seqNo:10966 2021-02-13T00:15:44Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.21 psi 2021-02-13T00:15:44Z- handleDepth()-Depth_Keller msg: 1613175344864 epoch sec; seqNo:11059; depth 16.20 2021-02-13T00:15:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175344.88 epoch sec; seqNo:11059; 4058739.92 north; 592245.96 east 2021-02-13T00:15:45Z- AHRS_M2 msg: 1613175345.27 epoch sec; seqNo:10967 2021-02-13T00:15:45Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.20 psi 2021-02-13T00:15:45Z- handleDepth()-Depth_Keller msg: 1613175345275 epoch sec; seqNo:11060; depth 16.33 2021-02-13T00:15:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:45Z- handleDvl() - RDI_Pathfinder msg: 1613175345.28 epoch sec; seqNo:5472 2021-02-13T00:15:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.15 , -0.06 2021-02-13T00:15:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175345.29 epoch sec; seqNo:11060; 4058740.30 north; 592245.96 east 2021-02-13T00:15:45Z- AHRS_M2 msg: 1613175345.67 epoch sec; seqNo:10968 2021-02-13T00:15:45Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.20 psi 2021-02-13T00:15:45Z- handleDepth()-Depth_Keller msg: 1613175345678 epoch sec; seqNo:11061; depth 16.46 2021-02-13T00:15:45Z- handleDvl() - RDI_Pathfinder msg: 1613175345.69 epoch sec; seqNo:5473 2021-02-13T00:15:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.53 , 101.43 , 71.12 , 111.17 2021-02-13T00:15:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.16 , -0.03 2021-02-13T00:15:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175345.70 epoch sec; seqNo:11061; 4058740.69 north; 592247.17 east 2021-02-13T00:15:46Z- AHRS_M2 msg: 1613175346.07 epoch sec; seqNo:10969 2021-02-13T00:15:46Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.20 psi 2021-02-13T00:15:46Z- handleDepth()-Depth_Keller msg: 1613175346078 epoch sec; seqNo:11062; depth 16.59 2021-02-13T00:15:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175346.12 epoch sec; seqNo:11062; 4058741.07 north; 592247.17 east 2021-02-13T00:15:46Z- AHRS_M2 msg: 1613175346.47 epoch sec; seqNo:10970 2021-02-13T00:15:46Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.21 psi 2021-02-13T00:15:46Z- handleDepth()-Depth_Keller msg: 1613175346480 epoch sec; seqNo:11063; depth 16.70 2021-02-13T00:15:46Z- handleDvl() - RDI_Pathfinder msg: 1613175346.49 epoch sec; seqNo:5474 2021-02-13T00:15:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.48 , 97.71 , 71.53 , 109.89 2021-02-13T00:15:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.14 , 0.01 2021-02-13T00:15:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175346.50 epoch sec; seqNo:11063; 4058741.45 north; 592247.16 east 2021-02-13T00:15:46Z- LcmTrn::handleMessages() - handling 44 messages took 0.61 ms and 132 calls... 2021-02-13T00:15:46Z- waiting for 1613175344.65, time is 1613175346.50 2021-02-13T00:15:46Z- LcmTrn::updateTrn() equating pose 1613175346.50 and meas times 1613175346.50 2021-02-13T00:15:46Z- LcmTrn::updateTrn() >>>> pose time 1613175346.50, meas time 1613175346.50 <<<< 2021-02-13T00:15:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175346.50 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:15:46Z- TerrainNav::motionUpdate() - currEstimate = (4058741, 592247.16, 16.70) 2021-02-13T00:15:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:15:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:15:46Z- TNavBF::Projecting Measurements 2021-02-13T00:15:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:15:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:15:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:15:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:15:46Z- TNavFilter::4 good beams. 2021-02-13T00:15:46Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:15:46Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:15:46Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:15:46Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:15:46Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:15:46Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:15:46Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:15:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:15:46Z- TerrainMapOctree: (vehN, vehE ) = (4058741.45, 592247.16). 2021-02-13T00:15:46Z- TerrainMapOctree: Min Distance = 3210.01. 2021-02-13T00:15:46Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:15:46Z- There is no specific good beam that all particles have in common. 2021-02-13T00:15:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:15:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.57 K: 0.119 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T00:15:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:15:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:15:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:15:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.114 ExpectedMeasDiff: nan currMeas.covariance: 8.19 2021-02-13T00:15:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:15:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 19.32 2021-02-13T00:15:46Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:15:46Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:15:46Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:15:46Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:15:46Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:15:46Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:15:46Z- Filter 0 No beams used in subcloud update 2021-02-13T00:15:46Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:15:46Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:15:46Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 711.5 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:15:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175346.50 sec. 2021-02-13T00:15:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:15:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:15:46Z- variances([0], [2], and psi_berg) = 711.476494 833.004357 0.000000 2021-02-13T00:15:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:15:46Z- LcmTrn::publishEstimates() - MLE : 4058740.95 592296.66 16.70 2021-02-13T00:15:46Z- LcmTrn::publishEstimates() - MMSE : 4058741.45 592247.16 16.70 2021-02-13T00:15:46Z- LcmTrn::publishEstimates() - COVAR: 711.48 833.00 1.08 0.00 2021-02-13T00:15:46Z- AHRS_M2 msg: 1613175346.88 epoch sec; seqNo:10971 2021-02-13T00:15:46Z- AHRS_M2 msg: 0.70 phi; -0.34 theta; -0.21 psi 2021-02-13T00:15:46Z- handleDepth()-Depth_Keller msg: 1613175346893 epoch sec; seqNo:11064; depth 16.85 2021-02-13T00:15:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175346.90 epoch sec; seqNo:11064; 4058741.45 north; 592247.16 east 2021-02-13T00:15:47Z- AHRS_M2 msg: 1613175347.28 epoch sec; seqNo:10972 2021-02-13T00:15:47Z- AHRS_M2 msg: 0.70 phi; -0.34 theta; -0.21 psi 2021-02-13T00:15:47Z- handleDepth()-Depth_Keller msg: 1613175347288 epoch sec; seqNo:11065; depth 16.97 2021-02-13T00:15:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175347.30 epoch sec; seqNo:11065; 4058741.83 north; 592247.16 east 2021-02-13T00:15:47Z- AHRS_M2 msg: 1613175347.69 epoch sec; seqNo:10973 2021-02-13T00:15:47Z- AHRS_M2 msg: 0.70 phi; -0.34 theta; -0.21 psi 2021-02-13T00:15:47Z- handleDepth()-Depth_Keller msg: 1613175347692 epoch sec; seqNo:11066; depth 17.10 2021-02-13T00:15:47Z- handleDvl() - RDI_Pathfinder msg: 1613175347.70 epoch sec; seqNo:5475 2021-02-13T00:15:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.07 , 97.78 , 78.31 , 108.74 2021-02-13T00:15:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.13 , 0.07 2021-02-13T00:15:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175347.71 epoch sec; seqNo:11066; 4058742.21 north; 592247.16 east 2021-02-13T00:15:48Z- AHRS_M2 msg: 1613175348.09 epoch sec; seqNo:10974 2021-02-13T00:15:48Z- AHRS_M2 msg: 0.70 phi; -0.34 theta; -0.20 psi 2021-02-13T00:15:48Z- handleDepth()-Depth_Keller msg: 1613175348098 epoch sec; seqNo:11067; depth 17.24 2021-02-13T00:15:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175348.11 epoch sec; seqNo:11067; 4058742.21 north; 592247.16 east 2021-02-13T00:15:48Z- AHRS_M2 msg: 1613175348.50 epoch sec; seqNo:10975 2021-02-13T00:15:48Z- AHRS_M2 msg: 0.70 phi; -0.34 theta; -0.19 psi 2021-02-13T00:15:48Z- handleDepth()-Depth_Keller msg: 1613175348500 epoch sec; seqNo:11068; depth 17.36 2021-02-13T00:15:48Z- handleDvl() - RDI_Pathfinder msg: 1613175348.51 epoch sec; seqNo:5476 2021-02-13T00:15:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.24 , 98.93 , 70.71 , 111.10 2021-02-13T00:15:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.07 , 0.12 2021-02-13T00:15:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175348.53 epoch sec; seqNo:11068; 4058742.60 north; 592248.37 east 2021-02-13T00:15:48Z- AHRS_M2 msg: 1613175348.90 epoch sec; seqNo:10976 2021-02-13T00:15:48Z- AHRS_M2 msg: 0.71 phi; -0.34 theta; -0.19 psi 2021-02-13T00:15:48Z- handleDepth()-Depth_Keller msg: 1613175348909 epoch sec; seqNo:11069; depth 17.50 2021-02-13T00:15:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:48Z- handleDvl() - RDI_Pathfinder msg: 1613175348.91 epoch sec; seqNo:5477 2021-02-13T00:15:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.07 , 0.12 2021-02-13T00:15:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175348.92 epoch sec; seqNo:11069; 4058742.98 north; 592248.37 east 2021-02-13T00:15:49Z- AHRS_M2 msg: 1613175349.31 epoch sec; seqNo:10977 2021-02-13T00:15:49Z- AHRS_M2 msg: 0.71 phi; -0.35 theta; -0.18 psi 2021-02-13T00:15:49Z- handleDepth()-Depth_Keller msg: 1613175349322 epoch sec; seqNo:11070; depth 17.62 2021-02-13T00:15:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175349.35 epoch sec; seqNo:11070; 4058742.98 north; 592248.37 east 2021-02-13T00:15:49Z- AHRS_M2 msg: 1613175349.71 epoch sec; seqNo:10978 2021-02-13T00:15:49Z- AHRS_M2 msg: 0.71 phi; -0.35 theta; -0.17 psi 2021-02-13T00:15:49Z- handleDepth()-Depth_Keller msg: 1613175349712 epoch sec; seqNo:11071; depth 17.76 2021-02-13T00:15:49Z- handleDvl() - RDI_Pathfinder msg: 1613175349.73 epoch sec; seqNo:5478 2021-02-13T00:15:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.53 , 97.78 , 70.30 , 111.17 2021-02-13T00:15:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , 0.09 2021-02-13T00:15:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175349.74 epoch sec; seqNo:11071; 4058743.36 north; 592248.36 east 2021-02-13T00:15:50Z- AHRS_M2 msg: 1613175350.11 epoch sec; seqNo:10979 2021-02-13T00:15:50Z- AHRS_M2 msg: 0.71 phi; -0.35 theta; -0.17 psi 2021-02-13T00:15:50Z- handleDepth()-Depth_Keller msg: 1613175350117 epoch sec; seqNo:11072; depth 17.88 2021-02-13T00:15:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:50Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:50Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:50Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:50Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:50Z- handleDvl() - RDI_Pathfinder msg: 1613175350.12 epoch sec; seqNo:5479 2021-02-13T00:15:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , 0.09 2021-02-13T00:15:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175350.14 epoch sec; seqNo:11072; 4058743.73 north; 592248.36 east 2021-02-13T00:15:50Z- AHRS_M2 msg: 1613175350.52 epoch sec; seqNo:10980 2021-02-13T00:15:50Z- AHRS_M2 msg: 0.72 phi; -0.35 theta; -0.17 psi 2021-02-13T00:15:50Z- handleDepth()-Depth_Keller msg: 1613175350525 epoch sec; seqNo:11073; depth 18.01 2021-02-13T00:15:50Z- handleDvl() - RDI_Pathfinder msg: 1613175350.54 epoch sec; seqNo:5480 2021-02-13T00:15:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 97.78 , 78.31 , 112.39 2021-02-13T00:15:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , 0.02 2021-02-13T00:15:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175350.57 epoch sec; seqNo:11073; 4058743.73 north; 592248.36 east 2021-02-13T00:15:50Z- AHRS_M2 msg: 1613175350.92 epoch sec; seqNo:10981 2021-02-13T00:15:50Z- AHRS_M2 msg: 0.72 phi; -0.35 theta; -0.17 psi 2021-02-13T00:15:50Z- handleDepth()-Depth_Keller msg: 1613175350929 epoch sec; seqNo:11074; depth 18.14 2021-02-13T00:15:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175350.94 epoch sec; seqNo:11074; 4058744.13 north; 592249.58 east 2021-02-13T00:15:51Z- AHRS_M2 msg: 1613175351.33 epoch sec; seqNo:10982 2021-02-13T00:15:51Z- AHRS_M2 msg: 0.72 phi; -0.35 theta; -0.17 psi 2021-02-13T00:15:51Z- handleDepth()-Depth_Keller msg: 1613175351337 epoch sec; seqNo:11075; depth 18.28 2021-02-13T00:15:51Z- handleDvl() - RDI_Pathfinder msg: 1613175351.35 epoch sec; seqNo:5481 2021-02-13T00:15:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.14 , 97.71 , 69.89 , 111.24 2021-02-13T00:15:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.04 2021-02-13T00:15:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175351.36 epoch sec; seqNo:11075; 4058744.13 north; 592249.58 east 2021-02-13T00:15:51Z- AHRS_M2 msg: 1613175351.73 epoch sec; seqNo:10983 2021-02-13T00:15:51Z- AHRS_M2 msg: 0.72 phi; -0.35 theta; -0.18 psi 2021-02-13T00:15:51Z- handleDepth()-Depth_Keller msg: 1613175351732 epoch sec; seqNo:11076; depth 18.41 2021-02-13T00:15:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175351.75 epoch sec; seqNo:11076; 4058744.51 north; 592249.57 east 2021-02-13T00:15:51Z- LcmTrn::handleMessages() - handling 46 messages took 0.77 ms and 131 calls... 2021-02-13T00:15:51Z- waiting for 1613175349.50, time is 1613175351.75 2021-02-13T00:15:51Z- LcmTrn::updateTrn() >>>> pose time 1613175351.75, meas time 1613175351.35 <<<< 2021-02-13T00:15:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175351.35 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:15:51Z- TerrainNav::motionUpdate() - currEstimate = (4058745, 592249.57, 18.41) 2021-02-13T00:15:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:15:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:15:51Z- TNavBF::Projecting Measurements 2021-02-13T00:15:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:15:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:15:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:15:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:15:51Z- TNavFilter::4 good beams. 2021-02-13T00:15:51Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:15:51Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:15:51Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:15:51Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:15:51Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:15:51Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:15:51Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:15:51Z- TNavBF::mmse selected from filter -1 2021-02-13T00:15:51Z- TerrainMapOctree: (vehN, vehE ) = (4058744.27, 592249.39). 2021-02-13T00:15:51Z- TerrainMapOctree: Min Distance = 3206.42. 2021-02-13T00:15:51Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:15:51Z- There is no specific good beam that all particles have in common. 2021-02-13T00:15:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:15:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 10.27 2021-02-13T00:15:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:15:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:15:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:15:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 7.82 2021-02-13T00:15:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:15:51Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 19.80 2021-02-13T00:15:51Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:15:51Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:15:51Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:15:51Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:15:51Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:15:51Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:15:51Z- Filter 0 No beams used in subcloud update 2021-02-13T00:15:51Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:15:51Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:15:51Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 711.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:15:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175351.35 sec. 2021-02-13T00:15:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:15:51Z- TNavBF::mmse selected from filter -1 2021-02-13T00:15:51Z- variances([0], [2], and psi_berg) = 711.401655 833.004313 0.000000 2021-02-13T00:15:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:15:51Z- LcmTrn::publishEstimates() - MLE : 4058744.01 592299.07 18.41 2021-02-13T00:15:51Z- LcmTrn::publishEstimates() - MMSE : 4058744.51 592249.57 18.41 2021-02-13T00:15:51Z- LcmTrn::publishEstimates() - COVAR: 711.40 833.00 0.93 0.00 2021-02-13T00:15:52Z- AHRS_M2 msg: 1613175352.13 epoch sec; seqNo:10984 2021-02-13T00:15:52Z- AHRS_M2 msg: 0.72 phi; -0.35 theta; -0.18 psi 2021-02-13T00:15:52Z- handleDepth()-Depth_Keller msg: 1613175352140 epoch sec; seqNo:11077; depth 18.52 2021-02-13T00:15:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175352.15 epoch sec; seqNo:11077; 4058744.88 north; 592249.57 east 2021-02-13T00:15:52Z- AHRS_M2 msg: 1613175352.54 epoch sec; seqNo:10985 2021-02-13T00:15:52Z- AHRS_M2 msg: 0.72 phi; -0.35 theta; -0.19 psi 2021-02-13T00:15:52Z- handleDepth()-Depth_Keller msg: 1613175352545 epoch sec; seqNo:11078; depth 18.67 2021-02-13T00:15:52Z- handleDvl() - RDI_Pathfinder msg: 1613175352.56 epoch sec; seqNo:5482 2021-02-13T00:15:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.84 , 97.78 , 70.71 , 108.74 2021-02-13T00:15:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.03 , -0.05 2021-02-13T00:15:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175352.57 epoch sec; seqNo:11078; 4058745.26 north; 592249.57 east 2021-02-13T00:15:52Z- AHRS_M2 msg: 1613175352.94 epoch sec; seqNo:10986 2021-02-13T00:15:52Z- AHRS_M2 msg: 0.73 phi; -0.35 theta; -0.19 psi 2021-02-13T00:15:52Z- handleDepth()-Depth_Keller msg: 1613175352949 epoch sec; seqNo:11079; depth 18.80 2021-02-13T00:15:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175352.96 epoch sec; seqNo:11079; 4058745.26 north; 592249.57 east 2021-02-13T00:15:53Z- AHRS_M2 msg: 1613175353.34 epoch sec; seqNo:10987 2021-02-13T00:15:53Z- AHRS_M2 msg: 0.73 phi; -0.35 theta; -0.19 psi 2021-02-13T00:15:53Z- handleDepth()-Depth_Keller msg: 1613175353348 epoch sec; seqNo:11080; depth 18.94 2021-02-13T00:15:53Z- handleDvl() - RDI_Pathfinder msg: 1613175353.37 epoch sec; seqNo:5483 2021-02-13T00:15:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.25 , 97.92 , 69.48 , 110.09 2021-02-13T00:15:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.06 , -0.01 2021-02-13T00:15:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175353.38 epoch sec; seqNo:11080; 4058745.65 north; 592250.78 east 2021-02-13T00:15:53Z- AHRS_M2 msg: 1613175353.75 epoch sec; seqNo:10988 2021-02-13T00:15:53Z- AHRS_M2 msg: 0.73 phi; -0.35 theta; -0.19 psi 2021-02-13T00:15:53Z- handleDepth()-Depth_Keller msg: 1613175353752 epoch sec; seqNo:11081; depth 19.08 2021-02-13T00:15:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:53Z- handleDvl() - RDI_Pathfinder msg: 1613175353.75 epoch sec; seqNo:5484 2021-02-13T00:15:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.06 , -0.01 2021-02-13T00:15:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175353.77 epoch sec; seqNo:11081; 4058746.03 north; 592250.78 east 2021-02-13T00:15:54Z- AHRS_M2 msg: 1613175354.16 epoch sec; seqNo:10989 2021-02-13T00:15:54Z- AHRS_M2 msg: 0.74 phi; -0.35 theta; -0.18 psi 2021-02-13T00:15:54Z- handleDepth()-Depth_Keller msg: 1613175354164 epoch sec; seqNo:11082; depth 19.21 2021-02-13T00:15:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175354.17 epoch sec; seqNo:11082; 4058746.41 north; 592250.77 east 2021-02-13T00:15:54Z- AHRS_M2 msg: 1613175354.56 epoch sec; seqNo:10990 2021-02-13T00:15:54Z- AHRS_M2 msg: 0.74 phi; -0.35 theta; -0.17 psi 2021-02-13T00:15:54Z- handleDepth()-Depth_Keller msg: 1613175354562 epoch sec; seqNo:11083; depth 19.34 2021-02-13T00:15:54Z- handleDvl() - RDI_Pathfinder msg: 1613175354.57 epoch sec; seqNo:5485 2021-02-13T00:15:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.84 , 95.42 , 68.67 , 110.02 2021-02-13T00:15:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.04 , 0.00 2021-02-13T00:15:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175354.58 epoch sec; seqNo:11083; 4058746.41 north; 592250.77 east 2021-02-13T00:15:54Z- AHRS_M2 msg: 1613175354.96 epoch sec; seqNo:10991 2021-02-13T00:15:54Z- AHRS_M2 msg: 0.75 phi; -0.35 theta; -0.16 psi 2021-02-13T00:15:54Z- handleDepth()-Depth_Keller msg: 1613175354969 epoch sec; seqNo:11084; depth 19.47 2021-02-13T00:15:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:54Z- handleDvl() - RDI_Pathfinder msg: 1613175354.97 epoch sec; seqNo:5486 2021-02-13T00:15:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.04 , 0.00 2021-02-13T00:15:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175354.99 epoch sec; seqNo:11084; 4058746.79 north; 592250.77 east 2021-02-13T00:15:55Z- AHRS_M2 msg: 1613175355.36 epoch sec; seqNo:10992 2021-02-13T00:15:55Z- AHRS_M2 msg: 0.75 phi; -0.35 theta; -0.15 psi 2021-02-13T00:15:55Z- handleDepth()-Depth_Keller msg: 1613175355369 epoch sec; seqNo:11085; depth 19.62 2021-02-13T00:15:55Z- handleDvl() - RDI_Pathfinder msg: 1613175355.38 epoch sec; seqNo:5487 2021-02-13T00:15:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.99 , 70.30 , 107.72 2021-02-13T00:15:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.00 , -0.04 2021-02-13T00:15:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175355.40 epoch sec; seqNo:11085; 4058747.18 north; 592251.99 east 2021-02-13T00:15:55Z- AHRS_M2 msg: 1613175355.77 epoch sec; seqNo:10993 2021-02-13T00:15:55Z- AHRS_M2 msg: 0.76 phi; -0.35 theta; -0.14 psi 2021-02-13T00:15:55Z- handleDepth()-Depth_Keller msg: 1613175355772 epoch sec; seqNo:11086; depth 19.77 2021-02-13T00:15:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175355.78 epoch sec; seqNo:11086; 4058747.18 north; 592251.99 east 2021-02-13T00:15:56Z- AHRS_M2 msg: 1613175356.17 epoch sec; seqNo:10994 2021-02-13T00:15:56Z- AHRS_M2 msg: 0.76 phi; -0.35 theta; -0.14 psi 2021-02-13T00:15:56Z- handleDepth()-Depth_Keller msg: 1613175356179 epoch sec; seqNo:11087; depth 19.90 2021-02-13T00:15:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:56Z- handleDvl() - RDI_Pathfinder msg: 1613175356.18 epoch sec; seqNo:5488 2021-02-13T00:15:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.00 , -0.04 2021-02-13T00:15:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175356.19 epoch sec; seqNo:11087; 4058747.56 north; 592251.98 east 2021-02-13T00:15:56Z- AHRS_M2 msg: 1613175356.58 epoch sec; seqNo:10995 2021-02-13T00:15:56Z- AHRS_M2 msg: 0.77 phi; -0.35 theta; -0.14 psi 2021-02-13T00:15:56Z- handleDepth()-Depth_Keller msg: 1613175356581 epoch sec; seqNo:11088; depth 20.02 2021-02-13T00:15:56Z- handleDvl() - RDI_Pathfinder msg: 1613175356.60 epoch sec; seqNo:5489 2021-02-13T00:15:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.66 , 97.85 , 69.07 , 106.37 2021-02-13T00:15:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , -0.12 2021-02-13T00:15:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175356.61 epoch sec; seqNo:11088; 4058747.94 north; 592251.98 east 2021-02-13T00:15:56Z- LcmTrn::handleMessages() - handling 44 messages took 0.62 ms and 132 calls... 2021-02-13T00:15:56Z- waiting for 1613175354.75, time is 1613175356.61 2021-02-13T00:15:56Z- LcmTrn::updateTrn() equating pose 1613175356.61 and meas times 1613175356.61 2021-02-13T00:15:56Z- LcmTrn::updateTrn() >>>> pose time 1613175356.61, meas time 1613175356.61 <<<< 2021-02-13T00:15:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175356.61 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:15:56Z- TerrainNav::motionUpdate() - currEstimate = (4058748, 592251.98, 20.02) 2021-02-13T00:15:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:15:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:15:56Z- TNavBF::Projecting Measurements 2021-02-13T00:15:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:15:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:15:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:15:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:15:56Z- TNavFilter::4 good beams. 2021-02-13T00:15:56Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:15:56Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:15:56Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:15:56Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:15:56Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:15:56Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:15:56Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:15:56Z- TNavBF::mmse selected from filter -1 2021-02-13T00:15:56Z- TerrainMapOctree: (vehN, vehE ) = (4058747.94, 592251.98). 2021-02-13T00:15:56Z- TerrainMapOctree: Min Distance = 3201.94. 2021-02-13T00:15:56Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:15:56Z- There is no specific good beam that all particles have in common. 2021-02-13T00:15:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:15:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 9.65 2021-02-13T00:15:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:15:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-13T00:15:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:15:56Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-13T00:15:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:15:57Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 18.10 2021-02-13T00:15:57Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:15:57Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:15:57Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:15:57Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:15:57Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:15:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:15:57Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:15:57Z- Filter 0 No beams used in subcloud update 2021-02-13T00:15:57Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:15:57Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:15:57Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 711.3 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:15:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175356.61 sec. 2021-02-13T00:15:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:15:57Z- TNavBF::mmse selected from filter -1 2021-02-13T00:15:57Z- variances([0], [2], and psi_berg) = 711.338930 833.004275 0.000000 2021-02-13T00:15:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:15:57Z- LcmTrn::publishEstimates() - MLE : 4058747.44 592301.48 20.02 2021-02-13T00:15:57Z- LcmTrn::publishEstimates() - MMSE : 4058747.94 592251.98 20.02 2021-02-13T00:15:57Z- LcmTrn::publishEstimates() - COVAR: 711.34 833.00 0.85 0.00 2021-02-13T00:15:57Z- AHRS_M2 msg: 1613175356.98 epoch sec; seqNo:10996 2021-02-13T00:15:57Z- AHRS_M2 msg: 0.77 phi; -0.36 theta; -0.15 psi 2021-02-13T00:15:57Z- handleDepth()-Depth_Keller msg: 1613175356989 epoch sec; seqNo:11089; depth 20.16 2021-02-13T00:15:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175357.00 epoch sec; seqNo:11089; 4058747.94 north; 592251.98 east 2021-02-13T00:15:57Z- AHRS_M2 msg: 1613175357.38 epoch sec; seqNo:10997 2021-02-13T00:15:57Z- AHRS_M2 msg: 0.77 phi; -0.36 theta; -0.16 psi 2021-02-13T00:15:57Z- handleDepth()-Depth_Keller msg: 1613175357391 epoch sec; seqNo:11090; depth 20.30 2021-02-13T00:15:57Z- handleDvl() - RDI_Pathfinder msg: 1613175357.40 epoch sec; seqNo:5490 2021-02-13T00:15:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 97.99 , 67.85 , 106.51 2021-02-13T00:15:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , -0.14 2021-02-13T00:15:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175357.41 epoch sec; seqNo:11090; 4058748.33 north; 592253.19 east 2021-02-13T00:15:57Z- AHRS_M2 msg: 1613175357.79 epoch sec; seqNo:10998 2021-02-13T00:15:57Z- AHRS_M2 msg: 0.77 phi; -0.36 theta; -0.17 psi 2021-02-13T00:15:57Z- handleDepth()-Depth_Keller msg: 1613175357792 epoch sec; seqNo:11091; depth 20.43 2021-02-13T00:15:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175357.82 epoch sec; seqNo:11091; 4058748.71 north; 592253.19 east 2021-02-13T00:15:58Z- AHRS_M2 msg: 1613175358.19 epoch sec; seqNo:10999 2021-02-13T00:15:58Z- AHRS_M2 msg: 0.77 phi; -0.36 theta; -0.18 psi 2021-02-13T00:15:58Z- handleDepth()-Depth_Keller msg: 1613175358196 epoch sec; seqNo:11092; depth 20.55 2021-02-13T00:15:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175358.20 epoch sec; seqNo:11092; 4058749.09 north; 592253.19 east 2021-02-13T00:15:58Z- AHRS_M2 msg: 1613175358.60 epoch sec; seqNo:11000 2021-02-13T00:15:58Z- AHRS_M2 msg: 0.77 phi; -0.36 theta; -0.19 psi 2021-02-13T00:15:58Z- handleDepth()-Depth_Keller msg: 1613175358605 epoch sec; seqNo:11093; depth 20.69 2021-02-13T00:15:58Z- handleDvl() - RDI_Pathfinder msg: 1613175358.62 epoch sec; seqNo:5491 2021-02-13T00:15:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 94.33 , 67.03 , 107.72 2021-02-13T00:15:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.14 , -0.07 2021-02-13T00:15:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175358.63 epoch sec; seqNo:11093; 4058749.09 north; 592253.19 east 2021-02-13T00:15:59Z- AHRS_M2 msg: 1613175359.00 epoch sec; seqNo:11001 2021-02-13T00:15:59Z- AHRS_M2 msg: 0.77 phi; -0.36 theta; -0.20 psi 2021-02-13T00:15:59Z- handleDepth()-Depth_Keller msg: 1613175359004 epoch sec; seqNo:11094; depth 20.83 2021-02-13T00:15:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175359.02 epoch sec; seqNo:11094; 4058749.47 north; 592253.18 east 2021-02-13T00:15:59Z- AHRS_M2 msg: 1613175359.40 epoch sec; seqNo:11002 2021-02-13T00:15:59Z- AHRS_M2 msg: 0.76 phi; -0.36 theta; -0.20 psi 2021-02-13T00:15:59Z- handleDepth()-Depth_Keller msg: 1613175359408 epoch sec; seqNo:11095; depth 20.96 2021-02-13T00:15:59Z- handleDvl() - RDI_Pathfinder msg: 1613175359.42 epoch sec; seqNo:5492 2021-02-13T00:15:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 94.40 , 67.85 , 106.57 2021-02-13T00:15:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.14 , 0.02 2021-02-13T00:15:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175359.44 epoch sec; seqNo:11095; 4058749.85 north; 592253.18 east 2021-02-13T00:15:59Z- AHRS_M2 msg: 1613175359.81 epoch sec; seqNo:11003 2021-02-13T00:15:59Z- AHRS_M2 msg: 0.76 phi; -0.36 theta; -0.21 psi 2021-02-13T00:15:59Z- handleDepth()-Depth_Keller msg: 1613175359812 epoch sec; seqNo:11096; depth 21.08 2021-02-13T00:15:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:15:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:15:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:15:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:15:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:15:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:15:59Z- handleDvl() - RDI_Pathfinder msg: 1613175359.81 epoch sec; seqNo:5493 2021-02-13T00:15:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:15:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.14 , 0.02 2021-02-13T00:15:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175359.83 epoch sec; seqNo:11096; 4058749.86 north; 592254.40 east 2021-02-13T00:16:00Z- AHRS_M2 msg: 1613175360.21 epoch sec; seqNo:11004 2021-02-13T00:16:00Z- AHRS_M2 msg: 0.75 phi; -0.37 theta; -0.21 psi 2021-02-13T00:16:00Z- handleDepth()-Depth_Keller msg: 1613175360217 epoch sec; seqNo:11097; depth 21.24 2021-02-13T00:16:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175360.23 epoch sec; seqNo:11097; 4058750.24 north; 592254.40 east 2021-02-13T00:16:00Z- AHRS_M2 msg: 1613175360.62 epoch sec; seqNo:11005 2021-02-13T00:16:00Z- AHRS_M2 msg: 0.75 phi; -0.37 theta; -0.21 psi 2021-02-13T00:16:00Z- handleDepth()-Depth_Keller msg: 1613175360620 epoch sec; seqNo:11098; depth 21.36 2021-02-13T00:16:00Z- handleDvl() - RDI_Pathfinder msg: 1613175360.64 epoch sec; seqNo:5494 2021-02-13T00:16:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 67.85 , 107.79 2021-02-13T00:16:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.06 , 0.09 2021-02-13T00:16:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175360.65 epoch sec; seqNo:11098; 4058750.62 north; 592254.39 east 2021-02-13T00:16:01Z- AHRS_M2 msg: 1613175361.02 epoch sec; seqNo:11006 2021-02-13T00:16:01Z- AHRS_M2 msg: 0.75 phi; -0.37 theta; -0.21 psi 2021-02-13T00:16:01Z- handleDepth()-Depth_Keller msg: 1613175361025 epoch sec; seqNo:11099; depth 21.50 2021-02-13T00:16:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:01Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:01Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:01Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:01Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:01Z- handleDvl() - RDI_Pathfinder msg: 1613175361.03 epoch sec; seqNo:5495 2021-02-13T00:16:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.06 , 0.09 2021-02-13T00:16:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175361.04 epoch sec; seqNo:11099; 4058750.62 north; 592254.39 east 2021-02-13T00:16:01Z- AHRS_M2 msg: 1613175361.42 epoch sec; seqNo:11007 2021-02-13T00:16:01Z- AHRS_M2 msg: 0.75 phi; -0.37 theta; -0.21 psi 2021-02-13T00:16:01Z- handleDepth()-Depth_Keller msg: 1613175361437 epoch sec; seqNo:11100; depth 21.64 2021-02-13T00:16:01Z- handleDvl() - RDI_Pathfinder msg: 1613175361.45 epoch sec; seqNo:5496 2021-02-13T00:16:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 97.92 , 66.21 , 108.87 2021-02-13T00:16:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.03 , 0.09 2021-02-13T00:16:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175361.46 epoch sec; seqNo:11100; 4058751.00 north; 592254.39 east 2021-02-13T00:16:01Z- AHRS_M2 msg: 1613175361.83 epoch sec; seqNo:11008 2021-02-13T00:16:01Z- AHRS_M2 msg: 0.75 phi; -0.38 theta; -0.21 psi 2021-02-13T00:16:01Z- handleDepth()-Depth_Keller msg: 1613175361839 epoch sec; seqNo:11101; depth 21.78 2021-02-13T00:16:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175361.87 epoch sec; seqNo:11101; 4058751.00 north; 592254.39 east 2021-02-13T00:16:02Z- LcmTrn::handleMessages() - handling 46 messages took 0.68 ms and 133 calls... 2021-02-13T00:16:02Z- waiting for 1613175359.61, time is 1613175361.87 2021-02-13T00:16:02Z- LcmTrn::updateTrn() >>>> pose time 1613175361.87, meas time 1613175361.45 <<<< 2021-02-13T00:16:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175361.45 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:16:02Z- TerrainNav::motionUpdate() - currEstimate = (4058751, 592254.39, 21.78) 2021-02-13T00:16:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:16:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:16:02Z- TNavBF::Projecting Measurements 2021-02-13T00:16:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:16:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:16:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:16:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:16:02Z- TNavFilter::4 good beams. 2021-02-13T00:16:02Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:16:02Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:16:02Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:16:02Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:16:02Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:16:02Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:16:02Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:16:02Z- TNavBF::mmse selected from filter -1 2021-02-13T00:16:02Z- TerrainMapOctree: (vehN, vehE ) = (4058750.75, 592254.19). 2021-02-13T00:16:02Z- TerrainMapOctree: Min Distance = 3198.35. 2021-02-13T00:16:02Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:16:02Z- There is no specific good beam that all particles have in common. 2021-02-13T00:16:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:16:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 8.95 2021-02-13T00:16:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:16:02Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:16:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:16:02Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 7.01 2021-02-13T00:16:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:16:02Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 18.96 2021-02-13T00:16:02Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:16:02Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:16:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:16:02Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:16:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:16:02Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:16:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:16:02Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:16:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:16:02Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:16:02Z- Filter 0 No beams used in subcloud update 2021-02-13T00:16:02Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:16:02Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:16:02Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 711.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:16:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175361.45 sec. 2021-02-13T00:16:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:16:02Z- TNavBF::mmse selected from filter -1 2021-02-13T00:16:02Z- variances([0], [2], and psi_berg) = 711.263956 833.004231 0.000000 2021-02-13T00:16:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:16:02Z- LcmTrn::publishEstimates() - MLE : 4058750.50 592303.89 21.78 2021-02-13T00:16:02Z- LcmTrn::publishEstimates() - MMSE : 4058751.00 592254.39 21.78 2021-02-13T00:16:02Z- LcmTrn::publishEstimates() - COVAR: 711.26 833.00 0.80 0.00 2021-02-13T00:16:02Z- AHRS_M2 msg: 1613175362.23 epoch sec; seqNo:11009 2021-02-13T00:16:02Z- AHRS_M2 msg: 0.75 phi; -0.38 theta; -0.21 psi 2021-02-13T00:16:02Z- handleDepth()-Depth_Keller msg: 1613175362247 epoch sec; seqNo:11102; depth 21.93 2021-02-13T00:16:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:02Z- handleDvl() - RDI_Pathfinder msg: 1613175362.25 epoch sec; seqNo:5497 2021-02-13T00:16:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.03 , 0.09 2021-02-13T00:16:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175362.28 epoch sec; seqNo:11102; 4058751.39 north; 592255.60 east 2021-02-13T00:16:02Z- AHRS_M2 msg: 1613175362.64 epoch sec; seqNo:11010 2021-02-13T00:16:02Z- AHRS_M2 msg: 0.74 phi; -0.38 theta; -0.21 psi 2021-02-13T00:16:02Z- handleDepth()-Depth_Keller msg: 1613175362649 epoch sec; seqNo:11103; depth 22.08 2021-02-13T00:16:02Z- handleDvl() - RDI_Pathfinder msg: 1613175362.66 epoch sec; seqNo:5498 2021-02-13T00:16:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.39 , 98.05 , 68.26 , 106.57 2021-02-13T00:16:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.09 , 0.04 2021-02-13T00:16:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175362.69 epoch sec; seqNo:11103; 4058751.77 north; 592255.60 east 2021-02-13T00:16:03Z- AHRS_M2 msg: 1613175363.04 epoch sec; seqNo:11011 2021-02-13T00:16:03Z- AHRS_M2 msg: 0.74 phi; -0.38 theta; -0.21 psi 2021-02-13T00:16:03Z- handleDepth()-Depth_Keller msg: 1613175363054 epoch sec; seqNo:11104; depth 22.20 2021-02-13T00:16:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175363.06 epoch sec; seqNo:11104; 4058751.77 north; 592255.60 east 2021-02-13T00:16:03Z- AHRS_M2 msg: 1613175363.44 epoch sec; seqNo:11012 2021-02-13T00:16:03Z- AHRS_M2 msg: 0.74 phi; -0.38 theta; -0.21 psi 2021-02-13T00:16:03Z- handleDepth()-Depth_Keller msg: 1613175363449 epoch sec; seqNo:11105; depth 22.35 2021-02-13T00:16:03Z- handleDvl() - RDI_Pathfinder msg: 1613175363.46 epoch sec; seqNo:5499 2021-02-13T00:16:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 65.40 , 111.44 2021-02-13T00:16:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.09 , -0.04 2021-02-13T00:16:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175363.48 epoch sec; seqNo:11105; 4058752.15 north; 592255.60 east 2021-02-13T00:16:03Z- AHRS_M2 msg: 1613175363.85 epoch sec; seqNo:11013 2021-02-13T00:16:03Z- AHRS_M2 msg: 0.74 phi; -0.38 theta; -0.22 psi 2021-02-13T00:16:03Z- handleDepth()-Depth_Keller msg: 1613175363852 epoch sec; seqNo:11106; depth 22.50 2021-02-13T00:16:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175363.86 epoch sec; seqNo:11106; 4058752.15 north; 592255.60 east 2021-02-13T00:16:04Z- AHRS_M2 msg: 1613175364.25 epoch sec; seqNo:11014 2021-02-13T00:16:04Z- AHRS_M2 msg: 0.73 phi; -0.38 theta; -0.22 psi 2021-02-13T00:16:04Z- handleDepth()-Depth_Keller msg: 1613175364258 epoch sec; seqNo:11107; depth 22.63 2021-02-13T00:16:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175364.27 epoch sec; seqNo:11107; 4058752.54 north; 592256.81 east 2021-02-13T00:16:04Z- AHRS_M2 msg: 1613175364.66 epoch sec; seqNo:11015 2021-02-13T00:16:04Z- AHRS_M2 msg: 0.73 phi; -0.38 theta; -0.22 psi 2021-02-13T00:16:04Z- handleDepth()-Depth_Keller msg: 1613175364660 epoch sec; seqNo:11108; depth 22.79 2021-02-13T00:16:04Z- handleDvl() - RDI_Pathfinder msg: 1613175364.68 epoch sec; seqNo:5500 2021-02-13T00:16:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 95.48 , 67.44 , 108.87 2021-02-13T00:16:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , -0.10 2021-02-13T00:16:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175364.69 epoch sec; seqNo:11108; 4058752.92 north; 592256.81 east 2021-02-13T00:16:05Z- AHRS_M2 msg: 1613175365.06 epoch sec; seqNo:11016 2021-02-13T00:16:05Z- AHRS_M2 msg: 0.72 phi; -0.38 theta; -0.23 psi 2021-02-13T00:16:05Z- handleDepth()-Depth_Keller msg: 1613175365064 epoch sec; seqNo:11109; depth 22.94 2021-02-13T00:16:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175365.08 epoch sec; seqNo:11109; 4058752.92 north; 592256.81 east 2021-02-13T00:16:05Z- AHRS_M2 msg: 1613175365.46 epoch sec; seqNo:11017 2021-02-13T00:16:05Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.23 psi 2021-02-13T00:16:05Z- handleDepth()-Depth_Keller msg: 1613175365468 epoch sec; seqNo:11110; depth 23.07 2021-02-13T00:16:05Z- handleDvl() - RDI_Pathfinder msg: 1613175365.48 epoch sec; seqNo:5501 2021-02-13T00:16:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 97.99 , 65.81 , 106.51 2021-02-13T00:16:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , -0.10 2021-02-13T00:16:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175365.49 epoch sec; seqNo:11110; 4058753.30 north; 592256.80 east 2021-02-13T00:16:05Z- AHRS_M2 msg: 1613175365.87 epoch sec; seqNo:11018 2021-02-13T00:16:05Z- AHRS_M2 msg: 0.71 phi; -0.37 theta; -0.23 psi 2021-02-13T00:16:05Z- handleDepth()-Depth_Keller msg: 1613175365878 epoch sec; seqNo:11111; depth 23.22 2021-02-13T00:16:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:05Z- handleDvl() - RDI_Pathfinder msg: 1613175365.88 epoch sec; seqNo:5502 2021-02-13T00:16:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , -0.10 2021-02-13T00:16:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175365.89 epoch sec; seqNo:11111; 4058753.68 north; 592256.80 east 2021-02-13T00:16:06Z- AHRS_M2 msg: 1613175366.27 epoch sec; seqNo:11019 2021-02-13T00:16:06Z- AHRS_M2 msg: 0.71 phi; -0.37 theta; -0.23 psi 2021-02-13T00:16:06Z- handleDepth()-Depth_Keller msg: 1613175366276 epoch sec; seqNo:11112; depth 23.36 2021-02-13T00:16:06Z- handleDvl() - RDI_Pathfinder msg: 1613175366.29 epoch sec; seqNo:5503 2021-02-13T00:16:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.94 , 98.12 , 67.44 , 107.86 2021-02-13T00:16:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.10 , -0.04 2021-02-13T00:16:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175366.31 epoch sec; seqNo:11112; 4058754.05 north; 592256.80 east 2021-02-13T00:16:06Z- AHRS_M2 msg: 1613175366.67 epoch sec; seqNo:11020 2021-02-13T00:16:06Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.23 psi 2021-02-13T00:16:06Z- handleDepth()-Depth_Keller msg: 1613175366680 epoch sec; seqNo:11113; depth 23.51 2021-02-13T00:16:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175366.69 epoch sec; seqNo:11113; 4058754.07 north; 592258.02 east 2021-02-13T00:16:07Z- AHRS_M2 msg: 1613175367.08 epoch sec; seqNo:11021 2021-02-13T00:16:07Z- AHRS_M2 msg: 0.70 phi; -0.36 theta; -0.23 psi 2021-02-13T00:16:07Z- handleDepth()-Depth_Keller msg: 1613175367089 epoch sec; seqNo:11114; depth 23.63 2021-02-13T00:16:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:07Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:07Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:07Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:07Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:07Z- handleDvl() - RDI_Pathfinder msg: 1613175367.09 epoch sec; seqNo:5504 2021-02-13T00:16:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.10 , -0.04 2021-02-13T00:16:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175367.10 epoch sec; seqNo:11114; 4058754.45 north; 592258.01 east 2021-02-13T00:16:07Z- LcmTrn::handleMessages() - handling 47 messages took 0.69 ms and 133 calls... 2021-02-13T00:16:07Z- waiting for 1613175364.87, time is 1613175367.10 2021-02-13T00:16:07Z- LcmTrn::updateTrn() equating pose 1613175367.10 and meas times 1613175367.10 2021-02-13T00:16:07Z- LcmTrn::updateTrn() >>>> pose time 1613175367.10, meas time 1613175367.10 <<<< 2021-02-13T00:16:07Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175367.10, ping # 0. 2021-02-13T00:16:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175367.10 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:16:07Z- TerrainNav::motionUpdate() - currEstimate = (4058754, 592258.01, 23.63) 2021-02-13T00:16:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:16:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:16:07Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:16:07Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:16:07Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175367.10 sec.; No relevant navigation data available 2021-02-13T00:16:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:16:07Z- TNavBF::mmse selected from filter -1 2021-02-13T00:16:07Z- variances([0], [2], and psi_berg) = 711.195543 833.004190 0.000000 2021-02-13T00:16:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:16:07Z- LcmTrn::publishEstimates() - MLE : 4058753.95 592307.51 23.63 2021-02-13T00:16:07Z- LcmTrn::publishEstimates() - MMSE : 4058754.45 592258.01 23.63 2021-02-13T00:16:07Z- LcmTrn::publishEstimates() - COVAR: 711.20 833.00 1.05 0.00 2021-02-13T00:16:07Z- AHRS_M2 msg: 1613175367.49 epoch sec; seqNo:11022 2021-02-13T00:16:07Z- AHRS_M2 msg: 0.69 phi; -0.36 theta; -0.23 psi 2021-02-13T00:16:07Z- handleDepth()-Depth_Keller msg: 1613175367497 epoch sec; seqNo:11115; depth 23.79 2021-02-13T00:16:07Z- handleDvl() - RDI_Pathfinder msg: 1613175367.51 epoch sec; seqNo:5505 2021-02-13T00:16:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.53 , 98.05 , 65.40 , 105.36 2021-02-13T00:16:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.11 , 0.03 2021-02-13T00:16:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175367.52 epoch sec; seqNo:11115; 4058754.83 north; 592258.01 east 2021-02-13T00:16:07Z- AHRS_M2 msg: 1613175367.89 epoch sec; seqNo:11023 2021-02-13T00:16:07Z- AHRS_M2 msg: 0.69 phi; -0.36 theta; -0.22 psi 2021-02-13T00:16:07Z- handleDepth()-Depth_Keller msg: 1613175367892 epoch sec; seqNo:11116; depth 23.94 2021-02-13T00:16:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175367.92 epoch sec; seqNo:11116; 4058755.20 north; 592258.00 east 2021-02-13T00:16:08Z- AHRS_M2 msg: 1613175368.29 epoch sec; seqNo:11024 2021-02-13T00:16:08Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.22 psi 2021-02-13T00:16:08Z- handleDepth()-Depth_Keller msg: 1613175368296 epoch sec; seqNo:11117; depth 24.06 2021-02-13T00:16:08Z- handleDvl() - RDI_Pathfinder msg: 1613175368.31 epoch sec; seqNo:5506 2021-02-13T00:16:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.53 , 98.19 , 65.81 , 104.27 2021-02-13T00:16:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , 0.05 2021-02-13T00:16:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175368.33 epoch sec; seqNo:11117; 4058755.20 north; 592258.00 east 2021-02-13T00:16:08Z- AHRS_M2 msg: 1613175368.70 epoch sec; seqNo:11025 2021-02-13T00:16:08Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.21 psi 2021-02-13T00:16:08Z- handleDepth()-Depth_Keller msg: 1613175368700 epoch sec; seqNo:11118; depth 24.22 2021-02-13T00:16:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175368.71 epoch sec; seqNo:11118; 4058755.58 north; 592258.00 east 2021-02-13T00:16:09Z- AHRS_M2 msg: 1613175369.10 epoch sec; seqNo:11026 2021-02-13T00:16:09Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.21 psi 2021-02-13T00:16:09Z- handleDepth()-Depth_Keller msg: 1613175369106 epoch sec; seqNo:11119; depth 24.34 2021-02-13T00:16:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175369.12 epoch sec; seqNo:11119; 4058755.98 north; 592259.22 east 2021-02-13T00:16:09Z- AHRS_M2 msg: 1613175369.50 epoch sec; seqNo:11027 2021-02-13T00:16:09Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.20 psi 2021-02-13T00:16:09Z- handleDepth()-Depth_Keller msg: 1613175369508 epoch sec; seqNo:11120; depth 24.48 2021-02-13T00:16:09Z- handleDvl() - RDI_Pathfinder msg: 1613175369.53 epoch sec; seqNo:5507 2021-02-13T00:16:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.53 , 92.10 , 65.40 , 101.84 2021-02-13T00:16:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , 0.04 2021-02-13T00:16:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175369.54 epoch sec; seqNo:11120; 4058755.98 north; 592259.22 east 2021-02-13T00:16:09Z- AHRS_M2 msg: 1613175369.91 epoch sec; seqNo:11028 2021-02-13T00:16:09Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.20 psi 2021-02-13T00:16:09Z- handleDepth()-Depth_Keller msg: 1613175369912 epoch sec; seqNo:11121; depth 24.61 2021-02-13T00:16:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175369.92 epoch sec; seqNo:11121; 4058756.35 north; 592259.21 east 2021-02-13T00:16:10Z- AHRS_M2 msg: 1613175370.31 epoch sec; seqNo:11029 2021-02-13T00:16:10Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.20 psi 2021-02-13T00:16:10Z- handleDepth()-Depth_Keller msg: 1613175370317 epoch sec; seqNo:11122; depth 24.74 2021-02-13T00:16:10Z- handleDvl() - RDI_Pathfinder msg: 1613175370.33 epoch sec; seqNo:5508 2021-02-13T00:16:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.94 , 89.87 , 64.17 , 100.83 2021-02-13T00:16:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , -0.03 2021-02-13T00:16:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175370.34 epoch sec; seqNo:11122; 4058756.73 north; 592259.21 east 2021-02-13T00:16:10Z- AHRS_M2 msg: 1613175370.72 epoch sec; seqNo:11030 2021-02-13T00:16:10Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.19 psi 2021-02-13T00:16:10Z- handleDepth()-Depth_Keller msg: 1613175370722 epoch sec; seqNo:11123; depth 24.87 2021-02-13T00:16:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:10Z- handleDvl() - RDI_Pathfinder msg: 1613175370.72 epoch sec; seqNo:5509 2021-02-13T00:16:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , -0.03 2021-02-13T00:16:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175370.73 epoch sec; seqNo:11123; 4058756.73 north; 592259.21 east 2021-02-13T00:16:11Z- AHRS_M2 msg: 1613175371.15 epoch sec; seqNo:11031 2021-02-13T00:16:11Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-13T00:16:11Z- handleDepth()-Depth_Keller msg: 1613175371154 epoch sec; seqNo:11124; depth 25.00 2021-02-13T00:16:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175371.16 epoch sec; seqNo:11124; 4058757.11 north; 592259.20 east 2021-02-13T00:16:11Z- AHRS_M2 msg: 1613175371.52 epoch sec; seqNo:11032 2021-02-13T00:16:11Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-13T00:16:11Z- handleDepth()-Depth_Keller msg: 1613175371533 epoch sec; seqNo:11125; depth 25.15 2021-02-13T00:16:11Z- handleDvl() - RDI_Pathfinder msg: 1613175371.55 epoch sec; seqNo:5510 2021-02-13T00:16:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.71 , 91.29 , 63.76 , 98.86 2021-02-13T00:16:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , -0.07 2021-02-13T00:16:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175371.56 epoch sec; seqNo:11125; 4058757.50 north; 592260.42 east 2021-02-13T00:16:11Z- AHRS_M2 msg: 1613175371.93 epoch sec; seqNo:11033 2021-02-13T00:16:11Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-13T00:16:11Z- handleDepth()-Depth_Keller msg: 1613175371932 epoch sec; seqNo:11126; depth 25.28 2021-02-13T00:16:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:11Z- handleDvl() - RDI_Pathfinder msg: 1613175371.93 epoch sec; seqNo:5511 2021-02-13T00:16:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , -0.07 2021-02-13T00:16:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175371.95 epoch sec; seqNo:11126; 4058757.50 north; 592260.42 east 2021-02-13T00:16:12Z- LcmTrn::handleMessages() - handling 43 messages took 0.58 ms and 132 calls... 2021-02-13T00:16:12Z- waiting for 1613175370.10, time is 1613175371.95 2021-02-13T00:16:12Z- LcmTrn::updateTrn() equating pose 1613175371.95 and meas times 1613175371.95 2021-02-13T00:16:12Z- LcmTrn::updateTrn() >>>> pose time 1613175371.95, meas time 1613175371.95 <<<< 2021-02-13T00:16:12Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175371.95, ping # 0. 2021-02-13T00:16:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175371.95 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:16:12Z- TerrainNav::motionUpdate() - currEstimate = (4058758, 592260.42, 25.28) 2021-02-13T00:16:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:16:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:16:12Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:16:12Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:16:12Z- TNavBF::Projecting Measurements 2021-02-13T00:16:12Z- TNavFilter::Measurement from time = 1613175371.95 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:16:12Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:16:12Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:16:12Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:16:12Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:16:12Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:16:12Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:16:12Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:16:12Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 711.1 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:16:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:16:12Z- TNavBF::mmse selected from filter -1 2021-02-13T00:16:12Z- variances([0], [2], and psi_berg) = 711.135253 833.004155 0.000000 2021-02-13T00:16:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:16:12Z- LcmTrn::publishEstimates() - MLE : 4058757.00 592302.92 25.28 2021-02-13T00:16:12Z- LcmTrn::publishEstimates() - MMSE : 4058757.50 592260.42 25.28 2021-02-13T00:16:12Z- LcmTrn::publishEstimates() - COVAR: 711.14 833.00 1.25 0.00 2021-02-13T00:16:12Z- AHRS_M2 msg: 1613175372.33 epoch sec; seqNo:11034 2021-02-13T00:16:12Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-13T00:16:12Z- handleDepth()-Depth_Keller msg: 1613175372341 epoch sec; seqNo:11127; depth 25.38 2021-02-13T00:16:12Z- handleDvl() - RDI_Pathfinder msg: 1613175372.35 epoch sec; seqNo:5512 2021-02-13T00:16:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.71 , 89.20 , 64.99 , 97.85 2021-02-13T00:16:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , -0.08 2021-02-13T00:16:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175372.36 epoch sec; seqNo:11127; 4058757.88 north; 592260.42 east 2021-02-13T00:16:12Z- AHRS_M2 msg: 1613175372.74 epoch sec; seqNo:11035 2021-02-13T00:16:12Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-13T00:16:12Z- handleDepth()-Depth_Keller msg: 1613175372740 epoch sec; seqNo:11128; depth 25.52 2021-02-13T00:16:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175372.76 epoch sec; seqNo:11128; 4058758.26 north; 592260.41 east 2021-02-13T00:16:13Z- AHRS_M2 msg: 1613175373.14 epoch sec; seqNo:11036 2021-02-13T00:16:13Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.19 psi 2021-02-13T00:16:13Z- handleDepth()-Depth_Keller msg: 1613175373150 epoch sec; seqNo:11129; depth 25.64 2021-02-13T00:16:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:13Z- handleDvl() - RDI_Pathfinder msg: 1613175373.15 epoch sec; seqNo:5513 2021-02-13T00:16:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , -0.08 2021-02-13T00:16:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175373.17 epoch sec; seqNo:11129; 4058758.64 north; 592260.41 east 2021-02-13T00:16:13Z- AHRS_M2 msg: 1613175373.54 epoch sec; seqNo:11037 2021-02-13T00:16:13Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.19 psi 2021-02-13T00:16:13Z- handleDepth()-Depth_Keller msg: 1613175373553 epoch sec; seqNo:11130; depth 25.77 2021-02-13T00:16:13Z- handleDvl() - RDI_Pathfinder msg: 1613175373.57 epoch sec; seqNo:5514 2021-02-13T00:16:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.90 , 88.18 , 64.18 , 97.92 2021-02-13T00:16:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , -0.03 2021-02-13T00:16:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175373.58 epoch sec; seqNo:11130; 4058758.64 north; 592260.41 east 2021-02-13T00:16:13Z- AHRS_M2 msg: 1613175373.95 epoch sec; seqNo:11038 2021-02-13T00:16:13Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.19 psi 2021-02-13T00:16:13Z- handleDepth()-Depth_Keller msg: 1613175373954 epoch sec; seqNo:11131; depth 25.89 2021-02-13T00:16:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175373.96 epoch sec; seqNo:11131; 4058759.03 north; 592261.63 east 2021-02-13T00:16:14Z- AHRS_M2 msg: 1613175374.35 epoch sec; seqNo:11039 2021-02-13T00:16:14Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-13T00:16:14Z- handleDepth()-Depth_Keller msg: 1613175374366 epoch sec; seqNo:11132; depth 26.01 2021-02-13T00:16:14Z- handleDvl() - RDI_Pathfinder msg: 1613175374.38 epoch sec; seqNo:5515 2021-02-13T00:16:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.49 , 88.18 , 63.36 , 96.84 2021-02-13T00:16:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.11 , 0.01 2021-02-13T00:16:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175374.39 epoch sec; seqNo:11132; 4058759.41 north; 592261.62 east 2021-02-13T00:16:14Z- AHRS_M2 msg: 1613175374.76 epoch sec; seqNo:11040 2021-02-13T00:16:14Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-13T00:16:14Z- handleDepth()-Depth_Keller msg: 1613175374765 epoch sec; seqNo:11133; depth 26.15 2021-02-13T00:16:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175374.78 epoch sec; seqNo:11133; 4058759.79 north; 592261.62 east 2021-02-13T00:16:15Z- AHRS_M2 msg: 1613175375.16 epoch sec; seqNo:11041 2021-02-13T00:16:15Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-13T00:16:15Z- handleDepth()-Depth_Keller msg: 1613175375166 epoch sec; seqNo:11134; depth 26.27 2021-02-13T00:16:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175375.17 epoch sec; seqNo:11134; 4058759.79 north; 592261.62 east 2021-02-13T00:16:15Z- AHRS_M2 msg: 1613175375.56 epoch sec; seqNo:11042 2021-02-13T00:16:15Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-13T00:16:15Z- handleDepth()-Depth_Keller msg: 1613175375578 epoch sec; seqNo:11135; depth 26.38 2021-02-13T00:16:15Z- handleDvl() - RDI_Pathfinder msg: 1613175375.59 epoch sec; seqNo:5516 2021-02-13T00:16:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.49 , 87.17 , 63.36 , 97.99 2021-02-13T00:16:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , 0.04 2021-02-13T00:16:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175375.60 epoch sec; seqNo:11135; 4058760.17 north; 592261.61 east 2021-02-13T00:16:15Z- AHRS_M2 msg: 1613175375.97 epoch sec; seqNo:11043 2021-02-13T00:16:15Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-13T00:16:15Z- handleDepth()-Depth_Keller msg: 1613175375972 epoch sec; seqNo:11136; depth 26.51 2021-02-13T00:16:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175375.98 epoch sec; seqNo:11136; 4058760.55 north; 592261.61 east 2021-02-13T00:16:16Z- AHRS_M2 msg: 1613175376.37 epoch sec; seqNo:11044 2021-02-13T00:16:16Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.18 psi 2021-02-13T00:16:16Z- handleDepth()-Depth_Keller msg: 1613175376381 epoch sec; seqNo:11137; depth 26.63 2021-02-13T00:16:16Z- handleDvl() - RDI_Pathfinder msg: 1613175376.39 epoch sec; seqNo:5517 2021-02-13T00:16:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.49 , 0.00 , 63.36 , 97.99 2021-02-13T00:16:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , 0.04 2021-02-13T00:16:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175376.40 epoch sec; seqNo:11137; 4058760.55 north; 592261.61 east 2021-02-13T00:16:16Z- AHRS_M2 msg: 1613175376.78 epoch sec; seqNo:11045 2021-02-13T00:16:16Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.18 psi 2021-02-13T00:16:16Z- handleDepth()-Depth_Keller msg: 1613175376787 epoch sec; seqNo:11138; depth 26.75 2021-02-13T00:16:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:16Z- handleDvl() - RDI_Pathfinder msg: 1613175376.79 epoch sec; seqNo:5518 2021-02-13T00:16:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , 0.04 2021-02-13T00:16:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175376.80 epoch sec; seqNo:11138; 4058760.94 north; 592262.83 east 2021-02-13T00:16:17Z- AHRS_M2 msg: 1613175377.18 epoch sec; seqNo:11046 2021-02-13T00:16:17Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.18 psi 2021-02-13T00:16:17Z- handleDepth()-Depth_Keller msg: 1613175377189 epoch sec; seqNo:11139; depth 26.86 2021-02-13T00:16:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175377.22 epoch sec; seqNo:11139; 4058761.32 north; 592262.82 east 2021-02-13T00:16:17Z- LcmTrn::handleMessages() - handling 46 messages took 0.71 ms and 132 calls... 2021-02-13T00:16:17Z- waiting for 1613175374.95, time is 1613175377.22 2021-02-13T00:16:17Z- LcmTrn::updateTrn() >>>> pose time 1613175377.22, meas time 1613175376.79 <<<< 2021-02-13T00:16:17Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175376.79, ping # 0. 2021-02-13T00:16:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175376.79 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:16:17Z- TerrainNav::motionUpdate() - currEstimate = (4058761, 592262.82, 26.86) 2021-02-13T00:16:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:16:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:16:17Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:16:17Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:16:17Z- TNavBF::Projecting Measurements 2021-02-13T00:16:17Z- TNavFilter::Measurement from time = 1613175376.79 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:16:17Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:16:17Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:16:17Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:16:17Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:16:17Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:16:17Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:16:17Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:16:17Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 711.1 EastVariance: 833.0 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-13T00:16:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:16:17Z- TNavBF::mmse selected from filter -1 2021-02-13T00:16:17Z- variances([0], [2], and psi_berg) = 711.054643 833.004107 0.000000 2021-02-13T00:16:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:16:17Z- LcmTrn::publishEstimates() - MLE : 4058760.82 592305.32 26.86 2021-02-13T00:16:17Z- LcmTrn::publishEstimates() - MMSE : 4058761.32 592262.82 26.86 2021-02-13T00:16:17Z- LcmTrn::publishEstimates() - COVAR: 711.05 833.00 1.48 0.00 2021-02-13T00:16:17Z- AHRS_M2 msg: 1613175377.58 epoch sec; seqNo:11047 2021-02-13T00:16:17Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.17 psi 2021-02-13T00:16:17Z- handleDepth()-Depth_Keller msg: 1613175377588 epoch sec; seqNo:11140; depth 26.99 2021-02-13T00:16:17Z- handleDvl() - RDI_Pathfinder msg: 1613175377.60 epoch sec; seqNo:5519 2021-02-13T00:16:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.67 , 86.08 , 62.94 , 97.99 2021-02-13T00:16:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.03 2021-02-13T00:16:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175377.62 epoch sec; seqNo:11140; 4058761.70 north; 592262.82 east 2021-02-13T00:16:17Z- AHRS_M2 msg: 1613175377.99 epoch sec; seqNo:11048 2021-02-13T00:16:17Z- AHRS_M2 msg: 0.69 phi; -0.33 theta; -0.17 psi 2021-02-13T00:16:17Z- handleDepth()-Depth_Keller msg: 1613175377995 epoch sec; seqNo:11141; depth 27.12 2021-02-13T00:16:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:18Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:18Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:18Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:18Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:18Z- handleDvl() - RDI_Pathfinder msg: 1613175378.00 epoch sec; seqNo:5520 2021-02-13T00:16:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.03 2021-02-13T00:16:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175378.01 epoch sec; seqNo:11141; 4058761.70 north; 592262.82 east 2021-02-13T00:16:18Z- AHRS_M2 msg: 1613175378.39 epoch sec; seqNo:11049 2021-02-13T00:16:18Z- AHRS_M2 msg: 0.69 phi; -0.33 theta; -0.16 psi 2021-02-13T00:16:18Z- handleDepth()-Depth_Keller msg: 1613175378398 epoch sec; seqNo:11142; depth 27.22 2021-02-13T00:16:18Z- handleDvl() - RDI_Pathfinder msg: 1613175378.41 epoch sec; seqNo:5521 2021-02-13T00:16:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 0.00 , 63.76 , 97.99 2021-02-13T00:16:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , 0.00 2021-02-13T00:16:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175378.42 epoch sec; seqNo:11142; 4058762.08 north; 592262.81 east 2021-02-13T00:16:18Z- AHRS_M2 msg: 1613175378.80 epoch sec; seqNo:11050 2021-02-13T00:16:18Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.16 psi 2021-02-13T00:16:18Z- handleDepth()-Depth_Keller msg: 1613175378803 epoch sec; seqNo:11143; depth 27.36 2021-02-13T00:16:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175378.83 epoch sec; seqNo:11143; 4058762.46 north; 592262.81 east 2021-02-13T00:16:19Z- AHRS_M2 msg: 1613175379.20 epoch sec; seqNo:11051 2021-02-13T00:16:19Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.16 psi 2021-02-13T00:16:19Z- handleDepth()-Depth_Keller msg: 1613175379218 epoch sec; seqNo:11144; depth 27.49 2021-02-13T00:16:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:19Z- handleDvl() - RDI_Pathfinder msg: 1613175379.23 epoch sec; seqNo:5522 2021-02-13T00:16:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , 0.00 2021-02-13T00:16:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175379.25 epoch sec; seqNo:11144; 4058762.47 north; 592264.03 east 2021-02-13T00:16:19Z- AHRS_M2 msg: 1613175379.61 epoch sec; seqNo:11052 2021-02-13T00:16:19Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.15 psi 2021-02-13T00:16:19Z- handleDepth()-Depth_Keller msg: 1613175379615 epoch sec; seqNo:11145; depth 27.63 2021-02-13T00:16:19Z- handleDvl() - RDI_Pathfinder msg: 1613175379.63 epoch sec; seqNo:5523 2021-02-13T00:16:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.08 , 83.93 , 62.13 , 97.99 2021-02-13T00:16:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , -0.02 2021-02-13T00:16:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175379.64 epoch sec; seqNo:11145; 4058762.85 north; 592264.03 east 2021-02-13T00:16:20Z- AHRS_M2 msg: 1613175380.01 epoch sec; seqNo:11053 2021-02-13T00:16:20Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.16 psi 2021-02-13T00:16:20Z- handleDepth()-Depth_Keller msg: 1613175380017 epoch sec; seqNo:11146; depth 27.75 2021-02-13T00:16:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175380.03 epoch sec; seqNo:11146; 4058763.23 north; 592264.02 east 2021-02-13T00:16:20Z- AHRS_M2 msg: 1613175380.41 epoch sec; seqNo:11054 2021-02-13T00:16:20Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.16 psi 2021-02-13T00:16:20Z- handleDepth()-Depth_Keller msg: 1613175380416 epoch sec; seqNo:11147; depth 27.86 2021-02-13T00:16:20Z- handleDvl() - RDI_Pathfinder msg: 1613175380.43 epoch sec; seqNo:5524 2021-02-13T00:16:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.67 , 83.93 , 62.13 , 97.99 2021-02-13T00:16:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , -0.05 2021-02-13T00:16:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175380.44 epoch sec; seqNo:11147; 4058763.60 north; 592264.02 east 2021-02-13T00:16:20Z- AHRS_M2 msg: 1613175380.82 epoch sec; seqNo:11055 2021-02-13T00:16:20Z- AHRS_M2 msg: 0.72 phi; -0.33 theta; -0.16 psi 2021-02-13T00:16:20Z- handleDepth()-Depth_Keller msg: 1613175380823 epoch sec; seqNo:11148; depth 28.00 2021-02-13T00:16:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175380.84 epoch sec; seqNo:11148; 4058763.60 north; 592264.02 east 2021-02-13T00:16:21Z- AHRS_M2 msg: 1613175381.24 epoch sec; seqNo:11056 2021-02-13T00:16:21Z- AHRS_M2 msg: 0.72 phi; -0.33 theta; -0.16 psi 2021-02-13T00:16:21Z- handleDepth()-Depth_Keller msg: 1613175381246 epoch sec; seqNo:11149; depth 28.12 2021-02-13T00:16:21Z- handleDvl() - RDI_Pathfinder msg: 1613175381.26 epoch sec; seqNo:5525 2021-02-13T00:16:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.67 , 83.93 , 61.72 , 97.99 2021-02-13T00:16:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.07 , -0.06 2021-02-13T00:16:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175381.27 epoch sec; seqNo:11149; 4058763.98 north; 592264.01 east 2021-02-13T00:16:21Z- AHRS_M2 msg: 1613175381.62 epoch sec; seqNo:11057 2021-02-13T00:16:21Z- AHRS_M2 msg: 0.73 phi; -0.33 theta; -0.16 psi 2021-02-13T00:16:21Z- handleDepth()-Depth_Keller msg: 1613175381628 epoch sec; seqNo:11150; depth 28.25 2021-02-13T00:16:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:21Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:21Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:21Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:21Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:21Z- handleDvl() - RDI_Pathfinder msg: 1613175381.63 epoch sec; seqNo:5526 2021-02-13T00:16:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.07 , -0.06 2021-02-13T00:16:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175381.64 epoch sec; seqNo:11150; 4058764.38 north; 592265.23 east 2021-02-13T00:16:22Z- AHRS_M2 msg: 1613175382.03 epoch sec; seqNo:11058 2021-02-13T00:16:22Z- AHRS_M2 msg: 0.73 phi; -0.33 theta; -0.16 psi 2021-02-13T00:16:22Z- handleDepth()-Depth_Keller msg: 1613175382033 epoch sec; seqNo:11151; depth 28.37 2021-02-13T00:16:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175382.04 epoch sec; seqNo:11151; 4058764.38 north; 592265.23 east 2021-02-13T00:16:22Z- LcmTrn::handleMessages() - handling 44 messages took 0.59 ms and 132 calls... 2021-02-13T00:16:22Z- waiting for 1613175380.22, time is 1613175382.04 2021-02-13T00:16:22Z- LcmTrn::updateTrn() >>>> pose time 1613175382.04, meas time 1613175381.63 <<<< 2021-02-13T00:16:22Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175381.63, ping # 0. 2021-02-13T00:16:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175381.63 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:16:22Z- TerrainNav::motionUpdate() - currEstimate = (4058764, 592265.23, 28.37) 2021-02-13T00:16:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:16:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:16:22Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:16:22Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:16:22Z- TNavBF::Projecting Measurements 2021-02-13T00:16:22Z- TNavFilter::Measurement from time = 1613175381.63 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:16:22Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:16:22Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:16:22Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:16:22Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:16:22Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:16:22Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:16:22Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:16:22Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 711.0 EastVariance: 833.0 DepthVariance: 1.7 this->DepthBiasCov: 1.7 2021-02-13T00:16:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:16:22Z- TNavBF::mmse selected from filter -1 2021-02-13T00:16:22Z- variances([0], [2], and psi_berg) = 710.979510 833.004063 0.000000 2021-02-13T00:16:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:16:22Z- LcmTrn::publishEstimates() - MLE : 4058763.88 592314.73 28.37 2021-02-13T00:16:22Z- LcmTrn::publishEstimates() - MMSE : 4058764.38 592265.23 28.37 2021-02-13T00:16:22Z- LcmTrn::publishEstimates() - COVAR: 710.98 833.00 1.68 0.00 2021-02-13T00:16:22Z- AHRS_M2 msg: 1613175382.43 epoch sec; seqNo:11059 2021-02-13T00:16:22Z- AHRS_M2 msg: 0.73 phi; -0.33 theta; -0.16 psi 2021-02-13T00:16:22Z- handleDepth()-Depth_Keller msg: 1613175382438 epoch sec; seqNo:11152; depth 28.50 2021-02-13T00:16:22Z- handleDvl() - RDI_Pathfinder msg: 1613175382.45 epoch sec; seqNo:5527 2021-02-13T00:16:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.26 , 83.93 , 61.72 , 97.99 2021-02-13T00:16:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.09 , -0.04 2021-02-13T00:16:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175382.46 epoch sec; seqNo:11152; 4058764.75 north; 592265.23 east 2021-02-13T00:16:22Z- AHRS_M2 msg: 1613175382.84 epoch sec; seqNo:11060 2021-02-13T00:16:22Z- AHRS_M2 msg: 0.74 phi; -0.33 theta; -0.16 psi 2021-02-13T00:16:22Z- handleDepth()-Depth_Keller msg: 1613175382841 epoch sec; seqNo:11153; depth 28.63 2021-02-13T00:16:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:22Z- handleDvl() - RDI_Pathfinder msg: 1613175382.84 epoch sec; seqNo:5528 2021-02-13T00:16:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.09 , -0.04 2021-02-13T00:16:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175382.86 epoch sec; seqNo:11153; 4058765.13 north; 592265.22 east 2021-02-13T00:16:23Z- AHRS_M2 msg: 1613175383.25 epoch sec; seqNo:11061 2021-02-13T00:16:23Z- AHRS_M2 msg: 0.74 phi; -0.33 theta; -0.16 psi 2021-02-13T00:16:23Z- handleDepth()-Depth_Keller msg: 1613175383252 epoch sec; seqNo:11154; depth 28.75 2021-02-13T00:16:23Z- handleDvl() - RDI_Pathfinder msg: 1613175383.26 epoch sec; seqNo:5529 2021-02-13T00:16:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.85 , 82.84 , 60.90 , 97.99 2021-02-13T00:16:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , -0.01 2021-02-13T00:16:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175383.29 epoch sec; seqNo:11154; 4058765.51 north; 592265.22 east 2021-02-13T00:16:23Z- AHRS_M2 msg: 1613175383.64 epoch sec; seqNo:11062 2021-02-13T00:16:23Z- AHRS_M2 msg: 0.74 phi; -0.33 theta; -0.16 psi 2021-02-13T00:16:23Z- handleDepth()-Depth_Keller msg: 1613175383650 epoch sec; seqNo:11155; depth 28.88 2021-02-13T00:16:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175383.66 epoch sec; seqNo:11155; 4058765.51 north; 592265.22 east 2021-02-13T00:16:24Z- AHRS_M2 msg: 1613175384.05 epoch sec; seqNo:11063 2021-02-13T00:16:24Z- AHRS_M2 msg: 0.74 phi; -0.33 theta; -0.16 psi 2021-02-13T00:16:24Z- handleDepth()-Depth_Keller msg: 1613175384052 epoch sec; seqNo:11156; depth 28.99 2021-02-13T00:16:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:24Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:24Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:24Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:24Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:24Z- handleDvl() - RDI_Pathfinder msg: 1613175384.05 epoch sec; seqNo:5530 2021-02-13T00:16:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , -0.01 2021-02-13T00:16:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175384.07 epoch sec; seqNo:11156; 4058765.90 north; 592266.44 east 2021-02-13T00:16:24Z- AHRS_M2 msg: 1613175384.45 epoch sec; seqNo:11064 2021-02-13T00:16:24Z- AHRS_M2 msg: 0.74 phi; -0.33 theta; -0.16 psi 2021-02-13T00:16:24Z- handleDepth()-Depth_Keller msg: 1613175384456 epoch sec; seqNo:11157; depth 29.11 2021-02-13T00:16:24Z- handleDvl() - RDI_Pathfinder msg: 1613175384.47 epoch sec; seqNo:5531 2021-02-13T00:16:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.85 , 82.84 , 61.72 , 97.99 2021-02-13T00:16:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.02 2021-02-13T00:16:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175384.49 epoch sec; seqNo:11157; 4058766.28 north; 592266.43 east 2021-02-13T00:16:24Z- AHRS_M2 msg: 1613175384.86 epoch sec; seqNo:11065 2021-02-13T00:16:24Z- AHRS_M2 msg: 0.74 phi; -0.33 theta; -0.16 psi 2021-02-13T00:16:24Z- handleDepth()-Depth_Keller msg: 1613175384863 epoch sec; seqNo:11158; depth 29.25 2021-02-13T00:16:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175384.87 epoch sec; seqNo:11158; 4058766.28 north; 592266.43 east 2021-02-13T00:16:25Z- AHRS_M2 msg: 1613175385.26 epoch sec; seqNo:11066 2021-02-13T00:16:25Z- AHRS_M2 msg: 0.74 phi; -0.32 theta; -0.17 psi 2021-02-13T00:16:25Z- handleDepth()-Depth_Keller msg: 1613175385266 epoch sec; seqNo:11159; depth 29.36 2021-02-13T00:16:25Z- handleDvl() - RDI_Pathfinder msg: 1613175385.28 epoch sec; seqNo:5532 2021-02-13T00:16:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.03 , 81.76 , 61.31 , 97.99 2021-02-13T00:16:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , 0.03 2021-02-13T00:16:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175385.29 epoch sec; seqNo:11159; 4058766.66 north; 592266.43 east 2021-02-13T00:16:25Z- AHRS_M2 msg: 1613175385.66 epoch sec; seqNo:11067 2021-02-13T00:16:25Z- AHRS_M2 msg: 0.74 phi; -0.31 theta; -0.17 psi 2021-02-13T00:16:25Z- handleDepth()-Depth_Keller msg: 1613175385678 epoch sec; seqNo:11160; depth 29.51 2021-02-13T00:16:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175385.69 epoch sec; seqNo:11160; 4058767.04 north; 592266.42 east 2021-02-13T00:16:26Z- AHRS_M2 msg: 1613175386.07 epoch sec; seqNo:11068 2021-02-13T00:16:26Z- AHRS_M2 msg: 0.74 phi; -0.28 theta; -0.18 psi 2021-02-13T00:16:26Z- handleDepth()-Depth_Keller msg: 1613175386077 epoch sec; seqNo:11161; depth 29.64 2021-02-13T00:16:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175386.09 epoch sec; seqNo:11161; 4058767.04 north; 592266.42 east 2021-02-13T00:16:26Z- AHRS_M2 msg: 1613175386.47 epoch sec; seqNo:11069 2021-02-13T00:16:26Z- AHRS_M2 msg: 0.73 phi; -0.25 theta; -0.18 psi 2021-02-13T00:16:26Z- handleDepth()-Depth_Keller msg: 1613175386477 epoch sec; seqNo:11162; depth 29.78 2021-02-13T00:16:26Z- handleDvl() - RDI_Pathfinder msg: 1613175386.49 epoch sec; seqNo:5533 2021-02-13T00:16:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:26Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:16:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175386.50 epoch sec; seqNo:11162; 4058767.43 north; 592267.64 east 2021-02-13T00:16:26Z- AHRS_M2 msg: 1613175386.88 epoch sec; seqNo:11070 2021-02-13T00:16:26Z- AHRS_M2 msg: 0.72 phi; -0.21 theta; -0.17 psi 2021-02-13T00:16:26Z- handleDepth()-Depth_Keller msg: 1613175386882 epoch sec; seqNo:11163; depth 29.91 2021-02-13T00:16:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175386.90 epoch sec; seqNo:11163; 4058767.81 north; 592267.64 east 2021-02-13T00:16:27Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-13T00:16:27Z- waiting for 1613175385.04, time is 1613175386.90 2021-02-13T00:16:27Z- LcmTrn::updateTrn() >>>> pose time 1613175386.90, meas time 1613175386.49 <<<< 2021-02-13T00:16:27Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175386.49, ping # 0. 2021-02-13T00:16:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175386.49 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:16:27Z- TerrainNav::motionUpdate() - currEstimate = (4058768, 592267.64, 29.91) 2021-02-13T00:16:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:16:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:16:27Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:16:27Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:16:27Z- TNavBF::Projecting Measurements 2021-02-13T00:16:27Z- TNavFilter::Measurement from time = 1613175386.49 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:16:27Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:16:27Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:16:27Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:16:27Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:16:27Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:16:27Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:16:27Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:16:27Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 710.9 EastVariance: 833.0 DepthVariance: 1.9 this->DepthBiasCov: 1.9 2021-02-13T00:16:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:16:27Z- TNavBF::mmse selected from filter -1 2021-02-13T00:16:27Z- variances([0], [2], and psi_berg) = 710.901587 833.004017 0.000000 2021-02-13T00:16:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:16:27Z- LcmTrn::publishEstimates() - MLE : 4058767.31 592317.14 29.91 2021-02-13T00:16:27Z- LcmTrn::publishEstimates() - MMSE : 4058767.81 592267.64 29.91 2021-02-13T00:16:27Z- LcmTrn::publishEstimates() - COVAR: 710.90 833.00 1.89 0.00 2021-02-13T00:16:27Z- AHRS_M2 msg: 1613175387.29 epoch sec; seqNo:11071 2021-02-13T00:16:27Z- AHRS_M2 msg: 0.72 phi; -0.16 theta; -0.17 psi 2021-02-13T00:16:27Z- handleDepth()-Depth_Keller msg: 1613175387290 epoch sec; seqNo:11164; depth 30.05 2021-02-13T00:16:27Z- handleDvl() - RDI_Pathfinder msg: 1613175387.31 epoch sec; seqNo:5534 2021-02-13T00:16:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.58 , 80.74 , 62.94 , 84.39 2021-02-13T00:16:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , 0.00 2021-02-13T00:16:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175387.32 epoch sec; seqNo:11164; 4058767.81 north; 592267.64 east 2021-02-13T00:16:27Z- AHRS_M2 msg: 1613175387.68 epoch sec; seqNo:11072 2021-02-13T00:16:27Z- AHRS_M2 msg: 0.71 phi; -0.12 theta; -0.16 psi 2021-02-13T00:16:27Z- handleDepth()-Depth_Keller msg: 1613175387693 epoch sec; seqNo:11165; depth 30.15 2021-02-13T00:16:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:27Z- handleDvl() - RDI_Pathfinder msg: 1613175387.69 epoch sec; seqNo:5535 2021-02-13T00:16:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , 0.00 2021-02-13T00:16:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175387.71 epoch sec; seqNo:11165; 4058768.19 north; 592267.63 east 2021-02-13T00:16:28Z- AHRS_M2 msg: 1613175388.09 epoch sec; seqNo:11073 2021-02-13T00:16:28Z- AHRS_M2 msg: 0.70 phi; -0.07 theta; -0.16 psi 2021-02-13T00:16:28Z- handleDepth()-Depth_Keller msg: 1613175388101 epoch sec; seqNo:11166; depth 30.26 2021-02-13T00:16:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175388.11 epoch sec; seqNo:11166; 4058768.57 north; 592267.63 east 2021-02-13T00:16:28Z- AHRS_M2 msg: 1613175388.49 epoch sec; seqNo:11074 2021-02-13T00:16:28Z- AHRS_M2 msg: 0.70 phi; -0.03 theta; -0.16 psi 2021-02-13T00:16:28Z- handleDepth()-Depth_Keller msg: 1613175388496 epoch sec; seqNo:11167; depth 30.34 2021-02-13T00:16:28Z- handleDvl() - RDI_Pathfinder msg: 1613175388.51 epoch sec; seqNo:5536 2021-02-13T00:16:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.94 , 78.51 , 64.99 , 78.51 2021-02-13T00:16:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , -0.03 2021-02-13T00:16:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175388.52 epoch sec; seqNo:11167; 4058768.58 north; 592268.85 east 2021-02-13T00:16:28Z- AHRS_M2 msg: 1613175388.90 epoch sec; seqNo:11075 2021-02-13T00:16:28Z- AHRS_M2 msg: 0.69 phi; 0.01 theta; -0.16 psi 2021-02-13T00:16:28Z- handleDepth()-Depth_Keller msg: 1613175388900 epoch sec; seqNo:11168; depth 30.41 2021-02-13T00:16:28Z- handleDvl() - RDI_Pathfinder msg: 1613175388.92 epoch sec; seqNo:5537 2021-02-13T00:16:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.04 , 0.00 , 0.00 , 67.81 2021-02-13T00:16:28Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:16:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175388.95 epoch sec; seqNo:11168; 4058768.96 north; 592268.84 east 2021-02-13T00:16:29Z- AHRS_M2 msg: 1613175389.30 epoch sec; seqNo:11076 2021-02-13T00:16:29Z- AHRS_M2 msg: 0.68 phi; 0.05 theta; -0.16 psi 2021-02-13T00:16:29Z- handleDepth()-Depth_Keller msg: 1613175389309 epoch sec; seqNo:11169; depth 30.48 2021-02-13T00:16:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175389.32 epoch sec; seqNo:11169; 4058769.34 north; 592268.84 east 2021-02-13T00:16:29Z- AHRS_M2 msg: 1613175389.70 epoch sec; seqNo:11077 2021-02-13T00:16:29Z- AHRS_M2 msg: 0.67 phi; 0.09 theta; -0.16 psi 2021-02-13T00:16:29Z- handleDepth()-Depth_Keller msg: 1613175389709 epoch sec; seqNo:11170; depth 30.54 2021-02-13T00:16:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175389.72 epoch sec; seqNo:11170; 4058769.72 north; 592268.84 east 2021-02-13T00:16:30Z- AHRS_M2 msg: 1613175390.11 epoch sec; seqNo:11078 2021-02-13T00:16:30Z- AHRS_M2 msg: 0.66 phi; 0.13 theta; -0.16 psi 2021-02-13T00:16:30Z- handleDepth()-Depth_Keller msg: 1613175390112 epoch sec; seqNo:11171; depth 30.55 2021-02-13T00:16:30Z- handleDvl() - RDI_Pathfinder msg: 1613175390.14 epoch sec; seqNo:5538 2021-02-13T00:16:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.90 , 0.00 , 0.00 , 64.68 2021-02-13T00:16:30Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:16:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175390.15 epoch sec; seqNo:11171; 4058769.72 north; 592268.84 east 2021-02-13T00:16:30Z- AHRS_M2 msg: 1613175390.51 epoch sec; seqNo:11079 2021-02-13T00:16:30Z- AHRS_M2 msg: 0.65 phi; 0.16 theta; -0.17 psi 2021-02-13T00:16:30Z- handleDepth()-Depth_Keller msg: 1613175390516 epoch sec; seqNo:11172; depth 30.55 2021-02-13T00:16:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175390.53 epoch sec; seqNo:11172; 4058770.11 north; 592270.05 east 2021-02-13T00:16:30Z- AHRS_M2 msg: 1613175390.93 epoch sec; seqNo:11080 2021-02-13T00:16:30Z- AHRS_M2 msg: 0.64 phi; 0.19 theta; -0.17 psi 2021-02-13T00:16:30Z- handleDepth()-Depth_Keller msg: 1613175390930 epoch sec; seqNo:11173; depth 30.57 2021-02-13T00:16:30Z- handleDvl() - RDI_Pathfinder msg: 1613175390.94 epoch sec; seqNo:5539 2021-02-13T00:16:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.35 , 0.00 , 0.00 , 62.46 2021-02-13T00:16:30Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:16:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175390.97 epoch sec; seqNo:11173; 4058770.49 north; 592270.05 east 2021-02-13T00:16:31Z- AHRS_M2 msg: 1613175391.32 epoch sec; seqNo:11081 2021-02-13T00:16:31Z- AHRS_M2 msg: 0.64 phi; 0.21 theta; -0.17 psi 2021-02-13T00:16:31Z- handleDepth()-Depth_Keller msg: 1613175391327 epoch sec; seqNo:11174; depth 30.54 2021-02-13T00:16:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:31Z- handleDvl() - RDI_Pathfinder msg: 1613175391.33 epoch sec; seqNo:5540 2021-02-13T00:16:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:31Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:16:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175391.34 epoch sec; seqNo:11174; 4058770.49 north; 592270.05 east 2021-02-13T00:16:31Z- AHRS_M2 msg: 1613175391.72 epoch sec; seqNo:11082 2021-02-13T00:16:31Z- AHRS_M2 msg: 0.63 phi; 0.24 theta; -0.16 psi 2021-02-13T00:16:31Z- handleDepth()-Depth_Keller msg: 1613175391733 epoch sec; seqNo:11175; depth 30.50 2021-02-13T00:16:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175391.75 epoch sec; seqNo:11175; 4058770.87 north; 592270.04 east 2021-02-13T00:16:32Z- AHRS_M2 msg: 1613175392.13 epoch sec; seqNo:11083 2021-02-13T00:16:32Z- AHRS_M2 msg: 0.63 phi; 0.26 theta; -0.16 psi 2021-02-13T00:16:32Z- handleDepth()-Depth_Keller msg: 1613175392138 epoch sec; seqNo:11176; depth 30.48 2021-02-13T00:16:32Z- handleDvl() - RDI_Pathfinder msg: 1613175392.16 epoch sec; seqNo:5541 2021-02-13T00:16:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 65.12 , 0.00 , 0.00 , 62.02 2021-02-13T00:16:32Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:16:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175392.17 epoch sec; seqNo:11176; 4058771.25 north; 592270.04 east 2021-02-13T00:16:32Z- LcmTrn::handleMessages() - handling 47 messages took 0.75 ms and 132 calls... 2021-02-13T00:16:32Z- waiting for 1613175389.90, time is 1613175392.17 2021-02-13T00:16:32Z- LcmTrn::updateTrn() equating pose 1613175392.17 and meas times 1613175392.17 2021-02-13T00:16:32Z- LcmTrn::updateTrn() >>>> pose time 1613175392.17, meas time 1613175392.17 <<<< 2021-02-13T00:16:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175392.17 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:16:32Z- TerrainNav::motionUpdate() - currEstimate = (4058771, 592270.04, 30.48) 2021-02-13T00:16:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:16:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:16:32Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:16:32Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:16:32Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175392.17 sec.; No relevant navigation data available 2021-02-13T00:16:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:16:32Z- TNavBF::mmse selected from filter -1 2021-02-13T00:16:32Z- variances([0], [2], and psi_berg) = 710.839206 833.003980 0.000000 2021-02-13T00:16:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:16:32Z- LcmTrn::publishEstimates() - MLE : 4058770.75 592319.54 30.48 2021-02-13T00:16:32Z- LcmTrn::publishEstimates() - MMSE : 4058771.25 592270.04 30.48 2021-02-13T00:16:32Z- LcmTrn::publishEstimates() - COVAR: 710.84 833.00 2.10 0.00 2021-02-13T00:16:32Z- AHRS_M2 msg: 1613175392.53 epoch sec; seqNo:11084 2021-02-13T00:16:32Z- AHRS_M2 msg: 0.62 phi; 0.28 theta; -0.16 psi 2021-02-13T00:16:32Z- handleDepth()-Depth_Keller msg: 1613175392541 epoch sec; seqNo:11177; depth 30.42 2021-02-13T00:16:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:32Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:32Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:32Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:32Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:32Z- handleDvl() - RDI_Pathfinder msg: 1613175392.54 epoch sec; seqNo:5542 2021-02-13T00:16:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:32Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:16:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175392.55 epoch sec; seqNo:11177; 4058771.63 north; 592270.04 east 2021-02-13T00:16:32Z- AHRS_M2 msg: 1613175392.94 epoch sec; seqNo:11085 2021-02-13T00:16:32Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.16 psi 2021-02-13T00:16:32Z- handleDepth()-Depth_Keller msg: 1613175392942 epoch sec; seqNo:11178; depth 30.37 2021-02-13T00:16:32Z- handleDvl() - RDI_Pathfinder msg: 1613175392.95 epoch sec; seqNo:5543 2021-02-13T00:16:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:32Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:16:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175392.97 epoch sec; seqNo:11178; 4058771.64 north; 592271.26 east 2021-02-13T00:16:33Z- AHRS_M2 msg: 1613175393.34 epoch sec; seqNo:11086 2021-02-13T00:16:33Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-13T00:16:33Z- handleDepth()-Depth_Keller msg: 1613175393344 epoch sec; seqNo:11179; depth 30.29 2021-02-13T00:16:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175393.36 epoch sec; seqNo:11179; 4058772.02 north; 592271.25 east 2021-02-13T00:16:33Z- AHRS_M2 msg: 1613175393.74 epoch sec; seqNo:11087 2021-02-13T00:16:33Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-13T00:16:33Z- handleDepth()-Depth_Keller msg: 1613175393748 epoch sec; seqNo:11180; depth 30.23 2021-02-13T00:16:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:33Z- handleDvl() - RDI_Pathfinder msg: 1613175393.75 epoch sec; seqNo:5544 2021-02-13T00:16:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:33Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:16:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175393.76 epoch sec; seqNo:11180; 4058772.40 north; 592271.25 east 2021-02-13T00:16:34Z- AHRS_M2 msg: 1613175394.15 epoch sec; seqNo:11088 2021-02-13T00:16:34Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-13T00:16:34Z- handleDepth()-Depth_Keller msg: 1613175394155 epoch sec; seqNo:11181; depth 30.15 2021-02-13T00:16:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175394.16 epoch sec; seqNo:11181; 4058772.78 north; 592271.24 east 2021-02-13T00:16:34Z- handleDepth()-Depth_Keller msg: 1613175394577 epoch sec; seqNo:11182; depth 30.05 2021-02-13T00:16:34Z- handleDvl() - RDI_Pathfinder msg: 1613175394.59 epoch sec; seqNo:5545 2021-02-13T00:16:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.63 , 82.84 , 97.99 , 60.09 2021-02-13T00:16:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.08 2021-02-13T00:16:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175394.62 epoch sec; seqNo:11182; 4058772.78 north; 592271.24 east 2021-02-13T00:16:34Z- AHRS_M2 msg: 1613175394.96 epoch sec; seqNo:11089 2021-02-13T00:16:34Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T00:16:34Z- handleDepth()-Depth_Keller msg: 1613175394961 epoch sec; seqNo:11183; depth 29.96 2021-02-13T00:16:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:34Z- handleDvl() - RDI_Pathfinder msg: 1613175394.96 epoch sec; seqNo:5546 2021-02-13T00:16:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.08 2021-02-13T00:16:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175394.99 epoch sec; seqNo:11183; 4058773.15 north; 592271.24 east 2021-02-13T00:16:35Z- AHRS_M2 msg: 1613175395.36 epoch sec; seqNo:11090 2021-02-13T00:16:35Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T00:16:35Z- handleDepth()-Depth_Keller msg: 1613175395364 epoch sec; seqNo:11184; depth 29.85 2021-02-13T00:16:35Z- handleDvl() - RDI_Pathfinder msg: 1613175395.38 epoch sec; seqNo:5547 2021-02-13T00:16:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.86 , 82.84 , 97.99 , 60.91 2021-02-13T00:16:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.07 , -0.06 2021-02-13T00:16:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175395.40 epoch sec; seqNo:11184; 4058773.53 north; 592271.24 east 2021-02-13T00:16:35Z- AHRS_M2 msg: 1613175395.76 epoch sec; seqNo:11091 2021-02-13T00:16:35Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.15 psi 2021-02-13T00:16:35Z- handleDepth()-Depth_Keller msg: 1613175395768 epoch sec; seqNo:11185; depth 29.75 2021-02-13T00:16:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175395.78 epoch sec; seqNo:11185; 4058773.55 north; 592272.46 east 2021-02-13T00:16:36Z- AHRS_M2 msg: 1613175396.17 epoch sec; seqNo:11092 2021-02-13T00:16:36Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.16 psi 2021-02-13T00:16:36Z- handleDepth()-Depth_Keller msg: 1613175396176 epoch sec; seqNo:11186; depth 29.65 2021-02-13T00:16:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:36Z- handleDvl() - RDI_Pathfinder msg: 1613175396.18 epoch sec; seqNo:5548 2021-02-13T00:16:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.07 , -0.06 2021-02-13T00:16:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175396.19 epoch sec; seqNo:11186; 4058773.93 north; 592272.45 east 2021-02-13T00:16:36Z- AHRS_M2 msg: 1613175396.57 epoch sec; seqNo:11093 2021-02-13T00:16:36Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.15 psi 2021-02-13T00:16:36Z- handleDepth()-Depth_Keller msg: 1613175396579 epoch sec; seqNo:11187; depth 29.53 2021-02-13T00:16:36Z- handleDvl() - RDI_Pathfinder msg: 1613175396.60 epoch sec; seqNo:5549 2021-02-13T00:16:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.67 , 83.93 , 97.99 , 60.50 2021-02-13T00:16:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.05 , -0.03 2021-02-13T00:16:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175396.61 epoch sec; seqNo:11187; 4058774.30 north; 592272.45 east 2021-02-13T00:16:36Z- AHRS_M2 msg: 1613175396.99 epoch sec; seqNo:11094 2021-02-13T00:16:36Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.15 psi 2021-02-13T00:16:36Z- handleDepth()-Depth_Keller msg: 1613175396990 epoch sec; seqNo:11188; depth 29.42 2021-02-13T00:16:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175397.02 epoch sec; seqNo:11188; 4058774.30 north; 592272.45 east 2021-02-13T00:16:37Z- LcmTrn::handleMessages() - handling 43 messages took 0.63 ms and 131 calls... 2021-02-13T00:16:37Z- waiting for 1613175395.17, time is 1613175397.02 2021-02-13T00:16:37Z- LcmTrn::updateTrn() >>>> pose time 1613175397.02, meas time 1613175396.60 <<<< 2021-02-13T00:16:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175396.60 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:16:37Z- TerrainNav::motionUpdate() - currEstimate = (4058774, 592272.45, 29.42) 2021-02-13T00:16:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:16:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:16:37Z- TNavBF::Projecting Measurements 2021-02-13T00:16:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:16:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:16:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:16:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:16:37Z- TNavFilter::4 good beams. 2021-02-13T00:16:37Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:16:37Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:16:37Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:16:37Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:16:37Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:16:37Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:16:37Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:16:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:16:37Z- TerrainMapOctree: (vehN, vehE ) = (4058774.04, 592272.24). 2021-02-13T00:16:37Z- TerrainMapOctree: Min Distance = 3168.90. 2021-02-13T00:16:37Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:16:37Z- There is no specific good beam that all particles have in common. 2021-02-13T00:16:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:16:37Z- Old Depth Bias: 0.00 DepthBiasCov: 2.29 K: 0.198 ExpectedMeasDiff: nan currMeas.covariance: 7.55 2021-02-13T00:16:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:16:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.83 K: 0.124 ExpectedMeasDiff: nan currMeas.covariance: 11.27 2021-02-13T00:16:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:16:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.61 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:16:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:16:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.47 K: 0.162 ExpectedMeasDiff: nan currMeas.covariance: 5.86 2021-02-13T00:16:37Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:16:37Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:16:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:16:37Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:16:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:16:37Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:16:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:16:37Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:16:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:16:37Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:16:37Z- Filter 0 No beams used in subcloud update 2021-02-13T00:16:37Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:16:37Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:16:37Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 710.8 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:16:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175396.60 sec. 2021-02-13T00:16:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:16:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:16:37Z- variances([0], [2], and psi_berg) = 710.764190 833.003935 0.000000 2021-02-13T00:16:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:16:37Z- LcmTrn::publishEstimates() - MLE : 4058773.80 592321.95 29.42 2021-02-13T00:16:37Z- LcmTrn::publishEstimates() - MMSE : 4058774.30 592272.45 29.42 2021-02-13T00:16:37Z- LcmTrn::publishEstimates() - COVAR: 710.76 833.00 1.25 0.00 2021-02-13T00:16:37Z- AHRS_M2 msg: 1613175397.38 epoch sec; seqNo:11095 2021-02-13T00:16:37Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.15 psi 2021-02-13T00:16:37Z- handleDepth()-Depth_Keller msg: 1613175397386 epoch sec; seqNo:11189; depth 29.31 2021-02-13T00:16:37Z- handleDvl() - RDI_Pathfinder msg: 1613175397.40 epoch sec; seqNo:5550 2021-02-13T00:16:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 83.93 , 97.99 , 61.31 2021-02-13T00:16:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.09 , 0.01 2021-02-13T00:16:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175397.41 epoch sec; seqNo:11189; 4058774.68 north; 592272.45 east 2021-02-13T00:16:37Z- AHRS_M2 msg: 1613175397.78 epoch sec; seqNo:11096 2021-02-13T00:16:37Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.14 psi 2021-02-13T00:16:37Z- handleDepth()-Depth_Keller msg: 1613175397790 epoch sec; seqNo:11190; depth 29.22 2021-02-13T00:16:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175397.81 epoch sec; seqNo:11190; 4058775.06 north; 592272.44 east 2021-02-13T00:16:38Z- AHRS_M2 msg: 1613175398.19 epoch sec; seqNo:11097 2021-02-13T00:16:38Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.14 psi 2021-02-13T00:16:38Z- handleDepth()-Depth_Keller msg: 1613175398196 epoch sec; seqNo:11191; depth 29.09 2021-02-13T00:16:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175398.24 epoch sec; seqNo:11191; 4058775.44 north; 592272.44 east 2021-02-13T00:16:38Z- AHRS_M2 msg: 1613175398.59 epoch sec; seqNo:11098 2021-02-13T00:16:38Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.13 psi 2021-02-13T00:16:38Z- handleDepth()-Depth_Keller msg: 1613175398596 epoch sec; seqNo:11192; depth 28.95 2021-02-13T00:16:38Z- handleDvl() - RDI_Pathfinder msg: 1613175398.61 epoch sec; seqNo:5551 2021-02-13T00:16:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.08 , 82.78 , 97.92 , 60.91 2021-02-13T00:16:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , 0.06 2021-02-13T00:16:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175398.63 epoch sec; seqNo:11192; 4058775.45 north; 592273.66 east 2021-02-13T00:16:38Z- AHRS_M2 msg: 1613175399.00 epoch sec; seqNo:11099 2021-02-13T00:16:38Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.13 psi 2021-02-13T00:16:39Z- handleDepth()-Depth_Keller msg: 1613175399000 epoch sec; seqNo:11193; depth 28.84 2021-02-13T00:16:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175399.01 epoch sec; seqNo:11193; 4058775.83 north; 592273.65 east 2021-02-13T00:16:39Z- AHRS_M2 msg: 1613175399.40 epoch sec; seqNo:11100 2021-02-13T00:16:39Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.13 psi 2021-02-13T00:16:39Z- handleDepth()-Depth_Keller msg: 1613175399407 epoch sec; seqNo:11194; depth 28.74 2021-02-13T00:16:39Z- handleDvl() - RDI_Pathfinder msg: 1613175399.42 epoch sec; seqNo:5552 2021-02-13T00:16:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.31 , 81.76 , 97.99 , 60.91 2021-02-13T00:16:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.01 , 0.07 2021-02-13T00:16:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175399.43 epoch sec; seqNo:11194; 4058775.83 north; 592273.65 east 2021-02-13T00:16:39Z- AHRS_M2 msg: 1613175399.80 epoch sec; seqNo:11101 2021-02-13T00:16:39Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.13 psi 2021-02-13T00:16:39Z- handleDepth()-Depth_Keller msg: 1613175399813 epoch sec; seqNo:11195; depth 28.60 2021-02-13T00:16:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:39Z- handleDvl() - RDI_Pathfinder msg: 1613175399.82 epoch sec; seqNo:5553 2021-02-13T00:16:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.01 , 0.07 2021-02-13T00:16:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175399.83 epoch sec; seqNo:11195; 4058776.21 north; 592273.65 east 2021-02-13T00:16:40Z- AHRS_M2 msg: 1613175400.21 epoch sec; seqNo:11102 2021-02-13T00:16:40Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.13 psi 2021-02-13T00:16:40Z- handleDepth()-Depth_Keller msg: 1613175400218 epoch sec; seqNo:11196; depth 28.49 2021-02-13T00:16:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175400.23 epoch sec; seqNo:11196; 4058776.59 north; 592273.65 east 2021-02-13T00:16:40Z- AHRS_M2 msg: 1613175400.61 epoch sec; seqNo:11103 2021-02-13T00:16:40Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.13 psi 2021-02-13T00:16:40Z- handleDepth()-Depth_Keller msg: 1613175400616 epoch sec; seqNo:11197; depth 28.36 2021-02-13T00:16:40Z- handleDvl() - RDI_Pathfinder msg: 1613175400.63 epoch sec; seqNo:5554 2021-02-13T00:16:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 81.76 , 97.99 , 60.91 2021-02-13T00:16:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , 0.06 2021-02-13T00:16:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175400.64 epoch sec; seqNo:11197; 4058776.59 north; 592273.65 east 2021-02-13T00:16:41Z- AHRS_M2 msg: 1613175401.02 epoch sec; seqNo:11104 2021-02-13T00:16:41Z- AHRS_M2 msg: 0.67 phi; 0.36 theta; -0.13 psi 2021-02-13T00:16:41Z- handleDepth()-Depth_Keller msg: 1613175401026 epoch sec; seqNo:11198; depth 28.23 2021-02-13T00:16:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:41Z- handleDvl() - RDI_Pathfinder msg: 1613175401.03 epoch sec; seqNo:5555 2021-02-13T00:16:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , 0.06 2021-02-13T00:16:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175401.04 epoch sec; seqNo:11198; 4058776.98 north; 592274.86 east 2021-02-13T00:16:41Z- AHRS_M2 msg: 1613175401.42 epoch sec; seqNo:11105 2021-02-13T00:16:41Z- AHRS_M2 msg: 0.67 phi; 0.36 theta; -0.12 psi 2021-02-13T00:16:41Z- handleDepth()-Depth_Keller msg: 1613175401434 epoch sec; seqNo:11199; depth 28.12 2021-02-13T00:16:41Z- handleDvl() - RDI_Pathfinder msg: 1613175401.44 epoch sec; seqNo:5556 2021-02-13T00:16:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.72 , 82.84 , 97.99 , 62.54 2021-02-13T00:16:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , 0.02 2021-02-13T00:16:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175401.46 epoch sec; seqNo:11199; 4058777.36 north; 592274.86 east 2021-02-13T00:16:41Z- AHRS_M2 msg: 1613175401.82 epoch sec; seqNo:11106 2021-02-13T00:16:41Z- AHRS_M2 msg: 0.68 phi; 0.35 theta; -0.12 psi 2021-02-13T00:16:41Z- handleDepth()-Depth_Keller msg: 1613175401828 epoch sec; seqNo:11200; depth 27.99 2021-02-13T00:16:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175401.84 epoch sec; seqNo:11200; 4058777.36 north; 592274.86 east 2021-02-13T00:16:42Z- AHRS_M2 msg: 1613175402.24 epoch sec; seqNo:11107 2021-02-13T00:16:42Z- AHRS_M2 msg: 0.68 phi; 0.35 theta; -0.12 psi 2021-02-13T00:16:42Z- handleDepth()-Depth_Keller msg: 1613175402247 epoch sec; seqNo:11201; depth 27.88 2021-02-13T00:16:42Z- handleDvl() - RDI_Pathfinder msg: 1613175402.26 epoch sec; seqNo:5557 2021-02-13T00:16:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.72 , 83.93 , 97.99 , 62.95 2021-02-13T00:16:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , -0.02 2021-02-13T00:16:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175402.28 epoch sec; seqNo:11201; 4058777.74 north; 592274.85 east 2021-02-13T00:16:42Z- LcmTrn::handleMessages() - handling 47 messages took 0.68 ms and 134 calls... 2021-02-13T00:16:42Z- waiting for 1613175400.02, time is 1613175402.28 2021-02-13T00:16:42Z- LcmTrn::updateTrn() equating pose 1613175402.28 and meas times 1613175402.28 2021-02-13T00:16:42Z- LcmTrn::updateTrn() >>>> pose time 1613175402.28, meas time 1613175402.28 <<<< 2021-02-13T00:16:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175402.28 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:16:42Z- TerrainNav::motionUpdate() - currEstimate = (4058778, 592274.85, 27.88) 2021-02-13T00:16:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:16:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:16:42Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175402.28 sec.; No relevant navigation data available 2021-02-13T00:16:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:16:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:16:42Z- variances([0], [2], and psi_berg) = 710.701903 833.003899 0.000000 2021-02-13T00:16:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:16:42Z- LcmTrn::publishEstimates() - MLE : 4058777.24 592324.35 27.88 2021-02-13T00:16:42Z- LcmTrn::publishEstimates() - MMSE : 4058777.74 592274.85 27.88 2021-02-13T00:16:42Z- LcmTrn::publishEstimates() - COVAR: 710.70 833.00 1.46 0.00 2021-02-13T00:16:42Z- AHRS_M2 msg: 1613175402.63 epoch sec; seqNo:11108 2021-02-13T00:16:42Z- AHRS_M2 msg: 0.69 phi; 0.35 theta; -0.12 psi 2021-02-13T00:16:42Z- handleDepth()-Depth_Keller msg: 1613175402645 epoch sec; seqNo:11202; depth 27.74 2021-02-13T00:16:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175402.66 epoch sec; seqNo:11202; 4058778.12 north; 592274.85 east 2021-02-13T00:16:43Z- AHRS_M2 msg: 1613175403.04 epoch sec; seqNo:11109 2021-02-13T00:16:43Z- AHRS_M2 msg: 0.69 phi; 0.35 theta; -0.12 psi 2021-02-13T00:16:43Z- handleDepth()-Depth_Keller msg: 1613175403040 epoch sec; seqNo:11203; depth 27.63 2021-02-13T00:16:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175403.06 epoch sec; seqNo:11203; 4058778.12 north; 592274.85 east 2021-02-13T00:16:43Z- AHRS_M2 msg: 1613175403.44 epoch sec; seqNo:11110 2021-02-13T00:16:43Z- AHRS_M2 msg: 0.70 phi; 0.34 theta; -0.13 psi 2021-02-13T00:16:43Z- handleDepth()-Depth_Keller msg: 1613175403444 epoch sec; seqNo:11204; depth 27.50 2021-02-13T00:16:43Z- handleDvl() - RDI_Pathfinder msg: 1613175403.46 epoch sec; seqNo:5558 2021-02-13T00:16:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 82.84 , 97.99 , 61.72 2021-02-13T00:16:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , -0.07 2021-02-13T00:16:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175403.48 epoch sec; seqNo:11204; 4058778.51 north; 592276.07 east 2021-02-13T00:16:43Z- AHRS_M2 msg: 1613175403.84 epoch sec; seqNo:11111 2021-02-13T00:16:43Z- AHRS_M2 msg: 0.71 phi; 0.34 theta; -0.13 psi 2021-02-13T00:16:43Z- handleDepth()-Depth_Keller msg: 1613175403854 epoch sec; seqNo:11205; depth 27.38 2021-02-13T00:16:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175403.87 epoch sec; seqNo:11205; 4058778.89 north; 592276.06 east 2021-02-13T00:16:44Z- AHRS_M2 msg: 1613175404.25 epoch sec; seqNo:11112 2021-02-13T00:16:44Z- AHRS_M2 msg: 0.71 phi; 0.34 theta; -0.13 psi 2021-02-13T00:16:44Z- handleDepth()-Depth_Keller msg: 1613175404255 epoch sec; seqNo:11206; depth 27.27 2021-02-13T00:16:44Z- handleDvl() - RDI_Pathfinder msg: 1613175404.27 epoch sec; seqNo:5559 2021-02-13T00:16:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 84.94 , 97.92 , 62.13 2021-02-13T00:16:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.07 , -0.07 2021-02-13T00:16:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175404.28 epoch sec; seqNo:11206; 4058778.89 north; 592276.06 east 2021-02-13T00:16:44Z- AHRS_M2 msg: 1613175404.65 epoch sec; seqNo:11113 2021-02-13T00:16:44Z- AHRS_M2 msg: 0.71 phi; 0.33 theta; -0.14 psi 2021-02-13T00:16:44Z- handleDepth()-Depth_Keller msg: 1613175404656 epoch sec; seqNo:11207; depth 27.14 2021-02-13T00:16:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:44Z- handleDvl() - RDI_Pathfinder msg: 1613175404.66 epoch sec; seqNo:5560 2021-02-13T00:16:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.07 , -0.07 2021-02-13T00:16:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175404.67 epoch sec; seqNo:11207; 4058779.27 north; 592276.06 east 2021-02-13T00:16:45Z- AHRS_M2 msg: 1613175405.07 epoch sec; seqNo:11114 2021-02-13T00:16:45Z- AHRS_M2 msg: 0.71 phi; 0.33 theta; -0.15 psi 2021-02-13T00:16:45Z- handleDepth()-Depth_Keller msg: 1613175405070 epoch sec; seqNo:11208; depth 27.03 2021-02-13T00:16:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175405.08 epoch sec; seqNo:11208; 4058779.65 north; 592276.05 east 2021-02-13T00:16:45Z- AHRS_M2 msg: 1613175405.46 epoch sec; seqNo:11115 2021-02-13T00:16:45Z- AHRS_M2 msg: 0.71 phi; 0.33 theta; -0.16 psi 2021-02-13T00:16:45Z- handleDepth()-Depth_Keller msg: 1613175405466 epoch sec; seqNo:11209; depth 26.92 2021-02-13T00:16:45Z- handleDvl() - RDI_Pathfinder msg: 1613175405.48 epoch sec; seqNo:5561 2021-02-13T00:16:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.31 , 83.93 , 97.99 , 62.95 2021-02-13T00:16:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.08 , -0.05 2021-02-13T00:16:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175405.49 epoch sec; seqNo:11209; 4058779.65 north; 592276.05 east 2021-02-13T00:16:45Z- AHRS_M2 msg: 1613175405.87 epoch sec; seqNo:11116 2021-02-13T00:16:45Z- AHRS_M2 msg: 0.71 phi; 0.33 theta; -0.16 psi 2021-02-13T00:16:45Z- handleDepth()-Depth_Keller msg: 1613175405877 epoch sec; seqNo:11210; depth 26.79 2021-02-13T00:16:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:45Z- handleDvl() - RDI_Pathfinder msg: 1613175405.88 epoch sec; seqNo:5562 2021-02-13T00:16:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.08 , -0.05 2021-02-13T00:16:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175405.90 epoch sec; seqNo:11210; 4058780.03 north; 592276.05 east 2021-02-13T00:16:46Z- AHRS_M2 msg: 1613175406.27 epoch sec; seqNo:11117 2021-02-13T00:16:46Z- AHRS_M2 msg: 0.71 phi; 0.32 theta; -0.17 psi 2021-02-13T00:16:46Z- handleDepth()-Depth_Keller msg: 1613175406273 epoch sec; seqNo:11211; depth 26.68 2021-02-13T00:16:46Z- handleDvl() - RDI_Pathfinder msg: 1613175406.29 epoch sec; seqNo:5563 2021-02-13T00:16:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.31 , 85.01 , 97.99 , 62.95 2021-02-13T00:16:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.11 , 0.01 2021-02-13T00:16:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175406.30 epoch sec; seqNo:11211; 4058780.42 north; 592277.27 east 2021-02-13T00:16:46Z- AHRS_M2 msg: 1613175406.67 epoch sec; seqNo:11118 2021-02-13T00:16:46Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.17 psi 2021-02-13T00:16:46Z- handleDepth()-Depth_Keller msg: 1613175406677 epoch sec; seqNo:11212; depth 26.57 2021-02-13T00:16:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175406.69 epoch sec; seqNo:11212; 4058780.42 north; 592277.27 east 2021-02-13T00:16:47Z- AHRS_M2 msg: 1613175407.08 epoch sec; seqNo:11119 2021-02-13T00:16:47Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.18 psi 2021-02-13T00:16:47Z- handleDepth()-Depth_Keller msg: 1613175407080 epoch sec; seqNo:11213; depth 26.43 2021-02-13T00:16:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:47Z- handleDvl() - RDI_Pathfinder msg: 1613175407.08 epoch sec; seqNo:5564 2021-02-13T00:16:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.11 , 0.01 2021-02-13T00:16:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175407.10 epoch sec; seqNo:11213; 4058780.80 north; 592277.26 east 2021-02-13T00:16:47Z- LcmTrn::handleMessages() - handling 43 messages took 0.65 ms and 130 calls... 2021-02-13T00:16:47Z- waiting for 1613175405.28, time is 1613175407.10 2021-02-13T00:16:47Z- LcmTrn::updateTrn() equating pose 1613175407.10 and meas times 1613175407.10 2021-02-13T00:16:47Z- LcmTrn::updateTrn() >>>> pose time 1613175407.10, meas time 1613175407.10 <<<< 2021-02-13T00:16:47Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175407.10, ping # 0. 2021-02-13T00:16:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175407.10 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:16:47Z- TerrainNav::motionUpdate() - currEstimate = (4058781, 592277.26, 26.43) 2021-02-13T00:16:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:16:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:16:47Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:16:47Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:16:47Z- TNavBF::Projecting Measurements 2021-02-13T00:16:47Z- TNavFilter::Measurement from time = 1613175407.10 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:16:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:16:47Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:16:47Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:16:47Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:16:47Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:16:47Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:16:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:16:47Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 710.6 EastVariance: 833.0 DepthVariance: 1.7 this->DepthBiasCov: 1.7 2021-02-13T00:16:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:16:47Z- TNavBF::mmse selected from filter -1 2021-02-13T00:16:47Z- variances([0], [2], and psi_berg) = 710.641941 833.003863 0.000000 2021-02-13T00:16:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:16:47Z- LcmTrn::publishEstimates() - MLE : 4058780.30 592326.76 26.43 2021-02-13T00:16:47Z- LcmTrn::publishEstimates() - MMSE : 4058780.80 592277.26 26.43 2021-02-13T00:16:47Z- LcmTrn::publishEstimates() - COVAR: 710.64 833.00 1.66 0.00 2021-02-13T00:16:47Z- AHRS_M2 msg: 1613175407.48 epoch sec; seqNo:11120 2021-02-13T00:16:47Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.18 psi 2021-02-13T00:16:47Z- handleDepth()-Depth_Keller msg: 1613175407484 epoch sec; seqNo:11214; depth 26.32 2021-02-13T00:16:47Z- handleDvl() - RDI_Pathfinder msg: 1613175407.50 epoch sec; seqNo:5565 2021-02-13T00:16:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:47Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:16:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175407.52 epoch sec; seqNo:11214; 4058781.18 north; 592277.26 east 2021-02-13T00:16:47Z- AHRS_M2 msg: 1613175407.88 epoch sec; seqNo:11121 2021-02-13T00:16:47Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.17 psi 2021-02-13T00:16:47Z- handleDepth()-Depth_Keller msg: 1613175407888 epoch sec; seqNo:11215; depth 26.22 2021-02-13T00:16:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175407.90 epoch sec; seqNo:11215; 4058781.18 north; 592277.26 east 2021-02-13T00:16:48Z- AHRS_M2 msg: 1613175408.29 epoch sec; seqNo:11122 2021-02-13T00:16:48Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.16 psi 2021-02-13T00:16:48Z- handleDepth()-Depth_Keller msg: 1613175408294 epoch sec; seqNo:11216; depth 26.10 2021-02-13T00:16:48Z- handleDvl() - RDI_Pathfinder msg: 1613175408.31 epoch sec; seqNo:5566 2021-02-13T00:16:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.49 , 86.76 , 96.23 , 63.36 2021-02-13T00:16:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.11 , 0.09 2021-02-13T00:16:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175408.32 epoch sec; seqNo:11216; 4058781.56 north; 592277.25 east 2021-02-13T00:16:48Z- AHRS_M2 msg: 1613175408.69 epoch sec; seqNo:11123 2021-02-13T00:16:48Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.15 psi 2021-02-13T00:16:48Z- handleDepth()-Depth_Keller msg: 1613175408706 epoch sec; seqNo:11217; depth 25.99 2021-02-13T00:16:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175408.72 epoch sec; seqNo:11217; 4058781.93 north; 592277.25 east 2021-02-13T00:16:49Z- AHRS_M2 msg: 1613175409.10 epoch sec; seqNo:11124 2021-02-13T00:16:49Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.15 psi 2021-02-13T00:16:49Z- handleDepth()-Depth_Keller msg: 1613175409100 epoch sec; seqNo:11218; depth 25.86 2021-02-13T00:16:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175409.11 epoch sec; seqNo:11218; 4058782.33 north; 592278.47 east 2021-02-13T00:16:49Z- AHRS_M2 msg: 1613175409.50 epoch sec; seqNo:11125 2021-02-13T00:16:49Z- AHRS_M2 msg: 0.71 phi; 0.32 theta; -0.14 psi 2021-02-13T00:16:49Z- handleDepth()-Depth_Keller msg: 1613175409509 epoch sec; seqNo:11219; depth 25.76 2021-02-13T00:16:49Z- handleDvl() - RDI_Pathfinder msg: 1613175409.52 epoch sec; seqNo:5567 2021-02-13T00:16:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 85.01 , 97.99 , 65.40 2021-02-13T00:16:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , 0.10 2021-02-13T00:16:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175409.53 epoch sec; seqNo:11219; 4058782.33 north; 592278.47 east 2021-02-13T00:16:49Z- AHRS_M2 msg: 1613175409.90 epoch sec; seqNo:11126 2021-02-13T00:16:49Z- AHRS_M2 msg: 0.71 phi; 0.32 theta; -0.14 psi 2021-02-13T00:16:49Z- handleDepth()-Depth_Keller msg: 1613175409910 epoch sec; seqNo:11220; depth 25.64 2021-02-13T00:16:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175409.92 epoch sec; seqNo:11220; 4058782.70 north; 592278.46 east 2021-02-13T00:16:50Z- AHRS_M2 msg: 1613175410.31 epoch sec; seqNo:11127 2021-02-13T00:16:50Z- AHRS_M2 msg: 0.71 phi; 0.32 theta; -0.13 psi 2021-02-13T00:16:50Z- handleDepth()-Depth_Keller msg: 1613175410312 epoch sec; seqNo:11221; depth 25.53 2021-02-13T00:16:50Z- handleDvl() - RDI_Pathfinder msg: 1613175410.34 epoch sec; seqNo:5568 2021-02-13T00:16:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.35 , 0.00 , 98.40 , 64.99 2021-02-13T00:16:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.00 , 0.07 2021-02-13T00:16:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175410.35 epoch sec; seqNo:11221; 4058783.08 north; 592278.46 east 2021-02-13T00:16:50Z- AHRS_M2 msg: 1613175410.71 epoch sec; seqNo:11128 2021-02-13T00:16:50Z- AHRS_M2 msg: 0.72 phi; 0.32 theta; -0.13 psi 2021-02-13T00:16:50Z- handleDepth()-Depth_Keller msg: 1613175410716 epoch sec; seqNo:11222; depth 25.41 2021-02-13T00:16:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:50Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:50Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:50Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:50Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:50Z- handleDvl() - RDI_Pathfinder msg: 1613175410.72 epoch sec; seqNo:5569 2021-02-13T00:16:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.00 , 0.07 2021-02-13T00:16:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175410.73 epoch sec; seqNo:11222; 4058783.46 north; 592278.46 east 2021-02-13T00:16:51Z- AHRS_M2 msg: 1613175411.12 epoch sec; seqNo:11129 2021-02-13T00:16:51Z- AHRS_M2 msg: 0.72 phi; 0.32 theta; -0.13 psi 2021-02-13T00:16:51Z- handleDepth()-Depth_Keller msg: 1613175411123 epoch sec; seqNo:11223; depth 25.30 2021-02-13T00:16:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175411.13 epoch sec; seqNo:11223; 4058783.48 north; 592279.68 east 2021-02-13T00:16:51Z- handleDepth()-Depth_Keller msg: 1613175411541 epoch sec; seqNo:11224; depth 25.21 2021-02-13T00:16:51Z- handleDvl() - RDI_Pathfinder msg: 1613175411.55 epoch sec; seqNo:5570 2021-02-13T00:16:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.76 , 85.01 , 97.99 , 63.77 2021-02-13T00:16:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , 0.05 2021-02-13T00:16:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175411.56 epoch sec; seqNo:11224; 4058783.85 north; 592279.67 east 2021-02-13T00:16:51Z- AHRS_M2 msg: 1613175411.92 epoch sec; seqNo:11130 2021-02-13T00:16:51Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.12 psi 2021-02-13T00:16:51Z- handleDepth()-Depth_Keller msg: 1613175411938 epoch sec; seqNo:11225; depth 25.08 2021-02-13T00:16:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:51Z- handleDvl() - RDI_Pathfinder msg: 1613175411.94 epoch sec; seqNo:5571 2021-02-13T00:16:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , 0.05 2021-02-13T00:16:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175411.95 epoch sec; seqNo:11225; 4058784.23 north; 592279.67 east 2021-02-13T00:16:52Z- AHRS_M2 msg: 1613175412.33 epoch sec; seqNo:11131 2021-02-13T00:16:52Z- AHRS_M2 msg: 0.74 phi; 0.32 theta; -0.12 psi 2021-02-13T00:16:52Z- handleDepth()-Depth_Keller msg: 1613175412336 epoch sec; seqNo:11226; depth 24.99 2021-02-13T00:16:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175412.37 epoch sec; seqNo:11226; 4058784.23 north; 592279.67 east 2021-02-13T00:16:52Z- LcmTrn::handleMessages() - handling 45 messages took 0.66 ms and 132 calls... 2021-02-13T00:16:52Z- waiting for 1613175410.10, time is 1613175412.37 2021-02-13T00:16:52Z- LcmTrn::updateTrn() >>>> pose time 1613175412.37, meas time 1613175411.94 <<<< 2021-02-13T00:16:52Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175411.94, ping # 0. 2021-02-13T00:16:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175411.94 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:16:52Z- TerrainNav::motionUpdate() - currEstimate = (4058784, 592279.67, 24.99) 2021-02-13T00:16:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:16:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:16:52Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:16:52Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:16:52Z- TNavBF::Projecting Measurements 2021-02-13T00:16:52Z- TNavFilter::Measurement from time = 1613175411.94 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:16:52Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:16:52Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:16:52Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:16:52Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:16:52Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:16:52Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:16:52Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:16:52Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 710.6 EastVariance: 833.0 DepthVariance: 1.9 this->DepthBiasCov: 1.9 2021-02-13T00:16:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:16:52Z- TNavBF::mmse selected from filter -1 2021-02-13T00:16:52Z- variances([0], [2], and psi_berg) = 710.564521 833.003818 0.000000 2021-02-13T00:16:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:16:52Z- LcmTrn::publishEstimates() - MLE : 4058783.73 592329.17 24.99 2021-02-13T00:16:52Z- LcmTrn::publishEstimates() - MMSE : 4058784.23 592279.67 24.99 2021-02-13T00:16:52Z- LcmTrn::publishEstimates() - COVAR: 710.56 833.00 1.87 0.00 2021-02-13T00:16:52Z- AHRS_M2 msg: 1613175412.73 epoch sec; seqNo:11132 2021-02-13T00:16:52Z- AHRS_M2 msg: 0.74 phi; 0.31 theta; -0.12 psi 2021-02-13T00:16:52Z- handleDepth()-Depth_Keller msg: 1613175412736 epoch sec; seqNo:11227; depth 24.88 2021-02-13T00:16:52Z- handleDvl() - RDI_Pathfinder msg: 1613175412.75 epoch sec; seqNo:5572 2021-02-13T00:16:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 85.01 , 97.99 , 127.21 2021-02-13T00:16:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , 0.05 , -0.06 2021-02-13T00:16:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175412.76 epoch sec; seqNo:11227; 4058784.61 north; 592279.66 east 2021-02-13T00:16:53Z- AHRS_M2 msg: 1613175413.15 epoch sec; seqNo:11133 2021-02-13T00:16:53Z- AHRS_M2 msg: 0.75 phi; 0.31 theta; -0.12 psi 2021-02-13T00:16:53Z- handleDepth()-Depth_Keller msg: 1613175413152 epoch sec; seqNo:11228; depth 24.75 2021-02-13T00:16:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:53Z- handleDvl() - RDI_Pathfinder msg: 1613175413.15 epoch sec; seqNo:5573 2021-02-13T00:16:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , 0.05 , -0.06 2021-02-13T00:16:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175413.17 epoch sec; seqNo:11228; 4058784.61 north; 592279.66 east 2021-02-13T00:16:53Z- AHRS_M2 msg: 1613175413.54 epoch sec; seqNo:11134 2021-02-13T00:16:53Z- AHRS_M2 msg: 0.75 phi; 0.31 theta; -0.12 psi 2021-02-13T00:16:53Z- handleDepth()-Depth_Keller msg: 1613175413544 epoch sec; seqNo:11229; depth 24.64 2021-02-13T00:16:53Z- handleDvl() - RDI_Pathfinder msg: 1613175413.57 epoch sec; seqNo:5574 2021-02-13T00:16:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 85.14 , 97.32 , 64.58 2021-02-13T00:16:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.03 , -0.02 2021-02-13T00:16:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175413.58 epoch sec; seqNo:11229; 4058785.00 north; 592280.88 east 2021-02-13T00:16:53Z- AHRS_M2 msg: 1613175413.95 epoch sec; seqNo:11135 2021-02-13T00:16:53Z- AHRS_M2 msg: 0.75 phi; 0.31 theta; -0.14 psi 2021-02-13T00:16:53Z- handleDepth()-Depth_Keller msg: 1613175413954 epoch sec; seqNo:11230; depth 24.53 2021-02-13T00:16:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175413.96 epoch sec; seqNo:11230; 4058785.00 north; 592280.88 east 2021-02-13T00:16:54Z- AHRS_M2 msg: 1613175414.35 epoch sec; seqNo:11136 2021-02-13T00:16:54Z- AHRS_M2 msg: 0.75 phi; 0.31 theta; -0.14 psi 2021-02-13T00:16:54Z- handleDepth()-Depth_Keller msg: 1613175414357 epoch sec; seqNo:11231; depth 24.42 2021-02-13T00:16:54Z- handleDvl() - RDI_Pathfinder msg: 1613175414.37 epoch sec; seqNo:5575 2021-02-13T00:16:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.17 , 87.04 , 97.86 , 65.81 2021-02-13T00:16:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.01 , -0.05 2021-02-13T00:16:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175414.38 epoch sec; seqNo:11231; 4058785.38 north; 592280.88 east 2021-02-13T00:16:54Z- AHRS_M2 msg: 1613175414.75 epoch sec; seqNo:11137 2021-02-13T00:16:54Z- AHRS_M2 msg: 0.75 phi; 0.31 theta; -0.15 psi 2021-02-13T00:16:54Z- handleDepth()-Depth_Keller msg: 1613175414759 epoch sec; seqNo:11232; depth 24.29 2021-02-13T00:16:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175414.77 epoch sec; seqNo:11232; 4058785.76 north; 592280.87 east 2021-02-13T00:16:55Z- AHRS_M2 msg: 1613175415.16 epoch sec; seqNo:11138 2021-02-13T00:16:55Z- AHRS_M2 msg: 0.75 phi; 0.31 theta; -0.16 psi 2021-02-13T00:16:55Z- handleDepth()-Depth_Keller msg: 1613175415174 epoch sec; seqNo:11233; depth 24.18 2021-02-13T00:16:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175415.19 epoch sec; seqNo:11233; 4058785.76 north; 592280.87 east 2021-02-13T00:16:55Z- AHRS_M2 msg: 1613175415.56 epoch sec; seqNo:11139 2021-02-13T00:16:55Z- AHRS_M2 msg: 0.75 phi; 0.31 theta; -0.16 psi 2021-02-13T00:16:55Z- handleDepth()-Depth_Keller msg: 1613175415569 epoch sec; seqNo:11234; depth 24.06 2021-02-13T00:16:55Z- handleDvl() - RDI_Pathfinder msg: 1613175415.58 epoch sec; seqNo:5576 2021-02-13T00:16:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.76 , 88.12 , 97.86 , 66.63 2021-02-13T00:16:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , -0.04 2021-02-13T00:16:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175415.60 epoch sec; seqNo:11234; 4058786.15 north; 592282.09 east 2021-02-13T00:16:55Z- AHRS_M2 msg: 1613175415.96 epoch sec; seqNo:11140 2021-02-13T00:16:55Z- AHRS_M2 msg: 0.74 phi; 0.31 theta; -0.17 psi 2021-02-13T00:16:55Z- handleDepth()-Depth_Keller msg: 1613175415973 epoch sec; seqNo:11235; depth 23.95 2021-02-13T00:16:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175415.99 epoch sec; seqNo:11235; 4058786.53 north; 592282.08 east 2021-02-13T00:16:56Z- AHRS_M2 msg: 1613175416.37 epoch sec; seqNo:11141 2021-02-13T00:16:56Z- AHRS_M2 msg: 0.74 phi; 0.31 theta; -0.18 psi 2021-02-13T00:16:56Z- handleDepth()-Depth_Keller msg: 1613175416381 epoch sec; seqNo:11236; depth 23.84 2021-02-13T00:16:56Z- handleDvl() - RDI_Pathfinder msg: 1613175416.39 epoch sec; seqNo:5577 2021-02-13T00:16:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:56Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:16:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175416.40 epoch sec; seqNo:11236; 4058786.53 north; 592282.08 east 2021-02-13T00:16:56Z- AHRS_M2 msg: 1613175416.77 epoch sec; seqNo:11142 2021-02-13T00:16:56Z- AHRS_M2 msg: 0.74 phi; 0.31 theta; -0.18 psi 2021-02-13T00:16:56Z- handleDepth()-Depth_Keller msg: 1613175416776 epoch sec; seqNo:11237; depth 23.73 2021-02-13T00:16:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:56Z- handleDvl() - RDI_Pathfinder msg: 1613175416.78 epoch sec; seqNo:5578 2021-02-13T00:16:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:56Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:16:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175416.80 epoch sec; seqNo:11237; 4058786.91 north; 592282.08 east 2021-02-13T00:16:57Z- AHRS_M2 msg: 1613175417.18 epoch sec; seqNo:11143 2021-02-13T00:16:57Z- AHRS_M2 msg: 0.73 phi; 0.31 theta; -0.19 psi 2021-02-13T00:16:57Z- handleDepth()-Depth_Keller msg: 1613175417180 epoch sec; seqNo:11238; depth 23.61 2021-02-13T00:16:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175417.19 epoch sec; seqNo:11238; 4058786.91 north; 592282.08 east 2021-02-13T00:16:57Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 131 calls... 2021-02-13T00:16:57Z- waiting for 1613175415.37, time is 1613175417.19 2021-02-13T00:16:57Z- LcmTrn::updateTrn() >>>> pose time 1613175417.19, meas time 1613175416.78 <<<< 2021-02-13T00:16:57Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175416.78, ping # 0. 2021-02-13T00:16:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175416.78 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:16:57Z- TerrainNav::motionUpdate() - currEstimate = (4058787, 592282.08, 23.61) 2021-02-13T00:16:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:16:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:16:57Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:16:57Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:16:57Z- TNavBF::Projecting Measurements 2021-02-13T00:16:57Z- TNavFilter::Measurement from time = 1613175416.78 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:16:57Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:16:57Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:16:57Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:16:57Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:16:57Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:16:57Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:16:57Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:16:57Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 710.5 EastVariance: 833.0 DepthVariance: 2.0 this->DepthBiasCov: 2.0 2021-02-13T00:16:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:16:57Z- TNavBF::mmse selected from filter -1 2021-02-13T00:16:57Z- variances([0], [2], and psi_berg) = 710.492591 833.003776 0.000000 2021-02-13T00:16:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:16:57Z- LcmTrn::publishEstimates() - MLE : 4058786.41 592331.58 23.61 2021-02-13T00:16:57Z- LcmTrn::publishEstimates() - MMSE : 4058786.91 592282.08 23.61 2021-02-13T00:16:57Z- LcmTrn::publishEstimates() - COVAR: 710.49 833.00 2.06 0.00 2021-02-13T00:16:57Z- AHRS_M2 msg: 1613175417.58 epoch sec; seqNo:11144 2021-02-13T00:16:57Z- AHRS_M2 msg: 0.73 phi; 0.31 theta; -0.19 psi 2021-02-13T00:16:57Z- handleDepth()-Depth_Keller msg: 1613175417584 epoch sec; seqNo:11239; depth 23.51 2021-02-13T00:16:57Z- handleDvl() - RDI_Pathfinder msg: 1613175417.60 epoch sec; seqNo:5579 2021-02-13T00:16:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 89.27 , 97.79 , 66.63 2021-02-13T00:16:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , 0.02 2021-02-13T00:16:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175417.61 epoch sec; seqNo:11239; 4058787.29 north; 592282.08 east 2021-02-13T00:16:57Z- AHRS_M2 msg: 1613175417.98 epoch sec; seqNo:11145 2021-02-13T00:16:57Z- AHRS_M2 msg: 0.73 phi; 0.31 theta; -0.19 psi 2021-02-13T00:16:57Z- handleDepth()-Depth_Keller msg: 1613175417990 epoch sec; seqNo:11240; depth 23.39 2021-02-13T00:16:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:16:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:16:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:16:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:16:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:16:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:16:57Z- handleDvl() - RDI_Pathfinder msg: 1613175417.99 epoch sec; seqNo:5580 2021-02-13T00:16:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:16:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , 0.02 2021-02-13T00:16:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175418.00 epoch sec; seqNo:11240; 4058787.68 north; 592283.29 east 2021-02-13T00:16:58Z- AHRS_M2 msg: 1613175418.39 epoch sec; seqNo:11146 2021-02-13T00:16:58Z- AHRS_M2 msg: 0.73 phi; 0.31 theta; -0.19 psi 2021-02-13T00:16:58Z- handleDepth()-Depth_Keller msg: 1613175418392 epoch sec; seqNo:11241; depth 23.27 2021-02-13T00:16:58Z- handleDvl() - RDI_Pathfinder msg: 1613175418.41 epoch sec; seqNo:5581 2021-02-13T00:16:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.35 , 98.26 , 98.26 , 68.26 2021-02-13T00:16:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.09 , 0.06 2021-02-13T00:16:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175418.42 epoch sec; seqNo:11241; 4058788.06 north; 592283.29 east 2021-02-13T00:16:58Z- AHRS_M2 msg: 1613175418.79 epoch sec; seqNo:11147 2021-02-13T00:16:58Z- AHRS_M2 msg: 0.72 phi; 0.31 theta; -0.18 psi 2021-02-13T00:16:58Z- handleDepth()-Depth_Keller msg: 1613175418796 epoch sec; seqNo:11242; depth 23.15 2021-02-13T00:16:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175418.81 epoch sec; seqNo:11242; 4058788.06 north; 592283.29 east 2021-02-13T00:16:59Z- AHRS_M2 msg: 1613175419.22 epoch sec; seqNo:11148 2021-02-13T00:16:59Z- AHRS_M2 msg: 0.72 phi; 0.31 theta; -0.17 psi 2021-02-13T00:16:59Z- handleDepth()-Depth_Keller msg: 1613175419222 epoch sec; seqNo:11243; depth 23.05 2021-02-13T00:16:59Z- handleDvl() - RDI_Pathfinder msg: 1613175419.23 epoch sec; seqNo:5582 2021-02-13T00:16:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.99 , 90.96 , 98.26 , 67.86 2021-02-13T00:16:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , 0.06 2021-02-13T00:16:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175419.24 epoch sec; seqNo:11243; 4058788.44 north; 592283.28 east 2021-02-13T00:16:59Z- AHRS_M2 msg: 1613175419.60 epoch sec; seqNo:11149 2021-02-13T00:16:59Z- AHRS_M2 msg: 0.72 phi; 0.31 theta; -0.17 psi 2021-02-13T00:16:59Z- handleDepth()-Depth_Keller msg: 1613175419605 epoch sec; seqNo:11244; depth 22.93 2021-02-13T00:16:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175419.63 epoch sec; seqNo:11244; 4058788.82 north; 592283.28 east 2021-02-13T00:17:00Z- AHRS_M2 msg: 1613175420.00 epoch sec; seqNo:11150 2021-02-13T00:17:00Z- AHRS_M2 msg: 0.72 phi; 0.31 theta; -0.16 psi 2021-02-13T00:17:00Z- handleDepth()-Depth_Keller msg: 1613175420008 epoch sec; seqNo:11245; depth 22.83 2021-02-13T00:17:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175420.02 epoch sec; seqNo:11245; 4058788.82 north; 592283.28 east 2021-02-13T00:17:00Z- AHRS_M2 msg: 1613175420.41 epoch sec; seqNo:11151 2021-02-13T00:17:00Z- AHRS_M2 msg: 0.72 phi; 0.31 theta; -0.16 psi 2021-02-13T00:17:00Z- handleDepth()-Depth_Keller msg: 1613175420417 epoch sec; seqNo:11246; depth 22.73 2021-02-13T00:17:00Z- handleDvl() - RDI_Pathfinder msg: 1613175420.43 epoch sec; seqNo:5583 2021-02-13T00:17:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 0.00 , 98.40 , 78.92 2021-02-13T00:17:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , 0.14 , -0.00 2021-02-13T00:17:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175420.44 epoch sec; seqNo:11246; 4058789.21 north; 592284.50 east 2021-02-13T00:17:00Z- AHRS_M2 msg: 1613175420.81 epoch sec; seqNo:11152 2021-02-13T00:17:00Z- AHRS_M2 msg: 0.72 phi; 0.31 theta; -0.16 psi 2021-02-13T00:17:00Z- handleDepth()-Depth_Keller msg: 1613175420818 epoch sec; seqNo:11247; depth 22.61 2021-02-13T00:17:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175420.83 epoch sec; seqNo:11247; 4058789.21 north; 592284.50 east 2021-02-13T00:17:01Z- AHRS_M2 msg: 1613175421.23 epoch sec; seqNo:11153 2021-02-13T00:17:01Z- AHRS_M2 msg: 0.72 phi; 0.31 theta; -0.17 psi 2021-02-13T00:17:01Z- handleDepth()-Depth_Keller msg: 1613175421237 epoch sec; seqNo:11248; depth 22.48 2021-02-13T00:17:01Z- handleDvl() - RDI_Pathfinder msg: 1613175421.25 epoch sec; seqNo:5584 2021-02-13T00:17:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 0.00 , 99.41 , 68.67 2021-02-13T00:17:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.04 , 0.05 2021-02-13T00:17:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175421.26 epoch sec; seqNo:11248; 4058789.59 north; 592284.49 east 2021-02-13T00:17:01Z- AHRS_M2 msg: 1613175421.62 epoch sec; seqNo:11154 2021-02-13T00:17:01Z- AHRS_M2 msg: 0.72 phi; 0.31 theta; -0.17 psi 2021-02-13T00:17:01Z- handleDepth()-Depth_Keller msg: 1613175421629 epoch sec; seqNo:11249; depth 22.38 2021-02-13T00:17:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:01Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:01Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:01Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:01Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:01Z- handleDvl() - RDI_Pathfinder msg: 1613175421.63 epoch sec; seqNo:5585 2021-02-13T00:17:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.04 , 0.05 2021-02-13T00:17:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175421.65 epoch sec; seqNo:11249; 4058789.59 north; 592284.49 east 2021-02-13T00:17:02Z- AHRS_M2 msg: 1613175422.02 epoch sec; seqNo:11155 2021-02-13T00:17:02Z- AHRS_M2 msg: 0.72 phi; 0.30 theta; -0.18 psi 2021-02-13T00:17:02Z- handleDepth()-Depth_Keller msg: 1613175422030 epoch sec; seqNo:11250; depth 22.26 2021-02-13T00:17:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175422.04 epoch sec; seqNo:11250; 4058789.97 north; 592284.49 east 2021-02-13T00:17:02Z- AHRS_M2 msg: 1613175422.43 epoch sec; seqNo:11156 2021-02-13T00:17:02Z- AHRS_M2 msg: 0.72 phi; 0.30 theta; -0.19 psi 2021-02-13T00:17:02Z- handleDepth()-Depth_Keller msg: 1613175422432 epoch sec; seqNo:11251; depth 22.15 2021-02-13T00:17:02Z- handleDvl() - RDI_Pathfinder msg: 1613175422.45 epoch sec; seqNo:5586 2021-02-13T00:17:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.76 , 90.21 , 97.79 , 69.08 2021-02-13T00:17:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.02 , 0.02 2021-02-13T00:17:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175422.46 epoch sec; seqNo:11251; 4058790.36 north; 592285.71 east 2021-02-13T00:17:02Z- LcmTrn::handleMessages() - handling 47 messages took 0.60 ms and 135 calls... 2021-02-13T00:17:02Z- waiting for 1613175420.19, time is 1613175422.46 2021-02-13T00:17:02Z- LcmTrn::updateTrn() equating pose 1613175422.46 and meas times 1613175422.46 2021-02-13T00:17:02Z- LcmTrn::updateTrn() >>>> pose time 1613175422.46, meas time 1613175422.46 <<<< 2021-02-13T00:17:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175422.46 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:17:02Z- TerrainNav::motionUpdate() - currEstimate = (4058790, 592285.71, 22.15) 2021-02-13T00:17:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:17:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:17:02Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175422.46 sec.; No relevant navigation data available 2021-02-13T00:17:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:17:02Z- TNavBF::mmse selected from filter -1 2021-02-13T00:17:02Z- variances([0], [2], and psi_berg) = 710.424757 833.003736 0.000000 2021-02-13T00:17:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:17:02Z- LcmTrn::publishEstimates() - MLE : 4058789.86 592334.21 22.15 2021-02-13T00:17:02Z- LcmTrn::publishEstimates() - MMSE : 4058790.36 592285.71 22.15 2021-02-13T00:17:02Z- LcmTrn::publishEstimates() - COVAR: 710.42 833.00 2.31 0.00 2021-02-13T00:17:02Z- AHRS_M2 msg: 1613175422.84 epoch sec; seqNo:11157 2021-02-13T00:17:02Z- AHRS_M2 msg: 0.71 phi; 0.30 theta; -0.20 psi 2021-02-13T00:17:02Z- handleDepth()-Depth_Keller msg: 1613175422839 epoch sec; seqNo:11252; depth 22.03 2021-02-13T00:17:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:02Z- handleDvl() - RDI_Pathfinder msg: 1613175422.84 epoch sec; seqNo:5587 2021-02-13T00:17:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.02 , 0.02 2021-02-13T00:17:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175422.85 epoch sec; seqNo:11252; 4058790.36 north; 592285.71 east 2021-02-13T00:17:03Z- AHRS_M2 msg: 1613175423.24 epoch sec; seqNo:11158 2021-02-13T00:17:03Z- AHRS_M2 msg: 0.71 phi; 0.30 theta; -0.21 psi 2021-02-13T00:17:03Z- handleDepth()-Depth_Keller msg: 1613175423251 epoch sec; seqNo:11253; depth 21.92 2021-02-13T00:17:03Z- handleDvl() - RDI_Pathfinder msg: 1613175423.26 epoch sec; seqNo:5588 2021-02-13T00:17:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.58 , 91.09 , 100.83 , 68.67 2021-02-13T00:17:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , -0.01 2021-02-13T00:17:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175423.27 epoch sec; seqNo:11253; 4058790.74 north; 592285.70 east 2021-02-13T00:17:03Z- AHRS_M2 msg: 1613175423.64 epoch sec; seqNo:11159 2021-02-13T00:17:03Z- AHRS_M2 msg: 0.71 phi; 0.30 theta; -0.21 psi 2021-02-13T00:17:03Z- handleDepth()-Depth_Keller msg: 1613175423646 epoch sec; seqNo:11254; depth 21.82 2021-02-13T00:17:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175423.66 epoch sec; seqNo:11254; 4058791.12 north; 592285.70 east 2021-02-13T00:17:04Z- AHRS_M2 msg: 1613175424.04 epoch sec; seqNo:11160 2021-02-13T00:17:04Z- AHRS_M2 msg: 0.71 phi; 0.30 theta; -0.21 psi 2021-02-13T00:17:04Z- handleDepth()-Depth_Keller msg: 1613175424053 epoch sec; seqNo:11255; depth 21.71 2021-02-13T00:17:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:04Z- handleDvl() - RDI_Pathfinder msg: 1613175424.06 epoch sec; seqNo:5589 2021-02-13T00:17:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , -0.01 2021-02-13T00:17:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175424.07 epoch sec; seqNo:11255; 4058791.12 north; 592285.70 east 2021-02-13T00:17:04Z- AHRS_M2 msg: 1613175424.45 epoch sec; seqNo:11161 2021-02-13T00:17:04Z- AHRS_M2 msg: 0.70 phi; 0.30 theta; -0.21 psi 2021-02-13T00:17:04Z- handleDepth()-Depth_Keller msg: 1613175424457 epoch sec; seqNo:11256; depth 21.61 2021-02-13T00:17:04Z- handleDvl() - RDI_Pathfinder msg: 1613175424.47 epoch sec; seqNo:5590 2021-02-13T00:17:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.76 , 98.33 , 100.76 , 69.90 2021-02-13T00:17:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.01 2021-02-13T00:17:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175424.48 epoch sec; seqNo:11256; 4058791.50 north; 592285.69 east 2021-02-13T00:17:04Z- AHRS_M2 msg: 1613175424.85 epoch sec; seqNo:11162 2021-02-13T00:17:04Z- AHRS_M2 msg: 0.70 phi; 0.30 theta; -0.21 psi 2021-02-13T00:17:04Z- handleDepth()-Depth_Keller msg: 1613175424858 epoch sec; seqNo:11257; depth 21.52 2021-02-13T00:17:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175424.87 epoch sec; seqNo:11257; 4058791.89 north; 592286.91 east 2021-02-13T00:17:05Z- AHRS_M2 msg: 1613175425.26 epoch sec; seqNo:11163 2021-02-13T00:17:05Z- AHRS_M2 msg: 0.70 phi; 0.30 theta; -0.21 psi 2021-02-13T00:17:05Z- handleDepth()-Depth_Keller msg: 1613175425265 epoch sec; seqNo:11258; depth 21.41 2021-02-13T00:17:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:05Z- handleDvl() - RDI_Pathfinder msg: 1613175425.27 epoch sec; seqNo:5591 2021-02-13T00:17:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.01 2021-02-13T00:17:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175425.28 epoch sec; seqNo:11258; 4058791.89 north; 592286.91 east 2021-02-13T00:17:05Z- AHRS_M2 msg: 1613175425.66 epoch sec; seqNo:11164 2021-02-13T00:17:05Z- AHRS_M2 msg: 0.69 phi; 0.30 theta; -0.21 psi 2021-02-13T00:17:05Z- handleDepth()-Depth_Keller msg: 1613175425669 epoch sec; seqNo:11259; depth 21.31 2021-02-13T00:17:05Z- handleDvl() - RDI_Pathfinder msg: 1613175425.68 epoch sec; seqNo:5592 2021-02-13T00:17:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 94.54 , 101.85 , 78.72 2021-02-13T00:17:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , 0.00 2021-02-13T00:17:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175425.70 epoch sec; seqNo:11259; 4058792.27 north; 592286.91 east 2021-02-13T00:17:06Z- AHRS_M2 msg: 1613175426.06 epoch sec; seqNo:11165 2021-02-13T00:17:06Z- AHRS_M2 msg: 0.69 phi; 0.30 theta; -0.20 psi 2021-02-13T00:17:06Z- handleDepth()-Depth_Keller msg: 1613175426068 epoch sec; seqNo:11260; depth 21.19 2021-02-13T00:17:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175426.08 epoch sec; seqNo:11260; 4058792.65 north; 592286.90 east 2021-02-13T00:17:06Z- AHRS_M2 msg: 1613175426.47 epoch sec; seqNo:11166 2021-02-13T00:17:06Z- AHRS_M2 msg: 0.69 phi; 0.30 theta; -0.20 psi 2021-02-13T00:17:06Z- handleDepth()-Depth_Keller msg: 1613175426474 epoch sec; seqNo:11261; depth 21.10 2021-02-13T00:17:06Z- handleDvl() - RDI_Pathfinder msg: 1613175426.49 epoch sec; seqNo:5593 2021-02-13T00:17:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.58 , 97.92 , 100.36 , 68.26 2021-02-13T00:17:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , 0.02 2021-02-13T00:17:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175426.50 epoch sec; seqNo:11261; 4058793.03 north; 592286.90 east 2021-02-13T00:17:06Z- AHRS_M2 msg: 1613175426.87 epoch sec; seqNo:11167 2021-02-13T00:17:06Z- AHRS_M2 msg: 0.69 phi; 0.30 theta; -0.19 psi 2021-02-13T00:17:06Z- handleDepth()-Depth_Keller msg: 1613175426881 epoch sec; seqNo:11262; depth 21.00 2021-02-13T00:17:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175426.91 epoch sec; seqNo:11262; 4058793.03 north; 592286.90 east 2021-02-13T00:17:07Z- AHRS_M2 msg: 1613175427.29 epoch sec; seqNo:11168 2021-02-13T00:17:07Z- AHRS_M2 msg: 0.69 phi; 0.30 theta; -0.19 psi 2021-02-13T00:17:07Z- handleDepth()-Depth_Keller msg: 1613175427298 epoch sec; seqNo:11263; depth 20.90 2021-02-13T00:17:07Z- handleDvl() - RDI_Pathfinder msg: 1613175427.32 epoch sec; seqNo:5594 2021-02-13T00:17:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 93.32 , 100.63 , 70.31 2021-02-13T00:17:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.01 , 0.03 2021-02-13T00:17:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175427.33 epoch sec; seqNo:11263; 4058793.42 north; 592288.11 east 2021-02-13T00:17:07Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-13T00:17:07Z- waiting for 1613175425.46, time is 1613175427.33 2021-02-13T00:17:07Z- LcmTrn::updateTrn() equating pose 1613175427.33 and meas times 1613175427.33 2021-02-13T00:17:07Z- LcmTrn::updateTrn() >>>> pose time 1613175427.33, meas time 1613175427.33 <<<< 2021-02-13T00:17:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175427.33 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:17:07Z- TerrainNav::motionUpdate() - currEstimate = (4058793, 592288.11, 20.90) 2021-02-13T00:17:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:17:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:17:07Z- TNavBF::Projecting Measurements 2021-02-13T00:17:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:17:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:17:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:17:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:17:07Z- TNavFilter::4 good beams. 2021-02-13T00:17:07Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:17:07Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:17:07Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:17:07Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:17:07Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:17:07Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:17:07Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:17:07Z- TNavBF::mmse selected from filter -1 2021-02-13T00:17:07Z- TerrainMapOctree: (vehN, vehE ) = (4058793.42, 592288.11). 2021-02-13T00:17:07Z- TerrainMapOctree: Min Distance = 3143.85. 2021-02-13T00:17:07Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:17:07Z- There is no specific good beam that all particles have in common. 2021-02-13T00:17:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:17:07Z- Old Depth Bias: 0.00 DepthBiasCov: 2.51 K: 0.177 ExpectedMeasDiff: nan currMeas.covariance: 9.91 2021-02-13T00:17:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:17:07Z- Old Depth Bias: 0.00 DepthBiasCov: 2.07 K: 0.116 ExpectedMeasDiff: nan currMeas.covariance: 13.94 2021-02-13T00:17:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:17:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.82 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 16.20 2021-02-13T00:17:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:17:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.66 K: 0.147 ExpectedMeasDiff: nan currMeas.covariance: 7.91 2021-02-13T00:17:07Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:17:07Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:17:07Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:17:07Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:17:07Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:17:07Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:17:07Z- Filter 0 No beams used in subcloud update 2021-02-13T00:17:07Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:17:07Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:17:07Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 710.4 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T00:17:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175427.33 sec. 2021-02-13T00:17:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:17:07Z- TNavBF::mmse selected from filter -1 2021-02-13T00:17:07Z- variances([0], [2], and psi_berg) = 710.364978 833.003701 0.000000 2021-02-13T00:17:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:17:07Z- LcmTrn::publishEstimates() - MLE : 4058792.92 592336.61 20.90 2021-02-13T00:17:07Z- LcmTrn::publishEstimates() - MMSE : 4058793.42 592288.11 20.90 2021-02-13T00:17:07Z- LcmTrn::publishEstimates() - COVAR: 710.36 833.00 1.41 0.00 2021-02-13T00:17:07Z- AHRS_M2 msg: 1613175427.68 epoch sec; seqNo:11169 2021-02-13T00:17:07Z- AHRS_M2 msg: 0.69 phi; 0.30 theta; -0.18 psi 2021-02-13T00:17:07Z- handleDepth()-Depth_Keller msg: 1613175427689 epoch sec; seqNo:11264; depth 20.78 2021-02-13T00:17:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:07Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:07Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:07Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:07Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:07Z- handleDvl() - RDI_Pathfinder msg: 1613175427.69 epoch sec; seqNo:5595 2021-02-13T00:17:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.01 , 0.03 2021-02-13T00:17:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175427.70 epoch sec; seqNo:11264; 4058793.80 north; 592288.11 east 2021-02-13T00:17:08Z- AHRS_M2 msg: 1613175428.08 epoch sec; seqNo:11170 2021-02-13T00:17:08Z- AHRS_M2 msg: 0.68 phi; 0.30 theta; -0.18 psi 2021-02-13T00:17:08Z- handleDepth()-Depth_Keller msg: 1613175428100 epoch sec; seqNo:11265; depth 20.70 2021-02-13T00:17:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175428.11 epoch sec; seqNo:11265; 4058793.80 north; 592288.11 east 2021-02-13T00:17:08Z- AHRS_M2 msg: 1613175428.49 epoch sec; seqNo:11171 2021-02-13T00:17:08Z- AHRS_M2 msg: 0.68 phi; 0.30 theta; -0.18 psi 2021-02-13T00:17:08Z- handleDepth()-Depth_Keller msg: 1613175428492 epoch sec; seqNo:11266; depth 20.58 2021-02-13T00:17:08Z- handleDvl() - RDI_Pathfinder msg: 1613175428.51 epoch sec; seqNo:5596 2021-02-13T00:17:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 98.13 , 99.34 , 70.31 2021-02-13T00:17:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.00 , 0.02 2021-02-13T00:17:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175428.52 epoch sec; seqNo:11266; 4058794.18 north; 592288.11 east 2021-02-13T00:17:08Z- AHRS_M2 msg: 1613175428.89 epoch sec; seqNo:11172 2021-02-13T00:17:08Z- AHRS_M2 msg: 0.68 phi; 0.30 theta; -0.18 psi 2021-02-13T00:17:08Z- handleDepth()-Depth_Keller msg: 1613175428896 epoch sec; seqNo:11267; depth 20.49 2021-02-13T00:17:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:08Z- handleDvl() - RDI_Pathfinder msg: 1613175428.90 epoch sec; seqNo:5597 2021-02-13T00:17:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.00 , 0.02 2021-02-13T00:17:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175428.91 epoch sec; seqNo:11267; 4058794.18 north; 592288.11 east 2021-02-13T00:17:09Z- AHRS_M2 msg: 1613175429.30 epoch sec; seqNo:11173 2021-02-13T00:17:09Z- AHRS_M2 msg: 0.68 phi; 0.30 theta; -0.18 psi 2021-02-13T00:17:09Z- handleDepth()-Depth_Keller msg: 1613175429301 epoch sec; seqNo:11268; depth 20.38 2021-02-13T00:17:09Z- handleDvl() - RDI_Pathfinder msg: 1613175429.31 epoch sec; seqNo:5598 2021-02-13T00:17:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 98.13 , 101.78 , 70.31 2021-02-13T00:17:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.02 , 0.03 2021-02-13T00:17:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175429.32 epoch sec; seqNo:11268; 4058794.55 north; 592288.10 east 2021-02-13T00:17:09Z- AHRS_M2 msg: 1613175429.70 epoch sec; seqNo:11174 2021-02-13T00:17:09Z- AHRS_M2 msg: 0.68 phi; 0.30 theta; -0.18 psi 2021-02-13T00:17:09Z- handleDepth()-Depth_Keller msg: 1613175429706 epoch sec; seqNo:11269; depth 20.27 2021-02-13T00:17:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175429.72 epoch sec; seqNo:11269; 4058794.93 north; 592288.10 east 2021-02-13T00:17:10Z- AHRS_M2 msg: 1613175430.10 epoch sec; seqNo:11175 2021-02-13T00:17:10Z- AHRS_M2 msg: 0.67 phi; 0.30 theta; -0.19 psi 2021-02-13T00:17:10Z- handleDepth()-Depth_Keller msg: 1613175430111 epoch sec; seqNo:11270; depth 20.18 2021-02-13T00:17:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:10Z- handleDvl() - RDI_Pathfinder msg: 1613175430.11 epoch sec; seqNo:5599 2021-02-13T00:17:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.02 , 0.03 2021-02-13T00:17:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175430.14 epoch sec; seqNo:11270; 4058794.95 north; 592289.32 east 2021-02-13T00:17:10Z- AHRS_M2 msg: 1613175430.51 epoch sec; seqNo:11176 2021-02-13T00:17:10Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.19 psi 2021-02-13T00:17:10Z- handleDepth()-Depth_Keller msg: 1613175430517 epoch sec; seqNo:11271; depth 20.07 2021-02-13T00:17:10Z- handleDvl() - RDI_Pathfinder msg: 1613175430.53 epoch sec; seqNo:5600 2021-02-13T00:17:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.21 , 97.99 , 101.64 , 78.51 2021-02-13T00:17:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.05 2021-02-13T00:17:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175430.56 epoch sec; seqNo:11271; 4058795.32 north; 592289.32 east 2021-02-13T00:17:10Z- AHRS_M2 msg: 1613175430.91 epoch sec; seqNo:11177 2021-02-13T00:17:10Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.19 psi 2021-02-13T00:17:10Z- handleDepth()-Depth_Keller msg: 1613175430925 epoch sec; seqNo:11272; depth 19.96 2021-02-13T00:17:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175430.94 epoch sec; seqNo:11272; 4058795.70 north; 592289.31 east 2021-02-13T00:17:11Z- AHRS_M2 msg: 1613175431.32 epoch sec; seqNo:11178 2021-02-13T00:17:11Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.19 psi 2021-02-13T00:17:11Z- handleDepth()-Depth_Keller msg: 1613175431331 epoch sec; seqNo:11273; depth 19.86 2021-02-13T00:17:11Z- handleDvl() - RDI_Pathfinder msg: 1613175431.34 epoch sec; seqNo:5601 2021-02-13T00:17:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.21 , 94.27 , 102.79 , 69.90 2021-02-13T00:17:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.04 2021-02-13T00:17:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175431.35 epoch sec; seqNo:11273; 4058795.70 north; 592289.31 east 2021-02-13T00:17:11Z- AHRS_M2 msg: 1613175431.72 epoch sec; seqNo:11179 2021-02-13T00:17:11Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.19 psi 2021-02-13T00:17:11Z- handleDepth()-Depth_Keller msg: 1613175431733 epoch sec; seqNo:11274; depth 19.73 2021-02-13T00:17:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175431.74 epoch sec; seqNo:11274; 4058796.08 north; 592289.31 east 2021-02-13T00:17:12Z- AHRS_M2 msg: 1613175432.13 epoch sec; seqNo:11180 2021-02-13T00:17:12Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.19 psi 2021-02-13T00:17:12Z- handleDepth()-Depth_Keller msg: 1613175432138 epoch sec; seqNo:11275; depth 19.65 2021-02-13T00:17:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175432.15 epoch sec; seqNo:11275; 4058796.46 north; 592289.30 east 2021-02-13T00:17:12Z- AHRS_M2 msg: 1613175432.53 epoch sec; seqNo:11181 2021-02-13T00:17:12Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.18 psi 2021-02-13T00:17:12Z- handleDepth()-Depth_Keller msg: 1613175432532 epoch sec; seqNo:11276; depth 19.52 2021-02-13T00:17:12Z- handleDvl() - RDI_Pathfinder msg: 1613175432.55 epoch sec; seqNo:5602 2021-02-13T00:17:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.62 , 94.47 , 103.00 , 71.12 2021-02-13T00:17:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , 0.01 2021-02-13T00:17:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175432.56 epoch sec; seqNo:11276; 4058796.85 north; 592290.52 east 2021-02-13T00:17:12Z- LcmTrn::handleMessages() - handling 47 messages took 0.70 ms and 133 calls... 2021-02-13T00:17:12Z- waiting for 1613175430.33, time is 1613175432.56 2021-02-13T00:17:12Z- LcmTrn::updateTrn() equating pose 1613175432.56 and meas times 1613175432.56 2021-02-13T00:17:12Z- LcmTrn::updateTrn() >>>> pose time 1613175432.56, meas time 1613175432.56 <<<< 2021-02-13T00:17:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175432.56 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:17:12Z- TerrainNav::motionUpdate() - currEstimate = (4058797, 592290.52, 19.52) 2021-02-13T00:17:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:17:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:17:12Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175432.56 sec.; No relevant navigation data available 2021-02-13T00:17:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:17:12Z- TNavBF::mmse selected from filter -1 2021-02-13T00:17:12Z- variances([0], [2], and psi_berg) = 710.302965 833.003664 0.000000 2021-02-13T00:17:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:17:12Z- LcmTrn::publishEstimates() - MLE : 4058796.35 592340.02 19.52 2021-02-13T00:17:12Z- LcmTrn::publishEstimates() - MMSE : 4058796.85 592290.52 19.52 2021-02-13T00:17:12Z- LcmTrn::publishEstimates() - COVAR: 710.30 833.00 1.63 0.00 2021-02-13T00:17:12Z- AHRS_M2 msg: 1613175432.94 epoch sec; seqNo:11182 2021-02-13T00:17:12Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.17 psi 2021-02-13T00:17:12Z- handleDepth()-Depth_Keller msg: 1613175432942 epoch sec; seqNo:11277; depth 19.43 2021-02-13T00:17:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175432.96 epoch sec; seqNo:11277; 4058796.85 north; 592290.52 east 2021-02-13T00:17:13Z- AHRS_M2 msg: 1613175433.34 epoch sec; seqNo:11183 2021-02-13T00:17:13Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.17 psi 2021-02-13T00:17:13Z- handleDepth()-Depth_Keller msg: 1613175433340 epoch sec; seqNo:11278; depth 19.34 2021-02-13T00:17:13Z- handleDvl() - RDI_Pathfinder msg: 1613175433.35 epoch sec; seqNo:5603 2021-02-13T00:17:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.26 , 98.13 , 105.43 , 71.53 2021-02-13T00:17:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , -0.03 2021-02-13T00:17:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175433.36 epoch sec; seqNo:11278; 4058797.23 north; 592290.52 east 2021-02-13T00:17:13Z- AHRS_M2 msg: 1613175433.74 epoch sec; seqNo:11184 2021-02-13T00:17:13Z- AHRS_M2 msg: 0.68 phi; 0.31 theta; -0.16 psi 2021-02-13T00:17:13Z- handleDepth()-Depth_Keller msg: 1613175433746 epoch sec; seqNo:11279; depth 19.22 2021-02-13T00:17:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:13Z- handleDvl() - RDI_Pathfinder msg: 1613175433.75 epoch sec; seqNo:5604 2021-02-13T00:17:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , -0.03 2021-02-13T00:17:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175433.76 epoch sec; seqNo:11279; 4058797.61 north; 592290.51 east 2021-02-13T00:17:14Z- AHRS_M2 msg: 1613175434.15 epoch sec; seqNo:11185 2021-02-13T00:17:14Z- AHRS_M2 msg: 0.68 phi; 0.31 theta; -0.15 psi 2021-02-13T00:17:14Z- handleDepth()-Depth_Keller msg: 1613175434165 epoch sec; seqNo:11280; depth 19.14 2021-02-13T00:17:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175434.17 epoch sec; seqNo:11280; 4058797.61 north; 592290.51 east 2021-02-13T00:17:14Z- AHRS_M2 msg: 1613175434.55 epoch sec; seqNo:11186 2021-02-13T00:17:14Z- AHRS_M2 msg: 0.69 phi; 0.31 theta; -0.15 psi 2021-02-13T00:17:14Z- handleDepth()-Depth_Keller msg: 1613175434557 epoch sec; seqNo:11281; depth 19.00 2021-02-13T00:17:14Z- handleDvl() - RDI_Pathfinder msg: 1613175434.58 epoch sec; seqNo:5605 2021-02-13T00:17:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 93.05 , 104.01 , 70.72 2021-02-13T00:17:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.00 , -0.04 2021-02-13T00:17:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175434.59 epoch sec; seqNo:11281; 4058798.00 north; 592291.73 east 2021-02-13T00:17:14Z- AHRS_M2 msg: 1613175434.96 epoch sec; seqNo:11187 2021-02-13T00:17:14Z- AHRS_M2 msg: 0.69 phi; 0.31 theta; -0.14 psi 2021-02-13T00:17:14Z- handleDepth()-Depth_Keller msg: 1613175434961 epoch sec; seqNo:11282; depth 18.90 2021-02-13T00:17:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:14Z- handleDvl() - RDI_Pathfinder msg: 1613175434.96 epoch sec; seqNo:5606 2021-02-13T00:17:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.00 , -0.04 2021-02-13T00:17:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175434.97 epoch sec; seqNo:11282; 4058798.38 north; 592291.72 east 2021-02-13T00:17:15Z- AHRS_M2 msg: 1613175435.37 epoch sec; seqNo:11188 2021-02-13T00:17:15Z- AHRS_M2 msg: 0.69 phi; 0.31 theta; -0.14 psi 2021-02-13T00:17:15Z- handleDepth()-Depth_Keller msg: 1613175435369 epoch sec; seqNo:11283; depth 18.80 2021-02-13T00:17:15Z- handleDvl() - RDI_Pathfinder msg: 1613175435.38 epoch sec; seqNo:5607 2021-02-13T00:17:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.80 , 98.06 , 106.58 , 69.49 2021-02-13T00:17:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , -0.01 2021-02-13T00:17:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175435.39 epoch sec; seqNo:11283; 4058798.38 north; 592291.72 east 2021-02-13T00:17:15Z- AHRS_M2 msg: 1613175435.76 epoch sec; seqNo:11189 2021-02-13T00:17:15Z- AHRS_M2 msg: 0.69 phi; 0.31 theta; -0.14 psi 2021-02-13T00:17:15Z- handleDepth()-Depth_Keller msg: 1613175435764 epoch sec; seqNo:11284; depth 18.68 2021-02-13T00:17:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175435.78 epoch sec; seqNo:11284; 4058798.76 north; 592291.72 east 2021-02-13T00:17:16Z- AHRS_M2 msg: 1613175436.20 epoch sec; seqNo:11190 2021-02-13T00:17:16Z- AHRS_M2 msg: 0.69 phi; 0.31 theta; -0.15 psi 2021-02-13T00:17:16Z- handleDepth()-Depth_Keller msg: 1613175436201 epoch sec; seqNo:11285; depth 18.58 2021-02-13T00:17:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:16Z- handleDvl() - RDI_Pathfinder msg: 1613175436.20 epoch sec; seqNo:5608 2021-02-13T00:17:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , -0.01 2021-02-13T00:17:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175436.21 epoch sec; seqNo:11285; 4058799.14 north; 592291.72 east 2021-02-13T00:17:16Z- AHRS_M2 msg: 1613175436.57 epoch sec; seqNo:11191 2021-02-13T00:17:16Z- AHRS_M2 msg: 0.68 phi; 0.31 theta; -0.15 psi 2021-02-13T00:17:16Z- handleDepth()-Depth_Keller msg: 1613175436577 epoch sec; seqNo:11286; depth 18.45 2021-02-13T00:17:16Z- handleDvl() - RDI_Pathfinder msg: 1613175436.59 epoch sec; seqNo:5609 2021-02-13T00:17:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 94.27 , 104.01 , 71.53 2021-02-13T00:17:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , 0.03 2021-02-13T00:17:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175436.60 epoch sec; seqNo:11286; 4058799.14 north; 592291.72 east 2021-02-13T00:17:16Z- AHRS_M2 msg: 1613175436.97 epoch sec; seqNo:11192 2021-02-13T00:17:16Z- AHRS_M2 msg: 0.68 phi; 0.31 theta; -0.16 psi 2021-02-13T00:17:16Z- handleDepth()-Depth_Keller msg: 1613175436978 epoch sec; seqNo:11287; depth 18.37 2021-02-13T00:17:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175436.99 epoch sec; seqNo:11287; 4058799.52 north; 592291.71 east 2021-02-13T00:17:17Z- AHRS_M2 msg: 1613175437.38 epoch sec; seqNo:11193 2021-02-13T00:17:17Z- AHRS_M2 msg: 0.68 phi; 0.31 theta; -0.17 psi 2021-02-13T00:17:17Z- handleDepth()-Depth_Keller msg: 1613175437386 epoch sec; seqNo:11288; depth 18.28 2021-02-13T00:17:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:17Z- handleDvl() - RDI_Pathfinder msg: 1613175437.39 epoch sec; seqNo:5610 2021-02-13T00:17:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , 0.03 2021-02-13T00:17:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175437.43 epoch sec; seqNo:11288; 4058799.91 north; 592292.93 east 2021-02-13T00:17:17Z- LcmTrn::handleMessages() - handling 44 messages took 0.60 ms and 132 calls... 2021-02-13T00:17:17Z- waiting for 1613175435.56, time is 1613175437.43 2021-02-13T00:17:17Z- LcmTrn::updateTrn() equating pose 1613175437.43 and meas times 1613175437.43 2021-02-13T00:17:17Z- LcmTrn::updateTrn() >>>> pose time 1613175437.43, meas time 1613175437.43 <<<< 2021-02-13T00:17:17Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175437.43, ping # 0. 2021-02-13T00:17:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175437.43 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:17:17Z- TerrainNav::motionUpdate() - currEstimate = (4058800, 592292.93, 18.28) 2021-02-13T00:17:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:17:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:17:17Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:17:17Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:17:17Z- TNavBF::Projecting Measurements 2021-02-13T00:17:17Z- TNavFilter::Measurement from time = 1613175437.43 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:17:17Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:17:17Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:17:17Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:17:17Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:17:17Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:17:17Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:17:17Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:17:17Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 710.2 EastVariance: 833.0 DepthVariance: 1.8 this->DepthBiasCov: 1.8 2021-02-13T00:17:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:17:17Z- TNavBF::mmse selected from filter -1 2021-02-13T00:17:17Z- variances([0], [2], and psi_berg) = 710.243266 833.003629 0.000000 2021-02-13T00:17:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:17:17Z- LcmTrn::publishEstimates() - MLE : 4058799.41 592342.43 18.28 2021-02-13T00:17:17Z- LcmTrn::publishEstimates() - MMSE : 4058799.91 592292.93 18.28 2021-02-13T00:17:17Z- LcmTrn::publishEstimates() - COVAR: 710.24 833.00 1.83 0.00 2021-02-13T00:17:17Z- AHRS_M2 msg: 1613175437.78 epoch sec; seqNo:11194 2021-02-13T00:17:17Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.17 psi 2021-02-13T00:17:17Z- handleDepth()-Depth_Keller msg: 1613175437789 epoch sec; seqNo:11289; depth 18.17 2021-02-13T00:17:17Z- handleDvl() - RDI_Pathfinder msg: 1613175437.80 epoch sec; seqNo:5611 2021-02-13T00:17:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.80 , 93.19 , 104.14 , 78.58 2021-02-13T00:17:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , 0.07 2021-02-13T00:17:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175437.81 epoch sec; seqNo:11289; 4058799.91 north; 592292.93 east 2021-02-13T00:17:18Z- AHRS_M2 msg: 1613175438.19 epoch sec; seqNo:11195 2021-02-13T00:17:18Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.17 psi 2021-02-13T00:17:18Z- handleDepth()-Depth_Keller msg: 1613175438194 epoch sec; seqNo:11290; depth 18.04 2021-02-13T00:17:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175438.20 epoch sec; seqNo:11290; 4058800.29 north; 592292.92 east 2021-02-13T00:17:18Z- AHRS_M2 msg: 1613175438.59 epoch sec; seqNo:11196 2021-02-13T00:17:18Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.17 psi 2021-02-13T00:17:18Z- handleDepth()-Depth_Keller msg: 1613175438593 epoch sec; seqNo:11291; depth 17.96 2021-02-13T00:17:18Z- handleDvl() - RDI_Pathfinder msg: 1613175438.61 epoch sec; seqNo:5612 2021-02-13T00:17:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 94.20 , 106.38 , 71.53 2021-02-13T00:17:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , 0.09 2021-02-13T00:17:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175438.62 epoch sec; seqNo:11291; 4058800.67 north; 592292.92 east 2021-02-13T00:17:18Z- AHRS_M2 msg: 1613175438.99 epoch sec; seqNo:11197 2021-02-13T00:17:18Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.16 psi 2021-02-13T00:17:19Z- handleDepth()-Depth_Keller msg: 1613175439001 epoch sec; seqNo:11292; depth 17.85 2021-02-13T00:17:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175439.01 epoch sec; seqNo:11292; 4058800.67 north; 592292.92 east 2021-02-13T00:17:19Z- AHRS_M2 msg: 1613175439.40 epoch sec; seqNo:11198 2021-02-13T00:17:19Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.16 psi 2021-02-13T00:17:19Z- handleDepth()-Depth_Keller msg: 1613175439400 epoch sec; seqNo:11293; depth 17.75 2021-02-13T00:17:19Z- handleDvl() - RDI_Pathfinder msg: 1613175439.42 epoch sec; seqNo:5613 2021-02-13T00:17:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.30 , 97.99 , 106.51 , 72.35 2021-02-13T00:17:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , 0.04 2021-02-13T00:17:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175439.43 epoch sec; seqNo:11293; 4058801.05 north; 592292.92 east 2021-02-13T00:17:19Z- AHRS_M2 msg: 1613175439.80 epoch sec; seqNo:11199 2021-02-13T00:17:19Z- AHRS_M2 msg: 0.69 phi; 0.32 theta; -0.16 psi 2021-02-13T00:17:19Z- handleDepth()-Depth_Keller msg: 1613175439809 epoch sec; seqNo:11294; depth 17.64 2021-02-13T00:17:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:19Z- handleDvl() - RDI_Pathfinder msg: 1613175439.81 epoch sec; seqNo:5614 2021-02-13T00:17:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , 0.04 2021-02-13T00:17:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175439.83 epoch sec; seqNo:11294; 4058801.44 north; 592294.13 east 2021-02-13T00:17:20Z- AHRS_M2 msg: 1613175440.20 epoch sec; seqNo:11200 2021-02-13T00:17:20Z- AHRS_M2 msg: 0.69 phi; 0.32 theta; -0.15 psi 2021-02-13T00:17:20Z- handleDepth()-Depth_Keller msg: 1613175440208 epoch sec; seqNo:11295; depth 17.54 2021-02-13T00:17:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175440.22 epoch sec; seqNo:11295; 4058801.44 north; 592294.13 east 2021-02-13T00:17:20Z- AHRS_M2 msg: 1613175440.61 epoch sec; seqNo:11201 2021-02-13T00:17:20Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.15 psi 2021-02-13T00:17:20Z- handleDepth()-Depth_Keller msg: 1613175440614 epoch sec; seqNo:11296; depth 17.43 2021-02-13T00:17:20Z- handleDvl() - RDI_Pathfinder msg: 1613175440.63 epoch sec; seqNo:5615 2021-02-13T00:17:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 170.89 , 97.86 , 106.38 , 70.31 2021-02-13T00:17:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.11 , -0.03 2021-02-13T00:17:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175440.64 epoch sec; seqNo:11296; 4058801.82 north; 592294.13 east 2021-02-13T00:17:21Z- AHRS_M2 msg: 1613175441.01 epoch sec; seqNo:11202 2021-02-13T00:17:21Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.15 psi 2021-02-13T00:17:21Z- handleDepth()-Depth_Keller msg: 1613175441016 epoch sec; seqNo:11297; depth 17.31 2021-02-13T00:17:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:21Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:21Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:21Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:21Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:21Z- handleDvl() - RDI_Pathfinder msg: 1613175441.02 epoch sec; seqNo:5616 2021-02-13T00:17:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.11 , -0.03 2021-02-13T00:17:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175441.04 epoch sec; seqNo:11297; 4058802.20 north; 592294.12 east 2021-02-13T00:17:21Z- AHRS_M2 msg: 1613175441.42 epoch sec; seqNo:11203 2021-02-13T00:17:21Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.16 psi 2021-02-13T00:17:21Z- handleDepth()-Depth_Keller msg: 1613175441420 epoch sec; seqNo:11298; depth 17.20 2021-02-13T00:17:21Z- handleDvl() - RDI_Pathfinder msg: 1613175441.43 epoch sec; seqNo:5617 2021-02-13T00:17:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 95.75 , 106.17 , 70.40 2021-02-13T00:17:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.10 2021-02-13T00:17:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175441.44 epoch sec; seqNo:11298; 4058802.20 north; 592294.12 east 2021-02-13T00:17:21Z- AHRS_M2 msg: 1613175441.82 epoch sec; seqNo:11204 2021-02-13T00:17:21Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.17 psi 2021-02-13T00:17:21Z- handleDepth()-Depth_Keller msg: 1613175441826 epoch sec; seqNo:11299; depth 17.11 2021-02-13T00:17:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175441.84 epoch sec; seqNo:11299; 4058802.59 north; 592295.34 east 2021-02-13T00:17:22Z- AHRS_M2 msg: 1613175442.22 epoch sec; seqNo:11205 2021-02-13T00:17:22Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.17 psi 2021-02-13T00:17:22Z- handleDepth()-Depth_Keller msg: 1613175442229 epoch sec; seqNo:11300; depth 16.98 2021-02-13T00:17:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:22Z- handleDvl() - RDI_Pathfinder msg: 1613175442.23 epoch sec; seqNo:5618 2021-02-13T00:17:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.10 2021-02-13T00:17:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175442.25 epoch sec; seqNo:11300; 4058802.97 north; 592295.34 east 2021-02-13T00:17:22Z- AHRS_M2 msg: 1613175442.63 epoch sec; seqNo:11206 2021-02-13T00:17:22Z- AHRS_M2 msg: 0.69 phi; 0.32 theta; -0.18 psi 2021-02-13T00:17:22Z- handleDepth()-Depth_Keller msg: 1613175442637 epoch sec; seqNo:11301; depth 16.88 2021-02-13T00:17:22Z- handleDvl() - RDI_Pathfinder msg: 1613175442.65 epoch sec; seqNo:5619 2021-02-13T00:17:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 97.71 , 107.45 , 78.24 2021-02-13T00:17:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.15 , -0.05 2021-02-13T00:17:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175442.67 epoch sec; seqNo:11301; 4058802.97 north; 592295.34 east 2021-02-13T00:17:22Z- LcmTrn::handleMessages() - handling 48 messages took 0.71 ms and 134 calls... 2021-02-13T00:17:22Z- waiting for 1613175440.43, time is 1613175442.67 2021-02-13T00:17:22Z- LcmTrn::updateTrn() equating pose 1613175442.67 and meas times 1613175442.67 2021-02-13T00:17:22Z- LcmTrn::updateTrn() >>>> pose time 1613175442.67, meas time 1613175442.67 <<<< 2021-02-13T00:17:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175442.67 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:17:22Z- TerrainNav::motionUpdate() - currEstimate = (4058803, 592295.34, 16.88) 2021-02-13T00:17:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:17:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:17:22Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175442.67 sec.; No relevant navigation data available 2021-02-13T00:17:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:17:22Z- TNavBF::mmse selected from filter -1 2021-02-13T00:17:22Z- variances([0], [2], and psi_berg) = 710.183606 833.003594 0.000000 2021-02-13T00:17:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:17:22Z- LcmTrn::publishEstimates() - MLE : 4058802.47 592344.84 16.88 2021-02-13T00:17:22Z- LcmTrn::publishEstimates() - MMSE : 4058802.97 592295.34 16.88 2021-02-13T00:17:22Z- LcmTrn::publishEstimates() - COVAR: 710.18 833.00 2.02 0.00 2021-02-13T00:17:23Z- AHRS_M2 msg: 1613175443.03 epoch sec; seqNo:11207 2021-02-13T00:17:23Z- AHRS_M2 msg: 0.69 phi; 0.32 theta; -0.18 psi 2021-02-13T00:17:23Z- handleDepth()-Depth_Keller msg: 1613175443036 epoch sec; seqNo:11302; depth 16.78 2021-02-13T00:17:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175443.05 epoch sec; seqNo:11302; 4058803.35 north; 592295.33 east 2021-02-13T00:17:23Z- AHRS_M2 msg: 1613175443.44 epoch sec; seqNo:11208 2021-02-13T00:17:23Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.18 psi 2021-02-13T00:17:23Z- handleDepth()-Depth_Keller msg: 1613175443442 epoch sec; seqNo:11303; depth 16.65 2021-02-13T00:17:23Z- handleDvl() - RDI_Pathfinder msg: 1613175443.45 epoch sec; seqNo:5620 2021-02-13T00:17:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 100.09 , 107.39 , 78.18 2021-02-13T00:17:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.16 , 0.06 2021-02-13T00:17:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175443.46 epoch sec; seqNo:11303; 4058803.73 north; 592295.33 east 2021-02-13T00:17:23Z- AHRS_M2 msg: 1613175443.84 epoch sec; seqNo:11209 2021-02-13T00:17:23Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.18 psi 2021-02-13T00:17:23Z- handleDepth()-Depth_Keller msg: 1613175443854 epoch sec; seqNo:11304; depth 16.54 2021-02-13T00:17:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175443.87 epoch sec; seqNo:11304; 4058804.10 north; 592295.33 east 2021-02-13T00:17:24Z- AHRS_M2 msg: 1613175444.24 epoch sec; seqNo:11210 2021-02-13T00:17:24Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.18 psi 2021-02-13T00:17:24Z- handleDepth()-Depth_Keller msg: 1613175444253 epoch sec; seqNo:11305; depth 16.43 2021-02-13T00:17:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175444.26 epoch sec; seqNo:11305; 4058804.48 north; 592295.32 east 2021-02-13T00:17:24Z- AHRS_M2 msg: 1613175444.65 epoch sec; seqNo:11211 2021-02-13T00:17:24Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.18 psi 2021-02-13T00:17:24Z- handleDepth()-Depth_Keller msg: 1613175444652 epoch sec; seqNo:11306; depth 16.32 2021-02-13T00:17:24Z- handleDvl() - RDI_Pathfinder msg: 1613175444.66 epoch sec; seqNo:5621 2021-02-13T00:17:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.83 , 97.65 , 108.61 , 73.17 2021-02-13T00:17:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , 0.14 2021-02-13T00:17:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175444.67 epoch sec; seqNo:11306; 4058804.48 north; 592295.32 east 2021-02-13T00:17:25Z- AHRS_M2 msg: 1613175445.05 epoch sec; seqNo:11212 2021-02-13T00:17:25Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.18 psi 2021-02-13T00:17:25Z- handleDepth()-Depth_Keller msg: 1613175445059 epoch sec; seqNo:11307; depth 16.22 2021-02-13T00:17:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175445.07 epoch sec; seqNo:11307; 4058804.87 north; 592296.54 east 2021-02-13T00:17:25Z- AHRS_M2 msg: 1613175445.46 epoch sec; seqNo:11213 2021-02-13T00:17:25Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.18 psi 2021-02-13T00:17:25Z- handleDepth()-Depth_Keller msg: 1613175445460 epoch sec; seqNo:11308; depth 16.09 2021-02-13T00:17:25Z- handleDvl() - RDI_Pathfinder msg: 1613175445.47 epoch sec; seqNo:5622 2021-02-13T00:17:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 96.50 , 109.89 , 78.24 2021-02-13T00:17:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , 0.09 2021-02-13T00:17:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175445.49 epoch sec; seqNo:11308; 4058805.25 north; 592296.53 east 2021-02-13T00:17:25Z- AHRS_M2 msg: 1613175445.86 epoch sec; seqNo:11214 2021-02-13T00:17:25Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.17 psi 2021-02-13T00:17:25Z- handleDepth()-Depth_Keller msg: 1613175445869 epoch sec; seqNo:11309; depth 15.99 2021-02-13T00:17:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:25Z- handleDvl() - RDI_Pathfinder msg: 1613175445.87 epoch sec; seqNo:5623 2021-02-13T00:17:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , 0.09 2021-02-13T00:17:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175445.88 epoch sec; seqNo:11309; 4058805.25 north; 592296.53 east 2021-02-13T00:17:26Z- AHRS_M2 msg: 1613175446.27 epoch sec; seqNo:11215 2021-02-13T00:17:26Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.17 psi 2021-02-13T00:17:26Z- handleDepth()-Depth_Keller msg: 1613175446277 epoch sec; seqNo:11310; depth 15.88 2021-02-13T00:17:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175446.29 epoch sec; seqNo:11310; 4058805.63 north; 592296.53 east 2021-02-13T00:17:26Z- AHRS_M2 msg: 1613175446.67 epoch sec; seqNo:11216 2021-02-13T00:17:26Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.16 psi 2021-02-13T00:17:26Z- handleDepth()-Depth_Keller msg: 1613175446685 epoch sec; seqNo:11311; depth 15.77 2021-02-13T00:17:26Z- handleDvl() - RDI_Pathfinder msg: 1613175446.70 epoch sec; seqNo:5624 2021-02-13T00:17:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.86 , 109.82 , 78.17 2021-02-13T00:17:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , -0.04 2021-02-13T00:17:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175446.71 epoch sec; seqNo:11311; 4058806.01 north; 592296.53 east 2021-02-13T00:17:27Z- AHRS_M2 msg: 1613175447.07 epoch sec; seqNo:11217 2021-02-13T00:17:27Z- AHRS_M2 msg: 0.68 phi; 0.33 theta; -0.16 psi 2021-02-13T00:17:27Z- handleDepth()-Depth_Keller msg: 1613175447081 epoch sec; seqNo:11312; depth 15.64 2021-02-13T00:17:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:27Z- handleDvl() - RDI_Pathfinder msg: 1613175447.08 epoch sec; seqNo:5625 2021-02-13T00:17:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , -0.04 2021-02-13T00:17:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175447.10 epoch sec; seqNo:11312; 4058806.02 north; 592297.75 east 2021-02-13T00:17:27Z- AHRS_M2 msg: 1613175447.47 epoch sec; seqNo:11218 2021-02-13T00:17:27Z- AHRS_M2 msg: 0.68 phi; 0.33 theta; -0.15 psi 2021-02-13T00:17:27Z- handleDepth()-Depth_Keller msg: 1613175447480 epoch sec; seqNo:11313; depth 15.54 2021-02-13T00:17:27Z- handleDvl() - RDI_Pathfinder msg: 1613175447.49 epoch sec; seqNo:5626 2021-02-13T00:17:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.45 , 113.68 , 73.17 2021-02-13T00:17:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , -0.12 2021-02-13T00:17:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175447.52 epoch sec; seqNo:11313; 4058806.40 north; 592297.74 east 2021-02-13T00:17:27Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-13T00:17:27Z- waiting for 1613175445.67, time is 1613175447.52 2021-02-13T00:17:27Z- LcmTrn::updateTrn() equating pose 1613175447.52 and meas times 1613175447.52 2021-02-13T00:17:27Z- LcmTrn::updateTrn() >>>> pose time 1613175447.52, meas time 1613175447.52 <<<< 2021-02-13T00:17:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175447.52 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:17:27Z- TerrainNav::motionUpdate() - currEstimate = (4058806, 592297.74, 15.54) 2021-02-13T00:17:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:17:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:17:27Z- TNavBF::Projecting Measurements 2021-02-13T00:17:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:17:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:17:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:17:27Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:17:27Z- TNavFilter::3 good beams. 2021-02-13T00:17:27Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:17:27Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:17:27Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:17:27Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:17:27Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:17:27Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:17:27Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:17:27Z- TNavBF::mmse selected from filter -1 2021-02-13T00:17:27Z- TerrainMapOctree: (vehN, vehE ) = (4058806.40, 592297.74). 2021-02-13T00:17:27Z- TerrainMapOctree: Min Distance = 3127.69. 2021-02-13T00:17:27Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:17:27Z- There is no specific good beam that all particles have in common. 2021-02-13T00:17:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:17:27Z- Old Depth Bias: 0.00 DepthBiasCov: 2.24 K: 0.117 ExpectedMeasDiff: nan currMeas.covariance: 15.19 2021-02-13T00:17:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:17:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.98 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 20.68 2021-02-13T00:17:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:17:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.82 K: 0.150 ExpectedMeasDiff: nan currMeas.covariance: 8.57 2021-02-13T00:17:27Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:17:27Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:17:27Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:17:27Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:17:27Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:17:27Z- Filter 0 No beams used in subcloud update 2021-02-13T00:17:27Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:17:27Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:17:27Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 710.1 EastVariance: 833.0 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-13T00:17:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175447.52 sec. 2021-02-13T00:17:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:17:27Z- TNavBF::mmse selected from filter -1 2021-02-13T00:17:27Z- variances([0], [2], and psi_berg) = 710.121716 833.003558 0.000000 2021-02-13T00:17:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:17:27Z- LcmTrn::publishEstimates() - MLE : 4058805.90 592347.24 15.54 2021-02-13T00:17:27Z- LcmTrn::publishEstimates() - MMSE : 4058806.40 592297.74 15.54 2021-02-13T00:17:27Z- LcmTrn::publishEstimates() - COVAR: 710.12 833.00 1.54 0.00 2021-02-13T00:17:27Z- AHRS_M2 msg: 1613175447.88 epoch sec; seqNo:11219 2021-02-13T00:17:27Z- AHRS_M2 msg: 0.69 phi; 0.33 theta; -0.14 psi 2021-02-13T00:17:27Z- handleDepth()-Depth_Keller msg: 1613175447890 epoch sec; seqNo:11314; depth 15.41 2021-02-13T00:17:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175447.90 epoch sec; seqNo:11314; 4058806.78 north; 592297.74 east 2021-02-13T00:17:28Z- AHRS_M2 msg: 1613175448.28 epoch sec; seqNo:11220 2021-02-13T00:17:28Z- AHRS_M2 msg: 0.70 phi; 0.33 theta; -0.14 psi 2021-02-13T00:17:28Z- handleDepth()-Depth_Keller msg: 1613175448293 epoch sec; seqNo:11315; depth 15.32 2021-02-13T00:17:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:28Z- handleDvl() - RDI_Pathfinder msg: 1613175448.30 epoch sec; seqNo:5627 2021-02-13T00:17:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , -0.12 2021-02-13T00:17:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175448.31 epoch sec; seqNo:11315; 4058806.78 north; 592297.74 east 2021-02-13T00:17:28Z- AHRS_M2 msg: 1613175448.69 epoch sec; seqNo:11221 2021-02-13T00:17:28Z- AHRS_M2 msg: 0.71 phi; 0.32 theta; -0.13 psi 2021-02-13T00:17:28Z- handleDepth()-Depth_Keller msg: 1613175448697 epoch sec; seqNo:11316; depth 15.20 2021-02-13T00:17:28Z- handleDvl() - RDI_Pathfinder msg: 1613175448.71 epoch sec; seqNo:5628 2021-02-13T00:17:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.64 , 97.52 , 109.69 , 78.58 2021-02-13T00:17:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , -0.12 2021-02-13T00:17:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175448.74 epoch sec; seqNo:11316; 4058807.16 north; 592297.73 east 2021-02-13T00:17:29Z- AHRS_M2 msg: 1613175449.09 epoch sec; seqNo:11222 2021-02-13T00:17:29Z- AHRS_M2 msg: 0.71 phi; 0.32 theta; -0.13 psi 2021-02-13T00:17:29Z- handleDepth()-Depth_Keller msg: 1613175449097 epoch sec; seqNo:11317; depth 15.10 2021-02-13T00:17:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175449.11 epoch sec; seqNo:11317; 4058807.54 north; 592297.73 east 2021-02-13T00:17:29Z- AHRS_M2 msg: 1613175449.50 epoch sec; seqNo:11223 2021-02-13T00:17:29Z- AHRS_M2 msg: 0.71 phi; 0.32 theta; -0.13 psi 2021-02-13T00:17:29Z- handleDepth()-Depth_Keller msg: 1613175449502 epoch sec; seqNo:11318; depth 14.98 2021-02-13T00:17:29Z- handleDvl() - RDI_Pathfinder msg: 1613175449.51 epoch sec; seqNo:5629 2021-02-13T00:17:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 97.71 , 109.89 , 73.57 2021-02-13T00:17:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.15 , -0.04 2021-02-13T00:17:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175449.52 epoch sec; seqNo:11318; 4058807.54 north; 592297.73 east 2021-02-13T00:17:29Z- AHRS_M2 msg: 1613175449.90 epoch sec; seqNo:11224 2021-02-13T00:17:29Z- AHRS_M2 msg: 0.71 phi; 0.32 theta; -0.12 psi 2021-02-13T00:17:29Z- handleDepth()-Depth_Keller msg: 1613175449909 epoch sec; seqNo:11319; depth 14.85 2021-02-13T00:17:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175449.93 epoch sec; seqNo:11319; 4058807.93 north; 592298.95 east 2021-02-13T00:17:30Z- AHRS_M2 msg: 1613175450.31 epoch sec; seqNo:11225 2021-02-13T00:17:30Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.12 psi 2021-02-13T00:17:30Z- handleDepth()-Depth_Keller msg: 1613175450314 epoch sec; seqNo:11320; depth 14.76 2021-02-13T00:17:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175450.35 epoch sec; seqNo:11320; 4058808.31 north; 592298.94 east 2021-02-13T00:17:30Z- AHRS_M2 msg: 1613175450.71 epoch sec; seqNo:11226 2021-02-13T00:17:30Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.12 psi 2021-02-13T00:17:30Z- handleDepth()-Depth_Keller msg: 1613175450712 epoch sec; seqNo:11321; depth 14.63 2021-02-13T00:17:30Z- handleDvl() - RDI_Pathfinder msg: 1613175450.72 epoch sec; seqNo:5630 2021-02-13T00:17:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.26 , 96.43 , 109.82 , 73.98 2021-02-13T00:17:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.15 , 0.09 2021-02-13T00:17:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175450.73 epoch sec; seqNo:11321; 4058808.31 north; 592298.94 east 2021-02-13T00:17:31Z- AHRS_M2 msg: 1613175451.11 epoch sec; seqNo:11227 2021-02-13T00:17:31Z- AHRS_M2 msg: 0.69 phi; 0.32 theta; -0.13 psi 2021-02-13T00:17:31Z- handleDepth()-Depth_Keller msg: 1613175451125 epoch sec; seqNo:11322; depth 14.53 2021-02-13T00:17:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175451.14 epoch sec; seqNo:11322; 4058808.69 north; 592298.94 east 2021-02-13T00:17:31Z- AHRS_M2 msg: 1613175451.52 epoch sec; seqNo:11228 2021-02-13T00:17:31Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.14 psi 2021-02-13T00:17:31Z- handleDepth()-Depth_Keller msg: 1613175451520 epoch sec; seqNo:11323; depth 14.40 2021-02-13T00:17:31Z- handleDvl() - RDI_Pathfinder msg: 1613175451.54 epoch sec; seqNo:5631 2021-02-13T00:17:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.48 , 95.21 , 109.82 , 74.39 2021-02-13T00:17:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , 0.18 2021-02-13T00:17:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175451.55 epoch sec; seqNo:11323; 4058809.07 north; 592298.93 east 2021-02-13T00:17:31Z- AHRS_M2 msg: 1613175451.92 epoch sec; seqNo:11229 2021-02-13T00:17:31Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.14 psi 2021-02-13T00:17:31Z- handleDepth()-Depth_Keller msg: 1613175451929 epoch sec; seqNo:11324; depth 14.28 2021-02-13T00:17:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:31Z- handleDvl() - RDI_Pathfinder msg: 1613175451.93 epoch sec; seqNo:5632 2021-02-13T00:17:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , 0.18 2021-02-13T00:17:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175451.95 epoch sec; seqNo:11324; 4058809.45 north; 592298.93 east 2021-02-13T00:17:32Z- AHRS_M2 msg: 1613175452.32 epoch sec; seqNo:11230 2021-02-13T00:17:32Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.15 psi 2021-02-13T00:17:32Z- handleDepth()-Depth_Keller msg: 1613175452333 epoch sec; seqNo:11325; depth 14.18 2021-02-13T00:17:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175452.34 epoch sec; seqNo:11325; 4058809.45 north; 592298.93 east 2021-02-13T00:17:32Z- AHRS_M2 msg: 1613175452.73 epoch sec; seqNo:11231 2021-02-13T00:17:32Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.16 psi 2021-02-13T00:17:32Z- handleDepth()-Depth_Keller msg: 1613175452734 epoch sec; seqNo:11326; depth 14.07 2021-02-13T00:17:32Z- handleDvl() - RDI_Pathfinder msg: 1613175452.75 epoch sec; seqNo:5633 2021-02-13T00:17:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.26 , 96.43 , 109.82 , 78.17 2021-02-13T00:17:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.15 2021-02-13T00:17:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175452.76 epoch sec; seqNo:11326; 4058809.84 north; 592300.15 east 2021-02-13T00:17:32Z- LcmTrn::handleMessages() - handling 46 messages took 0.71 ms and 132 calls... 2021-02-13T00:17:32Z- waiting for 1613175450.52, time is 1613175452.76 2021-02-13T00:17:32Z- LcmTrn::updateTrn() equating pose 1613175452.76 and meas times 1613175452.76 2021-02-13T00:17:32Z- LcmTrn::updateTrn() >>>> pose time 1613175452.76, meas time 1613175452.76 <<<< 2021-02-13T00:17:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175452.76 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:17:32Z- TerrainNav::motionUpdate() - currEstimate = (4058810, 592300.15, 14.07) 2021-02-13T00:17:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:17:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:17:32Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175452.76 sec.; No relevant navigation data available 2021-02-13T00:17:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:17:32Z- TNavBF::mmse selected from filter -1 2021-02-13T00:17:32Z- variances([0], [2], and psi_berg) = 710.059869 833.003522 0.000000 2021-02-13T00:17:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:17:32Z- LcmTrn::publishEstimates() - MLE : 4058809.34 592349.65 14.07 2021-02-13T00:17:32Z- LcmTrn::publishEstimates() - MMSE : 4058809.84 592300.15 14.07 2021-02-13T00:17:32Z- LcmTrn::publishEstimates() - COVAR: 710.06 833.00 1.76 0.00 2021-02-13T00:17:33Z- AHRS_M2 msg: 1613175453.13 epoch sec; seqNo:11232 2021-02-13T00:17:33Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.17 psi 2021-02-13T00:17:33Z- handleDepth()-Depth_Keller msg: 1613175453140 epoch sec; seqNo:11327; depth 13.94 2021-02-13T00:17:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:33Z- handleDvl() - RDI_Pathfinder msg: 1613175453.14 epoch sec; seqNo:5634 2021-02-13T00:17:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.15 2021-02-13T00:17:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175453.16 epoch sec; seqNo:11327; 4058810.22 north; 592300.14 east 2021-02-13T00:17:33Z- AHRS_M2 msg: 1613175453.54 epoch sec; seqNo:11233 2021-02-13T00:17:33Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.17 psi 2021-02-13T00:17:33Z- handleDepth()-Depth_Keller msg: 1613175453540 epoch sec; seqNo:11328; depth 13.84 2021-02-13T00:17:33Z- handleDvl() - RDI_Pathfinder msg: 1613175453.55 epoch sec; seqNo:5635 2021-02-13T00:17:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 99.07 , 112.59 , 75.21 2021-02-13T00:17:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.06 , 0.04 2021-02-13T00:17:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175453.58 epoch sec; seqNo:11328; 4058810.60 north; 592300.14 east 2021-02-13T00:17:33Z- AHRS_M2 msg: 1613175453.94 epoch sec; seqNo:11234 2021-02-13T00:17:33Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.17 psi 2021-02-13T00:17:33Z- handleDepth()-Depth_Keller msg: 1613175453953 epoch sec; seqNo:11329; depth 13.72 2021-02-13T00:17:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175453.96 epoch sec; seqNo:11329; 4058810.60 north; 592300.14 east 2021-02-13T00:17:34Z- AHRS_M2 msg: 1613175454.35 epoch sec; seqNo:11235 2021-02-13T00:17:34Z- AHRS_M2 msg: 0.68 phi; 0.34 theta; -0.17 psi 2021-02-13T00:17:34Z- handleDepth()-Depth_Keller msg: 1613175454351 epoch sec; seqNo:11330; depth 13.60 2021-02-13T00:17:34Z- handleDvl() - RDI_Pathfinder msg: 1613175454.38 epoch sec; seqNo:5636 2021-02-13T00:17:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 101.71 , 113.88 , 74.80 2021-02-13T00:17:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.05 , -0.08 2021-02-13T00:17:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175454.39 epoch sec; seqNo:11330; 4058810.98 north; 592300.13 east 2021-02-13T00:17:34Z- AHRS_M2 msg: 1613175454.75 epoch sec; seqNo:11236 2021-02-13T00:17:34Z- AHRS_M2 msg: 0.69 phi; 0.34 theta; -0.17 psi 2021-02-13T00:17:34Z- handleDepth()-Depth_Keller msg: 1613175454757 epoch sec; seqNo:11331; depth 13.50 2021-02-13T00:17:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175454.77 epoch sec; seqNo:11331; 4058811.37 north; 592301.35 east 2021-02-13T00:17:35Z- AHRS_M2 msg: 1613175455.15 epoch sec; seqNo:11237 2021-02-13T00:17:35Z- AHRS_M2 msg: 0.70 phi; 0.34 theta; -0.17 psi 2021-02-13T00:17:35Z- handleDepth()-Depth_Keller msg: 1613175455158 epoch sec; seqNo:11332; depth 13.39 2021-02-13T00:17:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175455.17 epoch sec; seqNo:11332; 4058811.37 north; 592301.35 east 2021-02-13T00:17:35Z- AHRS_M2 msg: 1613175455.56 epoch sec; seqNo:11238 2021-02-13T00:17:35Z- AHRS_M2 msg: 0.70 phi; 0.34 theta; -0.16 psi 2021-02-13T00:17:35Z- handleDepth()-Depth_Keller msg: 1613175455573 epoch sec; seqNo:11333; depth 13.30 2021-02-13T00:17:35Z- handleDvl() - RDI_Pathfinder msg: 1613175455.59 epoch sec; seqNo:5637 2021-02-13T00:17:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 101.64 , 116.51 , 78.65 2021-02-13T00:17:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , -0.16 2021-02-13T00:17:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175455.59 epoch sec; seqNo:11333; 4058811.75 north; 592301.35 east 2021-02-13T00:17:35Z- AHRS_M2 msg: 1613175455.96 epoch sec; seqNo:11239 2021-02-13T00:17:35Z- AHRS_M2 msg: 0.71 phi; 0.33 theta; -0.15 psi 2021-02-13T00:17:35Z- handleDepth()-Depth_Keller msg: 1613175455969 epoch sec; seqNo:11334; depth 13.18 2021-02-13T00:17:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175455.99 epoch sec; seqNo:11334; 4058812.13 north; 592301.34 east 2021-02-13T00:17:36Z- AHRS_M2 msg: 1613175456.38 epoch sec; seqNo:11240 2021-02-13T00:17:36Z- AHRS_M2 msg: 0.72 phi; 0.33 theta; -0.15 psi 2021-02-13T00:17:36Z- handleDepth()-Depth_Keller msg: 1613175456400 epoch sec; seqNo:11335; depth 13.05 2021-02-13T00:17:36Z- handleDvl() - RDI_Pathfinder msg: 1613175456.41 epoch sec; seqNo:5638 2021-02-13T00:17:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.27 , 101.50 , 115.03 , 78.51 2021-02-13T00:17:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.14 , -0.14 2021-02-13T00:17:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175456.42 epoch sec; seqNo:11335; 4058812.13 north; 592301.34 east 2021-02-13T00:17:36Z- AHRS_M2 msg: 1613175456.77 epoch sec; seqNo:11241 2021-02-13T00:17:36Z- AHRS_M2 msg: 0.72 phi; 0.33 theta; -0.14 psi 2021-02-13T00:17:36Z- handleDepth()-Depth_Keller msg: 1613175456772 epoch sec; seqNo:11336; depth 12.93 2021-02-13T00:17:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:36Z- handleDvl() - RDI_Pathfinder msg: 1613175456.77 epoch sec; seqNo:5639 2021-02-13T00:17:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.14 , -0.14 2021-02-13T00:17:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175456.79 epoch sec; seqNo:11336; 4058812.50 north; 592301.34 east 2021-02-13T00:17:37Z- AHRS_M2 msg: 1613175457.17 epoch sec; seqNo:11242 2021-02-13T00:17:37Z- AHRS_M2 msg: 0.72 phi; 0.33 theta; -0.14 psi 2021-02-13T00:17:37Z- handleDepth()-Depth_Keller msg: 1613175457178 epoch sec; seqNo:11337; depth 12.81 2021-02-13T00:17:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175457.19 epoch sec; seqNo:11337; 4058812.88 north; 592301.33 east 2021-02-13T00:17:37Z- AHRS_M2 msg: 1613175457.58 epoch sec; seqNo:11243 2021-02-13T00:17:37Z- AHRS_M2 msg: 0.72 phi; 0.32 theta; -0.14 psi 2021-02-13T00:17:37Z- handleDepth()-Depth_Keller msg: 1613175457580 epoch sec; seqNo:11338; depth 12.70 2021-02-13T00:17:37Z- handleDvl() - RDI_Pathfinder msg: 1613175457.60 epoch sec; seqNo:5640 2021-02-13T00:17:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.27 , 100.15 , 115.03 , 76.02 2021-02-13T00:17:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.24 , -0.01 2021-02-13T00:17:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175457.61 epoch sec; seqNo:11338; 4058812.90 north; 592302.56 east 2021-02-13T00:17:37Z- LcmTrn::handleMessages() - handling 43 messages took 0.68 ms and 130 calls... 2021-02-13T00:17:37Z- waiting for 1613175455.76, time is 1613175457.61 2021-02-13T00:17:37Z- LcmTrn::updateTrn() equating pose 1613175457.61 and meas times 1613175457.61 2021-02-13T00:17:37Z- LcmTrn::updateTrn() >>>> pose time 1613175457.61, meas time 1613175457.61 <<<< 2021-02-13T00:17:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175457.61 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:17:37Z- TerrainNav::motionUpdate() - currEstimate = (4058813, 592302.56, 12.70) 2021-02-13T00:17:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:17:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:17:37Z- TNavBF::Projecting Measurements 2021-02-13T00:17:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:17:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:17:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:17:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:17:37Z- TNavFilter::4 good beams. 2021-02-13T00:17:37Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:17:37Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:17:37Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:17:37Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:17:37Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:17:37Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:17:37Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:17:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:17:37Z- TerrainMapOctree: (vehN, vehE ) = (4058812.90, 592302.56). 2021-02-13T00:17:37Z- TerrainMapOctree: Min Distance = 3119.62. 2021-02-13T00:17:37Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:17:37Z- There is no specific good beam that all particles have in common. 2021-02-13T00:17:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:17:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.96 K: 0.128 ExpectedMeasDiff: nan currMeas.covariance: 11.63 2021-02-13T00:17:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:17:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.71 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 16.05 2021-02-13T00:17:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:17:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.56 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 21.17 2021-02-13T00:17:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:17:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.46 K: 0.117 ExpectedMeasDiff: nan currMeas.covariance: 9.25 2021-02-13T00:17:37Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:17:37Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:17:37Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:17:37Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:17:37Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:17:37Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:17:37Z- Filter 0 No beams used in subcloud update 2021-02-13T00:17:37Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:17:37Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:17:37Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 710.0 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:17:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175457.61 sec. 2021-02-13T00:17:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:17:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:17:37Z- variances([0], [2], and psi_berg) = 710.000329 833.003487 0.000000 2021-02-13T00:17:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:17:37Z- LcmTrn::publishEstimates() - MLE : 4058812.40 592352.06 12.70 2021-02-13T00:17:37Z- LcmTrn::publishEstimates() - MMSE : 4058812.90 592302.56 12.70 2021-02-13T00:17:37Z- LcmTrn::publishEstimates() - COVAR: 710.00 833.00 1.29 0.00 2021-02-13T00:17:37Z- AHRS_M2 msg: 1613175457.98 epoch sec; seqNo:11244 2021-02-13T00:17:37Z- AHRS_M2 msg: 0.72 phi; 0.32 theta; -0.14 psi 2021-02-13T00:17:37Z- handleDepth()-Depth_Keller msg: 1613175457984 epoch sec; seqNo:11339; depth 12.56 2021-02-13T00:17:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:37Z- handleDvl() - RDI_Pathfinder msg: 1613175457.99 epoch sec; seqNo:5641 2021-02-13T00:17:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.24 , -0.01 2021-02-13T00:17:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175458.00 epoch sec; seqNo:11339; 4058813.28 north; 592302.55 east 2021-02-13T00:17:38Z- AHRS_M2 msg: 1613175458.40 epoch sec; seqNo:11245 2021-02-13T00:17:38Z- AHRS_M2 msg: 0.71 phi; 0.32 theta; -0.15 psi 2021-02-13T00:17:38Z- handleDepth()-Depth_Keller msg: 1613175458402 epoch sec; seqNo:11340; depth 12.44 2021-02-13T00:17:38Z- handleDvl() - RDI_Pathfinder msg: 1613175458.41 epoch sec; seqNo:5642 2021-02-13T00:17:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 100.22 , 112.39 , 78.58 2021-02-13T00:17:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.21 , 0.18 2021-02-13T00:17:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175458.44 epoch sec; seqNo:11340; 4058813.65 north; 592302.55 east 2021-02-13T00:17:38Z- AHRS_M2 msg: 1613175458.79 epoch sec; seqNo:11246 2021-02-13T00:17:38Z- AHRS_M2 msg: 0.70 phi; 0.33 theta; -0.15 psi 2021-02-13T00:17:38Z- handleDepth()-Depth_Keller msg: 1613175458794 epoch sec; seqNo:11341; depth 12.31 2021-02-13T00:17:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175458.81 epoch sec; seqNo:11341; 4058814.03 north; 592302.54 east 2021-02-13T00:17:39Z- AHRS_M2 msg: 1613175459.19 epoch sec; seqNo:11247 2021-02-13T00:17:39Z- AHRS_M2 msg: 0.70 phi; 0.33 theta; -0.15 psi 2021-02-13T00:17:39Z- handleDepth()-Depth_Keller msg: 1613175459196 epoch sec; seqNo:11342; depth 12.18 2021-02-13T00:17:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:39Z- handleDvl() - RDI_Pathfinder msg: 1613175459.20 epoch sec; seqNo:5643 2021-02-13T00:17:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.21 , 0.18 2021-02-13T00:17:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175459.21 epoch sec; seqNo:11342; 4058814.41 north; 592302.54 east 2021-02-13T00:17:39Z- AHRS_M2 msg: 1613175459.60 epoch sec; seqNo:11248 2021-02-13T00:17:39Z- AHRS_M2 msg: 0.69 phi; 0.34 theta; -0.15 psi 2021-02-13T00:17:39Z- handleDepth()-Depth_Keller msg: 1613175459600 epoch sec; seqNo:11343; depth 12.05 2021-02-13T00:17:39Z- handleDvl() - RDI_Pathfinder msg: 1613175459.61 epoch sec; seqNo:5644 2021-02-13T00:17:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 100.22 , 116.44 , 78.58 2021-02-13T00:17:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.07 , 0.31 2021-02-13T00:17:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175459.63 epoch sec; seqNo:11343; 4058814.41 north; 592302.54 east 2021-02-13T00:17:40Z- AHRS_M2 msg: 1613175460.01 epoch sec; seqNo:11249 2021-02-13T00:17:40Z- AHRS_M2 msg: 0.69 phi; 0.34 theta; -0.14 psi 2021-02-13T00:17:40Z- handleDepth()-Depth_Keller msg: 1613175460012 epoch sec; seqNo:11344; depth 11.93 2021-02-13T00:17:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175460.02 epoch sec; seqNo:11344; 4058814.79 north; 592302.54 east 2021-02-13T00:17:40Z- AHRS_M2 msg: 1613175460.41 epoch sec; seqNo:11250 2021-02-13T00:17:40Z- AHRS_M2 msg: 0.69 phi; 0.35 theta; -0.14 psi 2021-02-13T00:17:40Z- handleDepth()-Depth_Keller msg: 1613175460411 epoch sec; seqNo:11345; depth 11.81 2021-02-13T00:17:40Z- handleDvl() - RDI_Pathfinder msg: 1613175460.42 epoch sec; seqNo:5645 2021-02-13T00:17:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 101.50 , 119.08 , 77.66 2021-02-13T00:17:40Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , 0.14 , 0.21 2021-02-13T00:17:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175460.43 epoch sec; seqNo:11345; 4058815.18 north; 592303.75 east 2021-02-13T00:17:40Z- AHRS_M2 msg: 1613175460.81 epoch sec; seqNo:11251 2021-02-13T00:17:40Z- AHRS_M2 msg: 0.69 phi; 0.35 theta; -0.15 psi 2021-02-13T00:17:40Z- handleDepth()-Depth_Keller msg: 1613175460822 epoch sec; seqNo:11346; depth 11.68 2021-02-13T00:17:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175460.83 epoch sec; seqNo:11346; 4058815.56 north; 592303.75 east 2021-02-13T00:17:41Z- AHRS_M2 msg: 1613175461.22 epoch sec; seqNo:11252 2021-02-13T00:17:41Z- AHRS_M2 msg: 0.70 phi; 0.35 theta; -0.14 psi 2021-02-13T00:17:41Z- handleDepth()-Depth_Keller msg: 1613175461222 epoch sec; seqNo:11347; depth 11.57 2021-02-13T00:17:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175461.23 epoch sec; seqNo:11347; 4058815.94 north; 592303.74 east 2021-02-13T00:17:41Z- AHRS_M2 msg: 1613175461.62 epoch sec; seqNo:11253 2021-02-13T00:17:41Z- AHRS_M2 msg: 0.71 phi; 0.34 theta; -0.14 psi 2021-02-13T00:17:41Z- handleDepth()-Depth_Keller msg: 1613175461620 epoch sec; seqNo:11348; depth 11.46 2021-02-13T00:17:41Z- handleDvl() - RDI_Pathfinder msg: 1613175461.63 epoch sec; seqNo:5646 2021-02-13T00:17:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.43 , 117.66 , 77.66 2021-02-13T00:17:41Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , 0.22 , -0.06 2021-02-13T00:17:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175461.64 epoch sec; seqNo:11348; 4058815.95 north; 592304.96 east 2021-02-13T00:17:42Z- AHRS_M2 msg: 1613175462.02 epoch sec; seqNo:11254 2021-02-13T00:17:42Z- AHRS_M2 msg: 0.72 phi; 0.33 theta; -0.14 psi 2021-02-13T00:17:42Z- handleDepth()-Depth_Keller msg: 1613175462026 epoch sec; seqNo:11349; depth 11.34 2021-02-13T00:17:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175462.04 epoch sec; seqNo:11349; 4058816.33 north; 592304.96 east 2021-02-13T00:17:42Z- AHRS_M2 msg: 1613175462.44 epoch sec; seqNo:11255 2021-02-13T00:17:42Z- AHRS_M2 msg: 0.74 phi; 0.32 theta; -0.14 psi 2021-02-13T00:17:42Z- handleDepth()-Depth_Keller msg: 1613175462459 epoch sec; seqNo:11350; depth 11.24 2021-02-13T00:17:42Z- handleDvl() - RDI_Pathfinder msg: 1613175462.47 epoch sec; seqNo:5647 2021-02-13T00:17:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 99.81 , 116.04 , 79.53 2021-02-13T00:17:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.14 , -0.28 2021-02-13T00:17:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175462.48 epoch sec; seqNo:11350; 4058816.33 north; 592304.96 east 2021-02-13T00:17:42Z- AHRS_M2 msg: 1613175462.83 epoch sec; seqNo:11256 2021-02-13T00:17:42Z- AHRS_M2 msg: 0.75 phi; 0.31 theta; -0.14 psi 2021-02-13T00:17:42Z- handleDepth()-Depth_Keller msg: 1613175462832 epoch sec; seqNo:11351; depth 11.11 2021-02-13T00:17:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:42Z- handleDvl() - RDI_Pathfinder msg: 1613175462.83 epoch sec; seqNo:5648 2021-02-13T00:17:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.14 , -0.28 2021-02-13T00:17:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175462.85 epoch sec; seqNo:11351; 4058816.71 north; 592304.96 east 2021-02-13T00:17:42Z- LcmTrn::handleMessages() - handling 47 messages took 0.69 ms and 133 calls... 2021-02-13T00:17:42Z- waiting for 1613175460.61, time is 1613175462.85 2021-02-13T00:17:42Z- LcmTrn::updateTrn() equating pose 1613175462.85 and meas times 1613175462.85 2021-02-13T00:17:42Z- LcmTrn::updateTrn() >>>> pose time 1613175462.85, meas time 1613175462.85 <<<< 2021-02-13T00:17:42Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175462.85, ping # 0. 2021-02-13T00:17:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175462.85 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:17:42Z- TerrainNav::motionUpdate() - currEstimate = (4058817, 592304.96, 11.11) 2021-02-13T00:17:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:17:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:17:42Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:17:42Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:17:42Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175462.85 sec.; No relevant navigation data available 2021-02-13T00:17:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:17:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:17:42Z- variances([0], [2], and psi_berg) = 709.936298 833.003449 0.000000 2021-02-13T00:17:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:17:42Z- LcmTrn::publishEstimates() - MLE : 4058816.21 592354.46 11.11 2021-02-13T00:17:42Z- LcmTrn::publishEstimates() - MMSE : 4058816.71 592304.96 11.11 2021-02-13T00:17:42Z- LcmTrn::publishEstimates() - COVAR: 709.94 833.00 1.52 0.00 2021-02-13T00:17:43Z- AHRS_M2 msg: 1613175463.23 epoch sec; seqNo:11257 2021-02-13T00:17:43Z- AHRS_M2 msg: 0.75 phi; 0.31 theta; -0.14 psi 2021-02-13T00:17:43Z- handleDepth()-Depth_Keller msg: 1613175463238 epoch sec; seqNo:11352; depth 11.02 2021-02-13T00:17:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175463.25 epoch sec; seqNo:11352; 4058816.71 north; 592304.96 east 2021-02-13T00:17:43Z- AHRS_M2 msg: 1613175463.63 epoch sec; seqNo:11258 2021-02-13T00:17:43Z- AHRS_M2 msg: 0.76 phi; 0.30 theta; -0.14 psi 2021-02-13T00:17:43Z- handleDepth()-Depth_Keller msg: 1613175463640 epoch sec; seqNo:11353; depth 10.90 2021-02-13T00:17:43Z- handleDvl() - RDI_Pathfinder msg: 1613175463.66 epoch sec; seqNo:5649 2021-02-13T00:17:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.43 , 116.31 , 79.80 2021-02-13T00:17:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.01 , -0.27 2021-02-13T00:17:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175463.67 epoch sec; seqNo:11353; 4058817.09 north; 592304.95 east 2021-02-13T00:17:44Z- AHRS_M2 msg: 1613175464.04 epoch sec; seqNo:11259 2021-02-13T00:17:44Z- AHRS_M2 msg: 0.76 phi; 0.30 theta; -0.14 psi 2021-02-13T00:17:44Z- handleDepth()-Depth_Keller msg: 1613175464049 epoch sec; seqNo:11354; depth 10.80 2021-02-13T00:17:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:44Z- handleDvl() - RDI_Pathfinder msg: 1613175464.05 epoch sec; seqNo:5650 2021-02-13T00:17:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.01 , -0.27 2021-02-13T00:17:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175464.07 epoch sec; seqNo:11354; 4058817.10 north; 592306.17 east 2021-02-13T00:17:44Z- AHRS_M2 msg: 1613175464.44 epoch sec; seqNo:11260 2021-02-13T00:17:44Z- AHRS_M2 msg: 0.75 phi; 0.30 theta; -0.14 psi 2021-02-13T00:17:44Z- handleDepth()-Depth_Keller msg: 1613175464448 epoch sec; seqNo:11355; depth 10.69 2021-02-13T00:17:44Z- handleDvl() - RDI_Pathfinder msg: 1613175464.46 epoch sec; seqNo:5651 2021-02-13T00:17:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.16 , 113.33 , 79.53 2021-02-13T00:17:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.06 , -0.09 2021-02-13T00:17:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175464.49 epoch sec; seqNo:11355; 4058817.48 north; 592306.17 east 2021-02-13T00:17:44Z- AHRS_M2 msg: 1613175464.85 epoch sec; seqNo:11261 2021-02-13T00:17:44Z- AHRS_M2 msg: 0.75 phi; 0.31 theta; -0.14 psi 2021-02-13T00:17:44Z- handleDepth()-Depth_Keller msg: 1613175464854 epoch sec; seqNo:11356; depth 10.58 2021-02-13T00:17:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175464.87 epoch sec; seqNo:11356; 4058817.86 north; 592306.16 east 2021-02-13T00:17:45Z- AHRS_M2 msg: 1613175465.25 epoch sec; seqNo:11262 2021-02-13T00:17:45Z- AHRS_M2 msg: 0.74 phi; 0.31 theta; -0.13 psi 2021-02-13T00:17:45Z- handleDepth()-Depth_Keller msg: 1613175465274 epoch sec; seqNo:11357; depth 10.46 2021-02-13T00:17:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:45Z- handleDvl() - RDI_Pathfinder msg: 1613175465.28 epoch sec; seqNo:5652 2021-02-13T00:17:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.06 , -0.09 2021-02-13T00:17:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175465.28 epoch sec; seqNo:11357; 4058817.86 north; 592306.16 east 2021-02-13T00:17:45Z- AHRS_M2 msg: 1613175465.66 epoch sec; seqNo:11263 2021-02-13T00:17:45Z- AHRS_M2 msg: 0.74 phi; 0.32 theta; -0.12 psi 2021-02-13T00:17:45Z- handleDepth()-Depth_Keller msg: 1613175465668 epoch sec; seqNo:11358; depth 10.35 2021-02-13T00:17:45Z- handleDvl() - RDI_Pathfinder msg: 1613175465.69 epoch sec; seqNo:5653 2021-02-13T00:17:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.36 , 102.58 , 113.40 , 76.43 2021-02-13T00:17:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.03 , 0.06 2021-02-13T00:17:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175465.70 epoch sec; seqNo:11358; 4058818.24 north; 592306.16 east 2021-02-13T00:17:46Z- AHRS_M2 msg: 1613175466.06 epoch sec; seqNo:11264 2021-02-13T00:17:46Z- AHRS_M2 msg: 0.74 phi; 0.32 theta; -0.13 psi 2021-02-13T00:17:46Z- handleDepth()-Depth_Keller msg: 1613175466067 epoch sec; seqNo:11359; depth 10.23 2021-02-13T00:17:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175466.08 epoch sec; seqNo:11359; 4058818.24 north; 592306.16 east 2021-02-13T00:17:46Z- AHRS_M2 msg: 1613175466.46 epoch sec; seqNo:11265 2021-02-13T00:17:46Z- AHRS_M2 msg: 0.74 phi; 0.32 theta; -0.14 psi 2021-02-13T00:17:46Z- handleDepth()-Depth_Keller msg: 1613175466469 epoch sec; seqNo:11360; depth 10.14 2021-02-13T00:17:46Z- handleDvl() - RDI_Pathfinder msg: 1613175466.48 epoch sec; seqNo:5654 2021-02-13T00:17:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.05 , 100.22 , 115.09 , 78.48 2021-02-13T00:17:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.05 , 0.08 2021-02-13T00:17:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175466.49 epoch sec; seqNo:11360; 4058818.62 north; 592306.16 east 2021-02-13T00:17:46Z- AHRS_M2 msg: 1613175466.87 epoch sec; seqNo:11266 2021-02-13T00:17:46Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.17 psi 2021-02-13T00:17:46Z- handleDepth()-Depth_Keller msg: 1613175466873 epoch sec; seqNo:11361; depth 10.01 2021-02-13T00:17:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175466.90 epoch sec; seqNo:11361; 4058818.63 north; 592307.38 east 2021-02-13T00:17:47Z- AHRS_M2 msg: 1613175467.28 epoch sec; seqNo:11267 2021-02-13T00:17:47Z- AHRS_M2 msg: 0.72 phi; 0.32 theta; -0.19 psi 2021-02-13T00:17:47Z- handleDepth()-Depth_Keller msg: 1613175467280 epoch sec; seqNo:11362; depth 9.91 2021-02-13T00:17:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175467.29 epoch sec; seqNo:11362; 4058819.01 north; 592307.37 east 2021-02-13T00:17:47Z- AHRS_M2 msg: 1613175467.68 epoch sec; seqNo:11268 2021-02-13T00:17:47Z- AHRS_M2 msg: 0.70 phi; 0.33 theta; -0.20 psi 2021-02-13T00:17:47Z- handleDepth()-Depth_Keller msg: 1613175467689 epoch sec; seqNo:11363; depth 9.77 2021-02-13T00:17:47Z- handleDvl() - RDI_Pathfinder msg: 1613175467.70 epoch sec; seqNo:5655 2021-02-13T00:17:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 101.50 , 115.03 , 79.86 2021-02-13T00:17:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.12 , 0.14 2021-02-13T00:17:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175467.71 epoch sec; seqNo:11363; 4058819.39 north; 592307.37 east 2021-02-13T00:17:47Z- LcmTrn::handleMessages() - handling 43 messages took 0.65 ms and 130 calls... 2021-02-13T00:17:47Z- waiting for 1613175465.85, time is 1613175467.71 2021-02-13T00:17:47Z- LcmTrn::updateTrn() equating pose 1613175467.71 and meas times 1613175467.71 2021-02-13T00:17:47Z- LcmTrn::updateTrn() >>>> pose time 1613175467.71, meas time 1613175467.71 <<<< 2021-02-13T00:17:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175467.71 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:17:47Z- TerrainNav::motionUpdate() - currEstimate = (4058819, 592307.37, 9.77) 2021-02-13T00:17:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:17:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:17:47Z- TNavBF::Projecting Measurements 2021-02-13T00:17:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:17:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:17:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:17:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:17:47Z- TNavFilter::4 good beams. 2021-02-13T00:17:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:17:47Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:17:47Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:17:47Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:17:47Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:17:47Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:17:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:17:47Z- TNavBF::mmse selected from filter -1 2021-02-13T00:17:47Z- TerrainMapOctree: (vehN, vehE ) = (4058819.39, 592307.37). 2021-02-13T00:17:47Z- TerrainMapOctree: Min Distance = 3111.54. 2021-02-13T00:17:47Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:17:47Z- There is no specific good beam that all particles have in common. 2021-02-13T00:17:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:17:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.70 K: 0.110 ExpectedMeasDiff: nan currMeas.covariance: 12.01 2021-02-13T00:17:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:17:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.51 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 16.48 2021-02-13T00:17:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:17:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 21.17 2021-02-13T00:17:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:17:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.32 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 10.21 2021-02-13T00:17:48Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:17:48Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:17:48Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:17:48Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:17:48Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:17:48Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:17:48Z- Filter 0 No beams used in subcloud update 2021-02-13T00:17:48Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:17:48Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:17:48Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 709.9 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:17:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175467.71 sec. 2021-02-13T00:17:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:17:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:17:48Z- variances([0], [2], and psi_berg) = 709.879075 833.003416 0.000000 2021-02-13T00:17:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:17:48Z- LcmTrn::publishEstimates() - MLE : 4058818.89 592356.87 9.77 2021-02-13T00:17:48Z- LcmTrn::publishEstimates() - MMSE : 4058819.39 592307.37 9.77 2021-02-13T00:17:48Z- LcmTrn::publishEstimates() - COVAR: 709.88 833.00 1.19 0.00 2021-02-13T00:17:48Z- AHRS_M2 msg: 1613175468.09 epoch sec; seqNo:11269 2021-02-13T00:17:48Z- AHRS_M2 msg: 0.69 phi; 0.34 theta; -0.20 psi 2021-02-13T00:17:48Z- handleDepth()-Depth_Keller msg: 1613175468089 epoch sec; seqNo:11364; depth 9.67 2021-02-13T00:17:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175468.10 epoch sec; seqNo:11364; 4058819.77 north; 592307.37 east 2021-02-13T00:17:48Z- AHRS_M2 msg: 1613175468.49 epoch sec; seqNo:11270 2021-02-13T00:17:48Z- AHRS_M2 msg: 0.68 phi; 0.35 theta; -0.21 psi 2021-02-13T00:17:48Z- handleDepth()-Depth_Keller msg: 1613175468492 epoch sec; seqNo:11365; depth 9.56 2021-02-13T00:17:48Z- handleDvl() - RDI_Pathfinder msg: 1613175468.51 epoch sec; seqNo:5656 2021-02-13T00:17:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 106.84 , 119.01 , 79.80 2021-02-13T00:17:48Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , -0.09 , 0.24 2021-02-13T00:17:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175468.52 epoch sec; seqNo:11365; 4058820.15 north; 592307.36 east 2021-02-13T00:17:48Z- AHRS_M2 msg: 1613175468.89 epoch sec; seqNo:11271 2021-02-13T00:17:48Z- AHRS_M2 msg: 0.67 phi; 0.36 theta; -0.20 psi 2021-02-13T00:17:48Z- handleDepth()-Depth_Keller msg: 1613175468896 epoch sec; seqNo:11366; depth 9.45 2021-02-13T00:17:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:48Z- handleDvl() - RDI_Pathfinder msg: 1613175468.90 epoch sec; seqNo:5657 2021-02-13T00:17:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:48Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , -0.09 , 0.24 2021-02-13T00:17:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175468.91 epoch sec; seqNo:11366; 4058820.54 north; 592308.58 east 2021-02-13T00:17:49Z- AHRS_M2 msg: 1613175469.30 epoch sec; seqNo:11272 2021-02-13T00:17:49Z- AHRS_M2 msg: 0.67 phi; 0.37 theta; -0.19 psi 2021-02-13T00:17:49Z- handleDepth()-Depth_Keller msg: 1613175469304 epoch sec; seqNo:11367; depth 9.36 2021-02-13T00:17:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175469.32 epoch sec; seqNo:11367; 4058820.54 north; 592308.58 east 2021-02-13T00:17:49Z- AHRS_M2 msg: 1613175469.70 epoch sec; seqNo:11273 2021-02-13T00:17:49Z- AHRS_M2 msg: 0.68 phi; 0.37 theta; -0.18 psi 2021-02-13T00:17:49Z- handleDepth()-Depth_Keller msg: 1613175469705 epoch sec; seqNo:11368; depth 9.26 2021-02-13T00:17:49Z- handleDvl() - RDI_Pathfinder msg: 1613175469.72 epoch sec; seqNo:5658 2021-02-13T00:17:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 108.40 , 121.79 , 80.14 2021-02-13T00:17:49Z- handleDvl() - RDI_Pathfinder msg: velocities 1.10 , 0.20 , 0.16 2021-02-13T00:17:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175469.73 epoch sec; seqNo:11368; 4058820.92 north; 592308.57 east 2021-02-13T00:17:50Z- AHRS_M2 msg: 1613175470.10 epoch sec; seqNo:11274 2021-02-13T00:17:50Z- AHRS_M2 msg: 0.68 phi; 0.37 theta; -0.17 psi 2021-02-13T00:17:50Z- handleDepth()-Depth_Keller msg: 1613175470119 epoch sec; seqNo:11369; depth 9.17 2021-02-13T00:17:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:50Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:50Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:50Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:50Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:50Z- handleDvl() - RDI_Pathfinder msg: 1613175470.12 epoch sec; seqNo:5659 2021-02-13T00:17:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:50Z- handleDvl() - RDI_Pathfinder msg: velocities 1.10 , 0.20 , 0.16 2021-02-13T00:17:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175470.14 epoch sec; seqNo:11369; 4058821.30 north; 592308.57 east 2021-02-13T00:17:50Z- AHRS_M2 msg: 1613175470.51 epoch sec; seqNo:11275 2021-02-13T00:17:50Z- AHRS_M2 msg: 0.70 phi; 0.36 theta; -0.15 psi 2021-02-13T00:17:50Z- handleDepth()-Depth_Keller msg: 1613175470513 epoch sec; seqNo:11370; depth 9.07 2021-02-13T00:17:50Z- handleDvl() - RDI_Pathfinder msg: 1613175470.53 epoch sec; seqNo:5660 2021-02-13T00:17:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 108.26 , 126.11 , 80.00 2021-02-13T00:17:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.41 , -0.16 2021-02-13T00:17:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175470.55 epoch sec; seqNo:11370; 4058821.31 north; 592309.79 east 2021-02-13T00:17:50Z- AHRS_M2 msg: 1613175470.91 epoch sec; seqNo:11276 2021-02-13T00:17:50Z- AHRS_M2 msg: 0.71 phi; 0.35 theta; -0.15 psi 2021-02-13T00:17:50Z- handleDepth()-Depth_Keller msg: 1613175470916 epoch sec; seqNo:11371; depth 8.97 2021-02-13T00:17:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175470.93 epoch sec; seqNo:11371; 4058821.69 north; 592309.79 east 2021-02-13T00:17:51Z- AHRS_M2 msg: 1613175471.32 epoch sec; seqNo:11277 2021-02-13T00:17:51Z- AHRS_M2 msg: 0.72 phi; 0.34 theta; -0.17 psi 2021-02-13T00:17:51Z- handleDepth()-Depth_Keller msg: 1613175471327 epoch sec; seqNo:11372; depth 8.85 2021-02-13T00:17:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:51Z- handleDvl() - RDI_Pathfinder msg: 1613175471.33 epoch sec; seqNo:5661 2021-02-13T00:17:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.41 , -0.16 2021-02-13T00:17:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175471.34 epoch sec; seqNo:11372; 4058821.69 north; 592309.79 east 2021-02-13T00:17:51Z- AHRS_M2 msg: 1613175471.72 epoch sec; seqNo:11278 2021-02-13T00:17:51Z- AHRS_M2 msg: 0.73 phi; 0.33 theta; -0.18 psi 2021-02-13T00:17:51Z- handleDepth()-Depth_Keller msg: 1613175471729 epoch sec; seqNo:11373; depth 8.74 2021-02-13T00:17:51Z- handleDvl() - RDI_Pathfinder msg: 1613175471.75 epoch sec; seqNo:5662 2021-02-13T00:17:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:51Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:17:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175471.76 epoch sec; seqNo:11373; 4058821.69 north; 592309.79 east 2021-02-13T00:17:52Z- AHRS_M2 msg: 1613175472.13 epoch sec; seqNo:11279 2021-02-13T00:17:52Z- AHRS_M2 msg: 0.73 phi; 0.32 theta; -0.20 psi 2021-02-13T00:17:52Z- handleDepth()-Depth_Keller msg: 1613175472132 epoch sec; seqNo:11374; depth 8.61 2021-02-13T00:17:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175472.15 epoch sec; seqNo:11374; 4058822.08 north; 592311.00 east 2021-02-13T00:17:52Z- AHRS_M2 msg: 1613175472.53 epoch sec; seqNo:11280 2021-02-13T00:17:52Z- AHRS_M2 msg: 0.72 phi; 0.31 theta; -0.20 psi 2021-02-13T00:17:52Z- handleDepth()-Depth_Keller msg: 1613175472540 epoch sec; seqNo:11375; depth 8.49 2021-02-13T00:17:52Z- handleDvl() - RDI_Pathfinder msg: 1613175472.55 epoch sec; seqNo:5663 2021-02-13T00:17:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.32 , 103.19 , 118.07 , 0.00 2021-02-13T00:17:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.54 , -0.14 , -0.30 2021-02-13T00:17:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175472.56 epoch sec; seqNo:11375; 4058822.08 north; 592311.00 east 2021-02-13T00:17:52Z- AHRS_M2 msg: 1613175472.93 epoch sec; seqNo:11281 2021-02-13T00:17:52Z- AHRS_M2 msg: 0.71 phi; 0.31 theta; -0.21 psi 2021-02-13T00:17:52Z- handleDepth()-Depth_Keller msg: 1613175472936 epoch sec; seqNo:11376; depth 8.37 2021-02-13T00:17:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175472.96 epoch sec; seqNo:11376; 4058822.46 north; 592311.00 east 2021-02-13T00:17:53Z- LcmTrn::handleMessages() - handling 47 messages took 0.76 ms and 132 calls... 2021-02-13T00:17:53Z- waiting for 1613175470.71, time is 1613175472.96 2021-02-13T00:17:53Z- LcmTrn::updateTrn() >>>> pose time 1613175472.96, meas time 1613175472.55 <<<< 2021-02-13T00:17:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175472.55 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:17:53Z- TerrainNav::motionUpdate() - currEstimate = (4058822, 592311.00, 8.37) 2021-02-13T00:17:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:17:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:17:53Z- TNavBF::Projecting Measurements 2021-02-13T00:17:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:17:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:17:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:17:53Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:17:53Z- TNavFilter::3 good beams. 2021-02-13T00:17:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:17:53Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:17:53Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:17:53Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:17:53Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:17:53Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:17:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:17:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:17:53Z- TerrainMapOctree: (vehN, vehE ) = (4058822.22, 592310.72). 2021-02-13T00:17:53Z- TerrainMapOctree: Min Distance = 3107.23. 2021-02-13T00:17:53Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:17:53Z- There is no specific good beam that all particles have in common. 2021-02-13T00:17:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:17:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.41 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 12.48 2021-02-13T00:17:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:17:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 17.04 2021-02-13T00:17:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:17:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 22.30 2021-02-13T00:17:53Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:17:53Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:17:53Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:17:53Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:17:53Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:17:53Z- Filter 0 No beams used in subcloud update 2021-02-13T00:17:53Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:17:53Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:17:53Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 709.8 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:17:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175472.55 sec. 2021-02-13T00:17:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:17:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:17:53Z- variances([0], [2], and psi_berg) = 709.797786 833.003369 0.000000 2021-02-13T00:17:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:17:53Z- LcmTrn::publishEstimates() - MLE : 4058821.96 592360.50 8.37 2021-02-13T00:17:53Z- LcmTrn::publishEstimates() - MMSE : 4058822.46 592311.00 8.37 2021-02-13T00:17:53Z- LcmTrn::publishEstimates() - COVAR: 709.80 833.00 1.16 0.00 2021-02-13T00:17:53Z- AHRS_M2 msg: 1613175473.34 epoch sec; seqNo:11282 2021-02-13T00:17:53Z- AHRS_M2 msg: 0.70 phi; 0.31 theta; -0.22 psi 2021-02-13T00:17:53Z- handleDepth()-Depth_Keller msg: 1613175473340 epoch sec; seqNo:11377; depth 8.26 2021-02-13T00:17:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175473.35 epoch sec; seqNo:11377; 4058822.46 north; 592311.00 east 2021-02-13T00:17:53Z- AHRS_M2 msg: 1613175473.74 epoch sec; seqNo:11283 2021-02-13T00:17:53Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.21 psi 2021-02-13T00:17:53Z- handleDepth()-Depth_Keller msg: 1613175473749 epoch sec; seqNo:11378; depth 8.14 2021-02-13T00:17:53Z- handleDvl() - RDI_Pathfinder msg: 1613175473.76 epoch sec; seqNo:5664 2021-02-13T00:17:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.44 , 109.75 , 118.68 , 82.97 2021-02-13T00:17:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.29 , 0.17 2021-02-13T00:17:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175473.77 epoch sec; seqNo:11378; 4058822.84 north; 592310.99 east 2021-02-13T00:17:54Z- AHRS_M2 msg: 1613175474.15 epoch sec; seqNo:11284 2021-02-13T00:17:54Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.20 psi 2021-02-13T00:17:54Z- handleDepth()-Depth_Keller msg: 1613175474154 epoch sec; seqNo:11379; depth 8.01 2021-02-13T00:17:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175474.17 epoch sec; seqNo:11379; 4058823.22 north; 592310.99 east 2021-02-13T00:17:54Z- AHRS_M2 msg: 1613175474.55 epoch sec; seqNo:11285 2021-02-13T00:17:54Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.18 psi 2021-02-13T00:17:54Z- handleDepth()-Depth_Keller msg: 1613175474562 epoch sec; seqNo:11380; depth 7.91 2021-02-13T00:17:54Z- handleDvl() - RDI_Pathfinder msg: 1613175474.57 epoch sec; seqNo:5665 2021-02-13T00:17:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 110.56 , 122.73 , 82.16 2021-02-13T00:17:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.06 , 0.38 2021-02-13T00:17:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175474.58 epoch sec; seqNo:11380; 4058823.60 north; 592310.99 east 2021-02-13T00:17:54Z- AHRS_M2 msg: 1613175474.96 epoch sec; seqNo:11286 2021-02-13T00:17:54Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.17 psi 2021-02-13T00:17:54Z- handleDepth()-Depth_Keller msg: 1613175474963 epoch sec; seqNo:11381; depth 7.79 2021-02-13T00:17:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:54Z- handleDvl() - RDI_Pathfinder msg: 1613175474.96 epoch sec; seqNo:5666 2021-02-13T00:17:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.06 , 0.38 2021-02-13T00:17:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175474.98 epoch sec; seqNo:11381; 4058823.97 north; 592310.98 east 2021-02-13T00:17:55Z- AHRS_M2 msg: 1613175475.36 epoch sec; seqNo:11287 2021-02-13T00:17:55Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.17 psi 2021-02-13T00:17:55Z- handleDepth()-Depth_Keller msg: 1613175475360 epoch sec; seqNo:11382; depth 7.68 2021-02-13T00:17:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175475.37 epoch sec; seqNo:11382; 4058823.99 north; 592312.20 east 2021-02-13T00:17:55Z- AHRS_M2 msg: 1613175475.76 epoch sec; seqNo:11288 2021-02-13T00:17:55Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.17 psi 2021-02-13T00:17:55Z- handleDepth()-Depth_Keller msg: 1613175475766 epoch sec; seqNo:11383; depth 7.57 2021-02-13T00:17:55Z- handleDvl() - RDI_Pathfinder msg: 1613175475.78 epoch sec; seqNo:5667 2021-02-13T00:17:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 107.79 , 121.31 , 82.10 2021-02-13T00:17:55Z- handleDvl() - RDI_Pathfinder msg: velocities 1.08 , 0.17 , 0.20 2021-02-13T00:17:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175475.80 epoch sec; seqNo:11383; 4058824.37 north; 592312.20 east 2021-02-13T00:17:56Z- AHRS_M2 msg: 1613175476.16 epoch sec; seqNo:11289 2021-02-13T00:17:56Z- AHRS_M2 msg: 0.65 phi; 0.38 theta; -0.18 psi 2021-02-13T00:17:56Z- handleDepth()-Depth_Keller msg: 1613175476178 epoch sec; seqNo:11384; depth 7.46 2021-02-13T00:17:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:56Z- handleDvl() - RDI_Pathfinder msg: 1613175476.18 epoch sec; seqNo:5668 2021-02-13T00:17:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:56Z- handleDvl() - RDI_Pathfinder msg: velocities 1.08 , 0.17 , 0.20 2021-02-13T00:17:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175476.19 epoch sec; seqNo:11384; 4058824.75 north; 592312.20 east 2021-02-13T00:17:56Z- AHRS_M2 msg: 1613175476.57 epoch sec; seqNo:11290 2021-02-13T00:17:56Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.18 psi 2021-02-13T00:17:56Z- handleDepth()-Depth_Keller msg: 1613175476572 epoch sec; seqNo:11385; depth 7.35 2021-02-13T00:17:56Z- handleDvl() - RDI_Pathfinder msg: 1613175476.58 epoch sec; seqNo:5669 2021-02-13T00:17:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 109.82 , 127.67 , 83.04 2021-02-13T00:17:56Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.21 , -0.12 2021-02-13T00:17:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175476.61 epoch sec; seqNo:11385; 4058825.12 north; 592312.19 east 2021-02-13T00:17:56Z- AHRS_M2 msg: 1613175476.97 epoch sec; seqNo:11291 2021-02-13T00:17:56Z- AHRS_M2 msg: 0.67 phi; 0.37 theta; -0.18 psi 2021-02-13T00:17:56Z- handleDepth()-Depth_Keller msg: 1613175476976 epoch sec; seqNo:11386; depth 7.23 2021-02-13T00:17:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175476.99 epoch sec; seqNo:11386; 4058825.14 north; 592313.41 east 2021-02-13T00:17:57Z- AHRS_M2 msg: 1613175477.38 epoch sec; seqNo:11292 2021-02-13T00:17:57Z- AHRS_M2 msg: 0.68 phi; 0.36 theta; -0.17 psi 2021-02-13T00:17:57Z- handleDepth()-Depth_Keller msg: 1613175477390 epoch sec; seqNo:11387; depth 7.13 2021-02-13T00:17:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:17:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:17:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:17:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:17:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:17:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:17:57Z- handleDvl() - RDI_Pathfinder msg: 1613175477.39 epoch sec; seqNo:5670 2021-02-13T00:17:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:17:57Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.21 , -0.12 2021-02-13T00:17:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175477.42 epoch sec; seqNo:11387; 4058825.52 north; 592313.41 east 2021-02-13T00:17:57Z- AHRS_M2 msg: 1613175477.78 epoch sec; seqNo:11293 2021-02-13T00:17:57Z- AHRS_M2 msg: 0.68 phi; 0.35 theta; -0.16 psi 2021-02-13T00:17:57Z- handleDepth()-Depth_Keller msg: 1613175477794 epoch sec; seqNo:11388; depth 7.00 2021-02-13T00:17:57Z- handleDvl() - RDI_Pathfinder msg: 1613175477.81 epoch sec; seqNo:5671 2021-02-13T00:17:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 111.10 , 127.47 , 166.14 2021-02-13T00:17:57Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , 0.13 , -0.17 2021-02-13T00:17:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175477.82 epoch sec; seqNo:11388; 4058825.90 north; 592313.40 east 2021-02-13T00:17:58Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-13T00:17:58Z- waiting for 1613175475.96, time is 1613175477.82 2021-02-13T00:17:58Z- LcmTrn::updateTrn() equating pose 1613175477.82 and meas times 1613175477.82 2021-02-13T00:17:58Z- LcmTrn::updateTrn() >>>> pose time 1613175477.82, meas time 1613175477.82 <<<< 2021-02-13T00:17:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175477.82 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:17:58Z- TerrainNav::motionUpdate() - currEstimate = (4058826, 592313.40, 7.00) 2021-02-13T00:17:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:17:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:17:58Z- TNavBF::Projecting Measurements 2021-02-13T00:17:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:17:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:17:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:17:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:17:58Z- TNavFilter::4 good beams. 2021-02-13T00:17:58Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:17:58Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:17:58Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:17:58Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:17:58Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:17:58Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:17:58Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:17:58Z- TNavBF::mmse selected from filter -1 2021-02-13T00:17:58Z- TerrainMapOctree: (vehN, vehE ) = (4058825.90, 592313.40). 2021-02-13T00:17:58Z- TerrainMapOctree: Min Distance = 3102.69. 2021-02-13T00:17:58Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:17:58Z- There is no specific good beam that all particles have in common. 2021-02-13T00:17:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:17:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:17:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:17:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 19.75 2021-02-13T00:17:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:17:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 26.00 2021-02-13T00:17:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:17:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.024 ExpectedMeasDiff: nan currMeas.covariance: 44.17 2021-02-13T00:17:58Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:17:58Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:17:58Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:17:58Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:17:58Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:17:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:17:58Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:17:58Z- Filter 0 No beams used in subcloud update 2021-02-13T00:17:58Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:17:58Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:17:58Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 709.7 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:17:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175477.82 sec. 2021-02-13T00:17:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:17:58Z- TNavBF::mmse selected from filter -1 2021-02-13T00:17:58Z- variances([0], [2], and psi_berg) = 709.736159 833.003333 0.000000 2021-02-13T00:17:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:17:58Z- LcmTrn::publishEstimates() - MLE : 4058825.40 592362.90 7.00 2021-02-13T00:17:58Z- LcmTrn::publishEstimates() - MMSE : 4058825.90 592313.40 7.00 2021-02-13T00:17:58Z- LcmTrn::publishEstimates() - COVAR: 709.74 833.00 1.12 0.00 2021-02-13T00:17:58Z- AHRS_M2 msg: 1613175478.18 epoch sec; seqNo:11294 2021-02-13T00:17:58Z- AHRS_M2 msg: 0.68 phi; 0.35 theta; -0.16 psi 2021-02-13T00:17:58Z- handleDepth()-Depth_Keller msg: 1613175478193 epoch sec; seqNo:11389; depth 6.90 2021-02-13T00:17:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175478.20 epoch sec; seqNo:11389; 4058825.90 north; 592313.40 east 2021-02-13T00:17:58Z- AHRS_M2 msg: 1613175478.59 epoch sec; seqNo:11295 2021-02-13T00:17:58Z- AHRS_M2 msg: 0.69 phi; 0.34 theta; -0.14 psi 2021-02-13T00:17:58Z- handleDepth()-Depth_Keller msg: 1613175478593 epoch sec; seqNo:11390; depth 6.79 2021-02-13T00:17:58Z- handleDvl() - RDI_Pathfinder msg: 1613175478.61 epoch sec; seqNo:5672 2021-02-13T00:17:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 107.65 , 122.53 , 82.37 2021-02-13T00:17:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.13 , -0.13 2021-02-13T00:17:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175478.62 epoch sec; seqNo:11390; 4058826.27 north; 592313.40 east 2021-02-13T00:17:58Z- AHRS_M2 msg: 1613175478.99 epoch sec; seqNo:11296 2021-02-13T00:17:58Z- AHRS_M2 msg: 0.70 phi; 0.34 theta; -0.13 psi 2021-02-13T00:17:59Z- handleDepth()-Depth_Keller msg: 1613175479005 epoch sec; seqNo:11391; depth 6.69 2021-02-13T00:17:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175479.02 epoch sec; seqNo:11391; 4058826.29 north; 592314.62 east 2021-02-13T00:17:59Z- AHRS_M2 msg: 1613175479.40 epoch sec; seqNo:11297 2021-02-13T00:17:59Z- AHRS_M2 msg: 0.70 phi; 0.33 theta; -0.12 psi 2021-02-13T00:17:59Z- handleDepth()-Depth_Keller msg: 1613175479400 epoch sec; seqNo:11392; depth 6.58 2021-02-13T00:17:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175479.42 epoch sec; seqNo:11392; 4058826.67 north; 592314.62 east 2021-02-13T00:17:59Z- AHRS_M2 msg: 1613175479.80 epoch sec; seqNo:11298 2021-02-13T00:17:59Z- AHRS_M2 msg: 0.71 phi; 0.33 theta; -0.13 psi 2021-02-13T00:17:59Z- handleDepth()-Depth_Keller msg: 1613175479809 epoch sec; seqNo:11393; depth 6.47 2021-02-13T00:17:59Z- handleDvl() - RDI_Pathfinder msg: 1613175479.82 epoch sec; seqNo:5673 2021-02-13T00:17:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 106.84 , 124.69 , 83.04 2021-02-13T00:17:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , 0.12 , -0.13 2021-02-13T00:17:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175479.84 epoch sec; seqNo:11393; 4058826.67 north; 592314.62 east 2021-02-13T00:18:00Z- AHRS_M2 msg: 1613175480.20 epoch sec; seqNo:11299 2021-02-13T00:18:00Z- AHRS_M2 msg: 0.71 phi; 0.33 theta; -0.13 psi 2021-02-13T00:18:00Z- handleDepth()-Depth_Keller msg: 1613175480213 epoch sec; seqNo:11394; depth 6.33 2021-02-13T00:18:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175480.23 epoch sec; seqNo:11394; 4058826.67 north; 592314.62 east 2021-02-13T00:18:00Z- AHRS_M2 msg: 1613175480.61 epoch sec; seqNo:11300 2021-02-13T00:18:00Z- AHRS_M2 msg: 0.71 phi; 0.33 theta; -0.15 psi 2021-02-13T00:18:00Z- handleDepth()-Depth_Keller msg: 1613175480613 epoch sec; seqNo:11395; depth 6.23 2021-02-13T00:18:00Z- handleDvl() - RDI_Pathfinder msg: 1613175480.62 epoch sec; seqNo:5674 2021-02-13T00:18:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 106.78 , 120.16 , 82.97 2021-02-13T00:18:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.07 , -0.18 2021-02-13T00:18:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175480.63 epoch sec; seqNo:11395; 4058827.05 north; 592314.61 east 2021-02-13T00:18:01Z- AHRS_M2 msg: 1613175481.01 epoch sec; seqNo:11301 2021-02-13T00:18:01Z- AHRS_M2 msg: 0.70 phi; 0.34 theta; -0.17 psi 2021-02-13T00:18:01Z- handleDepth()-Depth_Keller msg: 1613175481026 epoch sec; seqNo:11396; depth 6.12 2021-02-13T00:18:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:01Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:18:01Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:18:01Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:18:01Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:18:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:18:01Z- handleDvl() - RDI_Pathfinder msg: 1613175481.03 epoch sec; seqNo:5675 2021-02-13T00:18:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:18:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.07 , -0.18 2021-02-13T00:18:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175481.04 epoch sec; seqNo:11396; 4058827.42 north; 592314.61 east 2021-02-13T00:18:01Z- AHRS_M2 msg: 1613175481.42 epoch sec; seqNo:11302 2021-02-13T00:18:01Z- AHRS_M2 msg: 0.70 phi; 0.35 theta; -0.18 psi 2021-02-13T00:18:01Z- handleDepth()-Depth_Keller msg: 1613175481434 epoch sec; seqNo:11397; depth 5.98 2021-02-13T00:18:01Z- handleDvl() - RDI_Pathfinder msg: 1613175481.47 epoch sec; seqNo:5676 2021-02-13T00:18:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 109.82 , 123.21 , 83.04 2021-02-13T00:18:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.29 , 0.03 2021-02-13T00:18:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175481.49 epoch sec; seqNo:11397; 4058827.80 north; 592314.60 east 2021-02-13T00:18:01Z- AHRS_M2 msg: 1613175481.82 epoch sec; seqNo:11303 2021-02-13T00:18:01Z- AHRS_M2 msg: 0.70 phi; 0.36 theta; -0.18 psi 2021-02-13T00:18:01Z- handleDepth()-Depth_Keller msg: 1613175481824 epoch sec; seqNo:11398; depth 5.86 2021-02-13T00:18:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175481.84 epoch sec; seqNo:11398; 4058827.80 north; 592314.60 east 2021-02-13T00:18:02Z- AHRS_M2 msg: 1613175482.22 epoch sec; seqNo:11304 2021-02-13T00:18:02Z- AHRS_M2 msg: 0.69 phi; 0.37 theta; -0.17 psi 2021-02-13T00:18:02Z- handleDepth()-Depth_Keller msg: 1613175482238 epoch sec; seqNo:11399; depth 5.74 2021-02-13T00:18:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:18:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:18:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:18:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:18:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:18:02Z- handleDvl() - RDI_Pathfinder msg: 1613175482.24 epoch sec; seqNo:5677 2021-02-13T00:18:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:18:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.29 , 0.03 2021-02-13T00:18:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175482.25 epoch sec; seqNo:11399; 4058828.19 north; 592315.82 east 2021-02-13T00:18:02Z- AHRS_M2 msg: 1613175482.63 epoch sec; seqNo:11305 2021-02-13T00:18:02Z- AHRS_M2 msg: 0.68 phi; 0.37 theta; -0.15 psi 2021-02-13T00:18:02Z- handleDepth()-Depth_Keller msg: 1613175482650 epoch sec; seqNo:11400; depth 5.65 2021-02-13T00:18:02Z- handleDvl() - RDI_Pathfinder msg: 1613175482.66 epoch sec; seqNo:5678 2021-02-13T00:18:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 111.17 , 129.02 , 82.91 2021-02-13T00:18:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.14 , 0.31 2021-02-13T00:18:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175482.67 epoch sec; seqNo:11400; 4058828.57 north; 592315.82 east 2021-02-13T00:18:03Z- AHRS_M2 msg: 1613175483.04 epoch sec; seqNo:11306 2021-02-13T00:18:03Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.15 psi 2021-02-13T00:18:03Z- handleDepth()-Depth_Keller msg: 1613175483041 epoch sec; seqNo:11401; depth 5.51 2021-02-13T00:18:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175483.05 epoch sec; seqNo:11401; 4058828.95 north; 592315.81 east 2021-02-13T00:18:03Z- LcmTrn::handleMessages() - handling 46 messages took 0.70 ms and 132 calls... 2021-02-13T00:18:03Z- waiting for 1613175480.82, time is 1613175483.05 2021-02-13T00:18:03Z- LcmTrn::updateTrn() >>>> pose time 1613175483.05, meas time 1613175482.66 <<<< 2021-02-13T00:18:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175482.66 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:18:03Z- TerrainNav::motionUpdate() - currEstimate = (4058829, 592315.81, 5.51) 2021-02-13T00:18:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:18:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:18:03Z- TNavBF::Projecting Measurements 2021-02-13T00:18:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:18:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:18:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:18:03Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:18:03Z- TNavFilter::3 good beams. 2021-02-13T00:18:03Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:18:03Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:18:03Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:18:03Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:18:03Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:18:03Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:18:03Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:18:03Z- TNavBF::mmse selected from filter -1 2021-02-13T00:18:03Z- TerrainMapOctree: (vehN, vehE ) = (4058828.72, 592315.63). 2021-02-13T00:18:03Z- TerrainMapOctree: Min Distance = 3099.09. 2021-02-13T00:18:03Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:18:03Z- There is no specific good beam that all particles have in common. 2021-02-13T00:18:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:18:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-13T00:18:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:18:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 19.77 2021-02-13T00:18:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:18:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 26.63 2021-02-13T00:18:03Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:18:03Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:18:03Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:18:03Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:18:03Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:18:03Z- Filter 0 No beams used in subcloud update 2021-02-13T00:18:03Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:18:03Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:18:03Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 709.7 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:18:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175482.66 sec. 2021-02-13T00:18:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:18:03Z- TNavBF::mmse selected from filter -1 2021-02-13T00:18:03Z- variances([0], [2], and psi_berg) = 709.662901 833.003290 0.000000 2021-02-13T00:18:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:18:03Z- LcmTrn::publishEstimates() - MLE : 4058828.45 592365.31 5.51 2021-02-13T00:18:03Z- LcmTrn::publishEstimates() - MMSE : 4058828.95 592315.81 5.51 2021-02-13T00:18:03Z- LcmTrn::publishEstimates() - COVAR: 709.66 833.00 1.12 0.00 2021-02-13T00:18:03Z- AHRS_M2 msg: 1613175483.44 epoch sec; seqNo:11307 2021-02-13T00:18:03Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.17 psi 2021-02-13T00:18:03Z- handleDepth()-Depth_Keller msg: 1613175483441 epoch sec; seqNo:11402; depth 5.38 2021-02-13T00:18:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:18:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:18:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:18:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:18:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:18:03Z- handleDvl() - RDI_Pathfinder msg: 1613175483.44 epoch sec; seqNo:5679 2021-02-13T00:18:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:18:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.14 , 0.31 2021-02-13T00:18:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175483.45 epoch sec; seqNo:11402; 4058829.33 north; 592315.81 east 2021-02-13T00:18:03Z- AHRS_M2 msg: 1613175483.84 epoch sec; seqNo:11308 2021-02-13T00:18:03Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.19 psi 2021-02-13T00:18:03Z- handleDepth()-Depth_Keller msg: 1613175483846 epoch sec; seqNo:11403; depth 5.24 2021-02-13T00:18:03Z- handleDvl() - RDI_Pathfinder msg: 1613175483.86 epoch sec; seqNo:5680 2021-02-13T00:18:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.65 , 109.55 , 127.40 , 84.26 2021-02-13T00:18:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.09 , 0.31 2021-02-13T00:18:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175483.87 epoch sec; seqNo:11403; 4058829.33 north; 592315.81 east 2021-02-13T00:18:04Z- AHRS_M2 msg: 1613175484.24 epoch sec; seqNo:11309 2021-02-13T00:18:04Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.22 psi 2021-02-13T00:18:04Z- handleDepth()-Depth_Keller msg: 1613175484248 epoch sec; seqNo:11404; depth 5.08 2021-02-13T00:18:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175484.27 epoch sec; seqNo:11404; 4058829.72 north; 592317.02 east 2021-02-13T00:18:04Z- AHRS_M2 msg: 1613175484.65 epoch sec; seqNo:11310 2021-02-13T00:18:04Z- AHRS_M2 msg: 0.62 phi; 0.27 theta; -0.23 psi 2021-02-13T00:18:04Z- handleDepth()-Depth_Keller msg: 1613175484652 epoch sec; seqNo:11405; depth 4.95 2021-02-13T00:18:04Z- handleDvl() - RDI_Pathfinder msg: 1613175484.66 epoch sec; seqNo:5681 2021-02-13T00:18:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 108.13 , 124.49 , 0.00 2021-02-13T00:18:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.23 , 0.24 2021-02-13T00:18:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175484.69 epoch sec; seqNo:11405; 4058830.10 north; 592317.02 east 2021-02-13T00:18:05Z- AHRS_M2 msg: 1613175485.05 epoch sec; seqNo:11311 2021-02-13T00:18:05Z- AHRS_M2 msg: 0.63 phi; 0.24 theta; -0.23 psi 2021-02-13T00:18:05Z- handleDepth()-Depth_Keller msg: 1613175485056 epoch sec; seqNo:11406; depth 4.80 2021-02-13T00:18:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175485.07 epoch sec; seqNo:11406; 4058830.48 north; 592317.02 east 2021-02-13T00:18:05Z- AHRS_M2 msg: 1613175485.46 epoch sec; seqNo:11312 2021-02-13T00:18:05Z- AHRS_M2 msg: 0.64 phi; 0.20 theta; -0.22 psi 2021-02-13T00:18:05Z- handleDepth()-Depth_Keller msg: 1613175485463 epoch sec; seqNo:11407; depth 4.68 2021-02-13T00:18:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175485.47 epoch sec; seqNo:11407; 4058830.48 north; 592317.02 east 2021-02-13T00:18:05Z- AHRS_M2 msg: 1613175485.86 epoch sec; seqNo:11313 2021-02-13T00:18:05Z- AHRS_M2 msg: 0.66 phi; 0.16 theta; -0.21 psi 2021-02-13T00:18:05Z- handleDepth()-Depth_Keller msg: 1613175485864 epoch sec; seqNo:11408; depth 4.59 2021-02-13T00:18:05Z- handleDvl() - RDI_Pathfinder msg: 1613175485.88 epoch sec; seqNo:5682 2021-02-13T00:18:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 115.09 , 116.58 , 88.32 2021-02-13T00:18:05Z- handleDvl() - RDI_Pathfinder msg: velocities 1.11 , 0.42 , -0.01 2021-02-13T00:18:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175485.89 epoch sec; seqNo:11408; 4058830.87 north; 592318.23 east 2021-02-13T00:18:06Z- AHRS_M2 msg: 1613175486.26 epoch sec; seqNo:11314 2021-02-13T00:18:06Z- AHRS_M2 msg: 0.69 phi; 0.12 theta; -0.20 psi 2021-02-13T00:18:06Z- handleDepth()-Depth_Keller msg: 1613175486271 epoch sec; seqNo:11409; depth 4.50 2021-02-13T00:18:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175486.28 epoch sec; seqNo:11409; 4058830.87 north; 592318.23 east 2021-02-13T00:18:06Z- AHRS_M2 msg: 1613175486.67 epoch sec; seqNo:11315 2021-02-13T00:18:06Z- AHRS_M2 msg: 0.72 phi; 0.07 theta; -0.20 psi 2021-02-13T00:18:06Z- handleDepth()-Depth_Keller msg: 1613175486672 epoch sec; seqNo:11410; depth 4.41 2021-02-13T00:18:06Z- handleDvl() - RDI_Pathfinder msg: 1613175486.68 epoch sec; seqNo:5683 2021-02-13T00:18:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 108.13 , 108.13 , 97.71 2021-02-13T00:18:06Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.34 , -0.49 2021-02-13T00:18:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175486.69 epoch sec; seqNo:11410; 4058831.25 north; 592318.23 east 2021-02-13T00:18:07Z- AHRS_M2 msg: 1613175487.08 epoch sec; seqNo:11316 2021-02-13T00:18:07Z- AHRS_M2 msg: 0.75 phi; 0.02 theta; -0.19 psi 2021-02-13T00:18:07Z- handleDepth()-Depth_Keller msg: 1613175487086 epoch sec; seqNo:11411; depth 4.35 2021-02-13T00:18:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:07Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:18:07Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:18:07Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:18:07Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:18:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:18:07Z- handleDvl() - RDI_Pathfinder msg: 1613175487.09 epoch sec; seqNo:5684 2021-02-13T00:18:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:18:07Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.34 , -0.49 2021-02-13T00:18:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175487.10 epoch sec; seqNo:11411; 4058831.64 north; 592319.45 east 2021-02-13T00:18:07Z- AHRS_M2 msg: 1613175487.48 epoch sec; seqNo:11317 2021-02-13T00:18:07Z- AHRS_M2 msg: 0.78 phi; -0.03 theta; -0.18 psi 2021-02-13T00:18:07Z- handleDepth()-Depth_Keller msg: 1613175487497 epoch sec; seqNo:11412; depth 4.30 2021-02-13T00:18:07Z- handleDvl() - RDI_Pathfinder msg: 1613175487.51 epoch sec; seqNo:5685 2021-02-13T00:18:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 106.78 , 97.85 , 97.85 2021-02-13T00:18:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.12 , -0.63 2021-02-13T00:18:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175487.52 epoch sec; seqNo:11412; 4058832.02 north; 592319.44 east 2021-02-13T00:18:07Z- AHRS_M2 msg: 1613175487.88 epoch sec; seqNo:11318 2021-02-13T00:18:07Z- AHRS_M2 msg: 0.80 phi; -0.08 theta; -0.17 psi 2021-02-13T00:18:07Z- handleDepth()-Depth_Keller msg: 1613175487889 epoch sec; seqNo:11413; depth 4.24 2021-02-13T00:18:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175487.90 epoch sec; seqNo:11413; 4058832.02 north; 592319.44 east 2021-02-13T00:18:08Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-13T00:18:08Z- waiting for 1613175486.05, time is 1613175487.90 2021-02-13T00:18:08Z- LcmTrn::updateTrn() >>>> pose time 1613175487.90, meas time 1613175487.51 <<<< 2021-02-13T00:18:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175487.51 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:18:08Z- TerrainNav::motionUpdate() - currEstimate = (4058832, 592319.44, 4.24) 2021-02-13T00:18:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:18:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:18:08Z- TNavBF::Projecting Measurements 2021-02-13T00:18:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:18:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:18:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:18:08Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:18:08Z- TNavFilter::3 good beams. 2021-02-13T00:18:08Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:18:08Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:18:08Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:18:08Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:18:08Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:18:08Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:18:08Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:18:08Z- TNavBF::mmse selected from filter -1 2021-02-13T00:18:08Z- TerrainMapOctree: (vehN, vehE ) = (4058831.77, 592319.15). 2021-02-13T00:18:08Z- TerrainMapOctree: Min Distance = 3094.51. 2021-02-13T00:18:08Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:18:08Z- There is no specific good beam that all particles have in common. 2021-02-13T00:18:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:18:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 18.24 2021-02-13T00:18:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:18:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-13T00:18:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:18:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-13T00:18:08Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:18:08Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:18:08Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:18:08Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:18:08Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:18:08Z- Filter 0 No beams used in subcloud update 2021-02-13T00:18:08Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:18:08Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:18:08Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 709.6 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:18:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175487.51 sec. 2021-02-13T00:18:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:18:08Z- TNavBF::mmse selected from filter -1 2021-02-13T00:18:08Z- variances([0], [2], and psi_berg) = 709.581472 833.003243 0.000000 2021-02-13T00:18:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:18:08Z- LcmTrn::publishEstimates() - MLE : 4058831.52 592368.94 4.24 2021-02-13T00:18:08Z- LcmTrn::publishEstimates() - MMSE : 4058832.02 592319.44 4.24 2021-02-13T00:18:08Z- LcmTrn::publishEstimates() - COVAR: 709.58 833.00 1.12 0.00 2021-02-13T00:18:08Z- AHRS_M2 msg: 1613175488.28 epoch sec; seqNo:11319 2021-02-13T00:18:08Z- AHRS_M2 msg: 0.81 phi; -0.13 theta; -0.17 psi 2021-02-13T00:18:08Z- handleDepth()-Depth_Keller msg: 1613175488291 epoch sec; seqNo:11414; depth 4.21 2021-02-13T00:18:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:18:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:18:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:18:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:18:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:18:08Z- handleDvl() - RDI_Pathfinder msg: 1613175488.29 epoch sec; seqNo:5686 2021-02-13T00:18:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:18:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.12 , -0.63 2021-02-13T00:18:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175488.30 epoch sec; seqNo:11414; 4058832.40 north; 592319.44 east 2021-02-13T00:18:08Z- AHRS_M2 msg: 1613175488.69 epoch sec; seqNo:11320 2021-02-13T00:18:08Z- AHRS_M2 msg: 0.81 phi; -0.17 theta; -0.16 psi 2021-02-13T00:18:08Z- handleDepth()-Depth_Keller msg: 1613175488693 epoch sec; seqNo:11415; depth 4.17 2021-02-13T00:18:08Z- handleDvl() - RDI_Pathfinder msg: 1613175488.70 epoch sec; seqNo:5687 2021-02-13T00:18:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.09 , 96.97 , 101.03 2021-02-13T00:18:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.57 , -0.44 , -0.18 2021-02-13T00:18:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175488.72 epoch sec; seqNo:11415; 4058832.78 north; 592319.43 east 2021-02-13T00:18:09Z- AHRS_M2 msg: 1613175489.09 epoch sec; seqNo:11321 2021-02-13T00:18:09Z- AHRS_M2 msg: 0.81 phi; -0.20 theta; -0.15 psi 2021-02-13T00:18:09Z- handleDepth()-Depth_Keller msg: 1613175489096 epoch sec; seqNo:11416; depth 4.15 2021-02-13T00:18:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175489.11 epoch sec; seqNo:11416; 4058833.16 north; 592319.43 east 2021-02-13T00:18:09Z- AHRS_M2 msg: 1613175489.50 epoch sec; seqNo:11322 2021-02-13T00:18:09Z- AHRS_M2 msg: 0.80 phi; -0.23 theta; -0.14 psi 2021-02-13T00:18:09Z- handleDepth()-Depth_Keller msg: 1613175489500 epoch sec; seqNo:11417; depth 4.15 2021-02-13T00:18:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:09Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:18:09Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:18:09Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:18:09Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:18:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:18:09Z- handleDvl() - RDI_Pathfinder msg: 1613175489.50 epoch sec; seqNo:5688 2021-02-13T00:18:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:18:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.57 , -0.44 , -0.18 2021-02-13T00:18:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175489.51 epoch sec; seqNo:11417; 4058833.16 north; 592319.43 east 2021-02-13T00:18:09Z- AHRS_M2 msg: 1613175489.90 epoch sec; seqNo:11323 2021-02-13T00:18:09Z- AHRS_M2 msg: 0.78 phi; -0.25 theta; -0.14 psi 2021-02-13T00:18:09Z- handleDepth()-Depth_Keller msg: 1613175489905 epoch sec; seqNo:11418; depth 4.15 2021-02-13T00:18:09Z- handleDvl() - RDI_Pathfinder msg: 1613175489.92 epoch sec; seqNo:5689 2021-02-13T00:18:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.21 , 105.09 , 86.15 , 109.14 2021-02-13T00:18:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.59 , -0.30 , 0.42 2021-02-13T00:18:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175489.93 epoch sec; seqNo:11418; 4058833.54 north; 592319.43 east 2021-02-13T00:18:10Z- AHRS_M2 msg: 1613175490.30 epoch sec; seqNo:11324 2021-02-13T00:18:10Z- AHRS_M2 msg: 0.77 phi; -0.26 theta; -0.14 psi 2021-02-13T00:18:10Z- handleDepth()-Depth_Keller msg: 1613175490313 epoch sec; seqNo:11419; depth 4.17 2021-02-13T00:18:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175490.33 epoch sec; seqNo:11419; 4058833.54 north; 592319.43 east 2021-02-13T00:18:10Z- AHRS_M2 msg: 1613175490.71 epoch sec; seqNo:11325 2021-02-13T00:18:10Z- AHRS_M2 msg: 0.76 phi; -0.27 theta; -0.16 psi 2021-02-13T00:18:10Z- handleDepth()-Depth_Keller msg: 1613175490712 epoch sec; seqNo:11420; depth 4.21 2021-02-13T00:18:10Z- handleDvl() - RDI_Pathfinder msg: 1613175490.72 epoch sec; seqNo:5690 2021-02-13T00:18:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 104.00 , 0.00 , 116.18 2021-02-13T00:18:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.17 , 0.56 2021-02-13T00:18:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175490.75 epoch sec; seqNo:11420; 4058833.92 north; 592319.42 east 2021-02-13T00:18:11Z- AHRS_M2 msg: 1613175491.11 epoch sec; seqNo:11326 2021-02-13T00:18:11Z- AHRS_M2 msg: 0.74 phi; -0.28 theta; -0.18 psi 2021-02-13T00:18:11Z- handleDepth()-Depth_Keller msg: 1613175491121 epoch sec; seqNo:11421; depth 4.28 2021-02-13T00:18:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175491.14 epoch sec; seqNo:11421; 4058833.93 north; 592320.64 east 2021-02-13T00:18:11Z- AHRS_M2 msg: 1613175491.52 epoch sec; seqNo:11327 2021-02-13T00:18:11Z- AHRS_M2 msg: 0.74 phi; -0.28 theta; -0.19 psi 2021-02-13T00:18:11Z- handleDepth()-Depth_Keller msg: 1613175491521 epoch sec; seqNo:11422; depth 4.33 2021-02-13T00:18:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175491.53 epoch sec; seqNo:11422; 4058833.93 north; 592320.64 east 2021-02-13T00:18:11Z- AHRS_M2 msg: 1613175491.92 epoch sec; seqNo:11328 2021-02-13T00:18:11Z- AHRS_M2 msg: 0.73 phi; -0.29 theta; -0.21 psi 2021-02-13T00:18:11Z- handleDepth()-Depth_Keller msg: 1613175491929 epoch sec; seqNo:11423; depth 4.43 2021-02-13T00:18:11Z- handleDvl() - RDI_Pathfinder msg: 1613175491.95 epoch sec; seqNo:5691 2021-02-13T00:18:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.58 , 109.48 , 85.68 , 118.41 2021-02-13T00:18:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.48 , 0.20 2021-02-13T00:18:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175491.96 epoch sec; seqNo:11423; 4058834.31 north; 592320.64 east 2021-02-13T00:18:12Z- AHRS_M2 msg: 1613175492.32 epoch sec; seqNo:11329 2021-02-13T00:18:12Z- AHRS_M2 msg: 0.72 phi; -0.29 theta; -0.23 psi 2021-02-13T00:18:12Z- handleDepth()-Depth_Keller msg: 1613175492328 epoch sec; seqNo:11424; depth 4.49 2021-02-13T00:18:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175492.34 epoch sec; seqNo:11424; 4058834.31 north; 592320.64 east 2021-02-13T00:18:12Z- AHRS_M2 msg: 1613175492.73 epoch sec; seqNo:11330 2021-02-13T00:18:12Z- AHRS_M2 msg: 0.70 phi; -0.29 theta; -0.27 psi 2021-02-13T00:18:12Z- handleDepth()-Depth_Keller msg: 1613175492738 epoch sec; seqNo:11425; depth 4.57 2021-02-13T00:18:12Z- handleDvl() - RDI_Pathfinder msg: 1613175492.75 epoch sec; seqNo:5692 2021-02-13T00:18:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.41 , 112.73 , 85.95 , 120.16 2021-02-13T00:18:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.31 , -0.35 2021-02-13T00:18:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175492.76 epoch sec; seqNo:11425; 4058834.70 north; 592321.85 east 2021-02-13T00:18:13Z- AHRS_M2 msg: 1613175493.14 epoch sec; seqNo:11331 2021-02-13T00:18:13Z- AHRS_M2 msg: 0.70 phi; -0.28 theta; -0.31 psi 2021-02-13T00:18:13Z- handleDepth()-Depth_Keller msg: 1613175493146 epoch sec; seqNo:11426; depth 4.66 2021-02-13T00:18:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:18:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:18:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:18:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:18:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:18:13Z- handleDvl() - RDI_Pathfinder msg: 1613175493.15 epoch sec; seqNo:5693 2021-02-13T00:18:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:18:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.31 , -0.35 2021-02-13T00:18:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175493.16 epoch sec; seqNo:11426; 4058834.70 north; 592321.85 east 2021-02-13T00:18:13Z- LcmTrn::handleMessages() - handling 47 messages took 0.63 ms and 135 calls... 2021-02-13T00:18:13Z- waiting for 1613175490.90, time is 1613175493.16 2021-02-13T00:18:13Z- LcmTrn::updateTrn() equating pose 1613175493.16 and meas times 1613175493.16 2021-02-13T00:18:13Z- LcmTrn::updateTrn() >>>> pose time 1613175493.16, meas time 1613175493.16 <<<< 2021-02-13T00:18:13Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175493.16, ping # 0. 2021-02-13T00:18:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175493.16 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:18:13Z- TerrainNav::motionUpdate() - currEstimate = (4058835, 592321.85, 4.66) 2021-02-13T00:18:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:18:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:18:13Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:18:13Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:18:13Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175493.16 sec.; No relevant navigation data available 2021-02-13T00:18:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:18:13Z- TNavBF::mmse selected from filter -1 2021-02-13T00:18:13Z- variances([0], [2], and psi_berg) = 709.524472 833.003209 0.000000 2021-02-13T00:18:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:18:13Z- LcmTrn::publishEstimates() - MLE : 4058834.20 592371.35 4.66 2021-02-13T00:18:13Z- LcmTrn::publishEstimates() - MMSE : 4058834.70 592321.85 4.66 2021-02-13T00:18:13Z- LcmTrn::publishEstimates() - COVAR: 709.52 833.00 1.30 0.00 2021-02-13T00:18:13Z- AHRS_M2 msg: 1613175493.54 epoch sec; seqNo:11332 2021-02-13T00:18:13Z- AHRS_M2 msg: 0.69 phi; -0.29 theta; -0.34 psi 2021-02-13T00:18:13Z- handleDepth()-Depth_Keller msg: 1613175493540 epoch sec; seqNo:11427; depth 4.74 2021-02-13T00:18:13Z- handleDvl() - RDI_Pathfinder msg: 1613175493.55 epoch sec; seqNo:5694 2021-02-13T00:18:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 121.72 , 0.00 , 118.74 2021-02-13T00:18:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.27 , -0.30 2021-02-13T00:18:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175493.57 epoch sec; seqNo:11427; 4058835.08 north; 592321.85 east 2021-02-13T00:18:13Z- AHRS_M2 msg: 1613175493.94 epoch sec; seqNo:11333 2021-02-13T00:18:13Z- AHRS_M2 msg: 0.69 phi; -0.29 theta; -0.34 psi 2021-02-13T00:18:13Z- handleDepth()-Depth_Keller msg: 1613175493949 epoch sec; seqNo:11428; depth 4.82 2021-02-13T00:18:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175493.96 epoch sec; seqNo:11428; 4058835.46 north; 592321.85 east 2021-02-13T00:18:14Z- AHRS_M2 msg: 1613175494.34 epoch sec; seqNo:11334 2021-02-13T00:18:14Z- AHRS_M2 msg: 0.69 phi; -0.30 theta; -0.32 psi 2021-02-13T00:18:14Z- handleDepth()-Depth_Keller msg: 1613175494349 epoch sec; seqNo:11429; depth 4.93 2021-02-13T00:18:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:18:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:18:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:18:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:18:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:18:14Z- handleDvl() - RDI_Pathfinder msg: 1613175494.35 epoch sec; seqNo:5695 2021-02-13T00:18:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:18:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.27 , -0.30 2021-02-13T00:18:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175494.37 epoch sec; seqNo:11429; 4058835.84 north; 592321.84 east 2021-02-13T00:18:14Z- AHRS_M2 msg: 1613175494.75 epoch sec; seqNo:11335 2021-02-13T00:18:14Z- AHRS_M2 msg: 0.69 phi; -0.31 theta; -0.29 psi 2021-02-13T00:18:14Z- handleDepth()-Depth_Keller msg: 1613175494752 epoch sec; seqNo:11430; depth 5.02 2021-02-13T00:18:14Z- handleDvl() - RDI_Pathfinder msg: 1613175494.76 epoch sec; seqNo:5696 2021-02-13T00:18:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.82 , 125.50 , 97.24 , 125.50 2021-02-13T00:18:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.28 , 0.25 2021-02-13T00:18:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175494.79 epoch sec; seqNo:11430; 4058836.22 north; 592321.84 east 2021-02-13T00:18:15Z- AHRS_M2 msg: 1613175495.15 epoch sec; seqNo:11336 2021-02-13T00:18:15Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.24 psi 2021-02-13T00:18:15Z- handleDepth()-Depth_Keller msg: 1613175495158 epoch sec; seqNo:11431; depth 5.11 2021-02-13T00:18:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175495.17 epoch sec; seqNo:11431; 4058836.60 north; 592321.83 east 2021-02-13T00:18:15Z- AHRS_M2 msg: 1613175495.56 epoch sec; seqNo:11337 2021-02-13T00:18:15Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.21 psi 2021-02-13T00:18:15Z- handleDepth()-Depth_Keller msg: 1613175495565 epoch sec; seqNo:11432; depth 5.19 2021-02-13T00:18:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:15Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:18:15Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:18:15Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:18:15Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:18:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:18:15Z- handleDvl() - RDI_Pathfinder msg: 1613175495.57 epoch sec; seqNo:5697 2021-02-13T00:18:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:18:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.28 , 0.25 2021-02-13T00:18:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175495.58 epoch sec; seqNo:11432; 4058836.61 north; 592323.06 east 2021-02-13T00:18:15Z- AHRS_M2 msg: 1613175495.96 epoch sec; seqNo:11338 2021-02-13T00:18:15Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.20 psi 2021-02-13T00:18:15Z- handleDepth()-Depth_Keller msg: 1613175495969 epoch sec; seqNo:11433; depth 5.30 2021-02-13T00:18:15Z- handleDvl() - RDI_Pathfinder msg: 1613175495.99 epoch sec; seqNo:5698 2021-02-13T00:18:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.38 , 113.74 , 0.00 , 128.62 2021-02-13T00:18:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , 0.12 , 0.35 2021-02-13T00:18:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175496.00 epoch sec; seqNo:11433; 4058836.99 north; 592323.05 east 2021-02-13T00:18:16Z- AHRS_M2 msg: 1613175496.36 epoch sec; seqNo:11339 2021-02-13T00:18:16Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-13T00:18:16Z- handleDepth()-Depth_Keller msg: 1613175496368 epoch sec; seqNo:11434; depth 5.39 2021-02-13T00:18:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175496.38 epoch sec; seqNo:11434; 4058836.99 north; 592323.05 east 2021-02-13T00:18:16Z- AHRS_M2 msg: 1613175496.77 epoch sec; seqNo:11340 2021-02-13T00:18:16Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.19 psi 2021-02-13T00:18:16Z- handleDepth()-Depth_Keller msg: 1613175496772 epoch sec; seqNo:11435; depth 5.50 2021-02-13T00:18:16Z- handleDvl() - RDI_Pathfinder msg: 1613175496.78 epoch sec; seqNo:5699 2021-02-13T00:18:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.78 , 112.12 , 0.00 , 125.51 2021-02-13T00:18:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.30 , 0.08 2021-02-13T00:18:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175496.79 epoch sec; seqNo:11435; 4058836.99 north; 592323.05 east 2021-02-13T00:18:17Z- AHRS_M2 msg: 1613175497.18 epoch sec; seqNo:11341 2021-02-13T00:18:17Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.20 psi 2021-02-13T00:18:17Z- handleDepth()-Depth_Keller msg: 1613175497182 epoch sec; seqNo:11436; depth 5.61 2021-02-13T00:18:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175497.19 epoch sec; seqNo:11436; 4058837.37 north; 592323.05 east 2021-02-13T00:18:17Z- AHRS_M2 msg: 1613175497.58 epoch sec; seqNo:11342 2021-02-13T00:18:17Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.22 psi 2021-02-13T00:18:17Z- handleDepth()-Depth_Keller msg: 1613175497590 epoch sec; seqNo:11437; depth 5.74 2021-02-13T00:18:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175497.60 epoch sec; seqNo:11437; 4058837.37 north; 592323.05 east 2021-02-13T00:18:17Z- AHRS_M2 msg: 1613175497.98 epoch sec; seqNo:11343 2021-02-13T00:18:17Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.23 psi 2021-02-13T00:18:17Z- handleDepth()-Depth_Keller msg: 1613175497984 epoch sec; seqNo:11438; depth 5.85 2021-02-13T00:18:18Z- handleDvl() - RDI_Pathfinder msg: 1613175498.00 epoch sec; seqNo:5700 2021-02-13T00:18:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.29 , 113.61 , 83.85 , 128.48 2021-02-13T00:18:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.16 , -0.28 2021-02-13T00:18:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175498.02 epoch sec; seqNo:11438; 4058837.76 north; 592324.26 east 2021-02-13T00:18:18Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-13T00:18:18Z- waiting for 1613175496.16, time is 1613175498.02 2021-02-13T00:18:18Z- LcmTrn::updateTrn() equating pose 1613175498.02 and meas times 1613175498.02 2021-02-13T00:18:18Z- LcmTrn::updateTrn() >>>> pose time 1613175498.02, meas time 1613175498.02 <<<< 2021-02-13T00:18:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175498.02 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:18:18Z- TerrainNav::motionUpdate() - currEstimate = (4058838, 592324.26, 5.85) 2021-02-13T00:18:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:18:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:18:18Z- TNavBF::Projecting Measurements 2021-02-13T00:18:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:18:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:18:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:18:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:18:18Z- TNavFilter::4 good beams. 2021-02-13T00:18:18Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:18:18Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:18:18Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:18:18Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:18:18Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:18:18Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:18:18Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:18:18Z- TNavBF::mmse selected from filter -1 2021-02-13T00:18:18Z- TerrainMapOctree: (vehN, vehE ) = (4058837.76, 592324.26). 2021-02-13T00:18:18Z- TerrainMapOctree: Min Distance = 3086.64. 2021-02-13T00:18:18Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:18:18Z- There is no specific good beam that all particles have in common. 2021-02-13T00:18:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:18:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.50 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 13.33 2021-02-13T00:18:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:18:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.36 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 20.65 2021-02-13T00:18:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:18:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 11.25 2021-02-13T00:18:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:18:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 26.41 2021-02-13T00:18:18Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:18:18Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:18:18Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:18:18Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:18:18Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:18:18Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:18:18Z- Filter 0 No beams used in subcloud update 2021-02-13T00:18:18Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:18:18Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:18:18Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 709.5 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:18:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175498.02 sec. 2021-02-13T00:18:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:18:18Z- TNavBF::mmse selected from filter -1 2021-02-13T00:18:18Z- variances([0], [2], and psi_berg) = 709.465281 833.003175 0.000000 2021-02-13T00:18:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:18:18Z- LcmTrn::publishEstimates() - MLE : 4058837.26 592373.76 5.85 2021-02-13T00:18:18Z- LcmTrn::publishEstimates() - MMSE : 4058837.76 592324.26 5.85 2021-02-13T00:18:18Z- LcmTrn::publishEstimates() - COVAR: 709.47 833.00 1.12 0.00 2021-02-13T00:18:18Z- AHRS_M2 msg: 1613175498.39 epoch sec; seqNo:11344 2021-02-13T00:18:18Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.25 psi 2021-02-13T00:18:18Z- handleDepth()-Depth_Keller msg: 1613175498411 epoch sec; seqNo:11439; depth 5.98 2021-02-13T00:18:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175498.43 epoch sec; seqNo:11439; 4058838.14 north; 592324.26 east 2021-02-13T00:18:18Z- AHRS_M2 msg: 1613175498.79 epoch sec; seqNo:11345 2021-02-13T00:18:18Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.27 psi 2021-02-13T00:18:18Z- handleDepth()-Depth_Keller msg: 1613175498792 epoch sec; seqNo:11440; depth 6.09 2021-02-13T00:18:18Z- handleDvl() - RDI_Pathfinder msg: 1613175498.80 epoch sec; seqNo:5701 2021-02-13T00:18:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.28 , 115.57 , 82.84 , 128.96 2021-02-13T00:18:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.20 , -0.27 2021-02-13T00:18:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175498.83 epoch sec; seqNo:11440; 4058838.52 north; 592324.26 east 2021-02-13T00:18:19Z- AHRS_M2 msg: 1613175499.19 epoch sec; seqNo:11346 2021-02-13T00:18:19Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.27 psi 2021-02-13T00:18:19Z- handleDepth()-Depth_Keller msg: 1613175499201 epoch sec; seqNo:11441; depth 6.21 2021-02-13T00:18:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:18:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:18:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:18:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:18:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:18:19Z- handleDvl() - RDI_Pathfinder msg: 1613175499.20 epoch sec; seqNo:5702 2021-02-13T00:18:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:18:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.20 , -0.27 2021-02-13T00:18:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175499.22 epoch sec; seqNo:11441; 4058838.89 north; 592324.25 east 2021-02-13T00:18:19Z- AHRS_M2 msg: 1613175499.60 epoch sec; seqNo:11347 2021-02-13T00:18:19Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.26 psi 2021-02-13T00:18:19Z- handleDepth()-Depth_Keller msg: 1613175499609 epoch sec; seqNo:11442; depth 6.35 2021-02-13T00:18:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175499.62 epoch sec; seqNo:11442; 4058838.89 north; 592324.25 east 2021-02-13T00:18:20Z- AHRS_M2 msg: 1613175500.01 epoch sec; seqNo:11348 2021-02-13T00:18:20Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.20 psi 2021-02-13T00:18:20Z- handleDepth()-Depth_Keller msg: 1613175500015 epoch sec; seqNo:11443; depth 6.48 2021-02-13T00:18:20Z- handleDvl() - RDI_Pathfinder msg: 1613175500.03 epoch sec; seqNo:5703 2021-02-13T00:18:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.79 , 118.54 , 84.33 , 127.47 2021-02-13T00:18:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.26 , 0.17 2021-02-13T00:18:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175500.04 epoch sec; seqNo:11443; 4058839.27 north; 592324.25 east 2021-02-13T00:18:20Z- AHRS_M2 msg: 1613175500.41 epoch sec; seqNo:11349 2021-02-13T00:18:20Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.18 psi 2021-02-13T00:18:20Z- handleDepth()-Depth_Keller msg: 1613175500421 epoch sec; seqNo:11444; depth 6.63 2021-02-13T00:18:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:18:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:18:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:18:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:18:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:18:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:18:20Z- handleDvl() - RDI_Pathfinder msg: 1613175500.42 epoch sec; seqNo:5704 2021-02-13T00:18:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:18:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.26 , 0.17 2021-02-13T00:18:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175500.45 epoch sec; seqNo:11444; 4058839.65 north; 592324.24 east 2021-02-13T00:18:20Z- AHRS_M2 msg: 1613175500.81 epoch sec; seqNo:11350 2021-02-13T00:18:20Z- AHRS_M2 msg: 0.66 phi; -0.39 theta; -0.17 psi 2021-02-13T00:18:20Z- handleDepth()-Depth_Keller msg: 1613175500816 epoch sec; seqNo:11445; depth 6.77 2021-02-13T00:18:20Z- handleDvl() - RDI_Pathfinder msg: 1613175500.83 epoch sec; seqNo:5705 2021-02-13T00:18:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 111.10 , 82.84 , 127.47 2021-02-13T00:18:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , 0.05 , 0.41 2021-02-13T00:18:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175500.84 epoch sec; seqNo:11445; 4058839.67 north; 592325.46 east 2021-02-13T00:18:21Z- AHRS_M2 msg: 1613175501.22 epoch sec; seqNo:11351 2021-02-13T00:18:21Z- AHRS_M2 msg: 0.65 phi; -0.39 theta; -0.18 psi 2021-02-13T00:18:21Z- handleDepth()-Depth_Keller msg: 1613175501223 epoch sec; seqNo:11446; depth 6.89 2021-02-13T00:18:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175501.26 epoch sec; seqNo:11446; 4058840.04 north; 592325.46 east 2021-02-13T00:18:21Z- AHRS_M2 msg: 1613175501.62 epoch sec; seqNo:11352 2021-02-13T00:18:21Z- AHRS_M2 msg: 0.64 phi; -0.40 theta; -0.18 psi 2021-02-13T00:18:21Z- handleDepth()-Depth_Keller msg: 1613175501629 epoch sec; seqNo:11447; depth 7.02 2021-02-13T00:18:21Z- handleDvl() - RDI_Pathfinder msg: 1613175501.65 epoch sec; seqNo:5706 2021-02-13T00:18:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 109.62 , 0.00 , 136.39 2021-02-13T00:18:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , 0.36 , 0.18 2021-02-13T00:18:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175501.66 epoch sec; seqNo:11447; 4058840.04 north; 592325.46 east 2021-02-13T00:18:22Z- AHRS_M2 msg: 1613175502.02 epoch sec; seqNo:11353 2021-02-13T00:18:22Z- AHRS_M2 msg: 0.64 phi; -0.39 theta; -0.18 psi 2021-02-13T00:18:22Z- handleDepth()-Depth_Keller msg: 1613175502028 epoch sec; seqNo:11448; depth 7.13 2021-02-13T00:18:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175502.04 epoch sec; seqNo:11448; 4058840.04 north; 592325.46 east 2021-02-13T00:18:22Z- AHRS_M2 msg: 1613175502.44 epoch sec; seqNo:11354 2021-02-13T00:18:22Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.18 psi 2021-02-13T00:18:22Z- handleDepth()-Depth_Keller msg: 1613175502441 epoch sec; seqNo:11449; depth 7.26 2021-02-13T00:18:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175502.45 epoch sec; seqNo:11449; 4058840.42 north; 592325.46 east 2021-02-13T00:18:22Z- AHRS_M2 msg: 1613175502.83 epoch sec; seqNo:11355 2021-02-13T00:18:22Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.20 psi 2021-02-13T00:18:22Z- handleDepth()-Depth_Keller msg: 1613175502838 epoch sec; seqNo:11450; depth 7.41 2021-02-13T00:18:22Z- handleDvl() - RDI_Pathfinder msg: 1613175502.85 epoch sec; seqNo:5707 2021-02-13T00:18:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 110.62 , 79.69 , 134.42 2021-02-13T00:18:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.31 , -0.20 2021-02-13T00:18:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175502.86 epoch sec; seqNo:11450; 4058840.44 north; 592326.68 east 2021-02-13T00:18:23Z- AHRS_M2 msg: 1613175503.24 epoch sec; seqNo:11356 2021-02-13T00:18:23Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.21 psi 2021-02-13T00:18:23Z- handleDepth()-Depth_Keller msg: 1613175503241 epoch sec; seqNo:11451; depth 7.56 2021-02-13T00:18:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175503.26 epoch sec; seqNo:11451; 4058840.81 north; 592326.67 east 2021-02-13T00:18:23Z- LcmTrn::handleMessages() - handling 46 messages took 0.65 ms and 133 calls... 2021-02-13T00:18:23Z- waiting for 1613175501.02, time is 1613175503.26 2021-02-13T00:18:23Z- LcmTrn::updateTrn() >>>> pose time 1613175503.26, meas time 1613175502.85 <<<< 2021-02-13T00:18:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175502.85 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:18:23Z- TerrainNav::motionUpdate() - currEstimate = (4058841, 592326.67, 7.56) 2021-02-13T00:18:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:18:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:18:23Z- TNavBF::Projecting Measurements 2021-02-13T00:18:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:18:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:18:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:18:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:18:23Z- TNavFilter::4 good beams. 2021-02-13T00:18:23Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:18:23Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:18:23Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:18:23Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:18:23Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:18:23Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:18:23Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:18:23Z- TNavBF::mmse selected from filter -1 2021-02-13T00:18:23Z- TerrainMapOctree: (vehN, vehE ) = (4058840.58, 592326.48). 2021-02-13T00:18:23Z- TerrainMapOctree: Min Distance = 3083.06. 2021-02-13T00:18:23Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:18:23Z- There is no specific good beam that all particles have in common. 2021-02-13T00:18:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:18:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-13T00:18:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:18:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 19.58 2021-02-13T00:18:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:18:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 10.16 2021-02-13T00:18:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:18:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.033 ExpectedMeasDiff: nan currMeas.covariance: 28.91 2021-02-13T00:18:23Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:18:23Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:18:23Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:18:23Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:18:23Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:18:23Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:18:23Z- Filter 0 No beams used in subcloud update 2021-02-13T00:18:23Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:18:23Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:18:23Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 709.4 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:18:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175502.85 sec. 2021-02-13T00:18:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:18:23Z- TNavBF::mmse selected from filter -1 2021-02-13T00:18:23Z- variances([0], [2], and psi_berg) = 709.391955 833.003132 0.000000 2021-02-13T00:18:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:18:23Z- LcmTrn::publishEstimates() - MLE : 4058840.31 592376.17 7.56 2021-02-13T00:18:23Z- LcmTrn::publishEstimates() - MMSE : 4058840.81 592326.67 7.56 2021-02-13T00:18:23Z- LcmTrn::publishEstimates() - COVAR: 709.39 833.00 1.03 0.00 2021-02-13T00:18:23Z- AHRS_M2 msg: 1613175503.64 epoch sec; seqNo:11357 2021-02-13T00:18:23Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.23 psi 2021-02-13T00:18:23Z- handleDepth()-Depth_Keller msg: 1613175503644 epoch sec; seqNo:11452; depth 7.69 2021-02-13T00:18:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175503.65 epoch sec; seqNo:11452; 4058841.19 north; 592326.67 east 2021-02-13T00:18:24Z- AHRS_M2 msg: 1613175504.05 epoch sec; seqNo:11358 2021-02-13T00:18:24Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.23 psi 2021-02-13T00:18:24Z- handleDepth()-Depth_Keller msg: 1613175504049 epoch sec; seqNo:11453; depth 7.82 2021-02-13T00:18:24Z- handleDvl() - RDI_Pathfinder msg: 1613175504.06 epoch sec; seqNo:5708 2021-02-13T00:18:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.72 , 112.52 , 81.28 , 124.42 2021-02-13T00:18:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.01 , -0.34 2021-02-13T00:18:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175504.07 epoch sec; seqNo:11453; 4058841.57 north; 592326.66 east 2021-02-13T00:18:24Z- AHRS_M2 msg: 1613175504.45 epoch sec; seqNo:11359 2021-02-13T00:18:24Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.23 psi 2021-02-13T00:18:24Z- handleDepth()-Depth_Keller msg: 1613175504452 epoch sec; seqNo:11454; depth 7.95 2021-02-13T00:18:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175504.48 epoch sec; seqNo:11454; 4058841.96 north; 592327.88 east 2021-02-13T00:18:24Z- AHRS_M2 msg: 1613175504.85 epoch sec; seqNo:11360 2021-02-13T00:18:24Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.22 psi 2021-02-13T00:18:24Z- handleDepth()-Depth_Keller msg: 1613175504856 epoch sec; seqNo:11455; depth 8.10 2021-02-13T00:18:24Z- handleDvl() - RDI_Pathfinder msg: 1613175504.87 epoch sec; seqNo:5709 2021-02-13T00:18:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 112.86 , 83.11 , 118.81 2021-02-13T00:18:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.24 , -0.17 2021-02-13T00:18:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175504.89 epoch sec; seqNo:11455; 4058842.34 north; 592327.88 east 2021-02-13T00:18:25Z- AHRS_M2 msg: 1613175505.26 epoch sec; seqNo:11361 2021-02-13T00:18:25Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.21 psi 2021-02-13T00:18:25Z- handleDepth()-Depth_Keller msg: 1613175505265 epoch sec; seqNo:11456; depth 8.22 2021-02-13T00:18:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175505.28 epoch sec; seqNo:11456; 4058842.72 north; 592327.87 east 2021-02-13T00:18:25Z- AHRS_M2 msg: 1613175505.66 epoch sec; seqNo:11362 2021-02-13T00:18:25Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.19 psi 2021-02-13T00:18:25Z- handleDepth()-Depth_Keller msg: 1613175505677 epoch sec; seqNo:11457; depth 8.35 2021-02-13T00:18:25Z- handleDvl() - RDI_Pathfinder msg: 1613175505.69 epoch sec; seqNo:5710 2021-02-13T00:18:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 109.62 , 81.35 , 115.57 2021-02-13T00:18:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.23 , 0.05 2021-02-13T00:18:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175505.70 epoch sec; seqNo:11457; 4058843.10 north; 592327.87 east 2021-02-13T00:18:26Z- AHRS_M2 msg: 1613175506.06 epoch sec; seqNo:11363 2021-02-13T00:18:26Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.18 psi 2021-02-13T00:18:26Z- handleDepth()-Depth_Keller msg: 1613175506068 epoch sec; seqNo:11458; depth 8.47 2021-02-13T00:18:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175506.09 epoch sec; seqNo:11458; 4058843.10 north; 592327.87 east 2021-02-13T00:18:26Z- AHRS_M2 msg: 1613175506.47 epoch sec; seqNo:11364 2021-02-13T00:18:26Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.17 psi 2021-02-13T00:18:26Z- handleDepth()-Depth_Keller msg: 1613175506473 epoch sec; seqNo:11459; depth 8.59 2021-02-13T00:18:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175506.49 epoch sec; seqNo:11459; 4058843.48 north; 592327.86 east 2021-02-13T00:18:26Z- AHRS_M2 msg: 1613175506.87 epoch sec; seqNo:11365 2021-02-13T00:18:26Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.16 psi 2021-02-13T00:18:26Z- handleDepth()-Depth_Keller msg: 1613175506881 epoch sec; seqNo:11460; depth 8.71 2021-02-13T00:18:26Z- handleDvl() - RDI_Pathfinder msg: 1613175506.89 epoch sec; seqNo:5711 2021-02-13T00:18:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.50 , 105.36 , 81.56 , 114.28 2021-02-13T00:18:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.12 , 0.11 2021-02-13T00:18:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175506.90 epoch sec; seqNo:11460; 4058843.86 north; 592327.86 east 2021-02-13T00:18:27Z- AHRS_M2 msg: 1613175507.28 epoch sec; seqNo:11366 2021-02-13T00:18:27Z- AHRS_M2 msg: 0.64 phi; -0.29 theta; -0.15 psi 2021-02-13T00:18:27Z- handleDepth()-Depth_Keller msg: 1613175507283 epoch sec; seqNo:11461; depth 8.82 2021-02-13T00:18:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175507.29 epoch sec; seqNo:11461; 4058844.24 north; 592327.86 east 2021-02-13T00:18:27Z- AHRS_M2 msg: 1613175507.68 epoch sec; seqNo:11367 2021-02-13T00:18:27Z- AHRS_M2 msg: 0.64 phi; -0.29 theta; -0.16 psi 2021-02-13T00:18:27Z- handleDepth()-Depth_Keller msg: 1613175507685 epoch sec; seqNo:11462; depth 8.94 2021-02-13T00:18:27Z- handleDvl() - RDI_Pathfinder msg: 1613175507.70 epoch sec; seqNo:5712 2021-02-13T00:18:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.37 , 81.08 , 114.89 2021-02-13T00:18:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.08 , 0.06 2021-02-13T00:18:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175507.71 epoch sec; seqNo:11462; 4058844.25 north; 592329.08 east 2021-02-13T00:18:28Z- AHRS_M2 msg: 1613175508.08 epoch sec; seqNo:11368 2021-02-13T00:18:28Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.17 psi 2021-02-13T00:18:28Z- handleDepth()-Depth_Keller msg: 1613175508088 epoch sec; seqNo:11463; depth 9.04 2021-02-13T00:18:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175508.10 epoch sec; seqNo:11463; 4058844.63 north; 592329.07 east 2021-02-13T00:18:28Z- AHRS_M2 msg: 1613175508.49 epoch sec; seqNo:11369 2021-02-13T00:18:28Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.17 psi 2021-02-13T00:18:28Z- handleDepth()-Depth_Keller msg: 1613175508495 epoch sec; seqNo:11464; depth 9.15 2021-02-13T00:18:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175508.51 epoch sec; seqNo:11464; 4058845.01 north; 592329.07 east 2021-02-13T00:18:28Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:18:28Z- waiting for 1613175506.26, time is 1613175508.51 2021-02-13T00:18:28Z- LcmTrn::updateTrn() >>>> pose time 1613175508.51, meas time 1613175507.70 <<<< 2021-02-13T00:18:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175507.70 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:18:28Z- TerrainNav::motionUpdate() - currEstimate = (4058845, 592329.07, 9.15) 2021-02-13T00:18:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:18:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:18:28Z- TNavBF::Projecting Measurements 2021-02-13T00:18:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:18:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:18:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:18:28Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:18:28Z- TNavFilter::3 good beams. 2021-02-13T00:18:28Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:18:28Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:18:28Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:18:28Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:18:28Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:18:28Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:18:28Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:18:28Z- TNavBF::mmse selected from filter -1 2021-02-13T00:18:28Z- TerrainMapOctree: (vehN, vehE ) = (4058844.37, 592328.70). 2021-02-13T00:18:28Z- TerrainMapOctree: Min Distance = 3078.71. 2021-02-13T00:18:28Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:18:28Z- There is no specific good beam that all particles have in common. 2021-02-13T00:18:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:18:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 16.44 2021-02-13T00:18:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:18:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 10.52 2021-02-13T00:18:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:18:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 21.12 2021-02-13T00:18:28Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:18:28Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:18:28Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:18:28Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:18:28Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:18:28Z- Filter 0 No beams used in subcloud update 2021-02-13T00:18:28Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:18:28Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:18:28Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 709.3 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:18:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175507.70 sec. 2021-02-13T00:18:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:18:28Z- TNavBF::mmse selected from filter -1 2021-02-13T00:18:28Z- variances([0], [2], and psi_berg) = 709.305602 833.003082 0.000000 2021-02-13T00:18:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:18:28Z- LcmTrn::publishEstimates() - MLE : 4058844.51 592378.57 9.15 2021-02-13T00:18:28Z- LcmTrn::publishEstimates() - MMSE : 4058845.01 592329.07 9.15 2021-02-13T00:18:28Z- LcmTrn::publishEstimates() - COVAR: 709.31 833.00 1.05 0.00 2021-02-13T00:18:28Z- AHRS_M2 msg: 1613175508.89 epoch sec; seqNo:11370 2021-02-13T00:18:28Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.17 psi 2021-02-13T00:18:28Z- handleDepth()-Depth_Keller msg: 1613175508898 epoch sec; seqNo:11465; depth 9.23 2021-02-13T00:18:28Z- handleDvl() - RDI_Pathfinder msg: 1613175508.91 epoch sec; seqNo:5713 2021-02-13T00:18:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.28 , 105.21 , 78.88 , 117.38 2021-02-13T00:18:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.06 , 0.07 2021-02-13T00:18:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175508.92 epoch sec; seqNo:11465; 4058845.01 north; 592329.07 east 2021-02-13T00:18:29Z- AHRS_M2 msg: 1613175509.30 epoch sec; seqNo:11371 2021-02-13T00:18:29Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.17 psi 2021-02-13T00:18:29Z- handleDepth()-Depth_Keller msg: 1613175509300 epoch sec; seqNo:11466; depth 9.35 2021-02-13T00:18:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175509.32 epoch sec; seqNo:11466; 4058845.39 north; 592329.06 east 2021-02-13T00:18:29Z- AHRS_M2 msg: 1613175509.70 epoch sec; seqNo:11372 2021-02-13T00:18:29Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.17 psi 2021-02-13T00:18:29Z- handleDepth()-Depth_Keller msg: 1613175509704 epoch sec; seqNo:11467; depth 9.46 2021-02-13T00:18:29Z- handleDvl() - RDI_Pathfinder msg: 1613175509.71 epoch sec; seqNo:5714 2021-02-13T00:18:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 104.07 , 78.47 , 116.24 2021-02-13T00:18:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.02 , 0.12 2021-02-13T00:18:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175509.74 epoch sec; seqNo:11467; 4058845.39 north; 592329.06 east 2021-02-13T00:18:30Z- AHRS_M2 msg: 1613175510.10 epoch sec; seqNo:11373 2021-02-13T00:18:30Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.17 psi 2021-02-13T00:18:30Z- handleDepth()-Depth_Keller msg: 1613175510108 epoch sec; seqNo:11468; depth 9.57 2021-02-13T00:18:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175510.12 epoch sec; seqNo:11468; 4058845.77 north; 592329.06 east 2021-02-13T00:18:30Z- AHRS_M2 msg: 1613175510.51 epoch sec; seqNo:11374 2021-02-13T00:18:30Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.17 psi 2021-02-13T00:18:30Z- handleDepth()-Depth_Keller msg: 1613175510521 epoch sec; seqNo:11469; depth 9.67 2021-02-13T00:18:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175510.53 epoch sec; seqNo:11469; 4058846.15 north; 592329.06 east 2021-02-13T00:18:30Z- AHRS_M2 msg: 1613175510.91 epoch sec; seqNo:11375 2021-02-13T00:18:30Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.17 psi 2021-02-13T00:18:30Z- handleDepth()-Depth_Keller msg: 1613175510916 epoch sec; seqNo:11470; depth 9.78 2021-02-13T00:18:30Z- handleDvl() - RDI_Pathfinder msg: 1613175510.93 epoch sec; seqNo:5715 2021-02-13T00:18:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.91 , 104.13 , 79.79 , 117.65 2021-02-13T00:18:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.11 , 0.10 2021-02-13T00:18:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175510.94 epoch sec; seqNo:11470; 4058846.16 north; 592330.28 east 2021-02-13T00:18:31Z- AHRS_M2 msg: 1613175511.32 epoch sec; seqNo:11376 2021-02-13T00:18:31Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.17 psi 2021-02-13T00:18:31Z- handleDepth()-Depth_Keller msg: 1613175511325 epoch sec; seqNo:11471; depth 9.89 2021-02-13T00:18:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175511.34 epoch sec; seqNo:11471; 4058846.54 north; 592330.27 east 2021-02-13T00:18:31Z- AHRS_M2 msg: 1613175511.72 epoch sec; seqNo:11377 2021-02-13T00:18:31Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.17 psi 2021-02-13T00:18:31Z- handleDepth()-Depth_Keller msg: 1613175511729 epoch sec; seqNo:11472; depth 10.02 2021-02-13T00:18:31Z- handleDvl() - RDI_Pathfinder msg: 1613175511.74 epoch sec; seqNo:5716 2021-02-13T00:18:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.42 , 102.65 , 78.48 , 116.18 2021-02-13T00:18:31Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.18 , -0.02 2021-02-13T00:18:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175511.75 epoch sec; seqNo:11472; 4058846.92 north; 592330.27 east 2021-02-13T00:18:32Z- AHRS_M2 msg: 1613175512.13 epoch sec; seqNo:11378 2021-02-13T00:18:32Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.18 psi 2021-02-13T00:18:32Z- handleDepth()-Depth_Keller msg: 1613175512134 epoch sec; seqNo:11473; depth 10.14 2021-02-13T00:18:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175512.15 epoch sec; seqNo:11473; 4058846.92 north; 592330.27 east 2021-02-13T00:18:32Z- AHRS_M2 msg: 1613175512.53 epoch sec; seqNo:11379 2021-02-13T00:18:32Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.18 psi 2021-02-13T00:18:32Z- handleDepth()-Depth_Keller msg: 1613175512532 epoch sec; seqNo:11474; depth 10.26 2021-02-13T00:18:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175512.54 epoch sec; seqNo:11474; 4058847.31 north; 592331.49 east 2021-02-13T00:18:32Z- AHRS_M2 msg: 1613175512.93 epoch sec; seqNo:11380 2021-02-13T00:18:32Z- AHRS_M2 msg: 0.66 phi; -0.30 theta; -0.18 psi 2021-02-13T00:18:32Z- handleDepth()-Depth_Keller msg: 1613175512941 epoch sec; seqNo:11475; depth 10.36 2021-02-13T00:18:32Z- handleDvl() - RDI_Pathfinder msg: 1613175512.95 epoch sec; seqNo:5717 2021-02-13T00:18:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 104.14 , 79.80 , 112.25 2021-02-13T00:18:32Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.11 , -0.14 2021-02-13T00:18:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175512.98 epoch sec; seqNo:11475; 4058847.69 north; 592331.48 east 2021-02-13T00:18:33Z- AHRS_M2 msg: 1613175513.34 epoch sec; seqNo:11381 2021-02-13T00:18:33Z- AHRS_M2 msg: 0.67 phi; -0.30 theta; -0.17 psi 2021-02-13T00:18:33Z- handleDepth()-Depth_Keller msg: 1613175513342 epoch sec; seqNo:11476; depth 10.49 2021-02-13T00:18:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175513.35 epoch sec; seqNo:11476; 4058848.07 north; 592331.48 east 2021-02-13T00:18:33Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-13T00:18:33Z- waiting for 1613175511.51, time is 1613175513.35 2021-02-13T00:18:33Z- LcmTrn::updateTrn() >>>> pose time 1613175513.35, meas time 1613175512.95 <<<< 2021-02-13T00:18:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175512.95 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:18:33Z- TerrainNav::motionUpdate() - currEstimate = (4058848, 592331.48, 10.49) 2021-02-13T00:18:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:18:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:18:33Z- TNavBF::Projecting Measurements 2021-02-13T00:18:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:18:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:18:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:18:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:18:33Z- TNavFilter::4 good beams. 2021-02-13T00:18:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:18:33Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:18:33Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:18:33Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:18:33Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:18:33Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:18:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:18:33Z- TNavBF::mmse selected from filter -1 2021-02-13T00:18:33Z- TerrainMapOctree: (vehN, vehE ) = (4058847.81, 592331.28). 2021-02-13T00:18:33Z- TerrainMapOctree: Min Distance = 3074.41. 2021-02-13T00:18:33Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:18:33Z- There is no specific good beam that all particles have in common. 2021-02-13T00:18:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:18:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 11.99 2021-02-13T00:18:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:18:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 17.35 2021-02-13T00:18:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:18:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 10.19 2021-02-13T00:18:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:18:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 20.16 2021-02-13T00:18:33Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:18:33Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:18:33Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:18:33Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:18:33Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:18:33Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:18:33Z- Filter 0 No beams used in subcloud update 2021-02-13T00:18:33Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:18:33Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:18:33Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 709.2 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:18:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175512.95 sec. 2021-02-13T00:18:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:18:33Z- TNavBF::mmse selected from filter -1 2021-02-13T00:18:33Z- variances([0], [2], and psi_berg) = 709.232119 833.003040 0.000000 2021-02-13T00:18:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:18:33Z- LcmTrn::publishEstimates() - MLE : 4058847.57 592377.98 10.49 2021-02-13T00:18:33Z- LcmTrn::publishEstimates() - MMSE : 4058848.07 592331.48 10.49 2021-02-13T00:18:33Z- LcmTrn::publishEstimates() - COVAR: 709.23 833.00 0.95 0.00 2021-02-13T00:18:33Z- AHRS_M2 msg: 1613175513.74 epoch sec; seqNo:11382 2021-02-13T00:18:33Z- AHRS_M2 msg: 0.68 phi; -0.30 theta; -0.17 psi 2021-02-13T00:18:33Z- handleDepth()-Depth_Keller msg: 1613175513749 epoch sec; seqNo:11477; depth 10.61 2021-02-13T00:18:33Z- handleDvl() - RDI_Pathfinder msg: 1613175513.77 epoch sec; seqNo:5718 2021-02-13T00:18:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.21 , 104.14 , 79.80 , 112.25 2021-02-13T00:18:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , -0.20 2021-02-13T00:18:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175513.78 epoch sec; seqNo:11477; 4058848.44 north; 592331.47 east 2021-02-13T00:18:34Z- AHRS_M2 msg: 1613175514.15 epoch sec; seqNo:11383 2021-02-13T00:18:34Z- AHRS_M2 msg: 0.68 phi; -0.30 theta; -0.16 psi 2021-02-13T00:18:34Z- handleDepth()-Depth_Keller msg: 1613175514161 epoch sec; seqNo:11478; depth 10.74 2021-02-13T00:18:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175514.18 epoch sec; seqNo:11478; 4058848.44 north; 592331.47 east 2021-02-13T00:18:34Z- AHRS_M2 msg: 1613175514.55 epoch sec; seqNo:11384 2021-02-13T00:18:34Z- AHRS_M2 msg: 0.69 phi; -0.30 theta; -0.16 psi 2021-02-13T00:18:34Z- handleDepth()-Depth_Keller msg: 1613175514554 epoch sec; seqNo:11479; depth 10.86 2021-02-13T00:18:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175514.56 epoch sec; seqNo:11479; 4058848.84 north; 592332.69 east 2021-02-13T00:18:34Z- AHRS_M2 msg: 1613175514.95 epoch sec; seqNo:11385 2021-02-13T00:18:34Z- AHRS_M2 msg: 0.69 phi; -0.30 theta; -0.16 psi 2021-02-13T00:18:34Z- handleDepth()-Depth_Keller msg: 1613175514957 epoch sec; seqNo:11480; depth 10.96 2021-02-13T00:18:34Z- handleDvl() - RDI_Pathfinder msg: 1613175514.97 epoch sec; seqNo:5719 2021-02-13T00:18:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.27 , 101.50 , 79.86 , 108.26 2021-02-13T00:18:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.11 , -0.15 2021-02-13T00:18:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175514.98 epoch sec; seqNo:11480; 4058849.22 north; 592332.69 east 2021-02-13T00:18:35Z- AHRS_M2 msg: 1613175515.36 epoch sec; seqNo:11386 2021-02-13T00:18:35Z- AHRS_M2 msg: 0.69 phi; -0.30 theta; -0.15 psi 2021-02-13T00:18:35Z- handleDepth()-Depth_Keller msg: 1613175515360 epoch sec; seqNo:11481; depth 11.09 2021-02-13T00:18:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175515.38 epoch sec; seqNo:11481; 4058849.59 north; 592332.68 east 2021-02-13T00:18:35Z- AHRS_M2 msg: 1613175515.76 epoch sec; seqNo:11387 2021-02-13T00:18:35Z- AHRS_M2 msg: 0.69 phi; -0.30 theta; -0.15 psi 2021-02-13T00:18:35Z- handleDepth()-Depth_Keller msg: 1613175515764 epoch sec; seqNo:11482; depth 11.22 2021-02-13T00:18:35Z- handleDvl() - RDI_Pathfinder msg: 1613175515.78 epoch sec; seqNo:5720 2021-02-13T00:18:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.28 , 78.64 , 112.45 2021-02-13T00:18:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.11 , -0.04 2021-02-13T00:18:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175515.80 epoch sec; seqNo:11482; 4058849.97 north; 592332.68 east 2021-02-13T00:18:36Z- AHRS_M2 msg: 1613175516.16 epoch sec; seqNo:11388 2021-02-13T00:18:36Z- AHRS_M2 msg: 0.68 phi; -0.30 theta; -0.14 psi 2021-02-13T00:18:36Z- handleDepth()-Depth_Keller msg: 1613175516168 epoch sec; seqNo:11483; depth 11.32 2021-02-13T00:18:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175516.18 epoch sec; seqNo:11483; 4058849.97 north; 592332.68 east 2021-02-13T00:18:36Z- AHRS_M2 msg: 1613175516.57 epoch sec; seqNo:11389 2021-02-13T00:18:36Z- AHRS_M2 msg: 0.68 phi; -0.30 theta; -0.15 psi 2021-02-13T00:18:36Z- handleDepth()-Depth_Keller msg: 1613175516585 epoch sec; seqNo:11484; depth 11.43 2021-02-13T00:18:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175516.61 epoch sec; seqNo:11484; 4058850.35 north; 592332.67 east 2021-02-13T00:18:36Z- AHRS_M2 msg: 1613175516.98 epoch sec; seqNo:11390 2021-02-13T00:18:36Z- AHRS_M2 msg: 0.68 phi; -0.30 theta; -0.15 psi 2021-02-13T00:18:36Z- handleDepth()-Depth_Keller msg: 1613175516987 epoch sec; seqNo:11485; depth 11.56 2021-02-13T00:18:37Z- handleDvl() - RDI_Pathfinder msg: 1613175517.00 epoch sec; seqNo:5721 2021-02-13T00:18:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 98.66 , 77.65 , 112.18 2021-02-13T00:18:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , 0.04 2021-02-13T00:18:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175517.01 epoch sec; seqNo:11485; 4058850.74 north; 592333.89 east 2021-02-13T00:18:37Z- AHRS_M2 msg: 1613175517.40 epoch sec; seqNo:11391 2021-02-13T00:18:37Z- AHRS_M2 msg: 0.68 phi; -0.30 theta; -0.15 psi 2021-02-13T00:18:37Z- handleDepth()-Depth_Keller msg: 1613175517401 epoch sec; seqNo:11486; depth 11.67 2021-02-13T00:18:37Z- handleDvl() - RDI_Pathfinder msg: 1613175517.42 epoch sec; seqNo:5722 2021-02-13T00:18:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:18:37Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:18:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175517.43 epoch sec; seqNo:11486; 4058850.74 north; 592333.89 east 2021-02-13T00:18:37Z- AHRS_M2 msg: 1613175517.78 epoch sec; seqNo:11392 2021-02-13T00:18:37Z- AHRS_M2 msg: 0.69 phi; -0.30 theta; -0.15 psi 2021-02-13T00:18:37Z- handleDepth()-Depth_Keller msg: 1613175517785 epoch sec; seqNo:11487; depth 11.80 2021-02-13T00:18:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175517.80 epoch sec; seqNo:11487; 4058851.12 north; 592333.89 east 2021-02-13T00:18:38Z- AHRS_M2 msg: 1613175518.18 epoch sec; seqNo:11393 2021-02-13T00:18:38Z- AHRS_M2 msg: 0.69 phi; -0.30 theta; -0.15 psi 2021-02-13T00:18:38Z- handleDepth()-Depth_Keller msg: 1613175518193 epoch sec; seqNo:11488; depth 11.90 2021-02-13T00:18:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175518.21 epoch sec; seqNo:11488; 4058851.50 north; 592333.88 east 2021-02-13T00:18:38Z- AHRS_M2 msg: 1613175518.59 epoch sec; seqNo:11394 2021-02-13T00:18:38Z- AHRS_M2 msg: 0.70 phi; -0.30 theta; -0.15 psi 2021-02-13T00:18:38Z- handleDepth()-Depth_Keller msg: 1613175518592 epoch sec; seqNo:11489; depth 12.03 2021-02-13T00:18:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175518.60 epoch sec; seqNo:11489; 4058851.50 north; 592333.88 east 2021-02-13T00:18:38Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:18:38Z- waiting for 1613175516.35, time is 1613175518.60 2021-02-13T00:18:38Z- LcmTrn::updateTrn() >>>> pose time 1613175518.60, meas time 1613175517.42 <<<< 2021-02-13T00:18:38Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175517.42, ping # 0. 2021-02-13T00:18:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175517.42 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:18:38Z- TerrainNav::motionUpdate() - currEstimate = (4058852, 592333.88, 12.03) 2021-02-13T00:18:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:18:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:18:38Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:18:38Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:18:38Z- TNavBF::Projecting Measurements 2021-02-13T00:18:38Z- TNavFilter::Measurement from time = 1613175517.42 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:18:38Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:18:38Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:18:38Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:18:38Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:18:38Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:18:38Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:18:38Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:18:38Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 709.2 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:18:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:18:38Z- TNavBF::mmse selected from filter -1 2021-02-13T00:18:38Z- variances([0], [2], and psi_berg) = 709.149162 833.002991 0.000000 2021-02-13T00:18:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:18:38Z- LcmTrn::publishEstimates() - MLE : 4058851.00 592380.38 12.03 2021-02-13T00:18:38Z- LcmTrn::publishEstimates() - MMSE : 4058851.50 592333.88 12.03 2021-02-13T00:18:38Z- LcmTrn::publishEstimates() - COVAR: 709.15 833.00 1.17 0.00 2021-02-13T00:18:38Z- AHRS_M2 msg: 1613175518.99 epoch sec; seqNo:11395 2021-02-13T00:18:38Z- AHRS_M2 msg: 0.70 phi; -0.30 theta; -0.16 psi 2021-02-13T00:18:39Z- handleDepth()-Depth_Keller msg: 1613175519001 epoch sec; seqNo:11490; depth 12.13 2021-02-13T00:18:39Z- handleDvl() - RDI_Pathfinder msg: 1613175519.01 epoch sec; seqNo:5723 2021-02-13T00:18:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 98.93 , 78.65 , 111.10 2021-02-13T00:18:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , -0.04 2021-02-13T00:18:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175519.02 epoch sec; seqNo:11490; 4058851.88 north; 592333.88 east 2021-02-13T00:18:39Z- AHRS_M2 msg: 1613175519.40 epoch sec; seqNo:11396 2021-02-13T00:18:39Z- AHRS_M2 msg: 0.70 phi; -0.29 theta; -0.17 psi 2021-02-13T00:18:39Z- handleDepth()-Depth_Keller msg: 1613175519402 epoch sec; seqNo:11491; depth 12.28 2021-02-13T00:18:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175519.41 epoch sec; seqNo:11491; 4058852.27 north; 592335.10 east 2021-02-13T00:18:39Z- AHRS_M2 msg: 1613175519.80 epoch sec; seqNo:11397 2021-02-13T00:18:39Z- AHRS_M2 msg: 0.70 phi; -0.29 theta; -0.17 psi 2021-02-13T00:18:39Z- handleDepth()-Depth_Keller msg: 1613175519817 epoch sec; seqNo:11492; depth 12.38 2021-02-13T00:18:39Z- handleDvl() - RDI_Pathfinder msg: 1613175519.83 epoch sec; seqNo:5724 2021-02-13T00:18:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 98.93 , 78.65 , 111.10 2021-02-13T00:18:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , -0.06 2021-02-13T00:18:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175519.84 epoch sec; seqNo:11492; 4058852.65 north; 592335.09 east 2021-02-13T00:18:40Z- AHRS_M2 msg: 1613175520.20 epoch sec; seqNo:11398 2021-02-13T00:18:40Z- AHRS_M2 msg: 0.69 phi; -0.29 theta; -0.18 psi 2021-02-13T00:18:40Z- handleDepth()-Depth_Keller msg: 1613175520208 epoch sec; seqNo:11493; depth 12.51 2021-02-13T00:18:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175520.22 epoch sec; seqNo:11493; 4058852.65 north; 592335.09 east 2021-02-13T00:18:40Z- AHRS_M2 msg: 1613175520.62 epoch sec; seqNo:11399 2021-02-13T00:18:40Z- AHRS_M2 msg: 0.68 phi; -0.29 theta; -0.19 psi 2021-02-13T00:18:40Z- handleDepth()-Depth_Keller msg: 1613175520620 epoch sec; seqNo:11494; depth 12.59 2021-02-13T00:18:40Z- handleDvl() - RDI_Pathfinder msg: 1613175520.63 epoch sec; seqNo:5725 2021-02-13T00:18:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.71 , 78.71 , 109.82 2021-02-13T00:18:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.12 , 0.03 2021-02-13T00:18:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175520.64 epoch sec; seqNo:11494; 4058853.03 north; 592335.09 east 2021-02-13T00:18:41Z- AHRS_M2 msg: 1613175521.01 epoch sec; seqNo:11400 2021-02-13T00:18:41Z- AHRS_M2 msg: 0.67 phi; -0.28 theta; -0.20 psi 2021-02-13T00:18:41Z- handleDepth()-Depth_Keller msg: 1613175521016 epoch sec; seqNo:11495; depth 12.72 2021-02-13T00:18:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175521.03 epoch sec; seqNo:11495; 4058853.41 north; 592335.08 east 2021-02-13T00:18:41Z- AHRS_M2 msg: 1613175521.41 epoch sec; seqNo:11401 2021-02-13T00:18:41Z- AHRS_M2 msg: 0.66 phi; -0.28 theta; -0.20 psi 2021-02-13T00:18:41Z- handleDepth()-Depth_Keller msg: 1613175521425 epoch sec; seqNo:11496; depth 12.82 2021-02-13T00:18:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175521.43 epoch sec; seqNo:11496; 4058853.79 north; 592335.08 east 2021-02-13T00:18:41Z- AHRS_M2 msg: 1613175521.82 epoch sec; seqNo:11402 2021-02-13T00:18:41Z- AHRS_M2 msg: 0.64 phi; -0.28 theta; -0.21 psi 2021-02-13T00:18:41Z- handleDepth()-Depth_Keller msg: 1613175521824 epoch sec; seqNo:11497; depth 12.93 2021-02-13T00:18:41Z- handleDvl() - RDI_Pathfinder msg: 1613175521.84 epoch sec; seqNo:5726 2021-02-13T00:18:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.36 , 78.37 , 106.77 2021-02-13T00:18:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.10 , 0.14 2021-02-13T00:18:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175521.85 epoch sec; seqNo:11497; 4058853.79 north; 592335.08 east 2021-02-13T00:18:42Z- AHRS_M2 msg: 1613175522.23 epoch sec; seqNo:11403 2021-02-13T00:18:42Z- AHRS_M2 msg: 0.63 phi; -0.28 theta; -0.21 psi 2021-02-13T00:18:42Z- handleDepth()-Depth_Keller msg: 1613175522230 epoch sec; seqNo:11498; depth 13.02 2021-02-13T00:18:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175522.24 epoch sec; seqNo:11498; 4058854.18 north; 592336.30 east 2021-02-13T00:18:42Z- AHRS_M2 msg: 1613175522.63 epoch sec; seqNo:11404 2021-02-13T00:18:42Z- AHRS_M2 msg: 0.62 phi; -0.28 theta; -0.23 psi 2021-02-13T00:18:42Z- handleDepth()-Depth_Keller msg: 1613175522654 epoch sec; seqNo:11499; depth 13.12 2021-02-13T00:18:42Z- handleDvl() - RDI_Pathfinder msg: 1613175522.68 epoch sec; seqNo:5727 2021-02-13T00:18:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.57 , 101.50 , 78.51 , 109.61 2021-02-13T00:18:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , 0.15 2021-02-13T00:18:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175522.70 epoch sec; seqNo:11499; 4058854.56 north; 592336.29 east 2021-02-13T00:18:43Z- AHRS_M2 msg: 1613175523.03 epoch sec; seqNo:11405 2021-02-13T00:18:43Z- AHRS_M2 msg: 0.61 phi; -0.28 theta; -0.24 psi 2021-02-13T00:18:43Z- handleDepth()-Depth_Keller msg: 1613175523036 epoch sec; seqNo:11500; depth 13.24 2021-02-13T00:18:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175523.05 epoch sec; seqNo:11500; 4058854.56 north; 592336.29 east 2021-02-13T00:18:43Z- AHRS_M2 msg: 1613175523.44 epoch sec; seqNo:11406 2021-02-13T00:18:43Z- AHRS_M2 msg: 0.61 phi; -0.28 theta; -0.26 psi 2021-02-13T00:18:43Z- handleDepth()-Depth_Keller msg: 1613175523440 epoch sec; seqNo:11501; depth 13.33 2021-02-13T00:18:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175523.45 epoch sec; seqNo:11501; 4058854.94 north; 592336.29 east 2021-02-13T00:18:43Z- LcmTrn::handleMessages() - handling 41 messages took 0.68 ms and 128 calls... 2021-02-13T00:18:43Z- waiting for 1613175521.60, time is 1613175523.45 2021-02-13T00:18:43Z- LcmTrn::updateTrn() >>>> pose time 1613175523.45, meas time 1613175522.68 <<<< 2021-02-13T00:18:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175522.68 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:18:43Z- TerrainNav::motionUpdate() - currEstimate = (4058855, 592336.29, 13.33) 2021-02-13T00:18:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:18:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:18:43Z- TNavBF::Projecting Measurements 2021-02-13T00:18:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:18:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:18:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:18:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:18:43Z- TNavFilter::4 good beams. 2021-02-13T00:18:43Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:18:43Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:18:43Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:18:43Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:18:43Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:18:43Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:18:43Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:18:43Z- TNavBF::mmse selected from filter -1 2021-02-13T00:18:43Z- TerrainMapOctree: (vehN, vehE ) = (4058854.39, 592335.91). 2021-02-13T00:18:43Z- TerrainMapOctree: Min Distance = 3066.38. 2021-02-13T00:18:43Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:18:43Z- There is no specific good beam that all particles have in common. 2021-02-13T00:18:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:18:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.35 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 10.91 2021-02-13T00:18:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:18:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 16.48 2021-02-13T00:18:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:18:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:18:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:18:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 19.22 2021-02-13T00:18:43Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:18:43Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:18:43Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:18:43Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:18:43Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:18:43Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:18:43Z- Filter 0 No beams used in subcloud update 2021-02-13T00:18:43Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:18:43Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:18:43Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 709.1 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:18:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175522.68 sec. 2021-02-13T00:18:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:18:43Z- TNavBF::mmse selected from filter -1 2021-02-13T00:18:43Z- variances([0], [2], and psi_berg) = 709.068707 833.002945 0.000000 2021-02-13T00:18:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:18:43Z- LcmTrn::publishEstimates() - MLE : 4058854.44 592382.79 13.33 2021-02-13T00:18:43Z- LcmTrn::publishEstimates() - MMSE : 4058854.94 592336.29 13.33 2021-02-13T00:18:43Z- LcmTrn::publishEstimates() - COVAR: 709.07 833.00 1.02 0.00 2021-02-13T00:18:43Z- AHRS_M2 msg: 1613175523.84 epoch sec; seqNo:11407 2021-02-13T00:18:43Z- AHRS_M2 msg: 0.61 phi; -0.28 theta; -0.26 psi 2021-02-13T00:18:43Z- handleDepth()-Depth_Keller msg: 1613175523846 epoch sec; seqNo:11502; depth 13.44 2021-02-13T00:18:43Z- handleDvl() - RDI_Pathfinder msg: 1613175523.86 epoch sec; seqNo:5728 2021-02-13T00:18:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.42 , 104.41 , 78.71 , 111.17 2021-02-13T00:18:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.08 , 0.04 2021-02-13T00:18:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175523.87 epoch sec; seqNo:11502; 4058855.32 north; 592336.28 east 2021-02-13T00:18:44Z- AHRS_M2 msg: 1613175524.24 epoch sec; seqNo:11408 2021-02-13T00:18:44Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.26 psi 2021-02-13T00:18:44Z- handleDepth()-Depth_Keller msg: 1613175524248 epoch sec; seqNo:11503; depth 13.56 2021-02-13T00:18:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175524.27 epoch sec; seqNo:11503; 4058855.70 north; 592336.28 east 2021-02-13T00:18:44Z- AHRS_M2 msg: 1613175524.65 epoch sec; seqNo:11409 2021-02-13T00:18:44Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.25 psi 2021-02-13T00:18:44Z- handleDepth()-Depth_Keller msg: 1613175524657 epoch sec; seqNo:11504; depth 13.65 2021-02-13T00:18:44Z- handleDvl() - RDI_Pathfinder msg: 1613175524.67 epoch sec; seqNo:5729 2021-02-13T00:18:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.00 , 107.05 , 78.07 , 111.10 2021-02-13T00:18:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.03 , -0.05 2021-02-13T00:18:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175524.68 epoch sec; seqNo:11504; 4058855.71 north; 592337.50 east 2021-02-13T00:18:45Z- AHRS_M2 msg: 1613175525.05 epoch sec; seqNo:11410 2021-02-13T00:18:45Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.24 psi 2021-02-13T00:18:45Z- handleDepth()-Depth_Keller msg: 1613175525057 epoch sec; seqNo:11505; depth 13.76 2021-02-13T00:18:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175525.07 epoch sec; seqNo:11505; 4058856.09 north; 592337.50 east 2021-02-13T00:18:45Z- AHRS_M2 msg: 1613175525.46 epoch sec; seqNo:11411 2021-02-13T00:18:45Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.22 psi 2021-02-13T00:18:45Z- handleDepth()-Depth_Keller msg: 1613175525472 epoch sec; seqNo:11506; depth 13.87 2021-02-13T00:18:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175525.49 epoch sec; seqNo:11506; 4058856.47 north; 592337.49 east 2021-02-13T00:18:45Z- AHRS_M2 msg: 1613175525.86 epoch sec; seqNo:11412 2021-02-13T00:18:45Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.21 psi 2021-02-13T00:18:45Z- handleDepth()-Depth_Keller msg: 1613175525869 epoch sec; seqNo:11507; depth 13.98 2021-02-13T00:18:45Z- handleDvl() - RDI_Pathfinder msg: 1613175525.88 epoch sec; seqNo:5730 2021-02-13T00:18:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.35 , 104.34 , 78.65 , 108.40 2021-02-13T00:18:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , -0.06 2021-02-13T00:18:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175525.89 epoch sec; seqNo:11507; 4058856.84 north; 592337.49 east 2021-02-13T00:18:46Z- AHRS_M2 msg: 1613175526.28 epoch sec; seqNo:11413 2021-02-13T00:18:46Z- AHRS_M2 msg: 0.64 phi; -0.29 theta; -0.20 psi 2021-02-13T00:18:46Z- handleDepth()-Depth_Keller msg: 1613175526286 epoch sec; seqNo:11508; depth 14.09 2021-02-13T00:18:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175526.30 epoch sec; seqNo:11508; 4058857.22 north; 592337.48 east 2021-02-13T00:18:46Z- AHRS_M2 msg: 1613175526.67 epoch sec; seqNo:11414 2021-02-13T00:18:46Z- AHRS_M2 msg: 0.64 phi; -0.29 theta; -0.18 psi 2021-02-13T00:18:46Z- handleDepth()-Depth_Keller msg: 1613175526672 epoch sec; seqNo:11509; depth 14.21 2021-02-13T00:18:46Z- handleDvl() - RDI_Pathfinder msg: 1613175526.69 epoch sec; seqNo:5731 2021-02-13T00:18:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.42 , 100.35 , 75.21 , 107.11 2021-02-13T00:18:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , -0.02 2021-02-13T00:18:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175526.70 epoch sec; seqNo:11509; 4058857.22 north; 592337.48 east 2021-02-13T00:18:47Z- AHRS_M2 msg: 1613175527.07 epoch sec; seqNo:11415 2021-02-13T00:18:47Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.17 psi 2021-02-13T00:18:47Z- handleDepth()-Depth_Keller msg: 1613175527076 epoch sec; seqNo:11510; depth 14.30 2021-02-13T00:18:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175527.09 epoch sec; seqNo:11510; 4058857.62 north; 592338.70 east 2021-02-13T00:18:47Z- AHRS_M2 msg: 1613175527.47 epoch sec; seqNo:11416 2021-02-13T00:18:47Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.15 psi 2021-02-13T00:18:47Z- handleDepth()-Depth_Keller msg: 1613175527485 epoch sec; seqNo:11511; depth 14.42 2021-02-13T00:18:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175527.49 epoch sec; seqNo:11511; 4058857.99 north; 592338.70 east 2021-02-13T00:18:47Z- AHRS_M2 msg: 1613175527.88 epoch sec; seqNo:11417 2021-02-13T00:18:47Z- AHRS_M2 msg: 0.66 phi; -0.30 theta; -0.14 psi 2021-02-13T00:18:47Z- handleDepth()-Depth_Keller msg: 1613175527885 epoch sec; seqNo:11512; depth 14.51 2021-02-13T00:18:47Z- handleDvl() - RDI_Pathfinder msg: 1613175527.90 epoch sec; seqNo:5732 2021-02-13T00:18:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 97.65 , 78.17 , 108.60 2021-02-13T00:18:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , -0.00 2021-02-13T00:18:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175527.91 epoch sec; seqNo:11512; 4058858.37 north; 592338.69 east 2021-02-13T00:18:48Z- AHRS_M2 msg: 1613175528.28 epoch sec; seqNo:11418 2021-02-13T00:18:48Z- AHRS_M2 msg: 0.66 phi; -0.30 theta; -0.13 psi 2021-02-13T00:18:48Z- handleDepth()-Depth_Keller msg: 1613175528290 epoch sec; seqNo:11513; depth 14.64 2021-02-13T00:18:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175528.30 epoch sec; seqNo:11513; 4058858.37 north; 592338.69 east 2021-02-13T00:18:48Z- AHRS_M2 msg: 1613175528.69 epoch sec; seqNo:11419 2021-02-13T00:18:48Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.12 psi 2021-02-13T00:18:48Z- handleDepth()-Depth_Keller msg: 1613175528697 epoch sec; seqNo:11514; depth 14.75 2021-02-13T00:18:48Z- handleDvl() - RDI_Pathfinder msg: 1613175528.71 epoch sec; seqNo:5733 2021-02-13T00:18:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 95.01 , 74.39 , 107.18 2021-02-13T00:18:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.00 , -0.04 2021-02-13T00:18:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175528.72 epoch sec; seqNo:11514; 4058858.75 north; 592338.69 east 2021-02-13T00:18:48Z- LcmTrn::handleMessages() - handling 45 messages took 0.64 ms and 132 calls... 2021-02-13T00:18:48Z- waiting for 1613175526.45, time is 1613175528.72 2021-02-13T00:18:48Z- LcmTrn::updateTrn() equating pose 1613175528.72 and meas times 1613175528.72 2021-02-13T00:18:48Z- LcmTrn::updateTrn() >>>> pose time 1613175528.72, meas time 1613175528.72 <<<< 2021-02-13T00:18:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175528.72 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:18:48Z- TerrainNav::motionUpdate() - currEstimate = (4058859, 592338.69, 14.75) 2021-02-13T00:18:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:18:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:18:48Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175528.72 sec.; No relevant navigation data available 2021-02-13T00:18:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:18:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:18:48Z- variances([0], [2], and psi_berg) = 709.005327 833.002908 0.000000 2021-02-13T00:18:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:18:48Z- LcmTrn::publishEstimates() - MLE : 4058858.25 592385.19 14.75 2021-02-13T00:18:48Z- LcmTrn::publishEstimates() - MMSE : 4058858.75 592338.69 14.75 2021-02-13T00:18:48Z- LcmTrn::publishEstimates() - COVAR: 709.01 833.00 1.25 0.00 2021-02-13T00:18:49Z- AHRS_M2 msg: 1613175529.09 epoch sec; seqNo:11420 2021-02-13T00:18:49Z- AHRS_M2 msg: 0.68 phi; -0.31 theta; -0.12 psi 2021-02-13T00:18:49Z- handleDepth()-Depth_Keller msg: 1613175529096 epoch sec; seqNo:11515; depth 14.85 2021-02-13T00:18:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175529.11 epoch sec; seqNo:11515; 4058859.13 north; 592338.68 east 2021-02-13T00:18:49Z- AHRS_M2 msg: 1613175529.50 epoch sec; seqNo:11421 2021-02-13T00:18:49Z- AHRS_M2 msg: 0.68 phi; -0.31 theta; -0.12 psi 2021-02-13T00:18:49Z- handleDepth()-Depth_Keller msg: 1613175529505 epoch sec; seqNo:11516; depth 14.97 2021-02-13T00:18:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175529.52 epoch sec; seqNo:11516; 4058859.52 north; 592339.90 east 2021-02-13T00:18:49Z- AHRS_M2 msg: 1613175529.90 epoch sec; seqNo:11422 2021-02-13T00:18:49Z- AHRS_M2 msg: 0.69 phi; -0.31 theta; -0.12 psi 2021-02-13T00:18:49Z- handleDepth()-Depth_Keller msg: 1613175529905 epoch sec; seqNo:11517; depth 15.07 2021-02-13T00:18:49Z- handleDvl() - RDI_Pathfinder msg: 1613175529.92 epoch sec; seqNo:5734 2021-02-13T00:18:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 94.13 , 72.34 , 108.73 2021-02-13T00:18:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.05 , -0.09 2021-02-13T00:18:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175529.93 epoch sec; seqNo:11517; 4058859.52 north; 592339.90 east 2021-02-13T00:18:50Z- AHRS_M2 msg: 1613175530.30 epoch sec; seqNo:11423 2021-02-13T00:18:50Z- AHRS_M2 msg: 0.70 phi; -0.32 theta; -0.13 psi 2021-02-13T00:18:50Z- handleDepth()-Depth_Keller msg: 1613175530308 epoch sec; seqNo:11518; depth 15.20 2021-02-13T00:18:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175530.32 epoch sec; seqNo:11518; 4058859.90 north; 592339.90 east 2021-02-13T00:18:50Z- AHRS_M2 msg: 1613175530.71 epoch sec; seqNo:11424 2021-02-13T00:18:50Z- AHRS_M2 msg: 0.70 phi; -0.32 theta; -0.14 psi 2021-02-13T00:18:50Z- handleDepth()-Depth_Keller msg: 1613175530713 epoch sec; seqNo:11519; depth 15.31 2021-02-13T00:18:50Z- handleDvl() - RDI_Pathfinder msg: 1613175530.72 epoch sec; seqNo:5735 2021-02-13T00:18:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 97.71 , 78.78 , 107.18 2021-02-13T00:18:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.14 , -0.10 2021-02-13T00:18:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175530.73 epoch sec; seqNo:11519; 4058860.28 north; 592339.89 east 2021-02-13T00:18:51Z- AHRS_M2 msg: 1613175531.11 epoch sec; seqNo:11425 2021-02-13T00:18:51Z- AHRS_M2 msg: 0.70 phi; -0.32 theta; -0.15 psi 2021-02-13T00:18:51Z- handleDepth()-Depth_Keller msg: 1613175531125 epoch sec; seqNo:11520; depth 15.42 2021-02-13T00:18:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175531.14 epoch sec; seqNo:11520; 4058860.66 north; 592339.89 east 2021-02-13T00:18:51Z- AHRS_M2 msg: 1613175531.52 epoch sec; seqNo:11426 2021-02-13T00:18:51Z- AHRS_M2 msg: 0.70 phi; -0.32 theta; -0.16 psi 2021-02-13T00:18:51Z- handleDepth()-Depth_Keller msg: 1613175531520 epoch sec; seqNo:11521; depth 15.54 2021-02-13T00:18:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175531.54 epoch sec; seqNo:11521; 4058861.05 north; 592341.10 east 2021-02-13T00:18:51Z- AHRS_M2 msg: 1613175531.92 epoch sec; seqNo:11427 2021-02-13T00:18:51Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.16 psi 2021-02-13T00:18:51Z- handleDepth()-Depth_Keller msg: 1613175531929 epoch sec; seqNo:11522; depth 15.65 2021-02-13T00:18:51Z- handleDvl() - RDI_Pathfinder msg: 1613175531.94 epoch sec; seqNo:5736 2021-02-13T00:18:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 97.64 , 73.56 , 107.38 2021-02-13T00:18:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.20 , -0.06 2021-02-13T00:18:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175531.96 epoch sec; seqNo:11522; 4058861.43 north; 592341.10 east 2021-02-13T00:18:52Z- AHRS_M2 msg: 1613175532.32 epoch sec; seqNo:11428 2021-02-13T00:18:52Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.17 psi 2021-02-13T00:18:52Z- handleDepth()-Depth_Keller msg: 1613175532328 epoch sec; seqNo:11523; depth 15.79 2021-02-13T00:18:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175532.34 epoch sec; seqNo:11523; 4058861.81 north; 592341.10 east 2021-02-13T00:18:52Z- AHRS_M2 msg: 1613175532.73 epoch sec; seqNo:11429 2021-02-13T00:18:52Z- AHRS_M2 msg: 0.70 phi; -0.34 theta; -0.17 psi 2021-02-13T00:18:52Z- handleDepth()-Depth_Keller msg: 1613175532741 epoch sec; seqNo:11524; depth 15.90 2021-02-13T00:18:52Z- handleDvl() - RDI_Pathfinder msg: 1613175532.76 epoch sec; seqNo:5737 2021-02-13T00:18:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 135.50 , 96.56 , 72.75 , 107.51 2021-02-13T00:18:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.31 , -0.06 2021-02-13T00:18:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175532.77 epoch sec; seqNo:11524; 4058862.19 north; 592341.09 east 2021-02-13T00:18:53Z- AHRS_M2 msg: 1613175533.13 epoch sec; seqNo:11430 2021-02-13T00:18:53Z- AHRS_M2 msg: 0.70 phi; -0.34 theta; -0.17 psi 2021-02-13T00:18:53Z- handleDepth()-Depth_Keller msg: 1613175533148 epoch sec; seqNo:11525; depth 16.06 2021-02-13T00:18:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175533.16 epoch sec; seqNo:11525; 4058862.19 north; 592341.09 east 2021-02-13T00:18:53Z- AHRS_M2 msg: 1613175533.54 epoch sec; seqNo:11431 2021-02-13T00:18:53Z- AHRS_M2 msg: 0.70 phi; -0.35 theta; -0.18 psi 2021-02-13T00:18:53Z- handleDepth()-Depth_Keller msg: 1613175533540 epoch sec; seqNo:11526; depth 16.16 2021-02-13T00:18:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175533.57 epoch sec; seqNo:11526; 4058862.57 north; 592341.09 east 2021-02-13T00:18:53Z- LcmTrn::handleMessages() - handling 40 messages took 0.63 ms and 128 calls... 2021-02-13T00:18:53Z- waiting for 1613175531.72, time is 1613175533.57 2021-02-13T00:18:53Z- LcmTrn::updateTrn() >>>> pose time 1613175533.57, meas time 1613175532.76 <<<< 2021-02-13T00:18:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175532.76 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:18:53Z- TerrainNav::motionUpdate() - currEstimate = (4058863, 592341.09, 16.16) 2021-02-13T00:18:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:18:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:18:53Z- TNavBF::Projecting Measurements 2021-02-13T00:18:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:18:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:18:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:18:53Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:18:53Z- TNavFilter::3 good beams. 2021-02-13T00:18:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:18:53Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:18:53Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:18:53Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:18:53Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:18:53Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:18:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:18:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:18:53Z- TerrainMapOctree: (vehN, vehE ) = (4058861.92, 592340.68). 2021-02-13T00:18:53Z- TerrainMapOctree: Min Distance = 3057.52. 2021-02-13T00:18:53Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:18:53Z- There is no specific good beam that all particles have in common. 2021-02-13T00:18:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:18:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.44 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 14.92 2021-02-13T00:18:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:18:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.115 ExpectedMeasDiff: nan currMeas.covariance: 8.47 2021-02-13T00:18:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:18:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 18.49 2021-02-13T00:18:53Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:18:53Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:18:53Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:18:53Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:18:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:18:53Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:18:53Z- Filter 0 No beams used in subcloud update 2021-02-13T00:18:53Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:18:53Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:18:53Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 708.9 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:18:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175532.76 sec. 2021-02-13T00:18:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:18:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:18:53Z- variances([0], [2], and psi_berg) = 708.921583 833.002860 0.000000 2021-02-13T00:18:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:18:53Z- LcmTrn::publishEstimates() - MLE : 4058862.07 592390.59 16.16 2021-02-13T00:18:53Z- LcmTrn::publishEstimates() - MMSE : 4058862.57 592341.09 16.16 2021-02-13T00:18:53Z- LcmTrn::publishEstimates() - COVAR: 708.92 833.00 1.15 0.00 2021-02-13T00:18:53Z- AHRS_M2 msg: 1613175533.95 epoch sec; seqNo:11432 2021-02-13T00:18:53Z- AHRS_M2 msg: 0.70 phi; -0.35 theta; -0.17 psi 2021-02-13T00:18:53Z- handleDepth()-Depth_Keller msg: 1613175533950 epoch sec; seqNo:11527; depth 16.30 2021-02-13T00:18:53Z- handleDvl() - RDI_Pathfinder msg: 1613175533.96 epoch sec; seqNo:5738 2021-02-13T00:18:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.49 , 97.85 , 78.51 , 111.24 2021-02-13T00:18:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.17 , 0.01 2021-02-13T00:18:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175533.97 epoch sec; seqNo:11527; 4058862.95 north; 592341.08 east 2021-02-13T00:18:54Z- AHRS_M2 msg: 1613175534.35 epoch sec; seqNo:11433 2021-02-13T00:18:54Z- AHRS_M2 msg: 0.70 phi; -0.35 theta; -0.17 psi 2021-02-13T00:18:54Z- handleDepth()-Depth_Keller msg: 1613175534354 epoch sec; seqNo:11528; depth 16.43 2021-02-13T00:18:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175534.37 epoch sec; seqNo:11528; 4058863.32 north; 592341.08 east 2021-02-13T00:18:54Z- AHRS_M2 msg: 1613175534.75 epoch sec; seqNo:11434 2021-02-13T00:18:54Z- AHRS_M2 msg: 0.70 phi; -0.35 theta; -0.18 psi 2021-02-13T00:18:54Z- handleDepth()-Depth_Keller msg: 1613175534757 epoch sec; seqNo:11529; depth 16.55 2021-02-13T00:18:54Z- handleDvl() - RDI_Pathfinder msg: 1613175534.78 epoch sec; seqNo:5739 2021-02-13T00:18:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.29 , 97.71 , 71.53 , 109.89 2021-02-13T00:18:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.15 , 0.06 2021-02-13T00:18:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175534.79 epoch sec; seqNo:11529; 4058863.32 north; 592341.08 east 2021-02-13T00:18:55Z- AHRS_M2 msg: 1613175535.15 epoch sec; seqNo:11435 2021-02-13T00:18:55Z- AHRS_M2 msg: 0.70 phi; -0.35 theta; -0.19 psi 2021-02-13T00:18:55Z- handleDepth()-Depth_Keller msg: 1613175535158 epoch sec; seqNo:11530; depth 16.69 2021-02-13T00:18:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175535.17 epoch sec; seqNo:11530; 4058863.72 north; 592342.30 east 2021-02-13T00:18:55Z- AHRS_M2 msg: 1613175535.56 epoch sec; seqNo:11436 2021-02-13T00:18:55Z- AHRS_M2 msg: 0.70 phi; -0.35 theta; -0.19 psi 2021-02-13T00:18:55Z- handleDepth()-Depth_Keller msg: 1613175535586 epoch sec; seqNo:11531; depth 16.81 2021-02-13T00:18:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175535.61 epoch sec; seqNo:11531; 4058864.10 north; 592342.29 east 2021-02-13T00:18:55Z- AHRS_M2 msg: 1613175535.96 epoch sec; seqNo:11437 2021-02-13T00:18:55Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.19 psi 2021-02-13T00:18:55Z- handleDepth()-Depth_Keller msg: 1613175535969 epoch sec; seqNo:11532; depth 16.95 2021-02-13T00:18:55Z- handleDvl() - RDI_Pathfinder msg: 1613175535.98 epoch sec; seqNo:5740 2021-02-13T00:18:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 97.78 , 71.94 , 108.74 2021-02-13T00:18:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.09 , 0.14 2021-02-13T00:18:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175536.00 epoch sec; seqNo:11532; 4058864.10 north; 592342.29 east 2021-02-13T00:18:56Z- AHRS_M2 msg: 1613175536.36 epoch sec; seqNo:11438 2021-02-13T00:18:56Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.20 psi 2021-02-13T00:18:56Z- handleDepth()-Depth_Keller msg: 1613175536368 epoch sec; seqNo:11533; depth 17.07 2021-02-13T00:18:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175536.38 epoch sec; seqNo:11533; 4058864.47 north; 592342.29 east 2021-02-13T00:18:56Z- AHRS_M2 msg: 1613175536.77 epoch sec; seqNo:11439 2021-02-13T00:18:56Z- AHRS_M2 msg: 0.69 phi; -0.36 theta; -0.20 psi 2021-02-13T00:18:56Z- handleDepth()-Depth_Keller msg: 1613175536773 epoch sec; seqNo:11534; depth 17.19 2021-02-13T00:18:56Z- handleDvl() - RDI_Pathfinder msg: 1613175536.79 epoch sec; seqNo:5741 2021-02-13T00:18:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 96.57 , 78.31 , 112.39 2021-02-13T00:18:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , 0.18 2021-02-13T00:18:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175536.80 epoch sec; seqNo:11534; 4058864.47 north; 592342.29 east 2021-02-13T00:18:57Z- AHRS_M2 msg: 1613175537.17 epoch sec; seqNo:11440 2021-02-13T00:18:57Z- AHRS_M2 msg: 0.69 phi; -0.36 theta; -0.21 psi 2021-02-13T00:18:57Z- handleDepth()-Depth_Keller msg: 1613175537181 epoch sec; seqNo:11535; depth 17.34 2021-02-13T00:18:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175537.19 epoch sec; seqNo:11535; 4058864.85 north; 592342.28 east 2021-02-13T00:18:57Z- AHRS_M2 msg: 1613175537.61 epoch sec; seqNo:11441 2021-02-13T00:18:57Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.21 psi 2021-02-13T00:18:57Z- handleDepth()-Depth_Keller msg: 1613175537615 epoch sec; seqNo:11536; depth 17.46 2021-02-13T00:18:57Z- handleDvl() - RDI_Pathfinder msg: 1613175537.63 epoch sec; seqNo:5742 2021-02-13T00:18:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.14 , 99.07 , 70.71 , 112.59 2021-02-13T00:18:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.06 , 0.10 2021-02-13T00:18:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175537.64 epoch sec; seqNo:11536; 4058865.23 north; 592342.28 east 2021-02-13T00:18:57Z- AHRS_M2 msg: 1613175537.98 epoch sec; seqNo:11442 2021-02-13T00:18:57Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.21 psi 2021-02-13T00:18:57Z- handleDepth()-Depth_Keller msg: 1613175537987 epoch sec; seqNo:11537; depth 17.61 2021-02-13T00:18:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175538.01 epoch sec; seqNo:11537; 4058865.25 north; 592343.50 east 2021-02-13T00:18:58Z- AHRS_M2 msg: 1613175538.40 epoch sec; seqNo:11443 2021-02-13T00:18:58Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.21 psi 2021-02-13T00:18:58Z- handleDepth()-Depth_Keller msg: 1613175538400 epoch sec; seqNo:11538; depth 17.74 2021-02-13T00:18:58Z- handleDvl() - RDI_Pathfinder msg: 1613175538.41 epoch sec; seqNo:5743 2021-02-13T00:18:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:18:58Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:18:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175538.43 epoch sec; seqNo:11538; 4058865.62 north; 592343.50 east 2021-02-13T00:18:58Z- AHRS_M2 msg: 1613175538.79 epoch sec; seqNo:11444 2021-02-13T00:18:58Z- AHRS_M2 msg: 0.71 phi; -0.38 theta; -0.20 psi 2021-02-13T00:18:58Z- handleDepth()-Depth_Keller msg: 1613175538792 epoch sec; seqNo:11539; depth 17.89 2021-02-13T00:18:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175538.80 epoch sec; seqNo:11539; 4058865.62 north; 592343.50 east 2021-02-13T00:18:59Z- LcmTrn::handleMessages() - handling 45 messages took 0.71 ms and 131 calls... 2021-02-13T00:18:59Z- waiting for 1613175536.57, time is 1613175538.80 2021-02-13T00:18:59Z- LcmTrn::updateTrn() >>>> pose time 1613175538.80, meas time 1613175538.41 <<<< 2021-02-13T00:18:59Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175538.41, ping # 0. 2021-02-13T00:18:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175538.41 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:18:59Z- TerrainNav::motionUpdate() - currEstimate = (4058866, 592343.50, 17.89) 2021-02-13T00:18:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:18:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:18:59Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:18:59Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:18:59Z- TNavBF::Projecting Measurements 2021-02-13T00:18:59Z- TNavFilter::Measurement from time = 1613175538.41 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:18:59Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:18:59Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:18:59Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:18:59Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:18:59Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:18:59Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:18:59Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:18:59Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 708.9 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:18:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:18:59Z- TNavBF::mmse selected from filter -1 2021-02-13T00:18:59Z- variances([0], [2], and psi_berg) = 708.848956 833.002818 0.000000 2021-02-13T00:18:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:18:59Z- LcmTrn::publishEstimates() - MLE : 4058865.12 592393.00 17.89 2021-02-13T00:18:59Z- LcmTrn::publishEstimates() - MMSE : 4058865.62 592343.50 17.89 2021-02-13T00:18:59Z- LcmTrn::publishEstimates() - COVAR: 708.85 833.00 1.35 0.00 2021-02-13T00:18:59Z- AHRS_M2 msg: 1613175539.19 epoch sec; seqNo:11445 2021-02-13T00:18:59Z- AHRS_M2 msg: 0.71 phi; -0.38 theta; -0.19 psi 2021-02-13T00:18:59Z- handleDepth()-Depth_Keller msg: 1613175539198 epoch sec; seqNo:11540; depth 18.00 2021-02-13T00:18:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175539.21 epoch sec; seqNo:11540; 4058866.00 north; 592343.49 east 2021-02-13T00:18:59Z- AHRS_M2 msg: 1613175539.60 epoch sec; seqNo:11446 2021-02-13T00:18:59Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.18 psi 2021-02-13T00:18:59Z- handleDepth()-Depth_Keller msg: 1613175539605 epoch sec; seqNo:11541; depth 18.15 2021-02-13T00:18:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175539.63 epoch sec; seqNo:11541; 4058866.38 north; 592343.49 east 2021-02-13T00:19:00Z- AHRS_M2 msg: 1613175540.00 epoch sec; seqNo:11447 2021-02-13T00:19:00Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.18 psi 2021-02-13T00:19:00Z- handleDepth()-Depth_Keller msg: 1613175540004 epoch sec; seqNo:11542; depth 18.30 2021-02-13T00:19:00Z- handleDvl() - RDI_Pathfinder msg: 1613175540.02 epoch sec; seqNo:5744 2021-02-13T00:19:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 97.71 , 78.24 , 114.75 2021-02-13T00:19:00Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.07 , -0.12 2021-02-13T00:19:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175540.04 epoch sec; seqNo:11542; 4058866.39 north; 592344.71 east 2021-02-13T00:19:00Z- AHRS_M2 msg: 1613175540.40 epoch sec; seqNo:11448 2021-02-13T00:19:00Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.18 psi 2021-02-13T00:19:00Z- handleDepth()-Depth_Keller msg: 1613175540408 epoch sec; seqNo:11543; depth 18.43 2021-02-13T00:19:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:19:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:19:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:19:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:19:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:19:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:19:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:19:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:19:00Z- handleDvl() - RDI_Pathfinder msg: 1613175540.41 epoch sec; seqNo:5745 2021-02-13T00:19:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:19:00Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.07 , -0.12 2021-02-13T00:19:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175540.42 epoch sec; seqNo:11543; 4058866.77 north; 592344.71 east 2021-02-13T00:19:00Z- AHRS_M2 msg: 1613175540.81 epoch sec; seqNo:11449 2021-02-13T00:19:00Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.19 psi 2021-02-13T00:19:00Z- handleDepth()-Depth_Keller msg: 1613175540814 epoch sec; seqNo:11544; depth 18.56 2021-02-13T00:19:00Z- handleDvl() - RDI_Pathfinder msg: 1613175540.83 epoch sec; seqNo:5746 2021-02-13T00:19:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.71 , 97.65 , 71.12 , 113.88 2021-02-13T00:19:00Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.02 , -0.17 2021-02-13T00:19:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175540.84 epoch sec; seqNo:11544; 4058867.15 north; 592344.70 east 2021-02-13T00:19:01Z- AHRS_M2 msg: 1613175541.21 epoch sec; seqNo:11450 2021-02-13T00:19:01Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.19 psi 2021-02-13T00:19:01Z- handleDepth()-Depth_Keller msg: 1613175541219 epoch sec; seqNo:11545; depth 18.70 2021-02-13T00:19:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175541.25 epoch sec; seqNo:11545; 4058867.53 north; 592344.70 east 2021-02-13T00:19:01Z- AHRS_M2 msg: 1613175541.62 epoch sec; seqNo:11451 2021-02-13T00:19:01Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.20 psi 2021-02-13T00:19:01Z- handleDepth()-Depth_Keller msg: 1613175541625 epoch sec; seqNo:11546; depth 18.86 2021-02-13T00:19:01Z- handleDvl() - RDI_Pathfinder msg: 1613175541.64 epoch sec; seqNo:5747 2021-02-13T00:19:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.66 , 96.50 , 68.67 , 113.54 2021-02-13T00:19:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.13 , -0.12 2021-02-13T00:19:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175541.66 epoch sec; seqNo:11546; 4058867.91 north; 592344.69 east 2021-02-13T00:19:02Z- AHRS_M2 msg: 1613175542.02 epoch sec; seqNo:11452 2021-02-13T00:19:02Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.20 psi 2021-02-13T00:19:02Z- handleDepth()-Depth_Keller msg: 1613175542029 epoch sec; seqNo:11547; depth 19.02 2021-02-13T00:19:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175542.04 epoch sec; seqNo:11547; 4058867.92 north; 592345.91 east 2021-02-13T00:19:02Z- AHRS_M2 msg: 1613175542.42 epoch sec; seqNo:11453 2021-02-13T00:19:02Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.20 psi 2021-02-13T00:19:02Z- handleDepth()-Depth_Keller msg: 1613175542430 epoch sec; seqNo:11548; depth 19.15 2021-02-13T00:19:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175542.44 epoch sec; seqNo:11548; 4058868.30 north; 592345.91 east 2021-02-13T00:19:02Z- AHRS_M2 msg: 1613175542.83 epoch sec; seqNo:11454 2021-02-13T00:19:02Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.20 psi 2021-02-13T00:19:02Z- handleDepth()-Depth_Keller msg: 1613175542837 epoch sec; seqNo:11549; depth 19.29 2021-02-13T00:19:02Z- handleDvl() - RDI_Pathfinder msg: 1613175542.86 epoch sec; seqNo:5748 2021-02-13T00:19:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 96.63 , 78.38 , 110.02 2021-02-13T00:19:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.13 , -0.04 2021-02-13T00:19:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175542.87 epoch sec; seqNo:11549; 4058868.68 north; 592345.91 east 2021-02-13T00:19:03Z- AHRS_M2 msg: 1613175543.23 epoch sec; seqNo:11455 2021-02-13T00:19:03Z- AHRS_M2 msg: 0.73 phi; -0.37 theta; -0.19 psi 2021-02-13T00:19:03Z- handleDepth()-Depth_Keller msg: 1613175543241 epoch sec; seqNo:11550; depth 19.43 2021-02-13T00:19:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175543.25 epoch sec; seqNo:11550; 4058869.06 north; 592345.90 east 2021-02-13T00:19:03Z- AHRS_M2 msg: 1613175543.65 epoch sec; seqNo:11456 2021-02-13T00:19:03Z- AHRS_M2 msg: 0.73 phi; -0.37 theta; -0.19 psi 2021-02-13T00:19:03Z- handleDepth()-Depth_Keller msg: 1613175543651 epoch sec; seqNo:11551; depth 19.56 2021-02-13T00:19:03Z- handleDvl() - RDI_Pathfinder msg: 1613175543.66 epoch sec; seqNo:5749 2021-02-13T00:19:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.02 , 97.71 , 78.24 , 109.89 2021-02-13T00:19:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.10 , 0.00 2021-02-13T00:19:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175543.67 epoch sec; seqNo:11551; 4058869.44 north; 592345.90 east 2021-02-13T00:19:04Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-13T00:19:04Z- waiting for 1613175541.80, time is 1613175543.67 2021-02-13T00:19:04Z- LcmTrn::updateTrn() equating pose 1613175543.67 and meas times 1613175543.67 2021-02-13T00:19:04Z- LcmTrn::updateTrn() >>>> pose time 1613175543.67, meas time 1613175543.67 <<<< 2021-02-13T00:19:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175543.67 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:19:04Z- TerrainNav::motionUpdate() - currEstimate = (4058869, 592345.90, 19.56) 2021-02-13T00:19:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:19:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:19:04Z- TNavBF::Projecting Measurements 2021-02-13T00:19:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:19:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:19:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:19:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:19:04Z- TNavFilter::4 good beams. 2021-02-13T00:19:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:19:04Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:19:04Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:19:04Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:19:04Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:19:04Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:19:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:19:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:19:04Z- TerrainMapOctree: (vehN, vehE ) = (4058869.44, 592345.90). 2021-02-13T00:19:04Z- TerrainMapOctree: Min Distance = 3048.39. 2021-02-13T00:19:04Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:19:04Z- There is no specific good beam that all particles have in common. 2021-02-13T00:19:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:19:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.58 K: 0.126 ExpectedMeasDiff: nan currMeas.covariance: 9.25 2021-02-13T00:19:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:19:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:19:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:19:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 9.79 2021-02-13T00:19:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:19:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 19.32 2021-02-13T00:19:04Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:19:04Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:19:04Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:19:04Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:19:04Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:19:04Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:19:04Z- Filter 0 No beams used in subcloud update 2021-02-13T00:19:04Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:19:04Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:19:04Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 708.8 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:19:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175543.67 sec. 2021-02-13T00:19:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:19:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:19:04Z- variances([0], [2], and psi_berg) = 708.785729 833.002781 0.000000 2021-02-13T00:19:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:19:04Z- LcmTrn::publishEstimates() - MLE : 4058868.94 592395.40 19.56 2021-02-13T00:19:04Z- LcmTrn::publishEstimates() - MMSE : 4058869.44 592345.90 19.56 2021-02-13T00:19:04Z- LcmTrn::publishEstimates() - COVAR: 708.79 833.00 1.09 0.00 2021-02-13T00:19:04Z- AHRS_M2 msg: 1613175544.04 epoch sec; seqNo:11457 2021-02-13T00:19:04Z- AHRS_M2 msg: 0.73 phi; -0.37 theta; -0.19 psi 2021-02-13T00:19:04Z- handleDepth()-Depth_Keller msg: 1613175544057 epoch sec; seqNo:11552; depth 19.70 2021-02-13T00:19:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:19:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:19:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:19:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:19:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:19:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:19:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:19:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:19:04Z- handleDvl() - RDI_Pathfinder msg: 1613175544.06 epoch sec; seqNo:5750 2021-02-13T00:19:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:19:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.10 , 0.00 2021-02-13T00:19:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175544.07 epoch sec; seqNo:11552; 4058869.44 north; 592345.90 east 2021-02-13T00:19:04Z- AHRS_M2 msg: 1613175544.44 epoch sec; seqNo:11458 2021-02-13T00:19:04Z- AHRS_M2 msg: 0.73 phi; -0.37 theta; -0.19 psi 2021-02-13T00:19:04Z- handleDepth()-Depth_Keller msg: 1613175544448 epoch sec; seqNo:11553; depth 19.85 2021-02-13T00:19:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175544.46 epoch sec; seqNo:11553; 4058869.83 north; 592347.11 east 2021-02-13T00:19:04Z- AHRS_M2 msg: 1613175544.85 epoch sec; seqNo:11459 2021-02-13T00:19:04Z- AHRS_M2 msg: 0.73 phi; -0.37 theta; -0.18 psi 2021-02-13T00:19:04Z- handleDepth()-Depth_Keller msg: 1613175544852 epoch sec; seqNo:11554; depth 19.98 2021-02-13T00:19:04Z- handleDvl() - RDI_Pathfinder msg: 1613175544.86 epoch sec; seqNo:5751 2021-02-13T00:19:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.02 , 95.28 , 69.07 , 109.89 2021-02-13T00:19:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.02 2021-02-13T00:19:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175544.87 epoch sec; seqNo:11554; 4058870.21 north; 592347.11 east 2021-02-13T00:19:05Z- AHRS_M2 msg: 1613175545.25 epoch sec; seqNo:11460 2021-02-13T00:19:05Z- AHRS_M2 msg: 0.73 phi; -0.37 theta; -0.18 psi 2021-02-13T00:19:05Z- handleDepth()-Depth_Keller msg: 1613175545265 epoch sec; seqNo:11555; depth 20.13 2021-02-13T00:19:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175545.28 epoch sec; seqNo:11555; 4058870.21 north; 592347.11 east 2021-02-13T00:19:05Z- AHRS_M2 msg: 1613175545.66 epoch sec; seqNo:11461 2021-02-13T00:19:05Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.19 psi 2021-02-13T00:19:05Z- handleDepth()-Depth_Keller msg: 1613175545661 epoch sec; seqNo:11556; depth 20.27 2021-02-13T00:19:05Z- handleDvl() - RDI_Pathfinder msg: 1613175545.67 epoch sec; seqNo:5752 2021-02-13T00:19:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 95.55 , 67.44 , 108.94 2021-02-13T00:19:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , -0.05 2021-02-13T00:19:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175545.69 epoch sec; seqNo:11556; 4058870.59 north; 592347.11 east 2021-02-13T00:19:06Z- AHRS_M2 msg: 1613175546.06 epoch sec; seqNo:11462 2021-02-13T00:19:06Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.19 psi 2021-02-13T00:19:06Z- handleDepth()-Depth_Keller msg: 1613175546064 epoch sec; seqNo:11557; depth 20.41 2021-02-13T00:19:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175546.08 epoch sec; seqNo:11557; 4058870.97 north; 592347.10 east 2021-02-13T00:19:06Z- AHRS_M2 msg: 1613175546.46 epoch sec; seqNo:11463 2021-02-13T00:19:06Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.20 psi 2021-02-13T00:19:06Z- handleDepth()-Depth_Keller msg: 1613175546469 epoch sec; seqNo:11558; depth 20.53 2021-02-13T00:19:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175546.48 epoch sec; seqNo:11558; 4058870.97 north; 592347.10 east 2021-02-13T00:19:06Z- AHRS_M2 msg: 1613175546.87 epoch sec; seqNo:11464 2021-02-13T00:19:06Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.20 psi 2021-02-13T00:19:06Z- handleDepth()-Depth_Keller msg: 1613175546877 epoch sec; seqNo:11559; depth 20.67 2021-02-13T00:19:06Z- handleDvl() - RDI_Pathfinder msg: 1613175546.90 epoch sec; seqNo:5753 2021-02-13T00:19:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.99 , 67.03 , 108.94 2021-02-13T00:19:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.06 , -0.06 2021-02-13T00:19:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175546.91 epoch sec; seqNo:11559; 4058871.35 north; 592347.10 east 2021-02-13T00:19:07Z- AHRS_M2 msg: 1613175547.27 epoch sec; seqNo:11465 2021-02-13T00:19:07Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.21 psi 2021-02-13T00:19:07Z- handleDepth()-Depth_Keller msg: 1613175547279 epoch sec; seqNo:11560; depth 20.82 2021-02-13T00:19:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175547.29 epoch sec; seqNo:11560; 4058871.74 north; 592348.31 east 2021-02-13T00:19:07Z- AHRS_M2 msg: 1613175547.67 epoch sec; seqNo:11466 2021-02-13T00:19:07Z- AHRS_M2 msg: 0.72 phi; -0.37 theta; -0.21 psi 2021-02-13T00:19:07Z- handleDepth()-Depth_Keller msg: 1613175547686 epoch sec; seqNo:11561; depth 20.94 2021-02-13T00:19:07Z- handleDvl() - RDI_Pathfinder msg: 1613175547.70 epoch sec; seqNo:5754 2021-02-13T00:19:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.17 , 96.43 , 0.00 , 103.73 2021-02-13T00:19:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.09 , -0.03 2021-02-13T00:19:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175547.71 epoch sec; seqNo:11561; 4058871.74 north; 592348.31 east 2021-02-13T00:19:08Z- AHRS_M2 msg: 1613175548.08 epoch sec; seqNo:11467 2021-02-13T00:19:08Z- AHRS_M2 msg: 0.71 phi; -0.37 theta; -0.21 psi 2021-02-13T00:19:08Z- handleDepth()-Depth_Keller msg: 1613175548089 epoch sec; seqNo:11562; depth 21.10 2021-02-13T00:19:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175548.11 epoch sec; seqNo:11562; 4058872.12 north; 592348.31 east 2021-02-13T00:19:08Z- AHRS_M2 msg: 1613175548.48 epoch sec; seqNo:11468 2021-02-13T00:19:08Z- AHRS_M2 msg: 0.71 phi; -0.37 theta; -0.22 psi 2021-02-13T00:19:08Z- handleDepth()-Depth_Keller msg: 1613175548493 epoch sec; seqNo:11563; depth 21.24 2021-02-13T00:19:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175548.50 epoch sec; seqNo:11563; 4058872.50 north; 592348.31 east 2021-02-13T00:19:08Z- AHRS_M2 msg: 1613175548.89 epoch sec; seqNo:11469 2021-02-13T00:19:08Z- AHRS_M2 msg: 0.71 phi; -0.37 theta; -0.22 psi 2021-02-13T00:19:08Z- handleDepth()-Depth_Keller msg: 1613175548893 epoch sec; seqNo:11564; depth 21.39 2021-02-13T00:19:08Z- handleDvl() - RDI_Pathfinder msg: 1613175548.90 epoch sec; seqNo:5755 2021-02-13T00:19:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 97.65 , 78.71 , 108.47 2021-02-13T00:19:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , 0.01 2021-02-13T00:19:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175548.91 epoch sec; seqNo:11564; 4058872.87 north; 592348.30 east 2021-02-13T00:19:09Z- LcmTrn::handleMessages() - handling 45 messages took 0.66 ms and 132 calls... 2021-02-13T00:19:09Z- waiting for 1613175546.67, time is 1613175548.91 2021-02-13T00:19:09Z- LcmTrn::updateTrn() equating pose 1613175548.91 and meas times 1613175548.91 2021-02-13T00:19:09Z- LcmTrn::updateTrn() >>>> pose time 1613175548.91, meas time 1613175548.91 <<<< 2021-02-13T00:19:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175548.91 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:19:09Z- TerrainNav::motionUpdate() - currEstimate = (4058873, 592348.30, 21.39) 2021-02-13T00:19:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:19:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:19:09Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175548.91 sec.; No relevant navigation data available 2021-02-13T00:19:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:19:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:19:09Z- variances([0], [2], and psi_berg) = 708.724782 833.002746 0.000000 2021-02-13T00:19:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:19:09Z- LcmTrn::publishEstimates() - MLE : 4058872.37 592397.80 21.39 2021-02-13T00:19:09Z- LcmTrn::publishEstimates() - MMSE : 4058872.87 592348.30 21.39 2021-02-13T00:19:09Z- LcmTrn::publishEstimates() - COVAR: 708.72 833.00 1.30 0.00 2021-02-13T00:19:09Z- AHRS_M2 msg: 1613175549.29 epoch sec; seqNo:11470 2021-02-13T00:19:09Z- AHRS_M2 msg: 0.71 phi; -0.37 theta; -0.22 psi 2021-02-13T00:19:09Z- handleDepth()-Depth_Keller msg: 1613175549311 epoch sec; seqNo:11565; depth 21.52 2021-02-13T00:19:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175549.32 epoch sec; seqNo:11565; 4058872.87 north; 592348.30 east 2021-02-13T00:19:09Z- AHRS_M2 msg: 1613175549.70 epoch sec; seqNo:11471 2021-02-13T00:19:09Z- AHRS_M2 msg: 0.71 phi; -0.37 theta; -0.21 psi 2021-02-13T00:19:09Z- handleDepth()-Depth_Keller msg: 1613175549700 epoch sec; seqNo:11566; depth 21.67 2021-02-13T00:19:09Z- handleDvl() - RDI_Pathfinder msg: 1613175549.71 epoch sec; seqNo:5756 2021-02-13T00:19:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 97.78 , 66.21 , 107.52 2021-02-13T00:19:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.08 , 0.06 2021-02-13T00:19:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175549.73 epoch sec; seqNo:11566; 4058873.27 north; 592349.52 east 2021-02-13T00:19:10Z- AHRS_M2 msg: 1613175550.10 epoch sec; seqNo:11472 2021-02-13T00:19:10Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.21 psi 2021-02-13T00:19:10Z- handleDepth()-Depth_Keller msg: 1613175550109 epoch sec; seqNo:11567; depth 21.82 2021-02-13T00:19:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175550.12 epoch sec; seqNo:11567; 4058873.65 north; 592349.52 east 2021-02-13T00:19:10Z- AHRS_M2 msg: 1613175550.51 epoch sec; seqNo:11473 2021-02-13T00:19:10Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.21 psi 2021-02-13T00:19:10Z- handleDepth()-Depth_Keller msg: 1613175550514 epoch sec; seqNo:11568; depth 21.96 2021-02-13T00:19:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175550.53 epoch sec; seqNo:11568; 4058873.65 north; 592349.52 east 2021-02-13T00:19:10Z- AHRS_M2 msg: 1613175550.91 epoch sec; seqNo:11474 2021-02-13T00:19:10Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.21 psi 2021-02-13T00:19:10Z- handleDepth()-Depth_Keller msg: 1613175550912 epoch sec; seqNo:11569; depth 22.11 2021-02-13T00:19:10Z- handleDvl() - RDI_Pathfinder msg: 1613175550.93 epoch sec; seqNo:5757 2021-02-13T00:19:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.99 , 67.85 , 108.94 2021-02-13T00:19:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.01 , 0.08 2021-02-13T00:19:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175550.94 epoch sec; seqNo:11569; 4058874.02 north; 592349.51 east 2021-02-13T00:19:11Z- AHRS_M2 msg: 1613175551.32 epoch sec; seqNo:11475 2021-02-13T00:19:11Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.20 psi 2021-02-13T00:19:11Z- handleDepth()-Depth_Keller msg: 1613175551322 epoch sec; seqNo:11570; depth 22.26 2021-02-13T00:19:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175551.33 epoch sec; seqNo:11570; 4058874.02 north; 592349.51 east 2021-02-13T00:19:11Z- AHRS_M2 msg: 1613175551.72 epoch sec; seqNo:11476 2021-02-13T00:19:11Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.20 psi 2021-02-13T00:19:11Z- handleDepth()-Depth_Keller msg: 1613175551723 epoch sec; seqNo:11571; depth 22.40 2021-02-13T00:19:11Z- handleDvl() - RDI_Pathfinder msg: 1613175551.73 epoch sec; seqNo:5758 2021-02-13T00:19:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.39 , 97.92 , 66.21 , 107.65 2021-02-13T00:19:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.05 , 0.07 2021-02-13T00:19:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175551.74 epoch sec; seqNo:11571; 4058874.40 north; 592349.51 east 2021-02-13T00:19:12Z- AHRS_M2 msg: 1613175552.12 epoch sec; seqNo:11477 2021-02-13T00:19:12Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.21 psi 2021-02-13T00:19:12Z- handleDepth()-Depth_Keller msg: 1613175552129 epoch sec; seqNo:11572; depth 22.55 2021-02-13T00:19:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175552.14 epoch sec; seqNo:11572; 4058874.80 north; 592350.72 east 2021-02-13T00:19:12Z- AHRS_M2 msg: 1613175552.55 epoch sec; seqNo:11478 2021-02-13T00:19:12Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.21 psi 2021-02-13T00:19:12Z- handleDepth()-Depth_Keller msg: 1613175552555 epoch sec; seqNo:11573; depth 22.68 2021-02-13T00:19:12Z- handleDvl() - RDI_Pathfinder msg: 1613175552.57 epoch sec; seqNo:5759 2021-02-13T00:19:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.05 , 65.40 , 106.57 2021-02-13T00:19:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.09 , 0.00 2021-02-13T00:19:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175552.58 epoch sec; seqNo:11573; 4058874.80 north; 592350.72 east 2021-02-13T00:19:12Z- AHRS_M2 msg: 1613175552.93 epoch sec; seqNo:11479 2021-02-13T00:19:12Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.21 psi 2021-02-13T00:19:12Z- handleDepth()-Depth_Keller msg: 1613175552932 epoch sec; seqNo:11574; depth 22.82 2021-02-13T00:19:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175552.94 epoch sec; seqNo:11574; 4058875.17 north; 592350.72 east 2021-02-13T00:19:13Z- AHRS_M2 msg: 1613175553.33 epoch sec; seqNo:11480 2021-02-13T00:19:13Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.22 psi 2021-02-13T00:19:13Z- handleDepth()-Depth_Keller msg: 1613175553338 epoch sec; seqNo:11575; depth 22.96 2021-02-13T00:19:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175553.35 epoch sec; seqNo:11575; 4058875.17 north; 592350.72 east 2021-02-13T00:19:13Z- AHRS_M2 msg: 1613175553.74 epoch sec; seqNo:11481 2021-02-13T00:19:13Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.23 psi 2021-02-13T00:19:13Z- handleDepth()-Depth_Keller msg: 1613175553740 epoch sec; seqNo:11576; depth 23.10 2021-02-13T00:19:13Z- handleDvl() - RDI_Pathfinder msg: 1613175553.76 epoch sec; seqNo:5760 2021-02-13T00:19:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 97.78 , 66.62 , 106.30 2021-02-13T00:19:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.07 , -0.06 2021-02-13T00:19:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175553.77 epoch sec; seqNo:11576; 4058875.55 north; 592350.72 east 2021-02-13T00:19:14Z- AHRS_M2 msg: 1613175554.14 epoch sec; seqNo:11482 2021-02-13T00:19:14Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.23 psi 2021-02-13T00:19:14Z- LcmTrn::handleMessages() - handling 42 messages took 0.66 ms and 129 calls... 2021-02-13T00:19:14Z- waiting for 1613175551.91, time is 1613175554.14 2021-02-13T00:19:14Z- LcmTrn::updateTrn() >>>> pose time 1613175554.14, meas time 1613175553.76 <<<< 2021-02-13T00:19:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175553.76 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:19:14Z- TerrainNav::motionUpdate() - currEstimate = (4058876, 592350.72, 23.10) 2021-02-13T00:19:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:19:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:19:14Z- TNavBF::Projecting Measurements 2021-02-13T00:19:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:19:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:19:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:19:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:19:14Z- TNavFilter::4 good beams. 2021-02-13T00:19:14Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:19:14Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:19:14Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:19:14Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:19:14Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:19:14Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:19:14Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:19:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:19:14Z- TerrainMapOctree: (vehN, vehE ) = (4058875.36, 592350.54). 2021-02-13T00:19:14Z- TerrainMapOctree: Min Distance = 3040.87. 2021-02-13T00:19:14Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:19:14Z- There is no specific good beam that all particles have in common. 2021-02-13T00:19:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:19:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.47 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 9.81 2021-02-13T00:19:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:19:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-13T00:19:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:19:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.121 ExpectedMeasDiff: nan currMeas.covariance: 7.10 2021-02-13T00:19:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:19:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 18.08 2021-02-13T00:19:14Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:19:14Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:19:14Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:19:14Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:19:14Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:19:14Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:19:14Z- Filter 0 No beams used in subcloud update 2021-02-13T00:19:14Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:19:14Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:19:14Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 708.7 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:19:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175553.76 sec. 2021-02-13T00:19:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:19:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:19:14Z- variances([0], [2], and psi_berg) = 708.655120 833.002706 0.000000 2021-02-13T00:19:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:19:14Z- LcmTrn::publishEstimates() - MLE : 4058875.05 592400.22 23.10 2021-02-13T00:19:14Z- LcmTrn::publishEstimates() - MMSE : 4058875.55 592350.72 23.10 2021-02-13T00:19:14Z- LcmTrn::publishEstimates() - COVAR: 708.66 833.00 1.03 0.00 2021-02-13T00:19:14Z- handleDepth()-Depth_Keller msg: 1613175554153 epoch sec; seqNo:11577; depth 23.24 2021-02-13T00:19:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175554.17 epoch sec; seqNo:11577; 4058875.94 north; 592351.93 east 2021-02-13T00:19:14Z- AHRS_M2 msg: 1613175554.56 epoch sec; seqNo:11483 2021-02-13T00:19:14Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.23 psi 2021-02-13T00:19:14Z- handleDepth()-Depth_Keller msg: 1613175554563 epoch sec; seqNo:11578; depth 23.37 2021-02-13T00:19:14Z- handleDvl() - RDI_Pathfinder msg: 1613175554.58 epoch sec; seqNo:5761 2021-02-13T00:19:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.35 , 95.55 , 67.03 , 106.51 2021-02-13T00:19:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , -0.09 2021-02-13T00:19:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175554.59 epoch sec; seqNo:11578; 4058876.32 north; 592351.93 east 2021-02-13T00:19:14Z- AHRS_M2 msg: 1613175554.95 epoch sec; seqNo:11484 2021-02-13T00:19:14Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.23 psi 2021-02-13T00:19:14Z- handleDepth()-Depth_Keller msg: 1613175554953 epoch sec; seqNo:11579; depth 23.51 2021-02-13T00:19:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175554.96 epoch sec; seqNo:11579; 4058876.32 north; 592351.93 east 2021-02-13T00:19:15Z- AHRS_M2 msg: 1613175555.35 epoch sec; seqNo:11485 2021-02-13T00:19:15Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.22 psi 2021-02-13T00:19:15Z- handleDepth()-Depth_Keller msg: 1613175555367 epoch sec; seqNo:11580; depth 23.66 2021-02-13T00:19:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175555.38 epoch sec; seqNo:11580; 4058876.70 north; 592351.92 east 2021-02-13T00:19:15Z- AHRS_M2 msg: 1613175555.76 epoch sec; seqNo:11486 2021-02-13T00:19:15Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.22 psi 2021-02-13T00:19:15Z- handleDepth()-Depth_Keller msg: 1613175555760 epoch sec; seqNo:11581; depth 23.79 2021-02-13T00:19:15Z- handleDvl() - RDI_Pathfinder msg: 1613175555.77 epoch sec; seqNo:5762 2021-02-13T00:19:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.71 , 98.19 , 66.62 , 104.27 2021-02-13T00:19:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , -0.10 2021-02-13T00:19:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175555.79 epoch sec; seqNo:11581; 4058877.08 north; 592351.92 east 2021-02-13T00:19:16Z- AHRS_M2 msg: 1613175556.16 epoch sec; seqNo:11487 2021-02-13T00:19:16Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.21 psi 2021-02-13T00:19:16Z- handleDepth()-Depth_Keller msg: 1613175556165 epoch sec; seqNo:11582; depth 23.94 2021-02-13T00:19:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:19:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:19:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:19:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:19:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:19:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:19:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:19:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:19:16Z- handleDvl() - RDI_Pathfinder msg: 1613175556.17 epoch sec; seqNo:5763 2021-02-13T00:19:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:19:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , -0.10 2021-02-13T00:19:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175556.18 epoch sec; seqNo:11582; 4058877.46 north; 592351.92 east 2021-02-13T00:19:16Z- AHRS_M2 msg: 1613175556.57 epoch sec; seqNo:11488 2021-02-13T00:19:16Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.20 psi 2021-02-13T00:19:16Z- handleDepth()-Depth_Keller msg: 1613175556573 epoch sec; seqNo:11583; depth 24.07 2021-02-13T00:19:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175556.58 epoch sec; seqNo:11583; 4058877.85 north; 592353.13 east 2021-02-13T00:19:16Z- AHRS_M2 msg: 1613175556.97 epoch sec; seqNo:11489 2021-02-13T00:19:16Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.20 psi 2021-02-13T00:19:16Z- handleDepth()-Depth_Keller msg: 1613175556972 epoch sec; seqNo:11584; depth 24.20 2021-02-13T00:19:16Z- handleDvl() - RDI_Pathfinder msg: 1613175556.99 epoch sec; seqNo:5764 2021-02-13T00:19:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 91.97 , 0.00 , 102.92 2021-02-13T00:19:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , -0.09 2021-02-13T00:19:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175557.00 epoch sec; seqNo:11584; 4058877.85 north; 592353.13 east 2021-02-13T00:19:17Z- AHRS_M2 msg: 1613175557.37 epoch sec; seqNo:11490 2021-02-13T00:19:17Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-13T00:19:17Z- handleDepth()-Depth_Keller msg: 1613175557376 epoch sec; seqNo:11585; depth 24.33 2021-02-13T00:19:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175557.39 epoch sec; seqNo:11585; 4058878.23 north; 592353.13 east 2021-02-13T00:19:17Z- AHRS_M2 msg: 1613175557.78 epoch sec; seqNo:11491 2021-02-13T00:19:17Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-13T00:19:17Z- handleDepth()-Depth_Keller msg: 1613175557785 epoch sec; seqNo:11586; depth 24.49 2021-02-13T00:19:17Z- handleDvl() - RDI_Pathfinder msg: 1613175557.80 epoch sec; seqNo:5765 2021-02-13T00:19:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.65 , 65.40 , 102.52 2021-02-13T00:19:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.07 , -0.07 2021-02-13T00:19:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175557.81 epoch sec; seqNo:11586; 4058878.61 north; 592353.12 east 2021-02-13T00:19:18Z- AHRS_M2 msg: 1613175558.18 epoch sec; seqNo:11492 2021-02-13T00:19:18Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.18 psi 2021-02-13T00:19:18Z- handleDepth()-Depth_Keller msg: 1613175558185 epoch sec; seqNo:11587; depth 24.60 2021-02-13T00:19:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175558.20 epoch sec; seqNo:11587; 4058878.99 north; 592353.12 east 2021-02-13T00:19:18Z- AHRS_M2 msg: 1613175558.58 epoch sec; seqNo:11493 2021-02-13T00:19:18Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.18 psi 2021-02-13T00:19:18Z- handleDepth()-Depth_Keller msg: 1613175558588 epoch sec; seqNo:11588; depth 24.75 2021-02-13T00:19:18Z- handleDvl() - RDI_Pathfinder msg: 1613175558.61 epoch sec; seqNo:5766 2021-02-13T00:19:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.71 , 0.00 , 65.40 , 101.57 2021-02-13T00:19:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.08 , -0.05 2021-02-13T00:19:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175558.62 epoch sec; seqNo:11588; 4058878.99 north; 592353.12 east 2021-02-13T00:19:18Z- AHRS_M2 msg: 1613175558.99 epoch sec; seqNo:11494 2021-02-13T00:19:18Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.18 psi 2021-02-13T00:19:18Z- handleDepth()-Depth_Keller msg: 1613175558992 epoch sec; seqNo:11589; depth 24.87 2021-02-13T00:19:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175559.00 epoch sec; seqNo:11589; 4058879.37 north; 592353.12 east 2021-02-13T00:19:19Z- LcmTrn::handleMessages() - handling 44 messages took 0.63 ms and 132 calls... 2021-02-13T00:19:19Z- waiting for 1613175557.14, time is 1613175559.00 2021-02-13T00:19:19Z- LcmTrn::updateTrn() >>>> pose time 1613175559.00, meas time 1613175558.61 <<<< 2021-02-13T00:19:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175558.61 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:19:19Z- TerrainNav::motionUpdate() - currEstimate = (4058879, 592353.12, 24.87) 2021-02-13T00:19:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:19:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:19:19Z- TNavBF::Projecting Measurements 2021-02-13T00:19:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:19:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:19:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:19:19Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:19:19Z- TNavFilter::3 good beams. 2021-02-13T00:19:19Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:19:19Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:19:19Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:19:19Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:19:19Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:19:19Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:19:19Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:19:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:19:19Z- TerrainMapOctree: (vehN, vehE ) = (4058879.06, 592352.92). 2021-02-13T00:19:19Z- TerrainMapOctree: Min Distance = 3036.49. 2021-02-13T00:19:19Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:19:19Z- There is no specific good beam that all particles have in common. 2021-02-13T00:19:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:19:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-13T00:19:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:19:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-13T00:19:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:19:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 16.51 2021-02-13T00:19:19Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:19:19Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:19:19Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:19:19Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:19:19Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:19:19Z- Filter 0 No beams used in subcloud update 2021-02-13T00:19:19Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:19:19Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:19:19Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 708.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:19:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175558.61 sec. 2021-02-13T00:19:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:19:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:19:19Z- variances([0], [2], and psi_berg) = 708.576639 833.002661 0.000000 2021-02-13T00:19:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:19:19Z- LcmTrn::publishEstimates() - MLE : 4058878.87 592401.62 24.87 2021-02-13T00:19:19Z- LcmTrn::publishEstimates() - MMSE : 4058879.37 592353.12 24.87 2021-02-13T00:19:19Z- LcmTrn::publishEstimates() - COVAR: 708.58 833.00 0.94 0.00 2021-02-13T00:19:19Z- AHRS_M2 msg: 1613175559.40 epoch sec; seqNo:11495 2021-02-13T00:19:19Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.18 psi 2021-02-13T00:19:19Z- handleDepth()-Depth_Keller msg: 1613175559400 epoch sec; seqNo:11590; depth 25.02 2021-02-13T00:19:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175559.41 epoch sec; seqNo:11590; 4058879.76 north; 592354.33 east 2021-02-13T00:19:19Z- AHRS_M2 msg: 1613175559.80 epoch sec; seqNo:11496 2021-02-13T00:19:19Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.19 psi 2021-02-13T00:19:19Z- handleDepth()-Depth_Keller msg: 1613175559809 epoch sec; seqNo:11591; depth 25.14 2021-02-13T00:19:19Z- handleDvl() - RDI_Pathfinder msg: 1613175559.82 epoch sec; seqNo:5767 2021-02-13T00:19:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 0.00 , 65.40 , 101.23 2021-02-13T00:19:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , -0.06 2021-02-13T00:19:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175559.83 epoch sec; seqNo:11591; 4058880.14 north; 592354.33 east 2021-02-13T00:19:20Z- AHRS_M2 msg: 1613175560.20 epoch sec; seqNo:11497 2021-02-13T00:19:20Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.19 psi 2021-02-13T00:19:20Z- handleDepth()-Depth_Keller msg: 1613175560209 epoch sec; seqNo:11592; depth 25.29 2021-02-13T00:19:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175560.23 epoch sec; seqNo:11592; 4058880.14 north; 592354.33 east 2021-02-13T00:19:20Z- AHRS_M2 msg: 1613175560.60 epoch sec; seqNo:11498 2021-02-13T00:19:20Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.19 psi 2021-02-13T00:19:20Z- handleDepth()-Depth_Keller msg: 1613175560608 epoch sec; seqNo:11593; depth 25.41 2021-02-13T00:19:20Z- handleDvl() - RDI_Pathfinder msg: 1613175560.62 epoch sec; seqNo:5768 2021-02-13T00:19:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.30 , 0.00 , 63.35 , 99.95 2021-02-13T00:19:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , -0.05 2021-02-13T00:19:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175560.65 epoch sec; seqNo:11593; 4058880.52 north; 592354.32 east 2021-02-13T00:19:21Z- AHRS_M2 msg: 1613175561.01 epoch sec; seqNo:11499 2021-02-13T00:19:21Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.19 psi 2021-02-13T00:19:21Z- handleDepth()-Depth_Keller msg: 1613175561012 epoch sec; seqNo:11594; depth 25.55 2021-02-13T00:19:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175561.02 epoch sec; seqNo:11594; 4058880.90 north; 592354.32 east 2021-02-13T00:19:21Z- AHRS_M2 msg: 1613175561.41 epoch sec; seqNo:11500 2021-02-13T00:19:21Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-13T00:19:21Z- handleDepth()-Depth_Keller msg: 1613175561426 epoch sec; seqNo:11595; depth 25.68 2021-02-13T00:19:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175561.43 epoch sec; seqNo:11595; 4058881.27 north; 592354.32 east 2021-02-13T00:19:21Z- AHRS_M2 msg: 1613175561.82 epoch sec; seqNo:11501 2021-02-13T00:19:21Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-13T00:19:21Z- handleDepth()-Depth_Keller msg: 1613175561825 epoch sec; seqNo:11596; depth 25.81 2021-02-13T00:19:21Z- handleDvl() - RDI_Pathfinder msg: 1613175561.84 epoch sec; seqNo:5769 2021-02-13T00:19:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 87.17 , 63.35 , 99.07 2021-02-13T00:19:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.14 , -0.02 2021-02-13T00:19:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175561.85 epoch sec; seqNo:11596; 4058881.29 north; 592355.54 east 2021-02-13T00:19:22Z- AHRS_M2 msg: 1613175562.22 epoch sec; seqNo:11502 2021-02-13T00:19:22Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.20 psi 2021-02-13T00:19:22Z- handleDepth()-Depth_Keller msg: 1613175562225 epoch sec; seqNo:11597; depth 25.95 2021-02-13T00:19:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175562.24 epoch sec; seqNo:11597; 4058881.67 north; 592355.53 east 2021-02-13T00:19:22Z- AHRS_M2 msg: 1613175562.62 epoch sec; seqNo:11503 2021-02-13T00:19:22Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.20 psi 2021-02-13T00:19:22Z- handleDepth()-Depth_Keller msg: 1613175562629 epoch sec; seqNo:11598; depth 26.07 2021-02-13T00:19:22Z- handleDvl() - RDI_Pathfinder msg: 1613175562.64 epoch sec; seqNo:5770 2021-02-13T00:19:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 87.17 , 63.35 , 99.07 2021-02-13T00:19:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.12 , 0.04 2021-02-13T00:19:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175562.65 epoch sec; seqNo:11598; 4058882.05 north; 592355.53 east 2021-02-13T00:19:23Z- AHRS_M2 msg: 1613175563.03 epoch sec; seqNo:11504 2021-02-13T00:19:23Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.20 psi 2021-02-13T00:19:23Z- handleDepth()-Depth_Keller msg: 1613175563035 epoch sec; seqNo:11599; depth 26.22 2021-02-13T00:19:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175563.05 epoch sec; seqNo:11599; 4058882.42 north; 592355.52 east 2021-02-13T00:19:23Z- AHRS_M2 msg: 1613175563.43 epoch sec; seqNo:11505 2021-02-13T00:19:23Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.20 psi 2021-02-13T00:19:23Z- handleDepth()-Depth_Keller msg: 1613175563437 epoch sec; seqNo:11600; depth 26.35 2021-02-13T00:19:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175563.44 epoch sec; seqNo:11600; 4058882.80 north; 592355.52 east 2021-02-13T00:19:23Z- AHRS_M2 msg: 1613175563.84 epoch sec; seqNo:11506 2021-02-13T00:19:23Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-13T00:19:23Z- handleDepth()-Depth_Keller msg: 1613175563845 epoch sec; seqNo:11601; depth 26.48 2021-02-13T00:19:23Z- handleDvl() - RDI_Pathfinder msg: 1613175563.86 epoch sec; seqNo:5771 2021-02-13T00:19:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 87.17 , 64.17 , 97.99 2021-02-13T00:19:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , 0.08 2021-02-13T00:19:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175563.87 epoch sec; seqNo:11601; 4058882.80 north; 592355.52 east 2021-02-13T00:19:24Z- AHRS_M2 msg: 1613175564.24 epoch sec; seqNo:11507 2021-02-13T00:19:24Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-13T00:19:24Z- handleDepth()-Depth_Keller msg: 1613175564253 epoch sec; seqNo:11602; depth 26.61 2021-02-13T00:19:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175564.26 epoch sec; seqNo:11602; 4058883.18 north; 592355.52 east 2021-02-13T00:19:24Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 131 calls... 2021-02-13T00:19:24Z- waiting for 1613175562.00, time is 1613175564.26 2021-02-13T00:19:24Z- LcmTrn::updateTrn() >>>> pose time 1613175564.26, meas time 1613175563.86 <<<< 2021-02-13T00:19:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175563.86 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:19:24Z- TerrainNav::motionUpdate() - currEstimate = (4058883, 592355.52, 26.61) 2021-02-13T00:19:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:19:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:19:24Z- TNavBF::Projecting Measurements 2021-02-13T00:19:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:19:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:19:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:19:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:19:24Z- TNavFilter::4 good beams. 2021-02-13T00:19:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:19:24Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:19:24Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:19:24Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:19:24Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:19:24Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:19:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:19:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:19:24Z- TerrainMapOctree: (vehN, vehE ) = (4058882.89, 592355.33). 2021-02-13T00:19:24Z- TerrainMapOctree: Min Distance = 3032.00. 2021-02-13T00:19:24Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:19:24Z- There is no specific good beam that all particles have in common. 2021-02-13T00:19:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:19:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.108 ExpectedMeasDiff: nan currMeas.covariance: 7.82 2021-02-13T00:19:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:19:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 12.16 2021-02-13T00:19:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:19:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 6.59 2021-02-13T00:19:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:19:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:19:24Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:19:24Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:19:24Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:19:24Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:19:24Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:19:24Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:19:24Z- Filter 0 No beams used in subcloud update 2021-02-13T00:19:24Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:19:24Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:19:24Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 708.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:19:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175563.86 sec. 2021-02-13T00:19:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:19:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:19:24Z- variances([0], [2], and psi_berg) = 708.498566 833.002616 0.000000 2021-02-13T00:19:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:19:24Z- LcmTrn::publishEstimates() - MLE : 4058882.68 592404.02 26.61 2021-02-13T00:19:24Z- LcmTrn::publishEstimates() - MMSE : 4058883.18 592355.52 26.61 2021-02-13T00:19:24Z- LcmTrn::publishEstimates() - COVAR: 708.50 833.00 0.84 0.00 2021-02-13T00:19:24Z- AHRS_M2 msg: 1613175564.64 epoch sec; seqNo:11508 2021-02-13T00:19:24Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.19 psi 2021-02-13T00:19:24Z- handleDepth()-Depth_Keller msg: 1613175564653 epoch sec; seqNo:11603; depth 26.73 2021-02-13T00:19:24Z- handleDvl() - RDI_Pathfinder msg: 1613175564.66 epoch sec; seqNo:5772 2021-02-13T00:19:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 87.17 , 63.76 , 97.99 2021-02-13T00:19:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.02 , 0.09 2021-02-13T00:19:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175564.68 epoch sec; seqNo:11603; 4058883.56 north; 592355.51 east 2021-02-13T00:19:25Z- AHRS_M2 msg: 1613175565.05 epoch sec; seqNo:11509 2021-02-13T00:19:25Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.18 psi 2021-02-13T00:19:25Z- handleDepth()-Depth_Keller msg: 1613175565053 epoch sec; seqNo:11604; depth 26.88 2021-02-13T00:19:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175565.07 epoch sec; seqNo:11604; 4058883.57 north; 592356.73 east 2021-02-13T00:19:25Z- AHRS_M2 msg: 1613175565.46 epoch sec; seqNo:11510 2021-02-13T00:19:25Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.18 psi 2021-02-13T00:19:25Z- handleDepth()-Depth_Keller msg: 1613175565463 epoch sec; seqNo:11605; depth 26.99 2021-02-13T00:19:25Z- handleDvl() - RDI_Pathfinder msg: 1613175565.48 epoch sec; seqNo:5773 2021-02-13T00:19:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 0.00 , 63.35 , 97.99 2021-02-13T00:19:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , 0.07 2021-02-13T00:19:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175565.49 epoch sec; seqNo:11605; 4058883.95 north; 592356.73 east 2021-02-13T00:19:25Z- AHRS_M2 msg: 1613175565.86 epoch sec; seqNo:11511 2021-02-13T00:19:25Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.18 psi 2021-02-13T00:19:25Z- handleDepth()-Depth_Keller msg: 1613175565866 epoch sec; seqNo:11606; depth 27.12 2021-02-13T00:19:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175565.88 epoch sec; seqNo:11606; 4058883.95 north; 592356.73 east 2021-02-13T00:19:26Z- AHRS_M2 msg: 1613175566.26 epoch sec; seqNo:11512 2021-02-13T00:19:26Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-13T00:19:26Z- handleDepth()-Depth_Keller msg: 1613175566269 epoch sec; seqNo:11607; depth 27.24 2021-02-13T00:19:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175566.32 epoch sec; seqNo:11607; 4058884.33 north; 592356.73 east 2021-02-13T00:19:26Z- AHRS_M2 msg: 1613175566.66 epoch sec; seqNo:11513 2021-02-13T00:19:26Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-13T00:19:26Z- handleDepth()-Depth_Keller msg: 1613175566668 epoch sec; seqNo:11608; depth 27.38 2021-02-13T00:19:26Z- handleDvl() - RDI_Pathfinder msg: 1613175566.68 epoch sec; seqNo:5774 2021-02-13T00:19:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:19:26Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:19:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175566.71 epoch sec; seqNo:11608; 4058884.71 north; 592356.72 east 2021-02-13T00:19:27Z- AHRS_M2 msg: 1613175567.07 epoch sec; seqNo:11514 2021-02-13T00:19:27Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-13T00:19:27Z- handleDepth()-Depth_Keller msg: 1613175567072 epoch sec; seqNo:11609; depth 27.51 2021-02-13T00:19:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175567.08 epoch sec; seqNo:11609; 4058884.72 north; 592357.94 east 2021-02-13T00:19:27Z- AHRS_M2 msg: 1613175567.48 epoch sec; seqNo:11515 2021-02-13T00:19:27Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.18 psi 2021-02-13T00:19:27Z- handleDepth()-Depth_Keller msg: 1613175567489 epoch sec; seqNo:11610; depth 27.64 2021-02-13T00:19:27Z- handleDvl() - RDI_Pathfinder msg: 1613175567.51 epoch sec; seqNo:5775 2021-02-13T00:19:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.85 , 0.00 , 62.53 , 97.51 2021-02-13T00:19:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.08 , -0.05 2021-02-13T00:19:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175567.52 epoch sec; seqNo:11610; 4058885.10 north; 592357.94 east 2021-02-13T00:19:27Z- AHRS_M2 msg: 1613175567.88 epoch sec; seqNo:11516 2021-02-13T00:19:27Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.18 psi 2021-02-13T00:19:27Z- handleDepth()-Depth_Keller msg: 1613175567885 epoch sec; seqNo:11611; depth 27.79 2021-02-13T00:19:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175567.91 epoch sec; seqNo:11611; 4058885.48 north; 592357.93 east 2021-02-13T00:19:28Z- AHRS_M2 msg: 1613175568.28 epoch sec; seqNo:11517 2021-02-13T00:19:28Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.18 psi 2021-02-13T00:19:28Z- handleDepth()-Depth_Keller msg: 1613175568289 epoch sec; seqNo:11612; depth 27.91 2021-02-13T00:19:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175568.30 epoch sec; seqNo:11612; 4058885.86 north; 592357.93 east 2021-02-13T00:19:28Z- AHRS_M2 msg: 1613175568.69 epoch sec; seqNo:11518 2021-02-13T00:19:28Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.18 psi 2021-02-13T00:19:28Z- handleDepth()-Depth_Keller msg: 1613175568705 epoch sec; seqNo:11613; depth 28.03 2021-02-13T00:19:28Z- handleDvl() - RDI_Pathfinder msg: 1613175568.72 epoch sec; seqNo:5776 2021-02-13T00:19:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.67 , 0.00 , 62.94 , 97.99 2021-02-13T00:19:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.05 , -0.08 2021-02-13T00:19:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175568.74 epoch sec; seqNo:11613; 4058885.86 north; 592357.93 east 2021-02-13T00:19:29Z- AHRS_M2 msg: 1613175569.09 epoch sec; seqNo:11519 2021-02-13T00:19:29Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.18 psi 2021-02-13T00:19:29Z- handleDepth()-Depth_Keller msg: 1613175569094 epoch sec; seqNo:11614; depth 28.18 2021-02-13T00:19:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175569.11 epoch sec; seqNo:11614; 4058886.25 north; 592359.15 east 2021-02-13T00:19:29Z- LcmTrn::handleMessages() - handling 41 messages took 0.66 ms and 128 calls... 2021-02-13T00:19:29Z- waiting for 1613175567.26, time is 1613175569.11 2021-02-13T00:19:29Z- LcmTrn::updateTrn() >>>> pose time 1613175569.11, meas time 1613175568.72 <<<< 2021-02-13T00:19:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175568.72 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:19:29Z- TerrainNav::motionUpdate() - currEstimate = (4058886, 592359.15, 28.18) 2021-02-13T00:19:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:19:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:19:29Z- TNavBF::Projecting Measurements 2021-02-13T00:19:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:19:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:19:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:19:29Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:19:29Z- TNavFilter::3 good beams. 2021-02-13T00:19:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:19:29Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:19:29Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:19:29Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:19:29Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:19:29Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:19:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:19:29Z- TNavBF::mmse selected from filter -1 2021-02-13T00:19:29Z- TerrainMapOctree: (vehN, vehE ) = (4058886.01, 592358.86). 2021-02-13T00:19:29Z- TerrainMapOctree: Min Distance = 3027.36. 2021-02-13T00:19:29Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:19:29Z- There is no specific good beam that all particles have in common. 2021-02-13T00:19:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:19:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 7.54 2021-02-13T00:19:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:19:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 6.34 2021-02-13T00:19:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:19:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:19:29Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:19:29Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:19:29Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:19:29Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:19:29Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:19:29Z- Filter 0 No beams used in subcloud update 2021-02-13T00:19:29Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:19:29Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:19:29Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 708.4 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:19:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175568.72 sec. 2021-02-13T00:19:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:19:29Z- TNavBF::mmse selected from filter -1 2021-02-13T00:19:29Z- variances([0], [2], and psi_berg) = 708.418227 833.002570 0.000000 2021-02-13T00:19:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:19:29Z- LcmTrn::publishEstimates() - MLE : 4058885.75 592407.65 28.18 2021-02-13T00:19:29Z- LcmTrn::publishEstimates() - MMSE : 4058886.25 592359.15 28.18 2021-02-13T00:19:29Z- LcmTrn::publishEstimates() - COVAR: 708.42 833.00 0.83 0.00 2021-02-13T00:19:29Z- AHRS_M2 msg: 1613175569.52 epoch sec; seqNo:11520 2021-02-13T00:19:29Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.18 psi 2021-02-13T00:19:29Z- handleDepth()-Depth_Keller msg: 1613175569527 epoch sec; seqNo:11615; depth 28.29 2021-02-13T00:19:29Z- handleDvl() - RDI_Pathfinder msg: 1613175569.54 epoch sec; seqNo:5777 2021-02-13T00:19:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.26 , 85.00 , 61.72 , 97.99 2021-02-13T00:19:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , -0.07 2021-02-13T00:19:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175569.55 epoch sec; seqNo:11615; 4058886.63 north; 592359.14 east 2021-02-13T00:19:29Z- AHRS_M2 msg: 1613175569.89 epoch sec; seqNo:11521 2021-02-13T00:19:29Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.18 psi 2021-02-13T00:19:29Z- handleDepth()-Depth_Keller msg: 1613175569905 epoch sec; seqNo:11616; depth 28.44 2021-02-13T00:19:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175569.92 epoch sec; seqNo:11616; 4058886.63 north; 592359.14 east 2021-02-13T00:19:30Z- AHRS_M2 msg: 1613175570.30 epoch sec; seqNo:11522 2021-02-13T00:19:30Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.18 psi 2021-02-13T00:19:30Z- handleDepth()-Depth_Keller msg: 1613175570304 epoch sec; seqNo:11617; depth 28.54 2021-02-13T00:19:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175570.31 epoch sec; seqNo:11617; 4058887.01 north; 592359.14 east 2021-02-13T00:19:30Z- AHRS_M2 msg: 1613175570.71 epoch sec; seqNo:11523 2021-02-13T00:19:30Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.17 psi 2021-02-13T00:19:30Z- handleDepth()-Depth_Keller msg: 1613175570718 epoch sec; seqNo:11618; depth 28.68 2021-02-13T00:19:30Z- handleDvl() - RDI_Pathfinder msg: 1613175570.73 epoch sec; seqNo:5778 2021-02-13T00:19:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.44 , 85.00 , 61.72 , 97.99 2021-02-13T00:19:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , -0.05 2021-02-13T00:19:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175570.74 epoch sec; seqNo:11618; 4058887.39 north; 592359.13 east 2021-02-13T00:19:31Z- AHRS_M2 msg: 1613175571.11 epoch sec; seqNo:11524 2021-02-13T00:19:31Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.17 psi 2021-02-13T00:19:31Z- handleDepth()-Depth_Keller msg: 1613175571112 epoch sec; seqNo:11619; depth 28.80 2021-02-13T00:19:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175571.14 epoch sec; seqNo:11619; 4058887.77 north; 592359.13 east 2021-02-13T00:19:31Z- AHRS_M2 msg: 1613175571.53 epoch sec; seqNo:11525 2021-02-13T00:19:31Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.17 psi 2021-02-13T00:19:31Z- handleDepth()-Depth_Keller msg: 1613175571537 epoch sec; seqNo:11620; depth 28.92 2021-02-13T00:19:31Z- handleDvl() - RDI_Pathfinder msg: 1613175571.55 epoch sec; seqNo:5779 2021-02-13T00:19:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.85 , 83.92 , 0.00 , 97.99 2021-02-13T00:19:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , -0.05 2021-02-13T00:19:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175571.56 epoch sec; seqNo:11620; 4058887.78 north; 592360.35 east 2021-02-13T00:19:31Z- AHRS_M2 msg: 1613175571.92 epoch sec; seqNo:11526 2021-02-13T00:19:31Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.18 psi 2021-02-13T00:19:31Z- handleDepth()-Depth_Keller msg: 1613175571920 epoch sec; seqNo:11621; depth 29.07 2021-02-13T00:19:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175571.93 epoch sec; seqNo:11621; 4058888.16 north; 592360.35 east 2021-02-13T00:19:32Z- AHRS_M2 msg: 1613175572.32 epoch sec; seqNo:11527 2021-02-13T00:19:32Z- AHRS_M2 msg: 0.68 phi; -0.30 theta; -0.18 psi 2021-02-13T00:19:32Z- handleDepth()-Depth_Keller msg: 1613175572326 epoch sec; seqNo:11622; depth 29.19 2021-02-13T00:19:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175572.34 epoch sec; seqNo:11622; 4058888.54 north; 592360.34 east 2021-02-13T00:19:32Z- AHRS_M2 msg: 1613175572.72 epoch sec; seqNo:11528 2021-02-13T00:19:32Z- AHRS_M2 msg: 0.68 phi; -0.27 theta; -0.18 psi 2021-02-13T00:19:32Z- handleDepth()-Depth_Keller msg: 1613175572729 epoch sec; seqNo:11623; depth 29.33 2021-02-13T00:19:32Z- handleDvl() - RDI_Pathfinder msg: 1613175572.75 epoch sec; seqNo:5780 2021-02-13T00:19:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.03 , 83.72 , 60.90 , 98.32 2021-02-13T00:19:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , -0.04 2021-02-13T00:19:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175572.77 epoch sec; seqNo:11623; 4058888.92 north; 592360.34 east 2021-02-13T00:19:33Z- AHRS_M2 msg: 1613175573.13 epoch sec; seqNo:11529 2021-02-13T00:19:33Z- AHRS_M2 msg: 0.67 phi; -0.24 theta; -0.18 psi 2021-02-13T00:19:33Z- handleDepth()-Depth_Keller msg: 1613175573137 epoch sec; seqNo:11624; depth 29.47 2021-02-13T00:19:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175573.15 epoch sec; seqNo:11624; 4058888.92 north; 592360.34 east 2021-02-13T00:19:33Z- AHRS_M2 msg: 1613175573.53 epoch sec; seqNo:11530 2021-02-13T00:19:33Z- AHRS_M2 msg: 0.66 phi; -0.20 theta; -0.17 psi 2021-02-13T00:19:33Z- handleDepth()-Depth_Keller msg: 1613175573545 epoch sec; seqNo:11625; depth 29.61 2021-02-13T00:19:33Z- handleDvl() - RDI_Pathfinder msg: 1613175573.56 epoch sec; seqNo:5781 2021-02-13T00:19:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 81.76 , 62.53 , 86.08 2021-02-13T00:19:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.05 2021-02-13T00:19:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175573.57 epoch sec; seqNo:11625; 4058889.31 north; 592361.55 east 2021-02-13T00:19:33Z- AHRS_M2 msg: 1613175573.94 epoch sec; seqNo:11531 2021-02-13T00:19:33Z- AHRS_M2 msg: 0.66 phi; -0.15 theta; -0.16 psi 2021-02-13T00:19:33Z- handleDepth()-Depth_Keller msg: 1613175573949 epoch sec; seqNo:11626; depth 29.74 2021-02-13T00:19:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175573.96 epoch sec; seqNo:11626; 4058889.69 north; 592361.55 east 2021-02-13T00:19:34Z- AHRS_M2 msg: 1613175574.34 epoch sec; seqNo:11532 2021-02-13T00:19:34Z- AHRS_M2 msg: 0.65 phi; -0.11 theta; -0.16 psi 2021-02-13T00:19:34Z- handleDepth()-Depth_Keller msg: 1613175574344 epoch sec; seqNo:11627; depth 29.88 2021-02-13T00:19:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175574.36 epoch sec; seqNo:11627; 4058890.07 north; 592361.55 east 2021-02-13T00:19:34Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-13T00:19:34Z- waiting for 1613175572.11, time is 1613175574.36 2021-02-13T00:19:34Z- LcmTrn::updateTrn() >>>> pose time 1613175574.36, meas time 1613175573.56 <<<< 2021-02-13T00:19:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175573.56 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:19:34Z- TerrainNav::motionUpdate() - currEstimate = (4058890, 592361.55, 29.88) 2021-02-13T00:19:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:19:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:19:34Z- TNavBF::Projecting Measurements 2021-02-13T00:19:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:19:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:19:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:19:34Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:19:34Z- TNavFilter::3 good beams. 2021-02-13T00:19:34Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:19:34Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:19:34Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:19:34Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:19:34Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:19:34Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:19:34Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:19:34Z- TNavBF::mmse selected from filter -1 2021-02-13T00:19:34Z- TerrainMapOctree: (vehN, vehE ) = (4058889.48, 592361.18). 2021-02-13T00:19:34Z- TerrainMapOctree: Min Distance = 3023.19. 2021-02-13T00:19:34Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:19:34Z- There is no specific good beam that all particles have in common. 2021-02-13T00:19:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:19:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 10.69 2021-02-13T00:19:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:19:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 6.26 2021-02-13T00:19:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:19:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 11.86 2021-02-13T00:19:34Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:19:34Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:19:34Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:19:34Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:19:34Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:19:34Z- Filter 0 No beams used in subcloud update 2021-02-13T00:19:34Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:19:34Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:19:34Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 708.4 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:19:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175573.56 sec. 2021-02-13T00:19:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:19:34Z- TNavBF::mmse selected from filter -1 2021-02-13T00:19:34Z- variances([0], [2], and psi_berg) = 708.335722 833.002522 0.000000 2021-02-13T00:19:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:19:34Z- LcmTrn::publishEstimates() - MLE : 4058889.57 592410.05 29.88 2021-02-13T00:19:34Z- LcmTrn::publishEstimates() - MMSE : 4058890.07 592361.55 29.88 2021-02-13T00:19:34Z- LcmTrn::publishEstimates() - COVAR: 708.34 833.00 0.83 0.00 2021-02-13T00:19:34Z- AHRS_M2 msg: 1613175574.74 epoch sec; seqNo:11533 2021-02-13T00:19:34Z- AHRS_M2 msg: 0.64 phi; -0.06 theta; -0.15 psi 2021-02-13T00:19:34Z- handleDepth()-Depth_Keller msg: 1613175574748 epoch sec; seqNo:11628; depth 29.97 2021-02-13T00:19:34Z- handleDvl() - RDI_Pathfinder msg: 1613175574.76 epoch sec; seqNo:5782 2021-02-13T00:19:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.76 , 78.51 , 63.76 , 78.51 2021-02-13T00:19:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.12 , -0.06 2021-02-13T00:19:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175574.78 epoch sec; seqNo:11628; 4058890.45 north; 592361.54 east 2021-02-13T00:19:35Z- AHRS_M2 msg: 1613175575.15 epoch sec; seqNo:11534 2021-02-13T00:19:35Z- AHRS_M2 msg: 0.63 phi; -0.02 theta; -0.15 psi 2021-02-13T00:19:35Z- handleDepth()-Depth_Keller msg: 1613175575155 epoch sec; seqNo:11629; depth 30.08 2021-02-13T00:19:35Z- handleDvl() - RDI_Pathfinder msg: 1613175575.17 epoch sec; seqNo:5783 2021-02-13T00:19:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.38 , 0.00 , 67.13 , 0.00 2021-02-13T00:19:35Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:19:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175575.18 epoch sec; seqNo:11629; 4058890.82 north; 592361.54 east 2021-02-13T00:19:35Z- AHRS_M2 msg: 1613175575.56 epoch sec; seqNo:11535 2021-02-13T00:19:35Z- AHRS_M2 msg: 0.62 phi; 0.02 theta; -0.15 psi 2021-02-13T00:19:35Z- handleDepth()-Depth_Keller msg: 1613175575565 epoch sec; seqNo:11630; depth 30.15 2021-02-13T00:19:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175575.58 epoch sec; seqNo:11630; 4058890.82 north; 592361.54 east 2021-02-13T00:19:35Z- AHRS_M2 msg: 1613175575.96 epoch sec; seqNo:11536 2021-02-13T00:19:35Z- AHRS_M2 msg: 0.61 phi; 0.06 theta; -0.16 psi 2021-02-13T00:19:35Z- handleDepth()-Depth_Keller msg: 1613175575961 epoch sec; seqNo:11631; depth 30.21 2021-02-13T00:19:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175575.98 epoch sec; seqNo:11631; 4058891.20 north; 592361.53 east 2021-02-13T00:19:36Z- AHRS_M2 msg: 1613175576.36 epoch sec; seqNo:11537 2021-02-13T00:19:36Z- AHRS_M2 msg: 0.60 phi; 0.10 theta; -0.16 psi 2021-02-13T00:19:36Z- handleDepth()-Depth_Keller msg: 1613175576364 epoch sec; seqNo:11632; depth 30.26 2021-02-13T00:19:36Z- handleDvl() - RDI_Pathfinder msg: 1613175576.38 epoch sec; seqNo:5784 2021-02-13T00:19:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.90 , 0.00 , 0.00 , 65.56 2021-02-13T00:19:36Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:19:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175576.40 epoch sec; seqNo:11632; 4058891.60 north; 592362.75 east 2021-02-13T00:19:36Z- AHRS_M2 msg: 1613175576.76 epoch sec; seqNo:11538 2021-02-13T00:19:36Z- AHRS_M2 msg: 0.59 phi; 0.13 theta; -0.16 psi 2021-02-13T00:19:36Z- handleDepth()-Depth_Keller msg: 1613175576771 epoch sec; seqNo:11633; depth 30.29 2021-02-13T00:19:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175576.78 epoch sec; seqNo:11633; 4058891.97 north; 592362.75 east 2021-02-13T00:19:37Z- AHRS_M2 msg: 1613175577.19 epoch sec; seqNo:11539 2021-02-13T00:19:37Z- AHRS_M2 msg: 0.58 phi; 0.16 theta; -0.16 psi 2021-02-13T00:19:37Z- handleDepth()-Depth_Keller msg: 1613175577203 epoch sec; seqNo:11634; depth 30.30 2021-02-13T00:19:37Z- handleDvl() - RDI_Pathfinder msg: 1613175577.21 epoch sec; seqNo:5785 2021-02-13T00:19:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.79 , 0.00 , 0.00 , 63.35 2021-02-13T00:19:37Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:19:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175577.25 epoch sec; seqNo:11634; 4058892.35 north; 592362.74 east 2021-02-13T00:19:37Z- AHRS_M2 msg: 1613175577.57 epoch sec; seqNo:11540 2021-02-13T00:19:37Z- AHRS_M2 msg: 0.58 phi; 0.19 theta; -0.15 psi 2021-02-13T00:19:37Z- handleDepth()-Depth_Keller msg: 1613175577576 epoch sec; seqNo:11635; depth 30.32 2021-02-13T00:19:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175577.60 epoch sec; seqNo:11635; 4058892.73 north; 592362.74 east 2021-02-13T00:19:37Z- AHRS_M2 msg: 1613175577.98 epoch sec; seqNo:11541 2021-02-13T00:19:37Z- AHRS_M2 msg: 0.57 phi; 0.22 theta; -0.15 psi 2021-02-13T00:19:37Z- handleDepth()-Depth_Keller msg: 1613175577985 epoch sec; seqNo:11636; depth 30.29 2021-02-13T00:19:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175578.00 epoch sec; seqNo:11636; 4058893.11 north; 592362.73 east 2021-02-13T00:19:38Z- AHRS_M2 msg: 1613175578.38 epoch sec; seqNo:11542 2021-02-13T00:19:38Z- AHRS_M2 msg: 0.57 phi; 0.24 theta; -0.14 psi 2021-02-13T00:19:38Z- handleDepth()-Depth_Keller msg: 1613175578384 epoch sec; seqNo:11637; depth 30.28 2021-02-13T00:19:38Z- handleDvl() - RDI_Pathfinder msg: 1613175578.40 epoch sec; seqNo:5786 2021-02-13T00:19:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 65.56 , 0.00 , 0.00 , 62.02 2021-02-13T00:19:38Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:19:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175578.41 epoch sec; seqNo:11637; 4058893.11 north; 592362.73 east 2021-02-13T00:19:38Z- AHRS_M2 msg: 1613175578.78 epoch sec; seqNo:11543 2021-02-13T00:19:38Z- AHRS_M2 msg: 0.57 phi; 0.26 theta; -0.14 psi 2021-02-13T00:19:38Z- handleDepth()-Depth_Keller msg: 1613175578788 epoch sec; seqNo:11638; depth 30.25 2021-02-13T00:19:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175578.80 epoch sec; seqNo:11638; 4058893.49 north; 592362.73 east 2021-02-13T00:19:39Z- AHRS_M2 msg: 1613175579.21 epoch sec; seqNo:11544 2021-02-13T00:19:39Z- AHRS_M2 msg: 0.57 phi; 0.28 theta; -0.14 psi 2021-02-13T00:19:39Z- handleDepth()-Depth_Keller msg: 1613175579211 epoch sec; seqNo:11639; depth 30.20 2021-02-13T00:19:39Z- handleDvl() - RDI_Pathfinder msg: 1613175579.22 epoch sec; seqNo:5787 2021-02-13T00:19:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:19:39Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:19:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175579.23 epoch sec; seqNo:11639; 4058893.87 north; 592362.73 east 2021-02-13T00:19:39Z- AHRS_M2 msg: 1613175579.60 epoch sec; seqNo:11545 2021-02-13T00:19:39Z- AHRS_M2 msg: 0.57 phi; 0.30 theta; -0.14 psi 2021-02-13T00:19:39Z- handleDepth()-Depth_Keller msg: 1613175579603 epoch sec; seqNo:11640; depth 30.15 2021-02-13T00:19:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175579.62 epoch sec; seqNo:11640; 4058894.25 north; 592362.72 east 2021-02-13T00:19:39Z- LcmTrn::handleMessages() - handling 45 messages took 0.69 ms and 132 calls... 2021-02-13T00:19:39Z- waiting for 1613175577.36, time is 1613175579.62 2021-02-13T00:19:39Z- LcmTrn::updateTrn() >>>> pose time 1613175579.62, meas time 1613175579.22 <<<< 2021-02-13T00:19:39Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175579.22, ping # 0. 2021-02-13T00:19:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175579.22 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:19:39Z- TerrainNav::motionUpdate() - currEstimate = (4058894, 592362.72, 30.15) 2021-02-13T00:19:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:19:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:19:39Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:19:39Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:19:39Z- TNavBF::Projecting Measurements 2021-02-13T00:19:39Z- TNavFilter::Measurement from time = 1613175579.22 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:19:39Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:19:39Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:19:39Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:19:39Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:19:39Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:19:39Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:19:39Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:19:39Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 708.3 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:19:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:19:39Z- TNavBF::mmse selected from filter -1 2021-02-13T00:19:39Z- variances([0], [2], and psi_berg) = 708.259425 833.002478 0.000000 2021-02-13T00:19:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:19:39Z- LcmTrn::publishEstimates() - MLE : 4058893.75 592412.22 30.15 2021-02-13T00:19:39Z- LcmTrn::publishEstimates() - MMSE : 4058894.25 592362.72 30.15 2021-02-13T00:19:39Z- LcmTrn::publishEstimates() - COVAR: 708.26 833.00 1.05 0.00 2021-02-13T00:19:39Z- AHRS_M2 msg: 1613175579.99 epoch sec; seqNo:11546 2021-02-13T00:19:39Z- AHRS_M2 msg: 0.57 phi; 0.31 theta; -0.13 psi 2021-02-13T00:19:40Z- handleDepth()-Depth_Keller msg: 1613175580005 epoch sec; seqNo:11641; depth 30.09 2021-02-13T00:19:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175580.01 epoch sec; seqNo:11641; 4058894.64 north; 592363.94 east 2021-02-13T00:19:40Z- AHRS_M2 msg: 1613175580.40 epoch sec; seqNo:11547 2021-02-13T00:19:40Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.13 psi 2021-02-13T00:19:40Z- handleDepth()-Depth_Keller msg: 1613175580404 epoch sec; seqNo:11642; depth 30.02 2021-02-13T00:19:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175580.41 epoch sec; seqNo:11642; 4058895.02 north; 592363.94 east 2021-02-13T00:19:40Z- AHRS_M2 msg: 1613175580.80 epoch sec; seqNo:11548 2021-02-13T00:19:40Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.12 psi 2021-02-13T00:19:40Z- handleDepth()-Depth_Keller msg: 1613175580818 epoch sec; seqNo:11643; depth 29.93 2021-02-13T00:19:40Z- handleDvl() - RDI_Pathfinder msg: 1613175580.83 epoch sec; seqNo:5788 2021-02-13T00:19:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:19:40Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:19:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175580.84 epoch sec; seqNo:11643; 4058895.02 north; 592363.94 east 2021-02-13T00:19:41Z- AHRS_M2 msg: 1613175581.21 epoch sec; seqNo:11549 2021-02-13T00:19:41Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.12 psi 2021-02-13T00:19:41Z- handleDepth()-Depth_Keller msg: 1613175581217 epoch sec; seqNo:11644; depth 29.85 2021-02-13T00:19:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175581.23 epoch sec; seqNo:11644; 4058895.40 north; 592363.93 east 2021-02-13T00:19:41Z- handleDepth()-Depth_Keller msg: 1613175581633 epoch sec; seqNo:11645; depth 29.76 2021-02-13T00:19:41Z- handleDvl() - RDI_Pathfinder msg: 1613175581.64 epoch sec; seqNo:5789 2021-02-13T00:19:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.75 , 0.00 , 97.24 , 61.10 2021-02-13T00:19:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.05 , -0.06 2021-02-13T00:19:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175581.65 epoch sec; seqNo:11645; 4058895.78 north; 592363.93 east 2021-02-13T00:19:42Z- AHRS_M2 msg: 1613175582.02 epoch sec; seqNo:11550 2021-02-13T00:19:42Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.11 psi 2021-02-13T00:19:42Z- handleDepth()-Depth_Keller msg: 1613175582030 epoch sec; seqNo:11646; depth 29.66 2021-02-13T00:19:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175582.04 epoch sec; seqNo:11646; 4058895.78 north; 592363.93 east 2021-02-13T00:19:42Z- AHRS_M2 msg: 1613175582.42 epoch sec; seqNo:11551 2021-02-13T00:19:42Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.11 psi 2021-02-13T00:19:42Z- handleDepth()-Depth_Keller msg: 1613175582424 epoch sec; seqNo:11647; depth 29.56 2021-02-13T00:19:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175582.43 epoch sec; seqNo:11647; 4058896.16 north; 592363.92 east 2021-02-13T00:19:42Z- AHRS_M2 msg: 1613175582.82 epoch sec; seqNo:11552 2021-02-13T00:19:42Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.11 psi 2021-02-13T00:19:42Z- handleDepth()-Depth_Keller msg: 1613175582833 epoch sec; seqNo:11648; depth 29.45 2021-02-13T00:19:42Z- handleDvl() - RDI_Pathfinder msg: 1613175582.85 epoch sec; seqNo:5790 2021-02-13T00:19:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 79.59 , 97.99 , 59.67 2021-02-13T00:19:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.01 , -0.06 2021-02-13T00:19:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175582.86 epoch sec; seqNo:11648; 4058896.55 north; 592365.14 east 2021-02-13T00:19:43Z- AHRS_M2 msg: 1613175583.23 epoch sec; seqNo:11553 2021-02-13T00:19:43Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.10 psi 2021-02-13T00:19:43Z- handleDepth()-Depth_Keller msg: 1613175583234 epoch sec; seqNo:11649; depth 29.34 2021-02-13T00:19:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175583.25 epoch sec; seqNo:11649; 4058896.55 north; 592365.14 east 2021-02-13T00:19:43Z- AHRS_M2 msg: 1613175583.63 epoch sec; seqNo:11554 2021-02-13T00:19:43Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.10 psi 2021-02-13T00:19:43Z- handleDepth()-Depth_Keller msg: 1613175583636 epoch sec; seqNo:11650; depth 29.24 2021-02-13T00:19:43Z- handleDvl() - RDI_Pathfinder msg: 1613175583.65 epoch sec; seqNo:5791 2021-02-13T00:19:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.71 , 80.68 , 97.99 , 60.49 2021-02-13T00:19:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , -0.05 2021-02-13T00:19:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175583.66 epoch sec; seqNo:11650; 4058896.93 north; 592365.14 east 2021-02-13T00:19:44Z- AHRS_M2 msg: 1613175584.04 epoch sec; seqNo:11555 2021-02-13T00:19:44Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.10 psi 2021-02-13T00:19:44Z- handleDepth()-Depth_Keller msg: 1613175584045 epoch sec; seqNo:11651; depth 29.12 2021-02-13T00:19:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175584.06 epoch sec; seqNo:11651; 4058897.30 north; 592365.13 east 2021-02-13T00:19:44Z- AHRS_M2 msg: 1613175584.44 epoch sec; seqNo:11556 2021-02-13T00:19:44Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.10 psi 2021-02-13T00:19:44Z- handleDepth()-Depth_Keller msg: 1613175584446 epoch sec; seqNo:11652; depth 29.00 2021-02-13T00:19:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175584.45 epoch sec; seqNo:11652; 4058897.30 north; 592365.13 east 2021-02-13T00:19:44Z- LcmTrn::handleMessages() - handling 39 messages took 0.56 ms and 128 calls... 2021-02-13T00:19:44Z- waiting for 1613175582.62, time is 1613175584.45 2021-02-13T00:19:44Z- LcmTrn::updateTrn() >>>> pose time 1613175584.45, meas time 1613175583.65 <<<< 2021-02-13T00:19:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175583.65 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:19:44Z- TerrainNav::motionUpdate() - currEstimate = (4058897, 592365.13, 29.00) 2021-02-13T00:19:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:19:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:19:44Z- TNavBF::Projecting Measurements 2021-02-13T00:19:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:19:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:19:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:19:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:19:44Z- TNavFilter::4 good beams. 2021-02-13T00:19:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:19:44Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:19:44Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:19:44Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:19:44Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:19:44Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:19:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:19:44Z- TNavBF::mmse selected from filter -1 2021-02-13T00:19:44Z- TerrainMapOctree: (vehN, vehE ) = (4058896.80, 592364.73). 2021-02-13T00:19:44Z- TerrainMapOctree: Min Distance = 3015.26. 2021-02-13T00:19:44Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:19:44Z- There is no specific good beam that all particles have in common. 2021-02-13T00:19:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:19:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-13T00:19:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:19:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 10.41 2021-02-13T00:19:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:19:44Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:19:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:19:44Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.110 ExpectedMeasDiff: nan currMeas.covariance: 5.85 2021-02-13T00:19:44Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:19:44Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:19:44Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:19:44Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:19:44Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:19:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:19:44Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:19:44Z- Filter 0 No beams used in subcloud update 2021-02-13T00:19:44Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:19:44Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:19:44Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 708.2 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:19:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175583.65 sec. 2021-02-13T00:19:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:19:44Z- TNavBF::mmse selected from filter -1 2021-02-13T00:19:44Z- variances([0], [2], and psi_berg) = 708.182356 833.002434 0.000000 2021-02-13T00:19:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:19:44Z- LcmTrn::publishEstimates() - MLE : 4058896.80 592414.63 29.00 2021-02-13T00:19:44Z- LcmTrn::publishEstimates() - MMSE : 4058897.30 592365.13 29.00 2021-02-13T00:19:44Z- LcmTrn::publishEstimates() - COVAR: 708.18 833.00 0.87 0.00 2021-02-13T00:19:44Z- AHRS_M2 msg: 1613175584.84 epoch sec; seqNo:11557 2021-02-13T00:19:44Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.10 psi 2021-02-13T00:19:44Z- handleDepth()-Depth_Keller msg: 1613175584848 epoch sec; seqNo:11653; depth 28.88 2021-02-13T00:19:44Z- handleDvl() - RDI_Pathfinder msg: 1613175584.87 epoch sec; seqNo:5792 2021-02-13T00:19:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 80.68 , 97.99 , 60.08 2021-02-13T00:19:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.02 , -0.06 2021-02-13T00:19:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175584.88 epoch sec; seqNo:11653; 4058897.68 north; 592365.13 east 2021-02-13T00:19:45Z- AHRS_M2 msg: 1613175585.25 epoch sec; seqNo:11558 2021-02-13T00:19:45Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.10 psi 2021-02-13T00:19:45Z- handleDepth()-Depth_Keller msg: 1613175585256 epoch sec; seqNo:11654; depth 28.75 2021-02-13T00:19:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175585.27 epoch sec; seqNo:11654; 4058898.06 north; 592365.12 east 2021-02-13T00:19:45Z- AHRS_M2 msg: 1613175585.66 epoch sec; seqNo:11559 2021-02-13T00:19:45Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.09 psi 2021-02-13T00:19:45Z- handleDepth()-Depth_Keller msg: 1613175585666 epoch sec; seqNo:11655; depth 28.65 2021-02-13T00:19:45Z- handleDvl() - RDI_Pathfinder msg: 1613175585.68 epoch sec; seqNo:5793 2021-02-13T00:19:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 80.68 , 97.99 , 60.49 2021-02-13T00:19:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.03 , -0.08 2021-02-13T00:19:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175585.69 epoch sec; seqNo:11655; 4058898.08 north; 592366.34 east 2021-02-13T00:19:46Z- AHRS_M2 msg: 1613175586.06 epoch sec; seqNo:11560 2021-02-13T00:19:46Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.10 psi 2021-02-13T00:19:46Z- handleDepth()-Depth_Keller msg: 1613175586063 epoch sec; seqNo:11656; depth 28.52 2021-02-13T00:19:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175586.08 epoch sec; seqNo:11656; 4058898.45 north; 592366.34 east 2021-02-13T00:19:46Z- AHRS_M2 msg: 1613175586.46 epoch sec; seqNo:11561 2021-02-13T00:19:46Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.10 psi 2021-02-13T00:19:46Z- handleDepth()-Depth_Keller msg: 1613175586468 epoch sec; seqNo:11657; depth 28.41 2021-02-13T00:19:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175586.48 epoch sec; seqNo:11657; 4058898.45 north; 592366.34 east 2021-02-13T00:19:46Z- AHRS_M2 msg: 1613175586.86 epoch sec; seqNo:11562 2021-02-13T00:19:46Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.11 psi 2021-02-13T00:19:46Z- handleDepth()-Depth_Keller msg: 1613175586868 epoch sec; seqNo:11658; depth 28.28 2021-02-13T00:19:46Z- handleDvl() - RDI_Pathfinder msg: 1613175586.88 epoch sec; seqNo:5794 2021-02-13T00:19:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 0.00 , 97.99 , 62.13 2021-02-13T00:19:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.01 , -0.10 2021-02-13T00:19:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175586.89 epoch sec; seqNo:11658; 4058898.83 north; 592366.34 east 2021-02-13T00:19:47Z- AHRS_M2 msg: 1613175587.27 epoch sec; seqNo:11563 2021-02-13T00:19:47Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.12 psi 2021-02-13T00:19:47Z- handleDepth()-Depth_Keller msg: 1613175587274 epoch sec; seqNo:11659; depth 28.14 2021-02-13T00:19:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175587.29 epoch sec; seqNo:11659; 4058899.21 north; 592366.33 east 2021-02-13T00:19:47Z- AHRS_M2 msg: 1613175587.67 epoch sec; seqNo:11564 2021-02-13T00:19:47Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.13 psi 2021-02-13T00:19:47Z- handleDepth()-Depth_Keller msg: 1613175587676 epoch sec; seqNo:11660; depth 28.02 2021-02-13T00:19:47Z- handleDvl() - RDI_Pathfinder msg: 1613175587.70 epoch sec; seqNo:5795 2021-02-13T00:19:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 81.76 , 97.99 , 62.13 2021-02-13T00:19:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.10 , -0.07 2021-02-13T00:19:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175587.71 epoch sec; seqNo:11660; 4058899.21 north; 592366.33 east 2021-02-13T00:19:48Z- AHRS_M2 msg: 1613175588.08 epoch sec; seqNo:11565 2021-02-13T00:19:48Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.13 psi 2021-02-13T00:19:48Z- handleDepth()-Depth_Keller msg: 1613175588080 epoch sec; seqNo:11661; depth 27.90 2021-02-13T00:19:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175588.09 epoch sec; seqNo:11661; 4058899.59 north; 592366.33 east 2021-02-13T00:19:48Z- AHRS_M2 msg: 1613175588.48 epoch sec; seqNo:11566 2021-02-13T00:19:48Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.14 psi 2021-02-13T00:19:48Z- handleDepth()-Depth_Keller msg: 1613175588493 epoch sec; seqNo:11662; depth 27.79 2021-02-13T00:19:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175588.50 epoch sec; seqNo:11662; 4058899.98 north; 592367.54 east 2021-02-13T00:19:48Z- AHRS_M2 msg: 1613175588.88 epoch sec; seqNo:11567 2021-02-13T00:19:48Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.15 psi 2021-02-13T00:19:48Z- handleDepth()-Depth_Keller msg: 1613175588888 epoch sec; seqNo:11663; depth 27.66 2021-02-13T00:19:48Z- handleDvl() - RDI_Pathfinder msg: 1613175588.91 epoch sec; seqNo:5796 2021-02-13T00:19:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 83.85 , 97.92 , 63.35 2021-02-13T00:19:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.09 , -0.00 2021-02-13T00:19:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175588.92 epoch sec; seqNo:11663; 4058899.98 north; 592367.54 east 2021-02-13T00:19:49Z- AHRS_M2 msg: 1613175589.29 epoch sec; seqNo:11568 2021-02-13T00:19:49Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.15 psi 2021-02-13T00:19:49Z- handleDepth()-Depth_Keller msg: 1613175589292 epoch sec; seqNo:11664; depth 27.53 2021-02-13T00:19:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175589.30 epoch sec; seqNo:11664; 4058900.36 north; 592367.54 east 2021-02-13T00:19:49Z- AHRS_M2 msg: 1613175589.69 epoch sec; seqNo:11569 2021-02-13T00:19:49Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.16 psi 2021-02-13T00:19:49Z- handleDepth()-Depth_Keller msg: 1613175589696 epoch sec; seqNo:11665; depth 27.42 2021-02-13T00:19:49Z- handleDvl() - RDI_Pathfinder msg: 1613175589.71 epoch sec; seqNo:5797 2021-02-13T00:19:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.71 , 83.79 , 97.85 , 63.35 2021-02-13T00:19:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , 0.07 2021-02-13T00:19:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175589.72 epoch sec; seqNo:11665; 4058900.74 north; 592367.54 east 2021-02-13T00:19:49Z- LcmTrn::handleMessages() - handling 45 messages took 0.68 ms and 132 calls... 2021-02-13T00:19:49Z- waiting for 1613175587.45, time is 1613175589.72 2021-02-13T00:19:49Z- LcmTrn::updateTrn() equating pose 1613175589.72 and meas times 1613175589.72 2021-02-13T00:19:49Z- LcmTrn::updateTrn() >>>> pose time 1613175589.72, meas time 1613175589.72 <<<< 2021-02-13T00:19:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175589.72 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:19:49Z- TerrainNav::motionUpdate() - currEstimate = (4058901, 592367.54, 27.42) 2021-02-13T00:19:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:19:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:19:49Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175589.72 sec.; No relevant navigation data available 2021-02-13T00:19:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:19:49Z- TNavBF::mmse selected from filter -1 2021-02-13T00:19:49Z- variances([0], [2], and psi_berg) = 708.121811 833.002399 0.000000 2021-02-13T00:19:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:19:49Z- LcmTrn::publishEstimates() - MLE : 4058900.24 592417.04 27.42 2021-02-13T00:19:49Z- LcmTrn::publishEstimates() - MMSE : 4058900.74 592367.54 27.42 2021-02-13T00:19:49Z- LcmTrn::publishEstimates() - COVAR: 708.12 833.00 1.08 0.00 2021-02-13T00:19:50Z- AHRS_M2 msg: 1613175590.10 epoch sec; seqNo:11570 2021-02-13T00:19:50Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.16 psi 2021-02-13T00:19:50Z- handleDepth()-Depth_Keller msg: 1613175590102 epoch sec; seqNo:11666; depth 27.31 2021-02-13T00:19:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175590.11 epoch sec; seqNo:11666; 4058901.12 north; 592367.53 east 2021-02-13T00:19:50Z- AHRS_M2 msg: 1613175590.50 epoch sec; seqNo:11571 2021-02-13T00:19:50Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.16 psi 2021-02-13T00:19:50Z- handleDepth()-Depth_Keller msg: 1613175590509 epoch sec; seqNo:11667; depth 27.17 2021-02-13T00:19:50Z- handleDvl() - RDI_Pathfinder msg: 1613175590.53 epoch sec; seqNo:5798 2021-02-13T00:19:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:19:50Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:19:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175590.54 epoch sec; seqNo:11667; 4058901.12 north; 592367.53 east 2021-02-13T00:19:50Z- AHRS_M2 msg: 1613175590.90 epoch sec; seqNo:11572 2021-02-13T00:19:50Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.17 psi 2021-02-13T00:19:50Z- handleDepth()-Depth_Keller msg: 1613175590910 epoch sec; seqNo:11668; depth 27.06 2021-02-13T00:19:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175590.92 epoch sec; seqNo:11668; 4058901.50 north; 592367.53 east 2021-02-13T00:19:51Z- AHRS_M2 msg: 1613175591.33 epoch sec; seqNo:11573 2021-02-13T00:19:51Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.17 psi 2021-02-13T00:19:51Z- handleDepth()-Depth_Keller msg: 1613175591338 epoch sec; seqNo:11669; depth 26.95 2021-02-13T00:19:51Z- handleDvl() - RDI_Pathfinder msg: 1613175591.35 epoch sec; seqNo:5799 2021-02-13T00:19:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 62.56 2021-02-13T00:19:51Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:19:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175591.36 epoch sec; seqNo:11669; 4058901.89 north; 592368.74 east 2021-02-13T00:19:51Z- AHRS_M2 msg: 1613175591.71 epoch sec; seqNo:11574 2021-02-13T00:19:51Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.17 psi 2021-02-13T00:19:51Z- handleDepth()-Depth_Keller msg: 1613175591717 epoch sec; seqNo:11670; depth 26.83 2021-02-13T00:19:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175591.73 epoch sec; seqNo:11670; 4058901.89 north; 592368.74 east 2021-02-13T00:19:52Z- AHRS_M2 msg: 1613175592.12 epoch sec; seqNo:11575 2021-02-13T00:19:52Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.17 psi 2021-02-13T00:19:52Z- handleDepth()-Depth_Keller msg: 1613175592122 epoch sec; seqNo:11671; depth 26.73 2021-02-13T00:19:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175592.13 epoch sec; seqNo:11671; 4058902.27 north; 592368.74 east 2021-02-13T00:19:52Z- AHRS_M2 msg: 1613175592.52 epoch sec; seqNo:11576 2021-02-13T00:19:52Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.17 psi 2021-02-13T00:19:52Z- handleDepth()-Depth_Keller msg: 1613175592530 epoch sec; seqNo:11672; depth 26.61 2021-02-13T00:19:52Z- handleDvl() - RDI_Pathfinder msg: 1613175592.54 epoch sec; seqNo:5800 2021-02-13T00:19:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 70.38 , 0.00 , 0.00 , 63.94 2021-02-13T00:19:52Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:19:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175592.55 epoch sec; seqNo:11672; 4058902.65 north; 592368.74 east 2021-02-13T00:19:52Z- AHRS_M2 msg: 1613175592.92 epoch sec; seqNo:11577 2021-02-13T00:19:52Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.17 psi 2021-02-13T00:19:52Z- handleDepth()-Depth_Keller msg: 1613175592929 epoch sec; seqNo:11673; depth 26.48 2021-02-13T00:19:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175592.95 epoch sec; seqNo:11673; 4058903.03 north; 592368.73 east 2021-02-13T00:19:53Z- AHRS_M2 msg: 1613175593.33 epoch sec; seqNo:11578 2021-02-13T00:19:53Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.17 psi 2021-02-13T00:19:53Z- handleDepth()-Depth_Keller msg: 1613175593332 epoch sec; seqNo:11674; depth 26.37 2021-02-13T00:19:53Z- handleDvl() - RDI_Pathfinder msg: 1613175593.34 epoch sec; seqNo:5801 2021-02-13T00:19:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 70.84 , 0.00 , 0.00 , 64.86 2021-02-13T00:19:53Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:19:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175593.36 epoch sec; seqNo:11674; 4058903.03 north; 592368.73 east 2021-02-13T00:19:53Z- AHRS_M2 msg: 1613175593.73 epoch sec; seqNo:11579 2021-02-13T00:19:53Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.18 psi 2021-02-13T00:19:53Z- handleDepth()-Depth_Keller msg: 1613175593736 epoch sec; seqNo:11675; depth 26.26 2021-02-13T00:19:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175593.75 epoch sec; seqNo:11675; 4058903.41 north; 592368.73 east 2021-02-13T00:19:54Z- AHRS_M2 msg: 1613175594.14 epoch sec; seqNo:11580 2021-02-13T00:19:54Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.18 psi 2021-02-13T00:19:54Z- handleDepth()-Depth_Keller msg: 1613175594145 epoch sec; seqNo:11676; depth 26.14 2021-02-13T00:19:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175594.16 epoch sec; seqNo:11676; 4058903.80 north; 592369.94 east 2021-02-13T00:19:54Z- AHRS_M2 msg: 1613175594.54 epoch sec; seqNo:11581 2021-02-13T00:19:54Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.18 psi 2021-02-13T00:19:54Z- handleDepth()-Depth_Keller msg: 1613175594549 epoch sec; seqNo:11677; depth 26.04 2021-02-13T00:19:54Z- handleDvl() - RDI_Pathfinder msg: 1613175594.57 epoch sec; seqNo:5802 2021-02-13T00:19:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:19:54Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:19:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175594.58 epoch sec; seqNo:11677; 4058904.18 north; 592369.94 east 2021-02-13T00:19:54Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-13T00:19:54Z- waiting for 1613175592.72, time is 1613175594.58 2021-02-13T00:19:54Z- LcmTrn::updateTrn() equating pose 1613175594.58 and meas times 1613175594.58 2021-02-13T00:19:54Z- LcmTrn::updateTrn() >>>> pose time 1613175594.58, meas time 1613175594.58 <<<< 2021-02-13T00:19:54Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175594.58, ping # 0. 2021-02-13T00:19:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175594.58 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:19:54Z- TerrainNav::motionUpdate() - currEstimate = (4058904, 592369.94, 26.04) 2021-02-13T00:19:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:19:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:19:54Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:19:54Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:19:54Z- TNavBF::Projecting Measurements 2021-02-13T00:19:54Z- TNavFilter::Measurement from time = 1613175594.58 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:19:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:19:54Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:19:54Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:19:54Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:19:54Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:19:54Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:19:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:19:54Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 708.1 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:19:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:19:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:19:54Z- variances([0], [2], and psi_berg) = 708.061305 833.002365 0.000000 2021-02-13T00:19:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:19:54Z- LcmTrn::publishEstimates() - MLE : 4058903.68 592419.44 26.04 2021-02-13T00:19:54Z- LcmTrn::publishEstimates() - MMSE : 4058904.18 592369.94 26.04 2021-02-13T00:19:54Z- LcmTrn::publishEstimates() - COVAR: 708.06 833.00 1.30 0.00 2021-02-13T00:19:54Z- AHRS_M2 msg: 1613175594.94 epoch sec; seqNo:11582 2021-02-13T00:19:54Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.18 psi 2021-02-13T00:19:54Z- handleDepth()-Depth_Keller msg: 1613175594953 epoch sec; seqNo:11678; depth 25.91 2021-02-13T00:19:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175594.96 epoch sec; seqNo:11678; 4058904.18 north; 592369.94 east 2021-02-13T00:19:55Z- AHRS_M2 msg: 1613175595.35 epoch sec; seqNo:11583 2021-02-13T00:19:55Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.18 psi 2021-02-13T00:19:55Z- handleDepth()-Depth_Keller msg: 1613175595357 epoch sec; seqNo:11679; depth 25.80 2021-02-13T00:19:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175595.37 epoch sec; seqNo:11679; 4058904.56 north; 592369.94 east 2021-02-13T00:19:55Z- AHRS_M2 msg: 1613175595.76 epoch sec; seqNo:11584 2021-02-13T00:19:55Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.18 psi 2021-02-13T00:19:55Z- handleDepth()-Depth_Keller msg: 1613175595761 epoch sec; seqNo:11680; depth 25.68 2021-02-13T00:19:55Z- handleDvl() - RDI_Pathfinder msg: 1613175595.77 epoch sec; seqNo:5803 2021-02-13T00:19:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 88.25 , 97.99 , 64.58 2021-02-13T00:19:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.09 , -0.03 2021-02-13T00:19:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175595.78 epoch sec; seqNo:11680; 4058904.93 north; 592369.93 east 2021-02-13T00:19:56Z- AHRS_M2 msg: 1613175596.16 epoch sec; seqNo:11585 2021-02-13T00:19:56Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.18 psi 2021-02-13T00:19:56Z- handleDepth()-Depth_Keller msg: 1613175596161 epoch sec; seqNo:11681; depth 25.56 2021-02-13T00:19:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175596.18 epoch sec; seqNo:11681; 4058904.93 north; 592369.93 east 2021-02-13T00:19:56Z- AHRS_M2 msg: 1613175596.56 epoch sec; seqNo:11586 2021-02-13T00:19:56Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.18 psi 2021-02-13T00:19:56Z- handleDepth()-Depth_Keller msg: 1613175596569 epoch sec; seqNo:11682; depth 25.45 2021-02-13T00:19:56Z- handleDvl() - RDI_Pathfinder msg: 1613175596.59 epoch sec; seqNo:5804 2021-02-13T00:19:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 0.00 , 98.86 , 66.21 2021-02-13T00:19:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , -0.09 2021-02-13T00:19:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175596.60 epoch sec; seqNo:11682; 4058905.33 north; 592371.15 east 2021-02-13T00:19:56Z- AHRS_M2 msg: 1613175596.96 epoch sec; seqNo:11587 2021-02-13T00:19:56Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.18 psi 2021-02-13T00:19:56Z- handleDepth()-Depth_Keller msg: 1613175596977 epoch sec; seqNo:11683; depth 25.34 2021-02-13T00:19:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175596.99 epoch sec; seqNo:11683; 4058905.71 north; 592371.15 east 2021-02-13T00:19:57Z- AHRS_M2 msg: 1613175597.37 epoch sec; seqNo:11588 2021-02-13T00:19:57Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.18 psi 2021-02-13T00:19:57Z- handleDepth()-Depth_Keller msg: 1613175597372 epoch sec; seqNo:11684; depth 25.22 2021-02-13T00:19:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175597.39 epoch sec; seqNo:11684; 4058905.71 north; 592371.15 east 2021-02-13T00:19:57Z- AHRS_M2 msg: 1613175597.77 epoch sec; seqNo:11589 2021-02-13T00:19:57Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.18 psi 2021-02-13T00:19:57Z- handleDepth()-Depth_Keller msg: 1613175597781 epoch sec; seqNo:11685; depth 25.11 2021-02-13T00:19:57Z- handleDvl() - RDI_Pathfinder msg: 1613175597.80 epoch sec; seqNo:5805 2021-02-13T00:19:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 0.00 , 98.93 , 66.21 2021-02-13T00:19:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , -0.11 2021-02-13T00:19:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175597.81 epoch sec; seqNo:11685; 4058906.08 north; 592371.14 east 2021-02-13T00:19:58Z- AHRS_M2 msg: 1613175598.18 epoch sec; seqNo:11590 2021-02-13T00:19:58Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.18 psi 2021-02-13T00:19:58Z- handleDepth()-Depth_Keller msg: 1613175598185 epoch sec; seqNo:11686; depth 25.00 2021-02-13T00:19:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175598.20 epoch sec; seqNo:11686; 4058906.46 north; 592371.14 east 2021-02-13T00:19:58Z- AHRS_M2 msg: 1613175598.58 epoch sec; seqNo:11591 2021-02-13T00:19:58Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.19 psi 2021-02-13T00:19:58Z- handleDepth()-Depth_Keller msg: 1613175598586 epoch sec; seqNo:11687; depth 24.88 2021-02-13T00:19:58Z- handleDvl() - RDI_Pathfinder msg: 1613175598.60 epoch sec; seqNo:5806 2021-02-13T00:19:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.53 , 90.28 , 97.85 , 65.81 2021-02-13T00:19:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.07 , -0.10 2021-02-13T00:19:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175598.61 epoch sec; seqNo:11687; 4058906.84 north; 592371.13 east 2021-02-13T00:19:58Z- AHRS_M2 msg: 1613175598.98 epoch sec; seqNo:11592 2021-02-13T00:19:58Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.19 psi 2021-02-13T00:19:58Z- handleDepth()-Depth_Keller msg: 1613175598988 epoch sec; seqNo:11688; depth 24.78 2021-02-13T00:19:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175599.01 epoch sec; seqNo:11688; 4058906.84 north; 592371.13 east 2021-02-13T00:19:59Z- AHRS_M2 msg: 1613175599.39 epoch sec; seqNo:11593 2021-02-13T00:19:59Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.19 psi 2021-02-13T00:19:59Z- handleDepth()-Depth_Keller msg: 1613175599392 epoch sec; seqNo:11689; depth 24.67 2021-02-13T00:19:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175599.40 epoch sec; seqNo:11689; 4058907.23 north; 592372.35 east 2021-02-13T00:19:59Z- AHRS_M2 msg: 1613175599.79 epoch sec; seqNo:11594 2021-02-13T00:19:59Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.19 psi 2021-02-13T00:19:59Z- handleDepth()-Depth_Keller msg: 1613175599796 epoch sec; seqNo:11690; depth 24.56 2021-02-13T00:19:59Z- handleDvl() - RDI_Pathfinder msg: 1613175599.81 epoch sec; seqNo:5807 2021-02-13T00:19:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 0.00 , 97.85 , 65.40 2021-02-13T00:19:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.08 , -0.05 2021-02-13T00:19:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175599.82 epoch sec; seqNo:11690; 4058907.61 north; 592372.35 east 2021-02-13T00:19:59Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-13T00:19:59Z- waiting for 1613175597.58, time is 1613175599.82 2021-02-13T00:19:59Z- LcmTrn::updateTrn() equating pose 1613175599.82 and meas times 1613175599.82 2021-02-13T00:19:59Z- LcmTrn::updateTrn() >>>> pose time 1613175599.82, meas time 1613175599.82 <<<< 2021-02-13T00:19:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175599.82 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:19:59Z- TerrainNav::motionUpdate() - currEstimate = (4058908, 592372.35, 24.56) 2021-02-13T00:19:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:19:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:19:59Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175599.82 sec.; No relevant navigation data available 2021-02-13T00:19:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:19:59Z- TNavBF::mmse selected from filter -1 2021-02-13T00:19:59Z- variances([0], [2], and psi_berg) = 708.000839 833.002330 0.000000 2021-02-13T00:19:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:19:59Z- LcmTrn::publishEstimates() - MLE : 4058907.11 592421.85 24.56 2021-02-13T00:19:59Z- LcmTrn::publishEstimates() - MMSE : 4058907.61 592372.35 24.56 2021-02-13T00:19:59Z- LcmTrn::publishEstimates() - COVAR: 708.00 833.00 1.51 0.00 2021-02-13T00:20:00Z- AHRS_M2 msg: 1613175600.20 epoch sec; seqNo:11595 2021-02-13T00:20:00Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.19 psi 2021-02-13T00:20:00Z- handleDepth()-Depth_Keller msg: 1613175600201 epoch sec; seqNo:11691; depth 24.43 2021-02-13T00:20:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175600.21 epoch sec; seqNo:11691; 4058907.61 north; 592372.35 east 2021-02-13T00:20:00Z- AHRS_M2 msg: 1613175600.60 epoch sec; seqNo:11596 2021-02-13T00:20:00Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.18 psi 2021-02-13T00:20:00Z- handleDepth()-Depth_Keller msg: 1613175600604 epoch sec; seqNo:11692; depth 24.32 2021-02-13T00:20:00Z- handleDvl() - RDI_Pathfinder msg: 1613175600.62 epoch sec; seqNo:5808 2021-02-13T00:20:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.30 , 89.26 , 99.00 , 66.21 2021-02-13T00:20:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.10 , 0.02 2021-02-13T00:20:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175600.64 epoch sec; seqNo:11692; 4058907.99 north; 592372.34 east 2021-02-13T00:20:01Z- AHRS_M2 msg: 1613175601.00 epoch sec; seqNo:11597 2021-02-13T00:20:01Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.18 psi 2021-02-13T00:20:01Z- handleDepth()-Depth_Keller msg: 1613175601011 epoch sec; seqNo:11693; depth 24.21 2021-02-13T00:20:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175601.03 epoch sec; seqNo:11693; 4058908.37 north; 592372.34 east 2021-02-13T00:20:01Z- AHRS_M2 msg: 1613175601.42 epoch sec; seqNo:11598 2021-02-13T00:20:01Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.17 psi 2021-02-13T00:20:01Z- handleDepth()-Depth_Keller msg: 1613175601429 epoch sec; seqNo:11694; depth 24.11 2021-02-13T00:20:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175601.45 epoch sec; seqNo:11694; 4058908.37 north; 592372.34 east 2021-02-13T00:20:01Z- AHRS_M2 msg: 1613175601.81 epoch sec; seqNo:11599 2021-02-13T00:20:01Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.16 psi 2021-02-13T00:20:01Z- handleDepth()-Depth_Keller msg: 1613175601816 epoch sec; seqNo:11695; depth 23.99 2021-02-13T00:20:01Z- handleDvl() - RDI_Pathfinder msg: 1613175601.84 epoch sec; seqNo:5809 2021-02-13T00:20:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.16 , 0.00 , 97.85 , 66.21 2021-02-13T00:20:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.07 , 0.06 2021-02-13T00:20:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175601.85 epoch sec; seqNo:11695; 4058908.75 north; 592372.33 east 2021-02-13T00:20:02Z- AHRS_M2 msg: 1613175602.22 epoch sec; seqNo:11600 2021-02-13T00:20:02Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.15 psi 2021-02-13T00:20:02Z- handleDepth()-Depth_Keller msg: 1613175602221 epoch sec; seqNo:11696; depth 23.87 2021-02-13T00:20:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175602.24 epoch sec; seqNo:11696; 4058909.14 north; 592373.55 east 2021-02-13T00:20:02Z- AHRS_M2 msg: 1613175602.62 epoch sec; seqNo:11601 2021-02-13T00:20:02Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-13T00:20:02Z- handleDepth()-Depth_Keller msg: 1613175602631 epoch sec; seqNo:11697; depth 23.79 2021-02-13T00:20:02Z- handleDvl() - RDI_Pathfinder msg: 1613175602.64 epoch sec; seqNo:5810 2021-02-13T00:20:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 88.25 , 97.99 , 66.21 2021-02-13T00:20:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.08 2021-02-13T00:20:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175602.65 epoch sec; seqNo:11697; 4058909.52 north; 592373.55 east 2021-02-13T00:20:03Z- AHRS_M2 msg: 1613175603.03 epoch sec; seqNo:11602 2021-02-13T00:20:03Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.13 psi 2021-02-13T00:20:03Z- handleDepth()-Depth_Keller msg: 1613175603034 epoch sec; seqNo:11698; depth 23.66 2021-02-13T00:20:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175603.05 epoch sec; seqNo:11698; 4058909.52 north; 592373.55 east 2021-02-13T00:20:03Z- AHRS_M2 msg: 1613175603.43 epoch sec; seqNo:11603 2021-02-13T00:20:03Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.13 psi 2021-02-13T00:20:03Z- handleDepth()-Depth_Keller msg: 1613175603437 epoch sec; seqNo:11699; depth 23.56 2021-02-13T00:20:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175603.45 epoch sec; seqNo:11699; 4058909.90 north; 592373.54 east 2021-02-13T00:20:03Z- AHRS_M2 msg: 1613175603.83 epoch sec; seqNo:11604 2021-02-13T00:20:03Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.13 psi 2021-02-13T00:20:03Z- handleDepth()-Depth_Keller msg: 1613175603837 epoch sec; seqNo:11700; depth 23.44 2021-02-13T00:20:03Z- handleDvl() - RDI_Pathfinder msg: 1613175603.85 epoch sec; seqNo:5811 2021-02-13T00:20:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 0.00 , 98.39 , 67.03 2021-02-13T00:20:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , 0.07 2021-02-13T00:20:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175603.86 epoch sec; seqNo:11700; 4058910.28 north; 592373.54 east 2021-02-13T00:20:04Z- AHRS_M2 msg: 1613175604.24 epoch sec; seqNo:11605 2021-02-13T00:20:04Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.13 psi 2021-02-13T00:20:04Z- handleDepth()-Depth_Keller msg: 1613175604249 epoch sec; seqNo:11701; depth 23.34 2021-02-13T00:20:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175604.26 epoch sec; seqNo:11701; 4058910.66 north; 592373.53 east 2021-02-13T00:20:04Z- AHRS_M2 msg: 1613175604.64 epoch sec; seqNo:11606 2021-02-13T00:20:04Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.13 psi 2021-02-13T00:20:04Z- handleDepth()-Depth_Keller msg: 1613175604649 epoch sec; seqNo:11702; depth 23.22 2021-02-13T00:20:04Z- handleDvl() - RDI_Pathfinder msg: 1613175604.66 epoch sec; seqNo:5812 2021-02-13T00:20:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 0.00 , 100.01 , 67.85 2021-02-13T00:20:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , 0.04 2021-02-13T00:20:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175604.68 epoch sec; seqNo:11702; 4058910.67 north; 592374.75 east 2021-02-13T00:20:04Z- LcmTrn::handleMessages() - handling 41 messages took 0.69 ms and 127 calls... 2021-02-13T00:20:04Z- waiting for 1613175602.82, time is 1613175604.68 2021-02-13T00:20:04Z- LcmTrn::updateTrn() equating pose 1613175604.68 and meas times 1613175604.68 2021-02-13T00:20:04Z- LcmTrn::updateTrn() >>>> pose time 1613175604.68, meas time 1613175604.68 <<<< 2021-02-13T00:20:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175604.68 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:20:04Z- TerrainNav::motionUpdate() - currEstimate = (4058911, 592374.75, 23.22) 2021-02-13T00:20:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:20:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:20:04Z- TNavBF::Projecting Measurements 2021-02-13T00:20:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:20:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:20:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:20:04Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:20:04Z- TNavFilter::3 good beams. 2021-02-13T00:20:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:20:04Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:20:04Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:20:04Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:20:04Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:20:04Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:20:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:20:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:20:04Z- TerrainMapOctree: (vehN, vehE ) = (4058910.67, 592374.75). 2021-02-13T00:20:04Z- TerrainMapOctree: Min Distance = 2998.17. 2021-02-13T00:20:04Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:20:04Z- There is no specific good beam that all particles have in common. 2021-02-13T00:20:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:20:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.71 K: 0.129 ExpectedMeasDiff: nan currMeas.covariance: 9.83 2021-02-13T00:20:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:20:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.49 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 16.00 2021-02-13T00:20:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:20:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.131 ExpectedMeasDiff: nan currMeas.covariance: 7.37 2021-02-13T00:20:04Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:20:04Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:20:04Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:20:04Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:20:04Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:20:04Z- Filter 0 No beams used in subcloud update 2021-02-13T00:20:04Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:20:04Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:20:04Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 707.9 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:20:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175604.68 sec. 2021-02-13T00:20:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:20:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:20:04Z- variances([0], [2], and psi_berg) = 707.942628 833.002297 0.000000 2021-02-13T00:20:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:20:04Z- LcmTrn::publishEstimates() - MLE : 4058910.17 592424.25 23.22 2021-02-13T00:20:04Z- LcmTrn::publishEstimates() - MMSE : 4058910.67 592374.75 23.22 2021-02-13T00:20:04Z- LcmTrn::publishEstimates() - COVAR: 707.94 833.00 1.19 0.00 2021-02-13T00:20:05Z- AHRS_M2 msg: 1613175605.04 epoch sec; seqNo:11607 2021-02-13T00:20:05Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.13 psi 2021-02-13T00:20:05Z- handleDepth()-Depth_Keller msg: 1613175605048 epoch sec; seqNo:11703; depth 23.10 2021-02-13T00:20:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175605.06 epoch sec; seqNo:11703; 4058911.05 north; 592374.75 east 2021-02-13T00:20:05Z- AHRS_M2 msg: 1613175605.46 epoch sec; seqNo:11608 2021-02-13T00:20:05Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.14 psi 2021-02-13T00:20:05Z- handleDepth()-Depth_Keller msg: 1613175605466 epoch sec; seqNo:11704; depth 23.00 2021-02-13T00:20:05Z- handleDvl() - RDI_Pathfinder msg: 1613175605.48 epoch sec; seqNo:5813 2021-02-13T00:20:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 0.00 , 98.86 , 67.85 2021-02-13T00:20:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , -0.01 2021-02-13T00:20:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175605.49 epoch sec; seqNo:11704; 4058911.43 north; 592374.75 east 2021-02-13T00:20:05Z- AHRS_M2 msg: 1613175605.86 epoch sec; seqNo:11609 2021-02-13T00:20:05Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.15 psi 2021-02-13T00:20:05Z- handleDepth()-Depth_Keller msg: 1613175605865 epoch sec; seqNo:11705; depth 22.86 2021-02-13T00:20:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175605.89 epoch sec; seqNo:11705; 4058911.43 north; 592374.75 east 2021-02-13T00:20:06Z- AHRS_M2 msg: 1613175606.26 epoch sec; seqNo:11610 2021-02-13T00:20:06Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.15 psi 2021-02-13T00:20:06Z- handleDepth()-Depth_Keller msg: 1613175606260 epoch sec; seqNo:11706; depth 22.77 2021-02-13T00:20:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175606.27 epoch sec; seqNo:11706; 4058911.81 north; 592374.74 east 2021-02-13T00:20:06Z- AHRS_M2 msg: 1613175606.66 epoch sec; seqNo:11611 2021-02-13T00:20:06Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.16 psi 2021-02-13T00:20:06Z- handleDepth()-Depth_Keller msg: 1613175606672 epoch sec; seqNo:11707; depth 22.66 2021-02-13T00:20:06Z- handleDvl() - RDI_Pathfinder msg: 1613175606.68 epoch sec; seqNo:5814 2021-02-13T00:20:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.98 , 0.00 , 100.08 , 67.44 2021-02-13T00:20:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , -0.02 2021-02-13T00:20:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175606.69 epoch sec; seqNo:11707; 4058912.18 north; 592374.74 east 2021-02-13T00:20:07Z- AHRS_M2 msg: 1613175607.06 epoch sec; seqNo:11612 2021-02-13T00:20:07Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.16 psi 2021-02-13T00:20:07Z- handleDepth()-Depth_Keller msg: 1613175607069 epoch sec; seqNo:11708; depth 22.54 2021-02-13T00:20:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175607.08 epoch sec; seqNo:11708; 4058912.20 north; 592375.96 east 2021-02-13T00:20:07Z- AHRS_M2 msg: 1613175607.47 epoch sec; seqNo:11613 2021-02-13T00:20:07Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.17 psi 2021-02-13T00:20:07Z- handleDepth()-Depth_Keller msg: 1613175607473 epoch sec; seqNo:11709; depth 22.44 2021-02-13T00:20:07Z- handleDvl() - RDI_Pathfinder msg: 1613175607.48 epoch sec; seqNo:5815 2021-02-13T00:20:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.57 , 0.00 , 99.00 , 68.67 2021-02-13T00:20:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , -0.01 2021-02-13T00:20:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175607.49 epoch sec; seqNo:11709; 4058912.58 north; 592375.95 east 2021-02-13T00:20:07Z- AHRS_M2 msg: 1613175607.87 epoch sec; seqNo:11614 2021-02-13T00:20:07Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.17 psi 2021-02-13T00:20:07Z- handleDepth()-Depth_Keller msg: 1613175607877 epoch sec; seqNo:11710; depth 22.31 2021-02-13T00:20:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175607.90 epoch sec; seqNo:11710; 4058912.96 north; 592375.95 east 2021-02-13T00:20:08Z- AHRS_M2 msg: 1613175608.28 epoch sec; seqNo:11615 2021-02-13T00:20:08Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.18 psi 2021-02-13T00:20:08Z- handleDepth()-Depth_Keller msg: 1613175608280 epoch sec; seqNo:11711; depth 22.21 2021-02-13T00:20:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175608.29 epoch sec; seqNo:11711; 4058913.33 north; 592375.95 east 2021-02-13T00:20:08Z- AHRS_M2 msg: 1613175608.68 epoch sec; seqNo:11616 2021-02-13T00:20:08Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.18 psi 2021-02-13T00:20:08Z- handleDepth()-Depth_Keller msg: 1613175608689 epoch sec; seqNo:11712; depth 22.11 2021-02-13T00:20:08Z- handleDvl() - RDI_Pathfinder msg: 1613175608.70 epoch sec; seqNo:5816 2021-02-13T00:20:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 91.43 , 100.08 , 67.85 2021-02-13T00:20:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.07 , 0.04 2021-02-13T00:20:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175608.71 epoch sec; seqNo:11712; 4058913.33 north; 592375.95 east 2021-02-13T00:20:09Z- AHRS_M2 msg: 1613175609.08 epoch sec; seqNo:11617 2021-02-13T00:20:09Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.18 psi 2021-02-13T00:20:09Z- handleDepth()-Depth_Keller msg: 1613175609088 epoch sec; seqNo:11713; depth 21.99 2021-02-13T00:20:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175609.11 epoch sec; seqNo:11713; 4058913.71 north; 592375.94 east 2021-02-13T00:20:09Z- AHRS_M2 msg: 1613175609.50 epoch sec; seqNo:11618 2021-02-13T00:20:09Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.17 psi 2021-02-13T00:20:09Z- handleDepth()-Depth_Keller msg: 1613175609504 epoch sec; seqNo:11714; depth 21.88 2021-02-13T00:20:09Z- handleDvl() - RDI_Pathfinder msg: 1613175609.52 epoch sec; seqNo:5817 2021-02-13T00:20:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.39 , 90.95 , 101.91 , 68.26 2021-02-13T00:20:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.00 , 0.07 2021-02-13T00:20:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175609.53 epoch sec; seqNo:11714; 4058914.11 north; 592377.16 east 2021-02-13T00:20:09Z- AHRS_M2 msg: 1613175609.89 epoch sec; seqNo:11619 2021-02-13T00:20:09Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.17 psi 2021-02-13T00:20:09Z- handleDepth()-Depth_Keller msg: 1613175609901 epoch sec; seqNo:11715; depth 21.76 2021-02-13T00:20:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175609.91 epoch sec; seqNo:11715; 4058914.48 north; 592377.15 east 2021-02-13T00:20:09Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:20:09Z- waiting for 1613175607.68, time is 1613175609.91 2021-02-13T00:20:09Z- LcmTrn::updateTrn() >>>> pose time 1613175609.91, meas time 1613175609.52 <<<< 2021-02-13T00:20:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175609.52 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:20:09Z- TerrainNav::motionUpdate() - currEstimate = (4058914, 592377.15, 21.76) 2021-02-13T00:20:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:20:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:20:09Z- TNavBF::Projecting Measurements 2021-02-13T00:20:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:20:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:20:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:20:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:20:09Z- TNavFilter::4 good beams. 2021-02-13T00:20:09Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:20:09Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:20:09Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:20:09Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:20:09Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:20:09Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:20:09Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:20:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:20:09Z- TerrainMapOctree: (vehN, vehE ) = (4058914.20, 592376.98). 2021-02-13T00:20:09Z- TerrainMapOctree: Min Distance = 2994.02. 2021-02-13T00:20:09Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:20:09Z- There is no specific good beam that all particles have in common. 2021-02-13T00:20:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:20:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.41 K: 0.117 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-13T00:20:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:20:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 13.24 2021-02-13T00:20:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:20:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 16.62 2021-02-13T00:20:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:20:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-13T00:20:10Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:20:10Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:20:10Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:20:10Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:20:10Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:20:10Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:20:10Z- Filter 0 No beams used in subcloud update 2021-02-13T00:20:10Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:20:10Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:20:10Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 707.9 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:20:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175609.52 sec. 2021-02-13T00:20:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:20:10Z- TNavBF::mmse selected from filter -1 2021-02-13T00:20:10Z- variances([0], [2], and psi_berg) = 707.865327 833.002253 0.000000 2021-02-13T00:20:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:20:10Z- LcmTrn::publishEstimates() - MLE : 4058913.98 592426.65 21.76 2021-02-13T00:20:10Z- LcmTrn::publishEstimates() - MMSE : 4058914.48 592377.15 21.76 2021-02-13T00:20:10Z- LcmTrn::publishEstimates() - COVAR: 707.87 833.00 0.98 0.00 2021-02-13T00:20:10Z- AHRS_M2 msg: 1613175610.30 epoch sec; seqNo:11620 2021-02-13T00:20:10Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.16 psi 2021-02-13T00:20:10Z- handleDepth()-Depth_Keller msg: 1613175610303 epoch sec; seqNo:11716; depth 21.66 2021-02-13T00:20:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175610.31 epoch sec; seqNo:11716; 4058914.48 north; 592377.15 east 2021-02-13T00:20:10Z- AHRS_M2 msg: 1613175610.70 epoch sec; seqNo:11621 2021-02-13T00:20:10Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.15 psi 2021-02-13T00:20:10Z- handleDepth()-Depth_Keller msg: 1613175610709 epoch sec; seqNo:11717; depth 21.56 2021-02-13T00:20:10Z- handleDvl() - RDI_Pathfinder msg: 1613175610.73 epoch sec; seqNo:5818 2021-02-13T00:20:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 98.32 , 101.97 , 68.67 2021-02-13T00:20:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.08 , 0.03 2021-02-13T00:20:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175610.74 epoch sec; seqNo:11717; 4058914.86 north; 592377.15 east 2021-02-13T00:20:11Z- AHRS_M2 msg: 1613175611.10 epoch sec; seqNo:11622 2021-02-13T00:20:11Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.14 psi 2021-02-13T00:20:11Z- handleDepth()-Depth_Keller msg: 1613175611113 epoch sec; seqNo:11718; depth 21.44 2021-02-13T00:20:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175611.13 epoch sec; seqNo:11718; 4058915.24 north; 592377.15 east 2021-02-13T00:20:11Z- AHRS_M2 msg: 1613175611.51 epoch sec; seqNo:11623 2021-02-13T00:20:11Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.13 psi 2021-02-13T00:20:11Z- handleDepth()-Depth_Keller msg: 1613175611518 epoch sec; seqNo:11719; depth 21.34 2021-02-13T00:20:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175611.53 epoch sec; seqNo:11719; 4058915.24 north; 592377.15 east 2021-02-13T00:20:11Z- AHRS_M2 msg: 1613175611.91 epoch sec; seqNo:11624 2021-02-13T00:20:11Z- AHRS_M2 msg: 0.69 phi; 0.32 theta; -0.12 psi 2021-02-13T00:20:11Z- handleDepth()-Depth_Keller msg: 1613175611923 epoch sec; seqNo:11720; depth 21.24 2021-02-13T00:20:11Z- handleDvl() - RDI_Pathfinder msg: 1613175611.93 epoch sec; seqNo:5819 2021-02-13T00:20:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 88.38 , 102.99 , 0.00 2021-02-13T00:20:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.07 , -0.05 2021-02-13T00:20:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175611.94 epoch sec; seqNo:11720; 4058915.63 north; 592378.36 east 2021-02-13T00:20:12Z- AHRS_M2 msg: 1613175612.32 epoch sec; seqNo:11625 2021-02-13T00:20:12Z- AHRS_M2 msg: 0.70 phi; 0.32 theta; -0.11 psi 2021-02-13T00:20:12Z- handleDepth()-Depth_Keller msg: 1613175612324 epoch sec; seqNo:11721; depth 21.12 2021-02-13T00:20:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175612.35 epoch sec; seqNo:11721; 4058916.01 north; 592378.36 east 2021-02-13T00:20:12Z- AHRS_M2 msg: 1613175612.72 epoch sec; seqNo:11626 2021-02-13T00:20:12Z- AHRS_M2 msg: 0.71 phi; 0.32 theta; -0.11 psi 2021-02-13T00:20:12Z- handleDepth()-Depth_Keller msg: 1613175612724 epoch sec; seqNo:11722; depth 21.01 2021-02-13T00:20:12Z- handleDvl() - RDI_Pathfinder msg: 1613175612.74 epoch sec; seqNo:5820 2021-02-13T00:20:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 89.26 , 102.65 , 68.67 2021-02-13T00:20:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , -0.11 2021-02-13T00:20:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175612.76 epoch sec; seqNo:11722; 4058916.01 north; 592378.36 east 2021-02-13T00:20:13Z- AHRS_M2 msg: 1613175613.13 epoch sec; seqNo:11627 2021-02-13T00:20:13Z- AHRS_M2 msg: 0.71 phi; 0.32 theta; -0.11 psi 2021-02-13T00:20:13Z- handleDepth()-Depth_Keller msg: 1613175613141 epoch sec; seqNo:11723; depth 20.90 2021-02-13T00:20:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175613.15 epoch sec; seqNo:11723; 4058916.39 north; 592378.36 east 2021-02-13T00:20:13Z- AHRS_M2 msg: 1613175613.53 epoch sec; seqNo:11628 2021-02-13T00:20:13Z- AHRS_M2 msg: 0.72 phi; 0.32 theta; -0.11 psi 2021-02-13T00:20:13Z- handleDepth()-Depth_Keller msg: 1613175613539 epoch sec; seqNo:11724; depth 20.78 2021-02-13T00:20:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175613.58 epoch sec; seqNo:11724; 4058916.39 north; 592378.36 east 2021-02-13T00:20:13Z- AHRS_M2 msg: 1613175613.93 epoch sec; seqNo:11629 2021-02-13T00:20:13Z- AHRS_M2 msg: 0.72 phi; 0.31 theta; -0.12 psi 2021-02-13T00:20:13Z- handleDepth()-Depth_Keller msg: 1613175613941 epoch sec; seqNo:11725; depth 20.67 2021-02-13T00:20:13Z- handleDvl() - RDI_Pathfinder msg: 1613175613.95 epoch sec; seqNo:5821 2021-02-13T00:20:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.71 , 131.05 , 103.06 , 0.00 2021-02-13T00:20:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.04 , 0.63 , -0.51 2021-02-13T00:20:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175613.97 epoch sec; seqNo:11725; 4058916.39 north; 592378.36 east 2021-02-13T00:20:14Z- AHRS_M2 msg: 1613175614.34 epoch sec; seqNo:11630 2021-02-13T00:20:14Z- AHRS_M2 msg: 0.71 phi; 0.31 theta; -0.13 psi 2021-02-13T00:20:14Z- handleDepth()-Depth_Keller msg: 1613175614340 epoch sec; seqNo:11726; depth 20.56 2021-02-13T00:20:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175614.35 epoch sec; seqNo:11726; 4058916.03 north; 592379.58 east 2021-02-13T00:20:14Z- AHRS_M2 msg: 1613175614.74 epoch sec; seqNo:11631 2021-02-13T00:20:14Z- AHRS_M2 msg: 0.70 phi; 0.31 theta; -0.16 psi 2021-02-13T00:20:14Z- handleDepth()-Depth_Keller msg: 1613175614754 epoch sec; seqNo:11727; depth 20.45 2021-02-13T00:20:14Z- handleDvl() - RDI_Pathfinder msg: 1613175614.77 epoch sec; seqNo:5822 2021-02-13T00:20:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 90.28 , 102.18 , 68.67 2021-02-13T00:20:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.16 , 0.02 2021-02-13T00:20:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175614.78 epoch sec; seqNo:11727; 4058916.40 north; 592379.58 east 2021-02-13T00:20:15Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-13T00:20:15Z- waiting for 1613175612.91, time is 1613175614.78 2021-02-13T00:20:15Z- LcmTrn::updateTrn() equating pose 1613175614.78 and meas times 1613175614.78 2021-02-13T00:20:15Z- LcmTrn::updateTrn() >>>> pose time 1613175614.78, meas time 1613175614.78 <<<< 2021-02-13T00:20:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175614.78 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:20:15Z- TerrainNav::motionUpdate() - currEstimate = (4058916, 592379.58, 20.45) 2021-02-13T00:20:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:20:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:20:15Z- TNavBF::Projecting Measurements 2021-02-13T00:20:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:20:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:20:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:20:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:20:15Z- TNavFilter::4 good beams. 2021-02-13T00:20:15Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:20:15Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:20:15Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:20:15Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:20:15Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:20:15Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:20:15Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:20:15Z- TNavBF::mmse selected from filter -1 2021-02-13T00:20:15Z- TerrainMapOctree: (vehN, vehE ) = (4058916.40, 592379.58). 2021-02-13T00:20:15Z- TerrainMapOctree: Min Distance = 2990.68. 2021-02-13T00:20:15Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:20:15Z- There is no specific good beam that all particles have in common. 2021-02-13T00:20:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:20:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 9.15 2021-02-13T00:20:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:20:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 13.04 2021-02-13T00:20:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:20:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 16.70 2021-02-13T00:20:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:20:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 7.54 2021-02-13T00:20:15Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:20:15Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:20:15Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:20:15Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:20:15Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:20:15Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:20:15Z- Filter 0 No beams used in subcloud update 2021-02-13T00:20:15Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:20:15Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:20:15Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 707.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:20:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175614.78 sec. 2021-02-13T00:20:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:20:15Z- TNavBF::mmse selected from filter -1 2021-02-13T00:20:15Z- variances([0], [2], and psi_berg) = 707.813534 833.002223 0.000000 2021-02-13T00:20:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:20:15Z- LcmTrn::publishEstimates() - MLE : 4058915.90 592429.08 20.45 2021-02-13T00:20:15Z- LcmTrn::publishEstimates() - MMSE : 4058916.40 592379.58 20.45 2021-02-13T00:20:15Z- LcmTrn::publishEstimates() - COVAR: 707.81 833.00 0.83 0.00 2021-02-13T00:20:15Z- AHRS_M2 msg: 1613175615.15 epoch sec; seqNo:11632 2021-02-13T00:20:15Z- AHRS_M2 msg: 0.69 phi; 0.31 theta; -0.18 psi 2021-02-13T00:20:15Z- handleDepth()-Depth_Keller msg: 1613175615151 epoch sec; seqNo:11728; depth 20.33 2021-02-13T00:20:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175615.16 epoch sec; seqNo:11728; 4058916.78 north; 592379.57 east 2021-02-13T00:20:15Z- AHRS_M2 msg: 1613175615.55 epoch sec; seqNo:11633 2021-02-13T00:20:15Z- AHRS_M2 msg: 0.68 phi; 0.31 theta; -0.20 psi 2021-02-13T00:20:15Z- handleDepth()-Depth_Keller msg: 1613175615552 epoch sec; seqNo:11729; depth 20.21 2021-02-13T00:20:15Z- handleDvl() - RDI_Pathfinder msg: 1613175615.56 epoch sec; seqNo:5823 2021-02-13T00:20:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.21 , 98.26 , 103.12 , 69.48 2021-02-13T00:20:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.10 , 0.15 2021-02-13T00:20:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175615.57 epoch sec; seqNo:11729; 4058917.16 north; 592379.57 east 2021-02-13T00:20:15Z- AHRS_M2 msg: 1613175615.95 epoch sec; seqNo:11634 2021-02-13T00:20:15Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.21 psi 2021-02-13T00:20:15Z- handleDepth()-Depth_Keller msg: 1613175615957 epoch sec; seqNo:11730; depth 20.11 2021-02-13T00:20:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175615.97 epoch sec; seqNo:11730; 4058917.16 north; 592379.57 east 2021-02-13T00:20:16Z- AHRS_M2 msg: 1613175616.36 epoch sec; seqNo:11635 2021-02-13T00:20:16Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.22 psi 2021-02-13T00:20:16Z- handleDepth()-Depth_Keller msg: 1613175616374 epoch sec; seqNo:11731; depth 20.00 2021-02-13T00:20:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175616.38 epoch sec; seqNo:11731; 4058917.54 north; 592379.56 east 2021-02-13T00:20:16Z- AHRS_M2 msg: 1613175616.76 epoch sec; seqNo:11636 2021-02-13T00:20:16Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.23 psi 2021-02-13T00:20:16Z- handleDepth()-Depth_Keller msg: 1613175616764 epoch sec; seqNo:11732; depth 19.89 2021-02-13T00:20:16Z- handleDvl() - RDI_Pathfinder msg: 1613175616.78 epoch sec; seqNo:5824 2021-02-13T00:20:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 101.71 , 71.12 2021-02-13T00:20:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , 0.19 2021-02-13T00:20:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175616.80 epoch sec; seqNo:11732; 4058917.92 north; 592379.56 east 2021-02-13T00:20:17Z- AHRS_M2 msg: 1613175617.17 epoch sec; seqNo:11637 2021-02-13T00:20:17Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.23 psi 2021-02-13T00:20:17Z- handleDepth()-Depth_Keller msg: 1613175617172 epoch sec; seqNo:11733; depth 19.77 2021-02-13T00:20:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175617.18 epoch sec; seqNo:11733; 4058918.31 north; 592380.78 east 2021-02-13T00:20:17Z- AHRS_M2 msg: 1613175617.57 epoch sec; seqNo:11638 2021-02-13T00:20:17Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.23 psi 2021-02-13T00:20:17Z- handleDepth()-Depth_Keller msg: 1613175617577 epoch sec; seqNo:11734; depth 19.69 2021-02-13T00:20:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175617.59 epoch sec; seqNo:11734; 4058918.31 north; 592380.78 east 2021-02-13T00:20:17Z- AHRS_M2 msg: 1613175617.97 epoch sec; seqNo:11639 2021-02-13T00:20:17Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.22 psi 2021-02-13T00:20:17Z- handleDepth()-Depth_Keller msg: 1613175617986 epoch sec; seqNo:11735; depth 19.58 2021-02-13T00:20:18Z- handleDvl() - RDI_Pathfinder msg: 1613175618.00 epoch sec; seqNo:5825 2021-02-13T00:20:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 97.99 , 104.07 , 78.51 2021-02-13T00:20:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.11 , 0.11 2021-02-13T00:20:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175618.01 epoch sec; seqNo:11735; 4058918.69 north; 592380.77 east 2021-02-13T00:20:18Z- AHRS_M2 msg: 1613175618.38 epoch sec; seqNo:11640 2021-02-13T00:20:18Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.21 psi 2021-02-13T00:20:18Z- handleDepth()-Depth_Keller msg: 1613175618385 epoch sec; seqNo:11736; depth 19.47 2021-02-13T00:20:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175618.42 epoch sec; seqNo:11736; 4058919.07 north; 592380.77 east 2021-02-13T00:20:18Z- AHRS_M2 msg: 1613175618.78 epoch sec; seqNo:11641 2021-02-13T00:20:18Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.21 psi 2021-02-13T00:20:18Z- handleDepth()-Depth_Keller msg: 1613175618784 epoch sec; seqNo:11737; depth 19.34 2021-02-13T00:20:18Z- handleDvl() - RDI_Pathfinder msg: 1613175618.80 epoch sec; seqNo:5826 2021-02-13T00:20:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.39 , 97.85 , 106.37 , 69.89 2021-02-13T00:20:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.12 , -0.03 2021-02-13T00:20:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175618.81 epoch sec; seqNo:11737; 4058919.07 north; 592380.77 east 2021-02-13T00:20:19Z- AHRS_M2 msg: 1613175619.18 epoch sec; seqNo:11642 2021-02-13T00:20:19Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.21 psi 2021-02-13T00:20:19Z- handleDepth()-Depth_Keller msg: 1613175619190 epoch sec; seqNo:11738; depth 19.26 2021-02-13T00:20:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175619.20 epoch sec; seqNo:11738; 4058919.46 north; 592381.98 east 2021-02-13T00:20:19Z- AHRS_M2 msg: 1613175619.59 epoch sec; seqNo:11643 2021-02-13T00:20:19Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.21 psi 2021-02-13T00:20:19Z- handleDepth()-Depth_Keller msg: 1613175619620 epoch sec; seqNo:11739; depth 19.15 2021-02-13T00:20:19Z- handleDvl() - RDI_Pathfinder msg: 1613175619.64 epoch sec; seqNo:5827 2021-02-13T00:20:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.99 , 105.29 , 71.12 2021-02-13T00:20:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.04 , -0.10 2021-02-13T00:20:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175619.65 epoch sec; seqNo:11739; 4058919.84 north; 592381.98 east 2021-02-13T00:20:19Z- AHRS_M2 msg: 1613175620.00 epoch sec; seqNo:11644 2021-02-13T00:20:19Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.20 psi 2021-02-13T00:20:20Z- handleDepth()-Depth_Keller msg: 1613175620002 epoch sec; seqNo:11740; depth 19.02 2021-02-13T00:20:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175620.03 epoch sec; seqNo:11740; 4058919.84 north; 592381.98 east 2021-02-13T00:20:20Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:20:20Z- waiting for 1613175617.78, time is 1613175620.03 2021-02-13T00:20:20Z- LcmTrn::updateTrn() >>>> pose time 1613175620.03, meas time 1613175619.64 <<<< 2021-02-13T00:20:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175619.64 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:20:20Z- TerrainNav::motionUpdate() - currEstimate = (4058920, 592381.98, 19.02) 2021-02-13T00:20:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:20:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:20:20Z- TNavBF::Projecting Measurements 2021-02-13T00:20:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:20:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:20:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:20:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:20:20Z- TNavFilter::4 good beams. 2021-02-13T00:20:20Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:20:20Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:20:20Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:20:20Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:20:20Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:20:20Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:20:20Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:20:20Z- TNavBF::mmse selected from filter -1 2021-02-13T00:20:20Z- TerrainMapOctree: (vehN, vehE ) = (4058919.59, 592381.80). 2021-02-13T00:20:20Z- TerrainMapOctree: Min Distance = 2986.80. 2021-02-13T00:20:20Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:20:20Z- There is no specific good beam that all particles have in common. 2021-02-13T00:20:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:20:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:20:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:20:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:20:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:20:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 17.74 2021-02-13T00:20:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:20:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-13T00:20:20Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:20:20Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:20:20Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:20:20Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:20:20Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:20:20Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:20:20Z- Filter 0 No beams used in subcloud update 2021-02-13T00:20:20Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:20:20Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:20:20Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 707.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:20:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175619.64 sec. 2021-02-13T00:20:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:20:20Z- TNavBF::mmse selected from filter -1 2021-02-13T00:20:20Z- variances([0], [2], and psi_berg) = 707.739120 833.002181 0.000000 2021-02-13T00:20:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:20:20Z- LcmTrn::publishEstimates() - MLE : 4058919.34 592431.48 19.02 2021-02-13T00:20:20Z- LcmTrn::publishEstimates() - MMSE : 4058919.84 592381.98 19.02 2021-02-13T00:20:20Z- LcmTrn::publishEstimates() - COVAR: 707.74 833.00 0.81 0.00 2021-02-13T00:20:20Z- AHRS_M2 msg: 1613175620.40 epoch sec; seqNo:11645 2021-02-13T00:20:20Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.19 psi 2021-02-13T00:20:20Z- handleDepth()-Depth_Keller msg: 1613175620407 epoch sec; seqNo:11741; depth 18.93 2021-02-13T00:20:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175620.42 epoch sec; seqNo:11741; 4058920.22 north; 592381.98 east 2021-02-13T00:20:20Z- AHRS_M2 msg: 1613175620.80 epoch sec; seqNo:11646 2021-02-13T00:20:20Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.19 psi 2021-02-13T00:20:20Z- handleDepth()-Depth_Keller msg: 1613175620805 epoch sec; seqNo:11742; depth 18.81 2021-02-13T00:20:20Z- handleDvl() - RDI_Pathfinder msg: 1613175620.82 epoch sec; seqNo:5828 2021-02-13T00:20:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.92 , 107.65 , 70.71 2021-02-13T00:20:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , -0.11 2021-02-13T00:20:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175620.83 epoch sec; seqNo:11742; 4058920.22 north; 592381.98 east 2021-02-13T00:20:21Z- AHRS_M2 msg: 1613175621.20 epoch sec; seqNo:11647 2021-02-13T00:20:21Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.18 psi 2021-02-13T00:20:21Z- handleDepth()-Depth_Keller msg: 1613175621213 epoch sec; seqNo:11743; depth 18.70 2021-02-13T00:20:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175621.23 epoch sec; seqNo:11743; 4058920.60 north; 592381.97 east 2021-02-13T00:20:21Z- AHRS_M2 msg: 1613175621.63 epoch sec; seqNo:11648 2021-02-13T00:20:21Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.17 psi 2021-02-13T00:20:21Z- handleDepth()-Depth_Keller msg: 1613175621631 epoch sec; seqNo:11744; depth 18.59 2021-02-13T00:20:21Z- handleDvl() - RDI_Pathfinder msg: 1613175621.65 epoch sec; seqNo:5829 2021-02-13T00:20:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 97.85 , 106.37 , 0.00 2021-02-13T00:20:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.07 , -0.07 2021-02-13T00:20:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175621.66 epoch sec; seqNo:11744; 4058920.98 north; 592381.97 east 2021-02-13T00:20:22Z- AHRS_M2 msg: 1613175622.01 epoch sec; seqNo:11649 2021-02-13T00:20:22Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.17 psi 2021-02-13T00:20:22Z- handleDepth()-Depth_Keller msg: 1613175622025 epoch sec; seqNo:11745; depth 18.47 2021-02-13T00:20:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175622.04 epoch sec; seqNo:11745; 4058921.37 north; 592383.18 east 2021-02-13T00:20:22Z- AHRS_M2 msg: 1613175622.41 epoch sec; seqNo:11650 2021-02-13T00:20:22Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-13T00:20:22Z- handleDepth()-Depth_Keller msg: 1613175622420 epoch sec; seqNo:11746; depth 18.40 2021-02-13T00:20:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175622.43 epoch sec; seqNo:11746; 4058921.37 north; 592383.18 east 2021-02-13T00:20:22Z- AHRS_M2 msg: 1613175622.82 epoch sec; seqNo:11651 2021-02-13T00:20:22Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-13T00:20:22Z- handleDepth()-Depth_Keller msg: 1613175622829 epoch sec; seqNo:11747; depth 18.29 2021-02-13T00:20:22Z- handleDvl() - RDI_Pathfinder msg: 1613175622.84 epoch sec; seqNo:5830 2021-02-13T00:20:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.16 , 106.98 , 78.58 2021-02-13T00:20:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.09 , -0.03 2021-02-13T00:20:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175622.85 epoch sec; seqNo:11747; 4058921.75 north; 592383.18 east 2021-02-13T00:20:23Z- AHRS_M2 msg: 1613175623.22 epoch sec; seqNo:11652 2021-02-13T00:20:23Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-13T00:20:23Z- handleDepth()-Depth_Keller msg: 1613175623237 epoch sec; seqNo:11748; depth 18.16 2021-02-13T00:20:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175623.25 epoch sec; seqNo:11748; 4058922.13 north; 592383.18 east 2021-02-13T00:20:23Z- AHRS_M2 msg: 1613175623.63 epoch sec; seqNo:11653 2021-02-13T00:20:23Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-13T00:20:23Z- handleDepth()-Depth_Keller msg: 1613175623639 epoch sec; seqNo:11749; depth 18.05 2021-02-13T00:20:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175623.68 epoch sec; seqNo:11749; 4058922.13 north; 592383.18 east 2021-02-13T00:20:24Z- AHRS_M2 msg: 1613175624.03 epoch sec; seqNo:11654 2021-02-13T00:20:24Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-13T00:20:24Z- handleDepth()-Depth_Keller msg: 1613175624037 epoch sec; seqNo:11750; depth 17.92 2021-02-13T00:20:24Z- handleDvl() - RDI_Pathfinder msg: 1613175624.05 epoch sec; seqNo:5831 2021-02-13T00:20:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.00 , 97.58 , 107.05 , 78.65 2021-02-13T00:20:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.10 , -0.00 2021-02-13T00:20:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175624.06 epoch sec; seqNo:11750; 4058922.51 north; 592383.17 east 2021-02-13T00:20:24Z- AHRS_M2 msg: 1613175624.43 epoch sec; seqNo:11655 2021-02-13T00:20:24Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-13T00:20:24Z- handleDepth()-Depth_Keller msg: 1613175624441 epoch sec; seqNo:11751; depth 17.83 2021-02-13T00:20:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175624.46 epoch sec; seqNo:11751; 4058922.88 north; 592383.17 east 2021-02-13T00:20:24Z- AHRS_M2 msg: 1613175624.84 epoch sec; seqNo:11656 2021-02-13T00:20:24Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T00:20:24Z- handleDepth()-Depth_Keller msg: 1613175624844 epoch sec; seqNo:11752; depth 17.72 2021-02-13T00:20:24Z- handleDvl() - RDI_Pathfinder msg: 1613175624.86 epoch sec; seqNo:5832 2021-02-13T00:20:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.46 , 97.71 , 107.45 , 71.94 2021-02-13T00:20:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.12 , 0.03 2021-02-13T00:20:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175624.87 epoch sec; seqNo:11752; 4058923.26 north; 592383.16 east 2021-02-13T00:20:25Z- AHRS_M2 msg: 1613175625.24 epoch sec; seqNo:11657 2021-02-13T00:20:25Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.18 psi 2021-02-13T00:20:25Z- handleDepth()-Depth_Keller msg: 1613175625253 epoch sec; seqNo:11753; depth 17.61 2021-02-13T00:20:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175625.26 epoch sec; seqNo:11753; 4058923.28 north; 592384.39 east 2021-02-13T00:20:25Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-13T00:20:25Z- waiting for 1613175623.03, time is 1613175625.26 2021-02-13T00:20:25Z- LcmTrn::updateTrn() >>>> pose time 1613175625.26, meas time 1613175624.86 <<<< 2021-02-13T00:20:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175624.86 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:20:25Z- TerrainNav::motionUpdate() - currEstimate = (4058923, 592384.39, 17.61) 2021-02-13T00:20:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:20:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:20:25Z- TNavBF::Projecting Measurements 2021-02-13T00:20:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:20:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:20:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:20:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:20:25Z- TNavFilter::4 good beams. 2021-02-13T00:20:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:20:25Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:20:25Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:20:25Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:20:25Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:20:25Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:20:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:20:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:20:25Z- TerrainMapOctree: (vehN, vehE ) = (4058923.01, 592384.20). 2021-02-13T00:20:25Z- TerrainMapOctree: Min Distance = 2982.63. 2021-02-13T00:20:25Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:20:25Z- There is no specific good beam that all particles have in common. 2021-02-13T00:20:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:20:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 10.10 2021-02-13T00:20:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:20:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:20:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:20:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 18.47 2021-02-13T00:20:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:20:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 8.28 2021-02-13T00:20:25Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:20:25Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:20:25Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:20:25Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:20:25Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:20:25Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:20:25Z- Filter 0 No beams used in subcloud update 2021-02-13T00:20:25Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:20:25Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:20:25Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 707.7 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:20:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175624.86 sec. 2021-02-13T00:20:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:20:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:20:25Z- variances([0], [2], and psi_berg) = 707.664457 833.002138 0.000000 2021-02-13T00:20:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:20:25Z- LcmTrn::publishEstimates() - MLE : 4058922.78 592433.89 17.61 2021-02-13T00:20:25Z- LcmTrn::publishEstimates() - MMSE : 4058923.28 592384.39 17.61 2021-02-13T00:20:25Z- LcmTrn::publishEstimates() - COVAR: 707.66 833.00 0.79 0.00 2021-02-13T00:20:25Z- AHRS_M2 msg: 1613175625.65 epoch sec; seqNo:11658 2021-02-13T00:20:25Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.18 psi 2021-02-13T00:20:25Z- handleDepth()-Depth_Keller msg: 1613175625655 epoch sec; seqNo:11754; depth 17.50 2021-02-13T00:20:25Z- handleDvl() - RDI_Pathfinder msg: 1613175625.67 epoch sec; seqNo:5833 2021-02-13T00:20:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.59 , 97.85 , 105.15 , 72.35 2021-02-13T00:20:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.15 , 0.07 2021-02-13T00:20:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175625.68 epoch sec; seqNo:11754; 4058923.66 north; 592384.38 east 2021-02-13T00:20:26Z- AHRS_M2 msg: 1613175626.05 epoch sec; seqNo:11659 2021-02-13T00:20:26Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-13T00:20:26Z- handleDepth()-Depth_Keller msg: 1613175626057 epoch sec; seqNo:11755; depth 17.39 2021-02-13T00:20:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175626.08 epoch sec; seqNo:11755; 4058924.03 north; 592384.38 east 2021-02-13T00:20:26Z- AHRS_M2 msg: 1613175626.46 epoch sec; seqNo:11660 2021-02-13T00:20:26Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-13T00:20:26Z- handleDepth()-Depth_Keller msg: 1613175626461 epoch sec; seqNo:11756; depth 17.27 2021-02-13T00:20:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175626.47 epoch sec; seqNo:11756; 4058924.41 north; 592384.37 east 2021-02-13T00:20:26Z- AHRS_M2 msg: 1613175626.86 epoch sec; seqNo:11661 2021-02-13T00:20:26Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T00:20:26Z- handleDepth()-Depth_Keller msg: 1613175626864 epoch sec; seqNo:11757; depth 17.16 2021-02-13T00:20:26Z- handleDvl() - RDI_Pathfinder msg: 1613175626.88 epoch sec; seqNo:5834 2021-02-13T00:20:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.85 , 108.81 , 71.12 2021-02-13T00:20:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.13 , 0.13 2021-02-13T00:20:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175626.89 epoch sec; seqNo:11757; 4058924.79 north; 592384.37 east 2021-02-13T00:20:27Z- AHRS_M2 msg: 1613175627.26 epoch sec; seqNo:11662 2021-02-13T00:20:27Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-13T00:20:27Z- handleDepth()-Depth_Keller msg: 1613175627281 epoch sec; seqNo:11758; depth 17.06 2021-02-13T00:20:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175627.29 epoch sec; seqNo:11758; 4058924.79 north; 592384.37 east 2021-02-13T00:20:27Z- AHRS_M2 msg: 1613175627.68 epoch sec; seqNo:11663 2021-02-13T00:20:27Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-13T00:20:27Z- handleDepth()-Depth_Keller msg: 1613175627683 epoch sec; seqNo:11759; depth 16.94 2021-02-13T00:20:27Z- handleDvl() - RDI_Pathfinder msg: 1613175627.70 epoch sec; seqNo:5835 2021-02-13T00:20:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.07 , 97.65 , 109.82 , 78.71 2021-02-13T00:20:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.03 , 0.15 2021-02-13T00:20:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175627.71 epoch sec; seqNo:11759; 4058925.17 north; 592384.36 east 2021-02-13T00:20:28Z- AHRS_M2 msg: 1613175628.07 epoch sec; seqNo:11664 2021-02-13T00:20:28Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T00:20:28Z- handleDepth()-Depth_Keller msg: 1613175628076 epoch sec; seqNo:11760; depth 16.82 2021-02-13T00:20:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175628.09 epoch sec; seqNo:11760; 4058925.56 north; 592385.58 east 2021-02-13T00:20:28Z- AHRS_M2 msg: 1613175628.49 epoch sec; seqNo:11665 2021-02-13T00:20:28Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-13T00:20:28Z- handleDepth()-Depth_Keller msg: 1613175628490 epoch sec; seqNo:11761; depth 16.72 2021-02-13T00:20:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175628.50 epoch sec; seqNo:11761; 4058925.94 north; 592385.58 east 2021-02-13T00:20:28Z- AHRS_M2 msg: 1613175628.88 epoch sec; seqNo:11666 2021-02-13T00:20:28Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-13T00:20:28Z- handleDepth()-Depth_Keller msg: 1613175628884 epoch sec; seqNo:11762; depth 16.61 2021-02-13T00:20:28Z- handleDvl() - RDI_Pathfinder msg: 1613175628.90 epoch sec; seqNo:5836 2021-02-13T00:20:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 97.65 , 112.25 , 78.17 2021-02-13T00:20:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.06 , 0.09 2021-02-13T00:20:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175628.91 epoch sec; seqNo:11762; 4058926.32 north; 592385.57 east 2021-02-13T00:20:29Z- AHRS_M2 msg: 1613175629.28 epoch sec; seqNo:11667 2021-02-13T00:20:29Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T00:20:29Z- handleDepth()-Depth_Keller msg: 1613175629293 epoch sec; seqNo:11763; depth 16.48 2021-02-13T00:20:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175629.30 epoch sec; seqNo:11763; 4058926.32 north; 592385.57 east 2021-02-13T00:20:29Z- AHRS_M2 msg: 1613175629.69 epoch sec; seqNo:11668 2021-02-13T00:20:29Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.14 psi 2021-02-13T00:20:29Z- handleDepth()-Depth_Keller msg: 1613175629697 epoch sec; seqNo:11764; depth 16.37 2021-02-13T00:20:29Z- handleDvl() - RDI_Pathfinder msg: 1613175629.71 epoch sec; seqNo:5837 2021-02-13T00:20:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 97.71 , 111.24 , 78.78 2021-02-13T00:20:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.10 , -0.02 2021-02-13T00:20:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175629.72 epoch sec; seqNo:11764; 4058926.70 north; 592385.57 east 2021-02-13T00:20:30Z- AHRS_M2 msg: 1613175630.09 epoch sec; seqNo:11669 2021-02-13T00:20:30Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.13 psi 2021-02-13T00:20:30Z- handleDepth()-Depth_Keller msg: 1613175630099 epoch sec; seqNo:11765; depth 16.27 2021-02-13T00:20:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175630.11 epoch sec; seqNo:11765; 4058927.09 north; 592386.79 east 2021-02-13T00:20:30Z- LcmTrn::handleMessages() - handling 41 messages took 0.61 ms and 129 calls... 2021-02-13T00:20:30Z- waiting for 1613175628.26, time is 1613175630.11 2021-02-13T00:20:30Z- LcmTrn::updateTrn() >>>> pose time 1613175630.11, meas time 1613175629.71 <<<< 2021-02-13T00:20:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175629.71 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:20:30Z- TerrainNav::motionUpdate() - currEstimate = (4058927, 592386.79, 16.27) 2021-02-13T00:20:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:20:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:20:30Z- TNavBF::Projecting Measurements 2021-02-13T00:20:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:20:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:20:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:20:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:20:30Z- TNavFilter::4 good beams. 2021-02-13T00:20:30Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:20:30Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:20:30Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:20:30Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:20:30Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:20:30Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:20:30Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:20:30Z- TNavBF::mmse selected from filter -1 2021-02-13T00:20:30Z- TerrainMapOctree: (vehN, vehE ) = (4058926.78, 592386.59). 2021-02-13T00:20:30Z- TerrainMapOctree: Min Distance = 2978.20. 2021-02-13T00:20:30Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:20:30Z- There is no specific good beam that all particles have in common. 2021-02-13T00:20:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:20:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 10.98 2021-02-13T00:20:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:20:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:20:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:20:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 19.80 2021-02-13T00:20:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:20:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 9.93 2021-02-13T00:20:30Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:20:30Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:20:30Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:20:30Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:20:30Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:20:30Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:20:30Z- Filter 0 No beams used in subcloud update 2021-02-13T00:20:30Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:20:30Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:20:30Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 707.6 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:20:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175629.71 sec. 2021-02-13T00:20:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:20:30Z- TNavBF::mmse selected from filter -1 2021-02-13T00:20:30Z- variances([0], [2], and psi_berg) = 707.586754 833.002094 0.000000 2021-02-13T00:20:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:20:30Z- LcmTrn::publishEstimates() - MLE : 4058926.59 592436.29 16.27 2021-02-13T00:20:30Z- LcmTrn::publishEstimates() - MMSE : 4058927.09 592386.79 16.27 2021-02-13T00:20:30Z- LcmTrn::publishEstimates() - COVAR: 707.59 833.00 0.81 0.00 2021-02-13T00:20:30Z- AHRS_M2 msg: 1613175630.53 epoch sec; seqNo:11670 2021-02-13T00:20:30Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.13 psi 2021-02-13T00:20:30Z- handleDepth()-Depth_Keller msg: 1613175630534 epoch sec; seqNo:11766; depth 16.15 2021-02-13T00:20:30Z- handleDvl() - RDI_Pathfinder msg: 1613175630.55 epoch sec; seqNo:5838 2021-02-13T00:20:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.65 , 113.88 , 73.98 2021-02-13T00:20:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.01 , -0.12 2021-02-13T00:20:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175630.56 epoch sec; seqNo:11766; 4058927.09 north; 592386.79 east 2021-02-13T00:20:30Z- AHRS_M2 msg: 1613175630.90 epoch sec; seqNo:11671 2021-02-13T00:20:30Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.13 psi 2021-02-13T00:20:30Z- handleDepth()-Depth_Keller msg: 1613175630904 epoch sec; seqNo:11767; depth 16.01 2021-02-13T00:20:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175630.91 epoch sec; seqNo:11767; 4058927.47 north; 592386.78 east 2021-02-13T00:20:31Z- AHRS_M2 msg: 1613175631.31 epoch sec; seqNo:11672 2021-02-13T00:20:31Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.13 psi 2021-02-13T00:20:31Z- handleDepth()-Depth_Keller msg: 1613175631309 epoch sec; seqNo:11768; depth 15.91 2021-02-13T00:20:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175631.32 epoch sec; seqNo:11768; 4058927.47 north; 592386.78 east 2021-02-13T00:20:31Z- AHRS_M2 msg: 1613175631.71 epoch sec; seqNo:11673 2021-02-13T00:20:31Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.14 psi 2021-02-13T00:20:31Z- handleDepth()-Depth_Keller msg: 1613175631712 epoch sec; seqNo:11769; depth 15.80 2021-02-13T00:20:31Z- handleDvl() - RDI_Pathfinder msg: 1613175631.73 epoch sec; seqNo:5839 2021-02-13T00:20:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.57 , 97.44 , 110.97 , 73.98 2021-02-13T00:20:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.12 , -0.11 2021-02-13T00:20:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175631.74 epoch sec; seqNo:11769; 4058927.85 north; 592386.78 east 2021-02-13T00:20:32Z- AHRS_M2 msg: 1613175632.11 epoch sec; seqNo:11674 2021-02-13T00:20:32Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.14 psi 2021-02-13T00:20:32Z- handleDepth()-Depth_Keller msg: 1613175632117 epoch sec; seqNo:11770; depth 15.65 2021-02-13T00:20:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175632.13 epoch sec; seqNo:11770; 4058928.23 north; 592386.77 east 2021-02-13T00:20:32Z- AHRS_M2 msg: 1613175632.52 epoch sec; seqNo:11675 2021-02-13T00:20:32Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.15 psi 2021-02-13T00:20:32Z- handleDepth()-Depth_Keller msg: 1613175632520 epoch sec; seqNo:11771; depth 15.54 2021-02-13T00:20:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175632.53 epoch sec; seqNo:11771; 4058928.61 north; 592386.77 east 2021-02-13T00:20:32Z- AHRS_M2 msg: 1613175632.92 epoch sec; seqNo:11676 2021-02-13T00:20:32Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.16 psi 2021-02-13T00:20:32Z- handleDepth()-Depth_Keller msg: 1613175632926 epoch sec; seqNo:11772; depth 15.43 2021-02-13T00:20:32Z- handleDvl() - RDI_Pathfinder msg: 1613175632.94 epoch sec; seqNo:5840 2021-02-13T00:20:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 97.65 , 112.25 , 78.17 2021-02-13T00:20:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.15 , -0.00 2021-02-13T00:20:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175632.95 epoch sec; seqNo:11772; 4058928.61 north; 592386.77 east 2021-02-13T00:20:33Z- AHRS_M2 msg: 1613175633.33 epoch sec; seqNo:11677 2021-02-13T00:20:33Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.16 psi 2021-02-13T00:20:33Z- handleDepth()-Depth_Keller msg: 1613175633331 epoch sec; seqNo:11773; depth 15.31 2021-02-13T00:20:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175633.35 epoch sec; seqNo:11773; 4058929.00 north; 592387.99 east 2021-02-13T00:20:33Z- AHRS_M2 msg: 1613175633.73 epoch sec; seqNo:11678 2021-02-13T00:20:33Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.16 psi 2021-02-13T00:20:33Z- handleDepth()-Depth_Keller msg: 1613175633736 epoch sec; seqNo:11774; depth 15.18 2021-02-13T00:20:33Z- handleDvl() - RDI_Pathfinder msg: 1613175633.75 epoch sec; seqNo:5841 2021-02-13T00:20:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 97.65 , 112.52 , 78.71 2021-02-13T00:20:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.10 , 0.08 2021-02-13T00:20:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175633.77 epoch sec; seqNo:11774; 4058929.38 north; 592387.98 east 2021-02-13T00:20:34Z- AHRS_M2 msg: 1613175634.13 epoch sec; seqNo:11679 2021-02-13T00:20:34Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.15 psi 2021-02-13T00:20:34Z- handleDepth()-Depth_Keller msg: 1613175634137 epoch sec; seqNo:11775; depth 15.06 2021-02-13T00:20:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175634.15 epoch sec; seqNo:11775; 4058929.76 north; 592387.98 east 2021-02-13T00:20:34Z- AHRS_M2 msg: 1613175634.54 epoch sec; seqNo:11680 2021-02-13T00:20:34Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.15 psi 2021-02-13T00:20:34Z- handleDepth()-Depth_Keller msg: 1613175634547 epoch sec; seqNo:11776; depth 14.96 2021-02-13T00:20:34Z- handleDvl() - RDI_Pathfinder msg: 1613175634.56 epoch sec; seqNo:5842 2021-02-13T00:20:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.65 , 112.52 , 74.39 2021-02-13T00:20:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , 0.06 2021-02-13T00:20:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175634.57 epoch sec; seqNo:11776; 4058929.76 north; 592387.98 east 2021-02-13T00:20:34Z- AHRS_M2 msg: 1613175634.94 epoch sec; seqNo:11681 2021-02-13T00:20:34Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.15 psi 2021-02-13T00:20:34Z- handleDepth()-Depth_Keller msg: 1613175634954 epoch sec; seqNo:11777; depth 14.85 2021-02-13T00:20:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175634.97 epoch sec; seqNo:11777; 4058930.14 north; 592387.97 east 2021-02-13T00:20:35Z- AHRS_M2 msg: 1613175635.34 epoch sec; seqNo:11682 2021-02-13T00:20:35Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.15 psi 2021-02-13T00:20:35Z- handleDepth()-Depth_Keller msg: 1613175635353 epoch sec; seqNo:11778; depth 14.73 2021-02-13T00:20:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175635.36 epoch sec; seqNo:11778; 4058930.51 north; 592387.97 east 2021-02-13T00:20:35Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-13T00:20:35Z- waiting for 1613175633.11, time is 1613175635.36 2021-02-13T00:20:35Z- LcmTrn::updateTrn() >>>> pose time 1613175635.36, meas time 1613175634.56 <<<< 2021-02-13T00:20:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175634.56 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:20:35Z- TerrainNav::motionUpdate() - currEstimate = (4058931, 592387.97, 14.73) 2021-02-13T00:20:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:20:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:20:35Z- TNavBF::Projecting Measurements 2021-02-13T00:20:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:20:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:20:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:20:35Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:20:35Z- TNavFilter::3 good beams. 2021-02-13T00:20:35Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:20:35Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:20:35Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:20:35Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:20:35Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:20:35Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:20:35Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:20:35Z- TNavBF::mmse selected from filter -1 2021-02-13T00:20:35Z- TerrainMapOctree: (vehN, vehE ) = (4058929.99, 592387.79). 2021-02-13T00:20:35Z- TerrainMapOctree: Min Distance = 2974.94. 2021-02-13T00:20:35Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:20:35Z- There is no specific good beam that all particles have in common. 2021-02-13T00:20:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:20:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.26 2021-02-13T00:20:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:20:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 20.26 2021-02-13T00:20:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:20:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-13T00:20:35Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:20:35Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:20:35Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:20:35Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:20:35Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:20:35Z- Filter 0 No beams used in subcloud update 2021-02-13T00:20:35Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:20:35Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:20:35Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 707.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:20:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175634.56 sec. 2021-02-13T00:20:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:20:35Z- TNavBF::mmse selected from filter -1 2021-02-13T00:20:35Z- variances([0], [2], and psi_berg) = 707.513461 833.002052 0.000000 2021-02-13T00:20:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:20:35Z- LcmTrn::publishEstimates() - MLE : 4058930.01 592437.47 14.73 2021-02-13T00:20:35Z- LcmTrn::publishEstimates() - MMSE : 4058930.51 592387.97 14.73 2021-02-13T00:20:35Z- LcmTrn::publishEstimates() - COVAR: 707.51 833.00 0.84 0.00 2021-02-13T00:20:35Z- AHRS_M2 msg: 1613175635.75 epoch sec; seqNo:11683 2021-02-13T00:20:35Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.15 psi 2021-02-13T00:20:35Z- handleDepth()-Depth_Keller msg: 1613175635754 epoch sec; seqNo:11779; depth 14.61 2021-02-13T00:20:35Z- handleDvl() - RDI_Pathfinder msg: 1613175635.77 epoch sec; seqNo:5843 2021-02-13T00:20:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.51 , 111.04 , 73.98 2021-02-13T00:20:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.00 2021-02-13T00:20:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175635.78 epoch sec; seqNo:11779; 4058930.91 north; 592389.19 east 2021-02-13T00:20:36Z- AHRS_M2 msg: 1613175636.15 epoch sec; seqNo:11684 2021-02-13T00:20:36Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.16 psi 2021-02-13T00:20:36Z- handleDepth()-Depth_Keller msg: 1613175636158 epoch sec; seqNo:11780; depth 14.49 2021-02-13T00:20:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175636.18 epoch sec; seqNo:11780; 4058930.91 north; 592389.19 east 2021-02-13T00:20:36Z- AHRS_M2 msg: 1613175636.56 epoch sec; seqNo:11685 2021-02-13T00:20:36Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.16 psi 2021-02-13T00:20:36Z- handleDepth()-Depth_Keller msg: 1613175636560 epoch sec; seqNo:11781; depth 14.38 2021-02-13T00:20:36Z- handleDvl() - RDI_Pathfinder msg: 1613175636.58 epoch sec; seqNo:5844 2021-02-13T00:20:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.22 , 112.39 , 73.57 2021-02-13T00:20:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.08 , -0.01 2021-02-13T00:20:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175636.60 epoch sec; seqNo:11781; 4058931.28 north; 592389.18 east 2021-02-13T00:20:36Z- AHRS_M2 msg: 1613175636.96 epoch sec; seqNo:11686 2021-02-13T00:20:36Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.16 psi 2021-02-13T00:20:36Z- handleDepth()-Depth_Keller msg: 1613175636969 epoch sec; seqNo:11782; depth 14.27 2021-02-13T00:20:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175636.98 epoch sec; seqNo:11782; 4058931.66 north; 592389.18 east 2021-02-13T00:20:37Z- AHRS_M2 msg: 1613175637.37 epoch sec; seqNo:11687 2021-02-13T00:20:37Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.16 psi 2021-02-13T00:20:37Z- handleDepth()-Depth_Keller msg: 1613175637375 epoch sec; seqNo:11783; depth 14.17 2021-02-13T00:20:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175637.38 epoch sec; seqNo:11783; 4058931.66 north; 592389.18 east 2021-02-13T00:20:37Z- AHRS_M2 msg: 1613175637.77 epoch sec; seqNo:11688 2021-02-13T00:20:37Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.16 psi 2021-02-13T00:20:37Z- handleDepth()-Depth_Keller msg: 1613175637777 epoch sec; seqNo:11784; depth 14.03 2021-02-13T00:20:37Z- handleDvl() - RDI_Pathfinder msg: 1613175637.80 epoch sec; seqNo:5845 2021-02-13T00:20:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.15 , 112.32 , 75.61 2021-02-13T00:20:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.12 , 0.02 2021-02-13T00:20:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175637.81 epoch sec; seqNo:11784; 4058932.04 north; 592389.17 east 2021-02-13T00:20:38Z- AHRS_M2 msg: 1613175638.17 epoch sec; seqNo:11689 2021-02-13T00:20:38Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.16 psi 2021-02-13T00:20:38Z- handleDepth()-Depth_Keller msg: 1613175638176 epoch sec; seqNo:11785; depth 13.93 2021-02-13T00:20:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175638.19 epoch sec; seqNo:11785; 4058932.42 north; 592389.17 east 2021-02-13T00:20:38Z- AHRS_M2 msg: 1613175638.58 epoch sec; seqNo:11690 2021-02-13T00:20:38Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.16 psi 2021-02-13T00:20:38Z- handleDepth()-Depth_Keller msg: 1613175638583 epoch sec; seqNo:11786; depth 13.81 2021-02-13T00:20:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175638.59 epoch sec; seqNo:11786; 4058932.81 north; 592390.39 east 2021-02-13T00:20:38Z- AHRS_M2 msg: 1613175638.98 epoch sec; seqNo:11691 2021-02-13T00:20:38Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.16 psi 2021-02-13T00:20:38Z- handleDepth()-Depth_Keller msg: 1613175638997 epoch sec; seqNo:11787; depth 13.71 2021-02-13T00:20:39Z- handleDvl() - RDI_Pathfinder msg: 1613175639.01 epoch sec; seqNo:5846 2021-02-13T00:20:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.85 , 100.08 , 113.61 , 73.57 2021-02-13T00:20:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.12 , 0.02 2021-02-13T00:20:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175639.02 epoch sec; seqNo:11787; 4058933.19 north; 592390.38 east 2021-02-13T00:20:39Z- AHRS_M2 msg: 1613175639.39 epoch sec; seqNo:11692 2021-02-13T00:20:39Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.16 psi 2021-02-13T00:20:39Z- handleDepth()-Depth_Keller msg: 1613175639393 epoch sec; seqNo:11788; depth 13.59 2021-02-13T00:20:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175639.41 epoch sec; seqNo:11788; 4058933.19 north; 592390.38 east 2021-02-13T00:20:39Z- AHRS_M2 msg: 1613175639.79 epoch sec; seqNo:11693 2021-02-13T00:20:39Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.15 psi 2021-02-13T00:20:39Z- handleDepth()-Depth_Keller msg: 1613175639792 epoch sec; seqNo:11789; depth 13.45 2021-02-13T00:20:39Z- handleDvl() - RDI_Pathfinder msg: 1613175639.80 epoch sec; seqNo:5847 2021-02-13T00:20:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.48 , 99.00 , 113.88 , 74.80 2021-02-13T00:20:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , 0.01 2021-02-13T00:20:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175639.81 epoch sec; seqNo:11789; 4058933.57 north; 592390.38 east 2021-02-13T00:20:40Z- AHRS_M2 msg: 1613175640.19 epoch sec; seqNo:11694 2021-02-13T00:20:40Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.14 psi 2021-02-13T00:20:40Z- handleDepth()-Depth_Keller msg: 1613175640197 epoch sec; seqNo:11790; depth 13.34 2021-02-13T00:20:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175640.21 epoch sec; seqNo:11790; 4058933.95 north; 592390.37 east 2021-02-13T00:20:40Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-13T00:20:40Z- waiting for 1613175638.36, time is 1613175640.21 2021-02-13T00:20:40Z- LcmTrn::updateTrn() >>>> pose time 1613175640.21, meas time 1613175639.80 <<<< 2021-02-13T00:20:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175639.80 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:20:40Z- TerrainNav::motionUpdate() - currEstimate = (4058934, 592390.37, 13.34) 2021-02-13T00:20:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:20:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:20:40Z- TNavBF::Projecting Measurements 2021-02-13T00:20:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:20:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:20:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:20:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:20:40Z- TNavFilter::4 good beams. 2021-02-13T00:20:40Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:20:40Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:20:40Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:20:40Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:20:40Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:20:40Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:20:40Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:20:40Z- TNavBF::mmse selected from filter -1 2021-02-13T00:20:40Z- TerrainMapOctree: (vehN, vehE ) = (4058933.66, 592390.17). 2021-02-13T00:20:40Z- TerrainMapOctree: Min Distance = 2970.59. 2021-02-13T00:20:40Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:20:40Z- There is no specific good beam that all particles have in common. 2021-02-13T00:20:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:20:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 11.69 2021-02-13T00:20:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:20:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 15.68 2021-02-13T00:20:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:20:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 20.75 2021-02-13T00:20:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:20:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 8.95 2021-02-13T00:20:40Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:20:40Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:20:40Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:20:40Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:20:40Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:20:40Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:20:40Z- Filter 0 No beams used in subcloud update 2021-02-13T00:20:40Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:20:40Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:20:40Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 707.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:20:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175639.80 sec. 2021-02-13T00:20:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:20:40Z- TNavBF::mmse selected from filter -1 2021-02-13T00:20:40Z- variances([0], [2], and psi_berg) = 707.438491 833.002009 0.000000 2021-02-13T00:20:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:20:40Z- LcmTrn::publishEstimates() - MLE : 4058933.45 592439.87 13.34 2021-02-13T00:20:40Z- LcmTrn::publishEstimates() - MMSE : 4058933.95 592390.37 13.34 2021-02-13T00:20:40Z- LcmTrn::publishEstimates() - COVAR: 707.44 833.00 0.83 0.00 2021-02-13T00:20:40Z- AHRS_M2 msg: 1613175640.61 epoch sec; seqNo:11695 2021-02-13T00:20:40Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.13 psi 2021-02-13T00:20:40Z- handleDepth()-Depth_Keller msg: 1613175640610 epoch sec; seqNo:11791; depth 13.23 2021-02-13T00:20:40Z- handleDvl() - RDI_Pathfinder msg: 1613175640.63 epoch sec; seqNo:5848 2021-02-13T00:20:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.48 , 99.00 , 115.23 , 74.80 2021-02-13T00:20:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.01 , -0.01 2021-02-13T00:20:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175640.64 epoch sec; seqNo:11791; 4058934.33 north; 592390.37 east 2021-02-13T00:20:41Z- AHRS_M2 msg: 1613175641.01 epoch sec; seqNo:11696 2021-02-13T00:20:41Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.13 psi 2021-02-13T00:20:41Z- handleDepth()-Depth_Keller msg: 1613175641013 epoch sec; seqNo:11792; depth 13.11 2021-02-13T00:20:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175641.02 epoch sec; seqNo:11792; 4058934.33 north; 592390.37 east 2021-02-13T00:20:41Z- AHRS_M2 msg: 1613175641.41 epoch sec; seqNo:11697 2021-02-13T00:20:41Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.13 psi 2021-02-13T00:20:41Z- handleDepth()-Depth_Keller msg: 1613175641414 epoch sec; seqNo:11793; depth 13.00 2021-02-13T00:20:41Z- handleDvl() - RDI_Pathfinder msg: 1613175641.43 epoch sec; seqNo:5849 2021-02-13T00:20:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:20:41Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:20:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175641.45 epoch sec; seqNo:11793; 4058934.72 north; 592391.59 east 2021-02-13T00:20:41Z- AHRS_M2 msg: 1613175641.81 epoch sec; seqNo:11698 2021-02-13T00:20:41Z- AHRS_M2 msg: 0.68 phi; 0.35 theta; -0.12 psi 2021-02-13T00:20:41Z- handleDepth()-Depth_Keller msg: 1613175641812 epoch sec; seqNo:11794; depth 12.89 2021-02-13T00:20:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175641.82 epoch sec; seqNo:11794; 4058935.10 north; 592391.58 east 2021-02-13T00:20:42Z- AHRS_M2 msg: 1613175642.21 epoch sec; seqNo:11699 2021-02-13T00:20:42Z- AHRS_M2 msg: 0.68 phi; 0.35 theta; -0.13 psi 2021-02-13T00:20:42Z- handleDepth()-Depth_Keller msg: 1613175642226 epoch sec; seqNo:11795; depth 12.78 2021-02-13T00:20:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175642.23 epoch sec; seqNo:11795; 4058935.10 north; 592391.58 east 2021-02-13T00:20:42Z- AHRS_M2 msg: 1613175642.62 epoch sec; seqNo:11700 2021-02-13T00:20:42Z- AHRS_M2 msg: 0.68 phi; 0.35 theta; -0.13 psi 2021-02-13T00:20:42Z- handleDepth()-Depth_Keller msg: 1613175642620 epoch sec; seqNo:11796; depth 12.66 2021-02-13T00:20:42Z- handleDvl() - RDI_Pathfinder msg: 1613175642.63 epoch sec; seqNo:5850 2021-02-13T00:20:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.76 , 97.58 , 115.16 , 75.61 2021-02-13T00:20:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.05 , 0.03 2021-02-13T00:20:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175642.66 epoch sec; seqNo:11796; 4058935.48 north; 592391.58 east 2021-02-13T00:20:43Z- AHRS_M2 msg: 1613175643.02 epoch sec; seqNo:11701 2021-02-13T00:20:43Z- AHRS_M2 msg: 0.69 phi; 0.35 theta; -0.13 psi 2021-02-13T00:20:43Z- handleDepth()-Depth_Keller msg: 1613175643029 epoch sec; seqNo:11797; depth 12.52 2021-02-13T00:20:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175643.04 epoch sec; seqNo:11797; 4058935.48 north; 592391.58 east 2021-02-13T00:20:43Z- AHRS_M2 msg: 1613175643.42 epoch sec; seqNo:11702 2021-02-13T00:20:43Z- AHRS_M2 msg: 0.69 phi; 0.35 theta; -0.13 psi 2021-02-13T00:20:43Z- handleDepth()-Depth_Keller msg: 1613175643430 epoch sec; seqNo:11798; depth 12.41 2021-02-13T00:20:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175643.44 epoch sec; seqNo:11798; 4058935.86 north; 592391.57 east 2021-02-13T00:20:43Z- AHRS_M2 msg: 1613175643.83 epoch sec; seqNo:11703 2021-02-13T00:20:43Z- AHRS_M2 msg: 0.69 phi; 0.35 theta; -0.13 psi 2021-02-13T00:20:43Z- handleDepth()-Depth_Keller msg: 1613175643832 epoch sec; seqNo:11799; depth 12.29 2021-02-13T00:20:43Z- handleDvl() - RDI_Pathfinder msg: 1613175643.85 epoch sec; seqNo:5851 2021-02-13T00:20:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.76 , 98.93 , 116.51 , 78.65 2021-02-13T00:20:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.06 , 0.05 2021-02-13T00:20:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175643.86 epoch sec; seqNo:11799; 4058935.87 north; 592392.80 east 2021-02-13T00:20:44Z- AHRS_M2 msg: 1613175644.23 epoch sec; seqNo:11704 2021-02-13T00:20:44Z- AHRS_M2 msg: 0.69 phi; 0.35 theta; -0.13 psi 2021-02-13T00:20:44Z- handleDepth()-Depth_Keller msg: 1613175644237 epoch sec; seqNo:11800; depth 12.17 2021-02-13T00:20:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175644.25 epoch sec; seqNo:11800; 4058936.25 north; 592392.79 east 2021-02-13T00:20:44Z- AHRS_M2 msg: 1613175644.64 epoch sec; seqNo:11705 2021-02-13T00:20:44Z- AHRS_M2 msg: 0.69 phi; 0.35 theta; -0.14 psi 2021-02-13T00:20:44Z- handleDepth()-Depth_Keller msg: 1613175644642 epoch sec; seqNo:11801; depth 12.05 2021-02-13T00:20:44Z- handleDvl() - RDI_Pathfinder msg: 1613175644.65 epoch sec; seqNo:5852 2021-02-13T00:20:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:20:44Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:20:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175644.66 epoch sec; seqNo:11801; 4058936.63 north; 592392.79 east 2021-02-13T00:20:45Z- AHRS_M2 msg: 1613175645.04 epoch sec; seqNo:11706 2021-02-13T00:20:45Z- AHRS_M2 msg: 0.69 phi; 0.35 theta; -0.14 psi 2021-02-13T00:20:45Z- handleDepth()-Depth_Keller msg: 1613175645047 epoch sec; seqNo:11802; depth 11.92 2021-02-13T00:20:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175645.07 epoch sec; seqNo:11802; 4058936.63 north; 592392.79 east 2021-02-13T00:20:45Z- AHRS_M2 msg: 1613175645.44 epoch sec; seqNo:11707 2021-02-13T00:20:45Z- AHRS_M2 msg: 0.69 phi; 0.36 theta; -0.15 psi 2021-02-13T00:20:45Z- handleDepth()-Depth_Keller msg: 1613175645448 epoch sec; seqNo:11803; depth 11.80 2021-02-13T00:20:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175645.46 epoch sec; seqNo:11803; 4058937.01 north; 592392.78 east 2021-02-13T00:20:45Z- LcmTrn::handleMessages() - handling 44 messages took 0.60 ms and 132 calls... 2021-02-13T00:20:45Z- waiting for 1613175643.21, time is 1613175645.46 2021-02-13T00:20:45Z- LcmTrn::updateTrn() >>>> pose time 1613175645.46, meas time 1613175644.65 <<<< 2021-02-13T00:20:45Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175644.65, ping # 0. 2021-02-13T00:20:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175644.65 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:20:45Z- TerrainNav::motionUpdate() - currEstimate = (4058937, 592392.78, 11.80) 2021-02-13T00:20:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:20:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:20:45Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:20:45Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:20:45Z- TNavBF::Projecting Measurements 2021-02-13T00:20:45Z- TNavFilter::Measurement from time = 1613175644.65 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:20:45Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:20:45Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:20:45Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:20:45Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:20:45Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:20:45Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:20:45Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:20:45Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 707.4 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:20:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:20:45Z- TNavBF::mmse selected from filter -1 2021-02-13T00:20:45Z- variances([0], [2], and psi_berg) = 707.362615 833.001966 0.000000 2021-02-13T00:20:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:20:45Z- LcmTrn::publishEstimates() - MLE : 4058936.51 592441.28 11.80 2021-02-13T00:20:45Z- LcmTrn::publishEstimates() - MMSE : 4058937.01 592392.78 11.80 2021-02-13T00:20:45Z- LcmTrn::publishEstimates() - COVAR: 707.36 833.00 1.02 0.00 2021-02-13T00:20:45Z- AHRS_M2 msg: 1613175645.85 epoch sec; seqNo:11708 2021-02-13T00:20:45Z- AHRS_M2 msg: 0.68 phi; 0.36 theta; -0.16 psi 2021-02-13T00:20:45Z- handleDepth()-Depth_Keller msg: 1613175645852 epoch sec; seqNo:11804; depth 11.67 2021-02-13T00:20:45Z- handleDvl() - RDI_Pathfinder msg: 1613175645.86 epoch sec; seqNo:5853 2021-02-13T00:20:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 100.15 , 116.38 , 78.51 2021-02-13T00:20:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.11 , 0.00 2021-02-13T00:20:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175645.88 epoch sec; seqNo:11804; 4058937.01 north; 592392.78 east 2021-02-13T00:20:46Z- AHRS_M2 msg: 1613175646.25 epoch sec; seqNo:11709 2021-02-13T00:20:46Z- AHRS_M2 msg: 0.67 phi; 0.36 theta; -0.17 psi 2021-02-13T00:20:46Z- handleDepth()-Depth_Keller msg: 1613175646257 epoch sec; seqNo:11805; depth 11.54 2021-02-13T00:20:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175646.27 epoch sec; seqNo:11805; 4058937.40 north; 592394.00 east 2021-02-13T00:20:46Z- AHRS_M2 msg: 1613175646.68 epoch sec; seqNo:11710 2021-02-13T00:20:46Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.18 psi 2021-02-13T00:20:46Z- handleDepth()-Depth_Keller msg: 1613175646685 epoch sec; seqNo:11806; depth 11.40 2021-02-13T00:20:46Z- handleDvl() - RDI_Pathfinder msg: 1613175646.70 epoch sec; seqNo:5854 2021-02-13T00:20:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 104.21 , 119.08 , 78.51 2021-02-13T00:20:46Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.15 , 0.04 2021-02-13T00:20:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175646.71 epoch sec; seqNo:11806; 4058937.78 north; 592394.00 east 2021-02-13T00:20:47Z- AHRS_M2 msg: 1613175647.06 epoch sec; seqNo:11711 2021-02-13T00:20:47Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.18 psi 2021-02-13T00:20:47Z- handleDepth()-Depth_Keller msg: 1613175647064 epoch sec; seqNo:11807; depth 11.27 2021-02-13T00:20:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175647.08 epoch sec; seqNo:11807; 4058938.16 north; 592393.99 east 2021-02-13T00:20:47Z- AHRS_M2 msg: 1613175647.46 epoch sec; seqNo:11712 2021-02-13T00:20:47Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.17 psi 2021-02-13T00:20:47Z- handleDepth()-Depth_Keller msg: 1613175647467 epoch sec; seqNo:11808; depth 11.16 2021-02-13T00:20:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175647.48 epoch sec; seqNo:11808; 4058938.54 north; 592393.99 east 2021-02-13T00:20:47Z- AHRS_M2 msg: 1613175647.87 epoch sec; seqNo:11713 2021-02-13T00:20:47Z- AHRS_M2 msg: 0.66 phi; 0.38 theta; -0.17 psi 2021-02-13T00:20:47Z- handleDepth()-Depth_Keller msg: 1613175647873 epoch sec; seqNo:11809; depth 11.04 2021-02-13T00:20:47Z- handleDvl() - RDI_Pathfinder msg: 1613175647.88 epoch sec; seqNo:5855 2021-02-13T00:20:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 109.48 , 121.65 , 79.73 2021-02-13T00:20:47Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.02 , 0.04 2021-02-13T00:20:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175647.89 epoch sec; seqNo:11809; 4058938.91 north; 592393.98 east 2021-02-13T00:20:48Z- AHRS_M2 msg: 1613175648.27 epoch sec; seqNo:11714 2021-02-13T00:20:48Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.15 psi 2021-02-13T00:20:48Z- handleDepth()-Depth_Keller msg: 1613175648276 epoch sec; seqNo:11810; depth 10.92 2021-02-13T00:20:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175648.30 epoch sec; seqNo:11810; 4058939.29 north; 592393.98 east 2021-02-13T00:20:48Z- AHRS_M2 msg: 1613175648.68 epoch sec; seqNo:11715 2021-02-13T00:20:48Z- AHRS_M2 msg: 0.67 phi; 0.37 theta; -0.14 psi 2021-02-13T00:20:48Z- handleDepth()-Depth_Keller msg: 1613175648683 epoch sec; seqNo:11811; depth 10.80 2021-02-13T00:20:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175648.69 epoch sec; seqNo:11811; 4058939.31 north; 592395.20 east 2021-02-13T00:20:49Z- AHRS_M2 msg: 1613175649.08 epoch sec; seqNo:11716 2021-02-13T00:20:49Z- AHRS_M2 msg: 0.68 phi; 0.37 theta; -0.13 psi 2021-02-13T00:20:49Z- handleDepth()-Depth_Keller msg: 1613175649085 epoch sec; seqNo:11812; depth 10.68 2021-02-13T00:20:49Z- handleDvl() - RDI_Pathfinder msg: 1613175649.10 epoch sec; seqNo:5856 2021-02-13T00:20:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 102.52 , 120.10 , 79.53 2021-02-13T00:20:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.11 , -0.02 2021-02-13T00:20:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175649.11 epoch sec; seqNo:11812; 4058939.69 north; 592395.20 east 2021-02-13T00:20:49Z- AHRS_M2 msg: 1613175649.48 epoch sec; seqNo:11717 2021-02-13T00:20:49Z- AHRS_M2 msg: 0.70 phi; 0.36 theta; -0.13 psi 2021-02-13T00:20:49Z- handleDepth()-Depth_Keller msg: 1613175649490 epoch sec; seqNo:11813; depth 10.54 2021-02-13T00:20:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175649.50 epoch sec; seqNo:11813; 4058940.06 north; 592395.19 east 2021-02-13T00:20:49Z- AHRS_M2 msg: 1613175649.89 epoch sec; seqNo:11718 2021-02-13T00:20:49Z- AHRS_M2 msg: 0.71 phi; 0.36 theta; -0.13 psi 2021-02-13T00:20:49Z- handleDepth()-Depth_Keller msg: 1613175649897 epoch sec; seqNo:11814; depth 10.42 2021-02-13T00:20:49Z- handleDvl() - RDI_Pathfinder msg: 1613175649.91 epoch sec; seqNo:5857 2021-02-13T00:20:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 102.65 , 120.23 , 79.66 2021-02-13T00:20:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.14 , -0.11 2021-02-13T00:20:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175649.93 epoch sec; seqNo:11814; 4058940.06 north; 592395.19 east 2021-02-13T00:20:50Z- AHRS_M2 msg: 1613175650.29 epoch sec; seqNo:11719 2021-02-13T00:20:50Z- AHRS_M2 msg: 0.71 phi; 0.35 theta; -0.13 psi 2021-02-13T00:20:50Z- handleDepth()-Depth_Keller msg: 1613175650296 epoch sec; seqNo:11815; depth 10.28 2021-02-13T00:20:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175650.31 epoch sec; seqNo:11815; 4058940.44 north; 592395.19 east 2021-02-13T00:20:50Z- AHRS_M2 msg: 1613175650.70 epoch sec; seqNo:11720 2021-02-13T00:20:50Z- AHRS_M2 msg: 0.71 phi; 0.35 theta; -0.15 psi 2021-02-13T00:20:50Z- LcmTrn::handleMessages() - handling 42 messages took 0.62 ms and 130 calls... 2021-02-13T00:20:50Z- waiting for 1613175648.46, time is 1613175650.70 2021-02-13T00:20:50Z- LcmTrn::updateTrn() >>>> pose time 1613175650.70, meas time 1613175649.91 <<<< 2021-02-13T00:20:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175649.91 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:20:50Z- TerrainNav::motionUpdate() - currEstimate = (4058940, 592395.19, 10.28) 2021-02-13T00:20:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:20:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:20:50Z- TNavBF::Projecting Measurements 2021-02-13T00:20:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:20:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:20:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:20:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:20:50Z- TNavFilter::4 good beams. 2021-02-13T00:20:50Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:20:50Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:20:50Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:20:50Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:20:50Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:20:50Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:20:50Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:20:50Z- TNavBF::mmse selected from filter -1 2021-02-13T00:20:50Z- TerrainMapOctree: (vehN, vehE ) = (4058939.92, 592394.82). 2021-02-13T00:20:50Z- TerrainMapOctree: Min Distance = 2962.79. 2021-02-13T00:20:50Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:20:50Z- There is no specific good beam that all particles have in common. 2021-02-13T00:20:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:20:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-13T00:20:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:20:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 16.86 2021-02-13T00:20:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:20:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 23.13 2021-02-13T00:20:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:20:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 10.15 2021-02-13T00:20:50Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:20:50Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:20:50Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:20:50Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:20:50Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:20:50Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:20:50Z- Filter 0 No beams used in subcloud update 2021-02-13T00:20:50Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:20:50Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:20:50Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 707.3 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:20:50Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175649.91 sec. 2021-02-13T00:20:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:20:50Z- TNavBF::mmse selected from filter -1 2021-02-13T00:20:50Z- variances([0], [2], and psi_berg) = 707.284014 833.001921 0.000000 2021-02-13T00:20:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:20:50Z- LcmTrn::publishEstimates() - MLE : 4058939.94 592443.69 10.28 2021-02-13T00:20:50Z- LcmTrn::publishEstimates() - MMSE : 4058940.44 592395.19 10.28 2021-02-13T00:20:50Z- LcmTrn::publishEstimates() - COVAR: 707.28 833.00 0.97 0.00 2021-02-13T00:20:50Z- handleDepth()-Depth_Keller msg: 1613175650706 epoch sec; seqNo:11816; depth 10.17 2021-02-13T00:20:50Z- handleDvl() - RDI_Pathfinder msg: 1613175650.72 epoch sec; seqNo:5858 2021-02-13T00:20:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 103.80 , 126.79 , 0.00 2021-02-13T00:20:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.08 , -0.12 2021-02-13T00:20:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175650.73 epoch sec; seqNo:11816; 4058940.44 north; 592395.19 east 2021-02-13T00:20:51Z- AHRS_M2 msg: 1613175651.10 epoch sec; seqNo:11721 2021-02-13T00:20:51Z- AHRS_M2 msg: 0.71 phi; 0.34 theta; -0.16 psi 2021-02-13T00:20:51Z- handleDepth()-Depth_Keller msg: 1613175651104 epoch sec; seqNo:11817; depth 10.01 2021-02-13T00:20:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175651.12 epoch sec; seqNo:11817; 4058940.84 north; 592396.40 east 2021-02-13T00:20:51Z- AHRS_M2 msg: 1613175651.50 epoch sec; seqNo:11722 2021-02-13T00:20:51Z- AHRS_M2 msg: 0.69 phi; 0.34 theta; -0.18 psi 2021-02-13T00:20:51Z- handleDepth()-Depth_Keller msg: 1613175651509 epoch sec; seqNo:11818; depth 9.89 2021-02-13T00:20:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175651.52 epoch sec; seqNo:11818; 4058940.84 north; 592396.40 east 2021-02-13T00:20:51Z- AHRS_M2 msg: 1613175651.91 epoch sec; seqNo:11723 2021-02-13T00:20:51Z- AHRS_M2 msg: 0.68 phi; 0.34 theta; -0.20 psi 2021-02-13T00:20:51Z- handleDepth()-Depth_Keller msg: 1613175651912 epoch sec; seqNo:11819; depth 9.76 2021-02-13T00:20:51Z- handleDvl() - RDI_Pathfinder msg: 1613175651.92 epoch sec; seqNo:5859 2021-02-13T00:20:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 107.72 , 121.11 , 79.46 2021-02-13T00:20:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.26 , 0.03 2021-02-13T00:20:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175651.93 epoch sec; seqNo:11819; 4058941.21 north; 592396.40 east 2021-02-13T00:20:52Z- AHRS_M2 msg: 1613175652.31 epoch sec; seqNo:11724 2021-02-13T00:20:52Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.21 psi 2021-02-13T00:20:52Z- handleDepth()-Depth_Keller msg: 1613175652317 epoch sec; seqNo:11820; depth 9.61 2021-02-13T00:20:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175652.33 epoch sec; seqNo:11820; 4058941.59 north; 592396.40 east 2021-02-13T00:20:52Z- AHRS_M2 msg: 1613175652.72 epoch sec; seqNo:11725 2021-02-13T00:20:52Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.22 psi 2021-02-13T00:20:52Z- handleDepth()-Depth_Keller msg: 1613175652721 epoch sec; seqNo:11821; depth 9.48 2021-02-13T00:20:52Z- handleDvl() - RDI_Pathfinder msg: 1613175652.74 epoch sec; seqNo:5860 2021-02-13T00:20:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.44 , 112.73 , 123.14 , 81.49 2021-02-13T00:20:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.15 , 0.25 2021-02-13T00:20:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175652.80 epoch sec; seqNo:11821; 4058941.97 north; 592396.39 east 2021-02-13T00:20:53Z- AHRS_M2 msg: 1613175653.12 epoch sec; seqNo:11726 2021-02-13T00:20:53Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.22 psi 2021-02-13T00:20:53Z- handleDepth()-Depth_Keller msg: 1613175653127 epoch sec; seqNo:11822; depth 9.35 2021-02-13T00:20:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175653.16 epoch sec; seqNo:11822; 4058942.35 north; 592396.39 east 2021-02-13T00:20:53Z- AHRS_M2 msg: 1613175653.52 epoch sec; seqNo:11727 2021-02-13T00:20:53Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.22 psi 2021-02-13T00:20:53Z- handleDepth()-Depth_Keller msg: 1613175653529 epoch sec; seqNo:11823; depth 9.22 2021-02-13T00:20:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175653.54 epoch sec; seqNo:11823; 4058942.35 north; 592396.39 east 2021-02-13T00:20:53Z- AHRS_M2 msg: 1613175653.93 epoch sec; seqNo:11728 2021-02-13T00:20:53Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.21 psi 2021-02-13T00:20:53Z- handleDepth()-Depth_Keller msg: 1613175653933 epoch sec; seqNo:11824; depth 9.11 2021-02-13T00:20:53Z- handleDvl() - RDI_Pathfinder msg: 1613175653.95 epoch sec; seqNo:5861 2021-02-13T00:20:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 111.37 , 123.28 , 81.62 2021-02-13T00:20:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.05 , 0.26 2021-02-13T00:20:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175653.96 epoch sec; seqNo:11824; 4058942.74 north; 592397.60 east 2021-02-13T00:20:54Z- AHRS_M2 msg: 1613175654.34 epoch sec; seqNo:11729 2021-02-13T00:20:54Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.21 psi 2021-02-13T00:20:54Z- handleDepth()-Depth_Keller msg: 1613175654346 epoch sec; seqNo:11825; depth 8.99 2021-02-13T00:20:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175654.36 epoch sec; seqNo:11825; 4058943.12 north; 592397.60 east 2021-02-13T00:20:54Z- AHRS_M2 msg: 1613175654.75 epoch sec; seqNo:11730 2021-02-13T00:20:54Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.21 psi 2021-02-13T00:20:54Z- handleDepth()-Depth_Keller msg: 1613175654777 epoch sec; seqNo:11826; depth 8.87 2021-02-13T00:20:54Z- handleDvl() - RDI_Pathfinder msg: 1613175654.79 epoch sec; seqNo:5862 2021-02-13T00:20:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.65 , 109.55 , 124.42 , 81.28 2021-02-13T00:20:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.26 , 0.09 2021-02-13T00:20:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175654.80 epoch sec; seqNo:11826; 4058943.50 north; 592397.60 east 2021-02-13T00:20:55Z- AHRS_M2 msg: 1613175655.14 epoch sec; seqNo:11731 2021-02-13T00:20:55Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.20 psi 2021-02-13T00:20:55Z- handleDepth()-Depth_Keller msg: 1613175655149 epoch sec; seqNo:11827; depth 8.74 2021-02-13T00:20:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175655.16 epoch sec; seqNo:11827; 4058943.50 north; 592397.60 east 2021-02-13T00:20:55Z- AHRS_M2 msg: 1613175655.54 epoch sec; seqNo:11732 2021-02-13T00:20:55Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.20 psi 2021-02-13T00:20:55Z- handleDepth()-Depth_Keller msg: 1613175655557 epoch sec; seqNo:11828; depth 8.65 2021-02-13T00:20:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175655.57 epoch sec; seqNo:11828; 4058943.89 north; 592398.81 east 2021-02-13T00:20:55Z- LcmTrn::handleMessages() - handling 43 messages took 0.63 ms and 131 calls... 2021-02-13T00:20:55Z- waiting for 1613175653.70, time is 1613175655.57 2021-02-13T00:20:55Z- LcmTrn::updateTrn() >>>> pose time 1613175655.57, meas time 1613175654.79 <<<< 2021-02-13T00:20:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175654.79 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:20:55Z- TerrainNav::motionUpdate() - currEstimate = (4058944, 592398.81, 8.65) 2021-02-13T00:20:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:20:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:20:55Z- TNavBF::Projecting Measurements 2021-02-13T00:20:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:20:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:20:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:20:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:20:55Z- TNavFilter::4 good beams. 2021-02-13T00:20:55Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:20:55Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:20:55Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:20:55Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:20:55Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:20:55Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:20:55Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:20:55Z- TNavBF::mmse selected from filter -1 2021-02-13T00:20:55Z- TerrainMapOctree: (vehN, vehE ) = (4058943.34, 592398.23). 2021-02-13T00:20:55Z- TerrainMapOctree: Min Distance = 2957.99. 2021-02-13T00:20:55Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:20:55Z- There is no specific good beam that all particles have in common. 2021-02-13T00:20:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:20:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 15.26 2021-02-13T00:20:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:20:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 19.20 2021-02-13T00:20:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:20:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 24.77 2021-02-13T00:20:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:20:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 10.57 2021-02-13T00:20:55Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:20:55Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:20:55Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:20:55Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:20:55Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:20:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:20:55Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:20:55Z- Filter 0 No beams used in subcloud update 2021-02-13T00:20:55Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:20:55Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:20:55Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 707.2 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:20:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175654.79 sec. 2021-02-13T00:20:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:20:55Z- TNavBF::mmse selected from filter -1 2021-02-13T00:20:55Z- variances([0], [2], and psi_berg) = 707.197832 833.001873 0.000000 2021-02-13T00:20:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:20:55Z- LcmTrn::publishEstimates() - MLE : 4058943.39 592447.31 8.65 2021-02-13T00:20:55Z- LcmTrn::publishEstimates() - MMSE : 4058943.89 592398.81 8.65 2021-02-13T00:20:55Z- LcmTrn::publishEstimates() - COVAR: 707.20 833.00 0.97 0.00 2021-02-13T00:20:55Z- AHRS_M2 msg: 1613175655.95 epoch sec; seqNo:11733 2021-02-13T00:20:55Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.19 psi 2021-02-13T00:20:55Z- handleDepth()-Depth_Keller msg: 1613175655981 epoch sec; seqNo:11829; depth 8.53 2021-02-13T00:20:56Z- handleDvl() - RDI_Pathfinder msg: 1613175656.02 epoch sec; seqNo:5863 2021-02-13T00:20:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 109.75 , 121.65 , 81.49 2021-02-13T00:20:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.21 , -0.17 2021-02-13T00:20:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175656.03 epoch sec; seqNo:11829; 4058943.89 north; 592398.81 east 2021-02-13T00:20:56Z- AHRS_M2 msg: 1613175656.35 epoch sec; seqNo:11734 2021-02-13T00:20:56Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.17 psi 2021-02-13T00:20:56Z- handleDepth()-Depth_Keller msg: 1613175656356 epoch sec; seqNo:11830; depth 8.41 2021-02-13T00:20:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175656.37 epoch sec; seqNo:11830; 4058944.27 north; 592398.81 east 2021-02-13T00:20:56Z- AHRS_M2 msg: 1613175656.76 epoch sec; seqNo:11735 2021-02-13T00:20:56Z- AHRS_M2 msg: 0.68 phi; 0.31 theta; -0.15 psi 2021-02-13T00:20:56Z- handleDepth()-Depth_Keller msg: 1613175656764 epoch sec; seqNo:11831; depth 8.32 2021-02-13T00:20:56Z- handleDvl() - RDI_Pathfinder msg: 1613175656.78 epoch sec; seqNo:5864 2021-02-13T00:20:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 109.14 , 117.26 , 82.10 2021-02-13T00:20:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.02 , -0.30 2021-02-13T00:20:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175656.79 epoch sec; seqNo:11831; 4058944.27 north; 592398.81 east 2021-02-13T00:20:57Z- AHRS_M2 msg: 1613175657.16 epoch sec; seqNo:11736 2021-02-13T00:20:57Z- AHRS_M2 msg: 0.69 phi; 0.31 theta; -0.14 psi 2021-02-13T00:20:57Z- handleDepth()-Depth_Keller msg: 1613175657166 epoch sec; seqNo:11832; depth 8.21 2021-02-13T00:20:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175657.18 epoch sec; seqNo:11832; 4058944.65 north; 592398.81 east 2021-02-13T00:20:57Z- AHRS_M2 msg: 1613175657.56 epoch sec; seqNo:11737 2021-02-13T00:20:57Z- AHRS_M2 msg: 0.70 phi; 0.31 theta; -0.13 psi 2021-02-13T00:20:57Z- handleDepth()-Depth_Keller msg: 1613175657568 epoch sec; seqNo:11833; depth 8.12 2021-02-13T00:20:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175657.59 epoch sec; seqNo:11833; 4058945.03 north; 592398.80 east 2021-02-13T00:20:57Z- AHRS_M2 msg: 1613175657.97 epoch sec; seqNo:11738 2021-02-13T00:20:57Z- AHRS_M2 msg: 0.70 phi; 0.31 theta; -0.12 psi 2021-02-13T00:20:57Z- handleDepth()-Depth_Keller msg: 1613175657973 epoch sec; seqNo:11834; depth 8.01 2021-02-13T00:20:57Z- handleDvl() - RDI_Pathfinder msg: 1613175657.98 epoch sec; seqNo:5865 2021-02-13T00:20:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 103.80 , 117.19 , 81.49 2021-02-13T00:20:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.16 , -0.20 2021-02-13T00:20:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175658.01 epoch sec; seqNo:11834; 4058945.03 north; 592398.80 east 2021-02-13T00:20:58Z- AHRS_M2 msg: 1613175658.37 epoch sec; seqNo:11739 2021-02-13T00:20:58Z- AHRS_M2 msg: 0.70 phi; 0.31 theta; -0.12 psi 2021-02-13T00:20:58Z- handleDepth()-Depth_Keller msg: 1613175658376 epoch sec; seqNo:11835; depth 7.91 2021-02-13T00:20:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175658.39 epoch sec; seqNo:11835; 4058945.42 north; 592400.02 east 2021-02-13T00:20:58Z- AHRS_M2 msg: 1613175658.79 epoch sec; seqNo:11740 2021-02-13T00:20:58Z- AHRS_M2 msg: 0.70 phi; 0.31 theta; -0.11 psi 2021-02-13T00:20:58Z- handleDepth()-Depth_Keller msg: 1613175658791 epoch sec; seqNo:11836; depth 7.82 2021-02-13T00:20:58Z- handleDvl() - RDI_Pathfinder msg: 1613175658.80 epoch sec; seqNo:5866 2021-02-13T00:20:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.28 , 103.80 , 115.97 , 82.16 2021-02-13T00:20:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.18 , 0.06 2021-02-13T00:20:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175658.81 epoch sec; seqNo:11836; 4058945.80 north; 592400.01 east 2021-02-13T00:20:59Z- AHRS_M2 msg: 1613175659.18 epoch sec; seqNo:11741 2021-02-13T00:20:59Z- AHRS_M2 msg: 0.69 phi; 0.31 theta; -0.12 psi 2021-02-13T00:20:59Z- handleDepth()-Depth_Keller msg: 1613175659184 epoch sec; seqNo:11837; depth 7.73 2021-02-13T00:20:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175659.20 epoch sec; seqNo:11837; 4058946.18 north; 592400.01 east 2021-02-13T00:20:59Z- AHRS_M2 msg: 1613175659.58 epoch sec; seqNo:11742 2021-02-13T00:20:59Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.12 psi 2021-02-13T00:20:59Z- handleDepth()-Depth_Keller msg: 1613175659593 epoch sec; seqNo:11838; depth 7.64 2021-02-13T00:20:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175659.60 epoch sec; seqNo:11838; 4058946.18 north; 592400.01 east 2021-02-13T00:20:59Z- AHRS_M2 msg: 1613175659.99 epoch sec; seqNo:11743 2021-02-13T00:20:59Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.13 psi 2021-02-13T00:20:59Z- handleDepth()-Depth_Keller msg: 1613175659992 epoch sec; seqNo:11839; depth 7.55 2021-02-13T00:21:00Z- handleDvl() - RDI_Pathfinder msg: 1613175660.00 epoch sec; seqNo:5867 2021-02-13T00:21:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.17 , 102.58 , 116.11 , 0.00 2021-02-13T00:21:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , 0.22 2021-02-13T00:21:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175660.01 epoch sec; seqNo:11839; 4058946.56 north; 592400.01 east 2021-02-13T00:21:00Z- AHRS_M2 msg: 1613175660.39 epoch sec; seqNo:11744 2021-02-13T00:21:00Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.15 psi 2021-02-13T00:21:00Z- handleDepth()-Depth_Keller msg: 1613175660398 epoch sec; seqNo:11840; depth 7.44 2021-02-13T00:21:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175660.41 epoch sec; seqNo:11840; 4058946.94 north; 592400.00 east 2021-02-13T00:21:00Z- AHRS_M2 msg: 1613175660.80 epoch sec; seqNo:11745 2021-02-13T00:21:00Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.16 psi 2021-02-13T00:21:00Z- handleDepth()-Depth_Keller msg: 1613175660802 epoch sec; seqNo:11841; depth 7.32 2021-02-13T00:21:00Z- handleDvl() - RDI_Pathfinder msg: 1613175660.83 epoch sec; seqNo:5868 2021-02-13T00:21:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.65 , 105.09 , 118.47 , 79.29 2021-02-13T00:21:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.13 , 0.10 2021-02-13T00:21:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175660.84 epoch sec; seqNo:11841; 4058946.94 north; 592400.00 east 2021-02-13T00:21:00Z- LcmTrn::handleMessages() - handling 45 messages took 0.74 ms and 130 calls... 2021-02-13T00:21:00Z- waiting for 1613175658.57, time is 1613175660.84 2021-02-13T00:21:00Z- LcmTrn::updateTrn() equating pose 1613175660.84 and meas times 1613175660.84 2021-02-13T00:21:00Z- LcmTrn::updateTrn() >>>> pose time 1613175660.84, meas time 1613175660.84 <<<< 2021-02-13T00:21:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175660.84 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:21:00Z- TerrainNav::motionUpdate() - currEstimate = (4058947, 592400.00, 7.32) 2021-02-13T00:21:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:21:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:21:00Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175660.84 sec.; No relevant navigation data available 2021-02-13T00:21:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:21:00Z- TNavBF::mmse selected from filter -1 2021-02-13T00:21:00Z- variances([0], [2], and psi_berg) = 707.144961 833.001843 0.000000 2021-02-13T00:21:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:21:00Z- LcmTrn::publishEstimates() - MLE : 4058946.44 592442.50 7.32 2021-02-13T00:21:00Z- LcmTrn::publishEstimates() - MMSE : 4058946.94 592400.00 7.32 2021-02-13T00:21:00Z- LcmTrn::publishEstimates() - COVAR: 707.14 833.00 1.14 0.00 2021-02-13T00:21:01Z- AHRS_M2 msg: 1613175661.20 epoch sec; seqNo:11746 2021-02-13T00:21:01Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.17 psi 2021-02-13T00:21:01Z- handleDepth()-Depth_Keller msg: 1613175661204 epoch sec; seqNo:11842; depth 7.22 2021-02-13T00:21:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175661.21 epoch sec; seqNo:11842; 4058947.33 north; 592401.22 east 2021-02-13T00:21:01Z- AHRS_M2 msg: 1613175661.60 epoch sec; seqNo:11747 2021-02-13T00:21:01Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.17 psi 2021-02-13T00:21:01Z- handleDepth()-Depth_Keller msg: 1613175661609 epoch sec; seqNo:11843; depth 7.13 2021-02-13T00:21:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175661.62 epoch sec; seqNo:11843; 4058947.33 north; 592401.22 east 2021-02-13T00:21:02Z- AHRS_M2 msg: 1613175662.01 epoch sec; seqNo:11748 2021-02-13T00:21:02Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.19 psi 2021-02-13T00:21:02Z- handleDepth()-Depth_Keller msg: 1613175662014 epoch sec; seqNo:11844; depth 7.03 2021-02-13T00:21:02Z- handleDvl() - RDI_Pathfinder msg: 1613175662.03 epoch sec; seqNo:5869 2021-02-13T00:21:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.03 , 107.85 , 120.02 , 82.16 2021-02-13T00:21:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.09 , -0.10 2021-02-13T00:21:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175662.04 epoch sec; seqNo:11844; 4058947.71 north; 592401.21 east 2021-02-13T00:21:02Z- AHRS_M2 msg: 1613175662.41 epoch sec; seqNo:11749 2021-02-13T00:21:02Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.19 psi 2021-02-13T00:21:02Z- handleDepth()-Depth_Keller msg: 1613175662416 epoch sec; seqNo:11845; depth 6.92 2021-02-13T00:21:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175662.44 epoch sec; seqNo:11845; 4058948.09 north; 592401.21 east 2021-02-13T00:21:02Z- AHRS_M2 msg: 1613175662.82 epoch sec; seqNo:11750 2021-02-13T00:21:02Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.19 psi 2021-02-13T00:21:02Z- handleDepth()-Depth_Keller msg: 1613175662820 epoch sec; seqNo:11846; depth 6.81 2021-02-13T00:21:02Z- handleDvl() - RDI_Pathfinder msg: 1613175662.83 epoch sec; seqNo:5870 2021-02-13T00:21:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 111.37 , 123.27 , 83.11 2021-02-13T00:21:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , -0.13 2021-02-13T00:21:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175662.86 epoch sec; seqNo:11846; 4058948.09 north; 592401.21 east 2021-02-13T00:21:03Z- AHRS_M2 msg: 1613175663.22 epoch sec; seqNo:11751 2021-02-13T00:21:03Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.19 psi 2021-02-13T00:21:03Z- handleDepth()-Depth_Keller msg: 1613175663225 epoch sec; seqNo:11847; depth 6.70 2021-02-13T00:21:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175663.24 epoch sec; seqNo:11847; 4058948.48 north; 592402.43 east 2021-02-13T00:21:03Z- AHRS_M2 msg: 1613175663.62 epoch sec; seqNo:11752 2021-02-13T00:21:03Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.19 psi 2021-02-13T00:21:03Z- handleDepth()-Depth_Keller msg: 1613175663628 epoch sec; seqNo:11848; depth 6.60 2021-02-13T00:21:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175663.64 epoch sec; seqNo:11848; 4058948.86 north; 592402.42 east 2021-02-13T00:21:04Z- AHRS_M2 msg: 1613175664.03 epoch sec; seqNo:11753 2021-02-13T00:21:04Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.19 psi 2021-02-13T00:21:04Z- handleDepth()-Depth_Keller msg: 1613175664032 epoch sec; seqNo:11849; depth 6.48 2021-02-13T00:21:04Z- handleDvl() - RDI_Pathfinder msg: 1613175664.04 epoch sec; seqNo:5871 2021-02-13T00:21:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 111.37 , 114.35 , 83.11 2021-02-13T00:21:04Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.06 , -0.07 2021-02-13T00:21:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175664.06 epoch sec; seqNo:11849; 4058949.24 north; 592402.42 east 2021-02-13T00:21:04Z- AHRS_M2 msg: 1613175664.43 epoch sec; seqNo:11754 2021-02-13T00:21:04Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.20 psi 2021-02-13T00:21:04Z- handleDepth()-Depth_Keller msg: 1613175664441 epoch sec; seqNo:11850; depth 6.37 2021-02-13T00:21:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175664.45 epoch sec; seqNo:11850; 4058949.61 north; 592402.41 east 2021-02-13T00:21:04Z- AHRS_M2 msg: 1613175664.84 epoch sec; seqNo:11755 2021-02-13T00:21:04Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.20 psi 2021-02-13T00:21:04Z- handleDepth()-Depth_Keller msg: 1613175664842 epoch sec; seqNo:11851; depth 6.26 2021-02-13T00:21:04Z- handleDvl() - RDI_Pathfinder msg: 1613175664.85 epoch sec; seqNo:5872 2021-02-13T00:21:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 111.37 , 121.79 , 84.60 2021-02-13T00:21:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.01 , 0.11 2021-02-13T00:21:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175664.86 epoch sec; seqNo:11851; 4058949.61 north; 592402.41 east 2021-02-13T00:21:05Z- AHRS_M2 msg: 1613175665.24 epoch sec; seqNo:11756 2021-02-13T00:21:05Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.20 psi 2021-02-13T00:21:05Z- handleDepth()-Depth_Keller msg: 1613175665244 epoch sec; seqNo:11852; depth 6.13 2021-02-13T00:21:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175665.26 epoch sec; seqNo:11852; 4058949.99 north; 592402.41 east 2021-02-13T00:21:05Z- AHRS_M2 msg: 1613175665.64 epoch sec; seqNo:11757 2021-02-13T00:21:05Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.20 psi 2021-02-13T00:21:05Z- handleDepth()-Depth_Keller msg: 1613175665653 epoch sec; seqNo:11853; depth 6.03 2021-02-13T00:21:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175665.66 epoch sec; seqNo:11853; 4058949.99 north; 592402.41 east 2021-02-13T00:21:05Z- LcmTrn::handleMessages() - handling 40 messages took 0.61 ms and 128 calls... 2021-02-13T00:21:05Z- waiting for 1613175663.84, time is 1613175665.66 2021-02-13T00:21:05Z- LcmTrn::updateTrn() >>>> pose time 1613175665.66, meas time 1613175664.85 <<<< 2021-02-13T00:21:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175664.85 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:21:05Z- TerrainNav::motionUpdate() - currEstimate = (4058950, 592402.41, 6.03) 2021-02-13T00:21:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:21:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:21:05Z- TNavBF::Projecting Measurements 2021-02-13T00:21:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:21:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:21:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:21:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:21:05Z- TNavFilter::4 good beams. 2021-02-13T00:21:05Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:21:05Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:21:05Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:21:05Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:21:05Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:21:05Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:21:05Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:21:05Z- TNavBF::mmse selected from filter -1 2021-02-13T00:21:05Z- TerrainMapOctree: (vehN, vehE ) = (4058949.48, 592402.01). 2021-02-13T00:21:05Z- TerrainMapOctree: Min Distance = 2950.84. 2021-02-13T00:21:05Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:21:05Z- There is no specific good beam that all particles have in common. 2021-02-13T00:21:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:21:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:21:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:21:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 19.85 2021-02-13T00:21:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:21:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 23.73 2021-02-13T00:21:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:21:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 11.45 2021-02-13T00:21:05Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:21:05Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:21:05Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:21:05Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:21:05Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:21:05Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:21:05Z- Filter 0 No beams used in subcloud update 2021-02-13T00:21:05Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:21:05Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:21:05Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 707.1 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:21:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175664.85 sec. 2021-02-13T00:21:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:21:05Z- TNavBF::mmse selected from filter -1 2021-02-13T00:21:05Z- variances([0], [2], and psi_berg) = 707.068745 833.001799 0.000000 2021-02-13T00:21:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:21:05Z- LcmTrn::publishEstimates() - MLE : 4058949.49 592450.91 6.03 2021-02-13T00:21:05Z- LcmTrn::publishEstimates() - MMSE : 4058949.99 592402.41 6.03 2021-02-13T00:21:05Z- LcmTrn::publishEstimates() - COVAR: 707.07 833.00 1.05 0.00 2021-02-13T00:21:06Z- AHRS_M2 msg: 1613175666.05 epoch sec; seqNo:11758 2021-02-13T00:21:06Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.20 psi 2021-02-13T00:21:06Z- handleDepth()-Depth_Keller msg: 1613175666052 epoch sec; seqNo:11854; depth 5.94 2021-02-13T00:21:06Z- handleDvl() - RDI_Pathfinder msg: 1613175666.06 epoch sec; seqNo:5873 2021-02-13T00:21:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.34 , 111.16 , 120.09 , 84.39 2021-02-13T00:21:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.06 , 0.10 2021-02-13T00:21:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175666.07 epoch sec; seqNo:11854; 4058950.39 north; 592403.63 east 2021-02-13T00:21:06Z- AHRS_M2 msg: 1613175666.46 epoch sec; seqNo:11759 2021-02-13T00:21:06Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.21 psi 2021-02-13T00:21:06Z- handleDepth()-Depth_Keller msg: 1613175666469 epoch sec; seqNo:11855; depth 5.83 2021-02-13T00:21:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175666.48 epoch sec; seqNo:11855; 4058950.76 north; 592403.62 east 2021-02-13T00:21:06Z- AHRS_M2 msg: 1613175666.86 epoch sec; seqNo:11760 2021-02-13T00:21:06Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.22 psi 2021-02-13T00:21:06Z- handleDepth()-Depth_Keller msg: 1613175666860 epoch sec; seqNo:11856; depth 5.71 2021-02-13T00:21:06Z- handleDvl() - RDI_Pathfinder msg: 1613175666.88 epoch sec; seqNo:5874 2021-02-13T00:21:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.54 , 112.85 , 121.78 , 84.59 2021-02-13T00:21:06Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.00 , 0.03 2021-02-13T00:21:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175666.89 epoch sec; seqNo:11856; 4058951.14 north; 592403.62 east 2021-02-13T00:21:07Z- AHRS_M2 msg: 1613175667.26 epoch sec; seqNo:11761 2021-02-13T00:21:07Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.21 psi 2021-02-13T00:21:07Z- handleDepth()-Depth_Keller msg: 1613175667269 epoch sec; seqNo:11857; depth 5.60 2021-02-13T00:21:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175667.28 epoch sec; seqNo:11857; 4058951.14 north; 592403.62 east 2021-02-13T00:21:07Z- AHRS_M2 msg: 1613175667.66 epoch sec; seqNo:11762 2021-02-13T00:21:07Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.21 psi 2021-02-13T00:21:07Z- handleDepth()-Depth_Keller msg: 1613175667668 epoch sec; seqNo:11858; depth 5.50 2021-02-13T00:21:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175667.68 epoch sec; seqNo:11858; 4058951.52 north; 592403.61 east 2021-02-13T00:21:08Z- AHRS_M2 msg: 1613175668.07 epoch sec; seqNo:11763 2021-02-13T00:21:08Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.20 psi 2021-02-13T00:21:08Z- handleDepth()-Depth_Keller msg: 1613175668074 epoch sec; seqNo:11859; depth 5.40 2021-02-13T00:21:08Z- handleDvl() - RDI_Pathfinder msg: 1613175668.09 epoch sec; seqNo:5875 2021-02-13T00:21:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.91 , 114.27 , 126.17 , 84.53 2021-02-13T00:21:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.05 , -0.01 2021-02-13T00:21:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175668.10 epoch sec; seqNo:11859; 4058951.91 north; 592404.83 east 2021-02-13T00:21:08Z- AHRS_M2 msg: 1613175668.47 epoch sec; seqNo:11764 2021-02-13T00:21:08Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.20 psi 2021-02-13T00:21:08Z- handleDepth()-Depth_Keller msg: 1613175668476 epoch sec; seqNo:11860; depth 5.27 2021-02-13T00:21:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175668.50 epoch sec; seqNo:11860; 4058952.29 north; 592404.83 east 2021-02-13T00:21:08Z- AHRS_M2 msg: 1613175668.88 epoch sec; seqNo:11765 2021-02-13T00:21:08Z- AHRS_M2 msg: 0.61 phi; 0.27 theta; -0.18 psi 2021-02-13T00:21:08Z- handleDepth()-Depth_Keller msg: 1613175668880 epoch sec; seqNo:11861; depth 5.16 2021-02-13T00:21:08Z- handleDvl() - RDI_Pathfinder msg: 1613175668.89 epoch sec; seqNo:5876 2021-02-13T00:21:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.64 , 111.03 , 121.44 , 85.74 2021-02-13T00:21:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.17 , -0.07 2021-02-13T00:21:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175668.92 epoch sec; seqNo:11861; 4058952.29 north; 592404.83 east 2021-02-13T00:21:09Z- AHRS_M2 msg: 1613175669.28 epoch sec; seqNo:11766 2021-02-13T00:21:09Z- AHRS_M2 msg: 0.62 phi; 0.24 theta; -0.16 psi 2021-02-13T00:21:09Z- handleDepth()-Depth_Keller msg: 1613175669284 epoch sec; seqNo:11862; depth 5.05 2021-02-13T00:21:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175669.30 epoch sec; seqNo:11862; 4058952.67 north; 592404.82 east 2021-02-13T00:21:09Z- AHRS_M2 msg: 1613175669.68 epoch sec; seqNo:11767 2021-02-13T00:21:09Z- AHRS_M2 msg: 0.64 phi; 0.20 theta; -0.15 psi 2021-02-13T00:21:09Z- handleDepth()-Depth_Keller msg: 1613175669689 epoch sec; seqNo:11863; depth 4.94 2021-02-13T00:21:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175669.70 epoch sec; seqNo:11863; 4058953.05 north; 592404.82 east 2021-02-13T00:21:10Z- AHRS_M2 msg: 1613175670.09 epoch sec; seqNo:11768 2021-02-13T00:21:10Z- AHRS_M2 msg: 0.66 phi; 0.16 theta; -0.14 psi 2021-02-13T00:21:10Z- handleDepth()-Depth_Keller msg: 1613175670092 epoch sec; seqNo:11864; depth 4.85 2021-02-13T00:21:10Z- handleDvl() - RDI_Pathfinder msg: 1613175670.11 epoch sec; seqNo:5877 2021-02-13T00:21:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 108.19 , 112.65 , 85.88 2021-02-13T00:21:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.21 , -0.19 2021-02-13T00:21:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175670.13 epoch sec; seqNo:11864; 4058953.06 north; 592406.04 east 2021-02-13T00:21:10Z- AHRS_M2 msg: 1613175670.49 epoch sec; seqNo:11769 2021-02-13T00:21:10Z- AHRS_M2 msg: 0.68 phi; 0.12 theta; -0.14 psi 2021-02-13T00:21:10Z- handleDepth()-Depth_Keller msg: 1613175670501 epoch sec; seqNo:11865; depth 4.73 2021-02-13T00:21:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175670.51 epoch sec; seqNo:11865; 4058953.44 north; 592406.04 east 2021-02-13T00:21:10Z- AHRS_M2 msg: 1613175670.90 epoch sec; seqNo:11770 2021-02-13T00:21:10Z- AHRS_M2 msg: 0.69 phi; 0.07 theta; -0.15 psi 2021-02-13T00:21:10Z- handleDepth()-Depth_Keller msg: 1613175670902 epoch sec; seqNo:11866; depth 4.64 2021-02-13T00:21:10Z- handleDvl() - RDI_Pathfinder msg: 1613175670.92 epoch sec; seqNo:5878 2021-02-13T00:21:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.98 , 102.44 , 102.44 , 97.98 2021-02-13T00:21:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.02 , -0.35 2021-02-13T00:21:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175670.93 epoch sec; seqNo:11866; 4058953.44 north; 592406.04 east 2021-02-13T00:21:11Z- LcmTrn::handleMessages() - handling 45 messages took 0.68 ms and 132 calls... 2021-02-13T00:21:11Z- waiting for 1613175668.66, time is 1613175670.93 2021-02-13T00:21:11Z- LcmTrn::updateTrn() equating pose 1613175670.93 and meas times 1613175670.93 2021-02-13T00:21:11Z- LcmTrn::updateTrn() >>>> pose time 1613175670.93, meas time 1613175670.93 <<<< 2021-02-13T00:21:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175670.93 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:21:11Z- TerrainNav::motionUpdate() - currEstimate = (4058953, 592406.04, 4.64) 2021-02-13T00:21:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:21:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:21:11Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175670.93 sec.; No relevant navigation data available 2021-02-13T00:21:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:21:11Z- TNavBF::mmse selected from filter -1 2021-02-13T00:21:11Z- variances([0], [2], and psi_berg) = 707.003408 833.001762 0.000000 2021-02-13T00:21:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:21:11Z- LcmTrn::publishEstimates() - MLE : 4058952.94 592454.54 4.64 2021-02-13T00:21:11Z- LcmTrn::publishEstimates() - MMSE : 4058953.44 592406.04 4.64 2021-02-13T00:21:11Z- LcmTrn::publishEstimates() - COVAR: 707.00 833.00 1.30 0.00 2021-02-13T00:21:11Z- AHRS_M2 msg: 1613175671.30 epoch sec; seqNo:11771 2021-02-13T00:21:11Z- AHRS_M2 msg: 0.70 phi; 0.01 theta; -0.18 psi 2021-02-13T00:21:11Z- handleDepth()-Depth_Keller msg: 1613175671306 epoch sec; seqNo:11867; depth 4.56 2021-02-13T00:21:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175671.34 epoch sec; seqNo:11867; 4058953.82 north; 592406.03 east 2021-02-13T00:21:11Z- AHRS_M2 msg: 1613175671.71 epoch sec; seqNo:11772 2021-02-13T00:21:11Z- AHRS_M2 msg: 0.72 phi; -0.08 theta; -0.23 psi 2021-02-13T00:21:11Z- handleDepth()-Depth_Keller msg: 1613175671721 epoch sec; seqNo:11868; depth 4.49 2021-02-13T00:21:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175671.74 epoch sec; seqNo:11868; 4058954.20 north; 592406.03 east 2021-02-13T00:21:12Z- AHRS_M2 msg: 1613175672.11 epoch sec; seqNo:11773 2021-02-13T00:21:12Z- AHRS_M2 msg: 0.73 phi; -0.11 theta; -0.24 psi 2021-02-13T00:21:12Z- handleDepth()-Depth_Keller msg: 1613175672122 epoch sec; seqNo:11869; depth 4.43 2021-02-13T00:21:12Z- handleDvl() - RDI_Pathfinder msg: 1613175672.14 epoch sec; seqNo:5879 2021-02-13T00:21:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 105.15 , 97.03 , 97.03 2021-02-13T00:21:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , -0.38 , -0.15 2021-02-13T00:21:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175672.17 epoch sec; seqNo:11869; 4058954.20 north; 592406.03 east 2021-02-13T00:21:12Z- AHRS_M2 msg: 1613175672.51 epoch sec; seqNo:11774 2021-02-13T00:21:12Z- AHRS_M2 msg: 0.73 phi; -0.15 theta; -0.23 psi 2021-02-13T00:21:12Z- handleDepth()-Depth_Keller msg: 1613175672525 epoch sec; seqNo:11870; depth 4.39 2021-02-13T00:21:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175672.54 epoch sec; seqNo:11870; 4058954.58 north; 592406.02 east 2021-02-13T00:21:12Z- AHRS_M2 msg: 1613175672.92 epoch sec; seqNo:11775 2021-02-13T00:21:12Z- AHRS_M2 msg: 0.73 phi; -0.18 theta; -0.21 psi 2021-02-13T00:21:12Z- handleDepth()-Depth_Keller msg: 1613175672920 epoch sec; seqNo:11871; depth 4.38 2021-02-13T00:21:12Z- handleDvl() - RDI_Pathfinder msg: 1613175672.94 epoch sec; seqNo:5880 2021-02-13T00:21:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 112.25 , 97.37 , 104.13 2021-02-13T00:21:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , -0.29 , 0.36 2021-02-13T00:21:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175672.95 epoch sec; seqNo:11871; 4058954.96 north; 592406.02 east 2021-02-13T00:21:13Z- AHRS_M2 msg: 1613175673.32 epoch sec; seqNo:11776 2021-02-13T00:21:13Z- AHRS_M2 msg: 0.73 phi; -0.21 theta; -0.19 psi 2021-02-13T00:21:13Z- handleDepth()-Depth_Keller msg: 1613175673324 epoch sec; seqNo:11872; depth 4.38 2021-02-13T00:21:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175673.34 epoch sec; seqNo:11872; 4058954.96 north; 592406.02 east 2021-02-13T00:21:13Z- AHRS_M2 msg: 1613175673.73 epoch sec; seqNo:11777 2021-02-13T00:21:13Z- AHRS_M2 msg: 0.72 phi; -0.22 theta; -0.18 psi 2021-02-13T00:21:13Z- handleDepth()-Depth_Keller msg: 1613175673734 epoch sec; seqNo:11873; depth 4.39 2021-02-13T00:21:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175673.74 epoch sec; seqNo:11873; 4058955.34 north; 592406.02 east 2021-02-13T00:21:14Z- AHRS_M2 msg: 1613175674.13 epoch sec; seqNo:11778 2021-02-13T00:21:14Z- AHRS_M2 msg: 0.72 phi; -0.23 theta; -0.18 psi 2021-02-13T00:21:14Z- handleDepth()-Depth_Keller msg: 1613175674148 epoch sec; seqNo:11874; depth 4.42 2021-02-13T00:21:14Z- handleDvl() - RDI_Pathfinder msg: 1613175674.16 epoch sec; seqNo:5881 2021-02-13T00:21:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 108.12 , 97.71 , 109.61 2021-02-13T00:21:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , 0.00 , 0.38 2021-02-13T00:21:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175674.17 epoch sec; seqNo:11874; 4058955.35 north; 592407.24 east 2021-02-13T00:21:14Z- AHRS_M2 msg: 1613175674.53 epoch sec; seqNo:11779 2021-02-13T00:21:14Z- AHRS_M2 msg: 0.72 phi; -0.25 theta; -0.18 psi 2021-02-13T00:21:14Z- handleDepth()-Depth_Keller msg: 1613175674536 epoch sec; seqNo:11875; depth 4.47 2021-02-13T00:21:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175674.55 epoch sec; seqNo:11875; 4058955.73 north; 592407.23 east 2021-02-13T00:21:14Z- AHRS_M2 msg: 1613175674.94 epoch sec; seqNo:11780 2021-02-13T00:21:14Z- AHRS_M2 msg: 0.72 phi; -0.27 theta; -0.17 psi 2021-02-13T00:21:14Z- handleDepth()-Depth_Keller msg: 1613175674940 epoch sec; seqNo:11876; depth 4.52 2021-02-13T00:21:14Z- handleDvl() - RDI_Pathfinder msg: 1613175674.95 epoch sec; seqNo:5882 2021-02-13T00:21:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.41 , 106.77 , 85.95 , 114.21 2021-02-13T00:21:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.13 , 0.27 2021-02-13T00:21:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175674.96 epoch sec; seqNo:11876; 4058955.73 north; 592407.23 east 2021-02-13T00:21:15Z- AHRS_M2 msg: 1613175675.34 epoch sec; seqNo:11781 2021-02-13T00:21:15Z- AHRS_M2 msg: 0.71 phi; -0.28 theta; -0.17 psi 2021-02-13T00:21:15Z- handleDepth()-Depth_Keller msg: 1613175675349 epoch sec; seqNo:11877; depth 4.58 2021-02-13T00:21:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175675.36 epoch sec; seqNo:11877; 4058956.11 north; 592407.23 east 2021-02-13T00:21:15Z- AHRS_M2 msg: 1613175675.74 epoch sec; seqNo:11782 2021-02-13T00:21:15Z- AHRS_M2 msg: 0.70 phi; -0.29 theta; -0.19 psi 2021-02-13T00:21:15Z- handleDepth()-Depth_Keller msg: 1613175675748 epoch sec; seqNo:11878; depth 4.64 2021-02-13T00:21:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175675.77 epoch sec; seqNo:11878; 4058956.11 north; 592407.23 east 2021-02-13T00:21:16Z- LcmTrn::handleMessages() - handling 40 messages took 0.68 ms and 127 calls... 2021-02-13T00:21:16Z- waiting for 1613175673.93, time is 1613175675.77 2021-02-13T00:21:16Z- LcmTrn::updateTrn() >>>> pose time 1613175675.77, meas time 1613175674.95 <<<< 2021-02-13T00:21:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175674.95 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:21:16Z- TerrainNav::motionUpdate() - currEstimate = (4058956, 592407.23, 4.64) 2021-02-13T00:21:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:21:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:21:16Z- TNavBF::Projecting Measurements 2021-02-13T00:21:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:21:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:21:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:21:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:21:16Z- TNavFilter::4 good beams. 2021-02-13T00:21:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:21:16Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:21:16Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:21:16Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:21:16Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:21:16Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:21:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:21:16Z- TNavBF::mmse selected from filter -1 2021-02-13T00:21:16Z- TerrainMapOctree: (vehN, vehE ) = (4058955.66, 592407.03). 2021-02-13T00:21:16Z- TerrainMapOctree: Min Distance = 2942.88. 2021-02-13T00:21:16Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:21:16Z- There is no specific good beam that all particles have in common. 2021-02-13T00:21:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:21:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 13.08 2021-02-13T00:21:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:21:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 18.24 2021-02-13T00:21:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:21:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 11.82 2021-02-13T00:21:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:21:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 20.87 2021-02-13T00:21:16Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:21:16Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:21:16Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:21:16Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:21:16Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:21:16Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:21:16Z- Filter 0 No beams used in subcloud update 2021-02-13T00:21:16Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:21:16Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:21:16Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 707.0 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:21:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175674.95 sec. 2021-02-13T00:21:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:21:16Z- TNavBF::mmse selected from filter -1 2021-02-13T00:21:16Z- variances([0], [2], and psi_berg) = 706.937446 833.001725 0.000000 2021-02-13T00:21:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:21:16Z- LcmTrn::publishEstimates() - MLE : 4058955.61 592455.73 4.64 2021-02-13T00:21:16Z- LcmTrn::publishEstimates() - MMSE : 4058956.11 592407.23 4.64 2021-02-13T00:21:16Z- LcmTrn::publishEstimates() - COVAR: 706.94 833.00 1.09 0.00 2021-02-13T00:21:16Z- AHRS_M2 msg: 1613175676.15 epoch sec; seqNo:11783 2021-02-13T00:21:16Z- AHRS_M2 msg: 0.69 phi; -0.30 theta; -0.21 psi 2021-02-13T00:21:16Z- handleDepth()-Depth_Keller msg: 1613175676156 epoch sec; seqNo:11879; depth 4.72 2021-02-13T00:21:16Z- handleDvl() - RDI_Pathfinder msg: 1613175676.17 epoch sec; seqNo:5883 2021-02-13T00:21:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.69 , 107.92 , 84.93 , 121.44 2021-02-13T00:21:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.25 , 0.22 2021-02-13T00:21:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175676.19 epoch sec; seqNo:11879; 4058956.12 north; 592408.45 east 2021-02-13T00:21:16Z- AHRS_M2 msg: 1613175676.55 epoch sec; seqNo:11784 2021-02-13T00:21:16Z- AHRS_M2 msg: 0.68 phi; -0.30 theta; -0.22 psi 2021-02-13T00:21:16Z- handleDepth()-Depth_Keller msg: 1613175676556 epoch sec; seqNo:11880; depth 4.80 2021-02-13T00:21:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175676.57 epoch sec; seqNo:11880; 4058956.50 north; 592408.44 east 2021-02-13T00:21:16Z- AHRS_M2 msg: 1613175676.96 epoch sec; seqNo:11785 2021-02-13T00:21:16Z- AHRS_M2 msg: 0.68 phi; -0.30 theta; -0.23 psi 2021-02-13T00:21:16Z- handleDepth()-Depth_Keller msg: 1613175676962 epoch sec; seqNo:11881; depth 4.88 2021-02-13T00:21:16Z- handleDvl() - RDI_Pathfinder msg: 1613175676.97 epoch sec; seqNo:5884 2021-02-13T00:21:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.98 , 111.37 , 84.59 , 121.78 2021-02-13T00:21:16Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.34 , 0.00 2021-02-13T00:21:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175676.98 epoch sec; seqNo:11881; 4058956.50 north; 592408.44 east 2021-02-13T00:21:17Z- AHRS_M2 msg: 1613175677.36 epoch sec; seqNo:11786 2021-02-13T00:21:17Z- AHRS_M2 msg: 0.68 phi; -0.29 theta; -0.24 psi 2021-02-13T00:21:17Z- handleDepth()-Depth_Keller msg: 1613175677364 epoch sec; seqNo:11882; depth 4.99 2021-02-13T00:21:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175677.38 epoch sec; seqNo:11882; 4058956.88 north; 592408.44 east 2021-02-13T00:21:17Z- AHRS_M2 msg: 1613175677.77 epoch sec; seqNo:11787 2021-02-13T00:21:17Z- AHRS_M2 msg: 0.68 phi; -0.29 theta; -0.26 psi 2021-02-13T00:21:17Z- handleDepth()-Depth_Keller msg: 1613175677773 epoch sec; seqNo:11883; depth 5.11 2021-02-13T00:21:17Z- handleDvl() - RDI_Pathfinder msg: 1613175677.79 epoch sec; seqNo:5885 2021-02-13T00:21:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 115.63 , 0.00 , 126.04 2021-02-13T00:21:17Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , 0.08 , -0.36 2021-02-13T00:21:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175677.80 epoch sec; seqNo:11883; 4058957.27 north; 592409.66 east 2021-02-13T00:21:18Z- AHRS_M2 msg: 1613175678.17 epoch sec; seqNo:11788 2021-02-13T00:21:18Z- AHRS_M2 msg: 0.69 phi; -0.28 theta; -0.28 psi 2021-02-13T00:21:18Z- handleDepth()-Depth_Keller msg: 1613175678172 epoch sec; seqNo:11884; depth 5.22 2021-02-13T00:21:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175678.19 epoch sec; seqNo:11884; 4058957.65 north; 592409.65 east 2021-02-13T00:21:18Z- AHRS_M2 msg: 1613175678.57 epoch sec; seqNo:11789 2021-02-13T00:21:18Z- AHRS_M2 msg: 0.70 phi; -0.28 theta; -0.29 psi 2021-02-13T00:21:18Z- handleDepth()-Depth_Keller msg: 1613175678578 epoch sec; seqNo:11885; depth 5.31 2021-02-13T00:21:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175678.59 epoch sec; seqNo:11885; 4058958.03 north; 592409.65 east 2021-02-13T00:21:18Z- AHRS_M2 msg: 1613175678.98 epoch sec; seqNo:11790 2021-02-13T00:21:18Z- AHRS_M2 msg: 0.71 phi; -0.29 theta; -0.28 psi 2021-02-13T00:21:18Z- handleDepth()-Depth_Keller msg: 1613175678980 epoch sec; seqNo:11886; depth 5.41 2021-02-13T00:21:19Z- handleDvl() - RDI_Pathfinder msg: 1613175679.00 epoch sec; seqNo:5886 2021-02-13T00:21:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 118.06 , 85.34 , 124.01 2021-02-13T00:21:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.38 , -0.29 2021-02-13T00:21:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175679.01 epoch sec; seqNo:11886; 4058958.41 north; 592409.64 east 2021-02-13T00:21:19Z- AHRS_M2 msg: 1613175679.38 epoch sec; seqNo:11791 2021-02-13T00:21:19Z- AHRS_M2 msg: 0.71 phi; -0.31 theta; -0.24 psi 2021-02-13T00:21:19Z- handleDepth()-Depth_Keller msg: 1613175679385 epoch sec; seqNo:11887; depth 5.53 2021-02-13T00:21:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175679.40 epoch sec; seqNo:11887; 4058958.79 north; 592409.64 east 2021-02-13T00:21:19Z- AHRS_M2 msg: 1613175679.78 epoch sec; seqNo:11792 2021-02-13T00:21:19Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.20 psi 2021-02-13T00:21:19Z- handleDepth()-Depth_Keller msg: 1613175679788 epoch sec; seqNo:11888; depth 5.65 2021-02-13T00:21:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175679.80 epoch sec; seqNo:11888; 4058959.16 north; 592409.64 east 2021-02-13T00:21:20Z- AHRS_M2 msg: 1613175680.19 epoch sec; seqNo:11793 2021-02-13T00:21:20Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.16 psi 2021-02-13T00:21:20Z- handleDepth()-Depth_Keller msg: 1613175680205 epoch sec; seqNo:11889; depth 5.75 2021-02-13T00:21:20Z- handleDvl() - RDI_Pathfinder msg: 1613175680.22 epoch sec; seqNo:5887 2021-02-13T00:21:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.57 , 115.42 , 84.19 , 122.86 2021-02-13T00:21:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.62 , -0.31 , 0.21 2021-02-13T00:21:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175680.23 epoch sec; seqNo:11889; 4058959.54 north; 592409.63 east 2021-02-13T00:21:20Z- AHRS_M2 msg: 1613175680.59 epoch sec; seqNo:11794 2021-02-13T00:21:20Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.15 psi 2021-02-13T00:21:20Z- handleDepth()-Depth_Keller msg: 1613175680596 epoch sec; seqNo:11890; depth 5.86 2021-02-13T00:21:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175680.62 epoch sec; seqNo:11890; 4058959.54 north; 592409.63 east 2021-02-13T00:21:20Z- AHRS_M2 msg: 1613175681.00 epoch sec; seqNo:11795 2021-02-13T00:21:20Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.15 psi 2021-02-13T00:21:21Z- handleDepth()-Depth_Keller msg: 1613175681000 epoch sec; seqNo:11891; depth 5.96 2021-02-13T00:21:21Z- handleDvl() - RDI_Pathfinder msg: 1613175681.01 epoch sec; seqNo:5888 2021-02-13T00:21:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.64 , 108.05 , 82.77 , 127.39 2021-02-13T00:21:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , 0.17 , 0.30 2021-02-13T00:21:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175681.02 epoch sec; seqNo:11891; 4058959.54 north; 592409.63 east 2021-02-13T00:21:21Z- LcmTrn::handleMessages() - handling 45 messages took 0.69 ms and 132 calls... 2021-02-13T00:21:21Z- waiting for 1613175678.77, time is 1613175681.02 2021-02-13T00:21:21Z- LcmTrn::updateTrn() equating pose 1613175681.02 and meas times 1613175681.02 2021-02-13T00:21:21Z- LcmTrn::updateTrn() >>>> pose time 1613175681.02, meas time 1613175681.02 <<<< 2021-02-13T00:21:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175681.02 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:21:21Z- TerrainNav::motionUpdate() - currEstimate = (4058960, 592409.63, 5.96) 2021-02-13T00:21:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:21:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:21:21Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175681.02 sec.; No relevant navigation data available 2021-02-13T00:21:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:21:21Z- TNavBF::mmse selected from filter -1 2021-02-13T00:21:21Z- variances([0], [2], and psi_berg) = 706.877717 833.001691 0.000000 2021-02-13T00:21:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:21:21Z- LcmTrn::publishEstimates() - MLE : 4058959.04 592458.13 5.96 2021-02-13T00:21:21Z- LcmTrn::publishEstimates() - MMSE : 4058959.54 592409.63 5.96 2021-02-13T00:21:21Z- LcmTrn::publishEstimates() - COVAR: 706.88 833.00 1.31 0.00 2021-02-13T00:21:21Z- AHRS_M2 msg: 1613175681.42 epoch sec; seqNo:11796 2021-02-13T00:21:21Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.18 psi 2021-02-13T00:21:21Z- handleDepth()-Depth_Keller msg: 1613175681429 epoch sec; seqNo:11892; depth 6.06 2021-02-13T00:21:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175681.44 epoch sec; seqNo:11892; 4058959.94 north; 592410.85 east 2021-02-13T00:21:21Z- AHRS_M2 msg: 1613175681.82 epoch sec; seqNo:11797 2021-02-13T00:21:21Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.22 psi 2021-02-13T00:21:21Z- handleDepth()-Depth_Keller msg: 1613175681834 epoch sec; seqNo:11893; depth 6.15 2021-02-13T00:21:21Z- handleDvl() - RDI_Pathfinder msg: 1613175681.85 epoch sec; seqNo:5889 2021-02-13T00:21:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.32 , 111.16 , 79.29 , 120.09 2021-02-13T00:21:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , 0.26 , -0.19 2021-02-13T00:21:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175681.86 epoch sec; seqNo:11893; 4058959.94 north; 592410.85 east 2021-02-13T00:21:22Z- AHRS_M2 msg: 1613175682.21 epoch sec; seqNo:11798 2021-02-13T00:21:22Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.26 psi 2021-02-13T00:21:22Z- handleDepth()-Depth_Keller msg: 1613175682212 epoch sec; seqNo:11894; depth 6.27 2021-02-13T00:21:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175682.22 epoch sec; seqNo:11894; 4058960.31 north; 592410.84 east 2021-02-13T00:21:22Z- AHRS_M2 msg: 1613175682.62 epoch sec; seqNo:11799 2021-02-13T00:21:22Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.29 psi 2021-02-13T00:21:22Z- handleDepth()-Depth_Keller msg: 1613175682622 epoch sec; seqNo:11895; depth 6.36 2021-02-13T00:21:22Z- handleDvl() - RDI_Pathfinder msg: 1613175682.63 epoch sec; seqNo:5890 2021-02-13T00:21:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:21:22Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:21:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175682.64 epoch sec; seqNo:11895; 4058960.31 north; 592410.84 east 2021-02-13T00:21:23Z- AHRS_M2 msg: 1613175683.02 epoch sec; seqNo:11800 2021-02-13T00:21:23Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.30 psi 2021-02-13T00:21:23Z- handleDepth()-Depth_Keller msg: 1613175683029 epoch sec; seqNo:11896; depth 6.50 2021-02-13T00:21:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175683.04 epoch sec; seqNo:11896; 4058960.69 north; 592410.84 east 2021-02-13T00:21:23Z- AHRS_M2 msg: 1613175683.42 epoch sec; seqNo:11801 2021-02-13T00:21:23Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.28 psi 2021-02-13T00:21:23Z- handleDepth()-Depth_Keller msg: 1613175683427 epoch sec; seqNo:11897; depth 6.60 2021-02-13T00:21:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175683.45 epoch sec; seqNo:11897; 4058960.71 north; 592412.06 east 2021-02-13T00:21:23Z- AHRS_M2 msg: 1613175683.82 epoch sec; seqNo:11802 2021-02-13T00:21:23Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.25 psi 2021-02-13T00:21:23Z- handleDepth()-Depth_Keller msg: 1613175683828 epoch sec; seqNo:11898; depth 6.75 2021-02-13T00:21:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175683.84 epoch sec; seqNo:11898; 4058961.08 north; 592412.06 east 2021-02-13T00:21:24Z- AHRS_M2 msg: 1613175684.23 epoch sec; seqNo:11803 2021-02-13T00:21:24Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.21 psi 2021-02-13T00:21:24Z- handleDepth()-Depth_Keller msg: 1613175684239 epoch sec; seqNo:11899; depth 6.85 2021-02-13T00:21:24Z- handleDvl() - RDI_Pathfinder msg: 1613175684.25 epoch sec; seqNo:5891 2021-02-13T00:21:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.50 , 119.95 , 83.44 , 128.07 2021-02-13T00:21:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.37 , 0.04 2021-02-13T00:21:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175684.26 epoch sec; seqNo:11899; 4058961.46 north; 592412.05 east 2021-02-13T00:21:24Z- AHRS_M2 msg: 1613175684.64 epoch sec; seqNo:11804 2021-02-13T00:21:24Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.18 psi 2021-02-13T00:21:24Z- handleDepth()-Depth_Keller msg: 1613175684642 epoch sec; seqNo:11900; depth 7.00 2021-02-13T00:21:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175684.65 epoch sec; seqNo:11900; 4058961.46 north; 592412.05 east 2021-02-13T00:21:25Z- AHRS_M2 msg: 1613175685.04 epoch sec; seqNo:11805 2021-02-13T00:21:25Z- AHRS_M2 msg: 0.67 phi; -0.38 theta; -0.16 psi 2021-02-13T00:21:25Z- handleDepth()-Depth_Keller msg: 1613175685044 epoch sec; seqNo:11901; depth 7.11 2021-02-13T00:21:25Z- handleDvl() - RDI_Pathfinder msg: 1613175685.06 epoch sec; seqNo:5892 2021-02-13T00:21:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.27 , 111.10 , 81.35 , 130.43 2021-02-13T00:21:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.19 , 0.22 2021-02-13T00:21:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175685.07 epoch sec; seqNo:11901; 4058961.84 north; 592412.05 east 2021-02-13T00:21:25Z- AHRS_M2 msg: 1613175685.44 epoch sec; seqNo:11806 2021-02-13T00:21:25Z- AHRS_M2 msg: 0.66 phi; -0.38 theta; -0.15 psi 2021-02-13T00:21:25Z- handleDepth()-Depth_Keller msg: 1613175685448 epoch sec; seqNo:11902; depth 7.26 2021-02-13T00:21:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175685.46 epoch sec; seqNo:11902; 4058962.22 north; 592412.05 east 2021-02-13T00:21:25Z- AHRS_M2 msg: 1613175685.85 epoch sec; seqNo:11807 2021-02-13T00:21:25Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.14 psi 2021-02-13T00:21:25Z- handleDepth()-Depth_Keller msg: 1613175685856 epoch sec; seqNo:11903; depth 7.37 2021-02-13T00:21:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175685.87 epoch sec; seqNo:11903; 4058962.22 north; 592412.05 east 2021-02-13T00:21:26Z- LcmTrn::handleMessages() - handling 40 messages took 0.66 ms and 127 calls... 2021-02-13T00:21:26Z- waiting for 1613175684.02, time is 1613175685.87 2021-02-13T00:21:26Z- LcmTrn::updateTrn() >>>> pose time 1613175685.87, meas time 1613175685.06 <<<< 2021-02-13T00:21:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175685.06 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:21:26Z- TerrainNav::motionUpdate() - currEstimate = (4058962, 592412.05, 7.37) 2021-02-13T00:21:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:21:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:21:26Z- TNavBF::Projecting Measurements 2021-02-13T00:21:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:21:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:21:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:21:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:21:26Z- TNavFilter::4 good beams. 2021-02-13T00:21:26Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:21:26Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:21:26Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:21:26Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:21:26Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:21:26Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:21:26Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:21:26Z- TNavBF::mmse selected from filter -1 2021-02-13T00:21:26Z- TerrainMapOctree: (vehN, vehE ) = (4058961.78, 592411.64). 2021-02-13T00:21:26Z- TerrainMapOctree: Min Distance = 2935.22. 2021-02-13T00:21:26Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:21:26Z- There is no specific good beam that all particles have in common. 2021-02-13T00:21:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:21:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.46 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 13.04 2021-02-13T00:21:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:21:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 19.75 2021-02-13T00:21:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:21:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 10.59 2021-02-13T00:21:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:21:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 27.22 2021-02-13T00:21:26Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:21:26Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:21:26Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:21:26Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:21:26Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:21:26Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:21:26Z- Filter 0 No beams used in subcloud update 2021-02-13T00:21:26Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:21:26Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:21:26Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 706.8 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:21:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175685.06 sec. 2021-02-13T00:21:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:21:26Z- TNavBF::mmse selected from filter -1 2021-02-13T00:21:26Z- variances([0], [2], and psi_berg) = 706.804639 833.001650 0.000000 2021-02-13T00:21:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:21:26Z- LcmTrn::publishEstimates() - MLE : 4058961.72 592459.55 7.37 2021-02-13T00:21:26Z- LcmTrn::publishEstimates() - MMSE : 4058962.22 592412.05 7.37 2021-02-13T00:21:26Z- LcmTrn::publishEstimates() - COVAR: 706.80 833.00 1.12 0.00 2021-02-13T00:21:26Z- AHRS_M2 msg: 1613175686.25 epoch sec; seqNo:11808 2021-02-13T00:21:26Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.14 psi 2021-02-13T00:21:26Z- handleDepth()-Depth_Keller msg: 1613175686258 epoch sec; seqNo:11904; depth 7.50 2021-02-13T00:21:26Z- handleDvl() - RDI_Pathfinder msg: 1613175686.27 epoch sec; seqNo:5893 2021-02-13T00:21:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 106.77 , 81.48 , 129.08 2021-02-13T00:21:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.03 , 0.21 2021-02-13T00:21:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175686.28 epoch sec; seqNo:11904; 4058962.60 north; 592412.04 east 2021-02-13T00:21:26Z- AHRS_M2 msg: 1613175686.66 epoch sec; seqNo:11809 2021-02-13T00:21:26Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.15 psi 2021-02-13T00:21:26Z- handleDepth()-Depth_Keller msg: 1613175686665 epoch sec; seqNo:11905; depth 7.64 2021-02-13T00:21:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175686.68 epoch sec; seqNo:11905; 4058962.61 north; 592413.26 east 2021-02-13T00:21:27Z- AHRS_M2 msg: 1613175687.06 epoch sec; seqNo:11810 2021-02-13T00:21:27Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.15 psi 2021-02-13T00:21:27Z- handleDepth()-Depth_Keller msg: 1613175687064 epoch sec; seqNo:11906; depth 7.76 2021-02-13T00:21:27Z- handleDvl() - RDI_Pathfinder msg: 1613175687.08 epoch sec; seqNo:5894 2021-02-13T00:21:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 105.21 , 81.42 , 127.53 2021-02-13T00:21:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.17 , 0.07 2021-02-13T00:21:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175687.10 epoch sec; seqNo:11906; 4058962.99 north; 592413.26 east 2021-02-13T00:21:27Z- AHRS_M2 msg: 1613175687.46 epoch sec; seqNo:11811 2021-02-13T00:21:27Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.16 psi 2021-02-13T00:21:27Z- handleDepth()-Depth_Keller msg: 1613175687469 epoch sec; seqNo:11907; depth 7.90 2021-02-13T00:21:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175687.49 epoch sec; seqNo:11907; 4058962.99 north; 592413.26 east 2021-02-13T00:21:27Z- AHRS_M2 msg: 1613175687.87 epoch sec; seqNo:11812 2021-02-13T00:21:27Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-13T00:21:27Z- handleDepth()-Depth_Keller msg: 1613175687874 epoch sec; seqNo:11908; depth 8.02 2021-02-13T00:21:27Z- handleDvl() - RDI_Pathfinder msg: 1613175687.89 epoch sec; seqNo:5895 2021-02-13T00:21:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 106.77 , 0.00 , 121.64 2021-02-13T00:21:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.12 , -0.09 2021-02-13T00:21:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175687.91 epoch sec; seqNo:11908; 4058963.37 north; 592413.25 east 2021-02-13T00:21:28Z- AHRS_M2 msg: 1613175688.27 epoch sec; seqNo:11813 2021-02-13T00:21:28Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-13T00:21:28Z- handleDepth()-Depth_Keller msg: 1613175688282 epoch sec; seqNo:11909; depth 8.17 2021-02-13T00:21:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175688.30 epoch sec; seqNo:11909; 4058963.75 north; 592413.25 east 2021-02-13T00:21:28Z- AHRS_M2 msg: 1613175688.68 epoch sec; seqNo:11814 2021-02-13T00:21:28Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.20 psi 2021-02-13T00:21:28Z- handleDepth()-Depth_Keller msg: 1613175688693 epoch sec; seqNo:11910; depth 8.28 2021-02-13T00:21:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175688.71 epoch sec; seqNo:11910; 4058964.14 north; 592414.47 east 2021-02-13T00:21:29Z- AHRS_M2 msg: 1613175689.08 epoch sec; seqNo:11815 2021-02-13T00:21:29Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.21 psi 2021-02-13T00:21:29Z- handleDepth()-Depth_Keller msg: 1613175689086 epoch sec; seqNo:11911; depth 8.41 2021-02-13T00:21:29Z- handleDvl() - RDI_Pathfinder msg: 1613175689.10 epoch sec; seqNo:5896 2021-02-13T00:21:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.45 , 109.27 , 77.04 , 119.68 2021-02-13T00:21:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , -0.10 2021-02-13T00:21:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175689.11 epoch sec; seqNo:11911; 4058964.14 north; 592414.47 east 2021-02-13T00:21:29Z- AHRS_M2 msg: 1613175689.48 epoch sec; seqNo:11816 2021-02-13T00:21:29Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.21 psi 2021-02-13T00:21:29Z- handleDepth()-Depth_Keller msg: 1613175689495 epoch sec; seqNo:11912; depth 8.54 2021-02-13T00:21:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175689.51 epoch sec; seqNo:11912; 4058964.52 north; 592414.46 east 2021-02-13T00:21:29Z- AHRS_M2 msg: 1613175689.89 epoch sec; seqNo:11817 2021-02-13T00:21:29Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.21 psi 2021-02-13T00:21:29Z- handleDepth()-Depth_Keller msg: 1613175689897 epoch sec; seqNo:11913; depth 8.67 2021-02-13T00:21:29Z- handleDvl() - RDI_Pathfinder msg: 1613175689.91 epoch sec; seqNo:5897 2021-02-13T00:21:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.70 , 109.34 , 79.69 , 121.51 2021-02-13T00:21:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.00 , -0.01 2021-02-13T00:21:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175689.92 epoch sec; seqNo:11913; 4058964.90 north; 592414.46 east 2021-02-13T00:21:30Z- AHRS_M2 msg: 1613175690.29 epoch sec; seqNo:11818 2021-02-13T00:21:30Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.20 psi 2021-02-13T00:21:30Z- handleDepth()-Depth_Keller msg: 1613175690301 epoch sec; seqNo:11914; depth 8.78 2021-02-13T00:21:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175690.31 epoch sec; seqNo:11914; 4058964.90 north; 592414.46 east 2021-02-13T00:21:30Z- AHRS_M2 msg: 1613175690.70 epoch sec; seqNo:11819 2021-02-13T00:21:30Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.20 psi 2021-02-13T00:21:30Z- handleDepth()-Depth_Keller msg: 1613175690709 epoch sec; seqNo:11915; depth 8.91 2021-02-13T00:21:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175690.72 epoch sec; seqNo:11915; 4058965.28 north; 592414.45 east 2021-02-13T00:21:31Z- AHRS_M2 msg: 1613175691.10 epoch sec; seqNo:11820 2021-02-13T00:21:31Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-13T00:21:31Z- handleDepth()-Depth_Keller msg: 1613175691106 epoch sec; seqNo:11916; depth 9.04 2021-02-13T00:21:31Z- handleDvl() - RDI_Pathfinder msg: 1613175691.12 epoch sec; seqNo:5898 2021-02-13T00:21:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 110.56 , 0.00 , 124.08 2021-02-13T00:21:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.01 , 0.03 2021-02-13T00:21:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175691.13 epoch sec; seqNo:11916; 4058965.67 north; 592415.67 east 2021-02-13T00:21:31Z- LcmTrn::handleMessages() - handling 45 messages took 0.62 ms and 133 calls... 2021-02-13T00:21:31Z- waiting for 1613175688.87, time is 1613175691.13 2021-02-13T00:21:31Z- LcmTrn::updateTrn() equating pose 1613175691.13 and meas times 1613175691.13 2021-02-13T00:21:31Z- LcmTrn::updateTrn() >>>> pose time 1613175691.13, meas time 1613175691.13 <<<< 2021-02-13T00:21:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175691.13 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:21:31Z- TerrainNav::motionUpdate() - currEstimate = (4058966, 592415.67, 9.04) 2021-02-13T00:21:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:21:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:21:31Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175691.13 sec.; No relevant navigation data available 2021-02-13T00:21:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:21:31Z- TNavBF::mmse selected from filter -1 2021-02-13T00:21:31Z- variances([0], [2], and psi_berg) = 706.739490 833.001613 0.000000 2021-02-13T00:21:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:21:31Z- LcmTrn::publishEstimates() - MLE : 4058965.17 592458.17 9.04 2021-02-13T00:21:31Z- LcmTrn::publishEstimates() - MMSE : 4058965.67 592415.67 9.04 2021-02-13T00:21:31Z- LcmTrn::publishEstimates() - COVAR: 706.74 833.00 1.38 0.00 2021-02-13T00:21:31Z- AHRS_M2 msg: 1613175691.50 epoch sec; seqNo:11821 2021-02-13T00:21:31Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-13T00:21:31Z- handleDepth()-Depth_Keller msg: 1613175691513 epoch sec; seqNo:11917; depth 9.16 2021-02-13T00:21:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175691.54 epoch sec; seqNo:11917; 4058965.67 north; 592415.67 east 2021-02-13T00:21:31Z- AHRS_M2 msg: 1613175691.91 epoch sec; seqNo:11822 2021-02-13T00:21:31Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-13T00:21:31Z- handleDepth()-Depth_Keller msg: 1613175691912 epoch sec; seqNo:11918; depth 9.27 2021-02-13T00:21:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175691.92 epoch sec; seqNo:11918; 4058966.05 north; 592415.67 east 2021-02-13T00:21:32Z- AHRS_M2 msg: 1613175692.31 epoch sec; seqNo:11823 2021-02-13T00:21:32Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-13T00:21:32Z- handleDepth()-Depth_Keller msg: 1613175692317 epoch sec; seqNo:11919; depth 9.40 2021-02-13T00:21:32Z- handleDvl() - RDI_Pathfinder msg: 1613175692.33 epoch sec; seqNo:5899 2021-02-13T00:21:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.44 , 107.85 , 81.08 , 124.21 2021-02-13T00:21:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.04 , 0.00 2021-02-13T00:21:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175692.34 epoch sec; seqNo:11919; 4058966.43 north; 592415.66 east 2021-02-13T00:21:32Z- AHRS_M2 msg: 1613175692.72 epoch sec; seqNo:11824 2021-02-13T00:21:32Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-13T00:21:32Z- handleDepth()-Depth_Keller msg: 1613175692722 epoch sec; seqNo:11920; depth 9.52 2021-02-13T00:21:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175692.73 epoch sec; seqNo:11920; 4058966.43 north; 592415.66 east 2021-02-13T00:21:33Z- AHRS_M2 msg: 1613175693.12 epoch sec; seqNo:11825 2021-02-13T00:21:33Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.20 psi 2021-02-13T00:21:33Z- handleDepth()-Depth_Keller msg: 1613175693126 epoch sec; seqNo:11921; depth 9.65 2021-02-13T00:21:33Z- handleDvl() - RDI_Pathfinder msg: 1613175693.15 epoch sec; seqNo:5900 2021-02-13T00:21:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 108.32 , 80.07 , 120.22 2021-02-13T00:21:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , -0.02 2021-02-13T00:21:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175693.16 epoch sec; seqNo:11921; 4058966.81 north; 592415.66 east 2021-02-13T00:21:33Z- AHRS_M2 msg: 1613175693.53 epoch sec; seqNo:11826 2021-02-13T00:21:33Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.20 psi 2021-02-13T00:21:33Z- handleDepth()-Depth_Keller msg: 1613175693535 epoch sec; seqNo:11922; depth 9.79 2021-02-13T00:21:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175693.55 epoch sec; seqNo:11922; 4058967.20 north; 592416.87 east 2021-02-13T00:21:33Z- AHRS_M2 msg: 1613175693.93 epoch sec; seqNo:11827 2021-02-13T00:21:33Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.20 psi 2021-02-13T00:21:33Z- handleDepth()-Depth_Keller msg: 1613175693932 epoch sec; seqNo:11923; depth 9.90 2021-02-13T00:21:33Z- handleDvl() - RDI_Pathfinder msg: 1613175693.95 epoch sec; seqNo:5901 2021-02-13T00:21:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.43 , 108.26 , 0.00 , 124.62 2021-02-13T00:21:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , -0.05 2021-02-13T00:21:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175693.96 epoch sec; seqNo:11923; 4058967.58 north; 592416.87 east 2021-02-13T00:21:34Z- AHRS_M2 msg: 1613175694.33 epoch sec; seqNo:11828 2021-02-13T00:21:34Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.20 psi 2021-02-13T00:21:34Z- handleDepth()-Depth_Keller msg: 1613175694337 epoch sec; seqNo:11924; depth 10.04 2021-02-13T00:21:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175694.35 epoch sec; seqNo:11924; 4058967.58 north; 592416.87 east 2021-02-13T00:21:34Z- AHRS_M2 msg: 1613175694.73 epoch sec; seqNo:11829 2021-02-13T00:21:34Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.20 psi 2021-02-13T00:21:34Z- handleDepth()-Depth_Keller msg: 1613175694742 epoch sec; seqNo:11925; depth 10.17 2021-02-13T00:21:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175694.75 epoch sec; seqNo:11925; 4058967.96 north; 592416.87 east 2021-02-13T00:21:35Z- AHRS_M2 msg: 1613175695.14 epoch sec; seqNo:11830 2021-02-13T00:21:35Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.20 psi 2021-02-13T00:21:35Z- handleDepth()-Depth_Keller msg: 1613175695148 epoch sec; seqNo:11926; depth 10.31 2021-02-13T00:21:35Z- handleDvl() - RDI_Pathfinder msg: 1613175695.17 epoch sec; seqNo:5902 2021-02-13T00:21:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.10 , 107.92 , 0.00 , 124.28 2021-02-13T00:21:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , -0.09 2021-02-13T00:21:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175695.18 epoch sec; seqNo:11926; 4058968.34 north; 592416.86 east 2021-02-13T00:21:35Z- AHRS_M2 msg: 1613175695.54 epoch sec; seqNo:11831 2021-02-13T00:21:35Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.20 psi 2021-02-13T00:21:35Z- handleDepth()-Depth_Keller msg: 1613175695553 epoch sec; seqNo:11927; depth 10.43 2021-02-13T00:21:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175695.56 epoch sec; seqNo:11927; 4058968.73 north; 592418.08 east 2021-02-13T00:21:35Z- AHRS_M2 msg: 1613175695.96 epoch sec; seqNo:11832 2021-02-13T00:21:35Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.20 psi 2021-02-13T00:21:35Z- handleDepth()-Depth_Keller msg: 1613175695969 epoch sec; seqNo:11928; depth 10.56 2021-02-13T00:21:35Z- handleDvl() - RDI_Pathfinder msg: 1613175695.98 epoch sec; seqNo:5903 2021-02-13T00:21:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.68 , 106.50 , 77.24 , 121.37 2021-02-13T00:21:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , -0.08 2021-02-13T00:21:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175696.00 epoch sec; seqNo:11928; 4058968.73 north; 592418.08 east 2021-02-13T00:21:36Z- AHRS_M2 msg: 1613175696.35 epoch sec; seqNo:11833 2021-02-13T00:21:36Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.20 psi 2021-02-13T00:21:36Z- LcmTrn::handleMessages() - handling 42 messages took 0.70 ms and 128 calls... 2021-02-13T00:21:36Z- waiting for 1613175694.13, time is 1613175696.35 2021-02-13T00:21:36Z- LcmTrn::updateTrn() >>>> pose time 1613175696.35, meas time 1613175695.98 <<<< 2021-02-13T00:21:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175695.98 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:21:36Z- TerrainNav::motionUpdate() - currEstimate = (4058969, 592418.08, 10.56) 2021-02-13T00:21:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:21:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:21:36Z- TNavBF::Projecting Measurements 2021-02-13T00:21:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:21:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:21:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:21:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:21:36Z- TNavFilter::4 good beams. 2021-02-13T00:21:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:21:36Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:21:36Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:21:36Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:21:36Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:21:36Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:21:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:21:36Z- TNavBF::mmse selected from filter -1 2021-02-13T00:21:36Z- TerrainMapOctree: (vehN, vehE ) = (4058968.51, 592417.91). 2021-02-13T00:21:36Z- TerrainMapOctree: Min Distance = 2926.05. 2021-02-13T00:21:36Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:21:36Z- There is no specific good beam that all particles have in common. 2021-02-13T00:21:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:21:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.56 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 11.74 2021-02-13T00:21:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:21:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 18.15 2021-02-13T00:21:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:21:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 9.55 2021-02-13T00:21:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:21:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 23.57 2021-02-13T00:21:36Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:21:36Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:21:36Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:21:36Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:21:36Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:21:36Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:21:36Z- Filter 0 No beams used in subcloud update 2021-02-13T00:21:36Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:21:36Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:21:36Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 706.7 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:21:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175695.98 sec. 2021-02-13T00:21:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:21:36Z- TNavBF::mmse selected from filter -1 2021-02-13T00:21:36Z- variances([0], [2], and psi_berg) = 706.668855 833.001573 0.000000 2021-02-13T00:21:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:21:36Z- LcmTrn::publishEstimates() - MLE : 4058968.23 592460.58 10.56 2021-02-13T00:21:36Z- LcmTrn::publishEstimates() - MMSE : 4058968.73 592418.08 10.56 2021-02-13T00:21:36Z- LcmTrn::publishEstimates() - COVAR: 706.67 833.00 1.13 0.00 2021-02-13T00:21:36Z- handleDepth()-Depth_Keller msg: 1613175696358 epoch sec; seqNo:11929; depth 10.70 2021-02-13T00:21:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175696.37 epoch sec; seqNo:11929; 4058969.11 north; 592418.08 east 2021-02-13T00:21:36Z- AHRS_M2 msg: 1613175696.76 epoch sec; seqNo:11834 2021-02-13T00:21:36Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.20 psi 2021-02-13T00:21:36Z- handleDepth()-Depth_Keller msg: 1613175696765 epoch sec; seqNo:11930; depth 10.83 2021-02-13T00:21:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175696.78 epoch sec; seqNo:11930; 4058969.49 north; 592418.07 east 2021-02-13T00:21:37Z- AHRS_M2 msg: 1613175697.16 epoch sec; seqNo:11835 2021-02-13T00:21:37Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.20 psi 2021-02-13T00:21:37Z- handleDepth()-Depth_Keller msg: 1613175697165 epoch sec; seqNo:11931; depth 10.96 2021-02-13T00:21:37Z- handleDvl() - RDI_Pathfinder msg: 1613175697.18 epoch sec; seqNo:5904 2021-02-13T00:21:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.07 , 105.35 , 79.29 , 118.87 2021-02-13T00:21:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.14 , -0.03 2021-02-13T00:21:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175697.20 epoch sec; seqNo:11931; 4058969.86 north; 592418.07 east 2021-02-13T00:21:37Z- AHRS_M2 msg: 1613175697.56 epoch sec; seqNo:11836 2021-02-13T00:21:37Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.20 psi 2021-02-13T00:21:37Z- handleDepth()-Depth_Keller msg: 1613175697568 epoch sec; seqNo:11932; depth 11.07 2021-02-13T00:21:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175697.58 epoch sec; seqNo:11932; 4058969.86 north; 592418.07 east 2021-02-13T00:21:37Z- AHRS_M2 msg: 1613175697.98 epoch sec; seqNo:11837 2021-02-13T00:21:37Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.20 psi 2021-02-13T00:21:37Z- handleDepth()-Depth_Keller msg: 1613175697986 epoch sec; seqNo:11933; depth 11.18 2021-02-13T00:21:38Z- handleDvl() - RDI_Pathfinder msg: 1613175698.00 epoch sec; seqNo:5905 2021-02-13T00:21:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 105.42 , 79.73 , 108.12 2021-02-13T00:21:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.12 , 0.05 2021-02-13T00:21:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175698.01 epoch sec; seqNo:11933; 4058970.24 north; 592418.06 east 2021-02-13T00:21:38Z- AHRS_M2 msg: 1613175698.37 epoch sec; seqNo:11838 2021-02-13T00:21:38Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.20 psi 2021-02-13T00:21:38Z- handleDepth()-Depth_Keller msg: 1613175698386 epoch sec; seqNo:11934; depth 11.31 2021-02-13T00:21:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175698.40 epoch sec; seqNo:11934; 4058970.62 north; 592418.06 east 2021-02-13T00:21:38Z- AHRS_M2 msg: 1613175698.78 epoch sec; seqNo:11839 2021-02-13T00:21:38Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.19 psi 2021-02-13T00:21:38Z- handleDepth()-Depth_Keller msg: 1613175698785 epoch sec; seqNo:11935; depth 11.42 2021-02-13T00:21:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175698.79 epoch sec; seqNo:11935; 4058971.00 north; 592418.05 east 2021-02-13T00:21:39Z- AHRS_M2 msg: 1613175699.18 epoch sec; seqNo:11840 2021-02-13T00:21:39Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.18 psi 2021-02-13T00:21:39Z- handleDepth()-Depth_Keller msg: 1613175699190 epoch sec; seqNo:11936; depth 11.55 2021-02-13T00:21:39Z- handleDvl() - RDI_Pathfinder msg: 1613175699.20 epoch sec; seqNo:5906 2021-02-13T00:21:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 104.27 , 78.58 , 116.44 2021-02-13T00:21:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.06 , 0.10 2021-02-13T00:21:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175699.21 epoch sec; seqNo:11936; 4058971.01 north; 592419.28 east 2021-02-13T00:21:39Z- AHRS_M2 msg: 1613175699.59 epoch sec; seqNo:11841 2021-02-13T00:21:39Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-13T00:21:39Z- handleDepth()-Depth_Keller msg: 1613175699595 epoch sec; seqNo:11937; depth 11.65 2021-02-13T00:21:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175699.61 epoch sec; seqNo:11937; 4058971.39 north; 592419.27 east 2021-02-13T00:21:39Z- AHRS_M2 msg: 1613175699.99 epoch sec; seqNo:11842 2021-02-13T00:21:39Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-13T00:21:39Z- handleDepth()-Depth_Keller msg: 1613175699992 epoch sec; seqNo:11938; depth 11.77 2021-02-13T00:21:40Z- handleDvl() - RDI_Pathfinder msg: 1613175700.01 epoch sec; seqNo:5907 2021-02-13T00:21:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.20 , 105.49 , 78.44 , 119.01 2021-02-13T00:21:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , 0.09 2021-02-13T00:21:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175700.02 epoch sec; seqNo:11938; 4058971.77 north; 592419.27 east 2021-02-13T00:21:40Z- AHRS_M2 msg: 1613175700.39 epoch sec; seqNo:11843 2021-02-13T00:21:40Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-13T00:21:40Z- handleDepth()-Depth_Keller msg: 1613175700396 epoch sec; seqNo:11939; depth 11.90 2021-02-13T00:21:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175700.41 epoch sec; seqNo:11939; 4058971.77 north; 592419.27 east 2021-02-13T00:21:40Z- AHRS_M2 msg: 1613175700.80 epoch sec; seqNo:11844 2021-02-13T00:21:40Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.19 psi 2021-02-13T00:21:40Z- handleDepth()-Depth_Keller msg: 1613175700805 epoch sec; seqNo:11940; depth 12.01 2021-02-13T00:21:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175700.81 epoch sec; seqNo:11940; 4058972.15 north; 592419.26 east 2021-02-13T00:21:41Z- AHRS_M2 msg: 1613175701.20 epoch sec; seqNo:11845 2021-02-13T00:21:41Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.20 psi 2021-02-13T00:21:41Z- handleDepth()-Depth_Keller msg: 1613175701204 epoch sec; seqNo:11941; depth 12.13 2021-02-13T00:21:41Z- handleDvl() - RDI_Pathfinder msg: 1613175701.22 epoch sec; seqNo:5908 2021-02-13T00:21:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.71 , 78.37 , 116.24 2021-02-13T00:21:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , 0.05 2021-02-13T00:21:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175701.24 epoch sec; seqNo:11941; 4058972.53 north; 592419.26 east 2021-02-13T00:21:41Z- LcmTrn::handleMessages() - handling 43 messages took 0.59 ms and 132 calls... 2021-02-13T00:21:41Z- waiting for 1613175699.35, time is 1613175701.24 2021-02-13T00:21:41Z- LcmTrn::updateTrn() equating pose 1613175701.24 and meas times 1613175701.24 2021-02-13T00:21:41Z- LcmTrn::updateTrn() >>>> pose time 1613175701.24, meas time 1613175701.24 <<<< 2021-02-13T00:21:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175701.24 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:21:41Z- TerrainNav::motionUpdate() - currEstimate = (4058973, 592419.26, 12.13) 2021-02-13T00:21:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:21:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:21:41Z- TNavBF::Projecting Measurements 2021-02-13T00:21:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:21:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:21:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:21:41Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:21:41Z- TNavFilter::3 good beams. 2021-02-13T00:21:41Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:21:41Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:21:41Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:21:41Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:21:41Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:21:41Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:21:41Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:21:41Z- TNavBF::mmse selected from filter -1 2021-02-13T00:21:41Z- TerrainMapOctree: (vehN, vehE ) = (4058972.53, 592419.26). 2021-02-13T00:21:41Z- TerrainMapOctree: Min Distance = 2922.07. 2021-02-13T00:21:41Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:21:41Z- There is no specific good beam that all particles have in common. 2021-02-13T00:21:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:21:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 16.88 2021-02-13T00:21:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:21:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 9.83 2021-02-13T00:21:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:21:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 21.62 2021-02-13T00:21:41Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:21:41Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:21:41Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:21:41Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:21:41Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:21:41Z- Filter 0 No beams used in subcloud update 2021-02-13T00:21:41Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:21:41Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:21:41Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 706.6 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:21:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175701.24 sec. 2021-02-13T00:21:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:21:41Z- TNavBF::mmse selected from filter -1 2021-02-13T00:21:41Z- variances([0], [2], and psi_berg) = 706.610835 833.001541 0.000000 2021-02-13T00:21:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:21:41Z- LcmTrn::publishEstimates() - MLE : 4058972.03 592461.76 12.13 2021-02-13T00:21:41Z- LcmTrn::publishEstimates() - MMSE : 4058972.53 592419.26 12.13 2021-02-13T00:21:41Z- LcmTrn::publishEstimates() - COVAR: 706.61 833.00 1.07 0.00 2021-02-13T00:21:41Z- AHRS_M2 msg: 1613175701.60 epoch sec; seqNo:11846 2021-02-13T00:21:41Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.20 psi 2021-02-13T00:21:41Z- handleDepth()-Depth_Keller msg: 1613175701609 epoch sec; seqNo:11942; depth 12.25 2021-02-13T00:21:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175701.62 epoch sec; seqNo:11942; 4058972.92 north; 592420.48 east 2021-02-13T00:21:42Z- AHRS_M2 msg: 1613175702.01 epoch sec; seqNo:11847 2021-02-13T00:21:42Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.20 psi 2021-02-13T00:21:42Z- handleDepth()-Depth_Keller msg: 1613175702012 epoch sec; seqNo:11943; depth 12.38 2021-02-13T00:21:42Z- handleDvl() - RDI_Pathfinder msg: 1613175702.02 epoch sec; seqNo:5909 2021-02-13T00:21:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.58 , 102.51 , 78.17 , 114.68 2021-02-13T00:21:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.05 , 0.00 2021-02-13T00:21:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175702.03 epoch sec; seqNo:11943; 4058972.92 north; 592420.48 east 2021-02-13T00:21:42Z- AHRS_M2 msg: 1613175702.41 epoch sec; seqNo:11848 2021-02-13T00:21:42Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.19 psi 2021-02-13T00:21:42Z- handleDepth()-Depth_Keller msg: 1613175702421 epoch sec; seqNo:11944; depth 12.51 2021-02-13T00:21:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175702.43 epoch sec; seqNo:11944; 4058973.30 north; 592420.47 east 2021-02-13T00:21:42Z- AHRS_M2 msg: 1613175702.84 epoch sec; seqNo:11849 2021-02-13T00:21:42Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-13T00:21:42Z- handleDepth()-Depth_Keller msg: 1613175702846 epoch sec; seqNo:11945; depth 12.65 2021-02-13T00:21:42Z- handleDvl() - RDI_Pathfinder msg: 1613175702.86 epoch sec; seqNo:5910 2021-02-13T00:21:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 102.92 , 74.38 , 112.38 2021-02-13T00:21:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.06 , -0.07 2021-02-13T00:21:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175702.87 epoch sec; seqNo:11945; 4058973.68 north; 592420.47 east 2021-02-13T00:21:43Z- AHRS_M2 msg: 1613175703.23 epoch sec; seqNo:11850 2021-02-13T00:21:43Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.17 psi 2021-02-13T00:21:43Z- handleDepth()-Depth_Keller msg: 1613175703255 epoch sec; seqNo:11946; depth 12.77 2021-02-13T00:21:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175703.27 epoch sec; seqNo:11946; 4058974.06 north; 592420.46 east 2021-02-13T00:21:43Z- AHRS_M2 msg: 1613175703.62 epoch sec; seqNo:11851 2021-02-13T00:21:43Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.16 psi 2021-02-13T00:21:43Z- handleDepth()-Depth_Keller msg: 1613175703628 epoch sec; seqNo:11947; depth 12.90 2021-02-13T00:21:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175703.64 epoch sec; seqNo:11947; 4058974.06 north; 592420.46 east 2021-02-13T00:21:44Z- AHRS_M2 msg: 1613175704.03 epoch sec; seqNo:11852 2021-02-13T00:21:44Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.16 psi 2021-02-13T00:21:44Z- handleDepth()-Depth_Keller msg: 1613175704033 epoch sec; seqNo:11948; depth 12.99 2021-02-13T00:21:44Z- handleDvl() - RDI_Pathfinder msg: 1613175704.04 epoch sec; seqNo:5911 2021-02-13T00:21:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.05 , 101.63 , 78.64 , 112.45 2021-02-13T00:21:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.01 , -0.13 2021-02-13T00:21:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175704.05 epoch sec; seqNo:11948; 4058974.45 north; 592421.68 east 2021-02-13T00:21:44Z- AHRS_M2 msg: 1613175704.43 epoch sec; seqNo:11853 2021-02-13T00:21:44Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.15 psi 2021-02-13T00:21:44Z- handleDepth()-Depth_Keller msg: 1613175704441 epoch sec; seqNo:11949; depth 13.11 2021-02-13T00:21:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175704.45 epoch sec; seqNo:11949; 4058974.83 north; 592421.68 east 2021-02-13T00:21:44Z- AHRS_M2 msg: 1613175704.85 epoch sec; seqNo:11854 2021-02-13T00:21:44Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.15 psi 2021-02-13T00:21:44Z- handleDepth()-Depth_Keller msg: 1613175704861 epoch sec; seqNo:11950; depth 13.23 2021-02-13T00:21:44Z- handleDvl() - RDI_Pathfinder msg: 1613175704.87 epoch sec; seqNo:5912 2021-02-13T00:21:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 100.21 , 0.00 , 106.97 2021-02-13T00:21:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , -0.10 2021-02-13T00:21:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175704.88 epoch sec; seqNo:11950; 4058975.21 north; 592421.67 east 2021-02-13T00:21:45Z- AHRS_M2 msg: 1613175705.24 epoch sec; seqNo:11855 2021-02-13T00:21:45Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.14 psi 2021-02-13T00:21:45Z- handleDepth()-Depth_Keller msg: 1613175705257 epoch sec; seqNo:11951; depth 13.35 2021-02-13T00:21:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175705.28 epoch sec; seqNo:11951; 4058975.21 north; 592421.67 east 2021-02-13T00:21:45Z- AHRS_M2 msg: 1613175705.64 epoch sec; seqNo:11856 2021-02-13T00:21:45Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.13 psi 2021-02-13T00:21:45Z- handleDepth()-Depth_Keller msg: 1613175705653 epoch sec; seqNo:11952; depth 13.47 2021-02-13T00:21:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175705.66 epoch sec; seqNo:11952; 4058975.59 north; 592421.67 east 2021-02-13T00:21:46Z- AHRS_M2 msg: 1613175706.05 epoch sec; seqNo:11857 2021-02-13T00:21:46Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.12 psi 2021-02-13T00:21:46Z- handleDepth()-Depth_Keller msg: 1613175706069 epoch sec; seqNo:11953; depth 13.57 2021-02-13T00:21:46Z- handleDvl() - RDI_Pathfinder msg: 1613175706.08 epoch sec; seqNo:5913 2021-02-13T00:21:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.78 , 95.95 , 73.56 , 110.83 2021-02-13T00:21:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.15 , 0.01 2021-02-13T00:21:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175706.09 epoch sec; seqNo:11953; 4058975.97 north; 592421.66 east 2021-02-13T00:21:46Z- AHRS_M2 msg: 1613175706.45 epoch sec; seqNo:11858 2021-02-13T00:21:46Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.13 psi 2021-02-13T00:21:46Z- handleDepth()-Depth_Keller msg: 1613175706456 epoch sec; seqNo:11954; depth 13.70 2021-02-13T00:21:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175706.47 epoch sec; seqNo:11954; 4058976.34 north; 592421.66 east 2021-02-13T00:21:46Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:21:46Z- waiting for 1613175704.24, time is 1613175706.47 2021-02-13T00:21:46Z- LcmTrn::updateTrn() >>>> pose time 1613175706.47, meas time 1613175706.08 <<<< 2021-02-13T00:21:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175706.08 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:21:46Z- TerrainNav::motionUpdate() - currEstimate = (4058976, 592421.66, 13.70) 2021-02-13T00:21:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:21:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:21:46Z- TNavBF::Projecting Measurements 2021-02-13T00:21:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:21:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:21:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:21:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:21:46Z- TNavFilter::4 good beams. 2021-02-13T00:21:46Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:21:46Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:21:46Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:21:46Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:21:46Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:21:46Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:21:46Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:21:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:21:46Z- TerrainMapOctree: (vehN, vehE ) = (4058976.06, 592421.48). 2021-02-13T00:21:46Z- TerrainMapOctree: Min Distance = 2917.92. 2021-02-13T00:21:46Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:21:46Z- There is no specific good beam that all particles have in common. 2021-02-13T00:21:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:21:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 11.23 2021-02-13T00:21:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:21:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 14.73 2021-02-13T00:21:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:21:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 8.66 2021-02-13T00:21:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:21:46Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 19.65 2021-02-13T00:21:46Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:21:46Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:21:46Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:21:46Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:21:46Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:21:46Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:21:46Z- Filter 0 No beams used in subcloud update 2021-02-13T00:21:46Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:21:46Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:21:46Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 706.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:21:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175706.08 sec. 2021-02-13T00:21:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:21:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:21:46Z- variances([0], [2], and psi_berg) = 706.534704 833.001498 0.000000 2021-02-13T00:21:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:21:46Z- LcmTrn::publishEstimates() - MLE : 4058975.84 592464.16 13.70 2021-02-13T00:21:46Z- LcmTrn::publishEstimates() - MMSE : 4058976.34 592421.66 13.70 2021-02-13T00:21:46Z- LcmTrn::publishEstimates() - COVAR: 706.53 833.00 0.96 0.00 2021-02-13T00:21:46Z- AHRS_M2 msg: 1613175706.86 epoch sec; seqNo:11859 2021-02-13T00:21:46Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.13 psi 2021-02-13T00:21:46Z- handleDepth()-Depth_Keller msg: 1613175706869 epoch sec; seqNo:11955; depth 13.81 2021-02-13T00:21:46Z- handleDvl() - RDI_Pathfinder msg: 1613175706.88 epoch sec; seqNo:5914 2021-02-13T00:21:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.64 , 74.38 , 111.03 2021-02-13T00:21:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.15 , 0.08 2021-02-13T00:21:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175706.89 epoch sec; seqNo:11955; 4058976.74 north; 592422.88 east 2021-02-13T00:21:47Z- AHRS_M2 msg: 1613175707.26 epoch sec; seqNo:11860 2021-02-13T00:21:47Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.14 psi 2021-02-13T00:21:47Z- handleDepth()-Depth_Keller msg: 1613175707275 epoch sec; seqNo:11956; depth 13.94 2021-02-13T00:21:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175707.29 epoch sec; seqNo:11956; 4058976.74 north; 592422.88 east 2021-02-13T00:21:47Z- AHRS_M2 msg: 1613175707.66 epoch sec; seqNo:11861 2021-02-13T00:21:47Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.14 psi 2021-02-13T00:21:47Z- handleDepth()-Depth_Keller msg: 1613175707673 epoch sec; seqNo:11957; depth 14.05 2021-02-13T00:21:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175707.70 epoch sec; seqNo:11957; 4058977.11 north; 592422.87 east 2021-02-13T00:21:48Z- AHRS_M2 msg: 1613175708.07 epoch sec; seqNo:11862 2021-02-13T00:21:48Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.15 psi 2021-02-13T00:21:48Z- handleDepth()-Depth_Keller msg: 1613175708077 epoch sec; seqNo:11958; depth 14.16 2021-02-13T00:21:48Z- handleDvl() - RDI_Pathfinder msg: 1613175708.09 epoch sec; seqNo:5915 2021-02-13T00:21:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.51 , 74.79 , 111.03 2021-02-13T00:21:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.09 , 0.08 2021-02-13T00:21:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175708.11 epoch sec; seqNo:11958; 4058977.49 north; 592422.87 east 2021-02-13T00:21:48Z- AHRS_M2 msg: 1613175708.47 epoch sec; seqNo:11863 2021-02-13T00:21:48Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.15 psi 2021-02-13T00:21:48Z- handleDepth()-Depth_Keller msg: 1613175708477 epoch sec; seqNo:11959; depth 14.28 2021-02-13T00:21:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175708.49 epoch sec; seqNo:11959; 4058977.87 north; 592422.86 east 2021-02-13T00:21:48Z- AHRS_M2 msg: 1613175708.88 epoch sec; seqNo:11864 2021-02-13T00:21:48Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.16 psi 2021-02-13T00:21:48Z- handleDepth()-Depth_Keller msg: 1613175708896 epoch sec; seqNo:11960; depth 14.40 2021-02-13T00:21:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175708.92 epoch sec; seqNo:11960; 4058977.87 north; 592422.86 east 2021-02-13T00:21:49Z- AHRS_M2 msg: 1613175709.28 epoch sec; seqNo:11865 2021-02-13T00:21:49Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.17 psi 2021-02-13T00:21:49Z- handleDepth()-Depth_Keller msg: 1613175709284 epoch sec; seqNo:11961; depth 14.52 2021-02-13T00:21:49Z- handleDvl() - RDI_Pathfinder msg: 1613175709.31 epoch sec; seqNo:5916 2021-02-13T00:21:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 97.51 , 78.58 , 106.97 2021-02-13T00:21:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.07 , 0.03 2021-02-13T00:21:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175709.32 epoch sec; seqNo:11961; 4058978.25 north; 592422.86 east 2021-02-13T00:21:49Z- AHRS_M2 msg: 1613175709.68 epoch sec; seqNo:11866 2021-02-13T00:21:49Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.17 psi 2021-02-13T00:21:49Z- handleDepth()-Depth_Keller msg: 1613175709690 epoch sec; seqNo:11962; depth 14.64 2021-02-13T00:21:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175709.70 epoch sec; seqNo:11962; 4058978.64 north; 592424.08 east 2021-02-13T00:21:50Z- AHRS_M2 msg: 1613175710.09 epoch sec; seqNo:11867 2021-02-13T00:21:50Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.17 psi 2021-02-13T00:21:50Z- handleDepth()-Depth_Keller msg: 1613175710102 epoch sec; seqNo:11963; depth 14.76 2021-02-13T00:21:50Z- handleDvl() - RDI_Pathfinder msg: 1613175710.11 epoch sec; seqNo:5917 2021-02-13T00:21:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 99.06 , 74.38 , 109.88 2021-02-13T00:21:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , -0.01 2021-02-13T00:21:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175710.13 epoch sec; seqNo:11963; 4058979.02 north; 592424.07 east 2021-02-13T00:21:50Z- AHRS_M2 msg: 1613175710.49 epoch sec; seqNo:11868 2021-02-13T00:21:50Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.17 psi 2021-02-13T00:21:50Z- handleDepth()-Depth_Keller msg: 1613175710501 epoch sec; seqNo:11964; depth 14.87 2021-02-13T00:21:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175710.51 epoch sec; seqNo:11964; 4058979.40 north; 592424.07 east 2021-02-13T00:21:50Z- AHRS_M2 msg: 1613175710.90 epoch sec; seqNo:11869 2021-02-13T00:21:50Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.17 psi 2021-02-13T00:21:50Z- handleDepth()-Depth_Keller msg: 1613175710905 epoch sec; seqNo:11965; depth 14.99 2021-02-13T00:21:50Z- handleDvl() - RDI_Pathfinder msg: 1613175710.92 epoch sec; seqNo:5918 2021-02-13T00:21:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 96.43 , 71.93 , 111.03 2021-02-13T00:21:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , -0.04 2021-02-13T00:21:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175710.93 epoch sec; seqNo:11965; 4058979.40 north; 592424.07 east 2021-02-13T00:21:51Z- AHRS_M2 msg: 1613175711.30 epoch sec; seqNo:11870 2021-02-13T00:21:51Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.17 psi 2021-02-13T00:21:51Z- handleDepth()-Depth_Keller msg: 1613175711304 epoch sec; seqNo:11966; depth 15.11 2021-02-13T00:21:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175711.33 epoch sec; seqNo:11966; 4058979.78 north; 592424.06 east 2021-02-13T00:21:51Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-13T00:21:51Z- waiting for 1613175709.47, time is 1613175711.33 2021-02-13T00:21:51Z- LcmTrn::updateTrn() >>>> pose time 1613175711.33, meas time 1613175710.92 <<<< 2021-02-13T00:21:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175710.92 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:21:51Z- TerrainNav::motionUpdate() - currEstimate = (4058980, 592424.06, 15.11) 2021-02-13T00:21:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:21:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:21:51Z- TNavBF::Projecting Measurements 2021-02-13T00:21:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:21:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:21:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:21:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:21:51Z- TNavFilter::4 good beams. 2021-02-13T00:21:51Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:21:51Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:21:51Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:21:51Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:21:51Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:21:51Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:21:51Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:21:51Z- TNavBF::mmse selected from filter -1 2021-02-13T00:21:51Z- TerrainMapOctree: (vehN, vehE ) = (4058979.49, 592423.86). 2021-02-13T00:21:51Z- TerrainMapOctree: Min Distance = 2913.76. 2021-02-13T00:21:51Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:21:51Z- There is no specific good beam that all particles have in common. 2021-02-13T00:21:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:21:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 10.71 2021-02-13T00:21:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:21:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 14.88 2021-02-13T00:21:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:21:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 8.28 2021-02-13T00:21:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:21:51Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 19.72 2021-02-13T00:21:51Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:21:51Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:21:51Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:21:51Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:21:51Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:21:51Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:21:51Z- Filter 0 No beams used in subcloud update 2021-02-13T00:21:51Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:21:51Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:21:51Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 706.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:21:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175710.92 sec. 2021-02-13T00:21:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:21:51Z- TNavBF::mmse selected from filter -1 2021-02-13T00:21:51Z- variances([0], [2], and psi_berg) = 706.460584 833.001456 0.000000 2021-02-13T00:21:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:21:51Z- LcmTrn::publishEstimates() - MLE : 4058979.28 592473.56 15.11 2021-02-13T00:21:51Z- LcmTrn::publishEstimates() - MMSE : 4058979.78 592424.06 15.11 2021-02-13T00:21:51Z- LcmTrn::publishEstimates() - COVAR: 706.46 833.00 0.89 0.00 2021-02-13T00:21:51Z- AHRS_M2 msg: 1613175711.70 epoch sec; seqNo:11871 2021-02-13T00:21:51Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.17 psi 2021-02-13T00:21:51Z- handleDepth()-Depth_Keller msg: 1613175711713 epoch sec; seqNo:11967; depth 15.22 2021-02-13T00:21:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175711.72 epoch sec; seqNo:11967; 4058980.16 north; 592424.06 east 2021-02-13T00:21:52Z- AHRS_M2 msg: 1613175712.11 epoch sec; seqNo:11872 2021-02-13T00:21:52Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.17 psi 2021-02-13T00:21:52Z- handleDepth()-Depth_Keller msg: 1613175712112 epoch sec; seqNo:11968; depth 15.34 2021-02-13T00:21:52Z- handleDvl() - RDI_Pathfinder msg: 1613175712.13 epoch sec; seqNo:5919 2021-02-13T00:21:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 97.71 , 0.00 , 108.66 2021-02-13T00:21:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.06 , -0.03 2021-02-13T00:21:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175712.14 epoch sec; seqNo:11968; 4058980.55 north; 592425.28 east 2021-02-13T00:21:52Z- AHRS_M2 msg: 1613175712.51 epoch sec; seqNo:11873 2021-02-13T00:21:52Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.17 psi 2021-02-13T00:21:52Z- handleDepth()-Depth_Keller msg: 1613175712521 epoch sec; seqNo:11969; depth 15.46 2021-02-13T00:21:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175712.53 epoch sec; seqNo:11969; 4058980.55 north; 592425.28 east 2021-02-13T00:21:52Z- AHRS_M2 msg: 1613175712.91 epoch sec; seqNo:11874 2021-02-13T00:21:52Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.16 psi 2021-02-13T00:21:52Z- handleDepth()-Depth_Keller msg: 1613175712925 epoch sec; seqNo:11970; depth 15.57 2021-02-13T00:21:52Z- handleDvl() - RDI_Pathfinder msg: 1613175712.94 epoch sec; seqNo:5920 2021-02-13T00:21:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.02 , 78.44 , 110.89 2021-02-13T00:21:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.06 , 0.01 2021-02-13T00:21:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175712.95 epoch sec; seqNo:11970; 4058980.93 north; 592425.27 east 2021-02-13T00:21:53Z- AHRS_M2 msg: 1613175713.32 epoch sec; seqNo:11875 2021-02-13T00:21:53Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.16 psi 2021-02-13T00:21:53Z- handleDepth()-Depth_Keller msg: 1613175713324 epoch sec; seqNo:11971; depth 15.70 2021-02-13T00:21:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175713.34 epoch sec; seqNo:11971; 4058981.31 north; 592425.27 east 2021-02-13T00:21:53Z- AHRS_M2 msg: 1613175713.72 epoch sec; seqNo:11876 2021-02-13T00:21:53Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.17 psi 2021-02-13T00:21:53Z- handleDepth()-Depth_Keller msg: 1613175713729 epoch sec; seqNo:11972; depth 15.81 2021-02-13T00:21:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175713.74 epoch sec; seqNo:11972; 4058981.31 north; 592425.27 east 2021-02-13T00:21:54Z- AHRS_M2 msg: 1613175714.13 epoch sec; seqNo:11877 2021-02-13T00:21:54Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.17 psi 2021-02-13T00:21:54Z- handleDepth()-Depth_Keller msg: 1613175714143 epoch sec; seqNo:11973; depth 15.92 2021-02-13T00:21:54Z- handleDvl() - RDI_Pathfinder msg: 1613175714.16 epoch sec; seqNo:5921 2021-02-13T00:21:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.44 , 78.51 , 109.61 2021-02-13T00:21:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.04 2021-02-13T00:21:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175714.17 epoch sec; seqNo:11973; 4058981.69 north; 592425.26 east 2021-02-13T00:21:54Z- AHRS_M2 msg: 1613175714.53 epoch sec; seqNo:11878 2021-02-13T00:21:54Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.18 psi 2021-02-13T00:21:54Z- handleDepth()-Depth_Keller msg: 1613175714541 epoch sec; seqNo:11974; depth 16.04 2021-02-13T00:21:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175714.55 epoch sec; seqNo:11974; 4058982.07 north; 592425.26 east 2021-02-13T00:21:54Z- AHRS_M2 msg: 1613175714.93 epoch sec; seqNo:11879 2021-02-13T00:21:54Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.18 psi 2021-02-13T00:21:54Z- handleDepth()-Depth_Keller msg: 1613175714945 epoch sec; seqNo:11975; depth 16.16 2021-02-13T00:21:54Z- handleDvl() - RDI_Pathfinder msg: 1613175714.96 epoch sec; seqNo:5922 2021-02-13T00:21:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.21 , 98.79 , 78.51 , 110.96 2021-02-13T00:21:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.05 2021-02-13T00:21:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175714.99 epoch sec; seqNo:11975; 4058982.08 north; 592426.48 east 2021-02-13T00:21:55Z- AHRS_M2 msg: 1613175715.35 epoch sec; seqNo:11880 2021-02-13T00:21:55Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.18 psi 2021-02-13T00:21:55Z- handleDepth()-Depth_Keller msg: 1613175715350 epoch sec; seqNo:11976; depth 16.29 2021-02-13T00:21:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175715.36 epoch sec; seqNo:11976; 4058982.46 north; 592426.48 east 2021-02-13T00:21:55Z- AHRS_M2 msg: 1613175715.74 epoch sec; seqNo:11881 2021-02-13T00:21:55Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.18 psi 2021-02-13T00:21:55Z- handleDepth()-Depth_Keller msg: 1613175715748 epoch sec; seqNo:11977; depth 16.40 2021-02-13T00:21:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175715.76 epoch sec; seqNo:11977; 4058982.84 north; 592426.47 east 2021-02-13T00:21:56Z- AHRS_M2 msg: 1613175716.15 epoch sec; seqNo:11882 2021-02-13T00:21:56Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.18 psi 2021-02-13T00:21:56Z- handleDepth()-Depth_Keller msg: 1613175716161 epoch sec; seqNo:11978; depth 16.52 2021-02-13T00:21:56Z- handleDvl() - RDI_Pathfinder msg: 1613175716.17 epoch sec; seqNo:5923 2021-02-13T00:21:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.48 , 97.71 , 73.56 , 109.88 2021-02-13T00:21:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.02 2021-02-13T00:21:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175716.20 epoch sec; seqNo:11978; 4058982.84 north; 592426.47 east 2021-02-13T00:21:56Z- AHRS_M2 msg: 1613175716.55 epoch sec; seqNo:11883 2021-02-13T00:21:56Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.19 psi 2021-02-13T00:21:56Z- handleDepth()-Depth_Keller msg: 1613175716561 epoch sec; seqNo:11979; depth 16.65 2021-02-13T00:21:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175716.57 epoch sec; seqNo:11979; 4058983.22 north; 592426.47 east 2021-02-13T00:21:56Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-13T00:21:56Z- waiting for 1613175714.33, time is 1613175716.57 2021-02-13T00:21:56Z- LcmTrn::updateTrn() >>>> pose time 1613175716.57, meas time 1613175716.17 <<<< 2021-02-13T00:21:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175716.17 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:21:56Z- TerrainNav::motionUpdate() - currEstimate = (4058983, 592426.47, 16.65) 2021-02-13T00:21:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:21:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:21:56Z- TNavBF::Projecting Measurements 2021-02-13T00:21:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:21:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:21:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:21:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:21:56Z- TNavFilter::4 good beams. 2021-02-13T00:21:56Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:21:56Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:21:56Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:21:56Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:21:56Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:21:56Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:21:56Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:21:56Z- TNavBF::mmse selected from filter -1 2021-02-13T00:21:56Z- TerrainMapOctree: (vehN, vehE ) = (4058982.96, 592426.29). 2021-02-13T00:21:56Z- TerrainMapOctree: Min Distance = 2909.54. 2021-02-13T00:21:56Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:21:56Z- There is no specific good beam that all particles have in common. 2021-02-13T00:21:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:21:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 10.62 2021-02-13T00:21:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:21:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:21:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:21:56Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 8.66 2021-02-13T00:21:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:21:56Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 19.32 2021-02-13T00:21:56Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:21:56Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:21:56Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:21:56Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:21:56Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:21:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:21:56Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:21:56Z- Filter 0 No beams used in subcloud update 2021-02-13T00:21:56Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:21:56Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:21:56Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 706.4 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:21:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175716.17 sec. 2021-02-13T00:21:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:21:56Z- TNavBF::mmse selected from filter -1 2021-02-13T00:21:56Z- variances([0], [2], and psi_berg) = 706.387120 833.001415 0.000000 2021-02-13T00:21:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:21:56Z- LcmTrn::publishEstimates() - MLE : 4058982.72 592475.97 16.65 2021-02-13T00:21:56Z- LcmTrn::publishEstimates() - MMSE : 4058983.22 592426.47 16.65 2021-02-13T00:21:56Z- LcmTrn::publishEstimates() - COVAR: 706.39 833.00 0.85 0.00 2021-02-13T00:21:56Z- AHRS_M2 msg: 1613175716.96 epoch sec; seqNo:11884 2021-02-13T00:21:56Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.19 psi 2021-02-13T00:21:56Z- handleDepth()-Depth_Keller msg: 1613175716962 epoch sec; seqNo:11980; depth 16.76 2021-02-13T00:21:56Z- handleDvl() - RDI_Pathfinder msg: 1613175716.97 epoch sec; seqNo:5924 2021-02-13T00:21:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.67 , 98.93 , 78.24 , 109.88 2021-02-13T00:21:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , -0.03 2021-02-13T00:21:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175716.98 epoch sec; seqNo:11980; 4058983.61 north; 592427.69 east 2021-02-13T00:21:57Z- AHRS_M2 msg: 1613175717.36 epoch sec; seqNo:11885 2021-02-13T00:21:57Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.19 psi 2021-02-13T00:21:57Z- handleDepth()-Depth_Keller msg: 1613175717364 epoch sec; seqNo:11981; depth 16.90 2021-02-13T00:21:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175717.38 epoch sec; seqNo:11981; 4058983.99 north; 592427.68 east 2021-02-13T00:21:57Z- AHRS_M2 msg: 1613175717.76 epoch sec; seqNo:11886 2021-02-13T00:21:57Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.19 psi 2021-02-13T00:21:57Z- handleDepth()-Depth_Keller msg: 1613175717768 epoch sec; seqNo:11982; depth 17.02 2021-02-13T00:21:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175717.78 epoch sec; seqNo:11982; 4058983.99 north; 592427.68 east 2021-02-13T00:21:58Z- AHRS_M2 msg: 1613175718.17 epoch sec; seqNo:11887 2021-02-13T00:21:58Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.19 psi 2021-02-13T00:21:58Z- handleDepth()-Depth_Keller msg: 1613175718172 epoch sec; seqNo:11983; depth 17.15 2021-02-13T00:21:58Z- handleDvl() - RDI_Pathfinder msg: 1613175718.18 epoch sec; seqNo:5925 2021-02-13T00:21:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.13 , 99.06 , 70.70 , 108.53 2021-02-13T00:21:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.06 2021-02-13T00:21:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175718.19 epoch sec; seqNo:11983; 4058984.37 north; 592427.68 east 2021-02-13T00:21:58Z- AHRS_M2 msg: 1613175718.57 epoch sec; seqNo:11888 2021-02-13T00:21:58Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.19 psi 2021-02-13T00:21:58Z- handleDepth()-Depth_Keller msg: 1613175718585 epoch sec; seqNo:11984; depth 17.28 2021-02-13T00:21:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175718.60 epoch sec; seqNo:11984; 4058984.74 north; 592427.67 east 2021-02-13T00:21:58Z- AHRS_M2 msg: 1613175718.98 epoch sec; seqNo:11889 2021-02-13T00:21:58Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.19 psi 2021-02-13T00:21:58Z- handleDepth()-Depth_Keller msg: 1613175718981 epoch sec; seqNo:11985; depth 17.42 2021-02-13T00:21:59Z- handleDvl() - RDI_Pathfinder msg: 1613175719.00 epoch sec; seqNo:5926 2021-02-13T00:21:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.06 , 97.78 , 78.31 , 108.73 2021-02-13T00:21:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.11 , -0.07 2021-02-13T00:21:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175719.01 epoch sec; seqNo:11985; 4058985.12 north; 592427.67 east 2021-02-13T00:21:59Z- AHRS_M2 msg: 1613175719.38 epoch sec; seqNo:11890 2021-02-13T00:21:59Z- AHRS_M2 msg: 0.70 phi; -0.33 theta; -0.19 psi 2021-02-13T00:21:59Z- handleDepth()-Depth_Keller msg: 1613175719389 epoch sec; seqNo:11986; depth 17.54 2021-02-13T00:21:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175719.40 epoch sec; seqNo:11986; 4058985.14 north; 592428.89 east 2021-02-13T00:21:59Z- AHRS_M2 msg: 1613175719.79 epoch sec; seqNo:11891 2021-02-13T00:21:59Z- AHRS_M2 msg: 0.69 phi; -0.33 theta; -0.19 psi 2021-02-13T00:21:59Z- handleDepth()-Depth_Keller msg: 1613175719790 epoch sec; seqNo:11987; depth 17.66 2021-02-13T00:21:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175719.80 epoch sec; seqNo:11987; 4058985.52 north; 592428.89 east 2021-02-13T00:22:00Z- AHRS_M2 msg: 1613175720.19 epoch sec; seqNo:11892 2021-02-13T00:22:00Z- AHRS_M2 msg: 0.69 phi; -0.33 theta; -0.19 psi 2021-02-13T00:22:00Z- handleDepth()-Depth_Keller msg: 1613175720197 epoch sec; seqNo:11988; depth 17.78 2021-02-13T00:22:00Z- handleDvl() - RDI_Pathfinder msg: 1613175720.21 epoch sec; seqNo:5927 2021-02-13T00:22:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.46 , 97.71 , 70.70 , 108.66 2021-02-13T00:22:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.14 , -0.02 2021-02-13T00:22:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175720.23 epoch sec; seqNo:11988; 4058985.89 north; 592428.88 east 2021-02-13T00:22:00Z- AHRS_M2 msg: 1613175720.59 epoch sec; seqNo:11893 2021-02-13T00:22:00Z- AHRS_M2 msg: 0.69 phi; -0.33 theta; -0.20 psi 2021-02-13T00:22:00Z- handleDepth()-Depth_Keller msg: 1613175720599 epoch sec; seqNo:11989; depth 17.91 2021-02-13T00:22:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175720.61 epoch sec; seqNo:11989; 4058986.27 north; 592428.88 east 2021-02-13T00:22:01Z- AHRS_M2 msg: 1613175720.99 epoch sec; seqNo:11894 2021-02-13T00:22:01Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.20 psi 2021-02-13T00:22:01Z- handleDepth()-Depth_Keller msg: 1613175721003 epoch sec; seqNo:11990; depth 18.03 2021-02-13T00:22:01Z- handleDvl() - RDI_Pathfinder msg: 1613175721.01 epoch sec; seqNo:5928 2021-02-13T00:22:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.85 , 71.52 , 108.80 2021-02-13T00:22:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.11 , 0.02 2021-02-13T00:22:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175721.02 epoch sec; seqNo:11990; 4058986.27 north; 592428.88 east 2021-02-13T00:22:01Z- AHRS_M2 msg: 1613175721.40 epoch sec; seqNo:11895 2021-02-13T00:22:01Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.20 psi 2021-02-13T00:22:01Z- handleDepth()-Depth_Keller msg: 1613175721405 epoch sec; seqNo:11991; depth 18.15 2021-02-13T00:22:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175721.45 epoch sec; seqNo:11991; 4058986.65 north; 592428.87 east 2021-02-13T00:22:01Z- handleDepth()-Depth_Keller msg: 1613175721821 epoch sec; seqNo:11992; depth 18.26 2021-02-13T00:22:01Z- handleDvl() - RDI_Pathfinder msg: 1613175721.84 epoch sec; seqNo:5929 2021-02-13T00:22:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 97.64 , 70.29 , 108.46 2021-02-13T00:22:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.04 2021-02-13T00:22:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175721.85 epoch sec; seqNo:11992; 4058987.03 north; 592428.87 east 2021-02-13T00:22:01Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-13T00:22:01Z- waiting for 1613175719.57, time is 1613175721.85 2021-02-13T00:22:01Z- LcmTrn::updateTrn() equating pose 1613175721.85 and meas times 1613175721.85 2021-02-13T00:22:01Z- LcmTrn::updateTrn() >>>> pose time 1613175721.85, meas time 1613175721.85 <<<< 2021-02-13T00:22:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175721.85 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:22:01Z- TerrainNav::motionUpdate() - currEstimate = (4058987, 592428.87, 18.26) 2021-02-13T00:22:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:22:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:22:01Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175721.85 sec.; No relevant navigation data available 2021-02-13T00:22:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:22:01Z- TNavBF::mmse selected from filter -1 2021-02-13T00:22:01Z- variances([0], [2], and psi_berg) = 706.325569 833.001380 0.000000 2021-02-13T00:22:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:22:01Z- LcmTrn::publishEstimates() - MLE : 4058986.53 592478.37 18.26 2021-02-13T00:22:01Z- LcmTrn::publishEstimates() - MMSE : 4058987.03 592428.87 18.26 2021-02-13T00:22:01Z- LcmTrn::publishEstimates() - COVAR: 706.33 833.00 1.08 0.00 2021-02-13T00:22:02Z- AHRS_M2 msg: 1613175722.21 epoch sec; seqNo:11896 2021-02-13T00:22:02Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.21 psi 2021-02-13T00:22:02Z- handleDepth()-Depth_Keller msg: 1613175722228 epoch sec; seqNo:11993; depth 18.41 2021-02-13T00:22:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175722.24 epoch sec; seqNo:11993; 4058987.42 north; 592430.09 east 2021-02-13T00:22:02Z- AHRS_M2 msg: 1613175722.61 epoch sec; seqNo:11897 2021-02-13T00:22:02Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.21 psi 2021-02-13T00:22:02Z- handleDepth()-Depth_Keller msg: 1613175722623 epoch sec; seqNo:11994; depth 18.52 2021-02-13T00:22:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175722.64 epoch sec; seqNo:11994; 4058987.42 north; 592430.09 east 2021-02-13T00:22:03Z- AHRS_M2 msg: 1613175723.02 epoch sec; seqNo:11898 2021-02-13T00:22:03Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.20 psi 2021-02-13T00:22:03Z- handleDepth()-Depth_Keller msg: 1613175723026 epoch sec; seqNo:11995; depth 18.65 2021-02-13T00:22:03Z- handleDvl() - RDI_Pathfinder msg: 1613175723.04 epoch sec; seqNo:5930 2021-02-13T00:22:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 96.70 , 71.11 , 107.65 2021-02-13T00:22:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , 0.00 2021-02-13T00:22:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175723.06 epoch sec; seqNo:11995; 4058987.80 north; 592430.08 east 2021-02-13T00:22:03Z- AHRS_M2 msg: 1613175723.42 epoch sec; seqNo:11899 2021-02-13T00:22:03Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.20 psi 2021-02-13T00:22:03Z- handleDepth()-Depth_Keller msg: 1613175723427 epoch sec; seqNo:11996; depth 18.78 2021-02-13T00:22:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175723.44 epoch sec; seqNo:11996; 4058988.18 north; 592430.08 east 2021-02-13T00:22:03Z- AHRS_M2 msg: 1613175723.83 epoch sec; seqNo:11900 2021-02-13T00:22:03Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.20 psi 2021-02-13T00:22:03Z- handleDepth()-Depth_Keller msg: 1613175723840 epoch sec; seqNo:11997; depth 18.90 2021-02-13T00:22:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175723.86 epoch sec; seqNo:11997; 4058988.18 north; 592430.08 east 2021-02-13T00:22:04Z- AHRS_M2 msg: 1613175724.23 epoch sec; seqNo:11901 2021-02-13T00:22:04Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.20 psi 2021-02-13T00:22:04Z- handleDepth()-Depth_Keller msg: 1613175724232 epoch sec; seqNo:11998; depth 19.02 2021-02-13T00:22:04Z- handleDvl() - RDI_Pathfinder msg: 1613175724.26 epoch sec; seqNo:5931 2021-02-13T00:22:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 69.07 , 110.08 2021-02-13T00:22:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , -0.03 2021-02-13T00:22:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175724.27 epoch sec; seqNo:11998; 4058988.56 north; 592430.07 east 2021-02-13T00:22:04Z- AHRS_M2 msg: 1613175724.63 epoch sec; seqNo:11902 2021-02-13T00:22:04Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.20 psi 2021-02-13T00:22:04Z- handleDepth()-Depth_Keller msg: 1613175724637 epoch sec; seqNo:11999; depth 19.16 2021-02-13T00:22:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175724.65 epoch sec; seqNo:11999; 4058988.95 north; 592431.29 east 2021-02-13T00:22:05Z- AHRS_M2 msg: 1613175725.04 epoch sec; seqNo:11903 2021-02-13T00:22:05Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.20 psi 2021-02-13T00:22:05Z- handleDepth()-Depth_Keller msg: 1613175725049 epoch sec; seqNo:12000; depth 19.29 2021-02-13T00:22:05Z- handleDvl() - RDI_Pathfinder msg: 1613175725.06 epoch sec; seqNo:5932 2021-02-13T00:22:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 97.85 , 78.37 , 107.58 2021-02-13T00:22:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.02 2021-02-13T00:22:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175725.07 epoch sec; seqNo:12000; 4058988.95 north; 592431.29 east 2021-02-13T00:22:05Z- AHRS_M2 msg: 1613175725.44 epoch sec; seqNo:11904 2021-02-13T00:22:05Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.20 psi 2021-02-13T00:22:05Z- handleDepth()-Depth_Keller msg: 1613175725449 epoch sec; seqNo:12001; depth 19.43 2021-02-13T00:22:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175725.47 epoch sec; seqNo:12001; 4058989.33 north; 592431.29 east 2021-02-13T00:22:05Z- AHRS_M2 msg: 1613175725.86 epoch sec; seqNo:11905 2021-02-13T00:22:05Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.20 psi 2021-02-13T00:22:05Z- handleDepth()-Depth_Keller msg: 1613175725878 epoch sec; seqNo:12002; depth 19.55 2021-02-13T00:22:05Z- handleDvl() - RDI_Pathfinder msg: 1613175725.89 epoch sec; seqNo:5933 2021-02-13T00:22:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 96.56 , 70.70 , 106.30 2021-02-13T00:22:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , 0.03 2021-02-13T00:22:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175725.90 epoch sec; seqNo:12002; 4058989.71 north; 592431.28 east 2021-02-13T00:22:06Z- AHRS_M2 msg: 1613175726.25 epoch sec; seqNo:11906 2021-02-13T00:22:06Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.20 psi 2021-02-13T00:22:06Z- handleDepth()-Depth_Keller msg: 1613175726266 epoch sec; seqNo:12003; depth 19.69 2021-02-13T00:22:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175726.28 epoch sec; seqNo:12003; 4058989.71 north; 592431.28 east 2021-02-13T00:22:06Z- AHRS_M2 msg: 1613175726.65 epoch sec; seqNo:11907 2021-02-13T00:22:06Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.19 psi 2021-02-13T00:22:06Z- handleDepth()-Depth_Keller msg: 1613175726659 epoch sec; seqNo:12004; depth 19.80 2021-02-13T00:22:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175726.67 epoch sec; seqNo:12004; 4058990.09 north; 592431.28 east 2021-02-13T00:22:06Z- LcmTrn::handleMessages() - handling 40 messages took 0.55 ms and 129 calls... 2021-02-13T00:22:06Z- waiting for 1613175724.85, time is 1613175726.67 2021-02-13T00:22:06Z- LcmTrn::updateTrn() >>>> pose time 1613175726.67, meas time 1613175725.89 <<<< 2021-02-13T00:22:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175725.89 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:22:06Z- TerrainNav::motionUpdate() - currEstimate = (4058990, 592431.28, 19.80) 2021-02-13T00:22:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:22:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:22:06Z- TNavBF::Projecting Measurements 2021-02-13T00:22:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:22:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:22:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:22:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:22:06Z- TNavFilter::4 good beams. 2021-02-13T00:22:06Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:22:06Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:22:06Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:22:06Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:22:06Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:22:06Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:22:06Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:22:06Z- TNavBF::mmse selected from filter -1 2021-02-13T00:22:06Z- TerrainMapOctree: (vehN, vehE ) = (4058989.59, 592430.89). 2021-02-13T00:22:06Z- TerrainMapOctree: Min Distance = 2901.48. 2021-02-13T00:22:06Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:22:06Z- There is no specific good beam that all particles have in common. 2021-02-13T00:22:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:22:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 9.81 2021-02-13T00:22:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:22:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 14.92 2021-02-13T00:22:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:22:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-13T00:22:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:22:06Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 18.08 2021-02-13T00:22:06Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:22:06Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:22:06Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:22:06Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:22:06Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:22:06Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:22:06Z- Filter 0 No beams used in subcloud update 2021-02-13T00:22:06Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:22:06Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:22:06Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 706.3 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:22:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175725.89 sec. 2021-02-13T00:22:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:22:06Z- TNavBF::mmse selected from filter -1 2021-02-13T00:22:06Z- variances([0], [2], and psi_berg) = 706.250273 833.001338 0.000000 2021-02-13T00:22:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:22:06Z- LcmTrn::publishEstimates() - MLE : 4058989.59 592480.78 19.80 2021-02-13T00:22:06Z- LcmTrn::publishEstimates() - MMSE : 4058990.09 592431.28 19.80 2021-02-13T00:22:06Z- LcmTrn::publishEstimates() - COVAR: 706.25 833.00 0.94 0.00 2021-02-13T00:22:07Z- AHRS_M2 msg: 1613175727.06 epoch sec; seqNo:11908 2021-02-13T00:22:07Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.19 psi 2021-02-13T00:22:07Z- handleDepth()-Depth_Keller msg: 1613175727065 epoch sec; seqNo:12005; depth 19.92 2021-02-13T00:22:07Z- handleDvl() - RDI_Pathfinder msg: 1613175727.08 epoch sec; seqNo:5934 2021-02-13T00:22:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 69.48 , 107.65 2021-02-13T00:22:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , 0.07 2021-02-13T00:22:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175727.09 epoch sec; seqNo:12005; 4058990.48 north; 592432.49 east 2021-02-13T00:22:07Z- AHRS_M2 msg: 1613175727.46 epoch sec; seqNo:11909 2021-02-13T00:22:07Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.19 psi 2021-02-13T00:22:07Z- handleDepth()-Depth_Keller msg: 1613175727464 epoch sec; seqNo:12006; depth 20.08 2021-02-13T00:22:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175727.47 epoch sec; seqNo:12006; 4058990.48 north; 592432.49 east 2021-02-13T00:22:07Z- AHRS_M2 msg: 1613175727.88 epoch sec; seqNo:11910 2021-02-13T00:22:07Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.19 psi 2021-02-13T00:22:07Z- handleDepth()-Depth_Keller msg: 1613175727879 epoch sec; seqNo:12007; depth 20.19 2021-02-13T00:22:07Z- handleDvl() - RDI_Pathfinder msg: 1613175727.89 epoch sec; seqNo:5935 2021-02-13T00:22:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 67.84 , 107.65 2021-02-13T00:22:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , 0.06 2021-02-13T00:22:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175727.92 epoch sec; seqNo:12007; 4058990.86 north; 592432.49 east 2021-02-13T00:22:08Z- AHRS_M2 msg: 1613175728.27 epoch sec; seqNo:11911 2021-02-13T00:22:08Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.19 psi 2021-02-13T00:22:08Z- handleDepth()-Depth_Keller msg: 1613175728274 epoch sec; seqNo:12008; depth 20.33 2021-02-13T00:22:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175728.29 epoch sec; seqNo:12008; 4058991.24 north; 592432.49 east 2021-02-13T00:22:08Z- AHRS_M2 msg: 1613175728.67 epoch sec; seqNo:11912 2021-02-13T00:22:08Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.20 psi 2021-02-13T00:22:08Z- handleDepth()-Depth_Keller msg: 1613175728686 epoch sec; seqNo:12009; depth 20.45 2021-02-13T00:22:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175728.69 epoch sec; seqNo:12009; 4058991.24 north; 592432.49 east 2021-02-13T00:22:09Z- AHRS_M2 msg: 1613175729.08 epoch sec; seqNo:11913 2021-02-13T00:22:09Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.20 psi 2021-02-13T00:22:09Z- handleDepth()-Depth_Keller msg: 1613175729080 epoch sec; seqNo:12010; depth 20.58 2021-02-13T00:22:09Z- handleDvl() - RDI_Pathfinder msg: 1613175729.09 epoch sec; seqNo:5936 2021-02-13T00:22:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 78.44 , 108.86 2021-02-13T00:22:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.05 , -0.00 2021-02-13T00:22:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175729.11 epoch sec; seqNo:12010; 4058991.62 north; 592432.48 east 2021-02-13T00:22:09Z- AHRS_M2 msg: 1613175729.48 epoch sec; seqNo:11914 2021-02-13T00:22:09Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.20 psi 2021-02-13T00:22:09Z- handleDepth()-Depth_Keller msg: 1613175729484 epoch sec; seqNo:12011; depth 20.73 2021-02-13T00:22:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175729.49 epoch sec; seqNo:12011; 4058992.01 north; 592433.70 east 2021-02-13T00:22:09Z- AHRS_M2 msg: 1613175729.88 epoch sec; seqNo:11915 2021-02-13T00:22:09Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.21 psi 2021-02-13T00:22:09Z- handleDepth()-Depth_Keller msg: 1613175729897 epoch sec; seqNo:12012; depth 20.83 2021-02-13T00:22:09Z- handleDvl() - RDI_Pathfinder msg: 1613175729.91 epoch sec; seqNo:5937 2021-02-13T00:22:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.79 , 97.71 , 66.62 , 107.45 2021-02-13T00:22:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.02 , -0.07 2021-02-13T00:22:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175729.92 epoch sec; seqNo:12012; 4058992.01 north; 592433.70 east 2021-02-13T00:22:10Z- AHRS_M2 msg: 1613175730.29 epoch sec; seqNo:11916 2021-02-13T00:22:10Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.21 psi 2021-02-13T00:22:10Z- handleDepth()-Depth_Keller msg: 1613175730297 epoch sec; seqNo:12013; depth 20.98 2021-02-13T00:22:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175730.31 epoch sec; seqNo:12013; 4058992.39 north; 592433.70 east 2021-02-13T00:22:10Z- AHRS_M2 msg: 1613175730.69 epoch sec; seqNo:11917 2021-02-13T00:22:10Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.21 psi 2021-02-13T00:22:10Z- handleDepth()-Depth_Keller msg: 1613175730697 epoch sec; seqNo:12014; depth 21.10 2021-02-13T00:22:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175730.70 epoch sec; seqNo:12014; 4058992.77 north; 592433.69 east 2021-02-13T00:22:11Z- AHRS_M2 msg: 1613175731.10 epoch sec; seqNo:11918 2021-02-13T00:22:11Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.21 psi 2021-02-13T00:22:11Z- handleDepth()-Depth_Keller msg: 1613175731106 epoch sec; seqNo:12015; depth 21.22 2021-02-13T00:22:11Z- handleDvl() - RDI_Pathfinder msg: 1613175731.12 epoch sec; seqNo:5938 2021-02-13T00:22:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 67.43 , 105.35 2021-02-13T00:22:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , -0.14 2021-02-13T00:22:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175731.13 epoch sec; seqNo:12015; 4058993.14 north; 592433.69 east 2021-02-13T00:22:11Z- AHRS_M2 msg: 1613175731.50 epoch sec; seqNo:11919 2021-02-13T00:22:11Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.21 psi 2021-02-13T00:22:11Z- handleDepth()-Depth_Keller msg: 1613175731504 epoch sec; seqNo:12016; depth 21.37 2021-02-13T00:22:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175731.52 epoch sec; seqNo:12016; 4058993.16 north; 592434.91 east 2021-02-13T00:22:11Z- AHRS_M2 msg: 1613175731.91 epoch sec; seqNo:11920 2021-02-13T00:22:11Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.21 psi 2021-02-13T00:22:11Z- handleDepth()-Depth_Keller msg: 1613175731917 epoch sec; seqNo:12017; depth 21.49 2021-02-13T00:22:11Z- handleDvl() - RDI_Pathfinder msg: 1613175731.93 epoch sec; seqNo:5939 2021-02-13T00:22:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 95.55 , 67.84 , 104.07 2021-02-13T00:22:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.11 , -0.15 2021-02-13T00:22:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175731.95 epoch sec; seqNo:12017; 4058993.54 north; 592434.90 east 2021-02-13T00:22:11Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-13T00:22:11Z- waiting for 1613175729.67, time is 1613175731.95 2021-02-13T00:22:11Z- LcmTrn::updateTrn() equating pose 1613175731.95 and meas times 1613175731.95 2021-02-13T00:22:11Z- LcmTrn::updateTrn() >>>> pose time 1613175731.95, meas time 1613175731.95 <<<< 2021-02-13T00:22:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175731.95 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:22:11Z- TerrainNav::motionUpdate() - currEstimate = (4058994, 592434.90, 21.49) 2021-02-13T00:22:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:22:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:22:11Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175731.95 sec.; No relevant navigation data available 2021-02-13T00:22:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:22:11Z- TNavBF::mmse selected from filter -1 2021-02-13T00:22:11Z- variances([0], [2], and psi_berg) = 706.185516 833.001301 0.000000 2021-02-13T00:22:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:22:11Z- LcmTrn::publishEstimates() - MLE : 4058993.04 592483.40 21.49 2021-02-13T00:22:11Z- LcmTrn::publishEstimates() - MMSE : 4058993.54 592434.90 21.49 2021-02-13T00:22:11Z- LcmTrn::publishEstimates() - COVAR: 706.19 833.00 1.19 0.00 2021-02-13T00:22:12Z- AHRS_M2 msg: 1613175732.32 epoch sec; seqNo:11921 2021-02-13T00:22:12Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.21 psi 2021-02-13T00:22:12Z- handleDepth()-Depth_Keller msg: 1613175732320 epoch sec; seqNo:12018; depth 21.62 2021-02-13T00:22:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175732.33 epoch sec; seqNo:12018; 4058993.92 north; 592434.90 east 2021-02-13T00:22:12Z- AHRS_M2 msg: 1613175732.71 epoch sec; seqNo:11922 2021-02-13T00:22:12Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.21 psi 2021-02-13T00:22:12Z- handleDepth()-Depth_Keller msg: 1613175732724 epoch sec; seqNo:12019; depth 21.74 2021-02-13T00:22:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175732.73 epoch sec; seqNo:12019; 4058994.29 north; 592434.90 east 2021-02-13T00:22:13Z- AHRS_M2 msg: 1613175733.12 epoch sec; seqNo:11923 2021-02-13T00:22:13Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.21 psi 2021-02-13T00:22:13Z- handleDepth()-Depth_Keller msg: 1613175733129 epoch sec; seqNo:12020; depth 21.87 2021-02-13T00:22:13Z- handleDvl() - RDI_Pathfinder msg: 1613175733.15 epoch sec; seqNo:5940 2021-02-13T00:22:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 66.62 , 105.35 2021-02-13T00:22:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.15 , -0.09 2021-02-13T00:22:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175733.16 epoch sec; seqNo:12020; 4058994.67 north; 592434.89 east 2021-02-13T00:22:13Z- AHRS_M2 msg: 1613175733.52 epoch sec; seqNo:11924 2021-02-13T00:22:13Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.21 psi 2021-02-13T00:22:13Z- handleDepth()-Depth_Keller msg: 1613175733529 epoch sec; seqNo:12021; depth 22.01 2021-02-13T00:22:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175733.54 epoch sec; seqNo:12021; 4058994.67 north; 592434.89 east 2021-02-13T00:22:13Z- AHRS_M2 msg: 1613175733.92 epoch sec; seqNo:11925 2021-02-13T00:22:13Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.21 psi 2021-02-13T00:22:13Z- handleDepth()-Depth_Keller msg: 1613175733930 epoch sec; seqNo:12022; depth 22.11 2021-02-13T00:22:13Z- handleDvl() - RDI_Pathfinder msg: 1613175733.94 epoch sec; seqNo:5941 2021-02-13T00:22:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 68.25 , 105.28 2021-02-13T00:22:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.14 , 0.00 2021-02-13T00:22:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175733.95 epoch sec; seqNo:12022; 4058995.05 north; 592434.89 east 2021-02-13T00:22:14Z- AHRS_M2 msg: 1613175734.33 epoch sec; seqNo:11926 2021-02-13T00:22:14Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.21 psi 2021-02-13T00:22:14Z- handleDepth()-Depth_Keller msg: 1613175734342 epoch sec; seqNo:12023; depth 22.25 2021-02-13T00:22:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175734.35 epoch sec; seqNo:12023; 4058995.43 north; 592434.88 east 2021-02-13T00:22:14Z- AHRS_M2 msg: 1613175734.73 epoch sec; seqNo:11927 2021-02-13T00:22:14Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.21 psi 2021-02-13T00:22:14Z- handleDepth()-Depth_Keller msg: 1613175734737 epoch sec; seqNo:12024; depth 22.37 2021-02-13T00:22:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175734.76 epoch sec; seqNo:12024; 4058995.82 north; 592436.10 east 2021-02-13T00:22:15Z- AHRS_M2 msg: 1613175735.14 epoch sec; seqNo:11928 2021-02-13T00:22:15Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.21 psi 2021-02-13T00:22:15Z- handleDepth()-Depth_Keller msg: 1613175735144 epoch sec; seqNo:12025; depth 22.50 2021-02-13T00:22:15Z- handleDvl() - RDI_Pathfinder msg: 1613175735.16 epoch sec; seqNo:5942 2021-02-13T00:22:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 93.11 , 78.51 , 105.28 2021-02-13T00:22:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.12 , 0.08 2021-02-13T00:22:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175735.18 epoch sec; seqNo:12025; 4058995.82 north; 592436.10 east 2021-02-13T00:22:15Z- AHRS_M2 msg: 1613175735.54 epoch sec; seqNo:11929 2021-02-13T00:22:15Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.21 psi 2021-02-13T00:22:15Z- handleDepth()-Depth_Keller msg: 1613175735549 epoch sec; seqNo:12026; depth 22.62 2021-02-13T00:22:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175735.56 epoch sec; seqNo:12026; 4058996.20 north; 592436.10 east 2021-02-13T00:22:15Z- AHRS_M2 msg: 1613175735.94 epoch sec; seqNo:11930 2021-02-13T00:22:15Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.21 psi 2021-02-13T00:22:15Z- handleDepth()-Depth_Keller msg: 1613175735950 epoch sec; seqNo:12027; depth 22.74 2021-02-13T00:22:15Z- handleDvl() - RDI_Pathfinder msg: 1613175735.96 epoch sec; seqNo:5943 2021-02-13T00:22:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.98 , 65.80 , 104.07 2021-02-13T00:22:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.11 2021-02-13T00:22:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175735.97 epoch sec; seqNo:12027; 4058996.58 north; 592436.09 east 2021-02-13T00:22:16Z- AHRS_M2 msg: 1613175736.35 epoch sec; seqNo:11931 2021-02-13T00:22:16Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.20 psi 2021-02-13T00:22:16Z- handleDepth()-Depth_Keller msg: 1613175736357 epoch sec; seqNo:12028; depth 22.87 2021-02-13T00:22:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175736.37 epoch sec; seqNo:12028; 4058996.58 north; 592436.09 east 2021-02-13T00:22:16Z- AHRS_M2 msg: 1613175736.75 epoch sec; seqNo:11932 2021-02-13T00:22:16Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.20 psi 2021-02-13T00:22:16Z- handleDepth()-Depth_Keller msg: 1613175736759 epoch sec; seqNo:12029; depth 23.01 2021-02-13T00:22:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175736.77 epoch sec; seqNo:12029; 4058996.96 north; 592436.09 east 2021-02-13T00:22:16Z- LcmTrn::handleMessages() - handling 40 messages took 0.60 ms and 128 calls... 2021-02-13T00:22:16Z- waiting for 1613175734.95, time is 1613175736.77 2021-02-13T00:22:16Z- LcmTrn::updateTrn() >>>> pose time 1613175736.77, meas time 1613175735.96 <<<< 2021-02-13T00:22:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175735.96 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:22:16Z- TerrainNav::motionUpdate() - currEstimate = (4058997, 592436.09, 23.01) 2021-02-13T00:22:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:22:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:22:16Z- TNavBF::Projecting Measurements 2021-02-13T00:22:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:22:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:22:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:22:16Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:22:16Z- TNavFilter::3 good beams. 2021-02-13T00:22:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:22:16Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:22:16Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:22:16Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:22:16Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:22:16Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:22:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:22:16Z- TNavBF::mmse selected from filter -1 2021-02-13T00:22:16Z- TerrainMapOctree: (vehN, vehE ) = (4058996.39, 592435.89). 2021-02-13T00:22:16Z- TerrainMapOctree: Min Distance = 2893.05. 2021-02-13T00:22:16Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:22:16Z- There is no specific good beam that all particles have in common. 2021-02-13T00:22:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:22:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.35 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:22:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:22:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.126 ExpectedMeasDiff: nan currMeas.covariance: 6.93 2021-02-13T00:22:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:22:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 17.33 2021-02-13T00:22:17Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:22:17Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:22:17Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:22:17Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:22:17Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:22:17Z- Filter 0 No beams used in subcloud update 2021-02-13T00:22:17Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:22:17Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:22:17Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 706.1 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:22:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175735.96 sec. 2021-02-13T00:22:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:22:17Z- TNavBF::mmse selected from filter -1 2021-02-13T00:22:17Z- variances([0], [2], and psi_berg) = 706.112781 833.001260 0.000000 2021-02-13T00:22:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:22:17Z- LcmTrn::publishEstimates() - MLE : 4058996.46 592484.59 23.01 2021-02-13T00:22:17Z- LcmTrn::publishEstimates() - MMSE : 4058996.96 592436.09 23.01 2021-02-13T00:22:17Z- LcmTrn::publishEstimates() - COVAR: 706.11 833.00 1.06 0.00 2021-02-13T00:22:17Z- AHRS_M2 msg: 1613175737.16 epoch sec; seqNo:11933 2021-02-13T00:22:17Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.20 psi 2021-02-13T00:22:17Z- handleDepth()-Depth_Keller msg: 1613175737161 epoch sec; seqNo:12030; depth 23.12 2021-02-13T00:22:17Z- handleDvl() - RDI_Pathfinder msg: 1613175737.18 epoch sec; seqNo:5944 2021-02-13T00:22:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.37 , 97.85 , 66.21 , 103.93 2021-02-13T00:22:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , 0.09 2021-02-13T00:22:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175737.19 epoch sec; seqNo:12030; 4058997.34 north; 592436.08 east 2021-02-13T00:22:17Z- AHRS_M2 msg: 1613175737.56 epoch sec; seqNo:11934 2021-02-13T00:22:17Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-13T00:22:17Z- handleDepth()-Depth_Keller msg: 1613175737564 epoch sec; seqNo:12031; depth 23.25 2021-02-13T00:22:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175737.58 epoch sec; seqNo:12031; 4058997.35 north; 592437.30 east 2021-02-13T00:22:17Z- AHRS_M2 msg: 1613175737.96 epoch sec; seqNo:11935 2021-02-13T00:22:17Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-13T00:22:17Z- handleDepth()-Depth_Keller msg: 1613175737973 epoch sec; seqNo:12032; depth 23.34 2021-02-13T00:22:17Z- handleDvl() - RDI_Pathfinder msg: 1613175737.98 epoch sec; seqNo:5945 2021-02-13T00:22:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 98.05 , 67.43 , 101.70 2021-02-13T00:22:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.08 , 0.05 2021-02-13T00:22:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175737.99 epoch sec; seqNo:12032; 4058997.73 north; 592437.30 east 2021-02-13T00:22:18Z- AHRS_M2 msg: 1613175738.37 epoch sec; seqNo:11936 2021-02-13T00:22:18Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-13T00:22:18Z- handleDepth()-Depth_Keller msg: 1613175738379 epoch sec; seqNo:12033; depth 23.50 2021-02-13T00:22:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175738.39 epoch sec; seqNo:12033; 4058998.11 north; 592437.30 east 2021-02-13T00:22:18Z- AHRS_M2 msg: 1613175738.77 epoch sec; seqNo:11937 2021-02-13T00:22:18Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-13T00:22:18Z- handleDepth()-Depth_Keller msg: 1613175738776 epoch sec; seqNo:12034; depth 23.61 2021-02-13T00:22:18Z- handleDvl() - RDI_Pathfinder msg: 1613175738.79 epoch sec; seqNo:5946 2021-02-13T00:22:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.35 , 0.00 , 66.21 , 100.62 2021-02-13T00:22:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , 0.03 2021-02-13T00:22:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175738.81 epoch sec; seqNo:12034; 4058998.11 north; 592437.30 east 2021-02-13T00:22:19Z- AHRS_M2 msg: 1613175739.17 epoch sec; seqNo:11938 2021-02-13T00:22:19Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.20 psi 2021-02-13T00:22:19Z- handleDepth()-Depth_Keller msg: 1613175739189 epoch sec; seqNo:12035; depth 23.73 2021-02-13T00:22:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175739.20 epoch sec; seqNo:12035; 4058998.49 north; 592437.29 east 2021-02-13T00:22:19Z- AHRS_M2 msg: 1613175739.58 epoch sec; seqNo:11939 2021-02-13T00:22:19Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.20 psi 2021-02-13T00:22:19Z- handleDepth()-Depth_Keller msg: 1613175739586 epoch sec; seqNo:12036; depth 23.86 2021-02-13T00:22:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175739.60 epoch sec; seqNo:12036; 4058998.87 north; 592437.29 east 2021-02-13T00:22:19Z- AHRS_M2 msg: 1613175739.98 epoch sec; seqNo:11940 2021-02-13T00:22:19Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.20 psi 2021-02-13T00:22:19Z- handleDepth()-Depth_Keller msg: 1613175739993 epoch sec; seqNo:12037; depth 23.97 2021-02-13T00:22:20Z- handleDvl() - RDI_Pathfinder msg: 1613175740.01 epoch sec; seqNo:5947 2021-02-13T00:22:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 0.00 , 66.21 , 101.23 2021-02-13T00:22:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.01 2021-02-13T00:22:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175740.02 epoch sec; seqNo:12037; 4058998.88 north; 592438.51 east 2021-02-13T00:22:20Z- AHRS_M2 msg: 1613175740.40 epoch sec; seqNo:11941 2021-02-13T00:22:20Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-13T00:22:20Z- handleDepth()-Depth_Keller msg: 1613175740401 epoch sec; seqNo:12038; depth 24.09 2021-02-13T00:22:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175740.41 epoch sec; seqNo:12038; 4058999.26 north; 592438.50 east 2021-02-13T00:22:20Z- AHRS_M2 msg: 1613175740.79 epoch sec; seqNo:11942 2021-02-13T00:22:20Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.21 psi 2021-02-13T00:22:20Z- handleDepth()-Depth_Keller msg: 1613175740805 epoch sec; seqNo:12039; depth 24.22 2021-02-13T00:22:20Z- handleDvl() - RDI_Pathfinder msg: 1613175740.82 epoch sec; seqNo:5948 2021-02-13T00:22:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 97.78 , 65.81 , 102.11 2021-02-13T00:22:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.02 2021-02-13T00:22:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175740.83 epoch sec; seqNo:12039; 4058999.64 north; 592438.50 east 2021-02-13T00:22:21Z- AHRS_M2 msg: 1613175741.20 epoch sec; seqNo:11943 2021-02-13T00:22:21Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.21 psi 2021-02-13T00:22:21Z- handleDepth()-Depth_Keller msg: 1613175741202 epoch sec; seqNo:12040; depth 24.35 2021-02-13T00:22:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175741.21 epoch sec; seqNo:12040; 4058999.64 north; 592438.50 east 2021-02-13T00:22:21Z- AHRS_M2 msg: 1613175741.60 epoch sec; seqNo:11944 2021-02-13T00:22:21Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.21 psi 2021-02-13T00:22:21Z- handleDepth()-Depth_Keller msg: 1613175741604 epoch sec; seqNo:12041; depth 24.45 2021-02-13T00:22:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175741.62 epoch sec; seqNo:12041; 4059000.02 north; 592438.50 east 2021-02-13T00:22:22Z- AHRS_M2 msg: 1613175742.00 epoch sec; seqNo:11945 2021-02-13T00:22:22Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.21 psi 2021-02-13T00:22:22Z- handleDepth()-Depth_Keller msg: 1613175742008 epoch sec; seqNo:12042; depth 24.59 2021-02-13T00:22:22Z- handleDvl() - RDI_Pathfinder msg: 1613175742.02 epoch sec; seqNo:5949 2021-02-13T00:22:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.71 , 98.26 , 66.21 , 100.69 2021-02-13T00:22:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , -0.02 2021-02-13T00:22:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175742.05 epoch sec; seqNo:12042; 4059000.40 north; 592438.49 east 2021-02-13T00:22:22Z- LcmTrn::handleMessages() - handling 45 messages took 0.72 ms and 131 calls... 2021-02-13T00:22:22Z- waiting for 1613175739.77, time is 1613175742.05 2021-02-13T00:22:22Z- LcmTrn::updateTrn() equating pose 1613175742.05 and meas times 1613175742.05 2021-02-13T00:22:22Z- LcmTrn::updateTrn() >>>> pose time 1613175742.05, meas time 1613175742.05 <<<< 2021-02-13T00:22:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175742.05 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:22:22Z- TerrainNav::motionUpdate() - currEstimate = (4059000, 592438.49, 24.59) 2021-02-13T00:22:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:22:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:22:22Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175742.05 sec.; No relevant navigation data available 2021-02-13T00:22:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:22:22Z- TNavBF::mmse selected from filter -1 2021-02-13T00:22:22Z- variances([0], [2], and psi_berg) = 706.053586 833.001227 0.000000 2021-02-13T00:22:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:22:22Z- LcmTrn::publishEstimates() - MLE : 4058999.90 592487.99 24.59 2021-02-13T00:22:22Z- LcmTrn::publishEstimates() - MMSE : 4059000.40 592438.49 24.59 2021-02-13T00:22:22Z- LcmTrn::publishEstimates() - COVAR: 706.05 833.00 1.28 0.00 2021-02-13T00:22:22Z- AHRS_M2 msg: 1613175742.41 epoch sec; seqNo:11946 2021-02-13T00:22:22Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-13T00:22:22Z- handleDepth()-Depth_Keller msg: 1613175742415 epoch sec; seqNo:12043; depth 24.69 2021-02-13T00:22:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175742.43 epoch sec; seqNo:12043; 4059000.79 north; 592439.71 east 2021-02-13T00:22:22Z- AHRS_M2 msg: 1613175742.81 epoch sec; seqNo:11947 2021-02-13T00:22:22Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-13T00:22:22Z- handleDepth()-Depth_Keller msg: 1613175742819 epoch sec; seqNo:12044; depth 24.81 2021-02-13T00:22:22Z- handleDvl() - RDI_Pathfinder msg: 1613175742.83 epoch sec; seqNo:5950 2021-02-13T00:22:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 89.73 , 65.39 , 99.47 2021-02-13T00:22:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , -0.07 2021-02-13T00:22:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175742.84 epoch sec; seqNo:12044; 4059000.79 north; 592439.71 east 2021-02-13T00:22:23Z- AHRS_M2 msg: 1613175743.22 epoch sec; seqNo:11948 2021-02-13T00:22:23Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-13T00:22:23Z- handleDepth()-Depth_Keller msg: 1613175743220 epoch sec; seqNo:12045; depth 24.93 2021-02-13T00:22:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175743.24 epoch sec; seqNo:12045; 4059001.17 north; 592439.70 east 2021-02-13T00:22:23Z- AHRS_M2 msg: 1613175743.62 epoch sec; seqNo:11949 2021-02-13T00:22:23Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.20 psi 2021-02-13T00:22:23Z- handleDepth()-Depth_Keller msg: 1613175743628 epoch sec; seqNo:12046; depth 25.05 2021-02-13T00:22:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175743.66 epoch sec; seqNo:12046; 4059001.55 north; 592439.70 east 2021-02-13T00:22:24Z- AHRS_M2 msg: 1613175744.02 epoch sec; seqNo:11950 2021-02-13T00:22:24Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.20 psi 2021-02-13T00:22:24Z- handleDepth()-Depth_Keller msg: 1613175744028 epoch sec; seqNo:12047; depth 25.17 2021-02-13T00:22:24Z- handleDvl() - RDI_Pathfinder msg: 1613175744.04 epoch sec; seqNo:5951 2021-02-13T00:22:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 0.00 , 64.57 , 98.18 2021-02-13T00:22:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.08 , -0.08 2021-02-13T00:22:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175744.05 epoch sec; seqNo:12047; 4059001.92 north; 592439.70 east 2021-02-13T00:22:24Z- AHRS_M2 msg: 1613175744.43 epoch sec; seqNo:11951 2021-02-13T00:22:24Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.20 psi 2021-02-13T00:22:24Z- handleDepth()-Depth_Keller msg: 1613175744442 epoch sec; seqNo:12048; depth 25.29 2021-02-13T00:22:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175744.45 epoch sec; seqNo:12048; 4059002.30 north; 592439.69 east 2021-02-13T00:22:24Z- AHRS_M2 msg: 1613175744.83 epoch sec; seqNo:11952 2021-02-13T00:22:24Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.19 psi 2021-02-13T00:22:24Z- handleDepth()-Depth_Keller msg: 1613175744836 epoch sec; seqNo:12049; depth 25.41 2021-02-13T00:22:24Z- handleDvl() - RDI_Pathfinder msg: 1613175744.85 epoch sec; seqNo:5952 2021-02-13T00:22:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 89.33 , 64.98 , 97.98 2021-02-13T00:22:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.12 , -0.08 2021-02-13T00:22:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175744.86 epoch sec; seqNo:12049; 4059002.69 north; 592440.91 east 2021-02-13T00:22:25Z- AHRS_M2 msg: 1613175745.23 epoch sec; seqNo:11953 2021-02-13T00:22:25Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.19 psi 2021-02-13T00:22:25Z- handleDepth()-Depth_Keller msg: 1613175745245 epoch sec; seqNo:12050; depth 25.55 2021-02-13T00:22:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175745.26 epoch sec; seqNo:12050; 4059002.69 north; 592440.91 east 2021-02-13T00:22:25Z- AHRS_M2 msg: 1613175745.64 epoch sec; seqNo:11954 2021-02-13T00:22:25Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-13T00:22:25Z- handleDepth()-Depth_Keller msg: 1613175745644 epoch sec; seqNo:12051; depth 25.66 2021-02-13T00:22:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175745.65 epoch sec; seqNo:12051; 4059003.07 north; 592440.90 east 2021-02-13T00:22:26Z- AHRS_M2 msg: 1613175746.04 epoch sec; seqNo:11955 2021-02-13T00:22:26Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-13T00:22:26Z- handleDepth()-Depth_Keller msg: 1613175746050 epoch sec; seqNo:12052; depth 25.77 2021-02-13T00:22:26Z- handleDvl() - RDI_Pathfinder msg: 1613175746.06 epoch sec; seqNo:5953 2021-02-13T00:22:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 86.08 , 64.57 , 99.47 2021-02-13T00:22:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.14 , -0.05 2021-02-13T00:22:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175746.07 epoch sec; seqNo:12052; 4059003.45 north; 592440.90 east 2021-02-13T00:22:26Z- AHRS_M2 msg: 1613175746.45 epoch sec; seqNo:11956 2021-02-13T00:22:26Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-13T00:22:26Z- handleDepth()-Depth_Keller msg: 1613175746483 epoch sec; seqNo:12053; depth 25.89 2021-02-13T00:22:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175746.50 epoch sec; seqNo:12053; 4059003.83 north; 592440.90 east 2021-02-13T00:22:26Z- AHRS_M2 msg: 1613175746.85 epoch sec; seqNo:11957 2021-02-13T00:22:26Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.17 psi 2021-02-13T00:22:26Z- handleDepth()-Depth_Keller msg: 1613175746861 epoch sec; seqNo:12054; depth 26.03 2021-02-13T00:22:26Z- handleDvl() - RDI_Pathfinder msg: 1613175746.87 epoch sec; seqNo:5954 2021-02-13T00:22:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 87.17 , 64.58 , 97.99 2021-02-13T00:22:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.13 , 0.00 2021-02-13T00:22:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175746.89 epoch sec; seqNo:12054; 4059004.21 north; 592440.89 east 2021-02-13T00:22:27Z- LcmTrn::handleMessages() - handling 41 messages took 0.72 ms and 127 calls... 2021-02-13T00:22:27Z- waiting for 1613175745.05, time is 1613175746.89 2021-02-13T00:22:27Z- LcmTrn::updateTrn() equating pose 1613175746.89 and meas times 1613175746.89 2021-02-13T00:22:27Z- LcmTrn::updateTrn() >>>> pose time 1613175746.89, meas time 1613175746.89 <<<< 2021-02-13T00:22:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175746.89 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:22:27Z- TerrainNav::motionUpdate() - currEstimate = (4059004, 592440.89, 26.03) 2021-02-13T00:22:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:22:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:22:27Z- TNavBF::Projecting Measurements 2021-02-13T00:22:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:22:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:22:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:22:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:22:27Z- TNavFilter::4 good beams. 2021-02-13T00:22:27Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:22:27Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:22:27Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:22:27Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:22:27Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:22:27Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:22:27Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:22:27Z- TNavBF::mmse selected from filter -1 2021-02-13T00:22:27Z- TerrainMapOctree: (vehN, vehE ) = (4059004.21, 592440.89). 2021-02-13T00:22:27Z- TerrainMapOctree: Min Distance = 2883.82. 2021-02-13T00:22:27Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:22:27Z- There is no specific good beam that all particles have in common. 2021-02-13T00:22:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:22:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.51 K: 0.136 ExpectedMeasDiff: nan currMeas.covariance: 7.82 2021-02-13T00:22:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:22:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 12.16 2021-02-13T00:22:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:22:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.124 ExpectedMeasDiff: nan currMeas.covariance: 6.67 2021-02-13T00:22:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:22:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:22:27Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:22:27Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:22:27Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:22:27Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:22:27Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:22:27Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:22:27Z- Filter 0 No beams used in subcloud update 2021-02-13T00:22:27Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:22:27Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:22:27Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 706.0 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:22:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175746.89 sec. 2021-02-13T00:22:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:22:27Z- TNavBF::mmse selected from filter -1 2021-02-13T00:22:27Z- variances([0], [2], and psi_berg) = 705.992258 833.001193 0.000000 2021-02-13T00:22:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:22:27Z- LcmTrn::publishEstimates() - MLE : 4059003.71 592490.39 26.03 2021-02-13T00:22:27Z- LcmTrn::publishEstimates() - MMSE : 4059004.21 592440.89 26.03 2021-02-13T00:22:27Z- LcmTrn::publishEstimates() - COVAR: 705.99 833.00 0.98 0.00 2021-02-13T00:22:27Z- AHRS_M2 msg: 1613175747.26 epoch sec; seqNo:11958 2021-02-13T00:22:27Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.17 psi 2021-02-13T00:22:27Z- handleDepth()-Depth_Keller msg: 1613175747260 epoch sec; seqNo:12055; depth 26.14 2021-02-13T00:22:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175747.27 epoch sec; seqNo:12055; 4059004.21 north; 592440.89 east 2021-02-13T00:22:27Z- AHRS_M2 msg: 1613175747.66 epoch sec; seqNo:11959 2021-02-13T00:22:27Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.17 psi 2021-02-13T00:22:27Z- handleDepth()-Depth_Keller msg: 1613175747665 epoch sec; seqNo:12056; depth 26.24 2021-02-13T00:22:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175747.68 epoch sec; seqNo:12056; 4059004.60 north; 592442.11 east 2021-02-13T00:22:28Z- AHRS_M2 msg: 1613175748.06 epoch sec; seqNo:11960 2021-02-13T00:22:28Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.17 psi 2021-02-13T00:22:28Z- handleDepth()-Depth_Keller msg: 1613175748068 epoch sec; seqNo:12057; depth 26.37 2021-02-13T00:22:28Z- handleDvl() - RDI_Pathfinder msg: 1613175748.08 epoch sec; seqNo:5955 2021-02-13T00:22:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 162.90 , 64.17 , 97.99 2021-02-13T00:22:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.15 , 0.07 2021-02-13T00:22:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175748.10 epoch sec; seqNo:12057; 4059004.98 north; 592442.11 east 2021-02-13T00:22:28Z- AHRS_M2 msg: 1613175748.48 epoch sec; seqNo:11961 2021-02-13T00:22:28Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.17 psi 2021-02-13T00:22:28Z- handleDepth()-Depth_Keller msg: 1613175748482 epoch sec; seqNo:12058; depth 26.49 2021-02-13T00:22:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175748.49 epoch sec; seqNo:12058; 4059005.36 north; 592442.10 east 2021-02-13T00:22:28Z- AHRS_M2 msg: 1613175748.87 epoch sec; seqNo:11962 2021-02-13T00:22:28Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.17 psi 2021-02-13T00:22:28Z- handleDepth()-Depth_Keller msg: 1613175748885 epoch sec; seqNo:12059; depth 26.60 2021-02-13T00:22:28Z- handleDvl() - RDI_Pathfinder msg: 1613175748.90 epoch sec; seqNo:5956 2021-02-13T00:22:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 85.00 , 62.53 , 97.17 2021-02-13T00:22:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.00 , 0.10 2021-02-13T00:22:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175748.91 epoch sec; seqNo:12059; 4059005.36 north; 592442.10 east 2021-02-13T00:22:29Z- AHRS_M2 msg: 1613175749.28 epoch sec; seqNo:11963 2021-02-13T00:22:29Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.17 psi 2021-02-13T00:22:29Z- handleDepth()-Depth_Keller msg: 1613175749280 epoch sec; seqNo:12060; depth 26.72 2021-02-13T00:22:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175749.30 epoch sec; seqNo:12060; 4059005.74 north; 592442.10 east 2021-02-13T00:22:29Z- AHRS_M2 msg: 1613175749.68 epoch sec; seqNo:11964 2021-02-13T00:22:29Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.17 psi 2021-02-13T00:22:29Z- handleDepth()-Depth_Keller msg: 1613175749684 epoch sec; seqNo:12061; depth 26.83 2021-02-13T00:22:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175749.69 epoch sec; seqNo:12061; 4059005.74 north; 592442.10 east 2021-02-13T00:22:30Z- AHRS_M2 msg: 1613175750.08 epoch sec; seqNo:11965 2021-02-13T00:22:30Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.17 psi 2021-02-13T00:22:30Z- handleDepth()-Depth_Keller msg: 1613175750089 epoch sec; seqNo:12062; depth 26.96 2021-02-13T00:22:30Z- handleDvl() - RDI_Pathfinder msg: 1613175750.10 epoch sec; seqNo:5957 2021-02-13T00:22:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 84.86 , 62.94 , 97.85 2021-02-13T00:22:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , 0.08 2021-02-13T00:22:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175750.11 epoch sec; seqNo:12062; 4059006.12 north; 592442.09 east 2021-02-13T00:22:30Z- AHRS_M2 msg: 1613175750.49 epoch sec; seqNo:11966 2021-02-13T00:22:30Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.17 psi 2021-02-13T00:22:30Z- handleDepth()-Depth_Keller msg: 1613175750493 epoch sec; seqNo:12063; depth 27.09 2021-02-13T00:22:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175750.51 epoch sec; seqNo:12063; 4059006.51 north; 592443.31 east 2021-02-13T00:22:30Z- AHRS_M2 msg: 1613175750.89 epoch sec; seqNo:11967 2021-02-13T00:22:30Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-13T00:22:30Z- handleDepth()-Depth_Keller msg: 1613175750896 epoch sec; seqNo:12064; depth 27.19 2021-02-13T00:22:30Z- handleDvl() - RDI_Pathfinder msg: 1613175750.91 epoch sec; seqNo:5958 2021-02-13T00:22:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 85.88 , 63.35 , 97.78 2021-02-13T00:22:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.06 , 0.04 2021-02-13T00:22:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175750.93 epoch sec; seqNo:12064; 4059006.51 north; 592443.31 east 2021-02-13T00:22:31Z- AHRS_M2 msg: 1613175751.30 epoch sec; seqNo:11968 2021-02-13T00:22:31Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-13T00:22:31Z- handleDepth()-Depth_Keller msg: 1613175751305 epoch sec; seqNo:12065; depth 27.31 2021-02-13T00:22:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175751.32 epoch sec; seqNo:12065; 4059006.89 north; 592443.31 east 2021-02-13T00:22:31Z- AHRS_M2 msg: 1613175751.70 epoch sec; seqNo:11969 2021-02-13T00:22:31Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.18 psi 2021-02-13T00:22:31Z- handleDepth()-Depth_Keller msg: 1613175751713 epoch sec; seqNo:12066; depth 27.43 2021-02-13T00:22:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175751.73 epoch sec; seqNo:12066; 4059007.27 north; 592443.30 east 2021-02-13T00:22:32Z- AHRS_M2 msg: 1613175752.10 epoch sec; seqNo:11970 2021-02-13T00:22:32Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.18 psi 2021-02-13T00:22:32Z- handleDepth()-Depth_Keller msg: 1613175752108 epoch sec; seqNo:12067; depth 27.57 2021-02-13T00:22:32Z- handleDvl() - RDI_Pathfinder msg: 1613175752.13 epoch sec; seqNo:5959 2021-02-13T00:22:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.25 , 86.08 , 62.12 , 97.98 2021-02-13T00:22:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , -0.00 2021-02-13T00:22:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175752.14 epoch sec; seqNo:12067; 4059007.27 north; 592443.30 east 2021-02-13T00:22:32Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-13T00:22:32Z- waiting for 1613175749.89, time is 1613175752.14 2021-02-13T00:22:32Z- LcmTrn::updateTrn() equating pose 1613175752.14 and meas times 1613175752.14 2021-02-13T00:22:32Z- LcmTrn::updateTrn() >>>> pose time 1613175752.14, meas time 1613175752.14 <<<< 2021-02-13T00:22:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175752.14 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:22:32Z- TerrainNav::motionUpdate() - currEstimate = (4059007, 592443.30, 27.57) 2021-02-13T00:22:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:22:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:22:32Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175752.14 sec.; No relevant navigation data available 2021-02-13T00:22:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:22:32Z- TNavBF::mmse selected from filter -1 2021-02-13T00:22:32Z- variances([0], [2], and psi_berg) = 705.935307 833.001161 0.000000 2021-02-13T00:22:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:22:32Z- LcmTrn::publishEstimates() - MLE : 4059006.77 592492.80 27.57 2021-02-13T00:22:32Z- LcmTrn::publishEstimates() - MMSE : 4059007.27 592443.30 27.57 2021-02-13T00:22:32Z- LcmTrn::publishEstimates() - COVAR: 705.94 833.00 1.18 0.00 2021-02-13T00:22:32Z- AHRS_M2 msg: 1613175752.51 epoch sec; seqNo:11971 2021-02-13T00:22:32Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.18 psi 2021-02-13T00:22:32Z- handleDepth()-Depth_Keller msg: 1613175752512 epoch sec; seqNo:12068; depth 27.68 2021-02-13T00:22:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175752.52 epoch sec; seqNo:12068; 4059007.66 north; 592444.52 east 2021-02-13T00:22:32Z- AHRS_M2 msg: 1613175752.91 epoch sec; seqNo:11972 2021-02-13T00:22:32Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.18 psi 2021-02-13T00:22:32Z- handleDepth()-Depth_Keller msg: 1613175752921 epoch sec; seqNo:12069; depth 27.81 2021-02-13T00:22:32Z- handleDvl() - RDI_Pathfinder msg: 1613175752.93 epoch sec; seqNo:5960 2021-02-13T00:22:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.26 , 151.00 , 61.31 , 97.99 2021-02-13T00:22:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.27 , -0.10 2021-02-13T00:22:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175752.94 epoch sec; seqNo:12069; 4059008.04 north; 592444.51 east 2021-02-13T00:22:33Z- AHRS_M2 msg: 1613175753.32 epoch sec; seqNo:11973 2021-02-13T00:22:33Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.18 psi 2021-02-13T00:22:33Z- handleDepth()-Depth_Keller msg: 1613175753328 epoch sec; seqNo:12070; depth 27.95 2021-02-13T00:22:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175753.34 epoch sec; seqNo:12070; 4059008.04 north; 592444.51 east 2021-02-13T00:22:33Z- AHRS_M2 msg: 1613175753.72 epoch sec; seqNo:11974 2021-02-13T00:22:33Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-13T00:22:33Z- handleDepth()-Depth_Keller msg: 1613175753730 epoch sec; seqNo:12071; depth 28.05 2021-02-13T00:22:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175753.77 epoch sec; seqNo:12071; 4059008.42 north; 592444.51 east 2021-02-13T00:22:34Z- AHRS_M2 msg: 1613175754.13 epoch sec; seqNo:11975 2021-02-13T00:22:34Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-13T00:22:34Z- handleDepth()-Depth_Keller msg: 1613175754132 epoch sec; seqNo:12072; depth 28.18 2021-02-13T00:22:34Z- handleDvl() - RDI_Pathfinder msg: 1613175754.15 epoch sec; seqNo:5961 2021-02-13T00:22:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.85 , 85.34 , 61.31 , 97.51 2021-02-13T00:22:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , -0.03 2021-02-13T00:22:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175754.16 epoch sec; seqNo:12072; 4059008.80 north; 592444.51 east 2021-02-13T00:22:34Z- AHRS_M2 msg: 1613175754.53 epoch sec; seqNo:11976 2021-02-13T00:22:34Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-13T00:22:34Z- handleDepth()-Depth_Keller msg: 1613175754545 epoch sec; seqNo:12073; depth 28.32 2021-02-13T00:22:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175754.56 epoch sec; seqNo:12073; 4059008.81 north; 592445.73 east 2021-02-13T00:22:34Z- AHRS_M2 msg: 1613175754.93 epoch sec; seqNo:11977 2021-02-13T00:22:34Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-13T00:22:34Z- handleDepth()-Depth_Keller msg: 1613175754936 epoch sec; seqNo:12074; depth 28.43 2021-02-13T00:22:34Z- handleDvl() - RDI_Pathfinder msg: 1613175754.95 epoch sec; seqNo:5962 2021-02-13T00:22:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.85 , 86.08 , 62.13 , 97.99 2021-02-13T00:22:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , -0.02 2021-02-13T00:22:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175754.96 epoch sec; seqNo:12074; 4059009.19 north; 592445.72 east 2021-02-13T00:22:35Z- AHRS_M2 msg: 1613175755.34 epoch sec; seqNo:11978 2021-02-13T00:22:35Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-13T00:22:35Z- handleDepth()-Depth_Keller msg: 1613175755345 epoch sec; seqNo:12075; depth 28.54 2021-02-13T00:22:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175755.36 epoch sec; seqNo:12075; 4059009.57 north; 592445.72 east 2021-02-13T00:22:35Z- AHRS_M2 msg: 1613175755.76 epoch sec; seqNo:11979 2021-02-13T00:22:35Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-13T00:22:35Z- handleDepth()-Depth_Keller msg: 1613175755765 epoch sec; seqNo:12076; depth 28.67 2021-02-13T00:22:35Z- handleDvl() - RDI_Pathfinder msg: 1613175755.79 epoch sec; seqNo:5963 2021-02-13T00:22:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.62 , 85.00 , 60.49 , 97.99 2021-02-13T00:22:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , -0.03 2021-02-13T00:22:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175755.80 epoch sec; seqNo:12076; 4059009.95 north; 592445.71 east 2021-02-13T00:22:36Z- AHRS_M2 msg: 1613175756.15 epoch sec; seqNo:11980 2021-02-13T00:22:36Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-13T00:22:36Z- handleDepth()-Depth_Keller msg: 1613175756154 epoch sec; seqNo:12077; depth 28.79 2021-02-13T00:22:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175756.16 epoch sec; seqNo:12077; 4059009.95 north; 592445.71 east 2021-02-13T00:22:36Z- AHRS_M2 msg: 1613175756.55 epoch sec; seqNo:11981 2021-02-13T00:22:36Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-13T00:22:36Z- handleDepth()-Depth_Keller msg: 1613175756552 epoch sec; seqNo:12078; depth 28.91 2021-02-13T00:22:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175756.56 epoch sec; seqNo:12078; 4059010.34 north; 592446.93 east 2021-02-13T00:22:36Z- AHRS_M2 msg: 1613175756.95 epoch sec; seqNo:11982 2021-02-13T00:22:36Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-13T00:22:36Z- handleDepth()-Depth_Keller msg: 1613175756956 epoch sec; seqNo:12079; depth 29.03 2021-02-13T00:22:36Z- handleDvl() - RDI_Pathfinder msg: 1613175756.97 epoch sec; seqNo:5964 2021-02-13T00:22:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.62 , 167.23 , 60.49 , 97.99 2021-02-13T00:22:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , -0.04 2021-02-13T00:22:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175756.98 epoch sec; seqNo:12079; 4059010.72 north; 592446.93 east 2021-02-13T00:22:37Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-13T00:22:37Z- waiting for 1613175755.14, time is 1613175756.98 2021-02-13T00:22:37Z- LcmTrn::updateTrn() equating pose 1613175756.98 and meas times 1613175756.98 2021-02-13T00:22:37Z- LcmTrn::updateTrn() >>>> pose time 1613175756.98, meas time 1613175756.98 <<<< 2021-02-13T00:22:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175756.98 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:22:37Z- TerrainNav::motionUpdate() - currEstimate = (4059011, 592446.93, 29.03) 2021-02-13T00:22:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:22:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:22:37Z- TNavBF::Projecting Measurements 2021-02-13T00:22:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:22:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:22:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:22:37Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:22:37Z- TNavFilter::3 good beams. 2021-02-13T00:22:37Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:22:37Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:22:37Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:22:37Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:22:37Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:22:37Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:22:37Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:22:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:22:37Z- TerrainMapOctree: (vehN, vehE ) = (4059010.72, 592446.93). 2021-02-13T00:22:37Z- TerrainMapOctree: Min Distance = 2874.97. 2021-02-13T00:22:37Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:22:37Z- There is no specific good beam that all particles have in common. 2021-02-13T00:22:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:22:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.44 K: 0.140 ExpectedMeasDiff: nan currMeas.covariance: 7.10 2021-02-13T00:22:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:22:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.140 ExpectedMeasDiff: nan currMeas.covariance: 5.85 2021-02-13T00:22:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:22:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:22:37Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:22:37Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:22:37Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:22:37Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:22:37Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:22:37Z- Filter 0 No beams used in subcloud update 2021-02-13T00:22:37Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:22:37Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:22:37Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 705.9 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:22:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175756.98 sec. 2021-02-13T00:22:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:22:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:22:37Z- variances([0], [2], and psi_berg) = 705.870770 833.001125 0.000000 2021-02-13T00:22:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:22:37Z- LcmTrn::publishEstimates() - MLE : 4059010.22 592496.43 29.03 2021-02-13T00:22:37Z- LcmTrn::publishEstimates() - MMSE : 4059010.72 592446.93 29.03 2021-02-13T00:22:37Z- LcmTrn::publishEstimates() - COVAR: 705.87 833.00 1.00 0.00 2021-02-13T00:22:37Z- AHRS_M2 msg: 1613175757.36 epoch sec; seqNo:11983 2021-02-13T00:22:37Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.19 psi 2021-02-13T00:22:37Z- handleDepth()-Depth_Keller msg: 1613175757367 epoch sec; seqNo:12080; depth 29.16 2021-02-13T00:22:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175757.38 epoch sec; seqNo:12080; 4059010.72 north; 592446.93 east 2021-02-13T00:22:37Z- AHRS_M2 msg: 1613175757.76 epoch sec; seqNo:11984 2021-02-13T00:22:37Z- AHRS_M2 msg: 0.66 phi; -0.30 theta; -0.19 psi 2021-02-13T00:22:37Z- handleDepth()-Depth_Keller msg: 1613175757768 epoch sec; seqNo:12081; depth 29.29 2021-02-13T00:22:37Z- handleDvl() - RDI_Pathfinder msg: 1613175757.79 epoch sec; seqNo:5965 2021-02-13T00:22:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:22:37Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:22:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175757.80 epoch sec; seqNo:12081; 4059011.10 north; 592446.92 east 2021-02-13T00:22:38Z- AHRS_M2 msg: 1613175758.16 epoch sec; seqNo:11985 2021-02-13T00:22:38Z- AHRS_M2 msg: 0.65 phi; -0.27 theta; -0.19 psi 2021-02-13T00:22:38Z- handleDepth()-Depth_Keller msg: 1613175758168 epoch sec; seqNo:12082; depth 29.42 2021-02-13T00:22:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175758.18 epoch sec; seqNo:12082; 4059011.47 north; 592446.92 east 2021-02-13T00:22:38Z- AHRS_M2 msg: 1613175758.57 epoch sec; seqNo:11986 2021-02-13T00:22:38Z- AHRS_M2 msg: 0.65 phi; -0.24 theta; -0.18 psi 2021-02-13T00:22:38Z- handleDepth()-Depth_Keller msg: 1613175758572 epoch sec; seqNo:12083; depth 29.56 2021-02-13T00:22:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175758.58 epoch sec; seqNo:12083; 4059011.85 north; 592446.91 east 2021-02-13T00:22:38Z- AHRS_M2 msg: 1613175758.98 epoch sec; seqNo:11987 2021-02-13T00:22:38Z- AHRS_M2 msg: 0.64 phi; -0.20 theta; -0.17 psi 2021-02-13T00:22:38Z- handleDepth()-Depth_Keller msg: 1613175758983 epoch sec; seqNo:12084; depth 29.69 2021-02-13T00:22:38Z- handleDvl() - RDI_Pathfinder msg: 1613175758.99 epoch sec; seqNo:5966 2021-02-13T00:22:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.16 , 82.30 , 61.71 , 0.00 2021-02-13T00:22:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , 0.00 2021-02-13T00:22:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175759.01 epoch sec; seqNo:12084; 4059011.87 north; 592448.14 east 2021-02-13T00:22:39Z- AHRS_M2 msg: 1613175759.38 epoch sec; seqNo:11988 2021-02-13T00:22:39Z- AHRS_M2 msg: 0.63 phi; -0.15 theta; -0.16 psi 2021-02-13T00:22:39Z- handleDepth()-Depth_Keller msg: 1613175759380 epoch sec; seqNo:12085; depth 29.80 2021-02-13T00:22:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175759.40 epoch sec; seqNo:12085; 4059012.24 north; 592448.13 east 2021-02-13T00:22:39Z- AHRS_M2 msg: 1613175759.78 epoch sec; seqNo:11989 2021-02-13T00:22:39Z- AHRS_M2 msg: 0.62 phi; -0.11 theta; -0.16 psi 2021-02-13T00:22:39Z- handleDepth()-Depth_Keller msg: 1613175759784 epoch sec; seqNo:12086; depth 29.91 2021-02-13T00:22:39Z- handleDvl() - RDI_Pathfinder msg: 1613175759.80 epoch sec; seqNo:5967 2021-02-13T00:22:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.76 , 160.72 , 62.12 , 169.37 2021-02-13T00:22:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.19 , 0.65 , -0.39 2021-02-13T00:22:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175759.82 epoch sec; seqNo:12086; 4059012.24 north; 592448.13 east 2021-02-13T00:22:40Z- AHRS_M2 msg: 1613175760.18 epoch sec; seqNo:11990 2021-02-13T00:22:40Z- AHRS_M2 msg: 0.61 phi; -0.07 theta; -0.16 psi 2021-02-13T00:22:40Z- handleDepth()-Depth_Keller msg: 1613175760188 epoch sec; seqNo:12087; depth 30.02 2021-02-13T00:22:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175760.20 epoch sec; seqNo:12087; 4059012.24 north; 592448.13 east 2021-02-13T00:22:40Z- AHRS_M2 msg: 1613175760.59 epoch sec; seqNo:11991 2021-02-13T00:22:40Z- AHRS_M2 msg: 0.61 phi; -0.03 theta; -0.16 psi 2021-02-13T00:22:40Z- handleDepth()-Depth_Keller msg: 1613175760595 epoch sec; seqNo:12088; depth 30.12 2021-02-13T00:22:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175760.60 epoch sec; seqNo:12088; 4059012.24 north; 592448.13 east 2021-02-13T00:22:40Z- AHRS_M2 msg: 1613175760.99 epoch sec; seqNo:11992 2021-02-13T00:22:40Z- AHRS_M2 msg: 0.60 phi; 0.01 theta; -0.16 psi 2021-02-13T00:22:40Z- handleDepth()-Depth_Keller msg: 1613175760997 epoch sec; seqNo:12089; depth 30.19 2021-02-13T00:22:41Z- handleDvl() - RDI_Pathfinder msg: 1613175761.02 epoch sec; seqNo:5968 2021-02-13T00:22:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.32 , 77.90 , 65.53 , 0.00 2021-02-13T00:22:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.00 , 0.00 2021-02-13T00:22:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175761.03 epoch sec; seqNo:12089; 4059012.24 north; 592448.13 east 2021-02-13T00:22:41Z- AHRS_M2 msg: 1613175761.40 epoch sec; seqNo:11993 2021-02-13T00:22:41Z- AHRS_M2 msg: 0.59 phi; 0.05 theta; -0.15 psi 2021-02-13T00:22:41Z- handleDepth()-Depth_Keller msg: 1613175761400 epoch sec; seqNo:12090; depth 30.25 2021-02-13T00:22:41Z- handleDvl() - RDI_Pathfinder msg: 1613175761.41 epoch sec; seqNo:5969 2021-02-13T00:22:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.45 , 0.00 , 69.60 , 67.33 2021-02-13T00:22:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.03 , -0.01 2021-02-13T00:22:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175761.44 epoch sec; seqNo:12090; 4059012.64 north; 592449.35 east 2021-02-13T00:22:41Z- AHRS_M2 msg: 1613175761.80 epoch sec; seqNo:11994 2021-02-13T00:22:41Z- AHRS_M2 msg: 0.58 phi; 0.09 theta; -0.15 psi 2021-02-13T00:22:41Z- handleDepth()-Depth_Keller msg: 1613175761809 epoch sec; seqNo:12091; depth 30.29 2021-02-13T00:22:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175761.82 epoch sec; seqNo:12091; 4059013.02 north; 592449.34 east 2021-02-13T00:22:42Z- AHRS_M2 msg: 1613175762.20 epoch sec; seqNo:11995 2021-02-13T00:22:42Z- AHRS_M2 msg: 0.58 phi; 0.12 theta; -0.15 psi 2021-02-13T00:22:42Z- handleDepth()-Depth_Keller msg: 1613175762210 epoch sec; seqNo:12092; depth 30.31 2021-02-13T00:22:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175762.22 epoch sec; seqNo:12092; 4059013.39 north; 592449.34 east 2021-02-13T00:22:42Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:22:42Z- waiting for 1613175759.98, time is 1613175762.22 2021-02-13T00:22:42Z- LcmTrn::updateTrn() >>>> pose time 1613175762.22, meas time 1613175761.41 <<<< 2021-02-13T00:22:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175761.41 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:22:42Z- TerrainNav::motionUpdate() - currEstimate = (4059013, 592449.34, 30.31) 2021-02-13T00:22:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:22:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:22:42Z- TNavBF::Projecting Measurements 2021-02-13T00:22:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:22:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:22:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:22:42Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:22:42Z- TNavFilter::3 good beams. 2021-02-13T00:22:42Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:22:42Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:22:42Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:22:42Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:22:42Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:22:42Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:22:42Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:22:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:22:42Z- TerrainMapOctree: (vehN, vehE ) = (4059012.98, 592448.97). 2021-02-13T00:22:42Z- TerrainMapOctree: Min Distance = 2871.93. 2021-02-13T00:22:42Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:22:42Z- There is no specific good beam that all particles have in common. 2021-02-13T00:22:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:22:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.124 ExpectedMeasDiff: nan currMeas.covariance: 6.44 2021-02-13T00:22:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:22:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 7.75 2021-02-13T00:22:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:22:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 7.25 2021-02-13T00:22:42Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:22:42Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:22:42Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:22:42Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:22:42Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:22:42Z- Filter 0 No beams used in subcloud update 2021-02-13T00:22:42Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:22:42Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:22:42Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 705.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:22:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175761.41 sec. 2021-02-13T00:22:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:22:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:22:42Z- variances([0], [2], and psi_berg) = 705.798951 833.001085 0.000000 2021-02-13T00:22:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:22:42Z- LcmTrn::publishEstimates() - MLE : 4059012.89 592498.84 30.31 2021-02-13T00:22:42Z- LcmTrn::publishEstimates() - MMSE : 4059013.39 592449.34 30.31 2021-02-13T00:22:42Z- LcmTrn::publishEstimates() - COVAR: 705.80 833.00 0.86 0.00 2021-02-13T00:22:42Z- AHRS_M2 msg: 1613175762.61 epoch sec; seqNo:11996 2021-02-13T00:22:42Z- AHRS_M2 msg: 0.57 phi; 0.15 theta; -0.15 psi 2021-02-13T00:22:42Z- handleDepth()-Depth_Keller msg: 1613175762611 epoch sec; seqNo:12093; depth 30.34 2021-02-13T00:22:42Z- handleDvl() - RDI_Pathfinder msg: 1613175762.62 epoch sec; seqNo:5970 2021-02-13T00:22:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 64.10 , 0.00 , 0.00 , 64.40 2021-02-13T00:22:42Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:22:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175762.65 epoch sec; seqNo:12093; 4059013.39 north; 592449.34 east 2021-02-13T00:22:43Z- AHRS_M2 msg: 1613175763.01 epoch sec; seqNo:11997 2021-02-13T00:22:43Z- AHRS_M2 msg: 0.57 phi; 0.18 theta; -0.15 psi 2021-02-13T00:22:43Z- handleDepth()-Depth_Keller msg: 1613175763016 epoch sec; seqNo:12094; depth 30.33 2021-02-13T00:22:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175763.03 epoch sec; seqNo:12094; 4059013.77 north; 592449.34 east 2021-02-13T00:22:43Z- AHRS_M2 msg: 1613175763.43 epoch sec; seqNo:11998 2021-02-13T00:22:43Z- AHRS_M2 msg: 0.56 phi; 0.21 theta; -0.15 psi 2021-02-13T00:22:43Z- handleDepth()-Depth_Keller msg: 1613175763433 epoch sec; seqNo:12095; depth 30.32 2021-02-13T00:22:43Z- handleDvl() - RDI_Pathfinder msg: 1613175763.44 epoch sec; seqNo:5971 2021-02-13T00:22:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 65.17 , 0.00 , 0.00 , 63.04 2021-02-13T00:22:43Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:22:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175763.46 epoch sec; seqNo:12095; 4059014.15 north; 592449.33 east 2021-02-13T00:22:43Z- AHRS_M2 msg: 1613175763.82 epoch sec; seqNo:11999 2021-02-13T00:22:43Z- AHRS_M2 msg: 0.56 phi; 0.23 theta; -0.15 psi 2021-02-13T00:22:43Z- handleDepth()-Depth_Keller msg: 1613175763824 epoch sec; seqNo:12096; depth 30.31 2021-02-13T00:22:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175763.84 epoch sec; seqNo:12096; 4059014.17 north; 592450.55 east 2021-02-13T00:22:44Z- AHRS_M2 msg: 1613175764.22 epoch sec; seqNo:12000 2021-02-13T00:22:44Z- AHRS_M2 msg: 0.56 phi; 0.25 theta; -0.15 psi 2021-02-13T00:22:44Z- handleDepth()-Depth_Keller msg: 1613175764229 epoch sec; seqNo:12097; depth 30.25 2021-02-13T00:22:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175764.24 epoch sec; seqNo:12097; 4059014.54 north; 592450.55 east 2021-02-13T00:22:44Z- AHRS_M2 msg: 1613175764.63 epoch sec; seqNo:12001 2021-02-13T00:22:44Z- AHRS_M2 msg: 0.56 phi; 0.27 theta; -0.15 psi 2021-02-13T00:22:44Z- handleDepth()-Depth_Keller msg: 1613175764633 epoch sec; seqNo:12098; depth 30.21 2021-02-13T00:22:44Z- handleDvl() - RDI_Pathfinder msg: 1613175764.65 epoch sec; seqNo:5972 2021-02-13T00:22:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 66.02 , 0.00 , 0.00 , 61.76 2021-02-13T00:22:44Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:22:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175764.66 epoch sec; seqNo:12098; 4059014.92 north; 592450.54 east 2021-02-13T00:22:45Z- AHRS_M2 msg: 1613175765.03 epoch sec; seqNo:12002 2021-02-13T00:22:45Z- AHRS_M2 msg: 0.56 phi; 0.28 theta; -0.15 psi 2021-02-13T00:22:45Z- handleDepth()-Depth_Keller msg: 1613175765038 epoch sec; seqNo:12099; depth 30.16 2021-02-13T00:22:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175765.05 epoch sec; seqNo:12099; 4059015.30 north; 592450.54 east 2021-02-13T00:22:45Z- AHRS_M2 msg: 1613175765.44 epoch sec; seqNo:12003 2021-02-13T00:22:45Z- AHRS_M2 msg: 0.56 phi; 0.30 theta; -0.15 psi 2021-02-13T00:22:45Z- handleDepth()-Depth_Keller msg: 1613175765440 epoch sec; seqNo:12100; depth 30.10 2021-02-13T00:22:45Z- handleDvl() - RDI_Pathfinder msg: 1613175765.46 epoch sec; seqNo:5973 2021-02-13T00:22:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 66.45 , 0.00 , 0.00 , 60.91 2021-02-13T00:22:45Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:22:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175765.47 epoch sec; seqNo:12100; 4059015.30 north; 592450.54 east 2021-02-13T00:22:45Z- AHRS_M2 msg: 1613175765.84 epoch sec; seqNo:12004 2021-02-13T00:22:45Z- AHRS_M2 msg: 0.56 phi; 0.31 theta; -0.15 psi 2021-02-13T00:22:45Z- handleDepth()-Depth_Keller msg: 1613175765844 epoch sec; seqNo:12101; depth 30.03 2021-02-13T00:22:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175765.86 epoch sec; seqNo:12101; 4059015.68 north; 592450.54 east 2021-02-13T00:22:46Z- AHRS_M2 msg: 1613175766.24 epoch sec; seqNo:12005 2021-02-13T00:22:46Z- AHRS_M2 msg: 0.56 phi; 0.32 theta; -0.15 psi 2021-02-13T00:22:46Z- handleDepth()-Depth_Keller msg: 1613175766249 epoch sec; seqNo:12102; depth 29.94 2021-02-13T00:22:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175766.26 epoch sec; seqNo:12102; 4059016.06 north; 592450.53 east 2021-02-13T00:22:46Z- AHRS_M2 msg: 1613175766.65 epoch sec; seqNo:12006 2021-02-13T00:22:46Z- AHRS_M2 msg: 0.56 phi; 0.33 theta; -0.15 psi 2021-02-13T00:22:46Z- handleDepth()-Depth_Keller msg: 1613175766654 epoch sec; seqNo:12103; depth 29.86 2021-02-13T00:22:46Z- handleDvl() - RDI_Pathfinder msg: 1613175766.67 epoch sec; seqNo:5974 2021-02-13T00:22:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:22:46Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:22:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175766.68 epoch sec; seqNo:12103; 4059016.07 north; 592451.75 east 2021-02-13T00:22:47Z- AHRS_M2 msg: 1613175767.05 epoch sec; seqNo:12007 2021-02-13T00:22:47Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.14 psi 2021-02-13T00:22:47Z- handleDepth()-Depth_Keller msg: 1613175767056 epoch sec; seqNo:12104; depth 29.77 2021-02-13T00:22:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175767.08 epoch sec; seqNo:12104; 4059016.45 north; 592451.75 east 2021-02-13T00:22:47Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 129 calls... 2021-02-13T00:22:47Z- waiting for 1613175765.22, time is 1613175767.08 2021-02-13T00:22:47Z- LcmTrn::updateTrn() >>>> pose time 1613175767.08, meas time 1613175766.67 <<<< 2021-02-13T00:22:47Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175766.67, ping # 0. 2021-02-13T00:22:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175766.67 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:22:47Z- TerrainNav::motionUpdate() - currEstimate = (4059016, 592451.75, 29.77) 2021-02-13T00:22:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:22:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:22:47Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:22:47Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:22:47Z- TNavBF::Projecting Measurements 2021-02-13T00:22:47Z- TNavFilter::Measurement from time = 1613175766.67 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:22:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:22:47Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:22:47Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:22:47Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:22:47Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:22:47Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:22:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:22:47Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 705.7 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:22:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:22:47Z- TNavBF::mmse selected from filter -1 2021-02-13T00:22:47Z- variances([0], [2], and psi_berg) = 705.728070 833.001045 0.000000 2021-02-13T00:22:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:22:47Z- LcmTrn::publishEstimates() - MLE : 4059015.95 592501.25 29.77 2021-02-13T00:22:47Z- LcmTrn::publishEstimates() - MMSE : 4059016.45 592451.75 29.77 2021-02-13T00:22:47Z- LcmTrn::publishEstimates() - COVAR: 705.73 833.00 1.06 0.00 2021-02-13T00:22:47Z- AHRS_M2 msg: 1613175767.46 epoch sec; seqNo:12008 2021-02-13T00:22:47Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.14 psi 2021-02-13T00:22:47Z- handleDepth()-Depth_Keller msg: 1613175767465 epoch sec; seqNo:12105; depth 29.66 2021-02-13T00:22:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175767.47 epoch sec; seqNo:12105; 4059016.83 north; 592451.74 east 2021-02-13T00:22:47Z- AHRS_M2 msg: 1613175767.86 epoch sec; seqNo:12009 2021-02-13T00:22:47Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.13 psi 2021-02-13T00:22:47Z- handleDepth()-Depth_Keller msg: 1613175767864 epoch sec; seqNo:12106; depth 29.58 2021-02-13T00:22:47Z- handleDvl() - RDI_Pathfinder msg: 1613175767.88 epoch sec; seqNo:5975 2021-02-13T00:22:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.85 , 81.76 , 92.58 , 61.31 2021-02-13T00:22:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , -0.01 2021-02-13T00:22:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175767.89 epoch sec; seqNo:12106; 4059016.83 north; 592451.74 east 2021-02-13T00:22:48Z- AHRS_M2 msg: 1613175768.26 epoch sec; seqNo:12010 2021-02-13T00:22:48Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.13 psi 2021-02-13T00:22:48Z- handleDepth()-Depth_Keller msg: 1613175768277 epoch sec; seqNo:12107; depth 29.46 2021-02-13T00:22:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175768.29 epoch sec; seqNo:12107; 4059017.21 north; 592451.74 east 2021-02-13T00:22:48Z- AHRS_M2 msg: 1613175768.67 epoch sec; seqNo:12011 2021-02-13T00:22:48Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.12 psi 2021-02-13T00:22:48Z- handleDepth()-Depth_Keller msg: 1613175768677 epoch sec; seqNo:12108; depth 29.36 2021-02-13T00:22:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175768.69 epoch sec; seqNo:12108; 4059017.59 north; 592451.74 east 2021-02-13T00:22:49Z- AHRS_M2 msg: 1613175769.07 epoch sec; seqNo:12012 2021-02-13T00:22:49Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.12 psi 2021-02-13T00:22:49Z- handleDepth()-Depth_Keller msg: 1613175769076 epoch sec; seqNo:12109; depth 29.24 2021-02-13T00:22:49Z- handleDvl() - RDI_Pathfinder msg: 1613175769.09 epoch sec; seqNo:5976 2021-02-13T00:22:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 80.68 , 97.99 , 60.08 2021-02-13T00:22:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.01 , -0.01 2021-02-13T00:22:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175769.12 epoch sec; seqNo:12109; 4059017.59 north; 592451.74 east 2021-02-13T00:22:49Z- AHRS_M2 msg: 1613175769.48 epoch sec; seqNo:12013 2021-02-13T00:22:49Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.12 psi 2021-02-13T00:22:49Z- handleDepth()-Depth_Keller msg: 1613175769480 epoch sec; seqNo:12110; depth 29.13 2021-02-13T00:22:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175769.49 epoch sec; seqNo:12110; 4059017.98 north; 592452.95 east 2021-02-13T00:22:49Z- AHRS_M2 msg: 1613175769.88 epoch sec; seqNo:12014 2021-02-13T00:22:49Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.12 psi 2021-02-13T00:22:49Z- handleDepth()-Depth_Keller msg: 1613175769893 epoch sec; seqNo:12111; depth 29.02 2021-02-13T00:22:49Z- handleDvl() - RDI_Pathfinder msg: 1613175769.90 epoch sec; seqNo:5977 2021-02-13T00:22:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 81.76 , 97.99 , 60.08 2021-02-13T00:22:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.00 , -0.01 2021-02-13T00:22:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175769.91 epoch sec; seqNo:12111; 4059018.36 north; 592452.95 east 2021-02-13T00:22:50Z- AHRS_M2 msg: 1613175770.28 epoch sec; seqNo:12015 2021-02-13T00:22:50Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.11 psi 2021-02-13T00:22:50Z- handleDepth()-Depth_Keller msg: 1613175770288 epoch sec; seqNo:12112; depth 28.91 2021-02-13T00:22:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175770.31 epoch sec; seqNo:12112; 4059018.36 north; 592452.95 east 2021-02-13T00:22:50Z- AHRS_M2 msg: 1613175770.69 epoch sec; seqNo:12016 2021-02-13T00:22:50Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.11 psi 2021-02-13T00:22:50Z- handleDepth()-Depth_Keller msg: 1613175770693 epoch sec; seqNo:12113; depth 28.79 2021-02-13T00:22:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175770.70 epoch sec; seqNo:12113; 4059018.74 north; 592452.94 east 2021-02-13T00:22:51Z- AHRS_M2 msg: 1613175771.09 epoch sec; seqNo:12017 2021-02-13T00:22:51Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.11 psi 2021-02-13T00:22:51Z- handleDepth()-Depth_Keller msg: 1613175771096 epoch sec; seqNo:12114; depth 28.67 2021-02-13T00:22:51Z- handleDvl() - RDI_Pathfinder msg: 1613175771.11 epoch sec; seqNo:5978 2021-02-13T00:22:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.30 , 80.68 , 97.99 , 61.72 2021-02-13T00:22:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , -0.02 2021-02-13T00:22:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175771.12 epoch sec; seqNo:12114; 4059019.12 north; 592452.94 east 2021-02-13T00:22:51Z- AHRS_M2 msg: 1613175771.51 epoch sec; seqNo:12018 2021-02-13T00:22:51Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.11 psi 2021-02-13T00:22:51Z- handleDepth()-Depth_Keller msg: 1613175771513 epoch sec; seqNo:12115; depth 28.56 2021-02-13T00:22:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175771.53 epoch sec; seqNo:12115; 4059019.12 north; 592452.94 east 2021-02-13T00:22:51Z- AHRS_M2 msg: 1613175771.90 epoch sec; seqNo:12019 2021-02-13T00:22:51Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.11 psi 2021-02-13T00:22:51Z- handleDepth()-Depth_Keller msg: 1613175771904 epoch sec; seqNo:12116; depth 28.42 2021-02-13T00:22:51Z- handleDvl() - RDI_Pathfinder msg: 1613175771.92 epoch sec; seqNo:5979 2021-02-13T00:22:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 81.76 , 97.99 , 62.53 2021-02-13T00:22:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.01 2021-02-13T00:22:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175771.93 epoch sec; seqNo:12116; 4059019.50 north; 592452.94 east 2021-02-13T00:22:52Z- AHRS_M2 msg: 1613175772.30 epoch sec; seqNo:12020 2021-02-13T00:22:52Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.11 psi 2021-02-13T00:22:52Z- handleDepth()-Depth_Keller msg: 1613175772308 epoch sec; seqNo:12117; depth 28.31 2021-02-13T00:22:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175772.32 epoch sec; seqNo:12117; 4059019.89 north; 592454.15 east 2021-02-13T00:22:52Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-13T00:22:52Z- waiting for 1613175770.08, time is 1613175772.32 2021-02-13T00:22:52Z- LcmTrn::updateTrn() >>>> pose time 1613175772.32, meas time 1613175771.92 <<<< 2021-02-13T00:22:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175771.92 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:22:52Z- TerrainNav::motionUpdate() - currEstimate = (4059020, 592454.15, 28.31) 2021-02-13T00:22:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:22:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:22:52Z- TNavBF::Projecting Measurements 2021-02-13T00:22:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:22:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:22:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:22:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:22:52Z- TNavFilter::4 good beams. 2021-02-13T00:22:52Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:22:52Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:22:52Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:22:52Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:22:52Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:22:52Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:22:52Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:22:52Z- TNavBF::mmse selected from filter -1 2021-02-13T00:22:52Z- TerrainMapOctree: (vehN, vehE ) = (4059019.63, 592453.97). 2021-02-13T00:22:52Z- TerrainMapOctree: Min Distance = 2863.61. 2021-02-13T00:22:52Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:22:52Z- There is no specific good beam that all particles have in common. 2021-02-13T00:22:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:22:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:22:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:22:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 10.69 2021-02-13T00:22:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:22:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:22:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:22:52Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 6.26 2021-02-13T00:22:52Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:22:52Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:22:52Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:22:52Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:22:52Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:22:52Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:22:52Z- Filter 0 No beams used in subcloud update 2021-02-13T00:22:52Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:22:52Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:22:52Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 705.7 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:22:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175771.92 sec. 2021-02-13T00:22:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:22:52Z- TNavBF::mmse selected from filter -1 2021-02-13T00:22:52Z- variances([0], [2], and psi_berg) = 705.655187 833.001004 0.000000 2021-02-13T00:22:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:22:52Z- LcmTrn::publishEstimates() - MLE : 4059019.39 592503.65 28.31 2021-02-13T00:22:52Z- LcmTrn::publishEstimates() - MMSE : 4059019.89 592454.15 28.31 2021-02-13T00:22:52Z- LcmTrn::publishEstimates() - COVAR: 705.66 833.00 0.89 0.00 2021-02-13T00:22:52Z- AHRS_M2 msg: 1613175772.71 epoch sec; seqNo:12021 2021-02-13T00:22:52Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.12 psi 2021-02-13T00:22:52Z- handleDepth()-Depth_Keller msg: 1613175772717 epoch sec; seqNo:12118; depth 28.17 2021-02-13T00:22:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175772.72 epoch sec; seqNo:12118; 4059020.27 north; 592454.15 east 2021-02-13T00:22:53Z- AHRS_M2 msg: 1613175773.11 epoch sec; seqNo:12022 2021-02-13T00:22:53Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.12 psi 2021-02-13T00:22:53Z- handleDepth()-Depth_Keller msg: 1613175773130 epoch sec; seqNo:12119; depth 28.06 2021-02-13T00:22:53Z- handleDvl() - RDI_Pathfinder msg: 1613175773.15 epoch sec; seqNo:5980 2021-02-13T00:22:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 80.61 , 97.92 , 60.90 2021-02-13T00:22:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.03 2021-02-13T00:22:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175773.16 epoch sec; seqNo:12119; 4059020.27 north; 592454.15 east 2021-02-13T00:22:53Z- AHRS_M2 msg: 1613175773.52 epoch sec; seqNo:12023 2021-02-13T00:22:53Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.12 psi 2021-02-13T00:22:53Z- handleDepth()-Depth_Keller msg: 1613175773529 epoch sec; seqNo:12120; depth 27.93 2021-02-13T00:22:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175773.56 epoch sec; seqNo:12120; 4059020.65 north; 592454.14 east 2021-02-13T00:22:53Z- AHRS_M2 msg: 1613175773.92 epoch sec; seqNo:12024 2021-02-13T00:22:53Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.13 psi 2021-02-13T00:22:53Z- handleDepth()-Depth_Keller msg: 1613175773924 epoch sec; seqNo:12121; depth 27.80 2021-02-13T00:22:53Z- handleDvl() - RDI_Pathfinder msg: 1613175773.94 epoch sec; seqNo:5981 2021-02-13T00:22:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 82.84 , 97.99 , 62.53 2021-02-13T00:22:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.04 2021-02-13T00:22:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175773.95 epoch sec; seqNo:12121; 4059021.02 north; 592454.14 east 2021-02-13T00:22:54Z- AHRS_M2 msg: 1613175774.33 epoch sec; seqNo:12025 2021-02-13T00:22:54Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.13 psi 2021-02-13T00:22:54Z- handleDepth()-Depth_Keller msg: 1613175774334 epoch sec; seqNo:12122; depth 27.66 2021-02-13T00:22:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175774.35 epoch sec; seqNo:12122; 4059021.02 north; 592454.14 east 2021-02-13T00:22:54Z- AHRS_M2 msg: 1613175774.75 epoch sec; seqNo:12026 2021-02-13T00:22:54Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.14 psi 2021-02-13T00:22:54Z- handleDepth()-Depth_Keller msg: 1613175774764 epoch sec; seqNo:12123; depth 27.56 2021-02-13T00:22:54Z- handleDvl() - RDI_Pathfinder msg: 1613175774.78 epoch sec; seqNo:5982 2021-02-13T00:22:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 82.84 , 97.99 , 63.35 2021-02-13T00:22:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , 0.04 2021-02-13T00:22:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175774.79 epoch sec; seqNo:12123; 4059021.40 north; 592454.14 east 2021-02-13T00:22:55Z- AHRS_M2 msg: 1613175775.13 epoch sec; seqNo:12027 2021-02-13T00:22:55Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.14 psi 2021-02-13T00:22:55Z- handleDepth()-Depth_Keller msg: 1613175775145 epoch sec; seqNo:12124; depth 27.41 2021-02-13T00:22:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175775.16 epoch sec; seqNo:12124; 4059021.80 north; 592455.35 east 2021-02-13T00:22:55Z- AHRS_M2 msg: 1613175775.55 epoch sec; seqNo:12028 2021-02-13T00:22:55Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T00:22:55Z- handleDepth()-Depth_Keller msg: 1613175775558 epoch sec; seqNo:12125; depth 27.30 2021-02-13T00:22:55Z- handleDvl() - RDI_Pathfinder msg: 1613175775.57 epoch sec; seqNo:5983 2021-02-13T00:22:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:22:55Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:22:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175775.60 epoch sec; seqNo:12125; 4059022.17 north; 592455.35 east 2021-02-13T00:22:55Z- AHRS_M2 msg: 1613175775.94 epoch sec; seqNo:12029 2021-02-13T00:22:55Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-13T00:22:55Z- handleDepth()-Depth_Keller msg: 1613175775947 epoch sec; seqNo:12126; depth 27.18 2021-02-13T00:22:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175775.96 epoch sec; seqNo:12126; 4059022.17 north; 592455.35 east 2021-02-13T00:22:56Z- AHRS_M2 msg: 1613175776.34 epoch sec; seqNo:12030 2021-02-13T00:22:56Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-13T00:22:56Z- handleDepth()-Depth_Keller msg: 1613175776348 epoch sec; seqNo:12127; depth 27.06 2021-02-13T00:22:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175776.36 epoch sec; seqNo:12127; 4059022.55 north; 592455.34 east 2021-02-13T00:22:56Z- AHRS_M2 msg: 1613175776.75 epoch sec; seqNo:12031 2021-02-13T00:22:56Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-13T00:22:56Z- handleDepth()-Depth_Keller msg: 1613175776757 epoch sec; seqNo:12128; depth 26.94 2021-02-13T00:22:56Z- handleDvl() - RDI_Pathfinder msg: 1613175776.77 epoch sec; seqNo:5984 2021-02-13T00:22:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 71.29 , 0.00 , 0.00 , 63.94 2021-02-13T00:22:56Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:22:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175776.80 epoch sec; seqNo:12128; 4059022.93 north; 592455.34 east 2021-02-13T00:22:57Z- AHRS_M2 msg: 1613175777.15 epoch sec; seqNo:12032 2021-02-13T00:22:57Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-13T00:22:57Z- handleDepth()-Depth_Keller msg: 1613175777160 epoch sec; seqNo:12129; depth 26.81 2021-02-13T00:22:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175777.17 epoch sec; seqNo:12129; 4059022.93 north; 592455.34 east 2021-02-13T00:22:57Z- AHRS_M2 msg: 1613175777.56 epoch sec; seqNo:12033 2021-02-13T00:22:57Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-13T00:22:57Z- LcmTrn::handleMessages() - handling 42 messages took 0.70 ms and 128 calls... 2021-02-13T00:22:57Z- waiting for 1613175775.32, time is 1613175777.56 2021-02-13T00:22:57Z- LcmTrn::updateTrn() >>>> pose time 1613175777.56, meas time 1613175776.77 <<<< 2021-02-13T00:22:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175776.77 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:22:57Z- TerrainNav::motionUpdate() - currEstimate = (4059023, 592455.34, 26.81) 2021-02-13T00:22:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:22:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:22:57Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:22:57Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:22:57Z- TNavBF::Projecting Measurements 2021-02-13T00:22:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:22:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:22:57Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-13T00:22:57Z- TNavFilter::2 good beams. 2021-02-13T00:22:57Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:22:57Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:22:57Z- TNavBF:: beamIndex[1] = 3 2021-02-13T00:22:57Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:22:57Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:22:57Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-13T00:22:57Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:22:57Z- TNavBF::mmse selected from filter -1 2021-02-13T00:22:57Z- TerrainMapOctree: (vehN, vehE ) = (4059022.47, 592455.16). 2021-02-13T00:22:57Z- TerrainMapOctree: Min Distance = 2860.65. 2021-02-13T00:22:57Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:22:57Z- There is no specific good beam that all particles have in common. 2021-02-13T00:22:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:22:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 8.13 2021-02-13T00:22:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:22:57Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 6.54 2021-02-13T00:22:57Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:22:57Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:22:57Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:22:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:22:57Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:22:57Z- Filter 0 No beams used in subcloud update 2021-02-13T00:22:57Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:22:57Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:22:57Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 705.6 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:22:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175776.77 sec. 2021-02-13T00:22:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:22:57Z- TNavBF::mmse selected from filter -1 2021-02-13T00:22:57Z- variances([0], [2], and psi_berg) = 705.587117 833.000966 0.000000 2021-02-13T00:22:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:22:57Z- LcmTrn::publishEstimates() - MLE : 4059022.43 592504.84 26.81 2021-02-13T00:22:57Z- LcmTrn::publishEstimates() - MMSE : 4059022.93 592455.34 26.81 2021-02-13T00:22:57Z- LcmTrn::publishEstimates() - COVAR: 705.59 833.00 0.86 0.00 2021-02-13T00:22:57Z- handleDepth()-Depth_Keller msg: 1613175777566 epoch sec; seqNo:12130; depth 26.70 2021-02-13T00:22:57Z- handleDvl() - RDI_Pathfinder msg: 1613175777.58 epoch sec; seqNo:5985 2021-02-13T00:22:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 63.02 2021-02-13T00:22:57Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:22:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175777.59 epoch sec; seqNo:12130; 4059023.32 north; 592456.56 east 2021-02-13T00:22:57Z- AHRS_M2 msg: 1613175777.96 epoch sec; seqNo:12034 2021-02-13T00:22:57Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-13T00:22:57Z- handleDepth()-Depth_Keller msg: 1613175777964 epoch sec; seqNo:12131; depth 26.57 2021-02-13T00:22:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175777.98 epoch sec; seqNo:12131; 4059023.70 north; 592456.55 east 2021-02-13T00:22:58Z- AHRS_M2 msg: 1613175778.36 epoch sec; seqNo:12035 2021-02-13T00:22:58Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-13T00:22:58Z- handleDepth()-Depth_Keller msg: 1613175778368 epoch sec; seqNo:12132; depth 26.44 2021-02-13T00:22:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175778.38 epoch sec; seqNo:12132; 4059024.08 north; 592456.55 east 2021-02-13T00:22:58Z- AHRS_M2 msg: 1613175778.77 epoch sec; seqNo:12036 2021-02-13T00:22:58Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.16 psi 2021-02-13T00:22:58Z- handleDepth()-Depth_Keller msg: 1613175778772 epoch sec; seqNo:12133; depth 26.33 2021-02-13T00:22:58Z- handleDvl() - RDI_Pathfinder msg: 1613175778.78 epoch sec; seqNo:5986 2021-02-13T00:22:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 71.76 , 0.00 , 0.00 , 63.48 2021-02-13T00:22:58Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:22:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175778.80 epoch sec; seqNo:12133; 4059024.08 north; 592456.55 east 2021-02-13T00:22:59Z- AHRS_M2 msg: 1613175779.17 epoch sec; seqNo:12037 2021-02-13T00:22:59Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.16 psi 2021-02-13T00:22:59Z- handleDepth()-Depth_Keller msg: 1613175779185 epoch sec; seqNo:12134; depth 26.19 2021-02-13T00:22:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175779.20 epoch sec; seqNo:12134; 4059024.46 north; 592456.55 east 2021-02-13T00:22:59Z- AHRS_M2 msg: 1613175779.58 epoch sec; seqNo:12038 2021-02-13T00:22:59Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.17 psi 2021-02-13T00:22:59Z- handleDepth()-Depth_Keller msg: 1613175779580 epoch sec; seqNo:12135; depth 26.08 2021-02-13T00:22:59Z- handleDvl() - RDI_Pathfinder msg: 1613175779.60 epoch sec; seqNo:5987 2021-02-13T00:22:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:22:59Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:22:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175779.61 epoch sec; seqNo:12135; 4059024.84 north; 592456.54 east 2021-02-13T00:22:59Z- AHRS_M2 msg: 1613175779.98 epoch sec; seqNo:12039 2021-02-13T00:22:59Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.17 psi 2021-02-13T00:22:59Z- handleDepth()-Depth_Keller msg: 1613175779989 epoch sec; seqNo:12136; depth 25.95 2021-02-13T00:23:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175780.00 epoch sec; seqNo:12136; 4059024.84 north; 592456.54 east 2021-02-13T00:23:00Z- AHRS_M2 msg: 1613175780.38 epoch sec; seqNo:12040 2021-02-13T00:23:00Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.17 psi 2021-02-13T00:23:00Z- handleDepth()-Depth_Keller msg: 1613175780391 epoch sec; seqNo:12137; depth 25.82 2021-02-13T00:23:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175780.40 epoch sec; seqNo:12137; 4059025.23 north; 592457.76 east 2021-02-13T00:23:00Z- AHRS_M2 msg: 1613175780.79 epoch sec; seqNo:12041 2021-02-13T00:23:00Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.17 psi 2021-02-13T00:23:00Z- handleDepth()-Depth_Keller msg: 1613175780794 epoch sec; seqNo:12138; depth 25.70 2021-02-13T00:23:00Z- handleDvl() - RDI_Pathfinder msg: 1613175780.81 epoch sec; seqNo:5988 2021-02-13T00:23:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.53 , 87.17 , 99.07 , 64.17 2021-02-13T00:23:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.02 , -0.01 2021-02-13T00:23:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175780.82 epoch sec; seqNo:12138; 4059025.61 north; 592457.75 east 2021-02-13T00:23:01Z- AHRS_M2 msg: 1613175781.19 epoch sec; seqNo:12042 2021-02-13T00:23:01Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.17 psi 2021-02-13T00:23:01Z- handleDepth()-Depth_Keller msg: 1613175781197 epoch sec; seqNo:12139; depth 25.57 2021-02-13T00:23:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175781.21 epoch sec; seqNo:12139; 4059025.61 north; 592457.75 east 2021-02-13T00:23:01Z- AHRS_M2 msg: 1613175781.60 epoch sec; seqNo:12043 2021-02-13T00:23:01Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.17 psi 2021-02-13T00:23:01Z- handleDepth()-Depth_Keller msg: 1613175781601 epoch sec; seqNo:12140; depth 25.46 2021-02-13T00:23:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175781.61 epoch sec; seqNo:12140; 4059025.99 north; 592457.75 east 2021-02-13T00:23:02Z- AHRS_M2 msg: 1613175782.00 epoch sec; seqNo:12044 2021-02-13T00:23:02Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.16 psi 2021-02-13T00:23:02Z- handleDepth()-Depth_Keller msg: 1613175782009 epoch sec; seqNo:12141; depth 25.34 2021-02-13T00:23:02Z- handleDvl() - RDI_Pathfinder msg: 1613175782.02 epoch sec; seqNo:5989 2021-02-13T00:23:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 88.18 , 100.08 , 64.58 2021-02-13T00:23:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , -0.01 2021-02-13T00:23:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175782.03 epoch sec; seqNo:12141; 4059026.37 north; 592457.75 east 2021-02-13T00:23:02Z- AHRS_M2 msg: 1613175782.40 epoch sec; seqNo:12045 2021-02-13T00:23:02Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.16 psi 2021-02-13T00:23:02Z- handleDepth()-Depth_Keller msg: 1613175782408 epoch sec; seqNo:12142; depth 25.21 2021-02-13T00:23:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175782.42 epoch sec; seqNo:12142; 4059026.75 north; 592457.74 east 2021-02-13T00:23:02Z- LcmTrn::handleMessages() - handling 43 messages took 0.65 ms and 130 calls... 2021-02-13T00:23:02Z- waiting for 1613175780.56, time is 1613175782.42 2021-02-13T00:23:02Z- LcmTrn::updateTrn() >>>> pose time 1613175782.42, meas time 1613175782.02 <<<< 2021-02-13T00:23:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175782.02 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:23:02Z- TerrainNav::motionUpdate() - currEstimate = (4059027, 592457.74, 25.21) 2021-02-13T00:23:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:23:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:23:02Z- TNavBF::Projecting Measurements 2021-02-13T00:23:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:23:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:23:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:23:02Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:23:02Z- TNavFilter::3 good beams. 2021-02-13T00:23:02Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:23:02Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:23:02Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:23:02Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:23:02Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:23:02Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:23:02Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:23:02Z- TNavBF::mmse selected from filter -1 2021-02-13T00:23:02Z- TerrainMapOctree: (vehN, vehE ) = (4059026.43, 592457.54). 2021-02-13T00:23:02Z- TerrainMapOctree: Min Distance = 2856.08. 2021-02-13T00:23:02Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:23:02Z- There is no specific good beam that all particles have in common. 2021-02-13T00:23:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:23:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 12.44 2021-02-13T00:23:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:23:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 16.03 2021-02-13T00:23:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:23:02Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 6.67 2021-02-13T00:23:02Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:23:02Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:23:02Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:23:02Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:23:02Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:23:02Z- Filter 0 No beams used in subcloud update 2021-02-13T00:23:02Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:23:02Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:23:02Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 705.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:23:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175782.02 sec. 2021-02-13T00:23:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:23:02Z- TNavBF::mmse selected from filter -1 2021-02-13T00:23:02Z- variances([0], [2], and psi_berg) = 705.511120 833.000924 0.000000 2021-02-13T00:23:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:23:02Z- LcmTrn::publishEstimates() - MLE : 4059026.25 592507.24 25.21 2021-02-13T00:23:02Z- LcmTrn::publishEstimates() - MMSE : 4059026.75 592457.74 25.21 2021-02-13T00:23:02Z- LcmTrn::publishEstimates() - COVAR: 705.51 833.00 0.87 0.00 2021-02-13T00:23:02Z- AHRS_M2 msg: 1613175782.81 epoch sec; seqNo:12046 2021-02-13T00:23:02Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.15 psi 2021-02-13T00:23:02Z- handleDepth()-Depth_Keller msg: 1613175782812 epoch sec; seqNo:12143; depth 25.10 2021-02-13T00:23:02Z- handleDvl() - RDI_Pathfinder msg: 1613175782.82 epoch sec; seqNo:5990 2021-02-13T00:23:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.35 , 0.00 , 100.69 , 64.17 2021-02-13T00:23:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , 0.00 2021-02-13T00:23:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175782.84 epoch sec; seqNo:12143; 4059026.76 north; 592458.96 east 2021-02-13T00:23:03Z- AHRS_M2 msg: 1613175783.21 epoch sec; seqNo:12047 2021-02-13T00:23:03Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.15 psi 2021-02-13T00:23:03Z- handleDepth()-Depth_Keller msg: 1613175783218 epoch sec; seqNo:12144; depth 24.96 2021-02-13T00:23:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175783.23 epoch sec; seqNo:12144; 4059027.14 north; 592458.96 east 2021-02-13T00:23:03Z- AHRS_M2 msg: 1613175783.62 epoch sec; seqNo:12048 2021-02-13T00:23:03Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.14 psi 2021-02-13T00:23:03Z- handleDepth()-Depth_Keller msg: 1613175783630 epoch sec; seqNo:12145; depth 24.84 2021-02-13T00:23:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175783.64 epoch sec; seqNo:12145; 4059027.52 north; 592458.95 east 2021-02-13T00:23:04Z- AHRS_M2 msg: 1613175784.02 epoch sec; seqNo:12049 2021-02-13T00:23:04Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.14 psi 2021-02-13T00:23:04Z- handleDepth()-Depth_Keller msg: 1613175784024 epoch sec; seqNo:12146; depth 24.71 2021-02-13T00:23:04Z- handleDvl() - RDI_Pathfinder msg: 1613175784.04 epoch sec; seqNo:5991 2021-02-13T00:23:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.35 , 87.17 , 100.15 , 65.81 2021-02-13T00:23:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , 0.02 2021-02-13T00:23:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175784.05 epoch sec; seqNo:12146; 4059027.52 north; 592458.95 east 2021-02-13T00:23:04Z- AHRS_M2 msg: 1613175784.42 epoch sec; seqNo:12050 2021-02-13T00:23:04Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.14 psi 2021-02-13T00:23:04Z- handleDepth()-Depth_Keller msg: 1613175784433 epoch sec; seqNo:12147; depth 24.59 2021-02-13T00:23:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175784.44 epoch sec; seqNo:12147; 4059027.90 north; 592458.95 east 2021-02-13T00:23:04Z- AHRS_M2 msg: 1613175784.83 epoch sec; seqNo:12051 2021-02-13T00:23:04Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.14 psi 2021-02-13T00:23:04Z- handleDepth()-Depth_Keller msg: 1613175784841 epoch sec; seqNo:12148; depth 24.46 2021-02-13T00:23:04Z- handleDvl() - RDI_Pathfinder msg: 1613175784.85 epoch sec; seqNo:5992 2021-02-13T00:23:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.57 , 87.03 , 101.10 , 66.21 2021-02-13T00:23:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , 0.04 2021-02-13T00:23:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175784.86 epoch sec; seqNo:12148; 4059028.27 north; 592458.95 east 2021-02-13T00:23:05Z- AHRS_M2 msg: 1613175785.23 epoch sec; seqNo:12052 2021-02-13T00:23:05Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.14 psi 2021-02-13T00:23:05Z- handleDepth()-Depth_Keller msg: 1613175785239 epoch sec; seqNo:12149; depth 24.33 2021-02-13T00:23:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175785.26 epoch sec; seqNo:12149; 4059028.27 north; 592458.95 east 2021-02-13T00:23:05Z- AHRS_M2 msg: 1613175785.64 epoch sec; seqNo:12053 2021-02-13T00:23:05Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.14 psi 2021-02-13T00:23:05Z- handleDepth()-Depth_Keller msg: 1613175785645 epoch sec; seqNo:12150; depth 24.19 2021-02-13T00:23:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175785.65 epoch sec; seqNo:12150; 4059028.67 north; 592460.16 east 2021-02-13T00:23:06Z- AHRS_M2 msg: 1613175786.04 epoch sec; seqNo:12054 2021-02-13T00:23:06Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.14 psi 2021-02-13T00:23:06Z- handleDepth()-Depth_Keller msg: 1613175786049 epoch sec; seqNo:12151; depth 24.08 2021-02-13T00:23:06Z- handleDvl() - RDI_Pathfinder msg: 1613175786.06 epoch sec; seqNo:5993 2021-02-13T00:23:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 170.27 , 0.00 , 106.44 , 64.99 2021-02-13T00:23:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 1.80 , 0.10 2021-02-13T00:23:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175786.07 epoch sec; seqNo:12151; 4059028.29 north; 592460.17 east 2021-02-13T00:23:06Z- AHRS_M2 msg: 1613175786.44 epoch sec; seqNo:12055 2021-02-13T00:23:06Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.15 psi 2021-02-13T00:23:06Z- handleDepth()-Depth_Keller msg: 1613175786457 epoch sec; seqNo:12152; depth 23.96 2021-02-13T00:23:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175786.47 epoch sec; seqNo:12152; 4059028.30 north; 592461.39 east 2021-02-13T00:23:06Z- AHRS_M2 msg: 1613175786.87 epoch sec; seqNo:12056 2021-02-13T00:23:06Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.15 psi 2021-02-13T00:23:06Z- handleDepth()-Depth_Keller msg: 1613175786876 epoch sec; seqNo:12153; depth 23.82 2021-02-13T00:23:06Z- handleDvl() - RDI_Pathfinder msg: 1613175786.89 epoch sec; seqNo:5994 2021-02-13T00:23:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.06 , 88.32 , 101.71 , 66.42 2021-02-13T00:23:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , 0.11 2021-02-13T00:23:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175786.90 epoch sec; seqNo:12153; 4059028.68 north; 592461.38 east 2021-02-13T00:23:07Z- AHRS_M2 msg: 1613175787.25 epoch sec; seqNo:12057 2021-02-13T00:23:07Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.15 psi 2021-02-13T00:23:07Z- handleDepth()-Depth_Keller msg: 1613175787261 epoch sec; seqNo:12154; depth 23.68 2021-02-13T00:23:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175787.27 epoch sec; seqNo:12154; 4059028.68 north; 592461.38 east 2021-02-13T00:23:07Z- AHRS_M2 msg: 1613175787.67 epoch sec; seqNo:12058 2021-02-13T00:23:07Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.15 psi 2021-02-13T00:23:07Z- handleDepth()-Depth_Keller msg: 1613175787680 epoch sec; seqNo:12155; depth 23.57 2021-02-13T00:23:07Z- handleDvl() - RDI_Pathfinder msg: 1613175787.69 epoch sec; seqNo:5995 2021-02-13T00:23:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.39 , 0.00 , 103.26 , 64.99 2021-02-13T00:23:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.03 , 0.09 2021-02-13T00:23:07Z- LcmTrn::handleMessages() - handling 44 messages took 0.59 ms and 132 calls... 2021-02-13T00:23:07Z- waiting for 1613175785.42, time is 1613175787.69 2021-02-13T00:23:07Z- LcmTrn::updateTrn() equating pose 1613175787.69 and meas times 1613175787.69 2021-02-13T00:23:07Z- LcmTrn::updateTrn() >>>> pose time 1613175787.69, meas time 1613175787.69 <<<< 2021-02-13T00:23:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175787.69 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:23:07Z- TerrainNav::motionUpdate() - currEstimate = (4059029, 592461.38, 23.57) 2021-02-13T00:23:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:23:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:23:07Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175787.69 sec.; No relevant navigation data available 2021-02-13T00:23:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:23:07Z- TNavBF::mmse selected from filter -1 2021-02-13T00:23:07Z- variances([0], [2], and psi_berg) = 705.452807 833.000892 0.000000 2021-02-13T00:23:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:23:07Z- LcmTrn::publishEstimates() - MLE : 4059028.18 592510.88 23.57 2021-02-13T00:23:07Z- LcmTrn::publishEstimates() - MMSE : 4059028.68 592461.38 23.57 2021-02-13T00:23:07Z- LcmTrn::publishEstimates() - COVAR: 705.45 833.00 1.08 0.00 2021-02-13T00:23:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175787.72 epoch sec; seqNo:12155; 4059029.06 north; 592461.38 east 2021-02-13T00:23:08Z- AHRS_M2 msg: 1613175788.06 epoch sec; seqNo:12059 2021-02-13T00:23:08Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.15 psi 2021-02-13T00:23:08Z- handleDepth()-Depth_Keller msg: 1613175788070 epoch sec; seqNo:12156; depth 23.44 2021-02-13T00:23:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175788.08 epoch sec; seqNo:12156; 4059029.45 north; 592462.60 east 2021-02-13T00:23:08Z- AHRS_M2 msg: 1613175788.46 epoch sec; seqNo:12060 2021-02-13T00:23:08Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.15 psi 2021-02-13T00:23:08Z- handleDepth()-Depth_Keller msg: 1613175788468 epoch sec; seqNo:12157; depth 23.32 2021-02-13T00:23:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175788.48 epoch sec; seqNo:12157; 4059029.83 north; 592462.59 east 2021-02-13T00:23:08Z- AHRS_M2 msg: 1613175788.87 epoch sec; seqNo:12061 2021-02-13T00:23:08Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.16 psi 2021-02-13T00:23:08Z- handleDepth()-Depth_Keller msg: 1613175788877 epoch sec; seqNo:12158; depth 23.18 2021-02-13T00:23:08Z- handleDvl() - RDI_Pathfinder msg: 1613175788.89 epoch sec; seqNo:5996 2021-02-13T00:23:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 97.85 , 103.26 , 66.21 2021-02-13T00:23:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.09 , 0.05 2021-02-13T00:23:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175788.90 epoch sec; seqNo:12158; 4059030.21 north; 592462.59 east 2021-02-13T00:23:09Z- AHRS_M2 msg: 1613175789.27 epoch sec; seqNo:12062 2021-02-13T00:23:09Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.16 psi 2021-02-13T00:23:09Z- handleDepth()-Depth_Keller msg: 1613175789276 epoch sec; seqNo:12159; depth 23.06 2021-02-13T00:23:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175789.29 epoch sec; seqNo:12159; 4059030.21 north; 592462.59 east 2021-02-13T00:23:09Z- AHRS_M2 msg: 1613175789.68 epoch sec; seqNo:12063 2021-02-13T00:23:09Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.17 psi 2021-02-13T00:23:09Z- handleDepth()-Depth_Keller msg: 1613175789684 epoch sec; seqNo:12160; depth 22.91 2021-02-13T00:23:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175789.69 epoch sec; seqNo:12160; 4059030.59 north; 592462.58 east 2021-02-13T00:23:10Z- AHRS_M2 msg: 1613175790.08 epoch sec; seqNo:12064 2021-02-13T00:23:10Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.17 psi 2021-02-13T00:23:10Z- handleDepth()-Depth_Keller msg: 1613175790086 epoch sec; seqNo:12161; depth 22.79 2021-02-13T00:23:10Z- handleDvl() - RDI_Pathfinder msg: 1613175790.10 epoch sec; seqNo:5997 2021-02-13T00:23:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.21 , 90.82 , 105.42 , 67.44 2021-02-13T00:23:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , -0.06 2021-02-13T00:23:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175790.11 epoch sec; seqNo:12161; 4059030.97 north; 592462.58 east 2021-02-13T00:23:10Z- AHRS_M2 msg: 1613175790.48 epoch sec; seqNo:12065 2021-02-13T00:23:10Z- AHRS_M2 msg: 0.67 phi; 0.37 theta; -0.17 psi 2021-02-13T00:23:10Z- handleDepth()-Depth_Keller msg: 1613175790488 epoch sec; seqNo:12162; depth 22.66 2021-02-13T00:23:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175790.51 epoch sec; seqNo:12162; 4059031.36 north; 592463.80 east 2021-02-13T00:23:10Z- AHRS_M2 msg: 1613175790.89 epoch sec; seqNo:12066 2021-02-13T00:23:10Z- AHRS_M2 msg: 0.67 phi; 0.36 theta; -0.17 psi 2021-02-13T00:23:10Z- handleDepth()-Depth_Keller msg: 1613175790892 epoch sec; seqNo:12163; depth 22.52 2021-02-13T00:23:10Z- handleDvl() - RDI_Pathfinder msg: 1613175790.90 epoch sec; seqNo:5998 2021-02-13T00:23:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 92.10 , 107.93 , 0.00 2021-02-13T00:23:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , -0.13 2021-02-13T00:23:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175790.93 epoch sec; seqNo:12163; 4059031.36 north; 592463.80 east 2021-02-13T00:23:11Z- AHRS_M2 msg: 1613175791.29 epoch sec; seqNo:12067 2021-02-13T00:23:11Z- AHRS_M2 msg: 0.68 phi; 0.36 theta; -0.17 psi 2021-02-13T00:23:11Z- handleDepth()-Depth_Keller msg: 1613175791296 epoch sec; seqNo:12164; depth 22.38 2021-02-13T00:23:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175791.31 epoch sec; seqNo:12164; 4059031.74 north; 592463.79 east 2021-02-13T00:23:11Z- AHRS_M2 msg: 1613175791.71 epoch sec; seqNo:12068 2021-02-13T00:23:11Z- AHRS_M2 msg: 0.68 phi; 0.35 theta; -0.17 psi 2021-02-13T00:23:11Z- handleDepth()-Depth_Keller msg: 1613175791710 epoch sec; seqNo:12165; depth 22.26 2021-02-13T00:23:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175791.72 epoch sec; seqNo:12165; 4059032.12 north; 592463.79 east 2021-02-13T00:23:12Z- AHRS_M2 msg: 1613175792.10 epoch sec; seqNo:12069 2021-02-13T00:23:12Z- AHRS_M2 msg: 0.68 phi; 0.35 theta; -0.17 psi 2021-02-13T00:23:12Z- handleDepth()-Depth_Keller msg: 1613175792120 epoch sec; seqNo:12166; depth 22.13 2021-02-13T00:23:12Z- handleDvl() - RDI_Pathfinder msg: 1613175792.13 epoch sec; seqNo:5999 2021-02-13T00:23:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.17 , 92.78 , 104.95 , 66.21 2021-02-13T00:23:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.07 , -0.11 2021-02-13T00:23:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175792.15 epoch sec; seqNo:12166; 4059032.12 north; 592463.79 east 2021-02-13T00:23:12Z- AHRS_M2 msg: 1613175792.50 epoch sec; seqNo:12070 2021-02-13T00:23:12Z- AHRS_M2 msg: 0.68 phi; 0.34 theta; -0.16 psi 2021-02-13T00:23:12Z- handleDepth()-Depth_Keller msg: 1613175792508 epoch sec; seqNo:12167; depth 22.00 2021-02-13T00:23:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175792.52 epoch sec; seqNo:12167; 4059032.49 north; 592463.78 east 2021-02-13T00:23:12Z- LcmTrn::handleMessages() - handling 41 messages took 0.66 ms and 128 calls... 2021-02-13T00:23:12Z- waiting for 1613175790.69, time is 1613175792.52 2021-02-13T00:23:12Z- LcmTrn::updateTrn() >>>> pose time 1613175792.52, meas time 1613175792.13 <<<< 2021-02-13T00:23:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175792.13 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:23:12Z- TerrainNav::motionUpdate() - currEstimate = (4059032, 592463.78, 22.00) 2021-02-13T00:23:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:23:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:23:12Z- TNavBF::Projecting Measurements 2021-02-13T00:23:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:23:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:23:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:23:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:23:12Z- TNavFilter::4 good beams. 2021-02-13T00:23:12Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:23:12Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:23:12Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:23:12Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:23:12Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:23:12Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:23:12Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:23:12Z- TNavBF::mmse selected from filter -1 2021-02-13T00:23:12Z- TerrainMapOctree: (vehN, vehE ) = (4059032.19, 592463.59). 2021-02-13T00:23:12Z- TerrainMapOctree: Min Distance = 2847.79. 2021-02-13T00:23:12Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:23:12Z- There is no specific good beam that all particles have in common. 2021-02-13T00:23:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:23:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 9.78 2021-02-13T00:23:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:23:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 13.77 2021-02-13T00:23:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:23:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 17.62 2021-02-13T00:23:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:23:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 7.01 2021-02-13T00:23:12Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:23:12Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:23:12Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:23:12Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:23:12Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:23:12Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:23:12Z- Filter 0 No beams used in subcloud update 2021-02-13T00:23:12Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:23:12Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:23:12Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 705.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:23:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175792.13 sec. 2021-02-13T00:23:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:23:12Z- TNavBF::mmse selected from filter -1 2021-02-13T00:23:12Z- variances([0], [2], and psi_berg) = 705.377179 833.000850 0.000000 2021-02-13T00:23:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:23:12Z- LcmTrn::publishEstimates() - MLE : 4059031.99 592513.28 22.00 2021-02-13T00:23:12Z- LcmTrn::publishEstimates() - MMSE : 4059032.49 592463.78 22.00 2021-02-13T00:23:12Z- LcmTrn::publishEstimates() - COVAR: 705.38 833.00 0.93 0.00 2021-02-13T00:23:12Z- AHRS_M2 msg: 1613175792.91 epoch sec; seqNo:12071 2021-02-13T00:23:12Z- AHRS_M2 msg: 0.68 phi; 0.34 theta; -0.16 psi 2021-02-13T00:23:12Z- handleDepth()-Depth_Keller msg: 1613175792918 epoch sec; seqNo:12168; depth 21.86 2021-02-13T00:23:12Z- handleDvl() - RDI_Pathfinder msg: 1613175792.93 epoch sec; seqNo:6000 2021-02-13T00:23:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 98.12 , 104.21 , 67.85 2021-02-13T00:23:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.07 , -0.02 2021-02-13T00:23:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175792.94 epoch sec; seqNo:12168; 4059032.87 north; 592463.78 east 2021-02-13T00:23:13Z- AHRS_M2 msg: 1613175793.31 epoch sec; seqNo:12072 2021-02-13T00:23:13Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.17 psi 2021-02-13T00:23:13Z- handleDepth()-Depth_Keller msg: 1613175793326 epoch sec; seqNo:12169; depth 21.73 2021-02-13T00:23:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175793.34 epoch sec; seqNo:12169; 4059032.89 north; 592465.00 east 2021-02-13T00:23:13Z- AHRS_M2 msg: 1613175793.72 epoch sec; seqNo:12073 2021-02-13T00:23:13Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.17 psi 2021-02-13T00:23:13Z- handleDepth()-Depth_Keller msg: 1613175793720 epoch sec; seqNo:12170; depth 21.59 2021-02-13T00:23:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175793.73 epoch sec; seqNo:12170; 4059033.27 north; 592465.00 east 2021-02-13T00:23:14Z- AHRS_M2 msg: 1613175794.12 epoch sec; seqNo:12074 2021-02-13T00:23:14Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.17 psi 2021-02-13T00:23:14Z- handleDepth()-Depth_Keller msg: 1613175794127 epoch sec; seqNo:12171; depth 21.47 2021-02-13T00:23:14Z- handleDvl() - RDI_Pathfinder msg: 1613175794.14 epoch sec; seqNo:6001 2021-02-13T00:23:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.39 , 97.99 , 104.07 , 67.03 2021-02-13T00:23:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.06 , 0.07 2021-02-13T00:23:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175794.15 epoch sec; seqNo:12171; 4059033.27 north; 592465.00 east 2021-02-13T00:23:14Z- AHRS_M2 msg: 1613175794.52 epoch sec; seqNo:12075 2021-02-13T00:23:14Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.17 psi 2021-02-13T00:23:14Z- handleDepth()-Depth_Keller msg: 1613175794530 epoch sec; seqNo:12172; depth 21.34 2021-02-13T00:23:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175794.54 epoch sec; seqNo:12172; 4059033.64 north; 592464.99 east 2021-02-13T00:23:14Z- AHRS_M2 msg: 1613175794.93 epoch sec; seqNo:12076 2021-02-13T00:23:14Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.17 psi 2021-02-13T00:23:14Z- handleDepth()-Depth_Keller msg: 1613175794932 epoch sec; seqNo:12173; depth 21.21 2021-02-13T00:23:14Z- handleDvl() - RDI_Pathfinder msg: 1613175794.94 epoch sec; seqNo:6002 2021-02-13T00:23:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 98.12 , 104.21 , 68.26 2021-02-13T00:23:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.03 , 0.13 2021-02-13T00:23:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175794.97 epoch sec; seqNo:12173; 4059034.02 north; 592464.99 east 2021-02-13T00:23:15Z- AHRS_M2 msg: 1613175795.33 epoch sec; seqNo:12077 2021-02-13T00:23:15Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.17 psi 2021-02-13T00:23:15Z- handleDepth()-Depth_Keller msg: 1613175795336 epoch sec; seqNo:12174; depth 21.07 2021-02-13T00:23:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175795.35 epoch sec; seqNo:12174; 4059034.02 north; 592464.99 east 2021-02-13T00:23:15Z- AHRS_M2 msg: 1613175795.74 epoch sec; seqNo:12078 2021-02-13T00:23:15Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.16 psi 2021-02-13T00:23:15Z- handleDepth()-Depth_Keller msg: 1613175795742 epoch sec; seqNo:12175; depth 20.95 2021-02-13T00:23:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175795.75 epoch sec; seqNo:12175; 4059034.42 north; 592466.20 east 2021-02-13T00:23:16Z- AHRS_M2 msg: 1613175796.14 epoch sec; seqNo:12079 2021-02-13T00:23:16Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.16 psi 2021-02-13T00:23:16Z- handleDepth()-Depth_Keller msg: 1613175796149 epoch sec; seqNo:12176; depth 20.84 2021-02-13T00:23:16Z- handleDvl() - RDI_Pathfinder msg: 1613175796.16 epoch sec; seqNo:6003 2021-02-13T00:23:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 91.90 , 104.07 , 68.26 2021-02-13T00:23:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.07 , 0.11 2021-02-13T00:23:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175796.17 epoch sec; seqNo:12176; 4059034.79 north; 592466.20 east 2021-02-13T00:23:16Z- AHRS_M2 msg: 1613175796.54 epoch sec; seqNo:12080 2021-02-13T00:23:16Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.17 psi 2021-02-13T00:23:16Z- handleDepth()-Depth_Keller msg: 1613175796548 epoch sec; seqNo:12177; depth 20.69 2021-02-13T00:23:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175796.57 epoch sec; seqNo:12177; 4059034.79 north; 592466.20 east 2021-02-13T00:23:16Z- AHRS_M2 msg: 1613175796.95 epoch sec; seqNo:12081 2021-02-13T00:23:16Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.18 psi 2021-02-13T00:23:16Z- handleDepth()-Depth_Keller msg: 1613175796952 epoch sec; seqNo:12178; depth 20.57 2021-02-13T00:23:16Z- handleDvl() - RDI_Pathfinder msg: 1613175796.96 epoch sec; seqNo:6004 2021-02-13T00:23:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 98.12 , 102.99 , 69.48 2021-02-13T00:23:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.07 , 0.05 2021-02-13T00:23:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175796.98 epoch sec; seqNo:12178; 4059035.17 north; 592466.20 east 2021-02-13T00:23:17Z- AHRS_M2 msg: 1613175797.36 epoch sec; seqNo:12082 2021-02-13T00:23:17Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.19 psi 2021-02-13T00:23:17Z- handleDepth()-Depth_Keller msg: 1613175797364 epoch sec; seqNo:12179; depth 20.45 2021-02-13T00:23:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175797.38 epoch sec; seqNo:12179; 4059035.55 north; 592466.19 east 2021-02-13T00:23:17Z- AHRS_M2 msg: 1613175797.76 epoch sec; seqNo:12083 2021-02-13T00:23:17Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.21 psi 2021-02-13T00:23:17Z- handleDepth()-Depth_Keller msg: 1613175797769 epoch sec; seqNo:12180; depth 20.34 2021-02-13T00:23:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175797.78 epoch sec; seqNo:12180; 4059035.57 north; 592467.41 east 2021-02-13T00:23:17Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-13T00:23:17Z- waiting for 1613175795.52, time is 1613175797.78 2021-02-13T00:23:17Z- LcmTrn::updateTrn() >>>> pose time 1613175797.78, meas time 1613175796.96 <<<< 2021-02-13T00:23:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175796.96 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:23:17Z- TerrainNav::motionUpdate() - currEstimate = (4059036, 592467.41, 20.34) 2021-02-13T00:23:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:23:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:23:17Z- TNavBF::Projecting Measurements 2021-02-13T00:23:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:23:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:23:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:23:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:23:17Z- TNavFilter::4 good beams. 2021-02-13T00:23:17Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:23:17Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:23:17Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:23:17Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:23:17Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:23:17Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:23:17Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:23:17Z- TNavBF::mmse selected from filter -1 2021-02-13T00:23:17Z- TerrainMapOctree: (vehN, vehE ) = (4059035.09, 592466.85). 2021-02-13T00:23:17Z- TerrainMapOctree: Min Distance = 2843.49. 2021-02-13T00:23:17Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:23:17Z- There is no specific good beam that all particles have in common. 2021-02-13T00:23:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:23:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 9.15 2021-02-13T00:23:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:23:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 15.40 2021-02-13T00:23:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:23:17Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 16.97 2021-02-13T00:23:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:23:17Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 7.72 2021-02-13T00:23:17Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:23:17Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:23:17Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:23:17Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:23:17Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:23:17Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:23:17Z- Filter 0 No beams used in subcloud update 2021-02-13T00:23:17Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:23:17Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:23:17Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 705.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:23:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175796.96 sec. 2021-02-13T00:23:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:23:17Z- TNavBF::mmse selected from filter -1 2021-02-13T00:23:17Z- variances([0], [2], and psi_berg) = 705.295117 833.000804 0.000000 2021-02-13T00:23:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:23:17Z- LcmTrn::publishEstimates() - MLE : 4059035.07 592516.91 20.34 2021-02-13T00:23:17Z- LcmTrn::publishEstimates() - MMSE : 4059035.57 592467.41 20.34 2021-02-13T00:23:17Z- LcmTrn::publishEstimates() - COVAR: 705.30 833.00 0.88 0.00 2021-02-13T00:23:18Z- AHRS_M2 msg: 1613175798.16 epoch sec; seqNo:12084 2021-02-13T00:23:18Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.22 psi 2021-02-13T00:23:18Z- handleDepth()-Depth_Keller msg: 1613175798169 epoch sec; seqNo:12181; depth 20.22 2021-02-13T00:23:18Z- handleDvl() - RDI_Pathfinder msg: 1613175798.19 epoch sec; seqNo:6005 2021-02-13T00:23:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 104.14 , 69.48 2021-02-13T00:23:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , 0.01 2021-02-13T00:23:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175798.20 epoch sec; seqNo:12181; 4059035.94 north; 592467.41 east 2021-02-13T00:23:18Z- AHRS_M2 msg: 1613175798.56 epoch sec; seqNo:12085 2021-02-13T00:23:18Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.23 psi 2021-02-13T00:23:18Z- handleDepth()-Depth_Keller msg: 1613175798568 epoch sec; seqNo:12182; depth 20.08 2021-02-13T00:23:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175798.58 epoch sec; seqNo:12182; 4059036.32 north; 592467.40 east 2021-02-13T00:23:18Z- AHRS_M2 msg: 1613175798.97 epoch sec; seqNo:12086 2021-02-13T00:23:18Z- AHRS_M2 msg: 0.66 phi; 0.30 theta; -0.23 psi 2021-02-13T00:23:18Z- handleDepth()-Depth_Keller msg: 1613175798973 epoch sec; seqNo:12183; depth 19.98 2021-02-13T00:23:18Z- handleDvl() - RDI_Pathfinder msg: 1613175798.98 epoch sec; seqNo:6006 2021-02-13T00:23:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.39 , 97.99 , 102.85 , 78.51 2021-02-13T00:23:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.06 2021-02-13T00:23:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175798.99 epoch sec; seqNo:12183; 4059036.32 north; 592467.40 east 2021-02-13T00:23:19Z- AHRS_M2 msg: 1613175799.37 epoch sec; seqNo:12087 2021-02-13T00:23:19Z- AHRS_M2 msg: 0.65 phi; 0.30 theta; -0.23 psi 2021-02-13T00:23:19Z- handleDepth()-Depth_Keller msg: 1613175799378 epoch sec; seqNo:12184; depth 19.85 2021-02-13T00:23:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175799.39 epoch sec; seqNo:12184; 4059036.70 north; 592467.40 east 2021-02-13T00:23:19Z- AHRS_M2 msg: 1613175799.78 epoch sec; seqNo:12088 2021-02-13T00:23:19Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.22 psi 2021-02-13T00:23:19Z- handleDepth()-Depth_Keller msg: 1613175799780 epoch sec; seqNo:12185; depth 19.74 2021-02-13T00:23:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175799.80 epoch sec; seqNo:12185; 4059037.08 north; 592467.40 east 2021-02-13T00:23:20Z- AHRS_M2 msg: 1613175800.18 epoch sec; seqNo:12089 2021-02-13T00:23:20Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.22 psi 2021-02-13T00:23:20Z- handleDepth()-Depth_Keller msg: 1613175800189 epoch sec; seqNo:12186; depth 19.64 2021-02-13T00:23:20Z- handleDvl() - RDI_Pathfinder msg: 1613175800.20 epoch sec; seqNo:6007 2021-02-13T00:23:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.92 , 104.00 , 71.94 2021-02-13T00:23:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , 0.11 2021-02-13T00:23:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175800.22 epoch sec; seqNo:12186; 4059037.47 north; 592468.61 east 2021-02-13T00:23:20Z- AHRS_M2 msg: 1613175800.58 epoch sec; seqNo:12090 2021-02-13T00:23:20Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.21 psi 2021-02-13T00:23:20Z- handleDepth()-Depth_Keller msg: 1613175800593 epoch sec; seqNo:12187; depth 19.53 2021-02-13T00:23:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175800.60 epoch sec; seqNo:12187; 4059037.47 north; 592468.61 east 2021-02-13T00:23:20Z- AHRS_M2 msg: 1613175800.99 epoch sec; seqNo:12091 2021-02-13T00:23:20Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.21 psi 2021-02-13T00:23:21Z- handleDepth()-Depth_Keller msg: 1613175801001 epoch sec; seqNo:12188; depth 19.44 2021-02-13T00:23:21Z- handleDvl() - RDI_Pathfinder msg: 1613175801.01 epoch sec; seqNo:6008 2021-02-13T00:23:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 97.92 , 104.00 , 69.89 2021-02-13T00:23:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.00 , 0.08 2021-02-13T00:23:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175801.02 epoch sec; seqNo:12188; 4059037.85 north; 592468.61 east 2021-02-13T00:23:21Z- AHRS_M2 msg: 1613175801.39 epoch sec; seqNo:12092 2021-02-13T00:23:21Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.20 psi 2021-02-13T00:23:21Z- handleDepth()-Depth_Keller msg: 1613175801396 epoch sec; seqNo:12189; depth 19.32 2021-02-13T00:23:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175801.42 epoch sec; seqNo:12189; 4059038.23 north; 592468.61 east 2021-02-13T00:23:21Z- AHRS_M2 msg: 1613175801.80 epoch sec; seqNo:12093 2021-02-13T00:23:21Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.21 psi 2021-02-13T00:23:21Z- handleDepth()-Depth_Keller msg: 1613175801800 epoch sec; seqNo:12190; depth 19.22 2021-02-13T00:23:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175801.81 epoch sec; seqNo:12190; 4059038.61 north; 592468.60 east 2021-02-13T00:23:22Z- AHRS_M2 msg: 1613175802.20 epoch sec; seqNo:12094 2021-02-13T00:23:22Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.21 psi 2021-02-13T00:23:22Z- handleDepth()-Depth_Keller msg: 1613175802206 epoch sec; seqNo:12191; depth 19.11 2021-02-13T00:23:22Z- handleDvl() - RDI_Pathfinder msg: 1613175802.22 epoch sec; seqNo:6009 2021-02-13T00:23:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 102.92 , 71.53 2021-02-13T00:23:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.06 , -0.02 2021-02-13T00:23:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175802.23 epoch sec; seqNo:12191; 4059038.99 north; 592468.60 east 2021-02-13T00:23:22Z- AHRS_M2 msg: 1613175802.60 epoch sec; seqNo:12095 2021-02-13T00:23:22Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.21 psi 2021-02-13T00:23:22Z- handleDepth()-Depth_Keller msg: 1613175802617 epoch sec; seqNo:12192; depth 19.00 2021-02-13T00:23:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175802.63 epoch sec; seqNo:12192; 4059039.00 north; 592469.82 east 2021-02-13T00:23:22Z- LcmTrn::handleMessages() - handling 41 messages took 0.61 ms and 129 calls... 2021-02-13T00:23:22Z- waiting for 1613175800.78, time is 1613175802.63 2021-02-13T00:23:22Z- LcmTrn::updateTrn() >>>> pose time 1613175802.63, meas time 1613175802.22 <<<< 2021-02-13T00:23:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175802.22 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:23:22Z- TerrainNav::motionUpdate() - currEstimate = (4059039, 592469.82, 19.00) 2021-02-13T00:23:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:23:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:23:22Z- TNavBF::Projecting Measurements 2021-02-13T00:23:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:23:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:23:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:23:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:23:22Z- TNavFilter::4 good beams. 2021-02-13T00:23:22Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:23:22Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:23:22Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:23:22Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:23:22Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:23:22Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:23:22Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:23:22Z- TNavBF::mmse selected from filter -1 2021-02-13T00:23:22Z- TerrainMapOctree: (vehN, vehE ) = (4059038.71, 592469.61). 2021-02-13T00:23:22Z- TerrainMapOctree: Min Distance = 2838.94. 2021-02-13T00:23:22Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:23:22Z- There is no specific good beam that all particles have in common. 2021-02-13T00:23:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:23:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-13T00:23:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:23:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.38 2021-02-13T00:23:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:23:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 16.95 2021-02-13T00:23:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:23:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 8.19 2021-02-13T00:23:22Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:23:22Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:23:22Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:23:22Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:23:22Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:23:23Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:23:23Z- Filter 0 No beams used in subcloud update 2021-02-13T00:23:23Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:23:23Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:23:23Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 705.2 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:23:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175802.22 sec. 2021-02-13T00:23:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:23:23Z- TNavBF::mmse selected from filter -1 2021-02-13T00:23:23Z- variances([0], [2], and psi_berg) = 705.221931 833.000763 0.000000 2021-02-13T00:23:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:23:23Z- LcmTrn::publishEstimates() - MLE : 4059038.50 592519.32 19.00 2021-02-13T00:23:23Z- LcmTrn::publishEstimates() - MMSE : 4059039.00 592469.82 19.00 2021-02-13T00:23:23Z- LcmTrn::publishEstimates() - COVAR: 705.22 833.00 0.83 0.00 2021-02-13T00:23:23Z- AHRS_M2 msg: 1613175803.01 epoch sec; seqNo:12096 2021-02-13T00:23:23Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.20 psi 2021-02-13T00:23:23Z- handleDepth()-Depth_Keller msg: 1613175803012 epoch sec; seqNo:12193; depth 18.91 2021-02-13T00:23:23Z- handleDvl() - RDI_Pathfinder msg: 1613175803.03 epoch sec; seqNo:6010 2021-02-13T00:23:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.02 , 97.99 , 102.85 , 72.75 2021-02-13T00:23:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.03 , -0.11 2021-02-13T00:23:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175803.04 epoch sec; seqNo:12193; 4059039.38 north; 592469.81 east 2021-02-13T00:23:23Z- AHRS_M2 msg: 1613175803.41 epoch sec; seqNo:12097 2021-02-13T00:23:23Z- AHRS_M2 msg: 0.65 phi; 0.29 theta; -0.20 psi 2021-02-13T00:23:23Z- handleDepth()-Depth_Keller msg: 1613175803416 epoch sec; seqNo:12194; depth 18.79 2021-02-13T00:23:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175803.43 epoch sec; seqNo:12194; 4059039.76 north; 592469.81 east 2021-02-13T00:23:23Z- AHRS_M2 msg: 1613175803.82 epoch sec; seqNo:12098 2021-02-13T00:23:23Z- AHRS_M2 msg: 0.65 phi; 0.28 theta; -0.20 psi 2021-02-13T00:23:23Z- handleDepth()-Depth_Keller msg: 1613175803822 epoch sec; seqNo:12195; depth 18.70 2021-02-13T00:23:23Z- handleDvl() - RDI_Pathfinder msg: 1613175803.83 epoch sec; seqNo:6011 2021-02-13T00:23:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 97.99 , 102.85 , 70.71 2021-02-13T00:23:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.06 , -0.12 2021-02-13T00:23:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175803.84 epoch sec; seqNo:12195; 4059039.76 north; 592469.81 east 2021-02-13T00:23:24Z- AHRS_M2 msg: 1613175804.22 epoch sec; seqNo:12099 2021-02-13T00:23:24Z- AHRS_M2 msg: 0.65 phi; 0.28 theta; -0.20 psi 2021-02-13T00:23:24Z- handleDepth()-Depth_Keller msg: 1613175804225 epoch sec; seqNo:12196; depth 18.59 2021-02-13T00:23:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175804.24 epoch sec; seqNo:12196; 4059040.14 north; 592469.81 east 2021-02-13T00:23:24Z- AHRS_M2 msg: 1613175804.62 epoch sec; seqNo:12100 2021-02-13T00:23:24Z- AHRS_M2 msg: 0.64 phi; 0.28 theta; -0.19 psi 2021-02-13T00:23:24Z- handleDepth()-Depth_Keller msg: 1613175804628 epoch sec; seqNo:12197; depth 18.49 2021-02-13T00:23:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175804.64 epoch sec; seqNo:12197; 4059040.52 north; 592469.80 east 2021-02-13T00:23:25Z- AHRS_M2 msg: 1613175805.03 epoch sec; seqNo:12101 2021-02-13T00:23:25Z- AHRS_M2 msg: 0.64 phi; 0.28 theta; -0.19 psi 2021-02-13T00:23:25Z- handleDepth()-Depth_Keller msg: 1613175805037 epoch sec; seqNo:12198; depth 18.38 2021-02-13T00:23:25Z- handleDvl() - RDI_Pathfinder msg: 1613175805.05 epoch sec; seqNo:6012 2021-02-13T00:23:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 98.05 , 102.92 , 70.71 2021-02-13T00:23:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.12 , -0.06 2021-02-13T00:23:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175805.06 epoch sec; seqNo:12198; 4059040.52 north; 592469.80 east 2021-02-13T00:23:25Z- AHRS_M2 msg: 1613175805.43 epoch sec; seqNo:12102 2021-02-13T00:23:25Z- AHRS_M2 msg: 0.63 phi; 0.28 theta; -0.19 psi 2021-02-13T00:23:25Z- handleDepth()-Depth_Keller msg: 1613175805436 epoch sec; seqNo:12199; depth 18.28 2021-02-13T00:23:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175805.45 epoch sec; seqNo:12199; 4059040.91 north; 592471.02 east 2021-02-13T00:23:25Z- AHRS_M2 msg: 1613175805.86 epoch sec; seqNo:12103 2021-02-13T00:23:25Z- AHRS_M2 msg: 0.63 phi; 0.28 theta; -0.19 psi 2021-02-13T00:23:25Z- handleDepth()-Depth_Keller msg: 1613175805871 epoch sec; seqNo:12200; depth 18.19 2021-02-13T00:23:25Z- handleDvl() - RDI_Pathfinder msg: 1613175805.89 epoch sec; seqNo:6013 2021-02-13T00:23:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 98.05 , 101.71 , 78.58 2021-02-13T00:23:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.11 , 0.01 2021-02-13T00:23:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175805.90 epoch sec; seqNo:12200; 4059041.29 north; 592471.01 east 2021-02-13T00:23:26Z- AHRS_M2 msg: 1613175806.24 epoch sec; seqNo:12104 2021-02-13T00:23:26Z- AHRS_M2 msg: 0.62 phi; 0.28 theta; -0.18 psi 2021-02-13T00:23:26Z- handleDepth()-Depth_Keller msg: 1613175806249 epoch sec; seqNo:12201; depth 18.08 2021-02-13T00:23:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175806.27 epoch sec; seqNo:12201; 4059041.67 north; 592471.01 east 2021-02-13T00:23:26Z- AHRS_M2 msg: 1613175806.64 epoch sec; seqNo:12105 2021-02-13T00:23:26Z- AHRS_M2 msg: 0.62 phi; 0.28 theta; -0.18 psi 2021-02-13T00:23:26Z- handleDepth()-Depth_Keller msg: 1613175806648 epoch sec; seqNo:12202; depth 17.98 2021-02-13T00:23:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175806.66 epoch sec; seqNo:12202; 4059041.67 north; 592471.01 east 2021-02-13T00:23:27Z- AHRS_M2 msg: 1613175807.05 epoch sec; seqNo:12106 2021-02-13T00:23:27Z- AHRS_M2 msg: 0.61 phi; 0.28 theta; -0.19 psi 2021-02-13T00:23:27Z- handleDepth()-Depth_Keller msg: 1613175807057 epoch sec; seqNo:12203; depth 17.89 2021-02-13T00:23:27Z- handleDvl() - RDI_Pathfinder msg: 1613175807.07 epoch sec; seqNo:6014 2021-02-13T00:23:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.85 , 101.50 , 78.38 2021-02-13T00:23:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.10 , 0.05 2021-02-13T00:23:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175807.08 epoch sec; seqNo:12203; 4059042.04 north; 592471.01 east 2021-02-13T00:23:27Z- AHRS_M2 msg: 1613175807.45 epoch sec; seqNo:12107 2021-02-13T00:23:27Z- AHRS_M2 msg: 0.61 phi; 0.28 theta; -0.19 psi 2021-02-13T00:23:27Z- handleDepth()-Depth_Keller msg: 1613175807458 epoch sec; seqNo:12204; depth 17.80 2021-02-13T00:23:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175807.47 epoch sec; seqNo:12204; 4059042.42 north; 592471.00 east 2021-02-13T00:23:27Z- AHRS_M2 msg: 1613175807.86 epoch sec; seqNo:12108 2021-02-13T00:23:27Z- AHRS_M2 msg: 0.61 phi; 0.28 theta; -0.19 psi 2021-02-13T00:23:27Z- handleDepth()-Depth_Keller msg: 1613175807870 epoch sec; seqNo:12205; depth 17.70 2021-02-13T00:23:27Z- handleDvl() - RDI_Pathfinder msg: 1613175807.89 epoch sec; seqNo:6015 2021-02-13T00:23:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 96.29 , 101.71 , 71.94 2021-02-13T00:23:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , 0.07 2021-02-13T00:23:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175807.90 epoch sec; seqNo:12205; 4059042.80 north; 592471.00 east 2021-02-13T00:23:28Z- LcmTrn::handleMessages() - handling 45 messages took 0.68 ms and 132 calls... 2021-02-13T00:23:28Z- waiting for 1613175805.63, time is 1613175807.90 2021-02-13T00:23:28Z- LcmTrn::updateTrn() equating pose 1613175807.90 and meas times 1613175807.90 2021-02-13T00:23:28Z- LcmTrn::updateTrn() >>>> pose time 1613175807.90, meas time 1613175807.90 <<<< 2021-02-13T00:23:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175807.90 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:23:28Z- TerrainNav::motionUpdate() - currEstimate = (4059043, 592471.00, 17.70) 2021-02-13T00:23:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:23:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:23:28Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175807.90 sec.; No relevant navigation data available 2021-02-13T00:23:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:23:28Z- TNavBF::mmse selected from filter -1 2021-02-13T00:23:28Z- variances([0], [2], and psi_berg) = 705.164816 833.000732 0.000000 2021-02-13T00:23:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:23:28Z- LcmTrn::publishEstimates() - MLE : 4059042.30 592520.50 17.70 2021-02-13T00:23:28Z- LcmTrn::publishEstimates() - MMSE : 4059042.80 592471.00 17.70 2021-02-13T00:23:28Z- LcmTrn::publishEstimates() - COVAR: 705.16 833.00 1.03 0.00 2021-02-13T00:23:28Z- AHRS_M2 msg: 1613175808.26 epoch sec; seqNo:12109 2021-02-13T00:23:28Z- AHRS_M2 msg: 0.60 phi; 0.28 theta; -0.20 psi 2021-02-13T00:23:28Z- handleDepth()-Depth_Keller msg: 1613175808264 epoch sec; seqNo:12206; depth 17.58 2021-02-13T00:23:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175808.28 epoch sec; seqNo:12206; 4059042.80 north; 592471.00 east 2021-02-13T00:23:28Z- AHRS_M2 msg: 1613175808.66 epoch sec; seqNo:12110 2021-02-13T00:23:28Z- AHRS_M2 msg: 0.60 phi; 0.28 theta; -0.19 psi 2021-02-13T00:23:28Z- handleDepth()-Depth_Keller msg: 1613175808678 epoch sec; seqNo:12207; depth 17.50 2021-02-13T00:23:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175808.69 epoch sec; seqNo:12207; 4059043.19 north; 592472.21 east 2021-02-13T00:23:29Z- AHRS_M2 msg: 1613175809.08 epoch sec; seqNo:12111 2021-02-13T00:23:29Z- AHRS_M2 msg: 0.60 phi; 0.28 theta; -0.19 psi 2021-02-13T00:23:29Z- handleDepth()-Depth_Keller msg: 1613175809080 epoch sec; seqNo:12208; depth 17.40 2021-02-13T00:23:29Z- handleDvl() - RDI_Pathfinder msg: 1613175809.09 epoch sec; seqNo:6016 2021-02-13T00:23:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 102.92 , 71.52 2021-02-13T00:23:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , 0.07 2021-02-13T00:23:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175809.10 epoch sec; seqNo:12208; 4059043.57 north; 592472.21 east 2021-02-13T00:23:29Z- AHRS_M2 msg: 1613175809.47 epoch sec; seqNo:12112 2021-02-13T00:23:29Z- AHRS_M2 msg: 0.59 phi; 0.28 theta; -0.19 psi 2021-02-13T00:23:29Z- handleDepth()-Depth_Keller msg: 1613175809477 epoch sec; seqNo:12209; depth 17.31 2021-02-13T00:23:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175809.49 epoch sec; seqNo:12209; 4059043.95 north; 592472.21 east 2021-02-13T00:23:29Z- AHRS_M2 msg: 1613175809.88 epoch sec; seqNo:12113 2021-02-13T00:23:29Z- AHRS_M2 msg: 0.59 phi; 0.28 theta; -0.18 psi 2021-02-13T00:23:29Z- handleDepth()-Depth_Keller msg: 1613175809889 epoch sec; seqNo:12210; depth 17.22 2021-02-13T00:23:29Z- handleDvl() - RDI_Pathfinder msg: 1613175809.91 epoch sec; seqNo:6017 2021-02-13T00:23:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.06 , 97.98 , 104.07 , 78.51 2021-02-13T00:23:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , 0.09 2021-02-13T00:23:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175809.92 epoch sec; seqNo:12210; 4059043.95 north; 592472.21 east 2021-02-13T00:23:30Z- AHRS_M2 msg: 1613175810.28 epoch sec; seqNo:12114 2021-02-13T00:23:30Z- AHRS_M2 msg: 0.59 phi; 0.28 theta; -0.18 psi 2021-02-13T00:23:30Z- handleDepth()-Depth_Keller msg: 1613175810293 epoch sec; seqNo:12211; depth 17.12 2021-02-13T00:23:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175810.30 epoch sec; seqNo:12211; 4059044.33 north; 592472.20 east 2021-02-13T00:23:30Z- AHRS_M2 msg: 1613175810.69 epoch sec; seqNo:12115 2021-02-13T00:23:30Z- AHRS_M2 msg: 0.60 phi; 0.28 theta; -0.18 psi 2021-02-13T00:23:30Z- handleDepth()-Depth_Keller msg: 1613175810691 epoch sec; seqNo:12212; depth 17.04 2021-02-13T00:23:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175810.71 epoch sec; seqNo:12212; 4059044.71 north; 592472.20 east 2021-02-13T00:23:31Z- AHRS_M2 msg: 1613175811.09 epoch sec; seqNo:12116 2021-02-13T00:23:31Z- AHRS_M2 msg: 0.60 phi; 0.28 theta; -0.17 psi 2021-02-13T00:23:31Z- handleDepth()-Depth_Keller msg: 1613175811097 epoch sec; seqNo:12213; depth 16.93 2021-02-13T00:23:31Z- handleDvl() - RDI_Pathfinder msg: 1613175811.12 epoch sec; seqNo:6018 2021-02-13T00:23:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.52 , 95.34 , 101.43 , 73.56 2021-02-13T00:23:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.07 , 0.06 2021-02-13T00:23:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175811.13 epoch sec; seqNo:12213; 4059045.10 north; 592473.41 east 2021-02-13T00:23:31Z- AHRS_M2 msg: 1613175811.49 epoch sec; seqNo:12117 2021-02-13T00:23:31Z- AHRS_M2 msg: 0.60 phi; 0.28 theta; -0.17 psi 2021-02-13T00:23:31Z- handleDepth()-Depth_Keller msg: 1613175811496 epoch sec; seqNo:12214; depth 16.85 2021-02-13T00:23:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175811.51 epoch sec; seqNo:12214; 4059045.10 north; 592473.41 east 2021-02-13T00:23:31Z- AHRS_M2 msg: 1613175811.90 epoch sec; seqNo:12118 2021-02-13T00:23:31Z- AHRS_M2 msg: 0.61 phi; 0.28 theta; -0.17 psi 2021-02-13T00:23:31Z- handleDepth()-Depth_Keller msg: 1613175811902 epoch sec; seqNo:12215; depth 16.74 2021-02-13T00:23:31Z- handleDvl() - RDI_Pathfinder msg: 1613175811.91 epoch sec; seqNo:6019 2021-02-13T00:23:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.73 , 97.98 , 102.85 , 0.00 2021-02-13T00:23:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.07 , -0.01 2021-02-13T00:23:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175811.92 epoch sec; seqNo:12215; 4059045.48 north; 592473.41 east 2021-02-13T00:23:32Z- AHRS_M2 msg: 1613175812.30 epoch sec; seqNo:12119 2021-02-13T00:23:32Z- AHRS_M2 msg: 0.61 phi; 0.28 theta; -0.17 psi 2021-02-13T00:23:32Z- handleDepth()-Depth_Keller msg: 1613175812309 epoch sec; seqNo:12216; depth 16.67 2021-02-13T00:23:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175812.33 epoch sec; seqNo:12216; 4059045.86 north; 592473.41 east 2021-02-13T00:23:32Z- AHRS_M2 msg: 1613175812.70 epoch sec; seqNo:12120 2021-02-13T00:23:32Z- AHRS_M2 msg: 0.62 phi; 0.28 theta; -0.17 psi 2021-02-13T00:23:32Z- handleDepth()-Depth_Keller msg: 1613175812708 epoch sec; seqNo:12217; depth 16.57 2021-02-13T00:23:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175812.72 epoch sec; seqNo:12217; 4059045.86 north; 592473.41 east 2021-02-13T00:23:33Z- LcmTrn::handleMessages() - handling 40 messages took 0.60 ms and 128 calls... 2021-02-13T00:23:33Z- waiting for 1613175810.90, time is 1613175812.72 2021-02-13T00:23:33Z- LcmTrn::updateTrn() >>>> pose time 1613175812.72, meas time 1613175811.91 <<<< 2021-02-13T00:23:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175811.91 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:23:33Z- TerrainNav::motionUpdate() - currEstimate = (4059046, 592473.41, 16.57) 2021-02-13T00:23:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:23:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:23:33Z- TNavBF::Projecting Measurements 2021-02-13T00:23:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:23:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:23:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:23:33Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:23:33Z- TNavFilter::3 good beams. 2021-02-13T00:23:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:23:33Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:23:33Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:23:33Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:23:33Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:23:33Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:23:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:23:33Z- TNavBF::mmse selected from filter -1 2021-02-13T00:23:33Z- TerrainMapOctree: (vehN, vehE ) = (4059045.35, 592473.01). 2021-02-13T00:23:33Z- TerrainMapOctree: Min Distance = 2831.64. 2021-02-13T00:23:33Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:23:33Z- There is no specific good beam that all particles have in common. 2021-02-13T00:23:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:23:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 10.17 2021-02-13T00:23:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:23:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:23:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:23:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 16.92 2021-02-13T00:23:33Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:23:33Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:23:33Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:23:33Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:23:33Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:23:33Z- Filter 0 No beams used in subcloud update 2021-02-13T00:23:33Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:23:33Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:23:33Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 705.1 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:23:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175811.91 sec. 2021-02-13T00:23:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:23:33Z- TNavBF::mmse selected from filter -1 2021-02-13T00:23:33Z- variances([0], [2], and psi_berg) = 705.090272 833.000690 0.000000 2021-02-13T00:23:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:23:33Z- LcmTrn::publishEstimates() - MLE : 4059045.36 592522.91 16.57 2021-02-13T00:23:33Z- LcmTrn::publishEstimates() - MMSE : 4059045.86 592473.41 16.57 2021-02-13T00:23:33Z- LcmTrn::publishEstimates() - COVAR: 705.09 833.00 1.00 0.00 2021-02-13T00:23:33Z- AHRS_M2 msg: 1613175813.11 epoch sec; seqNo:12121 2021-02-13T00:23:33Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.17 psi 2021-02-13T00:23:33Z- handleDepth()-Depth_Keller msg: 1613175813117 epoch sec; seqNo:12218; depth 16.49 2021-02-13T00:23:33Z- handleDvl() - RDI_Pathfinder msg: 1613175813.13 epoch sec; seqNo:6020 2021-02-13T00:23:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.00 , 96.23 , 104.34 , 78.65 2021-02-13T00:23:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , -0.07 2021-02-13T00:23:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175813.16 epoch sec; seqNo:12218; 4059046.25 north; 592474.62 east 2021-02-13T00:23:33Z- AHRS_M2 msg: 1613175813.51 epoch sec; seqNo:12122 2021-02-13T00:23:33Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.17 psi 2021-02-13T00:23:33Z- handleDepth()-Depth_Keller msg: 1613175813521 epoch sec; seqNo:12219; depth 16.38 2021-02-13T00:23:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175813.54 epoch sec; seqNo:12219; 4059046.63 north; 592474.62 east 2021-02-13T00:23:33Z- AHRS_M2 msg: 1613175813.94 epoch sec; seqNo:12123 2021-02-13T00:23:33Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.17 psi 2021-02-13T00:23:33Z- handleDepth()-Depth_Keller msg: 1613175813947 epoch sec; seqNo:12220; depth 16.31 2021-02-13T00:23:33Z- handleDvl() - RDI_Pathfinder msg: 1613175813.96 epoch sec; seqNo:6021 2021-02-13T00:23:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.53 , 96.57 , 105.09 , 78.31 2021-02-13T00:23:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.13 , -0.07 2021-02-13T00:23:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175813.97 epoch sec; seqNo:12220; 4059047.01 north; 592474.61 east 2021-02-13T00:23:34Z- AHRS_M2 msg: 1613175814.32 epoch sec; seqNo:12124 2021-02-13T00:23:34Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.17 psi 2021-02-13T00:23:34Z- handleDepth()-Depth_Keller msg: 1613175814324 epoch sec; seqNo:12221; depth 16.21 2021-02-13T00:23:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175814.34 epoch sec; seqNo:12221; 4059047.01 north; 592474.61 east 2021-02-13T00:23:34Z- AHRS_M2 msg: 1613175814.73 epoch sec; seqNo:12125 2021-02-13T00:23:34Z- AHRS_M2 msg: 0.61 phi; 0.29 theta; -0.16 psi 2021-02-13T00:23:34Z- handleDepth()-Depth_Keller msg: 1613175814737 epoch sec; seqNo:12222; depth 16.13 2021-02-13T00:23:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175814.75 epoch sec; seqNo:12222; 4059047.39 north; 592474.61 east 2021-02-13T00:23:35Z- AHRS_M2 msg: 1613175815.13 epoch sec; seqNo:12126 2021-02-13T00:23:35Z- AHRS_M2 msg: 0.61 phi; 0.29 theta; -0.16 psi 2021-02-13T00:23:35Z- handleDepth()-Depth_Keller msg: 1613175815138 epoch sec; seqNo:12223; depth 16.04 2021-02-13T00:23:35Z- handleDvl() - RDI_Pathfinder msg: 1613175815.15 epoch sec; seqNo:6022 2021-02-13T00:23:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 97.78 , 105.08 , 78.31 2021-02-13T00:23:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.14 , -0.03 2021-02-13T00:23:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175815.16 epoch sec; seqNo:12223; 4059047.77 north; 592474.61 east 2021-02-13T00:23:35Z- AHRS_M2 msg: 1613175815.53 epoch sec; seqNo:12127 2021-02-13T00:23:35Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.15 psi 2021-02-13T00:23:35Z- handleDepth()-Depth_Keller msg: 1613175815536 epoch sec; seqNo:12224; depth 15.94 2021-02-13T00:23:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175815.56 epoch sec; seqNo:12224; 4059048.15 north; 592474.60 east 2021-02-13T00:23:35Z- AHRS_M2 msg: 1613175815.93 epoch sec; seqNo:12128 2021-02-13T00:23:35Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.14 psi 2021-02-13T00:23:35Z- handleDepth()-Depth_Keller msg: 1613175815945 epoch sec; seqNo:12225; depth 15.85 2021-02-13T00:23:35Z- handleDvl() - RDI_Pathfinder msg: 1613175815.96 epoch sec; seqNo:6023 2021-02-13T00:23:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 97.71 , 105.01 , 73.15 2021-02-13T00:23:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , 0.01 2021-02-13T00:23:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175815.98 epoch sec; seqNo:12225; 4059048.52 north; 592474.60 east 2021-02-13T00:23:36Z- AHRS_M2 msg: 1613175816.34 epoch sec; seqNo:12129 2021-02-13T00:23:36Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.13 psi 2021-02-13T00:23:36Z- handleDepth()-Depth_Keller msg: 1613175816345 epoch sec; seqNo:12226; depth 15.76 2021-02-13T00:23:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175816.36 epoch sec; seqNo:12226; 4059048.92 north; 592475.81 east 2021-02-13T00:23:36Z- AHRS_M2 msg: 1613175816.74 epoch sec; seqNo:12130 2021-02-13T00:23:36Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.12 psi 2021-02-13T00:23:36Z- handleDepth()-Depth_Keller msg: 1613175816753 epoch sec; seqNo:12227; depth 15.67 2021-02-13T00:23:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175816.77 epoch sec; seqNo:12227; 4059048.92 north; 592475.81 east 2021-02-13T00:23:37Z- AHRS_M2 msg: 1613175817.15 epoch sec; seqNo:12131 2021-02-13T00:23:37Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.11 psi 2021-02-13T00:23:37Z- handleDepth()-Depth_Keller msg: 1613175817161 epoch sec; seqNo:12228; depth 15.56 2021-02-13T00:23:37Z- handleDvl() - RDI_Pathfinder msg: 1613175817.17 epoch sec; seqNo:6024 2021-02-13T00:23:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.18 , 94.13 , 106.30 , 74.38 2021-02-13T00:23:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.01 2021-02-13T00:23:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175817.19 epoch sec; seqNo:12228; 4059049.30 north; 592475.81 east 2021-02-13T00:23:37Z- AHRS_M2 msg: 1613175817.55 epoch sec; seqNo:12132 2021-02-13T00:23:37Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.10 psi 2021-02-13T00:23:37Z- handleDepth()-Depth_Keller msg: 1613175817557 epoch sec; seqNo:12229; depth 15.45 2021-02-13T00:23:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175817.57 epoch sec; seqNo:12229; 4059049.67 north; 592475.81 east 2021-02-13T00:23:37Z- AHRS_M2 msg: 1613175817.96 epoch sec; seqNo:12133 2021-02-13T00:23:37Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.09 psi 2021-02-13T00:23:37Z- handleDepth()-Depth_Keller msg: 1613175817973 epoch sec; seqNo:12230; depth 15.35 2021-02-13T00:23:37Z- handleDvl() - RDI_Pathfinder msg: 1613175817.98 epoch sec; seqNo:6025 2021-02-13T00:23:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.39 , 97.78 , 106.30 , 72.34 2021-02-13T00:23:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , -0.01 2021-02-13T00:23:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175818.00 epoch sec; seqNo:12230; 4059049.67 north; 592475.81 east 2021-02-13T00:23:38Z- LcmTrn::handleMessages() - handling 45 messages took 0.66 ms and 132 calls... 2021-02-13T00:23:38Z- waiting for 1613175815.72, time is 1613175818.00 2021-02-13T00:23:38Z- LcmTrn::updateTrn() equating pose 1613175818.00 and meas times 1613175818.00 2021-02-13T00:23:38Z- LcmTrn::updateTrn() >>>> pose time 1613175818.00, meas time 1613175818.00 <<<< 2021-02-13T00:23:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175818.00 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:23:38Z- TerrainNav::motionUpdate() - currEstimate = (4059050, 592475.81, 15.35) 2021-02-13T00:23:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:23:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:23:38Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175818.00 sec.; No relevant navigation data available 2021-02-13T00:23:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:23:38Z- TNavBF::mmse selected from filter -1 2021-02-13T00:23:38Z- variances([0], [2], and psi_berg) = 705.029582 833.000657 0.000000 2021-02-13T00:23:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:23:38Z- LcmTrn::publishEstimates() - MLE : 4059049.17 592525.31 15.35 2021-02-13T00:23:38Z- LcmTrn::publishEstimates() - MMSE : 4059049.67 592475.81 15.35 2021-02-13T00:23:38Z- LcmTrn::publishEstimates() - COVAR: 705.03 833.00 1.23 0.00 2021-02-13T00:23:38Z- AHRS_M2 msg: 1613175818.36 epoch sec; seqNo:12134 2021-02-13T00:23:38Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.09 psi 2021-02-13T00:23:38Z- handleDepth()-Depth_Keller msg: 1613175818370 epoch sec; seqNo:12231; depth 15.26 2021-02-13T00:23:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175818.39 epoch sec; seqNo:12231; 4059050.05 north; 592475.80 east 2021-02-13T00:23:38Z- AHRS_M2 msg: 1613175818.77 epoch sec; seqNo:12135 2021-02-13T00:23:38Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.10 psi 2021-02-13T00:23:38Z- handleDepth()-Depth_Keller msg: 1613175818774 epoch sec; seqNo:12232; depth 15.15 2021-02-13T00:23:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175818.78 epoch sec; seqNo:12232; 4059050.45 north; 592477.02 east 2021-02-13T00:23:39Z- AHRS_M2 msg: 1613175819.17 epoch sec; seqNo:12136 2021-02-13T00:23:39Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.11 psi 2021-02-13T00:23:39Z- handleDepth()-Depth_Keller msg: 1613175819177 epoch sec; seqNo:12233; depth 15.05 2021-02-13T00:23:39Z- handleDvl() - RDI_Pathfinder msg: 1613175819.19 epoch sec; seqNo:6026 2021-02-13T00:23:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 92.84 , 107.45 , 78.24 2021-02-13T00:23:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.02 , -0.05 2021-02-13T00:23:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175819.20 epoch sec; seqNo:12233; 4059050.45 north; 592477.02 east 2021-02-13T00:23:39Z- AHRS_M2 msg: 1613175819.57 epoch sec; seqNo:12137 2021-02-13T00:23:39Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.11 psi 2021-02-13T00:23:39Z- handleDepth()-Depth_Keller msg: 1613175819578 epoch sec; seqNo:12234; depth 14.94 2021-02-13T00:23:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175819.59 epoch sec; seqNo:12234; 4059050.82 north; 592477.02 east 2021-02-13T00:23:39Z- AHRS_M2 msg: 1613175819.98 epoch sec; seqNo:12138 2021-02-13T00:23:39Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.12 psi 2021-02-13T00:23:39Z- handleDepth()-Depth_Keller msg: 1613175819980 epoch sec; seqNo:12235; depth 14.82 2021-02-13T00:23:40Z- handleDvl() - RDI_Pathfinder msg: 1613175820.00 epoch sec; seqNo:6027 2021-02-13T00:23:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 97.64 , 108.46 , 74.79 2021-02-13T00:23:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.03 , -0.08 2021-02-13T00:23:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175820.01 epoch sec; seqNo:12235; 4059051.20 north; 592477.01 east 2021-02-13T00:23:40Z- AHRS_M2 msg: 1613175820.38 epoch sec; seqNo:12139 2021-02-13T00:23:40Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.13 psi 2021-02-13T00:23:40Z- handleDepth()-Depth_Keller msg: 1613175820384 epoch sec; seqNo:12236; depth 14.71 2021-02-13T00:23:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175820.39 epoch sec; seqNo:12236; 4059051.20 north; 592477.01 east 2021-02-13T00:23:40Z- AHRS_M2 msg: 1613175820.78 epoch sec; seqNo:12140 2021-02-13T00:23:40Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.14 psi 2021-02-13T00:23:40Z- handleDepth()-Depth_Keller msg: 1613175820797 epoch sec; seqNo:12237; depth 14.60 2021-02-13T00:23:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175820.81 epoch sec; seqNo:12237; 4059051.58 north; 592477.01 east 2021-02-13T00:23:41Z- AHRS_M2 msg: 1613175821.19 epoch sec; seqNo:12141 2021-02-13T00:23:41Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.15 psi 2021-02-13T00:23:41Z- handleDepth()-Depth_Keller msg: 1613175821193 epoch sec; seqNo:12238; depth 14.50 2021-02-13T00:23:41Z- handleDvl() - RDI_Pathfinder msg: 1613175821.21 epoch sec; seqNo:6028 2021-02-13T00:23:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:23:41Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:23:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175821.22 epoch sec; seqNo:12238; 4059051.96 north; 592477.00 east 2021-02-13T00:23:41Z- AHRS_M2 msg: 1613175821.59 epoch sec; seqNo:12142 2021-02-13T00:23:41Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.16 psi 2021-02-13T00:23:41Z- handleDepth()-Depth_Keller msg: 1613175821601 epoch sec; seqNo:12239; depth 14.38 2021-02-13T00:23:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175821.61 epoch sec; seqNo:12239; 4059051.97 north; 592478.22 east 2021-02-13T00:23:42Z- AHRS_M2 msg: 1613175822.02 epoch sec; seqNo:12143 2021-02-13T00:23:42Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.18 psi 2021-02-13T00:23:42Z- handleDepth()-Depth_Keller msg: 1613175822023 epoch sec; seqNo:12240; depth 14.27 2021-02-13T00:23:42Z- handleDvl() - RDI_Pathfinder msg: 1613175822.04 epoch sec; seqNo:6029 2021-02-13T00:23:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:23:42Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:23:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175822.06 epoch sec; seqNo:12240; 4059052.35 north; 592478.22 east 2021-02-13T00:23:42Z- AHRS_M2 msg: 1613175822.40 epoch sec; seqNo:12144 2021-02-13T00:23:42Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.19 psi 2021-02-13T00:23:42Z- handleDepth()-Depth_Keller msg: 1613175822404 epoch sec; seqNo:12241; depth 14.16 2021-02-13T00:23:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175822.42 epoch sec; seqNo:12241; 4059052.35 north; 592478.22 east 2021-02-13T00:23:42Z- AHRS_M2 msg: 1613175822.80 epoch sec; seqNo:12145 2021-02-13T00:23:42Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.20 psi 2021-02-13T00:23:42Z- handleDepth()-Depth_Keller msg: 1613175822808 epoch sec; seqNo:12242; depth 14.05 2021-02-13T00:23:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175822.82 epoch sec; seqNo:12242; 4059052.73 north; 592478.22 east 2021-02-13T00:23:43Z- LcmTrn::handleMessages() - handling 40 messages took 0.60 ms and 128 calls... 2021-02-13T00:23:43Z- waiting for 1613175821.00, time is 1613175822.82 2021-02-13T00:23:43Z- LcmTrn::updateTrn() >>>> pose time 1613175822.82, meas time 1613175822.04 <<<< 2021-02-13T00:23:43Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175822.04, ping # 0. 2021-02-13T00:23:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175822.04 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:23:43Z- TerrainNav::motionUpdate() - currEstimate = (4059053, 592478.22, 14.05) 2021-02-13T00:23:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:23:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:23:43Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:23:43Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:23:43Z- TNavBF::Projecting Measurements 2021-02-13T00:23:43Z- TNavFilter::Measurement from time = 1613175822.04 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:23:43Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:23:43Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:23:43Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:23:43Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:23:43Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:23:43Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:23:43Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:23:43Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 705.0 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T00:23:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:23:43Z- TNavBF::mmse selected from filter -1 2021-02-13T00:23:43Z- variances([0], [2], and psi_berg) = 704.955316 833.000615 0.000000 2021-02-13T00:23:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:23:43Z- LcmTrn::publishEstimates() - MLE : 4059052.23 592527.72 14.05 2021-02-13T00:23:43Z- LcmTrn::publishEstimates() - MMSE : 4059052.73 592478.22 14.05 2021-02-13T00:23:43Z- LcmTrn::publishEstimates() - COVAR: 704.96 833.00 1.43 0.00 2021-02-13T00:23:43Z- AHRS_M2 msg: 1613175823.21 epoch sec; seqNo:12146 2021-02-13T00:23:43Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.20 psi 2021-02-13T00:23:43Z- handleDepth()-Depth_Keller msg: 1613175823211 epoch sec; seqNo:12243; depth 13.95 2021-02-13T00:23:43Z- handleDvl() - RDI_Pathfinder msg: 1613175823.22 epoch sec; seqNo:6030 2021-02-13T00:23:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.85 , 100.21 , 112.11 , 73.05 2021-02-13T00:23:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , 0.12 2021-02-13T00:23:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175823.23 epoch sec; seqNo:12243; 4059053.11 north; 592478.21 east 2021-02-13T00:23:43Z- AHRS_M2 msg: 1613175823.61 epoch sec; seqNo:12147 2021-02-13T00:23:43Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-13T00:23:43Z- handleDepth()-Depth_Keller msg: 1613175823618 epoch sec; seqNo:12244; depth 13.84 2021-02-13T00:23:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175823.63 epoch sec; seqNo:12244; 4059053.49 north; 592478.21 east 2021-02-13T00:23:44Z- AHRS_M2 msg: 1613175824.02 epoch sec; seqNo:12148 2021-02-13T00:23:44Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.18 psi 2021-02-13T00:23:44Z- handleDepth()-Depth_Keller msg: 1613175824020 epoch sec; seqNo:12245; depth 13.74 2021-02-13T00:23:44Z- handleDvl() - RDI_Pathfinder msg: 1613175824.04 epoch sec; seqNo:6031 2021-02-13T00:23:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.42 , 113.94 , 78.78 2021-02-13T00:23:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , 0.09 2021-02-13T00:23:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175824.05 epoch sec; seqNo:12245; 4059053.87 north; 592478.20 east 2021-02-13T00:23:44Z- AHRS_M2 msg: 1613175824.42 epoch sec; seqNo:12149 2021-02-13T00:23:44Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.18 psi 2021-02-13T00:23:44Z- handleDepth()-Depth_Keller msg: 1613175824426 epoch sec; seqNo:12246; depth 13.62 2021-02-13T00:23:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175824.44 epoch sec; seqNo:12246; 4059053.88 north; 592479.42 east 2021-02-13T00:23:44Z- AHRS_M2 msg: 1613175824.82 epoch sec; seqNo:12150 2021-02-13T00:23:44Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-13T00:23:44Z- handleDepth()-Depth_Keller msg: 1613175824833 epoch sec; seqNo:12247; depth 13.50 2021-02-13T00:23:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175824.84 epoch sec; seqNo:12247; 4059054.26 north; 592479.42 east 2021-02-13T00:23:45Z- AHRS_M2 msg: 1613175825.23 epoch sec; seqNo:12151 2021-02-13T00:23:45Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-13T00:23:45Z- handleDepth()-Depth_Keller msg: 1613175825234 epoch sec; seqNo:12248; depth 13.41 2021-02-13T00:23:45Z- handleDvl() - RDI_Pathfinder msg: 1613175825.25 epoch sec; seqNo:6032 2021-02-13T00:23:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.71 , 101.29 , 113.46 , 74.38 2021-02-13T00:23:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , 0.01 2021-02-13T00:23:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175825.26 epoch sec; seqNo:12248; 4059054.64 north; 592479.42 east 2021-02-13T00:23:45Z- AHRS_M2 msg: 1613175825.63 epoch sec; seqNo:12152 2021-02-13T00:23:45Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T00:23:45Z- handleDepth()-Depth_Keller msg: 1613175825636 epoch sec; seqNo:12249; depth 13.29 2021-02-13T00:23:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175825.66 epoch sec; seqNo:12249; 4059054.64 north; 592479.42 east 2021-02-13T00:23:46Z- AHRS_M2 msg: 1613175826.04 epoch sec; seqNo:12153 2021-02-13T00:23:46Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-13T00:23:46Z- handleDepth()-Depth_Keller msg: 1613175826046 epoch sec; seqNo:12250; depth 13.16 2021-02-13T00:23:46Z- handleDvl() - RDI_Pathfinder msg: 1613175826.06 epoch sec; seqNo:6033 2021-02-13T00:23:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 101.70 , 115.22 , 78.71 2021-02-13T00:23:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , -0.04 2021-02-13T00:23:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175826.08 epoch sec; seqNo:12250; 4059055.02 north; 592479.41 east 2021-02-13T00:23:46Z- AHRS_M2 msg: 1613175826.45 epoch sec; seqNo:12154 2021-02-13T00:23:46Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.18 psi 2021-02-13T00:23:46Z- handleDepth()-Depth_Keller msg: 1613175826450 epoch sec; seqNo:12251; depth 13.05 2021-02-13T00:23:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175826.46 epoch sec; seqNo:12251; 4059055.41 north; 592480.63 east 2021-02-13T00:23:46Z- AHRS_M2 msg: 1613175826.84 epoch sec; seqNo:12155 2021-02-13T00:23:46Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.18 psi 2021-02-13T00:23:46Z- handleDepth()-Depth_Keller msg: 1613175826853 epoch sec; seqNo:12252; depth 12.93 2021-02-13T00:23:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175826.86 epoch sec; seqNo:12252; 4059055.79 north; 592480.62 east 2021-02-13T00:23:47Z- AHRS_M2 msg: 1613175827.25 epoch sec; seqNo:12156 2021-02-13T00:23:47Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.19 psi 2021-02-13T00:23:47Z- handleDepth()-Depth_Keller msg: 1613175827253 epoch sec; seqNo:12253; depth 12.81 2021-02-13T00:23:47Z- handleDvl() - RDI_Pathfinder msg: 1613175827.27 epoch sec; seqNo:6034 2021-02-13T00:23:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.27 , 102.85 , 116.37 , 78.51 2021-02-13T00:23:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.02 , -0.05 2021-02-13T00:23:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175827.28 epoch sec; seqNo:12253; 4059055.79 north; 592480.62 east 2021-02-13T00:23:47Z- AHRS_M2 msg: 1613175827.65 epoch sec; seqNo:12157 2021-02-13T00:23:47Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.18 psi 2021-02-13T00:23:47Z- handleDepth()-Depth_Keller msg: 1613175827658 epoch sec; seqNo:12254; depth 12.70 2021-02-13T00:23:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175827.67 epoch sec; seqNo:12254; 4059056.17 north; 592480.62 east 2021-02-13T00:23:48Z- AHRS_M2 msg: 1613175828.06 epoch sec; seqNo:12158 2021-02-13T00:23:48Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.18 psi 2021-02-13T00:23:48Z- handleDepth()-Depth_Keller msg: 1613175828065 epoch sec; seqNo:12255; depth 12.58 2021-02-13T00:23:48Z- handleDvl() - RDI_Pathfinder msg: 1613175828.09 epoch sec; seqNo:6035 2021-02-13T00:23:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.20 , 105.49 , 119.01 , 74.38 2021-02-13T00:23:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , -0.07 2021-02-13T00:23:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175828.10 epoch sec; seqNo:12255; 4059056.55 north; 592480.62 east 2021-02-13T00:23:48Z- LcmTrn::handleMessages() - handling 45 messages took 0.71 ms and 131 calls... 2021-02-13T00:23:48Z- waiting for 1613175825.82, time is 1613175828.10 2021-02-13T00:23:48Z- LcmTrn::updateTrn() equating pose 1613175828.10 and meas times 1613175828.10 2021-02-13T00:23:48Z- LcmTrn::updateTrn() >>>> pose time 1613175828.10, meas time 1613175828.10 <<<< 2021-02-13T00:23:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175828.10 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:23:48Z- TerrainNav::motionUpdate() - currEstimate = (4059057, 592480.62, 12.58) 2021-02-13T00:23:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:23:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:23:48Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175828.10 sec.; No relevant navigation data available 2021-02-13T00:23:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:23:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:23:48Z- variances([0], [2], and psi_berg) = 704.894714 833.000582 0.000000 2021-02-13T00:23:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:23:48Z- LcmTrn::publishEstimates() - MLE : 4059056.05 592530.12 12.58 2021-02-13T00:23:48Z- LcmTrn::publishEstimates() - MMSE : 4059056.55 592480.62 12.58 2021-02-13T00:23:48Z- LcmTrn::publishEstimates() - COVAR: 704.89 833.00 1.66 0.00 2021-02-13T00:23:48Z- AHRS_M2 msg: 1613175828.46 epoch sec; seqNo:12159 2021-02-13T00:23:48Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-13T00:23:48Z- handleDepth()-Depth_Keller msg: 1613175828464 epoch sec; seqNo:12256; depth 12.45 2021-02-13T00:23:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175828.47 epoch sec; seqNo:12256; 4059056.93 north; 592480.61 east 2021-02-13T00:23:48Z- AHRS_M2 msg: 1613175828.86 epoch sec; seqNo:12160 2021-02-13T00:23:48Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-13T00:23:48Z- handleDepth()-Depth_Keller msg: 1613175828873 epoch sec; seqNo:12257; depth 12.34 2021-02-13T00:23:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175828.88 epoch sec; seqNo:12257; 4059056.93 north; 592480.61 east 2021-02-13T00:23:49Z- AHRS_M2 msg: 1613175829.27 epoch sec; seqNo:12161 2021-02-13T00:23:49Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T00:23:49Z- handleDepth()-Depth_Keller msg: 1613175829273 epoch sec; seqNo:12258; depth 12.22 2021-02-13T00:23:49Z- handleDvl() - RDI_Pathfinder msg: 1613175829.29 epoch sec; seqNo:6036 2021-02-13T00:23:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 143.35 , 104.13 , 117.65 , 78.44 2021-02-13T00:23:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.20 , -0.10 2021-02-13T00:23:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175829.30 epoch sec; seqNo:12258; 4059057.32 north; 592481.83 east 2021-02-13T00:23:49Z- AHRS_M2 msg: 1613175829.67 epoch sec; seqNo:12162 2021-02-13T00:23:49Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-13T00:23:49Z- handleDepth()-Depth_Keller msg: 1613175829676 epoch sec; seqNo:12259; depth 12.11 2021-02-13T00:23:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175829.69 epoch sec; seqNo:12259; 4059057.70 north; 592481.82 east 2021-02-13T00:23:50Z- AHRS_M2 msg: 1613175830.08 epoch sec; seqNo:12163 2021-02-13T00:23:50Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-13T00:23:50Z- handleDepth()-Depth_Keller msg: 1613175830080 epoch sec; seqNo:12260; depth 11.97 2021-02-13T00:23:50Z- handleDvl() - RDI_Pathfinder msg: 1613175830.10 epoch sec; seqNo:6037 2021-02-13T00:23:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.82 , 101.70 , 115.08 , 77.90 2021-02-13T00:23:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.11 , -0.01 2021-02-13T00:23:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175830.11 epoch sec; seqNo:12260; 4059058.07 north; 592481.82 east 2021-02-13T00:23:50Z- AHRS_M2 msg: 1613175830.48 epoch sec; seqNo:12164 2021-02-13T00:23:50Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-13T00:23:50Z- handleDepth()-Depth_Keller msg: 1613175830493 epoch sec; seqNo:12261; depth 11.87 2021-02-13T00:23:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175830.50 epoch sec; seqNo:12261; 4059058.45 north; 592481.82 east 2021-02-13T00:23:50Z- AHRS_M2 msg: 1613175830.89 epoch sec; seqNo:12165 2021-02-13T00:23:50Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-13T00:23:50Z- handleDepth()-Depth_Keller msg: 1613175830903 epoch sec; seqNo:12262; depth 11.74 2021-02-13T00:23:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175830.91 epoch sec; seqNo:12262; 4059058.45 north; 592481.82 east 2021-02-13T00:23:51Z- AHRS_M2 msg: 1613175831.29 epoch sec; seqNo:12166 2021-02-13T00:23:51Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-13T00:23:51Z- handleDepth()-Depth_Keller msg: 1613175831293 epoch sec; seqNo:12263; depth 11.61 2021-02-13T00:23:51Z- handleDvl() - RDI_Pathfinder msg: 1613175831.30 epoch sec; seqNo:6038 2021-02-13T00:23:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.62 , 101.50 , 117.72 , 78.51 2021-02-13T00:23:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.07 , 0.07 2021-02-13T00:23:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175831.33 epoch sec; seqNo:12263; 4059058.83 north; 592481.81 east 2021-02-13T00:23:51Z- AHRS_M2 msg: 1613175831.69 epoch sec; seqNo:12167 2021-02-13T00:23:51Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-13T00:23:51Z- handleDepth()-Depth_Keller msg: 1613175831701 epoch sec; seqNo:12264; depth 11.50 2021-02-13T00:23:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175831.71 epoch sec; seqNo:12264; 4059059.21 north; 592481.81 east 2021-02-13T00:23:52Z- AHRS_M2 msg: 1613175832.10 epoch sec; seqNo:12168 2021-02-13T00:23:52Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-13T00:23:52Z- handleDepth()-Depth_Keller msg: 1613175832101 epoch sec; seqNo:12265; depth 11.38 2021-02-13T00:23:52Z- handleDvl() - RDI_Pathfinder msg: 1613175832.11 epoch sec; seqNo:6039 2021-02-13T00:23:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 101.43 , 119.01 , 78.44 2021-02-13T00:23:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.09 2021-02-13T00:23:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175832.13 epoch sec; seqNo:12265; 4059059.21 north; 592481.81 east 2021-02-13T00:23:52Z- AHRS_M2 msg: 1613175832.50 epoch sec; seqNo:12169 2021-02-13T00:23:52Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.15 psi 2021-02-13T00:23:52Z- handleDepth()-Depth_Keller msg: 1613175832504 epoch sec; seqNo:12266; depth 11.24 2021-02-13T00:23:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175832.52 epoch sec; seqNo:12266; 4059059.59 north; 592481.80 east 2021-02-13T00:23:52Z- AHRS_M2 msg: 1613175832.90 epoch sec; seqNo:12170 2021-02-13T00:23:52Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.15 psi 2021-02-13T00:23:52Z- handleDepth()-Depth_Keller msg: 1613175832913 epoch sec; seqNo:12267; depth 11.12 2021-02-13T00:23:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175832.92 epoch sec; seqNo:12267; 4059059.98 north; 592483.02 east 2021-02-13T00:23:53Z- LcmTrn::handleMessages() - handling 40 messages took 0.62 ms and 128 calls... 2021-02-13T00:23:53Z- waiting for 1613175831.10, time is 1613175832.92 2021-02-13T00:23:53Z- LcmTrn::updateTrn() >>>> pose time 1613175832.92, meas time 1613175832.11 <<<< 2021-02-13T00:23:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175832.11 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:23:53Z- TerrainNav::motionUpdate() - currEstimate = (4059060, 592483.02, 11.12) 2021-02-13T00:23:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:23:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:23:53Z- TNavBF::Projecting Measurements 2021-02-13T00:23:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:23:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:23:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:23:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:23:53Z- TNavFilter::4 good beams. 2021-02-13T00:23:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:23:53Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:23:53Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:23:53Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:23:53Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:23:53Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:23:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:23:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:23:53Z- TerrainMapOctree: (vehN, vehE ) = (4059059.41, 592482.62). 2021-02-13T00:23:53Z- TerrainMapOctree: Min Distance = 2814.67. 2021-02-13T00:23:53Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:23:53Z- There is no specific good beam that all particles have in common. 2021-02-13T00:23:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:23:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.84 K: 0.118 ExpectedMeasDiff: nan currMeas.covariance: 11.99 2021-02-13T00:23:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:23:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.62 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 16.46 2021-02-13T00:23:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:23:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.49 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 22.66 2021-02-13T00:23:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:23:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.108 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T00:23:53Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:23:53Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:23:53Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:23:53Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:23:53Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:23:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:23:53Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:23:53Z- Filter 0 No beams used in subcloud update 2021-02-13T00:23:53Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:23:53Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:23:53Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 704.8 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:23:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175832.11 sec. 2021-02-13T00:23:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:23:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:23:53Z- variances([0], [2], and psi_berg) = 704.817515 833.000539 0.000000 2021-02-13T00:23:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:23:53Z- LcmTrn::publishEstimates() - MLE : 4059059.48 592532.52 11.12 2021-02-13T00:23:53Z- LcmTrn::publishEstimates() - MMSE : 4059059.98 592483.02 11.12 2021-02-13T00:23:53Z- LcmTrn::publishEstimates() - COVAR: 704.82 833.00 1.29 0.00 2021-02-13T00:23:53Z- AHRS_M2 msg: 1613175833.31 epoch sec; seqNo:12171 2021-02-13T00:23:53Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.15 psi 2021-02-13T00:23:53Z- handleDepth()-Depth_Keller msg: 1613175833317 epoch sec; seqNo:12268; depth 11.01 2021-02-13T00:23:53Z- handleDvl() - RDI_Pathfinder msg: 1613175833.33 epoch sec; seqNo:6040 2021-02-13T00:23:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.91 , 102.78 , 113.60 , 79.79 2021-02-13T00:23:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.08 2021-02-13T00:23:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175833.34 epoch sec; seqNo:12268; 4059060.36 north; 592483.02 east 2021-02-13T00:23:53Z- AHRS_M2 msg: 1613175833.71 epoch sec; seqNo:12172 2021-02-13T00:23:53Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.15 psi 2021-02-13T00:23:53Z- handleDepth()-Depth_Keller msg: 1613175833717 epoch sec; seqNo:12269; depth 10.88 2021-02-13T00:23:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175833.73 epoch sec; seqNo:12269; 4059060.36 north; 592483.02 east 2021-02-13T00:23:54Z- AHRS_M2 msg: 1613175834.12 epoch sec; seqNo:12173 2021-02-13T00:23:54Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.15 psi 2021-02-13T00:23:54Z- handleDepth()-Depth_Keller msg: 1613175834133 epoch sec; seqNo:12270; depth 10.75 2021-02-13T00:23:54Z- handleDvl() - RDI_Pathfinder msg: 1613175834.15 epoch sec; seqNo:6041 2021-02-13T00:23:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.91 , 102.78 , 120.36 , 78.47 2021-02-13T00:23:54Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.01 , 0.04 2021-02-13T00:23:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175834.16 epoch sec; seqNo:12270; 4059060.74 north; 592483.01 east 2021-02-13T00:23:54Z- AHRS_M2 msg: 1613175834.52 epoch sec; seqNo:12174 2021-02-13T00:23:54Z- AHRS_M2 msg: 0.61 phi; 0.38 theta; -0.15 psi 2021-02-13T00:23:54Z- handleDepth()-Depth_Keller msg: 1613175834524 epoch sec; seqNo:12271; depth 10.63 2021-02-13T00:23:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175834.54 epoch sec; seqNo:12271; 4059061.12 north; 592483.01 east 2021-02-13T00:23:54Z- AHRS_M2 msg: 1613175834.92 epoch sec; seqNo:12175 2021-02-13T00:23:54Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.15 psi 2021-02-13T00:23:54Z- handleDepth()-Depth_Keller msg: 1613175834937 epoch sec; seqNo:12272; depth 10.50 2021-02-13T00:23:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175834.98 epoch sec; seqNo:12272; 4059061.51 north; 592484.22 east 2021-02-13T00:23:55Z- AHRS_M2 msg: 1613175835.33 epoch sec; seqNo:12176 2021-02-13T00:23:55Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.14 psi 2021-02-13T00:23:55Z- handleDepth()-Depth_Keller msg: 1613175835334 epoch sec; seqNo:12273; depth 10.39 2021-02-13T00:23:55Z- handleDvl() - RDI_Pathfinder msg: 1613175835.35 epoch sec; seqNo:6042 2021-02-13T00:23:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.12 , 104.00 , 118.87 , 77.24 2021-02-13T00:23:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.07 , -0.09 2021-02-13T00:23:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175835.36 epoch sec; seqNo:12273; 4059061.89 north; 592484.22 east 2021-02-13T00:23:55Z- AHRS_M2 msg: 1613175835.73 epoch sec; seqNo:12177 2021-02-13T00:23:55Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.14 psi 2021-02-13T00:23:55Z- handleDepth()-Depth_Keller msg: 1613175835736 epoch sec; seqNo:12274; depth 10.27 2021-02-13T00:23:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175835.76 epoch sec; seqNo:12274; 4059061.89 north; 592484.22 east 2021-02-13T00:23:56Z- AHRS_M2 msg: 1613175836.14 epoch sec; seqNo:12178 2021-02-13T00:23:56Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.13 psi 2021-02-13T00:23:56Z- handleDepth()-Depth_Keller msg: 1613175836149 epoch sec; seqNo:12275; depth 10.17 2021-02-13T00:23:56Z- handleDvl() - RDI_Pathfinder msg: 1613175836.17 epoch sec; seqNo:6043 2021-02-13T00:23:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 102.65 , 126.98 , 0.00 2021-02-13T00:23:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , -0.25 2021-02-13T00:23:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175836.18 epoch sec; seqNo:12275; 4059062.27 north; 592484.22 east 2021-02-13T00:23:56Z- AHRS_M2 msg: 1613175836.56 epoch sec; seqNo:12179 2021-02-13T00:23:56Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.13 psi 2021-02-13T00:23:56Z- handleDepth()-Depth_Keller msg: 1613175836567 epoch sec; seqNo:12276; depth 10.03 2021-02-13T00:23:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175836.58 epoch sec; seqNo:12276; 4059062.65 north; 592484.21 east 2021-02-13T00:23:56Z- AHRS_M2 msg: 1613175836.94 epoch sec; seqNo:12180 2021-02-13T00:23:56Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.13 psi 2021-02-13T00:23:56Z- handleDepth()-Depth_Keller msg: 1613175836958 epoch sec; seqNo:12277; depth 9.90 2021-02-13T00:23:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175836.97 epoch sec; seqNo:12277; 4059062.65 north; 592484.21 east 2021-02-13T00:23:57Z- AHRS_M2 msg: 1613175837.35 epoch sec; seqNo:12181 2021-02-13T00:23:57Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.13 psi 2021-02-13T00:23:57Z- handleDepth()-Depth_Keller msg: 1613175837352 epoch sec; seqNo:12278; depth 9.78 2021-02-13T00:23:57Z- handleDvl() - RDI_Pathfinder msg: 1613175837.36 epoch sec; seqNo:6044 2021-02-13T00:23:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.87 , 101.77 , 119.61 , 80.94 2021-02-13T00:23:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.18 , -0.26 2021-02-13T00:23:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175837.38 epoch sec; seqNo:12278; 4059063.03 north; 592484.21 east 2021-02-13T00:23:57Z- AHRS_M2 msg: 1613175837.75 epoch sec; seqNo:12182 2021-02-13T00:23:57Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.14 psi 2021-02-13T00:23:57Z- handleDepth()-Depth_Keller msg: 1613175837758 epoch sec; seqNo:12279; depth 9.65 2021-02-13T00:23:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175837.77 epoch sec; seqNo:12279; 4059063.03 north; 592484.21 east 2021-02-13T00:23:58Z- AHRS_M2 msg: 1613175838.16 epoch sec; seqNo:12183 2021-02-13T00:23:58Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.15 psi 2021-02-13T00:23:58Z- handleDepth()-Depth_Keller msg: 1613175838166 epoch sec; seqNo:12280; depth 9.52 2021-02-13T00:23:58Z- handleDvl() - RDI_Pathfinder msg: 1613175838.18 epoch sec; seqNo:6045 2021-02-13T00:23:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.17 , 102.58 , 118.80 , 79.59 2021-02-13T00:23:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.35 , 0.04 2021-02-13T00:23:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175838.19 epoch sec; seqNo:12280; 4059063.42 north; 592485.43 east 2021-02-13T00:23:58Z- LcmTrn::handleMessages() - handling 45 messages took 0.71 ms and 131 calls... 2021-02-13T00:23:58Z- waiting for 1613175835.92, time is 1613175838.19 2021-02-13T00:23:58Z- LcmTrn::updateTrn() equating pose 1613175838.19 and meas times 1613175838.19 2021-02-13T00:23:58Z- LcmTrn::updateTrn() >>>> pose time 1613175838.19, meas time 1613175838.19 <<<< 2021-02-13T00:23:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175838.19 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:23:58Z- TerrainNav::motionUpdate() - currEstimate = (4059063, 592485.43, 9.52) 2021-02-13T00:23:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:23:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:23:58Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175838.19 sec.; No relevant navigation data available 2021-02-13T00:23:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:23:58Z- TNavBF::mmse selected from filter -1 2021-02-13T00:23:58Z- variances([0], [2], and psi_berg) = 704.759145 833.000507 0.000000 2021-02-13T00:23:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:23:58Z- LcmTrn::publishEstimates() - MLE : 4059062.92 592534.93 9.52 2021-02-13T00:23:58Z- LcmTrn::publishEstimates() - MMSE : 4059063.42 592485.43 9.52 2021-02-13T00:23:58Z- LcmTrn::publishEstimates() - COVAR: 704.76 833.00 1.50 0.00 2021-02-13T00:23:58Z- AHRS_M2 msg: 1613175838.56 epoch sec; seqNo:12184 2021-02-13T00:23:58Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.16 psi 2021-02-13T00:23:58Z- handleDepth()-Depth_Keller msg: 1613175838564 epoch sec; seqNo:12281; depth 9.39 2021-02-13T00:23:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175838.58 epoch sec; seqNo:12281; 4059063.80 north; 592485.42 east 2021-02-13T00:23:58Z- AHRS_M2 msg: 1613175838.96 epoch sec; seqNo:12185 2021-02-13T00:23:58Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.16 psi 2021-02-13T00:23:58Z- handleDepth()-Depth_Keller msg: 1613175838968 epoch sec; seqNo:12282; depth 9.27 2021-02-13T00:23:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175838.98 epoch sec; seqNo:12282; 4059064.18 north; 592485.42 east 2021-02-13T00:23:59Z- AHRS_M2 msg: 1613175839.37 epoch sec; seqNo:12186 2021-02-13T00:23:59Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.15 psi 2021-02-13T00:23:59Z- handleDepth()-Depth_Keller msg: 1613175839372 epoch sec; seqNo:12283; depth 9.14 2021-02-13T00:23:59Z- handleDvl() - RDI_Pathfinder msg: 1613175839.38 epoch sec; seqNo:6046 2021-02-13T00:23:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.03 , 105.15 , 117.32 , 80.81 2021-02-13T00:23:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.21 , 0.37 2021-02-13T00:23:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175839.39 epoch sec; seqNo:12283; 4059064.55 north; 592485.41 east 2021-02-13T00:23:59Z- AHRS_M2 msg: 1613175839.77 epoch sec; seqNo:12187 2021-02-13T00:23:59Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-13T00:23:59Z- handleDepth()-Depth_Keller msg: 1613175839778 epoch sec; seqNo:12284; depth 9.01 2021-02-13T00:23:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175839.79 epoch sec; seqNo:12284; 4059064.93 north; 592485.41 east 2021-02-13T00:24:00Z- AHRS_M2 msg: 1613175840.18 epoch sec; seqNo:12188 2021-02-13T00:24:00Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T00:24:00Z- handleDepth()-Depth_Keller msg: 1613175840184 epoch sec; seqNo:12285; depth 8.87 2021-02-13T00:24:00Z- handleDvl() - RDI_Pathfinder msg: 1613175840.21 epoch sec; seqNo:6047 2021-02-13T00:24:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.92 , 105.15 , 120.02 , 80.81 2021-02-13T00:24:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.03 , 0.41 2021-02-13T00:24:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175840.22 epoch sec; seqNo:12285; 4059065.31 north; 592485.40 east 2021-02-13T00:24:00Z- AHRS_M2 msg: 1613175840.58 epoch sec; seqNo:12189 2021-02-13T00:24:00Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.15 psi 2021-02-13T00:24:00Z- handleDepth()-Depth_Keller msg: 1613175840584 epoch sec; seqNo:12286; depth 8.74 2021-02-13T00:24:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175840.60 epoch sec; seqNo:12286; 4059065.31 north; 592485.40 east 2021-02-13T00:24:00Z- AHRS_M2 msg: 1613175840.98 epoch sec; seqNo:12190 2021-02-13T00:24:00Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.16 psi 2021-02-13T00:24:00Z- handleDepth()-Depth_Keller msg: 1613175840988 epoch sec; seqNo:12287; depth 8.62 2021-02-13T00:24:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175841.00 epoch sec; seqNo:12287; 4059065.70 north; 592486.62 east 2021-02-13T00:24:01Z- AHRS_M2 msg: 1613175841.39 epoch sec; seqNo:12191 2021-02-13T00:24:01Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.16 psi 2021-02-13T00:24:01Z- handleDepth()-Depth_Keller msg: 1613175841394 epoch sec; seqNo:12288; depth 8.49 2021-02-13T00:24:01Z- handleDvl() - RDI_Pathfinder msg: 1613175841.41 epoch sec; seqNo:6048 2021-02-13T00:24:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.76 , 105.28 , 121.51 , 80.94 2021-02-13T00:24:01Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.24 , 0.18 2021-02-13T00:24:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175841.42 epoch sec; seqNo:12288; 4059066.08 north; 592486.62 east 2021-02-13T00:24:01Z- AHRS_M2 msg: 1613175841.79 epoch sec; seqNo:12192 2021-02-13T00:24:01Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.17 psi 2021-02-13T00:24:01Z- handleDepth()-Depth_Keller msg: 1613175841801 epoch sec; seqNo:12289; depth 8.36 2021-02-13T00:24:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175841.82 epoch sec; seqNo:12289; 4059066.46 north; 592486.61 east 2021-02-13T00:24:02Z- AHRS_M2 msg: 1613175842.20 epoch sec; seqNo:12193 2021-02-13T00:24:02Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.17 psi 2021-02-13T00:24:02Z- handleDepth()-Depth_Keller msg: 1613175842201 epoch sec; seqNo:12290; depth 8.23 2021-02-13T00:24:02Z- handleDvl() - RDI_Pathfinder msg: 1613175842.21 epoch sec; seqNo:6049 2021-02-13T00:24:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.03 , 107.85 , 128.13 , 80.81 2021-02-13T00:24:02Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.24 , -0.15 2021-02-13T00:24:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175842.24 epoch sec; seqNo:12290; 4059066.46 north; 592486.61 east 2021-02-13T00:24:02Z- AHRS_M2 msg: 1613175842.60 epoch sec; seqNo:12194 2021-02-13T00:24:02Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.18 psi 2021-02-13T00:24:02Z- handleDepth()-Depth_Keller msg: 1613175842609 epoch sec; seqNo:12291; depth 8.11 2021-02-13T00:24:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175842.62 epoch sec; seqNo:12291; 4059066.85 north; 592487.83 east 2021-02-13T00:24:03Z- AHRS_M2 msg: 1613175843.00 epoch sec; seqNo:12195 2021-02-13T00:24:03Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.18 psi 2021-02-13T00:24:03Z- handleDepth()-Depth_Keller msg: 1613175843017 epoch sec; seqNo:12292; depth 7.99 2021-02-13T00:24:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175843.03 epoch sec; seqNo:12292; 4059067.23 north; 592487.83 east 2021-02-13T00:24:03Z- LcmTrn::handleMessages() - handling 40 messages took 0.63 ms and 128 calls... 2021-02-13T00:24:03Z- waiting for 1613175841.19, time is 1613175843.03 2021-02-13T00:24:03Z- LcmTrn::updateTrn() >>>> pose time 1613175843.03, meas time 1613175842.21 <<<< 2021-02-13T00:24:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175842.21 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:24:03Z- TerrainNav::motionUpdate() - currEstimate = (4059067, 592487.83, 7.99) 2021-02-13T00:24:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:24:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:24:03Z- TNavBF::Projecting Measurements 2021-02-13T00:24:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:24:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:24:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:24:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:24:03Z- TNavFilter::4 good beams. 2021-02-13T00:24:03Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:24:03Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:24:03Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:24:03Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:24:03Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:24:03Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:24:03Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:24:03Z- TNavBF::mmse selected from filter -1 2021-02-13T00:24:03Z- TerrainMapOctree: (vehN, vehE ) = (4059066.59, 592487.42). 2021-02-13T00:24:03Z- TerrainMapOctree: Min Distance = 2806.06. 2021-02-13T00:24:03Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:24:03Z- There is no specific good beam that all particles have in common. 2021-02-13T00:24:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:24:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.69 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 15.06 2021-02-13T00:24:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:24:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.54 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 18.61 2021-02-13T00:24:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:24:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 26.27 2021-02-13T00:24:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:24:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.36 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 10.45 2021-02-13T00:24:03Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:24:03Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:24:03Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:24:03Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:24:03Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:24:03Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:24:03Z- Filter 0 No beams used in subcloud update 2021-02-13T00:24:03Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:24:03Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:24:03Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 704.7 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:24:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175842.21 sec. 2021-02-13T00:24:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:24:03Z- TNavBF::mmse selected from filter -1 2021-02-13T00:24:03Z- variances([0], [2], and psi_berg) = 704.679183 833.000463 0.000000 2021-02-13T00:24:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:24:03Z- LcmTrn::publishEstimates() - MLE : 4059066.73 592536.33 7.99 2021-02-13T00:24:03Z- LcmTrn::publishEstimates() - MMSE : 4059067.23 592487.83 7.99 2021-02-13T00:24:03Z- LcmTrn::publishEstimates() - COVAR: 704.68 833.00 1.26 0.00 2021-02-13T00:24:03Z- AHRS_M2 msg: 1613175843.42 epoch sec; seqNo:12196 2021-02-13T00:24:03Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.19 psi 2021-02-13T00:24:03Z- handleDepth()-Depth_Keller msg: 1613175843427 epoch sec; seqNo:12293; depth 7.84 2021-02-13T00:24:03Z- handleDvl() - RDI_Pathfinder msg: 1613175843.44 epoch sec; seqNo:6050 2021-02-13T00:24:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 109.68 , 126.04 , 82.90 2021-02-13T00:24:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , -0.30 2021-02-13T00:24:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175843.45 epoch sec; seqNo:12293; 4059067.23 north; 592487.83 east 2021-02-13T00:24:03Z- AHRS_M2 msg: 1613175843.82 epoch sec; seqNo:12197 2021-02-13T00:24:03Z- AHRS_M2 msg: 0.67 phi; 0.35 theta; -0.18 psi 2021-02-13T00:24:03Z- handleDepth()-Depth_Keller msg: 1613175843823 epoch sec; seqNo:12294; depth 7.72 2021-02-13T00:24:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175843.83 epoch sec; seqNo:12294; 4059067.61 north; 592487.82 east 2021-02-13T00:24:04Z- AHRS_M2 msg: 1613175844.22 epoch sec; seqNo:12198 2021-02-13T00:24:04Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.17 psi 2021-02-13T00:24:04Z- handleDepth()-Depth_Keller msg: 1613175844229 epoch sec; seqNo:12295; depth 7.61 2021-02-13T00:24:04Z- handleDvl() - RDI_Pathfinder msg: 1613175844.24 epoch sec; seqNo:6051 2021-02-13T00:24:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 109.68 , 124.55 , 82.90 2021-02-13T00:24:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.17 , -0.18 2021-02-13T00:24:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175844.25 epoch sec; seqNo:12295; 4059067.99 north; 592487.82 east 2021-02-13T00:24:04Z- AHRS_M2 msg: 1613175844.62 epoch sec; seqNo:12199 2021-02-13T00:24:04Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.15 psi 2021-02-13T00:24:04Z- handleDepth()-Depth_Keller msg: 1613175844624 epoch sec; seqNo:12296; depth 7.47 2021-02-13T00:24:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175844.64 epoch sec; seqNo:12296; 4059067.99 north; 592487.82 east 2021-02-13T00:24:05Z- AHRS_M2 msg: 1613175845.02 epoch sec; seqNo:12200 2021-02-13T00:24:05Z- AHRS_M2 msg: 0.67 phi; 0.33 theta; -0.14 psi 2021-02-13T00:24:05Z- handleDepth()-Depth_Keller msg: 1613175845029 epoch sec; seqNo:12297; depth 7.36 2021-02-13T00:24:05Z- handleDvl() - RDI_Pathfinder msg: 1613175845.04 epoch sec; seqNo:6052 2021-02-13T00:24:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 108.19 , 123.06 , 0.00 2021-02-13T00:24:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.19 , 0.07 2021-02-13T00:24:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175845.06 epoch sec; seqNo:12297; 4059068.37 north; 592487.81 east 2021-02-13T00:24:05Z- AHRS_M2 msg: 1613175845.43 epoch sec; seqNo:12201 2021-02-13T00:24:05Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.13 psi 2021-02-13T00:24:05Z- handleDepth()-Depth_Keller msg: 1613175845432 epoch sec; seqNo:12298; depth 7.23 2021-02-13T00:24:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175845.44 epoch sec; seqNo:12298; 4059068.76 north; 592489.03 east 2021-02-13T00:24:05Z- AHRS_M2 msg: 1613175845.84 epoch sec; seqNo:12202 2021-02-13T00:24:05Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.12 psi 2021-02-13T00:24:05Z- handleDepth()-Depth_Keller msg: 1613175845850 epoch sec; seqNo:12299; depth 7.12 2021-02-13T00:24:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175845.86 epoch sec; seqNo:12299; 4059069.14 north; 592489.03 east 2021-02-13T00:24:06Z- AHRS_M2 msg: 1613175846.23 epoch sec; seqNo:12203 2021-02-13T00:24:06Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.12 psi 2021-02-13T00:24:06Z- handleDepth()-Depth_Keller msg: 1613175846241 epoch sec; seqNo:12300; depth 6.99 2021-02-13T00:24:06Z- handleDvl() - RDI_Pathfinder msg: 1613175846.27 epoch sec; seqNo:6053 2021-02-13T00:24:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.31 , 104.74 , 121.10 , 82.43 2021-02-13T00:24:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.00 , 0.19 2021-02-13T00:24:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175846.28 epoch sec; seqNo:12300; 4059069.14 north; 592489.03 east 2021-02-13T00:24:06Z- AHRS_M2 msg: 1613175846.64 epoch sec; seqNo:12204 2021-02-13T00:24:06Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.12 psi 2021-02-13T00:24:06Z- handleDepth()-Depth_Keller msg: 1613175846649 epoch sec; seqNo:12301; depth 6.87 2021-02-13T00:24:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175846.66 epoch sec; seqNo:12301; 4059069.52 north; 592489.02 east 2021-02-13T00:24:07Z- AHRS_M2 msg: 1613175847.05 epoch sec; seqNo:12205 2021-02-13T00:24:07Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.13 psi 2021-02-13T00:24:07Z- handleDepth()-Depth_Keller msg: 1613175847050 epoch sec; seqNo:12302; depth 6.74 2021-02-13T00:24:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175847.06 epoch sec; seqNo:12302; 4059069.90 north; 592489.02 east 2021-02-13T00:24:07Z- AHRS_M2 msg: 1613175847.45 epoch sec; seqNo:12206 2021-02-13T00:24:07Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-13T00:24:07Z- handleDepth()-Depth_Keller msg: 1613175847453 epoch sec; seqNo:12303; depth 6.60 2021-02-13T00:24:07Z- handleDvl() - RDI_Pathfinder msg: 1613175847.47 epoch sec; seqNo:6054 2021-02-13T00:24:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.98 , 102.44 , 118.80 , 81.62 2021-02-13T00:24:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.09 , 0.11 2021-02-13T00:24:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175847.48 epoch sec; seqNo:12303; 4059069.90 north; 592489.02 east 2021-02-13T00:24:07Z- AHRS_M2 msg: 1613175847.85 epoch sec; seqNo:12207 2021-02-13T00:24:07Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.16 psi 2021-02-13T00:24:07Z- handleDepth()-Depth_Keller msg: 1613175847856 epoch sec; seqNo:12304; depth 6.49 2021-02-13T00:24:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175847.87 epoch sec; seqNo:12304; 4059070.29 north; 592490.23 east 2021-02-13T00:24:08Z- AHRS_M2 msg: 1613175848.26 epoch sec; seqNo:12208 2021-02-13T00:24:08Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.17 psi 2021-02-13T00:24:08Z- handleDepth()-Depth_Keller msg: 1613175848269 epoch sec; seqNo:12305; depth 6.35 2021-02-13T00:24:08Z- handleDvl() - RDI_Pathfinder msg: 1613175848.28 epoch sec; seqNo:6055 2021-02-13T00:24:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.98 , 109.20 , 120.02 , 0.00 2021-02-13T00:24:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.09 , 0.02 2021-02-13T00:24:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175848.29 epoch sec; seqNo:12305; 4059070.67 north; 592490.23 east 2021-02-13T00:24:08Z- LcmTrn::handleMessages() - handling 45 messages took 0.63 ms and 133 calls... 2021-02-13T00:24:08Z- waiting for 1613175846.03, time is 1613175848.29 2021-02-13T00:24:08Z- LcmTrn::updateTrn() equating pose 1613175848.29 and meas times 1613175848.29 2021-02-13T00:24:08Z- LcmTrn::updateTrn() >>>> pose time 1613175848.29, meas time 1613175848.29 <<<< 2021-02-13T00:24:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175848.29 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:24:08Z- TerrainNav::motionUpdate() - currEstimate = (4059071, 592490.23, 6.35) 2021-02-13T00:24:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:24:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:24:08Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175848.29 sec.; No relevant navigation data available 2021-02-13T00:24:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:24:08Z- TNavBF::mmse selected from filter -1 2021-02-13T00:24:08Z- variances([0], [2], and psi_berg) = 704.620901 833.000430 0.000000 2021-02-13T00:24:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:24:08Z- LcmTrn::publishEstimates() - MLE : 4059070.17 592539.73 6.35 2021-02-13T00:24:08Z- LcmTrn::publishEstimates() - MMSE : 4059070.67 592490.23 6.35 2021-02-13T00:24:08Z- LcmTrn::publishEstimates() - COVAR: 704.62 833.00 1.48 0.00 2021-02-13T00:24:08Z- AHRS_M2 msg: 1613175848.66 epoch sec; seqNo:12209 2021-02-13T00:24:08Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.17 psi 2021-02-13T00:24:08Z- handleDepth()-Depth_Keller msg: 1613175848673 epoch sec; seqNo:12306; depth 6.26 2021-02-13T00:24:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175848.68 epoch sec; seqNo:12306; 4059070.67 north; 592490.23 east 2021-02-13T00:24:09Z- AHRS_M2 msg: 1613175849.06 epoch sec; seqNo:12210 2021-02-13T00:24:09Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.16 psi 2021-02-13T00:24:09Z- handleDepth()-Depth_Keller msg: 1613175849073 epoch sec; seqNo:12307; depth 6.13 2021-02-13T00:24:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175849.09 epoch sec; seqNo:12307; 4059071.05 north; 592490.23 east 2021-02-13T00:24:09Z- AHRS_M2 msg: 1613175849.47 epoch sec; seqNo:12211 2021-02-13T00:24:09Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.16 psi 2021-02-13T00:24:09Z- handleDepth()-Depth_Keller msg: 1613175849477 epoch sec; seqNo:12308; depth 6.00 2021-02-13T00:24:09Z- handleDvl() - RDI_Pathfinder msg: 1613175849.49 epoch sec; seqNo:6056 2021-02-13T00:24:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.44 , 109.34 , 125.70 , 84.05 2021-02-13T00:24:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.10 , -0.11 2021-02-13T00:24:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175849.51 epoch sec; seqNo:12308; 4059071.43 north; 592490.22 east 2021-02-13T00:24:09Z- AHRS_M2 msg: 1613175849.87 epoch sec; seqNo:12212 2021-02-13T00:24:09Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.17 psi 2021-02-13T00:24:09Z- handleDepth()-Depth_Keller msg: 1613175849877 epoch sec; seqNo:12309; depth 5.88 2021-02-13T00:24:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175849.89 epoch sec; seqNo:12309; 4059071.44 north; 592491.44 east 2021-02-13T00:24:10Z- AHRS_M2 msg: 1613175850.28 epoch sec; seqNo:12213 2021-02-13T00:24:10Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.18 psi 2021-02-13T00:24:10Z- handleDepth()-Depth_Keller msg: 1613175850282 epoch sec; seqNo:12310; depth 5.76 2021-02-13T00:24:10Z- handleDvl() - RDI_Pathfinder msg: 1613175850.29 epoch sec; seqNo:6057 2021-02-13T00:24:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 111.16 , 124.55 , 84.39 2021-02-13T00:24:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , -0.20 2021-02-13T00:24:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175850.30 epoch sec; seqNo:12310; 4059071.82 north; 592491.44 east 2021-02-13T00:24:10Z- AHRS_M2 msg: 1613175850.68 epoch sec; seqNo:12214 2021-02-13T00:24:10Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.19 psi 2021-02-13T00:24:10Z- handleDepth()-Depth_Keller msg: 1613175850684 epoch sec; seqNo:12311; depth 5.63 2021-02-13T00:24:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175850.70 epoch sec; seqNo:12311; 4059072.20 north; 592491.43 east 2021-02-13T00:24:11Z- AHRS_M2 msg: 1613175851.08 epoch sec; seqNo:12215 2021-02-13T00:24:11Z- AHRS_M2 msg: 0.67 phi; 0.29 theta; -0.20 psi 2021-02-13T00:24:11Z- handleDepth()-Depth_Keller msg: 1613175851092 epoch sec; seqNo:12312; depth 5.50 2021-02-13T00:24:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175851.10 epoch sec; seqNo:12312; 4059072.20 north; 592491.43 east 2021-02-13T00:24:11Z- AHRS_M2 msg: 1613175851.49 epoch sec; seqNo:12216 2021-02-13T00:24:11Z- AHRS_M2 msg: 0.67 phi; 0.26 theta; -0.19 psi 2021-02-13T00:24:11Z- handleDepth()-Depth_Keller msg: 1613175851496 epoch sec; seqNo:12313; depth 5.37 2021-02-13T00:24:11Z- handleDvl() - RDI_Pathfinder msg: 1613175851.51 epoch sec; seqNo:6058 2021-02-13T00:24:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.27 , 111.10 , 121.51 , 84.32 2021-02-13T00:24:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.12 , -0.05 2021-02-13T00:24:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175851.52 epoch sec; seqNo:12313; 4059072.58 north; 592491.43 east 2021-02-13T00:24:11Z- AHRS_M2 msg: 1613175851.89 epoch sec; seqNo:12217 2021-02-13T00:24:11Z- AHRS_M2 msg: 0.67 phi; 0.23 theta; -0.18 psi 2021-02-13T00:24:11Z- handleDepth()-Depth_Keller msg: 1613175851896 epoch sec; seqNo:12314; depth 5.24 2021-02-13T00:24:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175851.91 epoch sec; seqNo:12314; 4059072.96 north; 592491.43 east 2021-02-13T00:24:12Z- AHRS_M2 msg: 1613175852.30 epoch sec; seqNo:12218 2021-02-13T00:24:12Z- AHRS_M2 msg: 0.68 phi; 0.19 theta; -0.16 psi 2021-02-13T00:24:12Z- handleDepth()-Depth_Keller msg: 1613175852300 epoch sec; seqNo:12315; depth 5.13 2021-02-13T00:24:12Z- handleDvl() - RDI_Pathfinder msg: 1613175852.31 epoch sec; seqNo:6059 2021-02-13T00:24:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:24:12Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:24:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175852.34 epoch sec; seqNo:12315; 4059073.33 north; 592491.42 east 2021-02-13T00:24:12Z- AHRS_M2 msg: 1613175852.70 epoch sec; seqNo:12219 2021-02-13T00:24:12Z- AHRS_M2 msg: 0.69 phi; 0.14 theta; -0.16 psi 2021-02-13T00:24:12Z- handleDepth()-Depth_Keller msg: 1613175852704 epoch sec; seqNo:12316; depth 5.02 2021-02-13T00:24:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175852.71 epoch sec; seqNo:12316; 4059073.35 north; 592492.64 east 2021-02-13T00:24:13Z- AHRS_M2 msg: 1613175853.10 epoch sec; seqNo:12220 2021-02-13T00:24:13Z- AHRS_M2 msg: 0.70 phi; 0.10 theta; -0.17 psi 2021-02-13T00:24:13Z- handleDepth()-Depth_Keller msg: 1613175853110 epoch sec; seqNo:12317; depth 4.90 2021-02-13T00:24:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175853.12 epoch sec; seqNo:12317; 4059073.73 north; 592492.64 east 2021-02-13T00:24:13Z- AHRS_M2 msg: 1613175853.51 epoch sec; seqNo:12221 2021-02-13T00:24:13Z- AHRS_M2 msg: 0.71 phi; 0.05 theta; -0.20 psi 2021-02-13T00:24:13Z- handleDepth()-Depth_Keller msg: 1613175853512 epoch sec; seqNo:12318; depth 4.81 2021-02-13T00:24:13Z- handleDvl() - RDI_Pathfinder msg: 1613175853.53 epoch sec; seqNo:6060 2021-02-13T00:24:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 104.67 , 104.67 , 88.31 2021-02-13T00:24:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.06 , -0.17 2021-02-13T00:24:13Z- LcmTrn::handleMessages() - handling 43 messages took 0.71 ms and 129 calls... 2021-02-13T00:24:13Z- waiting for 1613175851.29, time is 1613175853.53 2021-02-13T00:24:13Z- LcmTrn::updateTrn() equating pose 1613175853.53 and meas times 1613175853.53 2021-02-13T00:24:13Z- LcmTrn::updateTrn() >>>> pose time 1613175853.53, meas time 1613175853.53 <<<< 2021-02-13T00:24:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175853.53 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:24:13Z- TerrainNav::motionUpdate() - currEstimate = (4059074, 592492.64, 4.81) 2021-02-13T00:24:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:24:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:24:13Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175853.53 sec.; No relevant navigation data available 2021-02-13T00:24:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:24:13Z- TNavBF::mmse selected from filter -1 2021-02-13T00:24:13Z- variances([0], [2], and psi_berg) = 704.564791 833.000399 0.000000 2021-02-13T00:24:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:24:13Z- LcmTrn::publishEstimates() - MLE : 4059073.23 592542.14 4.81 2021-02-13T00:24:13Z- LcmTrn::publishEstimates() - MMSE : 4059073.73 592492.64 4.81 2021-02-13T00:24:13Z- LcmTrn::publishEstimates() - COVAR: 704.56 833.00 1.67 0.00 2021-02-13T00:24:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175853.54 epoch sec; seqNo:12318; 4059074.10 north; 592492.63 east 2021-02-13T00:24:13Z- AHRS_M2 msg: 1613175853.91 epoch sec; seqNo:12222 2021-02-13T00:24:13Z- AHRS_M2 msg: 0.72 phi; 0.01 theta; -0.24 psi 2021-02-13T00:24:13Z- handleDepth()-Depth_Keller msg: 1613175853916 epoch sec; seqNo:12319; depth 4.71 2021-02-13T00:24:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175853.93 epoch sec; seqNo:12319; 4059074.48 north; 592492.63 east 2021-02-13T00:24:14Z- AHRS_M2 msg: 1613175854.32 epoch sec; seqNo:12223 2021-02-13T00:24:14Z- AHRS_M2 msg: 0.73 phi; -0.04 theta; -0.26 psi 2021-02-13T00:24:14Z- handleDepth()-Depth_Keller msg: 1613175854320 epoch sec; seqNo:12320; depth 4.63 2021-02-13T00:24:14Z- handleDvl() - RDI_Pathfinder msg: 1613175854.33 epoch sec; seqNo:6061 2021-02-13T00:24:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 109.34 , 97.17 , 97.17 2021-02-13T00:24:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.30 , 0.08 2021-02-13T00:24:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175854.34 epoch sec; seqNo:12320; 4059074.48 north; 592492.63 east 2021-02-13T00:24:14Z- AHRS_M2 msg: 1613175854.72 epoch sec; seqNo:12224 2021-02-13T00:24:14Z- AHRS_M2 msg: 0.74 phi; -0.08 theta; -0.25 psi 2021-02-13T00:24:14Z- handleDepth()-Depth_Keller msg: 1613175854726 epoch sec; seqNo:12321; depth 4.57 2021-02-13T00:24:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175854.74 epoch sec; seqNo:12321; 4059074.86 north; 592492.63 east 2021-02-13T00:24:15Z- AHRS_M2 msg: 1613175855.13 epoch sec; seqNo:12225 2021-02-13T00:24:15Z- AHRS_M2 msg: 0.74 phi; -0.13 theta; -0.23 psi 2021-02-13T00:24:15Z- handleDepth()-Depth_Keller msg: 1613175855133 epoch sec; seqNo:12322; depth 4.54 2021-02-13T00:24:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175855.15 epoch sec; seqNo:12322; 4059075.24 north; 592492.62 east 2021-02-13T00:24:15Z- AHRS_M2 msg: 1613175855.53 epoch sec; seqNo:12226 2021-02-13T00:24:15Z- AHRS_M2 msg: 0.74 phi; -0.17 theta; -0.20 psi 2021-02-13T00:24:15Z- handleDepth()-Depth_Keller msg: 1613175855532 epoch sec; seqNo:12323; depth 4.52 2021-02-13T00:24:15Z- handleDvl() - RDI_Pathfinder msg: 1613175855.54 epoch sec; seqNo:6062 2021-02-13T00:24:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 110.83 , 97.31 , 101.36 2021-02-13T00:24:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.63 , -0.06 , 0.56 2021-02-13T00:24:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175855.57 epoch sec; seqNo:12323; 4059075.24 north; 592492.62 east 2021-02-13T00:24:15Z- AHRS_M2 msg: 1613175855.93 epoch sec; seqNo:12227 2021-02-13T00:24:15Z- AHRS_M2 msg: 0.73 phi; -0.21 theta; -0.18 psi 2021-02-13T00:24:15Z- handleDepth()-Depth_Keller msg: 1613175855941 epoch sec; seqNo:12324; depth 4.51 2021-02-13T00:24:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175855.95 epoch sec; seqNo:12324; 4059075.62 north; 592492.62 east 2021-02-13T00:24:16Z- AHRS_M2 msg: 1613175856.34 epoch sec; seqNo:12228 2021-02-13T00:24:16Z- AHRS_M2 msg: 0.72 phi; -0.24 theta; -0.17 psi 2021-02-13T00:24:16Z- handleDepth()-Depth_Keller msg: 1613175856346 epoch sec; seqNo:12325; depth 4.52 2021-02-13T00:24:16Z- handleDvl() - RDI_Pathfinder msg: 1613175856.36 epoch sec; seqNo:6063 2021-02-13T00:24:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 107.99 , 0.00 , 110.69 2021-02-13T00:24:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , 0.29 , 0.49 2021-02-13T00:24:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175856.37 epoch sec; seqNo:12325; 4059075.63 north; 592493.84 east 2021-02-13T00:24:16Z- AHRS_M2 msg: 1613175856.74 epoch sec; seqNo:12229 2021-02-13T00:24:16Z- AHRS_M2 msg: 0.70 phi; -0.26 theta; -0.19 psi 2021-02-13T00:24:16Z- handleDepth()-Depth_Keller msg: 1613175856744 epoch sec; seqNo:12326; depth 4.51 2021-02-13T00:24:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175856.76 epoch sec; seqNo:12326; 4059075.63 north; 592493.84 east 2021-02-13T00:24:17Z- AHRS_M2 msg: 1613175857.15 epoch sec; seqNo:12230 2021-02-13T00:24:17Z- AHRS_M2 msg: 0.69 phi; -0.27 theta; -0.23 psi 2021-02-13T00:24:17Z- handleDepth()-Depth_Keller msg: 1613175857157 epoch sec; seqNo:12327; depth 4.56 2021-02-13T00:24:17Z- handleDvl() - RDI_Pathfinder msg: 1613175857.17 epoch sec; seqNo:6064 2021-02-13T00:24:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.91 , 109.81 , 0.00 , 117.25 2021-02-13T00:24:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.38 , 0.12 2021-02-13T00:24:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175857.19 epoch sec; seqNo:12327; 4059075.63 north; 592493.84 east 2021-02-13T00:24:17Z- AHRS_M2 msg: 1613175857.55 epoch sec; seqNo:12231 2021-02-13T00:24:17Z- AHRS_M2 msg: 0.69 phi; -0.27 theta; -0.27 psi 2021-02-13T00:24:17Z- handleDepth()-Depth_Keller msg: 1613175857552 epoch sec; seqNo:12328; depth 4.61 2021-02-13T00:24:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175857.56 epoch sec; seqNo:12328; 4059076.01 north; 592493.84 east 2021-02-13T00:24:17Z- AHRS_M2 msg: 1613175857.95 epoch sec; seqNo:12232 2021-02-13T00:24:17Z- AHRS_M2 msg: 0.69 phi; -0.28 theta; -0.29 psi 2021-02-13T00:24:17Z- handleDepth()-Depth_Keller msg: 1613175857958 epoch sec; seqNo:12329; depth 4.66 2021-02-13T00:24:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175857.97 epoch sec; seqNo:12329; 4059076.03 north; 592495.06 east 2021-02-13T00:24:18Z- AHRS_M2 msg: 1613175858.36 epoch sec; seqNo:12233 2021-02-13T00:24:18Z- AHRS_M2 msg: 0.70 phi; -0.28 theta; -0.30 psi 2021-02-13T00:24:18Z- handleDepth()-Depth_Keller msg: 1613175858360 epoch sec; seqNo:12330; depth 4.75 2021-02-13T00:24:18Z- handleDvl() - RDI_Pathfinder msg: 1613175858.38 epoch sec; seqNo:6065 2021-02-13T00:24:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.45 , 115.22 , 97.37 , 124.14 2021-02-13T00:24:18Z- handleDvl() - RDI_Pathfinder msg: velocities 1.13 , 0.15 , -0.23 2021-02-13T00:24:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175858.39 epoch sec; seqNo:12330; 4059076.40 north; 592495.05 east 2021-02-13T00:24:18Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-13T00:24:18Z- waiting for 1613175856.53, time is 1613175858.39 2021-02-13T00:24:18Z- LcmTrn::updateTrn() equating pose 1613175858.39 and meas times 1613175858.39 2021-02-13T00:24:18Z- LcmTrn::updateTrn() >>>> pose time 1613175858.39, meas time 1613175858.39 <<<< 2021-02-13T00:24:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175858.39 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:24:18Z- TerrainNav::motionUpdate() - currEstimate = (4059076, 592495.05, 4.75) 2021-02-13T00:24:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:24:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:24:18Z- TNavBF::Projecting Measurements 2021-02-13T00:24:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:24:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:24:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:24:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:24:18Z- TNavFilter::4 good beams. 2021-02-13T00:24:18Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:24:18Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:24:18Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:24:18Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:24:18Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:24:18Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:24:18Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:24:18Z- TNavBF::mmse selected from filter -1 2021-02-13T00:24:18Z- TerrainMapOctree: (vehN, vehE ) = (4059076.40, 592495.05). 2021-02-13T00:24:18Z- TerrainMapOctree: Min Distance = 2793.63. 2021-02-13T00:24:18Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:24:18Z- There is no specific good beam that all particles have in common. 2021-02-13T00:24:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:24:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.86 K: 0.115 ExpectedMeasDiff: nan currMeas.covariance: 12.52 2021-02-13T00:24:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:24:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.64 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 21.24 2021-02-13T00:24:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:24:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.53 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 15.17 2021-02-13T00:24:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:24:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.41 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 24.66 2021-02-13T00:24:18Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:24:18Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:24:18Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:24:18Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:24:18Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:24:18Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:24:18Z- Filter 0 No beams used in subcloud update 2021-02-13T00:24:18Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:24:18Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:24:18Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 704.5 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:24:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175858.39 sec. 2021-02-13T00:24:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:24:18Z- TNavBF::mmse selected from filter -1 2021-02-13T00:24:18Z- variances([0], [2], and psi_berg) = 704.510823 833.000370 0.000000 2021-02-13T00:24:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:24:18Z- LcmTrn::publishEstimates() - MLE : 4059075.90 592543.55 4.75 2021-02-13T00:24:18Z- LcmTrn::publishEstimates() - MMSE : 4059076.40 592495.05 4.75 2021-02-13T00:24:18Z- LcmTrn::publishEstimates() - COVAR: 704.51 833.00 1.34 0.00 2021-02-13T00:24:18Z- AHRS_M2 msg: 1613175858.76 epoch sec; seqNo:12234 2021-02-13T00:24:18Z- AHRS_M2 msg: 0.71 phi; -0.29 theta; -0.29 psi 2021-02-13T00:24:18Z- handleDepth()-Depth_Keller msg: 1613175858764 epoch sec; seqNo:12331; depth 4.83 2021-02-13T00:24:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175858.78 epoch sec; seqNo:12331; 4059076.78 north; 592495.05 east 2021-02-13T00:24:19Z- AHRS_M2 msg: 1613175859.16 epoch sec; seqNo:12235 2021-02-13T00:24:19Z- AHRS_M2 msg: 0.71 phi; -0.30 theta; -0.28 psi 2021-02-13T00:24:19Z- handleDepth()-Depth_Keller msg: 1613175859168 epoch sec; seqNo:12332; depth 4.93 2021-02-13T00:24:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175859.18 epoch sec; seqNo:12332; 4059077.18 north; 592496.26 east 2021-02-13T00:24:19Z- AHRS_M2 msg: 1613175859.57 epoch sec; seqNo:12236 2021-02-13T00:24:19Z- AHRS_M2 msg: 0.72 phi; -0.31 theta; -0.28 psi 2021-02-13T00:24:19Z- handleDepth()-Depth_Keller msg: 1613175859575 epoch sec; seqNo:12333; depth 5.04 2021-02-13T00:24:19Z- handleDvl() - RDI_Pathfinder msg: 1613175859.59 epoch sec; seqNo:6066 2021-02-13T00:24:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.16 , 119.89 , 87.16 , 122.86 2021-02-13T00:24:19Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , -0.14 , -0.35 2021-02-13T00:24:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175859.60 epoch sec; seqNo:12333; 4059077.55 north; 592496.26 east 2021-02-13T00:24:19Z- AHRS_M2 msg: 1613175859.97 epoch sec; seqNo:12237 2021-02-13T00:24:19Z- AHRS_M2 msg: 0.72 phi; -0.31 theta; -0.27 psi 2021-02-13T00:24:19Z- handleDepth()-Depth_Keller msg: 1613175859976 epoch sec; seqNo:12334; depth 5.13 2021-02-13T00:24:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175860.00 epoch sec; seqNo:12334; 4059077.93 north; 592496.26 east 2021-02-13T00:24:20Z- AHRS_M2 msg: 1613175860.38 epoch sec; seqNo:12238 2021-02-13T00:24:20Z- AHRS_M2 msg: 0.74 phi; -0.34 theta; -0.23 psi 2021-02-13T00:24:20Z- handleDepth()-Depth_Keller msg: 1613175860386 epoch sec; seqNo:12335; depth 5.24 2021-02-13T00:24:20Z- handleDvl() - RDI_Pathfinder msg: 1613175860.40 epoch sec; seqNo:6067 2021-02-13T00:24:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.30 , 120.02 , 85.81 , 127.46 2021-02-13T00:24:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.31 , -0.15 2021-02-13T00:24:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175860.42 epoch sec; seqNo:12335; 4059078.31 north; 592496.25 east 2021-02-13T00:24:20Z- AHRS_M2 msg: 1613175860.79 epoch sec; seqNo:12239 2021-02-13T00:24:20Z- AHRS_M2 msg: 0.75 phi; -0.37 theta; -0.20 psi 2021-02-13T00:24:20Z- handleDepth()-Depth_Keller msg: 1613175860797 epoch sec; seqNo:12336; depth 5.37 2021-02-13T00:24:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175860.81 epoch sec; seqNo:12336; 4059078.69 north; 592496.25 east 2021-02-13T00:24:21Z- AHRS_M2 msg: 1613175861.18 epoch sec; seqNo:12240 2021-02-13T00:24:21Z- AHRS_M2 msg: 0.74 phi; -0.39 theta; -0.18 psi 2021-02-13T00:24:21Z- handleDepth()-Depth_Keller msg: 1613175861190 epoch sec; seqNo:12337; depth 5.48 2021-02-13T00:24:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175861.20 epoch sec; seqNo:12337; 4059079.07 north; 592496.24 east 2021-02-13T00:24:21Z- AHRS_M2 msg: 1613175861.59 epoch sec; seqNo:12241 2021-02-13T00:24:21Z- AHRS_M2 msg: 0.73 phi; -0.41 theta; -0.17 psi 2021-02-13T00:24:21Z- handleDepth()-Depth_Keller msg: 1613175861599 epoch sec; seqNo:12338; depth 5.61 2021-02-13T00:24:21Z- handleDvl() - RDI_Pathfinder msg: 1613175861.63 epoch sec; seqNo:6068 2021-02-13T00:24:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.64 , 115.49 , 84.26 , 125.90 2021-02-13T00:24:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.53 , -0.16 , 0.12 2021-02-13T00:24:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175861.64 epoch sec; seqNo:12338; 4059079.07 north; 592496.24 east 2021-02-13T00:24:21Z- AHRS_M2 msg: 1613175861.99 epoch sec; seqNo:12242 2021-02-13T00:24:21Z- AHRS_M2 msg: 0.72 phi; -0.42 theta; -0.17 psi 2021-02-13T00:24:21Z- handleDepth()-Depth_Keller msg: 1613175861996 epoch sec; seqNo:12339; depth 5.74 2021-02-13T00:24:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175862.01 epoch sec; seqNo:12339; 4059079.45 north; 592496.24 east 2021-02-13T00:24:22Z- AHRS_M2 msg: 1613175862.40 epoch sec; seqNo:12243 2021-02-13T00:24:22Z- AHRS_M2 msg: 0.70 phi; -0.42 theta; -0.18 psi 2021-02-13T00:24:22Z- handleDepth()-Depth_Keller msg: 1613175862400 epoch sec; seqNo:12340; depth 5.83 2021-02-13T00:24:22Z- handleDvl() - RDI_Pathfinder msg: 1613175862.41 epoch sec; seqNo:6069 2021-02-13T00:24:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.11 , 110.96 , 78.88 , 134.76 2021-02-13T00:24:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.58 , 0.14 , 0.19 2021-02-13T00:24:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175862.42 epoch sec; seqNo:12340; 4059079.46 north; 592497.46 east 2021-02-13T00:24:22Z- handleDepth()-Depth_Keller msg: 1613175862821 epoch sec; seqNo:12341; depth 5.96 2021-02-13T00:24:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175862.83 epoch sec; seqNo:12341; 4059079.46 north; 592497.46 east 2021-02-13T00:24:23Z- AHRS_M2 msg: 1613175863.20 epoch sec; seqNo:12244 2021-02-13T00:24:23Z- AHRS_M2 msg: 0.67 phi; -0.40 theta; -0.20 psi 2021-02-13T00:24:23Z- handleDepth()-Depth_Keller msg: 1613175863209 epoch sec; seqNo:12342; depth 6.08 2021-02-13T00:24:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175863.22 epoch sec; seqNo:12342; 4059079.46 north; 592497.46 east 2021-02-13T00:24:23Z- AHRS_M2 msg: 1613175863.61 epoch sec; seqNo:12245 2021-02-13T00:24:23Z- AHRS_M2 msg: 0.66 phi; -0.38 theta; -0.21 psi 2021-02-13T00:24:23Z- handleDepth()-Depth_Keller msg: 1613175863617 epoch sec; seqNo:12343; depth 6.20 2021-02-13T00:24:23Z- handleDvl() - RDI_Pathfinder msg: 1613175863.63 epoch sec; seqNo:6070 2021-02-13T00:24:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 112.58 , 82.84 , 134.89 2021-02-13T00:24:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.23 , 0.11 2021-02-13T00:24:23Z- LcmTrn::handleMessages() - handling 42 messages took 0.75 ms and 127 calls... 2021-02-13T00:24:23Z- waiting for 1613175861.39, time is 1613175863.63 2021-02-13T00:24:23Z- LcmTrn::updateTrn() equating pose 1613175863.63 and meas times 1613175863.63 2021-02-13T00:24:23Z- LcmTrn::updateTrn() >>>> pose time 1613175863.63, meas time 1613175863.63 <<<< 2021-02-13T00:24:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175863.63 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:24:23Z- TerrainNav::motionUpdate() - currEstimate = (4059079, 592497.46, 6.20) 2021-02-13T00:24:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:24:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:24:23Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175863.63 sec.; No relevant navigation data available 2021-02-13T00:24:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:24:23Z- TNavBF::mmse selected from filter -1 2021-02-13T00:24:23Z- variances([0], [2], and psi_berg) = 704.454780 833.000339 0.000000 2021-02-13T00:24:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:24:23Z- LcmTrn::publishEstimates() - MLE : 4059078.96 592545.96 6.20 2021-02-13T00:24:23Z- LcmTrn::publishEstimates() - MMSE : 4059079.46 592497.46 6.20 2021-02-13T00:24:23Z- LcmTrn::publishEstimates() - COVAR: 704.45 833.00 1.53 0.00 2021-02-13T00:24:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175863.64 epoch sec; seqNo:12343; 4059079.84 north; 592497.46 east 2021-02-13T00:24:24Z- AHRS_M2 msg: 1613175864.01 epoch sec; seqNo:12246 2021-02-13T00:24:24Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.22 psi 2021-02-13T00:24:24Z- handleDepth()-Depth_Keller msg: 1613175864017 epoch sec; seqNo:12344; depth 6.35 2021-02-13T00:24:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175864.03 epoch sec; seqNo:12344; 4059079.84 north; 592497.46 east 2021-02-13T00:24:24Z- AHRS_M2 msg: 1613175864.42 epoch sec; seqNo:12247 2021-02-13T00:24:24Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.22 psi 2021-02-13T00:24:24Z- handleDepth()-Depth_Keller msg: 1613175864426 epoch sec; seqNo:12345; depth 6.48 2021-02-13T00:24:24Z- handleDvl() - RDI_Pathfinder msg: 1613175864.44 epoch sec; seqNo:6071 2021-02-13T00:24:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.51 , 113.87 , 82.63 , 134.69 2021-02-13T00:24:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , 0.11 , -0.01 2021-02-13T00:24:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175864.45 epoch sec; seqNo:12345; 4059080.22 north; 592497.45 east 2021-02-13T00:24:24Z- AHRS_M2 msg: 1613175864.82 epoch sec; seqNo:12248 2021-02-13T00:24:24Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.23 psi 2021-02-13T00:24:24Z- handleDepth()-Depth_Keller msg: 1613175864824 epoch sec; seqNo:12346; depth 6.64 2021-02-13T00:24:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175864.84 epoch sec; seqNo:12346; 4059080.61 north; 592498.67 east 2021-02-13T00:24:25Z- AHRS_M2 msg: 1613175865.22 epoch sec; seqNo:12249 2021-02-13T00:24:25Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.22 psi 2021-02-13T00:24:25Z- handleDepth()-Depth_Keller msg: 1613175865233 epoch sec; seqNo:12347; depth 6.77 2021-02-13T00:24:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175865.24 epoch sec; seqNo:12347; 4059080.99 north; 592498.66 east 2021-02-13T00:24:25Z- AHRS_M2 msg: 1613175865.63 epoch sec; seqNo:12250 2021-02-13T00:24:25Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.22 psi 2021-02-13T00:24:25Z- handleDepth()-Depth_Keller msg: 1613175865633 epoch sec; seqNo:12348; depth 6.92 2021-02-13T00:24:25Z- handleDvl() - RDI_Pathfinder msg: 1613175865.64 epoch sec; seqNo:6072 2021-02-13T00:24:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.51 , 115.36 , 84.13 , 127.25 2021-02-13T00:24:25Z- handleDvl() - RDI_Pathfinder msg: velocities 1.11 , 0.00 , -0.15 2021-02-13T00:24:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175865.65 epoch sec; seqNo:12348; 4059081.37 north; 592498.66 east 2021-02-13T00:24:26Z- AHRS_M2 msg: 1613175866.03 epoch sec; seqNo:12251 2021-02-13T00:24:26Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.22 psi 2021-02-13T00:24:26Z- handleDepth()-Depth_Keller msg: 1613175866036 epoch sec; seqNo:12349; depth 7.06 2021-02-13T00:24:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175866.05 epoch sec; seqNo:12349; 4059081.75 north; 592498.66 east 2021-02-13T00:24:26Z- AHRS_M2 msg: 1613175866.44 epoch sec; seqNo:12252 2021-02-13T00:24:26Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.21 psi 2021-02-13T00:24:26Z- handleDepth()-Depth_Keller msg: 1613175866445 epoch sec; seqNo:12350; depth 7.21 2021-02-13T00:24:26Z- handleDvl() - RDI_Pathfinder msg: 1613175866.46 epoch sec; seqNo:6073 2021-02-13T00:24:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.65 , 112.45 , 82.71 , 127.32 2021-02-13T00:24:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.11 , -0.25 2021-02-13T00:24:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175866.47 epoch sec; seqNo:12350; 4059082.14 north; 592499.87 east 2021-02-13T00:24:26Z- AHRS_M2 msg: 1613175866.84 epoch sec; seqNo:12253 2021-02-13T00:24:26Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.21 psi 2021-02-13T00:24:26Z- handleDepth()-Depth_Keller msg: 1613175866844 epoch sec; seqNo:12351; depth 7.37 2021-02-13T00:24:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175866.86 epoch sec; seqNo:12351; 4059082.52 north; 592499.87 east 2021-02-13T00:24:27Z- AHRS_M2 msg: 1613175867.24 epoch sec; seqNo:12254 2021-02-13T00:24:27Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.21 psi 2021-02-13T00:24:27Z- handleDepth()-Depth_Keller msg: 1613175867250 epoch sec; seqNo:12352; depth 7.48 2021-02-13T00:24:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175867.27 epoch sec; seqNo:12352; 4059082.52 north; 592499.87 east 2021-02-13T00:24:27Z- AHRS_M2 msg: 1613175867.65 epoch sec; seqNo:12255 2021-02-13T00:24:27Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.21 psi 2021-02-13T00:24:27Z- handleDepth()-Depth_Keller msg: 1613175867658 epoch sec; seqNo:12353; depth 7.59 2021-02-13T00:24:27Z- handleDvl() - RDI_Pathfinder msg: 1613175867.67 epoch sec; seqNo:6074 2021-02-13T00:24:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.43 , 109.74 , 79.69 , 118.67 2021-02-13T00:24:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.23 , -0.18 2021-02-13T00:24:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175867.68 epoch sec; seqNo:12353; 4059082.90 north; 592499.86 east 2021-02-13T00:24:28Z- AHRS_M2 msg: 1613175868.05 epoch sec; seqNo:12256 2021-02-13T00:24:28Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.22 psi 2021-02-13T00:24:28Z- handleDepth()-Depth_Keller msg: 1613175868056 epoch sec; seqNo:12354; depth 7.71 2021-02-13T00:24:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175868.07 epoch sec; seqNo:12354; 4059083.28 north; 592499.86 east 2021-02-13T00:24:28Z- AHRS_M2 msg: 1613175868.46 epoch sec; seqNo:12257 2021-02-13T00:24:28Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.23 psi 2021-02-13T00:24:28Z- handleDepth()-Depth_Keller msg: 1613175868460 epoch sec; seqNo:12355; depth 7.83 2021-02-13T00:24:28Z- handleDvl() - RDI_Pathfinder msg: 1613175868.47 epoch sec; seqNo:6075 2021-02-13T00:24:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.28 , 109.27 , 0.00 , 116.03 2021-02-13T00:24:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.23 , 0.09 2021-02-13T00:24:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175868.48 epoch sec; seqNo:12355; 4059083.66 north; 592499.86 east 2021-02-13T00:24:28Z- LcmTrn::handleMessages() - handling 42 messages took 0.64 ms and 129 calls... 2021-02-13T00:24:28Z- waiting for 1613175866.63, time is 1613175868.48 2021-02-13T00:24:28Z- LcmTrn::updateTrn() equating pose 1613175868.48 and meas times 1613175868.48 2021-02-13T00:24:28Z- LcmTrn::updateTrn() >>>> pose time 1613175868.48, meas time 1613175868.48 <<<< 2021-02-13T00:24:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175868.48 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:24:28Z- TerrainNav::motionUpdate() - currEstimate = (4059084, 592499.86, 7.83) 2021-02-13T00:24:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:24:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:24:28Z- TNavBF::Projecting Measurements 2021-02-13T00:24:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:24:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:24:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:24:28Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:24:28Z- TNavFilter::3 good beams. 2021-02-13T00:24:28Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:24:28Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:24:28Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:24:28Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:24:28Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:24:28Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:24:28Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:24:28Z- TNavBF::mmse selected from filter -1 2021-02-13T00:24:28Z- TerrainMapOctree: (vehN, vehE ) = (4059083.66, 592499.86). 2021-02-13T00:24:28Z- TerrainMapOctree: Min Distance = 2784.97. 2021-02-13T00:24:28Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:24:28Z- There is no specific good beam that all particles have in common. 2021-02-13T00:24:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:24:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.78 K: 0.115 ExpectedMeasDiff: nan currMeas.covariance: 11.91 2021-02-13T00:24:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:24:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.57 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 19.10 2021-02-13T00:24:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:24:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.46 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 21.54 2021-02-13T00:24:28Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:24:28Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:24:28Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:24:28Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:24:28Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:24:28Z- Filter 0 No beams used in subcloud update 2021-02-13T00:24:28Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:24:28Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:24:28Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 704.4 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T00:24:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175868.48 sec. 2021-02-13T00:24:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:24:28Z- TNavBF::mmse selected from filter -1 2021-02-13T00:24:28Z- variances([0], [2], and psi_berg) = 704.392384 833.000304 0.000000 2021-02-13T00:24:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:24:28Z- LcmTrn::publishEstimates() - MLE : 4059083.16 592548.36 7.83 2021-02-13T00:24:28Z- LcmTrn::publishEstimates() - MMSE : 4059083.66 592499.86 7.83 2021-02-13T00:24:28Z- LcmTrn::publishEstimates() - COVAR: 704.39 833.00 1.38 0.00 2021-02-13T00:24:28Z- AHRS_M2 msg: 1613175868.86 epoch sec; seqNo:12258 2021-02-13T00:24:28Z- AHRS_M2 msg: 0.57 phi; -0.30 theta; -0.24 psi 2021-02-13T00:24:28Z- handleDepth()-Depth_Keller msg: 1613175868865 epoch sec; seqNo:12356; depth 7.92 2021-02-13T00:24:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175868.88 epoch sec; seqNo:12356; 4059083.66 north; 592499.86 east 2021-02-13T00:24:29Z- AHRS_M2 msg: 1613175869.26 epoch sec; seqNo:12259 2021-02-13T00:24:29Z- AHRS_M2 msg: 0.56 phi; -0.30 theta; -0.25 psi 2021-02-13T00:24:29Z- handleDepth()-Depth_Keller msg: 1613175869270 epoch sec; seqNo:12357; depth 8.02 2021-02-13T00:24:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175869.28 epoch sec; seqNo:12357; 4059084.03 north; 592499.85 east 2021-02-13T00:24:29Z- AHRS_M2 msg: 1613175869.67 epoch sec; seqNo:12260 2021-02-13T00:24:29Z- AHRS_M2 msg: 0.54 phi; -0.30 theta; -0.25 psi 2021-02-13T00:24:29Z- handleDepth()-Depth_Keller msg: 1613175869672 epoch sec; seqNo:12358; depth 8.14 2021-02-13T00:24:29Z- handleDvl() - RDI_Pathfinder msg: 1613175869.69 epoch sec; seqNo:6076 2021-02-13T00:24:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.88 , 114.14 , 82.91 , 118.60 2021-02-13T00:24:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.06 , 0.27 2021-02-13T00:24:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175869.70 epoch sec; seqNo:12358; 4059084.41 north; 592499.85 east 2021-02-13T00:24:30Z- AHRS_M2 msg: 1613175870.07 epoch sec; seqNo:12261 2021-02-13T00:24:30Z- AHRS_M2 msg: 0.54 phi; -0.29 theta; -0.24 psi 2021-02-13T00:24:30Z- handleDepth()-Depth_Keller msg: 1613175870076 epoch sec; seqNo:12359; depth 8.25 2021-02-13T00:24:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175870.09 epoch sec; seqNo:12359; 4059084.41 north; 592499.85 east 2021-02-13T00:24:30Z- AHRS_M2 msg: 1613175870.48 epoch sec; seqNo:12262 2021-02-13T00:24:30Z- AHRS_M2 msg: 0.54 phi; -0.29 theta; -0.23 psi 2021-02-13T00:24:30Z- handleDepth()-Depth_Keller msg: 1613175870485 epoch sec; seqNo:12360; depth 8.36 2021-02-13T00:24:30Z- handleDvl() - RDI_Pathfinder msg: 1613175870.50 epoch sec; seqNo:6077 2021-02-13T00:24:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 109.14 , 124.01 , 118.60 2021-02-13T00:24:30Z- handleDvl() - RDI_Pathfinder msg: velocities 1.98 , -1.00 , -0.46 2021-02-13T00:24:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175870.51 epoch sec; seqNo:12360; 4059085.55 north; 592499.84 east 2021-02-13T00:24:30Z- AHRS_M2 msg: 1613175870.88 epoch sec; seqNo:12263 2021-02-13T00:24:30Z- AHRS_M2 msg: 0.54 phi; -0.28 theta; -0.22 psi 2021-02-13T00:24:30Z- handleDepth()-Depth_Keller msg: 1613175870886 epoch sec; seqNo:12361; depth 8.48 2021-02-13T00:24:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175870.90 epoch sec; seqNo:12361; 4059086.31 north; 592499.83 east 2021-02-13T00:24:31Z- AHRS_M2 msg: 1613175871.28 epoch sec; seqNo:12264 2021-02-13T00:24:31Z- AHRS_M2 msg: 0.56 phi; -0.28 theta; -0.20 psi 2021-02-13T00:24:31Z- handleDepth()-Depth_Keller msg: 1613175871293 epoch sec; seqNo:12362; depth 8.58 2021-02-13T00:24:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175871.31 epoch sec; seqNo:12362; 4059087.06 north; 592499.82 east 2021-02-13T00:24:31Z- AHRS_M2 msg: 1613175871.69 epoch sec; seqNo:12265 2021-02-13T00:24:31Z- AHRS_M2 msg: 0.57 phi; -0.27 theta; -0.19 psi 2021-02-13T00:24:31Z- handleDepth()-Depth_Keller msg: 1613175871692 epoch sec; seqNo:12363; depth 8.69 2021-02-13T00:24:31Z- handleDvl() - RDI_Pathfinder msg: 1613175871.70 epoch sec; seqNo:6078 2021-02-13T00:24:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 110.62 , 82.37 , 115.09 2021-02-13T00:24:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , -0.04 2021-02-13T00:24:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175871.73 epoch sec; seqNo:12363; 4059087.46 north; 592501.04 east 2021-02-13T00:24:32Z- AHRS_M2 msg: 1613175872.09 epoch sec; seqNo:12266 2021-02-13T00:24:32Z- AHRS_M2 msg: 0.57 phi; -0.27 theta; -0.19 psi 2021-02-13T00:24:32Z- handleDepth()-Depth_Keller msg: 1613175872099 epoch sec; seqNo:12364; depth 8.80 2021-02-13T00:24:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175872.11 epoch sec; seqNo:12364; 4059087.84 north; 592501.03 east 2021-02-13T00:24:32Z- AHRS_M2 msg: 1613175872.50 epoch sec; seqNo:12267 2021-02-13T00:24:32Z- AHRS_M2 msg: 0.58 phi; -0.27 theta; -0.18 psi 2021-02-13T00:24:32Z- handleDepth()-Depth_Keller msg: 1613175872500 epoch sec; seqNo:12365; depth 8.90 2021-02-13T00:24:32Z- handleDvl() - RDI_Pathfinder msg: 1613175872.52 epoch sec; seqNo:6079 2021-02-13T00:24:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.95 , 105.28 , 82.98 , 111.23 2021-02-13T00:24:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.11 , -0.03 2021-02-13T00:24:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175872.53 epoch sec; seqNo:12365; 4059088.21 north; 592501.03 east 2021-02-13T00:24:32Z- AHRS_M2 msg: 1613175872.90 epoch sec; seqNo:12268 2021-02-13T00:24:32Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.16 psi 2021-02-13T00:24:32Z- handleDepth()-Depth_Keller msg: 1613175872917 epoch sec; seqNo:12366; depth 9.01 2021-02-13T00:24:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175872.95 epoch sec; seqNo:12366; 4059088.59 north; 592501.02 east 2021-02-13T00:24:33Z- AHRS_M2 msg: 1613175873.30 epoch sec; seqNo:12269 2021-02-13T00:24:33Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.14 psi 2021-02-13T00:24:33Z- handleDepth()-Depth_Keller msg: 1613175873309 epoch sec; seqNo:12367; depth 9.13 2021-02-13T00:24:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175873.32 epoch sec; seqNo:12367; 4059088.97 north; 592501.02 east 2021-02-13T00:24:33Z- AHRS_M2 msg: 1613175873.71 epoch sec; seqNo:12270 2021-02-13T00:24:33Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.13 psi 2021-02-13T00:24:33Z- handleDepth()-Depth_Keller msg: 1613175873714 epoch sec; seqNo:12368; depth 9.21 2021-02-13T00:24:33Z- LcmTrn::handleMessages() - handling 42 messages took 0.69 ms and 128 calls... 2021-02-13T00:24:33Z- waiting for 1613175871.48, time is 1613175873.71 2021-02-13T00:24:33Z- LcmTrn::updateTrn() >>>> pose time 1613175873.71, meas time 1613175872.52 <<<< 2021-02-13T00:24:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175872.52 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:24:33Z- TerrainNav::motionUpdate() - currEstimate = (4059089, 592501.02, 9.21) 2021-02-13T00:24:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:24:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:24:33Z- TNavBF::Projecting Measurements 2021-02-13T00:24:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:24:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:24:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:24:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:24:33Z- TNavFilter::4 good beams. 2021-02-13T00:24:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:24:33Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:24:33Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:24:33Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:24:33Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:24:33Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:24:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:24:33Z- TNavBF::mmse selected from filter -1 2021-02-13T00:24:33Z- TerrainMapOctree: (vehN, vehE ) = (4059087.76, 592500.76). 2021-02-13T00:24:33Z- TerrainMapOctree: Min Distance = 2781.21. 2021-02-13T00:24:33Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:24:33Z- There is no specific good beam that all particles have in common. 2021-02-13T00:24:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:24:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.59 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 11.82 2021-02-13T00:24:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:24:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.42 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 17.74 2021-02-13T00:24:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:24:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 11.02 2021-02-13T00:24:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:24:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 19.80 2021-02-13T00:24:33Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:24:33Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:24:33Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:24:33Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:24:33Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:24:33Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:24:33Z- Filter 0 No beams used in subcloud update 2021-02-13T00:24:33Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:24:33Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:24:33Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 704.3 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:24:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175872.52 sec. 2021-02-13T00:24:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:24:33Z- TNavBF::mmse selected from filter -1 2021-02-13T00:24:33Z- variances([0], [2], and psi_berg) = 704.302657 833.000255 0.000000 2021-02-13T00:24:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:24:33Z- LcmTrn::publishEstimates() - MLE : 4059088.47 592549.52 9.21 2021-02-13T00:24:33Z- LcmTrn::publishEstimates() - MMSE : 4059088.97 592501.02 9.21 2021-02-13T00:24:33Z- LcmTrn::publishEstimates() - COVAR: 704.30 833.00 1.20 0.00 2021-02-13T00:24:33Z- handleDvl() - RDI_Pathfinder msg: 1613175873.73 epoch sec; seqNo:6080 2021-02-13T00:24:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 102.78 , 81.15 , 112.25 2021-02-13T00:24:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , 0.11 2021-02-13T00:24:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175873.74 epoch sec; seqNo:12368; 4059088.97 north; 592501.02 east 2021-02-13T00:24:34Z- AHRS_M2 msg: 1613175874.11 epoch sec; seqNo:12271 2021-02-13T00:24:34Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.12 psi 2021-02-13T00:24:34Z- handleDepth()-Depth_Keller msg: 1613175874117 epoch sec; seqNo:12369; depth 9.33 2021-02-13T00:24:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175874.14 epoch sec; seqNo:12369; 4059089.35 north; 592501.02 east 2021-02-13T00:24:34Z- AHRS_M2 msg: 1613175874.52 epoch sec; seqNo:12272 2021-02-13T00:24:34Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.11 psi 2021-02-13T00:24:34Z- handleDepth()-Depth_Keller msg: 1613175874520 epoch sec; seqNo:12370; depth 9.41 2021-02-13T00:24:34Z- handleDvl() - RDI_Pathfinder msg: 1613175874.53 epoch sec; seqNo:6081 2021-02-13T00:24:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.91 , 100.08 , 79.28 , 105.49 2021-02-13T00:24:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.12 , 0.13 2021-02-13T00:24:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175874.56 epoch sec; seqNo:12370; 4059089.74 north; 592502.23 east 2021-02-13T00:24:34Z- AHRS_M2 msg: 1613175874.92 epoch sec; seqNo:12273 2021-02-13T00:24:34Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.11 psi 2021-02-13T00:24:34Z- handleDepth()-Depth_Keller msg: 1613175874932 epoch sec; seqNo:12371; depth 9.51 2021-02-13T00:24:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175874.94 epoch sec; seqNo:12371; 4059089.74 north; 592502.23 east 2021-02-13T00:24:35Z- AHRS_M2 msg: 1613175875.35 epoch sec; seqNo:12274 2021-02-13T00:24:35Z- AHRS_M2 msg: 0.61 phi; -0.28 theta; -0.10 psi 2021-02-13T00:24:35Z- handleDepth()-Depth_Keller msg: 1613175875357 epoch sec; seqNo:12372; depth 9.60 2021-02-13T00:24:35Z- handleDvl() - RDI_Pathfinder msg: 1613175875.37 epoch sec; seqNo:6082 2021-02-13T00:24:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.18 , 99.00 , 80.07 , 111.16 2021-02-13T00:24:35Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.15 , -0.00 2021-02-13T00:24:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175875.39 epoch sec; seqNo:12372; 4059090.12 north; 592502.23 east 2021-02-13T00:24:35Z- AHRS_M2 msg: 1613175875.73 epoch sec; seqNo:12275 2021-02-13T00:24:35Z- AHRS_M2 msg: 0.62 phi; -0.28 theta; -0.10 psi 2021-02-13T00:24:35Z- handleDepth()-Depth_Keller msg: 1613175875732 epoch sec; seqNo:12373; depth 9.71 2021-02-13T00:24:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175875.75 epoch sec; seqNo:12373; 4059090.50 north; 592502.22 east 2021-02-13T00:24:36Z- AHRS_M2 msg: 1613175876.13 epoch sec; seqNo:12276 2021-02-13T00:24:36Z- AHRS_M2 msg: 0.63 phi; -0.28 theta; -0.09 psi 2021-02-13T00:24:36Z- handleDepth()-Depth_Keller msg: 1613175876140 epoch sec; seqNo:12374; depth 9.78 2021-02-13T00:24:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175876.15 epoch sec; seqNo:12374; 4059090.50 north; 592502.22 east 2021-02-13T00:24:36Z- AHRS_M2 msg: 1613175876.54 epoch sec; seqNo:12277 2021-02-13T00:24:36Z- AHRS_M2 msg: 0.65 phi; -0.28 theta; -0.10 psi 2021-02-13T00:24:36Z- handleDepth()-Depth_Keller msg: 1613175876546 epoch sec; seqNo:12375; depth 9.90 2021-02-13T00:24:36Z- handleDvl() - RDI_Pathfinder msg: 1613175876.56 epoch sec; seqNo:6083 2021-02-13T00:24:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.04 , 97.51 , 79.93 , 111.03 2021-02-13T00:24:36Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.07 , -0.17 2021-02-13T00:24:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175876.61 epoch sec; seqNo:12375; 4059090.89 north; 592503.44 east 2021-02-13T00:24:36Z- AHRS_M2 msg: 1613175876.94 epoch sec; seqNo:12278 2021-02-13T00:24:36Z- AHRS_M2 msg: 0.66 phi; -0.28 theta; -0.10 psi 2021-02-13T00:24:36Z- handleDepth()-Depth_Keller msg: 1613175876948 epoch sec; seqNo:12376; depth 10.00 2021-02-13T00:24:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175876.98 epoch sec; seqNo:12376; 4059091.27 north; 592503.44 east 2021-02-13T00:24:37Z- AHRS_M2 msg: 1613175877.34 epoch sec; seqNo:12279 2021-02-13T00:24:37Z- AHRS_M2 msg: 0.67 phi; -0.29 theta; -0.10 psi 2021-02-13T00:24:37Z- handleDepth()-Depth_Keller msg: 1613175877348 epoch sec; seqNo:12377; depth 10.13 2021-02-13T00:24:37Z- handleDvl() - RDI_Pathfinder msg: 1613175877.36 epoch sec; seqNo:6084 2021-02-13T00:24:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.51 , 77.24 , 109.68 2021-02-13T00:24:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.12 , -0.26 2021-02-13T00:24:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175877.39 epoch sec; seqNo:12377; 4059091.65 north; 592503.43 east 2021-02-13T00:24:37Z- AHRS_M2 msg: 1613175877.76 epoch sec; seqNo:12280 2021-02-13T00:24:37Z- AHRS_M2 msg: 0.68 phi; -0.30 theta; -0.13 psi 2021-02-13T00:24:37Z- handleDepth()-Depth_Keller msg: 1613175877759 epoch sec; seqNo:12378; depth 10.23 2021-02-13T00:24:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175877.77 epoch sec; seqNo:12378; 4059092.03 north; 592503.43 east 2021-02-13T00:24:38Z- AHRS_M2 msg: 1613175878.16 epoch sec; seqNo:12281 2021-02-13T00:24:38Z- AHRS_M2 msg: 0.68 phi; -0.31 theta; -0.13 psi 2021-02-13T00:24:38Z- handleDepth()-Depth_Keller msg: 1613175878162 epoch sec; seqNo:12379; depth 10.33 2021-02-13T00:24:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175878.17 epoch sec; seqNo:12379; 4059092.41 north; 592503.42 east 2021-02-13T00:24:38Z- AHRS_M2 msg: 1613175878.56 epoch sec; seqNo:12282 2021-02-13T00:24:38Z- AHRS_M2 msg: 0.68 phi; -0.31 theta; -0.14 psi 2021-02-13T00:24:38Z- handleDepth()-Depth_Keller msg: 1613175878566 epoch sec; seqNo:12380; depth 10.45 2021-02-13T00:24:38Z- handleDvl() - RDI_Pathfinder msg: 1613175878.59 epoch sec; seqNo:6085 2021-02-13T00:24:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 100.21 , 78.46 , 112.38 2021-02-13T00:24:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.23 , -0.18 2021-02-13T00:24:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175878.60 epoch sec; seqNo:12380; 4059092.80 north; 592504.64 east 2021-02-13T00:24:38Z- LcmTrn::handleMessages() - handling 43 messages took 0.70 ms and 129 calls... 2021-02-13T00:24:38Z- waiting for 1613175876.71, time is 1613175878.60 2021-02-13T00:24:38Z- LcmTrn::updateTrn() equating pose 1613175878.60 and meas times 1613175878.60 2021-02-13T00:24:38Z- LcmTrn::updateTrn() >>>> pose time 1613175878.60, meas time 1613175878.60 <<<< 2021-02-13T00:24:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175878.60 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:24:38Z- TerrainNav::motionUpdate() - currEstimate = (4059093, 592504.64, 10.45) 2021-02-13T00:24:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:24:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:24:38Z- TNavBF::Projecting Measurements 2021-02-13T00:24:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:24:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:24:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:24:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:24:38Z- TNavFilter::4 good beams. 2021-02-13T00:24:38Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:24:38Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:24:38Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:24:38Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:24:38Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:24:38Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:24:38Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:24:38Z- TNavBF::mmse selected from filter -1 2021-02-13T00:24:38Z- TerrainMapOctree: (vehN, vehE ) = (4059092.80, 592504.64). 2021-02-13T00:24:38Z- TerrainMapOctree: Min Distance = 2774.85. 2021-02-13T00:24:38Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:24:38Z- There is no specific good beam that all particles have in common. 2021-02-13T00:24:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:24:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.47 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 12.03 2021-02-13T00:24:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:24:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 16.07 2021-02-13T00:24:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:24:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T00:24:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:24:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 20.21 2021-02-13T00:24:38Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:24:38Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:24:38Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:24:38Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:24:38Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:24:38Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:24:38Z- Filter 0 No beams used in subcloud update 2021-02-13T00:24:38Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:24:38Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:24:38Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 704.2 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:24:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175878.60 sec. 2021-02-13T00:24:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:24:38Z- TNavBF::mmse selected from filter -1 2021-02-13T00:24:38Z- variances([0], [2], and psi_berg) = 704.237593 833.000219 0.000000 2021-02-13T00:24:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:24:38Z- LcmTrn::publishEstimates() - MLE : 4059092.30 592553.14 10.45 2021-02-13T00:24:38Z- LcmTrn::publishEstimates() - MMSE : 4059092.80 592504.64 10.45 2021-02-13T00:24:38Z- LcmTrn::publishEstimates() - COVAR: 704.24 833.00 1.06 0.00 2021-02-13T00:24:38Z- AHRS_M2 msg: 1613175878.97 epoch sec; seqNo:12283 2021-02-13T00:24:38Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.14 psi 2021-02-13T00:24:38Z- handleDepth()-Depth_Keller msg: 1613175878971 epoch sec; seqNo:12381; depth 10.56 2021-02-13T00:24:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175878.98 epoch sec; seqNo:12381; 4059093.18 north; 592504.64 east 2021-02-13T00:24:39Z- AHRS_M2 msg: 1613175879.37 epoch sec; seqNo:12284 2021-02-13T00:24:39Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.15 psi 2021-02-13T00:24:39Z- handleDepth()-Depth_Keller msg: 1613175879381 epoch sec; seqNo:12382; depth 10.66 2021-02-13T00:24:39Z- handleDvl() - RDI_Pathfinder msg: 1613175879.39 epoch sec; seqNo:6086 2021-02-13T00:24:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.86 , 76.02 , 111.03 2021-02-13T00:24:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.19 , 0.03 2021-02-13T00:24:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175879.40 epoch sec; seqNo:12382; 4059093.18 north; 592504.64 east 2021-02-13T00:24:39Z- AHRS_M2 msg: 1613175879.77 epoch sec; seqNo:12285 2021-02-13T00:24:39Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.16 psi 2021-02-13T00:24:39Z- handleDepth()-Depth_Keller msg: 1613175879776 epoch sec; seqNo:12383; depth 10.78 2021-02-13T00:24:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:24:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:24:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:24:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:24:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:24:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:24:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:24:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:24:39Z- handleDvl() - RDI_Pathfinder msg: 1613175879.78 epoch sec; seqNo:6087 2021-02-13T00:24:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:24:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.19 , 0.03 2021-02-13T00:24:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175879.80 epoch sec; seqNo:12383; 4059093.56 north; 592504.63 east 2021-02-13T00:24:40Z- AHRS_M2 msg: 1613175880.18 epoch sec; seqNo:12286 2021-02-13T00:24:40Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.17 psi 2021-02-13T00:24:40Z- handleDepth()-Depth_Keller msg: 1613175880180 epoch sec; seqNo:12384; depth 10.89 2021-02-13T00:24:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175880.19 epoch sec; seqNo:12384; 4059093.94 north; 592504.63 east 2021-02-13T00:24:40Z- AHRS_M2 msg: 1613175880.58 epoch sec; seqNo:12287 2021-02-13T00:24:40Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.18 psi 2021-02-13T00:24:40Z- handleDepth()-Depth_Keller msg: 1613175880584 epoch sec; seqNo:12385; depth 10.99 2021-02-13T00:24:40Z- handleDvl() - RDI_Pathfinder msg: 1613175880.60 epoch sec; seqNo:6088 2021-02-13T00:24:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.62 , 101.50 , 78.51 , 113.67 2021-02-13T00:24:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.09 , 0.14 2021-02-13T00:24:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175880.61 epoch sec; seqNo:12385; 4059094.32 north; 592504.62 east 2021-02-13T00:24:41Z- AHRS_M2 msg: 1613175881.04 epoch sec; seqNo:12288 2021-02-13T00:24:41Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.20 psi 2021-02-13T00:24:41Z- handleDepth()-Depth_Keller msg: 1613175881044 epoch sec; seqNo:12386; depth 11.10 2021-02-13T00:24:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:24:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:24:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:24:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:24:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:24:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:24:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:24:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:24:41Z- handleDvl() - RDI_Pathfinder msg: 1613175881.05 epoch sec; seqNo:6089 2021-02-13T00:24:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:24:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.09 , 0.14 2021-02-13T00:24:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175881.06 epoch sec; seqNo:12386; 4059094.32 north; 592504.62 east 2021-02-13T00:24:41Z- AHRS_M2 msg: 1613175881.39 epoch sec; seqNo:12289 2021-02-13T00:24:41Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.20 psi 2021-02-13T00:24:41Z- handleDepth()-Depth_Keller msg: 1613175881402 epoch sec; seqNo:12387; depth 11.22 2021-02-13T00:24:41Z- handleDvl() - RDI_Pathfinder msg: 1613175881.41 epoch sec; seqNo:6090 2021-02-13T00:24:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.28 , 102.85 , 76.01 , 115.02 2021-02-13T00:24:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.01 , 0.13 2021-02-13T00:24:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175881.42 epoch sec; seqNo:12387; 4059094.69 north; 592504.62 east 2021-02-13T00:24:41Z- AHRS_M2 msg: 1613175881.79 epoch sec; seqNo:12290 2021-02-13T00:24:41Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.21 psi 2021-02-13T00:24:41Z- handleDepth()-Depth_Keller msg: 1613175881796 epoch sec; seqNo:12388; depth 11.33 2021-02-13T00:24:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175881.81 epoch sec; seqNo:12388; 4059094.69 north; 592504.62 east 2021-02-13T00:24:42Z- AHRS_M2 msg: 1613175882.20 epoch sec; seqNo:12291 2021-02-13T00:24:42Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.22 psi 2021-02-13T00:24:42Z- handleDepth()-Depth_Keller msg: 1613175882200 epoch sec; seqNo:12389; depth 11.43 2021-02-13T00:24:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175882.21 epoch sec; seqNo:12389; 4059095.09 north; 592505.84 east 2021-02-13T00:24:42Z- AHRS_M2 msg: 1613175882.60 epoch sec; seqNo:12292 2021-02-13T00:24:42Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.23 psi 2021-02-13T00:24:42Z- handleDepth()-Depth_Keller msg: 1613175882613 epoch sec; seqNo:12390; depth 11.54 2021-02-13T00:24:42Z- handleDvl() - RDI_Pathfinder msg: 1613175882.62 epoch sec; seqNo:6091 2021-02-13T00:24:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 104.27 , 78.46 , 113.73 2021-02-13T00:24:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , 0.06 2021-02-13T00:24:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175882.63 epoch sec; seqNo:12390; 4059095.47 north; 592505.83 east 2021-02-13T00:24:43Z- AHRS_M2 msg: 1613175883.00 epoch sec; seqNo:12293 2021-02-13T00:24:43Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.24 psi 2021-02-13T00:24:43Z- handleDepth()-Depth_Keller msg: 1613175883008 epoch sec; seqNo:12391; depth 11.66 2021-02-13T00:24:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175883.03 epoch sec; seqNo:12391; 4059095.47 north; 592505.83 east 2021-02-13T00:24:43Z- AHRS_M2 msg: 1613175883.41 epoch sec; seqNo:12294 2021-02-13T00:24:43Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.23 psi 2021-02-13T00:24:43Z- handleDepth()-Depth_Keller msg: 1613175883411 epoch sec; seqNo:12392; depth 11.77 2021-02-13T00:24:43Z- handleDvl() - RDI_Pathfinder msg: 1613175883.42 epoch sec; seqNo:6092 2021-02-13T00:24:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.20 , 76.42 , 117.72 2021-02-13T00:24:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.03 , 0.05 2021-02-13T00:24:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175883.43 epoch sec; seqNo:12392; 4059095.84 north; 592505.83 east 2021-02-13T00:24:43Z- AHRS_M2 msg: 1613175883.81 epoch sec; seqNo:12295 2021-02-13T00:24:43Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.23 psi 2021-02-13T00:24:43Z- handleDepth()-Depth_Keller msg: 1613175883821 epoch sec; seqNo:12393; depth 11.88 2021-02-13T00:24:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175883.83 epoch sec; seqNo:12393; 4059096.22 north; 592505.83 east 2021-02-13T00:24:43Z- LcmTrn::handleMessages() - handling 46 messages took 0.70 ms and 132 calls... 2021-02-13T00:24:43Z- waiting for 1613175881.60, time is 1613175883.83 2021-02-13T00:24:43Z- LcmTrn::updateTrn() >>>> pose time 1613175883.83, meas time 1613175883.42 <<<< 2021-02-13T00:24:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175883.42 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:24:43Z- TerrainNav::motionUpdate() - currEstimate = (4059096, 592505.83, 11.88) 2021-02-13T00:24:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:24:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:24:43Z- TNavBF::Projecting Measurements 2021-02-13T00:24:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:24:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:24:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:24:43Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:24:43Z- TNavFilter::3 good beams. 2021-02-13T00:24:43Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:24:43Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:24:43Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:24:43Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:24:43Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:24:43Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:24:43Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:24:43Z- TNavBF::mmse selected from filter -1 2021-02-13T00:24:43Z- TerrainMapOctree: (vehN, vehE ) = (4059095.96, 592505.73). 2021-02-13T00:24:43Z- TerrainMapOctree: Min Distance = 2771.71. 2021-02-13T00:24:43Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:24:43Z- There is no specific good beam that all particles have in common. 2021-02-13T00:24:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:24:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 17.37 2021-02-13T00:24:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:24:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 9.34 2021-02-13T00:24:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:24:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 22.17 2021-02-13T00:24:43Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:24:43Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:24:43Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:24:43Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:24:43Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:24:43Z- Filter 0 No beams used in subcloud update 2021-02-13T00:24:43Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:24:43Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:24:43Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 704.2 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:24:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175883.42 sec. 2021-02-13T00:24:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:24:43Z- TNavBF::mmse selected from filter -1 2021-02-13T00:24:43Z- variances([0], [2], and psi_berg) = 704.170782 833.000183 0.000000 2021-02-13T00:24:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:24:43Z- LcmTrn::publishEstimates() - MLE : 4059095.72 592476.33 11.88 2021-02-13T00:24:43Z- LcmTrn::publishEstimates() - MMSE : 4059096.22 592505.83 11.88 2021-02-13T00:24:43Z- LcmTrn::publishEstimates() - COVAR: 704.17 833.00 1.02 0.00 2021-02-13T00:24:44Z- AHRS_M2 msg: 1613175884.22 epoch sec; seqNo:12296 2021-02-13T00:24:44Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.22 psi 2021-02-13T00:24:44Z- handleDepth()-Depth_Keller msg: 1613175884223 epoch sec; seqNo:12394; depth 12.00 2021-02-13T00:24:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175884.23 epoch sec; seqNo:12394; 4059096.60 north; 592505.82 east 2021-02-13T00:24:44Z- AHRS_M2 msg: 1613175884.62 epoch sec; seqNo:12297 2021-02-13T00:24:44Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.21 psi 2021-02-13T00:24:44Z- handleDepth()-Depth_Keller msg: 1613175884624 epoch sec; seqNo:12395; depth 12.11 2021-02-13T00:24:44Z- handleDvl() - RDI_Pathfinder msg: 1613175884.64 epoch sec; seqNo:6093 2021-02-13T00:24:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.58 , 107.92 , 78.18 , 111.98 2021-02-13T00:24:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.07 , 0.04 2021-02-13T00:24:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175884.65 epoch sec; seqNo:12395; 4059096.99 north; 592507.04 east 2021-02-13T00:24:45Z- AHRS_M2 msg: 1613175885.02 epoch sec; seqNo:12298 2021-02-13T00:24:45Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.21 psi 2021-02-13T00:24:45Z- handleDepth()-Depth_Keller msg: 1613175885033 epoch sec; seqNo:12396; depth 12.24 2021-02-13T00:24:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175885.04 epoch sec; seqNo:12396; 4059096.99 north; 592507.04 east 2021-02-13T00:24:45Z- AHRS_M2 msg: 1613175885.43 epoch sec; seqNo:12299 2021-02-13T00:24:45Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-13T00:24:45Z- handleDepth()-Depth_Keller msg: 1613175885432 epoch sec; seqNo:12397; depth 12.37 2021-02-13T00:24:45Z- handleDvl() - RDI_Pathfinder msg: 1613175885.44 epoch sec; seqNo:6094 2021-02-13T00:24:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 104.20 , 78.51 , 113.67 2021-02-13T00:24:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , 0.02 2021-02-13T00:24:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175885.45 epoch sec; seqNo:12397; 4059097.37 north; 592507.03 east 2021-02-13T00:24:45Z- AHRS_M2 msg: 1613175885.83 epoch sec; seqNo:12300 2021-02-13T00:24:45Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-13T00:24:45Z- handleDepth()-Depth_Keller msg: 1613175885838 epoch sec; seqNo:12398; depth 12.49 2021-02-13T00:24:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175885.85 epoch sec; seqNo:12398; 4059097.75 north; 592507.03 east 2021-02-13T00:24:46Z- AHRS_M2 msg: 1613175886.24 epoch sec; seqNo:12301 2021-02-13T00:24:46Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-13T00:24:46Z- handleDepth()-Depth_Keller msg: 1613175886247 epoch sec; seqNo:12399; depth 12.61 2021-02-13T00:24:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175886.29 epoch sec; seqNo:12399; 4059098.13 north; 592507.03 east 2021-02-13T00:24:46Z- AHRS_M2 msg: 1613175886.64 epoch sec; seqNo:12302 2021-02-13T00:24:46Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-13T00:24:46Z- handleDepth()-Depth_Keller msg: 1613175886644 epoch sec; seqNo:12400; depth 12.71 2021-02-13T00:24:46Z- handleDvl() - RDI_Pathfinder msg: 1613175886.66 epoch sec; seqNo:6095 2021-02-13T00:24:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 104.20 , 74.37 , 115.02 2021-02-13T00:24:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , 0.00 2021-02-13T00:24:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175886.68 epoch sec; seqNo:12400; 4059098.13 north; 592507.03 east 2021-02-13T00:24:47Z- AHRS_M2 msg: 1613175887.04 epoch sec; seqNo:12303 2021-02-13T00:24:47Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-13T00:24:47Z- handleDepth()-Depth_Keller msg: 1613175887053 epoch sec; seqNo:12401; depth 12.85 2021-02-13T00:24:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175887.06 epoch sec; seqNo:12401; 4059098.52 north; 592508.24 east 2021-02-13T00:24:47Z- AHRS_M2 msg: 1613175887.45 epoch sec; seqNo:12304 2021-02-13T00:24:47Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-13T00:24:47Z- handleDepth()-Depth_Keller msg: 1613175887458 epoch sec; seqNo:12402; depth 12.97 2021-02-13T00:24:47Z- handleDvl() - RDI_Pathfinder msg: 1613175887.47 epoch sec; seqNo:6096 2021-02-13T00:24:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 102.92 , 76.42 , 116.44 2021-02-13T00:24:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , -0.01 2021-02-13T00:24:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175887.48 epoch sec; seqNo:12402; 4059098.90 north; 592508.24 east 2021-02-13T00:24:47Z- AHRS_M2 msg: 1613175887.85 epoch sec; seqNo:12305 2021-02-13T00:24:47Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-13T00:24:47Z- handleDepth()-Depth_Keller msg: 1613175887861 epoch sec; seqNo:12403; depth 13.09 2021-02-13T00:24:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175887.88 epoch sec; seqNo:12403; 4059099.28 north; 592508.23 east 2021-02-13T00:24:48Z- AHRS_M2 msg: 1613175888.26 epoch sec; seqNo:12306 2021-02-13T00:24:48Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.18 psi 2021-02-13T00:24:48Z- handleDepth()-Depth_Keller msg: 1613175888260 epoch sec; seqNo:12404; depth 13.22 2021-02-13T00:24:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175888.27 epoch sec; seqNo:12404; 4059099.28 north; 592508.23 east 2021-02-13T00:24:48Z- AHRS_M2 msg: 1613175888.66 epoch sec; seqNo:12307 2021-02-13T00:24:48Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.18 psi 2021-02-13T00:24:48Z- handleDepth()-Depth_Keller msg: 1613175888664 epoch sec; seqNo:12405; depth 13.31 2021-02-13T00:24:48Z- handleDvl() - RDI_Pathfinder msg: 1613175888.68 epoch sec; seqNo:6097 2021-02-13T00:24:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 100.15 , 78.51 , 112.32 2021-02-13T00:24:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , -0.02 2021-02-13T00:24:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175888.70 epoch sec; seqNo:12405; 4059099.66 north; 592508.23 east 2021-02-13T00:24:48Z- LcmTrn::handleMessages() - handling 41 messages took 0.65 ms and 128 calls... 2021-02-13T00:24:48Z- waiting for 1613175886.83, time is 1613175888.70 2021-02-13T00:24:48Z- LcmTrn::updateTrn() equating pose 1613175888.70 and meas times 1613175888.70 2021-02-13T00:24:48Z- LcmTrn::updateTrn() >>>> pose time 1613175888.70, meas time 1613175888.70 <<<< 2021-02-13T00:24:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175888.70 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:24:48Z- TerrainNav::motionUpdate() - currEstimate = (4059100, 592508.23, 13.31) 2021-02-13T00:24:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:24:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:24:48Z- TNavBF::Projecting Measurements 2021-02-13T00:24:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:24:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:24:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:24:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:24:48Z- TNavFilter::4 good beams. 2021-02-13T00:24:48Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:24:48Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:24:48Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:24:48Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:24:48Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:24:48Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:24:48Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:24:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:24:48Z- TerrainMapOctree: (vehN, vehE ) = (4059099.66, 592508.23). 2021-02-13T00:24:48Z- TerrainMapOctree: Min Distance = 2767.26. 2021-02-13T00:24:48Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:24:48Z- There is no specific good beam that all particles have in common. 2021-02-13T00:24:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:24:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 11.27 2021-02-13T00:24:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:24:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 16.05 2021-02-13T00:24:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:24:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:24:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:24:48Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 20.18 2021-02-13T00:24:48Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:24:49Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:24:49Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:24:49Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:24:49Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:24:49Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:24:49Z- Filter 0 No beams used in subcloud update 2021-02-13T00:24:49Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:24:49Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:24:49Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 704.1 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:24:49Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175888.70 sec. 2021-02-13T00:24:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:24:49Z- TNavBF::mmse selected from filter -1 2021-02-13T00:24:49Z- variances([0], [2], and psi_berg) = 704.112820 833.000151 0.000000 2021-02-13T00:24:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:24:49Z- LcmTrn::publishEstimates() - MLE : 4059099.16 592478.73 13.31 2021-02-13T00:24:49Z- LcmTrn::publishEstimates() - MMSE : 4059099.66 592508.23 13.31 2021-02-13T00:24:49Z- LcmTrn::publishEstimates() - COVAR: 704.11 833.00 0.93 0.00 2021-02-13T00:24:49Z- AHRS_M2 msg: 1613175889.06 epoch sec; seqNo:12308 2021-02-13T00:24:49Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.19 psi 2021-02-13T00:24:49Z- handleDepth()-Depth_Keller msg: 1613175889069 epoch sec; seqNo:12406; depth 13.46 2021-02-13T00:24:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175889.08 epoch sec; seqNo:12406; 4059100.04 north; 592508.23 east 2021-02-13T00:24:49Z- AHRS_M2 msg: 1613175889.47 epoch sec; seqNo:12309 2021-02-13T00:24:49Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.19 psi 2021-02-13T00:24:49Z- handleDepth()-Depth_Keller msg: 1613175889472 epoch sec; seqNo:12407; depth 13.57 2021-02-13T00:24:49Z- handleDvl() - RDI_Pathfinder msg: 1613175889.48 epoch sec; seqNo:6098 2021-02-13T00:24:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.28 , 73.96 , 113.80 2021-02-13T00:24:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , -0.05 2021-02-13T00:24:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175889.49 epoch sec; seqNo:12407; 4059100.04 north; 592508.23 east 2021-02-13T00:24:49Z- AHRS_M2 msg: 1613175889.87 epoch sec; seqNo:12310 2021-02-13T00:24:49Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.20 psi 2021-02-13T00:24:49Z- handleDepth()-Depth_Keller msg: 1613175889877 epoch sec; seqNo:12408; depth 13.69 2021-02-13T00:24:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175889.89 epoch sec; seqNo:12408; 4059100.43 north; 592509.44 east 2021-02-13T00:24:50Z- AHRS_M2 msg: 1613175890.28 epoch sec; seqNo:12311 2021-02-13T00:24:50Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.21 psi 2021-02-13T00:24:50Z- handleDepth()-Depth_Keller msg: 1613175890280 epoch sec; seqNo:12409; depth 13.79 2021-02-13T00:24:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175890.30 epoch sec; seqNo:12409; 4059100.81 north; 592509.44 east 2021-02-13T00:24:50Z- AHRS_M2 msg: 1613175890.68 epoch sec; seqNo:12312 2021-02-13T00:24:50Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.21 psi 2021-02-13T00:24:50Z- handleDepth()-Depth_Keller msg: 1613175890684 epoch sec; seqNo:12410; depth 13.90 2021-02-13T00:24:50Z- handleDvl() - RDI_Pathfinder msg: 1613175890.70 epoch sec; seqNo:6099 2021-02-13T00:24:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.50 , 101.43 , 78.45 , 113.60 2021-02-13T00:24:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.07 2021-02-13T00:24:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175890.72 epoch sec; seqNo:12410; 4059101.19 north; 592509.43 east 2021-02-13T00:24:51Z- AHRS_M2 msg: 1613175891.09 epoch sec; seqNo:12313 2021-02-13T00:24:51Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.21 psi 2021-02-13T00:24:51Z- handleDepth()-Depth_Keller msg: 1613175891089 epoch sec; seqNo:12411; depth 14.03 2021-02-13T00:24:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175891.10 epoch sec; seqNo:12411; 4059101.57 north; 592509.43 east 2021-02-13T00:24:51Z- AHRS_M2 msg: 1613175891.49 epoch sec; seqNo:12314 2021-02-13T00:24:51Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.22 psi 2021-02-13T00:24:51Z- handleDepth()-Depth_Keller msg: 1613175891492 epoch sec; seqNo:12412; depth 14.14 2021-02-13T00:24:51Z- handleDvl() - RDI_Pathfinder msg: 1613175891.51 epoch sec; seqNo:6100 2021-02-13T00:24:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.71 , 102.99 , 74.37 , 111.10 2021-02-13T00:24:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.12 , -0.03 2021-02-13T00:24:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175891.52 epoch sec; seqNo:12412; 4059101.57 north; 592509.43 east 2021-02-13T00:24:51Z- AHRS_M2 msg: 1613175891.89 epoch sec; seqNo:12315 2021-02-13T00:24:51Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.22 psi 2021-02-13T00:24:51Z- handleDepth()-Depth_Keller msg: 1613175891901 epoch sec; seqNo:12413; depth 14.27 2021-02-13T00:24:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175891.91 epoch sec; seqNo:12413; 4059101.95 north; 592509.43 east 2021-02-13T00:24:52Z- AHRS_M2 msg: 1613175892.30 epoch sec; seqNo:12316 2021-02-13T00:24:52Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.21 psi 2021-02-13T00:24:52Z- handleDepth()-Depth_Keller msg: 1613175892301 epoch sec; seqNo:12414; depth 14.38 2021-02-13T00:24:52Z- handleDvl() - RDI_Pathfinder msg: 1613175892.31 epoch sec; seqNo:6101 2021-02-13T00:24:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.41 , 74.78 , 112.52 2021-02-13T00:24:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.21 , 0.03 2021-02-13T00:24:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175892.32 epoch sec; seqNo:12414; 4059102.32 north; 592509.42 east 2021-02-13T00:24:52Z- AHRS_M2 msg: 1613175892.70 epoch sec; seqNo:12317 2021-02-13T00:24:52Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.20 psi 2021-02-13T00:24:52Z- handleDepth()-Depth_Keller msg: 1613175892713 epoch sec; seqNo:12415; depth 14.51 2021-02-13T00:24:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175892.72 epoch sec; seqNo:12415; 4059102.72 north; 592510.64 east 2021-02-13T00:24:53Z- AHRS_M2 msg: 1613175893.10 epoch sec; seqNo:12318 2021-02-13T00:24:53Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.19 psi 2021-02-13T00:24:53Z- handleDepth()-Depth_Keller msg: 1613175893109 epoch sec; seqNo:12416; depth 14.64 2021-02-13T00:24:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175893.13 epoch sec; seqNo:12416; 4059103.09 north; 592510.63 east 2021-02-13T00:24:53Z- AHRS_M2 msg: 1613175893.51 epoch sec; seqNo:12319 2021-02-13T00:24:53Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.18 psi 2021-02-13T00:24:53Z- handleDepth()-Depth_Keller msg: 1613175893512 epoch sec; seqNo:12417; depth 14.76 2021-02-13T00:24:53Z- handleDvl() - RDI_Pathfinder msg: 1613175893.52 epoch sec; seqNo:6102 2021-02-13T00:24:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.01 , 101.29 , 73.56 , 112.11 2021-02-13T00:24:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.13 , 0.09 2021-02-13T00:24:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175893.55 epoch sec; seqNo:12417; 4059103.47 north; 592510.63 east 2021-02-13T00:24:53Z- AHRS_M2 msg: 1613175893.91 epoch sec; seqNo:12320 2021-02-13T00:24:53Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.17 psi 2021-02-13T00:24:53Z- handleDepth()-Depth_Keller msg: 1613175893921 epoch sec; seqNo:12418; depth 14.88 2021-02-13T00:24:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175893.94 epoch sec; seqNo:12418; 4059103.47 north; 592510.63 east 2021-02-13T00:24:54Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-13T00:24:54Z- waiting for 1613175891.70, time is 1613175893.94 2021-02-13T00:24:54Z- LcmTrn::updateTrn() >>>> pose time 1613175893.94, meas time 1613175893.52 <<<< 2021-02-13T00:24:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175893.52 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:24:54Z- TerrainNav::motionUpdate() - currEstimate = (4059103, 592510.63, 14.88) 2021-02-13T00:24:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:24:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:24:54Z- TNavBF::Projecting Measurements 2021-02-13T00:24:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:24:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:24:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:24:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:24:54Z- TNavFilter::4 good beams. 2021-02-13T00:24:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:24:54Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:24:54Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:24:54Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:24:54Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:24:54Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:24:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:24:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:24:54Z- TerrainMapOctree: (vehN, vehE ) = (4059103.17, 592510.44). 2021-02-13T00:24:54Z- TerrainMapOctree: Min Distance = 2763.14. 2021-02-13T00:24:54Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:24:54Z- There is no specific good beam that all particles have in common. 2021-02-13T00:24:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:24:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 10.50 2021-02-13T00:24:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:24:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 16.42 2021-02-13T00:24:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:24:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 8.66 2021-02-13T00:24:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:24:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 20.11 2021-02-13T00:24:54Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:24:54Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:24:54Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:24:54Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:24:54Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:24:54Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:24:54Z- Filter 0 No beams used in subcloud update 2021-02-13T00:24:54Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:24:54Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:24:54Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 704.1 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:24:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175893.52 sec. 2021-02-13T00:24:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:24:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:24:54Z- variances([0], [2], and psi_berg) = 704.038347 833.000110 0.000000 2021-02-13T00:24:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:24:54Z- LcmTrn::publishEstimates() - MLE : 4059102.97 592481.13 14.88 2021-02-13T00:24:54Z- LcmTrn::publishEstimates() - MMSE : 4059103.47 592510.63 14.88 2021-02-13T00:24:54Z- LcmTrn::publishEstimates() - COVAR: 704.04 833.00 0.88 0.00 2021-02-13T00:24:54Z- AHRS_M2 msg: 1613175894.32 epoch sec; seqNo:12321 2021-02-13T00:24:54Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.16 psi 2021-02-13T00:24:54Z- handleDepth()-Depth_Keller msg: 1613175894329 epoch sec; seqNo:12419; depth 15.01 2021-02-13T00:24:54Z- handleDvl() - RDI_Pathfinder msg: 1613175894.35 epoch sec; seqNo:6103 2021-02-13T00:24:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 97.71 , 73.97 , 111.23 2021-02-13T00:24:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , 0.11 2021-02-13T00:24:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175894.36 epoch sec; seqNo:12419; 4059103.85 north; 592510.63 east 2021-02-13T00:24:54Z- AHRS_M2 msg: 1613175894.73 epoch sec; seqNo:12322 2021-02-13T00:24:54Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.15 psi 2021-02-13T00:24:54Z- handleDepth()-Depth_Keller msg: 1613175894730 epoch sec; seqNo:12420; depth 15.15 2021-02-13T00:24:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175894.75 epoch sec; seqNo:12420; 4059104.23 north; 592510.62 east 2021-02-13T00:24:55Z- AHRS_M2 msg: 1613175895.13 epoch sec; seqNo:12323 2021-02-13T00:24:55Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.15 psi 2021-02-13T00:24:55Z- handleDepth()-Depth_Keller msg: 1613175895136 epoch sec; seqNo:12421; depth 15.27 2021-02-13T00:24:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175895.15 epoch sec; seqNo:12421; 4059104.23 north; 592510.62 east 2021-02-13T00:24:55Z- AHRS_M2 msg: 1613175895.53 epoch sec; seqNo:12324 2021-02-13T00:24:55Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.15 psi 2021-02-13T00:24:55Z- handleDepth()-Depth_Keller msg: 1613175895533 epoch sec; seqNo:12422; depth 15.39 2021-02-13T00:24:55Z- handleDvl() - RDI_Pathfinder msg: 1613175895.55 epoch sec; seqNo:6104 2021-02-13T00:24:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 97.64 , 78.17 , 112.25 2021-02-13T00:24:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , 0.10 2021-02-13T00:24:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175895.55 epoch sec; seqNo:12422; 4059104.61 north; 592510.62 east 2021-02-13T00:24:55Z- AHRS_M2 msg: 1613175895.93 epoch sec; seqNo:12325 2021-02-13T00:24:55Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.15 psi 2021-02-13T00:24:55Z- handleDepth()-Depth_Keller msg: 1613175895938 epoch sec; seqNo:12423; depth 15.51 2021-02-13T00:24:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175895.95 epoch sec; seqNo:12423; 4059105.00 north; 592511.83 east 2021-02-13T00:24:56Z- AHRS_M2 msg: 1613175896.34 epoch sec; seqNo:12326 2021-02-13T00:24:56Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.16 psi 2021-02-13T00:24:56Z- handleDepth()-Depth_Keller msg: 1613175896340 epoch sec; seqNo:12424; depth 15.65 2021-02-13T00:24:56Z- handleDvl() - RDI_Pathfinder msg: 1613175896.36 epoch sec; seqNo:6105 2021-02-13T00:24:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 97.64 , 73.56 , 115.22 2021-02-13T00:24:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.06 , 0.04 2021-02-13T00:24:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175896.37 epoch sec; seqNo:12424; 4059105.00 north; 592511.83 east 2021-02-13T00:24:56Z- AHRS_M2 msg: 1613175896.74 epoch sec; seqNo:12327 2021-02-13T00:24:56Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.17 psi 2021-02-13T00:24:56Z- handleDepth()-Depth_Keller msg: 1613175896749 epoch sec; seqNo:12425; depth 15.79 2021-02-13T00:24:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175896.76 epoch sec; seqNo:12425; 4059105.38 north; 592511.83 east 2021-02-13T00:24:57Z- AHRS_M2 msg: 1613175897.15 epoch sec; seqNo:12328 2021-02-13T00:24:57Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.17 psi 2021-02-13T00:24:57Z- handleDepth()-Depth_Keller msg: 1613175897153 epoch sec; seqNo:12426; depth 15.91 2021-02-13T00:24:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175897.16 epoch sec; seqNo:12426; 4059105.76 north; 592511.83 east 2021-02-13T00:24:57Z- AHRS_M2 msg: 1613175897.55 epoch sec; seqNo:12329 2021-02-13T00:24:57Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.18 psi 2021-02-13T00:24:57Z- handleDepth()-Depth_Keller msg: 1613175897554 epoch sec; seqNo:12427; depth 16.05 2021-02-13T00:24:57Z- handleDvl() - RDI_Pathfinder msg: 1613175897.57 epoch sec; seqNo:6106 2021-02-13T00:24:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 99.06 , 73.15 , 116.64 2021-02-13T00:24:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.05 , -0.05 2021-02-13T00:24:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175897.58 epoch sec; seqNo:12427; 4059106.14 north; 592511.82 east 2021-02-13T00:24:57Z- AHRS_M2 msg: 1613175897.95 epoch sec; seqNo:12330 2021-02-13T00:24:57Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.18 psi 2021-02-13T00:24:57Z- handleDepth()-Depth_Keller msg: 1613175897961 epoch sec; seqNo:12428; depth 16.18 2021-02-13T00:24:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175897.98 epoch sec; seqNo:12428; 4059106.15 north; 592513.04 east 2021-02-13T00:24:58Z- AHRS_M2 msg: 1613175898.36 epoch sec; seqNo:12331 2021-02-13T00:24:58Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.18 psi 2021-02-13T00:24:58Z- handleDepth()-Depth_Keller msg: 1613175898360 epoch sec; seqNo:12429; depth 16.33 2021-02-13T00:24:58Z- handleDvl() - RDI_Pathfinder msg: 1613175898.37 epoch sec; seqNo:6107 2021-02-13T00:24:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.42 , 100.35 , 71.93 , 116.57 2021-02-13T00:24:58Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.06 , -0.14 2021-02-13T00:24:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175898.40 epoch sec; seqNo:12429; 4059106.53 north; 592513.04 east 2021-02-13T00:24:58Z- AHRS_M2 msg: 1613175898.76 epoch sec; seqNo:12332 2021-02-13T00:24:58Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.18 psi 2021-02-13T00:24:58Z- handleDepth()-Depth_Keller msg: 1613175898764 epoch sec; seqNo:12430; depth 16.46 2021-02-13T00:24:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175898.78 epoch sec; seqNo:12430; 4059106.91 north; 592513.03 east 2021-02-13T00:24:59Z- LcmTrn::handleMessages() - handling 41 messages took 0.60 ms and 129 calls... 2021-02-13T00:24:59Z- waiting for 1613175896.94, time is 1613175898.78 2021-02-13T00:24:59Z- LcmTrn::updateTrn() >>>> pose time 1613175898.78, meas time 1613175898.37 <<<< 2021-02-13T00:24:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175898.37 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:24:59Z- TerrainNav::motionUpdate() - currEstimate = (4059107, 592513.03, 16.46) 2021-02-13T00:24:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:24:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:24:59Z- TNavBF::Projecting Measurements 2021-02-13T00:24:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:24:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:24:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:24:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:24:59Z- TNavFilter::4 good beams. 2021-02-13T00:24:59Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:24:59Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:24:59Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:24:59Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:24:59Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:24:59Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:24:59Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:24:59Z- TNavBF::mmse selected from filter -1 2021-02-13T00:24:59Z- TerrainMapOctree: (vehN, vehE ) = (4059106.62, 592512.83). 2021-02-13T00:24:59Z- TerrainMapOctree: Min Distance = 2758.95. 2021-02-13T00:24:59Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:24:59Z- There is no specific good beam that all particles have in common. 2021-02-13T00:24:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:24:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 10.61 2021-02-13T00:24:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:24:59Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 16.11 2021-02-13T00:24:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:24:59Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 8.28 2021-02-13T00:24:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:24:59Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.035 ExpectedMeasDiff: nan currMeas.covariance: 21.74 2021-02-13T00:24:59Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:24:59Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:24:59Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:24:59Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:24:59Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:24:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:24:59Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:24:59Z- Filter 0 No beams used in subcloud update 2021-02-13T00:24:59Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:24:59Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:24:59Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 704.0 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:24:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175898.37 sec. 2021-02-13T00:24:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:24:59Z- TNavBF::mmse selected from filter -1 2021-02-13T00:24:59Z- variances([0], [2], and psi_berg) = 703.966223 833.000070 0.000000 2021-02-13T00:24:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:24:59Z- LcmTrn::publishEstimates() - MLE : 4059106.41 592482.53 16.46 2021-02-13T00:24:59Z- LcmTrn::publishEstimates() - MMSE : 4059106.91 592513.03 16.46 2021-02-13T00:24:59Z- LcmTrn::publishEstimates() - COVAR: 703.97 833.00 0.85 0.00 2021-02-13T00:24:59Z- AHRS_M2 msg: 1613175899.19 epoch sec; seqNo:12333 2021-02-13T00:24:59Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.18 psi 2021-02-13T00:24:59Z- handleDepth()-Depth_Keller msg: 1613175899199 epoch sec; seqNo:12431; depth 16.63 2021-02-13T00:24:59Z- handleDvl() - RDI_Pathfinder msg: 1613175899.23 epoch sec; seqNo:6108 2021-02-13T00:24:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 99.00 , 70.70 , 115.22 2021-02-13T00:24:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.18 , -0.17 2021-02-13T00:24:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175899.24 epoch sec; seqNo:12431; 4059107.29 north; 592513.03 east 2021-02-13T00:24:59Z- AHRS_M2 msg: 1613175899.57 epoch sec; seqNo:12334 2021-02-13T00:24:59Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.18 psi 2021-02-13T00:24:59Z- handleDepth()-Depth_Keller msg: 1613175899572 epoch sec; seqNo:12432; depth 16.80 2021-02-13T00:24:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175899.59 epoch sec; seqNo:12432; 4059107.67 north; 592513.03 east 2021-02-13T00:24:59Z- AHRS_M2 msg: 1613175899.97 epoch sec; seqNo:12335 2021-02-13T00:24:59Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.18 psi 2021-02-13T00:24:59Z- handleDepth()-Depth_Keller msg: 1613175899979 epoch sec; seqNo:12433; depth 16.94 2021-02-13T00:25:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175899.99 epoch sec; seqNo:12433; 4059108.05 north; 592513.02 east 2021-02-13T00:25:00Z- AHRS_M2 msg: 1613175900.38 epoch sec; seqNo:12336 2021-02-13T00:25:00Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.18 psi 2021-02-13T00:25:00Z- handleDepth()-Depth_Keller msg: 1613175900380 epoch sec; seqNo:12434; depth 17.07 2021-02-13T00:25:00Z- handleDvl() - RDI_Pathfinder msg: 1613175900.39 epoch sec; seqNo:6109 2021-02-13T00:25:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.86 , 98.79 , 72.34 , 110.96 2021-02-13T00:25:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.22 , -0.09 2021-02-13T00:25:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175900.40 epoch sec; seqNo:12434; 4059108.42 north; 592513.02 east 2021-02-13T00:25:00Z- AHRS_M2 msg: 1613175900.78 epoch sec; seqNo:12337 2021-02-13T00:25:00Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.17 psi 2021-02-13T00:25:00Z- handleDepth()-Depth_Keller msg: 1613175900786 epoch sec; seqNo:12435; depth 17.22 2021-02-13T00:25:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175900.80 epoch sec; seqNo:12435; 4059108.82 north; 592514.24 east 2021-02-13T00:25:01Z- AHRS_M2 msg: 1613175901.19 epoch sec; seqNo:12338 2021-02-13T00:25:01Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.17 psi 2021-02-13T00:25:01Z- handleDepth()-Depth_Keller msg: 1613175901191 epoch sec; seqNo:12436; depth 17.35 2021-02-13T00:25:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175901.21 epoch sec; seqNo:12436; 4059109.20 north; 592514.23 east 2021-02-13T00:25:01Z- AHRS_M2 msg: 1613175901.59 epoch sec; seqNo:12339 2021-02-13T00:25:01Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.17 psi 2021-02-13T00:25:01Z- handleDepth()-Depth_Keller msg: 1613175901610 epoch sec; seqNo:12437; depth 17.50 2021-02-13T00:25:01Z- handleDvl() - RDI_Pathfinder msg: 1613175901.64 epoch sec; seqNo:6110 2021-02-13T00:25:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.46 , 97.71 , 71.11 , 115.96 2021-02-13T00:25:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.17 , 0.02 2021-02-13T00:25:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175901.66 epoch sec; seqNo:12437; 4059109.20 north; 592514.23 east 2021-02-13T00:25:01Z- AHRS_M2 msg: 1613175901.99 epoch sec; seqNo:12340 2021-02-13T00:25:01Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.16 psi 2021-02-13T00:25:02Z- handleDepth()-Depth_Keller msg: 1613175902001 epoch sec; seqNo:12438; depth 17.65 2021-02-13T00:25:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175902.01 epoch sec; seqNo:12438; 4059109.57 north; 592514.23 east 2021-02-13T00:25:02Z- AHRS_M2 msg: 1613175902.40 epoch sec; seqNo:12341 2021-02-13T00:25:02Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.16 psi 2021-02-13T00:25:02Z- handleDepth()-Depth_Keller msg: 1613175902403 epoch sec; seqNo:12439; depth 17.77 2021-02-13T00:25:02Z- handleDvl() - RDI_Pathfinder msg: 1613175902.41 epoch sec; seqNo:6111 2021-02-13T00:25:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 97.65 , 78.18 , 112.25 2021-02-13T00:25:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.04 , 0.10 2021-02-13T00:25:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175902.42 epoch sec; seqNo:12439; 4059109.57 north; 592514.23 east 2021-02-13T00:25:02Z- AHRS_M2 msg: 1613175902.80 epoch sec; seqNo:12342 2021-02-13T00:25:02Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.17 psi 2021-02-13T00:25:02Z- handleDepth()-Depth_Keller msg: 1613175902809 epoch sec; seqNo:12440; depth 17.91 2021-02-13T00:25:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175902.82 epoch sec; seqNo:12440; 4059109.95 north; 592514.22 east 2021-02-13T00:25:03Z- AHRS_M2 msg: 1613175903.20 epoch sec; seqNo:12343 2021-02-13T00:25:03Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.17 psi 2021-02-13T00:25:03Z- handleDepth()-Depth_Keller msg: 1613175903208 epoch sec; seqNo:12441; depth 18.04 2021-02-13T00:25:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175903.22 epoch sec; seqNo:12441; 4059110.33 north; 592514.22 east 2021-02-13T00:25:03Z- AHRS_M2 msg: 1613175903.61 epoch sec; seqNo:12344 2021-02-13T00:25:03Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.18 psi 2021-02-13T00:25:03Z- handleDepth()-Depth_Keller msg: 1613175903614 epoch sec; seqNo:12442; depth 18.17 2021-02-13T00:25:03Z- handleDvl() - RDI_Pathfinder msg: 1613175903.63 epoch sec; seqNo:6112 2021-02-13T00:25:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.14 , 97.71 , 69.47 , 112.58 2021-02-13T00:25:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.07 , 0.12 2021-02-13T00:25:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175903.64 epoch sec; seqNo:12442; 4059110.33 north; 592514.22 east 2021-02-13T00:25:04Z- AHRS_M2 msg: 1613175904.01 epoch sec; seqNo:12345 2021-02-13T00:25:04Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.19 psi 2021-02-13T00:25:04Z- handleDepth()-Depth_Keller msg: 1613175904021 epoch sec; seqNo:12443; depth 18.29 2021-02-13T00:25:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175904.04 epoch sec; seqNo:12443; 4059110.72 north; 592515.44 east 2021-02-13T00:25:04Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-13T00:25:04Z- waiting for 1613175901.78, time is 1613175904.04 2021-02-13T00:25:04Z- LcmTrn::updateTrn() >>>> pose time 1613175904.04, meas time 1613175903.63 <<<< 2021-02-13T00:25:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175903.63 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:25:04Z- TerrainNav::motionUpdate() - currEstimate = (4059111, 592515.44, 18.29) 2021-02-13T00:25:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:25:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:25:04Z- TNavBF::Projecting Measurements 2021-02-13T00:25:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:25:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:25:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:25:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:25:04Z- TNavFilter::4 good beams. 2021-02-13T00:25:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:25:04Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:25:04Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:25:04Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:25:04Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:25:04Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:25:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:25:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:25:04Z- TerrainMapOctree: (vehN, vehE ) = (4059110.43, 592515.25). 2021-02-13T00:25:04Z- TerrainMapOctree: Min Distance = 2754.48. 2021-02-13T00:25:04Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:25:04Z- There is no specific good beam that all particles have in common. 2021-02-13T00:25:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:25:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 10.28 2021-02-13T00:25:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:25:04Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:25:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:25:04Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 7.72 2021-02-13T00:25:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:25:04Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 20.28 2021-02-13T00:25:04Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:25:04Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:25:04Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:25:04Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:25:04Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:25:04Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:25:04Z- Filter 0 No beams used in subcloud update 2021-02-13T00:25:04Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:25:04Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:25:04Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 703.9 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:25:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175903.63 sec. 2021-02-13T00:25:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:25:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:25:04Z- variances([0], [2], and psi_berg) = 703.891910 833.000030 0.000000 2021-02-13T00:25:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:25:04Z- LcmTrn::publishEstimates() - MLE : 4059110.22 592484.94 18.29 2021-02-13T00:25:04Z- LcmTrn::publishEstimates() - MMSE : 4059110.72 592515.44 18.29 2021-02-13T00:25:04Z- LcmTrn::publishEstimates() - COVAR: 703.89 833.00 0.83 0.00 2021-02-13T00:25:04Z- AHRS_M2 msg: 1613175904.42 epoch sec; seqNo:12346 2021-02-13T00:25:04Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.19 psi 2021-02-13T00:25:04Z- handleDepth()-Depth_Keller msg: 1613175904420 epoch sec; seqNo:12444; depth 18.42 2021-02-13T00:25:04Z- handleDvl() - RDI_Pathfinder msg: 1613175904.43 epoch sec; seqNo:6113 2021-02-13T00:25:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.53 , 97.78 , 78.31 , 113.60 2021-02-13T00:25:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.12 , 0.08 2021-02-13T00:25:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175904.46 epoch sec; seqNo:12444; 4059110.72 north; 592515.44 east 2021-02-13T00:25:04Z- AHRS_M2 msg: 1613175904.82 epoch sec; seqNo:12347 2021-02-13T00:25:04Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.20 psi 2021-02-13T00:25:04Z- handleDepth()-Depth_Keller msg: 1613175904829 epoch sec; seqNo:12445; depth 18.55 2021-02-13T00:25:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175904.84 epoch sec; seqNo:12445; 4059111.10 north; 592515.43 east 2021-02-13T00:25:05Z- AHRS_M2 msg: 1613175905.22 epoch sec; seqNo:12348 2021-02-13T00:25:05Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.20 psi 2021-02-13T00:25:05Z- handleDepth()-Depth_Keller msg: 1613175905229 epoch sec; seqNo:12446; depth 18.69 2021-02-13T00:25:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175905.24 epoch sec; seqNo:12446; 4059111.10 north; 592515.43 east 2021-02-13T00:25:05Z- AHRS_M2 msg: 1613175905.63 epoch sec; seqNo:12349 2021-02-13T00:25:05Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.21 psi 2021-02-13T00:25:05Z- handleDepth()-Depth_Keller msg: 1613175905641 epoch sec; seqNo:12447; depth 18.81 2021-02-13T00:25:05Z- handleDvl() - RDI_Pathfinder msg: 1613175905.65 epoch sec; seqNo:6114 2021-02-13T00:25:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.13 , 99.06 , 78.78 , 112.58 2021-02-13T00:25:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.11 , 0.03 2021-02-13T00:25:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175905.67 epoch sec; seqNo:12447; 4059111.48 north; 592515.43 east 2021-02-13T00:25:06Z- AHRS_M2 msg: 1613175906.03 epoch sec; seqNo:12350 2021-02-13T00:25:06Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.21 psi 2021-02-13T00:25:06Z- handleDepth()-Depth_Keller msg: 1613175906040 epoch sec; seqNo:12448; depth 18.97 2021-02-13T00:25:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175906.05 epoch sec; seqNo:12448; 4059111.87 north; 592516.64 east 2021-02-13T00:25:06Z- AHRS_M2 msg: 1613175906.44 epoch sec; seqNo:12351 2021-02-13T00:25:06Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.21 psi 2021-02-13T00:25:06Z- handleDepth()-Depth_Keller msg: 1613175906440 epoch sec; seqNo:12449; depth 19.10 2021-02-13T00:25:06Z- handleDvl() - RDI_Pathfinder msg: 1613175906.45 epoch sec; seqNo:6115 2021-02-13T00:25:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 97.64 , 69.07 , 112.52 2021-02-13T00:25:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.06 , -0.04 2021-02-13T00:25:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175906.47 epoch sec; seqNo:12449; 4059111.87 north; 592516.64 east 2021-02-13T00:25:06Z- AHRS_M2 msg: 1613175906.84 epoch sec; seqNo:12352 2021-02-13T00:25:06Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.21 psi 2021-02-13T00:25:06Z- handleDepth()-Depth_Keller msg: 1613175906849 epoch sec; seqNo:12450; depth 19.25 2021-02-13T00:25:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175906.86 epoch sec; seqNo:12450; 4059112.25 north; 592516.64 east 2021-02-13T00:25:07Z- AHRS_M2 msg: 1613175907.25 epoch sec; seqNo:12353 2021-02-13T00:25:07Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.21 psi 2021-02-13T00:25:07Z- handleDepth()-Depth_Keller msg: 1613175907258 epoch sec; seqNo:12451; depth 19.39 2021-02-13T00:25:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175907.27 epoch sec; seqNo:12451; 4059112.63 north; 592516.64 east 2021-02-13T00:25:07Z- AHRS_M2 msg: 1613175907.65 epoch sec; seqNo:12354 2021-02-13T00:25:07Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.22 psi 2021-02-13T00:25:07Z- handleDepth()-Depth_Keller msg: 1613175907661 epoch sec; seqNo:12452; depth 19.53 2021-02-13T00:25:07Z- handleDvl() - RDI_Pathfinder msg: 1613175907.67 epoch sec; seqNo:6116 2021-02-13T00:25:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.37 , 99.06 , 69.89 , 112.45 2021-02-13T00:25:07Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.05 , -0.09 2021-02-13T00:25:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175907.68 epoch sec; seqNo:12452; 4059113.01 north; 592516.63 east 2021-02-13T00:25:08Z- AHRS_M2 msg: 1613175908.05 epoch sec; seqNo:12355 2021-02-13T00:25:08Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.22 psi 2021-02-13T00:25:08Z- handleDepth()-Depth_Keller msg: 1613175908057 epoch sec; seqNo:12453; depth 19.67 2021-02-13T00:25:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175908.08 epoch sec; seqNo:12453; 4059113.40 north; 592517.85 east 2021-02-13T00:25:08Z- AHRS_M2 msg: 1613175908.46 epoch sec; seqNo:12356 2021-02-13T00:25:08Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.22 psi 2021-02-13T00:25:08Z- handleDepth()-Depth_Keller msg: 1613175908461 epoch sec; seqNo:12454; depth 19.81 2021-02-13T00:25:08Z- handleDvl() - RDI_Pathfinder msg: 1613175908.47 epoch sec; seqNo:6117 2021-02-13T00:25:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.02 , 100.21 , 78.31 , 111.16 2021-02-13T00:25:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.13 , -0.10 2021-02-13T00:25:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175908.48 epoch sec; seqNo:12454; 4059113.40 north; 592517.85 east 2021-02-13T00:25:08Z- AHRS_M2 msg: 1613175908.86 epoch sec; seqNo:12357 2021-02-13T00:25:08Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.22 psi 2021-02-13T00:25:08Z- handleDepth()-Depth_Keller msg: 1613175908865 epoch sec; seqNo:12455; depth 19.94 2021-02-13T00:25:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175908.88 epoch sec; seqNo:12455; 4059113.78 north; 592517.84 east 2021-02-13T00:25:09Z- AHRS_M2 msg: 1613175909.26 epoch sec; seqNo:12358 2021-02-13T00:25:09Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.22 psi 2021-02-13T00:25:09Z- handleDepth()-Depth_Keller msg: 1613175909282 epoch sec; seqNo:12456; depth 20.08 2021-02-13T00:25:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175909.29 epoch sec; seqNo:12456; 4059114.16 north; 592517.84 east 2021-02-13T00:25:09Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-13T00:25:09Z- waiting for 1613175907.04, time is 1613175909.29 2021-02-13T00:25:09Z- LcmTrn::updateTrn() >>>> pose time 1613175909.29, meas time 1613175908.47 <<<< 2021-02-13T00:25:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175908.47 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:25:09Z- TerrainNav::motionUpdate() - currEstimate = (4059114, 592517.84, 20.08) 2021-02-13T00:25:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:25:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:25:09Z- TNavBF::Projecting Measurements 2021-02-13T00:25:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:25:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:25:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:25:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:25:09Z- TNavFilter::4 good beams. 2021-02-13T00:25:09Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:25:09Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:25:09Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:25:09Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:25:09Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:25:09Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:25:09Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:25:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:25:09Z- TerrainMapOctree: (vehN, vehE ) = (4059113.63, 592517.47). 2021-02-13T00:25:09Z- TerrainMapOctree: Min Distance = 2750.60. 2021-02-13T00:25:09Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:25:09Z- There is no specific good beam that all particles have in common. 2021-02-13T00:25:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:25:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 9.25 2021-02-13T00:25:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:25:09Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 16.07 2021-02-13T00:25:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:25:09Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 9.81 2021-02-13T00:25:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:25:09Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 19.77 2021-02-13T00:25:09Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:25:09Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:25:09Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:25:09Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:25:09Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:25:09Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:25:09Z- Filter 0 No beams used in subcloud update 2021-02-13T00:25:09Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:25:09Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:25:09Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 703.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:25:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175908.47 sec. 2021-02-13T00:25:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:25:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:25:09Z- variances([0], [2], and psi_berg) = 703.816022 832.999988 0.000000 2021-02-13T00:25:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:25:09Z- LcmTrn::publishEstimates() - MLE : 4059113.66 592487.34 20.08 2021-02-13T00:25:09Z- LcmTrn::publishEstimates() - MMSE : 4059114.16 592517.84 20.08 2021-02-13T00:25:09Z- LcmTrn::publishEstimates() - COVAR: 703.82 833.00 0.82 0.00 2021-02-13T00:25:09Z- AHRS_M2 msg: 1613175909.67 epoch sec; seqNo:12359 2021-02-13T00:25:09Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.22 psi 2021-02-13T00:25:09Z- handleDepth()-Depth_Keller msg: 1613175909677 epoch sec; seqNo:12457; depth 20.24 2021-02-13T00:25:09Z- handleDvl() - RDI_Pathfinder msg: 1613175909.69 epoch sec; seqNo:6118 2021-02-13T00:25:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.17 , 97.64 , 78.17 , 111.03 2021-02-13T00:25:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.17 , -0.06 2021-02-13T00:25:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175909.70 epoch sec; seqNo:12457; 4059114.54 north; 592517.84 east 2021-02-13T00:25:10Z- AHRS_M2 msg: 1613175910.08 epoch sec; seqNo:12360 2021-02-13T00:25:10Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.22 psi 2021-02-13T00:25:10Z- handleDepth()-Depth_Keller msg: 1613175910087 epoch sec; seqNo:12458; depth 20.37 2021-02-13T00:25:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175910.10 epoch sec; seqNo:12458; 4059114.92 north; 592517.83 east 2021-02-13T00:25:10Z- AHRS_M2 msg: 1613175910.47 epoch sec; seqNo:12361 2021-02-13T00:25:10Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.22 psi 2021-02-13T00:25:10Z- handleDepth()-Depth_Keller msg: 1613175910485 epoch sec; seqNo:12459; depth 20.49 2021-02-13T00:25:10Z- handleDvl() - RDI_Pathfinder msg: 1613175910.50 epoch sec; seqNo:6119 2021-02-13T00:25:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.79 , 97.64 , 78.17 , 109.81 2021-02-13T00:25:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.14 , -0.01 2021-02-13T00:25:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175910.52 epoch sec; seqNo:12459; 4059115.30 north; 592517.83 east 2021-02-13T00:25:10Z- AHRS_M2 msg: 1613175910.88 epoch sec; seqNo:12362 2021-02-13T00:25:10Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.22 psi 2021-02-13T00:25:10Z- handleDepth()-Depth_Keller msg: 1613175910885 epoch sec; seqNo:12460; depth 20.64 2021-02-13T00:25:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175910.90 epoch sec; seqNo:12460; 4059115.69 north; 592519.04 east 2021-02-13T00:25:11Z- AHRS_M2 msg: 1613175911.32 epoch sec; seqNo:12363 2021-02-13T00:25:11Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.22 psi 2021-02-13T00:25:11Z- handleDepth()-Depth_Keller msg: 1613175911325 epoch sec; seqNo:12461; depth 20.77 2021-02-13T00:25:11Z- handleDvl() - RDI_Pathfinder msg: 1613175911.34 epoch sec; seqNo:6120 2021-02-13T00:25:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 97.78 , 78.31 , 107.51 2021-02-13T00:25:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.10 , 0.01 2021-02-13T00:25:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175911.36 epoch sec; seqNo:12461; 4059115.69 north; 592519.04 east 2021-02-13T00:25:11Z- handleDepth()-Depth_Keller msg: 1613175911706 epoch sec; seqNo:12462; depth 20.92 2021-02-13T00:25:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175911.72 epoch sec; seqNo:12462; 4059116.07 north; 592519.04 east 2021-02-13T00:25:12Z- AHRS_M2 msg: 1613175912.10 epoch sec; seqNo:12364 2021-02-13T00:25:12Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.21 psi 2021-02-13T00:25:12Z- handleDepth()-Depth_Keller msg: 1613175912103 epoch sec; seqNo:12463; depth 21.04 2021-02-13T00:25:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175912.11 epoch sec; seqNo:12463; 4059116.45 north; 592519.04 east 2021-02-13T00:25:12Z- AHRS_M2 msg: 1613175912.50 epoch sec; seqNo:12365 2021-02-13T00:25:12Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.21 psi 2021-02-13T00:25:12Z- handleDepth()-Depth_Keller msg: 1613175912502 epoch sec; seqNo:12464; depth 21.17 2021-02-13T00:25:12Z- handleDvl() - RDI_Pathfinder msg: 1613175912.51 epoch sec; seqNo:6121 2021-02-13T00:25:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 97.78 , 66.61 , 105.08 2021-02-13T00:25:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , 0.03 2021-02-13T00:25:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175912.52 epoch sec; seqNo:12464; 4059116.45 north; 592519.04 east 2021-02-13T00:25:12Z- AHRS_M2 msg: 1613175912.90 epoch sec; seqNo:12366 2021-02-13T00:25:12Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.21 psi 2021-02-13T00:25:12Z- handleDepth()-Depth_Keller msg: 1613175912911 epoch sec; seqNo:12465; depth 21.30 2021-02-13T00:25:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175912.92 epoch sec; seqNo:12465; 4059116.83 north; 592519.03 east 2021-02-13T00:25:13Z- AHRS_M2 msg: 1613175913.30 epoch sec; seqNo:12367 2021-02-13T00:25:13Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.21 psi 2021-02-13T00:25:13Z- handleDepth()-Depth_Keller msg: 1613175913313 epoch sec; seqNo:12466; depth 21.42 2021-02-13T00:25:13Z- handleDvl() - RDI_Pathfinder msg: 1613175913.33 epoch sec; seqNo:6122 2021-02-13T00:25:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.38 , 98.12 , 67.43 , 107.85 2021-02-13T00:25:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.00 , 0.03 2021-02-13T00:25:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175913.34 epoch sec; seqNo:12466; 4059117.20 north; 592519.03 east 2021-02-13T00:25:13Z- AHRS_M2 msg: 1613175913.71 epoch sec; seqNo:12368 2021-02-13T00:25:13Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:25:13Z- handleDepth()-Depth_Keller msg: 1613175913715 epoch sec; seqNo:12467; depth 21.57 2021-02-13T00:25:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175913.73 epoch sec; seqNo:12467; 4059117.20 north; 592519.03 east 2021-02-13T00:25:14Z- AHRS_M2 msg: 1613175914.11 epoch sec; seqNo:12369 2021-02-13T00:25:14Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:25:14Z- handleDepth()-Depth_Keller msg: 1613175914125 epoch sec; seqNo:12468; depth 21.70 2021-02-13T00:25:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175914.15 epoch sec; seqNo:12468; 4059117.60 north; 592520.24 east 2021-02-13T00:25:14Z- LcmTrn::handleMessages() - handling 40 messages took 0.67 ms and 127 calls... 2021-02-13T00:25:14Z- waiting for 1613175912.29, time is 1613175914.15 2021-02-13T00:25:14Z- LcmTrn::updateTrn() >>>> pose time 1613175914.15, meas time 1613175913.33 <<<< 2021-02-13T00:25:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175913.33 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:25:14Z- TerrainNav::motionUpdate() - currEstimate = (4059118, 592520.24, 21.70) 2021-02-13T00:25:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:25:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:25:14Z- TNavBF::Projecting Measurements 2021-02-13T00:25:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:25:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:25:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:25:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:25:14Z- TNavFilter::4 good beams. 2021-02-13T00:25:14Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:25:14Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:25:14Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:25:14Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:25:14Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:25:14Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:25:14Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:25:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:25:14Z- TerrainMapOctree: (vehN, vehE ) = (4059117.02, 592519.84). 2021-02-13T00:25:14Z- TerrainMapOctree: Min Distance = 2746.46. 2021-02-13T00:25:14Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:25:14Z- There is no specific good beam that all particles have in common. 2021-02-13T00:25:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:25:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-13T00:25:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:25:14Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 15.40 2021-02-13T00:25:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:25:14Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 7.28 2021-02-13T00:25:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:25:14Z- Old Depth Bias: 0.00 DepthBiasCov: 0.79 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 18.61 2021-02-13T00:25:14Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:25:14Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:25:14Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:25:14Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:25:14Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:25:14Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:25:14Z- Filter 0 No beams used in subcloud update 2021-02-13T00:25:14Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:25:14Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:25:14Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 703.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:25:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175913.33 sec. 2021-02-13T00:25:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:25:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:25:14Z- variances([0], [2], and psi_berg) = 703.739695 832.999946 0.000000 2021-02-13T00:25:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:25:14Z- LcmTrn::publishEstimates() - MLE : 4059117.10 592489.74 21.70 2021-02-13T00:25:14Z- LcmTrn::publishEstimates() - MMSE : 4059117.60 592520.24 21.70 2021-02-13T00:25:14Z- LcmTrn::publishEstimates() - COVAR: 703.74 833.00 0.80 0.00 2021-02-13T00:25:14Z- AHRS_M2 msg: 1613175914.52 epoch sec; seqNo:12370 2021-02-13T00:25:14Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:25:14Z- handleDepth()-Depth_Keller msg: 1613175914520 epoch sec; seqNo:12469; depth 21.82 2021-02-13T00:25:14Z- handleDvl() - RDI_Pathfinder msg: 1613175914.54 epoch sec; seqNo:6123 2021-02-13T00:25:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 66.62 , 105.35 2021-02-13T00:25:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.02 , 0.01 2021-02-13T00:25:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175914.55 epoch sec; seqNo:12469; 4059117.98 north; 592520.24 east 2021-02-13T00:25:14Z- AHRS_M2 msg: 1613175914.92 epoch sec; seqNo:12371 2021-02-13T00:25:14Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:25:14Z- handleDepth()-Depth_Keller msg: 1613175914923 epoch sec; seqNo:12470; depth 21.97 2021-02-13T00:25:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175914.93 epoch sec; seqNo:12470; 4059118.35 north; 592520.24 east 2021-02-13T00:25:15Z- AHRS_M2 msg: 1613175915.32 epoch sec; seqNo:12372 2021-02-13T00:25:15Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:25:15Z- handleDepth()-Depth_Keller msg: 1613175915333 epoch sec; seqNo:12471; depth 22.08 2021-02-13T00:25:15Z- handleDvl() - RDI_Pathfinder msg: 1613175915.34 epoch sec; seqNo:6124 2021-02-13T00:25:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 97.98 , 64.98 , 105.28 2021-02-13T00:25:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , -0.02 2021-02-13T00:25:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175915.37 epoch sec; seqNo:12471; 4059118.35 north; 592520.24 east 2021-02-13T00:25:15Z- AHRS_M2 msg: 1613175915.73 epoch sec; seqNo:12373 2021-02-13T00:25:15Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:25:15Z- handleDepth()-Depth_Keller msg: 1613175915732 epoch sec; seqNo:12472; depth 22.21 2021-02-13T00:25:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175915.74 epoch sec; seqNo:12472; 4059118.73 north; 592520.23 east 2021-02-13T00:25:16Z- AHRS_M2 msg: 1613175916.15 epoch sec; seqNo:12374 2021-02-13T00:25:16Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:25:16Z- handleDepth()-Depth_Keller msg: 1613175916171 epoch sec; seqNo:12473; depth 22.36 2021-02-13T00:25:16Z- handleDvl() - RDI_Pathfinder msg: 1613175916.19 epoch sec; seqNo:6125 2021-02-13T00:25:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 92.10 , 66.62 , 104.27 2021-02-13T00:25:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , -0.01 2021-02-13T00:25:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175916.20 epoch sec; seqNo:12473; 4059119.11 north; 592520.23 east 2021-02-13T00:25:16Z- AHRS_M2 msg: 1613175916.54 epoch sec; seqNo:12375 2021-02-13T00:25:16Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-13T00:25:16Z- handleDepth()-Depth_Keller msg: 1613175916545 epoch sec; seqNo:12474; depth 22.51 2021-02-13T00:25:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175916.56 epoch sec; seqNo:12474; 4059119.50 north; 592521.44 east 2021-02-13T00:25:16Z- AHRS_M2 msg: 1613175916.94 epoch sec; seqNo:12376 2021-02-13T00:25:16Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-13T00:25:16Z- handleDepth()-Depth_Keller msg: 1613175916944 epoch sec; seqNo:12475; depth 22.61 2021-02-13T00:25:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175916.95 epoch sec; seqNo:12475; 4059119.50 north; 592521.44 east 2021-02-13T00:25:17Z- AHRS_M2 msg: 1613175917.35 epoch sec; seqNo:12377 2021-02-13T00:25:17Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-13T00:25:17Z- handleDepth()-Depth_Keller msg: 1613175917359 epoch sec; seqNo:12476; depth 22.74 2021-02-13T00:25:17Z- handleDvl() - RDI_Pathfinder msg: 1613175917.37 epoch sec; seqNo:6126 2021-02-13T00:25:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.25 , 67.03 , 100.68 2021-02-13T00:25:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , 0.04 2021-02-13T00:25:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175917.38 epoch sec; seqNo:12476; 4059119.88 north; 592521.44 east 2021-02-13T00:25:17Z- AHRS_M2 msg: 1613175917.75 epoch sec; seqNo:12378 2021-02-13T00:25:17Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-13T00:25:17Z- handleDepth()-Depth_Keller msg: 1613175917761 epoch sec; seqNo:12477; depth 22.88 2021-02-13T00:25:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175917.77 epoch sec; seqNo:12477; 4059120.26 north; 592521.44 east 2021-02-13T00:25:18Z- AHRS_M2 msg: 1613175918.15 epoch sec; seqNo:12379 2021-02-13T00:25:18Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.20 psi 2021-02-13T00:25:18Z- handleDepth()-Depth_Keller msg: 1613175918158 epoch sec; seqNo:12478; depth 23.02 2021-02-13T00:25:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175918.17 epoch sec; seqNo:12478; 4059120.26 north; 592521.44 east 2021-02-13T00:25:18Z- AHRS_M2 msg: 1613175918.56 epoch sec; seqNo:12380 2021-02-13T00:25:18Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.20 psi 2021-02-13T00:25:18Z- handleDepth()-Depth_Keller msg: 1613175918560 epoch sec; seqNo:12479; depth 23.15 2021-02-13T00:25:18Z- handleDvl() - RDI_Pathfinder msg: 1613175918.57 epoch sec; seqNo:6127 2021-02-13T00:25:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.37 , 97.85 , 65.80 , 103.93 2021-02-13T00:25:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.06 2021-02-13T00:25:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175918.58 epoch sec; seqNo:12479; 4059120.64 north; 592521.43 east 2021-02-13T00:25:18Z- AHRS_M2 msg: 1613175918.96 epoch sec; seqNo:12381 2021-02-13T00:25:18Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-13T00:25:18Z- handleDepth()-Depth_Keller msg: 1613175918965 epoch sec; seqNo:12480; depth 23.28 2021-02-13T00:25:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175918.98 epoch sec; seqNo:12480; 4059121.02 north; 592521.43 east 2021-02-13T00:25:19Z- AHRS_M2 msg: 1613175919.36 epoch sec; seqNo:12382 2021-02-13T00:25:19Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-13T00:25:19Z- handleDepth()-Depth_Keller msg: 1613175919369 epoch sec; seqNo:12481; depth 23.39 2021-02-13T00:25:19Z- handleDvl() - RDI_Pathfinder msg: 1613175919.39 epoch sec; seqNo:6128 2021-02-13T00:25:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.93 , 90.75 , 66.62 , 102.92 2021-02-13T00:25:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.04 , 0.05 2021-02-13T00:25:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175919.40 epoch sec; seqNo:12481; 4059121.41 north; 592522.64 east 2021-02-13T00:25:19Z- LcmTrn::handleMessages() - handling 45 messages took 0.66 ms and 132 calls... 2021-02-13T00:25:19Z- waiting for 1613175917.15, time is 1613175919.40 2021-02-13T00:25:19Z- LcmTrn::updateTrn() equating pose 1613175919.40 and meas times 1613175919.40 2021-02-13T00:25:19Z- LcmTrn::updateTrn() >>>> pose time 1613175919.40, meas time 1613175919.40 <<<< 2021-02-13T00:25:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175919.40 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:25:19Z- TerrainNav::motionUpdate() - currEstimate = (4059121, 592522.64, 23.39) 2021-02-13T00:25:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:25:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:25:19Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175919.40 sec.; No relevant navigation data available 2021-02-13T00:25:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:25:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:25:19Z- variances([0], [2], and psi_berg) = 703.679885 832.999914 0.000000 2021-02-13T00:25:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:25:19Z- LcmTrn::publishEstimates() - MLE : 4059120.91 592492.14 23.39 2021-02-13T00:25:19Z- LcmTrn::publishEstimates() - MMSE : 4059121.41 592522.64 23.39 2021-02-13T00:25:19Z- LcmTrn::publishEstimates() - COVAR: 703.68 833.00 1.02 0.00 2021-02-13T00:25:19Z- AHRS_M2 msg: 1613175919.77 epoch sec; seqNo:12383 2021-02-13T00:25:19Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-13T00:25:19Z- handleDepth()-Depth_Keller msg: 1613175919777 epoch sec; seqNo:12482; depth 23.52 2021-02-13T00:25:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175919.79 epoch sec; seqNo:12482; 4059121.41 north; 592522.64 east 2021-02-13T00:25:20Z- AHRS_M2 msg: 1613175920.17 epoch sec; seqNo:12384 2021-02-13T00:25:20Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-13T00:25:20Z- handleDepth()-Depth_Keller msg: 1613175920176 epoch sec; seqNo:12483; depth 23.64 2021-02-13T00:25:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175920.18 epoch sec; seqNo:12483; 4059121.79 north; 592522.64 east 2021-02-13T00:25:20Z- AHRS_M2 msg: 1613175920.58 epoch sec; seqNo:12385 2021-02-13T00:25:20Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-13T00:25:20Z- handleDepth()-Depth_Keller msg: 1613175920581 epoch sec; seqNo:12484; depth 23.77 2021-02-13T00:25:20Z- handleDvl() - RDI_Pathfinder msg: 1613175920.59 epoch sec; seqNo:6129 2021-02-13T00:25:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 98.39 , 64.98 , 102.04 2021-02-13T00:25:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.14 , -0.02 2021-02-13T00:25:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175920.60 epoch sec; seqNo:12484; 4059122.17 north; 592522.64 east 2021-02-13T00:25:20Z- AHRS_M2 msg: 1613175920.98 epoch sec; seqNo:12386 2021-02-13T00:25:20Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-13T00:25:20Z- handleDepth()-Depth_Keller msg: 1613175920994 epoch sec; seqNo:12485; depth 23.89 2021-02-13T00:25:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175921.00 epoch sec; seqNo:12485; 4059122.17 north; 592522.64 east 2021-02-13T00:25:21Z- AHRS_M2 msg: 1613175921.38 epoch sec; seqNo:12387 2021-02-13T00:25:21Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-13T00:25:21Z- handleDepth()-Depth_Keller msg: 1613175921393 epoch sec; seqNo:12486; depth 24.02 2021-02-13T00:25:21Z- handleDvl() - RDI_Pathfinder msg: 1613175921.41 epoch sec; seqNo:6130 2021-02-13T00:25:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 90.82 , 65.38 , 100.55 2021-02-13T00:25:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.01 , -0.05 2021-02-13T00:25:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175921.43 epoch sec; seqNo:12486; 4059122.55 north; 592522.63 east 2021-02-13T00:25:21Z- AHRS_M2 msg: 1613175921.79 epoch sec; seqNo:12388 2021-02-13T00:25:21Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-13T00:25:21Z- handleDepth()-Depth_Keller msg: 1613175921792 epoch sec; seqNo:12487; depth 24.14 2021-02-13T00:25:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175921.80 epoch sec; seqNo:12487; 4059122.94 north; 592523.85 east 2021-02-13T00:25:22Z- AHRS_M2 msg: 1613175922.21 epoch sec; seqNo:12389 2021-02-13T00:25:22Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-13T00:25:22Z- handleDepth()-Depth_Keller msg: 1613175922219 epoch sec; seqNo:12488; depth 24.27 2021-02-13T00:25:22Z- handleDvl() - RDI_Pathfinder msg: 1613175922.23 epoch sec; seqNo:6131 2021-02-13T00:25:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 90.21 , 64.57 , 99.94 2021-02-13T00:25:22Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.08 , -0.07 2021-02-13T00:25:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175922.25 epoch sec; seqNo:12488; 4059123.32 north; 592523.85 east 2021-02-13T00:25:22Z- AHRS_M2 msg: 1613175922.60 epoch sec; seqNo:12390 2021-02-13T00:25:22Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-13T00:25:22Z- handleDepth()-Depth_Keller msg: 1613175922610 epoch sec; seqNo:12489; depth 24.40 2021-02-13T00:25:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175922.62 epoch sec; seqNo:12489; 4059123.70 north; 592523.84 east 2021-02-13T00:25:23Z- AHRS_M2 msg: 1613175923.00 epoch sec; seqNo:12391 2021-02-13T00:25:23Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-13T00:25:23Z- handleDepth()-Depth_Keller msg: 1613175923004 epoch sec; seqNo:12490; depth 24.53 2021-02-13T00:25:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175923.01 epoch sec; seqNo:12490; 4059124.08 north; 592523.84 east 2021-02-13T00:25:23Z- AHRS_M2 msg: 1613175923.40 epoch sec; seqNo:12392 2021-02-13T00:25:23Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-13T00:25:23Z- handleDepth()-Depth_Keller msg: 1613175923408 epoch sec; seqNo:12491; depth 24.64 2021-02-13T00:25:23Z- handleDvl() - RDI_Pathfinder msg: 1613175923.42 epoch sec; seqNo:6132 2021-02-13T00:25:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 0.00 , 64.57 , 98.93 2021-02-13T00:25:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.13 , -0.05 2021-02-13T00:25:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175923.43 epoch sec; seqNo:12491; 4059124.45 north; 592523.83 east 2021-02-13T00:25:23Z- AHRS_M2 msg: 1613175923.81 epoch sec; seqNo:12393 2021-02-13T00:25:23Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:25:23Z- handleDepth()-Depth_Keller msg: 1613175923821 epoch sec; seqNo:12492; depth 24.79 2021-02-13T00:25:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175923.83 epoch sec; seqNo:12492; 4059124.45 north; 592523.83 east 2021-02-13T00:25:24Z- AHRS_M2 msg: 1613175924.21 epoch sec; seqNo:12394 2021-02-13T00:25:24Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.17 psi 2021-02-13T00:25:24Z- handleDepth()-Depth_Keller msg: 1613175924220 epoch sec; seqNo:12493; depth 24.91 2021-02-13T00:25:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175924.25 epoch sec; seqNo:12493; 4059124.85 north; 592525.05 east 2021-02-13T00:25:24Z- LcmTrn::handleMessages() - handling 40 messages took 0.63 ms and 128 calls... 2021-02-13T00:25:24Z- waiting for 1613175922.40, time is 1613175924.25 2021-02-13T00:25:24Z- LcmTrn::updateTrn() >>>> pose time 1613175924.25, meas time 1613175923.42 <<<< 2021-02-13T00:25:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175923.42 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:25:24Z- TerrainNav::motionUpdate() - currEstimate = (4059125, 592525.05, 24.91) 2021-02-13T00:25:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:25:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:25:24Z- TNavBF::Projecting Measurements 2021-02-13T00:25:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:25:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:25:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:25:24Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:25:24Z- TNavFilter::3 good beams. 2021-02-13T00:25:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:25:24Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:25:24Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:25:24Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:25:24Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:25:24Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:25:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:25:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:25:24Z- TerrainMapOctree: (vehN, vehE ) = (4059124.26, 592524.64). 2021-02-13T00:25:24Z- TerrainMapOctree: Min Distance = 2737.82. 2021-02-13T00:25:24Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:25:24Z- There is no specific good beam that all particles have in common. 2021-02-13T00:25:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:25:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-13T00:25:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:25:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 6.67 2021-02-13T00:25:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:25:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 15.66 2021-02-13T00:25:24Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:25:24Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:25:24Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:25:24Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:25:24Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:25:24Z- Filter 0 No beams used in subcloud update 2021-02-13T00:25:24Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:25:24Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:25:24Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 703.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:25:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175923.42 sec. 2021-02-13T00:25:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:25:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:25:24Z- variances([0], [2], and psi_berg) = 703.603582 832.999872 0.000000 2021-02-13T00:25:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:25:24Z- LcmTrn::publishEstimates() - MLE : 4059124.35 592494.55 24.91 2021-02-13T00:25:24Z- LcmTrn::publishEstimates() - MMSE : 4059124.85 592525.05 24.91 2021-02-13T00:25:24Z- LcmTrn::publishEstimates() - COVAR: 703.60 833.00 0.94 0.00 2021-02-13T00:25:24Z- AHRS_M2 msg: 1613175924.62 epoch sec; seqNo:12395 2021-02-13T00:25:24Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.17 psi 2021-02-13T00:25:24Z- handleDepth()-Depth_Keller msg: 1613175924620 epoch sec; seqNo:12494; depth 25.04 2021-02-13T00:25:24Z- handleDvl() - RDI_Pathfinder msg: 1613175924.63 epoch sec; seqNo:6133 2021-02-13T00:25:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 88.25 , 64.57 , 99.07 2021-02-13T00:25:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.13 , 0.00 2021-02-13T00:25:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175924.66 epoch sec; seqNo:12494; 4059125.23 north; 592525.05 east 2021-02-13T00:25:25Z- AHRS_M2 msg: 1613175925.02 epoch sec; seqNo:12396 2021-02-13T00:25:25Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.17 psi 2021-02-13T00:25:25Z- handleDepth()-Depth_Keller msg: 1613175925025 epoch sec; seqNo:12495; depth 25.15 2021-02-13T00:25:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175925.04 epoch sec; seqNo:12495; 4059125.60 north; 592525.04 east 2021-02-13T00:25:25Z- AHRS_M2 msg: 1613175925.42 epoch sec; seqNo:12397 2021-02-13T00:25:25Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.16 psi 2021-02-13T00:25:25Z- handleDepth()-Depth_Keller msg: 1613175925436 epoch sec; seqNo:12496; depth 25.27 2021-02-13T00:25:25Z- handleDvl() - RDI_Pathfinder msg: 1613175925.45 epoch sec; seqNo:6134 2021-02-13T00:25:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 86.22 , 64.16 , 98.39 2021-02-13T00:25:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.12 , 0.02 2021-02-13T00:25:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175925.46 epoch sec; seqNo:12496; 4059125.98 north; 592525.04 east 2021-02-13T00:25:25Z- AHRS_M2 msg: 1613175925.83 epoch sec; seqNo:12398 2021-02-13T00:25:25Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.16 psi 2021-02-13T00:25:25Z- handleDepth()-Depth_Keller msg: 1613175925837 epoch sec; seqNo:12497; depth 25.41 2021-02-13T00:25:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175925.86 epoch sec; seqNo:12497; 4059125.98 north; 592525.04 east 2021-02-13T00:25:26Z- AHRS_M2 msg: 1613175926.23 epoch sec; seqNo:12399 2021-02-13T00:25:26Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.16 psi 2021-02-13T00:25:26Z- handleDepth()-Depth_Keller msg: 1613175926241 epoch sec; seqNo:12498; depth 25.53 2021-02-13T00:25:26Z- handleDvl() - RDI_Pathfinder msg: 1613175926.25 epoch sec; seqNo:6135 2021-02-13T00:25:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.52 , 85.00 , 64.57 , 97.98 2021-02-13T00:25:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , 0.04 2021-02-13T00:25:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175926.28 epoch sec; seqNo:12498; 4059126.36 north; 592525.03 east 2021-02-13T00:25:26Z- AHRS_M2 msg: 1613175926.65 epoch sec; seqNo:12400 2021-02-13T00:25:26Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.16 psi 2021-02-13T00:25:26Z- handleDepth()-Depth_Keller msg: 1613175926650 epoch sec; seqNo:12499; depth 25.66 2021-02-13T00:25:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175926.66 epoch sec; seqNo:12499; 4059126.74 north; 592525.03 east 2021-02-13T00:25:27Z- AHRS_M2 msg: 1613175927.04 epoch sec; seqNo:12401 2021-02-13T00:25:27Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.17 psi 2021-02-13T00:25:27Z- handleDepth()-Depth_Keller msg: 1613175927054 epoch sec; seqNo:12500; depth 25.79 2021-02-13T00:25:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175927.07 epoch sec; seqNo:12500; 4059126.75 north; 592526.25 east 2021-02-13T00:25:27Z- AHRS_M2 msg: 1613175927.44 epoch sec; seqNo:12402 2021-02-13T00:25:27Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.17 psi 2021-02-13T00:25:27Z- handleDepth()-Depth_Keller msg: 1613175927448 epoch sec; seqNo:12501; depth 25.90 2021-02-13T00:25:27Z- handleDvl() - RDI_Pathfinder msg: 1613175927.46 epoch sec; seqNo:6136 2021-02-13T00:25:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 86.08 , 62.53 , 97.98 2021-02-13T00:25:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , 0.02 2021-02-13T00:25:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175927.49 epoch sec; seqNo:12501; 4059127.13 north; 592526.25 east 2021-02-13T00:25:27Z- AHRS_M2 msg: 1613175927.85 epoch sec; seqNo:12403 2021-02-13T00:25:27Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.17 psi 2021-02-13T00:25:27Z- handleDepth()-Depth_Keller msg: 1613175927852 epoch sec; seqNo:12502; depth 26.03 2021-02-13T00:25:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175927.86 epoch sec; seqNo:12502; 4059127.51 north; 592526.24 east 2021-02-13T00:25:28Z- AHRS_M2 msg: 1613175928.25 epoch sec; seqNo:12404 2021-02-13T00:25:28Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.17 psi 2021-02-13T00:25:28Z- handleDepth()-Depth_Keller msg: 1613175928265 epoch sec; seqNo:12503; depth 26.16 2021-02-13T00:25:28Z- handleDvl() - RDI_Pathfinder msg: 1613175928.28 epoch sec; seqNo:6137 2021-02-13T00:25:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 0.00 , 63.35 , 100.14 2021-02-13T00:25:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , 0.01 2021-02-13T00:25:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175928.29 epoch sec; seqNo:12503; 4059127.89 north; 592526.24 east 2021-02-13T00:25:28Z- AHRS_M2 msg: 1613175928.66 epoch sec; seqNo:12405 2021-02-13T00:25:28Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.17 psi 2021-02-13T00:25:28Z- handleDepth()-Depth_Keller msg: 1613175928660 epoch sec; seqNo:12504; depth 26.29 2021-02-13T00:25:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175928.68 epoch sec; seqNo:12504; 4059127.89 north; 592526.24 east 2021-02-13T00:25:29Z- AHRS_M2 msg: 1613175929.07 epoch sec; seqNo:12406 2021-02-13T00:25:29Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.19 psi 2021-02-13T00:25:29Z- handleDepth()-Depth_Keller msg: 1613175929070 epoch sec; seqNo:12505; depth 26.41 2021-02-13T00:25:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175929.08 epoch sec; seqNo:12505; 4059128.27 north; 592526.23 east 2021-02-13T00:25:29Z- AHRS_M2 msg: 1613175929.47 epoch sec; seqNo:12407 2021-02-13T00:25:29Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.19 psi 2021-02-13T00:25:29Z- handleDepth()-Depth_Keller msg: 1613175929472 epoch sec; seqNo:12506; depth 26.52 2021-02-13T00:25:29Z- handleDvl() - RDI_Pathfinder msg: 1613175929.48 epoch sec; seqNo:6138 2021-02-13T00:25:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 86.08 , 63.35 , 97.98 2021-02-13T00:25:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , -0.01 2021-02-13T00:25:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175929.49 epoch sec; seqNo:12506; 4059128.65 north; 592526.23 east 2021-02-13T00:25:29Z- LcmTrn::handleMessages() - handling 45 messages took 0.66 ms and 132 calls... 2021-02-13T00:25:29Z- waiting for 1613175927.25, time is 1613175929.49 2021-02-13T00:25:29Z- LcmTrn::updateTrn() equating pose 1613175929.49 and meas times 1613175929.49 2021-02-13T00:25:29Z- LcmTrn::updateTrn() >>>> pose time 1613175929.49, meas time 1613175929.49 <<<< 2021-02-13T00:25:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175929.49 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:25:29Z- TerrainNav::motionUpdate() - currEstimate = (4059129, 592526.23, 26.52) 2021-02-13T00:25:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:25:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:25:29Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175929.49 sec.; No relevant navigation data available 2021-02-13T00:25:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:25:29Z- TNavBF::mmse selected from filter -1 2021-02-13T00:25:29Z- variances([0], [2], and psi_berg) = 703.547457 832.999841 0.000000 2021-02-13T00:25:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:25:29Z- LcmTrn::publishEstimates() - MLE : 4059128.15 592495.73 26.52 2021-02-13T00:25:29Z- LcmTrn::publishEstimates() - MMSE : 4059128.65 592526.23 26.52 2021-02-13T00:25:29Z- LcmTrn::publishEstimates() - COVAR: 703.55 833.00 1.14 0.00 2021-02-13T00:25:29Z- AHRS_M2 msg: 1613175929.87 epoch sec; seqNo:12408 2021-02-13T00:25:29Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.19 psi 2021-02-13T00:25:29Z- handleDepth()-Depth_Keller msg: 1613175929881 epoch sec; seqNo:12507; depth 26.65 2021-02-13T00:25:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175929.89 epoch sec; seqNo:12507; 4059128.66 north; 592527.45 east 2021-02-13T00:25:30Z- AHRS_M2 msg: 1613175930.27 epoch sec; seqNo:12409 2021-02-13T00:25:30Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-13T00:25:30Z- handleDepth()-Depth_Keller msg: 1613175930276 epoch sec; seqNo:12508; depth 26.77 2021-02-13T00:25:30Z- handleDvl() - RDI_Pathfinder msg: 1613175930.29 epoch sec; seqNo:6139 2021-02-13T00:25:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 0.00 , 62.53 , 97.98 2021-02-13T00:25:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.02 2021-02-13T00:25:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175930.31 epoch sec; seqNo:12508; 4059129.04 north; 592527.45 east 2021-02-13T00:25:30Z- AHRS_M2 msg: 1613175930.68 epoch sec; seqNo:12410 2021-02-13T00:25:30Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-13T00:25:30Z- handleDepth()-Depth_Keller msg: 1613175930681 epoch sec; seqNo:12509; depth 26.91 2021-02-13T00:25:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175930.69 epoch sec; seqNo:12509; 4059129.42 north; 592527.44 east 2021-02-13T00:25:31Z- AHRS_M2 msg: 1613175931.09 epoch sec; seqNo:12411 2021-02-13T00:25:31Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-13T00:25:31Z- handleDepth()-Depth_Keller msg: 1613175931089 epoch sec; seqNo:12510; depth 27.03 2021-02-13T00:25:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175931.11 epoch sec; seqNo:12510; 4059129.42 north; 592527.44 east 2021-02-13T00:25:31Z- AHRS_M2 msg: 1613175931.48 epoch sec; seqNo:12412 2021-02-13T00:25:31Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-13T00:25:31Z- handleDepth()-Depth_Keller msg: 1613175931488 epoch sec; seqNo:12511; depth 27.15 2021-02-13T00:25:31Z- handleDvl() - RDI_Pathfinder msg: 1613175931.51 epoch sec; seqNo:6140 2021-02-13T00:25:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 0.00 , 62.12 , 97.98 2021-02-13T00:25:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , -0.02 2021-02-13T00:25:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175931.52 epoch sec; seqNo:12511; 4059129.80 north; 592527.44 east 2021-02-13T00:25:31Z- AHRS_M2 msg: 1613175931.89 epoch sec; seqNo:12413 2021-02-13T00:25:31Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-13T00:25:31Z- handleDepth()-Depth_Keller msg: 1613175931897 epoch sec; seqNo:12512; depth 27.28 2021-02-13T00:25:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175931.91 epoch sec; seqNo:12512; 4059130.19 north; 592528.65 east 2021-02-13T00:25:32Z- AHRS_M2 msg: 1613175932.29 epoch sec; seqNo:12414 2021-02-13T00:25:32Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-13T00:25:32Z- handleDepth()-Depth_Keller msg: 1613175932296 epoch sec; seqNo:12513; depth 27.43 2021-02-13T00:25:32Z- handleDvl() - RDI_Pathfinder msg: 1613175932.31 epoch sec; seqNo:6141 2021-02-13T00:25:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.66 , 87.16 , 62.94 , 97.98 2021-02-13T00:25:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.00 , 0.01 2021-02-13T00:25:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175932.32 epoch sec; seqNo:12513; 4059130.19 north; 592528.65 east 2021-02-13T00:25:32Z- AHRS_M2 msg: 1613175932.70 epoch sec; seqNo:12415 2021-02-13T00:25:32Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-13T00:25:32Z- handleDepth()-Depth_Keller msg: 1613175932708 epoch sec; seqNo:12514; depth 27.55 2021-02-13T00:25:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175932.72 epoch sec; seqNo:12514; 4059130.57 north; 592528.65 east 2021-02-13T00:25:33Z- AHRS_M2 msg: 1613175933.10 epoch sec; seqNo:12416 2021-02-13T00:25:33Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-13T00:25:33Z- handleDepth()-Depth_Keller msg: 1613175933115 epoch sec; seqNo:12515; depth 27.66 2021-02-13T00:25:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175933.12 epoch sec; seqNo:12515; 4059130.95 north; 592528.65 east 2021-02-13T00:25:33Z- AHRS_M2 msg: 1613175933.50 epoch sec; seqNo:12417 2021-02-13T00:25:33Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.19 psi 2021-02-13T00:25:33Z- handleDepth()-Depth_Keller msg: 1613175933508 epoch sec; seqNo:12516; depth 27.80 2021-02-13T00:25:33Z- handleDvl() - RDI_Pathfinder msg: 1613175933.52 epoch sec; seqNo:6142 2021-02-13T00:25:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.66 , 86.08 , 62.53 , 97.98 2021-02-13T00:25:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , 0.02 2021-02-13T00:25:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175933.55 epoch sec; seqNo:12516; 4059131.33 north; 592528.64 east 2021-02-13T00:25:33Z- AHRS_M2 msg: 1613175933.91 epoch sec; seqNo:12418 2021-02-13T00:25:33Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.20 psi 2021-02-13T00:25:33Z- handleDepth()-Depth_Keller msg: 1613175933912 epoch sec; seqNo:12517; depth 27.94 2021-02-13T00:25:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175933.92 epoch sec; seqNo:12517; 4059131.33 north; 592528.64 east 2021-02-13T00:25:34Z- AHRS_M2 msg: 1613175934.31 epoch sec; seqNo:12419 2021-02-13T00:25:34Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.20 psi 2021-02-13T00:25:34Z- handleDepth()-Depth_Keller msg: 1613175934317 epoch sec; seqNo:12518; depth 28.04 2021-02-13T00:25:34Z- handleDvl() - RDI_Pathfinder msg: 1613175934.33 epoch sec; seqNo:6143 2021-02-13T00:25:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 0.00 , 62.53 , 97.98 2021-02-13T00:25:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.02 2021-02-13T00:25:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175934.34 epoch sec; seqNo:12518; 4059131.72 north; 592529.86 east 2021-02-13T00:25:34Z- AHRS_M2 msg: 1613175934.72 epoch sec; seqNo:12420 2021-02-13T00:25:34Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.20 psi 2021-02-13T00:25:34Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-13T00:25:34Z- waiting for 1613175932.49, time is 1613175934.72 2021-02-13T00:25:34Z- LcmTrn::updateTrn() >>>> pose time 1613175934.72, meas time 1613175934.33 <<<< 2021-02-13T00:25:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175934.33 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:25:34Z- TerrainNav::motionUpdate() - currEstimate = (4059132, 592529.86, 28.04) 2021-02-13T00:25:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:25:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:25:34Z- TNavBF::Projecting Measurements 2021-02-13T00:25:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:25:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:25:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:25:34Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:25:34Z- TNavFilter::3 good beams. 2021-02-13T00:25:34Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:25:34Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:25:34Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:25:34Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:25:34Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:25:34Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:25:34Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:25:34Z- TNavBF::mmse selected from filter -1 2021-02-13T00:25:34Z- TerrainMapOctree: (vehN, vehE ) = (4059131.49, 592529.59). 2021-02-13T00:25:34Z- TerrainMapOctree: Min Distance = 2729.08. 2021-02-13T00:25:34Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:25:34Z- There is no specific good beam that all particles have in common. 2021-02-13T00:25:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:25:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.36 K: 0.130 ExpectedMeasDiff: nan currMeas.covariance: 7.36 2021-02-13T00:25:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:25:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.129 ExpectedMeasDiff: nan currMeas.covariance: 6.26 2021-02-13T00:25:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:25:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:25:34Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:25:34Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:25:34Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:25:34Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:25:34Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:25:34Z- Filter 0 No beams used in subcloud update 2021-02-13T00:25:34Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:25:34Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:25:34Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 703.5 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:25:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175934.33 sec. 2021-02-13T00:25:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:25:34Z- TNavBF::mmse selected from filter -1 2021-02-13T00:25:34Z- variances([0], [2], and psi_berg) = 703.471799 832.999800 0.000000 2021-02-13T00:25:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:25:34Z- LcmTrn::publishEstimates() - MLE : 4059131.22 592499.36 28.04 2021-02-13T00:25:34Z- LcmTrn::publishEstimates() - MMSE : 4059131.72 592529.86 28.04 2021-02-13T00:25:34Z- LcmTrn::publishEstimates() - COVAR: 703.47 833.00 0.99 0.00 2021-02-13T00:25:34Z- handleDepth()-Depth_Keller msg: 1613175934725 epoch sec; seqNo:12519; depth 28.19 2021-02-13T00:25:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175934.74 epoch sec; seqNo:12519; 4059132.10 north; 592529.85 east 2021-02-13T00:25:35Z- AHRS_M2 msg: 1613175935.12 epoch sec; seqNo:12421 2021-02-13T00:25:35Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.20 psi 2021-02-13T00:25:35Z- handleDepth()-Depth_Keller msg: 1613175935126 epoch sec; seqNo:12520; depth 28.31 2021-02-13T00:25:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175935.14 epoch sec; seqNo:12520; 4059132.10 north; 592529.85 east 2021-02-13T00:25:35Z- AHRS_M2 msg: 1613175935.52 epoch sec; seqNo:12422 2021-02-13T00:25:35Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.20 psi 2021-02-13T00:25:35Z- handleDepth()-Depth_Keller msg: 1613175935528 epoch sec; seqNo:12521; depth 28.44 2021-02-13T00:25:35Z- handleDvl() - RDI_Pathfinder msg: 1613175935.54 epoch sec; seqNo:6144 2021-02-13T00:25:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.03 , 135.84 , 0.00 , 94.74 2021-02-13T00:25:35Z- handleDvl() - RDI_Pathfinder msg: velocities 1.20 , 0.34 , -0.15 2021-02-13T00:25:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175935.55 epoch sec; seqNo:12521; 4059132.48 north; 592529.85 east 2021-02-13T00:25:35Z- AHRS_M2 msg: 1613175935.93 epoch sec; seqNo:12423 2021-02-13T00:25:35Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.20 psi 2021-02-13T00:25:35Z- handleDepth()-Depth_Keller msg: 1613175935941 epoch sec; seqNo:12522; depth 28.57 2021-02-13T00:25:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175935.96 epoch sec; seqNo:12522; 4059132.87 north; 592531.07 east 2021-02-13T00:25:36Z- AHRS_M2 msg: 1613175936.33 epoch sec; seqNo:12424 2021-02-13T00:25:36Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.20 psi 2021-02-13T00:25:36Z- handleDepth()-Depth_Keller msg: 1613175936341 epoch sec; seqNo:12523; depth 28.72 2021-02-13T00:25:36Z- handleDvl() - RDI_Pathfinder msg: 1613175936.36 epoch sec; seqNo:6145 2021-02-13T00:25:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.17 , 0.00 , 62.12 , 97.10 2021-02-13T00:25:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , -0.01 2021-02-13T00:25:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175936.37 epoch sec; seqNo:12523; 4059133.25 north; 592531.06 east 2021-02-13T00:25:36Z- AHRS_M2 msg: 1613175936.73 epoch sec; seqNo:12425 2021-02-13T00:25:36Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.20 psi 2021-02-13T00:25:36Z- handleDepth()-Depth_Keller msg: 1613175936745 epoch sec; seqNo:12524; depth 28.84 2021-02-13T00:25:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175936.76 epoch sec; seqNo:12524; 4059133.63 north; 592531.06 east 2021-02-13T00:25:37Z- AHRS_M2 msg: 1613175937.15 epoch sec; seqNo:12426 2021-02-13T00:25:37Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.20 psi 2021-02-13T00:25:37Z- handleDepth()-Depth_Keller msg: 1613175937152 epoch sec; seqNo:12525; depth 28.95 2021-02-13T00:25:37Z- handleDvl() - RDI_Pathfinder msg: 1613175937.16 epoch sec; seqNo:6146 2021-02-13T00:25:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.03 , 0.00 , 60.49 , 97.98 2021-02-13T00:25:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.03 , -0.05 2021-02-13T00:25:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175937.17 epoch sec; seqNo:12525; 4059133.63 north; 592531.06 east 2021-02-13T00:25:37Z- AHRS_M2 msg: 1613175937.54 epoch sec; seqNo:12427 2021-02-13T00:25:37Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.20 psi 2021-02-13T00:25:37Z- handleDepth()-Depth_Keller msg: 1613175937554 epoch sec; seqNo:12526; depth 29.11 2021-02-13T00:25:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175937.57 epoch sec; seqNo:12526; 4059134.01 north; 592531.05 east 2021-02-13T00:25:37Z- AHRS_M2 msg: 1613175937.95 epoch sec; seqNo:12428 2021-02-13T00:25:37Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.20 psi 2021-02-13T00:25:37Z- handleDepth()-Depth_Keller msg: 1613175937952 epoch sec; seqNo:12527; depth 29.22 2021-02-13T00:25:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175937.97 epoch sec; seqNo:12527; 4059134.38 north; 592531.05 east 2021-02-13T00:25:38Z- AHRS_M2 msg: 1613175938.36 epoch sec; seqNo:12429 2021-02-13T00:25:38Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.19 psi 2021-02-13T00:25:38Z- handleDepth()-Depth_Keller msg: 1613175938361 epoch sec; seqNo:12528; depth 29.37 2021-02-13T00:25:38Z- handleDvl() - RDI_Pathfinder msg: 1613175938.37 epoch sec; seqNo:6147 2021-02-13T00:25:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.21 , 0.00 , 60.90 , 97.98 2021-02-13T00:25:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , -0.03 2021-02-13T00:25:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175938.39 epoch sec; seqNo:12528; 4059134.78 north; 592532.27 east 2021-02-13T00:25:38Z- AHRS_M2 msg: 1613175938.76 epoch sec; seqNo:12430 2021-02-13T00:25:38Z- AHRS_M2 msg: 0.62 phi; -0.25 theta; -0.19 psi 2021-02-13T00:25:38Z- handleDepth()-Depth_Keller msg: 1613175938761 epoch sec; seqNo:12529; depth 29.52 2021-02-13T00:25:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175938.77 epoch sec; seqNo:12529; 4059135.15 north; 592532.26 east 2021-02-13T00:25:39Z- AHRS_M2 msg: 1613175939.17 epoch sec; seqNo:12431 2021-02-13T00:25:39Z- AHRS_M2 msg: 0.61 phi; -0.21 theta; -0.18 psi 2021-02-13T00:25:39Z- handleDepth()-Depth_Keller msg: 1613175939180 epoch sec; seqNo:12530; depth 29.64 2021-02-13T00:25:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175939.21 epoch sec; seqNo:12530; 4059135.15 north; 592532.26 east 2021-02-13T00:25:39Z- AHRS_M2 msg: 1613175939.56 epoch sec; seqNo:12432 2021-02-13T00:25:39Z- AHRS_M2 msg: 0.61 phi; -0.17 theta; -0.17 psi 2021-02-13T00:25:39Z- handleDepth()-Depth_Keller msg: 1613175939569 epoch sec; seqNo:12531; depth 29.78 2021-02-13T00:25:39Z- handleDvl() - RDI_Pathfinder msg: 1613175939.59 epoch sec; seqNo:6148 2021-02-13T00:25:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:25:39Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:25:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175939.60 epoch sec; seqNo:12531; 4059135.53 north; 592532.26 east 2021-02-13T00:25:39Z- LcmTrn::handleMessages() - handling 43 messages took 0.65 ms and 130 calls... 2021-02-13T00:25:39Z- waiting for 1613175937.72, time is 1613175939.60 2021-02-13T00:25:39Z- LcmTrn::updateTrn() equating pose 1613175939.60 and meas times 1613175939.60 2021-02-13T00:25:39Z- LcmTrn::updateTrn() >>>> pose time 1613175939.60, meas time 1613175939.60 <<<< 2021-02-13T00:25:39Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175939.60, ping # 0. 2021-02-13T00:25:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175939.60 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:25:39Z- TerrainNav::motionUpdate() - currEstimate = (4059136, 592532.26, 29.78) 2021-02-13T00:25:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:25:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:25:39Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:25:39Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:25:39Z- TNavBF::Projecting Measurements 2021-02-13T00:25:39Z- TNavFilter::Measurement from time = 1613175939.60 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:25:39Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:25:39Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:25:39Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:25:39Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:25:39Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:25:39Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:25:39Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:25:39Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 703.4 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:25:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:25:39Z- TNavBF::mmse selected from filter -1 2021-02-13T00:25:39Z- variances([0], [2], and psi_berg) = 703.412162 832.999768 0.000000 2021-02-13T00:25:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:25:39Z- LcmTrn::publishEstimates() - MLE : 4059135.03 592501.76 29.78 2021-02-13T00:25:39Z- LcmTrn::publishEstimates() - MMSE : 4059135.53 592532.26 29.78 2021-02-13T00:25:39Z- LcmTrn::publishEstimates() - COVAR: 703.41 833.00 1.22 0.00 2021-02-13T00:25:39Z- AHRS_M2 msg: 1613175939.97 epoch sec; seqNo:12433 2021-02-13T00:25:39Z- AHRS_M2 msg: 0.60 phi; -0.12 theta; -0.16 psi 2021-02-13T00:25:39Z- handleDepth()-Depth_Keller msg: 1613175939972 epoch sec; seqNo:12532; depth 29.88 2021-02-13T00:25:39Z- handleDvl() - RDI_Pathfinder msg: 1613175939.98 epoch sec; seqNo:6149 2021-02-13T00:25:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.94 , 0.00 , 63.94 , 0.00 2021-02-13T00:25:39Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:25:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175940.02 epoch sec; seqNo:12532; 4059135.91 north; 592532.26 east 2021-02-13T00:25:40Z- AHRS_M2 msg: 1613175940.37 epoch sec; seqNo:12434 2021-02-13T00:25:40Z- AHRS_M2 msg: 0.59 phi; -0.08 theta; -0.16 psi 2021-02-13T00:25:40Z- handleDepth()-Depth_Keller msg: 1613175940381 epoch sec; seqNo:12533; depth 29.99 2021-02-13T00:25:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175940.39 epoch sec; seqNo:12533; 4059136.29 north; 592532.25 east 2021-02-13T00:25:40Z- AHRS_M2 msg: 1613175940.78 epoch sec; seqNo:12435 2021-02-13T00:25:40Z- AHRS_M2 msg: 0.58 phi; -0.03 theta; -0.16 psi 2021-02-13T00:25:40Z- handleDepth()-Depth_Keller msg: 1613175940782 epoch sec; seqNo:12534; depth 30.07 2021-02-13T00:25:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175940.79 epoch sec; seqNo:12534; 4059136.68 north; 592533.47 east 2021-02-13T00:25:41Z- AHRS_M2 msg: 1613175941.18 epoch sec; seqNo:12436 2021-02-13T00:25:41Z- AHRS_M2 msg: 0.57 phi; 0.01 theta; -0.16 psi 2021-02-13T00:25:41Z- handleDepth()-Depth_Keller msg: 1613175941187 epoch sec; seqNo:12535; depth 30.12 2021-02-13T00:25:41Z- handleDvl() - RDI_Pathfinder msg: 1613175941.20 epoch sec; seqNo:6150 2021-02-13T00:25:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.56 , 0.00 , 68.08 , 0.00 2021-02-13T00:25:41Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:25:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175941.22 epoch sec; seqNo:12535; 4059136.68 north; 592533.47 east 2021-02-13T00:25:41Z- AHRS_M2 msg: 1613175941.58 epoch sec; seqNo:12437 2021-02-13T00:25:41Z- AHRS_M2 msg: 0.56 phi; 0.05 theta; -0.16 psi 2021-02-13T00:25:41Z- handleDepth()-Depth_Keller msg: 1613175941593 epoch sec; seqNo:12536; depth 30.19 2021-02-13T00:25:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175941.60 epoch sec; seqNo:12536; 4059137.06 north; 592533.46 east 2021-02-13T00:25:41Z- AHRS_M2 msg: 1613175941.99 epoch sec; seqNo:12438 2021-02-13T00:25:41Z- AHRS_M2 msg: 0.55 phi; 0.09 theta; -0.17 psi 2021-02-13T00:25:41Z- handleDepth()-Depth_Keller msg: 1613175941992 epoch sec; seqNo:12537; depth 30.23 2021-02-13T00:25:42Z- handleDvl() - RDI_Pathfinder msg: 1613175942.00 epoch sec; seqNo:6151 2021-02-13T00:25:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.02 , 0.00 , 0.00 , 0.00 2021-02-13T00:25:42Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:25:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175942.01 epoch sec; seqNo:12537; 4059137.44 north; 592533.46 east 2021-02-13T00:25:42Z- AHRS_M2 msg: 1613175942.39 epoch sec; seqNo:12439 2021-02-13T00:25:42Z- AHRS_M2 msg: 0.54 phi; 0.12 theta; -0.17 psi 2021-02-13T00:25:42Z- handleDepth()-Depth_Keller msg: 1613175942405 epoch sec; seqNo:12538; depth 30.25 2021-02-13T00:25:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175942.42 epoch sec; seqNo:12538; 4059137.82 north; 592533.46 east 2021-02-13T00:25:42Z- AHRS_M2 msg: 1613175942.80 epoch sec; seqNo:12440 2021-02-13T00:25:42Z- AHRS_M2 msg: 0.54 phi; 0.15 theta; -0.17 psi 2021-02-13T00:25:42Z- handleDepth()-Depth_Keller msg: 1613175942800 epoch sec; seqNo:12539; depth 30.25 2021-02-13T00:25:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:25:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:25:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:25:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:25:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:25:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:25:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:25:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:25:42Z- handleDvl() - RDI_Pathfinder msg: 1613175942.80 epoch sec; seqNo:6152 2021-02-13T00:25:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:25:42Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:25:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175942.82 epoch sec; seqNo:12539; 4059138.20 north; 592533.45 east 2021-02-13T00:25:43Z- AHRS_M2 msg: 1613175943.20 epoch sec; seqNo:12441 2021-02-13T00:25:43Z- AHRS_M2 msg: 0.53 phi; 0.18 theta; -0.17 psi 2021-02-13T00:25:43Z- handleDepth()-Depth_Keller msg: 1613175943204 epoch sec; seqNo:12540; depth 30.25 2021-02-13T00:25:43Z- handleDvl() - RDI_Pathfinder msg: 1613175943.22 epoch sec; seqNo:6153 2021-02-13T00:25:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:25:43Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:25:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175943.25 epoch sec; seqNo:12540; 4059138.59 north; 592534.67 east 2021-02-13T00:25:43Z- AHRS_M2 msg: 1613175943.60 epoch sec; seqNo:12442 2021-02-13T00:25:43Z- AHRS_M2 msg: 0.52 phi; 0.21 theta; -0.17 psi 2021-02-13T00:25:43Z- handleDepth()-Depth_Keller msg: 1613175943608 epoch sec; seqNo:12541; depth 30.22 2021-02-13T00:25:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175943.62 epoch sec; seqNo:12541; 4059138.59 north; 592534.67 east 2021-02-13T00:25:44Z- AHRS_M2 msg: 1613175944.01 epoch sec; seqNo:12443 2021-02-13T00:25:44Z- AHRS_M2 msg: 0.52 phi; 0.23 theta; -0.18 psi 2021-02-13T00:25:44Z- handleDepth()-Depth_Keller msg: 1613175944014 epoch sec; seqNo:12542; depth 30.21 2021-02-13T00:25:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:25:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:25:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:25:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:25:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:25:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:25:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:25:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:25:44Z- handleDvl() - RDI_Pathfinder msg: 1613175944.02 epoch sec; seqNo:6154 2021-02-13T00:25:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:25:44Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:25:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175944.02 epoch sec; seqNo:12542; 4059138.97 north; 592534.66 east 2021-02-13T00:25:44Z- AHRS_M2 msg: 1613175944.41 epoch sec; seqNo:12444 2021-02-13T00:25:44Z- AHRS_M2 msg: 0.52 phi; 0.25 theta; -0.18 psi 2021-02-13T00:25:44Z- handleDepth()-Depth_Keller msg: 1613175944416 epoch sec; seqNo:12543; depth 30.16 2021-02-13T00:25:44Z- handleDvl() - RDI_Pathfinder msg: 1613175944.43 epoch sec; seqNo:6155 2021-02-13T00:25:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.57 , 79.59 , 0.00 , 62.53 2021-02-13T00:25:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.06 , -0.00 2021-02-13T00:25:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175944.45 epoch sec; seqNo:12543; 4059139.35 north; 592534.66 east 2021-02-13T00:25:44Z- AHRS_M2 msg: 1613175944.82 epoch sec; seqNo:12445 2021-02-13T00:25:44Z- AHRS_M2 msg: 0.51 phi; 0.27 theta; -0.18 psi 2021-02-13T00:25:44Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-13T00:25:44Z- waiting for 1613175942.60, time is 1613175944.82 2021-02-13T00:25:44Z- LcmTrn::updateTrn() >>>> pose time 1613175944.82, meas time 1613175944.43 <<<< 2021-02-13T00:25:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175944.43 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:25:44Z- TerrainNav::motionUpdate() - currEstimate = (4059139, 592534.66, 30.16) 2021-02-13T00:25:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:25:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:25:44Z- TNavBF::Projecting Measurements 2021-02-13T00:25:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:25:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:25:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:25:44Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:25:44Z- TNavFilter::3 good beams. 2021-02-13T00:25:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:25:44Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:25:44Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:25:44Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:25:44Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:25:44Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:25:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:25:44Z- TNavBF::mmse selected from filter -1 2021-02-13T00:25:44Z- TerrainMapOctree: (vehN, vehE ) = (4059139.07, 592534.48). 2021-02-13T00:25:44Z- TerrainMapOctree: Min Distance = 2720.11. 2021-02-13T00:25:44Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:25:44Z- There is no specific good beam that all particles have in common. 2021-02-13T00:25:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:25:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.146 ExpectedMeasDiff: nan currMeas.covariance: 6.67 2021-02-13T00:25:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:25:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 10.14 2021-02-13T00:25:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:25:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.122 ExpectedMeasDiff: nan currMeas.covariance: 6.26 2021-02-13T00:25:44Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:25:44Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:25:44Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:25:44Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:25:44Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:25:44Z- Filter 0 No beams used in subcloud update 2021-02-13T00:25:44Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:25:44Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:25:44Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 703.4 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:25:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175944.43 sec. 2021-02-13T00:25:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:25:44Z- TNavBF::mmse selected from filter -1 2021-02-13T00:25:44Z- variances([0], [2], and psi_berg) = 703.338685 832.999728 0.000000 2021-02-13T00:25:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:25:44Z- LcmTrn::publishEstimates() - MLE : 4059138.85 592504.16 30.16 2021-02-13T00:25:44Z- LcmTrn::publishEstimates() - MMSE : 4059139.35 592534.66 30.16 2021-02-13T00:25:44Z- LcmTrn::publishEstimates() - COVAR: 703.34 833.00 0.99 0.00 2021-02-13T00:25:44Z- handleDepth()-Depth_Keller msg: 1613175944820 epoch sec; seqNo:12544; depth 30.11 2021-02-13T00:25:44Z- handleDvl() - RDI_Pathfinder msg: 1613175944.83 epoch sec; seqNo:6156 2021-02-13T00:25:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 66.03 , 0.00 , 0.00 , 62.47 2021-02-13T00:25:44Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:25:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175944.86 epoch sec; seqNo:12544; 4059139.73 north; 592534.66 east 2021-02-13T00:25:45Z- AHRS_M2 msg: 1613175945.22 epoch sec; seqNo:12446 2021-02-13T00:25:45Z- AHRS_M2 msg: 0.51 phi; 0.29 theta; -0.17 psi 2021-02-13T00:25:45Z- handleDepth()-Depth_Keller msg: 1613175945229 epoch sec; seqNo:12545; depth 30.05 2021-02-13T00:25:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:25:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:25:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:25:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:25:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:25:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:25:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:25:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:25:45Z- handleDvl() - RDI_Pathfinder msg: 1613175945.23 epoch sec; seqNo:6157 2021-02-13T00:25:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:25:45Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:25:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175945.25 epoch sec; seqNo:12545; 4059139.73 north; 592534.66 east 2021-02-13T00:25:45Z- AHRS_M2 msg: 1613175945.62 epoch sec; seqNo:12447 2021-02-13T00:25:45Z- AHRS_M2 msg: 0.51 phi; 0.30 theta; -0.17 psi 2021-02-13T00:25:45Z- handleDepth()-Depth_Keller msg: 1613175945630 epoch sec; seqNo:12546; depth 29.97 2021-02-13T00:25:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175945.64 epoch sec; seqNo:12546; 4059140.11 north; 592534.65 east 2021-02-13T00:25:46Z- AHRS_M2 msg: 1613175946.03 epoch sec; seqNo:12448 2021-02-13T00:25:46Z- AHRS_M2 msg: 0.52 phi; 0.32 theta; -0.16 psi 2021-02-13T00:25:46Z- handleDepth()-Depth_Keller msg: 1613175946037 epoch sec; seqNo:12547; depth 29.90 2021-02-13T00:25:46Z- handleDvl() - RDI_Pathfinder msg: 1613175946.06 epoch sec; seqNo:6158 2021-02-13T00:25:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 66.87 , 0.00 , 0.00 , 61.33 2021-02-13T00:25:46Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:25:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175946.07 epoch sec; seqNo:12547; 4059140.50 north; 592535.87 east 2021-02-13T00:25:46Z- AHRS_M2 msg: 1613175946.44 epoch sec; seqNo:12449 2021-02-13T00:25:46Z- AHRS_M2 msg: 0.52 phi; 0.34 theta; -0.15 psi 2021-02-13T00:25:46Z- handleDepth()-Depth_Keller msg: 1613175946442 epoch sec; seqNo:12548; depth 29.81 2021-02-13T00:25:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:25:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:25:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:25:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:25:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:25:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:25:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:25:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:25:46Z- handleDvl() - RDI_Pathfinder msg: 1613175946.44 epoch sec; seqNo:6159 2021-02-13T00:25:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:25:46Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:25:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175946.45 epoch sec; seqNo:12548; 4059140.88 north; 592535.86 east 2021-02-13T00:25:46Z- AHRS_M2 msg: 1613175946.85 epoch sec; seqNo:12450 2021-02-13T00:25:46Z- AHRS_M2 msg: 0.53 phi; 0.35 theta; -0.14 psi 2021-02-13T00:25:46Z- handleDepth()-Depth_Keller msg: 1613175946866 epoch sec; seqNo:12549; depth 29.72 2021-02-13T00:25:46Z- handleDvl() - RDI_Pathfinder msg: 1613175946.89 epoch sec; seqNo:6160 2021-02-13T00:25:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 68.57 , 0.00 , 0.00 , 61.76 2021-02-13T00:25:46Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:25:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175946.90 epoch sec; seqNo:12549; 4059140.88 north; 592535.86 east 2021-02-13T00:25:47Z- AHRS_M2 msg: 1613175947.25 epoch sec; seqNo:12451 2021-02-13T00:25:47Z- AHRS_M2 msg: 0.53 phi; 0.36 theta; -0.14 psi 2021-02-13T00:25:47Z- handleDepth()-Depth_Keller msg: 1613175947253 epoch sec; seqNo:12550; depth 29.63 2021-02-13T00:25:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175947.26 epoch sec; seqNo:12550; 4059141.26 north; 592535.86 east 2021-02-13T00:25:47Z- AHRS_M2 msg: 1613175947.64 epoch sec; seqNo:12452 2021-02-13T00:25:47Z- AHRS_M2 msg: 0.54 phi; 0.37 theta; -0.13 psi 2021-02-13T00:25:47Z- handleDepth()-Depth_Keller msg: 1613175947648 epoch sec; seqNo:12551; depth 29.52 2021-02-13T00:25:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175947.67 epoch sec; seqNo:12551; 4059141.63 north; 592535.86 east 2021-02-13T00:25:48Z- AHRS_M2 msg: 1613175948.05 epoch sec; seqNo:12453 2021-02-13T00:25:48Z- AHRS_M2 msg: 0.54 phi; 0.37 theta; -0.13 psi 2021-02-13T00:25:48Z- handleDepth()-Depth_Keller msg: 1613175948057 epoch sec; seqNo:12552; depth 29.42 2021-02-13T00:25:48Z- handleDvl() - RDI_Pathfinder msg: 1613175948.07 epoch sec; seqNo:6161 2021-02-13T00:25:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 69.42 , 0.00 , 0.00 , 61.76 2021-02-13T00:25:48Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:25:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175948.10 epoch sec; seqNo:12552; 4059141.63 north; 592535.86 east 2021-02-13T00:25:48Z- AHRS_M2 msg: 1613175948.45 epoch sec; seqNo:12454 2021-02-13T00:25:48Z- AHRS_M2 msg: 0.55 phi; 0.37 theta; -0.12 psi 2021-02-13T00:25:48Z- handleDepth()-Depth_Keller msg: 1613175948456 epoch sec; seqNo:12553; depth 29.30 2021-02-13T00:25:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175948.47 epoch sec; seqNo:12553; 4059142.01 north; 592535.85 east 2021-02-13T00:25:48Z- AHRS_M2 msg: 1613175948.87 epoch sec; seqNo:12455 2021-02-13T00:25:48Z- AHRS_M2 msg: 0.55 phi; 0.37 theta; -0.12 psi 2021-02-13T00:25:48Z- handleDepth()-Depth_Keller msg: 1613175948877 epoch sec; seqNo:12554; depth 29.18 2021-02-13T00:25:48Z- handleDvl() - RDI_Pathfinder msg: 1613175948.89 epoch sec; seqNo:6162 2021-02-13T00:25:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:25:48Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:25:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175948.90 epoch sec; seqNo:12554; 4059142.41 north; 592537.07 east 2021-02-13T00:25:49Z- AHRS_M2 msg: 1613175949.26 epoch sec; seqNo:12456 2021-02-13T00:25:49Z- AHRS_M2 msg: 0.56 phi; 0.37 theta; -0.11 psi 2021-02-13T00:25:49Z- handleDepth()-Depth_Keller msg: 1613175949278 epoch sec; seqNo:12555; depth 29.06 2021-02-13T00:25:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175949.29 epoch sec; seqNo:12555; 4059142.41 north; 592537.07 east 2021-02-13T00:25:49Z- AHRS_M2 msg: 1613175949.66 epoch sec; seqNo:12457 2021-02-13T00:25:49Z- AHRS_M2 msg: 0.56 phi; 0.37 theta; -0.11 psi 2021-02-13T00:25:49Z- handleDepth()-Depth_Keller msg: 1613175949668 epoch sec; seqNo:12556; depth 28.96 2021-02-13T00:25:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175949.68 epoch sec; seqNo:12556; 4059142.78 north; 592537.06 east 2021-02-13T00:25:49Z- LcmTrn::handleMessages() - handling 45 messages took 0.64 ms and 132 calls... 2021-02-13T00:25:49Z- waiting for 1613175947.82, time is 1613175949.68 2021-02-13T00:25:49Z- LcmTrn::updateTrn() >>>> pose time 1613175949.68, meas time 1613175948.89 <<<< 2021-02-13T00:25:49Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175948.89, ping # 0. 2021-02-13T00:25:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175948.89 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:25:49Z- TerrainNav::motionUpdate() - currEstimate = (4059143, 592537.06, 28.96) 2021-02-13T00:25:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:25:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:25:49Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:25:49Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:25:49Z- TNavBF::Projecting Measurements 2021-02-13T00:25:49Z- TNavFilter::Measurement from time = 1613175948.89 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:25:49Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:25:49Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:25:49Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:25:49Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:25:49Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:25:49Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:25:49Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:25:49Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 703.3 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:25:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:25:49Z- TNavBF::mmse selected from filter -1 2021-02-13T00:25:49Z- variances([0], [2], and psi_berg) = 703.262997 832.999686 0.000000 2021-02-13T00:25:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:25:49Z- LcmTrn::publishEstimates() - MLE : 4059142.28 592584.56 28.96 2021-02-13T00:25:49Z- LcmTrn::publishEstimates() - MMSE : 4059142.78 592537.06 28.96 2021-02-13T00:25:49Z- LcmTrn::publishEstimates() - COVAR: 703.26 833.00 1.21 0.00 2021-02-13T00:25:50Z- AHRS_M2 msg: 1613175950.07 epoch sec; seqNo:12458 2021-02-13T00:25:50Z- AHRS_M2 msg: 0.57 phi; 0.37 theta; -0.11 psi 2021-02-13T00:25:50Z- handleDepth()-Depth_Keller msg: 1613175950077 epoch sec; seqNo:12557; depth 28.84 2021-02-13T00:25:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175950.09 epoch sec; seqNo:12557; 4059143.16 north; 592537.06 east 2021-02-13T00:25:50Z- AHRS_M2 msg: 1613175950.47 epoch sec; seqNo:12459 2021-02-13T00:25:50Z- AHRS_M2 msg: 0.57 phi; 0.37 theta; -0.10 psi 2021-02-13T00:25:50Z- handleDepth()-Depth_Keller msg: 1613175950477 epoch sec; seqNo:12558; depth 28.69 2021-02-13T00:25:50Z- handleDvl() - RDI_Pathfinder msg: 1613175950.49 epoch sec; seqNo:6163 2021-02-13T00:25:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 80.67 , 97.98 , 60.08 2021-02-13T00:25:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.12 , -0.02 2021-02-13T00:25:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175950.50 epoch sec; seqNo:12558; 4059143.16 north; 592537.06 east 2021-02-13T00:25:50Z- AHRS_M2 msg: 1613175950.88 epoch sec; seqNo:12460 2021-02-13T00:25:50Z- AHRS_M2 msg: 0.57 phi; 0.37 theta; -0.10 psi 2021-02-13T00:25:50Z- handleDepth()-Depth_Keller msg: 1613175950890 epoch sec; seqNo:12559; depth 28.58 2021-02-13T00:25:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175950.90 epoch sec; seqNo:12559; 4059143.54 north; 592537.06 east 2021-02-13T00:25:51Z- AHRS_M2 msg: 1613175951.28 epoch sec; seqNo:12461 2021-02-13T00:25:51Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.10 psi 2021-02-13T00:25:51Z- handleDepth()-Depth_Keller msg: 1613175951284 epoch sec; seqNo:12560; depth 28.44 2021-02-13T00:25:51Z- handleDvl() - RDI_Pathfinder msg: 1613175951.30 epoch sec; seqNo:6164 2021-02-13T00:25:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 80.67 , 97.98 , 62.12 2021-02-13T00:25:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.11 , 0.04 2021-02-13T00:25:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175951.32 epoch sec; seqNo:12560; 4059143.92 north; 592537.05 east 2021-02-13T00:25:51Z- AHRS_M2 msg: 1613175951.68 epoch sec; seqNo:12462 2021-02-13T00:25:51Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.10 psi 2021-02-13T00:25:51Z- handleDepth()-Depth_Keller msg: 1613175951688 epoch sec; seqNo:12561; depth 28.31 2021-02-13T00:25:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175951.70 epoch sec; seqNo:12561; 4059144.31 north; 592538.27 east 2021-02-13T00:25:52Z- AHRS_M2 msg: 1613175952.09 epoch sec; seqNo:12463 2021-02-13T00:25:52Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.10 psi 2021-02-13T00:25:52Z- handleDepth()-Depth_Keller msg: 1613175952092 epoch sec; seqNo:12562; depth 28.18 2021-02-13T00:25:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175952.11 epoch sec; seqNo:12562; 4059144.69 north; 592538.26 east 2021-02-13T00:25:52Z- AHRS_M2 msg: 1613175952.49 epoch sec; seqNo:12464 2021-02-13T00:25:52Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.10 psi 2021-02-13T00:25:52Z- handleDepth()-Depth_Keller msg: 1613175952496 epoch sec; seqNo:12563; depth 28.06 2021-02-13T00:25:52Z- handleDvl() - RDI_Pathfinder msg: 1613175952.51 epoch sec; seqNo:6165 2021-02-13T00:25:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 81.75 , 97.98 , 60.90 2021-02-13T00:25:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , 0.09 2021-02-13T00:25:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175952.53 epoch sec; seqNo:12563; 4059144.69 north; 592538.26 east 2021-02-13T00:25:52Z- AHRS_M2 msg: 1613175952.90 epoch sec; seqNo:12465 2021-02-13T00:25:52Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.10 psi 2021-02-13T00:25:52Z- handleDepth()-Depth_Keller msg: 1613175952900 epoch sec; seqNo:12564; depth 27.93 2021-02-13T00:25:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175952.91 epoch sec; seqNo:12564; 4059145.07 north; 592538.26 east 2021-02-13T00:25:53Z- AHRS_M2 msg: 1613175953.30 epoch sec; seqNo:12466 2021-02-13T00:25:53Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.10 psi 2021-02-13T00:25:53Z- handleDepth()-Depth_Keller msg: 1613175953315 epoch sec; seqNo:12565; depth 27.81 2021-02-13T00:25:53Z- handleDvl() - RDI_Pathfinder msg: 1613175953.33 epoch sec; seqNo:6166 2021-02-13T00:25:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 80.67 , 97.98 , 62.53 2021-02-13T00:25:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , 0.10 2021-02-13T00:25:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175953.34 epoch sec; seqNo:12565; 4059145.45 north; 592538.26 east 2021-02-13T00:25:53Z- AHRS_M2 msg: 1613175953.70 epoch sec; seqNo:12467 2021-02-13T00:25:53Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.09 psi 2021-02-13T00:25:53Z- handleDepth()-Depth_Keller msg: 1613175953708 epoch sec; seqNo:12566; depth 27.66 2021-02-13T00:25:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175953.72 epoch sec; seqNo:12566; 4059145.83 north; 592538.25 east 2021-02-13T00:25:54Z- AHRS_M2 msg: 1613175954.13 epoch sec; seqNo:12468 2021-02-13T00:25:54Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.09 psi 2021-02-13T00:25:54Z- handleDepth()-Depth_Keller msg: 1613175954147 epoch sec; seqNo:12567; depth 27.54 2021-02-13T00:25:54Z- handleDvl() - RDI_Pathfinder msg: 1613175954.16 epoch sec; seqNo:6167 2021-02-13T00:25:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.93 , 78.47 , 97.98 , 63.35 2021-02-13T00:25:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.11 , 0.07 2021-02-13T00:25:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175954.17 epoch sec; seqNo:12567; 4059146.22 north; 592539.47 east 2021-02-13T00:25:54Z- AHRS_M2 msg: 1613175954.51 epoch sec; seqNo:12469 2021-02-13T00:25:54Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.09 psi 2021-02-13T00:25:54Z- handleDepth()-Depth_Keller msg: 1613175954517 epoch sec; seqNo:12568; depth 27.39 2021-02-13T00:25:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175954.53 epoch sec; seqNo:12568; 4059146.22 north; 592539.47 east 2021-02-13T00:25:54Z- AHRS_M2 msg: 1613175954.92 epoch sec; seqNo:12470 2021-02-13T00:25:54Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.09 psi 2021-02-13T00:25:54Z- LcmTrn::handleMessages() - handling 42 messages took 0.72 ms and 128 calls... 2021-02-13T00:25:54Z- waiting for 1613175952.68, time is 1613175954.92 2021-02-13T00:25:54Z- LcmTrn::updateTrn() >>>> pose time 1613175954.92, meas time 1613175954.16 <<<< 2021-02-13T00:25:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175954.16 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:25:54Z- TerrainNav::motionUpdate() - currEstimate = (4059146, 592539.47, 27.39) 2021-02-13T00:25:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:25:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:25:54Z- TNavBF::Projecting Measurements 2021-02-13T00:25:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:25:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:25:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:25:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:25:54Z- TNavFilter::4 good beams. 2021-02-13T00:25:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:25:54Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:25:54Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:25:54Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:25:54Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:25:54Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:25:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:25:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:25:54Z- TerrainMapOctree: (vehN, vehE ) = (4059145.73, 592539.12). 2021-02-13T00:25:54Z- TerrainMapOctree: Min Distance = 2712.02. 2021-02-13T00:25:54Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:25:54Z- There is no specific good beam that all particles have in common. 2021-02-13T00:25:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:25:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.39 K: 0.122 ExpectedMeasDiff: nan currMeas.covariance: 8.28 2021-02-13T00:25:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:25:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T00:25:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:25:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:25:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:25:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 6.42 2021-02-13T00:25:54Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:25:54Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:25:54Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:25:54Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:25:54Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:25:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:25:54Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:25:54Z- Filter 0 No beams used in subcloud update 2021-02-13T00:25:54Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:25:54Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:25:54Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 703.2 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:25:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175954.16 sec. 2021-02-13T00:25:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:25:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:25:54Z- variances([0], [2], and psi_berg) = 703.188129 832.999646 0.000000 2021-02-13T00:25:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:25:54Z- LcmTrn::publishEstimates() - MLE : 4059145.72 592586.97 27.39 2021-02-13T00:25:54Z- LcmTrn::publishEstimates() - MMSE : 4059146.22 592539.47 27.39 2021-02-13T00:25:54Z- LcmTrn::publishEstimates() - COVAR: 703.19 833.00 0.95 0.00 2021-02-13T00:25:54Z- handleDepth()-Depth_Keller msg: 1613175954922 epoch sec; seqNo:12569; depth 27.29 2021-02-13T00:25:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175954.93 epoch sec; seqNo:12569; 4059146.60 north; 592539.46 east 2021-02-13T00:25:55Z- AHRS_M2 msg: 1613175955.32 epoch sec; seqNo:12471 2021-02-13T00:25:55Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.09 psi 2021-02-13T00:25:55Z- handleDepth()-Depth_Keller msg: 1613175955325 epoch sec; seqNo:12570; depth 27.18 2021-02-13T00:25:55Z- handleDvl() - RDI_Pathfinder msg: 1613175955.34 epoch sec; seqNo:6168 2021-02-13T00:25:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 81.75 , 97.98 , 61.71 2021-02-13T00:25:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.12 , -0.01 2021-02-13T00:25:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175955.35 epoch sec; seqNo:12570; 4059146.98 north; 592539.46 east 2021-02-13T00:25:55Z- AHRS_M2 msg: 1613175955.72 epoch sec; seqNo:12472 2021-02-13T00:25:55Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.09 psi 2021-02-13T00:25:55Z- handleDepth()-Depth_Keller msg: 1613175955728 epoch sec; seqNo:12571; depth 27.05 2021-02-13T00:25:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175955.74 epoch sec; seqNo:12571; 4059146.98 north; 592539.46 east 2021-02-13T00:25:56Z- AHRS_M2 msg: 1613175956.13 epoch sec; seqNo:12473 2021-02-13T00:25:56Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.09 psi 2021-02-13T00:25:56Z- handleDepth()-Depth_Keller msg: 1613175956137 epoch sec; seqNo:12572; depth 26.91 2021-02-13T00:25:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175956.15 epoch sec; seqNo:12572; 4059147.36 north; 592539.46 east 2021-02-13T00:25:56Z- AHRS_M2 msg: 1613175956.53 epoch sec; seqNo:12474 2021-02-13T00:25:56Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.10 psi 2021-02-13T00:25:56Z- handleDepth()-Depth_Keller msg: 1613175956545 epoch sec; seqNo:12573; depth 26.80 2021-02-13T00:25:56Z- handleDvl() - RDI_Pathfinder msg: 1613175956.56 epoch sec; seqNo:6169 2021-02-13T00:25:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 81.75 , 97.98 , 61.71 2021-02-13T00:25:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.09 , -0.08 2021-02-13T00:25:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175956.57 epoch sec; seqNo:12573; 4059147.37 north; 592540.68 east 2021-02-13T00:25:56Z- AHRS_M2 msg: 1613175956.94 epoch sec; seqNo:12475 2021-02-13T00:25:56Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.10 psi 2021-02-13T00:25:56Z- handleDepth()-Depth_Keller msg: 1613175956941 epoch sec; seqNo:12574; depth 26.67 2021-02-13T00:25:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175956.96 epoch sec; seqNo:12574; 4059147.75 north; 592540.67 east 2021-02-13T00:25:57Z- AHRS_M2 msg: 1613175957.34 epoch sec; seqNo:12476 2021-02-13T00:25:57Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.11 psi 2021-02-13T00:25:57Z- handleDepth()-Depth_Keller msg: 1613175957349 epoch sec; seqNo:12575; depth 26.54 2021-02-13T00:25:57Z- handleDvl() - RDI_Pathfinder msg: 1613175957.36 epoch sec; seqNo:6170 2021-02-13T00:25:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 81.75 , 96.90 , 64.16 2021-02-13T00:25:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.04 , -0.06 2021-02-13T00:25:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175957.38 epoch sec; seqNo:12575; 4059148.13 north; 592540.67 east 2021-02-13T00:25:57Z- AHRS_M2 msg: 1613175957.74 epoch sec; seqNo:12477 2021-02-13T00:25:57Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.13 psi 2021-02-13T00:25:57Z- handleDepth()-Depth_Keller msg: 1613175957748 epoch sec; seqNo:12576; depth 26.41 2021-02-13T00:25:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175957.76 epoch sec; seqNo:12576; 4059148.13 north; 592540.67 east 2021-02-13T00:25:58Z- AHRS_M2 msg: 1613175958.15 epoch sec; seqNo:12478 2021-02-13T00:25:58Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.14 psi 2021-02-13T00:25:58Z- handleDepth()-Depth_Keller msg: 1613175958159 epoch sec; seqNo:12577; depth 26.30 2021-02-13T00:25:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175958.19 epoch sec; seqNo:12577; 4059148.51 north; 592540.66 east 2021-02-13T00:25:58Z- AHRS_M2 msg: 1613175958.55 epoch sec; seqNo:12479 2021-02-13T00:25:58Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.16 psi 2021-02-13T00:25:58Z- handleDepth()-Depth_Keller msg: 1613175958557 epoch sec; seqNo:12578; depth 26.17 2021-02-13T00:25:58Z- handleDvl() - RDI_Pathfinder msg: 1613175958.58 epoch sec; seqNo:6171 2021-02-13T00:25:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.52 , 84.93 , 97.91 , 63.76 2021-02-13T00:25:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , -0.02 2021-02-13T00:25:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175958.59 epoch sec; seqNo:12578; 4059148.89 north; 592540.66 east 2021-02-13T00:25:58Z- AHRS_M2 msg: 1613175958.96 epoch sec; seqNo:12480 2021-02-13T00:25:58Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.17 psi 2021-02-13T00:25:58Z- handleDepth()-Depth_Keller msg: 1613175958965 epoch sec; seqNo:12579; depth 26.03 2021-02-13T00:25:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175958.98 epoch sec; seqNo:12579; 4059148.90 north; 592541.88 east 2021-02-13T00:25:59Z- AHRS_M2 msg: 1613175959.36 epoch sec; seqNo:12481 2021-02-13T00:25:59Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.18 psi 2021-02-13T00:25:59Z- handleDepth()-Depth_Keller msg: 1613175959369 epoch sec; seqNo:12580; depth 25.92 2021-02-13T00:25:59Z- handleDvl() - RDI_Pathfinder msg: 1613175959.38 epoch sec; seqNo:6172 2021-02-13T00:25:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 86.08 , 97.98 , 64.98 2021-02-13T00:25:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , 0.04 2021-02-13T00:25:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175959.39 epoch sec; seqNo:12580; 4059149.28 north; 592541.88 east 2021-02-13T00:25:59Z- AHRS_M2 msg: 1613175959.77 epoch sec; seqNo:12482 2021-02-13T00:25:59Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.19 psi 2021-02-13T00:25:59Z- handleDepth()-Depth_Keller msg: 1613175959775 epoch sec; seqNo:12581; depth 25.80 2021-02-13T00:25:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175959.79 epoch sec; seqNo:12581; 4059149.66 north; 592541.87 east 2021-02-13T00:26:00Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 131 calls... 2021-02-13T00:26:00Z- waiting for 1613175957.92, time is 1613175959.79 2021-02-13T00:26:00Z- LcmTrn::updateTrn() >>>> pose time 1613175959.79, meas time 1613175959.38 <<<< 2021-02-13T00:26:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175959.38 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:26:00Z- TerrainNav::motionUpdate() - currEstimate = (4059150, 592541.87, 25.80) 2021-02-13T00:26:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:26:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:26:00Z- TNavBF::Projecting Measurements 2021-02-13T00:26:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:26:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:26:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:26:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:26:00Z- TNavFilter::4 good beams. 2021-02-13T00:26:00Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:26:00Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:26:00Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:26:00Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:26:00Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:26:00Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:26:00Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:26:00Z- TNavBF::mmse selected from filter -1 2021-02-13T00:26:00Z- TerrainMapOctree: (vehN, vehE ) = (4059149.37, 592541.67). 2021-02-13T00:26:00Z- TerrainMapOctree: Min Distance = 2707.59. 2021-02-13T00:26:00Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:26:00Z- There is no specific good beam that all particles have in common. 2021-02-13T00:26:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:26:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-13T00:26:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:26:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 11.86 2021-02-13T00:26:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:26:00Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:26:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:26:00Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 6.76 2021-02-13T00:26:00Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:26:00Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:26:00Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:26:00Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:26:00Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:26:00Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:26:00Z- Filter 0 No beams used in subcloud update 2021-02-13T00:26:00Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:26:00Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:26:00Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 703.1 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:26:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175959.38 sec. 2021-02-13T00:26:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:26:00Z- TNavBF::mmse selected from filter -1 2021-02-13T00:26:00Z- variances([0], [2], and psi_berg) = 703.116635 832.999607 0.000000 2021-02-13T00:26:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:26:00Z- LcmTrn::publishEstimates() - MLE : 4059149.16 592589.37 25.80 2021-02-13T00:26:00Z- LcmTrn::publishEstimates() - MMSE : 4059149.66 592541.87 25.80 2021-02-13T00:26:00Z- LcmTrn::publishEstimates() - COVAR: 703.12 833.00 0.83 0.00 2021-02-13T00:26:00Z- AHRS_M2 msg: 1613175960.17 epoch sec; seqNo:12483 2021-02-13T00:26:00Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.19 psi 2021-02-13T00:26:00Z- handleDepth()-Depth_Keller msg: 1613175960179 epoch sec; seqNo:12582; depth 25.68 2021-02-13T00:26:00Z- handleDvl() - RDI_Pathfinder msg: 1613175960.20 epoch sec; seqNo:6173 2021-02-13T00:26:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 0.00 , 97.91 , 64.98 2021-02-13T00:26:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , 0.06 2021-02-13T00:26:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175960.21 epoch sec; seqNo:12582; 4059149.66 north; 592541.87 east 2021-02-13T00:26:00Z- AHRS_M2 msg: 1613175960.57 epoch sec; seqNo:12484 2021-02-13T00:26:00Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.19 psi 2021-02-13T00:26:00Z- handleDepth()-Depth_Keller msg: 1613175960581 epoch sec; seqNo:12583; depth 25.56 2021-02-13T00:26:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175960.59 epoch sec; seqNo:12583; 4059150.04 north; 592541.87 east 2021-02-13T00:26:00Z- AHRS_M2 msg: 1613175960.98 epoch sec; seqNo:12485 2021-02-13T00:26:00Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.20 psi 2021-02-13T00:26:00Z- handleDepth()-Depth_Keller msg: 1613175960982 epoch sec; seqNo:12584; depth 25.44 2021-02-13T00:26:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175960.99 epoch sec; seqNo:12584; 4059150.41 north; 592541.87 east 2021-02-13T00:26:01Z- AHRS_M2 msg: 1613175961.38 epoch sec; seqNo:12486 2021-02-13T00:26:01Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.19 psi 2021-02-13T00:26:01Z- handleDepth()-Depth_Keller msg: 1613175961383 epoch sec; seqNo:12585; depth 25.32 2021-02-13T00:26:01Z- handleDvl() - RDI_Pathfinder msg: 1613175961.39 epoch sec; seqNo:6174 2021-02-13T00:26:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 0.00 , 99.06 , 64.16 2021-02-13T00:26:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , 0.04 2021-02-13T00:26:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175961.41 epoch sec; seqNo:12585; 4059150.43 north; 592543.09 east 2021-02-13T00:26:01Z- AHRS_M2 msg: 1613175961.78 epoch sec; seqNo:12487 2021-02-13T00:26:01Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.19 psi 2021-02-13T00:26:01Z- handleDepth()-Depth_Keller msg: 1613175961793 epoch sec; seqNo:12586; depth 25.21 2021-02-13T00:26:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175961.80 epoch sec; seqNo:12586; 4059150.81 north; 592543.08 east 2021-02-13T00:26:02Z- AHRS_M2 msg: 1613175962.21 epoch sec; seqNo:12488 2021-02-13T00:26:02Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.19 psi 2021-02-13T00:26:02Z- handleDepth()-Depth_Keller msg: 1613175962213 epoch sec; seqNo:12587; depth 25.10 2021-02-13T00:26:02Z- handleDvl() - RDI_Pathfinder msg: 1613175962.23 epoch sec; seqNo:6175 2021-02-13T00:26:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 89.33 , 99.06 , 64.98 2021-02-13T00:26:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , 0.01 2021-02-13T00:26:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175962.25 epoch sec; seqNo:12587; 4059151.18 north; 592543.08 east 2021-02-13T00:26:02Z- AHRS_M2 msg: 1613175962.59 epoch sec; seqNo:12489 2021-02-13T00:26:02Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.20 psi 2021-02-13T00:26:02Z- handleDepth()-Depth_Keller msg: 1613175962596 epoch sec; seqNo:12588; depth 24.95 2021-02-13T00:26:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175962.62 epoch sec; seqNo:12588; 4059151.18 north; 592543.08 east 2021-02-13T00:26:02Z- AHRS_M2 msg: 1613175963.00 epoch sec; seqNo:12490 2021-02-13T00:26:02Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-13T00:26:03Z- handleDepth()-Depth_Keller msg: 1613175963000 epoch sec; seqNo:12589; depth 24.85 2021-02-13T00:26:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175963.01 epoch sec; seqNo:12589; 4059151.56 north; 592543.07 east 2021-02-13T00:26:03Z- AHRS_M2 msg: 1613175963.40 epoch sec; seqNo:12491 2021-02-13T00:26:03Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.20 psi 2021-02-13T00:26:03Z- handleDepth()-Depth_Keller msg: 1613175963409 epoch sec; seqNo:12590; depth 24.74 2021-02-13T00:26:03Z- handleDvl() - RDI_Pathfinder msg: 1613175963.42 epoch sec; seqNo:6176 2021-02-13T00:26:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 90.27 , 97.85 , 65.39 2021-02-13T00:26:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.02 2021-02-13T00:26:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175963.43 epoch sec; seqNo:12590; 4059151.94 north; 592543.07 east 2021-02-13T00:26:03Z- AHRS_M2 msg: 1613175963.81 epoch sec; seqNo:12492 2021-02-13T00:26:03Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.20 psi 2021-02-13T00:26:03Z- handleDepth()-Depth_Keller msg: 1613175963815 epoch sec; seqNo:12591; depth 24.63 2021-02-13T00:26:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175963.83 epoch sec; seqNo:12591; 4059152.33 north; 592544.29 east 2021-02-13T00:26:04Z- AHRS_M2 msg: 1613175964.21 epoch sec; seqNo:12493 2021-02-13T00:26:04Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.20 psi 2021-02-13T00:26:04Z- handleDepth()-Depth_Keller msg: 1613175964213 epoch sec; seqNo:12592; depth 24.51 2021-02-13T00:26:04Z- handleDvl() - RDI_Pathfinder msg: 1613175964.23 epoch sec; seqNo:6177 2021-02-13T00:26:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 0.00 , 97.85 , 67.03 2021-02-13T00:26:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , -0.02 2021-02-13T00:26:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175964.24 epoch sec; seqNo:12592; 4059152.33 north; 592544.29 east 2021-02-13T00:26:04Z- AHRS_M2 msg: 1613175964.61 epoch sec; seqNo:12494 2021-02-13T00:26:04Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.20 psi 2021-02-13T00:26:04Z- handleDepth()-Depth_Keller msg: 1613175964617 epoch sec; seqNo:12593; depth 24.40 2021-02-13T00:26:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175964.63 epoch sec; seqNo:12593; 4059152.71 north; 592544.28 east 2021-02-13T00:26:05Z- AHRS_M2 msg: 1613175965.02 epoch sec; seqNo:12495 2021-02-13T00:26:05Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.20 psi 2021-02-13T00:26:05Z- handleDepth()-Depth_Keller msg: 1613175965020 epoch sec; seqNo:12594; depth 24.28 2021-02-13T00:26:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175965.03 epoch sec; seqNo:12594; 4059153.09 north; 592544.28 east 2021-02-13T00:26:05Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-13T00:26:05Z- waiting for 1613175962.79, time is 1613175965.03 2021-02-13T00:26:05Z- LcmTrn::updateTrn() >>>> pose time 1613175965.03, meas time 1613175964.23 <<<< 2021-02-13T00:26:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175964.23 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:26:05Z- TerrainNav::motionUpdate() - currEstimate = (4059153, 592544.28, 24.28) 2021-02-13T00:26:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:26:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:26:05Z- TNavBF::Projecting Measurements 2021-02-13T00:26:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:26:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:26:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:26:05Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:26:05Z- TNavFilter::3 good beams. 2021-02-13T00:26:05Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:26:05Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:26:05Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:26:05Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:26:05Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:26:05Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:26:05Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:26:05Z- TNavBF::mmse selected from filter -1 2021-02-13T00:26:05Z- TerrainMapOctree: (vehN, vehE ) = (4059152.57, 592543.91). 2021-02-13T00:26:05Z- TerrainMapOctree: Min Distance = 2703.69. 2021-02-13T00:26:05Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:26:05Z- There is no specific good beam that all particles have in common. 2021-02-13T00:26:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:26:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-13T00:26:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:26:05Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-13T00:26:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:26:05Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 7.19 2021-02-13T00:26:05Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:26:05Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:26:05Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:26:05Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:26:05Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:26:05Z- Filter 0 No beams used in subcloud update 2021-02-13T00:26:05Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:26:05Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:26:05Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 703.1 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:26:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175964.23 sec. 2021-02-13T00:26:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:26:05Z- TNavBF::mmse selected from filter -1 2021-02-13T00:26:05Z- variances([0], [2], and psi_berg) = 703.041520 832.999566 0.000000 2021-02-13T00:26:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:26:05Z- LcmTrn::publishEstimates() - MLE : 4059152.59 592591.78 24.28 2021-02-13T00:26:05Z- LcmTrn::publishEstimates() - MMSE : 4059153.09 592544.28 24.28 2021-02-13T00:26:05Z- LcmTrn::publishEstimates() - COVAR: 703.04 833.00 0.83 0.00 2021-02-13T00:26:05Z- AHRS_M2 msg: 1613175965.42 epoch sec; seqNo:12496 2021-02-13T00:26:05Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.20 psi 2021-02-13T00:26:05Z- handleDepth()-Depth_Keller msg: 1613175965430 epoch sec; seqNo:12595; depth 24.17 2021-02-13T00:26:05Z- handleDvl() - RDI_Pathfinder msg: 1613175965.44 epoch sec; seqNo:6178 2021-02-13T00:26:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 90.41 , 97.98 , 67.43 2021-02-13T00:26:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.08 , 0.03 2021-02-13T00:26:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175965.45 epoch sec; seqNo:12595; 4059153.47 north; 592544.27 east 2021-02-13T00:26:05Z- AHRS_M2 msg: 1613175965.82 epoch sec; seqNo:12497 2021-02-13T00:26:05Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.20 psi 2021-02-13T00:26:05Z- handleDepth()-Depth_Keller msg: 1613175965835 epoch sec; seqNo:12596; depth 24.04 2021-02-13T00:26:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175965.85 epoch sec; seqNo:12596; 4059153.47 north; 592544.27 east 2021-02-13T00:26:06Z- AHRS_M2 msg: 1613175966.23 epoch sec; seqNo:12498 2021-02-13T00:26:06Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.20 psi 2021-02-13T00:26:06Z- handleDepth()-Depth_Keller msg: 1613175966241 epoch sec; seqNo:12597; depth 23.93 2021-02-13T00:26:06Z- handleDvl() - RDI_Pathfinder msg: 1613175966.26 epoch sec; seqNo:6179 2021-02-13T00:26:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 90.21 , 97.78 , 67.03 2021-02-13T00:26:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , 0.08 2021-02-13T00:26:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175966.27 epoch sec; seqNo:12597; 4059153.85 north; 592544.27 east 2021-02-13T00:26:06Z- AHRS_M2 msg: 1613175966.63 epoch sec; seqNo:12499 2021-02-13T00:26:06Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.20 psi 2021-02-13T00:26:06Z- handleDepth()-Depth_Keller msg: 1613175966643 epoch sec; seqNo:12598; depth 23.83 2021-02-13T00:26:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175966.68 epoch sec; seqNo:12598; 4059154.24 north; 592545.49 east 2021-02-13T00:26:07Z- AHRS_M2 msg: 1613175967.04 epoch sec; seqNo:12500 2021-02-13T00:26:07Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.20 psi 2021-02-13T00:26:07Z- handleDepth()-Depth_Keller msg: 1613175967045 epoch sec; seqNo:12599; depth 23.71 2021-02-13T00:26:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175967.05 epoch sec; seqNo:12599; 4059154.62 north; 592545.48 east 2021-02-13T00:26:07Z- AHRS_M2 msg: 1613175967.44 epoch sec; seqNo:12501 2021-02-13T00:26:07Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.20 psi 2021-02-13T00:26:07Z- handleDepth()-Depth_Keller msg: 1613175967446 epoch sec; seqNo:12600; depth 23.60 2021-02-13T00:26:07Z- handleDvl() - RDI_Pathfinder msg: 1613175967.46 epoch sec; seqNo:6180 2021-02-13T00:26:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.93 , 91.36 , 98.93 , 66.62 2021-02-13T00:26:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.02 , 0.09 2021-02-13T00:26:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175967.47 epoch sec; seqNo:12600; 4059155.00 north; 592545.48 east 2021-02-13T00:26:07Z- AHRS_M2 msg: 1613175967.84 epoch sec; seqNo:12502 2021-02-13T00:26:07Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.20 psi 2021-02-13T00:26:07Z- handleDepth()-Depth_Keller msg: 1613175967849 epoch sec; seqNo:12601; depth 23.49 2021-02-13T00:26:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175967.87 epoch sec; seqNo:12601; 4059155.38 north; 592545.47 east 2021-02-13T00:26:08Z- AHRS_M2 msg: 1613175968.25 epoch sec; seqNo:12503 2021-02-13T00:26:08Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.20 psi 2021-02-13T00:26:08Z- handleDepth()-Depth_Keller msg: 1613175968257 epoch sec; seqNo:12602; depth 23.37 2021-02-13T00:26:08Z- handleDvl() - RDI_Pathfinder msg: 1613175968.27 epoch sec; seqNo:6181 2021-02-13T00:26:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 91.29 , 98.86 , 67.43 2021-02-13T00:26:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , 0.06 2021-02-13T00:26:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175968.29 epoch sec; seqNo:12602; 4059155.38 north; 592545.47 east 2021-02-13T00:26:08Z- AHRS_M2 msg: 1613175968.65 epoch sec; seqNo:12504 2021-02-13T00:26:08Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.19 psi 2021-02-13T00:26:08Z- handleDepth()-Depth_Keller msg: 1613175968658 epoch sec; seqNo:12603; depth 23.26 2021-02-13T00:26:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175968.67 epoch sec; seqNo:12603; 4059155.76 north; 592545.47 east 2021-02-13T00:26:09Z- AHRS_M2 msg: 1613175969.06 epoch sec; seqNo:12505 2021-02-13T00:26:09Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.19 psi 2021-02-13T00:26:09Z- handleDepth()-Depth_Keller msg: 1613175969060 epoch sec; seqNo:12604; depth 23.16 2021-02-13T00:26:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175969.07 epoch sec; seqNo:12604; 4059156.15 north; 592546.69 east 2021-02-13T00:26:09Z- AHRS_M2 msg: 1613175969.46 epoch sec; seqNo:12506 2021-02-13T00:26:09Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T00:26:09Z- handleDepth()-Depth_Keller msg: 1613175969469 epoch sec; seqNo:12605; depth 23.06 2021-02-13T00:26:09Z- handleDvl() - RDI_Pathfinder msg: 1613175969.48 epoch sec; seqNo:6182 2021-02-13T00:26:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 98.39 , 98.39 , 66.62 2021-02-13T00:26:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.06 , 0.01 2021-02-13T00:26:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175969.50 epoch sec; seqNo:12605; 4059156.53 north; 592546.68 east 2021-02-13T00:26:09Z- AHRS_M2 msg: 1613175969.86 epoch sec; seqNo:12507 2021-02-13T00:26:09Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.17 psi 2021-02-13T00:26:09Z- handleDepth()-Depth_Keller msg: 1613175969868 epoch sec; seqNo:12606; depth 22.94 2021-02-13T00:26:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175969.88 epoch sec; seqNo:12606; 4059156.53 north; 592546.68 east 2021-02-13T00:26:10Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-13T00:26:10Z- waiting for 1613175968.03, time is 1613175969.88 2021-02-13T00:26:10Z- LcmTrn::updateTrn() >>>> pose time 1613175969.88, meas time 1613175969.48 <<<< 2021-02-13T00:26:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175969.48 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:26:10Z- TerrainNav::motionUpdate() - currEstimate = (4059157, 592546.68, 22.94) 2021-02-13T00:26:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:26:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:26:10Z- TNavBF::Projecting Measurements 2021-02-13T00:26:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:26:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:26:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:26:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:26:10Z- TNavFilter::4 good beams. 2021-02-13T00:26:10Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:26:10Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:26:10Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:26:10Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:26:10Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:26:10Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:26:10Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:26:10Z- TNavBF::mmse selected from filter -1 2021-02-13T00:26:10Z- TerrainMapOctree: (vehN, vehE ) = (4059156.25, 592546.49). 2021-02-13T00:26:10Z- TerrainMapOctree: Min Distance = 2699.22. 2021-02-13T00:26:10Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:26:10Z- There is no specific good beam that all particles have in common. 2021-02-13T00:26:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:26:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-13T00:26:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:26:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 15.49 2021-02-13T00:26:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:26:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 15.49 2021-02-13T00:26:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:26:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 7.10 2021-02-13T00:26:10Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:26:10Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:26:10Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:26:10Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:26:10Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:26:10Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:26:10Z- Filter 0 No beams used in subcloud update 2021-02-13T00:26:10Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:26:10Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:26:10Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 703.0 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:26:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175969.48 sec. 2021-02-13T00:26:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:26:10Z- TNavBF::mmse selected from filter -1 2021-02-13T00:26:10Z- variances([0], [2], and psi_berg) = 702.970267 832.999527 0.000000 2021-02-13T00:26:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:26:10Z- LcmTrn::publishEstimates() - MLE : 4059156.03 592594.18 22.94 2021-02-13T00:26:10Z- LcmTrn::publishEstimates() - MMSE : 4059156.53 592546.68 22.94 2021-02-13T00:26:10Z- LcmTrn::publishEstimates() - COVAR: 702.97 833.00 0.78 0.00 2021-02-13T00:26:10Z- AHRS_M2 msg: 1613175970.28 epoch sec; seqNo:12508 2021-02-13T00:26:10Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T00:26:10Z- handleDepth()-Depth_Keller msg: 1613175970279 epoch sec; seqNo:12607; depth 22.84 2021-02-13T00:26:10Z- handleDvl() - RDI_Pathfinder msg: 1613175970.29 epoch sec; seqNo:6183 2021-02-13T00:26:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 89.73 , 99.47 , 67.03 2021-02-13T00:26:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.08 , -0.07 2021-02-13T00:26:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175970.30 epoch sec; seqNo:12607; 4059156.91 north; 592546.68 east 2021-02-13T00:26:10Z- AHRS_M2 msg: 1613175970.67 epoch sec; seqNo:12509 2021-02-13T00:26:10Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.16 psi 2021-02-13T00:26:10Z- handleDepth()-Depth_Keller msg: 1613175970677 epoch sec; seqNo:12608; depth 22.72 2021-02-13T00:26:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175970.69 epoch sec; seqNo:12608; 4059157.29 north; 592546.67 east 2021-02-13T00:26:11Z- AHRS_M2 msg: 1613175971.08 epoch sec; seqNo:12510 2021-02-13T00:26:11Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.15 psi 2021-02-13T00:26:11Z- handleDepth()-Depth_Keller msg: 1613175971085 epoch sec; seqNo:12609; depth 22.61 2021-02-13T00:26:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175971.10 epoch sec; seqNo:12609; 4059157.30 north; 592547.89 east 2021-02-13T00:26:11Z- AHRS_M2 msg: 1613175971.48 epoch sec; seqNo:12511 2021-02-13T00:26:11Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-13T00:26:11Z- handleDepth()-Depth_Keller msg: 1613175971484 epoch sec; seqNo:12610; depth 22.51 2021-02-13T00:26:11Z- handleDvl() - RDI_Pathfinder msg: 1613175971.49 epoch sec; seqNo:6184 2021-02-13T00:26:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 152.96 , 101.03 , 0.00 2021-02-13T00:26:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.48 , 0.40 , -0.34 2021-02-13T00:26:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175971.51 epoch sec; seqNo:12610; 4059157.68 north; 592547.89 east 2021-02-13T00:26:11Z- AHRS_M2 msg: 1613175971.88 epoch sec; seqNo:12512 2021-02-13T00:26:11Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-13T00:26:11Z- handleDepth()-Depth_Keller msg: 1613175971889 epoch sec; seqNo:12611; depth 22.39 2021-02-13T00:26:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175971.90 epoch sec; seqNo:12611; 4059157.68 north; 592547.89 east 2021-02-13T00:26:12Z- AHRS_M2 msg: 1613175972.32 epoch sec; seqNo:12513 2021-02-13T00:26:12Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-13T00:26:12Z- handleDepth()-Depth_Keller msg: 1613175972321 epoch sec; seqNo:12612; depth 22.27 2021-02-13T00:26:12Z- handleDvl() - RDI_Pathfinder msg: 1613175972.34 epoch sec; seqNo:6185 2021-02-13T00:26:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.62 , 89.80 , 100.22 , 66.86 2021-02-13T00:26:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , -0.13 2021-02-13T00:26:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175972.35 epoch sec; seqNo:12612; 4059157.68 north; 592547.89 east 2021-02-13T00:26:12Z- AHRS_M2 msg: 1613175972.69 epoch sec; seqNo:12514 2021-02-13T00:26:12Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:26:12Z- handleDepth()-Depth_Keller msg: 1613175972696 epoch sec; seqNo:12613; depth 22.14 2021-02-13T00:26:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175972.71 epoch sec; seqNo:12613; 4059158.06 north; 592547.89 east 2021-02-13T00:26:13Z- AHRS_M2 msg: 1613175973.11 epoch sec; seqNo:12515 2021-02-13T00:26:13Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:26:13Z- handleDepth()-Depth_Keller msg: 1613175973120 epoch sec; seqNo:12614; depth 22.04 2021-02-13T00:26:13Z- handleDvl() - RDI_Pathfinder msg: 1613175973.13 epoch sec; seqNo:6186 2021-02-13T00:26:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.97 , 0.00 , 98.39 , 67.43 2021-02-13T00:26:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.14 , -0.06 2021-02-13T00:26:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175973.16 epoch sec; seqNo:12614; 4059158.44 north; 592547.88 east 2021-02-13T00:26:13Z- AHRS_M2 msg: 1613175973.50 epoch sec; seqNo:12516 2021-02-13T00:26:13Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.15 psi 2021-02-13T00:26:13Z- handleDepth()-Depth_Keller msg: 1613175973514 epoch sec; seqNo:12615; depth 21.92 2021-02-13T00:26:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175973.53 epoch sec; seqNo:12615; 4059158.81 north; 592547.88 east 2021-02-13T00:26:13Z- AHRS_M2 msg: 1613175973.90 epoch sec; seqNo:12517 2021-02-13T00:26:13Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.15 psi 2021-02-13T00:26:13Z- handleDepth()-Depth_Keller msg: 1613175973908 epoch sec; seqNo:12616; depth 21.81 2021-02-13T00:26:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175973.92 epoch sec; seqNo:12616; 4059158.81 north; 592547.88 east 2021-02-13T00:26:14Z- AHRS_M2 msg: 1613175974.31 epoch sec; seqNo:12518 2021-02-13T00:26:14Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.15 psi 2021-02-13T00:26:14Z- handleDepth()-Depth_Keller msg: 1613175974312 epoch sec; seqNo:12617; depth 21.70 2021-02-13T00:26:14Z- handleDvl() - RDI_Pathfinder msg: 1613175974.32 epoch sec; seqNo:6187 2021-02-13T00:26:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 89.33 , 97.98 , 68.66 2021-02-13T00:26:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.15 , 0.06 2021-02-13T00:26:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175974.33 epoch sec; seqNo:12617; 4059159.21 north; 592549.10 east 2021-02-13T00:26:14Z- AHRS_M2 msg: 1613175974.71 epoch sec; seqNo:12519 2021-02-13T00:26:14Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.15 psi 2021-02-13T00:26:14Z- handleDepth()-Depth_Keller msg: 1613175974717 epoch sec; seqNo:12618; depth 21.60 2021-02-13T00:26:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175974.73 epoch sec; seqNo:12618; 4059159.59 north; 592549.09 east 2021-02-13T00:26:15Z- AHRS_M2 msg: 1613175975.12 epoch sec; seqNo:12520 2021-02-13T00:26:15Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.16 psi 2021-02-13T00:26:15Z- handleDepth()-Depth_Keller msg: 1613175975120 epoch sec; seqNo:12619; depth 21.47 2021-02-13T00:26:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175975.14 epoch sec; seqNo:12619; 4059159.96 north; 592549.09 east 2021-02-13T00:26:15Z- LcmTrn::handleMessages() - handling 44 messages took 0.62 ms and 132 calls... 2021-02-13T00:26:15Z- waiting for 1613175972.88, time is 1613175975.14 2021-02-13T00:26:15Z- LcmTrn::updateTrn() >>>> pose time 1613175975.14, meas time 1613175974.32 <<<< 2021-02-13T00:26:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175974.32 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:26:15Z- TerrainNav::motionUpdate() - currEstimate = (4059160, 592549.09, 21.47) 2021-02-13T00:26:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:26:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:26:15Z- TNavBF::Projecting Measurements 2021-02-13T00:26:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:26:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:26:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:26:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:26:15Z- TNavFilter::4 good beams. 2021-02-13T00:26:15Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:26:15Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:26:15Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:26:15Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:26:15Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:26:15Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:26:15Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:26:15Z- TNavBF::mmse selected from filter -1 2021-02-13T00:26:15Z- TerrainMapOctree: (vehN, vehE ) = (4059159.43, 592548.71). 2021-02-13T00:26:15Z- TerrainMapOctree: Min Distance = 2695.34. 2021-02-13T00:26:15Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:26:15Z- There is no specific good beam that all particles have in common. 2021-02-13T00:26:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:26:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:26:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:26:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 12.77 2021-02-13T00:26:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:26:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:26:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:26:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 7.54 2021-02-13T00:26:15Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:26:15Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:26:15Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:26:15Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:26:15Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:26:15Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:26:15Z- Filter 0 No beams used in subcloud update 2021-02-13T00:26:15Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:26:15Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:26:15Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 702.9 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:26:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175974.32 sec. 2021-02-13T00:26:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:26:15Z- TNavBF::mmse selected from filter -1 2021-02-13T00:26:15Z- variances([0], [2], and psi_berg) = 702.895147 832.999487 0.000000 2021-02-13T00:26:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:26:15Z- LcmTrn::publishEstimates() - MLE : 4059159.46 592596.59 21.47 2021-02-13T00:26:15Z- LcmTrn::publishEstimates() - MMSE : 4059159.96 592549.09 21.47 2021-02-13T00:26:15Z- LcmTrn::publishEstimates() - COVAR: 702.90 833.00 0.77 0.00 2021-02-13T00:26:15Z- AHRS_M2 msg: 1613175975.52 epoch sec; seqNo:12521 2021-02-13T00:26:15Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.16 psi 2021-02-13T00:26:15Z- handleDepth()-Depth_Keller msg: 1613175975524 epoch sec; seqNo:12620; depth 21.37 2021-02-13T00:26:15Z- handleDvl() - RDI_Pathfinder msg: 1613175975.54 epoch sec; seqNo:6188 2021-02-13T00:26:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 89.80 , 99.54 , 67.43 2021-02-13T00:26:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.11 , 0.14 2021-02-13T00:26:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175975.56 epoch sec; seqNo:12620; 4059159.96 north; 592549.09 east 2021-02-13T00:26:15Z- AHRS_M2 msg: 1613175975.92 epoch sec; seqNo:12522 2021-02-13T00:26:15Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-13T00:26:15Z- handleDepth()-Depth_Keller msg: 1613175975928 epoch sec; seqNo:12621; depth 21.25 2021-02-13T00:26:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175975.94 epoch sec; seqNo:12621; 4059160.34 north; 592549.08 east 2021-02-13T00:26:16Z- AHRS_M2 msg: 1613175976.33 epoch sec; seqNo:12523 2021-02-13T00:26:16Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-13T00:26:16Z- handleDepth()-Depth_Keller msg: 1613175976336 epoch sec; seqNo:12622; depth 21.14 2021-02-13T00:26:16Z- handleDvl() - RDI_Pathfinder msg: 1613175976.35 epoch sec; seqNo:6189 2021-02-13T00:26:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.20 , 98.12 , 99.33 , 67.84 2021-02-13T00:26:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.15 2021-02-13T00:26:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175976.36 epoch sec; seqNo:12622; 4059160.72 north; 592549.08 east 2021-02-13T00:26:16Z- AHRS_M2 msg: 1613175976.73 epoch sec; seqNo:12524 2021-02-13T00:26:16Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.18 psi 2021-02-13T00:26:16Z- handleDepth()-Depth_Keller msg: 1613175976741 epoch sec; seqNo:12623; depth 21.03 2021-02-13T00:26:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175976.76 epoch sec; seqNo:12623; 4059161.10 north; 592549.07 east 2021-02-13T00:26:17Z- AHRS_M2 msg: 1613175977.14 epoch sec; seqNo:12525 2021-02-13T00:26:17Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.18 psi 2021-02-13T00:26:17Z- handleDepth()-Depth_Keller msg: 1613175977158 epoch sec; seqNo:12624; depth 20.91 2021-02-13T00:26:17Z- handleDvl() - RDI_Pathfinder msg: 1613175977.19 epoch sec; seqNo:6190 2021-02-13T00:26:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.97 , 92.10 , 99.40 , 68.66 2021-02-13T00:26:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , 0.11 2021-02-13T00:26:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175977.20 epoch sec; seqNo:12624; 4059161.49 north; 592550.29 east 2021-02-13T00:26:17Z- AHRS_M2 msg: 1613175977.54 epoch sec; seqNo:12526 2021-02-13T00:26:17Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T00:26:17Z- handleDepth()-Depth_Keller msg: 1613175977549 epoch sec; seqNo:12625; depth 20.78 2021-02-13T00:26:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175977.56 epoch sec; seqNo:12625; 4059161.49 north; 592550.29 east 2021-02-13T00:26:17Z- AHRS_M2 msg: 1613175977.94 epoch sec; seqNo:12527 2021-02-13T00:26:17Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.19 psi 2021-02-13T00:26:17Z- handleDepth()-Depth_Keller msg: 1613175977950 epoch sec; seqNo:12626; depth 20.69 2021-02-13T00:26:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175977.96 epoch sec; seqNo:12626; 4059161.87 north; 592550.29 east 2021-02-13T00:26:18Z- AHRS_M2 msg: 1613175978.35 epoch sec; seqNo:12528 2021-02-13T00:26:18Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:26:18Z- handleDepth()-Depth_Keller msg: 1613175978357 epoch sec; seqNo:12627; depth 20.57 2021-02-13T00:26:18Z- handleDvl() - RDI_Pathfinder msg: 1613175978.38 epoch sec; seqNo:6191 2021-02-13T00:26:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 98.32 , 101.97 , 69.48 2021-02-13T00:26:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , 0.03 2021-02-13T00:26:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175978.39 epoch sec; seqNo:12627; 4059162.25 north; 592550.28 east 2021-02-13T00:26:18Z- AHRS_M2 msg: 1613175978.75 epoch sec; seqNo:12529 2021-02-13T00:26:18Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:26:18Z- handleDepth()-Depth_Keller msg: 1613175978756 epoch sec; seqNo:12628; depth 20.45 2021-02-13T00:26:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175978.77 epoch sec; seqNo:12628; 4059162.63 north; 592550.28 east 2021-02-13T00:26:19Z- AHRS_M2 msg: 1613175979.16 epoch sec; seqNo:12530 2021-02-13T00:26:19Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-13T00:26:19Z- handleDepth()-Depth_Keller msg: 1613175979162 epoch sec; seqNo:12629; depth 20.35 2021-02-13T00:26:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175979.17 epoch sec; seqNo:12629; 4059162.63 north; 592550.28 east 2021-02-13T00:26:19Z- AHRS_M2 msg: 1613175979.56 epoch sec; seqNo:12531 2021-02-13T00:26:19Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-13T00:26:19Z- handleDepth()-Depth_Keller msg: 1613175979566 epoch sec; seqNo:12630; depth 20.22 2021-02-13T00:26:19Z- handleDvl() - RDI_Pathfinder msg: 1613175979.58 epoch sec; seqNo:6192 2021-02-13T00:26:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 97.98 , 104.07 , 78.51 2021-02-13T00:26:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , -0.02 2021-02-13T00:26:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175979.59 epoch sec; seqNo:12630; 4059163.02 north; 592551.50 east 2021-02-13T00:26:19Z- AHRS_M2 msg: 1613175979.96 epoch sec; seqNo:12532 2021-02-13T00:26:19Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:26:19Z- handleDepth()-Depth_Keller msg: 1613175979968 epoch sec; seqNo:12631; depth 20.11 2021-02-13T00:26:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175979.99 epoch sec; seqNo:12631; 4059163.40 north; 592551.49 east 2021-02-13T00:26:20Z- AHRS_M2 msg: 1613175980.37 epoch sec; seqNo:12533 2021-02-13T00:26:20Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-13T00:26:20Z- handleDepth()-Depth_Keller msg: 1613175980381 epoch sec; seqNo:12632; depth 20.00 2021-02-13T00:26:20Z- handleDvl() - RDI_Pathfinder msg: 1613175980.40 epoch sec; seqNo:6193 2021-02-13T00:26:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:26:20Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:26:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175980.42 epoch sec; seqNo:12632; 4059163.78 north; 592551.49 east 2021-02-13T00:26:20Z- LcmTrn::handleMessages() - handling 45 messages took 0.79 ms and 130 calls... 2021-02-13T00:26:20Z- waiting for 1613175978.14, time is 1613175980.42 2021-02-13T00:26:20Z- LcmTrn::updateTrn() equating pose 1613175980.42 and meas times 1613175980.42 2021-02-13T00:26:20Z- LcmTrn::updateTrn() >>>> pose time 1613175980.42, meas time 1613175980.42 <<<< 2021-02-13T00:26:20Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613175980.42, ping # 0. 2021-02-13T00:26:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175980.42 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:26:20Z- TerrainNav::motionUpdate() - currEstimate = (4059164, 592551.49, 20.00) 2021-02-13T00:26:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:26:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:26:20Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:26:20Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:26:20Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175980.42 sec.; No relevant navigation data available 2021-02-13T00:26:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:26:20Z- TNavBF::mmse selected from filter -1 2021-02-13T00:26:20Z- variances([0], [2], and psi_berg) = 702.835879 832.999455 0.000000 2021-02-13T00:26:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:26:20Z- LcmTrn::publishEstimates() - MLE : 4059163.28 592598.99 20.00 2021-02-13T00:26:20Z- LcmTrn::publishEstimates() - MMSE : 4059163.78 592551.49 20.00 2021-02-13T00:26:20Z- LcmTrn::publishEstimates() - COVAR: 702.84 833.00 1.00 0.00 2021-02-13T00:26:20Z- AHRS_M2 msg: 1613175980.78 epoch sec; seqNo:12534 2021-02-13T00:26:20Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.14 psi 2021-02-13T00:26:20Z- handleDepth()-Depth_Keller msg: 1613175980789 epoch sec; seqNo:12633; depth 19.89 2021-02-13T00:26:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175980.80 epoch sec; seqNo:12633; 4059163.78 north; 592551.49 east 2021-02-13T00:26:21Z- AHRS_M2 msg: 1613175981.18 epoch sec; seqNo:12535 2021-02-13T00:26:21Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.13 psi 2021-02-13T00:26:21Z- handleDepth()-Depth_Keller msg: 1613175981183 epoch sec; seqNo:12634; depth 19.79 2021-02-13T00:26:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175981.19 epoch sec; seqNo:12634; 4059164.16 north; 592551.48 east 2021-02-13T00:26:21Z- AHRS_M2 msg: 1613175981.58 epoch sec; seqNo:12536 2021-02-13T00:26:21Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.13 psi 2021-02-13T00:26:21Z- handleDepth()-Depth_Keller msg: 1613175981589 epoch sec; seqNo:12635; depth 19.67 2021-02-13T00:26:21Z- handleDvl() - RDI_Pathfinder msg: 1613175981.61 epoch sec; seqNo:6194 2021-02-13T00:26:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.64 , 97.57 , 102.98 , 69.89 2021-02-13T00:26:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , -0.03 2021-02-13T00:26:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175981.62 epoch sec; seqNo:12635; 4059164.54 north; 592551.48 east 2021-02-13T00:26:21Z- AHRS_M2 msg: 1613175981.98 epoch sec; seqNo:12537 2021-02-13T00:26:21Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.12 psi 2021-02-13T00:26:21Z- handleDepth()-Depth_Keller msg: 1613175981988 epoch sec; seqNo:12636; depth 19.54 2021-02-13T00:26:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175982.00 epoch sec; seqNo:12636; 4059164.93 north; 592552.70 east 2021-02-13T00:26:22Z- AHRS_M2 msg: 1613175982.40 epoch sec; seqNo:12538 2021-02-13T00:26:22Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.12 psi 2021-02-13T00:26:22Z- handleDepth()-Depth_Keller msg: 1613175982405 epoch sec; seqNo:12637; depth 19.43 2021-02-13T00:26:22Z- handleDvl() - RDI_Pathfinder msg: 1613175982.42 epoch sec; seqNo:6195 2021-02-13T00:26:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 0.00 , 104.07 , 69.07 2021-02-13T00:26:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.02 2021-02-13T00:26:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175982.43 epoch sec; seqNo:12637; 4059164.93 north; 592552.70 east 2021-02-13T00:26:22Z- AHRS_M2 msg: 1613175982.79 epoch sec; seqNo:12539 2021-02-13T00:26:22Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.13 psi 2021-02-13T00:26:22Z- handleDepth()-Depth_Keller msg: 1613175982801 epoch sec; seqNo:12638; depth 19.31 2021-02-13T00:26:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175982.82 epoch sec; seqNo:12638; 4059165.31 north; 592552.69 east 2021-02-13T00:26:23Z- AHRS_M2 msg: 1613175983.20 epoch sec; seqNo:12540 2021-02-13T00:26:23Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.13 psi 2021-02-13T00:26:23Z- handleDepth()-Depth_Keller msg: 1613175983200 epoch sec; seqNo:12639; depth 19.19 2021-02-13T00:26:23Z- handleDvl() - RDI_Pathfinder msg: 1613175983.21 epoch sec; seqNo:6196 2021-02-13T00:26:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.06 , 0.00 , 104.40 , 70.29 2021-02-13T00:26:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , -0.01 2021-02-13T00:26:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175983.24 epoch sec; seqNo:12639; 4059165.69 north; 592552.69 east 2021-02-13T00:26:23Z- AHRS_M2 msg: 1613175983.60 epoch sec; seqNo:12541 2021-02-13T00:26:23Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.14 psi 2021-02-13T00:26:23Z- handleDepth()-Depth_Keller msg: 1613175983607 epoch sec; seqNo:12640; depth 19.07 2021-02-13T00:26:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175983.62 epoch sec; seqNo:12640; 4059165.69 north; 592552.69 east 2021-02-13T00:26:24Z- AHRS_M2 msg: 1613175984.00 epoch sec; seqNo:12542 2021-02-13T00:26:24Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.14 psi 2021-02-13T00:26:24Z- handleDepth()-Depth_Keller msg: 1613175984009 epoch sec; seqNo:12641; depth 18.95 2021-02-13T00:26:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175984.02 epoch sec; seqNo:12641; 4059166.07 north; 592552.68 east 2021-02-13T00:26:24Z- AHRS_M2 msg: 1613175984.41 epoch sec; seqNo:12543 2021-02-13T00:26:24Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.15 psi 2021-02-13T00:26:24Z- handleDepth()-Depth_Keller msg: 1613175984412 epoch sec; seqNo:12642; depth 18.83 2021-02-13T00:26:24Z- handleDvl() - RDI_Pathfinder msg: 1613175984.42 epoch sec; seqNo:6197 2021-02-13T00:26:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.06 , 98.25 , 105.55 , 78.78 2021-02-13T00:26:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.01 2021-02-13T00:26:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175984.45 epoch sec; seqNo:12642; 4059166.46 north; 592553.90 east 2021-02-13T00:26:24Z- AHRS_M2 msg: 1613175984.81 epoch sec; seqNo:12544 2021-02-13T00:26:24Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.15 psi 2021-02-13T00:26:24Z- handleDepth()-Depth_Keller msg: 1613175984816 epoch sec; seqNo:12643; depth 18.69 2021-02-13T00:26:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175984.83 epoch sec; seqNo:12643; 4059166.84 north; 592553.90 east 2021-02-13T00:26:25Z- AHRS_M2 msg: 1613175985.21 epoch sec; seqNo:12545 2021-02-13T00:26:25Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-13T00:26:25Z- handleDepth()-Depth_Keller msg: 1613175985223 epoch sec; seqNo:12644; depth 18.57 2021-02-13T00:26:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175985.23 epoch sec; seqNo:12644; 4059166.84 north; 592553.90 east 2021-02-13T00:26:25Z- LcmTrn::handleMessages() - handling 40 messages took 0.60 ms and 128 calls... 2021-02-13T00:26:25Z- waiting for 1613175983.42, time is 1613175985.23 2021-02-13T00:26:25Z- LcmTrn::updateTrn() >>>> pose time 1613175985.23, meas time 1613175984.42 <<<< 2021-02-13T00:26:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175984.42 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:26:25Z- TerrainNav::motionUpdate() - currEstimate = (4059167, 592553.90, 18.57) 2021-02-13T00:26:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:26:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:26:25Z- TNavBF::Projecting Measurements 2021-02-13T00:26:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:26:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:26:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:26:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:26:25Z- TNavFilter::4 good beams. 2021-02-13T00:26:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:26:25Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:26:25Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:26:25Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:26:25Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:26:25Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:26:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:26:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:26:25Z- TerrainMapOctree: (vehN, vehE ) = (4059166.32, 592553.49). 2021-02-13T00:26:25Z- TerrainMapOctree: Min Distance = 2686.98. 2021-02-13T00:26:25Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:26:25Z- There is no specific good beam that all particles have in common. 2021-02-13T00:26:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:26:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 9.75 2021-02-13T00:26:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:26:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 15.45 2021-02-13T00:26:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:26:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 17.83 2021-02-13T00:26:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:26:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 9.93 2021-02-13T00:26:25Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:26:25Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:26:25Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:26:25Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:26:25Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:26:25Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:26:25Z- Filter 0 No beams used in subcloud update 2021-02-13T00:26:25Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:26:25Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:26:25Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 702.8 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:26:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175984.42 sec. 2021-02-13T00:26:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:26:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:26:25Z- variances([0], [2], and psi_berg) = 702.763144 832.999415 0.000000 2021-02-13T00:26:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:26:25Z- LcmTrn::publishEstimates() - MLE : 4059166.34 592601.40 18.57 2021-02-13T00:26:25Z- LcmTrn::publishEstimates() - MMSE : 4059166.84 592553.90 18.57 2021-02-13T00:26:25Z- LcmTrn::publishEstimates() - COVAR: 702.76 833.00 0.91 0.00 2021-02-13T00:26:25Z- AHRS_M2 msg: 1613175985.62 epoch sec; seqNo:12546 2021-02-13T00:26:25Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-13T00:26:25Z- handleDepth()-Depth_Keller msg: 1613175985625 epoch sec; seqNo:12645; depth 18.46 2021-02-13T00:26:25Z- handleDvl() - RDI_Pathfinder msg: 1613175985.64 epoch sec; seqNo:6198 2021-02-13T00:26:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.52 , 92.91 , 105.08 , 69.48 2021-02-13T00:26:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.01 2021-02-13T00:26:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175985.65 epoch sec; seqNo:12645; 4059167.22 north; 592553.89 east 2021-02-13T00:26:26Z- AHRS_M2 msg: 1613175986.02 epoch sec; seqNo:12547 2021-02-13T00:26:26Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T00:26:26Z- handleDepth()-Depth_Keller msg: 1613175986028 epoch sec; seqNo:12646; depth 18.35 2021-02-13T00:26:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175986.04 epoch sec; seqNo:12646; 4059167.59 north; 592553.89 east 2021-02-13T00:26:26Z- AHRS_M2 msg: 1613175986.43 epoch sec; seqNo:12548 2021-02-13T00:26:26Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T00:26:26Z- handleDepth()-Depth_Keller msg: 1613175986441 epoch sec; seqNo:12647; depth 18.24 2021-02-13T00:26:26Z- handleDvl() - RDI_Pathfinder msg: 1613175986.45 epoch sec; seqNo:6199 2021-02-13T00:26:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.79 , 94.40 , 106.57 , 78.58 2021-02-13T00:26:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , 0.00 2021-02-13T00:26:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175986.46 epoch sec; seqNo:12647; 4059167.59 north; 592553.89 east 2021-02-13T00:26:26Z- AHRS_M2 msg: 1613175986.83 epoch sec; seqNo:12549 2021-02-13T00:26:26Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T00:26:26Z- handleDepth()-Depth_Keller msg: 1613175986848 epoch sec; seqNo:12648; depth 18.10 2021-02-13T00:26:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175986.86 epoch sec; seqNo:12648; 4059167.97 north; 592553.88 east 2021-02-13T00:26:27Z- AHRS_M2 msg: 1613175987.25 epoch sec; seqNo:12550 2021-02-13T00:26:27Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.17 psi 2021-02-13T00:26:27Z- handleDepth()-Depth_Keller msg: 1613175987251 epoch sec; seqNo:12649; depth 18.00 2021-02-13T00:26:27Z- handleDvl() - RDI_Pathfinder msg: 1613175987.27 epoch sec; seqNo:6200 2021-02-13T00:26:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.88 , 95.34 , 108.73 , 70.29 2021-02-13T00:26:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.01 2021-02-13T00:26:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175987.28 epoch sec; seqNo:12649; 4059168.36 north; 592555.10 east 2021-02-13T00:26:27Z- AHRS_M2 msg: 1613175987.64 epoch sec; seqNo:12551 2021-02-13T00:26:27Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.17 psi 2021-02-13T00:26:27Z- handleDepth()-Depth_Keller msg: 1613175987649 epoch sec; seqNo:12650; depth 17.88 2021-02-13T00:26:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175987.66 epoch sec; seqNo:12650; 4059168.74 north; 592555.10 east 2021-02-13T00:26:28Z- AHRS_M2 msg: 1613175988.04 epoch sec; seqNo:12552 2021-02-13T00:26:28Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-13T00:26:28Z- handleDepth()-Depth_Keller msg: 1613175988050 epoch sec; seqNo:12651; depth 17.76 2021-02-13T00:26:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175988.06 epoch sec; seqNo:12651; 4059168.74 north; 592555.10 east 2021-02-13T00:26:28Z- AHRS_M2 msg: 1613175988.45 epoch sec; seqNo:12553 2021-02-13T00:26:28Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.19 psi 2021-02-13T00:26:28Z- handleDepth()-Depth_Keller msg: 1613175988457 epoch sec; seqNo:12652; depth 17.65 2021-02-13T00:26:28Z- handleDvl() - RDI_Pathfinder msg: 1613175988.48 epoch sec; seqNo:6201 2021-02-13T00:26:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.06 , 97.91 , 108.86 , 72.34 2021-02-13T00:26:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , 0.03 2021-02-13T00:26:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175988.49 epoch sec; seqNo:12652; 4059169.12 north; 592555.09 east 2021-02-13T00:26:28Z- AHRS_M2 msg: 1613175988.85 epoch sec; seqNo:12554 2021-02-13T00:26:28Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.20 psi 2021-02-13T00:26:28Z- handleDepth()-Depth_Keller msg: 1613175988856 epoch sec; seqNo:12653; depth 17.51 2021-02-13T00:26:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175988.87 epoch sec; seqNo:12653; 4059169.50 north; 592555.09 east 2021-02-13T00:26:29Z- AHRS_M2 msg: 1613175989.26 epoch sec; seqNo:12555 2021-02-13T00:26:29Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.22 psi 2021-02-13T00:26:29Z- handleDepth()-Depth_Keller msg: 1613175989261 epoch sec; seqNo:12654; depth 17.40 2021-02-13T00:26:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175989.27 epoch sec; seqNo:12654; 4059169.50 north; 592555.09 east 2021-02-13T00:26:29Z- AHRS_M2 msg: 1613175989.66 epoch sec; seqNo:12556 2021-02-13T00:26:29Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.23 psi 2021-02-13T00:26:29Z- handleDepth()-Depth_Keller msg: 1613175989673 epoch sec; seqNo:12655; depth 17.26 2021-02-13T00:26:29Z- handleDvl() - RDI_Pathfinder msg: 1613175989.68 epoch sec; seqNo:6202 2021-02-13T00:26:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:26:29Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:26:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175989.69 epoch sec; seqNo:12655; 4059169.88 north; 592555.08 east 2021-02-13T00:26:30Z- AHRS_M2 msg: 1613175990.06 epoch sec; seqNo:12557 2021-02-13T00:26:30Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.24 psi 2021-02-13T00:26:30Z- handleDepth()-Depth_Keller msg: 1613175990068 epoch sec; seqNo:12656; depth 17.12 2021-02-13T00:26:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175990.09 epoch sec; seqNo:12656; 4059170.27 north; 592556.30 east 2021-02-13T00:26:30Z- AHRS_M2 msg: 1613175990.47 epoch sec; seqNo:12558 2021-02-13T00:26:30Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.24 psi 2021-02-13T00:26:30Z- handleDepth()-Depth_Keller msg: 1613175990472 epoch sec; seqNo:12657; depth 17.01 2021-02-13T00:26:30Z- handleDvl() - RDI_Pathfinder msg: 1613175990.48 epoch sec; seqNo:6203 2021-02-13T00:26:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.02 , 111.84 , 78.04 2021-02-13T00:26:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , 0.11 2021-02-13T00:26:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175990.51 epoch sec; seqNo:12657; 4059170.65 north; 592556.30 east 2021-02-13T00:26:30Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-13T00:26:30Z- waiting for 1613175988.23, time is 1613175990.51 2021-02-13T00:26:30Z- LcmTrn::updateTrn() equating pose 1613175990.51 and meas times 1613175990.51 2021-02-13T00:26:30Z- LcmTrn::updateTrn() >>>> pose time 1613175990.51, meas time 1613175990.51 <<<< 2021-02-13T00:26:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175990.51 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:26:30Z- TerrainNav::motionUpdate() - currEstimate = (4059171, 592556.30, 17.01) 2021-02-13T00:26:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:26:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:26:30Z- TerrainNav::Measurement type 1 not incorporated from time = 1613175990.51 sec.; No relevant navigation data available 2021-02-13T00:26:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:26:30Z- TNavBF::mmse selected from filter -1 2021-02-13T00:26:30Z- variances([0], [2], and psi_berg) = 702.703960 832.999383 0.000000 2021-02-13T00:26:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:26:30Z- LcmTrn::publishEstimates() - MLE : 4059170.15 592525.80 17.01 2021-02-13T00:26:30Z- LcmTrn::publishEstimates() - MMSE : 4059170.65 592556.30 17.01 2021-02-13T00:26:30Z- LcmTrn::publishEstimates() - COVAR: 702.70 833.00 1.14 0.00 2021-02-13T00:26:30Z- AHRS_M2 msg: 1613175990.87 epoch sec; seqNo:12559 2021-02-13T00:26:30Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.24 psi 2021-02-13T00:26:30Z- handleDepth()-Depth_Keller msg: 1613175990876 epoch sec; seqNo:12658; depth 16.87 2021-02-13T00:26:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175990.89 epoch sec; seqNo:12658; 4059170.65 north; 592556.30 east 2021-02-13T00:26:31Z- AHRS_M2 msg: 1613175991.28 epoch sec; seqNo:12560 2021-02-13T00:26:31Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.23 psi 2021-02-13T00:26:31Z- handleDepth()-Depth_Keller msg: 1613175991282 epoch sec; seqNo:12659; depth 16.75 2021-02-13T00:26:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175991.30 epoch sec; seqNo:12659; 4059171.03 north; 592556.29 east 2021-02-13T00:26:31Z- AHRS_M2 msg: 1613175991.70 epoch sec; seqNo:12561 2021-02-13T00:26:31Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.22 psi 2021-02-13T00:26:31Z- handleDepth()-Depth_Keller msg: 1613175991704 epoch sec; seqNo:12660; depth 16.64 2021-02-13T00:26:31Z- handleDvl() - RDI_Pathfinder msg: 1613175991.72 epoch sec; seqNo:6204 2021-02-13T00:26:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.73 , 101.36 , 110.83 , 73.97 2021-02-13T00:26:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.02 , 0.09 2021-02-13T00:26:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175991.73 epoch sec; seqNo:12660; 4059171.41 north; 592556.29 east 2021-02-13T00:26:32Z- AHRS_M2 msg: 1613175992.08 epoch sec; seqNo:12562 2021-02-13T00:26:32Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.20 psi 2021-02-13T00:26:32Z- handleDepth()-Depth_Keller msg: 1613175992088 epoch sec; seqNo:12661; depth 16.51 2021-02-13T00:26:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175992.10 epoch sec; seqNo:12661; 4059171.79 north; 592556.28 east 2021-02-13T00:26:32Z- AHRS_M2 msg: 1613175992.50 epoch sec; seqNo:12563 2021-02-13T00:26:32Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.19 psi 2021-02-13T00:26:32Z- handleDepth()-Depth_Keller msg: 1613175992503 epoch sec; seqNo:12662; depth 16.39 2021-02-13T00:26:32Z- handleDvl() - RDI_Pathfinder msg: 1613175992.52 epoch sec; seqNo:6205 2021-02-13T00:26:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.86 , 109.81 , 78.17 2021-02-13T00:26:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , 0.01 2021-02-13T00:26:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175992.54 epoch sec; seqNo:12662; 4059172.18 north; 592557.50 east 2021-02-13T00:26:32Z- AHRS_M2 msg: 1613175992.89 epoch sec; seqNo:12564 2021-02-13T00:26:32Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.18 psi 2021-02-13T00:26:32Z- handleDepth()-Depth_Keller msg: 1613175992899 epoch sec; seqNo:12663; depth 16.26 2021-02-13T00:26:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175992.91 epoch sec; seqNo:12663; 4059172.18 north; 592557.50 east 2021-02-13T00:26:33Z- AHRS_M2 msg: 1613175993.30 epoch sec; seqNo:12565 2021-02-13T00:26:33Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-13T00:26:33Z- handleDepth()-Depth_Keller msg: 1613175993300 epoch sec; seqNo:12664; depth 16.17 2021-02-13T00:26:33Z- handleDvl() - RDI_Pathfinder msg: 1613175993.31 epoch sec; seqNo:6206 2021-02-13T00:26:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.79 , 112.31 , 73.97 2021-02-13T00:26:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , -0.06 2021-02-13T00:26:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175993.32 epoch sec; seqNo:12664; 4059172.56 north; 592557.50 east 2021-02-13T00:26:33Z- AHRS_M2 msg: 1613175993.70 epoch sec; seqNo:12566 2021-02-13T00:26:33Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-13T00:26:33Z- handleDepth()-Depth_Keller msg: 1613175993713 epoch sec; seqNo:12665; depth 16.04 2021-02-13T00:26:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175993.72 epoch sec; seqNo:12665; 4059172.94 north; 592557.49 east 2021-02-13T00:26:34Z- AHRS_M2 msg: 1613175994.10 epoch sec; seqNo:12567 2021-02-13T00:26:34Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-13T00:26:34Z- handleDepth()-Depth_Keller msg: 1613175994108 epoch sec; seqNo:12666; depth 15.92 2021-02-13T00:26:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175994.13 epoch sec; seqNo:12666; 4059173.32 north; 592557.49 east 2021-02-13T00:26:34Z- AHRS_M2 msg: 1613175994.51 epoch sec; seqNo:12568 2021-02-13T00:26:34Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.14 psi 2021-02-13T00:26:34Z- handleDepth()-Depth_Keller msg: 1613175994517 epoch sec; seqNo:12667; depth 15.81 2021-02-13T00:26:34Z- handleDvl() - RDI_Pathfinder msg: 1613175994.54 epoch sec; seqNo:6207 2021-02-13T00:26:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.57 , 98.79 , 109.61 , 72.34 2021-02-13T00:26:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.04 , -0.09 2021-02-13T00:26:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175994.55 epoch sec; seqNo:12667; 4059173.32 north; 592557.49 east 2021-02-13T00:26:34Z- AHRS_M2 msg: 1613175994.91 epoch sec; seqNo:12569 2021-02-13T00:26:34Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-13T00:26:34Z- handleDepth()-Depth_Keller msg: 1613175994916 epoch sec; seqNo:12668; depth 15.68 2021-02-13T00:26:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175994.93 epoch sec; seqNo:12668; 4059173.69 north; 592557.48 east 2021-02-13T00:26:35Z- AHRS_M2 msg: 1613175995.32 epoch sec; seqNo:12570 2021-02-13T00:26:35Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-13T00:26:35Z- handleDepth()-Depth_Keller msg: 1613175995328 epoch sec; seqNo:12669; depth 15.57 2021-02-13T00:26:35Z- handleDvl() - RDI_Pathfinder msg: 1613175995.34 epoch sec; seqNo:6208 2021-02-13T00:26:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 97.71 , 109.88 , 73.56 2021-02-13T00:26:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.11 , -0.04 2021-02-13T00:26:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175995.35 epoch sec; seqNo:12669; 4059174.09 north; 592558.70 east 2021-02-13T00:26:35Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-13T00:26:35Z- waiting for 1613175993.51, time is 1613175995.35 2021-02-13T00:26:35Z- LcmTrn::updateTrn() equating pose 1613175995.35 and meas times 1613175995.35 2021-02-13T00:26:35Z- LcmTrn::updateTrn() >>>> pose time 1613175995.35, meas time 1613175995.35 <<<< 2021-02-13T00:26:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613175995.35 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:26:35Z- TerrainNav::motionUpdate() - currEstimate = (4059174, 592558.70, 15.57) 2021-02-13T00:26:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:26:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:26:35Z- TNavBF::Projecting Measurements 2021-02-13T00:26:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:26:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:26:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:26:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:26:35Z- TNavFilter::4 good beams. 2021-02-13T00:26:35Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:26:35Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:26:35Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:26:35Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:26:35Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:26:35Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:26:35Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:26:35Z- TNavBF::mmse selected from filter -1 2021-02-13T00:26:35Z- TerrainMapOctree: (vehN, vehE ) = (4059174.09, 592558.70). 2021-02-13T00:26:35Z- TerrainMapOctree: Min Distance = 2677.67. 2021-02-13T00:26:35Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:26:35Z- There is no specific good beam that all particles have in common. 2021-02-13T00:26:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:26:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.35 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 11.05 2021-02-13T00:26:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:26:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:26:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:26:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 19.32 2021-02-13T00:26:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:26:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 8.66 2021-02-13T00:26:35Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:26:35Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:26:35Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:26:35Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:26:35Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:26:35Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:26:35Z- Filter 0 No beams used in subcloud update 2021-02-13T00:26:35Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:26:35Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:26:35Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 702.6 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:26:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613175995.35 sec. 2021-02-13T00:26:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:26:35Z- TNavBF::mmse selected from filter -1 2021-02-13T00:26:35Z- variances([0], [2], and psi_berg) = 702.646907 832.999352 0.000000 2021-02-13T00:26:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:26:35Z- LcmTrn::publishEstimates() - MLE : 4059173.59 592528.20 15.57 2021-02-13T00:26:35Z- LcmTrn::publishEstimates() - MMSE : 4059174.09 592558.70 15.57 2021-02-13T00:26:35Z- LcmTrn::publishEstimates() - COVAR: 702.65 833.00 0.98 0.00 2021-02-13T00:26:35Z- AHRS_M2 msg: 1613175995.72 epoch sec; seqNo:12571 2021-02-13T00:26:35Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-13T00:26:35Z- handleDepth()-Depth_Keller msg: 1613175995724 epoch sec; seqNo:12670; depth 15.45 2021-02-13T00:26:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175995.74 epoch sec; seqNo:12670; 4059174.09 north; 592558.70 east 2021-02-13T00:26:36Z- AHRS_M2 msg: 1613175996.13 epoch sec; seqNo:12572 2021-02-13T00:26:36Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-13T00:26:36Z- handleDepth()-Depth_Keller msg: 1613175996134 epoch sec; seqNo:12671; depth 15.34 2021-02-13T00:26:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175996.16 epoch sec; seqNo:12671; 4059174.47 north; 592558.70 east 2021-02-13T00:26:36Z- AHRS_M2 msg: 1613175996.53 epoch sec; seqNo:12573 2021-02-13T00:26:36Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-13T00:26:36Z- handleDepth()-Depth_Keller msg: 1613175996532 epoch sec; seqNo:12672; depth 15.20 2021-02-13T00:26:36Z- handleDvl() - RDI_Pathfinder msg: 1613175996.54 epoch sec; seqNo:6209 2021-02-13T00:26:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 96.43 , 111.03 , 71.93 2021-02-13T00:26:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.15 , 0.06 2021-02-13T00:26:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175996.55 epoch sec; seqNo:12672; 4059174.84 north; 592558.69 east 2021-02-13T00:26:36Z- AHRS_M2 msg: 1613175996.93 epoch sec; seqNo:12574 2021-02-13T00:26:36Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.16 psi 2021-02-13T00:26:36Z- handleDepth()-Depth_Keller msg: 1613175996945 epoch sec; seqNo:12673; depth 15.08 2021-02-13T00:26:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175996.96 epoch sec; seqNo:12673; 4059174.84 north; 592558.69 east 2021-02-13T00:26:37Z- AHRS_M2 msg: 1613175997.34 epoch sec; seqNo:12575 2021-02-13T00:26:37Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-13T00:26:37Z- handleDepth()-Depth_Keller msg: 1613175997347 epoch sec; seqNo:12674; depth 14.95 2021-02-13T00:26:37Z- handleDvl() - RDI_Pathfinder msg: 1613175997.37 epoch sec; seqNo:6210 2021-02-13T00:26:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 98.93 , 112.31 , 74.38 2021-02-13T00:26:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.10 , 0.13 2021-02-13T00:26:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175997.38 epoch sec; seqNo:12674; 4059175.22 north; 592558.69 east 2021-02-13T00:26:37Z- AHRS_M2 msg: 1613175997.76 epoch sec; seqNo:12576 2021-02-13T00:26:37Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-13T00:26:37Z- handleDepth()-Depth_Keller msg: 1613175997789 epoch sec; seqNo:12675; depth 14.83 2021-02-13T00:26:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175997.83 epoch sec; seqNo:12675; 4059175.60 north; 592558.68 east 2021-02-13T00:26:38Z- AHRS_M2 msg: 1613175998.15 epoch sec; seqNo:12577 2021-02-13T00:26:38Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-13T00:26:38Z- handleDepth()-Depth_Keller msg: 1613175998156 epoch sec; seqNo:12676; depth 14.68 2021-02-13T00:26:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175998.16 epoch sec; seqNo:12676; 4059175.98 north; 592558.68 east 2021-02-13T00:26:38Z- AHRS_M2 msg: 1613175998.55 epoch sec; seqNo:12578 2021-02-13T00:26:38Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-13T00:26:38Z- handleDepth()-Depth_Keller msg: 1613175998559 epoch sec; seqNo:12677; depth 14.60 2021-02-13T00:26:38Z- handleDvl() - RDI_Pathfinder msg: 1613175998.57 epoch sec; seqNo:6211 2021-02-13T00:26:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 97.71 , 111.23 , 73.97 2021-02-13T00:26:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , 0.14 2021-02-13T00:26:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175998.58 epoch sec; seqNo:12677; 4059176.37 north; 592559.90 east 2021-02-13T00:26:38Z- AHRS_M2 msg: 1613175998.95 epoch sec; seqNo:12579 2021-02-13T00:26:38Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.18 psi 2021-02-13T00:26:38Z- handleDepth()-Depth_Keller msg: 1613175998956 epoch sec; seqNo:12678; depth 14.47 2021-02-13T00:26:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175998.97 epoch sec; seqNo:12678; 4059176.37 north; 592559.90 east 2021-02-13T00:26:39Z- AHRS_M2 msg: 1613175999.36 epoch sec; seqNo:12580 2021-02-13T00:26:39Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-13T00:26:39Z- handleDepth()-Depth_Keller msg: 1613175999360 epoch sec; seqNo:12679; depth 14.38 2021-02-13T00:26:39Z- handleDvl() - RDI_Pathfinder msg: 1613175999.37 epoch sec; seqNo:6212 2021-02-13T00:26:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.26 , 100.08 , 113.46 , 75.20 2021-02-13T00:26:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.04 , 0.03 2021-02-13T00:26:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175999.39 epoch sec; seqNo:12679; 4059176.75 north; 592559.89 east 2021-02-13T00:26:39Z- AHRS_M2 msg: 1613175999.76 epoch sec; seqNo:12581 2021-02-13T00:26:39Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-13T00:26:39Z- handleDepth()-Depth_Keller msg: 1613175999769 epoch sec; seqNo:12680; depth 14.26 2021-02-13T00:26:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613175999.78 epoch sec; seqNo:12680; 4059177.13 north; 592559.89 east 2021-02-13T00:26:40Z- AHRS_M2 msg: 1613176000.16 epoch sec; seqNo:12582 2021-02-13T00:26:40Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-13T00:26:40Z- handleDepth()-Depth_Keller msg: 1613176000170 epoch sec; seqNo:12681; depth 14.14 2021-02-13T00:26:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176000.18 epoch sec; seqNo:12681; 4059177.51 north; 592559.88 east 2021-02-13T00:26:40Z- AHRS_M2 msg: 1613176000.57 epoch sec; seqNo:12583 2021-02-13T00:26:40Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-13T00:26:40Z- handleDepth()-Depth_Keller msg: 1613176000573 epoch sec; seqNo:12682; depth 14.02 2021-02-13T00:26:40Z- handleDvl() - RDI_Pathfinder msg: 1613176000.59 epoch sec; seqNo:6213 2021-02-13T00:26:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 100.35 , 112.52 , 78.71 2021-02-13T00:26:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , -0.10 2021-02-13T00:26:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176000.60 epoch sec; seqNo:12682; 4059177.89 north; 592559.88 east 2021-02-13T00:26:40Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-13T00:26:40Z- waiting for 1613175998.35, time is 1613176000.60 2021-02-13T00:26:40Z- LcmTrn::updateTrn() equating pose 1613176000.60 and meas times 1613176000.60 2021-02-13T00:26:40Z- LcmTrn::updateTrn() >>>> pose time 1613176000.60, meas time 1613176000.60 <<<< 2021-02-13T00:26:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176000.60 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:26:40Z- TerrainNav::motionUpdate() - currEstimate = (4059178, 592559.88, 14.02) 2021-02-13T00:26:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:26:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:26:40Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176000.60 sec.; No relevant navigation data available 2021-02-13T00:26:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:26:40Z- TNavBF::mmse selected from filter -1 2021-02-13T00:26:40Z- variances([0], [2], and psi_berg) = 702.591359 832.999322 0.000000 2021-02-13T00:26:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:26:40Z- LcmTrn::publishEstimates() - MLE : 4059177.39 592529.38 14.02 2021-02-13T00:26:40Z- LcmTrn::publishEstimates() - MMSE : 4059177.89 592559.88 14.02 2021-02-13T00:26:40Z- LcmTrn::publishEstimates() - COVAR: 702.59 833.00 1.18 0.00 2021-02-13T00:26:40Z- AHRS_M2 msg: 1613176000.97 epoch sec; seqNo:12584 2021-02-13T00:26:40Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.16 psi 2021-02-13T00:26:40Z- handleDepth()-Depth_Keller msg: 1613176000981 epoch sec; seqNo:12683; depth 13.90 2021-02-13T00:26:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176000.99 epoch sec; seqNo:12683; 4059177.90 north; 592561.10 east 2021-02-13T00:26:41Z- AHRS_M2 msg: 1613176001.38 epoch sec; seqNo:12585 2021-02-13T00:26:41Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.16 psi 2021-02-13T00:26:41Z- handleDepth()-Depth_Keller msg: 1613176001382 epoch sec; seqNo:12684; depth 13.80 2021-02-13T00:26:41Z- handleDvl() - RDI_Pathfinder msg: 1613176001.39 epoch sec; seqNo:6214 2021-02-13T00:26:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.28 , 109.74 , 72.75 2021-02-13T00:26:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.10 , -0.18 2021-02-13T00:26:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176001.40 epoch sec; seqNo:12684; 4059178.28 north; 592561.10 east 2021-02-13T00:26:41Z- handleDepth()-Depth_Keller msg: 1613176001801 epoch sec; seqNo:12685; depth 13.68 2021-02-13T00:26:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176001.81 epoch sec; seqNo:12685; 4059178.66 north; 592561.09 east 2021-02-13T00:26:42Z- AHRS_M2 msg: 1613176002.18 epoch sec; seqNo:12586 2021-02-13T00:26:42Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.16 psi 2021-02-13T00:26:42Z- handleDepth()-Depth_Keller msg: 1613176002189 epoch sec; seqNo:12686; depth 13.58 2021-02-13T00:26:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176002.20 epoch sec; seqNo:12686; 4059178.66 north; 592561.09 east 2021-02-13T00:26:42Z- AHRS_M2 msg: 1613176002.59 epoch sec; seqNo:12587 2021-02-13T00:26:42Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.17 psi 2021-02-13T00:26:42Z- handleDepth()-Depth_Keller msg: 1613176002597 epoch sec; seqNo:12687; depth 13.46 2021-02-13T00:26:42Z- handleDvl() - RDI_Pathfinder msg: 1613176002.61 epoch sec; seqNo:6215 2021-02-13T00:26:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 100.21 , 112.38 , 74.79 2021-02-13T00:26:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.21 , -0.09 2021-02-13T00:26:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176002.62 epoch sec; seqNo:12687; 4059179.04 north; 592561.09 east 2021-02-13T00:26:42Z- AHRS_M2 msg: 1613176002.99 epoch sec; seqNo:12588 2021-02-13T00:26:42Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.17 psi 2021-02-13T00:26:42Z- handleDepth()-Depth_Keller msg: 1613176002997 epoch sec; seqNo:12688; depth 13.35 2021-02-13T00:26:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176003.01 epoch sec; seqNo:12688; 4059179.42 north; 592561.09 east 2021-02-13T00:26:43Z- AHRS_M2 msg: 1613176003.39 epoch sec; seqNo:12589 2021-02-13T00:26:43Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-13T00:26:43Z- handleDepth()-Depth_Keller msg: 1613176003405 epoch sec; seqNo:12689; depth 13.23 2021-02-13T00:26:43Z- handleDvl() - RDI_Pathfinder msg: 1613176003.42 epoch sec; seqNo:6216 2021-02-13T00:26:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.70 , 112.52 , 78.71 2021-02-13T00:26:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.19 , 0.12 2021-02-13T00:26:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176003.43 epoch sec; seqNo:12689; 4059179.80 north; 592561.08 east 2021-02-13T00:26:43Z- AHRS_M2 msg: 1613176003.80 epoch sec; seqNo:12590 2021-02-13T00:26:43Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-13T00:26:43Z- handleDepth()-Depth_Keller msg: 1613176003805 epoch sec; seqNo:12690; depth 13.11 2021-02-13T00:26:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176003.82 epoch sec; seqNo:12690; 4059180.17 north; 592561.08 east 2021-02-13T00:26:44Z- AHRS_M2 msg: 1613176004.20 epoch sec; seqNo:12591 2021-02-13T00:26:44Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.15 psi 2021-02-13T00:26:44Z- handleDepth()-Depth_Keller msg: 1613176004208 epoch sec; seqNo:12691; depth 13.00 2021-02-13T00:26:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176004.22 epoch sec; seqNo:12691; 4059180.17 north; 592561.08 east 2021-02-13T00:26:44Z- AHRS_M2 msg: 1613176004.61 epoch sec; seqNo:12592 2021-02-13T00:26:44Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-13T00:26:44Z- handleDepth()-Depth_Keller msg: 1613176004612 epoch sec; seqNo:12692; depth 12.88 2021-02-13T00:26:44Z- handleDvl() - RDI_Pathfinder msg: 1613176004.62 epoch sec; seqNo:6217 2021-02-13T00:26:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.71 , 101.29 , 114.82 , 75.20 2021-02-13T00:26:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , 0.25 2021-02-13T00:26:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176004.65 epoch sec; seqNo:12692; 4059180.57 north; 592562.29 east 2021-02-13T00:26:45Z- AHRS_M2 msg: 1613176005.01 epoch sec; seqNo:12593 2021-02-13T00:26:45Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-13T00:26:45Z- handleDepth()-Depth_Keller msg: 1613176005021 epoch sec; seqNo:12693; depth 12.76 2021-02-13T00:26:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176005.03 epoch sec; seqNo:12693; 4059180.95 north; 592562.29 east 2021-02-13T00:26:45Z- AHRS_M2 msg: 1613176005.42 epoch sec; seqNo:12594 2021-02-13T00:26:45Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-13T00:26:45Z- handleDepth()-Depth_Keller msg: 1613176005421 epoch sec; seqNo:12694; depth 12.63 2021-02-13T00:26:45Z- handleDvl() - RDI_Pathfinder msg: 1613176005.43 epoch sec; seqNo:6218 2021-02-13T00:26:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.28 , 109.74 , 76.83 2021-02-13T00:26:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.03 , 0.18 2021-02-13T00:26:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176005.44 epoch sec; seqNo:12694; 4059181.32 north; 592562.29 east 2021-02-13T00:26:45Z- LcmTrn::handleMessages() - handling 40 messages took 0.62 ms and 128 calls... 2021-02-13T00:26:45Z- waiting for 1613176003.60, time is 1613176005.44 2021-02-13T00:26:45Z- LcmTrn::updateTrn() equating pose 1613176005.44 and meas times 1613176005.44 2021-02-13T00:26:45Z- LcmTrn::updateTrn() >>>> pose time 1613176005.44, meas time 1613176005.44 <<<< 2021-02-13T00:26:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176005.44 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:26:45Z- TerrainNav::motionUpdate() - currEstimate = (4059181, 592562.29, 12.63) 2021-02-13T00:26:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:26:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:26:45Z- TNavBF::Projecting Measurements 2021-02-13T00:26:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:26:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:26:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:26:45Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:26:45Z- TNavFilter::3 good beams. 2021-02-13T00:26:45Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:26:45Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:26:45Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:26:45Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:26:45Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:26:45Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:26:45Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:26:45Z- TNavBF::mmse selected from filter -1 2021-02-13T00:26:45Z- TerrainMapOctree: (vehN, vehE ) = (4059181.32, 592562.29). 2021-02-13T00:26:45Z- TerrainMapOctree: Min Distance = 2669.79. 2021-02-13T00:26:45Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:26:45Z- There is no specific good beam that all particles have in common. 2021-02-13T00:26:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:26:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 16.09 2021-02-13T00:26:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:26:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 19.27 2021-02-13T00:26:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:26:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 9.45 2021-02-13T00:26:45Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:26:45Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:26:45Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:26:45Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:26:45Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:26:45Z- Filter 0 No beams used in subcloud update 2021-02-13T00:26:45Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:26:45Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:26:45Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 702.5 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:26:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176005.44 sec. 2021-02-13T00:26:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:26:45Z- TNavBF::mmse selected from filter -1 2021-02-13T00:26:45Z- variances([0], [2], and psi_berg) = 702.534376 832.999292 0.000000 2021-02-13T00:26:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:26:45Z- LcmTrn::publishEstimates() - MLE : 4059180.82 592531.79 12.63 2021-02-13T00:26:45Z- LcmTrn::publishEstimates() - MMSE : 4059181.32 592562.29 12.63 2021-02-13T00:26:45Z- LcmTrn::publishEstimates() - COVAR: 702.53 833.00 1.10 0.00 2021-02-13T00:26:45Z- AHRS_M2 msg: 1613176005.82 epoch sec; seqNo:12595 2021-02-13T00:26:45Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-13T00:26:45Z- handleDepth()-Depth_Keller msg: 1613176005825 epoch sec; seqNo:12695; depth 12.52 2021-02-13T00:26:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:26:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:26:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:26:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:26:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:26:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:26:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:26:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:26:45Z- handleDvl() - RDI_Pathfinder msg: 1613176005.83 epoch sec; seqNo:6219 2021-02-13T00:26:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:26:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.03 , 0.18 2021-02-13T00:26:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176005.85 epoch sec; seqNo:12695; 4059181.70 north; 592562.28 east 2021-02-13T00:26:46Z- AHRS_M2 msg: 1613176006.23 epoch sec; seqNo:12596 2021-02-13T00:26:46Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.14 psi 2021-02-13T00:26:46Z- handleDepth()-Depth_Keller msg: 1613176006231 epoch sec; seqNo:12696; depth 12.41 2021-02-13T00:26:46Z- handleDvl() - RDI_Pathfinder msg: 1613176006.24 epoch sec; seqNo:6220 2021-02-13T00:26:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.14 , 116.37 , 77.24 2021-02-13T00:26:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.16 , -0.01 2021-02-13T00:26:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176006.26 epoch sec; seqNo:12696; 4059181.70 north; 592562.28 east 2021-02-13T00:26:46Z- AHRS_M2 msg: 1613176006.63 epoch sec; seqNo:12597 2021-02-13T00:26:46Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.14 psi 2021-02-13T00:26:46Z- handleDepth()-Depth_Keller msg: 1613176006632 epoch sec; seqNo:12697; depth 12.27 2021-02-13T00:26:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176006.64 epoch sec; seqNo:12697; 4059182.10 north; 592563.50 east 2021-02-13T00:26:47Z- AHRS_M2 msg: 1613176007.03 epoch sec; seqNo:12598 2021-02-13T00:26:47Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.13 psi 2021-02-13T00:26:47Z- handleDepth()-Depth_Keller msg: 1613176007038 epoch sec; seqNo:12698; depth 12.16 2021-02-13T00:26:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:26:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:26:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:26:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:26:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:26:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:26:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:26:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:26:47Z- handleDvl() - RDI_Pathfinder msg: 1613176007.04 epoch sec; seqNo:6221 2021-02-13T00:26:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:26:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.16 , -0.01 2021-02-13T00:26:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176007.05 epoch sec; seqNo:12698; 4059182.47 north; 592563.49 east 2021-02-13T00:26:47Z- AHRS_M2 msg: 1613176007.44 epoch sec; seqNo:12599 2021-02-13T00:26:47Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.13 psi 2021-02-13T00:26:47Z- handleDepth()-Depth_Keller msg: 1613176007440 epoch sec; seqNo:12699; depth 12.05 2021-02-13T00:26:47Z- handleDvl() - RDI_Pathfinder msg: 1613176007.45 epoch sec; seqNo:6222 2021-02-13T00:26:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.77 , 101.29 , 117.52 , 78.31 2021-02-13T00:26:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.12 , -0.22 2021-02-13T00:26:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176007.47 epoch sec; seqNo:12699; 4059182.47 north; 592563.49 east 2021-02-13T00:26:47Z- AHRS_M2 msg: 1613176007.84 epoch sec; seqNo:12600 2021-02-13T00:26:47Z- AHRS_M2 msg: 0.67 phi; 0.36 theta; -0.13 psi 2021-02-13T00:26:47Z- handleDepth()-Depth_Keller msg: 1613176007844 epoch sec; seqNo:12700; depth 11.90 2021-02-13T00:26:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176007.86 epoch sec; seqNo:12700; 4059182.85 north; 592563.49 east 2021-02-13T00:26:48Z- AHRS_M2 msg: 1613176008.27 epoch sec; seqNo:12601 2021-02-13T00:26:48Z- AHRS_M2 msg: 0.68 phi; 0.35 theta; -0.12 psi 2021-02-13T00:26:48Z- handleDepth()-Depth_Keller msg: 1613176008274 epoch sec; seqNo:12701; depth 11.80 2021-02-13T00:26:48Z- handleDvl() - RDI_Pathfinder msg: 1613176008.29 epoch sec; seqNo:6223 2021-02-13T00:26:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.43 , 116.30 , 75.61 2021-02-13T00:26:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , -0.25 2021-02-13T00:26:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176008.32 epoch sec; seqNo:12701; 4059182.85 north; 592563.49 east 2021-02-13T00:26:48Z- AHRS_M2 msg: 1613176008.65 epoch sec; seqNo:12602 2021-02-13T00:26:48Z- AHRS_M2 msg: 0.69 phi; 0.34 theta; -0.11 psi 2021-02-13T00:26:48Z- handleDepth()-Depth_Keller msg: 1613176008652 epoch sec; seqNo:12702; depth 11.64 2021-02-13T00:26:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176008.67 epoch sec; seqNo:12702; 4059183.23 north; 592563.49 east 2021-02-13T00:26:49Z- AHRS_M2 msg: 1613176009.05 epoch sec; seqNo:12603 2021-02-13T00:26:49Z- AHRS_M2 msg: 0.69 phi; 0.34 theta; -0.11 psi 2021-02-13T00:26:49Z- handleDepth()-Depth_Keller msg: 1613176009056 epoch sec; seqNo:12703; depth 11.53 2021-02-13T00:26:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176009.06 epoch sec; seqNo:12703; 4059183.62 north; 592564.70 east 2021-02-13T00:26:49Z- AHRS_M2 msg: 1613176009.46 epoch sec; seqNo:12604 2021-02-13T00:26:49Z- AHRS_M2 msg: 0.69 phi; 0.33 theta; -0.12 psi 2021-02-13T00:26:49Z- handleDepth()-Depth_Keller msg: 1613176009460 epoch sec; seqNo:12704; depth 11.42 2021-02-13T00:26:49Z- handleDvl() - RDI_Pathfinder msg: 1613176009.47 epoch sec; seqNo:6224 2021-02-13T00:26:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.77 , 99.94 , 113.46 , 76.02 2021-02-13T00:26:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.25 , -0.07 2021-02-13T00:26:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176009.48 epoch sec; seqNo:12704; 4059183.62 north; 592564.70 east 2021-02-13T00:26:49Z- AHRS_M2 msg: 1613176009.86 epoch sec; seqNo:12605 2021-02-13T00:26:49Z- AHRS_M2 msg: 0.68 phi; 0.32 theta; -0.13 psi 2021-02-13T00:26:49Z- handleDepth()-Depth_Keller msg: 1613176009873 epoch sec; seqNo:12705; depth 11.26 2021-02-13T00:26:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176009.88 epoch sec; seqNo:12705; 4059184.00 north; 592564.70 east 2021-02-13T00:26:50Z- AHRS_M2 msg: 1613176010.26 epoch sec; seqNo:12606 2021-02-13T00:26:50Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.14 psi 2021-02-13T00:26:50Z- handleDepth()-Depth_Keller msg: 1613176010268 epoch sec; seqNo:12706; depth 11.13 2021-02-13T00:26:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176010.29 epoch sec; seqNo:12706; 4059184.38 north; 592564.69 east 2021-02-13T00:26:50Z- AHRS_M2 msg: 1613176010.67 epoch sec; seqNo:12607 2021-02-13T00:26:50Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.16 psi 2021-02-13T00:26:50Z- handleDepth()-Depth_Keller msg: 1613176010677 epoch sec; seqNo:12707; depth 11.00 2021-02-13T00:26:50Z- handleDvl() - RDI_Pathfinder msg: 1613176010.69 epoch sec; seqNo:6225 2021-02-13T00:26:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 100.21 , 113.73 , 78.58 2021-02-13T00:26:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.27 , 0.23 2021-02-13T00:26:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176010.71 epoch sec; seqNo:12707; 4059184.76 north; 592564.69 east 2021-02-13T00:26:50Z- LcmTrn::handleMessages() - handling 46 messages took 0.69 ms and 132 calls... 2021-02-13T00:26:50Z- waiting for 1613176008.44, time is 1613176010.71 2021-02-13T00:26:50Z- LcmTrn::updateTrn() equating pose 1613176010.71 and meas times 1613176010.71 2021-02-13T00:26:50Z- LcmTrn::updateTrn() >>>> pose time 1613176010.71, meas time 1613176010.71 <<<< 2021-02-13T00:26:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176010.71 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:26:50Z- TerrainNav::motionUpdate() - currEstimate = (4059185, 592564.69, 11.00) 2021-02-13T00:26:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:26:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:26:50Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176010.71 sec.; No relevant navigation data available 2021-02-13T00:26:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:26:50Z- TNavBF::mmse selected from filter -1 2021-02-13T00:26:50Z- variances([0], [2], and psi_berg) = 702.477427 832.999261 0.000000 2021-02-13T00:26:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:26:50Z- LcmTrn::publishEstimates() - MLE : 4059184.26 592534.19 11.00 2021-02-13T00:26:50Z- LcmTrn::publishEstimates() - MMSE : 4059184.76 592564.69 11.00 2021-02-13T00:26:50Z- LcmTrn::publishEstimates() - COVAR: 702.48 833.00 1.31 0.00 2021-02-13T00:26:51Z- AHRS_M2 msg: 1613176011.07 epoch sec; seqNo:12608 2021-02-13T00:26:51Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.16 psi 2021-02-13T00:26:51Z- handleDepth()-Depth_Keller msg: 1613176011081 epoch sec; seqNo:12708; depth 10.85 2021-02-13T00:26:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176011.09 epoch sec; seqNo:12708; 4059184.76 north; 592564.69 east 2021-02-13T00:26:51Z- AHRS_M2 msg: 1613176011.48 epoch sec; seqNo:12609 2021-02-13T00:26:51Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-13T00:26:51Z- handleDepth()-Depth_Keller msg: 1613176011481 epoch sec; seqNo:12709; depth 10.74 2021-02-13T00:26:51Z- handleDvl() - RDI_Pathfinder msg: 1613176011.49 epoch sec; seqNo:6226 2021-02-13T00:26:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.92 , 115.08 , 162.41 2021-02-13T00:26:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.31 , -0.66 , 0.01 2021-02-13T00:26:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176011.51 epoch sec; seqNo:12709; 4059185.14 north; 592564.69 east 2021-02-13T00:26:51Z- AHRS_M2 msg: 1613176011.88 epoch sec; seqNo:12610 2021-02-13T00:26:51Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-13T00:26:51Z- handleDepth()-Depth_Keller msg: 1613176011884 epoch sec; seqNo:12710; depth 10.59 2021-02-13T00:26:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176011.90 epoch sec; seqNo:12710; 4059185.52 north; 592564.68 east 2021-02-13T00:26:52Z- AHRS_M2 msg: 1613176012.28 epoch sec; seqNo:12611 2021-02-13T00:26:52Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T00:26:52Z- handleDepth()-Depth_Keller msg: 1613176012288 epoch sec; seqNo:12711; depth 10.51 2021-02-13T00:26:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176012.30 epoch sec; seqNo:12711; 4059185.52 north; 592564.68 east 2021-02-13T00:26:52Z- AHRS_M2 msg: 1613176012.69 epoch sec; seqNo:12612 2021-02-13T00:26:52Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T00:26:52Z- handleDepth()-Depth_Keller msg: 1613176012692 epoch sec; seqNo:12712; depth 10.38 2021-02-13T00:26:52Z- handleDvl() - RDI_Pathfinder msg: 1613176012.70 epoch sec; seqNo:6227 2021-02-13T00:26:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.82 , 104.67 , 116.57 , 79.39 2021-02-13T00:26:52Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , 0.25 , 0.17 2021-02-13T00:26:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176012.71 epoch sec; seqNo:12712; 4059185.90 north; 592564.68 east 2021-02-13T00:26:53Z- AHRS_M2 msg: 1613176013.09 epoch sec; seqNo:12613 2021-02-13T00:26:53Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-13T00:26:53Z- handleDepth()-Depth_Keller msg: 1613176013097 epoch sec; seqNo:12713; depth 10.26 2021-02-13T00:26:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176013.11 epoch sec; seqNo:12713; 4059186.28 north; 592564.67 east 2021-02-13T00:26:53Z- AHRS_M2 msg: 1613176013.50 epoch sec; seqNo:12614 2021-02-13T00:26:53Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.17 psi 2021-02-13T00:26:53Z- handleDepth()-Depth_Keller msg: 1613176013500 epoch sec; seqNo:12714; depth 10.15 2021-02-13T00:26:53Z- handleDvl() - RDI_Pathfinder msg: 1613176013.53 epoch sec; seqNo:6228 2021-02-13T00:26:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.84 , 104.07 , 120.29 , 79.73 2021-02-13T00:26:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.25 , -0.18 2021-02-13T00:26:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176013.54 epoch sec; seqNo:12714; 4059186.65 north; 592564.67 east 2021-02-13T00:26:53Z- AHRS_M2 msg: 1613176013.90 epoch sec; seqNo:12615 2021-02-13T00:26:53Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.17 psi 2021-02-13T00:26:53Z- handleDepth()-Depth_Keller msg: 1613176013905 epoch sec; seqNo:12715; depth 10.03 2021-02-13T00:26:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176013.92 epoch sec; seqNo:12715; 4059186.67 north; 592565.89 east 2021-02-13T00:26:54Z- AHRS_M2 msg: 1613176014.30 epoch sec; seqNo:12616 2021-02-13T00:26:54Z- AHRS_M2 msg: 0.67 phi; 0.32 theta; -0.18 psi 2021-02-13T00:26:54Z- handleDepth()-Depth_Keller msg: 1613176014310 epoch sec; seqNo:12716; depth 9.92 2021-02-13T00:26:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176014.32 epoch sec; seqNo:12716; 4059187.05 north; 592565.89 east 2021-02-13T00:26:54Z- AHRS_M2 msg: 1613176014.71 epoch sec; seqNo:12617 2021-02-13T00:26:54Z- AHRS_M2 msg: 0.67 phi; 0.31 theta; -0.18 psi 2021-02-13T00:26:54Z- handleDepth()-Depth_Keller msg: 1613176014712 epoch sec; seqNo:12717; depth 9.82 2021-02-13T00:26:54Z- handleDvl() - RDI_Pathfinder msg: 1613176014.73 epoch sec; seqNo:6229 2021-02-13T00:26:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.35 , 107.99 , 117.45 , 80.94 2021-02-13T00:26:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , -0.31 2021-02-13T00:26:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176014.74 epoch sec; seqNo:12717; 4059187.05 north; 592565.89 east 2021-02-13T00:26:55Z- AHRS_M2 msg: 1613176015.12 epoch sec; seqNo:12618 2021-02-13T00:26:55Z- AHRS_M2 msg: 0.67 phi; 0.30 theta; -0.18 psi 2021-02-13T00:26:55Z- handleDepth()-Depth_Keller msg: 1613176015124 epoch sec; seqNo:12718; depth 9.70 2021-02-13T00:26:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176015.14 epoch sec; seqNo:12718; 4059187.43 north; 592565.88 east 2021-02-13T00:26:55Z- AHRS_M2 msg: 1613176015.53 epoch sec; seqNo:12619 2021-02-13T00:26:55Z- AHRS_M2 msg: 0.66 phi; 0.29 theta; -0.18 psi 2021-02-13T00:26:55Z- handleDepth()-Depth_Keller msg: 1613176015541 epoch sec; seqNo:12719; depth 9.59 2021-02-13T00:26:55Z- handleDvl() - RDI_Pathfinder msg: 1613176015.55 epoch sec; seqNo:6230 2021-02-13T00:26:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.35 , 105.28 , 117.45 , 80.94 2021-02-13T00:26:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.17 , -0.20 2021-02-13T00:26:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176015.56 epoch sec; seqNo:12719; 4059187.80 north; 592565.88 east 2021-02-13T00:26:55Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-13T00:26:55Z- waiting for 1613176013.71, time is 1613176015.56 2021-02-13T00:26:55Z- LcmTrn::updateTrn() equating pose 1613176015.56 and meas times 1613176015.56 2021-02-13T00:26:55Z- LcmTrn::updateTrn() >>>> pose time 1613176015.56, meas time 1613176015.56 <<<< 2021-02-13T00:26:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176015.56 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:26:55Z- TerrainNav::motionUpdate() - currEstimate = (4059188, 592565.88, 9.59) 2021-02-13T00:26:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:26:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:26:55Z- TNavBF::Projecting Measurements 2021-02-13T00:26:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:26:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:26:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:26:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:26:55Z- TNavFilter::4 good beams. 2021-02-13T00:26:55Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:26:55Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:26:55Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:26:55Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:26:55Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:26:55Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:26:55Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:26:55Z- TNavBF::mmse selected from filter -1 2021-02-13T00:26:55Z- TerrainMapOctree: (vehN, vehE ) = (4059187.80, 592565.88). 2021-02-13T00:26:55Z- TerrainMapOctree: Min Distance = 2662.50. 2021-02-13T00:26:55Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:26:55Z- There is no specific good beam that all particles have in common. 2021-02-13T00:26:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:26:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.48 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 11.93 2021-02-13T00:26:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:26:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 17.73 2021-02-13T00:26:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:26:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 22.07 2021-02-13T00:26:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:26:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 10.48 2021-02-13T00:26:55Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:26:55Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:26:55Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:26:55Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:26:55Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:26:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:26:55Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:26:55Z- Filter 0 No beams used in subcloud update 2021-02-13T00:26:55Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:26:55Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:26:55Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 702.4 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:26:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176015.56 sec. 2021-02-13T00:26:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:26:55Z- TNavBF::mmse selected from filter -1 2021-02-13T00:26:55Z- variances([0], [2], and psi_berg) = 702.427191 832.999234 0.000000 2021-02-13T00:26:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:26:55Z- LcmTrn::publishEstimates() - MLE : 4059187.30 592535.38 9.59 2021-02-13T00:26:55Z- LcmTrn::publishEstimates() - MMSE : 4059187.80 592565.88 9.59 2021-02-13T00:26:55Z- LcmTrn::publishEstimates() - COVAR: 702.43 833.00 1.08 0.00 2021-02-13T00:26:55Z- AHRS_M2 msg: 1613176015.92 epoch sec; seqNo:12620 2021-02-13T00:26:55Z- AHRS_M2 msg: 0.66 phi; 0.29 theta; -0.18 psi 2021-02-13T00:26:55Z- handleDepth()-Depth_Keller msg: 1613176015924 epoch sec; seqNo:12720; depth 9.47 2021-02-13T00:26:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176015.94 epoch sec; seqNo:12720; 4059187.80 north; 592565.88 east 2021-02-13T00:26:56Z- AHRS_M2 msg: 1613176016.32 epoch sec; seqNo:12621 2021-02-13T00:26:56Z- AHRS_M2 msg: 0.65 phi; 0.29 theta; -0.18 psi 2021-02-13T00:26:56Z- handleDepth()-Depth_Keller msg: 1613176016328 epoch sec; seqNo:12721; depth 9.40 2021-02-13T00:26:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176016.34 epoch sec; seqNo:12721; 4059188.18 north; 592565.87 east 2021-02-13T00:26:56Z- handleDepth()-Depth_Keller msg: 1613176016753 epoch sec; seqNo:12722; depth 9.29 2021-02-13T00:26:56Z- handleDvl() - RDI_Pathfinder msg: 1613176016.76 epoch sec; seqNo:6231 2021-02-13T00:26:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.42 , 105.35 , 114.82 , 81.01 2021-02-13T00:26:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.20 , 0.00 2021-02-13T00:26:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176016.78 epoch sec; seqNo:12722; 4059188.58 north; 592567.09 east 2021-02-13T00:26:57Z- AHRS_M2 msg: 1613176017.14 epoch sec; seqNo:12622 2021-02-13T00:26:57Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.19 psi 2021-02-13T00:26:57Z- handleDepth()-Depth_Keller msg: 1613176017144 epoch sec; seqNo:12723; depth 9.17 2021-02-13T00:26:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176017.16 epoch sec; seqNo:12723; 4059188.58 north; 592567.09 east 2021-02-13T00:26:57Z- AHRS_M2 msg: 1613176017.54 epoch sec; seqNo:12623 2021-02-13T00:26:57Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.19 psi 2021-02-13T00:26:57Z- handleDepth()-Depth_Keller msg: 1613176017540 epoch sec; seqNo:12724; depth 9.09 2021-02-13T00:26:57Z- handleDvl() - RDI_Pathfinder msg: 1613176017.56 epoch sec; seqNo:6232 2021-02-13T00:26:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 106.77 , 114.88 , 81.08 2021-02-13T00:26:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.15 , 0.16 2021-02-13T00:26:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176017.57 epoch sec; seqNo:12724; 4059188.95 north; 592567.09 east 2021-02-13T00:26:57Z- AHRS_M2 msg: 1613176017.94 epoch sec; seqNo:12624 2021-02-13T00:26:57Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.19 psi 2021-02-13T00:26:57Z- handleDepth()-Depth_Keller msg: 1613176017945 epoch sec; seqNo:12725; depth 8.97 2021-02-13T00:26:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:26:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:26:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:26:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:26:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:26:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:26:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:26:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:26:57Z- handleDvl() - RDI_Pathfinder msg: 1613176017.95 epoch sec; seqNo:6233 2021-02-13T00:26:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:26:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.15 , 0.16 2021-02-13T00:26:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176017.97 epoch sec; seqNo:12725; 4059189.33 north; 592567.08 east 2021-02-13T00:26:58Z- AHRS_M2 msg: 1613176018.35 epoch sec; seqNo:12625 2021-02-13T00:26:58Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.19 psi 2021-02-13T00:26:58Z- handleDepth()-Depth_Keller msg: 1613176018363 epoch sec; seqNo:12726; depth 8.87 2021-02-13T00:26:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176018.39 epoch sec; seqNo:12726; 4059189.71 north; 592567.08 east 2021-02-13T00:26:58Z- AHRS_M2 msg: 1613176018.75 epoch sec; seqNo:12626 2021-02-13T00:26:58Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.19 psi 2021-02-13T00:26:58Z- handleDepth()-Depth_Keller msg: 1613176018762 epoch sec; seqNo:12727; depth 8.76 2021-02-13T00:26:58Z- handleDvl() - RDI_Pathfinder msg: 1613176018.77 epoch sec; seqNo:6234 2021-02-13T00:26:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.77 , 106.70 , 116.17 , 79.69 2021-02-13T00:26:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , 0.22 2021-02-13T00:26:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176018.78 epoch sec; seqNo:12727; 4059190.09 north; 592567.07 east 2021-02-13T00:26:59Z- AHRS_M2 msg: 1613176019.15 epoch sec; seqNo:12627 2021-02-13T00:26:59Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.19 psi 2021-02-13T00:26:59Z- handleDepth()-Depth_Keller msg: 1613176019158 epoch sec; seqNo:12728; depth 8.66 2021-02-13T00:26:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:26:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:26:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:26:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:26:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:26:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:26:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:26:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:26:59Z- handleDvl() - RDI_Pathfinder msg: 1613176019.16 epoch sec; seqNo:6235 2021-02-13T00:26:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:26:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , 0.22 2021-02-13T00:26:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176019.17 epoch sec; seqNo:12728; 4059190.09 north; 592567.07 east 2021-02-13T00:26:59Z- AHRS_M2 msg: 1613176019.57 epoch sec; seqNo:12628 2021-02-13T00:26:59Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.19 psi 2021-02-13T00:26:59Z- handleDepth()-Depth_Keller msg: 1613176019577 epoch sec; seqNo:12729; depth 8.56 2021-02-13T00:26:59Z- handleDvl() - RDI_Pathfinder msg: 1613176019.62 epoch sec; seqNo:6236 2021-02-13T00:26:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.12 , 106.70 , 117.52 , 81.01 2021-02-13T00:26:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.09 , 0.20 2021-02-13T00:26:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176019.63 epoch sec; seqNo:12729; 4059190.47 north; 592567.07 east 2021-02-13T00:26:59Z- AHRS_M2 msg: 1613176019.96 epoch sec; seqNo:12629 2021-02-13T00:26:59Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.18 psi 2021-02-13T00:26:59Z- handleDepth()-Depth_Keller msg: 1613176019964 epoch sec; seqNo:12730; depth 8.44 2021-02-13T00:26:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176019.98 epoch sec; seqNo:12730; 4059190.86 north; 592568.29 east 2021-02-13T00:27:00Z- AHRS_M2 msg: 1613176020.36 epoch sec; seqNo:12630 2021-02-13T00:27:00Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.16 psi 2021-02-13T00:27:00Z- handleDepth()-Depth_Keller msg: 1613176020373 epoch sec; seqNo:12731; depth 8.35 2021-02-13T00:27:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:27:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:27:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:27:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:27:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:27:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:27:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:27:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:27:00Z- handleDvl() - RDI_Pathfinder msg: 1613176020.38 epoch sec; seqNo:6237 2021-02-13T00:27:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:27:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.09 , 0.20 2021-02-13T00:27:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176020.38 epoch sec; seqNo:12731; 4059191.24 north; 592568.28 east 2021-02-13T00:27:00Z- AHRS_M2 msg: 1613176020.77 epoch sec; seqNo:12631 2021-02-13T00:27:00Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.15 psi 2021-02-13T00:27:00Z- handleDepth()-Depth_Keller msg: 1613176020776 epoch sec; seqNo:12732; depth 8.24 2021-02-13T00:27:00Z- handleDvl() - RDI_Pathfinder msg: 1613176020.79 epoch sec; seqNo:6238 2021-02-13T00:27:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 105.28 , 121.51 , 80.94 2021-02-13T00:27:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.19 , 0.05 2021-02-13T00:27:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176020.80 epoch sec; seqNo:12732; 4059191.24 north; 592568.28 east 2021-02-13T00:27:00Z- LcmTrn::handleMessages() - handling 46 messages took 0.65 ms and 133 calls... 2021-02-13T00:27:00Z- waiting for 1613176018.56, time is 1613176020.80 2021-02-13T00:27:00Z- LcmTrn::updateTrn() equating pose 1613176020.80 and meas times 1613176020.80 2021-02-13T00:27:00Z- LcmTrn::updateTrn() >>>> pose time 1613176020.80, meas time 1613176020.80 <<<< 2021-02-13T00:27:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176020.80 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:27:00Z- TerrainNav::motionUpdate() - currEstimate = (4059191, 592568.28, 8.24) 2021-02-13T00:27:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:27:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:27:00Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176020.80 sec.; No relevant navigation data available 2021-02-13T00:27:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:27:00Z- TNavBF::mmse selected from filter -1 2021-02-13T00:27:00Z- variances([0], [2], and psi_berg) = 702.370308 832.999203 0.000000 2021-02-13T00:27:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:27:00Z- LcmTrn::publishEstimates() - MLE : 4059190.74 592537.78 8.24 2021-02-13T00:27:00Z- LcmTrn::publishEstimates() - MMSE : 4059191.24 592568.28 8.24 2021-02-13T00:27:00Z- LcmTrn::publishEstimates() - COVAR: 702.37 833.00 1.30 0.00 2021-02-13T00:27:01Z- AHRS_M2 msg: 1613176021.17 epoch sec; seqNo:12632 2021-02-13T00:27:01Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.14 psi 2021-02-13T00:27:01Z- handleDepth()-Depth_Keller msg: 1613176021179 epoch sec; seqNo:12733; depth 8.12 2021-02-13T00:27:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176021.19 epoch sec; seqNo:12733; 4059191.62 north; 592568.28 east 2021-02-13T00:27:01Z- AHRS_M2 msg: 1613176021.58 epoch sec; seqNo:12633 2021-02-13T00:27:01Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.14 psi 2021-02-13T00:27:01Z- handleDepth()-Depth_Keller msg: 1613176021580 epoch sec; seqNo:12734; depth 8.02 2021-02-13T00:27:01Z- handleDvl() - RDI_Pathfinder msg: 1613176021.59 epoch sec; seqNo:6239 2021-02-13T00:27:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.69 , 106.57 , 122.79 , 80.88 2021-02-13T00:27:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.14 , -0.13 2021-02-13T00:27:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176021.61 epoch sec; seqNo:12734; 4059192.00 north; 592568.27 east 2021-02-13T00:27:01Z- AHRS_M2 msg: 1613176021.98 epoch sec; seqNo:12634 2021-02-13T00:27:01Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-13T00:27:01Z- handleDepth()-Depth_Keller msg: 1613176021993 epoch sec; seqNo:12735; depth 7.89 2021-02-13T00:27:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176022.01 epoch sec; seqNo:12735; 4059192.01 north; 592569.49 east 2021-02-13T00:27:02Z- AHRS_M2 msg: 1613176022.38 epoch sec; seqNo:12635 2021-02-13T00:27:02Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.15 psi 2021-02-13T00:27:02Z- handleDepth()-Depth_Keller msg: 1613176022388 epoch sec; seqNo:12736; depth 7.76 2021-02-13T00:27:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176022.40 epoch sec; seqNo:12736; 4059192.39 north; 592569.49 east 2021-02-13T00:27:02Z- AHRS_M2 msg: 1613176022.79 epoch sec; seqNo:12636 2021-02-13T00:27:02Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.16 psi 2021-02-13T00:27:02Z- handleDepth()-Depth_Keller msg: 1613176022792 epoch sec; seqNo:12737; depth 7.64 2021-02-13T00:27:02Z- handleDvl() - RDI_Pathfinder msg: 1613176022.80 epoch sec; seqNo:6240 2021-02-13T00:27:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.56 , 106.43 , 124.01 , 82.09 2021-02-13T00:27:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , -0.13 2021-02-13T00:27:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176022.83 epoch sec; seqNo:12737; 4059192.77 north; 592569.49 east 2021-02-13T00:27:03Z- AHRS_M2 msg: 1613176023.19 epoch sec; seqNo:12637 2021-02-13T00:27:03Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.17 psi 2021-02-13T00:27:03Z- handleDepth()-Depth_Keller msg: 1613176023201 epoch sec; seqNo:12738; depth 7.52 2021-02-13T00:27:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176023.22 epoch sec; seqNo:12738; 4059192.77 north; 592569.49 east 2021-02-13T00:27:03Z- AHRS_M2 msg: 1613176023.60 epoch sec; seqNo:12638 2021-02-13T00:27:03Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.16 psi 2021-02-13T00:27:03Z- handleDepth()-Depth_Keller msg: 1613176023607 epoch sec; seqNo:12739; depth 7.41 2021-02-13T00:27:03Z- handleDvl() - RDI_Pathfinder msg: 1613176023.62 epoch sec; seqNo:6241 2021-02-13T00:27:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.21 , 105.08 , 121.31 , 82.09 2021-02-13T00:27:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.03 , 0.05 2021-02-13T00:27:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176023.63 epoch sec; seqNo:12739; 4059193.15 north; 592569.48 east 2021-02-13T00:27:04Z- AHRS_M2 msg: 1613176024.00 epoch sec; seqNo:12639 2021-02-13T00:27:04Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.15 psi 2021-02-13T00:27:04Z- handleDepth()-Depth_Keller msg: 1613176024004 epoch sec; seqNo:12740; depth 7.30 2021-02-13T00:27:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:27:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:27:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:27:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:27:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:27:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:27:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:27:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:27:04Z- handleDvl() - RDI_Pathfinder msg: 1613176024.01 epoch sec; seqNo:6242 2021-02-13T00:27:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:27:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.03 , 0.05 2021-02-13T00:27:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176024.02 epoch sec; seqNo:12740; 4059193.53 north; 592569.48 east 2021-02-13T00:27:04Z- AHRS_M2 msg: 1613176024.40 epoch sec; seqNo:12640 2021-02-13T00:27:04Z- AHRS_M2 msg: 0.60 phi; 0.38 theta; -0.13 psi 2021-02-13T00:27:04Z- handleDepth()-Depth_Keller msg: 1613176024408 epoch sec; seqNo:12741; depth 7.19 2021-02-13T00:27:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176024.42 epoch sec; seqNo:12741; 4059193.92 north; 592570.70 east 2021-02-13T00:27:04Z- AHRS_M2 msg: 1613176024.81 epoch sec; seqNo:12641 2021-02-13T00:27:04Z- AHRS_M2 msg: 0.61 phi; 0.39 theta; -0.12 psi 2021-02-13T00:27:04Z- handleDepth()-Depth_Keller msg: 1613176024817 epoch sec; seqNo:12742; depth 7.05 2021-02-13T00:27:04Z- handleDvl() - RDI_Pathfinder msg: 1613176024.83 epoch sec; seqNo:6243 2021-02-13T00:27:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.62 , 107.85 , 128.13 , 82.16 2021-02-13T00:27:04Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.11 , 0.11 2021-02-13T00:27:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176024.84 epoch sec; seqNo:12742; 4059193.92 north; 592570.70 east 2021-02-13T00:27:05Z- AHRS_M2 msg: 1613176025.22 epoch sec; seqNo:12642 2021-02-13T00:27:05Z- AHRS_M2 msg: 0.62 phi; 0.39 theta; -0.10 psi 2021-02-13T00:27:05Z- handleDepth()-Depth_Keller msg: 1613176025223 epoch sec; seqNo:12743; depth 6.93 2021-02-13T00:27:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176025.24 epoch sec; seqNo:12743; 4059194.30 north; 592570.69 east 2021-02-13T00:27:05Z- AHRS_M2 msg: 1613176025.62 epoch sec; seqNo:12643 2021-02-13T00:27:05Z- AHRS_M2 msg: 0.64 phi; 0.39 theta; -0.09 psi 2021-02-13T00:27:05Z- handleDepth()-Depth_Keller msg: 1613176025625 epoch sec; seqNo:12744; depth 6.82 2021-02-13T00:27:05Z- handleDvl() - RDI_Pathfinder msg: 1613176025.64 epoch sec; seqNo:6244 2021-02-13T00:27:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.91 , 106.84 , 127.66 , 78.47 2021-02-13T00:27:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.19 , -0.12 2021-02-13T00:27:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176025.65 epoch sec; seqNo:12744; 4059194.68 north; 592570.69 east 2021-02-13T00:27:06Z- LcmTrn::handleMessages() - handling 42 messages took 0.64 ms and 130 calls... 2021-02-13T00:27:06Z- waiting for 1613176023.80, time is 1613176025.65 2021-02-13T00:27:06Z- LcmTrn::updateTrn() equating pose 1613176025.65 and meas times 1613176025.65 2021-02-13T00:27:06Z- LcmTrn::updateTrn() >>>> pose time 1613176025.65, meas time 1613176025.65 <<<< 2021-02-13T00:27:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176025.65 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:27:06Z- TerrainNav::motionUpdate() - currEstimate = (4059195, 592570.69, 6.82) 2021-02-13T00:27:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:27:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:27:06Z- TNavBF::Projecting Measurements 2021-02-13T00:27:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:27:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:27:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:27:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:27:06Z- TNavFilter::4 good beams. 2021-02-13T00:27:06Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:27:06Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:27:06Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:27:06Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:27:06Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:27:06Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:27:06Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:27:06Z- TNavBF::mmse selected from filter -1 2021-02-13T00:27:06Z- TerrainMapOctree: (vehN, vehE ) = (4059194.68, 592570.69). 2021-02-13T00:27:06Z- TerrainMapOctree: Min Distance = 2654.14. 2021-02-13T00:27:06Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:27:06Z- There is no specific good beam that all particles have in common. 2021-02-13T00:27:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:27:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.51 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:27:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:27:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.39 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 18.26 2021-02-13T00:27:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:27:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 26.08 2021-02-13T00:27:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:27:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T00:27:06Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:27:06Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:27:06Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:27:06Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:27:06Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:27:06Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:27:06Z- Filter 0 No beams used in subcloud update 2021-02-13T00:27:06Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:27:06Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:27:06Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 702.3 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:27:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176025.65 sec. 2021-02-13T00:27:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:27:06Z- TNavBF::mmse selected from filter -1 2021-02-13T00:27:06Z- variances([0], [2], and psi_berg) = 702.313460 832.999172 0.000000 2021-02-13T00:27:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:27:06Z- LcmTrn::publishEstimates() - MLE : 4059194.18 592540.19 6.82 2021-02-13T00:27:06Z- LcmTrn::publishEstimates() - MMSE : 4059194.68 592570.69 6.82 2021-02-13T00:27:06Z- LcmTrn::publishEstimates() - COVAR: 702.31 833.00 1.12 0.00 2021-02-13T00:27:06Z- AHRS_M2 msg: 1613176026.02 epoch sec; seqNo:12644 2021-02-13T00:27:06Z- AHRS_M2 msg: 0.65 phi; 0.39 theta; -0.08 psi 2021-02-13T00:27:06Z- handleDepth()-Depth_Keller msg: 1613176026024 epoch sec; seqNo:12745; depth 6.69 2021-02-13T00:27:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176026.04 epoch sec; seqNo:12745; 4059194.68 north; 592570.69 east 2021-02-13T00:27:06Z- AHRS_M2 msg: 1613176026.43 epoch sec; seqNo:12645 2021-02-13T00:27:06Z- AHRS_M2 msg: 0.66 phi; 0.38 theta; -0.09 psi 2021-02-13T00:27:06Z- handleDepth()-Depth_Keller msg: 1613176026440 epoch sec; seqNo:12746; depth 6.56 2021-02-13T00:27:06Z- handleDvl() - RDI_Pathfinder msg: 1613176026.45 epoch sec; seqNo:6245 2021-02-13T00:27:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 105.28 , 130.57 , 81.49 2021-02-13T00:27:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , 0.00 , -0.41 2021-02-13T00:27:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176026.46 epoch sec; seqNo:12746; 4059195.07 north; 592571.90 east 2021-02-13T00:27:06Z- AHRS_M2 msg: 1613176026.83 epoch sec; seqNo:12646 2021-02-13T00:27:06Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.11 psi 2021-02-13T00:27:06Z- handleDepth()-Depth_Keller msg: 1613176026832 epoch sec; seqNo:12747; depth 6.43 2021-02-13T00:27:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176026.85 epoch sec; seqNo:12747; 4059195.07 north; 592571.90 east 2021-02-13T00:27:07Z- AHRS_M2 msg: 1613176027.23 epoch sec; seqNo:12647 2021-02-13T00:27:07Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.14 psi 2021-02-13T00:27:07Z- handleDepth()-Depth_Keller msg: 1613176027237 epoch sec; seqNo:12748; depth 6.27 2021-02-13T00:27:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176027.25 epoch sec; seqNo:12748; 4059195.45 north; 592571.90 east 2021-02-13T00:27:07Z- AHRS_M2 msg: 1613176027.64 epoch sec; seqNo:12648 2021-02-13T00:27:07Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.17 psi 2021-02-13T00:27:07Z- handleDepth()-Depth_Keller msg: 1613176027640 epoch sec; seqNo:12749; depth 6.13 2021-02-13T00:27:07Z- handleDvl() - RDI_Pathfinder msg: 1613176027.65 epoch sec; seqNo:6246 2021-02-13T00:27:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:27:07Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:27:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176027.66 epoch sec; seqNo:12749; 4059195.45 north; 592571.90 east 2021-02-13T00:27:08Z- AHRS_M2 msg: 1613176028.04 epoch sec; seqNo:12649 2021-02-13T00:27:08Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.18 psi 2021-02-13T00:27:08Z- handleDepth()-Depth_Keller msg: 1613176028045 epoch sec; seqNo:12750; depth 5.98 2021-02-13T00:27:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176028.06 epoch sec; seqNo:12750; 4059195.83 north; 592571.90 east 2021-02-13T00:27:08Z- AHRS_M2 msg: 1613176028.45 epoch sec; seqNo:12650 2021-02-13T00:27:08Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.18 psi 2021-02-13T00:27:08Z- handleDepth()-Depth_Keller msg: 1613176028452 epoch sec; seqNo:12751; depth 5.87 2021-02-13T00:27:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176028.48 epoch sec; seqNo:12751; 4059195.83 north; 592571.90 east 2021-02-13T00:27:08Z- AHRS_M2 msg: 1613176028.85 epoch sec; seqNo:12651 2021-02-13T00:27:08Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.17 psi 2021-02-13T00:27:08Z- handleDepth()-Depth_Keller msg: 1613176028852 epoch sec; seqNo:12752; depth 5.73 2021-02-13T00:27:08Z- handleDvl() - RDI_Pathfinder msg: 1613176028.87 epoch sec; seqNo:6247 2021-02-13T00:27:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.80 , 109.14 , 125.50 , 82.37 2021-02-13T00:27:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.43 , 0.23 2021-02-13T00:27:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176028.89 epoch sec; seqNo:12752; 4059196.20 north; 592571.89 east 2021-02-13T00:27:09Z- AHRS_M2 msg: 1613176029.25 epoch sec; seqNo:12652 2021-02-13T00:27:09Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-13T00:27:09Z- handleDepth()-Depth_Keller msg: 1613176029256 epoch sec; seqNo:12753; depth 5.59 2021-02-13T00:27:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176029.27 epoch sec; seqNo:12753; 4059196.58 north; 592571.89 east 2021-02-13T00:27:09Z- AHRS_M2 msg: 1613176029.66 epoch sec; seqNo:12653 2021-02-13T00:27:09Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.14 psi 2021-02-13T00:27:09Z- handleDepth()-Depth_Keller msg: 1613176029669 epoch sec; seqNo:12754; depth 5.48 2021-02-13T00:27:09Z- handleDvl() - RDI_Pathfinder msg: 1613176029.68 epoch sec; seqNo:6248 2021-02-13T00:27:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.98 , 108.39 , 129.21 , 0.00 2021-02-13T00:27:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , 0.60 2021-02-13T00:27:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176029.69 epoch sec; seqNo:12754; 4059196.96 north; 592571.88 east 2021-02-13T00:27:10Z- AHRS_M2 msg: 1613176030.06 epoch sec; seqNo:12654 2021-02-13T00:27:10Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-13T00:27:10Z- handleDepth()-Depth_Keller msg: 1613176030064 epoch sec; seqNo:12755; depth 5.32 2021-02-13T00:27:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176030.08 epoch sec; seqNo:12755; 4059197.35 north; 592573.10 east 2021-02-13T00:27:10Z- AHRS_M2 msg: 1613176030.46 epoch sec; seqNo:12655 2021-02-13T00:27:10Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-13T00:27:10Z- handleDepth()-Depth_Keller msg: 1613176030468 epoch sec; seqNo:12756; depth 5.19 2021-02-13T00:27:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176030.48 epoch sec; seqNo:12756; 4059197.73 north; 592573.10 east 2021-02-13T00:27:10Z- handleDepth()-Depth_Keller msg: 1613176030889 epoch sec; seqNo:12757; depth 5.03 2021-02-13T00:27:10Z- handleDvl() - RDI_Pathfinder msg: 1613176030.91 epoch sec; seqNo:6249 2021-02-13T00:27:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.27 , 104.68 , 126.98 , 83.86 2021-02-13T00:27:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.25 , 0.34 2021-02-13T00:27:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176030.92 epoch sec; seqNo:12757; 4059197.73 north; 592573.10 east 2021-02-13T00:27:11Z- LcmTrn::handleMessages() - handling 43 messages took 0.58 ms and 132 calls... 2021-02-13T00:27:11Z- waiting for 1613176028.65, time is 1613176030.92 2021-02-13T00:27:11Z- LcmTrn::updateTrn() equating pose 1613176030.92 and meas times 1613176030.92 2021-02-13T00:27:11Z- LcmTrn::updateTrn() >>>> pose time 1613176030.92, meas time 1613176030.92 <<<< 2021-02-13T00:27:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176030.92 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:27:11Z- TerrainNav::motionUpdate() - currEstimate = (4059198, 592573.10, 5.03) 2021-02-13T00:27:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:27:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:27:11Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176030.92 sec.; No relevant navigation data available 2021-02-13T00:27:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:27:11Z- TNavBF::mmse selected from filter -1 2021-02-13T00:27:11Z- variances([0], [2], and psi_berg) = 702.258729 832.999143 0.000000 2021-02-13T00:27:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:27:11Z- LcmTrn::publishEstimates() - MLE : 4059197.23 592542.60 5.03 2021-02-13T00:27:11Z- LcmTrn::publishEstimates() - MMSE : 4059197.73 592573.10 5.03 2021-02-13T00:27:11Z- LcmTrn::publishEstimates() - COVAR: 702.26 833.00 1.32 0.00 2021-02-13T00:27:11Z- AHRS_M2 msg: 1613176031.27 epoch sec; seqNo:12656 2021-02-13T00:27:11Z- AHRS_M2 msg: 0.62 phi; 0.26 theta; -0.19 psi 2021-02-13T00:27:11Z- handleDepth()-Depth_Keller msg: 1613176031282 epoch sec; seqNo:12758; depth 4.86 2021-02-13T00:27:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176031.30 epoch sec; seqNo:12758; 4059198.11 north; 592573.09 east 2021-02-13T00:27:11Z- AHRS_M2 msg: 1613176031.68 epoch sec; seqNo:12657 2021-02-13T00:27:11Z- AHRS_M2 msg: 0.62 phi; 0.23 theta; -0.22 psi 2021-02-13T00:27:11Z- handleDepth()-Depth_Keller msg: 1613176031680 epoch sec; seqNo:12759; depth 4.75 2021-02-13T00:27:11Z- handleDvl() - RDI_Pathfinder msg: 1613176031.73 epoch sec; seqNo:6250 2021-02-13T00:27:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 109.81 , 115.76 , 0.00 2021-02-13T00:27:11Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.17 , -0.04 2021-02-13T00:27:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176031.74 epoch sec; seqNo:12759; 4059198.50 north; 592574.31 east 2021-02-13T00:27:12Z- AHRS_M2 msg: 1613176032.08 epoch sec; seqNo:12658 2021-02-13T00:27:12Z- AHRS_M2 msg: 0.63 phi; 0.19 theta; -0.24 psi 2021-02-13T00:27:12Z- handleDepth()-Depth_Keller msg: 1613176032088 epoch sec; seqNo:12760; depth 4.60 2021-02-13T00:27:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176032.10 epoch sec; seqNo:12760; 4059198.50 north; 592574.31 east 2021-02-13T00:27:12Z- AHRS_M2 msg: 1613176032.50 epoch sec; seqNo:12659 2021-02-13T00:27:12Z- AHRS_M2 msg: 0.67 phi; 0.10 theta; -0.23 psi 2021-02-13T00:27:12Z- handleDepth()-Depth_Keller msg: 1613176032502 epoch sec; seqNo:12761; depth 4.49 2021-02-13T00:27:12Z- handleDvl() - RDI_Pathfinder msg: 1613176032.51 epoch sec; seqNo:6251 2021-02-13T00:27:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 113.80 , 109.34 , 88.52 2021-02-13T00:27:12Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.05 , -0.13 2021-02-13T00:27:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176032.54 epoch sec; seqNo:12761; 4059198.88 north; 592574.30 east 2021-02-13T00:27:12Z- AHRS_M2 msg: 1613176032.89 epoch sec; seqNo:12660 2021-02-13T00:27:12Z- AHRS_M2 msg: 0.69 phi; 0.05 theta; -0.21 psi 2021-02-13T00:27:12Z- handleDepth()-Depth_Keller msg: 1613176032894 epoch sec; seqNo:12762; depth 4.41 2021-02-13T00:27:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176032.90 epoch sec; seqNo:12762; 4059199.26 north; 592574.30 east 2021-02-13T00:27:13Z- AHRS_M2 msg: 1613176033.29 epoch sec; seqNo:12661 2021-02-13T00:27:13Z- AHRS_M2 msg: 0.71 phi; -0.01 theta; -0.17 psi 2021-02-13T00:27:13Z- handleDepth()-Depth_Keller msg: 1613176033298 epoch sec; seqNo:12763; depth 4.34 2021-02-13T00:27:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176033.32 epoch sec; seqNo:12763; 4059199.64 north; 592574.30 east 2021-02-13T00:27:13Z- AHRS_M2 msg: 1613176033.70 epoch sec; seqNo:12662 2021-02-13T00:27:13Z- AHRS_M2 msg: 0.73 phi; -0.07 theta; -0.15 psi 2021-02-13T00:27:13Z- handleDepth()-Depth_Keller msg: 1613176033700 epoch sec; seqNo:12764; depth 4.27 2021-02-13T00:27:13Z- handleDvl() - RDI_Pathfinder msg: 1613176033.71 epoch sec; seqNo:6252 2021-02-13T00:27:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.88 , 109.68 , 99.27 , 97.78 2021-02-13T00:27:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.04 , -0.19 2021-02-13T00:27:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176033.73 epoch sec; seqNo:12764; 4059200.03 north; 592575.51 east 2021-02-13T00:27:14Z- AHRS_M2 msg: 1613176034.10 epoch sec; seqNo:12663 2021-02-13T00:27:14Z- AHRS_M2 msg: 0.73 phi; -0.12 theta; -0.15 psi 2021-02-13T00:27:14Z- handleDepth()-Depth_Keller msg: 1613176034110 epoch sec; seqNo:12765; depth 4.21 2021-02-13T00:27:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176034.12 epoch sec; seqNo:12765; 4059200.03 north; 592575.51 east 2021-02-13T00:27:14Z- AHRS_M2 msg: 1613176034.52 epoch sec; seqNo:12664 2021-02-13T00:27:14Z- AHRS_M2 msg: 0.73 phi; -0.15 theta; -0.18 psi 2021-02-13T00:27:14Z- handleDepth()-Depth_Keller msg: 1613176034541 epoch sec; seqNo:12766; depth 4.13 2021-02-13T00:27:14Z- handleDvl() - RDI_Pathfinder msg: 1613176034.55 epoch sec; seqNo:6253 2021-02-13T00:27:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.71 , 103.93 , 90.41 , 99.88 2021-02-13T00:27:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.15 , -0.18 2021-02-13T00:27:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176034.56 epoch sec; seqNo:12766; 4059200.41 north; 592575.51 east 2021-02-13T00:27:14Z- AHRS_M2 msg: 1613176034.91 epoch sec; seqNo:12665 2021-02-13T00:27:14Z- AHRS_M2 msg: 0.74 phi; -0.18 theta; -0.20 psi 2021-02-13T00:27:14Z- handleDepth()-Depth_Keller msg: 1613176034912 epoch sec; seqNo:12767; depth 4.10 2021-02-13T00:27:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176034.92 epoch sec; seqNo:12767; 4059200.79 north; 592575.50 east 2021-02-13T00:27:15Z- AHRS_M2 msg: 1613176035.31 epoch sec; seqNo:12666 2021-02-13T00:27:15Z- AHRS_M2 msg: 0.74 phi; -0.21 theta; -0.21 psi 2021-02-13T00:27:15Z- handleDepth()-Depth_Keller msg: 1613176035321 epoch sec; seqNo:12768; depth 4.08 2021-02-13T00:27:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176035.33 epoch sec; seqNo:12768; 4059200.79 north; 592575.50 east 2021-02-13T00:27:15Z- AHRS_M2 msg: 1613176035.72 epoch sec; seqNo:12667 2021-02-13T00:27:15Z- AHRS_M2 msg: 0.74 phi; -0.24 theta; -0.20 psi 2021-02-13T00:27:15Z- handleDepth()-Depth_Keller msg: 1613176035720 epoch sec; seqNo:12769; depth 4.08 2021-02-13T00:27:15Z- handleDvl() - RDI_Pathfinder msg: 1613176035.74 epoch sec; seqNo:6254 2021-02-13T00:27:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.98 , 112.32 , 0.00 , 108.26 2021-02-13T00:27:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.22 , 0.28 2021-02-13T00:27:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176035.75 epoch sec; seqNo:12769; 4059201.17 north; 592575.50 east 2021-02-13T00:27:16Z- AHRS_M2 msg: 1613176036.12 epoch sec; seqNo:12668 2021-02-13T00:27:16Z- AHRS_M2 msg: 0.74 phi; -0.26 theta; -0.20 psi 2021-02-13T00:27:16Z- handleDepth()-Depth_Keller msg: 1613176036127 epoch sec; seqNo:12770; depth 4.11 2021-02-13T00:27:16Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-13T00:27:16Z- waiting for 1613176033.92, time is 1613176036.12 2021-02-13T00:27:16Z- LcmTrn::updateTrn() >>>> pose time 1613176036.12, meas time 1613176035.74 <<<< 2021-02-13T00:27:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176035.74 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:27:16Z- TerrainNav::motionUpdate() - currEstimate = (4059201, 592575.50, 4.11) 2021-02-13T00:27:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:27:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:27:16Z- TNavBF::Projecting Measurements 2021-02-13T00:27:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:27:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:27:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:27:16Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:27:16Z- TNavFilter::3 good beams. 2021-02-13T00:27:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:27:16Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:27:16Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:27:16Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:27:16Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:27:16Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:27:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:27:16Z- TNavBF::mmse selected from filter -1 2021-02-13T00:27:16Z- TerrainMapOctree: (vehN, vehE ) = (4059200.92, 592575.32). 2021-02-13T00:27:16Z- TerrainMapOctree: Min Distance = 2646.37. 2021-02-13T00:27:16Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:27:16Z- There is no specific good beam that all particles have in common. 2021-02-13T00:27:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:27:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.52 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 12.38 2021-02-13T00:27:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:27:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 20.18 2021-02-13T00:27:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:27:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 18.75 2021-02-13T00:27:16Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:27:16Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:27:16Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:27:16Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:27:16Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:27:16Z- Filter 0 No beams used in subcloud update 2021-02-13T00:27:16Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:27:16Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:27:16Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 702.2 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:27:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176035.74 sec. 2021-02-13T00:27:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:27:16Z- TNavBF::mmse selected from filter -1 2021-02-13T00:27:16Z- variances([0], [2], and psi_berg) = 702.188684 832.999105 0.000000 2021-02-13T00:27:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:27:16Z- LcmTrn::publishEstimates() - MLE : 4059200.67 592545.00 4.11 2021-02-13T00:27:16Z- LcmTrn::publishEstimates() - MMSE : 4059201.17 592575.50 4.11 2021-02-13T00:27:16Z- LcmTrn::publishEstimates() - COVAR: 702.19 833.00 1.23 0.00 2021-02-13T00:27:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176036.15 epoch sec; seqNo:12770; 4059201.55 north; 592575.50 east 2021-02-13T00:27:16Z- AHRS_M2 msg: 1613176036.52 epoch sec; seqNo:12669 2021-02-13T00:27:16Z- AHRS_M2 msg: 0.72 phi; -0.28 theta; -0.20 psi 2021-02-13T00:27:16Z- handleDepth()-Depth_Keller msg: 1613176036528 epoch sec; seqNo:12771; depth 4.14 2021-02-13T00:27:16Z- handleDvl() - RDI_Pathfinder msg: 1613176036.54 epoch sec; seqNo:6255 2021-02-13T00:27:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 111.23 , 0.00 , 114.21 2021-02-13T00:27:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , 0.06 , 0.42 2021-02-13T00:27:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176036.55 epoch sec; seqNo:12771; 4059201.56 north; 592576.72 east 2021-02-13T00:27:16Z- AHRS_M2 msg: 1613176036.93 epoch sec; seqNo:12670 2021-02-13T00:27:16Z- AHRS_M2 msg: 0.71 phi; -0.30 theta; -0.21 psi 2021-02-13T00:27:16Z- handleDepth()-Depth_Keller msg: 1613176036941 epoch sec; seqNo:12772; depth 4.18 2021-02-13T00:27:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176036.95 epoch sec; seqNo:12772; 4059201.56 north; 592576.72 east 2021-02-13T00:27:17Z- AHRS_M2 msg: 1613176037.33 epoch sec; seqNo:12671 2021-02-13T00:27:17Z- AHRS_M2 msg: 0.69 phi; -0.31 theta; -0.23 psi 2021-02-13T00:27:17Z- handleDepth()-Depth_Keller msg: 1613176037337 epoch sec; seqNo:12773; depth 4.23 2021-02-13T00:27:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:27:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:27:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:27:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:27:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:27:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:27:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:27:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:27:17Z- handleDvl() - RDI_Pathfinder msg: 1613176037.34 epoch sec; seqNo:6256 2021-02-13T00:27:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:27:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , 0.06 , 0.42 2021-02-13T00:27:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176037.35 epoch sec; seqNo:12773; 4059201.94 north; 592576.71 east 2021-02-13T00:27:17Z- AHRS_M2 msg: 1613176037.74 epoch sec; seqNo:12672 2021-02-13T00:27:17Z- AHRS_M2 msg: 0.69 phi; -0.32 theta; -0.25 psi 2021-02-13T00:27:17Z- handleDepth()-Depth_Keller msg: 1613176037740 epoch sec; seqNo:12774; depth 4.28 2021-02-13T00:27:17Z- handleDvl() - RDI_Pathfinder msg: 1613176037.75 epoch sec; seqNo:6257 2021-02-13T00:27:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.01 , 113.80 , 85.55 , 125.70 2021-02-13T00:27:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.28 , 0.23 2021-02-13T00:27:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176037.77 epoch sec; seqNo:12774; 4059201.94 north; 592576.71 east 2021-02-13T00:27:18Z- AHRS_M2 msg: 1613176038.14 epoch sec; seqNo:12673 2021-02-13T00:27:18Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.27 psi 2021-02-13T00:27:18Z- handleDepth()-Depth_Keller msg: 1613176038149 epoch sec; seqNo:12775; depth 4.35 2021-02-13T00:27:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176038.16 epoch sec; seqNo:12775; 4059201.94 north; 592576.71 east 2021-02-13T00:27:18Z- AHRS_M2 msg: 1613176038.56 epoch sec; seqNo:12674 2021-02-13T00:27:18Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.27 psi 2021-02-13T00:27:18Z- handleDepth()-Depth_Keller msg: 1613176038560 epoch sec; seqNo:12776; depth 4.43 2021-02-13T00:27:18Z- handleDvl() - RDI_Pathfinder msg: 1613176038.57 epoch sec; seqNo:6258 2021-02-13T00:27:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 118.60 , 85.88 , 129.01 2021-02-13T00:27:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.31 , -0.01 2021-02-13T00:27:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176038.58 epoch sec; seqNo:12776; 4059202.33 north; 592577.93 east 2021-02-13T00:27:18Z- AHRS_M2 msg: 1613176038.95 epoch sec; seqNo:12675 2021-02-13T00:27:18Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.27 psi 2021-02-13T00:27:18Z- handleDepth()-Depth_Keller msg: 1613176038952 epoch sec; seqNo:12777; depth 4.52 2021-02-13T00:27:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176038.97 epoch sec; seqNo:12777; 4059202.71 north; 592577.92 east 2021-02-13T00:27:19Z- AHRS_M2 msg: 1613176039.35 epoch sec; seqNo:12676 2021-02-13T00:27:19Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.27 psi 2021-02-13T00:27:19Z- handleDepth()-Depth_Keller msg: 1613176039357 epoch sec; seqNo:12778; depth 4.61 2021-02-13T00:27:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176039.37 epoch sec; seqNo:12778; 4059202.71 north; 592577.92 east 2021-02-13T00:27:19Z- handleDepth()-Depth_Keller msg: 1613176039777 epoch sec; seqNo:12779; depth 4.71 2021-02-13T00:27:19Z- handleDvl() - RDI_Pathfinder msg: 1613176039.79 epoch sec; seqNo:6259 2021-02-13T00:27:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.01 , 121.24 , 85.55 , 130.16 2021-02-13T00:27:19Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.14 , -0.20 2021-02-13T00:27:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176039.81 epoch sec; seqNo:12779; 4059203.09 north; 592577.92 east 2021-02-13T00:27:20Z- AHRS_M2 msg: 1613176040.16 epoch sec; seqNo:12677 2021-02-13T00:27:20Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.25 psi 2021-02-13T00:27:20Z- handleDepth()-Depth_Keller msg: 1613176040168 epoch sec; seqNo:12780; depth 4.83 2021-02-13T00:27:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176040.18 epoch sec; seqNo:12780; 4059203.48 north; 592579.14 east 2021-02-13T00:27:20Z- AHRS_M2 msg: 1613176040.58 epoch sec; seqNo:12678 2021-02-13T00:27:20Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.25 psi 2021-02-13T00:27:20Z- handleDepth()-Depth_Keller msg: 1613176040584 epoch sec; seqNo:12781; depth 4.94 2021-02-13T00:27:20Z- handleDvl() - RDI_Pathfinder msg: 1613176040.60 epoch sec; seqNo:6260 2021-02-13T00:27:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.07 , 118.33 , 0.00 , 133.20 2021-02-13T00:27:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.11 , -0.40 2021-02-13T00:27:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176040.61 epoch sec; seqNo:12781; 4059203.86 north; 592579.13 east 2021-02-13T00:27:20Z- AHRS_M2 msg: 1613176040.97 epoch sec; seqNo:12679 2021-02-13T00:27:20Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.25 psi 2021-02-13T00:27:20Z- handleDepth()-Depth_Keller msg: 1613176040972 epoch sec; seqNo:12782; depth 5.06 2021-02-13T00:27:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:27:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:27:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:27:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:27:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:27:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:27:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:27:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:27:20Z- handleDvl() - RDI_Pathfinder msg: 1613176040.97 epoch sec; seqNo:6261 2021-02-13T00:27:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:27:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.11 , -0.40 2021-02-13T00:27:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176040.99 epoch sec; seqNo:12782; 4059204.24 north; 592579.13 east 2021-02-13T00:27:21Z- LcmTrn::handleMessages() - handling 43 messages took 0.61 ms and 131 calls... 2021-02-13T00:27:21Z- waiting for 1613176039.12, time is 1613176040.99 2021-02-13T00:27:21Z- LcmTrn::updateTrn() equating pose 1613176040.99 and meas times 1613176040.99 2021-02-13T00:27:21Z- LcmTrn::updateTrn() >>>> pose time 1613176040.99, meas time 1613176040.99 <<<< 2021-02-13T00:27:21Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613176040.99, ping # 0. 2021-02-13T00:27:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176040.99 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:27:21Z- TerrainNav::motionUpdate() - currEstimate = (4059204, 592579.13, 5.06) 2021-02-13T00:27:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:27:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:27:21Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:27:21Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:27:21Z- TNavBF::Projecting Measurements 2021-02-13T00:27:21Z- TNavFilter::Measurement from time = 1613176040.99 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:27:21Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:27:21Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:27:21Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:27:21Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:27:21Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:27:21Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:27:21Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:27:21Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 702.1 EastVariance: 833.0 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-13T00:27:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:27:21Z- TNavBF::mmse selected from filter -1 2021-02-13T00:27:21Z- variances([0], [2], and psi_berg) = 702.128277 832.999073 0.000000 2021-02-13T00:27:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:27:21Z- LcmTrn::publishEstimates() - MLE : 4059203.74 592546.63 5.06 2021-02-13T00:27:21Z- LcmTrn::publishEstimates() - MMSE : 4059204.24 592579.13 5.06 2021-02-13T00:27:21Z- LcmTrn::publishEstimates() - COVAR: 702.13 833.00 1.47 0.00 2021-02-13T00:27:21Z- AHRS_M2 msg: 1613176041.37 epoch sec; seqNo:12680 2021-02-13T00:27:21Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.26 psi 2021-02-13T00:27:21Z- handleDepth()-Depth_Keller msg: 1613176041376 epoch sec; seqNo:12783; depth 5.18 2021-02-13T00:27:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176041.38 epoch sec; seqNo:12783; 4059204.62 north; 592579.13 east 2021-02-13T00:27:21Z- AHRS_M2 msg: 1613176041.78 epoch sec; seqNo:12681 2021-02-13T00:27:21Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.27 psi 2021-02-13T00:27:21Z- handleDepth()-Depth_Keller msg: 1613176041789 epoch sec; seqNo:12784; depth 5.29 2021-02-13T00:27:21Z- handleDvl() - RDI_Pathfinder msg: 1613176041.80 epoch sec; seqNo:6262 2021-02-13T00:27:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.80 , 115.09 , 0.00 , 128.47 2021-02-13T00:27:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.41 , -0.44 2021-02-13T00:27:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176041.81 epoch sec; seqNo:12784; 4059205.00 north; 592579.12 east 2021-02-13T00:27:22Z- AHRS_M2 msg: 1613176042.18 epoch sec; seqNo:12682 2021-02-13T00:27:22Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.28 psi 2021-02-13T00:27:22Z- handleDepth()-Depth_Keller msg: 1613176042194 epoch sec; seqNo:12785; depth 5.42 2021-02-13T00:27:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:27:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:27:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:27:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:27:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:27:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:27:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:27:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:27:22Z- handleDvl() - RDI_Pathfinder msg: 1613176042.20 epoch sec; seqNo:6263 2021-02-13T00:27:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:27:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.41 , -0.44 2021-02-13T00:27:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176042.21 epoch sec; seqNo:12785; 4059205.38 north; 592579.12 east 2021-02-13T00:27:22Z- AHRS_M2 msg: 1613176042.59 epoch sec; seqNo:12683 2021-02-13T00:27:22Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.27 psi 2021-02-13T00:27:22Z- handleDepth()-Depth_Keller msg: 1613176042597 epoch sec; seqNo:12786; depth 5.53 2021-02-13T00:27:22Z- handleDvl() - RDI_Pathfinder msg: 1613176042.62 epoch sec; seqNo:6264 2021-02-13T00:27:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.32 , 122.52 , 0.00 , 126.98 2021-02-13T00:27:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.62 , 0.01 2021-02-13T00:27:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176042.63 epoch sec; seqNo:12786; 4059205.76 north; 592579.11 east 2021-02-13T00:27:22Z- AHRS_M2 msg: 1613176042.99 epoch sec; seqNo:12684 2021-02-13T00:27:22Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.23 psi 2021-02-13T00:27:22Z- handleDepth()-Depth_Keller msg: 1613176042994 epoch sec; seqNo:12787; depth 5.67 2021-02-13T00:27:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176043.01 epoch sec; seqNo:12787; 4059206.13 north; 592579.11 east 2021-02-13T00:27:23Z- AHRS_M2 msg: 1613176043.39 epoch sec; seqNo:12685 2021-02-13T00:27:23Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.19 psi 2021-02-13T00:27:23Z- handleDepth()-Depth_Keller msg: 1613176043396 epoch sec; seqNo:12788; depth 5.79 2021-02-13T00:27:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:27:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:27:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:27:23Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:27:23Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:27:23Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:27:23Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:27:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:27:23Z- handleDvl() - RDI_Pathfinder msg: 1613176043.40 epoch sec; seqNo:6265 2021-02-13T00:27:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:27:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.62 , 0.01 2021-02-13T00:27:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176043.42 epoch sec; seqNo:12788; 4059206.51 north; 592579.10 east 2021-02-13T00:27:23Z- AHRS_M2 msg: 1613176043.80 epoch sec; seqNo:12686 2021-02-13T00:27:23Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.15 psi 2021-02-13T00:27:23Z- handleDepth()-Depth_Keller msg: 1613176043800 epoch sec; seqNo:12789; depth 5.93 2021-02-13T00:27:23Z- handleDvl() - RDI_Pathfinder msg: 1613176043.81 epoch sec; seqNo:6266 2021-02-13T00:27:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.32 , 113.60 , 83.86 , 125.50 2021-02-13T00:27:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.54 , -0.31 , 0.52 2021-02-13T00:27:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176043.83 epoch sec; seqNo:12789; 4059206.51 north; 592579.10 east 2021-02-13T00:27:24Z- AHRS_M2 msg: 1613176044.20 epoch sec; seqNo:12687 2021-02-13T00:27:24Z- AHRS_M2 msg: 0.66 phi; -0.38 theta; -0.12 psi 2021-02-13T00:27:24Z- handleDepth()-Depth_Keller msg: 1613176044204 epoch sec; seqNo:12790; depth 6.04 2021-02-13T00:27:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176044.21 epoch sec; seqNo:12790; 4059206.51 north; 592579.10 east 2021-02-13T00:27:24Z- AHRS_M2 msg: 1613176044.63 epoch sec; seqNo:12688 2021-02-13T00:27:24Z- AHRS_M2 msg: 0.65 phi; -0.39 theta; -0.10 psi 2021-02-13T00:27:24Z- handleDepth()-Depth_Keller msg: 1613176044638 epoch sec; seqNo:12791; depth 6.15 2021-02-13T00:27:24Z- handleDvl() - RDI_Pathfinder msg: 1613176044.65 epoch sec; seqNo:6267 2021-02-13T00:27:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 108.26 , 82.98 , 129.08 2021-02-13T00:27:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.56 , 0.23 , 0.47 2021-02-13T00:27:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176044.67 epoch sec; seqNo:12791; 4059206.89 north; 592579.10 east 2021-02-13T00:27:25Z- AHRS_M2 msg: 1613176045.01 epoch sec; seqNo:12689 2021-02-13T00:27:25Z- AHRS_M2 msg: 0.63 phi; -0.39 theta; -0.11 psi 2021-02-13T00:27:25Z- handleDepth()-Depth_Keller msg: 1613176045012 epoch sec; seqNo:12792; depth 6.28 2021-02-13T00:27:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176045.03 epoch sec; seqNo:12792; 4059206.89 north; 592579.10 east 2021-02-13T00:27:25Z- AHRS_M2 msg: 1613176045.41 epoch sec; seqNo:12690 2021-02-13T00:27:25Z- AHRS_M2 msg: 0.61 phi; -0.38 theta; -0.15 psi 2021-02-13T00:27:25Z- handleDepth()-Depth_Keller msg: 1613176045416 epoch sec; seqNo:12793; depth 6.37 2021-02-13T00:27:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176045.42 epoch sec; seqNo:12793; 4059206.90 north; 592580.32 east 2021-02-13T00:27:25Z- AHRS_M2 msg: 1613176045.82 epoch sec; seqNo:12691 2021-02-13T00:27:25Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.19 psi 2021-02-13T00:27:25Z- handleDepth()-Depth_Keller msg: 1613176045820 epoch sec; seqNo:12794; depth 6.47 2021-02-13T00:27:25Z- handleDvl() - RDI_Pathfinder msg: 1613176045.83 epoch sec; seqNo:6268 2021-02-13T00:27:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 106.64 , 81.36 , 127.46 2021-02-13T00:27:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.37 , 0.07 2021-02-13T00:27:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176045.84 epoch sec; seqNo:12794; 4059206.90 north; 592580.32 east 2021-02-13T00:27:26Z- AHRS_M2 msg: 1613176046.22 epoch sec; seqNo:12692 2021-02-13T00:27:26Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.23 psi 2021-02-13T00:27:26Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-13T00:27:26Z- waiting for 1613176043.99, time is 1613176046.22 2021-02-13T00:27:26Z- LcmTrn::updateTrn() >>>> pose time 1613176046.22, meas time 1613176045.83 <<<< 2021-02-13T00:27:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176045.83 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:27:26Z- TerrainNav::motionUpdate() - currEstimate = (4059207, 592580.32, 6.47) 2021-02-13T00:27:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:27:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:27:26Z- TNavBF::Projecting Measurements 2021-02-13T00:27:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:27:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:27:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:27:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:27:26Z- TNavFilter::4 good beams. 2021-02-13T00:27:26Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:27:26Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:27:26Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:27:26Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:27:26Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:27:26Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:27:26Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:27:26Z- TNavBF::mmse selected from filter -1 2021-02-13T00:27:26Z- TerrainMapOctree: (vehN, vehE ) = (4059206.71, 592580.23). 2021-02-13T00:27:26Z- TerrainMapOctree: Min Distance = 2638.78. 2021-02-13T00:27:26Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:27:26Z- There is no specific good beam that all particles have in common. 2021-02-13T00:27:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:27:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.61 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:27:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:27:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.47 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 18.19 2021-02-13T00:27:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:27:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 10.59 2021-02-13T00:27:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:27:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 25.99 2021-02-13T00:27:26Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:27:26Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:27:26Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:27:26Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:27:26Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:27:26Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:27:26Z- Filter 0 No beams used in subcloud update 2021-02-13T00:27:26Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:27:26Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:27:26Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 702.1 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:27:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176045.83 sec. 2021-02-13T00:27:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:27:26Z- TNavBF::mmse selected from filter -1 2021-02-13T00:27:26Z- variances([0], [2], and psi_berg) = 702.069880 832.999041 0.000000 2021-02-13T00:27:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:27:26Z- LcmTrn::publishEstimates() - MLE : 4059206.40 592626.82 6.47 2021-02-13T00:27:26Z- LcmTrn::publishEstimates() - MMSE : 4059206.90 592580.32 6.47 2021-02-13T00:27:26Z- LcmTrn::publishEstimates() - COVAR: 702.07 833.00 1.19 0.00 2021-02-13T00:27:26Z- handleDepth()-Depth_Keller msg: 1613176046227 epoch sec; seqNo:12795; depth 6.58 2021-02-13T00:27:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176046.24 epoch sec; seqNo:12795; 4059207.28 north; 592580.32 east 2021-02-13T00:27:26Z- AHRS_M2 msg: 1613176046.65 epoch sec; seqNo:12693 2021-02-13T00:27:26Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.26 psi 2021-02-13T00:27:26Z- handleDepth()-Depth_Keller msg: 1613176046657 epoch sec; seqNo:12796; depth 6.71 2021-02-13T00:27:26Z- handleDvl() - RDI_Pathfinder msg: 1613176046.67 epoch sec; seqNo:6269 2021-02-13T00:27:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.41 , 117.18 , 82.98 , 133.54 2021-02-13T00:27:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.12 , -0.13 2021-02-13T00:27:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176046.68 epoch sec; seqNo:12796; 4059207.66 north; 592580.31 east 2021-02-13T00:27:27Z- AHRS_M2 msg: 1613176047.03 epoch sec; seqNo:12694 2021-02-13T00:27:27Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.27 psi 2021-02-13T00:27:27Z- handleDepth()-Depth_Keller msg: 1613176047037 epoch sec; seqNo:12797; depth 6.85 2021-02-13T00:27:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176047.05 epoch sec; seqNo:12797; 4059207.66 north; 592580.31 east 2021-02-13T00:27:27Z- AHRS_M2 msg: 1613176047.43 epoch sec; seqNo:12695 2021-02-13T00:27:27Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.26 psi 2021-02-13T00:27:27Z- handleDepth()-Depth_Keller msg: 1613176047436 epoch sec; seqNo:12798; depth 6.98 2021-02-13T00:27:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176047.44 epoch sec; seqNo:12798; 4059208.05 north; 592581.53 east 2021-02-13T00:27:27Z- AHRS_M2 msg: 1613176047.84 epoch sec; seqNo:12696 2021-02-13T00:27:27Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.24 psi 2021-02-13T00:27:27Z- handleDepth()-Depth_Keller msg: 1613176047850 epoch sec; seqNo:12799; depth 7.14 2021-02-13T00:27:27Z- handleDvl() - RDI_Pathfinder msg: 1613176047.86 epoch sec; seqNo:6270 2021-02-13T00:27:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.65 , 121.37 , 0.00 , 130.30 2021-02-13T00:27:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , -0.04 2021-02-13T00:27:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176047.88 epoch sec; seqNo:12799; 4059208.43 north; 592581.53 east 2021-02-13T00:27:28Z- AHRS_M2 msg: 1613176048.24 epoch sec; seqNo:12697 2021-02-13T00:27:28Z- AHRS_M2 msg: 0.60 phi; -0.38 theta; -0.22 psi 2021-02-13T00:27:28Z- handleDepth()-Depth_Keller msg: 1613176048249 epoch sec; seqNo:12800; depth 7.27 2021-02-13T00:27:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176048.26 epoch sec; seqNo:12800; 4059208.81 north; 592581.52 east 2021-02-13T00:27:28Z- AHRS_M2 msg: 1613176048.65 epoch sec; seqNo:12698 2021-02-13T00:27:28Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.20 psi 2021-02-13T00:27:28Z- handleDepth()-Depth_Keller msg: 1613176048655 epoch sec; seqNo:12801; depth 7.41 2021-02-13T00:27:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176048.68 epoch sec; seqNo:12801; 4059209.19 north; 592581.52 east 2021-02-13T00:27:29Z- AHRS_M2 msg: 1613176049.05 epoch sec; seqNo:12699 2021-02-13T00:27:29Z- AHRS_M2 msg: 0.59 phi; -0.37 theta; -0.18 psi 2021-02-13T00:27:29Z- handleDepth()-Depth_Keller msg: 1613176049055 epoch sec; seqNo:12802; depth 7.57 2021-02-13T00:27:29Z- handleDvl() - RDI_Pathfinder msg: 1613176049.07 epoch sec; seqNo:6271 2021-02-13T00:27:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 112.11 , 82.37 , 131.45 2021-02-13T00:27:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , -0.00 2021-02-13T00:27:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176049.08 epoch sec; seqNo:12802; 4059209.19 north; 592581.52 east 2021-02-13T00:27:29Z- AHRS_M2 msg: 1613176049.46 epoch sec; seqNo:12700 2021-02-13T00:27:29Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.17 psi 2021-02-13T00:27:29Z- handleDepth()-Depth_Keller msg: 1613176049467 epoch sec; seqNo:12803; depth 7.70 2021-02-13T00:27:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176049.48 epoch sec; seqNo:12803; 4059209.57 north; 592581.51 east 2021-02-13T00:27:29Z- AHRS_M2 msg: 1613176049.86 epoch sec; seqNo:12701 2021-02-13T00:27:29Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.17 psi 2021-02-13T00:27:29Z- handleDepth()-Depth_Keller msg: 1613176049865 epoch sec; seqNo:12804; depth 7.84 2021-02-13T00:27:29Z- handleDvl() - RDI_Pathfinder msg: 1613176049.88 epoch sec; seqNo:6272 2021-02-13T00:27:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.14 , 107.99 , 79.69 , 125.83 2021-02-13T00:27:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.02 2021-02-13T00:27:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176049.89 epoch sec; seqNo:12804; 4059209.96 north; 592582.73 east 2021-02-13T00:27:30Z- AHRS_M2 msg: 1613176050.26 epoch sec; seqNo:12702 2021-02-13T00:27:30Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.17 psi 2021-02-13T00:27:30Z- handleDepth()-Depth_Keller msg: 1613176050264 epoch sec; seqNo:12805; depth 8.00 2021-02-13T00:27:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176050.28 epoch sec; seqNo:12805; 4059210.34 north; 592582.73 east 2021-02-13T00:27:30Z- AHRS_M2 msg: 1613176050.67 epoch sec; seqNo:12703 2021-02-13T00:27:30Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.17 psi 2021-02-13T00:27:30Z- handleDepth()-Depth_Keller msg: 1613176050681 epoch sec; seqNo:12806; depth 8.12 2021-02-13T00:27:30Z- handleDvl() - RDI_Pathfinder msg: 1613176050.69 epoch sec; seqNo:6273 2021-02-13T00:27:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 106.43 , 78.87 , 124.01 2021-02-13T00:27:30Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.01 , 0.04 2021-02-13T00:27:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176050.70 epoch sec; seqNo:12806; 4059210.34 north; 592582.73 east 2021-02-13T00:27:31Z- AHRS_M2 msg: 1613176051.07 epoch sec; seqNo:12704 2021-02-13T00:27:31Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.17 psi 2021-02-13T00:27:31Z- handleDepth()-Depth_Keller msg: 1613176051082 epoch sec; seqNo:12807; depth 8.29 2021-02-13T00:27:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176051.10 epoch sec; seqNo:12807; 4059210.72 north; 592582.72 east 2021-02-13T00:27:31Z- LcmTrn::handleMessages() - handling 43 messages took 0.69 ms and 129 calls... 2021-02-13T00:27:31Z- waiting for 1613176049.22, time is 1613176051.10 2021-02-13T00:27:31Z- LcmTrn::updateTrn() >>>> pose time 1613176051.10, meas time 1613176050.69 <<<< 2021-02-13T00:27:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176050.69 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:27:31Z- TerrainNav::motionUpdate() - currEstimate = (4059211, 592582.72, 8.29) 2021-02-13T00:27:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:27:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:27:31Z- TNavBF::Projecting Measurements 2021-02-13T00:27:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:27:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:27:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:27:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:27:31Z- TNavFilter::4 good beams. 2021-02-13T00:27:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:27:31Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:27:31Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:27:31Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:27:31Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:27:31Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:27:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:27:31Z- TNavBF::mmse selected from filter -1 2021-02-13T00:27:31Z- TerrainMapOctree: (vehN, vehE ) = (4059210.40, 592582.52). 2021-02-13T00:27:31Z- TerrainMapOctree: Min Distance = 2634.47. 2021-02-13T00:27:31Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:27:31Z- There is no specific good beam that all particles have in common. 2021-02-13T00:27:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:27:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 15.05 2021-02-13T00:27:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:27:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 18.13 2021-02-13T00:27:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:27:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 9.95 2021-02-13T00:27:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:27:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 24.61 2021-02-13T00:27:31Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:27:31Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:27:31Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:27:31Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:27:31Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:27:31Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:27:31Z- Filter 0 No beams used in subcloud update 2021-02-13T00:27:31Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:27:31Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:27:31Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 702.0 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:27:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176050.69 sec. 2021-02-13T00:27:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:27:31Z- TNavBF::mmse selected from filter -1 2021-02-13T00:27:31Z- variances([0], [2], and psi_berg) = 701.996697 832.999002 0.000000 2021-02-13T00:27:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:27:31Z- LcmTrn::publishEstimates() - MLE : 4059210.22 592629.22 8.29 2021-02-13T00:27:31Z- LcmTrn::publishEstimates() - MMSE : 4059210.72 592582.72 8.29 2021-02-13T00:27:31Z- LcmTrn::publishEstimates() - COVAR: 702.00 833.00 1.07 0.00 2021-02-13T00:27:31Z- AHRS_M2 msg: 1613176051.48 epoch sec; seqNo:12705 2021-02-13T00:27:31Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.16 psi 2021-02-13T00:27:31Z- handleDepth()-Depth_Keller msg: 1613176051480 epoch sec; seqNo:12808; depth 8.42 2021-02-13T00:27:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176051.49 epoch sec; seqNo:12808; 4059211.10 north; 592582.72 east 2021-02-13T00:27:31Z- AHRS_M2 msg: 1613176051.88 epoch sec; seqNo:12706 2021-02-13T00:27:31Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.16 psi 2021-02-13T00:27:31Z- handleDepth()-Depth_Keller msg: 1613176051885 epoch sec; seqNo:12809; depth 8.56 2021-02-13T00:27:31Z- handleDvl() - RDI_Pathfinder msg: 1613176051.90 epoch sec; seqNo:6274 2021-02-13T00:27:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 105.42 , 81.62 , 120.29 2021-02-13T00:27:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.07 , -0.02 2021-02-13T00:27:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176051.91 epoch sec; seqNo:12809; 4059211.48 north; 592582.71 east 2021-02-13T00:27:32Z- AHRS_M2 msg: 1613176052.29 epoch sec; seqNo:12707 2021-02-13T00:27:32Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.16 psi 2021-02-13T00:27:32Z- handleDepth()-Depth_Keller msg: 1613176052296 epoch sec; seqNo:12810; depth 8.70 2021-02-13T00:27:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176052.31 epoch sec; seqNo:12810; 4059211.87 north; 592583.93 east 2021-02-13T00:27:32Z- AHRS_M2 msg: 1613176052.71 epoch sec; seqNo:12708 2021-02-13T00:27:32Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.16 psi 2021-02-13T00:27:32Z- handleDepth()-Depth_Keller msg: 1613176052713 epoch sec; seqNo:12811; depth 8.84 2021-02-13T00:27:32Z- handleDvl() - RDI_Pathfinder msg: 1613176052.72 epoch sec; seqNo:6275 2021-02-13T00:27:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 103.86 , 80.88 , 114.68 2021-02-13T00:27:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.10 , -0.15 2021-02-13T00:27:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176052.73 epoch sec; seqNo:12811; 4059211.87 north; 592583.93 east 2021-02-13T00:27:33Z- AHRS_M2 msg: 1613176053.09 epoch sec; seqNo:12709 2021-02-13T00:27:33Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.16 psi 2021-02-13T00:27:33Z- handleDepth()-Depth_Keller msg: 1613176053096 epoch sec; seqNo:12812; depth 8.97 2021-02-13T00:27:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176053.11 epoch sec; seqNo:12812; 4059212.25 north; 592583.93 east 2021-02-13T00:27:33Z- AHRS_M2 msg: 1613176053.50 epoch sec; seqNo:12710 2021-02-13T00:27:33Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.16 psi 2021-02-13T00:27:33Z- handleDepth()-Depth_Keller msg: 1613176053505 epoch sec; seqNo:12813; depth 9.08 2021-02-13T00:27:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176053.51 epoch sec; seqNo:12813; 4059212.63 north; 592583.92 east 2021-02-13T00:27:33Z- AHRS_M2 msg: 1613176053.90 epoch sec; seqNo:12711 2021-02-13T00:27:33Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.17 psi 2021-02-13T00:27:33Z- handleDepth()-Depth_Keller msg: 1613176053913 epoch sec; seqNo:12814; depth 9.19 2021-02-13T00:27:33Z- handleDvl() - RDI_Pathfinder msg: 1613176053.92 epoch sec; seqNo:6276 2021-02-13T00:27:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 102.72 , 81.08 , 113.53 2021-02-13T00:27:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , -0.21 2021-02-13T00:27:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176053.93 epoch sec; seqNo:12814; 4059213.01 north; 592583.92 east 2021-02-13T00:27:34Z- AHRS_M2 msg: 1613176054.31 epoch sec; seqNo:12712 2021-02-13T00:27:34Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.17 psi 2021-02-13T00:27:34Z- handleDepth()-Depth_Keller msg: 1613176054332 epoch sec; seqNo:12815; depth 9.29 2021-02-13T00:27:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176054.36 epoch sec; seqNo:12815; 4059213.40 north; 592585.13 east 2021-02-13T00:27:34Z- AHRS_M2 msg: 1613176054.71 epoch sec; seqNo:12713 2021-02-13T00:27:34Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.18 psi 2021-02-13T00:27:34Z- handleDepth()-Depth_Keller msg: 1613176054712 epoch sec; seqNo:12816; depth 9.41 2021-02-13T00:27:34Z- handleDvl() - RDI_Pathfinder msg: 1613176054.72 epoch sec; seqNo:6277 2021-02-13T00:27:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:27:34Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:27:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176054.75 epoch sec; seqNo:12816; 4059213.40 north; 592585.13 east 2021-02-13T00:27:35Z- AHRS_M2 msg: 1613176055.11 epoch sec; seqNo:12714 2021-02-13T00:27:35Z- AHRS_M2 msg: 0.58 phi; -0.27 theta; -0.19 psi 2021-02-13T00:27:35Z- handleDepth()-Depth_Keller msg: 1613176055121 epoch sec; seqNo:12817; depth 9.50 2021-02-13T00:27:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176055.13 epoch sec; seqNo:12817; 4059213.78 north; 592585.13 east 2021-02-13T00:27:35Z- AHRS_M2 msg: 1613176055.52 epoch sec; seqNo:12715 2021-02-13T00:27:35Z- AHRS_M2 msg: 0.58 phi; -0.27 theta; -0.19 psi 2021-02-13T00:27:35Z- handleDepth()-Depth_Keller msg: 1613176055520 epoch sec; seqNo:12818; depth 9.60 2021-02-13T00:27:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176055.54 epoch sec; seqNo:12818; 4059214.16 north; 592585.13 east 2021-02-13T00:27:35Z- AHRS_M2 msg: 1613176055.92 epoch sec; seqNo:12716 2021-02-13T00:27:35Z- AHRS_M2 msg: 0.57 phi; -0.26 theta; -0.19 psi 2021-02-13T00:27:35Z- handleDepth()-Depth_Keller msg: 1613176055924 epoch sec; seqNo:12819; depth 9.71 2021-02-13T00:27:35Z- handleDvl() - RDI_Pathfinder msg: 1613176055.94 epoch sec; seqNo:6278 2021-02-13T00:27:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.21 , 105.49 , 0.00 , 112.25 2021-02-13T00:27:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.19 , -0.01 2021-02-13T00:27:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176055.96 epoch sec; seqNo:12819; 4059214.53 north; 592585.12 east 2021-02-13T00:27:36Z- AHRS_M2 msg: 1613176056.32 epoch sec; seqNo:12717 2021-02-13T00:27:36Z- AHRS_M2 msg: 0.57 phi; -0.26 theta; -0.19 psi 2021-02-13T00:27:36Z- handleDepth()-Depth_Keller msg: 1613176056328 epoch sec; seqNo:12820; depth 9.80 2021-02-13T00:27:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176056.34 epoch sec; seqNo:12820; 4059214.91 north; 592585.12 east 2021-02-13T00:27:36Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-13T00:27:36Z- waiting for 1613176054.10, time is 1613176056.34 2021-02-13T00:27:36Z- LcmTrn::updateTrn() >>>> pose time 1613176056.34, meas time 1613176055.94 <<<< 2021-02-13T00:27:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176055.94 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:27:36Z- TerrainNav::motionUpdate() - currEstimate = (4059215, 592585.12, 9.80) 2021-02-13T00:27:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:27:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:27:36Z- TNavBF::Projecting Measurements 2021-02-13T00:27:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:27:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:27:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:27:36Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:27:36Z- TNavFilter::3 good beams. 2021-02-13T00:27:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:27:36Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:27:36Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:27:36Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:27:36Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:27:36Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:27:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:27:36Z- TNavBF::mmse selected from filter -1 2021-02-13T00:27:36Z- TerrainMapOctree: (vehN, vehE ) = (4059214.59, 592584.93). 2021-02-13T00:27:36Z- TerrainMapOctree: Min Distance = 2629.70. 2021-02-13T00:27:36Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:27:36Z- There is no specific good beam that all particles have in common. 2021-02-13T00:27:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:27:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 11.62 2021-02-13T00:27:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:27:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 17.80 2021-02-13T00:27:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:27:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 20.16 2021-02-13T00:27:36Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:27:36Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:27:36Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:27:36Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:27:36Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:27:36Z- Filter 0 No beams used in subcloud update 2021-02-13T00:27:36Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:27:36Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:27:36Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 701.9 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:27:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176055.94 sec. 2021-02-13T00:27:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:27:36Z- TNavBF::mmse selected from filter -1 2021-02-13T00:27:36Z- variances([0], [2], and psi_berg) = 701.921477 832.998962 0.000000 2021-02-13T00:27:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:27:36Z- LcmTrn::publishEstimates() - MLE : 4059214.41 592631.62 9.80 2021-02-13T00:27:36Z- LcmTrn::publishEstimates() - MMSE : 4059214.91 592585.12 9.80 2021-02-13T00:27:36Z- LcmTrn::publishEstimates() - COVAR: 701.92 833.00 1.08 0.00 2021-02-13T00:27:36Z- AHRS_M2 msg: 1613176056.76 epoch sec; seqNo:12718 2021-02-13T00:27:36Z- AHRS_M2 msg: 0.57 phi; -0.26 theta; -0.19 psi 2021-02-13T00:27:36Z- handleDepth()-Depth_Keller msg: 1613176056760 epoch sec; seqNo:12821; depth 9.91 2021-02-13T00:27:36Z- handleDvl() - RDI_Pathfinder msg: 1613176056.77 epoch sec; seqNo:6279 2021-02-13T00:27:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.80 , 103.73 , 79.39 , 109.14 2021-02-13T00:27:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.17 , 0.08 2021-02-13T00:27:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176056.80 epoch sec; seqNo:12821; 4059215.29 north; 592585.11 east 2021-02-13T00:27:37Z- AHRS_M2 msg: 1613176057.13 epoch sec; seqNo:12719 2021-02-13T00:27:37Z- AHRS_M2 msg: 0.57 phi; -0.26 theta; -0.19 psi 2021-02-13T00:27:37Z- handleDepth()-Depth_Keller msg: 1613176057145 epoch sec; seqNo:12822; depth 10.02 2021-02-13T00:27:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176057.16 epoch sec; seqNo:12822; 4059215.29 north; 592585.11 east 2021-02-13T00:27:37Z- AHRS_M2 msg: 1613176057.54 epoch sec; seqNo:12720 2021-02-13T00:27:37Z- AHRS_M2 msg: 0.57 phi; -0.26 theta; -0.19 psi 2021-02-13T00:27:37Z- handleDepth()-Depth_Keller msg: 1613176057570 epoch sec; seqNo:12823; depth 10.10 2021-02-13T00:27:37Z- handleDvl() - RDI_Pathfinder msg: 1613176057.59 epoch sec; seqNo:6280 2021-02-13T00:27:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.27 , 79.93 , 109.68 2021-02-13T00:27:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.11 , 0.07 2021-02-13T00:27:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176057.60 epoch sec; seqNo:12823; 4059215.68 north; 592586.33 east 2021-02-13T00:27:37Z- AHRS_M2 msg: 1613176057.94 epoch sec; seqNo:12721 2021-02-13T00:27:37Z- AHRS_M2 msg: 0.57 phi; -0.26 theta; -0.19 psi 2021-02-13T00:27:37Z- handleDepth()-Depth_Keller msg: 1613176057947 epoch sec; seqNo:12824; depth 10.20 2021-02-13T00:27:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176057.96 epoch sec; seqNo:12824; 4059216.06 north; 592586.33 east 2021-02-13T00:27:38Z- AHRS_M2 msg: 1613176058.35 epoch sec; seqNo:12722 2021-02-13T00:27:38Z- AHRS_M2 msg: 0.57 phi; -0.26 theta; -0.19 psi 2021-02-13T00:27:38Z- handleDepth()-Depth_Keller msg: 1613176058357 epoch sec; seqNo:12825; depth 10.29 2021-02-13T00:27:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176058.37 epoch sec; seqNo:12825; 4059216.44 north; 592586.32 east 2021-02-13T00:27:38Z- AHRS_M2 msg: 1613176058.75 epoch sec; seqNo:12723 2021-02-13T00:27:38Z- AHRS_M2 msg: 0.57 phi; -0.27 theta; -0.18 psi 2021-02-13T00:27:38Z- handleDepth()-Depth_Keller msg: 1613176058762 epoch sec; seqNo:12826; depth 10.39 2021-02-13T00:27:38Z- handleDvl() - RDI_Pathfinder msg: 1613176058.77 epoch sec; seqNo:6281 2021-02-13T00:27:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 102.58 , 80.95 , 107.99 2021-02-13T00:27:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.08 , 0.01 2021-02-13T00:27:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176058.78 epoch sec; seqNo:12826; 4059216.44 north; 592586.32 east 2021-02-13T00:27:39Z- AHRS_M2 msg: 1613176059.15 epoch sec; seqNo:12724 2021-02-13T00:27:39Z- AHRS_M2 msg: 0.58 phi; -0.27 theta; -0.18 psi 2021-02-13T00:27:39Z- handleDepth()-Depth_Keller msg: 1613176059165 epoch sec; seqNo:12827; depth 10.50 2021-02-13T00:27:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176059.18 epoch sec; seqNo:12827; 4059216.82 north; 592586.32 east 2021-02-13T00:27:39Z- AHRS_M2 msg: 1613176059.56 epoch sec; seqNo:12725 2021-02-13T00:27:39Z- AHRS_M2 msg: 0.57 phi; -0.27 theta; -0.18 psi 2021-02-13T00:27:39Z- handleDepth()-Depth_Keller msg: 1613176059565 epoch sec; seqNo:12828; depth 10.57 2021-02-13T00:27:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176059.57 epoch sec; seqNo:12828; 4059217.20 north; 592586.31 east 2021-02-13T00:27:39Z- AHRS_M2 msg: 1613176059.96 epoch sec; seqNo:12726 2021-02-13T00:27:39Z- AHRS_M2 msg: 0.57 phi; -0.28 theta; -0.18 psi 2021-02-13T00:27:39Z- handleDepth()-Depth_Keller msg: 1613176059965 epoch sec; seqNo:12829; depth 10.66 2021-02-13T00:27:39Z- handleDvl() - RDI_Pathfinder msg: 1613176059.98 epoch sec; seqNo:6282 2021-02-13T00:27:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 101.57 , 79.93 , 106.97 2021-02-13T00:27:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , 0.04 2021-02-13T00:27:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176059.99 epoch sec; seqNo:12829; 4059217.20 north; 592586.31 east 2021-02-13T00:27:40Z- AHRS_M2 msg: 1613176060.36 epoch sec; seqNo:12727 2021-02-13T00:27:40Z- AHRS_M2 msg: 0.57 phi; -0.28 theta; -0.18 psi 2021-02-13T00:27:40Z- handleDepth()-Depth_Keller msg: 1613176060373 epoch sec; seqNo:12830; depth 10.75 2021-02-13T00:27:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176060.38 epoch sec; seqNo:12830; 4059217.58 north; 592586.31 east 2021-02-13T00:27:40Z- AHRS_M2 msg: 1613176060.77 epoch sec; seqNo:12728 2021-02-13T00:27:40Z- AHRS_M2 msg: 0.57 phi; -0.28 theta; -0.18 psi 2021-02-13T00:27:40Z- handleDepth()-Depth_Keller msg: 1613176060772 epoch sec; seqNo:12831; depth 10.84 2021-02-13T00:27:40Z- handleDvl() - RDI_Pathfinder msg: 1613176060.78 epoch sec; seqNo:6283 2021-02-13T00:27:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 101.63 , 80.00 , 111.10 2021-02-13T00:27:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , 0.14 2021-02-13T00:27:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176060.79 epoch sec; seqNo:12831; 4059217.96 north; 592586.31 east 2021-02-13T00:27:41Z- AHRS_M2 msg: 1613176061.17 epoch sec; seqNo:12729 2021-02-13T00:27:41Z- AHRS_M2 msg: 0.57 phi; -0.28 theta; -0.17 psi 2021-02-13T00:27:41Z- handleDepth()-Depth_Keller msg: 1613176061178 epoch sec; seqNo:12832; depth 10.94 2021-02-13T00:27:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176061.19 epoch sec; seqNo:12832; 4059217.97 north; 592587.53 east 2021-02-13T00:27:41Z- LcmTrn::handleMessages() - handling 41 messages took 0.72 ms and 127 calls... 2021-02-13T00:27:41Z- waiting for 1613176059.34, time is 1613176061.19 2021-02-13T00:27:41Z- LcmTrn::updateTrn() >>>> pose time 1613176061.19, meas time 1613176060.78 <<<< 2021-02-13T00:27:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176060.78 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:27:41Z- TerrainNav::motionUpdate() - currEstimate = (4059218, 592587.53, 10.94) 2021-02-13T00:27:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:27:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:27:41Z- TNavBF::Projecting Measurements 2021-02-13T00:27:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:27:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:27:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:27:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:27:41Z- TNavFilter::4 good beams. 2021-02-13T00:27:41Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:27:41Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:27:41Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:27:41Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:27:41Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:27:41Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:27:41Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:27:41Z- TNavBF::mmse selected from filter -1 2021-02-13T00:27:41Z- TerrainMapOctree: (vehN, vehE ) = (4059217.71, 592587.33). 2021-02-13T00:27:41Z- TerrainMapOctree: Min Distance = 2625.77. 2021-02-13T00:27:41Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:27:41Z- There is no specific good beam that all particles have in common. 2021-02-13T00:27:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:27:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 11.67 2021-02-13T00:27:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:27:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 16.53 2021-02-13T00:27:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:27:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 10.24 2021-02-13T00:27:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:27:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 19.75 2021-02-13T00:27:41Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:27:41Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:27:41Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:27:41Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:27:41Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:27:41Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:27:41Z- Filter 0 No beams used in subcloud update 2021-02-13T00:27:41Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:27:41Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:27:41Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 701.9 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:27:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176060.78 sec. 2021-02-13T00:27:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:27:41Z- TNavBF::mmse selected from filter -1 2021-02-13T00:27:41Z- variances([0], [2], and psi_berg) = 701.853549 832.998925 0.000000 2021-02-13T00:27:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:27:41Z- LcmTrn::publishEstimates() - MLE : 4059217.47 592555.03 10.94 2021-02-13T00:27:41Z- LcmTrn::publishEstimates() - MMSE : 4059217.97 592587.53 10.94 2021-02-13T00:27:41Z- LcmTrn::publishEstimates() - COVAR: 701.85 833.00 0.97 0.00 2021-02-13T00:27:41Z- AHRS_M2 msg: 1613176061.58 epoch sec; seqNo:12730 2021-02-13T00:27:41Z- AHRS_M2 msg: 0.58 phi; -0.29 theta; -0.17 psi 2021-02-13T00:27:41Z- handleDepth()-Depth_Keller msg: 1613176061581 epoch sec; seqNo:12833; depth 11.04 2021-02-13T00:27:41Z- handleDvl() - RDI_Pathfinder msg: 1613176061.60 epoch sec; seqNo:6284 2021-02-13T00:27:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 101.57 , 77.64 , 111.03 2021-02-13T00:27:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , 0.19 2021-02-13T00:27:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176061.61 epoch sec; seqNo:12833; 4059218.35 north; 592587.52 east 2021-02-13T00:27:41Z- AHRS_M2 msg: 1613176061.98 epoch sec; seqNo:12731 2021-02-13T00:27:41Z- AHRS_M2 msg: 0.58 phi; -0.29 theta; -0.17 psi 2021-02-13T00:27:41Z- handleDepth()-Depth_Keller msg: 1613176061989 epoch sec; seqNo:12834; depth 11.15 2021-02-13T00:27:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176062.00 epoch sec; seqNo:12834; 4059218.73 north; 592587.52 east 2021-02-13T00:27:42Z- AHRS_M2 msg: 1613176062.38 epoch sec; seqNo:12732 2021-02-13T00:27:42Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.16 psi 2021-02-13T00:27:42Z- handleDepth()-Depth_Keller msg: 1613176062388 epoch sec; seqNo:12835; depth 11.24 2021-02-13T00:27:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176062.40 epoch sec; seqNo:12835; 4059219.11 north; 592587.51 east 2021-02-13T00:27:42Z- AHRS_M2 msg: 1613176062.79 epoch sec; seqNo:12733 2021-02-13T00:27:42Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.16 psi 2021-02-13T00:27:42Z- handleDepth()-Depth_Keller msg: 1613176062801 epoch sec; seqNo:12836; depth 11.35 2021-02-13T00:27:42Z- handleDvl() - RDI_Pathfinder msg: 1613176062.81 epoch sec; seqNo:6285 2021-02-13T00:27:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 101.63 , 78.05 , 112.45 2021-02-13T00:27:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.09 , 0.13 2021-02-13T00:27:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176062.82 epoch sec; seqNo:12836; 4059219.11 north; 592587.51 east 2021-02-13T00:27:43Z- AHRS_M2 msg: 1613176063.19 epoch sec; seqNo:12734 2021-02-13T00:27:43Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.16 psi 2021-02-13T00:27:43Z- handleDepth()-Depth_Keller msg: 1613176063206 epoch sec; seqNo:12837; depth 11.46 2021-02-13T00:27:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176063.22 epoch sec; seqNo:12837; 4059219.50 north; 592588.73 east 2021-02-13T00:27:43Z- AHRS_M2 msg: 1613176063.60 epoch sec; seqNo:12735 2021-02-13T00:27:43Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.15 psi 2021-02-13T00:27:43Z- handleDepth()-Depth_Keller msg: 1613176063601 epoch sec; seqNo:12838; depth 11.57 2021-02-13T00:27:43Z- handleDvl() - RDI_Pathfinder msg: 1613176063.61 epoch sec; seqNo:6286 2021-02-13T00:27:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.21 , 78.58 , 109.68 2021-02-13T00:27:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.08 , 0.00 2021-02-13T00:27:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176063.64 epoch sec; seqNo:12838; 4059219.88 north; 592588.73 east 2021-02-13T00:27:44Z- AHRS_M2 msg: 1613176064.00 epoch sec; seqNo:12736 2021-02-13T00:27:44Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.15 psi 2021-02-13T00:27:44Z- handleDepth()-Depth_Keller msg: 1613176064005 epoch sec; seqNo:12839; depth 11.68 2021-02-13T00:27:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176064.02 epoch sec; seqNo:12839; 4059219.88 north; 592588.73 east 2021-02-13T00:27:44Z- AHRS_M2 msg: 1613176064.40 epoch sec; seqNo:12737 2021-02-13T00:27:44Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.15 psi 2021-02-13T00:27:44Z- handleDepth()-Depth_Keller msg: 1613176064410 epoch sec; seqNo:12840; depth 11.80 2021-02-13T00:27:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176064.42 epoch sec; seqNo:12840; 4059220.26 north; 592588.72 east 2021-02-13T00:27:44Z- AHRS_M2 msg: 1613176064.81 epoch sec; seqNo:12738 2021-02-13T00:27:44Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.15 psi 2021-02-13T00:27:44Z- handleDepth()-Depth_Keller msg: 1613176064812 epoch sec; seqNo:12841; depth 11.89 2021-02-13T00:27:44Z- handleDvl() - RDI_Pathfinder msg: 1613176064.83 epoch sec; seqNo:6287 2021-02-13T00:27:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 101.43 , 76.42 , 113.60 2021-02-13T00:27:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , -0.11 2021-02-13T00:27:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176064.84 epoch sec; seqNo:12841; 4059220.64 north; 592588.72 east 2021-02-13T00:27:45Z- AHRS_M2 msg: 1613176065.21 epoch sec; seqNo:12739 2021-02-13T00:27:45Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.15 psi 2021-02-13T00:27:45Z- handleDepth()-Depth_Keller msg: 1613176065216 epoch sec; seqNo:12842; depth 12.01 2021-02-13T00:27:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176065.23 epoch sec; seqNo:12842; 4059221.01 north; 592588.71 east 2021-02-13T00:27:45Z- AHRS_M2 msg: 1613176065.62 epoch sec; seqNo:12740 2021-02-13T00:27:45Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.15 psi 2021-02-13T00:27:45Z- handleDepth()-Depth_Keller msg: 1613176065626 epoch sec; seqNo:12843; depth 12.13 2021-02-13T00:27:45Z- handleDvl() - RDI_Pathfinder msg: 1613176065.64 epoch sec; seqNo:6288 2021-02-13T00:27:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.21 , 76.01 , 113.73 2021-02-13T00:27:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.06 , -0.14 2021-02-13T00:27:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176065.65 epoch sec; seqNo:12843; 4059221.41 north; 592589.93 east 2021-02-13T00:27:46Z- AHRS_M2 msg: 1613176066.02 epoch sec; seqNo:12741 2021-02-13T00:27:46Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.15 psi 2021-02-13T00:27:46Z- handleDepth()-Depth_Keller msg: 1613176066024 epoch sec; seqNo:12844; depth 12.24 2021-02-13T00:27:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176066.05 epoch sec; seqNo:12844; 4059221.41 north; 592589.93 east 2021-02-13T00:27:46Z- AHRS_M2 msg: 1613176066.43 epoch sec; seqNo:12742 2021-02-13T00:27:46Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.16 psi 2021-02-13T00:27:46Z- handleDepth()-Depth_Keller msg: 1613176066434 epoch sec; seqNo:12845; depth 12.36 2021-02-13T00:27:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176066.44 epoch sec; seqNo:12845; 4059221.79 north; 592589.93 east 2021-02-13T00:27:46Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-13T00:27:46Z- waiting for 1613176064.19, time is 1613176066.44 2021-02-13T00:27:46Z- LcmTrn::updateTrn() >>>> pose time 1613176066.44, meas time 1613176065.64 <<<< 2021-02-13T00:27:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176065.64 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:27:46Z- TerrainNav::motionUpdate() - currEstimate = (4059222, 592589.93, 12.36) 2021-02-13T00:27:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:27:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:27:46Z- TNavBF::Projecting Measurements 2021-02-13T00:27:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:27:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:27:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:27:46Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:27:46Z- TNavFilter::3 good beams. 2021-02-13T00:27:46Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:27:46Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:27:46Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:27:46Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:27:46Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:27:46Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:27:46Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:27:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:27:46Z- TerrainMapOctree: (vehN, vehE ) = (4059221.20, 592589.56). 2021-02-13T00:27:46Z- TerrainMapOctree: Min Distance = 2621.65. 2021-02-13T00:27:46Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:27:46Z- There is no specific good beam that all particles have in common. 2021-02-13T00:27:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:27:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 16.07 2021-02-13T00:27:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:27:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 9.24 2021-02-13T00:27:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:27:46Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 20.70 2021-02-13T00:27:46Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:27:46Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:27:46Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:27:46Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:27:46Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:27:46Z- Filter 0 No beams used in subcloud update 2021-02-13T00:27:46Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:27:46Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:27:46Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 701.8 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:27:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176065.64 sec. 2021-02-13T00:27:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:27:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:27:46Z- variances([0], [2], and psi_berg) = 701.776731 832.998884 0.000000 2021-02-13T00:27:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:27:46Z- LcmTrn::publishEstimates() - MLE : 4059221.29 592557.43 12.36 2021-02-13T00:27:46Z- LcmTrn::publishEstimates() - MMSE : 4059221.79 592589.93 12.36 2021-02-13T00:27:46Z- LcmTrn::publishEstimates() - COVAR: 701.78 833.00 0.99 0.00 2021-02-13T00:27:46Z- AHRS_M2 msg: 1613176066.83 epoch sec; seqNo:12743 2021-02-13T00:27:46Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-13T00:27:46Z- handleDepth()-Depth_Keller msg: 1613176066836 epoch sec; seqNo:12846; depth 12.48 2021-02-13T00:27:46Z- handleDvl() - RDI_Pathfinder msg: 1613176066.85 epoch sec; seqNo:6289 2021-02-13T00:27:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 100.01 , 75.19 , 112.18 2021-02-13T00:27:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.13 , -0.09 2021-02-13T00:27:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176066.86 epoch sec; seqNo:12846; 4059222.16 north; 592589.92 east 2021-02-13T00:27:47Z- AHRS_M2 msg: 1613176067.23 epoch sec; seqNo:12744 2021-02-13T00:27:47Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-13T00:27:47Z- handleDepth()-Depth_Keller msg: 1613176067238 epoch sec; seqNo:12847; depth 12.60 2021-02-13T00:27:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176067.25 epoch sec; seqNo:12847; 4059222.54 north; 592589.92 east 2021-02-13T00:27:47Z- AHRS_M2 msg: 1613176067.64 epoch sec; seqNo:12745 2021-02-13T00:27:47Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.19 psi 2021-02-13T00:27:47Z- handleDepth()-Depth_Keller msg: 1613176067645 epoch sec; seqNo:12848; depth 12.73 2021-02-13T00:27:47Z- handleDvl() - RDI_Pathfinder msg: 1613176067.66 epoch sec; seqNo:6290 2021-02-13T00:27:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.13 , 100.35 , 74.78 , 112.52 2021-02-13T00:27:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.17 , -0.03 2021-02-13T00:27:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176067.67 epoch sec; seqNo:12848; 4059222.92 north; 592589.91 east 2021-02-13T00:27:48Z- AHRS_M2 msg: 1613176068.04 epoch sec; seqNo:12746 2021-02-13T00:27:48Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-13T00:27:48Z- handleDepth()-Depth_Keller msg: 1613176068053 epoch sec; seqNo:12849; depth 12.84 2021-02-13T00:27:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176068.07 epoch sec; seqNo:12849; 4059223.31 north; 592591.13 east 2021-02-13T00:27:48Z- AHRS_M2 msg: 1613176068.44 epoch sec; seqNo:12747 2021-02-13T00:27:48Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-13T00:27:48Z- handleDepth()-Depth_Keller msg: 1613176068448 epoch sec; seqNo:12850; depth 12.96 2021-02-13T00:27:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176068.46 epoch sec; seqNo:12850; 4059223.69 north; 592591.13 east 2021-02-13T00:27:48Z- AHRS_M2 msg: 1613176068.85 epoch sec; seqNo:12748 2021-02-13T00:27:48Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-13T00:27:48Z- handleDepth()-Depth_Keller msg: 1613176068852 epoch sec; seqNo:12851; depth 13.09 2021-02-13T00:27:48Z- handleDvl() - RDI_Pathfinder msg: 1613176068.86 epoch sec; seqNo:6291 2021-02-13T00:27:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 100.35 , 78.72 , 113.87 2021-02-13T00:27:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.13 , -0.01 2021-02-13T00:27:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176068.88 epoch sec; seqNo:12851; 4059223.69 north; 592591.13 east 2021-02-13T00:27:49Z- AHRS_M2 msg: 1613176069.25 epoch sec; seqNo:12749 2021-02-13T00:27:49Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-13T00:27:49Z- handleDepth()-Depth_Keller msg: 1613176069257 epoch sec; seqNo:12852; depth 13.22 2021-02-13T00:27:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176069.27 epoch sec; seqNo:12852; 4059224.07 north; 592591.12 east 2021-02-13T00:27:49Z- AHRS_M2 msg: 1613176069.66 epoch sec; seqNo:12750 2021-02-13T00:27:49Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:27:49Z- handleDepth()-Depth_Keller msg: 1613176069665 epoch sec; seqNo:12853; depth 13.32 2021-02-13T00:27:49Z- handleDvl() - RDI_Pathfinder msg: 1613176069.68 epoch sec; seqNo:6292 2021-02-13T00:27:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.28 , 78.65 , 111.10 2021-02-13T00:27:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.06 , -0.01 2021-02-13T00:27:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176069.69 epoch sec; seqNo:12853; 4059224.45 north; 592591.12 east 2021-02-13T00:27:50Z- AHRS_M2 msg: 1613176070.06 epoch sec; seqNo:12751 2021-02-13T00:27:50Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-13T00:27:50Z- handleDepth()-Depth_Keller msg: 1613176070064 epoch sec; seqNo:12854; depth 13.45 2021-02-13T00:27:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176070.08 epoch sec; seqNo:12854; 4059224.83 north; 592591.12 east 2021-02-13T00:27:50Z- AHRS_M2 msg: 1613176070.46 epoch sec; seqNo:12752 2021-02-13T00:27:50Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-13T00:27:50Z- handleDepth()-Depth_Keller msg: 1613176070473 epoch sec; seqNo:12855; depth 13.57 2021-02-13T00:27:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176070.48 epoch sec; seqNo:12855; 4059224.83 north; 592591.12 east 2021-02-13T00:27:50Z- AHRS_M2 msg: 1613176070.87 epoch sec; seqNo:12753 2021-02-13T00:27:50Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-13T00:27:50Z- handleDepth()-Depth_Keller msg: 1613176070872 epoch sec; seqNo:12856; depth 13.71 2021-02-13T00:27:50Z- handleDvl() - RDI_Pathfinder msg: 1613176070.88 epoch sec; seqNo:6293 2021-02-13T00:27:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.36 , 78.38 , 113.53 2021-02-13T00:27:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.08 , -0.08 2021-02-13T00:27:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176070.89 epoch sec; seqNo:12856; 4059225.21 north; 592591.11 east 2021-02-13T00:27:51Z- AHRS_M2 msg: 1613176071.27 epoch sec; seqNo:12754 2021-02-13T00:27:51Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-13T00:27:51Z- handleDepth()-Depth_Keller msg: 1613176071278 epoch sec; seqNo:12857; depth 13.84 2021-02-13T00:27:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176071.29 epoch sec; seqNo:12857; 4059225.60 north; 592592.33 east 2021-02-13T00:27:51Z- AHRS_M2 msg: 1613176071.68 epoch sec; seqNo:12755 2021-02-13T00:27:51Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.20 psi 2021-02-13T00:27:51Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-13T00:27:51Z- waiting for 1613176069.44, time is 1613176071.68 2021-02-13T00:27:51Z- LcmTrn::updateTrn() >>>> pose time 1613176071.68, meas time 1613176070.88 <<<< 2021-02-13T00:27:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176070.88 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:27:51Z- TerrainNav::motionUpdate() - currEstimate = (4059226, 592592.33, 13.84) 2021-02-13T00:27:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:27:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:27:51Z- TNavBF::Projecting Measurements 2021-02-13T00:27:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:27:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:27:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:27:51Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:27:51Z- TNavFilter::3 good beams. 2021-02-13T00:27:51Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:27:51Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:27:51Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:27:51Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:27:51Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:27:51Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:27:51Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:27:51Z- TNavBF::mmse selected from filter -1 2021-02-13T00:27:51Z- TerrainMapOctree: (vehN, vehE ) = (4059225.02, 592591.96). 2021-02-13T00:27:51Z- TerrainMapOctree: Min Distance = 2617.18. 2021-02-13T00:27:51Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:27:51Z- There is no specific good beam that all particles have in common. 2021-02-13T00:27:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:27:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 16.44 2021-02-13T00:27:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:27:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 9.83 2021-02-13T00:27:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:27:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 20.62 2021-02-13T00:27:51Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:27:51Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:27:51Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:27:51Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:27:51Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:27:51Z- Filter 0 No beams used in subcloud update 2021-02-13T00:27:51Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:27:51Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:27:51Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 701.7 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:27:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176070.88 sec. 2021-02-13T00:27:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:27:51Z- TNavBF::mmse selected from filter -1 2021-02-13T00:27:51Z- variances([0], [2], and psi_berg) = 701.700018 832.998843 0.000000 2021-02-13T00:27:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:27:51Z- LcmTrn::publishEstimates() - MLE : 4059225.10 592562.83 13.84 2021-02-13T00:27:51Z- LcmTrn::publishEstimates() - MMSE : 4059225.60 592592.33 13.84 2021-02-13T00:27:51Z- LcmTrn::publishEstimates() - COVAR: 701.70 833.00 1.00 0.00 2021-02-13T00:27:51Z- handleDepth()-Depth_Keller msg: 1613176071680 epoch sec; seqNo:12858; depth 13.96 2021-02-13T00:27:51Z- handleDvl() - RDI_Pathfinder msg: 1613176071.70 epoch sec; seqNo:6294 2021-02-13T00:27:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.37 , 101.30 , 74.78 , 114.82 2021-02-13T00:27:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.11 , -0.06 2021-02-13T00:27:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176071.71 epoch sec; seqNo:12858; 4059225.98 north; 592592.32 east 2021-02-13T00:27:52Z- AHRS_M2 msg: 1613176072.08 epoch sec; seqNo:12756 2021-02-13T00:27:52Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.20 psi 2021-02-13T00:27:52Z- handleDepth()-Depth_Keller msg: 1613176072089 epoch sec; seqNo:12859; depth 14.07 2021-02-13T00:27:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176072.10 epoch sec; seqNo:12859; 4059225.98 north; 592592.32 east 2021-02-13T00:27:52Z- AHRS_M2 msg: 1613176072.48 epoch sec; seqNo:12757 2021-02-13T00:27:52Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.20 psi 2021-02-13T00:27:52Z- handleDepth()-Depth_Keller msg: 1613176072488 epoch sec; seqNo:12860; depth 14.20 2021-02-13T00:27:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176072.50 epoch sec; seqNo:12860; 4059226.36 north; 592592.32 east 2021-02-13T00:27:52Z- AHRS_M2 msg: 1613176072.89 epoch sec; seqNo:12758 2021-02-13T00:27:52Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-13T00:27:52Z- handleDepth()-Depth_Keller msg: 1613176072894 epoch sec; seqNo:12861; depth 14.34 2021-02-13T00:27:52Z- handleDvl() - RDI_Pathfinder msg: 1613176072.90 epoch sec; seqNo:6295 2021-02-13T00:27:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.71 , 102.99 , 74.37 , 115.15 2021-02-13T00:27:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.14 , 0.03 2021-02-13T00:27:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176072.91 epoch sec; seqNo:12861; 4059226.74 north; 592592.32 east 2021-02-13T00:27:53Z- AHRS_M2 msg: 1613176073.29 epoch sec; seqNo:12759 2021-02-13T00:27:53Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-13T00:27:53Z- handleDepth()-Depth_Keller msg: 1613176073297 epoch sec; seqNo:12862; depth 14.47 2021-02-13T00:27:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176073.31 epoch sec; seqNo:12862; 4059227.12 north; 592592.31 east 2021-02-13T00:27:53Z- AHRS_M2 msg: 1613176073.70 epoch sec; seqNo:12760 2021-02-13T00:27:53Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-13T00:27:53Z- handleDepth()-Depth_Keller msg: 1613176073705 epoch sec; seqNo:12863; depth 14.61 2021-02-13T00:27:53Z- handleDvl() - RDI_Pathfinder msg: 1613176073.73 epoch sec; seqNo:6296 2021-02-13T00:27:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.36 , 96.23 , 72.33 , 113.80 2021-02-13T00:27:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.09 , 0.15 2021-02-13T00:27:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176073.74 epoch sec; seqNo:12863; 4059227.12 north; 592592.31 east 2021-02-13T00:27:54Z- AHRS_M2 msg: 1613176074.10 epoch sec; seqNo:12761 2021-02-13T00:27:54Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.17 psi 2021-02-13T00:27:54Z- handleDepth()-Depth_Keller msg: 1613176074113 epoch sec; seqNo:12864; depth 14.73 2021-02-13T00:27:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176074.13 epoch sec; seqNo:12864; 4059227.49 north; 592592.31 east 2021-02-13T00:27:54Z- AHRS_M2 msg: 1613176074.50 epoch sec; seqNo:12762 2021-02-13T00:27:54Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.16 psi 2021-02-13T00:27:54Z- handleDepth()-Depth_Keller msg: 1613176074508 epoch sec; seqNo:12865; depth 14.85 2021-02-13T00:27:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176074.52 epoch sec; seqNo:12865; 4059227.89 north; 592593.52 east 2021-02-13T00:27:54Z- AHRS_M2 msg: 1613176074.91 epoch sec; seqNo:12763 2021-02-13T00:27:54Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.16 psi 2021-02-13T00:27:54Z- handleDepth()-Depth_Keller msg: 1613176074912 epoch sec; seqNo:12866; depth 14.98 2021-02-13T00:27:54Z- handleDvl() - RDI_Pathfinder msg: 1613176074.92 epoch sec; seqNo:6297 2021-02-13T00:27:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 98.93 , 71.51 , 113.53 2021-02-13T00:27:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , 0.21 2021-02-13T00:27:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176074.94 epoch sec; seqNo:12866; 4059227.89 north; 592593.52 east 2021-02-13T00:27:55Z- AHRS_M2 msg: 1613176075.31 epoch sec; seqNo:12764 2021-02-13T00:27:55Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.16 psi 2021-02-13T00:27:55Z- handleDepth()-Depth_Keller msg: 1613176075315 epoch sec; seqNo:12867; depth 15.11 2021-02-13T00:27:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176075.33 epoch sec; seqNo:12867; 4059228.27 north; 592593.52 east 2021-02-13T00:27:55Z- AHRS_M2 msg: 1613176075.72 epoch sec; seqNo:12765 2021-02-13T00:27:55Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.16 psi 2021-02-13T00:27:55Z- handleDepth()-Depth_Keller msg: 1613176075722 epoch sec; seqNo:12868; depth 15.23 2021-02-13T00:27:55Z- handleDvl() - RDI_Pathfinder msg: 1613176075.73 epoch sec; seqNo:6298 2021-02-13T00:27:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 97.71 , 73.56 , 112.58 2021-02-13T00:27:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.11 , 0.15 2021-02-13T00:27:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176075.74 epoch sec; seqNo:12868; 4059228.64 north; 592593.52 east 2021-02-13T00:27:56Z- AHRS_M2 msg: 1613176076.12 epoch sec; seqNo:12766 2021-02-13T00:27:56Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.16 psi 2021-02-13T00:27:56Z- handleDepth()-Depth_Keller msg: 1613176076139 epoch sec; seqNo:12869; depth 15.36 2021-02-13T00:27:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176076.15 epoch sec; seqNo:12869; 4059228.64 north; 592593.52 east 2021-02-13T00:27:56Z- AHRS_M2 msg: 1613176076.52 epoch sec; seqNo:12767 2021-02-13T00:27:56Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.16 psi 2021-02-13T00:27:56Z- handleDepth()-Depth_Keller msg: 1613176076533 epoch sec; seqNo:12870; depth 15.48 2021-02-13T00:27:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176076.54 epoch sec; seqNo:12870; 4059229.04 north; 592594.73 east 2021-02-13T00:27:56Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-13T00:27:56Z- waiting for 1613176074.68, time is 1613176076.54 2021-02-13T00:27:56Z- LcmTrn::updateTrn() >>>> pose time 1613176076.54, meas time 1613176075.73 <<<< 2021-02-13T00:27:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176075.73 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:27:56Z- TerrainNav::motionUpdate() - currEstimate = (4059229, 592594.73, 15.48) 2021-02-13T00:27:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:27:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:27:56Z- TNavBF::Projecting Measurements 2021-02-13T00:27:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:27:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:27:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:27:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:27:56Z- TNavFilter::4 good beams. 2021-02-13T00:27:56Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:27:56Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:27:56Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:27:56Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:27:56Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:27:56Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:27:56Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:27:56Z- TNavBF::mmse selected from filter -1 2021-02-13T00:27:56Z- TerrainMapOctree: (vehN, vehE ) = (4059228.47, 592594.33). 2021-02-13T00:27:56Z- TerrainMapOctree: Min Distance = 2613.01. 2021-02-13T00:27:56Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:27:56Z- There is no specific good beam that all particles have in common. 2021-02-13T00:27:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:27:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 10.98 2021-02-13T00:27:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:27:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:27:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:27:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 8.66 2021-02-13T00:27:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:27:56Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 20.28 2021-02-13T00:27:56Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:27:56Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:27:56Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:27:56Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:27:56Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:27:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:27:56Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:27:56Z- Filter 0 No beams used in subcloud update 2021-02-13T00:27:56Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:27:56Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:27:56Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 701.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:27:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176075.73 sec. 2021-02-13T00:27:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:27:56Z- TNavBF::mmse selected from filter -1 2021-02-13T00:27:56Z- variances([0], [2], and psi_berg) = 701.625490 832.998803 0.000000 2021-02-13T00:27:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:27:56Z- LcmTrn::publishEstimates() - MLE : 4059228.54 592565.23 15.48 2021-02-13T00:27:56Z- LcmTrn::publishEstimates() - MMSE : 4059229.04 592594.73 15.48 2021-02-13T00:27:56Z- LcmTrn::publishEstimates() - COVAR: 701.63 833.00 0.92 0.00 2021-02-13T00:27:56Z- AHRS_M2 msg: 1613176076.93 epoch sec; seqNo:12768 2021-02-13T00:27:56Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.17 psi 2021-02-13T00:27:56Z- handleDepth()-Depth_Keller msg: 1613176076938 epoch sec; seqNo:12871; depth 15.60 2021-02-13T00:27:56Z- handleDvl() - RDI_Pathfinder msg: 1613176076.95 epoch sec; seqNo:6299 2021-02-13T00:27:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 96.43 , 78.18 , 115.90 2021-02-13T00:27:56Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , 0.12 , -0.03 2021-02-13T00:27:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176076.96 epoch sec; seqNo:12871; 4059229.41 north; 592594.73 east 2021-02-13T00:27:57Z- AHRS_M2 msg: 1613176077.33 epoch sec; seqNo:12769 2021-02-13T00:27:57Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.18 psi 2021-02-13T00:27:57Z- handleDepth()-Depth_Keller msg: 1613176077336 epoch sec; seqNo:12872; depth 15.74 2021-02-13T00:27:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176077.35 epoch sec; seqNo:12872; 4059229.79 north; 592594.72 east 2021-02-13T00:27:57Z- AHRS_M2 msg: 1613176077.74 epoch sec; seqNo:12770 2021-02-13T00:27:57Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-13T00:27:57Z- handleDepth()-Depth_Keller msg: 1613176077740 epoch sec; seqNo:12873; depth 15.86 2021-02-13T00:27:57Z- handleDvl() - RDI_Pathfinder msg: 1613176077.75 epoch sec; seqNo:6300 2021-02-13T00:27:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.93 , 71.51 , 111.10 2021-02-13T00:27:57Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , -0.05 , -0.16 2021-02-13T00:27:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176077.76 epoch sec; seqNo:12873; 4059229.79 north; 592594.72 east 2021-02-13T00:27:58Z- AHRS_M2 msg: 1613176078.14 epoch sec; seqNo:12771 2021-02-13T00:27:58Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-13T00:27:58Z- handleDepth()-Depth_Keller msg: 1613176078150 epoch sec; seqNo:12874; depth 16.00 2021-02-13T00:27:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176078.16 epoch sec; seqNo:12874; 4059230.17 north; 592594.72 east 2021-02-13T00:27:58Z- AHRS_M2 msg: 1613176078.57 epoch sec; seqNo:12772 2021-02-13T00:27:58Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-13T00:27:58Z- handleDepth()-Depth_Keller msg: 1613176078570 epoch sec; seqNo:12875; depth 16.12 2021-02-13T00:27:58Z- handleDvl() - RDI_Pathfinder msg: 1613176078.58 epoch sec; seqNo:6301 2021-02-13T00:27:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.00 , 96.23 , 71.92 , 109.75 2021-02-13T00:27:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.16 , -0.14 2021-02-13T00:27:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176078.59 epoch sec; seqNo:12875; 4059230.56 north; 592595.94 east 2021-02-13T00:27:58Z- AHRS_M2 msg: 1613176078.95 epoch sec; seqNo:12773 2021-02-13T00:27:58Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-13T00:27:58Z- handleDepth()-Depth_Keller msg: 1613176078952 epoch sec; seqNo:12876; depth 16.25 2021-02-13T00:27:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176078.96 epoch sec; seqNo:12876; 4059230.94 north; 592595.93 east 2021-02-13T00:27:59Z- AHRS_M2 msg: 1613176079.36 epoch sec; seqNo:12774 2021-02-13T00:27:59Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.19 psi 2021-02-13T00:27:59Z- handleDepth()-Depth_Keller msg: 1613176079364 epoch sec; seqNo:12877; depth 16.37 2021-02-13T00:27:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176079.38 epoch sec; seqNo:12877; 4059231.32 north; 592595.93 east 2021-02-13T00:27:59Z- AHRS_M2 msg: 1613176079.76 epoch sec; seqNo:12775 2021-02-13T00:27:59Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.18 psi 2021-02-13T00:27:59Z- handleDepth()-Depth_Keller msg: 1613176079767 epoch sec; seqNo:12878; depth 16.48 2021-02-13T00:27:59Z- handleDvl() - RDI_Pathfinder msg: 1613176079.78 epoch sec; seqNo:6302 2021-02-13T00:27:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 96.56 , 0.00 , 109.95 2021-02-13T00:27:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.18 , -0.01 2021-02-13T00:27:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176079.79 epoch sec; seqNo:12878; 4059231.70 north; 592595.92 east 2021-02-13T00:28:00Z- AHRS_M2 msg: 1613176080.16 epoch sec; seqNo:12776 2021-02-13T00:28:00Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.18 psi 2021-02-13T00:28:00Z- handleDepth()-Depth_Keller msg: 1613176080169 epoch sec; seqNo:12879; depth 16.60 2021-02-13T00:28:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176080.19 epoch sec; seqNo:12879; 4059232.08 north; 592595.92 east 2021-02-13T00:28:00Z- AHRS_M2 msg: 1613176080.56 epoch sec; seqNo:12777 2021-02-13T00:28:00Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.18 psi 2021-02-13T00:28:00Z- handleDepth()-Depth_Keller msg: 1613176080568 epoch sec; seqNo:12880; depth 16.72 2021-02-13T00:28:00Z- handleDvl() - RDI_Pathfinder msg: 1613176080.58 epoch sec; seqNo:6303 2021-02-13T00:28:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.69 , 96.02 , 72.74 , 110.89 2021-02-13T00:28:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.10 , 0.09 2021-02-13T00:28:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176080.61 epoch sec; seqNo:12880; 4059232.08 north; 592595.92 east 2021-02-13T00:28:00Z- AHRS_M2 msg: 1613176080.97 epoch sec; seqNo:12778 2021-02-13T00:28:00Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.18 psi 2021-02-13T00:28:00Z- handleDepth()-Depth_Keller msg: 1613176080972 epoch sec; seqNo:12881; depth 16.84 2021-02-13T00:28:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176080.98 epoch sec; seqNo:12881; 4059232.46 north; 592595.92 east 2021-02-13T00:28:01Z- AHRS_M2 msg: 1613176081.37 epoch sec; seqNo:12779 2021-02-13T00:28:01Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.18 psi 2021-02-13T00:28:01Z- handleDepth()-Depth_Keller msg: 1613176081382 epoch sec; seqNo:12882; depth 16.96 2021-02-13T00:28:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176081.39 epoch sec; seqNo:12882; 4059232.46 north; 592595.92 east 2021-02-13T00:28:01Z- AHRS_M2 msg: 1613176081.79 epoch sec; seqNo:12780 2021-02-13T00:28:01Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.18 psi 2021-02-13T00:28:01Z- handleDepth()-Depth_Keller msg: 1613176081803 epoch sec; seqNo:12883; depth 17.07 2021-02-13T00:28:01Z- handleDvl() - RDI_Pathfinder msg: 1613176081.82 epoch sec; seqNo:6304 2021-02-13T00:28:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.78 , 78.31 , 112.38 2021-02-13T00:28:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.03 , 0.09 2021-02-13T00:28:01Z- LcmTrn::handleMessages() - handling 44 messages took 0.60 ms and 132 calls... 2021-02-13T00:28:01Z- waiting for 1613176079.54, time is 1613176081.82 2021-02-13T00:28:01Z- LcmTrn::updateTrn() equating pose 1613176081.82 and meas times 1613176081.82 2021-02-13T00:28:01Z- LcmTrn::updateTrn() >>>> pose time 1613176081.82, meas time 1613176081.82 <<<< 2021-02-13T00:28:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176081.82 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:28:01Z- TerrainNav::motionUpdate() - currEstimate = (4059232, 592595.92, 17.07) 2021-02-13T00:28:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:28:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:28:01Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176081.82 sec.; No relevant navigation data available 2021-02-13T00:28:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:28:01Z- TNavBF::mmse selected from filter -1 2021-02-13T00:28:01Z- variances([0], [2], and psi_berg) = 701.573083 832.998775 0.000000 2021-02-13T00:28:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:28:01Z- LcmTrn::publishEstimates() - MLE : 4059231.96 592566.42 17.07 2021-02-13T00:28:01Z- LcmTrn::publishEstimates() - MMSE : 4059232.46 592595.92 17.07 2021-02-13T00:28:01Z- LcmTrn::publishEstimates() - COVAR: 701.57 833.00 1.11 0.00 2021-02-13T00:28:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176081.83 epoch sec; seqNo:12883; 4059232.85 north; 592597.13 east 2021-02-13T00:28:02Z- AHRS_M2 msg: 1613176082.18 epoch sec; seqNo:12781 2021-02-13T00:28:02Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.19 psi 2021-02-13T00:28:02Z- handleDepth()-Depth_Keller msg: 1613176082185 epoch sec; seqNo:12884; depth 17.21 2021-02-13T00:28:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176082.19 epoch sec; seqNo:12884; 4059233.23 north; 592597.13 east 2021-02-13T00:28:02Z- AHRS_M2 msg: 1613176082.62 epoch sec; seqNo:12782 2021-02-13T00:28:02Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.21 psi 2021-02-13T00:28:02Z- handleDepth()-Depth_Keller msg: 1613176082621 epoch sec; seqNo:12885; depth 17.34 2021-02-13T00:28:02Z- handleDvl() - RDI_Pathfinder msg: 1613176082.64 epoch sec; seqNo:6305 2021-02-13T00:28:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.73 , 78.45 , 113.60 2021-02-13T00:28:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.10 , 0.00 2021-02-13T00:28:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176082.65 epoch sec; seqNo:12885; 4059233.23 north; 592597.13 east 2021-02-13T00:28:03Z- handleDepth()-Depth_Keller msg: 1613176083013 epoch sec; seqNo:12886; depth 17.48 2021-02-13T00:28:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176083.02 epoch sec; seqNo:12886; 4059233.61 north; 592597.12 east 2021-02-13T00:28:03Z- AHRS_M2 msg: 1613176083.39 epoch sec; seqNo:12783 2021-02-13T00:28:03Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.22 psi 2021-02-13T00:28:03Z- handleDepth()-Depth_Keller msg: 1613176083402 epoch sec; seqNo:12887; depth 17.60 2021-02-13T00:28:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176083.42 epoch sec; seqNo:12887; 4059233.99 north; 592597.12 east 2021-02-13T00:28:03Z- AHRS_M2 msg: 1613176083.81 epoch sec; seqNo:12784 2021-02-13T00:28:03Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.23 psi 2021-02-13T00:28:03Z- handleDepth()-Depth_Keller msg: 1613176083810 epoch sec; seqNo:12888; depth 17.74 2021-02-13T00:28:03Z- handleDvl() - RDI_Pathfinder msg: 1613176083.83 epoch sec; seqNo:6306 2021-02-13T00:28:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.73 , 100.01 , 70.70 , 112.18 2021-02-13T00:28:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.00 , -0.10 2021-02-13T00:28:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176083.84 epoch sec; seqNo:12888; 4059234.38 north; 592598.34 east 2021-02-13T00:28:04Z- AHRS_M2 msg: 1613176084.20 epoch sec; seqNo:12785 2021-02-13T00:28:04Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.23 psi 2021-02-13T00:28:04Z- handleDepth()-Depth_Keller msg: 1613176084213 epoch sec; seqNo:12889; depth 17.87 2021-02-13T00:28:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176084.22 epoch sec; seqNo:12889; 4059234.38 north; 592598.34 east 2021-02-13T00:28:04Z- AHRS_M2 msg: 1613176084.60 epoch sec; seqNo:12786 2021-02-13T00:28:04Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.24 psi 2021-02-13T00:28:04Z- handleDepth()-Depth_Keller msg: 1613176084610 epoch sec; seqNo:12890; depth 18.00 2021-02-13T00:28:04Z- handleDvl() - RDI_Pathfinder msg: 1613176084.62 epoch sec; seqNo:6307 2021-02-13T00:28:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 101.36 , 69.47 , 111.10 2021-02-13T00:28:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.14 , -0.11 2021-02-13T00:28:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176084.63 epoch sec; seqNo:12890; 4059234.76 north; 592598.33 east 2021-02-13T00:28:05Z- AHRS_M2 msg: 1613176085.01 epoch sec; seqNo:12787 2021-02-13T00:28:05Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.24 psi 2021-02-13T00:28:05Z- handleDepth()-Depth_Keller msg: 1613176085017 epoch sec; seqNo:12891; depth 18.15 2021-02-13T00:28:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176085.03 epoch sec; seqNo:12891; 4059235.14 north; 592598.33 east 2021-02-13T00:28:05Z- AHRS_M2 msg: 1613176085.41 epoch sec; seqNo:12788 2021-02-13T00:28:05Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.23 psi 2021-02-13T00:28:05Z- handleDepth()-Depth_Keller msg: 1613176085416 epoch sec; seqNo:12892; depth 18.26 2021-02-13T00:28:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176085.42 epoch sec; seqNo:12892; 4059235.52 north; 592598.32 east 2021-02-13T00:28:05Z- AHRS_M2 msg: 1613176085.82 epoch sec; seqNo:12789 2021-02-13T00:28:05Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.23 psi 2021-02-13T00:28:05Z- handleDepth()-Depth_Keller msg: 1613176085838 epoch sec; seqNo:12893; depth 18.40 2021-02-13T00:28:05Z- handleDvl() - RDI_Pathfinder msg: 1613176085.85 epoch sec; seqNo:6308 2021-02-13T00:28:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.21 , 71.51 , 112.38 2021-02-13T00:28:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.22 , -0.03 2021-02-13T00:28:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176085.86 epoch sec; seqNo:12893; 4059235.89 north; 592598.32 east 2021-02-13T00:28:06Z- AHRS_M2 msg: 1613176086.22 epoch sec; seqNo:12790 2021-02-13T00:28:06Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.23 psi 2021-02-13T00:28:06Z- handleDepth()-Depth_Keller msg: 1613176086239 epoch sec; seqNo:12894; depth 18.55 2021-02-13T00:28:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176086.25 epoch sec; seqNo:12894; 4059236.27 north; 592598.32 east 2021-02-13T00:28:06Z- AHRS_M2 msg: 1613176086.62 epoch sec; seqNo:12791 2021-02-13T00:28:06Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.22 psi 2021-02-13T00:28:06Z- handleDepth()-Depth_Keller msg: 1613176086628 epoch sec; seqNo:12895; depth 18.68 2021-02-13T00:28:06Z- handleDvl() - RDI_Pathfinder msg: 1613176086.64 epoch sec; seqNo:6309 2021-02-13T00:28:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 100.21 , 78.58 , 111.03 2021-02-13T00:28:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.18 , 0.10 2021-02-13T00:28:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176086.66 epoch sec; seqNo:12895; 4059236.65 north; 592598.31 east 2021-02-13T00:28:06Z- LcmTrn::handleMessages() - handling 41 messages took 0.57 ms and 130 calls... 2021-02-13T00:28:06Z- waiting for 1613176084.82, time is 1613176086.66 2021-02-13T00:28:06Z- LcmTrn::updateTrn() equating pose 1613176086.66 and meas times 1613176086.66 2021-02-13T00:28:06Z- LcmTrn::updateTrn() >>>> pose time 1613176086.66, meas time 1613176086.66 <<<< 2021-02-13T00:28:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176086.66 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:28:06Z- TerrainNav::motionUpdate() - currEstimate = (4059237, 592598.31, 18.68) 2021-02-13T00:28:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:28:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:28:06Z- TNavBF::Projecting Measurements 2021-02-13T00:28:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:28:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:28:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:28:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:28:06Z- TNavFilter::4 good beams. 2021-02-13T00:28:06Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:28:06Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:28:06Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:28:06Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:28:06Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:28:06Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:28:06Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:28:06Z- TNavBF::mmse selected from filter -1 2021-02-13T00:28:06Z- TerrainMapOctree: (vehN, vehE ) = (4059236.65, 592598.31). 2021-02-13T00:28:06Z- TerrainMapOctree: Min Distance = 2604.16. 2021-02-13T00:28:06Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:28:06Z- There is no specific good beam that all particles have in common. 2021-02-13T00:28:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:28:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.35 K: 0.108 ExpectedMeasDiff: nan currMeas.covariance: 9.44 2021-02-13T00:28:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:28:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 16.07 2021-02-13T00:28:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:28:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-13T00:28:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:28:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 19.72 2021-02-13T00:28:06Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:28:06Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:28:06Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:28:06Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:28:06Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:28:06Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:28:06Z- Filter 0 No beams used in subcloud update 2021-02-13T00:28:06Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:28:06Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:28:06Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 701.5 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:28:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176086.66 sec. 2021-02-13T00:28:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:28:06Z- TNavBF::mmse selected from filter -1 2021-02-13T00:28:06Z- variances([0], [2], and psi_berg) = 701.512595 832.998743 0.000000 2021-02-13T00:28:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:28:06Z- LcmTrn::publishEstimates() - MLE : 4059236.15 592647.81 18.68 2021-02-13T00:28:06Z- LcmTrn::publishEstimates() - MMSE : 4059236.65 592598.31 18.68 2021-02-13T00:28:06Z- LcmTrn::publishEstimates() - COVAR: 701.51 833.00 0.98 0.00 2021-02-13T00:28:07Z- AHRS_M2 msg: 1613176087.03 epoch sec; seqNo:12792 2021-02-13T00:28:07Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.22 psi 2021-02-13T00:28:07Z- handleDepth()-Depth_Keller msg: 1613176087037 epoch sec; seqNo:12896; depth 18.82 2021-02-13T00:28:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176087.05 epoch sec; seqNo:12896; 4059236.67 north; 592599.53 east 2021-02-13T00:28:07Z- AHRS_M2 msg: 1613176087.43 epoch sec; seqNo:12793 2021-02-13T00:28:07Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.22 psi 2021-02-13T00:28:07Z- handleDepth()-Depth_Keller msg: 1613176087438 epoch sec; seqNo:12897; depth 18.95 2021-02-13T00:28:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176087.45 epoch sec; seqNo:12897; 4059237.04 north; 592599.53 east 2021-02-13T00:28:07Z- AHRS_M2 msg: 1613176087.84 epoch sec; seqNo:12794 2021-02-13T00:28:07Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.21 psi 2021-02-13T00:28:07Z- handleDepth()-Depth_Keller msg: 1613176087840 epoch sec; seqNo:12898; depth 19.09 2021-02-13T00:28:07Z- handleDvl() - RDI_Pathfinder msg: 1613176087.86 epoch sec; seqNo:6310 2021-02-13T00:28:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.18 , 100.08 , 78.18 , 111.03 2021-02-13T00:28:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.08 , 0.19 2021-02-13T00:28:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176087.87 epoch sec; seqNo:12898; 4059237.42 north; 592599.52 east 2021-02-13T00:28:08Z- AHRS_M2 msg: 1613176088.24 epoch sec; seqNo:12795 2021-02-13T00:28:08Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.21 psi 2021-02-13T00:28:08Z- handleDepth()-Depth_Keller msg: 1613176088249 epoch sec; seqNo:12899; depth 19.20 2021-02-13T00:28:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:28:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:28:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:28:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:28:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:28:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:28:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:28:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:28:08Z- handleDvl() - RDI_Pathfinder msg: 1613176088.25 epoch sec; seqNo:6311 2021-02-13T00:28:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:28:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.08 , 0.19 2021-02-13T00:28:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176088.26 epoch sec; seqNo:12899; 4059237.42 north; 592599.52 east 2021-02-13T00:28:08Z- AHRS_M2 msg: 1613176088.64 epoch sec; seqNo:12796 2021-02-13T00:28:08Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.21 psi 2021-02-13T00:28:08Z- handleDepth()-Depth_Keller msg: 1613176088647 epoch sec; seqNo:12900; depth 19.34 2021-02-13T00:28:08Z- handleDvl() - RDI_Pathfinder msg: 1613176088.66 epoch sec; seqNo:6312 2021-02-13T00:28:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 97.71 , 69.88 , 111.23 2021-02-13T00:28:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.07 , 0.19 2021-02-13T00:28:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176088.67 epoch sec; seqNo:12900; 4059237.80 north; 592599.52 east 2021-02-13T00:28:09Z- AHRS_M2 msg: 1613176089.05 epoch sec; seqNo:12797 2021-02-13T00:28:09Z- AHRS_M2 msg: 0.59 phi; -0.37 theta; -0.21 psi 2021-02-13T00:28:09Z- handleDepth()-Depth_Keller msg: 1613176089054 epoch sec; seqNo:12901; depth 19.48 2021-02-13T00:28:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176089.07 epoch sec; seqNo:12901; 4059237.80 north; 592599.52 east 2021-02-13T00:28:09Z- AHRS_M2 msg: 1613176089.45 epoch sec; seqNo:12798 2021-02-13T00:28:09Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.21 psi 2021-02-13T00:28:09Z- handleDepth()-Depth_Keller msg: 1613176089456 epoch sec; seqNo:12902; depth 19.61 2021-02-13T00:28:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:28:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:28:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:28:09Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:28:09Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:28:09Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:28:09Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:28:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:28:09Z- handleDvl() - RDI_Pathfinder msg: 1613176089.46 epoch sec; seqNo:6313 2021-02-13T00:28:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:28:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.07 , 0.19 2021-02-13T00:28:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176089.48 epoch sec; seqNo:12902; 4059238.18 north; 592599.52 east 2021-02-13T00:28:09Z- AHRS_M2 msg: 1613176089.86 epoch sec; seqNo:12799 2021-02-13T00:28:09Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.21 psi 2021-02-13T00:28:09Z- handleDepth()-Depth_Keller msg: 1613176089860 epoch sec; seqNo:12903; depth 19.74 2021-02-13T00:28:09Z- handleDvl() - RDI_Pathfinder msg: 1613176089.87 epoch sec; seqNo:6314 2021-02-13T00:28:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.42 , 96.63 , 68.24 , 111.23 2021-02-13T00:28:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.15 , 0.10 2021-02-13T00:28:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176089.90 epoch sec; seqNo:12903; 4059238.57 north; 592600.73 east 2021-02-13T00:28:10Z- AHRS_M2 msg: 1613176090.26 epoch sec; seqNo:12800 2021-02-13T00:28:10Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.21 psi 2021-02-13T00:28:10Z- handleDepth()-Depth_Keller msg: 1613176090264 epoch sec; seqNo:12904; depth 19.90 2021-02-13T00:28:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176090.28 epoch sec; seqNo:12904; 4059238.57 north; 592600.73 east 2021-02-13T00:28:10Z- AHRS_M2 msg: 1613176090.66 epoch sec; seqNo:12801 2021-02-13T00:28:10Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.21 psi 2021-02-13T00:28:10Z- handleDepth()-Depth_Keller msg: 1613176090678 epoch sec; seqNo:12905; depth 20.03 2021-02-13T00:28:10Z- handleDvl() - RDI_Pathfinder msg: 1613176090.69 epoch sec; seqNo:6315 2021-02-13T00:28:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.01 , 97.92 , 67.84 , 110.08 2021-02-13T00:28:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.18 , -0.05 2021-02-13T00:28:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176090.70 epoch sec; seqNo:12905; 4059238.95 north; 592600.73 east 2021-02-13T00:28:11Z- AHRS_M2 msg: 1613176091.07 epoch sec; seqNo:12802 2021-02-13T00:28:11Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.20 psi 2021-02-13T00:28:11Z- handleDepth()-Depth_Keller msg: 1613176091077 epoch sec; seqNo:12906; depth 20.18 2021-02-13T00:28:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176091.10 epoch sec; seqNo:12906; 4059239.33 north; 592600.72 east 2021-02-13T00:28:11Z- AHRS_M2 msg: 1613176091.47 epoch sec; seqNo:12803 2021-02-13T00:28:11Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.20 psi 2021-02-13T00:28:11Z- handleDepth()-Depth_Keller msg: 1613176091476 epoch sec; seqNo:12907; depth 20.30 2021-02-13T00:28:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176091.48 epoch sec; seqNo:12907; 4059239.33 north; 592600.72 east 2021-02-13T00:28:11Z- AHRS_M2 msg: 1613176091.88 epoch sec; seqNo:12804 2021-02-13T00:28:11Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.20 psi 2021-02-13T00:28:11Z- handleDepth()-Depth_Keller msg: 1613176091886 epoch sec; seqNo:12908; depth 20.44 2021-02-13T00:28:11Z- handleDvl() - RDI_Pathfinder msg: 1613176091.90 epoch sec; seqNo:6316 2021-02-13T00:28:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 97.92 , 68.65 , 108.87 2021-02-13T00:28:11Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.05 , -0.17 2021-02-13T00:28:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176091.91 epoch sec; seqNo:12908; 4059239.72 north; 592601.94 east 2021-02-13T00:28:11Z- LcmTrn::handleMessages() - handling 46 messages took 0.71 ms and 132 calls... 2021-02-13T00:28:11Z- waiting for 1613176089.66, time is 1613176091.91 2021-02-13T00:28:11Z- LcmTrn::updateTrn() equating pose 1613176091.91 and meas times 1613176091.91 2021-02-13T00:28:11Z- LcmTrn::updateTrn() >>>> pose time 1613176091.91, meas time 1613176091.91 <<<< 2021-02-13T00:28:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176091.91 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:28:11Z- TerrainNav::motionUpdate() - currEstimate = (4059240, 592601.94, 20.44) 2021-02-13T00:28:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:28:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:28:11Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176091.91 sec.; No relevant navigation data available 2021-02-13T00:28:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:28:11Z- TNavBF::mmse selected from filter -1 2021-02-13T00:28:11Z- variances([0], [2], and psi_berg) = 701.452625 832.998711 0.000000 2021-02-13T00:28:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:28:11Z- LcmTrn::publishEstimates() - MLE : 4059239.22 592651.44 20.44 2021-02-13T00:28:11Z- LcmTrn::publishEstimates() - MMSE : 4059239.72 592601.94 20.44 2021-02-13T00:28:11Z- LcmTrn::publishEstimates() - COVAR: 701.45 833.00 1.23 0.00 2021-02-13T00:28:12Z- AHRS_M2 msg: 1613176092.28 epoch sec; seqNo:12805 2021-02-13T00:28:12Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.19 psi 2021-02-13T00:28:12Z- handleDepth()-Depth_Keller msg: 1613176092294 epoch sec; seqNo:12909; depth 20.58 2021-02-13T00:28:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176092.31 epoch sec; seqNo:12909; 4059240.10 north; 592601.94 east 2021-02-13T00:28:12Z- AHRS_M2 msg: 1613176092.68 epoch sec; seqNo:12806 2021-02-13T00:28:12Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.19 psi 2021-02-13T00:28:12Z- handleDepth()-Depth_Keller msg: 1613176092693 epoch sec; seqNo:12910; depth 20.72 2021-02-13T00:28:12Z- handleDvl() - RDI_Pathfinder msg: 1613176092.71 epoch sec; seqNo:6317 2021-02-13T00:28:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.78 , 97.92 , 69.47 , 107.65 2021-02-13T00:28:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.09 , -0.17 2021-02-13T00:28:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176092.72 epoch sec; seqNo:12910; 4059240.48 north; 592601.93 east 2021-02-13T00:28:13Z- AHRS_M2 msg: 1613176093.09 epoch sec; seqNo:12807 2021-02-13T00:28:13Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.18 psi 2021-02-13T00:28:13Z- handleDepth()-Depth_Keller msg: 1613176093092 epoch sec; seqNo:12911; depth 20.86 2021-02-13T00:28:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:28:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:28:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:28:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:28:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:28:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:28:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:28:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:28:13Z- handleDvl() - RDI_Pathfinder msg: 1613176093.09 epoch sec; seqNo:6318 2021-02-13T00:28:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:28:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.09 , -0.17 2021-02-13T00:28:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176093.11 epoch sec; seqNo:12911; 4059240.86 north; 592601.93 east 2021-02-13T00:28:13Z- AHRS_M2 msg: 1613176093.52 epoch sec; seqNo:12808 2021-02-13T00:28:13Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.17 psi 2021-02-13T00:28:13Z- handleDepth()-Depth_Keller msg: 1613176093522 epoch sec; seqNo:12912; depth 20.98 2021-02-13T00:28:13Z- handleDvl() - RDI_Pathfinder msg: 1613176093.53 epoch sec; seqNo:6319 2021-02-13T00:28:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.19 , 97.98 , 67.02 , 105.28 2021-02-13T00:28:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.12 , -0.12 2021-02-13T00:28:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176093.56 epoch sec; seqNo:12912; 4059241.24 north; 592601.93 east 2021-02-13T00:28:13Z- AHRS_M2 msg: 1613176093.90 epoch sec; seqNo:12809 2021-02-13T00:28:13Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.16 psi 2021-02-13T00:28:13Z- handleDepth()-Depth_Keller msg: 1613176093900 epoch sec; seqNo:12913; depth 21.14 2021-02-13T00:28:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176093.92 epoch sec; seqNo:12913; 4059241.24 north; 592601.93 east 2021-02-13T00:28:14Z- AHRS_M2 msg: 1613176094.30 epoch sec; seqNo:12810 2021-02-13T00:28:14Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.16 psi 2021-02-13T00:28:14Z- handleDepth()-Depth_Keller msg: 1613176094304 epoch sec; seqNo:12914; depth 21.25 2021-02-13T00:28:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:28:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:28:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:28:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:28:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:28:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:28:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:28:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:28:14Z- handleDvl() - RDI_Pathfinder msg: 1613176094.31 epoch sec; seqNo:6320 2021-02-13T00:28:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:28:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.12 , -0.12 2021-02-13T00:28:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176094.31 epoch sec; seqNo:12914; 4059241.63 north; 592603.14 east 2021-02-13T00:28:14Z- AHRS_M2 msg: 1613176094.70 epoch sec; seqNo:12811 2021-02-13T00:28:14Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.16 psi 2021-02-13T00:28:14Z- handleDepth()-Depth_Keller msg: 1613176094713 epoch sec; seqNo:12915; depth 21.38 2021-02-13T00:28:14Z- handleDvl() - RDI_Pathfinder msg: 1613176094.72 epoch sec; seqNo:6321 2021-02-13T00:28:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 91.97 , 0.00 , 106.57 2021-02-13T00:28:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.13 , -0.02 2021-02-13T00:28:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176094.74 epoch sec; seqNo:12915; 4059242.01 north; 592603.14 east 2021-02-13T00:28:15Z- AHRS_M2 msg: 1613176095.11 epoch sec; seqNo:12812 2021-02-13T00:28:15Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.17 psi 2021-02-13T00:28:15Z- handleDepth()-Depth_Keller msg: 1613176095121 epoch sec; seqNo:12916; depth 21.50 2021-02-13T00:28:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176095.13 epoch sec; seqNo:12916; 4059242.39 north; 592603.13 east 2021-02-13T00:28:15Z- AHRS_M2 msg: 1613176095.51 epoch sec; seqNo:12813 2021-02-13T00:28:15Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.17 psi 2021-02-13T00:28:15Z- handleDepth()-Depth_Keller msg: 1613176095530 epoch sec; seqNo:12917; depth 21.63 2021-02-13T00:28:15Z- handleDvl() - RDI_Pathfinder msg: 1613176095.57 epoch sec; seqNo:6322 2021-02-13T00:28:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.79 , 90.95 , 66.20 , 105.82 2021-02-13T00:28:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , 0.04 2021-02-13T00:28:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176095.58 epoch sec; seqNo:12917; 4059242.39 north; 592603.13 east 2021-02-13T00:28:15Z- AHRS_M2 msg: 1613176095.92 epoch sec; seqNo:12814 2021-02-13T00:28:15Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.18 psi 2021-02-13T00:28:15Z- handleDepth()-Depth_Keller msg: 1613176095925 epoch sec; seqNo:12918; depth 21.76 2021-02-13T00:28:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176095.94 epoch sec; seqNo:12918; 4059242.77 north; 592603.13 east 2021-02-13T00:28:16Z- AHRS_M2 msg: 1613176096.32 epoch sec; seqNo:12815 2021-02-13T00:28:16Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.19 psi 2021-02-13T00:28:16Z- handleDepth()-Depth_Keller msg: 1613176096324 epoch sec; seqNo:12919; depth 21.88 2021-02-13T00:28:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176096.34 epoch sec; seqNo:12919; 4059243.15 north; 592603.13 east 2021-02-13T00:28:16Z- AHRS_M2 msg: 1613176096.72 epoch sec; seqNo:12816 2021-02-13T00:28:16Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.19 psi 2021-02-13T00:28:16Z- handleDepth()-Depth_Keller msg: 1613176096733 epoch sec; seqNo:12920; depth 22.00 2021-02-13T00:28:16Z- handleDvl() - RDI_Pathfinder msg: 1613176096.75 epoch sec; seqNo:6323 2021-02-13T00:28:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 92.10 , 67.84 , 104.27 2021-02-13T00:28:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.06 2021-02-13T00:28:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176096.78 epoch sec; seqNo:12920; 4059243.15 north; 592603.13 east 2021-02-13T00:28:16Z- LcmTrn::handleMessages() - handling 43 messages took 0.71 ms and 129 calls... 2021-02-13T00:28:16Z- waiting for 1613176094.91, time is 1613176096.78 2021-02-13T00:28:16Z- LcmTrn::updateTrn() equating pose 1613176096.78 and meas times 1613176096.78 2021-02-13T00:28:16Z- LcmTrn::updateTrn() >>>> pose time 1613176096.78, meas time 1613176096.78 <<<< 2021-02-13T00:28:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176096.78 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:28:16Z- TerrainNav::motionUpdate() - currEstimate = (4059243, 592603.13, 22.00) 2021-02-13T00:28:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:28:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:28:16Z- TNavBF::Projecting Measurements 2021-02-13T00:28:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:28:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:28:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:28:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:28:16Z- TNavFilter::4 good beams. 2021-02-13T00:28:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:28:16Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:28:16Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:28:16Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:28:16Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:28:16Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:28:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:28:16Z- TNavBF::mmse selected from filter -1 2021-02-13T00:28:16Z- TerrainMapOctree: (vehN, vehE ) = (4059243.15, 592603.13). 2021-02-13T00:28:16Z- TerrainMapOctree: Min Distance = 2596.09. 2021-02-13T00:28:16Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:28:17Z- There is no specific good beam that all particles have in common. 2021-02-13T00:28:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:28:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.41 K: 0.119 ExpectedMeasDiff: nan currMeas.covariance: 8.75 2021-02-13T00:28:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:28:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 13.57 2021-02-13T00:28:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:28:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 7.36 2021-02-13T00:28:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:28:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 17.40 2021-02-13T00:28:17Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:28:17Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:28:17Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:28:17Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:28:17Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:28:17Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:28:17Z- Filter 0 No beams used in subcloud update 2021-02-13T00:28:17Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:28:17Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:28:17Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 701.4 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:28:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176096.78 sec. 2021-02-13T00:28:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:28:17Z- TNavBF::mmse selected from filter -1 2021-02-13T00:28:17Z- variances([0], [2], and psi_berg) = 701.400315 832.998683 0.000000 2021-02-13T00:28:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:28:17Z- LcmTrn::publishEstimates() - MLE : 4059242.65 592573.63 22.00 2021-02-13T00:28:17Z- LcmTrn::publishEstimates() - MMSE : 4059243.15 592603.13 22.00 2021-02-13T00:28:17Z- LcmTrn::publishEstimates() - COVAR: 701.40 833.00 0.97 0.00 2021-02-13T00:28:17Z- AHRS_M2 msg: 1613176097.13 epoch sec; seqNo:12817 2021-02-13T00:28:17Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:28:17Z- handleDepth()-Depth_Keller msg: 1613176097137 epoch sec; seqNo:12921; depth 22.13 2021-02-13T00:28:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176097.15 epoch sec; seqNo:12921; 4059243.52 north; 592603.12 east 2021-02-13T00:28:17Z- AHRS_M2 msg: 1613176097.53 epoch sec; seqNo:12818 2021-02-13T00:28:17Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.21 psi 2021-02-13T00:28:17Z- handleDepth()-Depth_Keller msg: 1613176097545 epoch sec; seqNo:12922; depth 22.25 2021-02-13T00:28:17Z- handleDvl() - RDI_Pathfinder msg: 1613176097.56 epoch sec; seqNo:6324 2021-02-13T00:28:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 98.25 , 67.84 , 105.56 2021-02-13T00:28:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.06 2021-02-13T00:28:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176097.57 epoch sec; seqNo:12922; 4059243.92 north; 592604.34 east 2021-02-13T00:28:17Z- AHRS_M2 msg: 1613176097.93 epoch sec; seqNo:12819 2021-02-13T00:28:17Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.21 psi 2021-02-13T00:28:17Z- handleDepth()-Depth_Keller msg: 1613176097950 epoch sec; seqNo:12923; depth 22.37 2021-02-13T00:28:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:28:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:28:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:28:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:28:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:28:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:28:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:28:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:28:17Z- handleDvl() - RDI_Pathfinder msg: 1613176097.95 epoch sec; seqNo:6325 2021-02-13T00:28:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:28:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.06 2021-02-13T00:28:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176097.96 epoch sec; seqNo:12923; 4059243.92 north; 592604.34 east 2021-02-13T00:28:18Z- AHRS_M2 msg: 1613176098.34 epoch sec; seqNo:12820 2021-02-13T00:28:18Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.21 psi 2021-02-13T00:28:18Z- handleDepth()-Depth_Keller msg: 1613176098344 epoch sec; seqNo:12924; depth 22.49 2021-02-13T00:28:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176098.35 epoch sec; seqNo:12924; 4059244.30 north; 592604.33 east 2021-02-13T00:28:18Z- AHRS_M2 msg: 1613176098.74 epoch sec; seqNo:12821 2021-02-13T00:28:18Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.22 psi 2021-02-13T00:28:18Z- handleDepth()-Depth_Keller msg: 1613176098751 epoch sec; seqNo:12925; depth 22.60 2021-02-13T00:28:18Z- handleDvl() - RDI_Pathfinder msg: 1613176098.76 epoch sec; seqNo:6326 2021-02-13T00:28:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 67.84 , 104.14 2021-02-13T00:28:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , 0.03 2021-02-13T00:28:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176098.77 epoch sec; seqNo:12925; 4059244.67 north; 592604.33 east 2021-02-13T00:28:19Z- AHRS_M2 msg: 1613176099.15 epoch sec; seqNo:12822 2021-02-13T00:28:19Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.22 psi 2021-02-13T00:28:19Z- handleDepth()-Depth_Keller msg: 1613176099161 epoch sec; seqNo:12926; depth 22.74 2021-02-13T00:28:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:28:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:28:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:28:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:28:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:28:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:28:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:28:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:28:19Z- handleDvl() - RDI_Pathfinder msg: 1613176099.16 epoch sec; seqNo:6327 2021-02-13T00:28:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:28:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , 0.03 2021-02-13T00:28:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176099.17 epoch sec; seqNo:12926; 4059245.05 north; 592604.33 east 2021-02-13T00:28:19Z- AHRS_M2 msg: 1613176099.55 epoch sec; seqNo:12823 2021-02-13T00:28:19Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.21 psi 2021-02-13T00:28:19Z- handleDepth()-Depth_Keller msg: 1613176099556 epoch sec; seqNo:12927; depth 22.87 2021-02-13T00:28:19Z- handleDvl() - RDI_Pathfinder msg: 1613176099.57 epoch sec; seqNo:6328 2021-02-13T00:28:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 93.12 , 67.02 , 102.85 2021-02-13T00:28:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , 0.01 2021-02-13T00:28:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176099.59 epoch sec; seqNo:12927; 4059245.43 north; 592604.32 east 2021-02-13T00:28:19Z- AHRS_M2 msg: 1613176099.96 epoch sec; seqNo:12824 2021-02-13T00:28:19Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.21 psi 2021-02-13T00:28:19Z- handleDepth()-Depth_Keller msg: 1613176099965 epoch sec; seqNo:12928; depth 23.00 2021-02-13T00:28:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176099.98 epoch sec; seqNo:12928; 4059245.45 north; 592605.54 east 2021-02-13T00:28:20Z- AHRS_M2 msg: 1613176100.36 epoch sec; seqNo:12825 2021-02-13T00:28:20Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.21 psi 2021-02-13T00:28:20Z- handleDepth()-Depth_Keller msg: 1613176100377 epoch sec; seqNo:12929; depth 23.12 2021-02-13T00:28:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:28:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:28:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:28:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:28:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:28:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:28:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:28:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:28:20Z- handleDvl() - RDI_Pathfinder msg: 1613176100.38 epoch sec; seqNo:6329 2021-02-13T00:28:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:28:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , 0.01 2021-02-13T00:28:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176100.39 epoch sec; seqNo:12929; 4059245.82 north; 592605.54 east 2021-02-13T00:28:20Z- AHRS_M2 msg: 1613176100.77 epoch sec; seqNo:12826 2021-02-13T00:28:20Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.21 psi 2021-02-13T00:28:20Z- handleDepth()-Depth_Keller msg: 1613176100773 epoch sec; seqNo:12930; depth 23.23 2021-02-13T00:28:20Z- handleDvl() - RDI_Pathfinder msg: 1613176100.80 epoch sec; seqNo:6330 2021-02-13T00:28:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 92.24 , 66.20 , 100.75 2021-02-13T00:28:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.08 , 0.02 2021-02-13T00:28:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176100.81 epoch sec; seqNo:12930; 4059246.20 north; 592605.53 east 2021-02-13T00:28:21Z- AHRS_M2 msg: 1613176101.17 epoch sec; seqNo:12827 2021-02-13T00:28:21Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.20 psi 2021-02-13T00:28:21Z- handleDepth()-Depth_Keller msg: 1613176101181 epoch sec; seqNo:12931; depth 23.38 2021-02-13T00:28:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176101.19 epoch sec; seqNo:12931; 4059246.58 north; 592605.53 east 2021-02-13T00:28:21Z- AHRS_M2 msg: 1613176101.57 epoch sec; seqNo:12828 2021-02-13T00:28:21Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.20 psi 2021-02-13T00:28:21Z- handleDepth()-Depth_Keller msg: 1613176101582 epoch sec; seqNo:12932; depth 23.48 2021-02-13T00:28:21Z- handleDvl() - RDI_Pathfinder msg: 1613176101.59 epoch sec; seqNo:6331 2021-02-13T00:28:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 98.39 , 65.79 , 103.26 2021-02-13T00:28:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , 0.06 2021-02-13T00:28:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176101.60 epoch sec; seqNo:12932; 4059246.58 north; 592605.53 east 2021-02-13T00:28:21Z- AHRS_M2 msg: 1613176101.98 epoch sec; seqNo:12829 2021-02-13T00:28:21Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.20 psi 2021-02-13T00:28:21Z- handleDepth()-Depth_Keller msg: 1613176101985 epoch sec; seqNo:12933; depth 23.60 2021-02-13T00:28:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176102.00 epoch sec; seqNo:12933; 4059246.96 north; 592605.53 east 2021-02-13T00:28:22Z- LcmTrn::handleMessages() - handling 47 messages took 0.67 ms and 134 calls... 2021-02-13T00:28:22Z- waiting for 1613176099.78, time is 1613176102.00 2021-02-13T00:28:22Z- LcmTrn::updateTrn() >>>> pose time 1613176102.00, meas time 1613176101.59 <<<< 2021-02-13T00:28:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176101.59 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:28:22Z- TerrainNav::motionUpdate() - currEstimate = (4059247, 592605.53, 23.60) 2021-02-13T00:28:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:28:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:28:22Z- TNavBF::Projecting Measurements 2021-02-13T00:28:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:28:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:28:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:28:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:28:22Z- TNavFilter::4 good beams. 2021-02-13T00:28:22Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:28:22Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:28:22Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:28:22Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:28:22Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:28:22Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:28:22Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:28:22Z- TNavBF::mmse selected from filter -1 2021-02-13T00:28:22Z- TerrainMapOctree: (vehN, vehE ) = (4059246.67, 592605.34). 2021-02-13T00:28:22Z- TerrainMapOctree: Min Distance = 2591.96. 2021-02-13T00:28:22Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:28:22Z- There is no specific good beam that all particles have in common. 2021-02-13T00:28:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:28:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-13T00:28:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:28:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 15.49 2021-02-13T00:28:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:28:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 6.93 2021-02-13T00:28:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:28:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 17.06 2021-02-13T00:28:22Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:28:22Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:28:22Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:28:22Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:28:22Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:28:22Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:28:22Z- Filter 0 No beams used in subcloud update 2021-02-13T00:28:22Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:28:22Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:28:22Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 701.3 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:28:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176101.59 sec. 2021-02-13T00:28:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:28:22Z- TNavBF::mmse selected from filter -1 2021-02-13T00:28:22Z- variances([0], [2], and psi_berg) = 701.328086 832.998645 0.000000 2021-02-13T00:28:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:28:22Z- LcmTrn::publishEstimates() - MLE : 4059246.46 592576.03 23.60 2021-02-13T00:28:22Z- LcmTrn::publishEstimates() - MMSE : 4059246.96 592605.53 23.60 2021-02-13T00:28:22Z- LcmTrn::publishEstimates() - COVAR: 701.33 833.00 0.87 0.00 2021-02-13T00:28:22Z- AHRS_M2 msg: 1613176102.38 epoch sec; seqNo:12830 2021-02-13T00:28:22Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.19 psi 2021-02-13T00:28:22Z- handleDepth()-Depth_Keller msg: 1613176102384 epoch sec; seqNo:12934; depth 23.72 2021-02-13T00:28:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176102.39 epoch sec; seqNo:12934; 4059247.34 north; 592605.52 east 2021-02-13T00:28:22Z- AHRS_M2 msg: 1613176102.78 epoch sec; seqNo:12831 2021-02-13T00:28:22Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.19 psi 2021-02-13T00:28:22Z- handleDepth()-Depth_Keller msg: 1613176102797 epoch sec; seqNo:12935; depth 23.83 2021-02-13T00:28:22Z- handleDvl() - RDI_Pathfinder msg: 1613176102.81 epoch sec; seqNo:6332 2021-02-13T00:28:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 98.19 , 65.79 , 99.40 2021-02-13T00:28:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , 0.09 2021-02-13T00:28:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176102.82 epoch sec; seqNo:12935; 4059247.72 north; 592605.52 east 2021-02-13T00:28:23Z- AHRS_M2 msg: 1613176103.19 epoch sec; seqNo:12832 2021-02-13T00:28:23Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.19 psi 2021-02-13T00:28:23Z- handleDepth()-Depth_Keller msg: 1613176103192 epoch sec; seqNo:12936; depth 23.96 2021-02-13T00:28:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176103.21 epoch sec; seqNo:12936; 4059247.73 north; 592606.74 east 2021-02-13T00:28:23Z- AHRS_M2 msg: 1613176103.59 epoch sec; seqNo:12833 2021-02-13T00:28:23Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.19 psi 2021-02-13T00:28:23Z- handleDepth()-Depth_Keller msg: 1613176103601 epoch sec; seqNo:12937; depth 24.07 2021-02-13T00:28:23Z- handleDvl() - RDI_Pathfinder msg: 1613176103.61 epoch sec; seqNo:6333 2021-02-13T00:28:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 0.00 , 65.38 , 99.54 2021-02-13T00:28:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , 0.09 2021-02-13T00:28:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176103.63 epoch sec; seqNo:12937; 4059248.11 north; 592606.73 east 2021-02-13T00:28:23Z- AHRS_M2 msg: 1613176104.00 epoch sec; seqNo:12834 2021-02-13T00:28:23Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.18 psi 2021-02-13T00:28:24Z- handleDepth()-Depth_Keller msg: 1613176104000 epoch sec; seqNo:12938; depth 24.19 2021-02-13T00:28:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176104.01 epoch sec; seqNo:12938; 4059248.49 north; 592606.73 east 2021-02-13T00:28:24Z- AHRS_M2 msg: 1613176104.40 epoch sec; seqNo:12835 2021-02-13T00:28:24Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.18 psi 2021-02-13T00:28:24Z- handleDepth()-Depth_Keller msg: 1613176104406 epoch sec; seqNo:12939; depth 24.29 2021-02-13T00:28:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176104.41 epoch sec; seqNo:12939; 4059248.49 north; 592606.73 east 2021-02-13T00:28:24Z- AHRS_M2 msg: 1613176104.80 epoch sec; seqNo:12836 2021-02-13T00:28:24Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:24Z- handleDepth()-Depth_Keller msg: 1613176104808 epoch sec; seqNo:12940; depth 24.42 2021-02-13T00:28:24Z- handleDvl() - RDI_Pathfinder msg: 1613176104.83 epoch sec; seqNo:6334 2021-02-13T00:28:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 88.11 , 66.61 , 100.01 2021-02-13T00:28:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.12 , 0.03 2021-02-13T00:28:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176104.84 epoch sec; seqNo:12940; 4059248.87 north; 592606.73 east 2021-02-13T00:28:25Z- AHRS_M2 msg: 1613176105.21 epoch sec; seqNo:12837 2021-02-13T00:28:25Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:25Z- handleDepth()-Depth_Keller msg: 1613176105212 epoch sec; seqNo:12941; depth 24.52 2021-02-13T00:28:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176105.22 epoch sec; seqNo:12941; 4059249.26 north; 592607.94 east 2021-02-13T00:28:25Z- AHRS_M2 msg: 1613176105.61 epoch sec; seqNo:12838 2021-02-13T00:28:25Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:25Z- handleDepth()-Depth_Keller msg: 1613176105621 epoch sec; seqNo:12942; depth 24.64 2021-02-13T00:28:25Z- handleDvl() - RDI_Pathfinder msg: 1613176105.63 epoch sec; seqNo:6335 2021-02-13T00:28:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 0.00 , 65.38 , 98.19 2021-02-13T00:28:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.07 , -0.05 2021-02-13T00:28:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176105.64 epoch sec; seqNo:12942; 4059249.64 north; 592607.94 east 2021-02-13T00:28:26Z- AHRS_M2 msg: 1613176106.02 epoch sec; seqNo:12839 2021-02-13T00:28:26Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:26Z- handleDepth()-Depth_Keller msg: 1613176106022 epoch sec; seqNo:12943; depth 24.75 2021-02-13T00:28:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176106.03 epoch sec; seqNo:12943; 4059249.64 north; 592607.94 east 2021-02-13T00:28:26Z- AHRS_M2 msg: 1613176106.42 epoch sec; seqNo:12840 2021-02-13T00:28:26Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:26Z- handleDepth()-Depth_Keller msg: 1613176106440 epoch sec; seqNo:12944; depth 24.86 2021-02-13T00:28:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176106.45 epoch sec; seqNo:12944; 4059250.02 north; 592607.93 east 2021-02-13T00:28:26Z- AHRS_M2 msg: 1613176106.82 epoch sec; seqNo:12841 2021-02-13T00:28:26Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:26Z- handleDepth()-Depth_Keller msg: 1613176106833 epoch sec; seqNo:12945; depth 24.96 2021-02-13T00:28:26Z- handleDvl() - RDI_Pathfinder msg: 1613176106.85 epoch sec; seqNo:6336 2021-02-13T00:28:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 88.25 , 65.79 , 97.98 2021-02-13T00:28:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , -0.12 2021-02-13T00:28:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176106.87 epoch sec; seqNo:12945; 4059250.40 north; 592607.93 east 2021-02-13T00:28:27Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-13T00:28:27Z- waiting for 1613176105.00, time is 1613176106.87 2021-02-13T00:28:27Z- LcmTrn::updateTrn() equating pose 1613176106.87 and meas times 1613176106.87 2021-02-13T00:28:27Z- LcmTrn::updateTrn() >>>> pose time 1613176106.87, meas time 1613176106.87 <<<< 2021-02-13T00:28:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176106.87 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:28:27Z- TerrainNav::motionUpdate() - currEstimate = (4059250, 592607.93, 24.96) 2021-02-13T00:28:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:28:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:28:27Z- TNavBF::Projecting Measurements 2021-02-13T00:28:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:28:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:28:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:28:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:28:27Z- TNavFilter::4 good beams. 2021-02-13T00:28:27Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:28:27Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:28:27Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:28:27Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:28:27Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:28:27Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:28:27Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:28:27Z- TNavBF::mmse selected from filter -1 2021-02-13T00:28:27Z- TerrainMapOctree: (vehN, vehE ) = (4059250.40, 592607.93). 2021-02-13T00:28:27Z- TerrainMapOctree: Min Distance = 2587.43. 2021-02-13T00:28:27Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:28:27Z- There is no specific good beam that all particles have in common. 2021-02-13T00:28:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:28:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:28:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:28:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 12.46 2021-02-13T00:28:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:28:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 6.93 2021-02-13T00:28:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:28:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:28:27Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:28:27Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:28:27Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:28:27Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:28:27Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:28:27Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:28:27Z- Filter 0 No beams used in subcloud update 2021-02-13T00:28:27Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:28:27Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:28:27Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 701.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:28:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176106.87 sec. 2021-02-13T00:28:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:28:27Z- TNavBF::mmse selected from filter -1 2021-02-13T00:28:27Z- variances([0], [2], and psi_berg) = 701.271871 832.998615 0.000000 2021-02-13T00:28:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:28:27Z- LcmTrn::publishEstimates() - MLE : 4059249.90 592579.43 24.96 2021-02-13T00:28:27Z- LcmTrn::publishEstimates() - MMSE : 4059250.40 592607.93 24.96 2021-02-13T00:28:27Z- LcmTrn::publishEstimates() - COVAR: 701.27 833.00 0.80 0.00 2021-02-13T00:28:27Z- AHRS_M2 msg: 1613176107.23 epoch sec; seqNo:12842 2021-02-13T00:28:27Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:27Z- handleDepth()-Depth_Keller msg: 1613176107233 epoch sec; seqNo:12946; depth 25.09 2021-02-13T00:28:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176107.24 epoch sec; seqNo:12946; 4059250.78 north; 592607.93 east 2021-02-13T00:28:27Z- AHRS_M2 msg: 1613176107.63 epoch sec; seqNo:12843 2021-02-13T00:28:27Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:27Z- handleDepth()-Depth_Keller msg: 1613176107637 epoch sec; seqNo:12947; depth 25.19 2021-02-13T00:28:27Z- handleDvl() - RDI_Pathfinder msg: 1613176107.65 epoch sec; seqNo:6337 2021-02-13T00:28:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 87.44 , 78.92 , 98.39 2021-02-13T00:28:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.11 , -0.05 2021-02-13T00:28:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176107.66 epoch sec; seqNo:12947; 4059250.79 north; 592609.15 east 2021-02-13T00:28:28Z- AHRS_M2 msg: 1613176108.04 epoch sec; seqNo:12844 2021-02-13T00:28:28Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:28Z- handleDepth()-Depth_Keller msg: 1613176108050 epoch sec; seqNo:12948; depth 25.29 2021-02-13T00:28:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176108.06 epoch sec; seqNo:12948; 4059251.17 north; 592609.14 east 2021-02-13T00:28:28Z- AHRS_M2 msg: 1613176108.46 epoch sec; seqNo:12845 2021-02-13T00:28:28Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:28Z- handleDepth()-Depth_Keller msg: 1613176108468 epoch sec; seqNo:12949; depth 25.43 2021-02-13T00:28:28Z- handleDvl() - RDI_Pathfinder msg: 1613176108.48 epoch sec; seqNo:6338 2021-02-13T00:28:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 88.59 , 64.97 , 98.32 2021-02-13T00:28:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.16 , 0.00 2021-02-13T00:28:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176108.49 epoch sec; seqNo:12949; 4059251.55 north; 592609.14 east 2021-02-13T00:28:28Z- AHRS_M2 msg: 1613176108.85 epoch sec; seqNo:12846 2021-02-13T00:28:28Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:28Z- handleDepth()-Depth_Keller msg: 1613176108849 epoch sec; seqNo:12950; depth 25.53 2021-02-13T00:28:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176108.86 epoch sec; seqNo:12950; 4059251.93 north; 592609.13 east 2021-02-13T00:28:29Z- AHRS_M2 msg: 1613176109.25 epoch sec; seqNo:12847 2021-02-13T00:28:29Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:29Z- handleDepth()-Depth_Keller msg: 1613176109262 epoch sec; seqNo:12951; depth 25.63 2021-02-13T00:28:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176109.27 epoch sec; seqNo:12951; 4059252.30 north; 592609.13 east 2021-02-13T00:28:29Z- AHRS_M2 msg: 1613176109.65 epoch sec; seqNo:12848 2021-02-13T00:28:29Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:29Z- handleDepth()-Depth_Keller msg: 1613176109656 epoch sec; seqNo:12952; depth 25.73 2021-02-13T00:28:29Z- handleDvl() - RDI_Pathfinder msg: 1613176109.67 epoch sec; seqNo:6339 2021-02-13T00:28:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 87.10 , 63.75 , 97.92 2021-02-13T00:28:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.13 , 0.08 2021-02-13T00:28:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176109.69 epoch sec; seqNo:12952; 4059252.30 north; 592609.13 east 2021-02-13T00:28:30Z- AHRS_M2 msg: 1613176110.06 epoch sec; seqNo:12849 2021-02-13T00:28:30Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:30Z- handleDepth()-Depth_Keller msg: 1613176110060 epoch sec; seqNo:12953; depth 25.85 2021-02-13T00:28:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176110.07 epoch sec; seqNo:12953; 4059252.68 north; 592609.13 east 2021-02-13T00:28:30Z- AHRS_M2 msg: 1613176110.48 epoch sec; seqNo:12850 2021-02-13T00:28:30Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:30Z- handleDepth()-Depth_Keller msg: 1613176110488 epoch sec; seqNo:12954; depth 25.94 2021-02-13T00:28:30Z- handleDvl() - RDI_Pathfinder msg: 1613176110.50 epoch sec; seqNo:6340 2021-02-13T00:28:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 87.17 , 64.57 , 97.98 2021-02-13T00:28:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , 0.11 2021-02-13T00:28:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176110.53 epoch sec; seqNo:12954; 4059253.06 north; 592609.12 east 2021-02-13T00:28:30Z- AHRS_M2 msg: 1613176110.86 epoch sec; seqNo:12851 2021-02-13T00:28:30Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:30Z- handleDepth()-Depth_Keller msg: 1613176110868 epoch sec; seqNo:12955; depth 26.05 2021-02-13T00:28:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176110.88 epoch sec; seqNo:12955; 4059253.45 north; 592610.34 east 2021-02-13T00:28:31Z- AHRS_M2 msg: 1613176111.27 epoch sec; seqNo:12852 2021-02-13T00:28:31Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:31Z- handleDepth()-Depth_Keller msg: 1613176111276 epoch sec; seqNo:12956; depth 26.16 2021-02-13T00:28:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176111.29 epoch sec; seqNo:12956; 4059253.45 north; 592610.34 east 2021-02-13T00:28:31Z- AHRS_M2 msg: 1613176111.67 epoch sec; seqNo:12853 2021-02-13T00:28:31Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:31Z- handleDepth()-Depth_Keller msg: 1613176111681 epoch sec; seqNo:12957; depth 26.27 2021-02-13T00:28:31Z- handleDvl() - RDI_Pathfinder msg: 1613176111.69 epoch sec; seqNo:6341 2021-02-13T00:28:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 86.09 , 62.93 , 97.98 2021-02-13T00:28:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , 0.09 2021-02-13T00:28:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176111.70 epoch sec; seqNo:12957; 4059253.83 north; 592610.33 east 2021-02-13T00:28:32Z- AHRS_M2 msg: 1613176112.08 epoch sec; seqNo:12854 2021-02-13T00:28:32Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:32Z- handleDepth()-Depth_Keller msg: 1613176112087 epoch sec; seqNo:12958; depth 26.37 2021-02-13T00:28:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176112.10 epoch sec; seqNo:12958; 4059254.21 north; 592610.33 east 2021-02-13T00:28:32Z- LcmTrn::handleMessages() - handling 44 messages took 0.60 ms and 132 calls... 2021-02-13T00:28:32Z- waiting for 1613176109.87, time is 1613176112.10 2021-02-13T00:28:32Z- LcmTrn::updateTrn() >>>> pose time 1613176112.10, meas time 1613176111.69 <<<< 2021-02-13T00:28:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176111.69 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:28:32Z- TerrainNav::motionUpdate() - currEstimate = (4059254, 592610.33, 26.37) 2021-02-13T00:28:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:28:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:28:32Z- TNavBF::Projecting Measurements 2021-02-13T00:28:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:28:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:28:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:28:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:28:32Z- TNavFilter::4 good beams. 2021-02-13T00:28:32Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:28:32Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:28:32Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:28:32Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:28:32Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:28:32Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:28:32Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:28:32Z- TNavBF::mmse selected from filter -1 2021-02-13T00:28:32Z- TerrainMapOctree: (vehN, vehE ) = (4059253.92, 592610.14). 2021-02-13T00:28:32Z- TerrainMapOctree: Min Distance = 2583.31. 2021-02-13T00:28:32Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:28:32Z- There is no specific good beam that all particles have in common. 2021-02-13T00:28:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:28:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-13T00:28:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:28:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 11.86 2021-02-13T00:28:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:28:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 6.34 2021-02-13T00:28:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:28:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.77 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:28:32Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:28:32Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:28:32Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:28:32Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:28:32Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:28:32Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:28:32Z- Filter 0 No beams used in subcloud update 2021-02-13T00:28:32Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:28:32Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:28:32Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 701.2 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:28:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176111.69 sec. 2021-02-13T00:28:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:28:32Z- TNavBF::mmse selected from filter -1 2021-02-13T00:28:32Z- variances([0], [2], and psi_berg) = 701.199720 832.998576 0.000000 2021-02-13T00:28:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:28:32Z- LcmTrn::publishEstimates() - MLE : 4059253.71 592582.83 26.37 2021-02-13T00:28:32Z- LcmTrn::publishEstimates() - MMSE : 4059254.21 592610.33 26.37 2021-02-13T00:28:32Z- LcmTrn::publishEstimates() - COVAR: 701.20 833.00 0.76 0.00 2021-02-13T00:28:32Z- AHRS_M2 msg: 1613176112.50 epoch sec; seqNo:12855 2021-02-13T00:28:32Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:32Z- handleDepth()-Depth_Keller msg: 1613176112501 epoch sec; seqNo:12959; depth 26.48 2021-02-13T00:28:32Z- handleDvl() - RDI_Pathfinder msg: 1613176112.51 epoch sec; seqNo:6342 2021-02-13T00:28:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 86.09 , 62.12 , 97.98 2021-02-13T00:28:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , 0.03 2021-02-13T00:28:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176112.52 epoch sec; seqNo:12959; 4059254.59 north; 592610.33 east 2021-02-13T00:28:32Z- AHRS_M2 msg: 1613176112.88 epoch sec; seqNo:12856 2021-02-13T00:28:32Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:32Z- handleDepth()-Depth_Keller msg: 1613176112889 epoch sec; seqNo:12960; depth 26.61 2021-02-13T00:28:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176112.90 epoch sec; seqNo:12960; 4059254.59 north; 592610.33 east 2021-02-13T00:28:33Z- AHRS_M2 msg: 1613176113.29 epoch sec; seqNo:12857 2021-02-13T00:28:33Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:33Z- handleDepth()-Depth_Keller msg: 1613176113297 epoch sec; seqNo:12961; depth 26.70 2021-02-13T00:28:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176113.31 epoch sec; seqNo:12961; 4059254.97 north; 592610.32 east 2021-02-13T00:28:33Z- AHRS_M2 msg: 1613176113.69 epoch sec; seqNo:12858 2021-02-13T00:28:33Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:33Z- handleDepth()-Depth_Keller msg: 1613176113705 epoch sec; seqNo:12962; depth 26.82 2021-02-13T00:28:33Z- handleDvl() - RDI_Pathfinder msg: 1613176113.72 epoch sec; seqNo:6343 2021-02-13T00:28:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 85.00 , 63.75 , 97.98 2021-02-13T00:28:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , -0.00 2021-02-13T00:28:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176113.73 epoch sec; seqNo:12962; 4059255.36 north; 592611.54 east 2021-02-13T00:28:34Z- AHRS_M2 msg: 1613176114.10 epoch sec; seqNo:12859 2021-02-13T00:28:34Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:28:34Z- handleDepth()-Depth_Keller msg: 1613176114110 epoch sec; seqNo:12963; depth 26.92 2021-02-13T00:28:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176114.12 epoch sec; seqNo:12963; 4059255.74 north; 592611.53 east 2021-02-13T00:28:34Z- AHRS_M2 msg: 1613176114.50 epoch sec; seqNo:12860 2021-02-13T00:28:34Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-13T00:28:34Z- handleDepth()-Depth_Keller msg: 1613176114504 epoch sec; seqNo:12964; depth 27.03 2021-02-13T00:28:34Z- handleDvl() - RDI_Pathfinder msg: 1613176114.52 epoch sec; seqNo:6344 2021-02-13T00:28:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 86.09 , 62.52 , 97.98 2021-02-13T00:28:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , -0.01 2021-02-13T00:28:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176114.53 epoch sec; seqNo:12964; 4059255.74 north; 592611.53 east 2021-02-13T00:28:34Z- AHRS_M2 msg: 1613176114.90 epoch sec; seqNo:12861 2021-02-13T00:28:34Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-13T00:28:34Z- handleDepth()-Depth_Keller msg: 1613176114913 epoch sec; seqNo:12965; depth 27.16 2021-02-13T00:28:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176114.93 epoch sec; seqNo:12965; 4059256.12 north; 592611.53 east 2021-02-13T00:28:35Z- AHRS_M2 msg: 1613176115.31 epoch sec; seqNo:12862 2021-02-13T00:28:35Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-13T00:28:35Z- handleDepth()-Depth_Keller msg: 1613176115312 epoch sec; seqNo:12966; depth 27.27 2021-02-13T00:28:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176115.33 epoch sec; seqNo:12966; 4059256.50 north; 592611.53 east 2021-02-13T00:28:35Z- AHRS_M2 msg: 1613176115.71 epoch sec; seqNo:12863 2021-02-13T00:28:35Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-13T00:28:35Z- handleDepth()-Depth_Keller msg: 1613176115716 epoch sec; seqNo:12967; depth 27.39 2021-02-13T00:28:35Z- handleDvl() - RDI_Pathfinder msg: 1613176115.73 epoch sec; seqNo:6345 2021-02-13T00:28:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 0.00 , 62.52 , 97.98 2021-02-13T00:28:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , 0.02 2021-02-13T00:28:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176115.75 epoch sec; seqNo:12967; 4059256.88 north; 592611.52 east 2021-02-13T00:28:36Z- AHRS_M2 msg: 1613176116.12 epoch sec; seqNo:12864 2021-02-13T00:28:36Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.18 psi 2021-02-13T00:28:36Z- handleDepth()-Depth_Keller msg: 1613176116122 epoch sec; seqNo:12968; depth 27.49 2021-02-13T00:28:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176116.14 epoch sec; seqNo:12968; 4059257.27 north; 592612.74 east 2021-02-13T00:28:36Z- AHRS_M2 msg: 1613176116.52 epoch sec; seqNo:12865 2021-02-13T00:28:36Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-13T00:28:36Z- handleDepth()-Depth_Keller msg: 1613176116526 epoch sec; seqNo:12969; depth 27.61 2021-02-13T00:28:36Z- handleDvl() - RDI_Pathfinder msg: 1613176116.54 epoch sec; seqNo:6346 2021-02-13T00:28:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 86.09 , 62.93 , 97.98 2021-02-13T00:28:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , 0.05 2021-02-13T00:28:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176116.55 epoch sec; seqNo:12969; 4059257.27 north; 592612.74 east 2021-02-13T00:28:36Z- AHRS_M2 msg: 1613176116.92 epoch sec; seqNo:12866 2021-02-13T00:28:36Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-13T00:28:36Z- handleDepth()-Depth_Keller msg: 1613176116928 epoch sec; seqNo:12970; depth 27.73 2021-02-13T00:28:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176116.96 epoch sec; seqNo:12970; 4059257.65 north; 592612.74 east 2021-02-13T00:28:37Z- AHRS_M2 msg: 1613176117.33 epoch sec; seqNo:12867 2021-02-13T00:28:37Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.17 psi 2021-02-13T00:28:37Z- handleDepth()-Depth_Keller msg: 1613176117332 epoch sec; seqNo:12971; depth 27.85 2021-02-13T00:28:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176117.34 epoch sec; seqNo:12971; 4059258.03 north; 592612.73 east 2021-02-13T00:28:37Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:28:37Z- waiting for 1613176115.10, time is 1613176117.34 2021-02-13T00:28:37Z- LcmTrn::updateTrn() >>>> pose time 1613176117.34, meas time 1613176116.54 <<<< 2021-02-13T00:28:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176116.54 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:28:37Z- TerrainNav::motionUpdate() - currEstimate = (4059258, 592612.73, 27.85) 2021-02-13T00:28:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:28:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:28:37Z- TNavBF::Projecting Measurements 2021-02-13T00:28:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:28:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:28:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:28:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:28:37Z- TNavFilter::4 good beams. 2021-02-13T00:28:37Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:28:37Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:28:37Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:28:37Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:28:37Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:28:37Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:28:37Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:28:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:28:37Z- TerrainMapOctree: (vehN, vehE ) = (4059257.44, 592612.36). 2021-02-13T00:28:37Z- TerrainMapOctree: Min Distance = 2579.18. 2021-02-13T00:28:37Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:28:37Z- There is no specific good beam that all particles have in common. 2021-02-13T00:28:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:28:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 7.54 2021-02-13T00:28:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:28:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 11.86 2021-02-13T00:28:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:28:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 6.34 2021-02-13T00:28:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:28:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.74 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:28:37Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:28:37Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:28:37Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:28:37Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:28:37Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:28:37Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:28:37Z- Filter 0 No beams used in subcloud update 2021-02-13T00:28:37Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:28:37Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:28:37Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 701.1 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:28:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176116.54 sec. 2021-02-13T00:28:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:28:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:28:37Z- variances([0], [2], and psi_berg) = 701.123388 832.998536 0.000000 2021-02-13T00:28:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:28:37Z- LcmTrn::publishEstimates() - MLE : 4059257.53 592662.23 27.85 2021-02-13T00:28:37Z- LcmTrn::publishEstimates() - MMSE : 4059258.03 592612.73 27.85 2021-02-13T00:28:37Z- LcmTrn::publishEstimates() - COVAR: 701.12 833.00 0.74 0.00 2021-02-13T00:28:37Z- AHRS_M2 msg: 1613176117.73 epoch sec; seqNo:12868 2021-02-13T00:28:37Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.16 psi 2021-02-13T00:28:37Z- handleDepth()-Depth_Keller msg: 1613176117743 epoch sec; seqNo:12972; depth 27.95 2021-02-13T00:28:37Z- handleDvl() - RDI_Pathfinder msg: 1613176117.76 epoch sec; seqNo:6347 2021-02-13T00:28:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 0.00 , 62.93 , 97.98 2021-02-13T00:28:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , 0.06 2021-02-13T00:28:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176117.78 epoch sec; seqNo:12972; 4059258.41 north; 592612.73 east 2021-02-13T00:28:38Z- AHRS_M2 msg: 1613176118.15 epoch sec; seqNo:12869 2021-02-13T00:28:38Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.17 psi 2021-02-13T00:28:38Z- handleDepth()-Depth_Keller msg: 1613176118152 epoch sec; seqNo:12973; depth 28.09 2021-02-13T00:28:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176118.16 epoch sec; seqNo:12973; 4059258.41 north; 592612.73 east 2021-02-13T00:28:38Z- AHRS_M2 msg: 1613176118.54 epoch sec; seqNo:12870 2021-02-13T00:28:38Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-13T00:28:38Z- handleDepth()-Depth_Keller msg: 1613176118557 epoch sec; seqNo:12974; depth 28.20 2021-02-13T00:28:38Z- handleDvl() - RDI_Pathfinder msg: 1613176118.57 epoch sec; seqNo:6348 2021-02-13T00:28:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.61 , 0.00 , 60.89 , 97.98 2021-02-13T00:28:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , 0.07 2021-02-13T00:28:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176118.58 epoch sec; seqNo:12974; 4059258.80 north; 592613.94 east 2021-02-13T00:28:38Z- AHRS_M2 msg: 1613176118.95 epoch sec; seqNo:12871 2021-02-13T00:28:38Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-13T00:28:38Z- handleDepth()-Depth_Keller msg: 1613176118951 epoch sec; seqNo:12975; depth 28.32 2021-02-13T00:28:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176118.98 epoch sec; seqNo:12975; 4059259.18 north; 592613.94 east 2021-02-13T00:28:39Z- AHRS_M2 msg: 1613176119.35 epoch sec; seqNo:12872 2021-02-13T00:28:39Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-13T00:28:39Z- handleDepth()-Depth_Keller msg: 1613176119352 epoch sec; seqNo:12976; depth 28.44 2021-02-13T00:28:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176119.36 epoch sec; seqNo:12976; 4059259.18 north; 592613.94 east 2021-02-13T00:28:39Z- AHRS_M2 msg: 1613176119.75 epoch sec; seqNo:12873 2021-02-13T00:28:39Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-13T00:28:39Z- handleDepth()-Depth_Keller msg: 1613176119756 epoch sec; seqNo:12977; depth 28.54 2021-02-13T00:28:39Z- handleDvl() - RDI_Pathfinder msg: 1613176119.77 epoch sec; seqNo:6349 2021-02-13T00:28:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 83.92 , 61.30 , 97.98 2021-02-13T00:28:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , 0.07 2021-02-13T00:28:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176119.78 epoch sec; seqNo:12977; 4059259.55 north; 592613.94 east 2021-02-13T00:28:40Z- AHRS_M2 msg: 1613176120.16 epoch sec; seqNo:12874 2021-02-13T00:28:40Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.17 psi 2021-02-13T00:28:40Z- handleDepth()-Depth_Keller msg: 1613176120163 epoch sec; seqNo:12978; depth 28.67 2021-02-13T00:28:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176120.17 epoch sec; seqNo:12978; 4059259.93 north; 592613.93 east 2021-02-13T00:28:40Z- AHRS_M2 msg: 1613176120.56 epoch sec; seqNo:12875 2021-02-13T00:28:40Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.18 psi 2021-02-13T00:28:40Z- handleDepth()-Depth_Keller msg: 1613176120564 epoch sec; seqNo:12979; depth 28.79 2021-02-13T00:28:40Z- handleDvl() - RDI_Pathfinder msg: 1613176120.58 epoch sec; seqNo:6350 2021-02-13T00:28:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 82.84 , 61.30 , 94.74 2021-02-13T00:28:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , 0.05 2021-02-13T00:28:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176120.60 epoch sec; seqNo:12979; 4059259.93 north; 592613.93 east 2021-02-13T00:28:40Z- AHRS_M2 msg: 1613176120.97 epoch sec; seqNo:12876 2021-02-13T00:28:40Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.18 psi 2021-02-13T00:28:40Z- handleDepth()-Depth_Keller msg: 1613176120977 epoch sec; seqNo:12980; depth 28.92 2021-02-13T00:28:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176120.99 epoch sec; seqNo:12980; 4059260.33 north; 592615.15 east 2021-02-13T00:28:41Z- AHRS_M2 msg: 1613176121.37 epoch sec; seqNo:12877 2021-02-13T00:28:41Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-13T00:28:41Z- handleDepth()-Depth_Keller msg: 1613176121381 epoch sec; seqNo:12981; depth 29.02 2021-02-13T00:28:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176121.39 epoch sec; seqNo:12981; 4059260.70 north; 592615.14 east 2021-02-13T00:28:41Z- AHRS_M2 msg: 1613176121.80 epoch sec; seqNo:12878 2021-02-13T00:28:41Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-13T00:28:41Z- handleDepth()-Depth_Keller msg: 1613176121801 epoch sec; seqNo:12982; depth 29.13 2021-02-13T00:28:41Z- handleDvl() - RDI_Pathfinder msg: 1613176121.81 epoch sec; seqNo:6351 2021-02-13T00:28:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 85.00 , 60.07 , 97.98 2021-02-13T00:28:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.06 , 0.03 2021-02-13T00:28:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176121.82 epoch sec; seqNo:12982; 4059261.08 north; 592615.14 east 2021-02-13T00:28:42Z- AHRS_M2 msg: 1613176122.18 epoch sec; seqNo:12879 2021-02-13T00:28:42Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.19 psi 2021-02-13T00:28:42Z- handleDepth()-Depth_Keller msg: 1613176122180 epoch sec; seqNo:12983; depth 29.27 2021-02-13T00:28:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176122.19 epoch sec; seqNo:12983; 4059261.08 north; 592615.14 east 2021-02-13T00:28:42Z- LcmTrn::handleMessages() - handling 41 messages took 0.55 ms and 130 calls... 2021-02-13T00:28:42Z- waiting for 1613176120.34, time is 1613176122.19 2021-02-13T00:28:42Z- LcmTrn::updateTrn() >>>> pose time 1613176122.19, meas time 1613176121.81 <<<< 2021-02-13T00:28:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176121.81 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:28:42Z- TerrainNav::motionUpdate() - currEstimate = (4059261, 592615.14, 29.27) 2021-02-13T00:28:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:28:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:28:42Z- TNavBF::Projecting Measurements 2021-02-13T00:28:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:28:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:28:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:28:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:28:42Z- TNavFilter::4 good beams. 2021-02-13T00:28:42Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:28:42Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:28:42Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:28:42Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:28:42Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:28:42Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:28:42Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:28:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:28:42Z- TerrainMapOctree: (vehN, vehE ) = (4059260.84, 592614.95). 2021-02-13T00:28:42Z- TerrainMapOctree: Min Distance = 2574.90. 2021-02-13T00:28:42Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:28:42Z- There is no specific good beam that all particles have in common. 2021-02-13T00:28:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:28:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 7.27 2021-02-13T00:28:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:28:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 11.56 2021-02-13T00:28:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:28:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.79 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 5.77 2021-02-13T00:28:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:28:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.71 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:28:42Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:28:42Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:28:42Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:28:42Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:28:42Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:28:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:28:42Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:28:42Z- Filter 0 No beams used in subcloud update 2021-02-13T00:28:42Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:28:42Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:28:42Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 701.1 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:28:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176121.81 sec. 2021-02-13T00:28:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:28:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:28:42Z- variances([0], [2], and psi_berg) = 701.056408 832.998500 0.000000 2021-02-13T00:28:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:28:42Z- LcmTrn::publishEstimates() - MLE : 4059260.58 592664.64 29.27 2021-02-13T00:28:42Z- LcmTrn::publishEstimates() - MMSE : 4059261.08 592615.14 29.27 2021-02-13T00:28:42Z- LcmTrn::publishEstimates() - COVAR: 701.06 833.00 0.70 0.00 2021-02-13T00:28:42Z- AHRS_M2 msg: 1613176122.58 epoch sec; seqNo:12880 2021-02-13T00:28:42Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.20 psi 2021-02-13T00:28:42Z- handleDepth()-Depth_Keller msg: 1613176122593 epoch sec; seqNo:12984; depth 29.38 2021-02-13T00:28:42Z- handleDvl() - RDI_Pathfinder msg: 1613176122.60 epoch sec; seqNo:6352 2021-02-13T00:28:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:28:42Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:28:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176122.62 epoch sec; seqNo:12984; 4059261.46 north; 592615.14 east 2021-02-13T00:28:42Z- AHRS_M2 msg: 1613176122.98 epoch sec; seqNo:12881 2021-02-13T00:28:42Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.20 psi 2021-02-13T00:28:42Z- handleDepth()-Depth_Keller msg: 1613176122993 epoch sec; seqNo:12985; depth 29.51 2021-02-13T00:28:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176123.01 epoch sec; seqNo:12985; 4059261.85 north; 592616.35 east 2021-02-13T00:28:43Z- AHRS_M2 msg: 1613176123.39 epoch sec; seqNo:12882 2021-02-13T00:28:43Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.20 psi 2021-02-13T00:28:43Z- handleDepth()-Depth_Keller msg: 1613176123392 epoch sec; seqNo:12986; depth 29.65 2021-02-13T00:28:43Z- handleDvl() - RDI_Pathfinder msg: 1613176123.40 epoch sec; seqNo:6353 2021-02-13T00:28:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 65.77 , 0.00 , 59.79 , 0.00 2021-02-13T00:28:43Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:28:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176123.42 epoch sec; seqNo:12986; 4059261.85 north; 592616.35 east 2021-02-13T00:28:43Z- AHRS_M2 msg: 1613176123.79 epoch sec; seqNo:12883 2021-02-13T00:28:43Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.20 psi 2021-02-13T00:28:43Z- handleDepth()-Depth_Keller msg: 1613176123797 epoch sec; seqNo:12987; depth 29.77 2021-02-13T00:28:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176123.81 epoch sec; seqNo:12987; 4059262.23 north; 592616.35 east 2021-02-13T00:28:44Z- AHRS_M2 msg: 1613176124.20 epoch sec; seqNo:12884 2021-02-13T00:28:44Z- AHRS_M2 msg: 0.64 phi; -0.27 theta; -0.19 psi 2021-02-13T00:28:44Z- handleDepth()-Depth_Keller msg: 1613176124202 epoch sec; seqNo:12988; depth 29.89 2021-02-13T00:28:44Z- handleDvl() - RDI_Pathfinder msg: 1613176124.21 epoch sec; seqNo:6354 2021-02-13T00:28:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.93 , 0.00 , 61.17 , 0.00 2021-02-13T00:28:44Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:28:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176124.22 epoch sec; seqNo:12988; 4059262.61 north; 592616.34 east 2021-02-13T00:28:44Z- AHRS_M2 msg: 1613176124.60 epoch sec; seqNo:12885 2021-02-13T00:28:44Z- AHRS_M2 msg: 0.63 phi; -0.23 theta; -0.18 psi 2021-02-13T00:28:44Z- handleDepth()-Depth_Keller msg: 1613176124604 epoch sec; seqNo:12989; depth 30.03 2021-02-13T00:28:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176124.63 epoch sec; seqNo:12989; 4059262.61 north; 592616.34 east 2021-02-13T00:28:45Z- AHRS_M2 msg: 1613176125.00 epoch sec; seqNo:12886 2021-02-13T00:28:45Z- AHRS_M2 msg: 0.62 phi; -0.18 theta; -0.17 psi 2021-02-13T00:28:45Z- handleDepth()-Depth_Keller msg: 1613176125008 epoch sec; seqNo:12990; depth 30.16 2021-02-13T00:28:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176125.02 epoch sec; seqNo:12990; 4059263.00 north; 592617.56 east 2021-02-13T00:28:45Z- AHRS_M2 msg: 1613176125.41 epoch sec; seqNo:12887 2021-02-13T00:28:45Z- AHRS_M2 msg: 0.62 phi; -0.14 theta; -0.16 psi 2021-02-13T00:28:45Z- handleDepth()-Depth_Keller msg: 1613176125414 epoch sec; seqNo:12991; depth 30.27 2021-02-13T00:28:45Z- handleDvl() - RDI_Pathfinder msg: 1613176125.43 epoch sec; seqNo:6355 2021-02-13T00:28:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.47 , 0.00 , 0.00 , 0.00 2021-02-13T00:28:45Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:28:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176125.44 epoch sec; seqNo:12991; 4059263.38 north; 592617.56 east 2021-02-13T00:28:45Z- AHRS_M2 msg: 1613176125.81 epoch sec; seqNo:12888 2021-02-13T00:28:45Z- AHRS_M2 msg: 0.61 phi; -0.09 theta; -0.16 psi 2021-02-13T00:28:45Z- handleDepth()-Depth_Keller msg: 1613176125818 epoch sec; seqNo:12992; depth 30.39 2021-02-13T00:28:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176125.83 epoch sec; seqNo:12992; 4059263.76 north; 592617.55 east 2021-02-13T00:28:46Z- AHRS_M2 msg: 1613176126.22 epoch sec; seqNo:12889 2021-02-13T00:28:46Z- AHRS_M2 msg: 0.60 phi; -0.05 theta; -0.16 psi 2021-02-13T00:28:46Z- handleDepth()-Depth_Keller msg: 1613176126221 epoch sec; seqNo:12993; depth 30.46 2021-02-13T00:28:46Z- handleDvl() - RDI_Pathfinder msg: 1613176126.24 epoch sec; seqNo:6356 2021-02-13T00:28:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:28:46Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:28:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176126.25 epoch sec; seqNo:12993; 4059263.76 north; 592617.55 east 2021-02-13T00:28:46Z- AHRS_M2 msg: 1613176126.62 epoch sec; seqNo:12890 2021-02-13T00:28:46Z- AHRS_M2 msg: 0.59 phi; -0.00 theta; -0.16 psi 2021-02-13T00:28:46Z- handleDepth()-Depth_Keller msg: 1613176126624 epoch sec; seqNo:12994; depth 30.54 2021-02-13T00:28:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176126.63 epoch sec; seqNo:12994; 4059264.14 north; 592617.55 east 2021-02-13T00:28:47Z- AHRS_M2 msg: 1613176127.03 epoch sec; seqNo:12891 2021-02-13T00:28:47Z- AHRS_M2 msg: 0.58 phi; 0.04 theta; -0.16 psi 2021-02-13T00:28:47Z- handleDepth()-Depth_Keller msg: 1613176127033 epoch sec; seqNo:12995; depth 30.61 2021-02-13T00:28:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176127.04 epoch sec; seqNo:12995; 4059264.53 north; 592618.76 east 2021-02-13T00:28:47Z- AHRS_M2 msg: 1613176127.43 epoch sec; seqNo:12892 2021-02-13T00:28:47Z- AHRS_M2 msg: 0.57 phi; 0.07 theta; -0.16 psi 2021-02-13T00:28:47Z- handleDepth()-Depth_Keller msg: 1613176127434 epoch sec; seqNo:12996; depth 30.66 2021-02-13T00:28:47Z- handleDvl() - RDI_Pathfinder msg: 1613176127.45 epoch sec; seqNo:6357 2021-02-13T00:28:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.52 , 0.00 , 78.51 , 66.20 2021-02-13T00:28:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , -0.07 2021-02-13T00:28:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176127.46 epoch sec; seqNo:12996; 4059264.91 north; 592618.76 east 2021-02-13T00:28:47Z- LcmTrn::handleMessages() - handling 45 messages took 0.68 ms and 132 calls... 2021-02-13T00:28:47Z- waiting for 1613176125.19, time is 1613176127.46 2021-02-13T00:28:47Z- LcmTrn::updateTrn() equating pose 1613176127.46 and meas times 1613176127.46 2021-02-13T00:28:47Z- LcmTrn::updateTrn() >>>> pose time 1613176127.46, meas time 1613176127.46 <<<< 2021-02-13T00:28:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176127.46 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:28:47Z- TerrainNav::motionUpdate() - currEstimate = (4059265, 592618.76, 30.66) 2021-02-13T00:28:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:28:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:28:47Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176127.46 sec.; No relevant navigation data available 2021-02-13T00:28:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:28:47Z- TNavBF::mmse selected from filter -1 2021-02-13T00:28:47Z- variances([0], [2], and psi_berg) = 700.993578 832.998467 0.000000 2021-02-13T00:28:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:28:47Z- LcmTrn::publishEstimates() - MLE : 4059264.41 592590.26 30.66 2021-02-13T00:28:47Z- LcmTrn::publishEstimates() - MMSE : 4059264.91 592618.76 30.66 2021-02-13T00:28:47Z- LcmTrn::publishEstimates() - COVAR: 700.99 833.00 0.97 0.00 2021-02-13T00:28:47Z- AHRS_M2 msg: 1613176127.83 epoch sec; seqNo:12893 2021-02-13T00:28:47Z- AHRS_M2 msg: 0.56 phi; 0.11 theta; -0.16 psi 2021-02-13T00:28:47Z- handleDepth()-Depth_Keller msg: 1613176127836 epoch sec; seqNo:12997; depth 30.69 2021-02-13T00:28:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176127.86 epoch sec; seqNo:12997; 4059265.29 north; 592618.76 east 2021-02-13T00:28:48Z- AHRS_M2 msg: 1613176128.24 epoch sec; seqNo:12894 2021-02-13T00:28:48Z- AHRS_M2 msg: 0.55 phi; 0.14 theta; -0.16 psi 2021-02-13T00:28:48Z- handleDepth()-Depth_Keller msg: 1613176128241 epoch sec; seqNo:12998; depth 30.69 2021-02-13T00:28:48Z- handleDvl() - RDI_Pathfinder msg: 1613176128.25 epoch sec; seqNo:6358 2021-02-13T00:28:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.44 , 0.00 , 0.00 , 63.44 2021-02-13T00:28:48Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:28:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176128.28 epoch sec; seqNo:12998; 4059265.67 north; 592618.75 east 2021-02-13T00:28:48Z- AHRS_M2 msg: 1613176128.64 epoch sec; seqNo:12895 2021-02-13T00:28:48Z- AHRS_M2 msg: 0.55 phi; 0.17 theta; -0.16 psi 2021-02-13T00:28:48Z- handleDepth()-Depth_Keller msg: 1613176128645 epoch sec; seqNo:12999; depth 30.69 2021-02-13T00:28:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176128.67 epoch sec; seqNo:12999; 4059265.67 north; 592618.75 east 2021-02-13T00:28:49Z- AHRS_M2 msg: 1613176129.04 epoch sec; seqNo:12896 2021-02-13T00:28:49Z- AHRS_M2 msg: 0.54 phi; 0.20 theta; -0.16 psi 2021-02-13T00:28:49Z- handleDepth()-Depth_Keller msg: 1613176129050 epoch sec; seqNo:13000; depth 30.67 2021-02-13T00:28:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176129.06 epoch sec; seqNo:13000; 4059266.05 north; 592618.75 east 2021-02-13T00:28:49Z- AHRS_M2 msg: 1613176129.45 epoch sec; seqNo:12897 2021-02-13T00:28:49Z- AHRS_M2 msg: 0.54 phi; 0.23 theta; -0.16 psi 2021-02-13T00:28:49Z- handleDepth()-Depth_Keller msg: 1613176129452 epoch sec; seqNo:13001; depth 30.64 2021-02-13T00:28:49Z- handleDvl() - RDI_Pathfinder msg: 1613176129.47 epoch sec; seqNo:6359 2021-02-13T00:28:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.88 , 0.00 , 0.00 , 61.75 2021-02-13T00:28:49Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:28:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176129.48 epoch sec; seqNo:13001; 4059266.43 north; 592618.74 east 2021-02-13T00:28:49Z- AHRS_M2 msg: 1613176129.85 epoch sec; seqNo:12898 2021-02-13T00:28:49Z- AHRS_M2 msg: 0.53 phi; 0.25 theta; -0.16 psi 2021-02-13T00:28:49Z- handleDepth()-Depth_Keller msg: 1613176129856 epoch sec; seqNo:13002; depth 30.60 2021-02-13T00:28:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176129.87 epoch sec; seqNo:13002; 4059266.82 north; 592619.96 east 2021-02-13T00:28:50Z- AHRS_M2 msg: 1613176130.26 epoch sec; seqNo:12899 2021-02-13T00:28:50Z- AHRS_M2 msg: 0.53 phi; 0.27 theta; -0.16 psi 2021-02-13T00:28:50Z- handleDepth()-Depth_Keller msg: 1613176130262 epoch sec; seqNo:13003; depth 30.57 2021-02-13T00:28:50Z- handleDvl() - RDI_Pathfinder msg: 1613176130.27 epoch sec; seqNo:6360 2021-02-13T00:28:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 64.73 , 0.00 , 0.00 , 61.32 2021-02-13T00:28:50Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:28:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176130.29 epoch sec; seqNo:13003; 4059267.20 north; 592619.96 east 2021-02-13T00:28:50Z- AHRS_M2 msg: 1613176130.66 epoch sec; seqNo:12900 2021-02-13T00:28:50Z- AHRS_M2 msg: 0.52 phi; 0.29 theta; -0.16 psi 2021-02-13T00:28:50Z- handleDepth()-Depth_Keller msg: 1613176130664 epoch sec; seqNo:13004; depth 30.51 2021-02-13T00:28:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176130.68 epoch sec; seqNo:13004; 4059267.20 north; 592619.96 east 2021-02-13T00:28:51Z- AHRS_M2 msg: 1613176131.06 epoch sec; seqNo:12901 2021-02-13T00:28:51Z- AHRS_M2 msg: 0.52 phi; 0.30 theta; -0.16 psi 2021-02-13T00:28:51Z- handleDepth()-Depth_Keller msg: 1613176131073 epoch sec; seqNo:13005; depth 30.42 2021-02-13T00:28:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176131.08 epoch sec; seqNo:13005; 4059267.58 north; 592619.95 east 2021-02-13T00:28:51Z- AHRS_M2 msg: 1613176131.47 epoch sec; seqNo:12902 2021-02-13T00:28:51Z- AHRS_M2 msg: 0.52 phi; 0.32 theta; -0.15 psi 2021-02-13T00:28:51Z- handleDepth()-Depth_Keller msg: 1613176131473 epoch sec; seqNo:13006; depth 30.35 2021-02-13T00:28:51Z- handleDvl() - RDI_Pathfinder msg: 1613176131.49 epoch sec; seqNo:6361 2021-02-13T00:28:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 66.43 , 0.00 , 0.00 , 60.05 2021-02-13T00:28:51Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:28:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176131.50 epoch sec; seqNo:13006; 4059267.96 north; 592619.95 east 2021-02-13T00:28:51Z- AHRS_M2 msg: 1613176131.87 epoch sec; seqNo:12903 2021-02-13T00:28:51Z- AHRS_M2 msg: 0.52 phi; 0.33 theta; -0.15 psi 2021-02-13T00:28:51Z- handleDepth()-Depth_Keller msg: 1613176131876 epoch sec; seqNo:13007; depth 30.27 2021-02-13T00:28:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176131.89 epoch sec; seqNo:13007; 4059268.33 north; 592619.94 east 2021-02-13T00:28:52Z- AHRS_M2 msg: 1613176132.28 epoch sec; seqNo:12904 2021-02-13T00:28:52Z- AHRS_M2 msg: 0.52 phi; 0.34 theta; -0.14 psi 2021-02-13T00:28:52Z- handleDepth()-Depth_Keller msg: 1613176132280 epoch sec; seqNo:13008; depth 30.20 2021-02-13T00:28:52Z- handleDvl() - RDI_Pathfinder msg: 1613176132.29 epoch sec; seqNo:6362 2021-02-13T00:28:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:28:52Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:28:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176132.33 epoch sec; seqNo:13008; 4059268.71 north; 592619.94 east 2021-02-13T00:28:52Z- LcmTrn::handleMessages() - handling 41 messages took 0.65 ms and 128 calls... 2021-02-13T00:28:52Z- waiting for 1613176130.46, time is 1613176132.33 2021-02-13T00:28:52Z- LcmTrn::updateTrn() equating pose 1613176132.33 and meas times 1613176132.33 2021-02-13T00:28:52Z- LcmTrn::updateTrn() >>>> pose time 1613176132.33, meas time 1613176132.33 <<<< 2021-02-13T00:28:52Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613176132.33, ping # 0. 2021-02-13T00:28:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176132.33 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:28:52Z- TerrainNav::motionUpdate() - currEstimate = (4059269, 592619.94, 30.20) 2021-02-13T00:28:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:28:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:28:52Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:28:52Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:28:52Z- TNavBF::Projecting Measurements 2021-02-13T00:28:52Z- TNavFilter::Measurement from time = 1613176132.33 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:28:52Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:28:52Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:28:52Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:28:52Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:28:52Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:28:52Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:28:52Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:28:52Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 700.9 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:28:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:28:52Z- TNavBF::mmse selected from filter -1 2021-02-13T00:28:52Z- variances([0], [2], and psi_berg) = 700.939007 832.998438 0.000000 2021-02-13T00:28:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:28:52Z- LcmTrn::publishEstimates() - MLE : 4059268.21 592591.44 30.20 2021-02-13T00:28:52Z- LcmTrn::publishEstimates() - MMSE : 4059268.71 592619.94 30.20 2021-02-13T00:28:52Z- LcmTrn::publishEstimates() - COVAR: 700.94 833.00 1.17 0.00 2021-02-13T00:28:52Z- AHRS_M2 msg: 1613176132.68 epoch sec; seqNo:12905 2021-02-13T00:28:52Z- AHRS_M2 msg: 0.52 phi; 0.35 theta; -0.14 psi 2021-02-13T00:28:52Z- handleDepth()-Depth_Keller msg: 1613176132684 epoch sec; seqNo:13009; depth 30.09 2021-02-13T00:28:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176132.69 epoch sec; seqNo:13009; 4059268.71 north; 592619.94 east 2021-02-13T00:28:53Z- AHRS_M2 msg: 1613176133.09 epoch sec; seqNo:12906 2021-02-13T00:28:53Z- AHRS_M2 msg: 0.52 phi; 0.36 theta; -0.13 psi 2021-02-13T00:28:53Z- handleDepth()-Depth_Keller msg: 1613176133090 epoch sec; seqNo:13010; depth 29.98 2021-02-13T00:28:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176133.10 epoch sec; seqNo:13010; 4059269.10 north; 592621.16 east 2021-02-13T00:28:53Z- AHRS_M2 msg: 1613176133.49 epoch sec; seqNo:12907 2021-02-13T00:28:53Z- AHRS_M2 msg: 0.52 phi; 0.36 theta; -0.13 psi 2021-02-13T00:28:53Z- handleDepth()-Depth_Keller msg: 1613176133494 epoch sec; seqNo:13011; depth 29.89 2021-02-13T00:28:53Z- handleDvl() - RDI_Pathfinder msg: 1613176133.52 epoch sec; seqNo:6363 2021-02-13T00:28:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 0.00 , 97.98 , 59.25 2021-02-13T00:28:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , 0.10 2021-02-13T00:28:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176133.54 epoch sec; seqNo:13011; 4059269.48 north; 592621.15 east 2021-02-13T00:28:53Z- AHRS_M2 msg: 1613176133.89 epoch sec; seqNo:12908 2021-02-13T00:28:53Z- AHRS_M2 msg: 0.52 phi; 0.37 theta; -0.13 psi 2021-02-13T00:28:53Z- handleDepth()-Depth_Keller msg: 1613176133896 epoch sec; seqNo:13012; depth 29.75 2021-02-13T00:28:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176133.91 epoch sec; seqNo:13012; 4059269.86 north; 592621.15 east 2021-02-13T00:28:54Z- AHRS_M2 msg: 1613176134.30 epoch sec; seqNo:12909 2021-02-13T00:28:54Z- AHRS_M2 msg: 0.53 phi; 0.37 theta; -0.12 psi 2021-02-13T00:28:54Z- handleDepth()-Depth_Keller msg: 1613176134301 epoch sec; seqNo:13013; depth 29.66 2021-02-13T00:28:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176134.31 epoch sec; seqNo:13013; 4059270.24 north; 592621.14 east 2021-02-13T00:28:54Z- AHRS_M2 msg: 1613176134.70 epoch sec; seqNo:12910 2021-02-13T00:28:54Z- AHRS_M2 msg: 0.53 phi; 0.37 theta; -0.12 psi 2021-02-13T00:28:54Z- handleDepth()-Depth_Keller msg: 1613176134711 epoch sec; seqNo:13014; depth 29.53 2021-02-13T00:28:54Z- handleDvl() - RDI_Pathfinder msg: 1613176134.73 epoch sec; seqNo:6364 2021-02-13T00:28:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 81.76 , 96.90 , 59.25 2021-02-13T00:28:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.08 , 0.08 2021-02-13T00:28:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176134.74 epoch sec; seqNo:13014; 4059270.24 north; 592621.14 east 2021-02-13T00:28:55Z- AHRS_M2 msg: 1613176135.11 epoch sec; seqNo:12911 2021-02-13T00:28:55Z- AHRS_M2 msg: 0.55 phi; 0.38 theta; -0.10 psi 2021-02-13T00:28:55Z- handleDepth()-Depth_Keller msg: 1613176135114 epoch sec; seqNo:13015; depth 29.39 2021-02-13T00:28:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176135.14 epoch sec; seqNo:13015; 4059270.62 north; 592621.14 east 2021-02-13T00:28:55Z- AHRS_M2 msg: 1613176135.52 epoch sec; seqNo:12912 2021-02-13T00:28:55Z- AHRS_M2 msg: 0.56 phi; 0.37 theta; -0.10 psi 2021-02-13T00:28:55Z- handleDepth()-Depth_Keller msg: 1613176135529 epoch sec; seqNo:13016; depth 29.28 2021-02-13T00:28:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176135.55 epoch sec; seqNo:13016; 4059271.01 north; 592622.36 east 2021-02-13T00:28:55Z- AHRS_M2 msg: 1613176135.91 epoch sec; seqNo:12913 2021-02-13T00:28:55Z- AHRS_M2 msg: 0.57 phi; 0.37 theta; -0.10 psi 2021-02-13T00:28:55Z- handleDepth()-Depth_Keller msg: 1613176135926 epoch sec; seqNo:13017; depth 29.16 2021-02-13T00:28:55Z- handleDvl() - RDI_Pathfinder msg: 1613176135.94 epoch sec; seqNo:6365 2021-02-13T00:28:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 80.68 , 97.98 , 59.66 2021-02-13T00:28:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.10 , 0.03 2021-02-13T00:28:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176135.95 epoch sec; seqNo:13017; 4059271.39 north; 592622.35 east 2021-02-13T00:28:56Z- AHRS_M2 msg: 1613176136.32 epoch sec; seqNo:12914 2021-02-13T00:28:56Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.09 psi 2021-02-13T00:28:56Z- handleDepth()-Depth_Keller msg: 1613176136321 epoch sec; seqNo:13018; depth 29.03 2021-02-13T00:28:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176136.34 epoch sec; seqNo:13018; 4059271.39 north; 592622.35 east 2021-02-13T00:28:56Z- AHRS_M2 msg: 1613176136.72 epoch sec; seqNo:12915 2021-02-13T00:28:56Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.09 psi 2021-02-13T00:28:56Z- handleDepth()-Depth_Keller msg: 1613176136725 epoch sec; seqNo:13019; depth 28.91 2021-02-13T00:28:56Z- handleDvl() - RDI_Pathfinder msg: 1613176136.74 epoch sec; seqNo:6366 2021-02-13T00:28:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 79.60 , 97.98 , 62.12 2021-02-13T00:28:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.12 , -0.01 2021-02-13T00:28:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176136.75 epoch sec; seqNo:13019; 4059271.77 north; 592622.35 east 2021-02-13T00:28:57Z- AHRS_M2 msg: 1613176137.13 epoch sec; seqNo:12916 2021-02-13T00:28:57Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.09 psi 2021-02-13T00:28:57Z- handleDepth()-Depth_Keller msg: 1613176137134 epoch sec; seqNo:13020; depth 28.78 2021-02-13T00:28:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176137.15 epoch sec; seqNo:13020; 4059272.15 north; 592622.34 east 2021-02-13T00:28:57Z- AHRS_M2 msg: 1613176137.53 epoch sec; seqNo:12917 2021-02-13T00:28:57Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.10 psi 2021-02-13T00:28:57Z- handleDepth()-Depth_Keller msg: 1613176137532 epoch sec; seqNo:13021; depth 28.66 2021-02-13T00:28:57Z- handleDvl() - RDI_Pathfinder msg: 1613176137.55 epoch sec; seqNo:6367 2021-02-13T00:28:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 81.76 , 95.82 , 61.30 2021-02-13T00:28:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.09 , -0.06 2021-02-13T00:28:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176137.56 epoch sec; seqNo:13021; 4059272.15 north; 592622.34 east 2021-02-13T00:28:57Z- LcmTrn::handleMessages() - handling 44 messages took 0.76 ms and 129 calls... 2021-02-13T00:28:57Z- waiting for 1613176135.33, time is 1613176137.56 2021-02-13T00:28:57Z- LcmTrn::updateTrn() equating pose 1613176137.56 and meas times 1613176137.56 2021-02-13T00:28:57Z- LcmTrn::updateTrn() >>>> pose time 1613176137.56, meas time 1613176137.56 <<<< 2021-02-13T00:28:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176137.56 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:28:57Z- TerrainNav::motionUpdate() - currEstimate = (4059272, 592622.34, 28.66) 2021-02-13T00:28:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:28:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:28:57Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176137.56 sec.; No relevant navigation data available 2021-02-13T00:28:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:28:57Z- TNavBF::mmse selected from filter -1 2021-02-13T00:28:57Z- variances([0], [2], and psi_berg) = 700.883026 832.998408 0.000000 2021-02-13T00:28:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:28:57Z- LcmTrn::publishEstimates() - MLE : 4059271.65 592593.84 28.66 2021-02-13T00:28:57Z- LcmTrn::publishEstimates() - MMSE : 4059272.15 592622.34 28.66 2021-02-13T00:28:57Z- LcmTrn::publishEstimates() - COVAR: 700.88 833.00 1.39 0.00 2021-02-13T00:28:57Z- AHRS_M2 msg: 1613176137.93 epoch sec; seqNo:12918 2021-02-13T00:28:57Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.11 psi 2021-02-13T00:28:57Z- handleDepth()-Depth_Keller msg: 1613176137936 epoch sec; seqNo:13022; depth 28.52 2021-02-13T00:28:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176137.95 epoch sec; seqNo:13022; 4059272.54 north; 592623.56 east 2021-02-13T00:28:58Z- AHRS_M2 msg: 1613176138.34 epoch sec; seqNo:12919 2021-02-13T00:28:58Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.11 psi 2021-02-13T00:28:58Z- handleDepth()-Depth_Keller msg: 1613176138340 epoch sec; seqNo:13023; depth 28.40 2021-02-13T00:28:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176138.35 epoch sec; seqNo:13023; 4059272.54 north; 592623.56 east 2021-02-13T00:28:58Z- AHRS_M2 msg: 1613176138.74 epoch sec; seqNo:12920 2021-02-13T00:28:58Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.12 psi 2021-02-13T00:28:58Z- handleDepth()-Depth_Keller msg: 1613176138747 epoch sec; seqNo:13024; depth 28.27 2021-02-13T00:28:58Z- handleDvl() - RDI_Pathfinder msg: 1613176138.76 epoch sec; seqNo:6368 2021-02-13T00:28:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 81.76 , 97.98 , 61.30 2021-02-13T00:28:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.04 , -0.10 2021-02-13T00:28:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176138.77 epoch sec; seqNo:13024; 4059272.92 north; 592623.56 east 2021-02-13T00:28:59Z- AHRS_M2 msg: 1613176139.15 epoch sec; seqNo:12921 2021-02-13T00:28:59Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.13 psi 2021-02-13T00:28:59Z- handleDepth()-Depth_Keller msg: 1613176139155 epoch sec; seqNo:13025; depth 28.13 2021-02-13T00:28:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176139.17 epoch sec; seqNo:13025; 4059273.30 north; 592623.55 east 2021-02-13T00:28:59Z- AHRS_M2 msg: 1613176139.56 epoch sec; seqNo:12922 2021-02-13T00:28:59Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.13 psi 2021-02-13T00:28:59Z- handleDepth()-Depth_Keller msg: 1613176139564 epoch sec; seqNo:13026; depth 27.98 2021-02-13T00:28:59Z- handleDvl() - RDI_Pathfinder msg: 1613176139.58 epoch sec; seqNo:6369 2021-02-13T00:28:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 82.84 , 97.98 , 62.52 2021-02-13T00:28:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.02 , -0.10 2021-02-13T00:28:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176139.59 epoch sec; seqNo:13026; 4059273.30 north; 592623.55 east 2021-02-13T00:28:59Z- AHRS_M2 msg: 1613176139.95 epoch sec; seqNo:12923 2021-02-13T00:28:59Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-13T00:28:59Z- handleDepth()-Depth_Keller msg: 1613176139965 epoch sec; seqNo:13027; depth 27.86 2021-02-13T00:28:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176139.98 epoch sec; seqNo:13027; 4059273.68 north; 592623.55 east 2021-02-13T00:29:00Z- AHRS_M2 msg: 1613176140.36 epoch sec; seqNo:12924 2021-02-13T00:29:00Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-13T00:29:00Z- handleDepth()-Depth_Keller msg: 1613176140360 epoch sec; seqNo:13028; depth 27.73 2021-02-13T00:29:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176140.38 epoch sec; seqNo:13028; 4059274.06 north; 592623.55 east 2021-02-13T00:29:00Z- AHRS_M2 msg: 1613176140.76 epoch sec; seqNo:12925 2021-02-13T00:29:00Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-13T00:29:00Z- handleDepth()-Depth_Keller msg: 1613176140769 epoch sec; seqNo:13029; depth 27.61 2021-02-13T00:29:00Z- handleDvl() - RDI_Pathfinder msg: 1613176140.78 epoch sec; seqNo:6370 2021-02-13T00:29:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 82.77 , 97.92 , 62.93 2021-02-13T00:29:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.10 , -0.09 2021-02-13T00:29:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176140.80 epoch sec; seqNo:13029; 4059274.45 north; 592624.76 east 2021-02-13T00:29:01Z- AHRS_M2 msg: 1613176141.16 epoch sec; seqNo:12926 2021-02-13T00:29:01Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-13T00:29:01Z- handleDepth()-Depth_Keller msg: 1613176141169 epoch sec; seqNo:13030; depth 27.46 2021-02-13T00:29:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176141.18 epoch sec; seqNo:13030; 4059274.45 north; 592624.76 east 2021-02-13T00:29:01Z- AHRS_M2 msg: 1613176141.57 epoch sec; seqNo:12927 2021-02-13T00:29:01Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-13T00:29:01Z- handleDepth()-Depth_Keller msg: 1613176141572 epoch sec; seqNo:13031; depth 27.35 2021-02-13T00:29:01Z- handleDvl() - RDI_Pathfinder msg: 1613176141.59 epoch sec; seqNo:6371 2021-02-13T00:29:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:29:01Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:29:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176141.60 epoch sec; seqNo:13031; 4059274.83 north; 592624.76 east 2021-02-13T00:29:01Z- handleDepth()-Depth_Keller msg: 1613176141993 epoch sec; seqNo:13032; depth 27.20 2021-02-13T00:29:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176142.01 epoch sec; seqNo:13032; 4059275.21 north; 592624.75 east 2021-02-13T00:29:02Z- AHRS_M2 msg: 1613176142.38 epoch sec; seqNo:12928 2021-02-13T00:29:02Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.18 psi 2021-02-13T00:29:02Z- handleDepth()-Depth_Keller msg: 1613176142381 epoch sec; seqNo:13033; depth 27.07 2021-02-13T00:29:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176142.39 epoch sec; seqNo:13033; 4059275.21 north; 592624.75 east 2021-02-13T00:29:02Z- LcmTrn::handleMessages() - handling 39 messages took 0.61 ms and 127 calls... 2021-02-13T00:29:02Z- waiting for 1613176140.56, time is 1613176142.39 2021-02-13T00:29:02Z- LcmTrn::updateTrn() >>>> pose time 1613176142.39, meas time 1613176141.59 <<<< 2021-02-13T00:29:02Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613176141.59, ping # 0. 2021-02-13T00:29:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176141.59 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:29:02Z- TerrainNav::motionUpdate() - currEstimate = (4059275, 592624.75, 27.07) 2021-02-13T00:29:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:29:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:29:02Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:29:02Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:29:02Z- TNavBF::Projecting Measurements 2021-02-13T00:29:02Z- TNavFilter::Measurement from time = 1613176141.59 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:29:02Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:29:02Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:29:02Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:29:02Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:29:02Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:29:02Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:29:02Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:29:02Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 700.8 EastVariance: 833.0 DepthVariance: 1.6 this->DepthBiasCov: 1.6 2021-02-13T00:29:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:29:02Z- TNavBF::mmse selected from filter -1 2021-02-13T00:29:02Z- variances([0], [2], and psi_berg) = 700.811879 832.998370 0.000000 2021-02-13T00:29:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:29:02Z- LcmTrn::publishEstimates() - MLE : 4059274.71 592596.25 27.07 2021-02-13T00:29:02Z- LcmTrn::publishEstimates() - MMSE : 4059275.21 592624.75 27.07 2021-02-13T00:29:02Z- LcmTrn::publishEstimates() - COVAR: 700.81 833.00 1.58 0.00 2021-02-13T00:29:02Z- AHRS_M2 msg: 1613176142.78 epoch sec; seqNo:12929 2021-02-13T00:29:02Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.19 psi 2021-02-13T00:29:02Z- handleDepth()-Depth_Keller msg: 1613176142793 epoch sec; seqNo:13034; depth 26.95 2021-02-13T00:29:02Z- handleDvl() - RDI_Pathfinder msg: 1613176142.81 epoch sec; seqNo:6372 2021-02-13T00:29:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 86.22 , 97.04 , 63.75 2021-02-13T00:29:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.17 , 0.02 2021-02-13T00:29:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176142.82 epoch sec; seqNo:13034; 4059275.58 north; 592624.75 east 2021-02-13T00:29:03Z- AHRS_M2 msg: 1613176143.18 epoch sec; seqNo:12930 2021-02-13T00:29:03Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.19 psi 2021-02-13T00:29:03Z- handleDepth()-Depth_Keller msg: 1613176143188 epoch sec; seqNo:13035; depth 26.82 2021-02-13T00:29:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176143.21 epoch sec; seqNo:13035; 4059275.96 north; 592624.75 east 2021-02-13T00:29:03Z- AHRS_M2 msg: 1613176143.60 epoch sec; seqNo:12931 2021-02-13T00:29:03Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.19 psi 2021-02-13T00:29:03Z- handleDepth()-Depth_Keller msg: 1613176143609 epoch sec; seqNo:13036; depth 26.70 2021-02-13T00:29:03Z- handleDvl() - RDI_Pathfinder msg: 1613176143.63 epoch sec; seqNo:6373 2021-02-13T00:29:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 86.08 , 97.98 , 62.12 2021-02-13T00:29:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.13 , 0.09 2021-02-13T00:29:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176143.64 epoch sec; seqNo:13036; 4059276.34 north; 592624.74 east 2021-02-13T00:29:03Z- AHRS_M2 msg: 1613176143.99 epoch sec; seqNo:12932 2021-02-13T00:29:03Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.18 psi 2021-02-13T00:29:03Z- handleDepth()-Depth_Keller msg: 1613176143997 epoch sec; seqNo:13037; depth 26.56 2021-02-13T00:29:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176144.01 epoch sec; seqNo:13037; 4059276.73 north; 592625.96 east 2021-02-13T00:29:04Z- AHRS_M2 msg: 1613176144.40 epoch sec; seqNo:12933 2021-02-13T00:29:04Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.18 psi 2021-02-13T00:29:04Z- handleDepth()-Depth_Keller msg: 1613176144406 epoch sec; seqNo:13038; depth 26.44 2021-02-13T00:29:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176144.41 epoch sec; seqNo:13038; 4059277.11 north; 592625.95 east 2021-02-13T00:29:04Z- AHRS_M2 msg: 1613176144.80 epoch sec; seqNo:12934 2021-02-13T00:29:04Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.17 psi 2021-02-13T00:29:04Z- handleDepth()-Depth_Keller msg: 1613176144808 epoch sec; seqNo:13039; depth 26.33 2021-02-13T00:29:04Z- handleDvl() - RDI_Pathfinder msg: 1613176144.82 epoch sec; seqNo:6374 2021-02-13T00:29:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 86.08 , 97.98 , 63.35 2021-02-13T00:29:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , 0.14 2021-02-13T00:29:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176144.84 epoch sec; seqNo:13039; 4059277.49 north; 592625.95 east 2021-02-13T00:29:05Z- AHRS_M2 msg: 1613176145.20 epoch sec; seqNo:12935 2021-02-13T00:29:05Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.16 psi 2021-02-13T00:29:05Z- handleDepth()-Depth_Keller msg: 1613176145213 epoch sec; seqNo:13040; depth 26.20 2021-02-13T00:29:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176145.22 epoch sec; seqNo:13040; 4059277.49 north; 592625.95 east 2021-02-13T00:29:05Z- AHRS_M2 msg: 1613176145.61 epoch sec; seqNo:12936 2021-02-13T00:29:05Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.16 psi 2021-02-13T00:29:05Z- handleDepth()-Depth_Keller msg: 1613176145614 epoch sec; seqNo:13041; depth 26.07 2021-02-13T00:29:05Z- handleDvl() - RDI_Pathfinder msg: 1613176145.62 epoch sec; seqNo:6375 2021-02-13T00:29:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 86.08 , 97.98 , 64.57 2021-02-13T00:29:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , 0.14 2021-02-13T00:29:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176145.63 epoch sec; seqNo:13041; 4059277.87 north; 592625.95 east 2021-02-13T00:29:06Z- AHRS_M2 msg: 1613176146.01 epoch sec; seqNo:12937 2021-02-13T00:29:06Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.15 psi 2021-02-13T00:29:06Z- handleDepth()-Depth_Keller msg: 1613176146016 epoch sec; seqNo:13042; depth 25.95 2021-02-13T00:29:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176146.03 epoch sec; seqNo:13042; 4059278.25 north; 592625.94 east 2021-02-13T00:29:06Z- AHRS_M2 msg: 1613176146.42 epoch sec; seqNo:12938 2021-02-13T00:29:06Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-13T00:29:06Z- handleDepth()-Depth_Keller msg: 1613176146420 epoch sec; seqNo:13043; depth 25.82 2021-02-13T00:29:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176146.43 epoch sec; seqNo:13043; 4059278.64 north; 592627.16 east 2021-02-13T00:29:06Z- AHRS_M2 msg: 1613176146.82 epoch sec; seqNo:12939 2021-02-13T00:29:06Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-13T00:29:06Z- handleDepth()-Depth_Keller msg: 1613176146828 epoch sec; seqNo:13044; depth 25.70 2021-02-13T00:29:06Z- handleDvl() - RDI_Pathfinder msg: 1613176146.84 epoch sec; seqNo:6376 2021-02-13T00:29:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.93 , 86.08 , 97.98 , 63.35 2021-02-13T00:29:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , 0.09 2021-02-13T00:29:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176146.87 epoch sec; seqNo:13044; 4059279.02 north; 592627.15 east 2021-02-13T00:29:07Z- AHRS_M2 msg: 1613176147.22 epoch sec; seqNo:12940 2021-02-13T00:29:07Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-13T00:29:07Z- handleDepth()-Depth_Keller msg: 1613176147237 epoch sec; seqNo:13045; depth 25.56 2021-02-13T00:29:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176147.25 epoch sec; seqNo:13045; 4059279.02 north; 592627.15 east 2021-02-13T00:29:07Z- AHRS_M2 msg: 1613176147.63 epoch sec; seqNo:12941 2021-02-13T00:29:07Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-13T00:29:07Z- handleDepth()-Depth_Keller msg: 1613176147632 epoch sec; seqNo:13046; depth 25.44 2021-02-13T00:29:07Z- handleDvl() - RDI_Pathfinder msg: 1613176147.64 epoch sec; seqNo:6377 2021-02-13T00:29:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 87.16 , 97.98 , 65.39 2021-02-13T00:29:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.08 , 0.03 2021-02-13T00:29:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176147.67 epoch sec; seqNo:13046; 4059279.40 north; 592627.15 east 2021-02-13T00:29:07Z- LcmTrn::handleMessages() - handling 45 messages took 0.71 ms and 131 calls... 2021-02-13T00:29:07Z- waiting for 1613176145.39, time is 1613176147.67 2021-02-13T00:29:07Z- LcmTrn::updateTrn() equating pose 1613176147.67 and meas times 1613176147.67 2021-02-13T00:29:07Z- LcmTrn::updateTrn() >>>> pose time 1613176147.67, meas time 1613176147.67 <<<< 2021-02-13T00:29:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176147.67 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:29:07Z- TerrainNav::motionUpdate() - currEstimate = (4059279, 592627.15, 25.44) 2021-02-13T00:29:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:29:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:29:07Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176147.67 sec.; No relevant navigation data available 2021-02-13T00:29:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:29:07Z- TNavBF::mmse selected from filter -1 2021-02-13T00:29:07Z- variances([0], [2], and psi_berg) = 700.751883 832.998339 0.000000 2021-02-13T00:29:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:29:07Z- LcmTrn::publishEstimates() - MLE : 4059278.90 592598.65 25.44 2021-02-13T00:29:07Z- LcmTrn::publishEstimates() - MMSE : 4059279.40 592627.15 25.44 2021-02-13T00:29:07Z- LcmTrn::publishEstimates() - COVAR: 700.75 833.00 1.83 0.00 2021-02-13T00:29:08Z- AHRS_M2 msg: 1613176148.03 epoch sec; seqNo:12942 2021-02-13T00:29:08Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T00:29:08Z- handleDepth()-Depth_Keller msg: 1613176148045 epoch sec; seqNo:13047; depth 25.30 2021-02-13T00:29:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176148.06 epoch sec; seqNo:13047; 4059279.78 north; 592627.15 east 2021-02-13T00:29:08Z- AHRS_M2 msg: 1613176148.44 epoch sec; seqNo:12943 2021-02-13T00:29:08Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-13T00:29:08Z- handleDepth()-Depth_Keller msg: 1613176148446 epoch sec; seqNo:13048; depth 25.19 2021-02-13T00:29:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176148.48 epoch sec; seqNo:13048; 4059279.79 north; 592628.37 east 2021-02-13T00:29:08Z- AHRS_M2 msg: 1613176148.84 epoch sec; seqNo:12944 2021-02-13T00:29:08Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-13T00:29:08Z- handleDepth()-Depth_Keller msg: 1613176148846 epoch sec; seqNo:13049; depth 25.06 2021-02-13T00:29:08Z- handleDvl() - RDI_Pathfinder msg: 1613176148.86 epoch sec; seqNo:6378 2021-02-13T00:29:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.93 , 86.01 , 99.00 , 64.57 2021-02-13T00:29:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , -0.01 2021-02-13T00:29:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176148.87 epoch sec; seqNo:13049; 4059280.17 north; 592628.36 east 2021-02-13T00:29:09Z- AHRS_M2 msg: 1613176149.24 epoch sec; seqNo:12945 2021-02-13T00:29:09Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-13T00:29:09Z- handleDepth()-Depth_Keller msg: 1613176149258 epoch sec; seqNo:13050; depth 24.93 2021-02-13T00:29:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176149.27 epoch sec; seqNo:13050; 4059280.55 north; 592628.36 east 2021-02-13T00:29:09Z- AHRS_M2 msg: 1613176149.65 epoch sec; seqNo:12946 2021-02-13T00:29:09Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-13T00:29:09Z- handleDepth()-Depth_Keller msg: 1613176149653 epoch sec; seqNo:13051; depth 24.80 2021-02-13T00:29:09Z- handleDvl() - RDI_Pathfinder msg: 1613176149.66 epoch sec; seqNo:6379 2021-02-13T00:29:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 87.10 , 96.83 , 64.57 2021-02-13T00:29:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , -0.03 2021-02-13T00:29:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176149.67 epoch sec; seqNo:13051; 4059280.55 north; 592628.36 east 2021-02-13T00:29:10Z- AHRS_M2 msg: 1613176150.05 epoch sec; seqNo:12947 2021-02-13T00:29:10Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.14 psi 2021-02-13T00:29:10Z- handleDepth()-Depth_Keller msg: 1613176150058 epoch sec; seqNo:13052; depth 24.68 2021-02-13T00:29:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:29:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:29:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:29:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:29:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:29:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:29:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:29:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:29:10Z- handleDvl() - RDI_Pathfinder msg: 1613176150.06 epoch sec; seqNo:6380 2021-02-13T00:29:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:29:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , -0.03 2021-02-13T00:29:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176150.08 epoch sec; seqNo:13052; 4059280.93 north; 592628.35 east 2021-02-13T00:29:10Z- AHRS_M2 msg: 1613176150.46 epoch sec; seqNo:12948 2021-02-13T00:29:10Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.14 psi 2021-02-13T00:29:10Z- handleDepth()-Depth_Keller msg: 1613176150460 epoch sec; seqNo:13053; depth 24.56 2021-02-13T00:29:10Z- handleDvl() - RDI_Pathfinder msg: 1613176150.48 epoch sec; seqNo:6381 2021-02-13T00:29:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 0.00 , 97.78 , 66.21 2021-02-13T00:29:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.02 2021-02-13T00:29:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176150.49 epoch sec; seqNo:13053; 4059281.31 north; 592628.35 east 2021-02-13T00:29:10Z- AHRS_M2 msg: 1613176150.86 epoch sec; seqNo:12949 2021-02-13T00:29:10Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.14 psi 2021-02-13T00:29:10Z- handleDepth()-Depth_Keller msg: 1613176150864 epoch sec; seqNo:13054; depth 24.42 2021-02-13T00:29:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176150.88 epoch sec; seqNo:13054; 4059281.70 north; 592629.57 east 2021-02-13T00:29:11Z- AHRS_M2 msg: 1613176151.26 epoch sec; seqNo:12950 2021-02-13T00:29:11Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.14 psi 2021-02-13T00:29:11Z- handleDepth()-Depth_Keller msg: 1613176151268 epoch sec; seqNo:13055; depth 24.30 2021-02-13T00:29:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:29:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:29:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:29:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:29:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:29:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:29:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:29:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:29:11Z- handleDvl() - RDI_Pathfinder msg: 1613176151.27 epoch sec; seqNo:6382 2021-02-13T00:29:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:29:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.02 2021-02-13T00:29:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176151.28 epoch sec; seqNo:13055; 4059281.70 north; 592629.57 east 2021-02-13T00:29:11Z- AHRS_M2 msg: 1613176151.67 epoch sec; seqNo:12951 2021-02-13T00:29:11Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.14 psi 2021-02-13T00:29:11Z- handleDepth()-Depth_Keller msg: 1613176151682 epoch sec; seqNo:13056; depth 24.18 2021-02-13T00:29:11Z- handleDvl() - RDI_Pathfinder msg: 1613176151.70 epoch sec; seqNo:6383 2021-02-13T00:29:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 86.08 , 99.06 , 65.39 2021-02-13T00:29:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.01 2021-02-13T00:29:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176151.71 epoch sec; seqNo:13056; 4059282.08 north; 592629.56 east 2021-02-13T00:29:12Z- AHRS_M2 msg: 1613176152.08 epoch sec; seqNo:12952 2021-02-13T00:29:12Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.14 psi 2021-02-13T00:29:12Z- handleDepth()-Depth_Keller msg: 1613176152084 epoch sec; seqNo:13057; depth 24.03 2021-02-13T00:29:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176152.11 epoch sec; seqNo:13057; 4059282.46 north; 592629.56 east 2021-02-13T00:29:12Z- AHRS_M2 msg: 1613176152.48 epoch sec; seqNo:12953 2021-02-13T00:29:12Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.15 psi 2021-02-13T00:29:12Z- handleDepth()-Depth_Keller msg: 1613176152486 epoch sec; seqNo:13058; depth 23.91 2021-02-13T00:29:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:29:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:29:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:29:12Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:29:12Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:29:12Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:29:12Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:29:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:29:12Z- handleDvl() - RDI_Pathfinder msg: 1613176152.49 epoch sec; seqNo:6384 2021-02-13T00:29:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:29:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.01 2021-02-13T00:29:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176152.50 epoch sec; seqNo:13058; 4059282.46 north; 592629.56 east 2021-02-13T00:29:12Z- LcmTrn::handleMessages() - handling 43 messages took 0.63 ms and 131 calls... 2021-02-13T00:29:12Z- waiting for 1613176150.67, time is 1613176152.50 2021-02-13T00:29:12Z- LcmTrn::updateTrn() equating pose 1613176152.50 and meas times 1613176152.50 2021-02-13T00:29:12Z- LcmTrn::updateTrn() >>>> pose time 1613176152.50, meas time 1613176152.50 <<<< 2021-02-13T00:29:12Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613176152.50, ping # 0. 2021-02-13T00:29:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176152.50 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:29:12Z- TerrainNav::motionUpdate() - currEstimate = (4059282, 592629.56, 23.91) 2021-02-13T00:29:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:29:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:29:12Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:29:12Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:29:12Z- TNavBF::Projecting Measurements 2021-02-13T00:29:12Z- TNavFilter::Measurement from time = 1613176152.50 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:29:12Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:29:12Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:29:12Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:29:12Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:29:12Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:29:12Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:29:12Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:29:12Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 700.7 EastVariance: 833.0 DepthVariance: 2.0 this->DepthBiasCov: 2.0 2021-02-13T00:29:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:29:12Z- TNavBF::mmse selected from filter -1 2021-02-13T00:29:12Z- variances([0], [2], and psi_berg) = 700.698062 832.998310 0.000000 2021-02-13T00:29:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:29:12Z- LcmTrn::publishEstimates() - MLE : 4059281.96 592601.06 23.91 2021-02-13T00:29:12Z- LcmTrn::publishEstimates() - MMSE : 4059282.46 592629.56 23.91 2021-02-13T00:29:12Z- LcmTrn::publishEstimates() - COVAR: 700.70 833.00 2.03 0.00 2021-02-13T00:29:12Z- AHRS_M2 msg: 1613176152.88 epoch sec; seqNo:12954 2021-02-13T00:29:12Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.16 psi 2021-02-13T00:29:12Z- handleDepth()-Depth_Keller msg: 1613176152889 epoch sec; seqNo:13059; depth 23.78 2021-02-13T00:29:12Z- handleDvl() - RDI_Pathfinder msg: 1613176152.90 epoch sec; seqNo:6385 2021-02-13T00:29:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.37 , 88.11 , 100.01 , 65.39 2021-02-13T00:29:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , -0.04 2021-02-13T00:29:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176152.91 epoch sec; seqNo:13059; 4059282.84 north; 592629.55 east 2021-02-13T00:29:13Z- AHRS_M2 msg: 1613176153.28 epoch sec; seqNo:12955 2021-02-13T00:29:13Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.16 psi 2021-02-13T00:29:13Z- handleDepth()-Depth_Keller msg: 1613176153297 epoch sec; seqNo:13060; depth 23.65 2021-02-13T00:29:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176153.31 epoch sec; seqNo:13060; 4059283.21 north; 592629.55 east 2021-02-13T00:29:13Z- AHRS_M2 msg: 1613176153.69 epoch sec; seqNo:12956 2021-02-13T00:29:13Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.16 psi 2021-02-13T00:29:13Z- handleDepth()-Depth_Keller msg: 1613176153693 epoch sec; seqNo:13061; depth 23.53 2021-02-13T00:29:13Z- handleDvl() - RDI_Pathfinder msg: 1613176153.71 epoch sec; seqNo:6386 2021-02-13T00:29:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 0.00 , 97.17 , 65.80 2021-02-13T00:29:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , -0.07 2021-02-13T00:29:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176153.72 epoch sec; seqNo:13061; 4059283.61 north; 592630.77 east 2021-02-13T00:29:14Z- AHRS_M2 msg: 1613176154.09 epoch sec; seqNo:12957 2021-02-13T00:29:14Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.17 psi 2021-02-13T00:29:14Z- handleDepth()-Depth_Keller msg: 1613176154101 epoch sec; seqNo:13062; depth 23.39 2021-02-13T00:29:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176154.11 epoch sec; seqNo:13062; 4059283.61 north; 592630.77 east 2021-02-13T00:29:14Z- AHRS_M2 msg: 1613176154.50 epoch sec; seqNo:12958 2021-02-13T00:29:14Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.18 psi 2021-02-13T00:29:14Z- handleDepth()-Depth_Keller msg: 1613176154500 epoch sec; seqNo:13063; depth 23.26 2021-02-13T00:29:14Z- handleDvl() - RDI_Pathfinder msg: 1613176154.51 epoch sec; seqNo:6387 2021-02-13T00:29:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 0.00 , 102.31 , 67.43 2021-02-13T00:29:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , -0.06 2021-02-13T00:29:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176154.52 epoch sec; seqNo:13063; 4059283.99 north; 592630.76 east 2021-02-13T00:29:14Z- AHRS_M2 msg: 1613176154.90 epoch sec; seqNo:12959 2021-02-13T00:29:14Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.18 psi 2021-02-13T00:29:14Z- handleDepth()-Depth_Keller msg: 1613176154906 epoch sec; seqNo:13064; depth 23.14 2021-02-13T00:29:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:29:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:29:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:29:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:29:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:29:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:29:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:29:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:29:14Z- handleDvl() - RDI_Pathfinder msg: 1613176154.91 epoch sec; seqNo:6388 2021-02-13T00:29:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:29:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , -0.06 2021-02-13T00:29:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176154.92 epoch sec; seqNo:13064; 4059284.36 north; 592630.76 east 2021-02-13T00:29:15Z- AHRS_M2 msg: 1613176155.30 epoch sec; seqNo:12960 2021-02-13T00:29:15Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.19 psi 2021-02-13T00:29:15Z- handleDepth()-Depth_Keller msg: 1613176155318 epoch sec; seqNo:13065; depth 23.02 2021-02-13T00:29:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176155.33 epoch sec; seqNo:13065; 4059284.74 north; 592630.75 east 2021-02-13T00:29:15Z- AHRS_M2 msg: 1613176155.71 epoch sec; seqNo:12961 2021-02-13T00:29:15Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.20 psi 2021-02-13T00:29:15Z- handleDepth()-Depth_Keller msg: 1613176155717 epoch sec; seqNo:13066; depth 22.88 2021-02-13T00:29:15Z- handleDvl() - RDI_Pathfinder msg: 1613176155.73 epoch sec; seqNo:6389 2021-02-13T00:29:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 91.36 , 102.17 , 66.62 2021-02-13T00:29:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.17 , 0.00 2021-02-13T00:29:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176155.74 epoch sec; seqNo:13066; 4059284.74 north; 592630.75 east 2021-02-13T00:29:16Z- AHRS_M2 msg: 1613176156.12 epoch sec; seqNo:12962 2021-02-13T00:29:16Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.20 psi 2021-02-13T00:29:16Z- handleDepth()-Depth_Keller msg: 1613176156129 epoch sec; seqNo:13067; depth 22.73 2021-02-13T00:29:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:29:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:29:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:29:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:29:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:29:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:29:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:29:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:29:16Z- handleDvl() - RDI_Pathfinder msg: 1613176156.13 epoch sec; seqNo:6390 2021-02-13T00:29:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:29:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.17 , 0.00 2021-02-13T00:29:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176156.15 epoch sec; seqNo:13067; 4059285.12 north; 592630.75 east 2021-02-13T00:29:16Z- AHRS_M2 msg: 1613176156.53 epoch sec; seqNo:12963 2021-02-13T00:29:16Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.20 psi 2021-02-13T00:29:16Z- handleDepth()-Depth_Keller msg: 1613176156538 epoch sec; seqNo:13068; depth 22.62 2021-02-13T00:29:16Z- handleDvl() - RDI_Pathfinder msg: 1613176156.55 epoch sec; seqNo:6391 2021-02-13T00:29:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 98.32 , 101.97 , 78.85 2021-02-13T00:29:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.19 , 0.09 2021-02-13T00:29:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176156.56 epoch sec; seqNo:13068; 4059285.51 north; 592631.97 east 2021-02-13T00:29:16Z- handleDepth()-Depth_Keller msg: 1613176156945 epoch sec; seqNo:13069; depth 22.46 2021-02-13T00:29:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176156.96 epoch sec; seqNo:13069; 4059285.89 north; 592631.96 east 2021-02-13T00:29:17Z- AHRS_M2 msg: 1613176157.32 epoch sec; seqNo:12964 2021-02-13T00:29:17Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.20 psi 2021-02-13T00:29:17Z- handleDepth()-Depth_Keller msg: 1613176157329 epoch sec; seqNo:13070; depth 22.33 2021-02-13T00:29:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:29:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:29:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:29:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:29:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:29:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:29:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:29:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:29:17Z- handleDvl() - RDI_Pathfinder msg: 1613176157.33 epoch sec; seqNo:6392 2021-02-13T00:29:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:29:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.19 , 0.09 2021-02-13T00:29:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176157.34 epoch sec; seqNo:13070; 4059286.27 north; 592631.96 east 2021-02-13T00:29:17Z- AHRS_M2 msg: 1613176157.73 epoch sec; seqNo:12965 2021-02-13T00:29:17Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.19 psi 2021-02-13T00:29:17Z- handleDepth()-Depth_Keller msg: 1613176157737 epoch sec; seqNo:13071; depth 22.21 2021-02-13T00:29:17Z- handleDvl() - RDI_Pathfinder msg: 1613176157.75 epoch sec; seqNo:6393 2021-02-13T00:29:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.97 , 93.04 , 105.21 , 67.84 2021-02-13T00:29:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.10 , 0.17 2021-02-13T00:29:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176157.76 epoch sec; seqNo:13071; 4059286.65 north; 592631.95 east 2021-02-13T00:29:17Z- LcmTrn::handleMessages() - handling 47 messages took 0.69 ms and 134 calls... 2021-02-13T00:29:17Z- waiting for 1613176155.50, time is 1613176157.76 2021-02-13T00:29:17Z- LcmTrn::updateTrn() equating pose 1613176157.76 and meas times 1613176157.76 2021-02-13T00:29:17Z- LcmTrn::updateTrn() >>>> pose time 1613176157.76, meas time 1613176157.76 <<<< 2021-02-13T00:29:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176157.76 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:29:17Z- TerrainNav::motionUpdate() - currEstimate = (4059287, 592631.95, 22.21) 2021-02-13T00:29:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:29:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:29:17Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176157.76 sec.; No relevant navigation data available 2021-02-13T00:29:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:29:17Z- TNavBF::mmse selected from filter -1 2021-02-13T00:29:17Z- variances([0], [2], and psi_berg) = 700.638139 832.998278 0.000000 2021-02-13T00:29:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:29:17Z- LcmTrn::publishEstimates() - MLE : 4059286.15 592603.45 22.21 2021-02-13T00:29:17Z- LcmTrn::publishEstimates() - MMSE : 4059286.65 592631.95 22.21 2021-02-13T00:29:17Z- LcmTrn::publishEstimates() - COVAR: 700.64 833.00 2.27 0.00 2021-02-13T00:29:18Z- AHRS_M2 msg: 1613176158.14 epoch sec; seqNo:12966 2021-02-13T00:29:18Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.19 psi 2021-02-13T00:29:18Z- handleDepth()-Depth_Keller msg: 1613176158149 epoch sec; seqNo:13072; depth 22.08 2021-02-13T00:29:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176158.16 epoch sec; seqNo:13072; 4059287.03 north; 592631.95 east 2021-02-13T00:29:18Z- AHRS_M2 msg: 1613176158.54 epoch sec; seqNo:12967 2021-02-13T00:29:18Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.18 psi 2021-02-13T00:29:18Z- handleDepth()-Depth_Keller msg: 1613176158545 epoch sec; seqNo:13073; depth 21.96 2021-02-13T00:29:18Z- handleDvl() - RDI_Pathfinder msg: 1613176158.56 epoch sec; seqNo:6394 2021-02-13T00:29:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.38 , 98.18 , 104.27 , 67.43 2021-02-13T00:29:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.04 , 0.17 2021-02-13T00:29:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176158.57 epoch sec; seqNo:13073; 4059287.03 north; 592631.95 east 2021-02-13T00:29:18Z- AHRS_M2 msg: 1613176158.94 epoch sec; seqNo:12968 2021-02-13T00:29:18Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-13T00:29:18Z- handleDepth()-Depth_Keller msg: 1613176158944 epoch sec; seqNo:13074; depth 21.81 2021-02-13T00:29:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176158.96 epoch sec; seqNo:13074; 4059287.42 north; 592633.17 east 2021-02-13T00:29:19Z- AHRS_M2 msg: 1613176159.34 epoch sec; seqNo:12969 2021-02-13T00:29:19Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-13T00:29:19Z- handleDepth()-Depth_Keller msg: 1613176159349 epoch sec; seqNo:13075; depth 21.70 2021-02-13T00:29:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176159.36 epoch sec; seqNo:13075; 4059287.80 north; 592633.16 east 2021-02-13T00:29:19Z- AHRS_M2 msg: 1613176159.75 epoch sec; seqNo:12970 2021-02-13T00:29:19Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-13T00:29:19Z- handleDepth()-Depth_Keller msg: 1613176159754 epoch sec; seqNo:13076; depth 21.58 2021-02-13T00:29:19Z- handleDvl() - RDI_Pathfinder msg: 1613176159.77 epoch sec; seqNo:6395 2021-02-13T00:29:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.60 , 90.82 , 104.20 , 69.06 2021-02-13T00:29:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.12 , 0.10 2021-02-13T00:29:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176159.78 epoch sec; seqNo:13076; 4059288.18 north; 592633.16 east 2021-02-13T00:29:20Z- AHRS_M2 msg: 1613176160.15 epoch sec; seqNo:12971 2021-02-13T00:29:20Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-13T00:29:20Z- handleDepth()-Depth_Keller msg: 1613176160161 epoch sec; seqNo:13077; depth 21.43 2021-02-13T00:29:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176160.18 epoch sec; seqNo:13077; 4059288.18 north; 592633.16 east 2021-02-13T00:29:20Z- AHRS_M2 msg: 1613176160.56 epoch sec; seqNo:12972 2021-02-13T00:29:20Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-13T00:29:20Z- handleDepth()-Depth_Keller msg: 1613176160560 epoch sec; seqNo:13078; depth 21.30 2021-02-13T00:29:20Z- handleDvl() - RDI_Pathfinder msg: 1613176160.58 epoch sec; seqNo:6396 2021-02-13T00:29:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 98.19 , 105.49 , 67.02 2021-02-13T00:29:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.11 , 0.02 2021-02-13T00:29:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176160.61 epoch sec; seqNo:13078; 4059288.56 north; 592633.15 east 2021-02-13T00:29:20Z- AHRS_M2 msg: 1613176160.96 epoch sec; seqNo:12973 2021-02-13T00:29:20Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-13T00:29:20Z- handleDepth()-Depth_Keller msg: 1613176160966 epoch sec; seqNo:13079; depth 21.16 2021-02-13T00:29:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176160.98 epoch sec; seqNo:13079; 4059288.95 north; 592634.37 east 2021-02-13T00:29:21Z- AHRS_M2 msg: 1613176161.36 epoch sec; seqNo:12974 2021-02-13T00:29:21Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-13T00:29:21Z- handleDepth()-Depth_Keller msg: 1613176161373 epoch sec; seqNo:13080; depth 21.04 2021-02-13T00:29:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176161.39 epoch sec; seqNo:13080; 4059288.95 north; 592634.37 east 2021-02-13T00:29:21Z- AHRS_M2 msg: 1613176161.77 epoch sec; seqNo:12975 2021-02-13T00:29:21Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-13T00:29:21Z- handleDepth()-Depth_Keller msg: 1613176161772 epoch sec; seqNo:13081; depth 20.89 2021-02-13T00:29:21Z- handleDvl() - RDI_Pathfinder msg: 1613176161.78 epoch sec; seqNo:6397 2021-02-13T00:29:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.20 , 97.98 , 106.50 , 78.51 2021-02-13T00:29:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.01 2021-02-13T00:29:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176161.83 epoch sec; seqNo:13081; 4059289.33 north; 592634.37 east 2021-02-13T00:29:22Z- AHRS_M2 msg: 1613176162.17 epoch sec; seqNo:12976 2021-02-13T00:29:22Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-13T00:29:22Z- handleDepth()-Depth_Keller msg: 1613176162181 epoch sec; seqNo:13082; depth 20.76 2021-02-13T00:29:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176162.19 epoch sec; seqNo:13082; 4059289.71 north; 592634.36 east 2021-02-13T00:29:22Z- AHRS_M2 msg: 1613176162.58 epoch sec; seqNo:12977 2021-02-13T00:29:22Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-13T00:29:22Z- handleDepth()-Depth_Keller msg: 1613176162580 epoch sec; seqNo:13083; depth 20.63 2021-02-13T00:29:22Z- handleDvl() - RDI_Pathfinder msg: 1613176162.60 epoch sec; seqNo:6398 2021-02-13T00:29:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.20 , 92.91 , 105.08 , 68.66 2021-02-13T00:29:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , 0.02 2021-02-13T00:29:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176162.61 epoch sec; seqNo:13083; 4059289.71 north; 592634.36 east 2021-02-13T00:29:22Z- LcmTrn::handleMessages() - handling 41 messages took 0.66 ms and 128 calls... 2021-02-13T00:29:22Z- waiting for 1613176160.76, time is 1613176162.61 2021-02-13T00:29:22Z- LcmTrn::updateTrn() equating pose 1613176162.61 and meas times 1613176162.61 2021-02-13T00:29:22Z- LcmTrn::updateTrn() >>>> pose time 1613176162.61, meas time 1613176162.61 <<<< 2021-02-13T00:29:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176162.61 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:29:22Z- TerrainNav::motionUpdate() - currEstimate = (4059290, 592634.36, 20.63) 2021-02-13T00:29:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:29:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:29:22Z- TNavBF::Projecting Measurements 2021-02-13T00:29:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:29:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:29:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:29:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:29:22Z- TNavFilter::4 good beams. 2021-02-13T00:29:22Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:29:22Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:29:22Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:29:22Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:29:22Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:29:22Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:29:22Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:29:22Z- TNavBF::mmse selected from filter -1 2021-02-13T00:29:22Z- TerrainMapOctree: (vehN, vehE ) = (4059289.71, 592634.36). 2021-02-13T00:29:22Z- TerrainMapOctree: Min Distance = 2540.28. 2021-02-13T00:29:22Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:29:22Z- There is no specific good beam that all particles have in common. 2021-02-13T00:29:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:29:22Z- Old Depth Bias: 0.00 DepthBiasCov: 2.47 K: 0.187 ExpectedMeasDiff: nan currMeas.covariance: 9.05 2021-02-13T00:29:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:29:22Z- Old Depth Bias: 0.00 DepthBiasCov: 2.01 K: 0.115 ExpectedMeasDiff: nan currMeas.covariance: 13.81 2021-02-13T00:29:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:29:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.78 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 17.67 2021-02-13T00:29:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:29:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.63 K: 0.150 ExpectedMeasDiff: nan currMeas.covariance: 7.54 2021-02-13T00:29:22Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:29:22Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:29:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:29:22Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:29:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:29:22Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:29:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:29:22Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:29:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:29:22Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:29:22Z- Filter 0 No beams used in subcloud update 2021-02-13T00:29:22Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:29:22Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:29:22Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 700.6 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T00:29:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176162.61 sec. 2021-02-13T00:29:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:29:22Z- TNavBF::mmse selected from filter -1 2021-02-13T00:29:22Z- variances([0], [2], and psi_berg) = 700.584384 832.998250 0.000000 2021-02-13T00:29:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:29:22Z- LcmTrn::publishEstimates() - MLE : 4059289.21 592605.86 20.63 2021-02-13T00:29:22Z- LcmTrn::publishEstimates() - MMSE : 4059289.71 592634.36 20.63 2021-02-13T00:29:22Z- LcmTrn::publishEstimates() - COVAR: 700.58 833.00 1.39 0.00 2021-02-13T00:29:22Z- AHRS_M2 msg: 1613176162.98 epoch sec; seqNo:12978 2021-02-13T00:29:22Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-13T00:29:22Z- handleDepth()-Depth_Keller msg: 1613176162984 epoch sec; seqNo:13084; depth 20.49 2021-02-13T00:29:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176162.99 epoch sec; seqNo:13084; 4059290.09 north; 592634.36 east 2021-02-13T00:29:23Z- AHRS_M2 msg: 1613176163.38 epoch sec; seqNo:12979 2021-02-13T00:29:23Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-13T00:29:23Z- handleDepth()-Depth_Keller msg: 1613176163387 epoch sec; seqNo:13085; depth 20.36 2021-02-13T00:29:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176163.40 epoch sec; seqNo:13085; 4059290.48 north; 592635.58 east 2021-02-13T00:29:23Z- AHRS_M2 msg: 1613176163.79 epoch sec; seqNo:12980 2021-02-13T00:29:23Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.18 psi 2021-02-13T00:29:23Z- handleDepth()-Depth_Keller msg: 1613176163798 epoch sec; seqNo:13086; depth 20.23 2021-02-13T00:29:23Z- handleDvl() - RDI_Pathfinder msg: 1613176163.81 epoch sec; seqNo:6399 2021-02-13T00:29:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 93.32 , 105.49 , 70.29 2021-02-13T00:29:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , 0.04 2021-02-13T00:29:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176163.82 epoch sec; seqNo:13086; 4059290.86 north; 592635.57 east 2021-02-13T00:29:24Z- AHRS_M2 msg: 1613176164.19 epoch sec; seqNo:12981 2021-02-13T00:29:24Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.18 psi 2021-02-13T00:29:24Z- handleDepth()-Depth_Keller msg: 1613176164196 epoch sec; seqNo:13087; depth 20.09 2021-02-13T00:29:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176164.21 epoch sec; seqNo:13087; 4059290.86 north; 592635.57 east 2021-02-13T00:29:24Z- AHRS_M2 msg: 1613176164.60 epoch sec; seqNo:12982 2021-02-13T00:29:24Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.19 psi 2021-02-13T00:29:24Z- handleDepth()-Depth_Keller msg: 1613176164608 epoch sec; seqNo:13088; depth 19.96 2021-02-13T00:29:24Z- handleDvl() - RDI_Pathfinder msg: 1613176164.62 epoch sec; seqNo:6400 2021-02-13T00:29:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 98.05 , 105.35 , 70.29 2021-02-13T00:29:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , 0.01 2021-02-13T00:29:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176164.63 epoch sec; seqNo:13088; 4059291.24 north; 592635.57 east 2021-02-13T00:29:25Z- AHRS_M2 msg: 1613176165.00 epoch sec; seqNo:12983 2021-02-13T00:29:25Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.20 psi 2021-02-13T00:29:25Z- handleDepth()-Depth_Keller msg: 1613176165004 epoch sec; seqNo:13089; depth 19.83 2021-02-13T00:29:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176165.02 epoch sec; seqNo:13089; 4059291.62 north; 592635.56 east 2021-02-13T00:29:25Z- AHRS_M2 msg: 1613176165.40 epoch sec; seqNo:12984 2021-02-13T00:29:25Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.21 psi 2021-02-13T00:29:25Z- handleDepth()-Depth_Keller msg: 1613176165408 epoch sec; seqNo:13090; depth 19.69 2021-02-13T00:29:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176165.42 epoch sec; seqNo:13090; 4059291.62 north; 592635.56 east 2021-02-13T00:29:25Z- AHRS_M2 msg: 1613176165.81 epoch sec; seqNo:12985 2021-02-13T00:29:25Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.21 psi 2021-02-13T00:29:25Z- handleDepth()-Depth_Keller msg: 1613176165813 epoch sec; seqNo:13091; depth 19.58 2021-02-13T00:29:25Z- handleDvl() - RDI_Pathfinder msg: 1613176165.82 epoch sec; seqNo:6401 2021-02-13T00:29:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 106.50 , 69.48 2021-02-13T00:29:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , -0.03 2021-02-13T00:29:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176165.83 epoch sec; seqNo:13091; 4059292.01 north; 592636.78 east 2021-02-13T00:29:26Z- AHRS_M2 msg: 1613176166.21 epoch sec; seqNo:12986 2021-02-13T00:29:26Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.21 psi 2021-02-13T00:29:26Z- handleDepth()-Depth_Keller msg: 1613176166239 epoch sec; seqNo:13092; depth 19.44 2021-02-13T00:29:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176166.25 epoch sec; seqNo:13092; 4059292.39 north; 592636.78 east 2021-02-13T00:29:26Z- AHRS_M2 msg: 1613176166.62 epoch sec; seqNo:12987 2021-02-13T00:29:26Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.21 psi 2021-02-13T00:29:26Z- handleDepth()-Depth_Keller msg: 1613176166626 epoch sec; seqNo:13093; depth 19.30 2021-02-13T00:29:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176166.66 epoch sec; seqNo:13093; 4059292.76 north; 592636.77 east 2021-02-13T00:29:27Z- AHRS_M2 msg: 1613176167.02 epoch sec; seqNo:12988 2021-02-13T00:29:27Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.21 psi 2021-02-13T00:29:27Z- handleDepth()-Depth_Keller msg: 1613176167024 epoch sec; seqNo:13094; depth 19.17 2021-02-13T00:29:27Z- handleDvl() - RDI_Pathfinder msg: 1613176167.04 epoch sec; seqNo:6402 2021-02-13T00:29:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 96.70 , 106.43 , 78.44 2021-02-13T00:29:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , -0.07 2021-02-13T00:29:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176167.05 epoch sec; seqNo:13094; 4059292.76 north; 592636.77 east 2021-02-13T00:29:27Z- AHRS_M2 msg: 1613176167.42 epoch sec; seqNo:12989 2021-02-13T00:29:27Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.20 psi 2021-02-13T00:29:27Z- handleDepth()-Depth_Keller msg: 1613176167430 epoch sec; seqNo:13095; depth 19.07 2021-02-13T00:29:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176167.44 epoch sec; seqNo:13095; 4059293.14 north; 592636.77 east 2021-02-13T00:29:27Z- AHRS_M2 msg: 1613176167.83 epoch sec; seqNo:12990 2021-02-13T00:29:27Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.20 psi 2021-02-13T00:29:27Z- handleDepth()-Depth_Keller msg: 1613176167832 epoch sec; seqNo:13096; depth 18.95 2021-02-13T00:29:27Z- handleDvl() - RDI_Pathfinder msg: 1613176167.85 epoch sec; seqNo:6403 2021-02-13T00:29:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 97.78 , 105.08 , 78.31 2021-02-13T00:29:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.09 , -0.05 2021-02-13T00:29:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176167.86 epoch sec; seqNo:13096; 4059293.52 north; 592636.76 east 2021-02-13T00:29:27Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-13T00:29:27Z- waiting for 1613176165.61, time is 1613176167.86 2021-02-13T00:29:27Z- LcmTrn::updateTrn() equating pose 1613176167.86 and meas times 1613176167.86 2021-02-13T00:29:27Z- LcmTrn::updateTrn() >>>> pose time 1613176167.86, meas time 1613176167.86 <<<< 2021-02-13T00:29:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176167.86 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:29:27Z- TerrainNav::motionUpdate() - currEstimate = (4059294, 592636.76, 18.95) 2021-02-13T00:29:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:29:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:29:27Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176167.86 sec.; No relevant navigation data available 2021-02-13T00:29:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:29:27Z- TNavBF::mmse selected from filter -1 2021-02-13T00:29:27Z- variances([0], [2], and psi_berg) = 700.526568 832.998219 0.000000 2021-02-13T00:29:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:29:27Z- LcmTrn::publishEstimates() - MLE : 4059293.02 592608.26 18.95 2021-02-13T00:29:27Z- LcmTrn::publishEstimates() - MMSE : 4059293.52 592636.76 18.95 2021-02-13T00:29:27Z- LcmTrn::publishEstimates() - COVAR: 700.53 833.00 1.62 0.00 2021-02-13T00:29:28Z- AHRS_M2 msg: 1613176168.23 epoch sec; seqNo:12991 2021-02-13T00:29:28Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.20 psi 2021-02-13T00:29:28Z- handleDepth()-Depth_Keller msg: 1613176168236 epoch sec; seqNo:13097; depth 18.81 2021-02-13T00:29:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176168.25 epoch sec; seqNo:13097; 4059293.52 north; 592636.76 east 2021-02-13T00:29:28Z- AHRS_M2 msg: 1613176168.66 epoch sec; seqNo:12992 2021-02-13T00:29:28Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.20 psi 2021-02-13T00:29:28Z- handleDepth()-Depth_Keller msg: 1613176168671 epoch sec; seqNo:13098; depth 18.69 2021-02-13T00:29:28Z- handleDvl() - RDI_Pathfinder msg: 1613176168.68 epoch sec; seqNo:6404 2021-02-13T00:29:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 96.56 , 105.08 , 78.31 2021-02-13T00:29:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.16 , 0.02 2021-02-13T00:29:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176168.70 epoch sec; seqNo:13098; 4059293.91 north; 592637.98 east 2021-02-13T00:29:29Z- handleDepth()-Depth_Keller msg: 1613176169058 epoch sec; seqNo:13099; depth 18.56 2021-02-13T00:29:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176169.07 epoch sec; seqNo:13099; 4059294.29 north; 592637.98 east 2021-02-13T00:29:29Z- AHRS_M2 msg: 1613176169.45 epoch sec; seqNo:12993 2021-02-13T00:29:29Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.20 psi 2021-02-13T00:29:29Z- handleDepth()-Depth_Keller msg: 1613176169455 epoch sec; seqNo:13100; depth 18.45 2021-02-13T00:29:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176169.48 epoch sec; seqNo:13100; 4059294.67 north; 592637.97 east 2021-02-13T00:29:29Z- AHRS_M2 msg: 1613176169.85 epoch sec; seqNo:12994 2021-02-13T00:29:29Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.20 psi 2021-02-13T00:29:29Z- handleDepth()-Depth_Keller msg: 1613176169856 epoch sec; seqNo:13101; depth 18.34 2021-02-13T00:29:29Z- handleDvl() - RDI_Pathfinder msg: 1613176169.87 epoch sec; seqNo:6405 2021-02-13T00:29:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.20 , 97.78 , 103.86 , 69.48 2021-02-13T00:29:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.19 , 0.09 2021-02-13T00:29:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176169.88 epoch sec; seqNo:13101; 4059295.05 north; 592637.97 east 2021-02-13T00:29:30Z- AHRS_M2 msg: 1613176170.25 epoch sec; seqNo:12995 2021-02-13T00:29:30Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.20 psi 2021-02-13T00:29:30Z- handleDepth()-Depth_Keller msg: 1613176170258 epoch sec; seqNo:13102; depth 18.22 2021-02-13T00:29:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176170.27 epoch sec; seqNo:13102; 4059295.05 north; 592637.97 east 2021-02-13T00:29:30Z- AHRS_M2 msg: 1613176170.66 epoch sec; seqNo:12996 2021-02-13T00:29:30Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.19 psi 2021-02-13T00:29:30Z- handleDepth()-Depth_Keller msg: 1613176170660 epoch sec; seqNo:13103; depth 18.12 2021-02-13T00:29:30Z- handleDvl() - RDI_Pathfinder msg: 1613176170.67 epoch sec; seqNo:6406 2021-02-13T00:29:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 98.05 , 105.35 , 70.70 2021-02-13T00:29:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.14 , 0.14 2021-02-13T00:29:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176170.69 epoch sec; seqNo:13103; 4059295.43 north; 592637.96 east 2021-02-13T00:29:31Z- AHRS_M2 msg: 1613176171.06 epoch sec; seqNo:12997 2021-02-13T00:29:31Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.19 psi 2021-02-13T00:29:31Z- handleDepth()-Depth_Keller msg: 1613176171064 epoch sec; seqNo:13104; depth 17.99 2021-02-13T00:29:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176171.08 epoch sec; seqNo:13104; 4059295.81 north; 592637.96 east 2021-02-13T00:29:31Z- AHRS_M2 msg: 1613176171.47 epoch sec; seqNo:12998 2021-02-13T00:29:31Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.19 psi 2021-02-13T00:29:31Z- handleDepth()-Depth_Keller msg: 1613176171474 epoch sec; seqNo:13105; depth 17.90 2021-02-13T00:29:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176171.50 epoch sec; seqNo:13105; 4059296.19 north; 592637.96 east 2021-02-13T00:29:31Z- handleDepth()-Depth_Keller msg: 1613176171893 epoch sec; seqNo:13106; depth 17.78 2021-02-13T00:29:31Z- handleDvl() - RDI_Pathfinder msg: 1613176171.90 epoch sec; seqNo:6407 2021-02-13T00:29:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 97.98 , 105.28 , 72.75 2021-02-13T00:29:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.07 , 0.13 2021-02-13T00:29:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176171.91 epoch sec; seqNo:13106; 4059296.58 north; 592639.17 east 2021-02-13T00:29:32Z- AHRS_M2 msg: 1613176172.27 epoch sec; seqNo:12999 2021-02-13T00:29:32Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.17 psi 2021-02-13T00:29:32Z- handleDepth()-Depth_Keller msg: 1613176172277 epoch sec; seqNo:13107; depth 17.68 2021-02-13T00:29:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176172.29 epoch sec; seqNo:13107; 4059296.96 north; 592639.17 east 2021-02-13T00:29:32Z- AHRS_M2 msg: 1613176172.68 epoch sec; seqNo:13000 2021-02-13T00:29:32Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.17 psi 2021-02-13T00:29:32Z- handleDepth()-Depth_Keller msg: 1613176172681 epoch sec; seqNo:13108; depth 17.57 2021-02-13T00:29:32Z- handleDvl() - RDI_Pathfinder msg: 1613176172.69 epoch sec; seqNo:6408 2021-02-13T00:29:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.91 , 105.21 , 70.70 2021-02-13T00:29:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , 0.09 2021-02-13T00:29:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176172.70 epoch sec; seqNo:13108; 4059297.34 north; 592639.16 east 2021-02-13T00:29:33Z- LcmTrn::handleMessages() - handling 39 messages took 0.57 ms and 128 calls... 2021-02-13T00:29:33Z- waiting for 1613176170.86, time is 1613176172.70 2021-02-13T00:29:33Z- LcmTrn::updateTrn() equating pose 1613176172.70 and meas times 1613176172.70 2021-02-13T00:29:33Z- LcmTrn::updateTrn() >>>> pose time 1613176172.70, meas time 1613176172.70 <<<< 2021-02-13T00:29:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176172.70 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:29:33Z- TerrainNav::motionUpdate() - currEstimate = (4059297, 592639.16, 17.57) 2021-02-13T00:29:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:29:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:29:33Z- TNavBF::Projecting Measurements 2021-02-13T00:29:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:29:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:29:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:29:33Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:29:33Z- TNavFilter::3 good beams. 2021-02-13T00:29:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:29:33Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:29:33Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:29:33Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:29:33Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:29:33Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:29:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:29:33Z- TNavBF::mmse selected from filter -1 2021-02-13T00:29:33Z- TerrainMapOctree: (vehN, vehE ) = (4059297.34, 592639.16). 2021-02-13T00:29:33Z- TerrainMapOctree: Min Distance = 2531.34. 2021-02-13T00:29:33Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:29:33Z- There is no specific good beam that all particles have in common. 2021-02-13T00:29:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:29:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.85 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:29:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:29:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.67 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 17.71 2021-02-13T00:29:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:29:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.53 K: 0.136 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-13T00:29:33Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:29:33Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:29:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:29:33Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:29:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:29:33Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:29:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:29:33Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:29:33Z- Filter 0 No beams used in subcloud update 2021-02-13T00:29:33Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:29:33Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:29:33Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 700.5 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:29:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176172.70 sec. 2021-02-13T00:29:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:29:33Z- TNavBF::mmse selected from filter -1 2021-02-13T00:29:33Z- variances([0], [2], and psi_berg) = 700.468789 832.998189 0.000000 2021-02-13T00:29:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:29:33Z- LcmTrn::publishEstimates() - MLE : 4059296.84 592610.66 17.57 2021-02-13T00:29:33Z- LcmTrn::publishEstimates() - MMSE : 4059297.34 592639.16 17.57 2021-02-13T00:29:33Z- LcmTrn::publishEstimates() - COVAR: 700.47 833.00 1.33 0.00 2021-02-13T00:29:33Z- AHRS_M2 msg: 1613176173.08 epoch sec; seqNo:13001 2021-02-13T00:29:33Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.17 psi 2021-02-13T00:29:33Z- handleDepth()-Depth_Keller msg: 1613176173093 epoch sec; seqNo:13109; depth 17.47 2021-02-13T00:29:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176173.10 epoch sec; seqNo:13109; 4059297.34 north; 592639.16 east 2021-02-13T00:29:33Z- AHRS_M2 msg: 1613176173.48 epoch sec; seqNo:13002 2021-02-13T00:29:33Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.17 psi 2021-02-13T00:29:33Z- handleDepth()-Depth_Keller msg: 1613176173489 epoch sec; seqNo:13110; depth 17.35 2021-02-13T00:29:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176173.51 epoch sec; seqNo:13110; 4059297.72 north; 592639.16 east 2021-02-13T00:29:33Z- AHRS_M2 msg: 1613176173.89 epoch sec; seqNo:13003 2021-02-13T00:29:33Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.16 psi 2021-02-13T00:29:33Z- handleDepth()-Depth_Keller msg: 1613176173892 epoch sec; seqNo:13111; depth 17.23 2021-02-13T00:29:33Z- handleDvl() - RDI_Pathfinder msg: 1613176173.90 epoch sec; seqNo:6409 2021-02-13T00:29:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 97.65 , 103.73 , 72.74 2021-02-13T00:29:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , 0.04 2021-02-13T00:29:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176173.93 epoch sec; seqNo:13111; 4059298.10 north; 592639.16 east 2021-02-13T00:29:34Z- AHRS_M2 msg: 1613176174.29 epoch sec; seqNo:13004 2021-02-13T00:29:34Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.16 psi 2021-02-13T00:29:34Z- handleDepth()-Depth_Keller msg: 1613176174297 epoch sec; seqNo:13112; depth 17.11 2021-02-13T00:29:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176174.31 epoch sec; seqNo:13112; 4059298.47 north; 592639.15 east 2021-02-13T00:29:34Z- AHRS_M2 msg: 1613176174.70 epoch sec; seqNo:13005 2021-02-13T00:29:34Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.17 psi 2021-02-13T00:29:34Z- handleDepth()-Depth_Keller msg: 1613176174709 epoch sec; seqNo:13113; depth 17.00 2021-02-13T00:29:34Z- handleDvl() - RDI_Pathfinder msg: 1613176174.72 epoch sec; seqNo:6410 2021-02-13T00:29:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.69 , 97.85 , 106.37 , 78.38 2021-02-13T00:29:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , 0.04 2021-02-13T00:29:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176174.73 epoch sec; seqNo:13113; 4059298.87 north; 592640.37 east 2021-02-13T00:29:35Z- AHRS_M2 msg: 1613176175.10 epoch sec; seqNo:13006 2021-02-13T00:29:35Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.17 psi 2021-02-13T00:29:35Z- handleDepth()-Depth_Keller msg: 1613176175104 epoch sec; seqNo:13114; depth 16.88 2021-02-13T00:29:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176175.12 epoch sec; seqNo:13114; 4059298.87 north; 592640.37 east 2021-02-13T00:29:35Z- AHRS_M2 msg: 1613176175.51 epoch sec; seqNo:13007 2021-02-13T00:29:35Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.17 psi 2021-02-13T00:29:35Z- handleDepth()-Depth_Keller msg: 1613176175510 epoch sec; seqNo:13115; depth 16.77 2021-02-13T00:29:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176175.52 epoch sec; seqNo:13115; 4059299.24 north; 592640.36 east 2021-02-13T00:29:35Z- AHRS_M2 msg: 1613176175.91 epoch sec; seqNo:13008 2021-02-13T00:29:35Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.17 psi 2021-02-13T00:29:35Z- handleDepth()-Depth_Keller msg: 1613176175918 epoch sec; seqNo:13116; depth 16.65 2021-02-13T00:29:35Z- handleDvl() - RDI_Pathfinder msg: 1613176175.93 epoch sec; seqNo:6411 2021-02-13T00:29:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 95.28 , 107.45 , 72.33 2021-02-13T00:29:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.04 , 0.05 2021-02-13T00:29:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176175.94 epoch sec; seqNo:13116; 4059299.62 north; 592640.36 east 2021-02-13T00:29:36Z- AHRS_M2 msg: 1613176176.31 epoch sec; seqNo:13009 2021-02-13T00:29:36Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.17 psi 2021-02-13T00:29:36Z- handleDepth()-Depth_Keller msg: 1613176176316 epoch sec; seqNo:13117; depth 16.53 2021-02-13T00:29:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176176.33 epoch sec; seqNo:13117; 4059300.00 north; 592640.36 east 2021-02-13T00:29:36Z- AHRS_M2 msg: 1613176176.72 epoch sec; seqNo:13010 2021-02-13T00:29:36Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.16 psi 2021-02-13T00:29:36Z- handleDepth()-Depth_Keller msg: 1613176176725 epoch sec; seqNo:13118; depth 16.43 2021-02-13T00:29:36Z- handleDvl() - RDI_Pathfinder msg: 1613176176.74 epoch sec; seqNo:6412 2021-02-13T00:29:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.81 , 95.42 , 107.58 , 73.56 2021-02-13T00:29:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.09 , 0.04 2021-02-13T00:29:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176176.76 epoch sec; seqNo:13118; 4059300.02 north; 592641.58 east 2021-02-13T00:29:37Z- AHRS_M2 msg: 1613176177.12 epoch sec; seqNo:13011 2021-02-13T00:29:37Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.16 psi 2021-02-13T00:29:37Z- handleDepth()-Depth_Keller msg: 1613176177133 epoch sec; seqNo:13119; depth 16.32 2021-02-13T00:29:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176177.15 epoch sec; seqNo:13119; 4059300.39 north; 592641.57 east 2021-02-13T00:29:37Z- AHRS_M2 msg: 1613176177.53 epoch sec; seqNo:13012 2021-02-13T00:29:37Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T00:29:37Z- handleDepth()-Depth_Keller msg: 1613176177546 epoch sec; seqNo:13120; depth 16.20 2021-02-13T00:29:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176177.56 epoch sec; seqNo:13120; 4059300.77 north; 592641.57 east 2021-02-13T00:29:37Z- AHRS_M2 msg: 1613176177.93 epoch sec; seqNo:13013 2021-02-13T00:29:37Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-13T00:29:37Z- handleDepth()-Depth_Keller msg: 1613176177937 epoch sec; seqNo:13121; depth 16.09 2021-02-13T00:29:37Z- handleDvl() - RDI_Pathfinder msg: 1613176177.95 epoch sec; seqNo:6413 2021-02-13T00:29:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.88 , 97.85 , 108.80 , 78.37 2021-02-13T00:29:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.12 , -0.05 2021-02-13T00:29:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176177.96 epoch sec; seqNo:13121; 4059301.15 north; 592641.56 east 2021-02-13T00:29:38Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 130 calls... 2021-02-13T00:29:38Z- waiting for 1613176175.70, time is 1613176177.96 2021-02-13T00:29:38Z- LcmTrn::updateTrn() equating pose 1613176177.96 and meas times 1613176177.96 2021-02-13T00:29:38Z- LcmTrn::updateTrn() >>>> pose time 1613176177.96, meas time 1613176177.96 <<<< 2021-02-13T00:29:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176177.96 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:29:38Z- TerrainNav::motionUpdate() - currEstimate = (4059301, 592641.56, 16.09) 2021-02-13T00:29:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:29:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:29:38Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176177.96 sec.; No relevant navigation data available 2021-02-13T00:29:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:29:38Z- TNavBF::mmse selected from filter -1 2021-02-13T00:29:38Z- variances([0], [2], and psi_berg) = 700.411045 832.998158 0.000000 2021-02-13T00:29:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:29:38Z- LcmTrn::publishEstimates() - MLE : 4059300.65 592613.06 16.09 2021-02-13T00:29:38Z- LcmTrn::publishEstimates() - MMSE : 4059301.15 592641.56 16.09 2021-02-13T00:29:38Z- LcmTrn::publishEstimates() - COVAR: 700.41 833.00 1.56 0.00 2021-02-13T00:29:38Z- AHRS_M2 msg: 1613176178.33 epoch sec; seqNo:13014 2021-02-13T00:29:38Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-13T00:29:38Z- handleDepth()-Depth_Keller msg: 1613176178341 epoch sec; seqNo:13122; depth 15.98 2021-02-13T00:29:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176178.35 epoch sec; seqNo:13122; 4059301.15 north; 592641.56 east 2021-02-13T00:29:38Z- AHRS_M2 msg: 1613176178.74 epoch sec; seqNo:13015 2021-02-13T00:29:38Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.14 psi 2021-02-13T00:29:38Z- handleDepth()-Depth_Keller msg: 1613176178749 epoch sec; seqNo:13123; depth 15.87 2021-02-13T00:29:38Z- handleDvl() - RDI_Pathfinder msg: 1613176178.76 epoch sec; seqNo:6414 2021-02-13T00:29:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 97.64 , 107.38 , 72.75 2021-02-13T00:29:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.06 , -0.14 2021-02-13T00:29:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176178.77 epoch sec; seqNo:13123; 4059301.53 north; 592641.56 east 2021-02-13T00:29:39Z- AHRS_M2 msg: 1613176179.14 epoch sec; seqNo:13016 2021-02-13T00:29:39Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.14 psi 2021-02-13T00:29:39Z- handleDepth()-Depth_Keller msg: 1613176179148 epoch sec; seqNo:13124; depth 15.75 2021-02-13T00:29:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176179.16 epoch sec; seqNo:13124; 4059301.54 north; 592642.78 east 2021-02-13T00:29:39Z- AHRS_M2 msg: 1613176179.54 epoch sec; seqNo:13017 2021-02-13T00:29:39Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:29:39Z- handleDepth()-Depth_Keller msg: 1613176179549 epoch sec; seqNo:13125; depth 15.65 2021-02-13T00:29:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176179.56 epoch sec; seqNo:13125; 4059301.92 north; 592642.78 east 2021-02-13T00:29:39Z- AHRS_M2 msg: 1613176179.95 epoch sec; seqNo:13018 2021-02-13T00:29:39Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:29:39Z- handleDepth()-Depth_Keller msg: 1613176179954 epoch sec; seqNo:13126; depth 15.52 2021-02-13T00:29:39Z- handleDvl() - RDI_Pathfinder msg: 1613176179.97 epoch sec; seqNo:6415 2021-02-13T00:29:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 97.78 , 107.51 , 78.31 2021-02-13T00:29:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.05 , -0.19 2021-02-13T00:29:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176179.98 epoch sec; seqNo:13126; 4059302.30 north; 592642.77 east 2021-02-13T00:29:40Z- AHRS_M2 msg: 1613176180.35 epoch sec; seqNo:13019 2021-02-13T00:29:40Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:29:40Z- handleDepth()-Depth_Keller msg: 1613176180356 epoch sec; seqNo:13127; depth 15.40 2021-02-13T00:29:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176180.37 epoch sec; seqNo:13127; 4059302.30 north; 592642.77 east 2021-02-13T00:29:40Z- AHRS_M2 msg: 1613176180.76 epoch sec; seqNo:13020 2021-02-13T00:29:40Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-13T00:29:40Z- handleDepth()-Depth_Keller msg: 1613176180760 epoch sec; seqNo:13128; depth 15.30 2021-02-13T00:29:40Z- handleDvl() - RDI_Pathfinder msg: 1613176180.77 epoch sec; seqNo:6416 2021-02-13T00:29:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.83 , 97.65 , 107.38 , 72.74 2021-02-13T00:29:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.22 , -0.15 2021-02-13T00:29:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176180.79 epoch sec; seqNo:13128; 4059302.68 north; 592642.77 east 2021-02-13T00:29:41Z- AHRS_M2 msg: 1613176181.16 epoch sec; seqNo:13021 2021-02-13T00:29:41Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:29:41Z- handleDepth()-Depth_Keller msg: 1613176181165 epoch sec; seqNo:13129; depth 15.16 2021-02-13T00:29:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176181.18 epoch sec; seqNo:13129; 4059303.06 north; 592642.76 east 2021-02-13T00:29:41Z- AHRS_M2 msg: 1613176181.59 epoch sec; seqNo:13022 2021-02-13T00:29:41Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.14 psi 2021-02-13T00:29:41Z- handleDepth()-Depth_Keller msg: 1613176181596 epoch sec; seqNo:13130; depth 15.06 2021-02-13T00:29:41Z- handleDvl() - RDI_Pathfinder msg: 1613176181.61 epoch sec; seqNo:6417 2021-02-13T00:29:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 96.56 , 107.51 , 73.56 2021-02-13T00:29:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.32 , -0.04 2021-02-13T00:29:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176181.62 epoch sec; seqNo:13130; 4059303.44 north; 592642.76 east 2021-02-13T00:29:41Z- AHRS_M2 msg: 1613176181.97 epoch sec; seqNo:13023 2021-02-13T00:29:41Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:29:41Z- handleDepth()-Depth_Keller msg: 1613176181977 epoch sec; seqNo:13131; depth 14.92 2021-02-13T00:29:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176181.99 epoch sec; seqNo:13131; 4059303.44 north; 592642.76 east 2021-02-13T00:29:42Z- AHRS_M2 msg: 1613176182.37 epoch sec; seqNo:13024 2021-02-13T00:29:42Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-13T00:29:42Z- handleDepth()-Depth_Keller msg: 1613176182381 epoch sec; seqNo:13132; depth 14.82 2021-02-13T00:29:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176182.39 epoch sec; seqNo:13132; 4059303.82 north; 592642.76 east 2021-02-13T00:29:42Z- AHRS_M2 msg: 1613176182.79 epoch sec; seqNo:13025 2021-02-13T00:29:42Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.14 psi 2021-02-13T00:29:42Z- handleDepth()-Depth_Keller msg: 1613176182793 epoch sec; seqNo:13133; depth 14.71 2021-02-13T00:29:42Z- handleDvl() - RDI_Pathfinder msg: 1613176182.83 epoch sec; seqNo:6418 2021-02-13T00:29:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:29:42Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:29:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176182.86 epoch sec; seqNo:13133; 4059304.20 north; 592642.75 east 2021-02-13T00:29:43Z- LcmTrn::handleMessages() - handling 41 messages took 0.66 ms and 128 calls... 2021-02-13T00:29:43Z- waiting for 1613176180.96, time is 1613176182.86 2021-02-13T00:29:43Z- LcmTrn::updateTrn() equating pose 1613176182.86 and meas times 1613176182.86 2021-02-13T00:29:43Z- LcmTrn::updateTrn() >>>> pose time 1613176182.86, meas time 1613176182.86 <<<< 2021-02-13T00:29:43Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613176182.86, ping # 0. 2021-02-13T00:29:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176182.86 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:29:43Z- TerrainNav::motionUpdate() - currEstimate = (4059304, 592642.75, 14.71) 2021-02-13T00:29:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:29:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:29:43Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:29:43Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:29:43Z- TNavBF::Projecting Measurements 2021-02-13T00:29:43Z- TNavFilter::Measurement from time = 1613176182.86 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:29:43Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:29:43Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:29:43Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:29:43Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:29:43Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:29:43Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:29:43Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:29:43Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 700.4 EastVariance: 833.0 DepthVariance: 1.7 this->DepthBiasCov: 1.7 2021-02-13T00:29:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:29:43Z- TNavBF::mmse selected from filter -1 2021-02-13T00:29:43Z- variances([0], [2], and psi_berg) = 700.361910 832.998132 0.000000 2021-02-13T00:29:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:29:43Z- LcmTrn::publishEstimates() - MLE : 4059303.70 592614.25 14.71 2021-02-13T00:29:43Z- LcmTrn::publishEstimates() - MMSE : 4059304.20 592642.75 14.71 2021-02-13T00:29:43Z- LcmTrn::publishEstimates() - COVAR: 700.36 833.00 1.72 0.00 2021-02-13T00:29:43Z- AHRS_M2 msg: 1613176183.18 epoch sec; seqNo:13026 2021-02-13T00:29:43Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.14 psi 2021-02-13T00:29:43Z- handleDepth()-Depth_Keller msg: 1613176183184 epoch sec; seqNo:13134; depth 14.57 2021-02-13T00:29:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176183.20 epoch sec; seqNo:13134; 4059304.58 north; 592642.75 east 2021-02-13T00:29:43Z- AHRS_M2 msg: 1613176183.59 epoch sec; seqNo:13027 2021-02-13T00:29:43Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-13T00:29:43Z- handleDepth()-Depth_Keller msg: 1613176183597 epoch sec; seqNo:13135; depth 14.46 2021-02-13T00:29:43Z- handleDvl() - RDI_Pathfinder msg: 1613176183.64 epoch sec; seqNo:6419 2021-02-13T00:29:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.78 , 95.95 , 110.83 , 73.97 2021-02-13T00:29:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.21 , 0.26 2021-02-13T00:29:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176183.65 epoch sec; seqNo:13135; 4059304.97 north; 592643.96 east 2021-02-13T00:29:43Z- AHRS_M2 msg: 1613176183.99 epoch sec; seqNo:13028 2021-02-13T00:29:43Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T00:29:43Z- handleDepth()-Depth_Keller msg: 1613176183992 epoch sec; seqNo:13136; depth 14.34 2021-02-13T00:29:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176184.00 epoch sec; seqNo:13136; 4059305.35 north; 592643.96 east 2021-02-13T00:29:44Z- AHRS_M2 msg: 1613176184.39 epoch sec; seqNo:13029 2021-02-13T00:29:44Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.15 psi 2021-02-13T00:29:44Z- handleDepth()-Depth_Keller msg: 1613176184396 epoch sec; seqNo:13137; depth 14.23 2021-02-13T00:29:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176184.40 epoch sec; seqNo:13137; 4059305.72 north; 592643.96 east 2021-02-13T00:29:44Z- AHRS_M2 msg: 1613176184.80 epoch sec; seqNo:13030 2021-02-13T00:29:44Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.16 psi 2021-02-13T00:29:44Z- handleDepth()-Depth_Keller msg: 1613176184809 epoch sec; seqNo:13138; depth 14.11 2021-02-13T00:29:44Z- handleDvl() - RDI_Pathfinder msg: 1613176184.82 epoch sec; seqNo:6420 2021-02-13T00:29:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 97.64 , 108.60 , 75.61 2021-02-13T00:29:44Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.07 , 0.29 2021-02-13T00:29:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176184.83 epoch sec; seqNo:13138; 4059306.10 north; 592643.95 east 2021-02-13T00:29:45Z- AHRS_M2 msg: 1613176185.20 epoch sec; seqNo:13031 2021-02-13T00:29:45Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.17 psi 2021-02-13T00:29:45Z- handleDepth()-Depth_Keller msg: 1613176185204 epoch sec; seqNo:13139; depth 13.99 2021-02-13T00:29:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176185.23 epoch sec; seqNo:13139; 4059306.48 north; 592643.95 east 2021-02-13T00:29:45Z- AHRS_M2 msg: 1613176185.61 epoch sec; seqNo:13032 2021-02-13T00:29:45Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.17 psi 2021-02-13T00:29:45Z- handleDepth()-Depth_Keller msg: 1613176185611 epoch sec; seqNo:13140; depth 13.89 2021-02-13T00:29:45Z- handleDvl() - RDI_Pathfinder msg: 1613176185.62 epoch sec; seqNo:6421 2021-02-13T00:29:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.26 , 98.86 , 111.03 , 76.02 2021-02-13T00:29:45Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.09 , 0.19 2021-02-13T00:29:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176185.66 epoch sec; seqNo:13140; 4059306.86 north; 592643.94 east 2021-02-13T00:29:46Z- AHRS_M2 msg: 1613176186.01 epoch sec; seqNo:13033 2021-02-13T00:29:46Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.17 psi 2021-02-13T00:29:46Z- handleDepth()-Depth_Keller msg: 1613176186012 epoch sec; seqNo:13141; depth 13.75 2021-02-13T00:29:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176186.03 epoch sec; seqNo:13141; 4059306.87 north; 592645.16 east 2021-02-13T00:29:46Z- AHRS_M2 msg: 1613176186.41 epoch sec; seqNo:13034 2021-02-13T00:29:46Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.17 psi 2021-02-13T00:29:46Z- handleDepth()-Depth_Keller msg: 1613176186423 epoch sec; seqNo:13142; depth 13.66 2021-02-13T00:29:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176186.43 epoch sec; seqNo:13142; 4059307.25 north; 592645.16 east 2021-02-13T00:29:46Z- AHRS_M2 msg: 1613176186.83 epoch sec; seqNo:13035 2021-02-13T00:29:46Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-13T00:29:46Z- handleDepth()-Depth_Keller msg: 1613176186849 epoch sec; seqNo:13143; depth 13.55 2021-02-13T00:29:46Z- handleDvl() - RDI_Pathfinder msg: 1613176186.86 epoch sec; seqNo:6422 2021-02-13T00:29:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 99.00 , 112.52 , 75.20 2021-02-13T00:29:46Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.17 , 0.05 2021-02-13T00:29:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176186.87 epoch sec; seqNo:13143; 4059307.63 north; 592645.16 east 2021-02-13T00:29:47Z- AHRS_M2 msg: 1613176187.22 epoch sec; seqNo:13036 2021-02-13T00:29:47Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-13T00:29:47Z- handleDepth()-Depth_Keller msg: 1613176187231 epoch sec; seqNo:13144; depth 13.42 2021-02-13T00:29:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176187.24 epoch sec; seqNo:13144; 4059308.01 north; 592645.15 east 2021-02-13T00:29:47Z- AHRS_M2 msg: 1613176187.63 epoch sec; seqNo:13037 2021-02-13T00:29:47Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.13 psi 2021-02-13T00:29:47Z- handleDepth()-Depth_Keller msg: 1613176187631 epoch sec; seqNo:13145; depth 13.32 2021-02-13T00:29:47Z- handleDvl() - RDI_Pathfinder msg: 1613176187.64 epoch sec; seqNo:6423 2021-02-13T00:29:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.48 , 99.00 , 111.17 , 75.19 2021-02-13T00:29:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.18 , -0.07 2021-02-13T00:29:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176187.66 epoch sec; seqNo:13145; 4059308.02 north; 592646.37 east 2021-02-13T00:29:48Z- AHRS_M2 msg: 1613176188.03 epoch sec; seqNo:13038 2021-02-13T00:29:48Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.12 psi 2021-02-13T00:29:48Z- handleDepth()-Depth_Keller msg: 1613176188038 epoch sec; seqNo:13146; depth 13.21 2021-02-13T00:29:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176188.05 epoch sec; seqNo:13146; 4059308.40 north; 592646.37 east 2021-02-13T00:29:48Z- LcmTrn::handleMessages() - handling 44 messages took 0.73 ms and 130 calls... 2021-02-13T00:29:48Z- waiting for 1613176185.86, time is 1613176188.05 2021-02-13T00:29:48Z- LcmTrn::updateTrn() >>>> pose time 1613176188.05, meas time 1613176187.64 <<<< 2021-02-13T00:29:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176187.64 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:29:48Z- TerrainNav::motionUpdate() - currEstimate = (4059308, 592646.37, 13.21) 2021-02-13T00:29:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:29:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:29:48Z- TNavBF::Projecting Measurements 2021-02-13T00:29:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:29:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:29:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:29:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:29:48Z- TNavFilter::4 good beams. 2021-02-13T00:29:48Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:29:48Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:29:48Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:29:48Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:29:48Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:29:48Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:29:48Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:29:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:29:48Z- TerrainMapOctree: (vehN, vehE ) = (4059308.07, 592646.09). 2021-02-13T00:29:48Z- TerrainMapOctree: Min Distance = 2518.65. 2021-02-13T00:29:48Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:29:48Z- There is no specific good beam that all particles have in common. 2021-02-13T00:29:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:29:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.98 K: 0.129 ExpectedMeasDiff: nan currMeas.covariance: 11.69 2021-02-13T00:29:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:29:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.73 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 15.68 2021-02-13T00:29:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:29:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.57 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 19.77 2021-02-13T00:29:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:29:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.46 K: 0.120 ExpectedMeasDiff: nan currMeas.covariance: 9.05 2021-02-13T00:29:48Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:29:48Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:29:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:29:48Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:29:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:29:48Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:29:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:29:48Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:29:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:29:48Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:29:48Z- Filter 0 No beams used in subcloud update 2021-02-13T00:29:48Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:29:48Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:29:48Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 700.3 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:29:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176187.64 sec. 2021-02-13T00:29:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:29:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:29:48Z- variances([0], [2], and psi_berg) = 700.282598 832.998091 0.000000 2021-02-13T00:29:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:29:48Z- LcmTrn::publishEstimates() - MLE : 4059307.90 592617.87 13.21 2021-02-13T00:29:48Z- LcmTrn::publishEstimates() - MMSE : 4059308.40 592646.37 13.21 2021-02-13T00:29:48Z- LcmTrn::publishEstimates() - COVAR: 700.28 833.00 1.31 0.00 2021-02-13T00:29:48Z- AHRS_M2 msg: 1613176188.43 epoch sec; seqNo:13039 2021-02-13T00:29:48Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.11 psi 2021-02-13T00:29:48Z- handleDepth()-Depth_Keller msg: 1613176188436 epoch sec; seqNo:13147; depth 13.11 2021-02-13T00:29:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176188.44 epoch sec; seqNo:13147; 4059308.78 north; 592646.36 east 2021-02-13T00:29:48Z- AHRS_M2 msg: 1613176188.84 epoch sec; seqNo:13040 2021-02-13T00:29:48Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.11 psi 2021-02-13T00:29:48Z- handleDepth()-Depth_Keller msg: 1613176188840 epoch sec; seqNo:13148; depth 12.99 2021-02-13T00:29:48Z- handleDvl() - RDI_Pathfinder msg: 1613176188.85 epoch sec; seqNo:6424 2021-02-13T00:29:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.48 , 97.65 , 107.11 , 76.42 2021-02-13T00:29:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.11 , -0.12 2021-02-13T00:29:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176188.86 epoch sec; seqNo:13148; 4059308.78 north; 592646.36 east 2021-02-13T00:29:49Z- AHRS_M2 msg: 1613176189.24 epoch sec; seqNo:13041 2021-02-13T00:29:49Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.11 psi 2021-02-13T00:29:49Z- handleDepth()-Depth_Keller msg: 1613176189248 epoch sec; seqNo:13149; depth 12.88 2021-02-13T00:29:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176189.26 epoch sec; seqNo:13149; 4059309.16 north; 592646.36 east 2021-02-13T00:29:49Z- AHRS_M2 msg: 1613176189.67 epoch sec; seqNo:13042 2021-02-13T00:29:49Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.11 psi 2021-02-13T00:29:49Z- handleDepth()-Depth_Keller msg: 1613176189671 epoch sec; seqNo:13150; depth 12.76 2021-02-13T00:29:49Z- handleDvl() - RDI_Pathfinder msg: 1613176189.68 epoch sec; seqNo:6425 2021-02-13T00:29:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 97.65 , 111.03 , 78.18 2021-02-13T00:29:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.03 , -0.10 2021-02-13T00:29:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176189.69 epoch sec; seqNo:13150; 4059309.16 north; 592646.36 east 2021-02-13T00:29:50Z- AHRS_M2 msg: 1613176190.05 epoch sec; seqNo:13043 2021-02-13T00:29:50Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.12 psi 2021-02-13T00:29:50Z- handleDepth()-Depth_Keller msg: 1613176190052 epoch sec; seqNo:13151; depth 12.63 2021-02-13T00:29:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176190.06 epoch sec; seqNo:13151; 4059309.55 north; 592647.58 east 2021-02-13T00:29:50Z- AHRS_M2 msg: 1613176190.45 epoch sec; seqNo:13044 2021-02-13T00:29:50Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-13T00:29:50Z- handleDepth()-Depth_Keller msg: 1613176190457 epoch sec; seqNo:13152; depth 12.53 2021-02-13T00:29:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176190.47 epoch sec; seqNo:13152; 4059309.93 north; 592647.57 east 2021-02-13T00:29:50Z- AHRS_M2 msg: 1613176190.86 epoch sec; seqNo:13045 2021-02-13T00:29:50Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-13T00:29:50Z- handleDepth()-Depth_Keller msg: 1613176190863 epoch sec; seqNo:13153; depth 12.39 2021-02-13T00:29:50Z- handleDvl() - RDI_Pathfinder msg: 1613176190.87 epoch sec; seqNo:6426 2021-02-13T00:29:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 98.93 , 111.10 , 78.65 2021-02-13T00:29:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.07 , -0.06 2021-02-13T00:29:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176190.88 epoch sec; seqNo:13153; 4059309.93 north; 592647.57 east 2021-02-13T00:29:51Z- AHRS_M2 msg: 1613176191.26 epoch sec; seqNo:13046 2021-02-13T00:29:51Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T00:29:51Z- handleDepth()-Depth_Keller msg: 1613176191269 epoch sec; seqNo:13154; depth 12.27 2021-02-13T00:29:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176191.28 epoch sec; seqNo:13154; 4059310.31 north; 592647.57 east 2021-02-13T00:29:51Z- AHRS_M2 msg: 1613176191.67 epoch sec; seqNo:13047 2021-02-13T00:29:51Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.18 psi 2021-02-13T00:29:51Z- handleDepth()-Depth_Keller msg: 1613176191673 epoch sec; seqNo:13155; depth 12.15 2021-02-13T00:29:51Z- handleDvl() - RDI_Pathfinder msg: 1613176191.68 epoch sec; seqNo:6427 2021-02-13T00:29:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 102.99 , 117.86 , 77.24 2021-02-13T00:29:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.10 , -0.01 2021-02-13T00:29:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176191.69 epoch sec; seqNo:13155; 4059310.69 north; 592647.57 east 2021-02-13T00:29:52Z- AHRS_M2 msg: 1613176192.07 epoch sec; seqNo:13048 2021-02-13T00:29:52Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.19 psi 2021-02-13T00:29:52Z- handleDepth()-Depth_Keller msg: 1613176192077 epoch sec; seqNo:13156; depth 12.01 2021-02-13T00:29:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176192.10 epoch sec; seqNo:13156; 4059311.07 north; 592647.56 east 2021-02-13T00:29:52Z- AHRS_M2 msg: 1613176192.47 epoch sec; seqNo:13049 2021-02-13T00:29:52Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.18 psi 2021-02-13T00:29:52Z- handleDepth()-Depth_Keller msg: 1613176192476 epoch sec; seqNo:13157; depth 11.89 2021-02-13T00:29:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176192.48 epoch sec; seqNo:13157; 4059311.07 north; 592647.56 east 2021-02-13T00:29:52Z- AHRS_M2 msg: 1613176192.88 epoch sec; seqNo:13050 2021-02-13T00:29:52Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.18 psi 2021-02-13T00:29:52Z- handleDepth()-Depth_Keller msg: 1613176192886 epoch sec; seqNo:13158; depth 11.78 2021-02-13T00:29:52Z- handleDvl() - RDI_Pathfinder msg: 1613176192.90 epoch sec; seqNo:6428 2021-02-13T00:29:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.21 , 102.78 , 114.95 , 78.45 2021-02-13T00:29:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.09 , 0.04 2021-02-13T00:29:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176192.91 epoch sec; seqNo:13158; 4059311.46 north; 592648.78 east 2021-02-13T00:29:53Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-13T00:29:53Z- waiting for 1613176191.05, time is 1613176192.91 2021-02-13T00:29:53Z- LcmTrn::updateTrn() equating pose 1613176192.91 and meas times 1613176192.91 2021-02-13T00:29:53Z- LcmTrn::updateTrn() >>>> pose time 1613176192.91, meas time 1613176192.91 <<<< 2021-02-13T00:29:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176192.91 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:29:53Z- TerrainNav::motionUpdate() - currEstimate = (4059311, 592648.78, 11.78) 2021-02-13T00:29:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:29:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:29:53Z- TNavBF::Projecting Measurements 2021-02-13T00:29:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:29:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:29:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:29:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:29:53Z- TNavFilter::4 good beams. 2021-02-13T00:29:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:29:53Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:29:53Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:29:53Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:29:53Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:29:53Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:29:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:29:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:29:53Z- TerrainMapOctree: (vehN, vehE ) = (4059311.46, 592648.78). 2021-02-13T00:29:53Z- TerrainMapOctree: Min Distance = 2514.33. 2021-02-13T00:29:53Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:29:53Z- There is no specific good beam that all particles have in common. 2021-02-13T00:29:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:29:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.51 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 11.62 2021-02-13T00:29:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:29:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.36 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 16.90 2021-02-13T00:29:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:29:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 21.14 2021-02-13T00:29:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:29:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T00:29:53Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:29:53Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:29:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:29:53Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:29:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:29:53Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:29:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:29:53Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:29:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:29:53Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:29:53Z- Filter 0 No beams used in subcloud update 2021-02-13T00:29:53Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:29:53Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:29:53Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 700.2 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:29:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176192.91 sec. 2021-02-13T00:29:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:29:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:29:53Z- variances([0], [2], and psi_berg) = 700.229047 832.998062 0.000000 2021-02-13T00:29:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:29:53Z- LcmTrn::publishEstimates() - MLE : 4059310.96 592686.28 11.78 2021-02-13T00:29:53Z- LcmTrn::publishEstimates() - MMSE : 4059311.46 592648.78 11.78 2021-02-13T00:29:53Z- LcmTrn::publishEstimates() - COVAR: 700.23 833.00 1.09 0.00 2021-02-13T00:29:53Z- AHRS_M2 msg: 1613176193.28 epoch sec; seqNo:13051 2021-02-13T00:29:53Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-13T00:29:53Z- handleDepth()-Depth_Keller msg: 1613176193286 epoch sec; seqNo:13159; depth 11.66 2021-02-13T00:29:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176193.32 epoch sec; seqNo:13159; 4059311.84 north; 592648.77 east 2021-02-13T00:29:53Z- AHRS_M2 msg: 1613176193.68 epoch sec; seqNo:13052 2021-02-13T00:29:53Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-13T00:29:53Z- handleDepth()-Depth_Keller msg: 1613176193693 epoch sec; seqNo:13160; depth 11.54 2021-02-13T00:29:53Z- handleDvl() - RDI_Pathfinder msg: 1613176193.70 epoch sec; seqNo:6429 2021-02-13T00:29:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.28 , 102.85 , 115.02 , 77.64 2021-02-13T00:29:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , 0.01 2021-02-13T00:29:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176193.73 epoch sec; seqNo:13160; 4059312.22 north; 592648.77 east 2021-02-13T00:29:54Z- AHRS_M2 msg: 1613176194.10 epoch sec; seqNo:13053 2021-02-13T00:29:54Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T00:29:54Z- handleDepth()-Depth_Keller msg: 1613176194107 epoch sec; seqNo:13161; depth 11.41 2021-02-13T00:29:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176194.12 epoch sec; seqNo:13161; 4059312.22 north; 592648.77 east 2021-02-13T00:29:54Z- AHRS_M2 msg: 1613176194.49 epoch sec; seqNo:13054 2021-02-13T00:29:54Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T00:29:54Z- handleDepth()-Depth_Keller msg: 1613176194496 epoch sec; seqNo:13162; depth 11.29 2021-02-13T00:29:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176194.52 epoch sec; seqNo:13162; 4059312.60 north; 592648.77 east 2021-02-13T00:29:54Z- AHRS_M2 msg: 1613176194.89 epoch sec; seqNo:13055 2021-02-13T00:29:54Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-13T00:29:54Z- handleDepth()-Depth_Keller msg: 1613176194901 epoch sec; seqNo:13163; depth 11.17 2021-02-13T00:29:54Z- handleDvl() - RDI_Pathfinder msg: 1613176194.92 epoch sec; seqNo:6430 2021-02-13T00:29:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 104.20 , 116.37 , 0.00 2021-02-13T00:29:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , -0.06 2021-02-13T00:29:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176194.93 epoch sec; seqNo:13163; 4059312.98 north; 592648.76 east 2021-02-13T00:29:55Z- AHRS_M2 msg: 1613176195.30 epoch sec; seqNo:13056 2021-02-13T00:29:55Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-13T00:29:55Z- handleDepth()-Depth_Keller msg: 1613176195304 epoch sec; seqNo:13164; depth 11.07 2021-02-13T00:29:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176195.32 epoch sec; seqNo:13164; 4059313.35 north; 592648.76 east 2021-02-13T00:29:55Z- AHRS_M2 msg: 1613176195.71 epoch sec; seqNo:13057 2021-02-13T00:29:55Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.19 psi 2021-02-13T00:29:55Z- handleDepth()-Depth_Keller msg: 1613176195714 epoch sec; seqNo:13165; depth 10.95 2021-02-13T00:29:55Z- handleDvl() - RDI_Pathfinder msg: 1613176195.73 epoch sec; seqNo:6431 2021-02-13T00:29:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:29:55Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:29:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176195.74 epoch sec; seqNo:13165; 4059313.37 north; 592649.98 east 2021-02-13T00:29:56Z- AHRS_M2 msg: 1613176196.11 epoch sec; seqNo:13058 2021-02-13T00:29:56Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.20 psi 2021-02-13T00:29:56Z- handleDepth()-Depth_Keller msg: 1613176196112 epoch sec; seqNo:13166; depth 10.83 2021-02-13T00:29:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176196.13 epoch sec; seqNo:13166; 4059313.75 north; 592649.97 east 2021-02-13T00:29:56Z- AHRS_M2 msg: 1613176196.51 epoch sec; seqNo:13059 2021-02-13T00:29:56Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.21 psi 2021-02-13T00:29:56Z- handleDepth()-Depth_Keller msg: 1613176196521 epoch sec; seqNo:13167; depth 10.69 2021-02-13T00:29:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176196.54 epoch sec; seqNo:13167; 4059314.13 north; 592649.97 east 2021-02-13T00:29:56Z- AHRS_M2 msg: 1613176196.92 epoch sec; seqNo:13060 2021-02-13T00:29:56Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.22 psi 2021-02-13T00:29:56Z- handleDepth()-Depth_Keller msg: 1613176196925 epoch sec; seqNo:13168; depth 10.56 2021-02-13T00:29:56Z- handleDvl() - RDI_Pathfinder msg: 1613176196.94 epoch sec; seqNo:6432 2021-02-13T00:29:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 106.16 , 116.57 , 79.39 2021-02-13T00:29:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.22 , -0.07 2021-02-13T00:29:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176196.96 epoch sec; seqNo:13168; 4059314.13 north; 592649.97 east 2021-02-13T00:29:57Z- AHRS_M2 msg: 1613176197.33 epoch sec; seqNo:13061 2021-02-13T00:29:57Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.22 psi 2021-02-13T00:29:57Z- handleDepth()-Depth_Keller msg: 1613176197330 epoch sec; seqNo:13169; depth 10.45 2021-02-13T00:29:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176197.34 epoch sec; seqNo:13169; 4059314.50 north; 592649.97 east 2021-02-13T00:29:57Z- AHRS_M2 msg: 1613176197.75 epoch sec; seqNo:13062 2021-02-13T00:29:57Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.21 psi 2021-02-13T00:29:57Z- handleDepth()-Depth_Keller msg: 1613176197753 epoch sec; seqNo:13170; depth 10.34 2021-02-13T00:29:57Z- handleDvl() - RDI_Pathfinder msg: 1613176197.77 epoch sec; seqNo:6433 2021-02-13T00:29:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.79 , 106.77 , 113.53 , 81.08 2021-02-13T00:29:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.24 , 0.20 2021-02-13T00:29:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176197.78 epoch sec; seqNo:13170; 4059314.88 north; 592649.96 east 2021-02-13T00:29:58Z- AHRS_M2 msg: 1613176198.13 epoch sec; seqNo:13063 2021-02-13T00:29:58Z- AHRS_M2 msg: 0.61 phi; 0.29 theta; -0.21 psi 2021-02-13T00:29:58Z- handleDepth()-Depth_Keller msg: 1613176198137 epoch sec; seqNo:13171; depth 10.22 2021-02-13T00:29:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176198.16 epoch sec; seqNo:13171; 4059315.26 north; 592649.96 east 2021-02-13T00:29:58Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:29:58Z- waiting for 1613176195.91, time is 1613176198.16 2021-02-13T00:29:58Z- LcmTrn::updateTrn() >>>> pose time 1613176198.16, meas time 1613176197.77 <<<< 2021-02-13T00:29:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176197.77 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:29:58Z- TerrainNav::motionUpdate() - currEstimate = (4059315, 592649.96, 10.22) 2021-02-13T00:29:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:29:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:29:58Z- TNavBF::Projecting Measurements 2021-02-13T00:29:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:29:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:29:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:29:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:29:58Z- TNavFilter::4 good beams. 2021-02-13T00:29:58Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:29:58Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:29:58Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:29:58Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:29:58Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:29:58Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:29:58Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:29:58Z- TNavBF::mmse selected from filter -1 2021-02-13T00:29:58Z- TerrainMapOctree: (vehN, vehE ) = (4059314.98, 592649.87). 2021-02-13T00:29:58Z- TerrainMapOctree: Min Distance = 2510.92. 2021-02-13T00:29:58Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:29:58Z- There is no specific good beam that all particles have in common. 2021-02-13T00:29:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:29:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 11.23 2021-02-13T00:29:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:29:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 18.24 2021-02-13T00:29:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:29:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 20.62 2021-02-13T00:29:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:29:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 10.52 2021-02-13T00:29:58Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:29:58Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:29:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:29:58Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:29:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:29:58Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:29:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:29:58Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:29:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:29:58Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:29:58Z- Filter 0 No beams used in subcloud update 2021-02-13T00:29:58Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:29:58Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:29:58Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 700.2 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:29:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176197.77 sec. 2021-02-13T00:29:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:29:58Z- TNavBF::mmse selected from filter -1 2021-02-13T00:29:58Z- variances([0], [2], and psi_berg) = 700.162199 832.998027 0.000000 2021-02-13T00:29:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:29:58Z- LcmTrn::publishEstimates() - MLE : 4059314.76 592621.46 10.22 2021-02-13T00:29:58Z- LcmTrn::publishEstimates() - MMSE : 4059315.26 592649.96 10.22 2021-02-13T00:29:58Z- LcmTrn::publishEstimates() - COVAR: 700.16 833.00 0.98 0.00 2021-02-13T00:29:58Z- AHRS_M2 msg: 1613176198.53 epoch sec; seqNo:13064 2021-02-13T00:29:58Z- AHRS_M2 msg: 0.60 phi; 0.29 theta; -0.20 psi 2021-02-13T00:29:58Z- handleDepth()-Depth_Keller msg: 1613176198536 epoch sec; seqNo:13172; depth 10.12 2021-02-13T00:29:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176198.55 epoch sec; seqNo:13172; 4059315.64 north; 592649.95 east 2021-02-13T00:29:58Z- AHRS_M2 msg: 1613176198.94 epoch sec; seqNo:13065 2021-02-13T00:29:58Z- AHRS_M2 msg: 0.59 phi; 0.29 theta; -0.19 psi 2021-02-13T00:29:58Z- handleDepth()-Depth_Keller msg: 1613176198948 epoch sec; seqNo:13173; depth 10.03 2021-02-13T00:29:58Z- handleDvl() - RDI_Pathfinder msg: 1613176198.96 epoch sec; seqNo:6434 2021-02-13T00:29:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 106.84 , 113.60 , 81.15 2021-02-13T00:29:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , 0.38 2021-02-13T00:29:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176198.99 epoch sec; seqNo:13173; 4059316.03 north; 592651.17 east 2021-02-13T00:29:59Z- AHRS_M2 msg: 1613176199.34 epoch sec; seqNo:13066 2021-02-13T00:29:59Z- AHRS_M2 msg: 0.58 phi; 0.30 theta; -0.18 psi 2021-02-13T00:29:59Z- handleDepth()-Depth_Keller msg: 1613176199346 epoch sec; seqNo:13174; depth 9.92 2021-02-13T00:29:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176199.36 epoch sec; seqNo:13174; 4059316.03 north; 592651.17 east 2021-02-13T00:29:59Z- AHRS_M2 msg: 1613176199.76 epoch sec; seqNo:13067 2021-02-13T00:29:59Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.17 psi 2021-02-13T00:29:59Z- handleDepth()-Depth_Keller msg: 1613176199759 epoch sec; seqNo:13175; depth 9.85 2021-02-13T00:29:59Z- handleDvl() - RDI_Pathfinder msg: 1613176199.77 epoch sec; seqNo:6435 2021-02-13T00:29:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.15 , 105.08 , 114.54 , 80.75 2021-02-13T00:29:59Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.22 , 0.28 2021-02-13T00:29:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176199.78 epoch sec; seqNo:13175; 4059316.41 north; 592651.17 east 2021-02-13T00:30:00Z- AHRS_M2 msg: 1613176200.15 epoch sec; seqNo:13068 2021-02-13T00:30:00Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.17 psi 2021-02-13T00:30:00Z- handleDepth()-Depth_Keller msg: 1613176200161 epoch sec; seqNo:13176; depth 9.74 2021-02-13T00:30:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176200.18 epoch sec; seqNo:13176; 4059316.79 north; 592651.16 east 2021-02-13T00:30:00Z- AHRS_M2 msg: 1613176200.55 epoch sec; seqNo:13069 2021-02-13T00:30:00Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.16 psi 2021-02-13T00:30:00Z- handleDepth()-Depth_Keller msg: 1613176200556 epoch sec; seqNo:13177; depth 9.63 2021-02-13T00:30:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176200.57 epoch sec; seqNo:13177; 4059317.18 north; 592652.38 east 2021-02-13T00:30:00Z- AHRS_M2 msg: 1613176200.95 epoch sec; seqNo:13070 2021-02-13T00:30:00Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-13T00:30:00Z- handleDepth()-Depth_Keller msg: 1613176200969 epoch sec; seqNo:13178; depth 9.55 2021-02-13T00:30:00Z- handleDvl() - RDI_Pathfinder msg: 1613176200.98 epoch sec; seqNo:6436 2021-02-13T00:30:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.31 , 104.07 , 114.88 , 81.08 2021-02-13T00:30:00Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.23 , -0.04 2021-02-13T00:30:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176200.99 epoch sec; seqNo:13178; 4059317.56 north; 592652.37 east 2021-02-13T00:30:01Z- AHRS_M2 msg: 1613176201.36 epoch sec; seqNo:13071 2021-02-13T00:30:01Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-13T00:30:01Z- handleDepth()-Depth_Keller msg: 1613176201364 epoch sec; seqNo:13179; depth 9.45 2021-02-13T00:30:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176201.38 epoch sec; seqNo:13179; 4059317.56 north; 592652.37 east 2021-02-13T00:30:01Z- AHRS_M2 msg: 1613176201.76 epoch sec; seqNo:13072 2021-02-13T00:30:01Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.14 psi 2021-02-13T00:30:01Z- handleDepth()-Depth_Keller msg: 1613176201773 epoch sec; seqNo:13180; depth 9.36 2021-02-13T00:30:01Z- handleDvl() - RDI_Pathfinder msg: 1613176201.79 epoch sec; seqNo:6437 2021-02-13T00:30:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 103.86 , 117.39 , 0.00 2021-02-13T00:30:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.16 , -0.23 2021-02-13T00:30:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176201.81 epoch sec; seqNo:13180; 4059317.94 north; 592652.37 east 2021-02-13T00:30:02Z- AHRS_M2 msg: 1613176202.17 epoch sec; seqNo:13073 2021-02-13T00:30:02Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.14 psi 2021-02-13T00:30:02Z- handleDepth()-Depth_Keller msg: 1613176202173 epoch sec; seqNo:13181; depth 9.24 2021-02-13T00:30:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176202.18 epoch sec; seqNo:13181; 4059317.94 north; 592652.37 east 2021-02-13T00:30:02Z- AHRS_M2 msg: 1613176202.57 epoch sec; seqNo:13074 2021-02-13T00:30:02Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.15 psi 2021-02-13T00:30:02Z- handleDepth()-Depth_Keller msg: 1613176202585 epoch sec; seqNo:13182; depth 9.14 2021-02-13T00:30:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176202.60 epoch sec; seqNo:13182; 4059318.33 north; 592653.59 east 2021-02-13T00:30:02Z- AHRS_M2 msg: 1613176202.97 epoch sec; seqNo:13075 2021-02-13T00:30:02Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.15 psi 2021-02-13T00:30:02Z- handleDepth()-Depth_Keller msg: 1613176202985 epoch sec; seqNo:13183; depth 9.04 2021-02-13T00:30:03Z- handleDvl() - RDI_Pathfinder msg: 1613176203.01 epoch sec; seqNo:6438 2021-02-13T00:30:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.44 , 103.80 , 115.69 , 0.00 2021-02-13T00:30:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.00 , -0.22 2021-02-13T00:30:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176203.02 epoch sec; seqNo:13183; 4059318.71 north; 592653.58 east 2021-02-13T00:30:03Z- AHRS_M2 msg: 1613176203.38 epoch sec; seqNo:13076 2021-02-13T00:30:03Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.15 psi 2021-02-13T00:30:03Z- handleDepth()-Depth_Keller msg: 1613176203384 epoch sec; seqNo:13184; depth 8.94 2021-02-13T00:30:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176203.40 epoch sec; seqNo:13184; 4059318.71 north; 592653.58 east 2021-02-13T00:30:03Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-13T00:30:03Z- waiting for 1613176201.16, time is 1613176203.40 2021-02-13T00:30:03Z- LcmTrn::updateTrn() >>>> pose time 1613176203.40, meas time 1613176203.01 <<<< 2021-02-13T00:30:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176203.01 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:30:03Z- TerrainNav::motionUpdate() - currEstimate = (4059319, 592653.58, 8.94) 2021-02-13T00:30:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:30:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:30:03Z- TNavBF::Projecting Measurements 2021-02-13T00:30:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:30:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:30:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:30:03Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:30:03Z- TNavFilter::3 good beams. 2021-02-13T00:30:03Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:30:03Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:30:03Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:30:03Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:30:03Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:30:03Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:30:03Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:30:03Z- TNavBF::mmse selected from filter -1 2021-02-13T00:30:03Z- TerrainMapOctree: (vehN, vehE ) = (4059318.45, 592653.31). 2021-02-13T00:30:03Z- TerrainMapOctree: Min Distance = 2506.05. 2021-02-13T00:30:03Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:30:03Z- There is no specific good beam that all particles have in common. 2021-02-13T00:30:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:30:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 12.23 2021-02-13T00:30:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:30:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 17.24 2021-02-13T00:30:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:30:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 21.42 2021-02-13T00:30:03Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:30:03Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:30:03Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:30:03Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:30:03Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:30:03Z- Filter 0 No beams used in subcloud update 2021-02-13T00:30:03Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:30:03Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:30:03Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 700.1 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:30:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176203.01 sec. 2021-02-13T00:30:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:30:03Z- TNavBF::mmse selected from filter -1 2021-02-13T00:30:03Z- variances([0], [2], and psi_berg) = 700.087296 832.997988 0.000000 2021-02-13T00:30:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:30:03Z- LcmTrn::publishEstimates() - MLE : 4059318.21 592692.08 8.94 2021-02-13T00:30:03Z- LcmTrn::publishEstimates() - MMSE : 4059318.71 592653.58 8.94 2021-02-13T00:30:03Z- LcmTrn::publishEstimates() - COVAR: 700.09 833.00 1.03 0.00 2021-02-13T00:30:03Z- AHRS_M2 msg: 1613176203.80 epoch sec; seqNo:13077 2021-02-13T00:30:03Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-13T00:30:03Z- handleDepth()-Depth_Keller msg: 1613176203801 epoch sec; seqNo:13185; depth 8.84 2021-02-13T00:30:03Z- handleDvl() - RDI_Pathfinder msg: 1613176203.81 epoch sec; seqNo:6439 2021-02-13T00:30:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 103.86 , 115.76 , 81.56 2021-02-13T00:30:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.05 , -0.08 2021-02-13T00:30:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176203.82 epoch sec; seqNo:13185; 4059319.09 north; 592653.58 east 2021-02-13T00:30:04Z- AHRS_M2 msg: 1613176204.19 epoch sec; seqNo:13078 2021-02-13T00:30:04Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.16 psi 2021-02-13T00:30:04Z- handleDepth()-Depth_Keller msg: 1613176204202 epoch sec; seqNo:13186; depth 8.73 2021-02-13T00:30:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176204.21 epoch sec; seqNo:13186; 4059319.47 north; 592653.57 east 2021-02-13T00:30:04Z- AHRS_M2 msg: 1613176204.60 epoch sec; seqNo:13079 2021-02-13T00:30:04Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:30:04Z- handleDepth()-Depth_Keller msg: 1613176204603 epoch sec; seqNo:13187; depth 8.65 2021-02-13T00:30:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176204.61 epoch sec; seqNo:13187; 4059319.47 north; 592653.57 east 2021-02-13T00:30:04Z- AHRS_M2 msg: 1613176204.99 epoch sec; seqNo:13080 2021-02-13T00:30:04Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-13T00:30:05Z- handleDepth()-Depth_Keller msg: 1613176205005 epoch sec; seqNo:13188; depth 8.54 2021-02-13T00:30:05Z- handleDvl() - RDI_Pathfinder msg: 1613176205.02 epoch sec; seqNo:6440 2021-02-13T00:30:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 104.00 , 117.52 , 0.00 2021-02-13T00:30:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , -0.01 2021-02-13T00:30:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176205.03 epoch sec; seqNo:13188; 4059319.85 north; 592653.57 east 2021-02-13T00:30:05Z- AHRS_M2 msg: 1613176205.40 epoch sec; seqNo:13081 2021-02-13T00:30:05Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.18 psi 2021-02-13T00:30:05Z- handleDepth()-Depth_Keller msg: 1613176205405 epoch sec; seqNo:13189; depth 8.43 2021-02-13T00:30:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176205.42 epoch sec; seqNo:13189; 4059320.24 north; 592654.79 east 2021-02-13T00:30:05Z- AHRS_M2 msg: 1613176205.80 epoch sec; seqNo:13082 2021-02-13T00:30:05Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.18 psi 2021-02-13T00:30:05Z- handleDepth()-Depth_Keller msg: 1613176205818 epoch sec; seqNo:13190; depth 8.33 2021-02-13T00:30:05Z- handleDvl() - RDI_Pathfinder msg: 1613176205.83 epoch sec; seqNo:6441 2021-02-13T00:30:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.65 , 106.50 , 118.40 , 81.22 2021-02-13T00:30:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , -0.08 2021-02-13T00:30:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176205.84 epoch sec; seqNo:13190; 4059320.24 north; 592654.79 east 2021-02-13T00:30:06Z- AHRS_M2 msg: 1613176206.21 epoch sec; seqNo:13083 2021-02-13T00:30:06Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.18 psi 2021-02-13T00:30:06Z- handleDepth()-Depth_Keller msg: 1613176206217 epoch sec; seqNo:13191; depth 8.23 2021-02-13T00:30:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176206.23 epoch sec; seqNo:13191; 4059320.62 north; 592654.78 east 2021-02-13T00:30:06Z- AHRS_M2 msg: 1613176206.61 epoch sec; seqNo:13084 2021-02-13T00:30:06Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-13T00:30:06Z- handleDepth()-Depth_Keller msg: 1613176206625 epoch sec; seqNo:13192; depth 8.13 2021-02-13T00:30:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176206.63 epoch sec; seqNo:13192; 4059321.00 north; 592654.78 east 2021-02-13T00:30:07Z- AHRS_M2 msg: 1613176207.02 epoch sec; seqNo:13085 2021-02-13T00:30:07Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-13T00:30:07Z- handleDepth()-Depth_Keller msg: 1613176207025 epoch sec; seqNo:13193; depth 8.02 2021-02-13T00:30:07Z- handleDvl() - RDI_Pathfinder msg: 1613176207.04 epoch sec; seqNo:6442 2021-02-13T00:30:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.17 , 106.64 , 121.51 , 80.95 2021-02-13T00:30:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.20 , -0.04 2021-02-13T00:30:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176207.05 epoch sec; seqNo:13193; 4059321.38 north; 592654.77 east 2021-02-13T00:30:07Z- AHRS_M2 msg: 1613176207.42 epoch sec; seqNo:13086 2021-02-13T00:30:07Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-13T00:30:07Z- handleDepth()-Depth_Keller msg: 1613176207425 epoch sec; seqNo:13194; depth 7.90 2021-02-13T00:30:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176207.44 epoch sec; seqNo:13194; 4059321.75 north; 592654.77 east 2021-02-13T00:30:07Z- AHRS_M2 msg: 1613176207.83 epoch sec; seqNo:13087 2021-02-13T00:30:07Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:30:07Z- handleDepth()-Depth_Keller msg: 1613176207830 epoch sec; seqNo:13195; depth 7.81 2021-02-13T00:30:07Z- handleDvl() - RDI_Pathfinder msg: 1613176207.84 epoch sec; seqNo:6443 2021-02-13T00:30:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.98 , 105.42 , 117.32 , 81.62 2021-02-13T00:30:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.17 , 0.12 2021-02-13T00:30:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176207.85 epoch sec; seqNo:13195; 4059321.75 north; 592654.77 east 2021-02-13T00:30:08Z- AHRS_M2 msg: 1613176208.23 epoch sec; seqNo:13088 2021-02-13T00:30:08Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-13T00:30:08Z- handleDepth()-Depth_Keller msg: 1613176208234 epoch sec; seqNo:13196; depth 7.72 2021-02-13T00:30:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176208.24 epoch sec; seqNo:13196; 4059322.15 north; 592655.99 east 2021-02-13T00:30:08Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-13T00:30:08Z- waiting for 1613176206.40, time is 1613176208.24 2021-02-13T00:30:08Z- LcmTrn::updateTrn() >>>> pose time 1613176208.24, meas time 1613176207.84 <<<< 2021-02-13T00:30:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176207.84 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:30:08Z- TerrainNav::motionUpdate() - currEstimate = (4059322, 592655.99, 7.72) 2021-02-13T00:30:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:30:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:30:08Z- TNavBF::Projecting Measurements 2021-02-13T00:30:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:30:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:30:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:30:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:30:08Z- TNavFilter::4 good beams. 2021-02-13T00:30:08Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:30:08Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:30:08Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:30:08Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:30:08Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:30:08Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:30:08Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:30:08Z- TNavBF::mmse selected from filter -1 2021-02-13T00:30:08Z- TerrainMapOctree: (vehN, vehE ) = (4059321.86, 592655.79). 2021-02-13T00:30:08Z- TerrainMapOctree: Min Distance = 2501.85. 2021-02-13T00:30:08Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:30:08Z- There is no specific good beam that all particles have in common. 2021-02-13T00:30:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:30:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:30:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:30:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 17.78 2021-02-13T00:30:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:30:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 22.02 2021-02-13T00:30:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:30:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 10.66 2021-02-13T00:30:08Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:30:08Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:30:08Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:30:08Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:30:08Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:30:08Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:30:08Z- Filter 0 No beams used in subcloud update 2021-02-13T00:30:08Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:30:08Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:30:08Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 700.0 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:30:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176207.84 sec. 2021-02-13T00:30:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:30:08Z- TNavBF::mmse selected from filter -1 2021-02-13T00:30:08Z- variances([0], [2], and psi_berg) = 700.018192 832.997952 0.000000 2021-02-13T00:30:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:30:08Z- LcmTrn::publishEstimates() - MLE : 4059321.65 592688.49 7.72 2021-02-13T00:30:08Z- LcmTrn::publishEstimates() - MMSE : 4059322.15 592655.99 7.72 2021-02-13T00:30:08Z- LcmTrn::publishEstimates() - COVAR: 700.02 833.00 0.97 0.00 2021-02-13T00:30:08Z- AHRS_M2 msg: 1613176208.63 epoch sec; seqNo:13089 2021-02-13T00:30:08Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.15 psi 2021-02-13T00:30:08Z- handleDepth()-Depth_Keller msg: 1613176208641 epoch sec; seqNo:13197; depth 7.60 2021-02-13T00:30:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176208.66 epoch sec; seqNo:13197; 4059322.53 north; 592655.98 east 2021-02-13T00:30:09Z- AHRS_M2 msg: 1613176209.04 epoch sec; seqNo:13090 2021-02-13T00:30:09Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.14 psi 2021-02-13T00:30:09Z- handleDepth()-Depth_Keller msg: 1613176209040 epoch sec; seqNo:13198; depth 7.51 2021-02-13T00:30:09Z- handleDvl() - RDI_Pathfinder msg: 1613176209.05 epoch sec; seqNo:6444 2021-02-13T00:30:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.21 , 106.43 , 117.25 , 82.10 2021-02-13T00:30:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.09 , 0.17 2021-02-13T00:30:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176209.08 epoch sec; seqNo:13198; 4059322.90 north; 592655.98 east 2021-02-13T00:30:09Z- AHRS_M2 msg: 1613176209.44 epoch sec; seqNo:13091 2021-02-13T00:30:09Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.13 psi 2021-02-13T00:30:09Z- handleDepth()-Depth_Keller msg: 1613176209453 epoch sec; seqNo:13199; depth 7.41 2021-02-13T00:30:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176209.46 epoch sec; seqNo:13199; 4059323.28 north; 592655.97 east 2021-02-13T00:30:09Z- AHRS_M2 msg: 1613176209.84 epoch sec; seqNo:13092 2021-02-13T00:30:09Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.13 psi 2021-02-13T00:30:09Z- handleDepth()-Depth_Keller msg: 1613176209858 epoch sec; seqNo:13200; depth 7.32 2021-02-13T00:30:09Z- handleDvl() - RDI_Pathfinder msg: 1613176209.87 epoch sec; seqNo:6445 2021-02-13T00:30:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.70 , 102.51 , 122.79 , 82.23 2021-02-13T00:30:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.29 , -0.05 2021-02-13T00:30:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176209.88 epoch sec; seqNo:13200; 4059323.28 north; 592655.97 east 2021-02-13T00:30:10Z- AHRS_M2 msg: 1613176210.25 epoch sec; seqNo:13093 2021-02-13T00:30:10Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.13 psi 2021-02-13T00:30:10Z- handleDepth()-Depth_Keller msg: 1613176210261 epoch sec; seqNo:13201; depth 7.21 2021-02-13T00:30:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176210.28 epoch sec; seqNo:13201; 4059323.68 north; 592657.19 east 2021-02-13T00:30:10Z- AHRS_M2 msg: 1613176210.65 epoch sec; seqNo:13094 2021-02-13T00:30:10Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.14 psi 2021-02-13T00:30:10Z- handleDepth()-Depth_Keller msg: 1613176210656 epoch sec; seqNo:13202; depth 7.09 2021-02-13T00:30:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176210.66 epoch sec; seqNo:13202; 4059323.68 north; 592657.19 east 2021-02-13T00:30:11Z- AHRS_M2 msg: 1613176211.06 epoch sec; seqNo:13095 2021-02-13T00:30:11Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.15 psi 2021-02-13T00:30:11Z- handleDepth()-Depth_Keller msg: 1613176211063 epoch sec; seqNo:13203; depth 6.97 2021-02-13T00:30:11Z- handleDvl() - RDI_Pathfinder msg: 1613176211.07 epoch sec; seqNo:6446 2021-02-13T00:30:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.98 , 101.09 , 124.08 , 82.17 2021-02-13T00:30:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , 0.14 , -0.26 2021-02-13T00:30:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176211.09 epoch sec; seqNo:13203; 4059324.05 north; 592657.19 east 2021-02-13T00:30:11Z- AHRS_M2 msg: 1613176211.46 epoch sec; seqNo:13096 2021-02-13T00:30:11Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-13T00:30:11Z- handleDepth()-Depth_Keller msg: 1613176211464 epoch sec; seqNo:13204; depth 6.84 2021-02-13T00:30:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176211.48 epoch sec; seqNo:13204; 4059324.05 north; 592657.19 east 2021-02-13T00:30:11Z- AHRS_M2 msg: 1613176211.88 epoch sec; seqNo:13097 2021-02-13T00:30:11Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.18 psi 2021-02-13T00:30:11Z- handleDepth()-Depth_Keller msg: 1613176211880 epoch sec; seqNo:13205; depth 6.70 2021-02-13T00:30:11Z- handleDvl() - RDI_Pathfinder msg: 1613176211.90 epoch sec; seqNo:6447 2021-02-13T00:30:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.28 , 106.50 , 120.02 , 82.17 2021-02-13T00:30:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.21 , -0.17 2021-02-13T00:30:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176211.91 epoch sec; seqNo:13205; 4059324.43 north; 592657.18 east 2021-02-13T00:30:12Z- AHRS_M2 msg: 1613176212.27 epoch sec; seqNo:13098 2021-02-13T00:30:12Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.20 psi 2021-02-13T00:30:12Z- handleDepth()-Depth_Keller msg: 1613176212277 epoch sec; seqNo:13206; depth 6.57 2021-02-13T00:30:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176212.29 epoch sec; seqNo:13206; 4059324.43 north; 592657.18 east 2021-02-13T00:30:12Z- AHRS_M2 msg: 1613176212.67 epoch sec; seqNo:13099 2021-02-13T00:30:12Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.21 psi 2021-02-13T00:30:12Z- handleDepth()-Depth_Keller msg: 1613176212678 epoch sec; seqNo:13207; depth 6.43 2021-02-13T00:30:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176212.69 epoch sec; seqNo:13207; 4059324.81 north; 592657.18 east 2021-02-13T00:30:13Z- AHRS_M2 msg: 1613176213.08 epoch sec; seqNo:13100 2021-02-13T00:30:13Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.21 psi 2021-02-13T00:30:13Z- handleDepth()-Depth_Keller msg: 1613176213085 epoch sec; seqNo:13208; depth 6.32 2021-02-13T00:30:13Z- handleDvl() - RDI_Pathfinder msg: 1613176213.10 epoch sec; seqNo:6448 2021-02-13T00:30:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 111.91 , 121.37 , 83.52 2021-02-13T00:30:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.32 , 0.20 2021-02-13T00:30:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176213.11 epoch sec; seqNo:13208; 4059325.19 north; 592657.17 east 2021-02-13T00:30:13Z- AHRS_M2 msg: 1613176213.48 epoch sec; seqNo:13101 2021-02-13T00:30:13Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.19 psi 2021-02-13T00:30:13Z- handleDepth()-Depth_Keller msg: 1613176213484 epoch sec; seqNo:13209; depth 6.18 2021-02-13T00:30:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176213.49 epoch sec; seqNo:13209; 4059325.57 north; 592657.17 east 2021-02-13T00:30:13Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:30:13Z- waiting for 1613176211.24, time is 1613176213.49 2021-02-13T00:30:13Z- LcmTrn::updateTrn() >>>> pose time 1613176213.49, meas time 1613176213.10 <<<< 2021-02-13T00:30:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176213.10 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:30:13Z- TerrainNav::motionUpdate() - currEstimate = (4059326, 592657.17, 6.18) 2021-02-13T00:30:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:30:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:30:13Z- TNavBF::Projecting Measurements 2021-02-13T00:30:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:30:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:30:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:30:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:30:13Z- TNavFilter::4 good beams. 2021-02-13T00:30:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:30:13Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:30:13Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:30:13Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:30:13Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:30:13Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:30:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:30:13Z- TNavBF::mmse selected from filter -1 2021-02-13T00:30:13Z- TerrainMapOctree: (vehN, vehE ) = (4059325.31, 592657.08). 2021-02-13T00:30:13Z- TerrainMapOctree: Min Distance = 2498.36. 2021-02-13T00:30:13Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:30:13Z- There is no specific good beam that all particles have in common. 2021-02-13T00:30:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:30:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 12.65 2021-02-13T00:30:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:30:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 20.04 2021-02-13T00:30:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:30:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 23.57 2021-02-13T00:30:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:30:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 11.16 2021-02-13T00:30:13Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:30:13Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:30:13Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:30:13Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:30:13Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:30:13Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:30:13Z- Filter 0 No beams used in subcloud update 2021-02-13T00:30:13Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:30:13Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:30:13Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 700.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:30:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176213.10 sec. 2021-02-13T00:30:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:30:13Z- TNavBF::mmse selected from filter -1 2021-02-13T00:30:13Z- variances([0], [2], and psi_berg) = 699.954559 832.997918 0.000000 2021-02-13T00:30:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:30:13Z- LcmTrn::publishEstimates() - MLE : 4059325.07 592689.67 6.18 2021-02-13T00:30:13Z- LcmTrn::publishEstimates() - MMSE : 4059325.57 592657.17 6.18 2021-02-13T00:30:13Z- LcmTrn::publishEstimates() - COVAR: 699.95 833.00 0.92 0.00 2021-02-13T00:30:13Z- AHRS_M2 msg: 1613176213.89 epoch sec; seqNo:13102 2021-02-13T00:30:13Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.16 psi 2021-02-13T00:30:13Z- handleDepth()-Depth_Keller msg: 1613176213890 epoch sec; seqNo:13210; depth 6.07 2021-02-13T00:30:13Z- handleDvl() - RDI_Pathfinder msg: 1613176213.90 epoch sec; seqNo:6449 2021-02-13T00:30:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.55 , 112.86 , 130.70 , 83.11 2021-02-13T00:30:13Z- handleDvl() - RDI_Pathfinder msg: velocities 1.10 , -0.02 , 0.36 2021-02-13T00:30:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176213.91 epoch sec; seqNo:13210; 4059325.96 north; 592658.39 east 2021-02-13T00:30:14Z- AHRS_M2 msg: 1613176214.29 epoch sec; seqNo:13103 2021-02-13T00:30:14Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.15 psi 2021-02-13T00:30:14Z- handleDepth()-Depth_Keller msg: 1613176214294 epoch sec; seqNo:13211; depth 5.95 2021-02-13T00:30:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176214.31 epoch sec; seqNo:13211; 4059326.34 north; 592658.38 east 2021-02-13T00:30:14Z- AHRS_M2 msg: 1613176214.70 epoch sec; seqNo:13104 2021-02-13T00:30:14Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.14 psi 2021-02-13T00:30:14Z- handleDepth()-Depth_Keller msg: 1613176214700 epoch sec; seqNo:13212; depth 5.85 2021-02-13T00:30:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176214.74 epoch sec; seqNo:13212; 4059326.72 north; 592658.38 east 2021-02-13T00:30:15Z- AHRS_M2 msg: 1613176215.10 epoch sec; seqNo:13105 2021-02-13T00:30:15Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.14 psi 2021-02-13T00:30:15Z- handleDepth()-Depth_Keller msg: 1613176215100 epoch sec; seqNo:13213; depth 5.70 2021-02-13T00:30:15Z- handleDvl() - RDI_Pathfinder msg: 1613176215.11 epoch sec; seqNo:6450 2021-02-13T00:30:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 108.12 , 128.94 , 82.84 2021-02-13T00:30:15Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.22 , 0.16 2021-02-13T00:30:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176215.14 epoch sec; seqNo:13213; 4059327.10 north; 592658.37 east 2021-02-13T00:30:15Z- AHRS_M2 msg: 1613176215.50 epoch sec; seqNo:13106 2021-02-13T00:30:15Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.14 psi 2021-02-13T00:30:15Z- handleDepth()-Depth_Keller msg: 1613176215504 epoch sec; seqNo:13214; depth 5.59 2021-02-13T00:30:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176215.52 epoch sec; seqNo:13214; 4059327.49 north; 592659.59 east 2021-02-13T00:30:15Z- AHRS_M2 msg: 1613176215.90 epoch sec; seqNo:13107 2021-02-13T00:30:15Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-13T00:30:15Z- handleDepth()-Depth_Keller msg: 1613176215917 epoch sec; seqNo:13215; depth 5.45 2021-02-13T00:30:15Z- handleDvl() - RDI_Pathfinder msg: 1613176215.93 epoch sec; seqNo:6451 2021-02-13T00:30:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 108.12 , 125.97 , 84.33 2021-02-13T00:30:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.29 , -0.06 2021-02-13T00:30:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176215.94 epoch sec; seqNo:13215; 4059327.49 north; 592659.59 east 2021-02-13T00:30:16Z- AHRS_M2 msg: 1613176216.31 epoch sec; seqNo:13108 2021-02-13T00:30:16Z- AHRS_M2 msg: 0.63 phi; 0.29 theta; -0.14 psi 2021-02-13T00:30:16Z- handleDepth()-Depth_Keller msg: 1613176216312 epoch sec; seqNo:13216; depth 5.32 2021-02-13T00:30:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176216.33 epoch sec; seqNo:13216; 4059327.87 north; 592659.59 east 2021-02-13T00:30:16Z- AHRS_M2 msg: 1613176216.72 epoch sec; seqNo:13109 2021-02-13T00:30:16Z- AHRS_M2 msg: 0.65 phi; 0.25 theta; -0.14 psi 2021-02-13T00:30:16Z- handleDepth()-Depth_Keller msg: 1613176216736 epoch sec; seqNo:13217; depth 5.20 2021-02-13T00:30:16Z- handleDvl() - RDI_Pathfinder msg: 1613176216.76 epoch sec; seqNo:6452 2021-02-13T00:30:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.83 , 106.70 , 118.60 , 170.65 2021-02-13T00:30:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , 0.23 , -0.20 2021-02-13T00:30:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176216.77 epoch sec; seqNo:13217; 4059327.87 north; 592659.59 east 2021-02-13T00:30:17Z- AHRS_M2 msg: 1613176217.12 epoch sec; seqNo:13110 2021-02-13T00:30:17Z- AHRS_M2 msg: 0.67 phi; 0.21 theta; -0.13 psi 2021-02-13T00:30:17Z- handleDepth()-Depth_Keller msg: 1613176217129 epoch sec; seqNo:13218; depth 5.06 2021-02-13T00:30:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176217.14 epoch sec; seqNo:13218; 4059328.25 north; 592659.58 east 2021-02-13T00:30:17Z- AHRS_M2 msg: 1613176217.52 epoch sec; seqNo:13111 2021-02-13T00:30:17Z- AHRS_M2 msg: 0.68 phi; 0.17 theta; -0.13 psi 2021-02-13T00:30:17Z- handleDepth()-Depth_Keller msg: 1613176217525 epoch sec; seqNo:13219; depth 4.95 2021-02-13T00:30:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176217.53 epoch sec; seqNo:13219; 4059328.26 north; 592660.80 east 2021-02-13T00:30:17Z- AHRS_M2 msg: 1613176217.92 epoch sec; seqNo:13112 2021-02-13T00:30:17Z- AHRS_M2 msg: 0.69 phi; 0.13 theta; -0.13 psi 2021-02-13T00:30:17Z- handleDepth()-Depth_Keller msg: 1613176217929 epoch sec; seqNo:13220; depth 4.84 2021-02-13T00:30:17Z- handleDvl() - RDI_Pathfinder msg: 1613176217.95 epoch sec; seqNo:6453 2021-02-13T00:30:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 103.19 , 109.14 , 86.83 2021-02-13T00:30:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.06 , -0.17 2021-02-13T00:30:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176217.96 epoch sec; seqNo:13220; 4059328.64 north; 592660.80 east 2021-02-13T00:30:18Z- AHRS_M2 msg: 1613176218.33 epoch sec; seqNo:13113 2021-02-13T00:30:18Z- AHRS_M2 msg: 0.70 phi; 0.08 theta; -0.14 psi 2021-02-13T00:30:18Z- handleDepth()-Depth_Keller msg: 1613176218332 epoch sec; seqNo:13221; depth 4.74 2021-02-13T00:30:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176218.34 epoch sec; seqNo:13221; 4059328.64 north; 592660.80 east 2021-02-13T00:30:18Z- AHRS_M2 msg: 1613176218.73 epoch sec; seqNo:13114 2021-02-13T00:30:18Z- AHRS_M2 msg: 0.69 phi; 0.04 theta; -0.15 psi 2021-02-13T00:30:18Z- LcmTrn::handleMessages() - handling 42 messages took 0.70 ms and 128 calls... 2021-02-13T00:30:18Z- waiting for 1613176216.49, time is 1613176218.73 2021-02-13T00:30:18Z- LcmTrn::updateTrn() >>>> pose time 1613176218.73, meas time 1613176217.95 <<<< 2021-02-13T00:30:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176217.95 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:30:18Z- TerrainNav::motionUpdate() - currEstimate = (4059329, 592660.80, 4.74) 2021-02-13T00:30:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:30:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:30:18Z- TNavBF::Projecting Measurements 2021-02-13T00:30:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:30:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:30:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:30:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:30:18Z- TNavFilter::4 good beams. 2021-02-13T00:30:18Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:30:18Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:30:18Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:30:18Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:30:18Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:30:18Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:30:18Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:30:18Z- TNavBF::mmse selected from filter -1 2021-02-13T00:30:18Z- TerrainMapOctree: (vehN, vehE ) = (4059328.18, 592660.26). 2021-02-13T00:30:18Z- TerrainMapOctree: Min Distance = 2494.13. 2021-02-13T00:30:18Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:30:18Z- There is no specific good beam that all particles have in common. 2021-02-13T00:30:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:30:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-13T00:30:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:30:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 17.04 2021-02-13T00:30:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:30:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 19.06 2021-02-13T00:30:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:30:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 12.06 2021-02-13T00:30:18Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:30:18Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:30:18Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:30:18Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:30:18Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:30:18Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:30:18Z- Filter 0 No beams used in subcloud update 2021-02-13T00:30:18Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:30:18Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:30:18Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 699.9 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:30:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176217.95 sec. 2021-02-13T00:30:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:30:18Z- TNavBF::mmse selected from filter -1 2021-02-13T00:30:18Z- variances([0], [2], and psi_berg) = 699.877355 832.997878 0.000000 2021-02-13T00:30:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:30:18Z- LcmTrn::publishEstimates() - MLE : 4059328.14 592693.30 4.74 2021-02-13T00:30:18Z- LcmTrn::publishEstimates() - MMSE : 4059328.64 592660.80 4.74 2021-02-13T00:30:18Z- LcmTrn::publishEstimates() - COVAR: 699.88 833.00 0.93 0.00 2021-02-13T00:30:18Z- handleDepth()-Depth_Keller msg: 1613176218738 epoch sec; seqNo:13222; depth 4.65 2021-02-13T00:30:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176218.75 epoch sec; seqNo:13222; 4059329.02 north; 592660.80 east 2021-02-13T00:30:19Z- AHRS_M2 msg: 1613176219.14 epoch sec; seqNo:13115 2021-02-13T00:30:19Z- AHRS_M2 msg: 0.68 phi; 0.00 theta; -0.17 psi 2021-02-13T00:30:19Z- handleDepth()-Depth_Keller msg: 1613176219152 epoch sec; seqNo:13223; depth 4.56 2021-02-13T00:30:19Z- handleDvl() - RDI_Pathfinder msg: 1613176219.16 epoch sec; seqNo:6454 2021-02-13T00:30:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 102.51 , 99.81 , 90.35 2021-02-13T00:30:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.09 , -0.14 2021-02-13T00:30:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176219.17 epoch sec; seqNo:13223; 4059329.40 north; 592660.79 east 2021-02-13T00:30:19Z- AHRS_M2 msg: 1613176219.54 epoch sec; seqNo:13116 2021-02-13T00:30:19Z- AHRS_M2 msg: 0.68 phi; -0.03 theta; -0.20 psi 2021-02-13T00:30:19Z- handleDepth()-Depth_Keller msg: 1613176219544 epoch sec; seqNo:13224; depth 4.51 2021-02-13T00:30:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176219.56 epoch sec; seqNo:13224; 4059329.40 north; 592660.79 east 2021-02-13T00:30:19Z- AHRS_M2 msg: 1613176219.94 epoch sec; seqNo:13117 2021-02-13T00:30:19Z- AHRS_M2 msg: 0.67 phi; -0.06 theta; -0.23 psi 2021-02-13T00:30:19Z- handleDepth()-Depth_Keller msg: 1613176219949 epoch sec; seqNo:13225; depth 4.49 2021-02-13T00:30:19Z- handleDvl() - RDI_Pathfinder msg: 1613176219.96 epoch sec; seqNo:6455 2021-02-13T00:30:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.56 , 97.44 , 97.44 2021-02-13T00:30:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.09 , -0.07 2021-02-13T00:30:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176219.97 epoch sec; seqNo:13225; 4059329.79 north; 592662.01 east 2021-02-13T00:30:20Z- AHRS_M2 msg: 1613176220.35 epoch sec; seqNo:13118 2021-02-13T00:30:20Z- AHRS_M2 msg: 0.68 phi; -0.10 theta; -0.25 psi 2021-02-13T00:30:20Z- handleDepth()-Depth_Keller msg: 1613176220354 epoch sec; seqNo:13226; depth 4.45 2021-02-13T00:30:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176220.38 epoch sec; seqNo:13226; 4059330.17 north; 592662.00 east 2021-02-13T00:30:20Z- AHRS_M2 msg: 1613176220.75 epoch sec; seqNo:13119 2021-02-13T00:30:20Z- AHRS_M2 msg: 0.69 phi; -0.13 theta; -0.26 psi 2021-02-13T00:30:20Z- handleDepth()-Depth_Keller msg: 1613176220756 epoch sec; seqNo:13227; depth 4.44 2021-02-13T00:30:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176220.78 epoch sec; seqNo:13227; 4059330.55 north; 592662.00 east 2021-02-13T00:30:21Z- AHRS_M2 msg: 1613176221.16 epoch sec; seqNo:13120 2021-02-13T00:30:21Z- AHRS_M2 msg: 0.70 phi; -0.20 theta; -0.29 psi 2021-02-13T00:30:21Z- handleDepth()-Depth_Keller msg: 1613176221168 epoch sec; seqNo:13228; depth 4.44 2021-02-13T00:30:21Z- handleDvl() - RDI_Pathfinder msg: 1613176221.19 epoch sec; seqNo:6456 2021-02-13T00:30:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 113.26 , 97.04 , 102.44 2021-02-13T00:30:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.09 , -0.05 2021-02-13T00:30:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176221.20 epoch sec; seqNo:13228; 4059330.55 north; 592662.00 east 2021-02-13T00:30:21Z- AHRS_M2 msg: 1613176221.56 epoch sec; seqNo:13121 2021-02-13T00:30:21Z- AHRS_M2 msg: 0.70 phi; -0.23 theta; -0.31 psi 2021-02-13T00:30:21Z- handleDepth()-Depth_Keller msg: 1613176221567 epoch sec; seqNo:13229; depth 4.44 2021-02-13T00:30:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176221.58 epoch sec; seqNo:13229; 4059330.93 north; 592662.00 east 2021-02-13T00:30:21Z- AHRS_M2 msg: 1613176221.97 epoch sec; seqNo:13122 2021-02-13T00:30:21Z- AHRS_M2 msg: 0.69 phi; -0.25 theta; -0.31 psi 2021-02-13T00:30:21Z- handleDepth()-Depth_Keller msg: 1613176221977 epoch sec; seqNo:13230; depth 4.43 2021-02-13T00:30:21Z- handleDvl() - RDI_Pathfinder msg: 1613176221.99 epoch sec; seqNo:6457 2021-02-13T00:30:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.87 , 117.32 , 97.04 , 110.56 2021-02-13T00:30:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , -0.22 , 0.07 2021-02-13T00:30:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176222.00 epoch sec; seqNo:13230; 4059330.93 north; 592662.00 east 2021-02-13T00:30:22Z- AHRS_M2 msg: 1613176222.37 epoch sec; seqNo:13123 2021-02-13T00:30:22Z- AHRS_M2 msg: 0.69 phi; -0.26 theta; -0.31 psi 2021-02-13T00:30:22Z- handleDepth()-Depth_Keller msg: 1613176222376 epoch sec; seqNo:13231; depth 4.46 2021-02-13T00:30:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176222.40 epoch sec; seqNo:13231; 4059331.31 north; 592661.99 east 2021-02-13T00:30:22Z- AHRS_M2 msg: 1613176222.79 epoch sec; seqNo:13124 2021-02-13T00:30:22Z- AHRS_M2 msg: 0.68 phi; -0.27 theta; -0.30 psi 2021-02-13T00:30:22Z- handleDepth()-Depth_Keller msg: 1613176222799 epoch sec; seqNo:13232; depth 4.50 2021-02-13T00:30:22Z- handleDvl() - RDI_Pathfinder msg: 1613176222.81 epoch sec; seqNo:6458 2021-02-13T00:30:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.02 , 124.68 , 97.92 , 117.25 2021-02-13T00:30:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.21 , 0.41 2021-02-13T00:30:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176222.82 epoch sec; seqNo:13232; 4059331.68 north; 592661.99 east 2021-02-13T00:30:23Z- AHRS_M2 msg: 1613176223.17 epoch sec; seqNo:13125 2021-02-13T00:30:23Z- AHRS_M2 msg: 0.68 phi; -0.27 theta; -0.29 psi 2021-02-13T00:30:23Z- handleDepth()-Depth_Keller msg: 1613176223181 epoch sec; seqNo:13233; depth 4.55 2021-02-13T00:30:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176223.19 epoch sec; seqNo:13233; 4059331.68 north; 592661.99 east 2021-02-13T00:30:23Z- AHRS_M2 msg: 1613176223.58 epoch sec; seqNo:13126 2021-02-13T00:30:23Z- AHRS_M2 msg: 0.68 phi; -0.28 theta; -0.26 psi 2021-02-13T00:30:23Z- handleDepth()-Depth_Keller msg: 1613176223586 epoch sec; seqNo:13234; depth 4.61 2021-02-13T00:30:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176223.59 epoch sec; seqNo:13234; 4059332.08 north; 592663.20 east 2021-02-13T00:30:23Z- LcmTrn::handleMessages() - handling 43 messages took 0.63 ms and 131 calls... 2021-02-13T00:30:23Z- waiting for 1613176221.73, time is 1613176223.59 2021-02-13T00:30:23Z- LcmTrn::updateTrn() >>>> pose time 1613176223.59, meas time 1613176222.81 <<<< 2021-02-13T00:30:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176222.81 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:30:23Z- TerrainNav::motionUpdate() - currEstimate = (4059332, 592663.20, 4.61) 2021-02-13T00:30:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:30:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:30:23Z- TNavBF::Projecting Measurements 2021-02-13T00:30:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:30:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:30:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:30:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:30:23Z- TNavFilter::4 good beams. 2021-02-13T00:30:23Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:30:23Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:30:23Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:30:23Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:30:23Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:30:23Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:30:23Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:30:23Z- TNavBF::mmse selected from filter -1 2021-02-13T00:30:23Z- TerrainMapOctree: (vehN, vehE ) = (4059331.52, 592662.82). 2021-02-13T00:30:23Z- TerrainMapOctree: Min Distance = 2489.92. 2021-02-13T00:30:23Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:30:23Z- There is no specific good beam that all particles have in common. 2021-02-13T00:30:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:30:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 11.84 2021-02-13T00:30:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:30:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 24.87 2021-02-13T00:30:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:30:23Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:30:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:30:23Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 22.00 2021-02-13T00:30:23Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:30:23Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:30:23Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:30:23Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:30:23Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:30:23Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:30:23Z- Filter 0 No beams used in subcloud update 2021-02-13T00:30:23Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:30:23Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:30:23Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 699.8 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:30:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176222.81 sec. 2021-02-13T00:30:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:30:23Z- TNavBF::mmse selected from filter -1 2021-02-13T00:30:23Z- variances([0], [2], and psi_berg) = 699.804459 832.997839 0.000000 2021-02-13T00:30:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:30:23Z- LcmTrn::publishEstimates() - MLE : 4059331.58 592695.70 4.61 2021-02-13T00:30:23Z- LcmTrn::publishEstimates() - MMSE : 4059332.08 592663.20 4.61 2021-02-13T00:30:23Z- LcmTrn::publishEstimates() - COVAR: 699.80 833.00 0.93 0.00 2021-02-13T00:30:23Z- AHRS_M2 msg: 1613176223.98 epoch sec; seqNo:13127 2021-02-13T00:30:23Z- AHRS_M2 msg: 0.68 phi; -0.28 theta; -0.23 psi 2021-02-13T00:30:23Z- handleDepth()-Depth_Keller msg: 1613176223993 epoch sec; seqNo:13235; depth 4.69 2021-02-13T00:30:24Z- handleDvl() - RDI_Pathfinder msg: 1613176224.01 epoch sec; seqNo:6459 2021-02-13T00:30:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.17 , 119.89 , 0.00 , 119.89 2021-02-13T00:30:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.13 , 0.41 2021-02-13T00:30:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176224.02 epoch sec; seqNo:13235; 4059332.08 north; 592663.20 east 2021-02-13T00:30:24Z- AHRS_M2 msg: 1613176224.39 epoch sec; seqNo:13128 2021-02-13T00:30:24Z- AHRS_M2 msg: 0.68 phi; -0.29 theta; -0.21 psi 2021-02-13T00:30:24Z- handleDepth()-Depth_Keller msg: 1613176224397 epoch sec; seqNo:13236; depth 4.77 2021-02-13T00:30:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176224.41 epoch sec; seqNo:13236; 4059332.46 north; 592663.20 east 2021-02-13T00:30:24Z- AHRS_M2 msg: 1613176224.79 epoch sec; seqNo:13129 2021-02-13T00:30:24Z- AHRS_M2 msg: 0.68 phi; -0.31 theta; -0.19 psi 2021-02-13T00:30:24Z- handleDepth()-Depth_Keller msg: 1613176224797 epoch sec; seqNo:13237; depth 4.86 2021-02-13T00:30:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176224.81 epoch sec; seqNo:13237; 4059332.46 north; 592663.20 east 2021-02-13T00:30:25Z- AHRS_M2 msg: 1613176225.20 epoch sec; seqNo:13130 2021-02-13T00:30:25Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.20 psi 2021-02-13T00:30:25Z- handleDepth()-Depth_Keller msg: 1613176225202 epoch sec; seqNo:13238; depth 4.93 2021-02-13T00:30:25Z- handleDvl() - RDI_Pathfinder msg: 1613176225.21 epoch sec; seqNo:6460 2021-02-13T00:30:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.31 , 113.67 , 86.90 , 119.61 2021-02-13T00:30:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.33 , 0.05 2021-02-13T00:30:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176225.22 epoch sec; seqNo:13238; 4059332.83 north; 592663.20 east 2021-02-13T00:30:25Z- AHRS_M2 msg: 1613176225.60 epoch sec; seqNo:13131 2021-02-13T00:30:25Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.23 psi 2021-02-13T00:30:25Z- handleDepth()-Depth_Keller msg: 1613176225609 epoch sec; seqNo:13239; depth 5.02 2021-02-13T00:30:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176225.63 epoch sec; seqNo:13239; 4059332.85 north; 592664.42 east 2021-02-13T00:30:26Z- AHRS_M2 msg: 1613176226.01 epoch sec; seqNo:13132 2021-02-13T00:30:26Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.25 psi 2021-02-13T00:30:26Z- handleDepth()-Depth_Keller msg: 1613176226013 epoch sec; seqNo:13240; depth 5.09 2021-02-13T00:30:26Z- handleDvl() - RDI_Pathfinder msg: 1613176226.04 epoch sec; seqNo:6461 2021-02-13T00:30:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 112.58 , 84.33 , 123.00 2021-02-13T00:30:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.23 , -0.31 2021-02-13T00:30:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176226.05 epoch sec; seqNo:13240; 4059333.23 north; 592664.41 east 2021-02-13T00:30:26Z- AHRS_M2 msg: 1613176226.41 epoch sec; seqNo:13133 2021-02-13T00:30:26Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.27 psi 2021-02-13T00:30:26Z- handleDepth()-Depth_Keller msg: 1613176226429 epoch sec; seqNo:13241; depth 5.17 2021-02-13T00:30:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176226.44 epoch sec; seqNo:13241; 4059333.23 north; 592664.41 east 2021-02-13T00:30:26Z- AHRS_M2 msg: 1613176226.83 epoch sec; seqNo:13134 2021-02-13T00:30:26Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.28 psi 2021-02-13T00:30:26Z- handleDepth()-Depth_Keller msg: 1613176226835 epoch sec; seqNo:13242; depth 5.27 2021-02-13T00:30:26Z- handleDvl() - RDI_Pathfinder msg: 1613176226.85 epoch sec; seqNo:6462 2021-02-13T00:30:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 118.53 , 85.82 , 127.46 2021-02-13T00:30:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.22 , -0.27 2021-02-13T00:30:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176226.89 epoch sec; seqNo:13242; 4059333.60 north; 592664.41 east 2021-02-13T00:30:27Z- AHRS_M2 msg: 1613176227.21 epoch sec; seqNo:13135 2021-02-13T00:30:27Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.27 psi 2021-02-13T00:30:27Z- handleDepth()-Depth_Keller msg: 1613176227220 epoch sec; seqNo:13243; depth 5.36 2021-02-13T00:30:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176227.24 epoch sec; seqNo:13243; 4059333.98 north; 592664.40 east 2021-02-13T00:30:27Z- AHRS_M2 msg: 1613176227.62 epoch sec; seqNo:13136 2021-02-13T00:30:27Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.25 psi 2021-02-13T00:30:27Z- handleDepth()-Depth_Keller msg: 1613176227629 epoch sec; seqNo:13244; depth 5.45 2021-02-13T00:30:27Z- handleDvl() - RDI_Pathfinder msg: 1613176227.65 epoch sec; seqNo:6463 2021-02-13T00:30:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:30:27Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:30:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176227.66 epoch sec; seqNo:13244; 4059334.36 north; 592664.40 east 2021-02-13T00:30:28Z- AHRS_M2 msg: 1613176228.02 epoch sec; seqNo:13137 2021-02-13T00:30:28Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.22 psi 2021-02-13T00:30:28Z- handleDepth()-Depth_Keller msg: 1613176228028 epoch sec; seqNo:13245; depth 5.57 2021-02-13T00:30:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176228.04 epoch sec; seqNo:13245; 4059334.75 north; 592665.62 east 2021-02-13T00:30:28Z- AHRS_M2 msg: 1613176228.43 epoch sec; seqNo:13138 2021-02-13T00:30:28Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.20 psi 2021-02-13T00:30:28Z- handleDepth()-Depth_Keller msg: 1613176228445 epoch sec; seqNo:13246; depth 5.67 2021-02-13T00:30:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176228.45 epoch sec; seqNo:13246; 4059335.13 north; 592665.61 east 2021-02-13T00:30:28Z- AHRS_M2 msg: 1613176228.83 epoch sec; seqNo:13139 2021-02-13T00:30:28Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.19 psi 2021-02-13T00:30:28Z- handleDepth()-Depth_Keller msg: 1613176228842 epoch sec; seqNo:13247; depth 5.78 2021-02-13T00:30:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176228.86 epoch sec; seqNo:13247; 4059335.51 north; 592665.61 east 2021-02-13T00:30:28Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-13T00:30:28Z- waiting for 1613176226.59, time is 1613176228.86 2021-02-13T00:30:28Z- LcmTrn::updateTrn() >>>> pose time 1613176228.86, meas time 1613176227.65 <<<< 2021-02-13T00:30:28Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613176227.65, ping # 0. 2021-02-13T00:30:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176227.65 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:30:28Z- TerrainNav::motionUpdate() - currEstimate = (4059336, 592665.61, 5.78) 2021-02-13T00:30:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:30:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:30:28Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:30:28Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:30:28Z- TNavBF::Projecting Measurements 2021-02-13T00:30:28Z- TNavFilter::Measurement from time = 1613176227.65 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:30:28Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:30:28Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:30:28Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:30:28Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:30:28Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:30:28Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:30:28Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:30:28Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 699.8 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:30:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:30:28Z- TNavBF::mmse selected from filter -1 2021-02-13T00:30:28Z- variances([0], [2], and psi_berg) = 699.729411 832.997800 0.000000 2021-02-13T00:30:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:30:28Z- LcmTrn::publishEstimates() - MLE : 4059335.01 592637.11 5.78 2021-02-13T00:30:28Z- LcmTrn::publishEstimates() - MMSE : 4059335.51 592665.61 5.78 2021-02-13T00:30:28Z- LcmTrn::publishEstimates() - COVAR: 699.73 833.00 1.14 0.00 2021-02-13T00:30:29Z- AHRS_M2 msg: 1613176229.24 epoch sec; seqNo:13140 2021-02-13T00:30:29Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.17 psi 2021-02-13T00:30:29Z- handleDepth()-Depth_Keller msg: 1613176229240 epoch sec; seqNo:13248; depth 5.88 2021-02-13T00:30:29Z- handleDvl() - RDI_Pathfinder msg: 1613176229.25 epoch sec; seqNo:6464 2021-02-13T00:30:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.44 , 112.32 , 84.06 , 121.24 2021-02-13T00:30:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.16 , 0.41 2021-02-13T00:30:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176229.28 epoch sec; seqNo:13248; 4059335.89 north; 592665.60 east 2021-02-13T00:30:29Z- AHRS_M2 msg: 1613176229.64 epoch sec; seqNo:13141 2021-02-13T00:30:29Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.17 psi 2021-02-13T00:30:29Z- handleDepth()-Depth_Keller msg: 1613176229649 epoch sec; seqNo:13249; depth 5.98 2021-02-13T00:30:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176229.66 epoch sec; seqNo:13249; 4059335.89 north; 592665.60 east 2021-02-13T00:30:30Z- AHRS_M2 msg: 1613176230.05 epoch sec; seqNo:13142 2021-02-13T00:30:30Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.17 psi 2021-02-13T00:30:30Z- handleDepth()-Depth_Keller msg: 1613176230049 epoch sec; seqNo:13250; depth 6.08 2021-02-13T00:30:30Z- handleDvl() - RDI_Pathfinder msg: 1613176230.06 epoch sec; seqNo:6465 2021-02-13T00:30:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 109.68 , 82.91 , 123.06 2021-02-13T00:30:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , 0.16 , 0.36 2021-02-13T00:30:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176230.07 epoch sec; seqNo:13250; 4059335.89 north; 592665.60 east 2021-02-13T00:30:30Z- AHRS_M2 msg: 1613176230.45 epoch sec; seqNo:13143 2021-02-13T00:30:30Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.18 psi 2021-02-13T00:30:30Z- handleDepth()-Depth_Keller msg: 1613176230462 epoch sec; seqNo:13251; depth 6.19 2021-02-13T00:30:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176230.47 epoch sec; seqNo:13251; 4059336.27 north; 592665.60 east 2021-02-13T00:30:30Z- AHRS_M2 msg: 1613176230.85 epoch sec; seqNo:13144 2021-02-13T00:30:30Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.19 psi 2021-02-13T00:30:30Z- handleDepth()-Depth_Keller msg: 1613176230856 epoch sec; seqNo:13252; depth 6.29 2021-02-13T00:30:30Z- handleDvl() - RDI_Pathfinder msg: 1613176230.87 epoch sec; seqNo:6466 2021-02-13T00:30:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 109.75 , 79.69 , 124.62 2021-02-13T00:30:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.33 , 0.10 2021-02-13T00:30:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176230.89 epoch sec; seqNo:13252; 4059336.28 north; 592666.82 east 2021-02-13T00:30:31Z- AHRS_M2 msg: 1613176231.26 epoch sec; seqNo:13145 2021-02-13T00:30:31Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.21 psi 2021-02-13T00:30:31Z- handleDepth()-Depth_Keller msg: 1613176231265 epoch sec; seqNo:13253; depth 6.39 2021-02-13T00:30:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176231.28 epoch sec; seqNo:13253; 4059336.66 north; 592666.82 east 2021-02-13T00:30:31Z- AHRS_M2 msg: 1613176231.67 epoch sec; seqNo:13146 2021-02-13T00:30:31Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.22 psi 2021-02-13T00:30:31Z- handleDepth()-Depth_Keller msg: 1613176231674 epoch sec; seqNo:13254; depth 6.51 2021-02-13T00:30:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176231.68 epoch sec; seqNo:13254; 4059336.66 north; 592666.82 east 2021-02-13T00:30:32Z- AHRS_M2 msg: 1613176232.06 epoch sec; seqNo:13147 2021-02-13T00:30:32Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.24 psi 2021-02-13T00:30:32Z- handleDepth()-Depth_Keller msg: 1613176232068 epoch sec; seqNo:13255; depth 6.65 2021-02-13T00:30:32Z- handleDvl() - RDI_Pathfinder msg: 1613176232.09 epoch sec; seqNo:6467 2021-02-13T00:30:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 112.72 , 0.00 , 130.57 2021-02-13T00:30:32Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , 0.22 , -0.17 2021-02-13T00:30:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176232.10 epoch sec; seqNo:13255; 4059337.04 north; 592666.81 east 2021-02-13T00:30:32Z- AHRS_M2 msg: 1613176232.47 epoch sec; seqNo:13148 2021-02-13T00:30:32Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.25 psi 2021-02-13T00:30:32Z- handleDepth()-Depth_Keller msg: 1613176232473 epoch sec; seqNo:13256; depth 6.75 2021-02-13T00:30:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176232.48 epoch sec; seqNo:13256; 4059337.43 north; 592668.03 east 2021-02-13T00:30:32Z- AHRS_M2 msg: 1613176232.87 epoch sec; seqNo:13149 2021-02-13T00:30:32Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.25 psi 2021-02-13T00:30:32Z- handleDepth()-Depth_Keller msg: 1613176232881 epoch sec; seqNo:13257; depth 6.88 2021-02-13T00:30:32Z- handleDvl() - RDI_Pathfinder msg: 1613176232.89 epoch sec; seqNo:6468 2021-02-13T00:30:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.80 , 115.09 , 0.00 , 126.98 2021-02-13T00:30:32Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , -0.06 , -0.24 2021-02-13T00:30:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176232.90 epoch sec; seqNo:13257; 4059337.81 north; 592668.03 east 2021-02-13T00:30:33Z- AHRS_M2 msg: 1613176233.28 epoch sec; seqNo:13150 2021-02-13T00:30:33Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.24 psi 2021-02-13T00:30:33Z- handleDepth()-Depth_Keller msg: 1613176233282 epoch sec; seqNo:13258; depth 7.03 2021-02-13T00:30:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176233.29 epoch sec; seqNo:13258; 4059338.19 north; 592668.02 east 2021-02-13T00:30:33Z- AHRS_M2 msg: 1613176233.68 epoch sec; seqNo:13151 2021-02-13T00:30:33Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.23 psi 2021-02-13T00:30:33Z- handleDepth()-Depth_Keller msg: 1613176233684 epoch sec; seqNo:13259; depth 7.16 2021-02-13T00:30:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176233.70 epoch sec; seqNo:13259; 4059338.57 north; 592668.02 east 2021-02-13T00:30:34Z- LcmTrn::handleMessages() - handling 41 messages took 0.61 ms and 129 calls... 2021-02-13T00:30:34Z- waiting for 1613176231.86, time is 1613176233.70 2021-02-13T00:30:34Z- LcmTrn::updateTrn() >>>> pose time 1613176233.70, meas time 1613176232.89 <<<< 2021-02-13T00:30:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176232.89 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:30:34Z- TerrainNav::motionUpdate() - currEstimate = (4059339, 592668.02, 7.16) 2021-02-13T00:30:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:30:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:30:34Z- TNavBF::Projecting Measurements 2021-02-13T00:30:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:30:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:30:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:30:34Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:30:34Z- TNavFilter::3 good beams. 2021-02-13T00:30:34Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:30:34Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:30:34Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:30:34Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:30:34Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:30:34Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:30:34Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:30:34Z- TNavBF::mmse selected from filter -1 2021-02-13T00:30:34Z- TerrainMapOctree: (vehN, vehE ) = (4059338.06, 592667.62). 2021-02-13T00:30:34Z- TerrainMapOctree: Min Distance = 2481.83. 2021-02-13T00:30:34Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:30:34Z- There is no specific good beam that all particles have in common. 2021-02-13T00:30:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:30:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 12.90 2021-02-13T00:30:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:30:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 21.19 2021-02-13T00:30:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:30:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 25.80 2021-02-13T00:30:34Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:30:34Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:30:34Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:30:34Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:30:34Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:30:34Z- Filter 0 No beams used in subcloud update 2021-02-13T00:30:34Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:30:34Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:30:34Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 699.7 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:30:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176232.89 sec. 2021-02-13T00:30:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:30:34Z- TNavBF::mmse selected from filter -1 2021-02-13T00:30:34Z- variances([0], [2], and psi_berg) = 699.659084 832.997763 0.000000 2021-02-13T00:30:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:30:34Z- LcmTrn::publishEstimates() - MLE : 4059338.07 592639.52 7.16 2021-02-13T00:30:34Z- LcmTrn::publishEstimates() - MMSE : 4059338.57 592668.02 7.16 2021-02-13T00:30:34Z- LcmTrn::publishEstimates() - COVAR: 699.66 833.00 1.13 0.00 2021-02-13T00:30:34Z- AHRS_M2 msg: 1613176234.08 epoch sec; seqNo:13152 2021-02-13T00:30:34Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.21 psi 2021-02-13T00:30:34Z- handleDepth()-Depth_Keller msg: 1613176234088 epoch sec; seqNo:13260; depth 7.29 2021-02-13T00:30:34Z- handleDvl() - RDI_Pathfinder msg: 1613176234.10 epoch sec; seqNo:6469 2021-02-13T00:30:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.32 , 113.60 , 82.37 , 124.01 2021-02-13T00:30:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.21 , -0.26 2021-02-13T00:30:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176234.11 epoch sec; seqNo:13260; 4059338.95 north; 592668.01 east 2021-02-13T00:30:34Z- AHRS_M2 msg: 1613176234.49 epoch sec; seqNo:13153 2021-02-13T00:30:34Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.20 psi 2021-02-13T00:30:34Z- handleDepth()-Depth_Keller msg: 1613176234494 epoch sec; seqNo:13261; depth 7.42 2021-02-13T00:30:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176234.51 epoch sec; seqNo:13261; 4059339.33 north; 592668.01 east 2021-02-13T00:30:34Z- AHRS_M2 msg: 1613176234.89 epoch sec; seqNo:13154 2021-02-13T00:30:34Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.20 psi 2021-02-13T00:30:34Z- handleDepth()-Depth_Keller msg: 1613176234901 epoch sec; seqNo:13262; depth 7.54 2021-02-13T00:30:34Z- handleDvl() - RDI_Pathfinder msg: 1613176234.92 epoch sec; seqNo:6470 2021-02-13T00:30:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 109.81 , 81.56 , 121.71 2021-02-13T00:30:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.33 , -0.16 2021-02-13T00:30:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176234.93 epoch sec; seqNo:13262; 4059339.71 north; 592668.01 east 2021-02-13T00:30:35Z- AHRS_M2 msg: 1613176235.30 epoch sec; seqNo:13155 2021-02-13T00:30:35Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.19 psi 2021-02-13T00:30:35Z- handleDepth()-Depth_Keller msg: 1613176235305 epoch sec; seqNo:13263; depth 7.67 2021-02-13T00:30:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176235.32 epoch sec; seqNo:13263; 4059340.08 north; 592668.00 east 2021-02-13T00:30:35Z- AHRS_M2 msg: 1613176235.70 epoch sec; seqNo:13156 2021-02-13T00:30:35Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.17 psi 2021-02-13T00:30:35Z- handleDepth()-Depth_Keller msg: 1613176235706 epoch sec; seqNo:13264; depth 7.78 2021-02-13T00:30:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176235.71 epoch sec; seqNo:13264; 4059340.48 north; 592669.22 east 2021-02-13T00:30:36Z- AHRS_M2 msg: 1613176236.10 epoch sec; seqNo:13157 2021-02-13T00:30:36Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.16 psi 2021-02-13T00:30:36Z- handleDepth()-Depth_Keller msg: 1613176236108 epoch sec; seqNo:13265; depth 7.90 2021-02-13T00:30:36Z- handleDvl() - RDI_Pathfinder msg: 1613176236.13 epoch sec; seqNo:6471 2021-02-13T00:30:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 108.12 , 0.00 , 123.00 2021-02-13T00:30:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.28 , 0.11 2021-02-13T00:30:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176236.14 epoch sec; seqNo:13265; 4059340.48 north; 592669.22 east 2021-02-13T00:30:36Z- AHRS_M2 msg: 1613176236.51 epoch sec; seqNo:13158 2021-02-13T00:30:36Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.14 psi 2021-02-13T00:30:36Z- handleDepth()-Depth_Keller msg: 1613176236515 epoch sec; seqNo:13266; depth 8.04 2021-02-13T00:30:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176236.53 epoch sec; seqNo:13266; 4059340.86 north; 592669.21 east 2021-02-13T00:30:36Z- AHRS_M2 msg: 1613176236.92 epoch sec; seqNo:13159 2021-02-13T00:30:36Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.13 psi 2021-02-13T00:30:36Z- handleDepth()-Depth_Keller msg: 1613176236924 epoch sec; seqNo:13267; depth 8.15 2021-02-13T00:30:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176236.94 epoch sec; seqNo:13267; 4059341.23 north; 592669.21 east 2021-02-13T00:30:37Z- AHRS_M2 msg: 1613176237.32 epoch sec; seqNo:13160 2021-02-13T00:30:37Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.13 psi 2021-02-13T00:30:37Z- handleDepth()-Depth_Keller msg: 1613176237329 epoch sec; seqNo:13268; depth 8.27 2021-02-13T00:30:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176237.34 epoch sec; seqNo:13268; 4059341.23 north; 592669.21 east 2021-02-13T00:30:37Z- AHRS_M2 msg: 1613176237.72 epoch sec; seqNo:13161 2021-02-13T00:30:37Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.13 psi 2021-02-13T00:30:37Z- handleDepth()-Depth_Keller msg: 1613176237724 epoch sec; seqNo:13269; depth 8.39 2021-02-13T00:30:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176237.75 epoch sec; seqNo:13269; 4059341.61 north; 592669.21 east 2021-02-13T00:30:38Z- AHRS_M2 msg: 1613176238.13 epoch sec; seqNo:13162 2021-02-13T00:30:38Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.16 psi 2021-02-13T00:30:38Z- handleDepth()-Depth_Keller msg: 1613176238137 epoch sec; seqNo:13270; depth 8.51 2021-02-13T00:30:38Z- handleDvl() - RDI_Pathfinder msg: 1613176238.16 epoch sec; seqNo:6472 2021-02-13T00:30:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 102.44 , 79.69 , 118.67 2021-02-13T00:30:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.08 , 0.18 2021-02-13T00:30:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176238.17 epoch sec; seqNo:13270; 4059341.99 north; 592669.20 east 2021-02-13T00:30:38Z- AHRS_M2 msg: 1613176238.54 epoch sec; seqNo:13163 2021-02-13T00:30:38Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.17 psi 2021-02-13T00:30:38Z- handleDepth()-Depth_Keller msg: 1613176238543 epoch sec; seqNo:13271; depth 8.65 2021-02-13T00:30:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176238.55 epoch sec; seqNo:13271; 4059341.99 north; 592669.20 east 2021-02-13T00:30:38Z- AHRS_M2 msg: 1613176238.93 epoch sec; seqNo:13164 2021-02-13T00:30:38Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-13T00:30:38Z- handleDepth()-Depth_Keller msg: 1613176238937 epoch sec; seqNo:13272; depth 8.75 2021-02-13T00:30:38Z- handleDvl() - RDI_Pathfinder msg: 1613176238.96 epoch sec; seqNo:6473 2021-02-13T00:30:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 105.22 , 0.00 , 120.09 2021-02-13T00:30:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.15 , 0.04 2021-02-13T00:30:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176238.97 epoch sec; seqNo:13272; 4059342.37 north; 592669.20 east 2021-02-13T00:30:39Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 130 calls... 2021-02-13T00:30:39Z- waiting for 1613176236.70, time is 1613176238.97 2021-02-13T00:30:39Z- LcmTrn::updateTrn() equating pose 1613176238.97 and meas times 1613176238.97 2021-02-13T00:30:39Z- LcmTrn::updateTrn() >>>> pose time 1613176238.97, meas time 1613176238.97 <<<< 2021-02-13T00:30:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176238.97 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:30:39Z- TerrainNav::motionUpdate() - currEstimate = (4059342, 592669.20, 8.75) 2021-02-13T00:30:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:30:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:30:39Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176238.97 sec.; No relevant navigation data available 2021-02-13T00:30:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:30:39Z- TNavBF::mmse selected from filter -1 2021-02-13T00:30:39Z- variances([0], [2], and psi_berg) = 699.605287 832.997735 0.000000 2021-02-13T00:30:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:30:39Z- LcmTrn::publishEstimates() - MLE : 4059341.87 592699.70 8.75 2021-02-13T00:30:39Z- LcmTrn::publishEstimates() - MMSE : 4059342.37 592669.20 8.75 2021-02-13T00:30:39Z- LcmTrn::publishEstimates() - COVAR: 699.61 833.00 1.33 0.00 2021-02-13T00:30:39Z- AHRS_M2 msg: 1613176239.34 epoch sec; seqNo:13165 2021-02-13T00:30:39Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.19 psi 2021-02-13T00:30:39Z- handleDepth()-Depth_Keller msg: 1613176239340 epoch sec; seqNo:13273; depth 8.87 2021-02-13T00:30:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176239.35 epoch sec; seqNo:13273; 4059342.38 north; 592670.42 east 2021-02-13T00:30:39Z- AHRS_M2 msg: 1613176239.74 epoch sec; seqNo:13166 2021-02-13T00:30:39Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.20 psi 2021-02-13T00:30:39Z- handleDepth()-Depth_Keller msg: 1613176239744 epoch sec; seqNo:13274; depth 9.00 2021-02-13T00:30:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176239.75 epoch sec; seqNo:13274; 4059342.76 north; 592670.41 east 2021-02-13T00:30:40Z- AHRS_M2 msg: 1613176240.15 epoch sec; seqNo:13167 2021-02-13T00:30:40Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.21 psi 2021-02-13T00:30:40Z- handleDepth()-Depth_Keller msg: 1613176240161 epoch sec; seqNo:13275; depth 9.12 2021-02-13T00:30:40Z- handleDvl() - RDI_Pathfinder msg: 1613176240.17 epoch sec; seqNo:6474 2021-02-13T00:30:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.30 , 109.61 , 78.46 , 121.51 2021-02-13T00:30:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.10 , -0.05 2021-02-13T00:30:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176240.18 epoch sec; seqNo:13275; 4059343.14 north; 592670.41 east 2021-02-13T00:30:40Z- AHRS_M2 msg: 1613176240.55 epoch sec; seqNo:13168 2021-02-13T00:30:40Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.21 psi 2021-02-13T00:30:40Z- handleDepth()-Depth_Keller msg: 1613176240552 epoch sec; seqNo:13276; depth 9.26 2021-02-13T00:30:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176240.57 epoch sec; seqNo:13276; 4059343.52 north; 592670.41 east 2021-02-13T00:30:40Z- AHRS_M2 msg: 1613176240.95 epoch sec; seqNo:13169 2021-02-13T00:30:40Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.21 psi 2021-02-13T00:30:40Z- handleDepth()-Depth_Keller msg: 1613176240957 epoch sec; seqNo:13277; depth 9.36 2021-02-13T00:30:40Z- handleDvl() - RDI_Pathfinder msg: 1613176240.97 epoch sec; seqNo:6475 2021-02-13T00:30:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.29 , 110.62 , 80.88 , 121.44 2021-02-13T00:30:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.05 , -0.07 2021-02-13T00:30:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176240.99 epoch sec; seqNo:13277; 4059343.52 north; 592670.41 east 2021-02-13T00:30:41Z- AHRS_M2 msg: 1613176241.36 epoch sec; seqNo:13170 2021-02-13T00:30:41Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.20 psi 2021-02-13T00:30:41Z- handleDepth()-Depth_Keller msg: 1613176241365 epoch sec; seqNo:13278; depth 9.49 2021-02-13T00:30:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176241.38 epoch sec; seqNo:13278; 4059343.90 north; 592670.40 east 2021-02-13T00:30:41Z- AHRS_M2 msg: 1613176241.76 epoch sec; seqNo:13171 2021-02-13T00:30:41Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.19 psi 2021-02-13T00:30:41Z- handleDepth()-Depth_Keller msg: 1613176241773 epoch sec; seqNo:13279; depth 9.61 2021-02-13T00:30:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176241.78 epoch sec; seqNo:13279; 4059344.29 north; 592671.62 east 2021-02-13T00:30:42Z- AHRS_M2 msg: 1613176242.16 epoch sec; seqNo:13172 2021-02-13T00:30:42Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.19 psi 2021-02-13T00:30:42Z- handleDepth()-Depth_Keller msg: 1613176242173 epoch sec; seqNo:13280; depth 9.73 2021-02-13T00:30:42Z- handleDvl() - RDI_Pathfinder msg: 1613176242.19 epoch sec; seqNo:6476 2021-02-13T00:30:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.87 , 106.50 , 80.81 , 115.96 2021-02-13T00:30:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , -0.08 2021-02-13T00:30:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176242.20 epoch sec; seqNo:13280; 4059344.67 north; 592671.61 east 2021-02-13T00:30:42Z- AHRS_M2 msg: 1613176242.57 epoch sec; seqNo:13173 2021-02-13T00:30:42Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.18 psi 2021-02-13T00:30:42Z- handleDepth()-Depth_Keller msg: 1613176242577 epoch sec; seqNo:13281; depth 9.85 2021-02-13T00:30:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176242.59 epoch sec; seqNo:13281; 4059345.05 north; 592671.61 east 2021-02-13T00:30:42Z- AHRS_M2 msg: 1613176242.97 epoch sec; seqNo:13174 2021-02-13T00:30:42Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.18 psi 2021-02-13T00:30:42Z- handleDepth()-Depth_Keller msg: 1613176242978 epoch sec; seqNo:13282; depth 9.98 2021-02-13T00:30:42Z- handleDvl() - RDI_Pathfinder msg: 1613176242.99 epoch sec; seqNo:6477 2021-02-13T00:30:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 104.07 , 79.73 , 114.88 2021-02-13T00:30:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , -0.07 2021-02-13T00:30:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176243.00 epoch sec; seqNo:13282; 4059345.05 north; 592671.61 east 2021-02-13T00:30:43Z- AHRS_M2 msg: 1613176243.38 epoch sec; seqNo:13175 2021-02-13T00:30:43Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.18 psi 2021-02-13T00:30:43Z- handleDepth()-Depth_Keller msg: 1613176243389 epoch sec; seqNo:13283; depth 10.09 2021-02-13T00:30:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176243.40 epoch sec; seqNo:13283; 4059345.43 north; 592671.61 east 2021-02-13T00:30:43Z- AHRS_M2 msg: 1613176243.78 epoch sec; seqNo:13176 2021-02-13T00:30:43Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.18 psi 2021-02-13T00:30:43Z- handleDepth()-Depth_Keller msg: 1613176243784 epoch sec; seqNo:13284; depth 10.21 2021-02-13T00:30:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176243.81 epoch sec; seqNo:13284; 4059345.81 north; 592671.60 east 2021-02-13T00:30:44Z- LcmTrn::handleMessages() - handling 40 messages took 0.62 ms and 128 calls... 2021-02-13T00:30:44Z- waiting for 1613176241.97, time is 1613176243.81 2021-02-13T00:30:44Z- LcmTrn::updateTrn() >>>> pose time 1613176243.81, meas time 1613176242.99 <<<< 2021-02-13T00:30:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176242.99 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:30:44Z- TerrainNav::motionUpdate() - currEstimate = (4059346, 592671.60, 10.21) 2021-02-13T00:30:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:30:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:30:44Z- TNavBF::Projecting Measurements 2021-02-13T00:30:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:30:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:30:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:30:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:30:44Z- TNavFilter::4 good beams. 2021-02-13T00:30:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:30:44Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:30:44Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:30:44Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:30:44Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:30:44Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:30:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:30:44Z- TNavBF::mmse selected from filter -1 2021-02-13T00:30:44Z- TerrainMapOctree: (vehN, vehE ) = (4059345.23, 592671.20). 2021-02-13T00:30:44Z- TerrainMapOctree: Min Distance = 2474.02. 2021-02-13T00:30:44Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:30:44Z- There is no specific good beam that all particles have in common. 2021-02-13T00:30:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:30:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.51 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 11.60 2021-02-13T00:30:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:30:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.36 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 17.33 2021-02-13T00:30:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:30:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 10.17 2021-02-13T00:30:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:30:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 21.12 2021-02-13T00:30:44Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:30:44Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:30:44Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:30:44Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:30:44Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:30:44Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:30:44Z- Filter 0 No beams used in subcloud update 2021-02-13T00:30:44Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:30:44Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:30:44Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 699.6 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:30:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176242.99 sec. 2021-02-13T00:30:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:30:44Z- TNavBF::mmse selected from filter -1 2021-02-13T00:30:44Z- variances([0], [2], and psi_berg) = 699.532316 832.997697 0.000000 2021-02-13T00:30:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:30:44Z- LcmTrn::publishEstimates() - MLE : 4059345.31 592702.10 10.21 2021-02-13T00:30:44Z- LcmTrn::publishEstimates() - MMSE : 4059345.81 592671.60 10.21 2021-02-13T00:30:44Z- LcmTrn::publishEstimates() - COVAR: 699.53 833.00 1.13 0.00 2021-02-13T00:30:44Z- AHRS_M2 msg: 1613176244.18 epoch sec; seqNo:13177 2021-02-13T00:30:44Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.18 psi 2021-02-13T00:30:44Z- handleDepth()-Depth_Keller msg: 1613176244193 epoch sec; seqNo:13285; depth 10.30 2021-02-13T00:30:44Z- handleDvl() - RDI_Pathfinder msg: 1613176244.20 epoch sec; seqNo:6478 2021-02-13T00:30:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.14 , 79.80 , 113.60 2021-02-13T00:30:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.10 , 0.01 2021-02-13T00:30:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176244.23 epoch sec; seqNo:13285; 4059346.20 north; 592672.82 east 2021-02-13T00:30:44Z- AHRS_M2 msg: 1613176244.59 epoch sec; seqNo:13178 2021-02-13T00:30:44Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.18 psi 2021-02-13T00:30:44Z- handleDepth()-Depth_Keller msg: 1613176244592 epoch sec; seqNo:13286; depth 10.43 2021-02-13T00:30:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176244.60 epoch sec; seqNo:13286; 4059346.58 north; 592672.81 east 2021-02-13T00:30:44Z- AHRS_M2 msg: 1613176244.99 epoch sec; seqNo:13179 2021-02-13T00:30:44Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.17 psi 2021-02-13T00:30:45Z- handleDepth()-Depth_Keller msg: 1613176245006 epoch sec; seqNo:13287; depth 10.52 2021-02-13T00:30:45Z- handleDvl() - RDI_Pathfinder msg: 1613176245.02 epoch sec; seqNo:6479 2021-02-13T00:30:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.51 , 77.24 , 116.03 2021-02-13T00:30:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.11 , 0.14 2021-02-13T00:30:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176245.03 epoch sec; seqNo:13287; 4059346.58 north; 592672.81 east 2021-02-13T00:30:45Z- AHRS_M2 msg: 1613176245.39 epoch sec; seqNo:13180 2021-02-13T00:30:45Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.17 psi 2021-02-13T00:30:45Z- handleDepth()-Depth_Keller msg: 1613176245400 epoch sec; seqNo:13288; depth 10.64 2021-02-13T00:30:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176245.41 epoch sec; seqNo:13288; 4059346.96 north; 592672.81 east 2021-02-13T00:30:45Z- AHRS_M2 msg: 1613176245.80 epoch sec; seqNo:13181 2021-02-13T00:30:45Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.18 psi 2021-02-13T00:30:45Z- handleDepth()-Depth_Keller msg: 1613176245804 epoch sec; seqNo:13289; depth 10.74 2021-02-13T00:30:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176245.81 epoch sec; seqNo:13289; 4059347.34 north; 592672.81 east 2021-02-13T00:30:46Z- AHRS_M2 msg: 1613176246.20 epoch sec; seqNo:13182 2021-02-13T00:30:46Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.19 psi 2021-02-13T00:30:46Z- handleDepth()-Depth_Keller msg: 1613176246218 epoch sec; seqNo:13290; depth 10.85 2021-02-13T00:30:46Z- handleDvl() - RDI_Pathfinder msg: 1613176246.23 epoch sec; seqNo:6480 2021-02-13T00:30:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.36 , 101.23 , 79.60 , 113.40 2021-02-13T00:30:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , 0.17 2021-02-13T00:30:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176246.24 epoch sec; seqNo:13290; 4059347.71 north; 592672.80 east 2021-02-13T00:30:46Z- AHRS_M2 msg: 1613176246.61 epoch sec; seqNo:13183 2021-02-13T00:30:46Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.19 psi 2021-02-13T00:30:46Z- handleDepth()-Depth_Keller msg: 1613176246612 epoch sec; seqNo:13291; depth 10.96 2021-02-13T00:30:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176246.62 epoch sec; seqNo:13291; 4059347.71 north; 592672.80 east 2021-02-13T00:30:47Z- AHRS_M2 msg: 1613176247.01 epoch sec; seqNo:13184 2021-02-13T00:30:47Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.19 psi 2021-02-13T00:30:47Z- handleDepth()-Depth_Keller msg: 1613176247016 epoch sec; seqNo:13292; depth 11.06 2021-02-13T00:30:47Z- handleDvl() - RDI_Pathfinder msg: 1613176247.03 epoch sec; seqNo:6481 2021-02-13T00:30:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.20 , 79.87 , 113.67 2021-02-13T00:30:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.04 , 0.03 2021-02-13T00:30:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176247.04 epoch sec; seqNo:13292; 4059348.11 north; 592674.02 east 2021-02-13T00:30:47Z- AHRS_M2 msg: 1613176247.41 epoch sec; seqNo:13185 2021-02-13T00:30:47Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.19 psi 2021-02-13T00:30:47Z- handleDepth()-Depth_Keller msg: 1613176247422 epoch sec; seqNo:13293; depth 11.17 2021-02-13T00:30:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176247.43 epoch sec; seqNo:13293; 4059348.49 north; 592674.01 east 2021-02-13T00:30:47Z- AHRS_M2 msg: 1613176247.82 epoch sec; seqNo:13186 2021-02-13T00:30:47Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.20 psi 2021-02-13T00:30:47Z- handleDepth()-Depth_Keller msg: 1613176247824 epoch sec; seqNo:13294; depth 11.27 2021-02-13T00:30:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176247.85 epoch sec; seqNo:13294; 4059348.86 north; 592674.01 east 2021-02-13T00:30:48Z- AHRS_M2 msg: 1613176248.22 epoch sec; seqNo:13187 2021-02-13T00:30:48Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.20 psi 2021-02-13T00:30:48Z- handleDepth()-Depth_Keller msg: 1613176248229 epoch sec; seqNo:13295; depth 11.39 2021-02-13T00:30:48Z- handleDvl() - RDI_Pathfinder msg: 1613176248.24 epoch sec; seqNo:6482 2021-02-13T00:30:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.94 , 105.22 , 79.53 , 114.68 2021-02-13T00:30:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.01 , -0.09 2021-02-13T00:30:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176248.27 epoch sec; seqNo:13295; 4059348.86 north; 592674.01 east 2021-02-13T00:30:48Z- AHRS_M2 msg: 1613176248.63 epoch sec; seqNo:13188 2021-02-13T00:30:48Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.20 psi 2021-02-13T00:30:48Z- handleDepth()-Depth_Keller msg: 1613176248634 epoch sec; seqNo:13296; depth 11.51 2021-02-13T00:30:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176248.64 epoch sec; seqNo:13296; 4059349.24 north; 592674.01 east 2021-02-13T00:30:49Z- AHRS_M2 msg: 1613176249.03 epoch sec; seqNo:13189 2021-02-13T00:30:49Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.19 psi 2021-02-13T00:30:49Z- handleDepth()-Depth_Keller msg: 1613176249036 epoch sec; seqNo:13297; depth 11.62 2021-02-13T00:30:49Z- handleDvl() - RDI_Pathfinder msg: 1613176249.06 epoch sec; seqNo:6483 2021-02-13T00:30:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.86 , 104.14 , 79.80 , 109.54 2021-02-13T00:30:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , -0.09 2021-02-13T00:30:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176249.07 epoch sec; seqNo:13297; 4059349.62 north; 592674.00 east 2021-02-13T00:30:49Z- LcmTrn::handleMessages() - handling 45 messages took 0.67 ms and 132 calls... 2021-02-13T00:30:49Z- waiting for 1613176246.81, time is 1613176249.07 2021-02-13T00:30:49Z- LcmTrn::updateTrn() equating pose 1613176249.07 and meas times 1613176249.07 2021-02-13T00:30:49Z- LcmTrn::updateTrn() >>>> pose time 1613176249.07, meas time 1613176249.07 <<<< 2021-02-13T00:30:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176249.07 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:30:49Z- TerrainNav::motionUpdate() - currEstimate = (4059350, 592674.00, 11.62) 2021-02-13T00:30:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:30:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:30:49Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176249.07 sec.; No relevant navigation data available 2021-02-13T00:30:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:30:49Z- TNavBF::mmse selected from filter -1 2021-02-13T00:30:49Z- variances([0], [2], and psi_berg) = 699.475147 832.997667 0.000000 2021-02-13T00:30:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:30:49Z- LcmTrn::publishEstimates() - MLE : 4059349.12 592704.50 11.62 2021-02-13T00:30:49Z- LcmTrn::publishEstimates() - MMSE : 4059349.62 592674.00 11.62 2021-02-13T00:30:49Z- LcmTrn::publishEstimates() - COVAR: 699.48 833.00 1.36 0.00 2021-02-13T00:30:49Z- AHRS_M2 msg: 1613176249.44 epoch sec; seqNo:13190 2021-02-13T00:30:49Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.19 psi 2021-02-13T00:30:49Z- handleDepth()-Depth_Keller msg: 1613176249440 epoch sec; seqNo:13298; depth 11.72 2021-02-13T00:30:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176249.45 epoch sec; seqNo:13298; 4059350.01 north; 592675.22 east 2021-02-13T00:30:49Z- AHRS_M2 msg: 1613176249.84 epoch sec; seqNo:13191 2021-02-13T00:30:49Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.18 psi 2021-02-13T00:30:49Z- handleDepth()-Depth_Keller msg: 1613176249844 epoch sec; seqNo:13299; depth 11.83 2021-02-13T00:30:49Z- handleDvl() - RDI_Pathfinder msg: 1613176249.86 epoch sec; seqNo:6484 2021-02-13T00:30:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 104.27 , 78.05 , 109.68 2021-02-13T00:30:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.00 , -0.04 2021-02-13T00:30:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176249.87 epoch sec; seqNo:13299; 4059350.39 north; 592675.21 east 2021-02-13T00:30:50Z- AHRS_M2 msg: 1613176250.24 epoch sec; seqNo:13192 2021-02-13T00:30:50Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.18 psi 2021-02-13T00:30:50Z- handleDepth()-Depth_Keller msg: 1613176250257 epoch sec; seqNo:13300; depth 11.93 2021-02-13T00:30:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176250.27 epoch sec; seqNo:13300; 4059350.39 north; 592675.21 east 2021-02-13T00:30:50Z- AHRS_M2 msg: 1613176250.65 epoch sec; seqNo:13193 2021-02-13T00:30:50Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.17 psi 2021-02-13T00:30:50Z- handleDepth()-Depth_Keller msg: 1613176250652 epoch sec; seqNo:13301; depth 12.04 2021-02-13T00:30:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176250.67 epoch sec; seqNo:13301; 4059350.77 north; 592675.21 east 2021-02-13T00:30:51Z- AHRS_M2 msg: 1613176251.05 epoch sec; seqNo:13194 2021-02-13T00:30:51Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.17 psi 2021-02-13T00:30:51Z- handleDepth()-Depth_Keller msg: 1613176251057 epoch sec; seqNo:13302; depth 12.14 2021-02-13T00:30:51Z- handleDvl() - RDI_Pathfinder msg: 1613176251.07 epoch sec; seqNo:6485 2021-02-13T00:30:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 101.63 , 76.83 , 112.45 2021-02-13T00:30:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.04 , -0.01 2021-02-13T00:30:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176251.09 epoch sec; seqNo:13302; 4059351.15 north; 592675.21 east 2021-02-13T00:30:51Z- AHRS_M2 msg: 1613176251.46 epoch sec; seqNo:13195 2021-02-13T00:30:51Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.17 psi 2021-02-13T00:30:51Z- handleDepth()-Depth_Keller msg: 1613176251469 epoch sec; seqNo:13303; depth 12.23 2021-02-13T00:30:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176251.48 epoch sec; seqNo:13303; 4059351.53 north; 592675.20 east 2021-02-13T00:30:51Z- AHRS_M2 msg: 1613176251.86 epoch sec; seqNo:13196 2021-02-13T00:30:51Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.17 psi 2021-02-13T00:30:51Z- handleDepth()-Depth_Keller msg: 1613176251866 epoch sec; seqNo:13304; depth 12.33 2021-02-13T00:30:51Z- handleDvl() - RDI_Pathfinder msg: 1613176251.90 epoch sec; seqNo:6486 2021-02-13T00:30:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 100.28 , 76.01 , 111.10 2021-02-13T00:30:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.02 , -0.03 2021-02-13T00:30:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176251.91 epoch sec; seqNo:13304; 4059351.54 north; 592676.42 east 2021-02-13T00:30:52Z- AHRS_M2 msg: 1613176252.26 epoch sec; seqNo:13197 2021-02-13T00:30:52Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.18 psi 2021-02-13T00:30:52Z- handleDepth()-Depth_Keller msg: 1613176252267 epoch sec; seqNo:13305; depth 12.45 2021-02-13T00:30:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176252.28 epoch sec; seqNo:13305; 4059351.92 north; 592676.42 east 2021-02-13T00:30:52Z- AHRS_M2 msg: 1613176252.67 epoch sec; seqNo:13198 2021-02-13T00:30:52Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.18 psi 2021-02-13T00:30:52Z- handleDepth()-Depth_Keller msg: 1613176252672 epoch sec; seqNo:13306; depth 12.54 2021-02-13T00:30:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176252.68 epoch sec; seqNo:13306; 4059352.30 north; 592676.41 east 2021-02-13T00:30:53Z- AHRS_M2 msg: 1613176253.07 epoch sec; seqNo:13199 2021-02-13T00:30:53Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.19 psi 2021-02-13T00:30:53Z- handleDepth()-Depth_Keller msg: 1613176253080 epoch sec; seqNo:13307; depth 12.64 2021-02-13T00:30:53Z- handleDvl() - RDI_Pathfinder msg: 1613176253.10 epoch sec; seqNo:6487 2021-02-13T00:30:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 101.70 , 76.41 , 109.81 2021-02-13T00:30:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , -0.04 2021-02-13T00:30:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176253.11 epoch sec; seqNo:13307; 4059352.68 north; 592676.41 east 2021-02-13T00:30:53Z- AHRS_M2 msg: 1613176253.47 epoch sec; seqNo:13200 2021-02-13T00:30:53Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.19 psi 2021-02-13T00:30:53Z- handleDepth()-Depth_Keller msg: 1613176253480 epoch sec; seqNo:13308; depth 12.75 2021-02-13T00:30:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176253.50 epoch sec; seqNo:13308; 4059352.68 north; 592676.41 east 2021-02-13T00:30:53Z- AHRS_M2 msg: 1613176253.88 epoch sec; seqNo:13201 2021-02-13T00:30:53Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.19 psi 2021-02-13T00:30:53Z- handleDepth()-Depth_Keller msg: 1613176253884 epoch sec; seqNo:13309; depth 12.86 2021-02-13T00:30:53Z- handleDvl() - RDI_Pathfinder msg: 1613176253.90 epoch sec; seqNo:6488 2021-02-13T00:30:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 101.70 , 75.18 , 108.46 2021-02-13T00:30:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.14 , -0.03 2021-02-13T00:30:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176253.91 epoch sec; seqNo:13309; 4059353.06 north; 592676.41 east 2021-02-13T00:30:54Z- LcmTrn::handleMessages() - handling 41 messages took 0.68 ms and 128 calls... 2021-02-13T00:30:54Z- waiting for 1613176252.07, time is 1613176253.91 2021-02-13T00:30:54Z- LcmTrn::updateTrn() equating pose 1613176253.91 and meas times 1613176253.91 2021-02-13T00:30:54Z- LcmTrn::updateTrn() >>>> pose time 1613176253.91, meas time 1613176253.91 <<<< 2021-02-13T00:30:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176253.91 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:30:54Z- TerrainNav::motionUpdate() - currEstimate = (4059353, 592676.41, 12.86) 2021-02-13T00:30:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:30:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:30:54Z- TNavBF::Projecting Measurements 2021-02-13T00:30:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:30:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:30:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:30:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:30:54Z- TNavFilter::4 good beams. 2021-02-13T00:30:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:30:54Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:30:54Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:30:54Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:30:54Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:30:54Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:30:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:30:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:30:54Z- TerrainMapOctree: (vehN, vehE ) = (4059353.06, 592676.41). 2021-02-13T00:30:54Z- TerrainMapOctree: Min Distance = 2464.67. 2021-02-13T00:30:54Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:30:54Z- There is no specific good beam that all particles have in common. 2021-02-13T00:30:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:30:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.57 K: 0.110 ExpectedMeasDiff: nan currMeas.covariance: 10.96 2021-02-13T00:30:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:30:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 16.55 2021-02-13T00:30:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:30:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 9.04 2021-02-13T00:30:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:30:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 18.82 2021-02-13T00:30:54Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:30:54Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:30:54Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:30:54Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:30:54Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:30:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:30:54Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:30:54Z- Filter 0 No beams used in subcloud update 2021-02-13T00:30:54Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:30:54Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:30:54Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 699.4 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:30:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176253.91 sec. 2021-02-13T00:30:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:30:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:30:54Z- variances([0], [2], and psi_berg) = 699.420036 832.997638 0.000000 2021-02-13T00:30:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:30:54Z- LcmTrn::publishEstimates() - MLE : 4059352.56 592706.91 12.86 2021-02-13T00:30:54Z- LcmTrn::publishEstimates() - MMSE : 4059353.06 592676.41 12.86 2021-02-13T00:30:54Z- LcmTrn::publishEstimates() - COVAR: 699.42 833.00 1.10 0.00 2021-02-13T00:30:54Z- AHRS_M2 msg: 1613176254.28 epoch sec; seqNo:13202 2021-02-13T00:30:54Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.18 psi 2021-02-13T00:30:54Z- handleDepth()-Depth_Keller msg: 1613176254288 epoch sec; seqNo:13310; depth 12.96 2021-02-13T00:30:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176254.30 epoch sec; seqNo:13310; 4059353.44 north; 592676.40 east 2021-02-13T00:30:54Z- AHRS_M2 msg: 1613176254.69 epoch sec; seqNo:13203 2021-02-13T00:30:54Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.18 psi 2021-02-13T00:30:54Z- handleDepth()-Depth_Keller msg: 1613176254703 epoch sec; seqNo:13311; depth 13.07 2021-02-13T00:30:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176254.72 epoch sec; seqNo:13311; 4059353.83 north; 592677.62 east 2021-02-13T00:30:55Z- AHRS_M2 msg: 1613176255.09 epoch sec; seqNo:13204 2021-02-13T00:30:55Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.17 psi 2021-02-13T00:30:55Z- handleDepth()-Depth_Keller msg: 1613176255096 epoch sec; seqNo:13312; depth 13.19 2021-02-13T00:30:55Z- handleDvl() - RDI_Pathfinder msg: 1613176255.12 epoch sec; seqNo:6489 2021-02-13T00:30:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 100.42 , 78.79 , 108.53 2021-02-13T00:30:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.17 , 0.02 2021-02-13T00:30:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176255.13 epoch sec; seqNo:13312; 4059354.21 north; 592677.61 east 2021-02-13T00:30:55Z- AHRS_M2 msg: 1613176255.51 epoch sec; seqNo:13205 2021-02-13T00:30:55Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.16 psi 2021-02-13T00:30:55Z- handleDepth()-Depth_Keller msg: 1613176255510 epoch sec; seqNo:13313; depth 13.28 2021-02-13T00:30:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176255.52 epoch sec; seqNo:13313; 4059354.59 north; 592677.61 east 2021-02-13T00:30:55Z- AHRS_M2 msg: 1613176255.90 epoch sec; seqNo:13206 2021-02-13T00:30:55Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.17 psi 2021-02-13T00:30:55Z- handleDepth()-Depth_Keller msg: 1613176255910 epoch sec; seqNo:13314; depth 13.40 2021-02-13T00:30:55Z- handleDvl() - RDI_Pathfinder msg: 1613176255.92 epoch sec; seqNo:6490 2021-02-13T00:30:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.13 , 99.00 , 75.19 , 109.81 2021-02-13T00:30:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.17 , 0.09 2021-02-13T00:30:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176255.93 epoch sec; seqNo:13314; 4059354.59 north; 592677.61 east 2021-02-13T00:30:56Z- AHRS_M2 msg: 1613176256.30 epoch sec; seqNo:13207 2021-02-13T00:30:56Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.17 psi 2021-02-13T00:30:56Z- handleDepth()-Depth_Keller msg: 1613176256308 epoch sec; seqNo:13315; depth 13.50 2021-02-13T00:30:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176256.33 epoch sec; seqNo:13315; 4059354.97 north; 592677.61 east 2021-02-13T00:30:56Z- AHRS_M2 msg: 1613176256.71 epoch sec; seqNo:13208 2021-02-13T00:30:56Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.17 psi 2021-02-13T00:30:56Z- handleDepth()-Depth_Keller msg: 1613176256713 epoch sec; seqNo:13316; depth 13.61 2021-02-13T00:30:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176256.73 epoch sec; seqNo:13316; 4059355.34 north; 592677.60 east 2021-02-13T00:30:57Z- AHRS_M2 msg: 1613176257.11 epoch sec; seqNo:13209 2021-02-13T00:30:57Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.17 psi 2021-02-13T00:30:57Z- handleDepth()-Depth_Keller msg: 1613176257118 epoch sec; seqNo:13317; depth 13.69 2021-02-13T00:30:57Z- handleDvl() - RDI_Pathfinder msg: 1613176257.13 epoch sec; seqNo:6491 2021-02-13T00:30:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 99.07 , 76.42 , 108.53 2021-02-13T00:30:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , 0.18 2021-02-13T00:30:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176257.15 epoch sec; seqNo:13317; 4059355.72 north; 592677.60 east 2021-02-13T00:30:57Z- AHRS_M2 msg: 1613176257.53 epoch sec; seqNo:13210 2021-02-13T00:30:57Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.17 psi 2021-02-13T00:30:57Z- handleDepth()-Depth_Keller msg: 1613176257529 epoch sec; seqNo:13318; depth 13.82 2021-02-13T00:30:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176257.54 epoch sec; seqNo:13318; 4059355.72 north; 592677.60 east 2021-02-13T00:30:57Z- AHRS_M2 msg: 1613176257.92 epoch sec; seqNo:13211 2021-02-13T00:30:57Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.17 psi 2021-02-13T00:30:57Z- handleDepth()-Depth_Keller msg: 1613176257929 epoch sec; seqNo:13319; depth 13.91 2021-02-13T00:30:57Z- handleDvl() - RDI_Pathfinder msg: 1613176257.95 epoch sec; seqNo:6492 2021-02-13T00:30:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:30:57Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:30:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176257.96 epoch sec; seqNo:13319; 4059356.10 north; 592677.59 east 2021-02-13T00:30:58Z- AHRS_M2 msg: 1613176258.32 epoch sec; seqNo:13212 2021-02-13T00:30:58Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-13T00:30:58Z- handleDepth()-Depth_Keller msg: 1613176258333 epoch sec; seqNo:13320; depth 14.03 2021-02-13T00:30:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176258.34 epoch sec; seqNo:13320; 4059356.49 north; 592678.81 east 2021-02-13T00:30:58Z- AHRS_M2 msg: 1613176258.73 epoch sec; seqNo:13213 2021-02-13T00:30:58Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-13T00:30:58Z- handleDepth()-Depth_Keller msg: 1613176258739 epoch sec; seqNo:13321; depth 14.13 2021-02-13T00:30:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176258.75 epoch sec; seqNo:13321; 4059356.87 north; 592678.81 east 2021-02-13T00:30:59Z- AHRS_M2 msg: 1613176259.14 epoch sec; seqNo:13214 2021-02-13T00:30:59Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.18 psi 2021-02-13T00:30:59Z- handleDepth()-Depth_Keller msg: 1613176259142 epoch sec; seqNo:13322; depth 14.24 2021-02-13T00:30:59Z- handleDvl() - RDI_Pathfinder msg: 1613176259.16 epoch sec; seqNo:6493 2021-02-13T00:30:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.37 , 95.75 , 73.05 , 109.14 2021-02-13T00:30:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.11 , 0.14 2021-02-13T00:30:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176259.17 epoch sec; seqNo:13322; 4059356.87 north; 592678.81 east 2021-02-13T00:30:59Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:30:59Z- waiting for 1613176256.91, time is 1613176259.17 2021-02-13T00:30:59Z- LcmTrn::updateTrn() equating pose 1613176259.17 and meas times 1613176259.17 2021-02-13T00:30:59Z- LcmTrn::updateTrn() >>>> pose time 1613176259.17, meas time 1613176259.17 <<<< 2021-02-13T00:30:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176259.17 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:30:59Z- TerrainNav::motionUpdate() - currEstimate = (4059357, 592678.81, 14.24) 2021-02-13T00:30:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:30:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:30:59Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176259.17 sec.; No relevant navigation data available 2021-02-13T00:30:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:30:59Z- TNavBF::mmse selected from filter -1 2021-02-13T00:30:59Z- variances([0], [2], and psi_berg) = 699.362936 832.997608 0.000000 2021-02-13T00:30:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:30:59Z- LcmTrn::publishEstimates() - MLE : 4059356.37 592709.31 14.24 2021-02-13T00:30:59Z- LcmTrn::publishEstimates() - MMSE : 4059356.87 592678.81 14.24 2021-02-13T00:30:59Z- LcmTrn::publishEstimates() - COVAR: 699.36 833.00 1.33 0.00 2021-02-13T00:30:59Z- AHRS_M2 msg: 1613176259.54 epoch sec; seqNo:13215 2021-02-13T00:30:59Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.18 psi 2021-02-13T00:30:59Z- handleDepth()-Depth_Keller msg: 1613176259545 epoch sec; seqNo:13323; depth 14.34 2021-02-13T00:30:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176259.57 epoch sec; seqNo:13323; 4059357.25 north; 592678.80 east 2021-02-13T00:30:59Z- AHRS_M2 msg: 1613176259.94 epoch sec; seqNo:13216 2021-02-13T00:30:59Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.18 psi 2021-02-13T00:30:59Z- handleDepth()-Depth_Keller msg: 1613176259945 epoch sec; seqNo:13324; depth 14.46 2021-02-13T00:30:59Z- handleDvl() - RDI_Pathfinder msg: 1613176259.96 epoch sec; seqNo:6494 2021-02-13T00:30:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 96.56 , 74.78 , 112.38 2021-02-13T00:30:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.08 , 0.01 2021-02-13T00:30:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176259.98 epoch sec; seqNo:13324; 4059357.63 north; 592678.80 east 2021-02-13T00:31:00Z- AHRS_M2 msg: 1613176260.34 epoch sec; seqNo:13217 2021-02-13T00:31:00Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.18 psi 2021-02-13T00:31:00Z- handleDepth()-Depth_Keller msg: 1613176260349 epoch sec; seqNo:13325; depth 14.56 2021-02-13T00:31:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176260.36 epoch sec; seqNo:13325; 4059358.02 north; 592680.01 east 2021-02-13T00:31:00Z- AHRS_M2 msg: 1613176260.75 epoch sec; seqNo:13218 2021-02-13T00:31:00Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.18 psi 2021-02-13T00:31:00Z- handleDepth()-Depth_Keller msg: 1613176260754 epoch sec; seqNo:13326; depth 14.68 2021-02-13T00:31:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176260.76 epoch sec; seqNo:13326; 4059358.02 north; 592680.01 east 2021-02-13T00:31:01Z- AHRS_M2 msg: 1613176261.15 epoch sec; seqNo:13219 2021-02-13T00:31:01Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.18 psi 2021-02-13T00:31:01Z- handleDepth()-Depth_Keller msg: 1613176261158 epoch sec; seqNo:13327; depth 14.78 2021-02-13T00:31:01Z- handleDvl() - RDI_Pathfinder msg: 1613176261.18 epoch sec; seqNo:6495 2021-02-13T00:31:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.83 , 96.43 , 73.56 , 108.60 2021-02-13T00:31:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , -0.09 2021-02-13T00:31:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176261.19 epoch sec; seqNo:13327; 4059358.40 north; 592680.01 east 2021-02-13T00:31:01Z- AHRS_M2 msg: 1613176261.56 epoch sec; seqNo:13220 2021-02-13T00:31:01Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.18 psi 2021-02-13T00:31:01Z- handleDepth()-Depth_Keller msg: 1613176261560 epoch sec; seqNo:13328; depth 14.91 2021-02-13T00:31:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176261.57 epoch sec; seqNo:13328; 4059358.78 north; 592680.01 east 2021-02-13T00:31:01Z- AHRS_M2 msg: 1613176261.96 epoch sec; seqNo:13221 2021-02-13T00:31:01Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.18 psi 2021-02-13T00:31:01Z- handleDepth()-Depth_Keller msg: 1613176261968 epoch sec; seqNo:13329; depth 15.00 2021-02-13T00:31:01Z- handleDvl() - RDI_Pathfinder msg: 1613176261.98 epoch sec; seqNo:6496 2021-02-13T00:31:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.28 , 99.00 , 74.37 , 108.73 2021-02-13T00:31:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , -0.11 2021-02-13T00:31:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176261.99 epoch sec; seqNo:13329; 4059359.16 north; 592680.00 east 2021-02-13T00:31:02Z- AHRS_M2 msg: 1613176262.36 epoch sec; seqNo:13222 2021-02-13T00:31:02Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.18 psi 2021-02-13T00:31:02Z- handleDepth()-Depth_Keller msg: 1613176262371 epoch sec; seqNo:13330; depth 15.10 2021-02-13T00:31:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176262.38 epoch sec; seqNo:13330; 4059359.54 north; 592680.00 east 2021-02-13T00:31:02Z- AHRS_M2 msg: 1613176262.77 epoch sec; seqNo:13223 2021-02-13T00:31:02Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.18 psi 2021-02-13T00:31:02Z- handleDepth()-Depth_Keller msg: 1613176262772 epoch sec; seqNo:13331; depth 15.22 2021-02-13T00:31:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176262.79 epoch sec; seqNo:13331; 4059359.55 north; 592681.22 east 2021-02-13T00:31:03Z- AHRS_M2 msg: 1613176263.17 epoch sec; seqNo:13224 2021-02-13T00:31:03Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.18 psi 2021-02-13T00:31:03Z- handleDepth()-Depth_Keller msg: 1613176263181 epoch sec; seqNo:13332; depth 15.32 2021-02-13T00:31:03Z- handleDvl() - RDI_Pathfinder msg: 1613176263.19 epoch sec; seqNo:6497 2021-02-13T00:31:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 96.50 , 78.25 , 108.66 2021-02-13T00:31:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , -0.08 2021-02-13T00:31:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176263.21 epoch sec; seqNo:13332; 4059359.93 north; 592681.22 east 2021-02-13T00:31:03Z- AHRS_M2 msg: 1613176263.58 epoch sec; seqNo:13225 2021-02-13T00:31:03Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.18 psi 2021-02-13T00:31:03Z- handleDepth()-Depth_Keller msg: 1613176263581 epoch sec; seqNo:13333; depth 15.44 2021-02-13T00:31:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176263.59 epoch sec; seqNo:13333; 4059360.31 north; 592681.21 east 2021-02-13T00:31:03Z- AHRS_M2 msg: 1613176263.98 epoch sec; seqNo:13226 2021-02-13T00:31:03Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.17 psi 2021-02-13T00:31:03Z- handleDepth()-Depth_Keller msg: 1613176263992 epoch sec; seqNo:13334; depth 15.54 2021-02-13T00:31:04Z- handleDvl() - RDI_Pathfinder msg: 1613176264.00 epoch sec; seqNo:6498 2021-02-13T00:31:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.86 , 73.15 , 108.60 2021-02-13T00:31:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.10 , -0.02 2021-02-13T00:31:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176264.01 epoch sec; seqNo:13334; 4059360.69 north; 592681.21 east 2021-02-13T00:31:04Z- AHRS_M2 msg: 1613176264.39 epoch sec; seqNo:13227 2021-02-13T00:31:04Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.17 psi 2021-02-13T00:31:04Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-13T00:31:04Z- waiting for 1613176262.17, time is 1613176264.39 2021-02-13T00:31:04Z- LcmTrn::updateTrn() >>>> pose time 1613176264.39, meas time 1613176264.00 <<<< 2021-02-13T00:31:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176264.00 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:31:04Z- TerrainNav::motionUpdate() - currEstimate = (4059361, 592681.21, 15.54) 2021-02-13T00:31:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:31:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:31:04Z- TNavBF::Projecting Measurements 2021-02-13T00:31:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:31:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:31:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:31:04Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:31:04Z- TNavFilter::3 good beams. 2021-02-13T00:31:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:31:04Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:31:04Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:31:04Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:31:04Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:31:04Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:31:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:31:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:31:04Z- TerrainMapOctree: (vehN, vehE ) = (4059360.41, 592681.03). 2021-02-13T00:31:04Z- TerrainMapOctree: Min Distance = 2456.06. 2021-02-13T00:31:04Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:31:04Z- There is no specific good beam that all particles have in common. 2021-02-13T00:31:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:31:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.54 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 15.64 2021-02-13T00:31:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:31:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.41 K: 0.121 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-13T00:31:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:31:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 18.87 2021-02-13T00:31:04Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:31:04Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:31:04Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:31:04Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:31:04Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:31:04Z- Filter 0 No beams used in subcloud update 2021-02-13T00:31:04Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:31:04Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:31:04Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 699.3 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:31:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176264.00 sec. 2021-02-13T00:31:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:31:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:31:04Z- variances([0], [2], and psi_berg) = 699.292545 832.997572 0.000000 2021-02-13T00:31:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:31:04Z- LcmTrn::publishEstimates() - MLE : 4059360.19 592711.71 15.54 2021-02-13T00:31:04Z- LcmTrn::publishEstimates() - MMSE : 4059360.69 592681.21 15.54 2021-02-13T00:31:04Z- LcmTrn::publishEstimates() - COVAR: 699.29 833.00 1.19 0.00 2021-02-13T00:31:04Z- handleDepth()-Depth_Keller msg: 1613176264393 epoch sec; seqNo:13335; depth 15.65 2021-02-13T00:31:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176264.40 epoch sec; seqNo:13335; 4059361.07 north; 592681.20 east 2021-02-13T00:31:04Z- AHRS_M2 msg: 1613176264.81 epoch sec; seqNo:13228 2021-02-13T00:31:04Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.17 psi 2021-02-13T00:31:04Z- handleDepth()-Depth_Keller msg: 1613176264811 epoch sec; seqNo:13336; depth 15.76 2021-02-13T00:31:04Z- handleDvl() - RDI_Pathfinder msg: 1613176264.82 epoch sec; seqNo:6499 2021-02-13T00:31:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.00 , 97.58 , 73.96 , 109.75 2021-02-13T00:31:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , 0.06 2021-02-13T00:31:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176264.85 epoch sec; seqNo:13336; 4059361.45 north; 592681.20 east 2021-02-13T00:31:05Z- AHRS_M2 msg: 1613176265.19 epoch sec; seqNo:13229 2021-02-13T00:31:05Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:05Z- handleDepth()-Depth_Keller msg: 1613176265198 epoch sec; seqNo:13337; depth 15.86 2021-02-13T00:31:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176265.21 epoch sec; seqNo:13337; 4059361.45 north; 592681.20 east 2021-02-13T00:31:05Z- AHRS_M2 msg: 1613176265.60 epoch sec; seqNo:13230 2021-02-13T00:31:05Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:05Z- handleDepth()-Depth_Keller msg: 1613176265600 epoch sec; seqNo:13338; depth 15.98 2021-02-13T00:31:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176265.62 epoch sec; seqNo:13338; 4059361.84 north; 592682.42 east 2021-02-13T00:31:06Z- AHRS_M2 msg: 1613176266.00 epoch sec; seqNo:13231 2021-02-13T00:31:06Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:06Z- handleDepth()-Depth_Keller msg: 1613176266009 epoch sec; seqNo:13339; depth 16.09 2021-02-13T00:31:06Z- handleDvl() - RDI_Pathfinder msg: 1613176266.02 epoch sec; seqNo:6500 2021-02-13T00:31:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 96.50 , 73.56 , 108.66 2021-02-13T00:31:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , 0.09 2021-02-13T00:31:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176266.04 epoch sec; seqNo:13339; 4059362.22 north; 592682.41 east 2021-02-13T00:31:06Z- AHRS_M2 msg: 1613176266.40 epoch sec; seqNo:13232 2021-02-13T00:31:06Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:06Z- handleDepth()-Depth_Keller msg: 1613176266408 epoch sec; seqNo:13340; depth 16.22 2021-02-13T00:31:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176266.42 epoch sec; seqNo:13340; 4059362.22 north; 592682.41 east 2021-02-13T00:31:06Z- AHRS_M2 msg: 1613176266.83 epoch sec; seqNo:13233 2021-02-13T00:31:06Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.17 psi 2021-02-13T00:31:06Z- handleDepth()-Depth_Keller msg: 1613176266837 epoch sec; seqNo:13341; depth 16.33 2021-02-13T00:31:06Z- handleDvl() - RDI_Pathfinder msg: 1613176266.85 epoch sec; seqNo:6501 2021-02-13T00:31:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.75 , 96.56 , 73.15 , 109.95 2021-02-13T00:31:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.00 , 0.06 2021-02-13T00:31:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176266.87 epoch sec; seqNo:13341; 4059362.59 north; 592682.41 east 2021-02-13T00:31:07Z- AHRS_M2 msg: 1613176267.21 epoch sec; seqNo:13234 2021-02-13T00:31:07Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.17 psi 2021-02-13T00:31:07Z- handleDepth()-Depth_Keller msg: 1613176267216 epoch sec; seqNo:13342; depth 16.45 2021-02-13T00:31:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176267.23 epoch sec; seqNo:13342; 4059362.97 north; 592682.40 east 2021-02-13T00:31:07Z- AHRS_M2 msg: 1613176267.62 epoch sec; seqNo:13235 2021-02-13T00:31:07Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.17 psi 2021-02-13T00:31:07Z- handleDepth()-Depth_Keller msg: 1613176267620 epoch sec; seqNo:13343; depth 16.54 2021-02-13T00:31:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176267.63 epoch sec; seqNo:13343; 4059363.35 north; 592682.40 east 2021-02-13T00:31:08Z- AHRS_M2 msg: 1613176268.02 epoch sec; seqNo:13236 2021-02-13T00:31:08Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.17 psi 2021-02-13T00:31:08Z- handleDepth()-Depth_Keller msg: 1613176268025 epoch sec; seqNo:13344; depth 16.68 2021-02-13T00:31:08Z- handleDvl() - RDI_Pathfinder msg: 1613176268.04 epoch sec; seqNo:6502 2021-02-13T00:31:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.75 , 96.56 , 71.92 , 106.30 2021-02-13T00:31:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.00 , -0.05 2021-02-13T00:31:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176268.05 epoch sec; seqNo:13344; 4059363.74 north; 592683.62 east 2021-02-13T00:31:08Z- AHRS_M2 msg: 1613176268.42 epoch sec; seqNo:13237 2021-02-13T00:31:08Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.17 psi 2021-02-13T00:31:08Z- handleDepth()-Depth_Keller msg: 1613176268431 epoch sec; seqNo:13345; depth 16.80 2021-02-13T00:31:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176268.44 epoch sec; seqNo:13345; 4059363.74 north; 592683.62 east 2021-02-13T00:31:08Z- AHRS_M2 msg: 1613176268.84 epoch sec; seqNo:13238 2021-02-13T00:31:08Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.17 psi 2021-02-13T00:31:08Z- handleDepth()-Depth_Keller msg: 1613176268847 epoch sec; seqNo:13346; depth 16.92 2021-02-13T00:31:08Z- handleDvl() - RDI_Pathfinder msg: 1613176268.86 epoch sec; seqNo:6503 2021-02-13T00:31:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.60 , 97.85 , 71.51 , 103.93 2021-02-13T00:31:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.14 , -0.13 2021-02-13T00:31:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176268.87 epoch sec; seqNo:13346; 4059364.12 north; 592683.61 east 2021-02-13T00:31:09Z- AHRS_M2 msg: 1613176269.23 epoch sec; seqNo:13239 2021-02-13T00:31:09Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:09Z- handleDepth()-Depth_Keller msg: 1613176269237 epoch sec; seqNo:13347; depth 17.05 2021-02-13T00:31:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176269.25 epoch sec; seqNo:13347; 4059364.50 north; 592683.61 east 2021-02-13T00:31:09Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 131 calls... 2021-02-13T00:31:09Z- waiting for 1613176267.39, time is 1613176269.25 2021-02-13T00:31:09Z- LcmTrn::updateTrn() >>>> pose time 1613176269.25, meas time 1613176268.86 <<<< 2021-02-13T00:31:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176268.86 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:31:09Z- TerrainNav::motionUpdate() - currEstimate = (4059365, 592683.61, 17.05) 2021-02-13T00:31:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:31:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:31:09Z- TNavBF::Projecting Measurements 2021-02-13T00:31:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:31:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:31:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:31:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:31:09Z- TNavFilter::4 good beams. 2021-02-13T00:31:09Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:31:09Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:31:09Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:31:09Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:31:09Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:31:09Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:31:09Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:31:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:31:09Z- TerrainMapOctree: (vehN, vehE ) = (4059364.20, 592683.41). 2021-02-13T00:31:09Z- TerrainMapOctree: Min Distance = 2451.62. 2021-02-13T00:31:09Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:31:09Z- There is no specific good beam that all particles have in common. 2021-02-13T00:31:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:31:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 10.14 2021-02-13T00:31:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:31:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-13T00:31:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:31:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 8.18 2021-02-13T00:31:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:31:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 17.28 2021-02-13T00:31:09Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:31:09Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:31:09Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:31:09Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:31:09Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:31:09Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:31:09Z- Filter 0 No beams used in subcloud update 2021-02-13T00:31:09Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:31:09Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:31:09Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 699.2 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:31:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176268.86 sec. 2021-02-13T00:31:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:31:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:31:09Z- variances([0], [2], and psi_berg) = 699.221715 832.997535 0.000000 2021-02-13T00:31:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:31:09Z- LcmTrn::publishEstimates() - MLE : 4059364.00 592714.11 17.05 2021-02-13T00:31:09Z- LcmTrn::publishEstimates() - MMSE : 4059364.50 592683.61 17.05 2021-02-13T00:31:09Z- LcmTrn::publishEstimates() - COVAR: 699.22 833.00 1.01 0.00 2021-02-13T00:31:09Z- AHRS_M2 msg: 1613176269.64 epoch sec; seqNo:13240 2021-02-13T00:31:09Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:09Z- handleDepth()-Depth_Keller msg: 1613176269641 epoch sec; seqNo:13348; depth 17.17 2021-02-13T00:31:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176269.65 epoch sec; seqNo:13348; 4059364.88 north; 592683.60 east 2021-02-13T00:31:10Z- AHRS_M2 msg: 1613176270.04 epoch sec; seqNo:13241 2021-02-13T00:31:10Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.19 psi 2021-02-13T00:31:10Z- handleDepth()-Depth_Keller msg: 1613176270054 epoch sec; seqNo:13349; depth 17.28 2021-02-13T00:31:10Z- handleDvl() - RDI_Pathfinder msg: 1613176270.07 epoch sec; seqNo:6504 2021-02-13T00:31:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.67 , 97.92 , 72.33 , 106.43 2021-02-13T00:31:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.22 , -0.07 2021-02-13T00:31:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176270.08 epoch sec; seqNo:13349; 4059365.26 north; 592683.60 east 2021-02-13T00:31:10Z- AHRS_M2 msg: 1613176270.44 epoch sec; seqNo:13242 2021-02-13T00:31:10Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.19 psi 2021-02-13T00:31:10Z- handleDepth()-Depth_Keller msg: 1613176270449 epoch sec; seqNo:13350; depth 17.41 2021-02-13T00:31:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176270.46 epoch sec; seqNo:13350; 4059365.64 north; 592683.60 east 2021-02-13T00:31:10Z- AHRS_M2 msg: 1613176270.85 epoch sec; seqNo:13243 2021-02-13T00:31:10Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.19 psi 2021-02-13T00:31:10Z- handleDepth()-Depth_Keller msg: 1613176270856 epoch sec; seqNo:13351; depth 17.52 2021-02-13T00:31:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176270.87 epoch sec; seqNo:13351; 4059366.03 north; 592684.81 east 2021-02-13T00:31:11Z- AHRS_M2 msg: 1613176271.25 epoch sec; seqNo:13244 2021-02-13T00:31:11Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.19 psi 2021-02-13T00:31:11Z- handleDepth()-Depth_Keller msg: 1613176271256 epoch sec; seqNo:13352; depth 17.65 2021-02-13T00:31:11Z- handleDvl() - RDI_Pathfinder msg: 1613176271.27 epoch sec; seqNo:6505 2021-02-13T00:31:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 96.63 , 78.38 , 107.58 2021-02-13T00:31:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.22 , 0.09 2021-02-13T00:31:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176271.29 epoch sec; seqNo:13352; 4059366.03 north; 592684.81 east 2021-02-13T00:31:11Z- AHRS_M2 msg: 1613176271.66 epoch sec; seqNo:13245 2021-02-13T00:31:11Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.19 psi 2021-02-13T00:31:11Z- handleDepth()-Depth_Keller msg: 1613176271660 epoch sec; seqNo:13353; depth 17.76 2021-02-13T00:31:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176271.67 epoch sec; seqNo:13353; 4059366.41 north; 592684.81 east 2021-02-13T00:31:12Z- AHRS_M2 msg: 1613176272.06 epoch sec; seqNo:13246 2021-02-13T00:31:12Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-13T00:31:12Z- handleDepth()-Depth_Keller msg: 1613176272069 epoch sec; seqNo:13354; depth 17.87 2021-02-13T00:31:12Z- handleDvl() - RDI_Pathfinder msg: 1613176272.09 epoch sec; seqNo:6506 2021-02-13T00:31:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.46 , 97.71 , 69.47 , 107.45 2021-02-13T00:31:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.09 , 0.23 2021-02-13T00:31:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176272.11 epoch sec; seqNo:13354; 4059366.79 north; 592684.80 east 2021-02-13T00:31:12Z- AHRS_M2 msg: 1613176272.46 epoch sec; seqNo:13247 2021-02-13T00:31:12Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-13T00:31:12Z- handleDepth()-Depth_Keller msg: 1613176272474 epoch sec; seqNo:13355; depth 17.99 2021-02-13T00:31:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176272.48 epoch sec; seqNo:13355; 4059366.79 north; 592684.80 east 2021-02-13T00:31:12Z- AHRS_M2 msg: 1613176272.87 epoch sec; seqNo:13248 2021-02-13T00:31:12Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:31:12Z- handleDepth()-Depth_Keller msg: 1613176272878 epoch sec; seqNo:13356; depth 18.10 2021-02-13T00:31:12Z- handleDvl() - RDI_Pathfinder msg: 1613176272.90 epoch sec; seqNo:6507 2021-02-13T00:31:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.67 , 97.92 , 69.88 , 107.65 2021-02-13T00:31:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.10 , 0.23 2021-02-13T00:31:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176272.91 epoch sec; seqNo:13356; 4059367.17 north; 592684.80 east 2021-02-13T00:31:13Z- AHRS_M2 msg: 1613176273.27 epoch sec; seqNo:13249 2021-02-13T00:31:13Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-13T00:31:13Z- handleDepth()-Depth_Keller msg: 1613176273277 epoch sec; seqNo:13357; depth 18.22 2021-02-13T00:31:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176273.29 epoch sec; seqNo:13357; 4059367.17 north; 592684.80 east 2021-02-13T00:31:13Z- AHRS_M2 msg: 1613176273.68 epoch sec; seqNo:13250 2021-02-13T00:31:13Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.20 psi 2021-02-13T00:31:13Z- handleDepth()-Depth_Keller msg: 1613176273680 epoch sec; seqNo:13358; depth 18.33 2021-02-13T00:31:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176273.69 epoch sec; seqNo:13358; 4059367.55 north; 592684.80 east 2021-02-13T00:31:14Z- AHRS_M2 msg: 1613176274.08 epoch sec; seqNo:13251 2021-02-13T00:31:14Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.20 psi 2021-02-13T00:31:14Z- handleDepth()-Depth_Keller msg: 1613176274084 epoch sec; seqNo:13359; depth 18.43 2021-02-13T00:31:14Z- handleDvl() - RDI_Pathfinder msg: 1613176274.10 epoch sec; seqNo:6508 2021-02-13T00:31:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.60 , 95.42 , 70.70 , 108.80 2021-02-13T00:31:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.19 , 0.09 2021-02-13T00:31:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176274.11 epoch sec; seqNo:13359; 4059367.94 north; 592686.01 east 2021-02-13T00:31:14Z- AHRS_M2 msg: 1613176274.48 epoch sec; seqNo:13252 2021-02-13T00:31:14Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.21 psi 2021-02-13T00:31:14Z- handleDepth()-Depth_Keller msg: 1613176274488 epoch sec; seqNo:13360; depth 18.55 2021-02-13T00:31:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176274.50 epoch sec; seqNo:13360; 4059367.94 north; 592686.01 east 2021-02-13T00:31:14Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:31:14Z- waiting for 1613176272.25, time is 1613176274.50 2021-02-13T00:31:14Z- LcmTrn::updateTrn() >>>> pose time 1613176274.50, meas time 1613176274.10 <<<< 2021-02-13T00:31:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176274.10 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:31:14Z- TerrainNav::motionUpdate() - currEstimate = (4059368, 592686.01, 18.55) 2021-02-13T00:31:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:31:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:31:14Z- TNavBF::Projecting Measurements 2021-02-13T00:31:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:31:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:31:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:31:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:31:14Z- TNavFilter::4 good beams. 2021-02-13T00:31:14Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:31:14Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:31:14Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:31:14Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:31:14Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:31:14Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:31:14Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:31:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:31:14Z- TerrainMapOctree: (vehN, vehE ) = (4059367.68, 592685.83). 2021-02-13T00:31:14Z- TerrainMapOctree: Min Distance = 2447.40. 2021-02-13T00:31:14Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:31:14Z- There is no specific good beam that all particles have in common. 2021-02-13T00:31:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:31:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 10.14 2021-02-13T00:31:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:31:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 14.57 2021-02-13T00:31:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:31:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-13T00:31:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:31:14Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 18.94 2021-02-13T00:31:14Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:31:14Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:31:14Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:31:14Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:31:14Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:31:14Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:31:14Z- Filter 0 No beams used in subcloud update 2021-02-13T00:31:14Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:31:14Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:31:14Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 699.2 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:31:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176274.10 sec. 2021-02-13T00:31:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:31:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:31:14Z- variances([0], [2], and psi_berg) = 699.153768 832.997499 0.000000 2021-02-13T00:31:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:31:14Z- LcmTrn::publishEstimates() - MLE : 4059367.44 592720.51 18.55 2021-02-13T00:31:14Z- LcmTrn::publishEstimates() - MMSE : 4059367.94 592686.01 18.55 2021-02-13T00:31:14Z- LcmTrn::publishEstimates() - COVAR: 699.15 833.00 0.90 0.00 2021-02-13T00:31:14Z- AHRS_M2 msg: 1613176274.89 epoch sec; seqNo:13253 2021-02-13T00:31:14Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.21 psi 2021-02-13T00:31:14Z- handleDepth()-Depth_Keller msg: 1613176274892 epoch sec; seqNo:13361; depth 18.67 2021-02-13T00:31:14Z- handleDvl() - RDI_Pathfinder msg: 1613176274.90 epoch sec; seqNo:6509 2021-02-13T00:31:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.42 , 96.63 , 69.06 , 110.02 2021-02-13T00:31:14Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.12 , -0.08 2021-02-13T00:31:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176274.91 epoch sec; seqNo:13361; 4059368.32 north; 592686.01 east 2021-02-13T00:31:15Z- AHRS_M2 msg: 1613176275.29 epoch sec; seqNo:13254 2021-02-13T00:31:15Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.22 psi 2021-02-13T00:31:15Z- handleDepth()-Depth_Keller msg: 1613176275305 epoch sec; seqNo:13362; depth 18.79 2021-02-13T00:31:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176275.32 epoch sec; seqNo:13362; 4059368.70 north; 592686.00 east 2021-02-13T00:31:15Z- AHRS_M2 msg: 1613176275.70 epoch sec; seqNo:13255 2021-02-13T00:31:15Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.22 psi 2021-02-13T00:31:15Z- handleDepth()-Depth_Keller msg: 1613176275703 epoch sec; seqNo:13363; depth 18.92 2021-02-13T00:31:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176275.71 epoch sec; seqNo:13363; 4059368.71 north; 592687.22 east 2021-02-13T00:31:16Z- AHRS_M2 msg: 1613176276.10 epoch sec; seqNo:13256 2021-02-13T00:31:16Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.22 psi 2021-02-13T00:31:16Z- handleDepth()-Depth_Keller msg: 1613176276104 epoch sec; seqNo:13364; depth 19.05 2021-02-13T00:31:16Z- handleDvl() - RDI_Pathfinder msg: 1613176276.12 epoch sec; seqNo:6510 2021-02-13T00:31:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.42 , 99.13 , 69.88 , 107.65 2021-02-13T00:31:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.04 , -0.14 2021-02-13T00:31:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176276.15 epoch sec; seqNo:13364; 4059369.09 north; 592687.22 east 2021-02-13T00:31:16Z- AHRS_M2 msg: 1613176276.51 epoch sec; seqNo:13257 2021-02-13T00:31:16Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.21 psi 2021-02-13T00:31:16Z- handleDepth()-Depth_Keller msg: 1613176276511 epoch sec; seqNo:13365; depth 19.17 2021-02-13T00:31:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176276.52 epoch sec; seqNo:13365; 4059369.47 north; 592687.22 east 2021-02-13T00:31:16Z- AHRS_M2 msg: 1613176276.93 epoch sec; seqNo:13258 2021-02-13T00:31:16Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.20 psi 2021-02-13T00:31:16Z- handleDepth()-Depth_Keller msg: 1613176276934 epoch sec; seqNo:13366; depth 19.29 2021-02-13T00:31:16Z- handleDvl() - RDI_Pathfinder msg: 1613176276.95 epoch sec; seqNo:6511 2021-02-13T00:31:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.60 , 97.92 , 70.29 , 108.87 2021-02-13T00:31:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.10 , -0.09 2021-02-13T00:31:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176276.96 epoch sec; seqNo:13366; 4059369.85 north; 592687.21 east 2021-02-13T00:31:17Z- AHRS_M2 msg: 1613176277.31 epoch sec; seqNo:13259 2021-02-13T00:31:17Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.20 psi 2021-02-13T00:31:17Z- handleDepth()-Depth_Keller msg: 1613176277316 epoch sec; seqNo:13367; depth 19.43 2021-02-13T00:31:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176277.33 epoch sec; seqNo:13367; 4059370.22 north; 592687.21 east 2021-02-13T00:31:17Z- AHRS_M2 msg: 1613176277.72 epoch sec; seqNo:13260 2021-02-13T00:31:17Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.20 psi 2021-02-13T00:31:17Z- handleDepth()-Depth_Keller msg: 1613176277720 epoch sec; seqNo:13368; depth 19.53 2021-02-13T00:31:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176277.73 epoch sec; seqNo:13368; 4059370.60 north; 592687.20 east 2021-02-13T00:31:18Z- AHRS_M2 msg: 1613176278.12 epoch sec; seqNo:13261 2021-02-13T00:31:18Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.19 psi 2021-02-13T00:31:18Z- handleDepth()-Depth_Keller msg: 1613176278129 epoch sec; seqNo:13369; depth 19.68 2021-02-13T00:31:18Z- handleDvl() - RDI_Pathfinder msg: 1613176278.14 epoch sec; seqNo:6512 2021-02-13T00:31:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.60 , 97.92 , 78.45 , 107.65 2021-02-13T00:31:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.12 , 0.00 2021-02-13T00:31:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176278.15 epoch sec; seqNo:13369; 4059370.62 north; 592688.42 east 2021-02-13T00:31:18Z- AHRS_M2 msg: 1613176278.52 epoch sec; seqNo:13262 2021-02-13T00:31:18Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.19 psi 2021-02-13T00:31:18Z- handleDepth()-Depth_Keller msg: 1613176278528 epoch sec; seqNo:13370; depth 19.79 2021-02-13T00:31:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176278.55 epoch sec; seqNo:13370; 4059371.00 north; 592688.42 east 2021-02-13T00:31:18Z- AHRS_M2 msg: 1613176278.93 epoch sec; seqNo:13263 2021-02-13T00:31:18Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.20 psi 2021-02-13T00:31:18Z- handleDepth()-Depth_Keller msg: 1613176278932 epoch sec; seqNo:13371; depth 19.91 2021-02-13T00:31:18Z- handleDvl() - RDI_Pathfinder msg: 1613176278.94 epoch sec; seqNo:6513 2021-02-13T00:31:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.19 , 97.78 , 0.00 , 109.95 2021-02-13T00:31:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , 0.06 2021-02-13T00:31:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176278.97 epoch sec; seqNo:13371; 4059371.37 north; 592688.42 east 2021-02-13T00:31:19Z- AHRS_M2 msg: 1613176279.33 epoch sec; seqNo:13264 2021-02-13T00:31:19Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.20 psi 2021-02-13T00:31:19Z- handleDepth()-Depth_Keller msg: 1613176279337 epoch sec; seqNo:13372; depth 20.05 2021-02-13T00:31:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176279.35 epoch sec; seqNo:13372; 4059371.75 north; 592688.41 east 2021-02-13T00:31:19Z- LcmTrn::handleMessages() - handling 41 messages took 0.56 ms and 130 calls... 2021-02-13T00:31:19Z- waiting for 1613176277.50, time is 1613176279.35 2021-02-13T00:31:19Z- LcmTrn::updateTrn() >>>> pose time 1613176279.35, meas time 1613176278.94 <<<< 2021-02-13T00:31:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176278.94 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:31:19Z- TerrainNav::motionUpdate() - currEstimate = (4059372, 592688.41, 20.05) 2021-02-13T00:31:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:31:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:31:19Z- TNavBF::Projecting Measurements 2021-02-13T00:31:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:31:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:31:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:31:19Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:31:19Z- TNavFilter::3 good beams. 2021-02-13T00:31:19Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:31:19Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:31:19Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:31:19Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:31:19Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:31:19Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:31:19Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:31:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:31:19Z- TerrainMapOctree: (vehN, vehE ) = (4059371.44, 592688.21). 2021-02-13T00:31:19Z- TerrainMapOctree: Min Distance = 2442.99. 2021-02-13T00:31:19Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:31:19Z- There is no specific good beam that all particles have in common. 2021-02-13T00:31:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:31:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 9.05 2021-02-13T00:31:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:31:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-13T00:31:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:31:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 19.34 2021-02-13T00:31:19Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:31:19Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:31:19Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:31:19Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:31:19Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:31:19Z- Filter 0 No beams used in subcloud update 2021-02-13T00:31:19Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:31:19Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:31:19Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 699.1 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:31:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176278.94 sec. 2021-02-13T00:31:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:31:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:31:19Z- variances([0], [2], and psi_berg) = 699.082843 832.997462 0.000000 2021-02-13T00:31:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:31:19Z- LcmTrn::publishEstimates() - MLE : 4059371.25 592722.91 20.05 2021-02-13T00:31:19Z- LcmTrn::publishEstimates() - MMSE : 4059371.75 592688.41 20.05 2021-02-13T00:31:19Z- LcmTrn::publishEstimates() - COVAR: 699.08 833.00 0.93 0.00 2021-02-13T00:31:19Z- AHRS_M2 msg: 1613176279.74 epoch sec; seqNo:13265 2021-02-13T00:31:19Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.20 psi 2021-02-13T00:31:19Z- handleDepth()-Depth_Keller msg: 1613176279746 epoch sec; seqNo:13373; depth 20.16 2021-02-13T00:31:19Z- handleDvl() - RDI_Pathfinder msg: 1613176279.76 epoch sec; seqNo:6514 2021-02-13T00:31:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.60 , 97.51 , 68.24 , 106.97 2021-02-13T00:31:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , 0.06 2021-02-13T00:31:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176279.77 epoch sec; seqNo:13373; 4059371.75 north; 592688.41 east 2021-02-13T00:31:20Z- AHRS_M2 msg: 1613176280.14 epoch sec; seqNo:13266 2021-02-13T00:31:20Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.21 psi 2021-02-13T00:31:20Z- handleDepth()-Depth_Keller msg: 1613176280149 epoch sec; seqNo:13374; depth 20.28 2021-02-13T00:31:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176280.17 epoch sec; seqNo:13374; 4059372.13 north; 592688.41 east 2021-02-13T00:31:20Z- AHRS_M2 msg: 1613176280.54 epoch sec; seqNo:13267 2021-02-13T00:31:20Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.22 psi 2021-02-13T00:31:20Z- handleDepth()-Depth_Keller msg: 1613176280548 epoch sec; seqNo:13375; depth 20.41 2021-02-13T00:31:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176280.56 epoch sec; seqNo:13375; 4059372.51 north; 592688.40 east 2021-02-13T00:31:20Z- AHRS_M2 msg: 1613176280.95 epoch sec; seqNo:13268 2021-02-13T00:31:20Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.22 psi 2021-02-13T00:31:20Z- handleDepth()-Depth_Keller msg: 1613176280952 epoch sec; seqNo:13376; depth 20.52 2021-02-13T00:31:20Z- handleDvl() - RDI_Pathfinder msg: 1613176280.96 epoch sec; seqNo:6515 2021-02-13T00:31:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.78 , 97.98 , 69.06 , 105.28 2021-02-13T00:31:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , 0.01 2021-02-13T00:31:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176280.97 epoch sec; seqNo:13376; 4059372.52 north; 592689.62 east 2021-02-13T00:31:21Z- AHRS_M2 msg: 1613176281.35 epoch sec; seqNo:13269 2021-02-13T00:31:21Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.22 psi 2021-02-13T00:31:21Z- handleDepth()-Depth_Keller msg: 1613176281358 epoch sec; seqNo:13377; depth 20.67 2021-02-13T00:31:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176281.37 epoch sec; seqNo:13377; 4059372.90 north; 592689.62 east 2021-02-13T00:31:21Z- AHRS_M2 msg: 1613176281.76 epoch sec; seqNo:13270 2021-02-13T00:31:21Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.23 psi 2021-02-13T00:31:21Z- handleDepth()-Depth_Keller msg: 1613176281761 epoch sec; seqNo:13378; depth 20.79 2021-02-13T00:31:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176281.79 epoch sec; seqNo:13378; 4059373.28 north; 592689.62 east 2021-02-13T00:31:22Z- AHRS_M2 msg: 1613176282.16 epoch sec; seqNo:13271 2021-02-13T00:31:22Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.23 psi 2021-02-13T00:31:22Z- handleDepth()-Depth_Keller msg: 1613176282168 epoch sec; seqNo:13379; depth 20.93 2021-02-13T00:31:22Z- handleDvl() - RDI_Pathfinder msg: 1613176282.18 epoch sec; seqNo:6516 2021-02-13T00:31:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 68.24 , 107.79 2021-02-13T00:31:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.03 , -0.05 2021-02-13T00:31:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176282.20 epoch sec; seqNo:13379; 4059373.66 north; 592689.61 east 2021-02-13T00:31:22Z- AHRS_M2 msg: 1613176282.56 epoch sec; seqNo:13272 2021-02-13T00:31:22Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.23 psi 2021-02-13T00:31:22Z- handleDepth()-Depth_Keller msg: 1613176282568 epoch sec; seqNo:13380; depth 21.05 2021-02-13T00:31:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176282.58 epoch sec; seqNo:13380; 4059373.66 north; 592689.61 east 2021-02-13T00:31:22Z- AHRS_M2 msg: 1613176282.97 epoch sec; seqNo:13273 2021-02-13T00:31:22Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.23 psi 2021-02-13T00:31:22Z- handleDepth()-Depth_Keller msg: 1613176282981 epoch sec; seqNo:13381; depth 21.18 2021-02-13T00:31:22Z- handleDvl() - RDI_Pathfinder msg: 1613176282.99 epoch sec; seqNo:6517 2021-02-13T00:31:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 97.92 , 69.06 , 107.65 2021-02-13T00:31:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.09 , -0.06 2021-02-13T00:31:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176283.00 epoch sec; seqNo:13381; 4059374.04 north; 592689.61 east 2021-02-13T00:31:23Z- AHRS_M2 msg: 1613176283.37 epoch sec; seqNo:13274 2021-02-13T00:31:23Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.23 psi 2021-02-13T00:31:23Z- handleDepth()-Depth_Keller msg: 1613176283386 epoch sec; seqNo:13382; depth 21.29 2021-02-13T00:31:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176283.40 epoch sec; seqNo:13382; 4059374.43 north; 592690.82 east 2021-02-13T00:31:23Z- AHRS_M2 msg: 1613176283.78 epoch sec; seqNo:13275 2021-02-13T00:31:23Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.23 psi 2021-02-13T00:31:23Z- handleDepth()-Depth_Keller msg: 1613176283784 epoch sec; seqNo:13383; depth 21.42 2021-02-13T00:31:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176283.82 epoch sec; seqNo:13383; 4059374.81 north; 592690.82 east 2021-02-13T00:31:24Z- AHRS_M2 msg: 1613176284.18 epoch sec; seqNo:13276 2021-02-13T00:31:24Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.22 psi 2021-02-13T00:31:24Z- handleDepth()-Depth_Keller msg: 1613176284184 epoch sec; seqNo:13384; depth 21.54 2021-02-13T00:31:24Z- handleDvl() - RDI_Pathfinder msg: 1613176284.20 epoch sec; seqNo:6518 2021-02-13T00:31:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 96.77 , 67.02 , 110.15 2021-02-13T00:31:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.12 , -0.01 2021-02-13T00:31:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176284.21 epoch sec; seqNo:13384; 4059375.19 north; 592690.82 east 2021-02-13T00:31:24Z- AHRS_M2 msg: 1613176284.59 epoch sec; seqNo:13277 2021-02-13T00:31:24Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.22 psi 2021-02-13T00:31:24Z- handleDepth()-Depth_Keller msg: 1613176284590 epoch sec; seqNo:13385; depth 21.68 2021-02-13T00:31:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176284.60 epoch sec; seqNo:13385; 4059375.57 north; 592690.81 east 2021-02-13T00:31:24Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-13T00:31:24Z- waiting for 1613176282.35, time is 1613176284.60 2021-02-13T00:31:24Z- LcmTrn::updateTrn() >>>> pose time 1613176284.60, meas time 1613176284.20 <<<< 2021-02-13T00:31:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176284.20 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:31:24Z- TerrainNav::motionUpdate() - currEstimate = (4059376, 592690.81, 21.68) 2021-02-13T00:31:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:31:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:31:24Z- TNavBF::Projecting Measurements 2021-02-13T00:31:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:31:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:31:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:31:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:31:24Z- TNavFilter::4 good beams. 2021-02-13T00:31:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:31:24Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:31:24Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:31:24Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:31:24Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:31:24Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:31:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:31:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:31:24Z- TerrainMapOctree: (vehN, vehE ) = (4059375.28, 592690.63). 2021-02-13T00:31:24Z- TerrainMapOctree: Min Distance = 2438.49. 2021-02-13T00:31:24Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:31:24Z- There is no specific good beam that all particles have in common. 2021-02-13T00:31:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:31:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:31:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:31:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 14.98 2021-02-13T00:31:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:31:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 7.19 2021-02-13T00:31:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:31:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 19.41 2021-02-13T00:31:24Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:31:24Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:31:24Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:31:24Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:31:24Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:31:24Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:31:24Z- Filter 0 No beams used in subcloud update 2021-02-13T00:31:24Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:31:24Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:31:24Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 699.0 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:31:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176284.20 sec. 2021-02-13T00:31:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:31:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:31:24Z- variances([0], [2], and psi_berg) = 699.012440 832.997426 0.000000 2021-02-13T00:31:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:31:24Z- LcmTrn::publishEstimates() - MLE : 4059375.07 592724.31 21.68 2021-02-13T00:31:24Z- LcmTrn::publishEstimates() - MMSE : 4059375.57 592690.81 21.68 2021-02-13T00:31:24Z- LcmTrn::publishEstimates() - COVAR: 699.01 833.00 0.87 0.00 2021-02-13T00:31:24Z- AHRS_M2 msg: 1613176284.99 epoch sec; seqNo:13278 2021-02-13T00:31:24Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.22 psi 2021-02-13T00:31:25Z- handleDepth()-Depth_Keller msg: 1613176285002 epoch sec; seqNo:13386; depth 21.81 2021-02-13T00:31:25Z- handleDvl() - RDI_Pathfinder msg: 1613176285.01 epoch sec; seqNo:6519 2021-02-13T00:31:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 97.92 , 68.65 , 105.22 2021-02-13T00:31:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.12 , 0.04 2021-02-13T00:31:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176285.02 epoch sec; seqNo:13386; 4059375.57 north; 592690.81 east 2021-02-13T00:31:25Z- AHRS_M2 msg: 1613176285.39 epoch sec; seqNo:13279 2021-02-13T00:31:25Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.21 psi 2021-02-13T00:31:25Z- handleDepth()-Depth_Keller msg: 1613176285396 epoch sec; seqNo:13387; depth 21.93 2021-02-13T00:31:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176285.41 epoch sec; seqNo:13387; 4059375.95 north; 592690.81 east 2021-02-13T00:31:25Z- AHRS_M2 msg: 1613176285.80 epoch sec; seqNo:13280 2021-02-13T00:31:25Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.21 psi 2021-02-13T00:31:25Z- handleDepth()-Depth_Keller msg: 1613176285804 epoch sec; seqNo:13388; depth 22.03 2021-02-13T00:31:25Z- handleDvl() - RDI_Pathfinder msg: 1613176285.82 epoch sec; seqNo:6520 2021-02-13T00:31:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 97.98 , 67.84 , 102.85 2021-02-13T00:31:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.07 , 0.08 2021-02-13T00:31:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176285.83 epoch sec; seqNo:13388; 4059376.33 north; 592690.80 east 2021-02-13T00:31:26Z- AHRS_M2 msg: 1613176286.20 epoch sec; seqNo:13281 2021-02-13T00:31:26Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.21 psi 2021-02-13T00:31:26Z- handleDepth()-Depth_Keller msg: 1613176286210 epoch sec; seqNo:13389; depth 22.16 2021-02-13T00:31:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176286.22 epoch sec; seqNo:13389; 4059376.33 north; 592690.80 east 2021-02-13T00:31:26Z- AHRS_M2 msg: 1613176286.60 epoch sec; seqNo:13282 2021-02-13T00:31:26Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.20 psi 2021-02-13T00:31:26Z- handleDepth()-Depth_Keller msg: 1613176286608 epoch sec; seqNo:13390; depth 22.28 2021-02-13T00:31:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176286.62 epoch sec; seqNo:13390; 4059376.72 north; 592692.02 east 2021-02-13T00:31:27Z- AHRS_M2 msg: 1613176287.01 epoch sec; seqNo:13283 2021-02-13T00:31:27Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.20 psi 2021-02-13T00:31:27Z- handleDepth()-Depth_Keller msg: 1613176287012 epoch sec; seqNo:13391; depth 22.39 2021-02-13T00:31:27Z- handleDvl() - RDI_Pathfinder msg: 1613176287.02 epoch sec; seqNo:6521 2021-02-13T00:31:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 98.19 , 66.61 , 103.05 2021-02-13T00:31:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.00 , 0.08 2021-02-13T00:31:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176287.03 epoch sec; seqNo:13391; 4059377.10 north; 592692.02 east 2021-02-13T00:31:27Z- AHRS_M2 msg: 1613176287.41 epoch sec; seqNo:13284 2021-02-13T00:31:27Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.19 psi 2021-02-13T00:31:27Z- handleDepth()-Depth_Keller msg: 1613176287425 epoch sec; seqNo:13392; depth 22.52 2021-02-13T00:31:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176287.44 epoch sec; seqNo:13392; 4059377.10 north; 592692.02 east 2021-02-13T00:31:27Z- AHRS_M2 msg: 1613176287.82 epoch sec; seqNo:13285 2021-02-13T00:31:27Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.19 psi 2021-02-13T00:31:27Z- handleDepth()-Depth_Keller msg: 1613176287830 epoch sec; seqNo:13393; depth 22.64 2021-02-13T00:31:27Z- handleDvl() - RDI_Pathfinder msg: 1613176287.84 epoch sec; seqNo:6522 2021-02-13T00:31:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 90.89 , 66.20 , 101.84 2021-02-13T00:31:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.05 , 0.05 2021-02-13T00:31:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176287.85 epoch sec; seqNo:13393; 4059377.48 north; 592692.01 east 2021-02-13T00:31:28Z- AHRS_M2 msg: 1613176288.22 epoch sec; seqNo:13286 2021-02-13T00:31:28Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.19 psi 2021-02-13T00:31:28Z- handleDepth()-Depth_Keller msg: 1613176288225 epoch sec; seqNo:13394; depth 22.75 2021-02-13T00:31:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176288.24 epoch sec; seqNo:13394; 4059377.85 north; 592692.01 east 2021-02-13T00:31:28Z- AHRS_M2 msg: 1613176288.62 epoch sec; seqNo:13287 2021-02-13T00:31:28Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.19 psi 2021-02-13T00:31:28Z- handleDepth()-Depth_Keller msg: 1613176288633 epoch sec; seqNo:13395; depth 22.88 2021-02-13T00:31:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176288.64 epoch sec; seqNo:13395; 4059377.85 north; 592692.01 east 2021-02-13T00:31:29Z- AHRS_M2 msg: 1613176289.03 epoch sec; seqNo:13288 2021-02-13T00:31:29Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:31:29Z- handleDepth()-Depth_Keller msg: 1613176289034 epoch sec; seqNo:13396; depth 22.99 2021-02-13T00:31:29Z- handleDvl() - RDI_Pathfinder msg: 1613176289.05 epoch sec; seqNo:6523 2021-02-13T00:31:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 143.41 , 66.61 , 102.04 2021-02-13T00:31:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.03 2021-02-13T00:31:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176289.06 epoch sec; seqNo:13396; 4059378.23 north; 592692.00 east 2021-02-13T00:31:29Z- AHRS_M2 msg: 1613176289.43 epoch sec; seqNo:13289 2021-02-13T00:31:29Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:31:29Z- handleDepth()-Depth_Keller msg: 1613176289445 epoch sec; seqNo:13397; depth 23.10 2021-02-13T00:31:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176289.46 epoch sec; seqNo:13397; 4059378.63 north; 592693.22 east 2021-02-13T00:31:29Z- AHRS_M2 msg: 1613176289.84 epoch sec; seqNo:13290 2021-02-13T00:31:29Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.18 psi 2021-02-13T00:31:29Z- handleDepth()-Depth_Keller msg: 1613176289846 epoch sec; seqNo:13398; depth 23.23 2021-02-13T00:31:29Z- handleDvl() - RDI_Pathfinder msg: 1613176289.86 epoch sec; seqNo:6524 2021-02-13T00:31:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 90.48 , 67.43 , 101.30 2021-02-13T00:31:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.02 2021-02-13T00:31:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176289.87 epoch sec; seqNo:13398; 4059379.00 north; 592693.22 east 2021-02-13T00:31:29Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-13T00:31:29Z- waiting for 1613176287.60, time is 1613176289.87 2021-02-13T00:31:29Z- LcmTrn::updateTrn() equating pose 1613176289.87 and meas times 1613176289.87 2021-02-13T00:31:29Z- LcmTrn::updateTrn() >>>> pose time 1613176289.87, meas time 1613176289.87 <<<< 2021-02-13T00:31:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176289.87 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:31:29Z- TerrainNav::motionUpdate() - currEstimate = (4059379, 592693.22, 23.23) 2021-02-13T00:31:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:31:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:31:29Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176289.87 sec.; No relevant navigation data available 2021-02-13T00:31:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:31:29Z- TNavBF::mmse selected from filter -1 2021-02-13T00:31:29Z- variances([0], [2], and psi_berg) = 698.957601 832.997397 0.000000 2021-02-13T00:31:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:31:29Z- LcmTrn::publishEstimates() - MLE : 4059378.50 592726.72 23.23 2021-02-13T00:31:29Z- LcmTrn::publishEstimates() - MMSE : 4059379.00 592693.22 23.23 2021-02-13T00:31:29Z- LcmTrn::publishEstimates() - COVAR: 698.96 833.00 1.08 0.00 2021-02-13T00:31:30Z- AHRS_M2 msg: 1613176290.25 epoch sec; seqNo:13291 2021-02-13T00:31:30Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-13T00:31:30Z- handleDepth()-Depth_Keller msg: 1613176290250 epoch sec; seqNo:13399; depth 23.34 2021-02-13T00:31:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176290.26 epoch sec; seqNo:13399; 4059379.00 north; 592693.22 east 2021-02-13T00:31:30Z- AHRS_M2 msg: 1613176290.67 epoch sec; seqNo:13292 2021-02-13T00:31:30Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-13T00:31:30Z- handleDepth()-Depth_Keller msg: 1613176290682 epoch sec; seqNo:13400; depth 23.45 2021-02-13T00:31:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176290.71 epoch sec; seqNo:13400; 4059379.38 north; 592693.21 east 2021-02-13T00:31:31Z- AHRS_M2 msg: 1613176291.05 epoch sec; seqNo:13293 2021-02-13T00:31:31Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-13T00:31:31Z- handleDepth()-Depth_Keller msg: 1613176291056 epoch sec; seqNo:13401; depth 23.58 2021-02-13T00:31:31Z- handleDvl() - RDI_Pathfinder msg: 1613176291.07 epoch sec; seqNo:6525 2021-02-13T00:31:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.74 , 89.87 , 67.02 , 100.82 2021-02-13T00:31:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.01 2021-02-13T00:31:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176291.08 epoch sec; seqNo:13401; 4059379.76 north; 592693.21 east 2021-02-13T00:31:31Z- AHRS_M2 msg: 1613176291.46 epoch sec; seqNo:13294 2021-02-13T00:31:31Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-13T00:31:31Z- handleDepth()-Depth_Keller msg: 1613176291469 epoch sec; seqNo:13402; depth 23.69 2021-02-13T00:31:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176291.50 epoch sec; seqNo:13402; 4059380.14 north; 592693.20 east 2021-02-13T00:31:31Z- AHRS_M2 msg: 1613176291.86 epoch sec; seqNo:13295 2021-02-13T00:31:31Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-13T00:31:31Z- handleDepth()-Depth_Keller msg: 1613176291869 epoch sec; seqNo:13403; depth 23.80 2021-02-13T00:31:31Z- handleDvl() - RDI_Pathfinder msg: 1613176291.88 epoch sec; seqNo:6526 2021-02-13T00:31:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.74 , 0.00 , 78.92 , 97.17 2021-02-13T00:31:31Z- handleDvl() - RDI_Pathfinder msg: velocities 1.36 , 0.34 , -0.21 2021-02-13T00:31:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176291.89 epoch sec; seqNo:13403; 4059380.53 north; 592694.42 east 2021-02-13T00:31:32Z- AHRS_M2 msg: 1613176292.26 epoch sec; seqNo:13296 2021-02-13T00:31:32Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-13T00:31:32Z- handleDepth()-Depth_Keller msg: 1613176292270 epoch sec; seqNo:13404; depth 23.92 2021-02-13T00:31:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176292.28 epoch sec; seqNo:13404; 4059380.91 north; 592694.42 east 2021-02-13T00:31:32Z- AHRS_M2 msg: 1613176292.67 epoch sec; seqNo:13297 2021-02-13T00:31:32Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-13T00:31:32Z- handleDepth()-Depth_Keller msg: 1613176292672 epoch sec; seqNo:13405; depth 24.04 2021-02-13T00:31:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176292.70 epoch sec; seqNo:13405; 4059381.29 north; 592694.41 east 2021-02-13T00:31:33Z- AHRS_M2 msg: 1613176293.07 epoch sec; seqNo:13298 2021-02-13T00:31:33Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-13T00:31:33Z- handleDepth()-Depth_Keller msg: 1613176293076 epoch sec; seqNo:13406; depth 24.14 2021-02-13T00:31:33Z- handleDvl() - RDI_Pathfinder msg: 1613176293.09 epoch sec; seqNo:6527 2021-02-13T00:31:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:31:33Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:31:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176293.11 epoch sec; seqNo:13406; 4059381.68 north; 592695.63 east 2021-02-13T00:31:33Z- AHRS_M2 msg: 1613176293.47 epoch sec; seqNo:13299 2021-02-13T00:31:33Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-13T00:31:33Z- handleDepth()-Depth_Keller msg: 1613176293480 epoch sec; seqNo:13407; depth 24.26 2021-02-13T00:31:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176293.49 epoch sec; seqNo:13407; 4059382.06 north; 592695.63 east 2021-02-13T00:31:33Z- AHRS_M2 msg: 1613176293.88 epoch sec; seqNo:13300 2021-02-13T00:31:33Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-13T00:31:33Z- handleDepth()-Depth_Keller msg: 1613176293887 epoch sec; seqNo:13408; depth 24.37 2021-02-13T00:31:33Z- handleDvl() - RDI_Pathfinder msg: 1613176293.90 epoch sec; seqNo:6528 2021-02-13T00:31:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 88.11 , 65.79 , 97.85 2021-02-13T00:31:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , 0.05 2021-02-13T00:31:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176293.91 epoch sec; seqNo:13408; 4059382.06 north; 592695.63 east 2021-02-13T00:31:34Z- AHRS_M2 msg: 1613176294.28 epoch sec; seqNo:13301 2021-02-13T00:31:34Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-13T00:31:34Z- handleDepth()-Depth_Keller msg: 1613176294293 epoch sec; seqNo:13409; depth 24.49 2021-02-13T00:31:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176294.30 epoch sec; seqNo:13409; 4059382.44 north; 592695.62 east 2021-02-13T00:31:34Z- AHRS_M2 msg: 1613176294.69 epoch sec; seqNo:13302 2021-02-13T00:31:34Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.17 psi 2021-02-13T00:31:34Z- handleDepth()-Depth_Keller msg: 1613176294699 epoch sec; seqNo:13410; depth 24.59 2021-02-13T00:31:34Z- handleDvl() - RDI_Pathfinder msg: 1613176294.71 epoch sec; seqNo:6529 2021-02-13T00:31:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 0.00 , 65.79 , 97.92 2021-02-13T00:31:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , 0.03 2021-02-13T00:31:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176294.72 epoch sec; seqNo:13410; 4059382.83 north; 592696.84 east 2021-02-13T00:31:34Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-13T00:31:34Z- waiting for 1613176292.87, time is 1613176294.72 2021-02-13T00:31:34Z- LcmTrn::updateTrn() equating pose 1613176294.72 and meas times 1613176294.72 2021-02-13T00:31:34Z- LcmTrn::updateTrn() >>>> pose time 1613176294.72, meas time 1613176294.72 <<<< 2021-02-13T00:31:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176294.72 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:31:34Z- TerrainNav::motionUpdate() - currEstimate = (4059383, 592696.84, 24.59) 2021-02-13T00:31:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:31:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:31:34Z- TNavBF::Projecting Measurements 2021-02-13T00:31:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:31:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:31:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:31:34Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:31:34Z- TNavFilter::3 good beams. 2021-02-13T00:31:34Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:31:34Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:31:34Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:31:34Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:31:34Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:31:34Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:31:34Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:31:34Z- TNavBF::mmse selected from filter -1 2021-02-13T00:31:34Z- TerrainMapOctree: (vehN, vehE ) = (4059382.83, 592696.84). 2021-02-13T00:31:34Z- TerrainMapOctree: Min Distance = 2428.71. 2021-02-13T00:31:34Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:31:34Z- There is no specific good beam that all particles have in common. 2021-02-13T00:31:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:31:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.35 K: 0.121 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-13T00:31:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:31:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.120 ExpectedMeasDiff: nan currMeas.covariance: 6.93 2021-02-13T00:31:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:31:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:31:34Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:31:34Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:31:34Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:31:34Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:31:34Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:31:34Z- Filter 0 No beams used in subcloud update 2021-02-13T00:31:34Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:31:34Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:31:34Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 698.9 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:31:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176294.72 sec. 2021-02-13T00:31:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:31:34Z- TNavBF::mmse selected from filter -1 2021-02-13T00:31:34Z- variances([0], [2], and psi_berg) = 698.896165 832.997365 0.000000 2021-02-13T00:31:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:31:34Z- LcmTrn::publishEstimates() - MLE : 4059382.33 592730.34 24.59 2021-02-13T00:31:34Z- LcmTrn::publishEstimates() - MMSE : 4059382.83 592696.84 24.59 2021-02-13T00:31:34Z- LcmTrn::publishEstimates() - COVAR: 698.90 833.00 0.98 0.00 2021-02-13T00:31:35Z- AHRS_M2 msg: 1613176295.09 epoch sec; seqNo:13303 2021-02-13T00:31:35Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.17 psi 2021-02-13T00:31:35Z- handleDepth()-Depth_Keller msg: 1613176295096 epoch sec; seqNo:13411; depth 24.73 2021-02-13T00:31:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176295.12 epoch sec; seqNo:13411; 4059383.21 north; 592696.83 east 2021-02-13T00:31:35Z- AHRS_M2 msg: 1613176295.50 epoch sec; seqNo:13304 2021-02-13T00:31:35Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:35Z- handleDepth()-Depth_Keller msg: 1613176295505 epoch sec; seqNo:13412; depth 24.82 2021-02-13T00:31:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176295.51 epoch sec; seqNo:13412; 4059383.21 north; 592696.83 east 2021-02-13T00:31:35Z- AHRS_M2 msg: 1613176295.90 epoch sec; seqNo:13305 2021-02-13T00:31:35Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:35Z- handleDepth()-Depth_Keller msg: 1613176295909 epoch sec; seqNo:13413; depth 24.92 2021-02-13T00:31:35Z- handleDvl() - RDI_Pathfinder msg: 1613176295.92 epoch sec; seqNo:6530 2021-02-13T00:31:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 87.17 , 64.97 , 97.98 2021-02-13T00:31:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.01 2021-02-13T00:31:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176295.93 epoch sec; seqNo:13413; 4059383.59 north; 592696.83 east 2021-02-13T00:31:36Z- AHRS_M2 msg: 1613176296.30 epoch sec; seqNo:13306 2021-02-13T00:31:36Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:36Z- handleDepth()-Depth_Keller msg: 1613176296308 epoch sec; seqNo:13414; depth 25.06 2021-02-13T00:31:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176296.33 epoch sec; seqNo:13414; 4059383.97 north; 592696.83 east 2021-02-13T00:31:36Z- AHRS_M2 msg: 1613176296.71 epoch sec; seqNo:13307 2021-02-13T00:31:36Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:36Z- handleDepth()-Depth_Keller msg: 1613176296717 epoch sec; seqNo:13415; depth 25.16 2021-02-13T00:31:36Z- handleDvl() - RDI_Pathfinder msg: 1613176296.73 epoch sec; seqNo:6531 2021-02-13T00:31:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 0.00 , 64.97 , 97.78 2021-02-13T00:31:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , -0.02 2021-02-13T00:31:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176296.74 epoch sec; seqNo:13415; 4059384.35 north; 592696.82 east 2021-02-13T00:31:37Z- AHRS_M2 msg: 1613176297.11 epoch sec; seqNo:13308 2021-02-13T00:31:37Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:37Z- handleDepth()-Depth_Keller msg: 1613176297119 epoch sec; seqNo:13416; depth 25.27 2021-02-13T00:31:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176297.13 epoch sec; seqNo:13416; 4059384.36 north; 592698.04 east 2021-02-13T00:31:37Z- AHRS_M2 msg: 1613176297.53 epoch sec; seqNo:13309 2021-02-13T00:31:37Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:37Z- handleDepth()-Depth_Keller msg: 1613176297529 epoch sec; seqNo:13417; depth 25.36 2021-02-13T00:31:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176297.54 epoch sec; seqNo:13417; 4059384.74 north; 592698.04 east 2021-02-13T00:31:37Z- AHRS_M2 msg: 1613176297.92 epoch sec; seqNo:13310 2021-02-13T00:31:37Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:37Z- handleDepth()-Depth_Keller msg: 1613176297929 epoch sec; seqNo:13418; depth 25.47 2021-02-13T00:31:37Z- handleDvl() - RDI_Pathfinder msg: 1613176297.95 epoch sec; seqNo:6532 2021-02-13T00:31:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 88.25 , 63.75 , 97.98 2021-02-13T00:31:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , -0.03 2021-02-13T00:31:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176297.96 epoch sec; seqNo:13418; 4059385.12 north; 592698.03 east 2021-02-13T00:31:38Z- AHRS_M2 msg: 1613176298.32 epoch sec; seqNo:13311 2021-02-13T00:31:38Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.19 psi 2021-02-13T00:31:38Z- handleDepth()-Depth_Keller msg: 1613176298328 epoch sec; seqNo:13419; depth 25.58 2021-02-13T00:31:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176298.34 epoch sec; seqNo:13419; 4059385.50 north; 592698.03 east 2021-02-13T00:31:38Z- AHRS_M2 msg: 1613176298.74 epoch sec; seqNo:13312 2021-02-13T00:31:38Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.19 psi 2021-02-13T00:31:38Z- handleDepth()-Depth_Keller msg: 1613176298744 epoch sec; seqNo:13420; depth 25.70 2021-02-13T00:31:38Z- handleDvl() - RDI_Pathfinder msg: 1613176298.76 epoch sec; seqNo:6533 2021-02-13T00:31:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 87.10 , 64.16 , 97.92 2021-02-13T00:31:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.11 , -0.03 2021-02-13T00:31:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176298.77 epoch sec; seqNo:13420; 4059385.88 north; 592698.03 east 2021-02-13T00:31:39Z- AHRS_M2 msg: 1613176299.13 epoch sec; seqNo:13313 2021-02-13T00:31:39Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.19 psi 2021-02-13T00:31:39Z- handleDepth()-Depth_Keller msg: 1613176299147 epoch sec; seqNo:13421; depth 25.82 2021-02-13T00:31:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176299.16 epoch sec; seqNo:13421; 4059386.25 north; 592698.02 east 2021-02-13T00:31:39Z- AHRS_M2 msg: 1613176299.54 epoch sec; seqNo:13314 2021-02-13T00:31:39Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.19 psi 2021-02-13T00:31:39Z- handleDepth()-Depth_Keller msg: 1613176299545 epoch sec; seqNo:13422; depth 25.93 2021-02-13T00:31:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176299.55 epoch sec; seqNo:13422; 4059386.25 north; 592698.02 east 2021-02-13T00:31:39Z- AHRS_M2 msg: 1613176299.94 epoch sec; seqNo:13315 2021-02-13T00:31:39Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.19 psi 2021-02-13T00:31:39Z- handleDepth()-Depth_Keller msg: 1613176299945 epoch sec; seqNo:13423; depth 26.03 2021-02-13T00:31:39Z- handleDvl() - RDI_Pathfinder msg: 1613176299.96 epoch sec; seqNo:6534 2021-02-13T00:31:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 0.00 , 64.16 , 97.98 2021-02-13T00:31:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.18 , 0.01 2021-02-13T00:31:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176299.97 epoch sec; seqNo:13423; 4059386.65 north; 592699.24 east 2021-02-13T00:31:40Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-13T00:31:40Z- waiting for 1613176297.72, time is 1613176299.97 2021-02-13T00:31:40Z- LcmTrn::updateTrn() equating pose 1613176299.97 and meas times 1613176299.97 2021-02-13T00:31:40Z- LcmTrn::updateTrn() >>>> pose time 1613176299.97, meas time 1613176299.97 <<<< 2021-02-13T00:31:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176299.97 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:31:40Z- TerrainNav::motionUpdate() - currEstimate = (4059387, 592699.24, 26.03) 2021-02-13T00:31:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:31:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:31:40Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176299.97 sec.; No relevant navigation data available 2021-02-13T00:31:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:31:40Z- TNavBF::mmse selected from filter -1 2021-02-13T00:31:40Z- variances([0], [2], and psi_berg) = 698.839384 832.997336 0.000000 2021-02-13T00:31:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:31:40Z- LcmTrn::publishEstimates() - MLE : 4059386.15 592670.74 26.03 2021-02-13T00:31:40Z- LcmTrn::publishEstimates() - MMSE : 4059386.65 592699.24 26.03 2021-02-13T00:31:40Z- LcmTrn::publishEstimates() - COVAR: 698.84 833.00 1.21 0.00 2021-02-13T00:31:40Z- AHRS_M2 msg: 1613176300.35 epoch sec; seqNo:13316 2021-02-13T00:31:40Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.19 psi 2021-02-13T00:31:40Z- handleDepth()-Depth_Keller msg: 1613176300350 epoch sec; seqNo:13424; depth 26.15 2021-02-13T00:31:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176300.36 epoch sec; seqNo:13424; 4059387.03 north; 592699.23 east 2021-02-13T00:31:40Z- AHRS_M2 msg: 1613176300.75 epoch sec; seqNo:13317 2021-02-13T00:31:40Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.19 psi 2021-02-13T00:31:40Z- handleDepth()-Depth_Keller msg: 1613176300762 epoch sec; seqNo:13425; depth 26.27 2021-02-13T00:31:40Z- handleDvl() - RDI_Pathfinder msg: 1613176300.77 epoch sec; seqNo:6535 2021-02-13T00:31:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.66 , 86.01 , 63.35 , 97.91 2021-02-13T00:31:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.11 , 0.05 2021-02-13T00:31:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176300.78 epoch sec; seqNo:13425; 4059387.40 north; 592699.23 east 2021-02-13T00:31:41Z- AHRS_M2 msg: 1613176301.15 epoch sec; seqNo:13318 2021-02-13T00:31:41Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:41Z- handleDepth()-Depth_Keller msg: 1613176301165 epoch sec; seqNo:13426; depth 26.38 2021-02-13T00:31:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176301.18 epoch sec; seqNo:13426; 4059387.78 north; 592699.23 east 2021-02-13T00:31:41Z- AHRS_M2 msg: 1613176301.56 epoch sec; seqNo:13319 2021-02-13T00:31:41Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:41Z- handleDepth()-Depth_Keller msg: 1613176301563 epoch sec; seqNo:13427; depth 26.48 2021-02-13T00:31:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176301.57 epoch sec; seqNo:13427; 4059387.78 north; 592699.23 east 2021-02-13T00:31:41Z- AHRS_M2 msg: 1613176301.96 epoch sec; seqNo:13320 2021-02-13T00:31:41Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:41Z- handleDepth()-Depth_Keller msg: 1613176301965 epoch sec; seqNo:13428; depth 26.61 2021-02-13T00:31:41Z- handleDvl() - RDI_Pathfinder msg: 1613176301.99 epoch sec; seqNo:6536 2021-02-13T00:31:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 0.00 , 63.76 , 97.98 2021-02-13T00:31:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , 0.09 2021-02-13T00:31:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176302.00 epoch sec; seqNo:13428; 4059388.16 north; 592699.22 east 2021-02-13T00:31:42Z- AHRS_M2 msg: 1613176302.36 epoch sec; seqNo:13321 2021-02-13T00:31:42Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:42Z- handleDepth()-Depth_Keller msg: 1613176302368 epoch sec; seqNo:13429; depth 26.69 2021-02-13T00:31:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176302.38 epoch sec; seqNo:13429; 4059388.54 north; 592699.22 east 2021-02-13T00:31:42Z- AHRS_M2 msg: 1613176302.77 epoch sec; seqNo:13322 2021-02-13T00:31:42Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:42Z- handleDepth()-Depth_Keller msg: 1613176302773 epoch sec; seqNo:13430; depth 26.80 2021-02-13T00:31:42Z- handleDvl() - RDI_Pathfinder msg: 1613176302.78 epoch sec; seqNo:6537 2021-02-13T00:31:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 85.00 , 62.93 , 97.98 2021-02-13T00:31:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , 0.10 2021-02-13T00:31:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176302.79 epoch sec; seqNo:13430; 4059388.54 north; 592699.22 east 2021-02-13T00:31:43Z- AHRS_M2 msg: 1613176303.17 epoch sec; seqNo:13323 2021-02-13T00:31:43Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:43Z- handleDepth()-Depth_Keller msg: 1613176303185 epoch sec; seqNo:13431; depth 26.91 2021-02-13T00:31:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176303.20 epoch sec; seqNo:13431; 4059388.93 north; 592700.43 east 2021-02-13T00:31:43Z- AHRS_M2 msg: 1613176303.58 epoch sec; seqNo:13324 2021-02-13T00:31:43Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:43Z- handleDepth()-Depth_Keller msg: 1613176303580 epoch sec; seqNo:13432; depth 27.02 2021-02-13T00:31:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176303.60 epoch sec; seqNo:13432; 4059389.31 north; 592700.43 east 2021-02-13T00:31:43Z- AHRS_M2 msg: 1613176303.98 epoch sec; seqNo:13325 2021-02-13T00:31:43Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:43Z- handleDepth()-Depth_Keller msg: 1613176303989 epoch sec; seqNo:13433; depth 27.15 2021-02-13T00:31:44Z- handleDvl() - RDI_Pathfinder msg: 1613176304.01 epoch sec; seqNo:6538 2021-02-13T00:31:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 86.09 , 62.52 , 97.98 2021-02-13T00:31:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , 0.07 2021-02-13T00:31:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176304.02 epoch sec; seqNo:13433; 4059389.31 north; 592700.43 east 2021-02-13T00:31:44Z- AHRS_M2 msg: 1613176304.38 epoch sec; seqNo:13326 2021-02-13T00:31:44Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:44Z- handleDepth()-Depth_Keller msg: 1613176304393 epoch sec; seqNo:13434; depth 27.25 2021-02-13T00:31:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176304.40 epoch sec; seqNo:13434; 4059389.69 north; 592700.43 east 2021-02-13T00:31:44Z- AHRS_M2 msg: 1613176304.79 epoch sec; seqNo:13327 2021-02-13T00:31:44Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:44Z- handleDepth()-Depth_Keller msg: 1613176304792 epoch sec; seqNo:13435; depth 27.35 2021-02-13T00:31:44Z- handleDvl() - RDI_Pathfinder msg: 1613176304.81 epoch sec; seqNo:6539 2021-02-13T00:31:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 85.00 , 62.93 , 97.98 2021-02-13T00:31:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.04 2021-02-13T00:31:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176304.82 epoch sec; seqNo:13435; 4059390.07 north; 592700.42 east 2021-02-13T00:31:45Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 129 calls... 2021-02-13T00:31:45Z- waiting for 1613176302.97, time is 1613176304.82 2021-02-13T00:31:45Z- LcmTrn::updateTrn() equating pose 1613176304.82 and meas times 1613176304.82 2021-02-13T00:31:45Z- LcmTrn::updateTrn() >>>> pose time 1613176304.82, meas time 1613176304.82 <<<< 2021-02-13T00:31:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176304.82 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:31:45Z- TerrainNav::motionUpdate() - currEstimate = (4059390, 592700.42, 27.35) 2021-02-13T00:31:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:31:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:31:45Z- TNavBF::Projecting Measurements 2021-02-13T00:31:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:31:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:31:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:31:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:31:45Z- TNavFilter::4 good beams. 2021-02-13T00:31:45Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:31:45Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:31:45Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:31:45Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:31:45Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:31:45Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:31:45Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:31:45Z- TNavBF::mmse selected from filter -1 2021-02-13T00:31:45Z- TerrainMapOctree: (vehN, vehE ) = (4059390.07, 592700.42). 2021-02-13T00:31:45Z- TerrainMapOctree: Min Distance = 2420.85. 2021-02-13T00:31:45Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:31:45Z- There is no specific good beam that all particles have in common. 2021-02-13T00:31:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:31:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.130 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-13T00:31:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:31:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 11.56 2021-02-13T00:31:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:31:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.121 ExpectedMeasDiff: nan currMeas.covariance: 6.34 2021-02-13T00:31:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:31:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:31:45Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:31:45Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:31:45Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:31:45Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:31:45Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:31:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:31:45Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:31:45Z- Filter 0 No beams used in subcloud update 2021-02-13T00:31:45Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:31:45Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:31:45Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 698.8 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:31:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176304.82 sec. 2021-02-13T00:31:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:31:45Z- TNavBF::mmse selected from filter -1 2021-02-13T00:31:45Z- variances([0], [2], and psi_berg) = 698.788516 832.997310 0.000000 2021-02-13T00:31:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:31:45Z- LcmTrn::publishEstimates() - MLE : 4059389.57 592671.92 27.35 2021-02-13T00:31:45Z- LcmTrn::publishEstimates() - MMSE : 4059390.07 592700.42 27.35 2021-02-13T00:31:45Z- LcmTrn::publishEstimates() - COVAR: 698.79 833.00 0.93 0.00 2021-02-13T00:31:45Z- AHRS_M2 msg: 1613176305.19 epoch sec; seqNo:13328 2021-02-13T00:31:45Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.18 psi 2021-02-13T00:31:45Z- handleDepth()-Depth_Keller msg: 1613176305201 epoch sec; seqNo:13436; depth 27.46 2021-02-13T00:31:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176305.21 epoch sec; seqNo:13436; 4059390.45 north; 592700.42 east 2021-02-13T00:31:45Z- AHRS_M2 msg: 1613176305.60 epoch sec; seqNo:13329 2021-02-13T00:31:45Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-13T00:31:45Z- handleDepth()-Depth_Keller msg: 1613176305606 epoch sec; seqNo:13437; depth 27.58 2021-02-13T00:31:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176305.62 epoch sec; seqNo:13437; 4059390.46 north; 592701.64 east 2021-02-13T00:31:46Z- AHRS_M2 msg: 1613176306.00 epoch sec; seqNo:13330 2021-02-13T00:31:46Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.19 psi 2021-02-13T00:31:46Z- handleDepth()-Depth_Keller msg: 1613176306004 epoch sec; seqNo:13438; depth 27.68 2021-02-13T00:31:46Z- handleDvl() - RDI_Pathfinder msg: 1613176306.03 epoch sec; seqNo:6540 2021-02-13T00:31:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 0.00 , 62.52 , 97.98 2021-02-13T00:31:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.03 , 0.02 2021-02-13T00:31:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176306.04 epoch sec; seqNo:13438; 4059390.84 north; 592701.63 east 2021-02-13T00:31:46Z- AHRS_M2 msg: 1613176306.40 epoch sec; seqNo:13331 2021-02-13T00:31:46Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.19 psi 2021-02-13T00:31:46Z- handleDepth()-Depth_Keller msg: 1613176306418 epoch sec; seqNo:13439; depth 27.81 2021-02-13T00:31:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176306.43 epoch sec; seqNo:13439; 4059391.22 north; 592701.63 east 2021-02-13T00:31:46Z- AHRS_M2 msg: 1613176306.81 epoch sec; seqNo:13332 2021-02-13T00:31:46Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.18 psi 2021-02-13T00:31:46Z- handleDepth()-Depth_Keller msg: 1613176306821 epoch sec; seqNo:13440; depth 27.91 2021-02-13T00:31:46Z- handleDvl() - RDI_Pathfinder msg: 1613176306.84 epoch sec; seqNo:6541 2021-02-13T00:31:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 86.09 , 62.52 , 97.98 2021-02-13T00:31:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.02 , 0.02 2021-02-13T00:31:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176306.85 epoch sec; seqNo:13440; 4059391.60 north; 592701.63 east 2021-02-13T00:31:47Z- AHRS_M2 msg: 1613176307.22 epoch sec; seqNo:13333 2021-02-13T00:31:47Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.18 psi 2021-02-13T00:31:47Z- handleDepth()-Depth_Keller msg: 1613176307223 epoch sec; seqNo:13441; depth 28.04 2021-02-13T00:31:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176307.24 epoch sec; seqNo:13441; 4059391.60 north; 592701.63 east 2021-02-13T00:31:47Z- AHRS_M2 msg: 1613176307.62 epoch sec; seqNo:13334 2021-02-13T00:31:47Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.18 psi 2021-02-13T00:31:47Z- handleDepth()-Depth_Keller msg: 1613176307633 epoch sec; seqNo:13442; depth 28.14 2021-02-13T00:31:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176307.64 epoch sec; seqNo:13442; 4059391.98 north; 592701.62 east 2021-02-13T00:31:48Z- AHRS_M2 msg: 1613176308.02 epoch sec; seqNo:13335 2021-02-13T00:31:48Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.17 psi 2021-02-13T00:31:48Z- handleDepth()-Depth_Keller msg: 1613176308024 epoch sec; seqNo:13443; depth 28.26 2021-02-13T00:31:48Z- handleDvl() - RDI_Pathfinder msg: 1613176308.04 epoch sec; seqNo:6542 2021-02-13T00:31:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 86.09 , 61.71 , 97.98 2021-02-13T00:31:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.05 , 0.01 2021-02-13T00:31:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176308.06 epoch sec; seqNo:13443; 4059392.37 north; 592702.84 east 2021-02-13T00:31:48Z- AHRS_M2 msg: 1613176308.42 epoch sec; seqNo:13336 2021-02-13T00:31:48Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.17 psi 2021-02-13T00:31:48Z- handleDepth()-Depth_Keller msg: 1613176308428 epoch sec; seqNo:13444; depth 28.39 2021-02-13T00:31:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176308.44 epoch sec; seqNo:13444; 4059392.75 north; 592702.83 east 2021-02-13T00:31:48Z- AHRS_M2 msg: 1613176308.83 epoch sec; seqNo:13337 2021-02-13T00:31:48Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.17 psi 2021-02-13T00:31:48Z- handleDepth()-Depth_Keller msg: 1613176308833 epoch sec; seqNo:13445; depth 28.48 2021-02-13T00:31:48Z- handleDvl() - RDI_Pathfinder msg: 1613176308.84 epoch sec; seqNo:6543 2021-02-13T00:31:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 83.92 , 61.30 , 97.98 2021-02-13T00:31:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.03 , 0.00 2021-02-13T00:31:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176308.85 epoch sec; seqNo:13445; 4059392.75 north; 592702.83 east 2021-02-13T00:31:49Z- AHRS_M2 msg: 1613176309.23 epoch sec; seqNo:13338 2021-02-13T00:31:49Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.17 psi 2021-02-13T00:31:49Z- handleDepth()-Depth_Keller msg: 1613176309237 epoch sec; seqNo:13446; depth 28.61 2021-02-13T00:31:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176309.26 epoch sec; seqNo:13446; 4059393.13 north; 592702.83 east 2021-02-13T00:31:49Z- AHRS_M2 msg: 1613176309.64 epoch sec; seqNo:13339 2021-02-13T00:31:49Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.17 psi 2021-02-13T00:31:49Z- handleDepth()-Depth_Keller msg: 1613176309640 epoch sec; seqNo:13447; depth 28.72 2021-02-13T00:31:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176309.65 epoch sec; seqNo:13447; 4059393.51 north; 592702.83 east 2021-02-13T00:31:50Z- AHRS_M2 msg: 1613176310.04 epoch sec; seqNo:13340 2021-02-13T00:31:50Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.17 psi 2021-02-13T00:31:50Z- handleDepth()-Depth_Keller msg: 1613176310046 epoch sec; seqNo:13448; depth 28.84 2021-02-13T00:31:50Z- handleDvl() - RDI_Pathfinder msg: 1613176310.06 epoch sec; seqNo:6544 2021-02-13T00:31:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 83.92 , 60.89 , 97.98 2021-02-13T00:31:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.02 , -0.01 2021-02-13T00:31:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176310.07 epoch sec; seqNo:13448; 4059393.90 north; 592704.04 east 2021-02-13T00:31:50Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-13T00:31:50Z- waiting for 1613176307.82, time is 1613176310.07 2021-02-13T00:31:50Z- LcmTrn::updateTrn() equating pose 1613176310.07 and meas times 1613176310.07 2021-02-13T00:31:50Z- LcmTrn::updateTrn() >>>> pose time 1613176310.07, meas time 1613176310.07 <<<< 2021-02-13T00:31:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176310.07 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:31:50Z- TerrainNav::motionUpdate() - currEstimate = (4059394, 592704.04, 28.84) 2021-02-13T00:31:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:31:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:31:50Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176310.07 sec.; No relevant navigation data available 2021-02-13T00:31:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:31:50Z- TNavBF::mmse selected from filter -1 2021-02-13T00:31:50Z- variances([0], [2], and psi_berg) = 698.727191 832.997278 0.000000 2021-02-13T00:31:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:31:50Z- LcmTrn::publishEstimates() - MLE : 4059393.40 592675.54 28.84 2021-02-13T00:31:50Z- LcmTrn::publishEstimates() - MMSE : 4059393.90 592704.04 28.84 2021-02-13T00:31:50Z- LcmTrn::publishEstimates() - COVAR: 698.73 833.00 1.19 0.00 2021-02-13T00:31:50Z- AHRS_M2 msg: 1613176310.44 epoch sec; seqNo:13341 2021-02-13T00:31:50Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.18 psi 2021-02-13T00:31:50Z- handleDepth()-Depth_Keller msg: 1613176310448 epoch sec; seqNo:13449; depth 28.95 2021-02-13T00:31:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176310.47 epoch sec; seqNo:13449; 4059394.28 north; 592704.04 east 2021-02-13T00:31:50Z- AHRS_M2 msg: 1613176310.85 epoch sec; seqNo:13342 2021-02-13T00:31:50Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.18 psi 2021-02-13T00:31:50Z- handleDepth()-Depth_Keller msg: 1613176310852 epoch sec; seqNo:13450; depth 29.06 2021-02-13T00:31:50Z- handleDvl() - RDI_Pathfinder msg: 1613176310.86 epoch sec; seqNo:6545 2021-02-13T00:31:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 83.92 , 61.30 , 97.98 2021-02-13T00:31:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.02 , -0.02 2021-02-13T00:31:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176310.88 epoch sec; seqNo:13450; 4059394.28 north; 592704.04 east 2021-02-13T00:31:51Z- AHRS_M2 msg: 1613176311.25 epoch sec; seqNo:13343 2021-02-13T00:31:51Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.19 psi 2021-02-13T00:31:51Z- handleDepth()-Depth_Keller msg: 1613176311261 epoch sec; seqNo:13451; depth 29.18 2021-02-13T00:31:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176311.27 epoch sec; seqNo:13451; 4059394.66 north; 592704.03 east 2021-02-13T00:31:51Z- AHRS_M2 msg: 1613176311.66 epoch sec; seqNo:13344 2021-02-13T00:31:51Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.19 psi 2021-02-13T00:31:51Z- handleDepth()-Depth_Keller msg: 1613176311663 epoch sec; seqNo:13452; depth 29.30 2021-02-13T00:31:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176311.73 epoch sec; seqNo:13452; 4059395.03 north; 592704.03 east 2021-02-13T00:31:52Z- AHRS_M2 msg: 1613176312.06 epoch sec; seqNo:13345 2021-02-13T00:31:52Z- AHRS_M2 msg: 0.66 phi; -0.29 theta; -0.19 psi 2021-02-13T00:31:52Z- handleDepth()-Depth_Keller msg: 1613176312064 epoch sec; seqNo:13453; depth 29.45 2021-02-13T00:31:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176312.09 epoch sec; seqNo:13453; 4059395.41 north; 592704.03 east 2021-02-13T00:31:52Z- AHRS_M2 msg: 1613176312.46 epoch sec; seqNo:13346 2021-02-13T00:31:52Z- AHRS_M2 msg: 0.65 phi; -0.25 theta; -0.19 psi 2021-02-13T00:31:52Z- handleDepth()-Depth_Keller msg: 1613176312468 epoch sec; seqNo:13454; depth 29.58 2021-02-13T00:31:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176312.48 epoch sec; seqNo:13454; 4059395.43 north; 592705.25 east 2021-02-13T00:31:52Z- AHRS_M2 msg: 1613176312.87 epoch sec; seqNo:13347 2021-02-13T00:31:52Z- AHRS_M2 msg: 0.64 phi; -0.22 theta; -0.18 psi 2021-02-13T00:31:52Z- handleDepth()-Depth_Keller msg: 1613176312885 epoch sec; seqNo:13455; depth 29.69 2021-02-13T00:31:52Z- handleDvl() - RDI_Pathfinder msg: 1613176312.90 epoch sec; seqNo:6546 2021-02-13T00:31:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.98 , 83.92 , 62.12 , 0.00 2021-02-13T00:31:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , 0.02 2021-02-13T00:31:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176312.91 epoch sec; seqNo:13455; 4059395.81 north; 592705.24 east 2021-02-13T00:31:53Z- AHRS_M2 msg: 1613176313.27 epoch sec; seqNo:13348 2021-02-13T00:31:53Z- AHRS_M2 msg: 0.63 phi; -0.17 theta; -0.18 psi 2021-02-13T00:31:53Z- handleDepth()-Depth_Keller msg: 1613176313281 epoch sec; seqNo:13456; depth 29.81 2021-02-13T00:31:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176313.30 epoch sec; seqNo:13456; 4059396.18 north; 592705.24 east 2021-02-13T00:31:53Z- AHRS_M2 msg: 1613176313.68 epoch sec; seqNo:13349 2021-02-13T00:31:53Z- AHRS_M2 msg: 0.62 phi; -0.13 theta; -0.17 psi 2021-02-13T00:31:53Z- handleDepth()-Depth_Keller msg: 1613176313687 epoch sec; seqNo:13457; depth 29.92 2021-02-13T00:31:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176313.74 epoch sec; seqNo:13457; 4059396.56 north; 592705.23 east 2021-02-13T00:31:54Z- AHRS_M2 msg: 1613176314.08 epoch sec; seqNo:13350 2021-02-13T00:31:54Z- AHRS_M2 msg: 0.60 phi; -0.09 theta; -0.17 psi 2021-02-13T00:31:54Z- handleDepth()-Depth_Keller msg: 1613176314085 epoch sec; seqNo:13458; depth 30.03 2021-02-13T00:31:54Z- handleDvl() - RDI_Pathfinder msg: 1613176314.10 epoch sec; seqNo:6547 2021-02-13T00:31:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:31:54Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:31:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176314.11 epoch sec; seqNo:13458; 4059396.94 north; 592705.23 east 2021-02-13T00:31:54Z- AHRS_M2 msg: 1613176314.48 epoch sec; seqNo:13351 2021-02-13T00:31:54Z- AHRS_M2 msg: 0.59 phi; -0.04 theta; -0.17 psi 2021-02-13T00:31:54Z- handleDepth()-Depth_Keller msg: 1613176314490 epoch sec; seqNo:13459; depth 30.13 2021-02-13T00:31:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176314.50 epoch sec; seqNo:13459; 4059396.94 north; 592705.23 east 2021-02-13T00:31:54Z- AHRS_M2 msg: 1613176314.89 epoch sec; seqNo:13352 2021-02-13T00:31:54Z- AHRS_M2 msg: 0.58 phi; -0.00 theta; -0.17 psi 2021-02-13T00:31:54Z- handleDepth()-Depth_Keller msg: 1613176314897 epoch sec; seqNo:13460; depth 30.18 2021-02-13T00:31:54Z- handleDvl() - RDI_Pathfinder msg: 1613176314.91 epoch sec; seqNo:6548 2021-02-13T00:31:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.12 , 78.58 , 78.58 , 78.58 2021-02-13T00:31:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , 0.04 2021-02-13T00:31:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176314.93 epoch sec; seqNo:13460; 4059397.33 north; 592706.45 east 2021-02-13T00:31:55Z- LcmTrn::handleMessages() - handling 40 messages took 0.64 ms and 127 calls... 2021-02-13T00:31:55Z- waiting for 1613176313.07, time is 1613176314.93 2021-02-13T00:31:55Z- LcmTrn::updateTrn() equating pose 1613176314.93 and meas times 1613176314.93 2021-02-13T00:31:55Z- LcmTrn::updateTrn() >>>> pose time 1613176314.93, meas time 1613176314.93 <<<< 2021-02-13T00:31:55Z- TerrainNav:: Throwing out all beams because more than two are equal to 78.583 at t=1613176314.93, ping # 0. 2021-02-13T00:31:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176314.93 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:31:55Z- TerrainNav::motionUpdate() - currEstimate = (4059397, 592706.45, 30.18) 2021-02-13T00:31:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:31:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:31:55Z- TNavBF::Projecting Measurements 2021-02-13T00:31:55Z- TNavFilter::Measurement from time = 1613176314.93 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:31:55Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:31:55Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:31:55Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:31:55Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:31:55Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:31:55Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:31:55Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:31:55Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 698.7 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T00:31:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:31:55Z- TNavBF::mmse selected from filter -1 2021-02-13T00:31:55Z- variances([0], [2], and psi_berg) = 698.672518 832.997249 0.000000 2021-02-13T00:31:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:31:55Z- LcmTrn::publishEstimates() - MLE : 4059396.83 592677.95 30.18 2021-02-13T00:31:55Z- LcmTrn::publishEstimates() - MMSE : 4059397.33 592706.45 30.18 2021-02-13T00:31:55Z- LcmTrn::publishEstimates() - COVAR: 698.67 833.00 1.41 0.00 2021-02-13T00:31:55Z- AHRS_M2 msg: 1613176315.29 epoch sec; seqNo:13353 2021-02-13T00:31:55Z- AHRS_M2 msg: 0.57 phi; 0.04 theta; -0.17 psi 2021-02-13T00:31:55Z- handleDepth()-Depth_Keller msg: 1613176315296 epoch sec; seqNo:13461; depth 30.24 2021-02-13T00:31:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176315.31 epoch sec; seqNo:13461; 4059397.71 north; 592706.44 east 2021-02-13T00:31:55Z- AHRS_M2 msg: 1613176315.70 epoch sec; seqNo:13354 2021-02-13T00:31:55Z- AHRS_M2 msg: 0.56 phi; 0.08 theta; -0.16 psi 2021-02-13T00:31:55Z- handleDepth()-Depth_Keller msg: 1613176315700 epoch sec; seqNo:13462; depth 30.29 2021-02-13T00:31:55Z- handleDvl() - RDI_Pathfinder msg: 1613176315.71 epoch sec; seqNo:6549 2021-02-13T00:31:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.03 , 74.61 , 68.82 , 66.43 2021-02-13T00:31:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , 0.04 2021-02-13T00:31:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176315.72 epoch sec; seqNo:13462; 4059398.09 north; 592706.44 east 2021-02-13T00:31:56Z- AHRS_M2 msg: 1613176316.10 epoch sec; seqNo:13355 2021-02-13T00:31:56Z- AHRS_M2 msg: 0.55 phi; 0.12 theta; -0.16 psi 2021-02-13T00:31:56Z- handleDepth()-Depth_Keller msg: 1613176316105 epoch sec; seqNo:13463; depth 30.32 2021-02-13T00:31:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176316.12 epoch sec; seqNo:13463; 4059398.47 north; 592706.43 east 2021-02-13T00:31:56Z- AHRS_M2 msg: 1613176316.50 epoch sec; seqNo:13356 2021-02-13T00:31:56Z- AHRS_M2 msg: 0.54 phi; 0.15 theta; -0.16 psi 2021-02-13T00:31:56Z- handleDepth()-Depth_Keller msg: 1613176316508 epoch sec; seqNo:13464; depth 30.34 2021-02-13T00:31:56Z- handleDvl() - RDI_Pathfinder msg: 1613176316.53 epoch sec; seqNo:6550 2021-02-13T00:31:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.76 , 0.00 , 0.00 , 63.76 2021-02-13T00:31:56Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:31:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176316.54 epoch sec; seqNo:13464; 4059398.47 north; 592706.43 east 2021-02-13T00:31:56Z- AHRS_M2 msg: 1613176316.91 epoch sec; seqNo:13357 2021-02-13T00:31:56Z- AHRS_M2 msg: 0.54 phi; 0.19 theta; -0.16 psi 2021-02-13T00:31:56Z- handleDepth()-Depth_Keller msg: 1613176316913 epoch sec; seqNo:13465; depth 30.33 2021-02-13T00:31:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176316.96 epoch sec; seqNo:13465; 4059398.85 north; 592706.43 east 2021-02-13T00:31:57Z- AHRS_M2 msg: 1613176317.31 epoch sec; seqNo:13358 2021-02-13T00:31:57Z- AHRS_M2 msg: 0.53 phi; 0.21 theta; -0.16 psi 2021-02-13T00:31:57Z- handleDepth()-Depth_Keller msg: 1613176317316 epoch sec; seqNo:13466; depth 30.31 2021-02-13T00:31:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176317.33 epoch sec; seqNo:13466; 4059399.23 north; 592706.43 east 2021-02-13T00:31:57Z- AHRS_M2 msg: 1613176317.72 epoch sec; seqNo:13359 2021-02-13T00:31:57Z- AHRS_M2 msg: 0.53 phi; 0.24 theta; -0.16 psi 2021-02-13T00:31:57Z- handleDepth()-Depth_Keller msg: 1613176317733 epoch sec; seqNo:13467; depth 30.28 2021-02-13T00:31:57Z- handleDvl() - RDI_Pathfinder msg: 1613176317.75 epoch sec; seqNo:6551 2021-02-13T00:31:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 64.73 , 0.00 , 0.00 , 62.17 2021-02-13T00:31:57Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:31:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176317.76 epoch sec; seqNo:13467; 4059399.62 north; 592707.64 east 2021-02-13T00:31:58Z- AHRS_M2 msg: 1613176318.12 epoch sec; seqNo:13360 2021-02-13T00:31:58Z- AHRS_M2 msg: 0.52 phi; 0.26 theta; -0.16 psi 2021-02-13T00:31:58Z- handleDepth()-Depth_Keller msg: 1613176318127 epoch sec; seqNo:13468; depth 30.23 2021-02-13T00:31:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176318.15 epoch sec; seqNo:13468; 4059400.00 north; 592707.64 east 2021-02-13T00:31:58Z- AHRS_M2 msg: 1613176318.53 epoch sec; seqNo:13361 2021-02-13T00:31:58Z- AHRS_M2 msg: 0.52 phi; 0.28 theta; -0.16 psi 2021-02-13T00:31:58Z- handleDepth()-Depth_Keller msg: 1613176318534 epoch sec; seqNo:13469; depth 30.18 2021-02-13T00:31:58Z- handleDvl() - RDI_Pathfinder msg: 1613176318.56 epoch sec; seqNo:6552 2021-02-13T00:31:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 66.01 , 0.00 , 0.00 , 61.76 2021-02-13T00:31:58Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:31:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176318.57 epoch sec; seqNo:13469; 4059400.00 north; 592707.64 east 2021-02-13T00:31:58Z- AHRS_M2 msg: 1613176318.93 epoch sec; seqNo:13362 2021-02-13T00:31:58Z- AHRS_M2 msg: 0.52 phi; 0.29 theta; -0.15 psi 2021-02-13T00:31:58Z- handleDepth()-Depth_Keller msg: 1613176318932 epoch sec; seqNo:13470; depth 30.11 2021-02-13T00:31:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176318.94 epoch sec; seqNo:13470; 4059400.38 north; 592707.64 east 2021-02-13T00:31:59Z- AHRS_M2 msg: 1613176319.34 epoch sec; seqNo:13363 2021-02-13T00:31:59Z- AHRS_M2 msg: 0.52 phi; 0.31 theta; -0.15 psi 2021-02-13T00:31:59Z- handleDepth()-Depth_Keller msg: 1613176319342 epoch sec; seqNo:13471; depth 30.04 2021-02-13T00:31:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176319.36 epoch sec; seqNo:13471; 4059400.76 north; 592707.63 east 2021-02-13T00:31:59Z- AHRS_M2 msg: 1613176319.74 epoch sec; seqNo:13364 2021-02-13T00:31:59Z- AHRS_M2 msg: 0.52 phi; 0.32 theta; -0.14 psi 2021-02-13T00:31:59Z- handleDepth()-Depth_Keller msg: 1613176319740 epoch sec; seqNo:13472; depth 29.97 2021-02-13T00:31:59Z- handleDvl() - RDI_Pathfinder msg: 1613176319.77 epoch sec; seqNo:6553 2021-02-13T00:31:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 67.29 , 0.00 , 0.00 , 60.48 2021-02-13T00:31:59Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:31:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176319.78 epoch sec; seqNo:13472; 4059401.14 north; 592707.63 east 2021-02-13T00:32:00Z- AHRS_M2 msg: 1613176320.14 epoch sec; seqNo:13365 2021-02-13T00:32:00Z- AHRS_M2 msg: 0.52 phi; 0.33 theta; -0.14 psi 2021-02-13T00:32:00Z- handleDepth()-Depth_Keller msg: 1613176320149 epoch sec; seqNo:13473; depth 29.85 2021-02-13T00:32:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176320.17 epoch sec; seqNo:13473; 4059401.51 north; 592707.62 east 2021-02-13T00:32:00Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-13T00:32:00Z- waiting for 1613176317.93, time is 1613176320.17 2021-02-13T00:32:00Z- LcmTrn::updateTrn() >>>> pose time 1613176320.17, meas time 1613176319.77 <<<< 2021-02-13T00:32:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176319.77 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:32:00Z- TerrainNav::motionUpdate() - currEstimate = (4059402, 592707.62, 29.85) 2021-02-13T00:32:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:32:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:32:00Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:32:00Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:32:00Z- TNavBF::Projecting Measurements 2021-02-13T00:32:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:32:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:32:00Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-13T00:32:00Z- TNavFilter::2 good beams. 2021-02-13T00:32:00Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:32:00Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:32:00Z- TNavBF:: beamIndex[1] = 3 2021-02-13T00:32:00Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:32:00Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:32:00Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-13T00:32:00Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:32:00Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:00Z- TerrainMapOctree: (vehN, vehE ) = (4059401.19, 592707.53). 2021-02-13T00:32:00Z- TerrainMapOctree: Min Distance = 2407.76. 2021-02-13T00:32:00Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:32:00Z- There is no specific good beam that all particles have in common. 2021-02-13T00:32:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:32:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.61 K: 0.152 ExpectedMeasDiff: nan currMeas.covariance: 7.25 2021-02-13T00:32:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:32:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.153 ExpectedMeasDiff: nan currMeas.covariance: 5.85 2021-02-13T00:32:00Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:32:00Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:32:00Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:32:00Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:32:00Z- Filter 0 No beams used in subcloud update 2021-02-13T00:32:00Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:32:00Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:32:00Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 698.6 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:32:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176319.77 sec. 2021-02-13T00:32:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:32:00Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:00Z- variances([0], [2], and psi_berg) = 698.603639 832.997214 0.000000 2021-02-13T00:32:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:32:00Z- LcmTrn::publishEstimates() - MLE : 4059401.01 592679.12 29.85 2021-02-13T00:32:00Z- LcmTrn::publishEstimates() - MMSE : 4059401.51 592707.62 29.85 2021-02-13T00:32:00Z- LcmTrn::publishEstimates() - COVAR: 698.60 833.00 1.18 0.00 2021-02-13T00:32:00Z- AHRS_M2 msg: 1613176320.54 epoch sec; seqNo:13366 2021-02-13T00:32:00Z- AHRS_M2 msg: 0.53 phi; 0.34 theta; -0.14 psi 2021-02-13T00:32:00Z- handleDepth()-Depth_Keller msg: 1613176320548 epoch sec; seqNo:13474; depth 29.76 2021-02-13T00:32:00Z- handleDvl() - RDI_Pathfinder msg: 1613176320.56 epoch sec; seqNo:6554 2021-02-13T00:32:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:32:00Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:32:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176320.57 epoch sec; seqNo:13474; 4059401.53 north; 592708.84 east 2021-02-13T00:32:00Z- AHRS_M2 msg: 1613176320.95 epoch sec; seqNo:13367 2021-02-13T00:32:00Z- AHRS_M2 msg: 0.53 phi; 0.34 theta; -0.13 psi 2021-02-13T00:32:00Z- handleDepth()-Depth_Keller msg: 1613176320961 epoch sec; seqNo:13475; depth 29.69 2021-02-13T00:32:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176320.97 epoch sec; seqNo:13475; 4059401.91 north; 592708.84 east 2021-02-13T00:32:01Z- AHRS_M2 msg: 1613176321.35 epoch sec; seqNo:13368 2021-02-13T00:32:01Z- AHRS_M2 msg: 0.54 phi; 0.35 theta; -0.13 psi 2021-02-13T00:32:01Z- handleDepth()-Depth_Keller msg: 1613176321356 epoch sec; seqNo:13476; depth 29.56 2021-02-13T00:32:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176321.37 epoch sec; seqNo:13476; 4059402.29 north; 592708.84 east 2021-02-13T00:32:01Z- AHRS_M2 msg: 1613176321.76 epoch sec; seqNo:13369 2021-02-13T00:32:01Z- AHRS_M2 msg: 0.54 phi; 0.35 theta; -0.12 psi 2021-02-13T00:32:01Z- handleDepth()-Depth_Keller msg: 1613176321760 epoch sec; seqNo:13477; depth 29.46 2021-02-13T00:32:01Z- handleDvl() - RDI_Pathfinder msg: 1613176321.79 epoch sec; seqNo:6555 2021-02-13T00:32:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.66 , 80.67 , 97.98 , 61.30 2021-02-13T00:32:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.04 , -0.07 2021-02-13T00:32:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176321.80 epoch sec; seqNo:13477; 4059402.66 north; 592708.83 east 2021-02-13T00:32:02Z- AHRS_M2 msg: 1613176322.16 epoch sec; seqNo:13370 2021-02-13T00:32:02Z- AHRS_M2 msg: 0.55 phi; 0.35 theta; -0.12 psi 2021-02-13T00:32:02Z- handleDepth()-Depth_Keller msg: 1613176322165 epoch sec; seqNo:13478; depth 29.33 2021-02-13T00:32:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176322.18 epoch sec; seqNo:13478; 4059402.66 north; 592708.83 east 2021-02-13T00:32:02Z- AHRS_M2 msg: 1613176322.56 epoch sec; seqNo:13371 2021-02-13T00:32:02Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.11 psi 2021-02-13T00:32:02Z- handleDepth()-Depth_Keller msg: 1613176322570 epoch sec; seqNo:13479; depth 29.22 2021-02-13T00:32:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176322.58 epoch sec; seqNo:13479; 4059403.04 north; 592708.83 east 2021-02-13T00:32:02Z- AHRS_M2 msg: 1613176322.97 epoch sec; seqNo:13372 2021-02-13T00:32:02Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.11 psi 2021-02-13T00:32:02Z- handleDepth()-Depth_Keller msg: 1613176322972 epoch sec; seqNo:13480; depth 29.10 2021-02-13T00:32:02Z- handleDvl() - RDI_Pathfinder msg: 1613176322.99 epoch sec; seqNo:6556 2021-02-13T00:32:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 0.00 , 94.74 , 61.30 2021-02-13T00:32:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.00 , -0.08 2021-02-13T00:32:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176323.01 epoch sec; seqNo:13480; 4059403.42 north; 592708.82 east 2021-02-13T00:32:03Z- AHRS_M2 msg: 1613176323.37 epoch sec; seqNo:13373 2021-02-13T00:32:03Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.11 psi 2021-02-13T00:32:03Z- handleDepth()-Depth_Keller msg: 1613176323376 epoch sec; seqNo:13481; depth 28.96 2021-02-13T00:32:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176323.39 epoch sec; seqNo:13481; 4059403.42 north; 592708.82 east 2021-02-13T00:32:03Z- AHRS_M2 msg: 1613176323.78 epoch sec; seqNo:13374 2021-02-13T00:32:03Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.11 psi 2021-02-13T00:32:03Z- handleDepth()-Depth_Keller msg: 1613176323795 epoch sec; seqNo:13482; depth 28.86 2021-02-13T00:32:03Z- handleDvl() - RDI_Pathfinder msg: 1613176323.81 epoch sec; seqNo:6557 2021-02-13T00:32:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 80.67 , 94.74 , 61.30 2021-02-13T00:32:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.07 2021-02-13T00:32:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176323.82 epoch sec; seqNo:13482; 4059403.81 north; 592710.04 east 2021-02-13T00:32:04Z- AHRS_M2 msg: 1613176324.19 epoch sec; seqNo:13375 2021-02-13T00:32:04Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.12 psi 2021-02-13T00:32:04Z- handleDepth()-Depth_Keller msg: 1613176324192 epoch sec; seqNo:13483; depth 28.73 2021-02-13T00:32:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176324.21 epoch sec; seqNo:13483; 4059404.19 north; 592710.04 east 2021-02-13T00:32:04Z- AHRS_M2 msg: 1613176324.59 epoch sec; seqNo:13376 2021-02-13T00:32:04Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.12 psi 2021-02-13T00:32:04Z- handleDepth()-Depth_Keller msg: 1613176324590 epoch sec; seqNo:13484; depth 28.63 2021-02-13T00:32:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176324.60 epoch sec; seqNo:13484; 4059404.19 north; 592710.04 east 2021-02-13T00:32:04Z- AHRS_M2 msg: 1613176324.99 epoch sec; seqNo:13377 2021-02-13T00:32:04Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.13 psi 2021-02-13T00:32:04Z- handleDepth()-Depth_Keller msg: 1613176324993 epoch sec; seqNo:13485; depth 28.50 2021-02-13T00:32:05Z- handleDvl() - RDI_Pathfinder msg: 1613176325.01 epoch sec; seqNo:6558 2021-02-13T00:32:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 81.75 , 97.98 , 61.30 2021-02-13T00:32:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , -0.00 2021-02-13T00:32:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176325.02 epoch sec; seqNo:13485; 4059404.57 north; 592710.03 east 2021-02-13T00:32:05Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 129 calls... 2021-02-13T00:32:05Z- waiting for 1613176323.17, time is 1613176325.02 2021-02-13T00:32:05Z- LcmTrn::updateTrn() equating pose 1613176325.02 and meas times 1613176325.02 2021-02-13T00:32:05Z- LcmTrn::updateTrn() >>>> pose time 1613176325.02, meas time 1613176325.02 <<<< 2021-02-13T00:32:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176325.02 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:32:05Z- TerrainNav::motionUpdate() - currEstimate = (4059405, 592710.03, 28.50) 2021-02-13T00:32:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:32:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:32:05Z- TNavBF::Projecting Measurements 2021-02-13T00:32:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:32:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:32:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:32:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:32:05Z- TNavFilter::4 good beams. 2021-02-13T00:32:05Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:32:05Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:32:05Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:32:05Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:32:05Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:32:05Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:32:05Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:32:05Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:05Z- TerrainMapOctree: (vehN, vehE ) = (4059404.57, 592710.03). 2021-02-13T00:32:05Z- TerrainMapOctree: Min Distance = 2403.56. 2021-02-13T00:32:05Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:32:05Z- There is no specific good beam that all particles have in common. 2021-02-13T00:32:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:32:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.126 ExpectedMeasDiff: nan currMeas.covariance: 7.81 2021-02-13T00:32:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:32:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 10.69 2021-02-13T00:32:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:32:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:32:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:32:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.117 ExpectedMeasDiff: nan currMeas.covariance: 6.01 2021-02-13T00:32:05Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:32:05Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:32:05Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:32:05Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:32:05Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:32:05Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:32:05Z- Filter 0 No beams used in subcloud update 2021-02-13T00:32:05Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:32:05Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:32:05Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 698.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:32:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176325.02 sec. 2021-02-13T00:32:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:32:05Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:05Z- variances([0], [2], and psi_berg) = 698.551040 832.997187 0.000000 2021-02-13T00:32:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:32:05Z- LcmTrn::publishEstimates() - MLE : 4059404.07 592681.53 28.50 2021-02-13T00:32:05Z- LcmTrn::publishEstimates() - MMSE : 4059404.57 592710.03 28.50 2021-02-13T00:32:05Z- LcmTrn::publishEstimates() - COVAR: 698.55 833.00 0.91 0.00 2021-02-13T00:32:05Z- AHRS_M2 msg: 1613176325.39 epoch sec; seqNo:13378 2021-02-13T00:32:05Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.13 psi 2021-02-13T00:32:05Z- handleDepth()-Depth_Keller msg: 1613176325396 epoch sec; seqNo:13486; depth 28.36 2021-02-13T00:32:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176325.41 epoch sec; seqNo:13486; 4059404.95 north; 592710.03 east 2021-02-13T00:32:05Z- AHRS_M2 msg: 1613176325.80 epoch sec; seqNo:13379 2021-02-13T00:32:05Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.14 psi 2021-02-13T00:32:05Z- handleDepth()-Depth_Keller msg: 1613176325803 epoch sec; seqNo:13487; depth 28.23 2021-02-13T00:32:05Z- handleDvl() - RDI_Pathfinder msg: 1613176325.82 epoch sec; seqNo:6559 2021-02-13T00:32:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 0.00 , 97.98 , 60.90 2021-02-13T00:32:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.11 , 0.07 2021-02-13T00:32:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176325.84 epoch sec; seqNo:13487; 4059405.33 north; 592710.02 east 2021-02-13T00:32:06Z- AHRS_M2 msg: 1613176326.20 epoch sec; seqNo:13380 2021-02-13T00:32:06Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.14 psi 2021-02-13T00:32:06Z- handleDepth()-Depth_Keller msg: 1613176326209 epoch sec; seqNo:13488; depth 28.11 2021-02-13T00:32:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176326.22 epoch sec; seqNo:13488; 4059405.33 north; 592710.02 east 2021-02-13T00:32:06Z- AHRS_M2 msg: 1613176326.61 epoch sec; seqNo:13381 2021-02-13T00:32:06Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.15 psi 2021-02-13T00:32:06Z- handleDepth()-Depth_Keller msg: 1613176326611 epoch sec; seqNo:13489; depth 27.98 2021-02-13T00:32:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176326.62 epoch sec; seqNo:13489; 4059405.71 north; 592710.02 east 2021-02-13T00:32:07Z- AHRS_M2 msg: 1613176327.01 epoch sec; seqNo:13382 2021-02-13T00:32:07Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.15 psi 2021-02-13T00:32:07Z- handleDepth()-Depth_Keller msg: 1613176327013 epoch sec; seqNo:13490; depth 27.84 2021-02-13T00:32:07Z- handleDvl() - RDI_Pathfinder msg: 1613176327.03 epoch sec; seqNo:6560 2021-02-13T00:32:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 83.92 , 97.98 , 61.30 2021-02-13T00:32:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , 0.09 2021-02-13T00:32:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176327.05 epoch sec; seqNo:13490; 4059406.10 north; 592711.24 east 2021-02-13T00:32:07Z- AHRS_M2 msg: 1613176327.41 epoch sec; seqNo:13383 2021-02-13T00:32:07Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.15 psi 2021-02-13T00:32:07Z- handleDepth()-Depth_Keller msg: 1613176327416 epoch sec; seqNo:13491; depth 27.71 2021-02-13T00:32:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176327.43 epoch sec; seqNo:13491; 4059406.48 north; 592711.23 east 2021-02-13T00:32:07Z- AHRS_M2 msg: 1613176327.82 epoch sec; seqNo:13384 2021-02-13T00:32:07Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.16 psi 2021-02-13T00:32:07Z- handleDepth()-Depth_Keller msg: 1613176327827 epoch sec; seqNo:13492; depth 27.59 2021-02-13T00:32:07Z- handleDvl() - RDI_Pathfinder msg: 1613176327.84 epoch sec; seqNo:6561 2021-02-13T00:32:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 85.00 , 97.98 , 61.30 2021-02-13T00:32:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.08 2021-02-13T00:32:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176327.85 epoch sec; seqNo:13492; 4059406.86 north; 592711.23 east 2021-02-13T00:32:08Z- AHRS_M2 msg: 1613176328.22 epoch sec; seqNo:13385 2021-02-13T00:32:08Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.16 psi 2021-02-13T00:32:08Z- handleDepth()-Depth_Keller msg: 1613176328226 epoch sec; seqNo:13493; depth 27.46 2021-02-13T00:32:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176328.24 epoch sec; seqNo:13493; 4059406.86 north; 592711.23 east 2021-02-13T00:32:08Z- AHRS_M2 msg: 1613176328.62 epoch sec; seqNo:13386 2021-02-13T00:32:08Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.16 psi 2021-02-13T00:32:08Z- handleDepth()-Depth_Keller msg: 1613176328638 epoch sec; seqNo:13494; depth 27.33 2021-02-13T00:32:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176328.65 epoch sec; seqNo:13494; 4059407.24 north; 592711.22 east 2021-02-13T00:32:09Z- AHRS_M2 msg: 1613176329.03 epoch sec; seqNo:13387 2021-02-13T00:32:09Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.16 psi 2021-02-13T00:32:09Z- handleDepth()-Depth_Keller msg: 1613176329037 epoch sec; seqNo:13495; depth 27.23 2021-02-13T00:32:09Z- handleDvl() - RDI_Pathfinder msg: 1613176329.06 epoch sec; seqNo:6562 2021-02-13T00:32:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 85.00 , 97.98 , 63.76 2021-02-13T00:32:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.05 2021-02-13T00:32:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176329.07 epoch sec; seqNo:13495; 4059407.62 north; 592711.22 east 2021-02-13T00:32:09Z- AHRS_M2 msg: 1613176329.43 epoch sec; seqNo:13388 2021-02-13T00:32:09Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.16 psi 2021-02-13T00:32:09Z- handleDepth()-Depth_Keller msg: 1613176329436 epoch sec; seqNo:13496; depth 27.08 2021-02-13T00:32:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176329.45 epoch sec; seqNo:13496; 4059408.01 north; 592712.44 east 2021-02-13T00:32:09Z- AHRS_M2 msg: 1613176329.84 epoch sec; seqNo:13389 2021-02-13T00:32:09Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.16 psi 2021-02-13T00:32:09Z- handleDepth()-Depth_Keller msg: 1613176329848 epoch sec; seqNo:13497; depth 26.97 2021-02-13T00:32:09Z- handleDvl() - RDI_Pathfinder msg: 1613176329.86 epoch sec; seqNo:6563 2021-02-13T00:32:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 86.08 , 97.98 , 63.35 2021-02-13T00:32:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.02 2021-02-13T00:32:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176329.87 epoch sec; seqNo:13497; 4059408.39 north; 592712.43 east 2021-02-13T00:32:10Z- AHRS_M2 msg: 1613176330.24 epoch sec; seqNo:13390 2021-02-13T00:32:10Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.16 psi 2021-02-13T00:32:10Z- handleDepth()-Depth_Keller msg: 1613176330244 epoch sec; seqNo:13498; depth 26.83 2021-02-13T00:32:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176330.26 epoch sec; seqNo:13498; 4059408.39 north; 592712.43 east 2021-02-13T00:32:10Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:32:10Z- waiting for 1613176328.02, time is 1613176330.26 2021-02-13T00:32:10Z- LcmTrn::updateTrn() >>>> pose time 1613176330.26, meas time 1613176329.86 <<<< 2021-02-13T00:32:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176329.86 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:32:10Z- TerrainNav::motionUpdate() - currEstimate = (4059408, 592712.43, 26.83) 2021-02-13T00:32:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:32:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:32:10Z- TNavBF::Projecting Measurements 2021-02-13T00:32:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:32:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:32:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:32:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:32:10Z- TNavFilter::4 good beams. 2021-02-13T00:32:10Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:32:10Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:32:10Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:32:10Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:32:10Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:32:10Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:32:10Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:32:10Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:10Z- TerrainMapOctree: (vehN, vehE ) = (4059408.09, 592712.25). 2021-02-13T00:32:10Z- TerrainMapOctree: Min Distance = 2399.44. 2021-02-13T00:32:10Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:32:10Z- There is no specific good beam that all particles have in common. 2021-02-13T00:32:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:32:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 7.91 2021-02-13T00:32:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:32:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 11.86 2021-02-13T00:32:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:32:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:32:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:32:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 6.42 2021-02-13T00:32:10Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:32:10Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:32:10Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:32:10Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:32:10Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:32:10Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:32:10Z- Filter 0 No beams used in subcloud update 2021-02-13T00:32:10Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:32:10Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:32:10Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 698.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:32:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176329.86 sec. 2021-02-13T00:32:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:32:10Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:10Z- variances([0], [2], and psi_berg) = 698.481041 832.997150 0.000000 2021-02-13T00:32:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:32:10Z- LcmTrn::publishEstimates() - MLE : 4059407.89 592683.93 26.83 2021-02-13T00:32:10Z- LcmTrn::publishEstimates() - MMSE : 4059408.39 592712.43 26.83 2021-02-13T00:32:10Z- LcmTrn::publishEstimates() - COVAR: 698.48 833.00 0.82 0.00 2021-02-13T00:32:10Z- AHRS_M2 msg: 1613176330.64 epoch sec; seqNo:13391 2021-02-13T00:32:10Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.15 psi 2021-02-13T00:32:10Z- handleDepth()-Depth_Keller msg: 1613176330648 epoch sec; seqNo:13499; depth 26.71 2021-02-13T00:32:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176330.66 epoch sec; seqNo:13499; 4059408.77 north; 592712.43 east 2021-02-13T00:32:11Z- AHRS_M2 msg: 1613176331.05 epoch sec; seqNo:13392 2021-02-13T00:32:11Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-13T00:32:11Z- handleDepth()-Depth_Keller msg: 1613176331057 epoch sec; seqNo:13500; depth 26.59 2021-02-13T00:32:11Z- handleDvl() - RDI_Pathfinder msg: 1613176331.07 epoch sec; seqNo:6564 2021-02-13T00:32:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 86.08 , 96.90 , 64.16 2021-02-13T00:32:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.02 2021-02-13T00:32:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176331.08 epoch sec; seqNo:13500; 4059409.14 north; 592712.42 east 2021-02-13T00:32:11Z- AHRS_M2 msg: 1613176331.45 epoch sec; seqNo:13393 2021-02-13T00:32:11Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-13T00:32:11Z- handleDepth()-Depth_Keller msg: 1613176331458 epoch sec; seqNo:13501; depth 26.44 2021-02-13T00:32:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176331.47 epoch sec; seqNo:13501; 4059409.14 north; 592712.42 east 2021-02-13T00:32:11Z- AHRS_M2 msg: 1613176331.86 epoch sec; seqNo:13394 2021-02-13T00:32:11Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.14 psi 2021-02-13T00:32:11Z- handleDepth()-Depth_Keller msg: 1613176331861 epoch sec; seqNo:13502; depth 26.34 2021-02-13T00:32:11Z- handleDvl() - RDI_Pathfinder msg: 1613176331.87 epoch sec; seqNo:6565 2021-02-13T00:32:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.52 , 86.08 , 97.98 , 64.57 2021-02-13T00:32:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.02 2021-02-13T00:32:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176331.88 epoch sec; seqNo:13502; 4059409.52 north; 592712.42 east 2021-02-13T00:32:12Z- AHRS_M2 msg: 1613176332.26 epoch sec; seqNo:13395 2021-02-13T00:32:12Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.14 psi 2021-02-13T00:32:12Z- handleDepth()-Depth_Keller msg: 1613176332273 epoch sec; seqNo:13503; depth 26.21 2021-02-13T00:32:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176332.28 epoch sec; seqNo:13503; 4059409.91 north; 592713.64 east 2021-02-13T00:32:12Z- AHRS_M2 msg: 1613176332.66 epoch sec; seqNo:13396 2021-02-13T00:32:12Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-13T00:32:12Z- handleDepth()-Depth_Keller msg: 1613176332672 epoch sec; seqNo:13504; depth 26.08 2021-02-13T00:32:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176332.70 epoch sec; seqNo:13504; 4059410.29 north; 592713.63 east 2021-02-13T00:32:13Z- AHRS_M2 msg: 1613176333.07 epoch sec; seqNo:13397 2021-02-13T00:32:13Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.13 psi 2021-02-13T00:32:13Z- handleDepth()-Depth_Keller msg: 1613176333072 epoch sec; seqNo:13505; depth 25.95 2021-02-13T00:32:13Z- handleDvl() - RDI_Pathfinder msg: 1613176333.08 epoch sec; seqNo:6566 2021-02-13T00:32:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 84.93 , 97.91 , 63.76 2021-02-13T00:32:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.01 2021-02-13T00:32:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176333.11 epoch sec; seqNo:13505; 4059410.29 north; 592713.63 east 2021-02-13T00:32:13Z- AHRS_M2 msg: 1613176333.47 epoch sec; seqNo:13398 2021-02-13T00:32:13Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.13 psi 2021-02-13T00:32:13Z- handleDepth()-Depth_Keller msg: 1613176333477 epoch sec; seqNo:13506; depth 25.81 2021-02-13T00:32:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176333.49 epoch sec; seqNo:13506; 4059410.67 north; 592713.63 east 2021-02-13T00:32:13Z- AHRS_M2 msg: 1613176333.88 epoch sec; seqNo:13399 2021-02-13T00:32:13Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.13 psi 2021-02-13T00:32:13Z- handleDepth()-Depth_Keller msg: 1613176333882 epoch sec; seqNo:13507; depth 25.68 2021-02-13T00:32:13Z- handleDvl() - RDI_Pathfinder msg: 1613176333.89 epoch sec; seqNo:6567 2021-02-13T00:32:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 86.08 , 97.98 , 64.98 2021-02-13T00:32:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , -0.00 2021-02-13T00:32:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176333.90 epoch sec; seqNo:13507; 4059411.05 north; 592713.62 east 2021-02-13T00:32:14Z- AHRS_M2 msg: 1613176334.28 epoch sec; seqNo:13400 2021-02-13T00:32:14Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.14 psi 2021-02-13T00:32:14Z- handleDepth()-Depth_Keller msg: 1613176334289 epoch sec; seqNo:13508; depth 25.54 2021-02-13T00:32:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176334.30 epoch sec; seqNo:13508; 4059411.43 north; 592713.62 east 2021-02-13T00:32:14Z- AHRS_M2 msg: 1613176334.69 epoch sec; seqNo:13401 2021-02-13T00:32:14Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.14 psi 2021-02-13T00:32:14Z- handleDepth()-Depth_Keller msg: 1613176334701 epoch sec; seqNo:13509; depth 25.42 2021-02-13T00:32:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176334.73 epoch sec; seqNo:13509; 4059411.82 north; 592714.84 east 2021-02-13T00:32:15Z- AHRS_M2 msg: 1613176335.09 epoch sec; seqNo:13402 2021-02-13T00:32:15Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.14 psi 2021-02-13T00:32:15Z- handleDepth()-Depth_Keller msg: 1613176335094 epoch sec; seqNo:13510; depth 25.29 2021-02-13T00:32:15Z- handleDvl() - RDI_Pathfinder msg: 1613176335.11 epoch sec; seqNo:6568 2021-02-13T00:32:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 84.93 , 99.00 , 64.98 2021-02-13T00:32:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , -0.02 2021-02-13T00:32:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176335.12 epoch sec; seqNo:13510; 4059411.82 north; 592714.84 east 2021-02-13T00:32:15Z- AHRS_M2 msg: 1613176335.49 epoch sec; seqNo:13403 2021-02-13T00:32:15Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.14 psi 2021-02-13T00:32:15Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-13T00:32:15Z- waiting for 1613176333.26, time is 1613176335.49 2021-02-13T00:32:15Z- LcmTrn::updateTrn() >>>> pose time 1613176335.49, meas time 1613176335.11 <<<< 2021-02-13T00:32:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176335.11 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:32:15Z- TerrainNav::motionUpdate() - currEstimate = (4059412, 592714.84, 25.29) 2021-02-13T00:32:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:32:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:32:15Z- TNavBF::Projecting Measurements 2021-02-13T00:32:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:32:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:32:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:32:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:32:15Z- TNavFilter::4 good beams. 2021-02-13T00:32:15Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:32:15Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:32:15Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:32:15Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:32:15Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:32:15Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:32:15Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:32:15Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:15Z- TerrainMapOctree: (vehN, vehE ) = (4059411.57, 592714.66). 2021-02-13T00:32:15Z- TerrainMapOctree: Min Distance = 2395.23. 2021-02-13T00:32:15Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:32:15Z- There is no specific good beam that all particles have in common. 2021-02-13T00:32:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:32:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 8.47 2021-02-13T00:32:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:32:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 11.54 2021-02-13T00:32:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:32:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 15.68 2021-02-13T00:32:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:32:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 6.76 2021-02-13T00:32:15Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:32:15Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:32:15Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:32:15Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:32:15Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:32:15Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:32:15Z- Filter 0 No beams used in subcloud update 2021-02-13T00:32:15Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:32:15Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:32:15Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 698.4 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:32:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176335.11 sec. 2021-02-13T00:32:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:32:15Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:15Z- variances([0], [2], and psi_berg) = 698.413744 832.997116 0.000000 2021-02-13T00:32:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:32:15Z- LcmTrn::publishEstimates() - MLE : 4059411.32 592686.34 25.29 2021-02-13T00:32:15Z- LcmTrn::publishEstimates() - MMSE : 4059411.82 592714.84 25.29 2021-02-13T00:32:15Z- LcmTrn::publishEstimates() - COVAR: 698.41 833.00 0.77 0.00 2021-02-13T00:32:15Z- handleDepth()-Depth_Keller msg: 1613176335497 epoch sec; seqNo:13511; depth 25.15 2021-02-13T00:32:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176335.51 epoch sec; seqNo:13511; 4059412.20 north; 592714.83 east 2021-02-13T00:32:15Z- AHRS_M2 msg: 1613176335.90 epoch sec; seqNo:13404 2021-02-13T00:32:15Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.14 psi 2021-02-13T00:32:15Z- handleDepth()-Depth_Keller msg: 1613176335905 epoch sec; seqNo:13512; depth 25.02 2021-02-13T00:32:15Z- handleDvl() - RDI_Pathfinder msg: 1613176335.92 epoch sec; seqNo:6569 2021-02-13T00:32:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 87.10 , 99.00 , 64.16 2021-02-13T00:32:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , -0.03 2021-02-13T00:32:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176335.93 epoch sec; seqNo:13512; 4059412.58 north; 592714.83 east 2021-02-13T00:32:16Z- AHRS_M2 msg: 1613176336.30 epoch sec; seqNo:13405 2021-02-13T00:32:16Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.14 psi 2021-02-13T00:32:16Z- handleDepth()-Depth_Keller msg: 1613176336309 epoch sec; seqNo:13513; depth 24.89 2021-02-13T00:32:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176336.32 epoch sec; seqNo:13513; 4059412.58 north; 592714.83 east 2021-02-13T00:32:16Z- AHRS_M2 msg: 1613176336.70 epoch sec; seqNo:13406 2021-02-13T00:32:16Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-13T00:32:16Z- handleDepth()-Depth_Keller msg: 1613176336710 epoch sec; seqNo:13514; depth 24.76 2021-02-13T00:32:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176336.72 epoch sec; seqNo:13514; 4059412.96 north; 592714.82 east 2021-02-13T00:32:17Z- AHRS_M2 msg: 1613176337.11 epoch sec; seqNo:13407 2021-02-13T00:32:17Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-13T00:32:17Z- handleDepth()-Depth_Keller msg: 1613176337113 epoch sec; seqNo:13515; depth 24.64 2021-02-13T00:32:17Z- handleDvl() - RDI_Pathfinder msg: 1613176337.13 epoch sec; seqNo:6570 2021-02-13T00:32:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 86.96 , 101.02 , 63.76 2021-02-13T00:32:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , 0.00 2021-02-13T00:32:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176337.15 epoch sec; seqNo:13515; 4059413.34 north; 592714.82 east 2021-02-13T00:32:17Z- AHRS_M2 msg: 1613176337.51 epoch sec; seqNo:13408 2021-02-13T00:32:17Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-13T00:32:17Z- handleDepth()-Depth_Keller msg: 1613176337516 epoch sec; seqNo:13516; depth 24.50 2021-02-13T00:32:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176337.53 epoch sec; seqNo:13516; 4059413.73 north; 592716.04 east 2021-02-13T00:32:17Z- AHRS_M2 msg: 1613176337.92 epoch sec; seqNo:13409 2021-02-13T00:32:17Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-13T00:32:17Z- handleDepth()-Depth_Keller msg: 1613176337922 epoch sec; seqNo:13517; depth 24.38 2021-02-13T00:32:17Z- handleDvl() - RDI_Pathfinder msg: 1613176337.93 epoch sec; seqNo:6571 2021-02-13T00:32:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 0.00 , 99.60 , 114.21 2021-02-13T00:32:17Z- handleDvl() - RDI_Pathfinder msg: velocities 1.72 , -0.85 , 0.51 2021-02-13T00:32:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176337.94 epoch sec; seqNo:13517; 4059414.49 north; 592716.03 east 2021-02-13T00:32:18Z- AHRS_M2 msg: 1613176338.32 epoch sec; seqNo:13410 2021-02-13T00:32:18Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-13T00:32:18Z- handleDepth()-Depth_Keller msg: 1613176338327 epoch sec; seqNo:13518; depth 24.24 2021-02-13T00:32:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176338.34 epoch sec; seqNo:13518; 4059415.25 north; 592716.02 east 2021-02-13T00:32:18Z- AHRS_M2 msg: 1613176338.74 epoch sec; seqNo:13411 2021-02-13T00:32:18Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-13T00:32:18Z- handleDepth()-Depth_Keller msg: 1613176338741 epoch sec; seqNo:13519; depth 24.11 2021-02-13T00:32:18Z- handleDvl() - RDI_Pathfinder msg: 1613176338.76 epoch sec; seqNo:6572 2021-02-13T00:32:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 86.62 , 100.14 , 64.57 2021-02-13T00:32:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , 0.08 2021-02-13T00:32:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176338.77 epoch sec; seqNo:13519; 4059415.62 north; 592716.02 east 2021-02-13T00:32:19Z- AHRS_M2 msg: 1613176339.13 epoch sec; seqNo:13412 2021-02-13T00:32:19Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-13T00:32:19Z- handleDepth()-Depth_Keller msg: 1613176339135 epoch sec; seqNo:13520; depth 23.96 2021-02-13T00:32:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176339.15 epoch sec; seqNo:13520; 4059415.62 north; 592716.02 east 2021-02-13T00:32:19Z- AHRS_M2 msg: 1613176339.53 epoch sec; seqNo:13413 2021-02-13T00:32:19Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-13T00:32:19Z- handleDepth()-Depth_Keller msg: 1613176339538 epoch sec; seqNo:13521; depth 23.85 2021-02-13T00:32:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176339.55 epoch sec; seqNo:13521; 4059416.00 north; 592716.01 east 2021-02-13T00:32:19Z- AHRS_M2 msg: 1613176339.94 epoch sec; seqNo:13414 2021-02-13T00:32:19Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-13T00:32:19Z- handleDepth()-Depth_Keller msg: 1613176339940 epoch sec; seqNo:13522; depth 23.72 2021-02-13T00:32:19Z- handleDvl() - RDI_Pathfinder msg: 1613176339.97 epoch sec; seqNo:6573 2021-02-13T00:32:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 89.19 , 101.09 , 66.21 2021-02-13T00:32:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , 0.07 2021-02-13T00:32:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176339.98 epoch sec; seqNo:13522; 4059416.39 north; 592717.23 east 2021-02-13T00:32:20Z- AHRS_M2 msg: 1613176340.34 epoch sec; seqNo:13415 2021-02-13T00:32:20Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-13T00:32:20Z- handleDepth()-Depth_Keller msg: 1613176340344 epoch sec; seqNo:13523; depth 23.59 2021-02-13T00:32:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176340.36 epoch sec; seqNo:13523; 4059416.77 north; 592717.22 east 2021-02-13T00:32:20Z- LcmTrn::handleMessages() - handling 43 messages took 0.63 ms and 131 calls... 2021-02-13T00:32:20Z- waiting for 1613176338.49, time is 1613176340.36 2021-02-13T00:32:20Z- LcmTrn::updateTrn() >>>> pose time 1613176340.36, meas time 1613176339.97 <<<< 2021-02-13T00:32:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176339.97 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:32:20Z- TerrainNav::motionUpdate() - currEstimate = (4059417, 592717.22, 23.59) 2021-02-13T00:32:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:32:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:32:20Z- TNavBF::Projecting Measurements 2021-02-13T00:32:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:32:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:32:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:32:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:32:20Z- TNavFilter::4 good beams. 2021-02-13T00:32:20Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:32:20Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:32:20Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:32:20Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:32:20Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:32:20Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:32:20Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:32:20Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:20Z- TerrainMapOctree: (vehN, vehE ) = (4059416.38, 592717.03). 2021-02-13T00:32:20Z- TerrainMapOctree: Min Distance = 2390.01. 2021-02-13T00:32:20Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:32:20Z- There is no specific good beam that all particles have in common. 2021-02-13T00:32:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:32:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 8.66 2021-02-13T00:32:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:32:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 12.73 2021-02-13T00:32:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:32:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 16.35 2021-02-13T00:32:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:32:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 7.01 2021-02-13T00:32:20Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:32:20Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:32:20Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:32:20Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:32:20Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:32:20Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:32:20Z- Filter 0 No beams used in subcloud update 2021-02-13T00:32:20Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:32:20Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:32:20Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 698.4 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:32:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176339.97 sec. 2021-02-13T00:32:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:32:20Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:20Z- variances([0], [2], and psi_berg) = 698.336320 832.997076 0.000000 2021-02-13T00:32:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:32:20Z- LcmTrn::publishEstimates() - MLE : 4059416.27 592688.72 23.59 2021-02-13T00:32:20Z- LcmTrn::publishEstimates() - MMSE : 4059416.77 592717.22 23.59 2021-02-13T00:32:20Z- LcmTrn::publishEstimates() - COVAR: 698.34 833.00 0.78 0.00 2021-02-13T00:32:20Z- AHRS_M2 msg: 1613176340.74 epoch sec; seqNo:13416 2021-02-13T00:32:20Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.16 psi 2021-02-13T00:32:20Z- handleDepth()-Depth_Keller msg: 1613176340754 epoch sec; seqNo:13524; depth 23.46 2021-02-13T00:32:20Z- handleDvl() - RDI_Pathfinder msg: 1613176340.77 epoch sec; seqNo:6574 2021-02-13T00:32:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 0.00 , 102.04 , 66.21 2021-02-13T00:32:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , 0.03 2021-02-13T00:32:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176340.78 epoch sec; seqNo:13524; 4059416.77 north; 592717.22 east 2021-02-13T00:32:21Z- AHRS_M2 msg: 1613176341.16 epoch sec; seqNo:13417 2021-02-13T00:32:21Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-13T00:32:21Z- handleDepth()-Depth_Keller msg: 1613176341161 epoch sec; seqNo:13525; depth 23.33 2021-02-13T00:32:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176341.17 epoch sec; seqNo:13525; 4059417.15 north; 592717.22 east 2021-02-13T00:32:21Z- AHRS_M2 msg: 1613176341.56 epoch sec; seqNo:13418 2021-02-13T00:32:21Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.16 psi 2021-02-13T00:32:21Z- handleDepth()-Depth_Keller msg: 1613176341560 epoch sec; seqNo:13526; depth 23.20 2021-02-13T00:32:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176341.57 epoch sec; seqNo:13526; 4059417.53 north; 592717.22 east 2021-02-13T00:32:21Z- AHRS_M2 msg: 1613176341.98 epoch sec; seqNo:13419 2021-02-13T00:32:21Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.16 psi 2021-02-13T00:32:21Z- handleDepth()-Depth_Keller msg: 1613176341984 epoch sec; seqNo:13527; depth 23.08 2021-02-13T00:32:22Z- handleDvl() - RDI_Pathfinder msg: 1613176342.01 epoch sec; seqNo:6575 2021-02-13T00:32:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 89.87 , 103.25 , 66.21 2021-02-13T00:32:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.07 , 0.02 2021-02-13T00:32:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176342.02 epoch sec; seqNo:13527; 4059417.91 north; 592717.21 east 2021-02-13T00:32:22Z- AHRS_M2 msg: 1613176342.36 epoch sec; seqNo:13420 2021-02-13T00:32:22Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.16 psi 2021-02-13T00:32:22Z- handleDepth()-Depth_Keller msg: 1613176342366 epoch sec; seqNo:13528; depth 22.92 2021-02-13T00:32:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176342.38 epoch sec; seqNo:13528; 4059418.30 north; 592718.43 east 2021-02-13T00:32:22Z- AHRS_M2 msg: 1613176342.76 epoch sec; seqNo:13421 2021-02-13T00:32:22Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.17 psi 2021-02-13T00:32:22Z- handleDepth()-Depth_Keller msg: 1613176342768 epoch sec; seqNo:13529; depth 22.80 2021-02-13T00:32:22Z- handleDvl() - RDI_Pathfinder msg: 1613176342.79 epoch sec; seqNo:6576 2021-02-13T00:32:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.78 , 0.00 , 103.12 , 68.25 2021-02-13T00:32:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , 0.02 2021-02-13T00:32:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176342.80 epoch sec; seqNo:13529; 4059418.30 north; 592718.43 east 2021-02-13T00:32:23Z- AHRS_M2 msg: 1613176343.17 epoch sec; seqNo:13422 2021-02-13T00:32:23Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.17 psi 2021-02-13T00:32:23Z- handleDepth()-Depth_Keller msg: 1613176343177 epoch sec; seqNo:13530; depth 22.67 2021-02-13T00:32:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176343.19 epoch sec; seqNo:13530; 4059418.68 north; 592718.42 east 2021-02-13T00:32:23Z- AHRS_M2 msg: 1613176343.57 epoch sec; seqNo:13423 2021-02-13T00:32:23Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.17 psi 2021-02-13T00:32:23Z- handleDepth()-Depth_Keller msg: 1613176343577 epoch sec; seqNo:13531; depth 22.54 2021-02-13T00:32:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176343.59 epoch sec; seqNo:13531; 4059419.06 north; 592718.42 east 2021-02-13T00:32:23Z- AHRS_M2 msg: 1613176343.98 epoch sec; seqNo:13424 2021-02-13T00:32:23Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.17 psi 2021-02-13T00:32:23Z- handleDepth()-Depth_Keller msg: 1613176343980 epoch sec; seqNo:13532; depth 22.42 2021-02-13T00:32:24Z- handleDvl() - RDI_Pathfinder msg: 1613176344.00 epoch sec; seqNo:6577 2021-02-13T00:32:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.38 , 91.02 , 101.97 , 68.25 2021-02-13T00:32:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , 0.05 2021-02-13T00:32:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176344.01 epoch sec; seqNo:13532; 4059419.44 north; 592718.42 east 2021-02-13T00:32:24Z- AHRS_M2 msg: 1613176344.38 epoch sec; seqNo:13425 2021-02-13T00:32:24Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.18 psi 2021-02-13T00:32:24Z- handleDepth()-Depth_Keller msg: 1613176344389 epoch sec; seqNo:13533; depth 22.27 2021-02-13T00:32:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176344.40 epoch sec; seqNo:13533; 4059419.82 north; 592718.41 east 2021-02-13T00:32:24Z- AHRS_M2 msg: 1613176344.78 epoch sec; seqNo:13426 2021-02-13T00:32:24Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.18 psi 2021-02-13T00:32:24Z- handleDepth()-Depth_Keller msg: 1613176344794 epoch sec; seqNo:13534; depth 22.15 2021-02-13T00:32:24Z- handleDvl() - RDI_Pathfinder msg: 1613176344.81 epoch sec; seqNo:6578 2021-02-13T00:32:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.20 , 98.32 , 104.40 , 67.43 2021-02-13T00:32:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.04 , 0.06 2021-02-13T00:32:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176344.82 epoch sec; seqNo:13534; 4059419.83 north; 592719.63 east 2021-02-13T00:32:25Z- AHRS_M2 msg: 1613176345.19 epoch sec; seqNo:13427 2021-02-13T00:32:25Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.18 psi 2021-02-13T00:32:25Z- handleDepth()-Depth_Keller msg: 1613176345197 epoch sec; seqNo:13535; depth 22.02 2021-02-13T00:32:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176345.21 epoch sec; seqNo:13535; 4059420.21 north; 592719.63 east 2021-02-13T00:32:25Z- AHRS_M2 msg: 1613176345.60 epoch sec; seqNo:13428 2021-02-13T00:32:25Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.19 psi 2021-02-13T00:32:25Z- handleDepth()-Depth_Keller msg: 1613176345601 epoch sec; seqNo:13536; depth 21.88 2021-02-13T00:32:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176345.61 epoch sec; seqNo:13536; 4059420.59 north; 592719.62 east 2021-02-13T00:32:25Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-13T00:32:25Z- waiting for 1613176343.36, time is 1613176345.61 2021-02-13T00:32:25Z- LcmTrn::updateTrn() >>>> pose time 1613176345.61, meas time 1613176344.81 <<<< 2021-02-13T00:32:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176344.81 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:32:25Z- TerrainNav::motionUpdate() - currEstimate = (4059421, 592719.62, 21.88) 2021-02-13T00:32:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:32:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:32:25Z- TNavBF::Projecting Measurements 2021-02-13T00:32:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:32:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:32:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:32:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:32:25Z- TNavFilter::4 good beams. 2021-02-13T00:32:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:32:25Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:32:25Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:32:25Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:32:25Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:32:25Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:32:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:32:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:25Z- TerrainMapOctree: (vehN, vehE ) = (4059420.00, 592719.26). 2021-02-13T00:32:25Z- TerrainMapOctree: Min Distance = 2385.81. 2021-02-13T00:32:25Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:32:25Z- There is no specific good beam that all particles have in common. 2021-02-13T00:32:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:32:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 9.05 2021-02-13T00:32:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:32:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 15.47 2021-02-13T00:32:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:32:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 17.44 2021-02-13T00:32:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:32:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 7.28 2021-02-13T00:32:25Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:32:25Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:32:25Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:32:25Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:32:25Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:32:25Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:32:25Z- Filter 0 No beams used in subcloud update 2021-02-13T00:32:25Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:32:25Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:32:25Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 698.3 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:32:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176344.81 sec. 2021-02-13T00:32:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:32:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:25Z- variances([0], [2], and psi_berg) = 698.262288 832.997037 0.000000 2021-02-13T00:32:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:32:25Z- LcmTrn::publishEstimates() - MLE : 4059420.09 592691.12 21.88 2021-02-13T00:32:25Z- LcmTrn::publishEstimates() - MMSE : 4059420.59 592719.62 21.88 2021-02-13T00:32:25Z- LcmTrn::publishEstimates() - COVAR: 698.26 833.00 0.78 0.00 2021-02-13T00:32:25Z- AHRS_M2 msg: 1613176346.00 epoch sec; seqNo:13429 2021-02-13T00:32:25Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.19 psi 2021-02-13T00:32:26Z- handleDepth()-Depth_Keller msg: 1613176346001 epoch sec; seqNo:13537; depth 21.76 2021-02-13T00:32:26Z- handleDvl() - RDI_Pathfinder msg: 1613176346.01 epoch sec; seqNo:6579 2021-02-13T00:32:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 104.13 , 67.03 2021-02-13T00:32:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.07 , 0.03 2021-02-13T00:32:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176346.02 epoch sec; seqNo:13537; 4059420.59 north; 592719.62 east 2021-02-13T00:32:26Z- AHRS_M2 msg: 1613176346.40 epoch sec; seqNo:13430 2021-02-13T00:32:26Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.19 psi 2021-02-13T00:32:26Z- handleDepth()-Depth_Keller msg: 1613176346406 epoch sec; seqNo:13538; depth 21.61 2021-02-13T00:32:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176346.42 epoch sec; seqNo:13538; 4059420.97 north; 592719.62 east 2021-02-13T00:32:26Z- AHRS_M2 msg: 1613176346.83 epoch sec; seqNo:13431 2021-02-13T00:32:26Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.20 psi 2021-02-13T00:32:26Z- handleDepth()-Depth_Keller msg: 1613176346834 epoch sec; seqNo:13539; depth 21.48 2021-02-13T00:32:26Z- handleDvl() - RDI_Pathfinder msg: 1613176346.85 epoch sec; seqNo:6580 2021-02-13T00:32:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 105.35 , 67.03 2021-02-13T00:32:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , -0.02 2021-02-13T00:32:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176346.86 epoch sec; seqNo:13539; 4059421.35 north; 592719.62 east 2021-02-13T00:32:27Z- AHRS_M2 msg: 1613176347.21 epoch sec; seqNo:13432 2021-02-13T00:32:27Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.20 psi 2021-02-13T00:32:27Z- handleDepth()-Depth_Keller msg: 1613176347212 epoch sec; seqNo:13540; depth 21.33 2021-02-13T00:32:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176347.22 epoch sec; seqNo:13540; 4059421.36 north; 592720.84 east 2021-02-13T00:32:27Z- AHRS_M2 msg: 1613176347.61 epoch sec; seqNo:13433 2021-02-13T00:32:27Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.20 psi 2021-02-13T00:32:27Z- handleDepth()-Depth_Keller msg: 1613176347616 epoch sec; seqNo:13541; depth 21.21 2021-02-13T00:32:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176347.62 epoch sec; seqNo:13541; 4059421.74 north; 592720.83 east 2021-02-13T00:32:28Z- AHRS_M2 msg: 1613176348.02 epoch sec; seqNo:13434 2021-02-13T00:32:28Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.21 psi 2021-02-13T00:32:28Z- handleDepth()-Depth_Keller msg: 1613176348020 epoch sec; seqNo:13542; depth 21.09 2021-02-13T00:32:28Z- handleDvl() - RDI_Pathfinder msg: 1613176348.04 epoch sec; seqNo:6581 2021-02-13T00:32:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.20 , 97.98 , 106.50 , 67.84 2021-02-13T00:32:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , -0.04 2021-02-13T00:32:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176348.05 epoch sec; seqNo:13542; 4059422.12 north; 592720.83 east 2021-02-13T00:32:28Z- AHRS_M2 msg: 1613176348.42 epoch sec; seqNo:13435 2021-02-13T00:32:28Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.21 psi 2021-02-13T00:32:28Z- handleDepth()-Depth_Keller msg: 1613176348424 epoch sec; seqNo:13543; depth 20.93 2021-02-13T00:32:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176348.43 epoch sec; seqNo:13543; 4059422.12 north; 592720.83 east 2021-02-13T00:32:28Z- AHRS_M2 msg: 1613176348.82 epoch sec; seqNo:13436 2021-02-13T00:32:28Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.21 psi 2021-02-13T00:32:28Z- handleDepth()-Depth_Keller msg: 1613176348828 epoch sec; seqNo:13544; depth 20.81 2021-02-13T00:32:28Z- handleDvl() - RDI_Pathfinder msg: 1613176348.84 epoch sec; seqNo:6582 2021-02-13T00:32:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.97 , 95.61 , 106.57 , 70.29 2021-02-13T00:32:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , -0.00 2021-02-13T00:32:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176348.85 epoch sec; seqNo:13544; 4059422.50 north; 592720.82 east 2021-02-13T00:32:29Z- AHRS_M2 msg: 1613176349.23 epoch sec; seqNo:13437 2021-02-13T00:32:29Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.20 psi 2021-02-13T00:32:29Z- handleDepth()-Depth_Keller msg: 1613176349237 epoch sec; seqNo:13545; depth 20.68 2021-02-13T00:32:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176349.25 epoch sec; seqNo:13545; 4059422.87 north; 592720.82 east 2021-02-13T00:32:29Z- AHRS_M2 msg: 1613176349.63 epoch sec; seqNo:13438 2021-02-13T00:32:29Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.20 psi 2021-02-13T00:32:29Z- handleDepth()-Depth_Keller msg: 1613176349641 epoch sec; seqNo:13546; depth 20.55 2021-02-13T00:32:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176349.65 epoch sec; seqNo:13546; 4059423.25 north; 592720.82 east 2021-02-13T00:32:30Z- AHRS_M2 msg: 1613176350.04 epoch sec; seqNo:13439 2021-02-13T00:32:30Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.20 psi 2021-02-13T00:32:30Z- handleDepth()-Depth_Keller msg: 1613176350040 epoch sec; seqNo:13547; depth 20.41 2021-02-13T00:32:30Z- handleDvl() - RDI_Pathfinder msg: 1613176350.05 epoch sec; seqNo:6583 2021-02-13T00:32:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 95.61 , 105.35 , 68.66 2021-02-13T00:32:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.10 , 0.05 2021-02-13T00:32:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176350.06 epoch sec; seqNo:13547; 4059423.65 north; 592722.03 east 2021-02-13T00:32:30Z- AHRS_M2 msg: 1613176350.44 epoch sec; seqNo:13440 2021-02-13T00:32:30Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.19 psi 2021-02-13T00:32:30Z- handleDepth()-Depth_Keller msg: 1613176350450 epoch sec; seqNo:13548; depth 20.28 2021-02-13T00:32:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176350.46 epoch sec; seqNo:13548; 4059423.65 north; 592722.03 east 2021-02-13T00:32:30Z- LcmTrn::handleMessages() - handling 41 messages took 0.56 ms and 130 calls... 2021-02-13T00:32:30Z- waiting for 1613176348.61, time is 1613176350.46 2021-02-13T00:32:30Z- LcmTrn::updateTrn() >>>> pose time 1613176350.46, meas time 1613176350.05 <<<< 2021-02-13T00:32:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176350.05 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:32:30Z- TerrainNav::motionUpdate() - currEstimate = (4059424, 592722.03, 20.28) 2021-02-13T00:32:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:32:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:32:30Z- TNavBF::Projecting Measurements 2021-02-13T00:32:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:32:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:32:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:32:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:32:30Z- TNavFilter::4 good beams. 2021-02-13T00:32:30Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:32:30Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:32:30Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:32:30Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:32:30Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:32:30Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:32:30Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:32:30Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:30Z- TerrainMapOctree: (vehN, vehE ) = (4059423.39, 592721.83). 2021-02-13T00:32:30Z- TerrainMapOctree: Min Distance = 2381.56. 2021-02-13T00:32:30Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:32:30Z- There is no specific good beam that all particles have in common. 2021-02-13T00:32:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:32:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-13T00:32:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:32:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 14.63 2021-02-13T00:32:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:32:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 17.76 2021-02-13T00:32:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:32:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 7.54 2021-02-13T00:32:30Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:32:30Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:32:30Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:32:30Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:32:30Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:32:30Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:32:30Z- Filter 0 No beams used in subcloud update 2021-02-13T00:32:30Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:32:30Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:32:30Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 698.2 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:32:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176350.05 sec. 2021-02-13T00:32:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:32:30Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:30Z- variances([0], [2], and psi_berg) = 698.196914 832.997004 0.000000 2021-02-13T00:32:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:32:30Z- LcmTrn::publishEstimates() - MLE : 4059423.15 592693.53 20.28 2021-02-13T00:32:30Z- LcmTrn::publishEstimates() - MMSE : 4059423.65 592722.03 20.28 2021-02-13T00:32:30Z- LcmTrn::publishEstimates() - COVAR: 698.20 833.00 0.76 0.00 2021-02-13T00:32:30Z- AHRS_M2 msg: 1613176350.84 epoch sec; seqNo:13441 2021-02-13T00:32:30Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.18 psi 2021-02-13T00:32:30Z- handleDepth()-Depth_Keller msg: 1613176350848 epoch sec; seqNo:13549; depth 20.17 2021-02-13T00:32:30Z- handleDvl() - RDI_Pathfinder msg: 1613176350.87 epoch sec; seqNo:6584 2021-02-13T00:32:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 108.93 , 69.07 2021-02-13T00:32:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.03 , 0.10 2021-02-13T00:32:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176350.88 epoch sec; seqNo:13549; 4059424.02 north; 592722.03 east 2021-02-13T00:32:31Z- AHRS_M2 msg: 1613176351.25 epoch sec; seqNo:13442 2021-02-13T00:32:31Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.18 psi 2021-02-13T00:32:31Z- handleDepth()-Depth_Keller msg: 1613176351257 epoch sec; seqNo:13550; depth 20.02 2021-02-13T00:32:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176351.27 epoch sec; seqNo:13550; 4059424.40 north; 592722.02 east 2021-02-13T00:32:31Z- AHRS_M2 msg: 1613176351.65 epoch sec; seqNo:13443 2021-02-13T00:32:31Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.17 psi 2021-02-13T00:32:31Z- handleDepth()-Depth_Keller msg: 1613176351656 epoch sec; seqNo:13551; depth 19.90 2021-02-13T00:32:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176351.66 epoch sec; seqNo:13551; 4059424.78 north; 592722.02 east 2021-02-13T00:32:32Z- AHRS_M2 msg: 1613176352.06 epoch sec; seqNo:13444 2021-02-13T00:32:32Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.17 psi 2021-02-13T00:32:32Z- handleDepth()-Depth_Keller msg: 1613176352070 epoch sec; seqNo:13552; depth 19.78 2021-02-13T00:32:32Z- handleDvl() - RDI_Pathfinder msg: 1613176352.08 epoch sec; seqNo:6585 2021-02-13T00:32:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.37 , 94.19 , 107.58 , 69.89 2021-02-13T00:32:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.03 , 0.10 2021-02-13T00:32:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176352.09 epoch sec; seqNo:13552; 4059425.16 north; 592722.02 east 2021-02-13T00:32:32Z- AHRS_M2 msg: 1613176352.46 epoch sec; seqNo:13445 2021-02-13T00:32:32Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.17 psi 2021-02-13T00:32:32Z- handleDepth()-Depth_Keller msg: 1613176352465 epoch sec; seqNo:13553; depth 19.64 2021-02-13T00:32:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176352.48 epoch sec; seqNo:13553; 4059425.17 north; 592723.24 east 2021-02-13T00:32:32Z- AHRS_M2 msg: 1613176352.86 epoch sec; seqNo:13446 2021-02-13T00:32:32Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.16 psi 2021-02-13T00:32:32Z- handleDepth()-Depth_Keller msg: 1613176352873 epoch sec; seqNo:13554; depth 19.51 2021-02-13T00:32:32Z- handleDvl() - RDI_Pathfinder msg: 1613176352.88 epoch sec; seqNo:6586 2021-02-13T00:32:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 94.33 , 108.93 , 69.48 2021-02-13T00:32:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.07 , 0.06 2021-02-13T00:32:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176352.89 epoch sec; seqNo:13554; 4059425.55 north; 592723.23 east 2021-02-13T00:32:33Z- AHRS_M2 msg: 1613176353.27 epoch sec; seqNo:13447 2021-02-13T00:32:33Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.16 psi 2021-02-13T00:32:33Z- handleDepth()-Depth_Keller msg: 1613176353274 epoch sec; seqNo:13555; depth 19.36 2021-02-13T00:32:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176353.29 epoch sec; seqNo:13555; 4059425.93 north; 592723.23 east 2021-02-13T00:32:33Z- AHRS_M2 msg: 1613176353.67 epoch sec; seqNo:13448 2021-02-13T00:32:33Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-13T00:32:33Z- handleDepth()-Depth_Keller msg: 1613176353681 epoch sec; seqNo:13556; depth 19.25 2021-02-13T00:32:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176353.70 epoch sec; seqNo:13556; 4059425.93 north; 592723.23 east 2021-02-13T00:32:34Z- AHRS_M2 msg: 1613176354.08 epoch sec; seqNo:13449 2021-02-13T00:32:34Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-13T00:32:34Z- handleDepth()-Depth_Keller msg: 1613176354080 epoch sec; seqNo:13557; depth 19.11 2021-02-13T00:32:34Z- handleDvl() - RDI_Pathfinder msg: 1613176354.09 epoch sec; seqNo:6587 2021-02-13T00:32:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.06 , 95.55 , 110.15 , 70.29 2021-02-13T00:32:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , -0.02 2021-02-13T00:32:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176354.12 epoch sec; seqNo:13557; 4059426.31 north; 592723.22 east 2021-02-13T00:32:34Z- AHRS_M2 msg: 1613176354.48 epoch sec; seqNo:13450 2021-02-13T00:32:34Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-13T00:32:34Z- handleDepth()-Depth_Keller msg: 1613176354485 epoch sec; seqNo:13558; depth 18.98 2021-02-13T00:32:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176354.50 epoch sec; seqNo:13558; 4059426.69 north; 592723.22 east 2021-02-13T00:32:34Z- AHRS_M2 msg: 1613176354.88 epoch sec; seqNo:13451 2021-02-13T00:32:34Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.17 psi 2021-02-13T00:32:34Z- handleDepth()-Depth_Keller msg: 1613176354890 epoch sec; seqNo:13559; depth 18.84 2021-02-13T00:32:34Z- handleDvl() - RDI_Pathfinder msg: 1613176354.90 epoch sec; seqNo:6588 2021-02-13T00:32:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.06 , 94.26 , 108.86 , 71.11 2021-02-13T00:32:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , -0.05 2021-02-13T00:32:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176354.91 epoch sec; seqNo:13559; 4059426.70 north; 592724.44 east 2021-02-13T00:32:35Z- AHRS_M2 msg: 1613176355.29 epoch sec; seqNo:13452 2021-02-13T00:32:35Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.18 psi 2021-02-13T00:32:35Z- handleDepth()-Depth_Keller msg: 1613176355292 epoch sec; seqNo:13560; depth 18.71 2021-02-13T00:32:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176355.31 epoch sec; seqNo:13560; 4059427.08 north; 592724.44 east 2021-02-13T00:32:35Z- AHRS_M2 msg: 1613176355.69 epoch sec; seqNo:13453 2021-02-13T00:32:35Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.19 psi 2021-02-13T00:32:35Z- handleDepth()-Depth_Keller msg: 1613176355696 epoch sec; seqNo:13561; depth 18.59 2021-02-13T00:32:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176355.70 epoch sec; seqNo:13561; 4059427.46 north; 592724.43 east 2021-02-13T00:32:35Z- LcmTrn::handleMessages() - handling 44 messages took 0.60 ms and 132 calls... 2021-02-13T00:32:35Z- waiting for 1613176353.46, time is 1613176355.70 2021-02-13T00:32:35Z- LcmTrn::updateTrn() >>>> pose time 1613176355.70, meas time 1613176354.90 <<<< 2021-02-13T00:32:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176354.90 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:32:35Z- TerrainNav::motionUpdate() - currEstimate = (4059427, 592724.43, 18.59) 2021-02-13T00:32:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:32:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:32:35Z- TNavBF::Projecting Measurements 2021-02-13T00:32:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:32:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:32:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:32:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:32:35Z- TNavFilter::4 good beams. 2021-02-13T00:32:35Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:32:35Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:32:35Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:32:35Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:32:35Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:32:35Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:32:35Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:32:35Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:35Z- TerrainMapOctree: (vehN, vehE ) = (4059426.88, 592724.07). 2021-02-13T00:32:35Z- TerrainMapOctree: Min Distance = 2377.45. 2021-02-13T00:32:35Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:32:35Z- There is no specific good beam that all particles have in common. 2021-02-13T00:32:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:32:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 9.75 2021-02-13T00:32:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:32:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 14.22 2021-02-13T00:32:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:32:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 18.96 2021-02-13T00:32:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:32:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-13T00:32:35Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:32:35Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:32:35Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:32:35Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:32:35Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:32:35Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:32:35Z- Filter 0 No beams used in subcloud update 2021-02-13T00:32:35Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:32:35Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:32:35Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 698.1 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:32:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176354.90 sec. 2021-02-13T00:32:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:32:35Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:35Z- variances([0], [2], and psi_berg) = 698.123005 832.996966 0.000000 2021-02-13T00:32:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:32:35Z- LcmTrn::publishEstimates() - MLE : 4059426.96 592695.93 18.59 2021-02-13T00:32:35Z- LcmTrn::publishEstimates() - MMSE : 4059427.46 592724.43 18.59 2021-02-13T00:32:35Z- LcmTrn::publishEstimates() - COVAR: 698.12 833.00 0.78 0.00 2021-02-13T00:32:36Z- AHRS_M2 msg: 1613176356.10 epoch sec; seqNo:13454 2021-02-13T00:32:36Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-13T00:32:36Z- handleDepth()-Depth_Keller msg: 1613176356105 epoch sec; seqNo:13562; depth 18.46 2021-02-13T00:32:36Z- handleDvl() - RDI_Pathfinder msg: 1613176356.12 epoch sec; seqNo:6589 2021-02-13T00:32:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 96.70 , 107.65 , 71.52 2021-02-13T00:32:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.15 , 0.01 2021-02-13T00:32:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176356.13 epoch sec; seqNo:13562; 4059427.84 north; 592724.43 east 2021-02-13T00:32:36Z- AHRS_M2 msg: 1613176356.50 epoch sec; seqNo:13455 2021-02-13T00:32:36Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.20 psi 2021-02-13T00:32:36Z- handleDepth()-Depth_Keller msg: 1613176356505 epoch sec; seqNo:13563; depth 18.32 2021-02-13T00:32:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176356.52 epoch sec; seqNo:13563; 4059428.22 north; 592724.42 east 2021-02-13T00:32:36Z- AHRS_M2 msg: 1613176356.90 epoch sec; seqNo:13456 2021-02-13T00:32:36Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.20 psi 2021-02-13T00:32:36Z- handleDepth()-Depth_Keller msg: 1613176356910 epoch sec; seqNo:13564; depth 18.20 2021-02-13T00:32:36Z- handleDvl() - RDI_Pathfinder msg: 1613176356.93 epoch sec; seqNo:6590 2021-02-13T00:32:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 108.86 , 70.70 2021-02-13T00:32:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , 0.11 2021-02-13T00:32:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176356.94 epoch sec; seqNo:13564; 4059428.22 north; 592724.42 east 2021-02-13T00:32:37Z- AHRS_M2 msg: 1613176357.31 epoch sec; seqNo:13457 2021-02-13T00:32:37Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.20 psi 2021-02-13T00:32:37Z- handleDepth()-Depth_Keller msg: 1613176357317 epoch sec; seqNo:13565; depth 18.07 2021-02-13T00:32:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:32:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:32:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:32:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:32:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:32:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:32:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:32:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:32:37Z- handleDvl() - RDI_Pathfinder msg: 1613176357.32 epoch sec; seqNo:6591 2021-02-13T00:32:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:32:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , 0.11 2021-02-13T00:32:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176357.35 epoch sec; seqNo:13565; 4059428.60 north; 592724.42 east 2021-02-13T00:32:37Z- AHRS_M2 msg: 1613176357.71 epoch sec; seqNo:13458 2021-02-13T00:32:37Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.20 psi 2021-02-13T00:32:37Z- handleDepth()-Depth_Keller msg: 1613176357716 epoch sec; seqNo:13566; depth 17.96 2021-02-13T00:32:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176357.72 epoch sec; seqNo:13566; 4059428.98 north; 592724.42 east 2021-02-13T00:32:38Z- AHRS_M2 msg: 1613176358.12 epoch sec; seqNo:13459 2021-02-13T00:32:38Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.20 psi 2021-02-13T00:32:38Z- handleDepth()-Depth_Keller msg: 1613176358127 epoch sec; seqNo:13567; depth 17.84 2021-02-13T00:32:38Z- handleDvl() - RDI_Pathfinder msg: 1613176358.14 epoch sec; seqNo:6592 2021-02-13T00:32:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.37 , 97.85 , 108.80 , 71.93 2021-02-13T00:32:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.00 , 0.13 2021-02-13T00:32:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176358.15 epoch sec; seqNo:13567; 4059429.37 north; 592725.63 east 2021-02-13T00:32:38Z- AHRS_M2 msg: 1613176358.53 epoch sec; seqNo:13460 2021-02-13T00:32:38Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.19 psi 2021-02-13T00:32:38Z- handleDepth()-Depth_Keller msg: 1613176358535 epoch sec; seqNo:13568; depth 17.71 2021-02-13T00:32:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:32:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:32:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:32:38Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:32:38Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:32:38Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:32:38Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:32:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:32:38Z- handleDvl() - RDI_Pathfinder msg: 1613176358.54 epoch sec; seqNo:6593 2021-02-13T00:32:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:32:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.00 , 0.13 2021-02-13T00:32:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176358.55 epoch sec; seqNo:13568; 4059429.75 north; 592725.63 east 2021-02-13T00:32:38Z- AHRS_M2 msg: 1613176358.93 epoch sec; seqNo:13461 2021-02-13T00:32:38Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.18 psi 2021-02-13T00:32:38Z- handleDepth()-Depth_Keller msg: 1613176358930 epoch sec; seqNo:13569; depth 17.60 2021-02-13T00:32:38Z- handleDvl() - RDI_Pathfinder msg: 1613176358.94 epoch sec; seqNo:6594 2021-02-13T00:32:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.59 , 97.85 , 110.01 , 78.37 2021-02-13T00:32:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.10 , 0.05 2021-02-13T00:32:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176358.97 epoch sec; seqNo:13569; 4059429.75 north; 592725.63 east 2021-02-13T00:32:39Z- AHRS_M2 msg: 1613176359.33 epoch sec; seqNo:13462 2021-02-13T00:32:39Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.17 psi 2021-02-13T00:32:39Z- handleDepth()-Depth_Keller msg: 1613176359332 epoch sec; seqNo:13570; depth 17.44 2021-02-13T00:32:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176359.34 epoch sec; seqNo:13570; 4059430.13 north; 592725.63 east 2021-02-13T00:32:39Z- AHRS_M2 msg: 1613176359.73 epoch sec; seqNo:13463 2021-02-13T00:32:39Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-13T00:32:39Z- handleDepth()-Depth_Keller msg: 1613176359736 epoch sec; seqNo:13571; depth 17.34 2021-02-13T00:32:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:32:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:32:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:32:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:32:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:32:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:32:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:32:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:32:39Z- handleDvl() - RDI_Pathfinder msg: 1613176359.74 epoch sec; seqNo:6595 2021-02-13T00:32:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:32:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.10 , 0.05 2021-02-13T00:32:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176359.75 epoch sec; seqNo:13571; 4059430.50 north; 592725.62 east 2021-02-13T00:32:40Z- AHRS_M2 msg: 1613176360.14 epoch sec; seqNo:13464 2021-02-13T00:32:40Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-13T00:32:40Z- handleDepth()-Depth_Keller msg: 1613176360144 epoch sec; seqNo:13572; depth 17.21 2021-02-13T00:32:40Z- handleDvl() - RDI_Pathfinder msg: 1613176360.16 epoch sec; seqNo:6596 2021-02-13T00:32:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 97.64 , 113.46 , 71.11 2021-02-13T00:32:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.08 , -0.08 2021-02-13T00:32:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176360.17 epoch sec; seqNo:13572; 4059430.52 north; 592726.84 east 2021-02-13T00:32:40Z- AHRS_M2 msg: 1613176360.54 epoch sec; seqNo:13465 2021-02-13T00:32:40Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-13T00:32:40Z- handleDepth()-Depth_Keller msg: 1613176360549 epoch sec; seqNo:13573; depth 17.08 2021-02-13T00:32:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176360.56 epoch sec; seqNo:13573; 4059430.90 north; 592726.84 east 2021-02-13T00:32:40Z- AHRS_M2 msg: 1613176360.94 epoch sec; seqNo:13466 2021-02-13T00:32:40Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.18 psi 2021-02-13T00:32:40Z- handleDepth()-Depth_Keller msg: 1613176360953 epoch sec; seqNo:13574; depth 16.96 2021-02-13T00:32:40Z- handleDvl() - RDI_Pathfinder msg: 1613176360.96 epoch sec; seqNo:6597 2021-02-13T00:32:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.67 , 97.71 , 108.66 , 78.24 2021-02-13T00:32:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , -0.13 2021-02-13T00:32:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176360.97 epoch sec; seqNo:13574; 4059431.28 north; 592726.83 east 2021-02-13T00:32:41Z- LcmTrn::handleMessages() - handling 48 messages took 0.74 ms and 134 calls... 2021-02-13T00:32:41Z- waiting for 1613176358.70, time is 1613176360.97 2021-02-13T00:32:41Z- LcmTrn::updateTrn() equating pose 1613176360.97 and meas times 1613176360.97 2021-02-13T00:32:41Z- LcmTrn::updateTrn() >>>> pose time 1613176360.97, meas time 1613176360.97 <<<< 2021-02-13T00:32:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176360.97 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:32:41Z- TerrainNav::motionUpdate() - currEstimate = (4059431, 592726.83, 16.96) 2021-02-13T00:32:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:32:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:32:41Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176360.97 sec.; No relevant navigation data available 2021-02-13T00:32:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:32:41Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:41Z- variances([0], [2], and psi_berg) = 698.066690 832.996937 0.000000 2021-02-13T00:32:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:32:41Z- LcmTrn::publishEstimates() - MLE : 4059430.78 592698.33 16.96 2021-02-13T00:32:41Z- LcmTrn::publishEstimates() - MMSE : 4059431.28 592726.83 16.96 2021-02-13T00:32:41Z- LcmTrn::publishEstimates() - COVAR: 698.07 833.00 1.01 0.00 2021-02-13T00:32:41Z- AHRS_M2 msg: 1613176361.35 epoch sec; seqNo:13467 2021-02-13T00:32:41Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.18 psi 2021-02-13T00:32:41Z- handleDepth()-Depth_Keller msg: 1613176361355 epoch sec; seqNo:13575; depth 16.85 2021-02-13T00:32:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176361.37 epoch sec; seqNo:13575; 4059431.28 north; 592726.83 east 2021-02-13T00:32:41Z- AHRS_M2 msg: 1613176361.77 epoch sec; seqNo:13468 2021-02-13T00:32:41Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-13T00:32:41Z- handleDepth()-Depth_Keller msg: 1613176361772 epoch sec; seqNo:13576; depth 16.73 2021-02-13T00:32:41Z- handleDvl() - RDI_Pathfinder msg: 1613176361.78 epoch sec; seqNo:6598 2021-02-13T00:32:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.47 , 96.43 , 109.81 , 72.75 2021-02-13T00:32:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.12 , -0.09 2021-02-13T00:32:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176361.79 epoch sec; seqNo:13576; 4059431.65 north; 592726.83 east 2021-02-13T00:32:42Z- AHRS_M2 msg: 1613176362.16 epoch sec; seqNo:13469 2021-02-13T00:32:42Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-13T00:32:42Z- handleDepth()-Depth_Keller msg: 1613176362162 epoch sec; seqNo:13577; depth 16.59 2021-02-13T00:32:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176362.17 epoch sec; seqNo:13577; 4059432.03 north; 592726.83 east 2021-02-13T00:32:42Z- AHRS_M2 msg: 1613176362.56 epoch sec; seqNo:13470 2021-02-13T00:32:42Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-13T00:32:42Z- handleDepth()-Depth_Keller msg: 1613176362565 epoch sec; seqNo:13578; depth 16.48 2021-02-13T00:32:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176362.58 epoch sec; seqNo:13578; 4059432.03 north; 592726.83 east 2021-02-13T00:32:42Z- AHRS_M2 msg: 1613176362.97 epoch sec; seqNo:13471 2021-02-13T00:32:42Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-13T00:32:42Z- handleDepth()-Depth_Keller msg: 1613176362970 epoch sec; seqNo:13579; depth 16.36 2021-02-13T00:32:42Z- handleDvl() - RDI_Pathfinder msg: 1613176362.98 epoch sec; seqNo:6599 2021-02-13T00:32:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.81 , 97.85 , 108.80 , 73.56 2021-02-13T00:32:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.16 , 0.02 2021-02-13T00:32:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176362.99 epoch sec; seqNo:13579; 4059432.41 north; 592726.82 east 2021-02-13T00:32:43Z- AHRS_M2 msg: 1613176363.37 epoch sec; seqNo:13472 2021-02-13T00:32:43Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:32:43Z- handleDepth()-Depth_Keller msg: 1613176363382 epoch sec; seqNo:13580; depth 16.24 2021-02-13T00:32:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176363.39 epoch sec; seqNo:13580; 4059432.80 north; 592728.04 east 2021-02-13T00:32:43Z- AHRS_M2 msg: 1613176363.77 epoch sec; seqNo:13473 2021-02-13T00:32:43Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-13T00:32:43Z- handleDepth()-Depth_Keller msg: 1613176363781 epoch sec; seqNo:13581; depth 16.13 2021-02-13T00:32:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176363.79 epoch sec; seqNo:13581; 4059433.18 north; 592728.03 east 2021-02-13T00:32:44Z- AHRS_M2 msg: 1613176364.18 epoch sec; seqNo:13474 2021-02-13T00:32:44Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-13T00:32:44Z- handleDepth()-Depth_Keller msg: 1613176364185 epoch sec; seqNo:13582; depth 16.02 2021-02-13T00:32:44Z- handleDvl() - RDI_Pathfinder msg: 1613176364.20 epoch sec; seqNo:6600 2021-02-13T00:32:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 97.71 , 107.45 , 72.75 2021-02-13T00:32:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.13 , 0.11 2021-02-13T00:32:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176364.21 epoch sec; seqNo:13582; 4059433.18 north; 592728.03 east 2021-02-13T00:32:44Z- AHRS_M2 msg: 1613176364.58 epoch sec; seqNo:13475 2021-02-13T00:32:44Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-13T00:32:44Z- handleDepth()-Depth_Keller msg: 1613176364586 epoch sec; seqNo:13583; depth 15.90 2021-02-13T00:32:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176364.60 epoch sec; seqNo:13583; 4059433.56 north; 592728.03 east 2021-02-13T00:32:44Z- AHRS_M2 msg: 1613176364.98 epoch sec; seqNo:13476 2021-02-13T00:32:44Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.17 psi 2021-02-13T00:32:44Z- handleDepth()-Depth_Keller msg: 1613176364989 epoch sec; seqNo:13584; depth 15.77 2021-02-13T00:32:45Z- handleDvl() - RDI_Pathfinder msg: 1613176365.02 epoch sec; seqNo:6601 2021-02-13T00:32:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 97.78 , 107.51 , 73.56 2021-02-13T00:32:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , 0.14 2021-02-13T00:32:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176365.03 epoch sec; seqNo:13584; 4059433.94 north; 592728.03 east 2021-02-13T00:32:45Z- AHRS_M2 msg: 1613176365.39 epoch sec; seqNo:13477 2021-02-13T00:32:45Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T00:32:45Z- handleDepth()-Depth_Keller msg: 1613176365397 epoch sec; seqNo:13585; depth 15.63 2021-02-13T00:32:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176365.41 epoch sec; seqNo:13585; 4059434.32 north; 592728.02 east 2021-02-13T00:32:45Z- AHRS_M2 msg: 1613176365.79 epoch sec; seqNo:13478 2021-02-13T00:32:45Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.18 psi 2021-02-13T00:32:45Z- handleDepth()-Depth_Keller msg: 1613176365801 epoch sec; seqNo:13586; depth 15.55 2021-02-13T00:32:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176365.81 epoch sec; seqNo:13586; 4059434.70 north; 592728.02 east 2021-02-13T00:32:46Z- LcmTrn::handleMessages() - handling 40 messages took 0.61 ms and 128 calls... 2021-02-13T00:32:46Z- waiting for 1613176363.97, time is 1613176365.81 2021-02-13T00:32:46Z- LcmTrn::updateTrn() >>>> pose time 1613176365.81, meas time 1613176365.02 <<<< 2021-02-13T00:32:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176365.02 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:32:46Z- TerrainNav::motionUpdate() - currEstimate = (4059435, 592728.02, 15.55) 2021-02-13T00:32:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:32:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:32:46Z- TNavBF::Projecting Measurements 2021-02-13T00:32:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:32:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:32:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:32:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:32:46Z- TNavFilter::4 good beams. 2021-02-13T00:32:46Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:32:46Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:32:46Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:32:46Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:32:46Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:32:46Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:32:46Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:32:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:46Z- TerrainMapOctree: (vehN, vehE ) = (4059434.14, 592727.82). 2021-02-13T00:32:46Z- TerrainMapOctree: Min Distance = 2369.47. 2021-02-13T00:32:46Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:32:46Z- There is no specific good beam that all particles have in common. 2021-02-13T00:32:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:32:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 10.75 2021-02-13T00:32:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:32:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-13T00:32:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:32:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 18.49 2021-02-13T00:32:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:32:46Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 8.66 2021-02-13T00:32:46Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:32:46Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:32:46Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:32:46Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:32:46Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:32:46Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:32:46Z- Filter 0 No beams used in subcloud update 2021-02-13T00:32:46Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:32:46Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:32:46Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 698.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:32:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176365.02 sec. 2021-02-13T00:32:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:32:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:46Z- variances([0], [2], and psi_berg) = 698.000138 832.996902 0.000000 2021-02-13T00:32:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:32:46Z- LcmTrn::publishEstimates() - MLE : 4059434.20 592761.52 15.55 2021-02-13T00:32:46Z- LcmTrn::publishEstimates() - MMSE : 4059434.70 592728.02 15.55 2021-02-13T00:32:46Z- LcmTrn::publishEstimates() - COVAR: 698.00 833.00 0.90 0.00 2021-02-13T00:32:46Z- AHRS_M2 msg: 1613176366.20 epoch sec; seqNo:13479 2021-02-13T00:32:46Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.18 psi 2021-02-13T00:32:46Z- handleDepth()-Depth_Keller msg: 1613176366205 epoch sec; seqNo:13587; depth 15.42 2021-02-13T00:32:46Z- handleDvl() - RDI_Pathfinder msg: 1613176366.22 epoch sec; seqNo:6602 2021-02-13T00:32:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 96.49 , 109.88 , 78.24 2021-02-13T00:32:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.01 , 0.15 2021-02-13T00:32:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176366.23 epoch sec; seqNo:13587; 4059434.70 north; 592728.02 east 2021-02-13T00:32:46Z- AHRS_M2 msg: 1613176366.60 epoch sec; seqNo:13480 2021-02-13T00:32:46Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-13T00:32:46Z- handleDepth()-Depth_Keller msg: 1613176366605 epoch sec; seqNo:13588; depth 15.32 2021-02-13T00:32:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176366.63 epoch sec; seqNo:13588; 4059435.09 north; 592729.23 east 2021-02-13T00:32:47Z- AHRS_M2 msg: 1613176367.01 epoch sec; seqNo:13481 2021-02-13T00:32:47Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-13T00:32:47Z- handleDepth()-Depth_Keller msg: 1613176367017 epoch sec; seqNo:13589; depth 15.21 2021-02-13T00:32:47Z- handleDvl() - RDI_Pathfinder msg: 1613176367.05 epoch sec; seqNo:6603 2021-02-13T00:32:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 97.64 , 108.60 , 73.56 2021-02-13T00:32:47Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.10 , 0.10 2021-02-13T00:32:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176367.06 epoch sec; seqNo:13589; 4059435.47 north; 592729.23 east 2021-02-13T00:32:47Z- AHRS_M2 msg: 1613176367.41 epoch sec; seqNo:13482 2021-02-13T00:32:47Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-13T00:32:47Z- handleDepth()-Depth_Keller msg: 1613176367412 epoch sec; seqNo:13590; depth 15.10 2021-02-13T00:32:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176367.42 epoch sec; seqNo:13590; 4059435.85 north; 592729.23 east 2021-02-13T00:32:47Z- AHRS_M2 msg: 1613176367.83 epoch sec; seqNo:13483 2021-02-13T00:32:47Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.14 psi 2021-02-13T00:32:47Z- handleDepth()-Depth_Keller msg: 1613176367841 epoch sec; seqNo:13591; depth 15.00 2021-02-13T00:32:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176367.90 epoch sec; seqNo:13591; 4059436.23 north; 592729.22 east 2021-02-13T00:32:48Z- AHRS_M2 msg: 1613176368.22 epoch sec; seqNo:13484 2021-02-13T00:32:48Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.14 psi 2021-02-13T00:32:48Z- handleDepth()-Depth_Keller msg: 1613176368220 epoch sec; seqNo:13592; depth 14.87 2021-02-13T00:32:48Z- handleDvl() - RDI_Pathfinder msg: 1613176368.23 epoch sec; seqNo:6604 2021-02-13T00:32:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.32 , 96.49 , 111.10 , 78.24 2021-02-13T00:32:48Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.12 , -0.04 2021-02-13T00:32:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176368.25 epoch sec; seqNo:13592; 4059436.62 north; 592730.44 east 2021-02-13T00:32:48Z- AHRS_M2 msg: 1613176368.62 epoch sec; seqNo:13485 2021-02-13T00:32:48Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.14 psi 2021-02-13T00:32:48Z- handleDepth()-Depth_Keller msg: 1613176368633 epoch sec; seqNo:13593; depth 14.77 2021-02-13T00:32:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176368.64 epoch sec; seqNo:13593; 4059436.62 north; 592730.44 east 2021-02-13T00:32:49Z- AHRS_M2 msg: 1613176369.03 epoch sec; seqNo:13486 2021-02-13T00:32:49Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.14 psi 2021-02-13T00:32:49Z- handleDepth()-Depth_Keller msg: 1613176369035 epoch sec; seqNo:13594; depth 14.67 2021-02-13T00:32:49Z- handleDvl() - RDI_Pathfinder msg: 1613176369.05 epoch sec; seqNo:6605 2021-02-13T00:32:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.13 , 96.29 , 112.52 , 73.56 2021-02-13T00:32:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , -0.19 2021-02-13T00:32:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176369.06 epoch sec; seqNo:13594; 4059437.00 north; 592730.43 east 2021-02-13T00:32:49Z- AHRS_M2 msg: 1613176369.43 epoch sec; seqNo:13487 2021-02-13T00:32:49Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.14 psi 2021-02-13T00:32:49Z- handleDepth()-Depth_Keller msg: 1613176369432 epoch sec; seqNo:13595; depth 14.55 2021-02-13T00:32:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176369.45 epoch sec; seqNo:13595; 4059437.38 north; 592730.43 east 2021-02-13T00:32:49Z- AHRS_M2 msg: 1613176369.83 epoch sec; seqNo:13488 2021-02-13T00:32:49Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.14 psi 2021-02-13T00:32:49Z- handleDepth()-Depth_Keller msg: 1613176369841 epoch sec; seqNo:13596; depth 14.45 2021-02-13T00:32:49Z- handleDvl() - RDI_Pathfinder msg: 1613176369.86 epoch sec; seqNo:6606 2021-02-13T00:32:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.86 , 108.60 , 74.37 2021-02-13T00:32:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.16 , -0.21 2021-02-13T00:32:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176369.87 epoch sec; seqNo:13596; 4059437.38 north; 592730.43 east 2021-02-13T00:32:50Z- AHRS_M2 msg: 1613176370.24 epoch sec; seqNo:13489 2021-02-13T00:32:50Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.15 psi 2021-02-13T00:32:50Z- handleDepth()-Depth_Keller msg: 1613176370240 epoch sec; seqNo:13597; depth 14.33 2021-02-13T00:32:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176370.25 epoch sec; seqNo:13597; 4059437.76 north; 592730.43 east 2021-02-13T00:32:50Z- AHRS_M2 msg: 1613176370.65 epoch sec; seqNo:13490 2021-02-13T00:32:50Z- AHRS_M2 msg: 0.66 phi; 0.32 theta; -0.15 psi 2021-02-13T00:32:50Z- handleDepth()-Depth_Keller msg: 1613176370651 epoch sec; seqNo:13598; depth 14.20 2021-02-13T00:32:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176370.66 epoch sec; seqNo:13598; 4059438.13 north; 592730.42 east 2021-02-13T00:32:51Z- AHRS_M2 msg: 1613176371.04 epoch sec; seqNo:13491 2021-02-13T00:32:51Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.15 psi 2021-02-13T00:32:51Z- handleDepth()-Depth_Keller msg: 1613176371053 epoch sec; seqNo:13599; depth 14.09 2021-02-13T00:32:51Z- handleDvl() - RDI_Pathfinder msg: 1613176371.08 epoch sec; seqNo:6607 2021-02-13T00:32:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.57 , 111.10 , 78.64 2021-02-13T00:32:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.28 , -0.07 2021-02-13T00:32:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176371.09 epoch sec; seqNo:13599; 4059438.51 north; 592730.42 east 2021-02-13T00:32:51Z- LcmTrn::handleMessages() - handling 45 messages took 0.69 ms and 131 calls... 2021-02-13T00:32:51Z- waiting for 1613176368.81, time is 1613176371.09 2021-02-13T00:32:51Z- LcmTrn::updateTrn() equating pose 1613176371.09 and meas times 1613176371.09 2021-02-13T00:32:51Z- LcmTrn::updateTrn() >>>> pose time 1613176371.09, meas time 1613176371.09 <<<< 2021-02-13T00:32:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176371.09 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:32:51Z- TerrainNav::motionUpdate() - currEstimate = (4059439, 592730.42, 14.09) 2021-02-13T00:32:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:32:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:32:51Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176371.09 sec.; No relevant navigation data available 2021-02-13T00:32:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:32:51Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:51Z- variances([0], [2], and psi_berg) = 697.943896 832.996874 0.000000 2021-02-13T00:32:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:32:51Z- LcmTrn::publishEstimates() - MLE : 4059438.01 592763.92 14.09 2021-02-13T00:32:51Z- LcmTrn::publishEstimates() - MMSE : 4059438.51 592730.42 14.09 2021-02-13T00:32:51Z- LcmTrn::publishEstimates() - COVAR: 697.94 833.00 1.13 0.00 2021-02-13T00:32:51Z- AHRS_M2 msg: 1613176371.45 epoch sec; seqNo:13492 2021-02-13T00:32:51Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.15 psi 2021-02-13T00:32:51Z- handleDepth()-Depth_Keller msg: 1613176371452 epoch sec; seqNo:13600; depth 13.97 2021-02-13T00:32:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176371.46 epoch sec; seqNo:13600; 4059438.91 north; 592731.63 east 2021-02-13T00:32:51Z- AHRS_M2 msg: 1613176371.85 epoch sec; seqNo:13493 2021-02-13T00:32:51Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-13T00:32:51Z- handleDepth()-Depth_Keller msg: 1613176371861 epoch sec; seqNo:13601; depth 13.87 2021-02-13T00:32:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176371.87 epoch sec; seqNo:13601; 4059438.91 north; 592731.63 east 2021-02-13T00:32:52Z- AHRS_M2 msg: 1613176372.26 epoch sec; seqNo:13494 2021-02-13T00:32:52Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-13T00:32:52Z- handleDepth()-Depth_Keller msg: 1613176372266 epoch sec; seqNo:13602; depth 13.74 2021-02-13T00:32:52Z- handleDvl() - RDI_Pathfinder msg: 1613176372.28 epoch sec; seqNo:6608 2021-02-13T00:32:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.85 , 97.37 , 110.89 , 78.44 2021-02-13T00:32:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.18 , 0.11 2021-02-13T00:32:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176372.29 epoch sec; seqNo:13602; 4059439.28 north; 592731.63 east 2021-02-13T00:32:52Z- AHRS_M2 msg: 1613176372.66 epoch sec; seqNo:13495 2021-02-13T00:32:52Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-13T00:32:52Z- handleDepth()-Depth_Keller msg: 1613176372668 epoch sec; seqNo:13603; depth 13.62 2021-02-13T00:32:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176372.69 epoch sec; seqNo:13603; 4059439.66 north; 592731.63 east 2021-02-13T00:32:53Z- AHRS_M2 msg: 1613176373.06 epoch sec; seqNo:13496 2021-02-13T00:32:53Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.18 psi 2021-02-13T00:32:53Z- handleDepth()-Depth_Keller msg: 1613176373073 epoch sec; seqNo:13604; depth 13.52 2021-02-13T00:32:53Z- handleDvl() - RDI_Pathfinder msg: 1613176373.08 epoch sec; seqNo:6609 2021-02-13T00:32:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 99.00 , 111.16 , 73.97 2021-02-13T00:32:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , 0.19 2021-02-13T00:32:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176373.09 epoch sec; seqNo:13604; 4059440.04 north; 592731.62 east 2021-02-13T00:32:53Z- AHRS_M2 msg: 1613176373.47 epoch sec; seqNo:13497 2021-02-13T00:32:53Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-13T00:32:53Z- handleDepth()-Depth_Keller msg: 1613176373475 epoch sec; seqNo:13605; depth 13.38 2021-02-13T00:32:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176373.49 epoch sec; seqNo:13605; 4059440.42 north; 592731.62 east 2021-02-13T00:32:53Z- AHRS_M2 msg: 1613176373.88 epoch sec; seqNo:13498 2021-02-13T00:32:53Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-13T00:32:53Z- handleDepth()-Depth_Keller msg: 1613176373890 epoch sec; seqNo:13606; depth 13.29 2021-02-13T00:32:53Z- handleDvl() - RDI_Pathfinder msg: 1613176373.90 epoch sec; seqNo:6610 2021-02-13T00:32:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 100.42 , 112.58 , 78.78 2021-02-13T00:32:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.14 , 0.10 2021-02-13T00:32:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176373.92 epoch sec; seqNo:13606; 4059440.42 north; 592731.62 east 2021-02-13T00:32:54Z- AHRS_M2 msg: 1613176374.28 epoch sec; seqNo:13499 2021-02-13T00:32:54Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-13T00:32:54Z- handleDepth()-Depth_Keller msg: 1613176374285 epoch sec; seqNo:13607; depth 13.16 2021-02-13T00:32:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176374.30 epoch sec; seqNo:13607; 4059440.81 north; 592732.83 east 2021-02-13T00:32:54Z- AHRS_M2 msg: 1613176374.68 epoch sec; seqNo:13500 2021-02-13T00:32:54Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-13T00:32:54Z- handleDepth()-Depth_Keller msg: 1613176374688 epoch sec; seqNo:13608; depth 13.06 2021-02-13T00:32:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176374.70 epoch sec; seqNo:13608; 4059441.19 north; 592732.83 east 2021-02-13T00:32:55Z- AHRS_M2 msg: 1613176375.08 epoch sec; seqNo:13501 2021-02-13T00:32:55Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.18 psi 2021-02-13T00:32:55Z- handleDepth()-Depth_Keller msg: 1613176375099 epoch sec; seqNo:13609; depth 12.95 2021-02-13T00:32:55Z- handleDvl() - RDI_Pathfinder msg: 1613176375.11 epoch sec; seqNo:6611 2021-02-13T00:32:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.63 , 115.15 , 73.97 2021-02-13T00:32:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.25 , -0.06 2021-02-13T00:32:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176375.12 epoch sec; seqNo:13609; 4059441.19 north; 592732.83 east 2021-02-13T00:32:55Z- AHRS_M2 msg: 1613176375.49 epoch sec; seqNo:13502 2021-02-13T00:32:55Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-13T00:32:55Z- handleDepth()-Depth_Keller msg: 1613176375496 epoch sec; seqNo:13610; depth 12.82 2021-02-13T00:32:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176375.51 epoch sec; seqNo:13610; 4059441.57 north; 592732.83 east 2021-02-13T00:32:55Z- AHRS_M2 msg: 1613176375.90 epoch sec; seqNo:13503 2021-02-13T00:32:55Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.17 psi 2021-02-13T00:32:55Z- handleDepth()-Depth_Keller msg: 1613176375915 epoch sec; seqNo:13611; depth 12.72 2021-02-13T00:32:55Z- handleDvl() - RDI_Pathfinder msg: 1613176375.96 epoch sec; seqNo:6612 2021-02-13T00:32:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.78 , 101.36 , 113.53 , 78.37 2021-02-13T00:32:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.03 , -0.21 2021-02-13T00:32:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176375.98 epoch sec; seqNo:13611; 4059441.95 north; 592732.82 east 2021-02-13T00:32:56Z- LcmTrn::handleMessages() - handling 41 messages took 0.68 ms and 128 calls... 2021-02-13T00:32:56Z- waiting for 1613176374.09, time is 1613176375.98 2021-02-13T00:32:56Z- LcmTrn::updateTrn() equating pose 1613176375.98 and meas times 1613176375.98 2021-02-13T00:32:56Z- LcmTrn::updateTrn() >>>> pose time 1613176375.98, meas time 1613176375.98 <<<< 2021-02-13T00:32:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176375.98 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:32:56Z- TerrainNav::motionUpdate() - currEstimate = (4059442, 592732.82, 12.72) 2021-02-13T00:32:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:32:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:32:56Z- TNavBF::Projecting Measurements 2021-02-13T00:32:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:32:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:32:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:32:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:32:56Z- TNavFilter::4 good beams. 2021-02-13T00:32:56Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:32:56Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:32:56Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:32:56Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:32:56Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:32:56Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:32:56Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:32:56Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:56Z- TerrainMapOctree: (vehN, vehE ) = (4059441.95, 592732.82). 2021-02-13T00:32:56Z- TerrainMapOctree: Min Distance = 2360.27. 2021-02-13T00:32:56Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:32:56Z- There is no specific good beam that all particles have in common. 2021-02-13T00:32:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:32:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.35 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 11.23 2021-02-13T00:32:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:32:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 16.44 2021-02-13T00:32:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:32:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 20.62 2021-02-13T00:32:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:32:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 9.83 2021-02-13T00:32:56Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:32:56Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:32:56Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:32:56Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:32:56Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:32:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:32:56Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:32:56Z- Filter 0 No beams used in subcloud update 2021-02-13T00:32:56Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:32:56Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:32:56Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 697.9 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:32:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176375.98 sec. 2021-02-13T00:32:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:32:56Z- TNavBF::mmse selected from filter -1 2021-02-13T00:32:56Z- variances([0], [2], and psi_berg) = 697.889678 832.996846 0.000000 2021-02-13T00:32:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:32:56Z- LcmTrn::publishEstimates() - MLE : 4059441.45 592766.32 12.72 2021-02-13T00:32:56Z- LcmTrn::publishEstimates() - MMSE : 4059441.95 592732.82 12.72 2021-02-13T00:32:56Z- LcmTrn::publishEstimates() - COVAR: 697.89 833.00 0.99 0.00 2021-02-13T00:32:56Z- AHRS_M2 msg: 1613176376.30 epoch sec; seqNo:13504 2021-02-13T00:32:56Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.17 psi 2021-02-13T00:32:56Z- handleDepth()-Depth_Keller msg: 1613176376305 epoch sec; seqNo:13612; depth 12.59 2021-02-13T00:32:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176376.32 epoch sec; seqNo:13612; 4059441.96 north; 592734.04 east 2021-02-13T00:32:56Z- AHRS_M2 msg: 1613176376.70 epoch sec; seqNo:13505 2021-02-13T00:32:56Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.16 psi 2021-02-13T00:32:56Z- handleDepth()-Depth_Keller msg: 1613176376706 epoch sec; seqNo:13613; depth 12.49 2021-02-13T00:32:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176376.71 epoch sec; seqNo:13613; 4059442.34 north; 592734.04 east 2021-02-13T00:32:57Z- AHRS_M2 msg: 1613176377.10 epoch sec; seqNo:13506 2021-02-13T00:32:57Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.16 psi 2021-02-13T00:32:57Z- handleDepth()-Depth_Keller msg: 1613176377113 epoch sec; seqNo:13614; depth 12.38 2021-02-13T00:32:57Z- handleDvl() - RDI_Pathfinder msg: 1613176377.13 epoch sec; seqNo:6613 2021-02-13T00:32:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 100.21 , 113.73 , 78.58 2021-02-13T00:32:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.15 , -0.16 2021-02-13T00:32:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176377.15 epoch sec; seqNo:13614; 4059442.72 north; 592734.03 east 2021-02-13T00:32:57Z- AHRS_M2 msg: 1613176377.51 epoch sec; seqNo:13507 2021-02-13T00:32:57Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.17 psi 2021-02-13T00:32:57Z- handleDepth()-Depth_Keller msg: 1613176377512 epoch sec; seqNo:13615; depth 12.26 2021-02-13T00:32:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176377.52 epoch sec; seqNo:13615; 4059442.72 north; 592734.03 east 2021-02-13T00:32:57Z- AHRS_M2 msg: 1613176377.91 epoch sec; seqNo:13508 2021-02-13T00:32:57Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.17 psi 2021-02-13T00:32:57Z- handleDepth()-Depth_Keller msg: 1613176377917 epoch sec; seqNo:13616; depth 12.16 2021-02-13T00:32:57Z- handleDvl() - RDI_Pathfinder msg: 1613176377.93 epoch sec; seqNo:6614 2021-02-13T00:32:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 100.21 , 112.38 , 78.58 2021-02-13T00:32:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.20 , 0.05 2021-02-13T00:32:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176377.94 epoch sec; seqNo:13616; 4059443.10 north; 592734.03 east 2021-02-13T00:32:58Z- AHRS_M2 msg: 1613176378.32 epoch sec; seqNo:13509 2021-02-13T00:32:58Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-13T00:32:58Z- handleDepth()-Depth_Keller msg: 1613176378337 epoch sec; seqNo:13617; depth 12.03 2021-02-13T00:32:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176378.35 epoch sec; seqNo:13617; 4059443.48 north; 592734.03 east 2021-02-13T00:32:58Z- AHRS_M2 msg: 1613176378.72 epoch sec; seqNo:13510 2021-02-13T00:32:58Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-13T00:32:58Z- handleDepth()-Depth_Keller msg: 1613176378724 epoch sec; seqNo:13618; depth 11.93 2021-02-13T00:32:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176378.73 epoch sec; seqNo:13618; 4059443.86 north; 592734.02 east 2021-02-13T00:32:59Z- AHRS_M2 msg: 1613176379.13 epoch sec; seqNo:13511 2021-02-13T00:32:59Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-13T00:32:59Z- handleDepth()-Depth_Keller msg: 1613176379141 epoch sec; seqNo:13619; depth 11.81 2021-02-13T00:32:59Z- handleDvl() - RDI_Pathfinder msg: 1613176379.15 epoch sec; seqNo:6615 2021-02-13T00:32:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 101.63 , 109.74 , 78.64 2021-02-13T00:32:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.11 , 0.22 2021-02-13T00:32:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176379.17 epoch sec; seqNo:13619; 4059444.24 north; 592734.02 east 2021-02-13T00:32:59Z- AHRS_M2 msg: 1613176379.53 epoch sec; seqNo:13512 2021-02-13T00:32:59Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.18 psi 2021-02-13T00:32:59Z- handleDepth()-Depth_Keller msg: 1613176379534 epoch sec; seqNo:13620; depth 11.69 2021-02-13T00:32:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176379.55 epoch sec; seqNo:13620; 4059444.25 north; 592735.24 east 2021-02-13T00:32:59Z- AHRS_M2 msg: 1613176379.93 epoch sec; seqNo:13513 2021-02-13T00:32:59Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T00:32:59Z- handleDepth()-Depth_Keller msg: 1613176379941 epoch sec; seqNo:13621; depth 11.59 2021-02-13T00:32:59Z- handleDvl() - RDI_Pathfinder msg: 1613176379.96 epoch sec; seqNo:6616 2021-02-13T00:32:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 101.63 , 112.45 , 78.64 2021-02-13T00:32:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , 0.23 2021-02-13T00:32:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176379.97 epoch sec; seqNo:13621; 4059444.63 north; 592735.23 east 2021-02-13T00:33:00Z- AHRS_M2 msg: 1613176380.34 epoch sec; seqNo:13514 2021-02-13T00:33:00Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.19 psi 2021-02-13T00:33:00Z- handleDepth()-Depth_Keller msg: 1613176380340 epoch sec; seqNo:13622; depth 11.47 2021-02-13T00:33:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176380.35 epoch sec; seqNo:13622; 4059445.01 north; 592735.23 east 2021-02-13T00:33:00Z- AHRS_M2 msg: 1613176380.74 epoch sec; seqNo:13515 2021-02-13T00:33:00Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.19 psi 2021-02-13T00:33:00Z- handleDepth()-Depth_Keller msg: 1613176380745 epoch sec; seqNo:13623; depth 11.36 2021-02-13T00:33:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176380.76 epoch sec; seqNo:13623; 4059445.39 north; 592735.23 east 2021-02-13T00:33:01Z- AHRS_M2 msg: 1613176381.15 epoch sec; seqNo:13516 2021-02-13T00:33:01Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.20 psi 2021-02-13T00:33:01Z- handleDepth()-Depth_Keller msg: 1613176381154 epoch sec; seqNo:13624; depth 11.26 2021-02-13T00:33:01Z- handleDvl() - RDI_Pathfinder msg: 1613176381.17 epoch sec; seqNo:6617 2021-02-13T00:33:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 104.27 , 113.73 , 78.58 2021-02-13T00:33:01Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.15 , 0.12 2021-02-13T00:33:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176381.18 epoch sec; seqNo:13624; 4059445.76 north; 592735.22 east 2021-02-13T00:33:01Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-13T00:33:01Z- waiting for 1613176378.98, time is 1613176381.18 2021-02-13T00:33:01Z- LcmTrn::updateTrn() equating pose 1613176381.18 and meas times 1613176381.18 2021-02-13T00:33:01Z- LcmTrn::updateTrn() >>>> pose time 1613176381.18, meas time 1613176381.18 <<<< 2021-02-13T00:33:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176381.18 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:33:01Z- TerrainNav::motionUpdate() - currEstimate = (4059446, 592735.22, 11.26) 2021-02-13T00:33:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:33:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:33:01Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176381.18 sec.; No relevant navigation data available 2021-02-13T00:33:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:33:01Z- TNavBF::mmse selected from filter -1 2021-02-13T00:33:01Z- variances([0], [2], and psi_berg) = 697.833502 832.996817 0.000000 2021-02-13T00:33:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:33:01Z- LcmTrn::publishEstimates() - MLE : 4059445.26 592768.72 11.26 2021-02-13T00:33:01Z- LcmTrn::publishEstimates() - MMSE : 4059445.76 592735.22 11.26 2021-02-13T00:33:01Z- LcmTrn::publishEstimates() - COVAR: 697.83 833.00 1.22 0.00 2021-02-13T00:33:01Z- AHRS_M2 msg: 1613176381.55 epoch sec; seqNo:13517 2021-02-13T00:33:01Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.19 psi 2021-02-13T00:33:01Z- handleDepth()-Depth_Keller msg: 1613176381562 epoch sec; seqNo:13625; depth 11.13 2021-02-13T00:33:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176381.57 epoch sec; seqNo:13625; 4059445.78 north; 592736.44 east 2021-02-13T00:33:01Z- AHRS_M2 msg: 1613176381.95 epoch sec; seqNo:13518 2021-02-13T00:33:01Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-13T00:33:01Z- handleDepth()-Depth_Keller msg: 1613176381961 epoch sec; seqNo:13626; depth 11.03 2021-02-13T00:33:02Z- handleDvl() - RDI_Pathfinder msg: 1613176382.01 epoch sec; seqNo:6618 2021-02-13T00:33:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 105.56 , 119.07 , 79.87 2021-02-13T00:33:02Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.20 , -0.05 2021-02-13T00:33:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176382.04 epoch sec; seqNo:13626; 4059446.16 north; 592736.44 east 2021-02-13T00:33:02Z- AHRS_M2 msg: 1613176382.36 epoch sec; seqNo:13519 2021-02-13T00:33:02Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-13T00:33:02Z- handleDepth()-Depth_Keller msg: 1613176382360 epoch sec; seqNo:13627; depth 10.90 2021-02-13T00:33:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176382.37 epoch sec; seqNo:13627; 4059446.53 north; 592736.43 east 2021-02-13T00:33:02Z- AHRS_M2 msg: 1613176382.77 epoch sec; seqNo:13520 2021-02-13T00:33:02Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-13T00:33:02Z- handleDepth()-Depth_Keller msg: 1613176382771 epoch sec; seqNo:13628; depth 10.82 2021-02-13T00:33:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176382.78 epoch sec; seqNo:13628; 4059446.91 north; 592736.43 east 2021-02-13T00:33:03Z- AHRS_M2 msg: 1613176383.16 epoch sec; seqNo:13521 2021-02-13T00:33:03Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T00:33:03Z- handleDepth()-Depth_Keller msg: 1613176383173 epoch sec; seqNo:13629; depth 10.71 2021-02-13T00:33:03Z- handleDvl() - RDI_Pathfinder msg: 1613176383.19 epoch sec; seqNo:6619 2021-02-13T00:33:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.42 , 104.00 , 122.93 , 79.67 2021-02-13T00:33:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.14 , -0.19 2021-02-13T00:33:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176383.20 epoch sec; seqNo:13629; 4059446.93 north; 592737.65 east 2021-02-13T00:33:03Z- AHRS_M2 msg: 1613176383.57 epoch sec; seqNo:13522 2021-02-13T00:33:03Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-13T00:33:03Z- handleDepth()-Depth_Keller msg: 1613176383577 epoch sec; seqNo:13630; depth 10.58 2021-02-13T00:33:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176383.59 epoch sec; seqNo:13630; 4059447.31 north; 592737.65 east 2021-02-13T00:33:03Z- AHRS_M2 msg: 1613176383.98 epoch sec; seqNo:13523 2021-02-13T00:33:03Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.14 psi 2021-02-13T00:33:03Z- handleDepth()-Depth_Keller msg: 1613176383981 epoch sec; seqNo:13631; depth 10.48 2021-02-13T00:33:03Z- handleDvl() - RDI_Pathfinder msg: 1613176383.99 epoch sec; seqNo:6620 2021-02-13T00:33:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 103.93 , 120.16 , 78.25 2021-02-13T00:33:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , -0.26 2021-02-13T00:33:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176384.00 epoch sec; seqNo:13631; 4059447.31 north; 592737.65 east 2021-02-13T00:33:04Z- AHRS_M2 msg: 1613176384.38 epoch sec; seqNo:13524 2021-02-13T00:33:04Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.13 psi 2021-02-13T00:33:04Z- handleDepth()-Depth_Keller msg: 1613176384382 epoch sec; seqNo:13632; depth 10.33 2021-02-13T00:33:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176384.39 epoch sec; seqNo:13632; 4059447.68 north; 592737.64 east 2021-02-13T00:33:04Z- AHRS_M2 msg: 1613176384.78 epoch sec; seqNo:13525 2021-02-13T00:33:04Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.14 psi 2021-02-13T00:33:04Z- handleDepth()-Depth_Keller msg: 1613176384784 epoch sec; seqNo:13633; depth 10.22 2021-02-13T00:33:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176384.81 epoch sec; seqNo:13633; 4059448.06 north; 592737.64 east 2021-02-13T00:33:05Z- AHRS_M2 msg: 1613176385.18 epoch sec; seqNo:13526 2021-02-13T00:33:05Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.14 psi 2021-02-13T00:33:05Z- handleDepth()-Depth_Keller msg: 1613176385189 epoch sec; seqNo:13634; depth 10.09 2021-02-13T00:33:05Z- handleDvl() - RDI_Pathfinder msg: 1613176385.20 epoch sec; seqNo:6621 2021-02-13T00:33:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.51 , 103.73 , 118.60 , 79.39 2021-02-13T00:33:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.13 , -0.20 2021-02-13T00:33:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176385.22 epoch sec; seqNo:13634; 4059448.06 north; 592737.64 east 2021-02-13T00:33:05Z- AHRS_M2 msg: 1613176385.59 epoch sec; seqNo:13527 2021-02-13T00:33:05Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.15 psi 2021-02-13T00:33:05Z- handleDepth()-Depth_Keller msg: 1613176385593 epoch sec; seqNo:13635; depth 9.97 2021-02-13T00:33:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176385.61 epoch sec; seqNo:13635; 4059448.44 north; 592737.63 east 2021-02-13T00:33:05Z- AHRS_M2 msg: 1613176385.99 epoch sec; seqNo:13528 2021-02-13T00:33:05Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-13T00:33:06Z- handleDepth()-Depth_Keller msg: 1613176385996 epoch sec; seqNo:13636; depth 9.83 2021-02-13T00:33:06Z- handleDvl() - RDI_Pathfinder msg: 1613176386.01 epoch sec; seqNo:6622 2021-02-13T00:33:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.78 , 102.65 , 116.17 , 79.28 2021-02-13T00:33:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.26 , -0.00 2021-02-13T00:33:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176386.02 epoch sec; seqNo:13636; 4059448.82 north; 592737.63 east 2021-02-13T00:33:06Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-13T00:33:06Z- waiting for 1613176384.18, time is 1613176386.02 2021-02-13T00:33:06Z- LcmTrn::updateTrn() equating pose 1613176386.02 and meas times 1613176386.02 2021-02-13T00:33:06Z- LcmTrn::updateTrn() >>>> pose time 1613176386.02, meas time 1613176386.02 <<<< 2021-02-13T00:33:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176386.02 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:33:06Z- TerrainNav::motionUpdate() - currEstimate = (4059449, 592737.63, 9.83) 2021-02-13T00:33:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:33:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:33:06Z- TNavBF::Projecting Measurements 2021-02-13T00:33:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:33:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:33:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:33:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:33:06Z- TNavFilter::4 good beams. 2021-02-13T00:33:06Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:33:06Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:33:06Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:33:06Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:33:06Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:33:06Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:33:06Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:33:06Z- TNavBF::mmse selected from filter -1 2021-02-13T00:33:06Z- TerrainMapOctree: (vehN, vehE ) = (4059448.82, 592737.63). 2021-02-13T00:33:06Z- TerrainMapOctree: Min Distance = 2351.92. 2021-02-13T00:33:06Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:33:06Z- There is no specific good beam that all particles have in common. 2021-02-13T00:33:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:33:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.42 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 12.33 2021-02-13T00:33:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:33:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 16.86 2021-02-13T00:33:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:33:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 21.59 2021-02-13T00:33:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:33:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 10.06 2021-02-13T00:33:06Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:33:06Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:33:06Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:33:06Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:33:06Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:33:06Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:33:06Z- Filter 0 No beams used in subcloud update 2021-02-13T00:33:06Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:33:06Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:33:06Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 697.8 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:33:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176386.02 sec. 2021-02-13T00:33:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:33:06Z- TNavBF::mmse selected from filter -1 2021-02-13T00:33:06Z- variances([0], [2], and psi_berg) = 697.781332 832.996790 0.000000 2021-02-13T00:33:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:33:06Z- LcmTrn::publishEstimates() - MLE : 4059448.32 592771.13 9.83 2021-02-13T00:33:06Z- LcmTrn::publishEstimates() - MMSE : 4059448.82 592737.63 9.83 2021-02-13T00:33:06Z- LcmTrn::publishEstimates() - COVAR: 697.78 833.00 1.05 0.00 2021-02-13T00:33:06Z- AHRS_M2 msg: 1613176386.39 epoch sec; seqNo:13529 2021-02-13T00:33:06Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T00:33:06Z- handleDepth()-Depth_Keller msg: 1613176386401 epoch sec; seqNo:13637; depth 9.70 2021-02-13T00:33:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176386.41 epoch sec; seqNo:13637; 4059449.20 north; 592737.63 east 2021-02-13T00:33:06Z- AHRS_M2 msg: 1613176386.80 epoch sec; seqNo:13530 2021-02-13T00:33:06Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-13T00:33:06Z- handleDepth()-Depth_Keller msg: 1613176386804 epoch sec; seqNo:13638; depth 9.60 2021-02-13T00:33:06Z- handleDvl() - RDI_Pathfinder msg: 1613176386.82 epoch sec; seqNo:6623 2021-02-13T00:33:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.42 , 117.59 , 77.24 2021-02-13T00:33:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.16 , 0.24 2021-02-13T00:33:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176386.83 epoch sec; seqNo:13638; 4059449.59 north; 592738.84 east 2021-02-13T00:33:07Z- AHRS_M2 msg: 1613176387.20 epoch sec; seqNo:13531 2021-02-13T00:33:07Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-13T00:33:07Z- handleDepth()-Depth_Keller msg: 1613176387217 epoch sec; seqNo:13639; depth 9.46 2021-02-13T00:33:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176387.23 epoch sec; seqNo:13639; 4059449.59 north; 592738.84 east 2021-02-13T00:33:07Z- AHRS_M2 msg: 1613176387.61 epoch sec; seqNo:13532 2021-02-13T00:33:07Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-13T00:33:07Z- handleDepth()-Depth_Keller msg: 1613176387622 epoch sec; seqNo:13640; depth 9.33 2021-02-13T00:33:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176387.63 epoch sec; seqNo:13640; 4059449.97 north; 592738.84 east 2021-02-13T00:33:08Z- AHRS_M2 msg: 1613176388.01 epoch sec; seqNo:13533 2021-02-13T00:33:08Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-13T00:33:08Z- handleDepth()-Depth_Keller msg: 1613176388016 epoch sec; seqNo:13641; depth 9.20 2021-02-13T00:33:08Z- handleDvl() - RDI_Pathfinder msg: 1613176388.03 epoch sec; seqNo:6624 2021-02-13T00:33:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 106.50 , 117.32 , 78.05 2021-02-13T00:33:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , 0.29 2021-02-13T00:33:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176388.05 epoch sec; seqNo:13641; 4059450.35 north; 592738.83 east 2021-02-13T00:33:08Z- AHRS_M2 msg: 1613176388.42 epoch sec; seqNo:13534 2021-02-13T00:33:08Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-13T00:33:08Z- handleDepth()-Depth_Keller msg: 1613176388420 epoch sec; seqNo:13642; depth 9.08 2021-02-13T00:33:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176388.43 epoch sec; seqNo:13642; 4059450.73 north; 592738.83 east 2021-02-13T00:33:08Z- AHRS_M2 msg: 1613176388.82 epoch sec; seqNo:13535 2021-02-13T00:33:08Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.15 psi 2021-02-13T00:33:08Z- handleDepth()-Depth_Keller msg: 1613176388829 epoch sec; seqNo:13643; depth 8.96 2021-02-13T00:33:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176388.84 epoch sec; seqNo:13643; 4059451.11 north; 592738.83 east 2021-02-13T00:33:09Z- AHRS_M2 msg: 1613176389.22 epoch sec; seqNo:13536 2021-02-13T00:33:09Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.16 psi 2021-02-13T00:33:09Z- handleDepth()-Depth_Keller msg: 1613176389233 epoch sec; seqNo:13644; depth 8.84 2021-02-13T00:33:09Z- handleDvl() - RDI_Pathfinder msg: 1613176389.25 epoch sec; seqNo:6625 2021-02-13T00:33:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.48 , 104.00 , 122.93 , 81.02 2021-02-13T00:33:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.15 , 0.16 2021-02-13T00:33:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176389.26 epoch sec; seqNo:13644; 4059451.12 north; 592740.05 east 2021-02-13T00:33:09Z- AHRS_M2 msg: 1613176389.63 epoch sec; seqNo:13537 2021-02-13T00:33:09Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.16 psi 2021-02-13T00:33:09Z- handleDepth()-Depth_Keller msg: 1613176389632 epoch sec; seqNo:13645; depth 8.70 2021-02-13T00:33:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176389.64 epoch sec; seqNo:13645; 4059451.50 north; 592740.04 east 2021-02-13T00:33:10Z- AHRS_M2 msg: 1613176390.03 epoch sec; seqNo:13538 2021-02-13T00:33:10Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-13T00:33:10Z- handleDepth()-Depth_Keller msg: 1613176390036 epoch sec; seqNo:13646; depth 8.57 2021-02-13T00:33:10Z- handleDvl() - RDI_Pathfinder msg: 1613176390.05 epoch sec; seqNo:6626 2021-02-13T00:33:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.27 , 105.15 , 120.02 , 82.16 2021-02-13T00:33:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.22 , 0.02 2021-02-13T00:33:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176390.06 epoch sec; seqNo:13646; 4059451.88 north; 592740.04 east 2021-02-13T00:33:10Z- AHRS_M2 msg: 1613176390.44 epoch sec; seqNo:13539 2021-02-13T00:33:10Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.15 psi 2021-02-13T00:33:10Z- handleDepth()-Depth_Keller msg: 1613176390442 epoch sec; seqNo:13647; depth 8.45 2021-02-13T00:33:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176390.45 epoch sec; seqNo:13647; 4059451.88 north; 592740.04 east 2021-02-13T00:33:10Z- AHRS_M2 msg: 1613176390.84 epoch sec; seqNo:13540 2021-02-13T00:33:10Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-13T00:33:10Z- handleDepth()-Depth_Keller msg: 1613176390844 epoch sec; seqNo:13648; depth 8.31 2021-02-13T00:33:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176390.87 epoch sec; seqNo:13648; 4059452.27 north; 592741.26 east 2021-02-13T00:33:11Z- AHRS_M2 msg: 1613176391.24 epoch sec; seqNo:13541 2021-02-13T00:33:11Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-13T00:33:11Z- handleDepth()-Depth_Keller msg: 1613176391248 epoch sec; seqNo:13649; depth 8.18 2021-02-13T00:33:11Z- handleDvl() - RDI_Pathfinder msg: 1613176391.26 epoch sec; seqNo:6627 2021-02-13T00:33:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.34 , 106.57 , 121.44 , 80.88 2021-02-13T00:33:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.22 , -0.06 2021-02-13T00:33:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176391.29 epoch sec; seqNo:13649; 4059452.65 north; 592741.25 east 2021-02-13T00:33:11Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-13T00:33:11Z- waiting for 1613176389.02, time is 1613176391.29 2021-02-13T00:33:11Z- LcmTrn::updateTrn() equating pose 1613176391.29 and meas times 1613176391.29 2021-02-13T00:33:11Z- LcmTrn::updateTrn() >>>> pose time 1613176391.29, meas time 1613176391.29 <<<< 2021-02-13T00:33:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176391.29 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:33:11Z- TerrainNav::motionUpdate() - currEstimate = (4059453, 592741.25, 8.18) 2021-02-13T00:33:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:33:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:33:11Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176391.29 sec.; No relevant navigation data available 2021-02-13T00:33:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:33:11Z- TNavBF::mmse selected from filter -1 2021-02-13T00:33:11Z- variances([0], [2], and psi_berg) = 697.720662 832.996759 0.000000 2021-02-13T00:33:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:33:11Z- LcmTrn::publishEstimates() - MLE : 4059452.15 592774.75 8.18 2021-02-13T00:33:11Z- LcmTrn::publishEstimates() - MMSE : 4059452.65 592741.25 8.18 2021-02-13T00:33:11Z- LcmTrn::publishEstimates() - COVAR: 697.72 833.00 1.31 0.00 2021-02-13T00:33:11Z- AHRS_M2 msg: 1613176391.65 epoch sec; seqNo:13542 2021-02-13T00:33:11Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-13T00:33:11Z- handleDepth()-Depth_Keller msg: 1613176391661 epoch sec; seqNo:13650; depth 8.07 2021-02-13T00:33:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176391.67 epoch sec; seqNo:13650; 4059452.65 north; 592741.25 east 2021-02-13T00:33:12Z- AHRS_M2 msg: 1613176392.05 epoch sec; seqNo:13543 2021-02-13T00:33:12Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-13T00:33:12Z- handleDepth()-Depth_Keller msg: 1613176392061 epoch sec; seqNo:13651; depth 7.96 2021-02-13T00:33:12Z- handleDvl() - RDI_Pathfinder msg: 1613176392.08 epoch sec; seqNo:6628 2021-02-13T00:33:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 106.50 , 121.37 , 82.17 2021-02-13T00:33:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.14 , -0.15 2021-02-13T00:33:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176392.09 epoch sec; seqNo:13651; 4059453.03 north; 592741.25 east 2021-02-13T00:33:12Z- AHRS_M2 msg: 1613176392.45 epoch sec; seqNo:13544 2021-02-13T00:33:12Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-13T00:33:12Z- handleDepth()-Depth_Keller msg: 1613176392461 epoch sec; seqNo:13652; depth 7.82 2021-02-13T00:33:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176392.48 epoch sec; seqNo:13652; 4059453.03 north; 592741.25 east 2021-02-13T00:33:12Z- AHRS_M2 msg: 1613176392.87 epoch sec; seqNo:13545 2021-02-13T00:33:12Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.16 psi 2021-02-13T00:33:12Z- handleDepth()-Depth_Keller msg: 1613176392902 epoch sec; seqNo:13653; depth 7.69 2021-02-13T00:33:12Z- handleDvl() - RDI_Pathfinder msg: 1613176392.92 epoch sec; seqNo:6629 2021-02-13T00:33:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.55 , 106.91 , 120.29 , 81.62 2021-02-13T00:33:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , -0.18 2021-02-13T00:33:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176392.93 epoch sec; seqNo:13653; 4059453.42 north; 592742.46 east 2021-02-13T00:33:13Z- AHRS_M2 msg: 1613176393.27 epoch sec; seqNo:13546 2021-02-13T00:33:13Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.16 psi 2021-02-13T00:33:13Z- handleDepth()-Depth_Keller msg: 1613176393271 epoch sec; seqNo:13654; depth 7.56 2021-02-13T00:33:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176393.28 epoch sec; seqNo:13654; 4059453.80 north; 592742.46 east 2021-02-13T00:33:13Z- AHRS_M2 msg: 1613176393.67 epoch sec; seqNo:13547 2021-02-13T00:33:13Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-13T00:33:13Z- handleDepth()-Depth_Keller msg: 1613176393681 epoch sec; seqNo:13655; depth 7.45 2021-02-13T00:33:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176393.69 epoch sec; seqNo:13655; 4059453.80 north; 592742.46 east 2021-02-13T00:33:14Z- AHRS_M2 msg: 1613176394.07 epoch sec; seqNo:13548 2021-02-13T00:33:14Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-13T00:33:14Z- handleDepth()-Depth_Keller msg: 1613176394076 epoch sec; seqNo:13656; depth 7.31 2021-02-13T00:33:14Z- handleDvl() - RDI_Pathfinder msg: 1613176394.09 epoch sec; seqNo:6630 2021-02-13T00:33:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 107.85 , 121.37 , 82.17 2021-02-13T00:33:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.19 , -0.09 2021-02-13T00:33:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176394.11 epoch sec; seqNo:13656; 4059454.18 north; 592742.46 east 2021-02-13T00:33:14Z- AHRS_M2 msg: 1613176394.48 epoch sec; seqNo:13549 2021-02-13T00:33:14Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-13T00:33:14Z- handleDepth()-Depth_Keller msg: 1613176394480 epoch sec; seqNo:13657; depth 7.17 2021-02-13T00:33:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176394.49 epoch sec; seqNo:13657; 4059454.56 north; 592742.45 east 2021-02-13T00:33:14Z- AHRS_M2 msg: 1613176394.90 epoch sec; seqNo:13550 2021-02-13T00:33:14Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.18 psi 2021-02-13T00:33:14Z- handleDepth()-Depth_Keller msg: 1613176394909 epoch sec; seqNo:13658; depth 7.04 2021-02-13T00:33:14Z- handleDvl() - RDI_Pathfinder msg: 1613176394.93 epoch sec; seqNo:6631 2021-02-13T00:33:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.48 , 108.33 , 118.74 , 83.04 2021-02-13T00:33:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.19 , 0.08 2021-02-13T00:33:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176394.94 epoch sec; seqNo:13658; 4059454.94 north; 592742.45 east 2021-02-13T00:33:15Z- AHRS_M2 msg: 1613176395.29 epoch sec; seqNo:13551 2021-02-13T00:33:15Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-13T00:33:15Z- handleDepth()-Depth_Keller msg: 1613176395289 epoch sec; seqNo:13659; depth 6.90 2021-02-13T00:33:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176395.30 epoch sec; seqNo:13659; 4059455.31 north; 592742.44 east 2021-02-13T00:33:15Z- AHRS_M2 msg: 1613176395.69 epoch sec; seqNo:13552 2021-02-13T00:33:15Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.19 psi 2021-02-13T00:33:15Z- handleDepth()-Depth_Keller msg: 1613176395692 epoch sec; seqNo:13660; depth 6.78 2021-02-13T00:33:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176395.71 epoch sec; seqNo:13660; 4059455.31 north; 592742.44 east 2021-02-13T00:33:16Z- AHRS_M2 msg: 1613176396.09 epoch sec; seqNo:13553 2021-02-13T00:33:16Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.19 psi 2021-02-13T00:33:16Z- handleDepth()-Depth_Keller msg: 1613176396095 epoch sec; seqNo:13661; depth 6.64 2021-02-13T00:33:16Z- handleDvl() - RDI_Pathfinder msg: 1613176396.11 epoch sec; seqNo:6632 2021-02-13T00:33:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 107.85 , 121.37 , 82.17 2021-02-13T00:33:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.13 , 0.22 2021-02-13T00:33:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176396.14 epoch sec; seqNo:13661; 4059455.69 north; 592742.44 east 2021-02-13T00:33:16Z- LcmTrn::handleMessages() - handling 41 messages took 0.68 ms and 128 calls... 2021-02-13T00:33:16Z- waiting for 1613176394.29, time is 1613176396.14 2021-02-13T00:33:16Z- LcmTrn::updateTrn() equating pose 1613176396.14 and meas times 1613176396.14 2021-02-13T00:33:16Z- LcmTrn::updateTrn() >>>> pose time 1613176396.14, meas time 1613176396.14 <<<< 2021-02-13T00:33:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176396.14 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:33:16Z- TerrainNav::motionUpdate() - currEstimate = (4059456, 592742.44, 6.64) 2021-02-13T00:33:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:33:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:33:16Z- TNavBF::Projecting Measurements 2021-02-13T00:33:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:33:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:33:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:33:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:33:16Z- TNavFilter::4 good beams. 2021-02-13T00:33:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:33:16Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:33:16Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:33:16Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:33:16Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:33:16Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:33:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:33:16Z- TNavBF::mmse selected from filter -1 2021-02-13T00:33:16Z- TerrainMapOctree: (vehN, vehE ) = (4059455.69, 592742.44). 2021-02-13T00:33:16Z- TerrainMapOctree: Min Distance = 2343.56. 2021-02-13T00:33:16Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:33:16Z- There is no specific good beam that all particles have in common. 2021-02-13T00:33:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:33:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.48 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 12.65 2021-02-13T00:33:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:33:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 18.61 2021-02-13T00:33:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:33:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 23.57 2021-02-13T00:33:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:33:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 10.80 2021-02-13T00:33:16Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:33:16Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:33:16Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:33:16Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:33:16Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:33:16Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:33:16Z- Filter 0 No beams used in subcloud update 2021-02-13T00:33:16Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:33:16Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:33:16Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 697.7 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:33:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176396.14 sec. 2021-02-13T00:33:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:33:16Z- TNavBF::mmse selected from filter -1 2021-02-13T00:33:16Z- variances([0], [2], and psi_berg) = 697.672917 832.996734 0.000000 2021-02-13T00:33:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:33:16Z- LcmTrn::publishEstimates() - MLE : 4059455.19 592775.94 6.64 2021-02-13T00:33:16Z- LcmTrn::publishEstimates() - MMSE : 4059455.69 592742.44 6.64 2021-02-13T00:33:16Z- LcmTrn::publishEstimates() - COVAR: 697.67 833.00 1.10 0.00 2021-02-13T00:33:16Z- AHRS_M2 msg: 1613176396.50 epoch sec; seqNo:13554 2021-02-13T00:33:16Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.19 psi 2021-02-13T00:33:16Z- handleDepth()-Depth_Keller msg: 1613176396501 epoch sec; seqNo:13662; depth 6.51 2021-02-13T00:33:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176396.52 epoch sec; seqNo:13662; 4059456.09 north; 592743.66 east 2021-02-13T00:33:16Z- AHRS_M2 msg: 1613176396.91 epoch sec; seqNo:13555 2021-02-13T00:33:16Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.18 psi 2021-02-13T00:33:16Z- handleDepth()-Depth_Keller msg: 1613176396919 epoch sec; seqNo:13663; depth 6.37 2021-02-13T00:33:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176396.94 epoch sec; seqNo:13663; 4059456.46 north; 592743.65 east 2021-02-13T00:33:17Z- AHRS_M2 msg: 1613176397.30 epoch sec; seqNo:13556 2021-02-13T00:33:17Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.17 psi 2021-02-13T00:33:17Z- handleDepth()-Depth_Keller msg: 1613176397313 epoch sec; seqNo:13664; depth 6.26 2021-02-13T00:33:17Z- handleDvl() - RDI_Pathfinder msg: 1613176397.33 epoch sec; seqNo:6633 2021-02-13T00:33:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 109.21 , 121.37 , 83.52 2021-02-13T00:33:17Z- handleDvl() - RDI_Pathfinder msg: velocities 1.11 , 0.03 , 0.23 2021-02-13T00:33:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176397.34 epoch sec; seqNo:13664; 4059456.84 north; 592743.65 east 2021-02-13T00:33:17Z- AHRS_M2 msg: 1613176397.71 epoch sec; seqNo:13557 2021-02-13T00:33:17Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.16 psi 2021-02-13T00:33:17Z- handleDepth()-Depth_Keller msg: 1613176397717 epoch sec; seqNo:13665; depth 6.15 2021-02-13T00:33:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176397.73 epoch sec; seqNo:13665; 4059457.22 north; 592743.64 east 2021-02-13T00:33:18Z- AHRS_M2 msg: 1613176398.11 epoch sec; seqNo:13558 2021-02-13T00:33:18Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.14 psi 2021-02-13T00:33:18Z- handleDepth()-Depth_Keller msg: 1613176398119 epoch sec; seqNo:13666; depth 6.02 2021-02-13T00:33:18Z- handleDvl() - RDI_Pathfinder msg: 1613176398.13 epoch sec; seqNo:6634 2021-02-13T00:33:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 108.26 , 123.13 , 82.98 2021-02-13T00:33:18Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.25 , 0.14 2021-02-13T00:33:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176398.15 epoch sec; seqNo:13666; 4059457.60 north; 592743.64 east 2021-02-13T00:33:18Z- AHRS_M2 msg: 1613176398.52 epoch sec; seqNo:13559 2021-02-13T00:33:18Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.13 psi 2021-02-13T00:33:18Z- handleDepth()-Depth_Keller msg: 1613176398520 epoch sec; seqNo:13667; depth 5.90 2021-02-13T00:33:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176398.54 epoch sec; seqNo:13667; 4059457.99 north; 592744.86 east 2021-02-13T00:33:18Z- AHRS_M2 msg: 1613176398.92 epoch sec; seqNo:13560 2021-02-13T00:33:18Z- AHRS_M2 msg: 0.59 phi; 0.28 theta; -0.13 psi 2021-02-13T00:33:18Z- handleDepth()-Depth_Keller msg: 1613176398933 epoch sec; seqNo:13668; depth 5.79 2021-02-13T00:33:18Z- handleDvl() - RDI_Pathfinder msg: 1613176398.95 epoch sec; seqNo:6635 2021-02-13T00:33:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 106.77 , 120.16 , 84.46 2021-02-13T00:33:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.37 , -0.16 2021-02-13T00:33:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176398.96 epoch sec; seqNo:13668; 4059457.99 north; 592744.86 east 2021-02-13T00:33:19Z- AHRS_M2 msg: 1613176399.32 epoch sec; seqNo:13561 2021-02-13T00:33:19Z- AHRS_M2 msg: 0.60 phi; 0.24 theta; -0.16 psi 2021-02-13T00:33:19Z- handleDepth()-Depth_Keller msg: 1613176399328 epoch sec; seqNo:13669; depth 5.64 2021-02-13T00:33:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176399.34 epoch sec; seqNo:13669; 4059458.37 north; 592744.85 east 2021-02-13T00:33:19Z- AHRS_M2 msg: 1613176399.73 epoch sec; seqNo:13562 2021-02-13T00:33:19Z- AHRS_M2 msg: 0.61 phi; 0.19 theta; -0.20 psi 2021-02-13T00:33:19Z- handleDepth()-Depth_Keller msg: 1613176399734 epoch sec; seqNo:13670; depth 5.51 2021-02-13T00:33:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176399.75 epoch sec; seqNo:13670; 4059458.37 north; 592744.85 east 2021-02-13T00:33:20Z- AHRS_M2 msg: 1613176400.13 epoch sec; seqNo:13563 2021-02-13T00:33:20Z- AHRS_M2 msg: 0.62 phi; 0.14 theta; -0.24 psi 2021-02-13T00:33:20Z- handleDepth()-Depth_Keller msg: 1613176400143 epoch sec; seqNo:13671; depth 5.36 2021-02-13T00:33:20Z- handleDvl() - RDI_Pathfinder msg: 1613176400.17 epoch sec; seqNo:6636 2021-02-13T00:33:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.48 , 103.86 , 112.79 , 86.02 2021-02-13T00:33:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , 0.05 , -0.45 2021-02-13T00:33:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176400.18 epoch sec; seqNo:13671; 4059458.76 north; 592746.07 east 2021-02-13T00:33:20Z- AHRS_M2 msg: 1613176400.54 epoch sec; seqNo:13564 2021-02-13T00:33:20Z- AHRS_M2 msg: 0.64 phi; 0.09 theta; -0.27 psi 2021-02-13T00:33:20Z- handleDepth()-Depth_Keller msg: 1613176400540 epoch sec; seqNo:13672; depth 5.23 2021-02-13T00:33:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176400.55 epoch sec; seqNo:13672; 4059458.76 north; 592746.07 east 2021-02-13T00:33:20Z- AHRS_M2 msg: 1613176400.95 epoch sec; seqNo:13565 2021-02-13T00:33:20Z- AHRS_M2 msg: 0.66 phi; 0.04 theta; -0.28 psi 2021-02-13T00:33:20Z- handleDepth()-Depth_Keller msg: 1613176400957 epoch sec; seqNo:13673; depth 5.12 2021-02-13T00:33:20Z- handleDvl() - RDI_Pathfinder msg: 1613176400.97 epoch sec; seqNo:6637 2021-02-13T00:33:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 106.64 , 103.93 , 97.17 2021-02-13T00:33:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , -0.45 , -0.14 2021-02-13T00:33:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176400.99 epoch sec; seqNo:13673; 4059459.14 north; 592746.06 east 2021-02-13T00:33:21Z- AHRS_M2 msg: 1613176401.34 epoch sec; seqNo:13566 2021-02-13T00:33:21Z- AHRS_M2 msg: 0.67 phi; -0.01 theta; -0.27 psi 2021-02-13T00:33:21Z- handleDepth()-Depth_Keller msg: 1613176401357 epoch sec; seqNo:13674; depth 5.02 2021-02-13T00:33:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176401.37 epoch sec; seqNo:13674; 4059459.52 north; 592746.06 east 2021-02-13T00:33:21Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 132 calls... 2021-02-13T00:33:21Z- waiting for 1613176399.14, time is 1613176401.37 2021-02-13T00:33:21Z- LcmTrn::updateTrn() >>>> pose time 1613176401.37, meas time 1613176400.97 <<<< 2021-02-13T00:33:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176400.97 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:33:21Z- TerrainNav::motionUpdate() - currEstimate = (4059460, 592746.06, 5.02) 2021-02-13T00:33:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:33:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:33:21Z- TNavBF::Projecting Measurements 2021-02-13T00:33:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:33:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:33:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:33:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:33:21Z- TNavFilter::4 good beams. 2021-02-13T00:33:21Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:33:21Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:33:21Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:33:21Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:33:21Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:33:21Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:33:21Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:33:21Z- TNavBF::mmse selected from filter -1 2021-02-13T00:33:21Z- TerrainMapOctree: (vehN, vehE ) = (4059459.23, 592745.78). 2021-02-13T00:33:21Z- TerrainMapOctree: Min Distance = 2338.70. 2021-02-13T00:33:21Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:33:21Z- There is no specific good beam that all particles have in common. 2021-02-13T00:33:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:33:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 11.56 2021-02-13T00:33:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:33:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 18.19 2021-02-13T00:33:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:33:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 17.28 2021-02-13T00:33:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:33:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.11 2021-02-13T00:33:21Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:33:21Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:33:21Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:33:21Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:33:21Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:33:21Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:33:21Z- Filter 0 No beams used in subcloud update 2021-02-13T00:33:21Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:33:21Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:33:21Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 697.6 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:33:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176400.97 sec. 2021-02-13T00:33:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:33:21Z- TNavBF::mmse selected from filter -1 2021-02-13T00:33:21Z- variances([0], [2], and psi_berg) = 697.597933 832.996696 0.000000 2021-02-13T00:33:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:33:21Z- LcmTrn::publishEstimates() - MLE : 4059459.02 592719.56 5.02 2021-02-13T00:33:21Z- LcmTrn::publishEstimates() - MMSE : 4059459.52 592746.06 5.02 2021-02-13T00:33:21Z- LcmTrn::publishEstimates() - COVAR: 697.60 833.00 1.04 0.00 2021-02-13T00:33:21Z- AHRS_M2 msg: 1613176401.75 epoch sec; seqNo:13567 2021-02-13T00:33:21Z- AHRS_M2 msg: 0.68 phi; -0.06 theta; -0.25 psi 2021-02-13T00:33:21Z- handleDepth()-Depth_Keller msg: 1613176401752 epoch sec; seqNo:13675; depth 4.91 2021-02-13T00:33:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176401.76 epoch sec; seqNo:13675; 4059459.90 north; 592746.06 east 2021-02-13T00:33:22Z- AHRS_M2 msg: 1613176402.16 epoch sec; seqNo:13568 2021-02-13T00:33:22Z- AHRS_M2 msg: 0.69 phi; -0.10 theta; -0.22 psi 2021-02-13T00:33:22Z- handleDepth()-Depth_Keller msg: 1613176402162 epoch sec; seqNo:13676; depth 4.87 2021-02-13T00:33:22Z- handleDvl() - RDI_Pathfinder msg: 1613176402.18 epoch sec; seqNo:6638 2021-02-13T00:33:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 113.40 , 97.17 , 97.17 2021-02-13T00:33:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.40 , 0.35 2021-02-13T00:33:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176402.21 epoch sec; seqNo:13676; 4059460.28 north; 592746.05 east 2021-02-13T00:33:22Z- AHRS_M2 msg: 1613176402.56 epoch sec; seqNo:13569 2021-02-13T00:33:22Z- AHRS_M2 msg: 0.69 phi; -0.13 theta; -0.18 psi 2021-02-13T00:33:22Z- handleDepth()-Depth_Keller msg: 1613176402561 epoch sec; seqNo:13677; depth 4.82 2021-02-13T00:33:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176402.57 epoch sec; seqNo:13677; 4059460.28 north; 592746.05 east 2021-02-13T00:33:22Z- AHRS_M2 msg: 1613176402.96 epoch sec; seqNo:13570 2021-02-13T00:33:22Z- AHRS_M2 msg: 0.69 phi; -0.17 theta; -0.14 psi 2021-02-13T00:33:22Z- handleDepth()-Depth_Keller msg: 1613176402964 epoch sec; seqNo:13678; depth 4.79 2021-02-13T00:33:22Z- handleDvl() - RDI_Pathfinder msg: 1613176402.98 epoch sec; seqNo:6639 2021-02-13T00:33:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.36 , 106.64 , 97.17 , 102.58 2021-02-13T00:33:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.06 , 0.53 2021-02-13T00:33:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176402.99 epoch sec; seqNo:13678; 4059460.66 north; 592746.05 east 2021-02-13T00:33:23Z- AHRS_M2 msg: 1613176403.36 epoch sec; seqNo:13571 2021-02-13T00:33:23Z- AHRS_M2 msg: 0.69 phi; -0.20 theta; -0.11 psi 2021-02-13T00:33:23Z- handleDepth()-Depth_Keller msg: 1613176403368 epoch sec; seqNo:13679; depth 4.78 2021-02-13T00:33:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176403.38 epoch sec; seqNo:13679; 4059460.66 north; 592746.05 east 2021-02-13T00:33:23Z- AHRS_M2 msg: 1613176403.77 epoch sec; seqNo:13572 2021-02-13T00:33:23Z- AHRS_M2 msg: 0.70 phi; -0.22 theta; -0.10 psi 2021-02-13T00:33:23Z- handleDepth()-Depth_Keller msg: 1613176403772 epoch sec; seqNo:13680; depth 4.80 2021-02-13T00:33:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176403.78 epoch sec; seqNo:13680; 4059461.04 north; 592746.04 east 2021-02-13T00:33:24Z- AHRS_M2 msg: 1613176404.18 epoch sec; seqNo:13573 2021-02-13T00:33:24Z- AHRS_M2 msg: 0.70 phi; -0.25 theta; -0.10 psi 2021-02-13T00:33:24Z- handleDepth()-Depth_Keller msg: 1613176404191 epoch sec; seqNo:13681; depth 4.83 2021-02-13T00:33:24Z- handleDvl() - RDI_Pathfinder msg: 1613176404.20 epoch sec; seqNo:6640 2021-02-13T00:33:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.76 , 102.58 , 86.36 , 109.34 2021-02-13T00:33:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.36 , 0.49 2021-02-13T00:33:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176404.21 epoch sec; seqNo:13681; 4059461.04 north; 592746.04 east 2021-02-13T00:33:24Z- AHRS_M2 msg: 1613176404.58 epoch sec; seqNo:13574 2021-02-13T00:33:24Z- AHRS_M2 msg: 0.70 phi; -0.28 theta; -0.12 psi 2021-02-13T00:33:24Z- handleDepth()-Depth_Keller msg: 1613176404580 epoch sec; seqNo:13682; depth 4.85 2021-02-13T00:33:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176404.60 epoch sec; seqNo:13682; 4059461.05 north; 592747.26 east 2021-02-13T00:33:24Z- AHRS_M2 msg: 1613176404.98 epoch sec; seqNo:13575 2021-02-13T00:33:24Z- AHRS_M2 msg: 0.70 phi; -0.30 theta; -0.14 psi 2021-02-13T00:33:24Z- handleDepth()-Depth_Keller msg: 1613176404984 epoch sec; seqNo:13683; depth 4.92 2021-02-13T00:33:24Z- handleDvl() - RDI_Pathfinder msg: 1613176405.00 epoch sec; seqNo:6641 2021-02-13T00:33:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 101.30 , 85.07 , 114.82 2021-02-13T00:33:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.53 , 0.06 2021-02-13T00:33:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176405.02 epoch sec; seqNo:13683; 4059461.43 north; 592747.26 east 2021-02-13T00:33:25Z- AHRS_M2 msg: 1613176405.39 epoch sec; seqNo:13576 2021-02-13T00:33:25Z- AHRS_M2 msg: 0.71 phi; -0.32 theta; -0.17 psi 2021-02-13T00:33:25Z- handleDepth()-Depth_Keller msg: 1613176405394 epoch sec; seqNo:13684; depth 5.00 2021-02-13T00:33:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176405.41 epoch sec; seqNo:13684; 4059461.43 north; 592747.26 east 2021-02-13T00:33:25Z- AHRS_M2 msg: 1613176405.79 epoch sec; seqNo:13577 2021-02-13T00:33:25Z- AHRS_M2 msg: 0.71 phi; -0.34 theta; -0.20 psi 2021-02-13T00:33:25Z- handleDepth()-Depth_Keller msg: 1613176405794 epoch sec; seqNo:13685; depth 5.07 2021-02-13T00:33:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176405.80 epoch sec; seqNo:13685; 4059461.82 north; 592748.48 east 2021-02-13T00:33:26Z- AHRS_M2 msg: 1613176406.19 epoch sec; seqNo:13578 2021-02-13T00:33:26Z- AHRS_M2 msg: 0.72 phi; -0.36 theta; -0.24 psi 2021-02-13T00:33:26Z- handleDepth()-Depth_Keller msg: 1613176406196 epoch sec; seqNo:13686; depth 5.18 2021-02-13T00:33:26Z- handleDvl() - RDI_Pathfinder msg: 1613176406.22 epoch sec; seqNo:6642 2021-02-13T00:33:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.63 , 107.85 , 83.52 , 124.08 2021-02-13T00:33:26Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.27 , -0.34 2021-02-13T00:33:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176406.23 epoch sec; seqNo:13686; 4059461.82 north; 592748.48 east 2021-02-13T00:33:26Z- AHRS_M2 msg: 1613176406.60 epoch sec; seqNo:13579 2021-02-13T00:33:26Z- AHRS_M2 msg: 0.72 phi; -0.36 theta; -0.27 psi 2021-02-13T00:33:26Z- handleDepth()-Depth_Keller msg: 1613176406600 epoch sec; seqNo:13687; depth 5.28 2021-02-13T00:33:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176406.61 epoch sec; seqNo:13687; 4059462.20 north; 592748.47 east 2021-02-13T00:33:26Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-13T00:33:26Z- waiting for 1613176404.37, time is 1613176406.61 2021-02-13T00:33:26Z- LcmTrn::updateTrn() >>>> pose time 1613176406.61, meas time 1613176406.22 <<<< 2021-02-13T00:33:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176406.22 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:33:26Z- TerrainNav::motionUpdate() - currEstimate = (4059462, 592748.47, 5.28) 2021-02-13T00:33:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:33:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:33:26Z- TNavBF::Projecting Measurements 2021-02-13T00:33:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:33:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:33:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:33:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:33:26Z- TNavFilter::4 good beams. 2021-02-13T00:33:26Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:33:26Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:33:26Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:33:26Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:33:26Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:33:26Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:33:26Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:33:26Z- TNavBF::mmse selected from filter -1 2021-02-13T00:33:26Z- TerrainMapOctree: (vehN, vehE ) = (4059462.00, 592748.29). 2021-02-13T00:33:26Z- TerrainMapOctree: Min Distance = 2334.97. 2021-02-13T00:33:26Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:33:26Z- There is no specific good beam that all particles have in common. 2021-02-13T00:33:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:33:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 13.43 2021-02-13T00:33:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:33:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 18.61 2021-02-13T00:33:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:33:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 11.16 2021-02-13T00:33:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:33:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 24.63 2021-02-13T00:33:26Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:33:26Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:33:26Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:33:26Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:33:26Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:33:26Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:33:26Z- Filter 0 No beams used in subcloud update 2021-02-13T00:33:26Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:33:26Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:33:26Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 697.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:33:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176406.22 sec. 2021-02-13T00:33:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:33:26Z- TNavBF::mmse selected from filter -1 2021-02-13T00:33:26Z- variances([0], [2], and psi_berg) = 697.536060 832.996664 0.000000 2021-02-13T00:33:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:33:26Z- LcmTrn::publishEstimates() - MLE : 4059461.70 592721.97 5.28 2021-02-13T00:33:26Z- LcmTrn::publishEstimates() - MMSE : 4059462.20 592748.47 5.28 2021-02-13T00:33:26Z- LcmTrn::publishEstimates() - COVAR: 697.54 833.00 0.96 0.00 2021-02-13T00:33:27Z- AHRS_M2 msg: 1613176407.02 epoch sec; seqNo:13580 2021-02-13T00:33:27Z- AHRS_M2 msg: 0.73 phi; -0.37 theta; -0.28 psi 2021-02-13T00:33:27Z- handleDepth()-Depth_Keller msg: 1613176407027 epoch sec; seqNo:13688; depth 5.38 2021-02-13T00:33:27Z- handleDvl() - RDI_Pathfinder msg: 1613176407.05 epoch sec; seqNo:6643 2021-02-13T00:33:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.55 , 117.32 , 84.60 , 126.24 2021-02-13T00:33:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.18 , -0.35 2021-02-13T00:33:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176407.06 epoch sec; seqNo:13688; 4059462.58 north; 592748.47 east 2021-02-13T00:33:27Z- AHRS_M2 msg: 1613176407.40 epoch sec; seqNo:13581 2021-02-13T00:33:27Z- AHRS_M2 msg: 0.73 phi; -0.37 theta; -0.28 psi 2021-02-13T00:33:27Z- handleDepth()-Depth_Keller msg: 1613176407413 epoch sec; seqNo:13689; depth 5.54 2021-02-13T00:33:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176407.42 epoch sec; seqNo:13689; 4059462.97 north; 592749.69 east 2021-02-13T00:33:27Z- AHRS_M2 msg: 1613176407.81 epoch sec; seqNo:13582 2021-02-13T00:33:27Z- AHRS_M2 msg: 0.73 phi; -0.38 theta; -0.25 psi 2021-02-13T00:33:27Z- handleDepth()-Depth_Keller msg: 1613176407817 epoch sec; seqNo:13690; depth 5.64 2021-02-13T00:33:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176407.83 epoch sec; seqNo:13690; 4059463.35 north; 592749.68 east 2021-02-13T00:33:28Z- AHRS_M2 msg: 1613176408.21 epoch sec; seqNo:13583 2021-02-13T00:33:28Z- AHRS_M2 msg: 0.72 phi; -0.39 theta; -0.23 psi 2021-02-13T00:33:28Z- handleDepth()-Depth_Keller msg: 1613176408216 epoch sec; seqNo:13691; depth 5.79 2021-02-13T00:33:28Z- handleDvl() - RDI_Pathfinder msg: 1613176408.23 epoch sec; seqNo:6644 2021-02-13T00:33:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.79 , 123.00 , 84.33 , 133.40 2021-02-13T00:33:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.39 , -0.01 2021-02-13T00:33:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176408.26 epoch sec; seqNo:13691; 4059463.73 north; 592749.68 east 2021-02-13T00:33:28Z- AHRS_M2 msg: 1613176408.62 epoch sec; seqNo:13584 2021-02-13T00:33:28Z- AHRS_M2 msg: 0.71 phi; -0.39 theta; -0.23 psi 2021-02-13T00:33:28Z- handleDepth()-Depth_Keller msg: 1613176408620 epoch sec; seqNo:13692; depth 5.91 2021-02-13T00:33:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176408.63 epoch sec; seqNo:13692; 4059464.11 north; 592749.67 east 2021-02-13T00:33:29Z- AHRS_M2 msg: 1613176409.02 epoch sec; seqNo:13585 2021-02-13T00:33:29Z- AHRS_M2 msg: 0.70 phi; -0.40 theta; -0.23 psi 2021-02-13T00:33:29Z- handleDepth()-Depth_Keller msg: 1613176409026 epoch sec; seqNo:13693; depth 6.06 2021-02-13T00:33:29Z- handleDvl() - RDI_Pathfinder msg: 1613176409.04 epoch sec; seqNo:6645 2021-02-13T00:33:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.44 , 116.78 , 82.57 , 134.62 2021-02-13T00:33:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.18 , 0.20 2021-02-13T00:33:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176409.05 epoch sec; seqNo:13693; 4059464.49 north; 592749.67 east 2021-02-13T00:33:29Z- AHRS_M2 msg: 1613176409.42 epoch sec; seqNo:13586 2021-02-13T00:33:29Z- AHRS_M2 msg: 0.69 phi; -0.41 theta; -0.21 psi 2021-02-13T00:33:29Z- handleDepth()-Depth_Keller msg: 1613176409428 epoch sec; seqNo:13694; depth 6.19 2021-02-13T00:33:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176409.45 epoch sec; seqNo:13694; 4059464.49 north; 592749.67 east 2021-02-13T00:33:29Z- AHRS_M2 msg: 1613176409.83 epoch sec; seqNo:13587 2021-02-13T00:33:29Z- AHRS_M2 msg: 0.67 phi; -0.41 theta; -0.21 psi 2021-02-13T00:33:29Z- handleDepth()-Depth_Keller msg: 1613176409841 epoch sec; seqNo:13695; depth 6.35 2021-02-13T00:33:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176409.86 epoch sec; seqNo:13695; 4059464.86 north; 592749.66 east 2021-02-13T00:33:30Z- AHRS_M2 msg: 1613176410.24 epoch sec; seqNo:13588 2021-02-13T00:33:30Z- AHRS_M2 msg: 0.65 phi; -0.40 theta; -0.21 psi 2021-02-13T00:33:30Z- handleDepth()-Depth_Keller msg: 1613176410243 epoch sec; seqNo:13696; depth 6.49 2021-02-13T00:33:30Z- handleDvl() - RDI_Pathfinder msg: 1613176410.25 epoch sec; seqNo:6646 2021-02-13T00:33:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.49 , 113.80 , 82.57 , 134.62 2021-02-13T00:33:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.10 , 0.14 2021-02-13T00:33:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176410.28 epoch sec; seqNo:13696; 4059464.86 north; 592749.66 east 2021-02-13T00:33:30Z- AHRS_M2 msg: 1613176410.64 epoch sec; seqNo:13589 2021-02-13T00:33:30Z- AHRS_M2 msg: 0.65 phi; -0.39 theta; -0.22 psi 2021-02-13T00:33:30Z- handleDepth()-Depth_Keller msg: 1613176410642 epoch sec; seqNo:13697; depth 6.63 2021-02-13T00:33:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176410.65 epoch sec; seqNo:13697; 4059465.26 north; 592750.88 east 2021-02-13T00:33:31Z- AHRS_M2 msg: 1613176411.07 epoch sec; seqNo:13590 2021-02-13T00:33:31Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.23 psi 2021-02-13T00:33:31Z- handleDepth()-Depth_Keller msg: 1613176411069 epoch sec; seqNo:13698; depth 6.77 2021-02-13T00:33:31Z- handleDvl() - RDI_Pathfinder msg: 1613176411.08 epoch sec; seqNo:6647 2021-02-13T00:33:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.70 , 114.00 , 82.77 , 136.31 2021-02-13T00:33:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.08 , -0.05 2021-02-13T00:33:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176411.09 epoch sec; seqNo:13698; 4059465.26 north; 592750.88 east 2021-02-13T00:33:31Z- AHRS_M2 msg: 1613176411.44 epoch sec; seqNo:13591 2021-02-13T00:33:31Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.24 psi 2021-02-13T00:33:31Z- handleDepth()-Depth_Keller msg: 1613176411448 epoch sec; seqNo:13699; depth 6.93 2021-02-13T00:33:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176411.46 epoch sec; seqNo:13699; 4059465.64 north; 592750.88 east 2021-02-13T00:33:31Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-13T00:33:31Z- waiting for 1613176409.61, time is 1613176411.46 2021-02-13T00:33:31Z- LcmTrn::updateTrn() >>>> pose time 1613176411.46, meas time 1613176411.08 <<<< 2021-02-13T00:33:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176411.08 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:33:31Z- TerrainNav::motionUpdate() - currEstimate = (4059466, 592750.88, 6.93) 2021-02-13T00:33:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:33:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:33:31Z- TNavBF::Projecting Measurements 2021-02-13T00:33:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:33:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:33:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:33:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:33:31Z- TNavFilter::4 good beams. 2021-02-13T00:33:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:33:31Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:33:31Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:33:31Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:33:31Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:33:31Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:33:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:33:31Z- TNavBF::mmse selected from filter -1 2021-02-13T00:33:31Z- TerrainMapOctree: (vehN, vehE ) = (4059465.37, 592750.69). 2021-02-13T00:33:31Z- TerrainMapOctree: Min Distance = 2330.85. 2021-02-13T00:33:31Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:33:31Z- There is no specific good beam that all particles have in common. 2021-02-13T00:33:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:33:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 13.45 2021-02-13T00:33:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:33:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 20.80 2021-02-13T00:33:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:33:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 10.96 2021-02-13T00:33:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:33:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.029 ExpectedMeasDiff: nan currMeas.covariance: 29.73 2021-02-13T00:33:31Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:33:31Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:33:31Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:33:31Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:33:31Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:33:31Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:33:31Z- Filter 0 No beams used in subcloud update 2021-02-13T00:33:31Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:33:31Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:33:31Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 697.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:33:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176411.08 sec. 2021-02-13T00:33:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:33:31Z- TNavBF::mmse selected from filter -1 2021-02-13T00:33:31Z- variances([0], [2], and psi_berg) = 697.469119 832.996630 0.000000 2021-02-13T00:33:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:33:31Z- LcmTrn::publishEstimates() - MLE : 4059465.14 592724.38 6.93 2021-02-13T00:33:31Z- LcmTrn::publishEstimates() - MMSE : 4059465.64 592750.88 6.93 2021-02-13T00:33:31Z- LcmTrn::publishEstimates() - COVAR: 697.47 833.00 0.94 0.00 2021-02-13T00:33:31Z- AHRS_M2 msg: 1613176411.85 epoch sec; seqNo:13592 2021-02-13T00:33:31Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.24 psi 2021-02-13T00:33:31Z- handleDepth()-Depth_Keller msg: 1613176411853 epoch sec; seqNo:13700; depth 7.06 2021-02-13T00:33:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176411.86 epoch sec; seqNo:13700; 4059466.01 north; 592750.87 east 2021-02-13T00:33:32Z- AHRS_M2 msg: 1613176412.25 epoch sec; seqNo:13593 2021-02-13T00:33:32Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.23 psi 2021-02-13T00:33:32Z- handleDepth()-Depth_Keller msg: 1613176412262 epoch sec; seqNo:13701; depth 7.22 2021-02-13T00:33:32Z- handleDvl() - RDI_Pathfinder msg: 1613176412.27 epoch sec; seqNo:6648 2021-02-13T00:33:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.65 , 115.42 , 82.71 , 128.81 2021-02-13T00:33:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.05 , -0.02 2021-02-13T00:33:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176412.28 epoch sec; seqNo:13701; 4059466.39 north; 592750.87 east 2021-02-13T00:33:32Z- AHRS_M2 msg: 1613176412.66 epoch sec; seqNo:13594 2021-02-13T00:33:32Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.21 psi 2021-02-13T00:33:32Z- handleDepth()-Depth_Keller msg: 1613176412665 epoch sec; seqNo:13702; depth 7.36 2021-02-13T00:33:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176412.68 epoch sec; seqNo:13702; 4059466.39 north; 592750.87 east 2021-02-13T00:33:33Z- AHRS_M2 msg: 1613176413.06 epoch sec; seqNo:13595 2021-02-13T00:33:33Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.19 psi 2021-02-13T00:33:33Z- handleDepth()-Depth_Keller msg: 1613176413066 epoch sec; seqNo:13703; depth 7.51 2021-02-13T00:33:33Z- handleDvl() - RDI_Pathfinder msg: 1613176413.08 epoch sec; seqNo:6649 2021-02-13T00:33:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.86 , 112.65 , 79.28 , 124.55 2021-02-13T00:33:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.02 , 0.11 2021-02-13T00:33:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176413.09 epoch sec; seqNo:13703; 4059466.79 north; 592752.09 east 2021-02-13T00:33:33Z- AHRS_M2 msg: 1613176413.46 epoch sec; seqNo:13596 2021-02-13T00:33:33Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.18 psi 2021-02-13T00:33:33Z- handleDepth()-Depth_Keller msg: 1613176413476 epoch sec; seqNo:13704; depth 7.64 2021-02-13T00:33:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176413.49 epoch sec; seqNo:13704; 4059467.16 north; 592752.08 east 2021-02-13T00:33:33Z- AHRS_M2 msg: 1613176413.87 epoch sec; seqNo:13597 2021-02-13T00:33:33Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:33:33Z- handleDepth()-Depth_Keller msg: 1613176413872 epoch sec; seqNo:13705; depth 7.76 2021-02-13T00:33:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176413.89 epoch sec; seqNo:13705; 4059467.16 north; 592752.08 east 2021-02-13T00:33:34Z- AHRS_M2 msg: 1613176414.27 epoch sec; seqNo:13598 2021-02-13T00:33:34Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.19 psi 2021-02-13T00:33:34Z- handleDepth()-Depth_Keller msg: 1613176414276 epoch sec; seqNo:13706; depth 7.89 2021-02-13T00:33:34Z- handleDvl() - RDI_Pathfinder msg: 1613176414.29 epoch sec; seqNo:6650 2021-02-13T00:33:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 108.39 , 81.62 , 124.75 2021-02-13T00:33:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.18 , 0.08 2021-02-13T00:33:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176414.31 epoch sec; seqNo:13706; 4059467.54 north; 592752.08 east 2021-02-13T00:33:34Z- AHRS_M2 msg: 1613176414.68 epoch sec; seqNo:13599 2021-02-13T00:33:34Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.20 psi 2021-02-13T00:33:34Z- handleDepth()-Depth_Keller msg: 1613176414680 epoch sec; seqNo:13707; depth 8.04 2021-02-13T00:33:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176414.69 epoch sec; seqNo:13707; 4059467.54 north; 592752.08 east 2021-02-13T00:33:35Z- AHRS_M2 msg: 1613176415.10 epoch sec; seqNo:13600 2021-02-13T00:33:35Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.21 psi 2021-02-13T00:33:35Z- handleDepth()-Depth_Keller msg: 1613176415111 epoch sec; seqNo:13708; depth 8.15 2021-02-13T00:33:35Z- handleDvl() - RDI_Pathfinder msg: 1613176415.12 epoch sec; seqNo:6651 2021-02-13T00:33:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 107.79 , 80.75 , 121.31 2021-02-13T00:33:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.25 , -0.13 2021-02-13T00:33:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176415.15 epoch sec; seqNo:13708; 4059467.92 north; 592752.07 east 2021-02-13T00:33:35Z- AHRS_M2 msg: 1613176415.48 epoch sec; seqNo:13601 2021-02-13T00:33:35Z- AHRS_M2 msg: 0.57 phi; -0.31 theta; -0.23 psi 2021-02-13T00:33:35Z- handleDepth()-Depth_Keller msg: 1613176415488 epoch sec; seqNo:13709; depth 8.31 2021-02-13T00:33:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176415.50 epoch sec; seqNo:13709; 4059468.31 north; 592753.29 east 2021-02-13T00:33:35Z- AHRS_M2 msg: 1613176415.89 epoch sec; seqNo:13602 2021-02-13T00:33:35Z- AHRS_M2 msg: 0.57 phi; -0.31 theta; -0.24 psi 2021-02-13T00:33:35Z- handleDepth()-Depth_Keller msg: 1613176415892 epoch sec; seqNo:13710; depth 8.41 2021-02-13T00:33:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176415.90 epoch sec; seqNo:13710; 4059468.31 north; 592753.29 east 2021-02-13T00:33:36Z- AHRS_M2 msg: 1613176416.29 epoch sec; seqNo:13603 2021-02-13T00:33:36Z- AHRS_M2 msg: 0.57 phi; -0.30 theta; -0.24 psi 2021-02-13T00:33:36Z- handleDepth()-Depth_Keller msg: 1613176416297 epoch sec; seqNo:13711; depth 8.51 2021-02-13T00:33:36Z- handleDvl() - RDI_Pathfinder msg: 1613176416.31 epoch sec; seqNo:6652 2021-02-13T00:33:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.09 , 112.86 , 83.11 , 120.29 2021-02-13T00:33:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.08 , -0.31 2021-02-13T00:33:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176416.32 epoch sec; seqNo:13711; 4059468.69 north; 592753.29 east 2021-02-13T00:33:36Z- AHRS_M2 msg: 1613176416.70 epoch sec; seqNo:13604 2021-02-13T00:33:36Z- AHRS_M2 msg: 0.57 phi; -0.29 theta; -0.25 psi 2021-02-13T00:33:36Z- handleDepth()-Depth_Keller msg: 1613176416713 epoch sec; seqNo:13712; depth 8.63 2021-02-13T00:33:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176416.72 epoch sec; seqNo:13712; 4059469.07 north; 592753.28 east 2021-02-13T00:33:36Z- LcmTrn::handleMessages() - handling 44 messages took 0.62 ms and 132 calls... 2021-02-13T00:33:36Z- waiting for 1613176414.46, time is 1613176416.72 2021-02-13T00:33:36Z- LcmTrn::updateTrn() >>>> pose time 1613176416.72, meas time 1613176416.31 <<<< 2021-02-13T00:33:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176416.31 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:33:36Z- TerrainNav::motionUpdate() - currEstimate = (4059469, 592753.28, 8.63) 2021-02-13T00:33:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:33:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:33:36Z- TNavBF::Projecting Measurements 2021-02-13T00:33:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:33:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:33:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:33:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:33:36Z- TNavFilter::4 good beams. 2021-02-13T00:33:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:33:36Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:33:36Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:33:36Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:33:36Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:33:36Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:33:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:33:36Z- TNavBF::mmse selected from filter -1 2021-02-13T00:33:36Z- TerrainMapOctree: (vehN, vehE ) = (4059468.80, 592753.09). 2021-02-13T00:33:36Z- TerrainMapOctree: Min Distance = 2326.67. 2021-02-13T00:33:36Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:33:36Z- There is no specific good beam that all particles have in common. 2021-02-13T00:33:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:33:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 11.86 2021-02-13T00:33:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:33:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 20.38 2021-02-13T00:33:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:33:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 11.05 2021-02-13T00:33:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:33:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 23.15 2021-02-13T00:33:36Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:33:36Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:33:36Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:33:36Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:33:36Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:33:36Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:33:36Z- Filter 0 No beams used in subcloud update 2021-02-13T00:33:36Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:33:36Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:33:36Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 697.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:33:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176416.31 sec. 2021-02-13T00:33:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:33:36Z- TNavBF::mmse selected from filter -1 2021-02-13T00:33:36Z- variances([0], [2], and psi_berg) = 697.402191 832.996596 0.000000 2021-02-13T00:33:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:33:37Z- LcmTrn::publishEstimates() - MLE : 4059468.57 592726.78 8.63 2021-02-13T00:33:37Z- LcmTrn::publishEstimates() - MMSE : 4059469.07 592753.28 8.63 2021-02-13T00:33:37Z- LcmTrn::publishEstimates() - COVAR: 697.40 833.00 0.91 0.00 2021-02-13T00:33:37Z- AHRS_M2 msg: 1613176417.10 epoch sec; seqNo:13605 2021-02-13T00:33:37Z- AHRS_M2 msg: 0.57 phi; -0.28 theta; -0.24 psi 2021-02-13T00:33:37Z- handleDepth()-Depth_Keller msg: 1613176417109 epoch sec; seqNo:13713; depth 8.76 2021-02-13T00:33:37Z- handleDvl() - RDI_Pathfinder msg: 1613176417.12 epoch sec; seqNo:6653 2021-02-13T00:33:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 113.19 , 82.10 , 117.25 2021-02-13T00:33:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.14 , -0.25 2021-02-13T00:33:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176417.15 epoch sec; seqNo:13713; 4059469.45 north; 592753.28 east 2021-02-13T00:33:37Z- AHRS_M2 msg: 1613176417.50 epoch sec; seqNo:13606 2021-02-13T00:33:37Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.23 psi 2021-02-13T00:33:37Z- handleDepth()-Depth_Keller msg: 1613176417509 epoch sec; seqNo:13714; depth 8.89 2021-02-13T00:33:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176417.52 epoch sec; seqNo:13714; 4059469.83 north; 592753.27 east 2021-02-13T00:33:37Z- AHRS_M2 msg: 1613176417.91 epoch sec; seqNo:13607 2021-02-13T00:33:37Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.21 psi 2021-02-13T00:33:37Z- handleDepth()-Depth_Keller msg: 1613176417921 epoch sec; seqNo:13715; depth 9.00 2021-02-13T00:33:37Z- handleDvl() - RDI_Pathfinder msg: 1613176417.93 epoch sec; seqNo:6654 2021-02-13T00:33:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:33:37Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:33:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176417.94 epoch sec; seqNo:13715; 4059470.22 north; 592754.49 east 2021-02-13T00:33:38Z- AHRS_M2 msg: 1613176418.31 epoch sec; seqNo:13608 2021-02-13T00:33:38Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.19 psi 2021-02-13T00:33:38Z- handleDepth()-Depth_Keller msg: 1613176418316 epoch sec; seqNo:13716; depth 9.11 2021-02-13T00:33:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176418.34 epoch sec; seqNo:13716; 4059470.60 north; 592754.49 east 2021-02-13T00:33:38Z- AHRS_M2 msg: 1613176418.72 epoch sec; seqNo:13609 2021-02-13T00:33:38Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.17 psi 2021-02-13T00:33:38Z- handleDepth()-Depth_Keller msg: 1613176418720 epoch sec; seqNo:13717; depth 9.21 2021-02-13T00:33:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176418.73 epoch sec; seqNo:13717; 4059470.98 north; 592754.48 east 2021-02-13T00:33:39Z- AHRS_M2 msg: 1613176419.12 epoch sec; seqNo:13610 2021-02-13T00:33:39Z- AHRS_M2 msg: 0.57 phi; -0.27 theta; -0.16 psi 2021-02-13T00:33:39Z- handleDepth()-Depth_Keller msg: 1613176419124 epoch sec; seqNo:13718; depth 9.31 2021-02-13T00:33:39Z- handleDvl() - RDI_Pathfinder msg: 1613176419.14 epoch sec; seqNo:6655 2021-02-13T00:33:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.40 , 108.19 , 79.93 , 112.65 2021-02-13T00:33:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.21 , 0.21 2021-02-13T00:33:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176419.15 epoch sec; seqNo:13718; 4059471.36 north; 592754.48 east 2021-02-13T00:33:39Z- AHRS_M2 msg: 1613176419.53 epoch sec; seqNo:13611 2021-02-13T00:33:39Z- AHRS_M2 msg: 0.57 phi; -0.27 theta; -0.15 psi 2021-02-13T00:33:39Z- handleDepth()-Depth_Keller msg: 1613176419539 epoch sec; seqNo:13719; depth 9.42 2021-02-13T00:33:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176419.55 epoch sec; seqNo:13719; 4059471.36 north; 592754.48 east 2021-02-13T00:33:39Z- AHRS_M2 msg: 1613176419.93 epoch sec; seqNo:13612 2021-02-13T00:33:39Z- AHRS_M2 msg: 0.56 phi; -0.27 theta; -0.16 psi 2021-02-13T00:33:39Z- handleDepth()-Depth_Keller msg: 1613176419932 epoch sec; seqNo:13720; depth 9.52 2021-02-13T00:33:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176419.94 epoch sec; seqNo:13720; 4059471.74 north; 592754.47 east 2021-02-13T00:33:40Z- AHRS_M2 msg: 1613176420.33 epoch sec; seqNo:13613 2021-02-13T00:33:40Z- AHRS_M2 msg: 0.56 phi; -0.27 theta; -0.16 psi 2021-02-13T00:33:40Z- handleDepth()-Depth_Keller msg: 1613176420336 epoch sec; seqNo:13721; depth 9.61 2021-02-13T00:33:40Z- handleDvl() - RDI_Pathfinder msg: 1613176420.35 epoch sec; seqNo:6656 2021-02-13T00:33:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 101.43 , 81.15 , 113.60 2021-02-13T00:33:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , 0.35 2021-02-13T00:33:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176420.36 epoch sec; seqNo:13721; 4059472.12 north; 592754.47 east 2021-02-13T00:33:40Z- AHRS_M2 msg: 1613176420.74 epoch sec; seqNo:13614 2021-02-13T00:33:40Z- AHRS_M2 msg: 0.55 phi; -0.27 theta; -0.16 psi 2021-02-13T00:33:40Z- handleDepth()-Depth_Keller msg: 1613176420742 epoch sec; seqNo:13722; depth 9.69 2021-02-13T00:33:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176420.75 epoch sec; seqNo:13722; 4059472.12 north; 592754.47 east 2021-02-13T00:33:41Z- AHRS_M2 msg: 1613176421.14 epoch sec; seqNo:13615 2021-02-13T00:33:41Z- AHRS_M2 msg: 0.55 phi; -0.27 theta; -0.16 psi 2021-02-13T00:33:41Z- handleDepth()-Depth_Keller msg: 1613176421148 epoch sec; seqNo:13723; depth 9.78 2021-02-13T00:33:41Z- handleDvl() - RDI_Pathfinder msg: 1613176421.17 epoch sec; seqNo:6657 2021-02-13T00:33:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 102.78 , 78.87 , 110.89 2021-02-13T00:33:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.15 , 0.31 2021-02-13T00:33:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176421.18 epoch sec; seqNo:13723; 4059472.51 north; 592755.69 east 2021-02-13T00:33:41Z- AHRS_M2 msg: 1613176421.55 epoch sec; seqNo:13616 2021-02-13T00:33:41Z- AHRS_M2 msg: 0.56 phi; -0.27 theta; -0.15 psi 2021-02-13T00:33:41Z- handleDepth()-Depth_Keller msg: 1613176421557 epoch sec; seqNo:13724; depth 9.88 2021-02-13T00:33:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176421.58 epoch sec; seqNo:13724; 4059472.89 north; 592755.68 east 2021-02-13T00:33:41Z- AHRS_M2 msg: 1613176421.97 epoch sec; seqNo:13617 2021-02-13T00:33:41Z- AHRS_M2 msg: 0.56 phi; -0.28 theta; -0.16 psi 2021-02-13T00:33:41Z- handleDepth()-Depth_Keller msg: 1613176421970 epoch sec; seqNo:13725; depth 9.97 2021-02-13T00:33:41Z- handleDvl() - RDI_Pathfinder msg: 1613176421.99 epoch sec; seqNo:6658 2021-02-13T00:33:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 98.86 , 79.28 , 111.03 2021-02-13T00:33:41Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.26 , 0.06 2021-02-13T00:33:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176422.00 epoch sec; seqNo:13725; 4059473.27 north; 592755.68 east 2021-02-13T00:33:42Z- LcmTrn::handleMessages() - handling 45 messages took 0.69 ms and 131 calls... 2021-02-13T00:33:42Z- waiting for 1613176419.72, time is 1613176422.00 2021-02-13T00:33:42Z- LcmTrn::updateTrn() equating pose 1613176422.00 and meas times 1613176422.00 2021-02-13T00:33:42Z- LcmTrn::updateTrn() >>>> pose time 1613176422.00, meas time 1613176422.00 <<<< 2021-02-13T00:33:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176422.00 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:33:42Z- TerrainNav::motionUpdate() - currEstimate = (4059473, 592755.68, 9.97) 2021-02-13T00:33:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:33:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:33:42Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176422.00 sec.; No relevant navigation data available 2021-02-13T00:33:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:33:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:33:42Z- variances([0], [2], and psi_berg) = 697.344339 832.996566 0.000000 2021-02-13T00:33:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:33:42Z- LcmTrn::publishEstimates() - MLE : 4059472.77 592789.18 9.97 2021-02-13T00:33:42Z- LcmTrn::publishEstimates() - MMSE : 4059473.27 592755.68 9.97 2021-02-13T00:33:42Z- LcmTrn::publishEstimates() - COVAR: 697.34 833.00 1.16 0.00 2021-02-13T00:33:42Z- AHRS_M2 msg: 1613176422.35 epoch sec; seqNo:13618 2021-02-13T00:33:42Z- AHRS_M2 msg: 0.57 phi; -0.28 theta; -0.16 psi 2021-02-13T00:33:42Z- handleDepth()-Depth_Keller msg: 1613176422357 epoch sec; seqNo:13726; depth 10.09 2021-02-13T00:33:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176422.37 epoch sec; seqNo:13726; 4059473.27 north; 592755.68 east 2021-02-13T00:33:42Z- AHRS_M2 msg: 1613176422.76 epoch sec; seqNo:13619 2021-02-13T00:33:42Z- AHRS_M2 msg: 0.57 phi; -0.28 theta; -0.15 psi 2021-02-13T00:33:42Z- handleDepth()-Depth_Keller msg: 1613176422762 epoch sec; seqNo:13727; depth 10.17 2021-02-13T00:33:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176422.78 epoch sec; seqNo:13727; 4059473.64 north; 592755.67 east 2021-02-13T00:33:43Z- AHRS_M2 msg: 1613176423.16 epoch sec; seqNo:13620 2021-02-13T00:33:43Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.15 psi 2021-02-13T00:33:43Z- handleDepth()-Depth_Keller msg: 1613176423165 epoch sec; seqNo:13728; depth 10.28 2021-02-13T00:33:43Z- handleDvl() - RDI_Pathfinder msg: 1613176423.18 epoch sec; seqNo:6659 2021-02-13T00:33:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.76 , 102.99 , 80.00 , 112.45 2021-02-13T00:33:43Z- handleDvl() - RDI_Pathfinder msg: velocities 1.12 , 0.20 , -0.21 2021-02-13T00:33:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176423.20 epoch sec; seqNo:13728; 4059474.04 north; 592756.89 east 2021-02-13T00:33:43Z- AHRS_M2 msg: 1613176423.57 epoch sec; seqNo:13621 2021-02-13T00:33:43Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.15 psi 2021-02-13T00:33:43Z- handleDepth()-Depth_Keller msg: 1613176423571 epoch sec; seqNo:13729; depth 10.37 2021-02-13T00:33:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176423.58 epoch sec; seqNo:13729; 4059474.42 north; 592756.89 east 2021-02-13T00:33:43Z- AHRS_M2 msg: 1613176423.97 epoch sec; seqNo:13622 2021-02-13T00:33:43Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.16 psi 2021-02-13T00:33:43Z- handleDepth()-Depth_Keller msg: 1613176423986 epoch sec; seqNo:13730; depth 10.46 2021-02-13T00:33:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176423.99 epoch sec; seqNo:13730; 4059474.79 north; 592756.88 east 2021-02-13T00:33:44Z- AHRS_M2 msg: 1613176424.37 epoch sec; seqNo:13623 2021-02-13T00:33:44Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.17 psi 2021-02-13T00:33:44Z- handleDepth()-Depth_Keller msg: 1613176424376 epoch sec; seqNo:13731; depth 10.56 2021-02-13T00:33:44Z- handleDvl() - RDI_Pathfinder msg: 1613176424.39 epoch sec; seqNo:6660 2021-02-13T00:33:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.28 , 101.50 , 79.87 , 109.61 2021-02-13T00:33:44Z- handleDvl() - RDI_Pathfinder msg: velocities 1.08 , -0.01 , -0.29 2021-02-13T00:33:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176424.41 epoch sec; seqNo:13731; 4059475.17 north; 592756.88 east 2021-02-13T00:33:44Z- AHRS_M2 msg: 1613176424.78 epoch sec; seqNo:13624 2021-02-13T00:33:44Z- AHRS_M2 msg: 0.61 phi; -0.28 theta; -0.18 psi 2021-02-13T00:33:44Z- handleDepth()-Depth_Keller msg: 1613176424780 epoch sec; seqNo:13732; depth 10.67 2021-02-13T00:33:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176424.79 epoch sec; seqNo:13732; 4059475.56 north; 592758.09 east 2021-02-13T00:33:45Z- AHRS_M2 msg: 1613176425.18 epoch sec; seqNo:13625 2021-02-13T00:33:45Z- AHRS_M2 msg: 0.61 phi; -0.28 theta; -0.18 psi 2021-02-13T00:33:45Z- handleDepth()-Depth_Keller msg: 1613176425185 epoch sec; seqNo:13733; depth 10.79 2021-02-13T00:33:45Z- handleDvl() - RDI_Pathfinder msg: 1613176425.20 epoch sec; seqNo:6661 2021-02-13T00:33:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 102.92 , 79.93 , 109.68 2021-02-13T00:33:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.22 , -0.18 2021-02-13T00:33:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176425.21 epoch sec; seqNo:13733; 4059475.94 north; 592758.09 east 2021-02-13T00:33:45Z- AHRS_M2 msg: 1613176425.58 epoch sec; seqNo:13626 2021-02-13T00:33:45Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.18 psi 2021-02-13T00:33:45Z- handleDepth()-Depth_Keller msg: 1613176425599 epoch sec; seqNo:13734; depth 10.88 2021-02-13T00:33:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176425.61 epoch sec; seqNo:13734; 4059476.32 north; 592758.09 east 2021-02-13T00:33:45Z- AHRS_M2 msg: 1613176425.99 epoch sec; seqNo:13627 2021-02-13T00:33:45Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.17 psi 2021-02-13T00:33:45Z- handleDepth()-Depth_Keller msg: 1613176425997 epoch sec; seqNo:13735; depth 10.98 2021-02-13T00:33:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176426.04 epoch sec; seqNo:13735; 4059476.70 north; 592758.08 east 2021-02-13T00:33:46Z- AHRS_M2 msg: 1613176426.39 epoch sec; seqNo:13628 2021-02-13T00:33:46Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.17 psi 2021-02-13T00:33:46Z- handleDepth()-Depth_Keller msg: 1613176426396 epoch sec; seqNo:13736; depth 11.08 2021-02-13T00:33:46Z- handleDvl() - RDI_Pathfinder msg: 1613176426.41 epoch sec; seqNo:6662 2021-02-13T00:33:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 102.92 , 78.58 , 109.68 2021-02-13T00:33:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.26 , 0.02 2021-02-13T00:33:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176426.42 epoch sec; seqNo:13736; 4059477.08 north; 592758.08 east 2021-02-13T00:33:46Z- AHRS_M2 msg: 1613176426.80 epoch sec; seqNo:13629 2021-02-13T00:33:46Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.16 psi 2021-02-13T00:33:46Z- handleDepth()-Depth_Keller msg: 1613176426804 epoch sec; seqNo:13737; depth 11.18 2021-02-13T00:33:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176426.82 epoch sec; seqNo:13737; 4059477.08 north; 592758.08 east 2021-02-13T00:33:47Z- LcmTrn::handleMessages() - handling 40 messages took 0.71 ms and 126 calls... 2021-02-13T00:33:47Z- waiting for 1613176425.00, time is 1613176426.82 2021-02-13T00:33:47Z- LcmTrn::updateTrn() >>>> pose time 1613176426.82, meas time 1613176426.41 <<<< 2021-02-13T00:33:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176426.41 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:33:47Z- TerrainNav::motionUpdate() - currEstimate = (4059477, 592758.08, 11.18) 2021-02-13T00:33:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:33:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:33:47Z- TNavBF::Projecting Measurements 2021-02-13T00:33:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:33:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:33:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:33:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:33:47Z- TNavFilter::4 good beams. 2021-02-13T00:33:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:33:47Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:33:47Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:33:47Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:33:47Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:33:47Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:33:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:33:47Z- TNavBF::mmse selected from filter -1 2021-02-13T00:33:47Z- TerrainMapOctree: (vehN, vehE ) = (4059476.76, 592757.87). 2021-02-13T00:33:47Z- TerrainMapOctree: Min Distance = 2317.51. 2021-02-13T00:33:47Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:33:47Z- There is no specific good beam that all particles have in common. 2021-02-13T00:33:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:33:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 11.29 2021-02-13T00:33:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:33:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 16.95 2021-02-13T00:33:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:33:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-13T00:33:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:33:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 19.25 2021-02-13T00:33:47Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:33:47Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:33:47Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:33:47Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:33:47Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:33:47Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:33:47Z- Filter 0 No beams used in subcloud update 2021-02-13T00:33:47Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:33:47Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:33:47Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 697.3 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:33:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176426.41 sec. 2021-02-13T00:33:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:33:47Z- TNavBF::mmse selected from filter -1 2021-02-13T00:33:47Z- variances([0], [2], and psi_berg) = 697.274626 832.996531 0.000000 2021-02-13T00:33:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:33:47Z- LcmTrn::publishEstimates() - MLE : 4059476.58 592791.58 11.18 2021-02-13T00:33:47Z- LcmTrn::publishEstimates() - MMSE : 4059477.08 592758.08 11.18 2021-02-13T00:33:47Z- LcmTrn::publishEstimates() - COVAR: 697.27 833.00 1.02 0.00 2021-02-13T00:33:47Z- AHRS_M2 msg: 1613176427.21 epoch sec; seqNo:13630 2021-02-13T00:33:47Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.15 psi 2021-02-13T00:33:47Z- handleDepth()-Depth_Keller msg: 1613176427210 epoch sec; seqNo:13738; depth 11.29 2021-02-13T00:33:47Z- handleDvl() - RDI_Pathfinder msg: 1613176427.23 epoch sec; seqNo:6663 2021-02-13T00:33:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 98.93 , 78.65 , 111.10 2021-02-13T00:33:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.18 , 0.18 2021-02-13T00:33:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176427.24 epoch sec; seqNo:13738; 4059477.46 north; 592758.07 east 2021-02-13T00:33:47Z- AHRS_M2 msg: 1613176427.61 epoch sec; seqNo:13631 2021-02-13T00:33:47Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.15 psi 2021-02-13T00:33:47Z- handleDepth()-Depth_Keller msg: 1613176427617 epoch sec; seqNo:13739; depth 11.39 2021-02-13T00:33:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176427.63 epoch sec; seqNo:13739; 4059477.84 north; 592758.07 east 2021-02-13T00:33:48Z- AHRS_M2 msg: 1613176428.01 epoch sec; seqNo:13632 2021-02-13T00:33:48Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.15 psi 2021-02-13T00:33:48Z- handleDepth()-Depth_Keller msg: 1613176428013 epoch sec; seqNo:13740; depth 11.48 2021-02-13T00:33:48Z- handleDvl() - RDI_Pathfinder msg: 1613176428.03 epoch sec; seqNo:6664 2021-02-13T00:33:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.35 , 78.72 , 115.22 2021-02-13T00:33:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , 0.24 2021-02-13T00:33:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176428.04 epoch sec; seqNo:13740; 4059477.84 north; 592758.07 east 2021-02-13T00:33:48Z- AHRS_M2 msg: 1613176428.41 epoch sec; seqNo:13633 2021-02-13T00:33:48Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.16 psi 2021-02-13T00:33:48Z- handleDepth()-Depth_Keller msg: 1613176428418 epoch sec; seqNo:13741; depth 11.59 2021-02-13T00:33:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176428.43 epoch sec; seqNo:13741; 4059478.23 north; 592759.29 east 2021-02-13T00:33:48Z- AHRS_M2 msg: 1613176428.82 epoch sec; seqNo:13634 2021-02-13T00:33:48Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.16 psi 2021-02-13T00:33:48Z- handleDepth()-Depth_Keller msg: 1613176428825 epoch sec; seqNo:13742; depth 11.69 2021-02-13T00:33:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176428.83 epoch sec; seqNo:13742; 4059478.23 north; 592759.29 east 2021-02-13T00:33:49Z- AHRS_M2 msg: 1613176429.22 epoch sec; seqNo:13635 2021-02-13T00:33:49Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-13T00:33:49Z- handleDepth()-Depth_Keller msg: 1613176429229 epoch sec; seqNo:13743; depth 11.79 2021-02-13T00:33:49Z- handleDvl() - RDI_Pathfinder msg: 1613176429.24 epoch sec; seqNo:6665 2021-02-13T00:33:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 99.88 , 76.83 , 114.75 2021-02-13T00:33:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.18 , 0.18 2021-02-13T00:33:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176429.25 epoch sec; seqNo:13743; 4059478.61 north; 592759.28 east 2021-02-13T00:33:49Z- AHRS_M2 msg: 1613176429.63 epoch sec; seqNo:13636 2021-02-13T00:33:49Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.20 psi 2021-02-13T00:33:49Z- handleDepth()-Depth_Keller msg: 1613176429630 epoch sec; seqNo:13744; depth 11.91 2021-02-13T00:33:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176429.64 epoch sec; seqNo:13744; 4059478.99 north; 592759.28 east 2021-02-13T00:33:50Z- AHRS_M2 msg: 1613176430.03 epoch sec; seqNo:13637 2021-02-13T00:33:50Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.20 psi 2021-02-13T00:33:50Z- handleDepth()-Depth_Keller msg: 1613176430043 epoch sec; seqNo:13745; depth 12.03 2021-02-13T00:33:50Z- handleDvl() - RDI_Pathfinder msg: 1613176430.06 epoch sec; seqNo:6666 2021-02-13T00:33:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 104.07 , 77.24 , 116.24 2021-02-13T00:33:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.23 , 0.01 2021-02-13T00:33:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176430.06 epoch sec; seqNo:13745; 4059478.99 north; 592759.28 east 2021-02-13T00:33:50Z- AHRS_M2 msg: 1613176430.43 epoch sec; seqNo:13638 2021-02-13T00:33:50Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.20 psi 2021-02-13T00:33:50Z- handleDepth()-Depth_Keller msg: 1613176430436 epoch sec; seqNo:13746; depth 12.15 2021-02-13T00:33:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176430.45 epoch sec; seqNo:13746; 4059479.38 north; 592760.49 east 2021-02-13T00:33:50Z- AHRS_M2 msg: 1613176430.84 epoch sec; seqNo:13639 2021-02-13T00:33:50Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.21 psi 2021-02-13T00:33:50Z- handleDepth()-Depth_Keller msg: 1613176430845 epoch sec; seqNo:13747; depth 12.26 2021-02-13T00:33:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176430.85 epoch sec; seqNo:13747; 4059479.76 north; 592760.49 east 2021-02-13T00:33:51Z- AHRS_M2 msg: 1613176431.24 epoch sec; seqNo:13640 2021-02-13T00:33:51Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.21 psi 2021-02-13T00:33:51Z- handleDepth()-Depth_Keller msg: 1613176431246 epoch sec; seqNo:13748; depth 12.37 2021-02-13T00:33:51Z- handleDvl() - RDI_Pathfinder msg: 1613176431.26 epoch sec; seqNo:6667 2021-02-13T00:33:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.49 , 74.37 , 117.65 2021-02-13T00:33:51Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.14 , -0.14 2021-02-13T00:33:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176431.27 epoch sec; seqNo:13748; 4059480.14 north; 592760.49 east 2021-02-13T00:33:51Z- AHRS_M2 msg: 1613176431.64 epoch sec; seqNo:13641 2021-02-13T00:33:51Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-13T00:33:51Z- handleDepth()-Depth_Keller msg: 1613176431649 epoch sec; seqNo:13749; depth 12.49 2021-02-13T00:33:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176431.67 epoch sec; seqNo:13749; 4059480.14 north; 592760.49 east 2021-02-13T00:33:52Z- AHRS_M2 msg: 1613176432.06 epoch sec; seqNo:13642 2021-02-13T00:33:52Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-13T00:33:52Z- handleDepth()-Depth_Keller msg: 1613176432059 epoch sec; seqNo:13750; depth 12.62 2021-02-13T00:33:52Z- handleDvl() - RDI_Pathfinder msg: 1613176432.07 epoch sec; seqNo:6668 2021-02-13T00:33:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.58 , 105.22 , 0.00 , 111.98 2021-02-13T00:33:52Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.02 , -0.21 2021-02-13T00:33:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176432.09 epoch sec; seqNo:13750; 4059480.52 north; 592760.48 east 2021-02-13T00:33:52Z- LcmTrn::handleMessages() - handling 45 messages took 0.68 ms and 132 calls... 2021-02-13T00:33:52Z- waiting for 1613176429.82, time is 1613176432.09 2021-02-13T00:33:52Z- LcmTrn::updateTrn() equating pose 1613176432.09 and meas times 1613176432.09 2021-02-13T00:33:52Z- LcmTrn::updateTrn() >>>> pose time 1613176432.09, meas time 1613176432.09 <<<< 2021-02-13T00:33:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176432.09 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:33:52Z- TerrainNav::motionUpdate() - currEstimate = (4059481, 592760.48, 12.62) 2021-02-13T00:33:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:33:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:33:52Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176432.09 sec.; No relevant navigation data available 2021-02-13T00:33:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:33:52Z- TNavBF::mmse selected from filter -1 2021-02-13T00:33:52Z- variances([0], [2], and psi_berg) = 697.220792 832.996503 0.000000 2021-02-13T00:33:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:33:52Z- LcmTrn::publishEstimates() - MLE : 4059480.02 592793.98 12.62 2021-02-13T00:33:52Z- LcmTrn::publishEstimates() - MMSE : 4059480.52 592760.48 12.62 2021-02-13T00:33:52Z- LcmTrn::publishEstimates() - COVAR: 697.22 833.00 1.24 0.00 2021-02-13T00:33:52Z- AHRS_M2 msg: 1613176432.45 epoch sec; seqNo:13643 2021-02-13T00:33:52Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-13T00:33:52Z- handleDepth()-Depth_Keller msg: 1613176432458 epoch sec; seqNo:13751; depth 12.75 2021-02-13T00:33:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176432.47 epoch sec; seqNo:13751; 4059480.91 north; 592761.70 east 2021-02-13T00:33:52Z- AHRS_M2 msg: 1613176432.86 epoch sec; seqNo:13644 2021-02-13T00:33:52Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:33:52Z- handleDepth()-Depth_Keller msg: 1613176432869 epoch sec; seqNo:13752; depth 12.86 2021-02-13T00:33:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176432.88 epoch sec; seqNo:13752; 4059481.29 north; 592761.69 east 2021-02-13T00:33:53Z- AHRS_M2 msg: 1613176433.26 epoch sec; seqNo:13645 2021-02-13T00:33:53Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:33:53Z- handleDepth()-Depth_Keller msg: 1613176433269 epoch sec; seqNo:13753; depth 12.99 2021-02-13T00:33:53Z- handleDvl() - RDI_Pathfinder msg: 1613176433.29 epoch sec; seqNo:6669 2021-02-13T00:33:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.52 , 103.80 , 75.19 , 113.26 2021-02-13T00:33:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.18 , -0.15 2021-02-13T00:33:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176433.30 epoch sec; seqNo:13753; 4059481.67 north; 592761.69 east 2021-02-13T00:33:53Z- AHRS_M2 msg: 1613176433.66 epoch sec; seqNo:13646 2021-02-13T00:33:53Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.17 psi 2021-02-13T00:33:53Z- handleDepth()-Depth_Keller msg: 1613176433673 epoch sec; seqNo:13754; depth 13.13 2021-02-13T00:33:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176433.68 epoch sec; seqNo:13754; 4059482.04 north; 592761.69 east 2021-02-13T00:33:54Z- AHRS_M2 msg: 1613176434.07 epoch sec; seqNo:13647 2021-02-13T00:33:54Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.17 psi 2021-02-13T00:33:54Z- handleDepth()-Depth_Keller msg: 1613176434072 epoch sec; seqNo:13755; depth 13.25 2021-02-13T00:33:54Z- handleDvl() - RDI_Pathfinder msg: 1613176434.08 epoch sec; seqNo:6670 2021-02-13T00:33:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 102.99 , 74.78 , 113.80 2021-02-13T00:33:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.24 , -0.02 2021-02-13T00:33:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176434.09 epoch sec; seqNo:13755; 4059482.42 north; 592761.68 east 2021-02-13T00:33:54Z- AHRS_M2 msg: 1613176434.47 epoch sec; seqNo:13648 2021-02-13T00:33:54Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.16 psi 2021-02-13T00:33:54Z- handleDepth()-Depth_Keller msg: 1613176434485 epoch sec; seqNo:13756; depth 13.37 2021-02-13T00:33:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176434.50 epoch sec; seqNo:13756; 4059482.80 north; 592761.68 east 2021-02-13T00:33:54Z- AHRS_M2 msg: 1613176434.88 epoch sec; seqNo:13649 2021-02-13T00:33:54Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.15 psi 2021-02-13T00:33:54Z- handleDepth()-Depth_Keller msg: 1613176434880 epoch sec; seqNo:13757; depth 13.49 2021-02-13T00:33:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176434.90 epoch sec; seqNo:13757; 4059483.18 north; 592761.67 east 2021-02-13T00:33:55Z- AHRS_M2 msg: 1613176435.28 epoch sec; seqNo:13650 2021-02-13T00:33:55Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.16 psi 2021-02-13T00:33:55Z- handleDepth()-Depth_Keller msg: 1613176435284 epoch sec; seqNo:13758; depth 13.60 2021-02-13T00:33:55Z- handleDvl() - RDI_Pathfinder msg: 1613176435.30 epoch sec; seqNo:6671 2021-02-13T00:33:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:33:55Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:33:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176435.33 epoch sec; seqNo:13758; 4059483.18 north; 592761.67 east 2021-02-13T00:33:55Z- AHRS_M2 msg: 1613176435.68 epoch sec; seqNo:13651 2021-02-13T00:33:55Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.17 psi 2021-02-13T00:33:55Z- handleDepth()-Depth_Keller msg: 1613176435688 epoch sec; seqNo:13759; depth 13.74 2021-02-13T00:33:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176435.70 epoch sec; seqNo:13759; 4059483.57 north; 592762.89 east 2021-02-13T00:33:56Z- AHRS_M2 msg: 1613176436.09 epoch sec; seqNo:13652 2021-02-13T00:33:56Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.18 psi 2021-02-13T00:33:56Z- handleDepth()-Depth_Keller msg: 1613176436094 epoch sec; seqNo:13760; depth 13.86 2021-02-13T00:33:56Z- handleDvl() - RDI_Pathfinder msg: 1613176436.11 epoch sec; seqNo:6672 2021-02-13T00:33:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.52 , 101.09 , 72.33 , 114.61 2021-02-13T00:33:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.11 , 0.19 2021-02-13T00:33:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176436.12 epoch sec; seqNo:13760; 4059483.95 north; 592762.89 east 2021-02-13T00:33:56Z- AHRS_M2 msg: 1613176436.49 epoch sec; seqNo:13653 2021-02-13T00:33:56Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.18 psi 2021-02-13T00:33:56Z- handleDepth()-Depth_Keller msg: 1613176436501 epoch sec; seqNo:13761; depth 13.97 2021-02-13T00:33:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176436.52 epoch sec; seqNo:13761; 4059484.33 north; 592762.88 east 2021-02-13T00:33:56Z- AHRS_M2 msg: 1613176436.90 epoch sec; seqNo:13654 2021-02-13T00:33:56Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.18 psi 2021-02-13T00:33:56Z- handleDepth()-Depth_Keller msg: 1613176436900 epoch sec; seqNo:13762; depth 14.11 2021-02-13T00:33:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176436.91 epoch sec; seqNo:13762; 4059484.33 north; 592762.88 east 2021-02-13T00:33:57Z- LcmTrn::handleMessages() - handling 40 messages took 0.65 ms and 127 calls... 2021-02-13T00:33:57Z- waiting for 1613176435.09, time is 1613176436.91 2021-02-13T00:33:57Z- LcmTrn::updateTrn() >>>> pose time 1613176436.91, meas time 1613176436.11 <<<< 2021-02-13T00:33:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176436.11 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:33:57Z- TerrainNav::motionUpdate() - currEstimate = (4059484, 592762.88, 14.11) 2021-02-13T00:33:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:33:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:33:57Z- TNavBF::Projecting Measurements 2021-02-13T00:33:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:33:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:33:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:33:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:33:57Z- TNavFilter::4 good beams. 2021-02-13T00:33:57Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:33:57Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:33:57Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:33:57Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:33:57Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:33:57Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:33:57Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:33:57Z- TNavBF::mmse selected from filter -1 2021-02-13T00:33:57Z- TerrainMapOctree: (vehN, vehE ) = (4059483.70, 592762.48). 2021-02-13T00:33:57Z- TerrainMapOctree: Min Distance = 2309.23. 2021-02-13T00:33:57Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:33:57Z- There is no specific good beam that all particles have in common. 2021-02-13T00:33:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:33:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 11.16 2021-02-13T00:33:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:33:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 16.35 2021-02-13T00:33:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:33:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-13T00:33:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:33:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 21.02 2021-02-13T00:33:57Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:33:57Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:33:57Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:33:57Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:33:57Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:33:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:33:57Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:33:57Z- Filter 0 No beams used in subcloud update 2021-02-13T00:33:57Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:33:57Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:33:57Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 697.2 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:33:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176436.11 sec. 2021-02-13T00:33:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:33:57Z- TNavBF::mmse selected from filter -1 2021-02-13T00:33:57Z- variances([0], [2], and psi_berg) = 697.147115 832.996466 0.000000 2021-02-13T00:33:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:33:57Z- LcmTrn::publishEstimates() - MLE : 4059483.83 592796.38 14.11 2021-02-13T00:33:57Z- LcmTrn::publishEstimates() - MMSE : 4059484.33 592762.88 14.11 2021-02-13T00:33:57Z- LcmTrn::publishEstimates() - COVAR: 697.15 833.00 1.06 0.00 2021-02-13T00:33:57Z- AHRS_M2 msg: 1613176437.30 epoch sec; seqNo:13655 2021-02-13T00:33:57Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.18 psi 2021-02-13T00:33:57Z- handleDepth()-Depth_Keller msg: 1613176437309 epoch sec; seqNo:13763; depth 14.24 2021-02-13T00:33:57Z- handleDvl() - RDI_Pathfinder msg: 1613176437.32 epoch sec; seqNo:6673 2021-02-13T00:33:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.13 , 101.70 , 78.72 , 116.57 2021-02-13T00:33:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , 0.16 2021-02-13T00:33:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176437.33 epoch sec; seqNo:13763; 4059484.71 north; 592762.88 east 2021-02-13T00:33:57Z- AHRS_M2 msg: 1613176437.70 epoch sec; seqNo:13656 2021-02-13T00:33:57Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.18 psi 2021-02-13T00:33:57Z- handleDepth()-Depth_Keller msg: 1613176437711 epoch sec; seqNo:13764; depth 14.37 2021-02-13T00:33:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176437.72 epoch sec; seqNo:13764; 4059485.09 north; 592762.87 east 2021-02-13T00:33:58Z- AHRS_M2 msg: 1613176438.11 epoch sec; seqNo:13657 2021-02-13T00:33:58Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.18 psi 2021-02-13T00:33:58Z- handleDepth()-Depth_Keller msg: 1613176438118 epoch sec; seqNo:13765; depth 14.50 2021-02-13T00:33:58Z- handleDvl() - RDI_Pathfinder msg: 1613176438.14 epoch sec; seqNo:6674 2021-02-13T00:33:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 100.15 , 78.51 , 119.07 2021-02-13T00:33:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.09 , 0.09 2021-02-13T00:33:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176438.15 epoch sec; seqNo:13765; 4059485.09 north; 592762.87 east 2021-02-13T00:33:58Z- AHRS_M2 msg: 1613176438.51 epoch sec; seqNo:13658 2021-02-13T00:33:58Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.18 psi 2021-02-13T00:33:58Z- handleDepth()-Depth_Keller msg: 1613176438516 epoch sec; seqNo:13766; depth 14.65 2021-02-13T00:33:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176438.53 epoch sec; seqNo:13766; 4059485.48 north; 592764.09 east 2021-02-13T00:33:58Z- AHRS_M2 msg: 1613176438.92 epoch sec; seqNo:13659 2021-02-13T00:33:58Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.19 psi 2021-02-13T00:33:58Z- handleDepth()-Depth_Keller msg: 1613176438930 epoch sec; seqNo:13767; depth 14.75 2021-02-13T00:33:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176438.97 epoch sec; seqNo:13767; 4059485.86 north; 592764.09 east 2021-02-13T00:33:59Z- AHRS_M2 msg: 1613176439.32 epoch sec; seqNo:13660 2021-02-13T00:33:59Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.19 psi 2021-02-13T00:33:59Z- handleDepth()-Depth_Keller msg: 1613176439339 epoch sec; seqNo:13768; depth 14.89 2021-02-13T00:33:59Z- handleDvl() - RDI_Pathfinder msg: 1613176439.35 epoch sec; seqNo:6675 2021-02-13T00:33:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 101.50 , 71.92 , 117.72 2021-02-13T00:33:59Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.09 , 0.04 2021-02-13T00:33:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176439.36 epoch sec; seqNo:13768; 4059485.86 north; 592764.09 east 2021-02-13T00:33:59Z- AHRS_M2 msg: 1613176439.73 epoch sec; seqNo:13661 2021-02-13T00:33:59Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.20 psi 2021-02-13T00:33:59Z- handleDepth()-Depth_Keller msg: 1613176439731 epoch sec; seqNo:13769; depth 15.03 2021-02-13T00:33:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176439.74 epoch sec; seqNo:13769; 4059486.24 north; 592764.08 east 2021-02-13T00:34:00Z- AHRS_M2 msg: 1613176440.13 epoch sec; seqNo:13662 2021-02-13T00:34:00Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.20 psi 2021-02-13T00:34:00Z- handleDepth()-Depth_Keller msg: 1613176440138 epoch sec; seqNo:13770; depth 15.17 2021-02-13T00:34:00Z- handleDvl() - RDI_Pathfinder msg: 1613176440.15 epoch sec; seqNo:6676 2021-02-13T00:34:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.71 , 101.63 , 73.96 , 116.51 2021-02-13T00:34:00Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.03 , 0.01 2021-02-13T00:34:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176440.16 epoch sec; seqNo:13770; 4059486.62 north; 592764.08 east 2021-02-13T00:34:00Z- AHRS_M2 msg: 1613176440.53 epoch sec; seqNo:13663 2021-02-13T00:34:00Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.20 psi 2021-02-13T00:34:00Z- handleDepth()-Depth_Keller msg: 1613176440536 epoch sec; seqNo:13771; depth 15.32 2021-02-13T00:34:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176440.55 epoch sec; seqNo:13771; 4059487.01 north; 592765.30 east 2021-02-13T00:34:00Z- AHRS_M2 msg: 1613176440.96 epoch sec; seqNo:13664 2021-02-13T00:34:00Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.19 psi 2021-02-13T00:34:00Z- handleDepth()-Depth_Keller msg: 1613176440969 epoch sec; seqNo:13772; depth 15.45 2021-02-13T00:34:00Z- handleDvl() - RDI_Pathfinder msg: 1613176440.98 epoch sec; seqNo:6677 2021-02-13T00:34:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.71 , 101.63 , 73.96 , 119.21 2021-02-13T00:34:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , 0.00 2021-02-13T00:34:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176440.99 epoch sec; seqNo:13772; 4059487.39 north; 592765.29 east 2021-02-13T00:34:01Z- AHRS_M2 msg: 1613176441.34 epoch sec; seqNo:13665 2021-02-13T00:34:01Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.19 psi 2021-02-13T00:34:01Z- handleDepth()-Depth_Keller msg: 1613176441344 epoch sec; seqNo:13773; depth 15.62 2021-02-13T00:34:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176441.36 epoch sec; seqNo:13773; 4059487.39 north; 592765.29 east 2021-02-13T00:34:01Z- AHRS_M2 msg: 1613176441.74 epoch sec; seqNo:13666 2021-02-13T00:34:01Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.19 psi 2021-02-13T00:34:01Z- handleDepth()-Depth_Keller msg: 1613176441756 epoch sec; seqNo:13774; depth 15.73 2021-02-13T00:34:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176441.76 epoch sec; seqNo:13774; 4059487.77 north; 592765.29 east 2021-02-13T00:34:02Z- AHRS_M2 msg: 1613176442.15 epoch sec; seqNo:13667 2021-02-13T00:34:02Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.19 psi 2021-02-13T00:34:02Z- handleDepth()-Depth_Keller msg: 1613176442155 epoch sec; seqNo:13775; depth 15.89 2021-02-13T00:34:02Z- handleDvl() - RDI_Pathfinder msg: 1613176442.17 epoch sec; seqNo:6678 2021-02-13T00:34:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.64 , 100.21 , 73.56 , 115.09 2021-02-13T00:34:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.04 , -0.02 2021-02-13T00:34:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176442.18 epoch sec; seqNo:13775; 4059488.15 north; 592765.28 east 2021-02-13T00:34:02Z- LcmTrn::handleMessages() - handling 45 messages took 0.74 ms and 130 calls... 2021-02-13T00:34:02Z- waiting for 1613176439.91, time is 1613176442.18 2021-02-13T00:34:02Z- LcmTrn::updateTrn() equating pose 1613176442.18 and meas times 1613176442.18 2021-02-13T00:34:02Z- LcmTrn::updateTrn() >>>> pose time 1613176442.18, meas time 1613176442.18 <<<< 2021-02-13T00:34:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176442.18 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:34:02Z- TerrainNav::motionUpdate() - currEstimate = (4059488, 592765.28, 15.89) 2021-02-13T00:34:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:34:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:34:02Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176442.18 sec.; No relevant navigation data available 2021-02-13T00:34:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:34:02Z- TNavBF::mmse selected from filter -1 2021-02-13T00:34:02Z- variances([0], [2], and psi_berg) = 697.091380 832.996437 0.000000 2021-02-13T00:34:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:34:02Z- LcmTrn::publishEstimates() - MLE : 4059487.65 592738.78 15.89 2021-02-13T00:34:02Z- LcmTrn::publishEstimates() - MMSE : 4059488.15 592765.28 15.89 2021-02-13T00:34:02Z- LcmTrn::publishEstimates() - COVAR: 697.09 833.00 1.29 0.00 2021-02-13T00:34:02Z- AHRS_M2 msg: 1613176442.55 epoch sec; seqNo:13668 2021-02-13T00:34:02Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.19 psi 2021-02-13T00:34:02Z- handleDepth()-Depth_Keller msg: 1613176442556 epoch sec; seqNo:13776; depth 16.02 2021-02-13T00:34:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176442.57 epoch sec; seqNo:13776; 4059488.15 north; 592765.28 east 2021-02-13T00:34:02Z- AHRS_M2 msg: 1613176442.97 epoch sec; seqNo:13669 2021-02-13T00:34:02Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.19 psi 2021-02-13T00:34:02Z- handleDepth()-Depth_Keller msg: 1613176442974 epoch sec; seqNo:13777; depth 16.14 2021-02-13T00:34:02Z- handleDvl() - RDI_Pathfinder msg: 1613176442.99 epoch sec; seqNo:6679 2021-02-13T00:34:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 100.35 , 73.15 , 116.57 2021-02-13T00:34:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , -0.08 2021-02-13T00:34:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176443.00 epoch sec; seqNo:13777; 4059488.54 north; 592766.50 east 2021-02-13T00:34:03Z- AHRS_M2 msg: 1613176443.36 epoch sec; seqNo:13670 2021-02-13T00:34:03Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.19 psi 2021-02-13T00:34:03Z- handleDepth()-Depth_Keller msg: 1613176443364 epoch sec; seqNo:13778; depth 16.30 2021-02-13T00:34:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176443.39 epoch sec; seqNo:13778; 4059488.92 north; 592766.50 east 2021-02-13T00:34:03Z- AHRS_M2 msg: 1613176443.77 epoch sec; seqNo:13671 2021-02-13T00:34:03Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.19 psi 2021-02-13T00:34:03Z- handleDepth()-Depth_Keller msg: 1613176443773 epoch sec; seqNo:13779; depth 16.42 2021-02-13T00:34:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176443.79 epoch sec; seqNo:13779; 4059489.30 north; 592766.49 east 2021-02-13T00:34:04Z- AHRS_M2 msg: 1613176444.17 epoch sec; seqNo:13672 2021-02-13T00:34:04Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-13T00:34:04Z- handleDepth()-Depth_Keller msg: 1613176444181 epoch sec; seqNo:13780; depth 16.54 2021-02-13T00:34:04Z- handleDvl() - RDI_Pathfinder msg: 1613176444.19 epoch sec; seqNo:6680 2021-02-13T00:34:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.28 , 100.35 , 78.72 , 113.87 2021-02-13T00:34:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , -0.13 2021-02-13T00:34:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176444.20 epoch sec; seqNo:13780; 4059489.67 north; 592766.49 east 2021-02-13T00:34:04Z- AHRS_M2 msg: 1613176444.58 epoch sec; seqNo:13673 2021-02-13T00:34:04Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:04Z- handleDepth()-Depth_Keller msg: 1613176444584 epoch sec; seqNo:13781; depth 16.68 2021-02-13T00:34:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176444.60 epoch sec; seqNo:13781; 4059490.05 north; 592766.48 east 2021-02-13T00:34:04Z- AHRS_M2 msg: 1613176444.98 epoch sec; seqNo:13674 2021-02-13T00:34:04Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:04Z- handleDepth()-Depth_Keller msg: 1613176444981 epoch sec; seqNo:13782; depth 16.80 2021-02-13T00:34:05Z- handleDvl() - RDI_Pathfinder msg: 1613176445.00 epoch sec; seqNo:6681 2021-02-13T00:34:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.07 , 101.70 , 71.11 , 112.52 2021-02-13T00:34:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.13 , -0.11 2021-02-13T00:34:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176445.01 epoch sec; seqNo:13782; 4059490.05 north; 592766.48 east 2021-02-13T00:34:05Z- AHRS_M2 msg: 1613176445.38 epoch sec; seqNo:13675 2021-02-13T00:34:05Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:05Z- handleDepth()-Depth_Keller msg: 1613176445384 epoch sec; seqNo:13783; depth 16.95 2021-02-13T00:34:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176445.39 epoch sec; seqNo:13783; 4059490.45 north; 592767.70 east 2021-02-13T00:34:05Z- AHRS_M2 msg: 1613176445.78 epoch sec; seqNo:13676 2021-02-13T00:34:05Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:05Z- handleDepth()-Depth_Keller msg: 1613176445793 epoch sec; seqNo:13784; depth 17.08 2021-02-13T00:34:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176445.80 epoch sec; seqNo:13784; 4059490.82 north; 592767.70 east 2021-02-13T00:34:06Z- AHRS_M2 msg: 1613176446.19 epoch sec; seqNo:13677 2021-02-13T00:34:06Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:06Z- handleDepth()-Depth_Keller msg: 1613176446194 epoch sec; seqNo:13785; depth 17.21 2021-02-13T00:34:06Z- handleDvl() - RDI_Pathfinder msg: 1613176446.21 epoch sec; seqNo:6682 2021-02-13T00:34:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.05 , 96.43 , 71.51 , 111.03 2021-02-13T00:34:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.21 , -0.02 2021-02-13T00:34:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176446.22 epoch sec; seqNo:13785; 4059491.20 north; 592767.69 east 2021-02-13T00:34:06Z- AHRS_M2 msg: 1613176446.59 epoch sec; seqNo:13678 2021-02-13T00:34:06Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.19 psi 2021-02-13T00:34:06Z- handleDepth()-Depth_Keller msg: 1613176446601 epoch sec; seqNo:13786; depth 17.33 2021-02-13T00:34:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176446.61 epoch sec; seqNo:13786; 4059491.58 north; 592767.69 east 2021-02-13T00:34:07Z- AHRS_M2 msg: 1613176447.00 epoch sec; seqNo:13679 2021-02-13T00:34:07Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.18 psi 2021-02-13T00:34:07Z- handleDepth()-Depth_Keller msg: 1613176447007 epoch sec; seqNo:13787; depth 17.47 2021-02-13T00:34:07Z- handleDvl() - RDI_Pathfinder msg: 1613176447.02 epoch sec; seqNo:6683 2021-02-13T00:34:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.60 , 99.07 , 70.29 , 111.23 2021-02-13T00:34:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.17 , 0.09 2021-02-13T00:34:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176447.03 epoch sec; seqNo:13787; 4059491.96 north; 592767.68 east 2021-02-13T00:34:07Z- LcmTrn::handleMessages() - handling 41 messages took 0.57 ms and 130 calls... 2021-02-13T00:34:07Z- waiting for 1613176445.18, time is 1613176447.03 2021-02-13T00:34:07Z- LcmTrn::updateTrn() equating pose 1613176447.03 and meas times 1613176447.03 2021-02-13T00:34:07Z- LcmTrn::updateTrn() >>>> pose time 1613176447.03, meas time 1613176447.03 <<<< 2021-02-13T00:34:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176447.03 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:34:07Z- TerrainNav::motionUpdate() - currEstimate = (4059492, 592767.68, 17.47) 2021-02-13T00:34:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:34:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:34:07Z- TNavBF::Projecting Measurements 2021-02-13T00:34:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:34:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:34:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:34:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:34:07Z- TNavFilter::4 good beams. 2021-02-13T00:34:07Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:34:07Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:34:07Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:34:07Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:34:07Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:34:07Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:34:07Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:34:07Z- TNavBF::mmse selected from filter -1 2021-02-13T00:34:07Z- TerrainMapOctree: (vehN, vehE ) = (4059491.96, 592767.68). 2021-02-13T00:34:07Z- TerrainMapOctree: Min Distance = 2299.56. 2021-02-13T00:34:07Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:34:07Z- There is no specific good beam that all particles have in common. 2021-02-13T00:34:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:34:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.52 K: 0.114 ExpectedMeasDiff: nan currMeas.covariance: 10.14 2021-02-13T00:34:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:34:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.35 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 15.70 2021-02-13T00:34:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:34:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.115 ExpectedMeasDiff: nan currMeas.covariance: 7.90 2021-02-13T00:34:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:34:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 19.80 2021-02-13T00:34:07Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:34:07Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:34:07Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:34:07Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:34:07Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:34:07Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:34:07Z- Filter 0 No beams used in subcloud update 2021-02-13T00:34:07Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:34:07Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:34:07Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 697.0 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:34:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176447.03 sec. 2021-02-13T00:34:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:34:07Z- TNavBF::mmse selected from filter -1 2021-02-13T00:34:07Z- variances([0], [2], and psi_berg) = 697.035679 832.996409 0.000000 2021-02-13T00:34:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:34:07Z- LcmTrn::publishEstimates() - MLE : 4059491.46 592741.18 17.47 2021-02-13T00:34:07Z- LcmTrn::publishEstimates() - MMSE : 4059491.96 592767.68 17.47 2021-02-13T00:34:07Z- LcmTrn::publishEstimates() - COVAR: 697.04 833.00 1.05 0.00 2021-02-13T00:34:07Z- AHRS_M2 msg: 1613176447.40 epoch sec; seqNo:13680 2021-02-13T00:34:07Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.18 psi 2021-02-13T00:34:07Z- handleDepth()-Depth_Keller msg: 1613176447412 epoch sec; seqNo:13788; depth 17.60 2021-02-13T00:34:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176447.42 epoch sec; seqNo:13788; 4059492.34 north; 592767.68 east 2021-02-13T00:34:07Z- AHRS_M2 msg: 1613176447.80 epoch sec; seqNo:13681 2021-02-13T00:34:07Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.18 psi 2021-02-13T00:34:07Z- handleDepth()-Depth_Keller msg: 1613176447808 epoch sec; seqNo:13789; depth 17.74 2021-02-13T00:34:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176447.82 epoch sec; seqNo:13789; 4059492.34 north; 592767.68 east 2021-02-13T00:34:08Z- AHRS_M2 msg: 1613176448.21 epoch sec; seqNo:13682 2021-02-13T00:34:08Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.17 psi 2021-02-13T00:34:08Z- handleDepth()-Depth_Keller msg: 1613176448217 epoch sec; seqNo:13790; depth 17.86 2021-02-13T00:34:08Z- handleDvl() - RDI_Pathfinder msg: 1613176448.23 epoch sec; seqNo:6684 2021-02-13T00:34:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.53 , 97.78 , 71.51 , 108.73 2021-02-13T00:34:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , 0.12 2021-02-13T00:34:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176448.24 epoch sec; seqNo:13790; 4059492.72 north; 592767.68 east 2021-02-13T00:34:08Z- AHRS_M2 msg: 1613176448.61 epoch sec; seqNo:13683 2021-02-13T00:34:08Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.17 psi 2021-02-13T00:34:08Z- handleDepth()-Depth_Keller msg: 1613176448618 epoch sec; seqNo:13791; depth 17.99 2021-02-13T00:34:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176448.63 epoch sec; seqNo:13791; 4059493.11 north; 592768.89 east 2021-02-13T00:34:09Z- AHRS_M2 msg: 1613176449.02 epoch sec; seqNo:13684 2021-02-13T00:34:09Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.17 psi 2021-02-13T00:34:09Z- handleDepth()-Depth_Keller msg: 1613176449030 epoch sec; seqNo:13792; depth 18.11 2021-02-13T00:34:09Z- handleDvl() - RDI_Pathfinder msg: 1613176449.04 epoch sec; seqNo:6685 2021-02-13T00:34:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.53 , 97.78 , 71.51 , 108.73 2021-02-13T00:34:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , 0.09 2021-02-13T00:34:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176449.05 epoch sec; seqNo:13792; 4059493.11 north; 592768.89 east 2021-02-13T00:34:09Z- AHRS_M2 msg: 1613176449.42 epoch sec; seqNo:13685 2021-02-13T00:34:09Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.18 psi 2021-02-13T00:34:09Z- handleDepth()-Depth_Keller msg: 1613176449425 epoch sec; seqNo:13793; depth 18.27 2021-02-13T00:34:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176449.44 epoch sec; seqNo:13793; 4059493.49 north; 592768.89 east 2021-02-13T00:34:09Z- AHRS_M2 msg: 1613176449.83 epoch sec; seqNo:13686 2021-02-13T00:34:09Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.18 psi 2021-02-13T00:34:09Z- handleDepth()-Depth_Keller msg: 1613176449830 epoch sec; seqNo:13794; depth 18.38 2021-02-13T00:34:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176449.84 epoch sec; seqNo:13794; 4059493.87 north; 592768.88 east 2021-02-13T00:34:10Z- AHRS_M2 msg: 1613176450.23 epoch sec; seqNo:13687 2021-02-13T00:34:10Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.19 psi 2021-02-13T00:34:10Z- handleDepth()-Depth_Keller msg: 1613176450233 epoch sec; seqNo:13795; depth 18.51 2021-02-13T00:34:10Z- handleDvl() - RDI_Pathfinder msg: 1613176450.25 epoch sec; seqNo:6686 2021-02-13T00:34:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.53 , 97.78 , 78.31 , 108.73 2021-02-13T00:34:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , 0.03 2021-02-13T00:34:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176450.26 epoch sec; seqNo:13795; 4059493.87 north; 592768.88 east 2021-02-13T00:34:10Z- AHRS_M2 msg: 1613176450.63 epoch sec; seqNo:13688 2021-02-13T00:34:10Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:10Z- handleDepth()-Depth_Keller msg: 1613176450636 epoch sec; seqNo:13796; depth 18.63 2021-02-13T00:34:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176450.65 epoch sec; seqNo:13796; 4059494.25 north; 592768.88 east 2021-02-13T00:34:11Z- AHRS_M2 msg: 1613176451.04 epoch sec; seqNo:13689 2021-02-13T00:34:11Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:11Z- handleDepth()-Depth_Keller msg: 1613176451045 epoch sec; seqNo:13797; depth 18.75 2021-02-13T00:34:11Z- handleDvl() - RDI_Pathfinder msg: 1613176451.06 epoch sec; seqNo:6687 2021-02-13T00:34:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.18 , 97.65 , 71.11 , 107.38 2021-02-13T00:34:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , 0.02 2021-02-13T00:34:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176451.07 epoch sec; seqNo:13797; 4059494.63 north; 592768.88 east 2021-02-13T00:34:11Z- AHRS_M2 msg: 1613176451.44 epoch sec; seqNo:13690 2021-02-13T00:34:11Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:11Z- handleDepth()-Depth_Keller msg: 1613176451446 epoch sec; seqNo:13798; depth 18.89 2021-02-13T00:34:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176451.46 epoch sec; seqNo:13798; 4059495.02 north; 592770.09 east 2021-02-13T00:34:11Z- AHRS_M2 msg: 1613176451.84 epoch sec; seqNo:13691 2021-02-13T00:34:11Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.21 psi 2021-02-13T00:34:11Z- handleDepth()-Depth_Keller msg: 1613176451858 epoch sec; seqNo:13799; depth 19.01 2021-02-13T00:34:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176451.87 epoch sec; seqNo:13799; 4059495.02 north; 592770.09 east 2021-02-13T00:34:12Z- AHRS_M2 msg: 1613176452.25 epoch sec; seqNo:13692 2021-02-13T00:34:12Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.21 psi 2021-02-13T00:34:12Z- handleDepth()-Depth_Keller msg: 1613176452253 epoch sec; seqNo:13800; depth 19.15 2021-02-13T00:34:12Z- handleDvl() - RDI_Pathfinder msg: 1613176452.27 epoch sec; seqNo:6688 2021-02-13T00:34:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.01 , 96.70 , 70.70 , 107.65 2021-02-13T00:34:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , 0.05 2021-02-13T00:34:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176452.29 epoch sec; seqNo:13800; 4059495.40 north; 592770.09 east 2021-02-13T00:34:12Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-13T00:34:12Z- waiting for 1613176450.03, time is 1613176452.29 2021-02-13T00:34:12Z- LcmTrn::updateTrn() equating pose 1613176452.29 and meas times 1613176452.29 2021-02-13T00:34:12Z- LcmTrn::updateTrn() >>>> pose time 1613176452.29, meas time 1613176452.29 <<<< 2021-02-13T00:34:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176452.29 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:34:12Z- TerrainNav::motionUpdate() - currEstimate = (4059495, 592770.09, 19.15) 2021-02-13T00:34:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:34:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:34:12Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176452.29 sec.; No relevant navigation data available 2021-02-13T00:34:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:34:12Z- TNavBF::mmse selected from filter -1 2021-02-13T00:34:12Z- variances([0], [2], and psi_berg) = 696.981982 832.996382 0.000000 2021-02-13T00:34:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:34:12Z- LcmTrn::publishEstimates() - MLE : 4059494.90 592743.59 19.15 2021-02-13T00:34:12Z- LcmTrn::publishEstimates() - MMSE : 4059495.40 592770.09 19.15 2021-02-13T00:34:12Z- LcmTrn::publishEstimates() - COVAR: 696.98 833.00 1.27 0.00 2021-02-13T00:34:12Z- AHRS_M2 msg: 1613176452.65 epoch sec; seqNo:13693 2021-02-13T00:34:12Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.21 psi 2021-02-13T00:34:12Z- handleDepth()-Depth_Keller msg: 1613176452661 epoch sec; seqNo:13801; depth 19.27 2021-02-13T00:34:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176452.67 epoch sec; seqNo:13801; 4059495.78 north; 592770.08 east 2021-02-13T00:34:13Z- AHRS_M2 msg: 1613176453.06 epoch sec; seqNo:13694 2021-02-13T00:34:13Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:13Z- handleDepth()-Depth_Keller msg: 1613176453062 epoch sec; seqNo:13802; depth 19.39 2021-02-13T00:34:13Z- handleDvl() - RDI_Pathfinder msg: 1613176453.07 epoch sec; seqNo:6689 2021-02-13T00:34:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.42 , 97.98 , 78.51 , 110.15 2021-02-13T00:34:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.02 , 0.09 2021-02-13T00:34:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176453.08 epoch sec; seqNo:13802; 4059496.15 north; 592770.08 east 2021-02-13T00:34:13Z- AHRS_M2 msg: 1613176453.46 epoch sec; seqNo:13695 2021-02-13T00:34:13Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:13Z- handleDepth()-Depth_Keller msg: 1613176453464 epoch sec; seqNo:13803; depth 19.54 2021-02-13T00:34:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176453.48 epoch sec; seqNo:13803; 4059496.15 north; 592770.08 east 2021-02-13T00:34:13Z- AHRS_M2 msg: 1613176453.86 epoch sec; seqNo:13696 2021-02-13T00:34:13Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:13Z- handleDepth()-Depth_Keller msg: 1613176453869 epoch sec; seqNo:13804; depth 19.63 2021-02-13T00:34:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176453.88 epoch sec; seqNo:13804; 4059496.55 north; 592771.30 east 2021-02-13T00:34:14Z- AHRS_M2 msg: 1613176454.27 epoch sec; seqNo:13697 2021-02-13T00:34:14Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.19 psi 2021-02-13T00:34:14Z- handleDepth()-Depth_Keller msg: 1613176454272 epoch sec; seqNo:13805; depth 19.78 2021-02-13T00:34:14Z- handleDvl() - RDI_Pathfinder msg: 1613176454.28 epoch sec; seqNo:6690 2021-02-13T00:34:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.25 , 95.28 , 78.25 , 105.01 2021-02-13T00:34:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.02 , 0.07 2021-02-13T00:34:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176454.29 epoch sec; seqNo:13805; 4059496.93 north; 592771.29 east 2021-02-13T00:34:14Z- AHRS_M2 msg: 1613176454.67 epoch sec; seqNo:13698 2021-02-13T00:34:14Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-13T00:34:14Z- handleDepth()-Depth_Keller msg: 1613176454685 epoch sec; seqNo:13806; depth 19.90 2021-02-13T00:34:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176454.70 epoch sec; seqNo:13806; 4059497.30 north; 592771.29 east 2021-02-13T00:34:15Z- AHRS_M2 msg: 1613176455.08 epoch sec; seqNo:13699 2021-02-13T00:34:15Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-13T00:34:15Z- handleDepth()-Depth_Keller msg: 1613176455085 epoch sec; seqNo:13807; depth 20.04 2021-02-13T00:34:15Z- handleDvl() - RDI_Pathfinder msg: 1613176455.10 epoch sec; seqNo:6691 2021-02-13T00:34:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.01 , 97.85 , 68.65 , 106.37 2021-02-13T00:34:15Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.00 , 0.02 2021-02-13T00:34:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176455.11 epoch sec; seqNo:13807; 4059497.68 north; 592771.28 east 2021-02-13T00:34:15Z- AHRS_M2 msg: 1613176455.48 epoch sec; seqNo:13700 2021-02-13T00:34:15Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.17 psi 2021-02-13T00:34:15Z- handleDepth()-Depth_Keller msg: 1613176455484 epoch sec; seqNo:13808; depth 20.16 2021-02-13T00:34:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176455.49 epoch sec; seqNo:13808; 4059497.68 north; 592771.28 east 2021-02-13T00:34:15Z- AHRS_M2 msg: 1613176455.88 epoch sec; seqNo:13701 2021-02-13T00:34:15Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.17 psi 2021-02-13T00:34:15Z- handleDepth()-Depth_Keller msg: 1613176455893 epoch sec; seqNo:13809; depth 20.29 2021-02-13T00:34:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176455.90 epoch sec; seqNo:13809; 4059498.08 north; 592772.50 east 2021-02-13T00:34:16Z- AHRS_M2 msg: 1613176456.29 epoch sec; seqNo:13702 2021-02-13T00:34:16Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.17 psi 2021-02-13T00:34:16Z- handleDepth()-Depth_Keller msg: 1613176456301 epoch sec; seqNo:13810; depth 20.41 2021-02-13T00:34:16Z- handleDvl() - RDI_Pathfinder msg: 1613176456.32 epoch sec; seqNo:6692 2021-02-13T00:34:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 67.84 , 106.50 2021-02-13T00:34:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.03 , -0.03 2021-02-13T00:34:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176456.33 epoch sec; seqNo:13810; 4059498.45 north; 592772.50 east 2021-02-13T00:34:16Z- AHRS_M2 msg: 1613176456.69 epoch sec; seqNo:13703 2021-02-13T00:34:16Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.17 psi 2021-02-13T00:34:16Z- handleDepth()-Depth_Keller msg: 1613176456696 epoch sec; seqNo:13811; depth 20.55 2021-02-13T00:34:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176456.72 epoch sec; seqNo:13811; 4059498.83 north; 592772.49 east 2021-02-13T00:34:17Z- AHRS_M2 msg: 1613176457.10 epoch sec; seqNo:13704 2021-02-13T00:34:17Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.18 psi 2021-02-13T00:34:17Z- handleDepth()-Depth_Keller msg: 1613176457100 epoch sec; seqNo:13812; depth 20.67 2021-02-13T00:34:17Z- handleDvl() - RDI_Pathfinder msg: 1613176457.11 epoch sec; seqNo:6693 2021-02-13T00:34:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.60 , 94.33 , 68.65 , 106.50 2021-02-13T00:34:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , -0.07 2021-02-13T00:34:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176457.14 epoch sec; seqNo:13812; 4059499.21 north; 592772.49 east 2021-02-13T00:34:17Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-13T00:34:17Z- waiting for 1613176455.29, time is 1613176457.14 2021-02-13T00:34:17Z- LcmTrn::updateTrn() equating pose 1613176457.14 and meas times 1613176457.14 2021-02-13T00:34:17Z- LcmTrn::updateTrn() >>>> pose time 1613176457.14, meas time 1613176457.14 <<<< 2021-02-13T00:34:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176457.14 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:34:17Z- TerrainNav::motionUpdate() - currEstimate = (4059499, 592772.49, 20.67) 2021-02-13T00:34:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:34:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:34:17Z- TNavBF::Projecting Measurements 2021-02-13T00:34:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:34:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:34:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:34:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:34:17Z- TNavFilter::4 good beams. 2021-02-13T00:34:17Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:34:17Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:34:17Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:34:17Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:34:17Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:34:17Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:34:17Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:34:17Z- TNavBF::mmse selected from filter -1 2021-02-13T00:34:17Z- TerrainMapOctree: (vehN, vehE ) = (4059499.21, 592772.49). 2021-02-13T00:34:17Z- TerrainMapOctree: Min Distance = 2290.92. 2021-02-13T00:34:17Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:34:17Z- There is no specific good beam that all particles have in common. 2021-02-13T00:34:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:34:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.50 K: 0.121 ExpectedMeasDiff: nan currMeas.covariance: 9.14 2021-02-13T00:34:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:34:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.32 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 14.24 2021-02-13T00:34:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:34:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.116 ExpectedMeasDiff: nan currMeas.covariance: 7.54 2021-02-13T00:34:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:34:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 18.15 2021-02-13T00:34:17Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:34:17Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:34:17Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:34:17Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:34:17Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:34:17Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:34:17Z- Filter 0 No beams used in subcloud update 2021-02-13T00:34:17Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:34:17Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:34:17Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 696.9 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:34:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176457.14 sec. 2021-02-13T00:34:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:34:17Z- TNavBF::mmse selected from filter -1 2021-02-13T00:34:17Z- variances([0], [2], and psi_berg) = 696.926345 832.996353 0.000000 2021-02-13T00:34:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:34:17Z- LcmTrn::publishEstimates() - MLE : 4059498.71 592745.99 20.67 2021-02-13T00:34:17Z- LcmTrn::publishEstimates() - MMSE : 4059499.21 592772.49 20.67 2021-02-13T00:34:17Z- LcmTrn::publishEstimates() - COVAR: 696.93 833.00 1.02 0.00 2021-02-13T00:34:17Z- AHRS_M2 msg: 1613176457.50 epoch sec; seqNo:13705 2021-02-13T00:34:17Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.18 psi 2021-02-13T00:34:17Z- handleDepth()-Depth_Keller msg: 1613176457504 epoch sec; seqNo:13813; depth 20.80 2021-02-13T00:34:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176457.52 epoch sec; seqNo:13813; 4059499.21 north; 592772.49 east 2021-02-13T00:34:17Z- AHRS_M2 msg: 1613176457.92 epoch sec; seqNo:13706 2021-02-13T00:34:17Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.18 psi 2021-02-13T00:34:17Z- handleDepth()-Depth_Keller msg: 1613176457927 epoch sec; seqNo:13814; depth 20.93 2021-02-13T00:34:17Z- handleDvl() - RDI_Pathfinder msg: 1613176457.94 epoch sec; seqNo:6694 2021-02-13T00:34:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 98.05 , 67.02 , 107.79 2021-02-13T00:34:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , -0.10 2021-02-13T00:34:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176457.95 epoch sec; seqNo:13814; 4059499.59 north; 592772.48 east 2021-02-13T00:34:18Z- AHRS_M2 msg: 1613176458.31 epoch sec; seqNo:13707 2021-02-13T00:34:18Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-13T00:34:18Z- handleDepth()-Depth_Keller msg: 1613176458317 epoch sec; seqNo:13815; depth 21.06 2021-02-13T00:34:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176458.34 epoch sec; seqNo:13815; 4059499.98 north; 592773.70 east 2021-02-13T00:34:18Z- AHRS_M2 msg: 1613176458.71 epoch sec; seqNo:13708 2021-02-13T00:34:18Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-13T00:34:18Z- handleDepth()-Depth_Keller msg: 1613176458716 epoch sec; seqNo:13816; depth 21.19 2021-02-13T00:34:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176458.72 epoch sec; seqNo:13816; 4059500.36 north; 592773.70 east 2021-02-13T00:34:19Z- AHRS_M2 msg: 1613176459.12 epoch sec; seqNo:13709 2021-02-13T00:34:19Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-13T00:34:19Z- handleDepth()-Depth_Keller msg: 1613176459129 epoch sec; seqNo:13817; depth 21.31 2021-02-13T00:34:19Z- handleDvl() - RDI_Pathfinder msg: 1613176459.14 epoch sec; seqNo:6695 2021-02-13T00:34:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 94.40 , 69.47 , 106.57 2021-02-13T00:34:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.06 , -0.10 2021-02-13T00:34:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176459.15 epoch sec; seqNo:13817; 4059500.74 north; 592773.69 east 2021-02-13T00:34:19Z- AHRS_M2 msg: 1613176459.52 epoch sec; seqNo:13710 2021-02-13T00:34:19Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.20 psi 2021-02-13T00:34:19Z- handleDepth()-Depth_Keller msg: 1613176459530 epoch sec; seqNo:13818; depth 21.48 2021-02-13T00:34:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176459.55 epoch sec; seqNo:13818; 4059500.74 north; 592773.69 east 2021-02-13T00:34:19Z- AHRS_M2 msg: 1613176459.93 epoch sec; seqNo:13711 2021-02-13T00:34:19Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.20 psi 2021-02-13T00:34:19Z- handleDepth()-Depth_Keller msg: 1613176459933 epoch sec; seqNo:13819; depth 21.58 2021-02-13T00:34:19Z- handleDvl() - RDI_Pathfinder msg: 1613176459.95 epoch sec; seqNo:6696 2021-02-13T00:34:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 97.98 , 67.02 , 106.50 2021-02-13T00:34:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.12 , -0.07 2021-02-13T00:34:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176459.97 epoch sec; seqNo:13819; 4059501.12 north; 592773.69 east 2021-02-13T00:34:20Z- AHRS_M2 msg: 1613176460.33 epoch sec; seqNo:13712 2021-02-13T00:34:20Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.20 psi 2021-02-13T00:34:20Z- handleDepth()-Depth_Keller msg: 1613176460337 epoch sec; seqNo:13820; depth 21.71 2021-02-13T00:34:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176460.35 epoch sec; seqNo:13820; 4059501.50 north; 592773.68 east 2021-02-13T00:34:20Z- AHRS_M2 msg: 1613176460.73 epoch sec; seqNo:13713 2021-02-13T00:34:20Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.20 psi 2021-02-13T00:34:20Z- handleDepth()-Depth_Keller msg: 1613176460741 epoch sec; seqNo:13821; depth 21.84 2021-02-13T00:34:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176460.75 epoch sec; seqNo:13821; 4059501.88 north; 592773.68 east 2021-02-13T00:34:21Z- AHRS_M2 msg: 1613176461.14 epoch sec; seqNo:13714 2021-02-13T00:34:21Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:21Z- handleDepth()-Depth_Keller msg: 1613176461157 epoch sec; seqNo:13822; depth 21.97 2021-02-13T00:34:21Z- handleDvl() - RDI_Pathfinder msg: 1613176461.17 epoch sec; seqNo:6697 2021-02-13T00:34:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 98.05 , 67.84 , 105.35 2021-02-13T00:34:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.14 , -0.01 2021-02-13T00:34:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176461.18 epoch sec; seqNo:13822; 4059502.27 north; 592774.90 east 2021-02-13T00:34:21Z- AHRS_M2 msg: 1613176461.55 epoch sec; seqNo:13715 2021-02-13T00:34:21Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:21Z- handleDepth()-Depth_Keller msg: 1613176461550 epoch sec; seqNo:13823; depth 22.11 2021-02-13T00:34:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176461.57 epoch sec; seqNo:13823; 4059502.27 north; 592774.90 east 2021-02-13T00:34:21Z- AHRS_M2 msg: 1613176461.95 epoch sec; seqNo:13716 2021-02-13T00:34:21Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:21Z- handleDepth()-Depth_Keller msg: 1613176461953 epoch sec; seqNo:13824; depth 22.22 2021-02-13T00:34:21Z- handleDvl() - RDI_Pathfinder msg: 1613176461.96 epoch sec; seqNo:6698 2021-02-13T00:34:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 93.25 , 0.00 , 101.77 2021-02-13T00:34:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.14 , 0.06 2021-02-13T00:34:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176461.97 epoch sec; seqNo:13824; 4059502.65 north; 592774.89 east 2021-02-13T00:34:22Z- AHRS_M2 msg: 1613176462.35 epoch sec; seqNo:13717 2021-02-13T00:34:22Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:22Z- handleDepth()-Depth_Keller msg: 1613176462358 epoch sec; seqNo:13825; depth 22.37 2021-02-13T00:34:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176462.37 epoch sec; seqNo:13825; 4059503.03 north; 592774.89 east 2021-02-13T00:34:22Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-13T00:34:22Z- waiting for 1613176460.14, time is 1613176462.37 2021-02-13T00:34:22Z- LcmTrn::updateTrn() >>>> pose time 1613176462.37, meas time 1613176461.96 <<<< 2021-02-13T00:34:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176461.96 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:34:22Z- TerrainNav::motionUpdate() - currEstimate = (4059503, 592774.89, 22.37) 2021-02-13T00:34:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:34:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:34:22Z- TNavBF::Projecting Measurements 2021-02-13T00:34:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:34:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:34:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:34:22Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:34:22Z- TNavFilter::3 good beams. 2021-02-13T00:34:22Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:34:22Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:34:22Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:34:22Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:34:22Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:34:22Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:34:22Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:34:22Z- TNavBF::mmse selected from filter -1 2021-02-13T00:34:22Z- TerrainMapOctree: (vehN, vehE ) = (4059502.73, 592774.70). 2021-02-13T00:34:22Z- TerrainMapOctree: Min Distance = 2286.80. 2021-02-13T00:34:22Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:34:22Z- There is no specific good beam that all particles have in common. 2021-02-13T00:34:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:34:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 8.66 2021-02-13T00:34:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:34:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 13.91 2021-02-13T00:34:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:34:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 16.57 2021-02-13T00:34:22Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:34:22Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:34:22Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:34:22Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:34:22Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:34:22Z- Filter 0 No beams used in subcloud update 2021-02-13T00:34:22Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:34:22Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:34:22Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 696.9 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:34:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176461.96 sec. 2021-02-13T00:34:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:34:22Z- TNavBF::mmse selected from filter -1 2021-02-13T00:34:22Z- variances([0], [2], and psi_berg) = 696.857474 832.996318 0.000000 2021-02-13T00:34:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:34:22Z- LcmTrn::publishEstimates() - MLE : 4059502.53 592748.39 22.37 2021-02-13T00:34:22Z- LcmTrn::publishEstimates() - MMSE : 4059503.03 592774.89 22.37 2021-02-13T00:34:22Z- LcmTrn::publishEstimates() - COVAR: 696.86 833.00 0.99 0.00 2021-02-13T00:34:22Z- AHRS_M2 msg: 1613176462.75 epoch sec; seqNo:13718 2021-02-13T00:34:22Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:22Z- handleDepth()-Depth_Keller msg: 1613176462756 epoch sec; seqNo:13826; depth 22.48 2021-02-13T00:34:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176462.77 epoch sec; seqNo:13826; 4059503.41 north; 592774.88 east 2021-02-13T00:34:23Z- AHRS_M2 msg: 1613176463.16 epoch sec; seqNo:13719 2021-02-13T00:34:23Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:23Z- handleDepth()-Depth_Keller msg: 1613176463163 epoch sec; seqNo:13827; depth 22.61 2021-02-13T00:34:23Z- handleDvl() - RDI_Pathfinder msg: 1613176463.17 epoch sec; seqNo:6699 2021-02-13T00:34:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:34:23Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:34:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176463.18 epoch sec; seqNo:13827; 4059503.41 north; 592774.88 east 2021-02-13T00:34:23Z- AHRS_M2 msg: 1613176463.56 epoch sec; seqNo:13720 2021-02-13T00:34:23Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:23Z- handleDepth()-Depth_Keller msg: 1613176463566 epoch sec; seqNo:13828; depth 22.75 2021-02-13T00:34:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176463.58 epoch sec; seqNo:13828; 4059503.78 north; 592774.88 east 2021-02-13T00:34:23Z- AHRS_M2 msg: 1613176463.98 epoch sec; seqNo:13721 2021-02-13T00:34:23Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:23Z- handleDepth()-Depth_Keller msg: 1613176463998 epoch sec; seqNo:13829; depth 22.87 2021-02-13T00:34:24Z- handleDvl() - RDI_Pathfinder msg: 1613176464.01 epoch sec; seqNo:6700 2021-02-13T00:34:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.28 , 97.24 , 66.85 , 104.68 2021-02-13T00:34:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.10 2021-02-13T00:34:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176464.02 epoch sec; seqNo:13829; 4059504.16 north; 592774.88 east 2021-02-13T00:34:24Z- AHRS_M2 msg: 1613176464.37 epoch sec; seqNo:13722 2021-02-13T00:34:24Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:24Z- handleDepth()-Depth_Keller msg: 1613176464372 epoch sec; seqNo:13830; depth 23.01 2021-02-13T00:34:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176464.39 epoch sec; seqNo:13830; 4059504.56 north; 592776.09 east 2021-02-13T00:34:24Z- AHRS_M2 msg: 1613176464.77 epoch sec; seqNo:13723 2021-02-13T00:34:24Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:24Z- handleDepth()-Depth_Keller msg: 1613176464778 epoch sec; seqNo:13831; depth 23.12 2021-02-13T00:34:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176464.79 epoch sec; seqNo:13831; 4059504.56 north; 592776.09 east 2021-02-13T00:34:25Z- AHRS_M2 msg: 1613176465.18 epoch sec; seqNo:13724 2021-02-13T00:34:25Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:25Z- handleDepth()-Depth_Keller msg: 1613176465185 epoch sec; seqNo:13832; depth 23.24 2021-02-13T00:34:25Z- handleDvl() - RDI_Pathfinder msg: 1613176465.21 epoch sec; seqNo:6701 2021-02-13T00:34:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 92.10 , 66.20 , 103.05 2021-02-13T00:34:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.06 2021-02-13T00:34:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176465.22 epoch sec; seqNo:13832; 4059504.93 north; 592776.09 east 2021-02-13T00:34:25Z- AHRS_M2 msg: 1613176465.58 epoch sec; seqNo:13725 2021-02-13T00:34:25Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:25Z- handleDepth()-Depth_Keller msg: 1613176465584 epoch sec; seqNo:13833; depth 23.38 2021-02-13T00:34:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176465.60 epoch sec; seqNo:13833; 4059505.31 north; 592776.08 east 2021-02-13T00:34:25Z- AHRS_M2 msg: 1613176465.98 epoch sec; seqNo:13726 2021-02-13T00:34:25Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-13T00:34:25Z- handleDepth()-Depth_Keller msg: 1613176465988 epoch sec; seqNo:13834; depth 23.50 2021-02-13T00:34:26Z- handleDvl() - RDI_Pathfinder msg: 1613176466.00 epoch sec; seqNo:6702 2021-02-13T00:34:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.74 , 98.19 , 65.38 , 101.84 2021-02-13T00:34:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , 0.02 2021-02-13T00:34:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176466.01 epoch sec; seqNo:13834; 4059505.31 north; 592776.08 east 2021-02-13T00:34:26Z- AHRS_M2 msg: 1613176466.39 epoch sec; seqNo:13727 2021-02-13T00:34:26Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.19 psi 2021-02-13T00:34:26Z- handleDepth()-Depth_Keller msg: 1613176466397 epoch sec; seqNo:13835; depth 23.62 2021-02-13T00:34:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176466.41 epoch sec; seqNo:13835; 4059505.69 north; 592776.08 east 2021-02-13T00:34:26Z- AHRS_M2 msg: 1613176466.79 epoch sec; seqNo:13728 2021-02-13T00:34:26Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.19 psi 2021-02-13T00:34:26Z- handleDepth()-Depth_Keller msg: 1613176466796 epoch sec; seqNo:13836; depth 23.74 2021-02-13T00:34:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176466.81 epoch sec; seqNo:13836; 4059506.08 north; 592777.30 east 2021-02-13T00:34:27Z- handleDepth()-Depth_Keller msg: 1613176467217 epoch sec; seqNo:13837; depth 23.88 2021-02-13T00:34:27Z- handleDvl() - RDI_Pathfinder msg: 1613176467.23 epoch sec; seqNo:6703 2021-02-13T00:34:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.74 , 98.25 , 66.20 , 100.69 2021-02-13T00:34:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.00 2021-02-13T00:34:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176467.25 epoch sec; seqNo:13837; 4059506.46 north; 592777.29 east 2021-02-13T00:34:27Z- LcmTrn::handleMessages() - handling 40 messages took 0.61 ms and 128 calls... 2021-02-13T00:34:27Z- waiting for 1613176465.37, time is 1613176467.25 2021-02-13T00:34:27Z- LcmTrn::updateTrn() equating pose 1613176467.25 and meas times 1613176467.25 2021-02-13T00:34:27Z- LcmTrn::updateTrn() >>>> pose time 1613176467.25, meas time 1613176467.25 <<<< 2021-02-13T00:34:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176467.25 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:34:27Z- TerrainNav::motionUpdate() - currEstimate = (4059506, 592777.29, 23.88) 2021-02-13T00:34:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:34:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:34:27Z- TNavBF::Projecting Measurements 2021-02-13T00:34:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:34:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:34:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:34:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:34:27Z- TNavFilter::4 good beams. 2021-02-13T00:34:27Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:34:27Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:34:27Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:34:27Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:34:27Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:34:27Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:34:27Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:34:27Z- TNavBF::mmse selected from filter -1 2021-02-13T00:34:27Z- TerrainMapOctree: (vehN, vehE ) = (4059506.46, 592777.29). 2021-02-13T00:34:27Z- TerrainMapOctree: Min Distance = 2282.28. 2021-02-13T00:34:27Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:34:27Z- There is no specific good beam that all particles have in common. 2021-02-13T00:34:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:34:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 8.47 2021-02-13T00:34:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:34:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 15.45 2021-02-13T00:34:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:34:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 7.01 2021-02-13T00:34:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:34:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 16.22 2021-02-13T00:34:27Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:34:27Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:34:27Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:34:27Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:34:27Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:34:27Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:34:27Z- Filter 0 No beams used in subcloud update 2021-02-13T00:34:27Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:34:27Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:34:27Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 696.8 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:34:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176467.25 sec. 2021-02-13T00:34:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:34:27Z- TNavBF::mmse selected from filter -1 2021-02-13T00:34:27Z- variances([0], [2], and psi_berg) = 696.803878 832.996291 0.000000 2021-02-13T00:34:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:34:27Z- LcmTrn::publishEstimates() - MLE : 4059505.96 592750.79 23.88 2021-02-13T00:34:27Z- LcmTrn::publishEstimates() - MMSE : 4059506.46 592777.29 23.88 2021-02-13T00:34:27Z- LcmTrn::publishEstimates() - COVAR: 696.80 833.00 0.87 0.00 2021-02-13T00:34:27Z- AHRS_M2 msg: 1613176467.60 epoch sec; seqNo:13729 2021-02-13T00:34:27Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.18 psi 2021-02-13T00:34:27Z- handleDepth()-Depth_Keller msg: 1613176467606 epoch sec; seqNo:13838; depth 24.03 2021-02-13T00:34:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176467.62 epoch sec; seqNo:13838; 4059506.46 north; 592777.29 east 2021-02-13T00:34:28Z- AHRS_M2 msg: 1613176468.00 epoch sec; seqNo:13730 2021-02-13T00:34:28Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.18 psi 2021-02-13T00:34:28Z- handleDepth()-Depth_Keller msg: 1613176468018 epoch sec; seqNo:13839; depth 24.12 2021-02-13T00:34:28Z- handleDvl() - RDI_Pathfinder msg: 1613176468.03 epoch sec; seqNo:6704 2021-02-13T00:34:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:34:28Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:34:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176468.04 epoch sec; seqNo:13839; 4059506.84 north; 592777.29 east 2021-02-13T00:34:28Z- AHRS_M2 msg: 1613176468.41 epoch sec; seqNo:13731 2021-02-13T00:34:28Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:34:28Z- handleDepth()-Depth_Keller msg: 1613176468413 epoch sec; seqNo:13840; depth 24.25 2021-02-13T00:34:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176468.43 epoch sec; seqNo:13840; 4059507.22 north; 592777.28 east 2021-02-13T00:34:28Z- AHRS_M2 msg: 1613176468.82 epoch sec; seqNo:13732 2021-02-13T00:34:28Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:34:28Z- handleDepth()-Depth_Keller msg: 1613176468823 epoch sec; seqNo:13841; depth 24.39 2021-02-13T00:34:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176468.86 epoch sec; seqNo:13841; 4059507.60 north; 592777.28 east 2021-02-13T00:34:29Z- AHRS_M2 msg: 1613176469.22 epoch sec; seqNo:13733 2021-02-13T00:34:29Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:34:29Z- handleDepth()-Depth_Keller msg: 1613176469229 epoch sec; seqNo:13842; depth 24.50 2021-02-13T00:34:29Z- handleDvl() - RDI_Pathfinder msg: 1613176469.24 epoch sec; seqNo:6705 2021-02-13T00:34:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 88.52 , 64.16 , 99.47 2021-02-13T00:34:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , -0.01 2021-02-13T00:34:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176469.25 epoch sec; seqNo:13842; 4059507.61 north; 592778.50 east 2021-02-13T00:34:29Z- AHRS_M2 msg: 1613176469.62 epoch sec; seqNo:13734 2021-02-13T00:34:29Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.17 psi 2021-02-13T00:34:29Z- handleDepth()-Depth_Keller msg: 1613176469624 epoch sec; seqNo:13843; depth 24.63 2021-02-13T00:34:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176469.64 epoch sec; seqNo:13843; 4059507.99 north; 592778.50 east 2021-02-13T00:34:30Z- AHRS_M2 msg: 1613176470.02 epoch sec; seqNo:13735 2021-02-13T00:34:30Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.17 psi 2021-02-13T00:34:30Z- handleDepth()-Depth_Keller msg: 1613176470033 epoch sec; seqNo:13844; depth 24.75 2021-02-13T00:34:30Z- handleDvl() - RDI_Pathfinder msg: 1613176470.04 epoch sec; seqNo:6706 2021-02-13T00:34:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 0.00 , 64.57 , 98.93 2021-02-13T00:34:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , -0.02 2021-02-13T00:34:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176470.06 epoch sec; seqNo:13844; 4059508.37 north; 592778.49 east 2021-02-13T00:34:30Z- AHRS_M2 msg: 1613176470.43 epoch sec; seqNo:13736 2021-02-13T00:34:30Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:34:30Z- handleDepth()-Depth_Keller msg: 1613176470441 epoch sec; seqNo:13845; depth 24.87 2021-02-13T00:34:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176470.45 epoch sec; seqNo:13845; 4059508.75 north; 592778.49 east 2021-02-13T00:34:30Z- AHRS_M2 msg: 1613176470.84 epoch sec; seqNo:13737 2021-02-13T00:34:30Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:34:30Z- handleDepth()-Depth_Keller msg: 1613176470851 epoch sec; seqNo:13846; depth 25.00 2021-02-13T00:34:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176470.87 epoch sec; seqNo:13846; 4059508.75 north; 592778.49 east 2021-02-13T00:34:31Z- AHRS_M2 msg: 1613176471.24 epoch sec; seqNo:13738 2021-02-13T00:34:31Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:34:31Z- handleDepth()-Depth_Keller msg: 1613176471245 epoch sec; seqNo:13847; depth 25.11 2021-02-13T00:34:31Z- handleDvl() - RDI_Pathfinder msg: 1613176471.26 epoch sec; seqNo:6707 2021-02-13T00:34:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 87.17 , 65.38 , 99.07 2021-02-13T00:34:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , -0.02 2021-02-13T00:34:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176471.27 epoch sec; seqNo:13847; 4059509.13 north; 592778.48 east 2021-02-13T00:34:31Z- AHRS_M2 msg: 1613176471.64 epoch sec; seqNo:13739 2021-02-13T00:34:31Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:34:31Z- handleDepth()-Depth_Keller msg: 1613176471644 epoch sec; seqNo:13848; depth 25.24 2021-02-13T00:34:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176471.66 epoch sec; seqNo:13848; 4059509.51 north; 592778.48 east 2021-02-13T00:34:32Z- AHRS_M2 msg: 1613176472.04 epoch sec; seqNo:13740 2021-02-13T00:34:32Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.18 psi 2021-02-13T00:34:32Z- handleDepth()-Depth_Keller msg: 1613176472053 epoch sec; seqNo:13849; depth 25.35 2021-02-13T00:34:32Z- handleDvl() - RDI_Pathfinder msg: 1613176472.10 epoch sec; seqNo:6708 2021-02-13T00:34:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 88.18 , 64.57 , 97.92 2021-02-13T00:34:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , -0.01 2021-02-13T00:34:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176472.11 epoch sec; seqNo:13849; 4059509.90 north; 592779.70 east 2021-02-13T00:34:32Z- AHRS_M2 msg: 1613176472.46 epoch sec; seqNo:13741 2021-02-13T00:34:32Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-13T00:34:32Z- handleDepth()-Depth_Keller msg: 1613176472474 epoch sec; seqNo:13850; depth 25.48 2021-02-13T00:34:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:34:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:34:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:34:32Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:34:32Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:34:32Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:34:32Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:34:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:34:32Z- handleDvl() - RDI_Pathfinder msg: 1613176472.48 epoch sec; seqNo:6709 2021-02-13T00:34:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:34:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , -0.01 2021-02-13T00:34:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176472.49 epoch sec; seqNo:13850; 4059509.90 north; 592779.70 east 2021-02-13T00:34:32Z- LcmTrn::handleMessages() - handling 45 messages took 0.65 ms and 132 calls... 2021-02-13T00:34:32Z- waiting for 1613176470.25, time is 1613176472.49 2021-02-13T00:34:32Z- LcmTrn::updateTrn() equating pose 1613176472.49 and meas times 1613176472.49 2021-02-13T00:34:32Z- LcmTrn::updateTrn() >>>> pose time 1613176472.49, meas time 1613176472.49 <<<< 2021-02-13T00:34:32Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613176472.49, ping # 0. 2021-02-13T00:34:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176472.49 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:34:32Z- TerrainNav::motionUpdate() - currEstimate = (4059510, 592779.70, 25.48) 2021-02-13T00:34:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:34:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:34:32Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:34:32Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:34:32Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176472.49 sec.; No relevant navigation data available 2021-02-13T00:34:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:34:32Z- TNavBF::mmse selected from filter -1 2021-02-13T00:34:32Z- variances([0], [2], and psi_berg) = 696.750312 832.996264 0.000000 2021-02-13T00:34:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:34:32Z- LcmTrn::publishEstimates() - MLE : 4059509.40 592753.20 25.48 2021-02-13T00:34:32Z- LcmTrn::publishEstimates() - MMSE : 4059509.90 592779.70 25.48 2021-02-13T00:34:32Z- LcmTrn::publishEstimates() - COVAR: 696.75 833.00 1.08 0.00 2021-02-13T00:34:32Z- AHRS_M2 msg: 1613176472.86 epoch sec; seqNo:13742 2021-02-13T00:34:32Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-13T00:34:32Z- handleDepth()-Depth_Keller msg: 1613176472860 epoch sec; seqNo:13851; depth 25.60 2021-02-13T00:34:32Z- handleDvl() - RDI_Pathfinder msg: 1613176472.88 epoch sec; seqNo:6710 2021-02-13T00:34:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 0.00 , 64.97 , 97.92 2021-02-13T00:34:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , 0.00 2021-02-13T00:34:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176472.89 epoch sec; seqNo:13851; 4059510.28 north; 592779.69 east 2021-02-13T00:34:33Z- AHRS_M2 msg: 1613176473.26 epoch sec; seqNo:13743 2021-02-13T00:34:33Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-13T00:34:33Z- handleDepth()-Depth_Keller msg: 1613176473269 epoch sec; seqNo:13852; depth 25.72 2021-02-13T00:34:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176473.28 epoch sec; seqNo:13852; 4059510.66 north; 592779.69 east 2021-02-13T00:34:33Z- AHRS_M2 msg: 1613176473.66 epoch sec; seqNo:13744 2021-02-13T00:34:33Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-13T00:34:33Z- handleDepth()-Depth_Keller msg: 1613176473669 epoch sec; seqNo:13853; depth 25.84 2021-02-13T00:34:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176473.68 epoch sec; seqNo:13853; 4059511.04 north; 592779.68 east 2021-02-13T00:34:34Z- AHRS_M2 msg: 1613176474.07 epoch sec; seqNo:13745 2021-02-13T00:34:34Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-13T00:34:34Z- handleDepth()-Depth_Keller msg: 1613176474077 epoch sec; seqNo:13854; depth 25.96 2021-02-13T00:34:34Z- handleDvl() - RDI_Pathfinder msg: 1613176474.10 epoch sec; seqNo:6711 2021-02-13T00:34:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 0.00 , 63.34 , 97.98 2021-02-13T00:34:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , 0.04 2021-02-13T00:34:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176474.11 epoch sec; seqNo:13854; 4059511.41 north; 592779.68 east 2021-02-13T00:34:34Z- AHRS_M2 msg: 1613176474.47 epoch sec; seqNo:13746 2021-02-13T00:34:34Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-13T00:34:34Z- handleDepth()-Depth_Keller msg: 1613176474479 epoch sec; seqNo:13855; depth 26.08 2021-02-13T00:34:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176474.49 epoch sec; seqNo:13855; 4059511.43 north; 592780.90 east 2021-02-13T00:34:34Z- AHRS_M2 msg: 1613176474.88 epoch sec; seqNo:13747 2021-02-13T00:34:34Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-13T00:34:34Z- handleDepth()-Depth_Keller msg: 1613176474889 epoch sec; seqNo:13856; depth 26.20 2021-02-13T00:34:34Z- handleDvl() - RDI_Pathfinder msg: 1613176474.90 epoch sec; seqNo:6712 2021-02-13T00:34:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 0.00 , 64.16 , 97.98 2021-02-13T00:34:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.07 , 0.05 2021-02-13T00:34:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176474.91 epoch sec; seqNo:13856; 4059511.81 north; 592780.90 east 2021-02-13T00:34:35Z- AHRS_M2 msg: 1613176475.28 epoch sec; seqNo:13748 2021-02-13T00:34:35Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-13T00:34:35Z- handleDepth()-Depth_Keller msg: 1613176475285 epoch sec; seqNo:13857; depth 26.32 2021-02-13T00:34:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176475.30 epoch sec; seqNo:13857; 4059512.19 north; 592780.89 east 2021-02-13T00:34:35Z- AHRS_M2 msg: 1613176475.68 epoch sec; seqNo:13749 2021-02-13T00:34:35Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-13T00:34:35Z- handleDepth()-Depth_Keller msg: 1613176475688 epoch sec; seqNo:13858; depth 26.44 2021-02-13T00:34:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176475.70 epoch sec; seqNo:13858; 4059512.56 north; 592780.89 east 2021-02-13T00:34:36Z- handleDepth()-Depth_Keller msg: 1613176476109 epoch sec; seqNo:13859; depth 26.55 2021-02-13T00:34:36Z- handleDvl() - RDI_Pathfinder msg: 1613176476.12 epoch sec; seqNo:6713 2021-02-13T00:34:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 88.25 , 64.16 , 97.98 2021-02-13T00:34:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , 0.03 2021-02-13T00:34:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176476.14 epoch sec; seqNo:13859; 4059512.56 north; 592780.89 east 2021-02-13T00:34:36Z- AHRS_M2 msg: 1613176476.50 epoch sec; seqNo:13750 2021-02-13T00:34:36Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-13T00:34:36Z- handleDepth()-Depth_Keller msg: 1613176476507 epoch sec; seqNo:13860; depth 26.69 2021-02-13T00:34:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176476.52 epoch sec; seqNo:13860; 4059512.94 north; 592780.88 east 2021-02-13T00:34:36Z- AHRS_M2 msg: 1613176476.89 epoch sec; seqNo:13751 2021-02-13T00:34:36Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.19 psi 2021-02-13T00:34:36Z- handleDepth()-Depth_Keller msg: 1613176476904 epoch sec; seqNo:13861; depth 26.79 2021-02-13T00:34:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176476.91 epoch sec; seqNo:13861; 4059513.32 north; 592780.88 east 2021-02-13T00:34:37Z- handleDepth()-Depth_Keller msg: 1613176477321 epoch sec; seqNo:13862; depth 26.91 2021-02-13T00:34:37Z- handleDvl() - RDI_Pathfinder msg: 1613176477.34 epoch sec; seqNo:6714 2021-02-13T00:34:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 86.09 , 62.93 , 96.90 2021-02-13T00:34:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , -0.01 2021-02-13T00:34:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176477.35 epoch sec; seqNo:13862; 4059513.71 north; 592782.10 east 2021-02-13T00:34:37Z- LcmTrn::handleMessages() - handling 39 messages took 0.59 ms and 127 calls... 2021-02-13T00:34:37Z- waiting for 1613176475.49, time is 1613176477.35 2021-02-13T00:34:37Z- LcmTrn::updateTrn() equating pose 1613176477.35 and meas times 1613176477.35 2021-02-13T00:34:37Z- LcmTrn::updateTrn() >>>> pose time 1613176477.35, meas time 1613176477.35 <<<< 2021-02-13T00:34:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176477.35 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:34:37Z- TerrainNav::motionUpdate() - currEstimate = (4059514, 592782.10, 26.91) 2021-02-13T00:34:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:34:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:34:37Z- TNavBF::Projecting Measurements 2021-02-13T00:34:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:34:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:34:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:34:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:34:37Z- TNavFilter::4 good beams. 2021-02-13T00:34:37Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:34:37Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:34:37Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:34:37Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:34:37Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:34:37Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:34:37Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:34:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:34:37Z- TerrainMapOctree: (vehN, vehE ) = (4059513.71, 592782.10). 2021-02-13T00:34:37Z- TerrainMapOctree: Min Distance = 2273.64. 2021-02-13T00:34:37Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:34:37Z- There is no specific good beam that all particles have in common. 2021-02-13T00:34:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:34:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:34:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:34:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 11.86 2021-02-13T00:34:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:34:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.118 ExpectedMeasDiff: nan currMeas.covariance: 6.34 2021-02-13T00:34:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:34:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.02 2021-02-13T00:34:37Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:34:37Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:34:37Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:34:37Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:34:37Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:34:37Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:34:37Z- Filter 0 No beams used in subcloud update 2021-02-13T00:34:37Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:34:37Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:34:37Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 696.7 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:34:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176477.35 sec. 2021-02-13T00:34:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:34:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:34:37Z- variances([0], [2], and psi_berg) = 696.694812 832.996236 0.000000 2021-02-13T00:34:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:34:37Z- LcmTrn::publishEstimates() - MLE : 4059513.21 592755.60 26.91 2021-02-13T00:34:37Z- LcmTrn::publishEstimates() - MMSE : 4059513.71 592782.10 26.91 2021-02-13T00:34:37Z- LcmTrn::publishEstimates() - COVAR: 696.69 833.00 0.90 0.00 2021-02-13T00:34:37Z- AHRS_M2 msg: 1613176477.70 epoch sec; seqNo:13752 2021-02-13T00:34:37Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-13T00:34:37Z- handleDepth()-Depth_Keller msg: 1613176477721 epoch sec; seqNo:13863; depth 27.04 2021-02-13T00:34:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176477.74 epoch sec; seqNo:13863; 4059514.09 north; 592782.09 east 2021-02-13T00:34:38Z- AHRS_M2 msg: 1613176478.11 epoch sec; seqNo:13753 2021-02-13T00:34:38Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-13T00:34:38Z- handleDepth()-Depth_Keller msg: 1613176478119 epoch sec; seqNo:13864; depth 27.16 2021-02-13T00:34:38Z- handleDvl() - RDI_Pathfinder msg: 1613176478.14 epoch sec; seqNo:6715 2021-02-13T00:34:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.65 , 87.03 , 63.34 , 97.85 2021-02-13T00:34:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.03 2021-02-13T00:34:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176478.15 epoch sec; seqNo:13864; 4059514.09 north; 592782.09 east 2021-02-13T00:34:38Z- AHRS_M2 msg: 1613176478.52 epoch sec; seqNo:13754 2021-02-13T00:34:38Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-13T00:34:38Z- handleDepth()-Depth_Keller msg: 1613176478522 epoch sec; seqNo:13865; depth 27.26 2021-02-13T00:34:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176478.53 epoch sec; seqNo:13865; 4059514.47 north; 592782.09 east 2021-02-13T00:34:38Z- AHRS_M2 msg: 1613176478.92 epoch sec; seqNo:13755 2021-02-13T00:34:38Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-13T00:34:38Z- handleDepth()-Depth_Keller msg: 1613176478933 epoch sec; seqNo:13866; depth 27.39 2021-02-13T00:34:38Z- handleDvl() - RDI_Pathfinder msg: 1613176478.94 epoch sec; seqNo:6716 2021-02-13T00:34:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 87.17 , 62.93 , 97.98 2021-02-13T00:34:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , -0.02 2021-02-13T00:34:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176478.95 epoch sec; seqNo:13866; 4059514.85 north; 592782.08 east 2021-02-13T00:34:39Z- AHRS_M2 msg: 1613176479.32 epoch sec; seqNo:13756 2021-02-13T00:34:39Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-13T00:34:39Z- handleDepth()-Depth_Keller msg: 1613176479324 epoch sec; seqNo:13867; depth 27.51 2021-02-13T00:34:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176479.34 epoch sec; seqNo:13867; 4059515.23 north; 592782.08 east 2021-02-13T00:34:39Z- AHRS_M2 msg: 1613176479.72 epoch sec; seqNo:13757 2021-02-13T00:34:39Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-13T00:34:39Z- handleDepth()-Depth_Keller msg: 1613176479733 epoch sec; seqNo:13868; depth 27.63 2021-02-13T00:34:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176479.74 epoch sec; seqNo:13868; 4059515.23 north; 592782.08 east 2021-02-13T00:34:40Z- AHRS_M2 msg: 1613176480.13 epoch sec; seqNo:13758 2021-02-13T00:34:40Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-13T00:34:40Z- handleDepth()-Depth_Keller msg: 1613176480135 epoch sec; seqNo:13869; depth 27.76 2021-02-13T00:34:40Z- handleDvl() - RDI_Pathfinder msg: 1613176480.15 epoch sec; seqNo:6717 2021-02-13T00:34:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 87.17 , 62.52 , 97.98 2021-02-13T00:34:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.12 , -0.00 2021-02-13T00:34:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176480.16 epoch sec; seqNo:13869; 4059515.62 north; 592783.30 east 2021-02-13T00:34:40Z- AHRS_M2 msg: 1613176480.53 epoch sec; seqNo:13759 2021-02-13T00:34:40Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-13T00:34:40Z- handleDepth()-Depth_Keller msg: 1613176480539 epoch sec; seqNo:13870; depth 27.88 2021-02-13T00:34:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176480.55 epoch sec; seqNo:13870; 4059516.00 north; 592783.29 east 2021-02-13T00:34:40Z- AHRS_M2 msg: 1613176480.93 epoch sec; seqNo:13760 2021-02-13T00:34:40Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-13T00:34:40Z- handleDepth()-Depth_Keller msg: 1613176480945 epoch sec; seqNo:13871; depth 28.01 2021-02-13T00:34:40Z- handleDvl() - RDI_Pathfinder msg: 1613176480.96 epoch sec; seqNo:6718 2021-02-13T00:34:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.24 , 86.09 , 61.71 , 97.98 2021-02-13T00:34:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.10 , 0.03 2021-02-13T00:34:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176480.98 epoch sec; seqNo:13871; 4059516.38 north; 592783.29 east 2021-02-13T00:34:41Z- AHRS_M2 msg: 1613176481.34 epoch sec; seqNo:13761 2021-02-13T00:34:41Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-13T00:34:41Z- handleDepth()-Depth_Keller msg: 1613176481344 epoch sec; seqNo:13872; depth 28.14 2021-02-13T00:34:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176481.36 epoch sec; seqNo:13872; 4059516.76 north; 592783.28 east 2021-02-13T00:34:41Z- AHRS_M2 msg: 1613176481.74 epoch sec; seqNo:13762 2021-02-13T00:34:41Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-13T00:34:41Z- handleDepth()-Depth_Keller msg: 1613176481748 epoch sec; seqNo:13873; depth 28.26 2021-02-13T00:34:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176481.76 epoch sec; seqNo:13873; 4059516.76 north; 592783.28 east 2021-02-13T00:34:42Z- AHRS_M2 msg: 1613176482.15 epoch sec; seqNo:13763 2021-02-13T00:34:42Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-13T00:34:42Z- handleDepth()-Depth_Keller msg: 1613176482157 epoch sec; seqNo:13874; depth 28.38 2021-02-13T00:34:42Z- handleDvl() - RDI_Pathfinder msg: 1613176482.17 epoch sec; seqNo:6719 2021-02-13T00:34:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 0.00 , 61.30 , 97.98 2021-02-13T00:34:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , 0.06 2021-02-13T00:34:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176482.18 epoch sec; seqNo:13874; 4059517.14 north; 592783.28 east 2021-02-13T00:34:42Z- AHRS_M2 msg: 1613176482.55 epoch sec; seqNo:13764 2021-02-13T00:34:42Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.19 psi 2021-02-13T00:34:42Z- handleDepth()-Depth_Keller msg: 1613176482557 epoch sec; seqNo:13875; depth 28.52 2021-02-13T00:34:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176482.57 epoch sec; seqNo:13875; 4059517.52 north; 592783.28 east 2021-02-13T00:34:42Z- LcmTrn::handleMessages() - handling 44 messages took 0.63 ms and 132 calls... 2021-02-13T00:34:42Z- waiting for 1613176480.35, time is 1613176482.57 2021-02-13T00:34:42Z- LcmTrn::updateTrn() >>>> pose time 1613176482.57, meas time 1613176482.17 <<<< 2021-02-13T00:34:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176482.17 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:34:42Z- TerrainNav::motionUpdate() - currEstimate = (4059518, 592783.28, 28.52) 2021-02-13T00:34:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:34:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:34:42Z- TNavBF::Projecting Measurements 2021-02-13T00:34:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:34:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:34:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:34:42Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:34:42Z- TNavFilter::3 good beams. 2021-02-13T00:34:42Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:34:42Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:34:42Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:34:42Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:34:42Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:34:42Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:34:42Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:34:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:34:42Z- TerrainMapOctree: (vehN, vehE ) = (4059517.22, 592783.19). 2021-02-13T00:34:42Z- TerrainMapOctree: Min Distance = 2270.25. 2021-02-13T00:34:42Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:34:42Z- There is no specific good beam that all particles have in common. 2021-02-13T00:34:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:34:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.108 ExpectedMeasDiff: nan currMeas.covariance: 7.27 2021-02-13T00:34:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:34:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 6.01 2021-02-13T00:34:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:34:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:34:42Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:34:42Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:34:42Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:34:42Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:34:42Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:34:42Z- Filter 0 No beams used in subcloud update 2021-02-13T00:34:42Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:34:42Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:34:42Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 696.6 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:34:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176482.17 sec. 2021-02-13T00:34:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:34:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:34:42Z- variances([0], [2], and psi_berg) = 696.630302 832.996203 0.000000 2021-02-13T00:34:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:34:42Z- LcmTrn::publishEstimates() - MLE : 4059517.02 592756.78 28.52 2021-02-13T00:34:42Z- LcmTrn::publishEstimates() - MMSE : 4059517.52 592783.28 28.52 2021-02-13T00:34:42Z- LcmTrn::publishEstimates() - COVAR: 696.63 833.00 0.84 0.00 2021-02-13T00:34:42Z- AHRS_M2 msg: 1613176482.95 epoch sec; seqNo:13765 2021-02-13T00:34:42Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.19 psi 2021-02-13T00:34:42Z- handleDepth()-Depth_Keller msg: 1613176482960 epoch sec; seqNo:13876; depth 28.63 2021-02-13T00:34:42Z- handleDvl() - RDI_Pathfinder msg: 1613176482.97 epoch sec; seqNo:6720 2021-02-13T00:34:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 85.00 , 62.12 , 97.98 2021-02-13T00:34:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , 0.06 2021-02-13T00:34:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176482.98 epoch sec; seqNo:13876; 4059517.91 north; 592784.49 east 2021-02-13T00:34:43Z- AHRS_M2 msg: 1613176483.36 epoch sec; seqNo:13766 2021-02-13T00:34:43Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.19 psi 2021-02-13T00:34:43Z- handleDepth()-Depth_Keller msg: 1613176483367 epoch sec; seqNo:13877; depth 28.77 2021-02-13T00:34:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176483.38 epoch sec; seqNo:13877; 4059517.91 north; 592784.49 east 2021-02-13T00:34:43Z- AHRS_M2 msg: 1613176483.76 epoch sec; seqNo:13767 2021-02-13T00:34:43Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.19 psi 2021-02-13T00:34:43Z- handleDepth()-Depth_Keller msg: 1613176483768 epoch sec; seqNo:13878; depth 28.88 2021-02-13T00:34:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176483.78 epoch sec; seqNo:13878; 4059518.29 north; 592784.49 east 2021-02-13T00:34:44Z- AHRS_M2 msg: 1613176484.17 epoch sec; seqNo:13768 2021-02-13T00:34:44Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.19 psi 2021-02-13T00:34:44Z- handleDepth()-Depth_Keller msg: 1613176484172 epoch sec; seqNo:13879; depth 29.00 2021-02-13T00:34:44Z- handleDvl() - RDI_Pathfinder msg: 1613176484.18 epoch sec; seqNo:6721 2021-02-13T00:34:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.02 , 83.92 , 61.71 , 97.98 2021-02-13T00:34:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , 0.04 2021-02-13T00:34:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176484.19 epoch sec; seqNo:13879; 4059518.67 north; 592784.49 east 2021-02-13T00:34:44Z- AHRS_M2 msg: 1613176484.58 epoch sec; seqNo:13769 2021-02-13T00:34:44Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.19 psi 2021-02-13T00:34:44Z- handleDepth()-Depth_Keller msg: 1613176484580 epoch sec; seqNo:13880; depth 29.13 2021-02-13T00:34:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176484.60 epoch sec; seqNo:13880; 4059518.67 north; 592784.49 east 2021-02-13T00:34:44Z- AHRS_M2 msg: 1613176484.98 epoch sec; seqNo:13770 2021-02-13T00:34:44Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.19 psi 2021-02-13T00:34:44Z- handleDepth()-Depth_Keller msg: 1613176484980 epoch sec; seqNo:13881; depth 29.26 2021-02-13T00:34:45Z- handleDvl() - RDI_Pathfinder msg: 1613176485.00 epoch sec; seqNo:6722 2021-02-13T00:34:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.03 , 85.00 , 61.30 , 97.98 2021-02-13T00:34:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.00 , -0.01 2021-02-13T00:34:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176485.01 epoch sec; seqNo:13881; 4059519.04 north; 592784.48 east 2021-02-13T00:34:45Z- AHRS_M2 msg: 1613176485.38 epoch sec; seqNo:13771 2021-02-13T00:34:45Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-13T00:34:45Z- handleDepth()-Depth_Keller msg: 1613176485384 epoch sec; seqNo:13882; depth 29.39 2021-02-13T00:34:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176485.39 epoch sec; seqNo:13882; 4059519.44 north; 592785.70 east 2021-02-13T00:34:45Z- AHRS_M2 msg: 1613176485.78 epoch sec; seqNo:13772 2021-02-13T00:34:45Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-13T00:34:45Z- handleDepth()-Depth_Keller msg: 1613176485788 epoch sec; seqNo:13883; depth 29.54 2021-02-13T00:34:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176485.80 epoch sec; seqNo:13883; 4059519.81 north; 592785.69 east 2021-02-13T00:34:46Z- AHRS_M2 msg: 1613176486.19 epoch sec; seqNo:13773 2021-02-13T00:34:46Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.19 psi 2021-02-13T00:34:46Z- handleDepth()-Depth_Keller msg: 1613176486202 epoch sec; seqNo:13884; depth 29.66 2021-02-13T00:34:46Z- handleDvl() - RDI_Pathfinder msg: 1613176486.21 epoch sec; seqNo:6723 2021-02-13T00:34:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.21 , 83.92 , 60.08 , 97.98 2021-02-13T00:34:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.00 , -0.03 2021-02-13T00:34:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176486.22 epoch sec; seqNo:13884; 4059520.19 north; 592785.69 east 2021-02-13T00:34:46Z- AHRS_M2 msg: 1613176486.59 epoch sec; seqNo:13774 2021-02-13T00:34:46Z- AHRS_M2 msg: 0.62 phi; -0.27 theta; -0.18 psi 2021-02-13T00:34:46Z- handleDepth()-Depth_Keller msg: 1613176486601 epoch sec; seqNo:13885; depth 29.84 2021-02-13T00:34:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176486.63 epoch sec; seqNo:13885; 4059520.19 north; 592785.69 east 2021-02-13T00:34:46Z- AHRS_M2 msg: 1613176487.00 epoch sec; seqNo:13775 2021-02-13T00:34:46Z- AHRS_M2 msg: 0.61 phi; -0.23 theta; -0.17 psi 2021-02-13T00:34:47Z- handleDepth()-Depth_Keller msg: 1613176487000 epoch sec; seqNo:13886; depth 29.98 2021-02-13T00:34:47Z- handleDvl() - RDI_Pathfinder msg: 1613176487.01 epoch sec; seqNo:6724 2021-02-13T00:34:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.57 , 82.84 , 60.49 , 0.00 2021-02-13T00:34:47Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.06 , -0.05 2021-02-13T00:34:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176487.02 epoch sec; seqNo:13886; 4059520.57 north; 592785.69 east 2021-02-13T00:34:47Z- AHRS_M2 msg: 1613176487.40 epoch sec; seqNo:13776 2021-02-13T00:34:47Z- AHRS_M2 msg: 0.60 phi; -0.18 theta; -0.16 psi 2021-02-13T00:34:47Z- handleDepth()-Depth_Keller msg: 1613176487406 epoch sec; seqNo:13887; depth 30.11 2021-02-13T00:34:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176487.42 epoch sec; seqNo:13887; 4059520.95 north; 592785.68 east 2021-02-13T00:34:47Z- AHRS_M2 msg: 1613176487.80 epoch sec; seqNo:13777 2021-02-13T00:34:47Z- AHRS_M2 msg: 0.59 phi; -0.14 theta; -0.16 psi 2021-02-13T00:34:47Z- handleDepth()-Depth_Keller msg: 1613176487808 epoch sec; seqNo:13888; depth 30.24 2021-02-13T00:34:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176487.82 epoch sec; seqNo:13888; 4059521.34 north; 592786.90 east 2021-02-13T00:34:47Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 131 calls... 2021-02-13T00:34:47Z- waiting for 1613176485.57, time is 1613176487.82 2021-02-13T00:34:47Z- LcmTrn::updateTrn() >>>> pose time 1613176487.82, meas time 1613176487.01 <<<< 2021-02-13T00:34:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176487.01 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:34:47Z- TerrainNav::motionUpdate() - currEstimate = (4059521, 592786.90, 30.24) 2021-02-13T00:34:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:34:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:34:47Z- TNavBF::Projecting Measurements 2021-02-13T00:34:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:34:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:34:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:34:47Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:34:47Z- TNavFilter::3 good beams. 2021-02-13T00:34:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:34:47Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:34:47Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:34:47Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:34:47Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:34:47Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:34:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:34:47Z- TNavBF::mmse selected from filter -1 2021-02-13T00:34:47Z- TerrainMapOctree: (vehN, vehE ) = (4059520.75, 592786.34). 2021-02-13T00:34:47Z- TerrainMapOctree: Min Distance = 2265.53. 2021-02-13T00:34:47Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:34:47Z- There is no specific good beam that all particles have in common. 2021-02-13T00:34:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:34:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 6.67 2021-02-13T00:34:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:34:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 10.98 2021-02-13T00:34:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:34:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 5.85 2021-02-13T00:34:47Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:34:47Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:34:47Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:34:47Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:34:47Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:34:47Z- Filter 0 No beams used in subcloud update 2021-02-13T00:34:47Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:34:47Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:34:47Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 696.6 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:34:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176487.01 sec. 2021-02-13T00:34:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:34:47Z- TNavBF::mmse selected from filter -1 2021-02-13T00:34:47Z- variances([0], [2], and psi_berg) = 696.551651 832.996163 0.000000 2021-02-13T00:34:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:34:47Z- LcmTrn::publishEstimates() - MLE : 4059520.84 592760.40 30.24 2021-02-13T00:34:47Z- LcmTrn::publishEstimates() - MMSE : 4059521.34 592786.90 30.24 2021-02-13T00:34:47Z- LcmTrn::publishEstimates() - COVAR: 696.55 833.00 0.83 0.00 2021-02-13T00:34:48Z- AHRS_M2 msg: 1613176488.21 epoch sec; seqNo:13778 2021-02-13T00:34:48Z- AHRS_M2 msg: 0.58 phi; -0.09 theta; -0.15 psi 2021-02-13T00:34:48Z- handleDepth()-Depth_Keller msg: 1613176488212 epoch sec; seqNo:13889; depth 30.35 2021-02-13T00:34:48Z- handleDvl() - RDI_Pathfinder msg: 1613176488.22 epoch sec; seqNo:6725 2021-02-13T00:34:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.35 , 78.51 , 61.71 , 79.59 2021-02-13T00:34:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , -0.05 2021-02-13T00:34:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176488.25 epoch sec; seqNo:13889; 4059521.72 north; 592786.89 east 2021-02-13T00:34:48Z- AHRS_M2 msg: 1613176488.61 epoch sec; seqNo:13779 2021-02-13T00:34:48Z- AHRS_M2 msg: 0.57 phi; -0.05 theta; -0.15 psi 2021-02-13T00:34:48Z- handleDepth()-Depth_Keller msg: 1613176488616 epoch sec; seqNo:13890; depth 30.46 2021-02-13T00:34:48Z- handleDvl() - RDI_Pathfinder msg: 1613176488.63 epoch sec; seqNo:6726 2021-02-13T00:34:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.44 , 0.00 , 65.38 , 69.91 2021-02-13T00:34:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.07 , -0.00 2021-02-13T00:34:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176488.64 epoch sec; seqNo:13890; 4059522.10 north; 592786.89 east 2021-02-13T00:34:49Z- AHRS_M2 msg: 1613176489.02 epoch sec; seqNo:13780 2021-02-13T00:34:49Z- AHRS_M2 msg: 0.57 phi; -0.01 theta; -0.15 psi 2021-02-13T00:34:49Z- handleDepth()-Depth_Keller msg: 1613176489025 epoch sec; seqNo:13891; depth 30.52 2021-02-13T00:34:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176489.04 epoch sec; seqNo:13891; 4059522.48 north; 592786.89 east 2021-02-13T00:34:49Z- AHRS_M2 msg: 1613176489.42 epoch sec; seqNo:13781 2021-02-13T00:34:49Z- AHRS_M2 msg: 0.56 phi; 0.03 theta; -0.15 psi 2021-02-13T00:34:49Z- handleDepth()-Depth_Keller msg: 1613176489424 epoch sec; seqNo:13892; depth 30.58 2021-02-13T00:34:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176489.44 epoch sec; seqNo:13892; 4059522.48 north; 592786.89 east 2021-02-13T00:34:49Z- AHRS_M2 msg: 1613176489.82 epoch sec; seqNo:13782 2021-02-13T00:34:49Z- AHRS_M2 msg: 0.54 phi; 0.07 theta; -0.15 psi 2021-02-13T00:34:49Z- handleDepth()-Depth_Keller msg: 1613176489828 epoch sec; seqNo:13893; depth 30.64 2021-02-13T00:34:49Z- handleDvl() - RDI_Pathfinder msg: 1613176489.84 epoch sec; seqNo:6727 2021-02-13T00:34:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.16 , 73.59 , 0.00 , 66.54 2021-02-13T00:34:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.02 2021-02-13T00:34:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176489.86 epoch sec; seqNo:13893; 4059522.86 north; 592786.88 east 2021-02-13T00:34:50Z- AHRS_M2 msg: 1613176490.23 epoch sec; seqNo:13783 2021-02-13T00:34:50Z- AHRS_M2 msg: 0.54 phi; 0.11 theta; -0.15 psi 2021-02-13T00:34:50Z- handleDepth()-Depth_Keller msg: 1613176490233 epoch sec; seqNo:13894; depth 30.66 2021-02-13T00:34:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176490.24 epoch sec; seqNo:13894; 4059523.24 north; 592786.88 east 2021-02-13T00:34:50Z- AHRS_M2 msg: 1613176490.63 epoch sec; seqNo:13784 2021-02-13T00:34:50Z- AHRS_M2 msg: 0.53 phi; 0.14 theta; -0.15 psi 2021-02-13T00:34:50Z- handleDepth()-Depth_Keller msg: 1613176490638 epoch sec; seqNo:13895; depth 30.67 2021-02-13T00:34:50Z- handleDvl() - RDI_Pathfinder msg: 1613176490.65 epoch sec; seqNo:6728 2021-02-13T00:34:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.76 , 0.00 , 0.00 , 63.76 2021-02-13T00:34:50Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:34:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176490.66 epoch sec; seqNo:13895; 4059523.62 north; 592786.87 east 2021-02-13T00:34:51Z- AHRS_M2 msg: 1613176491.04 epoch sec; seqNo:13785 2021-02-13T00:34:51Z- AHRS_M2 msg: 0.52 phi; 0.17 theta; -0.15 psi 2021-02-13T00:34:51Z- handleDepth()-Depth_Keller msg: 1613176491046 epoch sec; seqNo:13896; depth 30.69 2021-02-13T00:34:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176491.06 epoch sec; seqNo:13896; 4059524.01 north; 592788.09 east 2021-02-13T00:34:51Z- handleDepth()-Depth_Keller msg: 1613176491459 epoch sec; seqNo:13897; depth 30.67 2021-02-13T00:34:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176491.47 epoch sec; seqNo:13897; 4059524.01 north; 592788.09 east 2021-02-13T00:34:51Z- AHRS_M2 msg: 1613176491.85 epoch sec; seqNo:13786 2021-02-13T00:34:51Z- AHRS_M2 msg: 0.52 phi; 0.22 theta; -0.15 psi 2021-02-13T00:34:51Z- handleDepth()-Depth_Keller msg: 1613176491851 epoch sec; seqNo:13898; depth 30.65 2021-02-13T00:34:51Z- handleDvl() - RDI_Pathfinder msg: 1613176491.86 epoch sec; seqNo:6729 2021-02-13T00:34:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 64.74 , 0.00 , 0.00 , 62.18 2021-02-13T00:34:51Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:34:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176491.87 epoch sec; seqNo:13898; 4059524.39 north; 592788.09 east 2021-02-13T00:34:52Z- AHRS_M2 msg: 1613176492.25 epoch sec; seqNo:13787 2021-02-13T00:34:52Z- AHRS_M2 msg: 0.51 phi; 0.24 theta; -0.15 psi 2021-02-13T00:34:52Z- handleDepth()-Depth_Keller msg: 1613176492259 epoch sec; seqNo:13899; depth 30.63 2021-02-13T00:34:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176492.27 epoch sec; seqNo:13899; 4059524.77 north; 592788.08 east 2021-02-13T00:34:52Z- AHRS_M2 msg: 1613176492.65 epoch sec; seqNo:13788 2021-02-13T00:34:52Z- AHRS_M2 msg: 0.51 phi; 0.26 theta; -0.15 psi 2021-02-13T00:34:52Z- handleDepth()-Depth_Keller msg: 1613176492656 epoch sec; seqNo:13900; depth 30.58 2021-02-13T00:34:52Z- handleDvl() - RDI_Pathfinder msg: 1613176492.68 epoch sec; seqNo:6730 2021-02-13T00:34:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 65.58 , 0.00 , 0.00 , 61.75 2021-02-13T00:34:52Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:34:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176492.69 epoch sec; seqNo:13900; 4059525.15 north; 592788.08 east 2021-02-13T00:34:52Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-13T00:34:52Z- waiting for 1613176490.82, time is 1613176492.69 2021-02-13T00:34:52Z- LcmTrn::updateTrn() equating pose 1613176492.69 and meas times 1613176492.69 2021-02-13T00:34:52Z- LcmTrn::updateTrn() >>>> pose time 1613176492.69, meas time 1613176492.69 <<<< 2021-02-13T00:34:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176492.69 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:34:52Z- TerrainNav::motionUpdate() - currEstimate = (4059525, 592788.08, 30.58) 2021-02-13T00:34:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:34:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:34:52Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:34:52Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:34:52Z- TNavBF::Projecting Measurements 2021-02-13T00:34:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:34:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:34:52Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-13T00:34:52Z- TNavFilter::2 good beams. 2021-02-13T00:34:52Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:34:52Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:34:52Z- TNavBF:: beamIndex[1] = 3 2021-02-13T00:34:52Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:34:52Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:34:52Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-13T00:34:52Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:34:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:34:53Z- TerrainMapOctree: (vehN, vehE ) = (4059525.15, 592788.08). 2021-02-13T00:34:53Z- TerrainMapOctree: Min Distance = 2261.05. 2021-02-13T00:34:53Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:34:53Z- There is no specific good beam that all particles have in common. 2021-02-13T00:34:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:34:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 6.88 2021-02-13T00:34:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:34:53Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 6.10 2021-02-13T00:34:53Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:34:53Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:34:53Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:34:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:34:53Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:34:53Z- Filter 0 No beams used in subcloud update 2021-02-13T00:34:53Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:34:53Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:34:53Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 696.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:34:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176492.69 sec. 2021-02-13T00:34:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:34:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:34:53Z- variances([0], [2], and psi_berg) = 696.499604 832.996137 0.000000 2021-02-13T00:34:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:34:53Z- LcmTrn::publishEstimates() - MLE : 4059524.65 592761.58 30.58 2021-02-13T00:34:53Z- LcmTrn::publishEstimates() - MMSE : 4059525.15 592788.08 30.58 2021-02-13T00:34:53Z- LcmTrn::publishEstimates() - COVAR: 696.50 833.00 0.82 0.00 2021-02-13T00:34:53Z- AHRS_M2 msg: 1613176493.06 epoch sec; seqNo:13789 2021-02-13T00:34:53Z- AHRS_M2 msg: 0.51 phi; 0.28 theta; -0.15 psi 2021-02-13T00:34:53Z- handleDepth()-Depth_Keller msg: 1613176493060 epoch sec; seqNo:13901; depth 30.52 2021-02-13T00:34:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176493.07 epoch sec; seqNo:13901; 4059525.15 north; 592788.08 east 2021-02-13T00:34:53Z- AHRS_M2 msg: 1613176493.46 epoch sec; seqNo:13790 2021-02-13T00:34:53Z- AHRS_M2 msg: 0.51 phi; 0.29 theta; -0.14 psi 2021-02-13T00:34:53Z- handleDepth()-Depth_Keller msg: 1613176493464 epoch sec; seqNo:13902; depth 30.47 2021-02-13T00:34:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176493.47 epoch sec; seqNo:13902; 4059525.52 north; 592788.07 east 2021-02-13T00:34:53Z- AHRS_M2 msg: 1613176493.86 epoch sec; seqNo:13791 2021-02-13T00:34:53Z- AHRS_M2 msg: 0.52 phi; 0.31 theta; -0.13 psi 2021-02-13T00:34:53Z- handleDepth()-Depth_Keller msg: 1613176493870 epoch sec; seqNo:13903; depth 30.41 2021-02-13T00:34:53Z- handleDvl() - RDI_Pathfinder msg: 1613176493.88 epoch sec; seqNo:6731 2021-02-13T00:34:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 66.86 , 0.00 , 0.00 , 60.90 2021-02-13T00:34:53Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:34:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176493.89 epoch sec; seqNo:13903; 4059525.90 north; 592788.07 east 2021-02-13T00:34:54Z- AHRS_M2 msg: 1613176494.27 epoch sec; seqNo:13792 2021-02-13T00:34:54Z- AHRS_M2 msg: 0.52 phi; 0.32 theta; -0.13 psi 2021-02-13T00:34:54Z- handleDepth()-Depth_Keller msg: 1613176494272 epoch sec; seqNo:13904; depth 30.34 2021-02-13T00:34:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176494.30 epoch sec; seqNo:13904; 4059526.29 north; 592789.29 east 2021-02-13T00:34:54Z- AHRS_M2 msg: 1613176494.67 epoch sec; seqNo:13793 2021-02-13T00:34:54Z- AHRS_M2 msg: 0.53 phi; 0.33 theta; -0.12 psi 2021-02-13T00:34:54Z- handleDepth()-Depth_Keller msg: 1613176494677 epoch sec; seqNo:13905; depth 30.25 2021-02-13T00:34:54Z- handleDvl() - RDI_Pathfinder msg: 1613176494.69 epoch sec; seqNo:6732 2021-02-13T00:34:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:34:54Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:34:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176494.72 epoch sec; seqNo:13905; 4059526.67 north; 592789.28 east 2021-02-13T00:34:55Z- AHRS_M2 msg: 1613176495.08 epoch sec; seqNo:13794 2021-02-13T00:34:55Z- AHRS_M2 msg: 0.54 phi; 0.34 theta; -0.11 psi 2021-02-13T00:34:55Z- handleDepth()-Depth_Keller msg: 1613176495080 epoch sec; seqNo:13906; depth 30.16 2021-02-13T00:34:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176495.10 epoch sec; seqNo:13906; 4059526.67 north; 592789.28 east 2021-02-13T00:34:55Z- AHRS_M2 msg: 1613176495.48 epoch sec; seqNo:13795 2021-02-13T00:34:55Z- AHRS_M2 msg: 0.54 phi; 0.35 theta; -0.11 psi 2021-02-13T00:34:55Z- handleDepth()-Depth_Keller msg: 1613176495491 epoch sec; seqNo:13907; depth 30.07 2021-02-13T00:34:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176495.50 epoch sec; seqNo:13907; 4059527.05 north; 592789.28 east 2021-02-13T00:34:55Z- AHRS_M2 msg: 1613176495.89 epoch sec; seqNo:13796 2021-02-13T00:34:55Z- AHRS_M2 msg: 0.55 phi; 0.35 theta; -0.11 psi 2021-02-13T00:34:55Z- handleDepth()-Depth_Keller msg: 1613176495903 epoch sec; seqNo:13908; depth 29.98 2021-02-13T00:34:55Z- handleDvl() - RDI_Pathfinder msg: 1613176495.91 epoch sec; seqNo:6733 2021-02-13T00:34:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.66 , 79.59 , 97.98 , 59.26 2021-02-13T00:34:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.07 , -0.09 2021-02-13T00:34:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176495.93 epoch sec; seqNo:13908; 4059527.43 north; 592789.27 east 2021-02-13T00:34:56Z- AHRS_M2 msg: 1613176496.29 epoch sec; seqNo:13797 2021-02-13T00:34:56Z- AHRS_M2 msg: 0.55 phi; 0.36 theta; -0.11 psi 2021-02-13T00:34:56Z- handleDepth()-Depth_Keller msg: 1613176496301 epoch sec; seqNo:13909; depth 29.86 2021-02-13T00:34:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176496.31 epoch sec; seqNo:13909; 4059527.43 north; 592789.27 east 2021-02-13T00:34:56Z- AHRS_M2 msg: 1613176496.69 epoch sec; seqNo:13798 2021-02-13T00:34:56Z- AHRS_M2 msg: 0.55 phi; 0.36 theta; -0.11 psi 2021-02-13T00:34:56Z- handleDepth()-Depth_Keller msg: 1613176496698 epoch sec; seqNo:13910; depth 29.76 2021-02-13T00:34:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176496.71 epoch sec; seqNo:13910; 4059527.81 north; 592789.27 east 2021-02-13T00:34:57Z- AHRS_M2 msg: 1613176497.10 epoch sec; seqNo:13799 2021-02-13T00:34:57Z- AHRS_M2 msg: 0.55 phi; 0.36 theta; -0.12 psi 2021-02-13T00:34:57Z- handleDepth()-Depth_Keller msg: 1613176497116 epoch sec; seqNo:13911; depth 29.65 2021-02-13T00:34:57Z- handleDvl() - RDI_Pathfinder msg: 1613176497.14 epoch sec; seqNo:6734 2021-02-13T00:34:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 77.24 , 97.98 , 59.25 2021-02-13T00:34:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.20 , -0.08 2021-02-13T00:34:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176497.16 epoch sec; seqNo:13911; 4059528.19 north; 592789.27 east 2021-02-13T00:34:57Z- AHRS_M2 msg: 1613176497.50 epoch sec; seqNo:13800 2021-02-13T00:34:57Z- AHRS_M2 msg: 0.55 phi; 0.36 theta; -0.12 psi 2021-02-13T00:34:57Z- handleDepth()-Depth_Keller msg: 1613176497504 epoch sec; seqNo:13912; depth 29.53 2021-02-13T00:34:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176497.52 epoch sec; seqNo:13912; 4059528.57 north; 592789.26 east 2021-02-13T00:34:57Z- AHRS_M2 msg: 1613176497.91 epoch sec; seqNo:13801 2021-02-13T00:34:57Z- AHRS_M2 msg: 0.55 phi; 0.36 theta; -0.12 psi 2021-02-13T00:34:57Z- handleDepth()-Depth_Keller msg: 1613176497915 epoch sec; seqNo:13913; depth 29.43 2021-02-13T00:34:57Z- handleDvl() - RDI_Pathfinder msg: 1613176497.93 epoch sec; seqNo:6735 2021-02-13T00:34:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 79.60 , 97.98 , 60.07 2021-02-13T00:34:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.10 , -0.01 2021-02-13T00:34:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176497.94 epoch sec; seqNo:13913; 4059528.57 north; 592789.26 east 2021-02-13T00:34:58Z- LcmTrn::handleMessages() - handling 44 messages took 0.74 ms and 130 calls... 2021-02-13T00:34:58Z- waiting for 1613176495.69, time is 1613176497.94 2021-02-13T00:34:58Z- LcmTrn::updateTrn() equating pose 1613176497.94 and meas times 1613176497.94 2021-02-13T00:34:58Z- LcmTrn::updateTrn() >>>> pose time 1613176497.94, meas time 1613176497.94 <<<< 2021-02-13T00:34:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176497.94 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:34:58Z- TerrainNav::motionUpdate() - currEstimate = (4059529, 592789.26, 29.43) 2021-02-13T00:34:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:34:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:34:58Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176497.94 sec.; No relevant navigation data available 2021-02-13T00:34:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:34:58Z- TNavBF::mmse selected from filter -1 2021-02-13T00:34:58Z- variances([0], [2], and psi_berg) = 696.449986 832.996112 0.000000 2021-02-13T00:34:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:34:58Z- LcmTrn::publishEstimates() - MLE : 4059528.07 592762.76 29.43 2021-02-13T00:34:58Z- LcmTrn::publishEstimates() - MMSE : 4059528.57 592789.26 29.43 2021-02-13T00:34:58Z- LcmTrn::publishEstimates() - COVAR: 696.45 833.00 1.01 0.00 2021-02-13T00:34:58Z- AHRS_M2 msg: 1613176498.31 epoch sec; seqNo:13802 2021-02-13T00:34:58Z- AHRS_M2 msg: 0.55 phi; 0.36 theta; -0.13 psi 2021-02-13T00:34:58Z- handleDepth()-Depth_Keller msg: 1613176498312 epoch sec; seqNo:13914; depth 29.31 2021-02-13T00:34:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176498.33 epoch sec; seqNo:13914; 4059528.96 north; 592790.48 east 2021-02-13T00:34:58Z- AHRS_M2 msg: 1613176498.71 epoch sec; seqNo:13803 2021-02-13T00:34:58Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.13 psi 2021-02-13T00:34:58Z- handleDepth()-Depth_Keller msg: 1613176498716 epoch sec; seqNo:13915; depth 29.21 2021-02-13T00:34:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176498.72 epoch sec; seqNo:13915; 4059529.34 north; 592790.47 east 2021-02-13T00:34:59Z- AHRS_M2 msg: 1613176499.12 epoch sec; seqNo:13804 2021-02-13T00:34:59Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.13 psi 2021-02-13T00:34:59Z- handleDepth()-Depth_Keller msg: 1613176499125 epoch sec; seqNo:13916; depth 29.10 2021-02-13T00:34:59Z- handleDvl() - RDI_Pathfinder msg: 1613176499.14 epoch sec; seqNo:6736 2021-02-13T00:34:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 78.47 , 97.98 , 60.49 2021-02-13T00:34:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , 0.02 2021-02-13T00:34:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176499.17 epoch sec; seqNo:13916; 4059529.72 north; 592790.47 east 2021-02-13T00:34:59Z- AHRS_M2 msg: 1613176499.52 epoch sec; seqNo:13805 2021-02-13T00:34:59Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.13 psi 2021-02-13T00:34:59Z- handleDepth()-Depth_Keller msg: 1613176499526 epoch sec; seqNo:13917; depth 28.98 2021-02-13T00:34:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176499.54 epoch sec; seqNo:13917; 4059529.72 north; 592790.47 east 2021-02-13T00:34:59Z- AHRS_M2 msg: 1613176499.92 epoch sec; seqNo:13806 2021-02-13T00:34:59Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.13 psi 2021-02-13T00:34:59Z- handleDepth()-Depth_Keller msg: 1613176499928 epoch sec; seqNo:13918; depth 28.87 2021-02-13T00:34:59Z- handleDvl() - RDI_Pathfinder msg: 1613176499.95 epoch sec; seqNo:6737 2021-02-13T00:34:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 80.67 , 97.98 , 61.30 2021-02-13T00:34:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , 0.02 2021-02-13T00:34:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176499.96 epoch sec; seqNo:13918; 4059530.10 north; 592790.47 east 2021-02-13T00:35:00Z- AHRS_M2 msg: 1613176500.33 epoch sec; seqNo:13807 2021-02-13T00:35:00Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.13 psi 2021-02-13T00:35:00Z- handleDepth()-Depth_Keller msg: 1613176500332 epoch sec; seqNo:13919; depth 28.75 2021-02-13T00:35:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176500.34 epoch sec; seqNo:13919; 4059530.48 north; 592790.46 east 2021-02-13T00:35:00Z- AHRS_M2 msg: 1613176500.73 epoch sec; seqNo:13808 2021-02-13T00:35:00Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.13 psi 2021-02-13T00:35:00Z- handleDepth()-Depth_Keller msg: 1613176500736 epoch sec; seqNo:13920; depth 28.64 2021-02-13T00:35:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176500.74 epoch sec; seqNo:13920; 4059530.85 north; 592790.46 east 2021-02-13T00:35:01Z- AHRS_M2 msg: 1613176501.14 epoch sec; seqNo:13809 2021-02-13T00:35:01Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.12 psi 2021-02-13T00:35:01Z- handleDepth()-Depth_Keller msg: 1613176501146 epoch sec; seqNo:13921; depth 28.54 2021-02-13T00:35:01Z- handleDvl() - RDI_Pathfinder msg: 1613176501.16 epoch sec; seqNo:6738 2021-02-13T00:35:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 79.29 , 97.98 , 62.12 2021-02-13T00:35:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , -0.00 2021-02-13T00:35:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176501.17 epoch sec; seqNo:13921; 4059530.87 north; 592791.68 east 2021-02-13T00:35:01Z- AHRS_M2 msg: 1613176501.54 epoch sec; seqNo:13810 2021-02-13T00:35:01Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.12 psi 2021-02-13T00:35:01Z- handleDepth()-Depth_Keller msg: 1613176501544 epoch sec; seqNo:13922; depth 28.40 2021-02-13T00:35:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176501.57 epoch sec; seqNo:13922; 4059531.25 north; 592791.67 east 2021-02-13T00:35:01Z- AHRS_M2 msg: 1613176501.94 epoch sec; seqNo:13811 2021-02-13T00:35:01Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.12 psi 2021-02-13T00:35:01Z- handleDepth()-Depth_Keller msg: 1613176501953 epoch sec; seqNo:13923; depth 28.29 2021-02-13T00:35:01Z- handleDvl() - RDI_Pathfinder msg: 1613176501.97 epoch sec; seqNo:6739 2021-02-13T00:35:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 81.75 , 97.98 , 62.12 2021-02-13T00:35:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.03 2021-02-13T00:35:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176501.98 epoch sec; seqNo:13923; 4059531.63 north; 592791.67 east 2021-02-13T00:35:02Z- AHRS_M2 msg: 1613176502.35 epoch sec; seqNo:13812 2021-02-13T00:35:02Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.12 psi 2021-02-13T00:35:02Z- handleDepth()-Depth_Keller msg: 1613176502357 epoch sec; seqNo:13924; depth 28.17 2021-02-13T00:35:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176502.37 epoch sec; seqNo:13924; 4059531.63 north; 592791.67 east 2021-02-13T00:35:02Z- AHRS_M2 msg: 1613176502.76 epoch sec; seqNo:13813 2021-02-13T00:35:02Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.12 psi 2021-02-13T00:35:02Z- handleDepth()-Depth_Keller msg: 1613176502770 epoch sec; seqNo:13925; depth 28.06 2021-02-13T00:35:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176502.78 epoch sec; seqNo:13925; 4059532.00 north; 592791.67 east 2021-02-13T00:35:03Z- LcmTrn::handleMessages() - handling 40 messages took 0.68 ms and 127 calls... 2021-02-13T00:35:03Z- waiting for 1613176500.94, time is 1613176502.78 2021-02-13T00:35:03Z- LcmTrn::updateTrn() >>>> pose time 1613176502.78, meas time 1613176501.97 <<<< 2021-02-13T00:35:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176501.97 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:35:03Z- TerrainNav::motionUpdate() - currEstimate = (4059532, 592791.67, 28.06) 2021-02-13T00:35:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:35:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:35:03Z- TNavBF::Projecting Measurements 2021-02-13T00:35:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:35:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:35:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:35:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:35:03Z- TNavFilter::4 good beams. 2021-02-13T00:35:03Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:35:03Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:35:03Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:35:03Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:35:03Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:35:03Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:35:03Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:35:03Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:03Z- TerrainMapOctree: (vehN, vehE ) = (4059531.43, 592791.26). 2021-02-13T00:35:03Z- TerrainMapOctree: Min Distance = 2254.20. 2021-02-13T00:35:03Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:35:03Z- There is no specific good beam that all particles have in common. 2021-02-13T00:35:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:35:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-13T00:35:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:35:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 10.69 2021-02-13T00:35:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:35:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:35:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:35:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 6.17 2021-02-13T00:35:03Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:35:03Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:35:03Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:35:03Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:35:03Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:35:03Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:35:03Z- Filter 0 No beams used in subcloud update 2021-02-13T00:35:03Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:35:03Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:35:03Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 696.4 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:35:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176501.97 sec. 2021-02-13T00:35:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:35:03Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:03Z- variances([0], [2], and psi_berg) = 696.379469 832.996076 0.000000 2021-02-13T00:35:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:35:03Z- LcmTrn::publishEstimates() - MLE : 4059531.50 592765.17 28.06 2021-02-13T00:35:03Z- LcmTrn::publishEstimates() - MMSE : 4059532.00 592791.67 28.06 2021-02-13T00:35:03Z- LcmTrn::publishEstimates() - COVAR: 696.38 833.00 0.86 0.00 2021-02-13T00:35:03Z- AHRS_M2 msg: 1613176503.16 epoch sec; seqNo:13814 2021-02-13T00:35:03Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.12 psi 2021-02-13T00:35:03Z- handleDepth()-Depth_Keller msg: 1613176503169 epoch sec; seqNo:13926; depth 27.94 2021-02-13T00:35:03Z- handleDvl() - RDI_Pathfinder msg: 1613176503.19 epoch sec; seqNo:6740 2021-02-13T00:35:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 81.75 , 97.98 , 0.00 2021-02-13T00:35:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.02 2021-02-13T00:35:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176503.20 epoch sec; seqNo:13926; 4059532.38 north; 592791.66 east 2021-02-13T00:35:03Z- AHRS_M2 msg: 1613176503.57 epoch sec; seqNo:13815 2021-02-13T00:35:03Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.13 psi 2021-02-13T00:35:03Z- handleDepth()-Depth_Keller msg: 1613176503571 epoch sec; seqNo:13927; depth 27.81 2021-02-13T00:35:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176503.58 epoch sec; seqNo:13927; 4059532.76 north; 592791.66 east 2021-02-13T00:35:03Z- AHRS_M2 msg: 1613176503.96 epoch sec; seqNo:13816 2021-02-13T00:35:03Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.13 psi 2021-02-13T00:35:03Z- handleDepth()-Depth_Keller msg: 1613176503970 epoch sec; seqNo:13928; depth 27.71 2021-02-13T00:35:03Z- handleDvl() - RDI_Pathfinder msg: 1613176503.98 epoch sec; seqNo:6741 2021-02-13T00:35:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 83.71 , 98.32 , 62.53 2021-02-13T00:35:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , 0.00 2021-02-13T00:35:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176503.99 epoch sec; seqNo:13928; 4059532.77 north; 592792.88 east 2021-02-13T00:35:04Z- AHRS_M2 msg: 1613176504.37 epoch sec; seqNo:13817 2021-02-13T00:35:04Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.14 psi 2021-02-13T00:35:04Z- handleDepth()-Depth_Keller msg: 1613176504372 epoch sec; seqNo:13929; depth 27.57 2021-02-13T00:35:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176504.39 epoch sec; seqNo:13929; 4059533.15 north; 592792.87 east 2021-02-13T00:35:04Z- AHRS_M2 msg: 1613176504.77 epoch sec; seqNo:13818 2021-02-13T00:35:04Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.14 psi 2021-02-13T00:35:04Z- handleDepth()-Depth_Keller msg: 1613176504781 epoch sec; seqNo:13930; depth 27.44 2021-02-13T00:35:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176504.79 epoch sec; seqNo:13930; 4059533.53 north; 592792.87 east 2021-02-13T00:35:05Z- AHRS_M2 msg: 1613176505.18 epoch sec; seqNo:13819 2021-02-13T00:35:05Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.14 psi 2021-02-13T00:35:05Z- handleDepth()-Depth_Keller msg: 1613176505180 epoch sec; seqNo:13931; depth 27.34 2021-02-13T00:35:05Z- handleDvl() - RDI_Pathfinder msg: 1613176505.19 epoch sec; seqNo:6742 2021-02-13T00:35:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 82.84 , 97.98 , 62.93 2021-02-13T00:35:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.04 2021-02-13T00:35:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176505.20 epoch sec; seqNo:13931; 4059533.91 north; 592792.87 east 2021-02-13T00:35:05Z- AHRS_M2 msg: 1613176505.59 epoch sec; seqNo:13820 2021-02-13T00:35:05Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.14 psi 2021-02-13T00:35:05Z- handleDepth()-Depth_Keller msg: 1613176505594 epoch sec; seqNo:13932; depth 27.21 2021-02-13T00:35:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176505.61 epoch sec; seqNo:13932; 4059533.91 north; 592792.87 east 2021-02-13T00:35:05Z- AHRS_M2 msg: 1613176505.98 epoch sec; seqNo:13821 2021-02-13T00:35:05Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.14 psi 2021-02-13T00:35:05Z- handleDepth()-Depth_Keller msg: 1613176505988 epoch sec; seqNo:13933; depth 27.09 2021-02-13T00:35:06Z- handleDvl() - RDI_Pathfinder msg: 1613176506.00 epoch sec; seqNo:6743 2021-02-13T00:35:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 83.92 , 97.98 , 62.52 2021-02-13T00:35:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , 0.04 2021-02-13T00:35:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176506.02 epoch sec; seqNo:13933; 4059534.29 north; 592792.86 east 2021-02-13T00:35:06Z- AHRS_M2 msg: 1613176506.39 epoch sec; seqNo:13822 2021-02-13T00:35:06Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.15 psi 2021-02-13T00:35:06Z- handleDepth()-Depth_Keller msg: 1613176506392 epoch sec; seqNo:13934; depth 26.99 2021-02-13T00:35:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176506.40 epoch sec; seqNo:13934; 4059534.67 north; 592792.86 east 2021-02-13T00:35:06Z- AHRS_M2 msg: 1613176506.79 epoch sec; seqNo:13823 2021-02-13T00:35:06Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.15 psi 2021-02-13T00:35:06Z- handleDepth()-Depth_Keller msg: 1613176506805 epoch sec; seqNo:13935; depth 26.86 2021-02-13T00:35:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176506.82 epoch sec; seqNo:13935; 4059535.06 north; 592794.07 east 2021-02-13T00:35:07Z- AHRS_M2 msg: 1613176507.19 epoch sec; seqNo:13824 2021-02-13T00:35:07Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.15 psi 2021-02-13T00:35:07Z- handleDepth()-Depth_Keller msg: 1613176507210 epoch sec; seqNo:13936; depth 26.74 2021-02-13T00:35:07Z- handleDvl() - RDI_Pathfinder msg: 1613176507.22 epoch sec; seqNo:6744 2021-02-13T00:35:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 84.93 , 99.00 , 64.16 2021-02-13T00:35:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , 0.02 2021-02-13T00:35:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176507.23 epoch sec; seqNo:13936; 4059535.06 north; 592794.07 east 2021-02-13T00:35:07Z- AHRS_M2 msg: 1613176507.60 epoch sec; seqNo:13825 2021-02-13T00:35:07Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.15 psi 2021-02-13T00:35:07Z- handleDepth()-Depth_Keller msg: 1613176507604 epoch sec; seqNo:13937; depth 26.62 2021-02-13T00:35:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176507.61 epoch sec; seqNo:13937; 4059535.44 north; 592794.07 east 2021-02-13T00:35:08Z- AHRS_M2 msg: 1613176508.00 epoch sec; seqNo:13826 2021-02-13T00:35:08Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.15 psi 2021-02-13T00:35:08Z- handleDepth()-Depth_Keller msg: 1613176508013 epoch sec; seqNo:13938; depth 26.49 2021-02-13T00:35:08Z- handleDvl() - RDI_Pathfinder msg: 1613176508.02 epoch sec; seqNo:6745 2021-02-13T00:35:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 0.00 , 99.06 , 63.76 2021-02-13T00:35:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , -0.01 2021-02-13T00:35:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176508.03 epoch sec; seqNo:13938; 4059535.82 north; 592794.07 east 2021-02-13T00:35:08Z- LcmTrn::handleMessages() - handling 45 messages took 0.63 ms and 133 calls... 2021-02-13T00:35:08Z- waiting for 1613176505.78, time is 1613176508.03 2021-02-13T00:35:08Z- LcmTrn::updateTrn() equating pose 1613176508.03 and meas times 1613176508.03 2021-02-13T00:35:08Z- LcmTrn::updateTrn() >>>> pose time 1613176508.03, meas time 1613176508.03 <<<< 2021-02-13T00:35:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176508.03 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:35:08Z- TerrainNav::motionUpdate() - currEstimate = (4059536, 592794.07, 26.49) 2021-02-13T00:35:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:35:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:35:08Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176508.03 sec.; No relevant navigation data available 2021-02-13T00:35:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:35:08Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:08Z- variances([0], [2], and psi_berg) = 696.324187 832.996048 0.000000 2021-02-13T00:35:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:35:08Z- LcmTrn::publishEstimates() - MLE : 4059535.32 592767.57 26.49 2021-02-13T00:35:08Z- LcmTrn::publishEstimates() - MMSE : 4059535.82 592794.07 26.49 2021-02-13T00:35:08Z- LcmTrn::publishEstimates() - COVAR: 696.32 833.00 1.09 0.00 2021-02-13T00:35:08Z- AHRS_M2 msg: 1613176508.41 epoch sec; seqNo:13827 2021-02-13T00:35:08Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.15 psi 2021-02-13T00:35:08Z- handleDepth()-Depth_Keller msg: 1613176508414 epoch sec; seqNo:13939; depth 26.37 2021-02-13T00:35:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176508.42 epoch sec; seqNo:13939; 4059536.20 north; 592794.06 east 2021-02-13T00:35:08Z- AHRS_M2 msg: 1613176508.81 epoch sec; seqNo:13828 2021-02-13T00:35:08Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.15 psi 2021-02-13T00:35:08Z- handleDepth()-Depth_Keller msg: 1613176508816 epoch sec; seqNo:13940; depth 26.25 2021-02-13T00:35:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176508.84 epoch sec; seqNo:13940; 4059536.58 north; 592794.06 east 2021-02-13T00:35:09Z- AHRS_M2 msg: 1613176509.21 epoch sec; seqNo:13829 2021-02-13T00:35:09Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.14 psi 2021-02-13T00:35:09Z- handleDepth()-Depth_Keller msg: 1613176509224 epoch sec; seqNo:13941; depth 26.14 2021-02-13T00:35:09Z- handleDvl() - RDI_Pathfinder msg: 1613176509.24 epoch sec; seqNo:6746 2021-02-13T00:35:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.52 , 129.35 , 101.23 , 0.00 2021-02-13T00:35:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.17 , 0.75 , -0.48 2021-02-13T00:35:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176509.27 epoch sec; seqNo:13941; 4059536.21 north; 592795.28 east 2021-02-13T00:35:09Z- AHRS_M2 msg: 1613176509.62 epoch sec; seqNo:13830 2021-02-13T00:35:09Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.14 psi 2021-02-13T00:35:09Z- handleDepth()-Depth_Keller msg: 1613176509637 epoch sec; seqNo:13942; depth 26.02 2021-02-13T00:35:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176509.65 epoch sec; seqNo:13942; 4059536.21 north; 592795.28 east 2021-02-13T00:35:10Z- AHRS_M2 msg: 1613176510.04 epoch sec; seqNo:13831 2021-02-13T00:35:10Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-13T00:35:10Z- handleDepth()-Depth_Keller msg: 1613176510044 epoch sec; seqNo:13943; depth 25.89 2021-02-13T00:35:10Z- handleDvl() - RDI_Pathfinder msg: 1613176510.06 epoch sec; seqNo:6747 2021-02-13T00:35:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 87.50 , 99.67 , 62.53 2021-02-13T00:35:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.07 , -0.11 2021-02-13T00:35:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176510.07 epoch sec; seqNo:13943; 4059536.21 north; 592795.28 east 2021-02-13T00:35:10Z- AHRS_M2 msg: 1613176510.43 epoch sec; seqNo:13832 2021-02-13T00:35:10Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-13T00:35:10Z- handleDepth()-Depth_Keller msg: 1613176510437 epoch sec; seqNo:13944; depth 25.76 2021-02-13T00:35:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176510.45 epoch sec; seqNo:13944; 4059536.59 north; 592795.28 east 2021-02-13T00:35:10Z- AHRS_M2 msg: 1613176510.84 epoch sec; seqNo:13833 2021-02-13T00:35:10Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-13T00:35:10Z- handleDepth()-Depth_Keller msg: 1613176510843 epoch sec; seqNo:13945; depth 25.66 2021-02-13T00:35:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176510.86 epoch sec; seqNo:13945; 4059536.97 north; 592795.27 east 2021-02-13T00:35:11Z- AHRS_M2 msg: 1613176511.24 epoch sec; seqNo:13834 2021-02-13T00:35:11Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.15 psi 2021-02-13T00:35:11Z- handleDepth()-Depth_Keller msg: 1613176511249 epoch sec; seqNo:13946; depth 25.52 2021-02-13T00:35:11Z- handleDvl() - RDI_Pathfinder msg: 1613176511.26 epoch sec; seqNo:6748 2021-02-13T00:35:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 88.05 , 98.86 , 64.97 2021-02-13T00:35:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , -0.09 2021-02-13T00:35:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176511.27 epoch sec; seqNo:13946; 4059536.97 north; 592795.27 east 2021-02-13T00:35:11Z- AHRS_M2 msg: 1613176511.64 epoch sec; seqNo:13835 2021-02-13T00:35:11Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.15 psi 2021-02-13T00:35:11Z- handleDepth()-Depth_Keller msg: 1613176511644 epoch sec; seqNo:13947; depth 25.40 2021-02-13T00:35:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176511.65 epoch sec; seqNo:13947; 4059537.35 north; 592795.27 east 2021-02-13T00:35:12Z- AHRS_M2 msg: 1613176512.04 epoch sec; seqNo:13836 2021-02-13T00:35:12Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-13T00:35:12Z- handleDepth()-Depth_Keller msg: 1613176512048 epoch sec; seqNo:13948; depth 25.28 2021-02-13T00:35:12Z- handleDvl() - RDI_Pathfinder msg: 1613176512.06 epoch sec; seqNo:6749 2021-02-13T00:35:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 86.02 , 99.00 , 64.97 2021-02-13T00:35:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.12 , -0.04 2021-02-13T00:35:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176512.07 epoch sec; seqNo:13948; 4059537.73 north; 592795.27 east 2021-02-13T00:35:12Z- AHRS_M2 msg: 1613176512.45 epoch sec; seqNo:13837 2021-02-13T00:35:12Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-13T00:35:12Z- handleDepth()-Depth_Keller msg: 1613176512453 epoch sec; seqNo:13949; depth 25.16 2021-02-13T00:35:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176512.47 epoch sec; seqNo:13949; 4059538.12 north; 592796.48 east 2021-02-13T00:35:12Z- AHRS_M2 msg: 1613176512.88 epoch sec; seqNo:13838 2021-02-13T00:35:12Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-13T00:35:12Z- handleDepth()-Depth_Keller msg: 1613176512887 epoch sec; seqNo:13950; depth 25.05 2021-02-13T00:35:12Z- handleDvl() - RDI_Pathfinder msg: 1613176512.90 epoch sec; seqNo:6750 2021-02-13T00:35:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 86.01 , 99.00 , 65.39 2021-02-13T00:35:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.10 , 0.03 2021-02-13T00:35:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176512.91 epoch sec; seqNo:13950; 4059538.50 north; 592796.48 east 2021-02-13T00:35:13Z- AHRS_M2 msg: 1613176513.26 epoch sec; seqNo:13839 2021-02-13T00:35:13Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-13T00:35:13Z- handleDepth()-Depth_Keller msg: 1613176513265 epoch sec; seqNo:13951; depth 24.92 2021-02-13T00:35:13Z- LcmTrn::handleMessages() - handling 43 messages took 0.69 ms and 129 calls... 2021-02-13T00:35:13Z- waiting for 1613176511.03, time is 1613176513.26 2021-02-13T00:35:13Z- LcmTrn::updateTrn() >>>> pose time 1613176513.26, meas time 1613176512.90 <<<< 2021-02-13T00:35:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176512.90 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:35:13Z- TerrainNav::motionUpdate() - currEstimate = (4059538, 592796.48, 24.92) 2021-02-13T00:35:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:35:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:35:13Z- TNavBF::Projecting Measurements 2021-02-13T00:35:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:35:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:35:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:35:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:35:13Z- TNavFilter::4 good beams. 2021-02-13T00:35:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:35:13Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:35:13Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:35:13Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:35:13Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:35:13Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:35:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:35:13Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:13Z- TerrainMapOctree: (vehN, vehE ) = (4059538.31, 592796.31). 2021-02-13T00:35:13Z- TerrainMapOctree: Min Distance = 2245.68. 2021-02-13T00:35:13Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:35:13Z- There is no specific good beam that all particles have in common. 2021-02-13T00:35:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:35:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-13T00:35:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:35:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 11.84 2021-02-13T00:35:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:35:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 15.68 2021-02-13T00:35:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:35:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-13T00:35:13Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:35:13Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:35:13Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:35:13Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:35:13Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:35:13Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:35:13Z- Filter 0 No beams used in subcloud update 2021-02-13T00:35:13Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:35:13Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:35:13Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 696.3 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:35:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176512.90 sec. 2021-02-13T00:35:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:35:13Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:13Z- variances([0], [2], and psi_berg) = 696.263511 832.996017 0.000000 2021-02-13T00:35:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:35:13Z- LcmTrn::publishEstimates() - MLE : 4059538.00 592769.98 24.92 2021-02-13T00:35:13Z- LcmTrn::publishEstimates() - MMSE : 4059538.50 592796.48 24.92 2021-02-13T00:35:13Z- LcmTrn::publishEstimates() - COVAR: 696.26 833.00 0.89 0.00 2021-02-13T00:35:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176513.28 epoch sec; seqNo:13951; 4059538.50 north; 592796.48 east 2021-02-13T00:35:13Z- AHRS_M2 msg: 1613176513.66 epoch sec; seqNo:13840 2021-02-13T00:35:13Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-13T00:35:13Z- handleDepth()-Depth_Keller msg: 1613176513677 epoch sec; seqNo:13952; depth 24.81 2021-02-13T00:35:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176513.69 epoch sec; seqNo:13952; 4059538.88 north; 592796.47 east 2021-02-13T00:35:14Z- AHRS_M2 msg: 1613176514.06 epoch sec; seqNo:13841 2021-02-13T00:35:14Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-13T00:35:14Z- handleDepth()-Depth_Keller msg: 1613176514070 epoch sec; seqNo:13953; depth 24.70 2021-02-13T00:35:14Z- handleDvl() - RDI_Pathfinder msg: 1613176514.08 epoch sec; seqNo:6751 2021-02-13T00:35:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 0.00 , 97.91 , 65.80 2021-02-13T00:35:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.07 2021-02-13T00:35:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176514.09 epoch sec; seqNo:13953; 4059539.25 north; 592796.47 east 2021-02-13T00:35:14Z- AHRS_M2 msg: 1613176514.47 epoch sec; seqNo:13842 2021-02-13T00:35:14Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-13T00:35:14Z- handleDepth()-Depth_Keller msg: 1613176514472 epoch sec; seqNo:13954; depth 24.59 2021-02-13T00:35:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176514.49 epoch sec; seqNo:13954; 4059539.63 north; 592796.47 east 2021-02-13T00:35:14Z- AHRS_M2 msg: 1613176514.89 epoch sec; seqNo:13843 2021-02-13T00:35:14Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-13T00:35:14Z- handleDepth()-Depth_Keller msg: 1613176514892 epoch sec; seqNo:13955; depth 24.49 2021-02-13T00:35:14Z- handleDvl() - RDI_Pathfinder msg: 1613176514.91 epoch sec; seqNo:6752 2021-02-13T00:35:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 88.25 , 97.98 , 65.80 2021-02-13T00:35:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , 0.07 2021-02-13T00:35:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176514.92 epoch sec; seqNo:13955; 4059539.63 north; 592796.47 east 2021-02-13T00:35:15Z- AHRS_M2 msg: 1613176515.28 epoch sec; seqNo:13844 2021-02-13T00:35:15Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-13T00:35:15Z- handleDepth()-Depth_Keller msg: 1613176515285 epoch sec; seqNo:13956; depth 24.35 2021-02-13T00:35:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176515.30 epoch sec; seqNo:13956; 4059540.03 north; 592797.68 east 2021-02-13T00:35:15Z- AHRS_M2 msg: 1613176515.68 epoch sec; seqNo:13845 2021-02-13T00:35:15Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-13T00:35:15Z- handleDepth()-Depth_Keller msg: 1613176515684 epoch sec; seqNo:13957; depth 24.26 2021-02-13T00:35:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176515.70 epoch sec; seqNo:13957; 4059540.40 north; 592797.68 east 2021-02-13T00:35:16Z- AHRS_M2 msg: 1613176516.08 epoch sec; seqNo:13846 2021-02-13T00:35:16Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T00:35:16Z- handleDepth()-Depth_Keller msg: 1613176516093 epoch sec; seqNo:13958; depth 24.13 2021-02-13T00:35:16Z- handleDvl() - RDI_Pathfinder msg: 1613176516.10 epoch sec; seqNo:6753 2021-02-13T00:35:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 87.03 , 98.93 , 65.39 2021-02-13T00:35:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , 0.05 2021-02-13T00:35:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176516.12 epoch sec; seqNo:13958; 4059540.40 north; 592797.68 east 2021-02-13T00:35:16Z- AHRS_M2 msg: 1613176516.49 epoch sec; seqNo:13847 2021-02-13T00:35:16Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T00:35:16Z- handleDepth()-Depth_Keller msg: 1613176516492 epoch sec; seqNo:13959; depth 24.02 2021-02-13T00:35:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176516.50 epoch sec; seqNo:13959; 4059540.78 north; 592797.67 east 2021-02-13T00:35:16Z- AHRS_M2 msg: 1613176516.89 epoch sec; seqNo:13848 2021-02-13T00:35:16Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T00:35:16Z- handleDepth()-Depth_Keller msg: 1613176516901 epoch sec; seqNo:13960; depth 23.92 2021-02-13T00:35:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176516.91 epoch sec; seqNo:13960; 4059541.16 north; 592797.67 east 2021-02-13T00:35:17Z- AHRS_M2 msg: 1613176517.30 epoch sec; seqNo:13849 2021-02-13T00:35:17Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T00:35:17Z- handleDepth()-Depth_Keller msg: 1613176517305 epoch sec; seqNo:13961; depth 23.79 2021-02-13T00:35:17Z- handleDvl() - RDI_Pathfinder msg: 1613176517.32 epoch sec; seqNo:6754 2021-02-13T00:35:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.37 , 89.19 , 97.85 , 67.03 2021-02-13T00:35:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , 0.03 2021-02-13T00:35:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176517.34 epoch sec; seqNo:13961; 4059541.55 north; 592798.89 east 2021-02-13T00:35:17Z- AHRS_M2 msg: 1613176517.70 epoch sec; seqNo:13850 2021-02-13T00:35:17Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-13T00:35:17Z- handleDepth()-Depth_Keller msg: 1613176517709 epoch sec; seqNo:13962; depth 23.68 2021-02-13T00:35:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176517.72 epoch sec; seqNo:13962; 4059541.55 north; 592798.89 east 2021-02-13T00:35:18Z- AHRS_M2 msg: 1613176518.10 epoch sec; seqNo:13851 2021-02-13T00:35:18Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-13T00:35:18Z- handleDepth()-Depth_Keller msg: 1613176518111 epoch sec; seqNo:13963; depth 23.58 2021-02-13T00:35:18Z- handleDvl() - RDI_Pathfinder msg: 1613176518.12 epoch sec; seqNo:6755 2021-02-13T00:35:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 0.00 , 98.39 , 67.43 2021-02-13T00:35:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , -0.01 2021-02-13T00:35:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176518.14 epoch sec; seqNo:13963; 4059541.93 north; 592798.88 east 2021-02-13T00:35:18Z- LcmTrn::handleMessages() - handling 42 messages took 0.64 ms and 130 calls... 2021-02-13T00:35:18Z- waiting for 1613176516.26, time is 1613176518.14 2021-02-13T00:35:18Z- LcmTrn::updateTrn() equating pose 1613176518.14 and meas times 1613176518.14 2021-02-13T00:35:18Z- LcmTrn::updateTrn() >>>> pose time 1613176518.14, meas time 1613176518.14 <<<< 2021-02-13T00:35:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176518.14 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:35:18Z- TerrainNav::motionUpdate() - currEstimate = (4059542, 592798.88, 23.58) 2021-02-13T00:35:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:35:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:35:18Z- TNavBF::Projecting Measurements 2021-02-13T00:35:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:35:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:35:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:35:18Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:35:18Z- TNavFilter::3 good beams. 2021-02-13T00:35:18Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:35:18Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:35:18Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:35:18Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:35:18Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:35:18Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:35:18Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:35:18Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:18Z- TerrainMapOctree: (vehN, vehE ) = (4059541.93, 592798.88). 2021-02-13T00:35:18Z- TerrainMapOctree: Min Distance = 2241.26. 2021-02-13T00:35:18Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:35:18Z- There is no specific good beam that all particles have in common. 2021-02-13T00:35:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:35:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 9.96 2021-02-13T00:35:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:35:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.49 2021-02-13T00:35:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:35:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 7.28 2021-02-13T00:35:18Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:35:18Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:35:18Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:35:18Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:35:18Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:35:18Z- Filter 0 No beams used in subcloud update 2021-02-13T00:35:18Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:35:18Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:35:18Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 696.2 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:35:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176518.14 sec. 2021-02-13T00:35:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:35:18Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:18Z- variances([0], [2], and psi_berg) = 696.210251 832.995991 0.000000 2021-02-13T00:35:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:35:18Z- LcmTrn::publishEstimates() - MLE : 4059541.43 592772.38 23.58 2021-02-13T00:35:18Z- LcmTrn::publishEstimates() - MMSE : 4059541.93 592798.88 23.58 2021-02-13T00:35:18Z- LcmTrn::publishEstimates() - COVAR: 696.21 833.00 0.86 0.00 2021-02-13T00:35:18Z- AHRS_M2 msg: 1613176518.51 epoch sec; seqNo:13852 2021-02-13T00:35:18Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-13T00:35:18Z- handleDepth()-Depth_Keller msg: 1613176518517 epoch sec; seqNo:13964; depth 23.45 2021-02-13T00:35:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176518.53 epoch sec; seqNo:13964; 4059542.31 north; 592798.88 east 2021-02-13T00:35:18Z- AHRS_M2 msg: 1613176518.91 epoch sec; seqNo:13853 2021-02-13T00:35:18Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.19 psi 2021-02-13T00:35:18Z- handleDepth()-Depth_Keller msg: 1613176518921 epoch sec; seqNo:13965; depth 23.36 2021-02-13T00:35:18Z- handleDvl() - RDI_Pathfinder msg: 1613176518.93 epoch sec; seqNo:6756 2021-02-13T00:35:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.93 , 97.85 , 101.09 , 0.00 2021-02-13T00:35:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , -0.01 2021-02-13T00:35:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176518.94 epoch sec; seqNo:13965; 4059542.31 north; 592798.88 east 2021-02-13T00:35:19Z- AHRS_M2 msg: 1613176519.32 epoch sec; seqNo:13854 2021-02-13T00:35:19Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-13T00:35:19Z- handleDepth()-Depth_Keller msg: 1613176519328 epoch sec; seqNo:13966; depth 23.25 2021-02-13T00:35:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176519.35 epoch sec; seqNo:13966; 4059542.69 north; 592798.87 east 2021-02-13T00:35:19Z- AHRS_M2 msg: 1613176519.72 epoch sec; seqNo:13855 2021-02-13T00:35:19Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-13T00:35:19Z- handleDepth()-Depth_Keller msg: 1613176519724 epoch sec; seqNo:13967; depth 23.12 2021-02-13T00:35:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176519.74 epoch sec; seqNo:13967; 4059543.08 north; 592800.09 east 2021-02-13T00:35:20Z- AHRS_M2 msg: 1613176520.13 epoch sec; seqNo:13856 2021-02-13T00:35:20Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-13T00:35:20Z- handleDepth()-Depth_Keller msg: 1613176520133 epoch sec; seqNo:13968; depth 23.03 2021-02-13T00:35:20Z- handleDvl() - RDI_Pathfinder msg: 1613176520.15 epoch sec; seqNo:6757 2021-02-13T00:35:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 91.62 , 100.14 , 67.84 2021-02-13T00:35:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , -0.00 2021-02-13T00:35:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176520.17 epoch sec; seqNo:13968; 4059543.46 north; 592800.09 east 2021-02-13T00:35:20Z- AHRS_M2 msg: 1613176520.53 epoch sec; seqNo:13857 2021-02-13T00:35:20Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-13T00:35:20Z- handleDepth()-Depth_Keller msg: 1613176520533 epoch sec; seqNo:13969; depth 22.91 2021-02-13T00:35:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176520.55 epoch sec; seqNo:13969; 4059543.46 north; 592800.09 east 2021-02-13T00:35:20Z- AHRS_M2 msg: 1613176520.93 epoch sec; seqNo:13858 2021-02-13T00:35:20Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.19 psi 2021-02-13T00:35:20Z- handleDepth()-Depth_Keller msg: 1613176520949 epoch sec; seqNo:13970; depth 22.80 2021-02-13T00:35:20Z- handleDvl() - RDI_Pathfinder msg: 1613176520.96 epoch sec; seqNo:6758 2021-02-13T00:35:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 98.05 , 100.48 , 67.03 2021-02-13T00:35:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.00 2021-02-13T00:35:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176520.97 epoch sec; seqNo:13970; 4059543.84 north; 592800.08 east 2021-02-13T00:35:21Z- AHRS_M2 msg: 1613176521.34 epoch sec; seqNo:13859 2021-02-13T00:35:21Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.19 psi 2021-02-13T00:35:21Z- handleDepth()-Depth_Keller msg: 1613176521345 epoch sec; seqNo:13971; depth 22.69 2021-02-13T00:35:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176521.36 epoch sec; seqNo:13971; 4059544.22 north; 592800.08 east 2021-02-13T00:35:21Z- AHRS_M2 msg: 1613176521.74 epoch sec; seqNo:13860 2021-02-13T00:35:21Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.19 psi 2021-02-13T00:35:21Z- handleDepth()-Depth_Keller msg: 1613176521746 epoch sec; seqNo:13972; depth 22.59 2021-02-13T00:35:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176521.75 epoch sec; seqNo:13972; 4059544.60 north; 592800.07 east 2021-02-13T00:35:22Z- AHRS_M2 msg: 1613176522.15 epoch sec; seqNo:13861 2021-02-13T00:35:22Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.19 psi 2021-02-13T00:35:22Z- handleDepth()-Depth_Keller msg: 1613176522153 epoch sec; seqNo:13973; depth 22.46 2021-02-13T00:35:22Z- handleDvl() - RDI_Pathfinder msg: 1613176522.17 epoch sec; seqNo:6759 2021-02-13T00:35:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.78 , 93.38 , 99.47 , 67.43 2021-02-13T00:35:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , -0.01 2021-02-13T00:35:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176522.18 epoch sec; seqNo:13973; 4059544.60 north; 592800.07 east 2021-02-13T00:35:22Z- AHRS_M2 msg: 1613176522.55 epoch sec; seqNo:13862 2021-02-13T00:35:22Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.19 psi 2021-02-13T00:35:22Z- handleDepth()-Depth_Keller msg: 1613176522553 epoch sec; seqNo:13974; depth 22.36 2021-02-13T00:35:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176522.57 epoch sec; seqNo:13974; 4059544.99 north; 592801.29 east 2021-02-13T00:35:22Z- AHRS_M2 msg: 1613176522.95 epoch sec; seqNo:13863 2021-02-13T00:35:22Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.19 psi 2021-02-13T00:35:22Z- handleDepth()-Depth_Keller msg: 1613176522958 epoch sec; seqNo:13975; depth 22.25 2021-02-13T00:35:22Z- handleDvl() - RDI_Pathfinder msg: 1613176522.97 epoch sec; seqNo:6760 2021-02-13T00:35:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.97 , 93.32 , 100.62 , 67.03 2021-02-13T00:35:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , -0.04 2021-02-13T00:35:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176522.98 epoch sec; seqNo:13975; 4059545.37 north; 592801.29 east 2021-02-13T00:35:23Z- AHRS_M2 msg: 1613176523.36 epoch sec; seqNo:13864 2021-02-13T00:35:23Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.18 psi 2021-02-13T00:35:23Z- handleDepth()-Depth_Keller msg: 1613176523360 epoch sec; seqNo:13976; depth 22.13 2021-02-13T00:35:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176523.38 epoch sec; seqNo:13976; 4059545.75 north; 592801.28 east 2021-02-13T00:35:23Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 131 calls... 2021-02-13T00:35:23Z- waiting for 1613176521.14, time is 1613176523.38 2021-02-13T00:35:23Z- LcmTrn::updateTrn() >>>> pose time 1613176523.38, meas time 1613176522.97 <<<< 2021-02-13T00:35:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176522.97 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:35:23Z- TerrainNav::motionUpdate() - currEstimate = (4059546, 592801.28, 22.13) 2021-02-13T00:35:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:35:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:35:23Z- TNavBF::Projecting Measurements 2021-02-13T00:35:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:35:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:35:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:35:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:35:23Z- TNavFilter::4 good beams. 2021-02-13T00:35:23Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:35:23Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:35:23Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:35:23Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:35:23Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:35:23Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:35:23Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:35:23Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:23Z- TerrainMapOctree: (vehN, vehE ) = (4059545.45, 592801.10). 2021-02-13T00:35:23Z- TerrainMapOctree: Min Distance = 2237.14. 2021-02-13T00:35:23Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:35:23Z- There is no specific good beam that all particles have in common. 2021-02-13T00:35:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:35:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 8.76 2021-02-13T00:35:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:35:23Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 13.93 2021-02-13T00:35:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:35:23Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 16.20 2021-02-13T00:35:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:35:23Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 7.19 2021-02-13T00:35:23Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:35:23Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:35:23Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:35:23Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:35:23Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:35:23Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:35:23Z- Filter 0 No beams used in subcloud update 2021-02-13T00:35:23Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:35:23Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:35:23Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 696.2 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:35:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176522.97 sec. 2021-02-13T00:35:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:35:23Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:23Z- variances([0], [2], and psi_berg) = 696.141859 832.995956 0.000000 2021-02-13T00:35:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:35:23Z- LcmTrn::publishEstimates() - MLE : 4059545.25 592774.78 22.13 2021-02-13T00:35:23Z- LcmTrn::publishEstimates() - MMSE : 4059545.75 592801.28 22.13 2021-02-13T00:35:23Z- LcmTrn::publishEstimates() - COVAR: 696.14 833.00 0.81 0.00 2021-02-13T00:35:23Z- AHRS_M2 msg: 1613176523.76 epoch sec; seqNo:13865 2021-02-13T00:35:23Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.18 psi 2021-02-13T00:35:23Z- handleDepth()-Depth_Keller msg: 1613176523769 epoch sec; seqNo:13977; depth 22.02 2021-02-13T00:35:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176523.78 epoch sec; seqNo:13977; 4059545.75 north; 592801.28 east 2021-02-13T00:35:24Z- AHRS_M2 msg: 1613176524.16 epoch sec; seqNo:13866 2021-02-13T00:35:24Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.18 psi 2021-02-13T00:35:24Z- handleDepth()-Depth_Keller msg: 1613176524169 epoch sec; seqNo:13978; depth 21.92 2021-02-13T00:35:24Z- handleDvl() - RDI_Pathfinder msg: 1613176524.18 epoch sec; seqNo:6761 2021-02-13T00:35:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 98.32 , 100.75 , 67.43 2021-02-13T00:35:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , -0.05 2021-02-13T00:35:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176524.19 epoch sec; seqNo:13978; 4059546.13 north; 592801.28 east 2021-02-13T00:35:24Z- AHRS_M2 msg: 1613176524.57 epoch sec; seqNo:13867 2021-02-13T00:35:24Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-13T00:35:24Z- handleDepth()-Depth_Keller msg: 1613176524574 epoch sec; seqNo:13979; depth 21.79 2021-02-13T00:35:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176524.59 epoch sec; seqNo:13979; 4059546.51 north; 592801.27 east 2021-02-13T00:35:24Z- AHRS_M2 msg: 1613176524.97 epoch sec; seqNo:13868 2021-02-13T00:35:24Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-13T00:35:24Z- handleDepth()-Depth_Keller msg: 1613176524988 epoch sec; seqNo:13980; depth 21.70 2021-02-13T00:35:25Z- handleDvl() - RDI_Pathfinder msg: 1613176525.00 epoch sec; seqNo:6762 2021-02-13T00:35:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 98.05 , 100.49 , 0.00 2021-02-13T00:35:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , -0.03 2021-02-13T00:35:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176525.01 epoch sec; seqNo:13980; 4059546.88 north; 592801.27 east 2021-02-13T00:35:25Z- AHRS_M2 msg: 1613176525.38 epoch sec; seqNo:13869 2021-02-13T00:35:25Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-13T00:35:25Z- handleDepth()-Depth_Keller msg: 1613176525380 epoch sec; seqNo:13981; depth 21.57 2021-02-13T00:35:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176525.39 epoch sec; seqNo:13981; 4059546.90 north; 592802.49 east 2021-02-13T00:35:25Z- AHRS_M2 msg: 1613176525.78 epoch sec; seqNo:13870 2021-02-13T00:35:25Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-13T00:35:25Z- handleDepth()-Depth_Keller msg: 1613176525784 epoch sec; seqNo:13982; depth 21.47 2021-02-13T00:35:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176525.79 epoch sec; seqNo:13982; 4059547.28 north; 592802.49 east 2021-02-13T00:35:26Z- AHRS_M2 msg: 1613176526.18 epoch sec; seqNo:13871 2021-02-13T00:35:26Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:35:26Z- handleDepth()-Depth_Keller msg: 1613176526193 epoch sec; seqNo:13983; depth 21.35 2021-02-13T00:35:26Z- handleDvl() - RDI_Pathfinder msg: 1613176526.21 epoch sec; seqNo:6763 2021-02-13T00:35:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.25 , 91.63 , 102.58 , 69.06 2021-02-13T00:35:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , 0.00 2021-02-13T00:35:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176526.22 epoch sec; seqNo:13983; 4059547.66 north; 592802.48 east 2021-02-13T00:35:26Z- AHRS_M2 msg: 1613176526.59 epoch sec; seqNo:13872 2021-02-13T00:35:26Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:35:26Z- handleDepth()-Depth_Keller msg: 1613176526592 epoch sec; seqNo:13984; depth 21.24 2021-02-13T00:35:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176526.60 epoch sec; seqNo:13984; 4059548.03 north; 592802.48 east 2021-02-13T00:35:26Z- AHRS_M2 msg: 1613176526.99 epoch sec; seqNo:13873 2021-02-13T00:35:26Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:35:26Z- handleDepth()-Depth_Keller msg: 1613176526996 epoch sec; seqNo:13985; depth 21.13 2021-02-13T00:35:27Z- handleDvl() - RDI_Pathfinder msg: 1613176527.01 epoch sec; seqNo:6764 2021-02-13T00:35:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.79 , 90.88 , 101.83 , 67.84 2021-02-13T00:35:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , 0.05 2021-02-13T00:35:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176527.02 epoch sec; seqNo:13985; 4059548.03 north; 592802.48 east 2021-02-13T00:35:27Z- AHRS_M2 msg: 1613176527.39 epoch sec; seqNo:13874 2021-02-13T00:35:27Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T00:35:27Z- handleDepth()-Depth_Keller msg: 1613176527403 epoch sec; seqNo:13986; depth 21.01 2021-02-13T00:35:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176527.42 epoch sec; seqNo:13986; 4059548.41 north; 592802.47 east 2021-02-13T00:35:27Z- AHRS_M2 msg: 1613176527.80 epoch sec; seqNo:13875 2021-02-13T00:35:27Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T00:35:27Z- handleDepth()-Depth_Keller msg: 1613176527813 epoch sec; seqNo:13987; depth 20.90 2021-02-13T00:35:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176527.86 epoch sec; seqNo:13987; 4059548.79 north; 592802.47 east 2021-02-13T00:35:28Z- AHRS_M2 msg: 1613176528.20 epoch sec; seqNo:13876 2021-02-13T00:35:28Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T00:35:28Z- handleDepth()-Depth_Keller msg: 1613176528209 epoch sec; seqNo:13988; depth 20.80 2021-02-13T00:35:28Z- handleDvl() - RDI_Pathfinder msg: 1613176528.22 epoch sec; seqNo:6765 2021-02-13T00:35:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.79 , 89.80 , 101.97 , 67.43 2021-02-13T00:35:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.06 2021-02-13T00:35:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176528.23 epoch sec; seqNo:13988; 4059549.18 north; 592803.69 east 2021-02-13T00:35:28Z- LcmTrn::handleMessages() - handling 41 messages took 0.69 ms and 127 calls... 2021-02-13T00:35:28Z- waiting for 1613176526.38, time is 1613176528.23 2021-02-13T00:35:28Z- LcmTrn::updateTrn() equating pose 1613176528.23 and meas times 1613176528.23 2021-02-13T00:35:28Z- LcmTrn::updateTrn() >>>> pose time 1613176528.23, meas time 1613176528.23 <<<< 2021-02-13T00:35:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176528.23 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:35:28Z- TerrainNav::motionUpdate() - currEstimate = (4059549, 592803.69, 20.80) 2021-02-13T00:35:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:35:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:35:28Z- TNavBF::Projecting Measurements 2021-02-13T00:35:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:35:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:35:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:35:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:35:28Z- TNavFilter::4 good beams. 2021-02-13T00:35:28Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:35:28Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:35:28Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:35:28Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:35:28Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:35:28Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:35:28Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:35:28Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:28Z- TerrainMapOctree: (vehN, vehE ) = (4059549.18, 592803.69). 2021-02-13T00:35:28Z- TerrainMapOctree: Min Distance = 2232.62. 2021-02-13T00:35:28Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:35:28Z- There is no specific good beam that all particles have in common. 2021-02-13T00:35:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:35:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 8.95 2021-02-13T00:35:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:35:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 12.90 2021-02-13T00:35:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:35:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 16.64 2021-02-13T00:35:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:35:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 7.28 2021-02-13T00:35:28Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:35:28Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:35:28Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:35:28Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:35:28Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:35:28Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:35:28Z- Filter 0 No beams used in subcloud update 2021-02-13T00:35:28Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:35:28Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:35:28Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 696.1 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:35:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176528.23 sec. 2021-02-13T00:35:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:35:28Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:28Z- variances([0], [2], and psi_berg) = 696.088668 832.995929 0.000000 2021-02-13T00:35:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:35:28Z- LcmTrn::publishEstimates() - MLE : 4059548.68 592777.19 20.80 2021-02-13T00:35:28Z- LcmTrn::publishEstimates() - MMSE : 4059549.18 592803.69 20.80 2021-02-13T00:35:28Z- LcmTrn::publishEstimates() - COVAR: 696.09 833.00 0.77 0.00 2021-02-13T00:35:28Z- AHRS_M2 msg: 1613176528.61 epoch sec; seqNo:13877 2021-02-13T00:35:28Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T00:35:28Z- handleDepth()-Depth_Keller msg: 1613176528614 epoch sec; seqNo:13989; depth 20.68 2021-02-13T00:35:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176528.62 epoch sec; seqNo:13989; 4059549.18 north; 592803.69 east 2021-02-13T00:35:29Z- AHRS_M2 msg: 1613176529.01 epoch sec; seqNo:13878 2021-02-13T00:35:29Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:35:29Z- handleDepth()-Depth_Keller msg: 1613176529018 epoch sec; seqNo:13990; depth 20.57 2021-02-13T00:35:29Z- handleDvl() - RDI_Pathfinder msg: 1613176529.04 epoch sec; seqNo:6766 2021-02-13T00:35:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:35:29Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:35:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176529.05 epoch sec; seqNo:13990; 4059549.56 north; 592803.68 east 2021-02-13T00:35:29Z- AHRS_M2 msg: 1613176529.42 epoch sec; seqNo:13879 2021-02-13T00:35:29Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:35:29Z- handleDepth()-Depth_Keller msg: 1613176529421 epoch sec; seqNo:13991; depth 20.46 2021-02-13T00:35:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:35:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:35:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:35:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:35:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:35:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:35:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:35:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:35:29Z- handleDvl() - RDI_Pathfinder msg: 1613176529.42 epoch sec; seqNo:6767 2021-02-13T00:35:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:35:29Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:35:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176529.44 epoch sec; seqNo:13991; 4059549.94 north; 592803.68 east 2021-02-13T00:35:29Z- AHRS_M2 msg: 1613176529.82 epoch sec; seqNo:13880 2021-02-13T00:35:29Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.16 psi 2021-02-13T00:35:29Z- handleDepth()-Depth_Keller msg: 1613176529831 epoch sec; seqNo:13992; depth 20.36 2021-02-13T00:35:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176529.84 epoch sec; seqNo:13992; 4059550.32 north; 592803.67 east 2021-02-13T00:35:30Z- AHRS_M2 msg: 1613176530.23 epoch sec; seqNo:13881 2021-02-13T00:35:30Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-13T00:35:30Z- handleDepth()-Depth_Keller msg: 1613176530234 epoch sec; seqNo:13993; depth 20.25 2021-02-13T00:35:30Z- handleDvl() - RDI_Pathfinder msg: 1613176530.25 epoch sec; seqNo:6768 2021-02-13T00:35:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.90 , 97.24 , 101.70 , 68.63 2021-02-13T00:35:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.00 2021-02-13T00:35:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176530.26 epoch sec; seqNo:13993; 4059550.70 north; 592803.67 east 2021-02-13T00:35:30Z- AHRS_M2 msg: 1613176530.63 epoch sec; seqNo:13882 2021-02-13T00:35:30Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.17 psi 2021-02-13T00:35:30Z- handleDepth()-Depth_Keller msg: 1613176530639 epoch sec; seqNo:13994; depth 20.15 2021-02-13T00:35:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:35:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:35:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:35:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:35:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:35:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:35:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:35:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:35:30Z- handleDvl() - RDI_Pathfinder msg: 1613176530.64 epoch sec; seqNo:6769 2021-02-13T00:35:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:35:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.00 2021-02-13T00:35:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176530.65 epoch sec; seqNo:13994; 4059550.71 north; 592804.89 east 2021-02-13T00:35:31Z- AHRS_M2 msg: 1613176531.03 epoch sec; seqNo:13883 2021-02-13T00:35:31Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.17 psi 2021-02-13T00:35:31Z- handleDepth()-Depth_Keller msg: 1613176531037 epoch sec; seqNo:13995; depth 20.05 2021-02-13T00:35:31Z- handleDvl() - RDI_Pathfinder msg: 1613176531.05 epoch sec; seqNo:6770 2021-02-13T00:35:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 100.48 , 70.29 2021-02-13T00:35:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.04 2021-02-13T00:35:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176531.06 epoch sec; seqNo:13995; 4059551.09 north; 592804.89 east 2021-02-13T00:35:31Z- AHRS_M2 msg: 1613176531.44 epoch sec; seqNo:13884 2021-02-13T00:35:31Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.17 psi 2021-02-13T00:35:31Z- handleDepth()-Depth_Keller msg: 1613176531441 epoch sec; seqNo:13996; depth 19.94 2021-02-13T00:35:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176531.46 epoch sec; seqNo:13996; 4059551.47 north; 592804.88 east 2021-02-13T00:35:31Z- AHRS_M2 msg: 1613176531.84 epoch sec; seqNo:13885 2021-02-13T00:35:31Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.18 psi 2021-02-13T00:35:31Z- handleDepth()-Depth_Keller msg: 1613176531844 epoch sec; seqNo:13997; depth 19.84 2021-02-13T00:35:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:35:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:35:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:35:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:35:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:35:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:35:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:35:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:35:31Z- handleDvl() - RDI_Pathfinder msg: 1613176531.85 epoch sec; seqNo:6771 2021-02-13T00:35:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:35:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.04 2021-02-13T00:35:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176531.86 epoch sec; seqNo:13997; 4059551.47 north; 592804.88 east 2021-02-13T00:35:32Z- AHRS_M2 msg: 1613176532.24 epoch sec; seqNo:13886 2021-02-13T00:35:32Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.18 psi 2021-02-13T00:35:32Z- handleDepth()-Depth_Keller msg: 1613176532248 epoch sec; seqNo:13998; depth 19.75 2021-02-13T00:35:32Z- handleDvl() - RDI_Pathfinder msg: 1613176532.26 epoch sec; seqNo:6772 2021-02-13T00:35:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 93.18 , 96.83 , 70.70 2021-02-13T00:35:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , 0.00 2021-02-13T00:35:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176532.27 epoch sec; seqNo:13998; 4059551.85 north; 592804.88 east 2021-02-13T00:35:32Z- AHRS_M2 msg: 1613176532.65 epoch sec; seqNo:13887 2021-02-13T00:35:32Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.18 psi 2021-02-13T00:35:32Z- handleDepth()-Depth_Keller msg: 1613176532654 epoch sec; seqNo:13999; depth 19.64 2021-02-13T00:35:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176532.66 epoch sec; seqNo:13999; 4059552.23 north; 592804.87 east 2021-02-13T00:35:33Z- AHRS_M2 msg: 1613176533.05 epoch sec; seqNo:13888 2021-02-13T00:35:33Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.18 psi 2021-02-13T00:35:33Z- handleDepth()-Depth_Keller msg: 1613176533056 epoch sec; seqNo:14000; depth 19.55 2021-02-13T00:35:33Z- handleDvl() - RDI_Pathfinder msg: 1613176533.08 epoch sec; seqNo:6773 2021-02-13T00:35:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 98.18 , 101.83 , 0.00 2021-02-13T00:35:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , 0.04 2021-02-13T00:35:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176533.09 epoch sec; seqNo:14000; 4059552.61 north; 592804.87 east 2021-02-13T00:35:33Z- AHRS_M2 msg: 1613176533.46 epoch sec; seqNo:13889 2021-02-13T00:35:33Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.19 psi 2021-02-13T00:35:33Z- handleDepth()-Depth_Keller msg: 1613176533465 epoch sec; seqNo:14001; depth 19.45 2021-02-13T00:35:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176533.49 epoch sec; seqNo:14001; 4059552.62 north; 592806.09 east 2021-02-13T00:35:33Z- LcmTrn::handleMessages() - handling 47 messages took 0.67 ms and 134 calls... 2021-02-13T00:35:33Z- waiting for 1613176531.23, time is 1613176533.49 2021-02-13T00:35:33Z- LcmTrn::updateTrn() >>>> pose time 1613176533.49, meas time 1613176533.08 <<<< 2021-02-13T00:35:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176533.08 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:35:33Z- TerrainNav::motionUpdate() - currEstimate = (4059553, 592806.09, 19.45) 2021-02-13T00:35:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:35:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:35:33Z- TNavBF::Projecting Measurements 2021-02-13T00:35:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:35:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:35:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:35:33Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:35:33Z- TNavFilter::3 good beams. 2021-02-13T00:35:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:35:33Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:35:33Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:35:33Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:35:33Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:35:33Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:35:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:35:33Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:33Z- TerrainMapOctree: (vehN, vehE ) = (4059552.35, 592805.90). 2021-02-13T00:35:33Z- TerrainMapOctree: Min Distance = 2228.77. 2021-02-13T00:35:33Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:35:33Z- There is no specific good beam that all particles have in common. 2021-02-13T00:35:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:35:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 9.15 2021-02-13T00:35:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:35:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 15.42 2021-02-13T00:35:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:35:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 16.59 2021-02-13T00:35:33Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:35:33Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:35:33Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:35:33Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:35:33Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:35:33Z- Filter 0 No beams used in subcloud update 2021-02-13T00:35:33Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:35:33Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:35:33Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 696.0 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:35:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176533.08 sec. 2021-02-13T00:35:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:35:33Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:33Z- variances([0], [2], and psi_berg) = 696.022736 832.995896 0.000000 2021-02-13T00:35:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:35:33Z- LcmTrn::publishEstimates() - MLE : 4059552.12 592779.59 19.45 2021-02-13T00:35:33Z- LcmTrn::publishEstimates() - MMSE : 4059552.62 592806.09 19.45 2021-02-13T00:35:33Z- LcmTrn::publishEstimates() - COVAR: 696.02 833.00 0.82 0.00 2021-02-13T00:35:33Z- AHRS_M2 msg: 1613176533.88 epoch sec; seqNo:13890 2021-02-13T00:35:33Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.19 psi 2021-02-13T00:35:33Z- handleDepth()-Depth_Keller msg: 1613176533886 epoch sec; seqNo:14002; depth 19.36 2021-02-13T00:35:33Z- handleDvl() - RDI_Pathfinder msg: 1613176533.91 epoch sec; seqNo:6774 2021-02-13T00:35:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.71 , 101.77 , 71.93 2021-02-13T00:35:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.08 2021-02-13T00:35:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176533.92 epoch sec; seqNo:14002; 4059553.00 north; 592806.09 east 2021-02-13T00:35:34Z- AHRS_M2 msg: 1613176534.26 epoch sec; seqNo:13891 2021-02-13T00:35:34Z- AHRS_M2 msg: 0.58 phi; 0.30 theta; -0.19 psi 2021-02-13T00:35:34Z- handleDepth()-Depth_Keller msg: 1613176534270 epoch sec; seqNo:14003; depth 19.23 2021-02-13T00:35:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:35:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:35:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:35:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:35:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:35:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:35:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:35:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:35:34Z- handleDvl() - RDI_Pathfinder msg: 1613176534.27 epoch sec; seqNo:6775 2021-02-13T00:35:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:35:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.08 2021-02-13T00:35:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176534.28 epoch sec; seqNo:14003; 4059553.38 north; 592806.08 east 2021-02-13T00:35:34Z- AHRS_M2 msg: 1613176534.67 epoch sec; seqNo:13892 2021-02-13T00:35:34Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.19 psi 2021-02-13T00:35:34Z- handleDepth()-Depth_Keller msg: 1613176534672 epoch sec; seqNo:14004; depth 19.14 2021-02-13T00:35:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176534.70 epoch sec; seqNo:14004; 4059553.38 north; 592806.08 east 2021-02-13T00:35:35Z- AHRS_M2 msg: 1613176535.07 epoch sec; seqNo:13893 2021-02-13T00:35:35Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.19 psi 2021-02-13T00:35:35Z- handleDepth()-Depth_Keller msg: 1613176535077 epoch sec; seqNo:14005; depth 19.05 2021-02-13T00:35:35Z- handleDvl() - RDI_Pathfinder msg: 1613176535.09 epoch sec; seqNo:6776 2021-02-13T00:35:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.20 , 97.71 , 101.36 , 78.24 2021-02-13T00:35:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.08 , 0.06 2021-02-13T00:35:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176535.11 epoch sec; seqNo:14005; 4059553.76 north; 592806.08 east 2021-02-13T00:35:35Z- AHRS_M2 msg: 1613176535.48 epoch sec; seqNo:13894 2021-02-13T00:35:35Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.19 psi 2021-02-13T00:35:35Z- handleDepth()-Depth_Keller msg: 1613176535483 epoch sec; seqNo:14006; depth 18.93 2021-02-13T00:35:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:35:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:35:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:35:35Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:35:35Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:35:35Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:35:35Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:35:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:35:35Z- handleDvl() - RDI_Pathfinder msg: 1613176535.48 epoch sec; seqNo:6777 2021-02-13T00:35:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:35:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.08 , 0.06 2021-02-13T00:35:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176535.50 epoch sec; seqNo:14006; 4059554.14 north; 592806.07 east 2021-02-13T00:35:35Z- AHRS_M2 msg: 1613176535.89 epoch sec; seqNo:13895 2021-02-13T00:35:35Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.19 psi 2021-02-13T00:35:35Z- handleDepth()-Depth_Keller msg: 1613176535901 epoch sec; seqNo:14007; depth 18.84 2021-02-13T00:35:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176535.92 epoch sec; seqNo:14007; 4059554.53 north; 592807.29 east 2021-02-13T00:35:36Z- AHRS_M2 msg: 1613176536.29 epoch sec; seqNo:13896 2021-02-13T00:35:36Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.19 psi 2021-02-13T00:35:36Z- handleDepth()-Depth_Keller msg: 1613176536302 epoch sec; seqNo:14008; depth 18.74 2021-02-13T00:35:36Z- handleDvl() - RDI_Pathfinder msg: 1613176536.31 epoch sec; seqNo:6778 2021-02-13T00:35:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 97.91 , 102.78 , 70.29 2021-02-13T00:35:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.10 , -0.01 2021-02-13T00:35:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176536.32 epoch sec; seqNo:14008; 4059554.53 north; 592807.29 east 2021-02-13T00:35:36Z- AHRS_M2 msg: 1613176536.69 epoch sec; seqNo:13897 2021-02-13T00:35:36Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.18 psi 2021-02-13T00:35:36Z- handleDepth()-Depth_Keller msg: 1613176536692 epoch sec; seqNo:14009; depth 18.64 2021-02-13T00:35:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:35:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:35:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:35:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:35:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:35:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:35:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:35:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:35:36Z- handleDvl() - RDI_Pathfinder msg: 1613176536.70 epoch sec; seqNo:6779 2021-02-13T00:35:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:35:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.10 , -0.01 2021-02-13T00:35:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176536.71 epoch sec; seqNo:14009; 4059554.91 north; 592807.29 east 2021-02-13T00:35:37Z- AHRS_M2 msg: 1613176537.09 epoch sec; seqNo:13898 2021-02-13T00:35:37Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.18 psi 2021-02-13T00:35:37Z- handleDepth()-Depth_Keller msg: 1613176537106 epoch sec; seqNo:14010; depth 18.55 2021-02-13T00:35:37Z- handleDvl() - RDI_Pathfinder msg: 1613176537.14 epoch sec; seqNo:6780 2021-02-13T00:35:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 95.61 , 104.13 , 70.70 2021-02-13T00:35:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.06 , -0.09 2021-02-13T00:35:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176537.17 epoch sec; seqNo:14010; 4059555.29 north; 592807.28 east 2021-02-13T00:35:37Z- AHRS_M2 msg: 1613176537.50 epoch sec; seqNo:13899 2021-02-13T00:35:37Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.18 psi 2021-02-13T00:35:37Z- handleDepth()-Depth_Keller msg: 1613176537503 epoch sec; seqNo:14011; depth 18.43 2021-02-13T00:35:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176537.51 epoch sec; seqNo:14011; 4059555.29 north; 592807.28 east 2021-02-13T00:35:37Z- AHRS_M2 msg: 1613176537.90 epoch sec; seqNo:13900 2021-02-13T00:35:37Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.18 psi 2021-02-13T00:35:37Z- handleDepth()-Depth_Keller msg: 1613176537904 epoch sec; seqNo:14012; depth 18.35 2021-02-13T00:35:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:35:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:35:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:35:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:35:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:35:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:35:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:35:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:35:37Z- handleDvl() - RDI_Pathfinder msg: 1613176537.91 epoch sec; seqNo:6781 2021-02-13T00:35:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:35:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.06 , -0.09 2021-02-13T00:35:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176537.91 epoch sec; seqNo:14012; 4059555.66 north; 592807.28 east 2021-02-13T00:35:38Z- AHRS_M2 msg: 1613176538.30 epoch sec; seqNo:13901 2021-02-13T00:35:38Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.18 psi 2021-02-13T00:35:38Z- handleDepth()-Depth_Keller msg: 1613176538309 epoch sec; seqNo:14013; depth 18.25 2021-02-13T00:35:38Z- handleDvl() - RDI_Pathfinder msg: 1613176538.33 epoch sec; seqNo:6782 2021-02-13T00:35:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 104.13 , 78.58 2021-02-13T00:35:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , -0.13 2021-02-13T00:35:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176538.34 epoch sec; seqNo:14013; 4059556.06 north; 592808.50 east 2021-02-13T00:35:38Z- AHRS_M2 msg: 1613176538.71 epoch sec; seqNo:13902 2021-02-13T00:35:38Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.18 psi 2021-02-13T00:35:38Z- handleDepth()-Depth_Keller msg: 1613176538716 epoch sec; seqNo:14014; depth 18.15 2021-02-13T00:35:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176538.73 epoch sec; seqNo:14014; 4059556.44 north; 592808.49 east 2021-02-13T00:35:38Z- LcmTrn::handleMessages() - handling 48 messages took 0.76 ms and 133 calls... 2021-02-13T00:35:38Z- waiting for 1613176536.49, time is 1613176538.73 2021-02-13T00:35:38Z- LcmTrn::updateTrn() >>>> pose time 1613176538.73, meas time 1613176538.33 <<<< 2021-02-13T00:35:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176538.33 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:35:38Z- TerrainNav::motionUpdate() - currEstimate = (4059556, 592808.49, 18.15) 2021-02-13T00:35:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:35:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:35:38Z- TNavBF::Projecting Measurements 2021-02-13T00:35:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:35:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:35:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:35:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:35:38Z- TNavFilter::4 good beams. 2021-02-13T00:35:38Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:35:38Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:35:38Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:35:38Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:35:38Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:35:38Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:35:38Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:35:38Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:38Z- TerrainMapOctree: (vehN, vehE ) = (4059556.14, 592808.31). 2021-02-13T00:35:38Z- TerrainMapOctree: Min Distance = 2224.33. 2021-02-13T00:35:38Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:35:38Z- There is no specific good beam that all particles have in common. 2021-02-13T00:35:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:35:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-13T00:35:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:35:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 15.38 2021-02-13T00:35:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:35:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 17.35 2021-02-13T00:35:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:35:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-13T00:35:38Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:35:38Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:35:38Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:35:38Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:35:38Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:35:38Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:35:38Z- Filter 0 No beams used in subcloud update 2021-02-13T00:35:38Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:35:38Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:35:38Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 696.0 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:35:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176538.33 sec. 2021-02-13T00:35:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:35:38Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:38Z- variances([0], [2], and psi_berg) = 695.954597 832.995862 0.000000 2021-02-13T00:35:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:35:38Z- LcmTrn::publishEstimates() - MLE : 4059555.94 592781.99 18.15 2021-02-13T00:35:38Z- LcmTrn::publishEstimates() - MMSE : 4059556.44 592808.49 18.15 2021-02-13T00:35:38Z- LcmTrn::publishEstimates() - COVAR: 695.95 833.00 0.82 0.00 2021-02-13T00:35:39Z- AHRS_M2 msg: 1613176539.11 epoch sec; seqNo:13903 2021-02-13T00:35:39Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-13T00:35:39Z- handleDepth()-Depth_Keller msg: 1613176539121 epoch sec; seqNo:14015; depth 18.07 2021-02-13T00:35:39Z- handleDvl() - RDI_Pathfinder msg: 1613176539.14 epoch sec; seqNo:6783 2021-02-13T00:35:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 97.78 , 105.08 , 71.11 2021-02-13T00:35:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , -0.10 2021-02-13T00:35:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176539.15 epoch sec; seqNo:14015; 4059556.44 north; 592808.49 east 2021-02-13T00:35:39Z- AHRS_M2 msg: 1613176539.52 epoch sec; seqNo:13904 2021-02-13T00:35:39Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-13T00:35:39Z- handleDepth()-Depth_Keller msg: 1613176539520 epoch sec; seqNo:14016; depth 17.96 2021-02-13T00:35:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176539.54 epoch sec; seqNo:14016; 4059556.81 north; 592808.49 east 2021-02-13T00:35:39Z- AHRS_M2 msg: 1613176539.92 epoch sec; seqNo:13905 2021-02-13T00:35:39Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-13T00:35:39Z- handleDepth()-Depth_Keller msg: 1613176539924 epoch sec; seqNo:14017; depth 17.87 2021-02-13T00:35:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176539.93 epoch sec; seqNo:14017; 4059557.19 north; 592808.48 east 2021-02-13T00:35:40Z- AHRS_M2 msg: 1613176540.32 epoch sec; seqNo:13906 2021-02-13T00:35:40Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T00:35:40Z- handleDepth()-Depth_Keller msg: 1613176540333 epoch sec; seqNo:14018; depth 17.77 2021-02-13T00:35:40Z- handleDvl() - RDI_Pathfinder msg: 1613176540.35 epoch sec; seqNo:6784 2021-02-13T00:35:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 94.33 , 105.28 , 78.51 2021-02-13T00:35:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.09 , -0.07 2021-02-13T00:35:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176540.36 epoch sec; seqNo:14018; 4059557.57 north; 592808.48 east 2021-02-13T00:35:40Z- AHRS_M2 msg: 1613176540.73 epoch sec; seqNo:13907 2021-02-13T00:35:40Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:35:40Z- handleDepth()-Depth_Keller msg: 1613176540738 epoch sec; seqNo:14019; depth 17.66 2021-02-13T00:35:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176540.75 epoch sec; seqNo:14019; 4059557.57 north; 592808.48 east 2021-02-13T00:35:41Z- AHRS_M2 msg: 1613176541.13 epoch sec; seqNo:13908 2021-02-13T00:35:41Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:35:41Z- handleDepth()-Depth_Keller msg: 1613176541140 epoch sec; seqNo:14020; depth 17.56 2021-02-13T00:35:41Z- handleDvl() - RDI_Pathfinder msg: 1613176541.16 epoch sec; seqNo:6785 2021-02-13T00:35:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.59 , 97.85 , 106.36 , 71.11 2021-02-13T00:35:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.10 , -0.05 2021-02-13T00:35:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176541.17 epoch sec; seqNo:14020; 4059557.95 north; 592808.48 east 2021-02-13T00:35:41Z- AHRS_M2 msg: 1613176541.54 epoch sec; seqNo:13909 2021-02-13T00:35:41Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:35:41Z- handleDepth()-Depth_Keller msg: 1613176541541 epoch sec; seqNo:14021; depth 17.44 2021-02-13T00:35:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176541.55 epoch sec; seqNo:14021; 4059558.34 north; 592809.69 east 2021-02-13T00:35:41Z- AHRS_M2 msg: 1613176541.94 epoch sec; seqNo:13910 2021-02-13T00:35:41Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:35:41Z- handleDepth()-Depth_Keller msg: 1613176541949 epoch sec; seqNo:14022; depth 17.34 2021-02-13T00:35:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176541.97 epoch sec; seqNo:14022; 4059558.72 north; 592809.69 east 2021-02-13T00:35:42Z- AHRS_M2 msg: 1613176542.34 epoch sec; seqNo:13911 2021-02-13T00:35:42Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:35:42Z- handleDepth()-Depth_Keller msg: 1613176542348 epoch sec; seqNo:14023; depth 17.24 2021-02-13T00:35:42Z- handleDvl() - RDI_Pathfinder msg: 1613176542.37 epoch sec; seqNo:6786 2021-02-13T00:35:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.29 , 97.85 , 106.36 , 78.37 2021-02-13T00:35:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.13 , -0.02 2021-02-13T00:35:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176542.38 epoch sec; seqNo:14023; 4059558.72 north; 592809.69 east 2021-02-13T00:35:42Z- AHRS_M2 msg: 1613176542.75 epoch sec; seqNo:13912 2021-02-13T00:35:42Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:35:42Z- handleDepth()-Depth_Keller msg: 1613176542755 epoch sec; seqNo:14024; depth 17.12 2021-02-13T00:35:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176542.77 epoch sec; seqNo:14024; 4059559.10 north; 592809.68 east 2021-02-13T00:35:43Z- AHRS_M2 msg: 1613176543.15 epoch sec; seqNo:13913 2021-02-13T00:35:43Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T00:35:43Z- handleDepth()-Depth_Keller msg: 1613176543159 epoch sec; seqNo:14025; depth 17.02 2021-02-13T00:35:43Z- handleDvl() - RDI_Pathfinder msg: 1613176543.17 epoch sec; seqNo:6787 2021-02-13T00:35:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.71 , 106.23 , 78.24 2021-02-13T00:35:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.17 , 0.03 2021-02-13T00:35:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176543.18 epoch sec; seqNo:14025; 4059559.48 north; 592809.68 east 2021-02-13T00:35:43Z- AHRS_M2 msg: 1613176543.56 epoch sec; seqNo:13914 2021-02-13T00:35:43Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.16 psi 2021-02-13T00:35:43Z- handleDepth()-Depth_Keller msg: 1613176543562 epoch sec; seqNo:14026; depth 16.91 2021-02-13T00:35:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176543.57 epoch sec; seqNo:14026; 4059559.86 north; 592809.68 east 2021-02-13T00:35:43Z- LcmTrn::handleMessages() - handling 41 messages took 0.66 ms and 128 calls... 2021-02-13T00:35:43Z- waiting for 1613176541.73, time is 1613176543.57 2021-02-13T00:35:43Z- LcmTrn::updateTrn() >>>> pose time 1613176543.57, meas time 1613176543.17 <<<< 2021-02-13T00:35:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176543.17 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:35:43Z- TerrainNav::motionUpdate() - currEstimate = (4059560, 592809.68, 16.91) 2021-02-13T00:35:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:35:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:35:43Z- TNavBF::Projecting Measurements 2021-02-13T00:35:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:35:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:35:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:35:43Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:35:43Z- TNavFilter::3 good beams. 2021-02-13T00:35:43Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:35:43Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:35:43Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:35:43Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:35:43Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:35:43Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:35:43Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:35:43Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:43Z- TerrainMapOctree: (vehN, vehE ) = (4059559.57, 592809.58). 2021-02-13T00:35:43Z- TerrainMapOctree: Min Distance = 2220.89. 2021-02-13T00:35:43Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:35:43Z- There is no specific good beam that all particles have in common. 2021-02-13T00:35:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:35:43Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:35:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:35:43Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 18.06 2021-02-13T00:35:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:35:43Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 9.79 2021-02-13T00:35:43Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:35:43Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:35:43Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:35:43Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:35:43Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:35:43Z- Filter 0 No beams used in subcloud update 2021-02-13T00:35:43Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:35:43Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:35:43Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 695.9 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:35:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176543.17 sec. 2021-02-13T00:35:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:35:43Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:43Z- variances([0], [2], and psi_berg) = 695.893155 832.995831 0.000000 2021-02-13T00:35:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:35:43Z- LcmTrn::publishEstimates() - MLE : 4059559.36 592783.18 16.91 2021-02-13T00:35:43Z- LcmTrn::publishEstimates() - MMSE : 4059559.86 592809.68 16.91 2021-02-13T00:35:43Z- LcmTrn::publishEstimates() - COVAR: 695.89 833.00 0.84 0.00 2021-02-13T00:35:43Z- AHRS_M2 msg: 1613176543.97 epoch sec; seqNo:13915 2021-02-13T00:35:43Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.17 psi 2021-02-13T00:35:43Z- handleDepth()-Depth_Keller msg: 1613176543976 epoch sec; seqNo:14027; depth 16.81 2021-02-13T00:35:43Z- handleDvl() - RDI_Pathfinder msg: 1613176543.99 epoch sec; seqNo:6788 2021-02-13T00:35:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.00 , 97.57 , 105.69 , 73.15 2021-02-13T00:35:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.10 , 0.10 2021-02-13T00:35:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176544.00 epoch sec; seqNo:14027; 4059560.24 north; 592809.67 east 2021-02-13T00:35:44Z- AHRS_M2 msg: 1613176544.37 epoch sec; seqNo:13916 2021-02-13T00:35:44Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.18 psi 2021-02-13T00:35:44Z- handleDepth()-Depth_Keller msg: 1613176544377 epoch sec; seqNo:14028; depth 16.70 2021-02-13T00:35:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176544.39 epoch sec; seqNo:14028; 4059560.24 north; 592809.67 east 2021-02-13T00:35:44Z- AHRS_M2 msg: 1613176544.77 epoch sec; seqNo:13917 2021-02-13T00:35:44Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.18 psi 2021-02-13T00:35:44Z- handleDepth()-Depth_Keller msg: 1613176544778 epoch sec; seqNo:14029; depth 16.61 2021-02-13T00:35:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176544.79 epoch sec; seqNo:14029; 4059560.63 north; 592810.89 east 2021-02-13T00:35:45Z- AHRS_M2 msg: 1613176545.17 epoch sec; seqNo:13918 2021-02-13T00:35:45Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T00:35:45Z- handleDepth()-Depth_Keller msg: 1613176545176 epoch sec; seqNo:14030; depth 16.51 2021-02-13T00:35:45Z- handleDvl() - RDI_Pathfinder msg: 1613176545.19 epoch sec; seqNo:6789 2021-02-13T00:35:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.81 , 97.85 , 106.36 , 73.97 2021-02-13T00:35:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , 0.13 2021-02-13T00:35:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176545.20 epoch sec; seqNo:14030; 4059561.01 north; 592810.88 east 2021-02-13T00:35:45Z- AHRS_M2 msg: 1613176545.58 epoch sec; seqNo:13919 2021-02-13T00:35:45Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-13T00:35:45Z- handleDepth()-Depth_Keller msg: 1613176545580 epoch sec; seqNo:14031; depth 16.42 2021-02-13T00:35:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176545.59 epoch sec; seqNo:14031; 4059561.39 north; 592810.88 east 2021-02-13T00:35:45Z- AHRS_M2 msg: 1613176545.98 epoch sec; seqNo:13920 2021-02-13T00:35:45Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T00:35:45Z- handleDepth()-Depth_Keller msg: 1613176545984 epoch sec; seqNo:14032; depth 16.31 2021-02-13T00:35:45Z- handleDvl() - RDI_Pathfinder msg: 1613176545.99 epoch sec; seqNo:6790 2021-02-13T00:35:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.56 , 108.73 , 71.93 2021-02-13T00:35:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.12 , 0.09 2021-02-13T00:35:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176546.01 epoch sec; seqNo:14032; 4059561.39 north; 592810.88 east 2021-02-13T00:35:46Z- AHRS_M2 msg: 1613176546.38 epoch sec; seqNo:13921 2021-02-13T00:35:46Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.14 psi 2021-02-13T00:35:46Z- handleDepth()-Depth_Keller msg: 1613176546389 epoch sec; seqNo:14033; depth 16.21 2021-02-13T00:35:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176546.40 epoch sec; seqNo:14033; 4059561.77 north; 592810.88 east 2021-02-13T00:35:46Z- AHRS_M2 msg: 1613176546.79 epoch sec; seqNo:13922 2021-02-13T00:35:46Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.13 psi 2021-02-13T00:35:46Z- handleDepth()-Depth_Keller msg: 1613176546792 epoch sec; seqNo:14034; depth 16.09 2021-02-13T00:35:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176546.81 epoch sec; seqNo:14034; 4059562.16 north; 592812.09 east 2021-02-13T00:35:47Z- AHRS_M2 msg: 1613176547.19 epoch sec; seqNo:13923 2021-02-13T00:35:47Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.13 psi 2021-02-13T00:35:47Z- handleDepth()-Depth_Keller msg: 1613176547201 epoch sec; seqNo:14035; depth 15.99 2021-02-13T00:35:47Z- handleDvl() - RDI_Pathfinder msg: 1613176547.22 epoch sec; seqNo:6791 2021-02-13T00:35:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 95.01 , 108.53 , 71.51 2021-02-13T00:35:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.10 , 0.01 2021-02-13T00:35:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176547.23 epoch sec; seqNo:14035; 4059562.16 north; 592812.09 east 2021-02-13T00:35:47Z- AHRS_M2 msg: 1613176547.61 epoch sec; seqNo:13924 2021-02-13T00:35:47Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-13T00:35:47Z- handleDepth()-Depth_Keller msg: 1613176547609 epoch sec; seqNo:14036; depth 15.88 2021-02-13T00:35:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176547.62 epoch sec; seqNo:14036; 4059562.54 north; 592812.09 east 2021-02-13T00:35:48Z- AHRS_M2 msg: 1613176548.01 epoch sec; seqNo:13925 2021-02-13T00:35:48Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-13T00:35:48Z- handleDepth()-Depth_Keller msg: 1613176548017 epoch sec; seqNo:14037; depth 15.77 2021-02-13T00:35:48Z- handleDvl() - RDI_Pathfinder msg: 1613176548.03 epoch sec; seqNo:6792 2021-02-13T00:35:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 97.78 , 107.51 , 73.15 2021-02-13T00:35:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.06 , -0.05 2021-02-13T00:35:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176548.04 epoch sec; seqNo:14037; 4059562.92 north; 592812.08 east 2021-02-13T00:35:48Z- AHRS_M2 msg: 1613176548.40 epoch sec; seqNo:13926 2021-02-13T00:35:48Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-13T00:35:48Z- handleDepth()-Depth_Keller msg: 1613176548408 epoch sec; seqNo:14038; depth 15.66 2021-02-13T00:35:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176548.42 epoch sec; seqNo:14038; 4059562.92 north; 592812.08 east 2021-02-13T00:35:48Z- AHRS_M2 msg: 1613176548.81 epoch sec; seqNo:13927 2021-02-13T00:35:48Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-13T00:35:48Z- handleDepth()-Depth_Keller msg: 1613176548813 epoch sec; seqNo:14039; depth 15.53 2021-02-13T00:35:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176548.82 epoch sec; seqNo:14039; 4059563.29 north; 592812.08 east 2021-02-13T00:35:48Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-13T00:35:48Z- waiting for 1613176546.57, time is 1613176548.82 2021-02-13T00:35:48Z- LcmTrn::updateTrn() >>>> pose time 1613176548.82, meas time 1613176548.03 <<<< 2021-02-13T00:35:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176548.03 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:35:48Z- TerrainNav::motionUpdate() - currEstimate = (4059563, 592812.08, 15.53) 2021-02-13T00:35:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:35:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:35:48Z- TNavBF::Projecting Measurements 2021-02-13T00:35:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:35:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:35:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:35:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:35:48Z- TNavFilter::4 good beams. 2021-02-13T00:35:48Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:35:48Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:35:48Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:35:48Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:35:48Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:35:48Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:35:48Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:35:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:48Z- TerrainMapOctree: (vehN, vehE ) = (4059562.78, 592811.72). 2021-02-13T00:35:48Z- TerrainMapOctree: Min Distance = 2217.07. 2021-02-13T00:35:48Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:35:48Z- There is no specific good beam that all particles have in common. 2021-02-13T00:35:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:35:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 10.75 2021-02-13T00:35:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:35:49Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-13T00:35:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:35:49Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 18.50 2021-02-13T00:35:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:35:49Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-13T00:35:49Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:35:49Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:35:49Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:35:49Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:35:49Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:35:49Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:35:49Z- Filter 0 No beams used in subcloud update 2021-02-13T00:35:49Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:35:49Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:35:49Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 695.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:35:49Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176548.03 sec. 2021-02-13T00:35:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:35:49Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:49Z- variances([0], [2], and psi_berg) = 695.823681 832.995796 0.000000 2021-02-13T00:35:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:35:49Z- LcmTrn::publishEstimates() - MLE : 4059562.79 592784.58 15.53 2021-02-13T00:35:49Z- LcmTrn::publishEstimates() - MMSE : 4059563.29 592812.08 15.53 2021-02-13T00:35:49Z- LcmTrn::publishEstimates() - COVAR: 695.82 833.00 0.82 0.00 2021-02-13T00:35:49Z- AHRS_M2 msg: 1613176549.21 epoch sec; seqNo:13928 2021-02-13T00:35:49Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-13T00:35:49Z- handleDepth()-Depth_Keller msg: 1613176549217 epoch sec; seqNo:14040; depth 15.43 2021-02-13T00:35:49Z- handleDvl() - RDI_Pathfinder msg: 1613176549.23 epoch sec; seqNo:6793 2021-02-13T00:35:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.18 , 99.00 , 108.73 , 74.79 2021-02-13T00:35:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.00 , -0.07 2021-02-13T00:35:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176549.24 epoch sec; seqNo:14040; 4059563.69 north; 592813.30 east 2021-02-13T00:35:49Z- AHRS_M2 msg: 1613176549.62 epoch sec; seqNo:13929 2021-02-13T00:35:49Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.16 psi 2021-02-13T00:35:49Z- handleDepth()-Depth_Keller msg: 1613176549625 epoch sec; seqNo:14041; depth 15.31 2021-02-13T00:35:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176549.64 epoch sec; seqNo:14041; 4059564.06 north; 592813.29 east 2021-02-13T00:35:50Z- AHRS_M2 msg: 1613176550.02 epoch sec; seqNo:13930 2021-02-13T00:35:50Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-13T00:35:50Z- handleDepth()-Depth_Keller msg: 1613176550029 epoch sec; seqNo:14042; depth 15.20 2021-02-13T00:35:50Z- handleDvl() - RDI_Pathfinder msg: 1613176550.04 epoch sec; seqNo:6794 2021-02-13T00:35:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 96.43 , 109.81 , 74.79 2021-02-13T00:35:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.02 , -0.06 2021-02-13T00:35:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176550.06 epoch sec; seqNo:14042; 4059564.44 north; 592813.29 east 2021-02-13T00:35:50Z- AHRS_M2 msg: 1613176550.42 epoch sec; seqNo:13931 2021-02-13T00:35:50Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.14 psi 2021-02-13T00:35:50Z- handleDepth()-Depth_Keller msg: 1613176550428 epoch sec; seqNo:14043; depth 15.09 2021-02-13T00:35:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176550.44 epoch sec; seqNo:14043; 4059564.44 north; 592813.29 east 2021-02-13T00:35:50Z- AHRS_M2 msg: 1613176550.83 epoch sec; seqNo:13932 2021-02-13T00:35:50Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.13 psi 2021-02-13T00:35:50Z- handleDepth()-Depth_Keller msg: 1613176550832 epoch sec; seqNo:14044; depth 15.00 2021-02-13T00:35:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176550.85 epoch sec; seqNo:14044; 4059564.82 north; 592813.28 east 2021-02-13T00:35:51Z- AHRS_M2 msg: 1613176551.23 epoch sec; seqNo:13933 2021-02-13T00:35:51Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.13 psi 2021-02-13T00:35:51Z- handleDepth()-Depth_Keller msg: 1613176551237 epoch sec; seqNo:14045; depth 14.89 2021-02-13T00:35:51Z- handleDvl() - RDI_Pathfinder msg: 1613176551.25 epoch sec; seqNo:6795 2021-02-13T00:35:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:35:51Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:35:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176551.27 epoch sec; seqNo:14045; 4059565.20 north; 592813.28 east 2021-02-13T00:35:51Z- AHRS_M2 msg: 1613176551.64 epoch sec; seqNo:13934 2021-02-13T00:35:51Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.12 psi 2021-02-13T00:35:51Z- handleDepth()-Depth_Keller msg: 1613176551640 epoch sec; seqNo:14046; depth 14.76 2021-02-13T00:35:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176551.65 epoch sec; seqNo:14046; 4059565.59 north; 592814.50 east 2021-02-13T00:35:52Z- AHRS_M2 msg: 1613176552.04 epoch sec; seqNo:13935 2021-02-13T00:35:52Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.12 psi 2021-02-13T00:35:52Z- handleDepth()-Depth_Keller msg: 1613176552050 epoch sec; seqNo:14047; depth 14.66 2021-02-13T00:35:52Z- handleDvl() - RDI_Pathfinder msg: 1613176552.07 epoch sec; seqNo:6796 2021-02-13T00:35:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.44 , 112.32 , 73.15 2021-02-13T00:35:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.05 , -0.10 2021-02-13T00:35:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176552.08 epoch sec; seqNo:14047; 4059565.59 north; 592814.50 east 2021-02-13T00:35:52Z- AHRS_M2 msg: 1613176552.44 epoch sec; seqNo:13936 2021-02-13T00:35:52Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.12 psi 2021-02-13T00:35:52Z- handleDepth()-Depth_Keller msg: 1613176552453 epoch sec; seqNo:14048; depth 14.51 2021-02-13T00:35:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176552.46 epoch sec; seqNo:14048; 4059565.97 north; 592814.49 east 2021-02-13T00:35:52Z- AHRS_M2 msg: 1613176552.85 epoch sec; seqNo:13937 2021-02-13T00:35:52Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.12 psi 2021-02-13T00:35:52Z- handleDepth()-Depth_Keller msg: 1613176552852 epoch sec; seqNo:14049; depth 14.42 2021-02-13T00:35:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176552.86 epoch sec; seqNo:14049; 4059566.35 north; 592814.49 east 2021-02-13T00:35:53Z- AHRS_M2 msg: 1613176553.25 epoch sec; seqNo:13938 2021-02-13T00:35:53Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.12 psi 2021-02-13T00:35:53Z- handleDepth()-Depth_Keller msg: 1613176553261 epoch sec; seqNo:14050; depth 14.31 2021-02-13T00:35:53Z- handleDvl() - RDI_Pathfinder msg: 1613176553.27 epoch sec; seqNo:6797 2021-02-13T00:35:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 96.15 , 112.38 , 74.38 2021-02-13T00:35:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.08 2021-02-13T00:35:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176553.28 epoch sec; seqNo:14050; 4059566.35 north; 592814.49 east 2021-02-13T00:35:53Z- AHRS_M2 msg: 1613176553.66 epoch sec; seqNo:13939 2021-02-13T00:35:53Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.11 psi 2021-02-13T00:35:53Z- handleDepth()-Depth_Keller msg: 1613176553660 epoch sec; seqNo:14051; depth 14.17 2021-02-13T00:35:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176553.68 epoch sec; seqNo:14051; 4059566.73 north; 592814.48 east 2021-02-13T00:35:54Z- AHRS_M2 msg: 1613176554.06 epoch sec; seqNo:13940 2021-02-13T00:35:54Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.11 psi 2021-02-13T00:35:54Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-13T00:35:54Z- waiting for 1613176551.82, time is 1613176554.06 2021-02-13T00:35:54Z- LcmTrn::updateTrn() >>>> pose time 1613176554.06, meas time 1613176553.27 <<<< 2021-02-13T00:35:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176553.27 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:35:54Z- TerrainNav::motionUpdate() - currEstimate = (4059567, 592814.48, 14.17) 2021-02-13T00:35:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:35:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:35:54Z- TNavBF::Projecting Measurements 2021-02-13T00:35:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:35:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:35:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:35:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:35:54Z- TNavFilter::4 good beams. 2021-02-13T00:35:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:35:54Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:35:54Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:35:54Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:35:54Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:35:54Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:35:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:35:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:54Z- TerrainMapOctree: (vehN, vehE ) = (4059566.21, 592814.12). 2021-02-13T00:35:54Z- TerrainMapOctree: Min Distance = 2212.89. 2021-02-13T00:35:54Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:35:54Z- There is no specific good beam that all particles have in common. 2021-02-13T00:35:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:35:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 11.29 2021-02-13T00:35:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:35:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 14.79 2021-02-13T00:35:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:35:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 20.21 2021-02-13T00:35:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:35:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-13T00:35:54Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:35:54Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:35:54Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:35:54Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:35:54Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:35:54Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:35:54Z- Filter 0 No beams used in subcloud update 2021-02-13T00:35:54Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:35:54Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:35:54Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 695.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:35:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176553.27 sec. 2021-02-13T00:35:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:35:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:54Z- variances([0], [2], and psi_berg) = 695.754284 832.995761 0.000000 2021-02-13T00:35:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:35:54Z- LcmTrn::publishEstimates() - MLE : 4059566.23 592846.98 14.17 2021-02-13T00:35:54Z- LcmTrn::publishEstimates() - MMSE : 4059566.73 592814.48 14.17 2021-02-13T00:35:54Z- LcmTrn::publishEstimates() - COVAR: 695.75 833.00 0.82 0.00 2021-02-13T00:35:54Z- handleDepth()-Depth_Keller msg: 1613176554065 epoch sec; seqNo:14052; depth 14.05 2021-02-13T00:35:54Z- handleDvl() - RDI_Pathfinder msg: 1613176554.08 epoch sec; seqNo:6798 2021-02-13T00:35:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 95.21 , 112.25 , 73.56 2021-02-13T00:35:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , -0.02 2021-02-13T00:35:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176554.10 epoch sec; seqNo:14052; 4059567.12 north; 592815.70 east 2021-02-13T00:35:54Z- AHRS_M2 msg: 1613176554.46 epoch sec; seqNo:13941 2021-02-13T00:35:54Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.11 psi 2021-02-13T00:35:54Z- handleDepth()-Depth_Keller msg: 1613176554468 epoch sec; seqNo:14053; depth 13.93 2021-02-13T00:35:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176554.48 epoch sec; seqNo:14053; 4059567.12 north; 592815.70 east 2021-02-13T00:35:54Z- AHRS_M2 msg: 1613176554.87 epoch sec; seqNo:13942 2021-02-13T00:35:54Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.12 psi 2021-02-13T00:35:54Z- handleDepth()-Depth_Keller msg: 1613176554874 epoch sec; seqNo:14054; depth 13.81 2021-02-13T00:35:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176554.88 epoch sec; seqNo:14054; 4059567.50 north; 592815.70 east 2021-02-13T00:35:55Z- AHRS_M2 msg: 1613176555.27 epoch sec; seqNo:13943 2021-02-13T00:35:55Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.14 psi 2021-02-13T00:35:55Z- handleDepth()-Depth_Keller msg: 1613176555281 epoch sec; seqNo:14055; depth 13.70 2021-02-13T00:35:55Z- handleDvl() - RDI_Pathfinder msg: 1613176555.30 epoch sec; seqNo:6799 2021-02-13T00:35:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.76 , 97.57 , 113.80 , 78.64 2021-02-13T00:35:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.00 , 0.02 2021-02-13T00:35:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176555.31 epoch sec; seqNo:14055; 4059567.88 north; 592815.69 east 2021-02-13T00:35:55Z- AHRS_M2 msg: 1613176555.68 epoch sec; seqNo:13944 2021-02-13T00:35:55Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.17 psi 2021-02-13T00:35:55Z- handleDepth()-Depth_Keller msg: 1613176555680 epoch sec; seqNo:14056; depth 13.55 2021-02-13T00:35:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176555.69 epoch sec; seqNo:14056; 4059567.88 north; 592815.69 east 2021-02-13T00:35:56Z- AHRS_M2 msg: 1613176556.08 epoch sec; seqNo:13945 2021-02-13T00:35:56Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.20 psi 2021-02-13T00:35:56Z- handleDepth()-Depth_Keller msg: 1613176556084 epoch sec; seqNo:14057; depth 13.44 2021-02-13T00:35:56Z- handleDvl() - RDI_Pathfinder msg: 1613176556.10 epoch sec; seqNo:6800 2021-02-13T00:35:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 98.86 , 113.73 , 78.58 2021-02-13T00:35:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.02 2021-02-13T00:35:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176556.11 epoch sec; seqNo:14057; 4059568.26 north; 592815.69 east 2021-02-13T00:35:56Z- AHRS_M2 msg: 1613176556.48 epoch sec; seqNo:13946 2021-02-13T00:35:56Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.21 psi 2021-02-13T00:35:56Z- handleDepth()-Depth_Keller msg: 1613176556490 epoch sec; seqNo:14058; depth 13.31 2021-02-13T00:35:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176556.50 epoch sec; seqNo:14058; 4059568.64 north; 592815.68 east 2021-02-13T00:35:56Z- AHRS_M2 msg: 1613176556.91 epoch sec; seqNo:13947 2021-02-13T00:35:56Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.22 psi 2021-02-13T00:35:56Z- handleDepth()-Depth_Keller msg: 1613176556913 epoch sec; seqNo:14059; depth 13.20 2021-02-13T00:35:56Z- handleDvl() - RDI_Pathfinder msg: 1613176556.92 epoch sec; seqNo:6801 2021-02-13T00:35:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.62 , 117.79 , 76.42 2021-02-13T00:35:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.05 2021-02-13T00:35:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176556.93 epoch sec; seqNo:14059; 4059568.65 north; 592816.90 east 2021-02-13T00:35:57Z- AHRS_M2 msg: 1613176557.29 epoch sec; seqNo:13948 2021-02-13T00:35:57Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.22 psi 2021-02-13T00:35:57Z- handleDepth()-Depth_Keller msg: 1613176557296 epoch sec; seqNo:14060; depth 13.08 2021-02-13T00:35:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176557.31 epoch sec; seqNo:14060; 4059569.03 north; 592816.90 east 2021-02-13T00:35:57Z- AHRS_M2 msg: 1613176557.70 epoch sec; seqNo:13949 2021-02-13T00:35:57Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.21 psi 2021-02-13T00:35:57Z- handleDepth()-Depth_Keller msg: 1613176557708 epoch sec; seqNo:14061; depth 12.97 2021-02-13T00:35:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176557.72 epoch sec; seqNo:14061; 4059569.41 north; 592816.90 east 2021-02-13T00:35:58Z- AHRS_M2 msg: 1613176558.11 epoch sec; seqNo:13950 2021-02-13T00:35:58Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.20 psi 2021-02-13T00:35:58Z- handleDepth()-Depth_Keller msg: 1613176558115 epoch sec; seqNo:14062; depth 12.86 2021-02-13T00:35:58Z- handleDvl() - RDI_Pathfinder msg: 1613176558.13 epoch sec; seqNo:6802 2021-02-13T00:35:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.52 , 105.15 , 114.61 , 78.11 2021-02-13T00:35:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.11 , 0.02 2021-02-13T00:35:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176558.16 epoch sec; seqNo:14062; 4059569.41 north; 592816.90 east 2021-02-13T00:35:58Z- AHRS_M2 msg: 1613176558.50 epoch sec; seqNo:13951 2021-02-13T00:35:58Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.20 psi 2021-02-13T00:35:58Z- handleDepth()-Depth_Keller msg: 1613176558517 epoch sec; seqNo:14063; depth 12.73 2021-02-13T00:35:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176558.53 epoch sec; seqNo:14063; 4059569.79 north; 592816.89 east 2021-02-13T00:35:58Z- AHRS_M2 msg: 1613176558.91 epoch sec; seqNo:13952 2021-02-13T00:35:58Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.20 psi 2021-02-13T00:35:58Z- handleDepth()-Depth_Keller msg: 1613176558916 epoch sec; seqNo:14064; depth 12.63 2021-02-13T00:35:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176558.95 epoch sec; seqNo:14064; 4059570.17 north; 592816.89 east 2021-02-13T00:35:59Z- LcmTrn::handleMessages() - handling 43 messages took 0.66 ms and 130 calls... 2021-02-13T00:35:59Z- waiting for 1613176557.06, time is 1613176558.95 2021-02-13T00:35:59Z- LcmTrn::updateTrn() >>>> pose time 1613176558.95, meas time 1613176558.13 <<<< 2021-02-13T00:35:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176558.13 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:35:59Z- TerrainNav::motionUpdate() - currEstimate = (4059570, 592816.89, 12.63) 2021-02-13T00:35:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:35:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:35:59Z- TNavBF::Projecting Measurements 2021-02-13T00:35:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:35:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:35:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:35:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:35:59Z- TNavFilter::4 good beams. 2021-02-13T00:35:59Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:35:59Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:35:59Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:35:59Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:35:59Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:35:59Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:35:59Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:35:59Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:59Z- TerrainMapOctree: (vehN, vehE ) = (4059569.59, 592816.49). 2021-02-13T00:35:59Z- TerrainMapOctree: Min Distance = 2208.79. 2021-02-13T00:35:59Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:35:59Z- There is no specific good beam that all particles have in common. 2021-02-13T00:35:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:35:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 11.16 2021-02-13T00:35:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:35:59Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 17.69 2021-02-13T00:35:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:35:59Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 21.02 2021-02-13T00:35:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:35:59Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 9.76 2021-02-13T00:35:59Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:35:59Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:35:59Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:35:59Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:35:59Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:35:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:35:59Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:35:59Z- Filter 0 No beams used in subcloud update 2021-02-13T00:35:59Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:35:59Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:35:59Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 695.7 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:35:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176558.13 sec. 2021-02-13T00:35:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:35:59Z- TNavBF::mmse selected from filter -1 2021-02-13T00:35:59Z- variances([0], [2], and psi_berg) = 695.684288 832.995726 0.000000 2021-02-13T00:35:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:35:59Z- LcmTrn::publishEstimates() - MLE : 4059569.67 592790.39 12.63 2021-02-13T00:35:59Z- LcmTrn::publishEstimates() - MMSE : 4059570.17 592816.89 12.63 2021-02-13T00:35:59Z- LcmTrn::publishEstimates() - COVAR: 695.68 833.00 0.83 0.00 2021-02-13T00:35:59Z- AHRS_M2 msg: 1613176559.31 epoch sec; seqNo:13953 2021-02-13T00:35:59Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.20 psi 2021-02-13T00:35:59Z- handleDepth()-Depth_Keller msg: 1613176559316 epoch sec; seqNo:14065; depth 12.53 2021-02-13T00:35:59Z- handleDvl() - RDI_Pathfinder msg: 1613176559.33 epoch sec; seqNo:6803 2021-02-13T00:35:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 104.20 , 115.02 , 75.19 2021-02-13T00:35:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.12 , 0.16 2021-02-13T00:35:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176559.35 epoch sec; seqNo:14065; 4059570.54 north; 592816.88 east 2021-02-13T00:35:59Z- AHRS_M2 msg: 1613176559.72 epoch sec; seqNo:13954 2021-02-13T00:35:59Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.20 psi 2021-02-13T00:35:59Z- handleDepth()-Depth_Keller msg: 1613176559720 epoch sec; seqNo:14066; depth 12.41 2021-02-13T00:35:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176559.73 epoch sec; seqNo:14066; 4059570.94 north; 592818.10 east 2021-02-13T00:36:00Z- AHRS_M2 msg: 1613176560.12 epoch sec; seqNo:13955 2021-02-13T00:36:00Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.20 psi 2021-02-13T00:36:00Z- handleDepth()-Depth_Keller msg: 1613176560129 epoch sec; seqNo:14067; depth 12.30 2021-02-13T00:36:00Z- handleDvl() - RDI_Pathfinder msg: 1613176560.14 epoch sec; seqNo:6804 2021-02-13T00:36:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 102.92 , 115.09 , 78.58 2021-02-13T00:36:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , 0.28 2021-02-13T00:36:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176560.16 epoch sec; seqNo:14067; 4059570.94 north; 592818.10 east 2021-02-13T00:36:00Z- AHRS_M2 msg: 1613176560.52 epoch sec; seqNo:13956 2021-02-13T00:36:00Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.20 psi 2021-02-13T00:36:00Z- handleDepth()-Depth_Keller msg: 1613176560528 epoch sec; seqNo:14068; depth 12.19 2021-02-13T00:36:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176560.55 epoch sec; seqNo:14068; 4059571.32 north; 592818.10 east 2021-02-13T00:36:00Z- AHRS_M2 msg: 1613176560.93 epoch sec; seqNo:13957 2021-02-13T00:36:00Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.20 psi 2021-02-13T00:36:00Z- handleDepth()-Depth_Keller msg: 1613176560932 epoch sec; seqNo:14069; depth 12.07 2021-02-13T00:36:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176560.94 epoch sec; seqNo:14069; 4059571.69 north; 592818.09 east 2021-02-13T00:36:01Z- AHRS_M2 msg: 1613176561.33 epoch sec; seqNo:13958 2021-02-13T00:36:01Z- AHRS_M2 msg: 0.55 phi; 0.35 theta; -0.19 psi 2021-02-13T00:36:01Z- handleDepth()-Depth_Keller msg: 1613176561341 epoch sec; seqNo:14070; depth 11.96 2021-02-13T00:36:01Z- handleDvl() - RDI_Pathfinder msg: 1613176561.35 epoch sec; seqNo:6805 2021-02-13T00:36:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.28 , 104.20 , 116.37 , 78.51 2021-02-13T00:36:01Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.23 , 0.22 2021-02-13T00:36:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176561.36 epoch sec; seqNo:14070; 4059572.07 north; 592818.09 east 2021-02-13T00:36:01Z- AHRS_M2 msg: 1613176561.74 epoch sec; seqNo:13959 2021-02-13T00:36:01Z- AHRS_M2 msg: 0.54 phi; 0.36 theta; -0.19 psi 2021-02-13T00:36:01Z- handleDepth()-Depth_Keller msg: 1613176561742 epoch sec; seqNo:14071; depth 11.86 2021-02-13T00:36:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176561.75 epoch sec; seqNo:14071; 4059572.47 north; 592819.30 east 2021-02-13T00:36:02Z- AHRS_M2 msg: 1613176562.18 epoch sec; seqNo:13960 2021-02-13T00:36:02Z- AHRS_M2 msg: 0.55 phi; 0.36 theta; -0.18 psi 2021-02-13T00:36:02Z- handleDepth()-Depth_Keller msg: 1613176562183 epoch sec; seqNo:14072; depth 11.75 2021-02-13T00:36:02Z- handleDvl() - RDI_Pathfinder msg: 1613176562.20 epoch sec; seqNo:6806 2021-02-13T00:36:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.14 , 119.01 , 78.45 2021-02-13T00:36:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.40 , -0.02 2021-02-13T00:36:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176562.23 epoch sec; seqNo:14072; 4059572.47 north; 592819.30 east 2021-02-13T00:36:02Z- AHRS_M2 msg: 1613176562.54 epoch sec; seqNo:13961 2021-02-13T00:36:02Z- AHRS_M2 msg: 0.55 phi; 0.36 theta; -0.18 psi 2021-02-13T00:36:02Z- handleDepth()-Depth_Keller msg: 1613176562547 epoch sec; seqNo:14073; depth 11.61 2021-02-13T00:36:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176562.56 epoch sec; seqNo:14073; 4059572.84 north; 592819.30 east 2021-02-13T00:36:02Z- AHRS_M2 msg: 1613176562.95 epoch sec; seqNo:13962 2021-02-13T00:36:02Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.18 psi 2021-02-13T00:36:02Z- handleDepth()-Depth_Keller msg: 1613176562952 epoch sec; seqNo:14074; depth 11.53 2021-02-13T00:36:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176562.96 epoch sec; seqNo:14074; 4059572.84 north; 592819.30 east 2021-02-13T00:36:03Z- AHRS_M2 msg: 1613176563.36 epoch sec; seqNo:13963 2021-02-13T00:36:03Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.18 psi 2021-02-13T00:36:03Z- handleDepth()-Depth_Keller msg: 1613176563369 epoch sec; seqNo:14075; depth 11.41 2021-02-13T00:36:03Z- handleDvl() - RDI_Pathfinder msg: 1613176563.38 epoch sec; seqNo:6807 2021-02-13T00:36:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.51 , 103.73 , 119.95 , 78.46 2021-02-13T00:36:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.31 , -0.31 2021-02-13T00:36:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176563.39 epoch sec; seqNo:14075; 4059573.24 north; 592820.52 east 2021-02-13T00:36:03Z- AHRS_M2 msg: 1613176563.76 epoch sec; seqNo:13964 2021-02-13T00:36:03Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.18 psi 2021-02-13T00:36:03Z- handleDepth()-Depth_Keller msg: 1613176563760 epoch sec; seqNo:14076; depth 11.30 2021-02-13T00:36:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176563.79 epoch sec; seqNo:14076; 4059573.24 north; 592820.52 east 2021-02-13T00:36:04Z- AHRS_M2 msg: 1613176564.16 epoch sec; seqNo:13965 2021-02-13T00:36:04Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-13T00:36:04Z- handleDepth()-Depth_Keller msg: 1613176564165 epoch sec; seqNo:14077; depth 11.18 2021-02-13T00:36:04Z- handleDvl() - RDI_Pathfinder msg: 1613176564.18 epoch sec; seqNo:6808 2021-02-13T00:36:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.42 , 104.00 , 118.87 , 76.42 2021-02-13T00:36:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.08 , -0.42 2021-02-13T00:36:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176564.20 epoch sec; seqNo:14077; 4059573.62 north; 592820.51 east 2021-02-13T00:36:04Z- LcmTrn::handleMessages() - handling 45 messages took 0.72 ms and 131 calls... 2021-02-13T00:36:04Z- waiting for 1613176561.95, time is 1613176564.20 2021-02-13T00:36:04Z- LcmTrn::updateTrn() equating pose 1613176564.20 and meas times 1613176564.20 2021-02-13T00:36:04Z- LcmTrn::updateTrn() >>>> pose time 1613176564.20, meas time 1613176564.20 <<<< 2021-02-13T00:36:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176564.20 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:36:04Z- TerrainNav::motionUpdate() - currEstimate = (4059574, 592820.51, 11.18) 2021-02-13T00:36:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:36:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:36:04Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176564.20 sec.; No relevant navigation data available 2021-02-13T00:36:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:36:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:36:04Z- variances([0], [2], and psi_berg) = 695.626467 832.995697 0.000000 2021-02-13T00:36:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:36:04Z- LcmTrn::publishEstimates() - MLE : 4059573.12 592794.01 11.18 2021-02-13T00:36:04Z- LcmTrn::publishEstimates() - MMSE : 4059573.62 592820.51 11.18 2021-02-13T00:36:04Z- LcmTrn::publishEstimates() - COVAR: 695.63 833.00 1.08 0.00 2021-02-13T00:36:04Z- AHRS_M2 msg: 1613176564.57 epoch sec; seqNo:13966 2021-02-13T00:36:04Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.16 psi 2021-02-13T00:36:04Z- handleDepth()-Depth_Keller msg: 1613176564575 epoch sec; seqNo:14078; depth 11.05 2021-02-13T00:36:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176564.59 epoch sec; seqNo:14078; 4059573.62 north; 592820.51 east 2021-02-13T00:36:04Z- AHRS_M2 msg: 1613176564.97 epoch sec; seqNo:13967 2021-02-13T00:36:04Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-13T00:36:04Z- handleDepth()-Depth_Keller msg: 1613176564977 epoch sec; seqNo:14079; depth 10.96 2021-02-13T00:36:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176564.98 epoch sec; seqNo:14079; 4059573.99 north; 592820.51 east 2021-02-13T00:36:05Z- AHRS_M2 msg: 1613176565.37 epoch sec; seqNo:13968 2021-02-13T00:36:05Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.15 psi 2021-02-13T00:36:05Z- handleDepth()-Depth_Keller msg: 1613176565381 epoch sec; seqNo:14080; depth 10.84 2021-02-13T00:36:05Z- handleDvl() - RDI_Pathfinder msg: 1613176565.40 epoch sec; seqNo:6809 2021-02-13T00:36:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 104.07 , 116.24 , 79.73 2021-02-13T00:36:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.20 , -0.27 2021-02-13T00:36:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176565.41 epoch sec; seqNo:14080; 4059574.37 north; 592820.50 east 2021-02-13T00:36:05Z- AHRS_M2 msg: 1613176565.78 epoch sec; seqNo:13969 2021-02-13T00:36:05Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.15 psi 2021-02-13T00:36:05Z- handleDepth()-Depth_Keller msg: 1613176565780 epoch sec; seqNo:14081; depth 10.71 2021-02-13T00:36:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176565.79 epoch sec; seqNo:14081; 4059574.37 north; 592820.50 east 2021-02-13T00:36:06Z- AHRS_M2 msg: 1613176566.18 epoch sec; seqNo:13970 2021-02-13T00:36:06Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-13T00:36:06Z- handleDepth()-Depth_Keller msg: 1613176566193 epoch sec; seqNo:14082; depth 10.59 2021-02-13T00:36:06Z- handleDvl() - RDI_Pathfinder msg: 1613176566.20 epoch sec; seqNo:6810 2021-02-13T00:36:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 101.36 , 116.24 , 78.38 2021-02-13T00:36:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.32 , 0.00 2021-02-13T00:36:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176566.21 epoch sec; seqNo:14082; 4059574.75 north; 592820.50 east 2021-02-13T00:36:06Z- AHRS_M2 msg: 1613176566.58 epoch sec; seqNo:13971 2021-02-13T00:36:06Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-13T00:36:06Z- handleDepth()-Depth_Keller msg: 1613176566588 epoch sec; seqNo:14083; depth 10.46 2021-02-13T00:36:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176566.61 epoch sec; seqNo:14083; 4059575.13 north; 592820.50 east 2021-02-13T00:36:06Z- AHRS_M2 msg: 1613176566.99 epoch sec; seqNo:13972 2021-02-13T00:36:06Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.15 psi 2021-02-13T00:36:06Z- handleDepth()-Depth_Keller msg: 1613176566997 epoch sec; seqNo:14084; depth 10.35 2021-02-13T00:36:07Z- handleDvl() - RDI_Pathfinder msg: 1613176567.01 epoch sec; seqNo:6811 2021-02-13T00:36:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.85 , 115.02 , 78.51 2021-02-13T00:36:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.25 , 0.25 2021-02-13T00:36:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176567.03 epoch sec; seqNo:14084; 4059575.52 north; 592821.71 east 2021-02-13T00:36:07Z- AHRS_M2 msg: 1613176567.39 epoch sec; seqNo:13973 2021-02-13T00:36:07Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.15 psi 2021-02-13T00:36:07Z- handleDepth()-Depth_Keller msg: 1613176567401 epoch sec; seqNo:14085; depth 10.21 2021-02-13T00:36:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176567.41 epoch sec; seqNo:14085; 4059575.90 north; 592821.71 east 2021-02-13T00:36:07Z- AHRS_M2 msg: 1613176567.80 epoch sec; seqNo:13974 2021-02-13T00:36:07Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.15 psi 2021-02-13T00:36:07Z- handleDepth()-Depth_Keller msg: 1613176567802 epoch sec; seqNo:14086; depth 10.10 2021-02-13T00:36:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176567.81 epoch sec; seqNo:14086; 4059576.28 north; 592821.71 east 2021-02-13T00:36:08Z- AHRS_M2 msg: 1613176568.20 epoch sec; seqNo:13975 2021-02-13T00:36:08Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.16 psi 2021-02-13T00:36:08Z- handleDepth()-Depth_Keller msg: 1613176568214 epoch sec; seqNo:14087; depth 9.98 2021-02-13T00:36:08Z- handleDvl() - RDI_Pathfinder msg: 1613176568.23 epoch sec; seqNo:6812 2021-02-13T00:36:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.10 , 102.51 , 116.03 , 79.52 2021-02-13T00:36:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.00 , 0.27 2021-02-13T00:36:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176568.24 epoch sec; seqNo:14087; 4059576.66 north; 592821.70 east 2021-02-13T00:36:08Z- AHRS_M2 msg: 1613176568.60 epoch sec; seqNo:13976 2021-02-13T00:36:08Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.16 psi 2021-02-13T00:36:08Z- handleDepth()-Depth_Keller msg: 1613176568613 epoch sec; seqNo:14088; depth 9.85 2021-02-13T00:36:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176568.62 epoch sec; seqNo:14088; 4059577.04 north; 592821.70 east 2021-02-13T00:36:09Z- AHRS_M2 msg: 1613176569.01 epoch sec; seqNo:13977 2021-02-13T00:36:09Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.16 psi 2021-02-13T00:36:09Z- handleDepth()-Depth_Keller msg: 1613176569012 epoch sec; seqNo:14089; depth 9.75 2021-02-13T00:36:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176569.02 epoch sec; seqNo:14089; 4059577.04 north; 592821.70 east 2021-02-13T00:36:09Z- LcmTrn::handleMessages() - handling 40 messages took 0.60 ms and 128 calls... 2021-02-13T00:36:09Z- waiting for 1613176567.20, time is 1613176569.02 2021-02-13T00:36:09Z- LcmTrn::updateTrn() >>>> pose time 1613176569.02, meas time 1613176568.23 <<<< 2021-02-13T00:36:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176568.23 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:36:09Z- TerrainNav::motionUpdate() - currEstimate = (4059577, 592821.70, 9.75) 2021-02-13T00:36:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:36:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:36:09Z- TNavBF::Projecting Measurements 2021-02-13T00:36:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:36:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:36:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:36:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:36:09Z- TNavFilter::4 good beams. 2021-02-13T00:36:09Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:36:09Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:36:09Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:36:09Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:36:09Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:36:09Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:36:09Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:36:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:36:09Z- TerrainMapOctree: (vehN, vehE ) = (4059576.47, 592821.50). 2021-02-13T00:36:09Z- TerrainMapOctree: Min Distance = 2200.30. 2021-02-13T00:36:09Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:36:09Z- There is no specific good beam that all particles have in common. 2021-02-13T00:36:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:36:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-13T00:36:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:36:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 16.81 2021-02-13T00:36:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:36:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 21.54 2021-02-13T00:36:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:36:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 10.12 2021-02-13T00:36:09Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:36:09Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:36:09Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:36:09Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:36:09Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:36:09Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:36:09Z- Filter 0 No beams used in subcloud update 2021-02-13T00:36:09Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:36:09Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:36:09Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 695.6 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:36:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176568.23 sec. 2021-02-13T00:36:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:36:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:36:09Z- variances([0], [2], and psi_berg) = 695.561583 832.995664 0.000000 2021-02-13T00:36:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:36:09Z- LcmTrn::publishEstimates() - MLE : 4059576.54 592795.20 9.75 2021-02-13T00:36:09Z- LcmTrn::publishEstimates() - MMSE : 4059577.04 592821.70 9.75 2021-02-13T00:36:09Z- LcmTrn::publishEstimates() - COVAR: 695.56 833.00 0.98 0.00 2021-02-13T00:36:09Z- AHRS_M2 msg: 1613176569.41 epoch sec; seqNo:13978 2021-02-13T00:36:09Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.16 psi 2021-02-13T00:36:09Z- handleDepth()-Depth_Keller msg: 1613176569418 epoch sec; seqNo:14090; depth 9.61 2021-02-13T00:36:09Z- handleDvl() - RDI_Pathfinder msg: 1613176569.43 epoch sec; seqNo:6813 2021-02-13T00:36:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 105.22 , 118.74 , 148.48 2021-02-13T00:36:09Z- handleDvl() - RDI_Pathfinder msg: velocities 1.12 , 0.18 , 0.11 2021-02-13T00:36:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176569.44 epoch sec; seqNo:14090; 4059577.43 north; 592822.91 east 2021-02-13T00:36:09Z- AHRS_M2 msg: 1613176569.82 epoch sec; seqNo:13979 2021-02-13T00:36:09Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.17 psi 2021-02-13T00:36:09Z- handleDepth()-Depth_Keller msg: 1613176569821 epoch sec; seqNo:14091; depth 9.48 2021-02-13T00:36:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176569.85 epoch sec; seqNo:14091; 4059577.81 north; 592822.91 east 2021-02-13T00:36:10Z- AHRS_M2 msg: 1613176570.22 epoch sec; seqNo:13980 2021-02-13T00:36:10Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.17 psi 2021-02-13T00:36:10Z- handleDepth()-Depth_Keller msg: 1613176570226 epoch sec; seqNo:14092; depth 9.37 2021-02-13T00:36:10Z- handleDvl() - RDI_Pathfinder msg: 1613176570.24 epoch sec; seqNo:6814 2021-02-13T00:36:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 104.68 , 121.04 , 79.39 2021-02-13T00:36:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.17 , -0.14 2021-02-13T00:36:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176570.25 epoch sec; seqNo:14092; 4059578.19 north; 592822.91 east 2021-02-13T00:36:10Z- AHRS_M2 msg: 1613176570.62 epoch sec; seqNo:13981 2021-02-13T00:36:10Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-13T00:36:10Z- handleDepth()-Depth_Keller msg: 1613176570638 epoch sec; seqNo:14093; depth 9.26 2021-02-13T00:36:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176570.65 epoch sec; seqNo:14093; 4059578.19 north; 592822.91 east 2021-02-13T00:36:11Z- AHRS_M2 msg: 1613176571.03 epoch sec; seqNo:13982 2021-02-13T00:36:11Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-13T00:36:11Z- handleDepth()-Depth_Keller msg: 1613176571032 epoch sec; seqNo:14094; depth 9.13 2021-02-13T00:36:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176571.04 epoch sec; seqNo:14094; 4059578.57 north; 592822.90 east 2021-02-13T00:36:11Z- AHRS_M2 msg: 1613176571.43 epoch sec; seqNo:13983 2021-02-13T00:36:11Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-13T00:36:11Z- handleDepth()-Depth_Keller msg: 1613176571436 epoch sec; seqNo:14095; depth 9.00 2021-02-13T00:36:11Z- handleDvl() - RDI_Pathfinder msg: 1613176571.45 epoch sec; seqNo:6815 2021-02-13T00:36:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.86 , 105.08 , 121.31 , 80.75 2021-02-13T00:36:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.01 , -0.18 2021-02-13T00:36:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176571.46 epoch sec; seqNo:14095; 4059578.58 north; 592824.12 east 2021-02-13T00:36:11Z- AHRS_M2 msg: 1613176571.85 epoch sec; seqNo:13984 2021-02-13T00:36:11Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-13T00:36:11Z- handleDepth()-Depth_Keller msg: 1613176571857 epoch sec; seqNo:14096; depth 8.87 2021-02-13T00:36:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176571.87 epoch sec; seqNo:14096; 4059578.96 north; 592824.12 east 2021-02-13T00:36:12Z- AHRS_M2 msg: 1613176572.24 epoch sec; seqNo:13985 2021-02-13T00:36:12Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-13T00:36:12Z- handleDepth()-Depth_Keller msg: 1613176572244 epoch sec; seqNo:14097; depth 8.75 2021-02-13T00:36:12Z- handleDvl() - RDI_Pathfinder msg: 1613176572.26 epoch sec; seqNo:6816 2021-02-13T00:36:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:36:12Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:36:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176572.27 epoch sec; seqNo:14097; 4059579.34 north; 592824.11 east 2021-02-13T00:36:12Z- AHRS_M2 msg: 1613176572.64 epoch sec; seqNo:13986 2021-02-13T00:36:12Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-13T00:36:12Z- handleDepth()-Depth_Keller msg: 1613176572648 epoch sec; seqNo:14098; depth 8.61 2021-02-13T00:36:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176572.66 epoch sec; seqNo:14098; 4059579.34 north; 592824.11 east 2021-02-13T00:36:13Z- AHRS_M2 msg: 1613176573.05 epoch sec; seqNo:13987 2021-02-13T00:36:13Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.16 psi 2021-02-13T00:36:13Z- handleDepth()-Depth_Keller msg: 1613176573057 epoch sec; seqNo:14099; depth 8.50 2021-02-13T00:36:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176573.07 epoch sec; seqNo:14099; 4059579.72 north; 592824.11 east 2021-02-13T00:36:13Z- AHRS_M2 msg: 1613176573.45 epoch sec; seqNo:13988 2021-02-13T00:36:13Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.16 psi 2021-02-13T00:36:13Z- handleDepth()-Depth_Keller msg: 1613176573465 epoch sec; seqNo:14100; depth 8.37 2021-02-13T00:36:13Z- handleDvl() - RDI_Pathfinder msg: 1613176573.48 epoch sec; seqNo:6817 2021-02-13T00:36:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.32 , 104.68 , 116.57 , 80.88 2021-02-13T00:36:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , 0.18 2021-02-13T00:36:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176573.49 epoch sec; seqNo:14100; 4059580.10 north; 592824.11 east 2021-02-13T00:36:13Z- AHRS_M2 msg: 1613176573.86 epoch sec; seqNo:13989 2021-02-13T00:36:13Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.17 psi 2021-02-13T00:36:13Z- handleDepth()-Depth_Keller msg: 1613176573861 epoch sec; seqNo:14101; depth 8.23 2021-02-13T00:36:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176573.88 epoch sec; seqNo:14101; 4059580.47 north; 592824.10 east 2021-02-13T00:36:14Z- AHRS_M2 msg: 1613176574.26 epoch sec; seqNo:13990 2021-02-13T00:36:14Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.17 psi 2021-02-13T00:36:14Z- handleDepth()-Depth_Keller msg: 1613176574265 epoch sec; seqNo:14102; depth 8.11 2021-02-13T00:36:14Z- handleDvl() - RDI_Pathfinder msg: 1613176574.28 epoch sec; seqNo:6818 2021-02-13T00:36:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.13 , 102.65 , 117.52 , 81.02 2021-02-13T00:36:14Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.08 , 0.21 2021-02-13T00:36:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176574.30 epoch sec; seqNo:14102; 4059580.47 north; 592824.10 east 2021-02-13T00:36:14Z- LcmTrn::handleMessages() - handling 45 messages took 0.69 ms and 131 calls... 2021-02-13T00:36:14Z- waiting for 1613176572.02, time is 1613176574.30 2021-02-13T00:36:14Z- LcmTrn::updateTrn() equating pose 1613176574.30 and meas times 1613176574.30 2021-02-13T00:36:14Z- LcmTrn::updateTrn() >>>> pose time 1613176574.30, meas time 1613176574.30 <<<< 2021-02-13T00:36:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176574.30 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:36:14Z- TerrainNav::motionUpdate() - currEstimate = (4059580, 592824.10, 8.11) 2021-02-13T00:36:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:36:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:36:14Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176574.30 sec.; No relevant navigation data available 2021-02-13T00:36:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:36:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:36:14Z- variances([0], [2], and psi_berg) = 695.508718 832.995638 0.000000 2021-02-13T00:36:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:36:14Z- LcmTrn::publishEstimates() - MLE : 4059579.97 592797.60 8.11 2021-02-13T00:36:14Z- LcmTrn::publishEstimates() - MMSE : 4059580.47 592824.10 8.11 2021-02-13T00:36:14Z- LcmTrn::publishEstimates() - COVAR: 695.51 833.00 1.20 0.00 2021-02-13T00:36:14Z- AHRS_M2 msg: 1613176574.66 epoch sec; seqNo:13991 2021-02-13T00:36:14Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.16 psi 2021-02-13T00:36:14Z- handleDepth()-Depth_Keller msg: 1613176574673 epoch sec; seqNo:14103; depth 7.99 2021-02-13T00:36:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176574.68 epoch sec; seqNo:14103; 4059580.87 north; 592825.32 east 2021-02-13T00:36:15Z- AHRS_M2 msg: 1613176575.07 epoch sec; seqNo:13992 2021-02-13T00:36:15Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.15 psi 2021-02-13T00:36:15Z- handleDepth()-Depth_Keller msg: 1613176575081 epoch sec; seqNo:14104; depth 7.87 2021-02-13T00:36:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176575.09 epoch sec; seqNo:14104; 4059581.25 north; 592825.31 east 2021-02-13T00:36:15Z- AHRS_M2 msg: 1613176575.47 epoch sec; seqNo:13993 2021-02-13T00:36:15Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.15 psi 2021-02-13T00:36:15Z- handleDepth()-Depth_Keller msg: 1613176575476 epoch sec; seqNo:14105; depth 7.76 2021-02-13T00:36:15Z- handleDvl() - RDI_Pathfinder msg: 1613176575.49 epoch sec; seqNo:6819 2021-02-13T00:36:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 105.35 , 122.93 , 82.37 2021-02-13T00:36:15Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.16 , 0.09 2021-02-13T00:36:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176575.51 epoch sec; seqNo:14105; 4059581.62 north; 592825.31 east 2021-02-13T00:36:15Z- AHRS_M2 msg: 1613176575.88 epoch sec; seqNo:13994 2021-02-13T00:36:15Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.15 psi 2021-02-13T00:36:15Z- handleDepth()-Depth_Keller msg: 1613176575880 epoch sec; seqNo:14106; depth 7.63 2021-02-13T00:36:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176575.89 epoch sec; seqNo:14106; 4059582.00 north; 592825.31 east 2021-02-13T00:36:16Z- AHRS_M2 msg: 1613176576.28 epoch sec; seqNo:13995 2021-02-13T00:36:16Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-13T00:36:16Z- handleDepth()-Depth_Keller msg: 1613176576284 epoch sec; seqNo:14107; depth 7.52 2021-02-13T00:36:16Z- handleDvl() - RDI_Pathfinder msg: 1613176576.30 epoch sec; seqNo:6820 2021-02-13T00:36:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 105.35 , 124.68 , 81.56 2021-02-13T00:36:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.14 , 0.00 2021-02-13T00:36:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176576.31 epoch sec; seqNo:14107; 4059582.02 north; 592826.53 east 2021-02-13T00:36:16Z- AHRS_M2 msg: 1613176576.68 epoch sec; seqNo:13996 2021-02-13T00:36:16Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-13T00:36:16Z- handleDepth()-Depth_Keller msg: 1613176576694 epoch sec; seqNo:14108; depth 7.41 2021-02-13T00:36:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176576.70 epoch sec; seqNo:14108; 4059582.39 north; 592826.52 east 2021-02-13T00:36:17Z- AHRS_M2 msg: 1613176577.09 epoch sec; seqNo:13997 2021-02-13T00:36:17Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.13 psi 2021-02-13T00:36:17Z- handleDepth()-Depth_Keller msg: 1613176577096 epoch sec; seqNo:14109; depth 7.28 2021-02-13T00:36:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176577.12 epoch sec; seqNo:14109; 4059582.77 north; 592826.52 east 2021-02-13T00:36:17Z- AHRS_M2 msg: 1613176577.49 epoch sec; seqNo:13998 2021-02-13T00:36:17Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.13 psi 2021-02-13T00:36:17Z- handleDepth()-Depth_Keller msg: 1613176577498 epoch sec; seqNo:14110; depth 7.16 2021-02-13T00:36:17Z- handleDvl() - RDI_Pathfinder msg: 1613176577.51 epoch sec; seqNo:6821 2021-02-13T00:36:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 103.86 , 121.71 , 81.56 2021-02-13T00:36:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.13 , 0.01 2021-02-13T00:36:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176577.52 epoch sec; seqNo:14110; 4059582.77 north; 592826.52 east 2021-02-13T00:36:17Z- AHRS_M2 msg: 1613176577.90 epoch sec; seqNo:13999 2021-02-13T00:36:17Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-13T00:36:17Z- handleDepth()-Depth_Keller msg: 1613176577900 epoch sec; seqNo:14111; depth 7.02 2021-02-13T00:36:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176577.92 epoch sec; seqNo:14111; 4059583.15 north; 592826.51 east 2021-02-13T00:36:18Z- AHRS_M2 msg: 1613176578.30 epoch sec; seqNo:14000 2021-02-13T00:36:18Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.16 psi 2021-02-13T00:36:18Z- handleDepth()-Depth_Keller msg: 1613176578304 epoch sec; seqNo:14112; depth 6.90 2021-02-13T00:36:18Z- handleDvl() - RDI_Pathfinder msg: 1613176578.32 epoch sec; seqNo:6822 2021-02-13T00:36:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.55 , 105.42 , 118.80 , 0.00 2021-02-13T00:36:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.13 , -0.04 2021-02-13T00:36:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176578.34 epoch sec; seqNo:14112; 4059583.54 north; 592827.73 east 2021-02-13T00:36:18Z- AHRS_M2 msg: 1613176578.70 epoch sec; seqNo:14001 2021-02-13T00:36:18Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.17 psi 2021-02-13T00:36:18Z- handleDepth()-Depth_Keller msg: 1613176578708 epoch sec; seqNo:14113; depth 6.78 2021-02-13T00:36:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176578.72 epoch sec; seqNo:14113; 4059583.54 north; 592827.73 east 2021-02-13T00:36:19Z- AHRS_M2 msg: 1613176579.11 epoch sec; seqNo:14002 2021-02-13T00:36:19Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.17 psi 2021-02-13T00:36:19Z- handleDepth()-Depth_Keller msg: 1613176579127 epoch sec; seqNo:14114; depth 6.67 2021-02-13T00:36:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176579.15 epoch sec; seqNo:14114; 4059583.92 north; 592827.73 east 2021-02-13T00:36:19Z- LcmTrn::handleMessages() - handling 40 messages took 0.63 ms and 128 calls... 2021-02-13T00:36:19Z- waiting for 1613176577.30, time is 1613176579.15 2021-02-13T00:36:19Z- LcmTrn::updateTrn() >>>> pose time 1613176579.15, meas time 1613176578.32 <<<< 2021-02-13T00:36:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176578.32 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:36:19Z- TerrainNav::motionUpdate() - currEstimate = (4059584, 592827.73, 6.67) 2021-02-13T00:36:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:36:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:36:19Z- TNavBF::Projecting Measurements 2021-02-13T00:36:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:36:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:36:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:36:19Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:36:19Z- TNavFilter::3 good beams. 2021-02-13T00:36:19Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:36:19Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:36:19Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:36:19Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:36:19Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:36:19Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:36:19Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:36:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:36:19Z- TerrainMapOctree: (vehN, vehE ) = (4059583.33, 592827.10). 2021-02-13T00:36:19Z- TerrainMapOctree: Min Distance = 2191.45. 2021-02-13T00:36:19Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:36:19Z- There is no specific good beam that all particles have in common. 2021-02-13T00:36:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:36:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.41 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 13.12 2021-02-13T00:36:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:36:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 17.78 2021-02-13T00:36:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:36:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 22.58 2021-02-13T00:36:19Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:36:19Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:36:19Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:36:19Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:36:19Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:36:19Z- Filter 0 No beams used in subcloud update 2021-02-13T00:36:19Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:36:19Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:36:19Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 695.5 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:36:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176578.32 sec. 2021-02-13T00:36:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:36:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:36:19Z- variances([0], [2], and psi_berg) = 695.432255 832.995600 0.000000 2021-02-13T00:36:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:36:19Z- LcmTrn::publishEstimates() - MLE : 4059583.42 592801.23 6.67 2021-02-13T00:36:19Z- LcmTrn::publishEstimates() - MMSE : 4059583.92 592827.73 6.67 2021-02-13T00:36:19Z- LcmTrn::publishEstimates() - COVAR: 695.43 833.00 1.19 0.00 2021-02-13T00:36:19Z- AHRS_M2 msg: 1613176579.51 epoch sec; seqNo:14003 2021-02-13T00:36:19Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-13T00:36:19Z- handleDepth()-Depth_Keller msg: 1613176579516 epoch sec; seqNo:14115; depth 6.55 2021-02-13T00:36:19Z- handleDvl() - RDI_Pathfinder msg: 1613176579.53 epoch sec; seqNo:6823 2021-02-13T00:36:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 109.61 , 121.51 , 84.33 2021-02-13T00:36:19Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.17 , -0.14 2021-02-13T00:36:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176579.54 epoch sec; seqNo:14115; 4059583.92 north; 592827.73 east 2021-02-13T00:36:19Z- AHRS_M2 msg: 1613176579.92 epoch sec; seqNo:14004 2021-02-13T00:36:19Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.16 psi 2021-02-13T00:36:19Z- handleDepth()-Depth_Keller msg: 1613176579920 epoch sec; seqNo:14116; depth 6.46 2021-02-13T00:36:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176579.93 epoch sec; seqNo:14116; 4059584.30 north; 592827.72 east 2021-02-13T00:36:20Z- AHRS_M2 msg: 1613176580.32 epoch sec; seqNo:14005 2021-02-13T00:36:20Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.15 psi 2021-02-13T00:36:20Z- handleDepth()-Depth_Keller msg: 1613176580324 epoch sec; seqNo:14117; depth 6.36 2021-02-13T00:36:20Z- handleDvl() - RDI_Pathfinder msg: 1613176580.34 epoch sec; seqNo:6824 2021-02-13T00:36:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.90 , 109.75 , 121.64 , 82.98 2021-02-13T00:36:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.08 , -0.38 2021-02-13T00:36:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176580.35 epoch sec; seqNo:14117; 4059584.68 north; 592827.72 east 2021-02-13T00:36:20Z- AHRS_M2 msg: 1613176580.72 epoch sec; seqNo:14006 2021-02-13T00:36:20Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-13T00:36:20Z- handleDepth()-Depth_Keller msg: 1613176580731 epoch sec; seqNo:14118; depth 6.23 2021-02-13T00:36:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176580.74 epoch sec; seqNo:14118; 4059584.69 north; 592828.94 east 2021-02-13T00:36:21Z- AHRS_M2 msg: 1613176581.15 epoch sec; seqNo:14007 2021-02-13T00:36:21Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.19 psi 2021-02-13T00:36:21Z- handleDepth()-Depth_Keller msg: 1613176581159 epoch sec; seqNo:14119; depth 6.13 2021-02-13T00:36:21Z- handleDvl() - RDI_Pathfinder msg: 1613176581.17 epoch sec; seqNo:6825 2021-02-13T00:36:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.04 , 106.91 , 123.27 , 81.62 2021-02-13T00:36:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.28 , -0.43 2021-02-13T00:36:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176581.18 epoch sec; seqNo:14119; 4059585.07 north; 592828.93 east 2021-02-13T00:36:21Z- handleDepth()-Depth_Keller msg: 1613176581553 epoch sec; seqNo:14120; depth 5.98 2021-02-13T00:36:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176581.57 epoch sec; seqNo:14120; 4059585.45 north; 592828.93 east 2021-02-13T00:36:21Z- AHRS_M2 msg: 1613176581.95 epoch sec; seqNo:14008 2021-02-13T00:36:21Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.22 psi 2021-02-13T00:36:21Z- handleDepth()-Depth_Keller msg: 1613176581955 epoch sec; seqNo:14121; depth 5.84 2021-02-13T00:36:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176581.96 epoch sec; seqNo:14121; 4059585.45 north; 592828.93 east 2021-02-13T00:36:22Z- AHRS_M2 msg: 1613176582.34 epoch sec; seqNo:14009 2021-02-13T00:36:22Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.21 psi 2021-02-13T00:36:22Z- handleDepth()-Depth_Keller msg: 1613176582346 epoch sec; seqNo:14122; depth 5.71 2021-02-13T00:36:22Z- handleDvl() - RDI_Pathfinder msg: 1613176582.36 epoch sec; seqNo:6826 2021-02-13T00:36:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 111.84 , 117.25 , 83.45 2021-02-13T00:36:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.56 , 0.03 2021-02-13T00:36:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176582.37 epoch sec; seqNo:14122; 4059585.83 north; 592828.93 east 2021-02-13T00:36:22Z- AHRS_M2 msg: 1613176582.74 epoch sec; seqNo:14010 2021-02-13T00:36:22Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.20 psi 2021-02-13T00:36:22Z- handleDepth()-Depth_Keller msg: 1613176582748 epoch sec; seqNo:14123; depth 5.59 2021-02-13T00:36:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176582.77 epoch sec; seqNo:14123; 4059586.21 north; 592828.92 east 2021-02-13T00:36:23Z- AHRS_M2 msg: 1613176583.16 epoch sec; seqNo:14011 2021-02-13T00:36:23Z- AHRS_M2 msg: 0.59 phi; 0.29 theta; -0.20 psi 2021-02-13T00:36:23Z- handleDepth()-Depth_Keller msg: 1613176583162 epoch sec; seqNo:14124; depth 5.45 2021-02-13T00:36:23Z- handleDvl() - RDI_Pathfinder msg: 1613176583.18 epoch sec; seqNo:6827 2021-02-13T00:36:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 109.81 , 118.74 , 84.53 2021-02-13T00:36:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.34 , 0.56 2021-02-13T00:36:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176583.19 epoch sec; seqNo:14124; 4059586.59 north; 592828.92 east 2021-02-13T00:36:23Z- AHRS_M2 msg: 1613176583.55 epoch sec; seqNo:14012 2021-02-13T00:36:23Z- AHRS_M2 msg: 0.60 phi; 0.27 theta; -0.18 psi 2021-02-13T00:36:23Z- handleDepth()-Depth_Keller msg: 1613176583558 epoch sec; seqNo:14125; depth 5.33 2021-02-13T00:36:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176583.57 epoch sec; seqNo:14125; 4059586.97 north; 592828.91 east 2021-02-13T00:36:23Z- AHRS_M2 msg: 1613176583.96 epoch sec; seqNo:14013 2021-02-13T00:36:23Z- AHRS_M2 msg: 0.61 phi; 0.24 theta; -0.16 psi 2021-02-13T00:36:23Z- handleDepth()-Depth_Keller msg: 1613176583964 epoch sec; seqNo:14126; depth 5.21 2021-02-13T00:36:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176583.98 epoch sec; seqNo:14126; 4059587.35 north; 592828.91 east 2021-02-13T00:36:24Z- AHRS_M2 msg: 1613176584.36 epoch sec; seqNo:14014 2021-02-13T00:36:24Z- AHRS_M2 msg: 0.62 phi; 0.20 theta; -0.13 psi 2021-02-13T00:36:24Z- handleDepth()-Depth_Keller msg: 1613176584365 epoch sec; seqNo:14127; depth 5.09 2021-02-13T00:36:24Z- handleDvl() - RDI_Pathfinder msg: 1613176584.38 epoch sec; seqNo:6828 2021-02-13T00:36:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 105.08 , 115.90 , 0.00 2021-02-13T00:36:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.08 , 0.27 , 0.62 2021-02-13T00:36:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176584.40 epoch sec; seqNo:14127; 4059587.73 north; 592828.91 east 2021-02-13T00:36:24Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-13T00:36:24Z- waiting for 1613176582.15, time is 1613176584.40 2021-02-13T00:36:24Z- LcmTrn::updateTrn() equating pose 1613176584.40 and meas times 1613176584.40 2021-02-13T00:36:24Z- LcmTrn::updateTrn() >>>> pose time 1613176584.40, meas time 1613176584.40 <<<< 2021-02-13T00:36:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176584.40 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:36:24Z- TerrainNav::motionUpdate() - currEstimate = (4059588, 592828.91, 5.09) 2021-02-13T00:36:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:36:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:36:24Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176584.40 sec.; No relevant navigation data available 2021-02-13T00:36:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:36:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:36:24Z- variances([0], [2], and psi_berg) = 695.380822 832.995574 0.000000 2021-02-13T00:36:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:36:24Z- LcmTrn::publishEstimates() - MLE : 4059587.23 592860.41 5.09 2021-02-13T00:36:24Z- LcmTrn::publishEstimates() - MMSE : 4059587.73 592828.91 5.09 2021-02-13T00:36:24Z- LcmTrn::publishEstimates() - COVAR: 695.38 833.00 1.40 0.00 2021-02-13T00:36:24Z- AHRS_M2 msg: 1613176584.77 epoch sec; seqNo:14015 2021-02-13T00:36:24Z- AHRS_M2 msg: 0.63 phi; 0.16 theta; -0.12 psi 2021-02-13T00:36:24Z- handleDepth()-Depth_Keller msg: 1613176584774 epoch sec; seqNo:14128; depth 4.98 2021-02-13T00:36:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176584.79 epoch sec; seqNo:14128; 4059588.12 north; 592830.12 east 2021-02-13T00:36:25Z- AHRS_M2 msg: 1613176585.17 epoch sec; seqNo:14016 2021-02-13T00:36:25Z- AHRS_M2 msg: 0.64 phi; 0.12 theta; -0.14 psi 2021-02-13T00:36:25Z- handleDepth()-Depth_Keller msg: 1613176585178 epoch sec; seqNo:14129; depth 4.89 2021-02-13T00:36:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176585.20 epoch sec; seqNo:14129; 4059588.50 north; 592830.12 east 2021-02-13T00:36:25Z- AHRS_M2 msg: 1613176585.57 epoch sec; seqNo:14017 2021-02-13T00:36:25Z- AHRS_M2 msg: 0.65 phi; 0.08 theta; -0.16 psi 2021-02-13T00:36:25Z- handleDepth()-Depth_Keller msg: 1613176585577 epoch sec; seqNo:14130; depth 4.78 2021-02-13T00:36:25Z- handleDvl() - RDI_Pathfinder msg: 1613176585.60 epoch sec; seqNo:6829 2021-02-13T00:36:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.14 , 102.04 , 106.50 , 87.17 2021-02-13T00:36:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.48 , 0.18 2021-02-13T00:36:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176585.61 epoch sec; seqNo:14130; 4059588.50 north; 592830.12 east 2021-02-13T00:36:25Z- AHRS_M2 msg: 1613176585.98 epoch sec; seqNo:14018 2021-02-13T00:36:25Z- AHRS_M2 msg: 0.66 phi; 0.05 theta; -0.18 psi 2021-02-13T00:36:25Z- handleDepth()-Depth_Keller msg: 1613176585980 epoch sec; seqNo:14131; depth 4.68 2021-02-13T00:36:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176585.99 epoch sec; seqNo:14131; 4059588.89 north; 592831.33 east 2021-02-13T00:36:26Z- AHRS_M2 msg: 1613176586.38 epoch sec; seqNo:14019 2021-02-13T00:36:26Z- AHRS_M2 msg: 0.68 phi; 0.01 theta; -0.21 psi 2021-02-13T00:36:26Z- handleDepth()-Depth_Keller msg: 1613176586384 epoch sec; seqNo:14132; depth 4.62 2021-02-13T00:36:26Z- handleDvl() - RDI_Pathfinder msg: 1613176586.40 epoch sec; seqNo:6830 2021-02-13T00:36:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.20 , 104.07 , 100.01 , 97.31 2021-02-13T00:36:26Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.29 , -0.21 2021-02-13T00:36:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176586.41 epoch sec; seqNo:14132; 4059588.89 north; 592831.33 east 2021-02-13T00:36:26Z- AHRS_M2 msg: 1613176586.78 epoch sec; seqNo:14020 2021-02-13T00:36:26Z- AHRS_M2 msg: 0.70 phi; -0.02 theta; -0.22 psi 2021-02-13T00:36:26Z- handleDepth()-Depth_Keller msg: 1613176586794 epoch sec; seqNo:14133; depth 4.54 2021-02-13T00:36:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176586.81 epoch sec; seqNo:14133; 4059589.27 north; 592831.33 east 2021-02-13T00:36:27Z- AHRS_M2 msg: 1613176587.22 epoch sec; seqNo:14021 2021-02-13T00:36:27Z- AHRS_M2 msg: 0.71 phi; -0.05 theta; -0.22 psi 2021-02-13T00:36:27Z- handleDepth()-Depth_Keller msg: 1613176587226 epoch sec; seqNo:14134; depth 4.51 2021-02-13T00:36:27Z- handleDvl() - RDI_Pathfinder msg: 1613176587.24 epoch sec; seqNo:6831 2021-02-13T00:36:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.42 , 109.41 , 97.24 , 97.24 2021-02-13T00:36:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.02 , -0.25 2021-02-13T00:36:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176587.25 epoch sec; seqNo:14134; 4059589.65 north; 592831.33 east 2021-02-13T00:36:27Z- AHRS_M2 msg: 1613176587.59 epoch sec; seqNo:14022 2021-02-13T00:36:27Z- AHRS_M2 msg: 0.72 phi; -0.08 theta; -0.22 psi 2021-02-13T00:36:27Z- handleDepth()-Depth_Keller msg: 1613176587596 epoch sec; seqNo:14135; depth 4.48 2021-02-13T00:36:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176587.61 epoch sec; seqNo:14135; 4059590.04 north; 592832.54 east 2021-02-13T00:36:27Z- AHRS_M2 msg: 1613176588.00 epoch sec; seqNo:14023 2021-02-13T00:36:27Z- AHRS_M2 msg: 0.73 phi; -0.10 theta; -0.21 psi 2021-02-13T00:36:28Z- handleDepth()-Depth_Keller msg: 1613176588001 epoch sec; seqNo:14136; depth 4.48 2021-02-13T00:36:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176588.01 epoch sec; seqNo:14136; 4059590.04 north; 592832.54 east 2021-02-13T00:36:28Z- AHRS_M2 msg: 1613176588.40 epoch sec; seqNo:14024 2021-02-13T00:36:28Z- AHRS_M2 msg: 0.74 phi; -0.13 theta; -0.20 psi 2021-02-13T00:36:28Z- handleDepth()-Depth_Keller msg: 1613176588414 epoch sec; seqNo:14137; depth 4.46 2021-02-13T00:36:28Z- handleDvl() - RDI_Pathfinder msg: 1613176588.43 epoch sec; seqNo:6832 2021-02-13T00:36:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.42 , 109.41 , 97.24 , 101.30 2021-02-13T00:36:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.13 , -0.11 2021-02-13T00:36:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176588.44 epoch sec; seqNo:14137; 4059590.42 north; 592832.54 east 2021-02-13T00:36:28Z- AHRS_M2 msg: 1613176588.80 epoch sec; seqNo:14025 2021-02-13T00:36:28Z- AHRS_M2 msg: 0.74 phi; -0.16 theta; -0.19 psi 2021-02-13T00:36:28Z- handleDepth()-Depth_Keller msg: 1613176588808 epoch sec; seqNo:14138; depth 4.47 2021-02-13T00:36:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176588.82 epoch sec; seqNo:14138; 4059590.80 north; 592832.53 east 2021-02-13T00:36:29Z- AHRS_M2 msg: 1613176589.23 epoch sec; seqNo:14026 2021-02-13T00:36:29Z- AHRS_M2 msg: 0.74 phi; -0.20 theta; -0.18 psi 2021-02-13T00:36:29Z- handleDepth()-Depth_Keller msg: 1613176589231 epoch sec; seqNo:14139; depth 4.49 2021-02-13T00:36:29Z- handleDvl() - RDI_Pathfinder msg: 1613176589.24 epoch sec; seqNo:6833 2021-02-13T00:36:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.36 , 107.99 , 89.06 , 106.64 2021-02-13T00:36:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , -0.08 , 0.05 2021-02-13T00:36:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176589.25 epoch sec; seqNo:14139; 4059591.17 north; 592832.53 east 2021-02-13T00:36:29Z- LcmTrn::handleMessages() - handling 41 messages took 0.59 ms and 130 calls... 2021-02-13T00:36:29Z- waiting for 1613176587.40, time is 1613176589.25 2021-02-13T00:36:29Z- LcmTrn::updateTrn() equating pose 1613176589.25 and meas times 1613176589.25 2021-02-13T00:36:29Z- LcmTrn::updateTrn() >>>> pose time 1613176589.25, meas time 1613176589.25 <<<< 2021-02-13T00:36:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176589.25 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:36:29Z- TerrainNav::motionUpdate() - currEstimate = (4059591, 592832.53, 4.49) 2021-02-13T00:36:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:36:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:36:29Z- TNavBF::Projecting Measurements 2021-02-13T00:36:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:36:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:36:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:36:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:36:29Z- TNavFilter::4 good beams. 2021-02-13T00:36:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:36:29Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:36:29Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:36:29Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:36:29Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:36:29Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:36:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:36:29Z- TNavBF::mmse selected from filter -1 2021-02-13T00:36:29Z- TerrainMapOctree: (vehN, vehE ) = (4059591.17, 592832.53). 2021-02-13T00:36:29Z- TerrainMapOctree: Min Distance = 2181.95. 2021-02-13T00:36:29Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:36:29Z- There is no specific good beam that all particles have in common. 2021-02-13T00:36:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:36:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.65 K: 0.108 ExpectedMeasDiff: nan currMeas.covariance: 11.93 2021-02-13T00:36:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:36:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.47 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 18.66 2021-02-13T00:36:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:36:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 12.69 2021-02-13T00:36:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:36:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 18.19 2021-02-13T00:36:29Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:36:29Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:36:29Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:36:29Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:36:29Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:36:29Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:36:29Z- Filter 0 No beams used in subcloud update 2021-02-13T00:36:29Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:36:29Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:36:29Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 695.3 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:36:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176589.25 sec. 2021-02-13T00:36:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:36:29Z- TNavBF::mmse selected from filter -1 2021-02-13T00:36:29Z- variances([0], [2], and psi_berg) = 695.323187 832.995545 0.000000 2021-02-13T00:36:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:36:29Z- LcmTrn::publishEstimates() - MLE : 4059590.67 592864.03 4.49 2021-02-13T00:36:29Z- LcmTrn::publishEstimates() - MMSE : 4059591.17 592832.53 4.49 2021-02-13T00:36:29Z- LcmTrn::publishEstimates() - COVAR: 695.32 833.00 1.18 0.00 2021-02-13T00:36:29Z- AHRS_M2 msg: 1613176589.61 epoch sec; seqNo:14027 2021-02-13T00:36:29Z- AHRS_M2 msg: 0.73 phi; -0.23 theta; -0.18 psi 2021-02-13T00:36:29Z- handleDepth()-Depth_Keller msg: 1613176589626 epoch sec; seqNo:14140; depth 4.51 2021-02-13T00:36:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176589.64 epoch sec; seqNo:14140; 4059591.17 north; 592832.53 east 2021-02-13T00:36:30Z- AHRS_M2 msg: 1613176590.02 epoch sec; seqNo:14028 2021-02-13T00:36:30Z- AHRS_M2 msg: 0.72 phi; -0.26 theta; -0.20 psi 2021-02-13T00:36:30Z- handleDepth()-Depth_Keller msg: 1613176590025 epoch sec; seqNo:14141; depth 4.52 2021-02-13T00:36:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176590.03 epoch sec; seqNo:14141; 4059591.55 north; 592832.53 east 2021-02-13T00:36:30Z- AHRS_M2 msg: 1613176590.42 epoch sec; seqNo:14029 2021-02-13T00:36:30Z- AHRS_M2 msg: 0.71 phi; -0.29 theta; -0.21 psi 2021-02-13T00:36:30Z- handleDepth()-Depth_Keller msg: 1613176590437 epoch sec; seqNo:14142; depth 4.55 2021-02-13T00:36:30Z- handleDvl() - RDI_Pathfinder msg: 1613176590.45 epoch sec; seqNo:6834 2021-02-13T00:36:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:36:30Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:36:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176590.48 epoch sec; seqNo:14142; 4059591.57 north; 592833.75 east 2021-02-13T00:36:30Z- AHRS_M2 msg: 1613176590.83 epoch sec; seqNo:14030 2021-02-13T00:36:30Z- AHRS_M2 msg: 0.69 phi; -0.31 theta; -0.24 psi 2021-02-13T00:36:30Z- handleDepth()-Depth_Keller msg: 1613176590830 epoch sec; seqNo:14143; depth 4.58 2021-02-13T00:36:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176590.84 epoch sec; seqNo:14143; 4059591.95 north; 592833.74 east 2021-02-13T00:36:31Z- AHRS_M2 msg: 1613176591.24 epoch sec; seqNo:14031 2021-02-13T00:36:31Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.26 psi 2021-02-13T00:36:31Z- handleDepth()-Depth_Keller msg: 1613176591252 epoch sec; seqNo:14144; depth 4.62 2021-02-13T00:36:31Z- handleDvl() - RDI_Pathfinder msg: 1613176591.27 epoch sec; seqNo:6835 2021-02-13T00:36:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 112.11 , 85.34 , 124.01 2021-02-13T00:36:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.03 , 0.29 2021-02-13T00:36:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176591.28 epoch sec; seqNo:14144; 4059591.95 north; 592833.74 east 2021-02-13T00:36:31Z- AHRS_M2 msg: 1613176591.63 epoch sec; seqNo:14032 2021-02-13T00:36:31Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.29 psi 2021-02-13T00:36:31Z- handleDepth()-Depth_Keller msg: 1613176591636 epoch sec; seqNo:14145; depth 4.69 2021-02-13T00:36:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176591.65 epoch sec; seqNo:14145; 4059592.32 north; 592833.74 east 2021-02-13T00:36:32Z- AHRS_M2 msg: 1613176592.04 epoch sec; seqNo:14033 2021-02-13T00:36:32Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.31 psi 2021-02-13T00:36:32Z- handleDepth()-Depth_Keller msg: 1613176592045 epoch sec; seqNo:14146; depth 4.77 2021-02-13T00:36:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176592.05 epoch sec; seqNo:14146; 4059592.32 north; 592833.74 east 2021-02-13T00:36:32Z- AHRS_M2 msg: 1613176592.44 epoch sec; seqNo:14034 2021-02-13T00:36:32Z- AHRS_M2 msg: 0.66 phi; -0.31 theta; -0.30 psi 2021-02-13T00:36:32Z- handleDepth()-Depth_Keller msg: 1613176592445 epoch sec; seqNo:14147; depth 4.86 2021-02-13T00:36:32Z- handleDvl() - RDI_Pathfinder msg: 1613176592.46 epoch sec; seqNo:6836 2021-02-13T00:36:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.28 , 118.53 , 85.82 , 128.94 2021-02-13T00:36:32Z- handleDvl() - RDI_Pathfinder msg: velocities 1.12 , 0.16 , 0.20 2021-02-13T00:36:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176592.47 epoch sec; seqNo:14147; 4059592.70 north; 592833.73 east 2021-02-13T00:36:32Z- AHRS_M2 msg: 1613176592.84 epoch sec; seqNo:14035 2021-02-13T00:36:32Z- AHRS_M2 msg: 0.66 phi; -0.31 theta; -0.29 psi 2021-02-13T00:36:32Z- handleDepth()-Depth_Keller msg: 1613176592854 epoch sec; seqNo:14148; depth 4.97 2021-02-13T00:36:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176592.87 epoch sec; seqNo:14148; 4059593.10 north; 592834.95 east 2021-02-13T00:36:33Z- AHRS_M2 msg: 1613176593.25 epoch sec; seqNo:14036 2021-02-13T00:36:33Z- AHRS_M2 msg: 0.66 phi; -0.31 theta; -0.26 psi 2021-02-13T00:36:33Z- handleDepth()-Depth_Keller msg: 1613176593253 epoch sec; seqNo:14149; depth 5.09 2021-02-13T00:36:33Z- handleDvl() - RDI_Pathfinder msg: 1613176593.27 epoch sec; seqNo:6837 2021-02-13T00:36:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.58 , 124.35 , 0.00 , 127.32 2021-02-13T00:36:33Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , 0.24 , -0.14 2021-02-13T00:36:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176593.28 epoch sec; seqNo:14149; 4059593.47 north; 592834.95 east 2021-02-13T00:36:33Z- AHRS_M2 msg: 1613176593.65 epoch sec; seqNo:14037 2021-02-13T00:36:33Z- AHRS_M2 msg: 0.67 phi; -0.32 theta; -0.24 psi 2021-02-13T00:36:33Z- handleDepth()-Depth_Keller msg: 1613176593656 epoch sec; seqNo:14150; depth 5.22 2021-02-13T00:36:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176593.67 epoch sec; seqNo:14150; 4059593.47 north; 592834.95 east 2021-02-13T00:36:34Z- AHRS_M2 msg: 1613176594.06 epoch sec; seqNo:14038 2021-02-13T00:36:34Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.22 psi 2021-02-13T00:36:34Z- handleDepth()-Depth_Keller msg: 1613176594061 epoch sec; seqNo:14151; depth 5.32 2021-02-13T00:36:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176594.07 epoch sec; seqNo:14151; 4059593.87 north; 592836.16 east 2021-02-13T00:36:34Z- AHRS_M2 msg: 1613176594.46 epoch sec; seqNo:14039 2021-02-13T00:36:34Z- AHRS_M2 msg: 0.68 phi; -0.34 theta; -0.22 psi 2021-02-13T00:36:34Z- handleDepth()-Depth_Keller msg: 1613176594469 epoch sec; seqNo:14152; depth 5.43 2021-02-13T00:36:34Z- handleDvl() - RDI_Pathfinder msg: 1613176594.49 epoch sec; seqNo:6838 2021-02-13T00:36:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.80 , 113.60 , 83.86 , 124.01 2021-02-13T00:36:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.16 , -0.45 2021-02-13T00:36:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176594.50 epoch sec; seqNo:14152; 4059594.24 north; 592836.16 east 2021-02-13T00:36:34Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-13T00:36:34Z- waiting for 1613176592.25, time is 1613176594.50 2021-02-13T00:36:34Z- LcmTrn::updateTrn() equating pose 1613176594.50 and meas times 1613176594.50 2021-02-13T00:36:34Z- LcmTrn::updateTrn() >>>> pose time 1613176594.50, meas time 1613176594.50 <<<< 2021-02-13T00:36:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176594.50 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:36:34Z- TerrainNav::motionUpdate() - currEstimate = (4059594, 592836.16, 5.43) 2021-02-13T00:36:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:36:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:36:34Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176594.50 sec.; No relevant navigation data available 2021-02-13T00:36:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:36:34Z- TNavBF::mmse selected from filter -1 2021-02-13T00:36:34Z- variances([0], [2], and psi_berg) = 695.267045 832.995517 0.000000 2021-02-13T00:36:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:36:34Z- LcmTrn::publishEstimates() - MLE : 4059593.74 592810.66 5.43 2021-02-13T00:36:34Z- LcmTrn::publishEstimates() - MMSE : 4059594.24 592836.16 5.43 2021-02-13T00:36:34Z- LcmTrn::publishEstimates() - COVAR: 695.27 833.00 1.42 0.00 2021-02-13T00:36:34Z- AHRS_M2 msg: 1613176594.86 epoch sec; seqNo:14040 2021-02-13T00:36:34Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.22 psi 2021-02-13T00:36:34Z- handleDepth()-Depth_Keller msg: 1613176594868 epoch sec; seqNo:14153; depth 5.54 2021-02-13T00:36:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176594.88 epoch sec; seqNo:14153; 4059594.62 north; 592836.16 east 2021-02-13T00:36:35Z- AHRS_M2 msg: 1613176595.28 epoch sec; seqNo:14041 2021-02-13T00:36:35Z- AHRS_M2 msg: 0.68 phi; -0.36 theta; -0.23 psi 2021-02-13T00:36:35Z- handleDepth()-Depth_Keller msg: 1613176595287 epoch sec; seqNo:14154; depth 5.64 2021-02-13T00:36:35Z- handleDvl() - RDI_Pathfinder msg: 1613176595.30 epoch sec; seqNo:6839 2021-02-13T00:36:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.41 , 114.21 , 82.98 , 126.11 2021-02-13T00:36:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.22 , -0.42 2021-02-13T00:36:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176595.31 epoch sec; seqNo:14154; 4059594.62 north; 592836.16 east 2021-02-13T00:36:35Z- AHRS_M2 msg: 1613176595.67 epoch sec; seqNo:14042 2021-02-13T00:36:35Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.22 psi 2021-02-13T00:36:35Z- handleDepth()-Depth_Keller msg: 1613176595676 epoch sec; seqNo:14155; depth 5.74 2021-02-13T00:36:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176595.69 epoch sec; seqNo:14155; 4059595.00 north; 592836.15 east 2021-02-13T00:36:36Z- AHRS_M2 msg: 1613176596.08 epoch sec; seqNo:14043 2021-02-13T00:36:36Z- AHRS_M2 msg: 0.69 phi; -0.37 theta; -0.22 psi 2021-02-13T00:36:36Z- handleDepth()-Depth_Keller msg: 1613176596083 epoch sec; seqNo:14156; depth 5.84 2021-02-13T00:36:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176596.09 epoch sec; seqNo:14156; 4059595.38 north; 592836.15 east 2021-02-13T00:36:36Z- AHRS_M2 msg: 1613176596.48 epoch sec; seqNo:14044 2021-02-13T00:36:36Z- AHRS_M2 msg: 0.68 phi; -0.38 theta; -0.22 psi 2021-02-13T00:36:36Z- handleDepth()-Depth_Keller msg: 1613176596489 epoch sec; seqNo:14157; depth 5.96 2021-02-13T00:36:36Z- handleDvl() - RDI_Pathfinder msg: 1613176596.50 epoch sec; seqNo:6840 2021-02-13T00:36:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 115.63 , 0.00 , 133.47 2021-02-13T00:36:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.44 , -0.01 2021-02-13T00:36:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176596.51 epoch sec; seqNo:14157; 4059595.76 north; 592836.14 east 2021-02-13T00:36:36Z- AHRS_M2 msg: 1613176596.88 epoch sec; seqNo:14045 2021-02-13T00:36:36Z- AHRS_M2 msg: 0.68 phi; -0.38 theta; -0.21 psi 2021-02-13T00:36:36Z- handleDepth()-Depth_Keller msg: 1613176596897 epoch sec; seqNo:14158; depth 6.10 2021-02-13T00:36:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176596.91 epoch sec; seqNo:14158; 4059596.14 north; 592836.14 east 2021-02-13T00:36:37Z- AHRS_M2 msg: 1613176597.29 epoch sec; seqNo:14046 2021-02-13T00:36:37Z- AHRS_M2 msg: 0.67 phi; -0.38 theta; -0.19 psi 2021-02-13T00:36:37Z- handleDepth()-Depth_Keller msg: 1613176597296 epoch sec; seqNo:14159; depth 6.22 2021-02-13T00:36:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:36:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:36:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:36:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:36:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:36:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:36:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:36:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:36:37Z- handleDvl() - RDI_Pathfinder msg: 1613176597.30 epoch sec; seqNo:6841 2021-02-13T00:36:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:36:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.44 , -0.01 2021-02-13T00:36:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176597.33 epoch sec; seqNo:14159; 4059596.52 north; 592836.13 east 2021-02-13T00:36:37Z- AHRS_M2 msg: 1613176597.69 epoch sec; seqNo:14047 2021-02-13T00:36:37Z- AHRS_M2 msg: 0.66 phi; -0.38 theta; -0.19 psi 2021-02-13T00:36:37Z- handleDepth()-Depth_Keller msg: 1613176597696 epoch sec; seqNo:14160; depth 6.36 2021-02-13T00:36:37Z- handleDvl() - RDI_Pathfinder msg: 1613176597.71 epoch sec; seqNo:6842 2021-02-13T00:36:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.29 , 113.60 , 82.37 , 135.91 2021-02-13T00:36:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.35 , 0.37 2021-02-13T00:36:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176597.74 epoch sec; seqNo:14160; 4059596.52 north; 592836.13 east 2021-02-13T00:36:38Z- AHRS_M2 msg: 1613176598.10 epoch sec; seqNo:14048 2021-02-13T00:36:38Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.18 psi 2021-02-13T00:36:38Z- handleDepth()-Depth_Keller msg: 1613176598109 epoch sec; seqNo:14161; depth 6.49 2021-02-13T00:36:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176598.13 epoch sec; seqNo:14161; 4059596.90 north; 592836.13 east 2021-02-13T00:36:38Z- AHRS_M2 msg: 1613176598.50 epoch sec; seqNo:14049 2021-02-13T00:36:38Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.18 psi 2021-02-13T00:36:38Z- handleDepth()-Depth_Keller msg: 1613176598511 epoch sec; seqNo:14162; depth 6.64 2021-02-13T00:36:38Z- handleDvl() - RDI_Pathfinder msg: 1613176598.54 epoch sec; seqNo:6843 2021-02-13T00:36:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.58 , 110.96 , 81.22 , 131.78 2021-02-13T00:36:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.02 , 0.48 2021-02-13T00:36:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176598.55 epoch sec; seqNo:14162; 4059597.29 north; 592837.35 east 2021-02-13T00:36:38Z- AHRS_M2 msg: 1613176598.91 epoch sec; seqNo:14050 2021-02-13T00:36:38Z- AHRS_M2 msg: 0.61 phi; -0.38 theta; -0.18 psi 2021-02-13T00:36:38Z- handleDepth()-Depth_Keller msg: 1613176598914 epoch sec; seqNo:14163; depth 6.79 2021-02-13T00:36:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176598.93 epoch sec; seqNo:14163; 4059597.29 north; 592837.35 east 2021-02-13T00:36:39Z- AHRS_M2 msg: 1613176599.32 epoch sec; seqNo:14051 2021-02-13T00:36:39Z- AHRS_M2 msg: 0.60 phi; -0.38 theta; -0.19 psi 2021-02-13T00:36:39Z- handleDepth()-Depth_Keller msg: 1613176599332 epoch sec; seqNo:14164; depth 6.91 2021-02-13T00:36:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176599.35 epoch sec; seqNo:14164; 4059597.29 north; 592837.35 east 2021-02-13T00:36:39Z- LcmTrn::handleMessages() - handling 41 messages took 0.70 ms and 127 calls... 2021-02-13T00:36:39Z- waiting for 1613176597.50, time is 1613176599.35 2021-02-13T00:36:39Z- LcmTrn::updateTrn() >>>> pose time 1613176599.35, meas time 1613176598.54 <<<< 2021-02-13T00:36:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176598.54 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:36:39Z- TerrainNav::motionUpdate() - currEstimate = (4059597, 592837.35, 6.91) 2021-02-13T00:36:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:36:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:36:39Z- TNavBF::Projecting Measurements 2021-02-13T00:36:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:36:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:36:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:36:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:36:39Z- TNavFilter::4 good beams. 2021-02-13T00:36:39Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:36:39Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:36:39Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:36:39Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:36:39Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:36:39Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:36:39Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:36:39Z- TNavBF::mmse selected from filter -1 2021-02-13T00:36:39Z- TerrainMapOctree: (vehN, vehE ) = (4059596.78, 592837.15). 2021-02-13T00:36:39Z- TerrainMapOctree: Min Distance = 2174.69. 2021-02-13T00:36:39Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:36:39Z- There is no specific good beam that all particles have in common. 2021-02-13T00:36:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:36:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.56 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 15.23 2021-02-13T00:36:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:36:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 19.70 2021-02-13T00:36:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:36:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 10.55 2021-02-13T00:36:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:36:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 27.79 2021-02-13T00:36:39Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:36:39Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:36:39Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:36:39Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:36:39Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:36:39Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:36:39Z- Filter 0 No beams used in subcloud update 2021-02-13T00:36:39Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:36:39Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:36:39Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 695.2 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:36:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176598.54 sec. 2021-02-13T00:36:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:36:39Z- TNavBF::mmse selected from filter -1 2021-02-13T00:36:39Z- variances([0], [2], and psi_berg) = 695.205553 832.995486 0.000000 2021-02-13T00:36:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:36:39Z- LcmTrn::publishEstimates() - MLE : 4059596.79 592867.85 6.91 2021-02-13T00:36:39Z- LcmTrn::publishEstimates() - MMSE : 4059597.29 592837.35 6.91 2021-02-13T00:36:39Z- LcmTrn::publishEstimates() - COVAR: 695.21 833.00 1.19 0.00 2021-02-13T00:36:39Z- AHRS_M2 msg: 1613176599.71 epoch sec; seqNo:14052 2021-02-13T00:36:39Z- AHRS_M2 msg: 0.59 phi; -0.37 theta; -0.20 psi 2021-02-13T00:36:39Z- handleDepth()-Depth_Keller msg: 1613176599716 epoch sec; seqNo:14165; depth 7.07 2021-02-13T00:36:39Z- handleDvl() - RDI_Pathfinder msg: 1613176599.73 epoch sec; seqNo:6844 2021-02-13T00:36:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.18 , 109.54 , 78.46 , 128.88 2021-02-13T00:36:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.32 , 0.21 2021-02-13T00:36:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176599.75 epoch sec; seqNo:14165; 4059597.67 north; 592837.34 east 2021-02-13T00:36:40Z- AHRS_M2 msg: 1613176600.12 epoch sec; seqNo:14053 2021-02-13T00:36:40Z- AHRS_M2 msg: 0.58 phi; -0.37 theta; -0.23 psi 2021-02-13T00:36:40Z- handleDepth()-Depth_Keller msg: 1613176600122 epoch sec; seqNo:14166; depth 7.19 2021-02-13T00:36:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176600.15 epoch sec; seqNo:14166; 4059597.67 north; 592837.34 east 2021-02-13T00:36:40Z- AHRS_M2 msg: 1613176600.52 epoch sec; seqNo:14054 2021-02-13T00:36:40Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.25 psi 2021-02-13T00:36:40Z- handleDepth()-Depth_Keller msg: 1613176600526 epoch sec; seqNo:14167; depth 7.34 2021-02-13T00:36:40Z- handleDvl() - RDI_Pathfinder msg: 1613176600.54 epoch sec; seqNo:6845 2021-02-13T00:36:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 111.30 , 81.56 , 135.10 2021-02-13T00:36:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.30 , -0.16 2021-02-13T00:36:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176600.55 epoch sec; seqNo:14167; 4059598.06 north; 592838.56 east 2021-02-13T00:36:40Z- AHRS_M2 msg: 1613176600.92 epoch sec; seqNo:14055 2021-02-13T00:36:40Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.25 psi 2021-02-13T00:36:40Z- handleDepth()-Depth_Keller msg: 1613176600928 epoch sec; seqNo:14168; depth 7.46 2021-02-13T00:36:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:36:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:36:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:36:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:36:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:36:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:36:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:36:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:36:40Z- handleDvl() - RDI_Pathfinder msg: 1613176600.93 epoch sec; seqNo:6846 2021-02-13T00:36:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:36:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.30 , -0.16 2021-02-13T00:36:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176600.96 epoch sec; seqNo:14168; 4059598.44 north; 592838.56 east 2021-02-13T00:36:41Z- AHRS_M2 msg: 1613176601.33 epoch sec; seqNo:14056 2021-02-13T00:36:41Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.24 psi 2021-02-13T00:36:41Z- handleDepth()-Depth_Keller msg: 1613176601333 epoch sec; seqNo:14169; depth 7.61 2021-02-13T00:36:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176601.35 epoch sec; seqNo:14169; 4059598.44 north; 592838.56 east 2021-02-13T00:36:41Z- AHRS_M2 msg: 1613176601.73 epoch sec; seqNo:14057 2021-02-13T00:36:41Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.24 psi 2021-02-13T00:36:41Z- handleDepth()-Depth_Keller msg: 1613176601736 epoch sec; seqNo:14170; depth 7.73 2021-02-13T00:36:41Z- handleDvl() - RDI_Pathfinder msg: 1613176601.75 epoch sec; seqNo:6847 2021-02-13T00:36:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.17 , 115.42 , 82.71 , 134.76 2021-02-13T00:36:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , -0.24 2021-02-13T00:36:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176601.76 epoch sec; seqNo:14170; 4059598.82 north; 592838.55 east 2021-02-13T00:36:42Z- AHRS_M2 msg: 1613176602.14 epoch sec; seqNo:14058 2021-02-13T00:36:42Z- AHRS_M2 msg: 0.57 phi; -0.31 theta; -0.23 psi 2021-02-13T00:36:42Z- handleDepth()-Depth_Keller msg: 1613176602146 epoch sec; seqNo:14171; depth 7.86 2021-02-13T00:36:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:36:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:36:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:36:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:36:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:36:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:36:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:36:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:36:42Z- handleDvl() - RDI_Pathfinder msg: 1613176602.15 epoch sec; seqNo:6848 2021-02-13T00:36:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:36:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , -0.24 2021-02-13T00:36:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176602.16 epoch sec; seqNo:14171; 4059599.20 north; 592838.55 east 2021-02-13T00:36:42Z- AHRS_M2 msg: 1613176602.54 epoch sec; seqNo:14059 2021-02-13T00:36:42Z- AHRS_M2 msg: 0.56 phi; -0.31 theta; -0.22 psi 2021-02-13T00:36:42Z- handleDepth()-Depth_Keller msg: 1613176602552 epoch sec; seqNo:14172; depth 8.00 2021-02-13T00:36:42Z- handleDvl() - RDI_Pathfinder msg: 1613176602.56 epoch sec; seqNo:6849 2021-02-13T00:36:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 114.00 , 82.77 , 124.42 2021-02-13T00:36:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.08 , -0.02 2021-02-13T00:36:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176602.57 epoch sec; seqNo:14172; 4059599.58 north; 592838.54 east 2021-02-13T00:36:42Z- AHRS_M2 msg: 1613176602.94 epoch sec; seqNo:14060 2021-02-13T00:36:42Z- AHRS_M2 msg: 0.56 phi; -0.30 theta; -0.20 psi 2021-02-13T00:36:42Z- handleDepth()-Depth_Keller msg: 1613176602948 epoch sec; seqNo:14173; depth 8.12 2021-02-13T00:36:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176602.98 epoch sec; seqNo:14173; 4059599.97 north; 592839.76 east 2021-02-13T00:36:43Z- AHRS_M2 msg: 1613176603.37 epoch sec; seqNo:14061 2021-02-13T00:36:43Z- AHRS_M2 msg: 0.56 phi; -0.30 theta; -0.20 psi 2021-02-13T00:36:43Z- handleDepth()-Depth_Keller msg: 1613176603375 epoch sec; seqNo:14174; depth 8.25 2021-02-13T00:36:43Z- handleDvl() - RDI_Pathfinder msg: 1613176603.39 epoch sec; seqNo:6850 2021-02-13T00:36:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.51 , 107.92 , 0.00 , 118.33 2021-02-13T00:36:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , 0.16 2021-02-13T00:36:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176603.40 epoch sec; seqNo:14174; 4059599.97 north; 592839.76 east 2021-02-13T00:36:43Z- AHRS_M2 msg: 1613176603.75 epoch sec; seqNo:14062 2021-02-13T00:36:43Z- AHRS_M2 msg: 0.56 phi; -0.30 theta; -0.19 psi 2021-02-13T00:36:43Z- handleDepth()-Depth_Keller msg: 1613176603758 epoch sec; seqNo:14175; depth 8.39 2021-02-13T00:36:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176603.77 epoch sec; seqNo:14175; 4059600.35 north; 592839.76 east 2021-02-13T00:36:44Z- AHRS_M2 msg: 1613176604.16 epoch sec; seqNo:14063 2021-02-13T00:36:44Z- AHRS_M2 msg: 0.55 phi; -0.30 theta; -0.18 psi 2021-02-13T00:36:44Z- handleDepth()-Depth_Keller msg: 1613176604169 epoch sec; seqNo:14176; depth 8.50 2021-02-13T00:36:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176604.18 epoch sec; seqNo:14176; 4059600.72 north; 592839.75 east 2021-02-13T00:36:44Z- AHRS_M2 msg: 1613176604.56 epoch sec; seqNo:14064 2021-02-13T00:36:44Z- AHRS_M2 msg: 0.55 phi; -0.30 theta; -0.18 psi 2021-02-13T00:36:44Z- handleDepth()-Depth_Keller msg: 1613176604564 epoch sec; seqNo:14177; depth 8.63 2021-02-13T00:36:44Z- handleDvl() - RDI_Pathfinder msg: 1613176604.59 epoch sec; seqNo:6851 2021-02-13T00:36:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.44 , 106.37 , 81.08 , 116.78 2021-02-13T00:36:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.18 , 0.08 2021-02-13T00:36:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176604.60 epoch sec; seqNo:14177; 4059600.72 north; 592839.75 east 2021-02-13T00:36:44Z- LcmTrn::handleMessages() - handling 47 messages took 0.73 ms and 133 calls... 2021-02-13T00:36:44Z- waiting for 1613176602.35, time is 1613176604.60 2021-02-13T00:36:44Z- LcmTrn::updateTrn() equating pose 1613176604.60 and meas times 1613176604.60 2021-02-13T00:36:44Z- LcmTrn::updateTrn() >>>> pose time 1613176604.60, meas time 1613176604.60 <<<< 2021-02-13T00:36:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176604.60 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:36:44Z- TerrainNav::motionUpdate() - currEstimate = (4059601, 592839.75, 8.63) 2021-02-13T00:36:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:36:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:36:44Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176604.60 sec.; No relevant navigation data available 2021-02-13T00:36:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:36:44Z- TNavBF::mmse selected from filter -1 2021-02-13T00:36:44Z- variances([0], [2], and psi_berg) = 695.152887 832.995460 0.000000 2021-02-13T00:36:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:36:44Z- LcmTrn::publishEstimates() - MLE : 4059600.22 592870.25 8.63 2021-02-13T00:36:44Z- LcmTrn::publishEstimates() - MMSE : 4059600.72 592839.75 8.63 2021-02-13T00:36:44Z- LcmTrn::publishEstimates() - COVAR: 695.15 833.00 1.40 0.00 2021-02-13T00:36:44Z- AHRS_M2 msg: 1613176604.96 epoch sec; seqNo:14065 2021-02-13T00:36:44Z- AHRS_M2 msg: 0.55 phi; -0.30 theta; -0.18 psi 2021-02-13T00:36:44Z- handleDepth()-Depth_Keller msg: 1613176604972 epoch sec; seqNo:14178; depth 8.75 2021-02-13T00:36:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176604.98 epoch sec; seqNo:14178; 4059601.10 north; 592839.75 east 2021-02-13T00:36:45Z- AHRS_M2 msg: 1613176605.39 epoch sec; seqNo:14066 2021-02-13T00:36:45Z- AHRS_M2 msg: 0.55 phi; -0.30 theta; -0.17 psi 2021-02-13T00:36:45Z- handleDepth()-Depth_Keller msg: 1613176605391 epoch sec; seqNo:14179; depth 8.86 2021-02-13T00:36:45Z- handleDvl() - RDI_Pathfinder msg: 1613176605.40 epoch sec; seqNo:6852 2021-02-13T00:36:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.51 , 105.08 , 0.00 , 115.90 2021-02-13T00:36:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.23 , -0.13 2021-02-13T00:36:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176605.42 epoch sec; seqNo:14179; 4059601.50 north; 592840.96 east 2021-02-13T00:36:45Z- AHRS_M2 msg: 1613176605.77 epoch sec; seqNo:14067 2021-02-13T00:36:45Z- AHRS_M2 msg: 0.56 phi; -0.29 theta; -0.18 psi 2021-02-13T00:36:45Z- handleDepth()-Depth_Keller msg: 1613176605778 epoch sec; seqNo:14180; depth 8.99 2021-02-13T00:36:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:36:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:36:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:36:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:36:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:36:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:36:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:36:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:36:45Z- handleDvl() - RDI_Pathfinder msg: 1613176605.78 epoch sec; seqNo:6853 2021-02-13T00:36:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:36:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.23 , -0.13 2021-02-13T00:36:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176605.79 epoch sec; seqNo:14180; 4059601.50 north; 592840.96 east 2021-02-13T00:36:46Z- AHRS_M2 msg: 1613176606.18 epoch sec; seqNo:14068 2021-02-13T00:36:46Z- AHRS_M2 msg: 0.57 phi; -0.29 theta; -0.19 psi 2021-02-13T00:36:46Z- handleDepth()-Depth_Keller msg: 1613176606180 epoch sec; seqNo:14181; depth 9.12 2021-02-13T00:36:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176606.20 epoch sec; seqNo:14181; 4059601.87 north; 592840.96 east 2021-02-13T00:36:46Z- AHRS_M2 msg: 1613176606.58 epoch sec; seqNo:14069 2021-02-13T00:36:46Z- AHRS_M2 msg: 0.58 phi; -0.29 theta; -0.19 psi 2021-02-13T00:36:46Z- handleDepth()-Depth_Keller msg: 1613176606587 epoch sec; seqNo:14182; depth 9.24 2021-02-13T00:36:46Z- handleDvl() - RDI_Pathfinder msg: 1613176606.60 epoch sec; seqNo:6854 2021-02-13T00:36:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.95 , 105.28 , 81.49 , 114.21 2021-02-13T00:36:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.07 , -0.28 2021-02-13T00:36:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176606.63 epoch sec; seqNo:14182; 4059602.25 north; 592840.96 east 2021-02-13T00:36:46Z- AHRS_M2 msg: 1613176606.99 epoch sec; seqNo:14070 2021-02-13T00:36:46Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.18 psi 2021-02-13T00:36:46Z- handleDepth()-Depth_Keller msg: 1613176606991 epoch sec; seqNo:14183; depth 9.34 2021-02-13T00:36:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176607.00 epoch sec; seqNo:14183; 4059602.63 north; 592840.95 east 2021-02-13T00:36:47Z- AHRS_M2 msg: 1613176607.40 epoch sec; seqNo:14071 2021-02-13T00:36:47Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.17 psi 2021-02-13T00:36:47Z- handleDepth()-Depth_Keller msg: 1613176607401 epoch sec; seqNo:14184; depth 9.47 2021-02-13T00:36:47Z- handleDvl() - RDI_Pathfinder msg: 1613176607.41 epoch sec; seqNo:6855 2021-02-13T00:36:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 105.42 , 78.05 , 113.53 2021-02-13T00:36:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , -0.23 2021-02-13T00:36:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176607.42 epoch sec; seqNo:14184; 4059603.02 north; 592842.17 east 2021-02-13T00:36:47Z- AHRS_M2 msg: 1613176607.79 epoch sec; seqNo:14072 2021-02-13T00:36:47Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.16 psi 2021-02-13T00:36:47Z- handleDepth()-Depth_Keller msg: 1613176607796 epoch sec; seqNo:14185; depth 9.60 2021-02-13T00:36:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176607.81 epoch sec; seqNo:14185; 4059603.40 north; 592842.16 east 2021-02-13T00:36:48Z- AHRS_M2 msg: 1613176608.20 epoch sec; seqNo:14073 2021-02-13T00:36:48Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.14 psi 2021-02-13T00:36:48Z- handleDepth()-Depth_Keller msg: 1613176608200 epoch sec; seqNo:14186; depth 9.71 2021-02-13T00:36:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176608.21 epoch sec; seqNo:14186; 4059603.78 north; 592842.16 east 2021-02-13T00:36:48Z- handleDepth()-Depth_Keller msg: 1613176608625 epoch sec; seqNo:14187; depth 9.83 2021-02-13T00:36:48Z- handleDvl() - RDI_Pathfinder msg: 1613176608.64 epoch sec; seqNo:6856 2021-02-13T00:36:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.28 , 102.85 , 79.87 , 110.96 2021-02-13T00:36:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.10 , -0.14 2021-02-13T00:36:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176608.65 epoch sec; seqNo:14187; 4059604.16 north; 592842.16 east 2021-02-13T00:36:49Z- AHRS_M2 msg: 1613176609.01 epoch sec; seqNo:14074 2021-02-13T00:36:49Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.12 psi 2021-02-13T00:36:49Z- handleDepth()-Depth_Keller msg: 1613176609014 epoch sec; seqNo:14188; depth 9.97 2021-02-13T00:36:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176609.03 epoch sec; seqNo:14188; 4059604.16 north; 592842.16 east 2021-02-13T00:36:49Z- AHRS_M2 msg: 1613176609.41 epoch sec; seqNo:14075 2021-02-13T00:36:49Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.11 psi 2021-02-13T00:36:49Z- handleDepth()-Depth_Keller msg: 1613176609414 epoch sec; seqNo:14189; depth 10.08 2021-02-13T00:36:49Z- handleDvl() - RDI_Pathfinder msg: 1613176609.43 epoch sec; seqNo:6857 2021-02-13T00:36:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 100.21 , 79.93 , 111.03 2021-02-13T00:36:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.12 , -0.10 2021-02-13T00:36:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176609.44 epoch sec; seqNo:14189; 4059604.54 north; 592842.15 east 2021-02-13T00:36:49Z- AHRS_M2 msg: 1613176609.81 epoch sec; seqNo:14076 2021-02-13T00:36:49Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.11 psi 2021-02-13T00:36:49Z- LcmTrn::handleMessages() - handling 42 messages took 0.60 ms and 130 calls... 2021-02-13T00:36:49Z- waiting for 1613176607.60, time is 1613176609.81 2021-02-13T00:36:49Z- LcmTrn::updateTrn() >>>> pose time 1613176609.81, meas time 1613176609.43 <<<< 2021-02-13T00:36:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176609.43 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:36:49Z- TerrainNav::motionUpdate() - currEstimate = (4059605, 592842.15, 10.08) 2021-02-13T00:36:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:36:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:36:49Z- TNavBF::Projecting Measurements 2021-02-13T00:36:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:36:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:36:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:36:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:36:49Z- TNavFilter::4 good beams. 2021-02-13T00:36:49Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:36:49Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:36:49Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:36:49Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:36:49Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:36:49Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:36:49Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:36:49Z- TNavBF::mmse selected from filter -1 2021-02-13T00:36:49Z- TerrainMapOctree: (vehN, vehE ) = (4059604.26, 592841.98). 2021-02-13T00:36:49Z- TerrainMapOctree: Min Distance = 2165.87. 2021-02-13T00:36:49Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:36:49Z- There is no specific good beam that all particles have in common. 2021-02-13T00:36:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:36:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.62 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 12.03 2021-02-13T00:36:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:36:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.45 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 16.07 2021-02-13T00:36:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:36:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 10.22 2021-02-13T00:36:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:36:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 19.72 2021-02-13T00:36:49Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:36:49Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:36:49Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:36:49Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:36:49Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:36:49Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:36:49Z- Filter 0 No beams used in subcloud update 2021-02-13T00:36:49Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:36:49Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:36:49Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 695.1 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:36:49Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176609.43 sec. 2021-02-13T00:36:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:36:49Z- TNavBF::mmse selected from filter -1 2021-02-13T00:36:49Z- variances([0], [2], and psi_berg) = 695.085625 832.995427 0.000000 2021-02-13T00:36:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:36:49Z- LcmTrn::publishEstimates() - MLE : 4059604.04 592872.65 10.08 2021-02-13T00:36:49Z- LcmTrn::publishEstimates() - MMSE : 4059604.54 592842.15 10.08 2021-02-13T00:36:49Z- LcmTrn::publishEstimates() - COVAR: 695.09 833.00 1.16 0.00 2021-02-13T00:36:49Z- handleDepth()-Depth_Keller msg: 1613176609816 epoch sec; seqNo:14190; depth 10.19 2021-02-13T00:36:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176609.83 epoch sec; seqNo:14190; 4059604.92 north; 592842.15 east 2021-02-13T00:36:50Z- AHRS_M2 msg: 1613176610.22 epoch sec; seqNo:14077 2021-02-13T00:36:50Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.12 psi 2021-02-13T00:36:50Z- handleDepth()-Depth_Keller msg: 1613176610220 epoch sec; seqNo:14191; depth 10.31 2021-02-13T00:36:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176610.23 epoch sec; seqNo:14191; 4059605.31 north; 592843.36 east 2021-02-13T00:36:50Z- AHRS_M2 msg: 1613176610.62 epoch sec; seqNo:14078 2021-02-13T00:36:50Z- AHRS_M2 msg: 0.62 phi; -0.28 theta; -0.14 psi 2021-02-13T00:36:50Z- handleDepth()-Depth_Keller msg: 1613176610631 epoch sec; seqNo:14192; depth 10.43 2021-02-13T00:36:50Z- handleDvl() - RDI_Pathfinder msg: 1613176610.64 epoch sec; seqNo:6858 2021-02-13T00:36:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 97.51 , 79.93 , 111.03 2021-02-13T00:36:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.18 , -0.08 2021-02-13T00:36:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176610.65 epoch sec; seqNo:14192; 4059605.69 north; 592843.36 east 2021-02-13T00:36:51Z- AHRS_M2 msg: 1613176611.02 epoch sec; seqNo:14079 2021-02-13T00:36:51Z- AHRS_M2 msg: 0.62 phi; -0.28 theta; -0.15 psi 2021-02-13T00:36:51Z- handleDepth()-Depth_Keller msg: 1613176611028 epoch sec; seqNo:14193; depth 10.56 2021-02-13T00:36:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176611.04 epoch sec; seqNo:14193; 4059606.07 north; 592843.36 east 2021-02-13T00:36:51Z- AHRS_M2 msg: 1613176611.43 epoch sec; seqNo:14080 2021-02-13T00:36:51Z- AHRS_M2 msg: 0.62 phi; -0.27 theta; -0.16 psi 2021-02-13T00:36:51Z- handleDepth()-Depth_Keller msg: 1613176611437 epoch sec; seqNo:14194; depth 10.69 2021-02-13T00:36:51Z- handleDvl() - RDI_Pathfinder msg: 1613176611.45 epoch sec; seqNo:6859 2021-02-13T00:36:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 100.28 , 78.65 , 111.10 2021-02-13T00:36:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.27 , 0.05 2021-02-13T00:36:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176611.47 epoch sec; seqNo:14194; 4059606.45 north; 592843.35 east 2021-02-13T00:36:51Z- AHRS_M2 msg: 1613176611.83 epoch sec; seqNo:14081 2021-02-13T00:36:51Z- AHRS_M2 msg: 0.62 phi; -0.27 theta; -0.17 psi 2021-02-13T00:36:51Z- handleDepth()-Depth_Keller msg: 1613176611845 epoch sec; seqNo:14195; depth 10.82 2021-02-13T00:36:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176611.86 epoch sec; seqNo:14195; 4059606.45 north; 592843.35 east 2021-02-13T00:36:52Z- AHRS_M2 msg: 1613176612.24 epoch sec; seqNo:14082 2021-02-13T00:36:52Z- AHRS_M2 msg: 0.62 phi; -0.27 theta; -0.17 psi 2021-02-13T00:36:52Z- handleDepth()-Depth_Keller msg: 1613176612249 epoch sec; seqNo:14196; depth 10.93 2021-02-13T00:36:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176612.26 epoch sec; seqNo:14196; 4059606.83 north; 592843.35 east 2021-02-13T00:36:52Z- AHRS_M2 msg: 1613176612.64 epoch sec; seqNo:14083 2021-02-13T00:36:52Z- AHRS_M2 msg: 0.61 phi; -0.28 theta; -0.16 psi 2021-02-13T00:36:52Z- handleDepth()-Depth_Keller msg: 1613176612645 epoch sec; seqNo:14197; depth 11.05 2021-02-13T00:36:52Z- handleDvl() - RDI_Pathfinder msg: 1613176612.66 epoch sec; seqNo:6860 2021-02-13T00:36:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 101.63 , 80.00 , 108.39 2021-02-13T00:36:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.17 , 0.26 2021-02-13T00:36:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176612.68 epoch sec; seqNo:14197; 4059607.20 north; 592843.34 east 2021-02-13T00:36:53Z- AHRS_M2 msg: 1613176613.05 epoch sec; seqNo:14084 2021-02-13T00:36:53Z- AHRS_M2 msg: 0.61 phi; -0.28 theta; -0.15 psi 2021-02-13T00:36:53Z- handleDepth()-Depth_Keller msg: 1613176613052 epoch sec; seqNo:14198; depth 11.16 2021-02-13T00:36:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176613.06 epoch sec; seqNo:14198; 4059607.58 north; 592843.34 east 2021-02-13T00:36:53Z- AHRS_M2 msg: 1613176613.45 epoch sec; seqNo:14085 2021-02-13T00:36:53Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.15 psi 2021-02-13T00:36:53Z- handleDepth()-Depth_Keller msg: 1613176613460 epoch sec; seqNo:14199; depth 11.26 2021-02-13T00:36:53Z- handleDvl() - RDI_Pathfinder msg: 1613176613.47 epoch sec; seqNo:6861 2021-02-13T00:36:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 98.93 , 78.65 , 111.10 2021-02-13T00:36:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.08 , 0.34 2021-02-13T00:36:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176613.48 epoch sec; seqNo:14199; 4059607.58 north; 592843.34 east 2021-02-13T00:36:53Z- AHRS_M2 msg: 1613176613.85 epoch sec; seqNo:14086 2021-02-13T00:36:53Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.16 psi 2021-02-13T00:36:53Z- handleDepth()-Depth_Keller msg: 1613176613856 epoch sec; seqNo:14200; depth 11.37 2021-02-13T00:36:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176613.87 epoch sec; seqNo:14200; 4059607.98 north; 592844.56 east 2021-02-13T00:36:54Z- AHRS_M2 msg: 1613176614.26 epoch sec; seqNo:14087 2021-02-13T00:36:54Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.16 psi 2021-02-13T00:36:54Z- handleDepth()-Depth_Keller msg: 1613176614260 epoch sec; seqNo:14201; depth 11.46 2021-02-13T00:36:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176614.27 epoch sec; seqNo:14201; 4059607.98 north; 592844.56 east 2021-02-13T00:36:54Z- AHRS_M2 msg: 1613176614.66 epoch sec; seqNo:14088 2021-02-13T00:36:54Z- AHRS_M2 msg: 0.58 phi; -0.27 theta; -0.16 psi 2021-02-13T00:36:54Z- handleDepth()-Depth_Keller msg: 1613176614673 epoch sec; seqNo:14202; depth 11.57 2021-02-13T00:36:54Z- handleDvl() - RDI_Pathfinder msg: 1613176614.68 epoch sec; seqNo:6862 2021-02-13T00:36:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.48 , 100.35 , 78.72 , 108.46 2021-02-13T00:36:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.29 , 0.22 2021-02-13T00:36:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176614.70 epoch sec; seqNo:14202; 4059608.35 north; 592844.55 east 2021-02-13T00:36:54Z- LcmTrn::handleMessages() - handling 43 messages took 0.66 ms and 130 calls... 2021-02-13T00:36:54Z- waiting for 1613176612.81, time is 1613176614.70 2021-02-13T00:36:54Z- LcmTrn::updateTrn() equating pose 1613176614.70 and meas times 1613176614.70 2021-02-13T00:36:54Z- LcmTrn::updateTrn() >>>> pose time 1613176614.70, meas time 1613176614.70 <<<< 2021-02-13T00:36:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176614.70 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:36:54Z- TerrainNav::motionUpdate() - currEstimate = (4059608, 592844.55, 11.57) 2021-02-13T00:36:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:36:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:36:54Z- TNavBF::Projecting Measurements 2021-02-13T00:36:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:36:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:36:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:36:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:36:54Z- TNavFilter::4 good beams. 2021-02-13T00:36:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:36:54Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:36:54Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:36:54Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:36:54Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:36:54Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:36:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:36:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:36:54Z- TerrainMapOctree: (vehN, vehE ) = (4059608.35, 592844.55). 2021-02-13T00:36:54Z- TerrainMapOctree: Min Distance = 2161.08. 2021-02-13T00:36:54Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:36:54Z- There is no specific good beam that all particles have in common. 2021-02-13T00:36:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:36:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.39 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 11.69 2021-02-13T00:36:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:36:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 16.11 2021-02-13T00:36:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:36:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 9.91 2021-02-13T00:36:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:36:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 18.82 2021-02-13T00:36:54Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:36:54Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:36:54Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:36:54Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:36:54Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:36:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:36:54Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:36:54Z- Filter 0 No beams used in subcloud update 2021-02-13T00:36:54Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:36:54Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:36:54Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 695.0 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:36:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176614.70 sec. 2021-02-13T00:36:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:36:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:36:54Z- variances([0], [2], and psi_berg) = 695.031095 832.995399 0.000000 2021-02-13T00:36:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:36:54Z- LcmTrn::publishEstimates() - MLE : 4059607.85 592875.05 11.57 2021-02-13T00:36:54Z- LcmTrn::publishEstimates() - MMSE : 4059608.35 592844.55 11.57 2021-02-13T00:36:54Z- LcmTrn::publishEstimates() - COVAR: 695.03 833.00 1.01 0.00 2021-02-13T00:36:55Z- AHRS_M2 msg: 1613176615.06 epoch sec; seqNo:14089 2021-02-13T00:36:55Z- AHRS_M2 msg: 0.59 phi; -0.27 theta; -0.17 psi 2021-02-13T00:36:55Z- handleDepth()-Depth_Keller msg: 1613176615070 epoch sec; seqNo:14203; depth 11.68 2021-02-13T00:36:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176615.08 epoch sec; seqNo:14203; 4059608.35 north; 592844.55 east 2021-02-13T00:36:55Z- AHRS_M2 msg: 1613176615.47 epoch sec; seqNo:14090 2021-02-13T00:36:55Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.18 psi 2021-02-13T00:36:55Z- handleDepth()-Depth_Keller msg: 1613176615472 epoch sec; seqNo:14204; depth 11.78 2021-02-13T00:36:55Z- handleDvl() - RDI_Pathfinder msg: 1613176615.49 epoch sec; seqNo:6863 2021-02-13T00:36:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.48 , 100.35 , 78.72 , 111.17 2021-02-13T00:36:55Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.36 , 0.02 2021-02-13T00:36:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176615.51 epoch sec; seqNo:14204; 4059608.75 north; 592845.77 east 2021-02-13T00:36:55Z- AHRS_M2 msg: 1613176615.87 epoch sec; seqNo:14091 2021-02-13T00:36:55Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.19 psi 2021-02-13T00:36:55Z- handleDepth()-Depth_Keller msg: 1613176615880 epoch sec; seqNo:14205; depth 11.87 2021-02-13T00:36:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176615.89 epoch sec; seqNo:14205; 4059608.75 north; 592845.77 east 2021-02-13T00:36:56Z- AHRS_M2 msg: 1613176616.29 epoch sec; seqNo:14092 2021-02-13T00:36:56Z- AHRS_M2 msg: 0.62 phi; -0.27 theta; -0.18 psi 2021-02-13T00:36:56Z- handleDepth()-Depth_Keller msg: 1613176616290 epoch sec; seqNo:14206; depth 11.99 2021-02-13T00:36:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176616.31 epoch sec; seqNo:14206; 4059609.13 north; 592845.76 east 2021-02-13T00:36:56Z- AHRS_M2 msg: 1613176616.68 epoch sec; seqNo:14093 2021-02-13T00:36:56Z- AHRS_M2 msg: 0.63 phi; -0.27 theta; -0.18 psi 2021-02-13T00:36:56Z- handleDepth()-Depth_Keller msg: 1613176616702 epoch sec; seqNo:14207; depth 12.09 2021-02-13T00:36:56Z- handleDvl() - RDI_Pathfinder msg: 1613176616.71 epoch sec; seqNo:6864 2021-02-13T00:36:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.99 , 80.00 , 109.75 2021-02-13T00:36:56Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.22 , -0.14 2021-02-13T00:36:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176616.72 epoch sec; seqNo:14207; 4059609.50 north; 592845.76 east 2021-02-13T00:36:57Z- AHRS_M2 msg: 1613176617.08 epoch sec; seqNo:14094 2021-02-13T00:36:57Z- AHRS_M2 msg: 0.63 phi; -0.27 theta; -0.18 psi 2021-02-13T00:36:57Z- handleDepth()-Depth_Keller msg: 1613176617089 epoch sec; seqNo:14208; depth 12.20 2021-02-13T00:36:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176617.10 epoch sec; seqNo:14208; 4059609.90 north; 592846.98 east 2021-02-13T00:36:57Z- AHRS_M2 msg: 1613176617.49 epoch sec; seqNo:14095 2021-02-13T00:36:57Z- AHRS_M2 msg: 0.64 phi; -0.27 theta; -0.17 psi 2021-02-13T00:36:57Z- handleDepth()-Depth_Keller msg: 1613176617499 epoch sec; seqNo:14209; depth 12.31 2021-02-13T00:36:57Z- handleDvl() - RDI_Pathfinder msg: 1613176617.51 epoch sec; seqNo:6865 2021-02-13T00:36:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.65 , 79.67 , 108.06 2021-02-13T00:36:57Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , 0.13 , -0.19 2021-02-13T00:36:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176617.52 epoch sec; seqNo:14209; 4059610.28 north; 592846.97 east 2021-02-13T00:36:57Z- AHRS_M2 msg: 1613176617.89 epoch sec; seqNo:14096 2021-02-13T00:36:57Z- AHRS_M2 msg: 0.64 phi; -0.27 theta; -0.17 psi 2021-02-13T00:36:57Z- handleDepth()-Depth_Keller msg: 1613176617898 epoch sec; seqNo:14210; depth 12.41 2021-02-13T00:36:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176617.91 epoch sec; seqNo:14210; 4059610.28 north; 592846.97 east 2021-02-13T00:36:58Z- AHRS_M2 msg: 1613176618.30 epoch sec; seqNo:14097 2021-02-13T00:36:58Z- AHRS_M2 msg: 0.64 phi; -0.27 theta; -0.17 psi 2021-02-13T00:36:58Z- handleDepth()-Depth_Keller msg: 1613176618300 epoch sec; seqNo:14211; depth 12.51 2021-02-13T00:36:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176618.32 epoch sec; seqNo:14211; 4059610.65 north; 592846.97 east 2021-02-13T00:36:58Z- AHRS_M2 msg: 1613176618.70 epoch sec; seqNo:14098 2021-02-13T00:36:58Z- AHRS_M2 msg: 0.64 phi; -0.27 theta; -0.17 psi 2021-02-13T00:36:58Z- handleDepth()-Depth_Keller msg: 1613176618706 epoch sec; seqNo:14212; depth 12.61 2021-02-13T00:36:58Z- handleDvl() - RDI_Pathfinder msg: 1613176618.72 epoch sec; seqNo:6866 2021-02-13T00:36:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.44 , 100.01 , 76.83 , 106.77 2021-02-13T00:36:58Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.00 , -0.17 2021-02-13T00:36:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176618.74 epoch sec; seqNo:14212; 4059611.05 north; 592848.19 east 2021-02-13T00:36:59Z- AHRS_M2 msg: 1613176619.10 epoch sec; seqNo:14099 2021-02-13T00:36:59Z- AHRS_M2 msg: 0.63 phi; -0.27 theta; -0.16 psi 2021-02-13T00:36:59Z- handleDepth()-Depth_Keller msg: 1613176619108 epoch sec; seqNo:14213; depth 12.71 2021-02-13T00:36:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176619.12 epoch sec; seqNo:14213; 4059611.43 north; 592848.18 east 2021-02-13T00:36:59Z- AHRS_M2 msg: 1613176619.51 epoch sec; seqNo:14100 2021-02-13T00:36:59Z- AHRS_M2 msg: 0.63 phi; -0.27 theta; -0.16 psi 2021-02-13T00:36:59Z- handleDepth()-Depth_Keller msg: 1613176619514 epoch sec; seqNo:14214; depth 12.81 2021-02-13T00:36:59Z- handleDvl() - RDI_Pathfinder msg: 1613176619.53 epoch sec; seqNo:6867 2021-02-13T00:36:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.83 , 96.43 , 77.24 , 107.38 2021-02-13T00:36:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.02 , -0.14 2021-02-13T00:36:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176619.54 epoch sec; seqNo:14214; 4059611.80 north; 592848.18 east 2021-02-13T00:36:59Z- AHRS_M2 msg: 1613176619.91 epoch sec; seqNo:14101 2021-02-13T00:36:59Z- AHRS_M2 msg: 0.63 phi; -0.28 theta; -0.16 psi 2021-02-13T00:36:59Z- handleDepth()-Depth_Keller msg: 1613176619916 epoch sec; seqNo:14215; depth 12.92 2021-02-13T00:36:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176619.93 epoch sec; seqNo:14215; 4059612.18 north; 592848.17 east 2021-02-13T00:37:00Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-13T00:37:00Z- waiting for 1613176617.70, time is 1613176619.93 2021-02-13T00:37:00Z- LcmTrn::updateTrn() >>>> pose time 1613176619.93, meas time 1613176619.53 <<<< 2021-02-13T00:37:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176619.53 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:37:00Z- TerrainNav::motionUpdate() - currEstimate = (4059612, 592848.17, 12.92) 2021-02-13T00:37:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:37:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:37:00Z- TNavBF::Projecting Measurements 2021-02-13T00:37:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:37:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:37:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:37:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:37:00Z- TNavFilter::4 good beams. 2021-02-13T00:37:00Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:37:00Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:37:00Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:37:00Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:37:00Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:37:00Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:37:00Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:37:00Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:00Z- TerrainMapOctree: (vehN, vehE ) = (4059611.89, 592847.89). 2021-02-13T00:37:00Z- TerrainMapOctree: Min Distance = 2156.23. 2021-02-13T00:37:00Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:37:00Z- There is no specific good beam that all particles have in common. 2021-02-13T00:37:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:37:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 10.71 2021-02-13T00:37:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:37:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 14.88 2021-02-13T00:37:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:37:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 9.54 2021-02-13T00:37:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:37:00Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 18.45 2021-02-13T00:37:00Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:37:00Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:37:00Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:37:00Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:37:00Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:37:00Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:37:00Z- Filter 0 No beams used in subcloud update 2021-02-13T00:37:00Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:37:00Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:37:00Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 695.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:37:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176619.53 sec. 2021-02-13T00:37:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:37:00Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:00Z- variances([0], [2], and psi_berg) = 694.958084 832.995363 0.000000 2021-02-13T00:37:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:37:00Z- LcmTrn::publishEstimates() - MLE : 4059611.68 592878.67 12.92 2021-02-13T00:37:00Z- LcmTrn::publishEstimates() - MMSE : 4059612.18 592848.17 12.92 2021-02-13T00:37:00Z- LcmTrn::publishEstimates() - COVAR: 694.96 833.00 0.95 0.00 2021-02-13T00:37:00Z- AHRS_M2 msg: 1613176620.32 epoch sec; seqNo:14102 2021-02-13T00:37:00Z- AHRS_M2 msg: 0.63 phi; -0.28 theta; -0.17 psi 2021-02-13T00:37:00Z- handleDepth()-Depth_Keller msg: 1613176620320 epoch sec; seqNo:14216; depth 13.01 2021-02-13T00:37:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176620.33 epoch sec; seqNo:14216; 4059612.56 north; 592848.17 east 2021-02-13T00:37:00Z- AHRS_M2 msg: 1613176620.72 epoch sec; seqNo:14103 2021-02-13T00:37:00Z- AHRS_M2 msg: 0.63 phi; -0.28 theta; -0.18 psi 2021-02-13T00:37:00Z- handleDepth()-Depth_Keller msg: 1613176620729 epoch sec; seqNo:14217; depth 13.11 2021-02-13T00:37:00Z- handleDvl() - RDI_Pathfinder msg: 1613176620.74 epoch sec; seqNo:6868 2021-02-13T00:37:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 97.71 , 78.25 , 107.45 2021-02-13T00:37:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , -0.12 2021-02-13T00:37:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176620.75 epoch sec; seqNo:14217; 4059612.56 north; 592848.17 east 2021-02-13T00:37:01Z- AHRS_M2 msg: 1613176621.13 epoch sec; seqNo:14104 2021-02-13T00:37:01Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.19 psi 2021-02-13T00:37:01Z- handleDepth()-Depth_Keller msg: 1613176621133 epoch sec; seqNo:14218; depth 13.21 2021-02-13T00:37:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176621.15 epoch sec; seqNo:14218; 4059612.95 north; 592849.39 east 2021-02-13T00:37:01Z- AHRS_M2 msg: 1613176621.53 epoch sec; seqNo:14105 2021-02-13T00:37:01Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.19 psi 2021-02-13T00:37:01Z- handleDepth()-Depth_Keller msg: 1613176621537 epoch sec; seqNo:14219; depth 13.32 2021-02-13T00:37:01Z- handleDvl() - RDI_Pathfinder msg: 1613176621.56 epoch sec; seqNo:6869 2021-02-13T00:37:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.83 , 100.08 , 75.19 , 106.16 2021-02-13T00:37:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , -0.14 2021-02-13T00:37:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176621.57 epoch sec; seqNo:14219; 4059613.33 north; 592849.38 east 2021-02-13T00:37:01Z- AHRS_M2 msg: 1613176621.93 epoch sec; seqNo:14106 2021-02-13T00:37:01Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.20 psi 2021-02-13T00:37:01Z- handleDepth()-Depth_Keller msg: 1613176621941 epoch sec; seqNo:14220; depth 13.41 2021-02-13T00:37:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176621.95 epoch sec; seqNo:14220; 4059613.71 north; 592849.38 east 2021-02-13T00:37:02Z- handleDepth()-Depth_Keller msg: 1613176622374 epoch sec; seqNo:14221; depth 13.52 2021-02-13T00:37:02Z- handleDvl() - RDI_Pathfinder msg: 1613176622.39 epoch sec; seqNo:6870 2021-02-13T00:37:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 101.36 , 75.19 , 106.23 2021-02-13T00:37:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.14 , -0.09 2021-02-13T00:37:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176622.40 epoch sec; seqNo:14221; 4059614.09 north; 592849.37 east 2021-02-13T00:37:02Z- AHRS_M2 msg: 1613176622.74 epoch sec; seqNo:14107 2021-02-13T00:37:02Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.22 psi 2021-02-13T00:37:02Z- handleDepth()-Depth_Keller msg: 1613176622757 epoch sec; seqNo:14222; depth 13.65 2021-02-13T00:37:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176622.77 epoch sec; seqNo:14222; 4059614.09 north; 592849.37 east 2021-02-13T00:37:03Z- AHRS_M2 msg: 1613176623.15 epoch sec; seqNo:14108 2021-02-13T00:37:03Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.22 psi 2021-02-13T00:37:03Z- handleDepth()-Depth_Keller msg: 1613176623153 epoch sec; seqNo:14223; depth 13.73 2021-02-13T00:37:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176623.17 epoch sec; seqNo:14223; 4059614.47 north; 592849.37 east 2021-02-13T00:37:03Z- AHRS_M2 msg: 1613176623.55 epoch sec; seqNo:14109 2021-02-13T00:37:03Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.23 psi 2021-02-13T00:37:03Z- handleDepth()-Depth_Keller msg: 1613176623552 epoch sec; seqNo:14224; depth 13.82 2021-02-13T00:37:03Z- handleDvl() - RDI_Pathfinder msg: 1613176623.56 epoch sec; seqNo:6871 2021-02-13T00:37:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.39 , 102.51 , 74.37 , 109.81 2021-02-13T00:37:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.20 , 0.04 2021-02-13T00:37:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176623.57 epoch sec; seqNo:14224; 4059614.85 north; 592849.36 east 2021-02-13T00:37:03Z- AHRS_M2 msg: 1613176623.95 epoch sec; seqNo:14110 2021-02-13T00:37:03Z- AHRS_M2 msg: 0.58 phi; -0.29 theta; -0.22 psi 2021-02-13T00:37:03Z- handleDepth()-Depth_Keller msg: 1613176623965 epoch sec; seqNo:14225; depth 13.94 2021-02-13T00:37:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176623.97 epoch sec; seqNo:14225; 4059615.23 north; 592849.36 east 2021-02-13T00:37:04Z- AHRS_M2 msg: 1613176624.36 epoch sec; seqNo:14111 2021-02-13T00:37:04Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.22 psi 2021-02-13T00:37:04Z- handleDepth()-Depth_Keller msg: 1613176624370 epoch sec; seqNo:14226; depth 14.05 2021-02-13T00:37:04Z- handleDvl() - RDI_Pathfinder msg: 1613176624.38 epoch sec; seqNo:6872 2021-02-13T00:37:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.39 , 102.51 , 75.19 , 108.60 2021-02-13T00:37:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.17 , 0.17 2021-02-13T00:37:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176624.39 epoch sec; seqNo:14226; 4059615.62 north; 592850.58 east 2021-02-13T00:37:04Z- AHRS_M2 msg: 1613176624.76 epoch sec; seqNo:14112 2021-02-13T00:37:04Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.21 psi 2021-02-13T00:37:04Z- handleDepth()-Depth_Keller msg: 1613176624764 epoch sec; seqNo:14227; depth 14.16 2021-02-13T00:37:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176624.78 epoch sec; seqNo:14227; 4059616.00 north; 592850.57 east 2021-02-13T00:37:05Z- LcmTrn::handleMessages() - handling 40 messages took 0.60 ms and 128 calls... 2021-02-13T00:37:05Z- waiting for 1613176622.93, time is 1613176624.78 2021-02-13T00:37:05Z- LcmTrn::updateTrn() >>>> pose time 1613176624.78, meas time 1613176624.38 <<<< 2021-02-13T00:37:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176624.38 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:37:05Z- TerrainNav::motionUpdate() - currEstimate = (4059616, 592850.57, 14.16) 2021-02-13T00:37:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:37:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:37:05Z- TNavBF::Projecting Measurements 2021-02-13T00:37:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:37:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:37:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:37:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:37:05Z- TNavFilter::4 good beams. 2021-02-13T00:37:05Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:37:05Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:37:05Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:37:05Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:37:05Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:37:05Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:37:05Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:37:05Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:05Z- TerrainMapOctree: (vehN, vehE ) = (4059615.69, 592850.38). 2021-02-13T00:37:05Z- TerrainMapOctree: Min Distance = 2151.72. 2021-02-13T00:37:05Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:37:05Z- There is no specific good beam that all particles have in common. 2021-02-13T00:37:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:37:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 10.09 2021-02-13T00:37:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:37:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 16.81 2021-02-13T00:37:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:37:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 9.05 2021-02-13T00:37:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:37:05Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 18.87 2021-02-13T00:37:05Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:37:05Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:37:05Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:37:05Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:37:05Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:37:05Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:37:05Z- Filter 0 No beams used in subcloud update 2021-02-13T00:37:05Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:37:05Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:37:05Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 694.9 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:37:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176624.38 sec. 2021-02-13T00:37:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:37:05Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:05Z- variances([0], [2], and psi_berg) = 694.890379 832.995329 0.000000 2021-02-13T00:37:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:37:05Z- LcmTrn::publishEstimates() - MLE : 4059615.50 592880.07 14.16 2021-02-13T00:37:05Z- LcmTrn::publishEstimates() - MMSE : 4059616.00 592850.57 14.16 2021-02-13T00:37:05Z- LcmTrn::publishEstimates() - COVAR: 694.89 833.00 0.90 0.00 2021-02-13T00:37:05Z- AHRS_M2 msg: 1613176625.16 epoch sec; seqNo:14113 2021-02-13T00:37:05Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.20 psi 2021-02-13T00:37:05Z- handleDepth()-Depth_Keller msg: 1613176625168 epoch sec; seqNo:14228; depth 14.27 2021-02-13T00:37:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176625.18 epoch sec; seqNo:14228; 4059616.00 north; 592850.57 east 2021-02-13T00:37:05Z- AHRS_M2 msg: 1613176625.57 epoch sec; seqNo:14114 2021-02-13T00:37:05Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.19 psi 2021-02-13T00:37:05Z- handleDepth()-Depth_Keller msg: 1613176625582 epoch sec; seqNo:14229; depth 14.39 2021-02-13T00:37:05Z- handleDvl() - RDI_Pathfinder msg: 1613176625.59 epoch sec; seqNo:6873 2021-02-13T00:37:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 100.08 , 0.00 , 109.81 2021-02-13T00:37:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , 0.23 2021-02-13T00:37:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176625.60 epoch sec; seqNo:14229; 4059616.38 north; 592850.57 east 2021-02-13T00:37:05Z- AHRS_M2 msg: 1613176625.97 epoch sec; seqNo:14115 2021-02-13T00:37:05Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-13T00:37:05Z- handleDepth()-Depth_Keller msg: 1613176625976 epoch sec; seqNo:14230; depth 14.52 2021-02-13T00:37:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176625.99 epoch sec; seqNo:14230; 4059616.76 north; 592850.56 east 2021-02-13T00:37:06Z- AHRS_M2 msg: 1613176626.39 epoch sec; seqNo:14116 2021-02-13T00:37:06Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.17 psi 2021-02-13T00:37:06Z- handleDepth()-Depth_Keller msg: 1613176626396 epoch sec; seqNo:14231; depth 14.62 2021-02-13T00:37:06Z- handleDvl() - RDI_Pathfinder msg: 1613176626.41 epoch sec; seqNo:6874 2021-02-13T00:37:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.08 , 98.66 , 73.15 , 109.48 2021-02-13T00:37:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , 0.19 2021-02-13T00:37:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176626.43 epoch sec; seqNo:14231; 4059617.13 north; 592850.56 east 2021-02-13T00:37:06Z- AHRS_M2 msg: 1613176626.78 epoch sec; seqNo:14117 2021-02-13T00:37:06Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.17 psi 2021-02-13T00:37:06Z- handleDepth()-Depth_Keller msg: 1613176626797 epoch sec; seqNo:14232; depth 14.75 2021-02-13T00:37:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176626.81 epoch sec; seqNo:14232; 4059617.13 north; 592850.56 east 2021-02-13T00:37:07Z- AHRS_M2 msg: 1613176627.21 epoch sec; seqNo:14118 2021-02-13T00:37:07Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.17 psi 2021-02-13T00:37:07Z- handleDepth()-Depth_Keller msg: 1613176627214 epoch sec; seqNo:14233; depth 14.87 2021-02-13T00:37:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176627.22 epoch sec; seqNo:14233; 4059617.53 north; 592851.78 east 2021-02-13T00:37:07Z- AHRS_M2 msg: 1613176627.59 epoch sec; seqNo:14119 2021-02-13T00:37:07Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.18 psi 2021-02-13T00:37:07Z- handleDepth()-Depth_Keller msg: 1613176627592 epoch sec; seqNo:14234; depth 15.00 2021-02-13T00:37:07Z- handleDvl() - RDI_Pathfinder msg: 1613176627.60 epoch sec; seqNo:6875 2021-02-13T00:37:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 96.50 , 78.25 , 113.53 2021-02-13T00:37:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.09 , 0.10 2021-02-13T00:37:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176627.63 epoch sec; seqNo:14234; 4059617.53 north; 592851.78 east 2021-02-13T00:37:07Z- AHRS_M2 msg: 1613176627.99 epoch sec; seqNo:14120 2021-02-13T00:37:07Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.19 psi 2021-02-13T00:37:08Z- handleDepth()-Depth_Keller msg: 1613176628001 epoch sec; seqNo:14235; depth 15.11 2021-02-13T00:37:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176628.01 epoch sec; seqNo:14235; 4059617.91 north; 592851.77 east 2021-02-13T00:37:08Z- AHRS_M2 msg: 1613176628.40 epoch sec; seqNo:14121 2021-02-13T00:37:08Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.20 psi 2021-02-13T00:37:08Z- handleDepth()-Depth_Keller msg: 1613176628409 epoch sec; seqNo:14236; depth 15.23 2021-02-13T00:37:08Z- handleDvl() - RDI_Pathfinder msg: 1613176628.42 epoch sec; seqNo:6876 2021-02-13T00:37:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.29 , 74.78 , 111.17 2021-02-13T00:37:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.12 , 0.03 2021-02-13T00:37:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176628.43 epoch sec; seqNo:14236; 4059618.28 north; 592851.77 east 2021-02-13T00:37:08Z- AHRS_M2 msg: 1613176628.80 epoch sec; seqNo:14122 2021-02-13T00:37:08Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.20 psi 2021-02-13T00:37:08Z- handleDepth()-Depth_Keller msg: 1613176628821 epoch sec; seqNo:14237; depth 15.36 2021-02-13T00:37:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176628.83 epoch sec; seqNo:14237; 4059618.66 north; 592851.76 east 2021-02-13T00:37:09Z- AHRS_M2 msg: 1613176629.20 epoch sec; seqNo:14123 2021-02-13T00:37:09Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.20 psi 2021-02-13T00:37:09Z- handleDepth()-Depth_Keller msg: 1613176629208 epoch sec; seqNo:14238; depth 15.48 2021-02-13T00:37:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176629.22 epoch sec; seqNo:14238; 4059618.66 north; 592851.76 east 2021-02-13T00:37:09Z- AHRS_M2 msg: 1613176629.61 epoch sec; seqNo:14124 2021-02-13T00:37:09Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.20 psi 2021-02-13T00:37:09Z- handleDepth()-Depth_Keller msg: 1613176629612 epoch sec; seqNo:14239; depth 15.60 2021-02-13T00:37:09Z- handleDvl() - RDI_Pathfinder msg: 1613176629.62 epoch sec; seqNo:6877 2021-02-13T00:37:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.29 , 100.21 , 73.96 , 108.33 2021-02-13T00:37:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.13 , -0.01 2021-02-13T00:37:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176629.63 epoch sec; seqNo:14239; 4059619.05 north; 592852.98 east 2021-02-13T00:37:10Z- AHRS_M2 msg: 1613176630.01 epoch sec; seqNo:14125 2021-02-13T00:37:10Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.20 psi 2021-02-13T00:37:10Z- handleDepth()-Depth_Keller msg: 1613176630021 epoch sec; seqNo:14240; depth 15.72 2021-02-13T00:37:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176630.03 epoch sec; seqNo:14240; 4059619.43 north; 592852.98 east 2021-02-13T00:37:10Z- LcmTrn::handleMessages() - handling 44 messages took 0.57 ms and 133 calls... 2021-02-13T00:37:10Z- waiting for 1613176627.78, time is 1613176630.03 2021-02-13T00:37:10Z- LcmTrn::updateTrn() >>>> pose time 1613176630.03, meas time 1613176629.62 <<<< 2021-02-13T00:37:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176629.62 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:37:10Z- TerrainNav::motionUpdate() - currEstimate = (4059619, 592852.98, 15.72) 2021-02-13T00:37:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:37:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:37:10Z- TNavBF::Projecting Measurements 2021-02-13T00:37:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:37:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:37:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:37:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:37:10Z- TNavFilter::4 good beams. 2021-02-13T00:37:10Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:37:10Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:37:10Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:37:10Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:37:10Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:37:10Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:37:10Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:37:10Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:10Z- TerrainMapOctree: (vehN, vehE ) = (4059619.17, 592852.79). 2021-02-13T00:37:10Z- TerrainMapOctree: Min Distance = 2147.51. 2021-02-13T00:37:10Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:37:10Z- There is no specific good beam that all particles have in common. 2021-02-13T00:37:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:37:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 10.57 2021-02-13T00:37:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:37:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 16.07 2021-02-13T00:37:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:37:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 8.75 2021-02-13T00:37:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:37:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 18.78 2021-02-13T00:37:10Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:37:10Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:37:10Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:37:10Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:37:10Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:37:10Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:37:10Z- Filter 0 No beams used in subcloud update 2021-02-13T00:37:10Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:37:10Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:37:10Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 694.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:37:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176629.62 sec. 2021-02-13T00:37:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:37:10Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:10Z- variances([0], [2], and psi_berg) = 694.825352 832.995297 0.000000 2021-02-13T00:37:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:37:10Z- LcmTrn::publishEstimates() - MLE : 4059618.93 592884.48 15.72 2021-02-13T00:37:10Z- LcmTrn::publishEstimates() - MMSE : 4059619.43 592852.98 15.72 2021-02-13T00:37:10Z- LcmTrn::publishEstimates() - COVAR: 694.83 833.00 0.85 0.00 2021-02-13T00:37:10Z- AHRS_M2 msg: 1613176630.43 epoch sec; seqNo:14126 2021-02-13T00:37:10Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.20 psi 2021-02-13T00:37:10Z- handleDepth()-Depth_Keller msg: 1613176630437 epoch sec; seqNo:14241; depth 15.85 2021-02-13T00:37:10Z- handleDvl() - RDI_Pathfinder msg: 1613176630.45 epoch sec; seqNo:6878 2021-02-13T00:37:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 101.30 , 78.18 , 109.81 2021-02-13T00:37:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.12 , -0.05 2021-02-13T00:37:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176630.46 epoch sec; seqNo:14241; 4059619.81 north; 592852.97 east 2021-02-13T00:37:10Z- AHRS_M2 msg: 1613176630.82 epoch sec; seqNo:14127 2021-02-13T00:37:10Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.21 psi 2021-02-13T00:37:10Z- handleDepth()-Depth_Keller msg: 1613176630824 epoch sec; seqNo:14242; depth 15.98 2021-02-13T00:37:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176630.84 epoch sec; seqNo:14242; 4059619.81 north; 592852.97 east 2021-02-13T00:37:11Z- AHRS_M2 msg: 1613176631.22 epoch sec; seqNo:14128 2021-02-13T00:37:11Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.21 psi 2021-02-13T00:37:11Z- handleDepth()-Depth_Keller msg: 1613176631233 epoch sec; seqNo:14243; depth 16.10 2021-02-13T00:37:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176631.24 epoch sec; seqNo:14243; 4059620.19 north; 592852.97 east 2021-02-13T00:37:11Z- AHRS_M2 msg: 1613176631.63 epoch sec; seqNo:14129 2021-02-13T00:37:11Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.20 psi 2021-02-13T00:37:11Z- handleDepth()-Depth_Keller msg: 1613176631641 epoch sec; seqNo:14244; depth 16.23 2021-02-13T00:37:11Z- handleDvl() - RDI_Pathfinder msg: 1613176631.65 epoch sec; seqNo:6879 2021-02-13T00:37:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.46 , 99.07 , 78.79 , 112.58 2021-02-13T00:37:11Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.08 , -0.11 2021-02-13T00:37:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176631.66 epoch sec; seqNo:14244; 4059620.58 north; 592854.19 east 2021-02-13T00:37:12Z- AHRS_M2 msg: 1613176632.03 epoch sec; seqNo:14130 2021-02-13T00:37:12Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.20 psi 2021-02-13T00:37:12Z- handleDepth()-Depth_Keller msg: 1613176632036 epoch sec; seqNo:14245; depth 16.36 2021-02-13T00:37:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176632.05 epoch sec; seqNo:14245; 4059620.96 north; 592854.18 east 2021-02-13T00:37:12Z- AHRS_M2 msg: 1613176632.44 epoch sec; seqNo:14131 2021-02-13T00:37:12Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-13T00:37:12Z- handleDepth()-Depth_Keller msg: 1613176632440 epoch sec; seqNo:14246; depth 16.48 2021-02-13T00:37:12Z- handleDvl() - RDI_Pathfinder msg: 1613176632.45 epoch sec; seqNo:6880 2021-02-13T00:37:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.14 , 100.42 , 73.56 , 107.18 2021-02-13T00:37:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.02 , -0.15 2021-02-13T00:37:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176632.47 epoch sec; seqNo:14246; 4059621.34 north; 592854.18 east 2021-02-13T00:37:12Z- AHRS_M2 msg: 1613176632.84 epoch sec; seqNo:14132 2021-02-13T00:37:12Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.18 psi 2021-02-13T00:37:12Z- handleDepth()-Depth_Keller msg: 1613176632845 epoch sec; seqNo:14247; depth 16.62 2021-02-13T00:37:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176632.87 epoch sec; seqNo:14247; 4059621.72 north; 592854.17 east 2021-02-13T00:37:13Z- AHRS_M2 msg: 1613176633.25 epoch sec; seqNo:14133 2021-02-13T00:37:13Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.17 psi 2021-02-13T00:37:13Z- handleDepth()-Depth_Keller msg: 1613176633255 epoch sec; seqNo:14248; depth 16.76 2021-02-13T00:37:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176633.26 epoch sec; seqNo:14248; 4059621.72 north; 592854.17 east 2021-02-13T00:37:13Z- AHRS_M2 msg: 1613176633.65 epoch sec; seqNo:14134 2021-02-13T00:37:13Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.17 psi 2021-02-13T00:37:13Z- handleDepth()-Depth_Keller msg: 1613176633657 epoch sec; seqNo:14249; depth 16.89 2021-02-13T00:37:13Z- handleDvl() - RDI_Pathfinder msg: 1613176633.67 epoch sec; seqNo:6881 2021-02-13T00:37:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.64 , 97.71 , 78.25 , 108.66 2021-02-13T00:37:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.12 , -0.14 2021-02-13T00:37:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176633.68 epoch sec; seqNo:14249; 4059622.10 north; 592854.17 east 2021-02-13T00:37:14Z- AHRS_M2 msg: 1613176634.05 epoch sec; seqNo:14135 2021-02-13T00:37:14Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.17 psi 2021-02-13T00:37:14Z- handleDepth()-Depth_Keller msg: 1613176634065 epoch sec; seqNo:14250; depth 16.99 2021-02-13T00:37:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176634.08 epoch sec; seqNo:14250; 4059622.49 north; 592855.39 east 2021-02-13T00:37:14Z- AHRS_M2 msg: 1613176634.46 epoch sec; seqNo:14136 2021-02-13T00:37:14Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.17 psi 2021-02-13T00:37:14Z- handleDepth()-Depth_Keller msg: 1613176634460 epoch sec; seqNo:14251; depth 17.14 2021-02-13T00:37:14Z- handleDvl() - RDI_Pathfinder msg: 1613176634.48 epoch sec; seqNo:6882 2021-02-13T00:37:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.46 , 95.28 , 69.88 , 109.88 2021-02-13T00:37:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.16 , -0.07 2021-02-13T00:37:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176634.49 epoch sec; seqNo:14251; 4059622.87 north; 592855.38 east 2021-02-13T00:37:14Z- AHRS_M2 msg: 1613176634.86 epoch sec; seqNo:14137 2021-02-13T00:37:14Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.16 psi 2021-02-13T00:37:14Z- handleDepth()-Depth_Keller msg: 1613176634864 epoch sec; seqNo:14252; depth 17.26 2021-02-13T00:37:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176634.88 epoch sec; seqNo:14252; 4059623.25 north; 592855.38 east 2021-02-13T00:37:15Z- AHRS_M2 msg: 1613176635.26 epoch sec; seqNo:14138 2021-02-13T00:37:15Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.16 psi 2021-02-13T00:37:15Z- handleDepth()-Depth_Keller msg: 1613176635269 epoch sec; seqNo:14253; depth 17.37 2021-02-13T00:37:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176635.28 epoch sec; seqNo:14253; 4059623.63 north; 592855.37 east 2021-02-13T00:37:15Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:37:15Z- waiting for 1613176633.03, time is 1613176635.28 2021-02-13T00:37:15Z- LcmTrn::updateTrn() >>>> pose time 1613176635.28, meas time 1613176634.48 <<<< 2021-02-13T00:37:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176634.48 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:37:15Z- TerrainNav::motionUpdate() - currEstimate = (4059624, 592855.37, 17.37) 2021-02-13T00:37:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:37:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:37:15Z- TNavBF::Projecting Measurements 2021-02-13T00:37:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:37:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:37:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:37:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:37:15Z- TNavFilter::4 good beams. 2021-02-13T00:37:15Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:37:15Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:37:15Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:37:15Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:37:15Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:37:15Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:37:15Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:37:15Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:15Z- TerrainMapOctree: (vehN, vehE ) = (4059622.99, 592855.01). 2021-02-13T00:37:15Z- TerrainMapOctree: Min Distance = 2143.17. 2021-02-13T00:37:15Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:37:15Z- There is no specific good beam that all particles have in common. 2021-02-13T00:37:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:37:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 10.10 2021-02-13T00:37:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:37:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 14.53 2021-02-13T00:37:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:37:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 7.81 2021-02-13T00:37:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:37:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 19.32 2021-02-13T00:37:15Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:37:15Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:37:15Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:37:15Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:37:15Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:37:15Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:37:15Z- Filter 0 No beams used in subcloud update 2021-02-13T00:37:15Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:37:15Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:37:15Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 694.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:37:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176634.48 sec. 2021-02-13T00:37:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:37:15Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:15Z- variances([0], [2], and psi_berg) = 694.751564 832.995260 0.000000 2021-02-13T00:37:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:37:15Z- LcmTrn::publishEstimates() - MLE : 4059623.13 592886.87 17.37 2021-02-13T00:37:15Z- LcmTrn::publishEstimates() - MMSE : 4059623.63 592855.37 17.37 2021-02-13T00:37:15Z- LcmTrn::publishEstimates() - COVAR: 694.75 833.00 0.84 0.00 2021-02-13T00:37:15Z- AHRS_M2 msg: 1613176635.67 epoch sec; seqNo:14139 2021-02-13T00:37:15Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.17 psi 2021-02-13T00:37:15Z- handleDepth()-Depth_Keller msg: 1613176635678 epoch sec; seqNo:14254; depth 17.52 2021-02-13T00:37:15Z- handleDvl() - RDI_Pathfinder msg: 1613176635.69 epoch sec; seqNo:6883 2021-02-13T00:37:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.60 , 97.85 , 78.38 , 108.80 2021-02-13T00:37:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.13 , -0.01 2021-02-13T00:37:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176635.70 epoch sec; seqNo:14254; 4059624.01 north; 592855.37 east 2021-02-13T00:37:16Z- AHRS_M2 msg: 1613176636.07 epoch sec; seqNo:14140 2021-02-13T00:37:16Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.17 psi 2021-02-13T00:37:16Z- handleDepth()-Depth_Keller msg: 1613176636081 epoch sec; seqNo:14255; depth 17.64 2021-02-13T00:37:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176636.10 epoch sec; seqNo:14255; 4059624.01 north; 592855.37 east 2021-02-13T00:37:16Z- AHRS_M2 msg: 1613176636.48 epoch sec; seqNo:14141 2021-02-13T00:37:16Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.17 psi 2021-02-13T00:37:16Z- handleDepth()-Depth_Keller msg: 1613176636480 epoch sec; seqNo:14256; depth 17.77 2021-02-13T00:37:16Z- handleDvl() - RDI_Pathfinder msg: 1613176636.49 epoch sec; seqNo:6884 2021-02-13T00:37:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.05 , 97.65 , 69.06 , 108.60 2021-02-13T00:37:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.13 , 0.01 2021-02-13T00:37:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176636.50 epoch sec; seqNo:14256; 4059624.39 north; 592855.37 east 2021-02-13T00:37:16Z- AHRS_M2 msg: 1613176636.88 epoch sec; seqNo:14142 2021-02-13T00:37:16Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.17 psi 2021-02-13T00:37:16Z- handleDepth()-Depth_Keller msg: 1613176636889 epoch sec; seqNo:14257; depth 17.90 2021-02-13T00:37:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176636.90 epoch sec; seqNo:14257; 4059624.76 north; 592855.36 east 2021-02-13T00:37:17Z- AHRS_M2 msg: 1613176637.28 epoch sec; seqNo:14143 2021-02-13T00:37:17Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.17 psi 2021-02-13T00:37:17Z- handleDepth()-Depth_Keller msg: 1613176637289 epoch sec; seqNo:14258; depth 18.03 2021-02-13T00:37:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176637.31 epoch sec; seqNo:14258; 4059625.16 north; 592856.58 east 2021-02-13T00:37:17Z- AHRS_M2 msg: 1613176637.69 epoch sec; seqNo:14144 2021-02-13T00:37:17Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.18 psi 2021-02-13T00:37:17Z- handleDepth()-Depth_Keller msg: 1613176637692 epoch sec; seqNo:14259; depth 18.18 2021-02-13T00:37:17Z- handleDvl() - RDI_Pathfinder msg: 1613176637.70 epoch sec; seqNo:6885 2021-02-13T00:37:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 97.92 , 71.11 , 110.08 2021-02-13T00:37:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.11 , 0.02 2021-02-13T00:37:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176637.71 epoch sec; seqNo:14259; 4059625.53 north; 592856.57 east 2021-02-13T00:37:18Z- AHRS_M2 msg: 1613176638.09 epoch sec; seqNo:14145 2021-02-13T00:37:18Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-13T00:37:18Z- handleDepth()-Depth_Keller msg: 1613176638096 epoch sec; seqNo:14260; depth 18.32 2021-02-13T00:37:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176638.13 epoch sec; seqNo:14260; 4059625.91 north; 592856.57 east 2021-02-13T00:37:18Z- AHRS_M2 msg: 1613176638.50 epoch sec; seqNo:14146 2021-02-13T00:37:18Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.17 psi 2021-02-13T00:37:18Z- handleDepth()-Depth_Keller msg: 1613176638500 epoch sec; seqNo:14261; depth 18.43 2021-02-13T00:37:18Z- handleDvl() - RDI_Pathfinder msg: 1613176638.51 epoch sec; seqNo:6886 2021-02-13T00:37:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 97.78 , 78.31 , 108.73 2021-02-13T00:37:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.10 , 0.03 2021-02-13T00:37:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176638.53 epoch sec; seqNo:14261; 4059626.29 north; 592856.57 east 2021-02-13T00:37:18Z- AHRS_M2 msg: 1613176638.90 epoch sec; seqNo:14147 2021-02-13T00:37:18Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.17 psi 2021-02-13T00:37:18Z- handleDepth()-Depth_Keller msg: 1613176638909 epoch sec; seqNo:14262; depth 18.59 2021-02-13T00:37:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176638.93 epoch sec; seqNo:14262; 4059626.29 north; 592856.57 east 2021-02-13T00:37:19Z- AHRS_M2 msg: 1613176639.31 epoch sec; seqNo:14148 2021-02-13T00:37:19Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.17 psi 2021-02-13T00:37:19Z- handleDepth()-Depth_Keller msg: 1613176639313 epoch sec; seqNo:14263; depth 18.72 2021-02-13T00:37:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176639.34 epoch sec; seqNo:14263; 4059626.67 north; 592856.56 east 2021-02-13T00:37:19Z- AHRS_M2 msg: 1613176639.71 epoch sec; seqNo:14149 2021-02-13T00:37:19Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.16 psi 2021-02-13T00:37:19Z- handleDepth()-Depth_Keller msg: 1613176639714 epoch sec; seqNo:14264; depth 18.88 2021-02-13T00:37:19Z- handleDvl() - RDI_Pathfinder msg: 1613176639.73 epoch sec; seqNo:6887 2021-02-13T00:37:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.25 , 97.71 , 69.06 , 107.45 2021-02-13T00:37:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , 0.07 2021-02-13T00:37:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176639.74 epoch sec; seqNo:14264; 4059627.05 north; 592856.56 east 2021-02-13T00:37:20Z- AHRS_M2 msg: 1613176640.12 epoch sec; seqNo:14150 2021-02-13T00:37:20Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.16 psi 2021-02-13T00:37:20Z- handleDepth()-Depth_Keller msg: 1613176640122 epoch sec; seqNo:14265; depth 19.00 2021-02-13T00:37:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176640.15 epoch sec; seqNo:14265; 4059627.44 north; 592857.77 east 2021-02-13T00:37:20Z- LcmTrn::handleMessages() - handling 41 messages took 0.69 ms and 127 calls... 2021-02-13T00:37:20Z- waiting for 1613176638.28, time is 1613176640.15 2021-02-13T00:37:20Z- LcmTrn::updateTrn() >>>> pose time 1613176640.15, meas time 1613176639.73 <<<< 2021-02-13T00:37:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176639.73 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:37:20Z- TerrainNav::motionUpdate() - currEstimate = (4059627, 592857.77, 19.00) 2021-02-13T00:37:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:37:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:37:20Z- TNavBF::Projecting Measurements 2021-02-13T00:37:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:37:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:37:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:37:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:37:20Z- TNavFilter::4 good beams. 2021-02-13T00:37:20Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:37:20Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:37:20Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:37:20Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:37:20Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:37:20Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:37:20Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:37:20Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:20Z- TerrainMapOctree: (vehN, vehE ) = (4059627.11, 592857.56). 2021-02-13T00:37:20Z- TerrainMapOctree: Min Distance = 2138.38. 2021-02-13T00:37:20Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:37:20Z- There is no specific good beam that all particles have in common. 2021-02-13T00:37:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:37:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 9.80 2021-02-13T00:37:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:37:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:37:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:37:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-13T00:37:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:37:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 18.47 2021-02-13T00:37:20Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:37:20Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:37:20Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:37:20Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:37:20Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:37:20Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:37:20Z- Filter 0 No beams used in subcloud update 2021-02-13T00:37:20Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:37:20Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:37:20Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 694.7 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:37:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176639.73 sec. 2021-02-13T00:37:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:37:20Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:20Z- variances([0], [2], and psi_berg) = 694.683602 832.995227 0.000000 2021-02-13T00:37:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:37:20Z- LcmTrn::publishEstimates() - MLE : 4059626.94 592889.27 19.00 2021-02-13T00:37:20Z- LcmTrn::publishEstimates() - MMSE : 4059627.44 592857.77 19.00 2021-02-13T00:37:20Z- LcmTrn::publishEstimates() - COVAR: 694.68 833.00 0.82 0.00 2021-02-13T00:37:20Z- AHRS_M2 msg: 1613176640.52 epoch sec; seqNo:14151 2021-02-13T00:37:20Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.15 psi 2021-02-13T00:37:20Z- handleDepth()-Depth_Keller msg: 1613176640520 epoch sec; seqNo:14266; depth 19.15 2021-02-13T00:37:20Z- handleDvl() - RDI_Pathfinder msg: 1613176640.53 epoch sec; seqNo:6888 2021-02-13T00:37:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.05 , 97.92 , 69.88 , 106.43 2021-02-13T00:37:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.01 , 0.09 2021-02-13T00:37:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176640.56 epoch sec; seqNo:14266; 4059627.44 north; 592857.77 east 2021-02-13T00:37:20Z- AHRS_M2 msg: 1613176640.92 epoch sec; seqNo:14152 2021-02-13T00:37:20Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.15 psi 2021-02-13T00:37:20Z- handleDepth()-Depth_Keller msg: 1613176640929 epoch sec; seqNo:14267; depth 19.28 2021-02-13T00:37:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176640.94 epoch sec; seqNo:14267; 4059627.82 north; 592857.77 east 2021-02-13T00:37:21Z- AHRS_M2 msg: 1613176641.35 epoch sec; seqNo:14153 2021-02-13T00:37:21Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.15 psi 2021-02-13T00:37:21Z- handleDepth()-Depth_Keller msg: 1613176641355 epoch sec; seqNo:14268; depth 19.43 2021-02-13T00:37:21Z- handleDvl() - RDI_Pathfinder msg: 1613176641.37 epoch sec; seqNo:6889 2021-02-13T00:37:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.67 , 97.92 , 67.84 , 108.87 2021-02-13T00:37:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.09 , 0.05 2021-02-13T00:37:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176641.40 epoch sec; seqNo:14268; 4059628.20 north; 592857.77 east 2021-02-13T00:37:21Z- AHRS_M2 msg: 1613176641.73 epoch sec; seqNo:14154 2021-02-13T00:37:21Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.15 psi 2021-02-13T00:37:21Z- handleDepth()-Depth_Keller msg: 1613176641732 epoch sec; seqNo:14269; depth 19.58 2021-02-13T00:37:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176641.75 epoch sec; seqNo:14269; 4059628.20 north; 592857.77 east 2021-02-13T00:37:22Z- AHRS_M2 msg: 1613176642.13 epoch sec; seqNo:14155 2021-02-13T00:37:22Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.16 psi 2021-02-13T00:37:22Z- handleDepth()-Depth_Keller msg: 1613176642140 epoch sec; seqNo:14270; depth 19.70 2021-02-13T00:37:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176642.15 epoch sec; seqNo:14270; 4059628.58 north; 592857.76 east 2021-02-13T00:37:22Z- AHRS_M2 msg: 1613176642.54 epoch sec; seqNo:14156 2021-02-13T00:37:22Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.16 psi 2021-02-13T00:37:22Z- handleDepth()-Depth_Keller msg: 1613176642540 epoch sec; seqNo:14271; depth 19.84 2021-02-13T00:37:22Z- handleDvl() - RDI_Pathfinder msg: 1613176642.55 epoch sec; seqNo:6890 2021-02-13T00:37:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.92 , 67.84 , 108.87 2021-02-13T00:37:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.19 , -0.02 2021-02-13T00:37:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176642.56 epoch sec; seqNo:14271; 4059628.59 north; 592858.98 east 2021-02-13T00:37:22Z- AHRS_M2 msg: 1613176642.94 epoch sec; seqNo:14157 2021-02-13T00:37:22Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.17 psi 2021-02-13T00:37:22Z- handleDepth()-Depth_Keller msg: 1613176642947 epoch sec; seqNo:14272; depth 19.98 2021-02-13T00:37:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176642.96 epoch sec; seqNo:14272; 4059628.97 north; 592858.98 east 2021-02-13T00:37:23Z- AHRS_M2 msg: 1613176643.34 epoch sec; seqNo:14158 2021-02-13T00:37:23Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.18 psi 2021-02-13T00:37:23Z- handleDepth()-Depth_Keller msg: 1613176643353 epoch sec; seqNo:14273; depth 20.12 2021-02-13T00:37:23Z- handleDvl() - RDI_Pathfinder msg: 1613176643.37 epoch sec; seqNo:6891 2021-02-13T00:37:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 97.65 , 67.43 , 107.38 2021-02-13T00:37:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.08 , -0.11 2021-02-13T00:37:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176643.38 epoch sec; seqNo:14273; 4059629.35 north; 592858.97 east 2021-02-13T00:37:23Z- AHRS_M2 msg: 1613176643.75 epoch sec; seqNo:14159 2021-02-13T00:37:23Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.19 psi 2021-02-13T00:37:23Z- handleDepth()-Depth_Keller msg: 1613176643757 epoch sec; seqNo:14274; depth 20.27 2021-02-13T00:37:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176643.77 epoch sec; seqNo:14274; 4059629.73 north; 592858.97 east 2021-02-13T00:37:24Z- AHRS_M2 msg: 1613176644.16 epoch sec; seqNo:14160 2021-02-13T00:37:24Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.19 psi 2021-02-13T00:37:24Z- handleDepth()-Depth_Keller msg: 1613176644168 epoch sec; seqNo:14275; depth 20.40 2021-02-13T00:37:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176644.18 epoch sec; seqNo:14275; 4059629.74 north; 592860.19 east 2021-02-13T00:37:24Z- AHRS_M2 msg: 1613176644.56 epoch sec; seqNo:14161 2021-02-13T00:37:24Z- AHRS_M2 msg: 0.66 phi; -0.36 theta; -0.20 psi 2021-02-13T00:37:24Z- handleDepth()-Depth_Keller msg: 1613176644565 epoch sec; seqNo:14276; depth 20.55 2021-02-13T00:37:24Z- handleDvl() - RDI_Pathfinder msg: 1613176644.59 epoch sec; seqNo:6892 2021-02-13T00:37:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.01 , 97.98 , 78.51 , 108.93 2021-02-13T00:37:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , -0.12 2021-02-13T00:37:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176644.60 epoch sec; seqNo:14276; 4059630.12 north; 592860.19 east 2021-02-13T00:37:24Z- AHRS_M2 msg: 1613176644.96 epoch sec; seqNo:14162 2021-02-13T00:37:24Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.20 psi 2021-02-13T00:37:24Z- handleDepth()-Depth_Keller msg: 1613176644964 epoch sec; seqNo:14277; depth 20.69 2021-02-13T00:37:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176644.98 epoch sec; seqNo:14277; 4059630.50 north; 592860.18 east 2021-02-13T00:37:25Z- AHRS_M2 msg: 1613176645.37 epoch sec; seqNo:14163 2021-02-13T00:37:25Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.20 psi 2021-02-13T00:37:25Z- handleDepth()-Depth_Keller msg: 1613176645371 epoch sec; seqNo:14278; depth 20.84 2021-02-13T00:37:25Z- handleDvl() - RDI_Pathfinder msg: 1613176645.38 epoch sec; seqNo:6893 2021-02-13T00:37:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.25 , 97.71 , 68.65 , 108.66 2021-02-13T00:37:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , -0.06 2021-02-13T00:37:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176645.39 epoch sec; seqNo:14278; 4059630.88 north; 592860.18 east 2021-02-13T00:37:25Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-13T00:37:25Z- waiting for 1613176643.15, time is 1613176645.39 2021-02-13T00:37:25Z- LcmTrn::updateTrn() equating pose 1613176645.39 and meas times 1613176645.39 2021-02-13T00:37:25Z- LcmTrn::updateTrn() >>>> pose time 1613176645.39, meas time 1613176645.39 <<<< 2021-02-13T00:37:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176645.39 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:37:25Z- TerrainNav::motionUpdate() - currEstimate = (4059631, 592860.18, 20.84) 2021-02-13T00:37:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:37:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:37:25Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176645.39 sec.; No relevant navigation data available 2021-02-13T00:37:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:37:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:25Z- variances([0], [2], and psi_berg) = 694.631225 832.995201 0.000000 2021-02-13T00:37:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:37:25Z- LcmTrn::publishEstimates() - MLE : 4059630.38 592890.68 20.84 2021-02-13T00:37:25Z- LcmTrn::publishEstimates() - MMSE : 4059630.88 592860.18 20.84 2021-02-13T00:37:25Z- LcmTrn::publishEstimates() - COVAR: 694.63 833.00 1.03 0.00 2021-02-13T00:37:25Z- AHRS_M2 msg: 1613176645.77 epoch sec; seqNo:14164 2021-02-13T00:37:25Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.20 psi 2021-02-13T00:37:25Z- handleDepth()-Depth_Keller msg: 1613176645774 epoch sec; seqNo:14279; depth 20.99 2021-02-13T00:37:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:37:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:37:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:37:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:37:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:37:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:37:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:37:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:37:25Z- handleDvl() - RDI_Pathfinder msg: 1613176645.78 epoch sec; seqNo:6894 2021-02-13T00:37:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:37:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , -0.06 2021-02-13T00:37:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176645.79 epoch sec; seqNo:14279; 4059630.88 north; 592860.18 east 2021-02-13T00:37:26Z- AHRS_M2 msg: 1613176646.17 epoch sec; seqNo:14165 2021-02-13T00:37:26Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.20 psi 2021-02-13T00:37:26Z- handleDepth()-Depth_Keller msg: 1613176646195 epoch sec; seqNo:14280; depth 21.15 2021-02-13T00:37:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176646.20 epoch sec; seqNo:14280; 4059631.26 north; 592860.17 east 2021-02-13T00:37:26Z- AHRS_M2 msg: 1613176646.58 epoch sec; seqNo:14166 2021-02-13T00:37:26Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.19 psi 2021-02-13T00:37:26Z- handleDepth()-Depth_Keller msg: 1613176646580 epoch sec; seqNo:14281; depth 21.28 2021-02-13T00:37:26Z- handleDvl() - RDI_Pathfinder msg: 1613176646.59 epoch sec; seqNo:6895 2021-02-13T00:37:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 94.27 , 68.65 , 108.87 2021-02-13T00:37:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.01 2021-02-13T00:37:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176646.62 epoch sec; seqNo:14281; 4059631.65 north; 592861.39 east 2021-02-13T00:37:26Z- AHRS_M2 msg: 1613176646.98 epoch sec; seqNo:14167 2021-02-13T00:37:26Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.19 psi 2021-02-13T00:37:26Z- handleDepth()-Depth_Keller msg: 1613176646990 epoch sec; seqNo:14282; depth 21.42 2021-02-13T00:37:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:37:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:37:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:37:26Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:37:26Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:37:26Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:37:26Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:37:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:37:26Z- handleDvl() - RDI_Pathfinder msg: 1613176646.99 epoch sec; seqNo:6896 2021-02-13T00:37:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:37:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.01 2021-02-13T00:37:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176647.01 epoch sec; seqNo:14282; 4059632.03 north; 592861.39 east 2021-02-13T00:37:27Z- AHRS_M2 msg: 1613176647.38 epoch sec; seqNo:14168 2021-02-13T00:37:27Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.19 psi 2021-02-13T00:37:27Z- handleDepth()-Depth_Keller msg: 1613176647390 epoch sec; seqNo:14283; depth 21.58 2021-02-13T00:37:27Z- handleDvl() - RDI_Pathfinder msg: 1613176647.40 epoch sec; seqNo:6897 2021-02-13T00:37:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 94.33 , 0.00 , 107.72 2021-02-13T00:37:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , 0.04 2021-02-13T00:37:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176647.41 epoch sec; seqNo:14283; 4059632.03 north; 592861.39 east 2021-02-13T00:37:27Z- AHRS_M2 msg: 1613176647.79 epoch sec; seqNo:14169 2021-02-13T00:37:27Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.19 psi 2021-02-13T00:37:27Z- handleDepth()-Depth_Keller msg: 1613176647792 epoch sec; seqNo:14284; depth 21.71 2021-02-13T00:37:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176647.81 epoch sec; seqNo:14284; 4059632.41 north; 592861.38 east 2021-02-13T00:37:28Z- AHRS_M2 msg: 1613176648.19 epoch sec; seqNo:14170 2021-02-13T00:37:28Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.20 psi 2021-02-13T00:37:28Z- handleDepth()-Depth_Keller msg: 1613176648200 epoch sec; seqNo:14285; depth 21.86 2021-02-13T00:37:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:37:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:37:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:37:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:37:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:37:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:37:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:37:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:37:28Z- handleDvl() - RDI_Pathfinder msg: 1613176648.20 epoch sec; seqNo:6898 2021-02-13T00:37:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:37:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , 0.04 2021-02-13T00:37:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176648.21 epoch sec; seqNo:14285; 4059632.79 north; 592861.38 east 2021-02-13T00:37:28Z- AHRS_M2 msg: 1613176648.60 epoch sec; seqNo:14171 2021-02-13T00:37:28Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.20 psi 2021-02-13T00:37:28Z- handleDepth()-Depth_Keller msg: 1613176648601 epoch sec; seqNo:14286; depth 22.01 2021-02-13T00:37:28Z- handleDvl() - RDI_Pathfinder msg: 1613176648.61 epoch sec; seqNo:6899 2021-02-13T00:37:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 92.17 , 67.02 , 108.39 2021-02-13T00:37:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , -0.01 2021-02-13T00:37:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176648.62 epoch sec; seqNo:14286; 4059632.79 north; 592861.38 east 2021-02-13T00:37:29Z- AHRS_M2 msg: 1613176649.00 epoch sec; seqNo:14172 2021-02-13T00:37:29Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.20 psi 2021-02-13T00:37:29Z- handleDepth()-Depth_Keller msg: 1613176649006 epoch sec; seqNo:14287; depth 22.16 2021-02-13T00:37:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176649.02 epoch sec; seqNo:14287; 4059633.18 north; 592862.59 east 2021-02-13T00:37:29Z- AHRS_M2 msg: 1613176649.40 epoch sec; seqNo:14173 2021-02-13T00:37:29Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.21 psi 2021-02-13T00:37:29Z- handleDepth()-Depth_Keller msg: 1613176649418 epoch sec; seqNo:14288; depth 22.29 2021-02-13T00:37:29Z- handleDvl() - RDI_Pathfinder msg: 1613176649.43 epoch sec; seqNo:6900 2021-02-13T00:37:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 98.19 , 67.02 , 109.14 2021-02-13T00:37:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.02 , -0.08 2021-02-13T00:37:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176649.45 epoch sec; seqNo:14288; 4059633.56 north; 592862.59 east 2021-02-13T00:37:29Z- AHRS_M2 msg: 1613176649.81 epoch sec; seqNo:14174 2021-02-13T00:37:29Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.21 psi 2021-02-13T00:37:29Z- handleDepth()-Depth_Keller msg: 1613176649817 epoch sec; seqNo:14289; depth 22.46 2021-02-13T00:37:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176649.84 epoch sec; seqNo:14289; 4059633.94 north; 592862.59 east 2021-02-13T00:37:30Z- AHRS_M2 msg: 1613176650.22 epoch sec; seqNo:14175 2021-02-13T00:37:30Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.22 psi 2021-02-13T00:37:30Z- handleDepth()-Depth_Keller msg: 1613176650228 epoch sec; seqNo:14290; depth 22.59 2021-02-13T00:37:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176650.24 epoch sec; seqNo:14290; 4059634.31 north; 592862.58 east 2021-02-13T00:37:30Z- LcmTrn::handleMessages() - handling 43 messages took 0.66 ms and 130 calls... 2021-02-13T00:37:30Z- waiting for 1613176648.39, time is 1613176650.24 2021-02-13T00:37:30Z- LcmTrn::updateTrn() >>>> pose time 1613176650.24, meas time 1613176649.43 <<<< 2021-02-13T00:37:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176649.43 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:37:30Z- TerrainNav::motionUpdate() - currEstimate = (4059634, 592862.58, 22.59) 2021-02-13T00:37:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:37:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:37:30Z- TNavBF::Projecting Measurements 2021-02-13T00:37:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:37:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:37:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:37:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:37:30Z- TNavFilter::4 good beams. 2021-02-13T00:37:30Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:37:30Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:37:30Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:37:30Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:37:30Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:37:30Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:37:30Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:37:30Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:30Z- TerrainMapOctree: (vehN, vehE ) = (4059633.74, 592862.18). 2021-02-13T00:37:30Z- TerrainMapOctree: Min Distance = 2130.34. 2021-02-13T00:37:30Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:37:30Z- There is no specific good beam that all particles have in common. 2021-02-13T00:37:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:37:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 8.75 2021-02-13T00:37:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:37:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.43 2021-02-13T00:37:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:37:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 7.19 2021-02-13T00:37:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:37:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 19.06 2021-02-13T00:37:30Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:37:30Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:37:30Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:37:30Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:37:30Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:37:30Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:37:30Z- Filter 0 No beams used in subcloud update 2021-02-13T00:37:30Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:37:30Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:37:30Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 694.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:37:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176649.43 sec. 2021-02-13T00:37:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:37:30Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:30Z- variances([0], [2], and psi_berg) = 694.562082 832.995166 0.000000 2021-02-13T00:37:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:37:30Z- LcmTrn::publishEstimates() - MLE : 4059633.81 592895.08 22.59 2021-02-13T00:37:30Z- LcmTrn::publishEstimates() - MMSE : 4059634.31 592862.58 22.59 2021-02-13T00:37:30Z- LcmTrn::publishEstimates() - COVAR: 694.56 833.00 0.91 0.00 2021-02-13T00:37:30Z- AHRS_M2 msg: 1613176650.62 epoch sec; seqNo:14176 2021-02-13T00:37:30Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.22 psi 2021-02-13T00:37:30Z- handleDepth()-Depth_Keller msg: 1613176650626 epoch sec; seqNo:14291; depth 22.75 2021-02-13T00:37:30Z- handleDvl() - RDI_Pathfinder msg: 1613176650.64 epoch sec; seqNo:6901 2021-02-13T00:37:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 97.98 , 0.00 , 107.72 2021-02-13T00:37:30Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.03 , -0.12 2021-02-13T00:37:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176650.65 epoch sec; seqNo:14291; 4059634.69 north; 592862.58 east 2021-02-13T00:37:31Z- AHRS_M2 msg: 1613176651.02 epoch sec; seqNo:14177 2021-02-13T00:37:31Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.22 psi 2021-02-13T00:37:31Z- handleDepth()-Depth_Keller msg: 1613176651025 epoch sec; seqNo:14292; depth 22.90 2021-02-13T00:37:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176651.05 epoch sec; seqNo:14292; 4059634.69 north; 592862.58 east 2021-02-13T00:37:31Z- AHRS_M2 msg: 1613176651.42 epoch sec; seqNo:14178 2021-02-13T00:37:31Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.22 psi 2021-02-13T00:37:31Z- handleDepth()-Depth_Keller msg: 1613176651433 epoch sec; seqNo:14293; depth 23.04 2021-02-13T00:37:31Z- handleDvl() - RDI_Pathfinder msg: 1613176651.44 epoch sec; seqNo:6902 2021-02-13T00:37:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 97.71 , 66.20 , 104.47 2021-02-13T00:37:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.16 , -0.10 2021-02-13T00:37:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176651.47 epoch sec; seqNo:14293; 4059635.09 north; 592863.79 east 2021-02-13T00:37:31Z- AHRS_M2 msg: 1613176651.83 epoch sec; seqNo:14179 2021-02-13T00:37:31Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.22 psi 2021-02-13T00:37:31Z- handleDepth()-Depth_Keller msg: 1613176651833 epoch sec; seqNo:14294; depth 23.17 2021-02-13T00:37:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:37:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:37:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:37:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:37:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:37:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:37:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:37:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:37:31Z- handleDvl() - RDI_Pathfinder msg: 1613176651.84 epoch sec; seqNo:6903 2021-02-13T00:37:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:37:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.16 , -0.10 2021-02-13T00:37:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176651.85 epoch sec; seqNo:14294; 4059635.46 north; 592863.79 east 2021-02-13T00:37:32Z- AHRS_M2 msg: 1613176652.24 epoch sec; seqNo:14180 2021-02-13T00:37:32Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.21 psi 2021-02-13T00:37:32Z- handleDepth()-Depth_Keller msg: 1613176652254 epoch sec; seqNo:14295; depth 23.30 2021-02-13T00:37:32Z- handleDvl() - RDI_Pathfinder msg: 1613176652.27 epoch sec; seqNo:6904 2021-02-13T00:37:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 98.19 , 66.20 , 101.84 2021-02-13T00:37:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.17 , -0.01 2021-02-13T00:37:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176652.28 epoch sec; seqNo:14295; 4059635.84 north; 592863.79 east 2021-02-13T00:37:32Z- AHRS_M2 msg: 1613176652.63 epoch sec; seqNo:14181 2021-02-13T00:37:32Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.21 psi 2021-02-13T00:37:32Z- handleDepth()-Depth_Keller msg: 1613176652645 epoch sec; seqNo:14296; depth 23.46 2021-02-13T00:37:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176652.66 epoch sec; seqNo:14296; 4059636.22 north; 592863.78 east 2021-02-13T00:37:33Z- AHRS_M2 msg: 1613176653.04 epoch sec; seqNo:14182 2021-02-13T00:37:33Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-13T00:37:33Z- handleDepth()-Depth_Keller msg: 1613176653053 epoch sec; seqNo:14297; depth 23.60 2021-02-13T00:37:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:37:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:37:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:37:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:37:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:37:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:37:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:37:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:37:33Z- handleDvl() - RDI_Pathfinder msg: 1613176653.06 epoch sec; seqNo:6905 2021-02-13T00:37:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:37:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.17 , -0.01 2021-02-13T00:37:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176653.06 epoch sec; seqNo:14297; 4059636.60 north; 592863.78 east 2021-02-13T00:37:33Z- AHRS_M2 msg: 1613176653.44 epoch sec; seqNo:14183 2021-02-13T00:37:33Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-13T00:37:33Z- handleDepth()-Depth_Keller msg: 1613176653453 epoch sec; seqNo:14298; depth 23.74 2021-02-13T00:37:33Z- handleDvl() - RDI_Pathfinder msg: 1613176653.47 epoch sec; seqNo:6906 2021-02-13T00:37:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 98.25 , 65.79 , 101.90 2021-02-13T00:37:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.10 , 0.09 2021-02-13T00:37:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176653.50 epoch sec; seqNo:14298; 4059636.98 north; 592863.77 east 2021-02-13T00:37:33Z- AHRS_M2 msg: 1613176653.85 epoch sec; seqNo:14184 2021-02-13T00:37:33Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.19 psi 2021-02-13T00:37:33Z- handleDepth()-Depth_Keller msg: 1613176653857 epoch sec; seqNo:14299; depth 23.88 2021-02-13T00:37:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176653.87 epoch sec; seqNo:14299; 4059636.98 north; 592863.77 east 2021-02-13T00:37:34Z- AHRS_M2 msg: 1613176654.25 epoch sec; seqNo:14185 2021-02-13T00:37:34Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.19 psi 2021-02-13T00:37:34Z- handleDepth()-Depth_Keller msg: 1613176654261 epoch sec; seqNo:14300; depth 24.01 2021-02-13T00:37:34Z- handleDvl() - RDI_Pathfinder msg: 1613176654.27 epoch sec; seqNo:6907 2021-02-13T00:37:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 89.74 , 64.57 , 100.69 2021-02-13T00:37:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.03 , 0.12 2021-02-13T00:37:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176654.28 epoch sec; seqNo:14300; 4059637.36 north; 592863.77 east 2021-02-13T00:37:34Z- AHRS_M2 msg: 1613176654.66 epoch sec; seqNo:14186 2021-02-13T00:37:34Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.19 psi 2021-02-13T00:37:34Z- handleDepth()-Depth_Keller msg: 1613176654662 epoch sec; seqNo:14301; depth 24.14 2021-02-13T00:37:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176654.67 epoch sec; seqNo:14301; 4059637.37 north; 592864.99 east 2021-02-13T00:37:35Z- AHRS_M2 msg: 1613176655.06 epoch sec; seqNo:14187 2021-02-13T00:37:35Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.18 psi 2021-02-13T00:37:35Z- handleDepth()-Depth_Keller msg: 1613176655064 epoch sec; seqNo:14302; depth 24.29 2021-02-13T00:37:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176655.07 epoch sec; seqNo:14302; 4059637.75 north; 592864.99 east 2021-02-13T00:37:35Z- AHRS_M2 msg: 1613176655.46 epoch sec; seqNo:14188 2021-02-13T00:37:35Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.18 psi 2021-02-13T00:37:35Z- handleDepth()-Depth_Keller msg: 1613176655468 epoch sec; seqNo:14303; depth 24.39 2021-02-13T00:37:35Z- handleDvl() - RDI_Pathfinder msg: 1613176655.48 epoch sec; seqNo:6908 2021-02-13T00:37:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:37:35Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:37:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176655.49 epoch sec; seqNo:14303; 4059638.13 north; 592864.98 east 2021-02-13T00:37:35Z- LcmTrn::handleMessages() - handling 47 messages took 0.76 ms and 132 calls... 2021-02-13T00:37:35Z- waiting for 1613176653.24, time is 1613176655.49 2021-02-13T00:37:35Z- LcmTrn::updateTrn() equating pose 1613176655.49 and meas times 1613176655.49 2021-02-13T00:37:35Z- LcmTrn::updateTrn() >>>> pose time 1613176655.49, meas time 1613176655.49 <<<< 2021-02-13T00:37:35Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613176655.49, ping # 0. 2021-02-13T00:37:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176655.49 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:37:35Z- TerrainNav::motionUpdate() - currEstimate = (4059638, 592864.98, 24.39) 2021-02-13T00:37:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:37:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:37:35Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:37:35Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:37:35Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176655.49 sec.; No relevant navigation data available 2021-02-13T00:37:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:37:35Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:35Z- variances([0], [2], and psi_berg) = 694.507854 832.995139 0.000000 2021-02-13T00:37:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:37:35Z- LcmTrn::publishEstimates() - MLE : 4059637.63 592897.48 24.39 2021-02-13T00:37:35Z- LcmTrn::publishEstimates() - MMSE : 4059638.13 592864.98 24.39 2021-02-13T00:37:35Z- LcmTrn::publishEstimates() - COVAR: 694.51 833.00 1.14 0.00 2021-02-13T00:37:35Z- AHRS_M2 msg: 1613176655.87 epoch sec; seqNo:14189 2021-02-13T00:37:35Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.18 psi 2021-02-13T00:37:35Z- handleDepth()-Depth_Keller msg: 1613176655875 epoch sec; seqNo:14304; depth 24.53 2021-02-13T00:37:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176655.89 epoch sec; seqNo:14304; 4059638.13 north; 592864.98 east 2021-02-13T00:37:36Z- AHRS_M2 msg: 1613176656.28 epoch sec; seqNo:14190 2021-02-13T00:37:36Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.19 psi 2021-02-13T00:37:36Z- handleDepth()-Depth_Keller msg: 1613176656300 epoch sec; seqNo:14305; depth 24.65 2021-02-13T00:37:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176656.32 epoch sec; seqNo:14305; 4059638.51 north; 592864.98 east 2021-02-13T00:37:36Z- AHRS_M2 msg: 1613176656.67 epoch sec; seqNo:14191 2021-02-13T00:37:36Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-13T00:37:36Z- handleDepth()-Depth_Keller msg: 1613176656685 epoch sec; seqNo:14306; depth 24.80 2021-02-13T00:37:36Z- handleDvl() - RDI_Pathfinder msg: 1613176656.70 epoch sec; seqNo:6909 2021-02-13T00:37:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 87.98 , 64.57 , 100.15 2021-02-13T00:37:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.16 , -0.02 2021-02-13T00:37:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176656.72 epoch sec; seqNo:14306; 4059638.51 north; 592864.98 east 2021-02-13T00:37:37Z- AHRS_M2 msg: 1613176657.08 epoch sec; seqNo:14192 2021-02-13T00:37:37Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-13T00:37:37Z- handleDepth()-Depth_Keller msg: 1613176657084 epoch sec; seqNo:14307; depth 24.91 2021-02-13T00:37:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176657.10 epoch sec; seqNo:14307; 4059638.90 north; 592866.19 east 2021-02-13T00:37:37Z- AHRS_M2 msg: 1613176657.48 epoch sec; seqNo:14193 2021-02-13T00:37:37Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-13T00:37:37Z- handleDepth()-Depth_Keller msg: 1613176657489 epoch sec; seqNo:14308; depth 25.04 2021-02-13T00:37:37Z- handleDvl() - RDI_Pathfinder msg: 1613176657.51 epoch sec; seqNo:6910 2021-02-13T00:37:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 88.65 , 66.20 , 99.61 2021-02-13T00:37:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.09 , -0.09 2021-02-13T00:37:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176657.52 epoch sec; seqNo:14308; 4059639.28 north; 592866.19 east 2021-02-13T00:37:37Z- AHRS_M2 msg: 1613176657.89 epoch sec; seqNo:14194 2021-02-13T00:37:37Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.20 psi 2021-02-13T00:37:37Z- handleDepth()-Depth_Keller msg: 1613176657892 epoch sec; seqNo:14309; depth 25.16 2021-02-13T00:37:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176657.90 epoch sec; seqNo:14309; 4059639.28 north; 592866.19 east 2021-02-13T00:37:38Z- AHRS_M2 msg: 1613176658.29 epoch sec; seqNo:14195 2021-02-13T00:37:38Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-13T00:37:38Z- handleDepth()-Depth_Keller msg: 1613176658301 epoch sec; seqNo:14310; depth 25.27 2021-02-13T00:37:38Z- handleDvl() - RDI_Pathfinder msg: 1613176658.31 epoch sec; seqNo:6911 2021-02-13T00:37:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:37:38Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:37:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176658.32 epoch sec; seqNo:14310; 4059639.66 north; 592866.19 east 2021-02-13T00:37:38Z- AHRS_M2 msg: 1613176658.69 epoch sec; seqNo:14196 2021-02-13T00:37:38Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-13T00:37:38Z- handleDepth()-Depth_Keller msg: 1613176658703 epoch sec; seqNo:14311; depth 25.42 2021-02-13T00:37:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176658.71 epoch sec; seqNo:14311; 4059640.04 north; 592866.18 east 2021-02-13T00:37:39Z- AHRS_M2 msg: 1613176659.10 epoch sec; seqNo:14197 2021-02-13T00:37:39Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-13T00:37:39Z- handleDepth()-Depth_Keller msg: 1613176659104 epoch sec; seqNo:14312; depth 25.52 2021-02-13T00:37:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176659.13 epoch sec; seqNo:14312; 4059640.43 north; 592867.40 east 2021-02-13T00:37:39Z- AHRS_M2 msg: 1613176659.50 epoch sec; seqNo:14198 2021-02-13T00:37:39Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-13T00:37:39Z- handleDepth()-Depth_Keller msg: 1613176659513 epoch sec; seqNo:14313; depth 25.65 2021-02-13T00:37:39Z- handleDvl() - RDI_Pathfinder msg: 1613176659.53 epoch sec; seqNo:6912 2021-02-13T00:37:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 0.00 , 63.75 , 100.01 2021-02-13T00:37:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.04 , -0.04 2021-02-13T00:37:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176659.56 epoch sec; seqNo:14313; 4059640.81 north; 592867.39 east 2021-02-13T00:37:39Z- AHRS_M2 msg: 1613176659.91 epoch sec; seqNo:14199 2021-02-13T00:37:39Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.20 psi 2021-02-13T00:37:39Z- handleDepth()-Depth_Keller msg: 1613176659917 epoch sec; seqNo:14314; depth 25.79 2021-02-13T00:37:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176659.93 epoch sec; seqNo:14314; 4059640.81 north; 592867.39 east 2021-02-13T00:37:40Z- AHRS_M2 msg: 1613176660.31 epoch sec; seqNo:14200 2021-02-13T00:37:40Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.20 psi 2021-02-13T00:37:40Z- handleDepth()-Depth_Keller msg: 1613176660322 epoch sec; seqNo:14315; depth 25.91 2021-02-13T00:37:40Z- handleDvl() - RDI_Pathfinder msg: 1613176660.33 epoch sec; seqNo:6913 2021-02-13T00:37:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 88.25 , 62.12 , 99.07 2021-02-13T00:37:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.05 , -0.01 2021-02-13T00:37:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176660.34 epoch sec; seqNo:14315; 4059641.19 north; 592867.39 east 2021-02-13T00:37:40Z- LcmTrn::handleMessages() - handling 41 messages took 0.72 ms and 127 calls... 2021-02-13T00:37:40Z- waiting for 1613176658.49, time is 1613176660.34 2021-02-13T00:37:40Z- LcmTrn::updateTrn() equating pose 1613176660.34 and meas times 1613176660.34 2021-02-13T00:37:40Z- LcmTrn::updateTrn() >>>> pose time 1613176660.34, meas time 1613176660.34 <<<< 2021-02-13T00:37:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176660.34 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:37:40Z- TerrainNav::motionUpdate() - currEstimate = (4059641, 592867.39, 25.91) 2021-02-13T00:37:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:37:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:37:40Z- TNavBF::Projecting Measurements 2021-02-13T00:37:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:37:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:37:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:37:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:37:40Z- TNavFilter::4 good beams. 2021-02-13T00:37:40Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:37:40Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:37:40Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:37:40Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:37:40Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:37:40Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:37:40Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:37:40Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:40Z- TerrainMapOctree: (vehN, vehE ) = (4059641.19, 592867.39). 2021-02-13T00:37:40Z- TerrainMapOctree: Min Distance = 2121.29. 2021-02-13T00:37:40Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:37:40Z- There is no specific good beam that all particles have in common. 2021-02-13T00:37:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:37:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.124 ExpectedMeasDiff: nan currMeas.covariance: 7.72 2021-02-13T00:37:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:37:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 12.46 2021-02-13T00:37:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:37:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.121 ExpectedMeasDiff: nan currMeas.covariance: 6.17 2021-02-13T00:37:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:37:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 15.70 2021-02-13T00:37:40Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:37:40Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:37:40Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:37:40Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:37:40Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:37:40Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:37:40Z- Filter 0 No beams used in subcloud update 2021-02-13T00:37:40Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:37:40Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:37:40Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 694.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:37:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176660.34 sec. 2021-02-13T00:37:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:37:40Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:40Z- variances([0], [2], and psi_berg) = 694.457491 832.995114 0.000000 2021-02-13T00:37:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:37:40Z- LcmTrn::publishEstimates() - MLE : 4059640.69 592899.89 25.91 2021-02-13T00:37:40Z- LcmTrn::publishEstimates() - MMSE : 4059641.19 592867.39 25.91 2021-02-13T00:37:40Z- LcmTrn::publishEstimates() - COVAR: 694.46 833.00 0.90 0.00 2021-02-13T00:37:40Z- AHRS_M2 msg: 1613176660.72 epoch sec; seqNo:14201 2021-02-13T00:37:40Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.20 psi 2021-02-13T00:37:40Z- handleDepth()-Depth_Keller msg: 1613176660725 epoch sec; seqNo:14316; depth 26.03 2021-02-13T00:37:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176660.74 epoch sec; seqNo:14316; 4059641.57 north; 592867.39 east 2021-02-13T00:37:41Z- AHRS_M2 msg: 1613176661.12 epoch sec; seqNo:14202 2021-02-13T00:37:41Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.20 psi 2021-02-13T00:37:41Z- handleDepth()-Depth_Keller msg: 1613176661131 epoch sec; seqNo:14317; depth 26.16 2021-02-13T00:37:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176661.16 epoch sec; seqNo:14317; 4059641.94 north; 592867.38 east 2021-02-13T00:37:41Z- AHRS_M2 msg: 1613176661.52 epoch sec; seqNo:14203 2021-02-13T00:37:41Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.20 psi 2021-02-13T00:37:41Z- handleDepth()-Depth_Keller msg: 1613176661536 epoch sec; seqNo:14318; depth 26.28 2021-02-13T00:37:41Z- handleDvl() - RDI_Pathfinder msg: 1613176661.55 epoch sec; seqNo:6914 2021-02-13T00:37:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.06 , 0.00 , 62.12 , 97.98 2021-02-13T00:37:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.09 , 0.01 2021-02-13T00:37:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176661.56 epoch sec; seqNo:14318; 4059642.32 north; 592867.38 east 2021-02-13T00:37:41Z- AHRS_M2 msg: 1613176661.93 epoch sec; seqNo:14204 2021-02-13T00:37:41Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.20 psi 2021-02-13T00:37:41Z- handleDepth()-Depth_Keller msg: 1613176661937 epoch sec; seqNo:14319; depth 26.40 2021-02-13T00:37:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176661.95 epoch sec; seqNo:14319; 4059642.34 north; 592868.60 east 2021-02-13T00:37:42Z- AHRS_M2 msg: 1613176662.33 epoch sec; seqNo:14205 2021-02-13T00:37:42Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.20 psi 2021-02-13T00:37:42Z- handleDepth()-Depth_Keller msg: 1613176662337 epoch sec; seqNo:14320; depth 26.51 2021-02-13T00:37:42Z- handleDvl() - RDI_Pathfinder msg: 1613176662.35 epoch sec; seqNo:6915 2021-02-13T00:37:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:37:42Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:37:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176662.36 epoch sec; seqNo:14320; 4059642.72 north; 592868.59 east 2021-02-13T00:37:42Z- AHRS_M2 msg: 1613176662.74 epoch sec; seqNo:14206 2021-02-13T00:37:42Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.20 psi 2021-02-13T00:37:42Z- handleDepth()-Depth_Keller msg: 1613176662745 epoch sec; seqNo:14321; depth 26.65 2021-02-13T00:37:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176662.76 epoch sec; seqNo:14321; 4059643.09 north; 592868.59 east 2021-02-13T00:37:43Z- AHRS_M2 msg: 1613176663.14 epoch sec; seqNo:14207 2021-02-13T00:37:43Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.20 psi 2021-02-13T00:37:43Z- handleDepth()-Depth_Keller msg: 1613176663150 epoch sec; seqNo:14322; depth 26.78 2021-02-13T00:37:43Z- handleDvl() - RDI_Pathfinder msg: 1613176663.16 epoch sec; seqNo:6916 2021-02-13T00:37:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 68.53 , 0.00 , 62.55 , 0.00 2021-02-13T00:37:43Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:37:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176663.17 epoch sec; seqNo:14322; 4059643.47 north; 592868.59 east 2021-02-13T00:37:43Z- AHRS_M2 msg: 1613176663.54 epoch sec; seqNo:14208 2021-02-13T00:37:43Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.20 psi 2021-02-13T00:37:43Z- handleDepth()-Depth_Keller msg: 1613176663553 epoch sec; seqNo:14323; depth 26.90 2021-02-13T00:37:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176663.57 epoch sec; seqNo:14323; 4059643.85 north; 592868.58 east 2021-02-13T00:37:43Z- AHRS_M2 msg: 1613176663.95 epoch sec; seqNo:14209 2021-02-13T00:37:43Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.20 psi 2021-02-13T00:37:43Z- handleDepth()-Depth_Keller msg: 1613176663953 epoch sec; seqNo:14324; depth 27.01 2021-02-13T00:37:43Z- handleDvl() - RDI_Pathfinder msg: 1613176663.97 epoch sec; seqNo:6917 2021-02-13T00:37:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 67.61 , 0.00 , 63.01 , 0.00 2021-02-13T00:37:43Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:37:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176664.00 epoch sec; seqNo:14324; 4059643.85 north; 592868.58 east 2021-02-13T00:37:44Z- AHRS_M2 msg: 1613176664.35 epoch sec; seqNo:14210 2021-02-13T00:37:44Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.20 psi 2021-02-13T00:37:44Z- handleDepth()-Depth_Keller msg: 1613176664357 epoch sec; seqNo:14325; depth 27.14 2021-02-13T00:37:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176664.37 epoch sec; seqNo:14325; 4059644.23 north; 592868.58 east 2021-02-13T00:37:44Z- AHRS_M2 msg: 1613176664.76 epoch sec; seqNo:14211 2021-02-13T00:37:44Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.20 psi 2021-02-13T00:37:44Z- handleDepth()-Depth_Keller msg: 1613176664761 epoch sec; seqNo:14326; depth 27.27 2021-02-13T00:37:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176664.77 epoch sec; seqNo:14326; 4059644.62 north; 592869.80 east 2021-02-13T00:37:45Z- AHRS_M2 msg: 1613176665.16 epoch sec; seqNo:14212 2021-02-13T00:37:45Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.20 psi 2021-02-13T00:37:45Z- handleDepth()-Depth_Keller msg: 1613176665165 epoch sec; seqNo:14327; depth 27.40 2021-02-13T00:37:45Z- handleDvl() - RDI_Pathfinder msg: 1613176665.18 epoch sec; seqNo:6918 2021-02-13T00:37:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 67.61 , 0.00 , 63.01 , 0.00 2021-02-13T00:37:45Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:37:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176665.19 epoch sec; seqNo:14327; 4059645.00 north; 592869.79 east 2021-02-13T00:37:45Z- AHRS_M2 msg: 1613176665.56 epoch sec; seqNo:14213 2021-02-13T00:37:45Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.20 psi 2021-02-13T00:37:45Z- handleDepth()-Depth_Keller msg: 1613176665568 epoch sec; seqNo:14328; depth 27.53 2021-02-13T00:37:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176665.58 epoch sec; seqNo:14328; 4059645.38 north; 592869.79 east 2021-02-13T00:37:45Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-13T00:37:45Z- waiting for 1613176663.34, time is 1613176665.58 2021-02-13T00:37:45Z- LcmTrn::updateTrn() >>>> pose time 1613176665.58, meas time 1613176665.18 <<<< 2021-02-13T00:37:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176665.18 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:37:45Z- TerrainNav::motionUpdate() - currEstimate = (4059645, 592869.79, 27.53) 2021-02-13T00:37:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:37:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:37:45Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:37:45Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:37:45Z- TNavBF::Projecting Measurements 2021-02-13T00:37:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:37:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:37:45Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-13T00:37:45Z- TNavFilter::2 good beams. 2021-02-13T00:37:45Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:37:45Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:37:45Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:37:45Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:37:45Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:37:45Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-13T00:37:45Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:37:45Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:45Z- TerrainMapOctree: (vehN, vehE ) = (4059645.06, 592869.61). 2021-02-13T00:37:45Z- TerrainMapOctree: Min Distance = 2116.91. 2021-02-13T00:37:45Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:37:45Z- There is no specific good beam that all particles have in common. 2021-02-13T00:37:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:37:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 7.31 2021-02-13T00:37:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:37:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 6.35 2021-02-13T00:37:45Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:37:45Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:37:45Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:37:45Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:37:45Z- Filter 0 No beams used in subcloud update 2021-02-13T00:37:45Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:37:45Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:37:45Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 694.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:37:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176665.18 sec. 2021-02-13T00:37:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:37:45Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:45Z- variances([0], [2], and psi_berg) = 694.388165 832.995080 0.000000 2021-02-13T00:37:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:37:45Z- LcmTrn::publishEstimates() - MLE : 4059644.88 592902.29 27.53 2021-02-13T00:37:45Z- LcmTrn::publishEstimates() - MMSE : 4059645.38 592869.79 27.53 2021-02-13T00:37:45Z- LcmTrn::publishEstimates() - COVAR: 694.39 833.00 0.91 0.00 2021-02-13T00:37:45Z- AHRS_M2 msg: 1613176665.97 epoch sec; seqNo:14214 2021-02-13T00:37:45Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.19 psi 2021-02-13T00:37:45Z- handleDepth()-Depth_Keller msg: 1613176665972 epoch sec; seqNo:14329; depth 27.65 2021-02-13T00:37:45Z- handleDvl() - RDI_Pathfinder msg: 1613176665.98 epoch sec; seqNo:6919 2021-02-13T00:37:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:37:45Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:37:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176666.00 epoch sec; seqNo:14329; 4059645.76 north; 592869.78 east 2021-02-13T00:37:46Z- AHRS_M2 msg: 1613176666.37 epoch sec; seqNo:14215 2021-02-13T00:37:46Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.18 psi 2021-02-13T00:37:46Z- handleDepth()-Depth_Keller msg: 1613176666382 epoch sec; seqNo:14330; depth 27.77 2021-02-13T00:37:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176666.39 epoch sec; seqNo:14330; 4059645.76 north; 592869.78 east 2021-02-13T00:37:46Z- AHRS_M2 msg: 1613176666.78 epoch sec; seqNo:14216 2021-02-13T00:37:46Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.18 psi 2021-02-13T00:37:46Z- handleDepth()-Depth_Keller msg: 1613176666779 epoch sec; seqNo:14331; depth 27.89 2021-02-13T00:37:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176666.79 epoch sec; seqNo:14331; 4059646.14 north; 592869.78 east 2021-02-13T00:37:47Z- AHRS_M2 msg: 1613176667.18 epoch sec; seqNo:14217 2021-02-13T00:37:47Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.17 psi 2021-02-13T00:37:47Z- handleDepth()-Depth_Keller msg: 1613176667184 epoch sec; seqNo:14332; depth 28.02 2021-02-13T00:37:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176667.20 epoch sec; seqNo:14332; 4059646.52 north; 592869.77 east 2021-02-13T00:37:47Z- AHRS_M2 msg: 1613176667.59 epoch sec; seqNo:14218 2021-02-13T00:37:47Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.17 psi 2021-02-13T00:37:47Z- handleDepth()-Depth_Keller msg: 1613176667594 epoch sec; seqNo:14333; depth 28.15 2021-02-13T00:37:47Z- handleDvl() - RDI_Pathfinder msg: 1613176667.61 epoch sec; seqNo:6920 2021-02-13T00:37:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 83.92 , 61.71 , 97.98 2021-02-13T00:37:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , 0.06 2021-02-13T00:37:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176667.62 epoch sec; seqNo:14333; 4059646.91 north; 592870.99 east 2021-02-13T00:37:47Z- AHRS_M2 msg: 1613176667.99 epoch sec; seqNo:14219 2021-02-13T00:37:47Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.17 psi 2021-02-13T00:37:47Z- handleDepth()-Depth_Keller msg: 1613176667997 epoch sec; seqNo:14334; depth 28.26 2021-02-13T00:37:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176668.02 epoch sec; seqNo:14334; 4059646.91 north; 592870.99 east 2021-02-13T00:37:48Z- AHRS_M2 msg: 1613176668.39 epoch sec; seqNo:14220 2021-02-13T00:37:48Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.17 psi 2021-02-13T00:37:48Z- handleDepth()-Depth_Keller msg: 1613176668401 epoch sec; seqNo:14335; depth 28.38 2021-02-13T00:37:48Z- handleDvl() - RDI_Pathfinder msg: 1613176668.41 epoch sec; seqNo:6921 2021-02-13T00:37:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 83.92 , 62.12 , 97.98 2021-02-13T00:37:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.09 , 0.03 2021-02-13T00:37:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176668.43 epoch sec; seqNo:14335; 4059647.29 north; 592870.99 east 2021-02-13T00:37:48Z- AHRS_M2 msg: 1613176668.80 epoch sec; seqNo:14221 2021-02-13T00:37:48Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.17 psi 2021-02-13T00:37:48Z- handleDepth()-Depth_Keller msg: 1613176668805 epoch sec; seqNo:14336; depth 28.51 2021-02-13T00:37:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176668.82 epoch sec; seqNo:14336; 4059647.67 north; 592870.98 east 2021-02-13T00:37:49Z- AHRS_M2 msg: 1613176669.20 epoch sec; seqNo:14222 2021-02-13T00:37:49Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.17 psi 2021-02-13T00:37:49Z- handleDepth()-Depth_Keller msg: 1613176669215 epoch sec; seqNo:14337; depth 28.62 2021-02-13T00:37:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176669.25 epoch sec; seqNo:14337; 4059648.05 north; 592870.98 east 2021-02-13T00:37:49Z- AHRS_M2 msg: 1613176669.60 epoch sec; seqNo:14223 2021-02-13T00:37:49Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.17 psi 2021-02-13T00:37:49Z- handleDepth()-Depth_Keller msg: 1613176669608 epoch sec; seqNo:14338; depth 28.76 2021-02-13T00:37:49Z- handleDvl() - RDI_Pathfinder msg: 1613176669.63 epoch sec; seqNo:6922 2021-02-13T00:37:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:37:49Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:37:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176669.64 epoch sec; seqNo:14338; 4059648.05 north; 592870.98 east 2021-02-13T00:37:50Z- AHRS_M2 msg: 1613176670.01 epoch sec; seqNo:14224 2021-02-13T00:37:50Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.17 psi 2021-02-13T00:37:50Z- handleDepth()-Depth_Keller msg: 1613176670012 epoch sec; seqNo:14339; depth 28.87 2021-02-13T00:37:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176670.02 epoch sec; seqNo:14339; 4059648.44 north; 592872.20 east 2021-02-13T00:37:50Z- AHRS_M2 msg: 1613176670.41 epoch sec; seqNo:14225 2021-02-13T00:37:50Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.16 psi 2021-02-13T00:37:50Z- handleDepth()-Depth_Keller msg: 1613176670421 epoch sec; seqNo:14340; depth 28.99 2021-02-13T00:37:50Z- handleDvl() - RDI_Pathfinder msg: 1613176670.43 epoch sec; seqNo:6923 2021-02-13T00:37:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.62 , 84.46 , 62.53 , 92.98 2021-02-13T00:37:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.03 , -0.03 2021-02-13T00:37:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176670.44 epoch sec; seqNo:14340; 4059648.82 north; 592872.19 east 2021-02-13T00:37:50Z- LcmTrn::handleMessages() - handling 41 messages took 0.70 ms and 127 calls... 2021-02-13T00:37:50Z- waiting for 1613176668.58, time is 1613176670.44 2021-02-13T00:37:50Z- LcmTrn::updateTrn() equating pose 1613176670.44 and meas times 1613176670.44 2021-02-13T00:37:50Z- LcmTrn::updateTrn() >>>> pose time 1613176670.44, meas time 1613176670.44 <<<< 2021-02-13T00:37:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176670.44 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:37:50Z- TerrainNav::motionUpdate() - currEstimate = (4059649, 592872.19, 28.99) 2021-02-13T00:37:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:37:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:37:50Z- TNavBF::Projecting Measurements 2021-02-13T00:37:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:37:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:37:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:37:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:37:50Z- TNavFilter::4 good beams. 2021-02-13T00:37:50Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:37:50Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:37:50Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:37:50Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:37:50Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:37:50Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:37:50Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:37:50Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:50Z- TerrainMapOctree: (vehN, vehE ) = (4059648.82, 592872.19). 2021-02-13T00:37:50Z- TerrainMapOctree: Min Distance = 2112.38. 2021-02-13T00:37:50Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:37:50Z- There is no specific good beam that all particles have in common. 2021-02-13T00:37:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:37:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 7.10 2021-02-13T00:37:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:37:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 11.41 2021-02-13T00:37:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:37:50Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 6.26 2021-02-13T00:37:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:37:50Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 13.83 2021-02-13T00:37:50Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:37:50Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:37:50Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:37:50Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:37:50Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:37:50Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:37:50Z- Filter 0 No beams used in subcloud update 2021-02-13T00:37:50Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:37:50Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:37:50Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 694.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:37:50Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176670.44 sec. 2021-02-13T00:37:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:37:50Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:50Z- variances([0], [2], and psi_berg) = 694.335951 832.995054 0.000000 2021-02-13T00:37:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:37:50Z- LcmTrn::publishEstimates() - MLE : 4059648.32 592902.69 28.99 2021-02-13T00:37:50Z- LcmTrn::publishEstimates() - MMSE : 4059648.82 592872.19 28.99 2021-02-13T00:37:50Z- LcmTrn::publishEstimates() - COVAR: 694.34 833.00 0.79 0.00 2021-02-13T00:37:50Z- AHRS_M2 msg: 1613176670.82 epoch sec; seqNo:14226 2021-02-13T00:37:50Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.15 psi 2021-02-13T00:37:50Z- handleDepth()-Depth_Keller msg: 1613176670822 epoch sec; seqNo:14341; depth 29.09 2021-02-13T00:37:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176670.83 epoch sec; seqNo:14341; 4059649.20 north; 592872.19 east 2021-02-13T00:37:51Z- AHRS_M2 msg: 1613176671.25 epoch sec; seqNo:14227 2021-02-13T00:37:51Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.15 psi 2021-02-13T00:37:51Z- handleDepth()-Depth_Keller msg: 1613176671256 epoch sec; seqNo:14342; depth 29.21 2021-02-13T00:37:51Z- handleDvl() - RDI_Pathfinder msg: 1613176671.27 epoch sec; seqNo:6924 2021-02-13T00:37:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 82.84 , 59.67 , 97.98 2021-02-13T00:37:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , -0.03 2021-02-13T00:37:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176671.28 epoch sec; seqNo:14342; 4059649.20 north; 592872.19 east 2021-02-13T00:37:51Z- AHRS_M2 msg: 1613176671.62 epoch sec; seqNo:14228 2021-02-13T00:37:51Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.15 psi 2021-02-13T00:37:51Z- handleDepth()-Depth_Keller msg: 1613176671631 epoch sec; seqNo:14343; depth 29.36 2021-02-13T00:37:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176671.65 epoch sec; seqNo:14343; 4059649.57 north; 592872.18 east 2021-02-13T00:37:52Z- AHRS_M2 msg: 1613176672.03 epoch sec; seqNo:14229 2021-02-13T00:37:52Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.15 psi 2021-02-13T00:37:52Z- handleDepth()-Depth_Keller msg: 1613176672034 epoch sec; seqNo:14344; depth 29.47 2021-02-13T00:37:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176672.05 epoch sec; seqNo:14344; 4059649.97 north; 592873.40 east 2021-02-13T00:37:52Z- AHRS_M2 msg: 1613176672.43 epoch sec; seqNo:14230 2021-02-13T00:37:52Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.15 psi 2021-02-13T00:37:52Z- handleDepth()-Depth_Keller msg: 1613176672445 epoch sec; seqNo:14345; depth 29.60 2021-02-13T00:37:52Z- handleDvl() - RDI_Pathfinder msg: 1613176672.46 epoch sec; seqNo:6925 2021-02-13T00:37:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.62 , 0.00 , 60.90 , 97.98 2021-02-13T00:37:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , -0.05 2021-02-13T00:37:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176672.47 epoch sec; seqNo:14345; 4059650.35 north; 592873.40 east 2021-02-13T00:37:52Z- AHRS_M2 msg: 1613176672.84 epoch sec; seqNo:14231 2021-02-13T00:37:52Z- AHRS_M2 msg: 0.62 phi; -0.26 theta; -0.15 psi 2021-02-13T00:37:52Z- handleDepth()-Depth_Keller msg: 1613176672845 epoch sec; seqNo:14346; depth 29.73 2021-02-13T00:37:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176672.86 epoch sec; seqNo:14346; 4059650.35 north; 592873.40 east 2021-02-13T00:37:53Z- AHRS_M2 msg: 1613176673.24 epoch sec; seqNo:14232 2021-02-13T00:37:53Z- AHRS_M2 msg: 0.62 phi; -0.22 theta; -0.15 psi 2021-02-13T00:37:53Z- handleDepth()-Depth_Keller msg: 1613176673249 epoch sec; seqNo:14347; depth 29.86 2021-02-13T00:37:53Z- handleDvl() - RDI_Pathfinder msg: 1613176673.26 epoch sec; seqNo:6926 2021-02-13T00:37:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.97 , 78.51 , 60.89 , 0.00 2021-02-13T00:37:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , -0.05 2021-02-13T00:37:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176673.29 epoch sec; seqNo:14347; 4059650.72 north; 592873.39 east 2021-02-13T00:37:53Z- AHRS_M2 msg: 1613176673.64 epoch sec; seqNo:14233 2021-02-13T00:37:53Z- AHRS_M2 msg: 0.61 phi; -0.18 theta; -0.14 psi 2021-02-13T00:37:53Z- handleDepth()-Depth_Keller msg: 1613176673649 epoch sec; seqNo:14348; depth 30.01 2021-02-13T00:37:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176673.66 epoch sec; seqNo:14348; 4059651.10 north; 592873.39 east 2021-02-13T00:37:54Z- AHRS_M2 msg: 1613176674.05 epoch sec; seqNo:14234 2021-02-13T00:37:54Z- AHRS_M2 msg: 0.61 phi; -0.14 theta; -0.14 psi 2021-02-13T00:37:54Z- handleDepth()-Depth_Keller msg: 1613176674052 epoch sec; seqNo:14349; depth 30.12 2021-02-13T00:37:54Z- handleDvl() - RDI_Pathfinder msg: 1613176674.07 epoch sec; seqNo:6927 2021-02-13T00:37:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 64.08 , 0.00 , 62.79 , 0.00 2021-02-13T00:37:54Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:37:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176674.08 epoch sec; seqNo:14349; 4059651.48 north; 592873.38 east 2021-02-13T00:37:54Z- AHRS_M2 msg: 1613176674.45 epoch sec; seqNo:14235 2021-02-13T00:37:54Z- AHRS_M2 msg: 0.60 phi; -0.09 theta; -0.14 psi 2021-02-13T00:37:54Z- handleDepth()-Depth_Keller msg: 1613176674456 epoch sec; seqNo:14350; depth 30.23 2021-02-13T00:37:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176674.47 epoch sec; seqNo:14350; 4059651.87 north; 592874.60 east 2021-02-13T00:37:54Z- AHRS_M2 msg: 1613176674.86 epoch sec; seqNo:14236 2021-02-13T00:37:54Z- AHRS_M2 msg: 0.59 phi; -0.05 theta; -0.14 psi 2021-02-13T00:37:54Z- handleDepth()-Depth_Keller msg: 1613176674864 epoch sec; seqNo:14351; depth 30.33 2021-02-13T00:37:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176674.88 epoch sec; seqNo:14351; 4059651.87 north; 592874.60 east 2021-02-13T00:37:55Z- AHRS_M2 msg: 1613176675.27 epoch sec; seqNo:14237 2021-02-13T00:37:55Z- AHRS_M2 msg: 0.59 phi; -0.01 theta; -0.14 psi 2021-02-13T00:37:55Z- handleDepth()-Depth_Keller msg: 1613176675273 epoch sec; seqNo:14352; depth 30.40 2021-02-13T00:37:55Z- handleDvl() - RDI_Pathfinder msg: 1613176675.29 epoch sec; seqNo:6928 2021-02-13T00:37:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.60 , 0.00 , 65.16 , 0.00 2021-02-13T00:37:55Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:37:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176675.30 epoch sec; seqNo:14352; 4059652.25 north; 592874.60 east 2021-02-13T00:37:55Z- AHRS_M2 msg: 1613176675.66 epoch sec; seqNo:14238 2021-02-13T00:37:55Z- AHRS_M2 msg: 0.58 phi; 0.03 theta; -0.15 psi 2021-02-13T00:37:55Z- handleDepth()-Depth_Keller msg: 1613176675673 epoch sec; seqNo:14353; depth 30.47 2021-02-13T00:37:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176675.69 epoch sec; seqNo:14353; 4059652.63 north; 592874.59 east 2021-02-13T00:37:55Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-13T00:37:55Z- waiting for 1613176673.44, time is 1613176675.69 2021-02-13T00:37:55Z- LcmTrn::updateTrn() >>>> pose time 1613176675.69, meas time 1613176675.29 <<<< 2021-02-13T00:37:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176675.29 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:37:55Z- TerrainNav::motionUpdate() - currEstimate = (4059653, 592874.59, 30.47) 2021-02-13T00:37:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:37:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:37:55Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:37:55Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:37:55Z- TNavBF::Projecting Measurements 2021-02-13T00:37:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:37:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:37:55Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-13T00:37:55Z- TNavFilter::2 good beams. 2021-02-13T00:37:55Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:37:55Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:37:55Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:37:55Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:37:55Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:37:55Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-13T00:37:55Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:37:55Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:55Z- TerrainMapOctree: (vehN, vehE ) = (4059652.34, 592874.41). 2021-02-13T00:37:55Z- TerrainMapOctree: Min Distance = 2108.27. 2021-02-13T00:37:55Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:37:55Z- There is no specific good beam that all particles have in common. 2021-02-13T00:37:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:37:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 6.27 2021-02-13T00:37:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:37:55Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 6.79 2021-02-13T00:37:55Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:37:55Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:37:55Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:37:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:37:55Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:37:55Z- Filter 0 No beams used in subcloud update 2021-02-13T00:37:55Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:37:55Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:37:55Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 694.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:37:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176675.29 sec. 2021-02-13T00:37:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:37:55Z- TNavBF::mmse selected from filter -1 2021-02-13T00:37:55Z- variances([0], [2], and psi_berg) = 694.268960 832.995021 0.000000 2021-02-13T00:37:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:37:55Z- LcmTrn::publishEstimates() - MLE : 4059652.13 592905.09 30.47 2021-02-13T00:37:55Z- LcmTrn::publishEstimates() - MMSE : 4059652.63 592874.59 30.47 2021-02-13T00:37:55Z- LcmTrn::publishEstimates() - COVAR: 694.27 833.00 0.82 0.00 2021-02-13T00:37:56Z- AHRS_M2 msg: 1613176676.07 epoch sec; seqNo:14239 2021-02-13T00:37:56Z- AHRS_M2 msg: 0.57 phi; 0.07 theta; -0.15 psi 2021-02-13T00:37:56Z- handleDepth()-Depth_Keller msg: 1613176676072 epoch sec; seqNo:14354; depth 30.51 2021-02-13T00:37:56Z- handleDvl() - RDI_Pathfinder msg: 1613176676.08 epoch sec; seqNo:6929 2021-02-13T00:37:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.17 , 0.00 , 0.00 , 66.01 2021-02-13T00:37:56Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:37:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176676.12 epoch sec; seqNo:14354; 4059653.01 north; 592874.59 east 2021-02-13T00:37:56Z- AHRS_M2 msg: 1613176676.47 epoch sec; seqNo:14240 2021-02-13T00:37:56Z- AHRS_M2 msg: 0.55 phi; 0.10 theta; -0.16 psi 2021-02-13T00:37:56Z- handleDepth()-Depth_Keller msg: 1613176676476 epoch sec; seqNo:14355; depth 30.56 2021-02-13T00:37:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176676.49 epoch sec; seqNo:14355; 4059653.39 north; 592874.58 east 2021-02-13T00:37:56Z- AHRS_M2 msg: 1613176676.89 epoch sec; seqNo:14241 2021-02-13T00:37:56Z- AHRS_M2 msg: 0.54 phi; 0.13 theta; -0.16 psi 2021-02-13T00:37:56Z- handleDepth()-Depth_Keller msg: 1613176676908 epoch sec; seqNo:14356; depth 30.57 2021-02-13T00:37:56Z- handleDvl() - RDI_Pathfinder msg: 1613176676.92 epoch sec; seqNo:6930 2021-02-13T00:37:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.03 , 0.00 , 0.00 , 63.45 2021-02-13T00:37:56Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:37:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176676.95 epoch sec; seqNo:14356; 4059653.78 north; 592875.80 east 2021-02-13T00:37:57Z- AHRS_M2 msg: 1613176677.29 epoch sec; seqNo:14242 2021-02-13T00:37:57Z- AHRS_M2 msg: 0.53 phi; 0.16 theta; -0.16 psi 2021-02-13T00:37:57Z- handleDepth()-Depth_Keller msg: 1613176677307 epoch sec; seqNo:14357; depth 30.57 2021-02-13T00:37:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176677.32 epoch sec; seqNo:14357; 4059653.78 north; 592875.80 east 2021-02-13T00:37:57Z- AHRS_M2 msg: 1613176677.69 epoch sec; seqNo:14243 2021-02-13T00:37:57Z- AHRS_M2 msg: 0.53 phi; 0.19 theta; -0.16 psi 2021-02-13T00:37:57Z- handleDepth()-Depth_Keller msg: 1613176677691 epoch sec; seqNo:14358; depth 30.57 2021-02-13T00:37:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176677.70 epoch sec; seqNo:14358; 4059654.16 north; 592875.80 east 2021-02-13T00:37:58Z- AHRS_M2 msg: 1613176678.09 epoch sec; seqNo:14244 2021-02-13T00:37:58Z- AHRS_M2 msg: 0.52 phi; 0.21 theta; -0.16 psi 2021-02-13T00:37:58Z- handleDepth()-Depth_Keller msg: 1613176678092 epoch sec; seqNo:14359; depth 30.55 2021-02-13T00:37:58Z- handleDvl() - RDI_Pathfinder msg: 1613176678.10 epoch sec; seqNo:6931 2021-02-13T00:37:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:37:58Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:37:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176678.11 epoch sec; seqNo:14359; 4059654.54 north; 592875.79 east 2021-02-13T00:37:58Z- AHRS_M2 msg: 1613176678.49 epoch sec; seqNo:14245 2021-02-13T00:37:58Z- AHRS_M2 msg: 0.51 phi; 0.23 theta; -0.16 psi 2021-02-13T00:37:58Z- handleDepth()-Depth_Keller msg: 1613176678497 epoch sec; seqNo:14360; depth 30.53 2021-02-13T00:37:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176678.51 epoch sec; seqNo:14360; 4059654.92 north; 592875.79 east 2021-02-13T00:37:58Z- AHRS_M2 msg: 1613176678.89 epoch sec; seqNo:14246 2021-02-13T00:37:58Z- AHRS_M2 msg: 0.51 phi; 0.25 theta; -0.15 psi 2021-02-13T00:37:58Z- handleDepth()-Depth_Keller msg: 1613176678910 epoch sec; seqNo:14361; depth 30.49 2021-02-13T00:37:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176678.92 epoch sec; seqNo:14361; 4059655.30 north; 592875.78 east 2021-02-13T00:37:59Z- AHRS_M2 msg: 1613176679.30 epoch sec; seqNo:14247 2021-02-13T00:37:59Z- AHRS_M2 msg: 0.51 phi; 0.27 theta; -0.15 psi 2021-02-13T00:37:59Z- handleDepth()-Depth_Keller msg: 1613176679304 epoch sec; seqNo:14362; depth 30.45 2021-02-13T00:37:59Z- handleDvl() - RDI_Pathfinder msg: 1613176679.32 epoch sec; seqNo:6932 2021-02-13T00:37:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.98 , 78.51 , 83.92 , 61.71 2021-02-13T00:37:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , 0.07 2021-02-13T00:37:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176679.34 epoch sec; seqNo:14362; 4059655.30 north; 592875.78 east 2021-02-13T00:37:59Z- AHRS_M2 msg: 1613176679.71 epoch sec; seqNo:14248 2021-02-13T00:37:59Z- AHRS_M2 msg: 0.51 phi; 0.29 theta; -0.15 psi 2021-02-13T00:37:59Z- handleDepth()-Depth_Keller msg: 1613176679711 epoch sec; seqNo:14363; depth 30.39 2021-02-13T00:37:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176679.72 epoch sec; seqNo:14363; 4059655.69 north; 592877.00 east 2021-02-13T00:38:00Z- AHRS_M2 msg: 1613176680.12 epoch sec; seqNo:14249 2021-02-13T00:38:00Z- AHRS_M2 msg: 0.51 phi; 0.30 theta; -0.14 psi 2021-02-13T00:38:00Z- handleDepth()-Depth_Keller msg: 1613176680121 epoch sec; seqNo:14364; depth 30.31 2021-02-13T00:38:00Z- handleDvl() - RDI_Pathfinder msg: 1613176680.14 epoch sec; seqNo:6933 2021-02-13T00:38:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.62 , 76.43 , 0.00 , 59.26 2021-02-13T00:38:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , 0.02 2021-02-13T00:38:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176680.15 epoch sec; seqNo:14364; 4059656.07 north; 592877.00 east 2021-02-13T00:38:00Z- AHRS_M2 msg: 1613176680.51 epoch sec; seqNo:14250 2021-02-13T00:38:00Z- AHRS_M2 msg: 0.52 phi; 0.31 theta; -0.14 psi 2021-02-13T00:38:00Z- handleDepth()-Depth_Keller msg: 1613176680516 epoch sec; seqNo:14365; depth 30.24 2021-02-13T00:38:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176680.53 epoch sec; seqNo:14365; 4059656.45 north; 592876.99 east 2021-02-13T00:38:00Z- AHRS_M2 msg: 1613176680.92 epoch sec; seqNo:14251 2021-02-13T00:38:00Z- AHRS_M2 msg: 0.52 phi; 0.32 theta; -0.13 psi 2021-02-13T00:38:00Z- handleDepth()-Depth_Keller msg: 1613176680920 epoch sec; seqNo:14366; depth 30.18 2021-02-13T00:38:00Z- handleDvl() - RDI_Pathfinder msg: 1613176680.93 epoch sec; seqNo:6934 2021-02-13T00:38:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 67.65 , 0.00 , 0.00 , 60.85 2021-02-13T00:38:00Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:38:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176680.96 epoch sec; seqNo:14366; 4059656.83 north; 592876.99 east 2021-02-13T00:38:00Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-13T00:38:00Z- waiting for 1613176678.69, time is 1613176680.96 2021-02-13T00:38:00Z- LcmTrn::updateTrn() equating pose 1613176680.96 and meas times 1613176680.96 2021-02-13T00:38:00Z- LcmTrn::updateTrn() >>>> pose time 1613176680.96, meas time 1613176680.96 <<<< 2021-02-13T00:38:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176680.96 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:38:00Z- TerrainNav::motionUpdate() - currEstimate = (4059657, 592876.99, 30.18) 2021-02-13T00:38:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:38:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:38:00Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:38:00Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:38:00Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176680.96 sec.; No relevant navigation data available 2021-02-13T00:38:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:38:00Z- TNavBF::mmse selected from filter -1 2021-02-13T00:38:00Z- variances([0], [2], and psi_berg) = 694.212996 832.994993 0.000000 2021-02-13T00:38:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:38:00Z- LcmTrn::publishEstimates() - MLE : 4059656.33 592907.49 30.18 2021-02-13T00:38:00Z- LcmTrn::publishEstimates() - MMSE : 4059656.83 592876.99 30.18 2021-02-13T00:38:00Z- LcmTrn::publishEstimates() - COVAR: 694.21 832.99 1.07 0.00 2021-02-13T00:38:01Z- AHRS_M2 msg: 1613176681.32 epoch sec; seqNo:14252 2021-02-13T00:38:01Z- AHRS_M2 msg: 0.53 phi; 0.33 theta; -0.13 psi 2021-02-13T00:38:01Z- handleDepth()-Depth_Keller msg: 1613176681330 epoch sec; seqNo:14367; depth 30.09 2021-02-13T00:38:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176681.34 epoch sec; seqNo:14367; 4059656.83 north; 592876.99 east 2021-02-13T00:38:01Z- AHRS_M2 msg: 1613176681.72 epoch sec; seqNo:14253 2021-02-13T00:38:01Z- AHRS_M2 msg: 0.54 phi; 0.34 theta; -0.12 psi 2021-02-13T00:38:01Z- handleDepth()-Depth_Keller msg: 1613176681729 epoch sec; seqNo:14368; depth 30.00 2021-02-13T00:38:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176681.75 epoch sec; seqNo:14368; 4059657.20 north; 592876.98 east 2021-02-13T00:38:02Z- AHRS_M2 msg: 1613176682.13 epoch sec; seqNo:14254 2021-02-13T00:38:02Z- AHRS_M2 msg: 0.54 phi; 0.34 theta; -0.12 psi 2021-02-13T00:38:02Z- handleDepth()-Depth_Keller msg: 1613176682137 epoch sec; seqNo:14369; depth 29.91 2021-02-13T00:38:02Z- handleDvl() - RDI_Pathfinder msg: 1613176682.16 epoch sec; seqNo:6935 2021-02-13T00:38:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 68.57 , 0.00 , 0.00 , 60.48 2021-02-13T00:38:02Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:38:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176682.17 epoch sec; seqNo:14369; 4059657.60 north; 592878.20 east 2021-02-13T00:38:02Z- AHRS_M2 msg: 1613176682.53 epoch sec; seqNo:14255 2021-02-13T00:38:02Z- AHRS_M2 msg: 0.55 phi; 0.35 theta; -0.12 psi 2021-02-13T00:38:02Z- handleDepth()-Depth_Keller msg: 1613176682541 epoch sec; seqNo:14370; depth 29.80 2021-02-13T00:38:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176682.55 epoch sec; seqNo:14370; 4059657.97 north; 592878.20 east 2021-02-13T00:38:02Z- AHRS_M2 msg: 1613176682.94 epoch sec; seqNo:14256 2021-02-13T00:38:02Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.12 psi 2021-02-13T00:38:02Z- handleDepth()-Depth_Keller msg: 1613176682947 epoch sec; seqNo:14371; depth 29.71 2021-02-13T00:38:02Z- handleDvl() - RDI_Pathfinder msg: 1613176682.96 epoch sec; seqNo:6936 2021-02-13T00:38:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 69.00 , 0.00 , 0.00 , 61.33 2021-02-13T00:38:02Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:38:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176682.97 epoch sec; seqNo:14371; 4059657.97 north; 592878.20 east 2021-02-13T00:38:03Z- AHRS_M2 msg: 1613176683.34 epoch sec; seqNo:14257 2021-02-13T00:38:03Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.12 psi 2021-02-13T00:38:03Z- handleDepth()-Depth_Keller msg: 1613176683344 epoch sec; seqNo:14372; depth 29.59 2021-02-13T00:38:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176683.36 epoch sec; seqNo:14372; 4059658.35 north; 592878.19 east 2021-02-13T00:38:03Z- AHRS_M2 msg: 1613176683.74 epoch sec; seqNo:14258 2021-02-13T00:38:03Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.12 psi 2021-02-13T00:38:03Z- handleDepth()-Depth_Keller msg: 1613176683757 epoch sec; seqNo:14373; depth 29.49 2021-02-13T00:38:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176683.77 epoch sec; seqNo:14373; 4059658.73 north; 592878.19 east 2021-02-13T00:38:04Z- AHRS_M2 msg: 1613176684.15 epoch sec; seqNo:14259 2021-02-13T00:38:04Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.12 psi 2021-02-13T00:38:04Z- handleDepth()-Depth_Keller msg: 1613176684157 epoch sec; seqNo:14374; depth 29.38 2021-02-13T00:38:04Z- handleDvl() - RDI_Pathfinder msg: 1613176684.17 epoch sec; seqNo:6937 2021-02-13T00:38:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 69.42 , 0.00 , 0.00 , 61.33 2021-02-13T00:38:04Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:38:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176684.18 epoch sec; seqNo:14374; 4059658.73 north; 592878.19 east 2021-02-13T00:38:04Z- AHRS_M2 msg: 1613176684.55 epoch sec; seqNo:14260 2021-02-13T00:38:04Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.12 psi 2021-02-13T00:38:04Z- handleDepth()-Depth_Keller msg: 1613176684571 epoch sec; seqNo:14375; depth 29.25 2021-02-13T00:38:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176684.58 epoch sec; seqNo:14375; 4059659.12 north; 592879.40 east 2021-02-13T00:38:04Z- AHRS_M2 msg: 1613176684.96 epoch sec; seqNo:14261 2021-02-13T00:38:04Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.12 psi 2021-02-13T00:38:04Z- handleDepth()-Depth_Keller msg: 1613176684973 epoch sec; seqNo:14376; depth 29.14 2021-02-13T00:38:04Z- handleDvl() - RDI_Pathfinder msg: 1613176684.99 epoch sec; seqNo:6938 2021-02-13T00:38:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:38:04Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:38:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176685.00 epoch sec; seqNo:14376; 4059659.50 north; 592879.40 east 2021-02-13T00:38:05Z- AHRS_M2 msg: 1613176685.36 epoch sec; seqNo:14262 2021-02-13T00:38:05Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.12 psi 2021-02-13T00:38:05Z- handleDepth()-Depth_Keller msg: 1613176685366 epoch sec; seqNo:14377; depth 29.01 2021-02-13T00:38:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176685.38 epoch sec; seqNo:14377; 4059659.88 north; 592879.40 east 2021-02-13T00:38:05Z- AHRS_M2 msg: 1613176685.76 epoch sec; seqNo:14263 2021-02-13T00:38:05Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.12 psi 2021-02-13T00:38:05Z- handleDepth()-Depth_Keller msg: 1613176685770 epoch sec; seqNo:14378; depth 28.92 2021-02-13T00:38:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176685.78 epoch sec; seqNo:14378; 4059659.88 north; 592879.40 east 2021-02-13T00:38:05Z- LcmTrn::handleMessages() - handling 40 messages took 0.60 ms and 128 calls... 2021-02-13T00:38:05Z- waiting for 1613176683.96, time is 1613176685.78 2021-02-13T00:38:05Z- LcmTrn::updateTrn() >>>> pose time 1613176685.78, meas time 1613176684.99 <<<< 2021-02-13T00:38:05Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613176684.99, ping # 0. 2021-02-13T00:38:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176684.99 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:38:05Z- TerrainNav::motionUpdate() - currEstimate = (4059660, 592879.40, 28.92) 2021-02-13T00:38:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:38:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:38:05Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:38:05Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:38:05Z- TNavBF::Projecting Measurements 2021-02-13T00:38:05Z- TNavFilter::Measurement from time = 1613176684.99 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:38:05Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:38:05Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:38:05Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:38:05Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:38:05Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:38:05Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:38:05Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:38:05Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 694.2 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:38:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:38:05Z- TNavBF::mmse selected from filter -1 2021-02-13T00:38:05Z- variances([0], [2], and psi_berg) = 694.146796 832.994961 0.000000 2021-02-13T00:38:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:38:05Z- LcmTrn::publishEstimates() - MLE : 4059659.38 592909.90 28.92 2021-02-13T00:38:05Z- LcmTrn::publishEstimates() - MMSE : 4059659.88 592879.40 28.92 2021-02-13T00:38:05Z- LcmTrn::publishEstimates() - COVAR: 694.15 832.99 1.27 0.00 2021-02-13T00:38:06Z- AHRS_M2 msg: 1613176686.17 epoch sec; seqNo:14264 2021-02-13T00:38:06Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.12 psi 2021-02-13T00:38:06Z- handleDepth()-Depth_Keller msg: 1613176686177 epoch sec; seqNo:14379; depth 28.79 2021-02-13T00:38:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176686.20 epoch sec; seqNo:14379; 4059660.26 north; 592879.39 east 2021-02-13T00:38:06Z- AHRS_M2 msg: 1613176686.57 epoch sec; seqNo:14265 2021-02-13T00:38:06Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.12 psi 2021-02-13T00:38:06Z- handleDepth()-Depth_Keller msg: 1613176686576 epoch sec; seqNo:14380; depth 28.67 2021-02-13T00:38:06Z- handleDvl() - RDI_Pathfinder msg: 1613176686.59 epoch sec; seqNo:6939 2021-02-13T00:38:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 81.75 , 97.98 , 62.12 2021-02-13T00:38:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.05 2021-02-13T00:38:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176686.61 epoch sec; seqNo:14380; 4059660.64 north; 592879.39 east 2021-02-13T00:38:06Z- AHRS_M2 msg: 1613176686.97 epoch sec; seqNo:14266 2021-02-13T00:38:06Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.12 psi 2021-02-13T00:38:06Z- handleDepth()-Depth_Keller msg: 1613176686985 epoch sec; seqNo:14381; depth 28.55 2021-02-13T00:38:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176686.99 epoch sec; seqNo:14381; 4059660.65 north; 592880.61 east 2021-02-13T00:38:07Z- AHRS_M2 msg: 1613176687.38 epoch sec; seqNo:14267 2021-02-13T00:38:07Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.12 psi 2021-02-13T00:38:07Z- handleDepth()-Depth_Keller msg: 1613176687385 epoch sec; seqNo:14382; depth 28.42 2021-02-13T00:38:07Z- handleDvl() - RDI_Pathfinder msg: 1613176687.40 epoch sec; seqNo:6940 2021-02-13T00:38:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 81.75 , 97.98 , 0.00 2021-02-13T00:38:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.06 , -0.08 2021-02-13T00:38:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176687.41 epoch sec; seqNo:14382; 4059661.03 north; 592880.60 east 2021-02-13T00:38:07Z- AHRS_M2 msg: 1613176687.79 epoch sec; seqNo:14268 2021-02-13T00:38:07Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.13 psi 2021-02-13T00:38:07Z- handleDepth()-Depth_Keller msg: 1613176687791 epoch sec; seqNo:14383; depth 28.30 2021-02-13T00:38:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176687.81 epoch sec; seqNo:14383; 4059661.41 north; 592880.60 east 2021-02-13T00:38:08Z- AHRS_M2 msg: 1613176688.20 epoch sec; seqNo:14269 2021-02-13T00:38:08Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.13 psi 2021-02-13T00:38:08Z- handleDepth()-Depth_Keller msg: 1613176688225 epoch sec; seqNo:14384; depth 28.17 2021-02-13T00:38:08Z- handleDvl() - RDI_Pathfinder msg: 1613176688.24 epoch sec; seqNo:6941 2021-02-13T00:38:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 82.30 , 98.12 , 62.53 2021-02-13T00:38:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , -0.06 2021-02-13T00:38:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176688.25 epoch sec; seqNo:14384; 4059661.79 north; 592880.60 east 2021-02-13T00:38:08Z- AHRS_M2 msg: 1613176688.59 epoch sec; seqNo:14270 2021-02-13T00:38:08Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.13 psi 2021-02-13T00:38:08Z- handleDepth()-Depth_Keller msg: 1613176688601 epoch sec; seqNo:14385; depth 28.05 2021-02-13T00:38:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176688.61 epoch sec; seqNo:14385; 4059661.79 north; 592880.60 east 2021-02-13T00:38:08Z- AHRS_M2 msg: 1613176689.00 epoch sec; seqNo:14271 2021-02-13T00:38:08Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-13T00:38:09Z- handleDepth()-Depth_Keller msg: 1613176689002 epoch sec; seqNo:14386; depth 27.93 2021-02-13T00:38:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176689.01 epoch sec; seqNo:14386; 4059662.17 north; 592880.59 east 2021-02-13T00:38:09Z- AHRS_M2 msg: 1613176689.40 epoch sec; seqNo:14272 2021-02-13T00:38:09Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-13T00:38:09Z- handleDepth()-Depth_Keller msg: 1613176689404 epoch sec; seqNo:14387; depth 27.81 2021-02-13T00:38:09Z- handleDvl() - RDI_Pathfinder msg: 1613176689.42 epoch sec; seqNo:6942 2021-02-13T00:38:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 82.84 , 97.98 , 62.12 2021-02-13T00:38:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.13 , -0.02 2021-02-13T00:38:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176689.43 epoch sec; seqNo:14387; 4059662.55 north; 592880.59 east 2021-02-13T00:38:09Z- AHRS_M2 msg: 1613176689.80 epoch sec; seqNo:14273 2021-02-13T00:38:09Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.15 psi 2021-02-13T00:38:09Z- handleDepth()-Depth_Keller msg: 1613176689808 epoch sec; seqNo:14388; depth 27.68 2021-02-13T00:38:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176689.82 epoch sec; seqNo:14388; 4059662.55 north; 592880.59 east 2021-02-13T00:38:10Z- AHRS_M2 msg: 1613176690.21 epoch sec; seqNo:14274 2021-02-13T00:38:10Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.15 psi 2021-02-13T00:38:10Z- handleDepth()-Depth_Keller msg: 1613176690233 epoch sec; seqNo:14389; depth 27.56 2021-02-13T00:38:10Z- handleDvl() - RDI_Pathfinder msg: 1613176690.26 epoch sec; seqNo:6943 2021-02-13T00:38:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 83.92 , 95.82 , 61.30 2021-02-13T00:38:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.15 , 0.05 2021-02-13T00:38:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176690.27 epoch sec; seqNo:14389; 4059662.94 north; 592881.80 east 2021-02-13T00:38:10Z- AHRS_M2 msg: 1613176690.61 epoch sec; seqNo:14275 2021-02-13T00:38:10Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.15 psi 2021-02-13T00:38:10Z- handleDepth()-Depth_Keller msg: 1613176690617 epoch sec; seqNo:14390; depth 27.43 2021-02-13T00:38:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176690.63 epoch sec; seqNo:14390; 4059663.32 north; 592881.80 east 2021-02-13T00:38:11Z- AHRS_M2 msg: 1613176691.02 epoch sec; seqNo:14276 2021-02-13T00:38:11Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.15 psi 2021-02-13T00:38:11Z- LcmTrn::handleMessages() - handling 42 messages took 0.66 ms and 129 calls... 2021-02-13T00:38:11Z- waiting for 1613176688.78, time is 1613176691.02 2021-02-13T00:38:11Z- LcmTrn::updateTrn() >>>> pose time 1613176691.02, meas time 1613176690.26 <<<< 2021-02-13T00:38:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176690.26 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:38:11Z- TerrainNav::motionUpdate() - currEstimate = (4059663, 592881.80, 27.43) 2021-02-13T00:38:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:38:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:38:11Z- TNavBF::Projecting Measurements 2021-02-13T00:38:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:38:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:38:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:38:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:38:11Z- TNavFilter::4 good beams. 2021-02-13T00:38:11Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:38:11Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:38:11Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:38:11Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:38:11Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:38:11Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:38:11Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:38:11Z- TNavBF::mmse selected from filter -1 2021-02-13T00:38:11Z- TerrainMapOctree: (vehN, vehE ) = (4059662.82, 592881.45). 2021-02-13T00:38:11Z- TerrainMapOctree: Min Distance = 2095.72. 2021-02-13T00:38:11Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:38:11Z- There is no specific good beam that all particles have in common. 2021-02-13T00:38:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:38:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.45 K: 0.134 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-13T00:38:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:38:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 11.27 2021-02-13T00:38:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:38:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 14.69 2021-02-13T00:38:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:38:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.121 ExpectedMeasDiff: nan currMeas.covariance: 6.01 2021-02-13T00:38:11Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:38:11Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:38:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:38:11Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:38:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:38:11Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:38:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:38:11Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:38:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:38:11Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:38:11Z- Filter 0 No beams used in subcloud update 2021-02-13T00:38:11Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:38:11Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:38:11Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 694.1 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:38:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176690.26 sec. 2021-02-13T00:38:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:38:11Z- TNavBF::mmse selected from filter -1 2021-02-13T00:38:11Z- variances([0], [2], and psi_berg) = 694.078852 832.994927 0.000000 2021-02-13T00:38:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:38:11Z- LcmTrn::publishEstimates() - MLE : 4059662.82 592912.30 27.43 2021-02-13T00:38:11Z- LcmTrn::publishEstimates() - MMSE : 4059663.32 592881.80 27.43 2021-02-13T00:38:11Z- LcmTrn::publishEstimates() - COVAR: 694.08 832.99 0.97 0.00 2021-02-13T00:38:11Z- handleDepth()-Depth_Keller msg: 1613176691025 epoch sec; seqNo:14391; depth 27.32 2021-02-13T00:38:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176691.03 epoch sec; seqNo:14391; 4059663.70 north; 592881.80 east 2021-02-13T00:38:11Z- AHRS_M2 msg: 1613176691.42 epoch sec; seqNo:14277 2021-02-13T00:38:11Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.15 psi 2021-02-13T00:38:11Z- handleDepth()-Depth_Keller msg: 1613176691429 epoch sec; seqNo:14392; depth 27.20 2021-02-13T00:38:11Z- handleDvl() - RDI_Pathfinder msg: 1613176691.44 epoch sec; seqNo:6944 2021-02-13T00:38:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 83.92 , 97.98 , 61.71 2021-02-13T00:38:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.11 , 0.12 2021-02-13T00:38:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176691.45 epoch sec; seqNo:14392; 4059664.08 north; 592881.79 east 2021-02-13T00:38:11Z- AHRS_M2 msg: 1613176691.83 epoch sec; seqNo:14278 2021-02-13T00:38:11Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.15 psi 2021-02-13T00:38:11Z- handleDepth()-Depth_Keller msg: 1613176691831 epoch sec; seqNo:14393; depth 27.08 2021-02-13T00:38:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176691.84 epoch sec; seqNo:14393; 4059664.45 north; 592881.79 east 2021-02-13T00:38:12Z- AHRS_M2 msg: 1613176692.23 epoch sec; seqNo:14279 2021-02-13T00:38:12Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.15 psi 2021-02-13T00:38:12Z- handleDepth()-Depth_Keller msg: 1613176692268 epoch sec; seqNo:14394; depth 26.98 2021-02-13T00:38:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176692.28 epoch sec; seqNo:14394; 4059664.83 north; 592881.78 east 2021-02-13T00:38:12Z- AHRS_M2 msg: 1613176692.63 epoch sec; seqNo:14280 2021-02-13T00:38:12Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.15 psi 2021-02-13T00:38:12Z- handleDepth()-Depth_Keller msg: 1613176692636 epoch sec; seqNo:14395; depth 26.84 2021-02-13T00:38:12Z- handleDvl() - RDI_Pathfinder msg: 1613176692.65 epoch sec; seqNo:6945 2021-02-13T00:38:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 85.00 , 97.98 , 63.76 2021-02-13T00:38:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.05 , 0.12 2021-02-13T00:38:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176692.66 epoch sec; seqNo:14395; 4059664.83 north; 592881.78 east 2021-02-13T00:38:13Z- AHRS_M2 msg: 1613176693.04 epoch sec; seqNo:14281 2021-02-13T00:38:13Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.15 psi 2021-02-13T00:38:13Z- handleDepth()-Depth_Keller msg: 1613176693040 epoch sec; seqNo:14396; depth 26.73 2021-02-13T00:38:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176693.05 epoch sec; seqNo:14396; 4059665.23 north; 592883.00 east 2021-02-13T00:38:13Z- AHRS_M2 msg: 1613176693.44 epoch sec; seqNo:14282 2021-02-13T00:38:13Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.15 psi 2021-02-13T00:38:13Z- handleDepth()-Depth_Keller msg: 1613176693445 epoch sec; seqNo:14397; depth 26.61 2021-02-13T00:38:13Z- handleDvl() - RDI_Pathfinder msg: 1613176693.46 epoch sec; seqNo:6946 2021-02-13T00:38:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 85.95 , 97.85 , 63.35 2021-02-13T00:38:13Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.07 , 0.10 2021-02-13T00:38:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176693.47 epoch sec; seqNo:14397; 4059665.60 north; 592883.00 east 2021-02-13T00:38:13Z- AHRS_M2 msg: 1613176693.84 epoch sec; seqNo:14283 2021-02-13T00:38:13Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.15 psi 2021-02-13T00:38:13Z- handleDepth()-Depth_Keller msg: 1613176693853 epoch sec; seqNo:14398; depth 26.48 2021-02-13T00:38:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176693.86 epoch sec; seqNo:14398; 4059665.98 north; 592882.99 east 2021-02-13T00:38:14Z- AHRS_M2 msg: 1613176694.25 epoch sec; seqNo:14284 2021-02-13T00:38:14Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.14 psi 2021-02-13T00:38:14Z- handleDepth()-Depth_Keller msg: 1613176694253 epoch sec; seqNo:14399; depth 26.37 2021-02-13T00:38:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176694.27 epoch sec; seqNo:14399; 4059666.36 north; 592882.99 east 2021-02-13T00:38:14Z- AHRS_M2 msg: 1613176694.65 epoch sec; seqNo:14285 2021-02-13T00:38:14Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.14 psi 2021-02-13T00:38:14Z- handleDepth()-Depth_Keller msg: 1613176694658 epoch sec; seqNo:14400; depth 26.26 2021-02-13T00:38:14Z- handleDvl() - RDI_Pathfinder msg: 1613176694.67 epoch sec; seqNo:6947 2021-02-13T00:38:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 83.92 , 97.98 , 63.76 2021-02-13T00:38:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.14 , 0.07 2021-02-13T00:38:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176694.68 epoch sec; seqNo:14400; 4059666.36 north; 592882.99 east 2021-02-13T00:38:15Z- AHRS_M2 msg: 1613176695.06 epoch sec; seqNo:14286 2021-02-13T00:38:15Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.13 psi 2021-02-13T00:38:15Z- handleDepth()-Depth_Keller msg: 1613176695060 epoch sec; seqNo:14401; depth 26.13 2021-02-13T00:38:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176695.08 epoch sec; seqNo:14401; 4059666.75 north; 592884.20 east 2021-02-13T00:38:15Z- AHRS_M2 msg: 1613176695.46 epoch sec; seqNo:14287 2021-02-13T00:38:15Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.13 psi 2021-02-13T00:38:15Z- handleDepth()-Depth_Keller msg: 1613176695469 epoch sec; seqNo:14402; depth 26.02 2021-02-13T00:38:15Z- handleDvl() - RDI_Pathfinder msg: 1613176695.49 epoch sec; seqNo:6948 2021-02-13T00:38:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:38:15Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:38:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176695.50 epoch sec; seqNo:14402; 4059667.13 north; 592884.20 east 2021-02-13T00:38:15Z- AHRS_M2 msg: 1613176695.87 epoch sec; seqNo:14288 2021-02-13T00:38:15Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.13 psi 2021-02-13T00:38:15Z- handleDepth()-Depth_Keller msg: 1613176695869 epoch sec; seqNo:14403; depth 25.89 2021-02-13T00:38:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176695.88 epoch sec; seqNo:14403; 4059667.51 north; 592884.20 east 2021-02-13T00:38:16Z- LcmTrn::handleMessages() - handling 43 messages took 0.67 ms and 130 calls... 2021-02-13T00:38:16Z- waiting for 1613176694.02, time is 1613176695.88 2021-02-13T00:38:16Z- LcmTrn::updateTrn() >>>> pose time 1613176695.88, meas time 1613176695.49 <<<< 2021-02-13T00:38:16Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613176695.49, ping # 0. 2021-02-13T00:38:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176695.49 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:38:16Z- TerrainNav::motionUpdate() - currEstimate = (4059668, 592884.20, 25.89) 2021-02-13T00:38:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:38:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:38:16Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:38:16Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:38:16Z- TNavBF::Projecting Measurements 2021-02-13T00:38:16Z- TNavFilter::Measurement from time = 1613176695.49 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:38:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:38:16Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:38:16Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:38:16Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:38:16Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:38:16Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:38:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:38:16Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 694.0 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:38:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:38:16Z- TNavBF::mmse selected from filter -1 2021-02-13T00:38:16Z- variances([0], [2], and psi_berg) = 694.009439 832.994893 0.000000 2021-02-13T00:38:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:38:16Z- LcmTrn::publishEstimates() - MLE : 4059667.01 592914.70 25.89 2021-02-13T00:38:16Z- LcmTrn::publishEstimates() - MMSE : 4059667.51 592884.20 25.89 2021-02-13T00:38:16Z- LcmTrn::publishEstimates() - COVAR: 694.01 832.99 1.22 0.00 2021-02-13T00:38:16Z- AHRS_M2 msg: 1613176696.27 epoch sec; seqNo:14289 2021-02-13T00:38:16Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.13 psi 2021-02-13T00:38:16Z- handleDepth()-Depth_Keller msg: 1613176696277 epoch sec; seqNo:14404; depth 25.77 2021-02-13T00:38:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176696.29 epoch sec; seqNo:14404; 4059667.51 north; 592884.20 east 2021-02-13T00:38:16Z- AHRS_M2 msg: 1613176696.67 epoch sec; seqNo:14290 2021-02-13T00:38:16Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.13 psi 2021-02-13T00:38:16Z- handleDepth()-Depth_Keller msg: 1613176696681 epoch sec; seqNo:14405; depth 25.67 2021-02-13T00:38:16Z- handleDvl() - RDI_Pathfinder msg: 1613176696.69 epoch sec; seqNo:6949 2021-02-13T00:38:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 84.46 , 99.33 , 64.16 2021-02-13T00:38:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.15 , -0.07 2021-02-13T00:38:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176696.72 epoch sec; seqNo:14405; 4059667.89 north; 592884.19 east 2021-02-13T00:38:17Z- AHRS_M2 msg: 1613176697.08 epoch sec; seqNo:14291 2021-02-13T00:38:17Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.13 psi 2021-02-13T00:38:17Z- handleDepth()-Depth_Keller msg: 1613176697083 epoch sec; seqNo:14406; depth 25.53 2021-02-13T00:38:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176697.10 epoch sec; seqNo:14406; 4059668.28 north; 592885.41 east 2021-02-13T00:38:17Z- AHRS_M2 msg: 1613176697.48 epoch sec; seqNo:14292 2021-02-13T00:38:17Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.13 psi 2021-02-13T00:38:17Z- handleDepth()-Depth_Keller msg: 1613176697486 epoch sec; seqNo:14407; depth 25.42 2021-02-13T00:38:17Z- handleDvl() - RDI_Pathfinder msg: 1613176697.50 epoch sec; seqNo:6950 2021-02-13T00:38:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 84.80 , 97.78 , 63.34 2021-02-13T00:38:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.12 , -0.12 2021-02-13T00:38:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176697.51 epoch sec; seqNo:14407; 4059668.28 north; 592885.41 east 2021-02-13T00:38:17Z- AHRS_M2 msg: 1613176697.88 epoch sec; seqNo:14293 2021-02-13T00:38:17Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.13 psi 2021-02-13T00:38:17Z- handleDepth()-Depth_Keller msg: 1613176697888 epoch sec; seqNo:14408; depth 25.29 2021-02-13T00:38:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176697.90 epoch sec; seqNo:14408; 4059668.66 north; 592885.40 east 2021-02-13T00:38:18Z- AHRS_M2 msg: 1613176698.29 epoch sec; seqNo:14294 2021-02-13T00:38:18Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.13 psi 2021-02-13T00:38:18Z- handleDepth()-Depth_Keller msg: 1613176698297 epoch sec; seqNo:14409; depth 25.17 2021-02-13T00:38:18Z- handleDvl() - RDI_Pathfinder msg: 1613176698.31 epoch sec; seqNo:6951 2021-02-13T00:38:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 0.00 , 98.93 , 64.57 2021-02-13T00:38:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , -0.15 2021-02-13T00:38:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176698.33 epoch sec; seqNo:14409; 4059669.04 north; 592885.40 east 2021-02-13T00:38:18Z- AHRS_M2 msg: 1613176698.69 epoch sec; seqNo:14295 2021-02-13T00:38:18Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.14 psi 2021-02-13T00:38:18Z- handleDepth()-Depth_Keller msg: 1613176698697 epoch sec; seqNo:14410; depth 25.04 2021-02-13T00:38:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176698.71 epoch sec; seqNo:14410; 4059669.04 north; 592885.40 east 2021-02-13T00:38:19Z- AHRS_M2 msg: 1613176699.10 epoch sec; seqNo:14296 2021-02-13T00:38:19Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.15 psi 2021-02-13T00:38:19Z- handleDepth()-Depth_Keller msg: 1613176699109 epoch sec; seqNo:14411; depth 24.92 2021-02-13T00:38:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176699.12 epoch sec; seqNo:14411; 4059669.43 north; 592886.62 east 2021-02-13T00:38:19Z- AHRS_M2 msg: 1613176699.50 epoch sec; seqNo:14297 2021-02-13T00:38:19Z- AHRS_M2 msg: 0.66 phi; 0.34 theta; -0.16 psi 2021-02-13T00:38:19Z- handleDepth()-Depth_Keller msg: 1613176699509 epoch sec; seqNo:14412; depth 24.80 2021-02-13T00:38:19Z- handleDvl() - RDI_Pathfinder msg: 1613176699.53 epoch sec; seqNo:6952 2021-02-13T00:38:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.33 , 0.00 , 99.00 , 64.57 2021-02-13T00:38:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , -0.14 2021-02-13T00:38:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176699.54 epoch sec; seqNo:14412; 4059669.81 north; 592886.61 east 2021-02-13T00:38:19Z- AHRS_M2 msg: 1613176699.90 epoch sec; seqNo:14298 2021-02-13T00:38:19Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.16 psi 2021-02-13T00:38:19Z- handleDepth()-Depth_Keller msg: 1613176699908 epoch sec; seqNo:14413; depth 24.67 2021-02-13T00:38:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176699.92 epoch sec; seqNo:14413; 4059669.81 north; 592886.61 east 2021-02-13T00:38:20Z- AHRS_M2 msg: 1613176700.32 epoch sec; seqNo:14299 2021-02-13T00:38:20Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.17 psi 2021-02-13T00:38:20Z- handleDepth()-Depth_Keller msg: 1613176700326 epoch sec; seqNo:14414; depth 24.55 2021-02-13T00:38:20Z- handleDvl() - RDI_Pathfinder msg: 1613176700.34 epoch sec; seqNo:6953 2021-02-13T00:38:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 87.17 , 99.07 , 65.38 2021-02-13T00:38:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.11 , -0.08 2021-02-13T00:38:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176700.37 epoch sec; seqNo:14414; 4059670.19 north; 592886.61 east 2021-02-13T00:38:20Z- AHRS_M2 msg: 1613176700.71 epoch sec; seqNo:14300 2021-02-13T00:38:20Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.18 psi 2021-02-13T00:38:20Z- handleDepth()-Depth_Keller msg: 1613176700718 epoch sec; seqNo:14415; depth 24.42 2021-02-13T00:38:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176700.73 epoch sec; seqNo:14415; 4059670.57 north; 592886.60 east 2021-02-13T00:38:21Z- AHRS_M2 msg: 1613176701.12 epoch sec; seqNo:14301 2021-02-13T00:38:21Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.19 psi 2021-02-13T00:38:21Z- handleDepth()-Depth_Keller msg: 1613176701122 epoch sec; seqNo:14416; depth 24.30 2021-02-13T00:38:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176701.15 epoch sec; seqNo:14416; 4059670.57 north; 592886.60 east 2021-02-13T00:38:21Z- LcmTrn::handleMessages() - handling 44 messages took 0.74 ms and 129 calls... 2021-02-13T00:38:21Z- waiting for 1613176698.88, time is 1613176701.15 2021-02-13T00:38:21Z- LcmTrn::updateTrn() >>>> pose time 1613176701.15, meas time 1613176700.34 <<<< 2021-02-13T00:38:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176700.34 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:38:21Z- TerrainNav::motionUpdate() - currEstimate = (4059671, 592886.60, 24.30) 2021-02-13T00:38:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:38:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:38:21Z- TNavBF::Projecting Measurements 2021-02-13T00:38:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:38:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:38:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:38:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:38:21Z- TNavFilter::4 good beams. 2021-02-13T00:38:21Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:38:21Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:38:21Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:38:21Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:38:21Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:38:21Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:38:21Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:38:21Z- TNavBF::mmse selected from filter -1 2021-02-13T00:38:21Z- TerrainMapOctree: (vehN, vehE ) = (4059670.10, 592886.23). 2021-02-13T00:38:21Z- TerrainMapOctree: Min Distance = 2087.09. 2021-02-13T00:38:21Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:38:21Z- There is no specific good beam that all particles have in common. 2021-02-13T00:38:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:38:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.39 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:38:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:38:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 12.16 2021-02-13T00:38:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:38:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 15.70 2021-02-13T00:38:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:38:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-13T00:38:21Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:38:21Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:38:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:38:21Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:38:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:38:21Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:38:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:38:21Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:38:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:38:21Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:38:21Z- Filter 0 No beams used in subcloud update 2021-02-13T00:38:21Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:38:21Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:38:21Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 694.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:38:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176700.34 sec. 2021-02-13T00:38:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:38:21Z- TNavBF::mmse selected from filter -1 2021-02-13T00:38:21Z- variances([0], [2], and psi_berg) = 693.943723 832.994861 0.000000 2021-02-13T00:38:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:38:21Z- LcmTrn::publishEstimates() - MLE : 4059670.07 592914.10 24.30 2021-02-13T00:38:21Z- LcmTrn::publishEstimates() - MMSE : 4059670.57 592886.60 24.30 2021-02-13T00:38:21Z- LcmTrn::publishEstimates() - COVAR: 693.94 832.99 0.98 0.00 2021-02-13T00:38:21Z- AHRS_M2 msg: 1613176701.52 epoch sec; seqNo:14302 2021-02-13T00:38:21Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.19 psi 2021-02-13T00:38:21Z- handleDepth()-Depth_Keller msg: 1613176701524 epoch sec; seqNo:14417; depth 24.18 2021-02-13T00:38:21Z- handleDvl() - RDI_Pathfinder msg: 1613176701.54 epoch sec; seqNo:6954 2021-02-13T00:38:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 136.11 , 0.00 , 97.17 , 65.80 2021-02-13T00:38:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 1.11 , 0.00 2021-02-13T00:38:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176701.56 epoch sec; seqNo:14417; 4059670.58 north; 592887.82 east 2021-02-13T00:38:21Z- AHRS_M2 msg: 1613176701.93 epoch sec; seqNo:14303 2021-02-13T00:38:21Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.20 psi 2021-02-13T00:38:21Z- handleDepth()-Depth_Keller msg: 1613176701935 epoch sec; seqNo:14418; depth 24.05 2021-02-13T00:38:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176701.95 epoch sec; seqNo:14418; 4059670.58 north; 592887.82 east 2021-02-13T00:38:22Z- AHRS_M2 msg: 1613176702.34 epoch sec; seqNo:14304 2021-02-13T00:38:22Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.21 psi 2021-02-13T00:38:22Z- handleDepth()-Depth_Keller msg: 1613176702340 epoch sec; seqNo:14419; depth 23.93 2021-02-13T00:38:22Z- handleDvl() - RDI_Pathfinder msg: 1613176702.35 epoch sec; seqNo:6955 2021-02-13T00:38:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 97.03 , 99.74 , 66.62 2021-02-13T00:38:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.12 , 0.08 2021-02-13T00:38:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176702.36 epoch sec; seqNo:14419; 4059670.96 north; 592887.82 east 2021-02-13T00:38:22Z- AHRS_M2 msg: 1613176702.73 epoch sec; seqNo:14305 2021-02-13T00:38:22Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.21 psi 2021-02-13T00:38:22Z- handleDepth()-Depth_Keller msg: 1613176702741 epoch sec; seqNo:14420; depth 23.81 2021-02-13T00:38:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176702.76 epoch sec; seqNo:14420; 4059671.34 north; 592887.82 east 2021-02-13T00:38:23Z- AHRS_M2 msg: 1613176703.14 epoch sec; seqNo:14306 2021-02-13T00:38:23Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.21 psi 2021-02-13T00:38:23Z- handleDepth()-Depth_Keller msg: 1613176703144 epoch sec; seqNo:14421; depth 23.68 2021-02-13T00:38:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176703.15 epoch sec; seqNo:14421; 4059671.72 north; 592887.81 east 2021-02-13T00:38:23Z- AHRS_M2 msg: 1613176703.54 epoch sec; seqNo:14307 2021-02-13T00:38:23Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.22 psi 2021-02-13T00:38:23Z- handleDepth()-Depth_Keller msg: 1613176703549 epoch sec; seqNo:14422; depth 23.57 2021-02-13T00:38:23Z- handleDvl() - RDI_Pathfinder msg: 1613176703.56 epoch sec; seqNo:6956 2021-02-13T00:38:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.52 , 91.02 , 100.75 , 66.21 2021-02-13T00:38:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.07 , 0.13 2021-02-13T00:38:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176703.57 epoch sec; seqNo:14422; 4059671.73 north; 592889.03 east 2021-02-13T00:38:23Z- AHRS_M2 msg: 1613176703.94 epoch sec; seqNo:14308 2021-02-13T00:38:23Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.22 psi 2021-02-13T00:38:23Z- handleDepth()-Depth_Keller msg: 1613176703950 epoch sec; seqNo:14423; depth 23.44 2021-02-13T00:38:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176703.96 epoch sec; seqNo:14423; 4059672.11 north; 592889.03 east 2021-02-13T00:38:24Z- AHRS_M2 msg: 1613176704.35 epoch sec; seqNo:14309 2021-02-13T00:38:24Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.22 psi 2021-02-13T00:38:24Z- handleDepth()-Depth_Keller msg: 1613176704368 epoch sec; seqNo:14424; depth 23.31 2021-02-13T00:38:24Z- handleDvl() - RDI_Pathfinder msg: 1613176704.39 epoch sec; seqNo:6957 2021-02-13T00:38:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.93 , 92.30 , 104.47 , 67.43 2021-02-13T00:38:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.00 , 0.13 2021-02-13T00:38:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176704.40 epoch sec; seqNo:14424; 4059672.49 north; 592889.03 east 2021-02-13T00:38:24Z- AHRS_M2 msg: 1613176704.75 epoch sec; seqNo:14310 2021-02-13T00:38:24Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.22 psi 2021-02-13T00:38:24Z- handleDepth()-Depth_Keller msg: 1613176704756 epoch sec; seqNo:14425; depth 23.19 2021-02-13T00:38:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176704.77 epoch sec; seqNo:14425; 4059672.87 north; 592889.02 east 2021-02-13T00:38:25Z- AHRS_M2 msg: 1613176705.16 epoch sec; seqNo:14311 2021-02-13T00:38:25Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.21 psi 2021-02-13T00:38:25Z- handleDepth()-Depth_Keller msg: 1613176705162 epoch sec; seqNo:14426; depth 23.09 2021-02-13T00:38:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176705.17 epoch sec; seqNo:14426; 4059673.25 north; 592889.02 east 2021-02-13T00:38:25Z- AHRS_M2 msg: 1613176705.56 epoch sec; seqNo:14312 2021-02-13T00:38:25Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.21 psi 2021-02-13T00:38:25Z- handleDepth()-Depth_Keller msg: 1613176705565 epoch sec; seqNo:14427; depth 22.96 2021-02-13T00:38:25Z- handleDvl() - RDI_Pathfinder msg: 1613176705.58 epoch sec; seqNo:6958 2021-02-13T00:38:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:38:25Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:38:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176705.59 epoch sec; seqNo:14427; 4059673.63 north; 592889.01 east 2021-02-13T00:38:25Z- AHRS_M2 msg: 1613176705.96 epoch sec; seqNo:14313 2021-02-13T00:38:25Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.20 psi 2021-02-13T00:38:25Z- handleDepth()-Depth_Keller msg: 1613176705968 epoch sec; seqNo:14428; depth 22.84 2021-02-13T00:38:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176705.99 epoch sec; seqNo:14428; 4059673.64 north; 592890.23 east 2021-02-13T00:38:26Z- LcmTrn::handleMessages() - handling 41 messages took 0.66 ms and 128 calls... 2021-02-13T00:38:26Z- waiting for 1613176704.15, time is 1613176705.99 2021-02-13T00:38:26Z- LcmTrn::updateTrn() >>>> pose time 1613176705.99, meas time 1613176705.58 <<<< 2021-02-13T00:38:26Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613176705.58, ping # 0. 2021-02-13T00:38:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176705.58 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:38:26Z- TerrainNav::motionUpdate() - currEstimate = (4059674, 592890.23, 22.84) 2021-02-13T00:38:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:38:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:38:26Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:38:26Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:38:26Z- TNavBF::Projecting Measurements 2021-02-13T00:38:26Z- TNavFilter::Measurement from time = 1613176705.58 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:38:26Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:38:26Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:38:26Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:38:26Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:38:26Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:38:26Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:38:26Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:38:26Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 693.9 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:38:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:38:26Z- TNavBF::mmse selected from filter -1 2021-02-13T00:38:26Z- variances([0], [2], and psi_berg) = 693.874671 832.994827 0.000000 2021-02-13T00:38:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:38:26Z- LcmTrn::publishEstimates() - MLE : 4059673.14 592917.73 22.84 2021-02-13T00:38:26Z- LcmTrn::publishEstimates() - MMSE : 4059673.64 592890.23 22.84 2021-02-13T00:38:26Z- LcmTrn::publishEstimates() - COVAR: 693.87 832.99 1.22 0.00 2021-02-13T00:38:26Z- AHRS_M2 msg: 1613176706.37 epoch sec; seqNo:14314 2021-02-13T00:38:26Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.20 psi 2021-02-13T00:38:26Z- handleDepth()-Depth_Keller msg: 1613176706377 epoch sec; seqNo:14429; depth 22.71 2021-02-13T00:38:26Z- handleDvl() - RDI_Pathfinder msg: 1613176706.40 epoch sec; seqNo:6959 2021-02-13T00:38:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.92 , 93.25 , 102.72 , 66.61 2021-02-13T00:38:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.08 , 0.06 2021-02-13T00:38:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176706.41 epoch sec; seqNo:14429; 4059674.02 north; 592890.23 east 2021-02-13T00:38:26Z- AHRS_M2 msg: 1613176706.77 epoch sec; seqNo:14315 2021-02-13T00:38:26Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.19 psi 2021-02-13T00:38:26Z- handleDepth()-Depth_Keller msg: 1613176706775 epoch sec; seqNo:14430; depth 22.59 2021-02-13T00:38:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176706.79 epoch sec; seqNo:14430; 4059674.40 north; 592890.23 east 2021-02-13T00:38:27Z- AHRS_M2 msg: 1613176707.18 epoch sec; seqNo:14316 2021-02-13T00:38:27Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.18 psi 2021-02-13T00:38:27Z- handleDepth()-Depth_Keller msg: 1613176707200 epoch sec; seqNo:14431; depth 22.48 2021-02-13T00:38:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176707.22 epoch sec; seqNo:14431; 4059674.78 north; 592890.22 east 2021-02-13T00:38:27Z- AHRS_M2 msg: 1613176707.58 epoch sec; seqNo:14317 2021-02-13T00:38:27Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.18 psi 2021-02-13T00:38:27Z- handleDepth()-Depth_Keller msg: 1613176707584 epoch sec; seqNo:14432; depth 22.35 2021-02-13T00:38:27Z- handleDvl() - RDI_Pathfinder msg: 1613176707.60 epoch sec; seqNo:6960 2021-02-13T00:38:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.37 , 98.32 , 103.19 , 68.24 2021-02-13T00:38:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.08 , 0.00 2021-02-13T00:38:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176707.62 epoch sec; seqNo:14432; 4059674.78 north; 592890.22 east 2021-02-13T00:38:27Z- AHRS_M2 msg: 1613176707.98 epoch sec; seqNo:14318 2021-02-13T00:38:27Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.18 psi 2021-02-13T00:38:27Z- handleDepth()-Depth_Keller msg: 1613176707989 epoch sec; seqNo:14433; depth 22.23 2021-02-13T00:38:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176708.01 epoch sec; seqNo:14433; 4059675.17 north; 592891.44 east 2021-02-13T00:38:28Z- AHRS_M2 msg: 1613176708.39 epoch sec; seqNo:14319 2021-02-13T00:38:28Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.18 psi 2021-02-13T00:38:28Z- handleDepth()-Depth_Keller msg: 1613176708392 epoch sec; seqNo:14434; depth 22.12 2021-02-13T00:38:28Z- handleDvl() - RDI_Pathfinder msg: 1613176708.42 epoch sec; seqNo:6961 2021-02-13T00:38:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 98.12 , 102.99 , 67.84 2021-02-13T00:38:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , -0.04 2021-02-13T00:38:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176708.43 epoch sec; seqNo:14434; 4059675.55 north; 592891.43 east 2021-02-13T00:38:28Z- AHRS_M2 msg: 1613176708.79 epoch sec; seqNo:14320 2021-02-13T00:38:28Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.18 psi 2021-02-13T00:38:28Z- handleDepth()-Depth_Keller msg: 1613176708801 epoch sec; seqNo:14435; depth 21.98 2021-02-13T00:38:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:38:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:38:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:38:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:38:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:38:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:38:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:38:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:38:28Z- handleDvl() - RDI_Pathfinder msg: 1613176708.80 epoch sec; seqNo:6962 2021-02-13T00:38:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:38:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , -0.04 2021-02-13T00:38:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176708.81 epoch sec; seqNo:14435; 4059675.93 north; 592891.43 east 2021-02-13T00:38:29Z- AHRS_M2 msg: 1613176709.20 epoch sec; seqNo:14321 2021-02-13T00:38:29Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.18 psi 2021-02-13T00:38:29Z- handleDepth()-Depth_Keller msg: 1613176709205 epoch sec; seqNo:14436; depth 21.86 2021-02-13T00:38:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176709.21 epoch sec; seqNo:14436; 4059675.93 north; 592891.43 east 2021-02-13T00:38:29Z- AHRS_M2 msg: 1613176709.60 epoch sec; seqNo:14322 2021-02-13T00:38:29Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.18 psi 2021-02-13T00:38:29Z- handleDepth()-Depth_Keller msg: 1613176709607 epoch sec; seqNo:14437; depth 21.74 2021-02-13T00:38:29Z- handleDvl() - RDI_Pathfinder msg: 1613176709.62 epoch sec; seqNo:6963 2021-02-13T00:38:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.64 , 98.12 , 104.20 , 67.84 2021-02-13T00:38:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , -0.06 2021-02-13T00:38:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176709.63 epoch sec; seqNo:14437; 4059676.30 north; 592891.43 east 2021-02-13T00:38:30Z- AHRS_M2 msg: 1613176710.01 epoch sec; seqNo:14323 2021-02-13T00:38:30Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.17 psi 2021-02-13T00:38:30Z- handleDepth()-Depth_Keller msg: 1613176710014 epoch sec; seqNo:14438; depth 21.60 2021-02-13T00:38:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:38:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:38:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:38:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:38:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:38:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:38:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:38:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:38:30Z- handleDvl() - RDI_Pathfinder msg: 1613176710.02 epoch sec; seqNo:6964 2021-02-13T00:38:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:38:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , -0.06 2021-02-13T00:38:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176710.03 epoch sec; seqNo:14438; 4059676.68 north; 592891.42 east 2021-02-13T00:38:30Z- AHRS_M2 msg: 1613176710.42 epoch sec; seqNo:14324 2021-02-13T00:38:30Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.17 psi 2021-02-13T00:38:30Z- handleDepth()-Depth_Keller msg: 1613176710429 epoch sec; seqNo:14439; depth 21.48 2021-02-13T00:38:30Z- handleDvl() - RDI_Pathfinder msg: 1613176710.44 epoch sec; seqNo:6965 2021-02-13T00:38:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 105.28 , 67.43 2021-02-13T00:38:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , -0.08 2021-02-13T00:38:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176710.45 epoch sec; seqNo:14439; 4059676.70 north; 592892.64 east 2021-02-13T00:38:30Z- AHRS_M2 msg: 1613176710.81 epoch sec; seqNo:14325 2021-02-13T00:38:30Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.16 psi 2021-02-13T00:38:30Z- handleDepth()-Depth_Keller msg: 1613176710816 epoch sec; seqNo:14440; depth 21.35 2021-02-13T00:38:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176710.83 epoch sec; seqNo:14440; 4059677.08 north; 592892.64 east 2021-02-13T00:38:31Z- AHRS_M2 msg: 1613176711.21 epoch sec; seqNo:14326 2021-02-13T00:38:31Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.16 psi 2021-02-13T00:38:31Z- handleDepth()-Depth_Keller msg: 1613176711221 epoch sec; seqNo:14441; depth 21.23 2021-02-13T00:38:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:38:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:38:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:38:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:38:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:38:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:38:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:38:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:38:31Z- handleDvl() - RDI_Pathfinder msg: 1613176711.22 epoch sec; seqNo:6966 2021-02-13T00:38:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:38:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , -0.08 2021-02-13T00:38:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176711.23 epoch sec; seqNo:14441; 4059677.45 north; 592892.63 east 2021-02-13T00:38:31Z- LcmTrn::handleMessages() - handling 47 messages took 0.68 ms and 134 calls... 2021-02-13T00:38:31Z- waiting for 1613176708.99, time is 1613176711.23 2021-02-13T00:38:31Z- LcmTrn::updateTrn() equating pose 1613176711.23 and meas times 1613176711.23 2021-02-13T00:38:31Z- LcmTrn::updateTrn() >>>> pose time 1613176711.23, meas time 1613176711.23 <<<< 2021-02-13T00:38:31Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613176711.23, ping # 0. 2021-02-13T00:38:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176711.23 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:38:31Z- TerrainNav::motionUpdate() - currEstimate = (4059677, 592892.63, 21.23) 2021-02-13T00:38:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:38:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:38:31Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:38:31Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:38:31Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176711.23 sec.; No relevant navigation data available 2021-02-13T00:38:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:38:31Z- TNavBF::mmse selected from filter -1 2021-02-13T00:38:31Z- variances([0], [2], and psi_berg) = 693.820834 832.994800 0.000000 2021-02-13T00:38:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:38:31Z- LcmTrn::publishEstimates() - MLE : 4059676.95 592920.13 21.23 2021-02-13T00:38:31Z- LcmTrn::publishEstimates() - MMSE : 4059677.45 592892.63 21.23 2021-02-13T00:38:31Z- LcmTrn::publishEstimates() - COVAR: 693.82 832.99 1.45 0.00 2021-02-13T00:38:31Z- AHRS_M2 msg: 1613176711.62 epoch sec; seqNo:14327 2021-02-13T00:38:31Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-13T00:38:31Z- handleDepth()-Depth_Keller msg: 1613176711625 epoch sec; seqNo:14442; depth 21.11 2021-02-13T00:38:31Z- handleDvl() - RDI_Pathfinder msg: 1613176711.64 epoch sec; seqNo:6967 2021-02-13T00:38:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 104.07 , 66.62 2021-02-13T00:38:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , -0.09 2021-02-13T00:38:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176711.65 epoch sec; seqNo:14442; 4059677.45 north; 592892.63 east 2021-02-13T00:38:32Z- AHRS_M2 msg: 1613176712.02 epoch sec; seqNo:14328 2021-02-13T00:38:32Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-13T00:38:32Z- handleDepth()-Depth_Keller msg: 1613176712029 epoch sec; seqNo:14443; depth 20.97 2021-02-13T00:38:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176712.04 epoch sec; seqNo:14443; 4059677.83 north; 592892.63 east 2021-02-13T00:38:32Z- AHRS_M2 msg: 1613176712.43 epoch sec; seqNo:14329 2021-02-13T00:38:32Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-13T00:38:32Z- handleDepth()-Depth_Keller msg: 1613176712445 epoch sec; seqNo:14444; depth 20.85 2021-02-13T00:38:32Z- handleDvl() - RDI_Pathfinder msg: 1613176712.46 epoch sec; seqNo:6968 2021-02-13T00:38:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 91.96 , 105.35 , 68.25 2021-02-13T00:38:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , -0.10 2021-02-13T00:38:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176712.47 epoch sec; seqNo:14444; 4059678.21 north; 592892.63 east 2021-02-13T00:38:32Z- AHRS_M2 msg: 1613176712.83 epoch sec; seqNo:14330 2021-02-13T00:38:32Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.14 psi 2021-02-13T00:38:32Z- handleDepth()-Depth_Keller msg: 1613176712835 epoch sec; seqNo:14445; depth 20.71 2021-02-13T00:38:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176712.86 epoch sec; seqNo:14445; 4059678.21 north; 592892.63 east 2021-02-13T00:38:33Z- AHRS_M2 msg: 1613176713.24 epoch sec; seqNo:14331 2021-02-13T00:38:33Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.14 psi 2021-02-13T00:38:33Z- handleDepth()-Depth_Keller msg: 1613176713241 epoch sec; seqNo:14446; depth 20.59 2021-02-13T00:38:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176713.25 epoch sec; seqNo:14446; 4059678.60 north; 592893.84 east 2021-02-13T00:38:33Z- AHRS_M2 msg: 1613176713.64 epoch sec; seqNo:14332 2021-02-13T00:38:33Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.14 psi 2021-02-13T00:38:33Z- handleDepth()-Depth_Keller msg: 1613176713649 epoch sec; seqNo:14447; depth 20.45 2021-02-13T00:38:33Z- handleDvl() - RDI_Pathfinder msg: 1613176713.66 epoch sec; seqNo:6969 2021-02-13T00:38:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:38:33Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:38:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176713.67 epoch sec; seqNo:14447; 4059678.98 north; 592893.84 east 2021-02-13T00:38:34Z- AHRS_M2 msg: 1613176714.04 epoch sec; seqNo:14333 2021-02-13T00:38:34Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.14 psi 2021-02-13T00:38:34Z- handleDepth()-Depth_Keller msg: 1613176714050 epoch sec; seqNo:14448; depth 20.30 2021-02-13T00:38:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176714.06 epoch sec; seqNo:14448; 4059678.98 north; 592893.84 east 2021-02-13T00:38:34Z- AHRS_M2 msg: 1613176714.45 epoch sec; seqNo:14334 2021-02-13T00:38:34Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.14 psi 2021-02-13T00:38:34Z- handleDepth()-Depth_Keller msg: 1613176714457 epoch sec; seqNo:14449; depth 20.18 2021-02-13T00:38:34Z- handleDvl() - RDI_Pathfinder msg: 1613176714.48 epoch sec; seqNo:6970 2021-02-13T00:38:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 97.37 , 105.49 , 78.44 2021-02-13T00:38:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.15 , 0.01 2021-02-13T00:38:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176714.49 epoch sec; seqNo:14449; 4059679.36 north; 592893.83 east 2021-02-13T00:38:34Z- AHRS_M2 msg: 1613176714.85 epoch sec; seqNo:14335 2021-02-13T00:38:34Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-13T00:38:34Z- handleDepth()-Depth_Keller msg: 1613176714861 epoch sec; seqNo:14450; depth 20.05 2021-02-13T00:38:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:38:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:38:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:38:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:38:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:38:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:38:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:38:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:38:34Z- handleDvl() - RDI_Pathfinder msg: 1613176714.86 epoch sec; seqNo:6971 2021-02-13T00:38:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:38:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.15 , 0.01 2021-02-13T00:38:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176714.87 epoch sec; seqNo:14450; 4059679.74 north; 592893.83 east 2021-02-13T00:38:35Z- AHRS_M2 msg: 1613176715.26 epoch sec; seqNo:14336 2021-02-13T00:38:35Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-13T00:38:35Z- handleDepth()-Depth_Keller msg: 1613176715265 epoch sec; seqNo:14451; depth 19.92 2021-02-13T00:38:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176715.27 epoch sec; seqNo:14451; 4059680.12 north; 592893.83 east 2021-02-13T00:38:35Z- AHRS_M2 msg: 1613176715.66 epoch sec; seqNo:14337 2021-02-13T00:38:35Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-13T00:38:35Z- handleDepth()-Depth_Keller msg: 1613176715668 epoch sec; seqNo:14452; depth 19.77 2021-02-13T00:38:35Z- handleDvl() - RDI_Pathfinder msg: 1613176715.68 epoch sec; seqNo:6972 2021-02-13T00:38:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 91.69 , 105.08 , 67.43 2021-02-13T00:38:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.14 , 0.10 2021-02-13T00:38:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176715.69 epoch sec; seqNo:14452; 4059680.50 north; 592893.82 east 2021-02-13T00:38:36Z- AHRS_M2 msg: 1613176716.06 epoch sec; seqNo:14338 2021-02-13T00:38:36Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-13T00:38:36Z- handleDepth()-Depth_Keller msg: 1613176716073 epoch sec; seqNo:14453; depth 19.64 2021-02-13T00:38:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:38:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:38:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:38:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:38:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:38:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:38:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:38:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:38:36Z- handleDvl() - RDI_Pathfinder msg: 1613176716.08 epoch sec; seqNo:6973 2021-02-13T00:38:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:38:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.14 , 0.10 2021-02-13T00:38:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176716.10 epoch sec; seqNo:14453; 4059680.50 north; 592893.82 east 2021-02-13T00:38:36Z- LcmTrn::handleMessages() - handling 43 messages took 0.63 ms and 131 calls... 2021-02-13T00:38:36Z- waiting for 1613176714.23, time is 1613176716.10 2021-02-13T00:38:36Z- LcmTrn::updateTrn() equating pose 1613176716.10 and meas times 1613176716.10 2021-02-13T00:38:36Z- LcmTrn::updateTrn() >>>> pose time 1613176716.10, meas time 1613176716.10 <<<< 2021-02-13T00:38:36Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613176716.10, ping # 0. 2021-02-13T00:38:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176716.10 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:38:36Z- TerrainNav::motionUpdate() - currEstimate = (4059680, 592893.82, 19.64) 2021-02-13T00:38:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:38:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:38:36Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:38:36Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:38:36Z- TNavBF::Projecting Measurements 2021-02-13T00:38:36Z- TNavFilter::Measurement from time = 1613176716.10 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:38:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:38:36Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:38:36Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:38:36Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:38:36Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:38:36Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:38:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:38:36Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 693.8 EastVariance: 833.0 DepthVariance: 1.6 this->DepthBiasCov: 1.6 2021-02-13T00:38:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:38:36Z- TNavBF::mmse selected from filter -1 2021-02-13T00:38:36Z- variances([0], [2], and psi_berg) = 693.775021 832.994778 0.000000 2021-02-13T00:38:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:38:36Z- LcmTrn::publishEstimates() - MLE : 4059680.00 592921.32 19.64 2021-02-13T00:38:36Z- LcmTrn::publishEstimates() - MMSE : 4059680.50 592893.82 19.64 2021-02-13T00:38:36Z- LcmTrn::publishEstimates() - COVAR: 693.78 832.99 1.62 0.00 2021-02-13T00:38:36Z- AHRS_M2 msg: 1613176716.47 epoch sec; seqNo:14339 2021-02-13T00:38:36Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.15 psi 2021-02-13T00:38:36Z- handleDepth()-Depth_Keller msg: 1613176716477 epoch sec; seqNo:14454; depth 19.52 2021-02-13T00:38:36Z- handleDvl() - RDI_Pathfinder msg: 1613176716.49 epoch sec; seqNo:6974 2021-02-13T00:38:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.06 , 92.03 , 106.63 , 69.07 2021-02-13T00:38:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , 0.15 2021-02-13T00:38:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176716.50 epoch sec; seqNo:14454; 4059680.89 north; 592895.04 east 2021-02-13T00:38:36Z- AHRS_M2 msg: 1613176716.87 epoch sec; seqNo:14340 2021-02-13T00:38:36Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.15 psi 2021-02-13T00:38:36Z- handleDepth()-Depth_Keller msg: 1613176716878 epoch sec; seqNo:14455; depth 19.39 2021-02-13T00:38:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176716.89 epoch sec; seqNo:14455; 4059681.27 north; 592895.03 east 2021-02-13T00:38:37Z- AHRS_M2 msg: 1613176717.28 epoch sec; seqNo:14341 2021-02-13T00:38:37Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.15 psi 2021-02-13T00:38:37Z- handleDepth()-Depth_Keller msg: 1613176717284 epoch sec; seqNo:14456; depth 19.26 2021-02-13T00:38:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:38:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:38:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:38:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:38:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:38:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:38:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:38:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:38:37Z- handleDvl() - RDI_Pathfinder msg: 1613176717.31 epoch sec; seqNo:6975 2021-02-13T00:38:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:38:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , 0.15 2021-02-13T00:38:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176717.32 epoch sec; seqNo:14456; 4059681.65 north; 592895.03 east 2021-02-13T00:38:37Z- AHRS_M2 msg: 1613176717.68 epoch sec; seqNo:14342 2021-02-13T00:38:37Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.16 psi 2021-02-13T00:38:37Z- handleDepth()-Depth_Keller msg: 1613176717684 epoch sec; seqNo:14457; depth 19.12 2021-02-13T00:38:37Z- handleDvl() - RDI_Pathfinder msg: 1613176717.70 epoch sec; seqNo:6976 2021-02-13T00:38:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.79 , 91.96 , 106.57 , 69.89 2021-02-13T00:38:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , 0.16 2021-02-13T00:38:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176717.71 epoch sec; seqNo:14457; 4059682.03 north; 592895.03 east 2021-02-13T00:38:38Z- AHRS_M2 msg: 1613176718.09 epoch sec; seqNo:14343 2021-02-13T00:38:38Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.17 psi 2021-02-13T00:38:38Z- handleDepth()-Depth_Keller msg: 1613176718091 epoch sec; seqNo:14458; depth 18.98 2021-02-13T00:38:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176718.10 epoch sec; seqNo:14458; 4059682.03 north; 592895.03 east 2021-02-13T00:38:38Z- AHRS_M2 msg: 1613176718.49 epoch sec; seqNo:14344 2021-02-13T00:38:38Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.18 psi 2021-02-13T00:38:38Z- handleDepth()-Depth_Keller msg: 1613176718494 epoch sec; seqNo:14459; depth 18.86 2021-02-13T00:38:38Z- handleDvl() - RDI_Pathfinder msg: 1613176718.51 epoch sec; seqNo:6977 2021-02-13T00:38:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.47 , 94.33 , 106.50 , 70.29 2021-02-13T00:38:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.07 , 0.13 2021-02-13T00:38:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176718.52 epoch sec; seqNo:14459; 4059682.41 north; 592895.02 east 2021-02-13T00:38:38Z- AHRS_M2 msg: 1613176718.89 epoch sec; seqNo:14345 2021-02-13T00:38:38Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.19 psi 2021-02-13T00:38:38Z- handleDepth()-Depth_Keller msg: 1613176718896 epoch sec; seqNo:14460; depth 18.73 2021-02-13T00:38:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176718.92 epoch sec; seqNo:14460; 4059682.80 north; 592896.24 east 2021-02-13T00:38:39Z- AHRS_M2 msg: 1613176719.30 epoch sec; seqNo:14346 2021-02-13T00:38:39Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.19 psi 2021-02-13T00:38:39Z- handleDepth()-Depth_Keller msg: 1613176719306 epoch sec; seqNo:14461; depth 18.60 2021-02-13T00:38:39Z- handleDvl() - RDI_Pathfinder msg: 1613176719.32 epoch sec; seqNo:6978 2021-02-13T00:38:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 97.98 , 100.42 , 69.89 2021-02-13T00:38:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.12 , 0.05 2021-02-13T00:38:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176719.34 epoch sec; seqNo:14461; 4059683.18 north; 592896.23 east 2021-02-13T00:38:39Z- AHRS_M2 msg: 1613176719.70 epoch sec; seqNo:14347 2021-02-13T00:38:39Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.19 psi 2021-02-13T00:38:39Z- handleDepth()-Depth_Keller msg: 1613176719706 epoch sec; seqNo:14462; depth 18.48 2021-02-13T00:38:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176719.72 epoch sec; seqNo:14462; 4059683.18 north; 592896.23 east 2021-02-13T00:38:40Z- AHRS_M2 msg: 1613176720.10 epoch sec; seqNo:14348 2021-02-13T00:38:40Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.19 psi 2021-02-13T00:38:40Z- handleDepth()-Depth_Keller msg: 1613176720108 epoch sec; seqNo:14463; depth 18.36 2021-02-13T00:38:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176720.13 epoch sec; seqNo:14463; 4059683.56 north; 592896.23 east 2021-02-13T00:38:40Z- AHRS_M2 msg: 1613176720.51 epoch sec; seqNo:14349 2021-02-13T00:38:40Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.19 psi 2021-02-13T00:38:40Z- handleDepth()-Depth_Keller msg: 1613176720517 epoch sec; seqNo:14464; depth 18.21 2021-02-13T00:38:40Z- handleDvl() - RDI_Pathfinder msg: 1613176720.54 epoch sec; seqNo:6979 2021-02-13T00:38:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 96.83 , 107.78 , 78.58 2021-02-13T00:38:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.13 , 0.03 2021-02-13T00:38:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176720.55 epoch sec; seqNo:14464; 4059683.93 north; 592896.23 east 2021-02-13T00:38:40Z- AHRS_M2 msg: 1613176720.91 epoch sec; seqNo:14350 2021-02-13T00:38:40Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.19 psi 2021-02-13T00:38:40Z- handleDepth()-Depth_Keller msg: 1613176720917 epoch sec; seqNo:14465; depth 18.09 2021-02-13T00:38:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176720.93 epoch sec; seqNo:14465; 4059683.95 north; 592897.45 east 2021-02-13T00:38:41Z- AHRS_M2 msg: 1613176721.32 epoch sec; seqNo:14351 2021-02-13T00:38:41Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.19 psi 2021-02-13T00:38:41Z- handleDepth()-Depth_Keller msg: 1613176721335 epoch sec; seqNo:14466; depth 17.98 2021-02-13T00:38:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176721.36 epoch sec; seqNo:14466; 4059684.33 north; 592897.44 east 2021-02-13T00:38:41Z- LcmTrn::handleMessages() - handling 45 messages took 0.74 ms and 131 calls... 2021-02-13T00:38:41Z- waiting for 1613176719.10, time is 1613176721.36 2021-02-13T00:38:41Z- LcmTrn::updateTrn() >>>> pose time 1613176721.36, meas time 1613176720.54 <<<< 2021-02-13T00:38:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176720.54 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:38:41Z- TerrainNav::motionUpdate() - currEstimate = (4059684, 592897.44, 17.98) 2021-02-13T00:38:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:38:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:38:41Z- TNavBF::Projecting Measurements 2021-02-13T00:38:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:38:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:38:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:38:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:38:41Z- TNavFilter::4 good beams. 2021-02-13T00:38:41Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:38:41Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:38:41Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:38:41Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:38:41Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:38:41Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:38:41Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:38:41Z- TNavBF::mmse selected from filter -1 2021-02-13T00:38:41Z- TerrainMapOctree: (vehN, vehE ) = (4059683.73, 592896.88). 2021-02-13T00:38:41Z- TerrainMapOctree: Min Distance = 2069.82. 2021-02-13T00:38:41Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:38:41Z- There is no specific good beam that all particles have in common. 2021-02-13T00:38:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:38:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.84 K: 0.137 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-13T00:38:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:38:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.59 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 15.00 2021-02-13T00:38:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:38:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.45 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 18.59 2021-02-13T00:38:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:38:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.36 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-13T00:38:41Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:38:41Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:38:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:38:41Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:38:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:38:41Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:38:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:38:41Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:38:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:38:41Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:38:41Z- Filter 0 No beams used in subcloud update 2021-02-13T00:38:41Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:38:41Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:38:41Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 693.7 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:38:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176720.54 sec. 2021-02-13T00:38:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:38:41Z- TNavBF::mmse selected from filter -1 2021-02-13T00:38:41Z- variances([0], [2], and psi_berg) = 693.698633 832.994740 0.000000 2021-02-13T00:38:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:38:41Z- LcmTrn::publishEstimates() - MLE : 4059683.83 592927.94 17.98 2021-02-13T00:38:41Z- LcmTrn::publishEstimates() - MMSE : 4059684.33 592897.44 17.98 2021-02-13T00:38:41Z- LcmTrn::publishEstimates() - COVAR: 693.70 832.99 1.26 0.00 2021-02-13T00:38:41Z- AHRS_M2 msg: 1613176721.72 epoch sec; seqNo:14352 2021-02-13T00:38:41Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T00:38:41Z- handleDepth()-Depth_Keller msg: 1613176721724 epoch sec; seqNo:14467; depth 17.86 2021-02-13T00:38:41Z- handleDvl() - RDI_Pathfinder msg: 1613176721.73 epoch sec; seqNo:6980 2021-02-13T00:38:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.06 , 97.71 , 106.23 , 78.24 2021-02-13T00:38:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.12 , -0.03 2021-02-13T00:38:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176721.76 epoch sec; seqNo:14467; 4059684.71 north; 592897.44 east 2021-02-13T00:38:42Z- AHRS_M2 msg: 1613176722.13 epoch sec; seqNo:14353 2021-02-13T00:38:42Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T00:38:42Z- handleDepth()-Depth_Keller msg: 1613176722131 epoch sec; seqNo:14468; depth 17.74 2021-02-13T00:38:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176722.14 epoch sec; seqNo:14468; 4059684.71 north; 592897.44 east 2021-02-13T00:38:42Z- AHRS_M2 msg: 1613176722.53 epoch sec; seqNo:14354 2021-02-13T00:38:42Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T00:38:42Z- handleDepth()-Depth_Keller msg: 1613176722538 epoch sec; seqNo:14469; depth 17.62 2021-02-13T00:38:42Z- handleDvl() - RDI_Pathfinder msg: 1613176722.55 epoch sec; seqNo:6981 2021-02-13T00:38:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.52 , 97.78 , 108.73 , 72.34 2021-02-13T00:38:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.07 , -0.06 2021-02-13T00:38:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176722.56 epoch sec; seqNo:14469; 4059685.08 north; 592897.43 east 2021-02-13T00:38:42Z- AHRS_M2 msg: 1613176722.93 epoch sec; seqNo:14355 2021-02-13T00:38:42Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T00:38:42Z- handleDepth()-Depth_Keller msg: 1613176722941 epoch sec; seqNo:14470; depth 17.50 2021-02-13T00:38:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176722.96 epoch sec; seqNo:14470; 4059685.46 north; 592897.43 east 2021-02-13T00:38:43Z- AHRS_M2 msg: 1613176723.35 epoch sec; seqNo:14356 2021-02-13T00:38:43Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.19 psi 2021-02-13T00:38:43Z- handleDepth()-Depth_Keller msg: 1613176723360 epoch sec; seqNo:14471; depth 17.38 2021-02-13T00:38:43Z- handleDvl() - RDI_Pathfinder msg: 1613176723.38 epoch sec; seqNo:6982 2021-02-13T00:38:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.52 , 97.78 , 108.73 , 70.70 2021-02-13T00:38:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , -0.11 2021-02-13T00:38:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176723.39 epoch sec; seqNo:14471; 4059685.48 north; 592898.65 east 2021-02-13T00:38:43Z- AHRS_M2 msg: 1613176723.74 epoch sec; seqNo:14357 2021-02-13T00:38:43Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.19 psi 2021-02-13T00:38:43Z- handleDepth()-Depth_Keller msg: 1613176723745 epoch sec; seqNo:14472; depth 17.25 2021-02-13T00:38:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176723.76 epoch sec; seqNo:14472; 4059685.86 north; 592898.65 east 2021-02-13T00:38:44Z- AHRS_M2 msg: 1613176724.15 epoch sec; seqNo:14358 2021-02-13T00:38:44Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.19 psi 2021-02-13T00:38:44Z- handleDepth()-Depth_Keller msg: 1613176724154 epoch sec; seqNo:14473; depth 17.13 2021-02-13T00:38:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176724.16 epoch sec; seqNo:14473; 4059686.23 north; 592898.64 east 2021-02-13T00:38:44Z- AHRS_M2 msg: 1613176724.55 epoch sec; seqNo:14359 2021-02-13T00:38:44Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-13T00:38:44Z- handleDepth()-Depth_Keller msg: 1613176724557 epoch sec; seqNo:14474; depth 17.00 2021-02-13T00:38:44Z- handleDvl() - RDI_Pathfinder msg: 1613176724.57 epoch sec; seqNo:6983 2021-02-13T00:38:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.06 , 97.85 , 110.01 , 71.93 2021-02-13T00:38:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.12 2021-02-13T00:38:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176724.59 epoch sec; seqNo:14474; 4059686.61 north; 592898.64 east 2021-02-13T00:38:44Z- AHRS_M2 msg: 1613176724.97 epoch sec; seqNo:14360 2021-02-13T00:38:44Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-13T00:38:44Z- handleDepth()-Depth_Keller msg: 1613176724977 epoch sec; seqNo:14475; depth 16.88 2021-02-13T00:38:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176724.99 epoch sec; seqNo:14475; 4059686.61 north; 592898.64 east 2021-02-13T00:38:45Z- AHRS_M2 msg: 1613176725.36 epoch sec; seqNo:14361 2021-02-13T00:38:45Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-13T00:38:45Z- handleDepth()-Depth_Keller msg: 1613176725370 epoch sec; seqNo:14476; depth 16.76 2021-02-13T00:38:45Z- handleDvl() - RDI_Pathfinder msg: 1613176725.38 epoch sec; seqNo:6984 2021-02-13T00:38:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 97.78 , 109.95 , 71.93 2021-02-13T00:38:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.09 , -0.10 2021-02-13T00:38:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176725.39 epoch sec; seqNo:14476; 4059686.99 north; 592898.63 east 2021-02-13T00:38:45Z- AHRS_M2 msg: 1613176725.76 epoch sec; seqNo:14362 2021-02-13T00:38:45Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-13T00:38:45Z- handleDepth()-Depth_Keller msg: 1613176725769 epoch sec; seqNo:14477; depth 16.64 2021-02-13T00:38:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176725.80 epoch sec; seqNo:14477; 4059687.37 north; 592898.63 east 2021-02-13T00:38:46Z- AHRS_M2 msg: 1613176726.17 epoch sec; seqNo:14363 2021-02-13T00:38:46Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-13T00:38:46Z- handleDepth()-Depth_Keller msg: 1613176726171 epoch sec; seqNo:14478; depth 16.53 2021-02-13T00:38:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176726.18 epoch sec; seqNo:14478; 4059687.38 north; 592899.85 east 2021-02-13T00:38:46Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-13T00:38:46Z- waiting for 1613176724.36, time is 1613176726.18 2021-02-13T00:38:46Z- LcmTrn::updateTrn() >>>> pose time 1613176726.18, meas time 1613176725.38 <<<< 2021-02-13T00:38:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176725.38 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:38:46Z- TerrainNav::motionUpdate() - currEstimate = (4059687, 592899.85, 16.53) 2021-02-13T00:38:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:38:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:38:46Z- TNavBF::Projecting Measurements 2021-02-13T00:38:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:38:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:38:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:38:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:38:46Z- TNavFilter::4 good beams. 2021-02-13T00:38:46Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:38:46Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:38:46Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:38:46Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:38:46Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:38:46Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:38:46Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:38:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:38:46Z- TerrainMapOctree: (vehN, vehE ) = (4059686.88, 592899.45). 2021-02-13T00:38:46Z- TerrainMapOctree: Min Distance = 2065.75. 2021-02-13T00:38:46Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:38:46Z- There is no specific good beam that all particles have in common. 2021-02-13T00:38:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:38:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.42 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 10.43 2021-02-13T00:38:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:38:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-13T00:38:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:38:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 19.34 2021-02-13T00:38:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:38:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 8.28 2021-02-13T00:38:46Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:38:46Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:38:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:38:46Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:38:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:38:46Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:38:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:38:46Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:38:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:38:46Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:38:46Z- Filter 0 No beams used in subcloud update 2021-02-13T00:38:46Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:38:46Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:38:46Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 693.7 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:38:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176725.38 sec. 2021-02-13T00:38:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:38:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:38:46Z- variances([0], [2], and psi_berg) = 693.632697 832.994708 0.000000 2021-02-13T00:38:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:38:46Z- LcmTrn::publishEstimates() - MLE : 4059686.88 592919.35 16.53 2021-02-13T00:38:46Z- LcmTrn::publishEstimates() - MMSE : 4059687.38 592899.85 16.53 2021-02-13T00:38:46Z- LcmTrn::publishEstimates() - COVAR: 693.63 832.99 1.04 0.00 2021-02-13T00:38:46Z- AHRS_M2 msg: 1613176726.57 epoch sec; seqNo:14364 2021-02-13T00:38:46Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-13T00:38:46Z- handleDepth()-Depth_Keller msg: 1613176726573 epoch sec; seqNo:14479; depth 16.42 2021-02-13T00:38:46Z- handleDvl() - RDI_Pathfinder msg: 1613176726.59 epoch sec; seqNo:6985 2021-02-13T00:38:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 96.56 , 106.30 , 78.31 2021-02-13T00:38:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.14 , -0.03 2021-02-13T00:38:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176726.60 epoch sec; seqNo:14479; 4059687.76 north; 592899.85 east 2021-02-13T00:38:46Z- AHRS_M2 msg: 1613176726.97 epoch sec; seqNo:14365 2021-02-13T00:38:46Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.18 psi 2021-02-13T00:38:46Z- handleDepth()-Depth_Keller msg: 1613176726986 epoch sec; seqNo:14480; depth 16.30 2021-02-13T00:38:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176727.00 epoch sec; seqNo:14480; 4059688.14 north; 592899.84 east 2021-02-13T00:38:47Z- AHRS_M2 msg: 1613176727.38 epoch sec; seqNo:14366 2021-02-13T00:38:47Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.18 psi 2021-02-13T00:38:47Z- handleDepth()-Depth_Keller msg: 1613176727385 epoch sec; seqNo:14481; depth 16.18 2021-02-13T00:38:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176727.42 epoch sec; seqNo:14481; 4059688.52 north; 592899.84 east 2021-02-13T00:38:47Z- AHRS_M2 msg: 1613176727.78 epoch sec; seqNo:14367 2021-02-13T00:38:47Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.18 psi 2021-02-13T00:38:47Z- handleDepth()-Depth_Keller msg: 1613176727784 epoch sec; seqNo:14482; depth 16.07 2021-02-13T00:38:47Z- handleDvl() - RDI_Pathfinder msg: 1613176727.80 epoch sec; seqNo:6986 2021-02-13T00:38:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.67 , 98.93 , 108.66 , 78.24 2021-02-13T00:38:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.15 , 0.07 2021-02-13T00:38:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176727.81 epoch sec; seqNo:14482; 4059688.90 north; 592899.83 east 2021-02-13T00:38:48Z- AHRS_M2 msg: 1613176728.19 epoch sec; seqNo:14368 2021-02-13T00:38:48Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.18 psi 2021-02-13T00:38:48Z- handleDepth()-Depth_Keller msg: 1613176728190 epoch sec; seqNo:14483; depth 15.94 2021-02-13T00:38:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176728.20 epoch sec; seqNo:14483; 4059688.90 north; 592899.83 east 2021-02-13T00:38:48Z- AHRS_M2 msg: 1613176728.59 epoch sec; seqNo:14369 2021-02-13T00:38:48Z- AHRS_M2 msg: 0.56 phi; 0.33 theta; -0.18 psi 2021-02-13T00:38:48Z- handleDepth()-Depth_Keller msg: 1613176728593 epoch sec; seqNo:14484; depth 15.84 2021-02-13T00:38:48Z- handleDvl() - RDI_Pathfinder msg: 1613176728.61 epoch sec; seqNo:6987 2021-02-13T00:38:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 97.64 , 108.60 , 78.17 2021-02-13T00:38:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , 0.13 2021-02-13T00:38:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176728.62 epoch sec; seqNo:14484; 4059689.28 north; 592899.83 east 2021-02-13T00:38:48Z- AHRS_M2 msg: 1613176728.99 epoch sec; seqNo:14370 2021-02-13T00:38:48Z- AHRS_M2 msg: 0.56 phi; 0.33 theta; -0.18 psi 2021-02-13T00:38:48Z- handleDepth()-Depth_Keller msg: 1613176728996 epoch sec; seqNo:14485; depth 15.69 2021-02-13T00:38:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176729.01 epoch sec; seqNo:14485; 4059689.66 north; 592899.83 east 2021-02-13T00:38:49Z- AHRS_M2 msg: 1613176729.41 epoch sec; seqNo:14371 2021-02-13T00:38:49Z- AHRS_M2 msg: 0.56 phi; 0.33 theta; -0.17 psi 2021-02-13T00:38:49Z- handleDepth()-Depth_Keller msg: 1613176729425 epoch sec; seqNo:14486; depth 15.59 2021-02-13T00:38:49Z- handleDvl() - RDI_Pathfinder msg: 1613176729.44 epoch sec; seqNo:6988 2021-02-13T00:38:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 97.64 , 109.81 , 78.17 2021-02-13T00:38:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.13 2021-02-13T00:38:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176729.45 epoch sec; seqNo:14486; 4059690.05 north; 592901.04 east 2021-02-13T00:38:49Z- AHRS_M2 msg: 1613176729.80 epoch sec; seqNo:14372 2021-02-13T00:38:49Z- AHRS_M2 msg: 0.56 phi; 0.33 theta; -0.17 psi 2021-02-13T00:38:49Z- handleDepth()-Depth_Keller msg: 1613176729806 epoch sec; seqNo:14487; depth 15.46 2021-02-13T00:38:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176729.82 epoch sec; seqNo:14487; 4059690.43 north; 592901.04 east 2021-02-13T00:38:50Z- AHRS_M2 msg: 1613176730.20 epoch sec; seqNo:14373 2021-02-13T00:38:50Z- AHRS_M2 msg: 0.56 phi; 0.33 theta; -0.16 psi 2021-02-13T00:38:50Z- handleDepth()-Depth_Keller msg: 1613176730208 epoch sec; seqNo:14488; depth 15.38 2021-02-13T00:38:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176730.22 epoch sec; seqNo:14488; 4059690.43 north; 592901.04 east 2021-02-13T00:38:50Z- AHRS_M2 msg: 1613176730.61 epoch sec; seqNo:14374 2021-02-13T00:38:50Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.16 psi 2021-02-13T00:38:50Z- handleDepth()-Depth_Keller msg: 1613176730612 epoch sec; seqNo:14489; depth 15.25 2021-02-13T00:38:50Z- handleDvl() - RDI_Pathfinder msg: 1613176730.62 epoch sec; seqNo:6989 2021-02-13T00:38:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.71 , 109.88 , 78.78 2021-02-13T00:38:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , 0.07 2021-02-13T00:38:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176730.66 epoch sec; seqNo:14489; 4059690.81 north; 592901.03 east 2021-02-13T00:38:51Z- AHRS_M2 msg: 1613176731.01 epoch sec; seqNo:14375 2021-02-13T00:38:51Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.16 psi 2021-02-13T00:38:51Z- handleDepth()-Depth_Keller msg: 1613176731015 epoch sec; seqNo:14490; depth 15.11 2021-02-13T00:38:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176731.03 epoch sec; seqNo:14490; 4059691.19 north; 592901.03 east 2021-02-13T00:38:51Z- AHRS_M2 msg: 1613176731.42 epoch sec; seqNo:14376 2021-02-13T00:38:51Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.15 psi 2021-02-13T00:38:51Z- handleDepth()-Depth_Keller msg: 1613176731424 epoch sec; seqNo:14491; depth 15.01 2021-02-13T00:38:51Z- handleDvl() - RDI_Pathfinder msg: 1613176731.44 epoch sec; seqNo:6990 2021-02-13T00:38:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.57 , 97.44 , 110.96 , 78.51 2021-02-13T00:38:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , 0.02 2021-02-13T00:38:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176731.45 epoch sec; seqNo:14491; 4059691.56 north; 592901.03 east 2021-02-13T00:38:51Z- LcmTrn::handleMessages() - handling 45 messages took 0.74 ms and 131 calls... 2021-02-13T00:38:51Z- waiting for 1613176729.18, time is 1613176731.45 2021-02-13T00:38:51Z- LcmTrn::updateTrn() equating pose 1613176731.45 and meas times 1613176731.45 2021-02-13T00:38:51Z- LcmTrn::updateTrn() >>>> pose time 1613176731.45, meas time 1613176731.45 <<<< 2021-02-13T00:38:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176731.45 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:38:51Z- TerrainNav::motionUpdate() - currEstimate = (4059692, 592901.03, 15.01) 2021-02-13T00:38:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:38:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:38:51Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176731.45 sec.; No relevant navigation data available 2021-02-13T00:38:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:38:51Z- TNavBF::mmse selected from filter -1 2021-02-13T00:38:51Z- variances([0], [2], and psi_berg) = 693.579986 832.994682 0.000000 2021-02-13T00:38:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:38:51Z- LcmTrn::publishEstimates() - MLE : 4059691.06 592920.53 15.01 2021-02-13T00:38:51Z- LcmTrn::publishEstimates() - MMSE : 4059691.56 592901.03 15.01 2021-02-13T00:38:51Z- LcmTrn::publishEstimates() - COVAR: 693.58 832.99 1.26 0.00 2021-02-13T00:38:51Z- AHRS_M2 msg: 1613176731.82 epoch sec; seqNo:14377 2021-02-13T00:38:51Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T00:38:51Z- handleDepth()-Depth_Keller msg: 1613176731824 epoch sec; seqNo:14492; depth 14.89 2021-02-13T00:38:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176731.84 epoch sec; seqNo:14492; 4059691.56 north; 592901.03 east 2021-02-13T00:38:52Z- AHRS_M2 msg: 1613176732.22 epoch sec; seqNo:14378 2021-02-13T00:38:52Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T00:38:52Z- handleDepth()-Depth_Keller msg: 1613176732229 epoch sec; seqNo:14493; depth 14.78 2021-02-13T00:38:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176732.24 epoch sec; seqNo:14493; 4059691.96 north; 592902.24 east 2021-02-13T00:38:52Z- AHRS_M2 msg: 1613176732.63 epoch sec; seqNo:14379 2021-02-13T00:38:52Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T00:38:52Z- handleDepth()-Depth_Keller msg: 1613176732633 epoch sec; seqNo:14494; depth 14.66 2021-02-13T00:38:52Z- handleDvl() - RDI_Pathfinder msg: 1613176732.64 epoch sec; seqNo:6991 2021-02-13T00:38:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 97.71 , 109.88 , 73.97 2021-02-13T00:38:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , 0.00 2021-02-13T00:38:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176732.66 epoch sec; seqNo:14494; 4059692.34 north; 592902.24 east 2021-02-13T00:38:53Z- AHRS_M2 msg: 1613176733.03 epoch sec; seqNo:14380 2021-02-13T00:38:53Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T00:38:53Z- handleDepth()-Depth_Keller msg: 1613176733046 epoch sec; seqNo:14495; depth 14.54 2021-02-13T00:38:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176733.06 epoch sec; seqNo:14495; 4059692.71 north; 592902.23 east 2021-02-13T00:38:53Z- AHRS_M2 msg: 1613176733.44 epoch sec; seqNo:14381 2021-02-13T00:38:53Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-13T00:38:53Z- handleDepth()-Depth_Keller msg: 1613176733444 epoch sec; seqNo:14496; depth 14.44 2021-02-13T00:38:53Z- handleDvl() - RDI_Pathfinder msg: 1613176733.45 epoch sec; seqNo:6992 2021-02-13T00:38:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 97.64 , 112.25 , 78.17 2021-02-13T00:38:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.01 2021-02-13T00:38:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176733.47 epoch sec; seqNo:14496; 4059692.71 north; 592902.23 east 2021-02-13T00:38:53Z- AHRS_M2 msg: 1613176733.84 epoch sec; seqNo:14382 2021-02-13T00:38:53Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-13T00:38:53Z- handleDepth()-Depth_Keller msg: 1613176733844 epoch sec; seqNo:14497; depth 14.31 2021-02-13T00:38:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176733.86 epoch sec; seqNo:14497; 4059693.09 north; 592902.23 east 2021-02-13T00:38:54Z- AHRS_M2 msg: 1613176734.25 epoch sec; seqNo:14383 2021-02-13T00:38:54Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-13T00:38:54Z- handleDepth()-Depth_Keller msg: 1613176734257 epoch sec; seqNo:14498; depth 14.20 2021-02-13T00:38:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176734.27 epoch sec; seqNo:14498; 4059693.47 north; 592902.23 east 2021-02-13T00:38:54Z- AHRS_M2 msg: 1613176734.65 epoch sec; seqNo:14384 2021-02-13T00:38:54Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.14 psi 2021-02-13T00:38:54Z- handleDepth()-Depth_Keller msg: 1613176734657 epoch sec; seqNo:14499; depth 14.09 2021-02-13T00:38:54Z- handleDvl() - RDI_Pathfinder msg: 1613176734.68 epoch sec; seqNo:6993 2021-02-13T00:38:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 97.64 , 111.16 , 78.71 2021-02-13T00:38:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , 0.02 2021-02-13T00:38:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176734.69 epoch sec; seqNo:14499; 4059693.86 north; 592903.44 east 2021-02-13T00:38:55Z- AHRS_M2 msg: 1613176735.05 epoch sec; seqNo:14385 2021-02-13T00:38:55Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.14 psi 2021-02-13T00:38:55Z- handleDepth()-Depth_Keller msg: 1613176735056 epoch sec; seqNo:14500; depth 13.97 2021-02-13T00:38:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176735.07 epoch sec; seqNo:14500; 4059693.86 north; 592903.44 east 2021-02-13T00:38:55Z- AHRS_M2 msg: 1613176735.46 epoch sec; seqNo:14386 2021-02-13T00:38:55Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.14 psi 2021-02-13T00:38:55Z- handleDepth()-Depth_Keller msg: 1613176735461 epoch sec; seqNo:14501; depth 13.86 2021-02-13T00:38:55Z- handleDvl() - RDI_Pathfinder msg: 1613176735.47 epoch sec; seqNo:6994 2021-02-13T00:38:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.64 , 111.16 , 73.97 2021-02-13T00:38:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.00 2021-02-13T00:38:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176735.49 epoch sec; seqNo:14501; 4059694.24 north; 592903.44 east 2021-02-13T00:38:55Z- AHRS_M2 msg: 1613176735.86 epoch sec; seqNo:14387 2021-02-13T00:38:55Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.14 psi 2021-02-13T00:38:55Z- handleDepth()-Depth_Keller msg: 1613176735879 epoch sec; seqNo:14502; depth 13.74 2021-02-13T00:38:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176735.89 epoch sec; seqNo:14502; 4059694.62 north; 592903.43 east 2021-02-13T00:38:56Z- AHRS_M2 msg: 1613176736.26 epoch sec; seqNo:14388 2021-02-13T00:38:56Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-13T00:38:56Z- handleDepth()-Depth_Keller msg: 1613176736273 epoch sec; seqNo:14503; depth 13.64 2021-02-13T00:38:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176736.29 epoch sec; seqNo:14503; 4059695.00 north; 592903.43 east 2021-02-13T00:38:56Z- LcmTrn::handleMessages() - handling 40 messages took 0.62 ms and 128 calls... 2021-02-13T00:38:56Z- waiting for 1613176734.45, time is 1613176736.29 2021-02-13T00:38:56Z- LcmTrn::updateTrn() >>>> pose time 1613176736.29, meas time 1613176735.47 <<<< 2021-02-13T00:38:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176735.47 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:38:56Z- TerrainNav::motionUpdate() - currEstimate = (4059695, 592903.43, 13.64) 2021-02-13T00:38:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:38:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:38:56Z- TNavBF::Projecting Measurements 2021-02-13T00:38:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:38:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:38:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:38:56Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:38:56Z- TNavFilter::3 good beams. 2021-02-13T00:38:56Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:38:56Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:38:56Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:38:56Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:38:56Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:38:56Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:38:56Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:38:56Z- TNavBF::mmse selected from filter -1 2021-02-13T00:38:56Z- TerrainMapOctree: (vehN, vehE ) = (4059694.42, 592903.02). 2021-02-13T00:38:56Z- TerrainMapOctree: Min Distance = 2057.71. 2021-02-13T00:38:56Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:38:56Z- There is no specific good beam that all particles have in common. 2021-02-13T00:38:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:38:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.44 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 15.25 2021-02-13T00:38:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:38:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 19.77 2021-02-13T00:38:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:38:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 8.76 2021-02-13T00:38:56Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:38:56Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:38:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:38:56Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:38:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:38:56Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:38:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:38:56Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:38:56Z- Filter 0 No beams used in subcloud update 2021-02-13T00:38:56Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:38:56Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:38:56Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 693.5 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:38:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176735.47 sec. 2021-02-13T00:38:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:38:56Z- TNavBF::mmse selected from filter -1 2021-02-13T00:38:56Z- variances([0], [2], and psi_berg) = 693.511508 832.994648 0.000000 2021-02-13T00:38:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:38:56Z- LcmTrn::publishEstimates() - MLE : 4059694.50 592922.93 13.64 2021-02-13T00:38:56Z- LcmTrn::publishEstimates() - MMSE : 4059695.00 592903.43 13.64 2021-02-13T00:38:56Z- LcmTrn::publishEstimates() - COVAR: 693.51 832.99 1.15 0.00 2021-02-13T00:38:56Z- AHRS_M2 msg: 1613176736.67 epoch sec; seqNo:14389 2021-02-13T00:38:56Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-13T00:38:56Z- handleDepth()-Depth_Keller msg: 1613176736678 epoch sec; seqNo:14504; depth 13.52 2021-02-13T00:38:56Z- handleDvl() - RDI_Pathfinder msg: 1613176736.69 epoch sec; seqNo:6995 2021-02-13T00:38:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 97.51 , 112.38 , 78.58 2021-02-13T00:38:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , -0.05 2021-02-13T00:38:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176736.71 epoch sec; seqNo:14504; 4059695.00 north; 592903.43 east 2021-02-13T00:38:57Z- AHRS_M2 msg: 1613176737.08 epoch sec; seqNo:14390 2021-02-13T00:38:57Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-13T00:38:57Z- handleDepth()-Depth_Keller msg: 1613176737083 epoch sec; seqNo:14505; depth 13.41 2021-02-13T00:38:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176737.10 epoch sec; seqNo:14505; 4059695.39 north; 592904.65 east 2021-02-13T00:38:57Z- AHRS_M2 msg: 1613176737.48 epoch sec; seqNo:14391 2021-02-13T00:38:57Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-13T00:38:57Z- handleDepth()-Depth_Keller msg: 1613176737488 epoch sec; seqNo:14506; depth 13.29 2021-02-13T00:38:57Z- handleDvl() - RDI_Pathfinder msg: 1613176737.50 epoch sec; seqNo:6996 2021-02-13T00:38:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.05 , 98.93 , 111.10 , 75.20 2021-02-13T00:38:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.10 2021-02-13T00:38:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176737.51 epoch sec; seqNo:14506; 4059695.77 north; 592904.64 east 2021-02-13T00:38:57Z- AHRS_M2 msg: 1613176737.88 epoch sec; seqNo:14392 2021-02-13T00:38:57Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-13T00:38:57Z- handleDepth()-Depth_Keller msg: 1613176737889 epoch sec; seqNo:14507; depth 13.16 2021-02-13T00:38:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176737.91 epoch sec; seqNo:14507; 4059696.15 north; 592904.64 east 2021-02-13T00:38:58Z- AHRS_M2 msg: 1613176738.28 epoch sec; seqNo:14393 2021-02-13T00:38:58Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-13T00:38:58Z- handleDepth()-Depth_Keller msg: 1613176738288 epoch sec; seqNo:14508; depth 13.06 2021-02-13T00:38:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176738.30 epoch sec; seqNo:14508; 4059696.15 north; 592904.64 east 2021-02-13T00:38:58Z- AHRS_M2 msg: 1613176738.69 epoch sec; seqNo:14394 2021-02-13T00:38:58Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-13T00:38:58Z- handleDepth()-Depth_Keller msg: 1613176738693 epoch sec; seqNo:14509; depth 12.96 2021-02-13T00:38:58Z- handleDvl() - RDI_Pathfinder msg: 1613176738.70 epoch sec; seqNo:6997 2021-02-13T00:38:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 99.06 , 111.23 , 78.78 2021-02-13T00:38:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.07 , -0.09 2021-02-13T00:38:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176738.71 epoch sec; seqNo:14509; 4059696.53 north; 592904.63 east 2021-02-13T00:38:59Z- AHRS_M2 msg: 1613176739.09 epoch sec; seqNo:14395 2021-02-13T00:38:59Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-13T00:38:59Z- handleDepth()-Depth_Keller msg: 1613176739098 epoch sec; seqNo:14510; depth 12.85 2021-02-13T00:38:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176739.11 epoch sec; seqNo:14510; 4059696.91 north; 592904.63 east 2021-02-13T00:38:59Z- AHRS_M2 msg: 1613176739.50 epoch sec; seqNo:14396 2021-02-13T00:38:59Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-13T00:38:59Z- handleDepth()-Depth_Keller msg: 1613176739500 epoch sec; seqNo:14511; depth 12.75 2021-02-13T00:38:59Z- handleDvl() - RDI_Pathfinder msg: 1613176739.52 epoch sec; seqNo:6998 2021-02-13T00:38:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 96.22 , 109.74 , 78.64 2021-02-13T00:38:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.09 , -0.03 2021-02-13T00:38:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176739.53 epoch sec; seqNo:14511; 4059697.29 north; 592904.63 east 2021-02-13T00:38:59Z- AHRS_M2 msg: 1613176739.90 epoch sec; seqNo:14397 2021-02-13T00:38:59Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-13T00:38:59Z- handleDepth()-Depth_Keller msg: 1613176739904 epoch sec; seqNo:14512; depth 12.63 2021-02-13T00:38:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176739.91 epoch sec; seqNo:14512; 4059697.30 north; 592905.85 east 2021-02-13T00:39:00Z- AHRS_M2 msg: 1613176740.32 epoch sec; seqNo:14398 2021-02-13T00:39:00Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-13T00:39:00Z- handleDepth()-Depth_Keller msg: 1613176740330 epoch sec; seqNo:14513; depth 12.53 2021-02-13T00:39:00Z- handleDvl() - RDI_Pathfinder msg: 1613176740.35 epoch sec; seqNo:6999 2021-02-13T00:39:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 99.06 , 112.58 , 78.78 2021-02-13T00:39:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , 0.02 2021-02-13T00:39:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176740.36 epoch sec; seqNo:14513; 4059697.68 north; 592905.84 east 2021-02-13T00:39:00Z- AHRS_M2 msg: 1613176740.71 epoch sec; seqNo:14399 2021-02-13T00:39:00Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-13T00:39:00Z- handleDepth()-Depth_Keller msg: 1613176740716 epoch sec; seqNo:14514; depth 12.40 2021-02-13T00:39:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176740.73 epoch sec; seqNo:14514; 4059698.06 north; 592905.84 east 2021-02-13T00:39:01Z- AHRS_M2 msg: 1613176741.11 epoch sec; seqNo:14400 2021-02-13T00:39:01Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-13T00:39:01Z- handleDepth()-Depth_Keller msg: 1613176741116 epoch sec; seqNo:14515; depth 12.30 2021-02-13T00:39:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176741.14 epoch sec; seqNo:14515; 4059698.44 north; 592905.83 east 2021-02-13T00:39:01Z- AHRS_M2 msg: 1613176741.52 epoch sec; seqNo:14401 2021-02-13T00:39:01Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-13T00:39:01Z- handleDepth()-Depth_Keller msg: 1613176741520 epoch sec; seqNo:14516; depth 12.18 2021-02-13T00:39:01Z- handleDvl() - RDI_Pathfinder msg: 1613176741.53 epoch sec; seqNo:7000 2021-02-13T00:39:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 100.01 , 110.83 , 78.38 2021-02-13T00:39:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , 0.06 2021-02-13T00:39:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176741.55 epoch sec; seqNo:14516; 4059698.44 north; 592905.83 east 2021-02-13T00:39:01Z- LcmTrn::handleMessages() - handling 45 messages took 0.58 ms and 134 calls... 2021-02-13T00:39:01Z- waiting for 1613176739.29, time is 1613176741.55 2021-02-13T00:39:01Z- LcmTrn::updateTrn() equating pose 1613176741.55 and meas times 1613176741.55 2021-02-13T00:39:01Z- LcmTrn::updateTrn() >>>> pose time 1613176741.55, meas time 1613176741.55 <<<< 2021-02-13T00:39:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176741.55 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:39:01Z- TerrainNav::motionUpdate() - currEstimate = (4059698, 592905.83, 12.18) 2021-02-13T00:39:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:39:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:39:01Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176741.55 sec.; No relevant navigation data available 2021-02-13T00:39:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:39:01Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:01Z- variances([0], [2], and psi_berg) = 693.459775 832.994623 0.000000 2021-02-13T00:39:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:39:01Z- LcmTrn::publishEstimates() - MLE : 4059697.94 592925.33 12.18 2021-02-13T00:39:01Z- LcmTrn::publishEstimates() - MMSE : 4059698.44 592905.83 12.18 2021-02-13T00:39:01Z- LcmTrn::publishEstimates() - COVAR: 693.46 832.99 1.37 0.00 2021-02-13T00:39:01Z- AHRS_M2 msg: 1613176741.92 epoch sec; seqNo:14402 2021-02-13T00:39:01Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-13T00:39:01Z- handleDepth()-Depth_Keller msg: 1613176741933 epoch sec; seqNo:14517; depth 12.07 2021-02-13T00:39:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176741.94 epoch sec; seqNo:14517; 4059698.82 north; 592905.83 east 2021-02-13T00:39:02Z- AHRS_M2 msg: 1613176742.32 epoch sec; seqNo:14403 2021-02-13T00:39:02Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-13T00:39:02Z- handleDepth()-Depth_Keller msg: 1613176742328 epoch sec; seqNo:14518; depth 11.95 2021-02-13T00:39:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176742.35 epoch sec; seqNo:14518; 4059699.19 north; 592905.83 east 2021-02-13T00:39:02Z- AHRS_M2 msg: 1613176742.73 epoch sec; seqNo:14404 2021-02-13T00:39:02Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:39:02Z- handleDepth()-Depth_Keller msg: 1613176742738 epoch sec; seqNo:14519; depth 11.84 2021-02-13T00:39:02Z- handleDvl() - RDI_Pathfinder msg: 1613176742.76 epoch sec; seqNo:7001 2021-02-13T00:39:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 98.93 , 115.15 , 78.64 2021-02-13T00:39:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.02 , 0.08 2021-02-13T00:39:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176742.77 epoch sec; seqNo:14519; 4059699.21 north; 592907.05 east 2021-02-13T00:39:03Z- AHRS_M2 msg: 1613176743.13 epoch sec; seqNo:14405 2021-02-13T00:39:03Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T00:39:03Z- handleDepth()-Depth_Keller msg: 1613176743137 epoch sec; seqNo:14520; depth 11.71 2021-02-13T00:39:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176743.15 epoch sec; seqNo:14520; 4059699.59 north; 592907.04 east 2021-02-13T00:39:03Z- AHRS_M2 msg: 1613176743.54 epoch sec; seqNo:14406 2021-02-13T00:39:03Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-13T00:39:03Z- handleDepth()-Depth_Keller msg: 1613176743542 epoch sec; seqNo:14521; depth 11.61 2021-02-13T00:39:03Z- handleDvl() - RDI_Pathfinder msg: 1613176743.55 epoch sec; seqNo:7002 2021-02-13T00:39:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.56 , 115.08 , 78.58 2021-02-13T00:39:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , 0.07 2021-02-13T00:39:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176743.56 epoch sec; seqNo:14521; 4059699.97 north; 592907.04 east 2021-02-13T00:39:03Z- AHRS_M2 msg: 1613176743.94 epoch sec; seqNo:14407 2021-02-13T00:39:03Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.19 psi 2021-02-13T00:39:03Z- handleDepth()-Depth_Keller msg: 1613176743944 epoch sec; seqNo:14522; depth 11.50 2021-02-13T00:39:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176743.96 epoch sec; seqNo:14522; 4059700.34 north; 592907.03 east 2021-02-13T00:39:04Z- AHRS_M2 msg: 1613176744.35 epoch sec; seqNo:14408 2021-02-13T00:39:04Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.19 psi 2021-02-13T00:39:04Z- handleDepth()-Depth_Keller msg: 1613176744355 epoch sec; seqNo:14523; depth 11.38 2021-02-13T00:39:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176744.39 epoch sec; seqNo:14523; 4059700.72 north; 592907.03 east 2021-02-13T00:39:04Z- AHRS_M2 msg: 1613176744.75 epoch sec; seqNo:14409 2021-02-13T00:39:04Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.19 psi 2021-02-13T00:39:04Z- handleDepth()-Depth_Keller msg: 1613176744752 epoch sec; seqNo:14524; depth 11.27 2021-02-13T00:39:04Z- handleDvl() - RDI_Pathfinder msg: 1613176744.77 epoch sec; seqNo:7003 2021-02-13T00:39:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 103.93 , 114.75 , 76.83 2021-02-13T00:39:04Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.04 , 0.01 2021-02-13T00:39:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176744.78 epoch sec; seqNo:14524; 4059700.72 north; 592907.03 east 2021-02-13T00:39:05Z- AHRS_M2 msg: 1613176745.16 epoch sec; seqNo:14410 2021-02-13T00:39:05Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.19 psi 2021-02-13T00:39:05Z- handleDepth()-Depth_Keller msg: 1613176745161 epoch sec; seqNo:14525; depth 11.17 2021-02-13T00:39:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176745.17 epoch sec; seqNo:14525; 4059701.12 north; 592908.25 east 2021-02-13T00:39:05Z- AHRS_M2 msg: 1613176745.56 epoch sec; seqNo:14411 2021-02-13T00:39:05Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-13T00:39:05Z- handleDepth()-Depth_Keller msg: 1613176745560 epoch sec; seqNo:14526; depth 11.07 2021-02-13T00:39:05Z- handleDvl() - RDI_Pathfinder msg: 1613176745.58 epoch sec; seqNo:7004 2021-02-13T00:39:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.27 , 104.20 , 119.07 , 77.24 2021-02-13T00:39:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.04 , -0.08 2021-02-13T00:39:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176745.59 epoch sec; seqNo:14526; 4059701.49 north; 592908.24 east 2021-02-13T00:39:05Z- AHRS_M2 msg: 1613176745.96 epoch sec; seqNo:14412 2021-02-13T00:39:05Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.18 psi 2021-02-13T00:39:05Z- handleDepth()-Depth_Keller msg: 1613176745969 epoch sec; seqNo:14527; depth 10.96 2021-02-13T00:39:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176745.98 epoch sec; seqNo:14527; 4059701.87 north; 592908.24 east 2021-02-13T00:39:06Z- AHRS_M2 msg: 1613176746.38 epoch sec; seqNo:14413 2021-02-13T00:39:06Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-13T00:39:06Z- handleDepth()-Depth_Keller msg: 1613176746390 epoch sec; seqNo:14528; depth 10.86 2021-02-13T00:39:06Z- handleDvl() - RDI_Pathfinder msg: 1613176746.41 epoch sec; seqNo:7005 2021-02-13T00:39:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 104.07 , 116.24 , 76.43 2021-02-13T00:39:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.11 2021-02-13T00:39:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176746.42 epoch sec; seqNo:14528; 4059701.87 north; 592908.24 east 2021-02-13T00:39:06Z- AHRS_M2 msg: 1613176746.77 epoch sec; seqNo:14414 2021-02-13T00:39:06Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-13T00:39:06Z- LcmTrn::handleMessages() - handling 42 messages took 0.75 ms and 127 calls... 2021-02-13T00:39:06Z- waiting for 1613176744.55, time is 1613176746.77 2021-02-13T00:39:06Z- LcmTrn::updateTrn() >>>> pose time 1613176746.77, meas time 1613176746.41 <<<< 2021-02-13T00:39:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176746.41 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:39:06Z- TerrainNav::motionUpdate() - currEstimate = (4059702, 592908.24, 10.86) 2021-02-13T00:39:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:39:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:39:06Z- TNavBF::Projecting Measurements 2021-02-13T00:39:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:39:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:39:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:39:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:39:06Z- TNavFilter::4 good beams. 2021-02-13T00:39:06Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:39:06Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:39:06Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:39:06Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:39:06Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:39:06Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:39:06Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:39:06Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:06Z- TerrainMapOctree: (vehN, vehE ) = (4059701.63, 592908.07). 2021-02-13T00:39:06Z- TerrainMapOctree: Min Distance = 2048.95. 2021-02-13T00:39:06Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:39:06Z- There is no specific good beam that all particles have in common. 2021-02-13T00:39:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:39:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.57 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 11.97 2021-02-13T00:39:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:39:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.41 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 17.33 2021-02-13T00:39:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:39:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 21.62 2021-02-13T00:39:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:39:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 9.35 2021-02-13T00:39:06Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:39:06Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:39:06Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:39:06Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:39:06Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:39:06Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:39:06Z- Filter 0 No beams used in subcloud update 2021-02-13T00:39:06Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:39:06Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:39:06Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 693.4 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:39:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176746.41 sec. 2021-02-13T00:39:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:39:06Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:06Z- variances([0], [2], and psi_berg) = 693.396286 832.994592 0.000000 2021-02-13T00:39:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:39:06Z- LcmTrn::publishEstimates() - MLE : 4059701.37 592927.74 10.86 2021-02-13T00:39:06Z- LcmTrn::publishEstimates() - MMSE : 4059701.87 592908.24 10.86 2021-02-13T00:39:06Z- LcmTrn::publishEstimates() - COVAR: 693.40 832.99 1.13 0.00 2021-02-13T00:39:06Z- handleDepth()-Depth_Keller msg: 1613176746774 epoch sec; seqNo:14529; depth 10.74 2021-02-13T00:39:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176746.79 epoch sec; seqNo:14529; 4059702.25 north; 592908.23 east 2021-02-13T00:39:07Z- AHRS_M2 msg: 1613176747.17 epoch sec; seqNo:14415 2021-02-13T00:39:07Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.17 psi 2021-02-13T00:39:07Z- handleDepth()-Depth_Keller msg: 1613176747176 epoch sec; seqNo:14530; depth 10.65 2021-02-13T00:39:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176747.19 epoch sec; seqNo:14530; 4059702.63 north; 592908.23 east 2021-02-13T00:39:07Z- AHRS_M2 msg: 1613176747.58 epoch sec; seqNo:14416 2021-02-13T00:39:07Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.17 psi 2021-02-13T00:39:07Z- handleDepth()-Depth_Keller msg: 1613176747584 epoch sec; seqNo:14531; depth 10.54 2021-02-13T00:39:07Z- handleDvl() - RDI_Pathfinder msg: 1613176747.60 epoch sec; seqNo:7006 2021-02-13T00:39:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.78 , 113.60 , 79.79 2021-02-13T00:39:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.12 , -0.04 2021-02-13T00:39:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176747.62 epoch sec; seqNo:14531; 4059702.63 north; 592908.23 east 2021-02-13T00:39:07Z- AHRS_M2 msg: 1613176747.98 epoch sec; seqNo:14417 2021-02-13T00:39:07Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-13T00:39:07Z- handleDepth()-Depth_Keller msg: 1613176747985 epoch sec; seqNo:14532; depth 10.43 2021-02-13T00:39:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176748.00 epoch sec; seqNo:14532; 4059703.02 north; 592909.45 east 2021-02-13T00:39:08Z- AHRS_M2 msg: 1613176748.38 epoch sec; seqNo:14418 2021-02-13T00:39:08Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-13T00:39:08Z- handleDepth()-Depth_Keller msg: 1613176748389 epoch sec; seqNo:14533; depth 10.29 2021-02-13T00:39:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176748.40 epoch sec; seqNo:14533; 4059703.40 north; 592909.44 east 2021-02-13T00:39:08Z- AHRS_M2 msg: 1613176748.79 epoch sec; seqNo:14419 2021-02-13T00:39:08Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.18 psi 2021-02-13T00:39:08Z- handleDepth()-Depth_Keller msg: 1613176748805 epoch sec; seqNo:14534; depth 10.19 2021-02-13T00:39:08Z- handleDvl() - RDI_Pathfinder msg: 1613176748.82 epoch sec; seqNo:7007 2021-02-13T00:39:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.35 , 102.58 , 113.39 , 79.59 2021-02-13T00:39:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.19 , 0.13 2021-02-13T00:39:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176748.83 epoch sec; seqNo:14534; 4059703.40 north; 592909.44 east 2021-02-13T00:39:09Z- AHRS_M2 msg: 1613176749.19 epoch sec; seqNo:14420 2021-02-13T00:39:09Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-13T00:39:09Z- handleDepth()-Depth_Keller msg: 1613176749201 epoch sec; seqNo:14535; depth 10.06 2021-02-13T00:39:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176749.22 epoch sec; seqNo:14535; 4059703.78 north; 592909.44 east 2021-02-13T00:39:09Z- AHRS_M2 msg: 1613176749.60 epoch sec; seqNo:14421 2021-02-13T00:39:09Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.17 psi 2021-02-13T00:39:09Z- handleDepth()-Depth_Keller msg: 1613176749603 epoch sec; seqNo:14536; depth 9.94 2021-02-13T00:39:09Z- handleDvl() - RDI_Pathfinder msg: 1613176749.62 epoch sec; seqNo:7008 2021-02-13T00:39:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.62 , 105.55 , 117.72 , 79.86 2021-02-13T00:39:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.01 , 0.28 2021-02-13T00:39:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176749.64 epoch sec; seqNo:14536; 4059704.16 north; 592909.43 east 2021-02-13T00:39:10Z- AHRS_M2 msg: 1613176750.00 epoch sec; seqNo:14422 2021-02-13T00:39:10Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.15 psi 2021-02-13T00:39:10Z- handleDepth()-Depth_Keller msg: 1613176750004 epoch sec; seqNo:14537; depth 9.84 2021-02-13T00:39:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176750.02 epoch sec; seqNo:14537; 4059704.54 north; 592909.43 east 2021-02-13T00:39:10Z- AHRS_M2 msg: 1613176750.41 epoch sec; seqNo:14423 2021-02-13T00:39:10Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.14 psi 2021-02-13T00:39:10Z- handleDepth()-Depth_Keller msg: 1613176750413 epoch sec; seqNo:14538; depth 9.73 2021-02-13T00:39:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176750.43 epoch sec; seqNo:14538; 4059704.92 north; 592909.43 east 2021-02-13T00:39:10Z- AHRS_M2 msg: 1613176750.81 epoch sec; seqNo:14424 2021-02-13T00:39:10Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.14 psi 2021-02-13T00:39:10Z- handleDepth()-Depth_Keller msg: 1613176750815 epoch sec; seqNo:14539; depth 9.61 2021-02-13T00:39:10Z- handleDvl() - RDI_Pathfinder msg: 1613176750.84 epoch sec; seqNo:7009 2021-02-13T00:39:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 104.00 , 117.52 , 79.66 2021-02-13T00:39:10Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.33 , 0.15 2021-02-13T00:39:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176750.86 epoch sec; seqNo:14539; 4059705.31 north; 592910.64 east 2021-02-13T00:39:11Z- AHRS_M2 msg: 1613176751.21 epoch sec; seqNo:14425 2021-02-13T00:39:11Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.14 psi 2021-02-13T00:39:11Z- handleDepth()-Depth_Keller msg: 1613176751221 epoch sec; seqNo:14540; depth 9.49 2021-02-13T00:39:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176751.23 epoch sec; seqNo:14540; 4059705.31 north; 592910.64 east 2021-02-13T00:39:11Z- AHRS_M2 msg: 1613176751.62 epoch sec; seqNo:14426 2021-02-13T00:39:11Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.15 psi 2021-02-13T00:39:11Z- handleDepth()-Depth_Keller msg: 1613176751625 epoch sec; seqNo:14541; depth 9.39 2021-02-13T00:39:11Z- handleDvl() - RDI_Pathfinder msg: 1613176751.64 epoch sec; seqNo:7010 2021-02-13T00:39:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 102.44 , 120.02 , 79.46 2021-02-13T00:39:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.35 , -0.26 2021-02-13T00:39:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176751.65 epoch sec; seqNo:14541; 4059705.69 north; 592910.64 east 2021-02-13T00:39:11Z- LcmTrn::handleMessages() - handling 43 messages took 0.59 ms and 131 calls... 2021-02-13T00:39:11Z- waiting for 1613176749.77, time is 1613176751.65 2021-02-13T00:39:11Z- LcmTrn::updateTrn() equating pose 1613176751.65 and meas times 1613176751.65 2021-02-13T00:39:11Z- LcmTrn::updateTrn() >>>> pose time 1613176751.65, meas time 1613176751.65 <<<< 2021-02-13T00:39:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176751.65 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:39:11Z- TerrainNav::motionUpdate() - currEstimate = (4059706, 592910.64, 9.39) 2021-02-13T00:39:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:39:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:39:11Z- TNavBF::Projecting Measurements 2021-02-13T00:39:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:39:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:39:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:39:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:39:11Z- TNavFilter::4 good beams. 2021-02-13T00:39:11Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:39:11Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:39:11Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:39:11Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:39:11Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:39:11Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:39:11Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:39:11Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:11Z- TerrainMapOctree: (vehN, vehE ) = (4059705.69, 592910.64). 2021-02-13T00:39:11Z- TerrainMapOctree: Min Distance = 2044.21. 2021-02-13T00:39:11Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:39:11Z- There is no specific good beam that all particles have in common. 2021-02-13T00:39:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:39:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.36 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 15.07 2021-02-13T00:39:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:39:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 16.79 2021-02-13T00:39:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:39:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 23.05 2021-02-13T00:39:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:39:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 10.10 2021-02-13T00:39:11Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:39:11Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:39:11Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:39:11Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:39:11Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:39:11Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:39:11Z- Filter 0 No beams used in subcloud update 2021-02-13T00:39:11Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:39:11Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:39:11Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 693.3 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:39:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176751.65 sec. 2021-02-13T00:39:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:39:11Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:11Z- variances([0], [2], and psi_berg) = 693.342720 832.994566 0.000000 2021-02-13T00:39:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:39:11Z- LcmTrn::publishEstimates() - MLE : 4059705.19 592931.14 9.39 2021-02-13T00:39:11Z- LcmTrn::publishEstimates() - MMSE : 4059705.69 592910.64 9.39 2021-02-13T00:39:11Z- LcmTrn::publishEstimates() - COVAR: 693.34 832.99 1.03 0.00 2021-02-13T00:39:12Z- AHRS_M2 msg: 1613176752.02 epoch sec; seqNo:14427 2021-02-13T00:39:12Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-13T00:39:12Z- handleDepth()-Depth_Keller msg: 1613176752026 epoch sec; seqNo:14542; depth 9.26 2021-02-13T00:39:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176752.04 epoch sec; seqNo:14542; 4059705.69 north; 592910.64 east 2021-02-13T00:39:12Z- AHRS_M2 msg: 1613176752.43 epoch sec; seqNo:14428 2021-02-13T00:39:12Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-13T00:39:12Z- handleDepth()-Depth_Keller msg: 1613176752457 epoch sec; seqNo:14543; depth 9.14 2021-02-13T00:39:12Z- handleDvl() - RDI_Pathfinder msg: 1613176752.47 epoch sec; seqNo:7011 2021-02-13T00:39:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 105.15 , 117.32 , 80.81 2021-02-13T00:39:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.04 , -0.47 2021-02-13T00:39:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176752.48 epoch sec; seqNo:14543; 4059706.08 north; 592911.86 east 2021-02-13T00:39:12Z- AHRS_M2 msg: 1613176752.83 epoch sec; seqNo:14429 2021-02-13T00:39:12Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-13T00:39:12Z- handleDepth()-Depth_Keller msg: 1613176752832 epoch sec; seqNo:14544; depth 9.00 2021-02-13T00:39:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176752.85 epoch sec; seqNo:14544; 4059706.08 north; 592911.86 east 2021-02-13T00:39:13Z- AHRS_M2 msg: 1613176753.23 epoch sec; seqNo:14430 2021-02-13T00:39:13Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.18 psi 2021-02-13T00:39:13Z- handleDepth()-Depth_Keller msg: 1613176753242 epoch sec; seqNo:14545; depth 8.90 2021-02-13T00:39:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176753.25 epoch sec; seqNo:14545; 4059706.46 north; 592911.85 east 2021-02-13T00:39:13Z- AHRS_M2 msg: 1613176753.64 epoch sec; seqNo:14431 2021-02-13T00:39:13Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-13T00:39:13Z- handleDepth()-Depth_Keller msg: 1613176753646 epoch sec; seqNo:14546; depth 8.77 2021-02-13T00:39:13Z- handleDvl() - RDI_Pathfinder msg: 1613176753.67 epoch sec; seqNo:7012 2021-02-13T00:39:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 106.50 , 120.02 , 80.81 2021-02-13T00:39:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.31 , -0.26 2021-02-13T00:39:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176753.68 epoch sec; seqNo:14546; 4059706.84 north; 592911.85 east 2021-02-13T00:39:14Z- AHRS_M2 msg: 1613176754.04 epoch sec; seqNo:14432 2021-02-13T00:39:14Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-13T00:39:14Z- handleDepth()-Depth_Keller msg: 1613176754053 epoch sec; seqNo:14547; depth 8.66 2021-02-13T00:39:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176754.06 epoch sec; seqNo:14547; 4059707.22 north; 592911.84 east 2021-02-13T00:39:14Z- AHRS_M2 msg: 1613176754.44 epoch sec; seqNo:14433 2021-02-13T00:39:14Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-13T00:39:14Z- handleDepth()-Depth_Keller msg: 1613176754454 epoch sec; seqNo:14548; depth 8.54 2021-02-13T00:39:14Z- handleDvl() - RDI_Pathfinder msg: 1613176754.47 epoch sec; seqNo:7013 2021-02-13T00:39:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.51 , 105.35 , 118.74 , 0.00 2021-02-13T00:39:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.38 , 0.16 2021-02-13T00:39:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176754.48 epoch sec; seqNo:14548; 4059707.22 north; 592911.84 east 2021-02-13T00:39:14Z- AHRS_M2 msg: 1613176754.85 epoch sec; seqNo:14434 2021-02-13T00:39:14Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-13T00:39:14Z- handleDepth()-Depth_Keller msg: 1613176754858 epoch sec; seqNo:14549; depth 8.40 2021-02-13T00:39:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176754.87 epoch sec; seqNo:14549; 4059707.60 north; 592911.84 east 2021-02-13T00:39:15Z- AHRS_M2 msg: 1613176755.25 epoch sec; seqNo:14435 2021-02-13T00:39:15Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-13T00:39:15Z- handleDepth()-Depth_Keller msg: 1613176755256 epoch sec; seqNo:14550; depth 8.29 2021-02-13T00:39:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176755.28 epoch sec; seqNo:14550; 4059707.97 north; 592911.83 east 2021-02-13T00:39:15Z- AHRS_M2 msg: 1613176755.66 epoch sec; seqNo:14436 2021-02-13T00:39:15Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.15 psi 2021-02-13T00:39:15Z- handleDepth()-Depth_Keller msg: 1613176755665 epoch sec; seqNo:14551; depth 8.15 2021-02-13T00:39:15Z- handleDvl() - RDI_Pathfinder msg: 1613176755.68 epoch sec; seqNo:7014 2021-02-13T00:39:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.86 , 103.73 , 118.60 , 78.05 2021-02-13T00:39:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.21 , 0.46 2021-02-13T00:39:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176755.70 epoch sec; seqNo:14551; 4059708.35 north; 592911.83 east 2021-02-13T00:39:16Z- AHRS_M2 msg: 1613176756.06 epoch sec; seqNo:14437 2021-02-13T00:39:16Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.15 psi 2021-02-13T00:39:16Z- handleDepth()-Depth_Keller msg: 1613176756064 epoch sec; seqNo:14552; depth 8.03 2021-02-13T00:39:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176756.08 epoch sec; seqNo:14552; 4059708.73 north; 592911.83 east 2021-02-13T00:39:16Z- AHRS_M2 msg: 1613176756.47 epoch sec; seqNo:14438 2021-02-13T00:39:16Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.15 psi 2021-02-13T00:39:16Z- handleDepth()-Depth_Keller msg: 1613176756476 epoch sec; seqNo:14553; depth 7.91 2021-02-13T00:39:16Z- handleDvl() - RDI_Pathfinder msg: 1613176756.49 epoch sec; seqNo:7015 2021-02-13T00:39:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.48 , 104.00 , 120.22 , 81.02 2021-02-13T00:39:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.16 , 0.44 2021-02-13T00:39:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176756.50 epoch sec; seqNo:14553; 4059709.12 north; 592913.04 east 2021-02-13T00:39:16Z- AHRS_M2 msg: 1613176756.87 epoch sec; seqNo:14439 2021-02-13T00:39:16Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.15 psi 2021-02-13T00:39:16Z- handleDepth()-Depth_Keller msg: 1613176756873 epoch sec; seqNo:14554; depth 7.79 2021-02-13T00:39:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176756.89 epoch sec; seqNo:14554; 4059709.50 north; 592913.04 east 2021-02-13T00:39:16Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-13T00:39:16Z- waiting for 1613176754.65, time is 1613176756.89 2021-02-13T00:39:16Z- LcmTrn::updateTrn() >>>> pose time 1613176756.89, meas time 1613176756.49 <<<< 2021-02-13T00:39:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176756.49 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:39:16Z- TerrainNav::motionUpdate() - currEstimate = (4059710, 592913.04, 7.79) 2021-02-13T00:39:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:39:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:39:16Z- TNavBF::Projecting Measurements 2021-02-13T00:39:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:39:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:39:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:39:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:39:16Z- TNavFilter::4 good beams. 2021-02-13T00:39:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:39:16Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:39:16Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:39:16Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:39:16Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:39:16Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:39:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:39:16Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:16Z- TerrainMapOctree: (vehN, vehE ) = (4059709.21, 592912.85). 2021-02-13T00:39:16Z- TerrainMapOctree: Min Distance = 2040.10. 2021-02-13T00:39:16Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:39:16Z- There is no specific good beam that all particles have in common. 2021-02-13T00:39:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:39:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 13.10 2021-02-13T00:39:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:39:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 17.31 2021-02-13T00:39:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:39:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 23.13 2021-02-13T00:39:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:39:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 10.50 2021-02-13T00:39:16Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:39:16Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:39:16Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:39:17Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:39:17Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:39:17Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:39:17Z- Filter 0 No beams used in subcloud update 2021-02-13T00:39:17Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:39:17Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:39:17Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 693.3 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:39:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176756.49 sec. 2021-02-13T00:39:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:39:17Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:17Z- variances([0], [2], and psi_berg) = 693.276438 832.994533 0.000000 2021-02-13T00:39:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:39:17Z- LcmTrn::publishEstimates() - MLE : 4059709.00 592933.54 7.79 2021-02-13T00:39:17Z- LcmTrn::publishEstimates() - MMSE : 4059709.50 592913.04 7.79 2021-02-13T00:39:17Z- LcmTrn::publishEstimates() - COVAR: 693.28 832.99 0.97 0.00 2021-02-13T00:39:17Z- AHRS_M2 msg: 1613176757.27 epoch sec; seqNo:14440 2021-02-13T00:39:17Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.15 psi 2021-02-13T00:39:17Z- handleDepth()-Depth_Keller msg: 1613176757281 epoch sec; seqNo:14555; depth 7.67 2021-02-13T00:39:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176757.32 epoch sec; seqNo:14555; 4059709.50 north; 592913.04 east 2021-02-13T00:39:17Z- AHRS_M2 msg: 1613176757.68 epoch sec; seqNo:14441 2021-02-13T00:39:17Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.16 psi 2021-02-13T00:39:17Z- handleDepth()-Depth_Keller msg: 1613176757680 epoch sec; seqNo:14556; depth 7.56 2021-02-13T00:39:17Z- handleDvl() - RDI_Pathfinder msg: 1613176757.69 epoch sec; seqNo:7016 2021-02-13T00:39:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 106.57 , 121.44 , 82.23 2021-02-13T00:39:17Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.43 , 0.09 2021-02-13T00:39:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176757.70 epoch sec; seqNo:14556; 4059709.88 north; 592913.03 east 2021-02-13T00:39:18Z- AHRS_M2 msg: 1613176758.08 epoch sec; seqNo:14442 2021-02-13T00:39:18Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.17 psi 2021-02-13T00:39:18Z- handleDepth()-Depth_Keller msg: 1613176758086 epoch sec; seqNo:14557; depth 7.45 2021-02-13T00:39:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176758.10 epoch sec; seqNo:14557; 4059710.27 north; 592914.25 east 2021-02-13T00:39:18Z- AHRS_M2 msg: 1613176758.48 epoch sec; seqNo:14443 2021-02-13T00:39:18Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.17 psi 2021-02-13T00:39:18Z- handleDepth()-Depth_Keller msg: 1613176758493 epoch sec; seqNo:14558; depth 7.32 2021-02-13T00:39:18Z- handleDvl() - RDI_Pathfinder msg: 1613176758.51 epoch sec; seqNo:7017 2021-02-13T00:39:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.98 , 108.39 , 123.27 , 81.62 2021-02-13T00:39:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.40 , -0.33 2021-02-13T00:39:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176758.53 epoch sec; seqNo:14558; 4059710.27 north; 592914.25 east 2021-02-13T00:39:18Z- AHRS_M2 msg: 1613176758.89 epoch sec; seqNo:14444 2021-02-13T00:39:18Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.18 psi 2021-02-13T00:39:18Z- handleDepth()-Depth_Keller msg: 1613176758897 epoch sec; seqNo:14559; depth 7.20 2021-02-13T00:39:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176758.91 epoch sec; seqNo:14559; 4059710.65 north; 592914.25 east 2021-02-13T00:39:19Z- AHRS_M2 msg: 1613176759.29 epoch sec; seqNo:14445 2021-02-13T00:39:19Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.20 psi 2021-02-13T00:39:19Z- handleDepth()-Depth_Keller msg: 1613176759300 epoch sec; seqNo:14560; depth 7.08 2021-02-13T00:39:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176759.31 epoch sec; seqNo:14560; 4059710.65 north; 592914.25 east 2021-02-13T00:39:19Z- AHRS_M2 msg: 1613176759.70 epoch sec; seqNo:14446 2021-02-13T00:39:19Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.22 psi 2021-02-13T00:39:19Z- handleDepth()-Depth_Keller msg: 1613176759705 epoch sec; seqNo:14561; depth 6.94 2021-02-13T00:39:19Z- handleDvl() - RDI_Pathfinder msg: 1613176759.72 epoch sec; seqNo:7018 2021-02-13T00:39:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 108.26 , 123.13 , 79.69 2021-02-13T00:39:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , -0.50 2021-02-13T00:39:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176759.73 epoch sec; seqNo:14561; 4059711.03 north; 592914.24 east 2021-02-13T00:39:20Z- AHRS_M2 msg: 1613176760.10 epoch sec; seqNo:14447 2021-02-13T00:39:20Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.22 psi 2021-02-13T00:39:20Z- handleDepth()-Depth_Keller msg: 1613176760104 epoch sec; seqNo:14562; depth 6.82 2021-02-13T00:39:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176760.12 epoch sec; seqNo:14562; 4059711.04 north; 592915.46 east 2021-02-13T00:39:20Z- AHRS_M2 msg: 1613176760.51 epoch sec; seqNo:14448 2021-02-13T00:39:20Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.22 psi 2021-02-13T00:39:20Z- handleDepth()-Depth_Keller msg: 1613176760510 epoch sec; seqNo:14563; depth 6.71 2021-02-13T00:39:20Z- handleDvl() - RDI_Pathfinder msg: 1613176760.52 epoch sec; seqNo:7019 2021-02-13T00:39:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 111.30 , 123.20 , 81.56 2021-02-13T00:39:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.44 , -0.16 2021-02-13T00:39:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176760.53 epoch sec; seqNo:14563; 4059711.42 north; 592915.46 east 2021-02-13T00:39:20Z- AHRS_M2 msg: 1613176760.91 epoch sec; seqNo:14449 2021-02-13T00:39:20Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.20 psi 2021-02-13T00:39:20Z- handleDepth()-Depth_Keller msg: 1613176760912 epoch sec; seqNo:14564; depth 6.57 2021-02-13T00:39:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176760.94 epoch sec; seqNo:14564; 4059711.80 north; 592915.46 east 2021-02-13T00:39:21Z- AHRS_M2 msg: 1613176761.31 epoch sec; seqNo:14450 2021-02-13T00:39:21Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-13T00:39:21Z- handleDepth()-Depth_Keller msg: 1613176761321 epoch sec; seqNo:14565; depth 6.46 2021-02-13T00:39:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176761.33 epoch sec; seqNo:14565; 4059712.18 north; 592915.45 east 2021-02-13T00:39:21Z- AHRS_M2 msg: 1613176761.72 epoch sec; seqNo:14451 2021-02-13T00:39:21Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.14 psi 2021-02-13T00:39:21Z- handleDepth()-Depth_Keller msg: 1613176761720 epoch sec; seqNo:14566; depth 6.36 2021-02-13T00:39:21Z- handleDvl() - RDI_Pathfinder msg: 1613176761.73 epoch sec; seqNo:7020 2021-02-13T00:39:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.55 , 109.88 , 121.78 , 83.11 2021-02-13T00:39:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.30 , 0.37 2021-02-13T00:39:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176761.74 epoch sec; seqNo:14566; 4059712.56 north; 592915.45 east 2021-02-13T00:39:22Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-13T00:39:22Z- waiting for 1613176759.89, time is 1613176761.74 2021-02-13T00:39:22Z- LcmTrn::updateTrn() equating pose 1613176761.74 and meas times 1613176761.74 2021-02-13T00:39:22Z- LcmTrn::updateTrn() >>>> pose time 1613176761.74, meas time 1613176761.74 <<<< 2021-02-13T00:39:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176761.74 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:39:22Z- TerrainNav::motionUpdate() - currEstimate = (4059713, 592915.45, 6.36) 2021-02-13T00:39:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:39:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:39:22Z- TNavBF::Projecting Measurements 2021-02-13T00:39:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:39:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:39:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:39:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:39:22Z- TNavFilter::4 good beams. 2021-02-13T00:39:22Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:39:22Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:39:22Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:39:22Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:39:22Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:39:22Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:39:22Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:39:22Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:22Z- TerrainMapOctree: (vehN, vehE ) = (4059712.56, 592915.45). 2021-02-13T00:39:22Z- TerrainMapOctree: Min Distance = 2035.87. 2021-02-13T00:39:22Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:39:22Z- There is no specific good beam that all particles have in common. 2021-02-13T00:39:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:39:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 13.12 2021-02-13T00:39:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:39:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 19.32 2021-02-13T00:39:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:39:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 23.73 2021-02-13T00:39:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:39:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 11.05 2021-02-13T00:39:22Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:39:22Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:39:22Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:39:22Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:39:22Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:39:22Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:39:22Z- Filter 0 No beams used in subcloud update 2021-02-13T00:39:22Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:39:22Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:39:22Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 693.2 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:39:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176761.74 sec. 2021-02-13T00:39:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:39:22Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:22Z- variances([0], [2], and psi_berg) = 693.226725 832.994509 0.000000 2021-02-13T00:39:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:39:22Z- LcmTrn::publishEstimates() - MLE : 4059712.06 592935.95 6.36 2021-02-13T00:39:22Z- LcmTrn::publishEstimates() - MMSE : 4059712.56 592915.45 6.36 2021-02-13T00:39:22Z- LcmTrn::publishEstimates() - COVAR: 693.23 832.99 0.92 0.00 2021-02-13T00:39:22Z- AHRS_M2 msg: 1613176762.12 epoch sec; seqNo:14452 2021-02-13T00:39:22Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.11 psi 2021-02-13T00:39:22Z- handleDepth()-Depth_Keller msg: 1613176762128 epoch sec; seqNo:14567; depth 6.24 2021-02-13T00:39:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176762.14 epoch sec; seqNo:14567; 4059712.94 north; 592915.44 east 2021-02-13T00:39:22Z- AHRS_M2 msg: 1613176762.52 epoch sec; seqNo:14453 2021-02-13T00:39:22Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.10 psi 2021-02-13T00:39:22Z- handleDepth()-Depth_Keller msg: 1613176762533 epoch sec; seqNo:14568; depth 6.12 2021-02-13T00:39:22Z- handleDvl() - RDI_Pathfinder msg: 1613176762.55 epoch sec; seqNo:7021 2021-02-13T00:39:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.98 , 103.93 , 120.29 , 83.11 2021-02-13T00:39:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.10 , 0.48 2021-02-13T00:39:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176762.56 epoch sec; seqNo:14568; 4059713.32 north; 592915.44 east 2021-02-13T00:39:22Z- AHRS_M2 msg: 1613176762.93 epoch sec; seqNo:14454 2021-02-13T00:39:22Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.09 psi 2021-02-13T00:39:22Z- handleDepth()-Depth_Keller msg: 1613176762932 epoch sec; seqNo:14569; depth 6.00 2021-02-13T00:39:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176762.94 epoch sec; seqNo:14569; 4059713.32 north; 592915.44 east 2021-02-13T00:39:23Z- AHRS_M2 msg: 1613176763.33 epoch sec; seqNo:14455 2021-02-13T00:39:23Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.11 psi 2021-02-13T00:39:23Z- handleDepth()-Depth_Keller msg: 1613176763341 epoch sec; seqNo:14570; depth 5.88 2021-02-13T00:39:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176763.35 epoch sec; seqNo:14570; 4059713.70 north; 592915.43 east 2021-02-13T00:39:23Z- AHRS_M2 msg: 1613176763.74 epoch sec; seqNo:14456 2021-02-13T00:39:23Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.13 psi 2021-02-13T00:39:23Z- handleDepth()-Depth_Keller msg: 1613176763742 epoch sec; seqNo:14571; depth 5.76 2021-02-13T00:39:23Z- handleDvl() - RDI_Pathfinder msg: 1613176763.75 epoch sec; seqNo:7022 2021-02-13T00:39:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.98 , 99.47 , 118.80 , 83.11 2021-02-13T00:39:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.33 , 0.17 2021-02-13T00:39:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176763.76 epoch sec; seqNo:14571; 4059714.09 north; 592916.65 east 2021-02-13T00:39:24Z- AHRS_M2 msg: 1613176764.14 epoch sec; seqNo:14457 2021-02-13T00:39:24Z- AHRS_M2 msg: 0.58 phi; 0.30 theta; -0.16 psi 2021-02-13T00:39:24Z- handleDepth()-Depth_Keller msg: 1613176764151 epoch sec; seqNo:14572; depth 5.63 2021-02-13T00:39:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176764.17 epoch sec; seqNo:14572; 4059714.09 north; 592916.65 east 2021-02-13T00:39:24Z- AHRS_M2 msg: 1613176764.54 epoch sec; seqNo:14458 2021-02-13T00:39:24Z- AHRS_M2 msg: 0.59 phi; 0.29 theta; -0.18 psi 2021-02-13T00:39:24Z- handleDepth()-Depth_Keller msg: 1613176764553 epoch sec; seqNo:14573; depth 5.51 2021-02-13T00:39:24Z- handleDvl() - RDI_Pathfinder msg: 1613176764.56 epoch sec; seqNo:7023 2021-02-13T00:39:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 103.80 , 115.96 , 84.87 2021-02-13T00:39:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.31 , -0.21 2021-02-13T00:39:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176764.59 epoch sec; seqNo:14573; 4059714.47 north; 592916.65 east 2021-02-13T00:39:24Z- AHRS_M2 msg: 1613176764.95 epoch sec; seqNo:14459 2021-02-13T00:39:24Z- AHRS_M2 msg: 0.60 phi; 0.26 theta; -0.20 psi 2021-02-13T00:39:24Z- handleDepth()-Depth_Keller msg: 1613176764956 epoch sec; seqNo:14574; depth 5.39 2021-02-13T00:39:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176764.97 epoch sec; seqNo:14574; 4059714.85 north; 592916.64 east 2021-02-13T00:39:25Z- AHRS_M2 msg: 1613176765.35 epoch sec; seqNo:14460 2021-02-13T00:39:25Z- AHRS_M2 msg: 0.62 phi; 0.23 theta; -0.21 psi 2021-02-13T00:39:25Z- handleDepth()-Depth_Keller msg: 1613176765358 epoch sec; seqNo:14575; depth 5.26 2021-02-13T00:39:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176765.37 epoch sec; seqNo:14575; 4059714.86 north; 592917.86 east 2021-02-13T00:39:25Z- AHRS_M2 msg: 1613176765.76 epoch sec; seqNo:14461 2021-02-13T00:39:25Z- AHRS_M2 msg: 0.65 phi; 0.18 theta; -0.20 psi 2021-02-13T00:39:25Z- handleDepth()-Depth_Keller msg: 1613176765760 epoch sec; seqNo:14576; depth 5.18 2021-02-13T00:39:25Z- handleDvl() - RDI_Pathfinder msg: 1613176765.78 epoch sec; seqNo:7024 2021-02-13T00:39:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.21 , 109.14 , 115.90 , 86.15 2021-02-13T00:39:25Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.12 , -0.39 2021-02-13T00:39:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176765.79 epoch sec; seqNo:14576; 4059715.24 north; 592917.86 east 2021-02-13T00:39:26Z- AHRS_M2 msg: 1613176766.16 epoch sec; seqNo:14462 2021-02-13T00:39:26Z- AHRS_M2 msg: 0.68 phi; 0.13 theta; -0.19 psi 2021-02-13T00:39:26Z- handleDepth()-Depth_Keller msg: 1613176766165 epoch sec; seqNo:14577; depth 5.08 2021-02-13T00:39:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176766.18 epoch sec; seqNo:14577; 4059715.62 north; 592917.86 east 2021-02-13T00:39:26Z- AHRS_M2 msg: 1613176766.56 epoch sec; seqNo:14463 2021-02-13T00:39:26Z- AHRS_M2 msg: 0.70 phi; 0.07 theta; -0.18 psi 2021-02-13T00:39:26Z- handleDepth()-Depth_Keller msg: 1613176766568 epoch sec; seqNo:14578; depth 5.01 2021-02-13T00:39:26Z- handleDvl() - RDI_Pathfinder msg: 1613176766.58 epoch sec; seqNo:7025 2021-02-13T00:39:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.51 , 106.43 , 105.08 , 87.51 2021-02-13T00:39:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.09 , -0.34 2021-02-13T00:39:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176766.59 epoch sec; seqNo:14578; 4059716.00 north; 592917.85 east 2021-02-13T00:39:26Z- AHRS_M2 msg: 1613176766.97 epoch sec; seqNo:14464 2021-02-13T00:39:26Z- AHRS_M2 msg: 0.71 phi; 0.02 theta; -0.16 psi 2021-02-13T00:39:26Z- handleDepth()-Depth_Keller msg: 1613176766977 epoch sec; seqNo:14579; depth 4.91 2021-02-13T00:39:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176766.99 epoch sec; seqNo:14579; 4059716.00 north; 592917.85 east 2021-02-13T00:39:27Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:39:27Z- waiting for 1613176764.74, time is 1613176766.99 2021-02-13T00:39:27Z- LcmTrn::updateTrn() >>>> pose time 1613176766.99, meas time 1613176766.58 <<<< 2021-02-13T00:39:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176766.58 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:39:27Z- TerrainNav::motionUpdate() - currEstimate = (4059716, 592917.85, 4.91) 2021-02-13T00:39:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:39:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:39:27Z- TNavBF::Projecting Measurements 2021-02-13T00:39:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:39:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:39:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:39:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:39:27Z- TNavFilter::4 good beams. 2021-02-13T00:39:27Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:39:27Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:39:27Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:39:27Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:39:27Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:39:27Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:39:27Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:39:27Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:27Z- TerrainMapOctree: (vehN, vehE ) = (4059715.73, 592917.66). 2021-02-13T00:39:27Z- TerrainMapOctree: Min Distance = 2032.02. 2021-02-13T00:39:27Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:39:27Z- There is no specific good beam that all particles have in common. 2021-02-13T00:39:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:39:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 12.25 2021-02-13T00:39:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:39:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 18.13 2021-02-13T00:39:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:39:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 17.67 2021-02-13T00:39:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:39:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 12.25 2021-02-13T00:39:27Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:39:27Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:39:27Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:39:27Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:39:27Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:39:27Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:39:27Z- Filter 0 No beams used in subcloud update 2021-02-13T00:39:27Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:39:27Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:39:27Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 693.2 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:39:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176766.58 sec. 2021-02-13T00:39:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:39:27Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:27Z- variances([0], [2], and psi_berg) = 693.162830 832.994478 0.000000 2021-02-13T00:39:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:39:27Z- LcmTrn::publishEstimates() - MLE : 4059715.50 592938.35 4.91 2021-02-13T00:39:27Z- LcmTrn::publishEstimates() - MMSE : 4059716.00 592917.85 4.91 2021-02-13T00:39:27Z- LcmTrn::publishEstimates() - COVAR: 693.16 832.99 0.89 0.00 2021-02-13T00:39:27Z- AHRS_M2 msg: 1613176767.37 epoch sec; seqNo:14465 2021-02-13T00:39:27Z- AHRS_M2 msg: 0.72 phi; -0.03 theta; -0.15 psi 2021-02-13T00:39:27Z- handleDepth()-Depth_Keller msg: 1613176767376 epoch sec; seqNo:14580; depth 4.85 2021-02-13T00:39:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176767.40 epoch sec; seqNo:14580; 4059716.37 north; 592917.85 east 2021-02-13T00:39:27Z- AHRS_M2 msg: 1613176767.78 epoch sec; seqNo:14466 2021-02-13T00:39:27Z- AHRS_M2 msg: 0.72 phi; -0.08 theta; -0.14 psi 2021-02-13T00:39:27Z- handleDepth()-Depth_Keller msg: 1613176767780 epoch sec; seqNo:14581; depth 4.80 2021-02-13T00:39:27Z- handleDvl() - RDI_Pathfinder msg: 1613176767.79 epoch sec; seqNo:7026 2021-02-13T00:39:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 104.07 , 97.31 , 97.31 2021-02-13T00:39:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.19 , -0.06 2021-02-13T00:39:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176767.82 epoch sec; seqNo:14581; 4059716.77 north; 592919.06 east 2021-02-13T00:39:28Z- AHRS_M2 msg: 1613176768.18 epoch sec; seqNo:14467 2021-02-13T00:39:28Z- AHRS_M2 msg: 0.72 phi; -0.12 theta; -0.15 psi 2021-02-13T00:39:28Z- handleDepth()-Depth_Keller msg: 1613176768184 epoch sec; seqNo:14582; depth 4.76 2021-02-13T00:39:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176768.20 epoch sec; seqNo:14582; 4059716.77 north; 592919.06 east 2021-02-13T00:39:28Z- AHRS_M2 msg: 1613176768.58 epoch sec; seqNo:14468 2021-02-13T00:39:28Z- AHRS_M2 msg: 0.71 phi; -0.15 theta; -0.16 psi 2021-02-13T00:39:28Z- handleDepth()-Depth_Keller msg: 1613176768590 epoch sec; seqNo:14583; depth 4.75 2021-02-13T00:39:28Z- handleDvl() - RDI_Pathfinder msg: 1613176768.64 epoch sec; seqNo:7027 2021-02-13T00:39:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.91 , 101.43 , 89.26 , 100.08 2021-02-13T00:39:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.61 , -0.11 , 0.17 2021-02-13T00:39:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176768.66 epoch sec; seqNo:14583; 4059717.15 north; 592919.06 east 2021-02-13T00:39:29Z- handleDepth()-Depth_Keller msg: 1613176769010 epoch sec; seqNo:14584; depth 4.73 2021-02-13T00:39:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176769.02 epoch sec; seqNo:14584; 4059717.52 north; 592919.06 east 2021-02-13T00:39:29Z- AHRS_M2 msg: 1613176769.40 epoch sec; seqNo:14469 2021-02-13T00:39:29Z- AHRS_M2 msg: 0.70 phi; -0.20 theta; -0.19 psi 2021-02-13T00:39:29Z- handleDepth()-Depth_Keller msg: 1613176769410 epoch sec; seqNo:14585; depth 4.73 2021-02-13T00:39:29Z- handleDvl() - RDI_Pathfinder msg: 1613176769.43 epoch sec; seqNo:7028 2021-02-13T00:39:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.51 , 105.62 , 86.69 , 109.68 2021-02-13T00:39:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , 0.03 , 0.22 2021-02-13T00:39:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176769.44 epoch sec; seqNo:14585; 4059717.52 north; 592919.06 east 2021-02-13T00:39:29Z- AHRS_M2 msg: 1613176769.80 epoch sec; seqNo:14470 2021-02-13T00:39:29Z- AHRS_M2 msg: 0.70 phi; -0.23 theta; -0.21 psi 2021-02-13T00:39:29Z- handleDepth()-Depth_Keller msg: 1613176769807 epoch sec; seqNo:14586; depth 4.77 2021-02-13T00:39:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176769.82 epoch sec; seqNo:14586; 4059717.52 north; 592919.06 east 2021-02-13T00:39:30Z- AHRS_M2 msg: 1613176770.20 epoch sec; seqNo:14471 2021-02-13T00:39:30Z- AHRS_M2 msg: 0.69 phi; -0.24 theta; -0.22 psi 2021-02-13T00:39:30Z- handleDepth()-Depth_Keller msg: 1613176770209 epoch sec; seqNo:14587; depth 4.78 2021-02-13T00:39:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176770.23 epoch sec; seqNo:14587; 4059717.90 north; 592919.05 east 2021-02-13T00:39:30Z- AHRS_M2 msg: 1613176770.60 epoch sec; seqNo:14472 2021-02-13T00:39:30Z- AHRS_M2 msg: 0.69 phi; -0.26 theta; -0.23 psi 2021-02-13T00:39:30Z- handleDepth()-Depth_Keller msg: 1613176770613 epoch sec; seqNo:14588; depth 4.82 2021-02-13T00:39:30Z- handleDvl() - RDI_Pathfinder msg: 1613176770.62 epoch sec; seqNo:7029 2021-02-13T00:39:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 109.21 , 86.22 , 115.96 2021-02-13T00:39:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.09 , 0.13 2021-02-13T00:39:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176770.66 epoch sec; seqNo:14588; 4059717.92 north; 592920.27 east 2021-02-13T00:39:31Z- AHRS_M2 msg: 1613176771.02 epoch sec; seqNo:14473 2021-02-13T00:39:31Z- AHRS_M2 msg: 0.67 phi; -0.28 theta; -0.24 psi 2021-02-13T00:39:31Z- handleDepth()-Depth_Keller msg: 1613176771019 epoch sec; seqNo:14589; depth 4.89 2021-02-13T00:39:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176771.03 epoch sec; seqNo:14589; 4059718.30 north; 592920.27 east 2021-02-13T00:39:31Z- AHRS_M2 msg: 1613176771.43 epoch sec; seqNo:14474 2021-02-13T00:39:31Z- AHRS_M2 msg: 0.67 phi; -0.28 theta; -0.24 psi 2021-02-13T00:39:31Z- handleDepth()-Depth_Keller msg: 1613176771439 epoch sec; seqNo:14590; depth 4.95 2021-02-13T00:39:31Z- handleDvl() - RDI_Pathfinder msg: 1613176771.45 epoch sec; seqNo:7030 2021-02-13T00:39:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 112.79 , 86.02 , 117.25 2021-02-13T00:39:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.07 , 0.01 2021-02-13T00:39:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176771.48 epoch sec; seqNo:14590; 4059718.30 north; 592920.27 east 2021-02-13T00:39:31Z- AHRS_M2 msg: 1613176771.82 epoch sec; seqNo:14475 2021-02-13T00:39:31Z- AHRS_M2 msg: 0.66 phi; -0.29 theta; -0.26 psi 2021-02-13T00:39:31Z- handleDepth()-Depth_Keller msg: 1613176771820 epoch sec; seqNo:14591; depth 5.01 2021-02-13T00:39:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176771.84 epoch sec; seqNo:14591; 4059718.67 north; 592920.26 east 2021-02-13T00:39:32Z- AHRS_M2 msg: 1613176772.22 epoch sec; seqNo:14476 2021-02-13T00:39:32Z- AHRS_M2 msg: 0.66 phi; -0.29 theta; -0.28 psi 2021-02-13T00:39:32Z- LcmTrn::handleMessages() - handling 41 messages took 0.65 ms and 128 calls... 2021-02-13T00:39:32Z- waiting for 1613176769.99, time is 1613176772.22 2021-02-13T00:39:32Z- LcmTrn::updateTrn() >>>> pose time 1613176772.22, meas time 1613176771.45 <<<< 2021-02-13T00:39:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176771.45 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:39:32Z- TerrainNav::motionUpdate() - currEstimate = (4059719, 592920.26, 5.01) 2021-02-13T00:39:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:39:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:39:32Z- TNavBF::Projecting Measurements 2021-02-13T00:39:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:39:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:39:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:39:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:39:32Z- TNavFilter::4 good beams. 2021-02-13T00:39:32Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:39:32Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:39:32Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:39:32Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:39:32Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:39:32Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:39:32Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:39:32Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:32Z- TerrainMapOctree: (vehN, vehE ) = (4059718.28, 592919.91). 2021-02-13T00:39:32Z- TerrainMapOctree: Min Distance = 2028.62. 2021-02-13T00:39:32Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:39:32Z- There is no specific good beam that all particles have in common. 2021-02-13T00:39:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:39:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:39:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:39:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 20.35 2021-02-13T00:39:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:39:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 11.84 2021-02-13T00:39:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:39:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 22.00 2021-02-13T00:39:32Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:39:32Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:39:32Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:39:32Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:39:32Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:39:32Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:39:32Z- Filter 0 No beams used in subcloud update 2021-02-13T00:39:32Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:39:32Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:39:32Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 693.1 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:39:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176771.45 sec. 2021-02-13T00:39:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:39:32Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:32Z- variances([0], [2], and psi_berg) = 693.100378 832.994447 0.000000 2021-02-13T00:39:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:39:32Z- LcmTrn::publishEstimates() - MLE : 4059718.17 592940.76 5.01 2021-02-13T00:39:32Z- LcmTrn::publishEstimates() - MMSE : 4059718.67 592920.26 5.01 2021-02-13T00:39:32Z- LcmTrn::publishEstimates() - COVAR: 693.10 832.99 0.88 0.00 2021-02-13T00:39:32Z- handleDepth()-Depth_Keller msg: 1613176772225 epoch sec; seqNo:14592; depth 5.08 2021-02-13T00:39:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176772.23 epoch sec; seqNo:14592; 4059719.05 north; 592920.26 east 2021-02-13T00:39:32Z- AHRS_M2 msg: 1613176772.62 epoch sec; seqNo:14477 2021-02-13T00:39:32Z- AHRS_M2 msg: 0.66 phi; -0.29 theta; -0.29 psi 2021-02-13T00:39:32Z- handleDepth()-Depth_Keller msg: 1613176772628 epoch sec; seqNo:14593; depth 5.16 2021-02-13T00:39:32Z- handleDvl() - RDI_Pathfinder msg: 1613176772.64 epoch sec; seqNo:7031 2021-02-13T00:39:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.37 , 115.63 , 0.00 , 121.58 2021-02-13T00:39:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.04 , 0.00 2021-02-13T00:39:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176772.65 epoch sec; seqNo:14593; 4059719.43 north; 592920.26 east 2021-02-13T00:39:33Z- AHRS_M2 msg: 1613176773.03 epoch sec; seqNo:14478 2021-02-13T00:39:33Z- AHRS_M2 msg: 0.67 phi; -0.30 theta; -0.29 psi 2021-02-13T00:39:33Z- handleDepth()-Depth_Keller msg: 1613176773033 epoch sec; seqNo:14594; depth 5.22 2021-02-13T00:39:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176773.04 epoch sec; seqNo:14594; 4059719.45 north; 592921.48 east 2021-02-13T00:39:33Z- AHRS_M2 msg: 1613176773.43 epoch sec; seqNo:14479 2021-02-13T00:39:33Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.26 psi 2021-02-13T00:39:33Z- handleDepth()-Depth_Keller msg: 1613176773441 epoch sec; seqNo:14595; depth 5.33 2021-02-13T00:39:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176773.46 epoch sec; seqNo:14595; 4059719.82 north; 592921.47 east 2021-02-13T00:39:33Z- AHRS_M2 msg: 1613176773.84 epoch sec; seqNo:14480 2021-02-13T00:39:33Z- AHRS_M2 msg: 0.66 phi; -0.33 theta; -0.23 psi 2021-02-13T00:39:33Z- handleDepth()-Depth_Keller msg: 1613176773840 epoch sec; seqNo:14596; depth 5.41 2021-02-13T00:39:33Z- handleDvl() - RDI_Pathfinder msg: 1613176773.85 epoch sec; seqNo:7032 2021-02-13T00:39:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.96 , 119.68 , 86.96 , 124.14 2021-02-13T00:39:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.10 , 0.19 2021-02-13T00:39:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176773.87 epoch sec; seqNo:14596; 4059720.20 north; 592921.47 east 2021-02-13T00:39:34Z- AHRS_M2 msg: 1613176774.24 epoch sec; seqNo:14481 2021-02-13T00:39:34Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.21 psi 2021-02-13T00:39:34Z- handleDepth()-Depth_Keller msg: 1613176774249 epoch sec; seqNo:14597; depth 5.52 2021-02-13T00:39:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176774.26 epoch sec; seqNo:14597; 4059720.20 north; 592921.47 east 2021-02-13T00:39:34Z- AHRS_M2 msg: 1613176774.64 epoch sec; seqNo:14482 2021-02-13T00:39:34Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.20 psi 2021-02-13T00:39:34Z- handleDepth()-Depth_Keller msg: 1613176774653 epoch sec; seqNo:14598; depth 5.60 2021-02-13T00:39:34Z- handleDvl() - RDI_Pathfinder msg: 1613176774.66 epoch sec; seqNo:7033 2021-02-13T00:39:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 114.07 , 84.33 , 124.48 2021-02-13T00:39:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.07 , 0.27 2021-02-13T00:39:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176774.67 epoch sec; seqNo:14598; 4059720.58 north; 592921.46 east 2021-02-13T00:39:35Z- AHRS_M2 msg: 1613176775.05 epoch sec; seqNo:14483 2021-02-13T00:39:35Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.20 psi 2021-02-13T00:39:35Z- handleDepth()-Depth_Keller msg: 1613176775063 epoch sec; seqNo:14599; depth 5.71 2021-02-13T00:39:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176775.07 epoch sec; seqNo:14599; 4059720.58 north; 592921.46 east 2021-02-13T00:39:35Z- AHRS_M2 msg: 1613176775.45 epoch sec; seqNo:14484 2021-02-13T00:39:35Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.21 psi 2021-02-13T00:39:35Z- handleDepth()-Depth_Keller msg: 1613176775465 epoch sec; seqNo:14600; depth 5.81 2021-02-13T00:39:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176775.49 epoch sec; seqNo:14600; 4059720.97 north; 592922.68 east 2021-02-13T00:39:35Z- AHRS_M2 msg: 1613176775.86 epoch sec; seqNo:14485 2021-02-13T00:39:35Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.21 psi 2021-02-13T00:39:35Z- handleDepth()-Depth_Keller msg: 1613176775860 epoch sec; seqNo:14601; depth 5.91 2021-02-13T00:39:35Z- handleDvl() - RDI_Pathfinder msg: 1613176775.87 epoch sec; seqNo:7034 2021-02-13T00:39:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.70 , 111.03 , 84.26 , 125.90 2021-02-13T00:39:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.28 , 0.15 2021-02-13T00:39:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176775.88 epoch sec; seqNo:14601; 4059720.97 north; 592922.68 east 2021-02-13T00:39:36Z- AHRS_M2 msg: 1613176776.26 epoch sec; seqNo:14486 2021-02-13T00:39:36Z- AHRS_M2 msg: 0.59 phi; -0.37 theta; -0.23 psi 2021-02-13T00:39:36Z- handleDepth()-Depth_Keller msg: 1613176776265 epoch sec; seqNo:14602; depth 6.01 2021-02-13T00:39:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176776.28 epoch sec; seqNo:14602; 4059721.35 north; 592922.68 east 2021-02-13T00:39:36Z- AHRS_M2 msg: 1613176776.66 epoch sec; seqNo:14487 2021-02-13T00:39:36Z- AHRS_M2 msg: 0.58 phi; -0.37 theta; -0.26 psi 2021-02-13T00:39:36Z- handleDepth()-Depth_Keller msg: 1613176776678 epoch sec; seqNo:14603; depth 6.13 2021-02-13T00:39:36Z- handleDvl() - RDI_Pathfinder msg: 1613176776.69 epoch sec; seqNo:7035 2021-02-13T00:39:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 114.21 , 82.98 , 129.08 2021-02-13T00:39:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.33 , -0.06 2021-02-13T00:39:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176776.70 epoch sec; seqNo:14603; 4059721.73 north; 592922.67 east 2021-02-13T00:39:37Z- AHRS_M2 msg: 1613176777.07 epoch sec; seqNo:14488 2021-02-13T00:39:37Z- AHRS_M2 msg: 0.57 phi; -0.36 theta; -0.28 psi 2021-02-13T00:39:37Z- handleDepth()-Depth_Keller msg: 1613176777072 epoch sec; seqNo:14604; depth 6.25 2021-02-13T00:39:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176777.08 epoch sec; seqNo:14604; 4059721.73 north; 592922.67 east 2021-02-13T00:39:37Z- LcmTrn::handleMessages() - handling 43 messages took 0.63 ms and 131 calls... 2021-02-13T00:39:37Z- waiting for 1613176775.22, time is 1613176777.08 2021-02-13T00:39:37Z- LcmTrn::updateTrn() >>>> pose time 1613176777.08, meas time 1613176776.69 <<<< 2021-02-13T00:39:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176776.69 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:39:37Z- TerrainNav::motionUpdate() - currEstimate = (4059722, 592922.67, 6.25) 2021-02-13T00:39:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:39:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:39:37Z- TNavBF::Projecting Measurements 2021-02-13T00:39:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:39:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:39:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:39:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:39:37Z- TNavFilter::4 good beams. 2021-02-13T00:39:37Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:39:37Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:39:37Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:39:37Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:39:37Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:39:37Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:39:37Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:39:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:37Z- TerrainMapOctree: (vehN, vehE ) = (4059721.48, 592922.48). 2021-02-13T00:39:37Z- TerrainMapOctree: Min Distance = 2024.52. 2021-02-13T00:39:37Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:39:37Z- There is no specific good beam that all particles have in common. 2021-02-13T00:39:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:39:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-13T00:39:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:39:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 20.87 2021-02-13T00:39:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:39:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 11.02 2021-02-13T00:39:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:39:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.030 ExpectedMeasDiff: nan currMeas.covariance: 26.66 2021-02-13T00:39:37Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:39:37Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:39:37Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:39:37Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:39:37Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:39:37Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:39:37Z- Filter 0 No beams used in subcloud update 2021-02-13T00:39:37Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:39:37Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:39:37Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 693.1 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:39:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176776.69 sec. 2021-02-13T00:39:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:39:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:37Z- variances([0], [2], and psi_berg) = 693.038706 832.994417 0.000000 2021-02-13T00:39:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:39:37Z- LcmTrn::publishEstimates() - MLE : 4059721.23 592942.17 6.25 2021-02-13T00:39:37Z- LcmTrn::publishEstimates() - MMSE : 4059721.73 592922.67 6.25 2021-02-13T00:39:37Z- LcmTrn::publishEstimates() - COVAR: 693.04 832.99 0.88 0.00 2021-02-13T00:39:37Z- AHRS_M2 msg: 1613176777.47 epoch sec; seqNo:14489 2021-02-13T00:39:37Z- AHRS_M2 msg: 0.57 phi; -0.36 theta; -0.29 psi 2021-02-13T00:39:37Z- handleDepth()-Depth_Keller msg: 1613176777476 epoch sec; seqNo:14605; depth 6.37 2021-02-13T00:39:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176777.48 epoch sec; seqNo:14605; 4059722.12 north; 592923.89 east 2021-02-13T00:39:37Z- AHRS_M2 msg: 1613176777.88 epoch sec; seqNo:14490 2021-02-13T00:39:37Z- AHRS_M2 msg: 0.57 phi; -0.35 theta; -0.29 psi 2021-02-13T00:39:37Z- handleDepth()-Depth_Keller msg: 1613176777881 epoch sec; seqNo:14606; depth 6.48 2021-02-13T00:39:37Z- handleDvl() - RDI_Pathfinder msg: 1613176777.89 epoch sec; seqNo:7036 2021-02-13T00:39:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.65 , 119.89 , 84.19 , 130.30 2021-02-13T00:39:37Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , 0.14 , -0.21 2021-02-13T00:39:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176777.90 epoch sec; seqNo:14606; 4059722.50 north; 592923.88 east 2021-02-13T00:39:38Z- AHRS_M2 msg: 1613176778.28 epoch sec; seqNo:14491 2021-02-13T00:39:38Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.29 psi 2021-02-13T00:39:38Z- handleDepth()-Depth_Keller msg: 1613176778284 epoch sec; seqNo:14607; depth 6.66 2021-02-13T00:39:38Z- handleDvl() - RDI_Pathfinder msg: 1613176778.30 epoch sec; seqNo:7037 2021-02-13T00:39:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:39:38Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:39:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176778.31 epoch sec; seqNo:14607; 4059722.88 north; 592923.88 east 2021-02-13T00:39:38Z- AHRS_M2 msg: 1613176778.68 epoch sec; seqNo:14492 2021-02-13T00:39:38Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.27 psi 2021-02-13T00:39:38Z- handleDepth()-Depth_Keller msg: 1613176778688 epoch sec; seqNo:14608; depth 6.76 2021-02-13T00:39:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176778.70 epoch sec; seqNo:14608; 4059723.26 north; 592923.88 east 2021-02-13T00:39:39Z- AHRS_M2 msg: 1613176779.09 epoch sec; seqNo:14493 2021-02-13T00:39:39Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.26 psi 2021-02-13T00:39:39Z- handleDepth()-Depth_Keller msg: 1613176779097 epoch sec; seqNo:14609; depth 6.90 2021-02-13T00:39:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176779.11 epoch sec; seqNo:14609; 4059723.65 north; 592925.09 east 2021-02-13T00:39:39Z- AHRS_M2 msg: 1613176779.49 epoch sec; seqNo:14494 2021-02-13T00:39:39Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.24 psi 2021-02-13T00:39:39Z- handleDepth()-Depth_Keller msg: 1613176779505 epoch sec; seqNo:14610; depth 7.03 2021-02-13T00:39:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176779.51 epoch sec; seqNo:14610; 4059724.03 north; 592925.09 east 2021-02-13T00:39:39Z- AHRS_M2 msg: 1613176779.90 epoch sec; seqNo:14495 2021-02-13T00:39:39Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.22 psi 2021-02-13T00:39:39Z- handleDepth()-Depth_Keller msg: 1613176779903 epoch sec; seqNo:14611; depth 7.15 2021-02-13T00:39:39Z- handleDvl() - RDI_Pathfinder msg: 1613176779.92 epoch sec; seqNo:7038 2021-02-13T00:39:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 119.95 , 116.98 , 84.26 , 121.44 2021-02-13T00:39:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.07 , -0.18 2021-02-13T00:39:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176779.93 epoch sec; seqNo:14611; 4059724.03 north; 592925.09 east 2021-02-13T00:39:40Z- AHRS_M2 msg: 1613176780.30 epoch sec; seqNo:14496 2021-02-13T00:39:40Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.21 psi 2021-02-13T00:39:40Z- handleDepth()-Depth_Keller msg: 1613176780309 epoch sec; seqNo:14612; depth 7.27 2021-02-13T00:39:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176780.32 epoch sec; seqNo:14612; 4059724.41 north; 592925.08 east 2021-02-13T00:39:40Z- AHRS_M2 msg: 1613176780.70 epoch sec; seqNo:14497 2021-02-13T00:39:40Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.20 psi 2021-02-13T00:39:40Z- handleDepth()-Depth_Keller msg: 1613176780710 epoch sec; seqNo:14613; depth 7.39 2021-02-13T00:39:40Z- handleDvl() - RDI_Pathfinder msg: 1613176780.72 epoch sec; seqNo:7039 2021-02-13T00:39:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 110.62 , 82.37 , 122.52 2021-02-13T00:39:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.32 , -0.02 2021-02-13T00:39:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176780.73 epoch sec; seqNo:14613; 4059724.79 north; 592925.08 east 2021-02-13T00:39:41Z- AHRS_M2 msg: 1613176781.11 epoch sec; seqNo:14498 2021-02-13T00:39:41Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.19 psi 2021-02-13T00:39:41Z- handleDepth()-Depth_Keller msg: 1613176781112 epoch sec; seqNo:14614; depth 7.49 2021-02-13T00:39:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176781.13 epoch sec; seqNo:14614; 4059725.17 north; 592925.08 east 2021-02-13T00:39:41Z- AHRS_M2 msg: 1613176781.51 epoch sec; seqNo:14499 2021-02-13T00:39:41Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.18 psi 2021-02-13T00:39:41Z- handleDepth()-Depth_Keller msg: 1613176781521 epoch sec; seqNo:14615; depth 7.59 2021-02-13T00:39:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176781.53 epoch sec; seqNo:14615; 4059725.17 north; 592925.08 east 2021-02-13T00:39:41Z- AHRS_M2 msg: 1613176781.92 epoch sec; seqNo:14500 2021-02-13T00:39:41Z- AHRS_M2 msg: 0.56 phi; -0.34 theta; -0.17 psi 2021-02-13T00:39:41Z- handleDepth()-Depth_Keller msg: 1613176781920 epoch sec; seqNo:14616; depth 7.71 2021-02-13T00:39:41Z- handleDvl() - RDI_Pathfinder msg: 1613176781.93 epoch sec; seqNo:7040 2021-02-13T00:39:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.72 , 108.06 , 81.29 , 119.95 2021-02-13T00:39:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.21 , 0.28 2021-02-13T00:39:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176781.94 epoch sec; seqNo:14616; 4059725.55 north; 592925.07 east 2021-02-13T00:39:42Z- AHRS_M2 msg: 1613176782.34 epoch sec; seqNo:14501 2021-02-13T00:39:42Z- AHRS_M2 msg: 0.55 phi; -0.34 theta; -0.17 psi 2021-02-13T00:39:42Z- handleDepth()-Depth_Keller msg: 1613176782346 epoch sec; seqNo:14617; depth 7.85 2021-02-13T00:39:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176782.36 epoch sec; seqNo:14617; 4059725.93 north; 592925.07 east 2021-02-13T00:39:42Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-13T00:39:42Z- waiting for 1613176780.08, time is 1613176782.36 2021-02-13T00:39:42Z- LcmTrn::updateTrn() >>>> pose time 1613176782.36, meas time 1613176781.93 <<<< 2021-02-13T00:39:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176781.93 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:39:42Z- TerrainNav::motionUpdate() - currEstimate = (4059726, 592925.07, 7.85) 2021-02-13T00:39:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:39:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:39:42Z- TNavBF::Projecting Measurements 2021-02-13T00:39:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:39:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:39:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:39:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:39:42Z- TNavFilter::4 good beams. 2021-02-13T00:39:42Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:39:42Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:39:42Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:39:42Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:39:42Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:39:42Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:39:42Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:39:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:42Z- TerrainMapOctree: (vehN, vehE ) = (4059725.59, 592924.88). 2021-02-13T00:39:42Z- TerrainMapOctree: Min Distance = 2019.85. 2021-02-13T00:39:42Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:39:42Z- There is no specific good beam that all particles have in common. 2021-02-13T00:39:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:39:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 12.59 2021-02-13T00:39:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:39:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 18.68 2021-02-13T00:39:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:39:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 10.57 2021-02-13T00:39:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:39:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.035 ExpectedMeasDiff: nan currMeas.covariance: 23.02 2021-02-13T00:39:42Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:39:42Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:39:42Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:39:42Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:39:42Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:39:42Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:39:42Z- Filter 0 No beams used in subcloud update 2021-02-13T00:39:42Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:39:42Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:39:42Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 693.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:39:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176781.93 sec. 2021-02-13T00:39:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:39:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:42Z- variances([0], [2], and psi_berg) = 692.970063 832.994384 0.000000 2021-02-13T00:39:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:39:42Z- LcmTrn::publishEstimates() - MLE : 4059725.43 592945.57 7.85 2021-02-13T00:39:42Z- LcmTrn::publishEstimates() - MMSE : 4059725.93 592925.07 7.85 2021-02-13T00:39:42Z- LcmTrn::publishEstimates() - COVAR: 692.97 832.99 0.89 0.00 2021-02-13T00:39:42Z- AHRS_M2 msg: 1613176782.73 epoch sec; seqNo:14502 2021-02-13T00:39:42Z- AHRS_M2 msg: 0.55 phi; -0.34 theta; -0.16 psi 2021-02-13T00:39:42Z- handleDepth()-Depth_Keller msg: 1613176782738 epoch sec; seqNo:14618; depth 7.96 2021-02-13T00:39:42Z- handleDvl() - RDI_Pathfinder msg: 1613176782.75 epoch sec; seqNo:7041 2021-02-13T00:39:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 106.57 , 80.88 , 122.79 2021-02-13T00:39:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , 0.38 2021-02-13T00:39:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176782.76 epoch sec; seqNo:14618; 4059725.93 north; 592925.07 east 2021-02-13T00:39:43Z- AHRS_M2 msg: 1613176783.13 epoch sec; seqNo:14503 2021-02-13T00:39:43Z- AHRS_M2 msg: 0.55 phi; -0.33 theta; -0.16 psi 2021-02-13T00:39:43Z- handleDepth()-Depth_Keller msg: 1613176783141 epoch sec; seqNo:14619; depth 8.08 2021-02-13T00:39:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176783.16 epoch sec; seqNo:14619; 4059726.32 north; 592926.28 east 2021-02-13T00:39:43Z- AHRS_M2 msg: 1613176783.53 epoch sec; seqNo:14504 2021-02-13T00:39:43Z- AHRS_M2 msg: 0.55 phi; -0.33 theta; -0.15 psi 2021-02-13T00:39:43Z- handleDepth()-Depth_Keller msg: 1613176783536 epoch sec; seqNo:14620; depth 8.19 2021-02-13T00:39:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176783.54 epoch sec; seqNo:14620; 4059726.32 north; 592926.28 east 2021-02-13T00:39:43Z- AHRS_M2 msg: 1613176783.94 epoch sec; seqNo:14505 2021-02-13T00:39:43Z- AHRS_M2 msg: 0.55 phi; -0.33 theta; -0.16 psi 2021-02-13T00:39:43Z- handleDepth()-Depth_Keller msg: 1613176783945 epoch sec; seqNo:14621; depth 8.32 2021-02-13T00:39:43Z- handleDvl() - RDI_Pathfinder msg: 1613176783.96 epoch sec; seqNo:7042 2021-02-13T00:39:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 106.50 , 80.81 , 121.37 2021-02-13T00:39:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.21 , 0.16 2021-02-13T00:39:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176783.97 epoch sec; seqNo:14621; 4059726.70 north; 592926.28 east 2021-02-13T00:39:44Z- AHRS_M2 msg: 1613176784.34 epoch sec; seqNo:14506 2021-02-13T00:39:44Z- AHRS_M2 msg: 0.56 phi; -0.32 theta; -0.17 psi 2021-02-13T00:39:44Z- handleDepth()-Depth_Keller msg: 1613176784353 epoch sec; seqNo:14622; depth 8.44 2021-02-13T00:39:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176784.36 epoch sec; seqNo:14622; 4059727.08 north; 592926.28 east 2021-02-13T00:39:44Z- AHRS_M2 msg: 1613176784.74 epoch sec; seqNo:14507 2021-02-13T00:39:44Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.17 psi 2021-02-13T00:39:44Z- handleDepth()-Depth_Keller msg: 1613176784748 epoch sec; seqNo:14623; depth 8.57 2021-02-13T00:39:44Z- handleDvl() - RDI_Pathfinder msg: 1613176784.77 epoch sec; seqNo:7043 2021-02-13T00:39:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.42 , 81.62 , 114.34 2021-02-13T00:39:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.14 , -0.13 2021-02-13T00:39:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176784.78 epoch sec; seqNo:14623; 4059727.45 north; 592926.27 east 2021-02-13T00:39:45Z- AHRS_M2 msg: 1613176785.15 epoch sec; seqNo:14508 2021-02-13T00:39:45Z- AHRS_M2 msg: 0.57 phi; -0.31 theta; -0.18 psi 2021-02-13T00:39:45Z- handleDepth()-Depth_Keller msg: 1613176785155 epoch sec; seqNo:14624; depth 8.68 2021-02-13T00:39:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176785.17 epoch sec; seqNo:14624; 4059727.47 north; 592927.49 east 2021-02-13T00:39:45Z- AHRS_M2 msg: 1613176785.55 epoch sec; seqNo:14509 2021-02-13T00:39:45Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-13T00:39:45Z- handleDepth()-Depth_Keller msg: 1613176785561 epoch sec; seqNo:14625; depth 8.78 2021-02-13T00:39:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176785.57 epoch sec; seqNo:14625; 4059727.85 north; 592927.49 east 2021-02-13T00:39:45Z- AHRS_M2 msg: 1613176785.96 epoch sec; seqNo:14510 2021-02-13T00:39:45Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.17 psi 2021-02-13T00:39:45Z- handleDepth()-Depth_Keller msg: 1613176785969 epoch sec; seqNo:14626; depth 8.89 2021-02-13T00:39:45Z- handleDvl() - RDI_Pathfinder msg: 1613176785.98 epoch sec; seqNo:7044 2021-02-13T00:39:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.51 , 106.43 , 80.75 , 113.19 2021-02-13T00:39:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.04 , -0.17 2021-02-13T00:39:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176785.99 epoch sec; seqNo:14626; 4059728.22 north; 592927.48 east 2021-02-13T00:39:46Z- AHRS_M2 msg: 1613176786.36 epoch sec; seqNo:14511 2021-02-13T00:39:46Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.17 psi 2021-02-13T00:39:46Z- handleDepth()-Depth_Keller msg: 1613176786374 epoch sec; seqNo:14627; depth 9.02 2021-02-13T00:39:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176786.38 epoch sec; seqNo:14627; 4059728.60 north; 592927.48 east 2021-02-13T00:39:46Z- AHRS_M2 msg: 1613176786.76 epoch sec; seqNo:14512 2021-02-13T00:39:46Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.16 psi 2021-02-13T00:39:46Z- handleDepth()-Depth_Keller msg: 1613176786768 epoch sec; seqNo:14628; depth 9.13 2021-02-13T00:39:46Z- handleDvl() - RDI_Pathfinder msg: 1613176786.78 epoch sec; seqNo:7045 2021-02-13T00:39:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 104.07 , 81.08 , 112.18 2021-02-13T00:39:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.12 , 0.03 2021-02-13T00:39:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176786.81 epoch sec; seqNo:14628; 4059728.98 north; 592927.48 east 2021-02-13T00:39:47Z- AHRS_M2 msg: 1613176787.17 epoch sec; seqNo:14513 2021-02-13T00:39:47Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.15 psi 2021-02-13T00:39:47Z- handleDepth()-Depth_Keller msg: 1613176787172 epoch sec; seqNo:14629; depth 9.26 2021-02-13T00:39:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176787.18 epoch sec; seqNo:14629; 4059728.98 north; 592927.48 east 2021-02-13T00:39:47Z- LcmTrn::handleMessages() - handling 41 messages took 0.60 ms and 129 calls... 2021-02-13T00:39:47Z- waiting for 1613176785.36, time is 1613176787.18 2021-02-13T00:39:47Z- LcmTrn::updateTrn() >>>> pose time 1613176787.18, meas time 1613176786.78 <<<< 2021-02-13T00:39:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176786.78 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:39:47Z- TerrainNav::motionUpdate() - currEstimate = (4059729, 592927.48, 9.26) 2021-02-13T00:39:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:39:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:39:47Z- TNavBF::Projecting Measurements 2021-02-13T00:39:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:39:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:39:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:39:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:39:47Z- TNavFilter::4 good beams. 2021-02-13T00:39:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:39:47Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:39:47Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:39:47Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:39:47Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:39:47Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:39:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:39:47Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:47Z- TerrainMapOctree: (vehN, vehE ) = (4059728.73, 592927.27). 2021-02-13T00:39:47Z- TerrainMapOctree: Min Distance = 2015.91. 2021-02-13T00:39:47Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:39:47Z- There is no specific good beam that all particles have in common. 2021-02-13T00:39:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:39:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 11.97 2021-02-13T00:39:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:39:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 17.33 2021-02-13T00:39:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:39:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 10.52 2021-02-13T00:39:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:39:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 20.13 2021-02-13T00:39:47Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:39:47Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:39:47Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:39:47Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:39:47Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:39:47Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:39:47Z- Filter 0 No beams used in subcloud update 2021-02-13T00:39:47Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:39:47Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:39:47Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 692.9 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:39:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176786.78 sec. 2021-02-13T00:39:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:39:47Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:47Z- variances([0], [2], and psi_berg) = 692.908301 832.994353 0.000000 2021-02-13T00:39:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:39:47Z- LcmTrn::publishEstimates() - MLE : 4059728.48 592947.98 9.26 2021-02-13T00:39:47Z- LcmTrn::publishEstimates() - MMSE : 4059728.98 592927.48 9.26 2021-02-13T00:39:47Z- LcmTrn::publishEstimates() - COVAR: 692.91 832.99 0.86 0.00 2021-02-13T00:39:47Z- AHRS_M2 msg: 1613176787.57 epoch sec; seqNo:14514 2021-02-13T00:39:47Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.15 psi 2021-02-13T00:39:47Z- handleDepth()-Depth_Keller msg: 1613176787577 epoch sec; seqNo:14630; depth 9.36 2021-02-13T00:39:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176787.59 epoch sec; seqNo:14630; 4059729.36 north; 592927.47 east 2021-02-13T00:39:47Z- AHRS_M2 msg: 1613176787.98 epoch sec; seqNo:14515 2021-02-13T00:39:47Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.14 psi 2021-02-13T00:39:47Z- handleDepth()-Depth_Keller msg: 1613176787991 epoch sec; seqNo:14631; depth 9.46 2021-02-13T00:39:48Z- handleDvl() - RDI_Pathfinder msg: 1613176788.00 epoch sec; seqNo:7046 2021-02-13T00:39:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 101.43 , 79.80 , 117.65 2021-02-13T00:39:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , 0.17 2021-02-13T00:39:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176788.01 epoch sec; seqNo:14631; 4059729.75 north; 592928.69 east 2021-02-13T00:39:48Z- AHRS_M2 msg: 1613176788.38 epoch sec; seqNo:14516 2021-02-13T00:39:48Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.14 psi 2021-02-13T00:39:48Z- handleDepth()-Depth_Keller msg: 1613176788388 epoch sec; seqNo:14632; depth 9.58 2021-02-13T00:39:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176788.40 epoch sec; seqNo:14632; 4059730.13 north; 592928.68 east 2021-02-13T00:39:48Z- AHRS_M2 msg: 1613176788.79 epoch sec; seqNo:14517 2021-02-13T00:39:48Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.14 psi 2021-02-13T00:39:48Z- handleDepth()-Depth_Keller msg: 1613176788792 epoch sec; seqNo:14633; depth 9.68 2021-02-13T00:39:48Z- handleDvl() - RDI_Pathfinder msg: 1613176788.80 epoch sec; seqNo:7047 2021-02-13T00:39:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.91 , 100.08 , 79.80 , 113.60 2021-02-13T00:39:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.09 , 0.08 2021-02-13T00:39:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176788.81 epoch sec; seqNo:14633; 4059730.13 north; 592928.68 east 2021-02-13T00:39:49Z- AHRS_M2 msg: 1613176789.19 epoch sec; seqNo:14518 2021-02-13T00:39:49Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.16 psi 2021-02-13T00:39:49Z- handleDepth()-Depth_Keller msg: 1613176789198 epoch sec; seqNo:14634; depth 9.80 2021-02-13T00:39:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176789.21 epoch sec; seqNo:14634; 4059730.51 north; 592928.68 east 2021-02-13T00:39:49Z- AHRS_M2 msg: 1613176789.59 epoch sec; seqNo:14519 2021-02-13T00:39:49Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.18 psi 2021-02-13T00:39:49Z- handleDepth()-Depth_Keller msg: 1613176789596 epoch sec; seqNo:14635; depth 9.91 2021-02-13T00:39:49Z- handleDvl() - RDI_Pathfinder msg: 1613176789.61 epoch sec; seqNo:7048 2021-02-13T00:39:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:39:49Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:39:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176789.63 epoch sec; seqNo:14635; 4059730.89 north; 592928.68 east 2021-02-13T00:39:49Z- AHRS_M2 msg: 1613176790.00 epoch sec; seqNo:14520 2021-02-13T00:39:49Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.19 psi 2021-02-13T00:39:50Z- handleDepth()-Depth_Keller msg: 1613176790000 epoch sec; seqNo:14636; depth 10.01 2021-02-13T00:39:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176790.01 epoch sec; seqNo:14636; 4059730.89 north; 592928.68 east 2021-02-13T00:39:50Z- AHRS_M2 msg: 1613176790.40 epoch sec; seqNo:14521 2021-02-13T00:39:50Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.19 psi 2021-02-13T00:39:50Z- handleDepth()-Depth_Keller msg: 1613176790405 epoch sec; seqNo:14637; depth 10.11 2021-02-13T00:39:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176790.42 epoch sec; seqNo:14637; 4059731.28 north; 592929.89 east 2021-02-13T00:39:50Z- AHRS_M2 msg: 1613176790.80 epoch sec; seqNo:14522 2021-02-13T00:39:50Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.19 psi 2021-02-13T00:39:50Z- handleDepth()-Depth_Keller msg: 1613176790808 epoch sec; seqNo:14638; depth 10.23 2021-02-13T00:39:50Z- handleDvl() - RDI_Pathfinder msg: 1613176790.83 epoch sec; seqNo:7049 2021-02-13T00:39:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 104.68 , 80.88 , 113.60 2021-02-13T00:39:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.07 , -0.10 2021-02-13T00:39:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176790.84 epoch sec; seqNo:14638; 4059731.66 north; 592929.89 east 2021-02-13T00:39:51Z- AHRS_M2 msg: 1613176791.21 epoch sec; seqNo:14523 2021-02-13T00:39:51Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.18 psi 2021-02-13T00:39:51Z- handleDepth()-Depth_Keller msg: 1613176791212 epoch sec; seqNo:14639; depth 10.35 2021-02-13T00:39:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176791.23 epoch sec; seqNo:14639; 4059732.04 north; 592929.88 east 2021-02-13T00:39:51Z- AHRS_M2 msg: 1613176791.61 epoch sec; seqNo:14524 2021-02-13T00:39:51Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.17 psi 2021-02-13T00:39:51Z- handleDepth()-Depth_Keller msg: 1613176791616 epoch sec; seqNo:14640; depth 10.45 2021-02-13T00:39:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176791.62 epoch sec; seqNo:14640; 4059732.42 north; 592929.88 east 2021-02-13T00:39:52Z- AHRS_M2 msg: 1613176792.02 epoch sec; seqNo:14525 2021-02-13T00:39:52Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.15 psi 2021-02-13T00:39:52Z- handleDepth()-Depth_Keller msg: 1613176792022 epoch sec; seqNo:14641; depth 10.55 2021-02-13T00:39:52Z- handleDvl() - RDI_Pathfinder msg: 1613176792.03 epoch sec; seqNo:7050 2021-02-13T00:39:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.86 , 104.14 , 79.80 , 113.60 2021-02-13T00:39:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.09 , -0.03 2021-02-13T00:39:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176792.04 epoch sec; seqNo:14641; 4059732.80 north; 592929.88 east 2021-02-13T00:39:52Z- handleDepth()-Depth_Keller msg: 1613176792441 epoch sec; seqNo:14642; depth 10.66 2021-02-13T00:39:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176792.45 epoch sec; seqNo:14642; 4059733.18 north; 592929.87 east 2021-02-13T00:39:52Z- LcmTrn::handleMessages() - handling 43 messages took 0.62 ms and 131 calls... 2021-02-13T00:39:52Z- waiting for 1613176790.18, time is 1613176792.45 2021-02-13T00:39:52Z- LcmTrn::updateTrn() >>>> pose time 1613176792.45, meas time 1613176792.03 <<<< 2021-02-13T00:39:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176792.03 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:39:52Z- TerrainNav::motionUpdate() - currEstimate = (4059733, 592929.87, 10.66) 2021-02-13T00:39:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:39:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:39:52Z- TNavBF::Projecting Measurements 2021-02-13T00:39:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:39:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:39:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:39:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:39:52Z- TNavFilter::4 good beams. 2021-02-13T00:39:52Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:39:52Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:39:52Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:39:52Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:39:52Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:39:52Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:39:52Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:39:52Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:52Z- TerrainMapOctree: (vehN, vehE ) = (4059732.84, 592929.68). 2021-02-13T00:39:52Z- TerrainMapOctree: Min Distance = 2011.22. 2021-02-13T00:39:52Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:39:52Z- There is no specific good beam that all particles have in common. 2021-02-13T00:39:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:39:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 11.25 2021-02-13T00:39:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:39:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 17.35 2021-02-13T00:39:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:39:52Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 10.19 2021-02-13T00:39:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:39:52Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 20.65 2021-02-13T00:39:52Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:39:52Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:39:52Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:39:52Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:39:52Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:39:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:39:52Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:39:52Z- Filter 0 No beams used in subcloud update 2021-02-13T00:39:52Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:39:52Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:39:52Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 692.9 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:39:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176792.03 sec. 2021-02-13T00:39:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:39:52Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:52Z- variances([0], [2], and psi_berg) = 692.839837 832.994320 0.000000 2021-02-13T00:39:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:39:52Z- LcmTrn::publishEstimates() - MLE : 4059732.68 592950.37 10.66 2021-02-13T00:39:52Z- LcmTrn::publishEstimates() - MMSE : 4059733.18 592929.87 10.66 2021-02-13T00:39:52Z- LcmTrn::publishEstimates() - COVAR: 692.84 832.99 0.87 0.00 2021-02-13T00:39:52Z- AHRS_M2 msg: 1613176792.82 epoch sec; seqNo:14526 2021-02-13T00:39:52Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.14 psi 2021-02-13T00:39:52Z- handleDepth()-Depth_Keller msg: 1613176792829 epoch sec; seqNo:14643; depth 10.77 2021-02-13T00:39:52Z- handleDvl() - RDI_Pathfinder msg: 1613176792.84 epoch sec; seqNo:7051 2021-02-13T00:39:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.28 , 100.15 , 79.87 , 110.96 2021-02-13T00:39:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , -0.01 2021-02-13T00:39:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176792.86 epoch sec; seqNo:14643; 4059733.19 north; 592931.09 east 2021-02-13T00:39:53Z- AHRS_M2 msg: 1613176793.23 epoch sec; seqNo:14527 2021-02-13T00:39:53Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.14 psi 2021-02-13T00:39:53Z- handleDepth()-Depth_Keller msg: 1613176793233 epoch sec; seqNo:14644; depth 10.88 2021-02-13T00:39:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176793.24 epoch sec; seqNo:14644; 4059733.57 north; 592931.09 east 2021-02-13T00:39:53Z- AHRS_M2 msg: 1613176793.65 epoch sec; seqNo:14528 2021-02-13T00:39:53Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.15 psi 2021-02-13T00:39:53Z- handleDepth()-Depth_Keller msg: 1613176793650 epoch sec; seqNo:14645; depth 10.97 2021-02-13T00:39:53Z- handleDvl() - RDI_Pathfinder msg: 1613176793.66 epoch sec; seqNo:7052 2021-02-13T00:39:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 98.86 , 78.58 , 113.73 2021-02-13T00:39:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , -0.04 2021-02-13T00:39:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176793.67 epoch sec; seqNo:14645; 4059733.95 north; 592931.08 east 2021-02-13T00:39:54Z- AHRS_M2 msg: 1613176794.04 epoch sec; seqNo:14529 2021-02-13T00:39:54Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.16 psi 2021-02-13T00:39:54Z- handleDepth()-Depth_Keller msg: 1613176794045 epoch sec; seqNo:14646; depth 11.06 2021-02-13T00:39:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176794.06 epoch sec; seqNo:14646; 4059734.33 north; 592931.08 east 2021-02-13T00:39:54Z- AHRS_M2 msg: 1613176794.44 epoch sec; seqNo:14530 2021-02-13T00:39:54Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.17 psi 2021-02-13T00:39:54Z- handleDepth()-Depth_Keller msg: 1613176794444 epoch sec; seqNo:14647; depth 11.17 2021-02-13T00:39:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176794.45 epoch sec; seqNo:14647; 4059734.33 north; 592931.08 east 2021-02-13T00:39:54Z- AHRS_M2 msg: 1613176794.84 epoch sec; seqNo:14531 2021-02-13T00:39:54Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.17 psi 2021-02-13T00:39:54Z- handleDepth()-Depth_Keller msg: 1613176794848 epoch sec; seqNo:14648; depth 11.25 2021-02-13T00:39:54Z- handleDvl() - RDI_Pathfinder msg: 1613176794.86 epoch sec; seqNo:7053 2021-02-13T00:39:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 100.21 , 78.05 , 111.03 2021-02-13T00:39:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.08 , -0.02 2021-02-13T00:39:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176794.87 epoch sec; seqNo:14648; 4059734.70 north; 592931.08 east 2021-02-13T00:39:55Z- AHRS_M2 msg: 1613176795.25 epoch sec; seqNo:14532 2021-02-13T00:39:55Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.17 psi 2021-02-13T00:39:55Z- handleDepth()-Depth_Keller msg: 1613176795254 epoch sec; seqNo:14649; depth 11.36 2021-02-13T00:39:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176795.27 epoch sec; seqNo:14649; 4059735.08 north; 592931.07 east 2021-02-13T00:39:55Z- AHRS_M2 msg: 1613176795.65 epoch sec; seqNo:14533 2021-02-13T00:39:55Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.16 psi 2021-02-13T00:39:55Z- handleDepth()-Depth_Keller msg: 1613176795656 epoch sec; seqNo:14650; depth 11.47 2021-02-13T00:39:55Z- handleDvl() - RDI_Pathfinder msg: 1613176795.67 epoch sec; seqNo:7054 2021-02-13T00:39:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 101.63 , 77.24 , 112.45 2021-02-13T00:39:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.10 , 0.00 2021-02-13T00:39:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176795.68 epoch sec; seqNo:14650; 4059735.48 north; 592932.29 east 2021-02-13T00:39:56Z- AHRS_M2 msg: 1613176796.06 epoch sec; seqNo:14534 2021-02-13T00:39:56Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.15 psi 2021-02-13T00:39:56Z- handleDepth()-Depth_Keller msg: 1613176796065 epoch sec; seqNo:14651; depth 11.57 2021-02-13T00:39:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176796.08 epoch sec; seqNo:14651; 4059735.85 north; 592932.28 east 2021-02-13T00:39:56Z- AHRS_M2 msg: 1613176796.46 epoch sec; seqNo:14535 2021-02-13T00:39:56Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.14 psi 2021-02-13T00:39:56Z- handleDepth()-Depth_Keller msg: 1613176796469 epoch sec; seqNo:14652; depth 11.67 2021-02-13T00:39:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176796.48 epoch sec; seqNo:14652; 4059735.85 north; 592932.28 east 2021-02-13T00:39:56Z- AHRS_M2 msg: 1613176796.87 epoch sec; seqNo:14536 2021-02-13T00:39:56Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.14 psi 2021-02-13T00:39:56Z- handleDepth()-Depth_Keller msg: 1613176796877 epoch sec; seqNo:14653; depth 11.78 2021-02-13T00:39:56Z- handleDvl() - RDI_Pathfinder msg: 1613176796.89 epoch sec; seqNo:7055 2021-02-13T00:39:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 98.93 , 0.00 , 109.75 2021-02-13T00:39:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.14 , -0.00 2021-02-13T00:39:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176796.90 epoch sec; seqNo:14653; 4059736.23 north; 592932.28 east 2021-02-13T00:39:57Z- AHRS_M2 msg: 1613176797.27 epoch sec; seqNo:14537 2021-02-13T00:39:57Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.13 psi 2021-02-13T00:39:57Z- handleDepth()-Depth_Keller msg: 1613176797277 epoch sec; seqNo:14654; depth 11.87 2021-02-13T00:39:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176797.29 epoch sec; seqNo:14654; 4059736.61 north; 592932.28 east 2021-02-13T00:39:57Z- AHRS_M2 msg: 1613176797.67 epoch sec; seqNo:14538 2021-02-13T00:39:57Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.14 psi 2021-02-13T00:39:57Z- handleDepth()-Depth_Keller msg: 1613176797676 epoch sec; seqNo:14655; depth 11.97 2021-02-13T00:39:57Z- handleDvl() - RDI_Pathfinder msg: 1613176797.69 epoch sec; seqNo:7056 2021-02-13T00:39:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.07 , 98.59 , 76.83 , 108.06 2021-02-13T00:39:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.13 , 0.03 2021-02-13T00:39:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176797.71 epoch sec; seqNo:14655; 4059736.99 north; 592932.27 east 2021-02-13T00:39:57Z- LcmTrn::handleMessages() - handling 45 messages took 0.68 ms and 132 calls... 2021-02-13T00:39:57Z- waiting for 1613176795.45, time is 1613176797.71 2021-02-13T00:39:57Z- LcmTrn::updateTrn() equating pose 1613176797.71 and meas times 1613176797.71 2021-02-13T00:39:57Z- LcmTrn::updateTrn() >>>> pose time 1613176797.71, meas time 1613176797.71 <<<< 2021-02-13T00:39:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176797.71 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:39:57Z- TerrainNav::motionUpdate() - currEstimate = (4059737, 592932.27, 11.97) 2021-02-13T00:39:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:39:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:39:57Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176797.71 sec.; No relevant navigation data available 2021-02-13T00:39:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:39:57Z- TNavBF::mmse selected from filter -1 2021-02-13T00:39:57Z- variances([0], [2], and psi_berg) = 692.786584 832.994294 0.000000 2021-02-13T00:39:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:39:57Z- LcmTrn::publishEstimates() - MLE : 4059736.49 592952.77 11.97 2021-02-13T00:39:57Z- LcmTrn::publishEstimates() - MMSE : 4059736.99 592932.27 11.97 2021-02-13T00:39:57Z- LcmTrn::publishEstimates() - COVAR: 692.79 832.99 1.10 0.00 2021-02-13T00:39:58Z- AHRS_M2 msg: 1613176798.08 epoch sec; seqNo:14539 2021-02-13T00:39:58Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.15 psi 2021-02-13T00:39:58Z- handleDepth()-Depth_Keller msg: 1613176798080 epoch sec; seqNo:14656; depth 12.08 2021-02-13T00:39:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176798.09 epoch sec; seqNo:14656; 4059737.37 north; 592932.27 east 2021-02-13T00:39:58Z- AHRS_M2 msg: 1613176798.48 epoch sec; seqNo:14540 2021-02-13T00:39:58Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.16 psi 2021-02-13T00:39:58Z- handleDepth()-Depth_Keller msg: 1613176798485 epoch sec; seqNo:14657; depth 12.17 2021-02-13T00:39:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176798.49 epoch sec; seqNo:14657; 4059737.37 north; 592932.27 east 2021-02-13T00:39:58Z- AHRS_M2 msg: 1613176798.88 epoch sec; seqNo:14541 2021-02-13T00:39:58Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.16 psi 2021-02-13T00:39:58Z- handleDepth()-Depth_Keller msg: 1613176798889 epoch sec; seqNo:14658; depth 12.28 2021-02-13T00:39:58Z- handleDvl() - RDI_Pathfinder msg: 1613176798.91 epoch sec; seqNo:7057 2021-02-13T00:39:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 99.07 , 75.60 , 111.23 2021-02-13T00:39:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.07 , 0.17 2021-02-13T00:39:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176798.92 epoch sec; seqNo:14658; 4059737.76 north; 592933.49 east 2021-02-13T00:39:59Z- AHRS_M2 msg: 1613176799.29 epoch sec; seqNo:14542 2021-02-13T00:39:59Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.17 psi 2021-02-13T00:39:59Z- handleDepth()-Depth_Keller msg: 1613176799292 epoch sec; seqNo:14659; depth 12.40 2021-02-13T00:39:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176799.30 epoch sec; seqNo:14659; 4059738.14 north; 592933.48 east 2021-02-13T00:39:59Z- AHRS_M2 msg: 1613176799.69 epoch sec; seqNo:14543 2021-02-13T00:39:59Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.17 psi 2021-02-13T00:39:59Z- handleDepth()-Depth_Keller msg: 1613176799701 epoch sec; seqNo:14660; depth 12.49 2021-02-13T00:39:59Z- handleDvl() - RDI_Pathfinder msg: 1613176799.71 epoch sec; seqNo:7058 2021-02-13T00:39:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.13 , 100.35 , 76.01 , 112.52 2021-02-13T00:39:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.10 , 0.24 2021-02-13T00:39:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176799.74 epoch sec; seqNo:14660; 4059738.14 north; 592933.48 east 2021-02-13T00:40:00Z- AHRS_M2 msg: 1613176800.10 epoch sec; seqNo:14544 2021-02-13T00:40:00Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-13T00:40:00Z- handleDepth()-Depth_Keller msg: 1613176800106 epoch sec; seqNo:14661; depth 12.60 2021-02-13T00:40:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176800.12 epoch sec; seqNo:14661; 4059738.52 north; 592933.48 east 2021-02-13T00:40:00Z- AHRS_M2 msg: 1613176800.50 epoch sec; seqNo:14545 2021-02-13T00:40:00Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-13T00:40:00Z- handleDepth()-Depth_Keller msg: 1613176800504 epoch sec; seqNo:14662; depth 12.71 2021-02-13T00:40:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176800.52 epoch sec; seqNo:14662; 4059738.52 north; 592933.48 east 2021-02-13T00:40:00Z- AHRS_M2 msg: 1613176800.90 epoch sec; seqNo:14546 2021-02-13T00:40:00Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.16 psi 2021-02-13T00:40:00Z- handleDepth()-Depth_Keller msg: 1613176800908 epoch sec; seqNo:14663; depth 12.81 2021-02-13T00:40:00Z- handleDvl() - RDI_Pathfinder msg: 1613176800.92 epoch sec; seqNo:7059 2021-02-13T00:40:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.13 , 100.35 , 74.37 , 115.22 2021-02-13T00:40:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.22 , 0.16 2021-02-13T00:40:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176800.93 epoch sec; seqNo:14663; 4059738.90 north; 592933.47 east 2021-02-13T00:40:01Z- AHRS_M2 msg: 1613176801.31 epoch sec; seqNo:14547 2021-02-13T00:40:01Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.16 psi 2021-02-13T00:40:01Z- handleDepth()-Depth_Keller msg: 1613176801314 epoch sec; seqNo:14664; depth 12.93 2021-02-13T00:40:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176801.33 epoch sec; seqNo:14664; 4059738.91 north; 592934.69 east 2021-02-13T00:40:01Z- AHRS_M2 msg: 1613176801.71 epoch sec; seqNo:14548 2021-02-13T00:40:01Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.16 psi 2021-02-13T00:40:01Z- handleDepth()-Depth_Keller msg: 1613176801716 epoch sec; seqNo:14665; depth 13.04 2021-02-13T00:40:01Z- handleDvl() - RDI_Pathfinder msg: 1613176801.73 epoch sec; seqNo:7060 2021-02-13T00:40:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.28 , 75.60 , 113.80 2021-02-13T00:40:01Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.18 , -0.02 2021-02-13T00:40:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176801.74 epoch sec; seqNo:14665; 4059739.29 north; 592934.69 east 2021-02-13T00:40:02Z- AHRS_M2 msg: 1613176802.12 epoch sec; seqNo:14549 2021-02-13T00:40:02Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.16 psi 2021-02-13T00:40:02Z- handleDepth()-Depth_Keller msg: 1613176802125 epoch sec; seqNo:14666; depth 13.17 2021-02-13T00:40:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176802.14 epoch sec; seqNo:14666; 4059739.67 north; 592934.69 east 2021-02-13T00:40:02Z- AHRS_M2 msg: 1613176802.52 epoch sec; seqNo:14550 2021-02-13T00:40:02Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.16 psi 2021-02-13T00:40:02Z- handleDepth()-Depth_Keller msg: 1613176802531 epoch sec; seqNo:14667; depth 13.27 2021-02-13T00:40:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176802.54 epoch sec; seqNo:14667; 4059740.05 north; 592934.68 east 2021-02-13T00:40:02Z- LcmTrn::handleMessages() - handling 40 messages took 0.61 ms and 128 calls... 2021-02-13T00:40:02Z- waiting for 1613176800.71, time is 1613176802.54 2021-02-13T00:40:02Z- LcmTrn::updateTrn() >>>> pose time 1613176802.54, meas time 1613176801.73 <<<< 2021-02-13T00:40:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176801.73 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:40:02Z- TerrainNav::motionUpdate() - currEstimate = (4059740, 592934.68, 13.27) 2021-02-13T00:40:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:40:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:40:02Z- TNavBF::Projecting Measurements 2021-02-13T00:40:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:40:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:40:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:40:02Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:40:02Z- TNavFilter::3 good beams. 2021-02-13T00:40:02Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:40:02Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:40:02Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:40:02Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:40:02Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:40:02Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:40:02Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:40:02Z- TNavBF::mmse selected from filter -1 2021-02-13T00:40:02Z- TerrainMapOctree: (vehN, vehE ) = (4059739.53, 592934.28). 2021-02-13T00:40:02Z- TerrainMapOctree: Min Distance = 2003.16. 2021-02-13T00:40:02Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:40:02Z- There is no specific good beam that all particles have in common. 2021-02-13T00:40:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:40:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 16.09 2021-02-13T00:40:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:40:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 9.14 2021-02-13T00:40:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:40:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 20.72 2021-02-13T00:40:02Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:40:02Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:40:02Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:40:02Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:40:02Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:40:02Z- Filter 0 No beams used in subcloud update 2021-02-13T00:40:02Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:40:02Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:40:02Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 692.7 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:40:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176801.73 sec. 2021-02-13T00:40:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:40:02Z- TNavBF::mmse selected from filter -1 2021-02-13T00:40:02Z- variances([0], [2], and psi_berg) = 692.721203 832.994262 0.000000 2021-02-13T00:40:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:40:02Z- LcmTrn::publishEstimates() - MLE : 4059739.55 592955.18 13.27 2021-02-13T00:40:02Z- LcmTrn::publishEstimates() - MMSE : 4059740.05 592934.68 13.27 2021-02-13T00:40:02Z- LcmTrn::publishEstimates() - COVAR: 692.72 832.99 1.05 0.00 2021-02-13T00:40:02Z- AHRS_M2 msg: 1613176802.92 epoch sec; seqNo:14551 2021-02-13T00:40:02Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.16 psi 2021-02-13T00:40:02Z- handleDepth()-Depth_Keller msg: 1613176802933 epoch sec; seqNo:14668; depth 13.40 2021-02-13T00:40:02Z- handleDvl() - RDI_Pathfinder msg: 1613176802.95 epoch sec; seqNo:7061 2021-02-13T00:40:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.79 , 100.01 , 78.38 , 110.83 2021-02-13T00:40:02Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.01 , -0.12 2021-02-13T00:40:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176802.96 epoch sec; seqNo:14668; 4059740.06 north; 592935.90 east 2021-02-13T00:40:03Z- AHRS_M2 msg: 1613176803.33 epoch sec; seqNo:14552 2021-02-13T00:40:03Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-13T00:40:03Z- handleDepth()-Depth_Keller msg: 1613176803337 epoch sec; seqNo:14669; depth 13.51 2021-02-13T00:40:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176803.35 epoch sec; seqNo:14669; 4059740.44 north; 592935.90 east 2021-02-13T00:40:03Z- AHRS_M2 msg: 1613176803.73 epoch sec; seqNo:14553 2021-02-13T00:40:03Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-13T00:40:03Z- handleDepth()-Depth_Keller msg: 1613176803739 epoch sec; seqNo:14670; depth 13.62 2021-02-13T00:40:03Z- handleDvl() - RDI_Pathfinder msg: 1613176803.75 epoch sec; seqNo:7062 2021-02-13T00:40:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 99.00 , 75.60 , 111.17 2021-02-13T00:40:03Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.07 , -0.09 2021-02-13T00:40:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176803.76 epoch sec; seqNo:14670; 4059740.82 north; 592935.89 east 2021-02-13T00:40:04Z- AHRS_M2 msg: 1613176804.14 epoch sec; seqNo:14554 2021-02-13T00:40:04Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-13T00:40:04Z- handleDepth()-Depth_Keller msg: 1613176804145 epoch sec; seqNo:14671; depth 13.75 2021-02-13T00:40:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176804.17 epoch sec; seqNo:14671; 4059741.20 north; 592935.89 east 2021-02-13T00:40:04Z- AHRS_M2 msg: 1613176804.57 epoch sec; seqNo:14555 2021-02-13T00:40:04Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-13T00:40:04Z- handleDepth()-Depth_Keller msg: 1613176804573 epoch sec; seqNo:14672; depth 13.86 2021-02-13T00:40:04Z- handleDvl() - RDI_Pathfinder msg: 1613176804.58 epoch sec; seqNo:7063 2021-02-13T00:40:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.57 , 98.79 , 73.56 , 112.32 2021-02-13T00:40:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.05 , -0.02 2021-02-13T00:40:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176804.59 epoch sec; seqNo:14672; 4059741.58 north; 592935.89 east 2021-02-13T00:40:04Z- handleDepth()-Depth_Keller msg: 1613176804961 epoch sec; seqNo:14673; depth 13.99 2021-02-13T00:40:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176804.98 epoch sec; seqNo:14673; 4059741.96 north; 592935.88 east 2021-02-13T00:40:05Z- AHRS_M2 msg: 1613176805.36 epoch sec; seqNo:14556 2021-02-13T00:40:05Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:40:05Z- handleDepth()-Depth_Keller msg: 1613176805361 epoch sec; seqNo:14674; depth 14.11 2021-02-13T00:40:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176805.37 epoch sec; seqNo:14674; 4059742.35 north; 592937.10 east 2021-02-13T00:40:05Z- AHRS_M2 msg: 1613176805.75 epoch sec; seqNo:14557 2021-02-13T00:40:05Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-13T00:40:05Z- handleDepth()-Depth_Keller msg: 1613176805757 epoch sec; seqNo:14675; depth 14.23 2021-02-13T00:40:05Z- handleDvl() - RDI_Pathfinder msg: 1613176805.77 epoch sec; seqNo:7064 2021-02-13T00:40:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 99.07 , 74.37 , 111.23 2021-02-13T00:40:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , -0.04 2021-02-13T00:40:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176805.79 epoch sec; seqNo:14675; 4059742.35 north; 592937.10 east 2021-02-13T00:40:06Z- AHRS_M2 msg: 1613176806.16 epoch sec; seqNo:14558 2021-02-13T00:40:06Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-13T00:40:06Z- handleDepth()-Depth_Keller msg: 1613176806165 epoch sec; seqNo:14676; depth 14.34 2021-02-13T00:40:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176806.18 epoch sec; seqNo:14676; 4059742.73 north; 592937.09 east 2021-02-13T00:40:06Z- AHRS_M2 msg: 1613176806.56 epoch sec; seqNo:14559 2021-02-13T00:40:06Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.20 psi 2021-02-13T00:40:06Z- handleDepth()-Depth_Keller msg: 1613176806573 epoch sec; seqNo:14677; depth 14.46 2021-02-13T00:40:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176806.58 epoch sec; seqNo:14677; 4059743.11 north; 592937.09 east 2021-02-13T00:40:06Z- AHRS_M2 msg: 1613176806.96 epoch sec; seqNo:14560 2021-02-13T00:40:06Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.21 psi 2021-02-13T00:40:06Z- handleDepth()-Depth_Keller msg: 1613176806973 epoch sec; seqNo:14678; depth 14.58 2021-02-13T00:40:06Z- handleDvl() - RDI_Pathfinder msg: 1613176806.99 epoch sec; seqNo:7065 2021-02-13T00:40:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.22 , 100.15 , 78.51 , 112.32 2021-02-13T00:40:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , -0.11 2021-02-13T00:40:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176807.00 epoch sec; seqNo:14678; 4059743.48 north; 592937.09 east 2021-02-13T00:40:07Z- AHRS_M2 msg: 1613176807.37 epoch sec; seqNo:14561 2021-02-13T00:40:07Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.22 psi 2021-02-13T00:40:07Z- handleDepth()-Depth_Keller msg: 1613176807373 epoch sec; seqNo:14679; depth 14.70 2021-02-13T00:40:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176807.38 epoch sec; seqNo:14679; 4059743.48 north; 592937.09 east 2021-02-13T00:40:07Z- AHRS_M2 msg: 1613176807.77 epoch sec; seqNo:14562 2021-02-13T00:40:07Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.22 psi 2021-02-13T00:40:07Z- handleDepth()-Depth_Keller msg: 1613176807776 epoch sec; seqNo:14680; depth 14.82 2021-02-13T00:40:07Z- handleDvl() - RDI_Pathfinder msg: 1613176807.79 epoch sec; seqNo:7066 2021-02-13T00:40:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.07 , 101.70 , 73.15 , 112.52 2021-02-13T00:40:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.11 , -0.08 2021-02-13T00:40:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176807.80 epoch sec; seqNo:14680; 4059743.88 north; 592938.30 east 2021-02-13T00:40:07Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-13T00:40:07Z- waiting for 1613176805.54, time is 1613176807.80 2021-02-13T00:40:07Z- LcmTrn::updateTrn() equating pose 1613176807.80 and meas times 1613176807.80 2021-02-13T00:40:07Z- LcmTrn::updateTrn() >>>> pose time 1613176807.80, meas time 1613176807.80 <<<< 2021-02-13T00:40:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176807.80 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:40:07Z- TerrainNav::motionUpdate() - currEstimate = (4059744, 592938.30, 14.82) 2021-02-13T00:40:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:40:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:40:07Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176807.80 sec.; No relevant navigation data available 2021-02-13T00:40:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:40:07Z- TNavBF::mmse selected from filter -1 2021-02-13T00:40:07Z- variances([0], [2], and psi_berg) = 692.663695 832.994234 0.000000 2021-02-13T00:40:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:40:07Z- LcmTrn::publishEstimates() - MLE : 4059743.38 592958.80 14.82 2021-02-13T00:40:07Z- LcmTrn::publishEstimates() - MMSE : 4059743.88 592938.30 14.82 2021-02-13T00:40:07Z- LcmTrn::publishEstimates() - COVAR: 692.66 832.99 1.32 0.00 2021-02-13T00:40:08Z- AHRS_M2 msg: 1613176808.18 epoch sec; seqNo:14563 2021-02-13T00:40:08Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.22 psi 2021-02-13T00:40:08Z- handleDepth()-Depth_Keller msg: 1613176808183 epoch sec; seqNo:14681; depth 14.93 2021-02-13T00:40:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176808.19 epoch sec; seqNo:14681; 4059744.26 north; 592938.30 east 2021-02-13T00:40:08Z- AHRS_M2 msg: 1613176808.58 epoch sec; seqNo:14564 2021-02-13T00:40:08Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.22 psi 2021-02-13T00:40:08Z- handleDepth()-Depth_Keller msg: 1613176808584 epoch sec; seqNo:14682; depth 15.06 2021-02-13T00:40:08Z- handleDvl() - RDI_Pathfinder msg: 1613176808.60 epoch sec; seqNo:7067 2021-02-13T00:40:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.05 , 101.77 , 78.79 , 111.23 2021-02-13T00:40:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.17 , 0.05 2021-02-13T00:40:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176808.61 epoch sec; seqNo:14682; 4059744.63 north; 592938.29 east 2021-02-13T00:40:08Z- AHRS_M2 msg: 1613176808.98 epoch sec; seqNo:14565 2021-02-13T00:40:08Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.21 psi 2021-02-13T00:40:08Z- handleDepth()-Depth_Keller msg: 1613176808989 epoch sec; seqNo:14683; depth 15.18 2021-02-13T00:40:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176809.00 epoch sec; seqNo:14683; 4059745.01 north; 592938.29 east 2021-02-13T00:40:09Z- AHRS_M2 msg: 1613176809.39 epoch sec; seqNo:14566 2021-02-13T00:40:09Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.21 psi 2021-02-13T00:40:09Z- handleDepth()-Depth_Keller msg: 1613176809405 epoch sec; seqNo:14684; depth 15.32 2021-02-13T00:40:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176809.41 epoch sec; seqNo:14684; 4059745.01 north; 592938.29 east 2021-02-13T00:40:09Z- AHRS_M2 msg: 1613176809.79 epoch sec; seqNo:14567 2021-02-13T00:40:09Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.20 psi 2021-02-13T00:40:09Z- handleDepth()-Depth_Keller msg: 1613176809801 epoch sec; seqNo:14685; depth 15.43 2021-02-13T00:40:09Z- handleDvl() - RDI_Pathfinder msg: 1613176809.82 epoch sec; seqNo:7068 2021-02-13T00:40:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.14 , 101.77 , 78.79 , 109.88 2021-02-13T00:40:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , 0.17 2021-02-13T00:40:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176809.83 epoch sec; seqNo:14685; 4059745.39 north; 592938.29 east 2021-02-13T00:40:10Z- AHRS_M2 msg: 1613176810.20 epoch sec; seqNo:14568 2021-02-13T00:40:10Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.20 psi 2021-02-13T00:40:10Z- handleDepth()-Depth_Keller msg: 1613176810200 epoch sec; seqNo:14686; depth 15.56 2021-02-13T00:40:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176810.21 epoch sec; seqNo:14686; 4059745.77 north; 592938.28 east 2021-02-13T00:40:10Z- AHRS_M2 msg: 1613176810.60 epoch sec; seqNo:14569 2021-02-13T00:40:10Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.19 psi 2021-02-13T00:40:10Z- handleDepth()-Depth_Keller msg: 1613176810609 epoch sec; seqNo:14687; depth 15.66 2021-02-13T00:40:10Z- handleDvl() - RDI_Pathfinder msg: 1613176810.62 epoch sec; seqNo:7069 2021-02-13T00:40:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.07 , 99.00 , 73.15 , 108.46 2021-02-13T00:40:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.06 , 0.17 2021-02-13T00:40:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176810.63 epoch sec; seqNo:14687; 4059745.77 north; 592938.28 east 2021-02-13T00:40:11Z- AHRS_M2 msg: 1613176811.00 epoch sec; seqNo:14570 2021-02-13T00:40:11Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.19 psi 2021-02-13T00:40:11Z- handleDepth()-Depth_Keller msg: 1613176811017 epoch sec; seqNo:14688; depth 15.79 2021-02-13T00:40:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176811.03 epoch sec; seqNo:14688; 4059746.16 north; 592939.50 east 2021-02-13T00:40:11Z- AHRS_M2 msg: 1613176811.41 epoch sec; seqNo:14571 2021-02-13T00:40:11Z- AHRS_M2 msg: 0.56 phi; -0.33 theta; -0.19 psi 2021-02-13T00:40:11Z- handleDepth()-Depth_Keller msg: 1613176811412 epoch sec; seqNo:14689; depth 15.90 2021-02-13T00:40:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176811.43 epoch sec; seqNo:14689; 4059746.54 north; 592939.49 east 2021-02-13T00:40:11Z- AHRS_M2 msg: 1613176811.81 epoch sec; seqNo:14572 2021-02-13T00:40:11Z- AHRS_M2 msg: 0.56 phi; -0.33 theta; -0.20 psi 2021-02-13T00:40:11Z- handleDepth()-Depth_Keller msg: 1613176811821 epoch sec; seqNo:14690; depth 16.02 2021-02-13T00:40:11Z- handleDvl() - RDI_Pathfinder msg: 1613176811.83 epoch sec; seqNo:7070 2021-02-13T00:40:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 97.71 , 73.15 , 111.10 2021-02-13T00:40:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.14 , 0.07 2021-02-13T00:40:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176811.86 epoch sec; seqNo:14690; 4059746.54 north; 592939.49 east 2021-02-13T00:40:12Z- AHRS_M2 msg: 1613176812.22 epoch sec; seqNo:14573 2021-02-13T00:40:12Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.20 psi 2021-02-13T00:40:12Z- handleDepth()-Depth_Keller msg: 1613176812220 epoch sec; seqNo:14691; depth 16.14 2021-02-13T00:40:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176812.24 epoch sec; seqNo:14691; 4059746.92 north; 592939.49 east 2021-02-13T00:40:12Z- AHRS_M2 msg: 1613176812.62 epoch sec; seqNo:14574 2021-02-13T00:40:12Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.20 psi 2021-02-13T00:40:12Z- handleDepth()-Depth_Keller msg: 1613176812626 epoch sec; seqNo:14692; depth 16.24 2021-02-13T00:40:12Z- handleDvl() - RDI_Pathfinder msg: 1613176812.64 epoch sec; seqNo:7071 2021-02-13T00:40:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.46 , 98.93 , 73.15 , 109.88 2021-02-13T00:40:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.12 , -0.04 2021-02-13T00:40:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176812.65 epoch sec; seqNo:14692; 4059747.30 north; 592939.49 east 2021-02-13T00:40:12Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-13T00:40:12Z- waiting for 1613176810.80, time is 1613176812.65 2021-02-13T00:40:12Z- LcmTrn::updateTrn() equating pose 1613176812.65 and meas times 1613176812.65 2021-02-13T00:40:12Z- LcmTrn::updateTrn() >>>> pose time 1613176812.65, meas time 1613176812.65 <<<< 2021-02-13T00:40:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176812.65 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:40:12Z- TerrainNav::motionUpdate() - currEstimate = (4059747, 592939.49, 16.24) 2021-02-13T00:40:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:40:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:40:12Z- TNavBF::Projecting Measurements 2021-02-13T00:40:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:40:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:40:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:40:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:40:12Z- TNavFilter::4 good beams. 2021-02-13T00:40:12Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:40:12Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:40:12Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:40:12Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:40:12Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:40:12Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:40:12Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:40:12Z- TNavBF::mmse selected from filter -1 2021-02-13T00:40:12Z- TerrainMapOctree: (vehN, vehE ) = (4059747.30, 592939.49). 2021-02-13T00:40:12Z- TerrainMapOctree: Min Distance = 1993.88. 2021-02-13T00:40:12Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:40:12Z- There is no specific good beam that all particles have in common. 2021-02-13T00:40:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:40:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.50 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 10.10 2021-02-13T00:40:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:40:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 15.66 2021-02-13T00:40:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:40:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-13T00:40:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:40:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 19.32 2021-02-13T00:40:12Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:40:13Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:40:13Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:40:13Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:40:13Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:40:13Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:40:13Z- Filter 0 No beams used in subcloud update 2021-02-13T00:40:13Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:40:13Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:40:13Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 692.6 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:40:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176812.65 sec. 2021-02-13T00:40:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:40:13Z- TNavBF::mmse selected from filter -1 2021-02-13T00:40:13Z- variances([0], [2], and psi_berg) = 692.616047 832.994211 0.000000 2021-02-13T00:40:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:40:13Z- LcmTrn::publishEstimates() - MLE : 4059746.80 592959.99 16.24 2021-02-13T00:40:13Z- LcmTrn::publishEstimates() - MMSE : 4059747.30 592939.49 16.24 2021-02-13T00:40:13Z- LcmTrn::publishEstimates() - COVAR: 692.62 832.99 1.05 0.00 2021-02-13T00:40:13Z- AHRS_M2 msg: 1613176813.02 epoch sec; seqNo:14575 2021-02-13T00:40:13Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.20 psi 2021-02-13T00:40:13Z- handleDepth()-Depth_Keller msg: 1613176813033 epoch sec; seqNo:14693; depth 16.36 2021-02-13T00:40:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176813.05 epoch sec; seqNo:14693; 4059747.30 north; 592939.49 east 2021-02-13T00:40:13Z- AHRS_M2 msg: 1613176813.43 epoch sec; seqNo:14576 2021-02-13T00:40:13Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.20 psi 2021-02-13T00:40:13Z- handleDepth()-Depth_Keller msg: 1613176813437 epoch sec; seqNo:14694; depth 16.48 2021-02-13T00:40:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176813.44 epoch sec; seqNo:14694; 4059747.69 north; 592940.70 east 2021-02-13T00:40:13Z- AHRS_M2 msg: 1613176813.83 epoch sec; seqNo:14577 2021-02-13T00:40:13Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.20 psi 2021-02-13T00:40:13Z- handleDepth()-Depth_Keller msg: 1613176813836 epoch sec; seqNo:14695; depth 16.59 2021-02-13T00:40:13Z- handleDvl() - RDI_Pathfinder msg: 1613176813.85 epoch sec; seqNo:7072 2021-02-13T00:40:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.46 , 100.15 , 72.33 , 108.66 2021-02-13T00:40:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.03 , -0.08 2021-02-13T00:40:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176813.86 epoch sec; seqNo:14695; 4059748.07 north; 592940.70 east 2021-02-13T00:40:14Z- AHRS_M2 msg: 1613176814.24 epoch sec; seqNo:14578 2021-02-13T00:40:14Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.19 psi 2021-02-13T00:40:14Z- handleDepth()-Depth_Keller msg: 1613176814242 epoch sec; seqNo:14696; depth 16.70 2021-02-13T00:40:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176814.25 epoch sec; seqNo:14696; 4059748.45 north; 592940.69 east 2021-02-13T00:40:14Z- AHRS_M2 msg: 1613176814.64 epoch sec; seqNo:14579 2021-02-13T00:40:14Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.19 psi 2021-02-13T00:40:14Z- handleDepth()-Depth_Keller msg: 1613176814649 epoch sec; seqNo:14697; depth 16.84 2021-02-13T00:40:14Z- handleDvl() - RDI_Pathfinder msg: 1613176814.67 epoch sec; seqNo:7073 2021-02-13T00:40:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 97.78 , 71.92 , 108.73 2021-02-13T00:40:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.08 , -0.05 2021-02-13T00:40:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176814.68 epoch sec; seqNo:14697; 4059748.83 north; 592940.69 east 2021-02-13T00:40:15Z- AHRS_M2 msg: 1613176815.04 epoch sec; seqNo:14580 2021-02-13T00:40:15Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.19 psi 2021-02-13T00:40:15Z- handleDepth()-Depth_Keller msg: 1613176815048 epoch sec; seqNo:14698; depth 16.94 2021-02-13T00:40:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176815.06 epoch sec; seqNo:14698; 4059748.83 north; 592940.69 east 2021-02-13T00:40:15Z- AHRS_M2 msg: 1613176815.45 epoch sec; seqNo:14581 2021-02-13T00:40:15Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.19 psi 2021-02-13T00:40:15Z- handleDepth()-Depth_Keller msg: 1613176815457 epoch sec; seqNo:14699; depth 17.06 2021-02-13T00:40:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176815.46 epoch sec; seqNo:14699; 4059749.21 north; 592940.69 east 2021-02-13T00:40:15Z- AHRS_M2 msg: 1613176815.85 epoch sec; seqNo:14582 2021-02-13T00:40:15Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.18 psi 2021-02-13T00:40:15Z- handleDepth()-Depth_Keller msg: 1613176815865 epoch sec; seqNo:14700; depth 17.19 2021-02-13T00:40:15Z- handleDvl() - RDI_Pathfinder msg: 1613176815.88 epoch sec; seqNo:7074 2021-02-13T00:40:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.60 , 97.85 , 78.38 , 107.58 2021-02-13T00:40:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.09 , 0.01 2021-02-13T00:40:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176815.89 epoch sec; seqNo:14700; 4059749.60 north; 592941.90 east 2021-02-13T00:40:16Z- AHRS_M2 msg: 1613176816.26 epoch sec; seqNo:14583 2021-02-13T00:40:16Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.18 psi 2021-02-13T00:40:16Z- handleDepth()-Depth_Keller msg: 1613176816265 epoch sec; seqNo:14701; depth 17.29 2021-02-13T00:40:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176816.28 epoch sec; seqNo:14701; 4059749.98 north; 592941.90 east 2021-02-13T00:40:16Z- AHRS_M2 msg: 1613176816.67 epoch sec; seqNo:14584 2021-02-13T00:40:16Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.18 psi 2021-02-13T00:40:16Z- handleDepth()-Depth_Keller msg: 1613176816677 epoch sec; seqNo:14702; depth 17.41 2021-02-13T00:40:16Z- handleDvl() - RDI_Pathfinder msg: 1613176816.69 epoch sec; seqNo:7075 2021-02-13T00:40:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.46 , 97.71 , 78.25 , 106.23 2021-02-13T00:40:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.10 , 0.05 2021-02-13T00:40:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176816.70 epoch sec; seqNo:14702; 4059750.36 north; 592941.89 east 2021-02-13T00:40:17Z- AHRS_M2 msg: 1613176817.06 epoch sec; seqNo:14585 2021-02-13T00:40:17Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.18 psi 2021-02-13T00:40:17Z- handleDepth()-Depth_Keller msg: 1613176817068 epoch sec; seqNo:14703; depth 17.53 2021-02-13T00:40:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176817.08 epoch sec; seqNo:14703; 4059750.74 north; 592941.89 east 2021-02-13T00:40:17Z- AHRS_M2 msg: 1613176817.47 epoch sec; seqNo:14586 2021-02-13T00:40:17Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-13T00:40:17Z- handleDepth()-Depth_Keller msg: 1613176817472 epoch sec; seqNo:14704; depth 17.64 2021-02-13T00:40:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176817.48 epoch sec; seqNo:14704; 4059751.11 north; 592941.89 east 2021-02-13T00:40:17Z- AHRS_M2 msg: 1613176817.88 epoch sec; seqNo:14587 2021-02-13T00:40:17Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.17 psi 2021-02-13T00:40:17Z- handleDepth()-Depth_Keller msg: 1613176817879 epoch sec; seqNo:14705; depth 17.75 2021-02-13T00:40:17Z- handleDvl() - RDI_Pathfinder msg: 1613176817.89 epoch sec; seqNo:7076 2021-02-13T00:40:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.46 , 97.71 , 78.25 , 106.23 2021-02-13T00:40:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.09 , 0.07 2021-02-13T00:40:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176817.92 epoch sec; seqNo:14705; 4059751.49 north; 592941.88 east 2021-02-13T00:40:18Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-13T00:40:18Z- waiting for 1613176815.65, time is 1613176817.92 2021-02-13T00:40:18Z- LcmTrn::updateTrn() equating pose 1613176817.92 and meas times 1613176817.92 2021-02-13T00:40:18Z- LcmTrn::updateTrn() >>>> pose time 1613176817.92, meas time 1613176817.92 <<<< 2021-02-13T00:40:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176817.92 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:40:18Z- TerrainNav::motionUpdate() - currEstimate = (4059751, 592941.88, 17.75) 2021-02-13T00:40:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:40:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:40:18Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176817.92 sec.; No relevant navigation data available 2021-02-13T00:40:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:40:18Z- TNavBF::mmse selected from filter -1 2021-02-13T00:40:18Z- variances([0], [2], and psi_berg) = 692.561050 832.994185 0.000000 2021-02-13T00:40:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:40:18Z- LcmTrn::publishEstimates() - MLE : 4059750.99 592962.38 17.75 2021-02-13T00:40:18Z- LcmTrn::publishEstimates() - MMSE : 4059751.49 592941.88 17.75 2021-02-13T00:40:18Z- LcmTrn::publishEstimates() - COVAR: 692.56 832.99 1.30 0.00 2021-02-13T00:40:18Z- AHRS_M2 msg: 1613176818.28 epoch sec; seqNo:14588 2021-02-13T00:40:18Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.17 psi 2021-02-13T00:40:18Z- handleDepth()-Depth_Keller msg: 1613176818282 epoch sec; seqNo:14706; depth 17.86 2021-02-13T00:40:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176818.30 epoch sec; seqNo:14706; 4059751.49 north; 592941.88 east 2021-02-13T00:40:18Z- AHRS_M2 msg: 1613176818.68 epoch sec; seqNo:14589 2021-02-13T00:40:18Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.17 psi 2021-02-13T00:40:18Z- handleDepth()-Depth_Keller msg: 1613176818685 epoch sec; seqNo:14707; depth 17.98 2021-02-13T00:40:18Z- handleDvl() - RDI_Pathfinder msg: 1613176818.70 epoch sec; seqNo:7077 2021-02-13T00:40:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.42 , 94.06 , 78.25 , 105.01 2021-02-13T00:40:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.05 , 0.06 2021-02-13T00:40:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176818.71 epoch sec; seqNo:14707; 4059751.89 north; 592943.10 east 2021-02-13T00:40:19Z- AHRS_M2 msg: 1613176819.08 epoch sec; seqNo:14590 2021-02-13T00:40:19Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.17 psi 2021-02-13T00:40:19Z- handleDepth()-Depth_Keller msg: 1613176819088 epoch sec; seqNo:14708; depth 18.08 2021-02-13T00:40:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176819.10 epoch sec; seqNo:14708; 4059752.26 north; 592943.09 east 2021-02-13T00:40:19Z- AHRS_M2 msg: 1613176819.49 epoch sec; seqNo:14591 2021-02-13T00:40:19Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.16 psi 2021-02-13T00:40:19Z- handleDepth()-Depth_Keller msg: 1613176819501 epoch sec; seqNo:14709; depth 18.20 2021-02-13T00:40:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176819.51 epoch sec; seqNo:14709; 4059752.64 north; 592943.09 east 2021-02-13T00:40:19Z- AHRS_M2 msg: 1613176819.89 epoch sec; seqNo:14592 2021-02-13T00:40:19Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.15 psi 2021-02-13T00:40:19Z- handleDepth()-Depth_Keller msg: 1613176819910 epoch sec; seqNo:14710; depth 18.31 2021-02-13T00:40:19Z- handleDvl() - RDI_Pathfinder msg: 1613176819.92 epoch sec; seqNo:7078 2021-02-13T00:40:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 97.92 , 78.45 , 105.22 2021-02-13T00:40:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.03 , 0.04 2021-02-13T00:40:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176819.93 epoch sec; seqNo:14710; 4059752.64 north; 592943.09 east 2021-02-13T00:40:20Z- AHRS_M2 msg: 1613176820.30 epoch sec; seqNo:14593 2021-02-13T00:40:20Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.15 psi 2021-02-13T00:40:20Z- handleDepth()-Depth_Keller msg: 1613176820300 epoch sec; seqNo:14711; depth 18.42 2021-02-13T00:40:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176820.31 epoch sec; seqNo:14711; 4059753.02 north; 592943.09 east 2021-02-13T00:40:20Z- AHRS_M2 msg: 1613176820.70 epoch sec; seqNo:14594 2021-02-13T00:40:20Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.15 psi 2021-02-13T00:40:20Z- handleDepth()-Depth_Keller msg: 1613176820705 epoch sec; seqNo:14712; depth 18.54 2021-02-13T00:40:20Z- handleDvl() - RDI_Pathfinder msg: 1613176820.72 epoch sec; seqNo:7079 2021-02-13T00:40:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 97.78 , 70.29 , 106.30 2021-02-13T00:40:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.06 , -0.00 2021-02-13T00:40:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176820.73 epoch sec; seqNo:14712; 4059753.40 north; 592943.08 east 2021-02-13T00:40:21Z- AHRS_M2 msg: 1613176821.10 epoch sec; seqNo:14595 2021-02-13T00:40:21Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.14 psi 2021-02-13T00:40:21Z- handleDepth()-Depth_Keller msg: 1613176821119 epoch sec; seqNo:14713; depth 18.66 2021-02-13T00:40:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176821.14 epoch sec; seqNo:14713; 4059753.79 north; 592944.30 east 2021-02-13T00:40:21Z- AHRS_M2 msg: 1613176821.52 epoch sec; seqNo:14596 2021-02-13T00:40:21Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.15 psi 2021-02-13T00:40:21Z- handleDepth()-Depth_Keller msg: 1613176821536 epoch sec; seqNo:14714; depth 18.76 2021-02-13T00:40:21Z- handleDvl() - RDI_Pathfinder msg: 1613176821.55 epoch sec; seqNo:7080 2021-02-13T00:40:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 97.92 , 69.06 , 105.22 2021-02-13T00:40:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , -0.05 2021-02-13T00:40:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176821.56 epoch sec; seqNo:14714; 4059753.79 north; 592944.30 east 2021-02-13T00:40:21Z- AHRS_M2 msg: 1613176821.91 epoch sec; seqNo:14597 2021-02-13T00:40:21Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.15 psi 2021-02-13T00:40:21Z- handleDepth()-Depth_Keller msg: 1613176821917 epoch sec; seqNo:14715; depth 18.88 2021-02-13T00:40:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176821.93 epoch sec; seqNo:14715; 4059754.17 north; 592944.29 east 2021-02-13T00:40:22Z- AHRS_M2 msg: 1613176822.32 epoch sec; seqNo:14598 2021-02-13T00:40:22Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.16 psi 2021-02-13T00:40:22Z- handleDepth()-Depth_Keller msg: 1613176822328 epoch sec; seqNo:14716; depth 19.00 2021-02-13T00:40:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176822.34 epoch sec; seqNo:14716; 4059754.55 north; 592944.29 east 2021-02-13T00:40:22Z- AHRS_M2 msg: 1613176822.72 epoch sec; seqNo:14599 2021-02-13T00:40:22Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.17 psi 2021-02-13T00:40:22Z- handleDepth()-Depth_Keller msg: 1613176822728 epoch sec; seqNo:14717; depth 19.11 2021-02-13T00:40:22Z- handleDvl() - RDI_Pathfinder msg: 1613176822.75 epoch sec; seqNo:7081 2021-02-13T00:40:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.98 , 69.88 , 104.07 2021-02-13T00:40:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , -0.08 2021-02-13T00:40:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176822.76 epoch sec; seqNo:14717; 4059754.93 north; 592944.29 east 2021-02-13T00:40:23Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-13T00:40:23Z- waiting for 1613176820.92, time is 1613176822.76 2021-02-13T00:40:23Z- LcmTrn::updateTrn() equating pose 1613176822.76 and meas times 1613176822.76 2021-02-13T00:40:23Z- LcmTrn::updateTrn() >>>> pose time 1613176822.76, meas time 1613176822.76 <<<< 2021-02-13T00:40:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176822.76 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:40:23Z- TerrainNav::motionUpdate() - currEstimate = (4059755, 592944.29, 19.11) 2021-02-13T00:40:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:40:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:40:23Z- TNavBF::Projecting Measurements 2021-02-13T00:40:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:40:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:40:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:40:23Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:40:23Z- TNavFilter::3 good beams. 2021-02-13T00:40:23Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:40:23Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:40:23Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:40:23Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:40:23Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:40:23Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:40:23Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:40:23Z- TNavBF::mmse selected from filter -1 2021-02-13T00:40:23Z- TerrainMapOctree: (vehN, vehE ) = (4059754.93, 592944.29). 2021-02-13T00:40:23Z- TerrainMapOctree: Min Distance = 1984.98. 2021-02-13T00:40:23Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:40:23Z- There is no specific good beam that all particles have in common. 2021-02-13T00:40:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:40:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.51 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:40:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:40:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.39 K: 0.127 ExpectedMeasDiff: nan currMeas.covariance: 7.81 2021-02-13T00:40:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:40:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 17.33 2021-02-13T00:40:23Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:40:23Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:40:23Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:40:23Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:40:23Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:40:23Z- Filter 0 No beams used in subcloud update 2021-02-13T00:40:23Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:40:23Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:40:23Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 692.5 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:40:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176822.76 sec. 2021-02-13T00:40:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:40:23Z- TNavBF::mmse selected from filter -1 2021-02-13T00:40:23Z- variances([0], [2], and psi_berg) = 692.509832 832.994160 0.000000 2021-02-13T00:40:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:40:23Z- LcmTrn::publishEstimates() - MLE : 4059754.43 592964.79 19.11 2021-02-13T00:40:23Z- LcmTrn::publishEstimates() - MMSE : 4059754.93 592944.29 19.11 2021-02-13T00:40:23Z- LcmTrn::publishEstimates() - COVAR: 692.51 832.99 1.14 0.00 2021-02-13T00:40:23Z- AHRS_M2 msg: 1613176823.13 epoch sec; seqNo:14600 2021-02-13T00:40:23Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.17 psi 2021-02-13T00:40:23Z- handleDepth()-Depth_Keller msg: 1613176823132 epoch sec; seqNo:14718; depth 19.22 2021-02-13T00:40:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176823.16 epoch sec; seqNo:14718; 4059754.93 north; 592944.29 east 2021-02-13T00:40:23Z- AHRS_M2 msg: 1613176823.53 epoch sec; seqNo:14601 2021-02-13T00:40:23Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-13T00:40:23Z- handleDepth()-Depth_Keller msg: 1613176823532 epoch sec; seqNo:14719; depth 19.35 2021-02-13T00:40:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176823.54 epoch sec; seqNo:14719; 4059755.32 north; 592945.50 east 2021-02-13T00:40:23Z- AHRS_M2 msg: 1613176823.94 epoch sec; seqNo:14602 2021-02-13T00:40:23Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-13T00:40:23Z- handleDepth()-Depth_Keller msg: 1613176823951 epoch sec; seqNo:14720; depth 19.46 2021-02-13T00:40:23Z- handleDvl() - RDI_Pathfinder msg: 1613176823.97 epoch sec; seqNo:7082 2021-02-13T00:40:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.25 , 97.71 , 69.47 , 105.01 2021-02-13T00:40:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.10 , -0.02 2021-02-13T00:40:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176823.98 epoch sec; seqNo:14720; 4059755.70 north; 592945.50 east 2021-02-13T00:40:24Z- AHRS_M2 msg: 1613176824.34 epoch sec; seqNo:14603 2021-02-13T00:40:24Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-13T00:40:24Z- handleDepth()-Depth_Keller msg: 1613176824340 epoch sec; seqNo:14721; depth 19.59 2021-02-13T00:40:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176824.36 epoch sec; seqNo:14721; 4059756.08 north; 592945.49 east 2021-02-13T00:40:24Z- AHRS_M2 msg: 1613176824.74 epoch sec; seqNo:14604 2021-02-13T00:40:24Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-13T00:40:24Z- handleDepth()-Depth_Keller msg: 1613176824744 epoch sec; seqNo:14722; depth 19.71 2021-02-13T00:40:24Z- handleDvl() - RDI_Pathfinder msg: 1613176824.76 epoch sec; seqNo:7083 2021-02-13T00:40:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 93.12 , 69.47 , 104.07 2021-02-13T00:40:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , 0.05 2021-02-13T00:40:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176824.78 epoch sec; seqNo:14722; 4059756.46 north; 592945.49 east 2021-02-13T00:40:25Z- AHRS_M2 msg: 1613176825.15 epoch sec; seqNo:14605 2021-02-13T00:40:25Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-13T00:40:25Z- handleDepth()-Depth_Keller msg: 1613176825154 epoch sec; seqNo:14723; depth 19.83 2021-02-13T00:40:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176825.16 epoch sec; seqNo:14723; 4059756.46 north; 592945.49 east 2021-02-13T00:40:25Z- AHRS_M2 msg: 1613176825.55 epoch sec; seqNo:14606 2021-02-13T00:40:25Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-13T00:40:25Z- handleDepth()-Depth_Keller msg: 1613176825553 epoch sec; seqNo:14724; depth 19.93 2021-02-13T00:40:25Z- handleDvl() - RDI_Pathfinder msg: 1613176825.57 epoch sec; seqNo:7084 2021-02-13T00:40:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.19 , 94.20 , 68.24 , 105.15 2021-02-13T00:40:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , 0.09 2021-02-13T00:40:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176825.58 epoch sec; seqNo:14724; 4059756.84 north; 592945.49 east 2021-02-13T00:40:25Z- AHRS_M2 msg: 1613176825.95 epoch sec; seqNo:14607 2021-02-13T00:40:25Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-13T00:40:25Z- handleDepth()-Depth_Keller msg: 1613176825957 epoch sec; seqNo:14725; depth 20.07 2021-02-13T00:40:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176825.97 epoch sec; seqNo:14725; 4059757.22 north; 592945.48 east 2021-02-13T00:40:26Z- AHRS_M2 msg: 1613176826.36 epoch sec; seqNo:14608 2021-02-13T00:40:26Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:40:26Z- handleDepth()-Depth_Keller msg: 1613176826365 epoch sec; seqNo:14726; depth 20.16 2021-02-13T00:40:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176826.37 epoch sec; seqNo:14726; 4059757.23 north; 592946.70 east 2021-02-13T00:40:26Z- AHRS_M2 msg: 1613176826.76 epoch sec; seqNo:14609 2021-02-13T00:40:26Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-13T00:40:26Z- handleDepth()-Depth_Keller msg: 1613176826773 epoch sec; seqNo:14727; depth 20.29 2021-02-13T00:40:26Z- handleDvl() - RDI_Pathfinder msg: 1613176826.79 epoch sec; seqNo:7085 2021-02-13T00:40:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 93.25 , 69.06 , 105.42 2021-02-13T00:40:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.09 2021-02-13T00:40:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176826.80 epoch sec; seqNo:14727; 4059757.61 north; 592946.70 east 2021-02-13T00:40:27Z- AHRS_M2 msg: 1613176827.16 epoch sec; seqNo:14610 2021-02-13T00:40:27Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-13T00:40:27Z- handleDepth()-Depth_Keller msg: 1613176827168 epoch sec; seqNo:14728; depth 20.41 2021-02-13T00:40:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176827.18 epoch sec; seqNo:14728; 4059757.99 north; 592946.69 east 2021-02-13T00:40:27Z- AHRS_M2 msg: 1613176827.59 epoch sec; seqNo:14611 2021-02-13T00:40:27Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.18 psi 2021-02-13T00:40:27Z- handleDepth()-Depth_Keller msg: 1613176827596 epoch sec; seqNo:14729; depth 20.52 2021-02-13T00:40:27Z- handleDvl() - RDI_Pathfinder msg: 1613176827.61 epoch sec; seqNo:7086 2021-02-13T00:40:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 69.06 , 104.14 2021-02-13T00:40:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , 0.07 2021-02-13T00:40:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176827.62 epoch sec; seqNo:14729; 4059758.37 north; 592946.69 east 2021-02-13T00:40:27Z- AHRS_M2 msg: 1613176827.97 epoch sec; seqNo:14612 2021-02-13T00:40:27Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.18 psi 2021-02-13T00:40:27Z- handleDepth()-Depth_Keller msg: 1613176827978 epoch sec; seqNo:14730; depth 20.67 2021-02-13T00:40:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176827.99 epoch sec; seqNo:14730; 4059758.37 north; 592946.69 east 2021-02-13T00:40:28Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:40:28Z- waiting for 1613176825.76, time is 1613176827.99 2021-02-13T00:40:28Z- LcmTrn::updateTrn() >>>> pose time 1613176827.99, meas time 1613176827.61 <<<< 2021-02-13T00:40:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176827.61 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:40:28Z- TerrainNav::motionUpdate() - currEstimate = (4059758, 592946.69, 20.67) 2021-02-13T00:40:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:40:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:40:28Z- TNavBF::Projecting Measurements 2021-02-13T00:40:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:40:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:40:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:40:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:40:28Z- TNavFilter::4 good beams. 2021-02-13T00:40:28Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:40:28Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:40:28Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:40:28Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:40:28Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:40:28Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:40:28Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:40:28Z- TNavBF::mmse selected from filter -1 2021-02-13T00:40:28Z- TerrainMapOctree: (vehN, vehE ) = (4059758.12, 592946.52). 2021-02-13T00:40:28Z- TerrainMapOctree: Min Distance = 1981.11. 2021-02-13T00:40:28Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:40:28Z- There is no specific good beam that all particles have in common. 2021-02-13T00:40:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:40:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-13T00:40:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:40:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 15.38 2021-02-13T00:40:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:40:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-13T00:40:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:40:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 17.35 2021-02-13T00:40:28Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:40:28Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:40:28Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:40:28Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:40:28Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:40:28Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:40:28Z- Filter 0 No beams used in subcloud update 2021-02-13T00:40:28Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:40:28Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:40:28Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 692.5 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:40:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176827.61 sec. 2021-02-13T00:40:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:40:28Z- TNavBF::mmse selected from filter -1 2021-02-13T00:40:28Z- variances([0], [2], and psi_berg) = 692.446801 832.994129 0.000000 2021-02-13T00:40:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:40:28Z- LcmTrn::publishEstimates() - MLE : 4059757.87 592967.19 20.67 2021-02-13T00:40:28Z- LcmTrn::publishEstimates() - MMSE : 4059758.37 592946.69 20.67 2021-02-13T00:40:28Z- LcmTrn::publishEstimates() - COVAR: 692.45 832.99 0.97 0.00 2021-02-13T00:40:28Z- AHRS_M2 msg: 1613176828.38 epoch sec; seqNo:14613 2021-02-13T00:40:28Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.17 psi 2021-02-13T00:40:28Z- handleDepth()-Depth_Keller msg: 1613176828380 epoch sec; seqNo:14731; depth 20.77 2021-02-13T00:40:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176828.40 epoch sec; seqNo:14731; 4059758.74 north; 592946.69 east 2021-02-13T00:40:28Z- AHRS_M2 msg: 1613176828.78 epoch sec; seqNo:14614 2021-02-13T00:40:28Z- AHRS_M2 msg: 0.66 phi; -0.35 theta; -0.17 psi 2021-02-13T00:40:28Z- handleDepth()-Depth_Keller msg: 1613176828789 epoch sec; seqNo:14732; depth 20.89 2021-02-13T00:40:28Z- handleDvl() - RDI_Pathfinder msg: 1613176828.80 epoch sec; seqNo:7087 2021-02-13T00:40:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 91.90 , 68.24 , 104.07 2021-02-13T00:40:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.03 , 0.03 2021-02-13T00:40:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176828.82 epoch sec; seqNo:14732; 4059759.14 north; 592947.90 east 2021-02-13T00:40:29Z- AHRS_M2 msg: 1613176829.18 epoch sec; seqNo:14615 2021-02-13T00:40:29Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.17 psi 2021-02-13T00:40:29Z- handleDepth()-Depth_Keller msg: 1613176829188 epoch sec; seqNo:14733; depth 21.03 2021-02-13T00:40:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176829.20 epoch sec; seqNo:14733; 4059759.14 north; 592947.90 east 2021-02-13T00:40:29Z- AHRS_M2 msg: 1613176829.59 epoch sec; seqNo:14616 2021-02-13T00:40:29Z- AHRS_M2 msg: 0.67 phi; -0.35 theta; -0.18 psi 2021-02-13T00:40:29Z- handleDepth()-Depth_Keller msg: 1613176829599 epoch sec; seqNo:14734; depth 21.15 2021-02-13T00:40:29Z- handleDvl() - RDI_Pathfinder msg: 1613176829.61 epoch sec; seqNo:7088 2021-02-13T00:40:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 98.12 , 68.24 , 105.42 2021-02-13T00:40:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , 0.01 2021-02-13T00:40:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176829.62 epoch sec; seqNo:14734; 4059759.52 north; 592947.90 east 2021-02-13T00:40:29Z- AHRS_M2 msg: 1613176829.99 epoch sec; seqNo:14617 2021-02-13T00:40:29Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.18 psi 2021-02-13T00:40:30Z- handleDepth()-Depth_Keller msg: 1613176829996 epoch sec; seqNo:14735; depth 21.29 2021-02-13T00:40:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176830.02 epoch sec; seqNo:14735; 4059759.89 north; 592947.89 east 2021-02-13T00:40:30Z- AHRS_M2 msg: 1613176830.40 epoch sec; seqNo:14618 2021-02-13T00:40:30Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.18 psi 2021-02-13T00:40:30Z- handleDepth()-Depth_Keller msg: 1613176830405 epoch sec; seqNo:14736; depth 21.41 2021-02-13T00:40:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176830.41 epoch sec; seqNo:14736; 4059760.27 north; 592947.89 east 2021-02-13T00:40:30Z- AHRS_M2 msg: 1613176830.80 epoch sec; seqNo:14619 2021-02-13T00:40:30Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.19 psi 2021-02-13T00:40:30Z- handleDepth()-Depth_Keller msg: 1613176830809 epoch sec; seqNo:14737; depth 21.54 2021-02-13T00:40:30Z- handleDvl() - RDI_Pathfinder msg: 1613176830.82 epoch sec; seqNo:7089 2021-02-13T00:40:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 66.20 , 106.50 2021-02-13T00:40:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.01 , -0.01 2021-02-13T00:40:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176830.83 epoch sec; seqNo:14737; 4059760.29 north; 592949.11 east 2021-02-13T00:40:31Z- AHRS_M2 msg: 1613176831.20 epoch sec; seqNo:14620 2021-02-13T00:40:31Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.19 psi 2021-02-13T00:40:31Z- handleDepth()-Depth_Keller msg: 1613176831217 epoch sec; seqNo:14738; depth 21.67 2021-02-13T00:40:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176831.23 epoch sec; seqNo:14738; 4059760.66 north; 592949.11 east 2021-02-13T00:40:31Z- AHRS_M2 msg: 1613176831.61 epoch sec; seqNo:14621 2021-02-13T00:40:31Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.19 psi 2021-02-13T00:40:31Z- handleDepth()-Depth_Keller msg: 1613176831612 epoch sec; seqNo:14739; depth 21.81 2021-02-13T00:40:31Z- handleDvl() - RDI_Pathfinder msg: 1613176831.63 epoch sec; seqNo:7090 2021-02-13T00:40:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.78 , 97.98 , 0.00 , 105.28 2021-02-13T00:40:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , -0.03 2021-02-13T00:40:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176831.64 epoch sec; seqNo:14739; 4059761.04 north; 592949.10 east 2021-02-13T00:40:32Z- AHRS_M2 msg: 1613176832.01 epoch sec; seqNo:14622 2021-02-13T00:40:32Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.20 psi 2021-02-13T00:40:32Z- handleDepth()-Depth_Keller msg: 1613176832016 epoch sec; seqNo:14740; depth 21.95 2021-02-13T00:40:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176832.03 epoch sec; seqNo:14740; 4059761.42 north; 592949.10 east 2021-02-13T00:40:32Z- AHRS_M2 msg: 1613176832.42 epoch sec; seqNo:14623 2021-02-13T00:40:32Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.20 psi 2021-02-13T00:40:32Z- handleDepth()-Depth_Keller msg: 1613176832420 epoch sec; seqNo:14741; depth 22.08 2021-02-13T00:40:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176832.43 epoch sec; seqNo:14741; 4059761.42 north; 592949.10 east 2021-02-13T00:40:32Z- AHRS_M2 msg: 1613176832.82 epoch sec; seqNo:14624 2021-02-13T00:40:32Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.21 psi 2021-02-13T00:40:32Z- handleDepth()-Depth_Keller msg: 1613176832830 epoch sec; seqNo:14742; depth 22.20 2021-02-13T00:40:32Z- handleDvl() - RDI_Pathfinder msg: 1613176832.84 epoch sec; seqNo:7091 2021-02-13T00:40:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 97.65 , 67.43 , 107.11 2021-02-13T00:40:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.03 , -0.06 2021-02-13T00:40:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176832.85 epoch sec; seqNo:14742; 4059761.81 north; 592950.31 east 2021-02-13T00:40:33Z- AHRS_M2 msg: 1613176833.22 epoch sec; seqNo:14625 2021-02-13T00:40:33Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.21 psi 2021-02-13T00:40:33Z- handleDepth()-Depth_Keller msg: 1613176833233 epoch sec; seqNo:14743; depth 22.35 2021-02-13T00:40:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176833.25 epoch sec; seqNo:14743; 4059762.19 north; 592950.31 east 2021-02-13T00:40:33Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-13T00:40:33Z- waiting for 1613176830.99, time is 1613176833.25 2021-02-13T00:40:33Z- LcmTrn::updateTrn() >>>> pose time 1613176833.25, meas time 1613176832.84 <<<< 2021-02-13T00:40:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176832.84 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:40:33Z- TerrainNav::motionUpdate() - currEstimate = (4059762, 592950.31, 22.35) 2021-02-13T00:40:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:40:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:40:33Z- TNavBF::Projecting Measurements 2021-02-13T00:40:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:40:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:40:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:40:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:40:33Z- TNavFilter::4 good beams. 2021-02-13T00:40:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:40:33Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:40:33Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:40:33Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:40:33Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:40:33Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:40:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:40:33Z- TNavBF::mmse selected from filter -1 2021-02-13T00:40:33Z- TerrainMapOctree: (vehN, vehE ) = (4059761.89, 592950.03). 2021-02-13T00:40:33Z- TerrainMapOctree: Min Distance = 1975.95. 2021-02-13T00:40:33Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:40:33Z- There is no specific good beam that all particles have in common. 2021-02-13T00:40:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:40:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 8.75 2021-02-13T00:40:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:40:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.26 2021-02-13T00:40:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:40:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 7.27 2021-02-13T00:40:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:40:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 18.36 2021-02-13T00:40:33Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:40:33Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:40:33Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:40:33Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:40:33Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:40:33Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:40:33Z- Filter 0 No beams used in subcloud update 2021-02-13T00:40:33Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:40:33Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:40:33Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 692.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:40:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176832.84 sec. 2021-02-13T00:40:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:40:33Z- TNavBF::mmse selected from filter -1 2021-02-13T00:40:33Z- variances([0], [2], and psi_berg) = 692.375717 832.994095 0.000000 2021-02-13T00:40:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:40:33Z- LcmTrn::publishEstimates() - MLE : 4059761.69 592971.81 22.35 2021-02-13T00:40:33Z- LcmTrn::publishEstimates() - MMSE : 4059762.19 592950.31 22.35 2021-02-13T00:40:33Z- LcmTrn::publishEstimates() - COVAR: 692.38 832.99 0.90 0.00 2021-02-13T00:40:33Z- AHRS_M2 msg: 1613176833.63 epoch sec; seqNo:14626 2021-02-13T00:40:33Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.22 psi 2021-02-13T00:40:33Z- handleDepth()-Depth_Keller msg: 1613176833637 epoch sec; seqNo:14744; depth 22.48 2021-02-13T00:40:33Z- handleDvl() - RDI_Pathfinder msg: 1613176833.65 epoch sec; seqNo:7092 2021-02-13T00:40:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 67.02 , 106.57 2021-02-13T00:40:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , -0.08 2021-02-13T00:40:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176833.67 epoch sec; seqNo:14744; 4059762.57 north; 592950.31 east 2021-02-13T00:40:34Z- AHRS_M2 msg: 1613176834.03 epoch sec; seqNo:14627 2021-02-13T00:40:34Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.23 psi 2021-02-13T00:40:34Z- handleDepth()-Depth_Keller msg: 1613176834036 epoch sec; seqNo:14745; depth 22.63 2021-02-13T00:40:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176834.05 epoch sec; seqNo:14745; 4059762.95 north; 592950.30 east 2021-02-13T00:40:34Z- AHRS_M2 msg: 1613176834.44 epoch sec; seqNo:14628 2021-02-13T00:40:34Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.24 psi 2021-02-13T00:40:34Z- handleDepth()-Depth_Keller msg: 1613176834440 epoch sec; seqNo:14746; depth 22.78 2021-02-13T00:40:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176834.45 epoch sec; seqNo:14746; 4059762.95 north; 592950.30 east 2021-02-13T00:40:34Z- AHRS_M2 msg: 1613176834.84 epoch sec; seqNo:14629 2021-02-13T00:40:34Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.24 psi 2021-02-13T00:40:34Z- handleDepth()-Depth_Keller msg: 1613176834844 epoch sec; seqNo:14747; depth 22.90 2021-02-13T00:40:34Z- handleDvl() - RDI_Pathfinder msg: 1613176834.86 epoch sec; seqNo:7093 2021-02-13T00:40:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 97.98 , 67.43 , 106.50 2021-02-13T00:40:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , -0.07 2021-02-13T00:40:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176834.88 epoch sec; seqNo:14747; 4059763.33 north; 592950.30 east 2021-02-13T00:40:35Z- AHRS_M2 msg: 1613176835.25 epoch sec; seqNo:14630 2021-02-13T00:40:35Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.24 psi 2021-02-13T00:40:35Z- handleDepth()-Depth_Keller msg: 1613176835252 epoch sec; seqNo:14748; depth 23.04 2021-02-13T00:40:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176835.26 epoch sec; seqNo:14748; 4059763.72 north; 592951.51 east 2021-02-13T00:40:35Z- AHRS_M2 msg: 1613176835.65 epoch sec; seqNo:14631 2021-02-13T00:40:35Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.24 psi 2021-02-13T00:40:35Z- handleDepth()-Depth_Keller msg: 1613176835654 epoch sec; seqNo:14749; depth 23.17 2021-02-13T00:40:35Z- handleDvl() - RDI_Pathfinder msg: 1613176835.67 epoch sec; seqNo:7094 2021-02-13T00:40:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 98.12 , 67.43 , 105.42 2021-02-13T00:40:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , -0.02 2021-02-13T00:40:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176835.68 epoch sec; seqNo:14749; 4059764.10 north; 592951.51 east 2021-02-13T00:40:36Z- AHRS_M2 msg: 1613176836.05 epoch sec; seqNo:14632 2021-02-13T00:40:36Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.25 psi 2021-02-13T00:40:36Z- handleDepth()-Depth_Keller msg: 1613176836057 epoch sec; seqNo:14750; depth 23.32 2021-02-13T00:40:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176836.08 epoch sec; seqNo:14750; 4059764.10 north; 592951.51 east 2021-02-13T00:40:36Z- AHRS_M2 msg: 1613176836.47 epoch sec; seqNo:14633 2021-02-13T00:40:36Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.24 psi 2021-02-13T00:40:36Z- handleDepth()-Depth_Keller msg: 1613176836471 epoch sec; seqNo:14751; depth 23.45 2021-02-13T00:40:36Z- handleDvl() - RDI_Pathfinder msg: 1613176836.49 epoch sec; seqNo:7095 2021-02-13T00:40:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.51 , 98.12 , 66.20 , 106.64 2021-02-13T00:40:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.06 , 0.02 2021-02-13T00:40:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176836.50 epoch sec; seqNo:14751; 4059764.48 north; 592951.51 east 2021-02-13T00:40:36Z- AHRS_M2 msg: 1613176836.87 epoch sec; seqNo:14634 2021-02-13T00:40:36Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.24 psi 2021-02-13T00:40:36Z- handleDepth()-Depth_Keller msg: 1613176836874 epoch sec; seqNo:14752; depth 23.60 2021-02-13T00:40:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176836.89 epoch sec; seqNo:14752; 4059764.86 north; 592951.50 east 2021-02-13T00:40:37Z- AHRS_M2 msg: 1613176837.27 epoch sec; seqNo:14635 2021-02-13T00:40:37Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.24 psi 2021-02-13T00:40:37Z- handleDepth()-Depth_Keller msg: 1613176837283 epoch sec; seqNo:14753; depth 23.73 2021-02-13T00:40:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176837.29 epoch sec; seqNo:14753; 4059765.24 north; 592951.50 east 2021-02-13T00:40:37Z- AHRS_M2 msg: 1613176837.67 epoch sec; seqNo:14636 2021-02-13T00:40:37Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.23 psi 2021-02-13T00:40:37Z- handleDepth()-Depth_Keller msg: 1613176837673 epoch sec; seqNo:14754; depth 23.86 2021-02-13T00:40:37Z- handleDvl() - RDI_Pathfinder msg: 1613176837.68 epoch sec; seqNo:7096 2021-02-13T00:40:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 93.25 , 65.38 , 104.20 2021-02-13T00:40:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.06 2021-02-13T00:40:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176837.71 epoch sec; seqNo:14754; 4059765.24 north; 592951.50 east 2021-02-13T00:40:38Z- AHRS_M2 msg: 1613176838.07 epoch sec; seqNo:14637 2021-02-13T00:40:38Z- AHRS_M2 msg: 0.57 phi; -0.36 theta; -0.23 psi 2021-02-13T00:40:38Z- handleDepth()-Depth_Keller msg: 1613176838077 epoch sec; seqNo:14755; depth 23.97 2021-02-13T00:40:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176838.09 epoch sec; seqNo:14755; 4059765.62 north; 592951.49 east 2021-02-13T00:40:38Z- LcmTrn::handleMessages() - handling 41 messages took 0.65 ms and 128 calls... 2021-02-13T00:40:38Z- waiting for 1613176836.25, time is 1613176838.09 2021-02-13T00:40:38Z- LcmTrn::updateTrn() >>>> pose time 1613176838.09, meas time 1613176837.68 <<<< 2021-02-13T00:40:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176837.68 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:40:38Z- TerrainNav::motionUpdate() - currEstimate = (4059766, 592951.49, 23.97) 2021-02-13T00:40:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:40:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:40:38Z- TNavBF::Projecting Measurements 2021-02-13T00:40:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:40:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:40:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:40:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:40:38Z- TNavFilter::4 good beams. 2021-02-13T00:40:38Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:40:38Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:40:38Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:40:38Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:40:38Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:40:38Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:40:38Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:40:38Z- TNavBF::mmse selected from filter -1 2021-02-13T00:40:38Z- TerrainMapOctree: (vehN, vehE ) = (4059765.33, 592951.40). 2021-02-13T00:40:38Z- TerrainMapOctree: Min Distance = 1972.46. 2021-02-13T00:40:38Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:40:38Z- There is no specific good beam that all particles have in common. 2021-02-13T00:40:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:40:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 9.90 2021-02-13T00:40:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:40:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 13.91 2021-02-13T00:40:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:40:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-13T00:40:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:40:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 17.37 2021-02-13T00:40:38Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:40:38Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:40:38Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:40:38Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:40:38Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:40:38Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:40:38Z- Filter 0 No beams used in subcloud update 2021-02-13T00:40:38Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:40:38Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:40:38Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 692.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:40:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176837.68 sec. 2021-02-13T00:40:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:40:38Z- TNavBF::mmse selected from filter -1 2021-02-13T00:40:38Z- variances([0], [2], and psi_berg) = 692.316529 832.994066 0.000000 2021-02-13T00:40:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:40:38Z- LcmTrn::publishEstimates() - MLE : 4059765.12 592972.99 23.97 2021-02-13T00:40:38Z- LcmTrn::publishEstimates() - MMSE : 4059765.62 592951.49 23.97 2021-02-13T00:40:38Z- LcmTrn::publishEstimates() - COVAR: 692.32 832.99 0.81 0.00 2021-02-13T00:40:38Z- AHRS_M2 msg: 1613176838.48 epoch sec; seqNo:14638 2021-02-13T00:40:38Z- AHRS_M2 msg: 0.57 phi; -0.36 theta; -0.22 psi 2021-02-13T00:40:38Z- handleDepth()-Depth_Keller msg: 1613176838486 epoch sec; seqNo:14756; depth 24.11 2021-02-13T00:40:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176838.51 epoch sec; seqNo:14756; 4059766.01 north; 592952.71 east 2021-02-13T00:40:38Z- AHRS_M2 msg: 1613176838.88 epoch sec; seqNo:14639 2021-02-13T00:40:38Z- AHRS_M2 msg: 0.57 phi; -0.36 theta; -0.22 psi 2021-02-13T00:40:38Z- handleDepth()-Depth_Keller msg: 1613176838885 epoch sec; seqNo:14757; depth 24.26 2021-02-13T00:40:38Z- handleDvl() - RDI_Pathfinder msg: 1613176838.91 epoch sec; seqNo:7097 2021-02-13T00:40:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 98.19 , 64.16 , 104.27 2021-02-13T00:40:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , 0.06 2021-02-13T00:40:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176838.92 epoch sec; seqNo:14757; 4059766.01 north; 592952.71 east 2021-02-13T00:40:39Z- AHRS_M2 msg: 1613176839.28 epoch sec; seqNo:14640 2021-02-13T00:40:39Z- AHRS_M2 msg: 0.57 phi; -0.36 theta; -0.22 psi 2021-02-13T00:40:39Z- handleDepth()-Depth_Keller msg: 1613176839293 epoch sec; seqNo:14758; depth 24.40 2021-02-13T00:40:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176839.30 epoch sec; seqNo:14758; 4059766.39 north; 592952.71 east 2021-02-13T00:40:39Z- AHRS_M2 msg: 1613176839.69 epoch sec; seqNo:14641 2021-02-13T00:40:39Z- AHRS_M2 msg: 0.57 phi; -0.35 theta; -0.21 psi 2021-02-13T00:40:39Z- handleDepth()-Depth_Keller msg: 1613176839705 epoch sec; seqNo:14759; depth 24.51 2021-02-13T00:40:39Z- handleDvl() - RDI_Pathfinder msg: 1613176839.72 epoch sec; seqNo:7098 2021-02-13T00:40:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 98.05 , 64.97 , 100.49 2021-02-13T00:40:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.09 , 0.03 2021-02-13T00:40:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176839.74 epoch sec; seqNo:14759; 4059766.77 north; 592952.70 east 2021-02-13T00:40:40Z- AHRS_M2 msg: 1613176840.10 epoch sec; seqNo:14642 2021-02-13T00:40:40Z- AHRS_M2 msg: 0.57 phi; -0.35 theta; -0.21 psi 2021-02-13T00:40:40Z- handleDepth()-Depth_Keller msg: 1613176840105 epoch sec; seqNo:14760; depth 24.67 2021-02-13T00:40:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176840.12 epoch sec; seqNo:14760; 4059766.77 north; 592952.70 east 2021-02-13T00:40:40Z- AHRS_M2 msg: 1613176840.50 epoch sec; seqNo:14643 2021-02-13T00:40:40Z- AHRS_M2 msg: 0.57 phi; -0.35 theta; -0.20 psi 2021-02-13T00:40:40Z- handleDepth()-Depth_Keller msg: 1613176840505 epoch sec; seqNo:14761; depth 24.78 2021-02-13T00:40:40Z- handleDvl() - RDI_Pathfinder msg: 1613176840.52 epoch sec; seqNo:7099 2021-02-13T00:40:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 0.00 , 64.97 , 100.75 2021-02-13T00:40:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , 0.02 2021-02-13T00:40:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176840.53 epoch sec; seqNo:14761; 4059767.15 north; 592952.70 east 2021-02-13T00:40:40Z- AHRS_M2 msg: 1613176840.90 epoch sec; seqNo:14644 2021-02-13T00:40:40Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.20 psi 2021-02-13T00:40:40Z- handleDepth()-Depth_Keller msg: 1613176840904 epoch sec; seqNo:14762; depth 24.93 2021-02-13T00:40:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176840.91 epoch sec; seqNo:14762; 4059767.54 north; 592953.91 east 2021-02-13T00:40:41Z- AHRS_M2 msg: 1613176841.30 epoch sec; seqNo:14645 2021-02-13T00:40:41Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.20 psi 2021-02-13T00:40:41Z- handleDepth()-Depth_Keller msg: 1613176841309 epoch sec; seqNo:14763; depth 25.04 2021-02-13T00:40:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176841.32 epoch sec; seqNo:14763; 4059767.92 north; 592953.91 east 2021-02-13T00:40:41Z- AHRS_M2 msg: 1613176841.71 epoch sec; seqNo:14646 2021-02-13T00:40:41Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.19 psi 2021-02-13T00:40:41Z- handleDepth()-Depth_Keller msg: 1613176841714 epoch sec; seqNo:14764; depth 25.17 2021-02-13T00:40:41Z- handleDvl() - RDI_Pathfinder msg: 1613176841.73 epoch sec; seqNo:7100 2021-02-13T00:40:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 0.00 , 63.75 , 100.15 2021-02-13T00:40:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.02 , -0.01 2021-02-13T00:40:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176841.74 epoch sec; seqNo:14764; 4059767.92 north; 592953.91 east 2021-02-13T00:40:42Z- AHRS_M2 msg: 1613176842.11 epoch sec; seqNo:14647 2021-02-13T00:40:42Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-13T00:40:42Z- handleDepth()-Depth_Keller msg: 1613176842120 epoch sec; seqNo:14765; depth 25.30 2021-02-13T00:40:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176842.13 epoch sec; seqNo:14765; 4059768.29 north; 592953.91 east 2021-02-13T00:40:42Z- AHRS_M2 msg: 1613176842.52 epoch sec; seqNo:14648 2021-02-13T00:40:42Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.18 psi 2021-02-13T00:40:42Z- handleDepth()-Depth_Keller msg: 1613176842520 epoch sec; seqNo:14766; depth 25.44 2021-02-13T00:40:42Z- handleDvl() - RDI_Pathfinder msg: 1613176842.53 epoch sec; seqNo:7101 2021-02-13T00:40:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 0.00 , 62.93 , 99.07 2021-02-13T00:40:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.09 , -0.01 2021-02-13T00:40:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176842.54 epoch sec; seqNo:14766; 4059768.67 north; 592953.90 east 2021-02-13T00:40:42Z- AHRS_M2 msg: 1613176842.92 epoch sec; seqNo:14649 2021-02-13T00:40:42Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:40:42Z- handleDepth()-Depth_Keller msg: 1613176842927 epoch sec; seqNo:14767; depth 25.57 2021-02-13T00:40:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176842.94 epoch sec; seqNo:14767; 4059769.05 north; 592953.90 east 2021-02-13T00:40:43Z- AHRS_M2 msg: 1613176843.32 epoch sec; seqNo:14650 2021-02-13T00:40:43Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.17 psi 2021-02-13T00:40:43Z- handleDepth()-Depth_Keller msg: 1613176843333 epoch sec; seqNo:14768; depth 25.69 2021-02-13T00:40:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176843.35 epoch sec; seqNo:14768; 4059769.43 north; 592953.89 east 2021-02-13T00:40:43Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-13T00:40:43Z- waiting for 1613176841.09, time is 1613176843.35 2021-02-13T00:40:43Z- LcmTrn::updateTrn() >>>> pose time 1613176843.35, meas time 1613176842.53 <<<< 2021-02-13T00:40:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176842.53 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:40:43Z- TerrainNav::motionUpdate() - currEstimate = (4059769, 592953.89, 25.69) 2021-02-13T00:40:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:40:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:40:43Z- TNavBF::Projecting Measurements 2021-02-13T00:40:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:40:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:40:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:40:43Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:40:43Z- TNavFilter::3 good beams. 2021-02-13T00:40:43Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:40:43Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:40:43Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:40:43Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:40:43Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:40:43Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:40:43Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:40:43Z- TNavBF::mmse selected from filter -1 2021-02-13T00:40:43Z- TerrainMapOctree: (vehN, vehE ) = (4059768.84, 592953.52). 2021-02-13T00:40:43Z- TerrainMapOctree: Min Distance = 1968.42. 2021-02-13T00:40:43Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:40:43Z- There is no specific good beam that all particles have in common. 2021-02-13T00:40:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:40:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 7.90 2021-02-13T00:40:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:40:43Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 6.34 2021-02-13T00:40:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:40:43Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 15.70 2021-02-13T00:40:43Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:40:43Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:40:43Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:40:43Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:40:43Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:40:43Z- Filter 0 No beams used in subcloud update 2021-02-13T00:40:43Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:40:43Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:40:43Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 692.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:40:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176842.53 sec. 2021-02-13T00:40:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:40:43Z- TNavBF::mmse selected from filter -1 2021-02-13T00:40:43Z- variances([0], [2], and psi_berg) = 692.246994 832.994032 0.000000 2021-02-13T00:40:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:40:43Z- LcmTrn::publishEstimates() - MLE : 4059768.93 592975.39 25.69 2021-02-13T00:40:43Z- LcmTrn::publishEstimates() - MMSE : 4059769.43 592953.89 25.69 2021-02-13T00:40:43Z- LcmTrn::publishEstimates() - COVAR: 692.25 832.99 0.82 0.00 2021-02-13T00:40:43Z- AHRS_M2 msg: 1613176843.73 epoch sec; seqNo:14651 2021-02-13T00:40:43Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.16 psi 2021-02-13T00:40:43Z- handleDepth()-Depth_Keller msg: 1613176843732 epoch sec; seqNo:14769; depth 25.81 2021-02-13T00:40:43Z- handleDvl() - RDI_Pathfinder msg: 1613176843.74 epoch sec; seqNo:7102 2021-02-13T00:40:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 87.17 , 62.52 , 97.98 2021-02-13T00:40:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.07 , -0.03 2021-02-13T00:40:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176843.77 epoch sec; seqNo:14769; 4059769.82 north; 592955.11 east 2021-02-13T00:40:44Z- AHRS_M2 msg: 1613176844.13 epoch sec; seqNo:14652 2021-02-13T00:40:44Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.16 psi 2021-02-13T00:40:44Z- handleDepth()-Depth_Keller msg: 1613176844141 epoch sec; seqNo:14770; depth 25.95 2021-02-13T00:40:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176844.15 epoch sec; seqNo:14770; 4059769.82 north; 592955.11 east 2021-02-13T00:40:44Z- AHRS_M2 msg: 1613176844.54 epoch sec; seqNo:14653 2021-02-13T00:40:44Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.15 psi 2021-02-13T00:40:44Z- handleDepth()-Depth_Keller msg: 1613176844542 epoch sec; seqNo:14771; depth 26.07 2021-02-13T00:40:44Z- handleDvl() - RDI_Pathfinder msg: 1613176844.55 epoch sec; seqNo:7103 2021-02-13T00:40:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 86.09 , 63.34 , 97.98 2021-02-13T00:40:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , -0.02 2021-02-13T00:40:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176844.56 epoch sec; seqNo:14771; 4059770.20 north; 592955.11 east 2021-02-13T00:40:44Z- AHRS_M2 msg: 1613176844.94 epoch sec; seqNo:14654 2021-02-13T00:40:44Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.15 psi 2021-02-13T00:40:44Z- handleDepth()-Depth_Keller msg: 1613176844944 epoch sec; seqNo:14772; depth 26.19 2021-02-13T00:40:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176844.96 epoch sec; seqNo:14772; 4059770.58 north; 592955.10 east 2021-02-13T00:40:45Z- AHRS_M2 msg: 1613176845.34 epoch sec; seqNo:14655 2021-02-13T00:40:45Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.15 psi 2021-02-13T00:40:45Z- handleDepth()-Depth_Keller msg: 1613176845353 epoch sec; seqNo:14773; depth 26.33 2021-02-13T00:40:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176845.36 epoch sec; seqNo:14773; 4059770.96 north; 592955.10 east 2021-02-13T00:40:45Z- AHRS_M2 msg: 1613176845.75 epoch sec; seqNo:14656 2021-02-13T00:40:45Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.15 psi 2021-02-13T00:40:45Z- handleDepth()-Depth_Keller msg: 1613176845752 epoch sec; seqNo:14774; depth 26.44 2021-02-13T00:40:45Z- handleDvl() - RDI_Pathfinder msg: 1613176845.76 epoch sec; seqNo:7104 2021-02-13T00:40:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 83.92 , 63.34 , 97.98 2021-02-13T00:40:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , -0.01 2021-02-13T00:40:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176845.77 epoch sec; seqNo:14774; 4059770.96 north; 592955.10 east 2021-02-13T00:40:46Z- AHRS_M2 msg: 1613176846.16 epoch sec; seqNo:14657 2021-02-13T00:40:46Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.15 psi 2021-02-13T00:40:46Z- handleDepth()-Depth_Keller msg: 1613176846160 epoch sec; seqNo:14775; depth 26.56 2021-02-13T00:40:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176846.17 epoch sec; seqNo:14775; 4059771.35 north; 592956.31 east 2021-02-13T00:40:46Z- AHRS_M2 msg: 1613176846.56 epoch sec; seqNo:14658 2021-02-13T00:40:46Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.15 psi 2021-02-13T00:40:46Z- handleDepth()-Depth_Keller msg: 1613176846560 epoch sec; seqNo:14776; depth 26.67 2021-02-13T00:40:46Z- handleDvl() - RDI_Pathfinder msg: 1613176846.58 epoch sec; seqNo:7105 2021-02-13T00:40:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 82.84 , 62.52 , 97.98 2021-02-13T00:40:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , -0.02 2021-02-13T00:40:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176846.59 epoch sec; seqNo:14776; 4059771.73 north; 592956.31 east 2021-02-13T00:40:46Z- AHRS_M2 msg: 1613176846.96 epoch sec; seqNo:14659 2021-02-13T00:40:46Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.15 psi 2021-02-13T00:40:46Z- handleDepth()-Depth_Keller msg: 1613176846964 epoch sec; seqNo:14777; depth 26.79 2021-02-13T00:40:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176846.97 epoch sec; seqNo:14777; 4059772.11 north; 592956.31 east 2021-02-13T00:40:47Z- AHRS_M2 msg: 1613176847.39 epoch sec; seqNo:14660 2021-02-13T00:40:47Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.16 psi 2021-02-13T00:40:47Z- handleDepth()-Depth_Keller msg: 1613176847390 epoch sec; seqNo:14778; depth 26.91 2021-02-13T00:40:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176847.41 epoch sec; seqNo:14778; 4059772.11 north; 592956.31 east 2021-02-13T00:40:47Z- AHRS_M2 msg: 1613176847.77 epoch sec; seqNo:14661 2021-02-13T00:40:47Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.16 psi 2021-02-13T00:40:47Z- handleDepth()-Depth_Keller msg: 1613176847774 epoch sec; seqNo:14779; depth 27.03 2021-02-13T00:40:47Z- handleDvl() - RDI_Pathfinder msg: 1613176847.79 epoch sec; seqNo:7106 2021-02-13T00:40:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 83.92 , 62.53 , 97.98 2021-02-13T00:40:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , -0.01 2021-02-13T00:40:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176847.80 epoch sec; seqNo:14779; 4059772.49 north; 592956.30 east 2021-02-13T00:40:48Z- AHRS_M2 msg: 1613176848.17 epoch sec; seqNo:14662 2021-02-13T00:40:48Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.17 psi 2021-02-13T00:40:48Z- handleDepth()-Depth_Keller msg: 1613176848181 epoch sec; seqNo:14780; depth 27.14 2021-02-13T00:40:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176848.19 epoch sec; seqNo:14780; 4059772.87 north; 592956.30 east 2021-02-13T00:40:48Z- AHRS_M2 msg: 1613176848.58 epoch sec; seqNo:14663 2021-02-13T00:40:48Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-13T00:40:48Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-13T00:40:48Z- waiting for 1613176846.35, time is 1613176848.58 2021-02-13T00:40:48Z- LcmTrn::updateTrn() >>>> pose time 1613176848.58, meas time 1613176847.79 <<<< 2021-02-13T00:40:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176847.79 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:40:48Z- TerrainNav::motionUpdate() - currEstimate = (4059773, 592956.30, 27.14) 2021-02-13T00:40:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:40:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:40:48Z- TNavBF::Projecting Measurements 2021-02-13T00:40:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:40:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:40:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:40:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:40:48Z- TNavFilter::4 good beams. 2021-02-13T00:40:48Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:40:48Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:40:48Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:40:48Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:40:48Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:40:48Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:40:48Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:40:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:40:48Z- TerrainMapOctree: (vehN, vehE ) = (4059772.35, 592955.93). 2021-02-13T00:40:48Z- TerrainMapOctree: Min Distance = 1964.19. 2021-02-13T00:40:48Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:40:48Z- There is no specific good beam that all particles have in common. 2021-02-13T00:40:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:40:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-13T00:40:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:40:48Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 11.27 2021-02-13T00:40:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:40:48Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 6.26 2021-02-13T00:40:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:40:48Z- Old Depth Bias: 0.00 DepthBiasCov: 0.76 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:40:48Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:40:48Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:40:48Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:40:48Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:40:48Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:40:48Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:40:48Z- Filter 0 No beams used in subcloud update 2021-02-13T00:40:48Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:40:48Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:40:48Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 692.2 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:40:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176847.79 sec. 2021-02-13T00:40:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:40:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:40:48Z- variances([0], [2], and psi_berg) = 692.180126 832.994000 0.000000 2021-02-13T00:40:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:40:48Z- LcmTrn::publishEstimates() - MLE : 4059772.37 592976.80 27.14 2021-02-13T00:40:48Z- LcmTrn::publishEstimates() - MMSE : 4059772.87 592956.30 27.14 2021-02-13T00:40:48Z- LcmTrn::publishEstimates() - COVAR: 692.18 832.99 0.76 0.00 2021-02-13T00:40:48Z- handleDepth()-Depth_Keller msg: 1613176848585 epoch sec; seqNo:14781; depth 27.26 2021-02-13T00:40:48Z- handleDvl() - RDI_Pathfinder msg: 1613176848.60 epoch sec; seqNo:7107 2021-02-13T00:40:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.66 , 83.92 , 62.94 , 97.98 2021-02-13T00:40:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , -0.02 2021-02-13T00:40:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176848.61 epoch sec; seqNo:14781; 4059773.26 north; 592957.51 east 2021-02-13T00:40:48Z- AHRS_M2 msg: 1613176848.98 epoch sec; seqNo:14664 2021-02-13T00:40:48Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-13T00:40:48Z- handleDepth()-Depth_Keller msg: 1613176848993 epoch sec; seqNo:14782; depth 27.38 2021-02-13T00:40:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176849.00 epoch sec; seqNo:14782; 4059773.26 north; 592957.51 east 2021-02-13T00:40:49Z- AHRS_M2 msg: 1613176849.38 epoch sec; seqNo:14665 2021-02-13T00:40:49Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-13T00:40:49Z- handleDepth()-Depth_Keller msg: 1613176849389 epoch sec; seqNo:14783; depth 27.50 2021-02-13T00:40:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176849.40 epoch sec; seqNo:14783; 4059773.64 north; 592957.51 east 2021-02-13T00:40:49Z- AHRS_M2 msg: 1613176849.79 epoch sec; seqNo:14666 2021-02-13T00:40:49Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-13T00:40:49Z- handleDepth()-Depth_Keller msg: 1613176849795 epoch sec; seqNo:14784; depth 27.61 2021-02-13T00:40:49Z- handleDvl() - RDI_Pathfinder msg: 1613176849.81 epoch sec; seqNo:7108 2021-02-13T00:40:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 86.08 , 61.30 , 97.98 2021-02-13T00:40:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , -0.01 2021-02-13T00:40:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176849.83 epoch sec; seqNo:14784; 4059774.02 north; 592957.51 east 2021-02-13T00:40:50Z- AHRS_M2 msg: 1613176850.20 epoch sec; seqNo:14667 2021-02-13T00:40:50Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-13T00:40:50Z- handleDepth()-Depth_Keller msg: 1613176850201 epoch sec; seqNo:14785; depth 27.76 2021-02-13T00:40:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176850.21 epoch sec; seqNo:14785; 4059774.40 north; 592957.50 east 2021-02-13T00:40:50Z- AHRS_M2 msg: 1613176850.60 epoch sec; seqNo:14668 2021-02-13T00:40:50Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-13T00:40:50Z- handleDepth()-Depth_Keller msg: 1613176850602 epoch sec; seqNo:14786; depth 27.86 2021-02-13T00:40:50Z- handleDvl() - RDI_Pathfinder msg: 1613176850.61 epoch sec; seqNo:7109 2021-02-13T00:40:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 84.80 , 62.53 , 97.78 2021-02-13T00:40:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , 0.03 2021-02-13T00:40:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176850.62 epoch sec; seqNo:14786; 4059774.40 north; 592957.50 east 2021-02-13T00:40:51Z- AHRS_M2 msg: 1613176851.00 epoch sec; seqNo:14669 2021-02-13T00:40:51Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-13T00:40:51Z- handleDepth()-Depth_Keller msg: 1613176851009 epoch sec; seqNo:14787; depth 27.99 2021-02-13T00:40:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176851.04 epoch sec; seqNo:14787; 4059774.79 north; 592958.72 east 2021-02-13T00:40:51Z- AHRS_M2 msg: 1613176851.40 epoch sec; seqNo:14670 2021-02-13T00:40:51Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-13T00:40:51Z- handleDepth()-Depth_Keller msg: 1613176851408 epoch sec; seqNo:14788; depth 28.12 2021-02-13T00:40:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176851.42 epoch sec; seqNo:14788; 4059775.17 north; 592958.71 east 2021-02-13T00:40:51Z- AHRS_M2 msg: 1613176851.81 epoch sec; seqNo:14671 2021-02-13T00:40:51Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-13T00:40:51Z- handleDepth()-Depth_Keller msg: 1613176851817 epoch sec; seqNo:14789; depth 28.23 2021-02-13T00:40:51Z- handleDvl() - RDI_Pathfinder msg: 1613176851.83 epoch sec; seqNo:7110 2021-02-13T00:40:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 85.00 , 61.30 , 93.66 2021-02-13T00:40:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , 0.05 2021-02-13T00:40:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176851.84 epoch sec; seqNo:14789; 4059775.55 north; 592958.71 east 2021-02-13T00:40:52Z- AHRS_M2 msg: 1613176852.21 epoch sec; seqNo:14672 2021-02-13T00:40:52Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-13T00:40:52Z- handleDepth()-Depth_Keller msg: 1613176852222 epoch sec; seqNo:14790; depth 28.34 2021-02-13T00:40:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176852.23 epoch sec; seqNo:14790; 4059775.55 north; 592958.71 east 2021-02-13T00:40:52Z- AHRS_M2 msg: 1613176852.62 epoch sec; seqNo:14673 2021-02-13T00:40:52Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-13T00:40:52Z- handleDepth()-Depth_Keller msg: 1613176852625 epoch sec; seqNo:14791; depth 28.47 2021-02-13T00:40:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176852.66 epoch sec; seqNo:14791; 4059775.92 north; 592958.71 east 2021-02-13T00:40:53Z- AHRS_M2 msg: 1613176853.02 epoch sec; seqNo:14674 2021-02-13T00:40:53Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-13T00:40:53Z- handleDepth()-Depth_Keller msg: 1613176853032 epoch sec; seqNo:14792; depth 28.58 2021-02-13T00:40:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176853.04 epoch sec; seqNo:14792; 4059776.30 north; 592958.70 east 2021-02-13T00:40:53Z- AHRS_M2 msg: 1613176853.43 epoch sec; seqNo:14675 2021-02-13T00:40:53Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-13T00:40:53Z- handleDepth()-Depth_Keller msg: 1613176853433 epoch sec; seqNo:14793; depth 28.70 2021-02-13T00:40:53Z- handleDvl() - RDI_Pathfinder msg: 1613176853.44 epoch sec; seqNo:7111 2021-02-13T00:40:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.02 , 0.00 , 61.71 , 97.98 2021-02-13T00:40:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , 0.08 2021-02-13T00:40:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176853.45 epoch sec; seqNo:14793; 4059776.32 north; 592959.92 east 2021-02-13T00:40:53Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-13T00:40:53Z- waiting for 1613176851.58, time is 1613176853.45 2021-02-13T00:40:53Z- LcmTrn::updateTrn() equating pose 1613176853.45 and meas times 1613176853.45 2021-02-13T00:40:53Z- LcmTrn::updateTrn() >>>> pose time 1613176853.45, meas time 1613176853.45 <<<< 2021-02-13T00:40:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176853.45 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:40:53Z- TerrainNav::motionUpdate() - currEstimate = (4059776, 592959.92, 28.70) 2021-02-13T00:40:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:40:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:40:53Z- TNavBF::Projecting Measurements 2021-02-13T00:40:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:40:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:40:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:40:53Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:40:53Z- TNavFilter::3 good beams. 2021-02-13T00:40:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:40:53Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:40:53Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:40:53Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:40:53Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:40:53Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:40:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:40:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:40:53Z- TerrainMapOctree: (vehN, vehE ) = (4059776.32, 592959.92). 2021-02-13T00:40:53Z- TerrainMapOctree: Min Distance = 1958.58. 2021-02-13T00:40:53Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:40:53Z- There is no specific good beam that all particles have in common. 2021-02-13T00:40:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:40:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 7.19 2021-02-13T00:40:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:40:53Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 6.09 2021-02-13T00:40:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:40:53Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:40:53Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:40:53Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:40:53Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:40:53Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:40:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:40:53Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:40:53Z- Filter 0 No beams used in subcloud update 2021-02-13T00:40:53Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:40:53Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:40:53Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 692.1 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:40:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176853.45 sec. 2021-02-13T00:40:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:40:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:40:53Z- variances([0], [2], and psi_berg) = 692.124404 832.993973 0.000000 2021-02-13T00:40:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:40:53Z- LcmTrn::publishEstimates() - MLE : 4059775.82 592980.42 28.70 2021-02-13T00:40:53Z- LcmTrn::publishEstimates() - MMSE : 4059776.32 592959.92 28.70 2021-02-13T00:40:53Z- LcmTrn::publishEstimates() - COVAR: 692.12 832.99 0.78 0.00 2021-02-13T00:40:53Z- AHRS_M2 msg: 1613176853.83 epoch sec; seqNo:14676 2021-02-13T00:40:53Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-13T00:40:53Z- handleDepth()-Depth_Keller msg: 1613176853832 epoch sec; seqNo:14794; depth 28.83 2021-02-13T00:40:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176853.85 epoch sec; seqNo:14794; 4059776.70 north; 592959.92 east 2021-02-13T00:40:54Z- AHRS_M2 msg: 1613176854.23 epoch sec; seqNo:14677 2021-02-13T00:40:54Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-13T00:40:54Z- handleDepth()-Depth_Keller msg: 1613176854241 epoch sec; seqNo:14795; depth 28.93 2021-02-13T00:40:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176854.25 epoch sec; seqNo:14795; 4059777.07 north; 592959.91 east 2021-02-13T00:40:54Z- AHRS_M2 msg: 1613176854.64 epoch sec; seqNo:14678 2021-02-13T00:40:54Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-13T00:40:54Z- handleDepth()-Depth_Keller msg: 1613176854640 epoch sec; seqNo:14796; depth 29.06 2021-02-13T00:40:54Z- handleDvl() - RDI_Pathfinder msg: 1613176854.65 epoch sec; seqNo:7112 2021-02-13T00:40:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 141.25 , 60.89 , 97.98 2021-02-13T00:40:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -1.12 , 0.05 2021-02-13T00:40:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176854.66 epoch sec; seqNo:14796; 4059777.45 north; 592959.91 east 2021-02-13T00:40:55Z- AHRS_M2 msg: 1613176855.04 epoch sec; seqNo:14679 2021-02-13T00:40:55Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-13T00:40:55Z- handleDepth()-Depth_Keller msg: 1613176855055 epoch sec; seqNo:14797; depth 29.16 2021-02-13T00:40:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176855.07 epoch sec; seqNo:14797; 4059778.21 north; 592959.90 east 2021-02-13T00:40:55Z- AHRS_M2 msg: 1613176855.44 epoch sec; seqNo:14680 2021-02-13T00:40:55Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-13T00:40:55Z- handleDepth()-Depth_Keller msg: 1613176855448 epoch sec; seqNo:14798; depth 29.29 2021-02-13T00:40:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176855.46 epoch sec; seqNo:14798; 4059778.58 north; 592958.68 east 2021-02-13T00:40:55Z- AHRS_M2 msg: 1613176855.85 epoch sec; seqNo:14681 2021-02-13T00:40:55Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.19 psi 2021-02-13T00:40:55Z- handleDepth()-Depth_Keller msg: 1613176855852 epoch sec; seqNo:14799; depth 29.41 2021-02-13T00:40:55Z- handleDvl() - RDI_Pathfinder msg: 1613176855.86 epoch sec; seqNo:7113 2021-02-13T00:40:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.38 , 83.52 , 61.30 , 92.98 2021-02-13T00:40:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.06 , 0.02 2021-02-13T00:40:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176855.88 epoch sec; seqNo:14799; 4059778.97 north; 592959.89 east 2021-02-13T00:40:56Z- AHRS_M2 msg: 1613176856.25 epoch sec; seqNo:14682 2021-02-13T00:40:56Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.19 psi 2021-02-13T00:40:56Z- handleDepth()-Depth_Keller msg: 1613176856266 epoch sec; seqNo:14800; depth 29.56 2021-02-13T00:40:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176856.28 epoch sec; seqNo:14800; 4059778.97 north; 592959.89 east 2021-02-13T00:40:56Z- AHRS_M2 msg: 1613176856.67 epoch sec; seqNo:14683 2021-02-13T00:40:56Z- AHRS_M2 msg: 0.60 phi; -0.23 theta; -0.18 psi 2021-02-13T00:40:56Z- handleDepth()-Depth_Keller msg: 1613176856679 epoch sec; seqNo:14801; depth 29.70 2021-02-13T00:40:56Z- handleDvl() - RDI_Pathfinder msg: 1613176856.69 epoch sec; seqNo:7114 2021-02-13T00:40:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:40:56Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:40:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176856.70 epoch sec; seqNo:14801; 4059779.35 north; 592959.89 east 2021-02-13T00:40:57Z- AHRS_M2 msg: 1613176857.06 epoch sec; seqNo:14684 2021-02-13T00:40:57Z- AHRS_M2 msg: 0.60 phi; -0.19 theta; -0.17 psi 2021-02-13T00:40:57Z- handleDepth()-Depth_Keller msg: 1613176857068 epoch sec; seqNo:14802; depth 29.82 2021-02-13T00:40:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176857.08 epoch sec; seqNo:14802; 4059779.73 north; 592959.89 east 2021-02-13T00:40:57Z- AHRS_M2 msg: 1613176857.47 epoch sec; seqNo:14685 2021-02-13T00:40:57Z- AHRS_M2 msg: 0.59 phi; -0.15 theta; -0.16 psi 2021-02-13T00:40:57Z- handleDepth()-Depth_Keller msg: 1613176857475 epoch sec; seqNo:14803; depth 29.94 2021-02-13T00:40:57Z- handleDvl() - RDI_Pathfinder msg: 1613176857.49 epoch sec; seqNo:7115 2021-02-13T00:40:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.53 , 78.92 , 61.30 , 81.35 2021-02-13T00:40:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , -0.04 2021-02-13T00:40:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176857.50 epoch sec; seqNo:14803; 4059780.11 north; 592959.88 east 2021-02-13T00:40:57Z- AHRS_M2 msg: 1613176857.87 epoch sec; seqNo:14686 2021-02-13T00:40:57Z- AHRS_M2 msg: 0.58 phi; -0.10 theta; -0.15 psi 2021-02-13T00:40:57Z- handleDepth()-Depth_Keller msg: 1613176857873 epoch sec; seqNo:14804; depth 30.05 2021-02-13T00:40:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176857.89 epoch sec; seqNo:14804; 4059780.50 north; 592961.10 east 2021-02-13T00:40:58Z- AHRS_M2 msg: 1613176858.27 epoch sec; seqNo:14687 2021-02-13T00:40:58Z- AHRS_M2 msg: 0.57 phi; -0.06 theta; -0.15 psi 2021-02-13T00:40:58Z- handleDepth()-Depth_Keller msg: 1613176858276 epoch sec; seqNo:14805; depth 30.15 2021-02-13T00:40:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176858.29 epoch sec; seqNo:14805; 4059780.88 north; 592961.09 east 2021-02-13T00:40:58Z- AHRS_M2 msg: 1613176858.68 epoch sec; seqNo:14688 2021-02-13T00:40:58Z- AHRS_M2 msg: 0.57 phi; -0.02 theta; -0.15 psi 2021-02-13T00:40:58Z- handleDepth()-Depth_Keller msg: 1613176858680 epoch sec; seqNo:14806; depth 30.25 2021-02-13T00:40:58Z- handleDvl() - RDI_Pathfinder msg: 1613176858.69 epoch sec; seqNo:7116 2021-02-13T00:40:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.53 , 78.51 , 64.57 , 78.51 2021-02-13T00:40:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , -0.06 2021-02-13T00:40:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176858.70 epoch sec; seqNo:14806; 4059780.88 north; 592961.09 east 2021-02-13T00:40:58Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-13T00:40:58Z- waiting for 1613176856.45, time is 1613176858.70 2021-02-13T00:40:58Z- LcmTrn::updateTrn() equating pose 1613176858.70 and meas times 1613176858.70 2021-02-13T00:40:58Z- LcmTrn::updateTrn() >>>> pose time 1613176858.70, meas time 1613176858.70 <<<< 2021-02-13T00:40:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176858.70 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:40:58Z- TerrainNav::motionUpdate() - currEstimate = (4059781, 592961.09, 30.25) 2021-02-13T00:40:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:40:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:40:58Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176858.70 sec.; No relevant navigation data available 2021-02-13T00:40:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:40:58Z- TNavBF::mmse selected from filter -1 2021-02-13T00:40:58Z- variances([0], [2], and psi_berg) = 692.070388 832.993947 0.000000 2021-02-13T00:40:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:40:58Z- LcmTrn::publishEstimates() - MLE : 4059780.38 592981.59 30.25 2021-02-13T00:40:58Z- LcmTrn::publishEstimates() - MMSE : 4059780.88 592961.09 30.25 2021-02-13T00:40:58Z- LcmTrn::publishEstimates() - COVAR: 692.07 832.99 1.02 0.00 2021-02-13T00:40:59Z- AHRS_M2 msg: 1613176859.10 epoch sec; seqNo:14689 2021-02-13T00:40:59Z- AHRS_M2 msg: 0.56 phi; 0.02 theta; -0.15 psi 2021-02-13T00:40:59Z- handleDepth()-Depth_Keller msg: 1613176859102 epoch sec; seqNo:14807; depth 30.31 2021-02-13T00:40:59Z- handleDvl() - RDI_Pathfinder msg: 1613176859.11 epoch sec; seqNo:7117 2021-02-13T00:40:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.69 , 0.00 , 0.00 , 0.00 2021-02-13T00:40:59Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:40:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176859.13 epoch sec; seqNo:14807; 4059781.25 north; 592961.09 east 2021-02-13T00:40:59Z- AHRS_M2 msg: 1613176859.48 epoch sec; seqNo:14690 2021-02-13T00:40:59Z- AHRS_M2 msg: 0.55 phi; 0.06 theta; -0.15 psi 2021-02-13T00:40:59Z- handleDepth()-Depth_Keller msg: 1613176859488 epoch sec; seqNo:14808; depth 30.36 2021-02-13T00:40:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176859.51 epoch sec; seqNo:14808; 4059781.63 north; 592961.09 east 2021-02-13T00:40:59Z- AHRS_M2 msg: 1613176859.89 epoch sec; seqNo:14691 2021-02-13T00:40:59Z- AHRS_M2 msg: 0.54 phi; 0.10 theta; -0.15 psi 2021-02-13T00:40:59Z- handleDepth()-Depth_Keller msg: 1613176859892 epoch sec; seqNo:14809; depth 30.40 2021-02-13T00:40:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176859.90 epoch sec; seqNo:14809; 4059782.01 north; 592961.08 east 2021-02-13T00:41:00Z- AHRS_M2 msg: 1613176860.29 epoch sec; seqNo:14692 2021-02-13T00:41:00Z- AHRS_M2 msg: 0.53 phi; 0.13 theta; -0.16 psi 2021-02-13T00:41:00Z- handleDepth()-Depth_Keller msg: 1613176860296 epoch sec; seqNo:14810; depth 30.43 2021-02-13T00:41:00Z- handleDvl() - RDI_Pathfinder msg: 1613176860.31 epoch sec; seqNo:7118 2021-02-13T00:41:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.89 , 0.00 , 0.00 , 64.22 2021-02-13T00:41:00Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:41:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176860.32 epoch sec; seqNo:14810; 4059782.39 north; 592961.08 east 2021-02-13T00:41:00Z- AHRS_M2 msg: 1613176860.70 epoch sec; seqNo:14693 2021-02-13T00:41:00Z- AHRS_M2 msg: 0.53 phi; 0.16 theta; -0.15 psi 2021-02-13T00:41:00Z- handleDepth()-Depth_Keller msg: 1613176860702 epoch sec; seqNo:14811; depth 30.44 2021-02-13T00:41:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176860.71 epoch sec; seqNo:14811; 4059782.78 north; 592962.29 east 2021-02-13T00:41:01Z- AHRS_M2 msg: 1613176861.10 epoch sec; seqNo:14694 2021-02-13T00:41:01Z- AHRS_M2 msg: 0.52 phi; 0.19 theta; -0.15 psi 2021-02-13T00:41:01Z- handleDepth()-Depth_Keller msg: 1613176861104 epoch sec; seqNo:14812; depth 30.44 2021-02-13T00:41:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176861.15 epoch sec; seqNo:14812; 4059782.78 north; 592962.29 east 2021-02-13T00:41:01Z- AHRS_M2 msg: 1613176861.50 epoch sec; seqNo:14695 2021-02-13T00:41:01Z- AHRS_M2 msg: 0.52 phi; 0.22 theta; -0.15 psi 2021-02-13T00:41:01Z- handleDepth()-Depth_Keller msg: 1613176861508 epoch sec; seqNo:14813; depth 30.41 2021-02-13T00:41:01Z- handleDvl() - RDI_Pathfinder msg: 1613176861.52 epoch sec; seqNo:7119 2021-02-13T00:41:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.78 , 0.00 , 0.00 , 62.45 2021-02-13T00:41:01Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:41:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176861.55 epoch sec; seqNo:14813; 4059783.16 north; 592962.29 east 2021-02-13T00:41:01Z- AHRS_M2 msg: 1613176861.91 epoch sec; seqNo:14696 2021-02-13T00:41:01Z- AHRS_M2 msg: 0.52 phi; 0.24 theta; -0.15 psi 2021-02-13T00:41:01Z- handleDepth()-Depth_Keller msg: 1613176861917 epoch sec; seqNo:14814; depth 30.38 2021-02-13T00:41:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176861.93 epoch sec; seqNo:14814; 4059783.54 north; 592962.29 east 2021-02-13T00:41:02Z- AHRS_M2 msg: 1613176862.32 epoch sec; seqNo:14697 2021-02-13T00:41:02Z- AHRS_M2 msg: 0.51 phi; 0.26 theta; -0.14 psi 2021-02-13T00:41:02Z- handleDepth()-Depth_Keller msg: 1613176862328 epoch sec; seqNo:14815; depth 30.34 2021-02-13T00:41:02Z- handleDvl() - RDI_Pathfinder msg: 1613176862.34 epoch sec; seqNo:7120 2021-02-13T00:41:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 65.11 , 0.00 , 0.00 , 61.12 2021-02-13T00:41:02Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:41:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176862.35 epoch sec; seqNo:14815; 4059783.92 north; 592962.28 east 2021-02-13T00:41:02Z- AHRS_M2 msg: 1613176862.72 epoch sec; seqNo:14698 2021-02-13T00:41:02Z- AHRS_M2 msg: 0.51 phi; 0.27 theta; -0.14 psi 2021-02-13T00:41:02Z- handleDepth()-Depth_Keller msg: 1613176862720 epoch sec; seqNo:14816; depth 30.28 2021-02-13T00:41:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176862.74 epoch sec; seqNo:14816; 4059784.30 north; 592962.28 east 2021-02-13T00:41:03Z- AHRS_M2 msg: 1613176863.12 epoch sec; seqNo:14699 2021-02-13T00:41:03Z- AHRS_M2 msg: 0.51 phi; 0.29 theta; -0.14 psi 2021-02-13T00:41:03Z- handleDepth()-Depth_Keller msg: 1613176863130 epoch sec; seqNo:14817; depth 30.24 2021-02-13T00:41:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176863.17 epoch sec; seqNo:14817; 4059784.30 north; 592962.28 east 2021-02-13T00:41:03Z- AHRS_M2 msg: 1613176863.52 epoch sec; seqNo:14700 2021-02-13T00:41:03Z- AHRS_M2 msg: 0.52 phi; 0.30 theta; -0.14 psi 2021-02-13T00:41:03Z- handleDepth()-Depth_Keller msg: 1613176863528 epoch sec; seqNo:14818; depth 30.17 2021-02-13T00:41:03Z- handleDvl() - RDI_Pathfinder msg: 1613176863.54 epoch sec; seqNo:7121 2021-02-13T00:41:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:41:03Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:41:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176863.55 epoch sec; seqNo:14818; 4059784.69 north; 592963.49 east 2021-02-13T00:41:03Z- LcmTrn::handleMessages() - handling 41 messages took 0.68 ms and 128 calls... 2021-02-13T00:41:03Z- waiting for 1613176861.70, time is 1613176863.55 2021-02-13T00:41:03Z- LcmTrn::updateTrn() equating pose 1613176863.55 and meas times 1613176863.55 2021-02-13T00:41:03Z- LcmTrn::updateTrn() >>>> pose time 1613176863.55, meas time 1613176863.55 <<<< 2021-02-13T00:41:03Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613176863.55, ping # 0. 2021-02-13T00:41:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176863.55 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:41:03Z- TerrainNav::motionUpdate() - currEstimate = (4059785, 592963.49, 30.17) 2021-02-13T00:41:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:41:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:41:03Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:41:03Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:41:03Z- TNavBF::Projecting Measurements 2021-02-13T00:41:03Z- TNavFilter::Measurement from time = 1613176863.55 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:41:03Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:41:03Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:41:03Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:41:03Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:41:03Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:41:03Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:41:03Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:41:03Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 692.0 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:41:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:41:03Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:03Z- variances([0], [2], and psi_berg) = 692.017564 832.993922 0.000000 2021-02-13T00:41:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:41:03Z- LcmTrn::publishEstimates() - MLE : 4059784.19 592983.99 30.17 2021-02-13T00:41:03Z- LcmTrn::publishEstimates() - MMSE : 4059784.69 592963.49 30.17 2021-02-13T00:41:03Z- LcmTrn::publishEstimates() - COVAR: 692.02 832.99 1.25 0.00 2021-02-13T00:41:03Z- AHRS_M2 msg: 1613176863.93 epoch sec; seqNo:14701 2021-02-13T00:41:03Z- AHRS_M2 msg: 0.52 phi; 0.31 theta; -0.14 psi 2021-02-13T00:41:03Z- handleDepth()-Depth_Keller msg: 1613176863934 epoch sec; seqNo:14819; depth 30.09 2021-02-13T00:41:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176863.94 epoch sec; seqNo:14819; 4059785.07 north; 592963.49 east 2021-02-13T00:41:04Z- AHRS_M2 msg: 1613176864.33 epoch sec; seqNo:14702 2021-02-13T00:41:04Z- AHRS_M2 msg: 0.52 phi; 0.31 theta; -0.13 psi 2021-02-13T00:41:04Z- handleDepth()-Depth_Keller msg: 1613176864336 epoch sec; seqNo:14820; depth 30.01 2021-02-13T00:41:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176864.35 epoch sec; seqNo:14820; 4059785.45 north; 592963.49 east 2021-02-13T00:41:04Z- AHRS_M2 msg: 1613176864.74 epoch sec; seqNo:14703 2021-02-13T00:41:04Z- AHRS_M2 msg: 0.53 phi; 0.32 theta; -0.13 psi 2021-02-13T00:41:04Z- handleDepth()-Depth_Keller msg: 1613176864740 epoch sec; seqNo:14821; depth 29.92 2021-02-13T00:41:04Z- handleDvl() - RDI_Pathfinder msg: 1613176864.75 epoch sec; seqNo:7122 2021-02-13T00:41:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 79.60 , 89.33 , 60.07 2021-02-13T00:41:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.16 , -0.06 2021-02-13T00:41:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176864.78 epoch sec; seqNo:14821; 4059785.83 north; 592963.48 east 2021-02-13T00:41:05Z- AHRS_M2 msg: 1613176865.14 epoch sec; seqNo:14704 2021-02-13T00:41:05Z- AHRS_M2 msg: 0.53 phi; 0.33 theta; -0.12 psi 2021-02-13T00:41:05Z- handleDepth()-Depth_Keller msg: 1613176865148 epoch sec; seqNo:14822; depth 29.83 2021-02-13T00:41:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176865.16 epoch sec; seqNo:14822; 4059785.83 north; 592963.48 east 2021-02-13T00:41:05Z- AHRS_M2 msg: 1613176865.54 epoch sec; seqNo:14705 2021-02-13T00:41:05Z- AHRS_M2 msg: 0.54 phi; 0.33 theta; -0.12 psi 2021-02-13T00:41:05Z- handleDepth()-Depth_Keller msg: 1613176865550 epoch sec; seqNo:14823; depth 29.75 2021-02-13T00:41:05Z- handleDvl() - RDI_Pathfinder msg: 1613176865.56 epoch sec; seqNo:7123 2021-02-13T00:41:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 79.59 , 97.98 , 61.30 2021-02-13T00:41:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.03 2021-02-13T00:41:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176865.57 epoch sec; seqNo:14823; 4059786.21 north; 592963.48 east 2021-02-13T00:41:05Z- AHRS_M2 msg: 1613176865.95 epoch sec; seqNo:14706 2021-02-13T00:41:05Z- AHRS_M2 msg: 0.54 phi; 0.33 theta; -0.12 psi 2021-02-13T00:41:05Z- handleDepth()-Depth_Keller msg: 1613176865953 epoch sec; seqNo:14824; depth 29.65 2021-02-13T00:41:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176865.97 epoch sec; seqNo:14824; 4059786.59 north; 592963.47 east 2021-02-13T00:41:06Z- AHRS_M2 msg: 1613176866.35 epoch sec; seqNo:14707 2021-02-13T00:41:06Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.12 psi 2021-02-13T00:41:06Z- handleDepth()-Depth_Keller msg: 1613176866356 epoch sec; seqNo:14825; depth 29.54 2021-02-13T00:41:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176866.36 epoch sec; seqNo:14825; 4059786.96 north; 592963.47 east 2021-02-13T00:41:06Z- AHRS_M2 msg: 1613176866.76 epoch sec; seqNo:14708 2021-02-13T00:41:06Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.11 psi 2021-02-13T00:41:06Z- handleDepth()-Depth_Keller msg: 1613176866761 epoch sec; seqNo:14826; depth 29.45 2021-02-13T00:41:06Z- handleDvl() - RDI_Pathfinder msg: 1613176866.77 epoch sec; seqNo:7124 2021-02-13T00:41:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.25 , 79.59 , 97.98 , 61.30 2021-02-13T00:41:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.04 2021-02-13T00:41:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176866.78 epoch sec; seqNo:14826; 4059786.98 north; 592964.69 east 2021-02-13T00:41:07Z- AHRS_M2 msg: 1613176867.16 epoch sec; seqNo:14709 2021-02-13T00:41:07Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.11 psi 2021-02-13T00:41:07Z- handleDepth()-Depth_Keller msg: 1613176867167 epoch sec; seqNo:14827; depth 29.34 2021-02-13T00:41:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176867.18 epoch sec; seqNo:14827; 4059787.36 north; 592964.69 east 2021-02-13T00:41:07Z- AHRS_M2 msg: 1613176867.56 epoch sec; seqNo:14710 2021-02-13T00:41:07Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.11 psi 2021-02-13T00:41:07Z- handleDepth()-Depth_Keller msg: 1613176867578 epoch sec; seqNo:14828; depth 29.24 2021-02-13T00:41:07Z- handleDvl() - RDI_Pathfinder msg: 1613176867.59 epoch sec; seqNo:7125 2021-02-13T00:41:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 0.00 , 97.98 , 61.30 2021-02-13T00:41:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , 0.00 2021-02-13T00:41:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176867.60 epoch sec; seqNo:14828; 4059787.74 north; 592964.68 east 2021-02-13T00:41:07Z- AHRS_M2 msg: 1613176867.97 epoch sec; seqNo:14711 2021-02-13T00:41:07Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.11 psi 2021-02-13T00:41:07Z- handleDepth()-Depth_Keller msg: 1613176867981 epoch sec; seqNo:14829; depth 29.12 2021-02-13T00:41:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176868.00 epoch sec; seqNo:14829; 4059788.11 north; 592964.68 east 2021-02-13T00:41:08Z- AHRS_M2 msg: 1613176868.37 epoch sec; seqNo:14712 2021-02-13T00:41:08Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.11 psi 2021-02-13T00:41:08Z- handleDepth()-Depth_Keller msg: 1613176868376 epoch sec; seqNo:14830; depth 29.02 2021-02-13T00:41:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176868.38 epoch sec; seqNo:14830; 4059788.11 north; 592964.68 east 2021-02-13T00:41:08Z- AHRS_M2 msg: 1613176868.78 epoch sec; seqNo:14713 2021-02-13T00:41:08Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.11 psi 2021-02-13T00:41:08Z- handleDepth()-Depth_Keller msg: 1613176868781 epoch sec; seqNo:14831; depth 28.90 2021-02-13T00:41:08Z- handleDvl() - RDI_Pathfinder msg: 1613176868.79 epoch sec; seqNo:7126 2021-02-13T00:41:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.47 , 80.68 , 97.98 , 59.66 2021-02-13T00:41:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , -0.01 2021-02-13T00:41:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176868.80 epoch sec; seqNo:14831; 4059788.49 north; 592964.67 east 2021-02-13T00:41:08Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-13T00:41:08Z- waiting for 1613176866.55, time is 1613176868.80 2021-02-13T00:41:08Z- LcmTrn::updateTrn() equating pose 1613176868.80 and meas times 1613176868.80 2021-02-13T00:41:08Z- LcmTrn::updateTrn() >>>> pose time 1613176868.80, meas time 1613176868.80 <<<< 2021-02-13T00:41:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176868.80 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:41:08Z- TerrainNav::motionUpdate() - currEstimate = (4059788, 592964.67, 28.90) 2021-02-13T00:41:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:41:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:41:08Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176868.80 sec.; No relevant navigation data available 2021-02-13T00:41:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:41:08Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:08Z- variances([0], [2], and psi_berg) = 691.967950 832.993898 0.000000 2021-02-13T00:41:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:41:08Z- LcmTrn::publishEstimates() - MLE : 4059787.99 592985.17 28.90 2021-02-13T00:41:08Z- LcmTrn::publishEstimates() - MMSE : 4059788.49 592964.67 28.90 2021-02-13T00:41:08Z- LcmTrn::publishEstimates() - COVAR: 691.97 832.99 1.45 0.00 2021-02-13T00:41:09Z- AHRS_M2 msg: 1613176869.18 epoch sec; seqNo:14714 2021-02-13T00:41:09Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.11 psi 2021-02-13T00:41:09Z- handleDepth()-Depth_Keller msg: 1613176869185 epoch sec; seqNo:14832; depth 28.78 2021-02-13T00:41:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176869.20 epoch sec; seqNo:14832; 4059788.87 north; 592964.67 east 2021-02-13T00:41:09Z- AHRS_M2 msg: 1613176869.58 epoch sec; seqNo:14715 2021-02-13T00:41:09Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.11 psi 2021-02-13T00:41:09Z- handleDepth()-Depth_Keller msg: 1613176869588 epoch sec; seqNo:14833; depth 28.67 2021-02-13T00:41:09Z- handleDvl() - RDI_Pathfinder msg: 1613176869.60 epoch sec; seqNo:7127 2021-02-13T00:41:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 79.59 , 92.57 , 62.12 2021-02-13T00:41:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , 0.02 2021-02-13T00:41:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176869.61 epoch sec; seqNo:14833; 4059789.26 north; 592965.89 east 2021-02-13T00:41:09Z- AHRS_M2 msg: 1613176869.99 epoch sec; seqNo:14716 2021-02-13T00:41:09Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.11 psi 2021-02-13T00:41:09Z- handleDepth()-Depth_Keller msg: 1613176869994 epoch sec; seqNo:14834; depth 28.57 2021-02-13T00:41:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176870.00 epoch sec; seqNo:14834; 4059789.26 north; 592965.89 east 2021-02-13T00:41:10Z- AHRS_M2 msg: 1613176870.39 epoch sec; seqNo:14717 2021-02-13T00:41:10Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.12 psi 2021-02-13T00:41:10Z- handleDepth()-Depth_Keller msg: 1613176870401 epoch sec; seqNo:14835; depth 28.45 2021-02-13T00:41:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176870.42 epoch sec; seqNo:14835; 4059789.64 north; 592965.88 east 2021-02-13T00:41:10Z- AHRS_M2 msg: 1613176870.80 epoch sec; seqNo:14718 2021-02-13T00:41:10Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.12 psi 2021-02-13T00:41:10Z- handleDepth()-Depth_Keller msg: 1613176870800 epoch sec; seqNo:14836; depth 28.31 2021-02-13T00:41:10Z- handleDvl() - RDI_Pathfinder msg: 1613176870.81 epoch sec; seqNo:7128 2021-02-13T00:41:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 81.75 , 92.57 , 60.90 2021-02-13T00:41:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.01 2021-02-13T00:41:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176870.84 epoch sec; seqNo:14836; 4059790.02 north; 592965.88 east 2021-02-13T00:41:11Z- AHRS_M2 msg: 1613176871.20 epoch sec; seqNo:14719 2021-02-13T00:41:11Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.12 psi 2021-02-13T00:41:11Z- handleDepth()-Depth_Keller msg: 1613176871209 epoch sec; seqNo:14837; depth 28.20 2021-02-13T00:41:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176871.22 epoch sec; seqNo:14837; 4059790.40 north; 592965.87 east 2021-02-13T00:41:11Z- AHRS_M2 msg: 1613176871.60 epoch sec; seqNo:14720 2021-02-13T00:41:11Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.12 psi 2021-02-13T00:41:11Z- handleDepth()-Depth_Keller msg: 1613176871617 epoch sec; seqNo:14838; depth 28.09 2021-02-13T00:41:11Z- handleDvl() - RDI_Pathfinder msg: 1613176871.63 epoch sec; seqNo:7129 2021-02-13T00:41:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 0.00 , 97.98 , 61.30 2021-02-13T00:41:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , -0.02 2021-02-13T00:41:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176871.64 epoch sec; seqNo:14838; 4059790.40 north; 592965.87 east 2021-02-13T00:41:12Z- AHRS_M2 msg: 1613176872.01 epoch sec; seqNo:14721 2021-02-13T00:41:12Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.12 psi 2021-02-13T00:41:12Z- handleDepth()-Depth_Keller msg: 1613176872013 epoch sec; seqNo:14839; depth 27.97 2021-02-13T00:41:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176872.03 epoch sec; seqNo:14839; 4059790.79 north; 592967.09 east 2021-02-13T00:41:12Z- AHRS_M2 msg: 1613176872.41 epoch sec; seqNo:14722 2021-02-13T00:41:12Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.12 psi 2021-02-13T00:41:12Z- handleDepth()-Depth_Keller msg: 1613176872418 epoch sec; seqNo:14840; depth 27.83 2021-02-13T00:41:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176872.43 epoch sec; seqNo:14840; 4059791.17 north; 592967.09 east 2021-02-13T00:41:12Z- AHRS_M2 msg: 1613176872.82 epoch sec; seqNo:14723 2021-02-13T00:41:12Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.12 psi 2021-02-13T00:41:12Z- handleDepth()-Depth_Keller msg: 1613176872820 epoch sec; seqNo:14841; depth 27.73 2021-02-13T00:41:12Z- handleDvl() - RDI_Pathfinder msg: 1613176872.83 epoch sec; seqNo:7130 2021-02-13T00:41:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 0.00 , 97.98 , 62.53 2021-02-13T00:41:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , -0.04 2021-02-13T00:41:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176872.85 epoch sec; seqNo:14841; 4059791.55 north; 592967.08 east 2021-02-13T00:41:13Z- AHRS_M2 msg: 1613176873.22 epoch sec; seqNo:14724 2021-02-13T00:41:13Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.12 psi 2021-02-13T00:41:13Z- handleDepth()-Depth_Keller msg: 1613176873234 epoch sec; seqNo:14842; depth 27.59 2021-02-13T00:41:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176873.25 epoch sec; seqNo:14842; 4059791.55 north; 592967.08 east 2021-02-13T00:41:13Z- AHRS_M2 msg: 1613176873.62 epoch sec; seqNo:14725 2021-02-13T00:41:13Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-13T00:41:13Z- handleDepth()-Depth_Keller msg: 1613176873631 epoch sec; seqNo:14843; depth 27.48 2021-02-13T00:41:13Z- handleDvl() - RDI_Pathfinder msg: 1613176873.65 epoch sec; seqNo:7131 2021-02-13T00:41:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 0.00 , 97.98 , 62.53 2021-02-13T00:41:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , -0.08 2021-02-13T00:41:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176873.66 epoch sec; seqNo:14843; 4059791.93 north; 592967.08 east 2021-02-13T00:41:13Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-13T00:41:13Z- waiting for 1613176871.80, time is 1613176873.66 2021-02-13T00:41:13Z- LcmTrn::updateTrn() equating pose 1613176873.66 and meas times 1613176873.66 2021-02-13T00:41:13Z- LcmTrn::updateTrn() >>>> pose time 1613176873.66, meas time 1613176873.66 <<<< 2021-02-13T00:41:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176873.66 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:41:13Z- TerrainNav::motionUpdate() - currEstimate = (4059792, 592967.08, 27.48) 2021-02-13T00:41:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:41:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:41:13Z- TNavBF::Projecting Measurements 2021-02-13T00:41:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:41:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:41:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:41:13Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:41:13Z- TNavFilter::3 good beams. 2021-02-13T00:41:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:41:13Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:41:13Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:41:13Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:41:13Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:41:13Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:41:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:41:13Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:13Z- TerrainMapOctree: (vehN, vehE ) = (4059791.93, 592967.08). 2021-02-13T00:41:13Z- TerrainMapOctree: Min Distance = 1942.13. 2021-02-13T00:41:13Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:41:13Z- There is no specific good beam that all particles have in common. 2021-02-13T00:41:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:41:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.67 K: 0.147 ExpectedMeasDiff: nan currMeas.covariance: 7.91 2021-02-13T00:41:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:41:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.42 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:41:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:41:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.141 ExpectedMeasDiff: nan currMeas.covariance: 6.26 2021-02-13T00:41:13Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:41:13Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:41:13Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:41:13Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:41:13Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:41:13Z- Filter 0 No beams used in subcloud update 2021-02-13T00:41:13Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:41:13Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:41:13Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 691.9 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:41:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176873.66 sec. 2021-02-13T00:41:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:41:13Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:13Z- variances([0], [2], and psi_berg) = 691.917052 832.993873 0.000000 2021-02-13T00:41:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:41:13Z- LcmTrn::publishEstimates() - MLE : 4059791.43 592987.58 27.48 2021-02-13T00:41:13Z- LcmTrn::publishEstimates() - MMSE : 4059791.93 592967.08 27.48 2021-02-13T00:41:13Z- LcmTrn::publishEstimates() - COVAR: 691.92 832.99 1.13 0.00 2021-02-13T00:41:14Z- AHRS_M2 msg: 1613176874.03 epoch sec; seqNo:14726 2021-02-13T00:41:14Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.14 psi 2021-02-13T00:41:14Z- handleDepth()-Depth_Keller msg: 1613176874045 epoch sec; seqNo:14844; depth 27.36 2021-02-13T00:41:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176874.06 epoch sec; seqNo:14844; 4059792.31 north; 592967.07 east 2021-02-13T00:41:14Z- AHRS_M2 msg: 1613176874.44 epoch sec; seqNo:14727 2021-02-13T00:41:14Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.14 psi 2021-02-13T00:41:14Z- handleDepth()-Depth_Keller msg: 1613176874444 epoch sec; seqNo:14845; depth 27.24 2021-02-13T00:41:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176874.46 epoch sec; seqNo:14845; 4059792.32 north; 592968.29 east 2021-02-13T00:41:14Z- AHRS_M2 msg: 1613176874.84 epoch sec; seqNo:14728 2021-02-13T00:41:14Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-13T00:41:14Z- handleDepth()-Depth_Keller msg: 1613176874840 epoch sec; seqNo:14846; depth 27.12 2021-02-13T00:41:14Z- handleDvl() - RDI_Pathfinder msg: 1613176874.86 epoch sec; seqNo:7132 2021-02-13T00:41:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 81.75 , 97.98 , 62.94 2021-02-13T00:41:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.06 2021-02-13T00:41:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176874.87 epoch sec; seqNo:14846; 4059792.70 north; 592968.29 east 2021-02-13T00:41:15Z- AHRS_M2 msg: 1613176875.24 epoch sec; seqNo:14729 2021-02-13T00:41:15Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.16 psi 2021-02-13T00:41:15Z- handleDepth()-Depth_Keller msg: 1613176875245 epoch sec; seqNo:14847; depth 26.99 2021-02-13T00:41:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176875.26 epoch sec; seqNo:14847; 4059793.08 north; 592968.29 east 2021-02-13T00:41:15Z- AHRS_M2 msg: 1613176875.65 epoch sec; seqNo:14730 2021-02-13T00:41:15Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.16 psi 2021-02-13T00:41:15Z- handleDepth()-Depth_Keller msg: 1613176875650 epoch sec; seqNo:14848; depth 26.87 2021-02-13T00:41:15Z- handleDvl() - RDI_Pathfinder msg: 1613176875.66 epoch sec; seqNo:7133 2021-02-13T00:41:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.88 , 0.00 , 97.98 , 62.12 2021-02-13T00:41:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , -0.03 2021-02-13T00:41:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176875.67 epoch sec; seqNo:14848; 4059793.46 north; 592968.28 east 2021-02-13T00:41:16Z- AHRS_M2 msg: 1613176876.05 epoch sec; seqNo:14731 2021-02-13T00:41:16Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-13T00:41:16Z- handleDepth()-Depth_Keller msg: 1613176876052 epoch sec; seqNo:14849; depth 26.75 2021-02-13T00:41:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176876.07 epoch sec; seqNo:14849; 4059793.46 north; 592968.28 east 2021-02-13T00:41:16Z- AHRS_M2 msg: 1613176876.45 epoch sec; seqNo:14732 2021-02-13T00:41:16Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.18 psi 2021-02-13T00:41:16Z- handleDepth()-Depth_Keller msg: 1613176876456 epoch sec; seqNo:14850; depth 26.62 2021-02-13T00:41:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176876.46 epoch sec; seqNo:14850; 4059793.84 north; 592968.28 east 2021-02-13T00:41:16Z- AHRS_M2 msg: 1613176876.86 epoch sec; seqNo:14733 2021-02-13T00:41:16Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.19 psi 2021-02-13T00:41:16Z- handleDepth()-Depth_Keller msg: 1613176876860 epoch sec; seqNo:14851; depth 26.49 2021-02-13T00:41:16Z- handleDvl() - RDI_Pathfinder msg: 1613176876.87 epoch sec; seqNo:7134 2021-02-13T00:41:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 158.55 , 97.98 , 64.16 2021-02-13T00:41:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , -0.01 2021-02-13T00:41:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176876.88 epoch sec; seqNo:14851; 4059794.22 north; 592968.27 east 2021-02-13T00:41:17Z- AHRS_M2 msg: 1613176877.26 epoch sec; seqNo:14734 2021-02-13T00:41:17Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.19 psi 2021-02-13T00:41:17Z- handleDepth()-Depth_Keller msg: 1613176877273 epoch sec; seqNo:14852; depth 26.37 2021-02-13T00:41:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176877.28 epoch sec; seqNo:14852; 4059794.22 north; 592968.27 east 2021-02-13T00:41:17Z- AHRS_M2 msg: 1613176877.66 epoch sec; seqNo:14735 2021-02-13T00:41:17Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.20 psi 2021-02-13T00:41:17Z- handleDepth()-Depth_Keller msg: 1613176877673 epoch sec; seqNo:14853; depth 26.24 2021-02-13T00:41:17Z- handleDvl() - RDI_Pathfinder msg: 1613176877.69 epoch sec; seqNo:7135 2021-02-13T00:41:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 87.57 , 97.03 , 64.57 2021-02-13T00:41:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.07 2021-02-13T00:41:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176877.70 epoch sec; seqNo:14853; 4059794.61 north; 592969.49 east 2021-02-13T00:41:18Z- AHRS_M2 msg: 1613176878.07 epoch sec; seqNo:14736 2021-02-13T00:41:18Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.20 psi 2021-02-13T00:41:18Z- handleDepth()-Depth_Keller msg: 1613176878072 epoch sec; seqNo:14854; depth 26.12 2021-02-13T00:41:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176878.08 epoch sec; seqNo:14854; 4059794.99 north; 592969.49 east 2021-02-13T00:41:18Z- AHRS_M2 msg: 1613176878.49 epoch sec; seqNo:14737 2021-02-13T00:41:18Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.20 psi 2021-02-13T00:41:18Z- handleDepth()-Depth_Keller msg: 1613176878498 epoch sec; seqNo:14855; depth 26.01 2021-02-13T00:41:18Z- handleDvl() - RDI_Pathfinder msg: 1613176878.51 epoch sec; seqNo:7136 2021-02-13T00:41:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 88.25 , 97.98 , 62.94 2021-02-13T00:41:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.03 , 0.09 2021-02-13T00:41:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176878.53 epoch sec; seqNo:14855; 4059795.36 north; 592969.48 east 2021-02-13T00:41:18Z- AHRS_M2 msg: 1613176878.88 epoch sec; seqNo:14738 2021-02-13T00:41:18Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.19 psi 2021-02-13T00:41:18Z- handleDepth()-Depth_Keller msg: 1613176878886 epoch sec; seqNo:14856; depth 25.85 2021-02-13T00:41:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176878.90 epoch sec; seqNo:14856; 4059795.74 north; 592969.48 east 2021-02-13T00:41:18Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:41:18Z- waiting for 1613176876.66, time is 1613176878.90 2021-02-13T00:41:18Z- LcmTrn::updateTrn() >>>> pose time 1613176878.90, meas time 1613176878.51 <<<< 2021-02-13T00:41:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176878.51 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:41:18Z- TerrainNav::motionUpdate() - currEstimate = (4059796, 592969.48, 25.85) 2021-02-13T00:41:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:41:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:41:18Z- TNavBF::Projecting Measurements 2021-02-13T00:41:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:41:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:41:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:41:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:41:18Z- TNavFilter::4 good beams. 2021-02-13T00:41:18Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:41:18Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:41:18Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:41:18Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:41:18Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:41:18Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:41:18Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:41:18Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:18Z- TerrainMapOctree: (vehN, vehE ) = (4059795.46, 592969.30). 2021-02-13T00:41:18Z- TerrainMapOctree: Min Distance = 1938.01. 2021-02-13T00:41:18Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:41:18Z- There is no specific good beam that all particles have in common. 2021-02-13T00:41:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:41:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.124 ExpectedMeasDiff: nan currMeas.covariance: 7.72 2021-02-13T00:41:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:41:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 12.46 2021-02-13T00:41:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:41:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:41:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:41:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 6.34 2021-02-13T00:41:18Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:41:18Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:41:19Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:41:19Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:41:19Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:41:19Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:41:19Z- Filter 0 No beams used in subcloud update 2021-02-13T00:41:19Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:41:19Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:41:19Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 691.9 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:41:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176878.51 sec. 2021-02-13T00:41:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:41:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:19Z- variances([0], [2], and psi_berg) = 691.851970 832.993842 0.000000 2021-02-13T00:41:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:41:19Z- LcmTrn::publishEstimates() - MLE : 4059795.24 592992.98 25.85 2021-02-13T00:41:19Z- LcmTrn::publishEstimates() - MMSE : 4059795.74 592969.48 25.85 2021-02-13T00:41:19Z- LcmTrn::publishEstimates() - COVAR: 691.85 832.99 0.92 0.00 2021-02-13T00:41:19Z- AHRS_M2 msg: 1613176879.28 epoch sec; seqNo:14739 2021-02-13T00:41:19Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.19 psi 2021-02-13T00:41:19Z- handleDepth()-Depth_Keller msg: 1613176879289 epoch sec; seqNo:14857; depth 25.74 2021-02-13T00:41:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176879.31 epoch sec; seqNo:14857; 4059796.12 north; 592969.47 east 2021-02-13T00:41:19Z- AHRS_M2 msg: 1613176879.68 epoch sec; seqNo:14740 2021-02-13T00:41:19Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-13T00:41:19Z- handleDepth()-Depth_Keller msg: 1613176879693 epoch sec; seqNo:14858; depth 25.62 2021-02-13T00:41:19Z- handleDvl() - RDI_Pathfinder msg: 1613176879.70 epoch sec; seqNo:7137 2021-02-13T00:41:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:41:19Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:41:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176879.73 epoch sec; seqNo:14858; 4059796.12 north; 592969.47 east 2021-02-13T00:41:20Z- AHRS_M2 msg: 1613176880.09 epoch sec; seqNo:14741 2021-02-13T00:41:20Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-13T00:41:20Z- handleDepth()-Depth_Keller msg: 1613176880097 epoch sec; seqNo:14859; depth 25.50 2021-02-13T00:41:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176880.11 epoch sec; seqNo:14859; 4059796.51 north; 592970.69 east 2021-02-13T00:41:20Z- AHRS_M2 msg: 1613176880.51 epoch sec; seqNo:14742 2021-02-13T00:41:20Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.17 psi 2021-02-13T00:41:20Z- handleDepth()-Depth_Keller msg: 1613176880522 epoch sec; seqNo:14860; depth 25.37 2021-02-13T00:41:20Z- handleDvl() - RDI_Pathfinder msg: 1613176880.54 epoch sec; seqNo:7138 2021-02-13T00:41:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 87.91 , 97.64 , 63.76 2021-02-13T00:41:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.01 , 0.07 2021-02-13T00:41:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176880.55 epoch sec; seqNo:14860; 4059796.89 north; 592970.69 east 2021-02-13T00:41:20Z- AHRS_M2 msg: 1613176880.90 epoch sec; seqNo:14743 2021-02-13T00:41:20Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.17 psi 2021-02-13T00:41:20Z- handleDepth()-Depth_Keller msg: 1613176880900 epoch sec; seqNo:14861; depth 25.24 2021-02-13T00:41:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176880.91 epoch sec; seqNo:14861; 4059797.27 north; 592970.68 east 2021-02-13T00:41:21Z- AHRS_M2 msg: 1613176881.30 epoch sec; seqNo:14744 2021-02-13T00:41:21Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.16 psi 2021-02-13T00:41:21Z- handleDepth()-Depth_Keller msg: 1613176881309 epoch sec; seqNo:14862; depth 25.12 2021-02-13T00:41:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176881.32 epoch sec; seqNo:14862; 4059797.65 north; 592970.68 east 2021-02-13T00:41:21Z- AHRS_M2 msg: 1613176881.70 epoch sec; seqNo:14745 2021-02-13T00:41:21Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.16 psi 2021-02-13T00:41:21Z- handleDepth()-Depth_Keller msg: 1613176881708 epoch sec; seqNo:14863; depth 24.99 2021-02-13T00:41:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176881.72 epoch sec; seqNo:14863; 4059797.65 north; 592970.68 east 2021-02-13T00:41:22Z- AHRS_M2 msg: 1613176882.11 epoch sec; seqNo:14746 2021-02-13T00:41:22Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-13T00:41:22Z- handleDepth()-Depth_Keller msg: 1613176882112 epoch sec; seqNo:14864; depth 24.87 2021-02-13T00:41:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176882.12 epoch sec; seqNo:14864; 4059798.04 north; 592971.89 east 2021-02-13T00:41:22Z- AHRS_M2 msg: 1613176882.51 epoch sec; seqNo:14747 2021-02-13T00:41:22Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-13T00:41:22Z- handleDepth()-Depth_Keller msg: 1613176882516 epoch sec; seqNo:14865; depth 24.74 2021-02-13T00:41:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176882.52 epoch sec; seqNo:14865; 4059798.42 north; 592971.89 east 2021-02-13T00:41:22Z- AHRS_M2 msg: 1613176882.92 epoch sec; seqNo:14748 2021-02-13T00:41:22Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.15 psi 2021-02-13T00:41:22Z- handleDepth()-Depth_Keller msg: 1613176882922 epoch sec; seqNo:14866; depth 24.62 2021-02-13T00:41:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176882.93 epoch sec; seqNo:14866; 4059798.80 north; 592971.89 east 2021-02-13T00:41:23Z- AHRS_M2 msg: 1613176883.32 epoch sec; seqNo:14749 2021-02-13T00:41:23Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-13T00:41:23Z- handleDepth()-Depth_Keller msg: 1613176883325 epoch sec; seqNo:14867; depth 24.49 2021-02-13T00:41:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176883.34 epoch sec; seqNo:14867; 4059799.18 north; 592971.88 east 2021-02-13T00:41:23Z- AHRS_M2 msg: 1613176883.72 epoch sec; seqNo:14750 2021-02-13T00:41:23Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-13T00:41:23Z- handleDepth()-Depth_Keller msg: 1613176883730 epoch sec; seqNo:14868; depth 24.37 2021-02-13T00:41:23Z- handleDvl() - RDI_Pathfinder msg: 1613176883.75 epoch sec; seqNo:7139 2021-02-13T00:41:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 87.50 , 99.67 , 66.21 2021-02-13T00:41:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , -0.03 2021-02-13T00:41:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176883.78 epoch sec; seqNo:14868; 4059799.18 north; 592971.88 east 2021-02-13T00:41:24Z- LcmTrn::handleMessages() - handling 39 messages took 0.64 ms and 126 calls... 2021-02-13T00:41:24Z- waiting for 1613176881.90, time is 1613176883.78 2021-02-13T00:41:24Z- LcmTrn::updateTrn() equating pose 1613176883.78 and meas times 1613176883.78 2021-02-13T00:41:24Z- LcmTrn::updateTrn() >>>> pose time 1613176883.78, meas time 1613176883.78 <<<< 2021-02-13T00:41:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176883.78 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:41:24Z- TerrainNav::motionUpdate() - currEstimate = (4059799, 592971.88, 24.37) 2021-02-13T00:41:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:41:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:41:24Z- TNavBF::Projecting Measurements 2021-02-13T00:41:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:41:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:41:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:41:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:41:24Z- TNavFilter::4 good beams. 2021-02-13T00:41:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:41:24Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:41:24Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:41:24Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:41:24Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:41:24Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:41:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:41:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:24Z- TerrainMapOctree: (vehN, vehE ) = (4059799.18, 592971.88). 2021-02-13T00:41:24Z- TerrainMapOctree: Min Distance = 1933.51. 2021-02-13T00:41:24Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:41:24Z- There is no specific good beam that all particles have in common. 2021-02-13T00:41:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:41:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 9.74 2021-02-13T00:41:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:41:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 12.25 2021-02-13T00:41:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:41:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 15.89 2021-02-13T00:41:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:41:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 7.01 2021-02-13T00:41:24Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:41:24Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:41:24Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:41:24Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:41:24Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:41:24Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:41:24Z- Filter 0 No beams used in subcloud update 2021-02-13T00:41:24Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:41:24Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:41:24Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 691.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:41:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176883.78 sec. 2021-02-13T00:41:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:41:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:24Z- variances([0], [2], and psi_berg) = 691.801133 832.993817 0.000000 2021-02-13T00:41:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:41:24Z- LcmTrn::publishEstimates() - MLE : 4059798.68 592995.38 24.37 2021-02-13T00:41:24Z- LcmTrn::publishEstimates() - MMSE : 4059799.18 592971.88 24.37 2021-02-13T00:41:24Z- LcmTrn::publishEstimates() - COVAR: 691.80 832.99 0.83 0.00 2021-02-13T00:41:24Z- AHRS_M2 msg: 1613176884.13 epoch sec; seqNo:14751 2021-02-13T00:41:24Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.14 psi 2021-02-13T00:41:24Z- handleDepth()-Depth_Keller msg: 1613176884137 epoch sec; seqNo:14869; depth 24.24 2021-02-13T00:41:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176884.15 epoch sec; seqNo:14869; 4059799.56 north; 592971.88 east 2021-02-13T00:41:24Z- AHRS_M2 msg: 1613176884.53 epoch sec; seqNo:14752 2021-02-13T00:41:24Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.14 psi 2021-02-13T00:41:24Z- handleDepth()-Depth_Keller msg: 1613176884546 epoch sec; seqNo:14870; depth 24.11 2021-02-13T00:41:24Z- handleDvl() - RDI_Pathfinder msg: 1613176884.56 epoch sec; seqNo:7140 2021-02-13T00:41:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 87.43 , 100.82 , 66.21 2021-02-13T00:41:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , -0.04 2021-02-13T00:41:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176884.57 epoch sec; seqNo:14870; 4059799.95 north; 592973.09 east 2021-02-13T00:41:24Z- AHRS_M2 msg: 1613176884.94 epoch sec; seqNo:14753 2021-02-13T00:41:24Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.14 psi 2021-02-13T00:41:24Z- handleDepth()-Depth_Keller msg: 1613176884941 epoch sec; seqNo:14871; depth 23.98 2021-02-13T00:41:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176884.95 epoch sec; seqNo:14871; 4059799.95 north; 592973.09 east 2021-02-13T00:41:25Z- AHRS_M2 msg: 1613176885.34 epoch sec; seqNo:14754 2021-02-13T00:41:25Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.14 psi 2021-02-13T00:41:25Z- handleDepth()-Depth_Keller msg: 1613176885349 epoch sec; seqNo:14872; depth 23.88 2021-02-13T00:41:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176885.36 epoch sec; seqNo:14872; 4059800.33 north; 592973.09 east 2021-02-13T00:41:25Z- AHRS_M2 msg: 1613176885.74 epoch sec; seqNo:14755 2021-02-13T00:41:25Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.14 psi 2021-02-13T00:41:25Z- handleDepth()-Depth_Keller msg: 1613176885750 epoch sec; seqNo:14873; depth 23.73 2021-02-13T00:41:25Z- handleDvl() - RDI_Pathfinder msg: 1613176885.76 epoch sec; seqNo:7141 2021-02-13T00:41:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.71 , 87.23 , 98.18 , 0.00 2021-02-13T00:41:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , -0.04 2021-02-13T00:41:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176885.77 epoch sec; seqNo:14873; 4059800.71 north; 592973.09 east 2021-02-13T00:41:26Z- AHRS_M2 msg: 1613176886.15 epoch sec; seqNo:14756 2021-02-13T00:41:26Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.14 psi 2021-02-13T00:41:26Z- handleDepth()-Depth_Keller msg: 1613176886155 epoch sec; seqNo:14874; depth 23.60 2021-02-13T00:41:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176886.18 epoch sec; seqNo:14874; 4059801.09 north; 592973.08 east 2021-02-13T00:41:26Z- AHRS_M2 msg: 1613176886.55 epoch sec; seqNo:14757 2021-02-13T00:41:26Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.14 psi 2021-02-13T00:41:26Z- handleDepth()-Depth_Keller msg: 1613176886556 epoch sec; seqNo:14875; depth 23.50 2021-02-13T00:41:26Z- handleDvl() - RDI_Pathfinder msg: 1613176886.57 epoch sec; seqNo:7142 2021-02-13T00:41:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 0.00 , 97.91 , 66.62 2021-02-13T00:41:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , -0.01 2021-02-13T00:41:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176886.58 epoch sec; seqNo:14875; 4059801.09 north; 592973.08 east 2021-02-13T00:41:26Z- AHRS_M2 msg: 1613176886.96 epoch sec; seqNo:14758 2021-02-13T00:41:26Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-13T00:41:26Z- handleDepth()-Depth_Keller msg: 1613176886965 epoch sec; seqNo:14876; depth 23.36 2021-02-13T00:41:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176886.97 epoch sec; seqNo:14876; 4059801.47 north; 592973.08 east 2021-02-13T00:41:27Z- AHRS_M2 msg: 1613176887.36 epoch sec; seqNo:14759 2021-02-13T00:41:27Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.13 psi 2021-02-13T00:41:27Z- handleDepth()-Depth_Keller msg: 1613176887373 epoch sec; seqNo:14877; depth 23.26 2021-02-13T00:41:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176887.38 epoch sec; seqNo:14877; 4059801.86 north; 592974.29 east 2021-02-13T00:41:27Z- AHRS_M2 msg: 1613176887.76 epoch sec; seqNo:14760 2021-02-13T00:41:27Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.13 psi 2021-02-13T00:41:27Z- handleDepth()-Depth_Keller msg: 1613176887773 epoch sec; seqNo:14878; depth 23.14 2021-02-13T00:41:27Z- handleDvl() - RDI_Pathfinder msg: 1613176887.79 epoch sec; seqNo:7143 2021-02-13T00:41:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 85.00 , 97.98 , 65.80 2021-02-13T00:41:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.02 2021-02-13T00:41:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176887.80 epoch sec; seqNo:14878; 4059802.24 north; 592974.29 east 2021-02-13T00:41:28Z- AHRS_M2 msg: 1613176888.17 epoch sec; seqNo:14761 2021-02-13T00:41:28Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.13 psi 2021-02-13T00:41:28Z- handleDepth()-Depth_Keller msg: 1613176888175 epoch sec; seqNo:14879; depth 23.01 2021-02-13T00:41:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176888.19 epoch sec; seqNo:14879; 4059802.24 north; 592974.29 east 2021-02-13T00:41:28Z- AHRS_M2 msg: 1613176888.57 epoch sec; seqNo:14762 2021-02-13T00:41:28Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.13 psi 2021-02-13T00:41:28Z- handleDepth()-Depth_Keller msg: 1613176888581 epoch sec; seqNo:14880; depth 22.93 2021-02-13T00:41:28Z- handleDvl() - RDI_Pathfinder msg: 1613176888.59 epoch sec; seqNo:7144 2021-02-13T00:41:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 86.01 , 97.91 , 66.21 2021-02-13T00:41:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , 0.01 2021-02-13T00:41:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176888.60 epoch sec; seqNo:14880; 4059802.62 north; 592974.29 east 2021-02-13T00:41:28Z- AHRS_M2 msg: 1613176888.97 epoch sec; seqNo:14763 2021-02-13T00:41:28Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.13 psi 2021-02-13T00:41:28Z- handleDepth()-Depth_Keller msg: 1613176888983 epoch sec; seqNo:14881; depth 22.79 2021-02-13T00:41:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176888.99 epoch sec; seqNo:14881; 4059802.99 north; 592974.28 east 2021-02-13T00:41:29Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-13T00:41:29Z- waiting for 1613176886.78, time is 1613176888.99 2021-02-13T00:41:29Z- LcmTrn::updateTrn() >>>> pose time 1613176888.99, meas time 1613176888.59 <<<< 2021-02-13T00:41:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176888.59 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:41:29Z- TerrainNav::motionUpdate() - currEstimate = (4059803, 592974.28, 22.79) 2021-02-13T00:41:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:41:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:41:29Z- TNavBF::Projecting Measurements 2021-02-13T00:41:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:41:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:41:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:41:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:41:29Z- TNavFilter::4 good beams. 2021-02-13T00:41:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:41:29Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:41:29Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:41:29Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:41:29Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:41:29Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:41:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:41:29Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:29Z- TerrainMapOctree: (vehN, vehE ) = (4059802.70, 592974.10). 2021-02-13T00:41:29Z- TerrainMapOctree: Min Distance = 1929.40. 2021-02-13T00:41:29Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:41:29Z- There is no specific good beam that all particles have in common. 2021-02-13T00:41:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:41:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T00:41:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:41:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 11.84 2021-02-13T00:41:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:41:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:41:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:41:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 7.01 2021-02-13T00:41:29Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:41:29Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:41:29Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:41:29Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:41:29Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:41:29Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:41:29Z- Filter 0 No beams used in subcloud update 2021-02-13T00:41:29Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:41:29Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:41:29Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 691.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:41:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176888.59 sec. 2021-02-13T00:41:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:41:29Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:29Z- variances([0], [2], and psi_berg) = 691.735917 832.993786 0.000000 2021-02-13T00:41:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:41:29Z- LcmTrn::publishEstimates() - MLE : 4059802.49 592997.78 22.79 2021-02-13T00:41:29Z- LcmTrn::publishEstimates() - MMSE : 4059802.99 592974.28 22.79 2021-02-13T00:41:29Z- LcmTrn::publishEstimates() - COVAR: 691.74 832.99 0.79 0.00 2021-02-13T00:41:29Z- AHRS_M2 msg: 1613176889.38 epoch sec; seqNo:14764 2021-02-13T00:41:29Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-13T00:41:29Z- handleDepth()-Depth_Keller msg: 1613176889400 epoch sec; seqNo:14882; depth 22.69 2021-02-13T00:41:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176889.41 epoch sec; seqNo:14882; 4059803.37 north; 592974.28 east 2021-02-13T00:41:29Z- AHRS_M2 msg: 1613176889.78 epoch sec; seqNo:14765 2021-02-13T00:41:29Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-13T00:41:29Z- handleDepth()-Depth_Keller msg: 1613176889787 epoch sec; seqNo:14883; depth 22.58 2021-02-13T00:41:29Z- handleDvl() - RDI_Pathfinder msg: 1613176889.80 epoch sec; seqNo:7145 2021-02-13T00:41:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 156.19 , 99.94 , 66.62 2021-02-13T00:41:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.29 , -0.08 2021-02-13T00:41:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176889.82 epoch sec; seqNo:14883; 4059803.39 north; 592975.50 east 2021-02-13T00:41:30Z- AHRS_M2 msg: 1613176890.19 epoch sec; seqNo:14766 2021-02-13T00:41:30Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-13T00:41:30Z- handleDepth()-Depth_Keller msg: 1613176890196 epoch sec; seqNo:14884; depth 22.46 2021-02-13T00:41:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176890.21 epoch sec; seqNo:14884; 4059803.77 north; 592975.49 east 2021-02-13T00:41:30Z- AHRS_M2 msg: 1613176890.60 epoch sec; seqNo:14767 2021-02-13T00:41:30Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-13T00:41:30Z- handleDepth()-Depth_Keller msg: 1613176890605 epoch sec; seqNo:14885; depth 22.36 2021-02-13T00:41:30Z- handleDvl() - RDI_Pathfinder msg: 1613176890.62 epoch sec; seqNo:7146 2021-02-13T00:41:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.97 , 87.57 , 99.74 , 67.84 2021-02-13T00:41:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , -0.02 2021-02-13T00:41:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176890.63 epoch sec; seqNo:14885; 4059803.77 north; 592975.49 east 2021-02-13T00:41:31Z- AHRS_M2 msg: 1613176891.01 epoch sec; seqNo:14768 2021-02-13T00:41:31Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.19 psi 2021-02-13T00:41:31Z- handleDepth()-Depth_Keller msg: 1613176891021 epoch sec; seqNo:14886; depth 22.24 2021-02-13T00:41:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176891.03 epoch sec; seqNo:14886; 4059804.14 north; 592975.49 east 2021-02-13T00:41:31Z- AHRS_M2 msg: 1613176891.41 epoch sec; seqNo:14769 2021-02-13T00:41:31Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.20 psi 2021-02-13T00:41:31Z- handleDepth()-Depth_Keller msg: 1613176891411 epoch sec; seqNo:14887; depth 22.11 2021-02-13T00:41:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176891.42 epoch sec; seqNo:14887; 4059804.52 north; 592975.49 east 2021-02-13T00:41:31Z- AHRS_M2 msg: 1613176891.81 epoch sec; seqNo:14770 2021-02-13T00:41:31Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.20 psi 2021-02-13T00:41:31Z- handleDepth()-Depth_Keller msg: 1613176891813 epoch sec; seqNo:14888; depth 22.02 2021-02-13T00:41:31Z- handleDvl() - RDI_Pathfinder msg: 1613176891.82 epoch sec; seqNo:7147 2021-02-13T00:41:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 97.78 , 99.94 , 67.84 2021-02-13T00:41:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.01 2021-02-13T00:41:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176891.83 epoch sec; seqNo:14888; 4059804.54 north; 592976.71 east 2021-02-13T00:41:32Z- AHRS_M2 msg: 1613176892.22 epoch sec; seqNo:14771 2021-02-13T00:41:32Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.20 psi 2021-02-13T00:41:32Z- handleDepth()-Depth_Keller msg: 1613176892222 epoch sec; seqNo:14889; depth 21.91 2021-02-13T00:41:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176892.23 epoch sec; seqNo:14889; 4059804.92 north; 592976.70 east 2021-02-13T00:41:32Z- AHRS_M2 msg: 1613176892.62 epoch sec; seqNo:14772 2021-02-13T00:41:32Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.20 psi 2021-02-13T00:41:32Z- handleDepth()-Depth_Keller msg: 1613176892625 epoch sec; seqNo:14890; depth 21.80 2021-02-13T00:41:32Z- handleDvl() - RDI_Pathfinder msg: 1613176892.64 epoch sec; seqNo:7148 2021-02-13T00:41:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 98.12 , 99.33 , 69.07 2021-02-13T00:41:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.07 , 0.04 2021-02-13T00:41:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176892.65 epoch sec; seqNo:14890; 4059805.29 north; 592976.70 east 2021-02-13T00:41:33Z- AHRS_M2 msg: 1613176893.02 epoch sec; seqNo:14773 2021-02-13T00:41:33Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.20 psi 2021-02-13T00:41:33Z- handleDepth()-Depth_Keller msg: 1613176893032 epoch sec; seqNo:14891; depth 21.70 2021-02-13T00:41:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176893.04 epoch sec; seqNo:14891; 4059805.67 north; 592976.69 east 2021-02-13T00:41:33Z- AHRS_M2 msg: 1613176893.43 epoch sec; seqNo:14774 2021-02-13T00:41:33Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.19 psi 2021-02-13T00:41:33Z- handleDepth()-Depth_Keller msg: 1613176893437 epoch sec; seqNo:14892; depth 21.59 2021-02-13T00:41:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176893.46 epoch sec; seqNo:14892; 4059805.67 north; 592976.69 east 2021-02-13T00:41:33Z- AHRS_M2 msg: 1613176893.83 epoch sec; seqNo:14775 2021-02-13T00:41:33Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.19 psi 2021-02-13T00:41:33Z- handleDepth()-Depth_Keller msg: 1613176893832 epoch sec; seqNo:14893; depth 21.48 2021-02-13T00:41:33Z- handleDvl() - RDI_Pathfinder msg: 1613176893.85 epoch sec; seqNo:7149 2021-02-13T00:41:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.18 , 100.62 , 69.07 2021-02-13T00:41:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , 0.05 2021-02-13T00:41:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176893.86 epoch sec; seqNo:14893; 4059806.05 north; 592976.69 east 2021-02-13T00:41:34Z- AHRS_M2 msg: 1613176894.23 epoch sec; seqNo:14776 2021-02-13T00:41:34Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.19 psi 2021-02-13T00:41:34Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-13T00:41:34Z- waiting for 1613176891.99, time is 1613176894.23 2021-02-13T00:41:34Z- LcmTrn::updateTrn() >>>> pose time 1613176894.23, meas time 1613176893.85 <<<< 2021-02-13T00:41:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176893.85 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:41:34Z- TerrainNav::motionUpdate() - currEstimate = (4059806, 592976.69, 21.48) 2021-02-13T00:41:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:41:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:41:34Z- TNavBF::Projecting Measurements 2021-02-13T00:41:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:41:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:41:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:41:34Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:41:34Z- TNavFilter::3 good beams. 2021-02-13T00:41:34Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:41:34Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:41:34Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:41:34Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:41:34Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:41:34Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:41:34Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:41:34Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:34Z- TerrainMapOctree: (vehN, vehE ) = (4059805.83, 592976.52). 2021-02-13T00:41:34Z- TerrainMapOctree: Min Distance = 1925.46. 2021-02-13T00:41:34Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:41:34Z- There is no specific good beam that all particles have in common. 2021-02-13T00:41:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:41:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.42 2021-02-13T00:41:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:41:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 16.20 2021-02-13T00:41:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:41:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-13T00:41:34Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:41:34Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:41:34Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:41:34Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:41:34Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:41:34Z- Filter 0 No beams used in subcloud update 2021-02-13T00:41:34Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:41:34Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:41:34Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 691.7 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:41:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176893.85 sec. 2021-02-13T00:41:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:41:34Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:34Z- variances([0], [2], and psi_berg) = 691.675618 832.993757 0.000000 2021-02-13T00:41:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:41:34Z- LcmTrn::publishEstimates() - MLE : 4059805.55 593000.19 21.48 2021-02-13T00:41:34Z- LcmTrn::publishEstimates() - MMSE : 4059806.05 592976.69 21.48 2021-02-13T00:41:34Z- LcmTrn::publishEstimates() - COVAR: 691.68 832.99 0.82 0.00 2021-02-13T00:41:34Z- handleDepth()-Depth_Keller msg: 1613176894238 epoch sec; seqNo:14894; depth 21.39 2021-02-13T00:41:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176894.25 epoch sec; seqNo:14894; 4059806.43 north; 592976.69 east 2021-02-13T00:41:34Z- AHRS_M2 msg: 1613176894.63 epoch sec; seqNo:14777 2021-02-13T00:41:34Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.18 psi 2021-02-13T00:41:34Z- handleDepth()-Depth_Keller msg: 1613176894646 epoch sec; seqNo:14895; depth 21.30 2021-02-13T00:41:34Z- handleDvl() - RDI_Pathfinder msg: 1613176894.66 epoch sec; seqNo:7150 2021-02-13T00:41:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 97.85 , 99.06 , 69.07 2021-02-13T00:41:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.04 2021-02-13T00:41:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176894.67 epoch sec; seqNo:14895; 4059806.82 north; 592977.90 east 2021-02-13T00:41:35Z- AHRS_M2 msg: 1613176895.05 epoch sec; seqNo:14778 2021-02-13T00:41:35Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.18 psi 2021-02-13T00:41:35Z- handleDepth()-Depth_Keller msg: 1613176895053 epoch sec; seqNo:14896; depth 21.17 2021-02-13T00:41:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176895.07 epoch sec; seqNo:14896; 4059806.82 north; 592977.90 east 2021-02-13T00:41:35Z- AHRS_M2 msg: 1613176895.46 epoch sec; seqNo:14779 2021-02-13T00:41:35Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.18 psi 2021-02-13T00:41:35Z- handleDepth()-Depth_Keller msg: 1613176895472 epoch sec; seqNo:14897; depth 21.07 2021-02-13T00:41:35Z- handleDvl() - RDI_Pathfinder msg: 1613176895.49 epoch sec; seqNo:7151 2021-02-13T00:41:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 90.88 , 99.40 , 68.66 2021-02-13T00:41:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.05 , -0.01 2021-02-13T00:41:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176895.50 epoch sec; seqNo:14897; 4059807.20 north; 592977.90 east 2021-02-13T00:41:35Z- AHRS_M2 msg: 1613176895.85 epoch sec; seqNo:14780 2021-02-13T00:41:35Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.18 psi 2021-02-13T00:41:35Z- handleDepth()-Depth_Keller msg: 1613176895852 epoch sec; seqNo:14898; depth 20.97 2021-02-13T00:41:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176895.86 epoch sec; seqNo:14898; 4059807.58 north; 592977.89 east 2021-02-13T00:41:36Z- AHRS_M2 msg: 1613176896.25 epoch sec; seqNo:14781 2021-02-13T00:41:36Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.18 psi 2021-02-13T00:41:36Z- handleDepth()-Depth_Keller msg: 1613176896266 epoch sec; seqNo:14899; depth 20.88 2021-02-13T00:41:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176896.27 epoch sec; seqNo:14899; 4059807.96 north; 592977.89 east 2021-02-13T00:41:36Z- AHRS_M2 msg: 1613176896.66 epoch sec; seqNo:14782 2021-02-13T00:41:36Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.18 psi 2021-02-13T00:41:36Z- handleDepth()-Depth_Keller msg: 1613176896660 epoch sec; seqNo:14900; depth 20.77 2021-02-13T00:41:36Z- handleDvl() - RDI_Pathfinder msg: 1613176896.67 epoch sec; seqNo:7152 2021-02-13T00:41:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.38 , 91.96 , 100.48 , 69.48 2021-02-13T00:41:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , -0.03 2021-02-13T00:41:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176896.69 epoch sec; seqNo:14900; 4059807.96 north; 592977.89 east 2021-02-13T00:41:37Z- AHRS_M2 msg: 1613176897.06 epoch sec; seqNo:14783 2021-02-13T00:41:37Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.18 psi 2021-02-13T00:41:37Z- handleDepth()-Depth_Keller msg: 1613176897065 epoch sec; seqNo:14901; depth 20.67 2021-02-13T00:41:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176897.08 epoch sec; seqNo:14901; 4059808.35 north; 592979.11 east 2021-02-13T00:41:37Z- AHRS_M2 msg: 1613176897.47 epoch sec; seqNo:14784 2021-02-13T00:41:37Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-13T00:41:37Z- handleDepth()-Depth_Keller msg: 1613176897472 epoch sec; seqNo:14902; depth 20.56 2021-02-13T00:41:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176897.48 epoch sec; seqNo:14902; 4059808.73 north; 592979.10 east 2021-02-13T00:41:37Z- AHRS_M2 msg: 1613176897.87 epoch sec; seqNo:14785 2021-02-13T00:41:37Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.17 psi 2021-02-13T00:41:37Z- handleDepth()-Depth_Keller msg: 1613176897872 epoch sec; seqNo:14903; depth 20.46 2021-02-13T00:41:37Z- handleDvl() - RDI_Pathfinder msg: 1613176897.89 epoch sec; seqNo:7153 2021-02-13T00:41:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.25 , 101.90 , 78.78 2021-02-13T00:41:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , -0.05 2021-02-13T00:41:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176897.90 epoch sec; seqNo:14903; 4059809.11 north; 592979.10 east 2021-02-13T00:41:38Z- AHRS_M2 msg: 1613176898.27 epoch sec; seqNo:14786 2021-02-13T00:41:38Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.17 psi 2021-02-13T00:41:38Z- handleDepth()-Depth_Keller msg: 1613176898276 epoch sec; seqNo:14904; depth 20.37 2021-02-13T00:41:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176898.29 epoch sec; seqNo:14904; 4059809.11 north; 592979.10 east 2021-02-13T00:41:38Z- AHRS_M2 msg: 1613176898.68 epoch sec; seqNo:14787 2021-02-13T00:41:38Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.17 psi 2021-02-13T00:41:38Z- handleDepth()-Depth_Keller msg: 1613176898681 epoch sec; seqNo:14905; depth 20.26 2021-02-13T00:41:38Z- handleDvl() - RDI_Pathfinder msg: 1613176898.69 epoch sec; seqNo:7154 2021-02-13T00:41:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.71 , 97.64 , 100.35 , 78.71 2021-02-13T00:41:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , -0.01 2021-02-13T00:41:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176898.70 epoch sec; seqNo:14905; 4059809.49 north; 592979.10 east 2021-02-13T00:41:39Z- AHRS_M2 msg: 1613176899.08 epoch sec; seqNo:14788 2021-02-13T00:41:39Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.17 psi 2021-02-13T00:41:39Z- handleDepth()-Depth_Keller msg: 1613176899093 epoch sec; seqNo:14906; depth 20.14 2021-02-13T00:41:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176899.10 epoch sec; seqNo:14906; 4059809.87 north; 592979.09 east 2021-02-13T00:41:39Z- LcmTrn::handleMessages() - handling 43 messages took 0.70 ms and 129 calls... 2021-02-13T00:41:39Z- waiting for 1613176897.23, time is 1613176899.10 2021-02-13T00:41:39Z- LcmTrn::updateTrn() >>>> pose time 1613176899.10, meas time 1613176898.69 <<<< 2021-02-13T00:41:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176898.69 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:41:39Z- TerrainNav::motionUpdate() - currEstimate = (4059810, 592979.09, 20.14) 2021-02-13T00:41:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:41:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:41:39Z- TNavBF::Projecting Measurements 2021-02-13T00:41:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:41:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:41:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:41:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:41:39Z- TNavFilter::4 good beams. 2021-02-13T00:41:39Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:41:39Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:41:39Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:41:39Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:41:39Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:41:39Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:41:39Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:41:39Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:39Z- TerrainMapOctree: (vehN, vehE ) = (4059809.55, 592978.89). 2021-02-13T00:41:39Z- TerrainMapOctree: Min Distance = 1921.10. 2021-02-13T00:41:39Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:41:39Z- There is no specific good beam that all particles have in common. 2021-02-13T00:41:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:41:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 9.91 2021-02-13T00:41:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:41:39Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 15.25 2021-02-13T00:41:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:41:39Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 16.11 2021-02-13T00:41:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:41:39Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 9.91 2021-02-13T00:41:39Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:41:39Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:41:39Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:41:39Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:41:39Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:41:39Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:41:39Z- Filter 0 No beams used in subcloud update 2021-02-13T00:41:39Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:41:39Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:41:39Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 691.6 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:41:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176898.69 sec. 2021-02-13T00:41:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:41:39Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:39Z- variances([0], [2], and psi_berg) = 691.610011 832.993725 0.000000 2021-02-13T00:41:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:41:39Z- LcmTrn::publishEstimates() - MLE : 4059809.37 593002.59 20.14 2021-02-13T00:41:39Z- LcmTrn::publishEstimates() - MMSE : 4059809.87 592979.09 20.14 2021-02-13T00:41:39Z- LcmTrn::publishEstimates() - COVAR: 691.61 832.99 0.81 0.00 2021-02-13T00:41:39Z- AHRS_M2 msg: 1613176899.48 epoch sec; seqNo:14789 2021-02-13T00:41:39Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-13T00:41:39Z- handleDepth()-Depth_Keller msg: 1613176899488 epoch sec; seqNo:14907; depth 20.06 2021-02-13T00:41:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176899.50 epoch sec; seqNo:14907; 4059810.26 north; 592980.31 east 2021-02-13T00:41:39Z- AHRS_M2 msg: 1613176899.89 epoch sec; seqNo:14790 2021-02-13T00:41:39Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.17 psi 2021-02-13T00:41:39Z- handleDepth()-Depth_Keller msg: 1613176899897 epoch sec; seqNo:14908; depth 19.95 2021-02-13T00:41:39Z- handleDvl() - RDI_Pathfinder msg: 1613176899.91 epoch sec; seqNo:7155 2021-02-13T00:41:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.37 , 97.85 , 100.28 , 69.48 2021-02-13T00:41:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , 0.00 2021-02-13T00:41:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176899.93 epoch sec; seqNo:14908; 4059810.26 north; 592980.31 east 2021-02-13T00:41:40Z- AHRS_M2 msg: 1613176900.29 epoch sec; seqNo:14791 2021-02-13T00:41:40Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.17 psi 2021-02-13T00:41:40Z- handleDepth()-Depth_Keller msg: 1613176900296 epoch sec; seqNo:14909; depth 19.84 2021-02-13T00:41:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176900.31 epoch sec; seqNo:14909; 4059810.64 north; 592980.30 east 2021-02-13T00:41:40Z- AHRS_M2 msg: 1613176900.70 epoch sec; seqNo:14792 2021-02-13T00:41:40Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.17 psi 2021-02-13T00:41:40Z- handleDepth()-Depth_Keller msg: 1613176900701 epoch sec; seqNo:14910; depth 19.75 2021-02-13T00:41:40Z- handleDvl() - RDI_Pathfinder msg: 1613176900.72 epoch sec; seqNo:7156 2021-02-13T00:41:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 101.70 , 68.66 2021-02-13T00:41:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , -0.03 2021-02-13T00:41:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176900.73 epoch sec; seqNo:14910; 4059811.02 north; 592980.30 east 2021-02-13T00:41:41Z- AHRS_M2 msg: 1613176901.10 epoch sec; seqNo:14793 2021-02-13T00:41:41Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.17 psi 2021-02-13T00:41:41Z- handleDepth()-Depth_Keller msg: 1613176901106 epoch sec; seqNo:14911; depth 19.65 2021-02-13T00:41:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176901.12 epoch sec; seqNo:14911; 4059811.40 north; 592980.30 east 2021-02-13T00:41:41Z- AHRS_M2 msg: 1613176901.50 epoch sec; seqNo:14794 2021-02-13T00:41:41Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-13T00:41:41Z- handleDepth()-Depth_Keller msg: 1613176901513 epoch sec; seqNo:14912; depth 19.54 2021-02-13T00:41:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176901.52 epoch sec; seqNo:14912; 4059811.77 north; 592980.29 east 2021-02-13T00:41:41Z- AHRS_M2 msg: 1613176901.91 epoch sec; seqNo:14795 2021-02-13T00:41:41Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-13T00:41:41Z- handleDepth()-Depth_Keller msg: 1613176901922 epoch sec; seqNo:14913; depth 19.46 2021-02-13T00:41:41Z- handleDvl() - RDI_Pathfinder msg: 1613176901.93 epoch sec; seqNo:7157 2021-02-13T00:41:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 101.70 , 78.58 2021-02-13T00:41:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , -0.06 2021-02-13T00:41:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176901.94 epoch sec; seqNo:14913; 4059811.77 north; 592980.29 east 2021-02-13T00:41:42Z- AHRS_M2 msg: 1613176902.31 epoch sec; seqNo:14796 2021-02-13T00:41:42Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-13T00:41:42Z- handleDepth()-Depth_Keller msg: 1613176902316 epoch sec; seqNo:14914; depth 19.34 2021-02-13T00:41:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176902.33 epoch sec; seqNo:14914; 4059812.17 north; 592981.51 east 2021-02-13T00:41:42Z- AHRS_M2 msg: 1613176902.71 epoch sec; seqNo:14797 2021-02-13T00:41:42Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-13T00:41:42Z- handleDepth()-Depth_Keller msg: 1613176902725 epoch sec; seqNo:14915; depth 19.24 2021-02-13T00:41:42Z- handleDvl() - RDI_Pathfinder msg: 1613176902.74 epoch sec; seqNo:7158 2021-02-13T00:41:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.37 , 97.85 , 102.71 , 71.11 2021-02-13T00:41:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , -0.09 2021-02-13T00:41:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176902.78 epoch sec; seqNo:14915; 4059812.55 north; 592981.50 east 2021-02-13T00:41:43Z- AHRS_M2 msg: 1613176903.13 epoch sec; seqNo:14798 2021-02-13T00:41:43Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.17 psi 2021-02-13T00:41:43Z- handleDepth()-Depth_Keller msg: 1613176903134 epoch sec; seqNo:14916; depth 19.14 2021-02-13T00:41:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176903.15 epoch sec; seqNo:14916; 4059812.92 north; 592981.50 east 2021-02-13T00:41:43Z- AHRS_M2 msg: 1613176903.53 epoch sec; seqNo:14799 2021-02-13T00:41:43Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.17 psi 2021-02-13T00:41:43Z- handleDepth()-Depth_Keller msg: 1613176903540 epoch sec; seqNo:14917; depth 19.03 2021-02-13T00:41:43Z- handleDvl() - RDI_Pathfinder msg: 1613176903.55 epoch sec; seqNo:7159 2021-02-13T00:41:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.02 , 97.98 , 101.63 , 70.70 2021-02-13T00:41:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , -0.06 2021-02-13T00:41:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176903.56 epoch sec; seqNo:14917; 4059813.30 north; 592981.50 east 2021-02-13T00:41:43Z- AHRS_M2 msg: 1613176903.93 epoch sec; seqNo:14800 2021-02-13T00:41:43Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.17 psi 2021-02-13T00:41:43Z- handleDepth()-Depth_Keller msg: 1613176903933 epoch sec; seqNo:14918; depth 18.94 2021-02-13T00:41:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176903.94 epoch sec; seqNo:14918; 4059813.30 north; 592981.50 east 2021-02-13T00:41:44Z- AHRS_M2 msg: 1613176904.33 epoch sec; seqNo:14801 2021-02-13T00:41:44Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.17 psi 2021-02-13T00:41:44Z- handleDepth()-Depth_Keller msg: 1613176904341 epoch sec; seqNo:14919; depth 18.84 2021-02-13T00:41:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176904.35 epoch sec; seqNo:14919; 4059813.68 north; 592981.49 east 2021-02-13T00:41:44Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-13T00:41:44Z- waiting for 1613176902.10, time is 1613176904.35 2021-02-13T00:41:44Z- LcmTrn::updateTrn() >>>> pose time 1613176904.35, meas time 1613176903.55 <<<< 2021-02-13T00:41:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176903.55 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:41:44Z- TerrainNav::motionUpdate() - currEstimate = (4059814, 592981.49, 18.84) 2021-02-13T00:41:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:41:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:41:44Z- TNavBF::Projecting Measurements 2021-02-13T00:41:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:41:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:41:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:41:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:41:44Z- TNavFilter::4 good beams. 2021-02-13T00:41:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:41:44Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:41:44Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:41:44Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:41:44Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:41:44Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:41:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:41:44Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:44Z- TerrainMapOctree: (vehN, vehE ) = (4059813.10, 592981.13). 2021-02-13T00:41:44Z- TerrainMapOctree: Min Distance = 1916.96. 2021-02-13T00:41:44Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:41:44Z- There is no specific good beam that all particles have in common. 2021-02-13T00:41:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:41:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 9.25 2021-02-13T00:41:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:41:44Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:41:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:41:44Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 16.53 2021-02-13T00:41:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:41:44Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-13T00:41:44Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:41:44Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:41:44Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:41:44Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:41:44Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:41:44Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:41:44Z- Filter 0 No beams used in subcloud update 2021-02-13T00:41:44Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:41:44Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:41:44Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 691.6 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:41:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176903.55 sec. 2021-02-13T00:41:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:41:44Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:44Z- variances([0], [2], and psi_berg) = 691.541112 832.993692 0.000000 2021-02-13T00:41:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:41:44Z- LcmTrn::publishEstimates() - MLE : 4059813.18 593001.99 18.84 2021-02-13T00:41:44Z- LcmTrn::publishEstimates() - MMSE : 4059813.68 592981.49 18.84 2021-02-13T00:41:44Z- LcmTrn::publishEstimates() - COVAR: 691.54 832.99 0.80 0.00 2021-02-13T00:41:44Z- AHRS_M2 msg: 1613176904.74 epoch sec; seqNo:14802 2021-02-13T00:41:44Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.17 psi 2021-02-13T00:41:44Z- handleDepth()-Depth_Keller msg: 1613176904742 epoch sec; seqNo:14920; depth 18.74 2021-02-13T00:41:44Z- handleDvl() - RDI_Pathfinder msg: 1613176904.75 epoch sec; seqNo:7160 2021-02-13T00:41:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 98.05 , 101.70 , 69.89 2021-02-13T00:41:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.02 2021-02-13T00:41:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176904.76 epoch sec; seqNo:14920; 4059814.06 north; 592981.49 east 2021-02-13T00:41:45Z- AHRS_M2 msg: 1613176905.14 epoch sec; seqNo:14803 2021-02-13T00:41:45Z- AHRS_M2 msg: 0.58 phi; 0.30 theta; -0.17 psi 2021-02-13T00:41:45Z- handleDepth()-Depth_Keller msg: 1613176905148 epoch sec; seqNo:14921; depth 18.63 2021-02-13T00:41:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176905.17 epoch sec; seqNo:14921; 4059814.45 north; 592982.70 east 2021-02-13T00:41:45Z- AHRS_M2 msg: 1613176905.55 epoch sec; seqNo:14804 2021-02-13T00:41:45Z- AHRS_M2 msg: 0.58 phi; 0.30 theta; -0.17 psi 2021-02-13T00:41:45Z- handleDepth()-Depth_Keller msg: 1613176905557 epoch sec; seqNo:14922; depth 18.54 2021-02-13T00:41:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176905.59 epoch sec; seqNo:14922; 4059814.45 north; 592982.70 east 2021-02-13T00:41:45Z- AHRS_M2 msg: 1613176905.95 epoch sec; seqNo:14805 2021-02-13T00:41:45Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.16 psi 2021-02-13T00:41:45Z- handleDepth()-Depth_Keller msg: 1613176905960 epoch sec; seqNo:14923; depth 18.45 2021-02-13T00:41:45Z- handleDvl() - RDI_Pathfinder msg: 1613176905.97 epoch sec; seqNo:7161 2021-02-13T00:41:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 102.92 , 0.00 2021-02-13T00:41:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.00 , 0.06 2021-02-13T00:41:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176905.98 epoch sec; seqNo:14923; 4059814.83 north; 592982.70 east 2021-02-13T00:41:46Z- AHRS_M2 msg: 1613176906.35 epoch sec; seqNo:14806 2021-02-13T00:41:46Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.16 psi 2021-02-13T00:41:46Z- handleDepth()-Depth_Keller msg: 1613176906361 epoch sec; seqNo:14924; depth 18.33 2021-02-13T00:41:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176906.37 epoch sec; seqNo:14924; 4059815.21 north; 592982.70 east 2021-02-13T00:41:46Z- AHRS_M2 msg: 1613176906.76 epoch sec; seqNo:14807 2021-02-13T00:41:46Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T00:41:46Z- handleDepth()-Depth_Keller msg: 1613176906765 epoch sec; seqNo:14925; depth 18.23 2021-02-13T00:41:46Z- handleDvl() - RDI_Pathfinder msg: 1613176906.78 epoch sec; seqNo:7162 2021-02-13T00:41:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.71 , 97.64 , 104.40 , 78.71 2021-02-13T00:41:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.06 , -0.01 2021-02-13T00:41:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176906.79 epoch sec; seqNo:14925; 4059815.21 north; 592982.70 east 2021-02-13T00:41:47Z- AHRS_M2 msg: 1613176907.16 epoch sec; seqNo:14808 2021-02-13T00:41:47Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-13T00:41:47Z- handleDepth()-Depth_Keller msg: 1613176907170 epoch sec; seqNo:14926; depth 18.13 2021-02-13T00:41:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176907.19 epoch sec; seqNo:14926; 4059815.59 north; 592982.69 east 2021-02-13T00:41:47Z- AHRS_M2 msg: 1613176907.59 epoch sec; seqNo:14809 2021-02-13T00:41:47Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-13T00:41:47Z- handleDepth()-Depth_Keller msg: 1613176907598 epoch sec; seqNo:14927; depth 18.03 2021-02-13T00:41:47Z- handleDvl() - RDI_Pathfinder msg: 1613176907.61 epoch sec; seqNo:7163 2021-02-13T00:41:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 97.78 , 103.86 , 70.70 2021-02-13T00:41:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , -0.09 2021-02-13T00:41:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176907.62 epoch sec; seqNo:14927; 4059815.98 north; 592983.91 east 2021-02-13T00:41:47Z- handleDepth()-Depth_Keller msg: 1613176907989 epoch sec; seqNo:14928; depth 17.94 2021-02-13T00:41:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176908.00 epoch sec; seqNo:14928; 4059816.36 north; 592983.90 east 2021-02-13T00:41:48Z- AHRS_M2 msg: 1613176908.38 epoch sec; seqNo:14810 2021-02-13T00:41:48Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-13T00:41:48Z- handleDepth()-Depth_Keller msg: 1613176908384 epoch sec; seqNo:14929; depth 17.82 2021-02-13T00:41:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176908.39 epoch sec; seqNo:14929; 4059816.36 north; 592983.90 east 2021-02-13T00:41:48Z- AHRS_M2 msg: 1613176908.78 epoch sec; seqNo:14811 2021-02-13T00:41:48Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-13T00:41:48Z- handleDepth()-Depth_Keller msg: 1613176908786 epoch sec; seqNo:14930; depth 17.71 2021-02-13T00:41:48Z- handleDvl() - RDI_Pathfinder msg: 1613176908.80 epoch sec; seqNo:7164 2021-02-13T00:41:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.06 , 97.98 , 104.07 , 70.29 2021-02-13T00:41:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.10 , -0.10 2021-02-13T00:41:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176908.81 epoch sec; seqNo:14930; 4059816.74 north; 592983.90 east 2021-02-13T00:41:49Z- AHRS_M2 msg: 1613176909.18 epoch sec; seqNo:14812 2021-02-13T00:41:49Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-13T00:41:49Z- handleDepth()-Depth_Keller msg: 1613176909184 epoch sec; seqNo:14931; depth 17.60 2021-02-13T00:41:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176909.20 epoch sec; seqNo:14931; 4059817.12 north; 592983.90 east 2021-02-13T00:41:49Z- LcmTrn::handleMessages() - handling 40 messages took 0.57 ms and 129 calls... 2021-02-13T00:41:49Z- waiting for 1613176907.35, time is 1613176909.20 2021-02-13T00:41:49Z- LcmTrn::updateTrn() >>>> pose time 1613176909.20, meas time 1613176908.80 <<<< 2021-02-13T00:41:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176908.80 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:41:49Z- TerrainNav::motionUpdate() - currEstimate = (4059817, 592983.90, 17.60) 2021-02-13T00:41:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:41:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:41:49Z- TNavBF::Projecting Measurements 2021-02-13T00:41:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:41:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:41:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:41:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:41:49Z- TNavFilter::4 good beams. 2021-02-13T00:41:49Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:41:49Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:41:49Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:41:49Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:41:49Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:41:49Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:41:49Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:41:49Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:49Z- TerrainMapOctree: (vehN, vehE ) = (4059816.83, 592983.69). 2021-02-13T00:41:49Z- TerrainMapOctree: Min Distance = 1912.46. 2021-02-13T00:41:49Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:41:49Z- There is no specific good beam that all particles have in common. 2021-02-13T00:41:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:41:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 9.75 2021-02-13T00:41:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:41:49Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:41:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:41:49Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 17.33 2021-02-13T00:41:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:41:49Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 7.91 2021-02-13T00:41:49Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:41:49Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:41:49Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:41:49Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:41:49Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:41:49Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:41:49Z- Filter 0 No beams used in subcloud update 2021-02-13T00:41:49Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:41:49Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:41:49Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 691.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:41:49Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176908.80 sec. 2021-02-13T00:41:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:41:49Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:49Z- variances([0], [2], and psi_berg) = 691.477960 832.993662 0.000000 2021-02-13T00:41:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:41:49Z- LcmTrn::publishEstimates() - MLE : 4059816.62 593004.40 17.60 2021-02-13T00:41:49Z- LcmTrn::publishEstimates() - MMSE : 4059817.12 592983.90 17.60 2021-02-13T00:41:49Z- LcmTrn::publishEstimates() - COVAR: 691.48 832.99 0.79 0.00 2021-02-13T00:41:49Z- AHRS_M2 msg: 1613176909.58 epoch sec; seqNo:14813 2021-02-13T00:41:49Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-13T00:41:49Z- handleDepth()-Depth_Keller msg: 1613176909588 epoch sec; seqNo:14932; depth 17.50 2021-02-13T00:41:49Z- handleDvl() - RDI_Pathfinder msg: 1613176909.60 epoch sec; seqNo:7165 2021-02-13T00:41:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.06 , 97.98 , 105.28 , 71.93 2021-02-13T00:41:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.15 , -0.01 2021-02-13T00:41:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176909.62 epoch sec; seqNo:14932; 4059817.50 north; 592983.89 east 2021-02-13T00:41:49Z- AHRS_M2 msg: 1613176909.99 epoch sec; seqNo:14814 2021-02-13T00:41:49Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.16 psi 2021-02-13T00:41:49Z- handleDepth()-Depth_Keller msg: 1613176909992 epoch sec; seqNo:14933; depth 17.40 2021-02-13T00:41:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:41:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:41:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:41:49Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:41:49Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:41:49Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:41:49Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:41:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:41:49Z- handleDvl() - RDI_Pathfinder msg: 1613176909.99 epoch sec; seqNo:7166 2021-02-13T00:41:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:41:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.15 , -0.01 2021-02-13T00:41:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176910.01 epoch sec; seqNo:14933; 4059817.88 north; 592983.89 east 2021-02-13T00:41:50Z- AHRS_M2 msg: 1613176910.39 epoch sec; seqNo:14815 2021-02-13T00:41:50Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.15 psi 2021-02-13T00:41:50Z- handleDepth()-Depth_Keller msg: 1613176910398 epoch sec; seqNo:14934; depth 17.30 2021-02-13T00:41:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176910.41 epoch sec; seqNo:14934; 4059817.88 north; 592983.89 east 2021-02-13T00:41:50Z- AHRS_M2 msg: 1613176910.80 epoch sec; seqNo:14816 2021-02-13T00:41:50Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-13T00:41:50Z- handleDepth()-Depth_Keller msg: 1613176910802 epoch sec; seqNo:14935; depth 17.19 2021-02-13T00:41:50Z- handleDvl() - RDI_Pathfinder msg: 1613176910.83 epoch sec; seqNo:7167 2021-02-13T00:41:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.66 , 94.26 , 104.00 , 78.44 2021-02-13T00:41:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.12 , 0.10 2021-02-13T00:41:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176910.84 epoch sec; seqNo:14935; 4059818.27 north; 592985.10 east 2021-02-13T00:41:51Z- AHRS_M2 msg: 1613176911.20 epoch sec; seqNo:14817 2021-02-13T00:41:51Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.15 psi 2021-02-13T00:41:51Z- handleDepth()-Depth_Keller msg: 1613176911204 epoch sec; seqNo:14936; depth 17.06 2021-02-13T00:41:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:41:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:41:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:41:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:41:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:41:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:41:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:41:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:41:51Z- handleDvl() - RDI_Pathfinder msg: 1613176911.21 epoch sec; seqNo:7168 2021-02-13T00:41:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:41:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.12 , 0.10 2021-02-13T00:41:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176911.22 epoch sec; seqNo:14936; 4059818.65 north; 592985.10 east 2021-02-13T00:41:51Z- AHRS_M2 msg: 1613176911.60 epoch sec; seqNo:14818 2021-02-13T00:41:51Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T00:41:51Z- handleDepth()-Depth_Keller msg: 1613176911617 epoch sec; seqNo:14937; depth 16.97 2021-02-13T00:41:51Z- handleDvl() - RDI_Pathfinder msg: 1613176911.63 epoch sec; seqNo:7169 2021-02-13T00:41:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.69 , 97.85 , 105.15 , 69.48 2021-02-13T00:41:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , 0.14 2021-02-13T00:41:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176911.64 epoch sec; seqNo:14937; 4059819.03 north; 592985.10 east 2021-02-13T00:41:52Z- AHRS_M2 msg: 1613176912.01 epoch sec; seqNo:14819 2021-02-13T00:41:52Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T00:41:52Z- handleDepth()-Depth_Keller msg: 1613176912012 epoch sec; seqNo:14938; depth 16.86 2021-02-13T00:41:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176912.03 epoch sec; seqNo:14938; 4059819.40 north; 592985.09 east 2021-02-13T00:41:52Z- AHRS_M2 msg: 1613176912.43 epoch sec; seqNo:14820 2021-02-13T00:41:52Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T00:41:52Z- handleDepth()-Depth_Keller msg: 1613176912447 epoch sec; seqNo:14939; depth 16.75 2021-02-13T00:41:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:41:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:41:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:41:52Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:41:52Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:41:52Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:41:52Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:41:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:41:52Z- handleDvl() - RDI_Pathfinder msg: 1613176912.45 epoch sec; seqNo:7170 2021-02-13T00:41:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:41:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , 0.14 2021-02-13T00:41:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176912.46 epoch sec; seqNo:14939; 4059819.78 north; 592985.09 east 2021-02-13T00:41:52Z- AHRS_M2 msg: 1613176912.82 epoch sec; seqNo:14821 2021-02-13T00:41:52Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.16 psi 2021-02-13T00:41:52Z- handleDepth()-Depth_Keller msg: 1613176912825 epoch sec; seqNo:14940; depth 16.62 2021-02-13T00:41:52Z- handleDvl() - RDI_Pathfinder msg: 1613176912.84 epoch sec; seqNo:7171 2021-02-13T00:41:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.88 , 97.91 , 105.21 , 78.44 2021-02-13T00:41:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.08 , 0.09 2021-02-13T00:41:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176912.85 epoch sec; seqNo:14940; 4059819.80 north; 592986.31 east 2021-02-13T00:41:53Z- AHRS_M2 msg: 1613176913.22 epoch sec; seqNo:14822 2021-02-13T00:41:53Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T00:41:53Z- handleDepth()-Depth_Keller msg: 1613176913229 epoch sec; seqNo:14941; depth 16.52 2021-02-13T00:41:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176913.24 epoch sec; seqNo:14941; 4059820.18 north; 592986.30 east 2021-02-13T00:41:53Z- AHRS_M2 msg: 1613176913.62 epoch sec; seqNo:14823 2021-02-13T00:41:53Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:41:53Z- handleDepth()-Depth_Keller msg: 1613176913628 epoch sec; seqNo:14942; depth 16.43 2021-02-13T00:41:53Z- handleDvl() - RDI_Pathfinder msg: 1613176913.65 epoch sec; seqNo:7172 2021-02-13T00:41:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.69 , 97.71 , 107.45 , 71.52 2021-02-13T00:41:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.12 , -0.05 2021-02-13T00:41:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176913.66 epoch sec; seqNo:14942; 4059820.55 north; 592986.30 east 2021-02-13T00:41:54Z- AHRS_M2 msg: 1613176914.03 epoch sec; seqNo:14824 2021-02-13T00:41:54Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-13T00:41:54Z- handleDepth()-Depth_Keller msg: 1613176914032 epoch sec; seqNo:14943; depth 16.32 2021-02-13T00:41:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176914.04 epoch sec; seqNo:14943; 4059820.93 north; 592986.30 east 2021-02-13T00:41:54Z- AHRS_M2 msg: 1613176914.43 epoch sec; seqNo:14825 2021-02-13T00:41:54Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-13T00:41:54Z- handleDepth()-Depth_Keller msg: 1613176914436 epoch sec; seqNo:14944; depth 16.21 2021-02-13T00:41:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176914.44 epoch sec; seqNo:14944; 4059820.93 north; 592986.30 east 2021-02-13T00:41:54Z- LcmTrn::handleMessages() - handling 47 messages took 0.61 ms and 135 calls... 2021-02-13T00:41:54Z- waiting for 1613176912.20, time is 1613176914.44 2021-02-13T00:41:54Z- LcmTrn::updateTrn() >>>> pose time 1613176914.44, meas time 1613176913.65 <<<< 2021-02-13T00:41:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176913.65 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:41:54Z- TerrainNav::motionUpdate() - currEstimate = (4059821, 592986.30, 16.21) 2021-02-13T00:41:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:41:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:41:54Z- TNavBF::Projecting Measurements 2021-02-13T00:41:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:41:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:41:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:41:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:41:54Z- TNavFilter::4 good beams. 2021-02-13T00:41:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:41:54Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:41:54Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:41:54Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:41:54Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:41:54Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:41:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:41:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:54Z- TerrainMapOctree: (vehN, vehE ) = (4059820.35, 592985.93). 2021-02-13T00:41:54Z- TerrainMapOctree: Min Distance = 1908.34. 2021-02-13T00:41:54Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:41:54Z- There is no specific good beam that all particles have in common. 2021-02-13T00:41:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:41:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 10.16 2021-02-13T00:41:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:41:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:41:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:41:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 18.47 2021-02-13T00:41:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:41:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 8.18 2021-02-13T00:41:54Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:41:54Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:41:54Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:41:54Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:41:54Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:41:54Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:41:54Z- Filter 0 No beams used in subcloud update 2021-02-13T00:41:54Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:41:54Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:41:54Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 691.4 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:41:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176913.65 sec. 2021-02-13T00:41:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:41:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:54Z- variances([0], [2], and psi_berg) = 691.409161 832.993629 0.000000 2021-02-13T00:41:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:41:54Z- LcmTrn::publishEstimates() - MLE : 4059820.43 593006.80 16.21 2021-02-13T00:41:54Z- LcmTrn::publishEstimates() - MMSE : 4059820.93 592986.30 16.21 2021-02-13T00:41:54Z- LcmTrn::publishEstimates() - COVAR: 691.41 832.99 0.80 0.00 2021-02-13T00:41:54Z- AHRS_M2 msg: 1613176914.84 epoch sec; seqNo:14826 2021-02-13T00:41:54Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-13T00:41:54Z- handleDepth()-Depth_Keller msg: 1613176914849 epoch sec; seqNo:14945; depth 16.11 2021-02-13T00:41:54Z- handleDvl() - RDI_Pathfinder msg: 1613176914.86 epoch sec; seqNo:7173 2021-02-13T00:41:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.03 , 97.85 , 106.36 , 78.37 2021-02-13T00:41:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.06 , -0.15 2021-02-13T00:41:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176914.87 epoch sec; seqNo:14945; 4059821.33 north; 592987.51 east 2021-02-13T00:41:55Z- AHRS_M2 msg: 1613176915.24 epoch sec; seqNo:14827 2021-02-13T00:41:55Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.14 psi 2021-02-13T00:41:55Z- handleDepth()-Depth_Keller msg: 1613176915244 epoch sec; seqNo:14946; depth 16.00 2021-02-13T00:41:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176915.26 epoch sec; seqNo:14946; 4059821.70 north; 592987.51 east 2021-02-13T00:41:55Z- AHRS_M2 msg: 1613176915.64 epoch sec; seqNo:14828 2021-02-13T00:41:55Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.14 psi 2021-02-13T00:41:55Z- handleDepth()-Depth_Keller msg: 1613176915650 epoch sec; seqNo:14947; depth 15.90 2021-02-13T00:41:55Z- handleDvl() - RDI_Pathfinder msg: 1613176915.66 epoch sec; seqNo:7174 2021-02-13T00:41:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.69 , 97.64 , 106.16 , 78.17 2021-02-13T00:41:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , -0.14 2021-02-13T00:41:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176915.67 epoch sec; seqNo:14947; 4059821.70 north; 592987.51 east 2021-02-13T00:41:56Z- AHRS_M2 msg: 1613176916.05 epoch sec; seqNo:14829 2021-02-13T00:41:56Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.14 psi 2021-02-13T00:41:56Z- handleDepth()-Depth_Keller msg: 1613176916053 epoch sec; seqNo:14948; depth 15.77 2021-02-13T00:41:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176916.07 epoch sec; seqNo:14948; 4059822.08 north; 592987.50 east 2021-02-13T00:41:56Z- AHRS_M2 msg: 1613176916.46 epoch sec; seqNo:14830 2021-02-13T00:41:56Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.14 psi 2021-02-13T00:41:56Z- handleDepth()-Depth_Keller msg: 1613176916465 epoch sec; seqNo:14949; depth 15.69 2021-02-13T00:41:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176916.49 epoch sec; seqNo:14949; 4059822.46 north; 592987.50 east 2021-02-13T00:41:56Z- AHRS_M2 msg: 1613176916.86 epoch sec; seqNo:14831 2021-02-13T00:41:56Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-13T00:41:56Z- handleDepth()-Depth_Keller msg: 1613176916860 epoch sec; seqNo:14950; depth 15.57 2021-02-13T00:41:56Z- handleDvl() - RDI_Pathfinder msg: 1613176916.87 epoch sec; seqNo:7175 2021-02-13T00:41:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 97.71 , 107.45 , 73.97 2021-02-13T00:41:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , -0.04 2021-02-13T00:41:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176916.88 epoch sec; seqNo:14950; 4059822.84 north; 592987.50 east 2021-02-13T00:41:57Z- AHRS_M2 msg: 1613176917.26 epoch sec; seqNo:14832 2021-02-13T00:41:57Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:41:57Z- handleDepth()-Depth_Keller msg: 1613176917266 epoch sec; seqNo:14951; depth 15.45 2021-02-13T00:41:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176917.28 epoch sec; seqNo:14951; 4059822.84 north; 592987.50 east 2021-02-13T00:41:57Z- AHRS_M2 msg: 1613176917.67 epoch sec; seqNo:14833 2021-02-13T00:41:57Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-13T00:41:57Z- handleDepth()-Depth_Keller msg: 1613176917679 epoch sec; seqNo:14952; depth 15.34 2021-02-13T00:41:57Z- handleDvl() - RDI_Pathfinder msg: 1613176917.70 epoch sec; seqNo:7176 2021-02-13T00:41:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 97.71 , 107.45 , 78.24 2021-02-13T00:41:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.08 , 0.08 2021-02-13T00:41:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176917.71 epoch sec; seqNo:14952; 4059823.23 north; 592988.71 east 2021-02-13T00:41:58Z- AHRS_M2 msg: 1613176918.07 epoch sec; seqNo:14834 2021-02-13T00:41:58Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-13T00:41:58Z- handleDepth()-Depth_Keller msg: 1613176918072 epoch sec; seqNo:14953; depth 15.21 2021-02-13T00:41:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176918.08 epoch sec; seqNo:14953; 4059823.61 north; 592988.71 east 2021-02-13T00:41:58Z- AHRS_M2 msg: 1613176918.47 epoch sec; seqNo:14835 2021-02-13T00:41:58Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T00:41:58Z- handleDepth()-Depth_Keller msg: 1613176918476 epoch sec; seqNo:14954; depth 15.12 2021-02-13T00:41:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176918.49 epoch sec; seqNo:14954; 4059823.99 north; 592988.70 east 2021-02-13T00:41:58Z- AHRS_M2 msg: 1613176918.88 epoch sec; seqNo:14836 2021-02-13T00:41:58Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T00:41:58Z- handleDepth()-Depth_Keller msg: 1613176918885 epoch sec; seqNo:14955; depth 15.01 2021-02-13T00:41:58Z- handleDvl() - RDI_Pathfinder msg: 1613176918.90 epoch sec; seqNo:7177 2021-02-13T00:41:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 97.64 , 108.60 , 73.15 2021-02-13T00:41:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.05 , 0.12 2021-02-13T00:41:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176918.91 epoch sec; seqNo:14955; 4059824.37 north; 592988.70 east 2021-02-13T00:41:59Z- AHRS_M2 msg: 1613176919.28 epoch sec; seqNo:14837 2021-02-13T00:41:59Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-13T00:41:59Z- handleDepth()-Depth_Keller msg: 1613176919284 epoch sec; seqNo:14956; depth 14.89 2021-02-13T00:41:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176919.30 epoch sec; seqNo:14956; 4059824.37 north; 592988.70 east 2021-02-13T00:41:59Z- AHRS_M2 msg: 1613176919.68 epoch sec; seqNo:14838 2021-02-13T00:41:59Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-13T00:41:59Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-13T00:41:59Z- waiting for 1613176917.44, time is 1613176919.68 2021-02-13T00:41:59Z- LcmTrn::updateTrn() >>>> pose time 1613176919.68, meas time 1613176918.90 <<<< 2021-02-13T00:41:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176918.90 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:41:59Z- TerrainNav::motionUpdate() - currEstimate = (4059824, 592988.70, 14.89) 2021-02-13T00:41:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:41:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:41:59Z- TNavBF::Projecting Measurements 2021-02-13T00:41:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:41:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:41:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:41:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:41:59Z- TNavFilter::4 good beams. 2021-02-13T00:41:59Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:41:59Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:41:59Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:41:59Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:41:59Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:41:59Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:41:59Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:41:59Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:59Z- TerrainMapOctree: (vehN, vehE ) = (4059823.86, 592988.34). 2021-02-13T00:41:59Z- TerrainMapOctree: Min Distance = 1904.12. 2021-02-13T00:41:59Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:41:59Z- There is no specific good beam that all particles have in common. 2021-02-13T00:41:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:41:59Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 10.71 2021-02-13T00:41:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:41:59Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 15.25 2021-02-13T00:41:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:41:59Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 18.87 2021-02-13T00:41:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:41:59Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-13T00:41:59Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:41:59Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:41:59Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:41:59Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:41:59Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:41:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:41:59Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:41:59Z- Filter 0 No beams used in subcloud update 2021-02-13T00:41:59Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:41:59Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:41:59Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 691.4 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:41:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176918.90 sec. 2021-02-13T00:41:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:41:59Z- TNavBF::mmse selected from filter -1 2021-02-13T00:41:59Z- variances([0], [2], and psi_berg) = 691.342951 832.993597 0.000000 2021-02-13T00:41:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:41:59Z- LcmTrn::publishEstimates() - MLE : 4059823.87 593009.20 14.89 2021-02-13T00:41:59Z- LcmTrn::publishEstimates() - MMSE : 4059824.37 592988.70 14.89 2021-02-13T00:41:59Z- LcmTrn::publishEstimates() - COVAR: 691.34 832.99 0.80 0.00 2021-02-13T00:41:59Z- handleDepth()-Depth_Keller msg: 1613176919689 epoch sec; seqNo:14957; depth 14.78 2021-02-13T00:41:59Z- handleDvl() - RDI_Pathfinder msg: 1613176919.70 epoch sec; seqNo:7178 2021-02-13T00:41:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 98.93 , 107.45 , 78.24 2021-02-13T00:41:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.15 , 0.05 2021-02-13T00:41:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176919.71 epoch sec; seqNo:14957; 4059824.75 north; 592988.70 east 2021-02-13T00:42:00Z- AHRS_M2 msg: 1613176920.09 epoch sec; seqNo:14839 2021-02-13T00:42:00Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-13T00:42:00Z- handleDepth()-Depth_Keller msg: 1613176920095 epoch sec; seqNo:14958; depth 14.67 2021-02-13T00:42:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176920.11 epoch sec; seqNo:14958; 4059825.14 north; 592989.91 east 2021-02-13T00:42:00Z- AHRS_M2 msg: 1613176920.49 epoch sec; seqNo:14840 2021-02-13T00:42:00Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-13T00:42:00Z- handleDepth()-Depth_Keller msg: 1613176920496 epoch sec; seqNo:14959; depth 14.56 2021-02-13T00:42:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176920.51 epoch sec; seqNo:14959; 4059825.52 north; 592989.91 east 2021-02-13T00:42:00Z- AHRS_M2 msg: 1613176920.90 epoch sec; seqNo:14841 2021-02-13T00:42:00Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-13T00:42:00Z- handleDepth()-Depth_Keller msg: 1613176920900 epoch sec; seqNo:14960; depth 14.46 2021-02-13T00:42:00Z- handleDvl() - RDI_Pathfinder msg: 1613176920.91 epoch sec; seqNo:7179 2021-02-13T00:42:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 98.86 , 111.03 , 78.17 2021-02-13T00:42:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.14 , -0.10 2021-02-13T00:42:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176920.92 epoch sec; seqNo:14960; 4059825.52 north; 592989.91 east 2021-02-13T00:42:01Z- AHRS_M2 msg: 1613176921.31 epoch sec; seqNo:14842 2021-02-13T00:42:01Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T00:42:01Z- handleDepth()-Depth_Keller msg: 1613176921312 epoch sec; seqNo:14961; depth 14.34 2021-02-13T00:42:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176921.32 epoch sec; seqNo:14961; 4059825.90 north; 592989.90 east 2021-02-13T00:42:01Z- AHRS_M2 msg: 1613176921.70 epoch sec; seqNo:14843 2021-02-13T00:42:01Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-13T00:42:01Z- handleDepth()-Depth_Keller msg: 1613176921708 epoch sec; seqNo:14962; depth 14.22 2021-02-13T00:42:01Z- handleDvl() - RDI_Pathfinder msg: 1613176921.73 epoch sec; seqNo:7180 2021-02-13T00:42:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 99.00 , 111.16 , 0.00 2021-02-13T00:42:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.08 , -0.20 2021-02-13T00:42:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176921.74 epoch sec; seqNo:14962; 4059825.90 north; 592989.90 east 2021-02-13T00:42:02Z- AHRS_M2 msg: 1613176922.11 epoch sec; seqNo:14844 2021-02-13T00:42:02Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.16 psi 2021-02-13T00:42:02Z- handleDepth()-Depth_Keller msg: 1613176922117 epoch sec; seqNo:14963; depth 14.11 2021-02-13T00:42:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176922.14 epoch sec; seqNo:14963; 4059826.29 north; 592991.12 east 2021-02-13T00:42:02Z- AHRS_M2 msg: 1613176922.51 epoch sec; seqNo:14845 2021-02-13T00:42:02Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.16 psi 2021-02-13T00:42:02Z- handleDepth()-Depth_Keller msg: 1613176922516 epoch sec; seqNo:14964; depth 14.02 2021-02-13T00:42:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176922.53 epoch sec; seqNo:14964; 4059826.67 north; 592991.12 east 2021-02-13T00:42:02Z- AHRS_M2 msg: 1613176922.92 epoch sec; seqNo:14846 2021-02-13T00:42:02Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.16 psi 2021-02-13T00:42:02Z- handleDepth()-Depth_Keller msg: 1613176922922 epoch sec; seqNo:14965; depth 13.90 2021-02-13T00:42:02Z- handleDvl() - RDI_Pathfinder msg: 1613176922.93 epoch sec; seqNo:7181 2021-02-13T00:42:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.36 , 98.59 , 110.76 , 78.31 2021-02-13T00:42:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , -0.21 2021-02-13T00:42:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176922.94 epoch sec; seqNo:14965; 4059826.67 north; 592991.12 east 2021-02-13T00:42:03Z- AHRS_M2 msg: 1613176923.32 epoch sec; seqNo:14847 2021-02-13T00:42:03Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.16 psi 2021-02-13T00:42:03Z- handleDepth()-Depth_Keller msg: 1613176923335 epoch sec; seqNo:14966; depth 13.79 2021-02-13T00:42:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176923.35 epoch sec; seqNo:14966; 4059827.05 north; 592991.11 east 2021-02-13T00:42:03Z- AHRS_M2 msg: 1613176923.72 epoch sec; seqNo:14848 2021-02-13T00:42:03Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.17 psi 2021-02-13T00:42:03Z- handleDepth()-Depth_Keller msg: 1613176923728 epoch sec; seqNo:14967; depth 13.66 2021-02-13T00:42:03Z- handleDvl() - RDI_Pathfinder msg: 1613176923.74 epoch sec; seqNo:7182 2021-02-13T00:42:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 99.06 , 109.88 , 74.38 2021-02-13T00:42:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.16 , -0.11 2021-02-13T00:42:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176923.76 epoch sec; seqNo:14967; 4059827.43 north; 592991.11 east 2021-02-13T00:42:04Z- AHRS_M2 msg: 1613176924.13 epoch sec; seqNo:14849 2021-02-13T00:42:04Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.18 psi 2021-02-13T00:42:04Z- handleDepth()-Depth_Keller msg: 1613176924135 epoch sec; seqNo:14968; depth 13.54 2021-02-13T00:42:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176924.15 epoch sec; seqNo:14968; 4059827.43 north; 592991.11 east 2021-02-13T00:42:04Z- AHRS_M2 msg: 1613176924.53 epoch sec; seqNo:14850 2021-02-13T00:42:04Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.18 psi 2021-02-13T00:42:04Z- handleDepth()-Depth_Keller msg: 1613176924537 epoch sec; seqNo:14969; depth 13.43 2021-02-13T00:42:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176924.55 epoch sec; seqNo:14969; 4059827.81 north; 592991.10 east 2021-02-13T00:42:04Z- LcmTrn::handleMessages() - handling 43 messages took 0.63 ms and 131 calls... 2021-02-13T00:42:04Z- waiting for 1613176922.68, time is 1613176924.55 2021-02-13T00:42:04Z- LcmTrn::updateTrn() >>>> pose time 1613176924.55, meas time 1613176923.74 <<<< 2021-02-13T00:42:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176923.74 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:42:04Z- TerrainNav::motionUpdate() - currEstimate = (4059828, 592991.10, 13.43) 2021-02-13T00:42:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:42:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:42:04Z- TNavBF::Projecting Measurements 2021-02-13T00:42:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:42:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:42:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:42:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:42:04Z- TNavFilter::4 good beams. 2021-02-13T00:42:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:42:04Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:42:04Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:42:04Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:42:04Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:42:04Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:42:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:42:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:42:04Z- TerrainMapOctree: (vehN, vehE ) = (4059827.24, 592990.70). 2021-02-13T00:42:04Z- TerrainMapOctree: Min Distance = 1900.02. 2021-02-13T00:42:04Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:42:04Z- There is no specific good beam that all particles have in common. 2021-02-13T00:42:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:42:04Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 10.98 2021-02-13T00:42:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:42:04Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 15.70 2021-02-13T00:42:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:42:04Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 19.32 2021-02-13T00:42:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:42:04Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-13T00:42:04Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:42:04Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:42:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:42:04Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:42:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:42:04Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:42:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:42:04Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:42:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:42:04Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:42:04Z- Filter 0 No beams used in subcloud update 2021-02-13T00:42:04Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:42:04Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:42:04Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 691.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:42:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176923.74 sec. 2021-02-13T00:42:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:42:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:42:04Z- variances([0], [2], and psi_berg) = 691.276187 832.993565 0.000000 2021-02-13T00:42:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:42:04Z- LcmTrn::publishEstimates() - MLE : 4059827.31 593011.60 13.43 2021-02-13T00:42:04Z- LcmTrn::publishEstimates() - MMSE : 4059827.81 592991.10 13.43 2021-02-13T00:42:04Z- LcmTrn::publishEstimates() - COVAR: 691.28 832.99 0.80 0.00 2021-02-13T00:42:04Z- AHRS_M2 msg: 1613176924.94 epoch sec; seqNo:14851 2021-02-13T00:42:04Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.19 psi 2021-02-13T00:42:04Z- handleDepth()-Depth_Keller msg: 1613176924940 epoch sec; seqNo:14970; depth 13.31 2021-02-13T00:42:04Z- handleDvl() - RDI_Pathfinder msg: 1613176924.96 epoch sec; seqNo:7183 2021-02-13T00:42:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 98.86 , 111.03 , 78.17 2021-02-13T00:42:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.17 , 0.08 2021-02-13T00:42:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176924.97 epoch sec; seqNo:14970; 4059828.18 north; 592991.10 east 2021-02-13T00:42:05Z- AHRS_M2 msg: 1613176925.35 epoch sec; seqNo:14852 2021-02-13T00:42:05Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.19 psi 2021-02-13T00:42:05Z- handleDepth()-Depth_Keller msg: 1613176925350 epoch sec; seqNo:14971; depth 13.19 2021-02-13T00:42:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176925.38 epoch sec; seqNo:14971; 4059828.58 north; 592992.32 east 2021-02-13T00:42:05Z- AHRS_M2 msg: 1613176925.75 epoch sec; seqNo:14853 2021-02-13T00:42:05Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.19 psi 2021-02-13T00:42:05Z- handleDepth()-Depth_Keller msg: 1613176925751 epoch sec; seqNo:14972; depth 13.09 2021-02-13T00:42:05Z- handleDvl() - RDI_Pathfinder msg: 1613176925.76 epoch sec; seqNo:7184 2021-02-13T00:42:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 100.35 , 109.81 , 0.00 2021-02-13T00:42:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.03 , 0.23 2021-02-13T00:42:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176925.77 epoch sec; seqNo:14972; 4059828.95 north; 592992.31 east 2021-02-13T00:42:06Z- AHRS_M2 msg: 1613176926.15 epoch sec; seqNo:14854 2021-02-13T00:42:06Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.19 psi 2021-02-13T00:42:06Z- handleDepth()-Depth_Keller msg: 1613176926161 epoch sec; seqNo:14973; depth 12.96 2021-02-13T00:42:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176926.17 epoch sec; seqNo:14973; 4059829.33 north; 592992.31 east 2021-02-13T00:42:06Z- AHRS_M2 msg: 1613176926.55 epoch sec; seqNo:14855 2021-02-13T00:42:06Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.19 psi 2021-02-13T00:42:06Z- handleDepth()-Depth_Keller msg: 1613176926556 epoch sec; seqNo:14974; depth 12.88 2021-02-13T00:42:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176926.57 epoch sec; seqNo:14974; 4059829.33 north; 592992.31 east 2021-02-13T00:42:06Z- AHRS_M2 msg: 1613176926.96 epoch sec; seqNo:14856 2021-02-13T00:42:06Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.18 psi 2021-02-13T00:42:06Z- handleDepth()-Depth_Keller msg: 1613176926960 epoch sec; seqNo:14975; depth 12.78 2021-02-13T00:42:06Z- handleDvl() - RDI_Pathfinder msg: 1613176926.97 epoch sec; seqNo:7185 2021-02-13T00:42:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.37 , 99.94 , 112.11 , 78.31 2021-02-13T00:42:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.10 , 0.20 2021-02-13T00:42:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176926.99 epoch sec; seqNo:14975; 4059829.71 north; 592992.30 east 2021-02-13T00:42:07Z- AHRS_M2 msg: 1613176927.36 epoch sec; seqNo:14857 2021-02-13T00:42:07Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.17 psi 2021-02-13T00:42:07Z- handleDepth()-Depth_Keller msg: 1613176927369 epoch sec; seqNo:14976; depth 12.65 2021-02-13T00:42:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176927.38 epoch sec; seqNo:14976; 4059830.10 north; 592993.52 east 2021-02-13T00:42:07Z- AHRS_M2 msg: 1613176927.77 epoch sec; seqNo:14858 2021-02-13T00:42:07Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T00:42:07Z- handleDepth()-Depth_Keller msg: 1613176927777 epoch sec; seqNo:14977; depth 12.56 2021-02-13T00:42:07Z- handleDvl() - RDI_Pathfinder msg: 1613176927.79 epoch sec; seqNo:7186 2021-02-13T00:42:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 100.35 , 113.87 , 78.71 2021-02-13T00:42:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.18 , 0.08 2021-02-13T00:42:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176927.80 epoch sec; seqNo:14977; 4059830.48 north; 592993.52 east 2021-02-13T00:42:08Z- AHRS_M2 msg: 1613176928.17 epoch sec; seqNo:14859 2021-02-13T00:42:08Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-13T00:42:08Z- handleDepth()-Depth_Keller msg: 1613176928178 epoch sec; seqNo:14978; depth 12.45 2021-02-13T00:42:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176928.19 epoch sec; seqNo:14978; 4059830.48 north; 592993.52 east 2021-02-13T00:42:08Z- AHRS_M2 msg: 1613176928.57 epoch sec; seqNo:14860 2021-02-13T00:42:08Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-13T00:42:08Z- handleDepth()-Depth_Keller msg: 1613176928576 epoch sec; seqNo:14979; depth 12.35 2021-02-13T00:42:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176928.61 epoch sec; seqNo:14979; 4059830.86 north; 592993.51 east 2021-02-13T00:42:08Z- AHRS_M2 msg: 1613176928.97 epoch sec; seqNo:14861 2021-02-13T00:42:08Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-13T00:42:08Z- handleDepth()-Depth_Keller msg: 1613176928980 epoch sec; seqNo:14980; depth 12.25 2021-02-13T00:42:08Z- handleDvl() - RDI_Pathfinder msg: 1613176928.99 epoch sec; seqNo:7187 2021-02-13T00:42:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:42:08Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:42:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176929.00 epoch sec; seqNo:14980; 4059831.24 north; 592993.51 east 2021-02-13T00:42:09Z- AHRS_M2 msg: 1613176929.39 epoch sec; seqNo:14862 2021-02-13T00:42:09Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.13 psi 2021-02-13T00:42:09Z- handleDepth()-Depth_Keller msg: 1613176929394 epoch sec; seqNo:14981; depth 12.14 2021-02-13T00:42:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176929.40 epoch sec; seqNo:14981; 4059831.25 north; 592994.73 east 2021-02-13T00:42:09Z- AHRS_M2 msg: 1613176929.78 epoch sec; seqNo:14863 2021-02-13T00:42:09Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-13T00:42:09Z- handleDepth()-Depth_Keller msg: 1613176929797 epoch sec; seqNo:14982; depth 12.03 2021-02-13T00:42:09Z- handleDvl() - RDI_Pathfinder msg: 1613176929.81 epoch sec; seqNo:7188 2021-02-13T00:42:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.08 , 97.98 , 111.37 , 78.64 2021-02-13T00:42:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.09 , -0.10 2021-02-13T00:42:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176929.82 epoch sec; seqNo:14982; 4059831.63 north; 592994.72 east 2021-02-13T00:42:09Z- LcmTrn::handleMessages() - handling 45 messages took 0.68 ms and 132 calls... 2021-02-13T00:42:09Z- waiting for 1613176927.55, time is 1613176929.82 2021-02-13T00:42:09Z- LcmTrn::updateTrn() equating pose 1613176929.82 and meas times 1613176929.82 2021-02-13T00:42:09Z- LcmTrn::updateTrn() >>>> pose time 1613176929.82, meas time 1613176929.82 <<<< 2021-02-13T00:42:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176929.82 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:42:09Z- TerrainNav::motionUpdate() - currEstimate = (4059832, 592994.72, 12.03) 2021-02-13T00:42:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:42:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:42:09Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176929.82 sec.; No relevant navigation data available 2021-02-13T00:42:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:42:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:42:09Z- variances([0], [2], and psi_berg) = 691.219546 832.993538 0.000000 2021-02-13T00:42:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:42:09Z- LcmTrn::publishEstimates() - MLE : 4059831.13 593015.22 12.03 2021-02-13T00:42:09Z- LcmTrn::publishEstimates() - MMSE : 4059831.63 592994.72 12.03 2021-02-13T00:42:09Z- LcmTrn::publishEstimates() - COVAR: 691.22 832.99 1.07 0.00 2021-02-13T00:42:10Z- AHRS_M2 msg: 1613176930.19 epoch sec; seqNo:14864 2021-02-13T00:42:10Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-13T00:42:10Z- handleDepth()-Depth_Keller msg: 1613176930192 epoch sec; seqNo:14983; depth 11.89 2021-02-13T00:42:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176930.20 epoch sec; seqNo:14983; 4059832.01 north; 592994.72 east 2021-02-13T00:42:10Z- AHRS_M2 msg: 1613176930.60 epoch sec; seqNo:14865 2021-02-13T00:42:10Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-13T00:42:10Z- handleDepth()-Depth_Keller msg: 1613176930602 epoch sec; seqNo:14984; depth 11.78 2021-02-13T00:42:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176930.64 epoch sec; seqNo:14984; 4059832.01 north; 592994.72 east 2021-02-13T00:42:10Z- AHRS_M2 msg: 1613176931.00 epoch sec; seqNo:14866 2021-02-13T00:42:10Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T00:42:11Z- handleDepth()-Depth_Keller msg: 1613176931005 epoch sec; seqNo:14985; depth 11.67 2021-02-13T00:42:11Z- handleDvl() - RDI_Pathfinder msg: 1613176931.02 epoch sec; seqNo:7189 2021-02-13T00:42:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 100.28 , 113.80 , 78.64 2021-02-13T00:42:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , -0.06 2021-02-13T00:42:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176931.03 epoch sec; seqNo:14985; 4059832.39 north; 592994.72 east 2021-02-13T00:42:11Z- AHRS_M2 msg: 1613176931.40 epoch sec; seqNo:14867 2021-02-13T00:42:11Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-13T00:42:11Z- handleDepth()-Depth_Keller msg: 1613176931409 epoch sec; seqNo:14986; depth 11.55 2021-02-13T00:42:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176931.42 epoch sec; seqNo:14986; 4059832.78 north; 592995.93 east 2021-02-13T00:42:11Z- AHRS_M2 msg: 1613176931.80 epoch sec; seqNo:14868 2021-02-13T00:42:11Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-13T00:42:11Z- handleDepth()-Depth_Keller msg: 1613176931808 epoch sec; seqNo:14987; depth 11.43 2021-02-13T00:42:11Z- handleDvl() - RDI_Pathfinder msg: 1613176931.83 epoch sec; seqNo:7190 2021-02-13T00:42:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.27 , 115.09 , 75.60 2021-02-13T00:42:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.00 , 0.00 2021-02-13T00:42:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176931.84 epoch sec; seqNo:14987; 4059833.16 north; 592995.93 east 2021-02-13T00:42:12Z- AHRS_M2 msg: 1613176932.21 epoch sec; seqNo:14869 2021-02-13T00:42:12Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-13T00:42:12Z- handleDepth()-Depth_Keller msg: 1613176932213 epoch sec; seqNo:14988; depth 11.30 2021-02-13T00:42:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176932.22 epoch sec; seqNo:14988; 4059833.16 north; 592995.93 east 2021-02-13T00:42:12Z- handleDepth()-Depth_Keller msg: 1613176932633 epoch sec; seqNo:14989; depth 11.20 2021-02-13T00:42:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176932.64 epoch sec; seqNo:14989; 4059833.54 north; 592995.92 east 2021-02-13T00:42:13Z- AHRS_M2 msg: 1613176933.02 epoch sec; seqNo:14870 2021-02-13T00:42:13Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-13T00:42:13Z- handleDepth()-Depth_Keller msg: 1613176933021 epoch sec; seqNo:14990; depth 11.09 2021-02-13T00:42:13Z- handleDvl() - RDI_Pathfinder msg: 1613176933.04 epoch sec; seqNo:7191 2021-02-13T00:42:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.36 , 103.93 , 116.10 , 78.25 2021-02-13T00:42:13Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.06 , -0.01 2021-02-13T00:42:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176933.05 epoch sec; seqNo:14990; 4059833.92 north; 592995.92 east 2021-02-13T00:42:13Z- AHRS_M2 msg: 1613176933.42 epoch sec; seqNo:14871 2021-02-13T00:42:13Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-13T00:42:13Z- handleDepth()-Depth_Keller msg: 1613176933424 epoch sec; seqNo:14991; depth 10.97 2021-02-13T00:42:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176933.43 epoch sec; seqNo:14991; 4059834.30 north; 592995.92 east 2021-02-13T00:42:13Z- AHRS_M2 msg: 1613176933.82 epoch sec; seqNo:14872 2021-02-13T00:42:13Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-13T00:42:13Z- handleDepth()-Depth_Keller msg: 1613176933828 epoch sec; seqNo:14992; depth 10.86 2021-02-13T00:42:13Z- handleDvl() - RDI_Pathfinder msg: 1613176933.84 epoch sec; seqNo:7192 2021-02-13T00:42:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.43 , 116.30 , 78.44 2021-02-13T00:42:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.11 , -0.12 2021-02-13T00:42:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176933.85 epoch sec; seqNo:14992; 4059834.31 north; 592997.14 east 2021-02-13T00:42:14Z- AHRS_M2 msg: 1613176934.23 epoch sec; seqNo:14873 2021-02-13T00:42:14Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-13T00:42:14Z- handleDepth()-Depth_Keller msg: 1613176934238 epoch sec; seqNo:14993; depth 10.77 2021-02-13T00:42:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176934.25 epoch sec; seqNo:14993; 4059834.69 north; 592997.13 east 2021-02-13T00:42:14Z- AHRS_M2 msg: 1613176934.63 epoch sec; seqNo:14874 2021-02-13T00:42:14Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-13T00:42:14Z- handleDepth()-Depth_Keller msg: 1613176934636 epoch sec; seqNo:14994; depth 10.64 2021-02-13T00:42:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176934.65 epoch sec; seqNo:14994; 4059835.07 north; 592997.13 east 2021-02-13T00:42:14Z- LcmTrn::handleMessages() - handling 39 messages took 0.63 ms and 127 calls... 2021-02-13T00:42:14Z- waiting for 1613176932.82, time is 1613176934.65 2021-02-13T00:42:14Z- LcmTrn::updateTrn() >>>> pose time 1613176934.65, meas time 1613176933.84 <<<< 2021-02-13T00:42:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176933.84 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:42:14Z- TerrainNav::motionUpdate() - currEstimate = (4059835, 592997.13, 10.64) 2021-02-13T00:42:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:42:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:42:14Z- TNavBF::Projecting Measurements 2021-02-13T00:42:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:42:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:42:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:42:14Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:42:14Z- TNavFilter::3 good beams. 2021-02-13T00:42:14Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:42:14Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:42:14Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:42:14Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:42:14Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:42:14Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:42:14Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:42:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:42:14Z- TerrainMapOctree: (vehN, vehE ) = (4059834.50, 592996.73). 2021-02-13T00:42:14Z- TerrainMapOctree: Min Distance = 1890.59. 2021-02-13T00:42:14Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:42:14Z- There is no specific good beam that all particles have in common. 2021-02-13T00:42:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:42:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 16.46 2021-02-13T00:42:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:42:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 21.64 2021-02-13T00:42:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:42:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T00:42:15Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:42:15Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:42:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:42:15Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:42:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:42:15Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:42:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:42:15Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:42:15Z- Filter 0 No beams used in subcloud update 2021-02-13T00:42:15Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:42:15Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:42:15Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 691.2 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:42:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176933.84 sec. 2021-02-13T00:42:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:42:15Z- TNavBF::mmse selected from filter -1 2021-02-13T00:42:15Z- variances([0], [2], and psi_berg) = 691.152828 832.993506 0.000000 2021-02-13T00:42:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:42:15Z- LcmTrn::publishEstimates() - MLE : 4059834.57 593017.63 10.64 2021-02-13T00:42:15Z- LcmTrn::publishEstimates() - MMSE : 4059835.07 592997.13 10.64 2021-02-13T00:42:15Z- LcmTrn::publishEstimates() - COVAR: 691.15 832.99 1.05 0.00 2021-02-13T00:42:15Z- AHRS_M2 msg: 1613176935.04 epoch sec; seqNo:14875 2021-02-13T00:42:15Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-13T00:42:15Z- handleDepth()-Depth_Keller msg: 1613176935040 epoch sec; seqNo:14995; depth 10.54 2021-02-13T00:42:15Z- handleDvl() - RDI_Pathfinder msg: 1613176935.05 epoch sec; seqNo:7193 2021-02-13T00:42:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.35 , 99.87 , 114.75 , 78.24 2021-02-13T00:42:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.10 , -0.20 2021-02-13T00:42:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176935.06 epoch sec; seqNo:14995; 4059835.07 north; 592997.13 east 2021-02-13T00:42:15Z- AHRS_M2 msg: 1613176935.44 epoch sec; seqNo:14876 2021-02-13T00:42:15Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.15 psi 2021-02-13T00:42:15Z- handleDepth()-Depth_Keller msg: 1613176935446 epoch sec; seqNo:14996; depth 10.42 2021-02-13T00:42:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176935.46 epoch sec; seqNo:14996; 4059835.45 north; 592997.12 east 2021-02-13T00:42:15Z- AHRS_M2 msg: 1613176935.84 epoch sec; seqNo:14877 2021-02-13T00:42:15Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.16 psi 2021-02-13T00:42:15Z- handleDepth()-Depth_Keller msg: 1613176935858 epoch sec; seqNo:14997; depth 10.32 2021-02-13T00:42:15Z- handleDvl() - RDI_Pathfinder msg: 1613176935.87 epoch sec; seqNo:7194 2021-02-13T00:42:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 101.56 , 113.73 , 78.58 2021-02-13T00:42:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.04 , -0.21 2021-02-13T00:42:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176935.88 epoch sec; seqNo:14997; 4059835.83 north; 592997.12 east 2021-02-13T00:42:16Z- AHRS_M2 msg: 1613176936.25 epoch sec; seqNo:14878 2021-02-13T00:42:16Z- AHRS_M2 msg: 0.63 phi; 0.29 theta; -0.18 psi 2021-02-13T00:42:16Z- handleDepth()-Depth_Keller msg: 1613176936252 epoch sec; seqNo:14998; depth 10.20 2021-02-13T00:42:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176936.26 epoch sec; seqNo:14998; 4059835.83 north; 592997.12 east 2021-02-13T00:42:16Z- AHRS_M2 msg: 1613176936.65 epoch sec; seqNo:14879 2021-02-13T00:42:16Z- AHRS_M2 msg: 0.63 phi; 0.28 theta; -0.20 psi 2021-02-13T00:42:16Z- handleDepth()-Depth_Keller msg: 1613176936661 epoch sec; seqNo:14999; depth 10.10 2021-02-13T00:42:16Z- handleDvl() - RDI_Pathfinder msg: 1613176936.67 epoch sec; seqNo:7195 2021-02-13T00:42:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.27 , 105.55 , 112.31 , 0.00 2021-02-13T00:42:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.17 , -0.05 2021-02-13T00:42:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176936.68 epoch sec; seqNo:14999; 4059836.22 north; 592998.34 east 2021-02-13T00:42:17Z- AHRS_M2 msg: 1613176937.06 epoch sec; seqNo:14880 2021-02-13T00:42:17Z- AHRS_M2 msg: 0.62 phi; 0.28 theta; -0.21 psi 2021-02-13T00:42:17Z- handleDepth()-Depth_Keller msg: 1613176937069 epoch sec; seqNo:15000; depth 9.97 2021-02-13T00:42:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176937.08 epoch sec; seqNo:15000; 4059836.60 north; 592998.33 east 2021-02-13T00:42:17Z- AHRS_M2 msg: 1613176937.46 epoch sec; seqNo:14881 2021-02-13T00:42:17Z- AHRS_M2 msg: 0.61 phi; 0.29 theta; -0.21 psi 2021-02-13T00:42:17Z- handleDepth()-Depth_Keller msg: 1613176937477 epoch sec; seqNo:15001; depth 9.89 2021-02-13T00:42:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176937.49 epoch sec; seqNo:15001; 4059836.60 north; 592998.33 east 2021-02-13T00:42:17Z- AHRS_M2 msg: 1613176937.86 epoch sec; seqNo:14882 2021-02-13T00:42:17Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.21 psi 2021-02-13T00:42:17Z- handleDepth()-Depth_Keller msg: 1613176937868 epoch sec; seqNo:15002; depth 9.77 2021-02-13T00:42:17Z- handleDvl() - RDI_Pathfinder msg: 1613176937.88 epoch sec; seqNo:7196 2021-02-13T00:42:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.44 , 105.08 , 113.19 , 82.09 2021-02-13T00:42:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.17 , 0.21 2021-02-13T00:42:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176937.92 epoch sec; seqNo:15002; 4059836.98 north; 592998.33 east 2021-02-13T00:42:18Z- AHRS_M2 msg: 1613176938.27 epoch sec; seqNo:14883 2021-02-13T00:42:18Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.20 psi 2021-02-13T00:42:18Z- handleDepth()-Depth_Keller msg: 1613176938277 epoch sec; seqNo:15003; depth 9.68 2021-02-13T00:42:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176938.29 epoch sec; seqNo:15003; 4059837.36 north; 592998.32 east 2021-02-13T00:42:18Z- AHRS_M2 msg: 1613176938.67 epoch sec; seqNo:14884 2021-02-13T00:42:18Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.19 psi 2021-02-13T00:42:18Z- handleDepth()-Depth_Keller msg: 1613176938678 epoch sec; seqNo:15004; depth 9.58 2021-02-13T00:42:18Z- handleDvl() - RDI_Pathfinder msg: 1613176938.69 epoch sec; seqNo:7197 2021-02-13T00:42:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.49 , 113.60 , 81.15 2021-02-13T00:42:18Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.03 , 0.30 2021-02-13T00:42:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176938.70 epoch sec; seqNo:15004; 4059837.73 north; 592998.32 east 2021-02-13T00:42:19Z- AHRS_M2 msg: 1613176939.08 epoch sec; seqNo:14885 2021-02-13T00:42:19Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.18 psi 2021-02-13T00:42:19Z- handleDepth()-Depth_Keller msg: 1613176939080 epoch sec; seqNo:15005; depth 9.49 2021-02-13T00:42:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176939.10 epoch sec; seqNo:15005; 4059838.11 north; 592998.32 east 2021-02-13T00:42:19Z- AHRS_M2 msg: 1613176939.48 epoch sec; seqNo:14886 2021-02-13T00:42:19Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.18 psi 2021-02-13T00:42:19Z- handleDepth()-Depth_Keller msg: 1613176939489 epoch sec; seqNo:15006; depth 9.38 2021-02-13T00:42:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176939.51 epoch sec; seqNo:15006; 4059838.51 north; 592999.53 east 2021-02-13T00:42:19Z- AHRS_M2 msg: 1613176939.88 epoch sec; seqNo:14887 2021-02-13T00:42:19Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.17 psi 2021-02-13T00:42:19Z- handleDepth()-Depth_Keller msg: 1613176939888 epoch sec; seqNo:15007; depth 9.27 2021-02-13T00:42:19Z- handleDvl() - RDI_Pathfinder msg: 1613176939.90 epoch sec; seqNo:7198 2021-02-13T00:42:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.92 , 106.50 , 117.32 , 80.81 2021-02-13T00:42:19Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.20 , 0.18 2021-02-13T00:42:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176939.93 epoch sec; seqNo:15007; 4059838.88 north; 592999.53 east 2021-02-13T00:42:20Z- LcmTrn::handleMessages() - handling 45 messages took 0.74 ms and 130 calls... 2021-02-13T00:42:20Z- waiting for 1613176937.65, time is 1613176939.93 2021-02-13T00:42:20Z- LcmTrn::updateTrn() equating pose 1613176939.93 and meas times 1613176939.93 2021-02-13T00:42:20Z- LcmTrn::updateTrn() >>>> pose time 1613176939.93, meas time 1613176939.93 <<<< 2021-02-13T00:42:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176939.93 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:42:20Z- TerrainNav::motionUpdate() - currEstimate = (4059839, 592999.53, 9.27) 2021-02-13T00:42:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:42:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:42:20Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176939.93 sec.; No relevant navigation data available 2021-02-13T00:42:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:42:20Z- TNavBF::mmse selected from filter -1 2021-02-13T00:42:20Z- variances([0], [2], and psi_berg) = 691.100511 832.993481 0.000000 2021-02-13T00:42:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:42:20Z- LcmTrn::publishEstimates() - MLE : 4059838.38 593020.03 9.27 2021-02-13T00:42:20Z- LcmTrn::publishEstimates() - MMSE : 4059838.88 592999.53 9.27 2021-02-13T00:42:20Z- LcmTrn::publishEstimates() - COVAR: 691.10 832.99 1.28 0.00 2021-02-13T00:42:20Z- AHRS_M2 msg: 1613176940.29 epoch sec; seqNo:14888 2021-02-13T00:42:20Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.17 psi 2021-02-13T00:42:20Z- handleDepth()-Depth_Keller msg: 1613176940293 epoch sec; seqNo:15008; depth 9.18 2021-02-13T00:42:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176940.31 epoch sec; seqNo:15008; 4059838.88 north; 592999.53 east 2021-02-13T00:42:20Z- AHRS_M2 msg: 1613176940.71 epoch sec; seqNo:14889 2021-02-13T00:42:20Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.17 psi 2021-02-13T00:42:20Z- handleDepth()-Depth_Keller msg: 1613176940715 epoch sec; seqNo:15009; depth 9.07 2021-02-13T00:42:20Z- handleDvl() - RDI_Pathfinder msg: 1613176940.73 epoch sec; seqNo:7199 2021-02-13T00:42:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.17 , 105.28 , 120.16 , 80.94 2021-02-13T00:42:20Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.23 , 0.00 2021-02-13T00:42:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176940.74 epoch sec; seqNo:15009; 4059839.26 north; 592999.52 east 2021-02-13T00:42:21Z- AHRS_M2 msg: 1613176941.10 epoch sec; seqNo:14890 2021-02-13T00:42:21Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.16 psi 2021-02-13T00:42:21Z- handleDepth()-Depth_Keller msg: 1613176941100 epoch sec; seqNo:15010; depth 8.94 2021-02-13T00:42:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176941.11 epoch sec; seqNo:15010; 4059839.66 north; 593000.74 east 2021-02-13T00:42:21Z- AHRS_M2 msg: 1613176941.50 epoch sec; seqNo:14891 2021-02-13T00:42:21Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.16 psi 2021-02-13T00:42:21Z- handleDepth()-Depth_Keller msg: 1613176941513 epoch sec; seqNo:15011; depth 8.84 2021-02-13T00:42:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176941.53 epoch sec; seqNo:15011; 4059839.66 north; 593000.74 east 2021-02-13T00:42:21Z- AHRS_M2 msg: 1613176941.90 epoch sec; seqNo:14892 2021-02-13T00:42:21Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.17 psi 2021-02-13T00:42:21Z- handleDepth()-Depth_Keller msg: 1613176941922 epoch sec; seqNo:15012; depth 8.72 2021-02-13T00:42:21Z- handleDvl() - RDI_Pathfinder msg: 1613176941.93 epoch sec; seqNo:7200 2021-02-13T00:42:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.85 , 106.43 , 122.66 , 0.00 2021-02-13T00:42:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.25 , -0.11 2021-02-13T00:42:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176941.95 epoch sec; seqNo:15012; 4059840.03 north; 593000.74 east 2021-02-13T00:42:22Z- AHRS_M2 msg: 1613176942.31 epoch sec; seqNo:14893 2021-02-13T00:42:22Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.17 psi 2021-02-13T00:42:22Z- handleDepth()-Depth_Keller msg: 1613176942317 epoch sec; seqNo:15013; depth 8.61 2021-02-13T00:42:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176942.33 epoch sec; seqNo:15013; 4059840.41 north; 593000.73 east 2021-02-13T00:42:22Z- AHRS_M2 msg: 1613176942.74 epoch sec; seqNo:14894 2021-02-13T00:42:22Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.16 psi 2021-02-13T00:42:22Z- handleDepth()-Depth_Keller msg: 1613176942747 epoch sec; seqNo:15014; depth 8.51 2021-02-13T00:42:22Z- handleDvl() - RDI_Pathfinder msg: 1613176942.76 epoch sec; seqNo:7201 2021-02-13T00:42:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.57 , 106.50 , 122.86 , 81.21 2021-02-13T00:42:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.16 , -0.22 2021-02-13T00:42:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176942.77 epoch sec; seqNo:15014; 4059840.41 north; 593000.73 east 2021-02-13T00:42:23Z- AHRS_M2 msg: 1613176943.12 epoch sec; seqNo:14895 2021-02-13T00:42:23Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.16 psi 2021-02-13T00:42:23Z- handleDepth()-Depth_Keller msg: 1613176943125 epoch sec; seqNo:15015; depth 8.38 2021-02-13T00:42:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176943.15 epoch sec; seqNo:15015; 4059840.81 north; 593001.95 east 2021-02-13T00:42:23Z- AHRS_M2 msg: 1613176943.52 epoch sec; seqNo:14896 2021-02-13T00:42:23Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-13T00:42:23Z- handleDepth()-Depth_Keller msg: 1613176943529 epoch sec; seqNo:15016; depth 8.26 2021-02-13T00:42:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176943.54 epoch sec; seqNo:15016; 4059840.81 north; 593001.95 east 2021-02-13T00:42:23Z- AHRS_M2 msg: 1613176943.92 epoch sec; seqNo:14897 2021-02-13T00:42:23Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.16 psi 2021-02-13T00:42:23Z- handleDepth()-Depth_Keller msg: 1613176943929 epoch sec; seqNo:15017; depth 8.15 2021-02-13T00:42:23Z- handleDvl() - RDI_Pathfinder msg: 1613176943.94 epoch sec; seqNo:7202 2021-02-13T00:42:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.91 , 106.84 , 124.69 , 80.07 2021-02-13T00:42:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.01 , -0.29 2021-02-13T00:42:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176943.95 epoch sec; seqNo:15017; 4059841.18 north; 593001.94 east 2021-02-13T00:42:24Z- AHRS_M2 msg: 1613176944.33 epoch sec; seqNo:14898 2021-02-13T00:42:24Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-13T00:42:24Z- handleDepth()-Depth_Keller msg: 1613176944342 epoch sec; seqNo:15018; depth 8.02 2021-02-13T00:42:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176944.35 epoch sec; seqNo:15018; 4059841.18 north; 593001.94 east 2021-02-13T00:42:24Z- AHRS_M2 msg: 1613176944.73 epoch sec; seqNo:14899 2021-02-13T00:42:24Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.18 psi 2021-02-13T00:42:24Z- handleDepth()-Depth_Keller msg: 1613176944736 epoch sec; seqNo:15019; depth 7.88 2021-02-13T00:42:24Z- handleDvl() - RDI_Pathfinder msg: 1613176944.75 epoch sec; seqNo:7203 2021-02-13T00:42:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 108.32 , 124.69 , 81.55 2021-02-13T00:42:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.22 , -0.19 2021-02-13T00:42:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176944.77 epoch sec; seqNo:15019; 4059841.56 north; 593001.94 east 2021-02-13T00:42:25Z- LcmTrn::handleMessages() - handling 41 messages took 0.67 ms and 128 calls... 2021-02-13T00:42:25Z- waiting for 1613176942.93, time is 1613176944.77 2021-02-13T00:42:25Z- LcmTrn::updateTrn() equating pose 1613176944.77 and meas times 1613176944.77 2021-02-13T00:42:25Z- LcmTrn::updateTrn() >>>> pose time 1613176944.77, meas time 1613176944.77 <<<< 2021-02-13T00:42:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176944.77 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:42:25Z- TerrainNav::motionUpdate() - currEstimate = (4059842, 593001.94, 7.88) 2021-02-13T00:42:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:42:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:42:25Z- TNavBF::Projecting Measurements 2021-02-13T00:42:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:42:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:42:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:42:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:42:25Z- TNavFilter::4 good beams. 2021-02-13T00:42:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:42:25Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:42:25Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:42:25Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:42:25Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:42:25Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:42:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:42:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:42:25Z- TerrainMapOctree: (vehN, vehE ) = (4059841.56, 593001.94). 2021-02-13T00:42:25Z- TerrainMapOctree: Min Distance = 1881.85. 2021-02-13T00:42:25Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:42:25Z- There is no specific good beam that all particles have in common. 2021-02-13T00:42:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:42:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.46 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 12.67 2021-02-13T00:42:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:42:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 18.77 2021-02-13T00:42:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:42:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 24.87 2021-02-13T00:42:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:42:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 10.64 2021-02-13T00:42:25Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:42:25Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:42:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:42:25Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:42:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:42:25Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:42:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:42:25Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:42:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:42:25Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:42:25Z- Filter 0 No beams used in subcloud update 2021-02-13T00:42:25Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:42:25Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:42:25Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 691.1 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:42:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176944.77 sec. 2021-02-13T00:42:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:42:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:42:25Z- variances([0], [2], and psi_berg) = 691.053750 832.993459 0.000000 2021-02-13T00:42:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:42:25Z- LcmTrn::publishEstimates() - MLE : 4059841.06 593022.44 7.88 2021-02-13T00:42:25Z- LcmTrn::publishEstimates() - MMSE : 4059841.56 593001.94 7.88 2021-02-13T00:42:25Z- LcmTrn::publishEstimates() - COVAR: 691.05 832.99 1.09 0.00 2021-02-13T00:42:25Z- AHRS_M2 msg: 1613176945.14 epoch sec; seqNo:14900 2021-02-13T00:42:25Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.19 psi 2021-02-13T00:42:25Z- handleDepth()-Depth_Keller msg: 1613176945144 epoch sec; seqNo:15020; depth 7.76 2021-02-13T00:42:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176945.16 epoch sec; seqNo:15020; 4059841.94 north; 593001.94 east 2021-02-13T00:42:25Z- AHRS_M2 msg: 1613176945.54 epoch sec; seqNo:14901 2021-02-13T00:42:25Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.19 psi 2021-02-13T00:42:25Z- handleDepth()-Depth_Keller msg: 1613176945553 epoch sec; seqNo:15021; depth 7.62 2021-02-13T00:42:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176945.56 epoch sec; seqNo:15021; 4059841.94 north; 593001.94 east 2021-02-13T00:42:25Z- AHRS_M2 msg: 1613176945.94 epoch sec; seqNo:14902 2021-02-13T00:42:25Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.19 psi 2021-02-13T00:42:25Z- handleDepth()-Depth_Keller msg: 1613176945948 epoch sec; seqNo:15022; depth 7.50 2021-02-13T00:42:25Z- handleDvl() - RDI_Pathfinder msg: 1613176945.97 epoch sec; seqNo:7204 2021-02-13T00:42:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.50 , 110.49 , 124.01 , 82.09 2021-02-13T00:42:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.29 , 0.08 2021-02-13T00:42:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176945.98 epoch sec; seqNo:15022; 4059842.32 north; 593001.93 east 2021-02-13T00:42:26Z- AHRS_M2 msg: 1613176946.35 epoch sec; seqNo:14903 2021-02-13T00:42:26Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.19 psi 2021-02-13T00:42:26Z- handleDepth()-Depth_Keller msg: 1613176946357 epoch sec; seqNo:15023; depth 7.37 2021-02-13T00:42:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176946.37 epoch sec; seqNo:15023; 4059842.70 north; 593001.93 east 2021-02-13T00:42:26Z- AHRS_M2 msg: 1613176946.75 epoch sec; seqNo:14904 2021-02-13T00:42:26Z- AHRS_M2 msg: 0.55 phi; 0.37 theta; -0.20 psi 2021-02-13T00:42:26Z- handleDepth()-Depth_Keller msg: 1613176946761 epoch sec; seqNo:15024; depth 7.25 2021-02-13T00:42:26Z- handleDvl() - RDI_Pathfinder msg: 1613176946.77 epoch sec; seqNo:7205 2021-02-13T00:42:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.86 , 113.19 , 122.66 , 0.00 2021-02-13T00:42:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.14 , 0.28 2021-02-13T00:42:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176946.78 epoch sec; seqNo:15024; 4059843.08 north; 593001.92 east 2021-02-13T00:42:27Z- AHRS_M2 msg: 1613176947.16 epoch sec; seqNo:14905 2021-02-13T00:42:27Z- AHRS_M2 msg: 0.53 phi; 0.38 theta; -0.20 psi 2021-02-13T00:42:27Z- handleDepth()-Depth_Keller msg: 1613176947163 epoch sec; seqNo:15025; depth 7.12 2021-02-13T00:42:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176947.17 epoch sec; seqNo:15025; 4059843.47 north; 593003.14 east 2021-02-13T00:42:27Z- handleDepth()-Depth_Keller msg: 1613176947581 epoch sec; seqNo:15026; depth 6.99 2021-02-13T00:42:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176947.59 epoch sec; seqNo:15026; 4059843.85 north; 593003.14 east 2021-02-13T00:42:27Z- AHRS_M2 msg: 1613176947.96 epoch sec; seqNo:14906 2021-02-13T00:42:27Z- AHRS_M2 msg: 0.52 phi; 0.39 theta; -0.19 psi 2021-02-13T00:42:27Z- handleDepth()-Depth_Keller msg: 1613176947973 epoch sec; seqNo:15027; depth 6.87 2021-02-13T00:42:27Z- handleDvl() - RDI_Pathfinder msg: 1613176947.99 epoch sec; seqNo:7206 2021-02-13T00:42:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.38 , 110.76 , 131.58 , 82.50 2021-02-13T00:42:27Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.11 , 0.24 2021-02-13T00:42:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176948.00 epoch sec; seqNo:15027; 4059844.23 north; 593003.13 east 2021-02-13T00:42:28Z- AHRS_M2 msg: 1613176948.37 epoch sec; seqNo:14907 2021-02-13T00:42:28Z- AHRS_M2 msg: 0.53 phi; 0.40 theta; -0.18 psi 2021-02-13T00:42:28Z- handleDepth()-Depth_Keller msg: 1613176948372 epoch sec; seqNo:15028; depth 6.75 2021-02-13T00:42:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176948.38 epoch sec; seqNo:15028; 4059844.61 north; 593003.13 east 2021-02-13T00:42:28Z- AHRS_M2 msg: 1613176948.78 epoch sec; seqNo:14908 2021-02-13T00:42:28Z- AHRS_M2 msg: 0.55 phi; 0.39 theta; -0.17 psi 2021-02-13T00:42:28Z- handleDepth()-Depth_Keller msg: 1613176948782 epoch sec; seqNo:15029; depth 6.63 2021-02-13T00:42:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176948.80 epoch sec; seqNo:15029; 4059844.99 north; 593003.12 east 2021-02-13T00:42:29Z- AHRS_M2 msg: 1613176949.18 epoch sec; seqNo:14909 2021-02-13T00:42:29Z- AHRS_M2 msg: 0.57 phi; 0.38 theta; -0.16 psi 2021-02-13T00:42:29Z- handleDepth()-Depth_Keller msg: 1613176949193 epoch sec; seqNo:15030; depth 6.50 2021-02-13T00:42:29Z- handleDvl() - RDI_Pathfinder msg: 1613176949.20 epoch sec; seqNo:7207 2021-02-13T00:42:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.64 , 112.52 , 131.85 , 81.28 2021-02-13T00:42:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.26 , 0.02 2021-02-13T00:42:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176949.21 epoch sec; seqNo:15030; 4059845.00 north; 593004.35 east 2021-02-13T00:42:29Z- AHRS_M2 msg: 1613176949.58 epoch sec; seqNo:14910 2021-02-13T00:42:29Z- AHRS_M2 msg: 0.58 phi; 0.38 theta; -0.14 psi 2021-02-13T00:42:29Z- handleDepth()-Depth_Keller msg: 1613176949584 epoch sec; seqNo:15031; depth 6.36 2021-02-13T00:42:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176949.60 epoch sec; seqNo:15031; 4059845.38 north; 593004.34 east 2021-02-13T00:42:29Z- AHRS_M2 msg: 1613176949.98 epoch sec; seqNo:14911 2021-02-13T00:42:29Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.13 psi 2021-02-13T00:42:29Z- handleDepth()-Depth_Keller msg: 1613176949989 epoch sec; seqNo:15032; depth 6.23 2021-02-13T00:42:30Z- handleDvl() - RDI_Pathfinder msg: 1613176950.00 epoch sec; seqNo:7208 2021-02-13T00:42:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.58 , 109.48 , 130.30 , 82.71 2021-02-13T00:42:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.18 , -0.15 2021-02-13T00:42:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176950.01 epoch sec; seqNo:15032; 4059845.76 north; 593004.34 east 2021-02-13T00:42:30Z- LcmTrn::handleMessages() - handling 43 messages took 0.60 ms and 131 calls... 2021-02-13T00:42:30Z- waiting for 1613176947.77, time is 1613176950.01 2021-02-13T00:42:30Z- LcmTrn::updateTrn() equating pose 1613176950.01 and meas times 1613176950.01 2021-02-13T00:42:30Z- LcmTrn::updateTrn() >>>> pose time 1613176950.01, meas time 1613176950.01 <<<< 2021-02-13T00:42:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176950.01 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:42:30Z- TerrainNav::motionUpdate() - currEstimate = (4059846, 593004.34, 6.23) 2021-02-13T00:42:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:42:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:42:30Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176950.01 sec.; No relevant navigation data available 2021-02-13T00:42:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:42:30Z- TNavBF::mmse selected from filter -1 2021-02-13T00:42:30Z- variances([0], [2], and psi_berg) = 690.999647 832.993433 0.000000 2021-02-13T00:42:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:42:30Z- LcmTrn::publishEstimates() - MLE : 4059845.26 593024.84 6.23 2021-02-13T00:42:30Z- LcmTrn::publishEstimates() - MMSE : 4059845.76 593004.34 6.23 2021-02-13T00:42:30Z- LcmTrn::publishEstimates() - COVAR: 691.00 832.99 1.33 0.00 2021-02-13T00:42:30Z- AHRS_M2 msg: 1613176950.39 epoch sec; seqNo:14912 2021-02-13T00:42:30Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.12 psi 2021-02-13T00:42:30Z- handleDepth()-Depth_Keller msg: 1613176950394 epoch sec; seqNo:15033; depth 6.08 2021-02-13T00:42:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176950.40 epoch sec; seqNo:15033; 4059845.76 north; 593004.34 east 2021-02-13T00:42:30Z- AHRS_M2 msg: 1613176950.79 epoch sec; seqNo:14913 2021-02-13T00:42:30Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.11 psi 2021-02-13T00:42:30Z- handleDepth()-Depth_Keller msg: 1613176950796 epoch sec; seqNo:15034; depth 5.95 2021-02-13T00:42:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176950.81 epoch sec; seqNo:15034; 4059846.14 north; 593004.33 east 2021-02-13T00:42:31Z- AHRS_M2 msg: 1613176951.20 epoch sec; seqNo:14914 2021-02-13T00:42:31Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.09 psi 2021-02-13T00:42:31Z- handleDepth()-Depth_Keller msg: 1613176951205 epoch sec; seqNo:15035; depth 5.82 2021-02-13T00:42:31Z- handleDvl() - RDI_Pathfinder msg: 1613176951.22 epoch sec; seqNo:7209 2021-02-13T00:42:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.64 , 106.57 , 124.42 , 82.77 2021-02-13T00:42:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , -0.08 2021-02-13T00:42:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176951.23 epoch sec; seqNo:15035; 4059846.15 north; 593005.55 east 2021-02-13T00:42:31Z- AHRS_M2 msg: 1613176951.60 epoch sec; seqNo:14915 2021-02-13T00:42:31Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.07 psi 2021-02-13T00:42:31Z- handleDepth()-Depth_Keller msg: 1613176951609 epoch sec; seqNo:15036; depth 5.69 2021-02-13T00:42:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176951.62 epoch sec; seqNo:15036; 4059846.53 north; 593005.55 east 2021-02-13T00:42:32Z- AHRS_M2 msg: 1613176952.01 epoch sec; seqNo:14916 2021-02-13T00:42:32Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.05 psi 2021-02-13T00:42:32Z- handleDepth()-Depth_Keller msg: 1613176952010 epoch sec; seqNo:15037; depth 5.55 2021-02-13T00:42:32Z- handleDvl() - RDI_Pathfinder msg: 1613176952.02 epoch sec; seqNo:7210 2021-02-13T00:42:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 103.79 , 123.13 , 172.21 2021-02-13T00:42:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.03 , -0.07 2021-02-13T00:42:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176952.03 epoch sec; seqNo:15037; 4059846.91 north; 593005.55 east 2021-02-13T00:42:32Z- AHRS_M2 msg: 1613176952.41 epoch sec; seqNo:14917 2021-02-13T00:42:32Z- AHRS_M2 msg: 0.65 phi; 0.24 theta; -0.04 psi 2021-02-13T00:42:32Z- handleDepth()-Depth_Keller msg: 1613176952412 epoch sec; seqNo:15038; depth 5.41 2021-02-13T00:42:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176952.45 epoch sec; seqNo:15038; 4059846.91 north; 593005.55 east 2021-02-13T00:42:32Z- AHRS_M2 msg: 1613176952.81 epoch sec; seqNo:14918 2021-02-13T00:42:32Z- AHRS_M2 msg: 0.66 phi; 0.19 theta; -0.05 psi 2021-02-13T00:42:32Z- handleDepth()-Depth_Keller msg: 1613176952821 epoch sec; seqNo:15039; depth 5.29 2021-02-13T00:42:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176952.83 epoch sec; seqNo:15039; 4059847.29 north; 593005.54 east 2021-02-13T00:42:33Z- AHRS_M2 msg: 1613176953.22 epoch sec; seqNo:14919 2021-02-13T00:42:33Z- AHRS_M2 msg: 0.67 phi; 0.14 theta; -0.08 psi 2021-02-13T00:42:33Z- handleDepth()-Depth_Keller msg: 1613176953220 epoch sec; seqNo:15040; depth 5.17 2021-02-13T00:42:33Z- handleDvl() - RDI_Pathfinder msg: 1613176953.23 epoch sec; seqNo:7211 2021-02-13T00:42:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:42:33Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:42:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176953.24 epoch sec; seqNo:15040; 4059847.66 north; 593005.54 east 2021-02-13T00:42:33Z- AHRS_M2 msg: 1613176953.62 epoch sec; seqNo:14920 2021-02-13T00:42:33Z- AHRS_M2 msg: 0.67 phi; 0.09 theta; -0.11 psi 2021-02-13T00:42:33Z- handleDepth()-Depth_Keller msg: 1613176953625 epoch sec; seqNo:15041; depth 5.06 2021-02-13T00:42:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176953.64 epoch sec; seqNo:15041; 4059847.68 north; 593006.76 east 2021-02-13T00:42:34Z- AHRS_M2 msg: 1613176954.03 epoch sec; seqNo:14921 2021-02-13T00:42:34Z- AHRS_M2 msg: 0.66 phi; 0.05 theta; -0.15 psi 2021-02-13T00:42:34Z- handleDepth()-Depth_Keller msg: 1613176954030 epoch sec; seqNo:15042; depth 4.97 2021-02-13T00:42:34Z- handleDvl() - RDI_Pathfinder msg: 1613176954.05 epoch sec; seqNo:7212 2021-02-13T00:42:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 97.24 , 100.21 , 97.24 2021-02-13T00:42:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.22 , -0.00 2021-02-13T00:42:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176954.06 epoch sec; seqNo:15042; 4059848.06 north; 593006.75 east 2021-02-13T00:42:34Z- AHRS_M2 msg: 1613176954.43 epoch sec; seqNo:14922 2021-02-13T00:42:34Z- AHRS_M2 msg: 0.66 phi; 0.01 theta; -0.18 psi 2021-02-13T00:42:34Z- handleDepth()-Depth_Keller msg: 1613176954432 epoch sec; seqNo:15043; depth 4.88 2021-02-13T00:42:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176954.45 epoch sec; seqNo:15043; 4059848.44 north; 593006.75 east 2021-02-13T00:42:34Z- AHRS_M2 msg: 1613176954.84 epoch sec; seqNo:14923 2021-02-13T00:42:34Z- AHRS_M2 msg: 0.65 phi; -0.03 theta; -0.19 psi 2021-02-13T00:42:34Z- handleDepth()-Depth_Keller msg: 1613176954840 epoch sec; seqNo:15044; depth 4.82 2021-02-13T00:42:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176954.85 epoch sec; seqNo:15044; 4059848.81 north; 593006.75 east 2021-02-13T00:42:35Z- LcmTrn::handleMessages() - handling 40 messages took 0.62 ms and 128 calls... 2021-02-13T00:42:35Z- waiting for 1613176953.01, time is 1613176954.85 2021-02-13T00:42:35Z- LcmTrn::updateTrn() >>>> pose time 1613176954.85, meas time 1613176954.05 <<<< 2021-02-13T00:42:35Z- TerrainNav:: Throwing out all beams because more than two are equal to 97.237 at t=1613176954.05, ping # 0. 2021-02-13T00:42:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176954.05 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:42:35Z- TerrainNav::motionUpdate() - currEstimate = (4059849, 593006.75, 4.82) 2021-02-13T00:42:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:42:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:42:35Z- TNavBF::Projecting Measurements 2021-02-13T00:42:35Z- TNavFilter::Measurement from time = 1613176954.05 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:42:35Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:42:35Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:42:35Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:42:35Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:42:35Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:42:35Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:42:35Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:42:35Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 691.0 EastVariance: 833.0 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-13T00:42:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:42:35Z- TNavBF::mmse selected from filter -1 2021-02-13T00:42:35Z- variances([0], [2], and psi_berg) = 690.935596 832.993402 0.000000 2021-02-13T00:42:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:42:35Z- LcmTrn::publishEstimates() - MLE : 4059848.31 593027.25 4.82 2021-02-13T00:42:35Z- LcmTrn::publishEstimates() - MMSE : 4059848.81 593006.75 4.82 2021-02-13T00:42:35Z- LcmTrn::publishEstimates() - COVAR: 690.94 832.99 1.53 0.00 2021-02-13T00:42:35Z- AHRS_M2 msg: 1613176955.24 epoch sec; seqNo:14924 2021-02-13T00:42:35Z- AHRS_M2 msg: 0.65 phi; -0.07 theta; -0.19 psi 2021-02-13T00:42:35Z- handleDepth()-Depth_Keller msg: 1613176955242 epoch sec; seqNo:15045; depth 4.79 2021-02-13T00:42:35Z- handleDvl() - RDI_Pathfinder msg: 1613176955.25 epoch sec; seqNo:7213 2021-02-13T00:42:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.35 , 105.28 , 97.17 , 97.17 2021-02-13T00:42:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , 0.28 2021-02-13T00:42:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176955.26 epoch sec; seqNo:15045; 4059848.81 north; 593006.75 east 2021-02-13T00:42:35Z- AHRS_M2 msg: 1613176955.64 epoch sec; seqNo:14925 2021-02-13T00:42:35Z- AHRS_M2 msg: 0.65 phi; -0.10 theta; -0.20 psi 2021-02-13T00:42:35Z- handleDepth()-Depth_Keller msg: 1613176955644 epoch sec; seqNo:15046; depth 4.77 2021-02-13T00:42:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176955.65 epoch sec; seqNo:15046; 4059849.19 north; 593006.74 east 2021-02-13T00:42:36Z- AHRS_M2 msg: 1613176956.05 epoch sec; seqNo:14926 2021-02-13T00:42:36Z- AHRS_M2 msg: 0.65 phi; -0.13 theta; -0.20 psi 2021-02-13T00:42:36Z- handleDepth()-Depth_Keller msg: 1613176956050 epoch sec; seqNo:15047; depth 4.75 2021-02-13T00:42:36Z- handleDvl() - RDI_Pathfinder msg: 1613176956.06 epoch sec; seqNo:7214 2021-02-13T00:42:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 106.77 , 97.31 , 100.01 2021-02-13T00:42:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.30 , 0.14 2021-02-13T00:42:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176956.09 epoch sec; seqNo:15047; 4059849.58 north; 593007.96 east 2021-02-13T00:42:36Z- AHRS_M2 msg: 1613176956.45 epoch sec; seqNo:14927 2021-02-13T00:42:36Z- AHRS_M2 msg: 0.65 phi; -0.15 theta; -0.20 psi 2021-02-13T00:42:36Z- handleDepth()-Depth_Keller msg: 1613176956456 epoch sec; seqNo:15048; depth 4.77 2021-02-13T00:42:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176956.47 epoch sec; seqNo:15048; 4059849.58 north; 593007.96 east 2021-02-13T00:42:36Z- AHRS_M2 msg: 1613176956.85 epoch sec; seqNo:14928 2021-02-13T00:42:36Z- AHRS_M2 msg: 0.66 phi; -0.17 theta; -0.20 psi 2021-02-13T00:42:36Z- handleDepth()-Depth_Keller msg: 1613176956858 epoch sec; seqNo:15049; depth 4.77 2021-02-13T00:42:36Z- handleDvl() - RDI_Pathfinder msg: 1613176956.87 epoch sec; seqNo:7215 2021-02-13T00:42:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:42:36Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:42:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176956.88 epoch sec; seqNo:15049; 4059849.96 north; 593007.95 east 2021-02-13T00:42:37Z- AHRS_M2 msg: 1613176957.26 epoch sec; seqNo:14929 2021-02-13T00:42:37Z- AHRS_M2 msg: 0.66 phi; -0.19 theta; -0.21 psi 2021-02-13T00:42:37Z- handleDepth()-Depth_Keller msg: 1613176957260 epoch sec; seqNo:15050; depth 4.79 2021-02-13T00:42:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176957.28 epoch sec; seqNo:15050; 4059849.96 north; 593007.95 east 2021-02-13T00:42:37Z- AHRS_M2 msg: 1613176957.66 epoch sec; seqNo:14930 2021-02-13T00:42:37Z- AHRS_M2 msg: 0.67 phi; -0.21 theta; -0.23 psi 2021-02-13T00:42:37Z- handleDepth()-Depth_Keller msg: 1613176957669 epoch sec; seqNo:15051; depth 4.82 2021-02-13T00:42:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176957.68 epoch sec; seqNo:15051; 4059849.98 north; 593009.17 east 2021-02-13T00:42:38Z- AHRS_M2 msg: 1613176958.07 epoch sec; seqNo:14931 2021-02-13T00:42:38Z- AHRS_M2 msg: 0.67 phi; -0.22 theta; -0.25 psi 2021-02-13T00:42:38Z- handleDepth()-Depth_Keller msg: 1613176958071 epoch sec; seqNo:15052; depth 4.86 2021-02-13T00:42:38Z- handleDvl() - RDI_Pathfinder msg: 1613176958.08 epoch sec; seqNo:7216 2021-02-13T00:42:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 111.37 , 87.57 , 112.85 2021-02-13T00:42:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.03 , -0.31 2021-02-13T00:42:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176958.09 epoch sec; seqNo:15052; 4059850.36 north; 593009.17 east 2021-02-13T00:42:38Z- AHRS_M2 msg: 1613176958.47 epoch sec; seqNo:14932 2021-02-13T00:42:38Z- AHRS_M2 msg: 0.68 phi; -0.23 theta; -0.27 psi 2021-02-13T00:42:38Z- handleDepth()-Depth_Keller msg: 1613176958472 epoch sec; seqNo:15053; depth 4.92 2021-02-13T00:42:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176958.50 epoch sec; seqNo:15053; 4059850.73 north; 593009.17 east 2021-02-13T00:42:38Z- AHRS_M2 msg: 1613176958.87 epoch sec; seqNo:14933 2021-02-13T00:42:38Z- AHRS_M2 msg: 0.68 phi; -0.24 theta; -0.27 psi 2021-02-13T00:42:38Z- handleDepth()-Depth_Keller msg: 1613176958876 epoch sec; seqNo:15054; depth 4.96 2021-02-13T00:42:38Z- handleDvl() - RDI_Pathfinder msg: 1613176958.89 epoch sec; seqNo:7217 2021-02-13T00:42:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:42:38Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:42:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176958.91 epoch sec; seqNo:15054; 4059851.11 north; 593009.16 east 2021-02-13T00:42:39Z- AHRS_M2 msg: 1613176959.28 epoch sec; seqNo:14934 2021-02-13T00:42:39Z- AHRS_M2 msg: 0.68 phi; -0.26 theta; -0.26 psi 2021-02-13T00:42:39Z- handleDepth()-Depth_Keller msg: 1613176959280 epoch sec; seqNo:15055; depth 5.04 2021-02-13T00:42:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176959.30 epoch sec; seqNo:15055; 4059851.49 north; 593009.16 east 2021-02-13T00:42:39Z- AHRS_M2 msg: 1613176959.69 epoch sec; seqNo:14935 2021-02-13T00:42:39Z- AHRS_M2 msg: 0.68 phi; -0.28 theta; -0.23 psi 2021-02-13T00:42:39Z- handleDepth()-Depth_Keller msg: 1613176959691 epoch sec; seqNo:15056; depth 5.11 2021-02-13T00:42:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176959.70 epoch sec; seqNo:15056; 4059851.49 north; 593009.16 east 2021-02-13T00:42:40Z- AHRS_M2 msg: 1613176960.09 epoch sec; seqNo:14936 2021-02-13T00:42:40Z- AHRS_M2 msg: 0.67 phi; -0.30 theta; -0.22 psi 2021-02-13T00:42:40Z- handleDepth()-Depth_Keller msg: 1613176960093 epoch sec; seqNo:15057; depth 5.19 2021-02-13T00:42:40Z- handleDvl() - RDI_Pathfinder msg: 1613176960.11 epoch sec; seqNo:7218 2021-02-13T00:42:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 115.96 , 0.00 , 121.37 2021-02-13T00:42:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.30 , 0.13 2021-02-13T00:42:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176960.12 epoch sec; seqNo:15057; 4059851.87 north; 593009.15 east 2021-02-13T00:42:40Z- LcmTrn::handleMessages() - handling 45 messages took 0.71 ms and 131 calls... 2021-02-13T00:42:40Z- waiting for 1613176957.85, time is 1613176960.12 2021-02-13T00:42:40Z- LcmTrn::updateTrn() equating pose 1613176960.12 and meas times 1613176960.12 2021-02-13T00:42:40Z- LcmTrn::updateTrn() >>>> pose time 1613176960.12, meas time 1613176960.12 <<<< 2021-02-13T00:42:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176960.12 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:42:40Z- TerrainNav::motionUpdate() - currEstimate = (4059852, 593009.15, 5.19) 2021-02-13T00:42:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:42:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:42:40Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176960.12 sec.; No relevant navigation data available 2021-02-13T00:42:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:42:40Z- TNavBF::mmse selected from filter -1 2021-02-13T00:42:40Z- variances([0], [2], and psi_berg) = 690.887092 832.993379 0.000000 2021-02-13T00:42:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:42:40Z- LcmTrn::publishEstimates() - MLE : 4059851.37 593029.65 5.19 2021-02-13T00:42:40Z- LcmTrn::publishEstimates() - MMSE : 4059851.87 593009.15 5.19 2021-02-13T00:42:40Z- LcmTrn::publishEstimates() - COVAR: 690.89 832.99 1.73 0.00 2021-02-13T00:42:40Z- AHRS_M2 msg: 1613176960.49 epoch sec; seqNo:14937 2021-02-13T00:42:40Z- AHRS_M2 msg: 0.67 phi; -0.31 theta; -0.21 psi 2021-02-13T00:42:40Z- handleDepth()-Depth_Keller msg: 1613176960495 epoch sec; seqNo:15058; depth 5.27 2021-02-13T00:42:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176960.51 epoch sec; seqNo:15058; 4059852.26 north; 593010.37 east 2021-02-13T00:42:40Z- AHRS_M2 msg: 1613176960.89 epoch sec; seqNo:14938 2021-02-13T00:42:40Z- AHRS_M2 msg: 0.66 phi; -0.32 theta; -0.21 psi 2021-02-13T00:42:40Z- handleDepth()-Depth_Keller msg: 1613176960901 epoch sec; seqNo:15059; depth 5.35 2021-02-13T00:42:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176960.91 epoch sec; seqNo:15059; 4059852.64 north; 593010.37 east 2021-02-13T00:42:41Z- AHRS_M2 msg: 1613176961.30 epoch sec; seqNo:14939 2021-02-13T00:42:41Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.20 psi 2021-02-13T00:42:41Z- handleDepth()-Depth_Keller msg: 1613176961302 epoch sec; seqNo:15060; depth 5.42 2021-02-13T00:42:41Z- handleDvl() - RDI_Pathfinder msg: 1613176961.31 epoch sec; seqNo:7219 2021-02-13T00:42:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.44 , 110.83 , 84.05 , 119.75 2021-02-13T00:42:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.15 , 0.31 2021-02-13T00:42:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176961.32 epoch sec; seqNo:15060; 4059852.64 north; 593010.37 east 2021-02-13T00:42:41Z- AHRS_M2 msg: 1613176961.70 epoch sec; seqNo:14940 2021-02-13T00:42:41Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:42:41Z- handleDepth()-Depth_Keller msg: 1613176961709 epoch sec; seqNo:15061; depth 5.53 2021-02-13T00:42:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176961.72 epoch sec; seqNo:15061; 4059853.02 north; 593010.36 east 2021-02-13T00:42:42Z- AHRS_M2 msg: 1613176962.10 epoch sec; seqNo:14941 2021-02-13T00:42:42Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-13T00:42:42Z- handleDepth()-Depth_Keller msg: 1613176962108 epoch sec; seqNo:15062; depth 5.64 2021-02-13T00:42:42Z- handleDvl() - RDI_Pathfinder msg: 1613176962.12 epoch sec; seqNo:7220 2021-02-13T00:42:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.41 , 109.74 , 0.00 , 124.62 2021-02-13T00:42:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.06 , 0.38 2021-02-13T00:42:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176962.13 epoch sec; seqNo:15062; 4059853.02 north; 593010.36 east 2021-02-13T00:42:42Z- AHRS_M2 msg: 1613176962.51 epoch sec; seqNo:14942 2021-02-13T00:42:42Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.19 psi 2021-02-13T00:42:42Z- handleDepth()-Depth_Keller msg: 1613176962521 epoch sec; seqNo:15063; depth 5.74 2021-02-13T00:42:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176962.53 epoch sec; seqNo:15063; 4059853.40 north; 593010.36 east 2021-02-13T00:42:42Z- AHRS_M2 msg: 1613176962.91 epoch sec; seqNo:14943 2021-02-13T00:42:42Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.20 psi 2021-02-13T00:42:42Z- handleDepth()-Depth_Keller msg: 1613176962916 epoch sec; seqNo:15064; depth 5.85 2021-02-13T00:42:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176962.94 epoch sec; seqNo:15064; 4059853.40 north; 593010.36 east 2021-02-13T00:42:43Z- AHRS_M2 msg: 1613176963.32 epoch sec; seqNo:14944 2021-02-13T00:42:43Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.22 psi 2021-02-13T00:42:43Z- handleDepth()-Depth_Keller msg: 1613176963320 epoch sec; seqNo:15065; depth 5.95 2021-02-13T00:42:43Z- handleDvl() - RDI_Pathfinder msg: 1613176963.33 epoch sec; seqNo:7221 2021-02-13T00:42:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.42 , 109.27 , 82.50 , 125.63 2021-02-13T00:42:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.29 , 0.16 2021-02-13T00:42:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176963.35 epoch sec; seqNo:15065; 4059853.79 north; 593011.57 east 2021-02-13T00:42:43Z- AHRS_M2 msg: 1613176963.72 epoch sec; seqNo:14945 2021-02-13T00:42:43Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.24 psi 2021-02-13T00:42:43Z- handleDepth()-Depth_Keller msg: 1613176963725 epoch sec; seqNo:15066; depth 6.06 2021-02-13T00:42:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176963.74 epoch sec; seqNo:15066; 4059853.79 north; 593011.57 east 2021-02-13T00:42:44Z- handleDepth()-Depth_Keller msg: 1613176964149 epoch sec; seqNo:15067; depth 6.17 2021-02-13T00:42:44Z- handleDvl() - RDI_Pathfinder msg: 1613176964.16 epoch sec; seqNo:7222 2021-02-13T00:42:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 112.79 , 83.04 , 123.20 2021-02-13T00:42:44Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.20 , -0.21 2021-02-13T00:42:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176964.17 epoch sec; seqNo:15067; 4059854.17 north; 593011.57 east 2021-02-13T00:42:44Z- AHRS_M2 msg: 1613176964.53 epoch sec; seqNo:14946 2021-02-13T00:42:44Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.25 psi 2021-02-13T00:42:44Z- handleDepth()-Depth_Keller msg: 1613176964533 epoch sec; seqNo:15068; depth 6.29 2021-02-13T00:42:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176964.54 epoch sec; seqNo:15068; 4059854.55 north; 593011.57 east 2021-02-13T00:42:44Z- AHRS_M2 msg: 1613176964.96 epoch sec; seqNo:14947 2021-02-13T00:42:44Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.25 psi 2021-02-13T00:42:44Z- handleDepth()-Depth_Keller msg: 1613176964973 epoch sec; seqNo:15069; depth 6.40 2021-02-13T00:42:44Z- handleDvl() - RDI_Pathfinder msg: 1613176964.99 epoch sec; seqNo:7223 2021-02-13T00:42:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 114.27 , 84.53 , 120.22 2021-02-13T00:42:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.09 , -0.25 2021-02-13T00:42:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176965.00 epoch sec; seqNo:15069; 4059854.93 north; 593011.56 east 2021-02-13T00:42:45Z- LcmTrn::handleMessages() - handling 40 messages took 0.55 ms and 129 calls... 2021-02-13T00:42:45Z- waiting for 1613176963.12, time is 1613176965.00 2021-02-13T00:42:45Z- LcmTrn::updateTrn() equating pose 1613176965.00 and meas times 1613176965.00 2021-02-13T00:42:45Z- LcmTrn::updateTrn() >>>> pose time 1613176965.00, meas time 1613176965.00 <<<< 2021-02-13T00:42:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176965.00 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:42:45Z- TerrainNav::motionUpdate() - currEstimate = (4059855, 593011.56, 6.40) 2021-02-13T00:42:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:42:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:42:45Z- TNavBF::Projecting Measurements 2021-02-13T00:42:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:42:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:42:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:42:45Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:42:45Z- TNavFilter::3 good beams. 2021-02-13T00:42:45Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:42:45Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:42:45Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:42:45Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:42:45Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:42:45Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:42:45Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:42:45Z- TNavBF::mmse selected from filter -1 2021-02-13T00:42:45Z- TerrainMapOctree: (vehN, vehE ) = (4059854.93, 593011.56). 2021-02-13T00:42:45Z- TerrainMapOctree: Min Distance = 1865.46. 2021-02-13T00:42:45Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:42:45Z- There is no specific good beam that all particles have in common. 2021-02-13T00:42:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:42:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.93 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 20.89 2021-02-13T00:42:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:42:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.78 K: 0.119 ExpectedMeasDiff: nan currMeas.covariance: 11.43 2021-02-13T00:42:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:42:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.57 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 23.13 2021-02-13T00:42:45Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:42:45Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:42:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:42:45Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:42:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:42:45Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:42:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:42:45Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:42:45Z- Filter 0 No beams used in subcloud update 2021-02-13T00:42:45Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:42:45Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:42:45Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 690.8 EastVariance: 833.0 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-13T00:42:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176965.00 sec. 2021-02-13T00:42:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:42:45Z- TNavBF::mmse selected from filter -1 2021-02-13T00:42:45Z- variances([0], [2], and psi_berg) = 690.838612 832.993356 0.000000 2021-02-13T00:42:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:42:45Z- LcmTrn::publishEstimates() - MLE : 4059854.43 593033.06 6.40 2021-02-13T00:42:45Z- LcmTrn::publishEstimates() - MMSE : 4059854.93 593011.56 6.40 2021-02-13T00:42:45Z- LcmTrn::publishEstimates() - COVAR: 690.84 832.99 1.47 0.00 2021-02-13T00:42:45Z- AHRS_M2 msg: 1613176965.34 epoch sec; seqNo:14948 2021-02-13T00:42:45Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.24 psi 2021-02-13T00:42:45Z- handleDepth()-Depth_Keller msg: 1613176965342 epoch sec; seqNo:15070; depth 6.51 2021-02-13T00:42:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176965.35 epoch sec; seqNo:15070; 4059855.31 north; 593011.56 east 2021-02-13T00:42:45Z- AHRS_M2 msg: 1613176965.74 epoch sec; seqNo:14949 2021-02-13T00:42:45Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.22 psi 2021-02-13T00:42:45Z- handleDepth()-Depth_Keller msg: 1613176965754 epoch sec; seqNo:15071; depth 6.61 2021-02-13T00:42:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176965.76 epoch sec; seqNo:15071; 4059855.70 north; 593012.77 east 2021-02-13T00:42:46Z- AHRS_M2 msg: 1613176966.15 epoch sec; seqNo:14950 2021-02-13T00:42:46Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.21 psi 2021-02-13T00:42:46Z- handleDepth()-Depth_Keller msg: 1613176966157 epoch sec; seqNo:15072; depth 6.74 2021-02-13T00:42:46Z- handleDvl() - RDI_Pathfinder msg: 1613176966.18 epoch sec; seqNo:7224 2021-02-13T00:42:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 112.45 , 82.70 , 121.37 2021-02-13T00:42:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.17 , 0.01 2021-02-13T00:42:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176966.19 epoch sec; seqNo:15072; 4059856.08 north; 593012.77 east 2021-02-13T00:42:46Z- AHRS_M2 msg: 1613176966.55 epoch sec; seqNo:14951 2021-02-13T00:42:46Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.20 psi 2021-02-13T00:42:46Z- handleDepth()-Depth_Keller msg: 1613176966553 epoch sec; seqNo:15073; depth 6.85 2021-02-13T00:42:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176966.56 epoch sec; seqNo:15073; 4059856.08 north; 593012.77 east 2021-02-13T00:42:46Z- AHRS_M2 msg: 1613176966.95 epoch sec; seqNo:14952 2021-02-13T00:42:46Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.18 psi 2021-02-13T00:42:46Z- handleDepth()-Depth_Keller msg: 1613176966958 epoch sec; seqNo:15074; depth 6.96 2021-02-13T00:42:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176966.97 epoch sec; seqNo:15074; 4059856.46 north; 593012.77 east 2021-02-13T00:42:47Z- AHRS_M2 msg: 1613176967.36 epoch sec; seqNo:14953 2021-02-13T00:42:47Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.16 psi 2021-02-13T00:42:47Z- handleDepth()-Depth_Keller msg: 1613176967365 epoch sec; seqNo:15075; depth 7.07 2021-02-13T00:42:47Z- handleDvl() - RDI_Pathfinder msg: 1613176967.38 epoch sec; seqNo:7225 2021-02-13T00:42:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.72 , 109.54 , 81.28 , 118.47 2021-02-13T00:42:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , 0.13 2021-02-13T00:42:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176967.39 epoch sec; seqNo:15075; 4059856.84 north; 593012.76 east 2021-02-13T00:42:47Z- AHRS_M2 msg: 1613176967.76 epoch sec; seqNo:14954 2021-02-13T00:42:47Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.15 psi 2021-02-13T00:42:47Z- handleDepth()-Depth_Keller msg: 1613176967769 epoch sec; seqNo:15076; depth 7.19 2021-02-13T00:42:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176967.78 epoch sec; seqNo:15076; 4059856.84 north; 593012.76 east 2021-02-13T00:42:48Z- AHRS_M2 msg: 1613176968.16 epoch sec; seqNo:14955 2021-02-13T00:42:48Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.15 psi 2021-02-13T00:42:48Z- handleDepth()-Depth_Keller msg: 1613176968177 epoch sec; seqNo:15077; depth 7.30 2021-02-13T00:42:48Z- handleDvl() - RDI_Pathfinder msg: 1613176968.19 epoch sec; seqNo:7226 2021-02-13T00:42:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.10 , 105.21 , 80.88 , 118.74 2021-02-13T00:42:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.06 , -0.03 2021-02-13T00:42:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176968.20 epoch sec; seqNo:15077; 4059857.21 north; 593012.76 east 2021-02-13T00:42:48Z- AHRS_M2 msg: 1613176968.57 epoch sec; seqNo:14956 2021-02-13T00:42:48Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.15 psi 2021-02-13T00:42:48Z- handleDepth()-Depth_Keller msg: 1613176968572 epoch sec; seqNo:15078; depth 7.42 2021-02-13T00:42:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176968.59 epoch sec; seqNo:15078; 4059857.61 north; 593013.97 east 2021-02-13T00:42:48Z- AHRS_M2 msg: 1613176968.97 epoch sec; seqNo:14957 2021-02-13T00:42:48Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.16 psi 2021-02-13T00:42:48Z- handleDepth()-Depth_Keller msg: 1613176968976 epoch sec; seqNo:15079; depth 7.53 2021-02-13T00:42:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176968.98 epoch sec; seqNo:15079; 4059857.61 north; 593013.97 east 2021-02-13T00:42:49Z- AHRS_M2 msg: 1613176969.38 epoch sec; seqNo:14958 2021-02-13T00:42:49Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.17 psi 2021-02-13T00:42:49Z- handleDepth()-Depth_Keller msg: 1613176969385 epoch sec; seqNo:15080; depth 7.62 2021-02-13T00:42:49Z- handleDvl() - RDI_Pathfinder msg: 1613176969.40 epoch sec; seqNo:7227 2021-02-13T00:42:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 105.08 , 80.74 , 113.19 2021-02-13T00:42:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.11 2021-02-13T00:42:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176969.41 epoch sec; seqNo:15080; 4059857.99 north; 593013.97 east 2021-02-13T00:42:49Z- AHRS_M2 msg: 1613176969.78 epoch sec; seqNo:14959 2021-02-13T00:42:49Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.18 psi 2021-02-13T00:42:49Z- handleDepth()-Depth_Keller msg: 1613176969786 epoch sec; seqNo:15081; depth 7.75 2021-02-13T00:42:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176969.80 epoch sec; seqNo:15081; 4059858.36 north; 593013.97 east 2021-02-13T00:42:50Z- AHRS_M2 msg: 1613176970.18 epoch sec; seqNo:14960 2021-02-13T00:42:50Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.18 psi 2021-02-13T00:42:50Z- handleDepth()-Depth_Keller msg: 1613176970188 epoch sec; seqNo:15082; depth 7.84 2021-02-13T00:42:50Z- handleDvl() - RDI_Pathfinder msg: 1613176970.21 epoch sec; seqNo:7228 2021-02-13T00:42:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 106.57 , 0.00 , 120.09 2021-02-13T00:42:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.13 , 0.06 2021-02-13T00:42:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176970.22 epoch sec; seqNo:15082; 4059858.74 north; 593013.96 east 2021-02-13T00:42:50Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 130 calls... 2021-02-13T00:42:50Z- waiting for 1613176968.00, time is 1613176970.22 2021-02-13T00:42:50Z- LcmTrn::updateTrn() equating pose 1613176970.22 and meas times 1613176970.22 2021-02-13T00:42:50Z- LcmTrn::updateTrn() >>>> pose time 1613176970.22, meas time 1613176970.22 <<<< 2021-02-13T00:42:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176970.22 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:42:50Z- TerrainNav::motionUpdate() - currEstimate = (4059859, 593013.96, 7.84) 2021-02-13T00:42:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:42:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:42:50Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176970.22 sec.; No relevant navigation data available 2021-02-13T00:42:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:42:50Z- TNavBF::mmse selected from filter -1 2021-02-13T00:42:50Z- variances([0], [2], and psi_berg) = 690.786467 832.993331 0.000000 2021-02-13T00:42:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:42:50Z- LcmTrn::publishEstimates() - MLE : 4059858.24 593035.46 7.84 2021-02-13T00:42:50Z- LcmTrn::publishEstimates() - MMSE : 4059858.74 593013.96 7.84 2021-02-13T00:42:50Z- LcmTrn::publishEstimates() - COVAR: 690.79 832.99 1.70 0.00 2021-02-13T00:42:50Z- AHRS_M2 msg: 1613176970.59 epoch sec; seqNo:14961 2021-02-13T00:42:50Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.18 psi 2021-02-13T00:42:50Z- handleDepth()-Depth_Keller msg: 1613176970592 epoch sec; seqNo:15083; depth 7.97 2021-02-13T00:42:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176970.61 epoch sec; seqNo:15083; 4059859.12 north; 593013.96 east 2021-02-13T00:42:51Z- AHRS_M2 msg: 1613176971.01 epoch sec; seqNo:14962 2021-02-13T00:42:51Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.18 psi 2021-02-13T00:42:51Z- handleDepth()-Depth_Keller msg: 1613176971009 epoch sec; seqNo:15084; depth 8.07 2021-02-13T00:42:51Z- handleDvl() - RDI_Pathfinder msg: 1613176971.02 epoch sec; seqNo:7229 2021-02-13T00:42:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.79 , 105.15 , 79.69 , 118.53 2021-02-13T00:42:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , 0.18 2021-02-13T00:42:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176971.03 epoch sec; seqNo:15084; 4059859.12 north; 593013.96 east 2021-02-13T00:42:51Z- AHRS_M2 msg: 1613176971.39 epoch sec; seqNo:14963 2021-02-13T00:42:51Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.16 psi 2021-02-13T00:42:51Z- handleDepth()-Depth_Keller msg: 1613176971403 epoch sec; seqNo:15085; depth 8.20 2021-02-13T00:42:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176971.41 epoch sec; seqNo:15085; 4059859.51 north; 593015.17 east 2021-02-13T00:42:51Z- AHRS_M2 msg: 1613176971.80 epoch sec; seqNo:14964 2021-02-13T00:42:51Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.15 psi 2021-02-13T00:42:51Z- handleDepth()-Depth_Keller msg: 1613176971804 epoch sec; seqNo:15086; depth 8.30 2021-02-13T00:42:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176971.83 epoch sec; seqNo:15086; 4059859.89 north; 593015.17 east 2021-02-13T00:42:52Z- AHRS_M2 msg: 1613176972.20 epoch sec; seqNo:14965 2021-02-13T00:42:52Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.15 psi 2021-02-13T00:42:52Z- handleDepth()-Depth_Keller msg: 1613176972213 epoch sec; seqNo:15087; depth 8.44 2021-02-13T00:42:52Z- handleDvl() - RDI_Pathfinder msg: 1613176972.22 epoch sec; seqNo:7230 2021-02-13T00:42:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 102.58 , 80.95 , 114.75 2021-02-13T00:42:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.15 , 0.07 2021-02-13T00:42:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176972.25 epoch sec; seqNo:15087; 4059859.89 north; 593015.17 east 2021-02-13T00:42:52Z- AHRS_M2 msg: 1613176972.61 epoch sec; seqNo:14966 2021-02-13T00:42:52Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.15 psi 2021-02-13T00:42:52Z- handleDepth()-Depth_Keller msg: 1613176972612 epoch sec; seqNo:15088; depth 8.55 2021-02-13T00:42:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176972.62 epoch sec; seqNo:15088; 4059860.27 north; 593015.17 east 2021-02-13T00:42:53Z- AHRS_M2 msg: 1613176973.03 epoch sec; seqNo:14967 2021-02-13T00:42:53Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.15 psi 2021-02-13T00:42:53Z- handleDepth()-Depth_Keller msg: 1613176973037 epoch sec; seqNo:15089; depth 8.66 2021-02-13T00:42:53Z- handleDvl() - RDI_Pathfinder msg: 1613176973.05 epoch sec; seqNo:7231 2021-02-13T00:42:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.31 , 105.42 , 78.06 , 118.94 2021-02-13T00:42:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.09 , -0.12 2021-02-13T00:42:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176973.06 epoch sec; seqNo:15089; 4059860.65 north; 593015.16 east 2021-02-13T00:42:53Z- AHRS_M2 msg: 1613176973.41 epoch sec; seqNo:14968 2021-02-13T00:42:53Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.15 psi 2021-02-13T00:42:53Z- handleDepth()-Depth_Keller msg: 1613176973425 epoch sec; seqNo:15090; depth 8.81 2021-02-13T00:42:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176973.44 epoch sec; seqNo:15090; 4059861.04 north; 593016.38 east 2021-02-13T00:42:53Z- AHRS_M2 msg: 1613176973.82 epoch sec; seqNo:14969 2021-02-13T00:42:53Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.15 psi 2021-02-13T00:42:53Z- handleDepth()-Depth_Keller msg: 1613176973824 epoch sec; seqNo:15091; depth 8.90 2021-02-13T00:42:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176973.83 epoch sec; seqNo:15091; 4059861.04 north; 593016.38 east 2021-02-13T00:42:54Z- AHRS_M2 msg: 1613176974.23 epoch sec; seqNo:14970 2021-02-13T00:42:54Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.16 psi 2021-02-13T00:42:54Z- handleDepth()-Depth_Keller msg: 1613176974233 epoch sec; seqNo:15092; depth 9.04 2021-02-13T00:42:54Z- handleDvl() - RDI_Pathfinder msg: 1613176974.24 epoch sec; seqNo:7232 2021-02-13T00:42:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 103.79 , 79.46 , 117.32 2021-02-13T00:42:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , -0.17 2021-02-13T00:42:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176974.25 epoch sec; seqNo:15092; 4059861.42 north; 593016.37 east 2021-02-13T00:42:54Z- AHRS_M2 msg: 1613176974.63 epoch sec; seqNo:14971 2021-02-13T00:42:54Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.16 psi 2021-02-13T00:42:54Z- handleDepth()-Depth_Keller msg: 1613176974634 epoch sec; seqNo:15093; depth 9.15 2021-02-13T00:42:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176974.64 epoch sec; seqNo:15093; 4059861.80 north; 593016.37 east 2021-02-13T00:42:55Z- AHRS_M2 msg: 1613176975.03 epoch sec; seqNo:14972 2021-02-13T00:42:55Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.17 psi 2021-02-13T00:42:55Z- handleDepth()-Depth_Keller msg: 1613176975041 epoch sec; seqNo:15094; depth 9.27 2021-02-13T00:42:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176975.06 epoch sec; seqNo:15094; 4059862.18 north; 593016.37 east 2021-02-13T00:42:55Z- LcmTrn::handleMessages() - handling 40 messages took 0.58 ms and 129 calls... 2021-02-13T00:42:55Z- waiting for 1613176973.22, time is 1613176975.06 2021-02-13T00:42:55Z- LcmTrn::updateTrn() >>>> pose time 1613176975.06, meas time 1613176974.24 <<<< 2021-02-13T00:42:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176974.24 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:42:55Z- TerrainNav::motionUpdate() - currEstimate = (4059862, 593016.37, 9.27) 2021-02-13T00:42:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:42:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:42:55Z- TNavBF::Projecting Measurements 2021-02-13T00:42:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:42:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:42:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:42:55Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:42:55Z- TNavFilter::3 good beams. 2021-02-13T00:42:55Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:42:55Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:42:55Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:42:55Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:42:55Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:42:55Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:42:55Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:42:55Z- TNavBF::mmse selected from filter -1 2021-02-13T00:42:55Z- TerrainMapOctree: (vehN, vehE ) = (4059861.60, 593015.96). 2021-02-13T00:42:55Z- TerrainMapOctree: Min Distance = 1857.55. 2021-02-13T00:42:55Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:42:55Z- There is no specific good beam that all particles have in common. 2021-02-13T00:42:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:42:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.88 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 17.24 2021-02-13T00:42:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:42:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.71 K: 0.126 ExpectedMeasDiff: nan currMeas.covariance: 10.10 2021-02-13T00:42:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:42:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.49 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 22.02 2021-02-13T00:42:55Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:42:55Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:42:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:42:55Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:42:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:42:55Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:42:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:42:55Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:42:55Z- Filter 0 No beams used in subcloud update 2021-02-13T00:42:55Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:42:55Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:42:55Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 690.7 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T00:42:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176974.24 sec. 2021-02-13T00:42:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:42:55Z- TNavBF::mmse selected from filter -1 2021-02-13T00:42:55Z- variances([0], [2], and psi_berg) = 690.719994 832.993300 0.000000 2021-02-13T00:42:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:42:55Z- LcmTrn::publishEstimates() - MLE : 4059861.68 593037.87 9.27 2021-02-13T00:42:55Z- LcmTrn::publishEstimates() - MMSE : 4059862.18 593016.37 9.27 2021-02-13T00:42:55Z- LcmTrn::publishEstimates() - COVAR: 690.72 832.99 1.44 0.00 2021-02-13T00:42:55Z- AHRS_M2 msg: 1613176975.44 epoch sec; seqNo:14973 2021-02-13T00:42:55Z- AHRS_M2 msg: 0.65 phi; -0.31 theta; -0.18 psi 2021-02-13T00:42:55Z- handleDepth()-Depth_Keller msg: 1613176975440 epoch sec; seqNo:15095; depth 9.38 2021-02-13T00:42:55Z- handleDvl() - RDI_Pathfinder msg: 1613176975.45 epoch sec; seqNo:7233 2021-02-13T00:42:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 103.80 , 79.46 , 113.26 2021-02-13T00:42:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.12 , -0.11 2021-02-13T00:42:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176975.46 epoch sec; seqNo:15095; 4059862.57 north; 593017.58 east 2021-02-13T00:42:55Z- AHRS_M2 msg: 1613176975.84 epoch sec; seqNo:14974 2021-02-13T00:42:55Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.18 psi 2021-02-13T00:42:55Z- handleDepth()-Depth_Keller msg: 1613176975845 epoch sec; seqNo:15096; depth 9.50 2021-02-13T00:42:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176975.86 epoch sec; seqNo:15096; 4059862.57 north; 593017.58 east 2021-02-13T00:42:56Z- AHRS_M2 msg: 1613176976.25 epoch sec; seqNo:14975 2021-02-13T00:42:56Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.18 psi 2021-02-13T00:42:56Z- handleDepth()-Depth_Keller msg: 1613176976262 epoch sec; seqNo:15097; depth 9.60 2021-02-13T00:42:56Z- handleDvl() - RDI_Pathfinder msg: 1613176976.28 epoch sec; seqNo:7234 2021-02-13T00:42:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.20 , 104.13 , 79.79 , 114.95 2021-02-13T00:42:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.10 , 0.04 2021-02-13T00:42:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176976.29 epoch sec; seqNo:15097; 4059862.95 north; 593017.58 east 2021-02-13T00:42:56Z- AHRS_M2 msg: 1613176976.65 epoch sec; seqNo:14976 2021-02-13T00:42:56Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.18 psi 2021-02-13T00:42:56Z- handleDepth()-Depth_Keller msg: 1613176976657 epoch sec; seqNo:15098; depth 9.73 2021-02-13T00:42:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:42:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:42:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:42:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:42:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:42:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:42:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:42:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:42:56Z- handleDvl() - RDI_Pathfinder msg: 1613176976.66 epoch sec; seqNo:7235 2021-02-13T00:42:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:42:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.10 , 0.04 2021-02-13T00:42:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176976.68 epoch sec; seqNo:15098; 4059863.33 north; 593017.57 east 2021-02-13T00:42:57Z- AHRS_M2 msg: 1613176977.06 epoch sec; seqNo:14977 2021-02-13T00:42:57Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.18 psi 2021-02-13T00:42:57Z- handleDepth()-Depth_Keller msg: 1613176977062 epoch sec; seqNo:15099; depth 9.82 2021-02-13T00:42:57Z- handleDvl() - RDI_Pathfinder msg: 1613176977.07 epoch sec; seqNo:7236 2021-02-13T00:42:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 104.13 , 77.24 , 112.25 2021-02-13T00:42:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.04 , 0.10 2021-02-13T00:42:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176977.10 epoch sec; seqNo:15099; 4059863.71 north; 593017.57 east 2021-02-13T00:42:57Z- AHRS_M2 msg: 1613176977.46 epoch sec; seqNo:14978 2021-02-13T00:42:57Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.19 psi 2021-02-13T00:42:57Z- handleDepth()-Depth_Keller msg: 1613176977487 epoch sec; seqNo:15100; depth 9.94 2021-02-13T00:42:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176977.51 epoch sec; seqNo:15100; 4059863.71 north; 593017.57 east 2021-02-13T00:42:57Z- AHRS_M2 msg: 1613176977.86 epoch sec; seqNo:14979 2021-02-13T00:42:57Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.20 psi 2021-02-13T00:42:57Z- handleDepth()-Depth_Keller msg: 1613176977888 epoch sec; seqNo:15101; depth 10.06 2021-02-13T00:42:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:42:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:42:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:42:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:42:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:42:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:42:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:42:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:42:57Z- handleDvl() - RDI_Pathfinder msg: 1613176977.89 epoch sec; seqNo:7237 2021-02-13T00:42:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:42:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.04 , 0.10 2021-02-13T00:42:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176977.90 epoch sec; seqNo:15101; 4059864.09 north; 593017.57 east 2021-02-13T00:42:58Z- AHRS_M2 msg: 1613176978.26 epoch sec; seqNo:14980 2021-02-13T00:42:58Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.21 psi 2021-02-13T00:42:58Z- handleDepth()-Depth_Keller msg: 1613176978273 epoch sec; seqNo:15102; depth 10.17 2021-02-13T00:42:58Z- handleDvl() - RDI_Pathfinder msg: 1613176978.29 epoch sec; seqNo:7238 2021-02-13T00:42:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.62 , 104.20 , 78.51 , 116.37 2021-02-13T00:42:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.09 , 0.04 2021-02-13T00:42:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176978.30 epoch sec; seqNo:15102; 4059864.48 north; 593018.78 east 2021-02-13T00:42:58Z- AHRS_M2 msg: 1613176978.67 epoch sec; seqNo:14981 2021-02-13T00:42:58Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.22 psi 2021-02-13T00:42:58Z- handleDepth()-Depth_Keller msg: 1613176978672 epoch sec; seqNo:15103; depth 10.27 2021-02-13T00:42:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176978.68 epoch sec; seqNo:15103; 4059864.48 north; 593018.78 east 2021-02-13T00:42:59Z- AHRS_M2 msg: 1613176979.07 epoch sec; seqNo:14982 2021-02-13T00:42:59Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.22 psi 2021-02-13T00:42:59Z- handleDepth()-Depth_Keller msg: 1613176979076 epoch sec; seqNo:15104; depth 10.39 2021-02-13T00:42:59Z- handleDvl() - RDI_Pathfinder msg: 1613176979.09 epoch sec; seqNo:7239 2021-02-13T00:42:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 106.77 , 79.73 , 114.88 2021-02-13T00:42:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , -0.04 2021-02-13T00:42:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176979.10 epoch sec; seqNo:15104; 4059864.86 north; 593018.78 east 2021-02-13T00:42:59Z- AHRS_M2 msg: 1613176979.47 epoch sec; seqNo:14983 2021-02-13T00:42:59Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.23 psi 2021-02-13T00:42:59Z- handleDepth()-Depth_Keller msg: 1613176979481 epoch sec; seqNo:15105; depth 10.52 2021-02-13T00:42:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176979.50 epoch sec; seqNo:15105; 4059865.24 north; 593018.77 east 2021-02-13T00:42:59Z- AHRS_M2 msg: 1613176979.88 epoch sec; seqNo:14984 2021-02-13T00:42:59Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.23 psi 2021-02-13T00:42:59Z- handleDepth()-Depth_Keller msg: 1613176979884 epoch sec; seqNo:15106; depth 10.63 2021-02-13T00:42:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176979.89 epoch sec; seqNo:15106; 4059865.24 north; 593018.77 east 2021-02-13T00:43:00Z- AHRS_M2 msg: 1613176980.29 epoch sec; seqNo:14985 2021-02-13T00:43:00Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.22 psi 2021-02-13T00:43:00Z- handleDepth()-Depth_Keller msg: 1613176980289 epoch sec; seqNo:15107; depth 10.76 2021-02-13T00:43:00Z- handleDvl() - RDI_Pathfinder msg: 1613176980.30 epoch sec; seqNo:7240 2021-02-13T00:43:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.44 , 106.43 , 79.39 , 115.90 2021-02-13T00:43:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.03 , 0.03 2021-02-13T00:43:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176980.31 epoch sec; seqNo:15107; 4059865.62 north; 593018.77 east 2021-02-13T00:43:00Z- LcmTrn::handleMessages() - handling 47 messages took 0.67 ms and 134 calls... 2021-02-13T00:43:00Z- waiting for 1613176978.06, time is 1613176980.31 2021-02-13T00:43:00Z- LcmTrn::updateTrn() equating pose 1613176980.31 and meas times 1613176980.31 2021-02-13T00:43:00Z- LcmTrn::updateTrn() >>>> pose time 1613176980.31, meas time 1613176980.31 <<<< 2021-02-13T00:43:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176980.31 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:43:00Z- TerrainNav::motionUpdate() - currEstimate = (4059866, 593018.77, 10.76) 2021-02-13T00:43:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:43:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:43:00Z- TerrainNav::Measurement type 1 not incorporated from time = 1613176980.31 sec.; No relevant navigation data available 2021-02-13T00:43:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:43:00Z- TNavBF::mmse selected from filter -1 2021-02-13T00:43:00Z- variances([0], [2], and psi_berg) = 690.669758 832.993276 0.000000 2021-02-13T00:43:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:43:00Z- LcmTrn::publishEstimates() - MLE : 4059865.12 593040.27 10.76 2021-02-13T00:43:00Z- LcmTrn::publishEstimates() - MMSE : 4059865.62 593018.77 10.76 2021-02-13T00:43:00Z- LcmTrn::publishEstimates() - COVAR: 690.67 832.99 1.66 0.00 2021-02-13T00:43:00Z- AHRS_M2 msg: 1613176980.69 epoch sec; seqNo:14986 2021-02-13T00:43:00Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.21 psi 2021-02-13T00:43:00Z- handleDepth()-Depth_Keller msg: 1613176980698 epoch sec; seqNo:15108; depth 10.88 2021-02-13T00:43:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176980.71 epoch sec; seqNo:15108; 4059866.01 north; 593019.99 east 2021-02-13T00:43:01Z- AHRS_M2 msg: 1613176981.09 epoch sec; seqNo:14987 2021-02-13T00:43:01Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.21 psi 2021-02-13T00:43:01Z- handleDepth()-Depth_Keller msg: 1613176981097 epoch sec; seqNo:15109; depth 10.99 2021-02-13T00:43:01Z- handleDvl() - RDI_Pathfinder msg: 1613176981.11 epoch sec; seqNo:7241 2021-02-13T00:43:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.86 , 106.84 , 0.00 , 112.25 2021-02-13T00:43:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , 0.11 2021-02-13T00:43:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176981.12 epoch sec; seqNo:15109; 4059866.39 north; 593019.98 east 2021-02-13T00:43:01Z- AHRS_M2 msg: 1613176981.50 epoch sec; seqNo:14988 2021-02-13T00:43:01Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.20 psi 2021-02-13T00:43:01Z- handleDepth()-Depth_Keller msg: 1613176981509 epoch sec; seqNo:15110; depth 11.11 2021-02-13T00:43:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176981.52 epoch sec; seqNo:15110; 4059866.39 north; 593019.98 east 2021-02-13T00:43:01Z- AHRS_M2 msg: 1613176981.91 epoch sec; seqNo:14989 2021-02-13T00:43:01Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.19 psi 2021-02-13T00:43:01Z- handleDepth()-Depth_Keller msg: 1613176981912 epoch sec; seqNo:15111; depth 11.22 2021-02-13T00:43:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176981.92 epoch sec; seqNo:15111; 4059866.77 north; 593019.98 east 2021-02-13T00:43:02Z- AHRS_M2 msg: 1613176982.30 epoch sec; seqNo:14990 2021-02-13T00:43:02Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.19 psi 2021-02-13T00:43:02Z- handleDepth()-Depth_Keller msg: 1613176982308 epoch sec; seqNo:15112; depth 11.33 2021-02-13T00:43:02Z- handleDvl() - RDI_Pathfinder msg: 1613176982.33 epoch sec; seqNo:7242 2021-02-13T00:43:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.45 , 103.73 , 78.04 , 114.54 2021-02-13T00:43:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.14 , 0.04 2021-02-13T00:43:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176982.34 epoch sec; seqNo:15112; 4059867.14 north; 593019.97 east 2021-02-13T00:43:02Z- AHRS_M2 msg: 1613176982.71 epoch sec; seqNo:14991 2021-02-13T00:43:02Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.19 psi 2021-02-13T00:43:02Z- handleDepth()-Depth_Keller msg: 1613176982713 epoch sec; seqNo:15113; depth 11.45 2021-02-13T00:43:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176982.73 epoch sec; seqNo:15113; 4059867.16 north; 593021.19 east 2021-02-13T00:43:03Z- AHRS_M2 msg: 1613176983.11 epoch sec; seqNo:14992 2021-02-13T00:43:03Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.19 psi 2021-02-13T00:43:03Z- handleDepth()-Depth_Keller msg: 1613176983117 epoch sec; seqNo:15114; depth 11.58 2021-02-13T00:43:03Z- handleDvl() - RDI_Pathfinder msg: 1613176983.13 epoch sec; seqNo:7243 2021-02-13T00:43:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.50 , 78.51 , 115.02 2021-02-13T00:43:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.13 , -0.09 2021-02-13T00:43:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176983.14 epoch sec; seqNo:15114; 4059867.54 north; 593021.19 east 2021-02-13T00:43:03Z- AHRS_M2 msg: 1613176983.52 epoch sec; seqNo:14993 2021-02-13T00:43:03Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.20 psi 2021-02-13T00:43:03Z- handleDepth()-Depth_Keller msg: 1613176983529 epoch sec; seqNo:15115; depth 11.70 2021-02-13T00:43:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176983.54 epoch sec; seqNo:15115; 4059867.92 north; 593021.19 east 2021-02-13T00:43:03Z- AHRS_M2 msg: 1613176983.92 epoch sec; seqNo:14994 2021-02-13T00:43:03Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.21 psi 2021-02-13T00:43:03Z- handleDepth()-Depth_Keller msg: 1613176983929 epoch sec; seqNo:15116; depth 11.82 2021-02-13T00:43:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176983.94 epoch sec; seqNo:15116; 4059868.29 north; 593021.18 east 2021-02-13T00:43:04Z- AHRS_M2 msg: 1613176984.32 epoch sec; seqNo:14995 2021-02-13T00:43:04Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.21 psi 2021-02-13T00:43:04Z- handleDepth()-Depth_Keller msg: 1613176984333 epoch sec; seqNo:15117; depth 11.94 2021-02-13T00:43:04Z- handleDvl() - RDI_Pathfinder msg: 1613176984.35 epoch sec; seqNo:7244 2021-02-13T00:43:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.65 , 102.58 , 76.83 , 113.39 2021-02-13T00:43:04Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.01 , -0.16 2021-02-13T00:43:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176984.36 epoch sec; seqNo:15117; 4059868.67 north; 593021.18 east 2021-02-13T00:43:04Z- AHRS_M2 msg: 1613176984.73 epoch sec; seqNo:14996 2021-02-13T00:43:04Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.21 psi 2021-02-13T00:43:04Z- handleDepth()-Depth_Keller msg: 1613176984732 epoch sec; seqNo:15118; depth 12.08 2021-02-13T00:43:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176984.75 epoch sec; seqNo:15118; 4059869.06 north; 593022.39 east 2021-02-13T00:43:05Z- AHRS_M2 msg: 1613176985.13 epoch sec; seqNo:14997 2021-02-13T00:43:05Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.21 psi 2021-02-13T00:43:05Z- handleDepth()-Depth_Keller msg: 1613176985141 epoch sec; seqNo:15119; depth 12.20 2021-02-13T00:43:05Z- handleDvl() - RDI_Pathfinder msg: 1613176985.16 epoch sec; seqNo:7245 2021-02-13T00:43:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.64 , 105.62 , 75.19 , 112.38 2021-02-13T00:43:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.11 , -0.07 2021-02-13T00:43:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176985.17 epoch sec; seqNo:15119; 4059869.06 north; 593022.39 east 2021-02-13T00:43:05Z- LcmTrn::handleMessages() - handling 41 messages took 0.65 ms and 128 calls... 2021-02-13T00:43:05Z- waiting for 1613176983.31, time is 1613176985.17 2021-02-13T00:43:05Z- LcmTrn::updateTrn() equating pose 1613176985.17 and meas times 1613176985.17 2021-02-13T00:43:05Z- LcmTrn::updateTrn() >>>> pose time 1613176985.17, meas time 1613176985.17 <<<< 2021-02-13T00:43:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176985.17 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:43:05Z- TerrainNav::motionUpdate() - currEstimate = (4059869, 593022.39, 12.20) 2021-02-13T00:43:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:43:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:43:05Z- TNavBF::Projecting Measurements 2021-02-13T00:43:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:43:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:43:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:43:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:43:05Z- TNavFilter::4 good beams. 2021-02-13T00:43:05Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:43:05Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:43:05Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:43:05Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:43:05Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:43:05Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:43:05Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:43:05Z- TNavBF::mmse selected from filter -1 2021-02-13T00:43:05Z- TerrainMapOctree: (vehN, vehE ) = (4059869.06, 593022.39). 2021-02-13T00:43:05Z- TerrainMapOctree: Min Distance = 1847.70. 2021-02-13T00:43:05Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:43:05Z- There is no specific good beam that all particles have in common. 2021-02-13T00:43:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:43:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.91 K: 0.131 ExpectedMeasDiff: nan currMeas.covariance: 10.93 2021-02-13T00:43:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:43:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.66 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 17.85 2021-02-13T00:43:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:43:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.53 K: 0.124 ExpectedMeasDiff: nan currMeas.covariance: 9.05 2021-02-13T00:43:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:43:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 20.21 2021-02-13T00:43:05Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:43:05Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:43:05Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:43:05Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:43:05Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:43:05Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:43:05Z- Filter 0 No beams used in subcloud update 2021-02-13T00:43:05Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:43:05Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:43:05Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 690.6 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:43:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176985.17 sec. 2021-02-13T00:43:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:43:05Z- TNavBF::mmse selected from filter -1 2021-02-13T00:43:05Z- variances([0], [2], and psi_berg) = 690.614912 832.993250 0.000000 2021-02-13T00:43:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:43:05Z- LcmTrn::publishEstimates() - MLE : 4059868.56 593043.89 12.20 2021-02-13T00:43:05Z- LcmTrn::publishEstimates() - MMSE : 4059869.06 593022.39 12.20 2021-02-13T00:43:05Z- LcmTrn::publishEstimates() - COVAR: 690.61 832.99 1.26 0.00 2021-02-13T00:43:05Z- AHRS_M2 msg: 1613176985.54 epoch sec; seqNo:14998 2021-02-13T00:43:05Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.19 psi 2021-02-13T00:43:05Z- handleDepth()-Depth_Keller msg: 1613176985545 epoch sec; seqNo:15120; depth 12.35 2021-02-13T00:43:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176985.56 epoch sec; seqNo:15120; 4059869.44 north; 593022.39 east 2021-02-13T00:43:05Z- AHRS_M2 msg: 1613176985.94 epoch sec; seqNo:14999 2021-02-13T00:43:05Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.18 psi 2021-02-13T00:43:05Z- handleDepth()-Depth_Keller msg: 1613176985945 epoch sec; seqNo:15121; depth 12.46 2021-02-13T00:43:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176985.95 epoch sec; seqNo:15121; 4059869.82 north; 593022.39 east 2021-02-13T00:43:06Z- AHRS_M2 msg: 1613176986.34 epoch sec; seqNo:15000 2021-02-13T00:43:06Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.16 psi 2021-02-13T00:43:06Z- handleDepth()-Depth_Keller msg: 1613176986354 epoch sec; seqNo:15122; depth 12.59 2021-02-13T00:43:06Z- handleDvl() - RDI_Pathfinder msg: 1613176986.37 epoch sec; seqNo:7246 2021-02-13T00:43:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.71 , 101.63 , 78.65 , 113.80 2021-02-13T00:43:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , 0.04 2021-02-13T00:43:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176986.38 epoch sec; seqNo:15122; 4059870.20 north; 593022.38 east 2021-02-13T00:43:06Z- AHRS_M2 msg: 1613176986.75 epoch sec; seqNo:15001 2021-02-13T00:43:06Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.15 psi 2021-02-13T00:43:06Z- handleDepth()-Depth_Keller msg: 1613176986752 epoch sec; seqNo:15123; depth 12.73 2021-02-13T00:43:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176986.76 epoch sec; seqNo:15123; 4059870.58 north; 593022.38 east 2021-02-13T00:43:07Z- AHRS_M2 msg: 1613176987.15 epoch sec; seqNo:15002 2021-02-13T00:43:07Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.14 psi 2021-02-13T00:43:07Z- handleDepth()-Depth_Keller msg: 1613176987158 epoch sec; seqNo:15124; depth 12.85 2021-02-13T00:43:07Z- handleDvl() - RDI_Pathfinder msg: 1613176987.17 epoch sec; seqNo:7247 2021-02-13T00:43:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 98.86 , 78.58 , 115.09 2021-02-13T00:43:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , 0.04 2021-02-13T00:43:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176987.18 epoch sec; seqNo:15124; 4059870.58 north; 593022.38 east 2021-02-13T00:43:07Z- AHRS_M2 msg: 1613176987.56 epoch sec; seqNo:15003 2021-02-13T00:43:07Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.15 psi 2021-02-13T00:43:07Z- handleDepth()-Depth_Keller msg: 1613176987573 epoch sec; seqNo:15125; depth 12.99 2021-02-13T00:43:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176987.58 epoch sec; seqNo:15125; 4059870.97 north; 593023.59 east 2021-02-13T00:43:07Z- AHRS_M2 msg: 1613176987.97 epoch sec; seqNo:15004 2021-02-13T00:43:07Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.16 psi 2021-02-13T00:43:07Z- handleDepth()-Depth_Keller msg: 1613176987977 epoch sec; seqNo:15126; depth 13.13 2021-02-13T00:43:07Z- handleDvl() - RDI_Pathfinder msg: 1613176987.99 epoch sec; seqNo:7248 2021-02-13T00:43:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 100.21 , 74.78 , 113.73 2021-02-13T00:43:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.07 , -0.02 2021-02-13T00:43:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176988.01 epoch sec; seqNo:15126; 4059871.35 north; 593023.59 east 2021-02-13T00:43:08Z- AHRS_M2 msg: 1613176988.37 epoch sec; seqNo:15005 2021-02-13T00:43:08Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-13T00:43:08Z- handleDepth()-Depth_Keller msg: 1613176988369 epoch sec; seqNo:15127; depth 13.25 2021-02-13T00:43:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176988.38 epoch sec; seqNo:15127; 4059871.35 north; 593023.59 east 2021-02-13T00:43:08Z- AHRS_M2 msg: 1613176988.77 epoch sec; seqNo:15006 2021-02-13T00:43:08Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.19 psi 2021-02-13T00:43:08Z- handleDepth()-Depth_Keller msg: 1613176988772 epoch sec; seqNo:15128; depth 13.38 2021-02-13T00:43:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176988.79 epoch sec; seqNo:15128; 4059871.73 north; 593023.59 east 2021-02-13T00:43:09Z- AHRS_M2 msg: 1613176989.17 epoch sec; seqNo:15007 2021-02-13T00:43:09Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.20 psi 2021-02-13T00:43:09Z- handleDepth()-Depth_Keller msg: 1613176989181 epoch sec; seqNo:15129; depth 13.51 2021-02-13T00:43:09Z- handleDvl() - RDI_Pathfinder msg: 1613176989.20 epoch sec; seqNo:7249 2021-02-13T00:43:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 101.70 , 78.72 , 113.87 2021-02-13T00:43:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.01 , -0.06 2021-02-13T00:43:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176989.21 epoch sec; seqNo:15129; 4059872.11 north; 593023.58 east 2021-02-13T00:43:09Z- AHRS_M2 msg: 1613176989.58 epoch sec; seqNo:15008 2021-02-13T00:43:09Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.20 psi 2021-02-13T00:43:09Z- handleDepth()-Depth_Keller msg: 1613176989583 epoch sec; seqNo:15130; depth 13.64 2021-02-13T00:43:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176989.60 epoch sec; seqNo:15130; 4059872.49 north; 593023.58 east 2021-02-13T00:43:09Z- AHRS_M2 msg: 1613176989.99 epoch sec; seqNo:15009 2021-02-13T00:43:09Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.20 psi 2021-02-13T00:43:09Z- handleDepth()-Depth_Keller msg: 1613176989992 epoch sec; seqNo:15131; depth 13.77 2021-02-13T00:43:10Z- handleDvl() - RDI_Pathfinder msg: 1613176990.00 epoch sec; seqNo:7250 2021-02-13T00:43:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.29 , 101.57 , 72.74 , 112.38 2021-02-13T00:43:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.05 , -0.02 2021-02-13T00:43:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176990.01 epoch sec; seqNo:15131; 4059872.88 north; 593024.79 east 2021-02-13T00:43:10Z- AHRS_M2 msg: 1613176990.38 epoch sec; seqNo:15010 2021-02-13T00:43:10Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.19 psi 2021-02-13T00:43:10Z- handleDepth()-Depth_Keller msg: 1613176990398 epoch sec; seqNo:15132; depth 13.90 2021-02-13T00:43:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176990.41 epoch sec; seqNo:15132; 4059873.26 north; 593024.79 east 2021-02-13T00:43:10Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-13T00:43:10Z- waiting for 1613176988.17, time is 1613176990.41 2021-02-13T00:43:10Z- LcmTrn::updateTrn() >>>> pose time 1613176990.41, meas time 1613176990.00 <<<< 2021-02-13T00:43:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176990.00 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:43:10Z- TerrainNav::motionUpdate() - currEstimate = (4059873, 593024.79, 13.90) 2021-02-13T00:43:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:43:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:43:10Z- TNavBF::Projecting Measurements 2021-02-13T00:43:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:43:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:43:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:43:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:43:10Z- TNavFilter::4 good beams. 2021-02-13T00:43:10Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:43:10Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:43:10Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:43:10Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:43:10Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:43:10Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:43:10Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:43:10Z- TNavBF::mmse selected from filter -1 2021-02-13T00:43:10Z- TerrainMapOctree: (vehN, vehE ) = (4059872.93, 593024.61). 2021-02-13T00:43:10Z- TerrainMapOctree: Min Distance = 1843.34. 2021-02-13T00:43:10Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:43:10Z- There is no specific good beam that all particles have in common. 2021-02-13T00:43:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:43:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.49 K: 0.108 ExpectedMeasDiff: nan currMeas.covariance: 10.57 2021-02-13T00:43:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:43:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 16.51 2021-02-13T00:43:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:43:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.108 ExpectedMeasDiff: nan currMeas.covariance: 8.47 2021-02-13T00:43:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:43:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 20.21 2021-02-13T00:43:10Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:43:10Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:43:10Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:43:10Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:43:10Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:43:10Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:43:10Z- Filter 0 No beams used in subcloud update 2021-02-13T00:43:10Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:43:10Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:43:10Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 690.6 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:43:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176990.00 sec. 2021-02-13T00:43:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:43:10Z- TNavBF::mmse selected from filter -1 2021-02-13T00:43:10Z- variances([0], [2], and psi_berg) = 690.548213 832.993218 0.000000 2021-02-13T00:43:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:43:10Z- LcmTrn::publishEstimates() - MLE : 4059872.76 593046.29 13.90 2021-02-13T00:43:10Z- LcmTrn::publishEstimates() - MMSE : 4059873.26 593024.79 13.90 2021-02-13T00:43:10Z- LcmTrn::publishEstimates() - COVAR: 690.55 832.99 1.07 0.00 2021-02-13T00:43:10Z- AHRS_M2 msg: 1613176990.79 epoch sec; seqNo:15011 2021-02-13T00:43:10Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.19 psi 2021-02-13T00:43:10Z- handleDepth()-Depth_Keller msg: 1613176990797 epoch sec; seqNo:15133; depth 14.02 2021-02-13T00:43:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176990.81 epoch sec; seqNo:15133; 4059873.26 north; 593024.79 east 2021-02-13T00:43:11Z- AHRS_M2 msg: 1613176991.19 epoch sec; seqNo:15012 2021-02-13T00:43:11Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.18 psi 2021-02-13T00:43:11Z- handleDepth()-Depth_Keller msg: 1613176991195 epoch sec; seqNo:15134; depth 14.15 2021-02-13T00:43:11Z- handleDvl() - RDI_Pathfinder msg: 1613176991.21 epoch sec; seqNo:7251 2021-02-13T00:43:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 170.72 , 100.42 , 72.74 , 109.88 2021-02-13T00:43:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , 0.03 2021-02-13T00:43:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176991.22 epoch sec; seqNo:15134; 4059873.64 north; 593024.79 east 2021-02-13T00:43:11Z- AHRS_M2 msg: 1613176991.60 epoch sec; seqNo:15013 2021-02-13T00:43:11Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.17 psi 2021-02-13T00:43:11Z- handleDepth()-Depth_Keller msg: 1613176991613 epoch sec; seqNo:15135; depth 14.29 2021-02-13T00:43:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176991.62 epoch sec; seqNo:15135; 4059874.02 north; 593024.78 east 2021-02-13T00:43:12Z- AHRS_M2 msg: 1613176992.00 epoch sec; seqNo:15014 2021-02-13T00:43:12Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.15 psi 2021-02-13T00:43:12Z- handleDepth()-Depth_Keller msg: 1613176992004 epoch sec; seqNo:15136; depth 14.41 2021-02-13T00:43:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176992.01 epoch sec; seqNo:15136; 4059874.40 north; 593024.78 east 2021-02-13T00:43:12Z- AHRS_M2 msg: 1613176992.40 epoch sec; seqNo:15015 2021-02-13T00:43:12Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.14 psi 2021-02-13T00:43:12Z- handleDepth()-Depth_Keller msg: 1613176992407 epoch sec; seqNo:15137; depth 14.53 2021-02-13T00:43:12Z- handleDvl() - RDI_Pathfinder msg: 1613176992.42 epoch sec; seqNo:7252 2021-02-13T00:43:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.37 , 97.24 , 72.60 , 110.62 2021-02-13T00:43:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , 0.02 2021-02-13T00:43:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176992.43 epoch sec; seqNo:15137; 4059874.40 north; 593024.78 east 2021-02-13T00:43:12Z- AHRS_M2 msg: 1613176992.81 epoch sec; seqNo:15016 2021-02-13T00:43:12Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.15 psi 2021-02-13T00:43:12Z- handleDepth()-Depth_Keller msg: 1613176992815 epoch sec; seqNo:15138; depth 14.66 2021-02-13T00:43:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176992.83 epoch sec; seqNo:15138; 4059874.79 north; 593025.99 east 2021-02-13T00:43:13Z- AHRS_M2 msg: 1613176993.21 epoch sec; seqNo:15017 2021-02-13T00:43:13Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.15 psi 2021-02-13T00:43:13Z- handleDepth()-Depth_Keller msg: 1613176993216 epoch sec; seqNo:15139; depth 14.78 2021-02-13T00:43:13Z- handleDvl() - RDI_Pathfinder msg: 1613176993.24 epoch sec; seqNo:7253 2021-02-13T00:43:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 95.28 , 0.00 , 108.66 2021-02-13T00:43:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , -0.01 2021-02-13T00:43:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176993.25 epoch sec; seqNo:15139; 4059875.17 north; 593025.99 east 2021-02-13T00:43:13Z- AHRS_M2 msg: 1613176993.62 epoch sec; seqNo:15018 2021-02-13T00:43:13Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.16 psi 2021-02-13T00:43:13Z- handleDepth()-Depth_Keller msg: 1613176993625 epoch sec; seqNo:15140; depth 14.88 2021-02-13T00:43:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176993.64 epoch sec; seqNo:15140; 4059875.55 north; 593025.99 east 2021-02-13T00:43:14Z- AHRS_M2 msg: 1613176994.03 epoch sec; seqNo:15019 2021-02-13T00:43:14Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.18 psi 2021-02-13T00:43:14Z- handleDepth()-Depth_Keller msg: 1613176994031 epoch sec; seqNo:15141; depth 15.01 2021-02-13T00:43:14Z- handleDvl() - RDI_Pathfinder msg: 1613176994.04 epoch sec; seqNo:7254 2021-02-13T00:43:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.44 , 73.15 , 106.90 2021-02-13T00:43:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.03 , -0.01 2021-02-13T00:43:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176994.05 epoch sec; seqNo:15141; 4059875.55 north; 593025.99 east 2021-02-13T00:43:14Z- AHRS_M2 msg: 1613176994.43 epoch sec; seqNo:15020 2021-02-13T00:43:14Z- AHRS_M2 msg: 0.57 phi; -0.30 theta; -0.19 psi 2021-02-13T00:43:14Z- handleDepth()-Depth_Keller msg: 1613176994432 epoch sec; seqNo:15142; depth 15.14 2021-02-13T00:43:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176994.45 epoch sec; seqNo:15142; 4059875.92 north; 593025.98 east 2021-02-13T00:43:14Z- AHRS_M2 msg: 1613176994.83 epoch sec; seqNo:15021 2021-02-13T00:43:14Z- AHRS_M2 msg: 0.57 phi; -0.30 theta; -0.20 psi 2021-02-13T00:43:14Z- handleDepth()-Depth_Keller msg: 1613176994837 epoch sec; seqNo:15143; depth 15.25 2021-02-13T00:43:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176994.85 epoch sec; seqNo:15143; 4059876.32 north; 593027.20 east 2021-02-13T00:43:15Z- AHRS_M2 msg: 1613176995.23 epoch sec; seqNo:15022 2021-02-13T00:43:15Z- AHRS_M2 msg: 0.56 phi; -0.30 theta; -0.20 psi 2021-02-13T00:43:15Z- handleDepth()-Depth_Keller msg: 1613176995241 epoch sec; seqNo:15144; depth 15.36 2021-02-13T00:43:15Z- handleDvl() - RDI_Pathfinder msg: 1613176995.25 epoch sec; seqNo:7255 2021-02-13T00:43:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.07 , 97.65 , 74.37 , 107.11 2021-02-13T00:43:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , 0.01 2021-02-13T00:43:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176995.26 epoch sec; seqNo:15144; 4059876.69 north; 593027.19 east 2021-02-13T00:43:15Z- AHRS_M2 msg: 1613176995.64 epoch sec; seqNo:15023 2021-02-13T00:43:15Z- AHRS_M2 msg: 0.56 phi; -0.30 theta; -0.21 psi 2021-02-13T00:43:15Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-13T00:43:15Z- waiting for 1613176993.41, time is 1613176995.64 2021-02-13T00:43:15Z- LcmTrn::updateTrn() >>>> pose time 1613176995.64, meas time 1613176995.25 <<<< 2021-02-13T00:43:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613176995.25 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:43:15Z- TerrainNav::motionUpdate() - currEstimate = (4059877, 593027.19, 15.36) 2021-02-13T00:43:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:43:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:43:15Z- TNavBF::Projecting Measurements 2021-02-13T00:43:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:43:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:43:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:43:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:43:15Z- TNavFilter::4 good beams. 2021-02-13T00:43:15Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:43:15Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:43:15Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:43:15Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:43:15Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:43:15Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:43:15Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:43:15Z- TNavBF::mmse selected from filter -1 2021-02-13T00:43:15Z- TerrainMapOctree: (vehN, vehE ) = (4059876.44, 593027.02). 2021-02-13T00:43:15Z- TerrainMapOctree: Min Distance = 1839.12. 2021-02-13T00:43:15Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:43:15Z- There is no specific good beam that all particles have in common. 2021-02-13T00:43:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:43:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 10.26 2021-02-13T00:43:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:43:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 15.26 2021-02-13T00:43:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:43:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-13T00:43:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:43:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 18.36 2021-02-13T00:43:15Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:43:15Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:43:15Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:43:15Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:43:15Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:43:15Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:43:15Z- Filter 0 No beams used in subcloud update 2021-02-13T00:43:15Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:43:15Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:43:15Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 690.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:43:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613176995.25 sec. 2021-02-13T00:43:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:43:15Z- TNavBF::mmse selected from filter -1 2021-02-13T00:43:15Z- variances([0], [2], and psi_berg) = 690.486279 832.993189 0.000000 2021-02-13T00:43:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:43:15Z- LcmTrn::publishEstimates() - MLE : 4059876.19 593048.69 15.36 2021-02-13T00:43:15Z- LcmTrn::publishEstimates() - MMSE : 4059876.69 593027.19 15.36 2021-02-13T00:43:15Z- LcmTrn::publishEstimates() - COVAR: 690.49 832.99 0.95 0.00 2021-02-13T00:43:15Z- handleDepth()-Depth_Keller msg: 1613176995653 epoch sec; seqNo:15145; depth 15.48 2021-02-13T00:43:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176995.67 epoch sec; seqNo:15145; 4059877.07 north; 593027.19 east 2021-02-13T00:43:16Z- AHRS_M2 msg: 1613176996.06 epoch sec; seqNo:15024 2021-02-13T00:43:16Z- AHRS_M2 msg: 0.56 phi; -0.30 theta; -0.21 psi 2021-02-13T00:43:16Z- handleDepth()-Depth_Keller msg: 1613176996072 epoch sec; seqNo:15146; depth 15.59 2021-02-13T00:43:16Z- handleDvl() - RDI_Pathfinder msg: 1613176996.08 epoch sec; seqNo:7256 2021-02-13T00:43:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.53 , 99.00 , 78.31 , 107.51 2021-02-13T00:43:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , 0.02 2021-02-13T00:43:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176996.09 epoch sec; seqNo:15146; 4059877.07 north; 593027.19 east 2021-02-13T00:43:16Z- AHRS_M2 msg: 1613176996.44 epoch sec; seqNo:15025 2021-02-13T00:43:16Z- AHRS_M2 msg: 0.56 phi; -0.30 theta; -0.20 psi 2021-02-13T00:43:16Z- handleDepth()-Depth_Keller msg: 1613176996448 epoch sec; seqNo:15147; depth 15.71 2021-02-13T00:43:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176996.46 epoch sec; seqNo:15147; 4059877.45 north; 593027.19 east 2021-02-13T00:43:16Z- AHRS_M2 msg: 1613176996.85 epoch sec; seqNo:15026 2021-02-13T00:43:16Z- AHRS_M2 msg: 0.55 phi; -0.30 theta; -0.20 psi 2021-02-13T00:43:16Z- handleDepth()-Depth_Keller msg: 1613176996852 epoch sec; seqNo:15148; depth 15.81 2021-02-13T00:43:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176996.86 epoch sec; seqNo:15148; 4059877.83 north; 593027.18 east 2021-02-13T00:43:17Z- AHRS_M2 msg: 1613176997.25 epoch sec; seqNo:15027 2021-02-13T00:43:17Z- AHRS_M2 msg: 0.55 phi; -0.30 theta; -0.20 psi 2021-02-13T00:43:17Z- handleDepth()-Depth_Keller msg: 1613176997265 epoch sec; seqNo:15149; depth 15.93 2021-02-13T00:43:17Z- handleDvl() - RDI_Pathfinder msg: 1613176997.28 epoch sec; seqNo:7257 2021-02-13T00:43:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.39 , 100.08 , 78.18 , 103.73 2021-02-13T00:43:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , -0.02 2021-02-13T00:43:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176997.29 epoch sec; seqNo:15149; 4059878.21 north; 593027.18 east 2021-02-13T00:43:17Z- AHRS_M2 msg: 1613176997.65 epoch sec; seqNo:15028 2021-02-13T00:43:17Z- AHRS_M2 msg: 0.56 phi; -0.30 theta; -0.19 psi 2021-02-13T00:43:17Z- handleDepth()-Depth_Keller msg: 1613176997668 epoch sec; seqNo:15150; depth 16.05 2021-02-13T00:43:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176997.68 epoch sec; seqNo:15150; 4059878.22 north; 593028.40 east 2021-02-13T00:43:18Z- AHRS_M2 msg: 1613176998.06 epoch sec; seqNo:15029 2021-02-13T00:43:18Z- AHRS_M2 msg: 0.56 phi; -0.30 theta; -0.19 psi 2021-02-13T00:43:18Z- handleDepth()-Depth_Keller msg: 1613176998065 epoch sec; seqNo:15151; depth 16.15 2021-02-13T00:43:18Z- handleDvl() - RDI_Pathfinder msg: 1613176998.08 epoch sec; seqNo:7258 2021-02-13T00:43:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.05 , 96.50 , 78.25 , 106.23 2021-02-13T00:43:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , -0.07 2021-02-13T00:43:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176998.10 epoch sec; seqNo:15151; 4059878.60 north; 593028.39 east 2021-02-13T00:43:18Z- AHRS_M2 msg: 1613176998.46 epoch sec; seqNo:15030 2021-02-13T00:43:18Z- AHRS_M2 msg: 0.56 phi; -0.30 theta; -0.19 psi 2021-02-13T00:43:18Z- handleDepth()-Depth_Keller msg: 1613176998468 epoch sec; seqNo:15152; depth 16.27 2021-02-13T00:43:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176998.48 epoch sec; seqNo:15152; 4059878.98 north; 593028.39 east 2021-02-13T00:43:18Z- AHRS_M2 msg: 1613176998.87 epoch sec; seqNo:15031 2021-02-13T00:43:18Z- AHRS_M2 msg: 0.56 phi; -0.30 theta; -0.19 psi 2021-02-13T00:43:18Z- handleDepth()-Depth_Keller msg: 1613176998882 epoch sec; seqNo:15153; depth 16.38 2021-02-13T00:43:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176998.90 epoch sec; seqNo:15153; 4059879.36 north; 593028.39 east 2021-02-13T00:43:19Z- AHRS_M2 msg: 1613176999.27 epoch sec; seqNo:15032 2021-02-13T00:43:19Z- AHRS_M2 msg: 0.57 phi; -0.30 theta; -0.20 psi 2021-02-13T00:43:19Z- handleDepth()-Depth_Keller msg: 1613176999276 epoch sec; seqNo:15154; depth 16.49 2021-02-13T00:43:19Z- handleDvl() - RDI_Pathfinder msg: 1613176999.29 epoch sec; seqNo:7259 2021-02-13T00:43:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.42 , 97.78 , 71.92 , 106.30 2021-02-13T00:43:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.05 , -0.09 2021-02-13T00:43:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176999.31 epoch sec; seqNo:15154; 4059879.74 north; 593028.38 east 2021-02-13T00:43:19Z- AHRS_M2 msg: 1613176999.68 epoch sec; seqNo:15033 2021-02-13T00:43:19Z- AHRS_M2 msg: 0.57 phi; -0.30 theta; -0.19 psi 2021-02-13T00:43:19Z- handleDepth()-Depth_Keller msg: 1613176999689 epoch sec; seqNo:15155; depth 16.61 2021-02-13T00:43:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613176999.70 epoch sec; seqNo:15155; 4059880.12 north; 593028.38 east 2021-02-13T00:43:20Z- AHRS_M2 msg: 1613177000.08 epoch sec; seqNo:15034 2021-02-13T00:43:20Z- AHRS_M2 msg: 0.57 phi; -0.30 theta; -0.18 psi 2021-02-13T00:43:20Z- handleDepth()-Depth_Keller msg: 1613177000086 epoch sec; seqNo:15156; depth 16.75 2021-02-13T00:43:20Z- handleDvl() - RDI_Pathfinder msg: 1613177000.10 epoch sec; seqNo:7260 2021-02-13T00:43:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 97.92 , 78.45 , 105.22 2021-02-13T00:43:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.11 , -0.05 2021-02-13T00:43:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177000.11 epoch sec; seqNo:15156; 4059880.51 north; 593029.59 east 2021-02-13T00:43:20Z- AHRS_M2 msg: 1613177000.48 epoch sec; seqNo:15035 2021-02-13T00:43:20Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.17 psi 2021-02-13T00:43:20Z- handleDepth()-Depth_Keller msg: 1613177000493 epoch sec; seqNo:15157; depth 16.86 2021-02-13T00:43:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177000.51 epoch sec; seqNo:15157; 4059880.51 north; 593029.59 east 2021-02-13T00:43:20Z- LcmTrn::handleMessages() - handling 43 messages took 0.68 ms and 130 calls... 2021-02-13T00:43:20Z- waiting for 1613176998.64, time is 1613177000.51 2021-02-13T00:43:20Z- LcmTrn::updateTrn() >>>> pose time 1613177000.51, meas time 1613177000.10 <<<< 2021-02-13T00:43:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177000.10 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:43:20Z- TerrainNav::motionUpdate() - currEstimate = (4059881, 593029.59, 16.86) 2021-02-13T00:43:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:43:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:43:20Z- TNavBF::Projecting Measurements 2021-02-13T00:43:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:43:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:43:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:43:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:43:20Z- TNavFilter::4 good beams. 2021-02-13T00:43:20Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:43:20Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:43:20Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:43:20Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:43:20Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:43:20Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:43:20Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:43:20Z- TNavBF::mmse selected from filter -1 2021-02-13T00:43:20Z- TerrainMapOctree: (vehN, vehE ) = (4059880.19, 593029.39). 2021-02-13T00:43:20Z- TerrainMapOctree: Min Distance = 1834.74. 2021-02-13T00:43:20Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:43:20Z- There is no specific good beam that all particles have in common. 2021-02-13T00:43:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:43:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T00:43:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:43:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:43:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:43:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T00:43:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:43:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 17.71 2021-02-13T00:43:20Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:43:20Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:43:20Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:43:20Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:43:20Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:43:20Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:43:20Z- Filter 0 No beams used in subcloud update 2021-02-13T00:43:20Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:43:20Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:43:20Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 690.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:43:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177000.10 sec. 2021-02-13T00:43:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:43:20Z- TNavBF::mmse selected from filter -1 2021-02-13T00:43:20Z- variances([0], [2], and psi_berg) = 690.421453 832.993158 0.000000 2021-02-13T00:43:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:43:20Z- LcmTrn::publishEstimates() - MLE : 4059880.01 593051.09 16.86 2021-02-13T00:43:20Z- LcmTrn::publishEstimates() - MMSE : 4059880.51 593029.59 16.86 2021-02-13T00:43:20Z- LcmTrn::publishEstimates() - COVAR: 690.42 832.99 0.89 0.00 2021-02-13T00:43:20Z- AHRS_M2 msg: 1613177000.89 epoch sec; seqNo:15036 2021-02-13T00:43:20Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.16 psi 2021-02-13T00:43:20Z- handleDepth()-Depth_Keller msg: 1613177000892 epoch sec; seqNo:15158; depth 16.99 2021-02-13T00:43:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177000.90 epoch sec; seqNo:15158; 4059880.89 north; 593029.59 east 2021-02-13T00:43:21Z- AHRS_M2 msg: 1613177001.29 epoch sec; seqNo:15037 2021-02-13T00:43:21Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.15 psi 2021-02-13T00:43:21Z- handleDepth()-Depth_Keller msg: 1613177001296 epoch sec; seqNo:15159; depth 17.10 2021-02-13T00:43:21Z- handleDvl() - RDI_Pathfinder msg: 1613177001.31 epoch sec; seqNo:7261 2021-02-13T00:43:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.52 , 95.34 , 78.31 , 105.08 2021-02-13T00:43:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.14 , 0.01 2021-02-13T00:43:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177001.32 epoch sec; seqNo:15159; 4059881.27 north; 593029.59 east 2021-02-13T00:43:21Z- AHRS_M2 msg: 1613177001.70 epoch sec; seqNo:15038 2021-02-13T00:43:21Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.15 psi 2021-02-13T00:43:21Z- handleDepth()-Depth_Keller msg: 1613177001706 epoch sec; seqNo:15160; depth 17.21 2021-02-13T00:43:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177001.72 epoch sec; seqNo:15160; 4059881.65 north; 593029.58 east 2021-02-13T00:43:22Z- AHRS_M2 msg: 1613177002.10 epoch sec; seqNo:15039 2021-02-13T00:43:22Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.15 psi 2021-02-13T00:43:22Z- handleDepth()-Depth_Keller msg: 1613177002104 epoch sec; seqNo:15161; depth 17.34 2021-02-13T00:43:22Z- handleDvl() - RDI_Pathfinder msg: 1613177002.12 epoch sec; seqNo:7262 2021-02-13T00:43:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.52 , 97.78 , 71.52 , 103.86 2021-02-13T00:43:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.14 , 0.08 2021-02-13T00:43:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177002.14 epoch sec; seqNo:15161; 4059882.03 north; 593029.58 east 2021-02-13T00:43:22Z- AHRS_M2 msg: 1613177002.51 epoch sec; seqNo:15040 2021-02-13T00:43:22Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.16 psi 2021-02-13T00:43:22Z- handleDepth()-Depth_Keller msg: 1613177002516 epoch sec; seqNo:15162; depth 17.46 2021-02-13T00:43:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177002.55 epoch sec; seqNo:15162; 4059882.40 north; 593029.57 east 2021-02-13T00:43:22Z- AHRS_M2 msg: 1613177002.92 epoch sec; seqNo:15041 2021-02-13T00:43:22Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.16 psi 2021-02-13T00:43:22Z- handleDepth()-Depth_Keller msg: 1613177002929 epoch sec; seqNo:15163; depth 17.57 2021-02-13T00:43:22Z- handleDvl() - RDI_Pathfinder msg: 1613177002.95 epoch sec; seqNo:7263 2021-02-13T00:43:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.06 , 94.26 , 69.89 , 104.00 2021-02-13T00:43:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.09 , 0.15 2021-02-13T00:43:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177002.96 epoch sec; seqNo:15163; 4059882.40 north; 593029.57 east 2021-02-13T00:43:23Z- AHRS_M2 msg: 1613177003.31 epoch sec; seqNo:15042 2021-02-13T00:43:23Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.16 psi 2021-02-13T00:43:23Z- handleDepth()-Depth_Keller msg: 1613177003321 epoch sec; seqNo:15164; depth 17.68 2021-02-13T00:43:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177003.34 epoch sec; seqNo:15164; 4059882.80 north; 593030.79 east 2021-02-13T00:43:23Z- AHRS_M2 msg: 1613177003.71 epoch sec; seqNo:15043 2021-02-13T00:43:23Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.16 psi 2021-02-13T00:43:23Z- handleDepth()-Depth_Keller msg: 1613177003725 epoch sec; seqNo:15165; depth 17.79 2021-02-13T00:43:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177003.74 epoch sec; seqNo:15165; 4059883.17 north; 593030.79 east 2021-02-13T00:43:24Z- AHRS_M2 msg: 1613177004.12 epoch sec; seqNo:15044 2021-02-13T00:43:24Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.16 psi 2021-02-13T00:43:24Z- handleDepth()-Depth_Keller msg: 1613177004126 epoch sec; seqNo:15166; depth 17.92 2021-02-13T00:43:24Z- handleDvl() - RDI_Pathfinder msg: 1613177004.14 epoch sec; seqNo:7264 2021-02-13T00:43:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 69.89 , 102.92 2021-02-13T00:43:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , 0.16 2021-02-13T00:43:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177004.16 epoch sec; seqNo:15166; 4059883.55 north; 593030.78 east 2021-02-13T00:43:24Z- AHRS_M2 msg: 1613177004.52 epoch sec; seqNo:15045 2021-02-13T00:43:24Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.16 psi 2021-02-13T00:43:24Z- handleDepth()-Depth_Keller msg: 1613177004533 epoch sec; seqNo:15167; depth 18.04 2021-02-13T00:43:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177004.54 epoch sec; seqNo:15167; 4059883.55 north; 593030.78 east 2021-02-13T00:43:24Z- AHRS_M2 msg: 1613177004.94 epoch sec; seqNo:15046 2021-02-13T00:43:24Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.17 psi 2021-02-13T00:43:24Z- handleDepth()-Depth_Keller msg: 1613177004940 epoch sec; seqNo:15168; depth 18.15 2021-02-13T00:43:24Z- handleDvl() - RDI_Pathfinder msg: 1613177004.96 epoch sec; seqNo:7265 2021-02-13T00:43:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 97.98 , 70.70 , 104.07 2021-02-13T00:43:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.11 , 0.10 2021-02-13T00:43:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177004.97 epoch sec; seqNo:15168; 4059883.93 north; 593030.78 east 2021-02-13T00:43:25Z- AHRS_M2 msg: 1613177005.33 epoch sec; seqNo:15047 2021-02-13T00:43:25Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.17 psi 2021-02-13T00:43:25Z- handleDepth()-Depth_Keller msg: 1613177005336 epoch sec; seqNo:15169; depth 18.26 2021-02-13T00:43:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177005.36 epoch sec; seqNo:15169; 4059884.31 north; 593030.77 east 2021-02-13T00:43:25Z- AHRS_M2 msg: 1613177005.74 epoch sec; seqNo:15048 2021-02-13T00:43:25Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.17 psi 2021-02-13T00:43:25Z- handleDepth()-Depth_Keller msg: 1613177005740 epoch sec; seqNo:15170; depth 18.38 2021-02-13T00:43:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177005.75 epoch sec; seqNo:15170; 4059884.70 north; 593031.99 east 2021-02-13T00:43:25Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-13T00:43:25Z- waiting for 1613177003.51, time is 1613177005.75 2021-02-13T00:43:25Z- LcmTrn::updateTrn() >>>> pose time 1613177005.75, meas time 1613177004.96 <<<< 2021-02-13T00:43:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177004.96 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:43:25Z- TerrainNav::motionUpdate() - currEstimate = (4059885, 593031.99, 18.38) 2021-02-13T00:43:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:43:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:43:25Z- TNavBF::Projecting Measurements 2021-02-13T00:43:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:43:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:43:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:43:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:43:25Z- TNavFilter::4 good beams. 2021-02-13T00:43:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:43:25Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:43:25Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:43:25Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:43:25Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:43:25Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:43:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:43:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:43:25Z- TerrainMapOctree: (vehN, vehE ) = (4059884.07, 593031.63). 2021-02-13T00:43:25Z- TerrainMapOctree: Min Distance = 1830.35. 2021-02-13T00:43:25Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:43:25Z- There is no specific good beam that all particles have in common. 2021-02-13T00:43:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:43:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 9.54 2021-02-13T00:43:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:43:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:43:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:43:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-13T00:43:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:43:25Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 17.33 2021-02-13T00:43:25Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:43:25Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:43:25Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:43:25Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:43:25Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:43:25Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:43:25Z- Filter 0 No beams used in subcloud update 2021-02-13T00:43:25Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:43:25Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:43:25Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 690.4 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:43:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177004.96 sec. 2021-02-13T00:43:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:43:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:43:25Z- variances([0], [2], and psi_berg) = 690.351059 832.993124 0.000000 2021-02-13T00:43:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:43:25Z- LcmTrn::publishEstimates() - MLE : 4059884.20 593053.49 18.38 2021-02-13T00:43:25Z- LcmTrn::publishEstimates() - MMSE : 4059884.70 593031.99 18.38 2021-02-13T00:43:25Z- LcmTrn::publishEstimates() - COVAR: 690.35 832.99 0.86 0.00 2021-02-13T00:43:26Z- AHRS_M2 msg: 1613177006.14 epoch sec; seqNo:15049 2021-02-13T00:43:26Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.17 psi 2021-02-13T00:43:26Z- handleDepth()-Depth_Keller msg: 1613177006153 epoch sec; seqNo:15171; depth 18.48 2021-02-13T00:43:26Z- handleDvl() - RDI_Pathfinder msg: 1613177006.16 epoch sec; seqNo:7266 2021-02-13T00:43:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.42 , 97.98 , 71.51 , 104.07 2021-02-13T00:43:26Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.15 , -0.01 2021-02-13T00:43:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177006.17 epoch sec; seqNo:15171; 4059884.70 north; 593031.99 east 2021-02-13T00:43:26Z- AHRS_M2 msg: 1613177006.54 epoch sec; seqNo:15050 2021-02-13T00:43:26Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.17 psi 2021-02-13T00:43:26Z- handleDepth()-Depth_Keller msg: 1613177006554 epoch sec; seqNo:15172; depth 18.61 2021-02-13T00:43:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177006.57 epoch sec; seqNo:15172; 4059885.08 north; 593031.99 east 2021-02-13T00:43:26Z- AHRS_M2 msg: 1613177006.95 epoch sec; seqNo:15051 2021-02-13T00:43:26Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.18 psi 2021-02-13T00:43:26Z- handleDepth()-Depth_Keller msg: 1613177006982 epoch sec; seqNo:15173; depth 18.73 2021-02-13T00:43:27Z- handleDvl() - RDI_Pathfinder msg: 1613177007.01 epoch sec; seqNo:7267 2021-02-13T00:43:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 69.88 , 102.85 2021-02-13T00:43:27Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.12 , -0.12 2021-02-13T00:43:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177007.03 epoch sec; seqNo:15173; 4059885.46 north; 593031.98 east 2021-02-13T00:43:27Z- AHRS_M2 msg: 1613177007.35 epoch sec; seqNo:15052 2021-02-13T00:43:27Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.18 psi 2021-02-13T00:43:27Z- handleDepth()-Depth_Keller msg: 1613177007361 epoch sec; seqNo:15174; depth 18.84 2021-02-13T00:43:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177007.37 epoch sec; seqNo:15174; 4059885.85 north; 593033.20 east 2021-02-13T00:43:27Z- AHRS_M2 msg: 1613177007.76 epoch sec; seqNo:15053 2021-02-13T00:43:27Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.18 psi 2021-02-13T00:43:27Z- handleDepth()-Depth_Keller msg: 1613177007761 epoch sec; seqNo:15175; depth 18.96 2021-02-13T00:43:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177007.77 epoch sec; seqNo:15175; 4059886.23 north; 593033.19 east 2021-02-13T00:43:28Z- AHRS_M2 msg: 1613177008.16 epoch sec; seqNo:15054 2021-02-13T00:43:28Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.18 psi 2021-02-13T00:43:28Z- handleDepth()-Depth_Keller msg: 1613177008165 epoch sec; seqNo:15176; depth 19.06 2021-02-13T00:43:28Z- handleDvl() - RDI_Pathfinder msg: 1613177008.18 epoch sec; seqNo:7268 2021-02-13T00:43:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 94.33 , 67.43 , 104.07 2021-02-13T00:43:28Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.00 , -0.16 2021-02-13T00:43:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177008.19 epoch sec; seqNo:15176; 4059886.61 north; 593033.19 east 2021-02-13T00:43:28Z- AHRS_M2 msg: 1613177008.56 epoch sec; seqNo:15055 2021-02-13T00:43:28Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.18 psi 2021-02-13T00:43:28Z- handleDepth()-Depth_Keller msg: 1613177008567 epoch sec; seqNo:15177; depth 19.20 2021-02-13T00:43:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177008.58 epoch sec; seqNo:15177; 4059886.61 north; 593033.19 east 2021-02-13T00:43:28Z- AHRS_M2 msg: 1613177008.97 epoch sec; seqNo:15056 2021-02-13T00:43:28Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.18 psi 2021-02-13T00:43:28Z- handleDepth()-Depth_Keller msg: 1613177008982 epoch sec; seqNo:15178; depth 19.31 2021-02-13T00:43:29Z- handleDvl() - RDI_Pathfinder msg: 1613177009.00 epoch sec; seqNo:7269 2021-02-13T00:43:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 98.12 , 69.48 , 102.98 2021-02-13T00:43:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.10 , -0.12 2021-02-13T00:43:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177009.03 epoch sec; seqNo:15178; 4059886.99 north; 593033.19 east 2021-02-13T00:43:29Z- AHRS_M2 msg: 1613177009.37 epoch sec; seqNo:15057 2021-02-13T00:43:29Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.17 psi 2021-02-13T00:43:29Z- handleDepth()-Depth_Keller msg: 1613177009378 epoch sec; seqNo:15179; depth 19.45 2021-02-13T00:43:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177009.39 epoch sec; seqNo:15179; 4059887.37 north; 593033.18 east 2021-02-13T00:43:29Z- AHRS_M2 msg: 1613177009.78 epoch sec; seqNo:15058 2021-02-13T00:43:29Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.17 psi 2021-02-13T00:43:29Z- handleDepth()-Depth_Keller msg: 1613177009780 epoch sec; seqNo:15180; depth 19.54 2021-02-13T00:43:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177009.80 epoch sec; seqNo:15180; 4059887.76 north; 593034.40 east 2021-02-13T00:43:30Z- AHRS_M2 msg: 1613177010.18 epoch sec; seqNo:15059 2021-02-13T00:43:30Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.17 psi 2021-02-13T00:43:30Z- handleDepth()-Depth_Keller msg: 1613177010184 epoch sec; seqNo:15181; depth 19.67 2021-02-13T00:43:30Z- handleDvl() - RDI_Pathfinder msg: 1613177010.20 epoch sec; seqNo:7270 2021-02-13T00:43:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 91.96 , 68.66 , 102.92 2021-02-13T00:43:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.15 , -0.04 2021-02-13T00:43:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177010.23 epoch sec; seqNo:15181; 4059888.14 north; 593034.39 east 2021-02-13T00:43:30Z- AHRS_M2 msg: 1613177010.58 epoch sec; seqNo:15060 2021-02-13T00:43:30Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.17 psi 2021-02-13T00:43:30Z- handleDepth()-Depth_Keller msg: 1613177010588 epoch sec; seqNo:15182; depth 19.81 2021-02-13T00:43:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177010.60 epoch sec; seqNo:15182; 4059888.52 north; 593034.39 east 2021-02-13T00:43:30Z- LcmTrn::handleMessages() - handling 41 messages took 0.72 ms and 127 calls... 2021-02-13T00:43:30Z- waiting for 1613177008.75, time is 1613177010.60 2021-02-13T00:43:30Z- LcmTrn::updateTrn() >>>> pose time 1613177010.60, meas time 1613177010.20 <<<< 2021-02-13T00:43:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177010.20 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:43:30Z- TerrainNav::motionUpdate() - currEstimate = (4059889, 593034.39, 19.81) 2021-02-13T00:43:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:43:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:43:30Z- TNavBF::Projecting Measurements 2021-02-13T00:43:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:43:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:43:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:43:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:43:30Z- TNavFilter::4 good beams. 2021-02-13T00:43:30Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:43:30Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:43:30Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:43:30Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:43:30Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:43:30Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:43:30Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:43:30Z- TNavBF::mmse selected from filter -1 2021-02-13T00:43:30Z- TerrainMapOctree: (vehN, vehE ) = (4059888.20, 593034.19). 2021-02-13T00:43:30Z- TerrainMapOctree: Min Distance = 1825.57. 2021-02-13T00:43:30Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:43:30Z- There is no specific good beam that all particles have in common. 2021-02-13T00:43:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:43:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-13T00:43:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:43:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 13.53 2021-02-13T00:43:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:43:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 7.54 2021-02-13T00:43:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:43:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 16.95 2021-02-13T00:43:31Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:43:31Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:43:31Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:43:31Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:43:31Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:43:31Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:43:31Z- Filter 0 No beams used in subcloud update 2021-02-13T00:43:31Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:43:31Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:43:31Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 690.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:43:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177010.20 sec. 2021-02-13T00:43:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:43:31Z- TNavBF::mmse selected from filter -1 2021-02-13T00:43:31Z- variances([0], [2], and psi_berg) = 690.286377 832.993094 0.000000 2021-02-13T00:43:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:43:31Z- LcmTrn::publishEstimates() - MLE : 4059888.02 593056.89 19.81 2021-02-13T00:43:31Z- LcmTrn::publishEstimates() - MMSE : 4059888.52 593034.39 19.81 2021-02-13T00:43:31Z- LcmTrn::publishEstimates() - COVAR: 690.29 832.99 0.82 0.00 2021-02-13T00:43:31Z- AHRS_M2 msg: 1613177010.99 epoch sec; seqNo:15061 2021-02-13T00:43:31Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.17 psi 2021-02-13T00:43:31Z- handleDepth()-Depth_Keller msg: 1613177011001 epoch sec; seqNo:15183; depth 19.91 2021-02-13T00:43:31Z- handleDvl() - RDI_Pathfinder msg: 1613177011.01 epoch sec; seqNo:7271 2021-02-13T00:43:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.64 , 98.12 , 68.25 , 100.55 2021-02-13T00:43:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.10 , 0.03 2021-02-13T00:43:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177011.02 epoch sec; seqNo:15183; 4059888.90 north; 593034.39 east 2021-02-13T00:43:31Z- AHRS_M2 msg: 1613177011.40 epoch sec; seqNo:15062 2021-02-13T00:43:31Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.17 psi 2021-02-13T00:43:31Z- handleDepth()-Depth_Keller msg: 1613177011404 epoch sec; seqNo:15184; depth 20.03 2021-02-13T00:43:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177011.41 epoch sec; seqNo:15184; 4059888.90 north; 593034.39 east 2021-02-13T00:43:31Z- AHRS_M2 msg: 1613177011.80 epoch sec; seqNo:15063 2021-02-13T00:43:31Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.17 psi 2021-02-13T00:43:31Z- handleDepth()-Depth_Keller msg: 1613177011805 epoch sec; seqNo:15185; depth 20.16 2021-02-13T00:43:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177011.81 epoch sec; seqNo:15185; 4059889.28 north; 593034.38 east 2021-02-13T00:43:32Z- AHRS_M2 msg: 1613177012.20 epoch sec; seqNo:15064 2021-02-13T00:43:32Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.17 psi 2021-02-13T00:43:32Z- handleDepth()-Depth_Keller msg: 1613177012209 epoch sec; seqNo:15186; depth 20.27 2021-02-13T00:43:32Z- handleDvl() - RDI_Pathfinder msg: 1613177012.22 epoch sec; seqNo:7272 2021-02-13T00:43:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 91.96 , 67.43 , 105.35 2021-02-13T00:43:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , 0.06 2021-02-13T00:43:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177012.23 epoch sec; seqNo:15186; 4059889.66 north; 593034.38 east 2021-02-13T00:43:32Z- AHRS_M2 msg: 1613177012.60 epoch sec; seqNo:15065 2021-02-13T00:43:32Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.17 psi 2021-02-13T00:43:32Z- handleDepth()-Depth_Keller msg: 1613177012618 epoch sec; seqNo:15187; depth 20.41 2021-02-13T00:43:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177012.63 epoch sec; seqNo:15187; 4059890.05 north; 593035.59 east 2021-02-13T00:43:33Z- AHRS_M2 msg: 1613177013.01 epoch sec; seqNo:15066 2021-02-13T00:43:33Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.17 psi 2021-02-13T00:43:33Z- handleDepth()-Depth_Keller msg: 1613177013013 epoch sec; seqNo:15188; depth 20.53 2021-02-13T00:43:33Z- handleDvl() - RDI_Pathfinder msg: 1613177013.03 epoch sec; seqNo:7273 2021-02-13T00:43:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.20 , 90.81 , 67.43 , 102.98 2021-02-13T00:43:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , 0.03 2021-02-13T00:43:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177013.04 epoch sec; seqNo:15188; 4059890.05 north; 593035.59 east 2021-02-13T00:43:33Z- AHRS_M2 msg: 1613177013.41 epoch sec; seqNo:15067 2021-02-13T00:43:33Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:43:33Z- handleDepth()-Depth_Keller msg: 1613177013425 epoch sec; seqNo:15189; depth 20.65 2021-02-13T00:43:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177013.44 epoch sec; seqNo:15189; 4059890.43 north; 593035.59 east 2021-02-13T00:43:33Z- AHRS_M2 msg: 1613177013.82 epoch sec; seqNo:15068 2021-02-13T00:43:33Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:43:33Z- handleDepth()-Depth_Keller msg: 1613177013820 epoch sec; seqNo:15190; depth 20.77 2021-02-13T00:43:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177013.84 epoch sec; seqNo:15190; 4059890.80 north; 593035.59 east 2021-02-13T00:43:34Z- AHRS_M2 msg: 1613177014.22 epoch sec; seqNo:15069 2021-02-13T00:43:34Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-13T00:43:34Z- handleDepth()-Depth_Keller msg: 1613177014224 epoch sec; seqNo:15191; depth 20.90 2021-02-13T00:43:34Z- handleDvl() - RDI_Pathfinder msg: 1613177014.24 epoch sec; seqNo:7274 2021-02-13T00:43:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.78 , 98.25 , 66.62 , 103.12 2021-02-13T00:43:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , -0.02 2021-02-13T00:43:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177014.26 epoch sec; seqNo:15191; 4059891.18 north; 593035.58 east 2021-02-13T00:43:34Z- AHRS_M2 msg: 1613177014.63 epoch sec; seqNo:15070 2021-02-13T00:43:34Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-13T00:43:34Z- handleDepth()-Depth_Keller msg: 1613177014631 epoch sec; seqNo:15192; depth 21.03 2021-02-13T00:43:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177014.64 epoch sec; seqNo:15192; 4059891.18 north; 593035.58 east 2021-02-13T00:43:35Z- AHRS_M2 msg: 1613177015.03 epoch sec; seqNo:15071 2021-02-13T00:43:35Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-13T00:43:35Z- handleDepth()-Depth_Keller msg: 1613177015039 epoch sec; seqNo:15193; depth 21.15 2021-02-13T00:43:35Z- handleDvl() - RDI_Pathfinder msg: 1613177015.05 epoch sec; seqNo:7275 2021-02-13T00:43:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 98.05 , 67.43 , 101.70 2021-02-13T00:43:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.02 2021-02-13T00:43:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177015.06 epoch sec; seqNo:15193; 4059891.58 north; 593036.80 east 2021-02-13T00:43:35Z- AHRS_M2 msg: 1613177015.43 epoch sec; seqNo:15072 2021-02-13T00:43:35Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-13T00:43:35Z- handleDepth()-Depth_Keller msg: 1613177015441 epoch sec; seqNo:15194; depth 21.29 2021-02-13T00:43:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177015.45 epoch sec; seqNo:15194; 4059891.95 north; 593036.79 east 2021-02-13T00:43:35Z- AHRS_M2 msg: 1613177015.84 epoch sec; seqNo:15073 2021-02-13T00:43:35Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.19 psi 2021-02-13T00:43:35Z- handleDepth()-Depth_Keller msg: 1613177015841 epoch sec; seqNo:15195; depth 21.41 2021-02-13T00:43:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177015.85 epoch sec; seqNo:15195; 4059892.33 north; 593036.79 east 2021-02-13T00:43:36Z- LcmTrn::handleMessages() - handling 44 messages took 0.60 ms and 132 calls... 2021-02-13T00:43:36Z- waiting for 1613177013.60, time is 1613177015.85 2021-02-13T00:43:36Z- LcmTrn::updateTrn() >>>> pose time 1613177015.85, meas time 1613177015.05 <<<< 2021-02-13T00:43:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177015.05 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:43:36Z- TerrainNav::motionUpdate() - currEstimate = (4059892, 593036.79, 21.41) 2021-02-13T00:43:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:43:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:43:36Z- TNavBF::Projecting Measurements 2021-02-13T00:43:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:43:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:43:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:43:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:43:36Z- TNavFilter::4 good beams. 2021-02-13T00:43:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:43:36Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:43:36Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:43:36Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:43:36Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:43:36Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:43:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:43:36Z- TNavBF::mmse selected from filter -1 2021-02-13T00:43:36Z- TerrainMapOctree: (vehN, vehE ) = (4059891.75, 593036.42). 2021-02-13T00:43:36Z- TerrainMapOctree: Min Distance = 1821.43. 2021-02-13T00:43:36Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:43:36Z- There is no specific good beam that all particles have in common. 2021-02-13T00:43:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:43:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-13T00:43:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:43:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 15.38 2021-02-13T00:43:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:43:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 7.28 2021-02-13T00:43:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:43:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.80 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 16.55 2021-02-13T00:43:36Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:43:36Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:43:36Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:43:36Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:43:36Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:43:36Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:43:36Z- Filter 0 No beams used in subcloud update 2021-02-13T00:43:36Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:43:36Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:43:36Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 690.2 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:43:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177015.05 sec. 2021-02-13T00:43:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:43:36Z- TNavBF::mmse selected from filter -1 2021-02-13T00:43:36Z- variances([0], [2], and psi_berg) = 690.218390 832.993062 0.000000 2021-02-13T00:43:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:43:36Z- LcmTrn::publishEstimates() - MLE : 4059891.83 593058.29 21.41 2021-02-13T00:43:36Z- LcmTrn::publishEstimates() - MMSE : 4059892.33 593036.79 21.41 2021-02-13T00:43:36Z- LcmTrn::publishEstimates() - COVAR: 690.22 832.99 0.80 0.00 2021-02-13T00:43:36Z- AHRS_M2 msg: 1613177016.24 epoch sec; seqNo:15074 2021-02-13T00:43:36Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.19 psi 2021-02-13T00:43:36Z- handleDepth()-Depth_Keller msg: 1613177016246 epoch sec; seqNo:15196; depth 21.55 2021-02-13T00:43:36Z- handleDvl() - RDI_Pathfinder msg: 1613177016.26 epoch sec; seqNo:7276 2021-02-13T00:43:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.71 , 98.18 , 67.84 , 104.27 2021-02-13T00:43:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.10 , 0.02 2021-02-13T00:43:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177016.27 epoch sec; seqNo:15196; 4059892.33 north; 593036.79 east 2021-02-13T00:43:36Z- AHRS_M2 msg: 1613177016.64 epoch sec; seqNo:15075 2021-02-13T00:43:36Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.19 psi 2021-02-13T00:43:36Z- handleDepth()-Depth_Keller msg: 1613177016648 epoch sec; seqNo:15197; depth 21.69 2021-02-13T00:43:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177016.66 epoch sec; seqNo:15197; 4059892.71 north; 593036.79 east 2021-02-13T00:43:37Z- AHRS_M2 msg: 1613177017.05 epoch sec; seqNo:15076 2021-02-13T00:43:37Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.20 psi 2021-02-13T00:43:37Z- handleDepth()-Depth_Keller msg: 1613177017055 epoch sec; seqNo:15198; depth 21.80 2021-02-13T00:43:37Z- handleDvl() - RDI_Pathfinder msg: 1613177017.07 epoch sec; seqNo:7277 2021-02-13T00:43:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 67.43 , 105.28 2021-02-13T00:43:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , 0.08 2021-02-13T00:43:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177017.08 epoch sec; seqNo:15198; 4059893.09 north; 593036.78 east 2021-02-13T00:43:37Z- AHRS_M2 msg: 1613177017.45 epoch sec; seqNo:15077 2021-02-13T00:43:37Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.20 psi 2021-02-13T00:43:37Z- handleDepth()-Depth_Keller msg: 1613177017458 epoch sec; seqNo:15199; depth 21.95 2021-02-13T00:43:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177017.47 epoch sec; seqNo:15199; 4059893.47 north; 593036.78 east 2021-02-13T00:43:37Z- AHRS_M2 msg: 1613177017.86 epoch sec; seqNo:15078 2021-02-13T00:43:37Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.20 psi 2021-02-13T00:43:37Z- handleDepth()-Depth_Keller msg: 1613177017861 epoch sec; seqNo:15200; depth 22.07 2021-02-13T00:43:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177017.90 epoch sec; seqNo:15200; 4059893.48 north; 593038.00 east 2021-02-13T00:43:38Z- AHRS_M2 msg: 1613177018.26 epoch sec; seqNo:15079 2021-02-13T00:43:38Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.20 psi 2021-02-13T00:43:38Z- handleDepth()-Depth_Keller msg: 1613177018269 epoch sec; seqNo:15201; depth 22.21 2021-02-13T00:43:38Z- handleDvl() - RDI_Pathfinder msg: 1613177018.29 epoch sec; seqNo:7278 2021-02-13T00:43:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 97.98 , 66.21 , 105.28 2021-02-13T00:43:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.13 2021-02-13T00:43:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177018.30 epoch sec; seqNo:15201; 4059893.86 north; 593037.99 east 2021-02-13T00:43:38Z- AHRS_M2 msg: 1613177018.66 epoch sec; seqNo:15080 2021-02-13T00:43:38Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-13T00:43:38Z- handleDepth()-Depth_Keller msg: 1613177018668 epoch sec; seqNo:15202; depth 22.36 2021-02-13T00:43:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177018.68 epoch sec; seqNo:15202; 4059894.24 north; 593037.99 east 2021-02-13T00:43:39Z- AHRS_M2 msg: 1613177019.07 epoch sec; seqNo:15081 2021-02-13T00:43:39Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.20 psi 2021-02-13T00:43:39Z- handleDepth()-Depth_Keller msg: 1613177019074 epoch sec; seqNo:15203; depth 22.48 2021-02-13T00:43:39Z- handleDvl() - RDI_Pathfinder msg: 1613177019.09 epoch sec; seqNo:7279 2021-02-13T00:43:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 98.12 , 67.02 , 106.64 2021-02-13T00:43:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.13 , 0.12 2021-02-13T00:43:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177019.10 epoch sec; seqNo:15203; 4059894.24 north; 593037.99 east 2021-02-13T00:43:39Z- AHRS_M2 msg: 1613177019.47 epoch sec; seqNo:15082 2021-02-13T00:43:39Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.20 psi 2021-02-13T00:43:39Z- handleDepth()-Depth_Keller msg: 1613177019486 epoch sec; seqNo:15204; depth 22.63 2021-02-13T00:43:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177019.50 epoch sec; seqNo:15204; 4059894.62 north; 593037.99 east 2021-02-13T00:43:39Z- AHRS_M2 msg: 1613177019.88 epoch sec; seqNo:15083 2021-02-13T00:43:39Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.20 psi 2021-02-13T00:43:39Z- handleDepth()-Depth_Keller msg: 1613177019887 epoch sec; seqNo:15205; depth 22.77 2021-02-13T00:43:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177019.93 epoch sec; seqNo:15205; 4059895.01 north; 593039.20 east 2021-02-13T00:43:40Z- AHRS_M2 msg: 1613177020.28 epoch sec; seqNo:15084 2021-02-13T00:43:40Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.20 psi 2021-02-13T00:43:40Z- handleDepth()-Depth_Keller msg: 1613177020289 epoch sec; seqNo:15206; depth 22.91 2021-02-13T00:43:40Z- handleDvl() - RDI_Pathfinder msg: 1613177020.30 epoch sec; seqNo:7280 2021-02-13T00:43:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.74 , 98.05 , 65.79 , 105.35 2021-02-13T00:43:40Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.16 , 0.03 2021-02-13T00:43:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177020.31 epoch sec; seqNo:15206; 4059895.01 north; 593039.20 east 2021-02-13T00:43:40Z- AHRS_M2 msg: 1613177020.68 epoch sec; seqNo:15085 2021-02-13T00:43:40Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.20 psi 2021-02-13T00:43:40Z- handleDepth()-Depth_Keller msg: 1613177020697 epoch sec; seqNo:15207; depth 23.05 2021-02-13T00:43:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177020.71 epoch sec; seqNo:15207; 4059895.39 north; 593039.20 east 2021-02-13T00:43:41Z- AHRS_M2 msg: 1613177021.09 epoch sec; seqNo:15086 2021-02-13T00:43:41Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.21 psi 2021-02-13T00:43:41Z- handleDepth()-Depth_Keller msg: 1613177021092 epoch sec; seqNo:15208; depth 23.18 2021-02-13T00:43:41Z- handleDvl() - RDI_Pathfinder msg: 1613177021.11 epoch sec; seqNo:7281 2021-02-13T00:43:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 98.12 , 66.62 , 105.42 2021-02-13T00:43:41Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.09 , -0.08 2021-02-13T00:43:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177021.12 epoch sec; seqNo:15208; 4059895.77 north; 593039.19 east 2021-02-13T00:43:41Z- LcmTrn::handleMessages() - handling 45 messages took 0.73 ms and 131 calls... 2021-02-13T00:43:41Z- waiting for 1613177018.85, time is 1613177021.12 2021-02-13T00:43:41Z- LcmTrn::updateTrn() equating pose 1613177021.12 and meas times 1613177021.12 2021-02-13T00:43:41Z- LcmTrn::updateTrn() >>>> pose time 1613177021.12, meas time 1613177021.12 <<<< 2021-02-13T00:43:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177021.12 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:43:41Z- TerrainNav::motionUpdate() - currEstimate = (4059896, 593039.19, 23.18) 2021-02-13T00:43:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:43:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:43:41Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177021.12 sec.; No relevant navigation data available 2021-02-13T00:43:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:43:41Z- TNavBF::mmse selected from filter -1 2021-02-13T00:43:41Z- variances([0], [2], and psi_berg) = 690.168417 832.993038 0.000000 2021-02-13T00:43:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:43:41Z- LcmTrn::publishEstimates() - MLE : 4059895.27 593060.69 23.18 2021-02-13T00:43:41Z- LcmTrn::publishEstimates() - MMSE : 4059895.77 593039.19 23.18 2021-02-13T00:43:41Z- LcmTrn::publishEstimates() - COVAR: 690.17 832.99 1.02 0.00 2021-02-13T00:43:41Z- AHRS_M2 msg: 1613177021.49 epoch sec; seqNo:15087 2021-02-13T00:43:41Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.21 psi 2021-02-13T00:43:41Z- handleDepth()-Depth_Keller msg: 1613177021496 epoch sec; seqNo:15209; depth 23.34 2021-02-13T00:43:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177021.51 epoch sec; seqNo:15209; 4059896.15 north; 593039.19 east 2021-02-13T00:43:41Z- AHRS_M2 msg: 1613177021.90 epoch sec; seqNo:15088 2021-02-13T00:43:41Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.21 psi 2021-02-13T00:43:41Z- handleDepth()-Depth_Keller msg: 1613177021900 epoch sec; seqNo:15210; depth 23.45 2021-02-13T00:43:41Z- handleDvl() - RDI_Pathfinder msg: 1613177021.91 epoch sec; seqNo:7282 2021-02-13T00:43:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 98.12 , 65.80 , 104.20 2021-02-13T00:43:41Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.00 , -0.17 2021-02-13T00:43:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177021.92 epoch sec; seqNo:15210; 4059896.54 north; 593040.41 east 2021-02-13T00:43:42Z- AHRS_M2 msg: 1613177022.30 epoch sec; seqNo:15089 2021-02-13T00:43:42Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.21 psi 2021-02-13T00:43:42Z- handleDepth()-Depth_Keller msg: 1613177022305 epoch sec; seqNo:15211; depth 23.60 2021-02-13T00:43:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177022.32 epoch sec; seqNo:15211; 4059896.54 north; 593040.41 east 2021-02-13T00:43:42Z- AHRS_M2 msg: 1613177022.70 epoch sec; seqNo:15090 2021-02-13T00:43:42Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.21 psi 2021-02-13T00:43:42Z- handleDepth()-Depth_Keller msg: 1613177022713 epoch sec; seqNo:15212; depth 23.75 2021-02-13T00:43:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177022.72 epoch sec; seqNo:15212; 4059896.92 north; 593040.40 east 2021-02-13T00:43:43Z- AHRS_M2 msg: 1613177023.11 epoch sec; seqNo:15091 2021-02-13T00:43:43Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.21 psi 2021-02-13T00:43:43Z- handleDepth()-Depth_Keller msg: 1613177023117 epoch sec; seqNo:15213; depth 23.87 2021-02-13T00:43:43Z- handleDvl() - RDI_Pathfinder msg: 1613177023.13 epoch sec; seqNo:7283 2021-02-13T00:43:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 93.32 , 0.00 , 99.40 2021-02-13T00:43:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.10 , -0.16 2021-02-13T00:43:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177023.14 epoch sec; seqNo:15213; 4059897.30 north; 593040.40 east 2021-02-13T00:43:43Z- AHRS_M2 msg: 1613177023.51 epoch sec; seqNo:15092 2021-02-13T00:43:43Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.21 psi 2021-02-13T00:43:43Z- handleDepth()-Depth_Keller msg: 1613177023518 epoch sec; seqNo:15214; depth 24.01 2021-02-13T00:43:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177023.53 epoch sec; seqNo:15214; 4059897.68 north; 593040.39 east 2021-02-13T00:43:43Z- AHRS_M2 msg: 1613177023.91 epoch sec; seqNo:15093 2021-02-13T00:43:43Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.21 psi 2021-02-13T00:43:43Z- handleDepth()-Depth_Keller msg: 1613177023920 epoch sec; seqNo:15215; depth 24.14 2021-02-13T00:43:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177023.93 epoch sec; seqNo:15215; 4059898.06 north; 593040.39 east 2021-02-13T00:43:44Z- AHRS_M2 msg: 1613177024.32 epoch sec; seqNo:15094 2021-02-13T00:43:44Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.21 psi 2021-02-13T00:43:44Z- handleDepth()-Depth_Keller msg: 1613177024329 epoch sec; seqNo:15216; depth 24.29 2021-02-13T00:43:44Z- handleDvl() - RDI_Pathfinder msg: 1613177024.34 epoch sec; seqNo:7284 2021-02-13T00:43:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 64.57 , 97.91 2021-02-13T00:43:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.21 , -0.08 2021-02-13T00:43:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177024.35 epoch sec; seqNo:15216; 4059898.43 north; 593040.39 east 2021-02-13T00:43:44Z- AHRS_M2 msg: 1613177024.72 epoch sec; seqNo:15095 2021-02-13T00:43:44Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.21 psi 2021-02-13T00:43:44Z- handleDepth()-Depth_Keller msg: 1613177024730 epoch sec; seqNo:15217; depth 24.42 2021-02-13T00:43:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177024.74 epoch sec; seqNo:15217; 4059898.83 north; 593041.60 east 2021-02-13T00:43:45Z- AHRS_M2 msg: 1613177025.13 epoch sec; seqNo:15096 2021-02-13T00:43:45Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.21 psi 2021-02-13T00:43:45Z- handleDepth()-Depth_Keller msg: 1613177025137 epoch sec; seqNo:15218; depth 24.56 2021-02-13T00:43:45Z- handleDvl() - RDI_Pathfinder msg: 1613177025.15 epoch sec; seqNo:7285 2021-02-13T00:43:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 98.18 , 63.35 , 101.83 2021-02-13T00:43:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.18 , -0.01 2021-02-13T00:43:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177025.17 epoch sec; seqNo:15218; 4059899.21 north; 593041.60 east 2021-02-13T00:43:45Z- AHRS_M2 msg: 1613177025.53 epoch sec; seqNo:15097 2021-02-13T00:43:45Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.21 psi 2021-02-13T00:43:45Z- handleDepth()-Depth_Keller msg: 1613177025541 epoch sec; seqNo:15219; depth 24.70 2021-02-13T00:43:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177025.55 epoch sec; seqNo:15219; 4059899.58 north; 593041.59 east 2021-02-13T00:43:45Z- AHRS_M2 msg: 1613177025.95 epoch sec; seqNo:15098 2021-02-13T00:43:45Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.20 psi 2021-02-13T00:43:45Z- handleDepth()-Depth_Keller msg: 1613177025954 epoch sec; seqNo:15220; depth 24.82 2021-02-13T00:43:45Z- handleDvl() - RDI_Pathfinder msg: 1613177025.97 epoch sec; seqNo:7286 2021-02-13T00:43:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 0.00 , 78.85 , 99.54 2021-02-13T00:43:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.19 , 0.11 2021-02-13T00:43:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177025.98 epoch sec; seqNo:15220; 4059899.58 north; 593041.59 east 2021-02-13T00:43:46Z- LcmTrn::handleMessages() - handling 41 messages took 0.69 ms and 128 calls... 2021-02-13T00:43:46Z- waiting for 1613177024.12, time is 1613177025.98 2021-02-13T00:43:46Z- LcmTrn::updateTrn() equating pose 1613177025.98 and meas times 1613177025.98 2021-02-13T00:43:46Z- LcmTrn::updateTrn() >>>> pose time 1613177025.98, meas time 1613177025.98 <<<< 2021-02-13T00:43:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177025.98 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:43:46Z- TerrainNav::motionUpdate() - currEstimate = (4059900, 593041.59, 24.82) 2021-02-13T00:43:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:43:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:43:46Z- TNavBF::Projecting Measurements 2021-02-13T00:43:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:43:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:43:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:43:46Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:43:46Z- TNavFilter::3 good beams. 2021-02-13T00:43:46Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:43:46Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:43:46Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:43:46Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:43:46Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:43:46Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:43:46Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:43:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:43:46Z- TerrainMapOctree: (vehN, vehE ) = (4059899.58, 593041.59). 2021-02-13T00:43:46Z- TerrainMapOctree: Min Distance = 1812.14. 2021-02-13T00:43:46Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:43:46Z- There is no specific good beam that all particles have in common. 2021-02-13T00:43:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:43:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 9.95 2021-02-13T00:43:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:43:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 9.95 2021-02-13T00:43:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:43:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.85 2021-02-13T00:43:46Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:43:46Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:43:46Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:43:46Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:43:46Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:43:46Z- Filter 0 No beams used in subcloud update 2021-02-13T00:43:46Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:43:46Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:43:46Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 690.1 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:43:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177025.98 sec. 2021-02-13T00:43:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:43:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:43:46Z- variances([0], [2], and psi_berg) = 690.116636 832.993013 0.000000 2021-02-13T00:43:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:43:46Z- LcmTrn::publishEstimates() - MLE : 4059899.08 593063.09 24.82 2021-02-13T00:43:46Z- LcmTrn::publishEstimates() - MMSE : 4059899.58 593041.59 24.82 2021-02-13T00:43:46Z- LcmTrn::publishEstimates() - COVAR: 690.12 832.99 0.97 0.00 2021-02-13T00:43:46Z- AHRS_M2 msg: 1613177026.34 epoch sec; seqNo:15099 2021-02-13T00:43:46Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.20 psi 2021-02-13T00:43:46Z- handleDepth()-Depth_Keller msg: 1613177026344 epoch sec; seqNo:15221; depth 24.96 2021-02-13T00:43:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177026.36 epoch sec; seqNo:15221; 4059899.96 north; 593041.59 east 2021-02-13T00:43:46Z- AHRS_M2 msg: 1613177026.74 epoch sec; seqNo:15100 2021-02-13T00:43:46Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.20 psi 2021-02-13T00:43:46Z- handleDepth()-Depth_Keller msg: 1613177026749 epoch sec; seqNo:15222; depth 25.08 2021-02-13T00:43:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177026.76 epoch sec; seqNo:15222; 4059900.34 north; 593041.59 east 2021-02-13T00:43:47Z- AHRS_M2 msg: 1613177027.15 epoch sec; seqNo:15101 2021-02-13T00:43:47Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.20 psi 2021-02-13T00:43:47Z- handleDepth()-Depth_Keller msg: 1613177027157 epoch sec; seqNo:15223; depth 25.23 2021-02-13T00:43:47Z- handleDvl() - RDI_Pathfinder msg: 1613177027.17 epoch sec; seqNo:7287 2021-02-13T00:43:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 89.60 , 63.35 , 99.33 2021-02-13T00:43:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.09 2021-02-13T00:43:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177027.18 epoch sec; seqNo:15223; 4059900.34 north; 593041.59 east 2021-02-13T00:43:47Z- AHRS_M2 msg: 1613177027.56 epoch sec; seqNo:15102 2021-02-13T00:43:47Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.20 psi 2021-02-13T00:43:47Z- handleDepth()-Depth_Keller msg: 1613177027561 epoch sec; seqNo:15224; depth 25.37 2021-02-13T00:43:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177027.58 epoch sec; seqNo:15224; 4059900.72 north; 593041.58 east 2021-02-13T00:43:47Z- AHRS_M2 msg: 1613177027.96 epoch sec; seqNo:15103 2021-02-13T00:43:47Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.19 psi 2021-02-13T00:43:47Z- handleDepth()-Depth_Keller msg: 1613177027961 epoch sec; seqNo:15225; depth 25.49 2021-02-13T00:43:47Z- handleDvl() - RDI_Pathfinder msg: 1613177027.98 epoch sec; seqNo:7288 2021-02-13T00:43:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 87.10 , 63.35 , 102.24 2021-02-13T00:43:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , 0.05 2021-02-13T00:43:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177027.99 epoch sec; seqNo:15225; 4059901.10 north; 593041.58 east 2021-02-13T00:43:48Z- AHRS_M2 msg: 1613177028.37 epoch sec; seqNo:15104 2021-02-13T00:43:48Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-13T00:43:48Z- handleDepth()-Depth_Keller msg: 1613177028370 epoch sec; seqNo:15226; depth 25.64 2021-02-13T00:43:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177028.38 epoch sec; seqNo:15226; 4059901.11 north; 593042.80 east 2021-02-13T00:43:48Z- AHRS_M2 msg: 1613177028.80 epoch sec; seqNo:15105 2021-02-13T00:43:48Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-13T00:43:48Z- handleDepth()-Depth_Keller msg: 1613177028807 epoch sec; seqNo:15227; depth 25.77 2021-02-13T00:43:48Z- handleDvl() - RDI_Pathfinder msg: 1613177028.82 epoch sec; seqNo:7289 2021-02-13T00:43:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 0.00 , 63.35 , 98.25 2021-02-13T00:43:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.06 , -0.01 2021-02-13T00:43:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177028.84 epoch sec; seqNo:15227; 4059901.49 north; 593042.79 east 2021-02-13T00:43:49Z- AHRS_M2 msg: 1613177029.17 epoch sec; seqNo:15106 2021-02-13T00:43:49Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.19 psi 2021-02-13T00:43:49Z- handleDepth()-Depth_Keller msg: 1613177029177 epoch sec; seqNo:15228; depth 25.91 2021-02-13T00:43:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177029.19 epoch sec; seqNo:15228; 4059901.87 north; 593042.79 east 2021-02-13T00:43:49Z- AHRS_M2 msg: 1613177029.57 epoch sec; seqNo:15107 2021-02-13T00:43:49Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.19 psi 2021-02-13T00:43:49Z- handleDepth()-Depth_Keller msg: 1613177029577 epoch sec; seqNo:15229; depth 26.02 2021-02-13T00:43:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177029.59 epoch sec; seqNo:15229; 4059902.25 north; 593042.79 east 2021-02-13T00:43:49Z- AHRS_M2 msg: 1613177029.97 epoch sec; seqNo:15108 2021-02-13T00:43:49Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-13T00:43:49Z- handleDepth()-Depth_Keller msg: 1613177029989 epoch sec; seqNo:15230; depth 26.15 2021-02-13T00:43:50Z- handleDvl() - RDI_Pathfinder msg: 1613177030.00 epoch sec; seqNo:7290 2021-02-13T00:43:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 88.25 , 63.76 , 97.98 2021-02-13T00:43:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , -0.03 2021-02-13T00:43:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177030.01 epoch sec; seqNo:15230; 4059902.25 north; 593042.79 east 2021-02-13T00:43:50Z- AHRS_M2 msg: 1613177030.38 epoch sec; seqNo:15109 2021-02-13T00:43:50Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-13T00:43:50Z- handleDepth()-Depth_Keller msg: 1613177030389 epoch sec; seqNo:15231; depth 26.30 2021-02-13T00:43:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177030.41 epoch sec; seqNo:15231; 4059902.63 north; 593042.78 east 2021-02-13T00:43:50Z- AHRS_M2 msg: 1613177030.80 epoch sec; seqNo:15110 2021-02-13T00:43:50Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-13T00:43:50Z- handleDepth()-Depth_Keller msg: 1613177030809 epoch sec; seqNo:15232; depth 26.42 2021-02-13T00:43:50Z- handleDvl() - RDI_Pathfinder msg: 1613177030.84 epoch sec; seqNo:7291 2021-02-13T00:43:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.66 , 0.00 , 63.76 , 97.98 2021-02-13T00:43:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , -0.02 2021-02-13T00:43:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177030.85 epoch sec; seqNo:15232; 4059903.02 north; 593044.00 east 2021-02-13T00:43:51Z- AHRS_M2 msg: 1613177031.19 epoch sec; seqNo:15111 2021-02-13T00:43:51Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-13T00:43:51Z- handleDepth()-Depth_Keller msg: 1613177031193 epoch sec; seqNo:15233; depth 26.56 2021-02-13T00:43:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:43:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:43:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:43:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:43:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:43:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:43:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:43:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:43:51Z- handleDvl() - RDI_Pathfinder msg: 1613177031.19 epoch sec; seqNo:7292 2021-02-13T00:43:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:43:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , -0.02 2021-02-13T00:43:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177031.21 epoch sec; seqNo:15233; 4059903.40 north; 593043.99 east 2021-02-13T00:43:51Z- LcmTrn::handleMessages() - handling 45 messages took 0.71 ms and 131 calls... 2021-02-13T00:43:51Z- waiting for 1613177028.98, time is 1613177031.21 2021-02-13T00:43:51Z- LcmTrn::updateTrn() equating pose 1613177031.21 and meas times 1613177031.21 2021-02-13T00:43:51Z- LcmTrn::updateTrn() >>>> pose time 1613177031.21, meas time 1613177031.21 <<<< 2021-02-13T00:43:51Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177031.21, ping # 0. 2021-02-13T00:43:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177031.21 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:43:51Z- TerrainNav::motionUpdate() - currEstimate = (4059903, 593043.99, 26.56) 2021-02-13T00:43:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:43:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:43:51Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:43:51Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:43:51Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177031.21 sec.; No relevant navigation data available 2021-02-13T00:43:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:43:51Z- TNavBF::mmse selected from filter -1 2021-02-13T00:43:51Z- variances([0], [2], and psi_berg) = 690.064884 832.992989 0.000000 2021-02-13T00:43:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:43:51Z- LcmTrn::publishEstimates() - MLE : 4059902.90 593065.49 26.56 2021-02-13T00:43:51Z- LcmTrn::publishEstimates() - MMSE : 4059903.40 593043.99 26.56 2021-02-13T00:43:51Z- LcmTrn::publishEstimates() - COVAR: 690.06 832.99 1.20 0.00 2021-02-13T00:43:51Z- AHRS_M2 msg: 1613177031.59 epoch sec; seqNo:15112 2021-02-13T00:43:51Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-13T00:43:51Z- handleDepth()-Depth_Keller msg: 1613177031606 epoch sec; seqNo:15234; depth 26.69 2021-02-13T00:43:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177031.61 epoch sec; seqNo:15234; 4059903.78 north; 593043.99 east 2021-02-13T00:43:51Z- AHRS_M2 msg: 1613177032.00 epoch sec; seqNo:15113 2021-02-13T00:43:51Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-13T00:43:52Z- handleDepth()-Depth_Keller msg: 1613177032000 epoch sec; seqNo:15235; depth 26.79 2021-02-13T00:43:52Z- handleDvl() - RDI_Pathfinder msg: 1613177032.01 epoch sec; seqNo:7293 2021-02-13T00:43:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:43:52Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:43:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177032.04 epoch sec; seqNo:15235; 4059903.78 north; 593043.99 east 2021-02-13T00:43:52Z- AHRS_M2 msg: 1613177032.40 epoch sec; seqNo:15114 2021-02-13T00:43:52Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-13T00:43:52Z- handleDepth()-Depth_Keller msg: 1613177032409 epoch sec; seqNo:15236; depth 26.94 2021-02-13T00:43:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:43:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:43:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:43:52Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:43:52Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:43:52Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:43:52Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:43:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:43:52Z- handleDvl() - RDI_Pathfinder msg: 1613177032.41 epoch sec; seqNo:7294 2021-02-13T00:43:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:43:52Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:43:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177032.42 epoch sec; seqNo:15236; 4059904.16 north; 593043.99 east 2021-02-13T00:43:52Z- AHRS_M2 msg: 1613177032.83 epoch sec; seqNo:15115 2021-02-13T00:43:52Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-13T00:43:52Z- handleDepth()-Depth_Keller msg: 1613177032834 epoch sec; seqNo:15237; depth 27.06 2021-02-13T00:43:52Z- handleDvl() - RDI_Pathfinder msg: 1613177032.85 epoch sec; seqNo:7295 2021-02-13T00:43:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.25 , 0.00 , 62.12 , 97.71 2021-02-13T00:43:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , 0.06 2021-02-13T00:43:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177032.87 epoch sec; seqNo:15237; 4059904.54 north; 593043.98 east 2021-02-13T00:43:53Z- AHRS_M2 msg: 1613177033.21 epoch sec; seqNo:15116 2021-02-13T00:43:53Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.18 psi 2021-02-13T00:43:53Z- handleDepth()-Depth_Keller msg: 1613177033212 epoch sec; seqNo:15238; depth 27.20 2021-02-13T00:43:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177033.23 epoch sec; seqNo:15238; 4059904.93 north; 593045.20 east 2021-02-13T00:43:53Z- AHRS_M2 msg: 1613177033.61 epoch sec; seqNo:15117 2021-02-13T00:43:53Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:43:53Z- handleDepth()-Depth_Keller msg: 1613177033616 epoch sec; seqNo:15239; depth 27.32 2021-02-13T00:43:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:43:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:43:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:43:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:43:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:43:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:43:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:43:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:43:53Z- handleDvl() - RDI_Pathfinder msg: 1613177033.62 epoch sec; seqNo:7296 2021-02-13T00:43:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:43:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , 0.06 2021-02-13T00:43:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177033.63 epoch sec; seqNo:15239; 4059904.93 north; 593045.20 east 2021-02-13T00:43:54Z- AHRS_M2 msg: 1613177034.02 epoch sec; seqNo:15118 2021-02-13T00:43:54Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:43:54Z- handleDepth()-Depth_Keller msg: 1613177034025 epoch sec; seqNo:15240; depth 27.46 2021-02-13T00:43:54Z- handleDvl() - RDI_Pathfinder msg: 1613177034.04 epoch sec; seqNo:7297 2021-02-13T00:43:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.66 , 85.00 , 62.53 , 97.98 2021-02-13T00:43:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.06 2021-02-13T00:43:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177034.05 epoch sec; seqNo:15240; 4059905.31 north; 593045.19 east 2021-02-13T00:43:54Z- AHRS_M2 msg: 1613177034.42 epoch sec; seqNo:15119 2021-02-13T00:43:54Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:43:54Z- handleDepth()-Depth_Keller msg: 1613177034433 epoch sec; seqNo:15241; depth 27.58 2021-02-13T00:43:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177034.44 epoch sec; seqNo:15241; 4059905.69 north; 593045.19 east 2021-02-13T00:43:54Z- AHRS_M2 msg: 1613177034.82 epoch sec; seqNo:15120 2021-02-13T00:43:54Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:43:54Z- handleDepth()-Depth_Keller msg: 1613177034830 epoch sec; seqNo:15242; depth 27.70 2021-02-13T00:43:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:43:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:43:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:43:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:43:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:43:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:43:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:43:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:43:54Z- handleDvl() - RDI_Pathfinder msg: 1613177034.83 epoch sec; seqNo:7298 2021-02-13T00:43:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:43:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.06 2021-02-13T00:43:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177034.86 epoch sec; seqNo:15242; 4059906.06 north; 593045.19 east 2021-02-13T00:43:55Z- AHRS_M2 msg: 1613177035.23 epoch sec; seqNo:15121 2021-02-13T00:43:55Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:43:55Z- handleDepth()-Depth_Keller msg: 1613177035236 epoch sec; seqNo:15243; depth 27.84 2021-02-13T00:43:55Z- handleDvl() - RDI_Pathfinder msg: 1613177035.25 epoch sec; seqNo:7299 2021-02-13T00:43:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.66 , 85.00 , 62.12 , 97.98 2021-02-13T00:43:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.03 , 0.03 2021-02-13T00:43:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177035.27 epoch sec; seqNo:15243; 4059906.44 north; 593045.18 east 2021-02-13T00:43:55Z- AHRS_M2 msg: 1613177035.63 epoch sec; seqNo:15122 2021-02-13T00:43:55Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:43:55Z- handleDepth()-Depth_Keller msg: 1613177035641 epoch sec; seqNo:15244; depth 27.97 2021-02-13T00:43:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177035.65 epoch sec; seqNo:15244; 4059906.44 north; 593045.18 east 2021-02-13T00:43:56Z- AHRS_M2 msg: 1613177036.04 epoch sec; seqNo:15123 2021-02-13T00:43:56Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:43:56Z- handleDepth()-Depth_Keller msg: 1613177036057 epoch sec; seqNo:15245; depth 28.10 2021-02-13T00:43:56Z- handleDvl() - RDI_Pathfinder msg: 1613177036.07 epoch sec; seqNo:7300 2021-02-13T00:43:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 83.92 , 61.30 , 97.98 2021-02-13T00:43:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.12 , -0.05 2021-02-13T00:43:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177036.08 epoch sec; seqNo:15245; 4059906.84 north; 593046.40 east 2021-02-13T00:43:56Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:43:56Z- waiting for 1613177034.21, time is 1613177036.08 2021-02-13T00:43:56Z- LcmTrn::updateTrn() equating pose 1613177036.08 and meas times 1613177036.08 2021-02-13T00:43:56Z- LcmTrn::updateTrn() >>>> pose time 1613177036.08, meas time 1613177036.08 <<<< 2021-02-13T00:43:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177036.08 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:43:56Z- TerrainNav::motionUpdate() - currEstimate = (4059907, 593046.40, 28.10) 2021-02-13T00:43:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:43:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:43:56Z- TNavBF::Projecting Measurements 2021-02-13T00:43:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:43:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:43:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:43:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:43:56Z- TNavFilter::4 good beams. 2021-02-13T00:43:56Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:43:56Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:43:56Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:43:56Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:43:56Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:43:56Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:43:56Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:43:56Z- TNavBF::mmse selected from filter -1 2021-02-13T00:43:56Z- TerrainMapOctree: (vehN, vehE ) = (4059906.84, 593046.40). 2021-02-13T00:43:56Z- TerrainMapOctree: Min Distance = 1803.54. 2021-02-13T00:43:56Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:43:56Z- There is no specific good beam that all particles have in common. 2021-02-13T00:43:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:43:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.41 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:43:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:43:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 11.27 2021-02-13T00:43:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:43:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.129 ExpectedMeasDiff: nan currMeas.covariance: 6.01 2021-02-13T00:43:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:43:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:43:56Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:43:56Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:43:56Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:43:56Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:43:56Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:43:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:43:56Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:43:56Z- Filter 0 No beams used in subcloud update 2021-02-13T00:43:56Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:43:56Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:43:56Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 690.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:43:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177036.08 sec. 2021-02-13T00:43:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:43:56Z- TNavBF::mmse selected from filter -1 2021-02-13T00:43:56Z- variances([0], [2], and psi_berg) = 690.014992 832.992965 0.000000 2021-02-13T00:43:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:43:56Z- LcmTrn::publishEstimates() - MLE : 4059906.34 593067.90 28.10 2021-02-13T00:43:56Z- LcmTrn::publishEstimates() - MMSE : 4059906.84 593046.40 28.10 2021-02-13T00:43:56Z- LcmTrn::publishEstimates() - COVAR: 690.01 832.99 0.94 0.00 2021-02-13T00:43:56Z- AHRS_M2 msg: 1613177036.44 epoch sec; seqNo:15124 2021-02-13T00:43:56Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:43:56Z- handleDepth()-Depth_Keller msg: 1613177036444 epoch sec; seqNo:15246; depth 28.25 2021-02-13T00:43:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177036.45 epoch sec; seqNo:15246; 4059907.21 north; 593046.39 east 2021-02-13T00:43:56Z- AHRS_M2 msg: 1613177036.85 epoch sec; seqNo:15125 2021-02-13T00:43:56Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:43:56Z- handleDepth()-Depth_Keller msg: 1613177036856 epoch sec; seqNo:15247; depth 28.35 2021-02-13T00:43:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177036.89 epoch sec; seqNo:15247; 4059907.59 north; 593046.39 east 2021-02-13T00:43:57Z- AHRS_M2 msg: 1613177037.25 epoch sec; seqNo:15126 2021-02-13T00:43:57Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-13T00:43:57Z- handleDepth()-Depth_Keller msg: 1613177037256 epoch sec; seqNo:15248; depth 28.49 2021-02-13T00:43:57Z- handleDvl() - RDI_Pathfinder msg: 1613177037.27 epoch sec; seqNo:7301 2021-02-13T00:43:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.03 , 83.85 , 61.71 , 97.91 2021-02-13T00:43:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.03 , -0.04 2021-02-13T00:43:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177037.28 epoch sec; seqNo:15248; 4059907.97 north; 593046.39 east 2021-02-13T00:43:57Z- AHRS_M2 msg: 1613177037.65 epoch sec; seqNo:15127 2021-02-13T00:43:57Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-13T00:43:57Z- handleDepth()-Depth_Keller msg: 1613177037656 epoch sec; seqNo:15249; depth 28.64 2021-02-13T00:43:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177037.67 epoch sec; seqNo:15249; 4059908.35 north; 593046.38 east 2021-02-13T00:43:58Z- AHRS_M2 msg: 1613177038.06 epoch sec; seqNo:15128 2021-02-13T00:43:58Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-13T00:43:58Z- handleDepth()-Depth_Keller msg: 1613177038065 epoch sec; seqNo:15250; depth 28.75 2021-02-13T00:43:58Z- handleDvl() - RDI_Pathfinder msg: 1613177038.08 epoch sec; seqNo:7302 2021-02-13T00:43:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.03 , 0.00 , 61.71 , 97.98 2021-02-13T00:43:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.05 , -0.06 2021-02-13T00:43:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177038.09 epoch sec; seqNo:15250; 4059908.73 north; 593046.38 east 2021-02-13T00:43:58Z- AHRS_M2 msg: 1613177038.46 epoch sec; seqNo:15129 2021-02-13T00:43:58Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-13T00:43:58Z- handleDepth()-Depth_Keller msg: 1613177038464 epoch sec; seqNo:15251; depth 28.88 2021-02-13T00:43:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:43:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:43:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:43:58Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:43:58Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:43:58Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:43:58Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:43:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:43:58Z- handleDvl() - RDI_Pathfinder msg: 1613177038.47 epoch sec; seqNo:7303 2021-02-13T00:43:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:43:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.05 , -0.06 2021-02-13T00:43:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177038.48 epoch sec; seqNo:15251; 4059908.74 north; 593047.60 east 2021-02-13T00:43:58Z- AHRS_M2 msg: 1613177038.87 epoch sec; seqNo:15130 2021-02-13T00:43:58Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.19 psi 2021-02-13T00:43:58Z- handleDepth()-Depth_Keller msg: 1613177038869 epoch sec; seqNo:15252; depth 29.01 2021-02-13T00:43:58Z- handleDvl() - RDI_Pathfinder msg: 1613177038.88 epoch sec; seqNo:7304 2021-02-13T00:43:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.21 , 0.00 , 60.49 , 97.98 2021-02-13T00:43:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.10 , -0.04 2021-02-13T00:43:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177038.89 epoch sec; seqNo:15252; 4059909.12 north; 593047.59 east 2021-02-13T00:43:59Z- AHRS_M2 msg: 1613177039.27 epoch sec; seqNo:15131 2021-02-13T00:43:59Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.19 psi 2021-02-13T00:43:59Z- handleDepth()-Depth_Keller msg: 1613177039272 epoch sec; seqNo:15253; depth 29.15 2021-02-13T00:43:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177039.29 epoch sec; seqNo:15253; 4059909.50 north; 593047.59 east 2021-02-13T00:43:59Z- AHRS_M2 msg: 1613177039.67 epoch sec; seqNo:15132 2021-02-13T00:43:59Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.19 psi 2021-02-13T00:43:59Z- handleDepth()-Depth_Keller msg: 1613177039676 epoch sec; seqNo:15254; depth 29.29 2021-02-13T00:43:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:43:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:43:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:43:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:43:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:43:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:43:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:43:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:43:59Z- handleDvl() - RDI_Pathfinder msg: 1613177039.68 epoch sec; seqNo:7305 2021-02-13T00:43:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:43:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.10 , -0.04 2021-02-13T00:43:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177039.69 epoch sec; seqNo:15254; 4059909.88 north; 593047.59 east 2021-02-13T00:44:00Z- AHRS_M2 msg: 1613177040.08 epoch sec; seqNo:15133 2021-02-13T00:44:00Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.19 psi 2021-02-13T00:44:00Z- handleDepth()-Depth_Keller msg: 1613177040085 epoch sec; seqNo:15255; depth 29.42 2021-02-13T00:44:00Z- handleDvl() - RDI_Pathfinder msg: 1613177040.10 epoch sec; seqNo:7306 2021-02-13T00:44:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.39 , 83.92 , 60.49 , 97.98 2021-02-13T00:44:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.09 , -0.01 2021-02-13T00:44:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177040.11 epoch sec; seqNo:15255; 4059910.26 north; 593047.58 east 2021-02-13T00:44:00Z- AHRS_M2 msg: 1613177040.48 epoch sec; seqNo:15134 2021-02-13T00:44:00Z- AHRS_M2 msg: 0.59 phi; -0.26 theta; -0.18 psi 2021-02-13T00:44:00Z- handleDepth()-Depth_Keller msg: 1613177040486 epoch sec; seqNo:15256; depth 29.56 2021-02-13T00:44:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177040.50 epoch sec; seqNo:15256; 4059910.64 north; 593047.58 east 2021-02-13T00:44:00Z- AHRS_M2 msg: 1613177040.88 epoch sec; seqNo:15135 2021-02-13T00:44:00Z- AHRS_M2 msg: 0.58 phi; -0.22 theta; -0.17 psi 2021-02-13T00:44:00Z- handleDepth()-Depth_Keller msg: 1613177040898 epoch sec; seqNo:15257; depth 29.71 2021-02-13T00:44:00Z- handleDvl() - RDI_Pathfinder msg: 1613177040.91 epoch sec; seqNo:7307 2021-02-13T00:44:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:44:00Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:44:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177040.92 epoch sec; seqNo:15257; 4059910.64 north; 593047.58 east 2021-02-13T00:44:01Z- AHRS_M2 msg: 1613177041.29 epoch sec; seqNo:15136 2021-02-13T00:44:01Z- AHRS_M2 msg: 0.57 phi; -0.18 theta; -0.16 psi 2021-02-13T00:44:01Z- handleDepth()-Depth_Keller msg: 1613177041292 epoch sec; seqNo:15258; depth 29.85 2021-02-13T00:44:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177041.30 epoch sec; seqNo:15258; 4059911.03 north; 593048.79 east 2021-02-13T00:44:01Z- LcmTrn::handleMessages() - handling 46 messages took 0.68 ms and 133 calls... 2021-02-13T00:44:01Z- waiting for 1613177039.08, time is 1613177041.30 2021-02-13T00:44:01Z- LcmTrn::updateTrn() >>>> pose time 1613177041.30, meas time 1613177040.91 <<<< 2021-02-13T00:44:01Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177040.91, ping # 0. 2021-02-13T00:44:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177040.91 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:44:01Z- TerrainNav::motionUpdate() - currEstimate = (4059911, 593048.79, 29.85) 2021-02-13T00:44:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:44:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:44:01Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:44:01Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:44:01Z- TNavBF::Projecting Measurements 2021-02-13T00:44:01Z- TNavFilter::Measurement from time = 1613177040.91 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:44:01Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:44:01Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:44:01Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:44:01Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:44:01Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:44:01Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:44:01Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:44:01Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 690.0 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:44:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:44:01Z- TNavBF::mmse selected from filter -1 2021-02-13T00:44:01Z- variances([0], [2], and psi_berg) = 689.948844 832.992934 0.000000 2021-02-13T00:44:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:44:01Z- LcmTrn::publishEstimates() - MLE : 4059910.53 593070.29 29.85 2021-02-13T00:44:01Z- LcmTrn::publishEstimates() - MMSE : 4059911.03 593048.79 29.85 2021-02-13T00:44:01Z- LcmTrn::publishEstimates() - COVAR: 689.95 832.99 1.19 0.00 2021-02-13T00:44:01Z- AHRS_M2 msg: 1613177041.70 epoch sec; seqNo:15137 2021-02-13T00:44:01Z- AHRS_M2 msg: 0.57 phi; -0.14 theta; -0.15 psi 2021-02-13T00:44:01Z- handleDepth()-Depth_Keller msg: 1613177041703 epoch sec; seqNo:15259; depth 29.97 2021-02-13T00:44:01Z- handleDvl() - RDI_Pathfinder msg: 1613177041.71 epoch sec; seqNo:7308 2021-02-13T00:44:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.56 , 0.00 , 0.00 , 0.00 2021-02-13T00:44:01Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:44:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177041.73 epoch sec; seqNo:15259; 4059911.41 north; 593048.79 east 2021-02-13T00:44:02Z- AHRS_M2 msg: 1613177042.10 epoch sec; seqNo:15138 2021-02-13T00:44:02Z- AHRS_M2 msg: 0.56 phi; -0.09 theta; -0.15 psi 2021-02-13T00:44:02Z- handleDepth()-Depth_Keller msg: 1613177042101 epoch sec; seqNo:15260; depth 30.07 2021-02-13T00:44:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:44:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:44:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:44:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:44:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:44:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:44:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:44:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:44:02Z- handleDvl() - RDI_Pathfinder msg: 1613177042.10 epoch sec; seqNo:7309 2021-02-13T00:44:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:44:02Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:44:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177042.12 epoch sec; seqNo:15260; 4059911.79 north; 593048.79 east 2021-02-13T00:44:02Z- AHRS_M2 msg: 1613177042.50 epoch sec; seqNo:15139 2021-02-13T00:44:02Z- AHRS_M2 msg: 0.55 phi; -0.05 theta; -0.15 psi 2021-02-13T00:44:02Z- handleDepth()-Depth_Keller msg: 1613177042505 epoch sec; seqNo:15261; depth 30.16 2021-02-13T00:44:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177042.55 epoch sec; seqNo:15261; 4059912.17 north; 593048.78 east 2021-02-13T00:44:02Z- AHRS_M2 msg: 1613177042.90 epoch sec; seqNo:15140 2021-02-13T00:44:02Z- AHRS_M2 msg: 0.54 phi; -0.01 theta; -0.15 psi 2021-02-13T00:44:02Z- handleDepth()-Depth_Keller msg: 1613177042913 epoch sec; seqNo:15262; depth 30.23 2021-02-13T00:44:02Z- handleDvl() - RDI_Pathfinder msg: 1613177042.92 epoch sec; seqNo:7310 2021-02-13T00:44:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.56 , 0.00 , 65.32 , 68.54 2021-02-13T00:44:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , -0.02 2021-02-13T00:44:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177042.95 epoch sec; seqNo:15262; 4059912.54 north; 593048.78 east 2021-02-13T00:44:03Z- AHRS_M2 msg: 1613177043.31 epoch sec; seqNo:15141 2021-02-13T00:44:03Z- AHRS_M2 msg: 0.54 phi; 0.04 theta; -0.15 psi 2021-02-13T00:44:03Z- handleDepth()-Depth_Keller msg: 1613177043312 epoch sec; seqNo:15263; depth 30.30 2021-02-13T00:44:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177043.32 epoch sec; seqNo:15263; 4059912.54 north; 593048.78 east 2021-02-13T00:44:03Z- AHRS_M2 msg: 1613177043.71 epoch sec; seqNo:15142 2021-02-13T00:44:03Z- AHRS_M2 msg: 0.53 phi; 0.08 theta; -0.15 psi 2021-02-13T00:44:03Z- handleDepth()-Depth_Keller msg: 1613177043717 epoch sec; seqNo:15264; depth 30.36 2021-02-13T00:44:03Z- handleDvl() - RDI_Pathfinder msg: 1613177043.73 epoch sec; seqNo:7311 2021-02-13T00:44:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.17 , 0.00 , 0.00 , 65.62 2021-02-13T00:44:03Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:44:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177043.74 epoch sec; seqNo:15264; 4059912.92 north; 593048.77 east 2021-02-13T00:44:04Z- AHRS_M2 msg: 1613177044.12 epoch sec; seqNo:15143 2021-02-13T00:44:04Z- AHRS_M2 msg: 0.52 phi; 0.11 theta; -0.15 psi 2021-02-13T00:44:04Z- handleDepth()-Depth_Keller msg: 1613177044121 epoch sec; seqNo:15265; depth 30.39 2021-02-13T00:44:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177044.14 epoch sec; seqNo:15265; 4059913.32 north; 593049.99 east 2021-02-13T00:44:04Z- AHRS_M2 msg: 1613177044.54 epoch sec; seqNo:15144 2021-02-13T00:44:04Z- AHRS_M2 msg: 0.52 phi; 0.15 theta; -0.15 psi 2021-02-13T00:44:04Z- handleDepth()-Depth_Keller msg: 1613177044551 epoch sec; seqNo:15266; depth 30.41 2021-02-13T00:44:04Z- handleDvl() - RDI_Pathfinder msg: 1613177044.56 epoch sec; seqNo:7312 2021-02-13T00:44:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.78 , 0.00 , 0.00 , 62.56 2021-02-13T00:44:04Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:44:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177044.58 epoch sec; seqNo:15266; 4059913.69 north; 593049.99 east 2021-02-13T00:44:04Z- AHRS_M2 msg: 1613177044.92 epoch sec; seqNo:15145 2021-02-13T00:44:04Z- AHRS_M2 msg: 0.52 phi; 0.18 theta; -0.14 psi 2021-02-13T00:44:04Z- handleDepth()-Depth_Keller msg: 1613177044928 epoch sec; seqNo:15267; depth 30.39 2021-02-13T00:44:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177044.94 epoch sec; seqNo:15267; 4059914.07 north; 593049.98 east 2021-02-13T00:44:05Z- AHRS_M2 msg: 1613177045.33 epoch sec; seqNo:15146 2021-02-13T00:44:05Z- AHRS_M2 msg: 0.52 phi; 0.21 theta; -0.14 psi 2021-02-13T00:44:05Z- handleDepth()-Depth_Keller msg: 1613177045339 epoch sec; seqNo:15268; depth 30.39 2021-02-13T00:44:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177045.35 epoch sec; seqNo:15268; 4059914.07 north; 593049.98 east 2021-02-13T00:44:05Z- AHRS_M2 msg: 1613177045.74 epoch sec; seqNo:15147 2021-02-13T00:44:05Z- AHRS_M2 msg: 0.52 phi; 0.25 theta; -0.14 psi 2021-02-13T00:44:05Z- handleDepth()-Depth_Keller msg: 1613177045742 epoch sec; seqNo:15269; depth 30.36 2021-02-13T00:44:05Z- handleDvl() - RDI_Pathfinder msg: 1613177045.76 epoch sec; seqNo:7313 2021-02-13T00:44:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 64.19 , 0.00 , 0.00 , 60.92 2021-02-13T00:44:05Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:44:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177045.77 epoch sec; seqNo:15269; 4059914.45 north; 593049.98 east 2021-02-13T00:44:06Z- AHRS_M2 msg: 1613177046.14 epoch sec; seqNo:15148 2021-02-13T00:44:06Z- AHRS_M2 msg: 0.51 phi; 0.27 theta; -0.14 psi 2021-02-13T00:44:06Z- handleDepth()-Depth_Keller msg: 1613177046147 epoch sec; seqNo:15270; depth 30.32 2021-02-13T00:44:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177046.16 epoch sec; seqNo:15270; 4059914.83 north; 593049.97 east 2021-02-13T00:44:06Z- LcmTrn::handleMessages() - handling 42 messages took 0.70 ms and 128 calls... 2021-02-13T00:44:06Z- waiting for 1613177044.30, time is 1613177046.16 2021-02-13T00:44:06Z- LcmTrn::updateTrn() >>>> pose time 1613177046.16, meas time 1613177045.76 <<<< 2021-02-13T00:44:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177045.76 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:44:06Z- TerrainNav::motionUpdate() - currEstimate = (4059915, 593049.97, 30.32) 2021-02-13T00:44:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:44:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:44:06Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:44:06Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:44:06Z- TNavBF::Projecting Measurements 2021-02-13T00:44:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:44:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:44:06Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-13T00:44:06Z- TNavFilter::2 good beams. 2021-02-13T00:44:06Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:44:06Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:44:06Z- TNavBF:: beamIndex[1] = 3 2021-02-13T00:44:06Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:44:06Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:44:06Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-13T00:44:06Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:44:06Z- TNavBF::mmse selected from filter -1 2021-02-13T00:44:06Z- TerrainMapOctree: (vehN, vehE ) = (4059914.52, 593049.88). 2021-02-13T00:44:06Z- TerrainMapOctree: Min Distance = 1795.49. 2021-02-13T00:44:06Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:44:06Z- There is no specific good beam that all particles have in common. 2021-02-13T00:44:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:44:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.142 ExpectedMeasDiff: nan currMeas.covariance: 6.59 2021-02-13T00:44:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:44:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.133 ExpectedMeasDiff: nan currMeas.covariance: 5.94 2021-02-13T00:44:06Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:44:06Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:44:06Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:44:06Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:44:06Z- Filter 0 No beams used in subcloud update 2021-02-13T00:44:06Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:44:06Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:44:06Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 689.9 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:44:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177045.76 sec. 2021-02-13T00:44:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:44:06Z- TNavBF::mmse selected from filter -1 2021-02-13T00:44:06Z- variances([0], [2], and psi_berg) = 689.888435 832.992906 0.000000 2021-02-13T00:44:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:44:06Z- LcmTrn::publishEstimates() - MLE : 4059914.33 593071.47 30.32 2021-02-13T00:44:06Z- LcmTrn::publishEstimates() - MMSE : 4059914.83 593049.97 30.32 2021-02-13T00:44:06Z- LcmTrn::publishEstimates() - COVAR: 689.89 832.99 1.04 0.00 2021-02-13T00:44:06Z- AHRS_M2 msg: 1613177046.55 epoch sec; seqNo:15149 2021-02-13T00:44:06Z- AHRS_M2 msg: 0.51 phi; 0.29 theta; -0.14 psi 2021-02-13T00:44:06Z- handleDepth()-Depth_Keller msg: 1613177046553 epoch sec; seqNo:15271; depth 30.27 2021-02-13T00:44:06Z- handleDvl() - RDI_Pathfinder msg: 1613177046.57 epoch sec; seqNo:7314 2021-02-13T00:44:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 65.42 , 0.00 , 0.00 , 60.51 2021-02-13T00:44:06Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:44:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177046.58 epoch sec; seqNo:15271; 4059915.21 north; 593049.97 east 2021-02-13T00:44:06Z- AHRS_M2 msg: 1613177046.94 epoch sec; seqNo:15150 2021-02-13T00:44:06Z- AHRS_M2 msg: 0.51 phi; 0.30 theta; -0.14 psi 2021-02-13T00:44:06Z- handleDepth()-Depth_Keller msg: 1613177046957 epoch sec; seqNo:15272; depth 30.23 2021-02-13T00:44:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177046.97 epoch sec; seqNo:15272; 4059915.21 north; 593049.97 east 2021-02-13T00:44:07Z- AHRS_M2 msg: 1613177047.35 epoch sec; seqNo:15151 2021-02-13T00:44:07Z- AHRS_M2 msg: 0.51 phi; 0.31 theta; -0.13 psi 2021-02-13T00:44:07Z- handleDepth()-Depth_Keller msg: 1613177047354 epoch sec; seqNo:15273; depth 30.16 2021-02-13T00:44:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177047.37 epoch sec; seqNo:15273; 4059915.60 north; 593051.19 east 2021-02-13T00:44:07Z- AHRS_M2 msg: 1613177047.75 epoch sec; seqNo:15152 2021-02-13T00:44:07Z- AHRS_M2 msg: 0.52 phi; 0.32 theta; -0.13 psi 2021-02-13T00:44:07Z- handleDepth()-Depth_Keller msg: 1613177047761 epoch sec; seqNo:15274; depth 30.09 2021-02-13T00:44:07Z- handleDvl() - RDI_Pathfinder msg: 1613177047.79 epoch sec; seqNo:7315 2021-02-13T00:44:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:44:07Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:44:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177047.80 epoch sec; seqNo:15274; 4059915.98 north; 593051.18 east 2021-02-13T00:44:08Z- AHRS_M2 msg: 1613177048.16 epoch sec; seqNo:15153 2021-02-13T00:44:08Z- AHRS_M2 msg: 0.52 phi; 0.33 theta; -0.13 psi 2021-02-13T00:44:08Z- handleDepth()-Depth_Keller msg: 1613177048169 epoch sec; seqNo:15275; depth 30.01 2021-02-13T00:44:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177048.18 epoch sec; seqNo:15275; 4059916.36 north; 593051.18 east 2021-02-13T00:44:08Z- AHRS_M2 msg: 1613177048.56 epoch sec; seqNo:15154 2021-02-13T00:44:08Z- AHRS_M2 msg: 0.52 phi; 0.34 theta; -0.13 psi 2021-02-13T00:44:08Z- handleDepth()-Depth_Keller msg: 1613177048573 epoch sec; seqNo:15276; depth 29.92 2021-02-13T00:44:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177048.58 epoch sec; seqNo:15276; 4059916.36 north; 593051.18 east 2021-02-13T00:44:08Z- AHRS_M2 msg: 1613177048.96 epoch sec; seqNo:15155 2021-02-13T00:44:08Z- AHRS_M2 msg: 0.52 phi; 0.35 theta; -0.13 psi 2021-02-13T00:44:08Z- handleDepth()-Depth_Keller msg: 1613177048973 epoch sec; seqNo:15277; depth 29.84 2021-02-13T00:44:08Z- handleDvl() - RDI_Pathfinder msg: 1613177048.99 epoch sec; seqNo:7316 2021-02-13T00:44:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 80.67 , 97.98 , 60.90 2021-02-13T00:44:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , 0.01 2021-02-13T00:44:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177049.00 epoch sec; seqNo:15277; 4059916.74 north; 593051.17 east 2021-02-13T00:44:09Z- AHRS_M2 msg: 1613177049.37 epoch sec; seqNo:15156 2021-02-13T00:44:09Z- AHRS_M2 msg: 0.52 phi; 0.35 theta; -0.13 psi 2021-02-13T00:44:09Z- handleDepth()-Depth_Keller msg: 1613177049372 epoch sec; seqNo:15278; depth 29.75 2021-02-13T00:44:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177049.38 epoch sec; seqNo:15278; 4059917.12 north; 593051.17 east 2021-02-13T00:44:09Z- AHRS_M2 msg: 1613177049.77 epoch sec; seqNo:15157 2021-02-13T00:44:09Z- AHRS_M2 msg: 0.52 phi; 0.35 theta; -0.13 psi 2021-02-13T00:44:09Z- handleDepth()-Depth_Keller msg: 1613177049783 epoch sec; seqNo:15279; depth 29.66 2021-02-13T00:44:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177049.79 epoch sec; seqNo:15279; 4059917.50 north; 593051.17 east 2021-02-13T00:44:10Z- AHRS_M2 msg: 1613177050.18 epoch sec; seqNo:15158 2021-02-13T00:44:10Z- AHRS_M2 msg: 0.53 phi; 0.36 theta; -0.12 psi 2021-02-13T00:44:10Z- handleDepth()-Depth_Keller msg: 1613177050185 epoch sec; seqNo:15280; depth 29.55 2021-02-13T00:44:10Z- handleDvl() - RDI_Pathfinder msg: 1613177050.20 epoch sec; seqNo:7317 2021-02-13T00:44:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 149.90 , 79.59 , 97.98 , 60.49 2021-02-13T00:44:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.17 , 0.08 2021-02-13T00:44:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177050.21 epoch sec; seqNo:15280; 4059917.51 north; 593052.39 east 2021-02-13T00:44:10Z- AHRS_M2 msg: 1613177050.58 epoch sec; seqNo:15159 2021-02-13T00:44:10Z- AHRS_M2 msg: 0.53 phi; 0.36 theta; -0.12 psi 2021-02-13T00:44:10Z- handleDepth()-Depth_Keller msg: 1613177050584 epoch sec; seqNo:15281; depth 29.44 2021-02-13T00:44:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177050.60 epoch sec; seqNo:15281; 4059917.89 north; 593052.38 east 2021-02-13T00:44:10Z- AHRS_M2 msg: 1613177050.98 epoch sec; seqNo:15160 2021-02-13T00:44:10Z- AHRS_M2 msg: 0.53 phi; 0.36 theta; -0.12 psi 2021-02-13T00:44:10Z- handleDepth()-Depth_Keller msg: 1613177050993 epoch sec; seqNo:15282; depth 29.34 2021-02-13T00:44:11Z- handleDvl() - RDI_Pathfinder msg: 1613177051.01 epoch sec; seqNo:7318 2021-02-13T00:44:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 79.73 , 97.31 , 60.49 2021-02-13T00:44:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , 0.02 2021-02-13T00:44:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177051.03 epoch sec; seqNo:15282; 4059918.27 north; 593052.38 east 2021-02-13T00:44:11Z- AHRS_M2 msg: 1613177051.39 epoch sec; seqNo:15161 2021-02-13T00:44:11Z- AHRS_M2 msg: 0.54 phi; 0.36 theta; -0.12 psi 2021-02-13T00:44:11Z- handleDepth()-Depth_Keller msg: 1613177051392 epoch sec; seqNo:15283; depth 29.22 2021-02-13T00:44:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177051.40 epoch sec; seqNo:15283; 4059918.27 north; 593052.38 east 2021-02-13T00:44:11Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-13T00:44:11Z- waiting for 1613177049.16, time is 1613177051.40 2021-02-13T00:44:11Z- LcmTrn::updateTrn() >>>> pose time 1613177051.40, meas time 1613177051.01 <<<< 2021-02-13T00:44:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177051.01 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:44:11Z- TerrainNav::motionUpdate() - currEstimate = (4059918, 593052.38, 29.22) 2021-02-13T00:44:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:44:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:44:11Z- TNavBF::Projecting Measurements 2021-02-13T00:44:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:44:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:44:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:44:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:44:11Z- TNavFilter::4 good beams. 2021-02-13T00:44:11Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:44:11Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:44:11Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:44:11Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:44:11Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:44:11Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:44:11Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:44:11Z- TNavBF::mmse selected from filter -1 2021-02-13T00:44:11Z- TerrainMapOctree: (vehN, vehE ) = (4059918.01, 593052.20). 2021-02-13T00:44:11Z- TerrainMapOctree: Min Distance = 1791.35. 2021-02-13T00:44:11Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:44:11Z- There is no specific good beam that all particles have in common. 2021-02-13T00:44:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:44:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.120 ExpectedMeasDiff: nan currMeas.covariance: 7.36 2021-02-13T00:44:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:44:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 10.17 2021-02-13T00:44:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:44:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 15.15 2021-02-13T00:44:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:44:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 5.85 2021-02-13T00:44:11Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:44:11Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:44:11Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:44:11Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:44:11Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:44:11Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:44:11Z- Filter 0 No beams used in subcloud update 2021-02-13T00:44:11Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:44:11Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:44:11Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 689.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:44:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177051.01 sec. 2021-02-13T00:44:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:44:11Z- TNavBF::mmse selected from filter -1 2021-02-13T00:44:11Z- variances([0], [2], and psi_berg) = 689.826848 832.992877 0.000000 2021-02-13T00:44:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:44:11Z- LcmTrn::publishEstimates() - MLE : 4059917.77 593073.88 29.22 2021-02-13T00:44:11Z- LcmTrn::publishEstimates() - MMSE : 4059918.27 593052.38 29.22 2021-02-13T00:44:11Z- LcmTrn::publishEstimates() - COVAR: 689.83 832.99 0.85 0.00 2021-02-13T00:44:11Z- AHRS_M2 msg: 1613177051.79 epoch sec; seqNo:15162 2021-02-13T00:44:11Z- AHRS_M2 msg: 0.55 phi; 0.36 theta; -0.11 psi 2021-02-13T00:44:11Z- handleDepth()-Depth_Keller msg: 1613177051798 epoch sec; seqNo:15284; depth 29.12 2021-02-13T00:44:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177051.81 epoch sec; seqNo:15284; 4059918.65 north; 593052.37 east 2021-02-13T00:44:12Z- AHRS_M2 msg: 1613177052.19 epoch sec; seqNo:15163 2021-02-13T00:44:12Z- AHRS_M2 msg: 0.55 phi; 0.36 theta; -0.11 psi 2021-02-13T00:44:12Z- handleDepth()-Depth_Keller msg: 1613177052210 epoch sec; seqNo:15285; depth 29.02 2021-02-13T00:44:12Z- handleDvl() - RDI_Pathfinder msg: 1613177052.22 epoch sec; seqNo:7319 2021-02-13T00:44:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 0.00 , 97.98 , 61.30 2021-02-13T00:44:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , -0.04 2021-02-13T00:44:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177052.23 epoch sec; seqNo:15285; 4059919.02 north; 593052.37 east 2021-02-13T00:44:12Z- AHRS_M2 msg: 1613177052.60 epoch sec; seqNo:15164 2021-02-13T00:44:12Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.11 psi 2021-02-13T00:44:12Z- handleDepth()-Depth_Keller msg: 1613177052604 epoch sec; seqNo:15286; depth 28.90 2021-02-13T00:44:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177052.62 epoch sec; seqNo:15286; 4059919.40 north; 593052.37 east 2021-02-13T00:44:13Z- AHRS_M2 msg: 1613177053.00 epoch sec; seqNo:15165 2021-02-13T00:44:13Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.11 psi 2021-02-13T00:44:13Z- handleDepth()-Depth_Keller msg: 1613177053008 epoch sec; seqNo:15287; depth 28.80 2021-02-13T00:44:13Z- handleDvl() - RDI_Pathfinder msg: 1613177053.02 epoch sec; seqNo:7320 2021-02-13T00:44:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 0.00 , 97.98 , 60.49 2021-02-13T00:44:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.03 , -0.09 2021-02-13T00:44:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177053.03 epoch sec; seqNo:15287; 4059919.42 north; 593053.59 east 2021-02-13T00:44:13Z- AHRS_M2 msg: 1613177053.41 epoch sec; seqNo:15166 2021-02-13T00:44:13Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.11 psi 2021-02-13T00:44:13Z- handleDepth()-Depth_Keller msg: 1613177053413 epoch sec; seqNo:15288; depth 28.68 2021-02-13T00:44:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177053.42 epoch sec; seqNo:15288; 4059919.80 north; 593053.58 east 2021-02-13T00:44:13Z- AHRS_M2 msg: 1613177053.82 epoch sec; seqNo:15167 2021-02-13T00:44:13Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.11 psi 2021-02-13T00:44:13Z- handleDepth()-Depth_Keller msg: 1613177053833 epoch sec; seqNo:15289; depth 28.56 2021-02-13T00:44:13Z- handleDvl() - RDI_Pathfinder msg: 1613177053.85 epoch sec; seqNo:7321 2021-02-13T00:44:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 78.06 , 97.98 , 61.71 2021-02-13T00:44:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , -0.11 2021-02-13T00:44:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177053.86 epoch sec; seqNo:15289; 4059920.17 north; 593053.58 east 2021-02-13T00:44:14Z- AHRS_M2 msg: 1613177054.22 epoch sec; seqNo:15168 2021-02-13T00:44:14Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.11 psi 2021-02-13T00:44:14Z- handleDepth()-Depth_Keller msg: 1613177054220 epoch sec; seqNo:15290; depth 28.43 2021-02-13T00:44:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177054.23 epoch sec; seqNo:15290; 4059920.17 north; 593053.58 east 2021-02-13T00:44:14Z- AHRS_M2 msg: 1613177054.62 epoch sec; seqNo:15169 2021-02-13T00:44:14Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.11 psi 2021-02-13T00:44:14Z- handleDepth()-Depth_Keller msg: 1613177054629 epoch sec; seqNo:15291; depth 28.32 2021-02-13T00:44:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177054.64 epoch sec; seqNo:15291; 4059920.55 north; 593053.57 east 2021-02-13T00:44:15Z- AHRS_M2 msg: 1613177055.03 epoch sec; seqNo:15170 2021-02-13T00:44:15Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.11 psi 2021-02-13T00:44:15Z- handleDepth()-Depth_Keller msg: 1613177055037 epoch sec; seqNo:15292; depth 28.21 2021-02-13T00:44:15Z- handleDvl() - RDI_Pathfinder msg: 1613177055.05 epoch sec; seqNo:7322 2021-02-13T00:44:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 81.75 , 97.98 , 60.90 2021-02-13T00:44:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.08 , -0.08 2021-02-13T00:44:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177055.06 epoch sec; seqNo:15292; 4059920.93 north; 593053.57 east 2021-02-13T00:44:15Z- AHRS_M2 msg: 1613177055.43 epoch sec; seqNo:15171 2021-02-13T00:44:15Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.11 psi 2021-02-13T00:44:15Z- handleDepth()-Depth_Keller msg: 1613177055432 epoch sec; seqNo:15293; depth 28.09 2021-02-13T00:44:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177055.45 epoch sec; seqNo:15293; 4059921.31 north; 593053.57 east 2021-02-13T00:44:15Z- AHRS_M2 msg: 1613177055.84 epoch sec; seqNo:15172 2021-02-13T00:44:15Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.12 psi 2021-02-13T00:44:15Z- handleDepth()-Depth_Keller msg: 1613177055841 epoch sec; seqNo:15294; depth 27.98 2021-02-13T00:44:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177055.89 epoch sec; seqNo:15294; 4059921.31 north; 593053.57 east 2021-02-13T00:44:16Z- AHRS_M2 msg: 1613177056.24 epoch sec; seqNo:15173 2021-02-13T00:44:16Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.12 psi 2021-02-13T00:44:16Z- handleDepth()-Depth_Keller msg: 1613177056241 epoch sec; seqNo:15295; depth 27.85 2021-02-13T00:44:16Z- handleDvl() - RDI_Pathfinder msg: 1613177056.25 epoch sec; seqNo:7323 2021-02-13T00:44:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 80.67 , 94.74 , 60.90 2021-02-13T00:44:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.10 , -0.04 2021-02-13T00:44:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177056.26 epoch sec; seqNo:15295; 4059921.70 north; 593054.78 east 2021-02-13T00:44:16Z- AHRS_M2 msg: 1613177056.64 epoch sec; seqNo:15174 2021-02-13T00:44:16Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.12 psi 2021-02-13T00:44:16Z- LcmTrn::handleMessages() - handling 42 messages took 0.60 ms and 130 calls... 2021-02-13T00:44:16Z- waiting for 1613177054.40, time is 1613177056.64 2021-02-13T00:44:16Z- LcmTrn::updateTrn() >>>> pose time 1613177056.64, meas time 1613177056.25 <<<< 2021-02-13T00:44:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177056.25 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:44:16Z- TerrainNav::motionUpdate() - currEstimate = (4059922, 593054.78, 27.85) 2021-02-13T00:44:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:44:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:44:16Z- TNavBF::Projecting Measurements 2021-02-13T00:44:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:44:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:44:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:44:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:44:16Z- TNavFilter::4 good beams. 2021-02-13T00:44:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:44:16Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:44:16Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:44:16Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:44:16Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:44:16Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:44:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:44:16Z- TNavBF::mmse selected from filter -1 2021-02-13T00:44:16Z- TerrainMapOctree: (vehN, vehE ) = (4059921.45, 593054.60). 2021-02-13T00:44:16Z- TerrainMapOctree: Min Distance = 1787.18. 2021-02-13T00:44:16Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:44:16Z- There is no specific good beam that all particles have in common. 2021-02-13T00:44:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:44:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 7.91 2021-02-13T00:44:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:44:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 10.41 2021-02-13T00:44:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:44:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 14.36 2021-02-13T00:44:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:44:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 5.93 2021-02-13T00:44:16Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:44:16Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:44:16Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:44:16Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:44:16Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:44:16Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:44:16Z- Filter 0 No beams used in subcloud update 2021-02-13T00:44:16Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:44:16Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:44:16Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 689.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:44:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177056.25 sec. 2021-02-13T00:44:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:44:16Z- TNavBF::mmse selected from filter -1 2021-02-13T00:44:16Z- variances([0], [2], and psi_berg) = 689.765431 832.992848 0.000000 2021-02-13T00:44:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:44:16Z- LcmTrn::publishEstimates() - MLE : 4059921.20 593076.28 27.85 2021-02-13T00:44:16Z- LcmTrn::publishEstimates() - MMSE : 4059921.70 593054.78 27.85 2021-02-13T00:44:16Z- LcmTrn::publishEstimates() - COVAR: 689.77 832.99 0.77 0.00 2021-02-13T00:44:16Z- handleDepth()-Depth_Keller msg: 1613177056647 epoch sec; seqNo:15296; depth 27.73 2021-02-13T00:44:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177056.66 epoch sec; seqNo:15296; 4059922.08 north; 593054.78 east 2021-02-13T00:44:17Z- AHRS_M2 msg: 1613177057.04 epoch sec; seqNo:15175 2021-02-13T00:44:17Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.13 psi 2021-02-13T00:44:17Z- handleDepth()-Depth_Keller msg: 1613177057049 epoch sec; seqNo:15297; depth 27.62 2021-02-13T00:44:17Z- handleDvl() - RDI_Pathfinder msg: 1613177057.07 epoch sec; seqNo:7324 2021-02-13T00:44:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 79.59 , 97.98 , 62.12 2021-02-13T00:44:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.08 , 0.00 2021-02-13T00:44:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177057.08 epoch sec; seqNo:15297; 4059922.46 north; 593054.77 east 2021-02-13T00:44:17Z- AHRS_M2 msg: 1613177057.45 epoch sec; seqNo:15176 2021-02-13T00:44:17Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.13 psi 2021-02-13T00:44:17Z- handleDepth()-Depth_Keller msg: 1613177057453 epoch sec; seqNo:15298; depth 27.50 2021-02-13T00:44:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177057.47 epoch sec; seqNo:15298; 4059922.46 north; 593054.77 east 2021-02-13T00:44:17Z- AHRS_M2 msg: 1613177057.87 epoch sec; seqNo:15177 2021-02-13T00:44:17Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.14 psi 2021-02-13T00:44:17Z- handleDepth()-Depth_Keller msg: 1613177057904 epoch sec; seqNo:15299; depth 27.39 2021-02-13T00:44:17Z- handleDvl() - RDI_Pathfinder msg: 1613177057.91 epoch sec; seqNo:7325 2021-02-13T00:44:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 81.75 , 97.98 , 61.30 2021-02-13T00:44:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.02 2021-02-13T00:44:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177057.92 epoch sec; seqNo:15299; 4059922.84 north; 593054.77 east 2021-02-13T00:44:18Z- AHRS_M2 msg: 1613177058.26 epoch sec; seqNo:15178 2021-02-13T00:44:18Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.14 psi 2021-02-13T00:44:18Z- handleDepth()-Depth_Keller msg: 1613177058265 epoch sec; seqNo:15300; depth 27.26 2021-02-13T00:44:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177058.28 epoch sec; seqNo:15300; 4059923.22 north; 593054.77 east 2021-02-13T00:44:18Z- AHRS_M2 msg: 1613177058.66 epoch sec; seqNo:15179 2021-02-13T00:44:18Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.14 psi 2021-02-13T00:44:18Z- handleDepth()-Depth_Keller msg: 1613177058664 epoch sec; seqNo:15301; depth 27.16 2021-02-13T00:44:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177058.68 epoch sec; seqNo:15301; 4059923.22 north; 593054.77 east 2021-02-13T00:44:19Z- AHRS_M2 msg: 1613177059.06 epoch sec; seqNo:15180 2021-02-13T00:44:19Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-13T00:44:19Z- handleDepth()-Depth_Keller msg: 1613177059069 epoch sec; seqNo:15302; depth 27.03 2021-02-13T00:44:19Z- handleDvl() - RDI_Pathfinder msg: 1613177059.08 epoch sec; seqNo:7326 2021-02-13T00:44:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 0.00 , 93.65 , 63.35 2021-02-13T00:44:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , 0.02 2021-02-13T00:44:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177059.10 epoch sec; seqNo:15302; 4059923.61 north; 593055.98 east 2021-02-13T00:44:19Z- AHRS_M2 msg: 1613177059.47 epoch sec; seqNo:15181 2021-02-13T00:44:19Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-13T00:44:19Z- handleDepth()-Depth_Keller msg: 1613177059477 epoch sec; seqNo:15303; depth 26.92 2021-02-13T00:44:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177059.49 epoch sec; seqNo:15303; 4059923.99 north; 593055.98 east 2021-02-13T00:44:19Z- AHRS_M2 msg: 1613177059.88 epoch sec; seqNo:15182 2021-02-13T00:44:19Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-13T00:44:19Z- handleDepth()-Depth_Keller msg: 1613177059889 epoch sec; seqNo:15304; depth 26.83 2021-02-13T00:44:19Z- handleDvl() - RDI_Pathfinder msg: 1613177059.90 epoch sec; seqNo:7327 2021-02-13T00:44:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 0.00 , 94.74 , 64.16 2021-02-13T00:44:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , 0.02 2021-02-13T00:44:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177059.91 epoch sec; seqNo:15304; 4059924.37 north; 593055.97 east 2021-02-13T00:44:20Z- AHRS_M2 msg: 1613177060.28 epoch sec; seqNo:15183 2021-02-13T00:44:20Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-13T00:44:20Z- handleDepth()-Depth_Keller msg: 1613177060294 epoch sec; seqNo:15305; depth 26.70 2021-02-13T00:44:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177060.31 epoch sec; seqNo:15305; 4059924.37 north; 593055.97 east 2021-02-13T00:44:20Z- AHRS_M2 msg: 1613177060.68 epoch sec; seqNo:15184 2021-02-13T00:44:20Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-13T00:44:20Z- handleDepth()-Depth_Keller msg: 1613177060689 epoch sec; seqNo:15306; depth 26.60 2021-02-13T00:44:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177060.70 epoch sec; seqNo:15306; 4059924.75 north; 593055.97 east 2021-02-13T00:44:21Z- AHRS_M2 msg: 1613177061.08 epoch sec; seqNo:15185 2021-02-13T00:44:21Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-13T00:44:21Z- handleDepth()-Depth_Keller msg: 1613177061093 epoch sec; seqNo:15307; depth 26.48 2021-02-13T00:44:21Z- handleDvl() - RDI_Pathfinder msg: 1613177061.11 epoch sec; seqNo:7328 2021-02-13T00:44:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 82.84 , 97.98 , 62.53 2021-02-13T00:44:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.04 2021-02-13T00:44:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177061.12 epoch sec; seqNo:15307; 4059925.13 north; 593055.97 east 2021-02-13T00:44:21Z- AHRS_M2 msg: 1613177061.49 epoch sec; seqNo:15186 2021-02-13T00:44:21Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-13T00:44:21Z- handleDepth()-Depth_Keller msg: 1613177061492 epoch sec; seqNo:15308; depth 26.38 2021-02-13T00:44:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177061.51 epoch sec; seqNo:15308; 4059925.51 north; 593055.96 east 2021-02-13T00:44:21Z- LcmTrn::handleMessages() - handling 43 messages took 0.63 ms and 131 calls... 2021-02-13T00:44:21Z- waiting for 1613177059.64, time is 1613177061.51 2021-02-13T00:44:21Z- LcmTrn::updateTrn() >>>> pose time 1613177061.51, meas time 1613177061.11 <<<< 2021-02-13T00:44:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177061.11 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:44:21Z- TerrainNav::motionUpdate() - currEstimate = (4059926, 593055.96, 26.38) 2021-02-13T00:44:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:44:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:44:21Z- TNavBF::Projecting Measurements 2021-02-13T00:44:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:44:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:44:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:44:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:44:21Z- TNavFilter::4 good beams. 2021-02-13T00:44:21Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:44:21Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:44:21Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:44:21Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:44:21Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:44:21Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:44:21Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:44:21Z- TNavBF::mmse selected from filter -1 2021-02-13T00:44:21Z- TerrainMapOctree: (vehN, vehE ) = (4059925.19, 593055.86). 2021-02-13T00:44:21Z- TerrainMapOctree: Min Distance = 1783.56. 2021-02-13T00:44:21Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:44:21Z- There is no specific good beam that all particles have in common. 2021-02-13T00:44:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:44:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 7.91 2021-02-13T00:44:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:44:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 10.98 2021-02-13T00:44:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:44:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:44:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:44:21Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 6.26 2021-02-13T00:44:21Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:44:21Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:44:21Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:44:21Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:44:21Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:44:21Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:44:21Z- Filter 0 No beams used in subcloud update 2021-02-13T00:44:21Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:44:21Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:44:21Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 689.7 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:44:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177061.11 sec. 2021-02-13T00:44:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:44:21Z- TNavBF::mmse selected from filter -1 2021-02-13T00:44:21Z- variances([0], [2], and psi_berg) = 689.705080 832.992819 0.000000 2021-02-13T00:44:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:44:21Z- LcmTrn::publishEstimates() - MLE : 4059925.01 593077.46 26.38 2021-02-13T00:44:21Z- LcmTrn::publishEstimates() - MMSE : 4059925.51 593055.96 26.38 2021-02-13T00:44:21Z- LcmTrn::publishEstimates() - COVAR: 689.71 832.99 0.72 0.00 2021-02-13T00:44:21Z- AHRS_M2 msg: 1613177061.90 epoch sec; seqNo:15187 2021-02-13T00:44:21Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.14 psi 2021-02-13T00:44:21Z- handleDepth()-Depth_Keller msg: 1613177061905 epoch sec; seqNo:15309; depth 26.27 2021-02-13T00:44:21Z- handleDvl() - RDI_Pathfinder msg: 1613177061.93 epoch sec; seqNo:7329 2021-02-13T00:44:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 0.00 , 97.98 , 62.12 2021-02-13T00:44:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.02 , 0.04 2021-02-13T00:44:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177061.94 epoch sec; seqNo:15309; 4059925.90 north; 593057.18 east 2021-02-13T00:44:22Z- AHRS_M2 msg: 1613177062.30 epoch sec; seqNo:15188 2021-02-13T00:44:22Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-13T00:44:22Z- handleDepth()-Depth_Keller msg: 1613177062300 epoch sec; seqNo:15310; depth 26.15 2021-02-13T00:44:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177062.32 epoch sec; seqNo:15310; 4059925.90 north; 593057.18 east 2021-02-13T00:44:22Z- AHRS_M2 msg: 1613177062.71 epoch sec; seqNo:15189 2021-02-13T00:44:22Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-13T00:44:22Z- handleDepth()-Depth_Keller msg: 1613177062717 epoch sec; seqNo:15311; depth 26.06 2021-02-13T00:44:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177062.72 epoch sec; seqNo:15311; 4059926.28 north; 593057.17 east 2021-02-13T00:44:23Z- AHRS_M2 msg: 1613177063.11 epoch sec; seqNo:15190 2021-02-13T00:44:23Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-13T00:44:23Z- handleDepth()-Depth_Keller msg: 1613177063119 epoch sec; seqNo:15312; depth 25.95 2021-02-13T00:44:23Z- handleDvl() - RDI_Pathfinder msg: 1613177063.13 epoch sec; seqNo:7330 2021-02-13T00:44:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 144.49 , 97.98 , 63.76 2021-02-13T00:44:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.74 , 0.03 2021-02-13T00:44:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177063.15 epoch sec; seqNo:15312; 4059926.28 north; 593057.17 east 2021-02-13T00:44:23Z- AHRS_M2 msg: 1613177063.51 epoch sec; seqNo:15191 2021-02-13T00:44:23Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-13T00:44:23Z- handleDepth()-Depth_Keller msg: 1613177063512 epoch sec; seqNo:15313; depth 25.84 2021-02-13T00:44:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177063.52 epoch sec; seqNo:15313; 4059926.67 north; 593058.39 east 2021-02-13T00:44:23Z- AHRS_M2 msg: 1613177063.93 epoch sec; seqNo:15192 2021-02-13T00:44:23Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-13T00:44:23Z- handleDepth()-Depth_Keller msg: 1613177063938 epoch sec; seqNo:15314; depth 25.74 2021-02-13T00:44:23Z- handleDvl() - RDI_Pathfinder msg: 1613177063.97 epoch sec; seqNo:7331 2021-02-13T00:44:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 84.53 , 97.91 , 62.53 2021-02-13T00:44:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , -0.03 2021-02-13T00:44:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177063.98 epoch sec; seqNo:15314; 4059926.67 north; 593058.39 east 2021-02-13T00:44:24Z- AHRS_M2 msg: 1613177064.32 epoch sec; seqNo:15193 2021-02-13T00:44:24Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-13T00:44:24Z- handleDepth()-Depth_Keller msg: 1613177064333 epoch sec; seqNo:15315; depth 25.61 2021-02-13T00:44:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177064.34 epoch sec; seqNo:15315; 4059927.05 north; 593058.39 east 2021-02-13T00:44:24Z- AHRS_M2 msg: 1613177064.73 epoch sec; seqNo:15194 2021-02-13T00:44:24Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-13T00:44:24Z- handleDepth()-Depth_Keller msg: 1613177064733 epoch sec; seqNo:15316; depth 25.52 2021-02-13T00:44:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177064.74 epoch sec; seqNo:15316; 4059927.43 north; 593058.38 east 2021-02-13T00:44:25Z- AHRS_M2 msg: 1613177065.13 epoch sec; seqNo:15195 2021-02-13T00:44:25Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-13T00:44:25Z- handleDepth()-Depth_Keller msg: 1613177065140 epoch sec; seqNo:15317; depth 25.40 2021-02-13T00:44:25Z- handleDvl() - RDI_Pathfinder msg: 1613177065.16 epoch sec; seqNo:7332 2021-02-13T00:44:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 85.00 , 97.98 , 64.57 2021-02-13T00:44:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , -0.05 2021-02-13T00:44:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177065.17 epoch sec; seqNo:15317; 4059927.80 north; 593058.38 east 2021-02-13T00:44:25Z- AHRS_M2 msg: 1613177065.54 epoch sec; seqNo:15196 2021-02-13T00:44:25Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.17 psi 2021-02-13T00:44:25Z- handleDepth()-Depth_Keller msg: 1613177065540 epoch sec; seqNo:15318; depth 25.30 2021-02-13T00:44:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177065.55 epoch sec; seqNo:15318; 4059927.82 north; 593059.60 east 2021-02-13T00:44:25Z- AHRS_M2 msg: 1613177065.97 epoch sec; seqNo:15197 2021-02-13T00:44:25Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.17 psi 2021-02-13T00:44:25Z- handleDepth()-Depth_Keller msg: 1613177065973 epoch sec; seqNo:15319; depth 25.19 2021-02-13T00:44:25Z- handleDvl() - RDI_Pathfinder msg: 1613177065.99 epoch sec; seqNo:7333 2021-02-13T00:44:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 86.08 , 97.98 , 63.76 2021-02-13T00:44:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.06 , -0.07 2021-02-13T00:44:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177066.00 epoch sec; seqNo:15319; 4059928.20 north; 593059.60 east 2021-02-13T00:44:26Z- AHRS_M2 msg: 1613177066.34 epoch sec; seqNo:15198 2021-02-13T00:44:26Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.18 psi 2021-02-13T00:44:26Z- handleDepth()-Depth_Keller msg: 1613177066357 epoch sec; seqNo:15320; depth 25.08 2021-02-13T00:44:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177066.37 epoch sec; seqNo:15320; 4059928.58 north; 593059.59 east 2021-02-13T00:44:26Z- AHRS_M2 msg: 1613177066.75 epoch sec; seqNo:15199 2021-02-13T00:44:26Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.18 psi 2021-02-13T00:44:26Z- handleDepth()-Depth_Keller msg: 1613177066751 epoch sec; seqNo:15321; depth 24.97 2021-02-13T00:44:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177066.76 epoch sec; seqNo:15321; 4059928.58 north; 593059.59 east 2021-02-13T00:44:26Z- LcmTrn::handleMessages() - handling 44 messages took 0.73 ms and 130 calls... 2021-02-13T00:44:26Z- waiting for 1613177064.51, time is 1613177066.76 2021-02-13T00:44:26Z- LcmTrn::updateTrn() >>>> pose time 1613177066.76, meas time 1613177065.99 <<<< 2021-02-13T00:44:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177065.99 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:44:26Z- TerrainNav::motionUpdate() - currEstimate = (4059929, 593059.59, 24.97) 2021-02-13T00:44:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:44:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:44:26Z- TNavBF::Projecting Measurements 2021-02-13T00:44:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:44:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:44:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:44:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:44:26Z- TNavFilter::4 good beams. 2021-02-13T00:44:26Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:44:26Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:44:26Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:44:26Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:44:26Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:44:26Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:44:26Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:44:26Z- TNavBF::mmse selected from filter -1 2021-02-13T00:44:26Z- TerrainMapOctree: (vehN, vehE ) = (4059928.13, 593059.06). 2021-02-13T00:44:26Z- TerrainMapOctree: Min Distance = 1779.24. 2021-02-13T00:44:26Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:44:26Z- There is no specific good beam that all particles have in common. 2021-02-13T00:44:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:44:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:44:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:44:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 11.86 2021-02-13T00:44:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:44:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:44:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:44:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.77 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 6.50 2021-02-13T00:44:26Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:44:26Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:44:26Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:44:26Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:44:26Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:44:26Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:44:26Z- Filter 0 No beams used in subcloud update 2021-02-13T00:44:26Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:44:26Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:44:26Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 689.7 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:44:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177065.99 sec. 2021-02-13T00:44:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:44:26Z- TNavBF::mmse selected from filter -1 2021-02-13T00:44:26Z- variances([0], [2], and psi_berg) = 689.635940 832.992787 0.000000 2021-02-13T00:44:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:44:26Z- LcmTrn::publishEstimates() - MLE : 4059928.08 593081.09 24.97 2021-02-13T00:44:26Z- LcmTrn::publishEstimates() - MMSE : 4059928.58 593059.59 24.97 2021-02-13T00:44:26Z- LcmTrn::publishEstimates() - COVAR: 689.64 832.99 0.74 0.00 2021-02-13T00:44:27Z- AHRS_M2 msg: 1613177067.15 epoch sec; seqNo:15200 2021-02-13T00:44:27Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.18 psi 2021-02-13T00:44:27Z- handleDepth()-Depth_Keller msg: 1613177067159 epoch sec; seqNo:15322; depth 24.86 2021-02-13T00:44:27Z- handleDvl() - RDI_Pathfinder msg: 1613177067.17 epoch sec; seqNo:7334 2021-02-13T00:44:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 88.04 , 97.78 , 65.39 2021-02-13T00:44:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.14 , -0.04 2021-02-13T00:44:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177067.20 epoch sec; seqNo:15322; 4059928.95 north; 593059.59 east 2021-02-13T00:44:27Z- AHRS_M2 msg: 1613177067.57 epoch sec; seqNo:15201 2021-02-13T00:44:27Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.19 psi 2021-02-13T00:44:27Z- handleDepth()-Depth_Keller msg: 1613177067572 epoch sec; seqNo:15323; depth 24.76 2021-02-13T00:44:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177067.59 epoch sec; seqNo:15323; 4059929.33 north; 593059.58 east 2021-02-13T00:44:27Z- AHRS_M2 msg: 1613177067.96 epoch sec; seqNo:15202 2021-02-13T00:44:27Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.19 psi 2021-02-13T00:44:27Z- handleDepth()-Depth_Keller msg: 1613177067981 epoch sec; seqNo:15324; depth 24.65 2021-02-13T00:44:27Z- handleDvl() - RDI_Pathfinder msg: 1613177067.99 epoch sec; seqNo:7335 2021-02-13T00:44:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 0.00 , 98.39 , 64.16 2021-02-13T00:44:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.14 , 0.02 2021-02-13T00:44:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177068.01 epoch sec; seqNo:15324; 4059929.71 north; 593059.58 east 2021-02-13T00:44:28Z- AHRS_M2 msg: 1613177068.36 epoch sec; seqNo:15203 2021-02-13T00:44:28Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-13T00:44:28Z- handleDepth()-Depth_Keller msg: 1613177068368 epoch sec; seqNo:15325; depth 24.54 2021-02-13T00:44:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177068.38 epoch sec; seqNo:15325; 4059930.09 north; 593059.57 east 2021-02-13T00:44:28Z- AHRS_M2 msg: 1613177068.77 epoch sec; seqNo:15204 2021-02-13T00:44:28Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.18 psi 2021-02-13T00:44:28Z- handleDepth()-Depth_Keller msg: 1613177068786 epoch sec; seqNo:15326; depth 24.45 2021-02-13T00:44:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177068.82 epoch sec; seqNo:15326; 4059930.48 north; 593060.79 east 2021-02-13T00:44:29Z- AHRS_M2 msg: 1613177069.17 epoch sec; seqNo:15205 2021-02-13T00:44:29Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.18 psi 2021-02-13T00:44:29Z- handleDepth()-Depth_Keller msg: 1613177069177 epoch sec; seqNo:15327; depth 24.33 2021-02-13T00:44:29Z- handleDvl() - RDI_Pathfinder msg: 1613177069.19 epoch sec; seqNo:7336 2021-02-13T00:44:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 0.00 , 97.98 , 65.80 2021-02-13T00:44:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.07 , 0.02 2021-02-13T00:44:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177069.20 epoch sec; seqNo:15327; 4059930.86 north; 593060.79 east 2021-02-13T00:44:29Z- AHRS_M2 msg: 1613177069.58 epoch sec; seqNo:15206 2021-02-13T00:44:29Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-13T00:44:29Z- handleDepth()-Depth_Keller msg: 1613177069580 epoch sec; seqNo:15328; depth 24.23 2021-02-13T00:44:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177069.60 epoch sec; seqNo:15328; 4059930.86 north; 593060.79 east 2021-02-13T00:44:29Z- AHRS_M2 msg: 1613177069.98 epoch sec; seqNo:15207 2021-02-13T00:44:29Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-13T00:44:29Z- handleDepth()-Depth_Keller msg: 1613177069986 epoch sec; seqNo:15329; depth 24.14 2021-02-13T00:44:30Z- handleDvl() - RDI_Pathfinder msg: 1613177070.00 epoch sec; seqNo:7337 2021-02-13T00:44:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 0.00 , 97.98 , 65.80 2021-02-13T00:44:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , 0.03 2021-02-13T00:44:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177070.03 epoch sec; seqNo:15329; 4059931.24 north; 593060.78 east 2021-02-13T00:44:30Z- AHRS_M2 msg: 1613177070.38 epoch sec; seqNo:15208 2021-02-13T00:44:30Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.16 psi 2021-02-13T00:44:30Z- handleDepth()-Depth_Keller msg: 1613177070389 epoch sec; seqNo:15330; depth 24.02 2021-02-13T00:44:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177070.40 epoch sec; seqNo:15330; 4059931.62 north; 593060.78 east 2021-02-13T00:44:30Z- AHRS_M2 msg: 1613177070.81 epoch sec; seqNo:15209 2021-02-13T00:44:30Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-13T00:44:30Z- handleDepth()-Depth_Keller msg: 1613177070820 epoch sec; seqNo:15331; depth 23.93 2021-02-13T00:44:30Z- handleDvl() - RDI_Pathfinder msg: 1613177070.83 epoch sec; seqNo:7338 2021-02-13T00:44:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.93 , 0.00 , 97.98 , 66.62 2021-02-13T00:44:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , 0.01 2021-02-13T00:44:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177070.86 epoch sec; seqNo:15331; 4059932.00 north; 593060.77 east 2021-02-13T00:44:31Z- AHRS_M2 msg: 1613177071.19 epoch sec; seqNo:15210 2021-02-13T00:44:31Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-13T00:44:31Z- handleDepth()-Depth_Keller msg: 1613177071201 epoch sec; seqNo:15332; depth 23.82 2021-02-13T00:44:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177071.22 epoch sec; seqNo:15332; 4059932.01 north; 593062.00 east 2021-02-13T00:44:31Z- AHRS_M2 msg: 1613177071.60 epoch sec; seqNo:15211 2021-02-13T00:44:31Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:31Z- handleDepth()-Depth_Keller msg: 1613177071605 epoch sec; seqNo:15333; depth 23.74 2021-02-13T00:44:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177071.61 epoch sec; seqNo:15333; 4059932.39 north; 593061.99 east 2021-02-13T00:44:31Z- LcmTrn::handleMessages() - handling 41 messages took 0.71 ms and 127 calls... 2021-02-13T00:44:31Z- waiting for 1613177069.76, time is 1613177071.61 2021-02-13T00:44:31Z- LcmTrn::updateTrn() >>>> pose time 1613177071.61, meas time 1613177070.83 <<<< 2021-02-13T00:44:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177070.83 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:44:31Z- TerrainNav::motionUpdate() - currEstimate = (4059932, 593061.99, 23.74) 2021-02-13T00:44:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:44:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:44:31Z- TNavBF::Projecting Measurements 2021-02-13T00:44:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:44:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:44:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:44:31Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:44:31Z- TNavFilter::3 good beams. 2021-02-13T00:44:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:44:31Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:44:31Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:44:31Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:44:31Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:44:31Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:44:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:44:31Z- TNavBF::mmse selected from filter -1 2021-02-13T00:44:31Z- TerrainMapOctree: (vehN, vehE ) = (4059931.78, 593061.60). 2021-02-13T00:44:31Z- TerrainMapOctree: Min Distance = 1774.82. 2021-02-13T00:44:31Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:44:31Z- There is no specific good beam that all particles have in common. 2021-02-13T00:44:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:44:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 8.28 2021-02-13T00:44:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:44:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:44:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:44:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 7.10 2021-02-13T00:44:32Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:44:32Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:44:32Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:44:32Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:44:32Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:44:32Z- Filter 0 No beams used in subcloud update 2021-02-13T00:44:32Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:44:32Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:44:32Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 689.6 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:44:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177070.83 sec. 2021-02-13T00:44:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:44:32Z- TNavBF::mmse selected from filter -1 2021-02-13T00:44:32Z- variances([0], [2], and psi_berg) = 689.568113 832.992755 0.000000 2021-02-13T00:44:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:44:32Z- LcmTrn::publishEstimates() - MLE : 4059931.89 593083.49 23.74 2021-02-13T00:44:32Z- LcmTrn::publishEstimates() - MMSE : 4059932.39 593061.99 23.74 2021-02-13T00:44:32Z- LcmTrn::publishEstimates() - COVAR: 689.57 832.99 0.78 0.00 2021-02-13T00:44:32Z- AHRS_M2 msg: 1613177072.00 epoch sec; seqNo:15212 2021-02-13T00:44:32Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:32Z- handleDepth()-Depth_Keller msg: 1613177072007 epoch sec; seqNo:15334; depth 23.61 2021-02-13T00:44:32Z- handleDvl() - RDI_Pathfinder msg: 1613177072.02 epoch sec; seqNo:7339 2021-02-13T00:44:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 87.16 , 97.98 , 66.21 2021-02-13T00:44:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , -0.01 2021-02-13T00:44:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177072.03 epoch sec; seqNo:15334; 4059932.77 north; 593061.99 east 2021-02-13T00:44:32Z- AHRS_M2 msg: 1613177072.40 epoch sec; seqNo:15213 2021-02-13T00:44:32Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:32Z- handleDepth()-Depth_Keller msg: 1613177072411 epoch sec; seqNo:15335; depth 23.52 2021-02-13T00:44:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177072.42 epoch sec; seqNo:15335; 4059933.15 north; 593061.98 east 2021-02-13T00:44:32Z- AHRS_M2 msg: 1613177072.81 epoch sec; seqNo:15214 2021-02-13T00:44:32Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:32Z- handleDepth()-Depth_Keller msg: 1613177072812 epoch sec; seqNo:15336; depth 23.40 2021-02-13T00:44:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177072.83 epoch sec; seqNo:15336; 4059933.15 north; 593061.98 east 2021-02-13T00:44:33Z- AHRS_M2 msg: 1613177073.21 epoch sec; seqNo:15215 2021-02-13T00:44:33Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:33Z- handleDepth()-Depth_Keller msg: 1613177073216 epoch sec; seqNo:15337; depth 23.32 2021-02-13T00:44:33Z- handleDvl() - RDI_Pathfinder msg: 1613177073.23 epoch sec; seqNo:7340 2021-02-13T00:44:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 87.43 , 98.39 , 66.62 2021-02-13T00:44:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , -0.03 2021-02-13T00:44:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177073.25 epoch sec; seqNo:15337; 4059933.53 north; 593061.98 east 2021-02-13T00:44:33Z- AHRS_M2 msg: 1613177073.62 epoch sec; seqNo:15216 2021-02-13T00:44:33Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:33Z- handleDepth()-Depth_Keller msg: 1613177073620 epoch sec; seqNo:15338; depth 23.20 2021-02-13T00:44:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177073.63 epoch sec; seqNo:15338; 4059933.92 north; 593063.20 east 2021-02-13T00:44:34Z- AHRS_M2 msg: 1613177074.02 epoch sec; seqNo:15217 2021-02-13T00:44:34Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:34Z- handleDepth()-Depth_Keller msg: 1613177074025 epoch sec; seqNo:15339; depth 23.11 2021-02-13T00:44:34Z- handleDvl() - RDI_Pathfinder msg: 1613177074.04 epoch sec; seqNo:7341 2021-02-13T00:44:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 0.00 , 98.39 , 150.71 2021-02-13T00:44:34Z- handleDvl() - RDI_Pathfinder msg: velocities -1.08 , 1.93 , -1.11 2021-02-13T00:44:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177074.05 epoch sec; seqNo:15339; 4059932.78 north; 593063.21 east 2021-02-13T00:44:34Z- AHRS_M2 msg: 1613177074.42 epoch sec; seqNo:15218 2021-02-13T00:44:34Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:34Z- handleDepth()-Depth_Keller msg: 1613177074430 epoch sec; seqNo:15340; depth 22.98 2021-02-13T00:44:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177074.44 epoch sec; seqNo:15340; 4059932.02 north; 593063.22 east 2021-02-13T00:44:34Z- AHRS_M2 msg: 1613177074.83 epoch sec; seqNo:15219 2021-02-13T00:44:34Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:34Z- handleDepth()-Depth_Keller msg: 1613177074837 epoch sec; seqNo:15341; depth 22.90 2021-02-13T00:44:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177074.85 epoch sec; seqNo:15341; 4059931.27 north; 593063.22 east 2021-02-13T00:44:35Z- AHRS_M2 msg: 1613177075.23 epoch sec; seqNo:15220 2021-02-13T00:44:35Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:35Z- handleDepth()-Depth_Keller msg: 1613177075242 epoch sec; seqNo:15342; depth 22.77 2021-02-13T00:44:35Z- handleDvl() - RDI_Pathfinder msg: 1613177075.25 epoch sec; seqNo:7342 2021-02-13T00:44:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.97 , 86.89 , 98.79 , 66.41 2021-02-13T00:44:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.00 2021-02-13T00:44:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177075.26 epoch sec; seqNo:15342; 4059931.27 north; 593063.22 east 2021-02-13T00:44:35Z- AHRS_M2 msg: 1613177075.64 epoch sec; seqNo:15221 2021-02-13T00:44:35Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:35Z- handleDepth()-Depth_Keller msg: 1613177075645 epoch sec; seqNo:15343; depth 22.67 2021-02-13T00:44:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177075.66 epoch sec; seqNo:15343; 4059931.65 north; 593063.22 east 2021-02-13T00:44:36Z- AHRS_M2 msg: 1613177076.04 epoch sec; seqNo:15222 2021-02-13T00:44:36Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:36Z- handleDepth()-Depth_Keller msg: 1613177076053 epoch sec; seqNo:15344; depth 22.56 2021-02-13T00:44:36Z- handleDvl() - RDI_Pathfinder msg: 1613177076.07 epoch sec; seqNo:7343 2021-02-13T00:44:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 87.43 , 98.39 , 65.80 2021-02-13T00:44:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , 0.01 2021-02-13T00:44:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177076.08 epoch sec; seqNo:15344; 4059932.04 north; 593064.44 east 2021-02-13T00:44:36Z- AHRS_M2 msg: 1613177076.44 epoch sec; seqNo:15223 2021-02-13T00:44:36Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:36Z- handleDepth()-Depth_Keller msg: 1613177076451 epoch sec; seqNo:15345; depth 22.46 2021-02-13T00:44:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177076.46 epoch sec; seqNo:15345; 4059932.42 north; 593064.43 east 2021-02-13T00:44:36Z- AHRS_M2 msg: 1613177076.87 epoch sec; seqNo:15224 2021-02-13T00:44:36Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:36Z- handleDepth()-Depth_Keller msg: 1613177076869 epoch sec; seqNo:15346; depth 22.35 2021-02-13T00:44:36Z- handleDvl() - RDI_Pathfinder msg: 1613177076.89 epoch sec; seqNo:7344 2021-02-13T00:44:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 87.43 , 98.39 , 67.43 2021-02-13T00:44:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , 0.01 2021-02-13T00:44:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177076.90 epoch sec; seqNo:15346; 4059932.80 north; 593064.43 east 2021-02-13T00:44:37Z- LcmTrn::handleMessages() - handling 45 messages took 0.61 ms and 133 calls... 2021-02-13T00:44:37Z- waiting for 1613177074.61, time is 1613177076.90 2021-02-13T00:44:37Z- LcmTrn::updateTrn() equating pose 1613177076.90 and meas times 1613177076.90 2021-02-13T00:44:37Z- LcmTrn::updateTrn() >>>> pose time 1613177076.90, meas time 1613177076.90 <<<< 2021-02-13T00:44:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177076.90 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:44:37Z- TerrainNav::motionUpdate() - currEstimate = (4059933, 593064.43, 22.35) 2021-02-13T00:44:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:44:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:44:37Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177076.90 sec.; No relevant navigation data available 2021-02-13T00:44:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:44:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:44:37Z- variances([0], [2], and psi_berg) = 689.530018 832.992737 0.000000 2021-02-13T00:44:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:44:37Z- LcmTrn::publishEstimates() - MLE : 4059932.30 593085.93 22.35 2021-02-13T00:44:37Z- LcmTrn::publishEstimates() - MMSE : 4059932.80 593064.43 22.35 2021-02-13T00:44:37Z- LcmTrn::publishEstimates() - COVAR: 689.53 832.99 0.91 0.00 2021-02-13T00:44:37Z- AHRS_M2 msg: 1613177077.25 epoch sec; seqNo:15225 2021-02-13T00:44:37Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:37Z- handleDepth()-Depth_Keller msg: 1613177077265 epoch sec; seqNo:15347; depth 22.23 2021-02-13T00:44:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177077.28 epoch sec; seqNo:15347; 4059932.80 north; 593064.43 east 2021-02-13T00:44:37Z- AHRS_M2 msg: 1613177077.65 epoch sec; seqNo:15226 2021-02-13T00:44:37Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:37Z- handleDepth()-Depth_Keller msg: 1613177077665 epoch sec; seqNo:15348; depth 22.14 2021-02-13T00:44:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177077.67 epoch sec; seqNo:15348; 4059933.17 north; 593064.42 east 2021-02-13T00:44:38Z- AHRS_M2 msg: 1613177078.06 epoch sec; seqNo:15227 2021-02-13T00:44:38Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:38Z- handleDepth()-Depth_Keller msg: 1613177078067 epoch sec; seqNo:15349; depth 22.02 2021-02-13T00:44:38Z- handleDvl() - RDI_Pathfinder msg: 1613177078.08 epoch sec; seqNo:7345 2021-02-13T00:44:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 118.33 , 97.78 , 68.25 2021-02-13T00:44:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.22 , -0.08 2021-02-13T00:44:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177078.09 epoch sec; seqNo:15349; 4059933.55 north; 593064.42 east 2021-02-13T00:44:38Z- AHRS_M2 msg: 1613177078.46 epoch sec; seqNo:15228 2021-02-13T00:44:38Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:38Z- handleDepth()-Depth_Keller msg: 1613177078478 epoch sec; seqNo:15350; depth 21.93 2021-02-13T00:44:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177078.49 epoch sec; seqNo:15350; 4059933.57 north; 593065.64 east 2021-02-13T00:44:38Z- AHRS_M2 msg: 1613177078.87 epoch sec; seqNo:15229 2021-02-13T00:44:38Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:38Z- handleDepth()-Depth_Keller msg: 1613177078872 epoch sec; seqNo:15351; depth 21.80 2021-02-13T00:44:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177078.88 epoch sec; seqNo:15351; 4059933.95 north; 593065.64 east 2021-02-13T00:44:39Z- AHRS_M2 msg: 1613177079.27 epoch sec; seqNo:15230 2021-02-13T00:44:39Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:39Z- handleDepth()-Depth_Keller msg: 1613177079282 epoch sec; seqNo:15352; depth 21.69 2021-02-13T00:44:39Z- handleDvl() - RDI_Pathfinder msg: 1613177079.29 epoch sec; seqNo:7346 2021-02-13T00:44:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:44:39Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:44:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177079.32 epoch sec; seqNo:15352; 4059933.95 north; 593065.64 east 2021-02-13T00:44:39Z- AHRS_M2 msg: 1613177079.68 epoch sec; seqNo:15231 2021-02-13T00:44:39Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:39Z- handleDepth()-Depth_Keller msg: 1613177079687 epoch sec; seqNo:15353; depth 21.59 2021-02-13T00:44:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177079.70 epoch sec; seqNo:15353; 4059934.32 north; 593065.63 east 2021-02-13T00:44:40Z- AHRS_M2 msg: 1613177080.08 epoch sec; seqNo:15232 2021-02-13T00:44:40Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:40Z- handleDepth()-Depth_Keller msg: 1613177080087 epoch sec; seqNo:15354; depth 21.48 2021-02-13T00:44:40Z- handleDvl() - RDI_Pathfinder msg: 1613177080.11 epoch sec; seqNo:7347 2021-02-13T00:44:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.04 , 88.85 , 99.67 , 67.03 2021-02-13T00:44:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , -0.01 2021-02-13T00:44:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177080.13 epoch sec; seqNo:15354; 4059934.70 north; 593065.63 east 2021-02-13T00:44:40Z- AHRS_M2 msg: 1613177080.48 epoch sec; seqNo:15233 2021-02-13T00:44:40Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:40Z- handleDepth()-Depth_Keller msg: 1613177080488 epoch sec; seqNo:15355; depth 21.36 2021-02-13T00:44:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177080.50 epoch sec; seqNo:15355; 4059934.72 north; 593066.85 east 2021-02-13T00:44:40Z- AHRS_M2 msg: 1613177080.89 epoch sec; seqNo:15234 2021-02-13T00:44:40Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:40Z- handleDepth()-Depth_Keller msg: 1613177080892 epoch sec; seqNo:15356; depth 21.26 2021-02-13T00:44:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177080.90 epoch sec; seqNo:15356; 4059935.10 north; 593066.84 east 2021-02-13T00:44:41Z- AHRS_M2 msg: 1613177081.29 epoch sec; seqNo:15235 2021-02-13T00:44:41Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:44:41Z- handleDepth()-Depth_Keller msg: 1613177081296 epoch sec; seqNo:15357; depth 21.16 2021-02-13T00:44:41Z- handleDvl() - RDI_Pathfinder msg: 1613177081.31 epoch sec; seqNo:7348 2021-02-13T00:44:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 89.80 , 99.54 , 78.85 2021-02-13T00:44:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.02 2021-02-13T00:44:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177081.32 epoch sec; seqNo:15357; 4059935.47 north; 593066.84 east 2021-02-13T00:44:41Z- AHRS_M2 msg: 1613177081.70 epoch sec; seqNo:15236 2021-02-13T00:44:41Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-13T00:44:41Z- handleDepth()-Depth_Keller msg: 1613177081700 epoch sec; seqNo:15358; depth 21.03 2021-02-13T00:44:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177081.71 epoch sec; seqNo:15358; 4059935.47 north; 593066.84 east 2021-02-13T00:44:42Z- LcmTrn::handleMessages() - handling 40 messages took 0.65 ms and 127 calls... 2021-02-13T00:44:42Z- waiting for 1613177079.90, time is 1613177081.71 2021-02-13T00:44:42Z- LcmTrn::updateTrn() >>>> pose time 1613177081.71, meas time 1613177081.31 <<<< 2021-02-13T00:44:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177081.31 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:44:42Z- TerrainNav::motionUpdate() - currEstimate = (4059935, 593066.84, 21.03) 2021-02-13T00:44:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:44:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:44:42Z- TNavBF::Projecting Measurements 2021-02-13T00:44:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:44:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:44:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:44:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:44:42Z- TNavFilter::4 good beams. 2021-02-13T00:44:42Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:44:42Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:44:42Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:44:42Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:44:42Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:44:42Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:44:42Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:44:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:44:42Z- TerrainMapOctree: (vehN, vehE ) = (4059935.25, 593066.64). 2021-02-13T00:44:42Z- TerrainMapOctree: Min Distance = 1768.87. 2021-02-13T00:44:42Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:44:42Z- There is no specific good beam that all particles have in common. 2021-02-13T00:44:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:44:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 9.95 2021-02-13T00:44:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:44:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 12.90 2021-02-13T00:44:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:44:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 15.85 2021-02-13T00:44:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:44:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 9.95 2021-02-13T00:44:42Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:44:42Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:44:42Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:44:42Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:44:42Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:44:42Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:44:42Z- Filter 0 No beams used in subcloud update 2021-02-13T00:44:42Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:44:42Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:44:42Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 689.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:44:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177081.31 sec. 2021-02-13T00:44:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:44:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:44:42Z- variances([0], [2], and psi_berg) = 689.472754 832.992710 0.000000 2021-02-13T00:44:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:44:42Z- LcmTrn::publishEstimates() - MLE : 4059934.97 593087.34 21.03 2021-02-13T00:44:42Z- LcmTrn::publishEstimates() - MMSE : 4059935.47 593066.84 21.03 2021-02-13T00:44:42Z- LcmTrn::publishEstimates() - COVAR: 689.47 832.99 0.83 0.00 2021-02-13T00:44:42Z- AHRS_M2 msg: 1613177082.10 epoch sec; seqNo:15237 2021-02-13T00:44:42Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-13T00:44:42Z- handleDepth()-Depth_Keller msg: 1613177082104 epoch sec; seqNo:15359; depth 20.92 2021-02-13T00:44:42Z- handleDvl() - RDI_Pathfinder msg: 1613177082.12 epoch sec; seqNo:7349 2021-02-13T00:44:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 0.00 , 99.26 , 166.20 2021-02-13T00:44:42Z- handleDvl() - RDI_Pathfinder msg: velocities -0.66 , 1.49 , -0.80 2021-02-13T00:44:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177082.13 epoch sec; seqNo:15359; 4059935.10 north; 593066.84 east 2021-02-13T00:44:42Z- AHRS_M2 msg: 1613177082.50 epoch sec; seqNo:15238 2021-02-13T00:44:42Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-13T00:44:42Z- handleDepth()-Depth_Keller msg: 1613177082517 epoch sec; seqNo:15360; depth 20.82 2021-02-13T00:44:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177082.53 epoch sec; seqNo:15360; 4059934.34 north; 593066.85 east 2021-02-13T00:44:42Z- AHRS_M2 msg: 1613177082.91 epoch sec; seqNo:15239 2021-02-13T00:44:42Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-13T00:44:42Z- handleDepth()-Depth_Keller msg: 1613177082912 epoch sec; seqNo:15361; depth 20.72 2021-02-13T00:44:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177082.92 epoch sec; seqNo:15361; 4059933.59 north; 593068.08 east 2021-02-13T00:44:43Z- AHRS_M2 msg: 1613177083.31 epoch sec; seqNo:15240 2021-02-13T00:44:43Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-13T00:44:43Z- handleDepth()-Depth_Keller msg: 1613177083316 epoch sec; seqNo:15362; depth 20.60 2021-02-13T00:44:43Z- handleDvl() - RDI_Pathfinder msg: 1613177083.33 epoch sec; seqNo:7350 2021-02-13T00:44:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.90 , 91.29 , 100.21 , 77.90 2021-02-13T00:44:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , 0.04 2021-02-13T00:44:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177083.34 epoch sec; seqNo:15362; 4059933.97 north; 593068.08 east 2021-02-13T00:44:43Z- AHRS_M2 msg: 1613177083.71 epoch sec; seqNo:15241 2021-02-13T00:44:43Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-13T00:44:43Z- handleDepth()-Depth_Keller msg: 1613177083723 epoch sec; seqNo:15363; depth 20.50 2021-02-13T00:44:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177083.73 epoch sec; seqNo:15363; 4059934.35 north; 593068.07 east 2021-02-13T00:44:44Z- AHRS_M2 msg: 1613177084.12 epoch sec; seqNo:15242 2021-02-13T00:44:44Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-13T00:44:44Z- handleDepth()-Depth_Keller msg: 1613177084129 epoch sec; seqNo:15364; depth 20.40 2021-02-13T00:44:44Z- handleDvl() - RDI_Pathfinder msg: 1613177084.15 epoch sec; seqNo:7351 2021-02-13T00:44:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.79 , 98.05 , 100.48 , 69.48 2021-02-13T00:44:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , 0.02 2021-02-13T00:44:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177084.16 epoch sec; seqNo:15364; 4059934.73 north; 593068.07 east 2021-02-13T00:44:44Z- AHRS_M2 msg: 1613177084.52 epoch sec; seqNo:15243 2021-02-13T00:44:44Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-13T00:44:44Z- handleDepth()-Depth_Keller msg: 1613177084533 epoch sec; seqNo:15365; depth 20.31 2021-02-13T00:44:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177084.55 epoch sec; seqNo:15365; 4059934.73 north; 593068.07 east 2021-02-13T00:44:44Z- AHRS_M2 msg: 1613177084.93 epoch sec; seqNo:15244 2021-02-13T00:44:44Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-13T00:44:44Z- handleDepth()-Depth_Keller msg: 1613177084931 epoch sec; seqNo:15366; depth 20.20 2021-02-13T00:44:44Z- handleDvl() - RDI_Pathfinder msg: 1613177084.94 epoch sec; seqNo:7352 2021-02-13T00:44:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.12 , 100.55 , 69.89 2021-02-13T00:44:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , -0.01 2021-02-13T00:44:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177084.95 epoch sec; seqNo:15366; 4059935.11 north; 593068.06 east 2021-02-13T00:44:45Z- AHRS_M2 msg: 1613177085.33 epoch sec; seqNo:15245 2021-02-13T00:44:45Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-13T00:44:45Z- handleDepth()-Depth_Keller msg: 1613177085349 epoch sec; seqNo:15367; depth 20.10 2021-02-13T00:44:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177085.36 epoch sec; seqNo:15367; 4059935.50 north; 593069.28 east 2021-02-13T00:44:45Z- AHRS_M2 msg: 1613177085.74 epoch sec; seqNo:15246 2021-02-13T00:44:45Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-13T00:44:45Z- handleDepth()-Depth_Keller msg: 1613177085740 epoch sec; seqNo:15368; depth 20.00 2021-02-13T00:44:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177085.75 epoch sec; seqNo:15368; 4059935.88 north; 593069.28 east 2021-02-13T00:44:46Z- AHRS_M2 msg: 1613177086.14 epoch sec; seqNo:15247 2021-02-13T00:44:46Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-13T00:44:46Z- handleDepth()-Depth_Keller msg: 1613177086147 epoch sec; seqNo:15369; depth 19.90 2021-02-13T00:44:46Z- handleDvl() - RDI_Pathfinder msg: 1613177086.16 epoch sec; seqNo:7353 2021-02-13T00:44:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 97.78 , 101.43 , 78.31 2021-02-13T00:44:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , -0.05 2021-02-13T00:44:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177086.17 epoch sec; seqNo:15369; 4059935.88 north; 593069.28 east 2021-02-13T00:44:46Z- AHRS_M2 msg: 1613177086.55 epoch sec; seqNo:15248 2021-02-13T00:44:46Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-13T00:44:46Z- handleDepth()-Depth_Keller msg: 1613177086549 epoch sec; seqNo:15370; depth 19.78 2021-02-13T00:44:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177086.56 epoch sec; seqNo:15370; 4059936.26 north; 593069.27 east 2021-02-13T00:44:46Z- AHRS_M2 msg: 1613177086.95 epoch sec; seqNo:15249 2021-02-13T00:44:46Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-13T00:44:46Z- handleDepth()-Depth_Keller msg: 1613177086962 epoch sec; seqNo:15371; depth 19.71 2021-02-13T00:44:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177086.97 epoch sec; seqNo:15371; 4059936.64 north; 593069.27 east 2021-02-13T00:44:47Z- LcmTrn::handleMessages() - handling 44 messages took 0.62 ms and 132 calls... 2021-02-13T00:44:47Z- waiting for 1613177084.71, time is 1613177086.97 2021-02-13T00:44:47Z- LcmTrn::updateTrn() >>>> pose time 1613177086.97, meas time 1613177086.16 <<<< 2021-02-13T00:44:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177086.16 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:44:47Z- TerrainNav::motionUpdate() - currEstimate = (4059937, 593069.27, 19.71) 2021-02-13T00:44:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:44:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:44:47Z- TNavBF::Projecting Measurements 2021-02-13T00:44:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:44:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:44:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:44:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:44:47Z- TNavFilter::4 good beams. 2021-02-13T00:44:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:44:47Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:44:47Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:44:47Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:44:47Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:44:47Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:44:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:44:47Z- TNavBF::mmse selected from filter -1 2021-02-13T00:44:47Z- TerrainMapOctree: (vehN, vehE ) = (4059936.46, 593068.90). 2021-02-13T00:44:47Z- TerrainMapOctree: Min Distance = 1766.47. 2021-02-13T00:44:47Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:44:47Z- There is no specific good beam that all particles have in common. 2021-02-13T00:44:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:44:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 9.15 2021-02-13T00:44:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:44:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-13T00:44:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:44:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 16.46 2021-02-13T00:44:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:44:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.79 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 9.81 2021-02-13T00:44:47Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:44:47Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:44:47Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:44:47Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:44:47Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:44:47Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:44:47Z- Filter 0 No beams used in subcloud update 2021-02-13T00:44:47Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:44:47Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:44:47Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 689.4 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:44:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177086.16 sec. 2021-02-13T00:44:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:44:47Z- TNavBF::mmse selected from filter -1 2021-02-13T00:44:47Z- variances([0], [2], and psi_berg) = 689.420627 832.992685 0.000000 2021-02-13T00:44:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:44:47Z- LcmTrn::publishEstimates() - MLE : 4059936.14 593089.77 19.71 2021-02-13T00:44:47Z- LcmTrn::publishEstimates() - MMSE : 4059936.64 593069.27 19.71 2021-02-13T00:44:47Z- LcmTrn::publishEstimates() - COVAR: 689.42 832.99 0.76 0.00 2021-02-13T00:44:47Z- AHRS_M2 msg: 1613177087.35 epoch sec; seqNo:15250 2021-02-13T00:44:47Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-13T00:44:47Z- handleDepth()-Depth_Keller msg: 1613177087361 epoch sec; seqNo:15372; depth 19.60 2021-02-13T00:44:47Z- handleDvl() - RDI_Pathfinder msg: 1613177087.37 epoch sec; seqNo:7354 2021-02-13T00:44:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 97.91 , 101.56 , 78.44 2021-02-13T00:44:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , -0.06 2021-02-13T00:44:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177087.39 epoch sec; seqNo:15372; 4059937.02 north; 593069.26 east 2021-02-13T00:44:47Z- handleDepth()-Depth_Keller msg: 1613177087777 epoch sec; seqNo:15373; depth 19.49 2021-02-13T00:44:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177087.79 epoch sec; seqNo:15373; 4059937.03 north; 593070.49 east 2021-02-13T00:44:48Z- AHRS_M2 msg: 1613177088.16 epoch sec; seqNo:15251 2021-02-13T00:44:48Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-13T00:44:48Z- handleDepth()-Depth_Keller msg: 1613177088178 epoch sec; seqNo:15374; depth 19.39 2021-02-13T00:44:48Z- handleDvl() - RDI_Pathfinder msg: 1613177088.19 epoch sec; seqNo:7355 2021-02-13T00:44:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 101.56 , 69.07 2021-02-13T00:44:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , -0.03 2021-02-13T00:44:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177088.20 epoch sec; seqNo:15374; 4059937.41 north; 593070.48 east 2021-02-13T00:44:48Z- AHRS_M2 msg: 1613177088.56 epoch sec; seqNo:15252 2021-02-13T00:44:48Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-13T00:44:48Z- handleDepth()-Depth_Keller msg: 1613177088573 epoch sec; seqNo:15375; depth 19.29 2021-02-13T00:44:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177088.59 epoch sec; seqNo:15375; 4059937.79 north; 593070.48 east 2021-02-13T00:44:48Z- AHRS_M2 msg: 1613177088.97 epoch sec; seqNo:15253 2021-02-13T00:44:48Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-13T00:44:48Z- handleDepth()-Depth_Keller msg: 1613177088972 epoch sec; seqNo:15376; depth 19.20 2021-02-13T00:44:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177088.98 epoch sec; seqNo:15376; 4059938.17 north; 593070.47 east 2021-02-13T00:44:49Z- AHRS_M2 msg: 1613177089.37 epoch sec; seqNo:15254 2021-02-13T00:44:49Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-13T00:44:49Z- handleDepth()-Depth_Keller msg: 1613177089376 epoch sec; seqNo:15377; depth 19.10 2021-02-13T00:44:49Z- handleDvl() - RDI_Pathfinder msg: 1613177089.39 epoch sec; seqNo:7356 2021-02-13T00:44:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 101.70 , 70.70 2021-02-13T00:44:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.00 , -0.00 2021-02-13T00:44:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177089.40 epoch sec; seqNo:15377; 4059938.17 north; 593070.47 east 2021-02-13T00:44:49Z- AHRS_M2 msg: 1613177089.78 epoch sec; seqNo:15255 2021-02-13T00:44:49Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-13T00:44:49Z- handleDepth()-Depth_Keller msg: 1613177089781 epoch sec; seqNo:15378; depth 19.00 2021-02-13T00:44:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177089.79 epoch sec; seqNo:15378; 4059938.54 north; 593070.47 east 2021-02-13T00:44:50Z- AHRS_M2 msg: 1613177090.18 epoch sec; seqNo:15256 2021-02-13T00:44:50Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-13T00:44:50Z- handleDepth()-Depth_Keller msg: 1613177090193 epoch sec; seqNo:15379; depth 18.90 2021-02-13T00:44:50Z- handleDvl() - RDI_Pathfinder msg: 1613177090.21 epoch sec; seqNo:7357 2021-02-13T00:44:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 93.11 , 102.85 , 71.52 2021-02-13T00:44:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.00 , 0.01 2021-02-13T00:44:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177090.23 epoch sec; seqNo:15379; 4059938.94 north; 593071.69 east 2021-02-13T00:44:50Z- AHRS_M2 msg: 1613177090.59 epoch sec; seqNo:15257 2021-02-13T00:44:50Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-13T00:44:50Z- handleDepth()-Depth_Keller msg: 1613177090590 epoch sec; seqNo:15380; depth 18.79 2021-02-13T00:44:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177090.60 epoch sec; seqNo:15380; 4059939.32 north; 593071.68 east 2021-02-13T00:44:50Z- AHRS_M2 msg: 1613177090.99 epoch sec; seqNo:15258 2021-02-13T00:44:50Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-13T00:44:51Z- handleDepth()-Depth_Keller msg: 1613177091012 epoch sec; seqNo:15381; depth 18.68 2021-02-13T00:44:51Z- handleDvl() - RDI_Pathfinder msg: 1613177091.04 epoch sec; seqNo:7358 2021-02-13T00:44:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 102.92 , 69.48 2021-02-13T00:44:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , -0.01 2021-02-13T00:44:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177091.07 epoch sec; seqNo:15381; 4059939.32 north; 593071.68 east 2021-02-13T00:44:51Z- AHRS_M2 msg: 1613177091.39 epoch sec; seqNo:15259 2021-02-13T00:44:51Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-13T00:44:51Z- handleDepth()-Depth_Keller msg: 1613177091396 epoch sec; seqNo:15382; depth 18.56 2021-02-13T00:44:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177091.41 epoch sec; seqNo:15382; 4059939.69 north; 593071.68 east 2021-02-13T00:44:51Z- AHRS_M2 msg: 1613177091.80 epoch sec; seqNo:15260 2021-02-13T00:44:51Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-13T00:44:51Z- handleDepth()-Depth_Keller msg: 1613177091805 epoch sec; seqNo:15383; depth 18.47 2021-02-13T00:44:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177091.81 epoch sec; seqNo:15383; 4059940.07 north; 593071.67 east 2021-02-13T00:44:52Z- AHRS_M2 msg: 1613177092.20 epoch sec; seqNo:15261 2021-02-13T00:44:52Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:44:52Z- handleDepth()-Depth_Keller msg: 1613177092206 epoch sec; seqNo:15384; depth 18.39 2021-02-13T00:44:52Z- handleDvl() - RDI_Pathfinder msg: 1613177092.22 epoch sec; seqNo:7359 2021-02-13T00:44:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 102.85 , 69.48 2021-02-13T00:44:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , -0.02 2021-02-13T00:44:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177092.23 epoch sec; seqNo:15384; 4059940.07 north; 593071.67 east 2021-02-13T00:44:52Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-13T00:44:52Z- waiting for 1613177089.97, time is 1613177092.23 2021-02-13T00:44:52Z- LcmTrn::updateTrn() equating pose 1613177092.23 and meas times 1613177092.23 2021-02-13T00:44:52Z- LcmTrn::updateTrn() >>>> pose time 1613177092.23, meas time 1613177092.23 <<<< 2021-02-13T00:44:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177092.23 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:44:52Z- TerrainNav::motionUpdate() - currEstimate = (4059940, 593071.67, 18.39) 2021-02-13T00:44:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:44:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:44:52Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177092.23 sec.; No relevant navigation data available 2021-02-13T00:44:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:44:52Z- TNavBF::mmse selected from filter -1 2021-02-13T00:44:52Z- variances([0], [2], and psi_berg) = 689.371069 832.992662 0.000000 2021-02-13T00:44:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:44:52Z- LcmTrn::publishEstimates() - MLE : 4059939.57 593092.17 18.39 2021-02-13T00:44:52Z- LcmTrn::publishEstimates() - MMSE : 4059940.07 593071.67 18.39 2021-02-13T00:44:52Z- LcmTrn::publishEstimates() - COVAR: 689.37 832.99 0.98 0.00 2021-02-13T00:44:52Z- AHRS_M2 msg: 1613177092.61 epoch sec; seqNo:15262 2021-02-13T00:44:52Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:44:52Z- handleDepth()-Depth_Keller msg: 1613177092615 epoch sec; seqNo:15385; depth 18.27 2021-02-13T00:44:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177092.64 epoch sec; seqNo:15385; 4059940.45 north; 593071.67 east 2021-02-13T00:44:53Z- AHRS_M2 msg: 1613177093.01 epoch sec; seqNo:15263 2021-02-13T00:44:53Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:44:53Z- handleDepth()-Depth_Keller msg: 1613177093013 epoch sec; seqNo:15386; depth 18.16 2021-02-13T00:44:53Z- handleDvl() - RDI_Pathfinder msg: 1613177093.03 epoch sec; seqNo:7360 2021-02-13T00:44:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.02 , 95.55 , 105.28 , 69.89 2021-02-13T00:44:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.04 2021-02-13T00:44:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177093.04 epoch sec; seqNo:15386; 4059940.84 north; 593072.89 east 2021-02-13T00:44:53Z- AHRS_M2 msg: 1613177093.41 epoch sec; seqNo:15264 2021-02-13T00:44:53Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:44:53Z- handleDepth()-Depth_Keller msg: 1613177093416 epoch sec; seqNo:15387; depth 18.06 2021-02-13T00:44:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177093.43 epoch sec; seqNo:15387; 4059941.22 north; 593072.88 east 2021-02-13T00:44:53Z- AHRS_M2 msg: 1613177093.82 epoch sec; seqNo:15265 2021-02-13T00:44:53Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:44:53Z- handleDepth()-Depth_Keller msg: 1613177093825 epoch sec; seqNo:15388; depth 17.95 2021-02-13T00:44:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177093.83 epoch sec; seqNo:15388; 4059941.22 north; 593072.88 east 2021-02-13T00:44:54Z- AHRS_M2 msg: 1613177094.22 epoch sec; seqNo:15266 2021-02-13T00:44:54Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:44:54Z- handleDepth()-Depth_Keller msg: 1613177094225 epoch sec; seqNo:15389; depth 17.85 2021-02-13T00:44:54Z- handleDvl() - RDI_Pathfinder msg: 1613177094.24 epoch sec; seqNo:7361 2021-02-13T00:44:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 95.61 , 105.35 , 71.93 2021-02-13T00:44:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.07 , -0.02 2021-02-13T00:44:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177094.25 epoch sec; seqNo:15389; 4059941.60 north; 593072.88 east 2021-02-13T00:44:54Z- AHRS_M2 msg: 1613177094.62 epoch sec; seqNo:15267 2021-02-13T00:44:54Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:44:54Z- handleDepth()-Depth_Keller msg: 1613177094629 epoch sec; seqNo:15390; depth 17.75 2021-02-13T00:44:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177094.64 epoch sec; seqNo:15390; 4059941.98 north; 593072.87 east 2021-02-13T00:44:55Z- AHRS_M2 msg: 1613177095.05 epoch sec; seqNo:15268 2021-02-13T00:44:55Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T00:44:55Z- handleDepth()-Depth_Keller msg: 1613177095054 epoch sec; seqNo:15391; depth 17.64 2021-02-13T00:44:55Z- handleDvl() - RDI_Pathfinder msg: 1613177095.07 epoch sec; seqNo:7362 2021-02-13T00:44:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 106.43 , 72.34 2021-02-13T00:44:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.06 , 0.02 2021-02-13T00:44:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177095.08 epoch sec; seqNo:15391; 4059942.36 north; 593072.87 east 2021-02-13T00:44:55Z- AHRS_M2 msg: 1613177095.43 epoch sec; seqNo:15269 2021-02-13T00:44:55Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T00:44:55Z- handleDepth()-Depth_Keller msg: 1613177095436 epoch sec; seqNo:15392; depth 17.53 2021-02-13T00:44:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177095.45 epoch sec; seqNo:15392; 4059942.74 north; 593072.86 east 2021-02-13T00:44:55Z- AHRS_M2 msg: 1613177095.84 epoch sec; seqNo:15270 2021-02-13T00:44:55Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.18 psi 2021-02-13T00:44:55Z- handleDepth()-Depth_Keller msg: 1613177095846 epoch sec; seqNo:15393; depth 17.44 2021-02-13T00:44:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177095.86 epoch sec; seqNo:15393; 4059943.13 north; 593074.08 east 2021-02-13T00:44:56Z- AHRS_M2 msg: 1613177096.24 epoch sec; seqNo:15271 2021-02-13T00:44:56Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.18 psi 2021-02-13T00:44:56Z- handleDepth()-Depth_Keller msg: 1613177096257 epoch sec; seqNo:15394; depth 17.32 2021-02-13T00:44:56Z- handleDvl() - RDI_Pathfinder msg: 1613177096.27 epoch sec; seqNo:7363 2021-02-13T00:44:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.37 , 97.85 , 106.36 , 78.37 2021-02-13T00:44:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , 0.04 2021-02-13T00:44:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177096.28 epoch sec; seqNo:15394; 4059943.13 north; 593074.08 east 2021-02-13T00:44:56Z- AHRS_M2 msg: 1613177096.64 epoch sec; seqNo:15272 2021-02-13T00:44:56Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.18 psi 2021-02-13T00:44:56Z- handleDepth()-Depth_Keller msg: 1613177096653 epoch sec; seqNo:15395; depth 17.21 2021-02-13T00:44:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177096.67 epoch sec; seqNo:15395; 4059943.51 north; 593074.08 east 2021-02-13T00:44:57Z- AHRS_M2 msg: 1613177097.06 epoch sec; seqNo:15273 2021-02-13T00:44:57Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.18 psi 2021-02-13T00:44:57Z- handleDepth()-Depth_Keller msg: 1613177097064 epoch sec; seqNo:15396; depth 17.10 2021-02-13T00:44:57Z- handleDvl() - RDI_Pathfinder msg: 1613177097.09 epoch sec; seqNo:7364 2021-02-13T00:44:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.67 , 97.71 , 106.23 , 72.75 2021-02-13T00:44:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , 0.00 2021-02-13T00:44:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177097.10 epoch sec; seqNo:15396; 4059943.89 north; 593074.07 east 2021-02-13T00:44:57Z- LcmTrn::handleMessages() - handling 41 messages took 0.60 ms and 129 calls... 2021-02-13T00:44:57Z- waiting for 1613177095.23, time is 1613177097.10 2021-02-13T00:44:57Z- LcmTrn::updateTrn() equating pose 1613177097.10 and meas times 1613177097.10 2021-02-13T00:44:57Z- LcmTrn::updateTrn() >>>> pose time 1613177097.10, meas time 1613177097.10 <<<< 2021-02-13T00:44:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177097.10 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:44:57Z- TerrainNav::motionUpdate() - currEstimate = (4059944, 593074.07, 17.10) 2021-02-13T00:44:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:44:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:44:57Z- TNavBF::Projecting Measurements 2021-02-13T00:44:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:44:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:44:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:44:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:44:57Z- TNavFilter::4 good beams. 2021-02-13T00:44:57Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:44:57Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:44:57Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:44:57Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:44:57Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:44:57Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:44:57Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:44:57Z- TNavBF::mmse selected from filter -1 2021-02-13T00:44:57Z- TerrainMapOctree: (vehN, vehE ) = (4059943.89, 593074.07). 2021-02-13T00:44:57Z- TerrainMapOctree: Min Distance = 1757.48. 2021-02-13T00:44:57Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:44:57Z- There is no specific good beam that all particles have in common. 2021-02-13T00:44:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:44:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 10.41 2021-02-13T00:44:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:44:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:44:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:44:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 18.06 2021-02-13T00:44:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:44:57Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 8.47 2021-02-13T00:44:57Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:44:57Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:44:57Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:44:57Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:44:57Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:44:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:44:57Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:44:57Z- Filter 0 No beams used in subcloud update 2021-02-13T00:44:57Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:44:57Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:44:57Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 689.3 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:44:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177097.10 sec. 2021-02-13T00:44:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:44:57Z- TNavBF::mmse selected from filter -1 2021-02-13T00:44:57Z- variances([0], [2], and psi_berg) = 689.319719 832.992638 0.000000 2021-02-13T00:44:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:44:57Z- LcmTrn::publishEstimates() - MLE : 4059943.39 593094.57 17.10 2021-02-13T00:44:57Z- LcmTrn::publishEstimates() - MMSE : 4059943.89 593074.07 17.10 2021-02-13T00:44:57Z- LcmTrn::publishEstimates() - COVAR: 689.32 832.99 0.89 0.00 2021-02-13T00:44:57Z- AHRS_M2 msg: 1613177097.46 epoch sec; seqNo:15274 2021-02-13T00:44:57Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.18 psi 2021-02-13T00:44:57Z- handleDepth()-Depth_Keller msg: 1613177097469 epoch sec; seqNo:15397; depth 17.00 2021-02-13T00:44:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177097.48 epoch sec; seqNo:15397; 4059944.27 north; 593074.07 east 2021-02-13T00:44:57Z- AHRS_M2 msg: 1613177097.86 epoch sec; seqNo:15275 2021-02-13T00:44:57Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.18 psi 2021-02-13T00:44:57Z- handleDepth()-Depth_Keller msg: 1613177097868 epoch sec; seqNo:15398; depth 16.89 2021-02-13T00:44:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177097.88 epoch sec; seqNo:15398; 4059944.65 north; 593074.06 east 2021-02-13T00:44:58Z- AHRS_M2 msg: 1613177098.27 epoch sec; seqNo:15276 2021-02-13T00:44:58Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-13T00:44:58Z- handleDepth()-Depth_Keller msg: 1613177098278 epoch sec; seqNo:15399; depth 16.78 2021-02-13T00:44:58Z- handleDvl() - RDI_Pathfinder msg: 1613177098.29 epoch sec; seqNo:7365 2021-02-13T00:44:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.81 , 97.85 , 106.36 , 78.37 2021-02-13T00:44:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , -0.08 2021-02-13T00:44:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177098.30 epoch sec; seqNo:15399; 4059944.66 north; 593075.29 east 2021-02-13T00:44:58Z- AHRS_M2 msg: 1613177098.67 epoch sec; seqNo:15277 2021-02-13T00:44:58Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-13T00:44:58Z- handleDepth()-Depth_Keller msg: 1613177098681 epoch sec; seqNo:15400; depth 16.66 2021-02-13T00:44:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177098.69 epoch sec; seqNo:15400; 4059945.04 north; 593075.28 east 2021-02-13T00:44:59Z- AHRS_M2 msg: 1613177099.08 epoch sec; seqNo:15278 2021-02-13T00:44:59Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-13T00:44:59Z- handleDepth()-Depth_Keller msg: 1613177099080 epoch sec; seqNo:15401; depth 16.57 2021-02-13T00:44:59Z- handleDvl() - RDI_Pathfinder msg: 1613177099.09 epoch sec; seqNo:7366 2021-02-13T00:44:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.71 , 107.45 , 78.24 2021-02-13T00:44:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , -0.10 2021-02-13T00:44:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177099.10 epoch sec; seqNo:15401; 4059945.42 north; 593075.28 east 2021-02-13T00:44:59Z- AHRS_M2 msg: 1613177099.48 epoch sec; seqNo:15279 2021-02-13T00:44:59Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-13T00:44:59Z- handleDepth()-Depth_Keller msg: 1613177099493 epoch sec; seqNo:15402; depth 16.46 2021-02-13T00:44:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177099.50 epoch sec; seqNo:15402; 4059945.42 north; 593075.28 east 2021-02-13T00:44:59Z- AHRS_M2 msg: 1613177099.88 epoch sec; seqNo:15280 2021-02-13T00:44:59Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-13T00:44:59Z- handleDepth()-Depth_Keller msg: 1613177099888 epoch sec; seqNo:15403; depth 16.35 2021-02-13T00:44:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177099.91 epoch sec; seqNo:15403; 4059945.80 north; 593075.27 east 2021-02-13T00:45:00Z- AHRS_M2 msg: 1613177100.29 epoch sec; seqNo:15281 2021-02-13T00:45:00Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-13T00:45:00Z- handleDepth()-Depth_Keller msg: 1613177100293 epoch sec; seqNo:15404; depth 16.24 2021-02-13T00:45:00Z- handleDvl() - RDI_Pathfinder msg: 1613177100.31 epoch sec; seqNo:7367 2021-02-13T00:45:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.93 , 97.51 , 108.32 , 78.58 2021-02-13T00:45:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.09 , -0.09 2021-02-13T00:45:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177100.32 epoch sec; seqNo:15404; 4059946.17 north; 593075.27 east 2021-02-13T00:45:00Z- AHRS_M2 msg: 1613177100.69 epoch sec; seqNo:15282 2021-02-13T00:45:00Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.17 psi 2021-02-13T00:45:00Z- handleDepth()-Depth_Keller msg: 1613177100701 epoch sec; seqNo:15405; depth 16.12 2021-02-13T00:45:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177100.71 epoch sec; seqNo:15405; 4059946.55 north; 593075.26 east 2021-02-13T00:45:01Z- AHRS_M2 msg: 1613177101.10 epoch sec; seqNo:15283 2021-02-13T00:45:01Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.17 psi 2021-02-13T00:45:01Z- handleDepth()-Depth_Keller msg: 1613177101113 epoch sec; seqNo:15406; depth 15.99 2021-02-13T00:45:01Z- handleDvl() - RDI_Pathfinder msg: 1613177101.12 epoch sec; seqNo:7368 2021-02-13T00:45:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.47 , 97.64 , 108.60 , 78.17 2021-02-13T00:45:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.10 , -0.00 2021-02-13T00:45:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177101.13 epoch sec; seqNo:15406; 4059946.57 north; 593076.49 east 2021-02-13T00:45:01Z- AHRS_M2 msg: 1613177101.50 epoch sec; seqNo:15284 2021-02-13T00:45:01Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.16 psi 2021-02-13T00:45:01Z- handleDepth()-Depth_Keller msg: 1613177101505 epoch sec; seqNo:15407; depth 15.88 2021-02-13T00:45:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177101.53 epoch sec; seqNo:15407; 4059946.95 north; 593076.48 east 2021-02-13T00:45:01Z- AHRS_M2 msg: 1613177101.90 epoch sec; seqNo:15285 2021-02-13T00:45:01Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.16 psi 2021-02-13T00:45:01Z- handleDepth()-Depth_Keller msg: 1613177101913 epoch sec; seqNo:15408; depth 15.78 2021-02-13T00:45:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177101.92 epoch sec; seqNo:15408; 4059947.32 north; 593076.48 east 2021-02-13T00:45:02Z- AHRS_M2 msg: 1613177102.31 epoch sec; seqNo:15286 2021-02-13T00:45:02Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.16 psi 2021-02-13T00:45:02Z- handleDepth()-Depth_Keller msg: 1613177102313 epoch sec; seqNo:15409; depth 15.66 2021-02-13T00:45:02Z- handleDvl() - RDI_Pathfinder msg: 1613177102.33 epoch sec; seqNo:7369 2021-02-13T00:45:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.67 , 97.71 , 111.10 , 78.24 2021-02-13T00:45:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.08 , 0.03 2021-02-13T00:45:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177102.34 epoch sec; seqNo:15409; 4059947.70 north; 593076.47 east 2021-02-13T00:45:02Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-13T00:45:02Z- waiting for 1613177100.10, time is 1613177102.34 2021-02-13T00:45:02Z- LcmTrn::updateTrn() equating pose 1613177102.34 and meas times 1613177102.34 2021-02-13T00:45:02Z- LcmTrn::updateTrn() >>>> pose time 1613177102.34, meas time 1613177102.34 <<<< 2021-02-13T00:45:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177102.34 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:45:02Z- TerrainNav::motionUpdate() - currEstimate = (4059948, 593076.47, 15.66) 2021-02-13T00:45:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:45:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:45:02Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177102.34 sec.; No relevant navigation data available 2021-02-13T00:45:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:45:02Z- TNavBF::mmse selected from filter -1 2021-02-13T00:45:02Z- variances([0], [2], and psi_berg) = 689.268397 832.992614 0.000000 2021-02-13T00:45:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:45:02Z- LcmTrn::publishEstimates() - MLE : 4059947.20 593096.97 15.66 2021-02-13T00:45:02Z- LcmTrn::publishEstimates() - MMSE : 4059947.70 593076.47 15.66 2021-02-13T00:45:02Z- LcmTrn::publishEstimates() - COVAR: 689.27 832.99 1.12 0.00 2021-02-13T00:45:02Z- AHRS_M2 msg: 1613177102.71 epoch sec; seqNo:15287 2021-02-13T00:45:02Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.16 psi 2021-02-13T00:45:02Z- handleDepth()-Depth_Keller msg: 1613177102725 epoch sec; seqNo:15410; depth 15.54 2021-02-13T00:45:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177102.73 epoch sec; seqNo:15410; 4059948.08 north; 593076.47 east 2021-02-13T00:45:03Z- AHRS_M2 msg: 1613177103.12 epoch sec; seqNo:15288 2021-02-13T00:45:03Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.16 psi 2021-02-13T00:45:03Z- handleDepth()-Depth_Keller msg: 1613177103121 epoch sec; seqNo:15411; depth 15.42 2021-02-13T00:45:03Z- handleDvl() - RDI_Pathfinder msg: 1613177103.14 epoch sec; seqNo:7370 2021-02-13T00:45:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.48 , 97.71 , 109.88 , 73.56 2021-02-13T00:45:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , 0.05 2021-02-13T00:45:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177103.16 epoch sec; seqNo:15411; 4059948.08 north; 593076.47 east 2021-02-13T00:45:03Z- AHRS_M2 msg: 1613177103.52 epoch sec; seqNo:15289 2021-02-13T00:45:03Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.16 psi 2021-02-13T00:45:03Z- handleDepth()-Depth_Keller msg: 1613177103524 epoch sec; seqNo:15412; depth 15.31 2021-02-13T00:45:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177103.54 epoch sec; seqNo:15412; 4059948.46 north; 593076.46 east 2021-02-13T00:45:03Z- AHRS_M2 msg: 1613177103.92 epoch sec; seqNo:15290 2021-02-13T00:45:03Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.16 psi 2021-02-13T00:45:03Z- handleDepth()-Depth_Keller msg: 1613177103929 epoch sec; seqNo:15413; depth 15.20 2021-02-13T00:45:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177103.94 epoch sec; seqNo:15413; 4059948.85 north; 593077.68 east 2021-02-13T00:45:04Z- AHRS_M2 msg: 1613177104.33 epoch sec; seqNo:15291 2021-02-13T00:45:04Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.15 psi 2021-02-13T00:45:04Z- handleDepth()-Depth_Keller msg: 1613177104332 epoch sec; seqNo:15414; depth 15.09 2021-02-13T00:45:04Z- handleDvl() - RDI_Pathfinder msg: 1613177104.35 epoch sec; seqNo:7371 2021-02-13T00:45:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.49 , 111.10 , 78.24 2021-02-13T00:45:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.03 , 0.05 2021-02-13T00:45:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177104.36 epoch sec; seqNo:15414; 4059949.23 north; 593077.68 east 2021-02-13T00:45:04Z- AHRS_M2 msg: 1613177104.73 epoch sec; seqNo:15292 2021-02-13T00:45:04Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.14 psi 2021-02-13T00:45:04Z- handleDepth()-Depth_Keller msg: 1613177104736 epoch sec; seqNo:15415; depth 14.97 2021-02-13T00:45:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177104.75 epoch sec; seqNo:15415; 4059949.61 north; 593077.67 east 2021-02-13T00:45:05Z- AHRS_M2 msg: 1613177105.14 epoch sec; seqNo:15293 2021-02-13T00:45:05Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.13 psi 2021-02-13T00:45:05Z- handleDepth()-Depth_Keller msg: 1613177105147 epoch sec; seqNo:15416; depth 14.86 2021-02-13T00:45:05Z- handleDvl() - RDI_Pathfinder msg: 1613177105.16 epoch sec; seqNo:7372 2021-02-13T00:45:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 97.44 , 112.31 , 78.51 2021-02-13T00:45:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.06 , 0.05 2021-02-13T00:45:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177105.18 epoch sec; seqNo:15416; 4059949.99 north; 593077.67 east 2021-02-13T00:45:05Z- AHRS_M2 msg: 1613177105.54 epoch sec; seqNo:15294 2021-02-13T00:45:05Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.12 psi 2021-02-13T00:45:05Z- handleDepth()-Depth_Keller msg: 1613177105550 epoch sec; seqNo:15417; depth 14.75 2021-02-13T00:45:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177105.56 epoch sec; seqNo:15417; 4059949.99 north; 593077.67 east 2021-02-13T00:45:05Z- AHRS_M2 msg: 1613177105.94 epoch sec; seqNo:15295 2021-02-13T00:45:05Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.11 psi 2021-02-13T00:45:05Z- handleDepth()-Depth_Keller msg: 1613177105952 epoch sec; seqNo:15418; depth 14.63 2021-02-13T00:45:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177105.98 epoch sec; seqNo:15418; 4059950.37 north; 593077.66 east 2021-02-13T00:45:06Z- AHRS_M2 msg: 1613177106.35 epoch sec; seqNo:15296 2021-02-13T00:45:06Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.11 psi 2021-02-13T00:45:06Z- handleDepth()-Depth_Keller msg: 1613177106357 epoch sec; seqNo:15419; depth 14.55 2021-02-13T00:45:06Z- handleDvl() - RDI_Pathfinder msg: 1613177106.37 epoch sec; seqNo:7373 2021-02-13T00:45:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:06Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:45:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177106.40 epoch sec; seqNo:15419; 4059950.76 north; 593078.88 east 2021-02-13T00:45:06Z- AHRS_M2 msg: 1613177106.75 epoch sec; seqNo:15297 2021-02-13T00:45:06Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.11 psi 2021-02-13T00:45:06Z- handleDepth()-Depth_Keller msg: 1613177106758 epoch sec; seqNo:15420; depth 14.40 2021-02-13T00:45:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177106.77 epoch sec; seqNo:15420; 4059950.76 north; 593078.88 east 2021-02-13T00:45:07Z- AHRS_M2 msg: 1613177107.16 epoch sec; seqNo:15298 2021-02-13T00:45:07Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.12 psi 2021-02-13T00:45:07Z- handleDepth()-Depth_Keller msg: 1613177107174 epoch sec; seqNo:15421; depth 14.30 2021-02-13T00:45:07Z- handleDvl() - RDI_Pathfinder msg: 1613177107.19 epoch sec; seqNo:7374 2021-02-13T00:45:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.86 , 97.03 , 110.56 , 72.34 2021-02-13T00:45:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , -0.06 2021-02-13T00:45:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177107.20 epoch sec; seqNo:15421; 4059951.14 north; 593078.88 east 2021-02-13T00:45:07Z- LcmTrn::handleMessages() - handling 41 messages took 0.69 ms and 128 calls... 2021-02-13T00:45:07Z- waiting for 1613177105.34, time is 1613177107.20 2021-02-13T00:45:07Z- LcmTrn::updateTrn() equating pose 1613177107.20 and meas times 1613177107.20 2021-02-13T00:45:07Z- LcmTrn::updateTrn() >>>> pose time 1613177107.20, meas time 1613177107.20 <<<< 2021-02-13T00:45:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177107.20 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:45:07Z- TerrainNav::motionUpdate() - currEstimate = (4059951, 593078.88, 14.30) 2021-02-13T00:45:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:45:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:45:07Z- TNavBF::Projecting Measurements 2021-02-13T00:45:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:45:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:45:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:45:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:45:07Z- TNavFilter::4 good beams. 2021-02-13T00:45:07Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:45:07Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:45:07Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:45:07Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:45:07Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:45:07Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:45:07Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:45:07Z- TNavBF::mmse selected from filter -1 2021-02-13T00:45:07Z- TerrainMapOctree: (vehN, vehE ) = (4059951.14, 593078.88). 2021-02-13T00:45:07Z- TerrainMapOctree: Min Distance = 1748.88. 2021-02-13T00:45:07Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:45:07Z- There is no specific good beam that all particles have in common. 2021-02-13T00:45:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:45:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 11.52 2021-02-13T00:45:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:45:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 15.06 2021-02-13T00:45:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:45:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 19.56 2021-02-13T00:45:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:45:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-13T00:45:07Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:45:07Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:45:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:45:07Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:45:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:45:07Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:45:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:45:07Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:45:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:45:07Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:45:07Z- Filter 0 No beams used in subcloud update 2021-02-13T00:45:07Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:45:07Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:45:07Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 689.2 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:45:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177107.20 sec. 2021-02-13T00:45:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:45:07Z- TNavBF::mmse selected from filter -1 2021-02-13T00:45:07Z- variances([0], [2], and psi_berg) = 689.218918 832.992591 0.000000 2021-02-13T00:45:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:45:07Z- LcmTrn::publishEstimates() - MLE : 4059950.64 593099.38 14.30 2021-02-13T00:45:07Z- LcmTrn::publishEstimates() - MMSE : 4059951.14 593078.88 14.30 2021-02-13T00:45:07Z- LcmTrn::publishEstimates() - COVAR: 689.22 832.99 0.97 0.00 2021-02-13T00:45:07Z- AHRS_M2 msg: 1613177107.56 epoch sec; seqNo:15299 2021-02-13T00:45:07Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.12 psi 2021-02-13T00:45:07Z- handleDepth()-Depth_Keller msg: 1613177107564 epoch sec; seqNo:15422; depth 14.16 2021-02-13T00:45:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:07Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:07Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:07Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:07Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:07Z- handleDvl() - RDI_Pathfinder msg: 1613177107.57 epoch sec; seqNo:7375 2021-02-13T00:45:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , -0.06 2021-02-13T00:45:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177107.58 epoch sec; seqNo:15422; 4059951.52 north; 593078.87 east 2021-02-13T00:45:07Z- AHRS_M2 msg: 1613177107.97 epoch sec; seqNo:15300 2021-02-13T00:45:07Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.12 psi 2021-02-13T00:45:07Z- handleDepth()-Depth_Keller msg: 1613177107974 epoch sec; seqNo:15423; depth 14.05 2021-02-13T00:45:07Z- handleDvl() - RDI_Pathfinder msg: 1613177107.99 epoch sec; seqNo:7376 2021-02-13T00:45:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.64 , 111.03 , 74.79 2021-02-13T00:45:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.08 2021-02-13T00:45:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177108.00 epoch sec; seqNo:15423; 4059951.90 north; 593078.87 east 2021-02-13T00:45:08Z- AHRS_M2 msg: 1613177108.37 epoch sec; seqNo:15301 2021-02-13T00:45:08Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-13T00:45:08Z- handleDepth()-Depth_Keller msg: 1613177108381 epoch sec; seqNo:15424; depth 13.91 2021-02-13T00:45:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177108.39 epoch sec; seqNo:15424; 4059951.90 north; 593078.87 east 2021-02-13T00:45:08Z- AHRS_M2 msg: 1613177108.77 epoch sec; seqNo:15302 2021-02-13T00:45:08Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.14 psi 2021-02-13T00:45:08Z- handleDepth()-Depth_Keller msg: 1613177108781 epoch sec; seqNo:15425; depth 13.81 2021-02-13T00:45:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:08Z- handleDvl() - RDI_Pathfinder msg: 1613177108.78 epoch sec; seqNo:7377 2021-02-13T00:45:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.08 2021-02-13T00:45:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177108.79 epoch sec; seqNo:15425; 4059952.29 north; 593080.09 east 2021-02-13T00:45:09Z- AHRS_M2 msg: 1613177109.18 epoch sec; seqNo:15303 2021-02-13T00:45:09Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-13T00:45:09Z- handleDepth()-Depth_Keller msg: 1613177109185 epoch sec; seqNo:15426; depth 13.69 2021-02-13T00:45:09Z- handleDvl() - RDI_Pathfinder msg: 1613177109.20 epoch sec; seqNo:7378 2021-02-13T00:45:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.86 , 112.38 , 73.15 2021-02-13T00:45:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.09 , -0.03 2021-02-13T00:45:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177109.21 epoch sec; seqNo:15426; 4059952.67 north; 593080.08 east 2021-02-13T00:45:09Z- AHRS_M2 msg: 1613177109.58 epoch sec; seqNo:15304 2021-02-13T00:45:09Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-13T00:45:09Z- handleDepth()-Depth_Keller msg: 1613177109585 epoch sec; seqNo:15427; depth 13.56 2021-02-13T00:45:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177109.60 epoch sec; seqNo:15427; 4059953.05 north; 593080.08 east 2021-02-13T00:45:10Z- AHRS_M2 msg: 1613177110.01 epoch sec; seqNo:15305 2021-02-13T00:45:10Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-13T00:45:10Z- handleDepth()-Depth_Keller msg: 1613177110012 epoch sec; seqNo:15428; depth 13.45 2021-02-13T00:45:10Z- handleDvl() - RDI_Pathfinder msg: 1613177110.02 epoch sec; seqNo:7379 2021-02-13T00:45:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 97.51 , 113.73 , 73.56 2021-02-13T00:45:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.08 , 0.04 2021-02-13T00:45:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177110.03 epoch sec; seqNo:15428; 4059953.43 north; 593080.07 east 2021-02-13T00:45:10Z- AHRS_M2 msg: 1613177110.39 epoch sec; seqNo:15306 2021-02-13T00:45:10Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-13T00:45:10Z- handleDepth()-Depth_Keller msg: 1613177110392 epoch sec; seqNo:15429; depth 13.32 2021-02-13T00:45:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177110.40 epoch sec; seqNo:15429; 4059953.43 north; 593080.07 east 2021-02-13T00:45:10Z- AHRS_M2 msg: 1613177110.79 epoch sec; seqNo:15307 2021-02-13T00:45:10Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-13T00:45:10Z- handleDepth()-Depth_Keller msg: 1613177110796 epoch sec; seqNo:15430; depth 13.19 2021-02-13T00:45:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177110.81 epoch sec; seqNo:15430; 4059953.80 north; 593080.07 east 2021-02-13T00:45:11Z- AHRS_M2 msg: 1613177111.20 epoch sec; seqNo:15308 2021-02-13T00:45:11Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-13T00:45:11Z- handleDepth()-Depth_Keller msg: 1613177111209 epoch sec; seqNo:15431; depth 13.09 2021-02-13T00:45:11Z- handleDvl() - RDI_Pathfinder msg: 1613177111.22 epoch sec; seqNo:7380 2021-02-13T00:45:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 99.06 , 113.94 , 78.78 2021-02-13T00:45:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.04 , 0.06 2021-02-13T00:45:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177111.23 epoch sec; seqNo:15431; 4059954.18 north; 593080.06 east 2021-02-13T00:45:11Z- AHRS_M2 msg: 1613177111.60 epoch sec; seqNo:15309 2021-02-13T00:45:11Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-13T00:45:11Z- handleDepth()-Depth_Keller msg: 1613177111614 epoch sec; seqNo:15432; depth 12.96 2021-02-13T00:45:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177111.63 epoch sec; seqNo:15432; 4059954.58 north; 593081.28 east 2021-02-13T00:45:12Z- AHRS_M2 msg: 1613177112.02 epoch sec; seqNo:15310 2021-02-13T00:45:12Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-13T00:45:12Z- handleDepth()-Depth_Keller msg: 1613177112022 epoch sec; seqNo:15433; depth 12.83 2021-02-13T00:45:12Z- handleDvl() - RDI_Pathfinder msg: 1613177112.04 epoch sec; seqNo:7381 2021-02-13T00:45:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.28 , 113.80 , 78.64 2021-02-13T00:45:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , 0.04 2021-02-13T00:45:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177112.06 epoch sec; seqNo:15433; 4059954.95 north; 593081.28 east 2021-02-13T00:45:12Z- AHRS_M2 msg: 1613177112.41 epoch sec; seqNo:15311 2021-02-13T00:45:12Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.16 psi 2021-02-13T00:45:12Z- handleDepth()-Depth_Keller msg: 1613177112412 epoch sec; seqNo:15434; depth 12.72 2021-02-13T00:45:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:12Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:12Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:12Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:12Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:12Z- handleDvl() - RDI_Pathfinder msg: 1613177112.41 epoch sec; seqNo:7382 2021-02-13T00:45:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , 0.04 2021-02-13T00:45:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177112.42 epoch sec; seqNo:15434; 4059955.33 north; 593081.27 east 2021-02-13T00:45:12Z- LcmTrn::handleMessages() - handling 47 messages took 0.70 ms and 133 calls... 2021-02-13T00:45:12Z- waiting for 1613177110.20, time is 1613177112.42 2021-02-13T00:45:12Z- LcmTrn::updateTrn() equating pose 1613177112.42 and meas times 1613177112.42 2021-02-13T00:45:12Z- LcmTrn::updateTrn() >>>> pose time 1613177112.42, meas time 1613177112.42 <<<< 2021-02-13T00:45:12Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177112.42, ping # 0. 2021-02-13T00:45:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177112.42 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:45:12Z- TerrainNav::motionUpdate() - currEstimate = (4059955, 593081.27, 12.72) 2021-02-13T00:45:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:45:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:45:12Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:45:12Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:45:12Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177112.42 sec.; No relevant navigation data available 2021-02-13T00:45:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:45:12Z- TNavBF::mmse selected from filter -1 2021-02-13T00:45:12Z- variances([0], [2], and psi_berg) = 689.165845 832.992566 0.000000 2021-02-13T00:45:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:45:12Z- LcmTrn::publishEstimates() - MLE : 4059954.83 593101.77 12.72 2021-02-13T00:45:12Z- LcmTrn::publishEstimates() - MMSE : 4059955.33 593081.27 12.72 2021-02-13T00:45:12Z- LcmTrn::publishEstimates() - COVAR: 689.17 832.99 1.22 0.00 2021-02-13T00:45:12Z- AHRS_M2 msg: 1613177112.81 epoch sec; seqNo:15312 2021-02-13T00:45:12Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.16 psi 2021-02-13T00:45:12Z- handleDepth()-Depth_Keller msg: 1613177112817 epoch sec; seqNo:15435; depth 12.60 2021-02-13T00:45:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177112.83 epoch sec; seqNo:15435; 4059955.33 north; 593081.27 east 2021-02-13T00:45:13Z- AHRS_M2 msg: 1613177113.22 epoch sec; seqNo:15313 2021-02-13T00:45:13Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.17 psi 2021-02-13T00:45:13Z- handleDepth()-Depth_Keller msg: 1613177113221 epoch sec; seqNo:15436; depth 12.47 2021-02-13T00:45:13Z- handleDvl() - RDI_Pathfinder msg: 1613177113.24 epoch sec; seqNo:7383 2021-02-13T00:45:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.65 , 117.52 , 78.31 2021-02-13T00:45:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , 0.02 2021-02-13T00:45:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177113.25 epoch sec; seqNo:15436; 4059955.71 north; 593081.27 east 2021-02-13T00:45:13Z- AHRS_M2 msg: 1613177113.62 epoch sec; seqNo:15314 2021-02-13T00:45:13Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.16 psi 2021-02-13T00:45:13Z- handleDepth()-Depth_Keller msg: 1613177113630 epoch sec; seqNo:15437; depth 12.32 2021-02-13T00:45:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:13Z- handleDvl() - RDI_Pathfinder msg: 1613177113.63 epoch sec; seqNo:7384 2021-02-13T00:45:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , 0.02 2021-02-13T00:45:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177113.65 epoch sec; seqNo:15437; 4059956.10 north; 593082.49 east 2021-02-13T00:45:14Z- AHRS_M2 msg: 1613177114.03 epoch sec; seqNo:15315 2021-02-13T00:45:14Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.16 psi 2021-02-13T00:45:14Z- handleDepth()-Depth_Keller msg: 1613177114032 epoch sec; seqNo:15438; depth 12.20 2021-02-13T00:45:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177114.04 epoch sec; seqNo:15438; 4059956.48 north; 593082.48 east 2021-02-13T00:45:14Z- AHRS_M2 msg: 1613177114.43 epoch sec; seqNo:15316 2021-02-13T00:45:14Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.16 psi 2021-02-13T00:45:14Z- handleDepth()-Depth_Keller msg: 1613177114432 epoch sec; seqNo:15439; depth 12.08 2021-02-13T00:45:14Z- handleDvl() - RDI_Pathfinder msg: 1613177114.44 epoch sec; seqNo:7385 2021-02-13T00:45:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 101.16 , 118.74 , 78.17 2021-02-13T00:45:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.08 , -0.02 2021-02-13T00:45:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177114.46 epoch sec; seqNo:15439; 4059956.48 north; 593082.48 east 2021-02-13T00:45:14Z- AHRS_M2 msg: 1613177114.83 epoch sec; seqNo:15317 2021-02-13T00:45:14Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.16 psi 2021-02-13T00:45:14Z- handleDepth()-Depth_Keller msg: 1613177114841 epoch sec; seqNo:15440; depth 11.94 2021-02-13T00:45:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:14Z- handleDvl() - RDI_Pathfinder msg: 1613177114.84 epoch sec; seqNo:7386 2021-02-13T00:45:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.08 , -0.02 2021-02-13T00:45:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177114.85 epoch sec; seqNo:15440; 4059956.86 north; 593082.48 east 2021-02-13T00:45:15Z- AHRS_M2 msg: 1613177115.24 epoch sec; seqNo:15318 2021-02-13T00:45:15Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.15 psi 2021-02-13T00:45:15Z- handleDepth()-Depth_Keller msg: 1613177115242 epoch sec; seqNo:15441; depth 11.81 2021-02-13T00:45:15Z- handleDvl() - RDI_Pathfinder msg: 1613177115.25 epoch sec; seqNo:7387 2021-02-13T00:45:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:15Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:45:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177115.27 epoch sec; seqNo:15441; 4059957.24 north; 593082.47 east 2021-02-13T00:45:15Z- AHRS_M2 msg: 1613177115.64 epoch sec; seqNo:15319 2021-02-13T00:45:15Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.14 psi 2021-02-13T00:45:15Z- handleDepth()-Depth_Keller msg: 1613177115644 epoch sec; seqNo:15442; depth 11.67 2021-02-13T00:45:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177115.66 epoch sec; seqNo:15442; 4059957.24 north; 593082.47 east 2021-02-13T00:45:16Z- AHRS_M2 msg: 1613177116.04 epoch sec; seqNo:15320 2021-02-13T00:45:16Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.14 psi 2021-02-13T00:45:16Z- handleDepth()-Depth_Keller msg: 1613177116048 epoch sec; seqNo:15443; depth 11.54 2021-02-13T00:45:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:16Z- handleDvl() - RDI_Pathfinder msg: 1613177116.05 epoch sec; seqNo:7388 2021-02-13T00:45:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:16Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:45:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177116.06 epoch sec; seqNo:15443; 4059957.63 north; 593083.69 east 2021-02-13T00:45:16Z- AHRS_M2 msg: 1613177116.45 epoch sec; seqNo:15321 2021-02-13T00:45:16Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.13 psi 2021-02-13T00:45:16Z- handleDepth()-Depth_Keller msg: 1613177116452 epoch sec; seqNo:15444; depth 11.40 2021-02-13T00:45:16Z- handleDvl() - RDI_Pathfinder msg: 1613177116.46 epoch sec; seqNo:7389 2021-02-13T00:45:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 101.70 , 118.06 , 77.04 2021-02-13T00:45:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.06 , -0.13 2021-02-13T00:45:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177116.47 epoch sec; seqNo:15444; 4059958.01 north; 593083.69 east 2021-02-13T00:45:16Z- AHRS_M2 msg: 1613177116.85 epoch sec; seqNo:15322 2021-02-13T00:45:16Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.12 psi 2021-02-13T00:45:16Z- handleDepth()-Depth_Keller msg: 1613177116865 epoch sec; seqNo:15445; depth 11.25 2021-02-13T00:45:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177116.88 epoch sec; seqNo:15445; 4059958.01 north; 593083.69 east 2021-02-13T00:45:17Z- AHRS_M2 msg: 1613177117.26 epoch sec; seqNo:15323 2021-02-13T00:45:17Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.12 psi 2021-02-13T00:45:17Z- handleDepth()-Depth_Keller msg: 1613177117264 epoch sec; seqNo:15446; depth 11.12 2021-02-13T00:45:17Z- handleDvl() - RDI_Pathfinder msg: 1613177117.29 epoch sec; seqNo:7390 2021-02-13T00:45:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 99.87 , 113.39 , 76.83 2021-02-13T00:45:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , -0.03 2021-02-13T00:45:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177117.30 epoch sec; seqNo:15446; 4059958.39 north; 593083.68 east 2021-02-13T00:45:17Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:45:17Z- waiting for 1613177115.42, time is 1613177117.30 2021-02-13T00:45:17Z- LcmTrn::updateTrn() equating pose 1613177117.30 and meas times 1613177117.30 2021-02-13T00:45:17Z- LcmTrn::updateTrn() >>>> pose time 1613177117.30, meas time 1613177117.30 <<<< 2021-02-13T00:45:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177117.30 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:45:17Z- TerrainNav::motionUpdate() - currEstimate = (4059958, 593083.68, 11.12) 2021-02-13T00:45:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:45:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:45:17Z- TNavBF::Projecting Measurements 2021-02-13T00:45:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:45:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:45:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:45:17Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:45:17Z- TNavFilter::3 good beams. 2021-02-13T00:45:17Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:45:17Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:45:17Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:45:17Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:45:17Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:45:17Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:45:17Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:45:17Z- TNavBF::mmse selected from filter -1 2021-02-13T00:45:17Z- TerrainMapOctree: (vehN, vehE ) = (4059958.39, 593083.68). 2021-02-13T00:45:17Z- TerrainMapOctree: Min Distance = 1740.27. 2021-02-13T00:45:17Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:45:17Z- There is no specific good beam that all particles have in common. 2021-02-13T00:45:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:45:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.42 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 15.96 2021-02-13T00:45:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:45:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 20.57 2021-02-13T00:45:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:45:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 9.45 2021-02-13T00:45:17Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:45:17Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:45:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:45:17Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:45:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:45:17Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:45:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:45:17Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:45:17Z- Filter 0 No beams used in subcloud update 2021-02-13T00:45:17Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:45:17Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:45:17Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 689.1 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:45:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177117.30 sec. 2021-02-13T00:45:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:45:17Z- TNavBF::mmse selected from filter -1 2021-02-13T00:45:17Z- variances([0], [2], and psi_berg) = 689.118231 832.992544 0.000000 2021-02-13T00:45:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:45:17Z- LcmTrn::publishEstimates() - MLE : 4059957.89 593104.18 11.12 2021-02-13T00:45:17Z- LcmTrn::publishEstimates() - MMSE : 4059958.39 593083.68 11.12 2021-02-13T00:45:17Z- LcmTrn::publishEstimates() - COVAR: 689.12 832.99 1.12 0.00 2021-02-13T00:45:17Z- AHRS_M2 msg: 1613177117.66 epoch sec; seqNo:15324 2021-02-13T00:45:17Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.13 psi 2021-02-13T00:45:17Z- handleDepth()-Depth_Keller msg: 1613177117664 epoch sec; seqNo:15447; depth 10.98 2021-02-13T00:45:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177117.68 epoch sec; seqNo:15447; 4059958.77 north; 593083.68 east 2021-02-13T00:45:18Z- AHRS_M2 msg: 1613177118.06 epoch sec; seqNo:15325 2021-02-13T00:45:18Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.15 psi 2021-02-13T00:45:18Z- handleDepth()-Depth_Keller msg: 1613177118070 epoch sec; seqNo:15448; depth 10.84 2021-02-13T00:45:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177118.08 epoch sec; seqNo:15448; 4059958.77 north; 593083.68 east 2021-02-13T00:45:18Z- AHRS_M2 msg: 1613177118.47 epoch sec; seqNo:15326 2021-02-13T00:45:18Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.16 psi 2021-02-13T00:45:18Z- handleDepth()-Depth_Keller msg: 1613177118472 epoch sec; seqNo:15449; depth 10.71 2021-02-13T00:45:18Z- handleDvl() - RDI_Pathfinder msg: 1613177118.49 epoch sec; seqNo:7391 2021-02-13T00:45:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.31 , 100.01 , 120.29 , 78.37 2021-02-13T00:45:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , 0.09 2021-02-13T00:45:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177118.50 epoch sec; seqNo:15449; 4059959.16 north; 593084.89 east 2021-02-13T00:45:18Z- AHRS_M2 msg: 1613177118.87 epoch sec; seqNo:15327 2021-02-13T00:45:18Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.16 psi 2021-02-13T00:45:18Z- handleDepth()-Depth_Keller msg: 1613177118875 epoch sec; seqNo:15450; depth 10.58 2021-02-13T00:45:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177118.89 epoch sec; seqNo:15450; 4059959.54 north; 593084.89 east 2021-02-13T00:45:19Z- AHRS_M2 msg: 1613177119.28 epoch sec; seqNo:15328 2021-02-13T00:45:19Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.16 psi 2021-02-13T00:45:19Z- handleDepth()-Depth_Keller msg: 1613177119282 epoch sec; seqNo:15451; depth 10.45 2021-02-13T00:45:19Z- handleDvl() - RDI_Pathfinder msg: 1613177119.29 epoch sec; seqNo:7392 2021-02-13T00:45:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 102.51 , 120.09 , 78.17 2021-02-13T00:45:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.05 , 0.08 2021-02-13T00:45:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177119.30 epoch sec; seqNo:15451; 4059959.54 north; 593084.89 east 2021-02-13T00:45:19Z- AHRS_M2 msg: 1613177119.68 epoch sec; seqNo:15329 2021-02-13T00:45:19Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.16 psi 2021-02-13T00:45:19Z- handleDepth()-Depth_Keller msg: 1613177119689 epoch sec; seqNo:15452; depth 10.32 2021-02-13T00:45:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:19Z- handleDvl() - RDI_Pathfinder msg: 1613177119.69 epoch sec; seqNo:7393 2021-02-13T00:45:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.05 , 0.08 2021-02-13T00:45:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177119.71 epoch sec; seqNo:15452; 4059959.92 north; 593084.89 east 2021-02-13T00:45:20Z- AHRS_M2 msg: 1613177120.09 epoch sec; seqNo:15330 2021-02-13T00:45:20Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.17 psi 2021-02-13T00:45:20Z- handleDepth()-Depth_Keller msg: 1613177120091 epoch sec; seqNo:15453; depth 10.17 2021-02-13T00:45:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177120.10 epoch sec; seqNo:15453; 4059960.30 north; 593084.88 east 2021-02-13T00:45:20Z- AHRS_M2 msg: 1613177120.49 epoch sec; seqNo:15331 2021-02-13T00:45:20Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.17 psi 2021-02-13T00:45:20Z- handleDepth()-Depth_Keller msg: 1613177120497 epoch sec; seqNo:15454; depth 10.05 2021-02-13T00:45:20Z- handleDvl() - RDI_Pathfinder msg: 1613177120.51 epoch sec; seqNo:7394 2021-02-13T00:45:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 102.71 , 116.24 , 79.73 2021-02-13T00:45:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.05 , 0.05 2021-02-13T00:45:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177120.52 epoch sec; seqNo:15454; 4059960.30 north; 593084.88 east 2021-02-13T00:45:20Z- AHRS_M2 msg: 1613177120.89 epoch sec; seqNo:15332 2021-02-13T00:45:20Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.18 psi 2021-02-13T00:45:20Z- handleDepth()-Depth_Keller msg: 1613177120900 epoch sec; seqNo:15455; depth 9.93 2021-02-13T00:45:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:20Z- handleDvl() - RDI_Pathfinder msg: 1613177120.90 epoch sec; seqNo:7395 2021-02-13T00:45:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.05 , 0.05 2021-02-13T00:45:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177120.91 epoch sec; seqNo:15455; 4059960.69 north; 593086.10 east 2021-02-13T00:45:21Z- AHRS_M2 msg: 1613177121.30 epoch sec; seqNo:15333 2021-02-13T00:45:21Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.19 psi 2021-02-13T00:45:21Z- handleDepth()-Depth_Keller msg: 1613177121301 epoch sec; seqNo:15456; depth 9.81 2021-02-13T00:45:21Z- handleDvl() - RDI_Pathfinder msg: 1613177121.32 epoch sec; seqNo:7396 2021-02-13T00:45:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 105.49 , 117.65 , 79.79 2021-02-13T00:45:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , 0.03 2021-02-13T00:45:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177121.33 epoch sec; seqNo:15456; 4059961.07 north; 593086.09 east 2021-02-13T00:45:21Z- AHRS_M2 msg: 1613177121.70 epoch sec; seqNo:15334 2021-02-13T00:45:21Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.20 psi 2021-02-13T00:45:21Z- handleDepth()-Depth_Keller msg: 1613177121704 epoch sec; seqNo:15457; depth 9.67 2021-02-13T00:45:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177121.71 epoch sec; seqNo:15457; 4059961.07 north; 593086.09 east 2021-02-13T00:45:22Z- AHRS_M2 msg: 1613177122.11 epoch sec; seqNo:15335 2021-02-13T00:45:22Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.21 psi 2021-02-13T00:45:22Z- handleDepth()-Depth_Keller msg: 1613177122110 epoch sec; seqNo:15458; depth 9.55 2021-02-13T00:45:22Z- handleDvl() - RDI_Pathfinder msg: 1613177122.12 epoch sec; seqNo:7397 2021-02-13T00:45:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:22Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:45:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177122.14 epoch sec; seqNo:15458; 4059961.45 north; 593086.09 east 2021-02-13T00:45:22Z- AHRS_M2 msg: 1613177122.51 epoch sec; seqNo:15336 2021-02-13T00:45:22Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.21 psi 2021-02-13T00:45:22Z- handleDepth()-Depth_Keller msg: 1613177122512 epoch sec; seqNo:15459; depth 9.44 2021-02-13T00:45:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177122.53 epoch sec; seqNo:15459; 4059961.83 north; 593086.09 east 2021-02-13T00:45:22Z- LcmTrn::handleMessages() - handling 46 messages took 0.71 ms and 132 calls... 2021-02-13T00:45:22Z- waiting for 1613177120.30, time is 1613177122.53 2021-02-13T00:45:22Z- LcmTrn::updateTrn() >>>> pose time 1613177122.53, meas time 1613177122.12 <<<< 2021-02-13T00:45:22Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177122.12, ping # 0. 2021-02-13T00:45:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177122.12 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:45:22Z- TerrainNav::motionUpdate() - currEstimate = (4059962, 593086.09, 9.44) 2021-02-13T00:45:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:45:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:45:22Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:45:22Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:45:22Z- TNavBF::Projecting Measurements 2021-02-13T00:45:22Z- TNavFilter::Measurement from time = 1613177122.12 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:45:22Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:45:22Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:45:22Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:45:22Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:45:22Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:45:22Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:45:22Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:45:22Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 689.1 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:45:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:45:22Z- TNavBF::mmse selected from filter -1 2021-02-13T00:45:22Z- variances([0], [2], and psi_berg) = 689.057029 832.992515 0.000000 2021-02-13T00:45:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:45:22Z- LcmTrn::publishEstimates() - MLE : 4059961.33 593106.59 9.44 2021-02-13T00:45:22Z- LcmTrn::publishEstimates() - MMSE : 4059961.83 593086.09 9.44 2021-02-13T00:45:22Z- LcmTrn::publishEstimates() - COVAR: 689.06 832.99 1.33 0.00 2021-02-13T00:45:22Z- AHRS_M2 msg: 1613177122.92 epoch sec; seqNo:15337 2021-02-13T00:45:22Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.22 psi 2021-02-13T00:45:22Z- handleDepth()-Depth_Keller msg: 1613177122926 epoch sec; seqNo:15460; depth 9.31 2021-02-13T00:45:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177122.95 epoch sec; seqNo:15460; 4059962.21 north; 593086.08 east 2021-02-13T00:45:23Z- AHRS_M2 msg: 1613177123.32 epoch sec; seqNo:15338 2021-02-13T00:45:23Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.22 psi 2021-02-13T00:45:23Z- handleDepth()-Depth_Keller msg: 1613177123325 epoch sec; seqNo:15461; depth 9.21 2021-02-13T00:45:23Z- handleDvl() - RDI_Pathfinder msg: 1613177123.34 epoch sec; seqNo:7398 2021-02-13T00:45:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.42 , 108.05 , 116.17 , 81.01 2021-02-13T00:45:23Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.01 , 0.02 2021-02-13T00:45:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177123.35 epoch sec; seqNo:15461; 4059962.22 north; 593087.30 east 2021-02-13T00:45:23Z- AHRS_M2 msg: 1613177123.72 epoch sec; seqNo:15339 2021-02-13T00:45:23Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.21 psi 2021-02-13T00:45:23Z- handleDepth()-Depth_Keller msg: 1613177123726 epoch sec; seqNo:15462; depth 9.11 2021-02-13T00:45:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177123.74 epoch sec; seqNo:15462; 4059962.60 north; 593087.30 east 2021-02-13T00:45:24Z- AHRS_M2 msg: 1613177124.13 epoch sec; seqNo:15340 2021-02-13T00:45:24Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.21 psi 2021-02-13T00:45:24Z- handleDepth()-Depth_Keller msg: 1613177124139 epoch sec; seqNo:15463; depth 8.99 2021-02-13T00:45:24Z- handleDvl() - RDI_Pathfinder msg: 1613177124.15 epoch sec; seqNo:7399 2021-02-13T00:45:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 108.05 , 118.87 , 81.01 2021-02-13T00:45:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.08 , -0.02 2021-02-13T00:45:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177124.17 epoch sec; seqNo:15463; 4059962.98 north; 593087.29 east 2021-02-13T00:45:24Z- AHRS_M2 msg: 1613177124.53 epoch sec; seqNo:15341 2021-02-13T00:45:24Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.20 psi 2021-02-13T00:45:24Z- handleDepth()-Depth_Keller msg: 1613177124532 epoch sec; seqNo:15464; depth 8.88 2021-02-13T00:45:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:24Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:24Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:24Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:24Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:24Z- handleDvl() - RDI_Pathfinder msg: 1613177124.53 epoch sec; seqNo:7400 2021-02-13T00:45:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.08 , -0.02 2021-02-13T00:45:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177124.55 epoch sec; seqNo:15464; 4059963.36 north; 593087.29 east 2021-02-13T00:45:24Z- AHRS_M2 msg: 1613177124.93 epoch sec; seqNo:15342 2021-02-13T00:45:24Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.19 psi 2021-02-13T00:45:24Z- handleDepth()-Depth_Keller msg: 1613177124941 epoch sec; seqNo:15465; depth 8.78 2021-02-13T00:45:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177124.95 epoch sec; seqNo:15465; 4059963.73 north; 593087.29 east 2021-02-13T00:45:25Z- AHRS_M2 msg: 1613177125.34 epoch sec; seqNo:15343 2021-02-13T00:45:25Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.19 psi 2021-02-13T00:45:25Z- handleDepth()-Depth_Keller msg: 1613177125342 epoch sec; seqNo:15466; depth 8.68 2021-02-13T00:45:25Z- handleDvl() - RDI_Pathfinder msg: 1613177125.36 epoch sec; seqNo:7401 2021-02-13T00:45:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.50 , 109.81 , 120.22 , 81.55 2021-02-13T00:45:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.09 , -0.12 2021-02-13T00:45:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177125.37 epoch sec; seqNo:15466; 4059963.75 north; 593088.51 east 2021-02-13T00:45:25Z- AHRS_M2 msg: 1613177125.74 epoch sec; seqNo:15344 2021-02-13T00:45:25Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-13T00:45:25Z- handleDepth()-Depth_Keller msg: 1613177125749 epoch sec; seqNo:15467; depth 8.57 2021-02-13T00:45:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177125.76 epoch sec; seqNo:15467; 4059964.13 north; 593088.50 east 2021-02-13T00:45:26Z- AHRS_M2 msg: 1613177126.16 epoch sec; seqNo:15345 2021-02-13T00:45:26Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-13T00:45:26Z- handleDepth()-Depth_Keller msg: 1613177126162 epoch sec; seqNo:15468; depth 8.47 2021-02-13T00:45:26Z- handleDvl() - RDI_Pathfinder msg: 1613177126.17 epoch sec; seqNo:7402 2021-02-13T00:45:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 105.15 , 118.67 , 82.16 2021-02-13T00:45:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , -0.17 2021-02-13T00:45:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177126.20 epoch sec; seqNo:15468; 4059964.51 north; 593088.50 east 2021-02-13T00:45:26Z- AHRS_M2 msg: 1613177126.55 epoch sec; seqNo:15346 2021-02-13T00:45:26Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.19 psi 2021-02-13T00:45:26Z- handleDepth()-Depth_Keller msg: 1613177126554 epoch sec; seqNo:15469; depth 8.34 2021-02-13T00:45:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177126.57 epoch sec; seqNo:15469; 4059964.51 north; 593088.50 east 2021-02-13T00:45:26Z- AHRS_M2 msg: 1613177126.95 epoch sec; seqNo:15347 2021-02-13T00:45:26Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.19 psi 2021-02-13T00:45:26Z- handleDepth()-Depth_Keller msg: 1613177126956 epoch sec; seqNo:15470; depth 8.25 2021-02-13T00:45:26Z- handleDvl() - RDI_Pathfinder msg: 1613177126.97 epoch sec; seqNo:7403 2021-02-13T00:45:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:26Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:45:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177126.98 epoch sec; seqNo:15470; 4059964.88 north; 593088.49 east 2021-02-13T00:45:27Z- AHRS_M2 msg: 1613177127.36 epoch sec; seqNo:15348 2021-02-13T00:45:27Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.19 psi 2021-02-13T00:45:27Z- handleDepth()-Depth_Keller msg: 1613177127360 epoch sec; seqNo:15471; depth 8.13 2021-02-13T00:45:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177127.37 epoch sec; seqNo:15471; 4059965.26 north; 593088.49 east 2021-02-13T00:45:27Z- LcmTrn::handleMessages() - handling 42 messages took 0.72 ms and 128 calls... 2021-02-13T00:45:27Z- waiting for 1613177125.53, time is 1613177127.37 2021-02-13T00:45:27Z- LcmTrn::updateTrn() >>>> pose time 1613177127.37, meas time 1613177126.97 <<<< 2021-02-13T00:45:27Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177126.97, ping # 0. 2021-02-13T00:45:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177126.97 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:45:27Z- TerrainNav::motionUpdate() - currEstimate = (4059965, 593088.49, 8.13) 2021-02-13T00:45:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:45:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:45:27Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:45:27Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:45:27Z- TNavBF::Projecting Measurements 2021-02-13T00:45:27Z- TNavFilter::Measurement from time = 1613177126.97 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:45:27Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:45:27Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:45:27Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:45:27Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:45:27Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:45:27Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:45:27Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:45:27Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 689.0 EastVariance: 833.0 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-13T00:45:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:45:27Z- TNavBF::mmse selected from filter -1 2021-02-13T00:45:27Z- variances([0], [2], and psi_berg) = 688.995596 832.992486 0.000000 2021-02-13T00:45:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:45:27Z- LcmTrn::publishEstimates() - MLE : 4059964.76 593108.99 8.13 2021-02-13T00:45:27Z- LcmTrn::publishEstimates() - MMSE : 4059965.26 593088.49 8.13 2021-02-13T00:45:27Z- LcmTrn::publishEstimates() - COVAR: 689.00 832.99 1.54 0.00 2021-02-13T00:45:27Z- AHRS_M2 msg: 1613177127.76 epoch sec; seqNo:15349 2021-02-13T00:45:27Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.18 psi 2021-02-13T00:45:27Z- handleDepth()-Depth_Keller msg: 1613177127766 epoch sec; seqNo:15472; depth 8.04 2021-02-13T00:45:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177127.77 epoch sec; seqNo:15472; 4059965.26 north; 593088.49 east 2021-02-13T00:45:28Z- AHRS_M2 msg: 1613177128.17 epoch sec; seqNo:15350 2021-02-13T00:45:28Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.18 psi 2021-02-13T00:45:28Z- handleDepth()-Depth_Keller msg: 1613177128174 epoch sec; seqNo:15473; depth 7.92 2021-02-13T00:45:28Z- handleDvl() - RDI_Pathfinder msg: 1613177128.19 epoch sec; seqNo:7404 2021-02-13T00:45:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.70 , 105.28 , 117.45 , 80.94 2021-02-13T00:45:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , 0.18 2021-02-13T00:45:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177128.20 epoch sec; seqNo:15473; 4059965.66 north; 593089.71 east 2021-02-13T00:45:28Z- AHRS_M2 msg: 1613177128.57 epoch sec; seqNo:15351 2021-02-13T00:45:28Z- AHRS_M2 msg: 0.58 phi; 0.30 theta; -0.17 psi 2021-02-13T00:45:28Z- handleDepth()-Depth_Keller msg: 1613177128578 epoch sec; seqNo:15474; depth 7.82 2021-02-13T00:45:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177128.59 epoch sec; seqNo:15474; 4059966.03 north; 593089.70 east 2021-02-13T00:45:28Z- AHRS_M2 msg: 1613177128.97 epoch sec; seqNo:15352 2021-02-13T00:45:28Z- AHRS_M2 msg: 0.57 phi; 0.31 theta; -0.17 psi 2021-02-13T00:45:28Z- handleDepth()-Depth_Keller msg: 1613177128979 epoch sec; seqNo:15475; depth 7.71 2021-02-13T00:45:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177128.99 epoch sec; seqNo:15475; 4059966.41 north; 593089.70 east 2021-02-13T00:45:29Z- AHRS_M2 msg: 1613177129.38 epoch sec; seqNo:15353 2021-02-13T00:45:29Z- AHRS_M2 msg: 0.56 phi; 0.32 theta; -0.17 psi 2021-02-13T00:45:29Z- handleDepth()-Depth_Keller msg: 1613177129380 epoch sec; seqNo:15476; depth 7.60 2021-02-13T00:45:29Z- handleDvl() - RDI_Pathfinder msg: 1613177129.39 epoch sec; seqNo:7405 2021-02-13T00:45:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 104.00 , 117.52 , 82.36 2021-02-13T00:45:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.14 , 0.17 2021-02-13T00:45:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177129.40 epoch sec; seqNo:15476; 4059966.41 north; 593089.70 east 2021-02-13T00:45:29Z- AHRS_M2 msg: 1613177129.78 epoch sec; seqNo:15354 2021-02-13T00:45:29Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.18 psi 2021-02-13T00:45:29Z- handleDepth()-Depth_Keller msg: 1613177129785 epoch sec; seqNo:15477; depth 7.50 2021-02-13T00:45:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177129.80 epoch sec; seqNo:15477; 4059966.79 north; 593089.69 east 2021-02-13T00:45:30Z- AHRS_M2 msg: 1613177130.18 epoch sec; seqNo:15355 2021-02-13T00:45:30Z- AHRS_M2 msg: 0.55 phi; 0.35 theta; -0.18 psi 2021-02-13T00:45:30Z- handleDepth()-Depth_Keller msg: 1613177130195 epoch sec; seqNo:15478; depth 7.40 2021-02-13T00:45:30Z- handleDvl() - RDI_Pathfinder msg: 1613177130.21 epoch sec; seqNo:7406 2021-02-13T00:45:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.92 , 106.50 , 121.37 , 82.16 2021-02-13T00:45:30Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.28 , 0.00 2021-02-13T00:45:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177130.22 epoch sec; seqNo:15478; 4059967.18 north; 593090.91 east 2021-02-13T00:45:30Z- AHRS_M2 msg: 1613177130.59 epoch sec; seqNo:15356 2021-02-13T00:45:30Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.18 psi 2021-02-13T00:45:30Z- handleDepth()-Depth_Keller msg: 1613177130594 epoch sec; seqNo:15479; depth 7.31 2021-02-13T00:45:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:30Z- handleDvl() - RDI_Pathfinder msg: 1613177130.60 epoch sec; seqNo:7407 2021-02-13T00:45:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:30Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.28 , 0.00 2021-02-13T00:45:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177130.62 epoch sec; seqNo:15479; 4059967.56 north; 593090.91 east 2021-02-13T00:45:31Z- AHRS_M2 msg: 1613177131.00 epoch sec; seqNo:15357 2021-02-13T00:45:31Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.17 psi 2021-02-13T00:45:31Z- handleDepth()-Depth_Keller msg: 1613177131009 epoch sec; seqNo:15480; depth 7.19 2021-02-13T00:45:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177131.03 epoch sec; seqNo:15480; 4059967.56 north; 593090.91 east 2021-02-13T00:45:31Z- AHRS_M2 msg: 1613177131.40 epoch sec; seqNo:15358 2021-02-13T00:45:31Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.17 psi 2021-02-13T00:45:31Z- handleDepth()-Depth_Keller msg: 1613177131402 epoch sec; seqNo:15481; depth 7.08 2021-02-13T00:45:31Z- handleDvl() - RDI_Pathfinder msg: 1613177131.41 epoch sec; seqNo:7408 2021-02-13T00:45:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.27 , 109.20 , 126.78 , 82.16 2021-02-13T00:45:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.24 , -0.19 2021-02-13T00:45:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177131.42 epoch sec; seqNo:15481; 4059967.94 north; 593090.90 east 2021-02-13T00:45:31Z- AHRS_M2 msg: 1613177131.80 epoch sec; seqNo:15359 2021-02-13T00:45:31Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.16 psi 2021-02-13T00:45:31Z- handleDepth()-Depth_Keller msg: 1613177131806 epoch sec; seqNo:15482; depth 6.96 2021-02-13T00:45:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:31Z- handleDvl() - RDI_Pathfinder msg: 1613177131.81 epoch sec; seqNo:7409 2021-02-13T00:45:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.24 , -0.19 2021-02-13T00:45:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177131.82 epoch sec; seqNo:15482; 4059967.94 north; 593090.90 east 2021-02-13T00:45:32Z- AHRS_M2 msg: 1613177132.20 epoch sec; seqNo:15360 2021-02-13T00:45:32Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.16 psi 2021-02-13T00:45:32Z- handleDepth()-Depth_Keller msg: 1613177132213 epoch sec; seqNo:15483; depth 6.85 2021-02-13T00:45:32Z- handleDvl() - RDI_Pathfinder msg: 1613177132.24 epoch sec; seqNo:7410 2021-02-13T00:45:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.91 , 108.32 , 124.69 , 81.55 2021-02-13T00:45:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.10 , -0.25 2021-02-13T00:45:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177132.25 epoch sec; seqNo:15483; 4059968.33 north; 593092.12 east 2021-02-13T00:45:32Z- AHRS_M2 msg: 1613177132.63 epoch sec; seqNo:15361 2021-02-13T00:45:32Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.16 psi 2021-02-13T00:45:32Z- handleDepth()-Depth_Keller msg: 1613177132642 epoch sec; seqNo:15484; depth 6.73 2021-02-13T00:45:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177132.65 epoch sec; seqNo:15484; 4059968.33 north; 593092.12 east 2021-02-13T00:45:32Z- LcmTrn::handleMessages() - handling 46 messages took 0.71 ms and 132 calls... 2021-02-13T00:45:32Z- waiting for 1613177130.37, time is 1613177132.65 2021-02-13T00:45:32Z- LcmTrn::updateTrn() >>>> pose time 1613177132.65, meas time 1613177132.24 <<<< 2021-02-13T00:45:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177132.24 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:45:32Z- TerrainNav::motionUpdate() - currEstimate = (4059968, 593092.12, 6.73) 2021-02-13T00:45:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:45:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:45:32Z- TNavBF::Projecting Measurements 2021-02-13T00:45:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:45:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:45:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:45:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:45:32Z- TNavFilter::4 good beams. 2021-02-13T00:45:32Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:45:32Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:45:32Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:45:32Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:45:32Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:45:32Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:45:32Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:45:32Z- TNavBF::mmse selected from filter -1 2021-02-13T00:45:32Z- TerrainMapOctree: (vehN, vehE ) = (4059968.09, 593091.83). 2021-02-13T00:45:32Z- TerrainMapOctree: Min Distance = 1727.61. 2021-02-13T00:45:32Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:45:32Z- There is no specific good beam that all particles have in common. 2021-02-13T00:45:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:45:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.77 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:45:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:45:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.60 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 18.77 2021-02-13T00:45:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:45:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.48 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 24.87 2021-02-13T00:45:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:45:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.41 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 10.64 2021-02-13T00:45:32Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:45:32Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:45:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:45:32Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:45:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:45:32Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:45:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:45:32Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:45:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:45:32Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:45:32Z- Filter 0 No beams used in subcloud update 2021-02-13T00:45:32Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:45:32Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:45:32Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 688.9 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:45:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177132.24 sec. 2021-02-13T00:45:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:45:32Z- TNavBF::mmse selected from filter -1 2021-02-13T00:45:32Z- variances([0], [2], and psi_berg) = 688.930471 832.992456 0.000000 2021-02-13T00:45:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:45:32Z- LcmTrn::publishEstimates() - MLE : 4059967.83 593112.62 6.73 2021-02-13T00:45:32Z- LcmTrn::publishEstimates() - MMSE : 4059968.33 593092.12 6.73 2021-02-13T00:45:32Z- LcmTrn::publishEstimates() - COVAR: 688.93 832.99 1.28 0.00 2021-02-13T00:45:33Z- AHRS_M2 msg: 1613177133.01 epoch sec; seqNo:15362 2021-02-13T00:45:33Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.15 psi 2021-02-13T00:45:33Z- handleDepth()-Depth_Keller msg: 1613177133021 epoch sec; seqNo:15485; depth 6.58 2021-02-13T00:45:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:33Z- handleDvl() - RDI_Pathfinder msg: 1613177133.02 epoch sec; seqNo:7411 2021-02-13T00:45:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.10 , -0.25 2021-02-13T00:45:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177133.03 epoch sec; seqNo:15485; 4059968.71 north; 593092.11 east 2021-02-13T00:45:33Z- AHRS_M2 msg: 1613177133.42 epoch sec; seqNo:15363 2021-02-13T00:45:33Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.15 psi 2021-02-13T00:45:33Z- handleDepth()-Depth_Keller msg: 1613177133420 epoch sec; seqNo:15486; depth 6.48 2021-02-13T00:45:33Z- handleDvl() - RDI_Pathfinder msg: 1613177133.44 epoch sec; seqNo:7412 2021-02-13T00:45:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.90 , 108.26 , 123.13 , 82.97 2021-02-13T00:45:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.07 , -0.21 2021-02-13T00:45:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177133.45 epoch sec; seqNo:15486; 4059969.09 north; 593092.11 east 2021-02-13T00:45:33Z- AHRS_M2 msg: 1613177133.82 epoch sec; seqNo:15364 2021-02-13T00:45:33Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.15 psi 2021-02-13T00:45:33Z- handleDepth()-Depth_Keller msg: 1613177133824 epoch sec; seqNo:15487; depth 6.33 2021-02-13T00:45:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177133.84 epoch sec; seqNo:15487; 4059969.09 north; 593092.11 east 2021-02-13T00:45:34Z- AHRS_M2 msg: 1613177134.22 epoch sec; seqNo:15365 2021-02-13T00:45:34Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.15 psi 2021-02-13T00:45:34Z- handleDepth()-Depth_Keller msg: 1613177134229 epoch sec; seqNo:15488; depth 6.21 2021-02-13T00:45:34Z- handleDvl() - RDI_Pathfinder msg: 1613177134.24 epoch sec; seqNo:7413 2021-02-13T00:45:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.03 , 108.39 , 129.21 , 81.62 2021-02-13T00:45:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.15 , -0.16 2021-02-13T00:45:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177134.25 epoch sec; seqNo:15488; 4059969.47 north; 593092.11 east 2021-02-13T00:45:34Z- AHRS_M2 msg: 1613177134.63 epoch sec; seqNo:15366 2021-02-13T00:45:34Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.15 psi 2021-02-13T00:45:34Z- handleDepth()-Depth_Keller msg: 1613177134633 epoch sec; seqNo:15489; depth 6.09 2021-02-13T00:45:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177134.65 epoch sec; seqNo:15489; 4059969.85 north; 593092.10 east 2021-02-13T00:45:35Z- AHRS_M2 msg: 1613177135.03 epoch sec; seqNo:15367 2021-02-13T00:45:35Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-13T00:45:35Z- handleDepth()-Depth_Keller msg: 1613177135036 epoch sec; seqNo:15490; depth 5.96 2021-02-13T00:45:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177135.05 epoch sec; seqNo:15490; 4059969.85 north; 593092.10 east 2021-02-13T00:45:35Z- AHRS_M2 msg: 1613177135.44 epoch sec; seqNo:15368 2021-02-13T00:45:35Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-13T00:45:35Z- handleDepth()-Depth_Keller msg: 1613177135440 epoch sec; seqNo:15491; depth 5.85 2021-02-13T00:45:35Z- handleDvl() - RDI_Pathfinder msg: 1613177135.45 epoch sec; seqNo:7414 2021-02-13T00:45:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 106.77 , 127.59 , 82.97 2021-02-13T00:45:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.20 , 0.00 2021-02-13T00:45:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177135.48 epoch sec; seqNo:15491; 4059970.23 north; 593092.10 east 2021-02-13T00:45:35Z- AHRS_M2 msg: 1613177135.84 epoch sec; seqNo:15369 2021-02-13T00:45:35Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.15 psi 2021-02-13T00:45:35Z- handleDepth()-Depth_Keller msg: 1613177135845 epoch sec; seqNo:15492; depth 5.72 2021-02-13T00:45:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177135.86 epoch sec; seqNo:15492; 4059970.61 north; 593092.09 east 2021-02-13T00:45:36Z- AHRS_M2 msg: 1613177136.24 epoch sec; seqNo:15370 2021-02-13T00:45:36Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.14 psi 2021-02-13T00:45:36Z- handleDepth()-Depth_Keller msg: 1613177136248 epoch sec; seqNo:15493; depth 5.58 2021-02-13T00:45:36Z- handleDvl() - RDI_Pathfinder msg: 1613177136.27 epoch sec; seqNo:7415 2021-02-13T00:45:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 108.19 , 121.58 , 82.90 2021-02-13T00:45:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.11 , 0.23 2021-02-13T00:45:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177136.28 epoch sec; seqNo:15493; 4059971.00 north; 593093.31 east 2021-02-13T00:45:36Z- AHRS_M2 msg: 1613177136.65 epoch sec; seqNo:15371 2021-02-13T00:45:36Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.14 psi 2021-02-13T00:45:36Z- handleDepth()-Depth_Keller msg: 1613177136652 epoch sec; seqNo:15494; depth 5.45 2021-02-13T00:45:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:36Z- handleDvl() - RDI_Pathfinder msg: 1613177136.65 epoch sec; seqNo:7416 2021-02-13T00:45:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.11 , 0.23 2021-02-13T00:45:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177136.66 epoch sec; seqNo:15494; 4059971.38 north; 593093.31 east 2021-02-13T00:45:37Z- AHRS_M2 msg: 1613177137.05 epoch sec; seqNo:15372 2021-02-13T00:45:37Z- AHRS_M2 msg: 0.59 phi; 0.27 theta; -0.15 psi 2021-02-13T00:45:37Z- handleDepth()-Depth_Keller msg: 1613177137061 epoch sec; seqNo:15495; depth 5.33 2021-02-13T00:45:37Z- handleDvl() - RDI_Pathfinder msg: 1613177137.07 epoch sec; seqNo:7417 2021-02-13T00:45:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.96 , 105.35 , 120.22 , 0.00 2021-02-13T00:45:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.12 , 0.20 2021-02-13T00:45:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177137.08 epoch sec; seqNo:15495; 4059971.38 north; 593093.31 east 2021-02-13T00:45:37Z- AHRS_M2 msg: 1613177137.46 epoch sec; seqNo:15373 2021-02-13T00:45:37Z- AHRS_M2 msg: 0.60 phi; 0.23 theta; -0.17 psi 2021-02-13T00:45:37Z- handleDepth()-Depth_Keller msg: 1613177137470 epoch sec; seqNo:15496; depth 5.19 2021-02-13T00:45:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177137.48 epoch sec; seqNo:15496; 4059971.76 north; 593093.30 east 2021-02-13T00:45:37Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-13T00:45:37Z- waiting for 1613177135.65, time is 1613177137.48 2021-02-13T00:45:37Z- LcmTrn::updateTrn() >>>> pose time 1613177137.48, meas time 1613177137.07 <<<< 2021-02-13T00:45:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177137.07 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:45:37Z- TerrainNav::motionUpdate() - currEstimate = (4059972, 593093.30, 5.19) 2021-02-13T00:45:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:45:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:45:37Z- TNavBF::Projecting Measurements 2021-02-13T00:45:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:45:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:45:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:45:37Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:45:37Z- TNavFilter::3 good beams. 2021-02-13T00:45:37Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:45:37Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:45:37Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:45:37Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:45:37Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:45:37Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:45:37Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:45:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:45:37Z- TerrainMapOctree: (vehN, vehE ) = (4059971.47, 593093.20). 2021-02-13T00:45:37Z- TerrainMapOctree: Min Distance = 1724.19. 2021-02-13T00:45:37Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:45:37Z- There is no specific good beam that all particles have in common. 2021-02-13T00:45:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:45:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.45 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 13.53 2021-02-13T00:45:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:45:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.32 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 17.76 2021-02-13T00:45:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:45:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 23.13 2021-02-13T00:45:37Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:45:37Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:45:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:45:37Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:45:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:45:37Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:45:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:45:37Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:45:37Z- Filter 0 No beams used in subcloud update 2021-02-13T00:45:37Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:45:37Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:45:37Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 688.9 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:45:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177137.07 sec. 2021-02-13T00:45:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:45:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:45:37Z- variances([0], [2], and psi_berg) = 688.873306 832.992429 0.000000 2021-02-13T00:45:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:45:37Z- LcmTrn::publishEstimates() - MLE : 4059971.26 593114.80 5.19 2021-02-13T00:45:37Z- LcmTrn::publishEstimates() - MMSE : 4059971.76 593093.30 5.19 2021-02-13T00:45:37Z- LcmTrn::publishEstimates() - COVAR: 688.87 832.99 1.20 0.00 2021-02-13T00:45:37Z- AHRS_M2 msg: 1613177137.86 epoch sec; seqNo:15374 2021-02-13T00:45:37Z- AHRS_M2 msg: 0.61 phi; 0.19 theta; -0.19 psi 2021-02-13T00:45:37Z- handleDepth()-Depth_Keller msg: 1613177137864 epoch sec; seqNo:15497; depth 5.08 2021-02-13T00:45:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:37Z- handleDvl() - RDI_Pathfinder msg: 1613177137.87 epoch sec; seqNo:7418 2021-02-13T00:45:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.12 , 0.20 2021-02-13T00:45:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177137.87 epoch sec; seqNo:15497; 4059972.14 north; 593093.30 east 2021-02-13T00:45:38Z- AHRS_M2 msg: 1613177138.26 epoch sec; seqNo:15375 2021-02-13T00:45:38Z- AHRS_M2 msg: 0.62 phi; 0.14 theta; -0.21 psi 2021-02-13T00:45:38Z- handleDepth()-Depth_Keller msg: 1613177138269 epoch sec; seqNo:15498; depth 4.97 2021-02-13T00:45:38Z- handleDvl() - RDI_Pathfinder msg: 1613177138.28 epoch sec; seqNo:7419 2021-02-13T00:45:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.57 , 106.50 , 112.45 , 85.68 2021-02-13T00:45:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.13 , -0.02 2021-02-13T00:45:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177138.29 epoch sec; seqNo:15498; 4059972.15 north; 593094.52 east 2021-02-13T00:45:38Z- AHRS_M2 msg: 1613177138.67 epoch sec; seqNo:15376 2021-02-13T00:45:38Z- AHRS_M2 msg: 0.63 phi; 0.09 theta; -0.23 psi 2021-02-13T00:45:38Z- handleDepth()-Depth_Keller msg: 1613177138674 epoch sec; seqNo:15499; depth 4.85 2021-02-13T00:45:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177138.68 epoch sec; seqNo:15499; 4059972.53 north; 593094.51 east 2021-02-13T00:45:39Z- AHRS_M2 msg: 1613177139.07 epoch sec; seqNo:15377 2021-02-13T00:45:39Z- AHRS_M2 msg: 0.63 phi; 0.05 theta; -0.25 psi 2021-02-13T00:45:39Z- handleDepth()-Depth_Keller msg: 1613177139081 epoch sec; seqNo:15500; depth 4.75 2021-02-13T00:45:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:39Z- handleDvl() - RDI_Pathfinder msg: 1613177139.08 epoch sec; seqNo:7420 2021-02-13T00:45:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.13 , -0.02 2021-02-13T00:45:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177139.10 epoch sec; seqNo:15500; 4059972.91 north; 593094.51 east 2021-02-13T00:45:39Z- AHRS_M2 msg: 1613177139.48 epoch sec; seqNo:15378 2021-02-13T00:45:39Z- AHRS_M2 msg: 0.63 phi; 0.01 theta; -0.27 psi 2021-02-13T00:45:39Z- handleDepth()-Depth_Keller msg: 1613177139480 epoch sec; seqNo:15501; depth 4.66 2021-02-13T00:45:39Z- handleDvl() - RDI_Pathfinder msg: 1613177139.49 epoch sec; seqNo:7421 2021-02-13T00:45:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.15 , 110.49 , 103.73 , 96.97 2021-02-13T00:45:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.10 , 0.03 2021-02-13T00:45:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177139.50 epoch sec; seqNo:15501; 4059972.91 north; 593094.51 east 2021-02-13T00:45:39Z- AHRS_M2 msg: 1613177139.88 epoch sec; seqNo:15379 2021-02-13T00:45:39Z- AHRS_M2 msg: 0.63 phi; -0.02 theta; -0.27 psi 2021-02-13T00:45:39Z- handleDepth()-Depth_Keller msg: 1613177139893 epoch sec; seqNo:15502; depth 4.61 2021-02-13T00:45:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177139.90 epoch sec; seqNo:15502; 4059973.28 north; 593094.51 east 2021-02-13T00:45:40Z- AHRS_M2 msg: 1613177140.28 epoch sec; seqNo:15380 2021-02-13T00:45:40Z- AHRS_M2 msg: 0.64 phi; -0.05 theta; -0.25 psi 2021-02-13T00:45:40Z- handleDepth()-Depth_Keller msg: 1613177140288 epoch sec; seqNo:15503; depth 4.55 2021-02-13T00:45:40Z- handleDvl() - RDI_Pathfinder msg: 1613177140.31 epoch sec; seqNo:7422 2021-02-13T00:45:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.58 , 114.68 , 98.45 , 97.10 2021-02-13T00:45:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , 0.31 2021-02-13T00:45:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177140.32 epoch sec; seqNo:15503; 4059973.66 north; 593094.50 east 2021-02-13T00:45:40Z- AHRS_M2 msg: 1613177140.69 epoch sec; seqNo:15381 2021-02-13T00:45:40Z- AHRS_M2 msg: 0.64 phi; -0.08 theta; -0.22 psi 2021-02-13T00:45:40Z- handleDepth()-Depth_Keller msg: 1613177140692 epoch sec; seqNo:15504; depth 4.53 2021-02-13T00:45:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177140.70 epoch sec; seqNo:15504; 4059973.68 north; 593095.72 east 2021-02-13T00:45:41Z- AHRS_M2 msg: 1613177141.11 epoch sec; seqNo:15382 2021-02-13T00:45:41Z- AHRS_M2 msg: 0.64 phi; -0.11 theta; -0.20 psi 2021-02-13T00:45:41Z- handleDepth()-Depth_Keller msg: 1613177141119 epoch sec; seqNo:15505; depth 4.52 2021-02-13T00:45:41Z- handleDvl() - RDI_Pathfinder msg: 1613177141.14 epoch sec; seqNo:7423 2021-02-13T00:45:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 107.99 , 97.17 , 101.23 2021-02-13T00:45:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.17 , 0.32 2021-02-13T00:45:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177141.15 epoch sec; seqNo:15505; 4059974.06 north; 593095.72 east 2021-02-13T00:45:41Z- AHRS_M2 msg: 1613177141.50 epoch sec; seqNo:15383 2021-02-13T00:45:41Z- AHRS_M2 msg: 0.65 phi; -0.13 theta; -0.18 psi 2021-02-13T00:45:41Z- handleDepth()-Depth_Keller msg: 1613177141509 epoch sec; seqNo:15506; depth 4.51 2021-02-13T00:45:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:41Z- handleDvl() - RDI_Pathfinder msg: 1613177141.51 epoch sec; seqNo:7424 2021-02-13T00:45:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.17 , 0.32 2021-02-13T00:45:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177141.52 epoch sec; seqNo:15506; 4059974.43 north; 593095.71 east 2021-02-13T00:45:41Z- AHRS_M2 msg: 1613177141.90 epoch sec; seqNo:15384 2021-02-13T00:45:41Z- AHRS_M2 msg: 0.65 phi; -0.15 theta; -0.17 psi 2021-02-13T00:45:41Z- handleDepth()-Depth_Keller msg: 1613177141904 epoch sec; seqNo:15507; depth 4.52 2021-02-13T00:45:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177141.92 epoch sec; seqNo:15507; 4059974.43 north; 593095.71 east 2021-02-13T00:45:42Z- AHRS_M2 msg: 1613177142.30 epoch sec; seqNo:15385 2021-02-13T00:45:42Z- AHRS_M2 msg: 0.66 phi; -0.17 theta; -0.15 psi 2021-02-13T00:45:42Z- handleDepth()-Depth_Keller msg: 1613177142308 epoch sec; seqNo:15508; depth 4.53 2021-02-13T00:45:42Z- handleDvl() - RDI_Pathfinder msg: 1613177142.32 epoch sec; seqNo:7425 2021-02-13T00:45:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.35 , 88.99 , 105.35 2021-02-13T00:45:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.35 , 0.05 2021-02-13T00:45:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177142.35 epoch sec; seqNo:15508; 4059974.83 north; 593096.93 east 2021-02-13T00:45:42Z- AHRS_M2 msg: 1613177142.71 epoch sec; seqNo:15386 2021-02-13T00:45:42Z- AHRS_M2 msg: 0.67 phi; -0.19 theta; -0.14 psi 2021-02-13T00:45:42Z- handleDepth()-Depth_Keller msg: 1613177142716 epoch sec; seqNo:15509; depth 4.55 2021-02-13T00:45:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:42Z- handleDvl() - RDI_Pathfinder msg: 1613177142.72 epoch sec; seqNo:7426 2021-02-13T00:45:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.35 , 0.05 2021-02-13T00:45:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177142.73 epoch sec; seqNo:15509; 4059974.83 north; 593096.93 east 2021-02-13T00:45:42Z- LcmTrn::handleMessages() - handling 48 messages took 0.68 ms and 135 calls... 2021-02-13T00:45:42Z- waiting for 1613177140.48, time is 1613177142.73 2021-02-13T00:45:42Z- LcmTrn::updateTrn() equating pose 1613177142.73 and meas times 1613177142.73 2021-02-13T00:45:42Z- LcmTrn::updateTrn() >>>> pose time 1613177142.73, meas time 1613177142.73 <<<< 2021-02-13T00:45:42Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177142.73, ping # 0. 2021-02-13T00:45:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177142.73 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:45:42Z- TerrainNav::motionUpdate() - currEstimate = (4059975, 593096.93, 4.55) 2021-02-13T00:45:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:45:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:45:42Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:45:42Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:45:42Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177142.73 sec.; No relevant navigation data available 2021-02-13T00:45:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:45:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:45:42Z- variances([0], [2], and psi_berg) = 688.820845 832.992405 0.000000 2021-02-13T00:45:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:45:42Z- LcmTrn::publishEstimates() - MLE : 4059974.33 593118.43 4.55 2021-02-13T00:45:42Z- LcmTrn::publishEstimates() - MMSE : 4059974.83 593096.93 4.55 2021-02-13T00:45:42Z- LcmTrn::publishEstimates() - COVAR: 688.82 832.99 1.44 0.00 2021-02-13T00:45:43Z- AHRS_M2 msg: 1613177143.12 epoch sec; seqNo:15387 2021-02-13T00:45:43Z- AHRS_M2 msg: 0.68 phi; -0.20 theta; -0.13 psi 2021-02-13T00:45:43Z- handleDepth()-Depth_Keller msg: 1613177143122 epoch sec; seqNo:15510; depth 4.60 2021-02-13T00:45:43Z- handleDvl() - RDI_Pathfinder msg: 1613177143.14 epoch sec; seqNo:7427 2021-02-13T00:45:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.03 , 105.15 , 0.00 , 107.85 2021-02-13T00:45:43Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.20 , -0.16 2021-02-13T00:45:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177143.15 epoch sec; seqNo:15510; 4059975.21 north; 593096.93 east 2021-02-13T00:45:43Z- AHRS_M2 msg: 1613177143.52 epoch sec; seqNo:15388 2021-02-13T00:45:43Z- AHRS_M2 msg: 0.69 phi; -0.21 theta; -0.12 psi 2021-02-13T00:45:43Z- handleDepth()-Depth_Keller msg: 1613177143520 epoch sec; seqNo:15511; depth 4.63 2021-02-13T00:45:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177143.54 epoch sec; seqNo:15511; 4059975.58 north; 593096.92 east 2021-02-13T00:45:43Z- AHRS_M2 msg: 1613177143.92 epoch sec; seqNo:15389 2021-02-13T00:45:43Z- AHRS_M2 msg: 0.69 phi; -0.22 theta; -0.13 psi 2021-02-13T00:45:43Z- handleDepth()-Depth_Keller msg: 1613177143924 epoch sec; seqNo:15512; depth 4.68 2021-02-13T00:45:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:43Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:43Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:43Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:43Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:43Z- handleDvl() - RDI_Pathfinder msg: 1613177143.93 epoch sec; seqNo:7428 2021-02-13T00:45:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:43Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.20 , -0.16 2021-02-13T00:45:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177143.93 epoch sec; seqNo:15512; 4059975.98 north; 593098.14 east 2021-02-13T00:45:44Z- AHRS_M2 msg: 1613177144.32 epoch sec; seqNo:15390 2021-02-13T00:45:44Z- AHRS_M2 msg: 0.69 phi; -0.23 theta; -0.12 psi 2021-02-13T00:45:44Z- handleDepth()-Depth_Keller msg: 1613177144328 epoch sec; seqNo:15513; depth 4.73 2021-02-13T00:45:44Z- handleDvl() - RDI_Pathfinder msg: 1613177144.35 epoch sec; seqNo:7429 2021-02-13T00:45:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 102.31 , 0.00 , 111.23 2021-02-13T00:45:44Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.01 , -0.18 2021-02-13T00:45:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177144.36 epoch sec; seqNo:15513; 4059976.36 north; 593098.13 east 2021-02-13T00:45:44Z- AHRS_M2 msg: 1613177144.73 epoch sec; seqNo:15391 2021-02-13T00:45:44Z- AHRS_M2 msg: 0.70 phi; -0.25 theta; -0.11 psi 2021-02-13T00:45:44Z- handleDepth()-Depth_Keller msg: 1613177144734 epoch sec; seqNo:15514; depth 4.79 2021-02-13T00:45:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177144.74 epoch sec; seqNo:15514; 4059976.36 north; 593098.13 east 2021-02-13T00:45:45Z- AHRS_M2 msg: 1613177145.16 epoch sec; seqNo:15392 2021-02-13T00:45:45Z- AHRS_M2 msg: 0.70 phi; -0.26 theta; -0.10 psi 2021-02-13T00:45:45Z- handleDepth()-Depth_Keller msg: 1613177145160 epoch sec; seqNo:15515; depth 4.86 2021-02-13T00:45:45Z- handleDvl() - RDI_Pathfinder msg: 1613177145.17 epoch sec; seqNo:7430 2021-02-13T00:45:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 102.24 , 84.39 , 112.65 2021-02-13T00:45:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.08 , -0.21 2021-02-13T00:45:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177145.19 epoch sec; seqNo:15515; 4059976.73 north; 593098.13 east 2021-02-13T00:45:45Z- AHRS_M2 msg: 1613177145.54 epoch sec; seqNo:15393 2021-02-13T00:45:45Z- AHRS_M2 msg: 0.70 phi; -0.27 theta; -0.12 psi 2021-02-13T00:45:45Z- handleDepth()-Depth_Keller msg: 1613177145540 epoch sec; seqNo:15516; depth 4.92 2021-02-13T00:45:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177145.55 epoch sec; seqNo:15516; 4059977.11 north; 593098.13 east 2021-02-13T00:45:45Z- AHRS_M2 msg: 1613177145.94 epoch sec; seqNo:15394 2021-02-13T00:45:45Z- AHRS_M2 msg: 0.69 phi; -0.28 theta; -0.14 psi 2021-02-13T00:45:45Z- handleDepth()-Depth_Keller msg: 1613177145944 epoch sec; seqNo:15517; depth 4.99 2021-02-13T00:45:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177145.95 epoch sec; seqNo:15517; 4059977.51 north; 593099.34 east 2021-02-13T00:45:46Z- AHRS_M2 msg: 1613177146.34 epoch sec; seqNo:15395 2021-02-13T00:45:46Z- AHRS_M2 msg: 0.69 phi; -0.29 theta; -0.17 psi 2021-02-13T00:45:46Z- handleDepth()-Depth_Keller msg: 1613177146357 epoch sec; seqNo:15518; depth 5.06 2021-02-13T00:45:46Z- handleDvl() - RDI_Pathfinder msg: 1613177146.37 epoch sec; seqNo:7431 2021-02-13T00:45:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 105.21 , 83.58 , 113.33 2021-02-13T00:45:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.24 , -0.17 2021-02-13T00:45:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177146.38 epoch sec; seqNo:15518; 4059977.88 north; 593099.34 east 2021-02-13T00:45:46Z- AHRS_M2 msg: 1613177146.75 epoch sec; seqNo:15396 2021-02-13T00:45:46Z- AHRS_M2 msg: 0.69 phi; -0.30 theta; -0.19 psi 2021-02-13T00:45:46Z- handleDepth()-Depth_Keller msg: 1613177146752 epoch sec; seqNo:15519; depth 5.14 2021-02-13T00:45:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177146.78 epoch sec; seqNo:15519; 4059978.26 north; 593099.33 east 2021-02-13T00:45:47Z- AHRS_M2 msg: 1613177147.15 epoch sec; seqNo:15397 2021-02-13T00:45:47Z- AHRS_M2 msg: 0.69 phi; -0.31 theta; -0.21 psi 2021-02-13T00:45:47Z- handleDepth()-Depth_Keller msg: 1613177147159 epoch sec; seqNo:15520; depth 5.19 2021-02-13T00:45:47Z- handleDvl() - RDI_Pathfinder msg: 1613177147.17 epoch sec; seqNo:7432 2021-02-13T00:45:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 109.41 , 83.71 , 118.87 2021-02-13T00:45:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.21 , 0.12 2021-02-13T00:45:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177147.19 epoch sec; seqNo:15520; 4059978.26 north; 593099.33 east 2021-02-13T00:45:47Z- AHRS_M2 msg: 1613177147.56 epoch sec; seqNo:15398 2021-02-13T00:45:47Z- AHRS_M2 msg: 0.69 phi; -0.32 theta; -0.19 psi 2021-02-13T00:45:47Z- handleDepth()-Depth_Keller msg: 1613177147560 epoch sec; seqNo:15521; depth 5.30 2021-02-13T00:45:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:47Z- handleDvl() - RDI_Pathfinder msg: 1613177147.56 epoch sec; seqNo:7433 2021-02-13T00:45:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.21 , 0.12 2021-02-13T00:45:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177147.58 epoch sec; seqNo:15521; 4059978.64 north; 593099.33 east 2021-02-13T00:45:47Z- LcmTrn::handleMessages() - handling 43 messages took 0.68 ms and 130 calls... 2021-02-13T00:45:47Z- waiting for 1613177145.73, time is 1613177147.58 2021-02-13T00:45:47Z- LcmTrn::updateTrn() equating pose 1613177147.58 and meas times 1613177147.58 2021-02-13T00:45:47Z- LcmTrn::updateTrn() >>>> pose time 1613177147.58, meas time 1613177147.58 <<<< 2021-02-13T00:45:47Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177147.58, ping # 0. 2021-02-13T00:45:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177147.58 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:45:47Z- TerrainNav::motionUpdate() - currEstimate = (4059979, 593099.33, 5.30) 2021-02-13T00:45:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:45:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:45:47Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:45:47Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:45:47Z- TNavBF::Projecting Measurements 2021-02-13T00:45:47Z- TNavFilter::Measurement from time = 1613177147.58 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:45:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:45:47Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:45:47Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:45:47Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:45:47Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:45:47Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:45:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:45:47Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 688.8 EastVariance: 833.0 DepthVariance: 1.7 this->DepthBiasCov: 1.7 2021-02-13T00:45:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:45:47Z- TNavBF::mmse selected from filter -1 2021-02-13T00:45:47Z- variances([0], [2], and psi_berg) = 688.769789 832.992381 0.000000 2021-02-13T00:45:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:45:47Z- LcmTrn::publishEstimates() - MLE : 4059978.14 593120.83 5.30 2021-02-13T00:45:47Z- LcmTrn::publishEstimates() - MMSE : 4059978.64 593099.33 5.30 2021-02-13T00:45:47Z- LcmTrn::publishEstimates() - COVAR: 688.77 832.99 1.67 0.00 2021-02-13T00:45:47Z- AHRS_M2 msg: 1613177147.97 epoch sec; seqNo:15399 2021-02-13T00:45:47Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.15 psi 2021-02-13T00:45:47Z- handleDepth()-Depth_Keller msg: 1613177147971 epoch sec; seqNo:15522; depth 5.38 2021-02-13T00:45:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177147.98 epoch sec; seqNo:15522; 4059979.02 north; 593099.33 east 2021-02-13T00:45:48Z- AHRS_M2 msg: 1613177148.37 epoch sec; seqNo:15400 2021-02-13T00:45:48Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.16 psi 2021-02-13T00:45:48Z- handleDepth()-Depth_Keller msg: 1613177148373 epoch sec; seqNo:15523; depth 5.48 2021-02-13T00:45:48Z- handleDvl() - RDI_Pathfinder msg: 1613177148.39 epoch sec; seqNo:7434 2021-02-13T00:45:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 109.68 , 82.90 , 121.58 2021-02-13T00:45:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , 0.08 , 0.22 2021-02-13T00:45:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177148.40 epoch sec; seqNo:15523; 4059979.02 north; 593099.33 east 2021-02-13T00:45:48Z- AHRS_M2 msg: 1613177148.77 epoch sec; seqNo:15401 2021-02-13T00:45:48Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.18 psi 2021-02-13T00:45:48Z- handleDepth()-Depth_Keller msg: 1613177148772 epoch sec; seqNo:15524; depth 5.58 2021-02-13T00:45:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:48Z- handleDvl() - RDI_Pathfinder msg: 1613177148.77 epoch sec; seqNo:7435 2021-02-13T00:45:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , 0.08 , 0.22 2021-02-13T00:45:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177148.79 epoch sec; seqNo:15524; 4059979.40 north; 593099.32 east 2021-02-13T00:45:49Z- AHRS_M2 msg: 1613177149.17 epoch sec; seqNo:15402 2021-02-13T00:45:49Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.21 psi 2021-02-13T00:45:49Z- handleDepth()-Depth_Keller msg: 1613177149176 epoch sec; seqNo:15525; depth 5.68 2021-02-13T00:45:49Z- handleDvl() - RDI_Pathfinder msg: 1613177149.19 epoch sec; seqNo:7436 2021-02-13T00:45:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.90 , 106.77 , 82.97 , 124.62 2021-02-13T00:45:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.28 , 0.05 2021-02-13T00:45:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177149.20 epoch sec; seqNo:15525; 4059979.41 north; 593100.54 east 2021-02-13T00:45:49Z- AHRS_M2 msg: 1613177149.58 epoch sec; seqNo:15403 2021-02-13T00:45:49Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.25 psi 2021-02-13T00:45:49Z- handleDepth()-Depth_Keller msg: 1613177149582 epoch sec; seqNo:15526; depth 5.79 2021-02-13T00:45:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177149.59 epoch sec; seqNo:15526; 4059979.79 north; 593100.54 east 2021-02-13T00:45:49Z- AHRS_M2 msg: 1613177149.98 epoch sec; seqNo:15404 2021-02-13T00:45:49Z- AHRS_M2 msg: 0.61 phi; -0.38 theta; -0.28 psi 2021-02-13T00:45:49Z- handleDepth()-Depth_Keller msg: 1613177149991 epoch sec; seqNo:15527; depth 5.88 2021-02-13T00:45:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177150.01 epoch sec; seqNo:15527; 4059979.79 north; 593100.54 east 2021-02-13T00:45:50Z- AHRS_M2 msg: 1613177150.38 epoch sec; seqNo:15405 2021-02-13T00:45:50Z- AHRS_M2 msg: 0.61 phi; -0.38 theta; -0.31 psi 2021-02-13T00:45:50Z- handleDepth()-Depth_Keller msg: 1613177150388 epoch sec; seqNo:15528; depth 6.01 2021-02-13T00:45:50Z- handleDvl() - RDI_Pathfinder msg: 1613177150.40 epoch sec; seqNo:7437 2021-02-13T00:45:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.98 , 115.83 , 83.11 , 120.29 2021-02-13T00:45:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.57 , 0.18 , -0.22 2021-02-13T00:45:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177150.42 epoch sec; seqNo:15528; 4059979.79 north; 593100.54 east 2021-02-13T00:45:50Z- AHRS_M2 msg: 1613177150.79 epoch sec; seqNo:15406 2021-02-13T00:45:50Z- AHRS_M2 msg: 0.60 phi; -0.38 theta; -0.32 psi 2021-02-13T00:45:50Z- handleDepth()-Depth_Keller msg: 1613177150792 epoch sec; seqNo:15529; depth 6.13 2021-02-13T00:45:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177150.80 epoch sec; seqNo:15529; 4059980.17 north; 593100.53 east 2021-02-13T00:45:51Z- AHRS_M2 msg: 1613177151.20 epoch sec; seqNo:15407 2021-02-13T00:45:51Z- AHRS_M2 msg: 0.60 phi; -0.38 theta; -0.31 psi 2021-02-13T00:45:51Z- handleDepth()-Depth_Keller msg: 1613177151200 epoch sec; seqNo:15530; depth 6.26 2021-02-13T00:45:51Z- handleDvl() - RDI_Pathfinder msg: 1613177151.21 epoch sec; seqNo:7438 2021-02-13T00:45:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 123.00 , 82.84 , 136.38 2021-02-13T00:45:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.10 , -0.10 2021-02-13T00:45:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177151.22 epoch sec; seqNo:15530; 4059980.55 north; 593100.53 east 2021-02-13T00:45:51Z- AHRS_M2 msg: 1613177151.60 epoch sec; seqNo:15408 2021-02-13T00:45:51Z- AHRS_M2 msg: 0.60 phi; -0.38 theta; -0.30 psi 2021-02-13T00:45:51Z- handleDepth()-Depth_Keller msg: 1613177151601 epoch sec; seqNo:15531; depth 6.39 2021-02-13T00:45:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177151.62 epoch sec; seqNo:15531; 4059980.94 north; 593101.75 east 2021-02-13T00:45:52Z- AHRS_M2 msg: 1613177152.00 epoch sec; seqNo:15409 2021-02-13T00:45:52Z- AHRS_M2 msg: 0.60 phi; -0.38 theta; -0.27 psi 2021-02-13T00:45:52Z- handleDepth()-Depth_Keller msg: 1613177152004 epoch sec; seqNo:15532; depth 6.54 2021-02-13T00:45:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177152.01 epoch sec; seqNo:15532; 4059980.94 north; 593101.75 east 2021-02-13T00:45:52Z- AHRS_M2 msg: 1613177152.40 epoch sec; seqNo:15410 2021-02-13T00:45:52Z- AHRS_M2 msg: 0.59 phi; -0.38 theta; -0.25 psi 2021-02-13T00:45:52Z- handleDepth()-Depth_Keller msg: 1613177152413 epoch sec; seqNo:15533; depth 6.69 2021-02-13T00:45:52Z- handleDvl() - RDI_Pathfinder msg: 1613177152.42 epoch sec; seqNo:7439 2021-02-13T00:45:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.31 , 122.52 , 83.85 , 135.91 2021-02-13T00:45:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.19 , 0.09 2021-02-13T00:45:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177152.43 epoch sec; seqNo:15533; 4059981.32 north; 593101.74 east 2021-02-13T00:45:52Z- AHRS_M2 msg: 1613177152.81 epoch sec; seqNo:15411 2021-02-13T00:45:52Z- AHRS_M2 msg: 0.60 phi; -0.38 theta; -0.23 psi 2021-02-13T00:45:52Z- handleDepth()-Depth_Keller msg: 1613177152817 epoch sec; seqNo:15534; depth 6.84 2021-02-13T00:45:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177152.87 epoch sec; seqNo:15534; 4059981.70 north; 593101.74 east 2021-02-13T00:45:52Z- LcmTrn::handleMessages() - handling 45 messages took 0.67 ms and 132 calls... 2021-02-13T00:45:52Z- waiting for 1613177150.58, time is 1613177152.87 2021-02-13T00:45:52Z- LcmTrn::updateTrn() >>>> pose time 1613177152.87, meas time 1613177152.42 <<<< 2021-02-13T00:45:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177152.42 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:45:52Z- TerrainNav::motionUpdate() - currEstimate = (4059982, 593101.74, 6.84) 2021-02-13T00:45:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:45:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:45:52Z- TNavBF::Projecting Measurements 2021-02-13T00:45:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:45:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:45:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:45:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:45:52Z- TNavFilter::4 good beams. 2021-02-13T00:45:52Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:45:52Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:45:52Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:45:52Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:45:52Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:45:52Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:45:52Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:45:52Z- TNavBF::mmse selected from filter -1 2021-02-13T00:45:52Z- TerrainMapOctree: (vehN, vehE ) = (4059981.44, 593101.54). 2021-02-13T00:45:52Z- TerrainMapOctree: Min Distance = 1711.20. 2021-02-13T00:45:52Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:45:52Z- There is no specific good beam that all particles have in common. 2021-02-13T00:45:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:45:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.85 K: 0.115 ExpectedMeasDiff: nan currMeas.covariance: 12.48 2021-02-13T00:45:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:45:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.64 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 24.02 2021-02-13T00:45:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:45:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.54 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 11.25 2021-02-13T00:45:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:45:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 29.55 2021-02-13T00:45:53Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:45:53Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:45:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:45:53Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:45:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:45:53Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:45:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:45:53Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:45:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:45:53Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:45:53Z- Filter 0 No beams used in subcloud update 2021-02-13T00:45:53Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:45:53Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:45:53Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 688.7 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:45:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177152.42 sec. 2021-02-13T00:45:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:45:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:45:53Z- variances([0], [2], and psi_berg) = 688.710696 832.992353 0.000000 2021-02-13T00:45:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:45:53Z- LcmTrn::publishEstimates() - MLE : 4059981.20 593123.24 6.84 2021-02-13T00:45:53Z- LcmTrn::publishEstimates() - MMSE : 4059981.70 593101.74 6.84 2021-02-13T00:45:53Z- LcmTrn::publishEstimates() - COVAR: 688.71 832.99 1.33 0.00 2021-02-13T00:45:53Z- AHRS_M2 msg: 1613177153.22 epoch sec; seqNo:15412 2021-02-13T00:45:53Z- AHRS_M2 msg: 0.60 phi; -0.38 theta; -0.21 psi 2021-02-13T00:45:53Z- handleDepth()-Depth_Keller msg: 1613177153220 epoch sec; seqNo:15535; depth 7.00 2021-02-13T00:45:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177153.25 epoch sec; seqNo:15535; 4059982.08 north; 593101.73 east 2021-02-13T00:45:53Z- AHRS_M2 msg: 1613177153.62 epoch sec; seqNo:15413 2021-02-13T00:45:53Z- AHRS_M2 msg: 0.60 phi; -0.38 theta; -0.19 psi 2021-02-13T00:45:53Z- handleDepth()-Depth_Keller msg: 1613177153625 epoch sec; seqNo:15536; depth 7.13 2021-02-13T00:45:53Z- handleDvl() - RDI_Pathfinder msg: 1613177153.64 epoch sec; seqNo:7440 2021-02-13T00:45:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.44 , 115.29 , 81.08 , 130.16 2021-02-13T00:45:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.13 , 0.12 2021-02-13T00:45:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177153.66 epoch sec; seqNo:15536; 4059982.46 north; 593101.73 east 2021-02-13T00:45:54Z- AHRS_M2 msg: 1613177154.02 epoch sec; seqNo:15414 2021-02-13T00:45:54Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.18 psi 2021-02-13T00:45:54Z- handleDepth()-Depth_Keller msg: 1613177154024 epoch sec; seqNo:15537; depth 7.29 2021-02-13T00:45:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177154.04 epoch sec; seqNo:15537; 4059982.84 north; 593101.73 east 2021-02-13T00:45:54Z- AHRS_M2 msg: 1613177154.42 epoch sec; seqNo:15415 2021-02-13T00:45:54Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.17 psi 2021-02-13T00:45:54Z- handleDepth()-Depth_Keller msg: 1613177154428 epoch sec; seqNo:15538; depth 7.43 2021-02-13T00:45:54Z- handleDvl() - RDI_Pathfinder msg: 1613177154.45 epoch sec; seqNo:7441 2021-02-13T00:45:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.14 , 107.99 , 81.21 , 127.32 2021-02-13T00:45:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , 0.13 2021-02-13T00:45:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177154.46 epoch sec; seqNo:15538; 4059982.84 north; 593101.73 east 2021-02-13T00:45:54Z- AHRS_M2 msg: 1613177154.83 epoch sec; seqNo:15416 2021-02-13T00:45:54Z- AHRS_M2 msg: 0.59 phi; -0.37 theta; -0.16 psi 2021-02-13T00:45:54Z- handleDepth()-Depth_Keller msg: 1613177154832 epoch sec; seqNo:15539; depth 7.60 2021-02-13T00:45:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:54Z- handleDvl() - RDI_Pathfinder msg: 1613177154.83 epoch sec; seqNo:7442 2021-02-13T00:45:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , 0.13 2021-02-13T00:45:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177154.86 epoch sec; seqNo:15539; 4059983.23 north; 593102.94 east 2021-02-13T00:45:55Z- AHRS_M2 msg: 1613177155.23 epoch sec; seqNo:15417 2021-02-13T00:45:55Z- AHRS_M2 msg: 0.59 phi; -0.37 theta; -0.16 psi 2021-02-13T00:45:55Z- handleDepth()-Depth_Keller msg: 1613177155237 epoch sec; seqNo:15540; depth 7.74 2021-02-13T00:45:55Z- handleDvl() - RDI_Pathfinder msg: 1613177155.25 epoch sec; seqNo:7443 2021-02-13T00:45:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.98 , 106.90 , 79.29 , 132.19 2021-02-13T00:45:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.10 , 0.19 2021-02-13T00:45:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177155.26 epoch sec; seqNo:15540; 4059983.61 north; 593102.94 east 2021-02-13T00:45:55Z- AHRS_M2 msg: 1613177155.64 epoch sec; seqNo:15418 2021-02-13T00:45:55Z- AHRS_M2 msg: 0.58 phi; -0.37 theta; -0.17 psi 2021-02-13T00:45:55Z- handleDepth()-Depth_Keller msg: 1613177155647 epoch sec; seqNo:15541; depth 7.89 2021-02-13T00:45:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177155.66 epoch sec; seqNo:15541; 4059983.61 north; 593102.94 east 2021-02-13T00:45:56Z- AHRS_M2 msg: 1613177156.04 epoch sec; seqNo:15419 2021-02-13T00:45:56Z- AHRS_M2 msg: 0.58 phi; -0.37 theta; -0.18 psi 2021-02-13T00:45:56Z- handleDepth()-Depth_Keller msg: 1613177156044 epoch sec; seqNo:15542; depth 8.04 2021-02-13T00:45:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:56Z- handleDvl() - RDI_Pathfinder msg: 1613177156.05 epoch sec; seqNo:7444 2021-02-13T00:45:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.10 , 0.19 2021-02-13T00:45:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177156.07 epoch sec; seqNo:15542; 4059983.99 north; 593102.93 east 2021-02-13T00:45:56Z- AHRS_M2 msg: 1613177156.44 epoch sec; seqNo:15420 2021-02-13T00:45:56Z- AHRS_M2 msg: 0.57 phi; -0.37 theta; -0.19 psi 2021-02-13T00:45:56Z- handleDepth()-Depth_Keller msg: 1613177156449 epoch sec; seqNo:15543; depth 8.14 2021-02-13T00:45:56Z- handleDvl() - RDI_Pathfinder msg: 1613177156.46 epoch sec; seqNo:7445 2021-02-13T00:45:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 105.21 , 81.42 , 126.04 2021-02-13T00:45:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.32 , 0.14 2021-02-13T00:45:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177156.48 epoch sec; seqNo:15543; 4059983.99 north; 593102.93 east 2021-02-13T00:45:56Z- AHRS_M2 msg: 1613177156.85 epoch sec; seqNo:15421 2021-02-13T00:45:56Z- AHRS_M2 msg: 0.57 phi; -0.37 theta; -0.20 psi 2021-02-13T00:45:56Z- handleDepth()-Depth_Keller msg: 1613177156852 epoch sec; seqNo:15544; depth 8.29 2021-02-13T00:45:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177156.86 epoch sec; seqNo:15544; 4059984.00 north; 593104.16 east 2021-02-13T00:45:57Z- AHRS_M2 msg: 1613177157.25 epoch sec; seqNo:15422 2021-02-13T00:45:57Z- AHRS_M2 msg: 0.57 phi; -0.36 theta; -0.21 psi 2021-02-13T00:45:57Z- handleDepth()-Depth_Keller msg: 1613177157262 epoch sec; seqNo:15545; depth 8.41 2021-02-13T00:45:57Z- handleDvl() - RDI_Pathfinder msg: 1613177157.27 epoch sec; seqNo:7446 2021-02-13T00:45:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.91 , 108.32 , 0.00 , 126.17 2021-02-13T00:45:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.34 , -0.11 2021-02-13T00:45:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177157.29 epoch sec; seqNo:15545; 4059984.38 north; 593104.15 east 2021-02-13T00:45:57Z- AHRS_M2 msg: 1613177157.66 epoch sec; seqNo:15423 2021-02-13T00:45:57Z- AHRS_M2 msg: 0.56 phi; -0.36 theta; -0.23 psi 2021-02-13T00:45:57Z- handleDepth()-Depth_Keller msg: 1613177157663 epoch sec; seqNo:15546; depth 8.56 2021-02-13T00:45:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177157.69 epoch sec; seqNo:15546; 4059984.76 north; 593104.15 east 2021-02-13T00:45:58Z- AHRS_M2 msg: 1613177158.06 epoch sec; seqNo:15424 2021-02-13T00:45:58Z- AHRS_M2 msg: 0.56 phi; -0.36 theta; -0.24 psi 2021-02-13T00:45:58Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-13T00:45:58Z- waiting for 1613177155.87, time is 1613177158.06 2021-02-13T00:45:58Z- LcmTrn::updateTrn() >>>> pose time 1613177158.06, meas time 1613177157.27 <<<< 2021-02-13T00:45:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177157.27 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:45:58Z- TerrainNav::motionUpdate() - currEstimate = (4059985, 593104.15, 8.56) 2021-02-13T00:45:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:45:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:45:58Z- TNavBF::Projecting Measurements 2021-02-13T00:45:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:45:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:45:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:45:58Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:45:58Z- TNavFilter::3 good beams. 2021-02-13T00:45:58Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:45:58Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:45:58Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:45:58Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:45:58Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:45:58Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:45:58Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:45:58Z- TNavBF::mmse selected from filter -1 2021-02-13T00:45:58Z- TerrainMapOctree: (vehN, vehE ) = (4059984.29, 593103.78). 2021-02-13T00:45:58Z- TerrainMapOctree: Min Distance = 1707.58. 2021-02-13T00:45:58Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:45:58Z- There is no specific good beam that all particles have in common. 2021-02-13T00:45:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:45:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.50 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:45:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:45:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 18.77 2021-02-13T00:45:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:45:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 25.47 2021-02-13T00:45:58Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:45:58Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:45:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:45:58Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:45:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:45:58Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:45:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:45:58Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:45:58Z- Filter 0 No beams used in subcloud update 2021-02-13T00:45:58Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:45:58Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:45:58Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 688.7 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:45:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177157.27 sec. 2021-02-13T00:45:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:45:58Z- TNavBF::mmse selected from filter -1 2021-02-13T00:45:58Z- variances([0], [2], and psi_berg) = 688.648614 832.992325 0.000000 2021-02-13T00:45:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:45:58Z- LcmTrn::publishEstimates() - MLE : 4059984.26 593125.65 8.56 2021-02-13T00:45:58Z- LcmTrn::publishEstimates() - MMSE : 4059984.76 593104.15 8.56 2021-02-13T00:45:58Z- LcmTrn::publishEstimates() - COVAR: 688.65 832.99 1.27 0.00 2021-02-13T00:45:58Z- handleDepth()-Depth_Keller msg: 1613177158064 epoch sec; seqNo:15547; depth 8.69 2021-02-13T00:45:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177158.07 epoch sec; seqNo:15547; 4059984.76 north; 593104.15 east 2021-02-13T00:45:58Z- AHRS_M2 msg: 1613177158.46 epoch sec; seqNo:15425 2021-02-13T00:45:58Z- AHRS_M2 msg: 0.56 phi; -0.35 theta; -0.25 psi 2021-02-13T00:45:58Z- handleDepth()-Depth_Keller msg: 1613177158477 epoch sec; seqNo:15548; depth 8.82 2021-02-13T00:45:58Z- handleDvl() - RDI_Pathfinder msg: 1613177158.49 epoch sec; seqNo:7447 2021-02-13T00:45:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.82 , 110.62 , 80.88 , 122.52 2021-02-13T00:45:58Z- handleDvl() - RDI_Pathfinder msg: velocities 1.10 , 0.12 , -0.28 2021-02-13T00:45:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177158.50 epoch sec; seqNo:15548; 4059985.14 north; 593104.14 east 2021-02-13T00:45:58Z- AHRS_M2 msg: 1613177158.87 epoch sec; seqNo:15426 2021-02-13T00:45:58Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.25 psi 2021-02-13T00:45:58Z- handleDepth()-Depth_Keller msg: 1613177158873 epoch sec; seqNo:15549; depth 8.97 2021-02-13T00:45:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177158.89 epoch sec; seqNo:15549; 4059985.53 north; 593105.36 east 2021-02-13T00:45:59Z- AHRS_M2 msg: 1613177159.27 epoch sec; seqNo:15427 2021-02-13T00:45:59Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.24 psi 2021-02-13T00:45:59Z- handleDepth()-Depth_Keller msg: 1613177159276 epoch sec; seqNo:15550; depth 9.10 2021-02-13T00:45:59Z- handleDvl() - RDI_Pathfinder msg: 1613177159.29 epoch sec; seqNo:7448 2021-02-13T00:45:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 110.56 , 78.05 , 118.67 2021-02-13T00:45:59Z- handleDvl() - RDI_Pathfinder msg: velocities 1.17 , -0.11 , -0.23 2021-02-13T00:45:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177159.32 epoch sec; seqNo:15550; 4059986.29 north; 593105.35 east 2021-02-13T00:45:59Z- AHRS_M2 msg: 1613177159.68 epoch sec; seqNo:15428 2021-02-13T00:45:59Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.23 psi 2021-02-13T00:45:59Z- handleDepth()-Depth_Keller msg: 1613177159680 epoch sec; seqNo:15551; depth 9.26 2021-02-13T00:45:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:45:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:45:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:45:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:45:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:45:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:45:59Z- handleDvl() - RDI_Pathfinder msg: 1613177159.68 epoch sec; seqNo:7449 2021-02-13T00:45:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:45:59Z- handleDvl() - RDI_Pathfinder msg: velocities 1.17 , -0.11 , -0.23 2021-02-13T00:45:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177159.69 epoch sec; seqNo:15551; 4059986.66 north; 593105.35 east 2021-02-13T00:46:00Z- AHRS_M2 msg: 1613177160.08 epoch sec; seqNo:15429 2021-02-13T00:46:00Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.21 psi 2021-02-13T00:46:00Z- handleDepth()-Depth_Keller msg: 1613177160085 epoch sec; seqNo:15552; depth 9.41 2021-02-13T00:46:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177160.09 epoch sec; seqNo:15552; 4059987.04 north; 593105.34 east 2021-02-13T00:46:00Z- AHRS_M2 msg: 1613177160.48 epoch sec; seqNo:15430 2021-02-13T00:46:00Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.19 psi 2021-02-13T00:46:00Z- handleDepth()-Depth_Keller msg: 1613177160493 epoch sec; seqNo:15553; depth 9.55 2021-02-13T00:46:00Z- handleDvl() - RDI_Pathfinder msg: 1613177160.51 epoch sec; seqNo:7450 2021-02-13T00:46:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 108.33 , 80.07 , 118.74 2021-02-13T00:46:00Z- handleDvl() - RDI_Pathfinder msg: velocities 1.08 , -0.18 , -0.10 2021-02-13T00:46:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177160.52 epoch sec; seqNo:15553; 4059987.42 north; 593105.34 east 2021-02-13T00:46:00Z- AHRS_M2 msg: 1613177160.89 epoch sec; seqNo:15431 2021-02-13T00:46:00Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:46:00Z- handleDepth()-Depth_Keller msg: 1613177160892 epoch sec; seqNo:15554; depth 9.70 2021-02-13T00:46:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:00Z- handleDvl() - RDI_Pathfinder msg: 1613177160.89 epoch sec; seqNo:7451 2021-02-13T00:46:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:00Z- handleDvl() - RDI_Pathfinder msg: velocities 1.08 , -0.18 , -0.10 2021-02-13T00:46:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177160.90 epoch sec; seqNo:15554; 4059987.80 north; 593105.33 east 2021-02-13T00:46:01Z- AHRS_M2 msg: 1613177161.30 epoch sec; seqNo:15432 2021-02-13T00:46:01Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.16 psi 2021-02-13T00:46:01Z- handleDepth()-Depth_Keller msg: 1613177161303 epoch sec; seqNo:15555; depth 9.84 2021-02-13T00:46:01Z- handleDvl() - RDI_Pathfinder msg: 1613177161.32 epoch sec; seqNo:7452 2021-02-13T00:46:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.01 , 100.89 , 80.07 , 115.76 2021-02-13T00:46:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.11 , -0.06 2021-02-13T00:46:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177161.33 epoch sec; seqNo:15555; 4059988.19 north; 593106.55 east 2021-02-13T00:46:01Z- AHRS_M2 msg: 1613177161.70 epoch sec; seqNo:15433 2021-02-13T00:46:01Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.15 psi 2021-02-13T00:46:01Z- handleDepth()-Depth_Keller msg: 1613177161702 epoch sec; seqNo:15556; depth 9.99 2021-02-13T00:46:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177161.71 epoch sec; seqNo:15556; 4059988.19 north; 593106.55 east 2021-02-13T00:46:02Z- AHRS_M2 msg: 1613177162.10 epoch sec; seqNo:15434 2021-02-13T00:46:02Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.14 psi 2021-02-13T00:46:02Z- handleDepth()-Depth_Keller msg: 1613177162105 epoch sec; seqNo:15557; depth 10.15 2021-02-13T00:46:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:02Z- handleDvl() - RDI_Pathfinder msg: 1613177162.11 epoch sec; seqNo:7453 2021-02-13T00:46:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.11 , -0.06 2021-02-13T00:46:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177162.12 epoch sec; seqNo:15557; 4059988.57 north; 593106.55 east 2021-02-13T00:46:02Z- AHRS_M2 msg: 1613177162.50 epoch sec; seqNo:15435 2021-02-13T00:46:02Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.14 psi 2021-02-13T00:46:02Z- handleDepth()-Depth_Keller msg: 1613177162508 epoch sec; seqNo:15558; depth 10.26 2021-02-13T00:46:02Z- handleDvl() - RDI_Pathfinder msg: 1613177162.52 epoch sec; seqNo:7454 2021-02-13T00:46:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 100.08 , 76.02 , 113.60 2021-02-13T00:46:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.05 , -0.06 2021-02-13T00:46:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177162.53 epoch sec; seqNo:15558; 4059988.95 north; 593106.54 east 2021-02-13T00:46:02Z- AHRS_M2 msg: 1613177162.91 epoch sec; seqNo:15436 2021-02-13T00:46:02Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.14 psi 2021-02-13T00:46:02Z- handleDepth()-Depth_Keller msg: 1613177162921 epoch sec; seqNo:15559; depth 10.40 2021-02-13T00:46:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177162.93 epoch sec; seqNo:15559; 4059989.33 north; 593106.54 east 2021-02-13T00:46:03Z- LcmTrn::handleMessages() - handling 46 messages took 0.65 ms and 133 calls... 2021-02-13T00:46:03Z- waiting for 1613177161.06, time is 1613177162.93 2021-02-13T00:46:03Z- LcmTrn::updateTrn() >>>> pose time 1613177162.93, meas time 1613177162.52 <<<< 2021-02-13T00:46:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177162.52 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:46:03Z- TerrainNav::motionUpdate() - currEstimate = (4059989, 593106.54, 10.40) 2021-02-13T00:46:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:46:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:46:03Z- TNavBF::Projecting Measurements 2021-02-13T00:46:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:46:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:46:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:46:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:46:03Z- TNavFilter::4 good beams. 2021-02-13T00:46:03Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:46:03Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:46:03Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:46:03Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:46:03Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:46:03Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:46:03Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:46:03Z- TNavBF::mmse selected from filter -1 2021-02-13T00:46:03Z- TerrainMapOctree: (vehN, vehE ) = (4059988.94, 593106.34). 2021-02-13T00:46:03Z- TerrainMapOctree: Min Distance = 1702.42. 2021-02-13T00:46:03Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:46:03Z- There is no specific good beam that all particles have in common. 2021-02-13T00:46:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:46:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.51 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 11.99 2021-02-13T00:46:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:46:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.36 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 16.02 2021-02-13T00:46:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:46:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 9.25 2021-02-13T00:46:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:46:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 20.65 2021-02-13T00:46:03Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:46:03Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:46:03Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:46:03Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:46:03Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:46:03Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:46:03Z- Filter 0 No beams used in subcloud update 2021-02-13T00:46:03Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:46:03Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:46:03Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 688.6 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:46:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177162.52 sec. 2021-02-13T00:46:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:46:03Z- TNavBF::mmse selected from filter -1 2021-02-13T00:46:03Z- variances([0], [2], and psi_berg) = 688.580576 832.992293 0.000000 2021-02-13T00:46:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:46:03Z- LcmTrn::publishEstimates() - MLE : 4059988.83 593128.04 10.40 2021-02-13T00:46:03Z- LcmTrn::publishEstimates() - MMSE : 4059989.33 593106.54 10.40 2021-02-13T00:46:03Z- LcmTrn::publishEstimates() - COVAR: 688.58 832.99 1.10 0.00 2021-02-13T00:46:03Z- AHRS_M2 msg: 1613177163.32 epoch sec; seqNo:15437 2021-02-13T00:46:03Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.15 psi 2021-02-13T00:46:03Z- handleDepth()-Depth_Keller msg: 1613177163319 epoch sec; seqNo:15560; depth 10.55 2021-02-13T00:46:03Z- handleDvl() - RDI_Pathfinder msg: 1613177163.34 epoch sec; seqNo:7455 2021-02-13T00:46:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.69 , 100.21 , 78.58 , 116.44 2021-02-13T00:46:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.02 , -0.07 2021-02-13T00:46:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177163.35 epoch sec; seqNo:15560; 4059989.33 north; 593106.54 east 2021-02-13T00:46:03Z- AHRS_M2 msg: 1613177163.72 epoch sec; seqNo:15438 2021-02-13T00:46:03Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.15 psi 2021-02-13T00:46:03Z- handleDepth()-Depth_Keller msg: 1613177163725 epoch sec; seqNo:15561; depth 10.67 2021-02-13T00:46:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177163.75 epoch sec; seqNo:15561; 4059989.71 north; 593106.53 east 2021-02-13T00:46:04Z- AHRS_M2 msg: 1613177164.12 epoch sec; seqNo:15439 2021-02-13T00:46:04Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.15 psi 2021-02-13T00:46:04Z- handleDepth()-Depth_Keller msg: 1613177164130 epoch sec; seqNo:15562; depth 10.80 2021-02-13T00:46:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177164.15 epoch sec; seqNo:15562; 4059990.09 north; 593106.53 east 2021-02-13T00:46:04Z- AHRS_M2 msg: 1613177164.53 epoch sec; seqNo:15440 2021-02-13T00:46:04Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.15 psi 2021-02-13T00:46:04Z- handleDepth()-Depth_Keller msg: 1613177164537 epoch sec; seqNo:15563; depth 10.92 2021-02-13T00:46:04Z- handleDvl() - RDI_Pathfinder msg: 1613177164.55 epoch sec; seqNo:7456 2021-02-13T00:46:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.28 , 100.15 , 75.19 , 110.96 2021-02-13T00:46:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.00 , -0.05 2021-02-13T00:46:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177164.56 epoch sec; seqNo:15563; 4059990.10 north; 593107.75 east 2021-02-13T00:46:04Z- AHRS_M2 msg: 1613177164.93 epoch sec; seqNo:15441 2021-02-13T00:46:04Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.16 psi 2021-02-13T00:46:04Z- handleDepth()-Depth_Keller msg: 1613177164933 epoch sec; seqNo:15564; depth 11.03 2021-02-13T00:46:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177164.95 epoch sec; seqNo:15564; 4059990.48 north; 593107.75 east 2021-02-13T00:46:05Z- AHRS_M2 msg: 1613177165.34 epoch sec; seqNo:15442 2021-02-13T00:46:05Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.19 psi 2021-02-13T00:46:05Z- handleDepth()-Depth_Keller msg: 1613177165342 epoch sec; seqNo:15565; depth 11.17 2021-02-13T00:46:05Z- handleDvl() - RDI_Pathfinder msg: 1613177165.36 epoch sec; seqNo:7457 2021-02-13T00:46:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 100.35 , 76.43 , 111.16 2021-02-13T00:46:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , -0.01 2021-02-13T00:46:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177165.37 epoch sec; seqNo:15565; 4059990.86 north; 593107.74 east 2021-02-13T00:46:05Z- AHRS_M2 msg: 1613177165.74 epoch sec; seqNo:15443 2021-02-13T00:46:05Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.20 psi 2021-02-13T00:46:05Z- handleDepth()-Depth_Keller msg: 1613177165744 epoch sec; seqNo:15566; depth 11.27 2021-02-13T00:46:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:05Z- handleDvl() - RDI_Pathfinder msg: 1613177165.75 epoch sec; seqNo:7458 2021-02-13T00:46:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , -0.01 2021-02-13T00:46:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177165.76 epoch sec; seqNo:15566; 4059991.24 north; 593107.74 east 2021-02-13T00:46:06Z- AHRS_M2 msg: 1613177166.16 epoch sec; seqNo:15444 2021-02-13T00:46:06Z- AHRS_M2 msg: 0.58 phi; -0.27 theta; -0.20 psi 2021-02-13T00:46:06Z- handleDepth()-Depth_Keller msg: 1613177166168 epoch sec; seqNo:15567; depth 11.37 2021-02-13T00:46:06Z- handleDvl() - RDI_Pathfinder msg: 1613177166.19 epoch sec; seqNo:7459 2021-02-13T00:46:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 101.70 , 76.02 , 111.16 2021-02-13T00:46:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , 0.12 2021-02-13T00:46:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177166.20 epoch sec; seqNo:15567; 4059991.24 north; 593107.74 east 2021-02-13T00:46:06Z- AHRS_M2 msg: 1613177166.54 epoch sec; seqNo:15445 2021-02-13T00:46:06Z- AHRS_M2 msg: 0.58 phi; -0.27 theta; -0.20 psi 2021-02-13T00:46:06Z- handleDepth()-Depth_Keller msg: 1613177166553 epoch sec; seqNo:15568; depth 11.50 2021-02-13T00:46:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177166.56 epoch sec; seqNo:15568; 4059991.62 north; 593107.73 east 2021-02-13T00:46:06Z- AHRS_M2 msg: 1613177166.95 epoch sec; seqNo:15446 2021-02-13T00:46:06Z- AHRS_M2 msg: 0.57 phi; -0.27 theta; -0.19 psi 2021-02-13T00:46:06Z- handleDepth()-Depth_Keller msg: 1613177166952 epoch sec; seqNo:15569; depth 11.60 2021-02-13T00:46:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:06Z- handleDvl() - RDI_Pathfinder msg: 1613177166.95 epoch sec; seqNo:7460 2021-02-13T00:46:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , 0.12 2021-02-13T00:46:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177166.96 epoch sec; seqNo:15569; 4059991.99 north; 593107.73 east 2021-02-13T00:46:07Z- AHRS_M2 msg: 1613177167.36 epoch sec; seqNo:15447 2021-02-13T00:46:07Z- AHRS_M2 msg: 0.57 phi; -0.27 theta; -0.18 psi 2021-02-13T00:46:07Z- handleDepth()-Depth_Keller msg: 1613177167364 epoch sec; seqNo:15570; depth 11.71 2021-02-13T00:46:07Z- handleDvl() - RDI_Pathfinder msg: 1613177167.38 epoch sec; seqNo:7461 2021-02-13T00:46:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 101.70 , 0.00 , 108.46 2021-02-13T00:46:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , 0.22 2021-02-13T00:46:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177167.39 epoch sec; seqNo:15570; 4059992.39 north; 593108.95 east 2021-02-13T00:46:07Z- AHRS_M2 msg: 1613177167.76 epoch sec; seqNo:15448 2021-02-13T00:46:07Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.18 psi 2021-02-13T00:46:07Z- handleDepth()-Depth_Keller msg: 1613177167762 epoch sec; seqNo:15571; depth 11.83 2021-02-13T00:46:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177167.78 epoch sec; seqNo:15571; 4059992.39 north; 593108.95 east 2021-02-13T00:46:08Z- AHRS_M2 msg: 1613177168.16 epoch sec; seqNo:15449 2021-02-13T00:46:08Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.18 psi 2021-02-13T00:46:08Z- handleDepth()-Depth_Keller msg: 1613177168165 epoch sec; seqNo:15572; depth 11.95 2021-02-13T00:46:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:08Z- handleDvl() - RDI_Pathfinder msg: 1613177168.17 epoch sec; seqNo:7462 2021-02-13T00:46:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , 0.22 2021-02-13T00:46:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177168.18 epoch sec; seqNo:15572; 4059992.77 north; 593108.94 east 2021-02-13T00:46:08Z- LcmTrn::handleMessages() - handling 47 messages took 0.73 ms and 133 calls... 2021-02-13T00:46:08Z- waiting for 1613177165.93, time is 1613177168.18 2021-02-13T00:46:08Z- LcmTrn::updateTrn() equating pose 1613177168.18 and meas times 1613177168.18 2021-02-13T00:46:08Z- LcmTrn::updateTrn() >>>> pose time 1613177168.18, meas time 1613177168.18 <<<< 2021-02-13T00:46:08Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177168.18, ping # 0. 2021-02-13T00:46:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177168.18 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:46:08Z- TerrainNav::motionUpdate() - currEstimate = (4059993, 593108.94, 11.95) 2021-02-13T00:46:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:46:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:46:08Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:46:08Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:46:08Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177168.18 sec.; No relevant navigation data available 2021-02-13T00:46:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:46:08Z- TNavBF::mmse selected from filter -1 2021-02-13T00:46:08Z- variances([0], [2], and psi_berg) = 688.531452 832.992270 0.000000 2021-02-13T00:46:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:46:08Z- LcmTrn::publishEstimates() - MLE : 4059992.27 593130.44 11.95 2021-02-13T00:46:08Z- LcmTrn::publishEstimates() - MMSE : 4059992.77 593108.94 11.95 2021-02-13T00:46:08Z- LcmTrn::publishEstimates() - COVAR: 688.53 832.99 1.31 0.00 2021-02-13T00:46:08Z- AHRS_M2 msg: 1613177168.56 epoch sec; seqNo:15450 2021-02-13T00:46:08Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.18 psi 2021-02-13T00:46:08Z- handleDepth()-Depth_Keller msg: 1613177168569 epoch sec; seqNo:15573; depth 12.05 2021-02-13T00:46:08Z- handleDvl() - RDI_Pathfinder msg: 1613177168.58 epoch sec; seqNo:7463 2021-02-13T00:46:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 99.94 , 78.31 , 109.41 2021-02-13T00:46:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.20 , 0.17 2021-02-13T00:46:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177168.59 epoch sec; seqNo:15573; 4059993.14 north; 593108.94 east 2021-02-13T00:46:08Z- AHRS_M2 msg: 1613177168.97 epoch sec; seqNo:15451 2021-02-13T00:46:08Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.18 psi 2021-02-13T00:46:08Z- handleDepth()-Depth_Keller msg: 1613177168974 epoch sec; seqNo:15574; depth 12.16 2021-02-13T00:46:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177168.98 epoch sec; seqNo:15574; 4059993.14 north; 593108.94 east 2021-02-13T00:46:09Z- AHRS_M2 msg: 1613177169.37 epoch sec; seqNo:15452 2021-02-13T00:46:09Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.18 psi 2021-02-13T00:46:09Z- handleDepth()-Depth_Keller msg: 1613177169376 epoch sec; seqNo:15575; depth 12.24 2021-02-13T00:46:09Z- handleDvl() - RDI_Pathfinder msg: 1613177169.39 epoch sec; seqNo:7464 2021-02-13T00:46:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.86 , 100.08 , 78.45 , 108.19 2021-02-13T00:46:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.24 , -0.01 2021-02-13T00:46:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177169.41 epoch sec; seqNo:15575; 4059993.54 north; 593110.16 east 2021-02-13T00:46:09Z- AHRS_M2 msg: 1613177169.78 epoch sec; seqNo:15453 2021-02-13T00:46:09Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.17 psi 2021-02-13T00:46:09Z- handleDepth()-Depth_Keller msg: 1613177169785 epoch sec; seqNo:15576; depth 12.37 2021-02-13T00:46:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177169.80 epoch sec; seqNo:15576; 4059993.91 north; 593110.15 east 2021-02-13T00:46:10Z- AHRS_M2 msg: 1613177170.19 epoch sec; seqNo:15454 2021-02-13T00:46:10Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.18 psi 2021-02-13T00:46:10Z- handleDepth()-Depth_Keller msg: 1613177170192 epoch sec; seqNo:15577; depth 12.48 2021-02-13T00:46:10Z- handleDvl() - RDI_Pathfinder msg: 1613177170.20 epoch sec; seqNo:7465 2021-02-13T00:46:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 100.35 , 76.83 , 108.46 2021-02-13T00:46:10Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.11 , -0.18 2021-02-13T00:46:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177170.22 epoch sec; seqNo:15577; 4059993.91 north; 593110.15 east 2021-02-13T00:46:10Z- AHRS_M2 msg: 1613177170.58 epoch sec; seqNo:15455 2021-02-13T00:46:10Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.18 psi 2021-02-13T00:46:10Z- handleDepth()-Depth_Keller msg: 1613177170597 epoch sec; seqNo:15578; depth 12.59 2021-02-13T00:46:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:10Z- handleDvl() - RDI_Pathfinder msg: 1613177170.60 epoch sec; seqNo:7466 2021-02-13T00:46:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:10Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.11 , -0.18 2021-02-13T00:46:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177170.61 epoch sec; seqNo:15578; 4059994.29 north; 593110.15 east 2021-02-13T00:46:10Z- AHRS_M2 msg: 1613177170.99 epoch sec; seqNo:15456 2021-02-13T00:46:10Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.19 psi 2021-02-13T00:46:10Z- handleDepth()-Depth_Keller msg: 1613177170992 epoch sec; seqNo:15579; depth 12.68 2021-02-13T00:46:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177171.01 epoch sec; seqNo:15579; 4059994.69 north; 593111.36 east 2021-02-13T00:46:11Z- AHRS_M2 msg: 1613177171.39 epoch sec; seqNo:15457 2021-02-13T00:46:11Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.20 psi 2021-02-13T00:46:11Z- handleDepth()-Depth_Keller msg: 1613177171396 epoch sec; seqNo:15580; depth 12.80 2021-02-13T00:46:11Z- handleDvl() - RDI_Pathfinder msg: 1613177171.41 epoch sec; seqNo:7467 2021-02-13T00:46:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.48 , 101.77 , 78.78 , 108.53 2021-02-13T00:46:11Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.07 , -0.19 2021-02-13T00:46:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177171.44 epoch sec; seqNo:15580; 4059995.06 north; 593111.36 east 2021-02-13T00:46:11Z- AHRS_M2 msg: 1613177171.80 epoch sec; seqNo:15458 2021-02-13T00:46:11Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.20 psi 2021-02-13T00:46:11Z- handleDepth()-Depth_Keller msg: 1613177171805 epoch sec; seqNo:15581; depth 12.90 2021-02-13T00:46:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:11Z- handleDvl() - RDI_Pathfinder msg: 1613177171.81 epoch sec; seqNo:7468 2021-02-13T00:46:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:11Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.07 , -0.19 2021-02-13T00:46:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177171.82 epoch sec; seqNo:15581; 4059995.44 north; 593111.36 east 2021-02-13T00:46:12Z- AHRS_M2 msg: 1613177172.20 epoch sec; seqNo:15459 2021-02-13T00:46:12Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.19 psi 2021-02-13T00:46:12Z- handleDepth()-Depth_Keller msg: 1613177172204 epoch sec; seqNo:15582; depth 13.02 2021-02-13T00:46:12Z- handleDvl() - RDI_Pathfinder msg: 1613177172.22 epoch sec; seqNo:7469 2021-02-13T00:46:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.87 , 100.35 , 78.72 , 107.11 2021-02-13T00:46:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.14 , -0.11 2021-02-13T00:46:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177172.23 epoch sec; seqNo:15582; 4059995.82 north; 593111.35 east 2021-02-13T00:46:12Z- AHRS_M2 msg: 1613177172.60 epoch sec; seqNo:15460 2021-02-13T00:46:12Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.19 psi 2021-02-13T00:46:12Z- handleDepth()-Depth_Keller msg: 1613177172613 epoch sec; seqNo:15583; depth 13.13 2021-02-13T00:46:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177172.62 epoch sec; seqNo:15583; 4059996.20 north; 593111.35 east 2021-02-13T00:46:13Z- AHRS_M2 msg: 1613177173.01 epoch sec; seqNo:15461 2021-02-13T00:46:13Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.18 psi 2021-02-13T00:46:13Z- handleDepth()-Depth_Keller msg: 1613177173014 epoch sec; seqNo:15584; depth 13.22 2021-02-13T00:46:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:13Z- handleDvl() - RDI_Pathfinder msg: 1613177173.02 epoch sec; seqNo:7470 2021-02-13T00:46:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.14 , -0.11 2021-02-13T00:46:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177173.02 epoch sec; seqNo:15584; 4059996.58 north; 593111.34 east 2021-02-13T00:46:13Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:46:13Z- waiting for 1613177171.18, time is 1613177173.02 2021-02-13T00:46:13Z- LcmTrn::updateTrn() equating pose 1613177173.02 and meas times 1613177173.02 2021-02-13T00:46:13Z- LcmTrn::updateTrn() >>>> pose time 1613177173.02, meas time 1613177173.02 <<<< 2021-02-13T00:46:13Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177173.02, ping # 0. 2021-02-13T00:46:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177173.02 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:46:13Z- TerrainNav::motionUpdate() - currEstimate = (4059997, 593111.34, 13.22) 2021-02-13T00:46:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:46:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:46:13Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:46:13Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:46:13Z- TNavBF::Projecting Measurements 2021-02-13T00:46:13Z- TNavFilter::Measurement from time = 1613177173.02 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:46:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:46:13Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:46:13Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:46:13Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:46:13Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:46:13Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:46:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:46:13Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 688.5 EastVariance: 833.0 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-13T00:46:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:46:13Z- TNavBF::mmse selected from filter -1 2021-02-13T00:46:13Z- variances([0], [2], and psi_berg) = 688.480551 832.992246 0.000000 2021-02-13T00:46:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:46:13Z- LcmTrn::publishEstimates() - MLE : 4059996.08 593131.84 13.22 2021-02-13T00:46:13Z- LcmTrn::publishEstimates() - MMSE : 4059996.58 593111.34 13.22 2021-02-13T00:46:13Z- LcmTrn::publishEstimates() - COVAR: 688.48 832.99 1.54 0.00 2021-02-13T00:46:13Z- AHRS_M2 msg: 1613177173.41 epoch sec; seqNo:15462 2021-02-13T00:46:13Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.18 psi 2021-02-13T00:46:13Z- handleDepth()-Depth_Keller msg: 1613177173416 epoch sec; seqNo:15585; depth 13.33 2021-02-13T00:46:13Z- handleDvl() - RDI_Pathfinder msg: 1613177173.43 epoch sec; seqNo:7471 2021-02-13T00:46:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 100.08 , 78.18 , 107.38 2021-02-13T00:46:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.11 , -0.03 2021-02-13T00:46:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177173.44 epoch sec; seqNo:15585; 4059996.96 north; 593111.34 east 2021-02-13T00:46:13Z- AHRS_M2 msg: 1613177173.82 epoch sec; seqNo:15463 2021-02-13T00:46:13Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.17 psi 2021-02-13T00:46:13Z- handleDepth()-Depth_Keller msg: 1613177173820 epoch sec; seqNo:15586; depth 13.45 2021-02-13T00:46:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177173.83 epoch sec; seqNo:15586; 4059996.96 north; 593111.34 east 2021-02-13T00:46:14Z- AHRS_M2 msg: 1613177174.22 epoch sec; seqNo:15464 2021-02-13T00:46:14Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.16 psi 2021-02-13T00:46:14Z- handleDepth()-Depth_Keller msg: 1613177174229 epoch sec; seqNo:15587; depth 13.55 2021-02-13T00:46:14Z- handleDvl() - RDI_Pathfinder msg: 1613177174.24 epoch sec; seqNo:7472 2021-02-13T00:46:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 97.64 , 75.20 , 107.38 2021-02-13T00:46:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , -0.01 2021-02-13T00:46:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177174.25 epoch sec; seqNo:15587; 4059997.35 north; 593112.56 east 2021-02-13T00:46:14Z- AHRS_M2 msg: 1613177174.62 epoch sec; seqNo:15465 2021-02-13T00:46:14Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.16 psi 2021-02-13T00:46:14Z- handleDepth()-Depth_Keller msg: 1613177174637 epoch sec; seqNo:15588; depth 13.65 2021-02-13T00:46:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177174.65 epoch sec; seqNo:15588; 4059997.73 north; 593112.55 east 2021-02-13T00:46:15Z- AHRS_M2 msg: 1613177175.03 epoch sec; seqNo:15466 2021-02-13T00:46:15Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.17 psi 2021-02-13T00:46:15Z- handleDepth()-Depth_Keller msg: 1613177175032 epoch sec; seqNo:15589; depth 13.76 2021-02-13T00:46:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177175.05 epoch sec; seqNo:15589; 4059998.11 north; 593112.55 east 2021-02-13T00:46:15Z- AHRS_M2 msg: 1613177175.43 epoch sec; seqNo:15467 2021-02-13T00:46:15Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.17 psi 2021-02-13T00:46:15Z- handleDepth()-Depth_Keller msg: 1613177175439 epoch sec; seqNo:15590; depth 13.88 2021-02-13T00:46:15Z- handleDvl() - RDI_Pathfinder msg: 1613177175.45 epoch sec; seqNo:7473 2021-02-13T00:46:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 97.71 , 0.00 , 105.01 2021-02-13T00:46:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , -0.03 2021-02-13T00:46:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177175.47 epoch sec; seqNo:15590; 4059998.49 north; 593112.54 east 2021-02-13T00:46:15Z- AHRS_M2 msg: 1613177175.84 epoch sec; seqNo:15468 2021-02-13T00:46:15Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.18 psi 2021-02-13T00:46:15Z- handleDepth()-Depth_Keller msg: 1613177175845 epoch sec; seqNo:15591; depth 13.99 2021-02-13T00:46:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177175.86 epoch sec; seqNo:15591; 4059998.49 north; 593112.54 east 2021-02-13T00:46:16Z- AHRS_M2 msg: 1613177176.24 epoch sec; seqNo:15469 2021-02-13T00:46:16Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.19 psi 2021-02-13T00:46:16Z- handleDepth()-Depth_Keller msg: 1613177176246 epoch sec; seqNo:15592; depth 14.08 2021-02-13T00:46:16Z- handleDvl() - RDI_Pathfinder msg: 1613177176.26 epoch sec; seqNo:7474 2021-02-13T00:46:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.86 , 100.14 , 74.38 , 105.55 2021-02-13T00:46:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , 0.00 2021-02-13T00:46:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177176.27 epoch sec; seqNo:15592; 4059998.87 north; 593112.54 east 2021-02-13T00:46:16Z- AHRS_M2 msg: 1613177176.64 epoch sec; seqNo:15470 2021-02-13T00:46:16Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.19 psi 2021-02-13T00:46:16Z- handleDepth()-Depth_Keller msg: 1613177176653 epoch sec; seqNo:15593; depth 14.21 2021-02-13T00:46:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:16Z- handleDvl() - RDI_Pathfinder msg: 1613177176.65 epoch sec; seqNo:7475 2021-02-13T00:46:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , 0.00 2021-02-13T00:46:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177176.67 epoch sec; seqNo:15593; 4059999.26 north; 593113.76 east 2021-02-13T00:46:17Z- AHRS_M2 msg: 1613177177.05 epoch sec; seqNo:15471 2021-02-13T00:46:17Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.19 psi 2021-02-13T00:46:17Z- handleDepth()-Depth_Keller msg: 1613177177056 epoch sec; seqNo:15594; depth 14.31 2021-02-13T00:46:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177177.07 epoch sec; seqNo:15594; 4059999.64 north; 593113.75 east 2021-02-13T00:46:17Z- AHRS_M2 msg: 1613177177.45 epoch sec; seqNo:15472 2021-02-13T00:46:17Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.18 psi 2021-02-13T00:46:17Z- handleDepth()-Depth_Keller msg: 1613177177456 epoch sec; seqNo:15595; depth 14.44 2021-02-13T00:46:17Z- handleDvl() - RDI_Pathfinder msg: 1613177177.47 epoch sec; seqNo:7476 2021-02-13T00:46:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.52 , 96.56 , 78.31 , 105.08 2021-02-13T00:46:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , 0.05 2021-02-13T00:46:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177177.48 epoch sec; seqNo:15595; 4060000.02 north; 593113.75 east 2021-02-13T00:46:17Z- AHRS_M2 msg: 1613177177.86 epoch sec; seqNo:15473 2021-02-13T00:46:17Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.18 psi 2021-02-13T00:46:17Z- handleDepth()-Depth_Keller msg: 1613177177865 epoch sec; seqNo:15596; depth 14.54 2021-02-13T00:46:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:17Z- handleDvl() - RDI_Pathfinder msg: 1613177177.87 epoch sec; seqNo:7477 2021-02-13T00:46:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , 0.05 2021-02-13T00:46:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177177.88 epoch sec; seqNo:15596; 4060000.02 north; 593113.75 east 2021-02-13T00:46:18Z- AHRS_M2 msg: 1613177178.28 epoch sec; seqNo:15474 2021-02-13T00:46:18Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.18 psi 2021-02-13T00:46:18Z- handleDepth()-Depth_Keller msg: 1613177178286 epoch sec; seqNo:15597; depth 14.65 2021-02-13T00:46:18Z- handleDvl() - RDI_Pathfinder msg: 1613177178.30 epoch sec; seqNo:7478 2021-02-13T00:46:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 97.78 , 78.31 , 106.30 2021-02-13T00:46:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , 0.08 2021-02-13T00:46:18Z- LcmTrn::handleMessages() - handling 46 messages took 0.70 ms and 132 calls... 2021-02-13T00:46:18Z- waiting for 1613177176.02, time is 1613177178.30 2021-02-13T00:46:18Z- LcmTrn::updateTrn() equating pose 1613177178.30 and meas times 1613177178.30 2021-02-13T00:46:18Z- LcmTrn::updateTrn() >>>> pose time 1613177178.30, meas time 1613177178.30 <<<< 2021-02-13T00:46:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177178.30 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:46:18Z- TerrainNav::motionUpdate() - currEstimate = (4060000, 593113.75, 14.65) 2021-02-13T00:46:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:46:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:46:18Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177178.30 sec.; No relevant navigation data available 2021-02-13T00:46:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:46:18Z- TNavBF::mmse selected from filter -1 2021-02-13T00:46:18Z- variances([0], [2], and psi_berg) = 688.431477 832.992224 0.000000 2021-02-13T00:46:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:46:18Z- LcmTrn::publishEstimates() - MLE : 4059999.52 593134.25 14.65 2021-02-13T00:46:18Z- LcmTrn::publishEstimates() - MMSE : 4060000.02 593113.75 14.65 2021-02-13T00:46:18Z- LcmTrn::publishEstimates() - COVAR: 688.43 832.99 1.76 0.00 2021-02-13T00:46:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177178.31 epoch sec; seqNo:15597; 4060000.40 north; 593113.74 east 2021-02-13T00:46:18Z- AHRS_M2 msg: 1613177178.66 epoch sec; seqNo:15475 2021-02-13T00:46:18Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.17 psi 2021-02-13T00:46:18Z- handleDepth()-Depth_Keller msg: 1613177178673 epoch sec; seqNo:15598; depth 14.75 2021-02-13T00:46:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177178.69 epoch sec; seqNo:15598; 4060000.77 north; 593113.74 east 2021-02-13T00:46:19Z- AHRS_M2 msg: 1613177179.07 epoch sec; seqNo:15476 2021-02-13T00:46:19Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.17 psi 2021-02-13T00:46:19Z- handleDepth()-Depth_Keller msg: 1613177179072 epoch sec; seqNo:15599; depth 14.86 2021-02-13T00:46:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:19Z- handleDvl() - RDI_Pathfinder msg: 1613177179.07 epoch sec; seqNo:7479 2021-02-13T00:46:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , 0.08 2021-02-13T00:46:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177179.08 epoch sec; seqNo:15599; 4060000.77 north; 593113.74 east 2021-02-13T00:46:19Z- AHRS_M2 msg: 1613177179.47 epoch sec; seqNo:15477 2021-02-13T00:46:19Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.17 psi 2021-02-13T00:46:19Z- handleDepth()-Depth_Keller msg: 1613177179481 epoch sec; seqNo:15600; depth 14.96 2021-02-13T00:46:19Z- handleDvl() - RDI_Pathfinder msg: 1613177179.49 epoch sec; seqNo:7480 2021-02-13T00:46:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.67 , 97.71 , 73.56 , 106.23 2021-02-13T00:46:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , 0.07 2021-02-13T00:46:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177179.50 epoch sec; seqNo:15600; 4060001.15 north; 593113.73 east 2021-02-13T00:46:19Z- AHRS_M2 msg: 1613177179.88 epoch sec; seqNo:15478 2021-02-13T00:46:19Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.18 psi 2021-02-13T00:46:19Z- handleDepth()-Depth_Keller msg: 1613177179880 epoch sec; seqNo:15601; depth 15.06 2021-02-13T00:46:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177179.90 epoch sec; seqNo:15601; 4060001.54 north; 593114.95 east 2021-02-13T00:46:20Z- AHRS_M2 msg: 1613177180.28 epoch sec; seqNo:15479 2021-02-13T00:46:20Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.18 psi 2021-02-13T00:46:20Z- handleDepth()-Depth_Keller msg: 1613177180284 epoch sec; seqNo:15602; depth 15.16 2021-02-13T00:46:20Z- handleDvl() - RDI_Pathfinder msg: 1613177180.30 epoch sec; seqNo:7481 2021-02-13T00:46:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 97.78 , 0.00 , 107.51 2021-02-13T00:46:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.04 , 0.06 2021-02-13T00:46:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177180.31 epoch sec; seqNo:15602; 4060001.92 north; 593114.95 east 2021-02-13T00:46:20Z- AHRS_M2 msg: 1613177180.68 epoch sec; seqNo:15480 2021-02-13T00:46:20Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.17 psi 2021-02-13T00:46:20Z- handleDepth()-Depth_Keller msg: 1613177180693 epoch sec; seqNo:15603; depth 15.27 2021-02-13T00:46:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177180.70 epoch sec; seqNo:15603; 4060001.92 north; 593114.95 east 2021-02-13T00:46:21Z- AHRS_M2 msg: 1613177181.09 epoch sec; seqNo:15481 2021-02-13T00:46:21Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.17 psi 2021-02-13T00:46:21Z- handleDepth()-Depth_Keller msg: 1613177181094 epoch sec; seqNo:15604; depth 15.37 2021-02-13T00:46:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177181.10 epoch sec; seqNo:15604; 4060002.30 north; 593114.94 east 2021-02-13T00:46:21Z- AHRS_M2 msg: 1613177181.49 epoch sec; seqNo:15482 2021-02-13T00:46:21Z- AHRS_M2 msg: 0.61 phi; -0.28 theta; -0.17 psi 2021-02-13T00:46:21Z- handleDepth()-Depth_Keller msg: 1613177181496 epoch sec; seqNo:15605; depth 15.48 2021-02-13T00:46:21Z- handleDvl() - RDI_Pathfinder msg: 1613177181.52 epoch sec; seqNo:7482 2021-02-13T00:46:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.86 , 97.44 , 78.51 , 104.20 2021-02-13T00:46:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.06 , 0.01 2021-02-13T00:46:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177181.53 epoch sec; seqNo:15605; 4060002.68 north; 593114.94 east 2021-02-13T00:46:21Z- AHRS_M2 msg: 1613177181.89 epoch sec; seqNo:15483 2021-02-13T00:46:21Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.17 psi 2021-02-13T00:46:21Z- handleDepth()-Depth_Keller msg: 1613177181905 epoch sec; seqNo:15606; depth 15.58 2021-02-13T00:46:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177181.93 epoch sec; seqNo:15606; 4060003.07 north; 593116.16 east 2021-02-13T00:46:22Z- AHRS_M2 msg: 1613177182.31 epoch sec; seqNo:15484 2021-02-13T00:46:22Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.16 psi 2021-02-13T00:46:22Z- handleDepth()-Depth_Keller msg: 1613177182310 epoch sec; seqNo:15607; depth 15.70 2021-02-13T00:46:22Z- handleDvl() - RDI_Pathfinder msg: 1613177182.32 epoch sec; seqNo:7483 2021-02-13T00:46:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.78 , 78.31 , 103.86 2021-02-13T00:46:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , -0.06 2021-02-13T00:46:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177182.33 epoch sec; seqNo:15607; 4060003.07 north; 593116.16 east 2021-02-13T00:46:22Z- AHRS_M2 msg: 1613177182.72 epoch sec; seqNo:15485 2021-02-13T00:46:22Z- AHRS_M2 msg: 0.63 phi; -0.29 theta; -0.16 psi 2021-02-13T00:46:22Z- handleDepth()-Depth_Keller msg: 1613177182725 epoch sec; seqNo:15608; depth 15.81 2021-02-13T00:46:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:22Z- handleDvl() - RDI_Pathfinder msg: 1613177182.73 epoch sec; seqNo:7484 2021-02-13T00:46:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , -0.06 2021-02-13T00:46:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177182.74 epoch sec; seqNo:15608; 4060003.45 north; 593116.15 east 2021-02-13T00:46:23Z- AHRS_M2 msg: 1613177183.11 epoch sec; seqNo:15486 2021-02-13T00:46:23Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.15 psi 2021-02-13T00:46:23Z- handleDepth()-Depth_Keller msg: 1613177183112 epoch sec; seqNo:15609; depth 15.92 2021-02-13T00:46:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177183.13 epoch sec; seqNo:15609; 4060003.83 north; 593116.15 east 2021-02-13T00:46:23Z- LcmTrn::handleMessages() - handling 43 messages took 0.61 ms and 131 calls... 2021-02-13T00:46:23Z- waiting for 1613177181.30, time is 1613177183.13 2021-02-13T00:46:23Z- LcmTrn::updateTrn() >>>> pose time 1613177183.13, meas time 1613177182.73 <<<< 2021-02-13T00:46:23Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177182.73, ping # 0. 2021-02-13T00:46:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177182.73 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:46:23Z- TerrainNav::motionUpdate() - currEstimate = (4060004, 593116.15, 15.92) 2021-02-13T00:46:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:46:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:46:23Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:46:23Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:46:23Z- TNavBF::Projecting Measurements 2021-02-13T00:46:23Z- TNavFilter::Measurement from time = 1613177182.73 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:46:23Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:46:23Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:46:23Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:46:23Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:46:23Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:46:23Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:46:23Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:46:23Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 688.4 EastVariance: 833.0 DepthVariance: 2.0 this->DepthBiasCov: 2.0 2021-02-13T00:46:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:46:23Z- TNavBF::mmse selected from filter -1 2021-02-13T00:46:23Z- variances([0], [2], and psi_berg) = 688.368148 832.992194 0.000000 2021-02-13T00:46:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:46:23Z- LcmTrn::publishEstimates() - MLE : 4060003.33 593137.65 15.92 2021-02-13T00:46:23Z- LcmTrn::publishEstimates() - MMSE : 4060003.83 593116.15 15.92 2021-02-13T00:46:23Z- LcmTrn::publishEstimates() - COVAR: 688.37 832.99 1.99 0.00 2021-02-13T00:46:23Z- AHRS_M2 msg: 1613177183.51 epoch sec; seqNo:15487 2021-02-13T00:46:23Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.15 psi 2021-02-13T00:46:23Z- handleDepth()-Depth_Keller msg: 1613177183516 epoch sec; seqNo:15610; depth 16.01 2021-02-13T00:46:23Z- handleDvl() - RDI_Pathfinder msg: 1613177183.53 epoch sec; seqNo:7485 2021-02-13T00:46:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.00 , 97.57 , 78.64 , 104.34 2021-02-13T00:46:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , -0.09 2021-02-13T00:46:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177183.55 epoch sec; seqNo:15610; 4060004.21 north; 593116.14 east 2021-02-13T00:46:23Z- AHRS_M2 msg: 1613177183.92 epoch sec; seqNo:15488 2021-02-13T00:46:23Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.15 psi 2021-02-13T00:46:23Z- handleDepth()-Depth_Keller msg: 1613177183920 epoch sec; seqNo:15611; depth 16.14 2021-02-13T00:46:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:23Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:23Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:23Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:23Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:23Z- handleDvl() - RDI_Pathfinder msg: 1613177183.92 epoch sec; seqNo:7486 2021-02-13T00:46:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , -0.09 2021-02-13T00:46:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177183.94 epoch sec; seqNo:15611; 4060004.59 north; 593116.14 east 2021-02-13T00:46:24Z- AHRS_M2 msg: 1613177184.32 epoch sec; seqNo:15489 2021-02-13T00:46:24Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.14 psi 2021-02-13T00:46:24Z- handleDepth()-Depth_Keller msg: 1613177184333 epoch sec; seqNo:15612; depth 16.24 2021-02-13T00:46:24Z- handleDvl() - RDI_Pathfinder msg: 1613177184.35 epoch sec; seqNo:7487 2021-02-13T00:46:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 97.78 , 70.29 , 103.86 2021-02-13T00:46:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , -0.06 2021-02-13T00:46:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177184.36 epoch sec; seqNo:15612; 4060004.98 north; 593117.35 east 2021-02-13T00:46:24Z- AHRS_M2 msg: 1613177184.72 epoch sec; seqNo:15490 2021-02-13T00:46:24Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.15 psi 2021-02-13T00:46:24Z- handleDepth()-Depth_Keller msg: 1613177184728 epoch sec; seqNo:15613; depth 16.36 2021-02-13T00:46:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177184.74 epoch sec; seqNo:15613; 4060004.98 north; 593117.35 east 2021-02-13T00:46:25Z- AHRS_M2 msg: 1613177185.13 epoch sec; seqNo:15491 2021-02-13T00:46:25Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.15 psi 2021-02-13T00:46:25Z- handleDepth()-Depth_Keller msg: 1613177185137 epoch sec; seqNo:15614; depth 16.46 2021-02-13T00:46:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:25Z- handleDvl() - RDI_Pathfinder msg: 1613177185.14 epoch sec; seqNo:7488 2021-02-13T00:46:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , -0.06 2021-02-13T00:46:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177185.16 epoch sec; seqNo:15614; 4060005.36 north; 593117.35 east 2021-02-13T00:46:25Z- AHRS_M2 msg: 1613177185.53 epoch sec; seqNo:15492 2021-02-13T00:46:25Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.16 psi 2021-02-13T00:46:25Z- handleDepth()-Depth_Keller msg: 1613177185537 epoch sec; seqNo:15615; depth 16.56 2021-02-13T00:46:25Z- handleDvl() - RDI_Pathfinder msg: 1613177185.55 epoch sec; seqNo:7489 2021-02-13T00:46:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.88 , 97.91 , 78.44 , 105.21 2021-02-13T00:46:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.11 , 0.01 2021-02-13T00:46:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177185.56 epoch sec; seqNo:15615; 4060005.74 north; 593117.35 east 2021-02-13T00:46:25Z- AHRS_M2 msg: 1613177185.94 epoch sec; seqNo:15493 2021-02-13T00:46:25Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.16 psi 2021-02-13T00:46:25Z- handleDepth()-Depth_Keller msg: 1613177185942 epoch sec; seqNo:15616; depth 16.68 2021-02-13T00:46:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177185.95 epoch sec; seqNo:15616; 4060006.12 north; 593117.34 east 2021-02-13T00:46:26Z- AHRS_M2 msg: 1613177186.34 epoch sec; seqNo:15494 2021-02-13T00:46:26Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.16 psi 2021-02-13T00:46:26Z- handleDepth()-Depth_Keller msg: 1613177186344 epoch sec; seqNo:15617; depth 16.79 2021-02-13T00:46:26Z- handleDvl() - RDI_Pathfinder msg: 1613177186.36 epoch sec; seqNo:7490 2021-02-13T00:46:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.25 , 97.71 , 71.51 , 105.01 2021-02-13T00:46:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.12 , 0.07 2021-02-13T00:46:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177186.37 epoch sec; seqNo:15617; 4060006.50 north; 593117.34 east 2021-02-13T00:46:26Z- AHRS_M2 msg: 1613177186.74 epoch sec; seqNo:15495 2021-02-13T00:46:26Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.16 psi 2021-02-13T00:46:26Z- handleDepth()-Depth_Keller msg: 1613177186753 epoch sec; seqNo:15618; depth 16.91 2021-02-13T00:46:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177186.78 epoch sec; seqNo:15618; 4060006.50 north; 593117.34 east 2021-02-13T00:46:27Z- AHRS_M2 msg: 1613177187.15 epoch sec; seqNo:15496 2021-02-13T00:46:27Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.16 psi 2021-02-13T00:46:27Z- handleDepth()-Depth_Keller msg: 1613177187161 epoch sec; seqNo:15619; depth 17.00 2021-02-13T00:46:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177187.17 epoch sec; seqNo:15619; 4060006.89 north; 593118.55 east 2021-02-13T00:46:27Z- AHRS_M2 msg: 1613177187.55 epoch sec; seqNo:15497 2021-02-13T00:46:27Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.16 psi 2021-02-13T00:46:27Z- handleDepth()-Depth_Keller msg: 1613177187558 epoch sec; seqNo:15620; depth 17.13 2021-02-13T00:46:27Z- handleDvl() - RDI_Pathfinder msg: 1613177187.57 epoch sec; seqNo:7491 2021-02-13T00:46:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.67 , 97.92 , 78.45 , 105.22 2021-02-13T00:46:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.03 , 0.07 2021-02-13T00:46:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177187.58 epoch sec; seqNo:15620; 4060007.27 north; 593118.55 east 2021-02-13T00:46:27Z- AHRS_M2 msg: 1613177187.96 epoch sec; seqNo:15498 2021-02-13T00:46:27Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.16 psi 2021-02-13T00:46:27Z- handleDepth()-Depth_Keller msg: 1613177187971 epoch sec; seqNo:15621; depth 17.25 2021-02-13T00:46:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177187.98 epoch sec; seqNo:15621; 4060007.65 north; 593118.55 east 2021-02-13T00:46:28Z- AHRS_M2 msg: 1613177188.36 epoch sec; seqNo:15499 2021-02-13T00:46:28Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.16 psi 2021-02-13T00:46:28Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:46:28Z- waiting for 1613177186.13, time is 1613177188.36 2021-02-13T00:46:28Z- LcmTrn::updateTrn() >>>> pose time 1613177188.36, meas time 1613177187.57 <<<< 2021-02-13T00:46:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177187.57 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:46:28Z- TerrainNav::motionUpdate() - currEstimate = (4060008, 593118.55, 17.25) 2021-02-13T00:46:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:46:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:46:28Z- TNavBF::Projecting Measurements 2021-02-13T00:46:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:46:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:46:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:46:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:46:28Z- TNavFilter::4 good beams. 2021-02-13T00:46:28Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:46:28Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:46:28Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:46:28Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:46:28Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:46:28Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:46:28Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:46:28Z- TNavBF::mmse selected from filter -1 2021-02-13T00:46:28Z- TerrainMapOctree: (vehN, vehE ) = (4060007.07, 593118.18). 2021-02-13T00:46:28Z- TerrainMapOctree: Min Distance = 1681.04. 2021-02-13T00:46:28Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:46:28Z- There is no specific good beam that all particles have in common. 2021-02-13T00:46:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:46:28Z- Old Depth Bias: 0.00 DepthBiasCov: 2.18 K: 0.155 ExpectedMeasDiff: nan currMeas.covariance: 10.15 2021-02-13T00:46:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:46:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.84 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:46:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:46:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.66 K: 0.126 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T00:46:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:46:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.45 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 17.71 2021-02-13T00:46:28Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:46:28Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:46:28Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:46:28Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:46:28Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:46:28Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:46:28Z- Filter 0 No beams used in subcloud update 2021-02-13T00:46:28Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:46:28Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:46:28Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 688.3 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T00:46:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177187.57 sec. 2021-02-13T00:46:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:46:28Z- TNavBF::mmse selected from filter -1 2021-02-13T00:46:28Z- variances([0], [2], and psi_berg) = 688.301563 832.992163 0.000000 2021-02-13T00:46:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:46:28Z- LcmTrn::publishEstimates() - MLE : 4060007.15 593140.05 17.25 2021-02-13T00:46:28Z- LcmTrn::publishEstimates() - MMSE : 4060007.65 593118.55 17.25 2021-02-13T00:46:28Z- LcmTrn::publishEstimates() - COVAR: 688.30 832.99 1.39 0.00 2021-02-13T00:46:28Z- handleDepth()-Depth_Keller msg: 1613177188369 epoch sec; seqNo:15622; depth 17.36 2021-02-13T00:46:28Z- handleDvl() - RDI_Pathfinder msg: 1613177188.38 epoch sec; seqNo:7492 2021-02-13T00:46:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.25 , 97.71 , 70.70 , 103.80 2021-02-13T00:46:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.06 2021-02-13T00:46:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177188.40 epoch sec; seqNo:15622; 4060007.65 north; 593118.55 east 2021-02-13T00:46:28Z- AHRS_M2 msg: 1613177188.76 epoch sec; seqNo:15500 2021-02-13T00:46:28Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.15 psi 2021-02-13T00:46:28Z- handleDepth()-Depth_Keller msg: 1613177188769 epoch sec; seqNo:15623; depth 17.47 2021-02-13T00:46:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:28Z- handleDvl() - RDI_Pathfinder msg: 1613177188.77 epoch sec; seqNo:7493 2021-02-13T00:46:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.06 2021-02-13T00:46:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177188.79 epoch sec; seqNo:15623; 4060008.03 north; 593118.54 east 2021-02-13T00:46:29Z- AHRS_M2 msg: 1613177189.18 epoch sec; seqNo:15501 2021-02-13T00:46:29Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.15 psi 2021-02-13T00:46:29Z- handleDepth()-Depth_Keller msg: 1613177189191 epoch sec; seqNo:15624; depth 17.59 2021-02-13T00:46:29Z- handleDvl() - RDI_Pathfinder msg: 1613177189.21 epoch sec; seqNo:7494 2021-02-13T00:46:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.05 , 97.98 , 78.51 , 104.07 2021-02-13T00:46:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , 0.04 2021-02-13T00:46:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177189.23 epoch sec; seqNo:15624; 4060008.40 north; 593118.54 east 2021-02-13T00:46:29Z- AHRS_M2 msg: 1613177189.57 epoch sec; seqNo:15502 2021-02-13T00:46:29Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.16 psi 2021-02-13T00:46:29Z- handleDepth()-Depth_Keller msg: 1613177189576 epoch sec; seqNo:15625; depth 17.70 2021-02-13T00:46:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177189.60 epoch sec; seqNo:15625; 4060008.80 north; 593119.75 east 2021-02-13T00:46:29Z- AHRS_M2 msg: 1613177189.97 epoch sec; seqNo:15503 2021-02-13T00:46:29Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.16 psi 2021-02-13T00:46:29Z- handleDepth()-Depth_Keller msg: 1613177189981 epoch sec; seqNo:15626; depth 17.81 2021-02-13T00:46:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:29Z- handleDvl() - RDI_Pathfinder msg: 1613177189.98 epoch sec; seqNo:7495 2021-02-13T00:46:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , 0.04 2021-02-13T00:46:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177189.99 epoch sec; seqNo:15626; 4060008.80 north; 593119.75 east 2021-02-13T00:46:30Z- AHRS_M2 msg: 1613177190.38 epoch sec; seqNo:15504 2021-02-13T00:46:30Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.17 psi 2021-02-13T00:46:30Z- handleDepth()-Depth_Keller msg: 1613177190384 epoch sec; seqNo:15627; depth 17.92 2021-02-13T00:46:30Z- handleDvl() - RDI_Pathfinder msg: 1613177190.40 epoch sec; seqNo:7496 2021-02-13T00:46:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.78 , 71.11 , 103.86 2021-02-13T00:46:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , 0.00 2021-02-13T00:46:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177190.41 epoch sec; seqNo:15627; 4060009.17 north; 593119.75 east 2021-02-13T00:46:30Z- AHRS_M2 msg: 1613177190.78 epoch sec; seqNo:15505 2021-02-13T00:46:30Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.18 psi 2021-02-13T00:46:30Z- handleDepth()-Depth_Keller msg: 1613177190791 epoch sec; seqNo:15628; depth 18.02 2021-02-13T00:46:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177190.80 epoch sec; seqNo:15628; 4060009.55 north; 593119.75 east 2021-02-13T00:46:31Z- AHRS_M2 msg: 1613177191.19 epoch sec; seqNo:15506 2021-02-13T00:46:31Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-13T00:46:31Z- handleDepth()-Depth_Keller msg: 1613177191197 epoch sec; seqNo:15629; depth 18.13 2021-02-13T00:46:31Z- handleDvl() - RDI_Pathfinder msg: 1613177191.22 epoch sec; seqNo:7497 2021-02-13T00:46:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 95.28 , 69.48 , 102.58 2021-02-13T00:46:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.01 2021-02-13T00:46:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177191.23 epoch sec; seqNo:15629; 4060009.93 north; 593119.74 east 2021-02-13T00:46:31Z- AHRS_M2 msg: 1613177191.59 epoch sec; seqNo:15507 2021-02-13T00:46:31Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.19 psi 2021-02-13T00:46:31Z- handleDepth()-Depth_Keller msg: 1613177191596 epoch sec; seqNo:15630; depth 18.25 2021-02-13T00:46:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177191.61 epoch sec; seqNo:15630; 4060009.93 north; 593119.74 east 2021-02-13T00:46:31Z- AHRS_M2 msg: 1613177191.99 epoch sec; seqNo:15508 2021-02-13T00:46:31Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-13T00:46:32Z- handleDepth()-Depth_Keller msg: 1613177192000 epoch sec; seqNo:15631; depth 18.35 2021-02-13T00:46:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177192.01 epoch sec; seqNo:15631; 4060010.32 north; 593120.96 east 2021-02-13T00:46:32Z- AHRS_M2 msg: 1613177192.40 epoch sec; seqNo:15509 2021-02-13T00:46:32Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-13T00:46:32Z- handleDepth()-Depth_Keller msg: 1613177192413 epoch sec; seqNo:15632; depth 18.47 2021-02-13T00:46:32Z- handleDvl() - RDI_Pathfinder msg: 1613177192.43 epoch sec; seqNo:7498 2021-02-13T00:46:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.37 , 96.63 , 70.70 , 106.36 2021-02-13T00:46:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , -0.01 2021-02-13T00:46:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177192.44 epoch sec; seqNo:15632; 4060010.70 north; 593120.95 east 2021-02-13T00:46:32Z- AHRS_M2 msg: 1613177192.80 epoch sec; seqNo:15510 2021-02-13T00:46:32Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-13T00:46:32Z- handleDepth()-Depth_Keller msg: 1613177192808 epoch sec; seqNo:15633; depth 18.58 2021-02-13T00:46:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177192.83 epoch sec; seqNo:15633; 4060011.08 north; 593120.95 east 2021-02-13T00:46:33Z- AHRS_M2 msg: 1613177193.21 epoch sec; seqNo:15511 2021-02-13T00:46:33Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.20 psi 2021-02-13T00:46:33Z- handleDepth()-Depth_Keller msg: 1613177193217 epoch sec; seqNo:15634; depth 18.71 2021-02-13T00:46:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177193.23 epoch sec; seqNo:15634; 4060011.08 north; 593120.95 east 2021-02-13T00:46:33Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-13T00:46:33Z- waiting for 1613177191.36, time is 1613177193.23 2021-02-13T00:46:33Z- LcmTrn::updateTrn() >>>> pose time 1613177193.23, meas time 1613177192.43 <<<< 2021-02-13T00:46:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177192.43 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:46:33Z- TerrainNav::motionUpdate() - currEstimate = (4060011, 593120.95, 18.71) 2021-02-13T00:46:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:46:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:46:33Z- TNavBF::Projecting Measurements 2021-02-13T00:46:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:46:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:46:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:46:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:46:33Z- TNavFilter::4 good beams. 2021-02-13T00:46:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:46:33Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:46:33Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:46:33Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:46:33Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:46:33Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:46:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:46:33Z- TNavBF::mmse selected from filter -1 2021-02-13T00:46:33Z- TerrainMapOctree: (vehN, vehE ) = (4060010.52, 593120.56). 2021-02-13T00:46:33Z- TerrainMapOctree: Min Distance = 1676.89. 2021-02-13T00:46:33Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:46:33Z- There is no specific good beam that all particles have in common. 2021-02-13T00:46:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:46:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.57 K: 0.119 ExpectedMeasDiff: nan currMeas.covariance: 9.83 2021-02-13T00:46:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:46:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 14.94 2021-02-13T00:46:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:46:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.116 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-13T00:46:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:46:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 18.10 2021-02-13T00:46:33Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:46:33Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:46:33Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:46:33Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:46:33Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:46:33Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:46:33Z- Filter 0 No beams used in subcloud update 2021-02-13T00:46:33Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:46:33Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:46:33Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 688.3 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:46:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177192.43 sec. 2021-02-13T00:46:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:46:33Z- TNavBF::mmse selected from filter -1 2021-02-13T00:46:33Z- variances([0], [2], and psi_berg) = 688.236931 832.992133 0.000000 2021-02-13T00:46:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:46:33Z- LcmTrn::publishEstimates() - MLE : 4060010.58 593142.45 18.71 2021-02-13T00:46:33Z- LcmTrn::publishEstimates() - MMSE : 4060011.08 593120.95 18.71 2021-02-13T00:46:33Z- LcmTrn::publishEstimates() - COVAR: 688.24 832.99 1.10 0.00 2021-02-13T00:46:33Z- AHRS_M2 msg: 1613177193.61 epoch sec; seqNo:15512 2021-02-13T00:46:33Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:46:33Z- handleDepth()-Depth_Keller msg: 1613177193616 epoch sec; seqNo:15635; depth 18.84 2021-02-13T00:46:33Z- handleDvl() - RDI_Pathfinder msg: 1613177193.63 epoch sec; seqNo:7499 2021-02-13T00:46:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 78.44 , 106.43 2021-02-13T00:46:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , -0.03 2021-02-13T00:46:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177193.64 epoch sec; seqNo:15635; 4060011.46 north; 593120.95 east 2021-02-13T00:46:34Z- AHRS_M2 msg: 1613177194.02 epoch sec; seqNo:15513 2021-02-13T00:46:34Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:46:34Z- handleDepth()-Depth_Keller msg: 1613177194029 epoch sec; seqNo:15636; depth 18.96 2021-02-13T00:46:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177194.04 epoch sec; seqNo:15636; 4060011.84 north; 593120.94 east 2021-02-13T00:46:34Z- AHRS_M2 msg: 1613177194.42 epoch sec; seqNo:15514 2021-02-13T00:46:34Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:46:34Z- handleDepth()-Depth_Keller msg: 1613177194424 epoch sec; seqNo:15637; depth 19.08 2021-02-13T00:46:34Z- handleDvl() - RDI_Pathfinder msg: 1613177194.45 epoch sec; seqNo:7500 2021-02-13T00:46:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.24 , 97.71 , 0.00 , 107.45 2021-02-13T00:46:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.05 2021-02-13T00:46:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177194.46 epoch sec; seqNo:15637; 4060012.23 north; 593122.16 east 2021-02-13T00:46:34Z- AHRS_M2 msg: 1613177194.82 epoch sec; seqNo:15515 2021-02-13T00:46:34Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.20 psi 2021-02-13T00:46:34Z- handleDepth()-Depth_Keller msg: 1613177194833 epoch sec; seqNo:15638; depth 19.21 2021-02-13T00:46:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:34Z- handleDvl() - RDI_Pathfinder msg: 1613177194.84 epoch sec; seqNo:7501 2021-02-13T00:46:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.05 2021-02-13T00:46:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177194.86 epoch sec; seqNo:15638; 4060012.23 north; 593122.16 east 2021-02-13T00:46:35Z- AHRS_M2 msg: 1613177195.23 epoch sec; seqNo:15516 2021-02-13T00:46:35Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-13T00:46:35Z- handleDepth()-Depth_Keller msg: 1613177195233 epoch sec; seqNo:15639; depth 19.35 2021-02-13T00:46:35Z- handleDvl() - RDI_Pathfinder msg: 1613177195.25 epoch sec; seqNo:7502 2021-02-13T00:46:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 97.51 , 67.43 , 112.38 2021-02-13T00:46:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.09 , -0.04 2021-02-13T00:46:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177195.28 epoch sec; seqNo:15639; 4060012.61 north; 593122.15 east 2021-02-13T00:46:35Z- AHRS_M2 msg: 1613177195.63 epoch sec; seqNo:15517 2021-02-13T00:46:35Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-13T00:46:35Z- handleDepth()-Depth_Keller msg: 1613177195641 epoch sec; seqNo:15640; depth 19.48 2021-02-13T00:46:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177195.65 epoch sec; seqNo:15640; 4060012.99 north; 593122.15 east 2021-02-13T00:46:36Z- AHRS_M2 msg: 1613177196.04 epoch sec; seqNo:15518 2021-02-13T00:46:36Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-13T00:46:36Z- handleDepth()-Depth_Keller msg: 1613177196041 epoch sec; seqNo:15641; depth 19.61 2021-02-13T00:46:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:36Z- handleDvl() - RDI_Pathfinder msg: 1613177196.04 epoch sec; seqNo:7503 2021-02-13T00:46:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.09 , -0.04 2021-02-13T00:46:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177196.05 epoch sec; seqNo:15641; 4060013.37 north; 593122.15 east 2021-02-13T00:46:36Z- AHRS_M2 msg: 1613177196.44 epoch sec; seqNo:15519 2021-02-13T00:46:36Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-13T00:46:36Z- handleDepth()-Depth_Keller msg: 1613177196448 epoch sec; seqNo:15642; depth 19.74 2021-02-13T00:46:36Z- handleDvl() - RDI_Pathfinder msg: 1613177196.46 epoch sec; seqNo:7504 2021-02-13T00:46:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 97.85 , 69.47 , 107.58 2021-02-13T00:46:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , 0.00 2021-02-13T00:46:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177196.49 epoch sec; seqNo:15642; 4060013.75 north; 593122.14 east 2021-02-13T00:46:36Z- AHRS_M2 msg: 1613177196.84 epoch sec; seqNo:15520 2021-02-13T00:46:36Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-13T00:46:36Z- handleDepth()-Depth_Keller msg: 1613177196851 epoch sec; seqNo:15643; depth 19.87 2021-02-13T00:46:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177196.86 epoch sec; seqNo:15643; 4060013.75 north; 593122.14 east 2021-02-13T00:46:37Z- AHRS_M2 msg: 1613177197.25 epoch sec; seqNo:15521 2021-02-13T00:46:37Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.18 psi 2021-02-13T00:46:37Z- handleDepth()-Depth_Keller msg: 1613177197257 epoch sec; seqNo:15644; depth 20.00 2021-02-13T00:46:37Z- handleDvl() - RDI_Pathfinder msg: 1613177197.28 epoch sec; seqNo:7505 2021-02-13T00:46:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.19 , 97.92 , 67.84 , 111.30 2021-02-13T00:46:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , 0.05 2021-02-13T00:46:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177197.29 epoch sec; seqNo:15644; 4060014.14 north; 593123.36 east 2021-02-13T00:46:37Z- AHRS_M2 msg: 1613177197.65 epoch sec; seqNo:15522 2021-02-13T00:46:37Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.18 psi 2021-02-13T00:46:37Z- handleDepth()-Depth_Keller msg: 1613177197656 epoch sec; seqNo:15645; depth 20.16 2021-02-13T00:46:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177197.67 epoch sec; seqNo:15645; 4060014.52 north; 593123.35 east 2021-02-13T00:46:38Z- AHRS_M2 msg: 1613177198.06 epoch sec; seqNo:15523 2021-02-13T00:46:38Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.18 psi 2021-02-13T00:46:38Z- handleDepth()-Depth_Keller msg: 1613177198069 epoch sec; seqNo:15646; depth 20.28 2021-02-13T00:46:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177198.08 epoch sec; seqNo:15646; 4060014.90 north; 593123.35 east 2021-02-13T00:46:38Z- AHRS_M2 msg: 1613177198.46 epoch sec; seqNo:15524 2021-02-13T00:46:38Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.19 psi 2021-02-13T00:46:38Z- handleDepth()-Depth_Keller msg: 1613177198479 epoch sec; seqNo:15647; depth 20.42 2021-02-13T00:46:38Z- handleDvl() - RDI_Pathfinder msg: 1613177198.50 epoch sec; seqNo:7506 2021-02-13T00:46:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 66.62 , 107.65 2021-02-13T00:46:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.05 2021-02-13T00:46:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177198.51 epoch sec; seqNo:15647; 4060014.90 north; 593123.35 east 2021-02-13T00:46:38Z- LcmTrn::handleMessages() - handling 47 messages took 0.73 ms and 133 calls... 2021-02-13T00:46:38Z- waiting for 1613177196.23, time is 1613177198.51 2021-02-13T00:46:38Z- LcmTrn::updateTrn() equating pose 1613177198.51 and meas times 1613177198.51 2021-02-13T00:46:38Z- LcmTrn::updateTrn() >>>> pose time 1613177198.51, meas time 1613177198.51 <<<< 2021-02-13T00:46:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177198.51 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:46:38Z- TerrainNav::motionUpdate() - currEstimate = (4060015, 593123.35, 20.42) 2021-02-13T00:46:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:46:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:46:38Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177198.51 sec.; No relevant navigation data available 2021-02-13T00:46:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:46:38Z- TNavBF::mmse selected from filter -1 2021-02-13T00:46:38Z- variances([0], [2], and psi_berg) = 688.186186 832.992110 0.000000 2021-02-13T00:46:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:46:38Z- LcmTrn::publishEstimates() - MLE : 4060014.40 593144.85 20.42 2021-02-13T00:46:38Z- LcmTrn::publishEstimates() - MMSE : 4060014.90 593123.35 20.42 2021-02-13T00:46:38Z- LcmTrn::publishEstimates() - COVAR: 688.19 832.99 1.33 0.00 2021-02-13T00:46:38Z- AHRS_M2 msg: 1613177198.86 epoch sec; seqNo:15525 2021-02-13T00:46:38Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.19 psi 2021-02-13T00:46:38Z- handleDepth()-Depth_Keller msg: 1613177198873 epoch sec; seqNo:15648; depth 20.56 2021-02-13T00:46:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177198.90 epoch sec; seqNo:15648; 4060015.28 north; 593123.35 east 2021-02-13T00:46:39Z- AHRS_M2 msg: 1613177199.27 epoch sec; seqNo:15526 2021-02-13T00:46:39Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.19 psi 2021-02-13T00:46:39Z- handleDepth()-Depth_Keller msg: 1613177199272 epoch sec; seqNo:15649; depth 20.69 2021-02-13T00:46:39Z- handleDvl() - RDI_Pathfinder msg: 1613177199.29 epoch sec; seqNo:7507 2021-02-13T00:46:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 67.84 , 108.93 2021-02-13T00:46:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.00 , 0.05 2021-02-13T00:46:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177199.30 epoch sec; seqNo:15649; 4060015.66 north; 593123.34 east 2021-02-13T00:46:39Z- AHRS_M2 msg: 1613177199.68 epoch sec; seqNo:15527 2021-02-13T00:46:39Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.19 psi 2021-02-13T00:46:39Z- handleDepth()-Depth_Keller msg: 1613177199683 epoch sec; seqNo:15650; depth 20.83 2021-02-13T00:46:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:39Z- handleDvl() - RDI_Pathfinder msg: 1613177199.68 epoch sec; seqNo:7508 2021-02-13T00:46:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.00 , 0.05 2021-02-13T00:46:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177199.70 epoch sec; seqNo:15650; 4060016.05 north; 593124.56 east 2021-02-13T00:46:40Z- AHRS_M2 msg: 1613177200.08 epoch sec; seqNo:15528 2021-02-13T00:46:40Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.19 psi 2021-02-13T00:46:40Z- handleDepth()-Depth_Keller msg: 1613177200086 epoch sec; seqNo:15651; depth 20.96 2021-02-13T00:46:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177200.10 epoch sec; seqNo:15651; 4060016.05 north; 593124.56 east 2021-02-13T00:46:40Z- AHRS_M2 msg: 1613177200.48 epoch sec; seqNo:15529 2021-02-13T00:46:40Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.19 psi 2021-02-13T00:46:40Z- handleDepth()-Depth_Keller msg: 1613177200484 epoch sec; seqNo:15652; depth 21.13 2021-02-13T00:46:40Z- handleDvl() - RDI_Pathfinder msg: 1613177200.50 epoch sec; seqNo:7509 2021-02-13T00:46:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 65.80 , 108.86 2021-02-13T00:46:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.01 , 0.04 2021-02-13T00:46:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177200.51 epoch sec; seqNo:15652; 4060016.43 north; 593124.55 east 2021-02-13T00:46:40Z- AHRS_M2 msg: 1613177200.88 epoch sec; seqNo:15530 2021-02-13T00:46:40Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.19 psi 2021-02-13T00:46:40Z- handleDepth()-Depth_Keller msg: 1613177200889 epoch sec; seqNo:15653; depth 21.27 2021-02-13T00:46:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177200.90 epoch sec; seqNo:15653; 4060016.80 north; 593124.55 east 2021-02-13T00:46:41Z- AHRS_M2 msg: 1613177201.29 epoch sec; seqNo:15531 2021-02-13T00:46:41Z- AHRS_M2 msg: 0.64 phi; -0.39 theta; -0.19 psi 2021-02-13T00:46:41Z- handleDepth()-Depth_Keller msg: 1613177201294 epoch sec; seqNo:15654; depth 21.39 2021-02-13T00:46:41Z- handleDvl() - RDI_Pathfinder msg: 1613177201.31 epoch sec; seqNo:7510 2021-02-13T00:46:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.79 , 97.98 , 67.03 , 105.28 2021-02-13T00:46:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , 0.02 2021-02-13T00:46:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177201.32 epoch sec; seqNo:15654; 4060016.80 north; 593124.55 east 2021-02-13T00:46:41Z- AHRS_M2 msg: 1613177201.69 epoch sec; seqNo:15532 2021-02-13T00:46:41Z- AHRS_M2 msg: 0.64 phi; -0.39 theta; -0.18 psi 2021-02-13T00:46:41Z- handleDepth()-Depth_Keller msg: 1613177201696 epoch sec; seqNo:15655; depth 21.55 2021-02-13T00:46:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177201.72 epoch sec; seqNo:15655; 4060017.18 north; 593124.55 east 2021-02-13T00:46:42Z- AHRS_M2 msg: 1613177202.10 epoch sec; seqNo:15533 2021-02-13T00:46:42Z- AHRS_M2 msg: 0.64 phi; -0.39 theta; -0.18 psi 2021-02-13T00:46:42Z- handleDepth()-Depth_Keller msg: 1613177202102 epoch sec; seqNo:15656; depth 21.68 2021-02-13T00:46:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177202.11 epoch sec; seqNo:15656; 4060017.58 north; 593125.76 east 2021-02-13T00:46:42Z- AHRS_M2 msg: 1613177202.50 epoch sec; seqNo:15534 2021-02-13T00:46:42Z- AHRS_M2 msg: 0.64 phi; -0.39 theta; -0.18 psi 2021-02-13T00:46:42Z- handleDepth()-Depth_Keller msg: 1613177202509 epoch sec; seqNo:15657; depth 21.83 2021-02-13T00:46:42Z- handleDvl() - RDI_Pathfinder msg: 1613177202.52 epoch sec; seqNo:7511 2021-02-13T00:46:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 67.43 , 107.78 2021-02-13T00:46:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.04 , 0.00 2021-02-13T00:46:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177202.53 epoch sec; seqNo:15657; 4060017.95 north; 593125.76 east 2021-02-13T00:46:42Z- AHRS_M2 msg: 1613177202.90 epoch sec; seqNo:15535 2021-02-13T00:46:42Z- AHRS_M2 msg: 0.63 phi; -0.39 theta; -0.19 psi 2021-02-13T00:46:42Z- handleDepth()-Depth_Keller msg: 1613177202910 epoch sec; seqNo:15658; depth 22.00 2021-02-13T00:46:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177202.92 epoch sec; seqNo:15658; 4060017.95 north; 593125.76 east 2021-02-13T00:46:43Z- AHRS_M2 msg: 1613177203.31 epoch sec; seqNo:15536 2021-02-13T00:46:43Z- AHRS_M2 msg: 0.63 phi; -0.39 theta; -0.19 psi 2021-02-13T00:46:43Z- handleDepth()-Depth_Keller msg: 1613177203317 epoch sec; seqNo:15659; depth 22.13 2021-02-13T00:46:43Z- handleDvl() - RDI_Pathfinder msg: 1613177203.34 epoch sec; seqNo:7512 2021-02-13T00:46:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 67.03 , 108.86 2021-02-13T00:46:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , -0.04 2021-02-13T00:46:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177203.35 epoch sec; seqNo:15659; 4060018.33 north; 593125.75 east 2021-02-13T00:46:43Z- LcmTrn::handleMessages() - handling 42 messages took 0.66 ms and 129 calls... 2021-02-13T00:46:43Z- waiting for 1613177201.51, time is 1613177203.35 2021-02-13T00:46:43Z- LcmTrn::updateTrn() equating pose 1613177203.35 and meas times 1613177203.35 2021-02-13T00:46:43Z- LcmTrn::updateTrn() >>>> pose time 1613177203.35, meas time 1613177203.35 <<<< 2021-02-13T00:46:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177203.35 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:46:43Z- TerrainNav::motionUpdate() - currEstimate = (4060018, 593125.75, 22.13) 2021-02-13T00:46:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:46:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:46:43Z- TNavBF::Projecting Measurements 2021-02-13T00:46:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:46:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:46:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:46:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:46:43Z- TNavFilter::4 good beams. 2021-02-13T00:46:43Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:46:43Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:46:43Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:46:43Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:46:43Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:46:43Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:46:43Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:46:43Z- TNavBF::mmse selected from filter -1 2021-02-13T00:46:43Z- TerrainMapOctree: (vehN, vehE ) = (4060018.33, 593125.75). 2021-02-13T00:46:43Z- TerrainMapOctree: Min Distance = 1667.61. 2021-02-13T00:46:43Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:46:43Z- There is no specific good beam that all particles have in common. 2021-02-13T00:46:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:46:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.54 K: 0.118 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T00:46:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:46:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.36 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:46:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:46:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.124 ExpectedMeasDiff: nan currMeas.covariance: 7.19 2021-02-13T00:46:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:46:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 18.96 2021-02-13T00:46:43Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:46:43Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:46:43Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:46:43Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:46:43Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:46:43Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:46:43Z- Filter 0 No beams used in subcloud update 2021-02-13T00:46:43Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:46:43Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:46:43Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 688.1 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:46:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177203.35 sec. 2021-02-13T00:46:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:46:43Z- TNavBF::mmse selected from filter -1 2021-02-13T00:46:43Z- variances([0], [2], and psi_berg) = 688.137263 832.992087 0.000000 2021-02-13T00:46:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:46:43Z- LcmTrn::publishEstimates() - MLE : 4060017.83 593147.25 22.13 2021-02-13T00:46:43Z- LcmTrn::publishEstimates() - MMSE : 4060018.33 593125.75 22.13 2021-02-13T00:46:43Z- LcmTrn::publishEstimates() - COVAR: 688.14 832.99 1.05 0.00 2021-02-13T00:46:43Z- AHRS_M2 msg: 1613177203.71 epoch sec; seqNo:15537 2021-02-13T00:46:43Z- AHRS_M2 msg: 0.62 phi; -0.39 theta; -0.20 psi 2021-02-13T00:46:43Z- handleDepth()-Depth_Keller msg: 1613177203716 epoch sec; seqNo:15660; depth 22.27 2021-02-13T00:46:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177203.73 epoch sec; seqNo:15660; 4060018.71 north; 593125.75 east 2021-02-13T00:46:44Z- AHRS_M2 msg: 1613177204.14 epoch sec; seqNo:15538 2021-02-13T00:46:44Z- AHRS_M2 msg: 0.62 phi; -0.39 theta; -0.21 psi 2021-02-13T00:46:44Z- handleDepth()-Depth_Keller msg: 1613177204143 epoch sec; seqNo:15661; depth 22.43 2021-02-13T00:46:44Z- handleDvl() - RDI_Pathfinder msg: 1613177204.16 epoch sec; seqNo:7513 2021-02-13T00:46:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.97 , 94.26 , 66.62 , 110.08 2021-02-13T00:46:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , -0.06 2021-02-13T00:46:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177204.18 epoch sec; seqNo:15661; 4060019.10 north; 593126.97 east 2021-02-13T00:46:44Z- AHRS_M2 msg: 1613177204.52 epoch sec; seqNo:15539 2021-02-13T00:46:44Z- AHRS_M2 msg: 0.61 phi; -0.39 theta; -0.22 psi 2021-02-13T00:46:44Z- handleDepth()-Depth_Keller msg: 1613177204529 epoch sec; seqNo:15662; depth 22.60 2021-02-13T00:46:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:44Z- handleDvl() - RDI_Pathfinder msg: 1613177204.53 epoch sec; seqNo:7514 2021-02-13T00:46:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , -0.06 2021-02-13T00:46:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177204.55 epoch sec; seqNo:15662; 4060019.10 north; 593126.97 east 2021-02-13T00:46:44Z- AHRS_M2 msg: 1613177204.92 epoch sec; seqNo:15540 2021-02-13T00:46:44Z- AHRS_M2 msg: 0.61 phi; -0.39 theta; -0.22 psi 2021-02-13T00:46:44Z- handleDepth()-Depth_Keller msg: 1613177204933 epoch sec; seqNo:15663; depth 22.73 2021-02-13T00:46:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177204.94 epoch sec; seqNo:15663; 4060019.48 north; 593126.96 east 2021-02-13T00:46:45Z- AHRS_M2 msg: 1613177205.33 epoch sec; seqNo:15541 2021-02-13T00:46:45Z- AHRS_M2 msg: 0.60 phi; -0.38 theta; -0.23 psi 2021-02-13T00:46:45Z- handleDepth()-Depth_Keller msg: 1613177205332 epoch sec; seqNo:15664; depth 22.89 2021-02-13T00:46:45Z- handleDvl() - RDI_Pathfinder msg: 1613177205.34 epoch sec; seqNo:7515 2021-02-13T00:46:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 94.46 , 78.64 , 109.07 2021-02-13T00:46:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , -0.05 2021-02-13T00:46:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177205.35 epoch sec; seqNo:15664; 4060019.86 north; 593126.96 east 2021-02-13T00:46:45Z- AHRS_M2 msg: 1613177205.74 epoch sec; seqNo:15542 2021-02-13T00:46:45Z- AHRS_M2 msg: 0.60 phi; -0.38 theta; -0.23 psi 2021-02-13T00:46:45Z- handleDepth()-Depth_Keller msg: 1613177205742 epoch sec; seqNo:15665; depth 23.03 2021-02-13T00:46:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:45Z- handleDvl() - RDI_Pathfinder msg: 1613177205.74 epoch sec; seqNo:7516 2021-02-13T00:46:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , -0.05 2021-02-13T00:46:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177205.76 epoch sec; seqNo:15665; 4060020.24 north; 593126.95 east 2021-02-13T00:46:46Z- AHRS_M2 msg: 1613177206.14 epoch sec; seqNo:15543 2021-02-13T00:46:46Z- AHRS_M2 msg: 0.59 phi; -0.38 theta; -0.23 psi 2021-02-13T00:46:46Z- handleDepth()-Depth_Keller msg: 1613177206150 epoch sec; seqNo:15666; depth 23.17 2021-02-13T00:46:46Z- handleDvl() - RDI_Pathfinder msg: 1613177206.16 epoch sec; seqNo:7517 2021-02-13T00:46:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 97.85 , 64.16 , 106.36 2021-02-13T00:46:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.08 , -0.07 2021-02-13T00:46:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177206.17 epoch sec; seqNo:15666; 4060020.62 north; 593126.95 east 2021-02-13T00:46:46Z- AHRS_M2 msg: 1613177206.54 epoch sec; seqNo:15544 2021-02-13T00:46:46Z- AHRS_M2 msg: 0.59 phi; -0.38 theta; -0.23 psi 2021-02-13T00:46:46Z- handleDepth()-Depth_Keller msg: 1613177206544 epoch sec; seqNo:15667; depth 23.33 2021-02-13T00:46:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177206.56 epoch sec; seqNo:15667; 4060020.62 north; 593126.95 east 2021-02-13T00:46:46Z- AHRS_M2 msg: 1613177206.94 epoch sec; seqNo:15545 2021-02-13T00:46:46Z- AHRS_M2 msg: 0.58 phi; -0.38 theta; -0.23 psi 2021-02-13T00:46:46Z- handleDepth()-Depth_Keller msg: 1613177206953 epoch sec; seqNo:15668; depth 23.46 2021-02-13T00:46:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:46Z- handleDvl() - RDI_Pathfinder msg: 1613177206.95 epoch sec; seqNo:7518 2021-02-13T00:46:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.08 , -0.07 2021-02-13T00:46:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177206.96 epoch sec; seqNo:15668; 4060021.01 north; 593128.17 east 2021-02-13T00:46:47Z- AHRS_M2 msg: 1613177207.35 epoch sec; seqNo:15546 2021-02-13T00:46:47Z- AHRS_M2 msg: 0.58 phi; -0.37 theta; -0.23 psi 2021-02-13T00:46:47Z- handleDepth()-Depth_Keller msg: 1613177207354 epoch sec; seqNo:15669; depth 23.61 2021-02-13T00:46:47Z- handleDvl() - RDI_Pathfinder msg: 1613177207.37 epoch sec; seqNo:7519 2021-02-13T00:46:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 98.18 , 0.00 , 105.49 2021-02-13T00:46:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.13 , -0.02 2021-02-13T00:46:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177207.38 epoch sec; seqNo:15669; 4060021.39 north; 593128.16 east 2021-02-13T00:46:47Z- AHRS_M2 msg: 1613177207.75 epoch sec; seqNo:15547 2021-02-13T00:46:47Z- AHRS_M2 msg: 0.57 phi; -0.37 theta; -0.23 psi 2021-02-13T00:46:47Z- handleDepth()-Depth_Keller msg: 1613177207761 epoch sec; seqNo:15670; depth 23.76 2021-02-13T00:46:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177207.79 epoch sec; seqNo:15670; 4060021.77 north; 593128.16 east 2021-02-13T00:46:48Z- AHRS_M2 msg: 1613177208.17 epoch sec; seqNo:15548 2021-02-13T00:46:48Z- AHRS_M2 msg: 0.57 phi; -0.37 theta; -0.22 psi 2021-02-13T00:46:48Z- handleDepth()-Depth_Keller msg: 1613177208180 epoch sec; seqNo:15671; depth 23.92 2021-02-13T00:46:48Z- handleDvl() - RDI_Pathfinder msg: 1613177208.19 epoch sec; seqNo:7520 2021-02-13T00:46:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.78 , 97.71 , 63.76 , 107.18 2021-02-13T00:46:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.15 , 0.02 2021-02-13T00:46:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177208.21 epoch sec; seqNo:15671; 4060022.15 north; 593128.15 east 2021-02-13T00:46:48Z- AHRS_M2 msg: 1613177208.56 epoch sec; seqNo:15549 2021-02-13T00:46:48Z- AHRS_M2 msg: 0.57 phi; -0.37 theta; -0.22 psi 2021-02-13T00:46:48Z- handleDepth()-Depth_Keller msg: 1613177208573 epoch sec; seqNo:15672; depth 24.06 2021-02-13T00:46:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177208.59 epoch sec; seqNo:15672; 4060022.53 north; 593128.15 east 2021-02-13T00:46:48Z- LcmTrn::handleMessages() - handling 47 messages took 0.75 ms and 132 calls... 2021-02-13T00:46:48Z- waiting for 1613177206.35, time is 1613177208.59 2021-02-13T00:46:48Z- LcmTrn::updateTrn() >>>> pose time 1613177208.59, meas time 1613177208.19 <<<< 2021-02-13T00:46:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177208.19 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:46:48Z- TerrainNav::motionUpdate() - currEstimate = (4060023, 593128.15, 24.06) 2021-02-13T00:46:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:46:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:46:48Z- TNavBF::Projecting Measurements 2021-02-13T00:46:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:46:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:46:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:46:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:46:48Z- TNavFilter::4 good beams. 2021-02-13T00:46:48Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:46:48Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:46:48Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:46:48Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:46:48Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:46:48Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:46:48Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:46:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:46:48Z- TerrainMapOctree: (vehN, vehE ) = (4060022.21, 593127.97). 2021-02-13T00:46:48Z- TerrainMapOctree: Min Distance = 1663.25. 2021-02-13T00:46:48Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:46:48Z- There is no specific good beam that all particles have in common. 2021-02-13T00:46:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:46:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 9.93 2021-02-13T00:46:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:46:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:46:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:46:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.116 ExpectedMeasDiff: nan currMeas.covariance: 6.50 2021-02-13T00:46:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:46:48Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 18.38 2021-02-13T00:46:48Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:46:48Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:46:48Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:46:48Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:46:48Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:46:48Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:46:48Z- Filter 0 No beams used in subcloud update 2021-02-13T00:46:48Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:46:48Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:46:48Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 688.1 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:46:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177208.19 sec. 2021-02-13T00:46:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:46:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:46:48Z- variances([0], [2], and psi_berg) = 688.072430 832.992057 0.000000 2021-02-13T00:46:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:46:48Z- LcmTrn::publishEstimates() - MLE : 4060022.03 593149.65 24.06 2021-02-13T00:46:48Z- LcmTrn::publishEstimates() - MMSE : 4060022.53 593128.15 24.06 2021-02-13T00:46:48Z- LcmTrn::publishEstimates() - COVAR: 688.07 832.99 0.93 0.00 2021-02-13T00:46:48Z- AHRS_M2 msg: 1613177208.96 epoch sec; seqNo:15550 2021-02-13T00:46:48Z- AHRS_M2 msg: 0.56 phi; -0.37 theta; -0.22 psi 2021-02-13T00:46:48Z- handleDepth()-Depth_Keller msg: 1613177208968 epoch sec; seqNo:15673; depth 24.19 2021-02-13T00:46:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177208.98 epoch sec; seqNo:15673; 4060022.53 north; 593128.15 east 2021-02-13T00:46:49Z- AHRS_M2 msg: 1613177209.37 epoch sec; seqNo:15551 2021-02-13T00:46:49Z- AHRS_M2 msg: 0.56 phi; -0.37 theta; -0.21 psi 2021-02-13T00:46:49Z- handleDepth()-Depth_Keller msg: 1613177209372 epoch sec; seqNo:15674; depth 24.33 2021-02-13T00:46:49Z- handleDvl() - RDI_Pathfinder msg: 1613177209.38 epoch sec; seqNo:7521 2021-02-13T00:46:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.05 , 64.98 , 104.13 2021-02-13T00:46:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , 0.06 2021-02-13T00:46:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177209.41 epoch sec; seqNo:15674; 4060022.91 north; 593128.15 east 2021-02-13T00:46:49Z- AHRS_M2 msg: 1613177209.77 epoch sec; seqNo:15552 2021-02-13T00:46:49Z- AHRS_M2 msg: 0.56 phi; -0.37 theta; -0.21 psi 2021-02-13T00:46:49Z- handleDepth()-Depth_Keller msg: 1613177209782 epoch sec; seqNo:15675; depth 24.48 2021-02-13T00:46:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177209.80 epoch sec; seqNo:15675; 4060023.29 north; 593128.14 east 2021-02-13T00:46:50Z- AHRS_M2 msg: 1613177210.18 epoch sec; seqNo:15553 2021-02-13T00:46:50Z- AHRS_M2 msg: 0.56 phi; -0.36 theta; -0.20 psi 2021-02-13T00:46:50Z- handleDepth()-Depth_Keller msg: 1613177210189 epoch sec; seqNo:15676; depth 24.61 2021-02-13T00:46:50Z- handleDvl() - RDI_Pathfinder msg: 1613177210.20 epoch sec; seqNo:7522 2021-02-13T00:46:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 90.54 , 63.76 , 101.50 2021-02-13T00:46:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.06 , 0.06 2021-02-13T00:46:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177210.21 epoch sec; seqNo:15676; 4060023.68 north; 593129.36 east 2021-02-13T00:46:50Z- AHRS_M2 msg: 1613177210.58 epoch sec; seqNo:15554 2021-02-13T00:46:50Z- AHRS_M2 msg: 0.56 phi; -0.36 theta; -0.19 psi 2021-02-13T00:46:50Z- handleDepth()-Depth_Keller msg: 1613177210590 epoch sec; seqNo:15677; depth 24.77 2021-02-13T00:46:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:50Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:50Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:50Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:50Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:50Z- handleDvl() - RDI_Pathfinder msg: 1613177210.59 epoch sec; seqNo:7523 2021-02-13T00:46:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.06 , 0.06 2021-02-13T00:46:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177210.60 epoch sec; seqNo:15677; 4060023.68 north; 593129.36 east 2021-02-13T00:46:50Z- AHRS_M2 msg: 1613177210.98 epoch sec; seqNo:15555 2021-02-13T00:46:50Z- AHRS_M2 msg: 0.56 phi; -0.36 theta; -0.19 psi 2021-02-13T00:46:50Z- handleDepth()-Depth_Keller msg: 1613177210993 epoch sec; seqNo:15678; depth 24.91 2021-02-13T00:46:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177211.01 epoch sec; seqNo:15678; 4060024.06 north; 593129.35 east 2021-02-13T00:46:51Z- AHRS_M2 msg: 1613177211.39 epoch sec; seqNo:15556 2021-02-13T00:46:51Z- AHRS_M2 msg: 0.56 phi; -0.36 theta; -0.18 psi 2021-02-13T00:46:51Z- handleDepth()-Depth_Keller msg: 1613177211397 epoch sec; seqNo:15679; depth 25.04 2021-02-13T00:46:51Z- handleDvl() - RDI_Pathfinder msg: 1613177211.41 epoch sec; seqNo:7524 2021-02-13T00:46:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 124.89 , 64.57 , 100.01 2021-02-13T00:46:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.16 , -0.03 2021-02-13T00:46:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177211.43 epoch sec; seqNo:15679; 4060024.43 north; 593129.35 east 2021-02-13T00:46:51Z- AHRS_M2 msg: 1613177211.79 epoch sec; seqNo:15557 2021-02-13T00:46:51Z- AHRS_M2 msg: 0.56 phi; -0.36 theta; -0.18 psi 2021-02-13T00:46:51Z- handleDepth()-Depth_Keller msg: 1613177211797 epoch sec; seqNo:15680; depth 25.19 2021-02-13T00:46:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:51Z- handleDvl() - RDI_Pathfinder msg: 1613177211.80 epoch sec; seqNo:7525 2021-02-13T00:46:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.16 , -0.03 2021-02-13T00:46:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177211.81 epoch sec; seqNo:15680; 4060024.43 north; 593129.35 east 2021-02-13T00:46:52Z- AHRS_M2 msg: 1613177212.20 epoch sec; seqNo:15558 2021-02-13T00:46:52Z- AHRS_M2 msg: 0.57 phi; -0.36 theta; -0.18 psi 2021-02-13T00:46:52Z- handleDepth()-Depth_Keller msg: 1613177212213 epoch sec; seqNo:15681; depth 25.30 2021-02-13T00:46:52Z- handleDvl() - RDI_Pathfinder msg: 1613177212.22 epoch sec; seqNo:7526 2021-02-13T00:46:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 0.00 , 63.35 , 101.43 2021-02-13T00:46:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.00 2021-02-13T00:46:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177212.24 epoch sec; seqNo:15681; 4060024.81 north; 593129.35 east 2021-02-13T00:46:52Z- AHRS_M2 msg: 1613177212.60 epoch sec; seqNo:15559 2021-02-13T00:46:52Z- AHRS_M2 msg: 0.57 phi; -0.36 theta; -0.18 psi 2021-02-13T00:46:52Z- handleDepth()-Depth_Keller msg: 1613177212604 epoch sec; seqNo:15682; depth 25.45 2021-02-13T00:46:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177212.62 epoch sec; seqNo:15682; 4060025.19 north; 593129.34 east 2021-02-13T00:46:53Z- AHRS_M2 msg: 1613177213.00 epoch sec; seqNo:15560 2021-02-13T00:46:53Z- AHRS_M2 msg: 0.57 phi; -0.35 theta; -0.18 psi 2021-02-13T00:46:53Z- handleDepth()-Depth_Keller msg: 1613177213013 epoch sec; seqNo:15683; depth 25.59 2021-02-13T00:46:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:53Z- handleDvl() - RDI_Pathfinder msg: 1613177213.02 epoch sec; seqNo:7527 2021-02-13T00:46:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.00 2021-02-13T00:46:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177213.02 epoch sec; seqNo:15683; 4060025.58 north; 593130.56 east 2021-02-13T00:46:53Z- AHRS_M2 msg: 1613177213.41 epoch sec; seqNo:15561 2021-02-13T00:46:53Z- AHRS_M2 msg: 0.57 phi; -0.35 theta; -0.18 psi 2021-02-13T00:46:53Z- handleDepth()-Depth_Keller msg: 1613177213418 epoch sec; seqNo:15684; depth 25.71 2021-02-13T00:46:53Z- handleDvl() - RDI_Pathfinder msg: 1613177213.43 epoch sec; seqNo:7528 2021-02-13T00:46:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 87.16 , 64.16 , 97.98 2021-02-13T00:46:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , -0.01 2021-02-13T00:46:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177213.44 epoch sec; seqNo:15684; 4060025.96 north; 593130.55 east 2021-02-13T00:46:53Z- LcmTrn::handleMessages() - handling 44 messages took 0.62 ms and 132 calls... 2021-02-13T00:46:53Z- waiting for 1613177211.59, time is 1613177213.44 2021-02-13T00:46:53Z- LcmTrn::updateTrn() equating pose 1613177213.44 and meas times 1613177213.44 2021-02-13T00:46:53Z- LcmTrn::updateTrn() >>>> pose time 1613177213.44, meas time 1613177213.44 <<<< 2021-02-13T00:46:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177213.44 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:46:53Z- TerrainNav::motionUpdate() - currEstimate = (4060026, 593130.55, 25.71) 2021-02-13T00:46:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:46:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:46:53Z- TNavBF::Projecting Measurements 2021-02-13T00:46:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:46:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:46:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:46:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:46:53Z- TNavFilter::4 good beams. 2021-02-13T00:46:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:46:53Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:46:53Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:46:53Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:46:53Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:46:53Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:46:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:46:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:46:53Z- TerrainMapOctree: (vehN, vehE ) = (4060025.96, 593130.55). 2021-02-13T00:46:53Z- TerrainMapOctree: Min Distance = 1658.74. 2021-02-13T00:46:53Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:46:53Z- There is no specific good beam that all particles have in common. 2021-02-13T00:46:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:46:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 7.81 2021-02-13T00:46:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:46:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 12.16 2021-02-13T00:46:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:46:53Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 6.59 2021-02-13T00:46:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:46:53Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:46:53Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:46:53Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:46:53Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:46:53Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:46:53Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:46:53Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:46:53Z- Filter 0 No beams used in subcloud update 2021-02-13T00:46:53Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:46:53Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:46:53Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 688.0 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:46:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177213.44 sec. 2021-02-13T00:46:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:46:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:46:53Z- variances([0], [2], and psi_berg) = 688.023565 832.992034 0.000000 2021-02-13T00:46:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:46:53Z- LcmTrn::publishEstimates() - MLE : 4060025.46 593152.05 25.71 2021-02-13T00:46:53Z- LcmTrn::publishEstimates() - MMSE : 4060025.96 593130.55 25.71 2021-02-13T00:46:53Z- LcmTrn::publishEstimates() - COVAR: 688.02 832.99 0.81 0.00 2021-02-13T00:46:53Z- AHRS_M2 msg: 1613177213.81 epoch sec; seqNo:15562 2021-02-13T00:46:53Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.18 psi 2021-02-13T00:46:53Z- handleDepth()-Depth_Keller msg: 1613177213821 epoch sec; seqNo:15685; depth 25.86 2021-02-13T00:46:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177213.84 epoch sec; seqNo:15685; 4060025.96 north; 593130.55 east 2021-02-13T00:46:54Z- AHRS_M2 msg: 1613177214.22 epoch sec; seqNo:15563 2021-02-13T00:46:54Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.18 psi 2021-02-13T00:46:54Z- handleDepth()-Depth_Keller msg: 1613177214220 epoch sec; seqNo:15686; depth 26.00 2021-02-13T00:46:54Z- handleDvl() - RDI_Pathfinder msg: 1613177214.23 epoch sec; seqNo:7529 2021-02-13T00:46:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 0.00 , 62.53 , 97.98 2021-02-13T00:46:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , -0.01 2021-02-13T00:46:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177214.24 epoch sec; seqNo:15686; 4060026.34 north; 593130.55 east 2021-02-13T00:46:54Z- AHRS_M2 msg: 1613177214.62 epoch sec; seqNo:15564 2021-02-13T00:46:54Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.17 psi 2021-02-13T00:46:54Z- handleDepth()-Depth_Keller msg: 1613177214629 epoch sec; seqNo:15687; depth 26.14 2021-02-13T00:46:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177214.64 epoch sec; seqNo:15687; 4060026.72 north; 593130.55 east 2021-02-13T00:46:55Z- AHRS_M2 msg: 1613177215.02 epoch sec; seqNo:15565 2021-02-13T00:46:55Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.17 psi 2021-02-13T00:46:55Z- handleDepth()-Depth_Keller msg: 1613177215036 epoch sec; seqNo:15688; depth 26.26 2021-02-13T00:46:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177215.05 epoch sec; seqNo:15688; 4060027.10 north; 593130.54 east 2021-02-13T00:46:55Z- AHRS_M2 msg: 1613177215.43 epoch sec; seqNo:15566 2021-02-13T00:46:55Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.17 psi 2021-02-13T00:46:55Z- handleDepth()-Depth_Keller msg: 1613177215437 epoch sec; seqNo:15689; depth 26.41 2021-02-13T00:46:55Z- handleDvl() - RDI_Pathfinder msg: 1613177215.46 epoch sec; seqNo:7530 2021-02-13T00:46:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 86.08 , 62.53 , 97.98 2021-02-13T00:46:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.05 , -0.02 2021-02-13T00:46:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177215.47 epoch sec; seqNo:15689; 4060027.48 north; 593130.54 east 2021-02-13T00:46:55Z- AHRS_M2 msg: 1613177215.83 epoch sec; seqNo:15567 2021-02-13T00:46:55Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.17 psi 2021-02-13T00:46:55Z- handleDepth()-Depth_Keller msg: 1613177215837 epoch sec; seqNo:15690; depth 26.54 2021-02-13T00:46:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177215.86 epoch sec; seqNo:15690; 4060027.49 north; 593131.76 east 2021-02-13T00:46:56Z- AHRS_M2 msg: 1613177216.24 epoch sec; seqNo:15568 2021-02-13T00:46:56Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.17 psi 2021-02-13T00:46:56Z- handleDepth()-Depth_Keller msg: 1613177216242 epoch sec; seqNo:15691; depth 26.66 2021-02-13T00:46:56Z- handleDvl() - RDI_Pathfinder msg: 1613177216.26 epoch sec; seqNo:7531 2021-02-13T00:46:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 85.00 , 61.30 , 97.98 2021-02-13T00:46:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.06 , -0.04 2021-02-13T00:46:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177216.27 epoch sec; seqNo:15691; 4060027.87 north; 593131.75 east 2021-02-13T00:46:56Z- AHRS_M2 msg: 1613177216.64 epoch sec; seqNo:15569 2021-02-13T00:46:56Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.17 psi 2021-02-13T00:46:56Z- handleDepth()-Depth_Keller msg: 1613177216657 epoch sec; seqNo:15692; depth 26.80 2021-02-13T00:46:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:56Z- handleDvl() - RDI_Pathfinder msg: 1613177216.66 epoch sec; seqNo:7532 2021-02-13T00:46:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.06 , -0.04 2021-02-13T00:46:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177216.67 epoch sec; seqNo:15692; 4060028.25 north; 593131.75 east 2021-02-13T00:46:57Z- AHRS_M2 msg: 1613177217.05 epoch sec; seqNo:15570 2021-02-13T00:46:57Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.16 psi 2021-02-13T00:46:57Z- handleDepth()-Depth_Keller msg: 1613177217054 epoch sec; seqNo:15693; depth 26.93 2021-02-13T00:46:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177217.10 epoch sec; seqNo:15693; 4060028.63 north; 593131.75 east 2021-02-13T00:46:57Z- AHRS_M2 msg: 1613177217.45 epoch sec; seqNo:15571 2021-02-13T00:46:57Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.16 psi 2021-02-13T00:46:57Z- handleDepth()-Depth_Keller msg: 1613177217452 epoch sec; seqNo:15694; depth 27.08 2021-02-13T00:46:57Z- handleDvl() - RDI_Pathfinder msg: 1613177217.46 epoch sec; seqNo:7533 2021-02-13T00:46:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:57Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:46:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177217.47 epoch sec; seqNo:15694; 4060029.01 north; 593131.74 east 2021-02-13T00:46:57Z- AHRS_M2 msg: 1613177217.85 epoch sec; seqNo:15572 2021-02-13T00:46:57Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.16 psi 2021-02-13T00:46:57Z- handleDepth()-Depth_Keller msg: 1613177217865 epoch sec; seqNo:15695; depth 27.20 2021-02-13T00:46:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:46:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:46:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:46:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:46:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:46:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:46:57Z- handleDvl() - RDI_Pathfinder msg: 1613177217.87 epoch sec; seqNo:7534 2021-02-13T00:46:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:46:57Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:46:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177217.88 epoch sec; seqNo:15695; 4060029.01 north; 593131.74 east 2021-02-13T00:46:58Z- AHRS_M2 msg: 1613177218.26 epoch sec; seqNo:15573 2021-02-13T00:46:58Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.16 psi 2021-02-13T00:46:58Z- handleDepth()-Depth_Keller msg: 1613177218260 epoch sec; seqNo:15696; depth 27.34 2021-02-13T00:46:58Z- handleDvl() - RDI_Pathfinder msg: 1613177218.28 epoch sec; seqNo:7535 2021-02-13T00:46:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.25 , 84.53 , 62.12 , 97.91 2021-02-13T00:46:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.09 , -0.04 2021-02-13T00:46:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177218.29 epoch sec; seqNo:15696; 4060029.40 north; 593132.96 east 2021-02-13T00:46:58Z- AHRS_M2 msg: 1613177218.66 epoch sec; seqNo:15574 2021-02-13T00:46:58Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.16 psi 2021-02-13T00:46:58Z- handleDepth()-Depth_Keller msg: 1613177218669 epoch sec; seqNo:15697; depth 27.48 2021-02-13T00:46:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177218.68 epoch sec; seqNo:15697; 4060029.78 north; 593132.95 east 2021-02-13T00:46:58Z- LcmTrn::handleMessages() - handling 46 messages took 0.70 ms and 132 calls... 2021-02-13T00:46:58Z- waiting for 1613177216.44, time is 1613177218.68 2021-02-13T00:46:58Z- LcmTrn::updateTrn() >>>> pose time 1613177218.68, meas time 1613177218.28 <<<< 2021-02-13T00:46:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177218.28 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:46:58Z- TerrainNav::motionUpdate() - currEstimate = (4060030, 593132.95, 27.48) 2021-02-13T00:46:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:46:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:46:58Z- TNavBF::Projecting Measurements 2021-02-13T00:46:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:46:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:46:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:46:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:46:58Z- TNavFilter::4 good beams. 2021-02-13T00:46:58Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:46:58Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:46:58Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:46:58Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:46:58Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:46:58Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:46:58Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:46:58Z- TNavBF::mmse selected from filter -1 2021-02-13T00:46:58Z- TerrainMapOctree: (vehN, vehE ) = (4060029.49, 593132.77). 2021-02-13T00:46:58Z- TerrainMapOctree: Min Distance = 1654.65. 2021-02-13T00:46:58Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:46:58Z- There is no specific good beam that all particles have in common. 2021-02-13T00:46:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:46:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-13T00:46:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:46:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 11.43 2021-02-13T00:46:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:46:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 6.17 2021-02-13T00:46:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:46:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:46:58Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:46:58Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:46:58Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:46:58Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:46:58Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:46:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:46:58Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:46:58Z- Filter 0 No beams used in subcloud update 2021-02-13T00:46:58Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:46:58Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:46:58Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 688.0 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:46:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177218.28 sec. 2021-02-13T00:46:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:46:58Z- TNavBF::mmse selected from filter -1 2021-02-13T00:46:58Z- variances([0], [2], and psi_berg) = 687.960909 832.992005 0.000000 2021-02-13T00:46:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:46:58Z- LcmTrn::publishEstimates() - MLE : 4060029.28 593154.45 27.48 2021-02-13T00:46:58Z- LcmTrn::publishEstimates() - MMSE : 4060029.78 593132.95 27.48 2021-02-13T00:46:58Z- LcmTrn::publishEstimates() - COVAR: 687.96 832.99 0.76 0.00 2021-02-13T00:46:59Z- AHRS_M2 msg: 1613177219.08 epoch sec; seqNo:15575 2021-02-13T00:46:59Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.16 psi 2021-02-13T00:46:59Z- handleDepth()-Depth_Keller msg: 1613177219095 epoch sec; seqNo:15698; depth 27.60 2021-02-13T00:46:59Z- handleDvl() - RDI_Pathfinder msg: 1613177219.12 epoch sec; seqNo:7536 2021-02-13T00:46:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 82.84 , 60.90 , 97.98 2021-02-13T00:46:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.12 , -0.02 2021-02-13T00:46:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177219.13 epoch sec; seqNo:15698; 4060030.16 north; 593132.95 east 2021-02-13T00:46:59Z- AHRS_M2 msg: 1613177219.47 epoch sec; seqNo:15576 2021-02-13T00:46:59Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.16 psi 2021-02-13T00:46:59Z- handleDepth()-Depth_Keller msg: 1613177219478 epoch sec; seqNo:15699; depth 27.74 2021-02-13T00:46:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177219.49 epoch sec; seqNo:15699; 4060030.54 north; 593132.95 east 2021-02-13T00:46:59Z- AHRS_M2 msg: 1613177219.87 epoch sec; seqNo:15577 2021-02-13T00:46:59Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.16 psi 2021-02-13T00:46:59Z- handleDepth()-Depth_Keller msg: 1613177219876 epoch sec; seqNo:15700; depth 27.87 2021-02-13T00:46:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177219.88 epoch sec; seqNo:15700; 4060030.54 north; 593132.95 east 2021-02-13T00:47:00Z- AHRS_M2 msg: 1613177220.28 epoch sec; seqNo:15578 2021-02-13T00:47:00Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.17 psi 2021-02-13T00:47:00Z- handleDepth()-Depth_Keller msg: 1613177220280 epoch sec; seqNo:15701; depth 28.00 2021-02-13T00:47:00Z- handleDvl() - RDI_Pathfinder msg: 1613177220.30 epoch sec; seqNo:7537 2021-02-13T00:47:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.25 , 82.84 , 60.90 , 93.65 2021-02-13T00:47:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.13 , 0.01 2021-02-13T00:47:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177220.31 epoch sec; seqNo:15701; 4060030.92 north; 593132.94 east 2021-02-13T00:47:00Z- AHRS_M2 msg: 1613177220.68 epoch sec; seqNo:15579 2021-02-13T00:47:00Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.17 psi 2021-02-13T00:47:00Z- handleDepth()-Depth_Keller msg: 1613177220686 epoch sec; seqNo:15702; depth 28.15 2021-02-13T00:47:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177220.70 epoch sec; seqNo:15702; 4060031.29 north; 593132.94 east 2021-02-13T00:47:01Z- AHRS_M2 msg: 1613177221.10 epoch sec; seqNo:15580 2021-02-13T00:47:01Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.17 psi 2021-02-13T00:47:01Z- handleDepth()-Depth_Keller msg: 1613177221110 epoch sec; seqNo:15703; depth 28.29 2021-02-13T00:47:01Z- handleDvl() - RDI_Pathfinder msg: 1613177221.12 epoch sec; seqNo:7538 2021-02-13T00:47:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 83.92 , 61.71 , 97.98 2021-02-13T00:47:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.10 , 0.04 2021-02-13T00:47:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177221.13 epoch sec; seqNo:15703; 4060031.67 north; 593132.93 east 2021-02-13T00:47:01Z- AHRS_M2 msg: 1613177221.49 epoch sec; seqNo:15581 2021-02-13T00:47:01Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.17 psi 2021-02-13T00:47:01Z- handleDepth()-Depth_Keller msg: 1613177221497 epoch sec; seqNo:15704; depth 28.43 2021-02-13T00:47:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:01Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:01Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:01Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:01Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:01Z- handleDvl() - RDI_Pathfinder msg: 1613177221.50 epoch sec; seqNo:7539 2021-02-13T00:47:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.10 , 0.04 2021-02-13T00:47:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177221.51 epoch sec; seqNo:15704; 4060032.06 north; 593134.15 east 2021-02-13T00:47:01Z- AHRS_M2 msg: 1613177221.89 epoch sec; seqNo:15582 2021-02-13T00:47:01Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.17 psi 2021-02-13T00:47:01Z- handleDepth()-Depth_Keller msg: 1613177221905 epoch sec; seqNo:15705; depth 28.56 2021-02-13T00:47:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177221.91 epoch sec; seqNo:15705; 4060032.06 north; 593134.15 east 2021-02-13T00:47:02Z- AHRS_M2 msg: 1613177222.30 epoch sec; seqNo:15583 2021-02-13T00:47:02Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.18 psi 2021-02-13T00:47:02Z- handleDepth()-Depth_Keller msg: 1613177222300 epoch sec; seqNo:15706; depth 28.69 2021-02-13T00:47:02Z- handleDvl() - RDI_Pathfinder msg: 1613177222.32 epoch sec; seqNo:7540 2021-02-13T00:47:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 82.84 , 60.49 , 97.98 2021-02-13T00:47:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.07 , 0.05 2021-02-13T00:47:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177222.33 epoch sec; seqNo:15706; 4060032.44 north; 593134.15 east 2021-02-13T00:47:02Z- AHRS_M2 msg: 1613177222.74 epoch sec; seqNo:15584 2021-02-13T00:47:02Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.18 psi 2021-02-13T00:47:02Z- handleDepth()-Depth_Keller msg: 1613177222740 epoch sec; seqNo:15707; depth 28.83 2021-02-13T00:47:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:02Z- handleDvl() - RDI_Pathfinder msg: 1613177222.74 epoch sec; seqNo:7541 2021-02-13T00:47:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.07 , 0.05 2021-02-13T00:47:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177222.75 epoch sec; seqNo:15707; 4060032.82 north; 593134.14 east 2021-02-13T00:47:03Z- AHRS_M2 msg: 1613177223.10 epoch sec; seqNo:15585 2021-02-13T00:47:03Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.18 psi 2021-02-13T00:47:03Z- handleDepth()-Depth_Keller msg: 1613177223108 epoch sec; seqNo:15708; depth 28.97 2021-02-13T00:47:03Z- handleDvl() - RDI_Pathfinder msg: 1613177223.12 epoch sec; seqNo:7542 2021-02-13T00:47:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.03 , 0.00 , 60.90 , 97.98 2021-02-13T00:47:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , 0.04 2021-02-13T00:47:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177223.14 epoch sec; seqNo:15708; 4060033.20 north; 593134.14 east 2021-02-13T00:47:03Z- AHRS_M2 msg: 1613177223.51 epoch sec; seqNo:15586 2021-02-13T00:47:03Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.18 psi 2021-02-13T00:47:03Z- handleDepth()-Depth_Keller msg: 1613177223512 epoch sec; seqNo:15709; depth 29.09 2021-02-13T00:47:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177223.53 epoch sec; seqNo:15709; 4060033.58 north; 593134.13 east 2021-02-13T00:47:03Z- AHRS_M2 msg: 1613177223.91 epoch sec; seqNo:15587 2021-02-13T00:47:03Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.18 psi 2021-02-13T00:47:03Z- handleDepth()-Depth_Keller msg: 1613177223916 epoch sec; seqNo:15710; depth 29.23 2021-02-13T00:47:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:03Z- handleDvl() - RDI_Pathfinder msg: 1613177223.92 epoch sec; seqNo:7543 2021-02-13T00:47:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , 0.04 2021-02-13T00:47:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177223.93 epoch sec; seqNo:15710; 4060033.59 north; 593135.35 east 2021-02-13T00:47:03Z- LcmTrn::handleMessages() - handling 47 messages took 0.67 ms and 134 calls... 2021-02-13T00:47:03Z- waiting for 1613177221.68, time is 1613177223.93 2021-02-13T00:47:03Z- LcmTrn::updateTrn() equating pose 1613177223.93 and meas times 1613177223.93 2021-02-13T00:47:03Z- LcmTrn::updateTrn() >>>> pose time 1613177223.93, meas time 1613177223.93 <<<< 2021-02-13T00:47:03Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177223.93, ping # 0. 2021-02-13T00:47:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177223.93 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:47:03Z- TerrainNav::motionUpdate() - currEstimate = (4060034, 593135.35, 29.23) 2021-02-13T00:47:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:47:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:47:03Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:47:03Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:47:03Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177223.93 sec.; No relevant navigation data available 2021-02-13T00:47:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:47:03Z- TNavBF::mmse selected from filter -1 2021-02-13T00:47:03Z- variances([0], [2], and psi_berg) = 687.910310 832.991982 0.000000 2021-02-13T00:47:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:47:03Z- LcmTrn::publishEstimates() - MLE : 4060033.09 593156.85 29.23 2021-02-13T00:47:03Z- LcmTrn::publishEstimates() - MMSE : 4060033.59 593135.35 29.23 2021-02-13T00:47:03Z- LcmTrn::publishEstimates() - COVAR: 687.91 832.99 0.99 0.00 2021-02-13T00:47:04Z- AHRS_M2 msg: 1613177224.32 epoch sec; seqNo:15588 2021-02-13T00:47:04Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:47:04Z- handleDepth()-Depth_Keller msg: 1613177224320 epoch sec; seqNo:15711; depth 29.36 2021-02-13T00:47:04Z- handleDvl() - RDI_Pathfinder msg: 1613177224.33 epoch sec; seqNo:7544 2021-02-13T00:47:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.62 , 82.84 , 59.67 , 97.98 2021-02-13T00:47:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , 0.01 2021-02-13T00:47:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177224.34 epoch sec; seqNo:15711; 4060033.97 north; 593135.35 east 2021-02-13T00:47:04Z- AHRS_M2 msg: 1613177224.72 epoch sec; seqNo:15589 2021-02-13T00:47:04Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.18 psi 2021-02-13T00:47:04Z- handleDepth()-Depth_Keller msg: 1613177224725 epoch sec; seqNo:15712; depth 29.51 2021-02-13T00:47:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177224.73 epoch sec; seqNo:15712; 4060034.35 north; 593135.35 east 2021-02-13T00:47:05Z- AHRS_M2 msg: 1613177225.13 epoch sec; seqNo:15590 2021-02-13T00:47:05Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.18 psi 2021-02-13T00:47:05Z- handleDepth()-Depth_Keller msg: 1613177225134 epoch sec; seqNo:15713; depth 29.66 2021-02-13T00:47:05Z- handleDvl() - RDI_Pathfinder msg: 1613177225.15 epoch sec; seqNo:7545 2021-02-13T00:47:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.57 , 82.84 , 58.44 , 97.98 2021-02-13T00:47:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.04 , -0.01 2021-02-13T00:47:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177225.16 epoch sec; seqNo:15713; 4060034.73 north; 593135.34 east 2021-02-13T00:47:05Z- AHRS_M2 msg: 1613177225.53 epoch sec; seqNo:15591 2021-02-13T00:47:05Z- AHRS_M2 msg: 0.62 phi; -0.26 theta; -0.18 psi 2021-02-13T00:47:05Z- handleDepth()-Depth_Keller msg: 1613177225532 epoch sec; seqNo:15714; depth 29.82 2021-02-13T00:47:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177225.54 epoch sec; seqNo:15714; 4060034.73 north; 593135.34 east 2021-02-13T00:47:05Z- AHRS_M2 msg: 1613177225.93 epoch sec; seqNo:15592 2021-02-13T00:47:05Z- AHRS_M2 msg: 0.62 phi; -0.22 theta; -0.17 psi 2021-02-13T00:47:05Z- handleDepth()-Depth_Keller msg: 1613177225936 epoch sec; seqNo:15715; depth 29.95 2021-02-13T00:47:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177225.94 epoch sec; seqNo:15715; 4060035.11 north; 593135.34 east 2021-02-13T00:47:06Z- AHRS_M2 msg: 1613177226.34 epoch sec; seqNo:15593 2021-02-13T00:47:06Z- AHRS_M2 msg: 0.61 phi; -0.18 theta; -0.16 psi 2021-02-13T00:47:06Z- handleDepth()-Depth_Keller msg: 1613177226341 epoch sec; seqNo:15716; depth 30.10 2021-02-13T00:47:06Z- handleDvl() - RDI_Pathfinder msg: 1613177226.35 epoch sec; seqNo:7546 2021-02-13T00:47:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:06Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:47:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177226.36 epoch sec; seqNo:15716; 4060035.50 north; 593136.55 east 2021-02-13T00:47:06Z- AHRS_M2 msg: 1613177226.74 epoch sec; seqNo:15594 2021-02-13T00:47:06Z- AHRS_M2 msg: 0.61 phi; -0.14 theta; -0.15 psi 2021-02-13T00:47:06Z- handleDepth()-Depth_Keller msg: 1613177226746 epoch sec; seqNo:15717; depth 30.22 2021-02-13T00:47:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177226.76 epoch sec; seqNo:15717; 4060035.88 north; 593136.55 east 2021-02-13T00:47:07Z- AHRS_M2 msg: 1613177227.15 epoch sec; seqNo:15595 2021-02-13T00:47:07Z- AHRS_M2 msg: 0.60 phi; -0.09 theta; -0.15 psi 2021-02-13T00:47:07Z- handleDepth()-Depth_Keller msg: 1613177227162 epoch sec; seqNo:15718; depth 30.34 2021-02-13T00:47:07Z- handleDvl() - RDI_Pathfinder msg: 1613177227.17 epoch sec; seqNo:7547 2021-02-13T00:47:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.53 , 78.17 , 62.53 , 0.00 2021-02-13T00:47:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , -0.08 2021-02-13T00:47:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177227.18 epoch sec; seqNo:15718; 4060036.26 north; 593136.55 east 2021-02-13T00:47:07Z- AHRS_M2 msg: 1613177227.55 epoch sec; seqNo:15596 2021-02-13T00:47:07Z- AHRS_M2 msg: 0.59 phi; -0.05 theta; -0.15 psi 2021-02-13T00:47:07Z- handleDepth()-Depth_Keller msg: 1613177227552 epoch sec; seqNo:15719; depth 30.46 2021-02-13T00:47:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177227.56 epoch sec; seqNo:15719; 4060036.26 north; 593136.55 east 2021-02-13T00:47:07Z- AHRS_M2 msg: 1613177227.95 epoch sec; seqNo:15597 2021-02-13T00:47:07Z- AHRS_M2 msg: 0.58 phi; -0.00 theta; -0.15 psi 2021-02-13T00:47:07Z- handleDepth()-Depth_Keller msg: 1613177227961 epoch sec; seqNo:15720; depth 30.54 2021-02-13T00:47:07Z- handleDvl() - RDI_Pathfinder msg: 1613177227.97 epoch sec; seqNo:7548 2021-02-13T00:47:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.15 , 0.00 , 66.03 , 0.00 2021-02-13T00:47:07Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:47:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177227.98 epoch sec; seqNo:15720; 4060036.64 north; 593136.54 east 2021-02-13T00:47:08Z- AHRS_M2 msg: 1613177228.36 epoch sec; seqNo:15598 2021-02-13T00:47:08Z- AHRS_M2 msg: 0.58 phi; 0.04 theta; -0.15 psi 2021-02-13T00:47:08Z- handleDepth()-Depth_Keller msg: 1613177228362 epoch sec; seqNo:15721; depth 30.60 2021-02-13T00:47:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177228.37 epoch sec; seqNo:15721; 4060037.02 north; 593136.54 east 2021-02-13T00:47:08Z- AHRS_M2 msg: 1613177228.77 epoch sec; seqNo:15599 2021-02-13T00:47:08Z- AHRS_M2 msg: 0.57 phi; 0.08 theta; -0.15 psi 2021-02-13T00:47:08Z- handleDepth()-Depth_Keller msg: 1613177228779 epoch sec; seqNo:15722; depth 30.65 2021-02-13T00:47:08Z- handleDvl() - RDI_Pathfinder msg: 1613177228.79 epoch sec; seqNo:7549 2021-02-13T00:47:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.18 , 0.00 , 0.00 , 64.74 2021-02-13T00:47:08Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:47:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177228.80 epoch sec; seqNo:15722; 4060037.41 north; 593137.75 east 2021-02-13T00:47:09Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-13T00:47:09Z- waiting for 1613177226.93, time is 1613177228.80 2021-02-13T00:47:09Z- LcmTrn::updateTrn() equating pose 1613177228.80 and meas times 1613177228.80 2021-02-13T00:47:09Z- LcmTrn::updateTrn() >>>> pose time 1613177228.80, meas time 1613177228.80 <<<< 2021-02-13T00:47:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177228.80 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:47:09Z- TerrainNav::motionUpdate() - currEstimate = (4060037, 593137.75, 30.65) 2021-02-13T00:47:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:47:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:47:09Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:47:09Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:47:09Z- TNavBF::Projecting Measurements 2021-02-13T00:47:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:47:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:47:09Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-13T00:47:09Z- TNavFilter::2 good beams. 2021-02-13T00:47:09Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:47:09Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:47:09Z- TNavBF:: beamIndex[1] = 3 2021-02-13T00:47:09Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:47:09Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:47:09Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-13T00:47:09Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:47:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:47:09Z- TerrainMapOctree: (vehN, vehE ) = (4060037.41, 593137.75). 2021-02-13T00:47:09Z- TerrainMapOctree: Min Distance = 1645.44. 2021-02-13T00:47:09Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:47:09Z- There is no specific good beam that all particles have in common. 2021-02-13T00:47:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:47:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.133 ExpectedMeasDiff: nan currMeas.covariance: 6.19 2021-02-13T00:47:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:47:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 6.71 2021-02-13T00:47:09Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:47:09Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:47:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:47:09Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:47:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:47:09Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:47:09Z- Filter 0 No beams used in subcloud update 2021-02-13T00:47:09Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:47:09Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:47:09Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 687.9 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:47:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177228.80 sec. 2021-02-13T00:47:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:47:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:47:09Z- variances([0], [2], and psi_berg) = 687.859738 832.991959 0.000000 2021-02-13T00:47:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:47:09Z- LcmTrn::publishEstimates() - MLE : 4060036.91 593159.25 30.65 2021-02-13T00:47:09Z- LcmTrn::publishEstimates() - MMSE : 4060037.41 593137.75 30.65 2021-02-13T00:47:09Z- LcmTrn::publishEstimates() - COVAR: 687.86 832.99 0.94 0.00 2021-02-13T00:47:09Z- AHRS_M2 msg: 1613177229.16 epoch sec; seqNo:15600 2021-02-13T00:47:09Z- AHRS_M2 msg: 0.56 phi; 0.11 theta; -0.16 psi 2021-02-13T00:47:09Z- handleDepth()-Depth_Keller msg: 1613177229168 epoch sec; seqNo:15723; depth 30.69 2021-02-13T00:47:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177229.18 epoch sec; seqNo:15723; 4060037.79 north; 593137.75 east 2021-02-13T00:47:09Z- AHRS_M2 msg: 1613177229.57 epoch sec; seqNo:15601 2021-02-13T00:47:09Z- AHRS_M2 msg: 0.55 phi; 0.15 theta; -0.16 psi 2021-02-13T00:47:09Z- handleDepth()-Depth_Keller msg: 1613177229572 epoch sec; seqNo:15724; depth 30.69 2021-02-13T00:47:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177229.58 epoch sec; seqNo:15724; 4060038.17 north; 593137.75 east 2021-02-13T00:47:09Z- AHRS_M2 msg: 1613177229.97 epoch sec; seqNo:15602 2021-02-13T00:47:09Z- AHRS_M2 msg: 0.54 phi; 0.18 theta; -0.16 psi 2021-02-13T00:47:09Z- handleDepth()-Depth_Keller msg: 1613177229978 epoch sec; seqNo:15725; depth 30.71 2021-02-13T00:47:09Z- handleDvl() - RDI_Pathfinder msg: 1613177229.99 epoch sec; seqNo:7550 2021-02-13T00:47:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.03 , 0.00 , 0.00 , 62.61 2021-02-13T00:47:09Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:47:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177230.00 epoch sec; seqNo:15725; 4060038.17 north; 593137.75 east 2021-02-13T00:47:10Z- AHRS_M2 msg: 1613177230.38 epoch sec; seqNo:15603 2021-02-13T00:47:10Z- AHRS_M2 msg: 0.53 phi; 0.20 theta; -0.17 psi 2021-02-13T00:47:10Z- handleDepth()-Depth_Keller msg: 1613177230380 epoch sec; seqNo:15726; depth 30.70 2021-02-13T00:47:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177230.40 epoch sec; seqNo:15726; 4060038.54 north; 593137.74 east 2021-02-13T00:47:10Z- AHRS_M2 msg: 1613177230.78 epoch sec; seqNo:15604 2021-02-13T00:47:10Z- AHRS_M2 msg: 0.53 phi; 0.23 theta; -0.17 psi 2021-02-13T00:47:10Z- handleDepth()-Depth_Keller msg: 1613177230784 epoch sec; seqNo:15727; depth 30.68 2021-02-13T00:47:10Z- handleDvl() - RDI_Pathfinder msg: 1613177230.80 epoch sec; seqNo:7551 2021-02-13T00:47:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.88 , 0.00 , 0.00 , 60.90 2021-02-13T00:47:10Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:47:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177230.82 epoch sec; seqNo:15727; 4060038.92 north; 593137.74 east 2021-02-13T00:47:11Z- AHRS_M2 msg: 1613177231.18 epoch sec; seqNo:15605 2021-02-13T00:47:11Z- AHRS_M2 msg: 0.52 phi; 0.25 theta; -0.17 psi 2021-02-13T00:47:11Z- handleDepth()-Depth_Keller msg: 1613177231188 epoch sec; seqNo:15728; depth 30.66 2021-02-13T00:47:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:11Z- handleDvl() - RDI_Pathfinder msg: 1613177231.19 epoch sec; seqNo:7552 2021-02-13T00:47:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:11Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:47:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177231.20 epoch sec; seqNo:15728; 4060039.30 north; 593137.73 east 2021-02-13T00:47:11Z- AHRS_M2 msg: 1613177231.59 epoch sec; seqNo:15606 2021-02-13T00:47:11Z- AHRS_M2 msg: 0.52 phi; 0.27 theta; -0.16 psi 2021-02-13T00:47:11Z- handleDepth()-Depth_Keller msg: 1613177231603 epoch sec; seqNo:15729; depth 30.62 2021-02-13T00:47:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177231.61 epoch sec; seqNo:15729; 4060039.68 north; 593137.73 east 2021-02-13T00:47:11Z- AHRS_M2 msg: 1613177231.99 epoch sec; seqNo:15607 2021-02-13T00:47:11Z- AHRS_M2 msg: 0.52 phi; 0.29 theta; -0.16 psi 2021-02-13T00:47:11Z- handleDepth()-Depth_Keller msg: 1613177231996 epoch sec; seqNo:15730; depth 30.57 2021-02-13T00:47:12Z- handleDvl() - RDI_Pathfinder msg: 1613177232.01 epoch sec; seqNo:7553 2021-02-13T00:47:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:12Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:47:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177232.03 epoch sec; seqNo:15730; 4060039.69 north; 593138.95 east 2021-02-13T00:47:12Z- AHRS_M2 msg: 1613177232.40 epoch sec; seqNo:15608 2021-02-13T00:47:12Z- AHRS_M2 msg: 0.52 phi; 0.31 theta; -0.16 psi 2021-02-13T00:47:12Z- handleDepth()-Depth_Keller msg: 1613177232400 epoch sec; seqNo:15731; depth 30.50 2021-02-13T00:47:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:12Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:12Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:12Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:12Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:12Z- handleDvl() - RDI_Pathfinder msg: 1613177232.40 epoch sec; seqNo:7554 2021-02-13T00:47:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:12Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:47:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177232.41 epoch sec; seqNo:15731; 4060040.07 north; 593138.95 east 2021-02-13T00:47:12Z- AHRS_M2 msg: 1613177232.80 epoch sec; seqNo:15609 2021-02-13T00:47:12Z- AHRS_M2 msg: 0.52 phi; 0.32 theta; -0.16 psi 2021-02-13T00:47:12Z- handleDepth()-Depth_Keller msg: 1613177232813 epoch sec; seqNo:15732; depth 30.44 2021-02-13T00:47:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177232.82 epoch sec; seqNo:15732; 4060040.45 north; 593138.94 east 2021-02-13T00:47:13Z- AHRS_M2 msg: 1613177233.20 epoch sec; seqNo:15610 2021-02-13T00:47:13Z- AHRS_M2 msg: 0.52 phi; 0.33 theta; -0.16 psi 2021-02-13T00:47:13Z- handleDepth()-Depth_Keller msg: 1613177233210 epoch sec; seqNo:15733; depth 30.37 2021-02-13T00:47:13Z- handleDvl() - RDI_Pathfinder msg: 1613177233.23 epoch sec; seqNo:7555 2021-02-13T00:47:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.39 , 79.59 , 0.00 , 59.67 2021-02-13T00:47:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , -0.09 2021-02-13T00:47:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177233.24 epoch sec; seqNo:15733; 4060040.83 north; 593138.94 east 2021-02-13T00:47:13Z- AHRS_M2 msg: 1613177233.61 epoch sec; seqNo:15611 2021-02-13T00:47:13Z- AHRS_M2 msg: 0.53 phi; 0.34 theta; -0.15 psi 2021-02-13T00:47:13Z- handleDepth()-Depth_Keller msg: 1613177233612 epoch sec; seqNo:15734; depth 30.27 2021-02-13T00:47:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:13Z- handleDvl() - RDI_Pathfinder msg: 1613177233.62 epoch sec; seqNo:7556 2021-02-13T00:47:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , -0.09 2021-02-13T00:47:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177233.68 epoch sec; seqNo:15734; 4060040.83 north; 593138.94 east 2021-02-13T00:47:14Z- AHRS_M2 msg: 1613177234.02 epoch sec; seqNo:15612 2021-02-13T00:47:14Z- AHRS_M2 msg: 0.53 phi; 0.36 theta; -0.15 psi 2021-02-13T00:47:14Z- handleDepth()-Depth_Keller msg: 1613177234025 epoch sec; seqNo:15735; depth 30.19 2021-02-13T00:47:14Z- handleDvl() - RDI_Pathfinder msg: 1613177234.04 epoch sec; seqNo:7557 2021-02-13T00:47:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 67.00 , 0.00 , 0.00 , 59.23 2021-02-13T00:47:14Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:47:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177234.06 epoch sec; seqNo:15735; 4060041.21 north; 593138.93 east 2021-02-13T00:47:14Z- LcmTrn::handleMessages() - handling 47 messages took 0.74 ms and 132 calls... 2021-02-13T00:47:14Z- waiting for 1613177231.80, time is 1613177234.06 2021-02-13T00:47:14Z- LcmTrn::updateTrn() equating pose 1613177234.06 and meas times 1613177234.06 2021-02-13T00:47:14Z- LcmTrn::updateTrn() >>>> pose time 1613177234.06, meas time 1613177234.06 <<<< 2021-02-13T00:47:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177234.06 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:47:14Z- TerrainNav::motionUpdate() - currEstimate = (4060041, 593138.93, 30.19) 2021-02-13T00:47:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:47:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:47:14Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:47:14Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:47:14Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177234.06 sec.; No relevant navigation data available 2021-02-13T00:47:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:47:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:47:14Z- variances([0], [2], and psi_berg) = 687.812237 832.991937 0.000000 2021-02-13T00:47:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:47:14Z- LcmTrn::publishEstimates() - MLE : 4060040.71 593160.43 30.19 2021-02-13T00:47:14Z- LcmTrn::publishEstimates() - MMSE : 4060041.21 593138.93 30.19 2021-02-13T00:47:14Z- LcmTrn::publishEstimates() - COVAR: 687.81 832.99 1.14 0.00 2021-02-13T00:47:14Z- AHRS_M2 msg: 1613177234.42 epoch sec; seqNo:15613 2021-02-13T00:47:14Z- AHRS_M2 msg: 0.53 phi; 0.36 theta; -0.15 psi 2021-02-13T00:47:14Z- handleDepth()-Depth_Keller msg: 1613177234423 epoch sec; seqNo:15736; depth 30.09 2021-02-13T00:47:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177234.43 epoch sec; seqNo:15736; 4060041.59 north; 593138.93 east 2021-02-13T00:47:14Z- AHRS_M2 msg: 1613177234.82 epoch sec; seqNo:15614 2021-02-13T00:47:14Z- AHRS_M2 msg: 0.53 phi; 0.37 theta; -0.15 psi 2021-02-13T00:47:14Z- handleDepth()-Depth_Keller msg: 1613177234826 epoch sec; seqNo:15737; depth 30.00 2021-02-13T00:47:14Z- handleDvl() - RDI_Pathfinder msg: 1613177234.84 epoch sec; seqNo:7558 2021-02-13T00:47:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 67.72 , 0.00 , 0.00 , 60.05 2021-02-13T00:47:14Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:47:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177234.85 epoch sec; seqNo:15737; 4060041.97 north; 593138.93 east 2021-02-13T00:47:15Z- AHRS_M2 msg: 1613177235.22 epoch sec; seqNo:15615 2021-02-13T00:47:15Z- AHRS_M2 msg: 0.53 phi; 0.37 theta; -0.15 psi 2021-02-13T00:47:15Z- handleDepth()-Depth_Keller msg: 1613177235228 epoch sec; seqNo:15738; depth 29.88 2021-02-13T00:47:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177235.25 epoch sec; seqNo:15738; 4060041.98 north; 593140.15 east 2021-02-13T00:47:15Z- AHRS_M2 msg: 1613177235.63 epoch sec; seqNo:15616 2021-02-13T00:47:15Z- AHRS_M2 msg: 0.53 phi; 0.37 theta; -0.14 psi 2021-02-13T00:47:15Z- handleDepth()-Depth_Keller msg: 1613177235632 epoch sec; seqNo:15739; depth 29.78 2021-02-13T00:47:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177235.64 epoch sec; seqNo:15739; 4060042.36 north; 593140.14 east 2021-02-13T00:47:16Z- AHRS_M2 msg: 1613177236.03 epoch sec; seqNo:15617 2021-02-13T00:47:16Z- AHRS_M2 msg: 0.53 phi; 0.37 theta; -0.14 psi 2021-02-13T00:47:16Z- handleDepth()-Depth_Keller msg: 1613177236036 epoch sec; seqNo:15740; depth 29.70 2021-02-13T00:47:16Z- handleDvl() - RDI_Pathfinder msg: 1613177236.05 epoch sec; seqNo:7559 2021-02-13T00:47:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 67.72 , 0.00 , 0.00 , 59.63 2021-02-13T00:47:16Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:47:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177236.06 epoch sec; seqNo:15740; 4060042.74 north; 593140.14 east 2021-02-13T00:47:16Z- AHRS_M2 msg: 1613177236.44 epoch sec; seqNo:15618 2021-02-13T00:47:16Z- AHRS_M2 msg: 0.53 phi; 0.37 theta; -0.14 psi 2021-02-13T00:47:16Z- handleDepth()-Depth_Keller msg: 1613177236442 epoch sec; seqNo:15741; depth 29.57 2021-02-13T00:47:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177236.45 epoch sec; seqNo:15741; 4060042.74 north; 593140.14 east 2021-02-13T00:47:16Z- AHRS_M2 msg: 1613177236.84 epoch sec; seqNo:15619 2021-02-13T00:47:16Z- AHRS_M2 msg: 0.53 phi; 0.37 theta; -0.14 psi 2021-02-13T00:47:16Z- handleDepth()-Depth_Keller msg: 1613177236847 epoch sec; seqNo:15742; depth 29.45 2021-02-13T00:47:16Z- handleDvl() - RDI_Pathfinder msg: 1613177236.88 epoch sec; seqNo:7560 2021-02-13T00:47:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 69.42 , 0.00 , 0.00 , 59.63 2021-02-13T00:47:16Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:47:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177236.89 epoch sec; seqNo:15742; 4060043.12 north; 593140.13 east 2021-02-13T00:47:17Z- AHRS_M2 msg: 1613177237.24 epoch sec; seqNo:15620 2021-02-13T00:47:17Z- AHRS_M2 msg: 0.53 phi; 0.37 theta; -0.14 psi 2021-02-13T00:47:17Z- handleDepth()-Depth_Keller msg: 1613177237253 epoch sec; seqNo:15743; depth 29.32 2021-02-13T00:47:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:17Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:17Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:17Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:17Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:17Z- handleDvl() - RDI_Pathfinder msg: 1613177237.26 epoch sec; seqNo:7561 2021-02-13T00:47:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:17Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:47:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177237.28 epoch sec; seqNo:15743; 4060043.50 north; 593140.13 east 2021-02-13T00:47:17Z- AHRS_M2 msg: 1613177237.65 epoch sec; seqNo:15621 2021-02-13T00:47:17Z- AHRS_M2 msg: 0.53 phi; 0.37 theta; -0.14 psi 2021-02-13T00:47:17Z- handleDepth()-Depth_Keller msg: 1613177237652 epoch sec; seqNo:15744; depth 29.22 2021-02-13T00:47:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177237.66 epoch sec; seqNo:15744; 4060043.50 north; 593140.13 east 2021-02-13T00:47:18Z- AHRS_M2 msg: 1613177238.05 epoch sec; seqNo:15622 2021-02-13T00:47:18Z- AHRS_M2 msg: 0.53 phi; 0.37 theta; -0.14 psi 2021-02-13T00:47:18Z- handleDepth()-Depth_Keller msg: 1613177238058 epoch sec; seqNo:15745; depth 29.11 2021-02-13T00:47:18Z- handleDvl() - RDI_Pathfinder msg: 1613177238.07 epoch sec; seqNo:7562 2021-02-13T00:47:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:18Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:47:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177238.08 epoch sec; seqNo:15745; 4060043.88 north; 593140.13 east 2021-02-13T00:47:18Z- AHRS_M2 msg: 1613177238.46 epoch sec; seqNo:15623 2021-02-13T00:47:18Z- AHRS_M2 msg: 0.54 phi; 0.37 theta; -0.13 psi 2021-02-13T00:47:18Z- handleDepth()-Depth_Keller msg: 1613177238460 epoch sec; seqNo:15746; depth 28.99 2021-02-13T00:47:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:18Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:18Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:18Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:18Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:18Z- handleDvl() - RDI_Pathfinder msg: 1613177238.46 epoch sec; seqNo:7563 2021-02-13T00:47:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:18Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:47:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177238.48 epoch sec; seqNo:15746; 4060044.25 north; 593140.12 east 2021-02-13T00:47:18Z- AHRS_M2 msg: 1613177238.86 epoch sec; seqNo:15624 2021-02-13T00:47:18Z- AHRS_M2 msg: 0.54 phi; 0.37 theta; -0.13 psi 2021-02-13T00:47:18Z- handleDepth()-Depth_Keller msg: 1613177238864 epoch sec; seqNo:15747; depth 28.88 2021-02-13T00:47:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177238.87 epoch sec; seqNo:15747; 4060044.25 north; 593140.12 east 2021-02-13T00:47:19Z- LcmTrn::handleMessages() - handling 42 messages took 0.60 ms and 130 calls... 2021-02-13T00:47:19Z- waiting for 1613177237.06, time is 1613177238.87 2021-02-13T00:47:19Z- LcmTrn::updateTrn() >>>> pose time 1613177238.87, meas time 1613177238.46 <<<< 2021-02-13T00:47:19Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177238.46, ping # 0. 2021-02-13T00:47:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177238.46 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:47:19Z- TerrainNav::motionUpdate() - currEstimate = (4060044, 593140.12, 28.88) 2021-02-13T00:47:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:47:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:47:19Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:47:19Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:47:19Z- TNavBF::Projecting Measurements 2021-02-13T00:47:19Z- TNavFilter::Measurement from time = 1613177238.46 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:47:19Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:47:19Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:47:19Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:47:19Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:47:19Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:47:19Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:47:19Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:47:19Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 687.8 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:47:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:47:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:47:19Z- variances([0], [2], and psi_berg) = 687.758540 832.991912 0.000000 2021-02-13T00:47:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:47:19Z- LcmTrn::publishEstimates() - MLE : 4060043.75 593160.62 28.88 2021-02-13T00:47:19Z- LcmTrn::publishEstimates() - MMSE : 4060044.25 593140.12 28.88 2021-02-13T00:47:19Z- LcmTrn::publishEstimates() - COVAR: 687.76 832.99 1.31 0.00 2021-02-13T00:47:19Z- AHRS_M2 msg: 1613177239.27 epoch sec; seqNo:15625 2021-02-13T00:47:19Z- AHRS_M2 msg: 0.55 phi; 0.36 theta; -0.13 psi 2021-02-13T00:47:19Z- handleDepth()-Depth_Keller msg: 1613177239277 epoch sec; seqNo:15748; depth 28.76 2021-02-13T00:47:19Z- handleDvl() - RDI_Pathfinder msg: 1613177239.29 epoch sec; seqNo:7564 2021-02-13T00:47:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 79.69 , 97.98 , 60.08 2021-02-13T00:47:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.08 , 0.03 2021-02-13T00:47:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177239.30 epoch sec; seqNo:15748; 4060044.65 north; 593141.34 east 2021-02-13T00:47:19Z- AHRS_M2 msg: 1613177239.67 epoch sec; seqNo:15626 2021-02-13T00:47:19Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.13 psi 2021-02-13T00:47:19Z- handleDepth()-Depth_Keller msg: 1613177239672 epoch sec; seqNo:15749; depth 28.65 2021-02-13T00:47:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:19Z- handleDvl() - RDI_Pathfinder msg: 1613177239.67 epoch sec; seqNo:7565 2021-02-13T00:47:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.08 , 0.03 2021-02-13T00:47:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177239.69 epoch sec; seqNo:15749; 4060045.03 north; 593141.33 east 2021-02-13T00:47:20Z- AHRS_M2 msg: 1613177240.09 epoch sec; seqNo:15627 2021-02-13T00:47:20Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.12 psi 2021-02-13T00:47:20Z- handleDepth()-Depth_Keller msg: 1613177240103 epoch sec; seqNo:15750; depth 28.55 2021-02-13T00:47:20Z- handleDvl() - RDI_Pathfinder msg: 1613177240.12 epoch sec; seqNo:7566 2021-02-13T00:47:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 80.67 , 97.98 , 62.12 2021-02-13T00:47:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.06 , -0.06 2021-02-13T00:47:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177240.13 epoch sec; seqNo:15750; 4060045.40 north; 593141.33 east 2021-02-13T00:47:20Z- AHRS_M2 msg: 1613177240.48 epoch sec; seqNo:15628 2021-02-13T00:47:20Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.12 psi 2021-02-13T00:47:20Z- handleDepth()-Depth_Keller msg: 1613177240480 epoch sec; seqNo:15751; depth 28.41 2021-02-13T00:47:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177240.49 epoch sec; seqNo:15751; 4060045.40 north; 593141.33 east 2021-02-13T00:47:20Z- AHRS_M2 msg: 1613177240.88 epoch sec; seqNo:15629 2021-02-13T00:47:20Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.12 psi 2021-02-13T00:47:20Z- handleDepth()-Depth_Keller msg: 1613177240886 epoch sec; seqNo:15752; depth 28.31 2021-02-13T00:47:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:20Z- handleDvl() - RDI_Pathfinder msg: 1613177240.89 epoch sec; seqNo:7567 2021-02-13T00:47:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.06 , -0.06 2021-02-13T00:47:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177240.89 epoch sec; seqNo:15752; 4060045.78 north; 593141.33 east 2021-02-13T00:47:21Z- AHRS_M2 msg: 1613177241.28 epoch sec; seqNo:15630 2021-02-13T00:47:21Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.12 psi 2021-02-13T00:47:21Z- handleDepth()-Depth_Keller msg: 1613177241288 epoch sec; seqNo:15753; depth 28.19 2021-02-13T00:47:21Z- handleDvl() - RDI_Pathfinder msg: 1613177241.31 epoch sec; seqNo:7568 2021-02-13T00:47:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 82.84 , 97.98 , 62.12 2021-02-13T00:47:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.03 , -0.12 2021-02-13T00:47:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177241.32 epoch sec; seqNo:15753; 4060046.16 north; 593141.32 east 2021-02-13T00:47:21Z- AHRS_M2 msg: 1613177241.69 epoch sec; seqNo:15631 2021-02-13T00:47:21Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.12 psi 2021-02-13T00:47:21Z- handleDepth()-Depth_Keller msg: 1613177241697 epoch sec; seqNo:15754; depth 28.09 2021-02-13T00:47:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177241.71 epoch sec; seqNo:15754; 4060046.17 north; 593142.54 east 2021-02-13T00:47:22Z- AHRS_M2 msg: 1613177242.10 epoch sec; seqNo:15632 2021-02-13T00:47:22Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.12 psi 2021-02-13T00:47:22Z- handleDepth()-Depth_Keller msg: 1613177242108 epoch sec; seqNo:15755; depth 27.96 2021-02-13T00:47:22Z- handleDvl() - RDI_Pathfinder msg: 1613177242.12 epoch sec; seqNo:7569 2021-02-13T00:47:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 81.69 , 97.91 , 61.71 2021-02-13T00:47:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , -0.13 2021-02-13T00:47:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177242.15 epoch sec; seqNo:15755; 4060046.55 north; 593142.54 east 2021-02-13T00:47:22Z- AHRS_M2 msg: 1613177242.50 epoch sec; seqNo:15633 2021-02-13T00:47:22Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.12 psi 2021-02-13T00:47:22Z- handleDepth()-Depth_Keller msg: 1613177242507 epoch sec; seqNo:15756; depth 27.83 2021-02-13T00:47:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177242.52 epoch sec; seqNo:15756; 4060046.93 north; 593142.53 east 2021-02-13T00:47:22Z- AHRS_M2 msg: 1613177242.90 epoch sec; seqNo:15634 2021-02-13T00:47:22Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.13 psi 2021-02-13T00:47:22Z- handleDepth()-Depth_Keller msg: 1613177242905 epoch sec; seqNo:15757; depth 27.73 2021-02-13T00:47:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177242.92 epoch sec; seqNo:15757; 4060046.93 north; 593142.53 east 2021-02-13T00:47:23Z- AHRS_M2 msg: 1613177243.30 epoch sec; seqNo:15635 2021-02-13T00:47:23Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.14 psi 2021-02-13T00:47:23Z- handleDepth()-Depth_Keller msg: 1613177243313 epoch sec; seqNo:15758; depth 27.61 2021-02-13T00:47:23Z- handleDvl() - RDI_Pathfinder msg: 1613177243.33 epoch sec; seqNo:7570 2021-02-13T00:47:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 0.00 , 97.98 , 62.12 2021-02-13T00:47:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.14 , -0.06 2021-02-13T00:47:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177243.35 epoch sec; seqNo:15758; 4060047.31 north; 593142.53 east 2021-02-13T00:47:23Z- AHRS_M2 msg: 1613177243.71 epoch sec; seqNo:15636 2021-02-13T00:47:23Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.14 psi 2021-02-13T00:47:23Z- handleDepth()-Depth_Keller msg: 1613177243712 epoch sec; seqNo:15759; depth 27.50 2021-02-13T00:47:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177243.72 epoch sec; seqNo:15759; 4060047.69 north; 593142.53 east 2021-02-13T00:47:24Z- AHRS_M2 msg: 1613177244.11 epoch sec; seqNo:15637 2021-02-13T00:47:24Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-13T00:47:24Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-13T00:47:24Z- waiting for 1613177241.87, time is 1613177244.11 2021-02-13T00:47:24Z- LcmTrn::updateTrn() >>>> pose time 1613177244.11, meas time 1613177243.33 <<<< 2021-02-13T00:47:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177243.33 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:47:24Z- TerrainNav::motionUpdate() - currEstimate = (4060048, 593142.53, 27.50) 2021-02-13T00:47:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:47:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:47:24Z- TNavBF::Projecting Measurements 2021-02-13T00:47:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:47:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:47:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:47:24Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:47:24Z- TNavFilter::3 good beams. 2021-02-13T00:47:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:47:24Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:47:24Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:47:24Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:47:24Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:47:24Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:47:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:47:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:47:24Z- TerrainMapOctree: (vehN, vehE ) = (4060047.17, 593142.16). 2021-02-13T00:47:24Z- TerrainMapOctree: Min Distance = 1635.26. 2021-02-13T00:47:24Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:47:24Z- There is no specific good beam that all particles have in common. 2021-02-13T00:47:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:47:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.49 K: 0.137 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-13T00:47:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:47:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:47:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:47:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.131 ExpectedMeasDiff: nan currMeas.covariance: 6.17 2021-02-13T00:47:24Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:47:24Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:47:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:47:24Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:47:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:47:24Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:47:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:47:24Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:47:24Z- Filter 0 No beams used in subcloud update 2021-02-13T00:47:24Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:47:24Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:47:24Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 687.7 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:47:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177243.33 sec. 2021-02-13T00:47:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:47:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:47:24Z- variances([0], [2], and psi_berg) = 687.694774 832.991883 0.000000 2021-02-13T00:47:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:47:24Z- LcmTrn::publishEstimates() - MLE : 4060047.19 593163.03 27.50 2021-02-13T00:47:24Z- LcmTrn::publishEstimates() - MMSE : 4060047.69 593142.53 27.50 2021-02-13T00:47:24Z- LcmTrn::publishEstimates() - COVAR: 687.69 832.99 1.07 0.00 2021-02-13T00:47:24Z- handleDepth()-Depth_Keller msg: 1613177244119 epoch sec; seqNo:15760; depth 27.39 2021-02-13T00:47:24Z- handleDvl() - RDI_Pathfinder msg: 1613177244.13 epoch sec; seqNo:7571 2021-02-13T00:47:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 82.84 , 97.98 , 62.12 2021-02-13T00:47:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.14 , 0.01 2021-02-13T00:47:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177244.15 epoch sec; seqNo:15760; 4060048.07 north; 593142.52 east 2021-02-13T00:47:24Z- AHRS_M2 msg: 1613177244.52 epoch sec; seqNo:15638 2021-02-13T00:47:24Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T00:47:24Z- handleDepth()-Depth_Keller msg: 1613177244520 epoch sec; seqNo:15761; depth 27.28 2021-02-13T00:47:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:24Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:24Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:24Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:24Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:24Z- handleDvl() - RDI_Pathfinder msg: 1613177244.52 epoch sec; seqNo:7572 2021-02-13T00:47:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.14 , 0.01 2021-02-13T00:47:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177244.54 epoch sec; seqNo:15761; 4060048.45 north; 593142.52 east 2021-02-13T00:47:24Z- AHRS_M2 msg: 1613177244.92 epoch sec; seqNo:15639 2021-02-13T00:47:24Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T00:47:24Z- handleDepth()-Depth_Keller msg: 1613177244929 epoch sec; seqNo:15762; depth 27.17 2021-02-13T00:47:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177244.94 epoch sec; seqNo:15762; 4060048.45 north; 593142.52 east 2021-02-13T00:47:25Z- AHRS_M2 msg: 1613177245.33 epoch sec; seqNo:15640 2021-02-13T00:47:25Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T00:47:25Z- handleDepth()-Depth_Keller msg: 1613177245331 epoch sec; seqNo:15763; depth 27.04 2021-02-13T00:47:25Z- handleDvl() - RDI_Pathfinder msg: 1613177245.34 epoch sec; seqNo:7573 2021-02-13T00:47:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 82.84 , 97.98 , 63.35 2021-02-13T00:47:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , 0.06 2021-02-13T00:47:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177245.35 epoch sec; seqNo:15763; 4060048.84 north; 593143.73 east 2021-02-13T00:47:25Z- AHRS_M2 msg: 1613177245.73 epoch sec; seqNo:15641 2021-02-13T00:47:25Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T00:47:25Z- handleDepth()-Depth_Keller msg: 1613177245734 epoch sec; seqNo:15764; depth 26.95 2021-02-13T00:47:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:25Z- handleDvl() - RDI_Pathfinder msg: 1613177245.74 epoch sec; seqNo:7574 2021-02-13T00:47:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , 0.06 2021-02-13T00:47:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177245.75 epoch sec; seqNo:15764; 4060049.22 north; 593143.73 east 2021-02-13T00:47:26Z- AHRS_M2 msg: 1613177246.13 epoch sec; seqNo:15642 2021-02-13T00:47:26Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T00:47:26Z- handleDepth()-Depth_Keller msg: 1613177246151 epoch sec; seqNo:15765; depth 26.86 2021-02-13T00:47:26Z- handleDvl() - RDI_Pathfinder msg: 1613177246.16 epoch sec; seqNo:7575 2021-02-13T00:47:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 83.85 , 93.59 , 62.94 2021-02-13T00:47:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.07 2021-02-13T00:47:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177246.17 epoch sec; seqNo:15765; 4060049.60 north; 593143.73 east 2021-02-13T00:47:26Z- AHRS_M2 msg: 1613177246.54 epoch sec; seqNo:15643 2021-02-13T00:47:26Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T00:47:26Z- handleDepth()-Depth_Keller msg: 1613177246540 epoch sec; seqNo:15766; depth 26.73 2021-02-13T00:47:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177246.55 epoch sec; seqNo:15766; 4060049.60 north; 593143.73 east 2021-02-13T00:47:26Z- AHRS_M2 msg: 1613177246.94 epoch sec; seqNo:15644 2021-02-13T00:47:26Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-13T00:47:26Z- handleDepth()-Depth_Keller msg: 1613177246944 epoch sec; seqNo:15767; depth 26.62 2021-02-13T00:47:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:26Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:26Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:26Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:26Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:26Z- handleDvl() - RDI_Pathfinder msg: 1613177246.95 epoch sec; seqNo:7576 2021-02-13T00:47:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.07 2021-02-13T00:47:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177246.95 epoch sec; seqNo:15767; 4060049.98 north; 593143.72 east 2021-02-13T00:47:27Z- AHRS_M2 msg: 1613177247.34 epoch sec; seqNo:15645 2021-02-13T00:47:27Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-13T00:47:27Z- handleDepth()-Depth_Keller msg: 1613177247354 epoch sec; seqNo:15768; depth 26.50 2021-02-13T00:47:27Z- handleDvl() - RDI_Pathfinder msg: 1613177247.37 epoch sec; seqNo:7577 2021-02-13T00:47:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:27Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:47:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177247.38 epoch sec; seqNo:15768; 4060050.36 north; 593143.72 east 2021-02-13T00:47:27Z- AHRS_M2 msg: 1613177247.76 epoch sec; seqNo:15646 2021-02-13T00:47:27Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T00:47:27Z- handleDepth()-Depth_Keller msg: 1613177247761 epoch sec; seqNo:15769; depth 26.40 2021-02-13T00:47:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177247.78 epoch sec; seqNo:15769; 4060050.75 north; 593144.93 east 2021-02-13T00:47:28Z- AHRS_M2 msg: 1613177248.15 epoch sec; seqNo:15647 2021-02-13T00:47:28Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:47:28Z- handleDepth()-Depth_Keller msg: 1613177248159 epoch sec; seqNo:15770; depth 26.30 2021-02-13T00:47:28Z- handleDvl() - RDI_Pathfinder msg: 1613177248.17 epoch sec; seqNo:7578 2021-02-13T00:47:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 0.00 , 97.24 , 64.16 2021-02-13T00:47:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , -0.01 2021-02-13T00:47:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177248.18 epoch sec; seqNo:15770; 4060050.75 north; 593144.93 east 2021-02-13T00:47:28Z- AHRS_M2 msg: 1613177248.56 epoch sec; seqNo:15648 2021-02-13T00:47:28Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:47:28Z- handleDepth()-Depth_Keller msg: 1613177248562 epoch sec; seqNo:15771; depth 26.19 2021-02-13T00:47:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177248.58 epoch sec; seqNo:15771; 4060051.13 north; 593144.93 east 2021-02-13T00:47:28Z- AHRS_M2 msg: 1613177248.96 epoch sec; seqNo:15649 2021-02-13T00:47:28Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:47:28Z- handleDepth()-Depth_Keller msg: 1613177248969 epoch sec; seqNo:15772; depth 26.09 2021-02-13T00:47:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177248.98 epoch sec; seqNo:15772; 4060051.51 north; 593144.93 east 2021-02-13T00:47:29Z- LcmTrn::handleMessages() - handling 46 messages took 0.64 ms and 133 calls... 2021-02-13T00:47:29Z- waiting for 1613177247.11, time is 1613177248.98 2021-02-13T00:47:29Z- LcmTrn::updateTrn() >>>> pose time 1613177248.98, meas time 1613177248.17 <<<< 2021-02-13T00:47:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177248.17 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:47:29Z- TerrainNav::motionUpdate() - currEstimate = (4060052, 593144.93, 26.09) 2021-02-13T00:47:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:47:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:47:29Z- TNavBF::Projecting Measurements 2021-02-13T00:47:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:47:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:47:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:47:29Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:47:29Z- TNavFilter::3 good beams. 2021-02-13T00:47:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:47:29Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:47:29Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:47:29Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:47:29Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:47:29Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:47:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:47:29Z- TNavBF::mmse selected from filter -1 2021-02-13T00:47:29Z- TerrainMapOctree: (vehN, vehE ) = (4060050.87, 593144.53). 2021-02-13T00:47:29Z- TerrainMapOctree: Min Distance = 1630.94. 2021-02-13T00:47:29Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:47:29Z- There is no specific good beam that all particles have in common. 2021-02-13T00:47:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:47:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.117 ExpectedMeasDiff: nan currMeas.covariance: 7.81 2021-02-13T00:47:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:47:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-13T00:47:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:47:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 6.59 2021-02-13T00:47:29Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:47:29Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:47:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:47:29Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:47:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:47:29Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:47:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:47:29Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:47:29Z- Filter 0 No beams used in subcloud update 2021-02-13T00:47:29Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:47:29Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:47:29Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 687.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:47:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177248.17 sec. 2021-02-13T00:47:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:47:29Z- TNavBF::mmse selected from filter -1 2021-02-13T00:47:29Z- variances([0], [2], and psi_berg) = 687.628171 832.991852 0.000000 2021-02-13T00:47:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:47:29Z- LcmTrn::publishEstimates() - MLE : 4060051.01 593164.43 26.09 2021-02-13T00:47:29Z- LcmTrn::publishEstimates() - MMSE : 4060051.51 593144.93 26.09 2021-02-13T00:47:29Z- LcmTrn::publishEstimates() - COVAR: 687.63 832.99 0.97 0.00 2021-02-13T00:47:29Z- AHRS_M2 msg: 1613177249.36 epoch sec; seqNo:15650 2021-02-13T00:47:29Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:47:29Z- handleDepth()-Depth_Keller msg: 1613177249369 epoch sec; seqNo:15773; depth 25.99 2021-02-13T00:47:29Z- handleDvl() - RDI_Pathfinder msg: 1613177249.38 epoch sec; seqNo:7579 2021-02-13T00:47:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 86.08 , 97.98 , 63.76 2021-02-13T00:47:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , -0.04 2021-02-13T00:47:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177249.39 epoch sec; seqNo:15773; 4060051.88 north; 593144.92 east 2021-02-13T00:47:29Z- AHRS_M2 msg: 1613177249.77 epoch sec; seqNo:15651 2021-02-13T00:47:29Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:47:29Z- handleDepth()-Depth_Keller msg: 1613177249781 epoch sec; seqNo:15774; depth 25.87 2021-02-13T00:47:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177249.79 epoch sec; seqNo:15774; 4060052.26 north; 593144.92 east 2021-02-13T00:47:30Z- AHRS_M2 msg: 1613177250.17 epoch sec; seqNo:15652 2021-02-13T00:47:30Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:47:30Z- handleDepth()-Depth_Keller msg: 1613177250186 epoch sec; seqNo:15775; depth 25.76 2021-02-13T00:47:30Z- handleDvl() - RDI_Pathfinder msg: 1613177250.20 epoch sec; seqNo:7580 2021-02-13T00:47:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 86.08 , 94.74 , 63.35 2021-02-13T00:47:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.09 , -0.03 2021-02-13T00:47:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177250.21 epoch sec; seqNo:15775; 4060052.26 north; 593144.92 east 2021-02-13T00:47:30Z- AHRS_M2 msg: 1613177250.58 epoch sec; seqNo:15653 2021-02-13T00:47:30Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:47:30Z- handleDepth()-Depth_Keller msg: 1613177250580 epoch sec; seqNo:15776; depth 25.67 2021-02-13T00:47:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:30Z- handleDvl() - RDI_Pathfinder msg: 1613177250.58 epoch sec; seqNo:7581 2021-02-13T00:47:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.09 , -0.03 2021-02-13T00:47:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177250.60 epoch sec; seqNo:15776; 4060052.66 north; 593146.13 east 2021-02-13T00:47:30Z- AHRS_M2 msg: 1613177250.98 epoch sec; seqNo:15654 2021-02-13T00:47:30Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-13T00:47:30Z- handleDepth()-Depth_Keller msg: 1613177250991 epoch sec; seqNo:15777; depth 25.56 2021-02-13T00:47:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177251.00 epoch sec; seqNo:15777; 4060053.03 north; 593146.13 east 2021-02-13T00:47:31Z- AHRS_M2 msg: 1613177251.39 epoch sec; seqNo:15655 2021-02-13T00:47:31Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-13T00:47:31Z- handleDepth()-Depth_Keller msg: 1613177251394 epoch sec; seqNo:15778; depth 25.45 2021-02-13T00:47:31Z- handleDvl() - RDI_Pathfinder msg: 1613177251.41 epoch sec; seqNo:7582 2021-02-13T00:47:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 83.92 , 94.74 , 64.57 2021-02-13T00:47:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.11 , -0.01 2021-02-13T00:47:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177251.42 epoch sec; seqNo:15778; 4060053.41 north; 593146.13 east 2021-02-13T00:47:31Z- AHRS_M2 msg: 1613177251.79 epoch sec; seqNo:15656 2021-02-13T00:47:31Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-13T00:47:31Z- handleDepth()-Depth_Keller msg: 1613177251792 epoch sec; seqNo:15779; depth 25.34 2021-02-13T00:47:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:31Z- handleDvl() - RDI_Pathfinder msg: 1613177251.79 epoch sec; seqNo:7583 2021-02-13T00:47:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.11 , -0.01 2021-02-13T00:47:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177251.81 epoch sec; seqNo:15779; 4060053.79 north; 593146.12 east 2021-02-13T00:47:32Z- AHRS_M2 msg: 1613177252.19 epoch sec; seqNo:15657 2021-02-13T00:47:32Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-13T00:47:32Z- handleDepth()-Depth_Keller msg: 1613177252196 epoch sec; seqNo:15780; depth 25.24 2021-02-13T00:47:32Z- handleDvl() - RDI_Pathfinder msg: 1613177252.21 epoch sec; seqNo:7584 2021-02-13T00:47:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 83.85 , 97.91 , 64.98 2021-02-13T00:47:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , 0.01 2021-02-13T00:47:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177252.22 epoch sec; seqNo:15780; 4060053.79 north; 593146.12 east 2021-02-13T00:47:32Z- AHRS_M2 msg: 1613177252.60 epoch sec; seqNo:15658 2021-02-13T00:47:32Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-13T00:47:32Z- handleDepth()-Depth_Keller msg: 1613177252602 epoch sec; seqNo:15781; depth 25.11 2021-02-13T00:47:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177252.61 epoch sec; seqNo:15781; 4060054.17 north; 593146.12 east 2021-02-13T00:47:33Z- AHRS_M2 msg: 1613177253.00 epoch sec; seqNo:15659 2021-02-13T00:47:33Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-13T00:47:33Z- handleDepth()-Depth_Keller msg: 1613177253005 epoch sec; seqNo:15782; depth 25.02 2021-02-13T00:47:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:33Z- handleDvl() - RDI_Pathfinder msg: 1613177253.01 epoch sec; seqNo:7585 2021-02-13T00:47:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , 0.01 2021-02-13T00:47:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177253.03 epoch sec; seqNo:15782; 4060054.55 north; 593146.11 east 2021-02-13T00:47:33Z- AHRS_M2 msg: 1613177253.40 epoch sec; seqNo:15660 2021-02-13T00:47:33Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-13T00:47:33Z- handleDepth()-Depth_Keller msg: 1613177253413 epoch sec; seqNo:15783; depth 24.92 2021-02-13T00:47:33Z- handleDvl() - RDI_Pathfinder msg: 1613177253.42 epoch sec; seqNo:7586 2021-02-13T00:47:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 84.86 , 97.85 , 64.57 2021-02-13T00:47:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , 0.02 2021-02-13T00:47:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177253.45 epoch sec; seqNo:15783; 4060054.94 north; 593147.33 east 2021-02-13T00:47:33Z- AHRS_M2 msg: 1613177253.81 epoch sec; seqNo:15661 2021-02-13T00:47:33Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-13T00:47:33Z- handleDepth()-Depth_Keller msg: 1613177253812 epoch sec; seqNo:15784; depth 24.81 2021-02-13T00:47:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177253.82 epoch sec; seqNo:15784; 4060055.32 north; 593147.33 east 2021-02-13T00:47:34Z- AHRS_M2 msg: 1613177254.21 epoch sec; seqNo:15662 2021-02-13T00:47:34Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-13T00:47:34Z- handleDepth()-Depth_Keller msg: 1613177254221 epoch sec; seqNo:15785; depth 24.71 2021-02-13T00:47:34Z- handleDvl() - RDI_Pathfinder msg: 1613177254.23 epoch sec; seqNo:7587 2021-02-13T00:47:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.52 , 85.00 , 97.98 , 0.00 2021-02-13T00:47:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.00 , 0.01 2021-02-13T00:47:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177254.24 epoch sec; seqNo:15785; 4060055.32 north; 593147.33 east 2021-02-13T00:47:34Z- LcmTrn::handleMessages() - handling 48 messages took 0.69 ms and 134 calls... 2021-02-13T00:47:34Z- waiting for 1613177251.98, time is 1613177254.24 2021-02-13T00:47:34Z- LcmTrn::updateTrn() equating pose 1613177254.24 and meas times 1613177254.24 2021-02-13T00:47:34Z- LcmTrn::updateTrn() >>>> pose time 1613177254.24, meas time 1613177254.24 <<<< 2021-02-13T00:47:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177254.24 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:47:34Z- TerrainNav::motionUpdate() - currEstimate = (4060055, 593147.33, 24.71) 2021-02-13T00:47:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:47:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:47:34Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177254.24 sec.; No relevant navigation data available 2021-02-13T00:47:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:47:34Z- TNavBF::mmse selected from filter -1 2021-02-13T00:47:34Z- variances([0], [2], and psi_berg) = 687.577747 832.991829 0.000000 2021-02-13T00:47:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:47:34Z- LcmTrn::publishEstimates() - MLE : 4060054.82 593167.83 24.71 2021-02-13T00:47:34Z- LcmTrn::publishEstimates() - MMSE : 4060055.32 593147.33 24.71 2021-02-13T00:47:34Z- LcmTrn::publishEstimates() - COVAR: 687.58 832.99 1.20 0.00 2021-02-13T00:47:34Z- AHRS_M2 msg: 1613177254.62 epoch sec; seqNo:15663 2021-02-13T00:47:34Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-13T00:47:34Z- handleDepth()-Depth_Keller msg: 1613177254620 epoch sec; seqNo:15786; depth 24.60 2021-02-13T00:47:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177254.64 epoch sec; seqNo:15786; 4060055.70 north; 593147.32 east 2021-02-13T00:47:35Z- AHRS_M2 msg: 1613177255.02 epoch sec; seqNo:15664 2021-02-13T00:47:35Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-13T00:47:35Z- handleDepth()-Depth_Keller msg: 1613177255026 epoch sec; seqNo:15787; depth 24.49 2021-02-13T00:47:35Z- handleDvl() - RDI_Pathfinder msg: 1613177255.04 epoch sec; seqNo:7588 2021-02-13T00:47:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.52 , 0.00 , 98.18 , 64.98 2021-02-13T00:47:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , -0.04 2021-02-13T00:47:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177255.07 epoch sec; seqNo:15787; 4060056.08 north; 593147.32 east 2021-02-13T00:47:35Z- AHRS_M2 msg: 1613177255.42 epoch sec; seqNo:15665 2021-02-13T00:47:35Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-13T00:47:35Z- handleDepth()-Depth_Keller msg: 1613177255433 epoch sec; seqNo:15788; depth 24.38 2021-02-13T00:47:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:35Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:35Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:35Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:35Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:35Z- handleDvl() - RDI_Pathfinder msg: 1613177255.43 epoch sec; seqNo:7589 2021-02-13T00:47:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , -0.04 2021-02-13T00:47:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177255.45 epoch sec; seqNo:15788; 4060056.46 north; 593147.31 east 2021-02-13T00:47:35Z- AHRS_M2 msg: 1613177255.83 epoch sec; seqNo:15666 2021-02-13T00:47:35Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-13T00:47:35Z- handleDepth()-Depth_Keller msg: 1613177255834 epoch sec; seqNo:15789; depth 24.27 2021-02-13T00:47:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177255.84 epoch sec; seqNo:15789; 4060056.47 north; 593148.53 east 2021-02-13T00:47:36Z- AHRS_M2 msg: 1613177256.23 epoch sec; seqNo:15667 2021-02-13T00:47:36Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-13T00:47:36Z- handleDepth()-Depth_Keller msg: 1613177256237 epoch sec; seqNo:15790; depth 24.17 2021-02-13T00:47:36Z- handleDvl() - RDI_Pathfinder msg: 1613177256.26 epoch sec; seqNo:7590 2021-02-13T00:47:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.52 , 87.16 , 97.98 , 66.21 2021-02-13T00:47:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , -0.06 2021-02-13T00:47:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177256.27 epoch sec; seqNo:15790; 4060056.85 north; 593148.53 east 2021-02-13T00:47:36Z- AHRS_M2 msg: 1613177256.64 epoch sec; seqNo:15668 2021-02-13T00:47:36Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-13T00:47:36Z- handleDepth()-Depth_Keller msg: 1613177256640 epoch sec; seqNo:15791; depth 24.06 2021-02-13T00:47:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:36Z- handleDvl() - RDI_Pathfinder msg: 1613177256.64 epoch sec; seqNo:7591 2021-02-13T00:47:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , -0.06 2021-02-13T00:47:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177256.65 epoch sec; seqNo:15791; 4060057.23 north; 593148.53 east 2021-02-13T00:47:37Z- AHRS_M2 msg: 1613177257.04 epoch sec; seqNo:15669 2021-02-13T00:47:37Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-13T00:47:37Z- handleDepth()-Depth_Keller msg: 1613177257046 epoch sec; seqNo:15792; depth 23.96 2021-02-13T00:47:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177257.06 epoch sec; seqNo:15792; 4060057.61 north; 593148.52 east 2021-02-13T00:47:37Z- AHRS_M2 msg: 1613177257.44 epoch sec; seqNo:15670 2021-02-13T00:47:37Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-13T00:47:37Z- handleDepth()-Depth_Keller msg: 1613177257448 epoch sec; seqNo:15793; depth 23.84 2021-02-13T00:47:37Z- handleDvl() - RDI_Pathfinder msg: 1613177257.46 epoch sec; seqNo:7592 2021-02-13T00:47:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:37Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:47:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177257.48 epoch sec; seqNo:15793; 4060057.61 north; 593148.52 east 2021-02-13T00:47:37Z- AHRS_M2 msg: 1613177257.85 epoch sec; seqNo:15671 2021-02-13T00:47:37Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-13T00:47:37Z- handleDepth()-Depth_Keller msg: 1613177257853 epoch sec; seqNo:15794; depth 23.73 2021-02-13T00:47:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:37Z- handleDvl() - RDI_Pathfinder msg: 1613177257.86 epoch sec; seqNo:7593 2021-02-13T00:47:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:37Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:47:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177257.87 epoch sec; seqNo:15794; 4060057.99 north; 593148.52 east 2021-02-13T00:47:38Z- AHRS_M2 msg: 1613177258.25 epoch sec; seqNo:15672 2021-02-13T00:47:38Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-13T00:47:38Z- handleDepth()-Depth_Keller msg: 1613177258265 epoch sec; seqNo:15795; depth 23.64 2021-02-13T00:47:38Z- handleDvl() - RDI_Pathfinder msg: 1613177258.28 epoch sec; seqNo:7594 2021-02-13T00:47:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 87.57 , 97.98 , 67.03 2021-02-13T00:47:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , -0.03 2021-02-13T00:47:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177258.30 epoch sec; seqNo:15795; 4060058.38 north; 593149.73 east 2021-02-13T00:47:38Z- AHRS_M2 msg: 1613177258.66 epoch sec; seqNo:15673 2021-02-13T00:47:38Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-13T00:47:38Z- handleDepth()-Depth_Keller msg: 1613177258665 epoch sec; seqNo:15796; depth 23.51 2021-02-13T00:47:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177258.68 epoch sec; seqNo:15796; 4060058.76 north; 593149.73 east 2021-02-13T00:47:39Z- AHRS_M2 msg: 1613177259.06 epoch sec; seqNo:15674 2021-02-13T00:47:39Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-13T00:47:39Z- handleDepth()-Depth_Keller msg: 1613177259064 epoch sec; seqNo:15797; depth 23.42 2021-02-13T00:47:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:39Z- handleDvl() - RDI_Pathfinder msg: 1613177259.07 epoch sec; seqNo:7595 2021-02-13T00:47:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , -0.03 2021-02-13T00:47:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177259.07 epoch sec; seqNo:15797; 4060058.76 north; 593149.73 east 2021-02-13T00:47:39Z- LcmTrn::handleMessages() - handling 44 messages took 0.61 ms and 132 calls... 2021-02-13T00:47:39Z- waiting for 1613177257.24, time is 1613177259.07 2021-02-13T00:47:39Z- LcmTrn::updateTrn() equating pose 1613177259.07 and meas times 1613177259.07 2021-02-13T00:47:39Z- LcmTrn::updateTrn() >>>> pose time 1613177259.07, meas time 1613177259.07 <<<< 2021-02-13T00:47:39Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177259.07, ping # 0. 2021-02-13T00:47:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177259.07 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:47:39Z- TerrainNav::motionUpdate() - currEstimate = (4060059, 593149.73, 23.42) 2021-02-13T00:47:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:47:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:47:39Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:47:39Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:47:39Z- TNavBF::Projecting Measurements 2021-02-13T00:47:39Z- TNavFilter::Measurement from time = 1613177259.07 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:47:39Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:47:39Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:47:39Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:47:39Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:47:39Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:47:39Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:47:39Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:47:39Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 687.5 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T00:47:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:47:39Z- TNavBF::mmse selected from filter -1 2021-02-13T00:47:39Z- variances([0], [2], and psi_berg) = 687.529134 832.991806 0.000000 2021-02-13T00:47:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:47:39Z- LcmTrn::publishEstimates() - MLE : 4060058.26 593170.23 23.42 2021-02-13T00:47:39Z- LcmTrn::publishEstimates() - MMSE : 4060058.76 593149.73 23.42 2021-02-13T00:47:39Z- LcmTrn::publishEstimates() - COVAR: 687.53 832.99 1.41 0.00 2021-02-13T00:47:39Z- AHRS_M2 msg: 1613177259.46 epoch sec; seqNo:15675 2021-02-13T00:47:39Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-13T00:47:39Z- handleDepth()-Depth_Keller msg: 1613177259477 epoch sec; seqNo:15798; depth 23.29 2021-02-13T00:47:39Z- handleDvl() - RDI_Pathfinder msg: 1613177259.49 epoch sec; seqNo:7596 2021-02-13T00:47:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:39Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:47:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177259.50 epoch sec; seqNo:15798; 4060059.14 north; 593149.73 east 2021-02-13T00:47:39Z- AHRS_M2 msg: 1613177259.87 epoch sec; seqNo:15676 2021-02-13T00:47:39Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-13T00:47:39Z- handleDepth()-Depth_Keller msg: 1613177259872 epoch sec; seqNo:15799; depth 23.20 2021-02-13T00:47:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177259.89 epoch sec; seqNo:15799; 4060059.51 north; 593149.72 east 2021-02-13T00:47:40Z- AHRS_M2 msg: 1613177260.27 epoch sec; seqNo:15677 2021-02-13T00:47:40Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-13T00:47:40Z- handleDepth()-Depth_Keller msg: 1613177260276 epoch sec; seqNo:15800; depth 23.10 2021-02-13T00:47:40Z- handleDvl() - RDI_Pathfinder msg: 1613177260.29 epoch sec; seqNo:7597 2021-02-13T00:47:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 86.42 , 97.24 , 66.62 2021-02-13T00:47:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , 0.06 2021-02-13T00:47:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177260.32 epoch sec; seqNo:15800; 4060059.89 north; 593149.72 east 2021-02-13T00:47:40Z- AHRS_M2 msg: 1613177260.68 epoch sec; seqNo:15678 2021-02-13T00:47:40Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-13T00:47:40Z- handleDepth()-Depth_Keller msg: 1613177260680 epoch sec; seqNo:15801; depth 22.97 2021-02-13T00:47:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177260.69 epoch sec; seqNo:15801; 4060059.89 north; 593149.72 east 2021-02-13T00:47:41Z- AHRS_M2 msg: 1613177261.11 epoch sec; seqNo:15679 2021-02-13T00:47:41Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-13T00:47:41Z- handleDepth()-Depth_Keller msg: 1613177261110 epoch sec; seqNo:15802; depth 22.88 2021-02-13T00:47:41Z- handleDvl() - RDI_Pathfinder msg: 1613177261.12 epoch sec; seqNo:7598 2021-02-13T00:47:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 0.00 , 96.70 , 65.80 2021-02-13T00:47:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.07 2021-02-13T00:47:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177261.15 epoch sec; seqNo:15802; 4060060.29 north; 593150.93 east 2021-02-13T00:47:41Z- AHRS_M2 msg: 1613177261.48 epoch sec; seqNo:15680 2021-02-13T00:47:41Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-13T00:47:41Z- handleDepth()-Depth_Keller msg: 1613177261488 epoch sec; seqNo:15803; depth 22.74 2021-02-13T00:47:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:41Z- handleDvl() - RDI_Pathfinder msg: 1613177261.49 epoch sec; seqNo:7599 2021-02-13T00:47:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.07 2021-02-13T00:47:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177261.51 epoch sec; seqNo:15803; 4060060.66 north; 593150.93 east 2021-02-13T00:47:41Z- AHRS_M2 msg: 1613177261.89 epoch sec; seqNo:15681 2021-02-13T00:47:41Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-13T00:47:41Z- handleDepth()-Depth_Keller msg: 1613177261892 epoch sec; seqNo:15804; depth 22.66 2021-02-13T00:47:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177261.90 epoch sec; seqNo:15804; 4060061.04 north; 593150.93 east 2021-02-13T00:47:42Z- AHRS_M2 msg: 1613177262.29 epoch sec; seqNo:15682 2021-02-13T00:47:42Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-13T00:47:42Z- handleDepth()-Depth_Keller msg: 1613177262297 epoch sec; seqNo:15805; depth 22.55 2021-02-13T00:47:42Z- handleDvl() - RDI_Pathfinder msg: 1613177262.31 epoch sec; seqNo:7600 2021-02-13T00:47:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 88.18 , 97.91 , 67.43 2021-02-13T00:47:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , 0.05 2021-02-13T00:47:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177262.32 epoch sec; seqNo:15805; 4060061.04 north; 593150.93 east 2021-02-13T00:47:42Z- AHRS_M2 msg: 1613177262.70 epoch sec; seqNo:15683 2021-02-13T00:47:42Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-13T00:47:42Z- handleDepth()-Depth_Keller msg: 1613177262701 epoch sec; seqNo:15806; depth 22.45 2021-02-13T00:47:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:42Z- handleDvl() - RDI_Pathfinder msg: 1613177262.70 epoch sec; seqNo:7601 2021-02-13T00:47:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , 0.05 2021-02-13T00:47:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177262.72 epoch sec; seqNo:15806; 4060061.42 north; 593150.92 east 2021-02-13T00:47:43Z- AHRS_M2 msg: 1613177263.10 epoch sec; seqNo:15684 2021-02-13T00:47:43Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-13T00:47:43Z- handleDepth()-Depth_Keller msg: 1613177263130 epoch sec; seqNo:15807; depth 22.32 2021-02-13T00:47:43Z- handleDvl() - RDI_Pathfinder msg: 1613177263.16 epoch sec; seqNo:7602 2021-02-13T00:47:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 88.65 , 98.39 , 78.92 2021-02-13T00:47:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , 0.04 , -0.05 2021-02-13T00:47:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177263.17 epoch sec; seqNo:15807; 4060061.80 north; 593150.92 east 2021-02-13T00:47:43Z- AHRS_M2 msg: 1613177263.50 epoch sec; seqNo:15685 2021-02-13T00:47:43Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-13T00:47:43Z- handleDepth()-Depth_Keller msg: 1613177263508 epoch sec; seqNo:15808; depth 22.21 2021-02-13T00:47:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177263.52 epoch sec; seqNo:15808; 4060061.80 north; 593150.92 east 2021-02-13T00:47:43Z- AHRS_M2 msg: 1613177263.91 epoch sec; seqNo:15686 2021-02-13T00:47:43Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-13T00:47:43Z- handleDepth()-Depth_Keller msg: 1613177263914 epoch sec; seqNo:15809; depth 22.12 2021-02-13T00:47:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:43Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:43Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:43Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:43Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:43Z- handleDvl() - RDI_Pathfinder msg: 1613177263.92 epoch sec; seqNo:7603 2021-02-13T00:47:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , 0.04 , -0.05 2021-02-13T00:47:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177263.93 epoch sec; seqNo:15809; 4060062.19 north; 593152.13 east 2021-02-13T00:47:44Z- AHRS_M2 msg: 1613177264.31 epoch sec; seqNo:15687 2021-02-13T00:47:44Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-13T00:47:44Z- LcmTrn::handleMessages() - handling 45 messages took 0.71 ms and 131 calls... 2021-02-13T00:47:44Z- waiting for 1613177262.07, time is 1613177264.31 2021-02-13T00:47:44Z- LcmTrn::updateTrn() >>>> pose time 1613177264.31, meas time 1613177263.92 <<<< 2021-02-13T00:47:44Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177263.92, ping # 0. 2021-02-13T00:47:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177263.92 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:47:44Z- TerrainNav::motionUpdate() - currEstimate = (4060062, 593152.13, 22.12) 2021-02-13T00:47:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:47:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:47:44Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:47:44Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:47:44Z- TNavBF::Projecting Measurements 2021-02-13T00:47:44Z- TNavFilter::Measurement from time = 1613177263.92 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:47:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:47:44Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:47:44Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:47:44Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:47:44Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:47:44Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:47:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:47:44Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 687.5 EastVariance: 833.0 DepthVariance: 1.6 this->DepthBiasCov: 1.6 2021-02-13T00:47:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:47:44Z- TNavBF::mmse selected from filter -1 2021-02-13T00:47:44Z- variances([0], [2], and psi_berg) = 687.469067 832.991779 0.000000 2021-02-13T00:47:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:47:44Z- LcmTrn::publishEstimates() - MLE : 4060061.69 593172.63 22.12 2021-02-13T00:47:44Z- LcmTrn::publishEstimates() - MMSE : 4060062.19 593152.13 22.12 2021-02-13T00:47:44Z- LcmTrn::publishEstimates() - COVAR: 687.47 832.99 1.62 0.00 2021-02-13T00:47:44Z- handleDepth()-Depth_Keller msg: 1613177264321 epoch sec; seqNo:15810; depth 22.02 2021-02-13T00:47:44Z- handleDvl() - RDI_Pathfinder msg: 1613177264.33 epoch sec; seqNo:7604 2021-02-13T00:47:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.16 , 0.00 , 98.26 , 67.03 2021-02-13T00:47:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , -0.04 2021-02-13T00:47:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177264.34 epoch sec; seqNo:15810; 4060062.19 north; 593152.13 east 2021-02-13T00:47:44Z- AHRS_M2 msg: 1613177264.72 epoch sec; seqNo:15688 2021-02-13T00:47:44Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-13T00:47:44Z- handleDepth()-Depth_Keller msg: 1613177264724 epoch sec; seqNo:15811; depth 21.92 2021-02-13T00:47:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177264.75 epoch sec; seqNo:15811; 4060062.57 north; 593152.13 east 2021-02-13T00:47:45Z- AHRS_M2 msg: 1613177265.12 epoch sec; seqNo:15689 2021-02-13T00:47:45Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-13T00:47:45Z- handleDepth()-Depth_Keller msg: 1613177265133 epoch sec; seqNo:15812; depth 21.79 2021-02-13T00:47:45Z- handleDvl() - RDI_Pathfinder msg: 1613177265.15 epoch sec; seqNo:7605 2021-02-13T00:47:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.98 , 0.00 , 97.99 , 69.07 2021-02-13T00:47:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , -0.06 2021-02-13T00:47:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177265.18 epoch sec; seqNo:15812; 4060062.95 north; 593152.13 east 2021-02-13T00:47:45Z- AHRS_M2 msg: 1613177265.53 epoch sec; seqNo:15690 2021-02-13T00:47:45Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-13T00:47:45Z- handleDepth()-Depth_Keller msg: 1613177265529 epoch sec; seqNo:15813; depth 21.69 2021-02-13T00:47:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177265.54 epoch sec; seqNo:15813; 4060063.33 north; 593152.12 east 2021-02-13T00:47:45Z- AHRS_M2 msg: 1613177265.93 epoch sec; seqNo:15691 2021-02-13T00:47:45Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-13T00:47:45Z- handleDepth()-Depth_Keller msg: 1613177265932 epoch sec; seqNo:15814; depth 21.60 2021-02-13T00:47:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177265.95 epoch sec; seqNo:15814; 4060063.33 north; 593152.12 east 2021-02-13T00:47:46Z- AHRS_M2 msg: 1613177266.33 epoch sec; seqNo:15692 2021-02-13T00:47:46Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-13T00:47:46Z- handleDepth()-Depth_Keller msg: 1613177266336 epoch sec; seqNo:15815; depth 21.48 2021-02-13T00:47:46Z- handleDvl() - RDI_Pathfinder msg: 1613177266.35 epoch sec; seqNo:7606 2021-02-13T00:47:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.97 , 90.34 , 100.08 , 67.03 2021-02-13T00:47:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.10 2021-02-13T00:47:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177266.37 epoch sec; seqNo:15815; 4060063.72 north; 593153.34 east 2021-02-13T00:47:46Z- AHRS_M2 msg: 1613177266.74 epoch sec; seqNo:15693 2021-02-13T00:47:46Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.15 psi 2021-02-13T00:47:46Z- handleDepth()-Depth_Keller msg: 1613177266740 epoch sec; seqNo:15816; depth 21.36 2021-02-13T00:47:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177266.75 epoch sec; seqNo:15816; 4060064.10 north; 593153.33 east 2021-02-13T00:47:47Z- AHRS_M2 msg: 1613177267.14 epoch sec; seqNo:15694 2021-02-13T00:47:47Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.16 psi 2021-02-13T00:47:47Z- handleDepth()-Depth_Keller msg: 1613177267151 epoch sec; seqNo:15817; depth 21.26 2021-02-13T00:47:47Z- handleDvl() - RDI_Pathfinder msg: 1613177267.16 epoch sec; seqNo:7607 2021-02-13T00:47:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.38 , 0.00 , 99.60 , 68.25 2021-02-13T00:47:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , -0.07 2021-02-13T00:47:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177267.18 epoch sec; seqNo:15817; 4060064.10 north; 593153.33 east 2021-02-13T00:47:47Z- AHRS_M2 msg: 1613177267.54 epoch sec; seqNo:15695 2021-02-13T00:47:47Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-13T00:47:47Z- handleDepth()-Depth_Keller msg: 1613177267554 epoch sec; seqNo:15818; depth 21.16 2021-02-13T00:47:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:47Z- handleDvl() - RDI_Pathfinder msg: 1613177267.56 epoch sec; seqNo:7608 2021-02-13T00:47:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , -0.07 2021-02-13T00:47:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177267.57 epoch sec; seqNo:15818; 4060064.48 north; 593153.33 east 2021-02-13T00:47:47Z- AHRS_M2 msg: 1613177267.96 epoch sec; seqNo:15696 2021-02-13T00:47:47Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-13T00:47:47Z- handleDepth()-Depth_Keller msg: 1613177267970 epoch sec; seqNo:15819; depth 21.04 2021-02-13T00:47:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177267.98 epoch sec; seqNo:15819; 4060064.86 north; 593153.33 east 2021-02-13T00:47:48Z- AHRS_M2 msg: 1613177268.36 epoch sec; seqNo:15697 2021-02-13T00:47:48Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-13T00:47:48Z- handleDepth()-Depth_Keller msg: 1613177268361 epoch sec; seqNo:15820; depth 20.95 2021-02-13T00:47:48Z- handleDvl() - RDI_Pathfinder msg: 1613177268.37 epoch sec; seqNo:7609 2021-02-13T00:47:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.38 , 97.91 , 101.16 , 69.07 2021-02-13T00:47:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.10 , -0.01 2021-02-13T00:47:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177268.38 epoch sec; seqNo:15820; 4060065.24 north; 593153.32 east 2021-02-13T00:47:48Z- AHRS_M2 msg: 1613177268.77 epoch sec; seqNo:15698 2021-02-13T00:47:48Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.18 psi 2021-02-13T00:47:48Z- handleDepth()-Depth_Keller msg: 1613177268774 epoch sec; seqNo:15821; depth 20.82 2021-02-13T00:47:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:48Z- handleDvl() - RDI_Pathfinder msg: 1613177268.78 epoch sec; seqNo:7610 2021-02-13T00:47:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.10 , -0.01 2021-02-13T00:47:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177268.79 epoch sec; seqNo:15821; 4060065.62 north; 593153.32 east 2021-02-13T00:47:49Z- AHRS_M2 msg: 1613177269.16 epoch sec; seqNo:15699 2021-02-13T00:47:49Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.19 psi 2021-02-13T00:47:49Z- handleDepth()-Depth_Keller msg: 1613177269179 epoch sec; seqNo:15822; depth 20.74 2021-02-13T00:47:49Z- handleDvl() - RDI_Pathfinder msg: 1613177269.19 epoch sec; seqNo:7611 2021-02-13T00:47:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.38 , 98.12 , 101.77 , 0.00 2021-02-13T00:47:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.09 , 0.09 2021-02-13T00:47:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177269.20 epoch sec; seqNo:15822; 4060065.63 north; 593154.54 east 2021-02-13T00:47:49Z- LcmTrn::handleMessages() - handling 46 messages took 0.72 ms and 132 calls... 2021-02-13T00:47:49Z- waiting for 1613177267.31, time is 1613177269.20 2021-02-13T00:47:49Z- LcmTrn::updateTrn() equating pose 1613177269.20 and meas times 1613177269.20 2021-02-13T00:47:49Z- LcmTrn::updateTrn() >>>> pose time 1613177269.20, meas time 1613177269.20 <<<< 2021-02-13T00:47:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177269.20 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:47:49Z- TerrainNav::motionUpdate() - currEstimate = (4060066, 593154.54, 20.74) 2021-02-13T00:47:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:47:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:47:49Z- TNavBF::Projecting Measurements 2021-02-13T00:47:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:47:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:47:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:47:49Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:47:49Z- TNavFilter::3 good beams. 2021-02-13T00:47:49Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:47:49Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:47:49Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:47:49Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:47:49Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:47:49Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:47:49Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:47:49Z- TNavBF::mmse selected from filter -1 2021-02-13T00:47:49Z- TerrainMapOctree: (vehN, vehE ) = (4060065.63, 593154.54). 2021-02-13T00:47:49Z- TerrainMapOctree: Min Distance = 1613.31. 2021-02-13T00:47:49Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:47:49Z- There is no specific good beam that all particles have in common. 2021-02-13T00:47:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:47:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.84 K: 0.148 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-13T00:47:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:47:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.57 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 15.40 2021-02-13T00:47:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:47:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 16.57 2021-02-13T00:47:49Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:47:49Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:47:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:47:49Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:47:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:47:49Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:47:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:47:49Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:47:49Z- Filter 0 No beams used in subcloud update 2021-02-13T00:47:49Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:47:49Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:47:49Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 687.4 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:47:49Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177269.20 sec. 2021-02-13T00:47:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:47:49Z- TNavBF::mmse selected from filter -1 2021-02-13T00:47:49Z- variances([0], [2], and psi_berg) = 687.420509 832.991756 0.000000 2021-02-13T00:47:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:47:49Z- LcmTrn::publishEstimates() - MLE : 4060065.13 593175.04 20.74 2021-02-13T00:47:49Z- LcmTrn::publishEstimates() - MMSE : 4060065.63 593154.54 20.74 2021-02-13T00:47:49Z- LcmTrn::publishEstimates() - COVAR: 687.42 832.99 1.33 0.00 2021-02-13T00:47:49Z- AHRS_M2 msg: 1613177269.57 epoch sec; seqNo:15700 2021-02-13T00:47:49Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.19 psi 2021-02-13T00:47:49Z- handleDepth()-Depth_Keller msg: 1613177269575 epoch sec; seqNo:15823; depth 20.61 2021-02-13T00:47:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177269.60 epoch sec; seqNo:15823; 4060066.01 north; 593154.53 east 2021-02-13T00:47:49Z- AHRS_M2 msg: 1613177269.97 epoch sec; seqNo:15701 2021-02-13T00:47:49Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.19 psi 2021-02-13T00:47:49Z- handleDepth()-Depth_Keller msg: 1613177269977 epoch sec; seqNo:15824; depth 20.52 2021-02-13T00:47:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:49Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:49Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:49Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:49Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:49Z- handleDvl() - RDI_Pathfinder msg: 1613177269.98 epoch sec; seqNo:7612 2021-02-13T00:47:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.09 , 0.09 2021-02-13T00:47:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177269.99 epoch sec; seqNo:15824; 4060066.39 north; 593154.53 east 2021-02-13T00:47:50Z- AHRS_M2 msg: 1613177270.37 epoch sec; seqNo:15702 2021-02-13T00:47:50Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T00:47:50Z- handleDepth()-Depth_Keller msg: 1613177270378 epoch sec; seqNo:15825; depth 20.44 2021-02-13T00:47:50Z- handleDvl() - RDI_Pathfinder msg: 1613177270.39 epoch sec; seqNo:7613 2021-02-13T00:47:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.38 , 97.64 , 100.08 , 69.48 2021-02-13T00:47:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.13 2021-02-13T00:47:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177270.40 epoch sec; seqNo:15825; 4060066.77 north; 593154.53 east 2021-02-13T00:47:50Z- AHRS_M2 msg: 1613177270.78 epoch sec; seqNo:15703 2021-02-13T00:47:50Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-13T00:47:50Z- handleDepth()-Depth_Keller msg: 1613177270780 epoch sec; seqNo:15826; depth 20.32 2021-02-13T00:47:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177270.81 epoch sec; seqNo:15826; 4060067.14 north; 593154.52 east 2021-02-13T00:47:51Z- AHRS_M2 msg: 1613177271.18 epoch sec; seqNo:15704 2021-02-13T00:47:51Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-13T00:47:51Z- handleDepth()-Depth_Keller msg: 1613177271187 epoch sec; seqNo:15827; depth 20.21 2021-02-13T00:47:51Z- handleDvl() - RDI_Pathfinder msg: 1613177271.20 epoch sec; seqNo:7614 2021-02-13T00:47:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:51Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:47:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177271.23 epoch sec; seqNo:15827; 4060067.14 north; 593154.52 east 2021-02-13T00:47:51Z- AHRS_M2 msg: 1613177271.58 epoch sec; seqNo:15705 2021-02-13T00:47:51Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-13T00:47:51Z- handleDepth()-Depth_Keller msg: 1613177271593 epoch sec; seqNo:15828; depth 20.10 2021-02-13T00:47:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177271.61 epoch sec; seqNo:15828; 4060067.52 north; 593154.52 east 2021-02-13T00:47:51Z- AHRS_M2 msg: 1613177271.99 epoch sec; seqNo:15706 2021-02-13T00:47:51Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-13T00:47:51Z- handleDepth()-Depth_Keller msg: 1613177271994 epoch sec; seqNo:15829; depth 20.00 2021-02-13T00:47:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177272.00 epoch sec; seqNo:15829; 4060067.92 north; 593155.73 east 2021-02-13T00:47:52Z- AHRS_M2 msg: 1613177272.39 epoch sec; seqNo:15707 2021-02-13T00:47:52Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-13T00:47:52Z- handleDepth()-Depth_Keller msg: 1613177272396 epoch sec; seqNo:15830; depth 19.89 2021-02-13T00:47:52Z- handleDvl() - RDI_Pathfinder msg: 1613177272.42 epoch sec; seqNo:7615 2021-02-13T00:47:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.18 , 104.27 , 98.18 2021-02-13T00:47:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.17 , -0.60 , -0.40 2021-02-13T00:47:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177272.43 epoch sec; seqNo:15830; 4060067.90 north; 593154.51 east 2021-02-13T00:47:52Z- AHRS_M2 msg: 1613177272.80 epoch sec; seqNo:15708 2021-02-13T00:47:52Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-13T00:47:52Z- handleDepth()-Depth_Keller msg: 1613177272800 epoch sec; seqNo:15831; depth 19.76 2021-02-13T00:47:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177272.81 epoch sec; seqNo:15831; 4060068.28 north; 593154.51 east 2021-02-13T00:47:53Z- AHRS_M2 msg: 1613177273.20 epoch sec; seqNo:15709 2021-02-13T00:47:53Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:47:53Z- handleDepth()-Depth_Keller msg: 1613177273207 epoch sec; seqNo:15832; depth 19.67 2021-02-13T00:47:53Z- handleDvl() - RDI_Pathfinder msg: 1613177273.22 epoch sec; seqNo:7616 2021-02-13T00:47:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.79 , 97.03 , 106.50 , 69.07 2021-02-13T00:47:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.07 , -0.07 2021-02-13T00:47:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177273.23 epoch sec; seqNo:15832; 4060068.66 north; 593154.50 east 2021-02-13T00:47:53Z- AHRS_M2 msg: 1613177273.60 epoch sec; seqNo:15710 2021-02-13T00:47:53Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:47:53Z- handleDepth()-Depth_Keller msg: 1613177273621 epoch sec; seqNo:15833; depth 19.56 2021-02-13T00:47:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:53Z- handleDvl() - RDI_Pathfinder msg: 1613177273.62 epoch sec; seqNo:7617 2021-02-13T00:47:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.07 , -0.07 2021-02-13T00:47:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177273.63 epoch sec; seqNo:15833; 4060068.67 north; 593155.73 east 2021-02-13T00:47:54Z- AHRS_M2 msg: 1613177274.01 epoch sec; seqNo:15711 2021-02-13T00:47:54Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:47:54Z- handleDepth()-Depth_Keller msg: 1613177274017 epoch sec; seqNo:15834; depth 19.45 2021-02-13T00:47:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177274.03 epoch sec; seqNo:15834; 4060069.05 north; 593155.72 east 2021-02-13T00:47:54Z- AHRS_M2 msg: 1613177274.41 epoch sec; seqNo:15712 2021-02-13T00:47:54Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:47:54Z- handleDepth()-Depth_Keller msg: 1613177274416 epoch sec; seqNo:15835; depth 19.33 2021-02-13T00:47:54Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-13T00:47:54Z- waiting for 1613177272.20, time is 1613177274.41 2021-02-13T00:47:54Z- LcmTrn::updateTrn() >>>> pose time 1613177274.41, meas time 1613177273.62 <<<< 2021-02-13T00:47:54Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177273.62, ping # 0. 2021-02-13T00:47:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177273.62 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:47:54Z- TerrainNav::motionUpdate() - currEstimate = (4060069, 593155.72, 19.33) 2021-02-13T00:47:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:47:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:47:54Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:47:54Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:47:54Z- TNavBF::Projecting Measurements 2021-02-13T00:47:54Z- TNavFilter::Measurement from time = 1613177273.62 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:47:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:47:54Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:47:54Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:47:54Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:47:54Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:47:54Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:47:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:47:54Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 687.4 EastVariance: 833.0 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-13T00:47:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:47:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:47:54Z- variances([0], [2], and psi_berg) = 687.361424 832.991729 0.000000 2021-02-13T00:47:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:47:54Z- LcmTrn::publishEstimates() - MLE : 4060068.55 593177.22 19.33 2021-02-13T00:47:54Z- LcmTrn::publishEstimates() - MMSE : 4060069.05 593155.72 19.33 2021-02-13T00:47:54Z- LcmTrn::publishEstimates() - COVAR: 687.36 832.99 1.52 0.00 2021-02-13T00:47:54Z- handleDvl() - RDI_Pathfinder msg: 1613177274.43 epoch sec; seqNo:7618 2021-02-13T00:47:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.05 , 101.70 , 78.58 2021-02-13T00:47:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , -0.10 2021-02-13T00:47:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177274.45 epoch sec; seqNo:15835; 4060069.43 north; 593155.72 east 2021-02-13T00:47:54Z- AHRS_M2 msg: 1613177274.82 epoch sec; seqNo:15713 2021-02-13T00:47:54Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.19 psi 2021-02-13T00:47:54Z- handleDepth()-Depth_Keller msg: 1613177274822 epoch sec; seqNo:15836; depth 19.24 2021-02-13T00:47:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:54Z- handleDvl() - RDI_Pathfinder msg: 1613177274.82 epoch sec; seqNo:7619 2021-02-13T00:47:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , -0.10 2021-02-13T00:47:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177274.84 epoch sec; seqNo:15836; 4060069.43 north; 593155.72 east 2021-02-13T00:47:55Z- AHRS_M2 msg: 1613177275.23 epoch sec; seqNo:15714 2021-02-13T00:47:55Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.19 psi 2021-02-13T00:47:55Z- handleDepth()-Depth_Keller msg: 1613177275234 epoch sec; seqNo:15837; depth 19.13 2021-02-13T00:47:55Z- handleDvl() - RDI_Pathfinder msg: 1613177275.24 epoch sec; seqNo:7620 2021-02-13T00:47:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.71 , 97.64 , 107.11 , 70.70 2021-02-13T00:47:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , -0.09 2021-02-13T00:47:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177275.26 epoch sec; seqNo:15837; 4060069.81 north; 593155.71 east 2021-02-13T00:47:55Z- AHRS_M2 msg: 1613177275.62 epoch sec; seqNo:15715 2021-02-13T00:47:55Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.19 psi 2021-02-13T00:47:55Z- handleDepth()-Depth_Keller msg: 1613177275633 epoch sec; seqNo:15838; depth 19.01 2021-02-13T00:47:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177275.64 epoch sec; seqNo:15838; 4060070.19 north; 593155.71 east 2021-02-13T00:47:56Z- AHRS_M2 msg: 1613177276.03 epoch sec; seqNo:15716 2021-02-13T00:47:56Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.19 psi 2021-02-13T00:47:56Z- handleDepth()-Depth_Keller msg: 1613177276032 epoch sec; seqNo:15839; depth 18.92 2021-02-13T00:47:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:56Z- handleDvl() - RDI_Pathfinder msg: 1613177276.03 epoch sec; seqNo:7621 2021-02-13T00:47:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , -0.09 2021-02-13T00:47:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177276.04 epoch sec; seqNo:15839; 4060070.20 north; 593156.93 east 2021-02-13T00:47:56Z- AHRS_M2 msg: 1613177276.43 epoch sec; seqNo:15717 2021-02-13T00:47:56Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T00:47:56Z- handleDepth()-Depth_Keller msg: 1613177276445 epoch sec; seqNo:15840; depth 18.82 2021-02-13T00:47:56Z- handleDvl() - RDI_Pathfinder msg: 1613177276.46 epoch sec; seqNo:7622 2021-02-13T00:47:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.20 , 97.91 , 104.00 , 69.07 2021-02-13T00:47:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.04 , -0.05 2021-02-13T00:47:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177276.47 epoch sec; seqNo:15840; 4060070.58 north; 593156.93 east 2021-02-13T00:47:56Z- AHRS_M2 msg: 1613177276.84 epoch sec; seqNo:15718 2021-02-13T00:47:56Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T00:47:56Z- handleDepth()-Depth_Keller msg: 1613177276845 epoch sec; seqNo:15841; depth 18.70 2021-02-13T00:47:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177276.86 epoch sec; seqNo:15841; 4060070.96 north; 593156.92 east 2021-02-13T00:47:57Z- AHRS_M2 msg: 1613177277.24 epoch sec; seqNo:15719 2021-02-13T00:47:57Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T00:47:57Z- handleDepth()-Depth_Keller msg: 1613177277245 epoch sec; seqNo:15842; depth 18.61 2021-02-13T00:47:57Z- handleDvl() - RDI_Pathfinder msg: 1613177277.26 epoch sec; seqNo:7623 2021-02-13T00:47:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 104.13 , 70.70 2021-02-13T00:47:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.04 2021-02-13T00:47:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177277.27 epoch sec; seqNo:15842; 4060071.34 north; 593156.92 east 2021-02-13T00:47:57Z- AHRS_M2 msg: 1613177277.64 epoch sec; seqNo:15720 2021-02-13T00:47:57Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T00:47:57Z- handleDepth()-Depth_Keller msg: 1613177277651 epoch sec; seqNo:15843; depth 18.48 2021-02-13T00:47:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177277.66 epoch sec; seqNo:15843; 4060071.34 north; 593156.92 east 2021-02-13T00:47:58Z- AHRS_M2 msg: 1613177278.05 epoch sec; seqNo:15721 2021-02-13T00:47:58Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T00:47:58Z- handleDepth()-Depth_Keller msg: 1613177278054 epoch sec; seqNo:15844; depth 18.38 2021-02-13T00:47:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177278.07 epoch sec; seqNo:15844; 4060071.72 north; 593156.91 east 2021-02-13T00:47:58Z- AHRS_M2 msg: 1613177278.45 epoch sec; seqNo:15722 2021-02-13T00:47:58Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T00:47:58Z- handleDepth()-Depth_Keller msg: 1613177278456 epoch sec; seqNo:15845; depth 18.28 2021-02-13T00:47:58Z- handleDvl() - RDI_Pathfinder msg: 1613177278.47 epoch sec; seqNo:7624 2021-02-13T00:47:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.02 , 97.91 , 105.21 , 70.29 2021-02-13T00:47:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , -0.05 2021-02-13T00:47:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177278.49 epoch sec; seqNo:15845; 4060072.10 north; 593156.91 east 2021-02-13T00:47:58Z- AHRS_M2 msg: 1613177278.86 epoch sec; seqNo:15723 2021-02-13T00:47:58Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.18 psi 2021-02-13T00:47:58Z- handleDepth()-Depth_Keller msg: 1613177278861 epoch sec; seqNo:15846; depth 18.17 2021-02-13T00:47:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177278.88 epoch sec; seqNo:15846; 4060072.10 north; 593156.91 east 2021-02-13T00:47:59Z- AHRS_M2 msg: 1613177279.26 epoch sec; seqNo:15724 2021-02-13T00:47:59Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.18 psi 2021-02-13T00:47:59Z- handleDepth()-Depth_Keller msg: 1613177279265 epoch sec; seqNo:15847; depth 18.08 2021-02-13T00:47:59Z- handleDvl() - RDI_Pathfinder msg: 1613177279.28 epoch sec; seqNo:7625 2021-02-13T00:47:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 94.33 , 105.28 , 78.51 2021-02-13T00:47:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , -0.05 2021-02-13T00:47:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177279.29 epoch sec; seqNo:15847; 4060072.49 north; 593158.13 east 2021-02-13T00:47:59Z- LcmTrn::handleMessages() - handling 45 messages took 0.65 ms and 132 calls... 2021-02-13T00:47:59Z- waiting for 1613177277.41, time is 1613177279.29 2021-02-13T00:47:59Z- LcmTrn::updateTrn() equating pose 1613177279.29 and meas times 1613177279.29 2021-02-13T00:47:59Z- LcmTrn::updateTrn() >>>> pose time 1613177279.29, meas time 1613177279.29 <<<< 2021-02-13T00:47:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177279.29 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:47:59Z- TerrainNav::motionUpdate() - currEstimate = (4060072, 593158.13, 18.08) 2021-02-13T00:47:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:47:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:47:59Z- TNavBF::Projecting Measurements 2021-02-13T00:47:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:47:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:47:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:47:59Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:47:59Z- TNavFilter::3 good beams. 2021-02-13T00:47:59Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:47:59Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:47:59Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:47:59Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:47:59Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:47:59Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:47:59Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:47:59Z- TNavBF::mmse selected from filter -1 2021-02-13T00:47:59Z- TerrainMapOctree: (vehN, vehE ) = (4060072.49, 593158.13). 2021-02-13T00:47:59Z- TerrainMapOctree: Min Distance = 1605.84. 2021-02-13T00:47:59Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:47:59Z- There is no specific good beam that all particles have in common. 2021-02-13T00:47:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:47:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.73 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 14.24 2021-02-13T00:47:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:47:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.56 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 17.73 2021-02-13T00:47:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:47:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.44 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:47:59Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:47:59Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:47:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:47:59Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:47:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:47:59Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:47:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:47:59Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:47:59Z- Filter 0 No beams used in subcloud update 2021-02-13T00:47:59Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:47:59Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:47:59Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 687.3 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:47:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177279.29 sec. 2021-02-13T00:47:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:47:59Z- TNavBF::mmse selected from filter -1 2021-02-13T00:47:59Z- variances([0], [2], and psi_berg) = 687.312921 832.991707 0.000000 2021-02-13T00:47:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:47:59Z- LcmTrn::publishEstimates() - MLE : 4060071.99 593179.63 18.08 2021-02-13T00:47:59Z- LcmTrn::publishEstimates() - MMSE : 4060072.49 593158.13 18.08 2021-02-13T00:47:59Z- LcmTrn::publishEstimates() - COVAR: 687.31 832.99 1.28 0.00 2021-02-13T00:47:59Z- AHRS_M2 msg: 1613177279.66 epoch sec; seqNo:15725 2021-02-13T00:47:59Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.18 psi 2021-02-13T00:47:59Z- handleDepth()-Depth_Keller msg: 1613177279671 epoch sec; seqNo:15848; depth 17.97 2021-02-13T00:47:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:47:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:47:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:47:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:47:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:47:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:47:59Z- handleDvl() - RDI_Pathfinder msg: 1613177279.67 epoch sec; seqNo:7626 2021-02-13T00:47:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:47:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , -0.05 2021-02-13T00:47:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177279.68 epoch sec; seqNo:15848; 4060072.87 north; 593158.12 east 2021-02-13T00:48:00Z- AHRS_M2 msg: 1613177280.07 epoch sec; seqNo:15726 2021-02-13T00:48:00Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.18 psi 2021-02-13T00:48:00Z- handleDepth()-Depth_Keller msg: 1613177280072 epoch sec; seqNo:15849; depth 17.86 2021-02-13T00:48:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177280.09 epoch sec; seqNo:15849; 4060073.25 north; 593158.12 east 2021-02-13T00:48:00Z- AHRS_M2 msg: 1613177280.47 epoch sec; seqNo:15727 2021-02-13T00:48:00Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.18 psi 2021-02-13T00:48:00Z- handleDepth()-Depth_Keller msg: 1613177280476 epoch sec; seqNo:15850; depth 17.74 2021-02-13T00:48:00Z- handleDvl() - RDI_Pathfinder msg: 1613177280.49 epoch sec; seqNo:7627 2021-02-13T00:48:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 97.64 , 105.76 , 78.71 2021-02-13T00:48:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.12 , -0.02 2021-02-13T00:48:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177280.51 epoch sec; seqNo:15850; 4060073.62 north; 593158.11 east 2021-02-13T00:48:00Z- AHRS_M2 msg: 1613177280.88 epoch sec; seqNo:15728 2021-02-13T00:48:00Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.17 psi 2021-02-13T00:48:00Z- handleDepth()-Depth_Keller msg: 1613177280881 epoch sec; seqNo:15851; depth 17.64 2021-02-13T00:48:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:48:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:48:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:48:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:48:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:48:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:48:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:48:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:48:00Z- handleDvl() - RDI_Pathfinder msg: 1613177280.88 epoch sec; seqNo:7628 2021-02-13T00:48:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:48:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.12 , -0.02 2021-02-13T00:48:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177280.89 epoch sec; seqNo:15851; 4060073.62 north; 593158.11 east 2021-02-13T00:48:01Z- AHRS_M2 msg: 1613177281.30 epoch sec; seqNo:15729 2021-02-13T00:48:01Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.17 psi 2021-02-13T00:48:01Z- handleDepth()-Depth_Keller msg: 1613177281299 epoch sec; seqNo:15852; depth 17.54 2021-02-13T00:48:01Z- handleDvl() - RDI_Pathfinder msg: 1613177281.31 epoch sec; seqNo:7629 2021-02-13T00:48:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.52 , 97.78 , 107.51 , 78.31 2021-02-13T00:48:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.10 , 0.01 2021-02-13T00:48:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177281.32 epoch sec; seqNo:15852; 4060074.00 north; 593158.11 east 2021-02-13T00:48:01Z- AHRS_M2 msg: 1613177281.68 epoch sec; seqNo:15730 2021-02-13T00:48:01Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.17 psi 2021-02-13T00:48:01Z- handleDepth()-Depth_Keller msg: 1613177281693 epoch sec; seqNo:15853; depth 17.42 2021-02-13T00:48:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177281.70 epoch sec; seqNo:15853; 4060074.38 north; 593158.10 east 2021-02-13T00:48:02Z- AHRS_M2 msg: 1613177282.09 epoch sec; seqNo:15731 2021-02-13T00:48:02Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.17 psi 2021-02-13T00:48:02Z- handleDepth()-Depth_Keller msg: 1613177282097 epoch sec; seqNo:15854; depth 17.32 2021-02-13T00:48:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:48:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:48:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:48:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:48:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:48:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:48:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:48:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:48:02Z- handleDvl() - RDI_Pathfinder msg: 1613177282.10 epoch sec; seqNo:7630 2021-02-13T00:48:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:48:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.10 , 0.01 2021-02-13T00:48:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177282.11 epoch sec; seqNo:15854; 4060074.76 north; 593158.10 east 2021-02-13T00:48:02Z- AHRS_M2 msg: 1613177282.49 epoch sec; seqNo:15732 2021-02-13T00:48:02Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.16 psi 2021-02-13T00:48:02Z- handleDepth()-Depth_Keller msg: 1613177282505 epoch sec; seqNo:15855; depth 17.20 2021-02-13T00:48:02Z- handleDvl() - RDI_Pathfinder msg: 1613177282.52 epoch sec; seqNo:7631 2021-02-13T00:48:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.46 , 97.71 , 107.45 , 78.24 2021-02-13T00:48:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.09 , 0.05 2021-02-13T00:48:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177282.53 epoch sec; seqNo:15855; 4060075.15 north; 593159.32 east 2021-02-13T00:48:02Z- AHRS_M2 msg: 1613177282.90 epoch sec; seqNo:15733 2021-02-13T00:48:02Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.16 psi 2021-02-13T00:48:02Z- handleDepth()-Depth_Keller msg: 1613177282901 epoch sec; seqNo:15856; depth 17.10 2021-02-13T00:48:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177282.92 epoch sec; seqNo:15856; 4060075.53 north; 593159.31 east 2021-02-13T00:48:03Z- AHRS_M2 msg: 1613177283.30 epoch sec; seqNo:15734 2021-02-13T00:48:03Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.16 psi 2021-02-13T00:48:03Z- handleDepth()-Depth_Keller msg: 1613177283304 epoch sec; seqNo:15857; depth 17.00 2021-02-13T00:48:03Z- handleDvl() - RDI_Pathfinder msg: 1613177283.32 epoch sec; seqNo:7632 2021-02-13T00:48:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.46 , 97.71 , 108.66 , 78.24 2021-02-13T00:48:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.05 , 0.06 2021-02-13T00:48:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177283.34 epoch sec; seqNo:15857; 4060075.91 north; 593159.31 east 2021-02-13T00:48:03Z- AHRS_M2 msg: 1613177283.70 epoch sec; seqNo:15735 2021-02-13T00:48:03Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.16 psi 2021-02-13T00:48:03Z- handleDepth()-Depth_Keller msg: 1613177283713 epoch sec; seqNo:15858; depth 16.88 2021-02-13T00:48:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177283.72 epoch sec; seqNo:15858; 4060075.91 north; 593159.31 east 2021-02-13T00:48:04Z- AHRS_M2 msg: 1613177284.11 epoch sec; seqNo:15736 2021-02-13T00:48:04Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.16 psi 2021-02-13T00:48:04Z- handleDepth()-Depth_Keller msg: 1613177284123 epoch sec; seqNo:15859; depth 16.76 2021-02-13T00:48:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177284.13 epoch sec; seqNo:15859; 4060076.29 north; 593159.30 east 2021-02-13T00:48:04Z- AHRS_M2 msg: 1613177284.51 epoch sec; seqNo:15737 2021-02-13T00:48:04Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.16 psi 2021-02-13T00:48:04Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-13T00:48:04Z- waiting for 1613177282.29, time is 1613177284.51 2021-02-13T00:48:04Z- LcmTrn::updateTrn() >>>> pose time 1613177284.51, meas time 1613177283.32 <<<< 2021-02-13T00:48:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177283.32 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:48:04Z- TerrainNav::motionUpdate() - currEstimate = (4060076, 593159.30, 16.76) 2021-02-13T00:48:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:48:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:48:04Z- TNavBF::Projecting Measurements 2021-02-13T00:48:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:48:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:48:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:48:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:48:04Z- TNavFilter::4 good beams. 2021-02-13T00:48:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:48:04Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:48:04Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:48:04Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:48:04Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:48:04Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:48:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:48:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:48:04Z- TerrainMapOctree: (vehN, vehE ) = (4060075.42, 593159.03). 2021-02-13T00:48:04Z- TerrainMapOctree: Min Distance = 1603.07. 2021-02-13T00:48:04Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:48:04Z- There is no specific good beam that all particles have in common. 2021-02-13T00:48:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:48:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.44 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 10.10 2021-02-13T00:48:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:48:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:48:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:48:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 18.89 2021-02-13T00:48:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:48:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 9.79 2021-02-13T00:48:04Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:48:04Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:48:04Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:48:04Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:48:04Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:48:04Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:48:04Z- Filter 0 No beams used in subcloud update 2021-02-13T00:48:04Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:48:04Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:48:04Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 687.3 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:48:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177283.32 sec. 2021-02-13T00:48:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:48:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:48:04Z- variances([0], [2], and psi_berg) = 687.248864 832.991678 0.000000 2021-02-13T00:48:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:48:04Z- LcmTrn::publishEstimates() - MLE : 4060075.79 593181.80 16.76 2021-02-13T00:48:04Z- LcmTrn::publishEstimates() - MMSE : 4060076.29 593159.30 16.76 2021-02-13T00:48:04Z- LcmTrn::publishEstimates() - COVAR: 687.25 832.99 1.07 0.00 2021-02-13T00:48:04Z- handleDepth()-Depth_Keller msg: 1613177284521 epoch sec; seqNo:15860; depth 16.66 2021-02-13T00:48:04Z- handleDvl() - RDI_Pathfinder msg: 1613177284.55 epoch sec; seqNo:7633 2021-02-13T00:48:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 97.64 , 108.60 , 70.29 2021-02-13T00:48:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , 0.04 2021-02-13T00:48:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177284.56 epoch sec; seqNo:15860; 4060076.67 north; 593159.30 east 2021-02-13T00:48:04Z- AHRS_M2 msg: 1613177284.92 epoch sec; seqNo:15738 2021-02-13T00:48:04Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.16 psi 2021-02-13T00:48:04Z- handleDepth()-Depth_Keller msg: 1613177284920 epoch sec; seqNo:15861; depth 16.54 2021-02-13T00:48:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177284.94 epoch sec; seqNo:15861; 4060077.06 north; 593160.52 east 2021-02-13T00:48:05Z- AHRS_M2 msg: 1613177285.33 epoch sec; seqNo:15739 2021-02-13T00:48:05Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.15 psi 2021-02-13T00:48:05Z- handleDepth()-Depth_Keller msg: 1613177285333 epoch sec; seqNo:15862; depth 16.43 2021-02-13T00:48:05Z- handleDvl() - RDI_Pathfinder msg: 1613177285.34 epoch sec; seqNo:7634 2021-02-13T00:48:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 97.78 , 108.73 , 78.31 2021-02-13T00:48:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.04 , 0.00 2021-02-13T00:48:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177285.36 epoch sec; seqNo:15862; 4060077.44 north; 593160.51 east 2021-02-13T00:48:05Z- AHRS_M2 msg: 1613177285.73 epoch sec; seqNo:15740 2021-02-13T00:48:05Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T00:48:05Z- handleDepth()-Depth_Keller msg: 1613177285733 epoch sec; seqNo:15863; depth 16.33 2021-02-13T00:48:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:48:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:48:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:48:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:48:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:48:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:48:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:48:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:48:05Z- handleDvl() - RDI_Pathfinder msg: 1613177285.73 epoch sec; seqNo:7635 2021-02-13T00:48:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:48:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.04 , 0.00 2021-02-13T00:48:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177285.75 epoch sec; seqNo:15863; 4060077.44 north; 593160.51 east 2021-02-13T00:48:06Z- AHRS_M2 msg: 1613177286.13 epoch sec; seqNo:15741 2021-02-13T00:48:06Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T00:48:06Z- handleDepth()-Depth_Keller msg: 1613177286137 epoch sec; seqNo:15864; depth 16.22 2021-02-13T00:48:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177286.15 epoch sec; seqNo:15864; 4060077.82 north; 593160.51 east 2021-02-13T00:48:06Z- AHRS_M2 msg: 1613177286.53 epoch sec; seqNo:15742 2021-02-13T00:48:06Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T00:48:06Z- handleDepth()-Depth_Keller msg: 1613177286536 epoch sec; seqNo:15865; depth 16.12 2021-02-13T00:48:06Z- handleDvl() - RDI_Pathfinder msg: 1613177286.55 epoch sec; seqNo:7636 2021-02-13T00:48:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 95.21 , 108.60 , 72.75 2021-02-13T00:48:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.05 , -0.01 2021-02-13T00:48:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177286.56 epoch sec; seqNo:15865; 4060078.20 north; 593160.50 east 2021-02-13T00:48:06Z- AHRS_M2 msg: 1613177286.93 epoch sec; seqNo:15743 2021-02-13T00:48:06Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T00:48:06Z- handleDepth()-Depth_Keller msg: 1613177286945 epoch sec; seqNo:15866; depth 16.00 2021-02-13T00:48:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:48:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:48:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:48:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:48:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:48:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:48:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:48:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:48:06Z- handleDvl() - RDI_Pathfinder msg: 1613177286.95 epoch sec; seqNo:7637 2021-02-13T00:48:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:48:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.05 , -0.01 2021-02-13T00:48:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177286.96 epoch sec; seqNo:15866; 4060078.20 north; 593160.50 east 2021-02-13T00:48:07Z- AHRS_M2 msg: 1613177287.34 epoch sec; seqNo:15744 2021-02-13T00:48:07Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T00:48:07Z- handleDepth()-Depth_Keller msg: 1613177287353 epoch sec; seqNo:15867; depth 15.89 2021-02-13T00:48:07Z- handleDvl() - RDI_Pathfinder msg: 1613177287.36 epoch sec; seqNo:7638 2021-02-13T00:48:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.78 , 108.73 , 78.31 2021-02-13T00:48:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.04 , -0.00 2021-02-13T00:48:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177287.38 epoch sec; seqNo:15867; 4060078.58 north; 593160.50 east 2021-02-13T00:48:07Z- AHRS_M2 msg: 1613177287.77 epoch sec; seqNo:15745 2021-02-13T00:48:07Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.15 psi 2021-02-13T00:48:07Z- handleDepth()-Depth_Keller msg: 1613177287779 epoch sec; seqNo:15868; depth 15.78 2021-02-13T00:48:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177287.79 epoch sec; seqNo:15868; 4060078.97 north; 593161.72 east 2021-02-13T00:48:08Z- AHRS_M2 msg: 1613177288.17 epoch sec; seqNo:15746 2021-02-13T00:48:08Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.15 psi 2021-02-13T00:48:08Z- handleDepth()-Depth_Keller msg: 1613177288181 epoch sec; seqNo:15869; depth 15.66 2021-02-13T00:48:08Z- handleDvl() - RDI_Pathfinder msg: 1613177288.19 epoch sec; seqNo:7639 2021-02-13T00:48:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.21 , 97.44 , 109.61 , 70.70 2021-02-13T00:48:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , 0.02 2021-02-13T00:48:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177288.20 epoch sec; seqNo:15869; 4060078.97 north; 593161.72 east 2021-02-13T00:48:08Z- AHRS_M2 msg: 1613177288.55 epoch sec; seqNo:15747 2021-02-13T00:48:08Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.14 psi 2021-02-13T00:48:08Z- handleDepth()-Depth_Keller msg: 1613177288561 epoch sec; seqNo:15870; depth 15.55 2021-02-13T00:48:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177288.57 epoch sec; seqNo:15870; 4060079.35 north; 593161.71 east 2021-02-13T00:48:08Z- AHRS_M2 msg: 1613177288.96 epoch sec; seqNo:15748 2021-02-13T00:48:08Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.13 psi 2021-02-13T00:48:08Z- handleDepth()-Depth_Keller msg: 1613177288965 epoch sec; seqNo:15871; depth 15.44 2021-02-13T00:48:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177288.97 epoch sec; seqNo:15871; 4060079.73 north; 593161.71 east 2021-02-13T00:48:09Z- AHRS_M2 msg: 1613177289.36 epoch sec; seqNo:15749 2021-02-13T00:48:09Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.13 psi 2021-02-13T00:48:09Z- handleDepth()-Depth_Keller msg: 1613177289366 epoch sec; seqNo:15872; depth 15.31 2021-02-13T00:48:09Z- handleDvl() - RDI_Pathfinder msg: 1613177289.38 epoch sec; seqNo:7640 2021-02-13T00:48:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 96.56 , 109.95 , 71.52 2021-02-13T00:48:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , 0.03 2021-02-13T00:48:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177289.39 epoch sec; seqNo:15872; 4060079.73 north; 593161.71 east 2021-02-13T00:48:09Z- LcmTrn::handleMessages() - handling 46 messages took 0.69 ms and 133 calls... 2021-02-13T00:48:09Z- waiting for 1613177287.51, time is 1613177289.39 2021-02-13T00:48:09Z- LcmTrn::updateTrn() equating pose 1613177289.39 and meas times 1613177289.39 2021-02-13T00:48:09Z- LcmTrn::updateTrn() >>>> pose time 1613177289.39, meas time 1613177289.39 <<<< 2021-02-13T00:48:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177289.39 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:48:09Z- TerrainNav::motionUpdate() - currEstimate = (4060080, 593161.71, 15.31) 2021-02-13T00:48:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:48:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:48:09Z- TNavBF::Projecting Measurements 2021-02-13T00:48:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:48:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:48:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:48:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:48:09Z- TNavFilter::4 good beams. 2021-02-13T00:48:09Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:48:09Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:48:09Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:48:09Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:48:09Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:48:09Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:48:09Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:48:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:48:09Z- TerrainMapOctree: (vehN, vehE ) = (4060079.73, 593161.71). 2021-02-13T00:48:09Z- TerrainMapOctree: Min Distance = 1598.10. 2021-02-13T00:48:09Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:48:09Z- There is no specific good beam that all particles have in common. 2021-02-13T00:48:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:48:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 10.75 2021-02-13T00:48:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:48:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 14.92 2021-02-13T00:48:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:48:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 19.34 2021-02-13T00:48:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:48:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 8.18 2021-02-13T00:48:09Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:48:09Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:48:09Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:48:09Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:48:09Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:48:09Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:48:09Z- Filter 0 No beams used in subcloud update 2021-02-13T00:48:09Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:48:09Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:48:09Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 687.2 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:48:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177289.39 sec. 2021-02-13T00:48:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:48:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:48:09Z- variances([0], [2], and psi_berg) = 687.200418 832.991655 0.000000 2021-02-13T00:48:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:48:09Z- LcmTrn::publishEstimates() - MLE : 4060079.23 593184.21 15.31 2021-02-13T00:48:09Z- LcmTrn::publishEstimates() - MMSE : 4060079.73 593161.71 15.31 2021-02-13T00:48:09Z- LcmTrn::publishEstimates() - COVAR: 687.20 832.99 0.94 0.00 2021-02-13T00:48:09Z- AHRS_M2 msg: 1613177289.76 epoch sec; seqNo:15750 2021-02-13T00:48:09Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.12 psi 2021-02-13T00:48:09Z- handleDepth()-Depth_Keller msg: 1613177289768 epoch sec; seqNo:15873; depth 15.20 2021-02-13T00:48:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177289.79 epoch sec; seqNo:15873; 4060080.10 north; 593161.70 east 2021-02-13T00:48:10Z- AHRS_M2 msg: 1613177290.17 epoch sec; seqNo:15751 2021-02-13T00:48:10Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.12 psi 2021-02-13T00:48:10Z- handleDepth()-Depth_Keller msg: 1613177290182 epoch sec; seqNo:15874; depth 15.08 2021-02-13T00:48:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177290.21 epoch sec; seqNo:15874; 4060080.50 north; 593162.92 east 2021-02-13T00:48:10Z- AHRS_M2 msg: 1613177290.57 epoch sec; seqNo:15752 2021-02-13T00:48:10Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.12 psi 2021-02-13T00:48:10Z- handleDepth()-Depth_Keller msg: 1613177290579 epoch sec; seqNo:15875; depth 14.96 2021-02-13T00:48:10Z- handleDvl() - RDI_Pathfinder msg: 1613177290.59 epoch sec; seqNo:7641 2021-02-13T00:48:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.39 , 97.78 , 111.16 , 71.93 2021-02-13T00:48:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.05 , 0.00 2021-02-13T00:48:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177290.60 epoch sec; seqNo:15875; 4060080.88 north; 593162.92 east 2021-02-13T00:48:10Z- AHRS_M2 msg: 1613177290.98 epoch sec; seqNo:15753 2021-02-13T00:48:10Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.11 psi 2021-02-13T00:48:10Z- handleDepth()-Depth_Keller msg: 1613177290982 epoch sec; seqNo:15876; depth 14.84 2021-02-13T00:48:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177291.00 epoch sec; seqNo:15876; 4060080.88 north; 593162.92 east 2021-02-13T00:48:11Z- AHRS_M2 msg: 1613177291.38 epoch sec; seqNo:15754 2021-02-13T00:48:11Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.11 psi 2021-02-13T00:48:11Z- handleDepth()-Depth_Keller msg: 1613177291397 epoch sec; seqNo:15877; depth 14.71 2021-02-13T00:48:11Z- handleDvl() - RDI_Pathfinder msg: 1613177291.41 epoch sec; seqNo:7642 2021-02-13T00:48:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 97.64 , 113.46 , 72.75 2021-02-13T00:48:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.05 , -0.06 2021-02-13T00:48:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177291.42 epoch sec; seqNo:15877; 4060081.25 north; 593162.91 east 2021-02-13T00:48:11Z- AHRS_M2 msg: 1613177291.78 epoch sec; seqNo:15755 2021-02-13T00:48:11Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.12 psi 2021-02-13T00:48:11Z- handleDepth()-Depth_Keller msg: 1613177291793 epoch sec; seqNo:15878; depth 14.61 2021-02-13T00:48:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:48:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:48:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:48:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:48:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:48:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:48:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:48:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:48:11Z- handleDvl() - RDI_Pathfinder msg: 1613177291.80 epoch sec; seqNo:7643 2021-02-13T00:48:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:48:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.05 , -0.06 2021-02-13T00:48:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177291.82 epoch sec; seqNo:15878; 4060081.63 north; 593162.91 east 2021-02-13T00:48:12Z- AHRS_M2 msg: 1613177292.19 epoch sec; seqNo:15756 2021-02-13T00:48:12Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.11 psi 2021-02-13T00:48:12Z- handleDepth()-Depth_Keller msg: 1613177292196 epoch sec; seqNo:15879; depth 14.49 2021-02-13T00:48:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177292.20 epoch sec; seqNo:15879; 4060082.03 north; 593164.12 east 2021-02-13T00:48:12Z- AHRS_M2 msg: 1613177292.59 epoch sec; seqNo:15757 2021-02-13T00:48:12Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.11 psi 2021-02-13T00:48:12Z- handleDepth()-Depth_Keller msg: 1613177292598 epoch sec; seqNo:15880; depth 14.36 2021-02-13T00:48:12Z- handleDvl() - RDI_Pathfinder msg: 1613177292.61 epoch sec; seqNo:7644 2021-02-13T00:48:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.54 , 97.71 , 113.94 , 78.78 2021-02-13T00:48:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , -0.15 2021-02-13T00:48:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177292.62 epoch sec; seqNo:15880; 4060082.03 north; 593164.12 east 2021-02-13T00:48:12Z- AHRS_M2 msg: 1613177293.00 epoch sec; seqNo:15758 2021-02-13T00:48:12Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.12 psi 2021-02-13T00:48:13Z- handleDepth()-Depth_Keller msg: 1613177293000 epoch sec; seqNo:15881; depth 14.22 2021-02-13T00:48:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177293.02 epoch sec; seqNo:15881; 4060082.40 north; 593164.12 east 2021-02-13T00:48:13Z- AHRS_M2 msg: 1613177293.40 epoch sec; seqNo:15759 2021-02-13T00:48:13Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.14 psi 2021-02-13T00:48:13Z- handleDepth()-Depth_Keller msg: 1613177293404 epoch sec; seqNo:15882; depth 14.10 2021-02-13T00:48:13Z- handleDvl() - RDI_Pathfinder msg: 1613177293.42 epoch sec; seqNo:7645 2021-02-13T00:48:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 96.22 , 116.50 , 75.20 2021-02-13T00:48:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , -0.17 2021-02-13T00:48:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177293.44 epoch sec; seqNo:15882; 4060082.78 north; 593164.12 east 2021-02-13T00:48:13Z- AHRS_M2 msg: 1613177293.80 epoch sec; seqNo:15760 2021-02-13T00:48:13Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.15 psi 2021-02-13T00:48:13Z- handleDepth()-Depth_Keller msg: 1613177293808 epoch sec; seqNo:15883; depth 13.98 2021-02-13T00:48:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177293.82 epoch sec; seqNo:15883; 4060082.78 north; 593164.12 east 2021-02-13T00:48:14Z- AHRS_M2 msg: 1613177294.21 epoch sec; seqNo:15761 2021-02-13T00:48:14Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.16 psi 2021-02-13T00:48:14Z- handleDepth()-Depth_Keller msg: 1613177294225 epoch sec; seqNo:15884; depth 13.85 2021-02-13T00:48:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177294.25 epoch sec; seqNo:15884; 4060083.16 north; 593164.11 east 2021-02-13T00:48:14Z- AHRS_M2 msg: 1613177294.61 epoch sec; seqNo:15762 2021-02-13T00:48:14Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-13T00:48:14Z- handleDepth()-Depth_Keller msg: 1613177294622 epoch sec; seqNo:15885; depth 13.72 2021-02-13T00:48:14Z- handleDvl() - RDI_Pathfinder msg: 1613177294.63 epoch sec; seqNo:7646 2021-02-13T00:48:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.05 , 98.93 , 113.80 , 78.64 2021-02-13T00:48:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.14 , -0.08 2021-02-13T00:48:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177294.64 epoch sec; seqNo:15885; 4060083.54 north; 593164.11 east 2021-02-13T00:48:14Z- LcmTrn::handleMessages() - handling 45 messages took 0.77 ms and 130 calls... 2021-02-13T00:48:14Z- waiting for 1613177292.39, time is 1613177294.64 2021-02-13T00:48:14Z- LcmTrn::updateTrn() equating pose 1613177294.64 and meas times 1613177294.64 2021-02-13T00:48:14Z- LcmTrn::updateTrn() >>>> pose time 1613177294.64, meas time 1613177294.64 <<<< 2021-02-13T00:48:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177294.64 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:48:14Z- TerrainNav::motionUpdate() - currEstimate = (4060084, 593164.11, 13.72) 2021-02-13T00:48:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:48:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:48:14Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177294.64 sec.; No relevant navigation data available 2021-02-13T00:48:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:48:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:48:14Z- variances([0], [2], and psi_berg) = 687.150218 832.991632 0.000000 2021-02-13T00:48:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:48:14Z- LcmTrn::publishEstimates() - MLE : 4060083.04 593186.61 13.72 2021-02-13T00:48:14Z- LcmTrn::publishEstimates() - MMSE : 4060083.54 593164.11 13.72 2021-02-13T00:48:14Z- LcmTrn::publishEstimates() - COVAR: 687.15 832.99 1.17 0.00 2021-02-13T00:48:15Z- AHRS_M2 msg: 1613177295.02 epoch sec; seqNo:15763 2021-02-13T00:48:15Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.16 psi 2021-02-13T00:48:15Z- handleDepth()-Depth_Keller msg: 1613177295020 epoch sec; seqNo:15886; depth 13.62 2021-02-13T00:48:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177295.03 epoch sec; seqNo:15886; 4060083.92 north; 593164.10 east 2021-02-13T00:48:15Z- AHRS_M2 msg: 1613177295.42 epoch sec; seqNo:15764 2021-02-13T00:48:15Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-13T00:48:15Z- handleDepth()-Depth_Keller msg: 1613177295425 epoch sec; seqNo:15887; depth 13.48 2021-02-13T00:48:15Z- handleDvl() - RDI_Pathfinder msg: 1613177295.44 epoch sec; seqNo:7647 2021-02-13T00:48:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.70 , 98.93 , 113.80 , 78.64 2021-02-13T00:48:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.10 , 0.04 2021-02-13T00:48:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177295.45 epoch sec; seqNo:15887; 4060083.93 north; 593165.32 east 2021-02-13T00:48:15Z- AHRS_M2 msg: 1613177295.82 epoch sec; seqNo:15765 2021-02-13T00:48:15Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T00:48:15Z- handleDepth()-Depth_Keller msg: 1613177295830 epoch sec; seqNo:15888; depth 13.38 2021-02-13T00:48:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177295.85 epoch sec; seqNo:15888; 4060084.31 north; 593165.32 east 2021-02-13T00:48:16Z- AHRS_M2 msg: 1613177296.25 epoch sec; seqNo:15766 2021-02-13T00:48:16Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T00:48:16Z- handleDepth()-Depth_Keller msg: 1613177296258 epoch sec; seqNo:15889; depth 13.24 2021-02-13T00:48:16Z- handleDvl() - RDI_Pathfinder msg: 1613177296.27 epoch sec; seqNo:7648 2021-02-13T00:48:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.05 , 98.93 , 112.45 , 78.64 2021-02-13T00:48:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , 0.10 2021-02-13T00:48:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177296.28 epoch sec; seqNo:15889; 4060084.69 north; 593165.32 east 2021-02-13T00:48:16Z- AHRS_M2 msg: 1613177296.63 epoch sec; seqNo:15767 2021-02-13T00:48:16Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T00:48:16Z- handleDepth()-Depth_Keller msg: 1613177296641 epoch sec; seqNo:15890; depth 13.13 2021-02-13T00:48:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177296.67 epoch sec; seqNo:15890; 4060085.07 north; 593165.31 east 2021-02-13T00:48:17Z- AHRS_M2 msg: 1613177297.04 epoch sec; seqNo:15768 2021-02-13T00:48:17Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-13T00:48:17Z- handleDepth()-Depth_Keller msg: 1613177297040 epoch sec; seqNo:15891; depth 13.02 2021-02-13T00:48:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177297.05 epoch sec; seqNo:15891; 4060085.07 north; 593165.31 east 2021-02-13T00:48:17Z- AHRS_M2 msg: 1613177297.44 epoch sec; seqNo:15769 2021-02-13T00:48:17Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T00:48:17Z- handleDepth()-Depth_Keller msg: 1613177297446 epoch sec; seqNo:15892; depth 12.91 2021-02-13T00:48:17Z- handleDvl() - RDI_Pathfinder msg: 1613177297.46 epoch sec; seqNo:7649 2021-02-13T00:48:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.05 , 100.28 , 113.80 , 76.02 2021-02-13T00:48:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.05 , 0.06 2021-02-13T00:48:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177297.47 epoch sec; seqNo:15892; 4060085.45 north; 593165.31 east 2021-02-13T00:48:17Z- AHRS_M2 msg: 1613177297.84 epoch sec; seqNo:15770 2021-02-13T00:48:17Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T00:48:17Z- handleDepth()-Depth_Keller msg: 1613177297866 epoch sec; seqNo:15893; depth 12.77 2021-02-13T00:48:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177297.88 epoch sec; seqNo:15893; 4060085.84 north; 593166.52 east 2021-02-13T00:48:18Z- AHRS_M2 msg: 1613177298.25 epoch sec; seqNo:15771 2021-02-13T00:48:18Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.19 psi 2021-02-13T00:48:18Z- handleDepth()-Depth_Keller msg: 1613177298253 epoch sec; seqNo:15894; depth 12.67 2021-02-13T00:48:18Z- handleDvl() - RDI_Pathfinder msg: 1613177298.26 epoch sec; seqNo:7650 2021-02-13T00:48:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.05 , 102.98 , 113.80 , 73.56 2021-02-13T00:48:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.03 2021-02-13T00:48:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177298.29 epoch sec; seqNo:15894; 4060085.84 north; 593166.52 east 2021-02-13T00:48:18Z- AHRS_M2 msg: 1613177298.65 epoch sec; seqNo:15772 2021-02-13T00:48:18Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.20 psi 2021-02-13T00:48:18Z- handleDepth()-Depth_Keller msg: 1613177298656 epoch sec; seqNo:15895; depth 12.54 2021-02-13T00:48:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177298.67 epoch sec; seqNo:15895; 4060086.22 north; 593166.52 east 2021-02-13T00:48:19Z- AHRS_M2 msg: 1613177299.06 epoch sec; seqNo:15773 2021-02-13T00:48:19Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.20 psi 2021-02-13T00:48:19Z- handleDepth()-Depth_Keller msg: 1613177299064 epoch sec; seqNo:15896; depth 12.42 2021-02-13T00:48:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177299.10 epoch sec; seqNo:15896; 4060086.60 north; 593166.52 east 2021-02-13T00:48:19Z- AHRS_M2 msg: 1613177299.46 epoch sec; seqNo:15774 2021-02-13T00:48:19Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.19 psi 2021-02-13T00:48:19Z- handleDepth()-Depth_Keller msg: 1613177299464 epoch sec; seqNo:15897; depth 12.30 2021-02-13T00:48:19Z- handleDvl() - RDI_Pathfinder msg: 1613177299.48 epoch sec; seqNo:7651 2021-02-13T00:48:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.34 , 119.21 , 74.38 2021-02-13T00:48:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.02 , 0.07 2021-02-13T00:48:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177299.50 epoch sec; seqNo:15897; 4060086.98 north; 593166.51 east 2021-02-13T00:48:19Z- LcmTrn::handleMessages() - handling 41 messages took 0.59 ms and 129 calls... 2021-02-13T00:48:19Z- waiting for 1613177297.64, time is 1613177299.50 2021-02-13T00:48:19Z- LcmTrn::updateTrn() equating pose 1613177299.50 and meas times 1613177299.50 2021-02-13T00:48:19Z- LcmTrn::updateTrn() >>>> pose time 1613177299.50, meas time 1613177299.50 <<<< 2021-02-13T00:48:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177299.50 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:48:19Z- TerrainNav::motionUpdate() - currEstimate = (4060087, 593166.51, 12.30) 2021-02-13T00:48:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:48:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:48:19Z- TNavBF::Projecting Measurements 2021-02-13T00:48:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:48:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:48:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:48:19Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:48:19Z- TNavFilter::3 good beams. 2021-02-13T00:48:19Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:48:19Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:48:19Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:48:19Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:48:19Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:48:19Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:48:19Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:48:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:48:19Z- TerrainMapOctree: (vehN, vehE ) = (4060086.98, 593166.51). 2021-02-13T00:48:19Z- TerrainMapOctree: Min Distance = 1589.52. 2021-02-13T00:48:19Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:48:19Z- There is no specific good beam that all particles have in common. 2021-02-13T00:48:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:48:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 17.42 2021-02-13T00:48:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:48:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 22.74 2021-02-13T00:48:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:48:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-13T00:48:19Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:48:19Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:48:19Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:48:19Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:48:19Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:48:19Z- Filter 0 No beams used in subcloud update 2021-02-13T00:48:19Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:48:19Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:48:19Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 687.1 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:48:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177299.50 sec. 2021-02-13T00:48:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:48:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:48:19Z- variances([0], [2], and psi_berg) = 687.101821 832.991610 0.000000 2021-02-13T00:48:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:48:19Z- LcmTrn::publishEstimates() - MLE : 4060086.48 593188.01 12.30 2021-02-13T00:48:19Z- LcmTrn::publishEstimates() - MMSE : 4060086.98 593166.51 12.30 2021-02-13T00:48:19Z- LcmTrn::publishEstimates() - COVAR: 687.10 832.99 1.10 0.00 2021-02-13T00:48:19Z- AHRS_M2 msg: 1613177299.87 epoch sec; seqNo:15775 2021-02-13T00:48:19Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.19 psi 2021-02-13T00:48:19Z- handleDepth()-Depth_Keller msg: 1613177299871 epoch sec; seqNo:15898; depth 12.16 2021-02-13T00:48:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177299.88 epoch sec; seqNo:15898; 4060086.98 north; 593166.51 east 2021-02-13T00:48:20Z- AHRS_M2 msg: 1613177300.27 epoch sec; seqNo:15776 2021-02-13T00:48:20Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.18 psi 2021-02-13T00:48:20Z- handleDepth()-Depth_Keller msg: 1613177300274 epoch sec; seqNo:15899; depth 12.07 2021-02-13T00:48:20Z- handleDvl() - RDI_Pathfinder msg: 1613177300.29 epoch sec; seqNo:7652 2021-02-13T00:48:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:48:20Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:48:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177300.30 epoch sec; seqNo:15899; 4060087.36 north; 593166.51 east 2021-02-13T00:48:20Z- AHRS_M2 msg: 1613177300.67 epoch sec; seqNo:15777 2021-02-13T00:48:20Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.18 psi 2021-02-13T00:48:20Z- handleDepth()-Depth_Keller msg: 1613177300686 epoch sec; seqNo:15900; depth 11.92 2021-02-13T00:48:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177300.70 epoch sec; seqNo:15900; 4060087.75 north; 593167.72 east 2021-02-13T00:48:21Z- AHRS_M2 msg: 1613177301.08 epoch sec; seqNo:15778 2021-02-13T00:48:21Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.17 psi 2021-02-13T00:48:21Z- handleDepth()-Depth_Keller msg: 1613177301080 epoch sec; seqNo:15901; depth 11.82 2021-02-13T00:48:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177301.09 epoch sec; seqNo:15901; 4060088.13 north; 593167.72 east 2021-02-13T00:48:21Z- AHRS_M2 msg: 1613177301.48 epoch sec; seqNo:15779 2021-02-13T00:48:21Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.16 psi 2021-02-13T00:48:21Z- handleDepth()-Depth_Keller msg: 1613177301489 epoch sec; seqNo:15902; depth 11.69 2021-02-13T00:48:21Z- handleDvl() - RDI_Pathfinder msg: 1613177301.50 epoch sec; seqNo:7653 2021-02-13T00:48:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.28 , 102.51 , 121.44 , 74.79 2021-02-13T00:48:21Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.11 , 0.12 2021-02-13T00:48:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177301.53 epoch sec; seqNo:15902; 4060088.51 north; 593167.72 east 2021-02-13T00:48:21Z- AHRS_M2 msg: 1613177301.89 epoch sec; seqNo:15780 2021-02-13T00:48:21Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.16 psi 2021-02-13T00:48:21Z- handleDepth()-Depth_Keller msg: 1613177301894 epoch sec; seqNo:15903; depth 11.57 2021-02-13T00:48:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177301.91 epoch sec; seqNo:15903; 4060088.88 north; 593167.71 east 2021-02-13T00:48:22Z- AHRS_M2 msg: 1613177302.29 epoch sec; seqNo:15781 2021-02-13T00:48:22Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.16 psi 2021-02-13T00:48:22Z- handleDepth()-Depth_Keller msg: 1613177302308 epoch sec; seqNo:15904; depth 11.44 2021-02-13T00:48:22Z- handleDvl() - RDI_Pathfinder msg: 1613177302.32 epoch sec; seqNo:7654 2021-02-13T00:48:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.91 , 101.43 , 119.01 , 77.24 2021-02-13T00:48:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.19 , 0.02 2021-02-13T00:48:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177302.33 epoch sec; seqNo:15904; 4060088.88 north; 593167.71 east 2021-02-13T00:48:22Z- AHRS_M2 msg: 1613177302.70 epoch sec; seqNo:15782 2021-02-13T00:48:22Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.15 psi 2021-02-13T00:48:22Z- handleDepth()-Depth_Keller msg: 1613177302702 epoch sec; seqNo:15905; depth 11.31 2021-02-13T00:48:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177302.72 epoch sec; seqNo:15905; 4060089.28 north; 593168.93 east 2021-02-13T00:48:23Z- AHRS_M2 msg: 1613177303.10 epoch sec; seqNo:15783 2021-02-13T00:48:23Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.15 psi 2021-02-13T00:48:23Z- handleDepth()-Depth_Keller msg: 1613177303104 epoch sec; seqNo:15906; depth 11.19 2021-02-13T00:48:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177303.11 epoch sec; seqNo:15906; 4060089.28 north; 593168.93 east 2021-02-13T00:48:23Z- AHRS_M2 msg: 1613177303.50 epoch sec; seqNo:15784 2021-02-13T00:48:23Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.16 psi 2021-02-13T00:48:23Z- handleDepth()-Depth_Keller msg: 1613177303508 epoch sec; seqNo:15907; depth 11.06 2021-02-13T00:48:23Z- handleDvl() - RDI_Pathfinder msg: 1613177303.52 epoch sec; seqNo:7655 2021-02-13T00:48:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.78 , 120.36 , 78.44 2021-02-13T00:48:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.17 , -0.14 2021-02-13T00:48:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177303.53 epoch sec; seqNo:15907; 4060089.66 north; 593168.92 east 2021-02-13T00:48:23Z- AHRS_M2 msg: 1613177303.90 epoch sec; seqNo:15785 2021-02-13T00:48:23Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-13T00:48:23Z- handleDepth()-Depth_Keller msg: 1613177303908 epoch sec; seqNo:15908; depth 10.92 2021-02-13T00:48:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177303.93 epoch sec; seqNo:15908; 4060090.03 north; 593168.92 east 2021-02-13T00:48:24Z- AHRS_M2 msg: 1613177304.31 epoch sec; seqNo:15786 2021-02-13T00:48:24Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-13T00:48:24Z- handleDepth()-Depth_Keller msg: 1613177304317 epoch sec; seqNo:15909; depth 10.80 2021-02-13T00:48:24Z- handleDvl() - RDI_Pathfinder msg: 1613177304.34 epoch sec; seqNo:7656 2021-02-13T00:48:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.50 , 102.38 , 117.25 , 77.24 2021-02-13T00:48:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.05 , -0.19 2021-02-13T00:48:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177304.35 epoch sec; seqNo:15909; 4060090.03 north; 593168.92 east 2021-02-13T00:48:24Z- AHRS_M2 msg: 1613177304.71 epoch sec; seqNo:15787 2021-02-13T00:48:24Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T00:48:24Z- handleDepth()-Depth_Keller msg: 1613177304716 epoch sec; seqNo:15910; depth 10.66 2021-02-13T00:48:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177304.73 epoch sec; seqNo:15910; 4060090.41 north; 593168.92 east 2021-02-13T00:48:24Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-13T00:48:24Z- waiting for 1613177302.50, time is 1613177304.73 2021-02-13T00:48:24Z- LcmTrn::updateTrn() >>>> pose time 1613177304.73, meas time 1613177304.34 <<<< 2021-02-13T00:48:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177304.34 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:48:24Z- TerrainNav::motionUpdate() - currEstimate = (4060090, 593168.92, 10.66) 2021-02-13T00:48:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:48:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:48:24Z- TNavBF::Projecting Measurements 2021-02-13T00:48:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:48:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:48:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:48:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:48:24Z- TNavFilter::4 good beams. 2021-02-13T00:48:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:48:24Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:48:24Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:48:24Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:48:24Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:48:24Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:48:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:48:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:48:24Z- TerrainMapOctree: (vehN, vehE ) = (4060090.16, 593168.74). 2021-02-13T00:48:24Z- TerrainMapOctree: Min Distance = 1585.68. 2021-02-13T00:48:24Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:48:24Z- There is no specific good beam that all particles have in common. 2021-02-13T00:48:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:48:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 12.25 2021-02-13T00:48:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:48:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 16.77 2021-02-13T00:48:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:48:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 22.00 2021-02-13T00:48:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:48:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 9.55 2021-02-13T00:48:24Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:48:24Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:48:24Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:48:24Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:48:24Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:48:24Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:48:24Z- Filter 0 No beams used in subcloud update 2021-02-13T00:48:24Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:48:24Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:48:24Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 687.1 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:48:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177304.34 sec. 2021-02-13T00:48:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:48:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:48:24Z- variances([0], [2], and psi_berg) = 687.042068 832.991583 0.000000 2021-02-13T00:48:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:48:24Z- LcmTrn::publishEstimates() - MLE : 4060089.91 593190.42 10.66 2021-02-13T00:48:24Z- LcmTrn::publishEstimates() - MMSE : 4060090.41 593168.92 10.66 2021-02-13T00:48:24Z- LcmTrn::publishEstimates() - COVAR: 687.04 832.99 0.99 0.00 2021-02-13T00:48:25Z- AHRS_M2 msg: 1613177305.12 epoch sec; seqNo:15788 2021-02-13T00:48:25Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T00:48:25Z- handleDepth()-Depth_Keller msg: 1613177305129 epoch sec; seqNo:15911; depth 10.52 2021-02-13T00:48:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177305.14 epoch sec; seqNo:15911; 4060090.43 north; 593170.14 east 2021-02-13T00:48:25Z- AHRS_M2 msg: 1613177305.52 epoch sec; seqNo:15789 2021-02-13T00:48:25Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.18 psi 2021-02-13T00:48:25Z- handleDepth()-Depth_Keller msg: 1613177305524 epoch sec; seqNo:15912; depth 10.40 2021-02-13T00:48:25Z- handleDvl() - RDI_Pathfinder msg: 1613177305.54 epoch sec; seqNo:7657 2021-02-13T00:48:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.49 , 112.25 , 79.79 2021-02-13T00:48:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.12 , -0.10 2021-02-13T00:48:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177305.55 epoch sec; seqNo:15912; 4060090.81 north; 593170.13 east 2021-02-13T00:48:25Z- AHRS_M2 msg: 1613177305.92 epoch sec; seqNo:15790 2021-02-13T00:48:25Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.17 psi 2021-02-13T00:48:25Z- handleDepth()-Depth_Keller msg: 1613177305933 epoch sec; seqNo:15913; depth 10.28 2021-02-13T00:48:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177305.94 epoch sec; seqNo:15913; 4060091.18 north; 593170.13 east 2021-02-13T00:48:26Z- AHRS_M2 msg: 1613177306.33 epoch sec; seqNo:15791 2021-02-13T00:48:26Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.17 psi 2021-02-13T00:48:26Z- handleDepth()-Depth_Keller msg: 1613177306333 epoch sec; seqNo:15914; depth 10.15 2021-02-13T00:48:26Z- handleDvl() - RDI_Pathfinder msg: 1613177306.34 epoch sec; seqNo:7658 2021-02-13T00:48:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.28 , 106.57 , 114.68 , 79.52 2021-02-13T00:48:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.06 2021-02-13T00:48:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177306.35 epoch sec; seqNo:15914; 4060091.56 north; 593170.12 east 2021-02-13T00:48:26Z- AHRS_M2 msg: 1613177306.73 epoch sec; seqNo:15792 2021-02-13T00:48:26Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.16 psi 2021-02-13T00:48:26Z- handleDepth()-Depth_Keller msg: 1613177306737 epoch sec; seqNo:15915; depth 10.02 2021-02-13T00:48:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177306.75 epoch sec; seqNo:15915; 4060091.56 north; 593170.12 east 2021-02-13T00:48:27Z- AHRS_M2 msg: 1613177307.14 epoch sec; seqNo:15793 2021-02-13T00:48:27Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.16 psi 2021-02-13T00:48:27Z- handleDepth()-Depth_Keller msg: 1613177307143 epoch sec; seqNo:15916; depth 9.90 2021-02-13T00:48:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177307.16 epoch sec; seqNo:15916; 4060091.94 north; 593170.12 east 2021-02-13T00:48:27Z- AHRS_M2 msg: 1613177307.54 epoch sec; seqNo:15794 2021-02-13T00:48:27Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-13T00:48:27Z- handleDepth()-Depth_Keller msg: 1613177307544 epoch sec; seqNo:15917; depth 9.78 2021-02-13T00:48:27Z- handleDvl() - RDI_Pathfinder msg: 1613177307.56 epoch sec; seqNo:7659 2021-02-13T00:48:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.07 , 118.94 , 78.37 2021-02-13T00:48:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , 0.02 2021-02-13T00:48:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177307.58 epoch sec; seqNo:15917; 4060092.32 north; 593170.12 east 2021-02-13T00:48:27Z- AHRS_M2 msg: 1613177307.95 epoch sec; seqNo:15795 2021-02-13T00:48:27Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-13T00:48:27Z- handleDepth()-Depth_Keller msg: 1613177307951 epoch sec; seqNo:15918; depth 9.65 2021-02-13T00:48:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177307.96 epoch sec; seqNo:15918; 4060092.71 north; 593171.33 east 2021-02-13T00:48:28Z- AHRS_M2 msg: 1613177308.35 epoch sec; seqNo:15796 2021-02-13T00:48:28Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.14 psi 2021-02-13T00:48:28Z- handleDepth()-Depth_Keller msg: 1613177308361 epoch sec; seqNo:15919; depth 9.54 2021-02-13T00:48:28Z- handleDvl() - RDI_Pathfinder msg: 1613177308.37 epoch sec; seqNo:7660 2021-02-13T00:48:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 102.38 , 118.60 , 79.39 2021-02-13T00:48:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , -0.10 2021-02-13T00:48:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177308.38 epoch sec; seqNo:15919; 4060092.71 north; 593171.33 east 2021-02-13T00:48:28Z- AHRS_M2 msg: 1613177308.75 epoch sec; seqNo:15797 2021-02-13T00:48:28Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-13T00:48:28Z- handleDepth()-Depth_Keller msg: 1613177308756 epoch sec; seqNo:15920; depth 9.41 2021-02-13T00:48:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177308.77 epoch sec; seqNo:15920; 4060093.09 north; 593171.33 east 2021-02-13T00:48:29Z- AHRS_M2 msg: 1613177309.16 epoch sec; seqNo:15798 2021-02-13T00:48:29Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-13T00:48:29Z- handleDepth()-Depth_Keller msg: 1613177309169 epoch sec; seqNo:15921; depth 9.28 2021-02-13T00:48:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177309.19 epoch sec; seqNo:15921; 4060093.47 north; 593171.32 east 2021-02-13T00:48:29Z- AHRS_M2 msg: 1613177309.56 epoch sec; seqNo:15799 2021-02-13T00:48:29Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-13T00:48:29Z- handleDepth()-Depth_Keller msg: 1613177309564 epoch sec; seqNo:15922; depth 9.16 2021-02-13T00:48:29Z- handleDvl() - RDI_Pathfinder msg: 1613177309.58 epoch sec; seqNo:7661 2021-02-13T00:48:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 103.86 , 120.09 , 79.52 2021-02-13T00:48:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.14 , -0.11 2021-02-13T00:48:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177309.59 epoch sec; seqNo:15922; 4060093.85 north; 593171.32 east 2021-02-13T00:48:29Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-13T00:48:29Z- waiting for 1613177307.73, time is 1613177309.59 2021-02-13T00:48:29Z- LcmTrn::updateTrn() equating pose 1613177309.59 and meas times 1613177309.59 2021-02-13T00:48:29Z- LcmTrn::updateTrn() >>>> pose time 1613177309.59, meas time 1613177309.59 <<<< 2021-02-13T00:48:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177309.59 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:48:29Z- TerrainNav::motionUpdate() - currEstimate = (4060094, 593171.32, 9.16) 2021-02-13T00:48:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:48:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:48:29Z- TNavBF::Projecting Measurements 2021-02-13T00:48:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:48:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:48:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:48:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:48:29Z- TNavFilter::4 good beams. 2021-02-13T00:48:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:48:29Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:48:29Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:48:29Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:48:29Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:48:29Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:48:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:48:29Z- TNavBF::mmse selected from filter -1 2021-02-13T00:48:29Z- TerrainMapOctree: (vehN, vehE ) = (4060093.85, 593171.32). 2021-02-13T00:48:29Z- TerrainMapOctree: Min Distance = 1581.21. 2021-02-13T00:48:29Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:48:29Z- There is no specific good beam that all particles have in common. 2021-02-13T00:48:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:48:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 12.67 2021-02-13T00:48:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:48:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 17.26 2021-02-13T00:48:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:48:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 23.07 2021-02-13T00:48:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:48:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 10.12 2021-02-13T00:48:29Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:48:29Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:48:30Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:48:30Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:48:30Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:48:30Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:48:30Z- Filter 0 No beams used in subcloud update 2021-02-13T00:48:30Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:48:30Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:48:30Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 687.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:48:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177309.59 sec. 2021-02-13T00:48:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:48:30Z- TNavBF::mmse selected from filter -1 2021-02-13T00:48:30Z- variances([0], [2], and psi_berg) = 686.993725 832.991561 0.000000 2021-02-13T00:48:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:48:30Z- LcmTrn::publishEstimates() - MLE : 4060093.35 593192.82 9.16 2021-02-13T00:48:30Z- LcmTrn::publishEstimates() - MMSE : 4060093.85 593171.32 9.16 2021-02-13T00:48:30Z- LcmTrn::publishEstimates() - COVAR: 686.99 832.99 0.93 0.00 2021-02-13T00:48:30Z- AHRS_M2 msg: 1613177309.96 epoch sec; seqNo:15800 2021-02-13T00:48:30Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-13T00:48:30Z- handleDepth()-Depth_Keller msg: 1613177309970 epoch sec; seqNo:15923; depth 9.04 2021-02-13T00:48:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177309.98 epoch sec; seqNo:15923; 4060093.85 north; 593171.32 east 2021-02-13T00:48:30Z- AHRS_M2 msg: 1613177310.37 epoch sec; seqNo:15801 2021-02-13T00:48:30Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T00:48:30Z- handleDepth()-Depth_Keller msg: 1613177310373 epoch sec; seqNo:15924; depth 8.92 2021-02-13T00:48:30Z- handleDvl() - RDI_Pathfinder msg: 1613177310.39 epoch sec; seqNo:7662 2021-02-13T00:48:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 103.93 , 117.45 , 78.88 2021-02-13T00:48:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.18 , 0.01 2021-02-13T00:48:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177310.41 epoch sec; seqNo:15924; 4060094.23 north; 593171.32 east 2021-02-13T00:48:30Z- AHRS_M2 msg: 1613177310.77 epoch sec; seqNo:15802 2021-02-13T00:48:30Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.17 psi 2021-02-13T00:48:30Z- handleDepth()-Depth_Keller msg: 1613177310781 epoch sec; seqNo:15925; depth 8.78 2021-02-13T00:48:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177310.79 epoch sec; seqNo:15925; 4060094.62 north; 593172.53 east 2021-02-13T00:48:31Z- AHRS_M2 msg: 1613177311.18 epoch sec; seqNo:15803 2021-02-13T00:48:31Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-13T00:48:31Z- handleDepth()-Depth_Keller msg: 1613177311186 epoch sec; seqNo:15926; depth 8.67 2021-02-13T00:48:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177311.20 epoch sec; seqNo:15926; 4060095.00 north; 593172.53 east 2021-02-13T00:48:31Z- AHRS_M2 msg: 1613177311.58 epoch sec; seqNo:15804 2021-02-13T00:48:31Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T00:48:31Z- handleDepth()-Depth_Keller msg: 1613177311585 epoch sec; seqNo:15927; depth 8.56 2021-02-13T00:48:31Z- handleDvl() - RDI_Pathfinder msg: 1613177311.60 epoch sec; seqNo:7663 2021-02-13T00:48:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 103.79 , 117.32 , 77.65 2021-02-13T00:48:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.08 , 0.11 2021-02-13T00:48:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177311.61 epoch sec; seqNo:15927; 4060095.38 north; 593172.52 east 2021-02-13T00:48:31Z- AHRS_M2 msg: 1613177311.98 epoch sec; seqNo:15805 2021-02-13T00:48:31Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-13T00:48:31Z- handleDepth()-Depth_Keller msg: 1613177311988 epoch sec; seqNo:15928; depth 8.47 2021-02-13T00:48:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177312.01 epoch sec; seqNo:15928; 4060095.76 north; 593172.52 east 2021-02-13T00:48:32Z- AHRS_M2 msg: 1613177312.39 epoch sec; seqNo:15806 2021-02-13T00:48:32Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.15 psi 2021-02-13T00:48:32Z- handleDepth()-Depth_Keller msg: 1613177312397 epoch sec; seqNo:15929; depth 8.37 2021-02-13T00:48:32Z- handleDvl() - RDI_Pathfinder msg: 1613177312.41 epoch sec; seqNo:7664 2021-02-13T00:48:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.19 , 102.71 , 114.88 , 81.08 2021-02-13T00:48:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.08 , 0.10 2021-02-13T00:48:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177312.43 epoch sec; seqNo:15929; 4060095.76 north; 593172.52 east 2021-02-13T00:48:32Z- AHRS_M2 msg: 1613177312.80 epoch sec; seqNo:15807 2021-02-13T00:48:32Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.14 psi 2021-02-13T00:48:32Z- handleDepth()-Depth_Keller msg: 1613177312802 epoch sec; seqNo:15930; depth 8.25 2021-02-13T00:48:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177312.81 epoch sec; seqNo:15930; 4060096.14 north; 593172.52 east 2021-02-13T00:48:33Z- AHRS_M2 msg: 1613177313.21 epoch sec; seqNo:15808 2021-02-13T00:48:33Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.15 psi 2021-02-13T00:48:33Z- handleDepth()-Depth_Keller msg: 1613177313225 epoch sec; seqNo:15931; depth 8.15 2021-02-13T00:48:33Z- handleDvl() - RDI_Pathfinder msg: 1613177313.24 epoch sec; seqNo:7665 2021-02-13T00:48:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.36 , 117.59 , 81.08 2021-02-13T00:48:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.14 , 0.04 2021-02-13T00:48:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177313.26 epoch sec; seqNo:15931; 4060096.53 north; 593173.73 east 2021-02-13T00:48:33Z- AHRS_M2 msg: 1613177313.60 epoch sec; seqNo:15809 2021-02-13T00:48:33Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.15 psi 2021-02-13T00:48:33Z- handleDepth()-Depth_Keller msg: 1613177313604 epoch sec; seqNo:15932; depth 8.04 2021-02-13T00:48:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177313.62 epoch sec; seqNo:15932; 4060096.53 north; 593173.73 east 2021-02-13T00:48:34Z- AHRS_M2 msg: 1613177314.00 epoch sec; seqNo:15810 2021-02-13T00:48:34Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.15 psi 2021-02-13T00:48:34Z- handleDepth()-Depth_Keller msg: 1613177314008 epoch sec; seqNo:15933; depth 7.95 2021-02-13T00:48:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177314.02 epoch sec; seqNo:15933; 4060096.91 north; 593173.73 east 2021-02-13T00:48:34Z- AHRS_M2 msg: 1613177314.41 epoch sec; seqNo:15811 2021-02-13T00:48:34Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.15 psi 2021-02-13T00:48:34Z- handleDepth()-Depth_Keller msg: 1613177314412 epoch sec; seqNo:15934; depth 7.85 2021-02-13T00:48:34Z- handleDvl() - RDI_Pathfinder msg: 1613177314.42 epoch sec; seqNo:7666 2021-02-13T00:48:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.92 , 102.44 , 114.61 , 80.81 2021-02-13T00:48:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.09 , 0.00 2021-02-13T00:48:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177314.43 epoch sec; seqNo:15934; 4060096.91 north; 593173.73 east 2021-02-13T00:48:34Z- AHRS_M2 msg: 1613177314.81 epoch sec; seqNo:15812 2021-02-13T00:48:34Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.16 psi 2021-02-13T00:48:34Z- handleDepth()-Depth_Keller msg: 1613177314825 epoch sec; seqNo:15935; depth 7.74 2021-02-13T00:48:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177314.84 epoch sec; seqNo:15935; 4060097.29 north; 593173.72 east 2021-02-13T00:48:35Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:48:35Z- waiting for 1613177312.59, time is 1613177314.84 2021-02-13T00:48:35Z- LcmTrn::updateTrn() >>>> pose time 1613177314.84, meas time 1613177314.42 <<<< 2021-02-13T00:48:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177314.42 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:48:35Z- TerrainNav::motionUpdate() - currEstimate = (4060097, 593173.72, 7.74) 2021-02-13T00:48:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:48:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:48:35Z- TNavBF::Projecting Measurements 2021-02-13T00:48:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:48:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:48:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:48:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:48:35Z- TNavFilter::4 good beams. 2021-02-13T00:48:35Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:48:35Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:48:35Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:48:35Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:48:35Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:48:35Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:48:35Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:48:35Z- TNavBF::mmse selected from filter -1 2021-02-13T00:48:35Z- TerrainMapOctree: (vehN, vehE ) = (4060097.01, 593173.54). 2021-02-13T00:48:35Z- TerrainMapOctree: Min Distance = 1577.39. 2021-02-13T00:48:35Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:48:35Z- There is no specific good beam that all particles have in common. 2021-02-13T00:48:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:48:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 12.65 2021-02-13T00:48:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:48:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 16.79 2021-02-13T00:48:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:48:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 21.02 2021-02-13T00:48:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:48:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 10.45 2021-02-13T00:48:35Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:48:35Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:48:35Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:48:35Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:48:35Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:48:35Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:48:35Z- Filter 0 No beams used in subcloud update 2021-02-13T00:48:35Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:48:35Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:48:35Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 686.9 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:48:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177314.42 sec. 2021-02-13T00:48:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:48:35Z- TNavBF::mmse selected from filter -1 2021-02-13T00:48:35Z- variances([0], [2], and psi_berg) = 686.933801 832.991533 0.000000 2021-02-13T00:48:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:48:35Z- LcmTrn::publishEstimates() - MLE : 4060096.79 593195.22 7.74 2021-02-13T00:48:35Z- LcmTrn::publishEstimates() - MMSE : 4060097.29 593173.72 7.74 2021-02-13T00:48:35Z- LcmTrn::publishEstimates() - COVAR: 686.93 832.99 0.89 0.00 2021-02-13T00:48:35Z- AHRS_M2 msg: 1613177315.21 epoch sec; seqNo:15813 2021-02-13T00:48:35Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.17 psi 2021-02-13T00:48:35Z- handleDepth()-Depth_Keller msg: 1613177315221 epoch sec; seqNo:15936; depth 7.65 2021-02-13T00:48:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177315.24 epoch sec; seqNo:15936; 4060097.66 north; 593173.72 east 2021-02-13T00:48:35Z- AHRS_M2 msg: 1613177315.62 epoch sec; seqNo:15814 2021-02-13T00:48:35Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-13T00:48:35Z- handleDepth()-Depth_Keller msg: 1613177315624 epoch sec; seqNo:15937; depth 7.57 2021-02-13T00:48:35Z- handleDvl() - RDI_Pathfinder msg: 1613177315.64 epoch sec; seqNo:7667 2021-02-13T00:48:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.84 , 104.07 , 113.53 , 82.43 2021-02-13T00:48:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , 0.04 2021-02-13T00:48:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177315.66 epoch sec; seqNo:15937; 4060097.68 north; 593174.94 east 2021-02-13T00:48:36Z- AHRS_M2 msg: 1613177316.02 epoch sec; seqNo:15815 2021-02-13T00:48:36Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-13T00:48:36Z- handleDepth()-Depth_Keller msg: 1613177316028 epoch sec; seqNo:15938; depth 7.46 2021-02-13T00:48:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177316.04 epoch sec; seqNo:15938; 4060098.06 north; 593174.94 east 2021-02-13T00:48:36Z- AHRS_M2 msg: 1613177316.43 epoch sec; seqNo:15816 2021-02-13T00:48:36Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:48:36Z- handleDepth()-Depth_Keller msg: 1613177316438 epoch sec; seqNo:15939; depth 7.37 2021-02-13T00:48:36Z- handleDvl() - RDI_Pathfinder msg: 1613177316.45 epoch sec; seqNo:7668 2021-02-13T00:48:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.31 , 105.42 , 117.59 , 82.43 2021-02-13T00:48:36Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.09 , 0.14 2021-02-13T00:48:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177316.46 epoch sec; seqNo:15939; 4060098.44 north; 593174.93 east 2021-02-13T00:48:36Z- AHRS_M2 msg: 1613177316.83 epoch sec; seqNo:15817 2021-02-13T00:48:36Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.14 psi 2021-02-13T00:48:36Z- handleDepth()-Depth_Keller msg: 1613177316836 epoch sec; seqNo:15940; depth 7.29 2021-02-13T00:48:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177316.85 epoch sec; seqNo:15940; 4060098.81 north; 593174.93 east 2021-02-13T00:48:37Z- AHRS_M2 msg: 1613177317.24 epoch sec; seqNo:15818 2021-02-13T00:48:37Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.13 psi 2021-02-13T00:48:37Z- handleDepth()-Depth_Keller msg: 1613177317245 epoch sec; seqNo:15941; depth 7.20 2021-02-13T00:48:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177317.25 epoch sec; seqNo:15941; 4060098.81 north; 593174.93 east 2021-02-13T00:48:37Z- AHRS_M2 msg: 1613177317.64 epoch sec; seqNo:15819 2021-02-13T00:48:37Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.12 psi 2021-02-13T00:48:37Z- handleDepth()-Depth_Keller msg: 1613177317645 epoch sec; seqNo:15942; depth 7.10 2021-02-13T00:48:37Z- handleDvl() - RDI_Pathfinder msg: 1613177317.66 epoch sec; seqNo:7669 2021-02-13T00:48:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 105.08 , 119.95 , 82.09 2021-02-13T00:48:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.26 , 0.11 2021-02-13T00:48:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177317.67 epoch sec; seqNo:15942; 4060099.21 north; 593176.14 east 2021-02-13T00:48:38Z- AHRS_M2 msg: 1613177318.04 epoch sec; seqNo:15820 2021-02-13T00:48:38Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.11 psi 2021-02-13T00:48:38Z- handleDepth()-Depth_Keller msg: 1613177318058 epoch sec; seqNo:15943; depth 6.99 2021-02-13T00:48:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177318.08 epoch sec; seqNo:15943; 4060099.58 north; 593176.14 east 2021-02-13T00:48:38Z- AHRS_M2 msg: 1613177318.45 epoch sec; seqNo:15821 2021-02-13T00:48:38Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.11 psi 2021-02-13T00:48:38Z- handleDepth()-Depth_Keller msg: 1613177318452 epoch sec; seqNo:15944; depth 6.87 2021-02-13T00:48:38Z- handleDvl() - RDI_Pathfinder msg: 1613177318.46 epoch sec; seqNo:7670 2021-02-13T00:48:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 103.73 , 121.31 , 82.09 2021-02-13T00:48:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.30 , -0.17 2021-02-13T00:48:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177318.49 epoch sec; seqNo:15944; 4060099.58 north; 593176.14 east 2021-02-13T00:48:38Z- AHRS_M2 msg: 1613177318.85 epoch sec; seqNo:15822 2021-02-13T00:48:38Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.12 psi 2021-02-13T00:48:38Z- handleDepth()-Depth_Keller msg: 1613177318856 epoch sec; seqNo:15945; depth 6.76 2021-02-13T00:48:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177318.87 epoch sec; seqNo:15945; 4060099.96 north; 593176.14 east 2021-02-13T00:48:39Z- AHRS_M2 msg: 1613177319.26 epoch sec; seqNo:15823 2021-02-13T00:48:39Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.14 psi 2021-02-13T00:48:39Z- handleDepth()-Depth_Keller msg: 1613177319262 epoch sec; seqNo:15946; depth 6.65 2021-02-13T00:48:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177319.28 epoch sec; seqNo:15946; 4060099.98 north; 593177.36 east 2021-02-13T00:48:39Z- AHRS_M2 msg: 1613177319.66 epoch sec; seqNo:15824 2021-02-13T00:48:39Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-13T00:48:39Z- handleDepth()-Depth_Keller msg: 1613177319669 epoch sec; seqNo:15947; depth 6.51 2021-02-13T00:48:39Z- handleDvl() - RDI_Pathfinder msg: 1613177319.69 epoch sec; seqNo:7671 2021-02-13T00:48:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 105.08 , 122.66 , 82.09 2021-02-13T00:48:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.03 , -0.36 2021-02-13T00:48:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177319.70 epoch sec; seqNo:15947; 4060100.36 north; 593177.35 east 2021-02-13T00:48:40Z- AHRS_M2 msg: 1613177320.07 epoch sec; seqNo:15825 2021-02-13T00:48:40Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.20 psi 2021-02-13T00:48:40Z- handleDepth()-Depth_Keller msg: 1613177320074 epoch sec; seqNo:15948; depth 6.37 2021-02-13T00:48:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177320.09 epoch sec; seqNo:15948; 4060100.36 north; 593177.35 east 2021-02-13T00:48:40Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:48:40Z- waiting for 1613177317.84, time is 1613177320.09 2021-02-13T00:48:40Z- LcmTrn::updateTrn() >>>> pose time 1613177320.09, meas time 1613177319.69 <<<< 2021-02-13T00:48:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177319.69 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:48:40Z- TerrainNav::motionUpdate() - currEstimate = (4060100, 593177.35, 6.37) 2021-02-13T00:48:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:48:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:48:40Z- TNavBF::Projecting Measurements 2021-02-13T00:48:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:48:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:48:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:48:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:48:40Z- TNavFilter::4 good beams. 2021-02-13T00:48:40Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:48:40Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:48:40Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:48:40Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:48:40Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:48:40Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:48:40Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:48:40Z- TNavBF::mmse selected from filter -1 2021-02-13T00:48:40Z- TerrainMapOctree: (vehN, vehE ) = (4060100.12, 593177.08). 2021-02-13T00:48:40Z- TerrainMapOctree: Min Distance = 1572.70. 2021-02-13T00:48:40Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:48:40Z- There is no specific good beam that all particles have in common. 2021-02-13T00:48:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:48:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 15.04 2021-02-13T00:48:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:48:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 17.67 2021-02-13T00:48:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:48:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 24.07 2021-02-13T00:48:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:48:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 10.78 2021-02-13T00:48:40Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:48:40Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:48:40Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:48:40Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:48:40Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:48:40Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:48:40Z- Filter 0 No beams used in subcloud update 2021-02-13T00:48:40Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:48:40Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:48:40Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 686.9 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:48:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177319.69 sec. 2021-02-13T00:48:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:48:40Z- TNavBF::mmse selected from filter -1 2021-02-13T00:48:40Z- variances([0], [2], and psi_berg) = 686.870230 832.991504 0.000000 2021-02-13T00:48:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:48:40Z- LcmTrn::publishEstimates() - MLE : 4060099.86 593198.85 6.37 2021-02-13T00:48:40Z- LcmTrn::publishEstimates() - MMSE : 4060100.36 593177.35 6.37 2021-02-13T00:48:40Z- LcmTrn::publishEstimates() - COVAR: 686.87 832.99 0.91 0.00 2021-02-13T00:48:40Z- AHRS_M2 msg: 1613177320.47 epoch sec; seqNo:15826 2021-02-13T00:48:40Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.20 psi 2021-02-13T00:48:40Z- handleDepth()-Depth_Keller msg: 1613177320476 epoch sec; seqNo:15949; depth 6.24 2021-02-13T00:48:40Z- handleDvl() - RDI_Pathfinder msg: 1613177320.49 epoch sec; seqNo:7672 2021-02-13T00:48:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.98 , 109.88 , 124.75 , 83.11 2021-02-13T00:48:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.19 , -0.17 2021-02-13T00:48:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177320.50 epoch sec; seqNo:15949; 4060100.73 north; 593177.35 east 2021-02-13T00:48:40Z- AHRS_M2 msg: 1613177320.87 epoch sec; seqNo:15827 2021-02-13T00:48:40Z- AHRS_M2 msg: 0.57 phi; 0.37 theta; -0.20 psi 2021-02-13T00:48:40Z- handleDepth()-Depth_Keller msg: 1613177320876 epoch sec; seqNo:15950; depth 6.11 2021-02-13T00:48:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177320.89 epoch sec; seqNo:15950; 4060101.11 north; 593177.34 east 2021-02-13T00:48:41Z- AHRS_M2 msg: 1613177321.29 epoch sec; seqNo:15828 2021-02-13T00:48:41Z- AHRS_M2 msg: 0.56 phi; 0.38 theta; -0.20 psi 2021-02-13T00:48:41Z- handleDepth()-Depth_Keller msg: 1613177321295 epoch sec; seqNo:15951; depth 6.00 2021-02-13T00:48:41Z- handleDvl() - RDI_Pathfinder msg: 1613177321.31 epoch sec; seqNo:7673 2021-02-13T00:48:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 108.19 , 123.06 , 82.90 2021-02-13T00:48:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.06 , 0.04 2021-02-13T00:48:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177321.32 epoch sec; seqNo:15951; 4060101.49 north; 593177.34 east 2021-02-13T00:48:41Z- AHRS_M2 msg: 1613177321.68 epoch sec; seqNo:15829 2021-02-13T00:48:41Z- AHRS_M2 msg: 0.56 phi; 0.39 theta; -0.19 psi 2021-02-13T00:48:41Z- handleDepth()-Depth_Keller msg: 1613177321689 epoch sec; seqNo:15952; depth 5.87 2021-02-13T00:48:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177321.70 epoch sec; seqNo:15952; 4060101.87 north; 593177.34 east 2021-02-13T00:48:42Z- AHRS_M2 msg: 1613177322.08 epoch sec; seqNo:15830 2021-02-13T00:48:42Z- AHRS_M2 msg: 0.56 phi; 0.40 theta; -0.18 psi 2021-02-13T00:48:42Z- handleDepth()-Depth_Keller msg: 1613177322097 epoch sec; seqNo:15953; depth 5.75 2021-02-13T00:48:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177322.11 epoch sec; seqNo:15953; 4060102.25 north; 593177.33 east 2021-02-13T00:48:42Z- AHRS_M2 msg: 1613177322.49 epoch sec; seqNo:15831 2021-02-13T00:48:42Z- AHRS_M2 msg: 0.58 phi; 0.39 theta; -0.19 psi 2021-02-13T00:48:42Z- handleDepth()-Depth_Keller msg: 1613177322492 epoch sec; seqNo:15954; depth 5.61 2021-02-13T00:48:42Z- handleDvl() - RDI_Pathfinder msg: 1613177322.51 epoch sec; seqNo:7674 2021-02-13T00:48:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 111.23 , 127.59 , 82.97 2021-02-13T00:48:42Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.09 , 0.00 2021-02-13T00:48:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177322.52 epoch sec; seqNo:15954; 4060102.26 north; 593178.55 east 2021-02-13T00:48:42Z- AHRS_M2 msg: 1613177322.89 epoch sec; seqNo:15832 2021-02-13T00:48:42Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.18 psi 2021-02-13T00:48:42Z- handleDepth()-Depth_Keller msg: 1613177322896 epoch sec; seqNo:15955; depth 5.48 2021-02-13T00:48:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177322.91 epoch sec; seqNo:15955; 4060102.64 north; 593178.55 east 2021-02-13T00:48:43Z- AHRS_M2 msg: 1613177323.30 epoch sec; seqNo:15833 2021-02-13T00:48:43Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-13T00:48:43Z- handleDepth()-Depth_Keller msg: 1613177323302 epoch sec; seqNo:15956; depth 5.36 2021-02-13T00:48:43Z- handleDvl() - RDI_Pathfinder msg: 1613177323.31 epoch sec; seqNo:7675 2021-02-13T00:48:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 111.10 , 130.43 , 82.84 2021-02-13T00:48:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.07 , -0.11 2021-02-13T00:48:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177323.32 epoch sec; seqNo:15956; 4060103.02 north; 593178.54 east 2021-02-13T00:48:43Z- AHRS_M2 msg: 1613177323.70 epoch sec; seqNo:15834 2021-02-13T00:48:43Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.15 psi 2021-02-13T00:48:43Z- handleDepth()-Depth_Keller msg: 1613177323709 epoch sec; seqNo:15957; depth 5.23 2021-02-13T00:48:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177323.72 epoch sec; seqNo:15957; 4060103.02 north; 593178.54 east 2021-02-13T00:48:44Z- AHRS_M2 msg: 1613177324.10 epoch sec; seqNo:15835 2021-02-13T00:48:44Z- AHRS_M2 msg: 0.66 phi; 0.24 theta; -0.14 psi 2021-02-13T00:48:44Z- handleDepth()-Depth_Keller msg: 1613177324113 epoch sec; seqNo:15958; depth 5.09 2021-02-13T00:48:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177324.14 epoch sec; seqNo:15958; 4060103.40 north; 593178.54 east 2021-02-13T00:48:44Z- AHRS_M2 msg: 1613177324.51 epoch sec; seqNo:15836 2021-02-13T00:48:44Z- AHRS_M2 msg: 0.67 phi; 0.19 theta; -0.13 psi 2021-02-13T00:48:44Z- handleDepth()-Depth_Keller msg: 1613177324512 epoch sec; seqNo:15959; depth 4.93 2021-02-13T00:48:44Z- handleDvl() - RDI_Pathfinder msg: 1613177324.52 epoch sec; seqNo:7676 2021-02-13T00:48:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.69 , 106.57 , 124.42 , 82.77 2021-02-13T00:48:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.54 , -0.05 , -0.12 2021-02-13T00:48:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177324.53 epoch sec; seqNo:15959; 4060103.40 north; 593178.54 east 2021-02-13T00:48:44Z- AHRS_M2 msg: 1613177324.91 epoch sec; seqNo:15837 2021-02-13T00:48:44Z- AHRS_M2 msg: 0.67 phi; 0.14 theta; -0.13 psi 2021-02-13T00:48:44Z- handleDepth()-Depth_Keller msg: 1613177324925 epoch sec; seqNo:15960; depth 4.79 2021-02-13T00:48:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177324.94 epoch sec; seqNo:15960; 4060103.78 north; 593178.54 east 2021-02-13T00:48:45Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-13T00:48:45Z- waiting for 1613177323.09, time is 1613177324.94 2021-02-13T00:48:45Z- LcmTrn::updateTrn() >>>> pose time 1613177324.94, meas time 1613177324.52 <<<< 2021-02-13T00:48:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177324.52 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:48:45Z- TerrainNav::motionUpdate() - currEstimate = (4060104, 593178.54, 4.79) 2021-02-13T00:48:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:48:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:48:45Z- TNavBF::Projecting Measurements 2021-02-13T00:48:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:48:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:48:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:48:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:48:45Z- TNavFilter::4 good beams. 2021-02-13T00:48:45Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:48:45Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:48:45Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:48:45Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:48:45Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:48:45Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:48:45Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:48:45Z- TNavBF::mmse selected from filter -1 2021-02-13T00:48:45Z- TerrainMapOctree: (vehN, vehE ) = (4060103.49, 593178.44). 2021-02-13T00:48:45Z- TerrainMapOctree: Min Distance = 1569.31. 2021-02-13T00:48:45Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:48:45Z- There is no specific good beam that all particles have in common. 2021-02-13T00:48:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:48:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 13.45 2021-02-13T00:48:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:48:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 18.17 2021-02-13T00:48:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:48:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.035 ExpectedMeasDiff: nan currMeas.covariance: 24.77 2021-02-13T00:48:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:48:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 10.96 2021-02-13T00:48:45Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:48:45Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:48:45Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:48:45Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:48:45Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:48:45Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:48:45Z- Filter 0 No beams used in subcloud update 2021-02-13T00:48:45Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:48:45Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:48:45Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 686.8 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:48:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177324.52 sec. 2021-02-13T00:48:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:48:45Z- TNavBF::mmse selected from filter -1 2021-02-13T00:48:45Z- variances([0], [2], and psi_berg) = 686.814258 832.991479 0.000000 2021-02-13T00:48:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:48:45Z- LcmTrn::publishEstimates() - MLE : 4060103.28 593200.04 4.79 2021-02-13T00:48:45Z- LcmTrn::publishEstimates() - MMSE : 4060103.78 593178.54 4.79 2021-02-13T00:48:45Z- LcmTrn::publishEstimates() - COVAR: 686.81 832.99 0.88 0.00 2021-02-13T00:48:45Z- AHRS_M2 msg: 1613177325.32 epoch sec; seqNo:15838 2021-02-13T00:48:45Z- AHRS_M2 msg: 0.67 phi; 0.10 theta; -0.15 psi 2021-02-13T00:48:45Z- handleDepth()-Depth_Keller msg: 1613177325325 epoch sec; seqNo:15961; depth 4.64 2021-02-13T00:48:45Z- handleDvl() - RDI_Pathfinder msg: 1613177325.34 epoch sec; seqNo:7677 2021-02-13T00:48:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.98 , 102.44 , 105.42 , 0.00 2021-02-13T00:48:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.61 , -0.30 , -0.03 2021-02-13T00:48:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177325.35 epoch sec; seqNo:15961; 4060104.17 north; 593179.75 east 2021-02-13T00:48:45Z- AHRS_M2 msg: 1613177325.72 epoch sec; seqNo:15839 2021-02-13T00:48:45Z- AHRS_M2 msg: 0.67 phi; 0.06 theta; -0.18 psi 2021-02-13T00:48:45Z- handleDepth()-Depth_Keller msg: 1613177325729 epoch sec; seqNo:15962; depth 4.52 2021-02-13T00:48:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177325.74 epoch sec; seqNo:15962; 4060104.17 north; 593179.75 east 2021-02-13T00:48:46Z- AHRS_M2 msg: 1613177326.13 epoch sec; seqNo:15840 2021-02-13T00:48:46Z- AHRS_M2 msg: 0.67 phi; 0.02 theta; -0.22 psi 2021-02-13T00:48:46Z- handleDepth()-Depth_Keller msg: 1613177326149 epoch sec; seqNo:15963; depth 4.42 2021-02-13T00:48:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177326.16 epoch sec; seqNo:15963; 4060104.55 north; 593179.75 east 2021-02-13T00:48:46Z- AHRS_M2 msg: 1613177326.53 epoch sec; seqNo:15841 2021-02-13T00:48:46Z- AHRS_M2 msg: 0.67 phi; -0.02 theta; -0.23 psi 2021-02-13T00:48:46Z- handleDepth()-Depth_Keller msg: 1613177326532 epoch sec; seqNo:15964; depth 4.33 2021-02-13T00:48:46Z- handleDvl() - RDI_Pathfinder msg: 1613177326.54 epoch sec; seqNo:7678 2021-02-13T00:48:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.01 , 106.84 , 97.91 , 91.96 2021-02-13T00:48:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.30 , 0.38 2021-02-13T00:48:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177326.56 epoch sec; seqNo:15964; 4060104.93 north; 593179.74 east 2021-02-13T00:48:46Z- AHRS_M2 msg: 1613177326.93 epoch sec; seqNo:15842 2021-02-13T00:48:46Z- AHRS_M2 msg: 0.66 phi; -0.05 theta; -0.23 psi 2021-02-13T00:48:46Z- handleDepth()-Depth_Keller msg: 1613177326936 epoch sec; seqNo:15965; depth 4.28 2021-02-13T00:48:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177326.95 epoch sec; seqNo:15965; 4060105.31 north; 593179.74 east 2021-02-13T00:48:47Z- AHRS_M2 msg: 1613177327.36 epoch sec; seqNo:15843 2021-02-13T00:48:47Z- AHRS_M2 msg: 0.65 phi; -0.08 theta; -0.22 psi 2021-02-13T00:48:47Z- handleDepth()-Depth_Keller msg: 1613177327361 epoch sec; seqNo:15966; depth 4.23 2021-02-13T00:48:47Z- handleDvl() - RDI_Pathfinder msg: 1613177327.37 epoch sec; seqNo:7679 2021-02-13T00:48:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.20 , 109.54 , 97.37 , 98.72 2021-02-13T00:48:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.16 , 0.62 2021-02-13T00:48:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177327.38 epoch sec; seqNo:15966; 4060105.31 north; 593179.74 east 2021-02-13T00:48:47Z- AHRS_M2 msg: 1613177327.74 epoch sec; seqNo:15844 2021-02-13T00:48:47Z- AHRS_M2 msg: 0.65 phi; -0.11 theta; -0.21 psi 2021-02-13T00:48:47Z- handleDepth()-Depth_Keller msg: 1613177327744 epoch sec; seqNo:15967; depth 4.20 2021-02-13T00:48:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177327.76 epoch sec; seqNo:15967; 4060105.69 north; 593179.74 east 2021-02-13T00:48:48Z- AHRS_M2 msg: 1613177328.15 epoch sec; seqNo:15845 2021-02-13T00:48:48Z- AHRS_M2 msg: 0.64 phi; -0.13 theta; -0.20 psi 2021-02-13T00:48:48Z- handleDepth()-Depth_Keller msg: 1613177328154 epoch sec; seqNo:15968; depth 4.19 2021-02-13T00:48:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177328.16 epoch sec; seqNo:15968; 4060105.70 north; 593180.96 east 2021-02-13T00:48:48Z- handleDepth()-Depth_Keller msg: 1613177328565 epoch sec; seqNo:15969; depth 4.20 2021-02-13T00:48:48Z- handleDvl() - RDI_Pathfinder msg: 1613177328.58 epoch sec; seqNo:7680 2021-02-13T00:48:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.77 , 108.05 , 90.48 , 104.00 2021-02-13T00:48:48Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.54 , 0.38 2021-02-13T00:48:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177328.59 epoch sec; seqNo:15969; 4060106.08 north; 593180.95 east 2021-02-13T00:48:48Z- AHRS_M2 msg: 1613177328.95 epoch sec; seqNo:15846 2021-02-13T00:48:48Z- AHRS_M2 msg: 0.65 phi; -0.16 theta; -0.23 psi 2021-02-13T00:48:48Z- handleDepth()-Depth_Keller msg: 1613177328959 epoch sec; seqNo:15970; depth 4.20 2021-02-13T00:48:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177328.97 epoch sec; seqNo:15970; 4060106.08 north; 593180.95 east 2021-02-13T00:48:49Z- AHRS_M2 msg: 1613177329.37 epoch sec; seqNo:15847 2021-02-13T00:48:49Z- AHRS_M2 msg: 0.66 phi; -0.17 theta; -0.25 psi 2021-02-13T00:48:49Z- handleDepth()-Depth_Keller msg: 1613177329379 epoch sec; seqNo:15971; depth 4.22 2021-02-13T00:48:49Z- handleDvl() - RDI_Pathfinder msg: 1613177329.39 epoch sec; seqNo:7681 2021-02-13T00:48:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.77 , 110.76 , 97.24 , 106.70 2021-02-13T00:48:49Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.51 , -0.09 2021-02-13T00:48:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177329.40 epoch sec; seqNo:15971; 4060106.47 north; 593182.17 east 2021-02-13T00:48:49Z- AHRS_M2 msg: 1613177329.76 epoch sec; seqNo:15848 2021-02-13T00:48:49Z- AHRS_M2 msg: 0.67 phi; -0.19 theta; -0.26 psi 2021-02-13T00:48:49Z- handleDepth()-Depth_Keller msg: 1613177329764 epoch sec; seqNo:15972; depth 4.25 2021-02-13T00:48:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177329.78 epoch sec; seqNo:15972; 4060106.47 north; 593182.17 east 2021-02-13T00:48:50Z- AHRS_M2 msg: 1613177330.16 epoch sec; seqNo:15849 2021-02-13T00:48:50Z- AHRS_M2 msg: 0.68 phi; -0.22 theta; -0.26 psi 2021-02-13T00:48:50Z- handleDepth()-Depth_Keller msg: 1613177330170 epoch sec; seqNo:15973; depth 4.27 2021-02-13T00:48:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177330.18 epoch sec; seqNo:15973; 4060106.85 north; 593182.17 east 2021-02-13T00:48:50Z- LcmTrn::handleMessages() - handling 43 messages took 0.61 ms and 131 calls... 2021-02-13T00:48:50Z- waiting for 1613177327.94, time is 1613177330.18 2021-02-13T00:48:50Z- LcmTrn::updateTrn() >>>> pose time 1613177330.18, meas time 1613177329.39 <<<< 2021-02-13T00:48:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177329.39 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:48:50Z- TerrainNav::motionUpdate() - currEstimate = (4060107, 593182.17, 4.27) 2021-02-13T00:48:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:48:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:48:50Z- TNavBF::Projecting Measurements 2021-02-13T00:48:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:48:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:48:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:48:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:48:50Z- TNavFilter::4 good beams. 2021-02-13T00:48:50Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:48:50Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:48:50Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:48:50Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:48:50Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:48:50Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:48:50Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:48:50Z- TNavBF::mmse selected from filter -1 2021-02-13T00:48:50Z- TerrainMapOctree: (vehN, vehE ) = (4060106.39, 593181.62). 2021-02-13T00:48:50Z- TerrainMapOctree: Min Distance = 1565.02. 2021-02-13T00:48:50Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:48:50Z- There is no specific good beam that all particles have in common. 2021-02-13T00:48:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:48:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 12.33 2021-02-13T00:48:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:48:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 19.63 2021-02-13T00:48:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:48:50Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-13T00:48:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:48:50Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 18.22 2021-02-13T00:48:50Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:48:50Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:48:50Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:48:50Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:48:50Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:48:50Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:48:50Z- Filter 0 No beams used in subcloud update 2021-02-13T00:48:50Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:48:50Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:48:50Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 686.8 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:48:50Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177329.39 sec. 2021-02-13T00:48:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:48:50Z- TNavBF::mmse selected from filter -1 2021-02-13T00:48:50Z- variances([0], [2], and psi_berg) = 686.747038 832.991448 0.000000 2021-02-13T00:48:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:48:50Z- LcmTrn::publishEstimates() - MLE : 4060106.35 593203.67 4.27 2021-02-13T00:48:50Z- LcmTrn::publishEstimates() - MMSE : 4060106.85 593182.17 4.27 2021-02-13T00:48:50Z- LcmTrn::publishEstimates() - COVAR: 686.75 832.99 0.91 0.00 2021-02-13T00:48:50Z- AHRS_M2 msg: 1613177330.57 epoch sec; seqNo:15850 2021-02-13T00:48:50Z- AHRS_M2 msg: 0.69 phi; -0.24 theta; -0.27 psi 2021-02-13T00:48:50Z- handleDepth()-Depth_Keller msg: 1613177330574 epoch sec; seqNo:15974; depth 4.30 2021-02-13T00:48:50Z- handleDvl() - RDI_Pathfinder msg: 1613177330.59 epoch sec; seqNo:7682 2021-02-13T00:48:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.15 , 115.90 , 96.97 , 113.19 2021-02-13T00:48:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , -0.39 2021-02-13T00:48:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177330.60 epoch sec; seqNo:15974; 4060107.23 north; 593182.16 east 2021-02-13T00:48:50Z- AHRS_M2 msg: 1613177330.97 epoch sec; seqNo:15851 2021-02-13T00:48:50Z- AHRS_M2 msg: 0.70 phi; -0.26 theta; -0.26 psi 2021-02-13T00:48:50Z- handleDepth()-Depth_Keller msg: 1613177330976 epoch sec; seqNo:15975; depth 4.34 2021-02-13T00:48:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177331.00 epoch sec; seqNo:15975; 4060107.62 north; 593183.38 east 2021-02-13T00:48:51Z- AHRS_M2 msg: 1613177331.38 epoch sec; seqNo:15852 2021-02-13T00:48:51Z- AHRS_M2 msg: 0.70 phi; -0.28 theta; -0.25 psi 2021-02-13T00:48:51Z- handleDepth()-Depth_Keller msg: 1613177331380 epoch sec; seqNo:15976; depth 4.38 2021-02-13T00:48:51Z- handleDvl() - RDI_Pathfinder msg: 1613177331.39 epoch sec; seqNo:7683 2021-02-13T00:48:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.44 , 115.69 , 87.44 , 117.18 2021-02-13T00:48:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.28 , -0.28 2021-02-13T00:48:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177331.42 epoch sec; seqNo:15976; 4060108.00 north; 593183.37 east 2021-02-13T00:48:51Z- AHRS_M2 msg: 1613177331.78 epoch sec; seqNo:15853 2021-02-13T00:48:51Z- AHRS_M2 msg: 0.70 phi; -0.29 theta; -0.24 psi 2021-02-13T00:48:51Z- handleDepth()-Depth_Keller msg: 1613177331784 epoch sec; seqNo:15977; depth 4.44 2021-02-13T00:48:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177331.80 epoch sec; seqNo:15977; 4060108.00 north; 593183.37 east 2021-02-13T00:48:52Z- AHRS_M2 msg: 1613177332.19 epoch sec; seqNo:15854 2021-02-13T00:48:52Z- AHRS_M2 msg: 0.71 phi; -0.30 theta; -0.23 psi 2021-02-13T00:48:52Z- handleDepth()-Depth_Keller msg: 1613177332191 epoch sec; seqNo:15978; depth 4.52 2021-02-13T00:48:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177332.20 epoch sec; seqNo:15978; 4060108.38 north; 593183.37 east 2021-02-13T00:48:52Z- AHRS_M2 msg: 1613177332.59 epoch sec; seqNo:15855 2021-02-13T00:48:52Z- AHRS_M2 msg: 0.71 phi; -0.31 theta; -0.21 psi 2021-02-13T00:48:52Z- handleDepth()-Depth_Keller msg: 1613177332593 epoch sec; seqNo:15979; depth 4.61 2021-02-13T00:48:52Z- handleDvl() - RDI_Pathfinder msg: 1613177332.61 epoch sec; seqNo:7684 2021-02-13T00:48:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.51 , 114.28 , 0.00 , 120.22 2021-02-13T00:48:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.32 , 0.03 2021-02-13T00:48:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177332.62 epoch sec; seqNo:15979; 4060108.76 north; 593183.37 east 2021-02-13T00:48:52Z- AHRS_M2 msg: 1613177332.99 epoch sec; seqNo:15856 2021-02-13T00:48:52Z- AHRS_M2 msg: 0.71 phi; -0.33 theta; -0.18 psi 2021-02-13T00:48:52Z- handleDepth()-Depth_Keller msg: 1613177332996 epoch sec; seqNo:15980; depth 4.70 2021-02-13T00:48:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177333.01 epoch sec; seqNo:15980; 4060109.14 north; 593183.36 east 2021-02-13T00:48:53Z- AHRS_M2 msg: 1613177333.40 epoch sec; seqNo:15857 2021-02-13T00:48:53Z- AHRS_M2 msg: 0.71 phi; -0.34 theta; -0.16 psi 2021-02-13T00:48:53Z- handleDepth()-Depth_Keller msg: 1613177333405 epoch sec; seqNo:15981; depth 4.81 2021-02-13T00:48:53Z- handleDvl() - RDI_Pathfinder msg: 1613177333.42 epoch sec; seqNo:7685 2021-02-13T00:48:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.07 , 107.92 , 0.00 , 122.79 2021-02-13T00:48:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.06 , 0.25 2021-02-13T00:48:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177333.43 epoch sec; seqNo:15981; 4060109.51 north; 593183.36 east 2021-02-13T00:48:53Z- AHRS_M2 msg: 1613177333.80 epoch sec; seqNo:15858 2021-02-13T00:48:53Z- AHRS_M2 msg: 0.70 phi; -0.36 theta; -0.15 psi 2021-02-13T00:48:53Z- handleDepth()-Depth_Keller msg: 1613177333806 epoch sec; seqNo:15982; depth 4.91 2021-02-13T00:48:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177333.82 epoch sec; seqNo:15982; 4060109.51 north; 593183.36 east 2021-02-13T00:48:54Z- AHRS_M2 msg: 1613177334.21 epoch sec; seqNo:15859 2021-02-13T00:48:54Z- AHRS_M2 msg: 0.68 phi; -0.37 theta; -0.14 psi 2021-02-13T00:48:54Z- handleDepth()-Depth_Keller msg: 1613177334211 epoch sec; seqNo:15983; depth 5.00 2021-02-13T00:48:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177334.23 epoch sec; seqNo:15983; 4060109.91 north; 593184.57 east 2021-02-13T00:48:54Z- AHRS_M2 msg: 1613177334.61 epoch sec; seqNo:15860 2021-02-13T00:48:54Z- AHRS_M2 msg: 0.67 phi; -0.38 theta; -0.15 psi 2021-02-13T00:48:54Z- handleDepth()-Depth_Keller msg: 1613177334617 epoch sec; seqNo:15984; depth 5.09 2021-02-13T00:48:54Z- handleDvl() - RDI_Pathfinder msg: 1613177334.63 epoch sec; seqNo:7686 2021-02-13T00:48:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.51 , 109.41 , 82.63 , 127.25 2021-02-13T00:48:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.28 , 0.20 2021-02-13T00:48:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177334.64 epoch sec; seqNo:15984; 4060109.91 north; 593184.57 east 2021-02-13T00:48:55Z- AHRS_M2 msg: 1613177335.02 epoch sec; seqNo:15861 2021-02-13T00:48:55Z- AHRS_M2 msg: 0.66 phi; -0.38 theta; -0.17 psi 2021-02-13T00:48:55Z- handleDepth()-Depth_Keller msg: 1613177335025 epoch sec; seqNo:15985; depth 5.21 2021-02-13T00:48:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177335.04 epoch sec; seqNo:15985; 4060109.91 north; 593184.57 east 2021-02-13T00:48:55Z- AHRS_M2 msg: 1613177335.41 epoch sec; seqNo:15862 2021-02-13T00:48:55Z- AHRS_M2 msg: 0.65 phi; -0.37 theta; -0.19 psi 2021-02-13T00:48:55Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-13T00:48:55Z- waiting for 1613177333.18, time is 1613177335.41 2021-02-13T00:48:55Z- LcmTrn::updateTrn() >>>> pose time 1613177335.41, meas time 1613177334.63 <<<< 2021-02-13T00:48:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177334.63 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:48:55Z- TerrainNav::motionUpdate() - currEstimate = (4060110, 593184.57, 5.21) 2021-02-13T00:48:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:48:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:48:55Z- TNavBF::Projecting Measurements 2021-02-13T00:48:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:48:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:48:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:48:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:48:55Z- TNavFilter::4 good beams. 2021-02-13T00:48:55Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:48:55Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:48:55Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:48:55Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:48:55Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:48:55Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:48:55Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:48:55Z- TNavBF::mmse selected from filter -1 2021-02-13T00:48:55Z- TerrainMapOctree: (vehN, vehE ) = (4060109.45, 593184.21). 2021-02-13T00:48:55Z- TerrainMapOctree: Min Distance = 1561.01. 2021-02-13T00:48:55Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:48:55Z- There is no specific good beam that all particles have in common. 2021-02-13T00:48:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:48:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.21 2021-02-13T00:48:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:48:55Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 19.15 2021-02-13T00:48:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:48:55Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 10.93 2021-02-13T00:48:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:48:55Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.031 ExpectedMeasDiff: nan currMeas.covariance: 25.91 2021-02-13T00:48:55Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:48:55Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:48:55Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:48:55Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:48:55Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:48:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:48:55Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:48:55Z- Filter 0 No beams used in subcloud update 2021-02-13T00:48:55Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:48:55Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:48:55Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 686.7 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:48:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177334.63 sec. 2021-02-13T00:48:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:48:55Z- TNavBF::mmse selected from filter -1 2021-02-13T00:48:55Z- variances([0], [2], and psi_berg) = 686.686298 832.991420 0.000000 2021-02-13T00:48:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:48:55Z- LcmTrn::publishEstimates() - MLE : 4060109.41 593206.07 5.21 2021-02-13T00:48:55Z- LcmTrn::publishEstimates() - MMSE : 4060109.91 593184.57 5.21 2021-02-13T00:48:55Z- LcmTrn::publishEstimates() - COVAR: 686.69 832.99 0.90 0.00 2021-02-13T00:48:55Z- handleDepth()-Depth_Keller msg: 1613177335425 epoch sec; seqNo:15986; depth 5.30 2021-02-13T00:48:55Z- handleDvl() - RDI_Pathfinder msg: 1613177335.44 epoch sec; seqNo:7687 2021-02-13T00:48:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 111.17 , 79.69 , 134.96 2021-02-13T00:48:55Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.28 , -0.12 2021-02-13T00:48:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177335.45 epoch sec; seqNo:15986; 4060110.29 north; 593184.57 east 2021-02-13T00:48:55Z- AHRS_M2 msg: 1613177335.82 epoch sec; seqNo:15863 2021-02-13T00:48:55Z- AHRS_M2 msg: 0.65 phi; -0.36 theta; -0.23 psi 2021-02-13T00:48:55Z- handleDepth()-Depth_Keller msg: 1613177335824 epoch sec; seqNo:15987; depth 5.43 2021-02-13T00:48:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177335.84 epoch sec; seqNo:15987; 4060110.68 north; 593185.79 east 2021-02-13T00:48:56Z- AHRS_M2 msg: 1613177336.23 epoch sec; seqNo:15864 2021-02-13T00:48:56Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.25 psi 2021-02-13T00:48:56Z- handleDepth()-Depth_Keller msg: 1613177336231 epoch sec; seqNo:15988; depth 5.55 2021-02-13T00:48:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177336.24 epoch sec; seqNo:15988; 4060110.68 north; 593185.79 east 2021-02-13T00:48:56Z- AHRS_M2 msg: 1613177336.63 epoch sec; seqNo:15865 2021-02-13T00:48:56Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.26 psi 2021-02-13T00:48:56Z- handleDepth()-Depth_Keller msg: 1613177336632 epoch sec; seqNo:15989; depth 5.67 2021-02-13T00:48:56Z- handleDvl() - RDI_Pathfinder msg: 1613177336.65 epoch sec; seqNo:7688 2021-02-13T00:48:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.65 , 115.42 , 82.71 , 131.78 2021-02-13T00:48:56Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , -0.14 , -0.19 2021-02-13T00:48:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177336.66 epoch sec; seqNo:15989; 4060111.06 north; 593185.78 east 2021-02-13T00:48:57Z- AHRS_M2 msg: 1613177337.04 epoch sec; seqNo:15866 2021-02-13T00:48:57Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.23 psi 2021-02-13T00:48:57Z- handleDepth()-Depth_Keller msg: 1613177337048 epoch sec; seqNo:15990; depth 5.81 2021-02-13T00:48:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177337.06 epoch sec; seqNo:15990; 4060111.44 north; 593185.78 east 2021-02-13T00:48:57Z- AHRS_M2 msg: 1613177337.44 epoch sec; seqNo:15867 2021-02-13T00:48:57Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.20 psi 2021-02-13T00:48:57Z- handleDepth()-Depth_Keller msg: 1613177337444 epoch sec; seqNo:15991; depth 5.93 2021-02-13T00:48:57Z- handleDvl() - RDI_Pathfinder msg: 1613177337.46 epoch sec; seqNo:7689 2021-02-13T00:48:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.30 , 117.05 , 84.33 , 123.00 2021-02-13T00:48:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.22 , 0.01 2021-02-13T00:48:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177337.47 epoch sec; seqNo:15991; 4060111.81 north; 593185.77 east 2021-02-13T00:48:57Z- AHRS_M2 msg: 1613177337.84 epoch sec; seqNo:15868 2021-02-13T00:48:57Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-13T00:48:57Z- handleDepth()-Depth_Keller msg: 1613177337849 epoch sec; seqNo:15992; depth 6.06 2021-02-13T00:48:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177337.86 epoch sec; seqNo:15992; 4060112.19 north; 593185.77 east 2021-02-13T00:48:58Z- AHRS_M2 msg: 1613177338.24 epoch sec; seqNo:15869 2021-02-13T00:48:58Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.18 psi 2021-02-13T00:48:58Z- handleDepth()-Depth_Keller msg: 1613177338249 epoch sec; seqNo:15993; depth 6.18 2021-02-13T00:48:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177338.26 epoch sec; seqNo:15993; 4060112.57 north; 593185.77 east 2021-02-13T00:48:58Z- AHRS_M2 msg: 1613177338.65 epoch sec; seqNo:15870 2021-02-13T00:48:58Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.19 psi 2021-02-13T00:48:58Z- handleDepth()-Depth_Keller msg: 1613177338652 epoch sec; seqNo:15994; depth 6.29 2021-02-13T00:48:58Z- handleDvl() - RDI_Pathfinder msg: 1613177338.67 epoch sec; seqNo:7690 2021-02-13T00:48:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 108.26 , 81.49 , 123.13 2021-02-13T00:48:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , 0.00 , -0.00 2021-02-13T00:48:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177338.68 epoch sec; seqNo:15994; 4060112.57 north; 593185.77 east 2021-02-13T00:48:59Z- AHRS_M2 msg: 1613177339.05 epoch sec; seqNo:15871 2021-02-13T00:48:59Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.21 psi 2021-02-13T00:48:59Z- handleDepth()-Depth_Keller msg: 1613177339056 epoch sec; seqNo:15995; depth 6.41 2021-02-13T00:48:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177339.07 epoch sec; seqNo:15995; 4060112.96 north; 593186.98 east 2021-02-13T00:48:59Z- AHRS_M2 msg: 1613177339.45 epoch sec; seqNo:15872 2021-02-13T00:48:59Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.24 psi 2021-02-13T00:48:59Z- handleDepth()-Depth_Keller msg: 1613177339465 epoch sec; seqNo:15996; depth 6.51 2021-02-13T00:48:59Z- handleDvl() - RDI_Pathfinder msg: 1613177339.48 epoch sec; seqNo:7691 2021-02-13T00:48:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 109.75 , 81.49 , 124.62 2021-02-13T00:48:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.03 , -0.10 2021-02-13T00:48:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177339.50 epoch sec; seqNo:15996; 4060112.96 north; 593186.98 east 2021-02-13T00:48:59Z- AHRS_M2 msg: 1613177339.86 epoch sec; seqNo:15873 2021-02-13T00:48:59Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.26 psi 2021-02-13T00:48:59Z- handleDepth()-Depth_Keller msg: 1613177339865 epoch sec; seqNo:15997; depth 6.63 2021-02-13T00:48:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177339.88 epoch sec; seqNo:15997; 4060113.34 north; 593186.98 east 2021-02-13T00:49:00Z- AHRS_M2 msg: 1613177340.26 epoch sec; seqNo:15874 2021-02-13T00:49:00Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.27 psi 2021-02-13T00:49:00Z- handleDepth()-Depth_Keller msg: 1613177340271 epoch sec; seqNo:15998; depth 6.73 2021-02-13T00:49:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177340.28 epoch sec; seqNo:15998; 4060113.72 north; 593186.97 east 2021-02-13T00:49:00Z- LcmTrn::handleMessages() - handling 43 messages took 0.63 ms and 131 calls... 2021-02-13T00:49:00Z- waiting for 1613177338.41, time is 1613177340.28 2021-02-13T00:49:00Z- LcmTrn::updateTrn() >>>> pose time 1613177340.28, meas time 1613177339.48 <<<< 2021-02-13T00:49:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177339.48 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:49:00Z- TerrainNav::motionUpdate() - currEstimate = (4060114, 593186.97, 6.73) 2021-02-13T00:49:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:49:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:49:00Z- TNavBF::Projecting Measurements 2021-02-13T00:49:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:49:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:49:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:49:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:49:00Z- TNavFilter::4 good beams. 2021-02-13T00:49:00Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:49:00Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:49:00Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:49:00Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:49:00Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:49:00Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:49:00Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:49:00Z- TNavBF::mmse selected from filter -1 2021-02-13T00:49:00Z- TerrainMapOctree: (vehN, vehE ) = (4060113.09, 593186.58). 2021-02-13T00:49:00Z- TerrainMapOctree: Min Distance = 1556.74. 2021-02-13T00:49:00Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:49:00Z- There is no specific good beam that all particles have in common. 2021-02-13T00:49:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:49:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-13T00:49:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:49:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 19.27 2021-02-13T00:49:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:49:00Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 10.62 2021-02-13T00:49:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:49:00Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.033 ExpectedMeasDiff: nan currMeas.covariance: 24.85 2021-02-13T00:49:00Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:49:00Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:49:00Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:49:00Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:49:00Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:49:00Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:49:00Z- Filter 0 No beams used in subcloud update 2021-02-13T00:49:00Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:49:00Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:49:00Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 686.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:49:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177339.48 sec. 2021-02-13T00:49:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:49:00Z- TNavBF::mmse selected from filter -1 2021-02-13T00:49:00Z- variances([0], [2], and psi_berg) = 686.620380 832.991390 0.000000 2021-02-13T00:49:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:49:00Z- LcmTrn::publishEstimates() - MLE : 4060113.22 593208.47 6.73 2021-02-13T00:49:00Z- LcmTrn::publishEstimates() - MMSE : 4060113.72 593186.97 6.73 2021-02-13T00:49:00Z- LcmTrn::publishEstimates() - COVAR: 686.62 832.99 0.91 0.00 2021-02-13T00:49:00Z- AHRS_M2 msg: 1613177340.67 epoch sec; seqNo:15875 2021-02-13T00:49:00Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.26 psi 2021-02-13T00:49:00Z- handleDepth()-Depth_Keller msg: 1613177340686 epoch sec; seqNo:15999; depth 6.87 2021-02-13T00:49:00Z- handleDvl() - RDI_Pathfinder msg: 1613177340.70 epoch sec; seqNo:7692 2021-02-13T00:49:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 115.63 , 82.91 , 129.01 2021-02-13T00:49:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.18 , 0.17 2021-02-13T00:49:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177340.71 epoch sec; seqNo:15999; 4060114.10 north; 593186.97 east 2021-02-13T00:49:01Z- AHRS_M2 msg: 1613177341.07 epoch sec; seqNo:15876 2021-02-13T00:49:01Z- AHRS_M2 msg: 0.58 phi; -0.37 theta; -0.24 psi 2021-02-13T00:49:01Z- handleDepth()-Depth_Keller msg: 1613177341076 epoch sec; seqNo:16000; depth 6.99 2021-02-13T00:49:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177341.09 epoch sec; seqNo:16000; 4060114.10 north; 593186.97 east 2021-02-13T00:49:01Z- AHRS_M2 msg: 1613177341.49 epoch sec; seqNo:15877 2021-02-13T00:49:01Z- AHRS_M2 msg: 0.57 phi; -0.38 theta; -0.22 psi 2021-02-13T00:49:01Z- handleDepth()-Depth_Keller msg: 1613177341490 epoch sec; seqNo:16001; depth 7.12 2021-02-13T00:49:01Z- handleDvl() - RDI_Pathfinder msg: 1613177341.50 epoch sec; seqNo:7693 2021-02-13T00:49:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.39 , 115.15 , 82.44 , 130.02 2021-02-13T00:49:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.04 , 0.44 2021-02-13T00:49:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177341.51 epoch sec; seqNo:16001; 4060114.48 north; 593186.97 east 2021-02-13T00:49:01Z- AHRS_M2 msg: 1613177341.88 epoch sec; seqNo:15878 2021-02-13T00:49:01Z- AHRS_M2 msg: 0.57 phi; -0.38 theta; -0.21 psi 2021-02-13T00:49:01Z- handleDepth()-Depth_Keller msg: 1613177341890 epoch sec; seqNo:16002; depth 7.26 2021-02-13T00:49:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177341.91 epoch sec; seqNo:16002; 4060114.48 north; 593186.97 east 2021-02-13T00:49:02Z- AHRS_M2 msg: 1613177342.28 epoch sec; seqNo:15879 2021-02-13T00:49:02Z- AHRS_M2 msg: 0.57 phi; -0.38 theta; -0.19 psi 2021-02-13T00:49:02Z- handleDepth()-Depth_Keller msg: 1613177342288 epoch sec; seqNo:16003; depth 7.37 2021-02-13T00:49:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177342.30 epoch sec; seqNo:16003; 4060114.86 north; 593186.96 east 2021-02-13T00:49:02Z- AHRS_M2 msg: 1613177342.69 epoch sec; seqNo:15880 2021-02-13T00:49:02Z- AHRS_M2 msg: 0.56 phi; -0.38 theta; -0.18 psi 2021-02-13T00:49:02Z- handleDepth()-Depth_Keller msg: 1613177342697 epoch sec; seqNo:16004; depth 7.51 2021-02-13T00:49:02Z- handleDvl() - RDI_Pathfinder msg: 1613177342.71 epoch sec; seqNo:7694 2021-02-13T00:49:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.34 , 114.14 , 79.69 , 127.53 2021-02-13T00:49:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.31 , 0.38 2021-02-13T00:49:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177342.72 epoch sec; seqNo:16004; 4060114.87 north; 593188.18 east 2021-02-13T00:49:03Z- AHRS_M2 msg: 1613177343.10 epoch sec; seqNo:15881 2021-02-13T00:49:03Z- AHRS_M2 msg: 0.56 phi; -0.37 theta; -0.17 psi 2021-02-13T00:49:03Z- handleDepth()-Depth_Keller msg: 1613177343102 epoch sec; seqNo:16005; depth 7.65 2021-02-13T00:49:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177343.11 epoch sec; seqNo:16005; 4060115.25 north; 593188.18 east 2021-02-13T00:49:03Z- AHRS_M2 msg: 1613177343.49 epoch sec; seqNo:15882 2021-02-13T00:49:03Z- AHRS_M2 msg: 0.56 phi; -0.37 theta; -0.18 psi 2021-02-13T00:49:03Z- handleDepth()-Depth_Keller msg: 1613177343500 epoch sec; seqNo:16006; depth 7.79 2021-02-13T00:49:03Z- handleDvl() - RDI_Pathfinder msg: 1613177343.52 epoch sec; seqNo:7695 2021-02-13T00:49:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.41 , 108.26 , 81.48 , 127.59 2021-02-13T00:49:03Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , 0.44 , 0.02 2021-02-13T00:49:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177343.53 epoch sec; seqNo:16006; 4060115.25 north; 593188.18 east 2021-02-13T00:49:03Z- AHRS_M2 msg: 1613177343.90 epoch sec; seqNo:15883 2021-02-13T00:49:03Z- AHRS_M2 msg: 0.56 phi; -0.36 theta; -0.19 psi 2021-02-13T00:49:03Z- handleDepth()-Depth_Keller msg: 1613177343904 epoch sec; seqNo:16007; depth 7.94 2021-02-13T00:49:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177343.91 epoch sec; seqNo:16007; 4060115.64 north; 593189.39 east 2021-02-13T00:49:04Z- AHRS_M2 msg: 1613177344.30 epoch sec; seqNo:15884 2021-02-13T00:49:04Z- AHRS_M2 msg: 0.57 phi; -0.35 theta; -0.20 psi 2021-02-13T00:49:04Z- handleDepth()-Depth_Keller msg: 1613177344308 epoch sec; seqNo:16008; depth 8.07 2021-02-13T00:49:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177344.32 epoch sec; seqNo:16008; 4060116.02 north; 593189.39 east 2021-02-13T00:49:04Z- AHRS_M2 msg: 1613177344.71 epoch sec; seqNo:15885 2021-02-13T00:49:04Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.21 psi 2021-02-13T00:49:04Z- handleDepth()-Depth_Keller msg: 1613177344714 epoch sec; seqNo:16009; depth 8.23 2021-02-13T00:49:04Z- handleDvl() - RDI_Pathfinder msg: 1613177344.73 epoch sec; seqNo:7696 2021-02-13T00:49:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 108.32 , 80.07 , 123.20 2021-02-13T00:49:04Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , 0.26 , -0.36 2021-02-13T00:49:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177344.74 epoch sec; seqNo:16009; 4060116.40 north; 593189.39 east 2021-02-13T00:49:05Z- AHRS_M2 msg: 1613177345.11 epoch sec; seqNo:15886 2021-02-13T00:49:05Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.21 psi 2021-02-13T00:49:05Z- handleDepth()-Depth_Keller msg: 1613177345121 epoch sec; seqNo:16010; depth 8.39 2021-02-13T00:49:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177345.14 epoch sec; seqNo:16010; 4060116.78 north; 593189.38 east 2021-02-13T00:49:05Z- AHRS_M2 msg: 1613177345.52 epoch sec; seqNo:15887 2021-02-13T00:49:05Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.21 psi 2021-02-13T00:49:05Z- handleDepth()-Depth_Keller msg: 1613177345525 epoch sec; seqNo:16011; depth 8.52 2021-02-13T00:49:05Z- handleDvl() - RDI_Pathfinder msg: 1613177345.54 epoch sec; seqNo:7697 2021-02-13T00:49:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 109.20 , 80.81 , 122.72 2021-02-13T00:49:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.02 , -0.41 2021-02-13T00:49:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177345.56 epoch sec; seqNo:16011; 4060117.17 north; 593190.60 east 2021-02-13T00:49:05Z- LcmTrn::handleMessages() - handling 45 messages took 0.74 ms and 131 calls... 2021-02-13T00:49:05Z- waiting for 1613177343.28, time is 1613177345.56 2021-02-13T00:49:05Z- LcmTrn::updateTrn() equating pose 1613177345.56 and meas times 1613177345.56 2021-02-13T00:49:05Z- LcmTrn::updateTrn() >>>> pose time 1613177345.56, meas time 1613177345.56 <<<< 2021-02-13T00:49:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177345.56 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:49:05Z- TerrainNav::motionUpdate() - currEstimate = (4060117, 593190.60, 8.52) 2021-02-13T00:49:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:49:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:49:05Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177345.56 sec.; No relevant navigation data available 2021-02-13T00:49:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:49:05Z- TNavBF::mmse selected from filter -1 2021-02-13T00:49:05Z- variances([0], [2], and psi_berg) = 686.567806 832.991366 0.000000 2021-02-13T00:49:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:49:05Z- LcmTrn::publishEstimates() - MLE : 4060116.67 593212.10 8.52 2021-02-13T00:49:05Z- LcmTrn::publishEstimates() - MMSE : 4060117.17 593190.60 8.52 2021-02-13T00:49:05Z- LcmTrn::publishEstimates() - COVAR: 686.57 832.99 1.17 0.00 2021-02-13T00:49:05Z- AHRS_M2 msg: 1613177345.92 epoch sec; seqNo:15888 2021-02-13T00:49:05Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.21 psi 2021-02-13T00:49:05Z- handleDepth()-Depth_Keller msg: 1613177345924 epoch sec; seqNo:16012; depth 8.66 2021-02-13T00:49:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177345.93 epoch sec; seqNo:16012; 4060117.17 north; 593190.60 east 2021-02-13T00:49:06Z- AHRS_M2 msg: 1613177346.33 epoch sec; seqNo:15889 2021-02-13T00:49:06Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.20 psi 2021-02-13T00:49:06Z- handleDepth()-Depth_Keller msg: 1613177346337 epoch sec; seqNo:16013; depth 8.78 2021-02-13T00:49:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177346.35 epoch sec; seqNo:16013; 4060117.55 north; 593190.59 east 2021-02-13T00:49:06Z- AHRS_M2 msg: 1613177346.73 epoch sec; seqNo:15890 2021-02-13T00:49:06Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.20 psi 2021-02-13T00:49:06Z- handleDepth()-Depth_Keller msg: 1613177346732 epoch sec; seqNo:16014; depth 8.93 2021-02-13T00:49:06Z- handleDvl() - RDI_Pathfinder msg: 1613177346.75 epoch sec; seqNo:7698 2021-02-13T00:49:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.82 , 109.61 , 79.87 , 121.51 2021-02-13T00:49:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.16 , -0.25 2021-02-13T00:49:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177346.76 epoch sec; seqNo:16014; 4060117.93 north; 593190.59 east 2021-02-13T00:49:07Z- AHRS_M2 msg: 1613177347.13 epoch sec; seqNo:15891 2021-02-13T00:49:07Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.19 psi 2021-02-13T00:49:07Z- handleDepth()-Depth_Keller msg: 1613177347141 epoch sec; seqNo:16015; depth 9.07 2021-02-13T00:49:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177347.15 epoch sec; seqNo:16015; 4060118.31 north; 593190.59 east 2021-02-13T00:49:07Z- AHRS_M2 msg: 1613177347.54 epoch sec; seqNo:15892 2021-02-13T00:49:07Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.19 psi 2021-02-13T00:49:07Z- handleDepth()-Depth_Keller msg: 1613177347540 epoch sec; seqNo:16016; depth 9.19 2021-02-13T00:49:07Z- handleDvl() - RDI_Pathfinder msg: 1613177347.55 epoch sec; seqNo:7699 2021-02-13T00:49:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 116.03 , 107.92 , 76.01 , 120.09 2021-02-13T00:49:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.11 , -0.06 2021-02-13T00:49:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177347.56 epoch sec; seqNo:16016; 4060118.69 north; 593190.58 east 2021-02-13T00:49:07Z- AHRS_M2 msg: 1613177347.94 epoch sec; seqNo:15893 2021-02-13T00:49:07Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.19 psi 2021-02-13T00:49:07Z- handleDepth()-Depth_Keller msg: 1613177347950 epoch sec; seqNo:16017; depth 9.34 2021-02-13T00:49:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177347.96 epoch sec; seqNo:16017; 4060119.07 north; 593190.58 east 2021-02-13T00:49:08Z- AHRS_M2 msg: 1613177348.34 epoch sec; seqNo:15894 2021-02-13T00:49:08Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.19 psi 2021-02-13T00:49:08Z- handleDepth()-Depth_Keller msg: 1613177348348 epoch sec; seqNo:16018; depth 9.49 2021-02-13T00:49:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177348.36 epoch sec; seqNo:16018; 4060119.07 north; 593190.58 east 2021-02-13T00:49:08Z- AHRS_M2 msg: 1613177348.75 epoch sec; seqNo:15895 2021-02-13T00:49:08Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.19 psi 2021-02-13T00:49:08Z- handleDepth()-Depth_Keller msg: 1613177348761 epoch sec; seqNo:16019; depth 9.59 2021-02-13T00:49:08Z- handleDvl() - RDI_Pathfinder msg: 1613177348.77 epoch sec; seqNo:7700 2021-02-13T00:49:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 106.57 , 79.79 , 118.47 2021-02-13T00:49:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.18 , 0.04 2021-02-13T00:49:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177348.78 epoch sec; seqNo:16019; 4060119.46 north; 593191.79 east 2021-02-13T00:49:09Z- AHRS_M2 msg: 1613177349.15 epoch sec; seqNo:15896 2021-02-13T00:49:09Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.18 psi 2021-02-13T00:49:09Z- handleDepth()-Depth_Keller msg: 1613177349170 epoch sec; seqNo:16020; depth 9.75 2021-02-13T00:49:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177349.18 epoch sec; seqNo:16020; 4060119.84 north; 593191.79 east 2021-02-13T00:49:09Z- AHRS_M2 msg: 1613177349.56 epoch sec; seqNo:15897 2021-02-13T00:49:09Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.16 psi 2021-02-13T00:49:09Z- handleDepth()-Depth_Keller msg: 1613177349560 epoch sec; seqNo:16021; depth 9.88 2021-02-13T00:49:09Z- handleDvl() - RDI_Pathfinder msg: 1613177349.57 epoch sec; seqNo:7701 2021-02-13T00:49:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.08 , 103.93 , 77.65 , 115.83 2021-02-13T00:49:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.11 , 0.10 2021-02-13T00:49:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177349.60 epoch sec; seqNo:16021; 4060120.22 north; 593191.79 east 2021-02-13T00:49:09Z- AHRS_M2 msg: 1613177349.96 epoch sec; seqNo:15898 2021-02-13T00:49:09Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.15 psi 2021-02-13T00:49:09Z- handleDepth()-Depth_Keller msg: 1613177349966 epoch sec; seqNo:16022; depth 10.01 2021-02-13T00:49:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177349.98 epoch sec; seqNo:16022; 4060120.59 north; 593191.78 east 2021-02-13T00:49:10Z- AHRS_M2 msg: 1613177350.38 epoch sec; seqNo:15899 2021-02-13T00:49:10Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.14 psi 2021-02-13T00:49:10Z- handleDepth()-Depth_Keller msg: 1613177350391 epoch sec; seqNo:16023; depth 10.14 2021-02-13T00:49:10Z- handleDvl() - RDI_Pathfinder msg: 1613177350.41 epoch sec; seqNo:7702 2021-02-13T00:49:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.21 , 100.08 , 76.83 , 112.25 2021-02-13T00:49:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , 0.10 2021-02-13T00:49:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177350.42 epoch sec; seqNo:16023; 4060120.59 north; 593191.78 east 2021-02-13T00:49:10Z- LcmTrn::handleMessages() - handling 41 messages took 0.59 ms and 129 calls... 2021-02-13T00:49:10Z- waiting for 1613177348.56, time is 1613177350.42 2021-02-13T00:49:10Z- LcmTrn::updateTrn() equating pose 1613177350.42 and meas times 1613177350.42 2021-02-13T00:49:10Z- LcmTrn::updateTrn() >>>> pose time 1613177350.42, meas time 1613177350.42 <<<< 2021-02-13T00:49:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177350.42 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:49:10Z- TerrainNav::motionUpdate() - currEstimate = (4060121, 593191.78, 10.14) 2021-02-13T00:49:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:49:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:49:10Z- TNavBF::Projecting Measurements 2021-02-13T00:49:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:49:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:49:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:49:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:49:10Z- TNavFilter::4 good beams. 2021-02-13T00:49:10Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:49:10Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:49:10Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:49:10Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:49:10Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:49:10Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:49:10Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:49:10Z- TNavBF::mmse selected from filter -1 2021-02-13T00:49:10Z- TerrainMapOctree: (vehN, vehE ) = (4060120.59, 593191.78). 2021-02-13T00:49:10Z- TerrainMapOctree: Min Distance = 1547.70. 2021-02-13T00:49:10Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:49:10Z- There is no specific good beam that all particles have in common. 2021-02-13T00:49:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:49:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.35 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 11.62 2021-02-13T00:49:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:49:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 16.03 2021-02-13T00:49:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:49:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 9.44 2021-02-13T00:49:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:49:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 20.16 2021-02-13T00:49:10Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:49:10Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:49:10Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:49:10Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:49:10Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:49:10Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:49:10Z- Filter 0 No beams used in subcloud update 2021-02-13T00:49:10Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:49:10Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:49:10Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 686.5 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:49:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177350.42 sec. 2021-02-13T00:49:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:49:10Z- TNavBF::mmse selected from filter -1 2021-02-13T00:49:10Z- variances([0], [2], and psi_berg) = 686.523103 832.991346 0.000000 2021-02-13T00:49:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:49:10Z- LcmTrn::publishEstimates() - MLE : 4060120.09 593213.28 10.14 2021-02-13T00:49:10Z- LcmTrn::publishEstimates() - MMSE : 4060120.59 593191.78 10.14 2021-02-13T00:49:10Z- LcmTrn::publishEstimates() - COVAR: 686.52 832.99 0.99 0.00 2021-02-13T00:49:10Z- AHRS_M2 msg: 1613177350.77 epoch sec; seqNo:15900 2021-02-13T00:49:10Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.14 psi 2021-02-13T00:49:10Z- handleDepth()-Depth_Keller msg: 1613177350772 epoch sec; seqNo:16024; depth 10.26 2021-02-13T00:49:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177350.79 epoch sec; seqNo:16024; 4060120.97 north; 593191.78 east 2021-02-13T00:49:11Z- AHRS_M2 msg: 1613177351.17 epoch sec; seqNo:15901 2021-02-13T00:49:11Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.15 psi 2021-02-13T00:49:11Z- handleDepth()-Depth_Keller msg: 1613177351176 epoch sec; seqNo:16025; depth 10.36 2021-02-13T00:49:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177351.18 epoch sec; seqNo:16025; 4060121.35 north; 593191.77 east 2021-02-13T00:49:11Z- AHRS_M2 msg: 1613177351.58 epoch sec; seqNo:15902 2021-02-13T00:49:11Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.16 psi 2021-02-13T00:49:11Z- handleDepth()-Depth_Keller msg: 1613177351582 epoch sec; seqNo:16026; depth 10.49 2021-02-13T00:49:11Z- handleDvl() - RDI_Pathfinder msg: 1613177351.59 epoch sec; seqNo:7703 2021-02-13T00:49:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 98.93 , 77.24 , 111.10 2021-02-13T00:49:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.09 , 0.04 2021-02-13T00:49:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177351.60 epoch sec; seqNo:16026; 4060121.36 north; 593192.99 east 2021-02-13T00:49:11Z- AHRS_M2 msg: 1613177351.98 epoch sec; seqNo:15903 2021-02-13T00:49:11Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.16 psi 2021-02-13T00:49:11Z- handleDepth()-Depth_Keller msg: 1613177351984 epoch sec; seqNo:16027; depth 10.61 2021-02-13T00:49:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177352.00 epoch sec; seqNo:16027; 4060121.74 north; 593192.99 east 2021-02-13T00:49:12Z- AHRS_M2 msg: 1613177352.40 epoch sec; seqNo:15904 2021-02-13T00:49:12Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.16 psi 2021-02-13T00:49:12Z- handleDepth()-Depth_Keller msg: 1613177352418 epoch sec; seqNo:16028; depth 10.71 2021-02-13T00:49:12Z- handleDvl() - RDI_Pathfinder msg: 1613177352.43 epoch sec; seqNo:7704 2021-02-13T00:49:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 100.35 , 78.71 , 111.16 2021-02-13T00:49:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.06 , -0.03 2021-02-13T00:49:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177352.44 epoch sec; seqNo:16028; 4060122.12 north; 593192.99 east 2021-02-13T00:49:12Z- AHRS_M2 msg: 1613177352.79 epoch sec; seqNo:15905 2021-02-13T00:49:12Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.17 psi 2021-02-13T00:49:12Z- handleDepth()-Depth_Keller msg: 1613177352795 epoch sec; seqNo:16029; depth 10.82 2021-02-13T00:49:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177352.84 epoch sec; seqNo:16029; 4060122.50 north; 593192.98 east 2021-02-13T00:49:13Z- AHRS_M2 msg: 1613177353.19 epoch sec; seqNo:15906 2021-02-13T00:49:13Z- AHRS_M2 msg: 0.60 phi; -0.27 theta; -0.18 psi 2021-02-13T00:49:13Z- handleDepth()-Depth_Keller msg: 1613177353205 epoch sec; seqNo:16030; depth 10.94 2021-02-13T00:49:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177353.21 epoch sec; seqNo:16030; 4060122.50 north; 593192.98 east 2021-02-13T00:49:13Z- AHRS_M2 msg: 1613177353.60 epoch sec; seqNo:15907 2021-02-13T00:49:13Z- AHRS_M2 msg: 0.60 phi; -0.27 theta; -0.19 psi 2021-02-13T00:49:13Z- handleDepth()-Depth_Keller msg: 1613177353615 epoch sec; seqNo:16031; depth 11.03 2021-02-13T00:49:13Z- handleDvl() - RDI_Pathfinder msg: 1613177353.63 epoch sec; seqNo:7705 2021-02-13T00:49:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.70 , 101.63 , 78.64 , 108.39 2021-02-13T00:49:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.05 , -0.04 2021-02-13T00:49:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177353.64 epoch sec; seqNo:16031; 4060122.88 north; 593192.98 east 2021-02-13T00:49:14Z- AHRS_M2 msg: 1613177354.00 epoch sec; seqNo:15908 2021-02-13T00:49:14Z- AHRS_M2 msg: 0.61 phi; -0.26 theta; -0.19 psi 2021-02-13T00:49:14Z- handleDepth()-Depth_Keller msg: 1613177354005 epoch sec; seqNo:16032; depth 11.16 2021-02-13T00:49:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177354.02 epoch sec; seqNo:16032; 4060123.27 north; 593194.19 east 2021-02-13T00:49:14Z- AHRS_M2 msg: 1613177354.41 epoch sec; seqNo:15909 2021-02-13T00:49:14Z- AHRS_M2 msg: 0.61 phi; -0.26 theta; -0.19 psi 2021-02-13T00:49:14Z- handleDepth()-Depth_Keller msg: 1613177354414 epoch sec; seqNo:16033; depth 11.25 2021-02-13T00:49:14Z- handleDvl() - RDI_Pathfinder msg: 1613177354.43 epoch sec; seqNo:7706 2021-02-13T00:49:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 101.70 , 78.05 , 108.46 2021-02-13T00:49:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.12 , 0.05 2021-02-13T00:49:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177354.44 epoch sec; seqNo:16033; 4060123.65 north; 593194.19 east 2021-02-13T00:49:14Z- AHRS_M2 msg: 1613177354.81 epoch sec; seqNo:15910 2021-02-13T00:49:14Z- AHRS_M2 msg: 0.61 phi; -0.26 theta; -0.18 psi 2021-02-13T00:49:14Z- handleDepth()-Depth_Keller msg: 1613177354816 epoch sec; seqNo:16034; depth 11.35 2021-02-13T00:49:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177354.83 epoch sec; seqNo:16034; 4060124.03 north; 593194.19 east 2021-02-13T00:49:15Z- AHRS_M2 msg: 1613177355.21 epoch sec; seqNo:15911 2021-02-13T00:49:15Z- AHRS_M2 msg: 0.61 phi; -0.27 theta; -0.17 psi 2021-02-13T00:49:15Z- handleDepth()-Depth_Keller msg: 1613177355226 epoch sec; seqNo:16035; depth 11.45 2021-02-13T00:49:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177355.23 epoch sec; seqNo:16035; 4060124.03 north; 593194.19 east 2021-02-13T00:49:15Z- AHRS_M2 msg: 1613177355.62 epoch sec; seqNo:15912 2021-02-13T00:49:15Z- AHRS_M2 msg: 0.60 phi; -0.27 theta; -0.16 psi 2021-02-13T00:49:15Z- handleDepth()-Depth_Keller msg: 1613177355625 epoch sec; seqNo:16036; depth 11.55 2021-02-13T00:49:15Z- handleDvl() - RDI_Pathfinder msg: 1613177355.64 epoch sec; seqNo:7707 2021-02-13T00:49:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 101.70 , 78.72 , 107.11 2021-02-13T00:49:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , 0.15 2021-02-13T00:49:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177355.65 epoch sec; seqNo:16036; 4060124.41 north; 593194.18 east 2021-02-13T00:49:15Z- LcmTrn::handleMessages() - handling 44 messages took 0.75 ms and 129 calls... 2021-02-13T00:49:15Z- waiting for 1613177353.42, time is 1613177355.65 2021-02-13T00:49:15Z- LcmTrn::updateTrn() equating pose 1613177355.65 and meas times 1613177355.65 2021-02-13T00:49:15Z- LcmTrn::updateTrn() >>>> pose time 1613177355.65, meas time 1613177355.65 <<<< 2021-02-13T00:49:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177355.65 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:49:15Z- TerrainNav::motionUpdate() - currEstimate = (4060124, 593194.18, 11.55) 2021-02-13T00:49:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:49:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:49:15Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177355.65 sec.; No relevant navigation data available 2021-02-13T00:49:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:49:15Z- TNavBF::mmse selected from filter -1 2021-02-13T00:49:15Z- variances([0], [2], and psi_berg) = 686.473256 832.991323 0.000000 2021-02-13T00:49:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:49:15Z- LcmTrn::publishEstimates() - MLE : 4060123.91 593215.68 11.55 2021-02-13T00:49:15Z- LcmTrn::publishEstimates() - MMSE : 4060124.41 593194.18 11.55 2021-02-13T00:49:15Z- LcmTrn::publishEstimates() - COVAR: 686.47 832.99 1.22 0.00 2021-02-13T00:49:16Z- AHRS_M2 msg: 1613177356.02 epoch sec; seqNo:15913 2021-02-13T00:49:16Z- AHRS_M2 msg: 0.60 phi; -0.27 theta; -0.16 psi 2021-02-13T00:49:16Z- handleDepth()-Depth_Keller msg: 1613177356024 epoch sec; seqNo:16037; depth 11.67 2021-02-13T00:49:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177356.04 epoch sec; seqNo:16037; 4060124.79 north; 593194.18 east 2021-02-13T00:49:16Z- AHRS_M2 msg: 1613177356.43 epoch sec; seqNo:15914 2021-02-13T00:49:16Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.16 psi 2021-02-13T00:49:16Z- handleDepth()-Depth_Keller msg: 1613177356434 epoch sec; seqNo:16038; depth 11.75 2021-02-13T00:49:16Z- handleDvl() - RDI_Pathfinder msg: 1613177356.45 epoch sec; seqNo:7708 2021-02-13T00:49:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.13 , 99.00 , 77.64 , 107.11 2021-02-13T00:49:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.11 , 0.21 2021-02-13T00:49:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177356.46 epoch sec; seqNo:16038; 4060125.17 north; 593194.17 east 2021-02-13T00:49:16Z- AHRS_M2 msg: 1613177356.83 epoch sec; seqNo:15915 2021-02-13T00:49:16Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.17 psi 2021-02-13T00:49:16Z- handleDepth()-Depth_Keller msg: 1613177356832 epoch sec; seqNo:16039; depth 11.86 2021-02-13T00:49:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177356.85 epoch sec; seqNo:16039; 4060125.18 north; 593195.39 east 2021-02-13T00:49:17Z- AHRS_M2 msg: 1613177357.23 epoch sec; seqNo:15916 2021-02-13T00:49:17Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.18 psi 2021-02-13T00:49:17Z- handleDepth()-Depth_Keller msg: 1613177357237 epoch sec; seqNo:16040; depth 11.96 2021-02-13T00:49:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177357.24 epoch sec; seqNo:16040; 4060125.56 north; 593195.39 east 2021-02-13T00:49:17Z- AHRS_M2 msg: 1613177357.63 epoch sec; seqNo:15917 2021-02-13T00:49:17Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.19 psi 2021-02-13T00:49:17Z- handleDepth()-Depth_Keller msg: 1613177357641 epoch sec; seqNo:16041; depth 12.06 2021-02-13T00:49:17Z- handleDvl() - RDI_Pathfinder msg: 1613177357.65 epoch sec; seqNo:7709 2021-02-13T00:49:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 99.07 , 0.00 , 108.53 2021-02-13T00:49:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.28 , 0.11 2021-02-13T00:49:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177357.66 epoch sec; seqNo:16041; 4060125.94 north; 593195.39 east 2021-02-13T00:49:18Z- AHRS_M2 msg: 1613177358.04 epoch sec; seqNo:15918 2021-02-13T00:49:18Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.21 psi 2021-02-13T00:49:18Z- handleDepth()-Depth_Keller msg: 1613177358046 epoch sec; seqNo:16042; depth 12.16 2021-02-13T00:49:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177358.06 epoch sec; seqNo:16042; 4060125.94 north; 593195.39 east 2021-02-13T00:49:18Z- AHRS_M2 msg: 1613177358.44 epoch sec; seqNo:15919 2021-02-13T00:49:18Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.22 psi 2021-02-13T00:49:18Z- handleDepth()-Depth_Keller msg: 1613177358448 epoch sec; seqNo:16043; depth 12.25 2021-02-13T00:49:18Z- handleDvl() - RDI_Pathfinder msg: 1613177358.47 epoch sec; seqNo:7710 2021-02-13T00:49:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.37 , 101.30 , 77.24 , 109.41 2021-02-13T00:49:18Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.27 , -0.15 2021-02-13T00:49:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177358.48 epoch sec; seqNo:16043; 4060126.33 north; 593196.60 east 2021-02-13T00:49:18Z- AHRS_M2 msg: 1613177358.85 epoch sec; seqNo:15920 2021-02-13T00:49:18Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.22 psi 2021-02-13T00:49:18Z- handleDepth()-Depth_Keller msg: 1613177358857 epoch sec; seqNo:16044; depth 12.36 2021-02-13T00:49:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177358.87 epoch sec; seqNo:16044; 4060126.71 north; 593196.60 east 2021-02-13T00:49:19Z- AHRS_M2 msg: 1613177359.27 epoch sec; seqNo:15921 2021-02-13T00:49:19Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.23 psi 2021-02-13T00:49:19Z- handleDepth()-Depth_Keller msg: 1613177359276 epoch sec; seqNo:16045; depth 12.47 2021-02-13T00:49:19Z- handleDvl() - RDI_Pathfinder msg: 1613177359.29 epoch sec; seqNo:7711 2021-02-13T00:49:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.46 , 104.41 , 0.00 , 107.11 2021-02-13T00:49:19Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.06 , -0.33 2021-02-13T00:49:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177359.30 epoch sec; seqNo:16045; 4060127.09 north; 593196.59 east 2021-02-13T00:49:19Z- AHRS_M2 msg: 1613177359.65 epoch sec; seqNo:15922 2021-02-13T00:49:19Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.22 psi 2021-02-13T00:49:19Z- handleDepth()-Depth_Keller msg: 1613177359670 epoch sec; seqNo:16046; depth 12.59 2021-02-13T00:49:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177359.68 epoch sec; seqNo:16046; 4060127.47 north; 593196.59 east 2021-02-13T00:49:20Z- AHRS_M2 msg: 1613177360.06 epoch sec; seqNo:15923 2021-02-13T00:49:20Z- AHRS_M2 msg: 0.61 phi; -0.28 theta; -0.21 psi 2021-02-13T00:49:20Z- handleDepth()-Depth_Keller msg: 1613177360064 epoch sec; seqNo:16047; depth 12.68 2021-02-13T00:49:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177360.07 epoch sec; seqNo:16047; 4060127.85 north; 593196.59 east 2021-02-13T00:49:20Z- AHRS_M2 msg: 1613177360.47 epoch sec; seqNo:15924 2021-02-13T00:49:20Z- AHRS_M2 msg: 0.61 phi; -0.28 theta; -0.21 psi 2021-02-13T00:49:20Z- handleDepth()-Depth_Keller msg: 1613177360471 epoch sec; seqNo:16048; depth 12.80 2021-02-13T00:49:20Z- handleDvl() - RDI_Pathfinder msg: 1613177360.48 epoch sec; seqNo:7712 2021-02-13T00:49:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.29 , 102.71 , 0.00 , 108.12 2021-02-13T00:49:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.25 , -0.29 2021-02-13T00:49:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177360.49 epoch sec; seqNo:16048; 4060128.24 north; 593197.80 east 2021-02-13T00:49:20Z- LcmTrn::handleMessages() - handling 41 messages took 0.67 ms and 128 calls... 2021-02-13T00:49:20Z- waiting for 1613177358.65, time is 1613177360.49 2021-02-13T00:49:20Z- LcmTrn::updateTrn() equating pose 1613177360.49 and meas times 1613177360.49 2021-02-13T00:49:20Z- LcmTrn::updateTrn() >>>> pose time 1613177360.49, meas time 1613177360.49 <<<< 2021-02-13T00:49:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177360.49 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:49:20Z- TerrainNav::motionUpdate() - currEstimate = (4060128, 593197.80, 12.80) 2021-02-13T00:49:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:49:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:49:20Z- TNavBF::Projecting Measurements 2021-02-13T00:49:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:49:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:49:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:49:20Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:49:20Z- TNavFilter::3 good beams. 2021-02-13T00:49:20Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:49:20Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:49:20Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:49:20Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:49:20Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:49:20Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:49:20Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:49:20Z- TNavBF::mmse selected from filter -1 2021-02-13T00:49:20Z- TerrainMapOctree: (vehN, vehE ) = (4060128.24, 593197.80). 2021-02-13T00:49:20Z- TerrainMapOctree: Min Distance = 1538.01. 2021-02-13T00:49:20Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:49:20Z- There is no specific good beam that all particles have in common. 2021-02-13T00:49:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:49:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.49 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 10.06 2021-02-13T00:49:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:49:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.32 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 16.88 2021-02-13T00:49:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:49:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 18.70 2021-02-13T00:49:20Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:49:20Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:49:20Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:49:20Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:49:20Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:49:20Z- Filter 0 No beams used in subcloud update 2021-02-13T00:49:20Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:49:20Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:49:20Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 686.4 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:49:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177360.49 sec. 2021-02-13T00:49:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:49:20Z- TNavBF::mmse selected from filter -1 2021-02-13T00:49:20Z- variances([0], [2], and psi_berg) = 686.419388 832.991299 0.000000 2021-02-13T00:49:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:49:20Z- LcmTrn::publishEstimates() - MLE : 4060127.74 593219.30 12.80 2021-02-13T00:49:20Z- LcmTrn::publishEstimates() - MMSE : 4060128.24 593197.80 12.80 2021-02-13T00:49:20Z- LcmTrn::publishEstimates() - COVAR: 686.42 832.99 1.17 0.00 2021-02-13T00:49:20Z- AHRS_M2 msg: 1613177360.87 epoch sec; seqNo:15925 2021-02-13T00:49:20Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.20 psi 2021-02-13T00:49:20Z- handleDepth()-Depth_Keller msg: 1613177360874 epoch sec; seqNo:16049; depth 12.90 2021-02-13T00:49:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177360.88 epoch sec; seqNo:16049; 4060128.62 north; 593197.80 east 2021-02-13T00:49:21Z- AHRS_M2 msg: 1613177361.27 epoch sec; seqNo:15926 2021-02-13T00:49:21Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.19 psi 2021-02-13T00:49:21Z- handleDepth()-Depth_Keller msg: 1613177361286 epoch sec; seqNo:16050; depth 13.01 2021-02-13T00:49:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177361.30 epoch sec; seqNo:16050; 4060128.99 north; 593197.79 east 2021-02-13T00:49:21Z- AHRS_M2 msg: 1613177361.68 epoch sec; seqNo:15927 2021-02-13T00:49:21Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.18 psi 2021-02-13T00:49:21Z- handleDepth()-Depth_Keller msg: 1613177361680 epoch sec; seqNo:16051; depth 13.11 2021-02-13T00:49:21Z- handleDvl() - RDI_Pathfinder msg: 1613177361.69 epoch sec; seqNo:7713 2021-02-13T00:49:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.08 , 101.36 , 75.20 , 110.83 2021-02-13T00:49:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.32 , -0.07 2021-02-13T00:49:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177361.71 epoch sec; seqNo:16051; 4060129.37 north; 593197.79 east 2021-02-13T00:49:22Z- AHRS_M2 msg: 1613177362.09 epoch sec; seqNo:15928 2021-02-13T00:49:22Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.17 psi 2021-02-13T00:49:22Z- handleDepth()-Depth_Keller msg: 1613177362090 epoch sec; seqNo:16052; depth 13.22 2021-02-13T00:49:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177362.10 epoch sec; seqNo:16052; 4060129.75 north; 593197.79 east 2021-02-13T00:49:22Z- AHRS_M2 msg: 1613177362.49 epoch sec; seqNo:15929 2021-02-13T00:49:22Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.16 psi 2021-02-13T00:49:22Z- handleDepth()-Depth_Keller msg: 1613177362493 epoch sec; seqNo:16053; depth 13.33 2021-02-13T00:49:22Z- handleDvl() - RDI_Pathfinder msg: 1613177362.51 epoch sec; seqNo:7714 2021-02-13T00:49:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.48 , 97.71 , 78.78 , 105.82 2021-02-13T00:49:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.21 , 0.16 2021-02-13T00:49:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177362.52 epoch sec; seqNo:16053; 4060129.75 north; 593197.79 east 2021-02-13T00:49:22Z- AHRS_M2 msg: 1613177362.89 epoch sec; seqNo:15930 2021-02-13T00:49:22Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.16 psi 2021-02-13T00:49:22Z- handleDepth()-Depth_Keller msg: 1613177362892 epoch sec; seqNo:16054; depth 13.43 2021-02-13T00:49:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177362.91 epoch sec; seqNo:16054; 4060130.13 north; 593197.78 east 2021-02-13T00:49:23Z- AHRS_M2 msg: 1613177363.29 epoch sec; seqNo:15931 2021-02-13T00:49:23Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.15 psi 2021-02-13T00:49:23Z- handleDepth()-Depth_Keller msg: 1613177363296 epoch sec; seqNo:16055; depth 13.53 2021-02-13T00:49:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177363.31 epoch sec; seqNo:16055; 4060130.51 north; 593197.78 east 2021-02-13T00:49:23Z- AHRS_M2 msg: 1613177363.69 epoch sec; seqNo:15932 2021-02-13T00:49:23Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.14 psi 2021-02-13T00:49:23Z- handleDepth()-Depth_Keller msg: 1613177363699 epoch sec; seqNo:16056; depth 13.65 2021-02-13T00:49:23Z- handleDvl() - RDI_Pathfinder msg: 1613177363.71 epoch sec; seqNo:7715 2021-02-13T00:49:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 97.64 , 73.56 , 111.03 2021-02-13T00:49:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.02 , 0.21 2021-02-13T00:49:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177363.72 epoch sec; seqNo:16056; 4060130.51 north; 593197.78 east 2021-02-13T00:49:24Z- AHRS_M2 msg: 1613177364.10 epoch sec; seqNo:15933 2021-02-13T00:49:24Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.15 psi 2021-02-13T00:49:24Z- handleDepth()-Depth_Keller msg: 1613177364105 epoch sec; seqNo:16057; depth 13.74 2021-02-13T00:49:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177364.12 epoch sec; seqNo:16057; 4060130.89 north; 593197.77 east 2021-02-13T00:49:24Z- AHRS_M2 msg: 1613177364.50 epoch sec; seqNo:15934 2021-02-13T00:49:24Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.15 psi 2021-02-13T00:49:24Z- handleDepth()-Depth_Keller msg: 1613177364508 epoch sec; seqNo:16058; depth 13.84 2021-02-13T00:49:24Z- handleDvl() - RDI_Pathfinder msg: 1613177364.53 epoch sec; seqNo:7716 2021-02-13T00:49:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 97.65 , 72.74 , 112.25 2021-02-13T00:49:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.10 , 0.15 2021-02-13T00:49:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177364.54 epoch sec; seqNo:16058; 4060130.90 north; 593198.99 east 2021-02-13T00:49:24Z- AHRS_M2 msg: 1613177364.91 epoch sec; seqNo:15935 2021-02-13T00:49:24Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.16 psi 2021-02-13T00:49:24Z- handleDepth()-Depth_Keller msg: 1613177364917 epoch sec; seqNo:16059; depth 13.97 2021-02-13T00:49:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177364.93 epoch sec; seqNo:16059; 4060131.28 north; 593198.99 east 2021-02-13T00:49:25Z- AHRS_M2 msg: 1613177365.31 epoch sec; seqNo:15936 2021-02-13T00:49:25Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.17 psi 2021-02-13T00:49:25Z- handleDepth()-Depth_Keller msg: 1613177365318 epoch sec; seqNo:16060; depth 14.09 2021-02-13T00:49:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177365.33 epoch sec; seqNo:16060; 4060131.66 north; 593198.99 east 2021-02-13T00:49:25Z- AHRS_M2 msg: 1613177365.72 epoch sec; seqNo:15937 2021-02-13T00:49:25Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-13T00:49:25Z- handleDepth()-Depth_Keller msg: 1613177365721 epoch sec; seqNo:16061; depth 14.19 2021-02-13T00:49:25Z- handleDvl() - RDI_Pathfinder msg: 1613177365.73 epoch sec; seqNo:7717 2021-02-13T00:49:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.04 , 97.65 , 78.18 , 109.81 2021-02-13T00:49:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.13 , 0.06 2021-02-13T00:49:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177365.75 epoch sec; seqNo:16061; 4060131.66 north; 593198.99 east 2021-02-13T00:49:25Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 131 calls... 2021-02-13T00:49:25Z- waiting for 1613177363.49, time is 1613177365.75 2021-02-13T00:49:25Z- LcmTrn::updateTrn() equating pose 1613177365.75 and meas times 1613177365.75 2021-02-13T00:49:25Z- LcmTrn::updateTrn() >>>> pose time 1613177365.75, meas time 1613177365.75 <<<< 2021-02-13T00:49:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177365.75 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:49:25Z- TerrainNav::motionUpdate() - currEstimate = (4060132, 593198.99, 14.19) 2021-02-13T00:49:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:49:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:49:25Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177365.75 sec.; No relevant navigation data available 2021-02-13T00:49:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:49:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:49:25Z- variances([0], [2], and psi_berg) = 686.374753 832.991279 0.000000 2021-02-13T00:49:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:49:25Z- LcmTrn::publishEstimates() - MLE : 4060131.16 593220.49 14.19 2021-02-13T00:49:25Z- LcmTrn::publishEstimates() - MMSE : 4060131.66 593198.99 14.19 2021-02-13T00:49:25Z- LcmTrn::publishEstimates() - COVAR: 686.37 832.99 1.35 0.00 2021-02-13T00:49:26Z- AHRS_M2 msg: 1613177366.12 epoch sec; seqNo:15938 2021-02-13T00:49:26Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-13T00:49:26Z- handleDepth()-Depth_Keller msg: 1613177366126 epoch sec; seqNo:16062; depth 14.32 2021-02-13T00:49:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177366.14 epoch sec; seqNo:16062; 4060132.04 north; 593198.98 east 2021-02-13T00:49:26Z- AHRS_M2 msg: 1613177366.53 epoch sec; seqNo:15939 2021-02-13T00:49:26Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-13T00:49:26Z- handleDepth()-Depth_Keller msg: 1613177366529 epoch sec; seqNo:16063; depth 14.44 2021-02-13T00:49:26Z- handleDvl() - RDI_Pathfinder msg: 1613177366.54 epoch sec; seqNo:7718 2021-02-13T00:49:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.48 , 99.06 , 73.15 , 111.23 2021-02-13T00:49:26Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.10 , 0.00 2021-02-13T00:49:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177366.55 epoch sec; seqNo:16063; 4060132.43 north; 593200.20 east 2021-02-13T00:49:26Z- AHRS_M2 msg: 1613177366.93 epoch sec; seqNo:15940 2021-02-13T00:49:26Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-13T00:49:26Z- handleDepth()-Depth_Keller msg: 1613177366942 epoch sec; seqNo:16064; depth 14.56 2021-02-13T00:49:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177366.95 epoch sec; seqNo:16064; 4060132.43 north; 593200.20 east 2021-02-13T00:49:27Z- AHRS_M2 msg: 1613177367.34 epoch sec; seqNo:15941 2021-02-13T00:49:27Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-13T00:49:27Z- handleDepth()-Depth_Keller msg: 1613177367354 epoch sec; seqNo:16065; depth 14.68 2021-02-13T00:49:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177367.38 epoch sec; seqNo:16065; 4060132.81 north; 593200.19 east 2021-02-13T00:49:27Z- AHRS_M2 msg: 1613177367.74 epoch sec; seqNo:15942 2021-02-13T00:49:27Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-13T00:49:27Z- handleDepth()-Depth_Keller msg: 1613177367743 epoch sec; seqNo:16066; depth 14.82 2021-02-13T00:49:27Z- handleDvl() - RDI_Pathfinder msg: 1613177367.76 epoch sec; seqNo:7719 2021-02-13T00:49:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 100.08 , 73.97 , 109.81 2021-02-13T00:49:27Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.04 , -0.04 2021-02-13T00:49:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177367.77 epoch sec; seqNo:16066; 4060133.19 north; 593200.19 east 2021-02-13T00:49:28Z- AHRS_M2 msg: 1613177368.15 epoch sec; seqNo:15943 2021-02-13T00:49:28Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.19 psi 2021-02-13T00:49:28Z- handleDepth()-Depth_Keller msg: 1613177368152 epoch sec; seqNo:16067; depth 14.92 2021-02-13T00:49:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177368.17 epoch sec; seqNo:16067; 4060133.57 north; 593200.19 east 2021-02-13T00:49:28Z- AHRS_M2 msg: 1613177368.54 epoch sec; seqNo:15944 2021-02-13T00:49:28Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.19 psi 2021-02-13T00:49:28Z- handleDepth()-Depth_Keller msg: 1613177368553 epoch sec; seqNo:16068; depth 15.06 2021-02-13T00:49:28Z- handleDvl() - RDI_Pathfinder msg: 1613177368.57 epoch sec; seqNo:7720 2021-02-13T00:49:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 98.86 , 73.56 , 108.60 2021-02-13T00:49:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.04 , -0.09 2021-02-13T00:49:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177368.58 epoch sec; seqNo:16068; 4060133.96 north; 593201.40 east 2021-02-13T00:49:28Z- AHRS_M2 msg: 1613177368.95 epoch sec; seqNo:15945 2021-02-13T00:49:28Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.19 psi 2021-02-13T00:49:28Z- handleDepth()-Depth_Keller msg: 1613177368957 epoch sec; seqNo:16069; depth 15.17 2021-02-13T00:49:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177368.97 epoch sec; seqNo:16069; 4060134.34 north; 593201.40 east 2021-02-13T00:49:29Z- AHRS_M2 msg: 1613177369.35 epoch sec; seqNo:15946 2021-02-13T00:49:29Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.20 psi 2021-02-13T00:49:29Z- handleDepth()-Depth_Keller msg: 1613177369356 epoch sec; seqNo:16070; depth 15.28 2021-02-13T00:49:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:49:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:49:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:49:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:49:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:49:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:49:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:49:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:49:29Z- handleDvl() - RDI_Pathfinder msg: 1613177369.36 epoch sec; seqNo:7721 2021-02-13T00:49:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:49:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.04 , -0.09 2021-02-13T00:49:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177369.37 epoch sec; seqNo:16070; 4060134.34 north; 593201.40 east 2021-02-13T00:49:29Z- AHRS_M2 msg: 1613177369.76 epoch sec; seqNo:15947 2021-02-13T00:49:29Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.20 psi 2021-02-13T00:49:29Z- handleDepth()-Depth_Keller msg: 1613177369762 epoch sec; seqNo:16071; depth 15.41 2021-02-13T00:49:29Z- handleDvl() - RDI_Pathfinder msg: 1613177369.77 epoch sec; seqNo:7722 2021-02-13T00:49:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.46 , 98.93 , 78.25 , 109.88 2021-02-13T00:49:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.13 , -0.08 2021-02-13T00:49:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177369.79 epoch sec; seqNo:16071; 4060134.72 north; 593201.39 east 2021-02-13T00:49:30Z- AHRS_M2 msg: 1613177370.16 epoch sec; seqNo:15948 2021-02-13T00:49:30Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.20 psi 2021-02-13T00:49:30Z- handleDepth()-Depth_Keller msg: 1613177370169 epoch sec; seqNo:16072; depth 15.52 2021-02-13T00:49:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177370.18 epoch sec; seqNo:16072; 4060135.10 north; 593201.39 east 2021-02-13T00:49:30Z- AHRS_M2 msg: 1613177370.56 epoch sec; seqNo:15949 2021-02-13T00:49:30Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.20 psi 2021-02-13T00:49:30Z- handleDepth()-Depth_Keller msg: 1613177370569 epoch sec; seqNo:16073; depth 15.65 2021-02-13T00:49:30Z- handleDvl() - RDI_Pathfinder msg: 1613177370.58 epoch sec; seqNo:7723 2021-02-13T00:49:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.64 , 98.86 , 71.51 , 107.38 2021-02-13T00:49:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.16 , -0.01 2021-02-13T00:49:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177370.60 epoch sec; seqNo:16073; 4060135.48 north; 593201.39 east 2021-02-13T00:49:30Z- LcmTrn::handleMessages() - handling 42 messages took 0.63 ms and 130 calls... 2021-02-13T00:49:30Z- waiting for 1613177368.75, time is 1613177370.60 2021-02-13T00:49:30Z- LcmTrn::updateTrn() equating pose 1613177370.60 and meas times 1613177370.60 2021-02-13T00:49:30Z- LcmTrn::updateTrn() >>>> pose time 1613177370.60, meas time 1613177370.60 <<<< 2021-02-13T00:49:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177370.60 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:49:30Z- TerrainNav::motionUpdate() - currEstimate = (4060135, 593201.39, 15.65) 2021-02-13T00:49:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:49:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:49:30Z- TNavBF::Projecting Measurements 2021-02-13T00:49:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:49:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:49:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:49:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:49:30Z- TNavFilter::4 good beams. 2021-02-13T00:49:30Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:49:30Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:49:30Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:49:30Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:49:30Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:49:30Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:49:30Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:49:30Z- TNavBF::mmse selected from filter -1 2021-02-13T00:49:30Z- TerrainMapOctree: (vehN, vehE ) = (4060135.48, 593201.39). 2021-02-13T00:49:30Z- TerrainMapOctree: Min Distance = 1530.28. 2021-02-13T00:49:30Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:49:30Z- There is no specific good beam that all particles have in common. 2021-02-13T00:49:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:49:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.58 K: 0.122 ExpectedMeasDiff: nan currMeas.covariance: 9.65 2021-02-13T00:49:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:49:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.39 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 15.64 2021-02-13T00:49:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:49:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.115 ExpectedMeasDiff: nan currMeas.covariance: 8.18 2021-02-13T00:49:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:49:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 18.45 2021-02-13T00:49:30Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:49:30Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:49:31Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:49:31Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:49:31Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:49:31Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:49:31Z- Filter 0 No beams used in subcloud update 2021-02-13T00:49:31Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:49:31Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:49:31Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 686.3 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:49:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177370.60 sec. 2021-02-13T00:49:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:49:31Z- TNavBF::mmse selected from filter -1 2021-02-13T00:49:31Z- variances([0], [2], and psi_berg) = 686.324983 832.991256 0.000000 2021-02-13T00:49:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:49:31Z- LcmTrn::publishEstimates() - MLE : 4060134.98 593222.89 15.65 2021-02-13T00:49:31Z- LcmTrn::publishEstimates() - MMSE : 4060135.48 593201.39 15.65 2021-02-13T00:49:31Z- LcmTrn::publishEstimates() - COVAR: 686.32 832.99 1.08 0.00 2021-02-13T00:49:31Z- AHRS_M2 msg: 1613177370.97 epoch sec; seqNo:15950 2021-02-13T00:49:31Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.21 psi 2021-02-13T00:49:31Z- handleDepth()-Depth_Keller msg: 1613177370983 epoch sec; seqNo:16074; depth 15.78 2021-02-13T00:49:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177370.99 epoch sec; seqNo:16074; 4060135.85 north; 593201.38 east 2021-02-13T00:49:31Z- AHRS_M2 msg: 1613177371.38 epoch sec; seqNo:15951 2021-02-13T00:49:31Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.21 psi 2021-02-13T00:49:31Z- handleDepth()-Depth_Keller msg: 1613177371383 epoch sec; seqNo:16075; depth 15.90 2021-02-13T00:49:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177371.41 epoch sec; seqNo:16075; 4060136.25 north; 593202.60 east 2021-02-13T00:49:31Z- AHRS_M2 msg: 1613177371.78 epoch sec; seqNo:15952 2021-02-13T00:49:31Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.21 psi 2021-02-13T00:49:31Z- handleDepth()-Depth_Keller msg: 1613177371784 epoch sec; seqNo:16076; depth 16.02 2021-02-13T00:49:31Z- handleDvl() - RDI_Pathfinder msg: 1613177371.80 epoch sec; seqNo:7724 2021-02-13T00:49:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.60 , 100.28 , 78.38 , 110.02 2021-02-13T00:49:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.14 , 0.10 2021-02-13T00:49:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177371.82 epoch sec; seqNo:16076; 4060136.62 north; 593202.59 east 2021-02-13T00:49:32Z- AHRS_M2 msg: 1613177372.18 epoch sec; seqNo:15953 2021-02-13T00:49:32Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-13T00:49:32Z- handleDepth()-Depth_Keller msg: 1613177372188 epoch sec; seqNo:16077; depth 16.15 2021-02-13T00:49:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177372.20 epoch sec; seqNo:16077; 4060136.62 north; 593202.59 east 2021-02-13T00:49:32Z- AHRS_M2 msg: 1613177372.59 epoch sec; seqNo:15954 2021-02-13T00:49:32Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-13T00:49:32Z- handleDepth()-Depth_Keller msg: 1613177372590 epoch sec; seqNo:16078; depth 16.27 2021-02-13T00:49:32Z- handleDvl() - RDI_Pathfinder msg: 1613177372.60 epoch sec; seqNo:7725 2021-02-13T00:49:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.78 , 99.06 , 70.70 , 109.88 2021-02-13T00:49:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , 0.16 2021-02-13T00:49:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177372.61 epoch sec; seqNo:16078; 4060137.00 north; 593202.59 east 2021-02-13T00:49:32Z- AHRS_M2 msg: 1613177372.99 epoch sec; seqNo:15955 2021-02-13T00:49:32Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-13T00:49:33Z- handleDepth()-Depth_Keller msg: 1613177373003 epoch sec; seqNo:16079; depth 16.39 2021-02-13T00:49:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177373.01 epoch sec; seqNo:16079; 4060137.38 north; 593202.58 east 2021-02-13T00:49:33Z- AHRS_M2 msg: 1613177373.40 epoch sec; seqNo:15956 2021-02-13T00:49:33Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.20 psi 2021-02-13T00:49:33Z- handleDepth()-Depth_Keller msg: 1613177373406 epoch sec; seqNo:16080; depth 16.52 2021-02-13T00:49:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177373.45 epoch sec; seqNo:16080; 4060137.38 north; 593202.58 east 2021-02-13T00:49:33Z- AHRS_M2 msg: 1613177373.80 epoch sec; seqNo:15957 2021-02-13T00:49:33Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-13T00:49:33Z- handleDepth()-Depth_Keller msg: 1613177373800 epoch sec; seqNo:16081; depth 16.65 2021-02-13T00:49:33Z- handleDvl() - RDI_Pathfinder msg: 1613177373.81 epoch sec; seqNo:7726 2021-02-13T00:49:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 99.00 , 78.31 , 112.38 2021-02-13T00:49:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.08 , 0.15 2021-02-13T00:49:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177373.82 epoch sec; seqNo:16081; 4060137.76 north; 593202.58 east 2021-02-13T00:49:34Z- AHRS_M2 msg: 1613177374.21 epoch sec; seqNo:15958 2021-02-13T00:49:34Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.20 psi 2021-02-13T00:49:34Z- handleDepth()-Depth_Keller msg: 1613177374213 epoch sec; seqNo:16082; depth 16.77 2021-02-13T00:49:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177374.22 epoch sec; seqNo:16082; 4060137.77 north; 593203.80 east 2021-02-13T00:49:34Z- AHRS_M2 msg: 1613177374.60 epoch sec; seqNo:15959 2021-02-13T00:49:34Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.20 psi 2021-02-13T00:49:34Z- handleDepth()-Depth_Keller msg: 1613177374618 epoch sec; seqNo:16083; depth 16.90 2021-02-13T00:49:34Z- handleDvl() - RDI_Pathfinder msg: 1613177374.63 epoch sec; seqNo:7727 2021-02-13T00:49:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.07 , 97.64 , 70.29 , 113.87 2021-02-13T00:49:34Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.17 , 0.06 2021-02-13T00:49:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177374.64 epoch sec; seqNo:16083; 4060138.15 north; 593203.80 east 2021-02-13T00:49:35Z- AHRS_M2 msg: 1613177375.01 epoch sec; seqNo:15960 2021-02-13T00:49:35Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.20 psi 2021-02-13T00:49:35Z- handleDepth()-Depth_Keller msg: 1613177375017 epoch sec; seqNo:16084; depth 17.03 2021-02-13T00:49:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177375.03 epoch sec; seqNo:16084; 4060138.53 north; 593203.79 east 2021-02-13T00:49:35Z- AHRS_M2 msg: 1613177375.41 epoch sec; seqNo:15961 2021-02-13T00:49:35Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.21 psi 2021-02-13T00:49:35Z- handleDepth()-Depth_Keller msg: 1613177375425 epoch sec; seqNo:16085; depth 17.15 2021-02-13T00:49:35Z- handleDvl() - RDI_Pathfinder msg: 1613177375.44 epoch sec; seqNo:7728 2021-02-13T00:49:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.47 , 97.71 , 71.52 , 111.10 2021-02-13T00:49:35Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , 0.12 , -0.05 2021-02-13T00:49:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177375.45 epoch sec; seqNo:16085; 4060138.91 north; 593203.79 east 2021-02-13T00:49:35Z- AHRS_M2 msg: 1613177375.82 epoch sec; seqNo:15962 2021-02-13T00:49:35Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.21 psi 2021-02-13T00:49:35Z- handleDepth()-Depth_Keller msg: 1613177375821 epoch sec; seqNo:16086; depth 17.29 2021-02-13T00:49:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177375.84 epoch sec; seqNo:16086; 4060138.91 north; 593203.79 east 2021-02-13T00:49:36Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:49:36Z- waiting for 1613177373.60, time is 1613177375.84 2021-02-13T00:49:36Z- LcmTrn::updateTrn() >>>> pose time 1613177375.84, meas time 1613177375.44 <<<< 2021-02-13T00:49:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177375.44 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:49:36Z- TerrainNav::motionUpdate() - currEstimate = (4060139, 593203.79, 17.29) 2021-02-13T00:49:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:49:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:49:36Z- TNavBF::Projecting Measurements 2021-02-13T00:49:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:49:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:49:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:49:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:49:36Z- TNavFilter::4 good beams. 2021-02-13T00:49:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:49:36Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:49:36Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:49:36Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:49:36Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:49:36Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:49:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:49:36Z- TNavBF::mmse selected from filter -1 2021-02-13T00:49:36Z- TerrainMapOctree: (vehN, vehE ) = (4060138.65, 593203.61). 2021-02-13T00:49:36Z- TerrainMapOctree: Min Distance = 1526.45. 2021-02-13T00:49:36Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:49:36Z- There is no specific good beam that all particles have in common. 2021-02-13T00:49:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:49:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T00:49:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:49:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:49:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:49:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 8.18 2021-02-13T00:49:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:49:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 19.75 2021-02-13T00:49:36Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:49:36Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:49:36Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:49:36Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:49:36Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:49:36Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:49:36Z- Filter 0 No beams used in subcloud update 2021-02-13T00:49:36Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:49:36Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:49:36Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 686.3 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:49:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177375.44 sec. 2021-02-13T00:49:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:49:36Z- TNavBF::mmse selected from filter -1 2021-02-13T00:49:36Z- variances([0], [2], and psi_berg) = 686.265609 832.991229 0.000000 2021-02-13T00:49:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:49:36Z- LcmTrn::publishEstimates() - MLE : 4060138.41 593225.29 17.29 2021-02-13T00:49:36Z- LcmTrn::publishEstimates() - MMSE : 4060138.91 593203.79 17.29 2021-02-13T00:49:36Z- LcmTrn::publishEstimates() - COVAR: 686.27 832.99 0.94 0.00 2021-02-13T00:49:36Z- AHRS_M2 msg: 1613177376.22 epoch sec; seqNo:15963 2021-02-13T00:49:36Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.21 psi 2021-02-13T00:49:36Z- handleDepth()-Depth_Keller msg: 1613177376229 epoch sec; seqNo:16087; depth 17.42 2021-02-13T00:49:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177376.24 epoch sec; seqNo:16087; 4060139.30 north; 593205.01 east 2021-02-13T00:49:36Z- AHRS_M2 msg: 1613177376.62 epoch sec; seqNo:15964 2021-02-13T00:49:36Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.21 psi 2021-02-13T00:49:36Z- handleDepth()-Depth_Keller msg: 1613177376628 epoch sec; seqNo:16088; depth 17.56 2021-02-13T00:49:36Z- handleDvl() - RDI_Pathfinder msg: 1613177376.64 epoch sec; seqNo:7729 2021-02-13T00:49:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 99.00 , 71.52 , 109.95 2021-02-13T00:49:36Z- handleDvl() - RDI_Pathfinder msg: velocities 1.08 , 0.03 , -0.15 2021-02-13T00:49:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177376.65 epoch sec; seqNo:16088; 4060139.68 north; 593205.00 east 2021-02-13T00:49:37Z- AHRS_M2 msg: 1613177377.03 epoch sec; seqNo:15965 2021-02-13T00:49:37Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.21 psi 2021-02-13T00:49:37Z- handleDepth()-Depth_Keller msg: 1613177377035 epoch sec; seqNo:16089; depth 17.71 2021-02-13T00:49:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177377.05 epoch sec; seqNo:16089; 4060140.06 north; 593205.00 east 2021-02-13T00:49:37Z- AHRS_M2 msg: 1613177377.43 epoch sec; seqNo:15966 2021-02-13T00:49:37Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-13T00:49:37Z- handleDepth()-Depth_Keller msg: 1613177377436 epoch sec; seqNo:16090; depth 17.83 2021-02-13T00:49:37Z- handleDvl() - RDI_Pathfinder msg: 1613177377.45 epoch sec; seqNo:7730 2021-02-13T00:49:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 99.06 , 78.37 , 111.23 2021-02-13T00:49:37Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.08 , -0.20 2021-02-13T00:49:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177377.46 epoch sec; seqNo:16090; 4060140.44 north; 593204.99 east 2021-02-13T00:49:37Z- AHRS_M2 msg: 1613177377.84 epoch sec; seqNo:15967 2021-02-13T00:49:37Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-13T00:49:37Z- handleDepth()-Depth_Keller msg: 1613177377851 epoch sec; seqNo:16091; depth 17.96 2021-02-13T00:49:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177377.88 epoch sec; seqNo:16091; 4060140.82 north; 593204.99 east 2021-02-13T00:49:38Z- AHRS_M2 msg: 1613177378.24 epoch sec; seqNo:15968 2021-02-13T00:49:38Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-13T00:49:38Z- handleDepth()-Depth_Keller msg: 1613177378246 epoch sec; seqNo:16092; depth 18.10 2021-02-13T00:49:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177378.26 epoch sec; seqNo:16092; 4060141.21 north; 593206.21 east 2021-02-13T00:49:38Z- AHRS_M2 msg: 1613177378.64 epoch sec; seqNo:15969 2021-02-13T00:49:38Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-13T00:49:38Z- handleDepth()-Depth_Keller msg: 1613177378648 epoch sec; seqNo:16093; depth 18.21 2021-02-13T00:49:38Z- handleDvl() - RDI_Pathfinder msg: 1613177378.67 epoch sec; seqNo:7731 2021-02-13T00:49:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.18 , 96.43 , 68.65 , 106.16 2021-02-13T00:49:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.16 , -0.18 2021-02-13T00:49:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177378.68 epoch sec; seqNo:16093; 4060141.59 north; 593206.20 east 2021-02-13T00:49:39Z- AHRS_M2 msg: 1613177379.05 epoch sec; seqNo:15970 2021-02-13T00:49:39Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-13T00:49:39Z- handleDepth()-Depth_Keller msg: 1613177379057 epoch sec; seqNo:16094; depth 18.35 2021-02-13T00:49:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177379.08 epoch sec; seqNo:16094; 4060141.97 north; 593206.20 east 2021-02-13T00:49:39Z- AHRS_M2 msg: 1613177379.45 epoch sec; seqNo:15971 2021-02-13T00:49:39Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-13T00:49:39Z- handleDepth()-Depth_Keller msg: 1613177379461 epoch sec; seqNo:16095; depth 18.48 2021-02-13T00:49:39Z- handleDvl() - RDI_Pathfinder msg: 1613177379.47 epoch sec; seqNo:7732 2021-02-13T00:49:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.19 , 95.55 , 70.70 , 106.50 2021-02-13T00:49:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.21 , -0.08 2021-02-13T00:49:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177379.48 epoch sec; seqNo:16095; 4060142.35 north; 593206.19 east 2021-02-13T00:49:39Z- AHRS_M2 msg: 1613177379.86 epoch sec; seqNo:15972 2021-02-13T00:49:39Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-13T00:49:39Z- handleDepth()-Depth_Keller msg: 1613177379869 epoch sec; seqNo:16096; depth 18.60 2021-02-13T00:49:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177379.88 epoch sec; seqNo:16096; 4060142.73 north; 593206.19 east 2021-02-13T00:49:40Z- AHRS_M2 msg: 1613177380.26 epoch sec; seqNo:15973 2021-02-13T00:49:40Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.18 psi 2021-02-13T00:49:40Z- handleDepth()-Depth_Keller msg: 1613177380264 epoch sec; seqNo:16097; depth 18.73 2021-02-13T00:49:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177380.27 epoch sec; seqNo:16097; 4060142.73 north; 593206.19 east 2021-02-13T00:49:40Z- AHRS_M2 msg: 1613177380.66 epoch sec; seqNo:15974 2021-02-13T00:49:40Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.18 psi 2021-02-13T00:49:40Z- handleDepth()-Depth_Keller msg: 1613177380668 epoch sec; seqNo:16098; depth 18.85 2021-02-13T00:49:40Z- handleDvl() - RDI_Pathfinder msg: 1613177380.68 epoch sec; seqNo:7733 2021-02-13T00:49:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 69.48 , 105.35 2021-02-13T00:49:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.19 , 0.07 2021-02-13T00:49:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177380.69 epoch sec; seqNo:16098; 4060143.11 north; 593206.18 east 2021-02-13T00:49:41Z- AHRS_M2 msg: 1613177381.07 epoch sec; seqNo:15975 2021-02-13T00:49:41Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.18 psi 2021-02-13T00:49:41Z- handleDepth()-Depth_Keller msg: 1613177381077 epoch sec; seqNo:16099; depth 18.98 2021-02-13T00:49:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177381.09 epoch sec; seqNo:16099; 4060143.48 north; 593206.18 east 2021-02-13T00:49:41Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:49:41Z- waiting for 1613177378.84, time is 1613177381.09 2021-02-13T00:49:41Z- LcmTrn::updateTrn() >>>> pose time 1613177381.09, meas time 1613177380.68 <<<< 2021-02-13T00:49:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177380.68 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:49:41Z- TerrainNav::motionUpdate() - currEstimate = (4060143, 593206.18, 18.98) 2021-02-13T00:49:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:49:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:49:41Z- TNavBF::Projecting Measurements 2021-02-13T00:49:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:49:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:49:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:49:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:49:41Z- TNavFilter::4 good beams. 2021-02-13T00:49:41Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:49:41Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:49:41Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:49:41Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:49:41Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:49:41Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:49:41Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:49:41Z- TNavBF::mmse selected from filter -1 2021-02-13T00:49:41Z- TerrainMapOctree: (vehN, vehE ) = (4060143.13, 593205.99). 2021-02-13T00:49:41Z- TerrainMapOctree: Min Distance = 1521.55. 2021-02-13T00:49:41Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:49:41Z- There is no specific good beam that all particles have in common. 2021-02-13T00:49:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:49:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-13T00:49:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:49:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 15.38 2021-02-13T00:49:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:49:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 7.72 2021-02-13T00:49:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:49:41Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 17.76 2021-02-13T00:49:41Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:49:41Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:49:41Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:49:41Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:49:41Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:49:41Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:49:41Z- Filter 0 No beams used in subcloud update 2021-02-13T00:49:41Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:49:41Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:49:41Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 686.2 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:49:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177380.68 sec. 2021-02-13T00:49:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:49:41Z- TNavBF::mmse selected from filter -1 2021-02-13T00:49:41Z- variances([0], [2], and psi_berg) = 686.199691 832.991199 0.000000 2021-02-13T00:49:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:49:41Z- LcmTrn::publishEstimates() - MLE : 4060142.98 593226.68 18.98 2021-02-13T00:49:41Z- LcmTrn::publishEstimates() - MMSE : 4060143.48 593206.18 18.98 2021-02-13T00:49:41Z- LcmTrn::publishEstimates() - COVAR: 686.20 832.99 0.89 0.00 2021-02-13T00:49:41Z- AHRS_M2 msg: 1613177381.47 epoch sec; seqNo:15976 2021-02-13T00:49:41Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.18 psi 2021-02-13T00:49:41Z- handleDepth()-Depth_Keller msg: 1613177381478 epoch sec; seqNo:16100; depth 19.11 2021-02-13T00:49:41Z- handleDvl() - RDI_Pathfinder msg: 1613177381.49 epoch sec; seqNo:7734 2021-02-13T00:49:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.02 , 97.91 , 68.66 , 108.86 2021-02-13T00:49:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , 0.21 2021-02-13T00:49:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177381.50 epoch sec; seqNo:16100; 4060143.86 north; 593206.18 east 2021-02-13T00:49:41Z- AHRS_M2 msg: 1613177381.88 epoch sec; seqNo:15977 2021-02-13T00:49:41Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.18 psi 2021-02-13T00:49:41Z- handleDepth()-Depth_Keller msg: 1613177381881 epoch sec; seqNo:16101; depth 19.24 2021-02-13T00:49:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177381.89 epoch sec; seqNo:16101; 4060143.86 north; 593206.18 east 2021-02-13T00:49:42Z- AHRS_M2 msg: 1613177382.30 epoch sec; seqNo:15978 2021-02-13T00:49:42Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.18 psi 2021-02-13T00:49:42Z- handleDepth()-Depth_Keller msg: 1613177382301 epoch sec; seqNo:16102; depth 19.36 2021-02-13T00:49:42Z- handleDvl() - RDI_Pathfinder msg: 1613177382.32 epoch sec; seqNo:7735 2021-02-13T00:49:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 97.91 , 69.48 , 111.30 2021-02-13T00:49:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , 0.23 2021-02-13T00:49:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177382.33 epoch sec; seqNo:16102; 4060144.25 north; 593207.39 east 2021-02-13T00:49:42Z- AHRS_M2 msg: 1613177382.68 epoch sec; seqNo:15979 2021-02-13T00:49:42Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.18 psi 2021-02-13T00:49:42Z- handleDepth()-Depth_Keller msg: 1613177382688 epoch sec; seqNo:16103; depth 19.49 2021-02-13T00:49:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177382.70 epoch sec; seqNo:16103; 4060144.63 north; 593207.39 east 2021-02-13T00:49:43Z- handleDepth()-Depth_Keller msg: 1613177383110 epoch sec; seqNo:16104; depth 19.62 2021-02-13T00:49:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177383.12 epoch sec; seqNo:16104; 4060144.63 north; 593207.39 east 2021-02-13T00:49:43Z- AHRS_M2 msg: 1613177383.49 epoch sec; seqNo:15980 2021-02-13T00:49:43Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.18 psi 2021-02-13T00:49:43Z- handleDepth()-Depth_Keller msg: 1613177383497 epoch sec; seqNo:16105; depth 19.77 2021-02-13T00:49:43Z- handleDvl() - RDI_Pathfinder msg: 1613177383.51 epoch sec; seqNo:7736 2021-02-13T00:49:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.85 , 68.66 , 107.58 2021-02-13T00:49:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.13 , 0.14 2021-02-13T00:49:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177383.54 epoch sec; seqNo:16105; 4060145.01 north; 593207.38 east 2021-02-13T00:49:43Z- AHRS_M2 msg: 1613177383.90 epoch sec; seqNo:15981 2021-02-13T00:49:43Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.18 psi 2021-02-13T00:49:43Z- handleDepth()-Depth_Keller msg: 1613177383905 epoch sec; seqNo:16106; depth 19.89 2021-02-13T00:49:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177383.91 epoch sec; seqNo:16106; 4060145.39 north; 593207.38 east 2021-02-13T00:49:44Z- AHRS_M2 msg: 1613177384.31 epoch sec; seqNo:15982 2021-02-13T00:49:44Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.18 psi 2021-02-13T00:49:44Z- handleDepth()-Depth_Keller msg: 1613177384317 epoch sec; seqNo:16107; depth 20.02 2021-02-13T00:49:44Z- handleDvl() - RDI_Pathfinder msg: 1613177384.33 epoch sec; seqNo:7737 2021-02-13T00:49:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.02 , 93.99 , 67.43 , 107.38 2021-02-13T00:49:44Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.12 , 0.01 2021-02-13T00:49:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177384.34 epoch sec; seqNo:16107; 4060145.40 north; 593208.60 east 2021-02-13T00:49:44Z- AHRS_M2 msg: 1613177384.70 epoch sec; seqNo:15983 2021-02-13T00:49:44Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.18 psi 2021-02-13T00:49:44Z- handleDepth()-Depth_Keller msg: 1613177384713 epoch sec; seqNo:16108; depth 20.16 2021-02-13T00:49:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177384.72 epoch sec; seqNo:16108; 4060145.78 north; 593208.60 east 2021-02-13T00:49:45Z- AHRS_M2 msg: 1613177385.11 epoch sec; seqNo:15984 2021-02-13T00:49:45Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.19 psi 2021-02-13T00:49:45Z- handleDepth()-Depth_Keller msg: 1613177385115 epoch sec; seqNo:16109; depth 20.29 2021-02-13T00:49:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177385.13 epoch sec; seqNo:16109; 4060146.16 north; 593208.59 east 2021-02-13T00:49:45Z- AHRS_M2 msg: 1613177385.51 epoch sec; seqNo:15985 2021-02-13T00:49:45Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.19 psi 2021-02-13T00:49:45Z- handleDepth()-Depth_Keller msg: 1613177385518 epoch sec; seqNo:16110; depth 20.41 2021-02-13T00:49:45Z- handleDvl() - RDI_Pathfinder msg: 1613177385.53 epoch sec; seqNo:7738 2021-02-13T00:49:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.20 , 98.12 , 67.43 , 107.85 2021-02-13T00:49:45Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.04 , -0.08 2021-02-13T00:49:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177385.54 epoch sec; seqNo:16110; 4060146.54 north; 593208.59 east 2021-02-13T00:49:45Z- AHRS_M2 msg: 1613177385.91 epoch sec; seqNo:15986 2021-02-13T00:49:45Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.19 psi 2021-02-13T00:49:45Z- handleDepth()-Depth_Keller msg: 1613177385920 epoch sec; seqNo:16111; depth 20.57 2021-02-13T00:49:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177385.94 epoch sec; seqNo:16111; 4060146.54 north; 593208.59 east 2021-02-13T00:49:46Z- LcmTrn::handleMessages() - handling 40 messages took 0.56 ms and 129 calls... 2021-02-13T00:49:46Z- waiting for 1613177384.09, time is 1613177385.94 2021-02-13T00:49:46Z- LcmTrn::updateTrn() >>>> pose time 1613177385.94, meas time 1613177385.53 <<<< 2021-02-13T00:49:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177385.53 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:49:46Z- TerrainNav::motionUpdate() - currEstimate = (4060147, 593208.59, 20.57) 2021-02-13T00:49:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:49:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:49:46Z- TNavBF::Projecting Measurements 2021-02-13T00:49:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:49:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:49:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:49:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:49:46Z- TNavFilter::4 good beams. 2021-02-13T00:49:46Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:49:46Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:49:46Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:49:46Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:49:46Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:49:46Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:49:46Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:49:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:49:46Z- TerrainMapOctree: (vehN, vehE ) = (4060146.28, 593208.39). 2021-02-13T00:49:46Z- TerrainMapOctree: Min Distance = 1517.61. 2021-02-13T00:49:46Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:49:46Z- There is no specific good beam that all particles have in common. 2021-02-13T00:49:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:49:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 9.05 2021-02-13T00:49:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:49:46Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.40 2021-02-13T00:49:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:49:46Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 7.28 2021-02-13T00:49:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:49:46Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 18.61 2021-02-13T00:49:46Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:49:46Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:49:46Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:49:46Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:49:46Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:49:46Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:49:46Z- Filter 0 No beams used in subcloud update 2021-02-13T00:49:46Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:49:46Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:49:46Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 686.2 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:49:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177385.53 sec. 2021-02-13T00:49:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:49:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:49:46Z- variances([0], [2], and psi_berg) = 686.142138 832.991173 0.000000 2021-02-13T00:49:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:49:46Z- LcmTrn::publishEstimates() - MLE : 4060146.04 593229.09 20.57 2021-02-13T00:49:46Z- LcmTrn::publishEstimates() - MMSE : 4060146.54 593208.59 20.57 2021-02-13T00:49:46Z- LcmTrn::publishEstimates() - COVAR: 686.14 832.99 0.82 0.00 2021-02-13T00:49:46Z- AHRS_M2 msg: 1613177386.32 epoch sec; seqNo:15987 2021-02-13T00:49:46Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.19 psi 2021-02-13T00:49:46Z- handleDepth()-Depth_Keller msg: 1613177386324 epoch sec; seqNo:16112; depth 20.70 2021-02-13T00:49:46Z- handleDvl() - RDI_Pathfinder msg: 1613177386.34 epoch sec; seqNo:7739 2021-02-13T00:49:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.20 , 97.98 , 68.66 , 105.28 2021-02-13T00:49:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.02 , -0.08 2021-02-13T00:49:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177386.36 epoch sec; seqNo:16112; 4060146.93 north; 593209.81 east 2021-02-13T00:49:46Z- AHRS_M2 msg: 1613177386.72 epoch sec; seqNo:15988 2021-02-13T00:49:46Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.20 psi 2021-02-13T00:49:46Z- handleDepth()-Depth_Keller msg: 1613177386733 epoch sec; seqNo:16113; depth 20.83 2021-02-13T00:49:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177386.74 epoch sec; seqNo:16113; 4060147.31 north; 593209.80 east 2021-02-13T00:49:47Z- AHRS_M2 msg: 1613177387.13 epoch sec; seqNo:15989 2021-02-13T00:49:47Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-13T00:49:47Z- handleDepth()-Depth_Keller msg: 1613177387141 epoch sec; seqNo:16114; depth 20.96 2021-02-13T00:49:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177387.15 epoch sec; seqNo:16114; 4060147.69 north; 593209.80 east 2021-02-13T00:49:47Z- AHRS_M2 msg: 1613177387.53 epoch sec; seqNo:15990 2021-02-13T00:49:47Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-13T00:49:47Z- handleDepth()-Depth_Keller msg: 1613177387546 epoch sec; seqNo:16115; depth 21.09 2021-02-13T00:49:47Z- handleDvl() - RDI_Pathfinder msg: 1613177387.56 epoch sec; seqNo:7740 2021-02-13T00:49:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 98.05 , 66.21 , 105.35 2021-02-13T00:49:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , -0.02 2021-02-13T00:49:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177387.57 epoch sec; seqNo:16115; 4060148.07 north; 593209.79 east 2021-02-13T00:49:47Z- AHRS_M2 msg: 1613177387.93 epoch sec; seqNo:15991 2021-02-13T00:49:47Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.20 psi 2021-02-13T00:49:47Z- handleDepth()-Depth_Keller msg: 1613177387940 epoch sec; seqNo:16116; depth 21.24 2021-02-13T00:49:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177387.95 epoch sec; seqNo:16116; 4060148.45 north; 593209.79 east 2021-02-13T00:49:48Z- AHRS_M2 msg: 1613177388.34 epoch sec; seqNo:15992 2021-02-13T00:49:48Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.20 psi 2021-02-13T00:49:48Z- handleDepth()-Depth_Keller msg: 1613177388344 epoch sec; seqNo:16117; depth 21.36 2021-02-13T00:49:48Z- handleDvl() - RDI_Pathfinder msg: 1613177388.36 epoch sec; seqNo:7741 2021-02-13T00:49:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.78 , 98.12 , 67.02 , 106.64 2021-02-13T00:49:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , 0.04 2021-02-13T00:49:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177388.37 epoch sec; seqNo:16117; 4060148.45 north; 593209.79 east 2021-02-13T00:49:48Z- AHRS_M2 msg: 1613177388.75 epoch sec; seqNo:15993 2021-02-13T00:49:48Z- AHRS_M2 msg: 0.59 phi; -0.37 theta; -0.20 psi 2021-02-13T00:49:48Z- handleDepth()-Depth_Keller msg: 1613177388755 epoch sec; seqNo:16118; depth 21.50 2021-02-13T00:49:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177388.77 epoch sec; seqNo:16118; 4060148.84 north; 593211.01 east 2021-02-13T00:49:49Z- AHRS_M2 msg: 1613177389.15 epoch sec; seqNo:15994 2021-02-13T00:49:49Z- AHRS_M2 msg: 0.59 phi; -0.37 theta; -0.20 psi 2021-02-13T00:49:49Z- handleDepth()-Depth_Keller msg: 1613177389162 epoch sec; seqNo:16119; depth 21.63 2021-02-13T00:49:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177389.19 epoch sec; seqNo:16119; 4060149.22 north; 593211.00 east 2021-02-13T00:49:49Z- AHRS_M2 msg: 1613177389.55 epoch sec; seqNo:15995 2021-02-13T00:49:49Z- AHRS_M2 msg: 0.59 phi; -0.37 theta; -0.20 psi 2021-02-13T00:49:49Z- handleDepth()-Depth_Keller msg: 1613177389556 epoch sec; seqNo:16120; depth 21.78 2021-02-13T00:49:49Z- handleDvl() - RDI_Pathfinder msg: 1613177389.57 epoch sec; seqNo:7742 2021-02-13T00:49:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.96 , 93.18 , 67.43 , 105.35 2021-02-13T00:49:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.05 , 0.05 2021-02-13T00:49:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177389.58 epoch sec; seqNo:16120; 4060149.60 north; 593211.00 east 2021-02-13T00:49:49Z- AHRS_M2 msg: 1613177389.96 epoch sec; seqNo:15996 2021-02-13T00:49:49Z- AHRS_M2 msg: 0.59 phi; -0.37 theta; -0.20 psi 2021-02-13T00:49:49Z- handleDepth()-Depth_Keller msg: 1613177389961 epoch sec; seqNo:16121; depth 21.91 2021-02-13T00:49:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177389.97 epoch sec; seqNo:16121; 4060149.60 north; 593211.00 east 2021-02-13T00:49:50Z- AHRS_M2 msg: 1613177390.36 epoch sec; seqNo:15997 2021-02-13T00:49:50Z- AHRS_M2 msg: 0.59 phi; -0.37 theta; -0.20 psi 2021-02-13T00:49:50Z- handleDepth()-Depth_Keller msg: 1613177390364 epoch sec; seqNo:16122; depth 22.01 2021-02-13T00:49:50Z- handleDvl() - RDI_Pathfinder msg: 1613177390.38 epoch sec; seqNo:7743 2021-02-13T00:49:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.97 , 98.18 , 66.62 , 106.70 2021-02-13T00:49:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.07 , -0.01 2021-02-13T00:49:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177390.39 epoch sec; seqNo:16122; 4060149.98 north; 593210.99 east 2021-02-13T00:49:50Z- AHRS_M2 msg: 1613177390.76 epoch sec; seqNo:15998 2021-02-13T00:49:50Z- AHRS_M2 msg: 0.59 phi; -0.37 theta; -0.20 psi 2021-02-13T00:49:50Z- handleDepth()-Depth_Keller msg: 1613177390768 epoch sec; seqNo:16123; depth 22.18 2021-02-13T00:49:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177390.78 epoch sec; seqNo:16123; 4060150.36 north; 593210.99 east 2021-02-13T00:49:51Z- AHRS_M2 msg: 1613177391.17 epoch sec; seqNo:15999 2021-02-13T00:49:51Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.20 psi 2021-02-13T00:49:51Z- handleDepth()-Depth_Keller msg: 1613177391173 epoch sec; seqNo:16124; depth 22.30 2021-02-13T00:49:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177391.18 epoch sec; seqNo:16124; 4060150.75 north; 593212.21 east 2021-02-13T00:49:51Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:49:51Z- waiting for 1613177388.94, time is 1613177391.18 2021-02-13T00:49:51Z- LcmTrn::updateTrn() >>>> pose time 1613177391.18, meas time 1613177390.38 <<<< 2021-02-13T00:49:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177390.38 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:49:51Z- TerrainNav::motionUpdate() - currEstimate = (4060151, 593212.21, 22.30) 2021-02-13T00:49:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:49:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:49:51Z- TNavBF::Projecting Measurements 2021-02-13T00:49:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:49:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:49:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:49:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:49:51Z- TNavFilter::4 good beams. 2021-02-13T00:49:51Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:49:51Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:49:51Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:49:51Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:49:51Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:49:51Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:49:51Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:49:51Z- TNavBF::mmse selected from filter -1 2021-02-13T00:49:51Z- TerrainMapOctree: (vehN, vehE ) = (4060150.11, 593211.65). 2021-02-13T00:49:51Z- TerrainMapOctree: Min Distance = 1512.59. 2021-02-13T00:49:51Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:49:51Z- There is no specific good beam that all particles have in common. 2021-02-13T00:49:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:49:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 8.76 2021-02-13T00:49:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:49:51Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 15.42 2021-02-13T00:49:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:49:51Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 7.10 2021-02-13T00:49:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:49:51Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 18.22 2021-02-13T00:49:51Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:49:51Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:49:51Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:49:51Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:49:51Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:49:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:49:51Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:49:51Z- Filter 0 No beams used in subcloud update 2021-02-13T00:49:51Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:49:51Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:49:51Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 686.1 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:49:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177390.38 sec. 2021-02-13T00:49:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:49:51Z- TNavBF::mmse selected from filter -1 2021-02-13T00:49:51Z- variances([0], [2], and psi_berg) = 686.069935 832.991140 0.000000 2021-02-13T00:49:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:49:51Z- LcmTrn::publishEstimates() - MLE : 4060150.25 593232.71 22.30 2021-02-13T00:49:51Z- LcmTrn::publishEstimates() - MMSE : 4060150.75 593212.21 22.30 2021-02-13T00:49:51Z- LcmTrn::publishEstimates() - COVAR: 686.07 832.99 0.84 0.00 2021-02-13T00:49:51Z- AHRS_M2 msg: 1613177391.57 epoch sec; seqNo:16000 2021-02-13T00:49:51Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.20 psi 2021-02-13T00:49:51Z- handleDepth()-Depth_Keller msg: 1613177391578 epoch sec; seqNo:16125; depth 22.46 2021-02-13T00:49:51Z- handleDvl() - RDI_Pathfinder msg: 1613177391.59 epoch sec; seqNo:7744 2021-02-13T00:49:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 98.05 , 65.80 , 107.78 2021-02-13T00:49:51Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.04 , -0.09 2021-02-13T00:49:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177391.60 epoch sec; seqNo:16125; 4060150.75 north; 593212.21 east 2021-02-13T00:49:51Z- AHRS_M2 msg: 1613177391.97 epoch sec; seqNo:16001 2021-02-13T00:49:51Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.20 psi 2021-02-13T00:49:51Z- handleDepth()-Depth_Keller msg: 1613177391985 epoch sec; seqNo:16126; depth 22.59 2021-02-13T00:49:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177392.00 epoch sec; seqNo:16126; 4060151.13 north; 593212.20 east 2021-02-13T00:49:52Z- AHRS_M2 msg: 1613177392.38 epoch sec; seqNo:16002 2021-02-13T00:49:52Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.20 psi 2021-02-13T00:49:52Z- handleDepth()-Depth_Keller msg: 1613177392389 epoch sec; seqNo:16127; depth 22.73 2021-02-13T00:49:52Z- handleDvl() - RDI_Pathfinder msg: 1613177392.40 epoch sec; seqNo:7745 2021-02-13T00:49:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 98.05 , 65.80 , 105.35 2021-02-13T00:49:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.07 , -0.13 2021-02-13T00:49:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177392.41 epoch sec; seqNo:16127; 4060151.51 north; 593212.20 east 2021-02-13T00:49:52Z- AHRS_M2 msg: 1613177392.78 epoch sec; seqNo:16003 2021-02-13T00:49:52Z- AHRS_M2 msg: 0.59 phi; -0.37 theta; -0.20 psi 2021-02-13T00:49:52Z- handleDepth()-Depth_Keller msg: 1613177392789 epoch sec; seqNo:16128; depth 22.87 2021-02-13T00:49:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177392.81 epoch sec; seqNo:16128; 4060151.88 north; 593212.19 east 2021-02-13T00:49:53Z- AHRS_M2 msg: 1613177393.19 epoch sec; seqNo:16004 2021-02-13T00:49:53Z- AHRS_M2 msg: 0.59 phi; -0.37 theta; -0.21 psi 2021-02-13T00:49:53Z- handleDepth()-Depth_Keller msg: 1613177393201 epoch sec; seqNo:16129; depth 23.03 2021-02-13T00:49:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177393.24 epoch sec; seqNo:16129; 4060152.26 north; 593212.19 east 2021-02-13T00:49:53Z- AHRS_M2 msg: 1613177393.59 epoch sec; seqNo:16005 2021-02-13T00:49:53Z- AHRS_M2 msg: 0.59 phi; -0.37 theta; -0.21 psi 2021-02-13T00:49:53Z- handleDepth()-Depth_Keller msg: 1613177393601 epoch sec; seqNo:16130; depth 23.17 2021-02-13T00:49:53Z- handleDvl() - RDI_Pathfinder msg: 1613177393.61 epoch sec; seqNo:7746 2021-02-13T00:49:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.93 , 98.18 , 65.80 , 104.27 2021-02-13T00:49:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.14 , -0.09 2021-02-13T00:49:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177393.62 epoch sec; seqNo:16130; 4060152.66 north; 593213.41 east 2021-02-13T00:49:54Z- AHRS_M2 msg: 1613177393.99 epoch sec; seqNo:16006 2021-02-13T00:49:54Z- AHRS_M2 msg: 0.58 phi; -0.37 theta; -0.21 psi 2021-02-13T00:49:54Z- handleDepth()-Depth_Keller msg: 1613177394003 epoch sec; seqNo:16131; depth 23.31 2021-02-13T00:49:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177394.01 epoch sec; seqNo:16131; 4060153.03 north; 593213.40 east 2021-02-13T00:49:54Z- AHRS_M2 msg: 1613177394.40 epoch sec; seqNo:16007 2021-02-13T00:49:54Z- AHRS_M2 msg: 0.57 phi; -0.36 theta; -0.21 psi 2021-02-13T00:49:54Z- handleDepth()-Depth_Keller msg: 1613177394405 epoch sec; seqNo:16132; depth 23.44 2021-02-13T00:49:54Z- handleDvl() - RDI_Pathfinder msg: 1613177394.42 epoch sec; seqNo:7747 2021-02-13T00:49:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.52 , 98.18 , 65.80 , 104.27 2021-02-13T00:49:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.16 , -0.01 2021-02-13T00:49:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177394.44 epoch sec; seqNo:16132; 4060153.03 north; 593213.40 east 2021-02-13T00:49:54Z- AHRS_M2 msg: 1613177394.81 epoch sec; seqNo:16008 2021-02-13T00:49:54Z- AHRS_M2 msg: 0.57 phi; -0.36 theta; -0.21 psi 2021-02-13T00:49:54Z- handleDepth()-Depth_Keller msg: 1613177394814 epoch sec; seqNo:16133; depth 23.57 2021-02-13T00:49:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:49:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:49:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:49:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:49:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:49:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:49:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:49:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:49:54Z- handleDvl() - RDI_Pathfinder msg: 1613177394.82 epoch sec; seqNo:7748 2021-02-13T00:49:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:49:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.16 , -0.01 2021-02-13T00:49:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177394.83 epoch sec; seqNo:16133; 4060153.41 north; 593213.40 east 2021-02-13T00:49:55Z- AHRS_M2 msg: 1613177395.21 epoch sec; seqNo:16009 2021-02-13T00:49:55Z- AHRS_M2 msg: 0.56 phi; -0.36 theta; -0.21 psi 2021-02-13T00:49:55Z- handleDepth()-Depth_Keller msg: 1613177395220 epoch sec; seqNo:16134; depth 23.71 2021-02-13T00:49:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177395.23 epoch sec; seqNo:16134; 4060153.79 north; 593213.39 east 2021-02-13T00:49:55Z- AHRS_M2 msg: 1613177395.61 epoch sec; seqNo:16010 2021-02-13T00:49:55Z- AHRS_M2 msg: 0.56 phi; -0.36 theta; -0.21 psi 2021-02-13T00:49:55Z- handleDepth()-Depth_Keller msg: 1613177395616 epoch sec; seqNo:16135; depth 23.84 2021-02-13T00:49:55Z- handleDvl() - RDI_Pathfinder msg: 1613177395.63 epoch sec; seqNo:7749 2021-02-13T00:49:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.71 , 98.18 , 65.39 , 101.83 2021-02-13T00:49:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.11 , 0.07 2021-02-13T00:49:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177395.64 epoch sec; seqNo:16135; 4060154.17 north; 593213.39 east 2021-02-13T00:49:56Z- AHRS_M2 msg: 1613177396.02 epoch sec; seqNo:16011 2021-02-13T00:49:56Z- AHRS_M2 msg: 0.55 phi; -0.36 theta; -0.20 psi 2021-02-13T00:49:56Z- handleDepth()-Depth_Keller msg: 1613177396020 epoch sec; seqNo:16136; depth 23.98 2021-02-13T00:49:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:49:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:49:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:49:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:49:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:49:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:49:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:49:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:49:56Z- handleDvl() - RDI_Pathfinder msg: 1613177396.02 epoch sec; seqNo:7750 2021-02-13T00:49:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:49:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.11 , 0.07 2021-02-13T00:49:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177396.03 epoch sec; seqNo:16136; 4060154.17 north; 593213.39 east 2021-02-13T00:49:56Z- AHRS_M2 msg: 1613177396.42 epoch sec; seqNo:16012 2021-02-13T00:49:56Z- AHRS_M2 msg: 0.55 phi; -0.35 theta; -0.20 psi 2021-02-13T00:49:56Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-13T00:49:56Z- waiting for 1613177394.18, time is 1613177396.42 2021-02-13T00:49:56Z- LcmTrn::updateTrn() >>>> pose time 1613177396.42, meas time 1613177396.02 <<<< 2021-02-13T00:49:56Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177396.02, ping # 0. 2021-02-13T00:49:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177396.02 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:49:56Z- TerrainNav::motionUpdate() - currEstimate = (4060154, 593213.39, 23.98) 2021-02-13T00:49:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:49:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:49:56Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:49:56Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:49:56Z- TNavBF::Projecting Measurements 2021-02-13T00:49:56Z- TNavFilter::Measurement from time = 1613177396.02 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:49:56Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:49:56Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:49:56Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:49:56Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:49:56Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:49:56Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:49:56Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:49:56Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 686.0 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:49:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:49:56Z- TNavBF::mmse selected from filter -1 2021-02-13T00:49:56Z- variances([0], [2], and psi_berg) = 686.014932 832.991115 0.000000 2021-02-13T00:49:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:49:56Z- LcmTrn::publishEstimates() - MLE : 4060153.67 593233.89 23.98 2021-02-13T00:49:56Z- LcmTrn::publishEstimates() - MMSE : 4060154.17 593213.39 23.98 2021-02-13T00:49:56Z- LcmTrn::publishEstimates() - COVAR: 686.01 832.99 1.02 0.00 2021-02-13T00:49:56Z- handleDepth()-Depth_Keller msg: 1613177396425 epoch sec; seqNo:16137; depth 24.11 2021-02-13T00:49:56Z- handleDvl() - RDI_Pathfinder msg: 1613177396.44 epoch sec; seqNo:7751 2021-02-13T00:49:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 98.12 , 64.57 , 101.77 2021-02-13T00:49:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , 0.10 2021-02-13T00:49:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177396.45 epoch sec; seqNo:16137; 4060154.55 north; 593213.38 east 2021-02-13T00:49:56Z- AHRS_M2 msg: 1613177396.82 epoch sec; seqNo:16013 2021-02-13T00:49:56Z- AHRS_M2 msg: 0.55 phi; -0.35 theta; -0.20 psi 2021-02-13T00:49:56Z- handleDepth()-Depth_Keller msg: 1613177396837 epoch sec; seqNo:16138; depth 24.24 2021-02-13T00:49:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177396.85 epoch sec; seqNo:16138; 4060154.93 north; 593213.38 east 2021-02-13T00:49:57Z- AHRS_M2 msg: 1613177397.25 epoch sec; seqNo:16014 2021-02-13T00:49:57Z- AHRS_M2 msg: 0.55 phi; -0.35 theta; -0.19 psi 2021-02-13T00:49:57Z- handleDepth()-Depth_Keller msg: 1613177397261 epoch sec; seqNo:16139; depth 24.38 2021-02-13T00:49:57Z- handleDvl() - RDI_Pathfinder msg: 1613177397.27 epoch sec; seqNo:7752 2021-02-13T00:49:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 0.00 , 64.57 , 101.97 2021-02-13T00:49:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , 0.11 2021-02-13T00:49:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177397.28 epoch sec; seqNo:16139; 4060155.32 north; 593214.60 east 2021-02-13T00:49:57Z- AHRS_M2 msg: 1613177397.63 epoch sec; seqNo:16015 2021-02-13T00:49:57Z- AHRS_M2 msg: 0.56 phi; -0.35 theta; -0.19 psi 2021-02-13T00:49:57Z- handleDepth()-Depth_Keller msg: 1613177397641 epoch sec; seqNo:16140; depth 24.53 2021-02-13T00:49:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177397.65 epoch sec; seqNo:16140; 4060155.32 north; 593214.60 east 2021-02-13T00:49:58Z- AHRS_M2 msg: 1613177398.04 epoch sec; seqNo:16016 2021-02-13T00:49:58Z- AHRS_M2 msg: 0.56 phi; -0.35 theta; -0.18 psi 2021-02-13T00:49:58Z- handleDepth()-Depth_Keller msg: 1613177398040 epoch sec; seqNo:16141; depth 24.63 2021-02-13T00:49:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177398.05 epoch sec; seqNo:16141; 4060155.70 north; 593214.59 east 2021-02-13T00:49:58Z- AHRS_M2 msg: 1613177398.44 epoch sec; seqNo:16017 2021-02-13T00:49:58Z- AHRS_M2 msg: 0.56 phi; -0.35 theta; -0.18 psi 2021-02-13T00:49:58Z- handleDepth()-Depth_Keller msg: 1613177398444 epoch sec; seqNo:16142; depth 24.75 2021-02-13T00:49:58Z- handleDvl() - RDI_Pathfinder msg: 1613177398.46 epoch sec; seqNo:7753 2021-02-13T00:49:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 0.00 , 64.16 , 99.94 2021-02-13T00:49:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.08 2021-02-13T00:49:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177398.47 epoch sec; seqNo:16142; 4060156.08 north; 593214.59 east 2021-02-13T00:49:58Z- AHRS_M2 msg: 1613177398.84 epoch sec; seqNo:16018 2021-02-13T00:49:58Z- AHRS_M2 msg: 0.56 phi; -0.34 theta; -0.18 psi 2021-02-13T00:49:58Z- handleDepth()-Depth_Keller msg: 1613177398853 epoch sec; seqNo:16143; depth 24.89 2021-02-13T00:49:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177398.88 epoch sec; seqNo:16143; 4060156.46 north; 593214.58 east 2021-02-13T00:49:59Z- AHRS_M2 msg: 1613177399.25 epoch sec; seqNo:16019 2021-02-13T00:49:59Z- AHRS_M2 msg: 0.56 phi; -0.34 theta; -0.18 psi 2021-02-13T00:49:59Z- handleDepth()-Depth_Keller msg: 1613177399253 epoch sec; seqNo:16144; depth 25.02 2021-02-13T00:49:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177399.26 epoch sec; seqNo:16144; 4060156.46 north; 593214.58 east 2021-02-13T00:49:59Z- AHRS_M2 msg: 1613177399.65 epoch sec; seqNo:16020 2021-02-13T00:49:59Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.17 psi 2021-02-13T00:49:59Z- handleDepth()-Depth_Keller msg: 1613177399657 epoch sec; seqNo:16145; depth 25.16 2021-02-13T00:49:59Z- handleDvl() - RDI_Pathfinder msg: 1613177399.67 epoch sec; seqNo:7754 2021-02-13T00:49:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.37 , 88.11 , 62.94 , 98.93 2021-02-13T00:49:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , 0.06 2021-02-13T00:49:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177399.68 epoch sec; seqNo:16145; 4060156.84 north; 593214.58 east 2021-02-13T00:50:00Z- AHRS_M2 msg: 1613177400.06 epoch sec; seqNo:16021 2021-02-13T00:50:00Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.17 psi 2021-02-13T00:50:00Z- handleDepth()-Depth_Keller msg: 1613177400070 epoch sec; seqNo:16146; depth 25.27 2021-02-13T00:50:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177400.08 epoch sec; seqNo:16146; 4060157.23 north; 593215.80 east 2021-02-13T00:50:00Z- AHRS_M2 msg: 1613177400.46 epoch sec; seqNo:16022 2021-02-13T00:50:00Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.17 psi 2021-02-13T00:50:00Z- handleDepth()-Depth_Keller msg: 1613177400473 epoch sec; seqNo:16147; depth 25.40 2021-02-13T00:50:00Z- handleDvl() - RDI_Pathfinder msg: 1613177400.48 epoch sec; seqNo:7755 2021-02-13T00:50:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 86.96 , 62.53 , 97.78 2021-02-13T00:50:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , 0.04 2021-02-13T00:50:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177400.51 epoch sec; seqNo:16147; 4060157.61 north; 593215.79 east 2021-02-13T00:50:00Z- AHRS_M2 msg: 1613177400.87 epoch sec; seqNo:16023 2021-02-13T00:50:00Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.17 psi 2021-02-13T00:50:00Z- handleDepth()-Depth_Keller msg: 1613177400870 epoch sec; seqNo:16148; depth 25.52 2021-02-13T00:50:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:50:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:50:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:50:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:50:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:50:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:50:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:50:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:50:00Z- handleDvl() - RDI_Pathfinder msg: 1613177400.87 epoch sec; seqNo:7756 2021-02-13T00:50:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:50:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , 0.04 2021-02-13T00:50:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177400.88 epoch sec; seqNo:16148; 4060157.61 north; 593215.79 east 2021-02-13T00:50:01Z- AHRS_M2 msg: 1613177401.27 epoch sec; seqNo:16024 2021-02-13T00:50:01Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.17 psi 2021-02-13T00:50:01Z- handleDepth()-Depth_Keller msg: 1613177401279 epoch sec; seqNo:16149; depth 25.65 2021-02-13T00:50:01Z- handleDvl() - RDI_Pathfinder msg: 1613177401.30 epoch sec; seqNo:7757 2021-02-13T00:50:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 86.08 , 63.76 , 97.98 2021-02-13T00:50:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.00 , -0.03 2021-02-13T00:50:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177401.31 epoch sec; seqNo:16149; 4060157.99 north; 593215.79 east 2021-02-13T00:50:01Z- LcmTrn::handleMessages() - handling 45 messages took 0.67 ms and 132 calls... 2021-02-13T00:50:01Z- waiting for 1613177399.42, time is 1613177401.31 2021-02-13T00:50:01Z- LcmTrn::updateTrn() equating pose 1613177401.31 and meas times 1613177401.31 2021-02-13T00:50:01Z- LcmTrn::updateTrn() >>>> pose time 1613177401.31, meas time 1613177401.31 <<<< 2021-02-13T00:50:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177401.31 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:50:01Z- TerrainNav::motionUpdate() - currEstimate = (4060158, 593215.79, 25.65) 2021-02-13T00:50:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:50:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:50:01Z- TNavBF::Projecting Measurements 2021-02-13T00:50:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:50:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:50:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:50:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:50:01Z- TNavFilter::4 good beams. 2021-02-13T00:50:01Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:50:01Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:50:01Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:50:01Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:50:01Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:50:01Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:50:01Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:50:01Z- TNavBF::mmse selected from filter -1 2021-02-13T00:50:01Z- TerrainMapOctree: (vehN, vehE ) = (4060157.99, 593215.79). 2021-02-13T00:50:01Z- TerrainMapOctree: Min Distance = 1504.02. 2021-02-13T00:50:01Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:50:01Z- There is no specific good beam that all particles have in common. 2021-02-13T00:50:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:50:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.117 ExpectedMeasDiff: nan currMeas.covariance: 7.72 2021-02-13T00:50:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:50:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 11.86 2021-02-13T00:50:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:50:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.110 ExpectedMeasDiff: nan currMeas.covariance: 6.50 2021-02-13T00:50:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:50:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:50:01Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:50:01Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:50:01Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:50:01Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:50:01Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:50:01Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:50:01Z- Filter 0 No beams used in subcloud update 2021-02-13T00:50:01Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:50:01Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:50:01Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 686.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:50:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177401.31 sec. 2021-02-13T00:50:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:50:01Z- TNavBF::mmse selected from filter -1 2021-02-13T00:50:01Z- variances([0], [2], and psi_berg) = 685.965349 832.991093 0.000000 2021-02-13T00:50:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:50:01Z- LcmTrn::publishEstimates() - MLE : 4060157.49 593236.29 25.65 2021-02-13T00:50:01Z- LcmTrn::publishEstimates() - MMSE : 4060157.99 593215.79 25.65 2021-02-13T00:50:01Z- LcmTrn::publishEstimates() - COVAR: 685.97 832.99 0.86 0.00 2021-02-13T00:50:01Z- AHRS_M2 msg: 1613177401.67 epoch sec; seqNo:16025 2021-02-13T00:50:01Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.16 psi 2021-02-13T00:50:01Z- handleDepth()-Depth_Keller msg: 1613177401676 epoch sec; seqNo:16150; depth 25.77 2021-02-13T00:50:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177401.69 epoch sec; seqNo:16150; 4060158.37 north; 593215.78 east 2021-02-13T00:50:02Z- AHRS_M2 msg: 1613177402.08 epoch sec; seqNo:16026 2021-02-13T00:50:02Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.16 psi 2021-02-13T00:50:02Z- handleDepth()-Depth_Keller msg: 1613177402085 epoch sec; seqNo:16151; depth 25.88 2021-02-13T00:50:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:50:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:50:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:50:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:50:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:50:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:50:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:50:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:50:02Z- handleDvl() - RDI_Pathfinder msg: 1613177402.09 epoch sec; seqNo:7758 2021-02-13T00:50:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:50:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.00 , -0.03 2021-02-13T00:50:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177402.09 epoch sec; seqNo:16151; 4060158.74 north; 593215.78 east 2021-02-13T00:50:02Z- AHRS_M2 msg: 1613177402.48 epoch sec; seqNo:16027 2021-02-13T00:50:02Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.16 psi 2021-02-13T00:50:02Z- handleDepth()-Depth_Keller msg: 1613177402484 epoch sec; seqNo:16152; depth 26.02 2021-02-13T00:50:02Z- handleDvl() - RDI_Pathfinder msg: 1613177402.50 epoch sec; seqNo:7759 2021-02-13T00:50:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 86.08 , 62.53 , 97.98 2021-02-13T00:50:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.02 , -0.06 2021-02-13T00:50:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177402.51 epoch sec; seqNo:16152; 4060159.14 north; 593217.00 east 2021-02-13T00:50:02Z- AHRS_M2 msg: 1613177402.88 epoch sec; seqNo:16028 2021-02-13T00:50:02Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.17 psi 2021-02-13T00:50:02Z- handleDepth()-Depth_Keller msg: 1613177402888 epoch sec; seqNo:16153; depth 26.14 2021-02-13T00:50:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177402.90 epoch sec; seqNo:16153; 4060159.14 north; 593217.00 east 2021-02-13T00:50:03Z- AHRS_M2 msg: 1613177403.29 epoch sec; seqNo:16029 2021-02-13T00:50:03Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.17 psi 2021-02-13T00:50:03Z- handleDepth()-Depth_Keller msg: 1613177403296 epoch sec; seqNo:16154; depth 26.26 2021-02-13T00:50:03Z- handleDvl() - RDI_Pathfinder msg: 1613177403.31 epoch sec; seqNo:7760 2021-02-13T00:50:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.66 , 85.00 , 61.71 , 97.98 2021-02-13T00:50:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.06 , -0.06 2021-02-13T00:50:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177403.33 epoch sec; seqNo:16154; 4060159.51 north; 593216.99 east 2021-02-13T00:50:03Z- AHRS_M2 msg: 1613177403.69 epoch sec; seqNo:16030 2021-02-13T00:50:03Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.17 psi 2021-02-13T00:50:03Z- handleDepth()-Depth_Keller msg: 1613177403701 epoch sec; seqNo:16155; depth 26.39 2021-02-13T00:50:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177403.71 epoch sec; seqNo:16155; 4060159.89 north; 593216.99 east 2021-02-13T00:50:04Z- AHRS_M2 msg: 1613177404.10 epoch sec; seqNo:16031 2021-02-13T00:50:04Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.17 psi 2021-02-13T00:50:04Z- handleDepth()-Depth_Keller msg: 1613177404102 epoch sec; seqNo:16156; depth 26.48 2021-02-13T00:50:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177404.11 epoch sec; seqNo:16156; 4060160.27 north; 593216.98 east 2021-02-13T00:50:04Z- AHRS_M2 msg: 1613177404.50 epoch sec; seqNo:16032 2021-02-13T00:50:04Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.17 psi 2021-02-13T00:50:04Z- handleDepth()-Depth_Keller msg: 1613177404509 epoch sec; seqNo:16157; depth 26.60 2021-02-13T00:50:04Z- handleDvl() - RDI_Pathfinder msg: 1613177404.53 epoch sec; seqNo:7761 2021-02-13T00:50:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.66 , 0.00 , 62.94 , 97.98 2021-02-13T00:50:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.16 , -0.04 2021-02-13T00:50:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177404.54 epoch sec; seqNo:16157; 4060160.65 north; 593216.98 east 2021-02-13T00:50:04Z- AHRS_M2 msg: 1613177404.90 epoch sec; seqNo:16033 2021-02-13T00:50:04Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-13T00:50:04Z- handleDepth()-Depth_Keller msg: 1613177404909 epoch sec; seqNo:16158; depth 26.74 2021-02-13T00:50:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177404.93 epoch sec; seqNo:16158; 4060161.03 north; 593216.98 east 2021-02-13T00:50:05Z- AHRS_M2 msg: 1613177405.33 epoch sec; seqNo:16034 2021-02-13T00:50:05Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-13T00:50:05Z- handleDepth()-Depth_Keller msg: 1613177405329 epoch sec; seqNo:16159; depth 26.83 2021-02-13T00:50:05Z- handleDvl() - RDI_Pathfinder msg: 1613177405.34 epoch sec; seqNo:7762 2021-02-13T00:50:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.25 , 83.92 , 62.12 , 97.98 2021-02-13T00:50:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.11 , -0.01 2021-02-13T00:50:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177405.37 epoch sec; seqNo:16159; 4060161.42 north; 593218.19 east 2021-02-13T00:50:05Z- handleDepth()-Depth_Keller msg: 1613177405737 epoch sec; seqNo:16160; depth 26.96 2021-02-13T00:50:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177405.75 epoch sec; seqNo:16160; 4060161.42 north; 593218.19 east 2021-02-13T00:50:06Z- AHRS_M2 msg: 1613177406.12 epoch sec; seqNo:16035 2021-02-13T00:50:06Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.16 psi 2021-02-13T00:50:06Z- handleDepth()-Depth_Keller msg: 1613177406136 epoch sec; seqNo:16161; depth 27.08 2021-02-13T00:50:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177406.15 epoch sec; seqNo:16161; 4060161.80 north; 593218.19 east 2021-02-13T00:50:06Z- AHRS_M2 msg: 1613177406.52 epoch sec; seqNo:16036 2021-02-13T00:50:06Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-13T00:50:06Z- handleDepth()-Depth_Keller msg: 1613177406524 epoch sec; seqNo:16162; depth 27.19 2021-02-13T00:50:06Z- handleDvl() - RDI_Pathfinder msg: 1613177406.54 epoch sec; seqNo:7763 2021-02-13T00:50:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 83.92 , 62.12 , 97.98 2021-02-13T00:50:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.10 , 0.02 2021-02-13T00:50:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177406.56 epoch sec; seqNo:16162; 4060162.18 north; 593218.18 east 2021-02-13T00:50:06Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-13T00:50:06Z- waiting for 1613177404.31, time is 1613177406.56 2021-02-13T00:50:06Z- LcmTrn::updateTrn() equating pose 1613177406.56 and meas times 1613177406.56 2021-02-13T00:50:06Z- LcmTrn::updateTrn() >>>> pose time 1613177406.56, meas time 1613177406.56 <<<< 2021-02-13T00:50:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177406.56 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:50:06Z- TerrainNav::motionUpdate() - currEstimate = (4060162, 593218.18, 27.19) 2021-02-13T00:50:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:50:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:50:06Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177406.56 sec.; No relevant navigation data available 2021-02-13T00:50:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:50:06Z- TNavBF::mmse selected from filter -1 2021-02-13T00:50:06Z- variances([0], [2], and psi_berg) = 685.914046 832.991070 0.000000 2021-02-13T00:50:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:50:06Z- LcmTrn::publishEstimates() - MLE : 4060161.68 593239.68 27.19 2021-02-13T00:50:06Z- LcmTrn::publishEstimates() - MMSE : 4060162.18 593218.18 27.19 2021-02-13T00:50:06Z- LcmTrn::publishEstimates() - COVAR: 685.91 832.99 1.11 0.00 2021-02-13T00:50:06Z- AHRS_M2 msg: 1613177406.92 epoch sec; seqNo:16037 2021-02-13T00:50:06Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.16 psi 2021-02-13T00:50:06Z- handleDepth()-Depth_Keller msg: 1613177406928 epoch sec; seqNo:16163; depth 27.31 2021-02-13T00:50:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177406.94 epoch sec; seqNo:16163; 4060162.56 north; 593218.18 east 2021-02-13T00:50:07Z- AHRS_M2 msg: 1613177407.34 epoch sec; seqNo:16038 2021-02-13T00:50:07Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-13T00:50:07Z- handleDepth()-Depth_Keller msg: 1613177407342 epoch sec; seqNo:16164; depth 27.43 2021-02-13T00:50:07Z- handleDvl() - RDI_Pathfinder msg: 1613177407.35 epoch sec; seqNo:7764 2021-02-13T00:50:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 0.00 , 61.71 , 97.98 2021-02-13T00:50:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , 0.05 2021-02-13T00:50:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177407.36 epoch sec; seqNo:16164; 4060162.94 north; 593218.18 east 2021-02-13T00:50:07Z- AHRS_M2 msg: 1613177407.73 epoch sec; seqNo:16039 2021-02-13T00:50:07Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-13T00:50:07Z- handleDepth()-Depth_Keller msg: 1613177407736 epoch sec; seqNo:16165; depth 27.54 2021-02-13T00:50:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177407.76 epoch sec; seqNo:16165; 4060162.94 north; 593218.18 east 2021-02-13T00:50:08Z- AHRS_M2 msg: 1613177408.14 epoch sec; seqNo:16040 2021-02-13T00:50:08Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.17 psi 2021-02-13T00:50:08Z- handleDepth()-Depth_Keller msg: 1613177408154 epoch sec; seqNo:16166; depth 27.67 2021-02-13T00:50:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177408.19 epoch sec; seqNo:16166; 4060163.32 north; 593218.17 east 2021-02-13T00:50:08Z- AHRS_M2 msg: 1613177408.54 epoch sec; seqNo:16041 2021-02-13T00:50:08Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.17 psi 2021-02-13T00:50:08Z- handleDepth()-Depth_Keller msg: 1613177408549 epoch sec; seqNo:16167; depth 27.79 2021-02-13T00:50:08Z- handleDvl() - RDI_Pathfinder msg: 1613177408.56 epoch sec; seqNo:7765 2021-02-13T00:50:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 0.00 , 60.90 , 91.49 2021-02-13T00:50:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , 0.04 2021-02-13T00:50:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177408.57 epoch sec; seqNo:16167; 4060163.71 north; 593219.39 east 2021-02-13T00:50:08Z- AHRS_M2 msg: 1613177408.94 epoch sec; seqNo:16042 2021-02-13T00:50:08Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.17 psi 2021-02-13T00:50:08Z- handleDepth()-Depth_Keller msg: 1613177408951 epoch sec; seqNo:16168; depth 27.89 2021-02-13T00:50:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177408.96 epoch sec; seqNo:16168; 4060164.09 north; 593219.38 east 2021-02-13T00:50:09Z- AHRS_M2 msg: 1613177409.35 epoch sec; seqNo:16043 2021-02-13T00:50:09Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.17 psi 2021-02-13T00:50:09Z- handleDepth()-Depth_Keller msg: 1613177409353 epoch sec; seqNo:16169; depth 28.00 2021-02-13T00:50:09Z- handleDvl() - RDI_Pathfinder msg: 1613177409.37 epoch sec; seqNo:7766 2021-02-13T00:50:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 82.84 , 61.30 , 97.98 2021-02-13T00:50:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , 0.04 2021-02-13T00:50:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177409.38 epoch sec; seqNo:16169; 4060164.09 north; 593219.38 east 2021-02-13T00:50:09Z- AHRS_M2 msg: 1613177409.75 epoch sec; seqNo:16044 2021-02-13T00:50:09Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.17 psi 2021-02-13T00:50:09Z- handleDepth()-Depth_Keller msg: 1613177409761 epoch sec; seqNo:16170; depth 28.13 2021-02-13T00:50:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177409.77 epoch sec; seqNo:16170; 4060164.47 north; 593219.38 east 2021-02-13T00:50:10Z- AHRS_M2 msg: 1613177410.16 epoch sec; seqNo:16045 2021-02-13T00:50:10Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.17 psi 2021-02-13T00:50:10Z- handleDepth()-Depth_Keller msg: 1613177410164 epoch sec; seqNo:16171; depth 28.26 2021-02-13T00:50:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177410.17 epoch sec; seqNo:16171; 4060164.85 north; 593219.38 east 2021-02-13T00:50:10Z- AHRS_M2 msg: 1613177410.56 epoch sec; seqNo:16046 2021-02-13T00:50:10Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.17 psi 2021-02-13T00:50:10Z- handleDepth()-Depth_Keller msg: 1613177410569 epoch sec; seqNo:16172; depth 28.37 2021-02-13T00:50:10Z- handleDvl() - RDI_Pathfinder msg: 1613177410.59 epoch sec; seqNo:7767 2021-02-13T00:50:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.03 , 82.84 , 61.30 , 91.49 2021-02-13T00:50:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , 0.04 2021-02-13T00:50:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177410.61 epoch sec; seqNo:16172; 4060165.22 north; 593219.37 east 2021-02-13T00:50:10Z- AHRS_M2 msg: 1613177410.97 epoch sec; seqNo:16047 2021-02-13T00:50:10Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.17 psi 2021-02-13T00:50:10Z- handleDepth()-Depth_Keller msg: 1613177410970 epoch sec; seqNo:16173; depth 28.50 2021-02-13T00:50:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177410.98 epoch sec; seqNo:16173; 4060165.22 north; 593219.37 east 2021-02-13T00:50:11Z- AHRS_M2 msg: 1613177411.37 epoch sec; seqNo:16048 2021-02-13T00:50:11Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.17 psi 2021-02-13T00:50:11Z- handleDepth()-Depth_Keller msg: 1613177411375 epoch sec; seqNo:16174; depth 28.60 2021-02-13T00:50:11Z- handleDvl() - RDI_Pathfinder msg: 1613177411.39 epoch sec; seqNo:7768 2021-02-13T00:50:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.03 , 0.00 , 60.08 , 97.98 2021-02-13T00:50:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.04 2021-02-13T00:50:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177411.40 epoch sec; seqNo:16174; 4060165.62 north; 593220.59 east 2021-02-13T00:50:11Z- LcmTrn::handleMessages() - handling 41 messages took 0.67 ms and 128 calls... 2021-02-13T00:50:11Z- waiting for 1613177409.56, time is 1613177411.40 2021-02-13T00:50:11Z- LcmTrn::updateTrn() equating pose 1613177411.40 and meas times 1613177411.40 2021-02-13T00:50:11Z- LcmTrn::updateTrn() >>>> pose time 1613177411.40, meas time 1613177411.40 <<<< 2021-02-13T00:50:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177411.40 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:50:11Z- TerrainNav::motionUpdate() - currEstimate = (4060166, 593220.59, 28.60) 2021-02-13T00:50:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:50:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:50:11Z- TNavBF::Projecting Measurements 2021-02-13T00:50:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:50:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:50:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:50:11Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:50:11Z- TNavFilter::3 good beams. 2021-02-13T00:50:11Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:50:11Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:50:11Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:50:11Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:50:11Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:50:11Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:50:11Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:50:11Z- TNavBF::mmse selected from filter -1 2021-02-13T00:50:11Z- TerrainMapOctree: (vehN, vehE ) = (4060165.62, 593220.59). 2021-02-13T00:50:11Z- TerrainMapOctree: Min Distance = 1495.18. 2021-02-13T00:50:11Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:50:11Z- There is no specific good beam that all particles have in common. 2021-02-13T00:50:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:50:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.32 K: 0.129 ExpectedMeasDiff: nan currMeas.covariance: 7.19 2021-02-13T00:50:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:50:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.133 ExpectedMeasDiff: nan currMeas.covariance: 5.77 2021-02-13T00:50:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:50:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:50:11Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:50:11Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:50:11Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:50:11Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:50:11Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:50:11Z- Filter 0 No beams used in subcloud update 2021-02-13T00:50:11Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:50:11Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:50:11Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 685.9 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:50:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177411.40 sec. 2021-02-13T00:50:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:50:11Z- TNavBF::mmse selected from filter -1 2021-02-13T00:50:11Z- variances([0], [2], and psi_berg) = 685.866268 832.991048 0.000000 2021-02-13T00:50:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:50:11Z- LcmTrn::publishEstimates() - MLE : 4060165.12 593242.09 28.60 2021-02-13T00:50:11Z- LcmTrn::publishEstimates() - MMSE : 4060165.62 593220.59 28.60 2021-02-13T00:50:11Z- LcmTrn::publishEstimates() - COVAR: 685.87 832.99 0.94 0.00 2021-02-13T00:50:11Z- AHRS_M2 msg: 1613177411.77 epoch sec; seqNo:16049 2021-02-13T00:50:11Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.17 psi 2021-02-13T00:50:11Z- handleDepth()-Depth_Keller msg: 1613177411778 epoch sec; seqNo:16175; depth 28.73 2021-02-13T00:50:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177411.79 epoch sec; seqNo:16175; 4060166.00 north; 593220.58 east 2021-02-13T00:50:12Z- AHRS_M2 msg: 1613177412.17 epoch sec; seqNo:16050 2021-02-13T00:50:12Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.17 psi 2021-02-13T00:50:12Z- handleDepth()-Depth_Keller msg: 1613177412190 epoch sec; seqNo:16176; depth 28.84 2021-02-13T00:50:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177412.20 epoch sec; seqNo:16176; 4060166.37 north; 593220.58 east 2021-02-13T00:50:12Z- AHRS_M2 msg: 1613177412.58 epoch sec; seqNo:16051 2021-02-13T00:50:12Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-13T00:50:12Z- handleDepth()-Depth_Keller msg: 1613177412584 epoch sec; seqNo:16177; depth 28.98 2021-02-13T00:50:12Z- handleDvl() - RDI_Pathfinder msg: 1613177412.60 epoch sec; seqNo:7769 2021-02-13T00:50:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.80 , 82.84 , 60.90 , 91.49 2021-02-13T00:50:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.03 , 0.04 2021-02-13T00:50:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177412.62 epoch sec; seqNo:16177; 4060166.37 north; 593220.58 east 2021-02-13T00:50:12Z- AHRS_M2 msg: 1613177412.98 epoch sec; seqNo:16052 2021-02-13T00:50:12Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-13T00:50:12Z- handleDepth()-Depth_Keller msg: 1613177412993 epoch sec; seqNo:16178; depth 29.08 2021-02-13T00:50:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177413.00 epoch sec; seqNo:16178; 4060166.75 north; 593220.58 east 2021-02-13T00:50:13Z- AHRS_M2 msg: 1613177413.39 epoch sec; seqNo:16053 2021-02-13T00:50:13Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-13T00:50:13Z- handleDepth()-Depth_Keller msg: 1613177413394 epoch sec; seqNo:16179; depth 29.21 2021-02-13T00:50:13Z- handleDvl() - RDI_Pathfinder msg: 1613177413.40 epoch sec; seqNo:7770 2021-02-13T00:50:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.62 , 82.84 , 60.90 , 97.98 2021-02-13T00:50:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , 0.04 2021-02-13T00:50:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177413.42 epoch sec; seqNo:16179; 4060167.13 north; 593220.57 east 2021-02-13T00:50:13Z- AHRS_M2 msg: 1613177413.79 epoch sec; seqNo:16054 2021-02-13T00:50:13Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.17 psi 2021-02-13T00:50:13Z- handleDepth()-Depth_Keller msg: 1613177413796 epoch sec; seqNo:16180; depth 29.33 2021-02-13T00:50:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177413.81 epoch sec; seqNo:16180; 4060167.52 north; 593221.79 east 2021-02-13T00:50:14Z- AHRS_M2 msg: 1613177414.20 epoch sec; seqNo:16055 2021-02-13T00:50:14Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.18 psi 2021-02-13T00:50:14Z- handleDepth()-Depth_Keller msg: 1613177414203 epoch sec; seqNo:16181; depth 29.46 2021-02-13T00:50:14Z- handleDvl() - RDI_Pathfinder msg: 1613177414.21 epoch sec; seqNo:7771 2021-02-13T00:50:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.98 , 81.75 , 60.08 , 0.00 2021-02-13T00:50:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.00 , 0.03 2021-02-13T00:50:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177414.23 epoch sec; seqNo:16181; 4060167.52 north; 593221.79 east 2021-02-13T00:50:14Z- AHRS_M2 msg: 1613177414.60 epoch sec; seqNo:16056 2021-02-13T00:50:14Z- AHRS_M2 msg: 0.64 phi; -0.28 theta; -0.18 psi 2021-02-13T00:50:14Z- handleDepth()-Depth_Keller msg: 1613177414609 epoch sec; seqNo:16182; depth 29.60 2021-02-13T00:50:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177414.62 epoch sec; seqNo:16182; 4060167.90 north; 593221.78 east 2021-02-13T00:50:15Z- AHRS_M2 msg: 1613177415.01 epoch sec; seqNo:16057 2021-02-13T00:50:15Z- AHRS_M2 msg: 0.63 phi; -0.25 theta; -0.18 psi 2021-02-13T00:50:15Z- handleDepth()-Depth_Keller msg: 1613177415009 epoch sec; seqNo:16183; depth 29.74 2021-02-13T00:50:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177415.02 epoch sec; seqNo:16183; 4060168.28 north; 593221.78 east 2021-02-13T00:50:15Z- AHRS_M2 msg: 1613177415.41 epoch sec; seqNo:16058 2021-02-13T00:50:15Z- AHRS_M2 msg: 0.63 phi; -0.21 theta; -0.17 psi 2021-02-13T00:50:15Z- handleDepth()-Depth_Keller msg: 1613177415412 epoch sec; seqNo:16184; depth 29.85 2021-02-13T00:50:15Z- handleDvl() - RDI_Pathfinder msg: 1613177415.43 epoch sec; seqNo:7772 2021-02-13T00:50:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.16 , 80.67 , 60.49 , 0.00 2021-02-13T00:50:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.02 , 0.01 2021-02-13T00:50:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177415.44 epoch sec; seqNo:16184; 4060168.66 north; 593221.78 east 2021-02-13T00:50:15Z- AHRS_M2 msg: 1613177415.81 epoch sec; seqNo:16059 2021-02-13T00:50:15Z- AHRS_M2 msg: 0.62 phi; -0.17 theta; -0.16 psi 2021-02-13T00:50:15Z- handleDepth()-Depth_Keller msg: 1613177415816 epoch sec; seqNo:16185; depth 29.97 2021-02-13T00:50:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177415.83 epoch sec; seqNo:16185; 4060169.05 north; 593222.99 east 2021-02-13T00:50:16Z- AHRS_M2 msg: 1613177416.22 epoch sec; seqNo:16060 2021-02-13T00:50:16Z- AHRS_M2 msg: 0.62 phi; -0.13 theta; -0.16 psi 2021-02-13T00:50:16Z- handleDepth()-Depth_Keller msg: 1613177416227 epoch sec; seqNo:16186; depth 30.08 2021-02-13T00:50:16Z- handleDvl() - RDI_Pathfinder msg: 1613177416.24 epoch sec; seqNo:7773 2021-02-13T00:50:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.53 , 78.51 , 62.94 , 0.00 2021-02-13T00:50:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , -0.02 2021-02-13T00:50:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177416.25 epoch sec; seqNo:16186; 4060169.05 north; 593222.99 east 2021-02-13T00:50:16Z- AHRS_M2 msg: 1613177416.62 epoch sec; seqNo:16061 2021-02-13T00:50:16Z- AHRS_M2 msg: 0.61 phi; -0.08 theta; -0.16 psi 2021-02-13T00:50:16Z- handleDepth()-Depth_Keller msg: 1613177416634 epoch sec; seqNo:16187; depth 30.19 2021-02-13T00:50:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177416.64 epoch sec; seqNo:16187; 4060169.43 north; 593222.99 east 2021-02-13T00:50:16Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-13T00:50:16Z- waiting for 1613177414.40, time is 1613177416.64 2021-02-13T00:50:16Z- LcmTrn::updateTrn() >>>> pose time 1613177416.64, meas time 1613177416.24 <<<< 2021-02-13T00:50:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177416.24 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:50:16Z- TerrainNav::motionUpdate() - currEstimate = (4060169, 593222.99, 30.19) 2021-02-13T00:50:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:50:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:50:16Z- TNavBF::Projecting Measurements 2021-02-13T00:50:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:50:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:50:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:50:16Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:50:16Z- TNavFilter::3 good beams. 2021-02-13T00:50:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:50:16Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:50:16Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:50:16Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:50:16Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:50:16Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:50:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:50:16Z- TNavBF::mmse selected from filter -1 2021-02-13T00:50:16Z- TerrainMapOctree: (vehN, vehE ) = (4060169.14, 593222.80). 2021-02-13T00:50:16Z- TerrainMapOctree: Min Distance = 1491.10. 2021-02-13T00:50:16Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:50:16Z- There is no specific good beam that all particles have in common. 2021-02-13T00:50:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:50:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.126 ExpectedMeasDiff: nan currMeas.covariance: 6.26 2021-02-13T00:50:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:50:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:50:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:50:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 6.34 2021-02-13T00:50:16Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:50:16Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:50:16Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:50:16Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:50:16Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:50:16Z- Filter 0 No beams used in subcloud update 2021-02-13T00:50:16Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:50:16Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:50:16Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 685.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:50:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177416.24 sec. 2021-02-13T00:50:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:50:16Z- TNavBF::mmse selected from filter -1 2021-02-13T00:50:16Z- variances([0], [2], and psi_berg) = 685.805000 832.991020 0.000000 2021-02-13T00:50:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:50:16Z- LcmTrn::publishEstimates() - MLE : 4060168.93 593244.49 30.19 2021-02-13T00:50:16Z- LcmTrn::publishEstimates() - MMSE : 4060169.43 593222.99 30.19 2021-02-13T00:50:16Z- LcmTrn::publishEstimates() - COVAR: 685.81 832.99 0.85 0.00 2021-02-13T00:50:17Z- AHRS_M2 msg: 1613177417.02 epoch sec; seqNo:16062 2021-02-13T00:50:17Z- AHRS_M2 msg: 0.60 phi; -0.04 theta; -0.16 psi 2021-02-13T00:50:17Z- handleDepth()-Depth_Keller msg: 1613177417033 epoch sec; seqNo:16188; depth 30.29 2021-02-13T00:50:17Z- handleDvl() - RDI_Pathfinder msg: 1613177417.05 epoch sec; seqNo:7774 2021-02-13T00:50:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.47 , 0.00 , 0.00 , 0.00 2021-02-13T00:50:17Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:50:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177417.06 epoch sec; seqNo:16188; 4060169.81 north; 593222.98 east 2021-02-13T00:50:17Z- AHRS_M2 msg: 1613177417.43 epoch sec; seqNo:16063 2021-02-13T00:50:17Z- AHRS_M2 msg: 0.59 phi; 0.00 theta; -0.16 psi 2021-02-13T00:50:17Z- handleDepth()-Depth_Keller msg: 1613177417432 epoch sec; seqNo:16189; depth 30.35 2021-02-13T00:50:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177417.44 epoch sec; seqNo:16189; 4060170.19 north; 593222.98 east 2021-02-13T00:50:17Z- AHRS_M2 msg: 1613177417.83 epoch sec; seqNo:16064 2021-02-13T00:50:17Z- AHRS_M2 msg: 0.58 phi; 0.05 theta; -0.16 psi 2021-02-13T00:50:17Z- handleDepth()-Depth_Keller msg: 1613177417837 epoch sec; seqNo:16190; depth 30.41 2021-02-13T00:50:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177417.85 epoch sec; seqNo:16190; 4060170.57 north; 593222.98 east 2021-02-13T00:50:18Z- AHRS_M2 msg: 1613177418.24 epoch sec; seqNo:16065 2021-02-13T00:50:18Z- AHRS_M2 msg: 0.57 phi; 0.08 theta; -0.16 psi 2021-02-13T00:50:18Z- handleDepth()-Depth_Keller msg: 1613177418250 epoch sec; seqNo:16191; depth 30.45 2021-02-13T00:50:18Z- handleDvl() - RDI_Pathfinder msg: 1613177418.26 epoch sec; seqNo:7775 2021-02-13T00:50:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.18 , 0.00 , 0.00 , 65.59 2021-02-13T00:50:18Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:50:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177418.27 epoch sec; seqNo:16191; 4060170.96 north; 593224.19 east 2021-02-13T00:50:18Z- AHRS_M2 msg: 1613177418.64 epoch sec; seqNo:16066 2021-02-13T00:50:18Z- AHRS_M2 msg: 0.56 phi; 0.12 theta; -0.16 psi 2021-02-13T00:50:18Z- handleDepth()-Depth_Keller msg: 1613177418644 epoch sec; seqNo:16192; depth 30.47 2021-02-13T00:50:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177418.65 epoch sec; seqNo:16192; 4060170.96 north; 593224.19 east 2021-02-13T00:50:19Z- AHRS_M2 msg: 1613177419.04 epoch sec; seqNo:16067 2021-02-13T00:50:19Z- AHRS_M2 msg: 0.55 phi; 0.15 theta; -0.17 psi 2021-02-13T00:50:19Z- handleDepth()-Depth_Keller msg: 1613177419048 epoch sec; seqNo:16193; depth 30.48 2021-02-13T00:50:19Z- handleDvl() - RDI_Pathfinder msg: 1613177419.06 epoch sec; seqNo:7776 2021-02-13T00:50:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.61 , 0.00 , 0.00 , 62.61 2021-02-13T00:50:19Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:50:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177419.07 epoch sec; seqNo:16193; 4060171.34 north; 593224.19 east 2021-02-13T00:50:19Z- AHRS_M2 msg: 1613177419.45 epoch sec; seqNo:16068 2021-02-13T00:50:19Z- AHRS_M2 msg: 0.54 phi; 0.18 theta; -0.17 psi 2021-02-13T00:50:19Z- handleDepth()-Depth_Keller msg: 1613177419458 epoch sec; seqNo:16194; depth 30.47 2021-02-13T00:50:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177419.47 epoch sec; seqNo:16194; 4060171.72 north; 593224.18 east 2021-02-13T00:50:19Z- AHRS_M2 msg: 1613177419.85 epoch sec; seqNo:16069 2021-02-13T00:50:19Z- AHRS_M2 msg: 0.54 phi; 0.21 theta; -0.17 psi 2021-02-13T00:50:19Z- handleDepth()-Depth_Keller msg: 1613177419856 epoch sec; seqNo:16195; depth 30.46 2021-02-13T00:50:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177419.86 epoch sec; seqNo:16195; 4060172.10 north; 593224.18 east 2021-02-13T00:50:20Z- AHRS_M2 msg: 1613177420.26 epoch sec; seqNo:16070 2021-02-13T00:50:20Z- AHRS_M2 msg: 0.53 phi; 0.24 theta; -0.17 psi 2021-02-13T00:50:20Z- handleDepth()-Depth_Keller msg: 1613177420265 epoch sec; seqNo:16196; depth 30.43 2021-02-13T00:50:20Z- handleDvl() - RDI_Pathfinder msg: 1613177420.28 epoch sec; seqNo:7777 2021-02-13T00:50:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.46 , 0.00 , 0.00 , 61.33 2021-02-13T00:50:20Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:50:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177420.31 epoch sec; seqNo:16196; 4060172.48 north; 593224.18 east 2021-02-13T00:50:20Z- AHRS_M2 msg: 1613177420.66 epoch sec; seqNo:16071 2021-02-13T00:50:20Z- AHRS_M2 msg: 0.52 phi; 0.26 theta; -0.17 psi 2021-02-13T00:50:20Z- handleDepth()-Depth_Keller msg: 1613177420665 epoch sec; seqNo:16197; depth 30.39 2021-02-13T00:50:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177420.68 epoch sec; seqNo:16197; 4060172.48 north; 593224.18 east 2021-02-13T00:50:21Z- AHRS_M2 msg: 1613177421.06 epoch sec; seqNo:16072 2021-02-13T00:50:21Z- AHRS_M2 msg: 0.52 phi; 0.28 theta; -0.17 psi 2021-02-13T00:50:21Z- handleDepth()-Depth_Keller msg: 1613177421070 epoch sec; seqNo:16198; depth 30.34 2021-02-13T00:50:21Z- handleDvl() - RDI_Pathfinder msg: 1613177421.08 epoch sec; seqNo:7778 2021-02-13T00:50:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:50:21Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:50:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177421.09 epoch sec; seqNo:16198; 4060172.85 north; 593224.17 east 2021-02-13T00:50:21Z- AHRS_M2 msg: 1613177421.47 epoch sec; seqNo:16073 2021-02-13T00:50:21Z- AHRS_M2 msg: 0.52 phi; 0.29 theta; -0.17 psi 2021-02-13T00:50:21Z- handleDepth()-Depth_Keller msg: 1613177421472 epoch sec; seqNo:16199; depth 30.27 2021-02-13T00:50:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177421.49 epoch sec; seqNo:16199; 4060173.25 north; 593225.39 east 2021-02-13T00:50:21Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-13T00:50:21Z- waiting for 1613177419.64, time is 1613177421.49 2021-02-13T00:50:21Z- LcmTrn::updateTrn() >>>> pose time 1613177421.49, meas time 1613177421.08 <<<< 2021-02-13T00:50:21Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177421.08, ping # 0. 2021-02-13T00:50:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177421.08 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:50:21Z- TerrainNav::motionUpdate() - currEstimate = (4060173, 593225.39, 30.27) 2021-02-13T00:50:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:50:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:50:21Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:50:21Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:50:21Z- TNavBF::Projecting Measurements 2021-02-13T00:50:21Z- TNavFilter::Measurement from time = 1613177421.08 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:50:21Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:50:21Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:50:21Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:50:21Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:50:21Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:50:21Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:50:21Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:50:21Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 685.8 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:50:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:50:21Z- TNavBF::mmse selected from filter -1 2021-02-13T00:50:21Z- variances([0], [2], and psi_berg) = 685.743293 832.990993 0.000000 2021-02-13T00:50:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:50:21Z- LcmTrn::publishEstimates() - MLE : 4060172.75 593246.89 30.27 2021-02-13T00:50:21Z- LcmTrn::publishEstimates() - MMSE : 4060173.25 593225.39 30.27 2021-02-13T00:50:21Z- LcmTrn::publishEstimates() - COVAR: 685.74 832.99 1.08 0.00 2021-02-13T00:50:21Z- AHRS_M2 msg: 1613177421.88 epoch sec; seqNo:16074 2021-02-13T00:50:21Z- AHRS_M2 msg: 0.51 phi; 0.31 theta; -0.17 psi 2021-02-13T00:50:21Z- handleDepth()-Depth_Keller msg: 1613177421884 epoch sec; seqNo:16200; depth 30.21 2021-02-13T00:50:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177421.89 epoch sec; seqNo:16200; 4060173.63 north; 593225.38 east 2021-02-13T00:50:22Z- AHRS_M2 msg: 1613177422.28 epoch sec; seqNo:16075 2021-02-13T00:50:22Z- AHRS_M2 msg: 0.51 phi; 0.32 theta; -0.17 psi 2021-02-13T00:50:22Z- handleDepth()-Depth_Keller msg: 1613177422280 epoch sec; seqNo:16201; depth 30.14 2021-02-13T00:50:22Z- handleDvl() - RDI_Pathfinder msg: 1613177422.30 epoch sec; seqNo:7779 2021-02-13T00:50:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.80 , 80.67 , 88.25 , 59.26 2021-02-13T00:50:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , -0.03 2021-02-13T00:50:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177422.31 epoch sec; seqNo:16201; 4060174.00 north; 593225.38 east 2021-02-13T00:50:22Z- AHRS_M2 msg: 1613177422.68 epoch sec; seqNo:16076 2021-02-13T00:50:22Z- AHRS_M2 msg: 0.51 phi; 0.33 theta; -0.16 psi 2021-02-13T00:50:22Z- handleDepth()-Depth_Keller msg: 1613177422684 epoch sec; seqNo:16202; depth 30.04 2021-02-13T00:50:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177422.71 epoch sec; seqNo:16202; 4060174.00 north; 593225.38 east 2021-02-13T00:50:23Z- AHRS_M2 msg: 1613177423.09 epoch sec; seqNo:16077 2021-02-13T00:50:23Z- AHRS_M2 msg: 0.52 phi; 0.35 theta; -0.15 psi 2021-02-13T00:50:23Z- handleDepth()-Depth_Keller msg: 1613177423106 epoch sec; seqNo:16203; depth 29.96 2021-02-13T00:50:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177423.12 epoch sec; seqNo:16203; 4060174.38 north; 593225.38 east 2021-02-13T00:50:23Z- AHRS_M2 msg: 1613177423.49 epoch sec; seqNo:16078 2021-02-13T00:50:23Z- AHRS_M2 msg: 0.52 phi; 0.36 theta; -0.14 psi 2021-02-13T00:50:23Z- handleDepth()-Depth_Keller msg: 1613177423496 epoch sec; seqNo:16204; depth 29.86 2021-02-13T00:50:23Z- handleDvl() - RDI_Pathfinder msg: 1613177423.51 epoch sec; seqNo:7780 2021-02-13T00:50:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.62 , 80.67 , 97.98 , 59.26 2021-02-13T00:50:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , -0.03 2021-02-13T00:50:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177423.53 epoch sec; seqNo:16204; 4060174.76 north; 593225.37 east 2021-02-13T00:50:23Z- AHRS_M2 msg: 1613177423.89 epoch sec; seqNo:16079 2021-02-13T00:50:23Z- AHRS_M2 msg: 0.52 phi; 0.36 theta; -0.14 psi 2021-02-13T00:50:23Z- handleDepth()-Depth_Keller msg: 1613177423897 epoch sec; seqNo:16205; depth 29.75 2021-02-13T00:50:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177423.91 epoch sec; seqNo:16205; 4060175.14 north; 593225.37 east 2021-02-13T00:50:24Z- AHRS_M2 msg: 1613177424.30 epoch sec; seqNo:16080 2021-02-13T00:50:24Z- AHRS_M2 msg: 0.52 phi; 0.37 theta; -0.14 psi 2021-02-13T00:50:24Z- handleDepth()-Depth_Keller msg: 1613177424300 epoch sec; seqNo:16206; depth 29.66 2021-02-13T00:50:24Z- handleDvl() - RDI_Pathfinder msg: 1613177424.32 epoch sec; seqNo:7781 2021-02-13T00:50:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.62 , 80.67 , 97.98 , 60.08 2021-02-13T00:50:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , -0.01 2021-02-13T00:50:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177424.33 epoch sec; seqNo:16206; 4060175.14 north; 593225.37 east 2021-02-13T00:50:24Z- AHRS_M2 msg: 1613177424.70 epoch sec; seqNo:16081 2021-02-13T00:50:24Z- AHRS_M2 msg: 0.52 phi; 0.37 theta; -0.14 psi 2021-02-13T00:50:24Z- handleDepth()-Depth_Keller msg: 1613177424704 epoch sec; seqNo:16207; depth 29.53 2021-02-13T00:50:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177424.71 epoch sec; seqNo:16207; 4060175.53 north; 593226.58 east 2021-02-13T00:50:25Z- AHRS_M2 msg: 1613177425.10 epoch sec; seqNo:16082 2021-02-13T00:50:25Z- AHRS_M2 msg: 0.53 phi; 0.37 theta; -0.14 psi 2021-02-13T00:50:25Z- handleDepth()-Depth_Keller msg: 1613177425108 epoch sec; seqNo:16208; depth 29.44 2021-02-13T00:50:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177425.12 epoch sec; seqNo:16208; 4060175.91 north; 593226.58 east 2021-02-13T00:50:25Z- AHRS_M2 msg: 1613177425.51 epoch sec; seqNo:16083 2021-02-13T00:50:25Z- AHRS_M2 msg: 0.53 phi; 0.37 theta; -0.14 psi 2021-02-13T00:50:25Z- handleDepth()-Depth_Keller msg: 1613177425517 epoch sec; seqNo:16209; depth 29.32 2021-02-13T00:50:25Z- handleDvl() - RDI_Pathfinder msg: 1613177425.53 epoch sec; seqNo:7782 2021-02-13T00:50:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.25 , 0.00 , 97.98 , 60.08 2021-02-13T00:50:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.07 , 0.00 2021-02-13T00:50:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177425.54 epoch sec; seqNo:16209; 4060176.29 north; 593226.58 east 2021-02-13T00:50:25Z- AHRS_M2 msg: 1613177425.91 epoch sec; seqNo:16084 2021-02-13T00:50:25Z- AHRS_M2 msg: 0.53 phi; 0.36 theta; -0.14 psi 2021-02-13T00:50:25Z- handleDepth()-Depth_Keller msg: 1613177425916 epoch sec; seqNo:16210; depth 29.21 2021-02-13T00:50:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177425.93 epoch sec; seqNo:16210; 4060176.67 north; 593226.57 east 2021-02-13T00:50:26Z- AHRS_M2 msg: 1613177426.33 epoch sec; seqNo:16085 2021-02-13T00:50:26Z- AHRS_M2 msg: 0.53 phi; 0.36 theta; -0.14 psi 2021-02-13T00:50:26Z- handleDepth()-Depth_Keller msg: 1613177426343 epoch sec; seqNo:16211; depth 29.10 2021-02-13T00:50:26Z- handleDvl() - RDI_Pathfinder msg: 1613177426.35 epoch sec; seqNo:7783 2021-02-13T00:50:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 81.75 , 97.98 , 59.67 2021-02-13T00:50:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.03 2021-02-13T00:50:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177426.37 epoch sec; seqNo:16211; 4060176.67 north; 593226.57 east 2021-02-13T00:50:26Z- AHRS_M2 msg: 1613177426.72 epoch sec; seqNo:16086 2021-02-13T00:50:26Z- AHRS_M2 msg: 0.53 phi; 0.36 theta; -0.13 psi 2021-02-13T00:50:26Z- handleDepth()-Depth_Keller msg: 1613177426724 epoch sec; seqNo:16212; depth 28.97 2021-02-13T00:50:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177426.73 epoch sec; seqNo:16212; 4060177.05 north; 593226.57 east 2021-02-13T00:50:26Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-13T00:50:26Z- waiting for 1613177424.49, time is 1613177426.73 2021-02-13T00:50:26Z- LcmTrn::updateTrn() >>>> pose time 1613177426.73, meas time 1613177426.35 <<<< 2021-02-13T00:50:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177426.35 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:50:26Z- TerrainNav::motionUpdate() - currEstimate = (4060177, 593226.57, 28.97) 2021-02-13T00:50:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:50:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:50:26Z- TNavBF::Projecting Measurements 2021-02-13T00:50:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:50:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:50:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:50:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:50:26Z- TNavFilter::4 good beams. 2021-02-13T00:50:26Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:50:26Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:50:26Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:50:26Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:50:26Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:50:26Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:50:26Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:50:26Z- TNavBF::mmse selected from filter -1 2021-02-13T00:50:26Z- TerrainMapOctree: (vehN, vehE ) = (4060176.77, 593226.48). 2021-02-13T00:50:26Z- TerrainMapOctree: Min Distance = 1483.04. 2021-02-13T00:50:26Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:50:26Z- There is no specific good beam that all particles have in common. 2021-02-13T00:50:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:50:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:50:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:50:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 10.69 2021-02-13T00:50:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:50:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:50:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:50:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.117 ExpectedMeasDiff: nan currMeas.covariance: 5.70 2021-02-13T00:50:26Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:50:26Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:50:26Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:50:26Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:50:26Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:50:26Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:50:26Z- Filter 0 No beams used in subcloud update 2021-02-13T00:50:26Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:50:26Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:50:26Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 685.7 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:50:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177426.35 sec. 2021-02-13T00:50:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:50:26Z- TNavBF::mmse selected from filter -1 2021-02-13T00:50:26Z- variances([0], [2], and psi_berg) = 685.686029 832.990967 0.000000 2021-02-13T00:50:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:50:26Z- LcmTrn::publishEstimates() - MLE : 4060176.55 593248.07 28.97 2021-02-13T00:50:26Z- LcmTrn::publishEstimates() - MMSE : 4060177.05 593226.57 28.97 2021-02-13T00:50:26Z- LcmTrn::publishEstimates() - COVAR: 685.69 832.99 0.89 0.00 2021-02-13T00:50:27Z- AHRS_M2 msg: 1613177427.13 epoch sec; seqNo:16087 2021-02-13T00:50:27Z- AHRS_M2 msg: 0.54 phi; 0.36 theta; -0.13 psi 2021-02-13T00:50:27Z- handleDepth()-Depth_Keller msg: 1613177427133 epoch sec; seqNo:16213; depth 28.86 2021-02-13T00:50:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177427.14 epoch sec; seqNo:16213; 4060177.43 north; 593226.56 east 2021-02-13T00:50:27Z- AHRS_M2 msg: 1613177427.53 epoch sec; seqNo:16088 2021-02-13T00:50:27Z- AHRS_M2 msg: 0.55 phi; 0.36 theta; -0.13 psi 2021-02-13T00:50:27Z- handleDepth()-Depth_Keller msg: 1613177427534 epoch sec; seqNo:16214; depth 28.74 2021-02-13T00:50:27Z- handleDvl() - RDI_Pathfinder msg: 1613177427.55 epoch sec; seqNo:7784 2021-02-13T00:50:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.25 , 80.67 , 97.98 , 60.90 2021-02-13T00:50:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , 0.01 2021-02-13T00:50:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177427.56 epoch sec; seqNo:16214; 4060177.82 north; 593227.78 east 2021-02-13T00:50:27Z- AHRS_M2 msg: 1613177427.93 epoch sec; seqNo:16089 2021-02-13T00:50:27Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.12 psi 2021-02-13T00:50:27Z- handleDepth()-Depth_Keller msg: 1613177427935 epoch sec; seqNo:16215; depth 28.61 2021-02-13T00:50:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177427.95 epoch sec; seqNo:16215; 4060177.82 north; 593227.78 east 2021-02-13T00:50:28Z- AHRS_M2 msg: 1613177428.34 epoch sec; seqNo:16090 2021-02-13T00:50:28Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.12 psi 2021-02-13T00:50:28Z- handleDepth()-Depth_Keller msg: 1613177428342 epoch sec; seqNo:16216; depth 28.50 2021-02-13T00:50:28Z- handleDvl() - RDI_Pathfinder msg: 1613177428.35 epoch sec; seqNo:7785 2021-02-13T00:50:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 80.67 , 97.98 , 61.30 2021-02-13T00:50:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , -0.02 2021-02-13T00:50:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177428.36 epoch sec; seqNo:16216; 4060178.20 north; 593227.78 east 2021-02-13T00:50:28Z- AHRS_M2 msg: 1613177428.74 epoch sec; seqNo:16091 2021-02-13T00:50:28Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.12 psi 2021-02-13T00:50:28Z- handleDepth()-Depth_Keller msg: 1613177428744 epoch sec; seqNo:16217; depth 28.37 2021-02-13T00:50:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177428.76 epoch sec; seqNo:16217; 4060178.58 north; 593227.77 east 2021-02-13T00:50:29Z- AHRS_M2 msg: 1613177429.15 epoch sec; seqNo:16092 2021-02-13T00:50:29Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.12 psi 2021-02-13T00:50:29Z- handleDepth()-Depth_Keller msg: 1613177429152 epoch sec; seqNo:16218; depth 28.28 2021-02-13T00:50:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177429.16 epoch sec; seqNo:16218; 4060178.58 north; 593227.77 east 2021-02-13T00:50:29Z- AHRS_M2 msg: 1613177429.55 epoch sec; seqNo:16093 2021-02-13T00:50:29Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.12 psi 2021-02-13T00:50:29Z- handleDepth()-Depth_Keller msg: 1613177429557 epoch sec; seqNo:16219; depth 28.15 2021-02-13T00:50:29Z- handleDvl() - RDI_Pathfinder msg: 1613177429.57 epoch sec; seqNo:7786 2021-02-13T00:50:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 80.67 , 97.98 , 61.30 2021-02-13T00:50:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , -0.07 2021-02-13T00:50:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177429.58 epoch sec; seqNo:16219; 4060178.96 north; 593227.77 east 2021-02-13T00:50:29Z- AHRS_M2 msg: 1613177429.95 epoch sec; seqNo:16094 2021-02-13T00:50:29Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.12 psi 2021-02-13T00:50:29Z- handleDepth()-Depth_Keller msg: 1613177429965 epoch sec; seqNo:16220; depth 28.02 2021-02-13T00:50:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177429.98 epoch sec; seqNo:16220; 4060179.33 north; 593227.76 east 2021-02-13T00:50:30Z- AHRS_M2 msg: 1613177430.36 epoch sec; seqNo:16095 2021-02-13T00:50:30Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.13 psi 2021-02-13T00:50:30Z- handleDepth()-Depth_Keller msg: 1613177430365 epoch sec; seqNo:16221; depth 27.89 2021-02-13T00:50:30Z- handleDvl() - RDI_Pathfinder msg: 1613177430.38 epoch sec; seqNo:7787 2021-02-13T00:50:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 81.75 , 97.98 , 62.53 2021-02-13T00:50:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.10 , -0.05 2021-02-13T00:50:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177430.40 epoch sec; seqNo:16221; 4060179.71 north; 593227.76 east 2021-02-13T00:50:30Z- AHRS_M2 msg: 1613177430.76 epoch sec; seqNo:16096 2021-02-13T00:50:30Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.13 psi 2021-02-13T00:50:30Z- handleDepth()-Depth_Keller msg: 1613177430769 epoch sec; seqNo:16222; depth 27.77 2021-02-13T00:50:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177430.78 epoch sec; seqNo:16222; 4060179.73 north; 593228.98 east 2021-02-13T00:50:31Z- AHRS_M2 msg: 1613177431.17 epoch sec; seqNo:16097 2021-02-13T00:50:31Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.13 psi 2021-02-13T00:50:31Z- handleDepth()-Depth_Keller msg: 1613177431177 epoch sec; seqNo:16223; depth 27.65 2021-02-13T00:50:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177431.18 epoch sec; seqNo:16223; 4060180.11 north; 593228.98 east 2021-02-13T00:50:31Z- AHRS_M2 msg: 1613177431.57 epoch sec; seqNo:16098 2021-02-13T00:50:31Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.13 psi 2021-02-13T00:50:31Z- handleDepth()-Depth_Keller msg: 1613177431582 epoch sec; seqNo:16224; depth 27.53 2021-02-13T00:50:31Z- handleDvl() - RDI_Pathfinder msg: 1613177431.59 epoch sec; seqNo:7788 2021-02-13T00:50:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:50:31Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:50:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177431.60 epoch sec; seqNo:16224; 4060180.48 north; 593228.97 east 2021-02-13T00:50:31Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-13T00:50:31Z- waiting for 1613177429.73, time is 1613177431.60 2021-02-13T00:50:31Z- LcmTrn::updateTrn() equating pose 1613177431.60 and meas times 1613177431.60 2021-02-13T00:50:31Z- LcmTrn::updateTrn() >>>> pose time 1613177431.60, meas time 1613177431.60 <<<< 2021-02-13T00:50:31Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177431.60, ping # 0. 2021-02-13T00:50:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177431.60 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:50:31Z- TerrainNav::motionUpdate() - currEstimate = (4060180, 593228.97, 27.53) 2021-02-13T00:50:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:50:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:50:31Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:50:31Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:50:31Z- TNavBF::Projecting Measurements 2021-02-13T00:50:31Z- TNavFilter::Measurement from time = 1613177431.60 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:50:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:50:31Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:50:31Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:50:31Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:50:31Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:50:31Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:50:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:50:31Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 685.6 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:50:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:50:31Z- TNavBF::mmse selected from filter -1 2021-02-13T00:50:31Z- variances([0], [2], and psi_berg) = 685.638364 832.990945 0.000000 2021-02-13T00:50:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:50:31Z- LcmTrn::publishEstimates() - MLE : 4060179.98 593250.47 27.53 2021-02-13T00:50:31Z- LcmTrn::publishEstimates() - MMSE : 4060180.48 593228.97 27.53 2021-02-13T00:50:31Z- LcmTrn::publishEstimates() - COVAR: 685.64 832.99 1.10 0.00 2021-02-13T00:50:31Z- AHRS_M2 msg: 1613177431.97 epoch sec; seqNo:16099 2021-02-13T00:50:31Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.13 psi 2021-02-13T00:50:31Z- handleDepth()-Depth_Keller msg: 1613177431981 epoch sec; seqNo:16225; depth 27.41 2021-02-13T00:50:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177432.01 epoch sec; seqNo:16225; 4060180.86 north; 593228.97 east 2021-02-13T00:50:32Z- AHRS_M2 msg: 1613177432.38 epoch sec; seqNo:16100 2021-02-13T00:50:32Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.13 psi 2021-02-13T00:50:32Z- handleDepth()-Depth_Keller msg: 1613177432385 epoch sec; seqNo:16226; depth 27.29 2021-02-13T00:50:32Z- handleDvl() - RDI_Pathfinder msg: 1613177432.40 epoch sec; seqNo:7789 2021-02-13T00:50:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 0.00 , 97.17 , 62.94 2021-02-13T00:50:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.09 , 0.08 2021-02-13T00:50:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177432.43 epoch sec; seqNo:16226; 4060181.24 north; 593228.96 east 2021-02-13T00:50:32Z- AHRS_M2 msg: 1613177432.78 epoch sec; seqNo:16101 2021-02-13T00:50:32Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.13 psi 2021-02-13T00:50:32Z- handleDepth()-Depth_Keller msg: 1613177432786 epoch sec; seqNo:16227; depth 27.16 2021-02-13T00:50:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177432.80 epoch sec; seqNo:16227; 4060181.24 north; 593228.96 east 2021-02-13T00:50:33Z- AHRS_M2 msg: 1613177433.18 epoch sec; seqNo:16102 2021-02-13T00:50:33Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.12 psi 2021-02-13T00:50:33Z- handleDepth()-Depth_Keller msg: 1613177433188 epoch sec; seqNo:16228; depth 27.04 2021-02-13T00:50:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177433.20 epoch sec; seqNo:16228; 4060181.62 north; 593228.96 east 2021-02-13T00:50:33Z- AHRS_M2 msg: 1613177433.59 epoch sec; seqNo:16103 2021-02-13T00:50:33Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.12 psi 2021-02-13T00:50:33Z- handleDepth()-Depth_Keller msg: 1613177433592 epoch sec; seqNo:16229; depth 26.95 2021-02-13T00:50:33Z- handleDvl() - RDI_Pathfinder msg: 1613177433.60 epoch sec; seqNo:7790 2021-02-13T00:50:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 81.75 , 97.98 , 0.00 2021-02-13T00:50:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.12 2021-02-13T00:50:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177433.61 epoch sec; seqNo:16229; 4060182.01 north; 593230.18 east 2021-02-13T00:50:33Z- AHRS_M2 msg: 1613177433.99 epoch sec; seqNo:16104 2021-02-13T00:50:33Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.12 psi 2021-02-13T00:50:34Z- handleDepth()-Depth_Keller msg: 1613177434001 epoch sec; seqNo:16230; depth 26.83 2021-02-13T00:50:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177434.01 epoch sec; seqNo:16230; 4060182.39 north; 593230.17 east 2021-02-13T00:50:34Z- AHRS_M2 msg: 1613177434.40 epoch sec; seqNo:16105 2021-02-13T00:50:34Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.11 psi 2021-02-13T00:50:34Z- handleDepth()-Depth_Keller msg: 1613177434421 epoch sec; seqNo:16231; depth 26.70 2021-02-13T00:50:34Z- handleDvl() - RDI_Pathfinder msg: 1613177434.43 epoch sec; seqNo:7791 2021-02-13T00:50:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.64 , 81.08 , 92.03 , 63.35 2021-02-13T00:50:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.03 , 0.08 2021-02-13T00:50:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177434.44 epoch sec; seqNo:16231; 4060182.77 north; 593230.17 east 2021-02-13T00:50:34Z- AHRS_M2 msg: 1613177434.80 epoch sec; seqNo:16106 2021-02-13T00:50:34Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.11 psi 2021-02-13T00:50:34Z- handleDepth()-Depth_Keller msg: 1613177434804 epoch sec; seqNo:16232; depth 26.59 2021-02-13T00:50:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177434.82 epoch sec; seqNo:16232; 4060182.77 north; 593230.17 east 2021-02-13T00:50:35Z- AHRS_M2 msg: 1613177435.21 epoch sec; seqNo:16107 2021-02-13T00:50:35Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.11 psi 2021-02-13T00:50:35Z- handleDepth()-Depth_Keller msg: 1613177435217 epoch sec; seqNo:16233; depth 26.48 2021-02-13T00:50:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177435.27 epoch sec; seqNo:16233; 4060183.15 north; 593230.16 east 2021-02-13T00:50:35Z- AHRS_M2 msg: 1613177435.61 epoch sec; seqNo:16108 2021-02-13T00:50:35Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.11 psi 2021-02-13T00:50:35Z- handleDepth()-Depth_Keller msg: 1613177435614 epoch sec; seqNo:16234; depth 26.37 2021-02-13T00:50:35Z- handleDvl() - RDI_Pathfinder msg: 1613177435.63 epoch sec; seqNo:7792 2021-02-13T00:50:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 81.75 , 92.57 , 62.53 2021-02-13T00:50:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.09 , -0.00 2021-02-13T00:50:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177435.64 epoch sec; seqNo:16234; 4060183.53 north; 593230.16 east 2021-02-13T00:50:36Z- AHRS_M2 msg: 1613177436.01 epoch sec; seqNo:16109 2021-02-13T00:50:36Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.11 psi 2021-02-13T00:50:36Z- handleDepth()-Depth_Keller msg: 1613177436017 epoch sec; seqNo:16235; depth 26.26 2021-02-13T00:50:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177436.03 epoch sec; seqNo:16235; 4060183.92 north; 593231.38 east 2021-02-13T00:50:36Z- AHRS_M2 msg: 1613177436.42 epoch sec; seqNo:16110 2021-02-13T00:50:36Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.11 psi 2021-02-13T00:50:36Z- handleDepth()-Depth_Keller msg: 1613177436423 epoch sec; seqNo:16236; depth 26.15 2021-02-13T00:50:36Z- handleDvl() - RDI_Pathfinder msg: 1613177436.43 epoch sec; seqNo:7793 2021-02-13T00:50:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 0.00 , 92.57 , 63.76 2021-02-13T00:50:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.05 , -0.09 2021-02-13T00:50:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177436.46 epoch sec; seqNo:16236; 4060183.92 north; 593231.38 east 2021-02-13T00:50:36Z- AHRS_M2 msg: 1613177436.82 epoch sec; seqNo:16111 2021-02-13T00:50:36Z- AHRS_M2 msg: 0.65 phi; 0.31 theta; -0.11 psi 2021-02-13T00:50:36Z- handleDepth()-Depth_Keller msg: 1613177436824 epoch sec; seqNo:16237; depth 26.03 2021-02-13T00:50:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177436.84 epoch sec; seqNo:16237; 4060184.30 north; 593231.37 east 2021-02-13T00:50:36Z- LcmTrn::handleMessages() - handling 44 messages took 0.76 ms and 129 calls... 2021-02-13T00:50:36Z- waiting for 1613177434.60, time is 1613177436.84 2021-02-13T00:50:36Z- LcmTrn::updateTrn() >>>> pose time 1613177436.84, meas time 1613177436.43 <<<< 2021-02-13T00:50:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177436.43 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:50:36Z- TerrainNav::motionUpdate() - currEstimate = (4060184, 593231.37, 26.03) 2021-02-13T00:50:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:50:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:50:36Z- TNavBF::Projecting Measurements 2021-02-13T00:50:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:50:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:50:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:50:36Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:50:36Z- TNavFilter::3 good beams. 2021-02-13T00:50:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:50:36Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:50:36Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:50:36Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:50:36Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:50:36Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:50:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:50:36Z- TNavBF::mmse selected from filter -1 2021-02-13T00:50:36Z- TerrainMapOctree: (vehN, vehE ) = (4060184.01, 593231.19). 2021-02-13T00:50:36Z- TerrainMapOctree: Min Distance = 1474.56. 2021-02-13T00:50:36Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:50:37Z- There is no specific good beam that all particles have in common. 2021-02-13T00:50:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:50:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.118 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-13T00:50:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:50:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 13.71 2021-02-13T00:50:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:50:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.116 ExpectedMeasDiff: nan currMeas.covariance: 6.50 2021-02-13T00:50:37Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:50:37Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:50:37Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:50:37Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:50:37Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:50:37Z- Filter 0 No beams used in subcloud update 2021-02-13T00:50:37Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:50:37Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:50:37Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 685.6 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:50:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177436.43 sec. 2021-02-13T00:50:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:50:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:50:37Z- variances([0], [2], and psi_berg) = 685.577249 832.990918 0.000000 2021-02-13T00:50:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:50:37Z- LcmTrn::publishEstimates() - MLE : 4060183.80 593252.87 26.03 2021-02-13T00:50:37Z- LcmTrn::publishEstimates() - MMSE : 4060184.30 593231.37 26.03 2021-02-13T00:50:37Z- LcmTrn::publishEstimates() - COVAR: 685.58 832.99 0.97 0.00 2021-02-13T00:50:37Z- AHRS_M2 msg: 1613177437.24 epoch sec; seqNo:16112 2021-02-13T00:50:37Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.11 psi 2021-02-13T00:50:37Z- handleDepth()-Depth_Keller msg: 1613177437255 epoch sec; seqNo:16238; depth 25.91 2021-02-13T00:50:37Z- handleDvl() - RDI_Pathfinder msg: 1613177437.27 epoch sec; seqNo:7794 2021-02-13T00:50:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.52 , 81.75 , 97.98 , 63.35 2021-02-13T00:50:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , -0.13 2021-02-13T00:50:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177437.30 epoch sec; seqNo:16238; 4060184.68 north; 593231.37 east 2021-02-13T00:50:37Z- AHRS_M2 msg: 1613177437.63 epoch sec; seqNo:16113 2021-02-13T00:50:37Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.12 psi 2021-02-13T00:50:37Z- handleDepth()-Depth_Keller msg: 1613177437631 epoch sec; seqNo:16239; depth 25.80 2021-02-13T00:50:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177437.65 epoch sec; seqNo:16239; 4060185.06 north; 593231.36 east 2021-02-13T00:50:38Z- AHRS_M2 msg: 1613177438.03 epoch sec; seqNo:16114 2021-02-13T00:50:38Z- AHRS_M2 msg: 0.66 phi; 0.31 theta; -0.13 psi 2021-02-13T00:50:38Z- handleDepth()-Depth_Keller msg: 1613177438036 epoch sec; seqNo:16240; depth 25.67 2021-02-13T00:50:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177438.04 epoch sec; seqNo:16240; 4060185.07 north; 593232.58 east 2021-02-13T00:50:38Z- AHRS_M2 msg: 1613177438.44 epoch sec; seqNo:16115 2021-02-13T00:50:38Z- AHRS_M2 msg: 0.66 phi; 0.30 theta; -0.13 psi 2021-02-13T00:50:38Z- handleDepth()-Depth_Keller msg: 1613177438440 epoch sec; seqNo:16241; depth 25.57 2021-02-13T00:50:38Z- handleDvl() - RDI_Pathfinder msg: 1613177438.45 epoch sec; seqNo:7795 2021-02-13T00:50:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 149.90 , 97.98 , 63.35 2021-02-13T00:50:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.09 , -0.10 2021-02-13T00:50:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177438.46 epoch sec; seqNo:16241; 4060185.45 north; 593232.58 east 2021-02-13T00:50:38Z- AHRS_M2 msg: 1613177438.84 epoch sec; seqNo:16116 2021-02-13T00:50:38Z- AHRS_M2 msg: 0.66 phi; 0.30 theta; -0.14 psi 2021-02-13T00:50:38Z- handleDepth()-Depth_Keller msg: 1613177438853 epoch sec; seqNo:16242; depth 25.43 2021-02-13T00:50:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177438.86 epoch sec; seqNo:16242; 4060185.83 north; 593232.58 east 2021-02-13T00:50:39Z- AHRS_M2 msg: 1613177439.27 epoch sec; seqNo:16117 2021-02-13T00:50:39Z- AHRS_M2 msg: 0.65 phi; 0.30 theta; -0.16 psi 2021-02-13T00:50:39Z- handleDepth()-Depth_Keller msg: 1613177439275 epoch sec; seqNo:16243; depth 25.34 2021-02-13T00:50:39Z- handleDvl() - RDI_Pathfinder msg: 1613177439.29 epoch sec; seqNo:7796 2021-02-13T00:50:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 83.92 , 97.44 , 64.98 2021-02-13T00:50:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.11 , -0.06 2021-02-13T00:50:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177439.30 epoch sec; seqNo:16243; 4060185.83 north; 593232.58 east 2021-02-13T00:50:39Z- handleDepth()-Depth_Keller msg: 1613177439673 epoch sec; seqNo:16244; depth 25.21 2021-02-13T00:50:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177439.69 epoch sec; seqNo:16244; 4060186.21 north; 593232.57 east 2021-02-13T00:50:40Z- AHRS_M2 msg: 1613177440.07 epoch sec; seqNo:16118 2021-02-13T00:50:40Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.18 psi 2021-02-13T00:50:40Z- handleDepth()-Depth_Keller msg: 1613177440077 epoch sec; seqNo:16245; depth 25.09 2021-02-13T00:50:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177440.09 epoch sec; seqNo:16245; 4060186.59 north; 593232.57 east 2021-02-13T00:50:40Z- AHRS_M2 msg: 1613177440.47 epoch sec; seqNo:16119 2021-02-13T00:50:40Z- AHRS_M2 msg: 0.64 phi; 0.29 theta; -0.18 psi 2021-02-13T00:50:40Z- handleDepth()-Depth_Keller msg: 1613177440470 epoch sec; seqNo:16246; depth 24.98 2021-02-13T00:50:40Z- handleDvl() - RDI_Pathfinder msg: 1613177440.48 epoch sec; seqNo:7797 2021-02-13T00:50:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 0.00 , 97.98 , 64.98 2021-02-13T00:50:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.14 , 0.02 2021-02-13T00:50:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177440.49 epoch sec; seqNo:16246; 4060186.96 north; 593232.56 east 2021-02-13T00:50:40Z- AHRS_M2 msg: 1613177440.86 epoch sec; seqNo:16120 2021-02-13T00:50:40Z- AHRS_M2 msg: 0.63 phi; 0.29 theta; -0.19 psi 2021-02-13T00:50:40Z- handleDepth()-Depth_Keller msg: 1613177440865 epoch sec; seqNo:16247; depth 24.87 2021-02-13T00:50:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177440.88 epoch sec; seqNo:16247; 4060186.96 north; 593232.56 east 2021-02-13T00:50:41Z- AHRS_M2 msg: 1613177441.27 epoch sec; seqNo:16121 2021-02-13T00:50:41Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.19 psi 2021-02-13T00:50:41Z- handleDepth()-Depth_Keller msg: 1613177441273 epoch sec; seqNo:16248; depth 24.77 2021-02-13T00:50:41Z- handleDvl() - RDI_Pathfinder msg: 1613177441.29 epoch sec; seqNo:7798 2021-02-13T00:50:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 86.08 , 97.98 , 65.39 2021-02-13T00:50:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.15 , 0.08 2021-02-13T00:50:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177441.30 epoch sec; seqNo:16248; 4060187.36 north; 593233.78 east 2021-02-13T00:50:41Z- AHRS_M2 msg: 1613177441.67 epoch sec; seqNo:16122 2021-02-13T00:50:41Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.19 psi 2021-02-13T00:50:41Z- handleDepth()-Depth_Keller msg: 1613177441674 epoch sec; seqNo:16249; depth 24.66 2021-02-13T00:50:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177441.68 epoch sec; seqNo:16249; 4060187.74 north; 593233.77 east 2021-02-13T00:50:42Z- AHRS_M2 msg: 1613177442.07 epoch sec; seqNo:16123 2021-02-13T00:50:42Z- AHRS_M2 msg: 0.61 phi; 0.29 theta; -0.20 psi 2021-02-13T00:50:42Z- LcmTrn::handleMessages() - handling 41 messages took 0.66 ms and 128 calls... 2021-02-13T00:50:42Z- waiting for 1613177439.84, time is 1613177442.07 2021-02-13T00:50:42Z- LcmTrn::updateTrn() >>>> pose time 1613177442.07, meas time 1613177441.29 <<<< 2021-02-13T00:50:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177441.29 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:50:42Z- TerrainNav::motionUpdate() - currEstimate = (4060188, 593233.77, 24.66) 2021-02-13T00:50:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:50:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:50:42Z- TNavBF::Projecting Measurements 2021-02-13T00:50:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:50:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:50:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:50:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:50:42Z- TNavFilter::4 good beams. 2021-02-13T00:50:42Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:50:42Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:50:42Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:50:42Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:50:42Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:50:42Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:50:42Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:50:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:50:42Z- TerrainMapOctree: (vehN, vehE ) = (4060187.22, 593233.41). 2021-02-13T00:50:42Z- TerrainMapOctree: Min Distance = 1470.70. 2021-02-13T00:50:42Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:50:42Z- There is no specific good beam that all particles have in common. 2021-02-13T00:50:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:50:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-13T00:50:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:50:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 11.86 2021-02-13T00:50:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:50:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:50:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:50:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-13T00:50:42Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:50:42Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:50:42Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:50:42Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:50:42Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:50:42Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:50:42Z- Filter 0 No beams used in subcloud update 2021-02-13T00:50:42Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:50:42Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:50:42Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 685.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:50:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177441.29 sec. 2021-02-13T00:50:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:50:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:50:42Z- variances([0], [2], and psi_berg) = 685.514919 832.990890 0.000000 2021-02-13T00:50:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:50:42Z- LcmTrn::publishEstimates() - MLE : 4060187.24 593254.27 24.66 2021-02-13T00:50:42Z- LcmTrn::publishEstimates() - MMSE : 4060187.74 593233.77 24.66 2021-02-13T00:50:42Z- LcmTrn::publishEstimates() - COVAR: 685.51 832.99 0.85 0.00 2021-02-13T00:50:42Z- handleDepth()-Depth_Keller msg: 1613177442086 epoch sec; seqNo:16250; depth 24.55 2021-02-13T00:50:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177442.09 epoch sec; seqNo:16250; 4060188.11 north; 593233.77 east 2021-02-13T00:50:42Z- AHRS_M2 msg: 1613177442.48 epoch sec; seqNo:16124 2021-02-13T00:50:42Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.20 psi 2021-02-13T00:50:42Z- handleDepth()-Depth_Keller msg: 1613177442480 epoch sec; seqNo:16251; depth 24.43 2021-02-13T00:50:42Z- handleDvl() - RDI_Pathfinder msg: 1613177442.49 epoch sec; seqNo:7799 2021-02-13T00:50:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 0.00 , 97.98 , 65.80 2021-02-13T00:50:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.11 , 0.15 2021-02-13T00:50:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177442.51 epoch sec; seqNo:16251; 4060188.49 north; 593233.77 east 2021-02-13T00:50:42Z- AHRS_M2 msg: 1613177442.90 epoch sec; seqNo:16125 2021-02-13T00:50:42Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.19 psi 2021-02-13T00:50:42Z- handleDepth()-Depth_Keller msg: 1613177442904 epoch sec; seqNo:16252; depth 24.32 2021-02-13T00:50:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177442.93 epoch sec; seqNo:16252; 4060188.49 north; 593233.77 east 2021-02-13T00:50:43Z- AHRS_M2 msg: 1613177443.29 epoch sec; seqNo:16126 2021-02-13T00:50:43Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.19 psi 2021-02-13T00:50:43Z- handleDepth()-Depth_Keller msg: 1613177443293 epoch sec; seqNo:16253; depth 24.21 2021-02-13T00:50:43Z- handleDvl() - RDI_Pathfinder msg: 1613177443.31 epoch sec; seqNo:7800 2021-02-13T00:50:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 149.90 , 97.98 , 66.62 2021-02-13T00:50:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.28 , 0.07 2021-02-13T00:50:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177443.32 epoch sec; seqNo:16253; 4060188.87 north; 593233.76 east 2021-02-13T00:50:43Z- AHRS_M2 msg: 1613177443.69 epoch sec; seqNo:16127 2021-02-13T00:50:43Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.19 psi 2021-02-13T00:50:43Z- handleDepth()-Depth_Keller msg: 1613177443691 epoch sec; seqNo:16254; depth 24.10 2021-02-13T00:50:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177443.71 epoch sec; seqNo:16254; 4060189.26 north; 593234.98 east 2021-02-13T00:50:44Z- AHRS_M2 msg: 1613177444.09 epoch sec; seqNo:16128 2021-02-13T00:50:44Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.18 psi 2021-02-13T00:50:44Z- handleDepth()-Depth_Keller msg: 1613177444096 epoch sec; seqNo:16255; depth 24.01 2021-02-13T00:50:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177444.10 epoch sec; seqNo:16255; 4060189.26 north; 593234.98 east 2021-02-13T00:50:44Z- AHRS_M2 msg: 1613177444.50 epoch sec; seqNo:16129 2021-02-13T00:50:44Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.17 psi 2021-02-13T00:50:44Z- handleDepth()-Depth_Keller msg: 1613177444500 epoch sec; seqNo:16256; depth 23.91 2021-02-13T00:50:44Z- handleDvl() - RDI_Pathfinder msg: 1613177444.51 epoch sec; seqNo:7801 2021-02-13T00:50:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 0.00 , 97.37 , 65.80 2021-02-13T00:50:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , 0.12 2021-02-13T00:50:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177444.52 epoch sec; seqNo:16256; 4060189.64 north; 593234.97 east 2021-02-13T00:50:44Z- AHRS_M2 msg: 1613177444.90 epoch sec; seqNo:16130 2021-02-13T00:50:44Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-13T00:50:44Z- handleDepth()-Depth_Keller msg: 1613177444906 epoch sec; seqNo:16257; depth 23.81 2021-02-13T00:50:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177444.92 epoch sec; seqNo:16257; 4060190.02 north; 593234.97 east 2021-02-13T00:50:45Z- AHRS_M2 msg: 1613177445.33 epoch sec; seqNo:16131 2021-02-13T00:50:45Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.16 psi 2021-02-13T00:50:45Z- handleDepth()-Depth_Keller msg: 1613177445334 epoch sec; seqNo:16258; depth 23.69 2021-02-13T00:50:45Z- handleDvl() - RDI_Pathfinder msg: 1613177445.35 epoch sec; seqNo:7802 2021-02-13T00:50:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 87.16 , 97.98 , 0.00 2021-02-13T00:50:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.06 , 0.07 2021-02-13T00:50:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177445.36 epoch sec; seqNo:16258; 4060190.40 north; 593234.97 east 2021-02-13T00:50:45Z- AHRS_M2 msg: 1613177445.71 epoch sec; seqNo:16132 2021-02-13T00:50:45Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.15 psi 2021-02-13T00:50:45Z- handleDepth()-Depth_Keller msg: 1613177445712 epoch sec; seqNo:16259; depth 23.58 2021-02-13T00:50:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177445.72 epoch sec; seqNo:16259; 4060190.41 north; 593236.19 east 2021-02-13T00:50:46Z- AHRS_M2 msg: 1613177446.11 epoch sec; seqNo:16133 2021-02-13T00:50:46Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.15 psi 2021-02-13T00:50:46Z- handleDepth()-Depth_Keller msg: 1613177446121 epoch sec; seqNo:16260; depth 23.50 2021-02-13T00:50:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177446.13 epoch sec; seqNo:16260; 4060190.79 north; 593236.18 east 2021-02-13T00:50:46Z- AHRS_M2 msg: 1613177446.52 epoch sec; seqNo:16134 2021-02-13T00:50:46Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.15 psi 2021-02-13T00:50:46Z- handleDepth()-Depth_Keller msg: 1613177446521 epoch sec; seqNo:16261; depth 23.38 2021-02-13T00:50:46Z- handleDvl() - RDI_Pathfinder msg: 1613177446.53 epoch sec; seqNo:7803 2021-02-13T00:50:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 0.00 , 97.91 , 78.44 2021-02-13T00:50:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.27 , -0.11 2021-02-13T00:50:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177446.54 epoch sec; seqNo:16261; 4060191.17 north; 593236.18 east 2021-02-13T00:50:46Z- AHRS_M2 msg: 1613177446.92 epoch sec; seqNo:16135 2021-02-13T00:50:46Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-13T00:50:46Z- handleDepth()-Depth_Keller msg: 1613177446924 epoch sec; seqNo:16262; depth 23.27 2021-02-13T00:50:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177446.94 epoch sec; seqNo:16262; 4060191.17 north; 593236.18 east 2021-02-13T00:50:47Z- LcmTrn::handleMessages() - handling 43 messages took 0.63 ms and 131 calls... 2021-02-13T00:50:47Z- waiting for 1613177445.07, time is 1613177446.94 2021-02-13T00:50:47Z- LcmTrn::updateTrn() >>>> pose time 1613177446.94, meas time 1613177446.53 <<<< 2021-02-13T00:50:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177446.53 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:50:47Z- TerrainNav::motionUpdate() - currEstimate = (4060191, 593236.18, 23.27) 2021-02-13T00:50:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:50:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:50:47Z- TNavBF::Projecting Measurements 2021-02-13T00:50:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:50:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:50:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:50:47Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:50:47Z- TNavFilter::3 good beams. 2021-02-13T00:50:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:50:47Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:50:47Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:50:47Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:50:47Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:50:47Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:50:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:50:47Z- TNavBF::mmse selected from filter -1 2021-02-13T00:50:47Z- TerrainMapOctree: (vehN, vehE ) = (4060190.88, 593235.98). 2021-02-13T00:50:47Z- TerrainMapOctree: Min Distance = 1466.28. 2021-02-13T00:50:47Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:50:47Z- There is no specific good beam that all particles have in common. 2021-02-13T00:50:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:50:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T00:50:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:50:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:50:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:50:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T00:50:47Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:50:47Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:50:47Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:50:47Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:50:47Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:50:47Z- Filter 0 No beams used in subcloud update 2021-02-13T00:50:47Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:50:47Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:50:47Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 685.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:50:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177446.53 sec. 2021-02-13T00:50:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:50:47Z- TNavBF::mmse selected from filter -1 2021-02-13T00:50:47Z- variances([0], [2], and psi_berg) = 685.455611 832.990863 0.000000 2021-02-13T00:50:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:50:47Z- LcmTrn::publishEstimates() - MLE : 4060190.67 593256.68 23.27 2021-02-13T00:50:47Z- LcmTrn::publishEstimates() - MMSE : 4060191.17 593236.18 23.27 2021-02-13T00:50:47Z- LcmTrn::publishEstimates() - COVAR: 685.46 832.99 0.86 0.00 2021-02-13T00:50:47Z- AHRS_M2 msg: 1613177447.32 epoch sec; seqNo:16136 2021-02-13T00:50:47Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-13T00:50:47Z- handleDepth()-Depth_Keller msg: 1613177447333 epoch sec; seqNo:16263; depth 23.16 2021-02-13T00:50:47Z- handleDvl() - RDI_Pathfinder msg: 1613177447.35 epoch sec; seqNo:7804 2021-02-13T00:50:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.71 , 0.00 , 98.18 , 66.21 2021-02-13T00:50:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.07 , -0.04 2021-02-13T00:50:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177447.36 epoch sec; seqNo:16263; 4060191.55 north; 593236.17 east 2021-02-13T00:50:47Z- AHRS_M2 msg: 1613177447.73 epoch sec; seqNo:16137 2021-02-13T00:50:47Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-13T00:50:47Z- handleDepth()-Depth_Keller msg: 1613177447737 epoch sec; seqNo:16264; depth 23.06 2021-02-13T00:50:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177447.75 epoch sec; seqNo:16264; 4060191.56 north; 593237.40 east 2021-02-13T00:50:48Z- AHRS_M2 msg: 1613177448.14 epoch sec; seqNo:16138 2021-02-13T00:50:48Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-13T00:50:48Z- handleDepth()-Depth_Keller msg: 1613177448152 epoch sec; seqNo:16265; depth 22.94 2021-02-13T00:50:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177448.17 epoch sec; seqNo:16265; 4060191.94 north; 593237.39 east 2021-02-13T00:50:48Z- AHRS_M2 msg: 1613177448.54 epoch sec; seqNo:16139 2021-02-13T00:50:48Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.15 psi 2021-02-13T00:50:48Z- handleDepth()-Depth_Keller msg: 1613177448540 epoch sec; seqNo:16266; depth 22.82 2021-02-13T00:50:48Z- handleDvl() - RDI_Pathfinder msg: 1613177448.56 epoch sec; seqNo:7805 2021-02-13T00:50:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 0.00 , 97.85 , 66.21 2021-02-13T00:50:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , -0.06 2021-02-13T00:50:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177448.57 epoch sec; seqNo:16266; 4060192.32 north; 593237.39 east 2021-02-13T00:50:48Z- AHRS_M2 msg: 1613177448.94 epoch sec; seqNo:16140 2021-02-13T00:50:48Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.16 psi 2021-02-13T00:50:48Z- handleDepth()-Depth_Keller msg: 1613177448950 epoch sec; seqNo:16267; depth 22.73 2021-02-13T00:50:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177448.96 epoch sec; seqNo:16267; 4060192.70 north; 593237.38 east 2021-02-13T00:50:49Z- AHRS_M2 msg: 1613177449.34 epoch sec; seqNo:16141 2021-02-13T00:50:49Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.16 psi 2021-02-13T00:50:49Z- handleDepth()-Depth_Keller msg: 1613177449349 epoch sec; seqNo:16268; depth 22.62 2021-02-13T00:50:49Z- handleDvl() - RDI_Pathfinder msg: 1613177449.36 epoch sec; seqNo:7806 2021-02-13T00:50:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 87.16 , 97.98 , 65.80 2021-02-13T00:50:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.07 2021-02-13T00:50:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177449.37 epoch sec; seqNo:16268; 4060192.70 north; 593237.38 east 2021-02-13T00:50:49Z- AHRS_M2 msg: 1613177449.75 epoch sec; seqNo:16142 2021-02-13T00:50:49Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.16 psi 2021-02-13T00:50:49Z- handleDepth()-Depth_Keller msg: 1613177449757 epoch sec; seqNo:16269; depth 22.53 2021-02-13T00:50:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177449.77 epoch sec; seqNo:16269; 4060193.08 north; 593237.38 east 2021-02-13T00:50:50Z- AHRS_M2 msg: 1613177450.15 epoch sec; seqNo:16143 2021-02-13T00:50:50Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.17 psi 2021-02-13T00:50:50Z- handleDepth()-Depth_Keller msg: 1613177450159 epoch sec; seqNo:16270; depth 22.40 2021-02-13T00:50:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177450.17 epoch sec; seqNo:16270; 4060193.47 north; 593238.60 east 2021-02-13T00:50:50Z- AHRS_M2 msg: 1613177450.56 epoch sec; seqNo:16144 2021-02-13T00:50:50Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.17 psi 2021-02-13T00:50:50Z- handleDepth()-Depth_Keller msg: 1613177450560 epoch sec; seqNo:16271; depth 22.30 2021-02-13T00:50:50Z- handleDvl() - RDI_Pathfinder msg: 1613177450.57 epoch sec; seqNo:7807 2021-02-13T00:50:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.37 , 89.19 , 97.85 , 66.62 2021-02-13T00:50:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , -0.06 2021-02-13T00:50:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177450.59 epoch sec; seqNo:16271; 4060193.47 north; 593238.60 east 2021-02-13T00:50:50Z- AHRS_M2 msg: 1613177450.96 epoch sec; seqNo:16145 2021-02-13T00:50:50Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.17 psi 2021-02-13T00:50:50Z- handleDepth()-Depth_Keller msg: 1613177450973 epoch sec; seqNo:16272; depth 22.18 2021-02-13T00:50:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177450.98 epoch sec; seqNo:16272; 4060193.85 north; 593238.59 east 2021-02-13T00:50:51Z- AHRS_M2 msg: 1613177451.36 epoch sec; seqNo:16146 2021-02-13T00:50:51Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.17 psi 2021-02-13T00:50:51Z- handleDepth()-Depth_Keller msg: 1613177451368 epoch sec; seqNo:16273; depth 22.08 2021-02-13T00:50:51Z- handleDvl() - RDI_Pathfinder msg: 1613177451.38 epoch sec; seqNo:7808 2021-02-13T00:50:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.34 , 88.65 , 98.39 , 66.21 2021-02-13T00:50:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.04 , -0.05 2021-02-13T00:50:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177451.40 epoch sec; seqNo:16273; 4060194.23 north; 593238.59 east 2021-02-13T00:50:51Z- AHRS_M2 msg: 1613177451.77 epoch sec; seqNo:16147 2021-02-13T00:50:51Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.17 psi 2021-02-13T00:50:51Z- handleDepth()-Depth_Keller msg: 1613177451777 epoch sec; seqNo:16274; depth 21.95 2021-02-13T00:50:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177451.79 epoch sec; seqNo:16274; 4060194.61 north; 593238.58 east 2021-02-13T00:50:52Z- AHRS_M2 msg: 1613177452.17 epoch sec; seqNo:16148 2021-02-13T00:50:52Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.17 psi 2021-02-13T00:50:52Z- handleDepth()-Depth_Keller msg: 1613177452177 epoch sec; seqNo:16275; depth 21.86 2021-02-13T00:50:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177452.20 epoch sec; seqNo:16275; 4060194.61 north; 593238.58 east 2021-02-13T00:50:52Z- LcmTrn::handleMessages() - handling 44 messages took 0.73 ms and 130 calls... 2021-02-13T00:50:52Z- waiting for 1613177449.94, time is 1613177452.20 2021-02-13T00:50:52Z- LcmTrn::updateTrn() >>>> pose time 1613177452.20, meas time 1613177451.38 <<<< 2021-02-13T00:50:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177451.38 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:50:52Z- TerrainNav::motionUpdate() - currEstimate = (4060195, 593238.58, 21.86) 2021-02-13T00:50:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:50:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:50:52Z- TNavBF::Projecting Measurements 2021-02-13T00:50:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:50:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:50:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:50:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:50:52Z- TNavFilter::4 good beams. 2021-02-13T00:50:52Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:50:52Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:50:52Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:50:52Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:50:52Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:50:52Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:50:52Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:50:52Z- TNavBF::mmse selected from filter -1 2021-02-13T00:50:52Z- TerrainMapOctree: (vehN, vehE ) = (4060194.07, 593238.21). 2021-02-13T00:50:52Z- TerrainMapOctree: Min Distance = 1462.43. 2021-02-13T00:50:52Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:50:52Z- There is no specific good beam that all particles have in common. 2021-02-13T00:50:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:50:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-13T00:50:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:50:52Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 12.57 2021-02-13T00:50:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:50:52Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 15.49 2021-02-13T00:50:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:50:52Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 7.01 2021-02-13T00:50:52Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:50:52Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:50:52Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:50:52Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:50:52Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:50:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:50:52Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:50:52Z- Filter 0 No beams used in subcloud update 2021-02-13T00:50:52Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:50:52Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:50:52Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 685.4 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:50:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177451.38 sec. 2021-02-13T00:50:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:50:52Z- TNavBF::mmse selected from filter -1 2021-02-13T00:50:52Z- variances([0], [2], and psi_berg) = 685.393173 832.990835 0.000000 2021-02-13T00:50:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:50:52Z- LcmTrn::publishEstimates() - MLE : 4060194.11 593259.08 21.86 2021-02-13T00:50:52Z- LcmTrn::publishEstimates() - MMSE : 4060194.61 593238.58 21.86 2021-02-13T00:50:52Z- LcmTrn::publishEstimates() - COVAR: 685.39 832.99 0.80 0.00 2021-02-13T00:50:52Z- AHRS_M2 msg: 1613177452.58 epoch sec; seqNo:16149 2021-02-13T00:50:52Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.17 psi 2021-02-13T00:50:52Z- handleDepth()-Depth_Keller msg: 1613177452585 epoch sec; seqNo:16276; depth 21.75 2021-02-13T00:50:52Z- handleDvl() - RDI_Pathfinder msg: 1613177452.60 epoch sec; seqNo:7809 2021-02-13T00:50:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.37 , 90.27 , 97.85 , 68.66 2021-02-13T00:50:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.01 , -0.06 2021-02-13T00:50:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177452.62 epoch sec; seqNo:16276; 4060194.99 north; 593238.58 east 2021-02-13T00:50:52Z- AHRS_M2 msg: 1613177452.98 epoch sec; seqNo:16150 2021-02-13T00:50:52Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.17 psi 2021-02-13T00:50:52Z- handleDepth()-Depth_Keller msg: 1613177452985 epoch sec; seqNo:16277; depth 21.63 2021-02-13T00:50:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:50:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:50:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:50:52Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:50:52Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:50:52Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:50:52Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:50:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:50:52Z- handleDvl() - RDI_Pathfinder msg: 1613177452.99 epoch sec; seqNo:7810 2021-02-13T00:50:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:50:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.01 , -0.06 2021-02-13T00:50:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177453.00 epoch sec; seqNo:16277; 4060195.38 north; 593239.80 east 2021-02-13T00:50:53Z- AHRS_M2 msg: 1613177453.38 epoch sec; seqNo:16151 2021-02-13T00:50:53Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.17 psi 2021-02-13T00:50:53Z- handleDepth()-Depth_Keller msg: 1613177453388 epoch sec; seqNo:16278; depth 21.54 2021-02-13T00:50:53Z- handleDvl() - RDI_Pathfinder msg: 1613177453.41 epoch sec; seqNo:7811 2021-02-13T00:50:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 0.00 , 98.32 , 67.43 2021-02-13T00:50:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , -0.07 2021-02-13T00:50:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177453.42 epoch sec; seqNo:16278; 4060195.38 north; 593239.80 east 2021-02-13T00:50:53Z- AHRS_M2 msg: 1613177453.79 epoch sec; seqNo:16152 2021-02-13T00:50:53Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.17 psi 2021-02-13T00:50:53Z- handleDepth()-Depth_Keller msg: 1613177453797 epoch sec; seqNo:16279; depth 21.42 2021-02-13T00:50:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177453.81 epoch sec; seqNo:16279; 4060195.76 north; 593239.79 east 2021-02-13T00:50:54Z- AHRS_M2 msg: 1613177454.20 epoch sec; seqNo:16153 2021-02-13T00:50:54Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.17 psi 2021-02-13T00:50:54Z- handleDepth()-Depth_Keller msg: 1613177454203 epoch sec; seqNo:16280; depth 21.32 2021-02-13T00:50:54Z- handleDvl() - RDI_Pathfinder msg: 1613177454.21 epoch sec; seqNo:7812 2021-02-13T00:50:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 0.00 , 97.98 , 67.84 2021-02-13T00:50:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.10 , -0.05 2021-02-13T00:50:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177454.24 epoch sec; seqNo:16280; 4060196.14 north; 593239.79 east 2021-02-13T00:50:54Z- AHRS_M2 msg: 1613177454.60 epoch sec; seqNo:16154 2021-02-13T00:50:54Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.17 psi 2021-02-13T00:50:54Z- handleDepth()-Depth_Keller msg: 1613177454605 epoch sec; seqNo:16281; depth 21.22 2021-02-13T00:50:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177454.62 epoch sec; seqNo:16281; 4060196.14 north; 593239.79 east 2021-02-13T00:50:55Z- AHRS_M2 msg: 1613177455.00 epoch sec; seqNo:16155 2021-02-13T00:50:55Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.17 psi 2021-02-13T00:50:55Z- handleDepth()-Depth_Keller msg: 1613177455004 epoch sec; seqNo:16282; depth 21.11 2021-02-13T00:50:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177455.02 epoch sec; seqNo:16282; 4060196.52 north; 593239.78 east 2021-02-13T00:50:55Z- AHRS_M2 msg: 1613177455.40 epoch sec; seqNo:16156 2021-02-13T00:50:55Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.17 psi 2021-02-13T00:50:55Z- handleDepth()-Depth_Keller msg: 1613177455413 epoch sec; seqNo:16283; depth 21.00 2021-02-13T00:50:55Z- handleDvl() - RDI_Pathfinder msg: 1613177455.42 epoch sec; seqNo:7813 2021-02-13T00:50:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 97.98 , 97.98 , 69.07 2021-02-13T00:50:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.24 , 0.02 2021-02-13T00:50:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177455.45 epoch sec; seqNo:16283; 4060196.89 north; 593239.78 east 2021-02-13T00:50:55Z- AHRS_M2 msg: 1613177455.81 epoch sec; seqNo:16157 2021-02-13T00:50:55Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.18 psi 2021-02-13T00:50:55Z- handleDepth()-Depth_Keller msg: 1613177455817 epoch sec; seqNo:16284; depth 20.88 2021-02-13T00:50:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177455.83 epoch sec; seqNo:16284; 4060197.27 north; 593239.77 east 2021-02-13T00:50:56Z- AHRS_M2 msg: 1613177456.24 epoch sec; seqNo:16158 2021-02-13T00:50:56Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.18 psi 2021-02-13T00:50:56Z- handleDepth()-Depth_Keller msg: 1613177456245 epoch sec; seqNo:16285; depth 20.79 2021-02-13T00:50:56Z- handleDvl() - RDI_Pathfinder msg: 1613177456.26 epoch sec; seqNo:7814 2021-02-13T00:50:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 90.95 , 98.25 , 68.25 2021-02-13T00:50:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.13 , 0.06 2021-02-13T00:50:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177456.28 epoch sec; seqNo:16285; 4060197.67 north; 593240.99 east 2021-02-13T00:50:56Z- AHRS_M2 msg: 1613177456.62 epoch sec; seqNo:16159 2021-02-13T00:50:56Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.18 psi 2021-02-13T00:50:56Z- handleDepth()-Depth_Keller msg: 1613177456620 epoch sec; seqNo:16286; depth 20.67 2021-02-13T00:50:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:50:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:50:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:50:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:50:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:50:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:50:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:50:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:50:56Z- handleDvl() - RDI_Pathfinder msg: 1613177456.62 epoch sec; seqNo:7815 2021-02-13T00:50:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:50:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.13 , 0.06 2021-02-13T00:50:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177456.64 epoch sec; seqNo:16286; 4060198.04 north; 593240.99 east 2021-02-13T00:50:57Z- AHRS_M2 msg: 1613177457.02 epoch sec; seqNo:16160 2021-02-13T00:50:57Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.18 psi 2021-02-13T00:50:57Z- handleDepth()-Depth_Keller msg: 1613177457031 epoch sec; seqNo:16287; depth 20.58 2021-02-13T00:50:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177457.05 epoch sec; seqNo:16287; 4060198.04 north; 593240.99 east 2021-02-13T00:50:57Z- LcmTrn::handleMessages() - handling 43 messages took 0.62 ms and 131 calls... 2021-02-13T00:50:57Z- waiting for 1613177455.20, time is 1613177457.05 2021-02-13T00:50:57Z- LcmTrn::updateTrn() >>>> pose time 1613177457.05, meas time 1613177456.62 <<<< 2021-02-13T00:50:57Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177456.62, ping # 0. 2021-02-13T00:50:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177456.62 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:50:57Z- TerrainNav::motionUpdate() - currEstimate = (4060198, 593240.99, 20.58) 2021-02-13T00:50:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:50:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:50:57Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:50:57Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:50:57Z- TNavBF::Projecting Measurements 2021-02-13T00:50:57Z- TNavFilter::Measurement from time = 1613177456.62 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:50:57Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:50:57Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:50:57Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:50:57Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:50:57Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:50:57Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:50:57Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:50:57Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 685.3 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:50:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:50:57Z- TNavBF::mmse selected from filter -1 2021-02-13T00:50:57Z- variances([0], [2], and psi_berg) = 685.333671 832.990808 0.000000 2021-02-13T00:50:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:50:57Z- LcmTrn::publishEstimates() - MLE : 4060197.54 593261.49 20.58 2021-02-13T00:50:57Z- LcmTrn::publishEstimates() - MMSE : 4060198.04 593240.99 20.58 2021-02-13T00:50:57Z- LcmTrn::publishEstimates() - COVAR: 685.33 832.99 1.01 0.00 2021-02-13T00:50:57Z- AHRS_M2 msg: 1613177457.43 epoch sec; seqNo:16161 2021-02-13T00:50:57Z- AHRS_M2 msg: 0.58 phi; 0.30 theta; -0.18 psi 2021-02-13T00:50:57Z- handleDepth()-Depth_Keller msg: 1613177457434 epoch sec; seqNo:16288; depth 20.48 2021-02-13T00:50:57Z- handleDvl() - RDI_Pathfinder msg: 1613177457.44 epoch sec; seqNo:7816 2021-02-13T00:50:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 98.39 , 99.60 , 69.07 2021-02-13T00:50:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.12 2021-02-13T00:50:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177457.45 epoch sec; seqNo:16288; 4060198.42 north; 593240.98 east 2021-02-13T00:50:57Z- AHRS_M2 msg: 1613177457.83 epoch sec; seqNo:16162 2021-02-13T00:50:57Z- AHRS_M2 msg: 0.58 phi; 0.30 theta; -0.18 psi 2021-02-13T00:50:57Z- handleDepth()-Depth_Keller msg: 1613177457845 epoch sec; seqNo:16289; depth 20.38 2021-02-13T00:50:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:50:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:50:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:50:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:50:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:50:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:50:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:50:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:50:57Z- handleDvl() - RDI_Pathfinder msg: 1613177457.85 epoch sec; seqNo:7817 2021-02-13T00:50:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:50:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.12 2021-02-13T00:50:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177457.86 epoch sec; seqNo:16289; 4060198.80 north; 593240.98 east 2021-02-13T00:50:58Z- AHRS_M2 msg: 1613177458.23 epoch sec; seqNo:16163 2021-02-13T00:50:58Z- AHRS_M2 msg: 0.58 phi; 0.30 theta; -0.18 psi 2021-02-13T00:50:58Z- handleDepth()-Depth_Keller msg: 1613177458239 epoch sec; seqNo:16290; depth 20.27 2021-02-13T00:50:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177458.25 epoch sec; seqNo:16290; 4060199.18 north; 593240.97 east 2021-02-13T00:50:58Z- AHRS_M2 msg: 1613177458.64 epoch sec; seqNo:16164 2021-02-13T00:50:58Z- AHRS_M2 msg: 0.57 phi; 0.30 theta; -0.18 psi 2021-02-13T00:50:58Z- handleDepth()-Depth_Keller msg: 1613177458645 epoch sec; seqNo:16291; depth 20.18 2021-02-13T00:50:58Z- handleDvl() - RDI_Pathfinder msg: 1613177458.66 epoch sec; seqNo:7818 2021-02-13T00:50:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:50:58Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:50:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177458.68 epoch sec; seqNo:16291; 4060199.56 north; 593240.97 east 2021-02-13T00:50:59Z- AHRS_M2 msg: 1613177459.04 epoch sec; seqNo:16165 2021-02-13T00:50:59Z- AHRS_M2 msg: 0.57 phi; 0.30 theta; -0.17 psi 2021-02-13T00:50:59Z- handleDepth()-Depth_Keller msg: 1613177459044 epoch sec; seqNo:16292; depth 20.07 2021-02-13T00:50:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:50:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:50:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:50:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:50:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:50:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:50:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:50:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:50:59Z- handleDvl() - RDI_Pathfinder msg: 1613177459.05 epoch sec; seqNo:7819 2021-02-13T00:50:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:50:59Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:50:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177459.06 epoch sec; seqNo:16292; 4060199.57 north; 593242.19 east 2021-02-13T00:50:59Z- AHRS_M2 msg: 1613177459.44 epoch sec; seqNo:16166 2021-02-13T00:50:59Z- AHRS_M2 msg: 0.57 phi; 0.30 theta; -0.17 psi 2021-02-13T00:50:59Z- handleDepth()-Depth_Keller msg: 1613177459450 epoch sec; seqNo:16293; depth 19.97 2021-02-13T00:50:59Z- handleDvl() - RDI_Pathfinder msg: 1613177459.46 epoch sec; seqNo:7820 2021-02-13T00:50:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.38 , 97.71 , 100.14 , 0.00 2021-02-13T00:50:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.12 , 0.13 2021-02-13T00:50:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177459.47 epoch sec; seqNo:16293; 4060199.95 north; 593242.19 east 2021-02-13T00:50:59Z- AHRS_M2 msg: 1613177459.85 epoch sec; seqNo:16167 2021-02-13T00:50:59Z- AHRS_M2 msg: 0.57 phi; 0.31 theta; -0.16 psi 2021-02-13T00:50:59Z- handleDepth()-Depth_Keller msg: 1613177459852 epoch sec; seqNo:16294; depth 19.87 2021-02-13T00:50:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177459.87 epoch sec; seqNo:16294; 4060200.33 north; 593242.18 east 2021-02-13T00:51:00Z- AHRS_M2 msg: 1613177460.25 epoch sec; seqNo:16168 2021-02-13T00:51:00Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.16 psi 2021-02-13T00:51:00Z- handleDepth()-Depth_Keller msg: 1613177460258 epoch sec; seqNo:16295; depth 19.76 2021-02-13T00:51:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:51:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:51:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:51:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:51:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:51:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:51:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:51:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:51:00Z- handleDvl() - RDI_Pathfinder msg: 1613177460.26 epoch sec; seqNo:7821 2021-02-13T00:51:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:51:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.12 , 0.13 2021-02-13T00:51:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177460.27 epoch sec; seqNo:16295; 4060200.71 north; 593242.18 east 2021-02-13T00:51:00Z- AHRS_M2 msg: 1613177460.66 epoch sec; seqNo:16169 2021-02-13T00:51:00Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.15 psi 2021-02-13T00:51:00Z- handleDepth()-Depth_Keller msg: 1613177460660 epoch sec; seqNo:16296; depth 19.66 2021-02-13T00:51:00Z- handleDvl() - RDI_Pathfinder msg: 1613177460.67 epoch sec; seqNo:7822 2021-02-13T00:51:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.79 , 91.62 , 100.14 , 78.24 2021-02-13T00:51:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.15 , 0.08 2021-02-13T00:51:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177460.68 epoch sec; seqNo:16296; 4060200.71 north; 593242.18 east 2021-02-13T00:51:01Z- AHRS_M2 msg: 1613177461.06 epoch sec; seqNo:16170 2021-02-13T00:51:01Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.15 psi 2021-02-13T00:51:01Z- handleDepth()-Depth_Keller msg: 1613177461066 epoch sec; seqNo:16297; depth 19.57 2021-02-13T00:51:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177461.08 epoch sec; seqNo:16297; 4060201.10 north; 593243.39 east 2021-02-13T00:51:01Z- AHRS_M2 msg: 1613177461.46 epoch sec; seqNo:16171 2021-02-13T00:51:01Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.15 psi 2021-02-13T00:51:01Z- handleDepth()-Depth_Keller msg: 1613177461468 epoch sec; seqNo:16298; depth 19.45 2021-02-13T00:51:01Z- handleDvl() - RDI_Pathfinder msg: 1613177461.49 epoch sec; seqNo:7823 2021-02-13T00:51:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.64 , 98.12 , 98.12 , 78.64 2021-02-13T00:51:01Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.16 , -0.01 2021-02-13T00:51:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177461.50 epoch sec; seqNo:16298; 4060201.48 north; 593243.39 east 2021-02-13T00:51:01Z- AHRS_M2 msg: 1613177461.87 epoch sec; seqNo:16172 2021-02-13T00:51:01Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-13T00:51:01Z- handleDepth()-Depth_Keller msg: 1613177461877 epoch sec; seqNo:16299; depth 19.34 2021-02-13T00:51:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177461.89 epoch sec; seqNo:16299; 4060201.86 north; 593243.39 east 2021-02-13T00:51:02Z- AHRS_M2 msg: 1613177462.30 epoch sec; seqNo:16173 2021-02-13T00:51:02Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-13T00:51:02Z- handleDepth()-Depth_Keller msg: 1613177462300 epoch sec; seqNo:16300; depth 19.24 2021-02-13T00:51:02Z- handleDvl() - RDI_Pathfinder msg: 1613177462.31 epoch sec; seqNo:7824 2021-02-13T00:51:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 97.91 , 101.56 , 69.07 2021-02-13T00:51:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.09 , -0.11 2021-02-13T00:51:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177462.33 epoch sec; seqNo:16300; 4060201.86 north; 593243.39 east 2021-02-13T00:51:02Z- LcmTrn::handleMessages() - handling 48 messages took 0.66 ms and 135 calls... 2021-02-13T00:51:02Z- waiting for 1613177460.05, time is 1613177462.33 2021-02-13T00:51:02Z- LcmTrn::updateTrn() equating pose 1613177462.33 and meas times 1613177462.33 2021-02-13T00:51:02Z- LcmTrn::updateTrn() >>>> pose time 1613177462.33, meas time 1613177462.33 <<<< 2021-02-13T00:51:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177462.33 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:51:02Z- TerrainNav::motionUpdate() - currEstimate = (4060202, 593243.39, 19.24) 2021-02-13T00:51:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:51:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:51:02Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177462.33 sec.; No relevant navigation data available 2021-02-13T00:51:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:51:02Z- TNavBF::mmse selected from filter -1 2021-02-13T00:51:02Z- variances([0], [2], and psi_berg) = 685.284437 832.990786 0.000000 2021-02-13T00:51:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:51:02Z- LcmTrn::publishEstimates() - MLE : 4060201.36 593263.89 19.24 2021-02-13T00:51:02Z- LcmTrn::publishEstimates() - MMSE : 4060201.86 593243.39 19.24 2021-02-13T00:51:02Z- LcmTrn::publishEstimates() - COVAR: 685.28 832.99 1.24 0.00 2021-02-13T00:51:02Z- AHRS_M2 msg: 1613177462.68 epoch sec; seqNo:16174 2021-02-13T00:51:02Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.15 psi 2021-02-13T00:51:02Z- handleDepth()-Depth_Keller msg: 1613177462686 epoch sec; seqNo:16301; depth 19.13 2021-02-13T00:51:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:51:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:51:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:51:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:51:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:51:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:51:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:51:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:51:02Z- handleDvl() - RDI_Pathfinder msg: 1613177462.69 epoch sec; seqNo:7825 2021-02-13T00:51:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:51:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.09 , -0.11 2021-02-13T00:51:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177462.70 epoch sec; seqNo:16301; 4060202.24 north; 593243.38 east 2021-02-13T00:51:03Z- AHRS_M2 msg: 1613177463.08 epoch sec; seqNo:16175 2021-02-13T00:51:03Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-13T00:51:03Z- handleDepth()-Depth_Keller msg: 1613177463084 epoch sec; seqNo:16302; depth 19.04 2021-02-13T00:51:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177463.09 epoch sec; seqNo:16302; 4060202.63 north; 593244.60 east 2021-02-13T00:51:03Z- AHRS_M2 msg: 1613177463.48 epoch sec; seqNo:16176 2021-02-13T00:51:03Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-13T00:51:03Z- handleDepth()-Depth_Keller msg: 1613177463493 epoch sec; seqNo:16303; depth 18.94 2021-02-13T00:51:03Z- handleDvl() - RDI_Pathfinder msg: 1613177463.50 epoch sec; seqNo:7826 2021-02-13T00:51:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 101.70 , 68.66 2021-02-13T00:51:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.05 , -0.17 2021-02-13T00:51:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177463.51 epoch sec; seqNo:16303; 4060202.63 north; 593244.60 east 2021-02-13T00:51:03Z- AHRS_M2 msg: 1613177463.89 epoch sec; seqNo:16177 2021-02-13T00:51:03Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-13T00:51:03Z- handleDepth()-Depth_Keller msg: 1613177463897 epoch sec; seqNo:16304; depth 18.83 2021-02-13T00:51:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:51:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:51:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:51:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:51:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:51:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:51:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:51:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:51:03Z- handleDvl() - RDI_Pathfinder msg: 1613177463.90 epoch sec; seqNo:7827 2021-02-13T00:51:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:51:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.05 , -0.17 2021-02-13T00:51:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177463.91 epoch sec; seqNo:16304; 4060203.01 north; 593244.59 east 2021-02-13T00:51:04Z- AHRS_M2 msg: 1613177464.29 epoch sec; seqNo:16178 2021-02-13T00:51:04Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.14 psi 2021-02-13T00:51:04Z- handleDepth()-Depth_Keller msg: 1613177464302 epoch sec; seqNo:16305; depth 18.72 2021-02-13T00:51:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177464.32 epoch sec; seqNo:16305; 4060203.39 north; 593244.59 east 2021-02-13T00:51:04Z- AHRS_M2 msg: 1613177464.70 epoch sec; seqNo:16179 2021-02-13T00:51:04Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.15 psi 2021-02-13T00:51:04Z- handleDepth()-Depth_Keller msg: 1613177464700 epoch sec; seqNo:16306; depth 18.61 2021-02-13T00:51:04Z- handleDvl() - RDI_Pathfinder msg: 1613177464.71 epoch sec; seqNo:7828 2021-02-13T00:51:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 101.70 , 69.48 2021-02-13T00:51:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , -0.15 2021-02-13T00:51:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177464.74 epoch sec; seqNo:16306; 4060203.77 north; 593244.59 east 2021-02-13T00:51:05Z- AHRS_M2 msg: 1613177465.10 epoch sec; seqNo:16180 2021-02-13T00:51:05Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.15 psi 2021-02-13T00:51:05Z- handleDepth()-Depth_Keller msg: 1613177465104 epoch sec; seqNo:16307; depth 18.51 2021-02-13T00:51:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:51:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:51:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:51:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:51:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:51:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:51:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:51:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:51:05Z- handleDvl() - RDI_Pathfinder msg: 1613177465.11 epoch sec; seqNo:7829 2021-02-13T00:51:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:51:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , -0.15 2021-02-13T00:51:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177465.12 epoch sec; seqNo:16307; 4060203.77 north; 593244.59 east 2021-02-13T00:51:05Z- AHRS_M2 msg: 1613177465.51 epoch sec; seqNo:16181 2021-02-13T00:51:05Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.15 psi 2021-02-13T00:51:05Z- handleDepth()-Depth_Keller msg: 1613177465513 epoch sec; seqNo:16308; depth 18.41 2021-02-13T00:51:05Z- handleDvl() - RDI_Pathfinder msg: 1613177465.52 epoch sec; seqNo:7830 2021-02-13T00:51:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 102.92 , 78.58 2021-02-13T00:51:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , -0.07 2021-02-13T00:51:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177465.54 epoch sec; seqNo:16308; 4060204.15 north; 593244.58 east 2021-02-13T00:51:05Z- AHRS_M2 msg: 1613177465.91 epoch sec; seqNo:16182 2021-02-13T00:51:05Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-13T00:51:05Z- handleDepth()-Depth_Keller msg: 1613177465912 epoch sec; seqNo:16309; depth 18.28 2021-02-13T00:51:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177465.93 epoch sec; seqNo:16309; 4060204.54 north; 593245.80 east 2021-02-13T00:51:06Z- AHRS_M2 msg: 1613177466.31 epoch sec; seqNo:16183 2021-02-13T00:51:06Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.16 psi 2021-02-13T00:51:06Z- handleDepth()-Depth_Keller msg: 1613177466316 epoch sec; seqNo:16310; depth 18.17 2021-02-13T00:51:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:51:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:51:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:51:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:51:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:51:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:51:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:51:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:51:06Z- handleDvl() - RDI_Pathfinder msg: 1613177466.32 epoch sec; seqNo:7831 2021-02-13T00:51:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:51:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , -0.07 2021-02-13T00:51:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177466.33 epoch sec; seqNo:16310; 4060204.92 north; 593245.79 east 2021-02-13T00:51:06Z- AHRS_M2 msg: 1613177466.72 epoch sec; seqNo:16184 2021-02-13T00:51:06Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.16 psi 2021-02-13T00:51:06Z- handleDepth()-Depth_Keller msg: 1613177466720 epoch sec; seqNo:16311; depth 18.07 2021-02-13T00:51:06Z- handleDvl() - RDI_Pathfinder msg: 1613177466.73 epoch sec; seqNo:7832 2021-02-13T00:51:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 92.91 , 101.43 , 78.31 2021-02-13T00:51:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , -0.01 2021-02-13T00:51:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177466.74 epoch sec; seqNo:16311; 4060205.30 north; 593245.79 east 2021-02-13T00:51:07Z- AHRS_M2 msg: 1613177467.12 epoch sec; seqNo:16185 2021-02-13T00:51:07Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-13T00:51:07Z- handleDepth()-Depth_Keller msg: 1613177467133 epoch sec; seqNo:16312; depth 17.97 2021-02-13T00:51:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177467.15 epoch sec; seqNo:16312; 4060205.30 north; 593245.79 east 2021-02-13T00:51:07Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-13T00:51:07Z- waiting for 1613177465.33, time is 1613177467.15 2021-02-13T00:51:07Z- LcmTrn::updateTrn() >>>> pose time 1613177467.15, meas time 1613177466.73 <<<< 2021-02-13T00:51:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177466.73 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:51:07Z- TerrainNav::motionUpdate() - currEstimate = (4060205, 593245.79, 17.97) 2021-02-13T00:51:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:51:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:51:07Z- TNavBF::Projecting Measurements 2021-02-13T00:51:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:51:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:51:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:51:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:51:07Z- TNavFilter::4 good beams. 2021-02-13T00:51:07Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:51:07Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:51:07Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:51:07Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:51:07Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:51:07Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:51:07Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:51:07Z- TNavBF::mmse selected from filter -1 2021-02-13T00:51:07Z- TerrainMapOctree: (vehN, vehE ) = (4060205.00, 593245.58). 2021-02-13T00:51:07Z- TerrainMapOctree: Min Distance = 1449.44. 2021-02-13T00:51:07Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:51:07Z- There is no specific good beam that all particles have in common. 2021-02-13T00:51:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:51:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.44 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 9.81 2021-02-13T00:51:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:51:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 13.81 2021-02-13T00:51:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:51:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 16.46 2021-02-13T00:51:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:51:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 9.81 2021-02-13T00:51:07Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:51:07Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:51:07Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:51:07Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:51:07Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:51:07Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:51:07Z- Filter 0 No beams used in subcloud update 2021-02-13T00:51:07Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:51:07Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:51:07Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 685.2 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:51:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177466.73 sec. 2021-02-13T00:51:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:51:07Z- TNavBF::mmse selected from filter -1 2021-02-13T00:51:07Z- variances([0], [2], and psi_berg) = 685.225092 832.990759 0.000000 2021-02-13T00:51:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:51:07Z- LcmTrn::publishEstimates() - MLE : 4060204.80 593266.29 17.97 2021-02-13T00:51:07Z- LcmTrn::publishEstimates() - MMSE : 4060205.30 593245.79 17.97 2021-02-13T00:51:07Z- LcmTrn::publishEstimates() - COVAR: 685.23 832.99 1.03 0.00 2021-02-13T00:51:07Z- AHRS_M2 msg: 1613177467.52 epoch sec; seqNo:16186 2021-02-13T00:51:07Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-13T00:51:07Z- handleDepth()-Depth_Keller msg: 1613177467533 epoch sec; seqNo:16313; depth 17.86 2021-02-13T00:51:07Z- handleDvl() - RDI_Pathfinder msg: 1613177467.56 epoch sec; seqNo:7833 2021-02-13T00:51:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 102.85 , 71.93 2021-02-13T00:51:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , 0.00 2021-02-13T00:51:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177467.57 epoch sec; seqNo:16313; 4060205.67 north; 593245.79 east 2021-02-13T00:51:07Z- AHRS_M2 msg: 1613177467.93 epoch sec; seqNo:16187 2021-02-13T00:51:07Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-13T00:51:07Z- handleDepth()-Depth_Keller msg: 1613177467933 epoch sec; seqNo:16314; depth 17.74 2021-02-13T00:51:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177467.94 epoch sec; seqNo:16314; 4060206.05 north; 593245.78 east 2021-02-13T00:51:08Z- AHRS_M2 msg: 1613177468.34 epoch sec; seqNo:16188 2021-02-13T00:51:08Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.15 psi 2021-02-13T00:51:08Z- handleDepth()-Depth_Keller msg: 1613177468341 epoch sec; seqNo:16315; depth 17.64 2021-02-13T00:51:08Z- handleDvl() - RDI_Pathfinder msg: 1613177468.35 epoch sec; seqNo:7834 2021-02-13T00:51:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 97.98 , 104.07 , 70.70 2021-02-13T00:51:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , -0.04 2021-02-13T00:51:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177468.36 epoch sec; seqNo:16315; 4060206.44 north; 593247.00 east 2021-02-13T00:51:08Z- AHRS_M2 msg: 1613177468.74 epoch sec; seqNo:16189 2021-02-13T00:51:08Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.14 psi 2021-02-13T00:51:08Z- handleDepth()-Depth_Keller msg: 1613177468742 epoch sec; seqNo:16316; depth 17.53 2021-02-13T00:51:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177468.75 epoch sec; seqNo:16316; 4060206.44 north; 593247.00 east 2021-02-13T00:51:09Z- AHRS_M2 msg: 1613177469.14 epoch sec; seqNo:16190 2021-02-13T00:51:09Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.14 psi 2021-02-13T00:51:09Z- handleDepth()-Depth_Keller msg: 1613177469158 epoch sec; seqNo:16317; depth 17.43 2021-02-13T00:51:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177469.17 epoch sec; seqNo:16317; 4060206.82 north; 593246.99 east 2021-02-13T00:51:09Z- AHRS_M2 msg: 1613177469.54 epoch sec; seqNo:16191 2021-02-13T00:51:09Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.14 psi 2021-02-13T00:51:09Z- handleDepth()-Depth_Keller msg: 1613177469553 epoch sec; seqNo:16318; depth 17.31 2021-02-13T00:51:09Z- handleDvl() - RDI_Pathfinder msg: 1613177469.56 epoch sec; seqNo:7835 2021-02-13T00:51:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:51:09Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:51:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177469.60 epoch sec; seqNo:16318; 4060207.20 north; 593246.99 east 2021-02-13T00:51:09Z- AHRS_M2 msg: 1613177469.95 epoch sec; seqNo:16192 2021-02-13T00:51:09Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:51:09Z- handleDepth()-Depth_Keller msg: 1613177469965 epoch sec; seqNo:16319; depth 17.19 2021-02-13T00:51:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177469.97 epoch sec; seqNo:16319; 4060207.20 north; 593246.99 east 2021-02-13T00:51:10Z- AHRS_M2 msg: 1613177470.35 epoch sec; seqNo:16193 2021-02-13T00:51:10Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:51:10Z- handleDepth()-Depth_Keller msg: 1613177470358 epoch sec; seqNo:16320; depth 17.09 2021-02-13T00:51:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177470.37 epoch sec; seqNo:16320; 4060207.58 north; 593246.99 east 2021-02-13T00:51:10Z- AHRS_M2 msg: 1613177470.76 epoch sec; seqNo:16194 2021-02-13T00:51:10Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T00:51:10Z- handleDepth()-Depth_Keller msg: 1613177470760 epoch sec; seqNo:16321; depth 16.97 2021-02-13T00:51:10Z- handleDvl() - RDI_Pathfinder msg: 1613177470.78 epoch sec; seqNo:7836 2021-02-13T00:51:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 97.91 , 103.86 , 78.58 2021-02-13T00:51:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , 0.02 2021-02-13T00:51:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177470.79 epoch sec; seqNo:16321; 4060207.96 north; 593246.98 east 2021-02-13T00:51:11Z- AHRS_M2 msg: 1613177471.16 epoch sec; seqNo:16195 2021-02-13T00:51:11Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-13T00:51:11Z- handleDepth()-Depth_Keller msg: 1613177471165 epoch sec; seqNo:16322; depth 16.87 2021-02-13T00:51:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177471.18 epoch sec; seqNo:16322; 4060208.35 north; 593248.20 east 2021-02-13T00:51:11Z- AHRS_M2 msg: 1613177471.57 epoch sec; seqNo:16196 2021-02-13T00:51:11Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.15 psi 2021-02-13T00:51:11Z- handleDepth()-Depth_Keller msg: 1613177471571 epoch sec; seqNo:16323; depth 16.75 2021-02-13T00:51:11Z- handleDvl() - RDI_Pathfinder msg: 1613177471.58 epoch sec; seqNo:7837 2021-02-13T00:51:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 97.78 , 105.08 , 72.34 2021-02-13T00:51:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.09 , 0.10 2021-02-13T00:51:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177471.59 epoch sec; seqNo:16323; 4060208.35 north; 593248.20 east 2021-02-13T00:51:11Z- AHRS_M2 msg: 1613177471.97 epoch sec; seqNo:16197 2021-02-13T00:51:11Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.15 psi 2021-02-13T00:51:11Z- handleDepth()-Depth_Keller msg: 1613177471973 epoch sec; seqNo:16324; depth 16.64 2021-02-13T00:51:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177471.98 epoch sec; seqNo:16324; 4060208.73 north; 593248.19 east 2021-02-13T00:51:12Z- AHRS_M2 msg: 1613177472.38 epoch sec; seqNo:16198 2021-02-13T00:51:12Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T00:51:12Z- handleDepth()-Depth_Keller msg: 1613177472386 epoch sec; seqNo:16325; depth 16.53 2021-02-13T00:51:12Z- handleDvl() - RDI_Pathfinder msg: 1613177472.40 epoch sec; seqNo:7838 2021-02-13T00:51:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.66 , 97.91 , 105.21 , 71.52 2021-02-13T00:51:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.05 , 0.15 2021-02-13T00:51:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177472.41 epoch sec; seqNo:16325; 4060209.11 north; 593248.19 east 2021-02-13T00:51:12Z- LcmTrn::handleMessages() - handling 45 messages took 0.67 ms and 132 calls... 2021-02-13T00:51:12Z- waiting for 1613177470.15, time is 1613177472.41 2021-02-13T00:51:12Z- LcmTrn::updateTrn() equating pose 1613177472.41 and meas times 1613177472.41 2021-02-13T00:51:12Z- LcmTrn::updateTrn() >>>> pose time 1613177472.41, meas time 1613177472.41 <<<< 2021-02-13T00:51:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177472.41 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:51:12Z- TerrainNav::motionUpdate() - currEstimate = (4060209, 593248.19, 16.53) 2021-02-13T00:51:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:51:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:51:12Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177472.41 sec.; No relevant navigation data available 2021-02-13T00:51:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:51:12Z- TNavBF::mmse selected from filter -1 2021-02-13T00:51:12Z- variances([0], [2], and psi_berg) = 685.175913 832.990737 0.000000 2021-02-13T00:51:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:51:12Z- LcmTrn::publishEstimates() - MLE : 4060208.61 593268.69 16.53 2021-02-13T00:51:12Z- LcmTrn::publishEstimates() - MMSE : 4060209.11 593248.19 16.53 2021-02-13T00:51:12Z- LcmTrn::publishEstimates() - COVAR: 685.18 832.99 1.26 0.00 2021-02-13T00:51:12Z- AHRS_M2 msg: 1613177472.78 epoch sec; seqNo:16199 2021-02-13T00:51:12Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T00:51:12Z- handleDepth()-Depth_Keller msg: 1613177472784 epoch sec; seqNo:16326; depth 16.42 2021-02-13T00:51:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177472.80 epoch sec; seqNo:16326; 4060209.49 north; 593248.19 east 2021-02-13T00:51:13Z- AHRS_M2 msg: 1613177473.18 epoch sec; seqNo:16200 2021-02-13T00:51:13Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.16 psi 2021-02-13T00:51:13Z- handleDepth()-Depth_Keller msg: 1613177473184 epoch sec; seqNo:16327; depth 16.30 2021-02-13T00:51:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177473.19 epoch sec; seqNo:16327; 4060209.87 north; 593248.18 east 2021-02-13T00:51:13Z- AHRS_M2 msg: 1613177473.59 epoch sec; seqNo:16201 2021-02-13T00:51:13Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.16 psi 2021-02-13T00:51:13Z- handleDepth()-Depth_Keller msg: 1613177473594 epoch sec; seqNo:16328; depth 16.19 2021-02-13T00:51:13Z- handleDvl() - RDI_Pathfinder msg: 1613177473.61 epoch sec; seqNo:7839 2021-02-13T00:51:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.88 , 97.91 , 106.43 , 78.44 2021-02-13T00:51:13Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.05 , 0.12 2021-02-13T00:51:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177473.62 epoch sec; seqNo:16328; 4060210.26 north; 593249.40 east 2021-02-13T00:51:13Z- AHRS_M2 msg: 1613177473.99 epoch sec; seqNo:16202 2021-02-13T00:51:13Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.16 psi 2021-02-13T00:51:13Z- handleDepth()-Depth_Keller msg: 1613177473993 epoch sec; seqNo:16329; depth 16.07 2021-02-13T00:51:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177474.02 epoch sec; seqNo:16329; 4060210.26 north; 593249.40 east 2021-02-13T00:51:14Z- AHRS_M2 msg: 1613177474.40 epoch sec; seqNo:16203 2021-02-13T00:51:14Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-13T00:51:14Z- handleDepth()-Depth_Keller msg: 1613177474427 epoch sec; seqNo:16330; depth 15.97 2021-02-13T00:51:14Z- handleDvl() - RDI_Pathfinder msg: 1613177474.45 epoch sec; seqNo:7840 2021-02-13T00:51:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.67 , 95.28 , 107.45 , 78.24 2021-02-13T00:51:14Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.13 , 0.05 2021-02-13T00:51:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177474.46 epoch sec; seqNo:16330; 4060210.64 north; 593249.39 east 2021-02-13T00:51:14Z- AHRS_M2 msg: 1613177474.80 epoch sec; seqNo:16204 2021-02-13T00:51:14Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-13T00:51:14Z- handleDepth()-Depth_Keller msg: 1613177474809 epoch sec; seqNo:16331; depth 15.85 2021-02-13T00:51:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177474.82 epoch sec; seqNo:16331; 4060211.02 north; 593249.39 east 2021-02-13T00:51:15Z- AHRS_M2 msg: 1613177475.20 epoch sec; seqNo:16205 2021-02-13T00:51:15Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-13T00:51:15Z- handleDepth()-Depth_Keller msg: 1613177475206 epoch sec; seqNo:16332; depth 15.75 2021-02-13T00:51:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177475.21 epoch sec; seqNo:16332; 4060211.40 north; 593249.39 east 2021-02-13T00:51:15Z- AHRS_M2 msg: 1613177475.60 epoch sec; seqNo:16206 2021-02-13T00:51:15Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-13T00:51:15Z- handleDepth()-Depth_Keller msg: 1613177475613 epoch sec; seqNo:16333; depth 15.64 2021-02-13T00:51:15Z- handleDvl() - RDI_Pathfinder msg: 1613177475.64 epoch sec; seqNo:7841 2021-02-13T00:51:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 96.49 , 108.66 , 73.97 2021-02-13T00:51:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.18 , -0.04 2021-02-13T00:51:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177475.65 epoch sec; seqNo:16333; 4060211.41 north; 593250.61 east 2021-02-13T00:51:16Z- AHRS_M2 msg: 1613177476.01 epoch sec; seqNo:16207 2021-02-13T00:51:16Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-13T00:51:16Z- handleDepth()-Depth_Keller msg: 1613177476017 epoch sec; seqNo:16334; depth 15.54 2021-02-13T00:51:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177476.05 epoch sec; seqNo:16334; 4060211.79 north; 593250.60 east 2021-02-13T00:51:16Z- AHRS_M2 msg: 1613177476.41 epoch sec; seqNo:16208 2021-02-13T00:51:16Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-13T00:51:16Z- handleDepth()-Depth_Keller msg: 1613177476416 epoch sec; seqNo:16335; depth 15.41 2021-02-13T00:51:16Z- handleDvl() - RDI_Pathfinder msg: 1613177476.43 epoch sec; seqNo:7842 2021-02-13T00:51:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 97.71 , 109.89 , 73.16 2021-02-13T00:51:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.14 , -0.12 2021-02-13T00:51:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177476.44 epoch sec; seqNo:16335; 4060212.17 north; 593250.60 east 2021-02-13T00:51:16Z- AHRS_M2 msg: 1613177476.82 epoch sec; seqNo:16209 2021-02-13T00:51:16Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-13T00:51:16Z- handleDepth()-Depth_Keller msg: 1613177476821 epoch sec; seqNo:16336; depth 15.31 2021-02-13T00:51:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177476.83 epoch sec; seqNo:16336; 4060212.17 north; 593250.60 east 2021-02-13T00:51:17Z- AHRS_M2 msg: 1613177477.22 epoch sec; seqNo:16210 2021-02-13T00:51:17Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-13T00:51:17Z- handleDepth()-Depth_Keller msg: 1613177477225 epoch sec; seqNo:16337; depth 15.18 2021-02-13T00:51:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177477.24 epoch sec; seqNo:16337; 4060212.55 north; 593250.59 east 2021-02-13T00:51:17Z- LcmTrn::handleMessages() - handling 40 messages took 0.67 ms and 127 calls... 2021-02-13T00:51:17Z- waiting for 1613177475.41, time is 1613177477.24 2021-02-13T00:51:17Z- LcmTrn::updateTrn() >>>> pose time 1613177477.24, meas time 1613177476.43 <<<< 2021-02-13T00:51:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177476.43 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:51:17Z- TerrainNav::motionUpdate() - currEstimate = (4060213, 593250.59, 15.18) 2021-02-13T00:51:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:51:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:51:17Z- TNavBF::Projecting Measurements 2021-02-13T00:51:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:51:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:51:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:51:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:51:17Z- TNavFilter::4 good beams. 2021-02-13T00:51:17Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:51:17Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:51:17Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:51:17Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:51:17Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:51:17Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:51:17Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:51:17Z- TNavBF::mmse selected from filter -1 2021-02-13T00:51:17Z- TerrainMapOctree: (vehN, vehE ) = (4060211.97, 593250.19). 2021-02-13T00:51:17Z- TerrainMapOctree: Min Distance = 1441.22. 2021-02-13T00:51:17Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:51:17Z- There is no specific good beam that all particles have in common. 2021-02-13T00:51:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:51:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.44 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 10.73 2021-02-13T00:51:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:51:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:51:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:51:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 19.32 2021-02-13T00:51:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:51:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-13T00:51:17Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:51:17Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:51:17Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:51:17Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:51:17Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:51:17Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:51:17Z- Filter 0 No beams used in subcloud update 2021-02-13T00:51:17Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:51:17Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:51:17Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 685.1 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:51:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177476.43 sec. 2021-02-13T00:51:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:51:17Z- TNavBF::mmse selected from filter -1 2021-02-13T00:51:17Z- variances([0], [2], and psi_berg) = 685.113240 832.990709 0.000000 2021-02-13T00:51:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:51:17Z- LcmTrn::publishEstimates() - MLE : 4060212.05 593271.09 15.18 2021-02-13T00:51:17Z- LcmTrn::publishEstimates() - MMSE : 4060212.55 593250.59 15.18 2021-02-13T00:51:17Z- LcmTrn::publishEstimates() - COVAR: 685.11 832.99 1.06 0.00 2021-02-13T00:51:17Z- AHRS_M2 msg: 1613177477.62 epoch sec; seqNo:16211 2021-02-13T00:51:17Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-13T00:51:17Z- handleDepth()-Depth_Keller msg: 1613177477633 epoch sec; seqNo:16338; depth 15.07 2021-02-13T00:51:17Z- handleDvl() - RDI_Pathfinder msg: 1613177477.65 epoch sec; seqNo:7843 2021-02-13T00:51:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 96.50 , 112.32 , 72.35 2021-02-13T00:51:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , -0.14 2021-02-13T00:51:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177477.66 epoch sec; seqNo:16338; 4060212.94 north; 593251.81 east 2021-02-13T00:51:18Z- AHRS_M2 msg: 1613177478.03 epoch sec; seqNo:16212 2021-02-13T00:51:18Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.16 psi 2021-02-13T00:51:18Z- handleDepth()-Depth_Keller msg: 1613177478032 epoch sec; seqNo:16339; depth 14.93 2021-02-13T00:51:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177478.04 epoch sec; seqNo:16339; 4060212.94 north; 593251.81 east 2021-02-13T00:51:18Z- AHRS_M2 msg: 1613177478.44 epoch sec; seqNo:16213 2021-02-13T00:51:18Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-13T00:51:18Z- handleDepth()-Depth_Keller msg: 1613177478440 epoch sec; seqNo:16340; depth 14.83 2021-02-13T00:51:18Z- handleDvl() - RDI_Pathfinder msg: 1613177478.45 epoch sec; seqNo:7844 2021-02-13T00:51:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 97.71 , 106.23 , 72.75 2021-02-13T00:51:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , -0.06 2021-02-13T00:51:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177478.46 epoch sec; seqNo:16340; 4060213.32 north; 593251.81 east 2021-02-13T00:51:18Z- AHRS_M2 msg: 1613177478.84 epoch sec; seqNo:16214 2021-02-13T00:51:18Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-13T00:51:18Z- handleDepth()-Depth_Keller msg: 1613177478840 epoch sec; seqNo:16341; depth 14.70 2021-02-13T00:51:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177478.86 epoch sec; seqNo:16341; 4060213.70 north; 593251.80 east 2021-02-13T00:51:19Z- AHRS_M2 msg: 1613177479.24 epoch sec; seqNo:16215 2021-02-13T00:51:19Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-13T00:51:19Z- handleDepth()-Depth_Keller msg: 1613177479249 epoch sec; seqNo:16342; depth 14.58 2021-02-13T00:51:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177479.26 epoch sec; seqNo:16342; 4060213.70 north; 593251.80 east 2021-02-13T00:51:19Z- AHRS_M2 msg: 1613177479.65 epoch sec; seqNo:16216 2021-02-13T00:51:19Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.18 psi 2021-02-13T00:51:19Z- handleDepth()-Depth_Keller msg: 1613177479654 epoch sec; seqNo:16343; depth 14.46 2021-02-13T00:51:19Z- handleDvl() - RDI_Pathfinder msg: 1613177479.67 epoch sec; seqNo:7845 2021-02-13T00:51:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 99.00 , 109.95 , 74.38 2021-02-13T00:51:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , 0.01 2021-02-13T00:51:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177479.68 epoch sec; seqNo:16343; 4060214.08 north; 593251.80 east 2021-02-13T00:51:20Z- AHRS_M2 msg: 1613177480.05 epoch sec; seqNo:16217 2021-02-13T00:51:20Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.18 psi 2021-02-13T00:51:20Z- handleDepth()-Depth_Keller msg: 1613177480054 epoch sec; seqNo:16344; depth 14.33 2021-02-13T00:51:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177480.07 epoch sec; seqNo:16344; 4060214.45 north; 593251.79 east 2021-02-13T00:51:20Z- AHRS_M2 msg: 1613177480.45 epoch sec; seqNo:16218 2021-02-13T00:51:20Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.18 psi 2021-02-13T00:51:20Z- handleDepth()-Depth_Keller msg: 1613177480456 epoch sec; seqNo:16345; depth 14.20 2021-02-13T00:51:20Z- handleDvl() - RDI_Pathfinder msg: 1613177480.47 epoch sec; seqNo:7846 2021-02-13T00:51:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.08 , 117.11 , 74.79 2021-02-13T00:51:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , 0.00 2021-02-13T00:51:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177480.48 epoch sec; seqNo:16345; 4060214.85 north; 593253.01 east 2021-02-13T00:51:20Z- AHRS_M2 msg: 1613177480.86 epoch sec; seqNo:16219 2021-02-13T00:51:20Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.18 psi 2021-02-13T00:51:20Z- handleDepth()-Depth_Keller msg: 1613177480860 epoch sec; seqNo:16346; depth 14.07 2021-02-13T00:51:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177480.87 epoch sec; seqNo:16346; 4060215.22 north; 593253.01 east 2021-02-13T00:51:21Z- AHRS_M2 msg: 1613177481.26 epoch sec; seqNo:16220 2021-02-13T00:51:21Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.18 psi 2021-02-13T00:51:21Z- handleDepth()-Depth_Keller msg: 1613177481264 epoch sec; seqNo:16347; depth 13.95 2021-02-13T00:51:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177481.27 epoch sec; seqNo:16347; 4060215.22 north; 593253.01 east 2021-02-13T00:51:21Z- AHRS_M2 msg: 1613177481.67 epoch sec; seqNo:16221 2021-02-13T00:51:21Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.18 psi 2021-02-13T00:51:21Z- handleDepth()-Depth_Keller msg: 1613177481676 epoch sec; seqNo:16348; depth 13.85 2021-02-13T00:51:21Z- handleDvl() - RDI_Pathfinder msg: 1613177481.69 epoch sec; seqNo:7847 2021-02-13T00:51:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:51:21Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:51:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177481.70 epoch sec; seqNo:16348; 4060215.60 north; 593253.00 east 2021-02-13T00:51:22Z- AHRS_M2 msg: 1613177482.07 epoch sec; seqNo:16222 2021-02-13T00:51:22Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.17 psi 2021-02-13T00:51:22Z- handleDepth()-Depth_Keller msg: 1613177482072 epoch sec; seqNo:16349; depth 13.71 2021-02-13T00:51:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177482.09 epoch sec; seqNo:16349; 4060215.98 north; 593253.00 east 2021-02-13T00:51:22Z- AHRS_M2 msg: 1613177482.47 epoch sec; seqNo:16223 2021-02-13T00:51:22Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.17 psi 2021-02-13T00:51:22Z- handleDepth()-Depth_Keller msg: 1613177482476 epoch sec; seqNo:16350; depth 13.58 2021-02-13T00:51:22Z- handleDvl() - RDI_Pathfinder msg: 1613177482.49 epoch sec; seqNo:7848 2021-02-13T00:51:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.90 , 100.75 , 117.11 , 78.44 2021-02-13T00:51:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , -0.08 2021-02-13T00:51:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177482.51 epoch sec; seqNo:16350; 4060216.36 north; 593252.99 east 2021-02-13T00:51:22Z- LcmTrn::handleMessages() - handling 45 messages took 0.64 ms and 132 calls... 2021-02-13T00:51:22Z- waiting for 1613177480.24, time is 1613177482.51 2021-02-13T00:51:22Z- LcmTrn::updateTrn() equating pose 1613177482.51 and meas times 1613177482.51 2021-02-13T00:51:22Z- LcmTrn::updateTrn() >>>> pose time 1613177482.51, meas time 1613177482.51 <<<< 2021-02-13T00:51:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177482.51 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:51:22Z- TerrainNav::motionUpdate() - currEstimate = (4060216, 593252.99, 13.58) 2021-02-13T00:51:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:51:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:51:22Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177482.51 sec.; No relevant navigation data available 2021-02-13T00:51:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:51:22Z- TNavBF::mmse selected from filter -1 2021-02-13T00:51:22Z- variances([0], [2], and psi_berg) = 685.064119 832.990687 0.000000 2021-02-13T00:51:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:51:22Z- LcmTrn::publishEstimates() - MLE : 4060215.86 593272.49 13.58 2021-02-13T00:51:22Z- LcmTrn::publishEstimates() - MMSE : 4060216.36 593252.99 13.58 2021-02-13T00:51:22Z- LcmTrn::publishEstimates() - COVAR: 685.06 832.99 1.29 0.00 2021-02-13T00:51:22Z- AHRS_M2 msg: 1613177482.88 epoch sec; seqNo:16224 2021-02-13T00:51:22Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.16 psi 2021-02-13T00:51:22Z- handleDepth()-Depth_Keller msg: 1613177482880 epoch sec; seqNo:16351; depth 13.47 2021-02-13T00:51:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177482.89 epoch sec; seqNo:16351; 4060216.37 north; 593254.21 east 2021-02-13T00:51:23Z- AHRS_M2 msg: 1613177483.28 epoch sec; seqNo:16225 2021-02-13T00:51:23Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.16 psi 2021-02-13T00:51:23Z- handleDepth()-Depth_Keller msg: 1613177483293 epoch sec; seqNo:16352; depth 13.35 2021-02-13T00:51:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177483.31 epoch sec; seqNo:16352; 4060216.75 north; 593254.21 east 2021-02-13T00:51:23Z- AHRS_M2 msg: 1613177483.68 epoch sec; seqNo:16226 2021-02-13T00:51:23Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.16 psi 2021-02-13T00:51:23Z- handleDepth()-Depth_Keller msg: 1613177483689 epoch sec; seqNo:16353; depth 13.23 2021-02-13T00:51:23Z- handleDvl() - RDI_Pathfinder msg: 1613177483.71 epoch sec; seqNo:7849 2021-02-13T00:51:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 100.21 , 112.38 , 74.38 2021-02-13T00:51:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.00 , -0.10 2021-02-13T00:51:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177483.72 epoch sec; seqNo:16353; 4060217.13 north; 593254.21 east 2021-02-13T00:51:24Z- AHRS_M2 msg: 1613177484.09 epoch sec; seqNo:16227 2021-02-13T00:51:24Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-13T00:51:24Z- handleDepth()-Depth_Keller msg: 1613177484097 epoch sec; seqNo:16354; depth 13.09 2021-02-13T00:51:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177484.11 epoch sec; seqNo:16354; 4060217.13 north; 593254.21 east 2021-02-13T00:51:24Z- AHRS_M2 msg: 1613177484.49 epoch sec; seqNo:16228 2021-02-13T00:51:24Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-13T00:51:24Z- handleDepth()-Depth_Keller msg: 1613177484498 epoch sec; seqNo:16355; depth 12.98 2021-02-13T00:51:24Z- handleDvl() - RDI_Pathfinder msg: 1613177484.51 epoch sec; seqNo:7850 2021-02-13T00:51:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:51:24Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:51:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177484.52 epoch sec; seqNo:16355; 4060217.51 north; 593254.20 east 2021-02-13T00:51:24Z- AHRS_M2 msg: 1613177484.89 epoch sec; seqNo:16229 2021-02-13T00:51:24Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.16 psi 2021-02-13T00:51:24Z- handleDepth()-Depth_Keller msg: 1613177484908 epoch sec; seqNo:16356; depth 12.85 2021-02-13T00:51:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177484.92 epoch sec; seqNo:16356; 4060217.89 north; 593254.20 east 2021-02-13T00:51:25Z- AHRS_M2 msg: 1613177485.30 epoch sec; seqNo:16230 2021-02-13T00:51:25Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.16 psi 2021-02-13T00:51:25Z- handleDepth()-Depth_Keller msg: 1613177485304 epoch sec; seqNo:16357; depth 12.73 2021-02-13T00:51:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177485.33 epoch sec; seqNo:16357; 4060217.89 north; 593254.20 east 2021-02-13T00:51:25Z- AHRS_M2 msg: 1613177485.70 epoch sec; seqNo:16231 2021-02-13T00:51:25Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.15 psi 2021-02-13T00:51:25Z- handleDepth()-Depth_Keller msg: 1613177485713 epoch sec; seqNo:16358; depth 12.61 2021-02-13T00:51:25Z- handleDvl() - RDI_Pathfinder msg: 1613177485.72 epoch sec; seqNo:7851 2021-02-13T00:51:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.02 , 114.54 , 78.04 2021-02-13T00:51:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.12 , 0.09 2021-02-13T00:51:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177485.75 epoch sec; seqNo:16358; 4060218.28 north; 593255.41 east 2021-02-13T00:51:26Z- AHRS_M2 msg: 1613177486.11 epoch sec; seqNo:16232 2021-02-13T00:51:26Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.15 psi 2021-02-13T00:51:26Z- handleDepth()-Depth_Keller msg: 1613177486112 epoch sec; seqNo:16359; depth 12.48 2021-02-13T00:51:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177486.12 epoch sec; seqNo:16359; 4060218.66 north; 593255.41 east 2021-02-13T00:51:26Z- AHRS_M2 msg: 1613177486.52 epoch sec; seqNo:16233 2021-02-13T00:51:26Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.15 psi 2021-02-13T00:51:26Z- handleDepth()-Depth_Keller msg: 1613177486525 epoch sec; seqNo:16360; depth 12.36 2021-02-13T00:51:26Z- handleDvl() - RDI_Pathfinder msg: 1613177486.54 epoch sec; seqNo:7852 2021-02-13T00:51:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.42 , 99.94 , 116.17 , 78.31 2021-02-13T00:51:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.04 , 0.19 2021-02-13T00:51:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177486.55 epoch sec; seqNo:16360; 4060219.04 north; 593255.41 east 2021-02-13T00:51:26Z- AHRS_M2 msg: 1613177486.92 epoch sec; seqNo:16234 2021-02-13T00:51:26Z- AHRS_M2 msg: 0.57 phi; 0.37 theta; -0.15 psi 2021-02-13T00:51:26Z- handleDepth()-Depth_Keller msg: 1613177486920 epoch sec; seqNo:16361; depth 12.23 2021-02-13T00:51:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177486.94 epoch sec; seqNo:16361; 4060219.42 north; 593255.40 east 2021-02-13T00:51:27Z- AHRS_M2 msg: 1613177487.32 epoch sec; seqNo:16235 2021-02-13T00:51:27Z- AHRS_M2 msg: 0.57 phi; 0.37 theta; -0.16 psi 2021-02-13T00:51:27Z- handleDepth()-Depth_Keller msg: 1613177487333 epoch sec; seqNo:16362; depth 12.10 2021-02-13T00:51:27Z- handleDvl() - RDI_Pathfinder msg: 1613177487.35 epoch sec; seqNo:7853 2021-02-13T00:51:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.14 , 113.67 , 74.38 2021-02-13T00:51:27Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , 0.07 , 0.15 2021-02-13T00:51:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177487.36 epoch sec; seqNo:16362; 4060219.80 north; 593255.40 east 2021-02-13T00:51:27Z- LcmTrn::handleMessages() - handling 41 messages took 0.67 ms and 128 calls... 2021-02-13T00:51:27Z- waiting for 1613177485.51, time is 1613177487.36 2021-02-13T00:51:27Z- LcmTrn::updateTrn() equating pose 1613177487.36 and meas times 1613177487.36 2021-02-13T00:51:27Z- LcmTrn::updateTrn() >>>> pose time 1613177487.36, meas time 1613177487.36 <<<< 2021-02-13T00:51:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177487.36 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:51:27Z- TerrainNav::motionUpdate() - currEstimate = (4060220, 593255.40, 12.10) 2021-02-13T00:51:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:51:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:51:27Z- TNavBF::Projecting Measurements 2021-02-13T00:51:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:51:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:51:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:51:27Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:51:27Z- TNavFilter::3 good beams. 2021-02-13T00:51:27Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:51:27Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:51:27Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:51:27Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:51:27Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:51:27Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:51:27Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:51:27Z- TNavBF::mmse selected from filter -1 2021-02-13T00:51:27Z- TerrainMapOctree: (vehN, vehE ) = (4060219.80, 593255.40). 2021-02-13T00:51:27Z- TerrainMapOctree: Min Distance = 1431.97. 2021-02-13T00:51:27Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:51:27Z- There is no specific good beam that all particles have in common. 2021-02-13T00:51:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:51:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.50 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 16.05 2021-02-13T00:51:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:51:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 20.67 2021-02-13T00:51:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:51:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.110 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-13T00:51:27Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:51:27Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:51:27Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:51:27Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:51:27Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:51:27Z- Filter 0 No beams used in subcloud update 2021-02-13T00:51:27Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:51:27Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:51:27Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 685.0 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:51:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177487.36 sec. 2021-02-13T00:51:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:51:27Z- TNavBF::mmse selected from filter -1 2021-02-13T00:51:27Z- variances([0], [2], and psi_berg) = 685.016761 832.990666 0.000000 2021-02-13T00:51:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:51:27Z- LcmTrn::publishEstimates() - MLE : 4060219.30 593275.90 12.10 2021-02-13T00:51:27Z- LcmTrn::publishEstimates() - MMSE : 4060219.80 593255.40 12.10 2021-02-13T00:51:27Z- LcmTrn::publishEstimates() - COVAR: 685.02 832.99 1.16 0.00 2021-02-13T00:51:27Z- AHRS_M2 msg: 1613177487.72 epoch sec; seqNo:16236 2021-02-13T00:51:27Z- AHRS_M2 msg: 0.56 phi; 0.38 theta; -0.16 psi 2021-02-13T00:51:27Z- handleDepth()-Depth_Keller msg: 1613177487733 epoch sec; seqNo:16363; depth 11.95 2021-02-13T00:51:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177487.74 epoch sec; seqNo:16363; 4060220.18 north; 593255.39 east 2021-02-13T00:51:28Z- AHRS_M2 msg: 1613177488.13 epoch sec; seqNo:16237 2021-02-13T00:51:28Z- AHRS_M2 msg: 0.57 phi; 0.38 theta; -0.16 psi 2021-02-13T00:51:28Z- handleDepth()-Depth_Keller msg: 1613177488143 epoch sec; seqNo:16364; depth 11.85 2021-02-13T00:51:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177488.15 epoch sec; seqNo:16364; 4060220.19 north; 593256.61 east 2021-02-13T00:51:28Z- AHRS_M2 msg: 1613177488.53 epoch sec; seqNo:16238 2021-02-13T00:51:28Z- AHRS_M2 msg: 0.57 phi; 0.38 theta; -0.16 psi 2021-02-13T00:51:28Z- handleDepth()-Depth_Keller msg: 1613177488536 epoch sec; seqNo:16365; depth 11.72 2021-02-13T00:51:28Z- handleDvl() - RDI_Pathfinder msg: 1613177488.55 epoch sec; seqNo:7854 2021-02-13T00:51:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 101.43 , 119.01 , 78.44 2021-02-13T00:51:28Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.21 , 0.00 2021-02-13T00:51:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177488.56 epoch sec; seqNo:16365; 4060220.57 north; 593256.61 east 2021-02-13T00:51:28Z- AHRS_M2 msg: 1613177488.94 epoch sec; seqNo:16239 2021-02-13T00:51:28Z- AHRS_M2 msg: 0.58 phi; 0.38 theta; -0.15 psi 2021-02-13T00:51:28Z- handleDepth()-Depth_Keller msg: 1613177488940 epoch sec; seqNo:16366; depth 11.58 2021-02-13T00:51:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177488.95 epoch sec; seqNo:16366; 4060220.95 north; 593256.61 east 2021-02-13T00:51:29Z- AHRS_M2 msg: 1613177489.34 epoch sec; seqNo:16240 2021-02-13T00:51:29Z- AHRS_M2 msg: 0.60 phi; 0.38 theta; -0.15 psi 2021-02-13T00:51:29Z- handleDepth()-Depth_Keller msg: 1613177489349 epoch sec; seqNo:16367; depth 11.49 2021-02-13T00:51:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177489.36 epoch sec; seqNo:16367; 4060221.33 north; 593256.60 east 2021-02-13T00:51:29Z- AHRS_M2 msg: 1613177489.75 epoch sec; seqNo:16241 2021-02-13T00:51:29Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.14 psi 2021-02-13T00:51:29Z- handleDepth()-Depth_Keller msg: 1613177489750 epoch sec; seqNo:16368; depth 11.35 2021-02-13T00:51:29Z- handleDvl() - RDI_Pathfinder msg: 1613177489.76 epoch sec; seqNo:7855 2021-02-13T00:51:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.91 , 96.02 , 120.36 , 78.44 2021-02-13T00:51:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.18 , -0.17 2021-02-13T00:51:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177489.77 epoch sec; seqNo:16368; 4060221.34 north; 593257.82 east 2021-02-13T00:51:30Z- AHRS_M2 msg: 1613177490.15 epoch sec; seqNo:16242 2021-02-13T00:51:30Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.15 psi 2021-02-13T00:51:30Z- handleDepth()-Depth_Keller msg: 1613177490156 epoch sec; seqNo:16369; depth 11.23 2021-02-13T00:51:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177490.17 epoch sec; seqNo:16369; 4060221.72 north; 593257.82 east 2021-02-13T00:51:30Z- AHRS_M2 msg: 1613177490.55 epoch sec; seqNo:16243 2021-02-13T00:51:30Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.15 psi 2021-02-13T00:51:30Z- handleDepth()-Depth_Keller msg: 1613177490556 epoch sec; seqNo:16370; depth 11.12 2021-02-13T00:51:30Z- handleDvl() - RDI_Pathfinder msg: 1613177490.57 epoch sec; seqNo:7856 2021-02-13T00:51:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.43 , 120.36 , 135.23 2021-02-13T00:51:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.63 , -0.14 , -0.32 2021-02-13T00:51:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177490.59 epoch sec; seqNo:16370; 4060221.72 north; 593257.82 east 2021-02-13T00:51:30Z- AHRS_M2 msg: 1613177490.96 epoch sec; seqNo:16244 2021-02-13T00:51:30Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.16 psi 2021-02-13T00:51:30Z- handleDepth()-Depth_Keller msg: 1613177490960 epoch sec; seqNo:16371; depth 10.97 2021-02-13T00:51:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177490.97 epoch sec; seqNo:16371; 4060222.10 north; 593257.81 east 2021-02-13T00:51:31Z- AHRS_M2 msg: 1613177491.36 epoch sec; seqNo:16245 2021-02-13T00:51:31Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.17 psi 2021-02-13T00:51:31Z- handleDepth()-Depth_Keller msg: 1613177491366 epoch sec; seqNo:16372; depth 10.84 2021-02-13T00:51:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177491.38 epoch sec; seqNo:16372; 4060222.10 north; 593257.81 east 2021-02-13T00:51:31Z- AHRS_M2 msg: 1613177491.77 epoch sec; seqNo:16246 2021-02-13T00:51:31Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.18 psi 2021-02-13T00:51:31Z- handleDepth()-Depth_Keller msg: 1613177491779 epoch sec; seqNo:16373; depth 10.72 2021-02-13T00:51:31Z- handleDvl() - RDI_Pathfinder msg: 1613177491.79 epoch sec; seqNo:7857 2021-02-13T00:51:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.82 , 103.19 , 118.06 , 77.90 2021-02-13T00:51:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.11 , -0.11 2021-02-13T00:51:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177491.81 epoch sec; seqNo:16373; 4060222.48 north; 593257.81 east 2021-02-13T00:51:32Z- AHRS_M2 msg: 1613177492.17 epoch sec; seqNo:16247 2021-02-13T00:51:32Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.18 psi 2021-02-13T00:51:32Z- handleDepth()-Depth_Keller msg: 1613177492177 epoch sec; seqNo:16374; depth 10.57 2021-02-13T00:51:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177492.19 epoch sec; seqNo:16374; 4060222.85 north; 593257.81 east 2021-02-13T00:51:32Z- AHRS_M2 msg: 1613177492.57 epoch sec; seqNo:16248 2021-02-13T00:51:32Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.18 psi 2021-02-13T00:51:32Z- handleDepth()-Depth_Keller msg: 1613177492576 epoch sec; seqNo:16375; depth 10.43 2021-02-13T00:51:32Z- handleDvl() - RDI_Pathfinder msg: 1613177492.59 epoch sec; seqNo:7858 2021-02-13T00:51:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 105.42 , 118.94 , 78.37 2021-02-13T00:51:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.12 , 0.10 2021-02-13T00:51:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177492.60 epoch sec; seqNo:16375; 4060223.23 north; 593257.80 east 2021-02-13T00:51:32Z- LcmTrn::handleMessages() - handling 44 messages took 0.61 ms and 132 calls... 2021-02-13T00:51:32Z- waiting for 1613177490.36, time is 1613177492.60 2021-02-13T00:51:32Z- LcmTrn::updateTrn() equating pose 1613177492.60 and meas times 1613177492.60 2021-02-13T00:51:32Z- LcmTrn::updateTrn() >>>> pose time 1613177492.60, meas time 1613177492.60 <<<< 2021-02-13T00:51:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177492.60 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:51:32Z- TerrainNav::motionUpdate() - currEstimate = (4060223, 593257.80, 10.43) 2021-02-13T00:51:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:51:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:51:32Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177492.60 sec.; No relevant navigation data available 2021-02-13T00:51:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:51:32Z- TNavBF::mmse selected from filter -1 2021-02-13T00:51:32Z- variances([0], [2], and psi_berg) = 684.969426 832.990645 0.000000 2021-02-13T00:51:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:51:32Z- LcmTrn::publishEstimates() - MLE : 4060222.73 593277.30 10.43 2021-02-13T00:51:32Z- LcmTrn::publishEstimates() - MMSE : 4060223.23 593257.80 10.43 2021-02-13T00:51:32Z- LcmTrn::publishEstimates() - COVAR: 684.97 832.99 1.37 0.00 2021-02-13T00:51:32Z- AHRS_M2 msg: 1613177492.98 epoch sec; seqNo:16249 2021-02-13T00:51:32Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.17 psi 2021-02-13T00:51:32Z- handleDepth()-Depth_Keller msg: 1613177492984 epoch sec; seqNo:16376; depth 10.31 2021-02-13T00:51:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177492.99 epoch sec; seqNo:16376; 4060223.23 north; 593257.80 east 2021-02-13T00:51:33Z- AHRS_M2 msg: 1613177493.38 epoch sec; seqNo:16250 2021-02-13T00:51:33Z- AHRS_M2 msg: 0.58 phi; 0.38 theta; -0.17 psi 2021-02-13T00:51:33Z- handleDepth()-Depth_Keller msg: 1613177493404 epoch sec; seqNo:16377; depth 10.18 2021-02-13T00:51:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177493.43 epoch sec; seqNo:16377; 4060223.61 north; 593257.80 east 2021-02-13T00:51:33Z- AHRS_M2 msg: 1613177493.78 epoch sec; seqNo:16251 2021-02-13T00:51:33Z- AHRS_M2 msg: 0.57 phi; 0.38 theta; -0.16 psi 2021-02-13T00:51:33Z- handleDepth()-Depth_Keller msg: 1613177493788 epoch sec; seqNo:16378; depth 10.04 2021-02-13T00:51:33Z- handleDvl() - RDI_Pathfinder msg: 1613177493.80 epoch sec; seqNo:7859 2021-02-13T00:51:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.35 , 121.58 , 78.31 2021-02-13T00:51:33Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.02 , 0.21 2021-02-13T00:51:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177493.82 epoch sec; seqNo:16378; 4060224.00 north; 593259.01 east 2021-02-13T00:51:34Z- AHRS_M2 msg: 1613177494.19 epoch sec; seqNo:16252 2021-02-13T00:51:34Z- AHRS_M2 msg: 0.57 phi; 0.39 theta; -0.16 psi 2021-02-13T00:51:34Z- handleDepth()-Depth_Keller msg: 1613177494193 epoch sec; seqNo:16379; depth 9.90 2021-02-13T00:51:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177494.20 epoch sec; seqNo:16379; 4060224.38 north; 593259.01 east 2021-02-13T00:51:34Z- AHRS_M2 msg: 1613177494.60 epoch sec; seqNo:16253 2021-02-13T00:51:34Z- AHRS_M2 msg: 0.58 phi; 0.39 theta; -0.15 psi 2021-02-13T00:51:34Z- handleDepth()-Depth_Keller msg: 1613177494601 epoch sec; seqNo:16380; depth 9.77 2021-02-13T00:51:34Z- handleDvl() - RDI_Pathfinder msg: 1613177494.61 epoch sec; seqNo:7860 2021-02-13T00:51:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.98 , 105.42 , 124.75 , 0.00 2021-02-13T00:51:34Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , 0.15 , 0.10 2021-02-13T00:51:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177494.62 epoch sec; seqNo:16380; 4060224.76 north; 593259.01 east 2021-02-13T00:51:34Z- AHRS_M2 msg: 1613177495.00 epoch sec; seqNo:16254 2021-02-13T00:51:34Z- AHRS_M2 msg: 0.59 phi; 0.39 theta; -0.14 psi 2021-02-13T00:51:35Z- handleDepth()-Depth_Keller msg: 1613177495000 epoch sec; seqNo:16381; depth 9.65 2021-02-13T00:51:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177495.02 epoch sec; seqNo:16381; 4060225.14 north; 593259.00 east 2021-02-13T00:51:35Z- AHRS_M2 msg: 1613177495.40 epoch sec; seqNo:16255 2021-02-13T00:51:35Z- AHRS_M2 msg: 0.60 phi; 0.39 theta; -0.14 psi 2021-02-13T00:51:35Z- handleDepth()-Depth_Keller msg: 1613177495404 epoch sec; seqNo:16382; depth 9.53 2021-02-13T00:51:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177495.41 epoch sec; seqNo:16382; 4060225.14 north; 593259.00 east 2021-02-13T00:51:35Z- AHRS_M2 msg: 1613177495.80 epoch sec; seqNo:16256 2021-02-13T00:51:35Z- AHRS_M2 msg: 0.62 phi; 0.39 theta; -0.15 psi 2021-02-13T00:51:35Z- handleDepth()-Depth_Keller msg: 1613177495808 epoch sec; seqNo:16383; depth 9.40 2021-02-13T00:51:35Z- handleDvl() - RDI_Pathfinder msg: 1613177495.82 epoch sec; seqNo:7861 2021-02-13T00:51:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.31 , 103.25 , 121.10 , 76.60 2021-02-13T00:51:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.21 , -0.12 2021-02-13T00:51:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177495.83 epoch sec; seqNo:16383; 4060225.53 north; 593260.22 east 2021-02-13T00:51:36Z- AHRS_M2 msg: 1613177496.21 epoch sec; seqNo:16257 2021-02-13T00:51:36Z- AHRS_M2 msg: 0.63 phi; 0.38 theta; -0.15 psi 2021-02-13T00:51:36Z- handleDepth()-Depth_Keller msg: 1613177496215 epoch sec; seqNo:16384; depth 9.26 2021-02-13T00:51:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177496.23 epoch sec; seqNo:16384; 4060225.91 north; 593260.21 east 2021-02-13T00:51:36Z- AHRS_M2 msg: 1613177496.61 epoch sec; seqNo:16258 2021-02-13T00:51:36Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.16 psi 2021-02-13T00:51:36Z- handleDepth()-Depth_Keller msg: 1613177496616 epoch sec; seqNo:16385; depth 9.11 2021-02-13T00:51:36Z- handleDvl() - RDI_Pathfinder msg: 1613177496.63 epoch sec; seqNo:7862 2021-02-13T00:51:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.03 , 103.79 , 129.49 , 79.46 2021-02-13T00:51:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.01 , -0.23 2021-02-13T00:51:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177496.65 epoch sec; seqNo:16385; 4060225.91 north; 593260.21 east 2021-02-13T00:51:37Z- AHRS_M2 msg: 1613177497.02 epoch sec; seqNo:16259 2021-02-13T00:51:37Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.16 psi 2021-02-13T00:51:37Z- handleDepth()-Depth_Keller msg: 1613177497025 epoch sec; seqNo:16386; depth 8.97 2021-02-13T00:51:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177497.04 epoch sec; seqNo:16386; 4060226.29 north; 593260.21 east 2021-02-13T00:51:37Z- AHRS_M2 msg: 1613177497.42 epoch sec; seqNo:16260 2021-02-13T00:51:37Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.16 psi 2021-02-13T00:51:37Z- handleDepth()-Depth_Keller msg: 1613177497430 epoch sec; seqNo:16387; depth 8.82 2021-02-13T00:51:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177497.44 epoch sec; seqNo:16387; 4060226.29 north; 593260.21 east 2021-02-13T00:51:37Z- AHRS_M2 msg: 1613177497.82 epoch sec; seqNo:16261 2021-02-13T00:51:37Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.16 psi 2021-02-13T00:51:37Z- LcmTrn::handleMessages() - handling 41 messages took 0.66 ms and 128 calls... 2021-02-13T00:51:37Z- waiting for 1613177495.60, time is 1613177497.82 2021-02-13T00:51:37Z- LcmTrn::updateTrn() >>>> pose time 1613177497.82, meas time 1613177496.63 <<<< 2021-02-13T00:51:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177496.63 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:51:37Z- TerrainNav::motionUpdate() - currEstimate = (4060226, 593260.21, 8.82) 2021-02-13T00:51:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:51:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:51:37Z- TNavBF::Projecting Measurements 2021-02-13T00:51:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:51:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:51:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:51:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:51:37Z- TNavFilter::4 good beams. 2021-02-13T00:51:37Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:51:37Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:51:37Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:51:37Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:51:37Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:51:37Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:51:37Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:51:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:51:37Z- TerrainMapOctree: (vehN, vehE ) = (4060225.59, 593259.66). 2021-02-13T00:51:37Z- TerrainMapOctree: Min Distance = 1424.85. 2021-02-13T00:51:37Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:51:37Z- There is no specific good beam that all particles have in common. 2021-02-13T00:51:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:51:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.53 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 15.06 2021-02-13T00:51:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:51:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 17.24 2021-02-13T00:51:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:51:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 26.83 2021-02-13T00:51:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:51:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 10.10 2021-02-13T00:51:37Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:51:37Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:51:37Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:51:37Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:51:37Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:51:37Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:51:37Z- Filter 0 No beams used in subcloud update 2021-02-13T00:51:37Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:51:37Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:51:37Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 684.9 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:51:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177496.63 sec. 2021-02-13T00:51:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:51:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:51:37Z- variances([0], [2], and psi_berg) = 684.907609 832.990617 0.000000 2021-02-13T00:51:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:51:37Z- LcmTrn::publishEstimates() - MLE : 4060225.79 593279.71 8.82 2021-02-13T00:51:37Z- LcmTrn::publishEstimates() - MMSE : 4060226.29 593260.21 8.82 2021-02-13T00:51:37Z- LcmTrn::publishEstimates() - COVAR: 684.91 832.99 1.17 0.00 2021-02-13T00:51:37Z- handleDepth()-Depth_Keller msg: 1613177497833 epoch sec; seqNo:16388; depth 8.67 2021-02-13T00:51:37Z- handleDvl() - RDI_Pathfinder msg: 1613177497.85 epoch sec; seqNo:7863 2021-02-13T00:51:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 109.81 , 124.69 , 80.07 2021-02-13T00:51:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.17 , -0.09 2021-02-13T00:51:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177497.86 epoch sec; seqNo:16388; 4060226.67 north; 593260.21 east 2021-02-13T00:51:38Z- AHRS_M2 msg: 1613177498.23 epoch sec; seqNo:16262 2021-02-13T00:51:38Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.16 psi 2021-02-13T00:51:38Z- handleDepth()-Depth_Keller msg: 1613177498237 epoch sec; seqNo:16389; depth 8.52 2021-02-13T00:51:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177498.25 epoch sec; seqNo:16389; 4060227.05 north; 593260.20 east 2021-02-13T00:51:38Z- AHRS_M2 msg: 1613177498.63 epoch sec; seqNo:16263 2021-02-13T00:51:38Z- AHRS_M2 msg: 0.61 phi; 0.38 theta; -0.16 psi 2021-02-13T00:51:38Z- handleDepth()-Depth_Keller msg: 1613177498639 epoch sec; seqNo:16390; depth 8.37 2021-02-13T00:51:38Z- handleDvl() - RDI_Pathfinder msg: 1613177498.65 epoch sec; seqNo:7864 2021-02-13T00:51:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 106.43 , 121.31 , 78.06 2021-02-13T00:51:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.12 , 0.10 2021-02-13T00:51:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177498.66 epoch sec; seqNo:16390; 4060227.44 north; 593261.42 east 2021-02-13T00:51:39Z- AHRS_M2 msg: 1613177499.04 epoch sec; seqNo:16264 2021-02-13T00:51:39Z- AHRS_M2 msg: 0.60 phi; 0.38 theta; -0.17 psi 2021-02-13T00:51:39Z- handleDepth()-Depth_Keller msg: 1613177499042 epoch sec; seqNo:16391; depth 8.23 2021-02-13T00:51:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177499.05 epoch sec; seqNo:16391; 4060227.44 north; 593261.42 east 2021-02-13T00:51:39Z- AHRS_M2 msg: 1613177499.45 epoch sec; seqNo:16265 2021-02-13T00:51:39Z- AHRS_M2 msg: 0.59 phi; 0.38 theta; -0.17 psi 2021-02-13T00:51:39Z- handleDepth()-Depth_Keller msg: 1613177499457 epoch sec; seqNo:16392; depth 8.09 2021-02-13T00:51:39Z- handleDvl() - RDI_Pathfinder msg: 1613177499.47 epoch sec; seqNo:7865 2021-02-13T00:51:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 107.78 , 126.71 , 79.69 2021-02-13T00:51:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , 0.15 2021-02-13T00:51:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177499.49 epoch sec; seqNo:16392; 4060227.82 north; 593261.41 east 2021-02-13T00:51:39Z- AHRS_M2 msg: 1613177499.84 epoch sec; seqNo:16266 2021-02-13T00:51:39Z- AHRS_M2 msg: 0.59 phi; 0.38 theta; -0.18 psi 2021-02-13T00:51:39Z- handleDepth()-Depth_Keller msg: 1613177499853 epoch sec; seqNo:16393; depth 7.93 2021-02-13T00:51:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177499.86 epoch sec; seqNo:16393; 4060228.20 north; 593261.41 east 2021-02-13T00:51:40Z- AHRS_M2 msg: 1613177500.25 epoch sec; seqNo:16267 2021-02-13T00:51:40Z- AHRS_M2 msg: 0.60 phi; 0.38 theta; -0.17 psi 2021-02-13T00:51:40Z- handleDepth()-Depth_Keller msg: 1613177500254 epoch sec; seqNo:16394; depth 7.79 2021-02-13T00:51:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177500.26 epoch sec; seqNo:16394; 4060228.58 north; 593261.41 east 2021-02-13T00:51:40Z- AHRS_M2 msg: 1613177500.65 epoch sec; seqNo:16268 2021-02-13T00:51:40Z- AHRS_M2 msg: 0.61 phi; 0.38 theta; -0.16 psi 2021-02-13T00:51:40Z- handleDepth()-Depth_Keller msg: 1613177500659 epoch sec; seqNo:16395; depth 7.66 2021-02-13T00:51:40Z- handleDvl() - RDI_Pathfinder msg: 1613177500.68 epoch sec; seqNo:7866 2021-02-13T00:51:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 108.26 , 124.62 , 80.00 2021-02-13T00:51:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , 0.11 2021-02-13T00:51:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177500.69 epoch sec; seqNo:16395; 4060228.96 north; 593261.40 east 2021-02-13T00:51:41Z- AHRS_M2 msg: 1613177501.06 epoch sec; seqNo:16269 2021-02-13T00:51:41Z- AHRS_M2 msg: 0.62 phi; 0.38 theta; -0.16 psi 2021-02-13T00:51:41Z- handleDepth()-Depth_Keller msg: 1613177501061 epoch sec; seqNo:16396; depth 7.53 2021-02-13T00:51:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177501.07 epoch sec; seqNo:16396; 4060228.97 north; 593262.62 east 2021-02-13T00:51:41Z- AHRS_M2 msg: 1613177501.48 epoch sec; seqNo:16270 2021-02-13T00:51:41Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.16 psi 2021-02-13T00:51:41Z- handleDepth()-Depth_Keller msg: 1613177501487 epoch sec; seqNo:16397; depth 7.39 2021-02-13T00:51:41Z- handleDvl() - RDI_Pathfinder msg: 1613177501.51 epoch sec; seqNo:7867 2021-02-13T00:51:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.41 , 105.28 , 124.62 , 80.00 2021-02-13T00:51:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.08 , 0.03 2021-02-13T00:51:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177501.52 epoch sec; seqNo:16397; 4060229.35 north; 593262.62 east 2021-02-13T00:51:41Z- AHRS_M2 msg: 1613177501.86 epoch sec; seqNo:16271 2021-02-13T00:51:41Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.16 psi 2021-02-13T00:51:41Z- handleDepth()-Depth_Keller msg: 1613177501868 epoch sec; seqNo:16398; depth 7.23 2021-02-13T00:51:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177501.89 epoch sec; seqNo:16398; 4060229.73 north; 593262.61 east 2021-02-13T00:51:42Z- AHRS_M2 msg: 1613177502.27 epoch sec; seqNo:16272 2021-02-13T00:51:42Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.16 psi 2021-02-13T00:51:42Z- handleDepth()-Depth_Keller msg: 1613177502273 epoch sec; seqNo:16399; depth 7.12 2021-02-13T00:51:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177502.28 epoch sec; seqNo:16399; 4060229.73 north; 593262.61 east 2021-02-13T00:51:42Z- AHRS_M2 msg: 1613177502.67 epoch sec; seqNo:16273 2021-02-13T00:51:42Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.17 psi 2021-02-13T00:51:42Z- handleDepth()-Depth_Keller msg: 1613177502681 epoch sec; seqNo:16400; depth 6.97 2021-02-13T00:51:42Z- handleDvl() - RDI_Pathfinder msg: 1613177502.69 epoch sec; seqNo:7868 2021-02-13T00:51:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.03 , 107.85 , 125.43 , 78.88 2021-02-13T00:51:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.03 , 0.02 2021-02-13T00:51:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177502.72 epoch sec; seqNo:16400; 4060230.11 north; 593262.61 east 2021-02-13T00:51:42Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-13T00:51:42Z- waiting for 1613177500.82, time is 1613177502.72 2021-02-13T00:51:42Z- LcmTrn::updateTrn() equating pose 1613177502.72 and meas times 1613177502.72 2021-02-13T00:51:42Z- LcmTrn::updateTrn() >>>> pose time 1613177502.72, meas time 1613177502.72 <<<< 2021-02-13T00:51:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177502.72 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:51:42Z- TerrainNav::motionUpdate() - currEstimate = (4060230, 593262.61, 6.97) 2021-02-13T00:51:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:51:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:51:42Z- TNavBF::Projecting Measurements 2021-02-13T00:51:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:51:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:51:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:51:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:51:42Z- TNavFilter::4 good beams. 2021-02-13T00:51:42Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:51:42Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:51:42Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:51:42Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:51:42Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:51:42Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:51:42Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:51:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:51:42Z- TerrainMapOctree: (vehN, vehE ) = (4060230.11, 593262.61). 2021-02-13T00:51:42Z- TerrainMapOctree: Min Distance = 1419.55. 2021-02-13T00:51:42Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:51:42Z- There is no specific good beam that all particles have in common. 2021-02-13T00:51:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:51:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 15.06 2021-02-13T00:51:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:51:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 18.61 2021-02-13T00:51:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:51:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 25.17 2021-02-13T00:51:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:51:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 9.95 2021-02-13T00:51:42Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:51:42Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:51:42Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:51:42Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:51:42Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:51:42Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:51:42Z- Filter 0 No beams used in subcloud update 2021-02-13T00:51:42Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:51:42Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:51:42Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 684.9 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:51:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177502.72 sec. 2021-02-13T00:51:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:51:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:51:42Z- variances([0], [2], and psi_berg) = 684.858592 832.990595 0.000000 2021-02-13T00:51:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:51:42Z- LcmTrn::publishEstimates() - MLE : 4060229.61 593282.11 6.97 2021-02-13T00:51:42Z- LcmTrn::publishEstimates() - MMSE : 4060230.11 593262.61 6.97 2021-02-13T00:51:42Z- LcmTrn::publishEstimates() - COVAR: 684.86 832.99 1.06 0.00 2021-02-13T00:51:43Z- AHRS_M2 msg: 1613177503.08 epoch sec; seqNo:16274 2021-02-13T00:51:43Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-13T00:51:43Z- handleDepth()-Depth_Keller msg: 1613177503086 epoch sec; seqNo:16401; depth 6.82 2021-02-13T00:51:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177503.10 epoch sec; seqNo:16401; 4060230.50 north; 593263.83 east 2021-02-13T00:51:43Z- AHRS_M2 msg: 1613177503.49 epoch sec; seqNo:16275 2021-02-13T00:51:43Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.18 psi 2021-02-13T00:51:43Z- handleDepth()-Depth_Keller msg: 1613177503497 epoch sec; seqNo:16402; depth 6.69 2021-02-13T00:51:43Z- handleDvl() - RDI_Pathfinder msg: 1613177503.51 epoch sec; seqNo:7869 2021-02-13T00:51:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.48 , 108.32 , 124.69 , 81.55 2021-02-13T00:51:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , 0.14 2021-02-13T00:51:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177503.52 epoch sec; seqNo:16402; 4060230.50 north; 593263.83 east 2021-02-13T00:51:43Z- AHRS_M2 msg: 1613177503.88 epoch sec; seqNo:16276 2021-02-13T00:51:43Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.17 psi 2021-02-13T00:51:43Z- handleDepth()-Depth_Keller msg: 1613177503888 epoch sec; seqNo:16403; depth 6.56 2021-02-13T00:51:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177503.91 epoch sec; seqNo:16403; 4060230.88 north; 593263.82 east 2021-02-13T00:51:44Z- AHRS_M2 msg: 1613177504.29 epoch sec; seqNo:16277 2021-02-13T00:51:44Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.17 psi 2021-02-13T00:51:44Z- handleDepth()-Depth_Keller msg: 1613177504293 epoch sec; seqNo:16404; depth 6.44 2021-02-13T00:51:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177504.30 epoch sec; seqNo:16404; 4060231.26 north; 593263.82 east 2021-02-13T00:51:44Z- AHRS_M2 msg: 1613177504.70 epoch sec; seqNo:16278 2021-02-13T00:51:44Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.16 psi 2021-02-13T00:51:44Z- handleDepth()-Depth_Keller msg: 1613177504700 epoch sec; seqNo:16405; depth 6.31 2021-02-13T00:51:44Z- handleDvl() - RDI_Pathfinder msg: 1613177504.71 epoch sec; seqNo:7870 2021-02-13T00:51:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.98 , 108.39 , 123.27 , 81.62 2021-02-13T00:51:44Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.26 , 0.18 2021-02-13T00:51:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177504.72 epoch sec; seqNo:16405; 4060231.63 north; 593263.81 east 2021-02-13T00:51:45Z- AHRS_M2 msg: 1613177505.10 epoch sec; seqNo:16279 2021-02-13T00:51:45Z- AHRS_M2 msg: 0.61 phi; 0.38 theta; -0.14 psi 2021-02-13T00:51:45Z- handleDepth()-Depth_Keller msg: 1613177505101 epoch sec; seqNo:16406; depth 6.19 2021-02-13T00:51:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177505.11 epoch sec; seqNo:16406; 4060231.65 north; 593265.03 east 2021-02-13T00:51:45Z- AHRS_M2 msg: 1613177505.50 epoch sec; seqNo:16280 2021-02-13T00:51:45Z- AHRS_M2 msg: 0.62 phi; 0.38 theta; -0.14 psi 2021-02-13T00:51:45Z- handleDepth()-Depth_Keller msg: 1613177505504 epoch sec; seqNo:16407; depth 6.07 2021-02-13T00:51:45Z- handleDvl() - RDI_Pathfinder msg: 1613177505.52 epoch sec; seqNo:7871 2021-02-13T00:51:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 108.26 , 124.62 , 82.97 2021-02-13T00:51:45Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.38 , -0.06 2021-02-13T00:51:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177505.53 epoch sec; seqNo:16407; 4060232.03 north; 593265.03 east 2021-02-13T00:51:45Z- AHRS_M2 msg: 1613177505.90 epoch sec; seqNo:16281 2021-02-13T00:51:45Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.14 psi 2021-02-13T00:51:45Z- handleDepth()-Depth_Keller msg: 1613177505908 epoch sec; seqNo:16408; depth 5.94 2021-02-13T00:51:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177505.92 epoch sec; seqNo:16408; 4060232.41 north; 593265.03 east 2021-02-13T00:51:46Z- AHRS_M2 msg: 1613177506.31 epoch sec; seqNo:16282 2021-02-13T00:51:46Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.15 psi 2021-02-13T00:51:46Z- handleDepth()-Depth_Keller msg: 1613177506312 epoch sec; seqNo:16409; depth 5.81 2021-02-13T00:51:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177506.32 epoch sec; seqNo:16409; 4060232.41 north; 593265.03 east 2021-02-13T00:51:46Z- AHRS_M2 msg: 1613177506.71 epoch sec; seqNo:16283 2021-02-13T00:51:46Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.17 psi 2021-02-13T00:51:46Z- handleDepth()-Depth_Keller msg: 1613177506723 epoch sec; seqNo:16410; depth 5.67 2021-02-13T00:51:46Z- handleDvl() - RDI_Pathfinder msg: 1613177506.73 epoch sec; seqNo:7872 2021-02-13T00:51:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 105.28 , 129.08 , 82.97 2021-02-13T00:51:46Z- handleDvl() - RDI_Pathfinder msg: velocities 1.15 , 0.18 , -0.47 2021-02-13T00:51:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177506.74 epoch sec; seqNo:16410; 4060232.80 north; 593266.24 east 2021-02-13T00:51:47Z- AHRS_M2 msg: 1613177507.12 epoch sec; seqNo:16284 2021-02-13T00:51:47Z- AHRS_M2 msg: 0.66 phi; 0.29 theta; -0.19 psi 2021-02-13T00:51:47Z- handleDepth()-Depth_Keller msg: 1613177507122 epoch sec; seqNo:16411; depth 5.53 2021-02-13T00:51:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177507.15 epoch sec; seqNo:16411; 4060233.18 north; 593266.24 east 2021-02-13T00:51:47Z- AHRS_M2 msg: 1613177507.52 epoch sec; seqNo:16285 2021-02-13T00:51:47Z- AHRS_M2 msg: 0.67 phi; 0.24 theta; -0.21 psi 2021-02-13T00:51:47Z- handleDepth()-Depth_Keller msg: 1613177507524 epoch sec; seqNo:16412; depth 5.39 2021-02-13T00:51:47Z- handleDvl() - RDI_Pathfinder msg: 1613177507.54 epoch sec; seqNo:7873 2021-02-13T00:51:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 106.70 , 120.09 , 84.39 2021-02-13T00:51:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.22 , -0.35 2021-02-13T00:51:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177507.57 epoch sec; seqNo:16412; 4060233.18 north; 593266.24 east 2021-02-13T00:51:47Z- AHRS_M2 msg: 1613177507.95 epoch sec; seqNo:16286 2021-02-13T00:51:47Z- AHRS_M2 msg: 0.68 phi; 0.14 theta; -0.22 psi 2021-02-13T00:51:47Z- handleDepth()-Depth_Keller msg: 1613177507958 epoch sec; seqNo:16413; depth 5.24 2021-02-13T00:51:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177507.98 epoch sec; seqNo:16413; 4060233.56 north; 593266.23 east 2021-02-13T00:51:48Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-13T00:51:48Z- waiting for 1613177505.72, time is 1613177507.98 2021-02-13T00:51:48Z- LcmTrn::updateTrn() >>>> pose time 1613177507.98, meas time 1613177507.54 <<<< 2021-02-13T00:51:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177507.54 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:51:48Z- TerrainNav::motionUpdate() - currEstimate = (4060234, 593266.23, 5.24) 2021-02-13T00:51:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:51:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:51:48Z- TNavBF::Projecting Measurements 2021-02-13T00:51:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:51:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:51:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:51:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:51:48Z- TNavFilter::4 good beams. 2021-02-13T00:51:48Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:51:48Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:51:48Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:51:48Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:51:48Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:51:48Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:51:48Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:51:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:51:48Z- TerrainMapOctree: (vehN, vehE ) = (4060233.27, 593265.93). 2021-02-13T00:51:48Z- TerrainMapOctree: Min Distance = 1414.97. 2021-02-13T00:51:48Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:51:48Z- There is no specific good beam that all particles have in common. 2021-02-13T00:51:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:51:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-13T00:51:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:51:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 18.22 2021-02-13T00:51:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:51:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 23.07 2021-02-13T00:51:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:51:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 11.39 2021-02-13T00:51:48Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:51:48Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:51:48Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:51:48Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:51:48Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:51:48Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:51:48Z- Filter 0 No beams used in subcloud update 2021-02-13T00:51:48Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:51:48Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:51:48Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 684.8 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:51:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177507.54 sec. 2021-02-13T00:51:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:51:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:51:48Z- variances([0], [2], and psi_berg) = 684.794234 832.990566 0.000000 2021-02-13T00:51:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:51:48Z- LcmTrn::publishEstimates() - MLE : 4060233.06 593289.73 5.24 2021-02-13T00:51:48Z- LcmTrn::publishEstimates() - MMSE : 4060233.56 593266.23 5.24 2021-02-13T00:51:48Z- LcmTrn::publishEstimates() - COVAR: 684.79 832.99 1.02 0.00 2021-02-13T00:51:48Z- handleDepth()-Depth_Keller msg: 1613177508353 epoch sec; seqNo:16414; depth 5.12 2021-02-13T00:51:48Z- handleDvl() - RDI_Pathfinder msg: 1613177508.36 epoch sec; seqNo:7874 2021-02-13T00:51:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:51:48Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:51:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177508.38 epoch sec; seqNo:16414; 4060233.93 north; 593266.23 east 2021-02-13T00:51:48Z- AHRS_M2 msg: 1613177508.74 epoch sec; seqNo:16287 2021-02-13T00:51:48Z- AHRS_M2 msg: 0.68 phi; 0.04 theta; -0.21 psi 2021-02-13T00:51:48Z- handleDepth()-Depth_Keller msg: 1613177508761 epoch sec; seqNo:16415; depth 5.01 2021-02-13T00:51:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177508.77 epoch sec; seqNo:16415; 4060234.31 north; 593266.23 east 2021-02-13T00:51:49Z- AHRS_M2 msg: 1613177509.14 epoch sec; seqNo:16288 2021-02-13T00:51:49Z- AHRS_M2 msg: 0.68 phi; -0.00 theta; -0.21 psi 2021-02-13T00:51:49Z- handleDepth()-Depth_Keller msg: 1613177509153 epoch sec; seqNo:16416; depth 4.89 2021-02-13T00:51:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177509.16 epoch sec; seqNo:16416; 4060234.31 north; 593266.23 east 2021-02-13T00:51:49Z- AHRS_M2 msg: 1613177509.54 epoch sec; seqNo:16289 2021-02-13T00:51:49Z- AHRS_M2 msg: 0.68 phi; -0.04 theta; -0.22 psi 2021-02-13T00:51:49Z- handleDepth()-Depth_Keller msg: 1613177509547 epoch sec; seqNo:16417; depth 4.83 2021-02-13T00:51:49Z- handleDvl() - RDI_Pathfinder msg: 1613177509.56 epoch sec; seqNo:7875 2021-02-13T00:51:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.53 , 105.35 , 97.91 , 97.91 2021-02-13T00:51:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.08 , 0.28 2021-02-13T00:51:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177509.57 epoch sec; seqNo:16417; 4060234.69 north; 593266.22 east 2021-02-13T00:51:49Z- AHRS_M2 msg: 1613177509.94 epoch sec; seqNo:16290 2021-02-13T00:51:49Z- AHRS_M2 msg: 0.68 phi; -0.08 theta; -0.22 psi 2021-02-13T00:51:49Z- handleDepth()-Depth_Keller msg: 1613177509954 epoch sec; seqNo:16418; depth 4.78 2021-02-13T00:51:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177509.97 epoch sec; seqNo:16418; 4060234.69 north; 593266.22 east 2021-02-13T00:51:50Z- AHRS_M2 msg: 1613177510.35 epoch sec; seqNo:16291 2021-02-13T00:51:50Z- AHRS_M2 msg: 0.68 phi; -0.11 theta; -0.21 psi 2021-02-13T00:51:50Z- handleDepth()-Depth_Keller msg: 1613177510352 epoch sec; seqNo:16419; depth 4.74 2021-02-13T00:51:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177510.36 epoch sec; seqNo:16419; 4060235.08 north; 593267.44 east 2021-02-13T00:51:50Z- AHRS_M2 msg: 1613177510.75 epoch sec; seqNo:16292 2021-02-13T00:51:50Z- AHRS_M2 msg: 0.69 phi; -0.14 theta; -0.21 psi 2021-02-13T00:51:50Z- handleDepth()-Depth_Keller msg: 1613177510756 epoch sec; seqNo:16420; depth 4.72 2021-02-13T00:51:50Z- handleDvl() - RDI_Pathfinder msg: 1613177510.77 epoch sec; seqNo:7876 2021-02-13T00:51:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 106.77 , 90.54 , 98.66 2021-02-13T00:51:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.24 , 0.26 2021-02-13T00:51:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177510.78 epoch sec; seqNo:16420; 4060235.08 north; 593267.44 east 2021-02-13T00:51:51Z- AHRS_M2 msg: 1613177511.16 epoch sec; seqNo:16293 2021-02-13T00:51:51Z- AHRS_M2 msg: 0.69 phi; -0.16 theta; -0.21 psi 2021-02-13T00:51:51Z- handleDepth()-Depth_Keller msg: 1613177511170 epoch sec; seqNo:16421; depth 4.73 2021-02-13T00:51:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177511.18 epoch sec; seqNo:16421; 4060235.46 north; 593267.43 east 2021-02-13T00:51:51Z- AHRS_M2 msg: 1613177511.56 epoch sec; seqNo:16294 2021-02-13T00:51:51Z- AHRS_M2 msg: 0.69 phi; -0.17 theta; -0.22 psi 2021-02-13T00:51:51Z- handleDepth()-Depth_Keller msg: 1613177511564 epoch sec; seqNo:16422; depth 4.72 2021-02-13T00:51:51Z- handleDvl() - RDI_Pathfinder msg: 1613177511.58 epoch sec; seqNo:7877 2021-02-13T00:51:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.62 , 105.55 , 0.00 , 105.55 2021-02-13T00:51:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.26 , -0.01 2021-02-13T00:51:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177511.59 epoch sec; seqNo:16422; 4060235.46 north; 593267.43 east 2021-02-13T00:51:51Z- AHRS_M2 msg: 1613177511.96 epoch sec; seqNo:16295 2021-02-13T00:51:51Z- AHRS_M2 msg: 0.70 phi; -0.19 theta; -0.24 psi 2021-02-13T00:51:51Z- handleDepth()-Depth_Keller msg: 1613177511968 epoch sec; seqNo:16423; depth 4.75 2021-02-13T00:51:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177511.98 epoch sec; seqNo:16423; 4060235.86 north; 593268.65 east 2021-02-13T00:51:52Z- AHRS_M2 msg: 1613177512.37 epoch sec; seqNo:16296 2021-02-13T00:51:52Z- AHRS_M2 msg: 0.71 phi; -0.20 theta; -0.27 psi 2021-02-13T00:51:52Z- handleDepth()-Depth_Keller msg: 1613177512378 epoch sec; seqNo:16424; depth 4.78 2021-02-13T00:51:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177512.39 epoch sec; seqNo:16424; 4060236.23 north; 593268.65 east 2021-02-13T00:51:52Z- AHRS_M2 msg: 1613177512.78 epoch sec; seqNo:16297 2021-02-13T00:51:52Z- AHRS_M2 msg: 0.72 phi; -0.20 theta; -0.28 psi 2021-02-13T00:51:52Z- handleDepth()-Depth_Keller msg: 1613177512781 epoch sec; seqNo:16425; depth 4.83 2021-02-13T00:51:52Z- handleDvl() - RDI_Pathfinder msg: 1613177512.79 epoch sec; seqNo:7878 2021-02-13T00:51:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.28 , 110.62 , 0.00 , 109.27 2021-02-13T00:51:52Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , -0.06 , -0.19 2021-02-13T00:51:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177512.80 epoch sec; seqNo:16425; 4060236.61 north; 593268.64 east 2021-02-13T00:51:53Z- LcmTrn::handleMessages() - handling 40 messages took 0.64 ms and 127 calls... 2021-02-13T00:51:53Z- waiting for 1613177510.98, time is 1613177512.80 2021-02-13T00:51:53Z- LcmTrn::updateTrn() equating pose 1613177512.80 and meas times 1613177512.80 2021-02-13T00:51:53Z- LcmTrn::updateTrn() >>>> pose time 1613177512.80, meas time 1613177512.80 <<<< 2021-02-13T00:51:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177512.80 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:51:53Z- TerrainNav::motionUpdate() - currEstimate = (4060237, 593268.64, 4.83) 2021-02-13T00:51:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:51:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:51:53Z- TNavBF::Projecting Measurements 2021-02-13T00:51:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:51:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:51:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:51:53Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:51:53Z- TNavFilter::3 good beams. 2021-02-13T00:51:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:51:53Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:51:53Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:51:53Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:51:53Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:51:53Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:51:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:51:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:51:53Z- TerrainMapOctree: (vehN, vehE ) = (4060236.61, 593268.64). 2021-02-13T00:51:53Z- TerrainMapOctree: Min Distance = 1410.68. 2021-02-13T00:51:53Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:51:53Z- There is no specific good beam that all particles have in common. 2021-02-13T00:51:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:51:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 11.91 2021-02-13T00:51:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:51:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 19.58 2021-02-13T00:51:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:51:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 19.10 2021-02-13T00:51:53Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:51:53Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:51:53Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:51:53Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:51:53Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:51:53Z- Filter 0 No beams used in subcloud update 2021-02-13T00:51:53Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:51:53Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:51:53Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 684.7 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:51:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177512.80 sec. 2021-02-13T00:51:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:51:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:51:53Z- variances([0], [2], and psi_berg) = 684.748740 832.990546 0.000000 2021-02-13T00:51:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:51:53Z- LcmTrn::publishEstimates() - MLE : 4060236.11 593292.14 4.83 2021-02-13T00:51:53Z- LcmTrn::publishEstimates() - MMSE : 4060236.61 593268.64 4.83 2021-02-13T00:51:53Z- LcmTrn::publishEstimates() - COVAR: 684.75 832.99 1.01 0.00 2021-02-13T00:51:53Z- AHRS_M2 msg: 1613177513.18 epoch sec; seqNo:16298 2021-02-13T00:51:53Z- AHRS_M2 msg: 0.74 phi; -0.22 theta; -0.28 psi 2021-02-13T00:51:53Z- handleDepth()-Depth_Keller msg: 1613177513187 epoch sec; seqNo:16426; depth 4.90 2021-02-13T00:51:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177513.20 epoch sec; seqNo:16426; 4060236.61 north; 593268.64 east 2021-02-13T00:51:53Z- AHRS_M2 msg: 1613177513.58 epoch sec; seqNo:16299 2021-02-13T00:51:53Z- AHRS_M2 msg: 0.75 phi; -0.24 theta; -0.27 psi 2021-02-13T00:51:53Z- handleDepth()-Depth_Keller msg: 1613177513589 epoch sec; seqNo:16427; depth 4.96 2021-02-13T00:51:53Z- handleDvl() - RDI_Pathfinder msg: 1613177513.61 epoch sec; seqNo:7879 2021-02-13T00:51:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.53 , 114.27 , 0.00 , 109.81 2021-02-13T00:51:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.35 , -0.03 2021-02-13T00:51:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177513.62 epoch sec; seqNo:16427; 4060236.99 north; 593268.64 east 2021-02-13T00:51:53Z- AHRS_M2 msg: 1613177513.98 epoch sec; seqNo:16300 2021-02-13T00:51:53Z- AHRS_M2 msg: 0.77 phi; -0.27 theta; -0.24 psi 2021-02-13T00:51:53Z- handleDepth()-Depth_Keller msg: 1613177513988 epoch sec; seqNo:16428; depth 5.05 2021-02-13T00:51:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177514.00 epoch sec; seqNo:16428; 4060237.37 north; 593268.63 east 2021-02-13T00:51:54Z- AHRS_M2 msg: 1613177514.41 epoch sec; seqNo:16301 2021-02-13T00:51:54Z- AHRS_M2 msg: 0.77 phi; -0.30 theta; -0.21 psi 2021-02-13T00:51:54Z- handleDepth()-Depth_Keller msg: 1613177514417 epoch sec; seqNo:16429; depth 5.12 2021-02-13T00:51:54Z- handleDvl() - RDI_Pathfinder msg: 1613177514.43 epoch sec; seqNo:7880 2021-02-13T00:51:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.43 , 112.72 , 0.00 , 114.21 2021-02-13T00:51:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.55 , -0.27 , 0.26 2021-02-13T00:51:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177514.45 epoch sec; seqNo:16429; 4060237.76 north; 593269.85 east 2021-02-13T00:51:54Z- AHRS_M2 msg: 1613177514.79 epoch sec; seqNo:16302 2021-02-13T00:51:54Z- AHRS_M2 msg: 0.75 phi; -0.34 theta; -0.19 psi 2021-02-13T00:51:54Z- handleDepth()-Depth_Keller msg: 1613177514805 epoch sec; seqNo:16430; depth 5.21 2021-02-13T00:51:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177514.82 epoch sec; seqNo:16430; 4060237.76 north; 593269.85 east 2021-02-13T00:51:55Z- AHRS_M2 msg: 1613177515.20 epoch sec; seqNo:16303 2021-02-13T00:51:55Z- AHRS_M2 msg: 0.74 phi; -0.36 theta; -0.17 psi 2021-02-13T00:51:55Z- handleDepth()-Depth_Keller msg: 1613177515211 epoch sec; seqNo:16431; depth 5.29 2021-02-13T00:51:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177515.23 epoch sec; seqNo:16431; 4060238.14 north; 593269.85 east 2021-02-13T00:51:55Z- AHRS_M2 msg: 1613177515.60 epoch sec; seqNo:16304 2021-02-13T00:51:55Z- AHRS_M2 msg: 0.71 phi; -0.38 theta; -0.16 psi 2021-02-13T00:51:55Z- handleDepth()-Depth_Keller msg: 1613177515604 epoch sec; seqNo:16432; depth 5.38 2021-02-13T00:51:55Z- handleDvl() - RDI_Pathfinder msg: 1613177515.62 epoch sec; seqNo:7881 2021-02-13T00:51:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.27 , 109.61 , 0.00 , 124.48 2021-02-13T00:51:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.58 , 0.13 , 0.46 2021-02-13T00:51:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177515.64 epoch sec; seqNo:16432; 4060238.14 north; 593269.85 east 2021-02-13T00:51:56Z- AHRS_M2 msg: 1613177516.00 epoch sec; seqNo:16305 2021-02-13T00:51:56Z- AHRS_M2 msg: 0.69 phi; -0.40 theta; -0.17 psi 2021-02-13T00:51:56Z- handleDepth()-Depth_Keller msg: 1613177516008 epoch sec; seqNo:16433; depth 5.48 2021-02-13T00:51:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177516.02 epoch sec; seqNo:16433; 4060238.14 north; 593269.85 east 2021-02-13T00:51:56Z- AHRS_M2 msg: 1613177516.41 epoch sec; seqNo:16306 2021-02-13T00:51:56Z- AHRS_M2 msg: 0.67 phi; -0.40 theta; -0.20 psi 2021-02-13T00:51:56Z- handleDepth()-Depth_Keller msg: 1613177516413 epoch sec; seqNo:16434; depth 5.58 2021-02-13T00:51:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177516.42 epoch sec; seqNo:16434; 4060238.14 north; 593269.85 east 2021-02-13T00:51:56Z- AHRS_M2 msg: 1613177516.81 epoch sec; seqNo:16307 2021-02-13T00:51:56Z- AHRS_M2 msg: 0.65 phi; -0.40 theta; -0.23 psi 2021-02-13T00:51:56Z- handleDepth()-Depth_Keller msg: 1613177516816 epoch sec; seqNo:16435; depth 5.69 2021-02-13T00:51:56Z- handleDvl() - RDI_Pathfinder msg: 1613177516.83 epoch sec; seqNo:7882 2021-02-13T00:51:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.44 , 109.34 , 82.57 , 134.62 2021-02-13T00:51:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.58 , 0.28 2021-02-13T00:51:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177516.84 epoch sec; seqNo:16435; 4060238.14 north; 593269.85 east 2021-02-13T00:51:57Z- AHRS_M2 msg: 1613177517.22 epoch sec; seqNo:16308 2021-02-13T00:51:57Z- AHRS_M2 msg: 0.64 phi; -0.40 theta; -0.27 psi 2021-02-13T00:51:57Z- handleDepth()-Depth_Keller msg: 1613177517221 epoch sec; seqNo:16436; depth 5.81 2021-02-13T00:51:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177517.23 epoch sec; seqNo:16436; 4060238.53 north; 593271.06 east 2021-02-13T00:51:57Z- AHRS_M2 msg: 1613177517.62 epoch sec; seqNo:16309 2021-02-13T00:51:57Z- AHRS_M2 msg: 0.64 phi; -0.39 theta; -0.30 psi 2021-02-13T00:51:57Z- handleDepth()-Depth_Keller msg: 1613177517624 epoch sec; seqNo:16437; depth 5.94 2021-02-13T00:51:57Z- handleDvl() - RDI_Pathfinder msg: 1613177517.64 epoch sec; seqNo:7883 2021-02-13T00:51:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.57 , 118.40 , 82.70 , 133.27 2021-02-13T00:51:57Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , 0.48 , -0.24 2021-02-13T00:51:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177517.65 epoch sec; seqNo:16437; 4060238.53 north; 593271.06 east 2021-02-13T00:51:58Z- AHRS_M2 msg: 1613177518.03 epoch sec; seqNo:16310 2021-02-13T00:51:58Z- AHRS_M2 msg: 0.64 phi; -0.39 theta; -0.31 psi 2021-02-13T00:51:58Z- handleDepth()-Depth_Keller msg: 1613177518037 epoch sec; seqNo:16438; depth 6.10 2021-02-13T00:51:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177518.05 epoch sec; seqNo:16438; 4060238.91 north; 593271.06 east 2021-02-13T00:51:58Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-13T00:51:58Z- waiting for 1613177515.80, time is 1613177518.05 2021-02-13T00:51:58Z- LcmTrn::updateTrn() >>>> pose time 1613177518.05, meas time 1613177517.64 <<<< 2021-02-13T00:51:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177517.64 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:51:58Z- TerrainNav::motionUpdate() - currEstimate = (4060239, 593271.06, 6.10) 2021-02-13T00:51:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:51:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:51:58Z- TNavBF::Projecting Measurements 2021-02-13T00:51:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:51:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:51:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:51:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:51:58Z- TNavFilter::4 good beams. 2021-02-13T00:51:58Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:51:58Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:51:58Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:51:58Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:51:58Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:51:58Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:51:58Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:51:58Z- TNavBF::mmse selected from filter -1 2021-02-13T00:51:58Z- TerrainMapOctree: (vehN, vehE ) = (4060238.73, 593270.87). 2021-02-13T00:51:58Z- TerrainMapOctree: Min Distance = 1407.61. 2021-02-13T00:51:58Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:51:58Z- There is no specific good beam that all particles have in common. 2021-02-13T00:51:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:51:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 15.23 2021-02-13T00:51:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:51:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 22.43 2021-02-13T00:51:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:51:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 10.94 2021-02-13T00:51:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:51:58Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.031 ExpectedMeasDiff: nan currMeas.covariance: 28.42 2021-02-13T00:51:58Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:51:58Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:51:58Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:51:58Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:51:58Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:51:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:51:58Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:51:58Z- Filter 0 No beams used in subcloud update 2021-02-13T00:51:58Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:51:58Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:51:58Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 684.7 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:51:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177517.64 sec. 2021-02-13T00:51:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:51:58Z- TNavBF::mmse selected from filter -1 2021-02-13T00:51:58Z- variances([0], [2], and psi_berg) = 684.696535 832.990523 0.000000 2021-02-13T00:51:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:51:58Z- LcmTrn::publishEstimates() - MLE : 4060238.41 593290.56 6.10 2021-02-13T00:51:58Z- LcmTrn::publishEstimates() - MMSE : 4060238.91 593271.06 6.10 2021-02-13T00:51:58Z- LcmTrn::publishEstimates() - COVAR: 684.70 832.99 0.95 0.00 2021-02-13T00:51:58Z- AHRS_M2 msg: 1613177518.43 epoch sec; seqNo:16311 2021-02-13T00:51:58Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.31 psi 2021-02-13T00:51:58Z- handleDepth()-Depth_Keller msg: 1613177518437 epoch sec; seqNo:16439; depth 6.24 2021-02-13T00:51:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177518.45 epoch sec; seqNo:16439; 4060239.30 north; 593272.28 east 2021-02-13T00:51:58Z- AHRS_M2 msg: 1613177518.83 epoch sec; seqNo:16312 2021-02-13T00:51:58Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.29 psi 2021-02-13T00:51:58Z- handleDepth()-Depth_Keller msg: 1613177518836 epoch sec; seqNo:16440; depth 6.40 2021-02-13T00:51:58Z- handleDvl() - RDI_Pathfinder msg: 1613177518.85 epoch sec; seqNo:7884 2021-02-13T00:51:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.52 , 124.21 , 84.05 , 136.11 2021-02-13T00:51:58Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , 0.08 , -0.39 2021-02-13T00:51:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177518.87 epoch sec; seqNo:16440; 4060239.68 north; 593272.27 east 2021-02-13T00:51:59Z- AHRS_M2 msg: 1613177519.24 epoch sec; seqNo:16313 2021-02-13T00:51:59Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.26 psi 2021-02-13T00:51:59Z- handleDepth()-Depth_Keller msg: 1613177519243 epoch sec; seqNo:16441; depth 6.55 2021-02-13T00:51:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177519.25 epoch sec; seqNo:16441; 4060240.06 north; 593272.27 east 2021-02-13T00:51:59Z- AHRS_M2 msg: 1613177519.64 epoch sec; seqNo:16314 2021-02-13T00:51:59Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.24 psi 2021-02-13T00:51:59Z- handleDepth()-Depth_Keller msg: 1613177519645 epoch sec; seqNo:16442; depth 6.71 2021-02-13T00:51:59Z- handleDvl() - RDI_Pathfinder msg: 1613177519.66 epoch sec; seqNo:7885 2021-02-13T00:51:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 118.19 , 82.50 , 127.12 2021-02-13T00:51:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.12 , -0.26 2021-02-13T00:51:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177519.67 epoch sec; seqNo:16442; 4060240.44 north; 593272.26 east 2021-02-13T00:52:00Z- AHRS_M2 msg: 1613177520.04 epoch sec; seqNo:16315 2021-02-13T00:52:00Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.22 psi 2021-02-13T00:52:00Z- handleDepth()-Depth_Keller msg: 1613177520053 epoch sec; seqNo:16443; depth 6.86 2021-02-13T00:52:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177520.06 epoch sec; seqNo:16443; 4060240.44 north; 593272.26 east 2021-02-13T00:52:00Z- AHRS_M2 msg: 1613177520.45 epoch sec; seqNo:16316 2021-02-13T00:52:00Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.22 psi 2021-02-13T00:52:00Z- handleDepth()-Depth_Keller msg: 1613177520452 epoch sec; seqNo:16444; depth 7.01 2021-02-13T00:52:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177520.46 epoch sec; seqNo:16444; 4060240.82 north; 593272.26 east 2021-02-13T00:52:00Z- AHRS_M2 msg: 1613177520.85 epoch sec; seqNo:16317 2021-02-13T00:52:00Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.22 psi 2021-02-13T00:52:00Z- handleDepth()-Depth_Keller msg: 1613177520856 epoch sec; seqNo:16445; depth 7.15 2021-02-13T00:52:00Z- handleDvl() - RDI_Pathfinder msg: 1613177520.87 epoch sec; seqNo:7886 2021-02-13T00:52:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.38 , 112.18 , 80.94 , 124.08 2021-02-13T00:52:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , -0.15 2021-02-13T00:52:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177520.89 epoch sec; seqNo:16445; 4060241.21 north; 593273.48 east 2021-02-13T00:52:01Z- AHRS_M2 msg: 1613177521.26 epoch sec; seqNo:16318 2021-02-13T00:52:01Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.21 psi 2021-02-13T00:52:01Z- handleDepth()-Depth_Keller msg: 1613177521263 epoch sec; seqNo:16446; depth 7.29 2021-02-13T00:52:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177521.29 epoch sec; seqNo:16446; 4060241.59 north; 593273.47 east 2021-02-13T00:52:01Z- AHRS_M2 msg: 1613177521.66 epoch sec; seqNo:16319 2021-02-13T00:52:01Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.21 psi 2021-02-13T00:52:01Z- handleDepth()-Depth_Keller msg: 1613177521666 epoch sec; seqNo:16447; depth 7.42 2021-02-13T00:52:01Z- handleDvl() - RDI_Pathfinder msg: 1613177521.68 epoch sec; seqNo:7887 2021-02-13T00:52:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 111.37 , 81.62 , 123.27 2021-02-13T00:52:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , -0.08 2021-02-13T00:52:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177521.69 epoch sec; seqNo:16447; 4060241.97 north; 593273.47 east 2021-02-13T00:52:02Z- AHRS_M2 msg: 1613177522.06 epoch sec; seqNo:16320 2021-02-13T00:52:02Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.22 psi 2021-02-13T00:52:02Z- handleDepth()-Depth_Keller msg: 1613177522069 epoch sec; seqNo:16448; depth 7.55 2021-02-13T00:52:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177522.08 epoch sec; seqNo:16448; 4060242.35 north; 593273.46 east 2021-02-13T00:52:02Z- AHRS_M2 msg: 1613177522.47 epoch sec; seqNo:16321 2021-02-13T00:52:02Z- AHRS_M2 msg: 0.57 phi; -0.31 theta; -0.22 psi 2021-02-13T00:52:02Z- handleDepth()-Depth_Keller msg: 1613177522474 epoch sec; seqNo:16449; depth 7.69 2021-02-13T00:52:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177522.48 epoch sec; seqNo:16449; 4060242.35 north; 593273.46 east 2021-02-13T00:52:02Z- AHRS_M2 msg: 1613177522.87 epoch sec; seqNo:16322 2021-02-13T00:52:02Z- AHRS_M2 msg: 0.57 phi; -0.30 theta; -0.23 psi 2021-02-13T00:52:02Z- handleDepth()-Depth_Keller msg: 1613177522876 epoch sec; seqNo:16450; depth 7.82 2021-02-13T00:52:02Z- handleDvl() - RDI_Pathfinder msg: 1613177522.89 epoch sec; seqNo:7888 2021-02-13T00:52:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.50 , 109.14 , 0.00 , 119.95 2021-02-13T00:52:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , -0.11 2021-02-13T00:52:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177522.90 epoch sec; seqNo:16450; 4060242.73 north; 593273.46 east 2021-02-13T00:52:03Z- AHRS_M2 msg: 1613177523.28 epoch sec; seqNo:16323 2021-02-13T00:52:03Z- AHRS_M2 msg: 0.57 phi; -0.29 theta; -0.23 psi 2021-02-13T00:52:03Z- LcmTrn::handleMessages() - handling 42 messages took 0.64 ms and 129 calls... 2021-02-13T00:52:03Z- waiting for 1613177521.05, time is 1613177523.28 2021-02-13T00:52:03Z- LcmTrn::updateTrn() >>>> pose time 1613177523.28, meas time 1613177522.89 <<<< 2021-02-13T00:52:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177522.89 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:52:03Z- TerrainNav::motionUpdate() - currEstimate = (4060243, 593273.46, 7.82) 2021-02-13T00:52:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:52:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:52:03Z- TNavBF::Projecting Measurements 2021-02-13T00:52:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:52:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:52:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:52:03Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:52:03Z- TNavFilter::3 good beams. 2021-02-13T00:52:03Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:52:03Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:52:03Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:52:03Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:52:03Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:52:03Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:52:03Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:52:03Z- TNavBF::mmse selected from filter -1 2021-02-13T00:52:03Z- TerrainMapOctree: (vehN, vehE ) = (4060242.44, 593273.28). 2021-02-13T00:52:03Z- TerrainMapOctree: Min Distance = 1403.26. 2021-02-13T00:52:03Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:52:03Z- There is no specific good beam that all particles have in common. 2021-02-13T00:52:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:52:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 12.25 2021-02-13T00:52:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:52:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 19.06 2021-02-13T00:52:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:52:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 23.02 2021-02-13T00:52:03Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:52:03Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:52:03Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:52:03Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:52:03Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:52:03Z- Filter 0 No beams used in subcloud update 2021-02-13T00:52:03Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:52:03Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:52:03Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 684.6 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:52:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177522.89 sec. 2021-02-13T00:52:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:52:03Z- TNavBF::mmse selected from filter -1 2021-02-13T00:52:03Z- variances([0], [2], and psi_berg) = 684.636159 832.990496 0.000000 2021-02-13T00:52:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:52:03Z- LcmTrn::publishEstimates() - MLE : 4060242.23 593291.96 7.82 2021-02-13T00:52:03Z- LcmTrn::publishEstimates() - MMSE : 4060242.73 593273.46 7.82 2021-02-13T00:52:03Z- LcmTrn::publishEstimates() - COVAR: 684.64 832.99 1.00 0.00 2021-02-13T00:52:03Z- handleDepth()-Depth_Keller msg: 1613177523280 epoch sec; seqNo:16451; depth 7.96 2021-02-13T00:52:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:03Z- handleDvl() - RDI_Pathfinder msg: 1613177523.28 epoch sec; seqNo:7889 2021-02-13T00:52:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , -0.11 2021-02-13T00:52:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177523.29 epoch sec; seqNo:16451; 4060243.11 north; 593273.45 east 2021-02-13T00:52:03Z- AHRS_M2 msg: 1613177523.69 epoch sec; seqNo:16324 2021-02-13T00:52:03Z- AHRS_M2 msg: 0.57 phi; -0.27 theta; -0.23 psi 2021-02-13T00:52:03Z- handleDepth()-Depth_Keller msg: 1613177523705 epoch sec; seqNo:16452; depth 8.08 2021-02-13T00:52:03Z- handleDvl() - RDI_Pathfinder msg: 1613177523.73 epoch sec; seqNo:7890 2021-02-13T00:52:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.26 , 111.03 , 81.28 , 116.98 2021-02-13T00:52:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.26 , -0.04 2021-02-13T00:52:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177523.75 epoch sec; seqNo:16452; 4060243.49 north; 593273.45 east 2021-02-13T00:52:04Z- AHRS_M2 msg: 1613177524.08 epoch sec; seqNo:16325 2021-02-13T00:52:04Z- AHRS_M2 msg: 0.58 phi; -0.26 theta; -0.21 psi 2021-02-13T00:52:04Z- handleDepth()-Depth_Keller msg: 1613177524090 epoch sec; seqNo:16453; depth 8.21 2021-02-13T00:52:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177524.10 epoch sec; seqNo:16453; 4060243.88 north; 593274.67 east 2021-02-13T00:52:04Z- AHRS_M2 msg: 1613177524.49 epoch sec; seqNo:16326 2021-02-13T00:52:04Z- AHRS_M2 msg: 0.59 phi; -0.25 theta; -0.19 psi 2021-02-13T00:52:04Z- handleDepth()-Depth_Keller msg: 1613177524492 epoch sec; seqNo:16454; depth 8.31 2021-02-13T00:52:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:04Z- handleDvl() - RDI_Pathfinder msg: 1613177524.49 epoch sec; seqNo:7891 2021-02-13T00:52:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.26 , -0.04 2021-02-13T00:52:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177524.52 epoch sec; seqNo:16454; 4060244.26 north; 593274.66 east 2021-02-13T00:52:04Z- AHRS_M2 msg: 1613177524.89 epoch sec; seqNo:16327 2021-02-13T00:52:04Z- AHRS_M2 msg: 0.59 phi; -0.25 theta; -0.15 psi 2021-02-13T00:52:04Z- handleDepth()-Depth_Keller msg: 1613177524901 epoch sec; seqNo:16455; depth 8.42 2021-02-13T00:52:04Z- handleDvl() - RDI_Pathfinder msg: 1613177524.91 epoch sec; seqNo:7892 2021-02-13T00:52:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 106.63 , 80.94 , 112.04 2021-02-13T00:52:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.20 , 0.15 2021-02-13T00:52:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177524.94 epoch sec; seqNo:16455; 4060244.64 north; 593274.66 east 2021-02-13T00:52:05Z- AHRS_M2 msg: 1613177525.30 epoch sec; seqNo:16328 2021-02-13T00:52:05Z- AHRS_M2 msg: 0.59 phi; -0.25 theta; -0.12 psi 2021-02-13T00:52:05Z- handleDepth()-Depth_Keller msg: 1613177525300 epoch sec; seqNo:16456; depth 8.53 2021-02-13T00:52:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177525.31 epoch sec; seqNo:16456; 4060244.64 north; 593274.66 east 2021-02-13T00:52:05Z- AHRS_M2 msg: 1613177525.72 epoch sec; seqNo:16329 2021-02-13T00:52:05Z- AHRS_M2 msg: 0.58 phi; -0.25 theta; -0.10 psi 2021-02-13T00:52:05Z- handleDepth()-Depth_Keller msg: 1613177525725 epoch sec; seqNo:16457; depth 8.61 2021-02-13T00:52:05Z- handleDvl() - RDI_Pathfinder msg: 1613177525.74 epoch sec; seqNo:7893 2021-02-13T00:52:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 97.37 , 79.79 , 108.19 2021-02-13T00:52:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.00 , 0.26 2021-02-13T00:52:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177525.75 epoch sec; seqNo:16457; 4060245.01 north; 593274.65 east 2021-02-13T00:52:06Z- handleDepth()-Depth_Keller msg: 1613177526125 epoch sec; seqNo:16458; depth 8.71 2021-02-13T00:52:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177526.14 epoch sec; seqNo:16458; 4060245.39 north; 593274.65 east 2021-02-13T00:52:06Z- AHRS_M2 msg: 1613177526.51 epoch sec; seqNo:16330 2021-02-13T00:52:06Z- AHRS_M2 msg: 0.58 phi; -0.25 theta; -0.11 psi 2021-02-13T00:52:06Z- handleDepth()-Depth_Keller msg: 1613177526519 epoch sec; seqNo:16459; depth 8.78 2021-02-13T00:52:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177526.54 epoch sec; seqNo:16459; 4060245.39 north; 593274.65 east 2021-02-13T00:52:06Z- AHRS_M2 msg: 1613177526.91 epoch sec; seqNo:16331 2021-02-13T00:52:06Z- AHRS_M2 msg: 0.58 phi; -0.25 theta; -0.12 psi 2021-02-13T00:52:06Z- handleDepth()-Depth_Keller msg: 1613177526921 epoch sec; seqNo:16460; depth 8.87 2021-02-13T00:52:06Z- handleDvl() - RDI_Pathfinder msg: 1613177526.93 epoch sec; seqNo:7894 2021-02-13T00:52:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.29 , 80.07 , 111.16 2021-02-13T00:52:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.15 , 0.19 2021-02-13T00:52:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177526.94 epoch sec; seqNo:16460; 4060245.77 north; 593274.65 east 2021-02-13T00:52:07Z- AHRS_M2 msg: 1613177527.32 epoch sec; seqNo:16332 2021-02-13T00:52:07Z- AHRS_M2 msg: 0.58 phi; -0.25 theta; -0.14 psi 2021-02-13T00:52:07Z- handleDepth()-Depth_Keller msg: 1613177527329 epoch sec; seqNo:16461; depth 8.96 2021-02-13T00:52:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177527.34 epoch sec; seqNo:16461; 4060245.78 north; 593275.87 east 2021-02-13T00:52:07Z- AHRS_M2 msg: 1613177527.72 epoch sec; seqNo:16333 2021-02-13T00:52:07Z- AHRS_M2 msg: 0.58 phi; -0.25 theta; -0.16 psi 2021-02-13T00:52:07Z- handleDepth()-Depth_Keller msg: 1613177527724 epoch sec; seqNo:16462; depth 9.04 2021-02-13T00:52:07Z- handleDvl() - RDI_Pathfinder msg: 1613177527.73 epoch sec; seqNo:7895 2021-02-13T00:52:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.91 , 97.37 , 0.00 , 108.19 2021-02-13T00:52:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.13 , 0.11 2021-02-13T00:52:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177527.76 epoch sec; seqNo:16462; 4060246.16 north; 593275.86 east 2021-02-13T00:52:08Z- AHRS_M2 msg: 1613177528.12 epoch sec; seqNo:16334 2021-02-13T00:52:08Z- AHRS_M2 msg: 0.58 phi; -0.25 theta; -0.18 psi 2021-02-13T00:52:08Z- handleDepth()-Depth_Keller msg: 1613177528133 epoch sec; seqNo:16463; depth 9.12 2021-02-13T00:52:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:08Z- handleDvl() - RDI_Pathfinder msg: 1613177528.13 epoch sec; seqNo:7896 2021-02-13T00:52:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.13 , 0.11 2021-02-13T00:52:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177528.15 epoch sec; seqNo:16463; 4060246.54 north; 593275.86 east 2021-02-13T00:52:08Z- LcmTrn::handleMessages() - handling 45 messages took 0.60 ms and 133 calls... 2021-02-13T00:52:08Z- waiting for 1613177526.28, time is 1613177528.15 2021-02-13T00:52:08Z- LcmTrn::updateTrn() equating pose 1613177528.15 and meas times 1613177528.15 2021-02-13T00:52:08Z- LcmTrn::updateTrn() >>>> pose time 1613177528.15, meas time 1613177528.15 <<<< 2021-02-13T00:52:08Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177528.15, ping # 0. 2021-02-13T00:52:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177528.15 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:52:08Z- TerrainNav::motionUpdate() - currEstimate = (4060247, 593275.86, 9.12) 2021-02-13T00:52:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:52:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:52:08Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:52:08Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:52:08Z- TNavBF::Projecting Measurements 2021-02-13T00:52:08Z- TNavFilter::Measurement from time = 1613177528.15 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:52:08Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:52:08Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:52:08Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:52:08Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:52:08Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:52:08Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:52:08Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:52:08Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 684.6 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:52:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:52:08Z- TNavBF::mmse selected from filter -1 2021-02-13T00:52:08Z- variances([0], [2], and psi_berg) = 684.587280 832.990474 0.000000 2021-02-13T00:52:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:52:08Z- LcmTrn::publishEstimates() - MLE : 4060246.04 593294.36 9.12 2021-02-13T00:52:08Z- LcmTrn::publishEstimates() - MMSE : 4060246.54 593275.86 9.12 2021-02-13T00:52:08Z- LcmTrn::publishEstimates() - COVAR: 684.59 832.99 1.23 0.00 2021-02-13T00:52:08Z- AHRS_M2 msg: 1613177528.53 epoch sec; seqNo:16335 2021-02-13T00:52:08Z- AHRS_M2 msg: 0.57 phi; -0.26 theta; -0.19 psi 2021-02-13T00:52:08Z- handleDepth()-Depth_Keller msg: 1613177528541 epoch sec; seqNo:16464; depth 9.22 2021-02-13T00:52:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177528.55 epoch sec; seqNo:16464; 4060246.54 north; 593275.86 east 2021-02-13T00:52:08Z- AHRS_M2 msg: 1613177528.93 epoch sec; seqNo:16336 2021-02-13T00:52:08Z- AHRS_M2 msg: 0.57 phi; -0.26 theta; -0.20 psi 2021-02-13T00:52:08Z- handleDepth()-Depth_Keller msg: 1613177528936 epoch sec; seqNo:16465; depth 9.29 2021-02-13T00:52:08Z- handleDvl() - RDI_Pathfinder msg: 1613177528.95 epoch sec; seqNo:7897 2021-02-13T00:52:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.07 , 101.29 , 81.01 , 105.35 2021-02-13T00:52:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.11 , 0.12 2021-02-13T00:52:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177528.96 epoch sec; seqNo:16465; 4060246.92 north; 593275.85 east 2021-02-13T00:52:09Z- AHRS_M2 msg: 1613177529.34 epoch sec; seqNo:16337 2021-02-13T00:52:09Z- AHRS_M2 msg: 0.56 phi; -0.27 theta; -0.20 psi 2021-02-13T00:52:09Z- handleDepth()-Depth_Keller msg: 1613177529340 epoch sec; seqNo:16466; depth 9.38 2021-02-13T00:52:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:09Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:09Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:09Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:09Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:09Z- handleDvl() - RDI_Pathfinder msg: 1613177529.34 epoch sec; seqNo:7898 2021-02-13T00:52:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.11 , 0.12 2021-02-13T00:52:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177529.35 epoch sec; seqNo:16466; 4060247.31 north; 593277.07 east 2021-02-13T00:52:09Z- AHRS_M2 msg: 1613177529.74 epoch sec; seqNo:16338 2021-02-13T00:52:09Z- AHRS_M2 msg: 0.56 phi; -0.28 theta; -0.20 psi 2021-02-13T00:52:09Z- handleDepth()-Depth_Keller msg: 1613177529749 epoch sec; seqNo:16467; depth 9.47 2021-02-13T00:52:09Z- handleDvl() - RDI_Pathfinder msg: 1613177529.76 epoch sec; seqNo:7899 2021-02-13T00:52:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 104.40 , 0.00 , 111.16 2021-02-13T00:52:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.20 , 0.09 2021-02-13T00:52:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177529.77 epoch sec; seqNo:16467; 4060247.69 north; 593277.07 east 2021-02-13T00:52:10Z- AHRS_M2 msg: 1613177530.15 epoch sec; seqNo:16339 2021-02-13T00:52:10Z- AHRS_M2 msg: 0.56 phi; -0.29 theta; -0.19 psi 2021-02-13T00:52:10Z- handleDepth()-Depth_Keller msg: 1613177530155 epoch sec; seqNo:16468; depth 9.56 2021-02-13T00:52:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177530.17 epoch sec; seqNo:16468; 4060247.69 north; 593277.07 east 2021-02-13T00:52:10Z- AHRS_M2 msg: 1613177530.55 epoch sec; seqNo:16340 2021-02-13T00:52:10Z- AHRS_M2 msg: 0.56 phi; -0.29 theta; -0.19 psi 2021-02-13T00:52:10Z- handleDepth()-Depth_Keller msg: 1613177530552 epoch sec; seqNo:16469; depth 9.66 2021-02-13T00:52:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:10Z- handleDvl() - RDI_Pathfinder msg: 1613177530.55 epoch sec; seqNo:7900 2021-02-13T00:52:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.20 , 0.09 2021-02-13T00:52:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177530.57 epoch sec; seqNo:16469; 4060248.07 north; 593277.06 east 2021-02-13T00:52:10Z- AHRS_M2 msg: 1613177530.95 epoch sec; seqNo:16341 2021-02-13T00:52:10Z- AHRS_M2 msg: 0.56 phi; -0.29 theta; -0.20 psi 2021-02-13T00:52:10Z- handleDepth()-Depth_Keller msg: 1613177530957 epoch sec; seqNo:16470; depth 9.76 2021-02-13T00:52:10Z- handleDvl() - RDI_Pathfinder msg: 1613177530.97 epoch sec; seqNo:7901 2021-02-13T00:52:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 103.73 , 80.74 , 110.49 2021-02-13T00:52:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.22 , -0.07 2021-02-13T00:52:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177530.99 epoch sec; seqNo:16470; 4060248.45 north; 593277.06 east 2021-02-13T00:52:11Z- AHRS_M2 msg: 1613177531.36 epoch sec; seqNo:16342 2021-02-13T00:52:11Z- AHRS_M2 msg: 0.57 phi; -0.29 theta; -0.20 psi 2021-02-13T00:52:11Z- handleDepth()-Depth_Keller msg: 1613177531360 epoch sec; seqNo:16471; depth 9.84 2021-02-13T00:52:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177531.37 epoch sec; seqNo:16471; 4060248.46 north; 593278.28 east 2021-02-13T00:52:11Z- AHRS_M2 msg: 1613177531.76 epoch sec; seqNo:16343 2021-02-13T00:52:11Z- AHRS_M2 msg: 0.57 phi; -0.29 theta; -0.21 psi 2021-02-13T00:52:11Z- handleDepth()-Depth_Keller msg: 1613177531766 epoch sec; seqNo:16472; depth 9.94 2021-02-13T00:52:11Z- handleDvl() - RDI_Pathfinder msg: 1613177531.78 epoch sec; seqNo:7902 2021-02-13T00:52:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 103.93 , 79.59 , 114.75 2021-02-13T00:52:11Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.16 , -0.18 2021-02-13T00:52:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177531.79 epoch sec; seqNo:16472; 4060248.84 north; 593278.27 east 2021-02-13T00:52:12Z- AHRS_M2 msg: 1613177532.16 epoch sec; seqNo:16344 2021-02-13T00:52:12Z- AHRS_M2 msg: 0.57 phi; -0.30 theta; -0.21 psi 2021-02-13T00:52:12Z- handleDepth()-Depth_Keller msg: 1613177532173 epoch sec; seqNo:16473; depth 10.04 2021-02-13T00:52:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177532.19 epoch sec; seqNo:16473; 4060249.22 north; 593278.27 east 2021-02-13T00:52:12Z- AHRS_M2 msg: 1613177532.57 epoch sec; seqNo:16345 2021-02-13T00:52:12Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.21 psi 2021-02-13T00:52:12Z- handleDepth()-Depth_Keller msg: 1613177532572 epoch sec; seqNo:16474; depth 10.15 2021-02-13T00:52:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177532.58 epoch sec; seqNo:16474; 4060249.60 north; 593278.27 east 2021-02-13T00:52:12Z- AHRS_M2 msg: 1613177532.97 epoch sec; seqNo:16346 2021-02-13T00:52:12Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.20 psi 2021-02-13T00:52:12Z- handleDepth()-Depth_Keller msg: 1613177532983 epoch sec; seqNo:16475; depth 10.27 2021-02-13T00:52:13Z- handleDvl() - RDI_Pathfinder msg: 1613177533.00 epoch sec; seqNo:7903 2021-02-13T00:52:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.85 , 105.49 , 79.79 , 113.60 2021-02-13T00:52:13Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.05 , -0.20 2021-02-13T00:52:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177533.02 epoch sec; seqNo:16475; 4060249.98 north; 593278.26 east 2021-02-13T00:52:13Z- LcmTrn::handleMessages() - handling 43 messages took 0.65 ms and 130 calls... 2021-02-13T00:52:13Z- waiting for 1613177531.15, time is 1613177533.02 2021-02-13T00:52:13Z- LcmTrn::updateTrn() equating pose 1613177533.02 and meas times 1613177533.02 2021-02-13T00:52:13Z- LcmTrn::updateTrn() >>>> pose time 1613177533.02, meas time 1613177533.02 <<<< 2021-02-13T00:52:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177533.02 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:52:13Z- TerrainNav::motionUpdate() - currEstimate = (4060250, 593278.26, 10.27) 2021-02-13T00:52:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:52:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:52:13Z- TNavBF::Projecting Measurements 2021-02-13T00:52:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:52:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:52:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:52:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:52:13Z- TNavFilter::4 good beams. 2021-02-13T00:52:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:52:13Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:52:13Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:52:13Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:52:13Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:52:13Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:52:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:52:13Z- TNavBF::mmse selected from filter -1 2021-02-13T00:52:13Z- TerrainMapOctree: (vehN, vehE ) = (4060249.98, 593278.26). 2021-02-13T00:52:13Z- TerrainMapOctree: Min Distance = 1394.39. 2021-02-13T00:52:13Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:52:13Z- There is no specific good beam that all particles have in common. 2021-02-13T00:52:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:52:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.44 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 11.25 2021-02-13T00:52:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:52:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 17.80 2021-02-13T00:52:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:52:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 10.19 2021-02-13T00:52:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:52:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 20.65 2021-02-13T00:52:13Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:52:13Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:52:13Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:52:13Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:52:13Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:52:13Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:52:13Z- Filter 0 No beams used in subcloud update 2021-02-13T00:52:13Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:52:13Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:52:13Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 684.5 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:52:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177533.02 sec. 2021-02-13T00:52:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:52:13Z- TNavBF::mmse selected from filter -1 2021-02-13T00:52:13Z- variances([0], [2], and psi_berg) = 684.540156 832.990453 0.000000 2021-02-13T00:52:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:52:13Z- LcmTrn::publishEstimates() - MLE : 4060249.48 593296.76 10.27 2021-02-13T00:52:13Z- LcmTrn::publishEstimates() - MMSE : 4060249.98 593278.26 10.27 2021-02-13T00:52:13Z- LcmTrn::publishEstimates() - COVAR: 684.54 832.99 1.05 0.00 2021-02-13T00:52:13Z- AHRS_M2 msg: 1613177533.38 epoch sec; seqNo:16347 2021-02-13T00:52:13Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.19 psi 2021-02-13T00:52:13Z- handleDepth()-Depth_Keller msg: 1613177533383 epoch sec; seqNo:16476; depth 10.40 2021-02-13T00:52:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177533.39 epoch sec; seqNo:16476; 4060250.37 north; 593279.48 east 2021-02-13T00:52:13Z- AHRS_M2 msg: 1613177533.78 epoch sec; seqNo:16348 2021-02-13T00:52:13Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.18 psi 2021-02-13T00:52:13Z- handleDepth()-Depth_Keller msg: 1613177533784 epoch sec; seqNo:16477; depth 10.49 2021-02-13T00:52:13Z- handleDvl() - RDI_Pathfinder msg: 1613177533.80 epoch sec; seqNo:7904 2021-02-13T00:52:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.78 , 78.44 , 114.95 2021-02-13T00:52:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.05 , -0.19 2021-02-13T00:52:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177533.82 epoch sec; seqNo:16477; 4060250.75 north; 593279.47 east 2021-02-13T00:52:14Z- AHRS_M2 msg: 1613177534.18 epoch sec; seqNo:16349 2021-02-13T00:52:14Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.16 psi 2021-02-13T00:52:14Z- handleDepth()-Depth_Keller msg: 1613177534193 epoch sec; seqNo:16478; depth 10.62 2021-02-13T00:52:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:14Z- handleDvl() - RDI_Pathfinder msg: 1613177534.19 epoch sec; seqNo:7905 2021-02-13T00:52:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.05 , -0.19 2021-02-13T00:52:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177534.20 epoch sec; seqNo:16478; 4060250.75 north; 593279.47 east 2021-02-13T00:52:14Z- AHRS_M2 msg: 1613177534.59 epoch sec; seqNo:16350 2021-02-13T00:52:14Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.16 psi 2021-02-13T00:52:14Z- handleDepth()-Depth_Keller msg: 1613177534597 epoch sec; seqNo:16479; depth 10.73 2021-02-13T00:52:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177534.61 epoch sec; seqNo:16479; 4060251.13 north; 593279.47 east 2021-02-13T00:52:14Z- AHRS_M2 msg: 1613177534.99 epoch sec; seqNo:16351 2021-02-13T00:52:14Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.16 psi 2021-02-13T00:52:15Z- handleDepth()-Depth_Keller msg: 1613177535001 epoch sec; seqNo:16480; depth 10.84 2021-02-13T00:52:15Z- handleDvl() - RDI_Pathfinder msg: 1613177535.02 epoch sec; seqNo:7906 2021-02-13T00:52:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 101.23 , 76.43 , 112.04 2021-02-13T00:52:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , -0.21 2021-02-13T00:52:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177535.03 epoch sec; seqNo:16480; 4060251.51 north; 593279.47 east 2021-02-13T00:52:15Z- AHRS_M2 msg: 1613177535.40 epoch sec; seqNo:16352 2021-02-13T00:52:15Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.16 psi 2021-02-13T00:52:15Z- handleDepth()-Depth_Keller msg: 1613177535400 epoch sec; seqNo:16481; depth 10.95 2021-02-13T00:52:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:15Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:15Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:15Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:15Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:15Z- handleDvl() - RDI_Pathfinder msg: 1613177535.40 epoch sec; seqNo:7907 2021-02-13T00:52:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , -0.21 2021-02-13T00:52:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177535.41 epoch sec; seqNo:16481; 4060251.90 north; 593280.68 east 2021-02-13T00:52:15Z- AHRS_M2 msg: 1613177535.80 epoch sec; seqNo:16353 2021-02-13T00:52:15Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.17 psi 2021-02-13T00:52:15Z- handleDepth()-Depth_Keller msg: 1613177535814 epoch sec; seqNo:16482; depth 11.07 2021-02-13T00:52:15Z- handleDvl() - RDI_Pathfinder msg: 1613177535.83 epoch sec; seqNo:7908 2021-02-13T00:52:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.05 , 100.28 , 78.64 , 111.10 2021-02-13T00:52:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.21 , -0.17 2021-02-13T00:52:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177535.86 epoch sec; seqNo:16482; 4060252.28 north; 593280.68 east 2021-02-13T00:52:16Z- AHRS_M2 msg: 1613177536.21 epoch sec; seqNo:16354 2021-02-13T00:52:16Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.17 psi 2021-02-13T00:52:16Z- handleDepth()-Depth_Keller msg: 1613177536210 epoch sec; seqNo:16483; depth 11.18 2021-02-13T00:52:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177536.22 epoch sec; seqNo:16483; 4060252.66 north; 593280.67 east 2021-02-13T00:52:16Z- AHRS_M2 msg: 1613177536.61 epoch sec; seqNo:16355 2021-02-13T00:52:16Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.17 psi 2021-02-13T00:52:16Z- handleDepth()-Depth_Keller msg: 1613177536612 epoch sec; seqNo:16484; depth 11.28 2021-02-13T00:52:16Z- handleDvl() - RDI_Pathfinder msg: 1613177536.63 epoch sec; seqNo:7909 2021-02-13T00:52:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.63 , 78.64 , 112.45 2021-02-13T00:52:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.31 , 0.01 2021-02-13T00:52:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177536.64 epoch sec; seqNo:16484; 4060253.04 north; 593280.67 east 2021-02-13T00:52:17Z- AHRS_M2 msg: 1613177537.01 epoch sec; seqNo:16356 2021-02-13T00:52:17Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.17 psi 2021-02-13T00:52:17Z- handleDepth()-Depth_Keller msg: 1613177537016 epoch sec; seqNo:16485; depth 11.40 2021-02-13T00:52:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177537.03 epoch sec; seqNo:16485; 4060253.04 north; 593280.67 east 2021-02-13T00:52:17Z- AHRS_M2 msg: 1613177537.42 epoch sec; seqNo:16357 2021-02-13T00:52:17Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.16 psi 2021-02-13T00:52:17Z- handleDepth()-Depth_Keller msg: 1613177537427 epoch sec; seqNo:16486; depth 11.52 2021-02-13T00:52:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177537.44 epoch sec; seqNo:16486; 4060253.41 north; 593280.67 east 2021-02-13T00:52:17Z- AHRS_M2 msg: 1613177537.83 epoch sec; seqNo:16358 2021-02-13T00:52:17Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.15 psi 2021-02-13T00:52:17Z- handleDepth()-Depth_Keller msg: 1613177537841 epoch sec; seqNo:16487; depth 11.64 2021-02-13T00:52:17Z- handleDvl() - RDI_Pathfinder msg: 1613177537.85 epoch sec; seqNo:7910 2021-02-13T00:52:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.07 , 101.29 , 78.31 , 113.46 2021-02-13T00:52:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.29 , 0.25 2021-02-13T00:52:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177537.86 epoch sec; seqNo:16487; 4060253.79 north; 593280.66 east 2021-02-13T00:52:18Z- handleDepth()-Depth_Keller msg: 1613177538244 epoch sec; seqNo:16488; depth 11.79 2021-02-13T00:52:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177538.26 epoch sec; seqNo:16488; 4060254.17 north; 593280.66 east 2021-02-13T00:52:18Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-13T00:52:18Z- waiting for 1613177536.02, time is 1613177538.26 2021-02-13T00:52:18Z- LcmTrn::updateTrn() >>>> pose time 1613177538.26, meas time 1613177537.85 <<<< 2021-02-13T00:52:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177537.85 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:52:18Z- TerrainNav::motionUpdate() - currEstimate = (4060254, 593280.66, 11.79) 2021-02-13T00:52:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:52:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:52:18Z- TNavBF::Projecting Measurements 2021-02-13T00:52:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:52:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:52:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:52:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:52:18Z- TNavFilter::4 good beams. 2021-02-13T00:52:18Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:52:18Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:52:18Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:52:18Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:52:18Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:52:18Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:52:18Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:52:18Z- TNavBF::mmse selected from filter -1 2021-02-13T00:52:18Z- TerrainMapOctree: (vehN, vehE ) = (4060253.85, 593280.47). 2021-02-13T00:52:18Z- TerrainMapOctree: Min Distance = 1390.07. 2021-02-13T00:52:18Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:52:18Z- There is no specific good beam that all particles have in common. 2021-02-13T00:52:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:52:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 11.58 2021-02-13T00:52:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:52:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 16.42 2021-02-13T00:52:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:52:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 9.81 2021-02-13T00:52:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:52:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 20.60 2021-02-13T00:52:18Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:52:18Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:52:18Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:52:18Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:52:18Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:52:18Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:52:18Z- Filter 0 No beams used in subcloud update 2021-02-13T00:52:18Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:52:18Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:52:18Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 684.5 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:52:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177537.85 sec. 2021-02-13T00:52:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:52:18Z- TNavBF::mmse selected from filter -1 2021-02-13T00:52:18Z- variances([0], [2], and psi_berg) = 684.477578 832.990425 0.000000 2021-02-13T00:52:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:52:18Z- LcmTrn::publishEstimates() - MLE : 4060253.67 593299.16 11.79 2021-02-13T00:52:18Z- LcmTrn::publishEstimates() - MMSE : 4060254.17 593280.66 11.79 2021-02-13T00:52:18Z- LcmTrn::publishEstimates() - COVAR: 684.48 832.99 0.98 0.00 2021-02-13T00:52:18Z- AHRS_M2 msg: 1613177538.65 epoch sec; seqNo:16359 2021-02-13T00:52:18Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.12 psi 2021-02-13T00:52:18Z- handleDepth()-Depth_Keller msg: 1613177538664 epoch sec; seqNo:16489; depth 11.91 2021-02-13T00:52:18Z- handleDvl() - RDI_Pathfinder msg: 1613177538.68 epoch sec; seqNo:7911 2021-02-13T00:52:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 98.86 , 78.58 , 115.08 2021-02-13T00:52:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.07 , 0.33 2021-02-13T00:52:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177538.69 epoch sec; seqNo:16489; 4060254.17 north; 593280.66 east 2021-02-13T00:52:19Z- AHRS_M2 msg: 1613177539.03 epoch sec; seqNo:16360 2021-02-13T00:52:19Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.12 psi 2021-02-13T00:52:19Z- handleDepth()-Depth_Keller msg: 1613177539036 epoch sec; seqNo:16490; depth 12.05 2021-02-13T00:52:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:19Z- handleDvl() - RDI_Pathfinder msg: 1613177539.04 epoch sec; seqNo:7912 2021-02-13T00:52:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.07 , 0.33 2021-02-13T00:52:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177539.05 epoch sec; seqNo:16490; 4060254.55 north; 593280.65 east 2021-02-13T00:52:19Z- AHRS_M2 msg: 1613177539.44 epoch sec; seqNo:16361 2021-02-13T00:52:19Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.12 psi 2021-02-13T00:52:19Z- handleDepth()-Depth_Keller msg: 1613177539446 epoch sec; seqNo:16491; depth 12.17 2021-02-13T00:52:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177539.46 epoch sec; seqNo:16491; 4060254.55 north; 593280.65 east 2021-02-13T00:52:19Z- AHRS_M2 msg: 1613177539.84 epoch sec; seqNo:16362 2021-02-13T00:52:19Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.13 psi 2021-02-13T00:52:19Z- handleDepth()-Depth_Keller msg: 1613177539849 epoch sec; seqNo:16492; depth 12.29 2021-02-13T00:52:19Z- handleDvl() - RDI_Pathfinder msg: 1613177539.87 epoch sec; seqNo:7913 2021-02-13T00:52:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.15 , 73.56 , 117.79 2021-02-13T00:52:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.13 , 0.24 2021-02-13T00:52:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177539.88 epoch sec; seqNo:16492; 4060254.94 north; 593281.87 east 2021-02-13T00:52:20Z- AHRS_M2 msg: 1613177540.24 epoch sec; seqNo:16363 2021-02-13T00:52:20Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.14 psi 2021-02-13T00:52:20Z- handleDepth()-Depth_Keller msg: 1613177540253 epoch sec; seqNo:16493; depth 12.44 2021-02-13T00:52:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:20Z- handleDvl() - RDI_Pathfinder msg: 1613177540.26 epoch sec; seqNo:7914 2021-02-13T00:52:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.13 , 0.24 2021-02-13T00:52:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177540.27 epoch sec; seqNo:16493; 4060254.94 north; 593281.87 east 2021-02-13T00:52:20Z- AHRS_M2 msg: 1613177540.65 epoch sec; seqNo:16364 2021-02-13T00:52:20Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.15 psi 2021-02-13T00:52:20Z- handleDepth()-Depth_Keller msg: 1613177540657 epoch sec; seqNo:16494; depth 12.56 2021-02-13T00:52:20Z- handleDvl() - RDI_Pathfinder msg: 1613177540.67 epoch sec; seqNo:7915 2021-02-13T00:52:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.91 , 98.72 , 75.20 , 123.06 2021-02-13T00:52:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.20 , 0.04 2021-02-13T00:52:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177540.68 epoch sec; seqNo:16494; 4060255.32 north; 593281.87 east 2021-02-13T00:52:21Z- AHRS_M2 msg: 1613177541.05 epoch sec; seqNo:16365 2021-02-13T00:52:21Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.16 psi 2021-02-13T00:52:21Z- handleDepth()-Depth_Keller msg: 1613177541056 epoch sec; seqNo:16495; depth 12.71 2021-02-13T00:52:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177541.07 epoch sec; seqNo:16495; 4060255.70 north; 593281.86 east 2021-02-13T00:52:21Z- AHRS_M2 msg: 1613177541.46 epoch sec; seqNo:16366 2021-02-13T00:52:21Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.17 psi 2021-02-13T00:52:21Z- handleDepth()-Depth_Keller msg: 1613177541466 epoch sec; seqNo:16496; depth 12.84 2021-02-13T00:52:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:21Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:21Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:21Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:21Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:21Z- handleDvl() - RDI_Pathfinder msg: 1613177541.47 epoch sec; seqNo:7916 2021-02-13T00:52:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.20 , 0.04 2021-02-13T00:52:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177541.48 epoch sec; seqNo:16496; 4060255.70 north; 593281.86 east 2021-02-13T00:52:21Z- AHRS_M2 msg: 1613177541.86 epoch sec; seqNo:16367 2021-02-13T00:52:21Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.19 psi 2021-02-13T00:52:21Z- handleDepth()-Depth_Keller msg: 1613177541864 epoch sec; seqNo:16497; depth 12.99 2021-02-13T00:52:21Z- handleDvl() - RDI_Pathfinder msg: 1613177541.88 epoch sec; seqNo:7917 2021-02-13T00:52:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.43 , 78.44 , 120.36 2021-02-13T00:52:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.09 , -0.05 2021-02-13T00:52:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177541.89 epoch sec; seqNo:16497; 4060256.09 north; 593283.08 east 2021-02-13T00:52:22Z- AHRS_M2 msg: 1613177542.26 epoch sec; seqNo:16368 2021-02-13T00:52:22Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.20 psi 2021-02-13T00:52:22Z- handleDepth()-Depth_Keller msg: 1613177542273 epoch sec; seqNo:16498; depth 13.12 2021-02-13T00:52:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177542.28 epoch sec; seqNo:16498; 4060256.47 north; 593283.07 east 2021-02-13T00:52:22Z- AHRS_M2 msg: 1613177542.67 epoch sec; seqNo:16369 2021-02-13T00:52:22Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.20 psi 2021-02-13T00:52:22Z- handleDepth()-Depth_Keller msg: 1613177542675 epoch sec; seqNo:16499; depth 13.26 2021-02-13T00:52:22Z- handleDvl() - RDI_Pathfinder msg: 1613177542.69 epoch sec; seqNo:7918 2021-02-13T00:52:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:22Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:52:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177542.70 epoch sec; seqNo:16499; 4060256.85 north; 593283.07 east 2021-02-13T00:52:23Z- AHRS_M2 msg: 1613177543.07 epoch sec; seqNo:16370 2021-02-13T00:52:23Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.20 psi 2021-02-13T00:52:23Z- handleDepth()-Depth_Keller msg: 1613177543082 epoch sec; seqNo:16500; depth 13.42 2021-02-13T00:52:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177543.10 epoch sec; seqNo:16500; 4060256.85 north; 593283.07 east 2021-02-13T00:52:23Z- AHRS_M2 msg: 1613177543.48 epoch sec; seqNo:16371 2021-02-13T00:52:23Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.19 psi 2021-02-13T00:52:23Z- handleDepth()-Depth_Keller msg: 1613177543486 epoch sec; seqNo:16501; depth 13.56 2021-02-13T00:52:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177543.49 epoch sec; seqNo:16501; 4060257.23 north; 593283.07 east 2021-02-13T00:52:23Z- LcmTrn::handleMessages() - handling 47 messages took 0.70 ms and 133 calls... 2021-02-13T00:52:23Z- waiting for 1613177541.26, time is 1613177543.49 2021-02-13T00:52:23Z- LcmTrn::updateTrn() >>>> pose time 1613177543.49, meas time 1613177542.69 <<<< 2021-02-13T00:52:23Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177542.69, ping # 0. 2021-02-13T00:52:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177542.69 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:52:23Z- TerrainNav::motionUpdate() - currEstimate = (4060257, 593283.07, 13.56) 2021-02-13T00:52:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:52:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:52:23Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:52:23Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:52:23Z- TNavBF::Projecting Measurements 2021-02-13T00:52:23Z- TNavFilter::Measurement from time = 1613177542.69 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:52:23Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:52:23Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:52:23Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:52:23Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:52:23Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:52:23Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:52:23Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:52:23Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 684.4 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:52:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:52:23Z- TNavBF::mmse selected from filter -1 2021-02-13T00:52:23Z- variances([0], [2], and psi_berg) = 684.418107 832.990399 0.000000 2021-02-13T00:52:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:52:23Z- LcmTrn::publishEstimates() - MLE : 4060256.73 593301.57 13.56 2021-02-13T00:52:23Z- LcmTrn::publishEstimates() - MMSE : 4060257.23 593283.07 13.56 2021-02-13T00:52:23Z- LcmTrn::publishEstimates() - COVAR: 684.42 832.99 1.18 0.00 2021-02-13T00:52:23Z- AHRS_M2 msg: 1613177543.89 epoch sec; seqNo:16372 2021-02-13T00:52:23Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.19 psi 2021-02-13T00:52:23Z- handleDepth()-Depth_Keller msg: 1613177543890 epoch sec; seqNo:16502; depth 13.70 2021-02-13T00:52:23Z- handleDvl() - RDI_Pathfinder msg: 1613177543.90 epoch sec; seqNo:7919 2021-02-13T00:52:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:23Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:52:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177543.91 epoch sec; seqNo:16502; 4060257.62 north; 593284.28 east 2021-02-13T00:52:24Z- AHRS_M2 msg: 1613177544.29 epoch sec; seqNo:16373 2021-02-13T00:52:24Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.19 psi 2021-02-13T00:52:24Z- handleDepth()-Depth_Keller msg: 1613177544290 epoch sec; seqNo:16503; depth 13.86 2021-02-13T00:52:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177544.30 epoch sec; seqNo:16503; 4060258.00 north; 593284.28 east 2021-02-13T00:52:24Z- AHRS_M2 msg: 1613177544.69 epoch sec; seqNo:16374 2021-02-13T00:52:24Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.19 psi 2021-02-13T00:52:24Z- handleDepth()-Depth_Keller msg: 1613177544692 epoch sec; seqNo:16504; depth 13.99 2021-02-13T00:52:24Z- handleDvl() - RDI_Pathfinder msg: 1613177544.70 epoch sec; seqNo:7920 2021-02-13T00:52:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 103.19 , 74.38 , 119.55 2021-02-13T00:52:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.16 , -0.00 2021-02-13T00:52:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177544.72 epoch sec; seqNo:16504; 4060258.00 north; 593284.28 east 2021-02-13T00:52:25Z- AHRS_M2 msg: 1613177545.09 epoch sec; seqNo:16375 2021-02-13T00:52:25Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.19 psi 2021-02-13T00:52:25Z- handleDepth()-Depth_Keller msg: 1613177545096 epoch sec; seqNo:16505; depth 14.16 2021-02-13T00:52:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:25Z- handleDvl() - RDI_Pathfinder msg: 1613177545.10 epoch sec; seqNo:7921 2021-02-13T00:52:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:25Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.16 , -0.00 2021-02-13T00:52:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177545.11 epoch sec; seqNo:16505; 4060258.38 north; 593284.27 east 2021-02-13T00:52:25Z- AHRS_M2 msg: 1613177545.50 epoch sec; seqNo:16376 2021-02-13T00:52:25Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.20 psi 2021-02-13T00:52:25Z- handleDepth()-Depth_Keller msg: 1613177545509 epoch sec; seqNo:16506; depth 14.30 2021-02-13T00:52:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177545.52 epoch sec; seqNo:16506; 4060258.77 north; 593285.49 east 2021-02-13T00:52:25Z- AHRS_M2 msg: 1613177545.90 epoch sec; seqNo:16377 2021-02-13T00:52:25Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.19 psi 2021-02-13T00:52:25Z- handleDepth()-Depth_Keller msg: 1613177545911 epoch sec; seqNo:16507; depth 14.43 2021-02-13T00:52:25Z- handleDvl() - RDI_Pathfinder msg: 1613177545.93 epoch sec; seqNo:7922 2021-02-13T00:52:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.16 , 78.17 , 113.33 2021-02-13T00:52:25Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.19 , -0.15 2021-02-13T00:52:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177545.94 epoch sec; seqNo:16507; 4060259.15 north; 593285.49 east 2021-02-13T00:52:26Z- AHRS_M2 msg: 1613177546.30 epoch sec; seqNo:16378 2021-02-13T00:52:26Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.19 psi 2021-02-13T00:52:26Z- handleDepth()-Depth_Keller msg: 1613177546309 epoch sec; seqNo:16508; depth 14.58 2021-02-13T00:52:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:26Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:26Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:26Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:26Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:26Z- handleDvl() - RDI_Pathfinder msg: 1613177546.31 epoch sec; seqNo:7923 2021-02-13T00:52:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:26Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.19 , -0.15 2021-02-13T00:52:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177546.33 epoch sec; seqNo:16508; 4060259.15 north; 593285.49 east 2021-02-13T00:52:26Z- AHRS_M2 msg: 1613177546.71 epoch sec; seqNo:16379 2021-02-13T00:52:26Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.19 psi 2021-02-13T00:52:26Z- handleDepth()-Depth_Keller msg: 1613177546713 epoch sec; seqNo:16509; depth 14.74 2021-02-13T00:52:26Z- handleDvl() - RDI_Pathfinder msg: 1613177546.72 epoch sec; seqNo:7924 2021-02-13T00:52:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.01 , 101.29 , 72.75 , 114.82 2021-02-13T00:52:26Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.10 , -0.28 2021-02-13T00:52:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177546.75 epoch sec; seqNo:16509; 4060259.53 north; 593285.48 east 2021-02-13T00:52:27Z- AHRS_M2 msg: 1613177547.11 epoch sec; seqNo:16380 2021-02-13T00:52:27Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.18 psi 2021-02-13T00:52:27Z- handleDepth()-Depth_Keller msg: 1613177547119 epoch sec; seqNo:16510; depth 14.89 2021-02-13T00:52:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177547.13 epoch sec; seqNo:16510; 4060259.91 north; 593285.48 east 2021-02-13T00:52:27Z- AHRS_M2 msg: 1613177547.52 epoch sec; seqNo:16381 2021-02-13T00:52:27Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:52:27Z- handleDepth()-Depth_Keller msg: 1613177547533 epoch sec; seqNo:16511; depth 15.02 2021-02-13T00:52:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:27Z- handleDvl() - RDI_Pathfinder msg: 1613177547.54 epoch sec; seqNo:7925 2021-02-13T00:52:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:27Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.10 , -0.28 2021-02-13T00:52:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177547.54 epoch sec; seqNo:16511; 4060260.30 north; 593286.69 east 2021-02-13T00:52:27Z- AHRS_M2 msg: 1613177547.92 epoch sec; seqNo:16382 2021-02-13T00:52:27Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:52:27Z- handleDepth()-Depth_Keller msg: 1613177547925 epoch sec; seqNo:16512; depth 15.16 2021-02-13T00:52:27Z- handleDvl() - RDI_Pathfinder msg: 1613177547.94 epoch sec; seqNo:7926 2021-02-13T00:52:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.13 , 99.06 , 78.78 , 112.58 2021-02-13T00:52:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.08 , -0.29 2021-02-13T00:52:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177547.98 epoch sec; seqNo:16512; 4060260.68 north; 593286.69 east 2021-02-13T00:52:28Z- AHRS_M2 msg: 1613177548.32 epoch sec; seqNo:16383 2021-02-13T00:52:28Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:52:28Z- handleDepth()-Depth_Keller msg: 1613177548328 epoch sec; seqNo:16513; depth 15.29 2021-02-13T00:52:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177548.34 epoch sec; seqNo:16513; 4060261.06 north; 593286.69 east 2021-02-13T00:52:28Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-13T00:52:28Z- waiting for 1613177546.49, time is 1613177548.34 2021-02-13T00:52:28Z- LcmTrn::updateTrn() >>>> pose time 1613177548.34, meas time 1613177547.94 <<<< 2021-02-13T00:52:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177547.94 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:52:28Z- TerrainNav::motionUpdate() - currEstimate = (4060261, 593286.69, 15.29) 2021-02-13T00:52:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:52:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:52:28Z- TNavBF::Projecting Measurements 2021-02-13T00:52:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:52:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:52:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:52:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:52:28Z- TNavFilter::4 good beams. 2021-02-13T00:52:28Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:52:28Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:52:28Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:52:28Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:52:28Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:52:28Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:52:28Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:52:28Z- TNavBF::mmse selected from filter -1 2021-02-13T00:52:28Z- TerrainMapOctree: (vehN, vehE ) = (4060260.75, 593286.39). 2021-02-13T00:52:28Z- TerrainMapOctree: Min Distance = 1381.00. 2021-02-13T00:52:28Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:52:28Z- There is no specific good beam that all particles have in common. 2021-02-13T00:52:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:52:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.42 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 10.27 2021-02-13T00:52:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:52:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 15.70 2021-02-13T00:52:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:52:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 9.93 2021-02-13T00:52:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:52:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 20.28 2021-02-13T00:52:28Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:52:28Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:52:28Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:52:28Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:52:28Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:52:28Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:52:28Z- Filter 0 No beams used in subcloud update 2021-02-13T00:52:28Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:52:28Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:52:28Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 684.4 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:52:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177547.94 sec. 2021-02-13T00:52:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:52:28Z- TNavBF::mmse selected from filter -1 2021-02-13T00:52:28Z- variances([0], [2], and psi_berg) = 684.352511 832.990369 0.000000 2021-02-13T00:52:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:52:28Z- LcmTrn::publishEstimates() - MLE : 4060260.56 593308.19 15.29 2021-02-13T00:52:28Z- LcmTrn::publishEstimates() - MMSE : 4060261.06 593286.69 15.29 2021-02-13T00:52:28Z- LcmTrn::publishEstimates() - COVAR: 684.35 832.99 1.05 0.00 2021-02-13T00:52:28Z- AHRS_M2 msg: 1613177548.73 epoch sec; seqNo:16384 2021-02-13T00:52:28Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-13T00:52:28Z- handleDepth()-Depth_Keller msg: 1613177548732 epoch sec; seqNo:16514; depth 15.41 2021-02-13T00:52:28Z- handleDvl() - RDI_Pathfinder msg: 1613177548.74 epoch sec; seqNo:7927 2021-02-13T00:52:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.07 , 99.00 , 78.71 , 111.16 2021-02-13T00:52:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.24 , -0.16 2021-02-13T00:52:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177548.75 epoch sec; seqNo:16514; 4060261.44 north; 593286.68 east 2021-02-13T00:52:29Z- AHRS_M2 msg: 1613177549.14 epoch sec; seqNo:16385 2021-02-13T00:52:29Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.20 psi 2021-02-13T00:52:29Z- handleDepth()-Depth_Keller msg: 1613177549142 epoch sec; seqNo:16515; depth 15.55 2021-02-13T00:52:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177549.16 epoch sec; seqNo:16515; 4060261.82 north; 593286.68 east 2021-02-13T00:52:29Z- AHRS_M2 msg: 1613177549.54 epoch sec; seqNo:16386 2021-02-13T00:52:29Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:52:29Z- handleDepth()-Depth_Keller msg: 1613177549550 epoch sec; seqNo:16516; depth 15.68 2021-02-13T00:52:29Z- handleDvl() - RDI_Pathfinder msg: 1613177549.59 epoch sec; seqNo:7928 2021-02-13T00:52:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.13 , 99.06 , 72.34 , 108.53 2021-02-13T00:52:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.24 , 0.06 2021-02-13T00:52:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177549.60 epoch sec; seqNo:16516; 4060262.19 north; 593286.67 east 2021-02-13T00:52:29Z- AHRS_M2 msg: 1613177549.94 epoch sec; seqNo:16387 2021-02-13T00:52:29Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-13T00:52:29Z- handleDepth()-Depth_Keller msg: 1613177549943 epoch sec; seqNo:16517; depth 15.82 2021-02-13T00:52:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:29Z- handleDvl() - RDI_Pathfinder msg: 1613177549.95 epoch sec; seqNo:7929 2021-02-13T00:52:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.24 , 0.06 2021-02-13T00:52:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177549.96 epoch sec; seqNo:16517; 4060262.19 north; 593286.67 east 2021-02-13T00:52:30Z- AHRS_M2 msg: 1613177550.35 epoch sec; seqNo:16388 2021-02-13T00:52:30Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-13T00:52:30Z- handleDepth()-Depth_Keller msg: 1613177550350 epoch sec; seqNo:16518; depth 15.94 2021-02-13T00:52:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177550.36 epoch sec; seqNo:16518; 4060262.57 north; 593286.67 east 2021-02-13T00:52:30Z- AHRS_M2 msg: 1613177550.75 epoch sec; seqNo:16389 2021-02-13T00:52:30Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-13T00:52:30Z- handleDepth()-Depth_Keller msg: 1613177550752 epoch sec; seqNo:16519; depth 16.07 2021-02-13T00:52:30Z- handleDvl() - RDI_Pathfinder msg: 1613177550.77 epoch sec; seqNo:7930 2021-02-13T00:52:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.06 , 99.00 , 71.11 , 111.16 2021-02-13T00:52:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.14 , 0.20 2021-02-13T00:52:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177550.78 epoch sec; seqNo:16519; 4060262.97 north; 593287.89 east 2021-02-13T00:52:31Z- AHRS_M2 msg: 1613177551.15 epoch sec; seqNo:16390 2021-02-13T00:52:31Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-13T00:52:31Z- handleDepth()-Depth_Keller msg: 1613177551159 epoch sec; seqNo:16520; depth 16.20 2021-02-13T00:52:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:31Z- handleDvl() - RDI_Pathfinder msg: 1613177551.16 epoch sec; seqNo:7931 2021-02-13T00:52:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.14 , 0.20 2021-02-13T00:52:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177551.18 epoch sec; seqNo:16520; 4060263.34 north; 593287.88 east 2021-02-13T00:52:31Z- AHRS_M2 msg: 1613177551.56 epoch sec; seqNo:16391 2021-02-13T00:52:31Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-13T00:52:31Z- handleDepth()-Depth_Keller msg: 1613177551564 epoch sec; seqNo:16521; depth 16.32 2021-02-13T00:52:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177551.58 epoch sec; seqNo:16521; 4060263.34 north; 593287.88 east 2021-02-13T00:52:31Z- AHRS_M2 msg: 1613177551.96 epoch sec; seqNo:16392 2021-02-13T00:52:31Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-13T00:52:31Z- handleDepth()-Depth_Keller msg: 1613177551964 epoch sec; seqNo:16522; depth 16.46 2021-02-13T00:52:31Z- handleDvl() - RDI_Pathfinder msg: 1613177551.98 epoch sec; seqNo:7932 2021-02-13T00:52:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.06 , 97.78 , 78.31 , 109.95 2021-02-13T00:52:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , 0.20 2021-02-13T00:52:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177551.99 epoch sec; seqNo:16522; 4060263.72 north; 593287.88 east 2021-02-13T00:52:32Z- AHRS_M2 msg: 1613177552.36 epoch sec; seqNo:16393 2021-02-13T00:52:32Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.18 psi 2021-02-13T00:52:32Z- handleDepth()-Depth_Keller msg: 1613177552370 epoch sec; seqNo:16523; depth 16.60 2021-02-13T00:52:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:32Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:32Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:32Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:32Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:32Z- handleDvl() - RDI_Pathfinder msg: 1613177552.37 epoch sec; seqNo:7933 2021-02-13T00:52:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , 0.20 2021-02-13T00:52:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177552.38 epoch sec; seqNo:16523; 4060263.72 north; 593287.88 east 2021-02-13T00:52:32Z- AHRS_M2 msg: 1613177552.77 epoch sec; seqNo:16394 2021-02-13T00:52:32Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.18 psi 2021-02-13T00:52:32Z- handleDepth()-Depth_Keller msg: 1613177552777 epoch sec; seqNo:16524; depth 16.72 2021-02-13T00:52:32Z- handleDvl() - RDI_Pathfinder msg: 1613177552.80 epoch sec; seqNo:7934 2021-02-13T00:52:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 97.64 , 70.29 , 107.38 2021-02-13T00:52:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , 0.11 2021-02-13T00:52:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177552.81 epoch sec; seqNo:16524; 4060264.10 north; 593287.87 east 2021-02-13T00:52:33Z- AHRS_M2 msg: 1613177553.17 epoch sec; seqNo:16395 2021-02-13T00:52:33Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-13T00:52:33Z- handleDepth()-Depth_Keller msg: 1613177553185 epoch sec; seqNo:16525; depth 16.87 2021-02-13T00:52:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177553.20 epoch sec; seqNo:16525; 4060264.48 north; 593287.87 east 2021-02-13T00:52:33Z- AHRS_M2 msg: 1613177553.58 epoch sec; seqNo:16396 2021-02-13T00:52:33Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.19 psi 2021-02-13T00:52:33Z- handleDepth()-Depth_Keller msg: 1613177553580 epoch sec; seqNo:16526; depth 17.00 2021-02-13T00:52:33Z- handleDvl() - RDI_Pathfinder msg: 1613177553.60 epoch sec; seqNo:7935 2021-02-13T00:52:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.06 , 97.85 , 71.93 , 110.01 2021-02-13T00:52:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.06 , 0.03 2021-02-13T00:52:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177553.61 epoch sec; seqNo:16526; 4060264.49 north; 593289.09 east 2021-02-13T00:52:33Z- LcmTrn::handleMessages() - handling 48 messages took 0.73 ms and 134 calls... 2021-02-13T00:52:33Z- waiting for 1613177551.34, time is 1613177553.61 2021-02-13T00:52:33Z- LcmTrn::updateTrn() equating pose 1613177553.61 and meas times 1613177553.61 2021-02-13T00:52:33Z- LcmTrn::updateTrn() >>>> pose time 1613177553.61, meas time 1613177553.61 <<<< 2021-02-13T00:52:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177553.61 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:52:33Z- TerrainNav::motionUpdate() - currEstimate = (4060264, 593289.09, 17.00) 2021-02-13T00:52:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:52:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:52:33Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177553.61 sec.; No relevant navigation data available 2021-02-13T00:52:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:52:33Z- TNavBF::mmse selected from filter -1 2021-02-13T00:52:33Z- variances([0], [2], and psi_berg) = 684.305502 832.990349 0.000000 2021-02-13T00:52:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:52:33Z- LcmTrn::publishEstimates() - MLE : 4060263.99 593310.59 17.00 2021-02-13T00:52:33Z- LcmTrn::publishEstimates() - MMSE : 4060264.49 593289.09 17.00 2021-02-13T00:52:33Z- LcmTrn::publishEstimates() - COVAR: 684.31 832.99 1.26 0.00 2021-02-13T00:52:33Z- AHRS_M2 msg: 1613177553.98 epoch sec; seqNo:16397 2021-02-13T00:52:33Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.20 psi 2021-02-13T00:52:33Z- handleDepth()-Depth_Keller msg: 1613177553984 epoch sec; seqNo:16527; depth 17.13 2021-02-13T00:52:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177553.99 epoch sec; seqNo:16527; 4060264.87 north; 593289.09 east 2021-02-13T00:52:34Z- AHRS_M2 msg: 1613177554.38 epoch sec; seqNo:16398 2021-02-13T00:52:34Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.20 psi 2021-02-13T00:52:34Z- handleDepth()-Depth_Keller msg: 1613177554388 epoch sec; seqNo:16528; depth 17.25 2021-02-13T00:52:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177554.40 epoch sec; seqNo:16528; 4060265.25 north; 593289.08 east 2021-02-13T00:52:34Z- AHRS_M2 msg: 1613177554.79 epoch sec; seqNo:16399 2021-02-13T00:52:34Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.20 psi 2021-02-13T00:52:34Z- handleDepth()-Depth_Keller msg: 1613177554801 epoch sec; seqNo:16529; depth 17.38 2021-02-13T00:52:34Z- handleDvl() - RDI_Pathfinder msg: 1613177554.81 epoch sec; seqNo:7936 2021-02-13T00:52:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 97.78 , 78.31 , 108.73 2021-02-13T00:52:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , 0.02 2021-02-13T00:52:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177554.82 epoch sec; seqNo:16529; 4060265.63 north; 593289.08 east 2021-02-13T00:52:35Z- AHRS_M2 msg: 1613177555.19 epoch sec; seqNo:16400 2021-02-13T00:52:35Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.20 psi 2021-02-13T00:52:35Z- handleDepth()-Depth_Keller msg: 1613177555201 epoch sec; seqNo:16530; depth 17.51 2021-02-13T00:52:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177555.22 epoch sec; seqNo:16530; 4060265.63 north; 593289.08 east 2021-02-13T00:52:35Z- AHRS_M2 msg: 1613177555.61 epoch sec; seqNo:16401 2021-02-13T00:52:35Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.20 psi 2021-02-13T00:52:35Z- handleDepth()-Depth_Keller msg: 1613177555617 epoch sec; seqNo:16531; depth 17.64 2021-02-13T00:52:35Z- handleDvl() - RDI_Pathfinder msg: 1613177555.63 epoch sec; seqNo:7937 2021-02-13T00:52:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.24 , 97.71 , 69.89 , 108.66 2021-02-13T00:52:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.05 , 0.06 2021-02-13T00:52:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177555.64 epoch sec; seqNo:16531; 4060266.01 north; 593289.07 east 2021-02-13T00:52:36Z- AHRS_M2 msg: 1613177556.00 epoch sec; seqNo:16402 2021-02-13T00:52:36Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.19 psi 2021-02-13T00:52:36Z- handleDepth()-Depth_Keller msg: 1613177556005 epoch sec; seqNo:16532; depth 17.78 2021-02-13T00:52:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:36Z- handleDvl() - RDI_Pathfinder msg: 1613177556.01 epoch sec; seqNo:7938 2021-02-13T00:52:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.05 , 0.06 2021-02-13T00:52:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177556.02 epoch sec; seqNo:16532; 4060266.40 north; 593290.29 east 2021-02-13T00:52:36Z- AHRS_M2 msg: 1613177556.40 epoch sec; seqNo:16403 2021-02-13T00:52:36Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.19 psi 2021-02-13T00:52:36Z- handleDepth()-Depth_Keller msg: 1613177556413 epoch sec; seqNo:16533; depth 17.89 2021-02-13T00:52:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177556.43 epoch sec; seqNo:16533; 4060266.78 north; 593290.29 east 2021-02-13T00:52:36Z- AHRS_M2 msg: 1613177556.81 epoch sec; seqNo:16404 2021-02-13T00:52:36Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.19 psi 2021-02-13T00:52:36Z- handleDepth()-Depth_Keller msg: 1613177556812 epoch sec; seqNo:16534; depth 18.01 2021-02-13T00:52:36Z- handleDvl() - RDI_Pathfinder msg: 1613177556.82 epoch sec; seqNo:7939 2021-02-13T00:52:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.83 , 97.85 , 69.07 , 106.36 2021-02-13T00:52:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.10 , 0.07 2021-02-13T00:52:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177556.85 epoch sec; seqNo:16534; 4060266.78 north; 593290.29 east 2021-02-13T00:52:37Z- AHRS_M2 msg: 1613177557.21 epoch sec; seqNo:16405 2021-02-13T00:52:37Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.19 psi 2021-02-13T00:52:37Z- handleDepth()-Depth_Keller msg: 1613177557217 epoch sec; seqNo:16535; depth 18.14 2021-02-13T00:52:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:37Z- handleDvl() - RDI_Pathfinder msg: 1613177557.22 epoch sec; seqNo:7940 2021-02-13T00:52:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.10 , 0.07 2021-02-13T00:52:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177557.23 epoch sec; seqNo:16535; 4060267.16 north; 593290.28 east 2021-02-13T00:52:37Z- AHRS_M2 msg: 1613177557.62 epoch sec; seqNo:16406 2021-02-13T00:52:37Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.19 psi 2021-02-13T00:52:37Z- handleDepth()-Depth_Keller msg: 1613177557629 epoch sec; seqNo:16536; depth 18.28 2021-02-13T00:52:37Z- handleDvl() - RDI_Pathfinder msg: 1613177557.65 epoch sec; seqNo:7941 2021-02-13T00:52:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 70.29 , 108.93 2021-02-13T00:52:37Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.13 , 0.03 2021-02-13T00:52:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177557.66 epoch sec; seqNo:16536; 4060267.54 north; 593290.28 east 2021-02-13T00:52:38Z- AHRS_M2 msg: 1613177558.02 epoch sec; seqNo:16407 2021-02-13T00:52:38Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.19 psi 2021-02-13T00:52:38Z- handleDepth()-Depth_Keller msg: 1613177558024 epoch sec; seqNo:16537; depth 18.40 2021-02-13T00:52:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177558.04 epoch sec; seqNo:16537; 4060267.93 north; 593291.49 east 2021-02-13T00:52:38Z- AHRS_M2 msg: 1613177558.42 epoch sec; seqNo:16408 2021-02-13T00:52:38Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.19 psi 2021-02-13T00:52:38Z- handleDepth()-Depth_Keller msg: 1613177558429 epoch sec; seqNo:16538; depth 18.52 2021-02-13T00:52:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:38Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:38Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:38Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:38Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:38Z- handleDvl() - RDI_Pathfinder msg: 1613177558.43 epoch sec; seqNo:7942 2021-02-13T00:52:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:38Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.13 , 0.03 2021-02-13T00:52:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177558.44 epoch sec; seqNo:16538; 4060268.31 north; 593291.49 east 2021-02-13T00:52:38Z- LcmTrn::handleMessages() - handling 43 messages took 0.61 ms and 131 calls... 2021-02-13T00:52:38Z- waiting for 1613177556.61, time is 1613177558.44 2021-02-13T00:52:38Z- LcmTrn::updateTrn() equating pose 1613177558.44 and meas times 1613177558.44 2021-02-13T00:52:38Z- LcmTrn::updateTrn() >>>> pose time 1613177558.44, meas time 1613177558.44 <<<< 2021-02-13T00:52:38Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177558.44, ping # 0. 2021-02-13T00:52:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177558.44 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:52:38Z- TerrainNav::motionUpdate() - currEstimate = (4060268, 593291.49, 18.52) 2021-02-13T00:52:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:52:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:52:38Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:52:38Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:52:38Z- TNavBF::Projecting Measurements 2021-02-13T00:52:38Z- TNavFilter::Measurement from time = 1613177558.44 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:52:38Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:52:38Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:52:38Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:52:38Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:52:38Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:52:38Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:52:38Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:52:38Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 684.3 EastVariance: 833.0 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-13T00:52:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:52:38Z- TNavBF::mmse selected from filter -1 2021-02-13T00:52:38Z- variances([0], [2], and psi_berg) = 684.256791 832.990327 0.000000 2021-02-13T00:52:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:52:38Z- LcmTrn::publishEstimates() - MLE : 4060267.81 593312.99 18.52 2021-02-13T00:52:38Z- LcmTrn::publishEstimates() - MMSE : 4060268.31 593291.49 18.52 2021-02-13T00:52:38Z- LcmTrn::publishEstimates() - COVAR: 684.26 832.99 1.49 0.00 2021-02-13T00:52:38Z- AHRS_M2 msg: 1613177558.83 epoch sec; seqNo:16409 2021-02-13T00:52:38Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.18 psi 2021-02-13T00:52:38Z- handleDepth()-Depth_Keller msg: 1613177558833 epoch sec; seqNo:16539; depth 18.65 2021-02-13T00:52:38Z- handleDvl() - RDI_Pathfinder msg: 1613177558.85 epoch sec; seqNo:7943 2021-02-13T00:52:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 97.85 , 78.37 , 106.36 2021-02-13T00:52:38Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.11 , -0.05 2021-02-13T00:52:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177558.86 epoch sec; seqNo:16539; 4060268.31 north; 593291.49 east 2021-02-13T00:52:39Z- AHRS_M2 msg: 1613177559.23 epoch sec; seqNo:16410 2021-02-13T00:52:39Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.18 psi 2021-02-13T00:52:39Z- handleDepth()-Depth_Keller msg: 1613177559237 epoch sec; seqNo:16540; depth 18.78 2021-02-13T00:52:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177559.25 epoch sec; seqNo:16540; 4060268.69 north; 593291.49 east 2021-02-13T00:52:39Z- AHRS_M2 msg: 1613177559.64 epoch sec; seqNo:16411 2021-02-13T00:52:39Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:52:39Z- handleDepth()-Depth_Keller msg: 1613177559640 epoch sec; seqNo:16541; depth 18.91 2021-02-13T00:52:39Z- handleDvl() - RDI_Pathfinder msg: 1613177559.65 epoch sec; seqNo:7944 2021-02-13T00:52:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 97.78 , 68.66 , 105.08 2021-02-13T00:52:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.04 , -0.11 2021-02-13T00:52:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177559.67 epoch sec; seqNo:16541; 4060269.07 north; 593291.48 east 2021-02-13T00:52:40Z- AHRS_M2 msg: 1613177560.04 epoch sec; seqNo:16412 2021-02-13T00:52:40Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.17 psi 2021-02-13T00:52:40Z- handleDepth()-Depth_Keller msg: 1613177560055 epoch sec; seqNo:16542; depth 19.05 2021-02-13T00:52:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177560.07 epoch sec; seqNo:16542; 4060269.46 north; 593292.70 east 2021-02-13T00:52:40Z- AHRS_M2 msg: 1613177560.45 epoch sec; seqNo:16413 2021-02-13T00:52:40Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.17 psi 2021-02-13T00:52:40Z- handleDepth()-Depth_Keller msg: 1613177560454 epoch sec; seqNo:16543; depth 19.18 2021-02-13T00:52:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177560.47 epoch sec; seqNo:16543; 4060269.84 north; 593292.69 east 2021-02-13T00:52:40Z- AHRS_M2 msg: 1613177560.85 epoch sec; seqNo:16414 2021-02-13T00:52:40Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.17 psi 2021-02-13T00:52:40Z- handleDepth()-Depth_Keller msg: 1613177560857 epoch sec; seqNo:16544; depth 19.30 2021-02-13T00:52:40Z- handleDvl() - RDI_Pathfinder msg: 1613177560.87 epoch sec; seqNo:7945 2021-02-13T00:52:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 67.43 , 106.50 2021-02-13T00:52:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , -0.13 2021-02-13T00:52:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177560.89 epoch sec; seqNo:16544; 4060270.22 north; 593292.69 east 2021-02-13T00:52:41Z- AHRS_M2 msg: 1613177561.25 epoch sec; seqNo:16415 2021-02-13T00:52:41Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.17 psi 2021-02-13T00:52:41Z- handleDepth()-Depth_Keller msg: 1613177561258 epoch sec; seqNo:16545; depth 19.43 2021-02-13T00:52:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177561.27 epoch sec; seqNo:16545; 4060270.22 north; 593292.69 east 2021-02-13T00:52:41Z- AHRS_M2 msg: 1613177561.65 epoch sec; seqNo:16416 2021-02-13T00:52:41Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.17 psi 2021-02-13T00:52:41Z- handleDepth()-Depth_Keller msg: 1613177561660 epoch sec; seqNo:16546; depth 19.55 2021-02-13T00:52:41Z- handleDvl() - RDI_Pathfinder msg: 1613177561.68 epoch sec; seqNo:7946 2021-02-13T00:52:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 93.11 , 69.48 , 106.50 2021-02-13T00:52:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.07 , -0.09 2021-02-13T00:52:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177561.69 epoch sec; seqNo:16546; 4060270.60 north; 593292.69 east 2021-02-13T00:52:42Z- AHRS_M2 msg: 1613177562.06 epoch sec; seqNo:16417 2021-02-13T00:52:42Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.18 psi 2021-02-13T00:52:42Z- handleDepth()-Depth_Keller msg: 1613177562065 epoch sec; seqNo:16547; depth 19.68 2021-02-13T00:52:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:42Z- handleDvl() - RDI_Pathfinder msg: 1613177562.07 epoch sec; seqNo:7947 2021-02-13T00:52:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.07 , -0.09 2021-02-13T00:52:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177562.08 epoch sec; seqNo:16547; 4060270.97 north; 593292.68 east 2021-02-13T00:52:42Z- AHRS_M2 msg: 1613177562.46 epoch sec; seqNo:16418 2021-02-13T00:52:42Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.18 psi 2021-02-13T00:52:42Z- handleDepth()-Depth_Keller msg: 1613177562469 epoch sec; seqNo:16548; depth 19.81 2021-02-13T00:52:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177562.49 epoch sec; seqNo:16548; 4060271.37 north; 593293.90 east 2021-02-13T00:52:42Z- AHRS_M2 msg: 1613177562.87 epoch sec; seqNo:16419 2021-02-13T00:52:42Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.17 psi 2021-02-13T00:52:42Z- handleDepth()-Depth_Keller msg: 1613177562873 epoch sec; seqNo:16549; depth 19.96 2021-02-13T00:52:42Z- handleDvl() - RDI_Pathfinder msg: 1613177562.89 epoch sec; seqNo:7948 2021-02-13T00:52:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.79 , 93.18 , 66.62 , 109.00 2021-02-13T00:52:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.09 , -0.03 2021-02-13T00:52:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177562.91 epoch sec; seqNo:16549; 4060271.75 north; 593293.89 east 2021-02-13T00:52:43Z- AHRS_M2 msg: 1613177563.27 epoch sec; seqNo:16420 2021-02-13T00:52:43Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.17 psi 2021-02-13T00:52:43Z- handleDepth()-Depth_Keller msg: 1613177563279 epoch sec; seqNo:16550; depth 20.09 2021-02-13T00:52:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:43Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:43Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:43Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:43Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:43Z- handleDvl() - RDI_Pathfinder msg: 1613177563.28 epoch sec; seqNo:7949 2021-02-13T00:52:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.09 , -0.03 2021-02-13T00:52:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177563.29 epoch sec; seqNo:16550; 4060272.12 north; 593293.89 east 2021-02-13T00:52:43Z- AHRS_M2 msg: 1613177563.67 epoch sec; seqNo:16421 2021-02-13T00:52:43Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.17 psi 2021-02-13T00:52:43Z- handleDepth()-Depth_Keller msg: 1613177563685 epoch sec; seqNo:16551; depth 20.21 2021-02-13T00:52:43Z- handleDvl() - RDI_Pathfinder msg: 1613177563.70 epoch sec; seqNo:7950 2021-02-13T00:52:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.20 , 98.12 , 67.03 , 106.63 2021-02-13T00:52:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.08 , 0.00 2021-02-13T00:52:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177563.71 epoch sec; seqNo:16551; 4060272.12 north; 593293.89 east 2021-02-13T00:52:43Z- LcmTrn::handleMessages() - handling 47 messages took 0.70 ms and 133 calls... 2021-02-13T00:52:43Z- waiting for 1613177561.44, time is 1613177563.71 2021-02-13T00:52:43Z- LcmTrn::updateTrn() equating pose 1613177563.71 and meas times 1613177563.71 2021-02-13T00:52:43Z- LcmTrn::updateTrn() >>>> pose time 1613177563.71, meas time 1613177563.71 <<<< 2021-02-13T00:52:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177563.71 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:52:43Z- TerrainNav::motionUpdate() - currEstimate = (4060272, 593293.89, 20.21) 2021-02-13T00:52:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:52:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:52:43Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177563.71 sec.; No relevant navigation data available 2021-02-13T00:52:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:52:43Z- TNavBF::mmse selected from filter -1 2021-02-13T00:52:43Z- variances([0], [2], and psi_berg) = 684.208104 832.990305 0.000000 2021-02-13T00:52:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:52:43Z- LcmTrn::publishEstimates() - MLE : 4060271.62 593314.39 20.21 2021-02-13T00:52:43Z- LcmTrn::publishEstimates() - MMSE : 4060272.12 593293.89 20.21 2021-02-13T00:52:43Z- LcmTrn::publishEstimates() - COVAR: 684.21 832.99 1.72 0.00 2021-02-13T00:52:44Z- AHRS_M2 msg: 1613177564.08 epoch sec; seqNo:16422 2021-02-13T00:52:44Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.17 psi 2021-02-13T00:52:44Z- handleDepth()-Depth_Keller msg: 1613177564089 epoch sec; seqNo:16552; depth 20.36 2021-02-13T00:52:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177564.10 epoch sec; seqNo:16552; 4060272.50 north; 593293.89 east 2021-02-13T00:52:44Z- AHRS_M2 msg: 1613177564.49 epoch sec; seqNo:16423 2021-02-13T00:52:44Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.16 psi 2021-02-13T00:52:44Z- handleDepth()-Depth_Keller msg: 1613177564492 epoch sec; seqNo:16553; depth 20.48 2021-02-13T00:52:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:44Z- handleDvl() - RDI_Pathfinder msg: 1613177564.50 epoch sec; seqNo:7951 2021-02-13T00:52:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.08 , 0.00 2021-02-13T00:52:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177564.52 epoch sec; seqNo:16553; 4060272.88 north; 593293.88 east 2021-02-13T00:52:44Z- AHRS_M2 msg: 1613177564.89 epoch sec; seqNo:16424 2021-02-13T00:52:44Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.16 psi 2021-02-13T00:52:44Z- handleDepth()-Depth_Keller msg: 1613177564894 epoch sec; seqNo:16554; depth 20.63 2021-02-13T00:52:44Z- handleDvl() - RDI_Pathfinder msg: 1613177564.91 epoch sec; seqNo:7952 2021-02-13T00:52:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.20 , 98.05 , 67.43 , 106.57 2021-02-13T00:52:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.10 , -0.00 2021-02-13T00:52:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177564.92 epoch sec; seqNo:16554; 4060273.26 north; 593293.88 east 2021-02-13T00:52:45Z- AHRS_M2 msg: 1613177565.29 epoch sec; seqNo:16425 2021-02-13T00:52:45Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.16 psi 2021-02-13T00:52:45Z- handleDepth()-Depth_Keller msg: 1613177565296 epoch sec; seqNo:16555; depth 20.76 2021-02-13T00:52:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177565.31 epoch sec; seqNo:16555; 4060273.65 north; 593295.09 east 2021-02-13T00:52:45Z- AHRS_M2 msg: 1613177565.70 epoch sec; seqNo:16426 2021-02-13T00:52:45Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.16 psi 2021-02-13T00:52:45Z- handleDepth()-Depth_Keller msg: 1613177565700 epoch sec; seqNo:16556; depth 20.90 2021-02-13T00:52:45Z- handleDvl() - RDI_Pathfinder msg: 1613177565.71 epoch sec; seqNo:7953 2021-02-13T00:52:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.79 , 90.75 , 66.62 , 104.13 2021-02-13T00:52:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.10 , 0.01 2021-02-13T00:52:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177565.72 epoch sec; seqNo:16556; 4060273.65 north; 593295.09 east 2021-02-13T00:52:46Z- AHRS_M2 msg: 1613177566.10 epoch sec; seqNo:16427 2021-02-13T00:52:46Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.16 psi 2021-02-13T00:52:46Z- handleDepth()-Depth_Keller msg: 1613177566109 epoch sec; seqNo:16557; depth 21.05 2021-02-13T00:52:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177566.12 epoch sec; seqNo:16557; 4060274.03 north; 593295.09 east 2021-02-13T00:52:46Z- AHRS_M2 msg: 1613177566.50 epoch sec; seqNo:16428 2021-02-13T00:52:46Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.17 psi 2021-02-13T00:52:46Z- handleDepth()-Depth_Keller msg: 1613177566533 epoch sec; seqNo:16558; depth 21.17 2021-02-13T00:52:46Z- handleDvl() - RDI_Pathfinder msg: 1613177566.56 epoch sec; seqNo:7954 2021-02-13T00:52:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.78 , 90.95 , 66.21 , 103.12 2021-02-13T00:52:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.07 , 0.05 2021-02-13T00:52:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177566.57 epoch sec; seqNo:16558; 4060274.41 north; 593295.09 east 2021-02-13T00:52:46Z- AHRS_M2 msg: 1613177566.91 epoch sec; seqNo:16429 2021-02-13T00:52:46Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.17 psi 2021-02-13T00:52:46Z- handleDepth()-Depth_Keller msg: 1613177566917 epoch sec; seqNo:16559; depth 21.32 2021-02-13T00:52:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:46Z- handleDvl() - RDI_Pathfinder msg: 1613177566.92 epoch sec; seqNo:7955 2021-02-13T00:52:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.07 , 0.05 2021-02-13T00:52:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177566.94 epoch sec; seqNo:16559; 4060274.79 north; 593295.08 east 2021-02-13T00:52:47Z- AHRS_M2 msg: 1613177567.31 epoch sec; seqNo:16430 2021-02-13T00:52:47Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.17 psi 2021-02-13T00:52:47Z- handleDepth()-Depth_Keller msg: 1613177567316 epoch sec; seqNo:16560; depth 21.46 2021-02-13T00:52:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177567.33 epoch sec; seqNo:16560; 4060274.79 north; 593295.08 east 2021-02-13T00:52:47Z- AHRS_M2 msg: 1613177567.72 epoch sec; seqNo:16431 2021-02-13T00:52:47Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.18 psi 2021-02-13T00:52:47Z- handleDepth()-Depth_Keller msg: 1613177567720 epoch sec; seqNo:16561; depth 21.57 2021-02-13T00:52:47Z- handleDvl() - RDI_Pathfinder msg: 1613177567.73 epoch sec; seqNo:7956 2021-02-13T00:52:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.79 , 98.18 , 66.62 , 103.05 2021-02-13T00:52:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , 0.11 2021-02-13T00:52:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177567.74 epoch sec; seqNo:16561; 4060275.17 north; 593295.08 east 2021-02-13T00:52:48Z- AHRS_M2 msg: 1613177568.12 epoch sec; seqNo:16432 2021-02-13T00:52:48Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.18 psi 2021-02-13T00:52:48Z- handleDepth()-Depth_Keller msg: 1613177568136 epoch sec; seqNo:16562; depth 21.72 2021-02-13T00:52:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:48Z- handleDvl() - RDI_Pathfinder msg: 1613177568.14 epoch sec; seqNo:7957 2021-02-13T00:52:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , 0.11 2021-02-13T00:52:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177568.15 epoch sec; seqNo:16562; 4060275.55 north; 593295.07 east 2021-02-13T00:52:48Z- AHRS_M2 msg: 1613177568.53 epoch sec; seqNo:16433 2021-02-13T00:52:48Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.18 psi 2021-02-13T00:52:48Z- handleDepth()-Depth_Keller msg: 1613177568533 epoch sec; seqNo:16563; depth 21.84 2021-02-13T00:52:48Z- handleDvl() - RDI_Pathfinder msg: 1613177568.56 epoch sec; seqNo:7958 2021-02-13T00:52:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.38 , 98.12 , 68.25 , 104.20 2021-02-13T00:52:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , 0.15 2021-02-13T00:52:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177568.57 epoch sec; seqNo:16563; 4060275.56 north; 593296.29 east 2021-02-13T00:52:48Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 131 calls... 2021-02-13T00:52:48Z- waiting for 1613177566.71, time is 1613177568.57 2021-02-13T00:52:48Z- LcmTrn::updateTrn() equating pose 1613177568.57 and meas times 1613177568.57 2021-02-13T00:52:48Z- LcmTrn::updateTrn() >>>> pose time 1613177568.57, meas time 1613177568.57 <<<< 2021-02-13T00:52:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177568.57 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:52:48Z- TerrainNav::motionUpdate() - currEstimate = (4060276, 593296.29, 21.84) 2021-02-13T00:52:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:52:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:52:48Z- TNavBF::Projecting Measurements 2021-02-13T00:52:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:52:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:52:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:52:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:52:48Z- TNavFilter::4 good beams. 2021-02-13T00:52:48Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:52:48Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:52:48Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:52:48Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:52:48Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:52:48Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:52:48Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:52:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:52:48Z- TerrainMapOctree: (vehN, vehE ) = (4060275.56, 593296.29). 2021-02-13T00:52:48Z- TerrainMapOctree: Min Distance = 1363.48. 2021-02-13T00:52:48Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:52:48Z- There is no specific good beam that all particles have in common. 2021-02-13T00:52:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:52:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.93 K: 0.155 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-13T00:52:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:52:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.64 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 15.40 2021-02-13T00:52:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:52:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.49 K: 0.140 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-13T00:52:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:52:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 17.37 2021-02-13T00:52:48Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:52:48Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:52:48Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:52:48Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:52:48Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:52:48Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:52:48Z- Filter 0 No beams used in subcloud update 2021-02-13T00:52:48Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:52:48Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:52:48Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 684.2 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:52:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177568.57 sec. 2021-02-13T00:52:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:52:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:52:48Z- variances([0], [2], and psi_berg) = 684.161165 832.990284 0.000000 2021-02-13T00:52:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:52:48Z- LcmTrn::publishEstimates() - MLE : 4060275.06 593318.79 21.84 2021-02-13T00:52:48Z- LcmTrn::publishEstimates() - MMSE : 4060275.56 593296.29 21.84 2021-02-13T00:52:48Z- LcmTrn::publishEstimates() - COVAR: 684.16 832.99 1.20 0.00 2021-02-13T00:52:48Z- AHRS_M2 msg: 1613177568.93 epoch sec; seqNo:16434 2021-02-13T00:52:48Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.18 psi 2021-02-13T00:52:48Z- handleDepth()-Depth_Keller msg: 1613177568933 epoch sec; seqNo:16564; depth 21.98 2021-02-13T00:52:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177568.95 epoch sec; seqNo:16564; 4060275.94 north; 593296.29 east 2021-02-13T00:52:49Z- AHRS_M2 msg: 1613177569.33 epoch sec; seqNo:16435 2021-02-13T00:52:49Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.18 psi 2021-02-13T00:52:49Z- handleDepth()-Depth_Keller msg: 1613177569341 epoch sec; seqNo:16565; depth 22.13 2021-02-13T00:52:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:49Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:49Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:49Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:49Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:49Z- handleDvl() - RDI_Pathfinder msg: 1613177569.34 epoch sec; seqNo:7959 2021-02-13T00:52:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , 0.15 2021-02-13T00:52:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177569.36 epoch sec; seqNo:16565; 4060276.32 north; 593296.29 east 2021-02-13T00:52:49Z- AHRS_M2 msg: 1613177569.73 epoch sec; seqNo:16436 2021-02-13T00:52:49Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.19 psi 2021-02-13T00:52:49Z- handleDepth()-Depth_Keller msg: 1613177569745 epoch sec; seqNo:16566; depth 22.25 2021-02-13T00:52:49Z- handleDvl() - RDI_Pathfinder msg: 1613177569.76 epoch sec; seqNo:7960 2021-02-13T00:52:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.97 , 98.12 , 0.00 , 104.20 2021-02-13T00:52:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.15 , 0.09 2021-02-13T00:52:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177569.78 epoch sec; seqNo:16566; 4060276.32 north; 593296.29 east 2021-02-13T00:52:50Z- AHRS_M2 msg: 1613177570.14 epoch sec; seqNo:16437 2021-02-13T00:52:50Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.19 psi 2021-02-13T00:52:50Z- handleDepth()-Depth_Keller msg: 1613177570149 epoch sec; seqNo:16567; depth 22.40 2021-02-13T00:52:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177570.16 epoch sec; seqNo:16567; 4060276.70 north; 593296.28 east 2021-02-13T00:52:50Z- AHRS_M2 msg: 1613177570.54 epoch sec; seqNo:16438 2021-02-13T00:52:50Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.19 psi 2021-02-13T00:52:50Z- handleDepth()-Depth_Keller msg: 1613177570548 epoch sec; seqNo:16568; depth 22.54 2021-02-13T00:52:50Z- handleDvl() - RDI_Pathfinder msg: 1613177570.57 epoch sec; seqNo:7961 2021-02-13T00:52:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.38 , 97.71 , 64.16 , 105.82 2021-02-13T00:52:50Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.16 , -0.03 2021-02-13T00:52:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177570.58 epoch sec; seqNo:16568; 4060277.09 north; 593297.50 east 2021-02-13T00:52:50Z- AHRS_M2 msg: 1613177570.95 epoch sec; seqNo:16439 2021-02-13T00:52:50Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.19 psi 2021-02-13T00:52:50Z- handleDepth()-Depth_Keller msg: 1613177570974 epoch sec; seqNo:16569; depth 22.68 2021-02-13T00:52:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177570.99 epoch sec; seqNo:16569; 4060277.47 north; 593297.49 east 2021-02-13T00:52:51Z- AHRS_M2 msg: 1613177571.35 epoch sec; seqNo:16440 2021-02-13T00:52:51Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.20 psi 2021-02-13T00:52:51Z- handleDepth()-Depth_Keller msg: 1613177571356 epoch sec; seqNo:16570; depth 22.83 2021-02-13T00:52:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177571.36 epoch sec; seqNo:16570; 4060277.47 north; 593297.49 east 2021-02-13T00:52:51Z- AHRS_M2 msg: 1613177571.76 epoch sec; seqNo:16441 2021-02-13T00:52:51Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.20 psi 2021-02-13T00:52:51Z- handleDepth()-Depth_Keller msg: 1613177571761 epoch sec; seqNo:16571; depth 22.95 2021-02-13T00:52:51Z- handleDvl() - RDI_Pathfinder msg: 1613177571.77 epoch sec; seqNo:7962 2021-02-13T00:52:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.64 , 98.12 , 66.21 , 102.98 2021-02-13T00:52:51Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.08 , -0.13 2021-02-13T00:52:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177571.78 epoch sec; seqNo:16571; 4060277.85 north; 593297.49 east 2021-02-13T00:52:52Z- AHRS_M2 msg: 1613177572.16 epoch sec; seqNo:16442 2021-02-13T00:52:52Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.20 psi 2021-02-13T00:52:52Z- handleDepth()-Depth_Keller msg: 1613177572180 epoch sec; seqNo:16572; depth 23.11 2021-02-13T00:52:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177572.19 epoch sec; seqNo:16572; 4060278.23 north; 593297.49 east 2021-02-13T00:52:52Z- AHRS_M2 msg: 1613177572.56 epoch sec; seqNo:16443 2021-02-13T00:52:52Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.21 psi 2021-02-13T00:52:52Z- handleDepth()-Depth_Keller msg: 1613177572568 epoch sec; seqNo:16573; depth 23.25 2021-02-13T00:52:52Z- handleDvl() - RDI_Pathfinder msg: 1613177572.58 epoch sec; seqNo:7963 2021-02-13T00:52:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 98.05 , 65.80 , 101.70 2021-02-13T00:52:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.04 , -0.15 2021-02-13T00:52:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177572.61 epoch sec; seqNo:16573; 4060278.62 north; 593298.70 east 2021-02-13T00:52:52Z- AHRS_M2 msg: 1613177572.99 epoch sec; seqNo:16444 2021-02-13T00:52:52Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.21 psi 2021-02-13T00:52:52Z- handleDepth()-Depth_Keller msg: 1613177572998 epoch sec; seqNo:16574; depth 23.39 2021-02-13T00:52:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:53Z- handleDvl() - RDI_Pathfinder msg: 1613177573.00 epoch sec; seqNo:7964 2021-02-13T00:52:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.04 , -0.15 2021-02-13T00:52:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177573.03 epoch sec; seqNo:16574; 4060279.00 north; 593298.70 east 2021-02-13T00:52:53Z- AHRS_M2 msg: 1613177573.37 epoch sec; seqNo:16445 2021-02-13T00:52:53Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.20 psi 2021-02-13T00:52:53Z- handleDepth()-Depth_Keller msg: 1613177573379 epoch sec; seqNo:16575; depth 23.54 2021-02-13T00:52:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177573.39 epoch sec; seqNo:16575; 4060279.38 north; 593298.69 east 2021-02-13T00:52:53Z- AHRS_M2 msg: 1613177573.78 epoch sec; seqNo:16446 2021-02-13T00:52:53Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.20 psi 2021-02-13T00:52:53Z- handleDepth()-Depth_Keller msg: 1613177573780 epoch sec; seqNo:16576; depth 23.67 2021-02-13T00:52:53Z- handleDvl() - RDI_Pathfinder msg: 1613177573.79 epoch sec; seqNo:7965 2021-02-13T00:52:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:53Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:52:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177573.82 epoch sec; seqNo:16576; 4060279.75 north; 593298.69 east 2021-02-13T00:52:53Z- LcmTrn::handleMessages() - handling 46 messages took 0.75 ms and 131 calls... 2021-02-13T00:52:53Z- waiting for 1613177571.57, time is 1613177573.82 2021-02-13T00:52:53Z- LcmTrn::updateTrn() equating pose 1613177573.82 and meas times 1613177573.82 2021-02-13T00:52:53Z- LcmTrn::updateTrn() >>>> pose time 1613177573.82, meas time 1613177573.82 <<<< 2021-02-13T00:52:53Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177573.82, ping # 0. 2021-02-13T00:52:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177573.82 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:52:53Z- TerrainNav::motionUpdate() - currEstimate = (4060280, 593298.69, 23.67) 2021-02-13T00:52:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:52:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:52:53Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:52:53Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:52:53Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177573.82 sec.; No relevant navigation data available 2021-02-13T00:52:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:52:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:52:53Z- variances([0], [2], and psi_berg) = 684.110815 832.990262 0.000000 2021-02-13T00:52:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:52:53Z- LcmTrn::publishEstimates() - MLE : 4060279.25 593321.19 23.67 2021-02-13T00:52:53Z- LcmTrn::publishEstimates() - MMSE : 4060279.75 593298.69 23.67 2021-02-13T00:52:53Z- LcmTrn::publishEstimates() - COVAR: 684.11 832.99 1.45 0.00 2021-02-13T00:52:54Z- AHRS_M2 msg: 1613177574.18 epoch sec; seqNo:16447 2021-02-13T00:52:54Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.20 psi 2021-02-13T00:52:54Z- handleDepth()-Depth_Keller msg: 1613177574191 epoch sec; seqNo:16577; depth 23.81 2021-02-13T00:52:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:54Z- handleDvl() - RDI_Pathfinder msg: 1613177574.19 epoch sec; seqNo:7966 2021-02-13T00:52:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:54Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:52:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177574.20 epoch sec; seqNo:16577; 4060279.75 north; 593298.69 east 2021-02-13T00:52:54Z- AHRS_M2 msg: 1613177574.59 epoch sec; seqNo:16448 2021-02-13T00:52:54Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.20 psi 2021-02-13T00:52:54Z- handleDepth()-Depth_Keller msg: 1613177574590 epoch sec; seqNo:16578; depth 23.94 2021-02-13T00:52:54Z- handleDvl() - RDI_Pathfinder msg: 1613177574.60 epoch sec; seqNo:7967 2021-02-13T00:52:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 97.91 , 64.98 , 97.91 2021-02-13T00:52:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.12 , -0.03 2021-02-13T00:52:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177574.61 epoch sec; seqNo:16578; 4060280.13 north; 593298.69 east 2021-02-13T00:52:54Z- AHRS_M2 msg: 1613177574.99 epoch sec; seqNo:16449 2021-02-13T00:52:54Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.20 psi 2021-02-13T00:52:54Z- handleDepth()-Depth_Keller msg: 1613177574992 epoch sec; seqNo:16579; depth 24.09 2021-02-13T00:52:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177575.02 epoch sec; seqNo:16579; 4060280.51 north; 593298.68 east 2021-02-13T00:52:55Z- AHRS_M2 msg: 1613177575.41 epoch sec; seqNo:16450 2021-02-13T00:52:55Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.20 psi 2021-02-13T00:52:55Z- handleDepth()-Depth_Keller msg: 1613177575418 epoch sec; seqNo:16580; depth 24.23 2021-02-13T00:52:55Z- handleDvl() - RDI_Pathfinder msg: 1613177575.43 epoch sec; seqNo:7968 2021-02-13T00:52:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 0.00 , 63.35 , 100.82 2021-02-13T00:52:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , 0.03 2021-02-13T00:52:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177575.44 epoch sec; seqNo:16580; 4060280.90 north; 593299.90 east 2021-02-13T00:52:55Z- AHRS_M2 msg: 1613177575.80 epoch sec; seqNo:16451 2021-02-13T00:52:55Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.20 psi 2021-02-13T00:52:55Z- handleDepth()-Depth_Keller msg: 1613177575800 epoch sec; seqNo:16581; depth 24.37 2021-02-13T00:52:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177575.81 epoch sec; seqNo:16581; 4060281.28 north; 593299.89 east 2021-02-13T00:52:56Z- AHRS_M2 msg: 1613177576.20 epoch sec; seqNo:16452 2021-02-13T00:52:56Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.20 psi 2021-02-13T00:52:56Z- handleDepth()-Depth_Keller msg: 1613177576213 epoch sec; seqNo:16582; depth 24.49 2021-02-13T00:52:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177576.22 epoch sec; seqNo:16582; 4060281.28 north; 593299.89 east 2021-02-13T00:52:56Z- AHRS_M2 msg: 1613177576.60 epoch sec; seqNo:16453 2021-02-13T00:52:56Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.20 psi 2021-02-13T00:52:56Z- handleDepth()-Depth_Keller msg: 1613177576609 epoch sec; seqNo:16583; depth 24.63 2021-02-13T00:52:56Z- handleDvl() - RDI_Pathfinder msg: 1613177576.63 epoch sec; seqNo:7969 2021-02-13T00:52:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 0.00 , 62.94 , 99.06 2021-02-13T00:52:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , 0.04 2021-02-13T00:52:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177576.64 epoch sec; seqNo:16583; 4060281.66 north; 593299.89 east 2021-02-13T00:52:57Z- AHRS_M2 msg: 1613177577.01 epoch sec; seqNo:16454 2021-02-13T00:52:57Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.20 psi 2021-02-13T00:52:57Z- handleDepth()-Depth_Keller msg: 1613177577012 epoch sec; seqNo:16584; depth 24.78 2021-02-13T00:52:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177577.04 epoch sec; seqNo:16584; 4060282.04 north; 593299.89 east 2021-02-13T00:52:57Z- AHRS_M2 msg: 1613177577.41 epoch sec; seqNo:16455 2021-02-13T00:52:57Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.19 psi 2021-02-13T00:52:57Z- handleDepth()-Depth_Keller msg: 1613177577416 epoch sec; seqNo:16585; depth 24.89 2021-02-13T00:52:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177577.43 epoch sec; seqNo:16585; 4060282.42 north; 593299.88 east 2021-02-13T00:52:57Z- AHRS_M2 msg: 1613177577.82 epoch sec; seqNo:16456 2021-02-13T00:52:57Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-13T00:52:57Z- handleDepth()-Depth_Keller msg: 1613177577827 epoch sec; seqNo:16586; depth 25.04 2021-02-13T00:52:57Z- handleDvl() - RDI_Pathfinder msg: 1613177577.84 epoch sec; seqNo:7970 2021-02-13T00:52:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 88.25 , 63.35 , 99.06 2021-02-13T00:52:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , 0.01 2021-02-13T00:52:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177577.85 epoch sec; seqNo:16586; 4060282.42 north; 593299.88 east 2021-02-13T00:52:58Z- AHRS_M2 msg: 1613177578.22 epoch sec; seqNo:16457 2021-02-13T00:52:58Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-13T00:52:58Z- handleDepth()-Depth_Keller msg: 1613177578234 epoch sec; seqNo:16587; depth 25.18 2021-02-13T00:52:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177578.25 epoch sec; seqNo:16587; 4060282.81 north; 593301.10 east 2021-02-13T00:52:58Z- AHRS_M2 msg: 1613177578.62 epoch sec; seqNo:16458 2021-02-13T00:52:58Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-13T00:52:58Z- handleDepth()-Depth_Keller msg: 1613177578628 epoch sec; seqNo:16588; depth 25.32 2021-02-13T00:52:58Z- handleDvl() - RDI_Pathfinder msg: 1613177578.64 epoch sec; seqNo:7971 2021-02-13T00:52:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 0.00 , 63.76 , 97.91 2021-02-13T00:52:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.09 , -0.02 2021-02-13T00:52:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177578.66 epoch sec; seqNo:16588; 4060283.19 north; 593301.09 east 2021-02-13T00:52:58Z- LcmTrn::handleMessages() - handling 42 messages took 0.68 ms and 129 calls... 2021-02-13T00:52:58Z- waiting for 1613177576.82, time is 1613177578.66 2021-02-13T00:52:58Z- LcmTrn::updateTrn() equating pose 1613177578.66 and meas times 1613177578.66 2021-02-13T00:52:58Z- LcmTrn::updateTrn() >>>> pose time 1613177578.66, meas time 1613177578.66 <<<< 2021-02-13T00:52:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177578.66 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:52:58Z- TerrainNav::motionUpdate() - currEstimate = (4060283, 593301.09, 25.32) 2021-02-13T00:52:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:52:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:52:58Z- TNavBF::Projecting Measurements 2021-02-13T00:52:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:52:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:52:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:52:58Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:52:58Z- TNavFilter::3 good beams. 2021-02-13T00:52:58Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:52:58Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:52:58Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:52:58Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:52:58Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:52:58Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:52:58Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:52:58Z- TNavBF::mmse selected from filter -1 2021-02-13T00:52:58Z- TerrainMapOctree: (vehN, vehE ) = (4060283.19, 593301.09). 2021-02-13T00:52:58Z- TerrainMapOctree: Min Distance = 1354.67. 2021-02-13T00:52:58Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:52:58Z- There is no specific good beam that all particles have in common. 2021-02-13T00:52:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:52:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.66 K: 0.126 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T00:52:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:52:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.45 K: 0.150 ExpectedMeasDiff: nan currMeas.covariance: 6.50 2021-02-13T00:52:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:52:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:52:58Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:52:59Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:52:59Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:52:59Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:52:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:52:59Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:52:59Z- Filter 0 No beams used in subcloud update 2021-02-13T00:52:59Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:52:59Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:52:59Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 684.1 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:52:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177578.66 sec. 2021-02-13T00:52:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:52:59Z- TNavBF::mmse selected from filter -1 2021-02-13T00:52:59Z- variances([0], [2], and psi_berg) = 684.063923 832.990241 0.000000 2021-02-13T00:52:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:52:59Z- LcmTrn::publishEstimates() - MLE : 4060282.69 593323.59 25.32 2021-02-13T00:52:59Z- LcmTrn::publishEstimates() - MMSE : 4060283.19 593301.09 25.32 2021-02-13T00:52:59Z- LcmTrn::publishEstimates() - COVAR: 684.06 832.99 1.15 0.00 2021-02-13T00:52:59Z- AHRS_M2 msg: 1613177579.03 epoch sec; seqNo:16459 2021-02-13T00:52:59Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-13T00:52:59Z- handleDepth()-Depth_Keller msg: 1613177579038 epoch sec; seqNo:16589; depth 25.47 2021-02-13T00:52:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:52:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:52:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:52:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:52:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:52:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:52:59Z- handleDvl() - RDI_Pathfinder msg: 1613177579.04 epoch sec; seqNo:7972 2021-02-13T00:52:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:52:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.09 , -0.02 2021-02-13T00:52:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177579.05 epoch sec; seqNo:16589; 4060283.57 north; 593301.09 east 2021-02-13T00:52:59Z- AHRS_M2 msg: 1613177579.43 epoch sec; seqNo:16460 2021-02-13T00:52:59Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-13T00:52:59Z- handleDepth()-Depth_Keller msg: 1613177579442 epoch sec; seqNo:16590; depth 25.59 2021-02-13T00:52:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177579.46 epoch sec; seqNo:16590; 4060283.95 north; 593301.09 east 2021-02-13T00:52:59Z- AHRS_M2 msg: 1613177579.84 epoch sec; seqNo:16461 2021-02-13T00:52:59Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.18 psi 2021-02-13T00:52:59Z- handleDepth()-Depth_Keller msg: 1613177579841 epoch sec; seqNo:16591; depth 25.72 2021-02-13T00:52:59Z- handleDvl() - RDI_Pathfinder msg: 1613177579.85 epoch sec; seqNo:7973 2021-02-13T00:52:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 87.10 , 63.76 , 97.91 2021-02-13T00:52:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.08 , -0.04 2021-02-13T00:52:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177579.88 epoch sec; seqNo:16591; 4060284.33 north; 593301.08 east 2021-02-13T00:53:00Z- AHRS_M2 msg: 1613177580.24 epoch sec; seqNo:16462 2021-02-13T00:53:00Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.18 psi 2021-02-13T00:53:00Z- handleDepth()-Depth_Keller msg: 1613177580253 epoch sec; seqNo:16592; depth 25.86 2021-02-13T00:53:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:00Z- handleDvl() - RDI_Pathfinder msg: 1613177580.26 epoch sec; seqNo:7974 2021-02-13T00:53:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.08 , -0.04 2021-02-13T00:53:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177580.27 epoch sec; seqNo:16592; 4060284.33 north; 593301.08 east 2021-02-13T00:53:00Z- AHRS_M2 msg: 1613177580.64 epoch sec; seqNo:16463 2021-02-13T00:53:00Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.19 psi 2021-02-13T00:53:00Z- handleDepth()-Depth_Keller msg: 1613177580657 epoch sec; seqNo:16593; depth 25.99 2021-02-13T00:53:00Z- handleDvl() - RDI_Pathfinder msg: 1613177580.67 epoch sec; seqNo:7975 2021-02-13T00:53:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.66 , 87.16 , 62.94 , 97.98 2021-02-13T00:53:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.11 , -0.01 2021-02-13T00:53:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177580.68 epoch sec; seqNo:16593; 4060284.71 north; 593301.08 east 2021-02-13T00:53:01Z- AHRS_M2 msg: 1613177581.05 epoch sec; seqNo:16464 2021-02-13T00:53:01Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.19 psi 2021-02-13T00:53:01Z- handleDepth()-Depth_Keller msg: 1613177581053 epoch sec; seqNo:16594; depth 26.12 2021-02-13T00:53:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177581.06 epoch sec; seqNo:16594; 4060285.09 north; 593301.07 east 2021-02-13T00:53:01Z- AHRS_M2 msg: 1613177581.45 epoch sec; seqNo:16465 2021-02-13T00:53:01Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.19 psi 2021-02-13T00:53:01Z- handleDepth()-Depth_Keller msg: 1613177581461 epoch sec; seqNo:16595; depth 26.25 2021-02-13T00:53:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:01Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:01Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:01Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:01Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:01Z- handleDvl() - RDI_Pathfinder msg: 1613177581.47 epoch sec; seqNo:7976 2021-02-13T00:53:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.11 , -0.01 2021-02-13T00:53:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177581.50 epoch sec; seqNo:16595; 4060285.48 north; 593302.29 east 2021-02-13T00:53:01Z- AHRS_M2 msg: 1613177581.86 epoch sec; seqNo:16466 2021-02-13T00:53:01Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.19 psi 2021-02-13T00:53:01Z- handleDepth()-Depth_Keller msg: 1613177581869 epoch sec; seqNo:16596; depth 26.37 2021-02-13T00:53:01Z- handleDvl() - RDI_Pathfinder msg: 1613177581.88 epoch sec; seqNo:7977 2021-02-13T00:53:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 0.00 , 62.94 , 97.98 2021-02-13T00:53:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.05 2021-02-13T00:53:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177581.89 epoch sec; seqNo:16596; 4060285.48 north; 593302.29 east 2021-02-13T00:53:02Z- AHRS_M2 msg: 1613177582.26 epoch sec; seqNo:16467 2021-02-13T00:53:02Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.18 psi 2021-02-13T00:53:02Z- handleDepth()-Depth_Keller msg: 1613177582267 epoch sec; seqNo:16597; depth 26.50 2021-02-13T00:53:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177582.29 epoch sec; seqNo:16597; 4060285.86 north; 593302.29 east 2021-02-13T00:53:02Z- AHRS_M2 msg: 1613177582.66 epoch sec; seqNo:16468 2021-02-13T00:53:02Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.18 psi 2021-02-13T00:53:02Z- handleDepth()-Depth_Keller msg: 1613177582668 epoch sec; seqNo:16598; depth 26.63 2021-02-13T00:53:02Z- handleDvl() - RDI_Pathfinder msg: 1613177582.68 epoch sec; seqNo:7978 2021-02-13T00:53:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.25 , 86.08 , 62.53 , 97.98 2021-02-13T00:53:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.10 2021-02-13T00:53:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177582.71 epoch sec; seqNo:16598; 4060286.23 north; 593302.28 east 2021-02-13T00:53:03Z- AHRS_M2 msg: 1613177583.07 epoch sec; seqNo:16469 2021-02-13T00:53:03Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.18 psi 2021-02-13T00:53:03Z- handleDepth()-Depth_Keller msg: 1613177583077 epoch sec; seqNo:16599; depth 26.76 2021-02-13T00:53:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177583.09 epoch sec; seqNo:16599; 4060286.61 north; 593302.28 east 2021-02-13T00:53:03Z- AHRS_M2 msg: 1613177583.47 epoch sec; seqNo:16470 2021-02-13T00:53:03Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.18 psi 2021-02-13T00:53:03Z- handleDepth()-Depth_Keller msg: 1613177583477 epoch sec; seqNo:16600; depth 26.89 2021-02-13T00:53:03Z- handleDvl() - RDI_Pathfinder msg: 1613177583.49 epoch sec; seqNo:7979 2021-02-13T00:53:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.66 , 0.00 , 61.71 , 97.98 2021-02-13T00:53:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , 0.09 2021-02-13T00:53:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177583.50 epoch sec; seqNo:16600; 4060286.61 north; 593302.28 east 2021-02-13T00:53:03Z- AHRS_M2 msg: 1613177583.88 epoch sec; seqNo:16471 2021-02-13T00:53:03Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.18 psi 2021-02-13T00:53:03Z- handleDepth()-Depth_Keller msg: 1613177583880 epoch sec; seqNo:16601; depth 27.01 2021-02-13T00:53:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:03Z- handleDvl() - RDI_Pathfinder msg: 1613177583.88 epoch sec; seqNo:7980 2021-02-13T00:53:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , 0.09 2021-02-13T00:53:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177583.90 epoch sec; seqNo:16601; 4060287.01 north; 593303.49 east 2021-02-13T00:53:04Z- LcmTrn::handleMessages() - handling 48 messages took 0.67 ms and 135 calls... 2021-02-13T00:53:04Z- waiting for 1613177581.66, time is 1613177583.90 2021-02-13T00:53:04Z- LcmTrn::updateTrn() equating pose 1613177583.90 and meas times 1613177583.90 2021-02-13T00:53:04Z- LcmTrn::updateTrn() >>>> pose time 1613177583.90, meas time 1613177583.90 <<<< 2021-02-13T00:53:04Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177583.90, ping # 0. 2021-02-13T00:53:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177583.90 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:53:04Z- TerrainNav::motionUpdate() - currEstimate = (4060287, 593303.49, 27.01) 2021-02-13T00:53:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:53:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:53:04Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:53:04Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:53:04Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177583.90 sec.; No relevant navigation data available 2021-02-13T00:53:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:53:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:53:04Z- variances([0], [2], and psi_berg) = 684.015334 832.990220 0.000000 2021-02-13T00:53:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:53:04Z- LcmTrn::publishEstimates() - MLE : 4060286.51 593325.99 27.01 2021-02-13T00:53:04Z- LcmTrn::publishEstimates() - MMSE : 4060287.01 593303.49 27.01 2021-02-13T00:53:04Z- LcmTrn::publishEstimates() - COVAR: 684.02 832.99 1.38 0.00 2021-02-13T00:53:04Z- AHRS_M2 msg: 1613177584.28 epoch sec; seqNo:16472 2021-02-13T00:53:04Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.18 psi 2021-02-13T00:53:04Z- handleDepth()-Depth_Keller msg: 1613177584284 epoch sec; seqNo:16602; depth 27.14 2021-02-13T00:53:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177584.29 epoch sec; seqNo:16602; 4060287.38 north; 593303.49 east 2021-02-13T00:53:04Z- AHRS_M2 msg: 1613177584.68 epoch sec; seqNo:16473 2021-02-13T00:53:04Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.18 psi 2021-02-13T00:53:04Z- handleDepth()-Depth_Keller msg: 1613177584688 epoch sec; seqNo:16603; depth 27.25 2021-02-13T00:53:04Z- handleDvl() - RDI_Pathfinder msg: 1613177584.70 epoch sec; seqNo:7981 2021-02-13T00:53:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.25 , 0.00 , 62.53 , 97.98 2021-02-13T00:53:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.07 , 0.03 2021-02-13T00:53:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177584.71 epoch sec; seqNo:16603; 4060287.38 north; 593303.49 east 2021-02-13T00:53:05Z- AHRS_M2 msg: 1613177585.09 epoch sec; seqNo:16474 2021-02-13T00:53:05Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.18 psi 2021-02-13T00:53:05Z- handleDepth()-Depth_Keller msg: 1613177585093 epoch sec; seqNo:16604; depth 27.38 2021-02-13T00:53:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:05Z- handleDvl() - RDI_Pathfinder msg: 1613177585.10 epoch sec; seqNo:7982 2021-02-13T00:53:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.07 , 0.03 2021-02-13T00:53:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177585.11 epoch sec; seqNo:16604; 4060287.76 north; 593303.49 east 2021-02-13T00:53:05Z- AHRS_M2 msg: 1613177585.50 epoch sec; seqNo:16475 2021-02-13T00:53:05Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.18 psi 2021-02-13T00:53:05Z- handleDepth()-Depth_Keller msg: 1613177585518 epoch sec; seqNo:16605; depth 27.51 2021-02-13T00:53:05Z- handleDvl() - RDI_Pathfinder msg: 1613177585.53 epoch sec; seqNo:7983 2021-02-13T00:53:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.25 , 85.00 , 61.30 , 94.74 2021-02-13T00:53:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , -0.04 2021-02-13T00:53:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177585.54 epoch sec; seqNo:16605; 4060288.14 north; 593303.48 east 2021-02-13T00:53:05Z- AHRS_M2 msg: 1613177585.90 epoch sec; seqNo:16476 2021-02-13T00:53:05Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.18 psi 2021-02-13T00:53:05Z- handleDepth()-Depth_Keller msg: 1613177585900 epoch sec; seqNo:16606; depth 27.65 2021-02-13T00:53:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177585.91 epoch sec; seqNo:16606; 4060288.52 north; 593303.48 east 2021-02-13T00:53:06Z- AHRS_M2 msg: 1613177586.30 epoch sec; seqNo:16477 2021-02-13T00:53:06Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.19 psi 2021-02-13T00:53:06Z- handleDepth()-Depth_Keller msg: 1613177586305 epoch sec; seqNo:16607; depth 27.78 2021-02-13T00:53:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:06Z- handleDvl() - RDI_Pathfinder msg: 1613177586.31 epoch sec; seqNo:7984 2021-02-13T00:53:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , -0.04 2021-02-13T00:53:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177586.32 epoch sec; seqNo:16607; 4060288.91 north; 593304.69 east 2021-02-13T00:53:06Z- AHRS_M2 msg: 1613177586.70 epoch sec; seqNo:16478 2021-02-13T00:53:06Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.18 psi 2021-02-13T00:53:06Z- handleDepth()-Depth_Keller msg: 1613177586718 epoch sec; seqNo:16608; depth 27.88 2021-02-13T00:53:06Z- handleDvl() - RDI_Pathfinder msg: 1613177586.73 epoch sec; seqNo:7985 2021-02-13T00:53:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.03 , 85.00 , 60.90 , 97.98 2021-02-13T00:53:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.04 , -0.08 2021-02-13T00:53:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177586.74 epoch sec; seqNo:16608; 4060288.91 north; 593304.69 east 2021-02-13T00:53:07Z- AHRS_M2 msg: 1613177587.11 epoch sec; seqNo:16479 2021-02-13T00:53:07Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:53:07Z- handleDepth()-Depth_Keller msg: 1613177587112 epoch sec; seqNo:16609; depth 28.02 2021-02-13T00:53:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177587.12 epoch sec; seqNo:16609; 4060289.29 north; 593304.69 east 2021-02-13T00:53:07Z- AHRS_M2 msg: 1613177587.51 epoch sec; seqNo:16480 2021-02-13T00:53:07Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:53:07Z- handleDepth()-Depth_Keller msg: 1613177587516 epoch sec; seqNo:16610; depth 28.15 2021-02-13T00:53:07Z- handleDvl() - RDI_Pathfinder msg: 1613177587.53 epoch sec; seqNo:7986 2021-02-13T00:53:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.03 , 0.00 , 61.71 , 97.98 2021-02-13T00:53:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.07 , -0.08 2021-02-13T00:53:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177587.54 epoch sec; seqNo:16610; 4060289.67 north; 593304.69 east 2021-02-13T00:53:07Z- AHRS_M2 msg: 1613177587.92 epoch sec; seqNo:16481 2021-02-13T00:53:07Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:53:07Z- handleDepth()-Depth_Keller msg: 1613177587921 epoch sec; seqNo:16611; depth 28.29 2021-02-13T00:53:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177587.93 epoch sec; seqNo:16611; 4060290.05 north; 593304.68 east 2021-02-13T00:53:08Z- AHRS_M2 msg: 1613177588.32 epoch sec; seqNo:16482 2021-02-13T00:53:08Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:53:08Z- handleDepth()-Depth_Keller msg: 1613177588324 epoch sec; seqNo:16612; depth 28.40 2021-02-13T00:53:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177588.34 epoch sec; seqNo:16612; 4060290.43 north; 593304.68 east 2021-02-13T00:53:08Z- AHRS_M2 msg: 1613177588.72 epoch sec; seqNo:16483 2021-02-13T00:53:08Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.17 psi 2021-02-13T00:53:08Z- handleDepth()-Depth_Keller msg: 1613177588728 epoch sec; seqNo:16613; depth 28.53 2021-02-13T00:53:08Z- handleDvl() - RDI_Pathfinder msg: 1613177588.74 epoch sec; seqNo:7987 2021-02-13T00:53:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.62 , 0.00 , 61.30 , 97.98 2021-02-13T00:53:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.10 , -0.04 2021-02-13T00:53:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177588.76 epoch sec; seqNo:16613; 4060290.81 north; 593304.67 east 2021-02-13T00:53:09Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-13T00:53:09Z- waiting for 1613177586.90, time is 1613177588.76 2021-02-13T00:53:09Z- LcmTrn::updateTrn() equating pose 1613177588.76 and meas times 1613177588.76 2021-02-13T00:53:09Z- LcmTrn::updateTrn() >>>> pose time 1613177588.76, meas time 1613177588.76 <<<< 2021-02-13T00:53:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177588.76 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:53:09Z- TerrainNav::motionUpdate() - currEstimate = (4060291, 593304.67, 28.53) 2021-02-13T00:53:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:53:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:53:09Z- TNavBF::Projecting Measurements 2021-02-13T00:53:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:53:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:53:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:53:09Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:53:09Z- TNavFilter::3 good beams. 2021-02-13T00:53:09Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:53:09Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:53:09Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:53:09Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:53:09Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:53:09Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:53:09Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:53:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:53:09Z- TerrainMapOctree: (vehN, vehE ) = (4060290.81, 593304.67). 2021-02-13T00:53:09Z- TerrainMapOctree: Min Distance = 1346.73. 2021-02-13T00:53:09Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:53:09Z- There is no specific good beam that all particles have in common. 2021-02-13T00:53:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:53:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.59 K: 0.152 ExpectedMeasDiff: nan currMeas.covariance: 7.10 2021-02-13T00:53:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:53:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.148 ExpectedMeasDiff: nan currMeas.covariance: 6.01 2021-02-13T00:53:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:53:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:53:09Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:53:09Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:53:09Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:53:09Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:53:09Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:53:09Z- Filter 0 No beams used in subcloud update 2021-02-13T00:53:09Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:53:09Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:53:09Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 684.0 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:53:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177588.76 sec. 2021-02-13T00:53:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:53:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:53:09Z- variances([0], [2], and psi_berg) = 683.969694 832.990199 0.000000 2021-02-13T00:53:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:53:09Z- LcmTrn::publishEstimates() - MLE : 4060290.31 593325.17 28.53 2021-02-13T00:53:09Z- LcmTrn::publishEstimates() - MMSE : 4060290.81 593304.67 28.53 2021-02-13T00:53:09Z- LcmTrn::publishEstimates() - COVAR: 683.97 832.99 1.07 0.00 2021-02-13T00:53:09Z- AHRS_M2 msg: 1613177589.13 epoch sec; seqNo:16484 2021-02-13T00:53:09Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.17 psi 2021-02-13T00:53:09Z- handleDepth()-Depth_Keller msg: 1613177589137 epoch sec; seqNo:16614; depth 28.65 2021-02-13T00:53:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177589.15 epoch sec; seqNo:16614; 4060291.20 north; 593305.89 east 2021-02-13T00:53:09Z- AHRS_M2 msg: 1613177589.53 epoch sec; seqNo:16485 2021-02-13T00:53:09Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.17 psi 2021-02-13T00:53:09Z- handleDepth()-Depth_Keller msg: 1613177589538 epoch sec; seqNo:16615; depth 28.78 2021-02-13T00:53:09Z- handleDvl() - RDI_Pathfinder msg: 1613177589.55 epoch sec; seqNo:7988 2021-02-13T00:53:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.21 , 82.84 , 60.90 , 97.98 2021-02-13T00:53:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.12 , -0.01 2021-02-13T00:53:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177589.56 epoch sec; seqNo:16615; 4060291.20 north; 593305.89 east 2021-02-13T00:53:09Z- AHRS_M2 msg: 1613177589.94 epoch sec; seqNo:16486 2021-02-13T00:53:09Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.17 psi 2021-02-13T00:53:09Z- handleDepth()-Depth_Keller msg: 1613177589940 epoch sec; seqNo:16616; depth 28.92 2021-02-13T00:53:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:09Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:09Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:09Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:09Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:09Z- handleDvl() - RDI_Pathfinder msg: 1613177589.94 epoch sec; seqNo:7989 2021-02-13T00:53:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.12 , -0.01 2021-02-13T00:53:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177589.97 epoch sec; seqNo:16616; 4060291.58 north; 593305.88 east 2021-02-13T00:53:10Z- AHRS_M2 msg: 1613177590.34 epoch sec; seqNo:16487 2021-02-13T00:53:10Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.17 psi 2021-02-13T00:53:10Z- handleDepth()-Depth_Keller msg: 1613177590344 epoch sec; seqNo:16617; depth 29.05 2021-02-13T00:53:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177590.35 epoch sec; seqNo:16617; 4060291.96 north; 593305.88 east 2021-02-13T00:53:10Z- AHRS_M2 msg: 1613177590.74 epoch sec; seqNo:16488 2021-02-13T00:53:10Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.17 psi 2021-02-13T00:53:10Z- handleDepth()-Depth_Keller msg: 1613177590753 epoch sec; seqNo:16618; depth 29.17 2021-02-13T00:53:10Z- handleDvl() - RDI_Pathfinder msg: 1613177590.76 epoch sec; seqNo:7990 2021-02-13T00:53:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.80 , 81.75 , 60.49 , 97.98 2021-02-13T00:53:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.11 , 0.03 2021-02-13T00:53:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177590.78 epoch sec; seqNo:16618; 4060292.34 north; 593305.88 east 2021-02-13T00:53:11Z- AHRS_M2 msg: 1613177591.15 epoch sec; seqNo:16489 2021-02-13T00:53:11Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.17 psi 2021-02-13T00:53:11Z- handleDepth()-Depth_Keller msg: 1613177591158 epoch sec; seqNo:16619; depth 29.30 2021-02-13T00:53:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:11Z- handleDvl() - RDI_Pathfinder msg: 1613177591.16 epoch sec; seqNo:7991 2021-02-13T00:53:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.11 , 0.03 2021-02-13T00:53:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177591.17 epoch sec; seqNo:16619; 4060292.72 north; 593305.87 east 2021-02-13T00:53:11Z- AHRS_M2 msg: 1613177591.55 epoch sec; seqNo:16490 2021-02-13T00:53:11Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.17 psi 2021-02-13T00:53:11Z- handleDepth()-Depth_Keller msg: 1613177591556 epoch sec; seqNo:16620; depth 29.45 2021-02-13T00:53:11Z- handleDvl() - RDI_Pathfinder msg: 1613177591.57 epoch sec; seqNo:7992 2021-02-13T00:53:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:11Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:53:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177591.59 epoch sec; seqNo:16620; 4060292.72 north; 593305.87 east 2021-02-13T00:53:11Z- AHRS_M2 msg: 1613177591.96 epoch sec; seqNo:16491 2021-02-13T00:53:11Z- AHRS_M2 msg: 0.61 phi; -0.26 theta; -0.17 psi 2021-02-13T00:53:11Z- handleDepth()-Depth_Keller msg: 1613177591960 epoch sec; seqNo:16621; depth 29.59 2021-02-13T00:53:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177591.97 epoch sec; seqNo:16621; 4060293.11 north; 593307.09 east 2021-02-13T00:53:12Z- AHRS_M2 msg: 1613177592.36 epoch sec; seqNo:16492 2021-02-13T00:53:12Z- AHRS_M2 msg: 0.61 phi; -0.23 theta; -0.16 psi 2021-02-13T00:53:12Z- handleDepth()-Depth_Keller msg: 1613177592368 epoch sec; seqNo:16622; depth 29.73 2021-02-13T00:53:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:12Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:12Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:12Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:12Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:12Z- handleDvl() - RDI_Pathfinder msg: 1613177592.37 epoch sec; seqNo:7993 2021-02-13T00:53:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:12Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:53:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177592.38 epoch sec; seqNo:16622; 4060293.49 north; 593307.08 east 2021-02-13T00:53:12Z- AHRS_M2 msg: 1613177592.77 epoch sec; seqNo:16493 2021-02-13T00:53:12Z- AHRS_M2 msg: 0.60 phi; -0.18 theta; -0.15 psi 2021-02-13T00:53:12Z- handleDepth()-Depth_Keller msg: 1613177592771 epoch sec; seqNo:16623; depth 29.85 2021-02-13T00:53:12Z- handleDvl() - RDI_Pathfinder msg: 1613177592.78 epoch sec; seqNo:7994 2021-02-13T00:53:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.31 , 0.00 , 60.66 , 85.34 2021-02-13T00:53:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.06 2021-02-13T00:53:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177592.80 epoch sec; seqNo:16623; 4060293.86 north; 593307.08 east 2021-02-13T00:53:13Z- AHRS_M2 msg: 1613177593.17 epoch sec; seqNo:16494 2021-02-13T00:53:13Z- AHRS_M2 msg: 0.59 phi; -0.14 theta; -0.15 psi 2021-02-13T00:53:13Z- handleDepth()-Depth_Keller msg: 1613177593182 epoch sec; seqNo:16624; depth 29.97 2021-02-13T00:53:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177593.19 epoch sec; seqNo:16624; 4060294.24 north; 593307.08 east 2021-02-13T00:53:13Z- AHRS_M2 msg: 1613177593.57 epoch sec; seqNo:16495 2021-02-13T00:53:13Z- AHRS_M2 msg: 0.59 phi; -0.10 theta; -0.15 psi 2021-02-13T00:53:13Z- handleDepth()-Depth_Keller msg: 1613177593581 epoch sec; seqNo:16625; depth 30.10 2021-02-13T00:53:13Z- handleDvl() - RDI_Pathfinder msg: 1613177593.60 epoch sec; seqNo:7995 2021-02-13T00:53:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.94 , 78.51 , 62.94 , 78.51 2021-02-13T00:53:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , 0.04 2021-02-13T00:53:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177593.62 epoch sec; seqNo:16625; 4060294.24 north; 593307.08 east 2021-02-13T00:53:13Z- AHRS_M2 msg: 1613177593.97 epoch sec; seqNo:16496 2021-02-13T00:53:13Z- AHRS_M2 msg: 0.58 phi; -0.06 theta; -0.15 psi 2021-02-13T00:53:13Z- handleDepth()-Depth_Keller msg: 1613177593981 epoch sec; seqNo:16626; depth 30.17 2021-02-13T00:53:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177594.00 epoch sec; seqNo:16626; 4060294.62 north; 593307.07 east 2021-02-13T00:53:14Z- LcmTrn::handleMessages() - handling 47 messages took 0.67 ms and 134 calls... 2021-02-13T00:53:14Z- waiting for 1613177591.76, time is 1613177594.00 2021-02-13T00:53:14Z- LcmTrn::updateTrn() >>>> pose time 1613177594.00, meas time 1613177593.60 <<<< 2021-02-13T00:53:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177593.60 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:53:14Z- TerrainNav::motionUpdate() - currEstimate = (4060295, 593307.07, 30.17) 2021-02-13T00:53:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:53:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:53:14Z- TNavBF::Projecting Measurements 2021-02-13T00:53:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:53:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:53:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:53:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:53:14Z- TNavFilter::4 good beams. 2021-02-13T00:53:14Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:53:14Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:53:14Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:53:14Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:53:14Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:53:14Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:53:14Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:53:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:53:14Z- TerrainMapOctree: (vehN, vehE ) = (4060294.33, 593306.89). 2021-02-13T00:53:14Z- TerrainMapOctree: Min Distance = 1342.67. 2021-02-13T00:53:14Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:53:14Z- There is no specific good beam that all particles have in common. 2021-02-13T00:53:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:53:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.137 ExpectedMeasDiff: nan currMeas.covariance: 6.34 2021-02-13T00:53:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:53:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:53:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:53:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 6.34 2021-02-13T00:53:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:53:14Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:53:14Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:53:14Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:53:14Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:53:14Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:53:14Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:53:14Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:53:14Z- Filter 0 No beams used in subcloud update 2021-02-13T00:53:14Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:53:14Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:53:14Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 683.9 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:53:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177593.60 sec. 2021-02-13T00:53:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:53:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:53:14Z- variances([0], [2], and psi_berg) = 683.909626 832.990173 0.000000 2021-02-13T00:53:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:53:14Z- LcmTrn::publishEstimates() - MLE : 4060294.12 593327.57 30.17 2021-02-13T00:53:14Z- LcmTrn::publishEstimates() - MMSE : 4060294.62 593307.07 30.17 2021-02-13T00:53:14Z- LcmTrn::publishEstimates() - COVAR: 683.91 832.99 0.85 0.00 2021-02-13T00:53:14Z- AHRS_M2 msg: 1613177594.38 epoch sec; seqNo:16497 2021-02-13T00:53:14Z- AHRS_M2 msg: 0.57 phi; -0.02 theta; -0.15 psi 2021-02-13T00:53:14Z- handleDepth()-Depth_Keller msg: 1613177594386 epoch sec; seqNo:16627; depth 30.26 2021-02-13T00:53:14Z- handleDvl() - RDI_Pathfinder msg: 1613177594.40 epoch sec; seqNo:7996 2021-02-13T00:53:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.47 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:14Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:53:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177594.41 epoch sec; seqNo:16627; 4060295.00 north; 593307.07 east 2021-02-13T00:53:14Z- AHRS_M2 msg: 1613177594.78 epoch sec; seqNo:16498 2021-02-13T00:53:14Z- AHRS_M2 msg: 0.57 phi; 0.02 theta; -0.15 psi 2021-02-13T00:53:14Z- handleDepth()-Depth_Keller msg: 1613177594798 epoch sec; seqNo:16628; depth 30.29 2021-02-13T00:53:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:14Z- handleDvl() - RDI_Pathfinder msg: 1613177594.80 epoch sec; seqNo:7997 2021-02-13T00:53:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:14Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:53:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177594.81 epoch sec; seqNo:16628; 4060295.39 north; 593308.28 east 2021-02-13T00:53:15Z- AHRS_M2 msg: 1613177595.20 epoch sec; seqNo:16499 2021-02-13T00:53:15Z- AHRS_M2 msg: 0.55 phi; 0.09 theta; -0.15 psi 2021-02-13T00:53:15Z- handleDepth()-Depth_Keller msg: 1613177595200 epoch sec; seqNo:16629; depth 30.34 2021-02-13T00:53:15Z- handleDvl() - RDI_Pathfinder msg: 1613177595.22 epoch sec; seqNo:7998 2021-02-13T00:53:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.45 , 0.00 , 0.00 , 64.67 2021-02-13T00:53:15Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:53:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177595.23 epoch sec; seqNo:16629; 4060295.39 north; 593308.28 east 2021-02-13T00:53:15Z- AHRS_M2 msg: 1613177595.60 epoch sec; seqNo:16500 2021-02-13T00:53:15Z- AHRS_M2 msg: 0.55 phi; 0.12 theta; -0.16 psi 2021-02-13T00:53:15Z- handleDepth()-Depth_Keller msg: 1613177595602 epoch sec; seqNo:16630; depth 30.38 2021-02-13T00:53:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177595.62 epoch sec; seqNo:16630; 4060295.77 north; 593308.28 east 2021-02-13T00:53:15Z- AHRS_M2 msg: 1613177596.00 epoch sec; seqNo:16501 2021-02-13T00:53:15Z- AHRS_M2 msg: 0.54 phi; 0.15 theta; -0.16 psi 2021-02-13T00:53:16Z- handleDepth()-Depth_Keller msg: 1613177596002 epoch sec; seqNo:16631; depth 30.40 2021-02-13T00:53:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:16Z- handleDvl() - RDI_Pathfinder msg: 1613177596.00 epoch sec; seqNo:7999 2021-02-13T00:53:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:16Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:53:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177596.01 epoch sec; seqNo:16631; 4060296.15 north; 593308.28 east 2021-02-13T00:53:16Z- AHRS_M2 msg: 1613177596.40 epoch sec; seqNo:16502 2021-02-13T00:53:16Z- AHRS_M2 msg: 0.54 phi; 0.18 theta; -0.16 psi 2021-02-13T00:53:16Z- handleDepth()-Depth_Keller msg: 1613177596404 epoch sec; seqNo:16632; depth 30.41 2021-02-13T00:53:16Z- handleDvl() - RDI_Pathfinder msg: 1613177596.42 epoch sec; seqNo:8000 2021-02-13T00:53:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.45 , 0.00 , 0.00 , 62.45 2021-02-13T00:53:16Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:53:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177596.43 epoch sec; seqNo:16632; 4060296.53 north; 593308.27 east 2021-02-13T00:53:16Z- AHRS_M2 msg: 1613177596.80 epoch sec; seqNo:16503 2021-02-13T00:53:16Z- AHRS_M2 msg: 0.54 phi; 0.20 theta; -0.16 psi 2021-02-13T00:53:16Z- handleDepth()-Depth_Keller msg: 1613177596813 epoch sec; seqNo:16633; depth 30.40 2021-02-13T00:53:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177596.82 epoch sec; seqNo:16633; 4060296.53 north; 593308.27 east 2021-02-13T00:53:17Z- handleDepth()-Depth_Keller msg: 1613177597233 epoch sec; seqNo:16634; depth 30.38 2021-02-13T00:53:17Z- handleDvl() - RDI_Pathfinder msg: 1613177597.25 epoch sec; seqNo:8001 2021-02-13T00:53:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.34 , 0.00 , 0.00 , 61.12 2021-02-13T00:53:17Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:53:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177597.26 epoch sec; seqNo:16634; 4060296.91 north; 593308.27 east 2021-02-13T00:53:17Z- AHRS_M2 msg: 1613177597.61 epoch sec; seqNo:16504 2021-02-13T00:53:17Z- AHRS_M2 msg: 0.53 phi; 0.24 theta; -0.16 psi 2021-02-13T00:53:17Z- handleDepth()-Depth_Keller msg: 1613177597618 epoch sec; seqNo:16635; depth 30.35 2021-02-13T00:53:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177597.63 epoch sec; seqNo:16635; 4060297.30 north; 593309.48 east 2021-02-13T00:53:18Z- AHRS_M2 msg: 1613177598.02 epoch sec; seqNo:16505 2021-02-13T00:53:18Z- AHRS_M2 msg: 0.53 phi; 0.25 theta; -0.16 psi 2021-02-13T00:53:18Z- handleDepth()-Depth_Keller msg: 1613177598037 epoch sec; seqNo:16636; depth 30.31 2021-02-13T00:53:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177598.05 epoch sec; seqNo:16636; 4060297.68 north; 593309.48 east 2021-02-13T00:53:18Z- AHRS_M2 msg: 1613177598.42 epoch sec; seqNo:16506 2021-02-13T00:53:18Z- AHRS_M2 msg: 0.53 phi; 0.26 theta; -0.16 psi 2021-02-13T00:53:18Z- handleDepth()-Depth_Keller msg: 1613177598424 epoch sec; seqNo:16637; depth 30.25 2021-02-13T00:53:18Z- handleDvl() - RDI_Pathfinder msg: 1613177598.44 epoch sec; seqNo:8002 2021-02-13T00:53:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:18Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:53:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177598.47 epoch sec; seqNo:16637; 4060297.68 north; 593309.48 east 2021-02-13T00:53:18Z- AHRS_M2 msg: 1613177598.82 epoch sec; seqNo:16507 2021-02-13T00:53:18Z- AHRS_M2 msg: 0.52 phi; 0.27 theta; -0.16 psi 2021-02-13T00:53:18Z- handleDepth()-Depth_Keller msg: 1613177598827 epoch sec; seqNo:16638; depth 30.20 2021-02-13T00:53:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177598.84 epoch sec; seqNo:16638; 4060298.06 north; 593309.48 east 2021-02-13T00:53:19Z- AHRS_M2 msg: 1613177599.23 epoch sec; seqNo:16508 2021-02-13T00:53:19Z- AHRS_M2 msg: 0.52 phi; 0.28 theta; -0.16 psi 2021-02-13T00:53:19Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-13T00:53:19Z- waiting for 1613177597.00, time is 1613177599.23 2021-02-13T00:53:19Z- LcmTrn::updateTrn() >>>> pose time 1613177599.23, meas time 1613177598.44 <<<< 2021-02-13T00:53:19Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177598.44, ping # 0. 2021-02-13T00:53:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177598.44 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:53:19Z- TerrainNav::motionUpdate() - currEstimate = (4060298, 593309.48, 30.20) 2021-02-13T00:53:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:53:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:53:19Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:53:19Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:53:19Z- TNavBF::Projecting Measurements 2021-02-13T00:53:19Z- TNavFilter::Measurement from time = 1613177598.44 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:53:19Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:53:19Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:53:19Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:53:19Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:53:19Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:53:19Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:53:19Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:53:19Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 683.9 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:53:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:53:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:53:19Z- variances([0], [2], and psi_berg) = 683.848321 832.990146 0.000000 2021-02-13T00:53:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:53:19Z- LcmTrn::publishEstimates() - MLE : 4060297.56 593329.98 30.20 2021-02-13T00:53:19Z- LcmTrn::publishEstimates() - MMSE : 4060298.06 593309.48 30.20 2021-02-13T00:53:19Z- LcmTrn::publishEstimates() - COVAR: 683.85 832.99 1.07 0.00 2021-02-13T00:53:19Z- handleDepth()-Depth_Keller msg: 1613177599234 epoch sec; seqNo:16639; depth 30.13 2021-02-13T00:53:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177599.24 epoch sec; seqNo:16639; 4060298.44 north; 593309.47 east 2021-02-13T00:53:19Z- AHRS_M2 msg: 1613177599.63 epoch sec; seqNo:16509 2021-02-13T00:53:19Z- AHRS_M2 msg: 0.52 phi; 0.29 theta; -0.15 psi 2021-02-13T00:53:19Z- handleDepth()-Depth_Keller msg: 1613177599636 epoch sec; seqNo:16640; depth 30.07 2021-02-13T00:53:19Z- handleDvl() - RDI_Pathfinder msg: 1613177599.66 epoch sec; seqNo:8003 2021-02-13T00:53:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.57 , 78.51 , 86.08 , 60.49 2021-02-13T00:53:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.09 , -0.05 2021-02-13T00:53:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177599.67 epoch sec; seqNo:16640; 4060298.82 north; 593309.47 east 2021-02-13T00:53:20Z- AHRS_M2 msg: 1613177600.04 epoch sec; seqNo:16510 2021-02-13T00:53:20Z- AHRS_M2 msg: 0.52 phi; 0.30 theta; -0.15 psi 2021-02-13T00:53:20Z- handleDepth()-Depth_Keller msg: 1613177600041 epoch sec; seqNo:16641; depth 29.99 2021-02-13T00:53:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177600.05 epoch sec; seqNo:16641; 4060298.82 north; 593309.47 east 2021-02-13T00:53:20Z- AHRS_M2 msg: 1613177600.44 epoch sec; seqNo:16511 2021-02-13T00:53:20Z- AHRS_M2 msg: 0.52 phi; 0.31 theta; -0.15 psi 2021-02-13T00:53:20Z- handleDepth()-Depth_Keller msg: 1613177600449 epoch sec; seqNo:16642; depth 29.91 2021-02-13T00:53:20Z- handleDvl() - RDI_Pathfinder msg: 1613177600.46 epoch sec; seqNo:8004 2021-02-13T00:53:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.80 , 76.43 , 0.00 , 60.90 2021-02-13T00:53:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , -0.01 2021-02-13T00:53:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177600.47 epoch sec; seqNo:16642; 4060299.20 north; 593309.46 east 2021-02-13T00:53:20Z- AHRS_M2 msg: 1613177600.84 epoch sec; seqNo:16512 2021-02-13T00:53:20Z- AHRS_M2 msg: 0.53 phi; 0.31 theta; -0.14 psi 2021-02-13T00:53:20Z- handleDepth()-Depth_Keller msg: 1613177600848 epoch sec; seqNo:16643; depth 29.83 2021-02-13T00:53:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:20Z- handleDvl() - RDI_Pathfinder msg: 1613177600.85 epoch sec; seqNo:8005 2021-02-13T00:53:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , -0.01 2021-02-13T00:53:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177600.86 epoch sec; seqNo:16643; 4060299.59 north; 593310.68 east 2021-02-13T00:53:21Z- AHRS_M2 msg: 1613177601.25 epoch sec; seqNo:16513 2021-02-13T00:53:21Z- AHRS_M2 msg: 0.53 phi; 0.32 theta; -0.14 psi 2021-02-13T00:53:21Z- handleDepth()-Depth_Keller msg: 1613177601259 epoch sec; seqNo:16644; depth 29.76 2021-02-13T00:53:21Z- handleDvl() - RDI_Pathfinder msg: 1613177601.27 epoch sec; seqNo:8006 2021-02-13T00:53:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 66.68 , 0.00 , 0.00 , 59.88 2021-02-13T00:53:21Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:53:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177601.28 epoch sec; seqNo:16644; 4060299.97 north; 593310.68 east 2021-02-13T00:53:21Z- AHRS_M2 msg: 1613177601.65 epoch sec; seqNo:16514 2021-02-13T00:53:21Z- AHRS_M2 msg: 0.53 phi; 0.32 theta; -0.14 psi 2021-02-13T00:53:21Z- handleDepth()-Depth_Keller msg: 1613177601658 epoch sec; seqNo:16645; depth 29.65 2021-02-13T00:53:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177601.67 epoch sec; seqNo:16645; 4060299.97 north; 593310.68 east 2021-02-13T00:53:22Z- AHRS_M2 msg: 1613177602.06 epoch sec; seqNo:16515 2021-02-13T00:53:22Z- AHRS_M2 msg: 0.53 phi; 0.33 theta; -0.14 psi 2021-02-13T00:53:22Z- handleDepth()-Depth_Keller msg: 1613177602060 epoch sec; seqNo:16646; depth 29.57 2021-02-13T00:53:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:22Z- handleDvl() - RDI_Pathfinder msg: 1613177602.06 epoch sec; seqNo:8007 2021-02-13T00:53:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:22Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:53:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177602.09 epoch sec; seqNo:16646; 4060300.35 north; 593310.67 east 2021-02-13T00:53:22Z- AHRS_M2 msg: 1613177602.46 epoch sec; seqNo:16516 2021-02-13T00:53:22Z- AHRS_M2 msg: 0.53 phi; 0.33 theta; -0.14 psi 2021-02-13T00:53:22Z- handleDepth()-Depth_Keller msg: 1613177602464 epoch sec; seqNo:16647; depth 29.46 2021-02-13T00:53:22Z- handleDvl() - RDI_Pathfinder msg: 1613177602.48 epoch sec; seqNo:8008 2021-02-13T00:53:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 67.29 , 0.00 , 0.00 , 60.48 2021-02-13T00:53:22Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:53:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177602.50 epoch sec; seqNo:16647; 4060300.72 north; 593310.67 east 2021-02-13T00:53:22Z- AHRS_M2 msg: 1613177602.86 epoch sec; seqNo:16517 2021-02-13T00:53:22Z- AHRS_M2 msg: 0.53 phi; 0.34 theta; -0.14 psi 2021-02-13T00:53:22Z- handleDepth()-Depth_Keller msg: 1613177602873 epoch sec; seqNo:16648; depth 29.36 2021-02-13T00:53:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177602.88 epoch sec; seqNo:16648; 4060301.10 north; 593310.66 east 2021-02-13T00:53:23Z- AHRS_M2 msg: 1613177603.27 epoch sec; seqNo:16518 2021-02-13T00:53:23Z- AHRS_M2 msg: 0.53 phi; 0.34 theta; -0.14 psi 2021-02-13T00:53:23Z- handleDepth()-Depth_Keller msg: 1613177603274 epoch sec; seqNo:16649; depth 29.28 2021-02-13T00:53:23Z- handleDvl() - RDI_Pathfinder msg: 1613177603.29 epoch sec; seqNo:8009 2021-02-13T00:53:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 68.14 , 0.00 , 0.00 , 60.48 2021-02-13T00:53:23Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:53:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177603.30 epoch sec; seqNo:16649; 4060301.48 north; 593310.66 east 2021-02-13T00:53:23Z- AHRS_M2 msg: 1613177603.67 epoch sec; seqNo:16519 2021-02-13T00:53:23Z- AHRS_M2 msg: 0.53 phi; 0.34 theta; -0.14 psi 2021-02-13T00:53:23Z- handleDepth()-Depth_Keller msg: 1613177603677 epoch sec; seqNo:16650; depth 29.17 2021-02-13T00:53:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177603.69 epoch sec; seqNo:16650; 4060301.48 north; 593310.66 east 2021-02-13T00:53:24Z- AHRS_M2 msg: 1613177604.08 epoch sec; seqNo:16520 2021-02-13T00:53:24Z- AHRS_M2 msg: 0.54 phi; 0.35 theta; -0.13 psi 2021-02-13T00:53:24Z- handleDepth()-Depth_Keller msg: 1613177604080 epoch sec; seqNo:16651; depth 29.07 2021-02-13T00:53:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177604.09 epoch sec; seqNo:16651; 4060301.86 north; 593310.66 east 2021-02-13T00:53:24Z- LcmTrn::handleMessages() - handling 45 messages took 0.64 ms and 132 calls... 2021-02-13T00:53:24Z- waiting for 1613177602.23, time is 1613177604.09 2021-02-13T00:53:24Z- LcmTrn::updateTrn() >>>> pose time 1613177604.09, meas time 1613177603.29 <<<< 2021-02-13T00:53:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177603.29 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:53:24Z- TerrainNav::motionUpdate() - currEstimate = (4060302, 593310.66, 29.07) 2021-02-13T00:53:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:53:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:53:24Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:53:24Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:53:24Z- TNavBF::Projecting Measurements 2021-02-13T00:53:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:53:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:53:24Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-13T00:53:24Z- TNavFilter::2 good beams. 2021-02-13T00:53:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:53:24Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:53:24Z- TNavBF:: beamIndex[1] = 3 2021-02-13T00:53:24Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:53:24Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:53:24Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-13T00:53:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:53:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:53:24Z- TerrainMapOctree: (vehN, vehE ) = (4060301.23, 593310.46). 2021-02-13T00:53:24Z- TerrainMapOctree: Min Distance = 1335.25. 2021-02-13T00:53:24Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:53:24Z- There is no specific good beam that all particles have in common. 2021-02-13T00:53:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:53:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.119 ExpectedMeasDiff: nan currMeas.covariance: 7.43 2021-02-13T00:53:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:53:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.126 ExpectedMeasDiff: nan currMeas.covariance: 5.85 2021-02-13T00:53:24Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:53:24Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:53:24Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:53:24Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:53:24Z- Filter 0 No beams used in subcloud update 2021-02-13T00:53:24Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:53:24Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:53:24Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 683.8 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:53:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177603.29 sec. 2021-02-13T00:53:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:53:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:53:24Z- variances([0], [2], and psi_berg) = 683.788195 832.990119 0.000000 2021-02-13T00:53:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:53:24Z- LcmTrn::publishEstimates() - MLE : 4060301.36 593332.16 29.07 2021-02-13T00:53:24Z- LcmTrn::publishEstimates() - MMSE : 4060301.86 593310.66 29.07 2021-02-13T00:53:24Z- LcmTrn::publishEstimates() - COVAR: 683.79 832.99 0.99 0.00 2021-02-13T00:53:24Z- AHRS_M2 msg: 1613177604.48 epoch sec; seqNo:16521 2021-02-13T00:53:24Z- AHRS_M2 msg: 0.54 phi; 0.35 theta; -0.13 psi 2021-02-13T00:53:24Z- handleDepth()-Depth_Keller msg: 1613177604486 epoch sec; seqNo:16652; depth 28.97 2021-02-13T00:53:24Z- handleDvl() - RDI_Pathfinder msg: 1613177604.50 epoch sec; seqNo:8010 2021-02-13T00:53:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 68.57 , 0.00 , 0.00 , 60.48 2021-02-13T00:53:24Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:53:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177604.51 epoch sec; seqNo:16652; 4060302.25 north; 593311.87 east 2021-02-13T00:53:24Z- AHRS_M2 msg: 1613177604.88 epoch sec; seqNo:16522 2021-02-13T00:53:24Z- AHRS_M2 msg: 0.54 phi; 0.35 theta; -0.13 psi 2021-02-13T00:53:24Z- handleDepth()-Depth_Keller msg: 1613177604898 epoch sec; seqNo:16653; depth 28.86 2021-02-13T00:53:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177604.91 epoch sec; seqNo:16653; 4060302.25 north; 593311.87 east 2021-02-13T00:53:25Z- AHRS_M2 msg: 1613177605.29 epoch sec; seqNo:16523 2021-02-13T00:53:25Z- AHRS_M2 msg: 0.55 phi; 0.35 theta; -0.13 psi 2021-02-13T00:53:25Z- handleDepth()-Depth_Keller msg: 1613177605292 epoch sec; seqNo:16654; depth 28.74 2021-02-13T00:53:25Z- handleDvl() - RDI_Pathfinder msg: 1613177605.30 epoch sec; seqNo:8011 2021-02-13T00:53:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:25Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:53:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177605.34 epoch sec; seqNo:16654; 4060302.63 north; 593311.87 east 2021-02-13T00:53:25Z- AHRS_M2 msg: 1613177605.69 epoch sec; seqNo:16524 2021-02-13T00:53:25Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.13 psi 2021-02-13T00:53:25Z- handleDepth()-Depth_Keller msg: 1613177605696 epoch sec; seqNo:16655; depth 28.62 2021-02-13T00:53:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:25Z- handleDvl() - RDI_Pathfinder msg: 1613177605.70 epoch sec; seqNo:8012 2021-02-13T00:53:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:25Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:53:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177605.71 epoch sec; seqNo:16655; 4060303.01 north; 593311.86 east 2021-02-13T00:53:26Z- AHRS_M2 msg: 1613177606.11 epoch sec; seqNo:16525 2021-02-13T00:53:26Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.13 psi 2021-02-13T00:53:26Z- handleDepth()-Depth_Keller msg: 1613177606117 epoch sec; seqNo:16656; depth 28.51 2021-02-13T00:53:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177606.13 epoch sec; seqNo:16656; 4060303.39 north; 593311.86 east 2021-02-13T00:53:26Z- AHRS_M2 msg: 1613177606.51 epoch sec; seqNo:16526 2021-02-13T00:53:26Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.13 psi 2021-02-13T00:53:26Z- handleDepth()-Depth_Keller msg: 1613177606514 epoch sec; seqNo:16657; depth 28.42 2021-02-13T00:53:26Z- handleDvl() - RDI_Pathfinder msg: 1613177606.53 epoch sec; seqNo:8013 2021-02-13T00:53:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.66 , 81.75 , 97.98 , 61.30 2021-02-13T00:53:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.02 , -0.09 2021-02-13T00:53:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177606.54 epoch sec; seqNo:16657; 4060303.39 north; 593311.86 east 2021-02-13T00:53:26Z- AHRS_M2 msg: 1613177606.90 epoch sec; seqNo:16527 2021-02-13T00:53:26Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.13 psi 2021-02-13T00:53:26Z- handleDepth()-Depth_Keller msg: 1613177606917 epoch sec; seqNo:16658; depth 28.31 2021-02-13T00:53:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:26Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:26Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:26Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:26Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:26Z- handleDvl() - RDI_Pathfinder msg: 1613177606.92 epoch sec; seqNo:8014 2021-02-13T00:53:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.02 , -0.09 2021-02-13T00:53:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177606.94 epoch sec; seqNo:16658; 4060303.77 north; 593311.86 east 2021-02-13T00:53:27Z- AHRS_M2 msg: 1613177607.31 epoch sec; seqNo:16528 2021-02-13T00:53:27Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.13 psi 2021-02-13T00:53:27Z- handleDepth()-Depth_Keller msg: 1613177607317 epoch sec; seqNo:16659; depth 28.17 2021-02-13T00:53:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177607.33 epoch sec; seqNo:16659; 4060304.15 north; 593311.85 east 2021-02-13T00:53:27Z- AHRS_M2 msg: 1613177607.71 epoch sec; seqNo:16529 2021-02-13T00:53:27Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.14 psi 2021-02-13T00:53:27Z- handleDepth()-Depth_Keller msg: 1613177607719 epoch sec; seqNo:16660; depth 28.06 2021-02-13T00:53:27Z- handleDvl() - RDI_Pathfinder msg: 1613177607.73 epoch sec; seqNo:8015 2021-02-13T00:53:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.66 , 0.00 , 97.98 , 60.08 2021-02-13T00:53:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.02 , -0.10 2021-02-13T00:53:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177607.74 epoch sec; seqNo:16660; 4060304.15 north; 593311.85 east 2021-02-13T00:53:28Z- AHRS_M2 msg: 1613177608.12 epoch sec; seqNo:16530 2021-02-13T00:53:28Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.14 psi 2021-02-13T00:53:28Z- handleDepth()-Depth_Keller msg: 1613177608125 epoch sec; seqNo:16661; depth 27.95 2021-02-13T00:53:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:28Z- handleDvl() - RDI_Pathfinder msg: 1613177608.13 epoch sec; seqNo:8016 2021-02-13T00:53:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.02 , -0.10 2021-02-13T00:53:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177608.16 epoch sec; seqNo:16661; 4060304.54 north; 593313.07 east 2021-02-13T00:53:28Z- AHRS_M2 msg: 1613177608.52 epoch sec; seqNo:16531 2021-02-13T00:53:28Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.14 psi 2021-02-13T00:53:28Z- handleDepth()-Depth_Keller msg: 1613177608524 epoch sec; seqNo:16662; depth 27.82 2021-02-13T00:53:28Z- handleDvl() - RDI_Pathfinder msg: 1613177608.54 epoch sec; seqNo:8017 2021-02-13T00:53:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.25 , 82.84 , 97.98 , 61.30 2021-02-13T00:53:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.13 , -0.06 2021-02-13T00:53:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177608.56 epoch sec; seqNo:16662; 4060304.92 north; 593313.06 east 2021-02-13T00:53:28Z- AHRS_M2 msg: 1613177608.92 epoch sec; seqNo:16532 2021-02-13T00:53:28Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.14 psi 2021-02-13T00:53:28Z- handleDepth()-Depth_Keller msg: 1613177608928 epoch sec; seqNo:16663; depth 27.72 2021-02-13T00:53:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177608.94 epoch sec; seqNo:16663; 4060304.92 north; 593313.06 east 2021-02-13T00:53:29Z- AHRS_M2 msg: 1613177609.33 epoch sec; seqNo:16533 2021-02-13T00:53:29Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.14 psi 2021-02-13T00:53:29Z- LcmTrn::handleMessages() - handling 45 messages took 0.68 ms and 132 calls... 2021-02-13T00:53:29Z- waiting for 1613177607.09, time is 1613177609.33 2021-02-13T00:53:29Z- LcmTrn::updateTrn() >>>> pose time 1613177609.33, meas time 1613177608.54 <<<< 2021-02-13T00:53:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177608.54 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:53:29Z- TerrainNav::motionUpdate() - currEstimate = (4060305, 593313.06, 27.72) 2021-02-13T00:53:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:53:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:53:29Z- TNavBF::Projecting Measurements 2021-02-13T00:53:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:53:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:53:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:53:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:53:29Z- TNavFilter::4 good beams. 2021-02-13T00:53:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:53:29Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:53:29Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:53:29Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:53:29Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:53:29Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:53:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:53:29Z- TNavBF::mmse selected from filter -1 2021-02-13T00:53:29Z- TerrainMapOctree: (vehN, vehE ) = (4060304.45, 593312.70). 2021-02-13T00:53:29Z- TerrainMapOctree: Min Distance = 1331.39. 2021-02-13T00:53:29Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:53:29Z- There is no specific good beam that all particles have in common. 2021-02-13T00:53:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:53:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-13T00:53:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:53:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 10.98 2021-02-13T00:53:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:53:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:53:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:53:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 6.01 2021-02-13T00:53:29Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:53:29Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:53:29Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:53:29Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:53:29Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:53:29Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:53:29Z- Filter 0 No beams used in subcloud update 2021-02-13T00:53:29Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:53:29Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:53:29Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 683.7 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:53:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177608.54 sec. 2021-02-13T00:53:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:53:29Z- TNavBF::mmse selected from filter -1 2021-02-13T00:53:29Z- variances([0], [2], and psi_berg) = 683.729210 832.990093 0.000000 2021-02-13T00:53:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:53:29Z- LcmTrn::publishEstimates() - MLE : 4060304.42 593334.56 27.72 2021-02-13T00:53:29Z- LcmTrn::publishEstimates() - MMSE : 4060304.92 593313.06 27.72 2021-02-13T00:53:29Z- LcmTrn::publishEstimates() - COVAR: 683.73 832.99 0.84 0.00 2021-02-13T00:53:29Z- handleDepth()-Depth_Keller msg: 1613177609333 epoch sec; seqNo:16664; depth 27.59 2021-02-13T00:53:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:29Z- handleDvl() - RDI_Pathfinder msg: 1613177609.34 epoch sec; seqNo:8018 2021-02-13T00:53:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.13 , -0.06 2021-02-13T00:53:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177609.34 epoch sec; seqNo:16664; 4060305.30 north; 593313.06 east 2021-02-13T00:53:29Z- AHRS_M2 msg: 1613177609.73 epoch sec; seqNo:16534 2021-02-13T00:53:29Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.14 psi 2021-02-13T00:53:29Z- handleDepth()-Depth_Keller msg: 1613177609737 epoch sec; seqNo:16665; depth 27.46 2021-02-13T00:53:29Z- handleDvl() - RDI_Pathfinder msg: 1613177609.76 epoch sec; seqNo:8019 2021-02-13T00:53:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 82.84 , 97.98 , 61.71 2021-02-13T00:53:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.20 , -0.00 2021-02-13T00:53:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177609.77 epoch sec; seqNo:16665; 4060305.68 north; 593313.06 east 2021-02-13T00:53:30Z- AHRS_M2 msg: 1613177610.14 epoch sec; seqNo:16535 2021-02-13T00:53:30Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.14 psi 2021-02-13T00:53:30Z- handleDepth()-Depth_Keller msg: 1613177610147 epoch sec; seqNo:16666; depth 27.35 2021-02-13T00:53:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177610.16 epoch sec; seqNo:16666; 4060306.05 north; 593313.05 east 2021-02-13T00:53:30Z- AHRS_M2 msg: 1613177610.57 epoch sec; seqNo:16536 2021-02-13T00:53:30Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.14 psi 2021-02-13T00:53:30Z- handleDepth()-Depth_Keller msg: 1613177610581 epoch sec; seqNo:16667; depth 27.25 2021-02-13T00:53:30Z- handleDvl() - RDI_Pathfinder msg: 1613177610.60 epoch sec; seqNo:8020 2021-02-13T00:53:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 82.77 , 94.67 , 61.30 2021-02-13T00:53:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.16 , 0.08 2021-02-13T00:53:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177610.61 epoch sec; seqNo:16667; 4060306.43 north; 593313.05 east 2021-02-13T00:53:30Z- AHRS_M2 msg: 1613177610.94 epoch sec; seqNo:16537 2021-02-13T00:53:30Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.14 psi 2021-02-13T00:53:30Z- handleDepth()-Depth_Keller msg: 1613177610953 epoch sec; seqNo:16668; depth 27.11 2021-02-13T00:53:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177610.97 epoch sec; seqNo:16668; 4060306.81 north; 593313.04 east 2021-02-13T00:53:31Z- AHRS_M2 msg: 1613177611.35 epoch sec; seqNo:16538 2021-02-13T00:53:31Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.13 psi 2021-02-13T00:53:31Z- handleDepth()-Depth_Keller msg: 1613177611352 epoch sec; seqNo:16669; depth 27.01 2021-02-13T00:53:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177611.36 epoch sec; seqNo:16669; 4060306.81 north; 593313.04 east 2021-02-13T00:53:31Z- AHRS_M2 msg: 1613177611.75 epoch sec; seqNo:16539 2021-02-13T00:53:31Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.12 psi 2021-02-13T00:53:31Z- handleDepth()-Depth_Keller msg: 1613177611756 epoch sec; seqNo:16670; depth 26.90 2021-02-13T00:53:31Z- handleDvl() - RDI_Pathfinder msg: 1613177611.77 epoch sec; seqNo:8021 2021-02-13T00:53:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 82.84 , 97.98 , 61.71 2021-02-13T00:53:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.16 , 0.09 2021-02-13T00:53:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177611.78 epoch sec; seqNo:16670; 4060307.20 north; 593314.26 east 2021-02-13T00:53:32Z- AHRS_M2 msg: 1613177612.17 epoch sec; seqNo:16540 2021-02-13T00:53:32Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.11 psi 2021-02-13T00:53:32Z- handleDepth()-Depth_Keller msg: 1613177612173 epoch sec; seqNo:16671; depth 26.78 2021-02-13T00:53:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177612.18 epoch sec; seqNo:16671; 4060307.58 north; 593314.26 east 2021-02-13T00:53:32Z- AHRS_M2 msg: 1613177612.56 epoch sec; seqNo:16541 2021-02-13T00:53:32Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.10 psi 2021-02-13T00:53:32Z- handleDepth()-Depth_Keller msg: 1613177612567 epoch sec; seqNo:16672; depth 26.66 2021-02-13T00:53:32Z- handleDvl() - RDI_Pathfinder msg: 1613177612.58 epoch sec; seqNo:8022 2021-02-13T00:53:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.52 , 80.67 , 97.98 , 62.53 2021-02-13T00:53:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.05 , 0.10 2021-02-13T00:53:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177612.60 epoch sec; seqNo:16672; 4060307.96 north; 593314.25 east 2021-02-13T00:53:32Z- AHRS_M2 msg: 1613177612.96 epoch sec; seqNo:16542 2021-02-13T00:53:32Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.10 psi 2021-02-13T00:53:32Z- handleDepth()-Depth_Keller msg: 1613177612968 epoch sec; seqNo:16673; depth 26.55 2021-02-13T00:53:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:32Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:32Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:32Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:32Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:32Z- handleDvl() - RDI_Pathfinder msg: 1613177612.97 epoch sec; seqNo:8023 2021-02-13T00:53:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.05 , 0.10 2021-02-13T00:53:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177612.98 epoch sec; seqNo:16673; 4060308.34 north; 593314.25 east 2021-02-13T00:53:33Z- AHRS_M2 msg: 1613177613.37 epoch sec; seqNo:16543 2021-02-13T00:53:33Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.09 psi 2021-02-13T00:53:33Z- handleDepth()-Depth_Keller msg: 1613177613377 epoch sec; seqNo:16674; depth 26.45 2021-02-13T00:53:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177613.39 epoch sec; seqNo:16674; 4060308.72 north; 593314.24 east 2021-02-13T00:53:33Z- AHRS_M2 msg: 1613177613.77 epoch sec; seqNo:16544 2021-02-13T00:53:33Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.09 psi 2021-02-13T00:53:33Z- handleDepth()-Depth_Keller msg: 1613177613776 epoch sec; seqNo:16675; depth 26.32 2021-02-13T00:53:33Z- handleDvl() - RDI_Pathfinder msg: 1613177613.79 epoch sec; seqNo:8024 2021-02-13T00:53:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.93 , 81.75 , 97.98 , 62.12 2021-02-13T00:53:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.02 , 0.07 2021-02-13T00:53:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177613.80 epoch sec; seqNo:16675; 4060308.72 north; 593314.24 east 2021-02-13T00:53:34Z- AHRS_M2 msg: 1613177614.18 epoch sec; seqNo:16545 2021-02-13T00:53:34Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.09 psi 2021-02-13T00:53:34Z- handleDepth()-Depth_Keller msg: 1613177614182 epoch sec; seqNo:16676; depth 26.21 2021-02-13T00:53:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:34Z- handleDvl() - RDI_Pathfinder msg: 1613177614.18 epoch sec; seqNo:8025 2021-02-13T00:53:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.02 , 0.07 2021-02-13T00:53:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177614.20 epoch sec; seqNo:16676; 4060309.11 north; 593315.46 east 2021-02-13T00:53:34Z- LcmTrn::handleMessages() - handling 46 messages took 0.63 ms and 134 calls... 2021-02-13T00:53:34Z- waiting for 1613177612.33, time is 1613177614.20 2021-02-13T00:53:34Z- LcmTrn::updateTrn() equating pose 1613177614.20 and meas times 1613177614.20 2021-02-13T00:53:34Z- LcmTrn::updateTrn() >>>> pose time 1613177614.20, meas time 1613177614.20 <<<< 2021-02-13T00:53:34Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177614.20, ping # 0. 2021-02-13T00:53:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177614.20 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:53:34Z- TerrainNav::motionUpdate() - currEstimate = (4060309, 593315.46, 26.21) 2021-02-13T00:53:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:53:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:53:34Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:53:34Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:53:34Z- TNavBF::Projecting Measurements 2021-02-13T00:53:34Z- TNavFilter::Measurement from time = 1613177614.20 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:53:34Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:53:34Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:53:34Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:53:34Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:53:34Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:53:34Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:53:34Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:53:34Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 683.7 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:53:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:53:34Z- TNavBF::mmse selected from filter -1 2021-02-13T00:53:34Z- variances([0], [2], and psi_berg) = 683.679085 832.990071 0.000000 2021-02-13T00:53:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:53:34Z- LcmTrn::publishEstimates() - MLE : 4060308.61 593336.96 26.21 2021-02-13T00:53:34Z- LcmTrn::publishEstimates() - MMSE : 4060309.11 593315.46 26.21 2021-02-13T00:53:34Z- LcmTrn::publishEstimates() - COVAR: 683.68 832.99 1.08 0.00 2021-02-13T00:53:34Z- AHRS_M2 msg: 1613177614.58 epoch sec; seqNo:16546 2021-02-13T00:53:34Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.09 psi 2021-02-13T00:53:34Z- handleDepth()-Depth_Keller msg: 1613177614588 epoch sec; seqNo:16677; depth 26.10 2021-02-13T00:53:34Z- handleDvl() - RDI_Pathfinder msg: 1613177614.60 epoch sec; seqNo:8026 2021-02-13T00:53:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 81.75 , 97.98 , 62.12 2021-02-13T00:53:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , 0.01 2021-02-13T00:53:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177614.61 epoch sec; seqNo:16677; 4060309.49 north; 593315.46 east 2021-02-13T00:53:34Z- AHRS_M2 msg: 1613177614.98 epoch sec; seqNo:16547 2021-02-13T00:53:34Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.10 psi 2021-02-13T00:53:34Z- handleDepth()-Depth_Keller msg: 1613177614990 epoch sec; seqNo:16678; depth 25.97 2021-02-13T00:53:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177615.00 epoch sec; seqNo:16678; 4060309.87 north; 593315.45 east 2021-02-13T00:53:35Z- AHRS_M2 msg: 1613177615.39 epoch sec; seqNo:16548 2021-02-13T00:53:35Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.11 psi 2021-02-13T00:53:35Z- handleDepth()-Depth_Keller msg: 1613177615393 epoch sec; seqNo:16679; depth 25.84 2021-02-13T00:53:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:35Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:35Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:35Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:35Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:35Z- handleDvl() - RDI_Pathfinder msg: 1613177615.39 epoch sec; seqNo:8027 2021-02-13T00:53:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , 0.01 2021-02-13T00:53:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177615.40 epoch sec; seqNo:16679; 4060309.87 north; 593315.45 east 2021-02-13T00:53:35Z- AHRS_M2 msg: 1613177615.79 epoch sec; seqNo:16549 2021-02-13T00:53:35Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.11 psi 2021-02-13T00:53:35Z- handleDepth()-Depth_Keller msg: 1613177615803 epoch sec; seqNo:16680; depth 25.73 2021-02-13T00:53:35Z- handleDvl() - RDI_Pathfinder msg: 1613177615.82 epoch sec; seqNo:8028 2021-02-13T00:53:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 82.77 , 97.91 , 64.16 2021-02-13T00:53:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , -0.03 2021-02-13T00:53:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177615.83 epoch sec; seqNo:16680; 4060310.25 north; 593315.45 east 2021-02-13T00:53:36Z- AHRS_M2 msg: 1613177616.19 epoch sec; seqNo:16550 2021-02-13T00:53:36Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.12 psi 2021-02-13T00:53:36Z- handleDepth()-Depth_Keller msg: 1613177616207 epoch sec; seqNo:16681; depth 25.61 2021-02-13T00:53:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177616.22 epoch sec; seqNo:16681; 4060310.63 north; 593315.44 east 2021-02-13T00:53:36Z- AHRS_M2 msg: 1613177616.60 epoch sec; seqNo:16551 2021-02-13T00:53:36Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.12 psi 2021-02-13T00:53:36Z- handleDepth()-Depth_Keller msg: 1613177616604 epoch sec; seqNo:16682; depth 25.49 2021-02-13T00:53:36Z- handleDvl() - RDI_Pathfinder msg: 1613177616.62 epoch sec; seqNo:8029 2021-02-13T00:53:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 84.86 , 97.85 , 63.35 2021-02-13T00:53:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.07 , -0.04 2021-02-13T00:53:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177616.63 epoch sec; seqNo:16682; 4060311.02 north; 593316.66 east 2021-02-13T00:53:37Z- AHRS_M2 msg: 1613177617.00 epoch sec; seqNo:16552 2021-02-13T00:53:37Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.13 psi 2021-02-13T00:53:37Z- handleDepth()-Depth_Keller msg: 1613177617013 epoch sec; seqNo:16683; depth 25.36 2021-02-13T00:53:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177617.05 epoch sec; seqNo:16683; 4060311.02 north; 593316.66 east 2021-02-13T00:53:37Z- AHRS_M2 msg: 1613177617.41 epoch sec; seqNo:16553 2021-02-13T00:53:37Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.13 psi 2021-02-13T00:53:37Z- handleDepth()-Depth_Keller msg: 1613177617412 epoch sec; seqNo:16684; depth 25.26 2021-02-13T00:53:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177617.42 epoch sec; seqNo:16684; 4060311.40 north; 593316.66 east 2021-02-13T00:53:37Z- AHRS_M2 msg: 1613177617.81 epoch sec; seqNo:16554 2021-02-13T00:53:37Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.14 psi 2021-02-13T00:53:37Z- handleDepth()-Depth_Keller msg: 1613177617818 epoch sec; seqNo:16685; depth 25.13 2021-02-13T00:53:37Z- handleDvl() - RDI_Pathfinder msg: 1613177617.83 epoch sec; seqNo:8030 2021-02-13T00:53:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 85.00 , 100.14 , 64.16 2021-02-13T00:53:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , -0.07 2021-02-13T00:53:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177617.84 epoch sec; seqNo:16685; 4060311.78 north; 593316.65 east 2021-02-13T00:53:38Z- AHRS_M2 msg: 1613177618.21 epoch sec; seqNo:16555 2021-02-13T00:53:38Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.14 psi 2021-02-13T00:53:38Z- handleDepth()-Depth_Keller msg: 1613177618221 epoch sec; seqNo:16686; depth 25.00 2021-02-13T00:53:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177618.23 epoch sec; seqNo:16686; 4060311.78 north; 593316.65 east 2021-02-13T00:53:38Z- handleDepth()-Depth_Keller msg: 1613177618641 epoch sec; seqNo:16687; depth 24.87 2021-02-13T00:53:38Z- handleDvl() - RDI_Pathfinder msg: 1613177618.65 epoch sec; seqNo:8031 2021-02-13T00:53:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:38Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:53:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177618.67 epoch sec; seqNo:16687; 4060312.16 north; 593316.65 east 2021-02-13T00:53:39Z- AHRS_M2 msg: 1613177619.03 epoch sec; seqNo:16556 2021-02-13T00:53:39Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.15 psi 2021-02-13T00:53:39Z- handleDepth()-Depth_Keller msg: 1613177619031 epoch sec; seqNo:16688; depth 24.74 2021-02-13T00:53:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:39Z- handleDvl() - RDI_Pathfinder msg: 1613177619.03 epoch sec; seqNo:8032 2021-02-13T00:53:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:39Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:53:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177619.04 epoch sec; seqNo:16688; 4060312.17 north; 593317.87 east 2021-02-13T00:53:39Z- AHRS_M2 msg: 1613177619.43 epoch sec; seqNo:16557 2021-02-13T00:53:39Z- AHRS_M2 msg: 0.66 phi; 0.36 theta; -0.16 psi 2021-02-13T00:53:39Z- handleDepth()-Depth_Keller msg: 1613177619442 epoch sec; seqNo:16689; depth 24.64 2021-02-13T00:53:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177619.46 epoch sec; seqNo:16689; 4060312.55 north; 593317.86 east 2021-02-13T00:53:39Z- LcmTrn::handleMessages() - handling 45 messages took 0.64 ms and 132 calls... 2021-02-13T00:53:39Z- waiting for 1613177617.20, time is 1613177619.46 2021-02-13T00:53:39Z- LcmTrn::updateTrn() >>>> pose time 1613177619.46, meas time 1613177619.03 <<<< 2021-02-13T00:53:39Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177619.03, ping # 0. 2021-02-13T00:53:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177619.03 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:53:39Z- TerrainNav::motionUpdate() - currEstimate = (4060313, 593317.86, 24.64) 2021-02-13T00:53:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:53:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:53:39Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:53:39Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:53:39Z- TNavBF::Projecting Measurements 2021-02-13T00:53:39Z- TNavFilter::Measurement from time = 1613177619.03 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:53:39Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:53:39Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:53:39Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:53:39Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:53:39Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:53:39Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:53:39Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:53:39Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 683.6 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:53:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:53:39Z- TNavBF::mmse selected from filter -1 2021-02-13T00:53:39Z- variances([0], [2], and psi_berg) = 683.621067 832.990045 0.000000 2021-02-13T00:53:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:53:39Z- LcmTrn::publishEstimates() - MLE : 4060312.05 593338.36 24.64 2021-02-13T00:53:39Z- LcmTrn::publishEstimates() - MMSE : 4060312.55 593317.86 24.64 2021-02-13T00:53:39Z- LcmTrn::publishEstimates() - COVAR: 683.62 832.99 1.29 0.00 2021-02-13T00:53:39Z- AHRS_M2 msg: 1613177619.83 epoch sec; seqNo:16558 2021-02-13T00:53:39Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.16 psi 2021-02-13T00:53:39Z- handleDepth()-Depth_Keller msg: 1613177619836 epoch sec; seqNo:16690; depth 24.50 2021-02-13T00:53:39Z- handleDvl() - RDI_Pathfinder msg: 1613177619.85 epoch sec; seqNo:8033 2021-02-13T00:53:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 0.00 , 98.52 , 63.76 2021-02-13T00:53:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.00 , -0.09 2021-02-13T00:53:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177619.88 epoch sec; seqNo:16690; 4060312.93 north; 593317.86 east 2021-02-13T00:53:40Z- AHRS_M2 msg: 1613177620.24 epoch sec; seqNo:16559 2021-02-13T00:53:40Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.17 psi 2021-02-13T00:53:40Z- handleDepth()-Depth_Keller msg: 1613177620245 epoch sec; seqNo:16691; depth 24.37 2021-02-13T00:53:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:40Z- handleDvl() - RDI_Pathfinder msg: 1613177620.25 epoch sec; seqNo:8034 2021-02-13T00:53:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.00 , -0.09 2021-02-13T00:53:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177620.26 epoch sec; seqNo:16691; 4060312.93 north; 593317.86 east 2021-02-13T00:53:40Z- AHRS_M2 msg: 1613177620.64 epoch sec; seqNo:16560 2021-02-13T00:53:40Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.18 psi 2021-02-13T00:53:40Z- handleDepth()-Depth_Keller msg: 1613177620646 epoch sec; seqNo:16692; depth 24.25 2021-02-13T00:53:40Z- handleDvl() - RDI_Pathfinder msg: 1613177620.66 epoch sec; seqNo:8035 2021-02-13T00:53:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.52 , 0.00 , 100.14 , 65.39 2021-02-13T00:53:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.09 , -0.05 2021-02-13T00:53:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177620.67 epoch sec; seqNo:16692; 4060313.31 north; 593317.85 east 2021-02-13T00:53:41Z- AHRS_M2 msg: 1613177621.04 epoch sec; seqNo:16561 2021-02-13T00:53:41Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.18 psi 2021-02-13T00:53:41Z- handleDepth()-Depth_Keller msg: 1613177621048 epoch sec; seqNo:16693; depth 24.10 2021-02-13T00:53:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177621.07 epoch sec; seqNo:16693; 4060313.68 north; 593317.85 east 2021-02-13T00:53:41Z- AHRS_M2 msg: 1613177621.45 epoch sec; seqNo:16562 2021-02-13T00:53:41Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.18 psi 2021-02-13T00:53:41Z- handleDepth()-Depth_Keller msg: 1613177621452 epoch sec; seqNo:16694; depth 23.99 2021-02-13T00:53:41Z- handleDvl() - RDI_Pathfinder msg: 1613177621.46 epoch sec; seqNo:8036 2021-02-13T00:53:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.93 , 0.00 , 100.14 , 64.98 2021-02-13T00:53:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.13 , -0.02 2021-02-13T00:53:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177621.49 epoch sec; seqNo:16694; 4060314.06 north; 593317.85 east 2021-02-13T00:53:41Z- AHRS_M2 msg: 1613177621.85 epoch sec; seqNo:16563 2021-02-13T00:53:41Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.19 psi 2021-02-13T00:53:41Z- handleDepth()-Depth_Keller msg: 1613177621858 epoch sec; seqNo:16695; depth 23.86 2021-02-13T00:53:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177621.87 epoch sec; seqNo:16695; 4060314.08 north; 593319.07 east 2021-02-13T00:53:42Z- AHRS_M2 msg: 1613177622.26 epoch sec; seqNo:16564 2021-02-13T00:53:42Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.19 psi 2021-02-13T00:53:42Z- handleDepth()-Depth_Keller msg: 1613177622260 epoch sec; seqNo:16696; depth 23.74 2021-02-13T00:53:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177622.28 epoch sec; seqNo:16696; 4060314.46 north; 593319.06 east 2021-02-13T00:53:42Z- AHRS_M2 msg: 1613177622.66 epoch sec; seqNo:16565 2021-02-13T00:53:42Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.19 psi 2021-02-13T00:53:42Z- handleDepth()-Depth_Keller msg: 1613177622667 epoch sec; seqNo:16697; depth 23.61 2021-02-13T00:53:42Z- handleDvl() - RDI_Pathfinder msg: 1613177622.68 epoch sec; seqNo:8037 2021-02-13T00:53:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.93 , 0.00 , 102.31 , 64.98 2021-02-13T00:53:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.12 , 0.04 2021-02-13T00:53:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177622.70 epoch sec; seqNo:16697; 4060314.83 north; 593319.06 east 2021-02-13T00:53:43Z- AHRS_M2 msg: 1613177623.06 epoch sec; seqNo:16566 2021-02-13T00:53:43Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.18 psi 2021-02-13T00:53:43Z- handleDepth()-Depth_Keller msg: 1613177623068 epoch sec; seqNo:16698; depth 23.47 2021-02-13T00:53:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177623.08 epoch sec; seqNo:16698; 4060315.21 north; 593319.05 east 2021-02-13T00:53:43Z- AHRS_M2 msg: 1613177623.47 epoch sec; seqNo:16567 2021-02-13T00:53:43Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.18 psi 2021-02-13T00:53:43Z- handleDepth()-Depth_Keller msg: 1613177623474 epoch sec; seqNo:16699; depth 23.35 2021-02-13T00:53:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177623.49 epoch sec; seqNo:16699; 4060315.21 north; 593319.05 east 2021-02-13T00:53:43Z- AHRS_M2 msg: 1613177623.87 epoch sec; seqNo:16568 2021-02-13T00:53:43Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.17 psi 2021-02-13T00:53:43Z- handleDepth()-Depth_Keller msg: 1613177623876 epoch sec; seqNo:16700; depth 23.23 2021-02-13T00:53:43Z- handleDvl() - RDI_Pathfinder msg: 1613177623.89 epoch sec; seqNo:8038 2021-02-13T00:53:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 89.33 , 103.39 , 65.39 2021-02-13T00:53:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , 0.06 2021-02-13T00:53:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177623.90 epoch sec; seqNo:16700; 4060315.59 north; 593319.05 east 2021-02-13T00:53:44Z- AHRS_M2 msg: 1613177624.28 epoch sec; seqNo:16569 2021-02-13T00:53:44Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.17 psi 2021-02-13T00:53:44Z- handleDepth()-Depth_Keller msg: 1613177624279 epoch sec; seqNo:16701; depth 23.11 2021-02-13T00:53:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177624.29 epoch sec; seqNo:16701; 4060315.97 north; 593319.05 east 2021-02-13T00:53:44Z- LcmTrn::handleMessages() - handling 42 messages took 0.61 ms and 130 calls... 2021-02-13T00:53:44Z- waiting for 1613177622.46, time is 1613177624.29 2021-02-13T00:53:44Z- LcmTrn::updateTrn() >>>> pose time 1613177624.29, meas time 1613177623.89 <<<< 2021-02-13T00:53:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177623.89 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:53:44Z- TerrainNav::motionUpdate() - currEstimate = (4060316, 593319.05, 23.11) 2021-02-13T00:53:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:53:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:53:44Z- TNavBF::Projecting Measurements 2021-02-13T00:53:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:53:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:53:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:53:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:53:44Z- TNavFilter::4 good beams. 2021-02-13T00:53:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:53:44Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:53:44Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:53:44Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:53:44Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:53:44Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:53:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:53:44Z- TNavBF::mmse selected from filter -1 2021-02-13T00:53:44Z- TerrainMapOctree: (vehN, vehE ) = (4060315.69, 593318.95). 2021-02-13T00:53:44Z- TerrainMapOctree: Min Distance = 1319.01. 2021-02-13T00:53:44Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:53:44Z- There is no specific good beam that all particles have in common. 2021-02-13T00:53:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:53:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.46 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:53:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:53:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 12.77 2021-02-13T00:53:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:53:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 17.10 2021-02-13T00:53:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:53:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.116 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-13T00:53:44Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:53:44Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:53:44Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:53:44Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:53:44Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:53:44Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:53:44Z- Filter 0 No beams used in subcloud update 2021-02-13T00:53:44Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:53:44Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:53:44Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 683.6 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:53:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177623.89 sec. 2021-02-13T00:53:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:53:44Z- TNavBF::mmse selected from filter -1 2021-02-13T00:53:44Z- variances([0], [2], and psi_berg) = 683.567120 832.990022 0.000000 2021-02-13T00:53:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:53:44Z- LcmTrn::publishEstimates() - MLE : 4060315.47 593339.55 23.11 2021-02-13T00:53:44Z- LcmTrn::publishEstimates() - MMSE : 4060315.97 593319.05 23.11 2021-02-13T00:53:44Z- LcmTrn::publishEstimates() - COVAR: 683.57 832.99 1.01 0.00 2021-02-13T00:53:44Z- AHRS_M2 msg: 1613177624.68 epoch sec; seqNo:16570 2021-02-13T00:53:44Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.16 psi 2021-02-13T00:53:44Z- handleDepth()-Depth_Keller msg: 1613177624684 epoch sec; seqNo:16702; depth 22.98 2021-02-13T00:53:44Z- handleDvl() - RDI_Pathfinder msg: 1613177624.69 epoch sec; seqNo:8039 2021-02-13T00:53:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.78 , 88.52 , 97.03 , 0.00 2021-02-13T00:53:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , 0.09 2021-02-13T00:53:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177624.71 epoch sec; seqNo:16702; 4060316.35 north; 593319.04 east 2021-02-13T00:53:45Z- AHRS_M2 msg: 1613177625.09 epoch sec; seqNo:16571 2021-02-13T00:53:45Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.16 psi 2021-02-13T00:53:45Z- handleDepth()-Depth_Keller msg: 1613177625090 epoch sec; seqNo:16703; depth 22.84 2021-02-13T00:53:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:45Z- handleDvl() - RDI_Pathfinder msg: 1613177625.09 epoch sec; seqNo:8040 2021-02-13T00:53:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , 0.09 2021-02-13T00:53:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177625.10 epoch sec; seqNo:16703; 4060316.36 north; 593320.26 east 2021-02-13T00:53:45Z- AHRS_M2 msg: 1613177625.49 epoch sec; seqNo:16572 2021-02-13T00:53:45Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.15 psi 2021-02-13T00:53:45Z- handleDepth()-Depth_Keller msg: 1613177625493 epoch sec; seqNo:16704; depth 22.72 2021-02-13T00:53:45Z- handleDvl() - RDI_Pathfinder msg: 1613177625.51 epoch sec; seqNo:8041 2021-02-13T00:53:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.97 , 0.00 , 102.78 , 65.80 2021-02-13T00:53:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , 0.10 2021-02-13T00:53:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177625.53 epoch sec; seqNo:16704; 4060316.74 north; 593320.26 east 2021-02-13T00:53:45Z- AHRS_M2 msg: 1613177625.89 epoch sec; seqNo:16573 2021-02-13T00:53:45Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.15 psi 2021-02-13T00:53:45Z- handleDepth()-Depth_Keller msg: 1613177625896 epoch sec; seqNo:16705; depth 22.59 2021-02-13T00:53:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177625.91 epoch sec; seqNo:16705; 4060317.12 north; 593320.25 east 2021-02-13T00:53:46Z- AHRS_M2 msg: 1613177626.30 epoch sec; seqNo:16574 2021-02-13T00:53:46Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.15 psi 2021-02-13T00:53:46Z- handleDepth()-Depth_Keller msg: 1613177626301 epoch sec; seqNo:16706; depth 22.48 2021-02-13T00:53:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:46Z- handleDvl() - RDI_Pathfinder msg: 1613177626.30 epoch sec; seqNo:8042 2021-02-13T00:53:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , 0.10 2021-02-13T00:53:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177626.31 epoch sec; seqNo:16706; 4060317.50 north; 593320.25 east 2021-02-13T00:53:46Z- AHRS_M2 msg: 1613177626.70 epoch sec; seqNo:16575 2021-02-13T00:53:46Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.15 psi 2021-02-13T00:53:46Z- handleDepth()-Depth_Keller msg: 1613177626704 epoch sec; seqNo:16707; depth 22.34 2021-02-13T00:53:46Z- handleDvl() - RDI_Pathfinder msg: 1613177626.71 epoch sec; seqNo:8043 2021-02-13T00:53:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 0.00 , 105.42 , 66.62 2021-02-13T00:53:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , 0.09 2021-02-13T00:53:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177626.73 epoch sec; seqNo:16707; 4060317.50 north; 593320.25 east 2021-02-13T00:53:47Z- AHRS_M2 msg: 1613177627.10 epoch sec; seqNo:16576 2021-02-13T00:53:47Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.15 psi 2021-02-13T00:53:47Z- handleDepth()-Depth_Keller msg: 1613177627108 epoch sec; seqNo:16708; depth 22.22 2021-02-13T00:53:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177627.12 epoch sec; seqNo:16708; 4060317.88 north; 593320.25 east 2021-02-13T00:53:47Z- AHRS_M2 msg: 1613177627.51 epoch sec; seqNo:16577 2021-02-13T00:53:47Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.15 psi 2021-02-13T00:53:47Z- handleDepth()-Depth_Keller msg: 1613177627520 epoch sec; seqNo:16709; depth 22.10 2021-02-13T00:53:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:47Z- handleDvl() - RDI_Pathfinder msg: 1613177627.53 epoch sec; seqNo:8044 2021-02-13T00:53:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , 0.09 2021-02-13T00:53:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177627.55 epoch sec; seqNo:16709; 4060318.27 north; 593321.46 east 2021-02-13T00:53:47Z- AHRS_M2 msg: 1613177627.91 epoch sec; seqNo:16578 2021-02-13T00:53:47Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.16 psi 2021-02-13T00:53:47Z- handleDepth()-Depth_Keller msg: 1613177627925 epoch sec; seqNo:16710; depth 21.97 2021-02-13T00:53:47Z- handleDvl() - RDI_Pathfinder msg: 1613177627.94 epoch sec; seqNo:8045 2021-02-13T00:53:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 0.00 , 103.25 , 78.92 2021-02-13T00:53:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.44 , 0.54 , -0.23 2021-02-13T00:53:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177627.95 epoch sec; seqNo:16710; 4060318.27 north; 593321.46 east 2021-02-13T00:53:48Z- AHRS_M2 msg: 1613177628.33 epoch sec; seqNo:16579 2021-02-13T00:53:48Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.17 psi 2021-02-13T00:53:48Z- handleDepth()-Depth_Keller msg: 1613177628331 epoch sec; seqNo:16711; depth 21.83 2021-02-13T00:53:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177628.35 epoch sec; seqNo:16711; 4060318.27 north; 593321.46 east 2021-02-13T00:53:48Z- AHRS_M2 msg: 1613177628.72 epoch sec; seqNo:16580 2021-02-13T00:53:48Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.18 psi 2021-02-13T00:53:48Z- handleDepth()-Depth_Keller msg: 1613177628724 epoch sec; seqNo:16712; depth 21.70 2021-02-13T00:53:48Z- handleDvl() - RDI_Pathfinder msg: 1613177628.74 epoch sec; seqNo:8046 2021-02-13T00:53:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 98.05 , 105.36 , 67.03 2021-02-13T00:53:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.05 , 0.00 2021-02-13T00:53:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177628.77 epoch sec; seqNo:16712; 4060318.65 north; 593321.46 east 2021-02-13T00:53:49Z- AHRS_M2 msg: 1613177629.13 epoch sec; seqNo:16581 2021-02-13T00:53:49Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.19 psi 2021-02-13T00:53:49Z- handleDepth()-Depth_Keller msg: 1613177629138 epoch sec; seqNo:16713; depth 21.60 2021-02-13T00:53:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177629.15 epoch sec; seqNo:16713; 4060318.65 north; 593321.46 east 2021-02-13T00:53:49Z- AHRS_M2 msg: 1613177629.53 epoch sec; seqNo:16582 2021-02-13T00:53:49Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.19 psi 2021-02-13T00:53:49Z- handleDepth()-Depth_Keller msg: 1613177629536 epoch sec; seqNo:16714; depth 21.46 2021-02-13T00:53:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177629.56 epoch sec; seqNo:16714; 4060319.04 north; 593322.68 east 2021-02-13T00:53:49Z- LcmTrn::handleMessages() - handling 47 messages took 0.73 ms and 133 calls... 2021-02-13T00:53:49Z- waiting for 1613177627.29, time is 1613177629.56 2021-02-13T00:53:49Z- LcmTrn::updateTrn() >>>> pose time 1613177629.56, meas time 1613177628.74 <<<< 2021-02-13T00:53:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177628.74 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:53:49Z- TerrainNav::motionUpdate() - currEstimate = (4060319, 593322.68, 21.46) 2021-02-13T00:53:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:53:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:53:49Z- TNavBF::Projecting Measurements 2021-02-13T00:53:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:53:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:53:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:53:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:53:49Z- TNavFilter::4 good beams. 2021-02-13T00:53:49Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:53:49Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:53:49Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:53:49Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:53:49Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:53:49Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:53:49Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:53:49Z- TNavBF::mmse selected from filter -1 2021-02-13T00:53:49Z- TerrainMapOctree: (vehN, vehE ) = (4060318.57, 593322.11). 2021-02-13T00:53:49Z- TerrainMapOctree: Min Distance = 1314.74. 2021-02-13T00:53:49Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:53:49Z- There is no specific good beam that all particles have in common. 2021-02-13T00:53:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:53:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 8.95 2021-02-13T00:53:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:53:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.38 2021-02-13T00:53:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:53:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 17.76 2021-02-13T00:53:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:53:49Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 7.19 2021-02-13T00:53:49Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:53:49Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:53:49Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:53:49Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:53:49Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:53:49Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:53:49Z- Filter 0 No beams used in subcloud update 2021-02-13T00:53:49Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:53:49Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:53:49Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 683.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:53:49Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177628.74 sec. 2021-02-13T00:53:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:53:49Z- TNavBF::mmse selected from filter -1 2021-02-13T00:53:49Z- variances([0], [2], and psi_berg) = 683.501958 832.989993 0.000000 2021-02-13T00:53:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:53:49Z- LcmTrn::publishEstimates() - MLE : 4060318.54 593343.18 21.46 2021-02-13T00:53:49Z- LcmTrn::publishEstimates() - MMSE : 4060319.04 593322.68 21.46 2021-02-13T00:53:49Z- LcmTrn::publishEstimates() - COVAR: 683.50 832.99 0.91 0.00 2021-02-13T00:53:49Z- AHRS_M2 msg: 1613177629.93 epoch sec; seqNo:16583 2021-02-13T00:53:49Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.19 psi 2021-02-13T00:53:49Z- handleDepth()-Depth_Keller msg: 1613177629936 epoch sec; seqNo:16715; depth 21.35 2021-02-13T00:53:49Z- handleDvl() - RDI_Pathfinder msg: 1613177629.95 epoch sec; seqNo:8047 2021-02-13T00:53:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 102.92 , 67.85 2021-02-13T00:53:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.07 , -0.03 2021-02-13T00:53:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177629.97 epoch sec; seqNo:16715; 4060319.42 north; 593322.67 east 2021-02-13T00:53:50Z- AHRS_M2 msg: 1613177630.34 epoch sec; seqNo:16584 2021-02-13T00:53:50Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-13T00:53:50Z- handleDepth()-Depth_Keller msg: 1613177630345 epoch sec; seqNo:16716; depth 21.22 2021-02-13T00:53:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177630.36 epoch sec; seqNo:16716; 4060319.80 north; 593322.67 east 2021-02-13T00:53:50Z- AHRS_M2 msg: 1613177630.74 epoch sec; seqNo:16585 2021-02-13T00:53:50Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-13T00:53:50Z- handleDepth()-Depth_Keller msg: 1613177630746 epoch sec; seqNo:16717; depth 21.10 2021-02-13T00:53:50Z- handleDvl() - RDI_Pathfinder msg: 1613177630.76 epoch sec; seqNo:8048 2021-02-13T00:53:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 93.18 , 104.14 , 68.26 2021-02-13T00:53:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.06 2021-02-13T00:53:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177630.77 epoch sec; seqNo:16717; 4060319.80 north; 593322.67 east 2021-02-13T00:53:51Z- AHRS_M2 msg: 1613177631.14 epoch sec; seqNo:16586 2021-02-13T00:53:51Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-13T00:53:51Z- handleDepth()-Depth_Keller msg: 1613177631149 epoch sec; seqNo:16718; depth 20.99 2021-02-13T00:53:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:51Z- handleDvl() - RDI_Pathfinder msg: 1613177631.15 epoch sec; seqNo:8049 2021-02-13T00:53:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.06 2021-02-13T00:53:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177631.17 epoch sec; seqNo:16718; 4060320.18 north; 593322.66 east 2021-02-13T00:53:51Z- AHRS_M2 msg: 1613177631.55 epoch sec; seqNo:16587 2021-02-13T00:53:51Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.19 psi 2021-02-13T00:53:51Z- handleDepth()-Depth_Keller msg: 1613177631557 epoch sec; seqNo:16719; depth 20.86 2021-02-13T00:53:51Z- handleDvl() - RDI_Pathfinder msg: 1613177631.57 epoch sec; seqNo:8050 2021-02-13T00:53:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 98.19 , 103.06 , 67.03 2021-02-13T00:53:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.09 , -0.09 2021-02-13T00:53:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177631.59 epoch sec; seqNo:16719; 4060320.56 north; 593322.66 east 2021-02-13T00:53:51Z- AHRS_M2 msg: 1613177631.95 epoch sec; seqNo:16588 2021-02-13T00:53:51Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.19 psi 2021-02-13T00:53:51Z- handleDepth()-Depth_Keller msg: 1613177631956 epoch sec; seqNo:16720; depth 20.75 2021-02-13T00:53:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177631.97 epoch sec; seqNo:16720; 4060320.57 north; 593323.88 east 2021-02-13T00:53:52Z- AHRS_M2 msg: 1613177632.37 epoch sec; seqNo:16589 2021-02-13T00:53:52Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.19 psi 2021-02-13T00:53:52Z- handleDepth()-Depth_Keller msg: 1613177632373 epoch sec; seqNo:16721; depth 20.64 2021-02-13T00:53:52Z- handleDvl() - RDI_Pathfinder msg: 1613177632.38 epoch sec; seqNo:8051 2021-02-13T00:53:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:52Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:53:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177632.39 epoch sec; seqNo:16721; 4060320.95 north; 593323.88 east 2021-02-13T00:53:52Z- AHRS_M2 msg: 1613177632.76 epoch sec; seqNo:16590 2021-02-13T00:53:52Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-13T00:53:52Z- handleDepth()-Depth_Keller msg: 1613177632765 epoch sec; seqNo:16722; depth 20.51 2021-02-13T00:53:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177632.79 epoch sec; seqNo:16722; 4060320.95 north; 593323.88 east 2021-02-13T00:53:53Z- AHRS_M2 msg: 1613177633.16 epoch sec; seqNo:16591 2021-02-13T00:53:53Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-13T00:53:53Z- handleDepth()-Depth_Keller msg: 1613177633169 epoch sec; seqNo:16723; depth 20.40 2021-02-13T00:53:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177633.18 epoch sec; seqNo:16723; 4060321.33 north; 593323.87 east 2021-02-13T00:53:53Z- AHRS_M2 msg: 1613177633.57 epoch sec; seqNo:16592 2021-02-13T00:53:53Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-13T00:53:53Z- handleDepth()-Depth_Keller msg: 1613177633577 epoch sec; seqNo:16724; depth 20.29 2021-02-13T00:53:53Z- handleDvl() - RDI_Pathfinder msg: 1613177633.59 epoch sec; seqNo:8052 2021-02-13T00:53:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.38 , 92.03 , 102.98 , 0.00 2021-02-13T00:53:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.04 , -0.11 2021-02-13T00:53:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177633.60 epoch sec; seqNo:16724; 4060321.71 north; 593323.87 east 2021-02-13T00:53:53Z- AHRS_M2 msg: 1613177633.98 epoch sec; seqNo:16593 2021-02-13T00:53:53Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.19 psi 2021-02-13T00:53:53Z- handleDepth()-Depth_Keller msg: 1613177633981 epoch sec; seqNo:16725; depth 20.17 2021-02-13T00:53:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177633.99 epoch sec; seqNo:16725; 4060321.71 north; 593323.87 east 2021-02-13T00:53:54Z- AHRS_M2 msg: 1613177634.38 epoch sec; seqNo:16594 2021-02-13T00:53:54Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.19 psi 2021-02-13T00:53:54Z- handleDepth()-Depth_Keller msg: 1613177634381 epoch sec; seqNo:16726; depth 20.05 2021-02-13T00:53:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177634.40 epoch sec; seqNo:16726; 4060322.10 north; 593325.08 east 2021-02-13T00:53:54Z- LcmTrn::handleMessages() - handling 42 messages took 0.66 ms and 129 calls... 2021-02-13T00:53:54Z- waiting for 1613177632.56, time is 1613177634.40 2021-02-13T00:53:54Z- LcmTrn::updateTrn() >>>> pose time 1613177634.40, meas time 1613177633.59 <<<< 2021-02-13T00:53:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177633.59 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:53:54Z- TerrainNav::motionUpdate() - currEstimate = (4060322, 593325.08, 20.05) 2021-02-13T00:53:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:53:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:53:54Z- TNavBF::Projecting Measurements 2021-02-13T00:53:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:53:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:53:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:53:54Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:53:54Z- TNavFilter::3 good beams. 2021-02-13T00:53:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:53:54Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:53:54Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:53:54Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:53:54Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:53:54Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:53:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:53:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:53:54Z- TerrainMapOctree: (vehN, vehE ) = (4060321.59, 593324.68). 2021-02-13T00:53:54Z- TerrainMapOctree: Min Distance = 1310.79. 2021-02-13T00:53:54Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:53:54Z- There is no specific good beam that all particles have in common. 2021-02-13T00:53:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:53:54Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 8.85 2021-02-13T00:53:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:53:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 13.55 2021-02-13T00:53:54Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:53:54Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 16.97 2021-02-13T00:53:54Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:53:54Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:53:54Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:53:54Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:53:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:53:54Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:53:54Z- Filter 0 No beams used in subcloud update 2021-02-13T00:53:54Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:53:54Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:53:54Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 683.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:53:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177633.59 sec. 2021-02-13T00:53:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:53:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:53:54Z- variances([0], [2], and psi_berg) = 683.442635 832.989967 0.000000 2021-02-13T00:53:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:53:54Z- LcmTrn::publishEstimates() - MLE : 4060321.60 593345.58 20.05 2021-02-13T00:53:54Z- LcmTrn::publishEstimates() - MMSE : 4060322.10 593325.08 20.05 2021-02-13T00:53:54Z- LcmTrn::publishEstimates() - COVAR: 683.44 832.99 0.91 0.00 2021-02-13T00:53:54Z- AHRS_M2 msg: 1613177634.78 epoch sec; seqNo:16595 2021-02-13T00:53:54Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.19 psi 2021-02-13T00:53:54Z- handleDepth()-Depth_Keller msg: 1613177634784 epoch sec; seqNo:16727; depth 19.94 2021-02-13T00:53:54Z- handleDvl() - RDI_Pathfinder msg: 1613177634.80 epoch sec; seqNo:8053 2021-02-13T00:53:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 92.91 , 102.65 , 67.84 2021-02-13T00:53:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.09 , -0.05 2021-02-13T00:53:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177634.82 epoch sec; seqNo:16727; 4060322.48 north; 593325.08 east 2021-02-13T00:53:55Z- AHRS_M2 msg: 1613177635.18 epoch sec; seqNo:16596 2021-02-13T00:53:55Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.19 psi 2021-02-13T00:53:55Z- handleDepth()-Depth_Keller msg: 1613177635193 epoch sec; seqNo:16728; depth 19.81 2021-02-13T00:53:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177635.21 epoch sec; seqNo:16728; 4060322.86 north; 593325.07 east 2021-02-13T00:53:55Z- AHRS_M2 msg: 1613177635.59 epoch sec; seqNo:16597 2021-02-13T00:53:55Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.19 psi 2021-02-13T00:53:55Z- handleDepth()-Depth_Keller msg: 1613177635594 epoch sec; seqNo:16729; depth 19.72 2021-02-13T00:53:55Z- handleDvl() - RDI_Pathfinder msg: 1613177635.61 epoch sec; seqNo:8054 2021-02-13T00:53:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.78 , 98.25 , 101.90 , 69.48 2021-02-13T00:53:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.13 , 0.04 2021-02-13T00:53:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177635.62 epoch sec; seqNo:16729; 4060322.86 north; 593325.07 east 2021-02-13T00:53:55Z- AHRS_M2 msg: 1613177635.99 epoch sec; seqNo:16598 2021-02-13T00:53:55Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.19 psi 2021-02-13T00:53:55Z- handleDepth()-Depth_Keller msg: 1613177635996 epoch sec; seqNo:16730; depth 19.60 2021-02-13T00:53:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:56Z- handleDvl() - RDI_Pathfinder msg: 1613177636.00 epoch sec; seqNo:8055 2021-02-13T00:53:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.13 , 0.04 2021-02-13T00:53:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177636.02 epoch sec; seqNo:16730; 4060323.24 north; 593325.07 east 2021-02-13T00:53:56Z- AHRS_M2 msg: 1613177636.39 epoch sec; seqNo:16599 2021-02-13T00:53:56Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.19 psi 2021-02-13T00:53:56Z- handleDepth()-Depth_Keller msg: 1613177636401 epoch sec; seqNo:16731; depth 19.49 2021-02-13T00:53:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177636.41 epoch sec; seqNo:16731; 4060323.61 north; 593325.07 east 2021-02-13T00:53:56Z- AHRS_M2 msg: 1613177636.80 epoch sec; seqNo:16600 2021-02-13T00:53:56Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.18 psi 2021-02-13T00:53:56Z- handleDepth()-Depth_Keller msg: 1613177636809 epoch sec; seqNo:16732; depth 19.37 2021-02-13T00:53:56Z- handleDvl() - RDI_Pathfinder msg: 1613177636.82 epoch sec; seqNo:8056 2021-02-13T00:53:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 91.90 , 100.42 , 70.29 2021-02-13T00:53:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.09 , 0.09 2021-02-13T00:53:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177636.83 epoch sec; seqNo:16732; 4060323.99 north; 593325.06 east 2021-02-13T00:53:57Z- AHRS_M2 msg: 1613177637.20 epoch sec; seqNo:16601 2021-02-13T00:53:57Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.18 psi 2021-02-13T00:53:57Z- handleDepth()-Depth_Keller msg: 1613177637209 epoch sec; seqNo:16733; depth 19.26 2021-02-13T00:53:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:57Z- handleDvl() - RDI_Pathfinder msg: 1613177637.21 epoch sec; seqNo:8057 2021-02-13T00:53:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.09 , 0.09 2021-02-13T00:53:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177637.23 epoch sec; seqNo:16733; 4060324.37 north; 593325.06 east 2021-02-13T00:53:57Z- AHRS_M2 msg: 1613177637.62 epoch sec; seqNo:16602 2021-02-13T00:53:57Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.18 psi 2021-02-13T00:53:57Z- handleDepth()-Depth_Keller msg: 1613177637622 epoch sec; seqNo:16734; depth 19.15 2021-02-13T00:53:57Z- handleDvl() - RDI_Pathfinder msg: 1613177637.63 epoch sec; seqNo:8058 2021-02-13T00:53:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 101.63 , 70.70 2021-02-13T00:53:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , 0.10 2021-02-13T00:53:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177637.64 epoch sec; seqNo:16734; 4060324.76 north; 593326.27 east 2021-02-13T00:53:58Z- AHRS_M2 msg: 1613177638.01 epoch sec; seqNo:16603 2021-02-13T00:53:58Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.17 psi 2021-02-13T00:53:58Z- handleDepth()-Depth_Keller msg: 1613177638016 epoch sec; seqNo:16735; depth 19.05 2021-02-13T00:53:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177638.03 epoch sec; seqNo:16735; 4060324.76 north; 593326.27 east 2021-02-13T00:53:58Z- AHRS_M2 msg: 1613177638.41 epoch sec; seqNo:16604 2021-02-13T00:53:58Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.17 psi 2021-02-13T00:53:58Z- handleDepth()-Depth_Keller msg: 1613177638421 epoch sec; seqNo:16736; depth 18.94 2021-02-13T00:53:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:53:58Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:53:58Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:53:58Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:53:58Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:53:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:53:58Z- handleDvl() - RDI_Pathfinder msg: 1613177638.42 epoch sec; seqNo:8059 2021-02-13T00:53:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:53:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , 0.10 2021-02-13T00:53:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177638.43 epoch sec; seqNo:16736; 4060325.14 north; 593326.27 east 2021-02-13T00:53:58Z- AHRS_M2 msg: 1613177638.82 epoch sec; seqNo:16605 2021-02-13T00:53:58Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.17 psi 2021-02-13T00:53:58Z- handleDepth()-Depth_Keller msg: 1613177638826 epoch sec; seqNo:16737; depth 18.82 2021-02-13T00:53:58Z- handleDvl() - RDI_Pathfinder msg: 1613177638.84 epoch sec; seqNo:8060 2021-02-13T00:53:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 97.98 , 101.63 , 69.48 2021-02-13T00:53:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , 0.07 2021-02-13T00:53:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177638.85 epoch sec; seqNo:16737; 4060325.52 north; 593326.27 east 2021-02-13T00:53:59Z- AHRS_M2 msg: 1613177639.23 epoch sec; seqNo:16606 2021-02-13T00:53:59Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.17 psi 2021-02-13T00:53:59Z- handleDepth()-Depth_Keller msg: 1613177639237 epoch sec; seqNo:16738; depth 18.72 2021-02-13T00:53:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177639.26 epoch sec; seqNo:16738; 4060325.90 north; 593326.26 east 2021-02-13T00:53:59Z- AHRS_M2 msg: 1613177639.63 epoch sec; seqNo:16607 2021-02-13T00:53:59Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.16 psi 2021-02-13T00:53:59Z- handleDepth()-Depth_Keller msg: 1613177639634 epoch sec; seqNo:16739; depth 18.60 2021-02-13T00:53:59Z- handleDvl() - RDI_Pathfinder msg: 1613177639.65 epoch sec; seqNo:8061 2021-02-13T00:53:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.64 , 93.25 , 101.77 , 78.64 2021-02-13T00:53:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , 0.02 2021-02-13T00:53:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177639.67 epoch sec; seqNo:16739; 4060326.28 north; 593326.26 east 2021-02-13T00:53:59Z- LcmTrn::handleMessages() - handling 48 messages took 0.70 ms and 134 calls... 2021-02-13T00:53:59Z- waiting for 1613177637.40, time is 1613177639.67 2021-02-13T00:53:59Z- LcmTrn::updateTrn() equating pose 1613177639.67 and meas times 1613177639.67 2021-02-13T00:53:59Z- LcmTrn::updateTrn() >>>> pose time 1613177639.67, meas time 1613177639.67 <<<< 2021-02-13T00:53:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177639.67 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:53:59Z- TerrainNav::motionUpdate() - currEstimate = (4060326, 593326.26, 18.60) 2021-02-13T00:53:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:53:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:53:59Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177639.67 sec.; No relevant navigation data available 2021-02-13T00:53:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:53:59Z- TNavBF::mmse selected from filter -1 2021-02-13T00:53:59Z- variances([0], [2], and psi_berg) = 683.395247 832.989946 0.000000 2021-02-13T00:53:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:53:59Z- LcmTrn::publishEstimates() - MLE : 4060325.78 593346.76 18.60 2021-02-13T00:53:59Z- LcmTrn::publishEstimates() - MMSE : 4060326.28 593326.26 18.60 2021-02-13T00:53:59Z- LcmTrn::publishEstimates() - COVAR: 683.40 832.99 1.13 0.00 2021-02-13T00:54:00Z- AHRS_M2 msg: 1613177640.03 epoch sec; seqNo:16608 2021-02-13T00:54:00Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.16 psi 2021-02-13T00:54:00Z- handleDepth()-Depth_Keller msg: 1613177640041 epoch sec; seqNo:16740; depth 18.53 2021-02-13T00:54:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177640.05 epoch sec; seqNo:16740; 4060326.29 north; 593327.48 east 2021-02-13T00:54:00Z- AHRS_M2 msg: 1613177640.43 epoch sec; seqNo:16609 2021-02-13T00:54:00Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.17 psi 2021-02-13T00:54:00Z- handleDepth()-Depth_Keller msg: 1613177640449 epoch sec; seqNo:16741; depth 18.40 2021-02-13T00:54:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177640.46 epoch sec; seqNo:16741; 4060326.67 north; 593327.47 east 2021-02-13T00:54:00Z- AHRS_M2 msg: 1613177640.84 epoch sec; seqNo:16610 2021-02-13T00:54:00Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.17 psi 2021-02-13T00:54:00Z- handleDepth()-Depth_Keller msg: 1613177640847 epoch sec; seqNo:16742; depth 18.30 2021-02-13T00:54:00Z- handleDvl() - RDI_Pathfinder msg: 1613177640.87 epoch sec; seqNo:8062 2021-02-13T00:54:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 101.56 , 78.44 2021-02-13T00:54:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.03 , -0.01 2021-02-13T00:54:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177640.88 epoch sec; seqNo:16742; 4060327.05 north; 593327.47 east 2021-02-13T00:54:01Z- AHRS_M2 msg: 1613177641.24 epoch sec; seqNo:16611 2021-02-13T00:54:01Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.17 psi 2021-02-13T00:54:01Z- handleDepth()-Depth_Keller msg: 1613177641249 epoch sec; seqNo:16743; depth 18.21 2021-02-13T00:54:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177641.26 epoch sec; seqNo:16743; 4060327.43 north; 593327.47 east 2021-02-13T00:54:01Z- AHRS_M2 msg: 1613177641.65 epoch sec; seqNo:16612 2021-02-13T00:54:01Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.16 psi 2021-02-13T00:54:01Z- handleDepth()-Depth_Keller msg: 1613177641653 epoch sec; seqNo:16744; depth 18.12 2021-02-13T00:54:01Z- handleDvl() - RDI_Pathfinder msg: 1613177641.66 epoch sec; seqNo:8063 2021-02-13T00:54:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.38 , 97.85 , 102.71 , 0.00 2021-02-13T00:54:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.00 , -0.03 2021-02-13T00:54:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177641.67 epoch sec; seqNo:16744; 4060327.81 north; 593327.46 east 2021-02-13T00:54:02Z- AHRS_M2 msg: 1613177642.05 epoch sec; seqNo:16613 2021-02-13T00:54:02Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.16 psi 2021-02-13T00:54:02Z- handleDepth()-Depth_Keller msg: 1613177642057 epoch sec; seqNo:16745; depth 18.02 2021-02-13T00:54:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:02Z- handleDvl() - RDI_Pathfinder msg: 1613177642.06 epoch sec; seqNo:8064 2021-02-13T00:54:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.00 , -0.03 2021-02-13T00:54:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177642.07 epoch sec; seqNo:16745; 4060327.81 north; 593327.46 east 2021-02-13T00:54:02Z- AHRS_M2 msg: 1613177642.45 epoch sec; seqNo:16614 2021-02-13T00:54:02Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.16 psi 2021-02-13T00:54:02Z- handleDepth()-Depth_Keller msg: 1613177642465 epoch sec; seqNo:16746; depth 17.90 2021-02-13T00:54:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177642.48 epoch sec; seqNo:16746; 4060328.19 north; 593327.46 east 2021-02-13T00:54:02Z- AHRS_M2 msg: 1613177642.86 epoch sec; seqNo:16615 2021-02-13T00:54:02Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.16 psi 2021-02-13T00:54:02Z- handleDepth()-Depth_Keller msg: 1613177642865 epoch sec; seqNo:16747; depth 17.81 2021-02-13T00:54:02Z- handleDvl() - RDI_Pathfinder msg: 1613177642.88 epoch sec; seqNo:8065 2021-02-13T00:54:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.64 , 103.73 , 78.17 2021-02-13T00:54:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.02 , -0.01 2021-02-13T00:54:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177642.89 epoch sec; seqNo:16747; 4060328.58 north; 593328.67 east 2021-02-13T00:54:03Z- AHRS_M2 msg: 1613177643.26 epoch sec; seqNo:16616 2021-02-13T00:54:03Z- AHRS_M2 msg: 0.58 phi; 0.30 theta; -0.16 psi 2021-02-13T00:54:03Z- handleDepth()-Depth_Keller msg: 1613177643281 epoch sec; seqNo:16748; depth 17.71 2021-02-13T00:54:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:03Z- handleDvl() - RDI_Pathfinder msg: 1613177643.28 epoch sec; seqNo:8066 2021-02-13T00:54:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.02 , -0.01 2021-02-13T00:54:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177643.30 epoch sec; seqNo:16748; 4060328.96 north; 593328.67 east 2021-02-13T00:54:03Z- AHRS_M2 msg: 1613177643.67 epoch sec; seqNo:16617 2021-02-13T00:54:03Z- AHRS_M2 msg: 0.58 phi; 0.30 theta; -0.16 psi 2021-02-13T00:54:03Z- handleDepth()-Depth_Keller msg: 1613177643673 epoch sec; seqNo:16749; depth 17.60 2021-02-13T00:54:03Z- handleDvl() - RDI_Pathfinder msg: 1613177643.69 epoch sec; seqNo:8067 2021-02-13T00:54:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.07 , 92.23 , 103.05 , 78.71 2021-02-13T00:54:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.05 , 0.01 2021-02-13T00:54:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177643.70 epoch sec; seqNo:16749; 4060329.34 north; 593328.67 east 2021-02-13T00:54:04Z- AHRS_M2 msg: 1613177644.07 epoch sec; seqNo:16618 2021-02-13T00:54:04Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.15 psi 2021-02-13T00:54:04Z- handleDepth()-Depth_Keller msg: 1613177644076 epoch sec; seqNo:16750; depth 17.50 2021-02-13T00:54:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177644.09 epoch sec; seqNo:16750; 4060329.34 north; 593328.67 east 2021-02-13T00:54:04Z- AHRS_M2 msg: 1613177644.47 epoch sec; seqNo:16619 2021-02-13T00:54:04Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.15 psi 2021-02-13T00:54:04Z- handleDepth()-Depth_Keller msg: 1613177644481 epoch sec; seqNo:16751; depth 17.40 2021-02-13T00:54:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:04Z- handleDvl() - RDI_Pathfinder msg: 1613177644.48 epoch sec; seqNo:8068 2021-02-13T00:54:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.05 , 0.01 2021-02-13T00:54:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177644.49 epoch sec; seqNo:16751; 4060329.72 north; 593328.66 east 2021-02-13T00:54:04Z- LcmTrn::handleMessages() - handling 43 messages took 0.65 ms and 130 calls... 2021-02-13T00:54:04Z- waiting for 1613177642.67, time is 1613177644.49 2021-02-13T00:54:04Z- LcmTrn::updateTrn() equating pose 1613177644.49 and meas times 1613177644.49 2021-02-13T00:54:04Z- LcmTrn::updateTrn() >>>> pose time 1613177644.49, meas time 1613177644.49 <<<< 2021-02-13T00:54:04Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177644.49, ping # 0. 2021-02-13T00:54:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177644.49 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:54:04Z- TerrainNav::motionUpdate() - currEstimate = (4060330, 593328.66, 17.40) 2021-02-13T00:54:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:54:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:54:04Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:54:04Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:54:04Z- TNavBF::Projecting Measurements 2021-02-13T00:54:04Z- TNavFilter::Measurement from time = 1613177644.49 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:54:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:54:04Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:54:04Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:54:04Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:54:04Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:54:04Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:54:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:54:04Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 683.3 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:54:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:54:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:54:04Z- variances([0], [2], and psi_berg) = 683.348701 832.989925 0.000000 2021-02-13T00:54:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:54:04Z- LcmTrn::publishEstimates() - MLE : 4060329.22 593349.16 17.40 2021-02-13T00:54:04Z- LcmTrn::publishEstimates() - MMSE : 4060329.72 593328.66 17.40 2021-02-13T00:54:04Z- LcmTrn::publishEstimates() - COVAR: 683.35 832.99 1.35 0.00 2021-02-13T00:54:04Z- AHRS_M2 msg: 1613177644.88 epoch sec; seqNo:16620 2021-02-13T00:54:04Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.15 psi 2021-02-13T00:54:04Z- handleDepth()-Depth_Keller msg: 1613177644886 epoch sec; seqNo:16752; depth 17.31 2021-02-13T00:54:04Z- handleDvl() - RDI_Pathfinder msg: 1613177644.90 epoch sec; seqNo:8069 2021-02-13T00:54:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.66 , 93.04 , 102.78 , 78.44 2021-02-13T00:54:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.12 , -0.02 2021-02-13T00:54:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177644.91 epoch sec; seqNo:16752; 4060330.11 north; 593329.88 east 2021-02-13T00:54:05Z- AHRS_M2 msg: 1613177645.28 epoch sec; seqNo:16621 2021-02-13T00:54:05Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-13T00:54:05Z- handleDepth()-Depth_Keller msg: 1613177645288 epoch sec; seqNo:16753; depth 17.21 2021-02-13T00:54:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177645.32 epoch sec; seqNo:16753; 4060330.11 north; 593329.88 east 2021-02-13T00:54:05Z- AHRS_M2 msg: 1613177645.69 epoch sec; seqNo:16622 2021-02-13T00:54:05Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-13T00:54:05Z- handleDepth()-Depth_Keller msg: 1613177645692 epoch sec; seqNo:16754; depth 17.08 2021-02-13T00:54:05Z- handleDvl() - RDI_Pathfinder msg: 1613177645.71 epoch sec; seqNo:8070 2021-02-13T00:54:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.69 , 97.91 , 104.00 , 69.89 2021-02-13T00:54:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.12 , -0.08 2021-02-13T00:54:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177645.73 epoch sec; seqNo:16754; 4060330.49 north; 593329.87 east 2021-02-13T00:54:06Z- AHRS_M2 msg: 1613177646.09 epoch sec; seqNo:16623 2021-02-13T00:54:06Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.15 psi 2021-02-13T00:54:06Z- handleDepth()-Depth_Keller msg: 1613177646109 epoch sec; seqNo:16755; depth 16.97 2021-02-13T00:54:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177646.12 epoch sec; seqNo:16755; 4060330.87 north; 593329.87 east 2021-02-13T00:54:06Z- AHRS_M2 msg: 1613177646.50 epoch sec; seqNo:16624 2021-02-13T00:54:06Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-13T00:54:06Z- handleDepth()-Depth_Keller msg: 1613177646506 epoch sec; seqNo:16756; depth 16.85 2021-02-13T00:54:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177646.51 epoch sec; seqNo:16756; 4060330.87 north; 593329.87 east 2021-02-13T00:54:06Z- AHRS_M2 msg: 1613177646.90 epoch sec; seqNo:16625 2021-02-13T00:54:06Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.15 psi 2021-02-13T00:54:06Z- handleDepth()-Depth_Keller msg: 1613177646913 epoch sec; seqNo:16757; depth 16.75 2021-02-13T00:54:06Z- handleDvl() - RDI_Pathfinder msg: 1613177646.92 epoch sec; seqNo:8071 2021-02-13T00:54:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.66 , 97.91 , 105.21 , 71.52 2021-02-13T00:54:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.04 , -0.11 2021-02-13T00:54:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177646.95 epoch sec; seqNo:16757; 4060331.24 north; 593329.87 east 2021-02-13T00:54:07Z- AHRS_M2 msg: 1613177647.30 epoch sec; seqNo:16626 2021-02-13T00:54:07Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.15 psi 2021-02-13T00:54:07Z- handleDepth()-Depth_Keller msg: 1613177647314 epoch sec; seqNo:16758; depth 16.63 2021-02-13T00:54:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177647.33 epoch sec; seqNo:16758; 4060331.26 north; 593331.09 east 2021-02-13T00:54:07Z- AHRS_M2 msg: 1613177647.71 epoch sec; seqNo:16627 2021-02-13T00:54:07Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.15 psi 2021-02-13T00:54:07Z- handleDepth()-Depth_Keller msg: 1613177647712 epoch sec; seqNo:16759; depth 16.53 2021-02-13T00:54:07Z- handleDvl() - RDI_Pathfinder msg: 1613177647.73 epoch sec; seqNo:8072 2021-02-13T00:54:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.47 , 97.91 , 105.21 , 78.44 2021-02-13T00:54:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.07 2021-02-13T00:54:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177647.74 epoch sec; seqNo:16759; 4060331.64 north; 593331.08 east 2021-02-13T00:54:08Z- AHRS_M2 msg: 1613177648.11 epoch sec; seqNo:16628 2021-02-13T00:54:08Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.15 psi 2021-02-13T00:54:08Z- handleDepth()-Depth_Keller msg: 1613177648121 epoch sec; seqNo:16760; depth 16.41 2021-02-13T00:54:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:08Z- handleDvl() - RDI_Pathfinder msg: 1613177648.12 epoch sec; seqNo:8073 2021-02-13T00:54:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.07 2021-02-13T00:54:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177648.14 epoch sec; seqNo:16760; 4060332.02 north; 593331.08 east 2021-02-13T00:54:08Z- AHRS_M2 msg: 1613177648.52 epoch sec; seqNo:16629 2021-02-13T00:54:08Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.16 psi 2021-02-13T00:54:08Z- handleDepth()-Depth_Keller msg: 1613177648525 epoch sec; seqNo:16761; depth 16.31 2021-02-13T00:54:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177648.53 epoch sec; seqNo:16761; 4060332.39 north; 593331.07 east 2021-02-13T00:54:08Z- AHRS_M2 msg: 1613177648.92 epoch sec; seqNo:16630 2021-02-13T00:54:08Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.16 psi 2021-02-13T00:54:08Z- handleDepth()-Depth_Keller msg: 1613177648924 epoch sec; seqNo:16762; depth 16.21 2021-02-13T00:54:08Z- handleDvl() - RDI_Pathfinder msg: 1613177648.94 epoch sec; seqNo:8074 2021-02-13T00:54:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 97.78 , 103.86 , 72.75 2021-02-13T00:54:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , 0.01 2021-02-13T00:54:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177648.95 epoch sec; seqNo:16762; 4060332.39 north; 593331.07 east 2021-02-13T00:54:09Z- AHRS_M2 msg: 1613177649.32 epoch sec; seqNo:16631 2021-02-13T00:54:09Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.16 psi 2021-02-13T00:54:09Z- handleDepth()-Depth_Keller msg: 1613177649328 epoch sec; seqNo:16763; depth 16.09 2021-02-13T00:54:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:09Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:09Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:09Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:09Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:09Z- handleDvl() - RDI_Pathfinder msg: 1613177649.33 epoch sec; seqNo:8075 2021-02-13T00:54:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , 0.01 2021-02-13T00:54:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177649.35 epoch sec; seqNo:16763; 4060332.77 north; 593331.07 east 2021-02-13T00:54:09Z- AHRS_M2 msg: 1613177649.73 epoch sec; seqNo:16632 2021-02-13T00:54:09Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.16 psi 2021-02-13T00:54:09Z- handleDepth()-Depth_Keller msg: 1613177649731 epoch sec; seqNo:16764; depth 15.99 2021-02-13T00:54:09Z- handleDvl() - RDI_Pathfinder msg: 1613177649.75 epoch sec; seqNo:8076 2021-02-13T00:54:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.59 , 97.85 , 105.15 , 78.37 2021-02-13T00:54:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.10 , 0.09 2021-02-13T00:54:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177649.77 epoch sec; seqNo:16764; 4060333.15 north; 593331.07 east 2021-02-13T00:54:09Z- LcmTrn::handleMessages() - handling 47 messages took 0.71 ms and 133 calls... 2021-02-13T00:54:09Z- waiting for 1613177647.49, time is 1613177649.77 2021-02-13T00:54:09Z- LcmTrn::updateTrn() equating pose 1613177649.77 and meas times 1613177649.77 2021-02-13T00:54:09Z- LcmTrn::updateTrn() >>>> pose time 1613177649.77, meas time 1613177649.77 <<<< 2021-02-13T00:54:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177649.77 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:54:09Z- TerrainNav::motionUpdate() - currEstimate = (4060333, 593331.07, 15.99) 2021-02-13T00:54:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:54:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:54:09Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177649.77 sec.; No relevant navigation data available 2021-02-13T00:54:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:54:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:54:09Z- variances([0], [2], and psi_berg) = 683.302178 832.989905 0.000000 2021-02-13T00:54:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:54:09Z- LcmTrn::publishEstimates() - MLE : 4060332.65 593351.57 15.99 2021-02-13T00:54:09Z- LcmTrn::publishEstimates() - MMSE : 4060333.15 593331.07 15.99 2021-02-13T00:54:09Z- LcmTrn::publishEstimates() - COVAR: 683.30 832.99 1.56 0.00 2021-02-13T00:54:10Z- AHRS_M2 msg: 1613177650.13 epoch sec; seqNo:16633 2021-02-13T00:54:10Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.16 psi 2021-02-13T00:54:10Z- handleDepth()-Depth_Keller msg: 1613177650140 epoch sec; seqNo:16765; depth 15.86 2021-02-13T00:54:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177650.15 epoch sec; seqNo:16765; 4060333.53 north; 593331.06 east 2021-02-13T00:54:10Z- AHRS_M2 msg: 1613177650.54 epoch sec; seqNo:16634 2021-02-13T00:54:10Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.16 psi 2021-02-13T00:54:10Z- handleDepth()-Depth_Keller msg: 1613177650549 epoch sec; seqNo:16766; depth 15.78 2021-02-13T00:54:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:10Z- handleDvl() - RDI_Pathfinder msg: 1613177650.55 epoch sec; seqNo:8077 2021-02-13T00:54:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.10 , 0.09 2021-02-13T00:54:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177650.56 epoch sec; seqNo:16766; 4060333.92 north; 593332.28 east 2021-02-13T00:54:10Z- AHRS_M2 msg: 1613177650.94 epoch sec; seqNo:16635 2021-02-13T00:54:10Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.17 psi 2021-02-13T00:54:10Z- handleDepth()-Depth_Keller msg: 1613177650944 epoch sec; seqNo:16767; depth 15.68 2021-02-13T00:54:10Z- handleDvl() - RDI_Pathfinder msg: 1613177650.97 epoch sec; seqNo:8078 2021-02-13T00:54:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.67 , 97.71 , 103.79 , 78.24 2021-02-13T00:54:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.02 , 0.12 2021-02-13T00:54:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177650.98 epoch sec; seqNo:16767; 4060334.30 north; 593332.27 east 2021-02-13T00:54:11Z- AHRS_M2 msg: 1613177651.34 epoch sec; seqNo:16636 2021-02-13T00:54:11Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.17 psi 2021-02-13T00:54:11Z- handleDepth()-Depth_Keller msg: 1613177651348 epoch sec; seqNo:16768; depth 15.57 2021-02-13T00:54:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177651.36 epoch sec; seqNo:16768; 4060334.30 north; 593332.27 east 2021-02-13T00:54:11Z- AHRS_M2 msg: 1613177651.75 epoch sec; seqNo:16637 2021-02-13T00:54:11Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.17 psi 2021-02-13T00:54:11Z- handleDepth()-Depth_Keller msg: 1613177651752 epoch sec; seqNo:16769; depth 15.45 2021-02-13T00:54:11Z- handleDvl() - RDI_Pathfinder msg: 1613177651.76 epoch sec; seqNo:8079 2021-02-13T00:54:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 97.78 , 108.73 , 73.56 2021-02-13T00:54:11Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.06 , 0.08 2021-02-13T00:54:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177651.77 epoch sec; seqNo:16769; 4060334.68 north; 593332.27 east 2021-02-13T00:54:12Z- AHRS_M2 msg: 1613177652.16 epoch sec; seqNo:16638 2021-02-13T00:54:12Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.17 psi 2021-02-13T00:54:12Z- handleDepth()-Depth_Keller msg: 1613177652165 epoch sec; seqNo:16770; depth 15.35 2021-02-13T00:54:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177652.18 epoch sec; seqNo:16770; 4060335.06 north; 593332.27 east 2021-02-13T00:54:12Z- AHRS_M2 msg: 1613177652.56 epoch sec; seqNo:16639 2021-02-13T00:54:12Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.17 psi 2021-02-13T00:54:12Z- handleDepth()-Depth_Keller msg: 1613177652564 epoch sec; seqNo:16771; depth 15.24 2021-02-13T00:54:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177652.60 epoch sec; seqNo:16771; 4060335.45 north; 593333.48 east 2021-02-13T00:54:12Z- AHRS_M2 msg: 1613177652.96 epoch sec; seqNo:16640 2021-02-13T00:54:12Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T00:54:12Z- handleDepth()-Depth_Keller msg: 1613177652964 epoch sec; seqNo:16772; depth 15.14 2021-02-13T00:54:12Z- handleDvl() - RDI_Pathfinder msg: 1613177652.98 epoch sec; seqNo:8080 2021-02-13T00:54:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 97.71 , 106.23 , 71.93 2021-02-13T00:54:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.10 , -0.02 2021-02-13T00:54:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177652.99 epoch sec; seqNo:16772; 4060335.83 north; 593333.48 east 2021-02-13T00:54:13Z- AHRS_M2 msg: 1613177653.37 epoch sec; seqNo:16641 2021-02-13T00:54:13Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T00:54:13Z- handleDepth()-Depth_Keller msg: 1613177653378 epoch sec; seqNo:16773; depth 15.04 2021-02-13T00:54:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177653.39 epoch sec; seqNo:16773; 4060335.83 north; 593333.48 east 2021-02-13T00:54:13Z- AHRS_M2 msg: 1613177653.77 epoch sec; seqNo:16642 2021-02-13T00:54:13Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:54:13Z- handleDepth()-Depth_Keller msg: 1613177653777 epoch sec; seqNo:16774; depth 14.91 2021-02-13T00:54:13Z- handleDvl() - RDI_Pathfinder msg: 1613177653.80 epoch sec; seqNo:8081 2021-02-13T00:54:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 97.78 , 108.73 , 78.31 2021-02-13T00:54:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.09 2021-02-13T00:54:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177653.81 epoch sec; seqNo:16774; 4060336.21 north; 593333.47 east 2021-02-13T00:54:14Z- AHRS_M2 msg: 1613177654.17 epoch sec; seqNo:16643 2021-02-13T00:54:14Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-13T00:54:14Z- handleDepth()-Depth_Keller msg: 1613177654176 epoch sec; seqNo:16775; depth 14.80 2021-02-13T00:54:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:14Z- handleDvl() - RDI_Pathfinder msg: 1613177654.18 epoch sec; seqNo:8082 2021-02-13T00:54:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.09 2021-02-13T00:54:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177654.19 epoch sec; seqNo:16775; 4060336.59 north; 593333.47 east 2021-02-13T00:54:14Z- AHRS_M2 msg: 1613177654.59 epoch sec; seqNo:16644 2021-02-13T00:54:14Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:54:14Z- handleDepth()-Depth_Keller msg: 1613177654601 epoch sec; seqNo:16776; depth 14.70 2021-02-13T00:54:14Z- handleDvl() - RDI_Pathfinder msg: 1613177654.62 epoch sec; seqNo:8083 2021-02-13T00:54:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 98.86 , 108.60 , 72.75 2021-02-13T00:54:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.11 2021-02-13T00:54:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177654.64 epoch sec; seqNo:16776; 4060336.97 north; 593333.47 east 2021-02-13T00:54:14Z- LcmTrn::handleMessages() - handling 43 messages took 0.60 ms and 131 calls... 2021-02-13T00:54:14Z- waiting for 1613177652.77, time is 1613177654.64 2021-02-13T00:54:14Z- LcmTrn::updateTrn() equating pose 1613177654.64 and meas times 1613177654.64 2021-02-13T00:54:14Z- LcmTrn::updateTrn() >>>> pose time 1613177654.64, meas time 1613177654.64 <<<< 2021-02-13T00:54:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177654.64 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:54:14Z- TerrainNav::motionUpdate() - currEstimate = (4060337, 593333.47, 14.70) 2021-02-13T00:54:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:54:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:54:14Z- TNavBF::Projecting Measurements 2021-02-13T00:54:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:54:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:54:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:54:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:54:14Z- TNavFilter::4 good beams. 2021-02-13T00:54:14Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:54:14Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:54:14Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:54:14Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:54:14Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:54:14Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:54:14Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:54:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:54:14Z- TerrainMapOctree: (vehN, vehE ) = (4060336.97, 593333.47). 2021-02-13T00:54:14Z- TerrainMapOctree: Min Distance = 1293.70. 2021-02-13T00:54:14Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:54:14Z- There is no specific good beam that all particles have in common. 2021-02-13T00:54:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:54:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.79 K: 0.126 ExpectedMeasDiff: nan currMeas.covariance: 10.71 2021-02-13T00:54:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:54:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.56 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 15.64 2021-02-13T00:54:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:54:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.43 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 18.87 2021-02-13T00:54:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:54:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.116 ExpectedMeasDiff: nan currMeas.covariance: 8.47 2021-02-13T00:54:14Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:54:14Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:54:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:54:14Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:54:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:54:14Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:54:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:54:14Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:54:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:54:14Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:54:14Z- Filter 0 No beams used in subcloud update 2021-02-13T00:54:14Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:54:14Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:54:14Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 683.3 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:54:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177654.64 sec. 2021-02-13T00:54:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:54:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:54:14Z- variances([0], [2], and psi_berg) = 683.253971 832.989884 0.000000 2021-02-13T00:54:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:54:14Z- LcmTrn::publishEstimates() - MLE : 4060336.47 593353.97 14.70 2021-02-13T00:54:14Z- LcmTrn::publishEstimates() - MMSE : 4060336.97 593333.47 14.70 2021-02-13T00:54:14Z- LcmTrn::publishEstimates() - COVAR: 683.25 832.99 1.19 0.00 2021-02-13T00:54:14Z- AHRS_M2 msg: 1613177654.98 epoch sec; seqNo:16645 2021-02-13T00:54:14Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:54:14Z- handleDepth()-Depth_Keller msg: 1613177654984 epoch sec; seqNo:16777; depth 14.57 2021-02-13T00:54:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177655.00 epoch sec; seqNo:16777; 4060336.98 north; 593334.69 east 2021-02-13T00:54:15Z- AHRS_M2 msg: 1613177655.39 epoch sec; seqNo:16646 2021-02-13T00:54:15Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:54:15Z- handleDepth()-Depth_Keller msg: 1613177655395 epoch sec; seqNo:16778; depth 14.46 2021-02-13T00:54:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:15Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:15Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:15Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:15Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:15Z- handleDvl() - RDI_Pathfinder msg: 1613177655.40 epoch sec; seqNo:8084 2021-02-13T00:54:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.11 2021-02-13T00:54:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177655.43 epoch sec; seqNo:16778; 4060337.36 north; 593334.68 east 2021-02-13T00:54:15Z- AHRS_M2 msg: 1613177655.79 epoch sec; seqNo:16647 2021-02-13T00:54:15Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T00:54:15Z- handleDepth()-Depth_Keller msg: 1613177655797 epoch sec; seqNo:16779; depth 14.36 2021-02-13T00:54:15Z- handleDvl() - RDI_Pathfinder msg: 1613177655.81 epoch sec; seqNo:8085 2021-02-13T00:54:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.67 , 98.93 , 109.88 , 78.24 2021-02-13T00:54:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , -0.06 2021-02-13T00:54:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177655.82 epoch sec; seqNo:16779; 4060337.74 north; 593334.68 east 2021-02-13T00:54:16Z- AHRS_M2 msg: 1613177656.19 epoch sec; seqNo:16648 2021-02-13T00:54:16Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T00:54:16Z- handleDepth()-Depth_Keller msg: 1613177656198 epoch sec; seqNo:16780; depth 14.23 2021-02-13T00:54:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177656.21 epoch sec; seqNo:16780; 4060338.12 north; 593334.67 east 2021-02-13T00:54:16Z- AHRS_M2 msg: 1613177656.60 epoch sec; seqNo:16649 2021-02-13T00:54:16Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T00:54:16Z- handleDepth()-Depth_Keller msg: 1613177656604 epoch sec; seqNo:16781; depth 14.12 2021-02-13T00:54:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:16Z- handleDvl() - RDI_Pathfinder msg: 1613177656.61 epoch sec; seqNo:8086 2021-02-13T00:54:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , -0.06 2021-02-13T00:54:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177656.65 epoch sec; seqNo:16781; 4060338.12 north; 593334.67 east 2021-02-13T00:54:17Z- AHRS_M2 msg: 1613177657.00 epoch sec; seqNo:16650 2021-02-13T00:54:17Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.17 psi 2021-02-13T00:54:17Z- handleDepth()-Depth_Keller msg: 1613177657005 epoch sec; seqNo:16782; depth 14.01 2021-02-13T00:54:17Z- handleDvl() - RDI_Pathfinder msg: 1613177657.02 epoch sec; seqNo:8087 2021-02-13T00:54:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.48 , 100.42 , 111.23 , 78.78 2021-02-13T00:54:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.03 , -0.00 2021-02-13T00:54:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177657.03 epoch sec; seqNo:16782; 4060338.50 north; 593334.67 east 2021-02-13T00:54:17Z- AHRS_M2 msg: 1613177657.41 epoch sec; seqNo:16651 2021-02-13T00:54:17Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.16 psi 2021-02-13T00:54:17Z- handleDepth()-Depth_Keller msg: 1613177657410 epoch sec; seqNo:16783; depth 13.92 2021-02-13T00:54:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177657.42 epoch sec; seqNo:16783; 4060338.87 north; 593334.67 east 2021-02-13T00:54:17Z- AHRS_M2 msg: 1613177657.81 epoch sec; seqNo:16652 2021-02-13T00:54:17Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.16 psi 2021-02-13T00:54:17Z- handleDepth()-Depth_Keller msg: 1613177657812 epoch sec; seqNo:16784; depth 13.80 2021-02-13T00:54:17Z- handleDvl() - RDI_Pathfinder msg: 1613177657.83 epoch sec; seqNo:8088 2021-02-13T00:54:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 97.64 , 109.81 , 78.71 2021-02-13T00:54:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , 0.05 2021-02-13T00:54:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177657.84 epoch sec; seqNo:16784; 4060339.27 north; 593335.88 east 2021-02-13T00:54:18Z- AHRS_M2 msg: 1613177658.21 epoch sec; seqNo:16653 2021-02-13T00:54:18Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T00:54:18Z- handleDepth()-Depth_Keller msg: 1613177658221 epoch sec; seqNo:16785; depth 13.68 2021-02-13T00:54:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177658.23 epoch sec; seqNo:16785; 4060339.65 north; 593335.88 east 2021-02-13T00:54:18Z- AHRS_M2 msg: 1613177658.62 epoch sec; seqNo:16654 2021-02-13T00:54:18Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T00:54:18Z- handleDepth()-Depth_Keller msg: 1613177658620 epoch sec; seqNo:16786; depth 13.57 2021-02-13T00:54:18Z- handleDvl() - RDI_Pathfinder msg: 1613177658.63 epoch sec; seqNo:8089 2021-02-13T00:54:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.36 , 109.88 , 78.78 2021-02-13T00:54:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.09 , 0.05 2021-02-13T00:54:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177658.64 epoch sec; seqNo:16786; 4060339.65 north; 593335.88 east 2021-02-13T00:54:19Z- AHRS_M2 msg: 1613177659.02 epoch sec; seqNo:16655 2021-02-13T00:54:19Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T00:54:19Z- handleDepth()-Depth_Keller msg: 1613177659026 epoch sec; seqNo:16787; depth 13.47 2021-02-13T00:54:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:19Z- handleDvl() - RDI_Pathfinder msg: 1613177659.03 epoch sec; seqNo:8090 2021-02-13T00:54:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.09 , 0.05 2021-02-13T00:54:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177659.04 epoch sec; seqNo:16787; 4060340.02 north; 593335.87 east 2021-02-13T00:54:19Z- AHRS_M2 msg: 1613177659.42 epoch sec; seqNo:16656 2021-02-13T00:54:19Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-13T00:54:19Z- handleDepth()-Depth_Keller msg: 1613177659429 epoch sec; seqNo:16788; depth 13.35 2021-02-13T00:54:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177659.44 epoch sec; seqNo:16788; 4060340.40 north; 593335.87 east 2021-02-13T00:54:19Z- AHRS_M2 msg: 1613177659.83 epoch sec; seqNo:16657 2021-02-13T00:54:19Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-13T00:54:19Z- handleDepth()-Depth_Keller msg: 1613177659832 epoch sec; seqNo:16789; depth 13.23 2021-02-13T00:54:19Z- handleDvl() - RDI_Pathfinder msg: 1613177659.84 epoch sec; seqNo:8091 2021-02-13T00:54:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 97.44 , 112.31 , 78.51 2021-02-13T00:54:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.10 , -0.03 2021-02-13T00:54:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177659.87 epoch sec; seqNo:16789; 4060340.42 north; 593337.09 east 2021-02-13T00:54:19Z- LcmTrn::handleMessages() - handling 47 messages took 0.74 ms and 133 calls... 2021-02-13T00:54:19Z- waiting for 1613177657.64, time is 1613177659.87 2021-02-13T00:54:19Z- LcmTrn::updateTrn() equating pose 1613177659.87 and meas times 1613177659.87 2021-02-13T00:54:19Z- LcmTrn::updateTrn() >>>> pose time 1613177659.87, meas time 1613177659.87 <<<< 2021-02-13T00:54:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177659.87 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:54:19Z- TerrainNav::motionUpdate() - currEstimate = (4060340, 593337.09, 13.23) 2021-02-13T00:54:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:54:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:54:19Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177659.87 sec.; No relevant navigation data available 2021-02-13T00:54:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:54:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:54:19Z- variances([0], [2], and psi_berg) = 683.203203 832.989861 0.000000 2021-02-13T00:54:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:54:19Z- LcmTrn::publishEstimates() - MLE : 4060339.92 593357.59 13.23 2021-02-13T00:54:19Z- LcmTrn::publishEstimates() - MMSE : 4060340.42 593337.09 13.23 2021-02-13T00:54:19Z- LcmTrn::publishEstimates() - COVAR: 683.20 832.99 1.44 0.00 2021-02-13T00:54:20Z- AHRS_M2 msg: 1613177660.23 epoch sec; seqNo:16658 2021-02-13T00:54:20Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-13T00:54:20Z- handleDepth()-Depth_Keller msg: 1613177660237 epoch sec; seqNo:16790; depth 13.12 2021-02-13T00:54:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:20Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:20Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:20Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:20Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:20Z- handleDvl() - RDI_Pathfinder msg: 1613177660.24 epoch sec; seqNo:8092 2021-02-13T00:54:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.10 , -0.03 2021-02-13T00:54:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177660.25 epoch sec; seqNo:16790; 4060340.80 north; 593337.09 east 2021-02-13T00:54:20Z- AHRS_M2 msg: 1613177660.65 epoch sec; seqNo:16659 2021-02-13T00:54:20Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-13T00:54:20Z- handleDepth()-Depth_Keller msg: 1613177660659 epoch sec; seqNo:16791; depth 13.00 2021-02-13T00:54:20Z- handleDvl() - RDI_Pathfinder msg: 1613177660.67 epoch sec; seqNo:8093 2021-02-13T00:54:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 100.35 , 112.52 , 78.71 2021-02-13T00:54:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.04 , -0.09 2021-02-13T00:54:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177660.68 epoch sec; seqNo:16791; 4060341.17 north; 593337.08 east 2021-02-13T00:54:21Z- AHRS_M2 msg: 1613177661.04 epoch sec; seqNo:16660 2021-02-13T00:54:21Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-13T00:54:21Z- handleDepth()-Depth_Keller msg: 1613177661089 epoch sec; seqNo:16792; depth 12.87 2021-02-13T00:54:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177661.11 epoch sec; seqNo:16792; 4060341.17 north; 593337.08 east 2021-02-13T00:54:21Z- AHRS_M2 msg: 1613177661.45 epoch sec; seqNo:16661 2021-02-13T00:54:21Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T00:54:21Z- handleDepth()-Depth_Keller msg: 1613177661451 epoch sec; seqNo:16793; depth 12.73 2021-02-13T00:54:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:21Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:21Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:21Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:21Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:21Z- handleDvl() - RDI_Pathfinder msg: 1613177661.45 epoch sec; seqNo:8094 2021-02-13T00:54:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.04 , -0.09 2021-02-13T00:54:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177661.46 epoch sec; seqNo:16793; 4060341.55 north; 593337.08 east 2021-02-13T00:54:21Z- AHRS_M2 msg: 1613177661.85 epoch sec; seqNo:16662 2021-02-13T00:54:21Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T00:54:21Z- handleDepth()-Depth_Keller msg: 1613177661857 epoch sec; seqNo:16794; depth 12.64 2021-02-13T00:54:21Z- handleDvl() - RDI_Pathfinder msg: 1613177661.87 epoch sec; seqNo:8095 2021-02-13T00:54:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.05 , 100.28 , 116.50 , 78.64 2021-02-13T00:54:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.06 , -0.04 2021-02-13T00:54:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177661.88 epoch sec; seqNo:16794; 4060341.93 north; 593337.07 east 2021-02-13T00:54:22Z- AHRS_M2 msg: 1613177662.25 epoch sec; seqNo:16663 2021-02-13T00:54:22Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-13T00:54:22Z- handleDepth()-Depth_Keller msg: 1613177662257 epoch sec; seqNo:16795; depth 12.51 2021-02-13T00:54:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177662.27 epoch sec; seqNo:16795; 4060341.93 north; 593337.07 east 2021-02-13T00:54:22Z- AHRS_M2 msg: 1613177662.65 epoch sec; seqNo:16664 2021-02-13T00:54:22Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-13T00:54:22Z- handleDepth()-Depth_Keller msg: 1613177662670 epoch sec; seqNo:16796; depth 12.38 2021-02-13T00:54:22Z- handleDvl() - RDI_Pathfinder msg: 1613177662.69 epoch sec; seqNo:8096 2021-02-13T00:54:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 100.14 , 112.31 , 74.38 2021-02-13T00:54:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , 0.06 2021-02-13T00:54:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177662.70 epoch sec; seqNo:16796; 4060342.32 north; 593338.29 east 2021-02-13T00:54:23Z- AHRS_M2 msg: 1613177663.09 epoch sec; seqNo:16665 2021-02-13T00:54:23Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.14 psi 2021-02-13T00:54:23Z- handleDepth()-Depth_Keller msg: 1613177663095 epoch sec; seqNo:16797; depth 12.25 2021-02-13T00:54:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177663.11 epoch sec; seqNo:16797; 4060342.70 north; 593338.29 east 2021-02-13T00:54:23Z- handleDepth()-Depth_Keller msg: 1613177663485 epoch sec; seqNo:16798; depth 12.13 2021-02-13T00:54:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177663.49 epoch sec; seqNo:16798; 4060343.08 north; 593338.28 east 2021-02-13T00:54:23Z- AHRS_M2 msg: 1613177663.88 epoch sec; seqNo:16666 2021-02-13T00:54:23Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.13 psi 2021-02-13T00:54:23Z- handleDepth()-Depth_Keller msg: 1613177663880 epoch sec; seqNo:16799; depth 12.03 2021-02-13T00:54:23Z- handleDvl() - RDI_Pathfinder msg: 1613177663.89 epoch sec; seqNo:8097 2021-02-13T00:54:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.47 , 99.00 , 111.16 , 78.71 2021-02-13T00:54:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.02 , 0.12 2021-02-13T00:54:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177663.90 epoch sec; seqNo:16799; 4060343.46 north; 593338.28 east 2021-02-13T00:54:24Z- AHRS_M2 msg: 1613177664.27 epoch sec; seqNo:16667 2021-02-13T00:54:24Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.13 psi 2021-02-13T00:54:24Z- handleDepth()-Depth_Keller msg: 1613177664286 epoch sec; seqNo:16800; depth 11.90 2021-02-13T00:54:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177664.30 epoch sec; seqNo:16800; 4060343.46 north; 593338.28 east 2021-02-13T00:54:24Z- AHRS_M2 msg: 1613177664.67 epoch sec; seqNo:16668 2021-02-13T00:54:24Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.13 psi 2021-02-13T00:54:24Z- handleDepth()-Depth_Keller msg: 1613177664679 epoch sec; seqNo:16801; depth 11.78 2021-02-13T00:54:24Z- handleDvl() - RDI_Pathfinder msg: 1613177664.69 epoch sec; seqNo:8098 2021-02-13T00:54:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.76 , 97.57 , 113.80 , 78.64 2021-02-13T00:54:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.05 , 0.10 2021-02-13T00:54:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177664.71 epoch sec; seqNo:16801; 4060343.84 north; 593338.27 east 2021-02-13T00:54:25Z- LcmTrn::handleMessages() - handling 42 messages took 0.68 ms and 129 calls... 2021-02-13T00:54:25Z- waiting for 1613177662.87, time is 1613177664.71 2021-02-13T00:54:25Z- LcmTrn::updateTrn() equating pose 1613177664.71 and meas times 1613177664.71 2021-02-13T00:54:25Z- LcmTrn::updateTrn() >>>> pose time 1613177664.71, meas time 1613177664.71 <<<< 2021-02-13T00:54:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177664.71 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:54:25Z- TerrainNav::motionUpdate() - currEstimate = (4060344, 593338.27, 11.78) 2021-02-13T00:54:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:54:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:54:25Z- TNavBF::Projecting Measurements 2021-02-13T00:54:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:54:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:54:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:54:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:54:25Z- TNavFilter::4 good beams. 2021-02-13T00:54:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:54:25Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:54:25Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:54:25Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:54:25Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:54:25Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:54:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:54:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:54:25Z- TerrainMapOctree: (vehN, vehE ) = (4060343.84, 593338.27). 2021-02-13T00:54:25Z- TerrainMapOctree: Min Distance = 1285.45. 2021-02-13T00:54:25Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:54:25Z- There is no specific good beam that all particles have in common. 2021-02-13T00:54:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:54:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.63 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 12.04 2021-02-13T00:54:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:54:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.45 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 15.23 2021-02-13T00:54:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:54:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 20.72 2021-02-13T00:54:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:54:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 9.90 2021-02-13T00:54:25Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:54:25Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:54:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:54:25Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:54:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:54:25Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:54:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:54:25Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:54:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:54:25Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:54:25Z- Filter 0 No beams used in subcloud update 2021-02-13T00:54:25Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:54:25Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:54:25Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 683.2 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:54:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177664.71 sec. 2021-02-13T00:54:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:54:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:54:25Z- variances([0], [2], and psi_berg) = 683.160034 832.989842 0.000000 2021-02-13T00:54:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:54:25Z- LcmTrn::publishEstimates() - MLE : 4060343.34 593358.77 11.78 2021-02-13T00:54:25Z- LcmTrn::publishEstimates() - MMSE : 4060343.84 593338.27 11.78 2021-02-13T00:54:25Z- LcmTrn::publishEstimates() - COVAR: 683.16 832.99 1.14 0.00 2021-02-13T00:54:25Z- AHRS_M2 msg: 1613177665.08 epoch sec; seqNo:16669 2021-02-13T00:54:25Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-13T00:54:25Z- handleDepth()-Depth_Keller msg: 1613177665084 epoch sec; seqNo:16802; depth 11.67 2021-02-13T00:54:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:25Z- handleDvl() - RDI_Pathfinder msg: 1613177665.09 epoch sec; seqNo:8099 2021-02-13T00:54:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.05 , 0.10 2021-02-13T00:54:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177665.10 epoch sec; seqNo:16802; 4060344.23 north; 593339.49 east 2021-02-13T00:54:25Z- AHRS_M2 msg: 1613177665.49 epoch sec; seqNo:16670 2021-02-13T00:54:25Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-13T00:54:25Z- handleDepth()-Depth_Keller msg: 1613177665491 epoch sec; seqNo:16803; depth 11.56 2021-02-13T00:54:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177665.50 epoch sec; seqNo:16803; 4060344.61 north; 593339.49 east 2021-02-13T00:54:25Z- AHRS_M2 msg: 1613177665.89 epoch sec; seqNo:16671 2021-02-13T00:54:25Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.13 psi 2021-02-13T00:54:25Z- handleDepth()-Depth_Keller msg: 1613177665892 epoch sec; seqNo:16804; depth 11.45 2021-02-13T00:54:25Z- handleDvl() - RDI_Pathfinder msg: 1613177665.90 epoch sec; seqNo:8100 2021-02-13T00:54:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:25Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:54:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177665.93 epoch sec; seqNo:16804; 4060344.99 north; 593339.48 east 2021-02-13T00:54:26Z- AHRS_M2 msg: 1613177666.29 epoch sec; seqNo:16672 2021-02-13T00:54:26Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.14 psi 2021-02-13T00:54:26Z- handleDepth()-Depth_Keller msg: 1613177666296 epoch sec; seqNo:16805; depth 11.31 2021-02-13T00:54:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:26Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:26Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:26Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:26Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:26Z- handleDvl() - RDI_Pathfinder msg: 1613177666.30 epoch sec; seqNo:8101 2021-02-13T00:54:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:26Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:54:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177666.31 epoch sec; seqNo:16805; 4060344.99 north; 593339.48 east 2021-02-13T00:54:26Z- AHRS_M2 msg: 1613177666.70 epoch sec; seqNo:16673 2021-02-13T00:54:26Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.14 psi 2021-02-13T00:54:26Z- handleDepth()-Depth_Keller msg: 1613177666709 epoch sec; seqNo:16806; depth 11.20 2021-02-13T00:54:26Z- handleDvl() - RDI_Pathfinder msg: 1613177666.72 epoch sec; seqNo:8102 2021-02-13T00:54:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:26Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:54:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177666.73 epoch sec; seqNo:16806; 4060345.37 north; 593339.48 east 2021-02-13T00:54:27Z- AHRS_M2 msg: 1613177667.10 epoch sec; seqNo:16674 2021-02-13T00:54:27Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.15 psi 2021-02-13T00:54:27Z- handleDepth()-Depth_Keller msg: 1613177667106 epoch sec; seqNo:16807; depth 11.08 2021-02-13T00:54:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177667.12 epoch sec; seqNo:16807; 4060345.76 north; 593340.69 east 2021-02-13T00:54:27Z- AHRS_M2 msg: 1613177667.50 epoch sec; seqNo:16675 2021-02-13T00:54:27Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-13T00:54:27Z- handleDepth()-Depth_Keller msg: 1613177667508 epoch sec; seqNo:16808; depth 10.98 2021-02-13T00:54:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:27Z- handleDvl() - RDI_Pathfinder msg: 1613177667.51 epoch sec; seqNo:8103 2021-02-13T00:54:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:27Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:54:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177667.53 epoch sec; seqNo:16808; 4060346.14 north; 593340.69 east 2021-02-13T00:54:27Z- AHRS_M2 msg: 1613177667.91 epoch sec; seqNo:16676 2021-02-13T00:54:27Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-13T00:54:27Z- handleDepth()-Depth_Keller msg: 1613177667917 epoch sec; seqNo:16809; depth 10.84 2021-02-13T00:54:27Z- handleDvl() - RDI_Pathfinder msg: 1613177667.93 epoch sec; seqNo:8104 2021-02-13T00:54:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 98.79 , 115.15 , 0.00 2021-02-13T00:54:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , -0.06 2021-02-13T00:54:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177667.96 epoch sec; seqNo:16809; 4060346.14 north; 593340.69 east 2021-02-13T00:54:28Z- AHRS_M2 msg: 1613177668.31 epoch sec; seqNo:16677 2021-02-13T00:54:28Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T00:54:28Z- handleDepth()-Depth_Keller msg: 1613177668321 epoch sec; seqNo:16810; depth 10.73 2021-02-13T00:54:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177668.33 epoch sec; seqNo:16810; 4060346.52 north; 593340.69 east 2021-02-13T00:54:28Z- AHRS_M2 msg: 1613177668.73 epoch sec; seqNo:16678 2021-02-13T00:54:28Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-13T00:54:28Z- handleDepth()-Depth_Keller msg: 1613177668730 epoch sec; seqNo:16811; depth 10.61 2021-02-13T00:54:28Z- handleDvl() - RDI_Pathfinder msg: 1613177668.74 epoch sec; seqNo:8105 2021-02-13T00:54:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 101.09 , 115.96 , 157.88 2021-02-13T00:54:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , 0.02 , -0.06 2021-02-13T00:54:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177668.75 epoch sec; seqNo:16811; 4060346.90 north; 593340.68 east 2021-02-13T00:54:29Z- AHRS_M2 msg: 1613177669.12 epoch sec; seqNo:16679 2021-02-13T00:54:29Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-13T00:54:29Z- handleDepth()-Depth_Keller msg: 1613177669126 epoch sec; seqNo:16812; depth 10.48 2021-02-13T00:54:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177669.15 epoch sec; seqNo:16812; 4060346.90 north; 593340.68 east 2021-02-13T00:54:29Z- AHRS_M2 msg: 1613177669.52 epoch sec; seqNo:16680 2021-02-13T00:54:29Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T00:54:29Z- handleDepth()-Depth_Keller msg: 1613177669528 epoch sec; seqNo:16813; depth 10.37 2021-02-13T00:54:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177669.54 epoch sec; seqNo:16813; 4060347.29 north; 593341.90 east 2021-02-13T00:54:29Z- AHRS_M2 msg: 1613177669.93 epoch sec; seqNo:16681 2021-02-13T00:54:29Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T00:54:29Z- handleDepth()-Depth_Keller msg: 1613177669932 epoch sec; seqNo:16814; depth 10.27 2021-02-13T00:54:29Z- handleDvl() - RDI_Pathfinder msg: 1613177669.94 epoch sec; seqNo:8106 2021-02-13T00:54:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.82 , 106.16 , 115.08 , 79.39 2021-02-13T00:54:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.09 , -0.02 2021-02-13T00:54:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177669.95 epoch sec; seqNo:16814; 4060347.29 north; 593341.90 east 2021-02-13T00:54:30Z- LcmTrn::handleMessages() - handling 47 messages took 0.65 ms and 134 calls... 2021-02-13T00:54:30Z- waiting for 1613177667.71, time is 1613177669.95 2021-02-13T00:54:30Z- LcmTrn::updateTrn() equating pose 1613177669.95 and meas times 1613177669.95 2021-02-13T00:54:30Z- LcmTrn::updateTrn() >>>> pose time 1613177669.95, meas time 1613177669.95 <<<< 2021-02-13T00:54:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177669.95 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:54:30Z- TerrainNav::motionUpdate() - currEstimate = (4060347, 593341.90, 10.27) 2021-02-13T00:54:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:54:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:54:30Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177669.95 sec.; No relevant navigation data available 2021-02-13T00:54:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:54:30Z- TNavBF::mmse selected from filter -1 2021-02-13T00:54:30Z- variances([0], [2], and psi_berg) = 683.109316 832.989820 0.000000 2021-02-13T00:54:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:54:30Z- LcmTrn::publishEstimates() - MLE : 4060346.79 593362.40 10.27 2021-02-13T00:54:30Z- LcmTrn::publishEstimates() - MMSE : 4060347.29 593341.90 10.27 2021-02-13T00:54:30Z- LcmTrn::publishEstimates() - COVAR: 683.11 832.99 1.39 0.00 2021-02-13T00:54:30Z- AHRS_M2 msg: 1613177670.33 epoch sec; seqNo:16682 2021-02-13T00:54:30Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-13T00:54:30Z- handleDepth()-Depth_Keller msg: 1613177670338 epoch sec; seqNo:16815; depth 10.14 2021-02-13T00:54:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177670.35 epoch sec; seqNo:16815; 4060347.67 north; 593341.89 east 2021-02-13T00:54:30Z- AHRS_M2 msg: 1613177670.74 epoch sec; seqNo:16683 2021-02-13T00:54:30Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-13T00:54:30Z- handleDepth()-Depth_Keller msg: 1613177670750 epoch sec; seqNo:16816; depth 10.01 2021-02-13T00:54:30Z- handleDvl() - RDI_Pathfinder msg: 1613177670.76 epoch sec; seqNo:8107 2021-02-13T00:54:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.71 , 114.88 , 79.73 2021-02-13T00:54:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.13 , -0.12 2021-02-13T00:54:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177670.77 epoch sec; seqNo:16816; 4060348.05 north; 593341.89 east 2021-02-13T00:54:31Z- AHRS_M2 msg: 1613177671.14 epoch sec; seqNo:16684 2021-02-13T00:54:31Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.18 psi 2021-02-13T00:54:31Z- handleDepth()-Depth_Keller msg: 1613177671147 epoch sec; seqNo:16817; depth 9.89 2021-02-13T00:54:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:31Z- handleDvl() - RDI_Pathfinder msg: 1613177671.15 epoch sec; seqNo:8108 2021-02-13T00:54:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.13 , -0.12 2021-02-13T00:54:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177671.18 epoch sec; seqNo:16817; 4060348.05 north; 593341.89 east 2021-02-13T00:54:31Z- AHRS_M2 msg: 1613177671.54 epoch sec; seqNo:16685 2021-02-13T00:54:31Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-13T00:54:31Z- handleDepth()-Depth_Keller msg: 1613177671548 epoch sec; seqNo:16818; depth 9.80 2021-02-13T00:54:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177671.56 epoch sec; seqNo:16818; 4060348.43 north; 593341.89 east 2021-02-13T00:54:31Z- AHRS_M2 msg: 1613177671.95 epoch sec; seqNo:16686 2021-02-13T00:54:31Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.20 psi 2021-02-13T00:54:31Z- handleDepth()-Depth_Keller msg: 1613177671955 epoch sec; seqNo:16819; depth 9.69 2021-02-13T00:54:31Z- handleDvl() - RDI_Pathfinder msg: 1613177671.97 epoch sec; seqNo:8109 2021-02-13T00:54:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.28 , 105.21 , 114.68 , 79.52 2021-02-13T00:54:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.12 2021-02-13T00:54:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177671.98 epoch sec; seqNo:16819; 4060348.82 north; 593343.10 east 2021-02-13T00:54:32Z- AHRS_M2 msg: 1613177672.35 epoch sec; seqNo:16687 2021-02-13T00:54:32Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.21 psi 2021-02-13T00:54:32Z- handleDepth()-Depth_Keller msg: 1613177672359 epoch sec; seqNo:16820; depth 9.56 2021-02-13T00:54:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:32Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:32Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:32Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:32Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:32Z- handleDvl() - RDI_Pathfinder msg: 1613177672.36 epoch sec; seqNo:8110 2021-02-13T00:54:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.12 2021-02-13T00:54:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177672.37 epoch sec; seqNo:16820; 4060348.82 north; 593343.10 east 2021-02-13T00:54:32Z- AHRS_M2 msg: 1613177672.76 epoch sec; seqNo:16688 2021-02-13T00:54:32Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.21 psi 2021-02-13T00:54:32Z- handleDepth()-Depth_Keller msg: 1613177672769 epoch sec; seqNo:16821; depth 9.46 2021-02-13T00:54:32Z- handleDvl() - RDI_Pathfinder msg: 1613177672.78 epoch sec; seqNo:8111 2021-02-13T00:54:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 104.07 , 118.94 , 79.73 2021-02-13T00:54:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.14 , 0.08 2021-02-13T00:54:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177672.79 epoch sec; seqNo:16821; 4060349.20 north; 593343.10 east 2021-02-13T00:54:33Z- AHRS_M2 msg: 1613177673.16 epoch sec; seqNo:16689 2021-02-13T00:54:33Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.20 psi 2021-02-13T00:54:33Z- handleDepth()-Depth_Keller msg: 1613177673167 epoch sec; seqNo:16822; depth 9.35 2021-02-13T00:54:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177673.18 epoch sec; seqNo:16822; 4060349.58 north; 593343.09 east 2021-02-13T00:54:33Z- AHRS_M2 msg: 1613177673.56 epoch sec; seqNo:16690 2021-02-13T00:54:33Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.19 psi 2021-02-13T00:54:33Z- handleDepth()-Depth_Keller msg: 1613177673568 epoch sec; seqNo:16823; depth 9.23 2021-02-13T00:54:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:33Z- handleDvl() - RDI_Pathfinder msg: 1613177673.57 epoch sec; seqNo:8112 2021-02-13T00:54:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.14 , 0.08 2021-02-13T00:54:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177673.59 epoch sec; seqNo:16823; 4060349.95 north; 593343.09 east 2021-02-13T00:54:33Z- AHRS_M2 msg: 1613177673.97 epoch sec; seqNo:16691 2021-02-13T00:54:33Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.17 psi 2021-02-13T00:54:33Z- handleDepth()-Depth_Keller msg: 1613177673977 epoch sec; seqNo:16824; depth 9.13 2021-02-13T00:54:33Z- handleDvl() - RDI_Pathfinder msg: 1613177673.99 epoch sec; seqNo:8113 2021-02-13T00:54:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 106.77 , 116.24 , 79.73 2021-02-13T00:54:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.06 , 0.21 2021-02-13T00:54:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177674.01 epoch sec; seqNo:16824; 4060350.33 north; 593343.09 east 2021-02-13T00:54:34Z- AHRS_M2 msg: 1613177674.40 epoch sec; seqNo:16692 2021-02-13T00:54:34Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.16 psi 2021-02-13T00:54:34Z- handleDepth()-Depth_Keller msg: 1613177674410 epoch sec; seqNo:16825; depth 9.02 2021-02-13T00:54:34Z- handleDvl() - RDI_Pathfinder msg: 1613177674.43 epoch sec; seqNo:8114 2021-02-13T00:54:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:34Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:54:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177674.44 epoch sec; seqNo:16825; 4060350.73 north; 593344.30 east 2021-02-13T00:54:34Z- AHRS_M2 msg: 1613177674.78 epoch sec; seqNo:16693 2021-02-13T00:54:34Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T00:54:34Z- handleDepth()-Depth_Keller msg: 1613177674784 epoch sec; seqNo:16826; depth 8.90 2021-02-13T00:54:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:34Z- handleDvl() - RDI_Pathfinder msg: 1613177674.79 epoch sec; seqNo:8115 2021-02-13T00:54:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:34Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:54:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177674.80 epoch sec; seqNo:16826; 4060350.73 north; 593344.30 east 2021-02-13T00:54:35Z- LcmTrn::handleMessages() - handling 45 messages took 0.68 ms and 132 calls... 2021-02-13T00:54:35Z- waiting for 1613177672.95, time is 1613177674.80 2021-02-13T00:54:35Z- LcmTrn::updateTrn() equating pose 1613177674.80 and meas times 1613177674.80 2021-02-13T00:54:35Z- LcmTrn::updateTrn() >>>> pose time 1613177674.80, meas time 1613177674.80 <<<< 2021-02-13T00:54:35Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177674.80, ping # 0. 2021-02-13T00:54:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177674.80 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:54:35Z- TerrainNav::motionUpdate() - currEstimate = (4060351, 593344.30, 8.90) 2021-02-13T00:54:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:54:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:54:35Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:54:35Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:54:35Z- TNavBF::Projecting Measurements 2021-02-13T00:54:35Z- TNavFilter::Measurement from time = 1613177674.80 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:54:35Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:54:35Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:54:35Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:54:35Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:54:35Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:54:35Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:54:35Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:54:35Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 683.1 EastVariance: 833.0 DepthVariance: 1.6 this->DepthBiasCov: 1.6 2021-02-13T00:54:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:54:35Z- TNavBF::mmse selected from filter -1 2021-02-13T00:54:35Z- variances([0], [2], and psi_berg) = 683.062908 832.989800 0.000000 2021-02-13T00:54:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:54:35Z- LcmTrn::publishEstimates() - MLE : 4060350.23 593364.80 8.90 2021-02-13T00:54:35Z- LcmTrn::publishEstimates() - MMSE : 4060350.73 593344.30 8.90 2021-02-13T00:54:35Z- LcmTrn::publishEstimates() - COVAR: 683.06 832.99 1.61 0.00 2021-02-13T00:54:35Z- AHRS_M2 msg: 1613177675.18 epoch sec; seqNo:16694 2021-02-13T00:54:35Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.14 psi 2021-02-13T00:54:35Z- handleDepth()-Depth_Keller msg: 1613177675188 epoch sec; seqNo:16827; depth 8.81 2021-02-13T00:54:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177675.20 epoch sec; seqNo:16827; 4060351.10 north; 593344.30 east 2021-02-13T00:54:35Z- AHRS_M2 msg: 1613177675.59 epoch sec; seqNo:16695 2021-02-13T00:54:35Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.14 psi 2021-02-13T00:54:35Z- handleDepth()-Depth_Keller msg: 1613177675593 epoch sec; seqNo:16828; depth 8.69 2021-02-13T00:54:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177675.61 epoch sec; seqNo:16828; 4060351.48 north; 593344.29 east 2021-02-13T00:54:35Z- AHRS_M2 msg: 1613177675.99 epoch sec; seqNo:16696 2021-02-13T00:54:35Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-13T00:54:35Z- handleDepth()-Depth_Keller msg: 1613177675997 epoch sec; seqNo:16829; depth 8.58 2021-02-13T00:54:36Z- handleDvl() - RDI_Pathfinder msg: 1613177676.01 epoch sec; seqNo:8116 2021-02-13T00:54:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.12 , 102.65 , 118.87 , 81.01 2021-02-13T00:54:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.22 , -0.19 2021-02-13T00:54:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177676.02 epoch sec; seqNo:16829; 4060351.86 north; 593344.29 east 2021-02-13T00:54:36Z- AHRS_M2 msg: 1613177676.42 epoch sec; seqNo:16697 2021-02-13T00:54:36Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-13T00:54:36Z- handleDepth()-Depth_Keller msg: 1613177676427 epoch sec; seqNo:16830; depth 8.46 2021-02-13T00:54:36Z- handleDvl() - RDI_Pathfinder msg: 1613177676.44 epoch sec; seqNo:8117 2021-02-13T00:54:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:36Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:54:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177676.46 epoch sec; seqNo:16830; 4060351.87 north; 593345.51 east 2021-02-13T00:54:36Z- AHRS_M2 msg: 1613177676.80 epoch sec; seqNo:16698 2021-02-13T00:54:36Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-13T00:54:36Z- handleDepth()-Depth_Keller msg: 1613177676804 epoch sec; seqNo:16831; depth 8.31 2021-02-13T00:54:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177676.82 epoch sec; seqNo:16831; 4060351.87 north; 593345.51 east 2021-02-13T00:54:37Z- AHRS_M2 msg: 1613177677.20 epoch sec; seqNo:16699 2021-02-13T00:54:37Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-13T00:54:37Z- handleDepth()-Depth_Keller msg: 1613177677208 epoch sec; seqNo:16832; depth 8.20 2021-02-13T00:54:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:37Z- handleDvl() - RDI_Pathfinder msg: 1613177677.21 epoch sec; seqNo:8118 2021-02-13T00:54:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:37Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:54:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177677.22 epoch sec; seqNo:16832; 4060352.25 north; 593345.51 east 2021-02-13T00:54:37Z- AHRS_M2 msg: 1613177677.61 epoch sec; seqNo:16700 2021-02-13T00:54:37Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.19 psi 2021-02-13T00:54:37Z- handleDepth()-Depth_Keller msg: 1613177677619 epoch sec; seqNo:16833; depth 8.10 2021-02-13T00:54:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177677.63 epoch sec; seqNo:16833; 4060352.25 north; 593345.51 east 2021-02-13T00:54:38Z- AHRS_M2 msg: 1613177678.01 epoch sec; seqNo:16701 2021-02-13T00:54:38Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.19 psi 2021-02-13T00:54:38Z- handleDepth()-Depth_Keller msg: 1613177678016 epoch sec; seqNo:16834; depth 7.97 2021-02-13T00:54:38Z- handleDvl() - RDI_Pathfinder msg: 1613177678.04 epoch sec; seqNo:8119 2021-02-13T00:54:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 106.70 , 116.17 , 81.01 2021-02-13T00:54:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.22 , 0.04 2021-02-13T00:54:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177678.05 epoch sec; seqNo:16834; 4060352.63 north; 593345.50 east 2021-02-13T00:54:38Z- handleDepth()-Depth_Keller msg: 1613177678437 epoch sec; seqNo:16835; depth 7.84 2021-02-13T00:54:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:38Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:38Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:38Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:38Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:38Z- handleDvl() - RDI_Pathfinder msg: 1613177678.44 epoch sec; seqNo:8120 2021-02-13T00:54:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.22 , 0.04 2021-02-13T00:54:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177678.45 epoch sec; seqNo:16835; 4060353.01 north; 593345.50 east 2021-02-13T00:54:38Z- AHRS_M2 msg: 1613177678.82 epoch sec; seqNo:16702 2021-02-13T00:54:38Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.18 psi 2021-02-13T00:54:38Z- handleDepth()-Depth_Keller msg: 1613177678833 epoch sec; seqNo:16836; depth 7.73 2021-02-13T00:54:38Z- handleDvl() - RDI_Pathfinder msg: 1613177678.85 epoch sec; seqNo:8121 2021-02-13T00:54:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.42 , 118.80 , 81.62 2021-02-13T00:54:38Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.10 , 0.20 2021-02-13T00:54:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177678.86 epoch sec; seqNo:16836; 4060353.39 north; 593345.49 east 2021-02-13T00:54:39Z- AHRS_M2 msg: 1613177679.22 epoch sec; seqNo:16703 2021-02-13T00:54:39Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.17 psi 2021-02-13T00:54:39Z- handleDepth()-Depth_Keller msg: 1613177679234 epoch sec; seqNo:16837; depth 7.62 2021-02-13T00:54:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177679.24 epoch sec; seqNo:16837; 4060353.78 north; 593346.71 east 2021-02-13T00:54:39Z- AHRS_M2 msg: 1613177679.66 epoch sec; seqNo:16704 2021-02-13T00:54:39Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.14 psi 2021-02-13T00:54:39Z- handleDepth()-Depth_Keller msg: 1613177679660 epoch sec; seqNo:16838; depth 7.53 2021-02-13T00:54:39Z- handleDvl() - RDI_Pathfinder msg: 1613177679.67 epoch sec; seqNo:8122 2021-02-13T00:54:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 105.35 , 120.22 , 0.00 2021-02-13T00:54:39Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , 0.15 , 0.14 2021-02-13T00:54:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177679.68 epoch sec; seqNo:16838; 4060354.16 north; 593346.71 east 2021-02-13T00:54:40Z- handleDepth()-Depth_Keller msg: 1613177680050 epoch sec; seqNo:16839; depth 7.39 2021-02-13T00:54:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177680.07 epoch sec; seqNo:16839; 4060354.54 north; 593346.70 east 2021-02-13T00:54:40Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-13T00:54:40Z- waiting for 1613177677.80, time is 1613177680.07 2021-02-13T00:54:40Z- LcmTrn::updateTrn() >>>> pose time 1613177680.07, meas time 1613177679.67 <<<< 2021-02-13T00:54:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177679.67 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:54:40Z- TerrainNav::motionUpdate() - currEstimate = (4060355, 593346.70, 7.39) 2021-02-13T00:54:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:54:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:54:40Z- TNavBF::Projecting Measurements 2021-02-13T00:54:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:54:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:54:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:54:40Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:54:40Z- TNavFilter::3 good beams. 2021-02-13T00:54:40Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:54:40Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:54:40Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:54:40Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:54:40Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:54:40Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:54:40Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:54:40Z- TNavBF::mmse selected from filter -1 2021-02-13T00:54:40Z- TerrainMapOctree: (vehN, vehE ) = (4060354.25, 593346.52). 2021-02-13T00:54:40Z- TerrainMapOctree: Min Distance = 1272.26. 2021-02-13T00:54:40Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:54:40Z- There is no specific good beam that all particles have in common. 2021-02-13T00:54:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:54:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.82 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 12.67 2021-02-13T00:54:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:54:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.62 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 17.76 2021-02-13T00:54:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:54:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.49 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 23.13 2021-02-13T00:54:40Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:54:40Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:54:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:54:40Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:54:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:54:40Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:54:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:54:40Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:54:40Z- Filter 0 No beams used in subcloud update 2021-02-13T00:54:40Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:54:40Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:54:40Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 683.0 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T00:54:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177679.67 sec. 2021-02-13T00:54:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:54:40Z- TNavBF::mmse selected from filter -1 2021-02-13T00:54:40Z- variances([0], [2], and psi_berg) = 683.003472 832.989774 0.000000 2021-02-13T00:54:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:54:40Z- LcmTrn::publishEstimates() - MLE : 4060354.04 593366.20 7.39 2021-02-13T00:54:40Z- LcmTrn::publishEstimates() - MMSE : 4060354.54 593346.70 7.39 2021-02-13T00:54:40Z- LcmTrn::publishEstimates() - COVAR: 683.00 832.99 1.43 0.00 2021-02-13T00:54:40Z- AHRS_M2 msg: 1613177680.45 epoch sec; seqNo:16705 2021-02-13T00:54:40Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.14 psi 2021-02-13T00:54:40Z- handleDepth()-Depth_Keller msg: 1613177680459 epoch sec; seqNo:16840; depth 7.31 2021-02-13T00:54:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177680.47 epoch sec; seqNo:16840; 4060354.54 north; 593346.70 east 2021-02-13T00:54:40Z- AHRS_M2 msg: 1613177680.85 epoch sec; seqNo:16706 2021-02-13T00:54:40Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-13T00:54:40Z- handleDepth()-Depth_Keller msg: 1613177680857 epoch sec; seqNo:16841; depth 7.19 2021-02-13T00:54:40Z- handleDvl() - RDI_Pathfinder msg: 1613177680.87 epoch sec; seqNo:8123 2021-02-13T00:54:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 103.66 , 121.51 , 81.35 2021-02-13T00:54:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.22 , -0.09 2021-02-13T00:54:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177680.88 epoch sec; seqNo:16841; 4060354.93 north; 593347.92 east 2021-02-13T00:54:41Z- AHRS_M2 msg: 1613177681.25 epoch sec; seqNo:16707 2021-02-13T00:54:41Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:54:41Z- handleDepth()-Depth_Keller msg: 1613177681262 epoch sec; seqNo:16842; depth 7.08 2021-02-13T00:54:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177681.27 epoch sec; seqNo:16842; 4060355.31 north; 593347.91 east 2021-02-13T00:54:41Z- AHRS_M2 msg: 1613177681.65 epoch sec; seqNo:16708 2021-02-13T00:54:41Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:54:41Z- handleDepth()-Depth_Keller msg: 1613177681657 epoch sec; seqNo:16843; depth 6.97 2021-02-13T00:54:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177681.66 epoch sec; seqNo:16843; 4060355.31 north; 593347.91 east 2021-02-13T00:54:42Z- AHRS_M2 msg: 1613177682.05 epoch sec; seqNo:16709 2021-02-13T00:54:42Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-13T00:54:42Z- handleDepth()-Depth_Keller msg: 1613177682056 epoch sec; seqNo:16844; depth 6.86 2021-02-13T00:54:42Z- handleDvl() - RDI_Pathfinder msg: 1613177682.07 epoch sec; seqNo:8124 2021-02-13T00:54:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.21 , 105.08 , 118.60 , 82.09 2021-02-13T00:54:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.08 , -0.19 2021-02-13T00:54:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177682.08 epoch sec; seqNo:16844; 4060355.69 north; 593347.91 east 2021-02-13T00:54:42Z- AHRS_M2 msg: 1613177682.46 epoch sec; seqNo:16710 2021-02-13T00:54:42Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-13T00:54:42Z- handleDepth()-Depth_Keller msg: 1613177682465 epoch sec; seqNo:16845; depth 6.74 2021-02-13T00:54:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177682.48 epoch sec; seqNo:16845; 4060356.07 north; 593347.91 east 2021-02-13T00:54:42Z- AHRS_M2 msg: 1613177682.86 epoch sec; seqNo:16711 2021-02-13T00:54:42Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.16 psi 2021-02-13T00:54:42Z- handleDepth()-Depth_Keller msg: 1613177682869 epoch sec; seqNo:16846; depth 6.64 2021-02-13T00:54:42Z- handleDvl() - RDI_Pathfinder msg: 1613177682.88 epoch sec; seqNo:8125 2021-02-13T00:54:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 106.57 , 118.74 , 0.00 2021-02-13T00:54:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.03 , -0.10 2021-02-13T00:54:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177682.90 epoch sec; seqNo:16846; 4060356.07 north; 593347.91 east 2021-02-13T00:54:43Z- AHRS_M2 msg: 1613177683.26 epoch sec; seqNo:16712 2021-02-13T00:54:43Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-13T00:54:43Z- handleDepth()-Depth_Keller msg: 1613177683268 epoch sec; seqNo:16847; depth 6.53 2021-02-13T00:54:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:43Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:43Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:43Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:43Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:43Z- handleDvl() - RDI_Pathfinder msg: 1613177683.27 epoch sec; seqNo:8126 2021-02-13T00:54:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.03 , -0.10 2021-02-13T00:54:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177683.28 epoch sec; seqNo:16847; 4060356.46 north; 593349.12 east 2021-02-13T00:54:43Z- AHRS_M2 msg: 1613177683.67 epoch sec; seqNo:16713 2021-02-13T00:54:43Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.16 psi 2021-02-13T00:54:43Z- handleDepth()-Depth_Keller msg: 1613177683690 epoch sec; seqNo:16848; depth 6.43 2021-02-13T00:54:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177683.71 epoch sec; seqNo:16848; 4060356.84 north; 593349.12 east 2021-02-13T00:54:44Z- AHRS_M2 msg: 1613177684.07 epoch sec; seqNo:16714 2021-02-13T00:54:44Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.16 psi 2021-02-13T00:54:44Z- handleDepth()-Depth_Keller msg: 1613177684081 epoch sec; seqNo:16849; depth 6.32 2021-02-13T00:54:44Z- handleDvl() - RDI_Pathfinder msg: 1613177684.09 epoch sec; seqNo:8127 2021-02-13T00:54:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 105.15 , 120.02 , 82.84 2021-02-13T00:54:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.06 , -0.08 2021-02-13T00:54:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177684.11 epoch sec; seqNo:16849; 4060356.84 north; 593349.12 east 2021-02-13T00:54:44Z- AHRS_M2 msg: 1613177684.47 epoch sec; seqNo:16715 2021-02-13T00:54:44Z- AHRS_M2 msg: 0.63 phi; 0.29 theta; -0.18 psi 2021-02-13T00:54:44Z- handleDepth()-Depth_Keller msg: 1613177684485 epoch sec; seqNo:16850; depth 6.20 2021-02-13T00:54:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:44Z- handleDvl() - RDI_Pathfinder msg: 1613177684.49 epoch sec; seqNo:8128 2021-02-13T00:54:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.06 , -0.08 2021-02-13T00:54:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177684.50 epoch sec; seqNo:16850; 4060357.22 north; 593349.11 east 2021-02-13T00:54:44Z- AHRS_M2 msg: 1613177684.88 epoch sec; seqNo:16716 2021-02-13T00:54:44Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.18 psi 2021-02-13T00:54:44Z- handleDepth()-Depth_Keller msg: 1613177684886 epoch sec; seqNo:16851; depth 6.10 2021-02-13T00:54:44Z- handleDvl() - RDI_Pathfinder msg: 1613177684.90 epoch sec; seqNo:8129 2021-02-13T00:54:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 102.44 , 117.32 , 83.11 2021-02-13T00:54:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.09 , -0.02 2021-02-13T00:54:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177684.91 epoch sec; seqNo:16851; 4060357.60 north; 593349.11 east 2021-02-13T00:54:45Z- LcmTrn::handleMessages() - handling 43 messages took 0.62 ms and 131 calls... 2021-02-13T00:54:45Z- waiting for 1613177683.07, time is 1613177684.91 2021-02-13T00:54:45Z- LcmTrn::updateTrn() equating pose 1613177684.91 and meas times 1613177684.91 2021-02-13T00:54:45Z- LcmTrn::updateTrn() >>>> pose time 1613177684.91, meas time 1613177684.91 <<<< 2021-02-13T00:54:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177684.91 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:54:45Z- TerrainNav::motionUpdate() - currEstimate = (4060358, 593349.11, 6.10) 2021-02-13T00:54:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:54:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:54:45Z- TNavBF::Projecting Measurements 2021-02-13T00:54:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:54:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:54:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:54:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:54:45Z- TNavFilter::4 good beams. 2021-02-13T00:54:45Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:54:45Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:54:45Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:54:45Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:54:45Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:54:45Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:54:45Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:54:45Z- TNavBF::mmse selected from filter -1 2021-02-13T00:54:45Z- TerrainMapOctree: (vehN, vehE ) = (4060357.60, 593349.11). 2021-02-13T00:54:45Z- TerrainMapOctree: Min Distance = 1268.06. 2021-02-13T00:54:45Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:54:45Z- There is no specific good beam that all particles have in common. 2021-02-13T00:54:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:54:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.62 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 12.69 2021-02-13T00:54:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:54:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.46 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 16.79 2021-02-13T00:54:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:54:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.35 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 22.02 2021-02-13T00:54:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:54:45Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 11.05 2021-02-13T00:54:45Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:54:45Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:54:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:54:45Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:54:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:54:45Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:54:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:54:45Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:54:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:54:45Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:54:45Z- Filter 0 No beams used in subcloud update 2021-02-13T00:54:45Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:54:45Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:54:45Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 683.0 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:54:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177684.91 sec. 2021-02-13T00:54:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:54:45Z- TNavBF::mmse selected from filter -1 2021-02-13T00:54:45Z- variances([0], [2], and psi_berg) = 682.958814 832.989754 0.000000 2021-02-13T00:54:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:54:45Z- LcmTrn::publishEstimates() - MLE : 4060357.10 593331.61 6.10 2021-02-13T00:54:45Z- LcmTrn::publishEstimates() - MMSE : 4060357.60 593349.11 6.10 2021-02-13T00:54:45Z- LcmTrn::publishEstimates() - COVAR: 682.96 832.99 1.16 0.00 2021-02-13T00:54:45Z- AHRS_M2 msg: 1613177685.28 epoch sec; seqNo:16717 2021-02-13T00:54:45Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.19 psi 2021-02-13T00:54:45Z- handleDepth()-Depth_Keller msg: 1613177685288 epoch sec; seqNo:16852; depth 5.99 2021-02-13T00:54:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177685.31 epoch sec; seqNo:16852; 4060357.60 north; 593349.11 east 2021-02-13T00:54:45Z- AHRS_M2 msg: 1613177685.69 epoch sec; seqNo:16718 2021-02-13T00:54:45Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.19 psi 2021-02-13T00:54:45Z- handleDepth()-Depth_Keller msg: 1613177685692 epoch sec; seqNo:16853; depth 5.88 2021-02-13T00:54:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:45Z- handleDvl() - RDI_Pathfinder msg: 1613177685.69 epoch sec; seqNo:8130 2021-02-13T00:54:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.09 , -0.02 2021-02-13T00:54:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177685.71 epoch sec; seqNo:16853; 4060357.98 north; 593349.11 east 2021-02-13T00:54:46Z- AHRS_M2 msg: 1613177686.09 epoch sec; seqNo:16719 2021-02-13T00:54:46Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.18 psi 2021-02-13T00:54:46Z- handleDepth()-Depth_Keller msg: 1613177686096 epoch sec; seqNo:16854; depth 5.78 2021-02-13T00:54:46Z- handleDvl() - RDI_Pathfinder msg: 1613177686.11 epoch sec; seqNo:8131 2021-02-13T00:54:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 107.99 , 114.75 , 83.65 2021-02-13T00:54:46Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.08 , 0.19 2021-02-13T00:54:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177686.12 epoch sec; seqNo:16854; 4060358.37 north; 593350.32 east 2021-02-13T00:54:46Z- AHRS_M2 msg: 1613177686.50 epoch sec; seqNo:16720 2021-02-13T00:54:46Z- AHRS_M2 msg: 0.60 phi; 0.29 theta; -0.17 psi 2021-02-13T00:54:46Z- handleDepth()-Depth_Keller msg: 1613177686501 epoch sec; seqNo:16855; depth 5.67 2021-02-13T00:54:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177686.51 epoch sec; seqNo:16855; 4060358.75 north; 593350.32 east 2021-02-13T00:54:46Z- AHRS_M2 msg: 1613177686.90 epoch sec; seqNo:16721 2021-02-13T00:54:46Z- AHRS_M2 msg: 0.61 phi; 0.27 theta; -0.16 psi 2021-02-13T00:54:46Z- handleDepth()-Depth_Keller msg: 1613177686904 epoch sec; seqNo:16856; depth 5.58 2021-02-13T00:54:46Z- handleDvl() - RDI_Pathfinder msg: 1613177686.92 epoch sec; seqNo:8132 2021-02-13T00:54:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.34 , 107.92 , 117.38 , 83.58 2021-02-13T00:54:46Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , 0.18 , 0.19 2021-02-13T00:54:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177686.93 epoch sec; seqNo:16856; 4060359.13 north; 593350.31 east 2021-02-13T00:54:47Z- AHRS_M2 msg: 1613177687.30 epoch sec; seqNo:16722 2021-02-13T00:54:47Z- AHRS_M2 msg: 0.62 phi; 0.25 theta; -0.15 psi 2021-02-13T00:54:47Z- handleDepth()-Depth_Keller msg: 1613177687308 epoch sec; seqNo:16857; depth 5.47 2021-02-13T00:54:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177687.32 epoch sec; seqNo:16857; 4060359.13 north; 593350.31 east 2021-02-13T00:54:47Z- AHRS_M2 msg: 1613177687.71 epoch sec; seqNo:16723 2021-02-13T00:54:47Z- AHRS_M2 msg: 0.63 phi; 0.22 theta; -0.15 psi 2021-02-13T00:54:47Z- handleDepth()-Depth_Keller msg: 1613177687716 epoch sec; seqNo:16858; depth 5.36 2021-02-13T00:54:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177687.74 epoch sec; seqNo:16858; 4060359.52 north; 593351.53 east 2021-02-13T00:54:48Z- AHRS_M2 msg: 1613177688.13 epoch sec; seqNo:16724 2021-02-13T00:54:48Z- AHRS_M2 msg: 0.65 phi; 0.18 theta; -0.15 psi 2021-02-13T00:54:48Z- handleDepth()-Depth_Keller msg: 1613177688131 epoch sec; seqNo:16859; depth 5.25 2021-02-13T00:54:48Z- handleDvl() - RDI_Pathfinder msg: 1613177688.15 epoch sec; seqNo:8133 2021-02-13T00:54:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.03 , 105.15 , 114.61 , 83.51 2021-02-13T00:54:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.27 , -0.04 2021-02-13T00:54:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177688.18 epoch sec; seqNo:16859; 4060359.90 north; 593351.53 east 2021-02-13T00:54:48Z- AHRS_M2 msg: 1613177688.52 epoch sec; seqNo:16725 2021-02-13T00:54:48Z- AHRS_M2 msg: 0.67 phi; 0.14 theta; -0.16 psi 2021-02-13T00:54:48Z- handleDepth()-Depth_Keller msg: 1613177688521 epoch sec; seqNo:16860; depth 5.14 2021-02-13T00:54:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177688.55 epoch sec; seqNo:16860; 4060360.28 north; 593351.52 east 2021-02-13T00:54:48Z- AHRS_M2 msg: 1613177688.92 epoch sec; seqNo:16726 2021-02-13T00:54:48Z- AHRS_M2 msg: 0.68 phi; 0.10 theta; -0.16 psi 2021-02-13T00:54:48Z- handleDepth()-Depth_Keller msg: 1613177688924 epoch sec; seqNo:16861; depth 5.05 2021-02-13T00:54:48Z- handleDvl() - RDI_Pathfinder msg: 1613177688.94 epoch sec; seqNo:8134 2021-02-13T00:54:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.44 , 107.85 , 86.22 2021-02-13T00:54:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.19 , -0.12 2021-02-13T00:54:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177688.95 epoch sec; seqNo:16861; 4060360.28 north; 593351.52 east 2021-02-13T00:54:49Z- AHRS_M2 msg: 1613177689.33 epoch sec; seqNo:16727 2021-02-13T00:54:49Z- AHRS_M2 msg: 0.69 phi; 0.06 theta; -0.15 psi 2021-02-13T00:54:49Z- handleDepth()-Depth_Keller msg: 1613177689329 epoch sec; seqNo:16862; depth 4.98 2021-02-13T00:54:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:49Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:49Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:49Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:49Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:49Z- handleDvl() - RDI_Pathfinder msg: 1613177689.33 epoch sec; seqNo:8135 2021-02-13T00:54:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.19 , -0.12 2021-02-13T00:54:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177689.34 epoch sec; seqNo:16862; 4060360.67 north; 593352.74 east 2021-02-13T00:54:49Z- AHRS_M2 msg: 1613177689.73 epoch sec; seqNo:16728 2021-02-13T00:54:49Z- AHRS_M2 msg: 0.71 phi; 0.02 theta; -0.14 psi 2021-02-13T00:54:49Z- handleDepth()-Depth_Keller msg: 1613177689733 epoch sec; seqNo:16863; depth 4.90 2021-02-13T00:54:49Z- handleDvl() - RDI_Pathfinder msg: 1613177689.75 epoch sec; seqNo:8136 2021-02-13T00:54:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.51 , 99.81 , 97.10 2021-02-13T00:54:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.14 , -0.14 2021-02-13T00:54:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177689.76 epoch sec; seqNo:16863; 4060360.67 north; 593352.74 east 2021-02-13T00:54:50Z- AHRS_M2 msg: 1613177690.13 epoch sec; seqNo:16729 2021-02-13T00:54:50Z- AHRS_M2 msg: 0.71 phi; -0.03 theta; -0.15 psi 2021-02-13T00:54:50Z- handleDepth()-Depth_Keller msg: 1613177690141 epoch sec; seqNo:16864; depth 4.84 2021-02-13T00:54:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177690.15 epoch sec; seqNo:16864; 4060361.05 north; 593352.73 east 2021-02-13T00:54:50Z- LcmTrn::handleMessages() - handling 46 messages took 0.66 ms and 133 calls... 2021-02-13T00:54:50Z- waiting for 1613177687.91, time is 1613177690.15 2021-02-13T00:54:50Z- LcmTrn::updateTrn() >>>> pose time 1613177690.15, meas time 1613177689.75 <<<< 2021-02-13T00:54:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177689.75 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:54:50Z- TerrainNav::motionUpdate() - currEstimate = (4060361, 593352.73, 4.84) 2021-02-13T00:54:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:54:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:54:50Z- TNavBF::Projecting Measurements 2021-02-13T00:54:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:54:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:54:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:54:50Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:54:50Z- TNavFilter::3 good beams. 2021-02-13T00:54:50Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:54:50Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:54:50Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:54:50Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:54:50Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:54:50Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:54:50Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:54:50Z- TNavBF::mmse selected from filter -1 2021-02-13T00:54:50Z- TerrainMapOctree: (vehN, vehE ) = (4060360.78, 593352.45). 2021-02-13T00:54:50Z- TerrainMapOctree: Min Distance = 1263.45. 2021-02-13T00:54:50Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:54:50Z- There is no specific good beam that all particles have in common. 2021-02-13T00:54:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:54:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 16.81 2021-02-13T00:54:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:54:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 15.94 2021-02-13T00:54:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:54:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-13T00:54:50Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:54:50Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:54:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:54:50Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:54:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:54:50Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:54:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:54:50Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:54:50Z- Filter 0 No beams used in subcloud update 2021-02-13T00:54:50Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:54:50Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:54:50Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 682.9 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:54:50Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177689.75 sec. 2021-02-13T00:54:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:54:50Z- TNavBF::mmse selected from filter -1 2021-02-13T00:54:50Z- variances([0], [2], and psi_berg) = 682.896152 832.989727 0.000000 2021-02-13T00:54:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:54:50Z- LcmTrn::publishEstimates() - MLE : 4060360.55 593334.23 4.84 2021-02-13T00:54:50Z- LcmTrn::publishEstimates() - MMSE : 4060361.05 593352.73 4.84 2021-02-13T00:54:50Z- LcmTrn::publishEstimates() - COVAR: 682.90 832.99 1.15 0.00 2021-02-13T00:54:50Z- AHRS_M2 msg: 1613177690.54 epoch sec; seqNo:16730 2021-02-13T00:54:50Z- AHRS_M2 msg: 0.71 phi; -0.07 theta; -0.16 psi 2021-02-13T00:54:50Z- handleDepth()-Depth_Keller msg: 1613177690541 epoch sec; seqNo:16865; depth 4.80 2021-02-13T00:54:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177690.55 epoch sec; seqNo:16865; 4060361.05 north; 593352.73 east 2021-02-13T00:54:50Z- AHRS_M2 msg: 1613177690.94 epoch sec; seqNo:16731 2021-02-13T00:54:50Z- AHRS_M2 msg: 0.71 phi; -0.10 theta; -0.18 psi 2021-02-13T00:54:50Z- handleDepth()-Depth_Keller msg: 1613177690944 epoch sec; seqNo:16866; depth 4.75 2021-02-13T00:54:50Z- handleDvl() - RDI_Pathfinder msg: 1613177690.96 epoch sec; seqNo:8137 2021-02-13T00:54:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.22 , 101.09 , 92.98 , 97.03 2021-02-13T00:54:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , 0.06 , -0.19 2021-02-13T00:54:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177690.97 epoch sec; seqNo:16866; 4060361.43 north; 593352.73 east 2021-02-13T00:54:51Z- AHRS_M2 msg: 1613177691.34 epoch sec; seqNo:16732 2021-02-13T00:54:51Z- AHRS_M2 msg: 0.70 phi; -0.13 theta; -0.21 psi 2021-02-13T00:54:51Z- handleDepth()-Depth_Keller msg: 1613177691348 epoch sec; seqNo:16867; depth 4.72 2021-02-13T00:54:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177691.36 epoch sec; seqNo:16867; 4060361.80 north; 593352.73 east 2021-02-13T00:54:51Z- AHRS_M2 msg: 1613177691.75 epoch sec; seqNo:16733 2021-02-13T00:54:51Z- AHRS_M2 msg: 0.69 phi; -0.15 theta; -0.24 psi 2021-02-13T00:54:51Z- handleDepth()-Depth_Keller msg: 1613177691752 epoch sec; seqNo:16868; depth 4.69 2021-02-13T00:54:51Z- handleDvl() - RDI_Pathfinder msg: 1613177691.76 epoch sec; seqNo:8138 2021-02-13T00:54:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.34 , 106.97 , 97.51 , 102.92 2021-02-13T00:54:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.11 , -0.08 2021-02-13T00:54:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177691.77 epoch sec; seqNo:16868; 4060361.82 north; 593353.95 east 2021-02-13T00:54:52Z- AHRS_M2 msg: 1613177692.16 epoch sec; seqNo:16734 2021-02-13T00:54:52Z- AHRS_M2 msg: 0.68 phi; -0.17 theta; -0.27 psi 2021-02-13T00:54:52Z- handleDepth()-Depth_Keller msg: 1613177692160 epoch sec; seqNo:16869; depth 4.69 2021-02-13T00:54:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177692.17 epoch sec; seqNo:16869; 4060362.20 north; 593353.94 east 2021-02-13T00:54:52Z- AHRS_M2 msg: 1613177692.56 epoch sec; seqNo:16735 2021-02-13T00:54:52Z- AHRS_M2 msg: 0.67 phi; -0.19 theta; -0.30 psi 2021-02-13T00:54:52Z- handleDepth()-Depth_Keller msg: 1613177692560 epoch sec; seqNo:16870; depth 4.70 2021-02-13T00:54:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177692.58 epoch sec; seqNo:16870; 4060362.58 north; 593353.94 east 2021-02-13T00:54:52Z- AHRS_M2 msg: 1613177692.96 epoch sec; seqNo:16736 2021-02-13T00:54:52Z- AHRS_M2 msg: 0.66 phi; -0.20 theta; -0.31 psi 2021-02-13T00:54:52Z- handleDepth()-Depth_Keller msg: 1613177692963 epoch sec; seqNo:16871; depth 4.74 2021-02-13T00:54:52Z- handleDvl() - RDI_Pathfinder msg: 1613177692.97 epoch sec; seqNo:8139 2021-02-13T00:54:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 114.75 , 89.06 , 110.69 2021-02-13T00:54:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.12 , 0.11 2021-02-13T00:54:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177692.99 epoch sec; seqNo:16871; 4060362.95 north; 593353.93 east 2021-02-13T00:54:53Z- AHRS_M2 msg: 1613177693.36 epoch sec; seqNo:16737 2021-02-13T00:54:53Z- AHRS_M2 msg: 0.66 phi; -0.22 theta; -0.31 psi 2021-02-13T00:54:53Z- handleDepth()-Depth_Keller msg: 1613177693368 epoch sec; seqNo:16872; depth 4.77 2021-02-13T00:54:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177693.38 epoch sec; seqNo:16872; 4060362.95 north; 593353.93 east 2021-02-13T00:54:53Z- AHRS_M2 msg: 1613177693.77 epoch sec; seqNo:16738 2021-02-13T00:54:53Z- AHRS_M2 msg: 0.66 phi; -0.23 theta; -0.29 psi 2021-02-13T00:54:53Z- handleDepth()-Depth_Keller msg: 1613177693778 epoch sec; seqNo:16873; depth 4.81 2021-02-13T00:54:53Z- handleDvl() - RDI_Pathfinder msg: 1613177693.79 epoch sec; seqNo:8140 2021-02-13T00:54:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.86 , 118.67 , 97.03 , 113.26 2021-02-13T00:54:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , 0.19 2021-02-13T00:54:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177693.80 epoch sec; seqNo:16873; 4060363.33 north; 593353.93 east 2021-02-13T00:54:54Z- AHRS_M2 msg: 1613177694.17 epoch sec; seqNo:16739 2021-02-13T00:54:54Z- AHRS_M2 msg: 0.66 phi; -0.25 theta; -0.28 psi 2021-02-13T00:54:54Z- handleDepth()-Depth_Keller msg: 1613177694178 epoch sec; seqNo:16874; depth 4.86 2021-02-13T00:54:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:54Z- handleDvl() - RDI_Pathfinder msg: 1613177694.18 epoch sec; seqNo:8141 2021-02-13T00:54:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , 0.19 2021-02-13T00:54:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177694.19 epoch sec; seqNo:16874; 4060363.71 north; 593353.93 east 2021-02-13T00:54:54Z- AHRS_M2 msg: 1613177694.58 epoch sec; seqNo:16740 2021-02-13T00:54:54Z- AHRS_M2 msg: 0.66 phi; -0.27 theta; -0.26 psi 2021-02-13T00:54:54Z- handleDepth()-Depth_Keller msg: 1613177694580 epoch sec; seqNo:16875; depth 4.90 2021-02-13T00:54:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177694.60 epoch sec; seqNo:16875; 4060363.73 north; 593355.15 east 2021-02-13T00:54:54Z- AHRS_M2 msg: 1613177694.98 epoch sec; seqNo:16741 2021-02-13T00:54:54Z- AHRS_M2 msg: 0.65 phi; -0.28 theta; -0.25 psi 2021-02-13T00:54:54Z- handleDepth()-Depth_Keller msg: 1613177694984 epoch sec; seqNo:16876; depth 4.96 2021-02-13T00:54:54Z- handleDvl() - RDI_Pathfinder msg: 1613177694.99 epoch sec; seqNo:8142 2021-02-13T00:54:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.88 , 115.63 , 0.00 , 117.11 2021-02-13T00:54:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , 0.22 2021-02-13T00:54:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177695.02 epoch sec; seqNo:16876; 4060364.10 north; 593355.14 east 2021-02-13T00:54:55Z- AHRS_M2 msg: 1613177695.38 epoch sec; seqNo:16742 2021-02-13T00:54:55Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; -0.25 psi 2021-02-13T00:54:55Z- handleDepth()-Depth_Keller msg: 1613177695388 epoch sec; seqNo:16877; depth 5.03 2021-02-13T00:54:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:55Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:55Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:55Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:55Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:55Z- handleDvl() - RDI_Pathfinder msg: 1613177695.39 epoch sec; seqNo:8143 2021-02-13T00:54:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , 0.22 2021-02-13T00:54:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177695.40 epoch sec; seqNo:16877; 4060364.10 north; 593355.14 east 2021-02-13T00:54:55Z- LcmTrn::handleMessages() - handling 46 messages took 0.67 ms and 133 calls... 2021-02-13T00:54:55Z- waiting for 1613177693.15, time is 1613177695.40 2021-02-13T00:54:55Z- LcmTrn::updateTrn() equating pose 1613177695.40 and meas times 1613177695.40 2021-02-13T00:54:55Z- LcmTrn::updateTrn() >>>> pose time 1613177695.40, meas time 1613177695.40 <<<< 2021-02-13T00:54:55Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177695.40, ping # 0. 2021-02-13T00:54:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177695.40 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:54:55Z- TerrainNav::motionUpdate() - currEstimate = (4060364, 593355.14, 5.03) 2021-02-13T00:54:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:54:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:54:55Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:54:55Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:54:55Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177695.40 sec.; No relevant navigation data available 2021-02-13T00:54:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:54:55Z- TNavBF::mmse selected from filter -1 2021-02-13T00:54:55Z- variances([0], [2], and psi_berg) = 682.851544 832.989707 0.000000 2021-02-13T00:54:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:54:55Z- LcmTrn::publishEstimates() - MLE : 4060363.60 593336.64 5.03 2021-02-13T00:54:55Z- LcmTrn::publishEstimates() - MMSE : 4060364.10 593355.14 5.03 2021-02-13T00:54:55Z- LcmTrn::publishEstimates() - COVAR: 682.85 832.99 1.35 0.00 2021-02-13T00:54:55Z- AHRS_M2 msg: 1613177695.79 epoch sec; seqNo:16743 2021-02-13T00:54:55Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.24 psi 2021-02-13T00:54:55Z- handleDepth()-Depth_Keller msg: 1613177695793 epoch sec; seqNo:16878; depth 5.11 2021-02-13T00:54:55Z- handleDvl() - RDI_Pathfinder msg: 1613177695.81 epoch sec; seqNo:8144 2021-02-13T00:54:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 110.96 , 85.68 , 121.37 2021-02-13T00:54:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.13 , 0.27 2021-02-13T00:54:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177695.82 epoch sec; seqNo:16878; 4060364.48 north; 593355.14 east 2021-02-13T00:54:56Z- AHRS_M2 msg: 1613177696.19 epoch sec; seqNo:16744 2021-02-13T00:54:56Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.23 psi 2021-02-13T00:54:56Z- handleDepth()-Depth_Keller msg: 1613177696196 epoch sec; seqNo:16879; depth 5.19 2021-02-13T00:54:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177696.21 epoch sec; seqNo:16879; 4060364.86 north; 593355.13 east 2021-02-13T00:54:56Z- AHRS_M2 msg: 1613177696.60 epoch sec; seqNo:16745 2021-02-13T00:54:56Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.23 psi 2021-02-13T00:54:56Z- handleDepth()-Depth_Keller msg: 1613177696600 epoch sec; seqNo:16880; depth 5.30 2021-02-13T00:54:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:54:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:54:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:54:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:54:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:54:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:54:56Z- handleDvl() - RDI_Pathfinder msg: 1613177696.60 epoch sec; seqNo:8145 2021-02-13T00:54:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:54:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.13 , 0.27 2021-02-13T00:54:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177696.61 epoch sec; seqNo:16880; 4060364.86 north; 593355.13 east 2021-02-13T00:54:57Z- AHRS_M2 msg: 1613177697.00 epoch sec; seqNo:16746 2021-02-13T00:54:57Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.23 psi 2021-02-13T00:54:57Z- handleDepth()-Depth_Keller msg: 1613177697009 epoch sec; seqNo:16881; depth 5.39 2021-02-13T00:54:57Z- handleDvl() - RDI_Pathfinder msg: 1613177697.03 epoch sec; seqNo:8146 2021-02-13T00:54:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.57 , 115.42 , 84.19 , 119.89 2021-02-13T00:54:57Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.25 , 0.04 2021-02-13T00:54:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177697.05 epoch sec; seqNo:16881; 4060365.25 north; 593356.35 east 2021-02-13T00:54:57Z- AHRS_M2 msg: 1613177697.40 epoch sec; seqNo:16747 2021-02-13T00:54:57Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.24 psi 2021-02-13T00:54:57Z- handleDepth()-Depth_Keller msg: 1613177697409 epoch sec; seqNo:16882; depth 5.48 2021-02-13T00:54:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177697.42 epoch sec; seqNo:16882; 4060365.63 north; 593356.35 east 2021-02-13T00:54:57Z- AHRS_M2 msg: 1613177697.81 epoch sec; seqNo:16748 2021-02-13T00:54:57Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.26 psi 2021-02-13T00:54:57Z- handleDepth()-Depth_Keller msg: 1613177697823 epoch sec; seqNo:16883; depth 5.57 2021-02-13T00:54:57Z- handleDvl() - RDI_Pathfinder msg: 1613177697.84 epoch sec; seqNo:8147 2021-02-13T00:54:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 114.07 , 84.32 , 120.02 2021-02-13T00:54:57Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.13 , -0.21 2021-02-13T00:54:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177697.85 epoch sec; seqNo:16883; 4060365.63 north; 593356.35 east 2021-02-13T00:54:58Z- AHRS_M2 msg: 1613177698.22 epoch sec; seqNo:16749 2021-02-13T00:54:58Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.26 psi 2021-02-13T00:54:58Z- handleDepth()-Depth_Keller msg: 1613177698221 epoch sec; seqNo:16884; depth 5.69 2021-02-13T00:54:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177698.24 epoch sec; seqNo:16884; 4060366.01 north; 593356.34 east 2021-02-13T00:54:58Z- AHRS_M2 msg: 1613177698.62 epoch sec; seqNo:16750 2021-02-13T00:54:58Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.25 psi 2021-02-13T00:54:58Z- handleDepth()-Depth_Keller msg: 1613177698620 epoch sec; seqNo:16885; depth 5.80 2021-02-13T00:54:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177698.63 epoch sec; seqNo:16885; 4060366.40 north; 593357.56 east 2021-02-13T00:54:59Z- AHRS_M2 msg: 1613177699.02 epoch sec; seqNo:16751 2021-02-13T00:54:59Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.24 psi 2021-02-13T00:54:59Z- handleDepth()-Depth_Keller msg: 1613177699030 epoch sec; seqNo:16886; depth 5.89 2021-02-13T00:54:59Z- handleDvl() - RDI_Pathfinder msg: 1613177699.04 epoch sec; seqNo:8148 2021-02-13T00:54:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 116.91 , 84.19 , 121.37 2021-02-13T00:54:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.11 , -0.19 2021-02-13T00:54:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177699.05 epoch sec; seqNo:16886; 4060366.78 north; 593357.55 east 2021-02-13T00:54:59Z- AHRS_M2 msg: 1613177699.42 epoch sec; seqNo:16752 2021-02-13T00:54:59Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.23 psi 2021-02-13T00:54:59Z- handleDepth()-Depth_Keller msg: 1613177699428 epoch sec; seqNo:16887; depth 6.02 2021-02-13T00:54:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177699.44 epoch sec; seqNo:16887; 4060367.16 north; 593357.55 east 2021-02-13T00:54:59Z- AHRS_M2 msg: 1613177699.83 epoch sec; seqNo:16753 2021-02-13T00:54:59Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.23 psi 2021-02-13T00:54:59Z- handleDepth()-Depth_Keller msg: 1613177699837 epoch sec; seqNo:16888; depth 6.11 2021-02-13T00:54:59Z- handleDvl() - RDI_Pathfinder msg: 1613177699.85 epoch sec; seqNo:8149 2021-02-13T00:54:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 115.29 , 84.05 , 122.72 2021-02-13T00:54:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.15 , -0.06 2021-02-13T00:54:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177699.87 epoch sec; seqNo:16888; 4060367.54 north; 593357.55 east 2021-02-13T00:55:00Z- AHRS_M2 msg: 1613177700.23 epoch sec; seqNo:16754 2021-02-13T00:55:00Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.22 psi 2021-02-13T00:55:00Z- handleDepth()-Depth_Keller msg: 1613177700241 epoch sec; seqNo:16889; depth 6.22 2021-02-13T00:55:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:00Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:00Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:00Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:00Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:00Z- handleDvl() - RDI_Pathfinder msg: 1613177700.24 epoch sec; seqNo:8150 2021-02-13T00:55:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.15 , -0.06 2021-02-13T00:55:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177700.25 epoch sec; seqNo:16889; 4060367.54 north; 593357.55 east 2021-02-13T00:55:00Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 131 calls... 2021-02-13T00:55:00Z- waiting for 1613177698.40, time is 1613177700.25 2021-02-13T00:55:00Z- LcmTrn::updateTrn() equating pose 1613177700.25 and meas times 1613177700.25 2021-02-13T00:55:00Z- LcmTrn::updateTrn() >>>> pose time 1613177700.25, meas time 1613177700.25 <<<< 2021-02-13T00:55:00Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177700.25, ping # 0. 2021-02-13T00:55:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177700.25 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:55:00Z- TerrainNav::motionUpdate() - currEstimate = (4060368, 593357.55, 6.22) 2021-02-13T00:55:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:55:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:55:00Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:55:00Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:55:00Z- TNavBF::Projecting Measurements 2021-02-13T00:55:00Z- TNavFilter::Measurement from time = 1613177700.25 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:55:00Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:55:00Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:55:00Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:55:00Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:55:00Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:55:00Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:55:00Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:55:00Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 682.8 EastVariance: 833.0 DepthVariance: 1.6 this->DepthBiasCov: 1.6 2021-02-13T00:55:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:55:00Z- TNavBF::mmse selected from filter -1 2021-02-13T00:55:00Z- variances([0], [2], and psi_berg) = 682.805260 832.989687 0.000000 2021-02-13T00:55:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:55:00Z- LcmTrn::publishEstimates() - MLE : 4060367.04 593341.05 6.22 2021-02-13T00:55:00Z- LcmTrn::publishEstimates() - MMSE : 4060367.54 593357.55 6.22 2021-02-13T00:55:00Z- LcmTrn::publishEstimates() - COVAR: 682.81 832.99 1.56 0.00 2021-02-13T00:55:00Z- AHRS_M2 msg: 1613177700.64 epoch sec; seqNo:16755 2021-02-13T00:55:00Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-13T00:55:00Z- handleDepth()-Depth_Keller msg: 1613177700649 epoch sec; seqNo:16890; depth 6.33 2021-02-13T00:55:00Z- handleDvl() - RDI_Pathfinder msg: 1613177700.66 epoch sec; seqNo:8151 2021-02-13T00:55:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:00Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:55:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177700.67 epoch sec; seqNo:16890; 4060367.92 north; 593357.54 east 2021-02-13T00:55:01Z- AHRS_M2 msg: 1613177701.04 epoch sec; seqNo:16756 2021-02-13T00:55:01Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-13T00:55:01Z- handleDepth()-Depth_Keller msg: 1613177701049 epoch sec; seqNo:16891; depth 6.45 2021-02-13T00:55:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177701.07 epoch sec; seqNo:16891; 4060368.30 north; 593357.54 east 2021-02-13T00:55:01Z- AHRS_M2 msg: 1613177701.44 epoch sec; seqNo:16757 2021-02-13T00:55:01Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.18 psi 2021-02-13T00:55:01Z- handleDepth()-Depth_Keller msg: 1613177701448 epoch sec; seqNo:16892; depth 6.56 2021-02-13T00:55:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:01Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:01Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:01Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:01Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:01Z- handleDvl() - RDI_Pathfinder msg: 1613177701.45 epoch sec; seqNo:8152 2021-02-13T00:55:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:01Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:55:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177701.46 epoch sec; seqNo:16892; 4060368.30 north; 593357.54 east 2021-02-13T00:55:01Z- AHRS_M2 msg: 1613177701.85 epoch sec; seqNo:16758 2021-02-13T00:55:01Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.19 psi 2021-02-13T00:55:01Z- handleDepth()-Depth_Keller msg: 1613177701853 epoch sec; seqNo:16893; depth 6.65 2021-02-13T00:55:01Z- handleDvl() - RDI_Pathfinder msg: 1613177701.87 epoch sec; seqNo:8153 2021-02-13T00:55:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.52 , 107.85 , 82.57 , 122.72 2021-02-13T00:55:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.63 , 0.07 , 0.17 2021-02-13T00:55:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177701.88 epoch sec; seqNo:16893; 4060368.69 north; 593358.75 east 2021-02-13T00:55:02Z- AHRS_M2 msg: 1613177702.25 epoch sec; seqNo:16759 2021-02-13T00:55:02Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.20 psi 2021-02-13T00:55:02Z- handleDepth()-Depth_Keller msg: 1613177702258 epoch sec; seqNo:16894; depth 6.76 2021-02-13T00:55:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177702.27 epoch sec; seqNo:16894; 4060368.69 north; 593358.75 east 2021-02-13T00:55:02Z- AHRS_M2 msg: 1613177702.66 epoch sec; seqNo:16760 2021-02-13T00:55:02Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.21 psi 2021-02-13T00:55:02Z- handleDepth()-Depth_Keller msg: 1613177702665 epoch sec; seqNo:16895; depth 6.86 2021-02-13T00:55:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:02Z- handleDvl() - RDI_Pathfinder msg: 1613177702.67 epoch sec; seqNo:8154 2021-02-13T00:55:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.63 , 0.07 , 0.17 2021-02-13T00:55:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177702.68 epoch sec; seqNo:16895; 4060369.07 north; 593358.75 east 2021-02-13T00:55:03Z- AHRS_M2 msg: 1613177703.06 epoch sec; seqNo:16761 2021-02-13T00:55:03Z- AHRS_M2 msg: 0.57 phi; -0.36 theta; -0.22 psi 2021-02-13T00:55:03Z- handleDepth()-Depth_Keller msg: 1613177703063 epoch sec; seqNo:16896; depth 6.96 2021-02-13T00:55:03Z- handleDvl() - RDI_Pathfinder msg: 1613177703.08 epoch sec; seqNo:8155 2021-02-13T00:55:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 107.78 , 0.00 , 124.01 2021-02-13T00:55:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.18 , 0.14 2021-02-13T00:55:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177703.13 epoch sec; seqNo:16896; 4060369.07 north; 593358.75 east 2021-02-13T00:55:03Z- AHRS_M2 msg: 1613177703.47 epoch sec; seqNo:16762 2021-02-13T00:55:03Z- AHRS_M2 msg: 0.57 phi; -0.35 theta; -0.23 psi 2021-02-13T00:55:03Z- handleDepth()-Depth_Keller msg: 1613177703477 epoch sec; seqNo:16897; depth 7.07 2021-02-13T00:55:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177703.49 epoch sec; seqNo:16897; 4060369.45 north; 593358.75 east 2021-02-13T00:55:03Z- AHRS_M2 msg: 1613177703.88 epoch sec; seqNo:16763 2021-02-13T00:55:03Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.24 psi 2021-02-13T00:55:03Z- handleDepth()-Depth_Keller msg: 1613177703891 epoch sec; seqNo:16898; depth 7.20 2021-02-13T00:55:03Z- handleDvl() - RDI_Pathfinder msg: 1613177703.91 epoch sec; seqNo:8156 2021-02-13T00:55:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.38 , 112.18 , 82.43 , 128.54 2021-02-13T00:55:03Z- handleDvl() - RDI_Pathfinder msg: velocities 1.14 , 0.19 , 0.06 2021-02-13T00:55:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177703.94 epoch sec; seqNo:16898; 4060369.83 north; 593358.74 east 2021-02-13T00:55:04Z- AHRS_M2 msg: 1613177704.27 epoch sec; seqNo:16764 2021-02-13T00:55:04Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.23 psi 2021-02-13T00:55:04Z- handleDepth()-Depth_Keller msg: 1613177704277 epoch sec; seqNo:16899; depth 7.33 2021-02-13T00:55:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177704.30 epoch sec; seqNo:16899; 4060370.22 north; 593359.96 east 2021-02-13T00:55:04Z- AHRS_M2 msg: 1613177704.68 epoch sec; seqNo:16765 2021-02-13T00:55:04Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.22 psi 2021-02-13T00:55:04Z- handleDepth()-Depth_Keller msg: 1613177704685 epoch sec; seqNo:16900; depth 7.44 2021-02-13T00:55:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177704.69 epoch sec; seqNo:16900; 4060370.22 north; 593359.96 east 2021-02-13T00:55:05Z- AHRS_M2 msg: 1613177705.08 epoch sec; seqNo:16766 2021-02-13T00:55:05Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.21 psi 2021-02-13T00:55:05Z- handleDepth()-Depth_Keller msg: 1613177705084 epoch sec; seqNo:16901; depth 7.59 2021-02-13T00:55:05Z- handleDvl() - RDI_Pathfinder msg: 1613177705.10 epoch sec; seqNo:8157 2021-02-13T00:55:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.43 , 115.69 , 81.48 , 124.62 2021-02-13T00:55:05Z- handleDvl() - RDI_Pathfinder msg: velocities 1.18 , 0.07 , -0.06 2021-02-13T00:55:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177705.11 epoch sec; seqNo:16901; 4060370.60 north; 593359.95 east 2021-02-13T00:55:05Z- AHRS_M2 msg: 1613177705.48 epoch sec; seqNo:16767 2021-02-13T00:55:05Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-13T00:55:05Z- handleDepth()-Depth_Keller msg: 1613177705497 epoch sec; seqNo:16902; depth 7.72 2021-02-13T00:55:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177705.51 epoch sec; seqNo:16902; 4060370.98 north; 593359.95 east 2021-02-13T00:55:05Z- LcmTrn::handleMessages() - handling 46 messages took 0.73 ms and 132 calls... 2021-02-13T00:55:05Z- waiting for 1613177703.25, time is 1613177705.51 2021-02-13T00:55:05Z- LcmTrn::updateTrn() >>>> pose time 1613177705.51, meas time 1613177705.10 <<<< 2021-02-13T00:55:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177705.10 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:55:05Z- TerrainNav::motionUpdate() - currEstimate = (4060371, 593359.95, 7.72) 2021-02-13T00:55:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:55:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:55:05Z- TNavBF::Projecting Measurements 2021-02-13T00:55:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:55:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:55:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:55:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:55:05Z- TNavFilter::4 good beams. 2021-02-13T00:55:05Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:55:05Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:55:05Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:55:05Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:55:05Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:55:05Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:55:05Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:55:05Z- TNavBF::mmse selected from filter -1 2021-02-13T00:55:05Z- TerrainMapOctree: (vehN, vehE ) = (4060370.70, 593359.76). 2021-02-13T00:55:05Z- TerrainMapOctree: Min Distance = 1251.28. 2021-02-13T00:55:05Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:55:05Z- There is no specific good beam that all particles have in common. 2021-02-13T00:55:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:55:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.76 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 12.23 2021-02-13T00:55:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:55:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.56 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 21.42 2021-02-13T00:55:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:55:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.46 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 10.62 2021-02-13T00:55:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:55:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 24.85 2021-02-13T00:55:05Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:55:05Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:55:05Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:55:05Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:55:05Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:55:05Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:55:05Z- Filter 0 No beams used in subcloud update 2021-02-13T00:55:05Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:55:05Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:55:05Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 682.8 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:55:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177705.10 sec. 2021-02-13T00:55:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:55:05Z- TNavBF::mmse selected from filter -1 2021-02-13T00:55:05Z- variances([0], [2], and psi_berg) = 682.747839 832.989662 0.000000 2021-02-13T00:55:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:55:05Z- LcmTrn::publishEstimates() - MLE : 4060370.48 593343.45 7.72 2021-02-13T00:55:05Z- LcmTrn::publishEstimates() - MMSE : 4060370.98 593359.95 7.72 2021-02-13T00:55:05Z- LcmTrn::publishEstimates() - COVAR: 682.75 832.99 1.26 0.00 2021-02-13T00:55:05Z- AHRS_M2 msg: 1613177705.89 epoch sec; seqNo:16768 2021-02-13T00:55:05Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.17 psi 2021-02-13T00:55:05Z- handleDepth()-Depth_Keller msg: 1613177705892 epoch sec; seqNo:16903; depth 7.85 2021-02-13T00:55:05Z- handleDvl() - RDI_Pathfinder msg: 1613177705.91 epoch sec; seqNo:8158 2021-02-13T00:55:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 108.32 , 81.55 , 120.22 2021-02-13T00:55:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.07 , -0.16 2021-02-13T00:55:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177705.92 epoch sec; seqNo:16903; 4060371.37 north; 593361.17 east 2021-02-13T00:55:06Z- AHRS_M2 msg: 1613177706.29 epoch sec; seqNo:16769 2021-02-13T00:55:06Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.16 psi 2021-02-13T00:55:06Z- handleDepth()-Depth_Keller msg: 1613177706296 epoch sec; seqNo:16904; depth 7.99 2021-02-13T00:55:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:06Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:06Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:06Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:06Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:06Z- handleDvl() - RDI_Pathfinder msg: 1613177706.30 epoch sec; seqNo:8159 2021-02-13T00:55:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.07 , -0.16 2021-02-13T00:55:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177706.31 epoch sec; seqNo:16904; 4060371.75 north; 593361.16 east 2021-02-13T00:55:06Z- AHRS_M2 msg: 1613177706.70 epoch sec; seqNo:16770 2021-02-13T00:55:06Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.15 psi 2021-02-13T00:55:06Z- handleDepth()-Depth_Keller msg: 1613177706702 epoch sec; seqNo:16905; depth 8.12 2021-02-13T00:55:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177706.71 epoch sec; seqNo:16905; 4060372.13 north; 593361.16 east 2021-02-13T00:55:07Z- AHRS_M2 msg: 1613177707.10 epoch sec; seqNo:16771 2021-02-13T00:55:07Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.15 psi 2021-02-13T00:55:07Z- handleDepth()-Depth_Keller msg: 1613177707109 epoch sec; seqNo:16906; depth 8.21 2021-02-13T00:55:07Z- handleDvl() - RDI_Pathfinder msg: 1613177707.13 epoch sec; seqNo:8160 2021-02-13T00:55:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.43 , 106.77 , 80.00 , 118.67 2021-02-13T00:55:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.13 , -0.18 2021-02-13T00:55:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177707.14 epoch sec; seqNo:16906; 4060372.51 north; 593361.15 east 2021-02-13T00:55:07Z- AHRS_M2 msg: 1613177707.50 epoch sec; seqNo:16772 2021-02-13T00:55:07Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.16 psi 2021-02-13T00:55:07Z- handleDepth()-Depth_Keller msg: 1613177707508 epoch sec; seqNo:16907; depth 8.34 2021-02-13T00:55:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:07Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:07Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:07Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:07Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:07Z- handleDvl() - RDI_Pathfinder msg: 1613177707.51 epoch sec; seqNo:8161 2021-02-13T00:55:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.13 , -0.18 2021-02-13T00:55:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177707.53 epoch sec; seqNo:16907; 4060372.88 north; 593361.15 east 2021-02-13T00:55:07Z- AHRS_M2 msg: 1613177707.91 epoch sec; seqNo:16773 2021-02-13T00:55:07Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.16 psi 2021-02-13T00:55:07Z- handleDepth()-Depth_Keller msg: 1613177707913 epoch sec; seqNo:16908; depth 8.46 2021-02-13T00:55:07Z- handleDvl() - RDI_Pathfinder msg: 1613177707.92 epoch sec; seqNo:8162 2021-02-13T00:55:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 103.93 , 79.59 , 116.10 2021-02-13T00:55:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.11 , -0.00 2021-02-13T00:55:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177707.94 epoch sec; seqNo:16908; 4060372.88 north; 593361.15 east 2021-02-13T00:55:08Z- AHRS_M2 msg: 1613177708.31 epoch sec; seqNo:16774 2021-02-13T00:55:08Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.17 psi 2021-02-13T00:55:08Z- handleDepth()-Depth_Keller msg: 1613177708321 epoch sec; seqNo:16909; depth 8.57 2021-02-13T00:55:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177708.33 epoch sec; seqNo:16909; 4060373.26 north; 593361.15 east 2021-02-13T00:55:08Z- AHRS_M2 msg: 1613177708.73 epoch sec; seqNo:16775 2021-02-13T00:55:08Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.17 psi 2021-02-13T00:55:08Z- handleDepth()-Depth_Keller msg: 1613177708737 epoch sec; seqNo:16910; depth 8.69 2021-02-13T00:55:08Z- handleDvl() - RDI_Pathfinder msg: 1613177708.76 epoch sec; seqNo:8163 2021-02-13T00:55:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.15 , 77.65 , 120.02 2021-02-13T00:55:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , 0.16 2021-02-13T00:55:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177708.77 epoch sec; seqNo:16910; 4060373.66 north; 593362.36 east 2021-02-13T00:55:09Z- AHRS_M2 msg: 1613177709.12 epoch sec; seqNo:16776 2021-02-13T00:55:09Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.17 psi 2021-02-13T00:55:09Z- handleDepth()-Depth_Keller msg: 1613177709127 epoch sec; seqNo:16911; depth 8.83 2021-02-13T00:55:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177709.14 epoch sec; seqNo:16911; 4060373.66 north; 593362.36 east 2021-02-13T00:55:09Z- AHRS_M2 msg: 1613177709.53 epoch sec; seqNo:16777 2021-02-13T00:55:09Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.18 psi 2021-02-13T00:55:09Z- handleDepth()-Depth_Keller msg: 1613177709534 epoch sec; seqNo:16912; depth 8.94 2021-02-13T00:55:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177709.56 epoch sec; seqNo:16912; 4060374.03 north; 593362.36 east 2021-02-13T00:55:09Z- AHRS_M2 msg: 1613177709.93 epoch sec; seqNo:16778 2021-02-13T00:55:09Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.18 psi 2021-02-13T00:55:09Z- handleDepth()-Depth_Keller msg: 1613177709937 epoch sec; seqNo:16913; depth 9.08 2021-02-13T00:55:09Z- handleDvl() - RDI_Pathfinder msg: 1613177709.95 epoch sec; seqNo:8164 2021-02-13T00:55:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.50 , 101.02 , 79.39 , 119.95 2021-02-13T00:55:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.15 , 0.10 2021-02-13T00:55:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177709.97 epoch sec; seqNo:16913; 4060374.41 north; 593362.35 east 2021-02-13T00:55:10Z- AHRS_M2 msg: 1613177710.33 epoch sec; seqNo:16779 2021-02-13T00:55:10Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.18 psi 2021-02-13T00:55:10Z- handleDepth()-Depth_Keller msg: 1613177710336 epoch sec; seqNo:16914; depth 9.21 2021-02-13T00:55:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177710.35 epoch sec; seqNo:16914; 4060374.41 north; 593362.35 east 2021-02-13T00:55:10Z- LcmTrn::handleMessages() - handling 43 messages took 0.65 ms and 130 calls... 2021-02-13T00:55:10Z- waiting for 1613177708.51, time is 1613177710.35 2021-02-13T00:55:10Z- LcmTrn::updateTrn() >>>> pose time 1613177710.35, meas time 1613177709.95 <<<< 2021-02-13T00:55:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177709.95 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:55:10Z- TerrainNav::motionUpdate() - currEstimate = (4060374, 593362.35, 9.21) 2021-02-13T00:55:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:55:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:55:10Z- TNavBF::Projecting Measurements 2021-02-13T00:55:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:55:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:55:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:55:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:55:10Z- TNavFilter::4 good beams. 2021-02-13T00:55:10Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:55:10Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:55:10Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:55:10Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:55:10Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:55:10Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:55:10Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:55:10Z- TNavBF::mmse selected from filter -1 2021-02-13T00:55:10Z- TerrainMapOctree: (vehN, vehE ) = (4060374.13, 593362.16). 2021-02-13T00:55:10Z- TerrainMapOctree: Min Distance = 1247.16. 2021-02-13T00:55:10Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:55:10Z- There is no specific good beam that all particles have in common. 2021-02-13T00:55:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:55:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.46 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 12.25 2021-02-13T00:55:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:55:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.32 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 16.33 2021-02-13T00:55:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:55:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 10.08 2021-02-13T00:55:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:55:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 23.02 2021-02-13T00:55:10Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:55:10Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:55:10Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:55:10Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:55:10Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:55:10Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:55:10Z- Filter 0 No beams used in subcloud update 2021-02-13T00:55:10Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:55:10Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:55:10Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 682.7 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:55:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177709.95 sec. 2021-02-13T00:55:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:55:10Z- TNavBF::mmse selected from filter -1 2021-02-13T00:55:10Z- variances([0], [2], and psi_berg) = 682.690279 832.989637 0.000000 2021-02-13T00:55:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:55:10Z- LcmTrn::publishEstimates() - MLE : 4060373.91 593380.85 9.21 2021-02-13T00:55:10Z- LcmTrn::publishEstimates() - MMSE : 4060374.41 593362.35 9.21 2021-02-13T00:55:10Z- LcmTrn::publishEstimates() - COVAR: 682.69 832.99 1.08 0.00 2021-02-13T00:55:10Z- AHRS_M2 msg: 1613177710.74 epoch sec; seqNo:16780 2021-02-13T00:55:10Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.17 psi 2021-02-13T00:55:10Z- handleDepth()-Depth_Keller msg: 1613177710742 epoch sec; seqNo:16915; depth 9.33 2021-02-13T00:55:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177710.75 epoch sec; seqNo:16915; 4060374.79 north; 593362.35 east 2021-02-13T00:55:11Z- AHRS_M2 msg: 1613177711.15 epoch sec; seqNo:16781 2021-02-13T00:55:11Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-13T00:55:11Z- handleDepth()-Depth_Keller msg: 1613177711156 epoch sec; seqNo:16916; depth 9.47 2021-02-13T00:55:11Z- handleDvl() - RDI_Pathfinder msg: 1613177711.17 epoch sec; seqNo:8165 2021-02-13T00:55:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.84 , 105.42 , 78.37 , 121.64 2021-02-13T00:55:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.15 , -0.06 2021-02-13T00:55:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177711.18 epoch sec; seqNo:16916; 4060375.18 north; 593363.57 east 2021-02-13T00:55:11Z- AHRS_M2 msg: 1613177711.54 epoch sec; seqNo:16782 2021-02-13T00:55:11Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-13T00:55:11Z- handleDepth()-Depth_Keller msg: 1613177711548 epoch sec; seqNo:16917; depth 9.61 2021-02-13T00:55:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177711.56 epoch sec; seqNo:16917; 4060375.18 north; 593363.57 east 2021-02-13T00:55:11Z- AHRS_M2 msg: 1613177711.95 epoch sec; seqNo:16783 2021-02-13T00:55:11Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-13T00:55:11Z- handleDepth()-Depth_Keller msg: 1613177711952 epoch sec; seqNo:16918; depth 9.74 2021-02-13T00:55:11Z- handleDvl() - RDI_Pathfinder msg: 1613177711.96 epoch sec; seqNo:8166 2021-02-13T00:55:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.70 , 106.63 , 76.43 , 121.51 2021-02-13T00:55:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.01 , -0.10 2021-02-13T00:55:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177711.97 epoch sec; seqNo:16918; 4060375.56 north; 593363.56 east 2021-02-13T00:55:12Z- AHRS_M2 msg: 1613177712.35 epoch sec; seqNo:16784 2021-02-13T00:55:12Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-13T00:55:12Z- handleDepth()-Depth_Keller msg: 1613177712358 epoch sec; seqNo:16919; depth 9.87 2021-02-13T00:55:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:12Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:12Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:12Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:12Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:12Z- handleDvl() - RDI_Pathfinder msg: 1613177712.36 epoch sec; seqNo:8167 2021-02-13T00:55:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.01 , -0.10 2021-02-13T00:55:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177712.37 epoch sec; seqNo:16919; 4060375.94 north; 593363.56 east 2021-02-13T00:55:12Z- AHRS_M2 msg: 1613177712.76 epoch sec; seqNo:16785 2021-02-13T00:55:12Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-13T00:55:12Z- handleDepth()-Depth_Keller msg: 1613177712784 epoch sec; seqNo:16920; depth 10.01 2021-02-13T00:55:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177712.81 epoch sec; seqNo:16920; 4060376.32 north; 593363.55 east 2021-02-13T00:55:13Z- AHRS_M2 msg: 1613177713.16 epoch sec; seqNo:16786 2021-02-13T00:55:13Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-13T00:55:13Z- handleDepth()-Depth_Keller msg: 1613177713166 epoch sec; seqNo:16921; depth 10.16 2021-02-13T00:55:13Z- handleDvl() - RDI_Pathfinder msg: 1613177713.18 epoch sec; seqNo:8168 2021-02-13T00:55:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 106.63 , 76.02 , 117.45 2021-02-13T00:55:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.08 , 0.02 2021-02-13T00:55:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177713.20 epoch sec; seqNo:16921; 4060376.71 north; 593364.77 east 2021-02-13T00:55:13Z- AHRS_M2 msg: 1613177713.57 epoch sec; seqNo:16787 2021-02-13T00:55:13Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:55:13Z- handleDepth()-Depth_Keller msg: 1613177713569 epoch sec; seqNo:16922; depth 10.28 2021-02-13T00:55:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:13Z- handleDvl() - RDI_Pathfinder msg: 1613177713.57 epoch sec; seqNo:8169 2021-02-13T00:55:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.08 , 0.02 2021-02-13T00:55:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177713.58 epoch sec; seqNo:16922; 4060376.71 north; 593364.77 east 2021-02-13T00:55:13Z- AHRS_M2 msg: 1613177713.97 epoch sec; seqNo:16788 2021-02-13T00:55:13Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.17 psi 2021-02-13T00:55:13Z- handleDepth()-Depth_Keller msg: 1613177713973 epoch sec; seqNo:16923; depth 10.42 2021-02-13T00:55:13Z- handleDvl() - RDI_Pathfinder msg: 1613177713.98 epoch sec; seqNo:8170 2021-02-13T00:55:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.20 , 104.13 , 77.65 , 116.30 2021-02-13T00:55:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , 0.10 2021-02-13T00:55:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177714.00 epoch sec; seqNo:16923; 4060377.09 north; 593364.77 east 2021-02-13T00:55:14Z- AHRS_M2 msg: 1613177714.37 epoch sec; seqNo:16789 2021-02-13T00:55:14Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.17 psi 2021-02-13T00:55:14Z- handleDepth()-Depth_Keller msg: 1613177714378 epoch sec; seqNo:16924; depth 10.56 2021-02-13T00:55:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177714.40 epoch sec; seqNo:16924; 4060377.47 north; 593364.76 east 2021-02-13T00:55:14Z- AHRS_M2 msg: 1613177714.78 epoch sec; seqNo:16790 2021-02-13T00:55:14Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-13T00:55:14Z- handleDepth()-Depth_Keller msg: 1613177714781 epoch sec; seqNo:16925; depth 10.70 2021-02-13T00:55:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:14Z- handleDvl() - RDI_Pathfinder msg: 1613177714.79 epoch sec; seqNo:8171 2021-02-13T00:55:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , 0.10 2021-02-13T00:55:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177714.79 epoch sec; seqNo:16925; 4060377.47 north; 593364.76 east 2021-02-13T00:55:15Z- AHRS_M2 msg: 1613177715.19 epoch sec; seqNo:16791 2021-02-13T00:55:15Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.19 psi 2021-02-13T00:55:15Z- handleDepth()-Depth_Keller msg: 1613177715197 epoch sec; seqNo:16926; depth 10.82 2021-02-13T00:55:15Z- handleDvl() - RDI_Pathfinder msg: 1613177715.21 epoch sec; seqNo:8172 2021-02-13T00:55:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 102.78 , 78.44 , 117.65 2021-02-13T00:55:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.12 , 0.08 2021-02-13T00:55:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177715.22 epoch sec; seqNo:16926; 4060377.85 north; 593364.76 east 2021-02-13T00:55:15Z- AHRS_M2 msg: 1613177715.59 epoch sec; seqNo:16792 2021-02-13T00:55:15Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.19 psi 2021-02-13T00:55:15Z- handleDepth()-Depth_Keller msg: 1613177715597 epoch sec; seqNo:16927; depth 10.94 2021-02-13T00:55:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177715.62 epoch sec; seqNo:16927; 4060378.24 north; 593365.97 east 2021-02-13T00:55:15Z- LcmTrn::handleMessages() - handling 47 messages took 0.68 ms and 134 calls... 2021-02-13T00:55:15Z- waiting for 1613177713.35, time is 1613177715.62 2021-02-13T00:55:15Z- LcmTrn::updateTrn() >>>> pose time 1613177715.62, meas time 1613177715.21 <<<< 2021-02-13T00:55:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177715.21 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:55:15Z- TerrainNav::motionUpdate() - currEstimate = (4060378, 593365.97, 10.94) 2021-02-13T00:55:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:55:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:55:15Z- TNavBF::Projecting Measurements 2021-02-13T00:55:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:55:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:55:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:55:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:55:15Z- TNavFilter::4 good beams. 2021-02-13T00:55:15Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:55:15Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:55:15Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:55:15Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:55:15Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:55:15Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:55:15Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:55:15Z- TNavBF::mmse selected from filter -1 2021-02-13T00:55:15Z- TerrainMapOctree: (vehN, vehE ) = (4060377.95, 593365.70). 2021-02-13T00:55:15Z- TerrainMapOctree: Min Distance = 1241.96. 2021-02-13T00:55:15Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:55:15Z- There is no specific good beam that all particles have in common. 2021-02-13T00:55:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:55:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 11.99 2021-02-13T00:55:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:55:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 16.90 2021-02-13T00:55:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:55:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T00:55:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:55:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 22.15 2021-02-13T00:55:15Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:55:15Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:55:15Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:55:15Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:55:15Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:55:15Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:55:15Z- Filter 0 No beams used in subcloud update 2021-02-13T00:55:15Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:55:15Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:55:15Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 682.6 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:55:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177715.21 sec. 2021-02-13T00:55:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:55:15Z- TNavBF::mmse selected from filter -1 2021-02-13T00:55:15Z- variances([0], [2], and psi_berg) = 682.626161 832.989609 0.000000 2021-02-13T00:55:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:55:15Z- LcmTrn::publishEstimates() - MLE : 4060377.74 593384.47 10.94 2021-02-13T00:55:15Z- LcmTrn::publishEstimates() - MMSE : 4060378.24 593365.97 10.94 2021-02-13T00:55:15Z- LcmTrn::publishEstimates() - COVAR: 682.63 832.99 1.02 0.00 2021-02-13T00:55:15Z- AHRS_M2 msg: 1613177715.99 epoch sec; seqNo:16793 2021-02-13T00:55:15Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:55:15Z- handleDepth()-Depth_Keller msg: 1613177715992 epoch sec; seqNo:16928; depth 11.08 2021-02-13T00:55:16Z- handleDvl() - RDI_Pathfinder msg: 1613177716.00 epoch sec; seqNo:8173 2021-02-13T00:55:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.13 , 78.44 , 117.65 2021-02-13T00:55:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.17 , 0.03 2021-02-13T00:55:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177716.01 epoch sec; seqNo:16928; 4060378.24 north; 593365.97 east 2021-02-13T00:55:16Z- AHRS_M2 msg: 1613177716.40 epoch sec; seqNo:16794 2021-02-13T00:55:16Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:55:16Z- handleDepth()-Depth_Keller msg: 1613177716401 epoch sec; seqNo:16929; depth 11.22 2021-02-13T00:55:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177716.41 epoch sec; seqNo:16929; 4060378.62 north; 593365.97 east 2021-02-13T00:55:16Z- AHRS_M2 msg: 1613177716.80 epoch sec; seqNo:16795 2021-02-13T00:55:16Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.21 psi 2021-02-13T00:55:16Z- handleDepth()-Depth_Keller msg: 1613177716801 epoch sec; seqNo:16930; depth 11.34 2021-02-13T00:55:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177716.82 epoch sec; seqNo:16930; 4060378.62 north; 593365.97 east 2021-02-13T00:55:17Z- AHRS_M2 msg: 1613177717.20 epoch sec; seqNo:16796 2021-02-13T00:55:17Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.22 psi 2021-02-13T00:55:17Z- handleDepth()-Depth_Keller msg: 1613177717209 epoch sec; seqNo:16931; depth 11.48 2021-02-13T00:55:17Z- handleDvl() - RDI_Pathfinder msg: 1613177717.22 epoch sec; seqNo:8174 2021-02-13T00:55:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.86 , 105.15 , 78.10 , 118.67 2021-02-13T00:55:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.19 , -0.04 2021-02-13T00:55:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177717.24 epoch sec; seqNo:16931; 4060379.00 north; 593365.97 east 2021-02-13T00:55:17Z- AHRS_M2 msg: 1613177717.61 epoch sec; seqNo:16797 2021-02-13T00:55:17Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.23 psi 2021-02-13T00:55:17Z- handleDepth()-Depth_Keller msg: 1613177717609 epoch sec; seqNo:16932; depth 11.62 2021-02-13T00:55:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177717.62 epoch sec; seqNo:16932; 4060379.39 north; 593367.18 east 2021-02-13T00:55:18Z- AHRS_M2 msg: 1613177718.01 epoch sec; seqNo:16798 2021-02-13T00:55:18Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.23 psi 2021-02-13T00:55:18Z- handleDepth()-Depth_Keller msg: 1613177718011 epoch sec; seqNo:16933; depth 11.73 2021-02-13T00:55:18Z- handleDvl() - RDI_Pathfinder msg: 1613177718.03 epoch sec; seqNo:8175 2021-02-13T00:55:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.78 , 106.77 , 78.37 , 120.29 2021-02-13T00:55:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.15 , -0.15 2021-02-13T00:55:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177718.04 epoch sec; seqNo:16933; 4060379.77 north; 593367.18 east 2021-02-13T00:55:18Z- AHRS_M2 msg: 1613177718.41 epoch sec; seqNo:16799 2021-02-13T00:55:18Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.22 psi 2021-02-13T00:55:18Z- handleDepth()-Depth_Keller msg: 1613177718421 epoch sec; seqNo:16934; depth 11.87 2021-02-13T00:55:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:18Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:18Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:18Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:18Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:18Z- handleDvl() - RDI_Pathfinder msg: 1613177718.42 epoch sec; seqNo:8176 2021-02-13T00:55:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.15 , -0.15 2021-02-13T00:55:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177718.43 epoch sec; seqNo:16934; 4060379.77 north; 593367.18 east 2021-02-13T00:55:18Z- AHRS_M2 msg: 1613177718.82 epoch sec; seqNo:16800 2021-02-13T00:55:18Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.22 psi 2021-02-13T00:55:18Z- handleDepth()-Depth_Keller msg: 1613177718822 epoch sec; seqNo:16935; depth 12.00 2021-02-13T00:55:18Z- handleDvl() - RDI_Pathfinder msg: 1613177718.83 epoch sec; seqNo:8177 2021-02-13T00:55:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.35 , 76.83 , 118.87 2021-02-13T00:55:18Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.08 , -0.21 2021-02-13T00:55:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177718.85 epoch sec; seqNo:16935; 4060380.15 north; 593367.17 east 2021-02-13T00:55:19Z- AHRS_M2 msg: 1613177719.22 epoch sec; seqNo:16801 2021-02-13T00:55:19Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.21 psi 2021-02-13T00:55:19Z- handleDepth()-Depth_Keller msg: 1613177719224 epoch sec; seqNo:16936; depth 12.16 2021-02-13T00:55:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177719.23 epoch sec; seqNo:16936; 4060380.54 north; 593368.39 east 2021-02-13T00:55:19Z- AHRS_M2 msg: 1613177719.62 epoch sec; seqNo:16802 2021-02-13T00:55:19Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.21 psi 2021-02-13T00:55:19Z- handleDepth()-Depth_Keller msg: 1613177719628 epoch sec; seqNo:16937; depth 12.28 2021-02-13T00:55:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:19Z- handleDvl() - RDI_Pathfinder msg: 1613177719.63 epoch sec; seqNo:8178 2021-02-13T00:55:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:19Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.08 , -0.21 2021-02-13T00:55:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177719.64 epoch sec; seqNo:16937; 4060380.92 north; 593368.39 east 2021-02-13T00:55:20Z- AHRS_M2 msg: 1613177720.03 epoch sec; seqNo:16803 2021-02-13T00:55:20Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.20 psi 2021-02-13T00:55:20Z- handleDepth()-Depth_Keller msg: 1613177720033 epoch sec; seqNo:16938; depth 12.40 2021-02-13T00:55:20Z- handleDvl() - RDI_Pathfinder msg: 1613177720.05 epoch sec; seqNo:8179 2021-02-13T00:55:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.09 , 103.73 , 78.04 , 111.84 2021-02-13T00:55:20Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.07 , -0.21 2021-02-13T00:55:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177720.06 epoch sec; seqNo:16938; 4060381.30 north; 593368.38 east 2021-02-13T00:55:20Z- AHRS_M2 msg: 1613177720.43 epoch sec; seqNo:16804 2021-02-13T00:55:20Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.20 psi 2021-02-13T00:55:20Z- handleDepth()-Depth_Keller msg: 1613177720438 epoch sec; seqNo:16939; depth 12.54 2021-02-13T00:55:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177720.45 epoch sec; seqNo:16939; 4060381.68 north; 593368.38 east 2021-02-13T00:55:20Z- AHRS_M2 msg: 1613177720.84 epoch sec; seqNo:16805 2021-02-13T00:55:20Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.20 psi 2021-02-13T00:55:20Z- handleDepth()-Depth_Keller msg: 1613177720854 epoch sec; seqNo:16940; depth 12.67 2021-02-13T00:55:20Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-13T00:55:20Z- waiting for 1613177718.62, time is 1613177720.84 2021-02-13T00:55:20Z- LcmTrn::updateTrn() >>>> pose time 1613177720.84, meas time 1613177720.05 <<<< 2021-02-13T00:55:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177720.05 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:55:20Z- TerrainNav::motionUpdate() - currEstimate = (4060382, 593368.38, 12.67) 2021-02-13T00:55:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:55:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:55:20Z- TNavBF::Projecting Measurements 2021-02-13T00:55:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:55:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:55:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:55:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:55:20Z- TNavFilter::4 good beams. 2021-02-13T00:55:20Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:55:20Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:55:20Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:55:20Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:55:20Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:55:20Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:55:20Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:55:20Z- TNavBF::mmse selected from filter -1 2021-02-13T00:55:20Z- TerrainMapOctree: (vehN, vehE ) = (4060381.16, 593368.01). 2021-02-13T00:55:20Z- TerrainMapOctree: Min Distance = 1238.06. 2021-02-13T00:55:20Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:55:20Z- There is no specific good beam that all particles have in common. 2021-02-13T00:55:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:55:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 10.78 2021-02-13T00:55:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:55:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 17.21 2021-02-13T00:55:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:55:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 9.74 2021-02-13T00:55:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:55:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 20.01 2021-02-13T00:55:20Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:55:20Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:55:20Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:55:20Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:55:20Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:55:20Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:55:20Z- Filter 0 No beams used in subcloud update 2021-02-13T00:55:20Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:55:20Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:55:20Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 682.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:55:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177720.05 sec. 2021-02-13T00:55:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:55:20Z- TNavBF::mmse selected from filter -1 2021-02-13T00:55:20Z- variances([0], [2], and psi_berg) = 682.565644 832.989582 0.000000 2021-02-13T00:55:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:55:20Z- LcmTrn::publishEstimates() - MLE : 4060381.18 593385.88 12.67 2021-02-13T00:55:20Z- LcmTrn::publishEstimates() - MMSE : 4060381.68 593368.38 12.67 2021-02-13T00:55:20Z- LcmTrn::publishEstimates() - COVAR: 682.57 832.99 0.94 0.00 2021-02-13T00:55:20Z- handleDvl() - RDI_Pathfinder msg: 1613177720.87 epoch sec; seqNo:8180 2021-02-13T00:55:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.35 , 101.63 , 78.64 , 109.74 2021-02-13T00:55:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.13 , -0.12 2021-02-13T00:55:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177720.88 epoch sec; seqNo:16940; 4060382.06 north; 593368.37 east 2021-02-13T00:55:21Z- AHRS_M2 msg: 1613177721.24 epoch sec; seqNo:16806 2021-02-13T00:55:21Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.20 psi 2021-02-13T00:55:21Z- handleDepth()-Depth_Keller msg: 1613177721244 epoch sec; seqNo:16941; depth 12.81 2021-02-13T00:55:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177721.26 epoch sec; seqNo:16941; 4060382.44 north; 593368.37 east 2021-02-13T00:55:21Z- AHRS_M2 msg: 1613177721.64 epoch sec; seqNo:16807 2021-02-13T00:55:21Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.19 psi 2021-02-13T00:55:21Z- handleDepth()-Depth_Keller msg: 1613177721686 epoch sec; seqNo:16942; depth 12.91 2021-02-13T00:55:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177721.69 epoch sec; seqNo:16942; 4060382.83 north; 593369.59 east 2021-02-13T00:55:22Z- AHRS_M2 msg: 1613177722.05 epoch sec; seqNo:16808 2021-02-13T00:55:22Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.18 psi 2021-02-13T00:55:22Z- handleDepth()-Depth_Keller msg: 1613177722085 epoch sec; seqNo:16943; depth 13.07 2021-02-13T00:55:22Z- handleDvl() - RDI_Pathfinder msg: 1613177722.10 epoch sec; seqNo:8181 2021-02-13T00:55:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 101.70 , 78.71 , 109.81 2021-02-13T00:55:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.17 , -0.02 2021-02-13T00:55:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177722.11 epoch sec; seqNo:16943; 4060383.21 north; 593369.58 east 2021-02-13T00:55:22Z- AHRS_M2 msg: 1613177722.45 epoch sec; seqNo:16809 2021-02-13T00:55:22Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.17 psi 2021-02-13T00:55:22Z- handleDepth()-Depth_Keller msg: 1613177722457 epoch sec; seqNo:16944; depth 13.17 2021-02-13T00:55:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177722.47 epoch sec; seqNo:16944; 4060383.21 north; 593369.58 east 2021-02-13T00:55:22Z- AHRS_M2 msg: 1613177722.86 epoch sec; seqNo:16810 2021-02-13T00:55:22Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.17 psi 2021-02-13T00:55:22Z- handleDepth()-Depth_Keller msg: 1613177722861 epoch sec; seqNo:16945; depth 13.29 2021-02-13T00:55:22Z- handleDvl() - RDI_Pathfinder msg: 1613177722.87 epoch sec; seqNo:8182 2021-02-13T00:55:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 99.00 , 78.71 , 108.46 2021-02-13T00:55:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.13 , 0.08 2021-02-13T00:55:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177722.88 epoch sec; seqNo:16945; 4060383.59 north; 593369.58 east 2021-02-13T00:55:23Z- AHRS_M2 msg: 1613177723.26 epoch sec; seqNo:16811 2021-02-13T00:55:23Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.17 psi 2021-02-13T00:55:23Z- handleDepth()-Depth_Keller msg: 1613177723266 epoch sec; seqNo:16946; depth 13.41 2021-02-13T00:55:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:23Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:23Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:23Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:23Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:23Z- handleDvl() - RDI_Pathfinder msg: 1613177723.27 epoch sec; seqNo:8183 2021-02-13T00:55:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.13 , 0.08 2021-02-13T00:55:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177723.28 epoch sec; seqNo:16946; 4060383.96 north; 593369.57 east 2021-02-13T00:55:23Z- AHRS_M2 msg: 1613177723.66 epoch sec; seqNo:16812 2021-02-13T00:55:23Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.17 psi 2021-02-13T00:55:23Z- handleDepth()-Depth_Keller msg: 1613177723669 epoch sec; seqNo:16947; depth 13.54 2021-02-13T00:55:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177723.68 epoch sec; seqNo:16947; 4060384.34 north; 593369.57 east 2021-02-13T00:55:24Z- AHRS_M2 msg: 1613177724.07 epoch sec; seqNo:16813 2021-02-13T00:55:24Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.17 psi 2021-02-13T00:55:24Z- handleDepth()-Depth_Keller msg: 1613177724073 epoch sec; seqNo:16948; depth 13.65 2021-02-13T00:55:24Z- handleDvl() - RDI_Pathfinder msg: 1613177724.09 epoch sec; seqNo:8184 2021-02-13T00:55:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.21 , 97.44 , 78.51 , 108.26 2021-02-13T00:55:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , 0.14 2021-02-13T00:55:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177724.10 epoch sec; seqNo:16948; 4060384.34 north; 593369.57 east 2021-02-13T00:55:24Z- AHRS_M2 msg: 1613177724.48 epoch sec; seqNo:16814 2021-02-13T00:55:24Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.17 psi 2021-02-13T00:55:24Z- handleDepth()-Depth_Keller msg: 1613177724481 epoch sec; seqNo:16949; depth 13.77 2021-02-13T00:55:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:24Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:24Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:24Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:24Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:24Z- handleDvl() - RDI_Pathfinder msg: 1613177724.48 epoch sec; seqNo:8185 2021-02-13T00:55:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , 0.14 2021-02-13T00:55:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177724.49 epoch sec; seqNo:16949; 4060384.72 north; 593369.57 east 2021-02-13T00:55:24Z- AHRS_M2 msg: 1613177724.88 epoch sec; seqNo:16815 2021-02-13T00:55:24Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.17 psi 2021-02-13T00:55:24Z- handleDepth()-Depth_Keller msg: 1613177724881 epoch sec; seqNo:16950; depth 13.90 2021-02-13T00:55:24Z- handleDvl() - RDI_Pathfinder msg: 1613177724.89 epoch sec; seqNo:8186 2021-02-13T00:55:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 98.86 , 78.17 , 106.16 2021-02-13T00:55:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.06 , 0.16 2021-02-13T00:55:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177724.91 epoch sec; seqNo:16950; 4060385.11 north; 593370.78 east 2021-02-13T00:55:25Z- AHRS_M2 msg: 1613177725.28 epoch sec; seqNo:16816 2021-02-13T00:55:25Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.17 psi 2021-02-13T00:55:25Z- handleDepth()-Depth_Keller msg: 1613177725285 epoch sec; seqNo:16951; depth 14.01 2021-02-13T00:55:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177725.30 epoch sec; seqNo:16951; 4060385.11 north; 593370.78 east 2021-02-13T00:55:25Z- AHRS_M2 msg: 1613177725.68 epoch sec; seqNo:16817 2021-02-13T00:55:25Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.18 psi 2021-02-13T00:55:25Z- handleDepth()-Depth_Keller msg: 1613177725691 epoch sec; seqNo:16952; depth 14.14 2021-02-13T00:55:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:25Z- handleDvl() - RDI_Pathfinder msg: 1613177725.69 epoch sec; seqNo:8187 2021-02-13T00:55:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.06 , 0.16 2021-02-13T00:55:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177725.70 epoch sec; seqNo:16952; 4060385.49 north; 593370.78 east 2021-02-13T00:55:25Z- LcmTrn::handleMessages() - handling 45 messages took 0.67 ms and 132 calls... 2021-02-13T00:55:25Z- waiting for 1613177723.84, time is 1613177725.70 2021-02-13T00:55:25Z- LcmTrn::updateTrn() equating pose 1613177725.70 and meas times 1613177725.70 2021-02-13T00:55:25Z- LcmTrn::updateTrn() >>>> pose time 1613177725.70, meas time 1613177725.70 <<<< 2021-02-13T00:55:25Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177725.70, ping # 0. 2021-02-13T00:55:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177725.70 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:55:25Z- TerrainNav::motionUpdate() - currEstimate = (4060385, 593370.78, 14.14) 2021-02-13T00:55:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:55:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:55:25Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:55:25Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:55:25Z- TNavBF::Projecting Measurements 2021-02-13T00:55:25Z- TNavFilter::Measurement from time = 1613177725.70 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:55:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:55:25Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:55:25Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:55:25Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:55:25Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:55:25Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:55:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:55:25Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 682.5 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:55:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:55:25Z- TNavBF::mmse selected from filter -1 2021-02-13T00:55:25Z- variances([0], [2], and psi_berg) = 682.517804 832.989561 0.000000 2021-02-13T00:55:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:55:25Z- LcmTrn::publishEstimates() - MLE : 4060384.99 593388.28 14.14 2021-02-13T00:55:25Z- LcmTrn::publishEstimates() - MMSE : 4060385.49 593370.78 14.14 2021-02-13T00:55:25Z- LcmTrn::publishEstimates() - COVAR: 682.52 832.99 1.17 0.00 2021-02-13T00:55:26Z- AHRS_M2 msg: 1613177726.09 epoch sec; seqNo:16818 2021-02-13T00:55:26Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.18 psi 2021-02-13T00:55:26Z- handleDepth()-Depth_Keller msg: 1613177726093 epoch sec; seqNo:16953; depth 14.25 2021-02-13T00:55:26Z- handleDvl() - RDI_Pathfinder msg: 1613177726.11 epoch sec; seqNo:8188 2021-02-13T00:55:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 97.64 , 78.17 , 109.81 2021-02-13T00:55:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.18 , 0.09 2021-02-13T00:55:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177726.12 epoch sec; seqNo:16953; 4060385.49 north; 593370.78 east 2021-02-13T00:55:26Z- AHRS_M2 msg: 1613177726.49 epoch sec; seqNo:16819 2021-02-13T00:55:26Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.19 psi 2021-02-13T00:55:26Z- handleDepth()-Depth_Keller msg: 1613177726501 epoch sec; seqNo:16954; depth 14.35 2021-02-13T00:55:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177726.51 epoch sec; seqNo:16954; 4060385.87 north; 593370.77 east 2021-02-13T00:55:26Z- AHRS_M2 msg: 1613177726.90 epoch sec; seqNo:16820 2021-02-13T00:55:26Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.19 psi 2021-02-13T00:55:26Z- handleDepth()-Depth_Keller msg: 1613177726901 epoch sec; seqNo:16955; depth 14.47 2021-02-13T00:55:26Z- handleDvl() - RDI_Pathfinder msg: 1613177726.91 epoch sec; seqNo:8189 2021-02-13T00:55:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.48 , 99.06 , 78.78 , 108.53 2021-02-13T00:55:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.20 , 0.02 2021-02-13T00:55:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177726.92 epoch sec; seqNo:16955; 4060386.26 north; 593371.99 east 2021-02-13T00:55:27Z- AHRS_M2 msg: 1613177727.30 epoch sec; seqNo:16821 2021-02-13T00:55:27Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.20 psi 2021-02-13T00:55:27Z- handleDepth()-Depth_Keller msg: 1613177727306 epoch sec; seqNo:16956; depth 14.58 2021-02-13T00:55:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177727.32 epoch sec; seqNo:16956; 4060386.26 north; 593371.99 east 2021-02-13T00:55:27Z- AHRS_M2 msg: 1613177727.70 epoch sec; seqNo:16822 2021-02-13T00:55:27Z- AHRS_M2 msg: 0.58 phi; -0.29 theta; -0.21 psi 2021-02-13T00:55:27Z- handleDepth()-Depth_Keller msg: 1613177727730 epoch sec; seqNo:16957; depth 14.70 2021-02-13T00:55:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177727.74 epoch sec; seqNo:16957; 4060386.64 north; 593371.99 east 2021-02-13T00:55:28Z- AHRS_M2 msg: 1613177728.11 epoch sec; seqNo:16823 2021-02-13T00:55:28Z- AHRS_M2 msg: 0.58 phi; -0.29 theta; -0.22 psi 2021-02-13T00:55:28Z- handleDepth()-Depth_Keller msg: 1613177728173 epoch sec; seqNo:16958; depth 14.82 2021-02-13T00:55:28Z- handleDvl() - RDI_Pathfinder msg: 1613177728.21 epoch sec; seqNo:8190 2021-02-13T00:55:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.13 , 100.42 , 78.78 , 107.18 2021-02-13T00:55:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.17 , -0.01 2021-02-13T00:55:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177728.22 epoch sec; seqNo:16958; 4060387.02 north; 593371.98 east 2021-02-13T00:55:28Z- AHRS_M2 msg: 1613177728.51 epoch sec; seqNo:16824 2021-02-13T00:55:28Z- AHRS_M2 msg: 0.58 phi; -0.29 theta; -0.23 psi 2021-02-13T00:55:28Z- handleDepth()-Depth_Keller msg: 1613177728568 epoch sec; seqNo:16959; depth 14.95 2021-02-13T00:55:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177728.58 epoch sec; seqNo:16959; 4060387.40 north; 593371.98 east 2021-02-13T00:55:28Z- AHRS_M2 msg: 1613177728.92 epoch sec; seqNo:16825 2021-02-13T00:55:28Z- AHRS_M2 msg: 0.57 phi; -0.29 theta; -0.23 psi 2021-02-13T00:55:28Z- handleDepth()-Depth_Keller msg: 1613177728953 epoch sec; seqNo:16960; depth 15.05 2021-02-13T00:55:28Z- handleDvl() - RDI_Pathfinder msg: 1613177728.97 epoch sec; seqNo:8191 2021-02-13T00:55:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.65 , 101.29 , 78.17 , 109.81 2021-02-13T00:55:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.13 , 0.01 2021-02-13T00:55:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177728.98 epoch sec; seqNo:16960; 4060387.41 north; 593373.20 east 2021-02-13T00:55:29Z- AHRS_M2 msg: 1613177729.32 epoch sec; seqNo:16826 2021-02-13T00:55:29Z- AHRS_M2 msg: 0.57 phi; -0.29 theta; -0.23 psi 2021-02-13T00:55:29Z- handleDepth()-Depth_Keller msg: 1613177729338 epoch sec; seqNo:16961; depth 15.16 2021-02-13T00:55:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:29Z- handleDvl() - RDI_Pathfinder msg: 1613177729.34 epoch sec; seqNo:8192 2021-02-13T00:55:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.13 , 0.01 2021-02-13T00:55:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177729.35 epoch sec; seqNo:16961; 4060387.79 north; 593373.19 east 2021-02-13T00:55:29Z- AHRS_M2 msg: 1613177729.72 epoch sec; seqNo:16827 2021-02-13T00:55:29Z- AHRS_M2 msg: 0.57 phi; -0.29 theta; -0.22 psi 2021-02-13T00:55:29Z- handleDepth()-Depth_Keller msg: 1613177729729 epoch sec; seqNo:16962; depth 15.25 2021-02-13T00:55:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177729.74 epoch sec; seqNo:16962; 4060388.17 north; 593373.19 east 2021-02-13T00:55:30Z- AHRS_M2 msg: 1613177730.13 epoch sec; seqNo:16828 2021-02-13T00:55:30Z- AHRS_M2 msg: 0.56 phi; -0.29 theta; -0.22 psi 2021-02-13T00:55:30Z- handleDepth()-Depth_Keller msg: 1613177730139 epoch sec; seqNo:16963; depth 15.36 2021-02-13T00:55:30Z- handleDvl() - RDI_Pathfinder msg: 1613177730.15 epoch sec; seqNo:8193 2021-02-13T00:55:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.78 , 101.77 , 78.78 , 105.82 2021-02-13T00:55:30Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.12 , 0.00 2021-02-13T00:55:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177730.16 epoch sec; seqNo:16963; 4060388.55 north; 593373.19 east 2021-02-13T00:55:30Z- AHRS_M2 msg: 1613177730.53 epoch sec; seqNo:16829 2021-02-13T00:55:30Z- AHRS_M2 msg: 0.56 phi; -0.29 theta; -0.21 psi 2021-02-13T00:55:30Z- handleDepth()-Depth_Keller msg: 1613177730542 epoch sec; seqNo:16964; depth 15.47 2021-02-13T00:55:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:30Z- handleDvl() - RDI_Pathfinder msg: 1613177730.54 epoch sec; seqNo:8194 2021-02-13T00:55:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:30Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.12 , 0.00 2021-02-13T00:55:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177730.56 epoch sec; seqNo:16964; 4060388.55 north; 593373.19 east 2021-02-13T00:55:30Z- AHRS_M2 msg: 1613177730.94 epoch sec; seqNo:16830 2021-02-13T00:55:30Z- AHRS_M2 msg: 0.56 phi; -0.28 theta; -0.20 psi 2021-02-13T00:55:30Z- handleDepth()-Depth_Keller msg: 1613177730940 epoch sec; seqNo:16965; depth 15.57 2021-02-13T00:55:30Z- handleDvl() - RDI_Pathfinder msg: 1613177730.95 epoch sec; seqNo:8195 2021-02-13T00:55:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.39 , 100.08 , 78.17 , 104.94 2021-02-13T00:55:30Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , 0.08 , -0.05 2021-02-13T00:55:30Z- LcmTrn::handleMessages() - handling 46 messages took 0.80 ms and 130 calls... 2021-02-13T00:55:30Z- waiting for 1613177728.70, time is 1613177730.95 2021-02-13T00:55:30Z- LcmTrn::updateTrn() equating pose 1613177730.95 and meas times 1613177730.95 2021-02-13T00:55:30Z- LcmTrn::updateTrn() >>>> pose time 1613177730.95, meas time 1613177730.95 <<<< 2021-02-13T00:55:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177730.95 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:55:30Z- TerrainNav::motionUpdate() - currEstimate = (4060389, 593373.19, 15.57) 2021-02-13T00:55:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:55:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:55:30Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177730.95 sec.; No relevant navigation data available 2021-02-13T00:55:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:55:30Z- TNavBF::mmse selected from filter -1 2021-02-13T00:55:30Z- variances([0], [2], and psi_berg) = 682.473370 832.989542 0.000000 2021-02-13T00:55:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:55:30Z- LcmTrn::publishEstimates() - MLE : 4060388.05 593390.69 15.57 2021-02-13T00:55:30Z- LcmTrn::publishEstimates() - MMSE : 4060388.55 593373.19 15.57 2021-02-13T00:55:30Z- LcmTrn::publishEstimates() - COVAR: 682.47 832.99 1.37 0.00 2021-02-13T00:55:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177730.99 epoch sec; seqNo:16965; 4060388.94 north; 593374.40 east 2021-02-13T00:55:31Z- AHRS_M2 msg: 1613177731.34 epoch sec; seqNo:16831 2021-02-13T00:55:31Z- AHRS_M2 msg: 0.57 phi; -0.28 theta; -0.19 psi 2021-02-13T00:55:31Z- handleDepth()-Depth_Keller msg: 1613177731349 epoch sec; seqNo:16966; depth 15.69 2021-02-13T00:55:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177731.36 epoch sec; seqNo:16966; 4060389.32 north; 593374.40 east 2021-02-13T00:55:31Z- AHRS_M2 msg: 1613177731.74 epoch sec; seqNo:16832 2021-02-13T00:55:31Z- AHRS_M2 msg: 0.57 phi; -0.28 theta; -0.19 psi 2021-02-13T00:55:31Z- handleDepth()-Depth_Keller msg: 1613177731761 epoch sec; seqNo:16967; depth 15.78 2021-02-13T00:55:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:31Z- handleDvl() - RDI_Pathfinder msg: 1613177731.76 epoch sec; seqNo:8196 2021-02-13T00:55:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:31Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , 0.08 , -0.05 2021-02-13T00:55:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177731.77 epoch sec; seqNo:16967; 4060389.70 north; 593374.39 east 2021-02-13T00:55:32Z- AHRS_M2 msg: 1613177732.16 epoch sec; seqNo:16833 2021-02-13T00:55:32Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.18 psi 2021-02-13T00:55:32Z- handleDepth()-Depth_Keller msg: 1613177732166 epoch sec; seqNo:16968; depth 15.89 2021-02-13T00:55:32Z- handleDvl() - RDI_Pathfinder msg: 1613177732.18 epoch sec; seqNo:8197 2021-02-13T00:55:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.24 , 96.49 , 78.24 , 102.58 2021-02-13T00:55:32Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , 0.02 , -0.15 2021-02-13T00:55:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177732.19 epoch sec; seqNo:16968; 4060390.08 north; 593374.39 east 2021-02-13T00:55:32Z- AHRS_M2 msg: 1613177732.56 epoch sec; seqNo:16834 2021-02-13T00:55:32Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.18 psi 2021-02-13T00:55:32Z- handleDepth()-Depth_Keller msg: 1613177732561 epoch sec; seqNo:16969; depth 16.01 2021-02-13T00:55:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177732.57 epoch sec; seqNo:16969; 4060390.46 north; 593374.39 east 2021-02-13T00:55:32Z- AHRS_M2 msg: 1613177732.96 epoch sec; seqNo:16835 2021-02-13T00:55:32Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.17 psi 2021-02-13T00:55:32Z- handleDepth()-Depth_Keller msg: 1613177732969 epoch sec; seqNo:16970; depth 16.12 2021-02-13T00:55:32Z- handleDvl() - RDI_Pathfinder msg: 1613177732.98 epoch sec; seqNo:8198 2021-02-13T00:55:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 97.85 , 78.37 , 102.71 2021-02-13T00:55:32Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.07 , -0.21 2021-02-13T00:55:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177732.99 epoch sec; seqNo:16970; 4060390.85 north; 593375.60 east 2021-02-13T00:55:33Z- AHRS_M2 msg: 1613177733.36 epoch sec; seqNo:16836 2021-02-13T00:55:33Z- AHRS_M2 msg: 0.59 phi; -0.28 theta; -0.17 psi 2021-02-13T00:55:33Z- handleDepth()-Depth_Keller msg: 1613177733367 epoch sec; seqNo:16971; depth 16.21 2021-02-13T00:55:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177733.38 epoch sec; seqNo:16971; 4060391.23 north; 593375.60 east 2021-02-13T00:55:33Z- AHRS_M2 msg: 1613177733.76 epoch sec; seqNo:16837 2021-02-13T00:55:33Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.17 psi 2021-02-13T00:55:33Z- handleDepth()-Depth_Keller msg: 1613177733773 epoch sec; seqNo:16972; depth 16.32 2021-02-13T00:55:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177733.80 epoch sec; seqNo:16972; 4060391.61 north; 593375.59 east 2021-02-13T00:55:34Z- AHRS_M2 msg: 1613177734.17 epoch sec; seqNo:16838 2021-02-13T00:55:34Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.16 psi 2021-02-13T00:55:34Z- handleDepth()-Depth_Keller msg: 1613177734173 epoch sec; seqNo:16973; depth 16.42 2021-02-13T00:55:34Z- handleDvl() - RDI_Pathfinder msg: 1613177734.18 epoch sec; seqNo:8199 2021-02-13T00:55:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.37 , 97.85 , 78.37 , 101.50 2021-02-13T00:55:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.16 , -0.17 2021-02-13T00:55:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177734.19 epoch sec; seqNo:16973; 4060391.99 north; 593375.59 east 2021-02-13T00:55:34Z- AHRS_M2 msg: 1613177734.57 epoch sec; seqNo:16839 2021-02-13T00:55:34Z- AHRS_M2 msg: 0.58 phi; -0.29 theta; -0.16 psi 2021-02-13T00:55:34Z- handleDepth()-Depth_Keller msg: 1613177734581 epoch sec; seqNo:16974; depth 16.53 2021-02-13T00:55:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177734.59 epoch sec; seqNo:16974; 4060392.37 north; 593375.59 east 2021-02-13T00:55:34Z- AHRS_M2 msg: 1613177734.98 epoch sec; seqNo:16840 2021-02-13T00:55:34Z- AHRS_M2 msg: 0.58 phi; -0.29 theta; -0.17 psi 2021-02-13T00:55:34Z- handleDepth()-Depth_Keller msg: 1613177734981 epoch sec; seqNo:16975; depth 16.64 2021-02-13T00:55:35Z- handleDvl() - RDI_Pathfinder msg: 1613177734.99 epoch sec; seqNo:8200 2021-02-13T00:55:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 97.85 , 78.37 , 102.71 2021-02-13T00:55:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.20 , -0.06 2021-02-13T00:55:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177735.01 epoch sec; seqNo:16975; 4060392.74 north; 593375.58 east 2021-02-13T00:55:35Z- AHRS_M2 msg: 1613177735.38 epoch sec; seqNo:16841 2021-02-13T00:55:35Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.17 psi 2021-02-13T00:55:35Z- handleDepth()-Depth_Keller msg: 1613177735389 epoch sec; seqNo:16976; depth 16.74 2021-02-13T00:55:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:35Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:35Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:35Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:35Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:35Z- handleDvl() - RDI_Pathfinder msg: 1613177735.39 epoch sec; seqNo:8201 2021-02-13T00:55:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.20 , -0.06 2021-02-13T00:55:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177735.40 epoch sec; seqNo:16976; 4060393.12 north; 593375.58 east 2021-02-13T00:55:35Z- AHRS_M2 msg: 1613177735.79 epoch sec; seqNo:16842 2021-02-13T00:55:35Z- AHRS_M2 msg: 0.57 phi; -0.30 theta; -0.18 psi 2021-02-13T00:55:35Z- handleDepth()-Depth_Keller msg: 1613177735793 epoch sec; seqNo:16977; depth 16.84 2021-02-13T00:55:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177735.81 epoch sec; seqNo:16977; 4060393.50 north; 593375.57 east 2021-02-13T00:55:36Z- LcmTrn::handleMessages() - handling 43 messages took 0.63 ms and 131 calls... 2021-02-13T00:55:36Z- waiting for 1613177733.95, time is 1613177735.81 2021-02-13T00:55:36Z- LcmTrn::updateTrn() >>>> pose time 1613177735.81, meas time 1613177735.39 <<<< 2021-02-13T00:55:36Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177735.39, ping # 0. 2021-02-13T00:55:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177735.39 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:55:36Z- TerrainNav::motionUpdate() - currEstimate = (4060394, 593375.57, 16.84) 2021-02-13T00:55:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:55:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:55:36Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:55:36Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:55:36Z- TNavBF::Projecting Measurements 2021-02-13T00:55:36Z- TNavFilter::Measurement from time = 1613177735.39 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:55:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:55:36Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:55:36Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:55:36Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:55:36Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:55:36Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:55:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:55:36Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 682.4 EastVariance: 833.0 DepthVariance: 1.6 this->DepthBiasCov: 1.6 2021-02-13T00:55:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:55:36Z- TNavBF::mmse selected from filter -1 2021-02-13T00:55:36Z- variances([0], [2], and psi_berg) = 682.407385 832.989513 0.000000 2021-02-13T00:55:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:55:36Z- LcmTrn::publishEstimates() - MLE : 4060393.00 593393.07 16.84 2021-02-13T00:55:36Z- LcmTrn::publishEstimates() - MMSE : 4060393.50 593375.57 16.84 2021-02-13T00:55:36Z- LcmTrn::publishEstimates() - COVAR: 682.41 832.99 1.65 0.00 2021-02-13T00:55:36Z- AHRS_M2 msg: 1613177736.19 epoch sec; seqNo:16843 2021-02-13T00:55:36Z- AHRS_M2 msg: 0.57 phi; -0.30 theta; -0.18 psi 2021-02-13T00:55:36Z- handleDepth()-Depth_Keller msg: 1613177736195 epoch sec; seqNo:16978; depth 16.94 2021-02-13T00:55:36Z- handleDvl() - RDI_Pathfinder msg: 1613177736.21 epoch sec; seqNo:8202 2021-02-13T00:55:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.37 , 97.85 , 78.37 , 102.71 2021-02-13T00:55:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.16 , 0.06 2021-02-13T00:55:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177736.22 epoch sec; seqNo:16978; 4060393.51 north; 593376.79 east 2021-02-13T00:55:36Z- AHRS_M2 msg: 1613177736.59 epoch sec; seqNo:16844 2021-02-13T00:55:36Z- AHRS_M2 msg: 0.57 phi; -0.30 theta; -0.18 psi 2021-02-13T00:55:36Z- handleDepth()-Depth_Keller msg: 1613177736597 epoch sec; seqNo:16979; depth 17.06 2021-02-13T00:55:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:36Z- handleDvl() - RDI_Pathfinder msg: 1613177736.60 epoch sec; seqNo:8203 2021-02-13T00:55:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.16 , 0.06 2021-02-13T00:55:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177736.62 epoch sec; seqNo:16979; 4060393.89 north; 593376.79 east 2021-02-13T00:55:36Z- AHRS_M2 msg: 1613177736.99 epoch sec; seqNo:16845 2021-02-13T00:55:36Z- AHRS_M2 msg: 0.57 phi; -0.30 theta; -0.17 psi 2021-02-13T00:55:37Z- handleDepth()-Depth_Keller msg: 1613177737005 epoch sec; seqNo:16980; depth 17.17 2021-02-13T00:55:37Z- handleDvl() - RDI_Pathfinder msg: 1613177737.02 epoch sec; seqNo:8204 2021-02-13T00:55:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.37 , 97.85 , 71.11 , 102.71 2021-02-13T00:55:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , 0.14 2021-02-13T00:55:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177737.03 epoch sec; seqNo:16980; 4060394.27 north; 593376.79 east 2021-02-13T00:55:37Z- AHRS_M2 msg: 1613177737.40 epoch sec; seqNo:16846 2021-02-13T00:55:37Z- AHRS_M2 msg: 0.57 phi; -0.30 theta; -0.17 psi 2021-02-13T00:55:37Z- handleDepth()-Depth_Keller msg: 1613177737407 epoch sec; seqNo:16981; depth 17.28 2021-02-13T00:55:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177737.42 epoch sec; seqNo:16981; 4060394.65 north; 593376.78 east 2021-02-13T00:55:37Z- AHRS_M2 msg: 1613177737.80 epoch sec; seqNo:16847 2021-02-13T00:55:37Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.17 psi 2021-02-13T00:55:37Z- handleDepth()-Depth_Keller msg: 1613177737813 epoch sec; seqNo:16982; depth 17.41 2021-02-13T00:55:37Z- handleDvl() - RDI_Pathfinder msg: 1613177737.83 epoch sec; seqNo:8205 2021-02-13T00:55:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 71.11 , 104.07 2021-02-13T00:55:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , 0.18 2021-02-13T00:55:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177737.85 epoch sec; seqNo:16982; 4060394.65 north; 593376.78 east 2021-02-13T00:55:38Z- AHRS_M2 msg: 1613177738.21 epoch sec; seqNo:16848 2021-02-13T00:55:38Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.17 psi 2021-02-13T00:55:38Z- handleDepth()-Depth_Keller msg: 1613177738217 epoch sec; seqNo:16983; depth 17.52 2021-02-13T00:55:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177738.23 epoch sec; seqNo:16983; 4060395.03 north; 593376.78 east 2021-02-13T00:55:38Z- AHRS_M2 msg: 1613177738.61 epoch sec; seqNo:16849 2021-02-13T00:55:38Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.17 psi 2021-02-13T00:55:38Z- handleDepth()-Depth_Keller msg: 1613177738618 epoch sec; seqNo:16984; depth 17.63 2021-02-13T00:55:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177738.63 epoch sec; seqNo:16984; 4060395.03 north; 593376.78 east 2021-02-13T00:55:39Z- AHRS_M2 msg: 1613177739.02 epoch sec; seqNo:16850 2021-02-13T00:55:39Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.16 psi 2021-02-13T00:55:39Z- handleDepth()-Depth_Keller msg: 1613177739030 epoch sec; seqNo:16985; depth 17.74 2021-02-13T00:55:39Z- handleDvl() - RDI_Pathfinder msg: 1613177739.04 epoch sec; seqNo:8206 2021-02-13T00:55:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 97.91 , 69.07 , 102.78 2021-02-13T00:55:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , 0.16 2021-02-13T00:55:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177739.05 epoch sec; seqNo:16985; 4060395.42 north; 593377.99 east 2021-02-13T00:55:39Z- AHRS_M2 msg: 1613177739.42 epoch sec; seqNo:16851 2021-02-13T00:55:39Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.16 psi 2021-02-13T00:55:39Z- handleDepth()-Depth_Keller msg: 1613177739425 epoch sec; seqNo:16986; depth 17.87 2021-02-13T00:55:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177739.44 epoch sec; seqNo:16986; 4060395.80 north; 593377.99 east 2021-02-13T00:55:39Z- AHRS_M2 msg: 1613177739.82 epoch sec; seqNo:16852 2021-02-13T00:55:39Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.17 psi 2021-02-13T00:55:39Z- handleDepth()-Depth_Keller msg: 1613177739829 epoch sec; seqNo:16987; depth 17.97 2021-02-13T00:55:39Z- handleDvl() - RDI_Pathfinder msg: 1613177739.84 epoch sec; seqNo:8207 2021-02-13T00:55:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 91.96 , 71.11 , 104.13 2021-02-13T00:55:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.11 , 0.11 2021-02-13T00:55:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177739.85 epoch sec; seqNo:16987; 4060396.18 north; 593377.98 east 2021-02-13T00:55:40Z- AHRS_M2 msg: 1613177740.23 epoch sec; seqNo:16853 2021-02-13T00:55:40Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.17 psi 2021-02-13T00:55:40Z- handleDepth()-Depth_Keller msg: 1613177740245 epoch sec; seqNo:16988; depth 18.09 2021-02-13T00:55:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:40Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:40Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:40Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:40Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:40Z- handleDvl() - RDI_Pathfinder msg: 1613177740.25 epoch sec; seqNo:8208 2021-02-13T00:55:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.11 , 0.11 2021-02-13T00:55:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177740.26 epoch sec; seqNo:16988; 4060396.18 north; 593377.98 east 2021-02-13T00:55:40Z- AHRS_M2 msg: 1613177740.63 epoch sec; seqNo:16854 2021-02-13T00:55:40Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.18 psi 2021-02-13T00:55:40Z- handleDepth()-Depth_Keller msg: 1613177740637 epoch sec; seqNo:16989; depth 18.19 2021-02-13T00:55:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177740.65 epoch sec; seqNo:16989; 4060396.56 north; 593377.98 east 2021-02-13T00:55:41Z- AHRS_M2 msg: 1613177741.04 epoch sec; seqNo:16855 2021-02-13T00:55:41Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.18 psi 2021-02-13T00:55:41Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-13T00:55:41Z- waiting for 1613177738.81, time is 1613177741.04 2021-02-13T00:55:41Z- LcmTrn::updateTrn() >>>> pose time 1613177741.04, meas time 1613177740.25 <<<< 2021-02-13T00:55:41Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177740.25, ping # 0. 2021-02-13T00:55:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177740.25 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:55:41Z- TerrainNav::motionUpdate() - currEstimate = (4060397, 593377.98, 18.19) 2021-02-13T00:55:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:55:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:55:41Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:55:41Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:55:41Z- TNavBF::Projecting Measurements 2021-02-13T00:55:41Z- TNavFilter::Measurement from time = 1613177740.25 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:55:41Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:55:41Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:55:41Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:55:41Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:55:41Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:55:41Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:55:41Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:55:41Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 682.4 EastVariance: 833.0 DepthVariance: 1.8 this->DepthBiasCov: 1.8 2021-02-13T00:55:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:55:41Z- TNavBF::mmse selected from filter -1 2021-02-13T00:55:41Z- variances([0], [2], and psi_berg) = 682.349249 832.989488 0.000000 2021-02-13T00:55:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:55:41Z- LcmTrn::publishEstimates() - MLE : 4060396.06 593395.48 18.19 2021-02-13T00:55:41Z- LcmTrn::publishEstimates() - MMSE : 4060396.56 593377.98 18.19 2021-02-13T00:55:41Z- LcmTrn::publishEstimates() - COVAR: 682.35 832.99 1.85 0.00 2021-02-13T00:55:41Z- handleDepth()-Depth_Keller msg: 1613177741049 epoch sec; seqNo:16990; depth 18.29 2021-02-13T00:55:41Z- handleDvl() - RDI_Pathfinder msg: 1613177741.06 epoch sec; seqNo:8209 2021-02-13T00:55:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 69.07 , 104.13 2021-02-13T00:55:41Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.11 , 0.05 2021-02-13T00:55:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177741.08 epoch sec; seqNo:16990; 4060396.94 north; 593377.98 east 2021-02-13T00:55:41Z- AHRS_M2 msg: 1613177741.44 epoch sec; seqNo:16856 2021-02-13T00:55:41Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.18 psi 2021-02-13T00:55:41Z- handleDepth()-Depth_Keller msg: 1613177741507 epoch sec; seqNo:16991; depth 18.41 2021-02-13T00:55:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:41Z- handleDvl() - RDI_Pathfinder msg: 1613177741.51 epoch sec; seqNo:8210 2021-02-13T00:55:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:41Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.11 , 0.05 2021-02-13T00:55:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177741.52 epoch sec; seqNo:16991; 4060397.33 north; 593379.19 east 2021-02-13T00:55:41Z- AHRS_M2 msg: 1613177741.84 epoch sec; seqNo:16857 2021-02-13T00:55:41Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.19 psi 2021-02-13T00:55:41Z- handleDepth()-Depth_Keller msg: 1613177741858 epoch sec; seqNo:16992; depth 18.55 2021-02-13T00:55:41Z- handleDvl() - RDI_Pathfinder msg: 1613177741.87 epoch sec; seqNo:8211 2021-02-13T00:55:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 69.07 , 101.63 2021-02-13T00:55:41Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.10 , -0.01 2021-02-13T00:55:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177741.88 epoch sec; seqNo:16992; 4060397.33 north; 593379.19 east 2021-02-13T00:55:42Z- AHRS_M2 msg: 1613177742.25 epoch sec; seqNo:16858 2021-02-13T00:55:42Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.18 psi 2021-02-13T00:55:42Z- handleDepth()-Depth_Keller msg: 1613177742345 epoch sec; seqNo:16993; depth 18.66 2021-02-13T00:55:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177742.36 epoch sec; seqNo:16993; 4060397.71 north; 593379.19 east 2021-02-13T00:55:42Z- AHRS_M2 msg: 1613177742.65 epoch sec; seqNo:16859 2021-02-13T00:55:42Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.18 psi 2021-02-13T00:55:42Z- handleDepth()-Depth_Keller msg: 1613177742697 epoch sec; seqNo:16994; depth 18.80 2021-02-13T00:55:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:42Z- handleDvl() - RDI_Pathfinder msg: 1613177742.70 epoch sec; seqNo:8212 2021-02-13T00:55:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:42Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.10 , -0.01 2021-02-13T00:55:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177742.72 epoch sec; seqNo:16994; 4060398.09 north; 593379.18 east 2021-02-13T00:55:43Z- AHRS_M2 msg: 1613177743.06 epoch sec; seqNo:16860 2021-02-13T00:55:43Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.18 psi 2021-02-13T00:55:43Z- handleDepth()-Depth_Keller msg: 1613177743081 epoch sec; seqNo:16995; depth 18.91 2021-02-13T00:55:43Z- handleDvl() - RDI_Pathfinder msg: 1613177743.10 epoch sec; seqNo:8213 2021-02-13T00:55:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 69.48 , 105.35 2021-02-13T00:55:43Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.04 , -0.01 2021-02-13T00:55:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177743.11 epoch sec; seqNo:16995; 4060398.48 north; 593380.40 east 2021-02-13T00:55:43Z- AHRS_M2 msg: 1613177743.46 epoch sec; seqNo:16861 2021-02-13T00:55:43Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.18 psi 2021-02-13T00:55:43Z- handleDepth()-Depth_Keller msg: 1613177743482 epoch sec; seqNo:16996; depth 19.03 2021-02-13T00:55:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177743.50 epoch sec; seqNo:16996; 4060398.86 north; 593380.40 east 2021-02-13T00:55:43Z- AHRS_M2 msg: 1613177743.86 epoch sec; seqNo:16862 2021-02-13T00:55:43Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.18 psi 2021-02-13T00:55:43Z- handleDepth()-Depth_Keller msg: 1613177743869 epoch sec; seqNo:16997; depth 19.15 2021-02-13T00:55:43Z- handleDvl() - RDI_Pathfinder msg: 1613177743.88 epoch sec; seqNo:8214 2021-02-13T00:55:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 78.44 , 101.56 2021-02-13T00:55:43Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.03 , -0.03 2021-02-13T00:55:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177743.90 epoch sec; seqNo:16997; 4060398.86 north; 593380.40 east 2021-02-13T00:55:44Z- AHRS_M2 msg: 1613177744.27 epoch sec; seqNo:16863 2021-02-13T00:55:44Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-13T00:55:44Z- handleDepth()-Depth_Keller msg: 1613177744273 epoch sec; seqNo:16998; depth 19.29 2021-02-13T00:55:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177744.29 epoch sec; seqNo:16998; 4060399.24 north; 593380.39 east 2021-02-13T00:55:44Z- AHRS_M2 msg: 1613177744.68 epoch sec; seqNo:16864 2021-02-13T00:55:44Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.17 psi 2021-02-13T00:55:44Z- handleDepth()-Depth_Keller msg: 1613177744687 epoch sec; seqNo:16999; depth 19.41 2021-02-13T00:55:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177744.69 epoch sec; seqNo:16999; 4060399.62 north; 593380.39 east 2021-02-13T00:55:45Z- AHRS_M2 msg: 1613177745.08 epoch sec; seqNo:16865 2021-02-13T00:55:45Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.17 psi 2021-02-13T00:55:45Z- handleDepth()-Depth_Keller msg: 1613177745085 epoch sec; seqNo:17000; depth 19.53 2021-02-13T00:55:45Z- handleDvl() - RDI_Pathfinder msg: 1613177745.10 epoch sec; seqNo:8215 2021-02-13T00:55:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.37 , 92.98 , 67.43 , 105.15 2021-02-13T00:55:45Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.03 , -0.05 2021-02-13T00:55:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177745.11 epoch sec; seqNo:17000; 4060400.01 north; 593381.61 east 2021-02-13T00:55:45Z- AHRS_M2 msg: 1613177745.48 epoch sec; seqNo:16866 2021-02-13T00:55:45Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.16 psi 2021-02-13T00:55:45Z- handleDepth()-Depth_Keller msg: 1613177745489 epoch sec; seqNo:17001; depth 19.66 2021-02-13T00:55:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177745.50 epoch sec; seqNo:17001; 4060400.39 north; 593381.60 east 2021-02-13T00:55:45Z- AHRS_M2 msg: 1613177745.88 epoch sec; seqNo:16867 2021-02-13T00:55:45Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.16 psi 2021-02-13T00:55:45Z- handleDepth()-Depth_Keller msg: 1613177745889 epoch sec; seqNo:17002; depth 19.78 2021-02-13T00:55:45Z- handleDvl() - RDI_Pathfinder msg: 1613177745.90 epoch sec; seqNo:8216 2021-02-13T00:55:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.64 , 98.12 , 68.66 , 102.98 2021-02-13T00:55:45Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.00 , -0.10 2021-02-13T00:55:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177745.91 epoch sec; seqNo:17002; 4060400.77 north; 593381.60 east 2021-02-13T00:55:46Z- LcmTrn::handleMessages() - handling 46 messages took 0.75 ms and 131 calls... 2021-02-13T00:55:46Z- waiting for 1613177744.04, time is 1613177745.91 2021-02-13T00:55:46Z- LcmTrn::updateTrn() equating pose 1613177745.91 and meas times 1613177745.91 2021-02-13T00:55:46Z- LcmTrn::updateTrn() >>>> pose time 1613177745.91, meas time 1613177745.91 <<<< 2021-02-13T00:55:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177745.91 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:55:46Z- TerrainNav::motionUpdate() - currEstimate = (4060401, 593381.60, 19.78) 2021-02-13T00:55:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:55:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:55:46Z- TNavBF::Projecting Measurements 2021-02-13T00:55:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:55:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:55:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:55:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:55:46Z- TNavFilter::4 good beams. 2021-02-13T00:55:46Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:55:46Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:55:46Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:55:46Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:55:46Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:55:46Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:55:46Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:55:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:55:46Z- TerrainMapOctree: (vehN, vehE ) = (4060400.77, 593381.60). 2021-02-13T00:55:46Z- TerrainMapOctree: Min Distance = 1214.63. 2021-02-13T00:55:46Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:55:46Z- There is no specific good beam that all particles have in common. 2021-02-13T00:55:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:55:46Z- Old Depth Bias: 0.00 DepthBiasCov: 2.13 K: 0.155 ExpectedMeasDiff: nan currMeas.covariance: 9.90 2021-02-13T00:55:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:55:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.80 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 15.40 2021-02-13T00:55:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:55:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.63 K: 0.149 ExpectedMeasDiff: nan currMeas.covariance: 7.54 2021-02-13T00:55:46Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:55:46Z- Old Depth Bias: 0.00 DepthBiasCov: 1.39 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 16.97 2021-02-13T00:55:46Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:55:46Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:55:46Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:55:46Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:55:46Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:46Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:55:46Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:55:46Z- Filter 0 No beams used in subcloud update 2021-02-13T00:55:46Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:55:46Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:55:46Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 682.3 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T00:55:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177745.91 sec. 2021-02-13T00:55:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:55:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:55:46Z- variances([0], [2], and psi_berg) = 682.296290 832.989465 0.000000 2021-02-13T00:55:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:55:46Z- LcmTrn::publishEstimates() - MLE : 4060400.27 593399.10 19.78 2021-02-13T00:55:46Z- LcmTrn::publishEstimates() - MMSE : 4060400.77 593381.60 19.78 2021-02-13T00:55:46Z- LcmTrn::publishEstimates() - COVAR: 682.30 832.99 1.29 0.00 2021-02-13T00:55:46Z- AHRS_M2 msg: 1613177746.29 epoch sec; seqNo:16868 2021-02-13T00:55:46Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.16 psi 2021-02-13T00:55:46Z- handleDepth()-Depth_Keller msg: 1613177746301 epoch sec; seqNo:17003; depth 19.91 2021-02-13T00:55:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:46Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:46Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:46Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:46Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:46Z- handleDvl() - RDI_Pathfinder msg: 1613177746.30 epoch sec; seqNo:8217 2021-02-13T00:55:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:46Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.00 , -0.10 2021-02-13T00:55:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177746.31 epoch sec; seqNo:17003; 4060401.14 north; 593381.59 east 2021-02-13T00:55:46Z- AHRS_M2 msg: 1613177746.70 epoch sec; seqNo:16869 2021-02-13T00:55:46Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.16 psi 2021-02-13T00:55:46Z- handleDepth()-Depth_Keller msg: 1613177746710 epoch sec; seqNo:17004; depth 20.03 2021-02-13T00:55:46Z- handleDvl() - RDI_Pathfinder msg: 1613177746.73 epoch sec; seqNo:8218 2021-02-13T00:55:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.38 , 90.68 , 67.84 , 101.63 2021-02-13T00:55:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.05 , -0.13 2021-02-13T00:55:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177746.74 epoch sec; seqNo:17004; 4060401.52 north; 593381.59 east 2021-02-13T00:55:47Z- AHRS_M2 msg: 1613177747.10 epoch sec; seqNo:16870 2021-02-13T00:55:47Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.16 psi 2021-02-13T00:55:47Z- handleDepth()-Depth_Keller msg: 1613177747105 epoch sec; seqNo:17005; depth 20.18 2021-02-13T00:55:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177747.12 epoch sec; seqNo:17005; 4060401.52 north; 593381.59 east 2021-02-13T00:55:47Z- AHRS_M2 msg: 1613177747.50 epoch sec; seqNo:16871 2021-02-13T00:55:47Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-13T00:55:47Z- handleDepth()-Depth_Keller msg: 1613177747513 epoch sec; seqNo:17006; depth 20.28 2021-02-13T00:55:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:47Z- handleDvl() - RDI_Pathfinder msg: 1613177747.52 epoch sec; seqNo:8219 2021-02-13T00:55:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.05 , -0.13 2021-02-13T00:55:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177747.52 epoch sec; seqNo:17006; 4060401.92 north; 593382.81 east 2021-02-13T00:55:47Z- AHRS_M2 msg: 1613177747.91 epoch sec; seqNo:16872 2021-02-13T00:55:47Z- AHRS_M2 msg: 0.64 phi; -0.33 theta; -0.17 psi 2021-02-13T00:55:47Z- handleDepth()-Depth_Keller msg: 1613177747922 epoch sec; seqNo:17007; depth 20.40 2021-02-13T00:55:47Z- handleDvl() - RDI_Pathfinder msg: 1613177747.93 epoch sec; seqNo:8220 2021-02-13T00:55:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.79 , 91.02 , 67.84 , 101.97 2021-02-13T00:55:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.14 , -0.11 2021-02-13T00:55:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177747.95 epoch sec; seqNo:17007; 4060402.29 north; 593382.80 east 2021-02-13T00:55:48Z- AHRS_M2 msg: 1613177748.31 epoch sec; seqNo:16873 2021-02-13T00:55:48Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.17 psi 2021-02-13T00:55:48Z- handleDepth()-Depth_Keller msg: 1613177748317 epoch sec; seqNo:17008; depth 20.54 2021-02-13T00:55:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177748.33 epoch sec; seqNo:17008; 4060402.67 north; 593382.80 east 2021-02-13T00:55:48Z- AHRS_M2 msg: 1613177748.71 epoch sec; seqNo:16874 2021-02-13T00:55:48Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.18 psi 2021-02-13T00:55:48Z- handleDepth()-Depth_Keller msg: 1613177748719 epoch sec; seqNo:17009; depth 20.66 2021-02-13T00:55:48Z- handleDvl() - RDI_Pathfinder msg: 1613177748.74 epoch sec; seqNo:8221 2021-02-13T00:55:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.97 , 91.02 , 66.62 , 104.40 2021-02-13T00:55:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.17 , -0.03 2021-02-13T00:55:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177748.75 epoch sec; seqNo:17009; 4060403.05 north; 593382.79 east 2021-02-13T00:55:49Z- AHRS_M2 msg: 1613177749.12 epoch sec; seqNo:16875 2021-02-13T00:55:49Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.19 psi 2021-02-13T00:55:49Z- handleDepth()-Depth_Keller msg: 1613177749127 epoch sec; seqNo:17010; depth 20.80 2021-02-13T00:55:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177749.15 epoch sec; seqNo:17010; 4060403.43 north; 593382.79 east 2021-02-13T00:55:49Z- AHRS_M2 msg: 1613177749.53 epoch sec; seqNo:16876 2021-02-13T00:55:49Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.19 psi 2021-02-13T00:55:49Z- handleDepth()-Depth_Keller msg: 1613177749535 epoch sec; seqNo:17011; depth 20.92 2021-02-13T00:55:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177749.54 epoch sec; seqNo:17011; 4060403.43 north; 593382.79 east 2021-02-13T00:55:49Z- AHRS_M2 msg: 1613177749.93 epoch sec; seqNo:16877 2021-02-13T00:55:49Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-13T00:55:49Z- handleDepth()-Depth_Keller msg: 1613177749931 epoch sec; seqNo:17012; depth 21.06 2021-02-13T00:55:49Z- handleDvl() - RDI_Pathfinder msg: 1613177749.94 epoch sec; seqNo:8222 2021-02-13T00:55:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 90.95 , 78.78 , 103.12 2021-02-13T00:55:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , -0.14 , 0.14 2021-02-13T00:55:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177749.96 epoch sec; seqNo:17012; 4060403.81 north; 593382.78 east 2021-02-13T00:55:50Z- AHRS_M2 msg: 1613177750.33 epoch sec; seqNo:16878 2021-02-13T00:55:50Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-13T00:55:50Z- handleDepth()-Depth_Keller msg: 1613177750333 epoch sec; seqNo:17013; depth 21.18 2021-02-13T00:55:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177750.35 epoch sec; seqNo:17013; 4060404.19 north; 593382.78 east 2021-02-13T00:55:50Z- AHRS_M2 msg: 1613177750.74 epoch sec; seqNo:16879 2021-02-13T00:55:50Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-13T00:55:50Z- handleDepth()-Depth_Keller msg: 1613177750746 epoch sec; seqNo:17014; depth 21.31 2021-02-13T00:55:50Z- handleDvl() - RDI_Pathfinder msg: 1613177750.76 epoch sec; seqNo:8223 2021-02-13T00:55:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 91.96 , 67.03 , 102.92 2021-02-13T00:55:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , 0.14 2021-02-13T00:55:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177750.77 epoch sec; seqNo:17014; 4060404.20 north; 593384.00 east 2021-02-13T00:55:51Z- AHRS_M2 msg: 1613177751.14 epoch sec; seqNo:16880 2021-02-13T00:55:51Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.19 psi 2021-02-13T00:55:51Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-13T00:55:51Z- waiting for 1613177748.91, time is 1613177751.14 2021-02-13T00:55:51Z- LcmTrn::updateTrn() >>>> pose time 1613177751.14, meas time 1613177750.76 <<<< 2021-02-13T00:55:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177750.76 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:55:51Z- TerrainNav::motionUpdate() - currEstimate = (4060404, 593384.00, 21.31) 2021-02-13T00:55:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:55:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:55:51Z- TNavBF::Projecting Measurements 2021-02-13T00:55:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:55:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:55:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:55:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:55:51Z- TNavFilter::4 good beams. 2021-02-13T00:55:51Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:55:51Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:55:51Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:55:51Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:55:51Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:55:51Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:55:51Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:55:51Z- TNavBF::mmse selected from filter -1 2021-02-13T00:55:51Z- TerrainMapOctree: (vehN, vehE ) = (4060403.95, 593383.83). 2021-02-13T00:55:51Z- TerrainMapOctree: Min Distance = 1210.81. 2021-02-13T00:55:51Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:55:51Z- There is no specific good beam that all particles have in common. 2021-02-13T00:55:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:55:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.49 K: 0.125 ExpectedMeasDiff: nan currMeas.covariance: 8.66 2021-02-13T00:55:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:55:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 13.53 2021-02-13T00:55:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:55:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.119 ExpectedMeasDiff: nan currMeas.covariance: 7.19 2021-02-13T00:55:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:55:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 16.95 2021-02-13T00:55:51Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:55:51Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:55:51Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:55:51Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:55:51Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:55:51Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:55:51Z- Filter 0 No beams used in subcloud update 2021-02-13T00:55:51Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:55:51Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:55:51Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 682.3 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:55:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177750.76 sec. 2021-02-13T00:55:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:55:51Z- TNavBF::mmse selected from filter -1 2021-02-13T00:55:51Z- variances([0], [2], and psi_berg) = 682.239575 832.989440 0.000000 2021-02-13T00:55:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:55:51Z- LcmTrn::publishEstimates() - MLE : 4060403.70 593401.50 21.31 2021-02-13T00:55:51Z- LcmTrn::publishEstimates() - MMSE : 4060404.20 593384.00 21.31 2021-02-13T00:55:51Z- LcmTrn::publishEstimates() - COVAR: 682.24 832.99 1.02 0.00 2021-02-13T00:55:51Z- handleDepth()-Depth_Keller msg: 1613177751152 epoch sec; seqNo:17015; depth 21.43 2021-02-13T00:55:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:51Z- handleDvl() - RDI_Pathfinder msg: 1613177751.15 epoch sec; seqNo:8224 2021-02-13T00:55:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , 0.14 2021-02-13T00:55:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177751.17 epoch sec; seqNo:17015; 4060404.58 north; 593384.00 east 2021-02-13T00:55:51Z- AHRS_M2 msg: 1613177751.54 epoch sec; seqNo:16881 2021-02-13T00:55:51Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-13T00:55:51Z- handleDepth()-Depth_Keller msg: 1613177751549 epoch sec; seqNo:17016; depth 21.56 2021-02-13T00:55:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177751.56 epoch sec; seqNo:17016; 4060404.96 north; 593383.99 east 2021-02-13T00:55:51Z- AHRS_M2 msg: 1613177751.95 epoch sec; seqNo:16882 2021-02-13T00:55:51Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.19 psi 2021-02-13T00:55:51Z- handleDepth()-Depth_Keller msg: 1613177751951 epoch sec; seqNo:17017; depth 21.69 2021-02-13T00:55:51Z- handleDvl() - RDI_Pathfinder msg: 1613177751.96 epoch sec; seqNo:8225 2021-02-13T00:55:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.97 , 98.32 , 66.62 , 101.97 2021-02-13T00:55:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , 0.14 2021-02-13T00:55:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177751.97 epoch sec; seqNo:17017; 4060404.96 north; 593383.99 east 2021-02-13T00:55:52Z- AHRS_M2 msg: 1613177752.35 epoch sec; seqNo:16883 2021-02-13T00:55:52Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-13T00:55:52Z- handleDepth()-Depth_Keller msg: 1613177752362 epoch sec; seqNo:17018; depth 21.83 2021-02-13T00:55:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:52Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:52Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:52Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:52Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:52Z- handleDvl() - RDI_Pathfinder msg: 1613177752.36 epoch sec; seqNo:8226 2021-02-13T00:55:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , 0.14 2021-02-13T00:55:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177752.37 epoch sec; seqNo:17018; 4060405.34 north; 593383.99 east 2021-02-13T00:55:52Z- AHRS_M2 msg: 1613177752.75 epoch sec; seqNo:16884 2021-02-13T00:55:52Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.20 psi 2021-02-13T00:55:52Z- handleDepth()-Depth_Keller msg: 1613177752757 epoch sec; seqNo:17019; depth 21.95 2021-02-13T00:55:52Z- handleDvl() - RDI_Pathfinder msg: 1613177752.77 epoch sec; seqNo:8227 2021-02-13T00:55:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 98.18 , 66.62 , 101.83 2021-02-13T00:55:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.10 , 0.08 2021-02-13T00:55:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177752.79 epoch sec; seqNo:17019; 4060405.72 north; 593383.98 east 2021-02-13T00:55:53Z- AHRS_M2 msg: 1613177753.18 epoch sec; seqNo:16885 2021-02-13T00:55:53Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.20 psi 2021-02-13T00:55:53Z- handleDepth()-Depth_Keller msg: 1613177753181 epoch sec; seqNo:17020; depth 22.09 2021-02-13T00:55:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177753.19 epoch sec; seqNo:17020; 4060405.73 north; 593385.20 east 2021-02-13T00:55:53Z- AHRS_M2 msg: 1613177753.57 epoch sec; seqNo:16886 2021-02-13T00:55:53Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:55:53Z- handleDepth()-Depth_Keller msg: 1613177753577 epoch sec; seqNo:17021; depth 22.21 2021-02-13T00:55:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:53Z- handleDvl() - RDI_Pathfinder msg: 1613177753.58 epoch sec; seqNo:8228 2021-02-13T00:55:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.10 , 0.08 2021-02-13T00:55:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177753.59 epoch sec; seqNo:17021; 4060406.11 north; 593385.20 east 2021-02-13T00:55:53Z- AHRS_M2 msg: 1613177753.96 epoch sec; seqNo:16887 2021-02-13T00:55:53Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:55:53Z- handleDepth()-Depth_Keller msg: 1613177753969 epoch sec; seqNo:17022; depth 22.35 2021-02-13T00:55:53Z- handleDvl() - RDI_Pathfinder msg: 1613177753.99 epoch sec; seqNo:8229 2021-02-13T00:55:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.71 , 98.18 , 66.62 , 101.83 2021-02-13T00:55:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.08 , 0.02 2021-02-13T00:55:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177754.00 epoch sec; seqNo:17022; 4060406.49 north; 593385.20 east 2021-02-13T00:55:54Z- AHRS_M2 msg: 1613177754.37 epoch sec; seqNo:16888 2021-02-13T00:55:54Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:55:54Z- handleDepth()-Depth_Keller msg: 1613177754373 epoch sec; seqNo:17023; depth 22.47 2021-02-13T00:55:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177754.38 epoch sec; seqNo:17023; 4060406.49 north; 593385.20 east 2021-02-13T00:55:54Z- AHRS_M2 msg: 1613177754.78 epoch sec; seqNo:16889 2021-02-13T00:55:54Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:55:54Z- handleDepth()-Depth_Keller msg: 1613177754787 epoch sec; seqNo:17024; depth 22.59 2021-02-13T00:55:54Z- handleDvl() - RDI_Pathfinder msg: 1613177754.80 epoch sec; seqNo:8230 2021-02-13T00:55:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 66.62 , 102.92 2021-02-13T00:55:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.04 , -0.04 2021-02-13T00:55:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177754.82 epoch sec; seqNo:17024; 4060406.87 north; 593385.19 east 2021-02-13T00:55:55Z- AHRS_M2 msg: 1613177755.18 epoch sec; seqNo:16890 2021-02-13T00:55:55Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:55:55Z- handleDepth()-Depth_Keller msg: 1613177755246 epoch sec; seqNo:17025; depth 22.75 2021-02-13T00:55:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177755.26 epoch sec; seqNo:17025; 4060407.26 north; 593386.41 east 2021-02-13T00:55:55Z- AHRS_M2 msg: 1613177755.58 epoch sec; seqNo:16891 2021-02-13T00:55:55Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:55:55Z- handleDepth()-Depth_Keller msg: 1613177755670 epoch sec; seqNo:17026; depth 22.89 2021-02-13T00:55:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177755.68 epoch sec; seqNo:17026; 4060407.64 north; 593386.40 east 2021-02-13T00:55:55Z- AHRS_M2 msg: 1613177755.98 epoch sec; seqNo:16892 2021-02-13T00:55:55Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:55:56Z- handleDepth()-Depth_Keller msg: 1613177756029 epoch sec; seqNo:17027; depth 23.03 2021-02-13T00:55:56Z- handleDvl() - RDI_Pathfinder msg: 1613177756.05 epoch sec; seqNo:8231 2021-02-13T00:55:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 91.96 , 64.98 , 102.92 2021-02-13T00:55:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.06 , -0.07 2021-02-13T00:55:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177756.07 epoch sec; seqNo:17027; 4060408.02 north; 593386.40 east 2021-02-13T00:55:56Z- LcmTrn::handleMessages() - handling 46 messages took 0.70 ms and 132 calls... 2021-02-13T00:55:56Z- waiting for 1613177754.14, time is 1613177756.07 2021-02-13T00:55:56Z- LcmTrn::updateTrn() equating pose 1613177756.07 and meas times 1613177756.07 2021-02-13T00:55:56Z- LcmTrn::updateTrn() >>>> pose time 1613177756.07, meas time 1613177756.07 <<<< 2021-02-13T00:55:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177756.07 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:55:56Z- TerrainNav::motionUpdate() - currEstimate = (4060408, 593386.40, 23.03) 2021-02-13T00:55:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:55:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:55:56Z- TNavBF::Projecting Measurements 2021-02-13T00:55:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:55:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:55:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:55:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:55:56Z- TNavFilter::4 good beams. 2021-02-13T00:55:56Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:55:56Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:55:56Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:55:56Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:55:56Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:55:56Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:55:56Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:55:56Z- TNavBF::mmse selected from filter -1 2021-02-13T00:55:56Z- TerrainMapOctree: (vehN, vehE ) = (4060408.02, 593386.40). 2021-02-13T00:55:56Z- TerrainMapOctree: Min Distance = 1206.13. 2021-02-13T00:55:56Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:55:56Z- There is no specific good beam that all particles have in common. 2021-02-13T00:55:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:55:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-13T00:55:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:55:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 13.53 2021-02-13T00:55:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:55:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.110 ExpectedMeasDiff: nan currMeas.covariance: 6.76 2021-02-13T00:55:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:55:56Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 16.95 2021-02-13T00:55:56Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:55:56Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:55:56Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:55:56Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:55:56Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:55:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:55:56Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:55:56Z- Filter 0 No beams used in subcloud update 2021-02-13T00:55:56Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:55:56Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:55:56Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 682.2 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:55:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177756.07 sec. 2021-02-13T00:55:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:55:56Z- TNavBF::mmse selected from filter -1 2021-02-13T00:55:56Z- variances([0], [2], and psi_berg) = 682.191895 832.989420 0.000000 2021-02-13T00:55:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:55:56Z- LcmTrn::publishEstimates() - MLE : 4060407.52 593404.90 23.03 2021-02-13T00:55:56Z- LcmTrn::publishEstimates() - MMSE : 4060408.02 593386.40 23.03 2021-02-13T00:55:56Z- LcmTrn::publishEstimates() - COVAR: 682.19 832.99 0.89 0.00 2021-02-13T00:55:56Z- AHRS_M2 msg: 1613177756.39 epoch sec; seqNo:16893 2021-02-13T00:55:56Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:55:56Z- handleDepth()-Depth_Keller msg: 1613177756425 epoch sec; seqNo:17028; depth 23.15 2021-02-13T00:55:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177756.45 epoch sec; seqNo:17028; 4060408.02 north; 593386.40 east 2021-02-13T00:55:56Z- AHRS_M2 msg: 1613177756.79 epoch sec; seqNo:16894 2021-02-13T00:55:56Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:55:56Z- handleDepth()-Depth_Keller msg: 1613177756817 epoch sec; seqNo:17029; depth 23.29 2021-02-13T00:55:56Z- handleDvl() - RDI_Pathfinder msg: 1613177756.83 epoch sec; seqNo:8232 2021-02-13T00:55:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 98.25 , 65.80 , 101.90 2021-02-13T00:55:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.01 , -0.08 2021-02-13T00:55:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177756.84 epoch sec; seqNo:17029; 4060408.40 north; 593386.40 east 2021-02-13T00:55:57Z- AHRS_M2 msg: 1613177757.19 epoch sec; seqNo:16895 2021-02-13T00:55:57Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T00:55:57Z- handleDepth()-Depth_Keller msg: 1613177757208 epoch sec; seqNo:17030; depth 23.43 2021-02-13T00:55:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:57Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:57Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:57Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:57Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:57Z- handleDvl() - RDI_Pathfinder msg: 1613177757.21 epoch sec; seqNo:8233 2021-02-13T00:55:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.01 , -0.08 2021-02-13T00:55:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177757.22 epoch sec; seqNo:17030; 4060408.78 north; 593386.39 east 2021-02-13T00:55:57Z- AHRS_M2 msg: 1613177757.60 epoch sec; seqNo:16896 2021-02-13T00:55:57Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.20 psi 2021-02-13T00:55:57Z- handleDepth()-Depth_Keller msg: 1613177757609 epoch sec; seqNo:17031; depth 23.55 2021-02-13T00:55:57Z- handleDvl() - RDI_Pathfinder msg: 1613177757.62 epoch sec; seqNo:8234 2021-02-13T00:55:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.52 , 0.00 , 64.98 , 101.90 2021-02-13T00:55:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.04 , -0.05 2021-02-13T00:55:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177757.63 epoch sec; seqNo:17031; 4060409.17 north; 593387.61 east 2021-02-13T00:55:58Z- AHRS_M2 msg: 1613177758.00 epoch sec; seqNo:16897 2021-02-13T00:55:58Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.20 psi 2021-02-13T00:55:58Z- handleDepth()-Depth_Keller msg: 1613177758011 epoch sec; seqNo:17032; depth 23.70 2021-02-13T00:55:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177758.02 epoch sec; seqNo:17032; 4060409.55 north; 593387.60 east 2021-02-13T00:55:58Z- AHRS_M2 msg: 1613177758.41 epoch sec; seqNo:16898 2021-02-13T00:55:58Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.20 psi 2021-02-13T00:55:58Z- handleDepth()-Depth_Keller msg: 1613177758413 epoch sec; seqNo:17033; depth 23.83 2021-02-13T00:55:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:55:58Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:55:58Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:55:58Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:55:58Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:55:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:55:58Z- handleDvl() - RDI_Pathfinder msg: 1613177758.41 epoch sec; seqNo:8235 2021-02-13T00:55:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:55:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.04 , -0.05 2021-02-13T00:55:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177758.42 epoch sec; seqNo:17033; 4060409.92 north; 593387.60 east 2021-02-13T00:55:58Z- AHRS_M2 msg: 1613177758.81 epoch sec; seqNo:16899 2021-02-13T00:55:58Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.20 psi 2021-02-13T00:55:58Z- handleDepth()-Depth_Keller msg: 1613177758817 epoch sec; seqNo:17034; depth 23.96 2021-02-13T00:55:58Z- handleDvl() - RDI_Pathfinder msg: 1613177758.83 epoch sec; seqNo:8236 2021-02-13T00:55:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 90.41 , 65.39 , 102.31 2021-02-13T00:55:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.07 , -0.01 2021-02-13T00:55:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177758.84 epoch sec; seqNo:17034; 4060409.92 north; 593387.60 east 2021-02-13T00:55:59Z- AHRS_M2 msg: 1613177759.22 epoch sec; seqNo:16900 2021-02-13T00:55:59Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.20 psi 2021-02-13T00:55:59Z- handleDepth()-Depth_Keller msg: 1613177759226 epoch sec; seqNo:17035; depth 24.11 2021-02-13T00:55:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177759.24 epoch sec; seqNo:17035; 4060410.30 north; 593387.60 east 2021-02-13T00:55:59Z- AHRS_M2 msg: 1613177759.64 epoch sec; seqNo:16901 2021-02-13T00:55:59Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.20 psi 2021-02-13T00:55:59Z- handleDepth()-Depth_Keller msg: 1613177759647 epoch sec; seqNo:17036; depth 24.26 2021-02-13T00:55:59Z- handleDvl() - RDI_Pathfinder msg: 1613177759.66 epoch sec; seqNo:8237 2021-02-13T00:55:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 89.73 , 62.94 , 101.90 2021-02-13T00:55:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , 0.02 2021-02-13T00:55:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177759.67 epoch sec; seqNo:17036; 4060410.68 north; 593387.59 east 2021-02-13T00:56:00Z- AHRS_M2 msg: 1613177760.02 epoch sec; seqNo:16902 2021-02-13T00:56:00Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-13T00:56:00Z- handleDepth()-Depth_Keller msg: 1613177760029 epoch sec; seqNo:17037; depth 24.39 2021-02-13T00:56:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177760.04 epoch sec; seqNo:17037; 4060411.07 north; 593388.81 east 2021-02-13T00:56:00Z- AHRS_M2 msg: 1613177760.43 epoch sec; seqNo:16903 2021-02-13T00:56:00Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-13T00:56:00Z- handleDepth()-Depth_Keller msg: 1613177760434 epoch sec; seqNo:17038; depth 24.53 2021-02-13T00:56:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177760.44 epoch sec; seqNo:17038; 4060411.07 north; 593388.81 east 2021-02-13T00:56:00Z- AHRS_M2 msg: 1613177760.83 epoch sec; seqNo:16904 2021-02-13T00:56:00Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.20 psi 2021-02-13T00:56:00Z- handleDepth()-Depth_Keller msg: 1613177760841 epoch sec; seqNo:17039; depth 24.66 2021-02-13T00:56:00Z- handleDvl() - RDI_Pathfinder msg: 1613177760.86 epoch sec; seqNo:8238 2021-02-13T00:56:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 89.19 , 63.35 , 102.17 2021-02-13T00:56:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.06 , 0.05 2021-02-13T00:56:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177760.87 epoch sec; seqNo:17039; 4060411.45 north; 593388.80 east 2021-02-13T00:56:01Z- AHRS_M2 msg: 1613177761.24 epoch sec; seqNo:16905 2021-02-13T00:56:01Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.20 psi 2021-02-13T00:56:01Z- handleDepth()-Depth_Keller msg: 1613177761242 epoch sec; seqNo:17040; depth 24.82 2021-02-13T00:56:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177761.25 epoch sec; seqNo:17040; 4060411.83 north; 593388.80 east 2021-02-13T00:56:01Z- LcmTrn::handleMessages() - handling 46 messages took 0.74 ms and 131 calls... 2021-02-13T00:56:01Z- waiting for 1613177759.07, time is 1613177761.25 2021-02-13T00:56:01Z- LcmTrn::updateTrn() >>>> pose time 1613177761.25, meas time 1613177760.86 <<<< 2021-02-13T00:56:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177760.86 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:56:01Z- TerrainNav::motionUpdate() - currEstimate = (4060412, 593388.80, 24.82) 2021-02-13T00:56:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:56:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:56:01Z- TNavBF::Projecting Measurements 2021-02-13T00:56:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:56:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:56:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:56:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:56:01Z- TNavFilter::4 good beams. 2021-02-13T00:56:01Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:56:01Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:56:01Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:56:01Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:56:01Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:56:01Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:56:01Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:56:01Z- TNavBF::mmse selected from filter -1 2021-02-13T00:56:01Z- TerrainMapOctree: (vehN, vehE ) = (4060411.54, 593388.62). 2021-02-13T00:56:01Z- TerrainMapOctree: Min Distance = 1202.08. 2021-02-13T00:56:01Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:56:01Z- There is no specific good beam that all particles have in common. 2021-02-13T00:56:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:56:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-13T00:56:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:56:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 12.73 2021-02-13T00:56:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:56:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 6.42 2021-02-13T00:56:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:56:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 16.70 2021-02-13T00:56:01Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:56:01Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:56:01Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:56:01Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:56:01Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:56:01Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:56:01Z- Filter 0 No beams used in subcloud update 2021-02-13T00:56:01Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:56:01Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:56:01Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 682.1 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:56:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177760.86 sec. 2021-02-13T00:56:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:56:01Z- TNavBF::mmse selected from filter -1 2021-02-13T00:56:01Z- variances([0], [2], and psi_berg) = 682.132953 832.989394 0.000000 2021-02-13T00:56:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:56:01Z- LcmTrn::publishEstimates() - MLE : 4060411.33 593407.30 24.82 2021-02-13T00:56:01Z- LcmTrn::publishEstimates() - MMSE : 4060411.83 593388.80 24.82 2021-02-13T00:56:01Z- LcmTrn::publishEstimates() - COVAR: 682.13 832.99 0.81 0.00 2021-02-13T00:56:01Z- AHRS_M2 msg: 1613177761.66 epoch sec; seqNo:16906 2021-02-13T00:56:01Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.20 psi 2021-02-13T00:56:01Z- handleDepth()-Depth_Keller msg: 1613177761668 epoch sec; seqNo:17041; depth 24.94 2021-02-13T00:56:01Z- handleDvl() - RDI_Pathfinder msg: 1613177761.68 epoch sec; seqNo:8239 2021-02-13T00:56:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 88.11 , 62.94 , 101.09 2021-02-13T00:56:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , 0.04 2021-02-13T00:56:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177761.70 epoch sec; seqNo:17041; 4060412.21 north; 593388.80 east 2021-02-13T00:56:02Z- AHRS_M2 msg: 1613177762.04 epoch sec; seqNo:16907 2021-02-13T00:56:02Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.20 psi 2021-02-13T00:56:02Z- handleDepth()-Depth_Keller msg: 1613177762049 epoch sec; seqNo:17042; depth 25.10 2021-02-13T00:56:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:56:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:56:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:56:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:56:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:56:02Z- handleDvl() - RDI_Pathfinder msg: 1613177762.05 epoch sec; seqNo:8240 2021-02-13T00:56:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:56:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , 0.04 2021-02-13T00:56:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177762.07 epoch sec; seqNo:17042; 4060412.59 north; 593388.79 east 2021-02-13T00:56:02Z- AHRS_M2 msg: 1613177762.45 epoch sec; seqNo:16908 2021-02-13T00:56:02Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.20 psi 2021-02-13T00:56:02Z- handleDepth()-Depth_Keller msg: 1613177762453 epoch sec; seqNo:17043; depth 25.24 2021-02-13T00:56:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177762.46 epoch sec; seqNo:17043; 4060412.59 north; 593388.79 east 2021-02-13T00:56:02Z- AHRS_M2 msg: 1613177762.85 epoch sec; seqNo:16909 2021-02-13T00:56:02Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.21 psi 2021-02-13T00:56:02Z- handleDepth()-Depth_Keller msg: 1613177762861 epoch sec; seqNo:17044; depth 25.38 2021-02-13T00:56:02Z- handleDvl() - RDI_Pathfinder msg: 1613177762.87 epoch sec; seqNo:8241 2021-02-13T00:56:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 0.00 , 64.57 , 100.08 2021-02-13T00:56:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , 0.00 2021-02-13T00:56:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177762.88 epoch sec; seqNo:17044; 4060412.98 north; 593390.01 east 2021-02-13T00:56:03Z- AHRS_M2 msg: 1613177763.26 epoch sec; seqNo:16910 2021-02-13T00:56:03Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.21 psi 2021-02-13T00:56:03Z- handleDepth()-Depth_Keller msg: 1613177763264 epoch sec; seqNo:17045; depth 25.51 2021-02-13T00:56:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:03Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:56:03Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:56:03Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:56:03Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:56:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:56:03Z- handleDvl() - RDI_Pathfinder msg: 1613177763.27 epoch sec; seqNo:8242 2021-02-13T00:56:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:56:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , 0.00 2021-02-13T00:56:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177763.28 epoch sec; seqNo:17045; 4060413.36 north; 593390.00 east 2021-02-13T00:56:03Z- AHRS_M2 msg: 1613177763.67 epoch sec; seqNo:16911 2021-02-13T00:56:03Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.21 psi 2021-02-13T00:56:03Z- handleDepth()-Depth_Keller msg: 1613177763679 epoch sec; seqNo:17046; depth 25.66 2021-02-13T00:56:03Z- handleDvl() - RDI_Pathfinder msg: 1613177763.69 epoch sec; seqNo:8243 2021-02-13T00:56:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 89.33 , 62.94 , 99.06 2021-02-13T00:56:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.03 , -0.03 2021-02-13T00:56:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177763.71 epoch sec; seqNo:17046; 4060413.74 north; 593390.00 east 2021-02-13T00:56:04Z- AHRS_M2 msg: 1613177764.06 epoch sec; seqNo:16912 2021-02-13T00:56:04Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.21 psi 2021-02-13T00:56:04Z- handleDepth()-Depth_Keller msg: 1613177764069 epoch sec; seqNo:17047; depth 25.81 2021-02-13T00:56:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177764.08 epoch sec; seqNo:17047; 4060413.74 north; 593390.00 east 2021-02-13T00:56:04Z- AHRS_M2 msg: 1613177764.47 epoch sec; seqNo:16913 2021-02-13T00:56:04Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.21 psi 2021-02-13T00:56:04Z- handleDepth()-Depth_Keller msg: 1613177764473 epoch sec; seqNo:17048; depth 25.93 2021-02-13T00:56:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:56:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:56:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:56:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:56:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:56:04Z- handleDvl() - RDI_Pathfinder msg: 1613177764.47 epoch sec; seqNo:8244 2021-02-13T00:56:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:56:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.03 , -0.03 2021-02-13T00:56:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177764.48 epoch sec; seqNo:17048; 4060414.12 north; 593390.00 east 2021-02-13T00:56:04Z- AHRS_M2 msg: 1613177764.87 epoch sec; seqNo:16914 2021-02-13T00:56:04Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.21 psi 2021-02-13T00:56:04Z- handleDepth()-Depth_Keller msg: 1613177764878 epoch sec; seqNo:17049; depth 26.06 2021-02-13T00:56:04Z- handleDvl() - RDI_Pathfinder msg: 1613177764.89 epoch sec; seqNo:8245 2021-02-13T00:56:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 89.26 , 63.76 , 96.83 2021-02-13T00:56:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.05 , -0.04 2021-02-13T00:56:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177764.90 epoch sec; seqNo:17049; 4060414.50 north; 593389.99 east 2021-02-13T00:56:05Z- AHRS_M2 msg: 1613177765.28 epoch sec; seqNo:16915 2021-02-13T00:56:05Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.21 psi 2021-02-13T00:56:05Z- handleDepth()-Depth_Keller msg: 1613177765283 epoch sec; seqNo:17050; depth 26.21 2021-02-13T00:56:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177765.31 epoch sec; seqNo:17050; 4060414.89 north; 593391.21 east 2021-02-13T00:56:05Z- AHRS_M2 msg: 1613177765.68 epoch sec; seqNo:16916 2021-02-13T00:56:05Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.21 psi 2021-02-13T00:56:05Z- handleDepth()-Depth_Keller msg: 1613177765685 epoch sec; seqNo:17051; depth 26.34 2021-02-13T00:56:05Z- handleDvl() - RDI_Pathfinder msg: 1613177765.70 epoch sec; seqNo:8246 2021-02-13T00:56:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.43 , 87.16 , 63.35 , 97.98 2021-02-13T00:56:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.02 , -0.01 2021-02-13T00:56:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177765.72 epoch sec; seqNo:17051; 4060415.27 north; 593391.20 east 2021-02-13T00:56:06Z- AHRS_M2 msg: 1613177766.08 epoch sec; seqNo:16917 2021-02-13T00:56:06Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.20 psi 2021-02-13T00:56:06Z- handleDepth()-Depth_Keller msg: 1613177766089 epoch sec; seqNo:17052; depth 26.49 2021-02-13T00:56:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177766.11 epoch sec; seqNo:17052; 4060415.65 north; 593391.20 east 2021-02-13T00:56:06Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-13T00:56:06Z- waiting for 1613177764.25, time is 1613177766.11 2021-02-13T00:56:06Z- LcmTrn::updateTrn() >>>> pose time 1613177766.11, meas time 1613177765.70 <<<< 2021-02-13T00:56:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177765.70 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:56:06Z- TerrainNav::motionUpdate() - currEstimate = (4060416, 593391.20, 26.49) 2021-02-13T00:56:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:56:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:56:06Z- TNavBF::Projecting Measurements 2021-02-13T00:56:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:56:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:56:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:56:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:56:06Z- TNavFilter::4 good beams. 2021-02-13T00:56:06Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:56:06Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:56:06Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:56:06Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:56:06Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:56:06Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:56:06Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:56:06Z- TNavBF::mmse selected from filter -1 2021-02-13T00:56:06Z- TerrainMapOctree: (vehN, vehE ) = (4060415.33, 593391.00). 2021-02-13T00:56:06Z- TerrainMapOctree: Min Distance = 1197.73. 2021-02-13T00:56:06Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:56:06Z- There is no specific good beam that all particles have in common. 2021-02-13T00:56:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:56:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 7.28 2021-02-13T00:56:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:56:06Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 12.16 2021-02-13T00:56:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:56:06Z- Old Depth Bias: 0.00 DepthBiasCov: 0.86 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 6.42 2021-02-13T00:56:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:56:06Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:56:06Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:56:06Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:56:06Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:56:06Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:56:06Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:56:06Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:56:06Z- Filter 0 No beams used in subcloud update 2021-02-13T00:56:06Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:56:06Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:56:06Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 682.1 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:56:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177765.70 sec. 2021-02-13T00:56:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:56:06Z- TNavBF::mmse selected from filter -1 2021-02-13T00:56:06Z- variances([0], [2], and psi_berg) = 682.073545 832.989368 0.000000 2021-02-13T00:56:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:56:06Z- LcmTrn::publishEstimates() - MLE : 4060415.15 593409.70 26.49 2021-02-13T00:56:06Z- LcmTrn::publishEstimates() - MMSE : 4060415.65 593391.20 26.49 2021-02-13T00:56:06Z- LcmTrn::publishEstimates() - COVAR: 682.07 832.99 0.77 0.00 2021-02-13T00:56:06Z- AHRS_M2 msg: 1613177766.49 epoch sec; seqNo:16918 2021-02-13T00:56:06Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.19 psi 2021-02-13T00:56:06Z- handleDepth()-Depth_Keller msg: 1613177766500 epoch sec; seqNo:17053; depth 26.61 2021-02-13T00:56:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177766.51 epoch sec; seqNo:17053; 4060415.65 north; 593391.20 east 2021-02-13T00:56:06Z- AHRS_M2 msg: 1613177766.90 epoch sec; seqNo:16919 2021-02-13T00:56:06Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.18 psi 2021-02-13T00:56:06Z- handleDepth()-Depth_Keller msg: 1613177766901 epoch sec; seqNo:17054; depth 26.76 2021-02-13T00:56:06Z- handleDvl() - RDI_Pathfinder msg: 1613177766.92 epoch sec; seqNo:8247 2021-02-13T00:56:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 0.00 , 62.53 , 97.98 2021-02-13T00:56:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , -0.00 2021-02-13T00:56:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177766.93 epoch sec; seqNo:17054; 4060416.03 north; 593391.20 east 2021-02-13T00:56:07Z- AHRS_M2 msg: 1613177767.30 epoch sec; seqNo:16920 2021-02-13T00:56:07Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.18 psi 2021-02-13T00:56:07Z- handleDepth()-Depth_Keller msg: 1613177767305 epoch sec; seqNo:17055; depth 26.89 2021-02-13T00:56:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177767.32 epoch sec; seqNo:17055; 4060416.41 north; 593391.19 east 2021-02-13T00:56:07Z- AHRS_M2 msg: 1613177767.70 epoch sec; seqNo:16921 2021-02-13T00:56:07Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.18 psi 2021-02-13T00:56:07Z- handleDepth()-Depth_Keller msg: 1613177767706 epoch sec; seqNo:17056; depth 27.03 2021-02-13T00:56:07Z- handleDvl() - RDI_Pathfinder msg: 1613177767.72 epoch sec; seqNo:8248 2021-02-13T00:56:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.25 , 0.00 , 61.30 , 97.98 2021-02-13T00:56:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.01 2021-02-13T00:56:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177767.73 epoch sec; seqNo:17056; 4060416.78 north; 593391.19 east 2021-02-13T00:56:08Z- AHRS_M2 msg: 1613177768.10 epoch sec; seqNo:16922 2021-02-13T00:56:08Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.17 psi 2021-02-13T00:56:08Z- handleDepth()-Depth_Keller msg: 1613177768118 epoch sec; seqNo:17057; depth 27.17 2021-02-13T00:56:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:56:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:56:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:56:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:56:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:56:08Z- handleDvl() - RDI_Pathfinder msg: 1613177768.12 epoch sec; seqNo:8249 2021-02-13T00:56:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:56:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.01 2021-02-13T00:56:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177768.13 epoch sec; seqNo:17057; 4060417.18 north; 593392.40 east 2021-02-13T00:56:08Z- AHRS_M2 msg: 1613177768.51 epoch sec; seqNo:16923 2021-02-13T00:56:08Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.17 psi 2021-02-13T00:56:08Z- handleDepth()-Depth_Keller msg: 1613177768580 epoch sec; seqNo:17058; depth 27.31 2021-02-13T00:56:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177768.59 epoch sec; seqNo:17058; 4060417.18 north; 593392.40 east 2021-02-13T00:56:08Z- AHRS_M2 msg: 1613177768.91 epoch sec; seqNo:16924 2021-02-13T00:56:08Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.17 psi 2021-02-13T00:56:08Z- handleDepth()-Depth_Keller msg: 1613177768925 epoch sec; seqNo:17059; depth 27.45 2021-02-13T00:56:08Z- handleDvl() - RDI_Pathfinder msg: 1613177768.94 epoch sec; seqNo:8250 2021-02-13T00:56:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 85.00 , 62.12 , 97.98 2021-02-13T00:56:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.02 2021-02-13T00:56:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177768.96 epoch sec; seqNo:17059; 4060417.56 north; 593392.40 east 2021-02-13T00:56:09Z- AHRS_M2 msg: 1613177769.32 epoch sec; seqNo:16925 2021-02-13T00:56:09Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.17 psi 2021-02-13T00:56:09Z- handleDepth()-Depth_Keller msg: 1613177769410 epoch sec; seqNo:17060; depth 27.58 2021-02-13T00:56:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:09Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:56:09Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:56:09Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:56:09Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:56:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:56:09Z- handleDvl() - RDI_Pathfinder msg: 1613177769.41 epoch sec; seqNo:8251 2021-02-13T00:56:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:56:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.02 2021-02-13T00:56:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177769.43 epoch sec; seqNo:17060; 4060417.93 north; 593392.40 east 2021-02-13T00:56:09Z- AHRS_M2 msg: 1613177769.72 epoch sec; seqNo:16926 2021-02-13T00:56:09Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.18 psi 2021-02-13T00:56:09Z- handleDepth()-Depth_Keller msg: 1613177769759 epoch sec; seqNo:17061; depth 27.74 2021-02-13T00:56:09Z- handleDvl() - RDI_Pathfinder msg: 1613177769.77 epoch sec; seqNo:8252 2021-02-13T00:56:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.03 , 0.00 , 60.90 , 97.78 2021-02-13T00:56:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , -0.03 2021-02-13T00:56:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177769.78 epoch sec; seqNo:17061; 4060418.31 north; 593392.39 east 2021-02-13T00:56:10Z- AHRS_M2 msg: 1613177770.12 epoch sec; seqNo:16927 2021-02-13T00:56:10Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.18 psi 2021-02-13T00:56:10Z- handleDepth()-Depth_Keller msg: 1613177770156 epoch sec; seqNo:17062; depth 27.85 2021-02-13T00:56:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177770.17 epoch sec; seqNo:17062; 4060418.31 north; 593392.39 east 2021-02-13T00:56:10Z- AHRS_M2 msg: 1613177770.53 epoch sec; seqNo:16928 2021-02-13T00:56:10Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.18 psi 2021-02-13T00:56:10Z- handleDepth()-Depth_Keller msg: 1613177770553 epoch sec; seqNo:17063; depth 27.99 2021-02-13T00:56:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:56:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:56:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:56:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:56:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:56:10Z- handleDvl() - RDI_Pathfinder msg: 1613177770.56 epoch sec; seqNo:8253 2021-02-13T00:56:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:56:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , -0.03 2021-02-13T00:56:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177770.56 epoch sec; seqNo:17063; 4060418.69 north; 593392.39 east 2021-02-13T00:56:10Z- AHRS_M2 msg: 1613177770.93 epoch sec; seqNo:16929 2021-02-13T00:56:10Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.18 psi 2021-02-13T00:56:10Z- handleDepth()-Depth_Keller msg: 1613177770939 epoch sec; seqNo:17064; depth 28.13 2021-02-13T00:56:10Z- handleDvl() - RDI_Pathfinder msg: 1613177770.95 epoch sec; seqNo:8254 2021-02-13T00:56:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 82.84 , 60.08 , 97.98 2021-02-13T00:56:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.19 , -0.06 2021-02-13T00:56:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177770.96 epoch sec; seqNo:17064; 4060419.08 north; 593393.60 east 2021-02-13T00:56:11Z- AHRS_M2 msg: 1613177771.34 epoch sec; seqNo:16930 2021-02-13T00:56:11Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.18 psi 2021-02-13T00:56:11Z- handleDepth()-Depth_Keller msg: 1613177771341 epoch sec; seqNo:17065; depth 28.25 2021-02-13T00:56:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177771.36 epoch sec; seqNo:17065; 4060419.46 north; 593393.60 east 2021-02-13T00:56:11Z- LcmTrn::handleMessages() - handling 47 messages took 0.82 ms and 131 calls... 2021-02-13T00:56:11Z- waiting for 1613177769.11, time is 1613177771.36 2021-02-13T00:56:11Z- LcmTrn::updateTrn() >>>> pose time 1613177771.36, meas time 1613177770.95 <<<< 2021-02-13T00:56:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177770.95 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:56:11Z- TerrainNav::motionUpdate() - currEstimate = (4060419, 593393.60, 28.25) 2021-02-13T00:56:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:56:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:56:11Z- TNavBF::Projecting Measurements 2021-02-13T00:56:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:56:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:56:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:56:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:56:11Z- TNavFilter::4 good beams. 2021-02-13T00:56:11Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:56:11Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:56:11Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:56:11Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:56:11Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:56:11Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:56:11Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:56:11Z- TNavBF::mmse selected from filter -1 2021-02-13T00:56:11Z- TerrainMapOctree: (vehN, vehE ) = (4060419.16, 593393.41). 2021-02-13T00:56:11Z- TerrainMapOctree: Min Distance = 1193.33. 2021-02-13T00:56:11Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:56:11Z- There is no specific good beam that all particles have in common. 2021-02-13T00:56:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:56:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:56:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:56:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 10.98 2021-02-13T00:56:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:56:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 5.77 2021-02-13T00:56:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:56:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.76 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:56:11Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:56:11Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:56:11Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:56:11Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:56:11Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:56:11Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:56:11Z- Filter 0 No beams used in subcloud update 2021-02-13T00:56:11Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:56:11Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:56:11Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 682.0 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T00:56:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177770.95 sec. 2021-02-13T00:56:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:56:11Z- TNavBF::mmse selected from filter -1 2021-02-13T00:56:11Z- variances([0], [2], and psi_berg) = 682.014553 832.989343 0.000000 2021-02-13T00:56:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:56:11Z- LcmTrn::publishEstimates() - MLE : 4060418.96 593413.10 28.25 2021-02-13T00:56:11Z- LcmTrn::publishEstimates() - MMSE : 4060419.46 593393.60 28.25 2021-02-13T00:56:11Z- LcmTrn::publishEstimates() - COVAR: 682.01 832.99 0.74 0.00 2021-02-13T00:56:11Z- AHRS_M2 msg: 1613177771.74 epoch sec; seqNo:16931 2021-02-13T00:56:11Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.18 psi 2021-02-13T00:56:11Z- handleDepth()-Depth_Keller msg: 1613177771749 epoch sec; seqNo:17066; depth 28.39 2021-02-13T00:56:11Z- handleDvl() - RDI_Pathfinder msg: 1613177771.76 epoch sec; seqNo:8255 2021-02-13T00:56:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.21 , 83.92 , 59.26 , 97.98 2021-02-13T00:56:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.08 , 0.00 2021-02-13T00:56:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177771.79 epoch sec; seqNo:17066; 4060419.84 north; 593393.60 east 2021-02-13T00:56:12Z- AHRS_M2 msg: 1613177772.15 epoch sec; seqNo:16932 2021-02-13T00:56:12Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.18 psi 2021-02-13T00:56:12Z- handleDepth()-Depth_Keller msg: 1613177772154 epoch sec; seqNo:17067; depth 28.52 2021-02-13T00:56:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177772.17 epoch sec; seqNo:17067; 4060420.22 north; 593393.59 east 2021-02-13T00:56:12Z- AHRS_M2 msg: 1613177772.55 epoch sec; seqNo:16933 2021-02-13T00:56:12Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.18 psi 2021-02-13T00:56:12Z- handleDepth()-Depth_Keller msg: 1613177772553 epoch sec; seqNo:17068; depth 28.68 2021-02-13T00:56:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177772.56 epoch sec; seqNo:17068; 4060420.22 north; 593393.59 east 2021-02-13T00:56:12Z- AHRS_M2 msg: 1613177772.95 epoch sec; seqNo:16934 2021-02-13T00:56:12Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.18 psi 2021-02-13T00:56:12Z- handleDepth()-Depth_Keller msg: 1613177772959 epoch sec; seqNo:17069; depth 28.81 2021-02-13T00:56:12Z- handleDvl() - RDI_Pathfinder msg: 1613177772.97 epoch sec; seqNo:8256 2021-02-13T00:56:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.62 , 83.92 , 59.67 , 97.98 2021-02-13T00:56:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.09 , 0.02 2021-02-13T00:56:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177772.98 epoch sec; seqNo:17069; 4060420.60 north; 593393.59 east 2021-02-13T00:56:13Z- AHRS_M2 msg: 1613177773.36 epoch sec; seqNo:16935 2021-02-13T00:56:13Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.18 psi 2021-02-13T00:56:13Z- handleDepth()-Depth_Keller msg: 1613177773365 epoch sec; seqNo:17070; depth 28.94 2021-02-13T00:56:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177773.38 epoch sec; seqNo:17070; 4060420.98 north; 593393.58 east 2021-02-13T00:56:13Z- AHRS_M2 msg: 1613177773.76 epoch sec; seqNo:16936 2021-02-13T00:56:13Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.18 psi 2021-02-13T00:56:13Z- handleDepth()-Depth_Keller msg: 1613177773765 epoch sec; seqNo:17071; depth 29.06 2021-02-13T00:56:13Z- handleDvl() - RDI_Pathfinder msg: 1613177773.78 epoch sec; seqNo:8257 2021-02-13T00:56:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 82.84 , 59.67 , 97.98 2021-02-13T00:56:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.20 , 0.11 2021-02-13T00:56:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177773.79 epoch sec; seqNo:17071; 4060421.37 north; 593394.80 east 2021-02-13T00:56:14Z- AHRS_M2 msg: 1613177774.16 epoch sec; seqNo:16937 2021-02-13T00:56:14Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.18 psi 2021-02-13T00:56:14Z- handleDepth()-Depth_Keller msg: 1613177774169 epoch sec; seqNo:17072; depth 29.20 2021-02-13T00:56:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:14Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:56:14Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:56:14Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:56:14Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:56:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:56:14Z- handleDvl() - RDI_Pathfinder msg: 1613177774.17 epoch sec; seqNo:8258 2021-02-13T00:56:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:56:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.20 , 0.11 2021-02-13T00:56:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177774.18 epoch sec; seqNo:17072; 4060421.37 north; 593394.80 east 2021-02-13T00:56:14Z- AHRS_M2 msg: 1613177774.57 epoch sec; seqNo:16938 2021-02-13T00:56:14Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-13T00:56:14Z- handleDepth()-Depth_Keller msg: 1613177774577 epoch sec; seqNo:17073; depth 29.32 2021-02-13T00:56:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177774.59 epoch sec; seqNo:17073; 4060421.75 north; 593394.80 east 2021-02-13T00:56:14Z- AHRS_M2 msg: 1613177774.98 epoch sec; seqNo:16939 2021-02-13T00:56:14Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.18 psi 2021-02-13T00:56:14Z- handleDepth()-Depth_Keller msg: 1613177774986 epoch sec; seqNo:17074; depth 29.47 2021-02-13T00:56:15Z- handleDvl() - RDI_Pathfinder msg: 1613177775.00 epoch sec; seqNo:8259 2021-02-13T00:56:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 81.75 , 59.26 , 97.98 2021-02-13T00:56:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.08 , 0.08 2021-02-13T00:56:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177775.01 epoch sec; seqNo:17074; 4060422.13 north; 593394.79 east 2021-02-13T00:56:15Z- AHRS_M2 msg: 1613177775.38 epoch sec; seqNo:16940 2021-02-13T00:56:15Z- AHRS_M2 msg: 0.57 phi; -0.29 theta; -0.18 psi 2021-02-13T00:56:15Z- handleDepth()-Depth_Keller msg: 1613177775383 epoch sec; seqNo:17075; depth 29.63 2021-02-13T00:56:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:15Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:56:15Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:56:15Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:56:15Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:56:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:56:15Z- handleDvl() - RDI_Pathfinder msg: 1613177775.38 epoch sec; seqNo:8260 2021-02-13T00:56:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:56:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.08 , 0.08 2021-02-13T00:56:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177775.41 epoch sec; seqNo:17075; 4060422.13 north; 593394.79 east 2021-02-13T00:56:15Z- AHRS_M2 msg: 1613177775.78 epoch sec; seqNo:16941 2021-02-13T00:56:15Z- AHRS_M2 msg: 0.57 phi; -0.25 theta; -0.17 psi 2021-02-13T00:56:15Z- handleDepth()-Depth_Keller msg: 1613177775789 epoch sec; seqNo:17076; depth 29.78 2021-02-13T00:56:15Z- handleDvl() - RDI_Pathfinder msg: 1613177775.80 epoch sec; seqNo:8261 2021-02-13T00:56:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:56:15Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:56:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177775.83 epoch sec; seqNo:17076; 4060422.51 north; 593394.79 east 2021-02-13T00:56:16Z- AHRS_M2 msg: 1613177776.18 epoch sec; seqNo:16942 2021-02-13T00:56:16Z- AHRS_M2 msg: 0.56 phi; -0.21 theta; -0.16 psi 2021-02-13T00:56:16Z- handleDepth()-Depth_Keller msg: 1613177776197 epoch sec; seqNo:17077; depth 29.92 2021-02-13T00:56:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177776.21 epoch sec; seqNo:17077; 4060422.90 north; 593396.00 east 2021-02-13T00:56:16Z- AHRS_M2 msg: 1613177776.59 epoch sec; seqNo:16943 2021-02-13T00:56:16Z- AHRS_M2 msg: 0.56 phi; -0.17 theta; -0.15 psi 2021-02-13T00:56:16Z- handleDepth()-Depth_Keller msg: 1613177776594 epoch sec; seqNo:17078; depth 30.05 2021-02-13T00:56:16Z- LcmTrn::handleMessages() - handling 45 messages took 0.74 ms and 130 calls... 2021-02-13T00:56:16Z- waiting for 1613177774.36, time is 1613177776.59 2021-02-13T00:56:16Z- LcmTrn::updateTrn() >>>> pose time 1613177776.59, meas time 1613177775.80 <<<< 2021-02-13T00:56:16Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177775.80, ping # 0. 2021-02-13T00:56:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177775.80 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:56:16Z- TerrainNav::motionUpdate() - currEstimate = (4060423, 593396.00, 30.05) 2021-02-13T00:56:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:56:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:56:16Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:56:16Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:56:16Z- TNavBF::Projecting Measurements 2021-02-13T00:56:16Z- TNavFilter::Measurement from time = 1613177775.80 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:56:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:56:16Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:56:16Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:56:16Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:56:16Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:56:16Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:56:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:56:16Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 682.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:56:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:56:16Z- TNavBF::mmse selected from filter -1 2021-02-13T00:56:16Z- variances([0], [2], and psi_berg) = 681.954461 832.989316 0.000000 2021-02-13T00:56:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:56:16Z- LcmTrn::publishEstimates() - MLE : 4060422.40 593415.50 30.05 2021-02-13T00:56:16Z- LcmTrn::publishEstimates() - MMSE : 4060422.90 593396.00 30.05 2021-02-13T00:56:16Z- LcmTrn::publishEstimates() - COVAR: 681.95 832.99 0.96 0.00 2021-02-13T00:56:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:56:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:56:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:56:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:56:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:56:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:56:16Z- handleDvl() - RDI_Pathfinder msg: 1613177776.60 epoch sec; seqNo:8262 2021-02-13T00:56:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:56:16Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:56:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177776.61 epoch sec; seqNo:17078; 4060422.90 north; 593396.00 east 2021-02-13T00:56:16Z- AHRS_M2 msg: 1613177776.99 epoch sec; seqNo:16944 2021-02-13T00:56:16Z- AHRS_M2 msg: 0.55 phi; -0.13 theta; -0.14 psi 2021-02-13T00:56:16Z- handleDepth()-Depth_Keller msg: 1613177776997 epoch sec; seqNo:17079; depth 30.18 2021-02-13T00:56:17Z- handleDvl() - RDI_Pathfinder msg: 1613177777.02 epoch sec; seqNo:8263 2021-02-13T00:56:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.35 , 78.37 , 60.90 , 81.62 2021-02-13T00:56:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.03 , -0.02 2021-02-13T00:56:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177777.03 epoch sec; seqNo:17079; 4060423.28 north; 593396.00 east 2021-02-13T00:56:17Z- AHRS_M2 msg: 1613177777.40 epoch sec; seqNo:16945 2021-02-13T00:56:17Z- AHRS_M2 msg: 0.55 phi; -0.08 theta; -0.14 psi 2021-02-13T00:56:17Z- handleDepth()-Depth_Keller msg: 1613177777401 epoch sec; seqNo:17080; depth 30.29 2021-02-13T00:56:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177777.41 epoch sec; seqNo:17080; 4060423.66 north; 593396.00 east 2021-02-13T00:56:17Z- AHRS_M2 msg: 1613177777.80 epoch sec; seqNo:16946 2021-02-13T00:56:17Z- AHRS_M2 msg: 0.54 phi; -0.04 theta; -0.14 psi 2021-02-13T00:56:17Z- handleDepth()-Depth_Keller msg: 1613177777805 epoch sec; seqNo:17081; depth 30.39 2021-02-13T00:56:17Z- handleDvl() - RDI_Pathfinder msg: 1613177777.82 epoch sec; seqNo:8264 2021-02-13T00:56:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.12 , 78.51 , 63.35 , 78.51 2021-02-13T00:56:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.06 2021-02-13T00:56:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177777.83 epoch sec; seqNo:17081; 4060424.04 north; 593395.99 east 2021-02-13T00:56:18Z- AHRS_M2 msg: 1613177778.21 epoch sec; seqNo:16947 2021-02-13T00:56:18Z- AHRS_M2 msg: 0.54 phi; -0.00 theta; -0.14 psi 2021-02-13T00:56:18Z- handleDepth()-Depth_Keller msg: 1613177778234 epoch sec; seqNo:17082; depth 30.46 2021-02-13T00:56:18Z- handleDvl() - RDI_Pathfinder msg: 1613177778.26 epoch sec; seqNo:8265 2021-02-13T00:56:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.15 , 0.00 , 0.00 , 66.36 2021-02-13T00:56:18Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:56:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177778.27 epoch sec; seqNo:17082; 4060424.41 north; 593395.99 east 2021-02-13T00:56:18Z- AHRS_M2 msg: 1613177778.61 epoch sec; seqNo:16948 2021-02-13T00:56:18Z- AHRS_M2 msg: 0.53 phi; 0.03 theta; -0.14 psi 2021-02-13T00:56:18Z- handleDepth()-Depth_Keller msg: 1613177778613 epoch sec; seqNo:17083; depth 30.53 2021-02-13T00:56:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177778.63 epoch sec; seqNo:17083; 4060424.81 north; 593397.20 east 2021-02-13T00:56:19Z- AHRS_M2 msg: 1613177779.01 epoch sec; seqNo:16949 2021-02-13T00:56:19Z- AHRS_M2 msg: 0.53 phi; 0.07 theta; -0.14 psi 2021-02-13T00:56:19Z- handleDepth()-Depth_Keller msg: 1613177779017 epoch sec; seqNo:17084; depth 30.57 2021-02-13T00:56:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177779.03 epoch sec; seqNo:17084; 4060425.19 north; 593397.20 east 2021-02-13T00:56:19Z- AHRS_M2 msg: 1613177779.42 epoch sec; seqNo:16950 2021-02-13T00:56:19Z- AHRS_M2 msg: 0.52 phi; 0.10 theta; -0.14 psi 2021-02-13T00:56:19Z- handleDepth()-Depth_Keller msg: 1613177779422 epoch sec; seqNo:17085; depth 30.62 2021-02-13T00:56:19Z- handleDvl() - RDI_Pathfinder msg: 1613177779.43 epoch sec; seqNo:8266 2021-02-13T00:56:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.45 , 0.00 , 0.00 , 63.78 2021-02-13T00:56:19Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:56:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177779.44 epoch sec; seqNo:17085; 4060425.56 north; 593397.20 east 2021-02-13T00:56:19Z- AHRS_M2 msg: 1613177779.82 epoch sec; seqNo:16951 2021-02-13T00:56:19Z- AHRS_M2 msg: 0.52 phi; 0.13 theta; -0.14 psi 2021-02-13T00:56:19Z- handleDepth()-Depth_Keller msg: 1613177779827 epoch sec; seqNo:17086; depth 30.65 2021-02-13T00:56:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177779.84 epoch sec; seqNo:17086; 4060425.56 north; 593397.20 east 2021-02-13T00:56:20Z- AHRS_M2 msg: 1613177780.22 epoch sec; seqNo:16952 2021-02-13T00:56:20Z- AHRS_M2 msg: 0.52 phi; 0.15 theta; -0.14 psi 2021-02-13T00:56:20Z- handleDepth()-Depth_Keller msg: 1613177780229 epoch sec; seqNo:17087; depth 30.65 2021-02-13T00:56:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177780.24 epoch sec; seqNo:17087; 4060425.94 north; 593397.19 east 2021-02-13T00:56:20Z- AHRS_M2 msg: 1613177780.63 epoch sec; seqNo:16953 2021-02-13T00:56:20Z- AHRS_M2 msg: 0.51 phi; 0.18 theta; -0.14 psi 2021-02-13T00:56:20Z- handleDepth()-Depth_Keller msg: 1613177780639 epoch sec; seqNo:17088; depth 30.64 2021-02-13T00:56:20Z- handleDvl() - RDI_Pathfinder msg: 1613177780.65 epoch sec; seqNo:8267 2021-02-13T00:56:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.90 , 0.00 , 0.00 , 61.12 2021-02-13T00:56:20Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:56:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177780.67 epoch sec; seqNo:17088; 4060426.32 north; 593397.19 east 2021-02-13T00:56:21Z- AHRS_M2 msg: 1613177781.04 epoch sec; seqNo:16954 2021-02-13T00:56:21Z- AHRS_M2 msg: 0.51 phi; 0.20 theta; -0.14 psi 2021-02-13T00:56:21Z- handleDepth()-Depth_Keller msg: 1613177781041 epoch sec; seqNo:17089; depth 30.62 2021-02-13T00:56:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177781.05 epoch sec; seqNo:17089; 4060426.70 north; 593397.18 east 2021-02-13T00:56:21Z- AHRS_M2 msg: 1613177781.44 epoch sec; seqNo:16955 2021-02-13T00:56:21Z- AHRS_M2 msg: 0.51 phi; 0.22 theta; -0.14 psi 2021-02-13T00:56:21Z- handleDepth()-Depth_Keller msg: 1613177781443 epoch sec; seqNo:17090; depth 30.61 2021-02-13T00:56:21Z- handleDvl() - RDI_Pathfinder msg: 1613177781.46 epoch sec; seqNo:8268 2021-02-13T00:56:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.78 , 0.00 , 0.00 , 60.68 2021-02-13T00:56:21Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:56:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177781.47 epoch sec; seqNo:17090; 4060427.08 north; 593397.18 east 2021-02-13T00:56:21Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-13T00:56:21Z- waiting for 1613177779.59, time is 1613177781.47 2021-02-13T00:56:21Z- LcmTrn::updateTrn() equating pose 1613177781.47 and meas times 1613177781.47 2021-02-13T00:56:21Z- LcmTrn::updateTrn() >>>> pose time 1613177781.47, meas time 1613177781.47 <<<< 2021-02-13T00:56:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177781.47 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:56:21Z- TerrainNav::motionUpdate() - currEstimate = (4060427, 593397.18, 30.61) 2021-02-13T00:56:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:56:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:56:21Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:56:21Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:56:21Z- TNavBF::Projecting Measurements 2021-02-13T00:56:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:56:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:56:21Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-13T00:56:21Z- TNavFilter::2 good beams. 2021-02-13T00:56:21Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:56:21Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:56:21Z- TNavBF:: beamIndex[1] = 3 2021-02-13T00:56:21Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:56:21Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:56:21Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-13T00:56:21Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:56:21Z- TNavBF::mmse selected from filter -1 2021-02-13T00:56:21Z- TerrainMapOctree: (vehN, vehE ) = (4060427.08, 593397.18). 2021-02-13T00:56:21Z- TerrainMapOctree: Min Distance = 1185.12. 2021-02-13T00:56:21Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:56:21Z- There is no specific good beam that all particles have in common. 2021-02-13T00:56:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:56:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.125 ExpectedMeasDiff: nan currMeas.covariance: 6.51 2021-02-13T00:56:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:56:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.119 ExpectedMeasDiff: nan currMeas.covariance: 5.89 2021-02-13T00:56:21Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:56:21Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:56:21Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:56:21Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:56:21Z- Filter 0 No beams used in subcloud update 2021-02-13T00:56:21Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:56:21Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:56:21Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 681.9 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:56:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177781.47 sec. 2021-02-13T00:56:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:56:21Z- TNavBF::mmse selected from filter -1 2021-02-13T00:56:21Z- variances([0], [2], and psi_berg) = 681.907796 832.989296 0.000000 2021-02-13T00:56:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:56:21Z- LcmTrn::publishEstimates() - MLE : 4060426.58 593416.68 30.61 2021-02-13T00:56:21Z- LcmTrn::publishEstimates() - MMSE : 4060427.08 593397.18 30.61 2021-02-13T00:56:21Z- LcmTrn::publishEstimates() - COVAR: 681.91 832.99 0.91 0.00 2021-02-13T00:56:21Z- AHRS_M2 msg: 1613177781.84 epoch sec; seqNo:16956 2021-02-13T00:56:21Z- AHRS_M2 msg: 0.51 phi; 0.24 theta; -0.14 psi 2021-02-13T00:56:21Z- handleDepth()-Depth_Keller msg: 1613177781924 epoch sec; seqNo:17091; depth 30.59 2021-02-13T00:56:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177781.93 epoch sec; seqNo:17091; 4060427.47 north; 593398.39 east 2021-02-13T00:56:22Z- AHRS_M2 msg: 1613177782.24 epoch sec; seqNo:16957 2021-02-13T00:56:22Z- AHRS_M2 msg: 0.51 phi; 0.25 theta; -0.13 psi 2021-02-13T00:56:22Z- handleDepth()-Depth_Keller msg: 1613177782257 epoch sec; seqNo:17092; depth 30.53 2021-02-13T00:56:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177782.27 epoch sec; seqNo:17092; 4060427.85 north; 593398.39 east 2021-02-13T00:56:22Z- AHRS_M2 msg: 1613177782.65 epoch sec; seqNo:16958 2021-02-13T00:56:22Z- AHRS_M2 msg: 0.51 phi; 0.27 theta; -0.13 psi 2021-02-13T00:56:22Z- handleDepth()-Depth_Keller msg: 1613177782741 epoch sec; seqNo:17093; depth 30.50 2021-02-13T00:56:22Z- handleDvl() - RDI_Pathfinder msg: 1613177782.76 epoch sec; seqNo:8269 2021-02-13T00:56:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:56:22Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:56:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177782.77 epoch sec; seqNo:17093; 4060428.23 north; 593398.39 east 2021-02-13T00:56:23Z- AHRS_M2 msg: 1613177783.05 epoch sec; seqNo:16959 2021-02-13T00:56:23Z- AHRS_M2 msg: 0.51 phi; 0.28 theta; -0.12 psi 2021-02-13T00:56:23Z- handleDepth()-Depth_Keller msg: 1613177783094 epoch sec; seqNo:17094; depth 30.42 2021-02-13T00:56:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177783.11 epoch sec; seqNo:17094; 4060428.23 north; 593398.39 east 2021-02-13T00:56:23Z- AHRS_M2 msg: 1613177783.46 epoch sec; seqNo:16960 2021-02-13T00:56:23Z- AHRS_M2 msg: 0.52 phi; 0.29 theta; -0.12 psi 2021-02-13T00:56:23Z- handleDepth()-Depth_Keller msg: 1613177783491 epoch sec; seqNo:17095; depth 30.38 2021-02-13T00:56:23Z- handleDvl() - RDI_Pathfinder msg: 1613177783.51 epoch sec; seqNo:8270 2021-02-13T00:56:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:56:23Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:56:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177783.52 epoch sec; seqNo:17095; 4060428.61 north; 593398.38 east 2021-02-13T00:56:23Z- AHRS_M2 msg: 1613177783.87 epoch sec; seqNo:16961 2021-02-13T00:56:23Z- AHRS_M2 msg: 0.52 phi; 0.31 theta; -0.11 psi 2021-02-13T00:56:23Z- handleDepth()-Depth_Keller msg: 1613177783889 epoch sec; seqNo:17096; depth 30.30 2021-02-13T00:56:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177783.91 epoch sec; seqNo:17096; 4060428.99 north; 593398.38 east 2021-02-13T00:56:24Z- AHRS_M2 msg: 1613177784.27 epoch sec; seqNo:16962 2021-02-13T00:56:24Z- AHRS_M2 msg: 0.53 phi; 0.32 theta; -0.10 psi 2021-02-13T00:56:24Z- handleDepth()-Depth_Keller msg: 1613177784275 epoch sec; seqNo:17097; depth 30.24 2021-02-13T00:56:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177784.28 epoch sec; seqNo:17097; 4060429.37 north; 593398.37 east 2021-02-13T00:56:24Z- AHRS_M2 msg: 1613177784.67 epoch sec; seqNo:16963 2021-02-13T00:56:24Z- AHRS_M2 msg: 0.53 phi; 0.32 theta; -0.10 psi 2021-02-13T00:56:24Z- handleDepth()-Depth_Keller msg: 1613177784677 epoch sec; seqNo:17098; depth 30.16 2021-02-13T00:56:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177784.69 epoch sec; seqNo:17098; 4060429.74 north; 593398.37 east 2021-02-13T00:56:25Z- AHRS_M2 msg: 1613177785.08 epoch sec; seqNo:16964 2021-02-13T00:56:25Z- AHRS_M2 msg: 0.54 phi; 0.33 theta; -0.09 psi 2021-02-13T00:56:25Z- handleDepth()-Depth_Keller msg: 1613177785081 epoch sec; seqNo:17099; depth 30.08 2021-02-13T00:56:25Z- handleDvl() - RDI_Pathfinder msg: 1613177785.09 epoch sec; seqNo:8271 2021-02-13T00:56:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.90 , 77.90 , 97.24 , 60.21 2021-02-13T00:56:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.07 , -0.04 2021-02-13T00:56:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177785.13 epoch sec; seqNo:17099; 4060429.76 north; 593399.59 east 2021-02-13T00:56:25Z- AHRS_M2 msg: 1613177785.48 epoch sec; seqNo:16965 2021-02-13T00:56:25Z- AHRS_M2 msg: 0.54 phi; 0.34 theta; -0.09 psi 2021-02-13T00:56:25Z- handleDepth()-Depth_Keller msg: 1613177785485 epoch sec; seqNo:17100; depth 29.99 2021-02-13T00:56:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177785.50 epoch sec; seqNo:17100; 4060430.14 north; 593399.59 east 2021-02-13T00:56:25Z- AHRS_M2 msg: 1613177785.89 epoch sec; seqNo:16966 2021-02-13T00:56:25Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.09 psi 2021-02-13T00:56:25Z- handleDepth()-Depth_Keller msg: 1613177785898 epoch sec; seqNo:17101; depth 29.91 2021-02-13T00:56:25Z- handleDvl() - RDI_Pathfinder msg: 1613177785.91 epoch sec; seqNo:8272 2021-02-13T00:56:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.84 , 78.51 , 0.00 , 58.03 2021-02-13T00:56:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , -0.02 2021-02-13T00:56:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177785.92 epoch sec; seqNo:17101; 4060430.52 north; 593399.58 east 2021-02-13T00:56:26Z- AHRS_M2 msg: 1613177786.29 epoch sec; seqNo:16967 2021-02-13T00:56:26Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.09 psi 2021-02-13T00:56:26Z- handleDepth()-Depth_Keller msg: 1613177786293 epoch sec; seqNo:17102; depth 29.80 2021-02-13T00:56:26Z- handleDvl() - RDI_Pathfinder msg: 1613177786.31 epoch sec; seqNo:8273 2021-02-13T00:56:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 68.94 , 0.00 , 0.00 , 59.23 2021-02-13T00:56:26Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:56:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177786.32 epoch sec; seqNo:17102; 4060430.52 north; 593399.58 east 2021-02-13T00:56:26Z- LcmTrn::handleMessages() - handling 41 messages took 0.85 ms and 124 calls... 2021-02-13T00:56:26Z- waiting for 1613177784.47, time is 1613177786.32 2021-02-13T00:56:26Z- LcmTrn::updateTrn() equating pose 1613177786.32 and meas times 1613177786.32 2021-02-13T00:56:26Z- LcmTrn::updateTrn() >>>> pose time 1613177786.32, meas time 1613177786.32 <<<< 2021-02-13T00:56:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177786.32 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:56:26Z- TerrainNav::motionUpdate() - currEstimate = (4060431, 593399.58, 29.80) 2021-02-13T00:56:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:56:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:56:26Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:56:26Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:56:26Z- TNavBF::Projecting Measurements 2021-02-13T00:56:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:56:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:56:26Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-13T00:56:26Z- TNavFilter::2 good beams. 2021-02-13T00:56:26Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:56:26Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:56:26Z- TNavBF:: beamIndex[1] = 3 2021-02-13T00:56:26Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:56:26Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:56:26Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-13T00:56:26Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:56:26Z- TNavBF::mmse selected from filter -1 2021-02-13T00:56:26Z- TerrainMapOctree: (vehN, vehE ) = (4060430.52, 593399.58). 2021-02-13T00:56:26Z- TerrainMapOctree: Min Distance = 1181.00. 2021-02-13T00:56:26Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:56:26Z- There is no specific good beam that all particles have in common. 2021-02-13T00:56:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:56:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 7.61 2021-02-13T00:56:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:56:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.120 ExpectedMeasDiff: nan currMeas.covariance: 5.61 2021-02-13T00:56:26Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:56:26Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:56:26Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:56:26Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:56:26Z- Filter 0 No beams used in subcloud update 2021-02-13T00:56:26Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:56:26Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:56:26Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 681.9 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:56:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177786.32 sec. 2021-02-13T00:56:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:56:26Z- TNavBF::mmse selected from filter -1 2021-02-13T00:56:26Z- variances([0], [2], and psi_berg) = 681.861962 832.989276 0.000000 2021-02-13T00:56:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:56:26Z- LcmTrn::publishEstimates() - MLE : 4060430.02 593419.08 29.80 2021-02-13T00:56:26Z- LcmTrn::publishEstimates() - MMSE : 4060430.52 593399.58 29.80 2021-02-13T00:56:26Z- LcmTrn::publishEstimates() - COVAR: 681.86 832.99 0.88 0.00 2021-02-13T00:56:26Z- AHRS_M2 msg: 1613177786.69 epoch sec; seqNo:16968 2021-02-13T00:56:26Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.09 psi 2021-02-13T00:56:26Z- handleDepth()-Depth_Keller msg: 1613177786697 epoch sec; seqNo:17103; depth 29.72 2021-02-13T00:56:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177786.71 epoch sec; seqNo:17103; 4060430.89 north; 593399.58 east 2021-02-13T00:56:27Z- AHRS_M2 msg: 1613177787.10 epoch sec; seqNo:16969 2021-02-13T00:56:27Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.09 psi 2021-02-13T00:56:27Z- handleDepth()-Depth_Keller msg: 1613177787101 epoch sec; seqNo:17104; depth 29.62 2021-02-13T00:56:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177787.11 epoch sec; seqNo:17104; 4060431.27 north; 593399.57 east 2021-02-13T00:56:27Z- AHRS_M2 msg: 1613177787.50 epoch sec; seqNo:16970 2021-02-13T00:56:27Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.09 psi 2021-02-13T00:56:27Z- handleDepth()-Depth_Keller msg: 1613177787517 epoch sec; seqNo:17105; depth 29.52 2021-02-13T00:56:27Z- handleDvl() - RDI_Pathfinder msg: 1613177787.53 epoch sec; seqNo:8274 2021-02-13T00:56:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 60.48 2021-02-13T00:56:27Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:56:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177787.54 epoch sec; seqNo:17105; 4060431.65 north; 593399.57 east 2021-02-13T00:56:27Z- AHRS_M2 msg: 1613177787.90 epoch sec; seqNo:16971 2021-02-13T00:56:27Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.08 psi 2021-02-13T00:56:27Z- handleDepth()-Depth_Keller msg: 1613177787909 epoch sec; seqNo:17106; depth 29.43 2021-02-13T00:56:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177787.93 epoch sec; seqNo:17106; 4060431.65 north; 593399.57 east 2021-02-13T00:56:28Z- AHRS_M2 msg: 1613177788.33 epoch sec; seqNo:16972 2021-02-13T00:56:28Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.08 psi 2021-02-13T00:56:28Z- handleDepth()-Depth_Keller msg: 1613177788338 epoch sec; seqNo:17107; depth 29.31 2021-02-13T00:56:28Z- handleDvl() - RDI_Pathfinder msg: 1613177788.35 epoch sec; seqNo:8275 2021-02-13T00:56:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 59.20 2021-02-13T00:56:28Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:56:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177788.36 epoch sec; seqNo:17107; 4060432.04 north; 593400.79 east 2021-02-13T00:56:28Z- AHRS_M2 msg: 1613177788.71 epoch sec; seqNo:16973 2021-02-13T00:56:28Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.08 psi 2021-02-13T00:56:28Z- handleDepth()-Depth_Keller msg: 1613177788717 epoch sec; seqNo:17108; depth 29.20 2021-02-13T00:56:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177788.73 epoch sec; seqNo:17108; 4060432.42 north; 593400.78 east 2021-02-13T00:56:29Z- AHRS_M2 msg: 1613177789.12 epoch sec; seqNo:16974 2021-02-13T00:56:29Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.08 psi 2021-02-13T00:56:29Z- handleDepth()-Depth_Keller msg: 1613177789125 epoch sec; seqNo:17109; depth 29.09 2021-02-13T00:56:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177789.14 epoch sec; seqNo:17109; 4060432.42 north; 593400.78 east 2021-02-13T00:56:29Z- AHRS_M2 msg: 1613177789.52 epoch sec; seqNo:16975 2021-02-13T00:56:29Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.08 psi 2021-02-13T00:56:29Z- handleDepth()-Depth_Keller msg: 1613177789525 epoch sec; seqNo:17110; depth 29.00 2021-02-13T00:56:29Z- handleDvl() - RDI_Pathfinder msg: 1613177789.54 epoch sec; seqNo:8276 2021-02-13T00:56:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 69.85 , 0.00 , 0.00 , 60.90 2021-02-13T00:56:29Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:56:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177789.55 epoch sec; seqNo:17110; 4060432.80 north; 593400.78 east 2021-02-13T00:56:29Z- AHRS_M2 msg: 1613177789.92 epoch sec; seqNo:16976 2021-02-13T00:56:29Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.09 psi 2021-02-13T00:56:29Z- handleDepth()-Depth_Keller msg: 1613177789929 epoch sec; seqNo:17111; depth 28.88 2021-02-13T00:56:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177789.94 epoch sec; seqNo:17111; 4060433.18 north; 593400.77 east 2021-02-13T00:56:30Z- AHRS_M2 msg: 1613177790.34 epoch sec; seqNo:16977 2021-02-13T00:56:30Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.09 psi 2021-02-13T00:56:30Z- handleDepth()-Depth_Keller msg: 1613177790340 epoch sec; seqNo:17112; depth 28.77 2021-02-13T00:56:30Z- handleDvl() - RDI_Pathfinder msg: 1613177790.35 epoch sec; seqNo:8277 2021-02-13T00:56:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:56:30Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:56:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177790.38 epoch sec; seqNo:17112; 4060433.18 north; 593400.77 east 2021-02-13T00:56:30Z- AHRS_M2 msg: 1613177790.73 epoch sec; seqNo:16978 2021-02-13T00:56:30Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.10 psi 2021-02-13T00:56:30Z- handleDepth()-Depth_Keller msg: 1613177790738 epoch sec; seqNo:17113; depth 28.66 2021-02-13T00:56:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177790.75 epoch sec; seqNo:17113; 4060433.56 north; 593400.77 east 2021-02-13T00:56:31Z- AHRS_M2 msg: 1613177791.14 epoch sec; seqNo:16979 2021-02-13T00:56:31Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.10 psi 2021-02-13T00:56:31Z- handleDepth()-Depth_Keller msg: 1613177791151 epoch sec; seqNo:17114; depth 28.55 2021-02-13T00:56:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177791.16 epoch sec; seqNo:17114; 4060433.94 north; 593400.77 east 2021-02-13T00:56:31Z- AHRS_M2 msg: 1613177791.54 epoch sec; seqNo:16980 2021-02-13T00:56:31Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.11 psi 2021-02-13T00:56:31Z- handleDepth()-Depth_Keller msg: 1613177791545 epoch sec; seqNo:17115; depth 28.42 2021-02-13T00:56:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177791.55 epoch sec; seqNo:17115; 4060433.95 north; 593401.99 east 2021-02-13T00:56:31Z- LcmTrn::handleMessages() - handling 43 messages took 0.72 ms and 129 calls... 2021-02-13T00:56:31Z- waiting for 1613177789.32, time is 1613177791.55 2021-02-13T00:56:31Z- LcmTrn::updateTrn() >>>> pose time 1613177791.55, meas time 1613177790.35 <<<< 2021-02-13T00:56:31Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177790.35, ping # 0. 2021-02-13T00:56:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177790.35 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:56:31Z- TerrainNav::motionUpdate() - currEstimate = (4060434, 593401.99, 28.42) 2021-02-13T00:56:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:56:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:56:31Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:56:31Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:56:31Z- TNavBF::Projecting Measurements 2021-02-13T00:56:31Z- TNavFilter::Measurement from time = 1613177790.35 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:56:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:56:31Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:56:31Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:56:31Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:56:31Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:56:31Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:56:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:56:31Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 681.8 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:56:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:56:31Z- TNavBF::mmse selected from filter -1 2021-02-13T00:56:31Z- variances([0], [2], and psi_berg) = 681.799808 832.989249 0.000000 2021-02-13T00:56:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:56:31Z- LcmTrn::publishEstimates() - MLE : 4060433.45 593421.49 28.42 2021-02-13T00:56:31Z- LcmTrn::publishEstimates() - MMSE : 4060433.95 593401.99 28.42 2021-02-13T00:56:31Z- LcmTrn::publishEstimates() - COVAR: 681.80 832.99 1.09 0.00 2021-02-13T00:56:31Z- AHRS_M2 msg: 1613177791.94 epoch sec; seqNo:16981 2021-02-13T00:56:31Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.11 psi 2021-02-13T00:56:31Z- handleDepth()-Depth_Keller msg: 1613177791949 epoch sec; seqNo:17116; depth 28.32 2021-02-13T00:56:31Z- handleDvl() - RDI_Pathfinder msg: 1613177791.96 epoch sec; seqNo:8278 2021-02-13T00:56:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 0.00 , 97.99 , 59.26 2021-02-13T00:56:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , -0.03 2021-02-13T00:56:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177791.97 epoch sec; seqNo:17116; 4060434.33 north; 593401.98 east 2021-02-13T00:56:32Z- AHRS_M2 msg: 1613177792.35 epoch sec; seqNo:16982 2021-02-13T00:56:32Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.11 psi 2021-02-13T00:56:32Z- handleDepth()-Depth_Keller msg: 1613177792355 epoch sec; seqNo:17117; depth 28.18 2021-02-13T00:56:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177792.37 epoch sec; seqNo:17117; 4060434.71 north; 593401.98 east 2021-02-13T00:56:32Z- AHRS_M2 msg: 1613177792.75 epoch sec; seqNo:16983 2021-02-13T00:56:32Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.11 psi 2021-02-13T00:56:32Z- handleDepth()-Depth_Keller msg: 1613177792761 epoch sec; seqNo:17118; depth 28.07 2021-02-13T00:56:32Z- handleDvl() - RDI_Pathfinder msg: 1613177792.78 epoch sec; seqNo:8279 2021-02-13T00:56:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 80.67 , 97.98 , 60.49 2021-02-13T00:56:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , -0.06 2021-02-13T00:56:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177792.79 epoch sec; seqNo:17118; 4060435.09 north; 593401.97 east 2021-02-13T00:56:33Z- AHRS_M2 msg: 1613177793.16 epoch sec; seqNo:16984 2021-02-13T00:56:33Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.11 psi 2021-02-13T00:56:33Z- handleDepth()-Depth_Keller msg: 1613177793179 epoch sec; seqNo:17119; depth 27.94 2021-02-13T00:56:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177793.20 epoch sec; seqNo:17119; 4060435.47 north; 593401.97 east 2021-02-13T00:56:33Z- AHRS_M2 msg: 1613177793.56 epoch sec; seqNo:16985 2021-02-13T00:56:33Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.11 psi 2021-02-13T00:56:33Z- handleDepth()-Depth_Keller msg: 1613177793573 epoch sec; seqNo:17120; depth 27.82 2021-02-13T00:56:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177793.58 epoch sec; seqNo:17120; 4060435.47 north; 593401.97 east 2021-02-13T00:56:33Z- AHRS_M2 msg: 1613177793.96 epoch sec; seqNo:16986 2021-02-13T00:56:33Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.11 psi 2021-02-13T00:56:33Z- handleDepth()-Depth_Keller msg: 1613177793974 epoch sec; seqNo:17121; depth 27.70 2021-02-13T00:56:33Z- handleDvl() - RDI_Pathfinder msg: 1613177793.99 epoch sec; seqNo:8280 2021-02-13T00:56:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 81.75 , 97.98 , 60.49 2021-02-13T00:56:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , -0.09 2021-02-13T00:56:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177794.00 epoch sec; seqNo:17121; 4060435.85 north; 593401.97 east 2021-02-13T00:56:34Z- AHRS_M2 msg: 1613177794.37 epoch sec; seqNo:16987 2021-02-13T00:56:34Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.12 psi 2021-02-13T00:56:34Z- handleDepth()-Depth_Keller msg: 1613177794377 epoch sec; seqNo:17122; depth 27.59 2021-02-13T00:56:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177794.39 epoch sec; seqNo:17122; 4060436.24 north; 593403.18 east 2021-02-13T00:56:34Z- AHRS_M2 msg: 1613177794.77 epoch sec; seqNo:16988 2021-02-13T00:56:34Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.12 psi 2021-02-13T00:56:34Z- handleDepth()-Depth_Keller msg: 1613177794781 epoch sec; seqNo:17123; depth 27.47 2021-02-13T00:56:34Z- handleDvl() - RDI_Pathfinder msg: 1613177794.79 epoch sec; seqNo:8281 2021-02-13T00:56:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 82.84 , 97.98 , 61.30 2021-02-13T00:56:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , -0.10 2021-02-13T00:56:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177794.80 epoch sec; seqNo:17123; 4060436.24 north; 593403.18 east 2021-02-13T00:56:35Z- AHRS_M2 msg: 1613177795.18 epoch sec; seqNo:16989 2021-02-13T00:56:35Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.12 psi 2021-02-13T00:56:35Z- handleDepth()-Depth_Keller msg: 1613177795246 epoch sec; seqNo:17124; depth 27.33 2021-02-13T00:56:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177795.26 epoch sec; seqNo:17124; 4060436.62 north; 593403.18 east 2021-02-13T00:56:35Z- AHRS_M2 msg: 1613177795.60 epoch sec; seqNo:16990 2021-02-13T00:56:35Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.13 psi 2021-02-13T00:56:35Z- handleDepth()-Depth_Keller msg: 1613177795609 epoch sec; seqNo:17125; depth 27.18 2021-02-13T00:56:35Z- handleDvl() - RDI_Pathfinder msg: 1613177795.62 epoch sec; seqNo:8282 2021-02-13T00:56:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 0.00 , 97.98 , 61.30 2021-02-13T00:56:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , -0.08 2021-02-13T00:56:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177795.63 epoch sec; seqNo:17125; 4060437.00 north; 593403.17 east 2021-02-13T00:56:35Z- AHRS_M2 msg: 1613177795.98 epoch sec; seqNo:16991 2021-02-13T00:56:35Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.13 psi 2021-02-13T00:56:36Z- handleDepth()-Depth_Keller msg: 1613177796014 epoch sec; seqNo:17126; depth 27.08 2021-02-13T00:56:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177796.03 epoch sec; seqNo:17126; 4060437.00 north; 593403.17 east 2021-02-13T00:56:36Z- AHRS_M2 msg: 1613177796.39 epoch sec; seqNo:16992 2021-02-13T00:56:36Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.14 psi 2021-02-13T00:56:36Z- handleDepth()-Depth_Keller msg: 1613177796485 epoch sec; seqNo:17127; depth 26.96 2021-02-13T00:56:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177796.50 epoch sec; seqNo:17127; 4060437.37 north; 593403.17 east 2021-02-13T00:56:36Z- AHRS_M2 msg: 1613177796.79 epoch sec; seqNo:16993 2021-02-13T00:56:36Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.14 psi 2021-02-13T00:56:36Z- LcmTrn::handleMessages() - handling 42 messages took 0.72 ms and 128 calls... 2021-02-13T00:56:36Z- waiting for 1613177794.55, time is 1613177796.79 2021-02-13T00:56:36Z- LcmTrn::updateTrn() >>>> pose time 1613177796.79, meas time 1613177795.62 <<<< 2021-02-13T00:56:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177795.62 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:56:36Z- TerrainNav::motionUpdate() - currEstimate = (4060437, 593403.17, 26.96) 2021-02-13T00:56:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:56:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:56:36Z- TNavBF::Projecting Measurements 2021-02-13T00:56:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:56:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:56:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:56:36Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:56:36Z- TNavFilter::3 good beams. 2021-02-13T00:56:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:56:36Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:56:36Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:56:36Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:56:36Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:56:36Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:56:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:56:36Z- TNavBF::mmse selected from filter -1 2021-02-13T00:56:36Z- TerrainMapOctree: (vehN, vehE ) = (4060436.61, 593402.90). 2021-02-13T00:56:36Z- TerrainMapOctree: Min Distance = 1174.38. 2021-02-13T00:56:36Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:56:36Z- There is no specific good beam that all particles have in common. 2021-02-13T00:56:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:56:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-13T00:56:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:56:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:56:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:56:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.118 ExpectedMeasDiff: nan currMeas.covariance: 6.01 2021-02-13T00:56:36Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:56:36Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:56:36Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:56:36Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:56:36Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:56:36Z- Filter 0 No beams used in subcloud update 2021-02-13T00:56:36Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:56:36Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:56:36Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 681.8 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:56:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177795.62 sec. 2021-02-13T00:56:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:56:36Z- TNavBF::mmse selected from filter -1 2021-02-13T00:56:36Z- variances([0], [2], and psi_berg) = 681.742206 832.989225 0.000000 2021-02-13T00:56:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:56:36Z- LcmTrn::publishEstimates() - MLE : 4060436.87 593422.67 26.96 2021-02-13T00:56:36Z- LcmTrn::publishEstimates() - MMSE : 4060437.37 593403.17 26.96 2021-02-13T00:56:36Z- LcmTrn::publishEstimates() - COVAR: 681.74 832.99 0.95 0.00 2021-02-13T00:56:36Z- handleDepth()-Depth_Keller msg: 1613177796805 epoch sec; seqNo:17128; depth 26.83 2021-02-13T00:56:36Z- handleDvl() - RDI_Pathfinder msg: 1613177796.82 epoch sec; seqNo:8283 2021-02-13T00:56:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 83.92 , 97.98 , 60.90 2021-02-13T00:56:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.10 , -0.03 2021-02-13T00:56:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177796.83 epoch sec; seqNo:17128; 4060437.75 north; 593403.17 east 2021-02-13T00:56:37Z- AHRS_M2 msg: 1613177797.19 epoch sec; seqNo:16994 2021-02-13T00:56:37Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.15 psi 2021-02-13T00:56:37Z- handleDepth()-Depth_Keller msg: 1613177797221 epoch sec; seqNo:17129; depth 26.72 2021-02-13T00:56:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177797.23 epoch sec; seqNo:17129; 4060437.77 north; 593404.39 east 2021-02-13T00:56:37Z- AHRS_M2 msg: 1613177797.60 epoch sec; seqNo:16995 2021-02-13T00:56:37Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.15 psi 2021-02-13T00:56:37Z- handleDepth()-Depth_Keller msg: 1613177797642 epoch sec; seqNo:17130; depth 26.60 2021-02-13T00:56:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177797.66 epoch sec; seqNo:17130; 4060438.15 north; 593404.38 east 2021-02-13T00:56:38Z- AHRS_M2 msg: 1613177798.00 epoch sec; seqNo:16996 2021-02-13T00:56:38Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.15 psi 2021-02-13T00:56:38Z- handleDepth()-Depth_Keller msg: 1613177798009 epoch sec; seqNo:17131; depth 26.47 2021-02-13T00:56:38Z- handleDvl() - RDI_Pathfinder msg: 1613177798.02 epoch sec; seqNo:8284 2021-02-13T00:56:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 84.93 , 99.00 , 61.71 2021-02-13T00:56:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.12 , 0.05 2021-02-13T00:56:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177798.03 epoch sec; seqNo:17131; 4060438.52 north; 593404.38 east 2021-02-13T00:56:38Z- AHRS_M2 msg: 1613177798.41 epoch sec; seqNo:16997 2021-02-13T00:56:38Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.16 psi 2021-02-13T00:56:38Z- handleDepth()-Depth_Keller msg: 1613177798414 epoch sec; seqNo:17132; depth 26.36 2021-02-13T00:56:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177798.42 epoch sec; seqNo:17132; 4060438.90 north; 593404.37 east 2021-02-13T00:56:38Z- AHRS_M2 msg: 1613177798.81 epoch sec; seqNo:16998 2021-02-13T00:56:38Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.16 psi 2021-02-13T00:56:38Z- handleDepth()-Depth_Keller msg: 1613177798817 epoch sec; seqNo:17133; depth 26.23 2021-02-13T00:56:38Z- handleDvl() - RDI_Pathfinder msg: 1613177798.84 epoch sec; seqNo:8285 2021-02-13T00:56:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.70 , 85.00 , 101.23 , 62.94 2021-02-13T00:56:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , 0.10 2021-02-13T00:56:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177798.85 epoch sec; seqNo:17133; 4060438.90 north; 593404.37 east 2021-02-13T00:56:39Z- AHRS_M2 msg: 1613177799.22 epoch sec; seqNo:16999 2021-02-13T00:56:39Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.17 psi 2021-02-13T00:56:39Z- handleDepth()-Depth_Keller msg: 1613177799221 epoch sec; seqNo:17134; depth 26.09 2021-02-13T00:56:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177799.23 epoch sec; seqNo:17134; 4060439.28 north; 593404.37 east 2021-02-13T00:56:39Z- AHRS_M2 msg: 1613177799.62 epoch sec; seqNo:17000 2021-02-13T00:56:39Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.17 psi 2021-02-13T00:56:39Z- handleDepth()-Depth_Keller msg: 1613177799627 epoch sec; seqNo:17135; depth 25.98 2021-02-13T00:56:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177799.64 epoch sec; seqNo:17135; 4060439.66 north; 593404.37 east 2021-02-13T00:56:40Z- AHRS_M2 msg: 1613177800.02 epoch sec; seqNo:17001 2021-02-13T00:56:40Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.17 psi 2021-02-13T00:56:40Z- handleDepth()-Depth_Keller msg: 1613177800033 epoch sec; seqNo:17136; depth 25.87 2021-02-13T00:56:40Z- handleDvl() - RDI_Pathfinder msg: 1613177800.05 epoch sec; seqNo:8286 2021-02-13T00:56:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 147.74 , 100.14 , 62.53 2021-02-13T00:56:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.28 , 0.18 2021-02-13T00:56:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177800.08 epoch sec; seqNo:17136; 4060440.04 north; 593404.36 east 2021-02-13T00:56:40Z- AHRS_M2 msg: 1613177800.43 epoch sec; seqNo:17002 2021-02-13T00:56:40Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.18 psi 2021-02-13T00:56:40Z- handleDepth()-Depth_Keller msg: 1613177800433 epoch sec; seqNo:17137; depth 25.72 2021-02-13T00:56:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177800.45 epoch sec; seqNo:17137; 4060440.43 north; 593405.58 east 2021-02-13T00:56:40Z- AHRS_M2 msg: 1613177800.84 epoch sec; seqNo:17003 2021-02-13T00:56:40Z- AHRS_M2 msg: 0.61 phi; 0.38 theta; -0.18 psi 2021-02-13T00:56:40Z- handleDepth()-Depth_Keller msg: 1613177800844 epoch sec; seqNo:17138; depth 25.61 2021-02-13T00:56:40Z- handleDvl() - RDI_Pathfinder msg: 1613177800.86 epoch sec; seqNo:8287 2021-02-13T00:56:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.52 , 87.77 , 101.29 , 62.53 2021-02-13T00:56:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , 0.06 2021-02-13T00:56:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177800.87 epoch sec; seqNo:17138; 4060440.81 north; 593405.57 east 2021-02-13T00:56:41Z- AHRS_M2 msg: 1613177801.24 epoch sec; seqNo:17004 2021-02-13T00:56:41Z- AHRS_M2 msg: 0.61 phi; 0.38 theta; -0.18 psi 2021-02-13T00:56:41Z- handleDepth()-Depth_Keller msg: 1613177801246 epoch sec; seqNo:17139; depth 25.48 2021-02-13T00:56:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177801.27 epoch sec; seqNo:17139; 4060440.81 north; 593405.57 east 2021-02-13T00:56:41Z- AHRS_M2 msg: 1613177801.66 epoch sec; seqNo:17005 2021-02-13T00:56:41Z- AHRS_M2 msg: 0.61 phi; 0.38 theta; -0.18 psi 2021-02-13T00:56:41Z- handleDepth()-Depth_Keller msg: 1613177801667 epoch sec; seqNo:17140; depth 25.36 2021-02-13T00:56:41Z- handleDvl() - RDI_Pathfinder msg: 1613177801.69 epoch sec; seqNo:8288 2021-02-13T00:56:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.11 , 87.10 , 103.32 , 64.57 2021-02-13T00:56:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.08 , 0.00 2021-02-13T00:56:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177801.70 epoch sec; seqNo:17140; 4060441.19 north; 593405.57 east 2021-02-13T00:56:41Z- LcmTrn::handleMessages() - handling 44 messages took 0.78 ms and 129 calls... 2021-02-13T00:56:41Z- waiting for 1613177799.79, time is 1613177801.70 2021-02-13T00:56:41Z- LcmTrn::updateTrn() equating pose 1613177801.70 and meas times 1613177801.70 2021-02-13T00:56:41Z- LcmTrn::updateTrn() >>>> pose time 1613177801.70, meas time 1613177801.70 <<<< 2021-02-13T00:56:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177801.70 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:56:41Z- TerrainNav::motionUpdate() - currEstimate = (4060441, 593405.57, 25.36) 2021-02-13T00:56:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:56:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:56:41Z- TNavBF::Projecting Measurements 2021-02-13T00:56:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:56:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:56:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:56:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:56:41Z- TNavFilter::4 good beams. 2021-02-13T00:56:41Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:56:41Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:56:41Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:56:41Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:56:41Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:56:41Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:56:41Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:56:41Z- TNavBF::mmse selected from filter -1 2021-02-13T00:56:41Z- TerrainMapOctree: (vehN, vehE ) = (4060441.19, 593405.57). 2021-02-13T00:56:41Z- TerrainMapOctree: Min Distance = 1169.29. 2021-02-13T00:56:41Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:56:41Z- There is no specific good beam that all particles have in common. 2021-02-13T00:56:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:56:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-13T00:56:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:56:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 12.14 2021-02-13T00:56:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:56:41Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 17.08 2021-02-13T00:56:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:56:41Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 6.67 2021-02-13T00:56:41Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:56:41Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:56:41Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:56:41Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:56:41Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:56:41Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:56:41Z- Filter 0 No beams used in subcloud update 2021-02-13T00:56:41Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:56:41Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:56:41Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 681.7 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:56:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177801.70 sec. 2021-02-13T00:56:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:56:41Z- TNavBF::mmse selected from filter -1 2021-02-13T00:56:41Z- variances([0], [2], and psi_berg) = 681.694771 832.989204 0.000000 2021-02-13T00:56:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:56:41Z- LcmTrn::publishEstimates() - MLE : 4060440.69 593425.07 25.36 2021-02-13T00:56:41Z- LcmTrn::publishEstimates() - MMSE : 4060441.19 593405.57 25.36 2021-02-13T00:56:41Z- LcmTrn::publishEstimates() - COVAR: 681.69 832.99 0.84 0.00 2021-02-13T00:56:42Z- AHRS_M2 msg: 1613177802.05 epoch sec; seqNo:17006 2021-02-13T00:56:42Z- AHRS_M2 msg: 0.62 phi; 0.38 theta; -0.18 psi 2021-02-13T00:56:42Z- handleDepth()-Depth_Keller msg: 1613177802059 epoch sec; seqNo:17141; depth 25.22 2021-02-13T00:56:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177802.07 epoch sec; seqNo:17141; 4060441.57 north; 593405.57 east 2021-02-13T00:56:42Z- AHRS_M2 msg: 1613177802.45 epoch sec; seqNo:17007 2021-02-13T00:56:42Z- AHRS_M2 msg: 0.62 phi; 0.38 theta; -0.18 psi 2021-02-13T00:56:42Z- handleDepth()-Depth_Keller msg: 1613177802458 epoch sec; seqNo:17142; depth 25.10 2021-02-13T00:56:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177802.47 epoch sec; seqNo:17142; 4060441.57 north; 593405.57 east 2021-02-13T00:56:42Z- AHRS_M2 msg: 1613177802.86 epoch sec; seqNo:17008 2021-02-13T00:56:42Z- AHRS_M2 msg: 0.62 phi; 0.38 theta; -0.18 psi 2021-02-13T00:56:42Z- handleDepth()-Depth_Keller msg: 1613177802861 epoch sec; seqNo:17143; depth 24.98 2021-02-13T00:56:42Z- handleDvl() - RDI_Pathfinder msg: 1613177802.87 epoch sec; seqNo:8289 2021-02-13T00:56:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 0.00 , 102.18 , 63.35 2021-02-13T00:56:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.08 , -0.04 2021-02-13T00:56:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177802.89 epoch sec; seqNo:17143; 4060441.96 north; 593406.78 east 2021-02-13T00:56:43Z- AHRS_M2 msg: 1613177803.26 epoch sec; seqNo:17009 2021-02-13T00:56:43Z- AHRS_M2 msg: 0.62 phi; 0.38 theta; -0.18 psi 2021-02-13T00:56:43Z- handleDepth()-Depth_Keller msg: 1613177803265 epoch sec; seqNo:17144; depth 24.84 2021-02-13T00:56:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177803.28 epoch sec; seqNo:17144; 4060442.34 north; 593406.78 east 2021-02-13T00:56:43Z- AHRS_M2 msg: 1613177803.66 epoch sec; seqNo:17010 2021-02-13T00:56:43Z- AHRS_M2 msg: 0.62 phi; 0.38 theta; -0.17 psi 2021-02-13T00:56:43Z- handleDepth()-Depth_Keller msg: 1613177803671 epoch sec; seqNo:17145; depth 24.72 2021-02-13T00:56:43Z- handleDvl() - RDI_Pathfinder msg: 1613177803.68 epoch sec; seqNo:8290 2021-02-13T00:56:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.94 , 0.00 , 103.26 , 63.35 2021-02-13T00:56:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , -0.04 2021-02-13T00:56:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177803.69 epoch sec; seqNo:17145; 4060442.72 north; 593406.77 east 2021-02-13T00:56:44Z- AHRS_M2 msg: 1613177804.07 epoch sec; seqNo:17011 2021-02-13T00:56:44Z- AHRS_M2 msg: 0.62 phi; 0.38 theta; -0.17 psi 2021-02-13T00:56:44Z- handleDepth()-Depth_Keller msg: 1613177804077 epoch sec; seqNo:17146; depth 24.59 2021-02-13T00:56:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177804.09 epoch sec; seqNo:17146; 4060442.72 north; 593406.77 east 2021-02-13T00:56:44Z- AHRS_M2 msg: 1613177804.47 epoch sec; seqNo:17012 2021-02-13T00:56:44Z- AHRS_M2 msg: 0.62 phi; 0.38 theta; -0.16 psi 2021-02-13T00:56:44Z- handleDepth()-Depth_Keller msg: 1613177804477 epoch sec; seqNo:17147; depth 24.45 2021-02-13T00:56:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177804.48 epoch sec; seqNo:17147; 4060443.10 north; 593406.77 east 2021-02-13T00:56:44Z- AHRS_M2 msg: 1613177804.88 epoch sec; seqNo:17013 2021-02-13T00:56:44Z- AHRS_M2 msg: 0.62 phi; 0.38 theta; -0.16 psi 2021-02-13T00:56:44Z- handleDepth()-Depth_Keller msg: 1613177804882 epoch sec; seqNo:17148; depth 24.33 2021-02-13T00:56:44Z- handleDvl() - RDI_Pathfinder msg: 1613177804.89 epoch sec; seqNo:8291 2021-02-13T00:56:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.94 , 89.33 , 103.39 , 65.40 2021-02-13T00:56:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , -0.05 2021-02-13T00:56:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177804.90 epoch sec; seqNo:17148; 4060443.48 north; 593406.76 east 2021-02-13T00:56:45Z- AHRS_M2 msg: 1613177805.28 epoch sec; seqNo:17014 2021-02-13T00:56:45Z- AHRS_M2 msg: 0.62 phi; 0.38 theta; -0.15 psi 2021-02-13T00:56:45Z- handleDepth()-Depth_Keller msg: 1613177805294 epoch sec; seqNo:17149; depth 24.21 2021-02-13T00:56:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177805.31 epoch sec; seqNo:17149; 4060443.49 north; 593407.99 east 2021-02-13T00:56:45Z- AHRS_M2 msg: 1613177805.68 epoch sec; seqNo:17015 2021-02-13T00:56:45Z- AHRS_M2 msg: 0.62 phi; 0.38 theta; -0.14 psi 2021-02-13T00:56:45Z- handleDepth()-Depth_Keller msg: 1613177805689 epoch sec; seqNo:17150; depth 24.08 2021-02-13T00:56:45Z- handleDvl() - RDI_Pathfinder msg: 1613177805.70 epoch sec; seqNo:8292 2021-02-13T00:56:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 0.00 , 100.83 , 63.76 2021-02-13T00:56:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , -0.04 2021-02-13T00:56:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177805.72 epoch sec; seqNo:17150; 4060443.87 north; 593407.98 east 2021-02-13T00:56:46Z- AHRS_M2 msg: 1613177806.09 epoch sec; seqNo:17016 2021-02-13T00:56:46Z- AHRS_M2 msg: 0.62 phi; 0.38 theta; -0.14 psi 2021-02-13T00:56:46Z- handleDepth()-Depth_Keller msg: 1613177806097 epoch sec; seqNo:17151; depth 23.94 2021-02-13T00:56:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177806.11 epoch sec; seqNo:17151; 4060444.25 north; 593407.98 east 2021-02-13T00:56:46Z- AHRS_M2 msg: 1613177806.49 epoch sec; seqNo:17017 2021-02-13T00:56:46Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.14 psi 2021-02-13T00:56:46Z- handleDepth()-Depth_Keller msg: 1613177806498 epoch sec; seqNo:17152; depth 23.81 2021-02-13T00:56:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177806.51 epoch sec; seqNo:17152; 4060444.63 north; 593407.97 east 2021-02-13T00:56:46Z- AHRS_M2 msg: 1613177806.90 epoch sec; seqNo:17018 2021-02-13T00:56:46Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.15 psi 2021-02-13T00:56:46Z- handleDepth()-Depth_Keller msg: 1613177806901 epoch sec; seqNo:17153; depth 23.68 2021-02-13T00:56:46Z- handleDvl() - RDI_Pathfinder msg: 1613177806.92 epoch sec; seqNo:8293 2021-02-13T00:56:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 153.01 , 0.00 , 104.34 , 65.39 2021-02-13T00:56:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 1.39 , -0.04 2021-02-13T00:56:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177806.93 epoch sec; seqNo:17153; 4060444.26 north; 593409.20 east 2021-02-13T00:56:46Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-13T00:56:46Z- waiting for 1613177804.70, time is 1613177806.93 2021-02-13T00:56:46Z- LcmTrn::updateTrn() equating pose 1613177806.93 and meas times 1613177806.93 2021-02-13T00:56:46Z- LcmTrn::updateTrn() >>>> pose time 1613177806.93, meas time 1613177806.93 <<<< 2021-02-13T00:56:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177806.93 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:56:46Z- TerrainNav::motionUpdate() - currEstimate = (4060444, 593409.20, 23.68) 2021-02-13T00:56:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:56:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:56:46Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177806.93 sec.; No relevant navigation data available 2021-02-13T00:56:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:56:46Z- TNavBF::mmse selected from filter -1 2021-02-13T00:56:46Z- variances([0], [2], and psi_berg) = 681.646080 832.989183 0.000000 2021-02-13T00:56:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:56:46Z- LcmTrn::publishEstimates() - MLE : 4060443.76 593428.70 23.68 2021-02-13T00:56:46Z- LcmTrn::publishEstimates() - MMSE : 4060444.26 593409.20 23.68 2021-02-13T00:56:46Z- LcmTrn::publishEstimates() - COVAR: 681.65 832.99 1.08 0.00 2021-02-13T00:56:47Z- AHRS_M2 msg: 1613177807.30 epoch sec; seqNo:17019 2021-02-13T00:56:47Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.15 psi 2021-02-13T00:56:47Z- handleDepth()-Depth_Keller msg: 1613177807309 epoch sec; seqNo:17154; depth 23.54 2021-02-13T00:56:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177807.32 epoch sec; seqNo:17154; 4060444.26 north; 593409.20 east 2021-02-13T00:56:47Z- AHRS_M2 msg: 1613177807.71 epoch sec; seqNo:17020 2021-02-13T00:56:47Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.16 psi 2021-02-13T00:56:47Z- handleDepth()-Depth_Keller msg: 1613177807729 epoch sec; seqNo:17155; depth 23.40 2021-02-13T00:56:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177807.75 epoch sec; seqNo:17155; 4060444.27 north; 593410.42 east 2021-02-13T00:56:48Z- AHRS_M2 msg: 1613177808.11 epoch sec; seqNo:17021 2021-02-13T00:56:48Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.16 psi 2021-02-13T00:56:48Z- handleDepth()-Depth_Keller msg: 1613177808113 epoch sec; seqNo:17156; depth 23.27 2021-02-13T00:56:48Z- handleDvl() - RDI_Pathfinder msg: 1613177808.14 epoch sec; seqNo:8294 2021-02-13T00:56:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.44 , 103.39 , 65.80 2021-02-13T00:56:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , -0.04 2021-02-13T00:56:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177808.15 epoch sec; seqNo:17156; 4060444.65 north; 593410.41 east 2021-02-13T00:56:48Z- AHRS_M2 msg: 1613177808.51 epoch sec; seqNo:17022 2021-02-13T00:56:48Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.17 psi 2021-02-13T00:56:48Z- handleDepth()-Depth_Keller msg: 1613177808586 epoch sec; seqNo:17157; depth 23.14 2021-02-13T00:56:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177808.60 epoch sec; seqNo:17157; 4060444.65 north; 593410.41 east 2021-02-13T00:56:48Z- AHRS_M2 msg: 1613177808.92 epoch sec; seqNo:17023 2021-02-13T00:56:48Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.18 psi 2021-02-13T00:56:48Z- handleDepth()-Depth_Keller msg: 1613177808928 epoch sec; seqNo:17158; depth 23.00 2021-02-13T00:56:48Z- handleDvl() - RDI_Pathfinder msg: 1613177808.94 epoch sec; seqNo:8295 2021-02-13T00:56:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 97.85 , 102.71 , 66.62 2021-02-13T00:56:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , -0.04 2021-02-13T00:56:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177808.97 epoch sec; seqNo:17158; 4060445.03 north; 593410.41 east 2021-02-13T00:56:49Z- AHRS_M2 msg: 1613177809.32 epoch sec; seqNo:17024 2021-02-13T00:56:49Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.18 psi 2021-02-13T00:56:49Z- handleDepth()-Depth_Keller msg: 1613177809345 epoch sec; seqNo:17159; depth 22.89 2021-02-13T00:56:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177809.36 epoch sec; seqNo:17159; 4060445.41 north; 593410.41 east 2021-02-13T00:56:49Z- AHRS_M2 msg: 1613177809.73 epoch sec; seqNo:17025 2021-02-13T00:56:49Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.19 psi 2021-02-13T00:56:49Z- handleDepth()-Depth_Keller msg: 1613177809806 epoch sec; seqNo:17160; depth 22.76 2021-02-13T00:56:49Z- handleDvl() - RDI_Pathfinder msg: 1613177809.82 epoch sec; seqNo:8296 2021-02-13T00:56:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.71 , 151.73 , 101.83 , 0.00 2021-02-13T00:56:49Z- handleDvl() - RDI_Pathfinder msg: velocities -0.01 , 0.84 , -0.47 2021-02-13T00:56:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177809.83 epoch sec; seqNo:17160; 4060445.03 north; 593410.41 east 2021-02-13T00:56:50Z- AHRS_M2 msg: 1613177810.13 epoch sec; seqNo:17026 2021-02-13T00:56:50Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-13T00:56:50Z- handleDepth()-Depth_Keller msg: 1613177810146 epoch sec; seqNo:17161; depth 22.63 2021-02-13T00:56:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177810.18 epoch sec; seqNo:17161; 4060444.67 north; 593411.63 east 2021-02-13T00:56:50Z- AHRS_M2 msg: 1613177810.53 epoch sec; seqNo:17027 2021-02-13T00:56:50Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-13T00:56:50Z- handleDepth()-Depth_Keller msg: 1613177810557 epoch sec; seqNo:17162; depth 22.52 2021-02-13T00:56:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177810.57 epoch sec; seqNo:17162; 4060444.67 north; 593411.63 east 2021-02-13T00:56:50Z- AHRS_M2 msg: 1613177810.93 epoch sec; seqNo:17028 2021-02-13T00:56:50Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-13T00:56:50Z- handleDepth()-Depth_Keller msg: 1613177810961 epoch sec; seqNo:17163; depth 22.41 2021-02-13T00:56:50Z- handleDvl() - RDI_Pathfinder msg: 1613177810.97 epoch sec; seqNo:8297 2021-02-13T00:56:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.90 , 97.24 , 101.70 , 64.64 2021-02-13T00:56:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.04 2021-02-13T00:56:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177810.98 epoch sec; seqNo:17163; 4060444.67 north; 593411.63 east 2021-02-13T00:56:51Z- AHRS_M2 msg: 1613177811.34 epoch sec; seqNo:17029 2021-02-13T00:56:51Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.19 psi 2021-02-13T00:56:51Z- handleDepth()-Depth_Keller msg: 1613177811349 epoch sec; seqNo:17164; depth 22.30 2021-02-13T00:56:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177811.36 epoch sec; seqNo:17164; 4060445.04 north; 593411.63 east 2021-02-13T00:56:51Z- AHRS_M2 msg: 1613177811.74 epoch sec; seqNo:17030 2021-02-13T00:56:51Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-13T00:56:51Z- handleDepth()-Depth_Keller msg: 1613177811749 epoch sec; seqNo:17165; depth 22.17 2021-02-13T00:56:51Z- handleDvl() - RDI_Pathfinder msg: 1613177811.76 epoch sec; seqNo:8298 2021-02-13T00:56:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 0.00 , 101.83 , 67.03 2021-02-13T00:56:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , 0.04 2021-02-13T00:56:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177811.77 epoch sec; seqNo:17165; 4060445.42 north; 593411.63 east 2021-02-13T00:56:52Z- LcmTrn::handleMessages() - handling 41 messages took 0.77 ms and 126 calls... 2021-02-13T00:56:52Z- waiting for 1613177809.93, time is 1613177811.77 2021-02-13T00:56:52Z- LcmTrn::updateTrn() equating pose 1613177811.77 and meas times 1613177811.77 2021-02-13T00:56:52Z- LcmTrn::updateTrn() >>>> pose time 1613177811.77, meas time 1613177811.77 <<<< 2021-02-13T00:56:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177811.77 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:56:52Z- TerrainNav::motionUpdate() - currEstimate = (4060445, 593411.63, 22.17) 2021-02-13T00:56:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:56:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:56:52Z- TNavBF::Projecting Measurements 2021-02-13T00:56:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:56:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:56:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:56:52Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:56:52Z- TNavFilter::3 good beams. 2021-02-13T00:56:52Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:56:52Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:56:52Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:56:52Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:56:52Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:56:52Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:56:52Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:56:52Z- TNavBF::mmse selected from filter -1 2021-02-13T00:56:52Z- TerrainMapOctree: (vehN, vehE ) = (4060445.42, 593411.63). 2021-02-13T00:56:52Z- TerrainMapOctree: Min Distance = 1161.99. 2021-02-13T00:56:52Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:56:52Z- There is no specific good beam that all particles have in common. 2021-02-13T00:56:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:56:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-13T00:56:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:56:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 16.59 2021-02-13T00:56:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:56:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 7.19 2021-02-13T00:56:52Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:56:52Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:56:52Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:56:52Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:56:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:56:52Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:56:52Z- Filter 0 No beams used in subcloud update 2021-02-13T00:56:52Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:56:52Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:56:52Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 681.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:56:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177811.77 sec. 2021-02-13T00:56:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:56:52Z- TNavBF::mmse selected from filter -1 2021-02-13T00:56:52Z- variances([0], [2], and psi_berg) = 681.609452 832.989167 0.000000 2021-02-13T00:56:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:56:52Z- LcmTrn::publishEstimates() - MLE : 4060444.92 593431.13 22.17 2021-02-13T00:56:52Z- LcmTrn::publishEstimates() - MMSE : 4060445.42 593411.63 22.17 2021-02-13T00:56:52Z- LcmTrn::publishEstimates() - COVAR: 681.61 832.99 0.92 0.00 2021-02-13T00:56:52Z- AHRS_M2 msg: 1613177812.15 epoch sec; seqNo:17031 2021-02-13T00:56:52Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-13T00:56:52Z- handleDepth()-Depth_Keller msg: 1613177812158 epoch sec; seqNo:17166; depth 22.07 2021-02-13T00:56:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177812.17 epoch sec; seqNo:17166; 4060445.80 north; 593411.62 east 2021-02-13T00:56:52Z- AHRS_M2 msg: 1613177812.55 epoch sec; seqNo:17032 2021-02-13T00:56:52Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:56:52Z- handleDepth()-Depth_Keller msg: 1613177812563 epoch sec; seqNo:17167; depth 21.96 2021-02-13T00:56:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177812.57 epoch sec; seqNo:17167; 4060445.80 north; 593411.62 east 2021-02-13T00:56:52Z- AHRS_M2 msg: 1613177812.96 epoch sec; seqNo:17033 2021-02-13T00:56:52Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:56:52Z- handleDepth()-Depth_Keller msg: 1613177812964 epoch sec; seqNo:17168; depth 21.84 2021-02-13T00:56:52Z- handleDvl() - RDI_Pathfinder msg: 1613177812.98 epoch sec; seqNo:8299 2021-02-13T00:56:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 90.34 , 100.08 , 66.62 2021-02-13T00:56:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , 0.04 2021-02-13T00:56:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177813.00 epoch sec; seqNo:17168; 4060446.19 north; 593412.84 east 2021-02-13T00:56:53Z- AHRS_M2 msg: 1613177813.36 epoch sec; seqNo:17034 2021-02-13T00:56:53Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:56:53Z- handleDepth()-Depth_Keller msg: 1613177813366 epoch sec; seqNo:17169; depth 21.73 2021-02-13T00:56:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177813.38 epoch sec; seqNo:17169; 4060446.57 north; 593412.83 east 2021-02-13T00:56:53Z- AHRS_M2 msg: 1613177813.76 epoch sec; seqNo:17035 2021-02-13T00:56:53Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:56:53Z- handleDepth()-Depth_Keller msg: 1613177813771 epoch sec; seqNo:17170; depth 21.62 2021-02-13T00:56:53Z- handleDvl() - RDI_Pathfinder msg: 1613177813.78 epoch sec; seqNo:8300 2021-02-13T00:56:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.15 , 91.02 , 99.54 , 66.62 2021-02-13T00:56:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , 0.04 2021-02-13T00:56:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177813.79 epoch sec; seqNo:17170; 4060446.95 north; 593412.83 east 2021-02-13T00:56:54Z- AHRS_M2 msg: 1613177814.17 epoch sec; seqNo:17036 2021-02-13T00:56:54Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T00:56:54Z- handleDepth()-Depth_Keller msg: 1613177814178 epoch sec; seqNo:17171; depth 21.51 2021-02-13T00:56:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177814.19 epoch sec; seqNo:17171; 4060446.95 north; 593412.83 east 2021-02-13T00:56:54Z- AHRS_M2 msg: 1613177814.57 epoch sec; seqNo:17037 2021-02-13T00:56:54Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.18 psi 2021-02-13T00:56:54Z- handleDepth()-Depth_Keller msg: 1613177814577 epoch sec; seqNo:17172; depth 21.42 2021-02-13T00:56:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177814.59 epoch sec; seqNo:17172; 4060447.33 north; 593412.83 east 2021-02-13T00:56:54Z- AHRS_M2 msg: 1613177814.98 epoch sec; seqNo:17038 2021-02-13T00:56:54Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.18 psi 2021-02-13T00:56:54Z- handleDepth()-Depth_Keller msg: 1613177814985 epoch sec; seqNo:17173; depth 21.30 2021-02-13T00:56:54Z- handleDvl() - RDI_Pathfinder msg: 1613177815.00 epoch sec; seqNo:8301 2021-02-13T00:56:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 98.39 , 99.61 , 78.92 2021-02-13T00:56:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , 0.02 2021-02-13T00:56:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177815.01 epoch sec; seqNo:17173; 4060447.71 north; 593412.82 east 2021-02-13T00:56:55Z- AHRS_M2 msg: 1613177815.38 epoch sec; seqNo:17039 2021-02-13T00:56:55Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.18 psi 2021-02-13T00:56:55Z- handleDepth()-Depth_Keller msg: 1613177815387 epoch sec; seqNo:17174; depth 21.20 2021-02-13T00:56:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177815.40 epoch sec; seqNo:17174; 4060447.72 north; 593414.04 east 2021-02-13T00:56:55Z- AHRS_M2 msg: 1613177815.78 epoch sec; seqNo:17040 2021-02-13T00:56:55Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.18 psi 2021-02-13T00:56:55Z- handleDepth()-Depth_Keller msg: 1613177815793 epoch sec; seqNo:17175; depth 21.11 2021-02-13T00:56:55Z- handleDvl() - RDI_Pathfinder msg: 1613177815.81 epoch sec; seqNo:8302 2021-02-13T00:56:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 97.85 , 99.07 , 69.07 2021-02-13T00:56:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , 0.00 2021-02-13T00:56:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177815.82 epoch sec; seqNo:17175; 4060448.10 north; 593414.04 east 2021-02-13T00:56:56Z- AHRS_M2 msg: 1613177816.19 epoch sec; seqNo:17041 2021-02-13T00:56:56Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.18 psi 2021-02-13T00:56:56Z- handleDepth()-Depth_Keller msg: 1613177816193 epoch sec; seqNo:17176; depth 21.00 2021-02-13T00:56:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177816.20 epoch sec; seqNo:17176; 4060448.48 north; 593414.03 east 2021-02-13T00:56:56Z- AHRS_M2 msg: 1613177816.59 epoch sec; seqNo:17042 2021-02-13T00:56:56Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.18 psi 2021-02-13T00:56:56Z- handleDepth()-Depth_Keller msg: 1613177816605 epoch sec; seqNo:17177; depth 20.90 2021-02-13T00:56:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177816.65 epoch sec; seqNo:17177; 4060448.86 north; 593414.03 east 2021-02-13T00:56:57Z- AHRS_M2 msg: 1613177816.99 epoch sec; seqNo:17043 2021-02-13T00:56:57Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.18 psi 2021-02-13T00:56:57Z- handleDepth()-Depth_Keller msg: 1613177817014 epoch sec; seqNo:17178; depth 20.80 2021-02-13T00:56:57Z- handleDvl() - RDI_Pathfinder msg: 1613177817.03 epoch sec; seqNo:8303 2021-02-13T00:56:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.39 , 90.82 , 100.56 , 68.26 2021-02-13T00:56:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.10 , -0.06 2021-02-13T00:56:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177817.04 epoch sec; seqNo:17178; 4060448.86 north; 593414.03 east 2021-02-13T00:56:57Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-13T00:56:57Z- waiting for 1613177814.77, time is 1613177817.04 2021-02-13T00:56:57Z- LcmTrn::updateTrn() equating pose 1613177817.04 and meas times 1613177817.04 2021-02-13T00:56:57Z- LcmTrn::updateTrn() >>>> pose time 1613177817.04, meas time 1613177817.04 <<<< 2021-02-13T00:56:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177817.04 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:56:57Z- TerrainNav::motionUpdate() - currEstimate = (4060449, 593414.03, 20.80) 2021-02-13T00:56:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:56:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:56:57Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177817.04 sec.; No relevant navigation data available 2021-02-13T00:56:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:56:57Z- TNavBF::mmse selected from filter -1 2021-02-13T00:56:57Z- variances([0], [2], and psi_berg) = 681.563758 832.989147 0.000000 2021-02-13T00:56:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:56:57Z- LcmTrn::publishEstimates() - MLE : 4060448.36 593433.53 20.80 2021-02-13T00:56:57Z- LcmTrn::publishEstimates() - MMSE : 4060448.86 593414.03 20.80 2021-02-13T00:56:57Z- LcmTrn::publishEstimates() - COVAR: 681.56 832.99 1.14 0.00 2021-02-13T00:56:57Z- AHRS_M2 msg: 1613177817.40 epoch sec; seqNo:17044 2021-02-13T00:56:57Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.17 psi 2021-02-13T00:56:57Z- handleDepth()-Depth_Keller msg: 1613177817411 epoch sec; seqNo:17179; depth 20.71 2021-02-13T00:56:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177817.43 epoch sec; seqNo:17179; 4060449.25 north; 593415.25 east 2021-02-13T00:56:57Z- AHRS_M2 msg: 1613177817.81 epoch sec; seqNo:17045 2021-02-13T00:56:57Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-13T00:56:57Z- handleDepth()-Depth_Keller msg: 1613177817814 epoch sec; seqNo:17180; depth 20.61 2021-02-13T00:56:57Z- handleDvl() - RDI_Pathfinder msg: 1613177817.83 epoch sec; seqNo:8304 2021-02-13T00:56:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 91.02 , 99.54 , 78.85 2021-02-13T00:56:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.07 , -0.08 2021-02-13T00:56:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177817.85 epoch sec; seqNo:17180; 4060449.63 north; 593415.24 east 2021-02-13T00:56:58Z- AHRS_M2 msg: 1613177818.21 epoch sec; seqNo:17046 2021-02-13T00:56:58Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.16 psi 2021-02-13T00:56:58Z- handleDepth()-Depth_Keller msg: 1613177818229 epoch sec; seqNo:17181; depth 20.50 2021-02-13T00:56:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177818.24 epoch sec; seqNo:17181; 4060450.01 north; 593415.24 east 2021-02-13T00:56:58Z- AHRS_M2 msg: 1613177818.61 epoch sec; seqNo:17047 2021-02-13T00:56:58Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.16 psi 2021-02-13T00:56:58Z- handleDepth()-Depth_Keller msg: 1613177818621 epoch sec; seqNo:17182; depth 20.41 2021-02-13T00:56:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177818.63 epoch sec; seqNo:17182; 4060450.01 north; 593415.24 east 2021-02-13T00:56:59Z- AHRS_M2 msg: 1613177819.02 epoch sec; seqNo:17048 2021-02-13T00:56:59Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.16 psi 2021-02-13T00:56:59Z- handleDepth()-Depth_Keller msg: 1613177819025 epoch sec; seqNo:17183; depth 20.31 2021-02-13T00:56:59Z- handleDvl() - RDI_Pathfinder msg: 1613177819.04 epoch sec; seqNo:8305 2021-02-13T00:56:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 97.78 , 100.22 , 68.67 2021-02-13T00:56:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , -0.11 2021-02-13T00:56:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177819.06 epoch sec; seqNo:17183; 4060450.39 north; 593415.23 east 2021-02-13T00:56:59Z- AHRS_M2 msg: 1613177819.42 epoch sec; seqNo:17049 2021-02-13T00:56:59Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.16 psi 2021-02-13T00:56:59Z- handleDepth()-Depth_Keller msg: 1613177819425 epoch sec; seqNo:17184; depth 20.20 2021-02-13T00:56:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177819.44 epoch sec; seqNo:17184; 4060450.77 north; 593415.23 east 2021-02-13T00:56:59Z- AHRS_M2 msg: 1613177819.82 epoch sec; seqNo:17050 2021-02-13T00:56:59Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.16 psi 2021-02-13T00:56:59Z- handleDepth()-Depth_Keller msg: 1613177819829 epoch sec; seqNo:17185; depth 20.11 2021-02-13T00:56:59Z- handleDvl() - RDI_Pathfinder msg: 1613177819.85 epoch sec; seqNo:8306 2021-02-13T00:56:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.71 , 98.19 , 100.62 , 68.67 2021-02-13T00:56:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , -0.11 2021-02-13T00:56:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177819.86 epoch sec; seqNo:17185; 4060451.16 north; 593416.45 east 2021-02-13T00:57:00Z- AHRS_M2 msg: 1613177820.23 epoch sec; seqNo:17051 2021-02-13T00:57:00Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.16 psi 2021-02-13T00:57:00Z- handleDepth()-Depth_Keller msg: 1613177820233 epoch sec; seqNo:17186; depth 20.00 2021-02-13T00:57:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177820.25 epoch sec; seqNo:17186; 4060451.16 north; 593416.45 east 2021-02-13T00:57:00Z- AHRS_M2 msg: 1613177820.63 epoch sec; seqNo:17052 2021-02-13T00:57:00Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-13T00:57:00Z- handleDepth()-Depth_Keller msg: 1613177820645 epoch sec; seqNo:17187; depth 19.90 2021-02-13T00:57:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177820.66 epoch sec; seqNo:17187; 4060451.54 north; 593416.44 east 2021-02-13T00:57:01Z- AHRS_M2 msg: 1613177821.04 epoch sec; seqNo:17053 2021-02-13T00:57:01Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.16 psi 2021-02-13T00:57:01Z- handleDepth()-Depth_Keller msg: 1613177821051 epoch sec; seqNo:17188; depth 19.80 2021-02-13T00:57:01Z- handleDvl() - RDI_Pathfinder msg: 1613177821.06 epoch sec; seqNo:8307 2021-02-13T00:57:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.05 , 101.71 , 69.89 2021-02-13T00:57:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.11 , -0.06 2021-02-13T00:57:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177821.07 epoch sec; seqNo:17188; 4060451.92 north; 593416.44 east 2021-02-13T00:57:01Z- AHRS_M2 msg: 1613177821.44 epoch sec; seqNo:17054 2021-02-13T00:57:01Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-13T00:57:01Z- handleDepth()-Depth_Keller msg: 1613177821445 epoch sec; seqNo:17189; depth 19.68 2021-02-13T00:57:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177821.46 epoch sec; seqNo:17189; 4060452.30 north; 593416.43 east 2021-02-13T00:57:01Z- AHRS_M2 msg: 1613177821.84 epoch sec; seqNo:17055 2021-02-13T00:57:01Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.15 psi 2021-02-13T00:57:01Z- handleDepth()-Depth_Keller msg: 1613177821853 epoch sec; seqNo:17190; depth 19.58 2021-02-13T00:57:01Z- handleDvl() - RDI_Pathfinder msg: 1613177821.87 epoch sec; seqNo:8308 2021-02-13T00:57:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 97.78 , 101.43 , 69.89 2021-02-13T00:57:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , -0.02 2021-02-13T00:57:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177821.88 epoch sec; seqNo:17190; 4060452.30 north; 593416.43 east 2021-02-13T00:57:02Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-13T00:57:02Z- waiting for 1613177820.04, time is 1613177821.88 2021-02-13T00:57:02Z- LcmTrn::updateTrn() equating pose 1613177821.88 and meas times 1613177821.88 2021-02-13T00:57:02Z- LcmTrn::updateTrn() >>>> pose time 1613177821.88, meas time 1613177821.88 <<<< 2021-02-13T00:57:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177821.88 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:57:02Z- TerrainNav::motionUpdate() - currEstimate = (4060452, 593416.43, 19.58) 2021-02-13T00:57:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:57:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:57:02Z- TNavBF::Projecting Measurements 2021-02-13T00:57:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:57:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:57:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:57:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:57:02Z- TNavFilter::4 good beams. 2021-02-13T00:57:02Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:57:02Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:57:02Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:57:02Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:57:02Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:57:02Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:57:02Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:57:02Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:02Z- TerrainMapOctree: (vehN, vehE ) = (4060452.30, 593416.43). 2021-02-13T00:57:02Z- TerrainMapOctree: Min Distance = 1153.76. 2021-02-13T00:57:02Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:57:02Z- There is no specific good beam that all particles have in common. 2021-02-13T00:57:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:57:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.35 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 9.81 2021-02-13T00:57:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:57:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-13T00:57:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:57:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 16.46 2021-02-13T00:57:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:57:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 7.82 2021-02-13T00:57:02Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:57:02Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:57:02Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:57:02Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:57:02Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:57:02Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:57:02Z- Filter 0 No beams used in subcloud update 2021-02-13T00:57:02Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:57:02Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:57:02Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 681.5 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:57:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177821.88 sec. 2021-02-13T00:57:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:57:02Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:02Z- variances([0], [2], and psi_berg) = 681.518087 832.989128 0.000000 2021-02-13T00:57:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:57:02Z- LcmTrn::publishEstimates() - MLE : 4060451.80 593436.93 19.58 2021-02-13T00:57:02Z- LcmTrn::publishEstimates() - MMSE : 4060452.30 593416.43 19.58 2021-02-13T00:57:02Z- LcmTrn::publishEstimates() - COVAR: 681.52 832.99 0.96 0.00 2021-02-13T00:57:02Z- AHRS_M2 msg: 1613177822.25 epoch sec; seqNo:17056 2021-02-13T00:57:02Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.15 psi 2021-02-13T00:57:02Z- handleDepth()-Depth_Keller msg: 1613177822317 epoch sec; seqNo:17191; depth 19.46 2021-02-13T00:57:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177822.33 epoch sec; seqNo:17191; 4060452.67 north; 593416.43 east 2021-02-13T00:57:02Z- AHRS_M2 msg: 1613177822.65 epoch sec; seqNo:17057 2021-02-13T00:57:02Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T00:57:02Z- handleDepth()-Depth_Keller msg: 1613177822666 epoch sec; seqNo:17192; depth 19.35 2021-02-13T00:57:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177822.67 epoch sec; seqNo:17192; 4060453.05 north; 593416.43 east 2021-02-13T00:57:03Z- AHRS_M2 msg: 1613177823.06 epoch sec; seqNo:17058 2021-02-13T00:57:03Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T00:57:03Z- handleDepth()-Depth_Keller msg: 1613177823081 epoch sec; seqNo:17193; depth 19.25 2021-02-13T00:57:03Z- handleDvl() - RDI_Pathfinder msg: 1613177823.10 epoch sec; seqNo:8309 2021-02-13T00:57:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 101.71 , 0.00 2021-02-13T00:57:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.01 2021-02-13T00:57:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177823.11 epoch sec; seqNo:17193; 4060453.45 north; 593417.64 east 2021-02-13T00:57:03Z- AHRS_M2 msg: 1613177823.46 epoch sec; seqNo:17059 2021-02-13T00:57:03Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T00:57:03Z- handleDepth()-Depth_Keller msg: 1613177823557 epoch sec; seqNo:17194; depth 19.14 2021-02-13T00:57:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177823.57 epoch sec; seqNo:17194; 4060453.82 north; 593417.64 east 2021-02-13T00:57:03Z- AHRS_M2 msg: 1613177823.86 epoch sec; seqNo:17060 2021-02-13T00:57:03Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T00:57:03Z- handleDepth()-Depth_Keller msg: 1613177823876 epoch sec; seqNo:17195; depth 19.02 2021-02-13T00:57:03Z- handleDvl() - RDI_Pathfinder msg: 1613177823.91 epoch sec; seqNo:8310 2021-02-13T00:57:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.84 , 90.89 , 103.06 , 68.67 2021-02-13T00:57:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , 0.03 2021-02-13T00:57:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177823.92 epoch sec; seqNo:17195; 4060453.82 north; 593417.64 east 2021-02-13T00:57:04Z- AHRS_M2 msg: 1613177824.27 epoch sec; seqNo:17061 2021-02-13T00:57:04Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T00:57:04Z- handleDepth()-Depth_Keller msg: 1613177824293 epoch sec; seqNo:17196; depth 18.92 2021-02-13T00:57:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177824.31 epoch sec; seqNo:17196; 4060454.20 north; 593417.63 east 2021-02-13T00:57:04Z- AHRS_M2 msg: 1613177824.68 epoch sec; seqNo:17062 2021-02-13T00:57:04Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T00:57:04Z- handleDepth()-Depth_Keller msg: 1613177824713 epoch sec; seqNo:17197; depth 18.82 2021-02-13T00:57:04Z- handleDvl() - RDI_Pathfinder msg: 1613177824.73 epoch sec; seqNo:8311 2021-02-13T00:57:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.71 , 98.18 , 101.83 , 70.29 2021-02-13T00:57:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , 0.03 2021-02-13T00:57:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177824.74 epoch sec; seqNo:17197; 4060454.58 north; 593417.63 east 2021-02-13T00:57:05Z- AHRS_M2 msg: 1613177825.08 epoch sec; seqNo:17063 2021-02-13T00:57:05Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T00:57:05Z- handleDepth()-Depth_Keller msg: 1613177825090 epoch sec; seqNo:17198; depth 18.71 2021-02-13T00:57:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177825.10 epoch sec; seqNo:17198; 4060454.58 north; 593417.63 east 2021-02-13T00:57:05Z- AHRS_M2 msg: 1613177825.48 epoch sec; seqNo:17064 2021-02-13T00:57:05Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T00:57:05Z- handleDepth()-Depth_Keller msg: 1613177825484 epoch sec; seqNo:17199; depth 18.62 2021-02-13T00:57:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177825.49 epoch sec; seqNo:17199; 4060454.97 north; 593418.85 east 2021-02-13T00:57:05Z- AHRS_M2 msg: 1613177825.88 epoch sec; seqNo:17065 2021-02-13T00:57:05Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T00:57:05Z- handleDepth()-Depth_Keller msg: 1613177825889 epoch sec; seqNo:17200; depth 18.50 2021-02-13T00:57:05Z- handleDvl() - RDI_Pathfinder msg: 1613177825.90 epoch sec; seqNo:8312 2021-02-13T00:57:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 91.90 , 102.85 , 78.51 2021-02-13T00:57:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , 0.03 2021-02-13T00:57:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177825.91 epoch sec; seqNo:17200; 4060455.35 north; 593418.84 east 2021-02-13T00:57:06Z- AHRS_M2 msg: 1613177826.29 epoch sec; seqNo:17066 2021-02-13T00:57:06Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T00:57:06Z- handleDepth()-Depth_Keller msg: 1613177826305 epoch sec; seqNo:17201; depth 18.40 2021-02-13T00:57:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177826.32 epoch sec; seqNo:17201; 4060455.73 north; 593418.84 east 2021-02-13T00:57:06Z- AHRS_M2 msg: 1613177826.70 epoch sec; seqNo:17067 2021-02-13T00:57:06Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.16 psi 2021-02-13T00:57:06Z- handleDepth()-Depth_Keller msg: 1613177826710 epoch sec; seqNo:17202; depth 18.28 2021-02-13T00:57:06Z- handleDvl() - RDI_Pathfinder msg: 1613177826.72 epoch sec; seqNo:8313 2021-02-13T00:57:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.92 , 104.00 , 70.30 2021-02-13T00:57:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , 0.02 2021-02-13T00:57:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177826.73 epoch sec; seqNo:17202; 4060455.73 north; 593418.84 east 2021-02-13T00:57:07Z- AHRS_M2 msg: 1613177827.10 epoch sec; seqNo:17068 2021-02-13T00:57:07Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.16 psi 2021-02-13T00:57:07Z- handleDepth()-Depth_Keller msg: 1613177827100 epoch sec; seqNo:17203; depth 18.19 2021-02-13T00:57:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177827.11 epoch sec; seqNo:17203; 4060456.11 north; 593418.83 east 2021-02-13T00:57:07Z- LcmTrn::handleMessages() - handling 44 messages took 0.89 ms and 126 calls... 2021-02-13T00:57:07Z- waiting for 1613177824.88, time is 1613177827.11 2021-02-13T00:57:07Z- LcmTrn::updateTrn() >>>> pose time 1613177827.11, meas time 1613177826.72 <<<< 2021-02-13T00:57:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177826.72 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:57:07Z- TerrainNav::motionUpdate() - currEstimate = (4060456, 593418.83, 18.19) 2021-02-13T00:57:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:57:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:57:07Z- TNavBF::Projecting Measurements 2021-02-13T00:57:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:57:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:57:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:57:07Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:57:07Z- TNavFilter::3 good beams. 2021-02-13T00:57:07Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:57:07Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:57:07Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:57:07Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:57:07Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:57:07Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:57:07Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:57:07Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:07Z- TerrainMapOctree: (vehN, vehE ) = (4060455.83, 593418.66). 2021-02-13T00:57:07Z- TerrainMapOctree: Min Distance = 1149.71. 2021-02-13T00:57:07Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:57:07Z- There is no specific good beam that all particles have in common. 2021-02-13T00:57:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:57:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:57:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:57:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 17.31 2021-02-13T00:57:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:57:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 7.91 2021-02-13T00:57:07Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:57:07Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:57:07Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:57:07Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:57:07Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:57:07Z- Filter 0 No beams used in subcloud update 2021-02-13T00:57:07Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:57:07Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:57:07Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 681.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:57:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177826.72 sec. 2021-02-13T00:57:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:57:07Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:07Z- variances([0], [2], and psi_berg) = 681.459654 832.989103 0.000000 2021-02-13T00:57:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:57:07Z- LcmTrn::publishEstimates() - MLE : 4060455.61 593439.33 18.19 2021-02-13T00:57:07Z- LcmTrn::publishEstimates() - MMSE : 4060456.11 593418.83 18.19 2021-02-13T00:57:07Z- LcmTrn::publishEstimates() - COVAR: 681.46 832.99 0.95 0.00 2021-02-13T00:57:07Z- AHRS_M2 msg: 1613177827.50 epoch sec; seqNo:17069 2021-02-13T00:57:07Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-13T00:57:07Z- handleDepth()-Depth_Keller msg: 1613177827504 epoch sec; seqNo:17204; depth 18.09 2021-02-13T00:57:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177827.52 epoch sec; seqNo:17204; 4060456.49 north; 593418.83 east 2021-02-13T00:57:07Z- AHRS_M2 msg: 1613177827.90 epoch sec; seqNo:17070 2021-02-13T00:57:07Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-13T00:57:07Z- handleDepth()-Depth_Keller msg: 1613177827908 epoch sec; seqNo:17205; depth 17.98 2021-02-13T00:57:07Z- handleDvl() - RDI_Pathfinder msg: 1613177827.92 epoch sec; seqNo:8314 2021-02-13T00:57:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.46 , 97.71 , 102.58 , 78.24 2021-02-13T00:57:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.09 , -0.02 2021-02-13T00:57:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177827.94 epoch sec; seqNo:17205; 4060456.50 north; 593420.05 east 2021-02-13T00:57:08Z- AHRS_M2 msg: 1613177828.31 epoch sec; seqNo:17071 2021-02-13T00:57:08Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-13T00:57:08Z- handleDepth()-Depth_Keller msg: 1613177828315 epoch sec; seqNo:17206; depth 17.87 2021-02-13T00:57:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177828.33 epoch sec; seqNo:17206; 4060456.88 north; 593420.05 east 2021-02-13T00:57:08Z- AHRS_M2 msg: 1613177828.71 epoch sec; seqNo:17072 2021-02-13T00:57:08Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T00:57:08Z- handleDepth()-Depth_Keller msg: 1613177828721 epoch sec; seqNo:17207; depth 17.77 2021-02-13T00:57:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177828.73 epoch sec; seqNo:17207; 4060457.26 north; 593420.04 east 2021-02-13T00:57:09Z- AHRS_M2 msg: 1613177829.12 epoch sec; seqNo:17073 2021-02-13T00:57:09Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T00:57:09Z- handleDepth()-Depth_Keller msg: 1613177829124 epoch sec; seqNo:17208; depth 17.65 2021-02-13T00:57:09Z- handleDvl() - RDI_Pathfinder msg: 1613177829.15 epoch sec; seqNo:8315 2021-02-13T00:57:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 92.92 , 105.09 , 78.31 2021-02-13T00:57:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , -0.07 2021-02-13T00:57:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177829.16 epoch sec; seqNo:17208; 4060457.26 north; 593420.04 east 2021-02-13T00:57:09Z- AHRS_M2 msg: 1613177829.52 epoch sec; seqNo:17074 2021-02-13T00:57:09Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-13T00:57:09Z- handleDepth()-Depth_Keller msg: 1613177829524 epoch sec; seqNo:17209; depth 17.53 2021-02-13T00:57:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177829.54 epoch sec; seqNo:17209; 4060457.64 north; 593420.04 east 2021-02-13T00:57:09Z- AHRS_M2 msg: 1613177829.92 epoch sec; seqNo:17075 2021-02-13T00:57:09Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-13T00:57:09Z- handleDepth()-Depth_Keller msg: 1613177829929 epoch sec; seqNo:17210; depth 17.43 2021-02-13T00:57:09Z- handleDvl() - RDI_Pathfinder msg: 1613177829.94 epoch sec; seqNo:8316 2021-02-13T00:57:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.92 , 104.00 , 70.30 2021-02-13T00:57:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.03 , -0.06 2021-02-13T00:57:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177829.95 epoch sec; seqNo:17210; 4060458.02 north; 593420.03 east 2021-02-13T00:57:10Z- AHRS_M2 msg: 1613177830.34 epoch sec; seqNo:17076 2021-02-13T00:57:10Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-13T00:57:10Z- handleDepth()-Depth_Keller msg: 1613177830340 epoch sec; seqNo:17211; depth 17.32 2021-02-13T00:57:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177830.35 epoch sec; seqNo:17211; 4060458.41 north; 593421.25 east 2021-02-13T00:57:10Z- AHRS_M2 msg: 1613177830.73 epoch sec; seqNo:17077 2021-02-13T00:57:10Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-13T00:57:10Z- handleDepth()-Depth_Keller msg: 1613177830737 epoch sec; seqNo:17212; depth 17.18 2021-02-13T00:57:10Z- handleDvl() - RDI_Pathfinder msg: 1613177830.75 epoch sec; seqNo:8317 2021-02-13T00:57:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 95.28 , 105.01 , 72.34 2021-02-13T00:57:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.03 , 0.01 2021-02-13T00:57:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177830.77 epoch sec; seqNo:17212; 4060458.79 north; 593421.25 east 2021-02-13T00:57:11Z- AHRS_M2 msg: 1613177831.14 epoch sec; seqNo:17078 2021-02-13T00:57:11Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.18 psi 2021-02-13T00:57:11Z- handleDepth()-Depth_Keller msg: 1613177831145 epoch sec; seqNo:17213; depth 17.09 2021-02-13T00:57:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:57:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:57:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:57:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:57:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:57:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:57:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:57:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:57:11Z- handleDvl() - RDI_Pathfinder msg: 1613177831.15 epoch sec; seqNo:8318 2021-02-13T00:57:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:57:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.03 , 0.01 2021-02-13T00:57:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177831.16 epoch sec; seqNo:17213; 4060458.79 north; 593421.25 east 2021-02-13T00:57:11Z- AHRS_M2 msg: 1613177831.54 epoch sec; seqNo:17079 2021-02-13T00:57:11Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.17 psi 2021-02-13T00:57:11Z- handleDepth()-Depth_Keller msg: 1613177831546 epoch sec; seqNo:17214; depth 16.99 2021-02-13T00:57:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177831.56 epoch sec; seqNo:17214; 4060459.17 north; 593421.24 east 2021-02-13T00:57:11Z- AHRS_M2 msg: 1613177831.94 epoch sec; seqNo:17080 2021-02-13T00:57:11Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.17 psi 2021-02-13T00:57:11Z- handleDepth()-Depth_Keller msg: 1613177831949 epoch sec; seqNo:17215; depth 16.86 2021-02-13T00:57:11Z- handleDvl() - RDI_Pathfinder msg: 1613177831.96 epoch sec; seqNo:8319 2021-02-13T00:57:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 94.06 , 108.66 , 70.29 2021-02-13T00:57:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.02 , 0.03 2021-02-13T00:57:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177831.98 epoch sec; seqNo:17215; 4060459.55 north; 593421.24 east 2021-02-13T00:57:12Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-13T00:57:12Z- waiting for 1613177830.11, time is 1613177831.98 2021-02-13T00:57:12Z- LcmTrn::updateTrn() equating pose 1613177831.98 and meas times 1613177831.98 2021-02-13T00:57:12Z- LcmTrn::updateTrn() >>>> pose time 1613177831.98, meas time 1613177831.98 <<<< 2021-02-13T00:57:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177831.98 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:57:12Z- TerrainNav::motionUpdate() - currEstimate = (4060460, 593421.24, 16.86) 2021-02-13T00:57:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:57:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:57:12Z- TNavBF::Projecting Measurements 2021-02-13T00:57:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:57:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:57:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:57:12Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:57:12Z- TNavFilter::3 good beams. 2021-02-13T00:57:12Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:57:12Z- TNavBF:: beamIndex[0] = 1 2021-02-13T00:57:12Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:57:12Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:57:12Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:57:12Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:57:12Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:57:12Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:12Z- TerrainMapOctree: (vehN, vehE ) = (4060459.55, 593421.24). 2021-02-13T00:57:12Z- TerrainMapOctree: Min Distance = 1145.28. 2021-02-13T00:57:12Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:57:12Z- There is no specific good beam that all particles have in common. 2021-02-13T00:57:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:57:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 14.16 2021-02-13T00:57:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:57:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 18.89 2021-02-13T00:57:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:57:12Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 7.91 2021-02-13T00:57:12Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:57:12Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:57:12Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:57:12Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:57:12Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:57:12Z- Filter 0 No beams used in subcloud update 2021-02-13T00:57:12Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:57:12Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:57:12Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 681.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:57:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177831.98 sec. 2021-02-13T00:57:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:57:12Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:12Z- variances([0], [2], and psi_berg) = 681.414032 832.989083 0.000000 2021-02-13T00:57:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:57:12Z- LcmTrn::publishEstimates() - MLE : 4060459.05 593441.74 16.86 2021-02-13T00:57:12Z- LcmTrn::publishEstimates() - MMSE : 4060459.55 593421.24 16.86 2021-02-13T00:57:12Z- LcmTrn::publishEstimates() - COVAR: 681.41 832.99 0.93 0.00 2021-02-13T00:57:12Z- AHRS_M2 msg: 1613177832.35 epoch sec; seqNo:17081 2021-02-13T00:57:12Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.16 psi 2021-02-13T00:57:12Z- handleDepth()-Depth_Keller msg: 1613177832355 epoch sec; seqNo:17216; depth 16.76 2021-02-13T00:57:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:57:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:57:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:57:12Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:57:12Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:57:12Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:57:12Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:57:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:57:12Z- handleDvl() - RDI_Pathfinder msg: 1613177832.36 epoch sec; seqNo:8320 2021-02-13T00:57:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:57:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.02 , 0.03 2021-02-13T00:57:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177832.37 epoch sec; seqNo:17216; 4060459.93 north; 593421.23 east 2021-02-13T00:57:12Z- AHRS_M2 msg: 1613177832.75 epoch sec; seqNo:17082 2021-02-13T00:57:12Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.15 psi 2021-02-13T00:57:12Z- handleDepth()-Depth_Keller msg: 1613177832762 epoch sec; seqNo:17217; depth 16.64 2021-02-13T00:57:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177832.77 epoch sec; seqNo:17217; 4060460.32 north; 593422.45 east 2021-02-13T00:57:13Z- AHRS_M2 msg: 1613177833.16 epoch sec; seqNo:17083 2021-02-13T00:57:13Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T00:57:13Z- handleDepth()-Depth_Keller msg: 1613177833162 epoch sec; seqNo:17218; depth 16.54 2021-02-13T00:57:13Z- handleDvl() - RDI_Pathfinder msg: 1613177833.17 epoch sec; seqNo:8321 2021-02-13T00:57:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 97.65 , 107.39 , 78.17 2021-02-13T00:57:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.08 , -0.02 2021-02-13T00:57:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177833.22 epoch sec; seqNo:17218; 4060460.70 north; 593422.45 east 2021-02-13T00:57:13Z- handleDepth()-Depth_Keller msg: 1613177833581 epoch sec; seqNo:17219; depth 16.42 2021-02-13T00:57:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:57:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:57:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:57:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:57:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:57:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:57:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:57:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:57:13Z- handleDvl() - RDI_Pathfinder msg: 1613177833.58 epoch sec; seqNo:8322 2021-02-13T00:57:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:57:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.08 , -0.02 2021-02-13T00:57:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177833.61 epoch sec; seqNo:17219; 4060460.70 north; 593422.45 east 2021-02-13T00:57:13Z- AHRS_M2 msg: 1613177833.96 epoch sec; seqNo:17084 2021-02-13T00:57:13Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.14 psi 2021-02-13T00:57:13Z- handleDepth()-Depth_Keller msg: 1613177833972 epoch sec; seqNo:17220; depth 16.32 2021-02-13T00:57:13Z- handleDvl() - RDI_Pathfinder msg: 1613177833.98 epoch sec; seqNo:8323 2021-02-13T00:57:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 97.71 , 111.10 , 78.24 2021-02-13T00:57:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.10 , -0.11 2021-02-13T00:57:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177834.00 epoch sec; seqNo:17220; 4060461.08 north; 593422.44 east 2021-02-13T00:57:14Z- AHRS_M2 msg: 1613177834.37 epoch sec; seqNo:17085 2021-02-13T00:57:14Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-13T00:57:14Z- handleDepth()-Depth_Keller msg: 1613177834391 epoch sec; seqNo:17221; depth 16.22 2021-02-13T00:57:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177834.40 epoch sec; seqNo:17221; 4060461.08 north; 593422.44 east 2021-02-13T00:57:14Z- AHRS_M2 msg: 1613177834.78 epoch sec; seqNo:17086 2021-02-13T00:57:14Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-13T00:57:14Z- handleDepth()-Depth_Keller msg: 1613177834788 epoch sec; seqNo:17222; depth 16.09 2021-02-13T00:57:14Z- handleDvl() - RDI_Pathfinder msg: 1613177834.81 epoch sec; seqNo:8324 2021-02-13T00:57:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.88 , 97.71 , 107.18 , 78.78 2021-02-13T00:57:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , -0.16 2021-02-13T00:57:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177834.82 epoch sec; seqNo:17222; 4060461.45 north; 593422.44 east 2021-02-13T00:57:15Z- AHRS_M2 msg: 1613177835.18 epoch sec; seqNo:17087 2021-02-13T00:57:15Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T00:57:15Z- handleDepth()-Depth_Keller msg: 1613177835188 epoch sec; seqNo:17223; depth 15.96 2021-02-13T00:57:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177835.20 epoch sec; seqNo:17223; 4060461.85 north; 593423.65 east 2021-02-13T00:57:15Z- AHRS_M2 msg: 1613177835.58 epoch sec; seqNo:17088 2021-02-13T00:57:15Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-13T00:57:15Z- handleDepth()-Depth_Keller msg: 1613177835588 epoch sec; seqNo:17224; depth 15.85 2021-02-13T00:57:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177835.60 epoch sec; seqNo:17224; 4060461.85 north; 593423.65 east 2021-02-13T00:57:15Z- AHRS_M2 msg: 1613177835.98 epoch sec; seqNo:17089 2021-02-13T00:57:15Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-13T00:57:16Z- handleDepth()-Depth_Keller msg: 1613177836057 epoch sec; seqNo:17225; depth 15.73 2021-02-13T00:57:16Z- handleDvl() - RDI_Pathfinder msg: 1613177836.07 epoch sec; seqNo:8325 2021-02-13T00:57:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 97.71 , 108.67 , 78.24 2021-02-13T00:57:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.11 , -0.09 2021-02-13T00:57:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177836.08 epoch sec; seqNo:17225; 4060462.23 north; 593423.65 east 2021-02-13T00:57:16Z- AHRS_M2 msg: 1613177836.39 epoch sec; seqNo:17090 2021-02-13T00:57:16Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-13T00:57:16Z- handleDepth()-Depth_Keller msg: 1613177836400 epoch sec; seqNo:17226; depth 15.61 2021-02-13T00:57:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177836.43 epoch sec; seqNo:17226; 4060462.60 north; 593423.65 east 2021-02-13T00:57:16Z- AHRS_M2 msg: 1613177836.79 epoch sec; seqNo:17091 2021-02-13T00:57:16Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.17 psi 2021-02-13T00:57:16Z- handleDepth()-Depth_Keller msg: 1613177836878 epoch sec; seqNo:17227; depth 15.51 2021-02-13T00:57:16Z- handleDvl() - RDI_Pathfinder msg: 1613177836.89 epoch sec; seqNo:8326 2021-02-13T00:57:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.70 , 97.65 , 109.82 , 71.12 2021-02-13T00:57:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.12 , 0.05 2021-02-13T00:57:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177836.90 epoch sec; seqNo:17227; 4060462.98 north; 593423.64 east 2021-02-13T00:57:17Z- AHRS_M2 msg: 1613177837.20 epoch sec; seqNo:17092 2021-02-13T00:57:17Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.16 psi 2021-02-13T00:57:17Z- handleDepth()-Depth_Keller msg: 1613177837229 epoch sec; seqNo:17228; depth 15.38 2021-02-13T00:57:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177837.24 epoch sec; seqNo:17228; 4060463.36 north; 593423.64 east 2021-02-13T00:57:17Z- LcmTrn::handleMessages() - handling 45 messages took 0.73 ms and 131 calls... 2021-02-13T00:57:17Z- waiting for 1613177834.98, time is 1613177837.24 2021-02-13T00:57:17Z- LcmTrn::updateTrn() >>>> pose time 1613177837.24, meas time 1613177836.89 <<<< 2021-02-13T00:57:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177836.89 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:57:17Z- TerrainNav::motionUpdate() - currEstimate = (4060463, 593423.64, 15.38) 2021-02-13T00:57:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:57:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:57:17Z- TNavBF::Projecting Measurements 2021-02-13T00:57:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:57:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:57:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:57:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:57:17Z- TNavFilter::4 good beams. 2021-02-13T00:57:17Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:57:17Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:57:17Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:57:17Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:57:17Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:57:17Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:57:17Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:57:17Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:17Z- TerrainMapOctree: (vehN, vehE ) = (4060463.11, 593423.48). 2021-02-13T00:57:17Z- TerrainMapOctree: Min Distance = 1141.20. 2021-02-13T00:57:17Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:57:17Z- There is no specific good beam that all particles have in common. 2021-02-13T00:57:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:57:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 10.16 2021-02-13T00:57:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:57:17Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 15.26 2021-02-13T00:57:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:57:17Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 19.30 2021-02-13T00:57:17Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:57:17Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-13T00:57:17Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:57:17Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:57:17Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:57:17Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:57:17Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:57:17Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:57:17Z- Filter 0 No beams used in subcloud update 2021-02-13T00:57:17Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:57:17Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:57:17Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 681.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:57:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177836.89 sec. 2021-02-13T00:57:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:57:17Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:17Z- variances([0], [2], and psi_berg) = 681.356154 832.989058 0.000000 2021-02-13T00:57:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:57:17Z- LcmTrn::publishEstimates() - MLE : 4060462.86 593444.14 15.38 2021-02-13T00:57:17Z- LcmTrn::publishEstimates() - MMSE : 4060463.36 593423.64 15.38 2021-02-13T00:57:17Z- LcmTrn::publishEstimates() - COVAR: 681.36 832.99 0.87 0.00 2021-02-13T00:57:17Z- AHRS_M2 msg: 1613177837.60 epoch sec; seqNo:17093 2021-02-13T00:57:17Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.15 psi 2021-02-13T00:57:17Z- handleDepth()-Depth_Keller msg: 1613177837625 epoch sec; seqNo:17229; depth 15.27 2021-02-13T00:57:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177837.63 epoch sec; seqNo:17229; 4060463.36 north; 593423.64 east 2021-02-13T00:57:18Z- AHRS_M2 msg: 1613177838.00 epoch sec; seqNo:17094 2021-02-13T00:57:18Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.15 psi 2021-02-13T00:57:18Z- handleDepth()-Depth_Keller msg: 1613177838034 epoch sec; seqNo:17230; depth 15.16 2021-02-13T00:57:18Z- handleDvl() - RDI_Pathfinder msg: 1613177838.05 epoch sec; seqNo:8327 2021-02-13T00:57:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 95.35 , 105.09 , 78.31 2021-02-13T00:57:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , 0.13 2021-02-13T00:57:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177838.06 epoch sec; seqNo:17230; 4060463.75 north; 593424.85 east 2021-02-13T00:57:18Z- AHRS_M2 msg: 1613177838.41 epoch sec; seqNo:17095 2021-02-13T00:57:18Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.15 psi 2021-02-13T00:57:18Z- handleDepth()-Depth_Keller msg: 1613177838414 epoch sec; seqNo:17231; depth 15.04 2021-02-13T00:57:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177838.43 epoch sec; seqNo:17231; 4060464.13 north; 593424.85 east 2021-02-13T00:57:18Z- AHRS_M2 msg: 1613177838.81 epoch sec; seqNo:17096 2021-02-13T00:57:18Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.14 psi 2021-02-13T00:57:18Z- handleDepth()-Depth_Keller msg: 1613177838819 epoch sec; seqNo:17232; depth 14.97 2021-02-13T00:57:18Z- handleDvl() - RDI_Pathfinder msg: 1613177838.83 epoch sec; seqNo:8328 2021-02-13T00:57:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.78 , 108.74 , 78.31 2021-02-13T00:57:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , 0.11 2021-02-13T00:57:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177838.84 epoch sec; seqNo:17232; 4060464.51 north; 593424.85 east 2021-02-13T00:57:19Z- AHRS_M2 msg: 1613177839.22 epoch sec; seqNo:17097 2021-02-13T00:57:19Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.14 psi 2021-02-13T00:57:19Z- handleDepth()-Depth_Keller msg: 1613177839221 epoch sec; seqNo:17233; depth 14.83 2021-02-13T00:57:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177839.23 epoch sec; seqNo:17233; 4060464.89 north; 593424.84 east 2021-02-13T00:57:19Z- AHRS_M2 msg: 1613177839.62 epoch sec; seqNo:17098 2021-02-13T00:57:19Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.14 psi 2021-02-13T00:57:19Z- handleDepth()-Depth_Keller msg: 1613177839629 epoch sec; seqNo:17234; depth 14.73 2021-02-13T00:57:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177839.64 epoch sec; seqNo:17234; 4060464.89 north; 593424.84 east 2021-02-13T00:57:20Z- AHRS_M2 msg: 1613177840.03 epoch sec; seqNo:17099 2021-02-13T00:57:20Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.14 psi 2021-02-13T00:57:20Z- handleDepth()-Depth_Keller msg: 1613177840030 epoch sec; seqNo:17235; depth 14.62 2021-02-13T00:57:20Z- handleDvl() - RDI_Pathfinder msg: 1613177840.04 epoch sec; seqNo:8329 2021-02-13T00:57:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 94.74 , 108.26 , 78.51 2021-02-13T00:57:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.14 , 0.01 2021-02-13T00:57:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177840.05 epoch sec; seqNo:17235; 4060465.27 north; 593424.84 east 2021-02-13T00:57:20Z- AHRS_M2 msg: 1613177840.43 epoch sec; seqNo:17100 2021-02-13T00:57:20Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-13T00:57:20Z- handleDepth()-Depth_Keller msg: 1613177840452 epoch sec; seqNo:17236; depth 14.49 2021-02-13T00:57:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177840.46 epoch sec; seqNo:17236; 4060465.66 north; 593426.05 east 2021-02-13T00:57:20Z- AHRS_M2 msg: 1613177840.83 epoch sec; seqNo:17101 2021-02-13T00:57:20Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-13T00:57:20Z- handleDepth()-Depth_Keller msg: 1613177840841 epoch sec; seqNo:17237; depth 14.38 2021-02-13T00:57:20Z- handleDvl() - RDI_Pathfinder msg: 1613177840.85 epoch sec; seqNo:8330 2021-02-13T00:57:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.70 , 97.58 , 109.75 , 78.65 2021-02-13T00:57:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.08 , -0.08 2021-02-13T00:57:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177840.87 epoch sec; seqNo:17237; 4060465.66 north; 593426.05 east 2021-02-13T00:57:21Z- AHRS_M2 msg: 1613177841.24 epoch sec; seqNo:17102 2021-02-13T00:57:21Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-13T00:57:21Z- handleDepth()-Depth_Keller msg: 1613177841245 epoch sec; seqNo:17238; depth 14.27 2021-02-13T00:57:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177841.26 epoch sec; seqNo:17238; 4060466.04 north; 593426.05 east 2021-02-13T00:57:21Z- AHRS_M2 msg: 1613177841.64 epoch sec; seqNo:17103 2021-02-13T00:57:21Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.15 psi 2021-02-13T00:57:21Z- handleDepth()-Depth_Keller msg: 1613177841650 epoch sec; seqNo:17239; depth 14.17 2021-02-13T00:57:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177841.67 epoch sec; seqNo:17239; 4060466.42 north; 593426.05 east 2021-02-13T00:57:22Z- AHRS_M2 msg: 1613177842.04 epoch sec; seqNo:17104 2021-02-13T00:57:22Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-13T00:57:22Z- handleDepth()-Depth_Keller msg: 1613177842049 epoch sec; seqNo:17240; depth 14.04 2021-02-13T00:57:22Z- handleDvl() - RDI_Pathfinder msg: 1613177842.07 epoch sec; seqNo:8331 2021-02-13T00:57:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 97.71 , 111.24 , 72.75 2021-02-13T00:57:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , -0.06 2021-02-13T00:57:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177842.08 epoch sec; seqNo:17240; 4060466.42 north; 593426.05 east 2021-02-13T00:57:22Z- AHRS_M2 msg: 1613177842.45 epoch sec; seqNo:17105 2021-02-13T00:57:22Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-13T00:57:22Z- handleDepth()-Depth_Keller msg: 1613177842457 epoch sec; seqNo:17241; depth 13.93 2021-02-13T00:57:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177842.47 epoch sec; seqNo:17241; 4060466.80 north; 593426.04 east 2021-02-13T00:57:22Z- LcmTrn::handleMessages() - handling 44 messages took 0.69 ms and 130 calls... 2021-02-13T00:57:22Z- waiting for 1613177840.24, time is 1613177842.47 2021-02-13T00:57:22Z- LcmTrn::updateTrn() >>>> pose time 1613177842.47, meas time 1613177842.07 <<<< 2021-02-13T00:57:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177842.07 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:57:22Z- TerrainNav::motionUpdate() - currEstimate = (4060467, 593426.04, 13.93) 2021-02-13T00:57:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:57:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:57:22Z- TNavBF::Projecting Measurements 2021-02-13T00:57:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:57:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:57:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:57:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:57:22Z- TNavFilter::4 good beams. 2021-02-13T00:57:22Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:57:22Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:57:22Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:57:22Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:57:22Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:57:22Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:57:22Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:57:22Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:22Z- TerrainMapOctree: (vehN, vehE ) = (4060466.54, 593425.86). 2021-02-13T00:57:22Z- TerrainMapOctree: Min Distance = 1137.11. 2021-02-13T00:57:22Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:57:22Z- There is no specific good beam that all particles have in common. 2021-02-13T00:57:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:57:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 10.98 2021-02-13T00:57:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:57:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:57:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:57:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 19.80 2021-02-13T00:57:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:57:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 8.47 2021-02-13T00:57:22Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:57:22Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:57:22Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:57:22Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:57:22Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:57:22Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:57:22Z- Filter 0 No beams used in subcloud update 2021-02-13T00:57:22Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:57:22Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:57:22Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 681.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:57:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177842.07 sec. 2021-02-13T00:57:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:57:22Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:22Z- variances([0], [2], and psi_berg) = 681.299748 832.989034 0.000000 2021-02-13T00:57:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:57:22Z- LcmTrn::publishEstimates() - MLE : 4060466.30 593445.54 13.93 2021-02-13T00:57:22Z- LcmTrn::publishEstimates() - MMSE : 4060466.80 593426.04 13.93 2021-02-13T00:57:22Z- LcmTrn::publishEstimates() - COVAR: 681.30 832.99 0.84 0.00 2021-02-13T00:57:22Z- AHRS_M2 msg: 1613177842.85 epoch sec; seqNo:17106 2021-02-13T00:57:22Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.17 psi 2021-02-13T00:57:22Z- handleDepth()-Depth_Keller msg: 1613177842858 epoch sec; seqNo:17242; depth 13.82 2021-02-13T00:57:22Z- handleDvl() - RDI_Pathfinder msg: 1613177842.87 epoch sec; seqNo:8332 2021-02-13T00:57:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.64 , 109.81 , 72.75 2021-02-13T00:57:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.04 , 0.02 2021-02-13T00:57:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177842.88 epoch sec; seqNo:17242; 4060467.19 north; 593427.26 east 2021-02-13T00:57:23Z- AHRS_M2 msg: 1613177843.26 epoch sec; seqNo:17107 2021-02-13T00:57:23Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.18 psi 2021-02-13T00:57:23Z- handleDepth()-Depth_Keller msg: 1613177843261 epoch sec; seqNo:17243; depth 13.69 2021-02-13T00:57:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177843.28 epoch sec; seqNo:17243; 4060467.57 north; 593427.25 east 2021-02-13T00:57:23Z- AHRS_M2 msg: 1613177843.66 epoch sec; seqNo:17108 2021-02-13T00:57:23Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.18 psi 2021-02-13T00:57:23Z- handleDepth()-Depth_Keller msg: 1613177843669 epoch sec; seqNo:17244; depth 13.59 2021-02-13T00:57:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177843.68 epoch sec; seqNo:17244; 4060467.95 north; 593427.25 east 2021-02-13T00:57:24Z- AHRS_M2 msg: 1613177844.06 epoch sec; seqNo:17109 2021-02-13T00:57:24Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.18 psi 2021-02-13T00:57:24Z- handleDepth()-Depth_Keller msg: 1613177844069 epoch sec; seqNo:17245; depth 13.47 2021-02-13T00:57:24Z- handleDvl() - RDI_Pathfinder msg: 1613177844.08 epoch sec; seqNo:8333 2021-02-13T00:57:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.28 , 113.80 , 78.64 2021-02-13T00:57:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.04 , 0.05 2021-02-13T00:57:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177844.09 epoch sec; seqNo:17245; 4060467.95 north; 593427.25 east 2021-02-13T00:57:24Z- AHRS_M2 msg: 1613177844.47 epoch sec; seqNo:17110 2021-02-13T00:57:24Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.16 psi 2021-02-13T00:57:24Z- handleDepth()-Depth_Keller msg: 1613177844475 epoch sec; seqNo:17246; depth 13.34 2021-02-13T00:57:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177844.49 epoch sec; seqNo:17246; 4060468.33 north; 593427.24 east 2021-02-13T00:57:24Z- AHRS_M2 msg: 1613177844.88 epoch sec; seqNo:17111 2021-02-13T00:57:24Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.16 psi 2021-02-13T00:57:24Z- handleDepth()-Depth_Keller msg: 1613177844889 epoch sec; seqNo:17247; depth 13.25 2021-02-13T00:57:24Z- handleDvl() - RDI_Pathfinder msg: 1613177844.90 epoch sec; seqNo:8334 2021-02-13T00:57:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.07 , 99.95 , 114.82 , 78.31 2021-02-13T00:57:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.16 , -0.03 2021-02-13T00:57:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177844.91 epoch sec; seqNo:17247; 4060468.72 north; 593428.46 east 2021-02-13T00:57:25Z- AHRS_M2 msg: 1613177845.28 epoch sec; seqNo:17112 2021-02-13T00:57:25Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.15 psi 2021-02-13T00:57:25Z- handleDepth()-Depth_Keller msg: 1613177845285 epoch sec; seqNo:17248; depth 13.11 2021-02-13T00:57:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177845.30 epoch sec; seqNo:17248; 4060469.10 north; 593428.46 east 2021-02-13T00:57:25Z- AHRS_M2 msg: 1613177845.68 epoch sec; seqNo:17113 2021-02-13T00:57:25Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-13T00:57:25Z- handleDepth()-Depth_Keller msg: 1613177845685 epoch sec; seqNo:17249; depth 13.01 2021-02-13T00:57:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177845.69 epoch sec; seqNo:17249; 4060469.10 north; 593428.46 east 2021-02-13T00:57:26Z- AHRS_M2 msg: 1613177846.09 epoch sec; seqNo:17114 2021-02-13T00:57:26Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-13T00:57:26Z- handleDepth()-Depth_Keller msg: 1613177846097 epoch sec; seqNo:17250; depth 12.89 2021-02-13T00:57:26Z- handleDvl() - RDI_Pathfinder msg: 1613177846.11 epoch sec; seqNo:8335 2021-02-13T00:57:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 98.86 , 111.03 , 78.58 2021-02-13T00:57:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.12 , -0.15 2021-02-13T00:57:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177846.12 epoch sec; seqNo:17250; 4060469.48 north; 593428.45 east 2021-02-13T00:57:26Z- AHRS_M2 msg: 1613177846.49 epoch sec; seqNo:17115 2021-02-13T00:57:26Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.16 psi 2021-02-13T00:57:26Z- handleDepth()-Depth_Keller msg: 1613177846493 epoch sec; seqNo:17251; depth 12.75 2021-02-13T00:57:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177846.52 epoch sec; seqNo:17251; 4060469.86 north; 593428.45 east 2021-02-13T00:57:26Z- AHRS_M2 msg: 1613177846.89 epoch sec; seqNo:17116 2021-02-13T00:57:26Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-13T00:57:26Z- handleDepth()-Depth_Keller msg: 1613177846897 epoch sec; seqNo:17252; depth 12.64 2021-02-13T00:57:26Z- handleDvl() - RDI_Pathfinder msg: 1613177846.91 epoch sec; seqNo:8336 2021-02-13T00:57:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.12 , 99.00 , 111.16 , 78.71 2021-02-13T00:57:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.13 2021-02-13T00:57:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177846.93 epoch sec; seqNo:17252; 4060469.86 north; 593428.45 east 2021-02-13T00:57:27Z- AHRS_M2 msg: 1613177847.30 epoch sec; seqNo:17117 2021-02-13T00:57:27Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.18 psi 2021-02-13T00:57:27Z- handleDepth()-Depth_Keller msg: 1613177847301 epoch sec; seqNo:17253; depth 12.51 2021-02-13T00:57:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177847.31 epoch sec; seqNo:17253; 4060470.25 north; 593429.67 east 2021-02-13T00:57:27Z- LcmTrn::handleMessages() - handling 41 messages took 0.61 ms and 129 calls... 2021-02-13T00:57:27Z- waiting for 1613177845.47, time is 1613177847.31 2021-02-13T00:57:27Z- LcmTrn::updateTrn() >>>> pose time 1613177847.31, meas time 1613177846.91 <<<< 2021-02-13T00:57:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177846.91 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:57:27Z- TerrainNav::motionUpdate() - currEstimate = (4060470, 593429.67, 12.51) 2021-02-13T00:57:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:57:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:57:27Z- TNavBF::Projecting Measurements 2021-02-13T00:57:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:57:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:57:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:57:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:57:27Z- TNavFilter::4 good beams. 2021-02-13T00:57:27Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:57:27Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:57:27Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:57:27Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:57:27Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:57:27Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:57:27Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:57:27Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:27Z- TerrainMapOctree: (vehN, vehE ) = (4060469.96, 593429.36). 2021-02-13T00:57:27Z- TerrainMapOctree: Min Distance = 1132.21. 2021-02-13T00:57:27Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:57:27Z- There is no specific good beam that all particles have in common. 2021-02-13T00:57:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:57:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 11.32 2021-02-13T00:57:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:57:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.68 2021-02-13T00:57:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:57:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 19.77 2021-02-13T00:57:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:57:27Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 9.91 2021-02-13T00:57:27Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:57:27Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:57:27Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:57:27Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:57:27Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:57:27Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:57:27Z- Filter 0 No beams used in subcloud update 2021-02-13T00:57:27Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:57:27Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:57:27Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 681.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:57:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177846.91 sec. 2021-02-13T00:57:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:57:27Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:27Z- variances([0], [2], and psi_berg) = 681.237768 832.989007 0.000000 2021-02-13T00:57:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:57:27Z- LcmTrn::publishEstimates() - MLE : 4060469.75 593449.17 12.51 2021-02-13T00:57:27Z- LcmTrn::publishEstimates() - MMSE : 4060470.25 593429.67 12.51 2021-02-13T00:57:27Z- LcmTrn::publishEstimates() - COVAR: 681.24 832.99 0.86 0.00 2021-02-13T00:57:27Z- AHRS_M2 msg: 1613177847.70 epoch sec; seqNo:17118 2021-02-13T00:57:27Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.19 psi 2021-02-13T00:57:27Z- handleDepth()-Depth_Keller msg: 1613177847707 epoch sec; seqNo:17254; depth 12.40 2021-02-13T00:57:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177847.71 epoch sec; seqNo:17254; 4060470.63 north; 593429.66 east 2021-02-13T00:57:28Z- AHRS_M2 msg: 1613177848.11 epoch sec; seqNo:17119 2021-02-13T00:57:28Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.19 psi 2021-02-13T00:57:28Z- handleDepth()-Depth_Keller msg: 1613177848113 epoch sec; seqNo:17255; depth 12.28 2021-02-13T00:57:28Z- handleDvl() - RDI_Pathfinder msg: 1613177848.14 epoch sec; seqNo:8337 2021-02-13T00:57:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 101.71 , 112.52 , 78.71 2021-02-13T00:57:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.12 , 0.01 2021-02-13T00:57:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177848.15 epoch sec; seqNo:17255; 4060471.01 north; 593429.66 east 2021-02-13T00:57:28Z- AHRS_M2 msg: 1613177848.51 epoch sec; seqNo:17120 2021-02-13T00:57:28Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.19 psi 2021-02-13T00:57:28Z- handleDepth()-Depth_Keller msg: 1613177848517 epoch sec; seqNo:17256; depth 12.14 2021-02-13T00:57:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177848.53 epoch sec; seqNo:17256; 4060471.01 north; 593429.66 east 2021-02-13T00:57:28Z- AHRS_M2 msg: 1613177848.91 epoch sec; seqNo:17121 2021-02-13T00:57:28Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.18 psi 2021-02-13T00:57:28Z- handleDepth()-Depth_Keller msg: 1613177848982 epoch sec; seqNo:17257; depth 12.03 2021-02-13T00:57:29Z- handleDvl() - RDI_Pathfinder msg: 1613177849.00 epoch sec; seqNo:8338 2021-02-13T00:57:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.64 , 112.46 , 76.43 2021-02-13T00:57:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.06 , 0.11 2021-02-13T00:57:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177849.02 epoch sec; seqNo:17257; 4060471.38 north; 593429.65 east 2021-02-13T00:57:29Z- AHRS_M2 msg: 1613177849.32 epoch sec; seqNo:17122 2021-02-13T00:57:29Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.17 psi 2021-02-13T00:57:29Z- handleDepth()-Depth_Keller msg: 1613177849330 epoch sec; seqNo:17258; depth 11.90 2021-02-13T00:57:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177849.34 epoch sec; seqNo:17258; 4060471.76 north; 593429.65 east 2021-02-13T00:57:29Z- AHRS_M2 msg: 1613177849.73 epoch sec; seqNo:17123 2021-02-13T00:57:29Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.16 psi 2021-02-13T00:57:29Z- handleDepth()-Depth_Keller msg: 1613177849753 epoch sec; seqNo:17259; depth 11.81 2021-02-13T00:57:29Z- handleDvl() - RDI_Pathfinder msg: 1613177849.77 epoch sec; seqNo:8339 2021-02-13T00:57:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.36 , 116.24 , 76.02 2021-02-13T00:57:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.07 , 0.07 2021-02-13T00:57:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177849.78 epoch sec; seqNo:17259; 4060472.14 north; 593429.64 east 2021-02-13T00:57:30Z- AHRS_M2 msg: 1613177850.12 epoch sec; seqNo:17124 2021-02-13T00:57:30Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.15 psi 2021-02-13T00:57:30Z- handleDepth()-Depth_Keller msg: 1613177850229 epoch sec; seqNo:17260; depth 11.69 2021-02-13T00:57:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177850.24 epoch sec; seqNo:17260; 4060472.53 north; 593430.86 east 2021-02-13T00:57:30Z- AHRS_M2 msg: 1613177850.53 epoch sec; seqNo:17125 2021-02-13T00:57:30Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.13 psi 2021-02-13T00:57:30Z- handleDepth()-Depth_Keller msg: 1613177850540 epoch sec; seqNo:17261; depth 11.56 2021-02-13T00:57:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177850.55 epoch sec; seqNo:17261; 4060472.53 north; 593430.86 east 2021-02-13T00:57:30Z- AHRS_M2 msg: 1613177850.93 epoch sec; seqNo:17126 2021-02-13T00:57:30Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.12 psi 2021-02-13T00:57:30Z- handleDepth()-Depth_Keller msg: 1613177850963 epoch sec; seqNo:17262; depth 11.48 2021-02-13T00:57:30Z- handleDvl() - RDI_Pathfinder msg: 1613177850.98 epoch sec; seqNo:8340 2021-02-13T00:57:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.59 , 116.17 , 78.31 2021-02-13T00:57:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.12 , -0.04 2021-02-13T00:57:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177851.00 epoch sec; seqNo:17262; 4060472.91 north; 593430.86 east 2021-02-13T00:57:31Z- AHRS_M2 msg: 1613177851.34 epoch sec; seqNo:17127 2021-02-13T00:57:31Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.12 psi 2021-02-13T00:57:31Z- handleDepth()-Depth_Keller msg: 1613177851359 epoch sec; seqNo:17263; depth 11.35 2021-02-13T00:57:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177851.38 epoch sec; seqNo:17263; 4060472.91 north; 593430.86 east 2021-02-13T00:57:31Z- AHRS_M2 msg: 1613177851.74 epoch sec; seqNo:17128 2021-02-13T00:57:31Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.11 psi 2021-02-13T00:57:31Z- handleDepth()-Depth_Keller msg: 1613177851745 epoch sec; seqNo:17264; depth 11.24 2021-02-13T00:57:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177851.76 epoch sec; seqNo:17264; 4060473.29 north; 593430.85 east 2021-02-13T00:57:32Z- AHRS_M2 msg: 1613177852.15 epoch sec; seqNo:17129 2021-02-13T00:57:32Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.12 psi 2021-02-13T00:57:32Z- handleDepth()-Depth_Keller msg: 1613177852158 epoch sec; seqNo:17265; depth 11.13 2021-02-13T00:57:32Z- handleDvl() - RDI_Pathfinder msg: 1613177852.17 epoch sec; seqNo:8341 2021-02-13T00:57:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.78 , 98.59 , 117.53 , 78.31 2021-02-13T00:57:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.07 , -0.12 2021-02-13T00:57:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177852.20 epoch sec; seqNo:17265; 4060473.67 north; 593430.85 east 2021-02-13T00:57:32Z- AHRS_M2 msg: 1613177852.56 epoch sec; seqNo:17130 2021-02-13T00:57:32Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.13 psi 2021-02-13T00:57:32Z- handleDepth()-Depth_Keller msg: 1613177852559 epoch sec; seqNo:17266; depth 10.99 2021-02-13T00:57:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177852.57 epoch sec; seqNo:17266; 4060473.68 north; 593432.07 east 2021-02-13T00:57:32Z- LcmTrn::handleMessages() - handling 44 messages took 0.77 ms and 129 calls... 2021-02-13T00:57:32Z- waiting for 1613177850.31, time is 1613177852.57 2021-02-13T00:57:32Z- LcmTrn::updateTrn() >>>> pose time 1613177852.57, meas time 1613177852.17 <<<< 2021-02-13T00:57:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177852.17 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:57:32Z- TerrainNav::motionUpdate() - currEstimate = (4060474, 593432.07, 10.99) 2021-02-13T00:57:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:57:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:57:32Z- TNavBF::Projecting Measurements 2021-02-13T00:57:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:57:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:57:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:57:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:57:32Z- TNavFilter::4 good beams. 2021-02-13T00:57:32Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:57:32Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:57:32Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:57:32Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:57:32Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:57:32Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:57:32Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:57:32Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:32Z- TerrainMapOctree: (vehN, vehE ) = (4060473.42, 593431.89). 2021-02-13T00:57:32Z- TerrainMapOctree: Min Distance = 1128.00. 2021-02-13T00:57:32Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:57:32Z- There is no specific good beam that all particles have in common. 2021-02-13T00:57:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:57:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 12.33 2021-02-13T00:57:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:57:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.55 2021-02-13T00:57:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:57:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 22.10 2021-02-13T00:57:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:57:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 9.81 2021-02-13T00:57:32Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:57:32Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:57:32Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:57:32Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:57:32Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:57:32Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:57:32Z- Filter 0 No beams used in subcloud update 2021-02-13T00:57:32Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:57:32Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:57:32Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 681.2 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:57:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177852.17 sec. 2021-02-13T00:57:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:57:32Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:32Z- variances([0], [2], and psi_berg) = 681.181491 832.988983 0.000000 2021-02-13T00:57:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:57:32Z- LcmTrn::publishEstimates() - MLE : 4060473.18 593451.57 10.99 2021-02-13T00:57:32Z- LcmTrn::publishEstimates() - MMSE : 4060473.68 593432.07 10.99 2021-02-13T00:57:32Z- LcmTrn::publishEstimates() - COVAR: 681.18 832.99 0.84 0.00 2021-02-13T00:57:32Z- AHRS_M2 msg: 1613177852.95 epoch sec; seqNo:17131 2021-02-13T00:57:32Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.14 psi 2021-02-13T00:57:32Z- handleDepth()-Depth_Keller msg: 1613177852957 epoch sec; seqNo:17267; depth 10.88 2021-02-13T00:57:32Z- handleDvl() - RDI_Pathfinder msg: 1613177852.97 epoch sec; seqNo:8342 2021-02-13T00:57:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.79 , 113.67 , 75.61 2021-02-13T00:57:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , -0.08 2021-02-13T00:57:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177852.98 epoch sec; seqNo:17267; 4060474.06 north; 593432.06 east 2021-02-13T00:57:33Z- AHRS_M2 msg: 1613177853.36 epoch sec; seqNo:17132 2021-02-13T00:57:33Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-13T00:57:33Z- handleDepth()-Depth_Keller msg: 1613177853361 epoch sec; seqNo:17268; depth 10.76 2021-02-13T00:57:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177853.38 epoch sec; seqNo:17268; 4060474.06 north; 593432.06 east 2021-02-13T00:57:33Z- AHRS_M2 msg: 1613177853.76 epoch sec; seqNo:17133 2021-02-13T00:57:33Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-13T00:57:33Z- handleDepth()-Depth_Keller msg: 1613177853764 epoch sec; seqNo:17269; depth 10.64 2021-02-13T00:57:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177853.77 epoch sec; seqNo:17269; 4060474.44 north; 593432.06 east 2021-02-13T00:57:34Z- AHRS_M2 msg: 1613177854.17 epoch sec; seqNo:17134 2021-02-13T00:57:34Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-13T00:57:34Z- handleDepth()-Depth_Keller msg: 1613177854173 epoch sec; seqNo:17270; depth 10.54 2021-02-13T00:57:34Z- handleDvl() - RDI_Pathfinder msg: 1613177854.18 epoch sec; seqNo:8343 2021-02-13T00:57:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:57:34Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:57:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177854.19 epoch sec; seqNo:17270; 4060474.82 north; 593432.06 east 2021-02-13T00:57:34Z- AHRS_M2 msg: 1613177854.57 epoch sec; seqNo:17135 2021-02-13T00:57:34Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T00:57:34Z- handleDepth()-Depth_Keller msg: 1613177854573 epoch sec; seqNo:17271; depth 10.41 2021-02-13T00:57:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177854.60 epoch sec; seqNo:17271; 4060474.82 north; 593432.06 east 2021-02-13T00:57:34Z- AHRS_M2 msg: 1613177854.97 epoch sec; seqNo:17136 2021-02-13T00:57:34Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-13T00:57:34Z- handleDepth()-Depth_Keller msg: 1613177854976 epoch sec; seqNo:17272; depth 10.29 2021-02-13T00:57:34Z- handleDvl() - RDI_Pathfinder msg: 1613177854.99 epoch sec; seqNo:8344 2021-02-13T00:57:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.75 , 103.60 , 115.50 , 76.43 2021-02-13T00:57:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , 0.14 2021-02-13T00:57:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177855.01 epoch sec; seqNo:17272; 4060475.21 north; 593433.27 east 2021-02-13T00:57:35Z- AHRS_M2 msg: 1613177855.38 epoch sec; seqNo:17137 2021-02-13T00:57:35Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-13T00:57:35Z- handleDepth()-Depth_Keller msg: 1613177855380 epoch sec; seqNo:17273; depth 10.19 2021-02-13T00:57:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177855.39 epoch sec; seqNo:17273; 4060475.59 north; 593433.27 east 2021-02-13T00:57:35Z- AHRS_M2 msg: 1613177855.78 epoch sec; seqNo:17138 2021-02-13T00:57:35Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.18 psi 2021-02-13T00:57:35Z- handleDepth()-Depth_Keller msg: 1613177855787 epoch sec; seqNo:17274; depth 10.08 2021-02-13T00:57:35Z- handleDvl() - RDI_Pathfinder msg: 1613177855.80 epoch sec; seqNo:8345 2021-02-13T00:57:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.85 , 117.73 , 79.86 2021-02-13T00:57:35Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.14 , 0.12 2021-02-13T00:57:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177855.81 epoch sec; seqNo:17274; 4060475.97 north; 593433.26 east 2021-02-13T00:57:36Z- AHRS_M2 msg: 1613177856.18 epoch sec; seqNo:17139 2021-02-13T00:57:36Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.17 psi 2021-02-13T00:57:36Z- handleDepth()-Depth_Keller msg: 1613177856189 epoch sec; seqNo:17275; depth 9.96 2021-02-13T00:57:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177856.23 epoch sec; seqNo:17275; 4060476.35 north; 593433.26 east 2021-02-13T00:57:36Z- AHRS_M2 msg: 1613177856.59 epoch sec; seqNo:17140 2021-02-13T00:57:36Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-13T00:57:36Z- handleDepth()-Depth_Keller msg: 1613177856592 epoch sec; seqNo:17276; depth 9.84 2021-02-13T00:57:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177856.60 epoch sec; seqNo:17276; 4060476.35 north; 593433.26 east 2021-02-13T00:57:36Z- AHRS_M2 msg: 1613177856.99 epoch sec; seqNo:17141 2021-02-13T00:57:36Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-13T00:57:36Z- handleDepth()-Depth_Keller msg: 1613177856996 epoch sec; seqNo:17277; depth 9.74 2021-02-13T00:57:37Z- handleDvl() - RDI_Pathfinder msg: 1613177857.01 epoch sec; seqNo:8346 2021-02-13T00:57:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 103.80 , 117.32 , 79.46 2021-02-13T00:57:37Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.19 , -0.05 2021-02-13T00:57:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177857.02 epoch sec; seqNo:17277; 4060476.74 north; 593434.48 east 2021-02-13T00:57:37Z- AHRS_M2 msg: 1613177857.40 epoch sec; seqNo:17142 2021-02-13T00:57:37Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-13T00:57:37Z- handleDepth()-Depth_Keller msg: 1613177857405 epoch sec; seqNo:17278; depth 9.64 2021-02-13T00:57:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177857.42 epoch sec; seqNo:17278; 4060477.12 north; 593434.47 east 2021-02-13T00:57:37Z- LcmTrn::handleMessages() - handling 41 messages took 0.61 ms and 129 calls... 2021-02-13T00:57:37Z- waiting for 1613177855.57, time is 1613177857.42 2021-02-13T00:57:37Z- LcmTrn::updateTrn() >>>> pose time 1613177857.42, meas time 1613177857.01 <<<< 2021-02-13T00:57:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177857.01 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:57:37Z- TerrainNav::motionUpdate() - currEstimate = (4060477, 593434.47, 9.64) 2021-02-13T00:57:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:57:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:57:37Z- TNavBF::Projecting Measurements 2021-02-13T00:57:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:57:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:57:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:57:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:57:37Z- TNavFilter::4 good beams. 2021-02-13T00:57:37Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:57:37Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:57:37Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:57:37Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:57:37Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:57:37Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:57:37Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:57:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:37Z- TerrainMapOctree: (vehN, vehE ) = (4060476.83, 593434.27). 2021-02-13T00:57:37Z- TerrainMapOctree: Min Distance = 1123.92. 2021-02-13T00:57:37Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:57:37Z- There is no specific good beam that all particles have in common. 2021-02-13T00:57:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:57:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 15.07 2021-02-13T00:57:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:57:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 17.24 2021-02-13T00:57:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:57:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 22.02 2021-02-13T00:57:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:57:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 10.10 2021-02-13T00:57:37Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:57:37Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:57:37Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:57:37Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:57:37Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:57:37Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:57:37Z- Filter 0 No beams used in subcloud update 2021-02-13T00:57:37Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:57:37Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:57:37Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 681.1 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:57:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177857.01 sec. 2021-02-13T00:57:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:57:37Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:37Z- variances([0], [2], and psi_berg) = 681.124762 832.988958 0.000000 2021-02-13T00:57:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:57:37Z- LcmTrn::publishEstimates() - MLE : 4060476.62 593453.97 9.64 2021-02-13T00:57:37Z- LcmTrn::publishEstimates() - MMSE : 4060477.12 593434.47 9.64 2021-02-13T00:57:37Z- LcmTrn::publishEstimates() - COVAR: 681.12 832.99 0.85 0.00 2021-02-13T00:57:37Z- AHRS_M2 msg: 1613177857.80 epoch sec; seqNo:17143 2021-02-13T00:57:37Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-13T00:57:37Z- handleDepth()-Depth_Keller msg: 1613177857805 epoch sec; seqNo:17279; depth 9.51 2021-02-13T00:57:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177857.81 epoch sec; seqNo:17279; 4060477.12 north; 593434.47 east 2021-02-13T00:57:38Z- AHRS_M2 msg: 1613177858.20 epoch sec; seqNo:17144 2021-02-13T00:57:38Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-13T00:57:38Z- handleDepth()-Depth_Keller msg: 1613177858210 epoch sec; seqNo:17280; depth 9.40 2021-02-13T00:57:38Z- handleDvl() - RDI_Pathfinder msg: 1613177858.25 epoch sec; seqNo:8347 2021-02-13T00:57:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 105.21 , 116.03 , 79.52 2021-02-13T00:57:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.11 , -0.18 2021-02-13T00:57:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177858.26 epoch sec; seqNo:17280; 4060477.50 north; 593434.47 east 2021-02-13T00:57:38Z- AHRS_M2 msg: 1613177858.61 epoch sec; seqNo:17145 2021-02-13T00:57:38Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-13T00:57:38Z- handleDepth()-Depth_Keller msg: 1613177858618 epoch sec; seqNo:17281; depth 9.26 2021-02-13T00:57:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177858.63 epoch sec; seqNo:17281; 4060477.88 north; 593434.46 east 2021-02-13T00:57:39Z- AHRS_M2 msg: 1613177859.01 epoch sec; seqNo:17146 2021-02-13T00:57:39Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-13T00:57:39Z- handleDepth()-Depth_Keller msg: 1613177859017 epoch sec; seqNo:17282; depth 9.16 2021-02-13T00:57:39Z- handleDvl() - RDI_Pathfinder msg: 1613177859.03 epoch sec; seqNo:8348 2021-02-13T00:57:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.07 , 118.94 , 77.24 2021-02-13T00:57:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.12 2021-02-13T00:57:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177859.05 epoch sec; seqNo:17282; 4060478.27 north; 593435.68 east 2021-02-13T00:57:39Z- AHRS_M2 msg: 1613177859.42 epoch sec; seqNo:17147 2021-02-13T00:57:39Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T00:57:39Z- handleDepth()-Depth_Keller msg: 1613177859421 epoch sec; seqNo:17283; depth 9.04 2021-02-13T00:57:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177859.43 epoch sec; seqNo:17283; 4060478.27 north; 593435.68 east 2021-02-13T00:57:39Z- AHRS_M2 msg: 1613177859.82 epoch sec; seqNo:17148 2021-02-13T00:57:39Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T00:57:39Z- handleDepth()-Depth_Keller msg: 1613177859827 epoch sec; seqNo:17284; depth 8.92 2021-02-13T00:57:39Z- handleDvl() - RDI_Pathfinder msg: 1613177859.84 epoch sec; seqNo:8349 2021-02-13T00:57:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:57:39Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:57:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177859.85 epoch sec; seqNo:17284; 4060478.65 north; 593435.68 east 2021-02-13T00:57:40Z- AHRS_M2 msg: 1613177860.22 epoch sec; seqNo:17149 2021-02-13T00:57:40Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-13T00:57:40Z- handleDepth()-Depth_Keller msg: 1613177860233 epoch sec; seqNo:17285; depth 8.80 2021-02-13T00:57:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177860.24 epoch sec; seqNo:17285; 4060479.03 north; 593435.67 east 2021-02-13T00:57:40Z- AHRS_M2 msg: 1613177860.63 epoch sec; seqNo:17150 2021-02-13T00:57:40Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-13T00:57:40Z- handleDepth()-Depth_Keller msg: 1613177860632 epoch sec; seqNo:17286; depth 8.68 2021-02-13T00:57:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177860.64 epoch sec; seqNo:17286; 4060479.03 north; 593435.67 east 2021-02-13T00:57:41Z- AHRS_M2 msg: 1613177861.03 epoch sec; seqNo:17151 2021-02-13T00:57:41Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-13T00:57:41Z- handleDepth()-Depth_Keller msg: 1613177861039 epoch sec; seqNo:17287; depth 8.57 2021-02-13T00:57:41Z- handleDvl() - RDI_Pathfinder msg: 1613177861.05 epoch sec; seqNo:8350 2021-02-13T00:57:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 105.15 , 118.68 , 79.46 2021-02-13T00:57:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.13 , 0.02 2021-02-13T00:57:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177861.06 epoch sec; seqNo:17287; 4060479.41 north; 593435.67 east 2021-02-13T00:57:41Z- AHRS_M2 msg: 1613177861.44 epoch sec; seqNo:17152 2021-02-13T00:57:41Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-13T00:57:41Z- handleDepth()-Depth_Keller msg: 1613177861441 epoch sec; seqNo:17288; depth 8.45 2021-02-13T00:57:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177861.46 epoch sec; seqNo:17288; 4060479.41 north; 593435.67 east 2021-02-13T00:57:41Z- AHRS_M2 msg: 1613177861.85 epoch sec; seqNo:17153 2021-02-13T00:57:41Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T00:57:41Z- handleDepth()-Depth_Keller msg: 1613177861884 epoch sec; seqNo:17289; depth 8.33 2021-02-13T00:57:41Z- handleDvl() - RDI_Pathfinder msg: 1613177861.90 epoch sec; seqNo:8351 2021-02-13T00:57:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 105.15 , 118.68 , 79.46 2021-02-13T00:57:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.13 , -0.07 2021-02-13T00:57:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177861.91 epoch sec; seqNo:17289; 4060479.80 north; 593436.88 east 2021-02-13T00:57:42Z- AHRS_M2 msg: 1613177862.24 epoch sec; seqNo:17154 2021-02-13T00:57:42Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.18 psi 2021-02-13T00:57:42Z- handleDepth()-Depth_Keller msg: 1613177862255 epoch sec; seqNo:17290; depth 8.20 2021-02-13T00:57:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177862.27 epoch sec; seqNo:17290; 4060480.18 north; 593436.88 east 2021-02-13T00:57:42Z- AHRS_M2 msg: 1613177862.65 epoch sec; seqNo:17155 2021-02-13T00:57:42Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-13T00:57:42Z- handleDepth()-Depth_Keller msg: 1613177862718 epoch sec; seqNo:17291; depth 8.10 2021-02-13T00:57:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177862.73 epoch sec; seqNo:17291; 4060480.18 north; 593436.88 east 2021-02-13T00:57:42Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-13T00:57:42Z- waiting for 1613177860.42, time is 1613177862.73 2021-02-13T00:57:42Z- LcmTrn::updateTrn() >>>> pose time 1613177862.73, meas time 1613177861.90 <<<< 2021-02-13T00:57:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177861.90 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:57:42Z- TerrainNav::motionUpdate() - currEstimate = (4060480, 593436.88, 8.10) 2021-02-13T00:57:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:57:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:57:42Z- TNavBF::Projecting Measurements 2021-02-13T00:57:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:57:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:57:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:57:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:57:42Z- TNavFilter::4 good beams. 2021-02-13T00:57:42Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:57:42Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:57:42Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:57:42Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:57:42Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:57:42Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:57:42Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:57:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:42Z- TerrainMapOctree: (vehN, vehE ) = (4060479.70, 593436.50). 2021-02-13T00:57:42Z- TerrainMapOctree: Min Distance = 1120.33. 2021-02-13T00:57:42Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:57:42Z- There is no specific good beam that all particles have in common. 2021-02-13T00:57:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:57:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 12.27 2021-02-13T00:57:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:57:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 17.69 2021-02-13T00:57:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:57:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 22.53 2021-02-13T00:57:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:57:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 10.10 2021-02-13T00:57:42Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:57:42Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:57:42Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:57:42Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:57:42Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:57:42Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:57:42Z- Filter 0 No beams used in subcloud update 2021-02-13T00:57:42Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:57:42Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:57:42Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 681.1 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:57:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177861.90 sec. 2021-02-13T00:57:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:57:42Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:42Z- variances([0], [2], and psi_berg) = 681.067224 832.988934 0.000000 2021-02-13T00:57:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:57:42Z- LcmTrn::publishEstimates() - MLE : 4060479.68 593456.38 8.10 2021-02-13T00:57:42Z- LcmTrn::publishEstimates() - MMSE : 4060480.18 593436.88 8.10 2021-02-13T00:57:42Z- LcmTrn::publishEstimates() - COVAR: 681.07 832.99 0.84 0.00 2021-02-13T00:57:43Z- AHRS_M2 msg: 1613177863.05 epoch sec; seqNo:17156 2021-02-13T00:57:43Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.19 psi 2021-02-13T00:57:43Z- handleDepth()-Depth_Keller msg: 1613177863064 epoch sec; seqNo:17292; depth 7.97 2021-02-13T00:57:43Z- handleDvl() - RDI_Pathfinder msg: 1613177863.08 epoch sec; seqNo:8352 2021-02-13T00:57:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 131.05 , 105.36 , 118.88 , 0.00 2021-02-13T00:57:43Z- handleDvl() - RDI_Pathfinder msg: velocities 2.35 , 1.53 , 0.76 2021-02-13T00:57:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177863.10 epoch sec; seqNo:17292; 4060480.57 north; 593438.10 east 2021-02-13T00:57:43Z- AHRS_M2 msg: 1613177863.46 epoch sec; seqNo:17157 2021-02-13T00:57:43Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.20 psi 2021-02-13T00:57:43Z- handleDepth()-Depth_Keller msg: 1613177863481 epoch sec; seqNo:17293; depth 7.87 2021-02-13T00:57:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177863.49 epoch sec; seqNo:17293; 4060480.96 north; 593439.31 east 2021-02-13T00:57:43Z- AHRS_M2 msg: 1613177863.86 epoch sec; seqNo:17158 2021-02-13T00:57:43Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.20 psi 2021-02-13T00:57:43Z- handleDepth()-Depth_Keller msg: 1613177863970 epoch sec; seqNo:17294; depth 7.75 2021-02-13T00:57:43Z- handleDvl() - RDI_Pathfinder msg: 1613177863.98 epoch sec; seqNo:8353 2021-02-13T00:57:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.82 , 110.62 , 121.04 , 80.88 2021-02-13T00:57:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.10 , 0.03 2021-02-13T00:57:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177864.00 epoch sec; seqNo:17294; 4060481.34 north; 593439.31 east 2021-02-13T00:57:44Z- AHRS_M2 msg: 1613177864.26 epoch sec; seqNo:17159 2021-02-13T00:57:44Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.20 psi 2021-02-13T00:57:44Z- handleDepth()-Depth_Keller msg: 1613177864276 epoch sec; seqNo:17295; depth 7.63 2021-02-13T00:57:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177864.29 epoch sec; seqNo:17295; 4060481.72 north; 593439.31 east 2021-02-13T00:57:44Z- AHRS_M2 msg: 1613177864.67 epoch sec; seqNo:17160 2021-02-13T00:57:44Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.20 psi 2021-02-13T00:57:44Z- handleDepth()-Depth_Keller msg: 1613177864693 epoch sec; seqNo:17296; depth 7.54 2021-02-13T00:57:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177864.70 epoch sec; seqNo:17296; 4060482.10 north; 593439.30 east 2021-02-13T00:57:45Z- AHRS_M2 msg: 1613177865.07 epoch sec; seqNo:17161 2021-02-13T00:57:45Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.20 psi 2021-02-13T00:57:45Z- handleDepth()-Depth_Keller msg: 1613177865081 epoch sec; seqNo:17297; depth 7.44 2021-02-13T00:57:45Z- handleDvl() - RDI_Pathfinder msg: 1613177865.10 epoch sec; seqNo:8354 2021-02-13T00:57:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 109.21 , 118.68 , 82.16 2021-02-13T00:57:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , 0.14 2021-02-13T00:57:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177865.11 epoch sec; seqNo:17297; 4060482.10 north; 593439.30 east 2021-02-13T00:57:45Z- AHRS_M2 msg: 1613177865.47 epoch sec; seqNo:17162 2021-02-13T00:57:45Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.19 psi 2021-02-13T00:57:45Z- handleDepth()-Depth_Keller msg: 1613177865501 epoch sec; seqNo:17298; depth 7.36 2021-02-13T00:57:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177865.52 epoch sec; seqNo:17298; 4060482.48 north; 593439.30 east 2021-02-13T00:57:45Z- AHRS_M2 msg: 1613177865.89 epoch sec; seqNo:17163 2021-02-13T00:57:45Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.18 psi 2021-02-13T00:57:45Z- handleDepth()-Depth_Keller msg: 1613177865898 epoch sec; seqNo:17299; depth 7.25 2021-02-13T00:57:45Z- handleDvl() - RDI_Pathfinder msg: 1613177865.91 epoch sec; seqNo:8355 2021-02-13T00:57:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 106.78 , 118.68 , 81.49 2021-02-13T00:57:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.19 , 0.09 2021-02-13T00:57:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177865.92 epoch sec; seqNo:17299; 4060482.87 north; 593440.51 east 2021-02-13T00:57:46Z- AHRS_M2 msg: 1613177866.28 epoch sec; seqNo:17164 2021-02-13T00:57:46Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.16 psi 2021-02-13T00:57:46Z- handleDepth()-Depth_Keller msg: 1613177866290 epoch sec; seqNo:17300; depth 7.14 2021-02-13T00:57:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177866.30 epoch sec; seqNo:17300; 4060483.25 north; 593440.51 east 2021-02-13T00:57:46Z- AHRS_M2 msg: 1613177866.69 epoch sec; seqNo:17165 2021-02-13T00:57:46Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.15 psi 2021-02-13T00:57:46Z- handleDepth()-Depth_Keller msg: 1613177866693 epoch sec; seqNo:17301; depth 7.04 2021-02-13T00:57:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177866.71 epoch sec; seqNo:17301; 4060483.25 north; 593440.51 east 2021-02-13T00:57:47Z- AHRS_M2 msg: 1613177867.09 epoch sec; seqNo:17166 2021-02-13T00:57:47Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.15 psi 2021-02-13T00:57:47Z- handleDepth()-Depth_Keller msg: 1613177867101 epoch sec; seqNo:17302; depth 6.92 2021-02-13T00:57:47Z- handleDvl() - RDI_Pathfinder msg: 1613177867.11 epoch sec; seqNo:8356 2021-02-13T00:57:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 105.22 , 121.45 , 0.00 2021-02-13T00:57:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.20 , -0.11 2021-02-13T00:57:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177867.14 epoch sec; seqNo:17302; 4060483.63 north; 593440.51 east 2021-02-13T00:57:47Z- AHRS_M2 msg: 1613177867.50 epoch sec; seqNo:17167 2021-02-13T00:57:47Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-13T00:57:47Z- handleDepth()-Depth_Keller msg: 1613177867507 epoch sec; seqNo:17303; depth 6.80 2021-02-13T00:57:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177867.52 epoch sec; seqNo:17303; 4060483.63 north; 593440.51 east 2021-02-13T00:57:47Z- AHRS_M2 msg: 1613177867.90 epoch sec; seqNo:17168 2021-02-13T00:57:47Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-13T00:57:47Z- handleDepth()-Depth_Keller msg: 1613177867922 epoch sec; seqNo:17304; depth 6.69 2021-02-13T00:57:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177867.95 epoch sec; seqNo:17304; 4060484.02 north; 593441.72 east 2021-02-13T00:57:47Z- LcmTrn::handleMessages() - handling 44 messages took 0.84 ms and 127 calls... 2021-02-13T00:57:47Z- waiting for 1613177865.73, time is 1613177867.95 2021-02-13T00:57:47Z- LcmTrn::updateTrn() >>>> pose time 1613177867.95, meas time 1613177867.11 <<<< 2021-02-13T00:57:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177867.11 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:57:47Z- TerrainNav::motionUpdate() - currEstimate = (4060484, 593441.72, 6.69) 2021-02-13T00:57:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:57:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:57:47Z- TNavBF::Projecting Measurements 2021-02-13T00:57:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:57:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:57:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:57:47Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:57:47Z- TNavFilter::3 good beams. 2021-02-13T00:57:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:57:47Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:57:47Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:57:47Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:57:47Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:57:47Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:57:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:57:47Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:47Z- TerrainMapOctree: (vehN, vehE ) = (4060483.40, 593440.95). 2021-02-13T00:57:47Z- TerrainMapOctree: Min Distance = 1114.56. 2021-02-13T00:57:47Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:57:47Z- There is no specific good beam that all particles have in common. 2021-02-13T00:57:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:57:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-13T00:57:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:57:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 17.71 2021-02-13T00:57:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:57:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 23.60 2021-02-13T00:57:47Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:57:47Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:57:48Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:57:48Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:57:48Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:57:48Z- Filter 0 No beams used in subcloud update 2021-02-13T00:57:48Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:57:48Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:57:48Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 681.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:57:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177867.11 sec. 2021-02-13T00:57:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:57:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:48Z- variances([0], [2], and psi_berg) = 680.994615 832.988903 0.000000 2021-02-13T00:57:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:57:48Z- LcmTrn::publishEstimates() - MLE : 4060483.52 593461.22 6.69 2021-02-13T00:57:48Z- LcmTrn::publishEstimates() - MMSE : 4060484.02 593441.72 6.69 2021-02-13T00:57:48Z- LcmTrn::publishEstimates() - COVAR: 680.99 832.99 1.00 0.00 2021-02-13T00:57:48Z- AHRS_M2 msg: 1613177868.30 epoch sec; seqNo:17169 2021-02-13T00:57:48Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-13T00:57:48Z- handleDepth()-Depth_Keller msg: 1613177868313 epoch sec; seqNo:17305; depth 6.55 2021-02-13T00:57:48Z- handleDvl() - RDI_Pathfinder msg: 1613177868.33 epoch sec; seqNo:8357 2021-02-13T00:57:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.44 , 106.37 , 119.76 , 82.57 2021-02-13T00:57:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.02 , -0.20 2021-02-13T00:57:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177868.35 epoch sec; seqNo:17305; 4060484.02 north; 593441.72 east 2021-02-13T00:57:48Z- AHRS_M2 msg: 1613177868.71 epoch sec; seqNo:17170 2021-02-13T00:57:48Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-13T00:57:48Z- handleDepth()-Depth_Keller msg: 1613177868713 epoch sec; seqNo:17306; depth 6.43 2021-02-13T00:57:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177868.73 epoch sec; seqNo:17306; 4060484.40 north; 593441.72 east 2021-02-13T00:57:49Z- AHRS_M2 msg: 1613177869.11 epoch sec; seqNo:17171 2021-02-13T00:57:49Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T00:57:49Z- handleDepth()-Depth_Keller msg: 1613177869129 epoch sec; seqNo:17307; depth 6.29 2021-02-13T00:57:49Z- handleDvl() - RDI_Pathfinder msg: 1613177869.14 epoch sec; seqNo:8358 2021-02-13T00:57:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 106.90 , 121.78 , 83.11 2021-02-13T00:57:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.25 , -0.01 2021-02-13T00:57:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177869.15 epoch sec; seqNo:17307; 4060484.78 north; 593441.71 east 2021-02-13T00:57:49Z- AHRS_M2 msg: 1613177869.52 epoch sec; seqNo:17172 2021-02-13T00:57:49Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.19 psi 2021-02-13T00:57:49Z- handleDepth()-Depth_Keller msg: 1613177869526 epoch sec; seqNo:17308; depth 6.17 2021-02-13T00:57:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177869.55 epoch sec; seqNo:17308; 4060485.16 north; 593441.71 east 2021-02-13T00:57:49Z- AHRS_M2 msg: 1613177869.93 epoch sec; seqNo:17173 2021-02-13T00:57:49Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.17 psi 2021-02-13T00:57:49Z- handleDepth()-Depth_Keller msg: 1613177869948 epoch sec; seqNo:17309; depth 6.08 2021-02-13T00:57:49Z- handleDvl() - RDI_Pathfinder msg: 1613177869.97 epoch sec; seqNo:8359 2021-02-13T00:57:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.92 , 109.20 , 118.67 , 82.16 2021-02-13T00:57:49Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , -0.07 , 0.27 2021-02-13T00:57:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177869.98 epoch sec; seqNo:17309; 4060485.54 north; 593441.70 east 2021-02-13T00:57:50Z- AHRS_M2 msg: 1613177870.32 epoch sec; seqNo:17174 2021-02-13T00:57:50Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.16 psi 2021-02-13T00:57:50Z- handleDepth()-Depth_Keller msg: 1613177870333 epoch sec; seqNo:17310; depth 5.96 2021-02-13T00:57:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177870.34 epoch sec; seqNo:17310; 4060485.91 north; 593441.70 east 2021-02-13T00:57:50Z- AHRS_M2 msg: 1613177870.73 epoch sec; seqNo:17175 2021-02-13T00:57:50Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.14 psi 2021-02-13T00:57:50Z- handleDepth()-Depth_Keller msg: 1613177870738 epoch sec; seqNo:17311; depth 5.85 2021-02-13T00:57:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177870.75 epoch sec; seqNo:17311; 4060486.31 north; 593442.92 east 2021-02-13T00:57:51Z- AHRS_M2 msg: 1613177871.13 epoch sec; seqNo:17176 2021-02-13T00:57:51Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.12 psi 2021-02-13T00:57:51Z- handleDepth()-Depth_Keller msg: 1613177871145 epoch sec; seqNo:17312; depth 5.75 2021-02-13T00:57:51Z- handleDvl() - RDI_Pathfinder msg: 1613177871.17 epoch sec; seqNo:8360 2021-02-13T00:57:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.03 , 107.85 , 122.72 , 82.16 2021-02-13T00:57:51Z- handleDvl() - RDI_Pathfinder msg: velocities 1.12 , 0.31 , 0.22 2021-02-13T00:57:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177871.18 epoch sec; seqNo:17312; 4060486.69 north; 593442.91 east 2021-02-13T00:57:51Z- AHRS_M2 msg: 1613177871.54 epoch sec; seqNo:17177 2021-02-13T00:57:51Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.11 psi 2021-02-13T00:57:51Z- handleDepth()-Depth_Keller msg: 1613177871541 epoch sec; seqNo:17313; depth 5.62 2021-02-13T00:57:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177871.55 epoch sec; seqNo:17313; 4060486.69 north; 593442.91 east 2021-02-13T00:57:51Z- AHRS_M2 msg: 1613177871.94 epoch sec; seqNo:17178 2021-02-13T00:57:51Z- AHRS_M2 msg: 0.62 phi; 0.27 theta; -0.13 psi 2021-02-13T00:57:51Z- handleDepth()-Depth_Keller msg: 1613177871946 epoch sec; seqNo:17314; depth 5.51 2021-02-13T00:57:51Z- handleDvl() - RDI_Pathfinder msg: 1613177871.96 epoch sec; seqNo:8361 2021-02-13T00:57:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.91 , 103.86 , 120.22 , 83.04 2021-02-13T00:57:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.41 , -0.18 2021-02-13T00:57:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177871.97 epoch sec; seqNo:17314; 4060487.08 north; 593444.13 east 2021-02-13T00:57:52Z- AHRS_M2 msg: 1613177872.34 epoch sec; seqNo:17179 2021-02-13T00:57:52Z- AHRS_M2 msg: 0.63 phi; 0.23 theta; -0.15 psi 2021-02-13T00:57:52Z- handleDepth()-Depth_Keller msg: 1613177872351 epoch sec; seqNo:17315; depth 5.38 2021-02-13T00:57:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177872.36 epoch sec; seqNo:17315; 4060487.08 north; 593444.13 east 2021-02-13T00:57:52Z- AHRS_M2 msg: 1613177872.75 epoch sec; seqNo:17180 2021-02-13T00:57:52Z- AHRS_M2 msg: 0.65 phi; 0.18 theta; -0.16 psi 2021-02-13T00:57:52Z- handleDepth()-Depth_Keller msg: 1613177872753 epoch sec; seqNo:17316; depth 5.26 2021-02-13T00:57:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177872.77 epoch sec; seqNo:17316; 4060487.46 north; 593444.13 east 2021-02-13T00:57:53Z- LcmTrn::handleMessages() - handling 41 messages took 0.70 ms and 127 calls... 2021-02-13T00:57:53Z- waiting for 1613177870.95, time is 1613177872.77 2021-02-13T00:57:53Z- LcmTrn::updateTrn() >>>> pose time 1613177872.77, meas time 1613177871.96 <<<< 2021-02-13T00:57:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177871.96 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:57:53Z- TerrainNav::motionUpdate() - currEstimate = (4060487, 593444.13, 5.26) 2021-02-13T00:57:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:57:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:57:53Z- TNavBF::Projecting Measurements 2021-02-13T00:57:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:57:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:57:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:57:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:57:53Z- TNavFilter::4 good beams. 2021-02-13T00:57:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:57:53Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:57:53Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:57:53Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:57:53Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:57:53Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:57:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:57:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:53Z- TerrainMapOctree: (vehN, vehE ) = (4060486.88, 593443.72). 2021-02-13T00:57:53Z- TerrainMapOctree: Min Distance = 1110.16. 2021-02-13T00:57:53Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:57:53Z- There is no specific good beam that all particles have in common. 2021-02-13T00:57:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:57:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:57:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:57:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 17.26 2021-02-13T00:57:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:57:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 23.13 2021-02-13T00:57:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:57:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 11.03 2021-02-13T00:57:53Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:57:53Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:57:53Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:57:53Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:57:53Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:57:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:57:53Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:57:53Z- Filter 0 No beams used in subcloud update 2021-02-13T00:57:53Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:57:53Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:57:53Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 681.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:57:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177871.96 sec. 2021-02-13T00:57:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:57:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:53Z- variances([0], [2], and psi_berg) = 680.934571 832.988877 0.000000 2021-02-13T00:57:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:57:53Z- LcmTrn::publishEstimates() - MLE : 4060486.96 593463.63 5.26 2021-02-13T00:57:53Z- LcmTrn::publishEstimates() - MMSE : 4060487.46 593444.13 5.26 2021-02-13T00:57:53Z- LcmTrn::publishEstimates() - COVAR: 680.93 832.99 0.96 0.00 2021-02-13T00:57:53Z- AHRS_M2 msg: 1613177873.16 epoch sec; seqNo:17181 2021-02-13T00:57:53Z- AHRS_M2 msg: 0.66 phi; 0.13 theta; -0.18 psi 2021-02-13T00:57:53Z- handleDepth()-Depth_Keller msg: 1613177873161 epoch sec; seqNo:17317; depth 5.15 2021-02-13T00:57:53Z- handleDvl() - RDI_Pathfinder msg: 1613177873.17 epoch sec; seqNo:8362 2021-02-13T00:57:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.54 , 103.93 , 111.37 , 86.08 2021-02-13T00:57:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.15 , -0.38 2021-02-13T00:57:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177873.20 epoch sec; seqNo:17317; 4060487.46 north; 593444.13 east 2021-02-13T00:57:53Z- AHRS_M2 msg: 1613177873.56 epoch sec; seqNo:17182 2021-02-13T00:57:53Z- AHRS_M2 msg: 0.66 phi; 0.09 theta; -0.20 psi 2021-02-13T00:57:53Z- handleDepth()-Depth_Keller msg: 1613177873561 epoch sec; seqNo:17318; depth 5.02 2021-02-13T00:57:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177873.57 epoch sec; seqNo:17318; 4060487.83 north; 593444.12 east 2021-02-13T00:57:53Z- AHRS_M2 msg: 1613177873.97 epoch sec; seqNo:17183 2021-02-13T00:57:53Z- AHRS_M2 msg: 0.67 phi; 0.04 theta; -0.21 psi 2021-02-13T00:57:53Z- handleDepth()-Depth_Keller msg: 1613177873974 epoch sec; seqNo:17319; depth 4.96 2021-02-13T00:57:53Z- handleDvl() - RDI_Pathfinder msg: 1613177873.99 epoch sec; seqNo:8363 2021-02-13T00:57:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.35 , 105.28 , 106.63 , 87.70 2021-02-13T00:57:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.11 , -0.14 2021-02-13T00:57:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177874.00 epoch sec; seqNo:17319; 4060488.23 north; 593445.34 east 2021-02-13T00:57:54Z- AHRS_M2 msg: 1613177874.36 epoch sec; seqNo:17184 2021-02-13T00:57:54Z- AHRS_M2 msg: 0.67 phi; 0.00 theta; -0.21 psi 2021-02-13T00:57:54Z- handleDepth()-Depth_Keller msg: 1613177874369 epoch sec; seqNo:17320; depth 4.88 2021-02-13T00:57:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177874.39 epoch sec; seqNo:17320; 4060488.61 north; 593445.33 east 2021-02-13T00:57:54Z- AHRS_M2 msg: 1613177874.77 epoch sec; seqNo:17185 2021-02-13T00:57:54Z- AHRS_M2 msg: 0.68 phi; -0.04 theta; -0.20 psi 2021-02-13T00:57:54Z- handleDepth()-Depth_Keller msg: 1613177874773 epoch sec; seqNo:17321; depth 4.84 2021-02-13T00:57:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177874.78 epoch sec; seqNo:17321; 4060488.61 north; 593445.33 east 2021-02-13T00:57:55Z- AHRS_M2 msg: 1613177875.17 epoch sec; seqNo:17186 2021-02-13T00:57:55Z- AHRS_M2 msg: 0.68 phi; -0.07 theta; -0.18 psi 2021-02-13T00:57:55Z- handleDepth()-Depth_Keller msg: 1613177875177 epoch sec; seqNo:17322; depth 4.81 2021-02-13T00:57:55Z- handleDvl() - RDI_Pathfinder msg: 1613177875.19 epoch sec; seqNo:8364 2021-02-13T00:57:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 105.42 , 97.31 , 97.31 2021-02-13T00:57:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.01 , 0.06 2021-02-13T00:57:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177875.20 epoch sec; seqNo:17322; 4060488.98 north; 593445.33 east 2021-02-13T00:57:55Z- AHRS_M2 msg: 1613177875.58 epoch sec; seqNo:17187 2021-02-13T00:57:55Z- AHRS_M2 msg: 0.67 phi; -0.10 theta; -0.16 psi 2021-02-13T00:57:55Z- handleDepth()-Depth_Keller msg: 1613177875582 epoch sec; seqNo:17323; depth 4.79 2021-02-13T00:57:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177875.59 epoch sec; seqNo:17323; 4060489.36 north; 593445.33 east 2021-02-13T00:57:55Z- AHRS_M2 msg: 1613177875.98 epoch sec; seqNo:17188 2021-02-13T00:57:55Z- AHRS_M2 msg: 0.67 phi; -0.12 theta; -0.17 psi 2021-02-13T00:57:55Z- handleDepth()-Depth_Keller msg: 1613177875985 epoch sec; seqNo:17324; depth 4.78 2021-02-13T00:57:56Z- handleDvl() - RDI_Pathfinder msg: 1613177876.00 epoch sec; seqNo:8365 2021-02-13T00:57:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 104.13 , 89.26 , 100.08 2021-02-13T00:57:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.14 , 0.05 2021-02-13T00:57:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177876.01 epoch sec; seqNo:17324; 4060489.36 north; 593445.33 east 2021-02-13T00:57:56Z- AHRS_M2 msg: 1613177876.38 epoch sec; seqNo:17189 2021-02-13T00:57:56Z- AHRS_M2 msg: 0.67 phi; -0.14 theta; -0.18 psi 2021-02-13T00:57:56Z- handleDepth()-Depth_Keller msg: 1613177876453 epoch sec; seqNo:17325; depth 4.76 2021-02-13T00:57:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177876.47 epoch sec; seqNo:17325; 4060489.76 north; 593446.54 east 2021-02-13T00:57:56Z- AHRS_M2 msg: 1613177876.79 epoch sec; seqNo:17190 2021-02-13T00:57:56Z- AHRS_M2 msg: 0.67 phi; -0.15 theta; -0.19 psi 2021-02-13T00:57:56Z- handleDepth()-Depth_Keller msg: 1613177876800 epoch sec; seqNo:17326; depth 4.79 2021-02-13T00:57:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177876.81 epoch sec; seqNo:17326; 4060489.76 north; 593446.54 east 2021-02-13T00:57:57Z- AHRS_M2 msg: 1613177877.19 epoch sec; seqNo:17191 2021-02-13T00:57:57Z- AHRS_M2 msg: 0.67 phi; -0.16 theta; -0.22 psi 2021-02-13T00:57:57Z- handleDepth()-Depth_Keller msg: 1613177877217 epoch sec; seqNo:17327; depth 4.81 2021-02-13T00:57:57Z- handleDvl() - RDI_Pathfinder msg: 1613177877.23 epoch sec; seqNo:8366 2021-02-13T00:57:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:57:57Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:57:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177877.26 epoch sec; seqNo:17327; 4060490.13 north; 593446.54 east 2021-02-13T00:57:57Z- AHRS_M2 msg: 1613177877.60 epoch sec; seqNo:17192 2021-02-13T00:57:57Z- AHRS_M2 msg: 0.67 phi; -0.16 theta; -0.24 psi 2021-02-13T00:57:57Z- handleDepth()-Depth_Keller msg: 1613177877621 epoch sec; seqNo:17328; depth 4.82 2021-02-13T00:57:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177877.64 epoch sec; seqNo:17328; 4060490.13 north; 593446.54 east 2021-02-13T00:57:58Z- AHRS_M2 msg: 1613177878.01 epoch sec; seqNo:17193 2021-02-13T00:57:58Z- AHRS_M2 msg: 0.68 phi; -0.17 theta; -0.25 psi 2021-02-13T00:57:58Z- handleDepth()-Depth_Keller msg: 1613177878078 epoch sec; seqNo:17329; depth 4.87 2021-02-13T00:57:58Z- handleDvl() - RDI_Pathfinder msg: 1613177878.09 epoch sec; seqNo:8367 2021-02-13T00:57:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 109.20 , 97.31 , 106.23 2021-02-13T00:57:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.10 , -0.11 2021-02-13T00:57:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177878.10 epoch sec; seqNo:17329; 4060490.51 north; 593446.53 east 2021-02-13T00:57:58Z- LcmTrn::handleMessages() - handling 45 messages took 0.74 ms and 130 calls... 2021-02-13T00:57:58Z- waiting for 1613177875.77, time is 1613177878.10 2021-02-13T00:57:58Z- LcmTrn::updateTrn() equating pose 1613177878.10 and meas times 1613177878.10 2021-02-13T00:57:58Z- LcmTrn::updateTrn() >>>> pose time 1613177878.10, meas time 1613177878.10 <<<< 2021-02-13T00:57:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177878.10 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:57:58Z- TerrainNav::motionUpdate() - currEstimate = (4060491, 593446.53, 4.87) 2021-02-13T00:57:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:57:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:57:58Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177878.10 sec.; No relevant navigation data available 2021-02-13T00:57:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:57:58Z- TNavBF::mmse selected from filter -1 2021-02-13T00:57:58Z- variances([0], [2], and psi_berg) = 680.890858 832.988858 0.000000 2021-02-13T00:57:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:57:58Z- LcmTrn::publishEstimates() - MLE : 4060490.01 593466.03 4.87 2021-02-13T00:57:58Z- LcmTrn::publishEstimates() - MMSE : 4060490.51 593446.53 4.87 2021-02-13T00:57:58Z- LcmTrn::publishEstimates() - COVAR: 680.89 832.99 1.16 0.00 2021-02-13T00:57:58Z- AHRS_M2 msg: 1613177878.40 epoch sec; seqNo:17194 2021-02-13T00:57:58Z- AHRS_M2 msg: 0.68 phi; -0.18 theta; -0.26 psi 2021-02-13T00:57:58Z- handleDepth()-Depth_Keller msg: 1613177878416 epoch sec; seqNo:17330; depth 4.91 2021-02-13T00:57:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177878.43 epoch sec; seqNo:17330; 4060490.89 north; 593446.53 east 2021-02-13T00:57:58Z- AHRS_M2 msg: 1613177878.81 epoch sec; seqNo:17195 2021-02-13T00:57:58Z- AHRS_M2 msg: 0.69 phi; -0.20 theta; -0.25 psi 2021-02-13T00:57:58Z- handleDepth()-Depth_Keller msg: 1613177878837 epoch sec; seqNo:17331; depth 4.93 2021-02-13T00:57:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177878.85 epoch sec; seqNo:17331; 4060491.28 north; 593447.75 east 2021-02-13T00:57:59Z- AHRS_M2 msg: 1613177879.21 epoch sec; seqNo:17196 2021-02-13T00:57:59Z- AHRS_M2 msg: 0.69 phi; -0.22 theta; -0.23 psi 2021-02-13T00:57:59Z- handleDepth()-Depth_Keller msg: 1613177879253 epoch sec; seqNo:17332; depth 4.99 2021-02-13T00:57:59Z- handleDvl() - RDI_Pathfinder msg: 1613177879.27 epoch sec; seqNo:8368 2021-02-13T00:57:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.86 , 113.26 , 0.00 , 106.50 2021-02-13T00:57:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.18 , 0.08 2021-02-13T00:57:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177879.28 epoch sec; seqNo:17332; 4060491.66 north; 593447.74 east 2021-02-13T00:57:59Z- AHRS_M2 msg: 1613177879.62 epoch sec; seqNo:17197 2021-02-13T00:57:59Z- AHRS_M2 msg: 0.69 phi; -0.24 theta; -0.21 psi 2021-02-13T00:57:59Z- handleDepth()-Depth_Keller msg: 1613177879625 epoch sec; seqNo:17333; depth 5.04 2021-02-13T00:57:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177879.64 epoch sec; seqNo:17333; 4060491.66 north; 593447.74 east 2021-02-13T00:58:00Z- AHRS_M2 msg: 1613177880.02 epoch sec; seqNo:17198 2021-02-13T00:58:00Z- AHRS_M2 msg: 0.68 phi; -0.26 theta; -0.19 psi 2021-02-13T00:58:00Z- handleDepth()-Depth_Keller msg: 1613177880026 epoch sec; seqNo:17334; depth 5.09 2021-02-13T00:58:00Z- handleDvl() - RDI_Pathfinder msg: 1613177880.04 epoch sec; seqNo:8369 2021-02-13T00:58:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.08 , 108.39 , 0.00 , 112.85 2021-02-13T00:58:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , 0.01 , 0.18 2021-02-13T00:58:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177880.05 epoch sec; seqNo:17334; 4060492.04 north; 593447.74 east 2021-02-13T00:58:00Z- AHRS_M2 msg: 1613177880.42 epoch sec; seqNo:17199 2021-02-13T00:58:00Z- AHRS_M2 msg: 0.67 phi; -0.28 theta; -0.18 psi 2021-02-13T00:58:00Z- handleDepth()-Depth_Keller msg: 1613177880433 epoch sec; seqNo:17335; depth 5.15 2021-02-13T00:58:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177880.44 epoch sec; seqNo:17335; 4060492.04 north; 593447.74 east 2021-02-13T00:58:00Z- AHRS_M2 msg: 1613177880.83 epoch sec; seqNo:17200 2021-02-13T00:58:00Z- AHRS_M2 msg: 0.67 phi; -0.29 theta; -0.17 psi 2021-02-13T00:58:00Z- handleDepth()-Depth_Keller msg: 1613177880835 epoch sec; seqNo:17336; depth 5.21 2021-02-13T00:58:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177880.84 epoch sec; seqNo:17336; 4060492.42 north; 593447.73 east 2021-02-13T00:58:01Z- AHRS_M2 msg: 1613177881.23 epoch sec; seqNo:17201 2021-02-13T00:58:01Z- AHRS_M2 msg: 0.66 phi; -0.30 theta; -0.18 psi 2021-02-13T00:58:01Z- handleDepth()-Depth_Keller msg: 1613177881249 epoch sec; seqNo:17337; depth 5.28 2021-02-13T00:58:01Z- handleDvl() - RDI_Pathfinder msg: 1613177881.28 epoch sec; seqNo:8370 2021-02-13T00:58:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.91 , 105.35 , 83.04 , 118.74 2021-02-13T00:58:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.20 , 0.17 2021-02-13T00:58:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177881.29 epoch sec; seqNo:17337; 4060492.43 north; 593448.95 east 2021-02-13T00:58:01Z- AHRS_M2 msg: 1613177881.64 epoch sec; seqNo:17202 2021-02-13T00:58:01Z- AHRS_M2 msg: 0.66 phi; -0.30 theta; -0.20 psi 2021-02-13T00:58:01Z- handleDepth()-Depth_Keller msg: 1613177881640 epoch sec; seqNo:17338; depth 5.34 2021-02-13T00:58:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177881.65 epoch sec; seqNo:17338; 4060492.81 north; 593448.95 east 2021-02-13T00:58:02Z- AHRS_M2 msg: 1613177882.04 epoch sec; seqNo:17203 2021-02-13T00:58:02Z- AHRS_M2 msg: 0.66 phi; -0.30 theta; -0.22 psi 2021-02-13T00:58:02Z- handleDepth()-Depth_Keller msg: 1613177882050 epoch sec; seqNo:17339; depth 5.41 2021-02-13T00:58:02Z- handleDvl() - RDI_Pathfinder msg: 1613177882.06 epoch sec; seqNo:8371 2021-02-13T00:58:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.98 , 109.88 , 83.11 , 118.80 2021-02-13T00:58:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.19 , 0.02 2021-02-13T00:58:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177882.07 epoch sec; seqNo:17339; 4060492.81 north; 593448.95 east 2021-02-13T00:58:02Z- AHRS_M2 msg: 1613177882.44 epoch sec; seqNo:17204 2021-02-13T00:58:02Z- AHRS_M2 msg: 0.66 phi; -0.30 theta; -0.24 psi 2021-02-13T00:58:02Z- handleDepth()-Depth_Keller msg: 1613177882449 epoch sec; seqNo:17340; depth 5.51 2021-02-13T00:58:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177882.46 epoch sec; seqNo:17340; 4060493.19 north; 593448.95 east 2021-02-13T00:58:02Z- AHRS_M2 msg: 1613177882.85 epoch sec; seqNo:17205 2021-02-13T00:58:02Z- AHRS_M2 msg: 0.66 phi; -0.30 theta; -0.25 psi 2021-02-13T00:58:02Z- handleDepth()-Depth_Keller msg: 1613177882860 epoch sec; seqNo:17341; depth 5.61 2021-02-13T00:58:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177882.90 epoch sec; seqNo:17341; 4060493.57 north; 593448.94 east 2021-02-13T00:58:03Z- LcmTrn::handleMessages() - handling 40 messages took 0.78 ms and 125 calls... 2021-02-13T00:58:03Z- waiting for 1613177881.10, time is 1613177882.90 2021-02-13T00:58:03Z- LcmTrn::updateTrn() >>>> pose time 1613177882.90, meas time 1613177882.06 <<<< 2021-02-13T00:58:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177882.06 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:58:03Z- TerrainNav::motionUpdate() - currEstimate = (4060494, 593448.94, 5.61) 2021-02-13T00:58:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:58:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:58:03Z- TNavBF::Projecting Measurements 2021-02-13T00:58:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:58:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:58:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:58:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:58:03Z- TNavFilter::4 good beams. 2021-02-13T00:58:03Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:58:03Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:58:03Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:58:03Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:58:03Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:58:03Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:58:03Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:58:03Z- TNavBF::mmse selected from filter -1 2021-02-13T00:58:03Z- TerrainMapOctree: (vehN, vehE ) = (4060493.04, 593448.52). 2021-02-13T00:58:03Z- TerrainMapOctree: Min Distance = 1102.43. 2021-02-13T00:58:03Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:58:03Z- There is no specific good beam that all particles have in common. 2021-02-13T00:58:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:58:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:58:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:58:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 19.32 2021-02-13T00:58:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:58:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 11.05 2021-02-13T00:58:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:58:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 22.58 2021-02-13T00:58:03Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:58:03Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:58:03Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:58:03Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:58:03Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:58:03Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:58:03Z- Filter 0 No beams used in subcloud update 2021-02-13T00:58:03Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:58:03Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:58:03Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 680.9 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:58:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177882.06 sec. 2021-02-13T00:58:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:58:03Z- TNavBF::mmse selected from filter -1 2021-02-13T00:58:03Z- variances([0], [2], and psi_berg) = 680.832929 832.988833 0.000000 2021-02-13T00:58:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:58:03Z- LcmTrn::publishEstimates() - MLE : 4060493.07 593468.44 5.61 2021-02-13T00:58:03Z- LcmTrn::publishEstimates() - MMSE : 4060493.57 593448.94 5.61 2021-02-13T00:58:03Z- LcmTrn::publishEstimates() - COVAR: 680.83 832.99 1.05 0.00 2021-02-13T00:58:03Z- AHRS_M2 msg: 1613177883.29 epoch sec; seqNo:17206 2021-02-13T00:58:03Z- AHRS_M2 msg: 0.67 phi; -0.30 theta; -0.25 psi 2021-02-13T00:58:03Z- handleDepth()-Depth_Keller msg: 1613177883311 epoch sec; seqNo:17342; depth 5.70 2021-02-13T00:58:03Z- handleDvl() - RDI_Pathfinder msg: 1613177883.32 epoch sec; seqNo:8372 2021-02-13T00:58:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.50 , 112.79 , 0.00 , 121.71 2021-02-13T00:58:03Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , -0.04 , -0.05 2021-02-13T00:58:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177883.34 epoch sec; seqNo:17342; 4060493.96 north; 593450.16 east 2021-02-13T00:58:03Z- AHRS_M2 msg: 1613177883.66 epoch sec; seqNo:17207 2021-02-13T00:58:03Z- AHRS_M2 msg: 0.68 phi; -0.30 theta; -0.24 psi 2021-02-13T00:58:03Z- handleDepth()-Depth_Keller msg: 1613177883677 epoch sec; seqNo:17343; depth 5.80 2021-02-13T00:58:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177883.69 epoch sec; seqNo:17343; 4060493.96 north; 593450.16 east 2021-02-13T00:58:04Z- AHRS_M2 msg: 1613177884.06 epoch sec; seqNo:17208 2021-02-13T00:58:04Z- AHRS_M2 msg: 0.68 phi; -0.31 theta; -0.23 psi 2021-02-13T00:58:04Z- handleDepth()-Depth_Keller msg: 1613177884065 epoch sec; seqNo:17344; depth 5.91 2021-02-13T00:58:04Z- handleDvl() - RDI_Pathfinder msg: 1613177884.08 epoch sec; seqNo:8373 2021-02-13T00:58:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 113.87 , 84.12 , 121.31 2021-02-13T00:58:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.17 , 0.10 2021-02-13T00:58:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177884.09 epoch sec; seqNo:17344; 4060494.34 north; 593450.15 east 2021-02-13T00:58:04Z- AHRS_M2 msg: 1613177884.46 epoch sec; seqNo:17209 2021-02-13T00:58:04Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.21 psi 2021-02-13T00:58:04Z- handleDepth()-Depth_Keller msg: 1613177884469 epoch sec; seqNo:17345; depth 6.00 2021-02-13T00:58:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177884.48 epoch sec; seqNo:17345; 4060494.72 north; 593450.15 east 2021-02-13T00:58:04Z- AHRS_M2 msg: 1613177884.87 epoch sec; seqNo:17210 2021-02-13T00:58:04Z- AHRS_M2 msg: 0.67 phi; -0.34 theta; -0.20 psi 2021-02-13T00:58:04Z- handleDepth()-Depth_Keller msg: 1613177884877 epoch sec; seqNo:17346; depth 6.10 2021-02-13T00:58:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177884.89 epoch sec; seqNo:17346; 4060495.10 north; 593450.15 east 2021-02-13T00:58:05Z- AHRS_M2 msg: 1613177885.28 epoch sec; seqNo:17211 2021-02-13T00:58:05Z- AHRS_M2 msg: 0.65 phi; -0.35 theta; -0.19 psi 2021-02-13T00:58:05Z- handleDepth()-Depth_Keller msg: 1613177885295 epoch sec; seqNo:17347; depth 6.20 2021-02-13T00:58:05Z- handleDvl() - RDI_Pathfinder msg: 1613177885.31 epoch sec; seqNo:8374 2021-02-13T00:58:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.57 , 112.45 , 82.70 , 119.89 2021-02-13T00:58:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.06 , 0.28 2021-02-13T00:58:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177885.32 epoch sec; seqNo:17347; 4060495.10 north; 593450.15 east 2021-02-13T00:58:05Z- AHRS_M2 msg: 1613177885.68 epoch sec; seqNo:17212 2021-02-13T00:58:05Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.19 psi 2021-02-13T00:58:05Z- handleDepth()-Depth_Keller msg: 1613177885681 epoch sec; seqNo:17348; depth 6.31 2021-02-13T00:58:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177885.69 epoch sec; seqNo:17348; 4060495.49 north; 593451.36 east 2021-02-13T00:58:06Z- AHRS_M2 msg: 1613177886.08 epoch sec; seqNo:17213 2021-02-13T00:58:06Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.19 psi 2021-02-13T00:58:06Z- handleDepth()-Depth_Keller msg: 1613177886084 epoch sec; seqNo:17349; depth 6.40 2021-02-13T00:58:06Z- handleDvl() - RDI_Pathfinder msg: 1613177886.10 epoch sec; seqNo:8375 2021-02-13T00:58:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.41 , 109.74 , 82.97 , 127.59 2021-02-13T00:58:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.32 , 0.20 2021-02-13T00:58:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177886.11 epoch sec; seqNo:17349; 4060495.49 north; 593451.36 east 2021-02-13T00:58:06Z- AHRS_M2 msg: 1613177886.49 epoch sec; seqNo:17214 2021-02-13T00:58:06Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.22 psi 2021-02-13T00:58:06Z- handleDepth()-Depth_Keller msg: 1613177886496 epoch sec; seqNo:17350; depth 6.53 2021-02-13T00:58:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177886.51 epoch sec; seqNo:17350; 4060495.87 north; 593451.36 east 2021-02-13T00:58:06Z- AHRS_M2 msg: 1613177886.90 epoch sec; seqNo:17215 2021-02-13T00:58:06Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.24 psi 2021-02-13T00:58:06Z- handleDepth()-Depth_Keller msg: 1613177886916 epoch sec; seqNo:17351; depth 6.64 2021-02-13T00:58:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177886.94 epoch sec; seqNo:17351; 4060495.87 north; 593451.36 east 2021-02-13T00:58:07Z- AHRS_M2 msg: 1613177887.29 epoch sec; seqNo:17216 2021-02-13T00:58:07Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.24 psi 2021-02-13T00:58:07Z- handleDepth()-Depth_Keller msg: 1613177887298 epoch sec; seqNo:17352; depth 6.76 2021-02-13T00:58:07Z- handleDvl() - RDI_Pathfinder msg: 1613177887.31 epoch sec; seqNo:8376 2021-02-13T00:58:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.41 , 111.23 , 78.88 , 129.08 2021-02-13T00:58:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.31 , -0.05 2021-02-13T00:58:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177887.34 epoch sec; seqNo:17352; 4060496.26 north; 593452.57 east 2021-02-13T00:58:07Z- AHRS_M2 msg: 1613177887.70 epoch sec; seqNo:17217 2021-02-13T00:58:07Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.25 psi 2021-02-13T00:58:07Z- handleDepth()-Depth_Keller msg: 1613177887701 epoch sec; seqNo:17353; depth 6.89 2021-02-13T00:58:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177887.71 epoch sec; seqNo:17353; 4060496.26 north; 593452.57 east 2021-02-13T00:58:08Z- AHRS_M2 msg: 1613177888.10 epoch sec; seqNo:17218 2021-02-13T00:58:08Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.24 psi 2021-02-13T00:58:08Z- handleDepth()-Depth_Keller msg: 1613177888106 epoch sec; seqNo:17354; depth 7.00 2021-02-13T00:58:08Z- handleDvl() - RDI_Pathfinder msg: 1613177888.12 epoch sec; seqNo:8377 2021-02-13T00:58:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 112.79 , 81.56 , 127.67 2021-02-13T00:58:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.13 , -0.23 2021-02-13T00:58:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177888.13 epoch sec; seqNo:17354; 4060496.64 north; 593452.57 east 2021-02-13T00:58:08Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-13T00:58:08Z- waiting for 1613177885.90, time is 1613177888.13 2021-02-13T00:58:08Z- LcmTrn::updateTrn() equating pose 1613177888.13 and meas times 1613177888.13 2021-02-13T00:58:08Z- LcmTrn::updateTrn() >>>> pose time 1613177888.13, meas time 1613177888.13 <<<< 2021-02-13T00:58:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177888.13 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:58:08Z- TerrainNav::motionUpdate() - currEstimate = (4060497, 593452.57, 7.00) 2021-02-13T00:58:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:58:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:58:08Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177888.13 sec.; No relevant navigation data available 2021-02-13T00:58:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:58:08Z- TNavBF::mmse selected from filter -1 2021-02-13T00:58:08Z- variances([0], [2], and psi_berg) = 680.784670 832.988813 0.000000 2021-02-13T00:58:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:58:08Z- LcmTrn::publishEstimates() - MLE : 4060496.14 593472.07 7.00 2021-02-13T00:58:08Z- LcmTrn::publishEstimates() - MMSE : 4060496.64 593452.57 7.00 2021-02-13T00:58:08Z- LcmTrn::publishEstimates() - COVAR: 680.78 832.99 1.30 0.00 2021-02-13T00:58:08Z- AHRS_M2 msg: 1613177888.50 epoch sec; seqNo:17219 2021-02-13T00:58:08Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.25 psi 2021-02-13T00:58:08Z- handleDepth()-Depth_Keller msg: 1613177888509 epoch sec; seqNo:17355; depth 7.13 2021-02-13T00:58:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177888.53 epoch sec; seqNo:17355; 4060497.02 north; 593452.57 east 2021-02-13T00:58:08Z- AHRS_M2 msg: 1613177888.93 epoch sec; seqNo:17220 2021-02-13T00:58:08Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.25 psi 2021-02-13T00:58:08Z- handleDepth()-Depth_Keller msg: 1613177888940 epoch sec; seqNo:17356; depth 7.25 2021-02-13T00:58:08Z- handleDvl() - RDI_Pathfinder msg: 1613177888.96 epoch sec; seqNo:8378 2021-02-13T00:58:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.30 , 117.05 , 79.29 , 130.44 2021-02-13T00:58:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , -0.23 2021-02-13T00:58:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177888.97 epoch sec; seqNo:17356; 4060497.40 north; 593452.56 east 2021-02-13T00:58:09Z- AHRS_M2 msg: 1613177889.32 epoch sec; seqNo:17221 2021-02-13T00:58:09Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.24 psi 2021-02-13T00:58:09Z- handleDepth()-Depth_Keller msg: 1613177889391 epoch sec; seqNo:17357; depth 7.38 2021-02-13T00:58:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177889.40 epoch sec; seqNo:17357; 4060497.79 north; 593453.78 east 2021-02-13T00:58:09Z- AHRS_M2 msg: 1613177889.71 epoch sec; seqNo:17222 2021-02-13T00:58:09Z- AHRS_M2 msg: 0.63 phi; -0.36 theta; -0.22 psi 2021-02-13T00:58:09Z- handleDepth()-Depth_Keller msg: 1613177889736 epoch sec; seqNo:17358; depth 7.53 2021-02-13T00:58:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177889.75 epoch sec; seqNo:17358; 4060498.17 north; 593453.77 east 2021-02-13T00:58:10Z- AHRS_M2 msg: 1613177890.12 epoch sec; seqNo:17223 2021-02-13T00:58:10Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.21 psi 2021-02-13T00:58:10Z- handleDepth()-Depth_Keller msg: 1613177890151 epoch sec; seqNo:17359; depth 7.62 2021-02-13T00:58:10Z- handleDvl() - RDI_Pathfinder msg: 1613177890.16 epoch sec; seqNo:8379 2021-02-13T00:58:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 117.11 , 81.42 , 124.55 2021-02-13T00:58:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.10 , -0.07 2021-02-13T00:58:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177890.17 epoch sec; seqNo:17359; 4060498.17 north; 593453.77 east 2021-02-13T00:58:10Z- AHRS_M2 msg: 1613177890.52 epoch sec; seqNo:17224 2021-02-13T00:58:10Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.19 psi 2021-02-13T00:58:10Z- handleDepth()-Depth_Keller msg: 1613177890549 epoch sec; seqNo:17360; depth 7.76 2021-02-13T00:58:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177890.56 epoch sec; seqNo:17360; 4060498.55 north; 593453.77 east 2021-02-13T00:58:10Z- AHRS_M2 msg: 1613177890.93 epoch sec; seqNo:17225 2021-02-13T00:58:10Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.19 psi 2021-02-13T00:58:10Z- handleDepth()-Depth_Keller msg: 1613177890941 epoch sec; seqNo:17361; depth 7.89 2021-02-13T00:58:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177890.95 epoch sec; seqNo:17361; 4060498.93 north; 593453.77 east 2021-02-13T00:58:11Z- AHRS_M2 msg: 1613177891.33 epoch sec; seqNo:17226 2021-02-13T00:58:11Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.19 psi 2021-02-13T00:58:11Z- handleDepth()-Depth_Keller msg: 1613177891337 epoch sec; seqNo:17362; depth 8.00 2021-02-13T00:58:11Z- handleDvl() - RDI_Pathfinder msg: 1613177891.35 epoch sec; seqNo:8380 2021-02-13T00:58:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.44 , 110.83 , 81.08 , 124.21 2021-02-13T00:58:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , 0.10 2021-02-13T00:58:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177891.38 epoch sec; seqNo:17362; 4060499.31 north; 593453.76 east 2021-02-13T00:58:11Z- AHRS_M2 msg: 1613177891.74 epoch sec; seqNo:17227 2021-02-13T00:58:11Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.19 psi 2021-02-13T00:58:11Z- handleDepth()-Depth_Keller msg: 1613177891804 epoch sec; seqNo:17363; depth 8.16 2021-02-13T00:58:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177891.82 epoch sec; seqNo:17363; 4060499.69 north; 593453.76 east 2021-02-13T00:58:12Z- AHRS_M2 msg: 1613177892.14 epoch sec; seqNo:17228 2021-02-13T00:58:12Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.20 psi 2021-02-13T00:58:12Z- handleDepth()-Depth_Keller msg: 1613177892158 epoch sec; seqNo:17364; depth 8.30 2021-02-13T00:58:12Z- handleDvl() - RDI_Pathfinder msg: 1613177892.17 epoch sec; seqNo:8381 2021-02-13T00:58:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 108.26 , 0.00 , 123.13 2021-02-13T00:58:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.10 , 0.04 2021-02-13T00:58:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177892.18 epoch sec; seqNo:17364; 4060499.70 north; 593454.98 east 2021-02-13T00:58:12Z- AHRS_M2 msg: 1613177892.54 epoch sec; seqNo:17229 2021-02-13T00:58:12Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.21 psi 2021-02-13T00:58:12Z- handleDepth()-Depth_Keller msg: 1613177892569 epoch sec; seqNo:17365; depth 8.44 2021-02-13T00:58:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177892.58 epoch sec; seqNo:17365; 4060500.08 north; 593454.97 east 2021-02-13T00:58:12Z- AHRS_M2 msg: 1613177892.95 epoch sec; seqNo:17230 2021-02-13T00:58:12Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.20 psi 2021-02-13T00:58:12Z- handleDepth()-Depth_Keller msg: 1613177892974 epoch sec; seqNo:17366; depth 8.57 2021-02-13T00:58:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177892.99 epoch sec; seqNo:17366; 4060500.08 north; 593454.97 east 2021-02-13T00:58:13Z- LcmTrn::handleMessages() - handling 40 messages took 0.64 ms and 128 calls... 2021-02-13T00:58:13Z- waiting for 1613177891.13, time is 1613177892.99 2021-02-13T00:58:13Z- LcmTrn::updateTrn() >>>> pose time 1613177892.99, meas time 1613177892.17 <<<< 2021-02-13T00:58:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177892.17 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:58:13Z- TerrainNav::motionUpdate() - currEstimate = (4060500, 593454.97, 8.57) 2021-02-13T00:58:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:58:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:58:13Z- TNavBF::Projecting Measurements 2021-02-13T00:58:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:58:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:58:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:58:13Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:58:13Z- TNavFilter::3 good beams. 2021-02-13T00:58:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:58:13Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:58:13Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:58:13Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:58:13Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:58:13Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:58:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:58:13Z- TNavBF::mmse selected from filter -1 2021-02-13T00:58:13Z- TerrainMapOctree: (vehN, vehE ) = (4060499.50, 593454.57). 2021-02-13T00:58:13Z- TerrainMapOctree: Min Distance = 1093.60. 2021-02-13T00:58:13Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:58:13Z- There is no specific good beam that all particles have in common. 2021-02-13T00:58:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:58:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.48 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-13T00:58:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:58:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.36 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 18.75 2021-02-13T00:58:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:58:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 24.26 2021-02-13T00:58:13Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:58:13Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:58:13Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:58:13Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:58:13Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:58:13Z- Filter 0 No beams used in subcloud update 2021-02-13T00:58:13Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:58:13Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:58:13Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 680.7 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:58:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177892.17 sec. 2021-02-13T00:58:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:58:13Z- TNavBF::mmse selected from filter -1 2021-02-13T00:58:13Z- variances([0], [2], and psi_berg) = 680.724782 832.988787 0.000000 2021-02-13T00:58:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:58:13Z- LcmTrn::publishEstimates() - MLE : 4060499.58 593473.47 8.57 2021-02-13T00:58:13Z- LcmTrn::publishEstimates() - MMSE : 4060500.08 593454.97 8.57 2021-02-13T00:58:13Z- LcmTrn::publishEstimates() - COVAR: 680.72 832.99 1.25 0.00 2021-02-13T00:58:13Z- AHRS_M2 msg: 1613177893.36 epoch sec; seqNo:17231 2021-02-13T00:58:13Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.20 psi 2021-02-13T00:58:13Z- handleDepth()-Depth_Keller msg: 1613177893365 epoch sec; seqNo:17367; depth 8.72 2021-02-13T00:58:13Z- handleDvl() - RDI_Pathfinder msg: 1613177893.38 epoch sec; seqNo:8382 2021-02-13T00:58:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.31 , 106.16 , 79.39 , 122.52 2021-02-13T00:58:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.06 , -0.09 2021-02-13T00:58:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177893.39 epoch sec; seqNo:17367; 4060500.46 north; 593454.97 east 2021-02-13T00:58:13Z- AHRS_M2 msg: 1613177893.76 epoch sec; seqNo:17232 2021-02-13T00:58:13Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.19 psi 2021-02-13T00:58:13Z- handleDepth()-Depth_Keller msg: 1613177893767 epoch sec; seqNo:17368; depth 8.87 2021-02-13T00:58:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177893.78 epoch sec; seqNo:17368; 4060500.84 north; 593454.97 east 2021-02-13T00:58:14Z- AHRS_M2 msg: 1613177894.16 epoch sec; seqNo:17233 2021-02-13T00:58:14Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.19 psi 2021-02-13T00:58:14Z- handleDepth()-Depth_Keller msg: 1613177894173 epoch sec; seqNo:17369; depth 9.02 2021-02-13T00:58:14Z- handleDvl() - RDI_Pathfinder msg: 1613177894.18 epoch sec; seqNo:8383 2021-02-13T00:58:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 107.92 , 78.06 , 121.44 2021-02-13T00:58:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , -0.10 2021-02-13T00:58:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177894.20 epoch sec; seqNo:17369; 4060501.21 north; 593454.96 east 2021-02-13T00:58:14Z- AHRS_M2 msg: 1613177894.57 epoch sec; seqNo:17234 2021-02-13T00:58:14Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.19 psi 2021-02-13T00:58:14Z- handleDepth()-Depth_Keller msg: 1613177894578 epoch sec; seqNo:17370; depth 9.16 2021-02-13T00:58:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177894.59 epoch sec; seqNo:17370; 4060501.23 north; 593456.18 east 2021-02-13T00:58:14Z- AHRS_M2 msg: 1613177894.97 epoch sec; seqNo:17235 2021-02-13T00:58:14Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.19 psi 2021-02-13T00:58:14Z- handleDepth()-Depth_Keller msg: 1613177894978 epoch sec; seqNo:17371; depth 9.30 2021-02-13T00:58:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177894.99 epoch sec; seqNo:17371; 4060501.61 north; 593456.18 east 2021-02-13T00:58:15Z- AHRS_M2 msg: 1613177895.38 epoch sec; seqNo:17236 2021-02-13T00:58:15Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.20 psi 2021-02-13T00:58:15Z- handleDepth()-Depth_Keller msg: 1613177895385 epoch sec; seqNo:17372; depth 9.44 2021-02-13T00:58:15Z- handleDvl() - RDI_Pathfinder msg: 1613177895.40 epoch sec; seqNo:8384 2021-02-13T00:58:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.01 , 106.84 , 78.58 , 121.71 2021-02-13T00:58:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , -0.05 2021-02-13T00:58:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177895.42 epoch sec; seqNo:17372; 4060501.99 north; 593456.17 east 2021-02-13T00:58:15Z- AHRS_M2 msg: 1613177895.78 epoch sec; seqNo:17237 2021-02-13T00:58:15Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.21 psi 2021-02-13T00:58:15Z- handleDepth()-Depth_Keller msg: 1613177895789 epoch sec; seqNo:17373; depth 9.59 2021-02-13T00:58:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177895.80 epoch sec; seqNo:17373; 4060502.36 north; 593456.17 east 2021-02-13T00:58:16Z- AHRS_M2 msg: 1613177896.19 epoch sec; seqNo:17238 2021-02-13T00:58:16Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.22 psi 2021-02-13T00:58:16Z- handleDepth()-Depth_Keller msg: 1613177896193 epoch sec; seqNo:17374; depth 9.71 2021-02-13T00:58:16Z- handleDvl() - RDI_Pathfinder msg: 1613177896.20 epoch sec; seqNo:8385 2021-02-13T00:58:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 106.63 , 78.24 , 118.80 2021-02-13T00:58:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , 0.07 2021-02-13T00:58:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177896.21 epoch sec; seqNo:17374; 4060502.36 north; 593456.17 east 2021-02-13T00:58:16Z- AHRS_M2 msg: 1613177896.59 epoch sec; seqNo:17239 2021-02-13T00:58:16Z- AHRS_M2 msg: 0.56 phi; -0.33 theta; -0.22 psi 2021-02-13T00:58:16Z- handleDepth()-Depth_Keller msg: 1613177896593 epoch sec; seqNo:17375; depth 9.85 2021-02-13T00:58:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177896.61 epoch sec; seqNo:17375; 4060502.74 north; 593456.16 east 2021-02-13T00:58:16Z- AHRS_M2 msg: 1613177896.99 epoch sec; seqNo:17240 2021-02-13T00:58:16Z- AHRS_M2 msg: 0.55 phi; -0.33 theta; -0.22 psi 2021-02-13T00:58:16Z- handleDepth()-Depth_Keller msg: 1613177896997 epoch sec; seqNo:17376; depth 9.99 2021-02-13T00:58:17Z- handleDvl() - RDI_Pathfinder msg: 1613177897.01 epoch sec; seqNo:8386 2021-02-13T00:58:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:58:17Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:58:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177897.03 epoch sec; seqNo:17376; 4060503.12 north; 593456.16 east 2021-02-13T00:58:17Z- AHRS_M2 msg: 1613177897.40 epoch sec; seqNo:17241 2021-02-13T00:58:17Z- AHRS_M2 msg: 0.55 phi; -0.33 theta; -0.21 psi 2021-02-13T00:58:17Z- handleDepth()-Depth_Keller msg: 1613177897401 epoch sec; seqNo:17377; depth 10.13 2021-02-13T00:58:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177897.41 epoch sec; seqNo:17377; 4060503.51 north; 593457.38 east 2021-02-13T00:58:17Z- AHRS_M2 msg: 1613177897.80 epoch sec; seqNo:17242 2021-02-13T00:58:17Z- AHRS_M2 msg: 0.55 phi; -0.33 theta; -0.20 psi 2021-02-13T00:58:17Z- handleDepth()-Depth_Keller msg: 1613177897806 epoch sec; seqNo:17378; depth 10.24 2021-02-13T00:58:17Z- handleDvl() - RDI_Pathfinder msg: 1613177897.82 epoch sec; seqNo:8387 2021-02-13T00:58:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:58:17Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:58:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177897.83 epoch sec; seqNo:17378; 4060503.51 north; 593457.38 east 2021-02-13T00:58:18Z- AHRS_M2 msg: 1613177898.20 epoch sec; seqNo:17243 2021-02-13T00:58:18Z- AHRS_M2 msg: 0.55 phi; -0.32 theta; -0.19 psi 2021-02-13T00:58:18Z- handleDepth()-Depth_Keller msg: 1613177898213 epoch sec; seqNo:17379; depth 10.37 2021-02-13T00:58:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177898.23 epoch sec; seqNo:17379; 4060503.89 north; 593457.37 east 2021-02-13T00:58:18Z- LcmTrn::handleMessages() - handling 45 messages took 0.67 ms and 132 calls... 2021-02-13T00:58:18Z- waiting for 1613177895.99, time is 1613177898.23 2021-02-13T00:58:18Z- LcmTrn::updateTrn() >>>> pose time 1613177898.23, meas time 1613177897.82 <<<< 2021-02-13T00:58:18Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177897.82, ping # 0. 2021-02-13T00:58:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177897.82 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:58:18Z- TerrainNav::motionUpdate() - currEstimate = (4060504, 593457.37, 10.37) 2021-02-13T00:58:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:58:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:58:18Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:58:18Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:58:18Z- TNavBF::Projecting Measurements 2021-02-13T00:58:18Z- TNavFilter::Measurement from time = 1613177897.82 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:58:18Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:58:18Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:58:18Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:58:18Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:58:18Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:58:18Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:58:18Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:58:18Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 680.7 EastVariance: 833.0 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-13T00:58:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:58:18Z- TNavBF::mmse selected from filter -1 2021-02-13T00:58:18Z- variances([0], [2], and psi_berg) = 680.666563 832.988762 0.000000 2021-02-13T00:58:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:58:18Z- LcmTrn::publishEstimates() - MLE : 4060503.39 593475.87 10.37 2021-02-13T00:58:18Z- LcmTrn::publishEstimates() - MMSE : 4060503.89 593457.37 10.37 2021-02-13T00:58:18Z- LcmTrn::publishEstimates() - COVAR: 680.67 832.99 1.48 0.00 2021-02-13T00:58:18Z- AHRS_M2 msg: 1613177898.61 epoch sec; seqNo:17244 2021-02-13T00:58:18Z- AHRS_M2 msg: 0.55 phi; -0.32 theta; -0.18 psi 2021-02-13T00:58:18Z- handleDepth()-Depth_Keller msg: 1613177898613 epoch sec; seqNo:17380; depth 10.51 2021-02-13T00:58:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177898.62 epoch sec; seqNo:17380; 4060504.27 north; 593457.37 east 2021-02-13T00:58:19Z- AHRS_M2 msg: 1613177899.03 epoch sec; seqNo:17245 2021-02-13T00:58:19Z- AHRS_M2 msg: 0.56 phi; -0.31 theta; -0.17 psi 2021-02-13T00:58:19Z- handleDepth()-Depth_Keller msg: 1613177899042 epoch sec; seqNo:17381; depth 10.64 2021-02-13T00:58:19Z- handleDvl() - RDI_Pathfinder msg: 1613177899.06 epoch sec; seqNo:8388 2021-02-13T00:58:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 102.71 , 77.24 , 114.88 2021-02-13T00:58:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.25 , -0.02 2021-02-13T00:58:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177899.08 epoch sec; seqNo:17381; 4060504.65 north; 593457.36 east 2021-02-13T00:58:19Z- AHRS_M2 msg: 1613177899.42 epoch sec; seqNo:17246 2021-02-13T00:58:19Z- AHRS_M2 msg: 0.57 phi; -0.31 theta; -0.17 psi 2021-02-13T00:58:19Z- handleDepth()-Depth_Keller msg: 1613177899426 epoch sec; seqNo:17382; depth 10.80 2021-02-13T00:58:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177899.44 epoch sec; seqNo:17382; 4060504.65 north; 593457.36 east 2021-02-13T00:58:19Z- AHRS_M2 msg: 1613177899.82 epoch sec; seqNo:17247 2021-02-13T00:58:19Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.16 psi 2021-02-13T00:58:19Z- handleDepth()-Depth_Keller msg: 1613177899824 epoch sec; seqNo:17383; depth 10.91 2021-02-13T00:58:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177899.85 epoch sec; seqNo:17383; 4060505.04 north; 593458.58 east 2021-02-13T00:58:20Z- AHRS_M2 msg: 1613177900.23 epoch sec; seqNo:17248 2021-02-13T00:58:20Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.16 psi 2021-02-13T00:58:20Z- handleDepth()-Depth_Keller msg: 1613177900232 epoch sec; seqNo:17384; depth 11.02 2021-02-13T00:58:20Z- handleDvl() - RDI_Pathfinder msg: 1613177900.24 epoch sec; seqNo:8389 2021-02-13T00:58:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.50 , 78.51 , 110.96 2021-02-13T00:58:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.17 , -0.16 2021-02-13T00:58:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177900.26 epoch sec; seqNo:17384; 4060505.42 north; 593458.58 east 2021-02-13T00:58:20Z- AHRS_M2 msg: 1613177900.63 epoch sec; seqNo:17249 2021-02-13T00:58:20Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.16 psi 2021-02-13T00:58:20Z- handleDepth()-Depth_Keller msg: 1613177900633 epoch sec; seqNo:17385; depth 11.15 2021-02-13T00:58:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177900.64 epoch sec; seqNo:17385; 4060505.80 north; 593458.57 east 2021-02-13T00:58:21Z- AHRS_M2 msg: 1613177901.03 epoch sec; seqNo:17250 2021-02-13T00:58:21Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.16 psi 2021-02-13T00:58:21Z- handleDepth()-Depth_Keller msg: 1613177901039 epoch sec; seqNo:17386; depth 11.25 2021-02-13T00:58:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177901.05 epoch sec; seqNo:17386; 4060505.80 north; 593458.57 east 2021-02-13T00:58:21Z- AHRS_M2 msg: 1613177901.44 epoch sec; seqNo:17251 2021-02-13T00:58:21Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.16 psi 2021-02-13T00:58:21Z- handleDepth()-Depth_Keller msg: 1613177901441 epoch sec; seqNo:17387; depth 11.37 2021-02-13T00:58:21Z- handleDvl() - RDI_Pathfinder msg: 1613177901.46 epoch sec; seqNo:8390 2021-02-13T00:58:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.78 , 100.01 , 78.37 , 112.18 2021-02-13T00:58:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , -0.11 2021-02-13T00:58:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177901.47 epoch sec; seqNo:17387; 4060506.18 north; 593458.57 east 2021-02-13T00:58:21Z- AHRS_M2 msg: 1613177901.84 epoch sec; seqNo:17252 2021-02-13T00:58:21Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.15 psi 2021-02-13T00:58:21Z- handleDepth()-Depth_Keller msg: 1613177901845 epoch sec; seqNo:17388; depth 11.48 2021-02-13T00:58:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177901.86 epoch sec; seqNo:17388; 4060506.57 north; 593459.79 east 2021-02-13T00:58:22Z- AHRS_M2 msg: 1613177902.24 epoch sec; seqNo:17253 2021-02-13T00:58:22Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.14 psi 2021-02-13T00:58:22Z- handleDepth()-Depth_Keller msg: 1613177902250 epoch sec; seqNo:17389; depth 11.58 2021-02-13T00:58:22Z- handleDvl() - RDI_Pathfinder msg: 1613177902.26 epoch sec; seqNo:8391 2021-02-13T00:58:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:58:22Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:58:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177902.27 epoch sec; seqNo:17389; 4060506.95 north; 593459.78 east 2021-02-13T00:58:22Z- AHRS_M2 msg: 1613177902.65 epoch sec; seqNo:17254 2021-02-13T00:58:22Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.14 psi 2021-02-13T00:58:22Z- handleDepth()-Depth_Keller msg: 1613177902721 epoch sec; seqNo:17390; depth 11.68 2021-02-13T00:58:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177902.73 epoch sec; seqNo:17390; 4060507.33 north; 593459.78 east 2021-02-13T00:58:23Z- AHRS_M2 msg: 1613177903.08 epoch sec; seqNo:17255 2021-02-13T00:58:23Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.14 psi 2021-02-13T00:58:23Z- handleDepth()-Depth_Keller msg: 1613177903095 epoch sec; seqNo:17391; depth 11.80 2021-02-13T00:58:23Z- handleDvl() - RDI_Pathfinder msg: 1613177903.11 epoch sec; seqNo:8392 2021-02-13T00:58:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.74 , 97.64 , 74.79 , 108.05 2021-02-13T00:58:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.14 , 0.11 2021-02-13T00:58:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177903.13 epoch sec; seqNo:17391; 4060507.33 north; 593459.78 east 2021-02-13T00:58:23Z- LcmTrn::handleMessages() - handling 41 messages took 0.69 ms and 128 calls... 2021-02-13T00:58:23Z- waiting for 1613177901.23, time is 1613177903.13 2021-02-13T00:58:23Z- LcmTrn::updateTrn() equating pose 1613177903.13 and meas times 1613177903.13 2021-02-13T00:58:23Z- LcmTrn::updateTrn() >>>> pose time 1613177903.13, meas time 1613177903.13 <<<< 2021-02-13T00:58:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177903.13 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:58:23Z- TerrainNav::motionUpdate() - currEstimate = (4060507, 593459.78, 11.80) 2021-02-13T00:58:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:58:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:58:23Z- TNavBF::Projecting Measurements 2021-02-13T00:58:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:58:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:58:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:58:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:58:23Z- TNavFilter::4 good beams. 2021-02-13T00:58:23Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:58:23Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:58:23Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:58:23Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:58:23Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:58:23Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:58:23Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:58:23Z- TNavBF::mmse selected from filter -1 2021-02-13T00:58:23Z- TerrainMapOctree: (vehN, vehE ) = (4060507.33, 593459.78). 2021-02-13T00:58:23Z- TerrainMapOctree: Min Distance = 1084.44. 2021-02-13T00:58:23Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:58:23Z- There is no specific good beam that all particles have in common. 2021-02-13T00:58:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:58:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.69 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 11.76 2021-02-13T00:58:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:58:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.50 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 15.25 2021-02-13T00:58:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:58:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.115 ExpectedMeasDiff: nan currMeas.covariance: 8.95 2021-02-13T00:58:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:58:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 18.68 2021-02-13T00:58:23Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:58:23Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:58:23Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:58:23Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:58:23Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:58:23Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:58:23Z- Filter 0 No beams used in subcloud update 2021-02-13T00:58:23Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:58:23Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:58:23Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 680.6 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:58:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177903.13 sec. 2021-02-13T00:58:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:58:23Z- TNavBF::mmse selected from filter -1 2021-02-13T00:58:23Z- variances([0], [2], and psi_berg) = 680.621313 832.988743 0.000000 2021-02-13T00:58:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:58:23Z- LcmTrn::publishEstimates() - MLE : 4060506.83 593478.28 11.80 2021-02-13T00:58:23Z- LcmTrn::publishEstimates() - MMSE : 4060507.33 593459.78 11.80 2021-02-13T00:58:23Z- LcmTrn::publishEstimates() - COVAR: 680.62 832.99 1.15 0.00 2021-02-13T00:58:23Z- handleDepth()-Depth_Keller msg: 1613177903493 epoch sec; seqNo:17392; depth 11.89 2021-02-13T00:58:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177903.50 epoch sec; seqNo:17392; 4060507.71 north; 593459.77 east 2021-02-13T00:58:23Z- AHRS_M2 msg: 1613177903.87 epoch sec; seqNo:17256 2021-02-13T00:58:23Z- AHRS_M2 msg: 0.59 phi; -0.27 theta; -0.15 psi 2021-02-13T00:58:23Z- handleDepth()-Depth_Keller msg: 1613177903893 epoch sec; seqNo:17393; depth 11.98 2021-02-13T00:58:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:58:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:58:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:58:23Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:58:23Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:58:23Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:58:23Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:58:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:58:23Z- handleDvl() - RDI_Pathfinder msg: 1613177903.89 epoch sec; seqNo:8393 2021-02-13T00:58:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:58:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.14 , 0.11 2021-02-13T00:58:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177903.91 epoch sec; seqNo:17393; 4060508.10 north; 593460.99 east 2021-02-13T00:58:24Z- AHRS_M2 msg: 1613177904.27 epoch sec; seqNo:17257 2021-02-13T00:58:24Z- AHRS_M2 msg: 0.60 phi; -0.27 theta; -0.16 psi 2021-02-13T00:58:24Z- handleDepth()-Depth_Keller msg: 1613177904270 epoch sec; seqNo:17394; depth 12.08 2021-02-13T00:58:24Z- handleDvl() - RDI_Pathfinder msg: 1613177904.28 epoch sec; seqNo:8394 2021-02-13T00:58:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.26 , 97.64 , 78.17 , 108.60 2021-02-13T00:58:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , 0.20 , -0.02 2021-02-13T00:58:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177904.29 epoch sec; seqNo:17394; 4060508.10 north; 593460.99 east 2021-02-13T00:58:24Z- AHRS_M2 msg: 1613177904.67 epoch sec; seqNo:17258 2021-02-13T00:58:24Z- AHRS_M2 msg: 0.60 phi; -0.26 theta; -0.17 psi 2021-02-13T00:58:24Z- handleDepth()-Depth_Keller msg: 1613177904682 epoch sec; seqNo:17395; depth 12.18 2021-02-13T00:58:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177904.69 epoch sec; seqNo:17395; 4060508.48 north; 593460.98 east 2021-02-13T00:58:25Z- AHRS_M2 msg: 1613177905.07 epoch sec; seqNo:17259 2021-02-13T00:58:25Z- AHRS_M2 msg: 0.61 phi; -0.26 theta; -0.18 psi 2021-02-13T00:58:25Z- handleDepth()-Depth_Keller msg: 1613177905077 epoch sec; seqNo:17396; depth 12.26 2021-02-13T00:58:25Z- handleDvl() - RDI_Pathfinder msg: 1613177905.09 epoch sec; seqNo:8395 2021-02-13T00:58:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.48 , 97.71 , 78.78 , 105.82 2021-02-13T00:58:25Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , 0.08 , -0.21 2021-02-13T00:58:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177905.10 epoch sec; seqNo:17396; 4060508.86 north; 593460.98 east 2021-02-13T00:58:25Z- AHRS_M2 msg: 1613177905.48 epoch sec; seqNo:17260 2021-02-13T00:58:25Z- AHRS_M2 msg: 0.62 phi; -0.26 theta; -0.19 psi 2021-02-13T00:58:25Z- handleDepth()-Depth_Keller msg: 1613177905545 epoch sec; seqNo:17397; depth 12.37 2021-02-13T00:58:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177905.56 epoch sec; seqNo:17397; 4060509.25 north; 593462.20 east 2021-02-13T00:58:25Z- AHRS_M2 msg: 1613177905.88 epoch sec; seqNo:17261 2021-02-13T00:58:25Z- AHRS_M2 msg: 0.63 phi; -0.25 theta; -0.19 psi 2021-02-13T00:58:25Z- handleDepth()-Depth_Keller msg: 1613177905908 epoch sec; seqNo:17398; depth 12.48 2021-02-13T00:58:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177905.93 epoch sec; seqNo:17398; 4060509.63 north; 593462.19 east 2021-02-13T00:58:26Z- AHRS_M2 msg: 1613177906.28 epoch sec; seqNo:17262 2021-02-13T00:58:26Z- AHRS_M2 msg: 0.63 phi; -0.26 theta; -0.19 psi 2021-02-13T00:58:26Z- handleDepth()-Depth_Keller msg: 1613177906310 epoch sec; seqNo:17399; depth 12.56 2021-02-13T00:58:26Z- handleDvl() - RDI_Pathfinder msg: 1613177906.32 epoch sec; seqNo:8396 2021-02-13T00:58:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 98.86 , 78.17 , 102.51 2021-02-13T00:58:26Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.11 , -0.24 2021-02-13T00:58:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177906.33 epoch sec; seqNo:17399; 4060510.01 north; 593462.19 east 2021-02-13T00:58:26Z- AHRS_M2 msg: 1613177906.69 epoch sec; seqNo:17263 2021-02-13T00:58:26Z- AHRS_M2 msg: 0.64 phi; -0.26 theta; -0.18 psi 2021-02-13T00:58:26Z- handleDepth()-Depth_Keller msg: 1613177906713 epoch sec; seqNo:17400; depth 12.67 2021-02-13T00:58:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177906.74 epoch sec; seqNo:17400; 4060510.39 north; 593462.18 east 2021-02-13T00:58:27Z- AHRS_M2 msg: 1613177907.09 epoch sec; seqNo:17264 2021-02-13T00:58:27Z- AHRS_M2 msg: 0.64 phi; -0.27 theta; -0.17 psi 2021-02-13T00:58:27Z- handleDepth()-Depth_Keller msg: 1613177907101 epoch sec; seqNo:17401; depth 12.76 2021-02-13T00:58:27Z- handleDvl() - RDI_Pathfinder msg: 1613177907.11 epoch sec; seqNo:8397 2021-02-13T00:58:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 101.43 , 74.79 , 103.86 2021-02-13T00:58:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.23 , -0.08 2021-02-13T00:58:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177907.12 epoch sec; seqNo:17401; 4060510.77 north; 593462.18 east 2021-02-13T00:58:27Z- AHRS_M2 msg: 1613177907.50 epoch sec; seqNo:17265 2021-02-13T00:58:27Z- AHRS_M2 msg: 0.63 phi; -0.28 theta; -0.16 psi 2021-02-13T00:58:27Z- handleDepth()-Depth_Keller msg: 1613177907501 epoch sec; seqNo:17402; depth 12.86 2021-02-13T00:58:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:58:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:58:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:58:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:58:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:58:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:58:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:58:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:58:27Z- handleDvl() - RDI_Pathfinder msg: 1613177907.50 epoch sec; seqNo:8398 2021-02-13T00:58:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:58:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.23 , -0.08 2021-02-13T00:58:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177907.52 epoch sec; seqNo:17402; 4060511.14 north; 593462.18 east 2021-02-13T00:58:27Z- AHRS_M2 msg: 1613177907.90 epoch sec; seqNo:17266 2021-02-13T00:58:27Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.16 psi 2021-02-13T00:58:27Z- handleDepth()-Depth_Keller msg: 1613177907905 epoch sec; seqNo:17403; depth 12.93 2021-02-13T00:58:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177907.91 epoch sec; seqNo:17403; 4060511.52 north; 593462.17 east 2021-02-13T00:58:28Z- AHRS_M2 msg: 1613177908.30 epoch sec; seqNo:17267 2021-02-13T00:58:28Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.17 psi 2021-02-13T00:58:28Z- handleDepth()-Depth_Keller msg: 1613177908309 epoch sec; seqNo:17404; depth 13.02 2021-02-13T00:58:28Z- handleDvl() - RDI_Pathfinder msg: 1613177908.32 epoch sec; seqNo:8399 2021-02-13T00:58:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.89 , 97.71 , 78.24 , 106.23 2021-02-13T00:58:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.15 , 0.13 2021-02-13T00:58:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177908.33 epoch sec; seqNo:17404; 4060511.52 north; 593462.17 east 2021-02-13T00:58:28Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-13T00:58:28Z- waiting for 1613177906.13, time is 1613177908.33 2021-02-13T00:58:28Z- LcmTrn::updateTrn() equating pose 1613177908.33 and meas times 1613177908.33 2021-02-13T00:58:28Z- LcmTrn::updateTrn() >>>> pose time 1613177908.33, meas time 1613177908.33 <<<< 2021-02-13T00:58:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177908.33 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:58:28Z- TerrainNav::motionUpdate() - currEstimate = (4060512, 593462.17, 13.02) 2021-02-13T00:58:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:58:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:58:28Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177908.33 sec.; No relevant navigation data available 2021-02-13T00:58:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:58:28Z- TNavBF::mmse selected from filter -1 2021-02-13T00:58:28Z- variances([0], [2], and psi_berg) = 680.572774 832.988722 0.000000 2021-02-13T00:58:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:58:28Z- LcmTrn::publishEstimates() - MLE : 4060511.02 593480.67 13.02 2021-02-13T00:58:28Z- LcmTrn::publishEstimates() - MMSE : 4060511.52 593462.17 13.02 2021-02-13T00:58:28Z- LcmTrn::publishEstimates() - COVAR: 680.57 832.99 1.40 0.00 2021-02-13T00:58:28Z- AHRS_M2 msg: 1613177908.71 epoch sec; seqNo:17268 2021-02-13T00:58:28Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.19 psi 2021-02-13T00:58:28Z- handleDepth()-Depth_Keller msg: 1613177908715 epoch sec; seqNo:17405; depth 13.12 2021-02-13T00:58:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:58:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:58:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:58:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:58:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:58:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:58:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:58:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:58:28Z- handleDvl() - RDI_Pathfinder msg: 1613177908.72 epoch sec; seqNo:8400 2021-02-13T00:58:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:58:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.15 , 0.13 2021-02-13T00:58:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177908.73 epoch sec; seqNo:17405; 4060511.92 north; 593463.39 east 2021-02-13T00:58:29Z- AHRS_M2 msg: 1613177909.11 epoch sec; seqNo:17269 2021-02-13T00:58:29Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.22 psi 2021-02-13T00:58:29Z- handleDepth()-Depth_Keller msg: 1613177909117 epoch sec; seqNo:17406; depth 13.22 2021-02-13T00:58:29Z- handleDvl() - RDI_Pathfinder msg: 1613177909.14 epoch sec; seqNo:8401 2021-02-13T00:58:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 98.86 , 73.97 , 108.60 2021-02-13T00:58:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.04 , 0.25 2021-02-13T00:58:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177909.16 epoch sec; seqNo:17406; 4060511.92 north; 593463.39 east 2021-02-13T00:58:29Z- AHRS_M2 msg: 1613177909.52 epoch sec; seqNo:17270 2021-02-13T00:58:29Z- AHRS_M2 msg: 0.57 phi; -0.31 theta; -0.24 psi 2021-02-13T00:58:29Z- handleDepth()-Depth_Keller msg: 1613177909521 epoch sec; seqNo:17407; depth 13.31 2021-02-13T00:58:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177909.53 epoch sec; seqNo:17407; 4060512.29 north; 593463.38 east 2021-02-13T00:58:29Z- AHRS_M2 msg: 1613177909.92 epoch sec; seqNo:17271 2021-02-13T00:58:29Z- AHRS_M2 msg: 0.56 phi; -0.31 theta; -0.25 psi 2021-02-13T00:58:29Z- handleDepth()-Depth_Keller msg: 1613177909925 epoch sec; seqNo:17408; depth 13.42 2021-02-13T00:58:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:58:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:58:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:58:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:58:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:58:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:58:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:58:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:58:29Z- handleDvl() - RDI_Pathfinder msg: 1613177909.93 epoch sec; seqNo:8402 2021-02-13T00:58:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:58:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.04 , 0.25 2021-02-13T00:58:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177909.93 epoch sec; seqNo:17408; 4060512.29 north; 593463.38 east 2021-02-13T00:58:30Z- AHRS_M2 msg: 1613177910.32 epoch sec; seqNo:17272 2021-02-13T00:58:30Z- AHRS_M2 msg: 0.56 phi; -0.31 theta; -0.26 psi 2021-02-13T00:58:30Z- handleDepth()-Depth_Keller msg: 1613177910329 epoch sec; seqNo:17409; depth 13.52 2021-02-13T00:58:30Z- handleDvl() - RDI_Pathfinder msg: 1613177910.35 epoch sec; seqNo:8403 2021-02-13T00:58:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.88 , 105.02 , 78.24 , 112.32 2021-02-13T00:58:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.21 , 0.22 2021-02-13T00:58:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177910.37 epoch sec; seqNo:17409; 4060512.67 north; 593463.38 east 2021-02-13T00:58:30Z- AHRS_M2 msg: 1613177910.73 epoch sec; seqNo:17273 2021-02-13T00:58:30Z- AHRS_M2 msg: 0.56 phi; -0.31 theta; -0.25 psi 2021-02-13T00:58:30Z- handleDepth()-Depth_Keller msg: 1613177910732 epoch sec; seqNo:17410; depth 13.63 2021-02-13T00:58:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177910.74 epoch sec; seqNo:17410; 4060513.05 north; 593463.38 east 2021-02-13T00:58:31Z- AHRS_M2 msg: 1613177911.13 epoch sec; seqNo:17274 2021-02-13T00:58:31Z- AHRS_M2 msg: 0.56 phi; -0.31 theta; -0.25 psi 2021-02-13T00:58:31Z- handleDepth()-Depth_Keller msg: 1613177911140 epoch sec; seqNo:17411; depth 13.74 2021-02-13T00:58:31Z- handleDvl() - RDI_Pathfinder msg: 1613177911.15 epoch sec; seqNo:8404 2021-02-13T00:58:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.29 , 105.69 , 78.65 , 112.46 2021-02-13T00:58:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.25 , 0.09 2021-02-13T00:58:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177911.18 epoch sec; seqNo:17411; 4060513.07 north; 593464.60 east 2021-02-13T00:58:31Z- AHRS_M2 msg: 1613177911.54 epoch sec; seqNo:17275 2021-02-13T00:58:31Z- AHRS_M2 msg: 0.57 phi; -0.31 theta; -0.23 psi 2021-02-13T00:58:31Z- handleDepth()-Depth_Keller msg: 1613177911545 epoch sec; seqNo:17412; depth 13.85 2021-02-13T00:58:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177911.57 epoch sec; seqNo:17412; 4060513.44 north; 593464.59 east 2021-02-13T00:58:31Z- AHRS_M2 msg: 1613177911.94 epoch sec; seqNo:17276 2021-02-13T00:58:31Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.22 psi 2021-02-13T00:58:31Z- handleDepth()-Depth_Keller msg: 1613177911946 epoch sec; seqNo:17413; depth 13.95 2021-02-13T00:58:31Z- handleDvl() - RDI_Pathfinder msg: 1613177911.96 epoch sec; seqNo:8405 2021-02-13T00:58:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.47 , 102.98 , 73.15 , 112.45 2021-02-13T00:58:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.19 , -0.03 2021-02-13T00:58:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177911.97 epoch sec; seqNo:17413; 4060513.82 north; 593464.59 east 2021-02-13T00:58:32Z- AHRS_M2 msg: 1613177912.34 epoch sec; seqNo:17277 2021-02-13T00:58:32Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.21 psi 2021-02-13T00:58:32Z- handleDepth()-Depth_Keller msg: 1613177912349 epoch sec; seqNo:17414; depth 14.08 2021-02-13T00:58:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:58:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:58:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:58:32Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:58:32Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:58:32Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:58:32Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:58:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:58:32Z- handleDvl() - RDI_Pathfinder msg: 1613177912.35 epoch sec; seqNo:8406 2021-02-13T00:58:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:58:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.19 , -0.03 2021-02-13T00:58:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177912.36 epoch sec; seqNo:17414; 4060513.82 north; 593464.59 east 2021-02-13T00:58:32Z- AHRS_M2 msg: 1613177912.76 epoch sec; seqNo:17278 2021-02-13T00:58:32Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.20 psi 2021-02-13T00:58:32Z- handleDepth()-Depth_Keller msg: 1613177912759 epoch sec; seqNo:17415; depth 14.18 2021-02-13T00:58:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177912.77 epoch sec; seqNo:17415; 4060514.21 north; 593465.80 east 2021-02-13T00:58:33Z- AHRS_M2 msg: 1613177913.15 epoch sec; seqNo:17279 2021-02-13T00:58:33Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.19 psi 2021-02-13T00:58:33Z- handleDepth()-Depth_Keller msg: 1613177913165 epoch sec; seqNo:17416; depth 14.29 2021-02-13T00:58:33Z- handleDvl() - RDI_Pathfinder msg: 1613177913.18 epoch sec; seqNo:8407 2021-02-13T00:58:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.13 , 100.42 , 74.79 , 108.53 2021-02-13T00:58:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.07 , -0.10 2021-02-13T00:58:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177913.19 epoch sec; seqNo:17416; 4060514.59 north; 593465.80 east 2021-02-13T00:58:33Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-13T00:58:33Z- waiting for 1613177911.33, time is 1613177913.19 2021-02-13T00:58:33Z- LcmTrn::updateTrn() equating pose 1613177913.19 and meas times 1613177913.19 2021-02-13T00:58:33Z- LcmTrn::updateTrn() >>>> pose time 1613177913.19, meas time 1613177913.19 <<<< 2021-02-13T00:58:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177913.19 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:58:33Z- TerrainNav::motionUpdate() - currEstimate = (4060515, 593465.80, 14.29) 2021-02-13T00:58:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:58:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:58:33Z- TNavBF::Projecting Measurements 2021-02-13T00:58:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:58:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:58:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:58:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:58:33Z- TNavFilter::4 good beams. 2021-02-13T00:58:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:58:33Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:58:33Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:58:33Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:58:33Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:58:33Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:58:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:58:33Z- TNavBF::mmse selected from filter -1 2021-02-13T00:58:33Z- TerrainMapOctree: (vehN, vehE ) = (4060514.59, 593465.80). 2021-02-13T00:58:33Z- TerrainMapOctree: Min Distance = 1075.08. 2021-02-13T00:58:33Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:58:33Z- There is no specific good beam that all particles have in common. 2021-02-13T00:58:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:58:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.64 K: 0.120 ExpectedMeasDiff: nan currMeas.covariance: 10.27 2021-02-13T00:58:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:58:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.45 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 16.13 2021-02-13T00:58:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:58:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 8.95 2021-02-13T00:58:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:58:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 18.84 2021-02-13T00:58:33Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:58:33Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:58:33Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:58:33Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:58:33Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:58:33Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:58:33Z- Filter 0 No beams used in subcloud update 2021-02-13T00:58:33Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:58:33Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:58:33Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 680.5 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:58:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177913.19 sec. 2021-02-13T00:58:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:58:33Z- TNavBF::mmse selected from filter -1 2021-02-13T00:58:33Z- variances([0], [2], and psi_berg) = 680.524644 832.988701 0.000000 2021-02-13T00:58:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:58:33Z- LcmTrn::publishEstimates() - MLE : 4060514.09 593484.30 14.29 2021-02-13T00:58:33Z- LcmTrn::publishEstimates() - MMSE : 4060514.59 593465.80 14.29 2021-02-13T00:58:33Z- LcmTrn::publishEstimates() - COVAR: 680.52 832.99 1.12 0.00 2021-02-13T00:58:33Z- AHRS_M2 msg: 1613177913.56 epoch sec; seqNo:17280 2021-02-13T00:58:33Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.18 psi 2021-02-13T00:58:33Z- handleDepth()-Depth_Keller msg: 1613177913562 epoch sec; seqNo:17417; depth 14.41 2021-02-13T00:58:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177913.57 epoch sec; seqNo:17417; 4060514.97 north; 593465.80 east 2021-02-13T00:58:33Z- AHRS_M2 msg: 1613177913.96 epoch sec; seqNo:17281 2021-02-13T00:58:33Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.18 psi 2021-02-13T00:58:33Z- handleDepth()-Depth_Keller msg: 1613177913969 epoch sec; seqNo:17418; depth 14.52 2021-02-13T00:58:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177914.02 epoch sec; seqNo:17418; 4060515.35 north; 593465.79 east 2021-02-13T00:58:34Z- AHRS_M2 msg: 1613177914.36 epoch sec; seqNo:17282 2021-02-13T00:58:34Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.17 psi 2021-02-13T00:58:34Z- handleDepth()-Depth_Keller msg: 1613177914369 epoch sec; seqNo:17419; depth 14.63 2021-02-13T00:58:34Z- handleDvl() - RDI_Pathfinder msg: 1613177914.38 epoch sec; seqNo:8408 2021-02-13T00:58:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.69 , 98.86 , 78.17 , 108.60 2021-02-13T00:58:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.00 , -0.08 2021-02-13T00:58:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177914.41 epoch sec; seqNo:17419; 4060515.35 north; 593465.79 east 2021-02-13T00:58:34Z- AHRS_M2 msg: 1613177914.77 epoch sec; seqNo:17283 2021-02-13T00:58:34Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.17 psi 2021-02-13T00:58:34Z- handleDepth()-Depth_Keller msg: 1613177914773 epoch sec; seqNo:17420; depth 14.76 2021-02-13T00:58:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177914.78 epoch sec; seqNo:17420; 4060515.74 north; 593467.01 east 2021-02-13T00:58:35Z- AHRS_M2 msg: 1613177915.17 epoch sec; seqNo:17284 2021-02-13T00:58:35Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.17 psi 2021-02-13T00:58:35Z- handleDepth()-Depth_Keller msg: 1613177915185 epoch sec; seqNo:17421; depth 14.87 2021-02-13T00:58:35Z- handleDvl() - RDI_Pathfinder msg: 1613177915.20 epoch sec; seqNo:8409 2021-02-13T00:58:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.64 , 71.52 , 109.81 2021-02-13T00:58:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.05 , -0.01 2021-02-13T00:58:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177915.21 epoch sec; seqNo:17421; 4060516.12 north; 593467.00 east 2021-02-13T00:58:35Z- AHRS_M2 msg: 1613177915.58 epoch sec; seqNo:17285 2021-02-13T00:58:35Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-13T00:58:35Z- handleDepth()-Depth_Keller msg: 1613177915581 epoch sec; seqNo:17422; depth 14.97 2021-02-13T00:58:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177915.60 epoch sec; seqNo:17422; 4060516.50 north; 593467.00 east 2021-02-13T00:58:35Z- AHRS_M2 msg: 1613177915.99 epoch sec; seqNo:17286 2021-02-13T00:58:35Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-13T00:58:36Z- handleDepth()-Depth_Keller msg: 1613177916072 epoch sec; seqNo:17423; depth 15.09 2021-02-13T00:58:36Z- handleDvl() - RDI_Pathfinder msg: 1613177916.08 epoch sec; seqNo:8410 2021-02-13T00:58:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.42 , 97.64 , 72.75 , 108.46 2021-02-13T00:58:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , 0.05 2021-02-13T00:58:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177916.10 epoch sec; seqNo:17423; 4060516.88 north; 593467.00 east 2021-02-13T00:58:36Z- AHRS_M2 msg: 1613177916.38 epoch sec; seqNo:17287 2021-02-13T00:58:36Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.17 psi 2021-02-13T00:58:36Z- handleDepth()-Depth_Keller msg: 1613177916400 epoch sec; seqNo:17424; depth 15.23 2021-02-13T00:58:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177916.41 epoch sec; seqNo:17424; 4060516.88 north; 593467.00 east 2021-02-13T00:58:36Z- AHRS_M2 msg: 1613177916.79 epoch sec; seqNo:17288 2021-02-13T00:58:36Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.17 psi 2021-02-13T00:58:36Z- handleDepth()-Depth_Keller msg: 1613177916813 epoch sec; seqNo:17425; depth 15.32 2021-02-13T00:58:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177916.82 epoch sec; seqNo:17425; 4060517.26 north; 593466.99 east 2021-02-13T00:58:37Z- AHRS_M2 msg: 1613177917.19 epoch sec; seqNo:17289 2021-02-13T00:58:37Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.17 psi 2021-02-13T00:58:37Z- handleDepth()-Depth_Keller msg: 1613177917213 epoch sec; seqNo:17426; depth 15.43 2021-02-13T00:58:37Z- handleDvl() - RDI_Pathfinder msg: 1613177917.23 epoch sec; seqNo:8411 2021-02-13T00:58:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 96.56 , 78.31 , 112.38 2021-02-13T00:58:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.08 , 0.04 2021-02-13T00:58:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177917.24 epoch sec; seqNo:17426; 4060517.65 north; 593468.21 east 2021-02-13T00:58:37Z- AHRS_M2 msg: 1613177917.60 epoch sec; seqNo:17290 2021-02-13T00:58:37Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-13T00:58:37Z- handleDepth()-Depth_Keller msg: 1613177917601 epoch sec; seqNo:17427; depth 15.56 2021-02-13T00:58:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177917.61 epoch sec; seqNo:17427; 4060518.03 north; 593468.20 east 2021-02-13T00:58:38Z- AHRS_M2 msg: 1613177918.00 epoch sec; seqNo:17291 2021-02-13T00:58:38Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.16 psi 2021-02-13T00:58:38Z- handleDepth()-Depth_Keller msg: 1613177918005 epoch sec; seqNo:17428; depth 15.68 2021-02-13T00:58:38Z- handleDvl() - RDI_Pathfinder msg: 1613177918.02 epoch sec; seqNo:8412 2021-02-13T00:58:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.29 , 97.71 , 72.34 , 108.53 2021-02-13T00:58:38Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.08 , -0.06 2021-02-13T00:58:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177918.03 epoch sec; seqNo:17428; 4060518.41 north; 593468.20 east 2021-02-13T00:58:38Z- AHRS_M2 msg: 1613177918.41 epoch sec; seqNo:17292 2021-02-13T00:58:38Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.17 psi 2021-02-13T00:58:38Z- handleDepth()-Depth_Keller msg: 1613177918425 epoch sec; seqNo:17429; depth 15.81 2021-02-13T00:58:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177918.44 epoch sec; seqNo:17429; 4060518.41 north; 593468.20 east 2021-02-13T00:58:38Z- LcmTrn::handleMessages() - handling 44 messages took 0.76 ms and 129 calls... 2021-02-13T00:58:38Z- waiting for 1613177916.19, time is 1613177918.44 2021-02-13T00:58:38Z- LcmTrn::updateTrn() >>>> pose time 1613177918.44, meas time 1613177918.02 <<<< 2021-02-13T00:58:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177918.02 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:58:38Z- TerrainNav::motionUpdate() - currEstimate = (4060518, 593468.20, 15.81) 2021-02-13T00:58:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:58:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:58:38Z- TNavBF::Projecting Measurements 2021-02-13T00:58:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:58:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:58:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:58:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:58:38Z- TNavFilter::4 good beams. 2021-02-13T00:58:38Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:58:38Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:58:38Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:58:38Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:58:38Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:58:38Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:58:38Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:58:38Z- TNavBF::mmse selected from filter -1 2021-02-13T00:58:38Z- TerrainMapOctree: (vehN, vehE ) = (4060518.10, 593468.01). 2021-02-13T00:58:38Z- TerrainMapOctree: Min Distance = 1071.07. 2021-02-13T00:58:38Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:58:38Z- There is no specific good beam that all particles have in common. 2021-02-13T00:58:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:58:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 10.06 2021-02-13T00:58:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:58:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T00:58:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:58:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-13T00:58:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:58:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 18.84 2021-02-13T00:58:38Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:58:38Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:58:38Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:58:38Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:58:38Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:58:38Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:58:38Z- Filter 0 No beams used in subcloud update 2021-02-13T00:58:38Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:58:38Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:58:38Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 680.5 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:58:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177918.02 sec. 2021-02-13T00:58:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:58:38Z- TNavBF::mmse selected from filter -1 2021-02-13T00:58:38Z- variances([0], [2], and psi_berg) = 680.466466 832.988677 0.000000 2021-02-13T00:58:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:58:38Z- LcmTrn::publishEstimates() - MLE : 4060517.91 593487.70 15.81 2021-02-13T00:58:38Z- LcmTrn::publishEstimates() - MMSE : 4060518.41 593468.20 15.81 2021-02-13T00:58:38Z- LcmTrn::publishEstimates() - COVAR: 680.47 832.99 0.98 0.00 2021-02-13T00:58:38Z- AHRS_M2 msg: 1613177918.81 epoch sec; seqNo:17293 2021-02-13T00:58:38Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.17 psi 2021-02-13T00:58:38Z- handleDepth()-Depth_Keller msg: 1613177918879 epoch sec; seqNo:17430; depth 15.92 2021-02-13T00:58:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177918.89 epoch sec; seqNo:17430; 4060518.79 north; 593468.20 east 2021-02-13T00:58:39Z- AHRS_M2 msg: 1613177919.21 epoch sec; seqNo:17294 2021-02-13T00:58:39Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.17 psi 2021-02-13T00:58:39Z- handleDepth()-Depth_Keller msg: 1613177919225 epoch sec; seqNo:17431; depth 16.07 2021-02-13T00:58:39Z- handleDvl() - RDI_Pathfinder msg: 1613177919.25 epoch sec; seqNo:8413 2021-02-13T00:58:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 97.78 , 78.31 , 108.73 2021-02-13T00:58:39Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.03 , -0.13 2021-02-13T00:58:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177919.27 epoch sec; seqNo:17431; 4060519.18 north; 593469.41 east 2021-02-13T00:58:39Z- AHRS_M2 msg: 1613177919.62 epoch sec; seqNo:17295 2021-02-13T00:58:39Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.17 psi 2021-02-13T00:58:39Z- handleDepth()-Depth_Keller msg: 1613177919643 epoch sec; seqNo:17432; depth 16.19 2021-02-13T00:58:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177919.66 epoch sec; seqNo:17432; 4060519.56 north; 593469.41 east 2021-02-13T00:58:40Z- AHRS_M2 msg: 1613177920.02 epoch sec; seqNo:17296 2021-02-13T00:58:40Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.17 psi 2021-02-13T00:58:40Z- handleDepth()-Depth_Keller msg: 1613177920041 epoch sec; seqNo:17433; depth 16.30 2021-02-13T00:58:40Z- handleDvl() - RDI_Pathfinder msg: 1613177920.05 epoch sec; seqNo:8414 2021-02-13T00:58:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.24 , 97.64 , 69.48 , 108.60 2021-02-13T00:58:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.14 , -0.09 2021-02-13T00:58:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177920.06 epoch sec; seqNo:17433; 4060519.94 north; 593469.40 east 2021-02-13T00:58:40Z- AHRS_M2 msg: 1613177920.42 epoch sec; seqNo:17297 2021-02-13T00:58:40Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.18 psi 2021-02-13T00:58:40Z- handleDepth()-Depth_Keller msg: 1613177920429 epoch sec; seqNo:17434; depth 16.44 2021-02-13T00:58:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177920.45 epoch sec; seqNo:17434; 4060520.32 north; 593469.40 east 2021-02-13T00:58:40Z- AHRS_M2 msg: 1613177920.83 epoch sec; seqNo:17298 2021-02-13T00:58:40Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.18 psi 2021-02-13T00:58:40Z- handleDepth()-Depth_Keller msg: 1613177920839 epoch sec; seqNo:17435; depth 16.55 2021-02-13T00:58:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177920.85 epoch sec; seqNo:17435; 4060520.70 north; 593469.40 east 2021-02-13T00:58:41Z- AHRS_M2 msg: 1613177921.24 epoch sec; seqNo:17299 2021-02-13T00:58:41Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.18 psi 2021-02-13T00:58:41Z- handleDepth()-Depth_Keller msg: 1613177921242 epoch sec; seqNo:17436; depth 16.68 2021-02-13T00:58:41Z- handleDvl() - RDI_Pathfinder msg: 1613177921.25 epoch sec; seqNo:8415 2021-02-13T00:58:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.91 , 71.11 , 108.86 2021-02-13T00:58:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.18 , 0.02 2021-02-13T00:58:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177921.26 epoch sec; seqNo:17436; 4060521.07 north; 593469.39 east 2021-02-13T00:58:41Z- AHRS_M2 msg: 1613177921.65 epoch sec; seqNo:17300 2021-02-13T00:58:41Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.18 psi 2021-02-13T00:58:41Z- handleDepth()-Depth_Keller msg: 1613177921657 epoch sec; seqNo:17437; depth 16.81 2021-02-13T00:58:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177921.67 epoch sec; seqNo:17437; 4060521.07 north; 593469.39 east 2021-02-13T00:58:42Z- AHRS_M2 msg: 1613177922.04 epoch sec; seqNo:17301 2021-02-13T00:58:42Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.18 psi 2021-02-13T00:58:42Z- handleDepth()-Depth_Keller msg: 1613177922048 epoch sec; seqNo:17438; depth 16.92 2021-02-13T00:58:42Z- handleDvl() - RDI_Pathfinder msg: 1613177922.06 epoch sec; seqNo:8416 2021-02-13T00:58:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.85 , 78.37 , 107.58 2021-02-13T00:58:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , 0.14 2021-02-13T00:58:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177922.08 epoch sec; seqNo:17438; 4060521.47 north; 593470.61 east 2021-02-13T00:58:42Z- AHRS_M2 msg: 1613177922.45 epoch sec; seqNo:17302 2021-02-13T00:58:42Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:58:42Z- handleDepth()-Depth_Keller msg: 1613177922453 epoch sec; seqNo:17439; depth 17.05 2021-02-13T00:58:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177922.46 epoch sec; seqNo:17439; 4060521.85 north; 593470.60 east 2021-02-13T00:58:42Z- AHRS_M2 msg: 1613177922.85 epoch sec; seqNo:17303 2021-02-13T00:58:42Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.18 psi 2021-02-13T00:58:42Z- handleDepth()-Depth_Keller msg: 1613177922857 epoch sec; seqNo:17440; depth 17.18 2021-02-13T00:58:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177922.87 epoch sec; seqNo:17440; 4060522.22 north; 593470.60 east 2021-02-13T00:58:43Z- AHRS_M2 msg: 1613177923.26 epoch sec; seqNo:17304 2021-02-13T00:58:43Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.18 psi 2021-02-13T00:58:43Z- handleDepth()-Depth_Keller msg: 1613177923261 epoch sec; seqNo:17441; depth 17.30 2021-02-13T00:58:43Z- handleDvl() - RDI_Pathfinder msg: 1613177923.31 epoch sec; seqNo:8417 2021-02-13T00:58:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.93 , 97.51 , 71.11 , 106.97 2021-02-13T00:58:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.03 , 0.18 2021-02-13T00:58:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177923.32 epoch sec; seqNo:17441; 4060522.22 north; 593470.60 east 2021-02-13T00:58:43Z- AHRS_M2 msg: 1613177923.66 epoch sec; seqNo:17305 2021-02-13T00:58:43Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.18 psi 2021-02-13T00:58:43Z- handleDepth()-Depth_Keller msg: 1613177923664 epoch sec; seqNo:17442; depth 17.43 2021-02-13T00:58:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177923.67 epoch sec; seqNo:17442; 4060522.60 north; 593470.60 east 2021-02-13T00:58:43Z- LcmTrn::handleMessages() - handling 44 messages took 0.80 ms and 128 calls... 2021-02-13T00:58:43Z- waiting for 1613177921.44, time is 1613177923.67 2021-02-13T00:58:43Z- LcmTrn::updateTrn() >>>> pose time 1613177923.67, meas time 1613177923.31 <<<< 2021-02-13T00:58:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177923.31 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:58:43Z- TerrainNav::motionUpdate() - currEstimate = (4060523, 593470.60, 17.43) 2021-02-13T00:58:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:58:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:58:43Z- TNavBF::Projecting Measurements 2021-02-13T00:58:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:58:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:58:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:58:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:58:43Z- TNavFilter::4 good beams. 2021-02-13T00:58:43Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:58:43Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:58:43Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:58:43Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:58:43Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:58:43Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:58:43Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:58:43Z- TNavBF::mmse selected from filter -1 2021-02-13T00:58:43Z- TerrainMapOctree: (vehN, vehE ) = (4060522.31, 593470.43). 2021-02-13T00:58:43Z- TerrainMapOctree: Min Distance = 1066.43. 2021-02-13T00:58:43Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:58:43Z- There is no specific good beam that all particles have in common. 2021-02-13T00:58:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:58:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 10.22 2021-02-13T00:58:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:58:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 15.21 2021-02-13T00:58:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:58:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-13T00:58:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:58:43Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 18.31 2021-02-13T00:58:43Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:58:43Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:58:43Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:58:43Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:58:43Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:58:43Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:58:43Z- Filter 0 No beams used in subcloud update 2021-02-13T00:58:43Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:58:43Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:58:43Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 680.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:58:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177923.31 sec. 2021-02-13T00:58:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:58:43Z- TNavBF::mmse selected from filter -1 2021-02-13T00:58:43Z- variances([0], [2], and psi_berg) = 680.406887 832.988651 0.000000 2021-02-13T00:58:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:58:43Z- LcmTrn::publishEstimates() - MLE : 4060522.10 593490.10 17.43 2021-02-13T00:58:43Z- LcmTrn::publishEstimates() - MMSE : 4060522.60 593470.60 17.43 2021-02-13T00:58:43Z- LcmTrn::publishEstimates() - COVAR: 680.41 832.99 0.91 0.00 2021-02-13T00:58:44Z- AHRS_M2 msg: 1613177924.08 epoch sec; seqNo:17306 2021-02-13T00:58:44Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.19 psi 2021-02-13T00:58:44Z- handleDepth()-Depth_Keller msg: 1613177924089 epoch sec; seqNo:17443; depth 17.55 2021-02-13T00:58:44Z- handleDvl() - RDI_Pathfinder msg: 1613177924.11 epoch sec; seqNo:8418 2021-02-13T00:58:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 97.85 , 78.38 , 106.37 2021-02-13T00:58:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.15 , 0.13 2021-02-13T00:58:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177924.12 epoch sec; seqNo:17443; 4060522.60 north; 593470.60 east 2021-02-13T00:58:44Z- AHRS_M2 msg: 1613177924.47 epoch sec; seqNo:17307 2021-02-13T00:58:44Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.19 psi 2021-02-13T00:58:44Z- handleDepth()-Depth_Keller msg: 1613177924478 epoch sec; seqNo:17444; depth 17.68 2021-02-13T00:58:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177924.49 epoch sec; seqNo:17444; 4060522.98 north; 593470.59 east 2021-02-13T00:58:44Z- AHRS_M2 msg: 1613177924.87 epoch sec; seqNo:17308 2021-02-13T00:58:44Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.19 psi 2021-02-13T00:58:44Z- handleDepth()-Depth_Keller msg: 1613177924886 epoch sec; seqNo:17445; depth 17.78 2021-02-13T00:58:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177924.91 epoch sec; seqNo:17445; 4060523.37 north; 593471.81 east 2021-02-13T00:58:45Z- AHRS_M2 msg: 1613177925.28 epoch sec; seqNo:17309 2021-02-13T00:58:45Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.19 psi 2021-02-13T00:58:45Z- handleDepth()-Depth_Keller msg: 1613177925280 epoch sec; seqNo:17446; depth 17.91 2021-02-13T00:58:45Z- handleDvl() - RDI_Pathfinder msg: 1613177925.29 epoch sec; seqNo:8419 2021-02-13T00:58:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.71 , 70.30 , 108.67 2021-02-13T00:58:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.18 , 0.02 2021-02-13T00:58:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177925.31 epoch sec; seqNo:17446; 4060523.37 north; 593471.81 east 2021-02-13T00:58:45Z- AHRS_M2 msg: 1613177925.68 epoch sec; seqNo:17310 2021-02-13T00:58:45Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-13T00:58:45Z- handleDepth()-Depth_Keller msg: 1613177925685 epoch sec; seqNo:17447; depth 18.02 2021-02-13T00:58:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177925.69 epoch sec; seqNo:17447; 4060523.75 north; 593471.80 east 2021-02-13T00:58:46Z- AHRS_M2 msg: 1613177926.09 epoch sec; seqNo:17311 2021-02-13T00:58:46Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.20 psi 2021-02-13T00:58:46Z- handleDepth()-Depth_Keller msg: 1613177926097 epoch sec; seqNo:17448; depth 18.15 2021-02-13T00:58:46Z- handleDvl() - RDI_Pathfinder msg: 1613177926.11 epoch sec; seqNo:8420 2021-02-13T00:58:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 97.71 , 68.66 , 107.18 2021-02-13T00:58:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.15 , -0.05 2021-02-13T00:58:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177926.12 epoch sec; seqNo:17448; 4060524.13 north; 593471.80 east 2021-02-13T00:58:46Z- AHRS_M2 msg: 1613177926.49 epoch sec; seqNo:17312 2021-02-13T00:58:46Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.21 psi 2021-02-13T00:58:46Z- handleDepth()-Depth_Keller msg: 1613177926493 epoch sec; seqNo:17449; depth 18.26 2021-02-13T00:58:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177926.51 epoch sec; seqNo:17449; 4060524.13 north; 593471.80 east 2021-02-13T00:58:46Z- AHRS_M2 msg: 1613177926.89 epoch sec; seqNo:17313 2021-02-13T00:58:46Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.21 psi 2021-02-13T00:58:46Z- handleDepth()-Depth_Keller msg: 1613177926897 epoch sec; seqNo:17450; depth 18.37 2021-02-13T00:58:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177926.90 epoch sec; seqNo:17450; 4060524.52 north; 593473.02 east 2021-02-13T00:58:47Z- AHRS_M2 msg: 1613177927.30 epoch sec; seqNo:17314 2021-02-13T00:58:47Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.21 psi 2021-02-13T00:58:47Z- handleDepth()-Depth_Keller msg: 1613177927304 epoch sec; seqNo:17451; depth 18.48 2021-02-13T00:58:47Z- handleDvl() - RDI_Pathfinder msg: 1613177927.32 epoch sec; seqNo:8421 2021-02-13T00:58:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.98 , 69.48 , 106.50 2021-02-13T00:58:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.07 , -0.06 2021-02-13T00:58:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177927.33 epoch sec; seqNo:17451; 4060524.90 north; 593473.01 east 2021-02-13T00:58:47Z- AHRS_M2 msg: 1613177927.70 epoch sec; seqNo:17315 2021-02-13T00:58:47Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.20 psi 2021-02-13T00:58:47Z- handleDepth()-Depth_Keller msg: 1613177927706 epoch sec; seqNo:17452; depth 18.60 2021-02-13T00:58:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177927.72 epoch sec; seqNo:17452; 4060525.28 north; 593473.01 east 2021-02-13T00:58:48Z- AHRS_M2 msg: 1613177928.10 epoch sec; seqNo:17316 2021-02-13T00:58:48Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.20 psi 2021-02-13T00:58:48Z- handleDepth()-Depth_Keller msg: 1613177928109 epoch sec; seqNo:17453; depth 18.72 2021-02-13T00:58:48Z- handleDvl() - RDI_Pathfinder msg: 1613177928.13 epoch sec; seqNo:8422 2021-02-13T00:58:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.20 , 97.91 , 70.70 , 105.21 2021-02-13T00:58:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.00 , -0.05 2021-02-13T00:58:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177928.14 epoch sec; seqNo:17453; 4060525.66 north; 593473.00 east 2021-02-13T00:58:48Z- AHRS_M2 msg: 1613177928.51 epoch sec; seqNo:17317 2021-02-13T00:58:48Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.20 psi 2021-02-13T00:58:48Z- handleDepth()-Depth_Keller msg: 1613177928513 epoch sec; seqNo:17454; depth 18.83 2021-02-13T00:58:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177928.52 epoch sec; seqNo:17454; 4060525.66 north; 593473.00 east 2021-02-13T00:58:48Z- LcmTrn::handleMessages() - handling 41 messages took 0.66 ms and 128 calls... 2021-02-13T00:58:48Z- waiting for 1613177926.67, time is 1613177928.52 2021-02-13T00:58:48Z- LcmTrn::updateTrn() >>>> pose time 1613177928.52, meas time 1613177928.13 <<<< 2021-02-13T00:58:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177928.13 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:58:48Z- TerrainNav::motionUpdate() - currEstimate = (4060526, 593473.00, 18.83) 2021-02-13T00:58:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:58:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:58:48Z- TNavBF::Projecting Measurements 2021-02-13T00:58:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:58:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:58:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:58:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:58:48Z- TNavFilter::4 good beams. 2021-02-13T00:58:48Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:58:48Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:58:48Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:58:48Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:58:48Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:58:48Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:58:48Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:58:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:58:48Z- TerrainMapOctree: (vehN, vehE ) = (4060525.41, 593472.81). 2021-02-13T00:58:48Z- TerrainMapOctree: Min Distance = 1062.57. 2021-02-13T00:58:48Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:58:48Z- There is no specific good beam that all particles have in common. 2021-02-13T00:58:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:58:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 9.05 2021-02-13T00:58:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:58:48Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T00:58:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:58:48Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-13T00:58:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:58:48Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 17.71 2021-02-13T00:58:48Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:58:48Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:58:48Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:58:48Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:58:48Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:58:48Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:58:48Z- Filter 0 No beams used in subcloud update 2021-02-13T00:58:48Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:58:48Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:58:48Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 680.4 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:58:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177928.13 sec. 2021-02-13T00:58:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:58:48Z- TNavBF::mmse selected from filter -1 2021-02-13T00:58:48Z- variances([0], [2], and psi_berg) = 680.352818 832.988628 0.000000 2021-02-13T00:58:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:58:48Z- LcmTrn::publishEstimates() - MLE : 4060525.16 593492.50 18.83 2021-02-13T00:58:48Z- LcmTrn::publishEstimates() - MMSE : 4060525.66 593473.00 18.83 2021-02-13T00:58:48Z- LcmTrn::publishEstimates() - COVAR: 680.35 832.99 0.83 0.00 2021-02-13T00:58:48Z- AHRS_M2 msg: 1613177928.93 epoch sec; seqNo:17318 2021-02-13T00:58:48Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.20 psi 2021-02-13T00:58:49Z- handleDepth()-Depth_Keller msg: 1613177929005 epoch sec; seqNo:17455; depth 18.95 2021-02-13T00:58:49Z- handleDvl() - RDI_Pathfinder msg: 1613177929.02 epoch sec; seqNo:8423 2021-02-13T00:58:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 97.98 , 68.66 , 107.72 2021-02-13T00:58:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.02 , -0.04 2021-02-13T00:58:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177929.03 epoch sec; seqNo:17455; 4060526.05 north; 593474.22 east 2021-02-13T00:58:49Z- AHRS_M2 msg: 1613177929.32 epoch sec; seqNo:17319 2021-02-13T00:58:49Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-13T00:58:49Z- handleDepth()-Depth_Keller msg: 1613177929328 epoch sec; seqNo:17456; depth 19.10 2021-02-13T00:58:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177929.36 epoch sec; seqNo:17456; 4060526.43 north; 593474.22 east 2021-02-13T00:58:49Z- AHRS_M2 msg: 1613177929.72 epoch sec; seqNo:17320 2021-02-13T00:58:49Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.18 psi 2021-02-13T00:58:49Z- handleDepth()-Depth_Keller msg: 1613177929745 epoch sec; seqNo:17457; depth 19.19 2021-02-13T00:58:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177929.77 epoch sec; seqNo:17457; 4060526.81 north; 593474.21 east 2021-02-13T00:58:50Z- AHRS_M2 msg: 1613177930.13 epoch sec; seqNo:17321 2021-02-13T00:58:50Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.18 psi 2021-02-13T00:58:50Z- handleDepth()-Depth_Keller msg: 1613177930134 epoch sec; seqNo:17458; depth 19.33 2021-02-13T00:58:50Z- handleDvl() - RDI_Pathfinder msg: 1613177930.15 epoch sec; seqNo:8424 2021-02-13T00:58:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.39 , 97.99 , 78.51 , 105.29 2021-02-13T00:58:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.07 , -0.01 2021-02-13T00:58:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177930.16 epoch sec; seqNo:17458; 4060527.19 north; 593474.21 east 2021-02-13T00:58:50Z- AHRS_M2 msg: 1613177930.53 epoch sec; seqNo:17322 2021-02-13T00:58:50Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.17 psi 2021-02-13T00:58:50Z- handleDepth()-Depth_Keller msg: 1613177930553 epoch sec; seqNo:17459; depth 19.45 2021-02-13T00:58:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177930.56 epoch sec; seqNo:17459; 4060527.57 north; 593474.20 east 2021-02-13T00:58:50Z- AHRS_M2 msg: 1613177930.93 epoch sec; seqNo:17323 2021-02-13T00:58:50Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.16 psi 2021-02-13T00:58:50Z- handleDepth()-Depth_Keller msg: 1613177930941 epoch sec; seqNo:17460; depth 19.57 2021-02-13T00:58:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177930.95 epoch sec; seqNo:17460; 4060527.57 north; 593474.20 east 2021-02-13T00:58:51Z- AHRS_M2 msg: 1613177931.34 epoch sec; seqNo:17324 2021-02-13T00:58:51Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.16 psi 2021-02-13T00:58:51Z- handleDepth()-Depth_Keller msg: 1613177931341 epoch sec; seqNo:17461; depth 19.67 2021-02-13T00:58:51Z- handleDvl() - RDI_Pathfinder msg: 1613177931.35 epoch sec; seqNo:8425 2021-02-13T00:58:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 97.85 , 78.38 , 103.94 2021-02-13T00:58:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.07 , 0.04 2021-02-13T00:58:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177931.36 epoch sec; seqNo:17461; 4060527.96 north; 593475.42 east 2021-02-13T00:58:51Z- AHRS_M2 msg: 1613177931.74 epoch sec; seqNo:17325 2021-02-13T00:58:51Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.17 psi 2021-02-13T00:58:51Z- handleDepth()-Depth_Keller msg: 1613177931753 epoch sec; seqNo:17462; depth 19.79 2021-02-13T00:58:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177931.76 epoch sec; seqNo:17462; 4060528.34 north; 593475.42 east 2021-02-13T00:58:52Z- AHRS_M2 msg: 1613177932.15 epoch sec; seqNo:17326 2021-02-13T00:58:52Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:58:52Z- handleDepth()-Depth_Keller msg: 1613177932158 epoch sec; seqNo:17463; depth 19.91 2021-02-13T00:58:52Z- handleDvl() - RDI_Pathfinder msg: 1613177932.17 epoch sec; seqNo:8426 2021-02-13T00:58:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.78 , 68.25 , 100.21 2021-02-13T00:58:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , 0.05 2021-02-13T00:58:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177932.18 epoch sec; seqNo:17463; 4060528.72 north; 593475.41 east 2021-02-13T00:58:52Z- AHRS_M2 msg: 1613177932.55 epoch sec; seqNo:17327 2021-02-13T00:58:52Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T00:58:52Z- handleDepth()-Depth_Keller msg: 1613177932616 epoch sec; seqNo:17464; depth 20.01 2021-02-13T00:58:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177932.63 epoch sec; seqNo:17464; 4060529.10 north; 593475.41 east 2021-02-13T00:58:52Z- AHRS_M2 msg: 1613177932.95 epoch sec; seqNo:17328 2021-02-13T00:58:52Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.17 psi 2021-02-13T00:58:52Z- handleDepth()-Depth_Keller msg: 1613177932968 epoch sec; seqNo:17465; depth 20.16 2021-02-13T00:58:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177933.01 epoch sec; seqNo:17465; 4060529.10 north; 593475.41 east 2021-02-13T00:58:53Z- AHRS_M2 msg: 1613177933.36 epoch sec; seqNo:17329 2021-02-13T00:58:53Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.16 psi 2021-02-13T00:58:53Z- handleDepth()-Depth_Keller msg: 1613177933374 epoch sec; seqNo:17466; depth 20.26 2021-02-13T00:58:53Z- handleDvl() - RDI_Pathfinder msg: 1613177933.39 epoch sec; seqNo:8427 2021-02-13T00:58:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.37 , 91.76 , 67.03 , 103.93 2021-02-13T00:58:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , 0.06 2021-02-13T00:58:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177933.40 epoch sec; seqNo:17466; 4060529.48 north; 593475.40 east 2021-02-13T00:58:53Z- AHRS_M2 msg: 1613177933.76 epoch sec; seqNo:17330 2021-02-13T00:58:53Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.17 psi 2021-02-13T00:58:53Z- handleDepth()-Depth_Keller msg: 1613177933785 epoch sec; seqNo:17467; depth 20.36 2021-02-13T00:58:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177933.80 epoch sec; seqNo:17467; 4060529.85 north; 593475.40 east 2021-02-13T00:58:53Z- LcmTrn::handleMessages() - handling 44 messages took 0.78 ms and 129 calls... 2021-02-13T00:58:53Z- waiting for 1613177931.52, time is 1613177933.80 2021-02-13T00:58:53Z- LcmTrn::updateTrn() >>>> pose time 1613177933.80, meas time 1613177933.39 <<<< 2021-02-13T00:58:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177933.39 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:58:53Z- TerrainNav::motionUpdate() - currEstimate = (4060530, 593475.40, 20.36) 2021-02-13T00:58:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:58:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:58:53Z- TNavBF::Projecting Measurements 2021-02-13T00:58:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:58:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:58:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:58:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:58:53Z- TNavFilter::4 good beams. 2021-02-13T00:58:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:58:53Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:58:53Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:58:53Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:58:53Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:58:53Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:58:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:58:53Z- TNavBF::mmse selected from filter -1 2021-02-13T00:58:53Z- TerrainMapOctree: (vehN, vehE ) = (4060529.53, 593475.21). 2021-02-13T00:58:53Z- TerrainMapOctree: Min Distance = 1058.01. 2021-02-13T00:58:53Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:58:53Z- There is no specific good beam that all particles have in common. 2021-02-13T00:58:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:58:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.084 ExpectedMeasDiff: nan currMeas.covariance: 9.83 2021-02-13T00:58:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:58:53Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 13.47 2021-02-13T00:58:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:58:53Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 7.19 2021-02-13T00:58:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:58:53Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 17.28 2021-02-13T00:58:53Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:58:53Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:58:54Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:58:54Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:58:54Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:58:54Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:58:54Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:58:54Z- Filter 0 No beams used in subcloud update 2021-02-13T00:58:54Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:58:54Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:58:54Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 680.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:58:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177933.39 sec. 2021-02-13T00:58:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:58:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:58:54Z- variances([0], [2], and psi_berg) = 680.292841 832.988603 0.000000 2021-02-13T00:58:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:58:54Z- LcmTrn::publishEstimates() - MLE : 4060529.35 593493.90 20.36 2021-02-13T00:58:54Z- LcmTrn::publishEstimates() - MMSE : 4060529.85 593475.40 20.36 2021-02-13T00:58:54Z- LcmTrn::publishEstimates() - COVAR: 680.29 832.99 0.81 0.00 2021-02-13T00:58:54Z- AHRS_M2 msg: 1613177934.16 epoch sec; seqNo:17331 2021-02-13T00:58:54Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.17 psi 2021-02-13T00:58:54Z- handleDepth()-Depth_Keller msg: 1613177934169 epoch sec; seqNo:17468; depth 20.50 2021-02-13T00:58:54Z- handleDvl() - RDI_Pathfinder msg: 1613177934.18 epoch sec; seqNo:8428 2021-02-13T00:58:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.71 , 98.18 , 68.25 , 103.05 2021-02-13T00:58:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , 0.06 2021-02-13T00:58:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177934.19 epoch sec; seqNo:17468; 4060530.25 north; 593476.62 east 2021-02-13T00:58:54Z- AHRS_M2 msg: 1613177934.57 epoch sec; seqNo:17332 2021-02-13T00:58:54Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.18 psi 2021-02-13T00:58:54Z- handleDepth()-Depth_Keller msg: 1613177934574 epoch sec; seqNo:17469; depth 20.61 2021-02-13T00:58:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177934.59 epoch sec; seqNo:17469; 4060530.25 north; 593476.62 east 2021-02-13T00:58:54Z- AHRS_M2 msg: 1613177934.97 epoch sec; seqNo:17333 2021-02-13T00:58:54Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.19 psi 2021-02-13T00:58:54Z- handleDepth()-Depth_Keller msg: 1613177934976 epoch sec; seqNo:17470; depth 20.72 2021-02-13T00:58:54Z- handleDvl() - RDI_Pathfinder msg: 1613177934.99 epoch sec; seqNo:8429 2021-02-13T00:58:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.97 , 91.90 , 67.84 , 105.28 2021-02-13T00:58:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.00 , 0.05 2021-02-13T00:58:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177935.01 epoch sec; seqNo:17470; 4060530.63 north; 593476.61 east 2021-02-13T00:58:55Z- AHRS_M2 msg: 1613177935.38 epoch sec; seqNo:17334 2021-02-13T00:58:55Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.20 psi 2021-02-13T00:58:55Z- handleDepth()-Depth_Keller msg: 1613177935385 epoch sec; seqNo:17471; depth 20.86 2021-02-13T00:58:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177935.40 epoch sec; seqNo:17471; 4060531.00 north; 593476.61 east 2021-02-13T00:58:55Z- AHRS_M2 msg: 1613177935.78 epoch sec; seqNo:17335 2021-02-13T00:58:55Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.20 psi 2021-02-13T00:58:55Z- handleDepth()-Depth_Keller msg: 1613177935793 epoch sec; seqNo:17472; depth 20.97 2021-02-13T00:58:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177935.80 epoch sec; seqNo:17472; 4060531.38 north; 593476.60 east 2021-02-13T00:58:56Z- AHRS_M2 msg: 1613177936.18 epoch sec; seqNo:17336 2021-02-13T00:58:56Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.19 psi 2021-02-13T00:58:56Z- handleDepth()-Depth_Keller msg: 1613177936198 epoch sec; seqNo:17473; depth 21.10 2021-02-13T00:58:56Z- handleDvl() - RDI_Pathfinder msg: 1613177936.21 epoch sec; seqNo:8430 2021-02-13T00:58:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 93.32 , 67.03 , 109.14 2021-02-13T00:58:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.02 , 0.06 2021-02-13T00:58:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177936.23 epoch sec; seqNo:17473; 4060531.38 north; 593476.60 east 2021-02-13T00:58:56Z- AHRS_M2 msg: 1613177936.59 epoch sec; seqNo:17337 2021-02-13T00:58:56Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.19 psi 2021-02-13T00:58:56Z- handleDepth()-Depth_Keller msg: 1613177936592 epoch sec; seqNo:17474; depth 21.24 2021-02-13T00:58:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177936.60 epoch sec; seqNo:17474; 4060531.77 north; 593477.82 east 2021-02-13T00:58:56Z- AHRS_M2 msg: 1613177936.99 epoch sec; seqNo:17338 2021-02-13T00:58:56Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.19 psi 2021-02-13T00:58:56Z- handleDepth()-Depth_Keller msg: 1613177936996 epoch sec; seqNo:17475; depth 21.36 2021-02-13T00:58:57Z- handleDvl() - RDI_Pathfinder msg: 1613177937.01 epoch sec; seqNo:8431 2021-02-13T00:58:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.56 , 91.69 , 67.03 , 106.30 2021-02-13T00:58:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , 0.06 2021-02-13T00:58:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177937.02 epoch sec; seqNo:17475; 4060532.15 north; 593477.82 east 2021-02-13T00:58:57Z- AHRS_M2 msg: 1613177937.39 epoch sec; seqNo:17339 2021-02-13T00:58:57Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.19 psi 2021-02-13T00:58:57Z- handleDepth()-Depth_Keller msg: 1613177937400 epoch sec; seqNo:17476; depth 21.49 2021-02-13T00:58:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177937.42 epoch sec; seqNo:17476; 4060532.53 north; 593477.81 east 2021-02-13T00:58:57Z- AHRS_M2 msg: 1613177937.81 epoch sec; seqNo:17340 2021-02-13T00:58:57Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.19 psi 2021-02-13T00:58:57Z- handleDepth()-Depth_Keller msg: 1613177937810 epoch sec; seqNo:17477; depth 21.62 2021-02-13T00:58:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177937.82 epoch sec; seqNo:17477; 4060532.53 north; 593477.81 east 2021-02-13T00:58:58Z- AHRS_M2 msg: 1613177938.21 epoch sec; seqNo:17341 2021-02-13T00:58:58Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.19 psi 2021-02-13T00:58:58Z- handleDepth()-Depth_Keller msg: 1613177938213 epoch sec; seqNo:17478; depth 21.77 2021-02-13T00:58:58Z- handleDvl() - RDI_Pathfinder msg: 1613177938.23 epoch sec; seqNo:8432 2021-02-13T00:58:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.57 , 92.17 , 66.62 , 104.34 2021-02-13T00:58:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.05 , 0.05 2021-02-13T00:58:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177938.24 epoch sec; seqNo:17478; 4060532.91 north; 593477.81 east 2021-02-13T00:58:58Z- AHRS_M2 msg: 1613177938.61 epoch sec; seqNo:17342 2021-02-13T00:58:58Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.19 psi 2021-02-13T00:58:58Z- handleDepth()-Depth_Keller msg: 1613177938615 epoch sec; seqNo:17479; depth 21.90 2021-02-13T00:58:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177938.63 epoch sec; seqNo:17479; 4060533.29 north; 593477.80 east 2021-02-13T00:58:59Z- AHRS_M2 msg: 1613177939.01 epoch sec; seqNo:17343 2021-02-13T00:58:59Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.19 psi 2021-02-13T00:58:59Z- handleDepth()-Depth_Keller msg: 1613177939017 epoch sec; seqNo:17480; depth 22.04 2021-02-13T00:58:59Z- handleDvl() - RDI_Pathfinder msg: 1613177939.03 epoch sec; seqNo:8433 2021-02-13T00:58:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.39 , 98.26 , 78.78 , 105.56 2021-02-13T00:58:59Z- handleDvl() - RDI_Pathfinder msg: velocities 1.08 , 0.04 , 0.00 2021-02-13T00:58:59Z- LcmTrn::handleMessages() - handling 44 messages took 0.70 ms and 130 calls... 2021-02-13T00:58:59Z- waiting for 1613177936.80, time is 1613177939.03 2021-02-13T00:58:59Z- LcmTrn::updateTrn() equating pose 1613177939.03 and meas times 1613177939.03 2021-02-13T00:58:59Z- LcmTrn::updateTrn() >>>> pose time 1613177939.03, meas time 1613177939.03 <<<< 2021-02-13T00:58:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177939.03 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:58:59Z- TerrainNav::motionUpdate() - currEstimate = (4060533, 593477.80, 22.04) 2021-02-13T00:58:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:58:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:58:59Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177939.03 sec.; No relevant navigation data available 2021-02-13T00:58:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:58:59Z- TNavBF::mmse selected from filter -1 2021-02-13T00:58:59Z- variances([0], [2], and psi_berg) = 680.247765 832.988583 0.000000 2021-02-13T00:58:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:58:59Z- LcmTrn::publishEstimates() - MLE : 4060532.79 593496.30 22.04 2021-02-13T00:58:59Z- LcmTrn::publishEstimates() - MMSE : 4060533.29 593477.80 22.04 2021-02-13T00:58:59Z- LcmTrn::publishEstimates() - COVAR: 680.25 832.99 1.03 0.00 2021-02-13T00:58:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177939.05 epoch sec; seqNo:17480; 4060533.68 north; 593479.02 east 2021-02-13T00:58:59Z- AHRS_M2 msg: 1613177939.42 epoch sec; seqNo:17344 2021-02-13T00:58:59Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.19 psi 2021-02-13T00:58:59Z- handleDepth()-Depth_Keller msg: 1613177939420 epoch sec; seqNo:17481; depth 22.16 2021-02-13T00:58:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177939.43 epoch sec; seqNo:17481; 4060534.06 north; 593479.02 east 2021-02-13T00:58:59Z- AHRS_M2 msg: 1613177939.82 epoch sec; seqNo:17345 2021-02-13T00:58:59Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.19 psi 2021-02-13T00:58:59Z- handleDepth()-Depth_Keller msg: 1613177939833 epoch sec; seqNo:17482; depth 22.29 2021-02-13T00:58:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177939.84 epoch sec; seqNo:17482; 4060534.44 north; 593479.01 east 2021-02-13T00:59:00Z- AHRS_M2 msg: 1613177940.22 epoch sec; seqNo:17346 2021-02-13T00:59:00Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.19 psi 2021-02-13T00:59:00Z- handleDepth()-Depth_Keller msg: 1613177940229 epoch sec; seqNo:17483; depth 22.42 2021-02-13T00:59:00Z- handleDvl() - RDI_Pathfinder msg: 1613177940.25 epoch sec; seqNo:8434 2021-02-13T00:59:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.52 , 97.85 , 64.57 , 103.93 2021-02-13T00:59:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.09 , 0.02 2021-02-13T00:59:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177940.26 epoch sec; seqNo:17483; 4060534.44 north; 593479.01 east 2021-02-13T00:59:00Z- AHRS_M2 msg: 1613177940.63 epoch sec; seqNo:17347 2021-02-13T00:59:00Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.18 psi 2021-02-13T00:59:00Z- handleDepth()-Depth_Keller msg: 1613177940633 epoch sec; seqNo:17484; depth 22.55 2021-02-13T00:59:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177940.65 epoch sec; seqNo:17484; 4060534.82 north; 593479.01 east 2021-02-13T00:59:01Z- AHRS_M2 msg: 1613177941.03 epoch sec; seqNo:17348 2021-02-13T00:59:01Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.18 psi 2021-02-13T00:59:01Z- handleDepth()-Depth_Keller msg: 1613177941037 epoch sec; seqNo:17485; depth 22.68 2021-02-13T00:59:01Z- handleDvl() - RDI_Pathfinder msg: 1613177941.05 epoch sec; seqNo:8435 2021-02-13T00:59:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.78 , 98.25 , 64.57 , 103.12 2021-02-13T00:59:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.14 , -0.05 2021-02-13T00:59:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177941.06 epoch sec; seqNo:17485; 4060535.21 north; 593480.22 east 2021-02-13T00:59:01Z- AHRS_M2 msg: 1613177941.44 epoch sec; seqNo:17349 2021-02-13T00:59:01Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.18 psi 2021-02-13T00:59:01Z- handleDepth()-Depth_Keller msg: 1613177941445 epoch sec; seqNo:17486; depth 22.81 2021-02-13T00:59:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177941.46 epoch sec; seqNo:17486; 4060535.21 north; 593480.22 east 2021-02-13T00:59:01Z- AHRS_M2 msg: 1613177941.84 epoch sec; seqNo:17350 2021-02-13T00:59:01Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.19 psi 2021-02-13T00:59:01Z- handleDepth()-Depth_Keller msg: 1613177941854 epoch sec; seqNo:17487; depth 22.93 2021-02-13T00:59:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177941.86 epoch sec; seqNo:17487; 4060535.59 north; 593480.22 east 2021-02-13T00:59:02Z- AHRS_M2 msg: 1613177942.24 epoch sec; seqNo:17351 2021-02-13T00:59:02Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.19 psi 2021-02-13T00:59:02Z- handleDepth()-Depth_Keller msg: 1613177942252 epoch sec; seqNo:17488; depth 23.07 2021-02-13T00:59:02Z- handleDvl() - RDI_Pathfinder msg: 1613177942.26 epoch sec; seqNo:8436 2021-02-13T00:59:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 89.60 , 64.99 , 101.77 2021-02-13T00:59:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.07 , -0.06 2021-02-13T00:59:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177942.29 epoch sec; seqNo:17488; 4060535.97 north; 593480.22 east 2021-02-13T00:59:02Z- AHRS_M2 msg: 1613177942.65 epoch sec; seqNo:17352 2021-02-13T00:59:02Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.19 psi 2021-02-13T00:59:02Z- handleDepth()-Depth_Keller msg: 1613177942717 epoch sec; seqNo:17489; depth 23.18 2021-02-13T00:59:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177942.73 epoch sec; seqNo:17489; 4060536.35 north; 593480.21 east 2021-02-13T00:59:03Z- AHRS_M2 msg: 1613177943.05 epoch sec; seqNo:17353 2021-02-13T00:59:03Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.19 psi 2021-02-13T00:59:03Z- handleDepth()-Depth_Keller msg: 1613177943069 epoch sec; seqNo:17490; depth 23.33 2021-02-13T00:59:03Z- handleDvl() - RDI_Pathfinder msg: 1613177943.09 epoch sec; seqNo:8437 2021-02-13T00:59:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 0.00 , 65.81 , 100.76 2021-02-13T00:59:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.00 , -0.08 2021-02-13T00:59:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177943.10 epoch sec; seqNo:17490; 4060536.74 north; 593481.43 east 2021-02-13T00:59:03Z- AHRS_M2 msg: 1613177943.45 epoch sec; seqNo:17354 2021-02-13T00:59:03Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.19 psi 2021-02-13T00:59:03Z- handleDepth()-Depth_Keller msg: 1613177943481 epoch sec; seqNo:17491; depth 23.44 2021-02-13T00:59:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177943.49 epoch sec; seqNo:17491; 4060537.12 north; 593481.42 east 2021-02-13T00:59:03Z- AHRS_M2 msg: 1613177943.86 epoch sec; seqNo:17355 2021-02-13T00:59:03Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.19 psi 2021-02-13T00:59:03Z- handleDepth()-Depth_Keller msg: 1613177943903 epoch sec; seqNo:17492; depth 23.56 2021-02-13T00:59:03Z- handleDvl() - RDI_Pathfinder msg: 1613177943.91 epoch sec; seqNo:8438 2021-02-13T00:59:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 0.00 , 63.76 , 100.15 2021-02-13T00:59:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.06 , -0.05 2021-02-13T00:59:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177943.93 epoch sec; seqNo:17492; 4060537.12 north; 593481.42 east 2021-02-13T00:59:04Z- LcmTrn::handleMessages() - handling 42 messages took 0.73 ms and 128 calls... 2021-02-13T00:59:04Z- waiting for 1613177942.03, time is 1613177943.93 2021-02-13T00:59:04Z- LcmTrn::updateTrn() equating pose 1613177943.93 and meas times 1613177943.93 2021-02-13T00:59:04Z- LcmTrn::updateTrn() >>>> pose time 1613177943.93, meas time 1613177943.93 <<<< 2021-02-13T00:59:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177943.93 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:59:04Z- TerrainNav::motionUpdate() - currEstimate = (4060537, 593481.42, 23.56) 2021-02-13T00:59:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:59:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:59:04Z- TNavBF::Projecting Measurements 2021-02-13T00:59:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:59:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:59:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:59:04Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:59:04Z- TNavFilter::3 good beams. 2021-02-13T00:59:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:59:04Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:59:04Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:59:04Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:59:04Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:59:04Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:59:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:59:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:59:04Z- TerrainMapOctree: (vehN, vehE ) = (4060537.12, 593481.42). 2021-02-13T00:59:04Z- TerrainMapOctree: Min Distance = 1048.29. 2021-02-13T00:59:04Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:59:04Z- There is no specific good beam that all particles have in common. 2021-02-13T00:59:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:59:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:59:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:59:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.124 ExpectedMeasDiff: nan currMeas.covariance: 6.50 2021-02-13T00:59:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:59:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 16.05 2021-02-13T00:59:04Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:59:04Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:59:04Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:59:04Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:59:04Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:59:04Z- Filter 0 No beams used in subcloud update 2021-02-13T00:59:04Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:59:04Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:59:04Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 680.2 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:59:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177943.93 sec. 2021-02-13T00:59:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:59:04Z- TNavBF::mmse selected from filter -1 2021-02-13T00:59:04Z- variances([0], [2], and psi_berg) = 680.197262 832.988562 0.000000 2021-02-13T00:59:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:59:04Z- LcmTrn::publishEstimates() - MLE : 4060536.62 593499.92 23.56 2021-02-13T00:59:04Z- LcmTrn::publishEstimates() - MMSE : 4060537.12 593481.42 23.56 2021-02-13T00:59:04Z- LcmTrn::publishEstimates() - COVAR: 680.20 832.99 0.97 0.00 2021-02-13T00:59:04Z- AHRS_M2 msg: 1613177944.26 epoch sec; seqNo:17356 2021-02-13T00:59:04Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.19 psi 2021-02-13T00:59:04Z- handleDepth()-Depth_Keller msg: 1613177944281 epoch sec; seqNo:17493; depth 23.72 2021-02-13T00:59:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177944.29 epoch sec; seqNo:17493; 4060537.50 north; 593481.42 east 2021-02-13T00:59:04Z- AHRS_M2 msg: 1613177944.67 epoch sec; seqNo:17357 2021-02-13T00:59:04Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.19 psi 2021-02-13T00:59:04Z- handleDepth()-Depth_Keller msg: 1613177944673 epoch sec; seqNo:17494; depth 23.83 2021-02-13T00:59:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177944.68 epoch sec; seqNo:17494; 4060537.88 north; 593481.41 east 2021-02-13T00:59:05Z- AHRS_M2 msg: 1613177945.07 epoch sec; seqNo:17358 2021-02-13T00:59:05Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.19 psi 2021-02-13T00:59:05Z- handleDepth()-Depth_Keller msg: 1613177945077 epoch sec; seqNo:17495; depth 23.96 2021-02-13T00:59:05Z- handleDvl() - RDI_Pathfinder msg: 1613177945.09 epoch sec; seqNo:8439 2021-02-13T00:59:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.30 , 114.15 , 63.35 , 101.16 2021-02-13T00:59:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.08 , -0.02 2021-02-13T00:59:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177945.10 epoch sec; seqNo:17495; 4060538.26 north; 593481.41 east 2021-02-13T00:59:05Z- AHRS_M2 msg: 1613177945.48 epoch sec; seqNo:17359 2021-02-13T00:59:05Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.19 psi 2021-02-13T00:59:05Z- handleDepth()-Depth_Keller msg: 1613177945481 epoch sec; seqNo:17496; depth 24.09 2021-02-13T00:59:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177945.49 epoch sec; seqNo:17496; 4060538.63 north; 593481.41 east 2021-02-13T00:59:05Z- AHRS_M2 msg: 1613177945.88 epoch sec; seqNo:17360 2021-02-13T00:59:05Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.18 psi 2021-02-13T00:59:05Z- handleDepth()-Depth_Keller msg: 1613177945983 epoch sec; seqNo:17497; depth 24.21 2021-02-13T00:59:06Z- handleDvl() - RDI_Pathfinder msg: 1613177946.00 epoch sec; seqNo:8440 2021-02-13T00:59:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.48 , 88.25 , 63.35 , 99.20 2021-02-13T00:59:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.08 , 0.03 2021-02-13T00:59:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177946.01 epoch sec; seqNo:17497; 4060539.03 north; 593482.62 east 2021-02-13T00:59:06Z- AHRS_M2 msg: 1613177946.29 epoch sec; seqNo:17361 2021-02-13T00:59:06Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.18 psi 2021-02-13T00:59:06Z- handleDepth()-Depth_Keller msg: 1613177946297 epoch sec; seqNo:17498; depth 24.38 2021-02-13T00:59:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177946.32 epoch sec; seqNo:17498; 4060539.03 north; 593482.62 east 2021-02-13T00:59:06Z- AHRS_M2 msg: 1613177946.69 epoch sec; seqNo:17362 2021-02-13T00:59:06Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.18 psi 2021-02-13T00:59:06Z- handleDepth()-Depth_Keller msg: 1613177946713 epoch sec; seqNo:17499; depth 24.47 2021-02-13T00:59:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177946.73 epoch sec; seqNo:17499; 4060539.41 north; 593482.62 east 2021-02-13T00:59:07Z- AHRS_M2 msg: 1613177947.09 epoch sec; seqNo:17363 2021-02-13T00:59:07Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.18 psi 2021-02-13T00:59:07Z- handleDepth()-Depth_Keller msg: 1613177947117 epoch sec; seqNo:17500; depth 24.61 2021-02-13T00:59:07Z- handleDvl() - RDI_Pathfinder msg: 1613177947.13 epoch sec; seqNo:8441 2021-02-13T00:59:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.29 , 87.10 , 64.16 , 97.91 2021-02-13T00:59:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.09 , 0.07 2021-02-13T00:59:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177947.16 epoch sec; seqNo:17500; 4060539.78 north; 593482.61 east 2021-02-13T00:59:07Z- AHRS_M2 msg: 1613177947.50 epoch sec; seqNo:17364 2021-02-13T00:59:07Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.17 psi 2021-02-13T00:59:07Z- handleDepth()-Depth_Keller msg: 1613177947500 epoch sec; seqNo:17501; depth 24.75 2021-02-13T00:59:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177947.52 epoch sec; seqNo:17501; 4060540.16 north; 593482.61 east 2021-02-13T00:59:07Z- AHRS_M2 msg: 1613177947.91 epoch sec; seqNo:17365 2021-02-13T00:59:07Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.17 psi 2021-02-13T00:59:07Z- handleDepth()-Depth_Keller msg: 1613177947913 epoch sec; seqNo:17502; depth 24.87 2021-02-13T00:59:07Z- handleDvl() - RDI_Pathfinder msg: 1613177947.93 epoch sec; seqNo:8442 2021-02-13T00:59:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.30 , 87.17 , 64.17 , 97.99 2021-02-13T00:59:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.10 2021-02-13T00:59:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177947.94 epoch sec; seqNo:17502; 4060540.16 north; 593482.61 east 2021-02-13T00:59:08Z- AHRS_M2 msg: 1613177948.31 epoch sec; seqNo:17366 2021-02-13T00:59:08Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.17 psi 2021-02-13T00:59:08Z- handleDepth()-Depth_Keller msg: 1613177948327 epoch sec; seqNo:17503; depth 24.99 2021-02-13T00:59:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177948.34 epoch sec; seqNo:17503; 4060540.54 north; 593482.61 east 2021-02-13T00:59:08Z- AHRS_M2 msg: 1613177948.71 epoch sec; seqNo:17367 2021-02-13T00:59:08Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.17 psi 2021-02-13T00:59:08Z- handleDepth()-Depth_Keller msg: 1613177948718 epoch sec; seqNo:17504; depth 25.12 2021-02-13T00:59:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177948.74 epoch sec; seqNo:17504; 4060540.92 north; 593482.60 east 2021-02-13T00:59:09Z- AHRS_M2 msg: 1613177949.11 epoch sec; seqNo:17368 2021-02-13T00:59:09Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.17 psi 2021-02-13T00:59:09Z- handleDepth()-Depth_Keller msg: 1613177949125 epoch sec; seqNo:17505; depth 25.24 2021-02-13T00:59:09Z- handleDvl() - RDI_Pathfinder msg: 1613177949.14 epoch sec; seqNo:8443 2021-02-13T00:59:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 87.17 , 62.94 , 97.99 2021-02-13T00:59:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , 0.10 2021-02-13T00:59:09Z- LcmTrn::handleMessages() - handling 43 messages took 0.79 ms and 127 calls... 2021-02-13T00:59:09Z- waiting for 1613177946.93, time is 1613177949.14 2021-02-13T00:59:09Z- LcmTrn::updateTrn() equating pose 1613177949.14 and meas times 1613177949.14 2021-02-13T00:59:09Z- LcmTrn::updateTrn() >>>> pose time 1613177949.14, meas time 1613177949.14 <<<< 2021-02-13T00:59:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177949.14 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:59:09Z- TerrainNav::motionUpdate() - currEstimate = (4060541, 593482.60, 25.24) 2021-02-13T00:59:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:59:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:59:09Z- TerrainNav::Measurement type 1 not incorporated from time = 1613177949.14 sec.; No relevant navigation data available 2021-02-13T00:59:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:59:09Z- TNavBF::mmse selected from filter -1 2021-02-13T00:59:09Z- variances([0], [2], and psi_berg) = 680.153389 832.988543 0.000000 2021-02-13T00:59:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:59:09Z- LcmTrn::publishEstimates() - MLE : 4060540.42 593501.10 25.24 2021-02-13T00:59:09Z- LcmTrn::publishEstimates() - MMSE : 4060540.92 593482.60 25.24 2021-02-13T00:59:09Z- LcmTrn::publishEstimates() - COVAR: 680.15 832.99 1.17 0.00 2021-02-13T00:59:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177949.16 epoch sec; seqNo:17505; 4060541.31 north; 593483.82 east 2021-02-13T00:59:09Z- AHRS_M2 msg: 1613177949.52 epoch sec; seqNo:17369 2021-02-13T00:59:09Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.17 psi 2021-02-13T00:59:09Z- handleDepth()-Depth_Keller msg: 1613177949529 epoch sec; seqNo:17506; depth 25.40 2021-02-13T00:59:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177949.54 epoch sec; seqNo:17506; 4060541.31 north; 593483.82 east 2021-02-13T00:59:09Z- AHRS_M2 msg: 1613177949.92 epoch sec; seqNo:17370 2021-02-13T00:59:09Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.17 psi 2021-02-13T00:59:09Z- handleDepth()-Depth_Keller msg: 1613177949925 epoch sec; seqNo:17507; depth 25.52 2021-02-13T00:59:09Z- handleDvl() - RDI_Pathfinder msg: 1613177949.94 epoch sec; seqNo:8444 2021-02-13T00:59:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.30 , 86.96 , 63.35 , 97.78 2021-02-13T00:59:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , 0.07 2021-02-13T00:59:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177949.95 epoch sec; seqNo:17507; 4060541.69 north; 593483.81 east 2021-02-13T00:59:10Z- AHRS_M2 msg: 1613177950.33 epoch sec; seqNo:17371 2021-02-13T00:59:10Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.17 psi 2021-02-13T00:59:10Z- handleDepth()-Depth_Keller msg: 1613177950337 epoch sec; seqNo:17508; depth 25.64 2021-02-13T00:59:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177950.35 epoch sec; seqNo:17508; 4060542.07 north; 593483.81 east 2021-02-13T00:59:10Z- AHRS_M2 msg: 1613177950.73 epoch sec; seqNo:17372 2021-02-13T00:59:10Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.17 psi 2021-02-13T00:59:10Z- handleDepth()-Depth_Keller msg: 1613177950733 epoch sec; seqNo:17509; depth 25.77 2021-02-13T00:59:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177950.74 epoch sec; seqNo:17509; 4060542.07 north; 593483.81 east 2021-02-13T00:59:11Z- AHRS_M2 msg: 1613177951.13 epoch sec; seqNo:17373 2021-02-13T00:59:11Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.17 psi 2021-02-13T00:59:11Z- handleDepth()-Depth_Keller msg: 1613177951140 epoch sec; seqNo:17510; depth 25.90 2021-02-13T00:59:11Z- handleDvl() - RDI_Pathfinder msg: 1613177951.16 epoch sec; seqNo:8445 2021-02-13T00:59:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 0.00 , 62.53 , 97.99 2021-02-13T00:59:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.07 , 0.01 2021-02-13T00:59:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177951.17 epoch sec; seqNo:17510; 4060542.45 north; 593483.81 east 2021-02-13T00:59:11Z- AHRS_M2 msg: 1613177951.54 epoch sec; seqNo:17374 2021-02-13T00:59:11Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.18 psi 2021-02-13T00:59:11Z- handleDepth()-Depth_Keller msg: 1613177951541 epoch sec; seqNo:17511; depth 26.03 2021-02-13T00:59:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177951.56 epoch sec; seqNo:17511; 4060542.84 north; 593485.02 east 2021-02-13T00:59:11Z- AHRS_M2 msg: 1613177951.96 epoch sec; seqNo:17375 2021-02-13T00:59:11Z- AHRS_M2 msg: 0.57 phi; -0.35 theta; -0.18 psi 2021-02-13T00:59:11Z- handleDepth()-Depth_Keller msg: 1613177951975 epoch sec; seqNo:17512; depth 26.17 2021-02-13T00:59:11Z- handleDvl() - RDI_Pathfinder msg: 1613177951.99 epoch sec; seqNo:8446 2021-02-13T00:59:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 86.08 , 61.72 , 97.99 2021-02-13T00:59:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , -0.03 2021-02-13T00:59:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177952.00 epoch sec; seqNo:17512; 4060543.22 north; 593485.02 east 2021-02-13T00:59:12Z- AHRS_M2 msg: 1613177952.34 epoch sec; seqNo:17376 2021-02-13T00:59:12Z- AHRS_M2 msg: 0.57 phi; -0.35 theta; -0.18 psi 2021-02-13T00:59:12Z- handleDepth()-Depth_Keller msg: 1613177952356 epoch sec; seqNo:17513; depth 26.32 2021-02-13T00:59:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177952.37 epoch sec; seqNo:17513; 4060543.60 north; 593485.01 east 2021-02-13T00:59:12Z- AHRS_M2 msg: 1613177952.75 epoch sec; seqNo:17377 2021-02-13T00:59:12Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.19 psi 2021-02-13T00:59:12Z- handleDepth()-Depth_Keller msg: 1613177952755 epoch sec; seqNo:17514; depth 26.41 2021-02-13T00:59:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177952.76 epoch sec; seqNo:17514; 4060543.60 north; 593485.01 east 2021-02-13T00:59:13Z- AHRS_M2 msg: 1613177953.15 epoch sec; seqNo:17378 2021-02-13T00:59:13Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.19 psi 2021-02-13T00:59:13Z- handleDepth()-Depth_Keller msg: 1613177953159 epoch sec; seqNo:17515; depth 26.54 2021-02-13T00:59:13Z- handleDvl() - RDI_Pathfinder msg: 1613177953.17 epoch sec; seqNo:8447 2021-02-13T00:59:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.44 , 85.95 , 62.94 , 97.85 2021-02-13T00:59:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.04 , -0.03 2021-02-13T00:59:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177953.20 epoch sec; seqNo:17515; 4060543.98 north; 593485.01 east 2021-02-13T00:59:13Z- AHRS_M2 msg: 1613177953.56 epoch sec; seqNo:17379 2021-02-13T00:59:13Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.19 psi 2021-02-13T00:59:13Z- handleDepth()-Depth_Keller msg: 1613177953564 epoch sec; seqNo:17516; depth 26.69 2021-02-13T00:59:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177953.58 epoch sec; seqNo:17516; 4060544.37 north; 593486.23 east 2021-02-13T00:59:13Z- AHRS_M2 msg: 1613177953.96 epoch sec; seqNo:17380 2021-02-13T00:59:13Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.19 psi 2021-02-13T00:59:13Z- handleDepth()-Depth_Keller msg: 1613177953969 epoch sec; seqNo:17517; depth 26.80 2021-02-13T00:59:13Z- handleDvl() - RDI_Pathfinder msg: 1613177953.99 epoch sec; seqNo:8448 2021-02-13T00:59:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.44 , 0.00 , 61.31 , 97.99 2021-02-13T00:59:13Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.01 , -0.05 2021-02-13T00:59:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177954.00 epoch sec; seqNo:17517; 4060544.75 north; 593486.22 east 2021-02-13T00:59:14Z- LcmTrn::handleMessages() - handling 42 messages took 0.64 ms and 129 calls... 2021-02-13T00:59:14Z- waiting for 1613177952.14, time is 1613177954.00 2021-02-13T00:59:14Z- LcmTrn::updateTrn() equating pose 1613177954.00 and meas times 1613177954.00 2021-02-13T00:59:14Z- LcmTrn::updateTrn() >>>> pose time 1613177954.00, meas time 1613177954.00 <<<< 2021-02-13T00:59:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177954.00 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:59:14Z- TerrainNav::motionUpdate() - currEstimate = (4060545, 593486.22, 26.80) 2021-02-13T00:59:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:59:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:59:14Z- TNavBF::Projecting Measurements 2021-02-13T00:59:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:59:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:59:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:59:14Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:59:14Z- TNavFilter::3 good beams. 2021-02-13T00:59:14Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:59:14Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:59:14Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:59:14Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:59:14Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:59:14Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:59:14Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:59:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:59:14Z- TerrainMapOctree: (vehN, vehE ) = (4060544.75, 593486.22). 2021-02-13T00:59:14Z- TerrainMapOctree: Min Distance = 1039.58. 2021-02-13T00:59:14Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:59:14Z- There is no specific good beam that all particles have in common. 2021-02-13T00:59:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:59:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.44 K: 0.138 ExpectedMeasDiff: nan currMeas.covariance: 7.28 2021-02-13T00:59:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:59:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.138 ExpectedMeasDiff: nan currMeas.covariance: 6.01 2021-02-13T00:59:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:59:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:59:14Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:59:14Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:59:14Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:59:14Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:59:14Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:59:14Z- Filter 0 No beams used in subcloud update 2021-02-13T00:59:14Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:59:14Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:59:14Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 680.1 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:59:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177954.00 sec. 2021-02-13T00:59:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:59:14Z- TNavBF::mmse selected from filter -1 2021-02-13T00:59:14Z- variances([0], [2], and psi_berg) = 680.102935 832.988522 0.000000 2021-02-13T00:59:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:59:14Z- LcmTrn::publishEstimates() - MLE : 4060544.25 593504.72 26.80 2021-02-13T00:59:14Z- LcmTrn::publishEstimates() - MMSE : 4060544.75 593486.22 26.80 2021-02-13T00:59:14Z- LcmTrn::publishEstimates() - COVAR: 680.10 832.99 1.01 0.00 2021-02-13T00:59:14Z- AHRS_M2 msg: 1613177954.37 epoch sec; seqNo:17381 2021-02-13T00:59:14Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.19 psi 2021-02-13T00:59:14Z- handleDepth()-Depth_Keller msg: 1613177954376 epoch sec; seqNo:17518; depth 26.94 2021-02-13T00:59:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177954.40 epoch sec; seqNo:17518; 4060545.13 north; 593486.22 east 2021-02-13T00:59:14Z- AHRS_M2 msg: 1613177954.77 epoch sec; seqNo:17382 2021-02-13T00:59:14Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.19 psi 2021-02-13T00:59:14Z- handleDepth()-Depth_Keller msg: 1613177954777 epoch sec; seqNo:17519; depth 27.07 2021-02-13T00:59:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177954.79 epoch sec; seqNo:17519; 4060545.51 north; 593486.21 east 2021-02-13T00:59:15Z- AHRS_M2 msg: 1613177955.18 epoch sec; seqNo:17383 2021-02-13T00:59:15Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.19 psi 2021-02-13T00:59:15Z- handleDepth()-Depth_Keller msg: 1613177955187 epoch sec; seqNo:17520; depth 27.18 2021-02-13T00:59:15Z- handleDvl() - RDI_Pathfinder msg: 1613177955.20 epoch sec; seqNo:8449 2021-02-13T00:59:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.03 , 85.00 , 61.72 , 97.99 2021-02-13T00:59:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.08 , -0.03 2021-02-13T00:59:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177955.21 epoch sec; seqNo:17520; 4060545.51 north; 593486.21 east 2021-02-13T00:59:15Z- AHRS_M2 msg: 1613177955.58 epoch sec; seqNo:17384 2021-02-13T00:59:15Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.18 psi 2021-02-13T00:59:15Z- handleDepth()-Depth_Keller msg: 1613177955585 epoch sec; seqNo:17521; depth 27.32 2021-02-13T00:59:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177955.60 epoch sec; seqNo:17521; 4060545.89 north; 593486.21 east 2021-02-13T00:59:15Z- AHRS_M2 msg: 1613177955.98 epoch sec; seqNo:17385 2021-02-13T00:59:15Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.18 psi 2021-02-13T00:59:16Z- handleDepth()-Depth_Keller msg: 1613177956049 epoch sec; seqNo:17522; depth 27.44 2021-02-13T00:59:16Z- handleDvl() - RDI_Pathfinder msg: 1613177956.06 epoch sec; seqNo:8450 2021-02-13T00:59:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.03 , 0.00 , 62.53 , 93.66 2021-02-13T00:59:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , -0.01 2021-02-13T00:59:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177956.08 epoch sec; seqNo:17522; 4060546.26 north; 593486.21 east 2021-02-13T00:59:16Z- AHRS_M2 msg: 1613177956.39 epoch sec; seqNo:17386 2021-02-13T00:59:16Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.18 psi 2021-02-13T00:59:16Z- handleDepth()-Depth_Keller msg: 1613177956399 epoch sec; seqNo:17523; depth 27.60 2021-02-13T00:59:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177956.41 epoch sec; seqNo:17523; 4060546.66 north; 593487.42 east 2021-02-13T00:59:16Z- AHRS_M2 msg: 1613177956.79 epoch sec; seqNo:17387 2021-02-13T00:59:16Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.18 psi 2021-02-13T00:59:16Z- handleDepth()-Depth_Keller msg: 1613177956814 epoch sec; seqNo:17524; depth 27.70 2021-02-13T00:59:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177956.84 epoch sec; seqNo:17524; 4060547.04 north; 593487.42 east 2021-02-13T00:59:17Z- AHRS_M2 msg: 1613177957.19 epoch sec; seqNo:17388 2021-02-13T00:59:17Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.18 psi 2021-02-13T00:59:17Z- handleDepth()-Depth_Keller msg: 1613177957217 epoch sec; seqNo:17525; depth 27.82 2021-02-13T00:59:17Z- handleDvl() - RDI_Pathfinder msg: 1613177957.23 epoch sec; seqNo:8451 2021-02-13T00:59:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.21 , 0.00 , 61.31 , 97.99 2021-02-13T00:59:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.02 2021-02-13T00:59:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177957.25 epoch sec; seqNo:17525; 4060547.41 north; 593487.41 east 2021-02-13T00:59:17Z- AHRS_M2 msg: 1613177957.61 epoch sec; seqNo:17389 2021-02-13T00:59:17Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.18 psi 2021-02-13T00:59:17Z- handleDepth()-Depth_Keller msg: 1613177957614 epoch sec; seqNo:17526; depth 27.97 2021-02-13T00:59:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177957.62 epoch sec; seqNo:17526; 4060547.41 north; 593487.41 east 2021-02-13T00:59:18Z- AHRS_M2 msg: 1613177958.00 epoch sec; seqNo:17390 2021-02-13T00:59:18Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-13T00:59:18Z- handleDepth()-Depth_Keller msg: 1613177958013 epoch sec; seqNo:17527; depth 28.08 2021-02-13T00:59:18Z- handleDvl() - RDI_Pathfinder msg: 1613177958.03 epoch sec; seqNo:8452 2021-02-13T00:59:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.40 , 83.92 , 60.49 , 97.99 2021-02-13T00:59:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , 0.01 2021-02-13T00:59:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177958.04 epoch sec; seqNo:17527; 4060547.79 north; 593487.41 east 2021-02-13T00:59:18Z- handleDepth()-Depth_Keller msg: 1613177958425 epoch sec; seqNo:17528; depth 28.20 2021-02-13T00:59:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177958.44 epoch sec; seqNo:17528; 4060548.17 north; 593487.41 east 2021-02-13T00:59:18Z- AHRS_M2 msg: 1613177958.83 epoch sec; seqNo:17391 2021-02-13T00:59:18Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-13T00:59:18Z- handleDepth()-Depth_Keller msg: 1613177958838 epoch sec; seqNo:17529; depth 28.33 2021-02-13T00:59:18Z- handleDvl() - RDI_Pathfinder msg: 1613177958.86 epoch sec; seqNo:8453 2021-02-13T00:59:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.03 , 0.00 , 61.31 , 97.99 2021-02-13T00:59:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , 0.01 2021-02-13T00:59:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177958.87 epoch sec; seqNo:17529; 4060548.55 north; 593487.40 east 2021-02-13T00:59:19Z- AHRS_M2 msg: 1613177959.21 epoch sec; seqNo:17392 2021-02-13T00:59:19Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-13T00:59:19Z- LcmTrn::handleMessages() - handling 41 messages took 0.74 ms and 127 calls... 2021-02-13T00:59:19Z- waiting for 1613177957.00, time is 1613177959.21 2021-02-13T00:59:19Z- LcmTrn::updateTrn() >>>> pose time 1613177959.21, meas time 1613177958.86 <<<< 2021-02-13T00:59:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177958.86 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:59:19Z- TerrainNav::motionUpdate() - currEstimate = (4060549, 593487.40, 28.33) 2021-02-13T00:59:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:59:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:59:19Z- TNavBF::Projecting Measurements 2021-02-13T00:59:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:59:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:59:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:59:19Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:59:19Z- TNavFilter::3 good beams. 2021-02-13T00:59:19Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:59:19Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:59:19Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:59:19Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:59:19Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:59:19Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:59:19Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:59:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:59:19Z- TerrainMapOctree: (vehN, vehE ) = (4060548.29, 593487.32). 2021-02-13T00:59:19Z- TerrainMapOctree: Min Distance = 1036.38. 2021-02-13T00:59:19Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:59:19Z- There is no specific good beam that all particles have in common. 2021-02-13T00:59:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:59:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.118 ExpectedMeasDiff: nan currMeas.covariance: 7.19 2021-02-13T00:59:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:59:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.120 ExpectedMeasDiff: nan currMeas.covariance: 6.01 2021-02-13T00:59:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:59:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:59:19Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:59:19Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:59:19Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:59:19Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:59:19Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:59:19Z- Filter 0 No beams used in subcloud update 2021-02-13T00:59:19Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:59:19Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:59:19Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 680.1 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T00:59:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177958.86 sec. 2021-02-13T00:59:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:59:19Z- TNavBF::mmse selected from filter -1 2021-02-13T00:59:19Z- variances([0], [2], and psi_berg) = 680.049178 832.988499 0.000000 2021-02-13T00:59:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:59:19Z- LcmTrn::publishEstimates() - MLE : 4060548.05 593505.90 28.33 2021-02-13T00:59:19Z- LcmTrn::publishEstimates() - MMSE : 4060548.55 593487.40 28.33 2021-02-13T00:59:19Z- LcmTrn::publishEstimates() - COVAR: 680.05 832.99 0.89 0.00 2021-02-13T00:59:19Z- handleDepth()-Depth_Keller msg: 1613177959281 epoch sec; seqNo:17530; depth 28.46 2021-02-13T00:59:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177959.29 epoch sec; seqNo:17530; 4060548.94 north; 593488.62 east 2021-02-13T00:59:19Z- AHRS_M2 msg: 1613177959.62 epoch sec; seqNo:17393 2021-02-13T00:59:19Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-13T00:59:19Z- handleDepth()-Depth_Keller msg: 1613177959633 epoch sec; seqNo:17531; depth 28.58 2021-02-13T00:59:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177959.66 epoch sec; seqNo:17531; 4060548.94 north; 593488.62 east 2021-02-13T00:59:20Z- AHRS_M2 msg: 1613177960.02 epoch sec; seqNo:17394 2021-02-13T00:59:20Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-13T00:59:20Z- handleDepth()-Depth_Keller msg: 1613177960045 epoch sec; seqNo:17532; depth 28.70 2021-02-13T00:59:20Z- handleDvl() - RDI_Pathfinder msg: 1613177960.06 epoch sec; seqNo:8454 2021-02-13T00:59:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.99 , 80.68 , 59.67 , 97.99 2021-02-13T00:59:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.08 , 0.02 2021-02-13T00:59:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177960.07 epoch sec; seqNo:17532; 4060549.32 north; 593488.61 east 2021-02-13T00:59:20Z- AHRS_M2 msg: 1613177960.42 epoch sec; seqNo:17395 2021-02-13T00:59:20Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-13T00:59:20Z- handleDepth()-Depth_Keller msg: 1613177960453 epoch sec; seqNo:17533; depth 28.81 2021-02-13T00:59:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177960.46 epoch sec; seqNo:17533; 4060549.70 north; 593488.61 east 2021-02-13T00:59:20Z- AHRS_M2 msg: 1613177960.83 epoch sec; seqNo:17396 2021-02-13T00:59:20Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-13T00:59:20Z- handleDepth()-Depth_Keller msg: 1613177960837 epoch sec; seqNo:17534; depth 28.93 2021-02-13T00:59:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177960.86 epoch sec; seqNo:17534; 4060550.08 north; 593488.61 east 2021-02-13T00:59:21Z- AHRS_M2 msg: 1613177961.23 epoch sec; seqNo:17397 2021-02-13T00:59:21Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.18 psi 2021-02-13T00:59:21Z- handleDepth()-Depth_Keller msg: 1613177961237 epoch sec; seqNo:17535; depth 29.04 2021-02-13T00:59:21Z- handleDvl() - RDI_Pathfinder msg: 1613177961.25 epoch sec; seqNo:8455 2021-02-13T00:59:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.81 , 81.76 , 59.26 , 97.99 2021-02-13T00:59:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , 0.05 2021-02-13T00:59:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177961.26 epoch sec; seqNo:17535; 4060550.08 north; 593488.61 east 2021-02-13T00:59:21Z- AHRS_M2 msg: 1613177961.64 epoch sec; seqNo:17398 2021-02-13T00:59:21Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.18 psi 2021-02-13T00:59:21Z- handleDepth()-Depth_Keller msg: 1613177961650 epoch sec; seqNo:17536; depth 29.17 2021-02-13T00:59:21Z- handleDvl() - RDI_Pathfinder msg: 1613177961.66 epoch sec; seqNo:8456 2021-02-13T00:59:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:59:21Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177961.67 epoch sec; seqNo:17536; 4060550.46 north; 593488.60 east 2021-02-13T00:59:22Z- AHRS_M2 msg: 1613177962.04 epoch sec; seqNo:17399 2021-02-13T00:59:22Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.18 psi 2021-02-13T00:59:22Z- handleDepth()-Depth_Keller msg: 1613177962044 epoch sec; seqNo:17537; depth 29.30 2021-02-13T00:59:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177962.06 epoch sec; seqNo:17537; 4060550.84 north; 593488.60 east 2021-02-13T00:59:22Z- AHRS_M2 msg: 1613177962.44 epoch sec; seqNo:17400 2021-02-13T00:59:22Z- AHRS_M2 msg: 0.57 phi; -0.31 theta; -0.18 psi 2021-02-13T00:59:22Z- handleDepth()-Depth_Keller msg: 1613177962453 epoch sec; seqNo:17538; depth 29.41 2021-02-13T00:59:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177962.46 epoch sec; seqNo:17538; 4060551.23 north; 593489.81 east 2021-02-13T00:59:22Z- AHRS_M2 msg: 1613177962.85 epoch sec; seqNo:17401 2021-02-13T00:59:22Z- AHRS_M2 msg: 0.57 phi; -0.29 theta; -0.18 psi 2021-02-13T00:59:22Z- handleDepth()-Depth_Keller msg: 1613177962857 epoch sec; seqNo:17539; depth 29.55 2021-02-13T00:59:22Z- handleDvl() - RDI_Pathfinder msg: 1613177962.87 epoch sec; seqNo:8457 2021-02-13T00:59:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 64.40 , 0.00 , 0.00 , 0.00 2021-02-13T00:59:22Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177962.88 epoch sec; seqNo:17539; 4060551.61 north; 593489.81 east 2021-02-13T00:59:23Z- AHRS_M2 msg: 1613177963.25 epoch sec; seqNo:17402 2021-02-13T00:59:23Z- AHRS_M2 msg: 0.57 phi; -0.27 theta; -0.18 psi 2021-02-13T00:59:23Z- handleDepth()-Depth_Keller msg: 1613177963258 epoch sec; seqNo:17540; depth 29.68 2021-02-13T00:59:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177963.27 epoch sec; seqNo:17540; 4060551.61 north; 593489.81 east 2021-02-13T00:59:23Z- AHRS_M2 msg: 1613177963.66 epoch sec; seqNo:17403 2021-02-13T00:59:23Z- AHRS_M2 msg: 0.56 phi; -0.23 theta; -0.17 psi 2021-02-13T00:59:23Z- handleDepth()-Depth_Keller msg: 1613177963670 epoch sec; seqNo:17541; depth 29.81 2021-02-13T00:59:23Z- handleDvl() - RDI_Pathfinder msg: 1613177963.68 epoch sec; seqNo:8458 2021-02-13T00:59:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.94 , 0.00 , 58.42 , 0.00 2021-02-13T00:59:23Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177963.69 epoch sec; seqNo:17541; 4060551.99 north; 593489.81 east 2021-02-13T00:59:24Z- AHRS_M2 msg: 1613177964.06 epoch sec; seqNo:17404 2021-02-13T00:59:24Z- AHRS_M2 msg: 0.55 phi; -0.19 theta; -0.16 psi 2021-02-13T00:59:24Z- handleDepth()-Depth_Keller msg: 1613177964064 epoch sec; seqNo:17542; depth 29.94 2021-02-13T00:59:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:24Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:59:24Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:59:24Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:59:24Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:59:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:59:24Z- handleDvl() - RDI_Pathfinder msg: 1613177964.07 epoch sec; seqNo:8459 2021-02-13T00:59:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:59:24Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177964.08 epoch sec; seqNo:17542; 4060552.37 north; 593489.80 east 2021-02-13T00:59:24Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-13T00:59:24Z- waiting for 1613177962.21, time is 1613177964.08 2021-02-13T00:59:24Z- LcmTrn::updateTrn() equating pose 1613177964.08 and meas times 1613177964.08 2021-02-13T00:59:24Z- LcmTrn::updateTrn() >>>> pose time 1613177964.08, meas time 1613177964.08 <<<< 2021-02-13T00:59:24Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177964.08, ping # 0. 2021-02-13T00:59:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177964.08 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:59:24Z- TerrainNav::motionUpdate() - currEstimate = (4060552, 593489.80, 29.94) 2021-02-13T00:59:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:59:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:59:24Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:59:24Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:59:24Z- TNavBF::Projecting Measurements 2021-02-13T00:59:24Z- TNavFilter::Measurement from time = 1613177964.08 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:59:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:59:24Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:59:24Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:59:24Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:59:24Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:59:24Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:59:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:59:24Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 680.0 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:59:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:59:24Z- TNavBF::mmse selected from filter -1 2021-02-13T00:59:24Z- variances([0], [2], and psi_berg) = 680.002565 832.988479 0.000000 2021-02-13T00:59:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:59:24Z- LcmTrn::publishEstimates() - MLE : 4060551.87 593508.30 29.94 2021-02-13T00:59:24Z- LcmTrn::publishEstimates() - MMSE : 4060552.37 593489.80 29.94 2021-02-13T00:59:24Z- LcmTrn::publishEstimates() - COVAR: 680.00 832.99 1.12 0.00 2021-02-13T00:59:24Z- AHRS_M2 msg: 1613177964.46 epoch sec; seqNo:17405 2021-02-13T00:59:24Z- AHRS_M2 msg: 0.55 phi; -0.15 theta; -0.15 psi 2021-02-13T00:59:24Z- handleDepth()-Depth_Keller msg: 1613177964468 epoch sec; seqNo:17543; depth 30.07 2021-02-13T00:59:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177964.48 epoch sec; seqNo:17543; 4060552.74 north; 593489.80 east 2021-02-13T00:59:24Z- AHRS_M2 msg: 1613177964.87 epoch sec; seqNo:17406 2021-02-13T00:59:24Z- AHRS_M2 msg: 0.54 phi; -0.11 theta; -0.14 psi 2021-02-13T00:59:24Z- handleDepth()-Depth_Keller msg: 1613177964881 epoch sec; seqNo:17544; depth 30.18 2021-02-13T00:59:24Z- handleDvl() - RDI_Pathfinder msg: 1613177964.89 epoch sec; seqNo:8460 2021-02-13T00:59:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.56 , 0.00 , 62.10 , 0.00 2021-02-13T00:59:24Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177964.90 epoch sec; seqNo:17544; 4060553.12 north; 593489.79 east 2021-02-13T00:59:25Z- AHRS_M2 msg: 1613177965.27 epoch sec; seqNo:17407 2021-02-13T00:59:25Z- AHRS_M2 msg: 0.54 phi; -0.07 theta; -0.14 psi 2021-02-13T00:59:25Z- handleDepth()-Depth_Keller msg: 1613177965281 epoch sec; seqNo:17545; depth 30.27 2021-02-13T00:59:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:59:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:59:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:59:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:59:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:59:25Z- handleDvl() - RDI_Pathfinder msg: 1613177965.28 epoch sec; seqNo:8461 2021-02-13T00:59:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:59:25Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177965.30 epoch sec; seqNo:17545; 4060553.52 north; 593491.01 east 2021-02-13T00:59:25Z- AHRS_M2 msg: 1613177965.68 epoch sec; seqNo:17408 2021-02-13T00:59:25Z- AHRS_M2 msg: 0.53 phi; -0.02 theta; -0.14 psi 2021-02-13T00:59:25Z- handleDepth()-Depth_Keller msg: 1613177965680 epoch sec; seqNo:17546; depth 30.36 2021-02-13T00:59:25Z- handleDvl() - RDI_Pathfinder msg: 1613177965.69 epoch sec; seqNo:8462 2021-02-13T00:59:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:59:25Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177965.72 epoch sec; seqNo:17546; 4060553.89 north; 593491.01 east 2021-02-13T00:59:26Z- AHRS_M2 msg: 1613177966.08 epoch sec; seqNo:17409 2021-02-13T00:59:26Z- AHRS_M2 msg: 0.53 phi; 0.02 theta; -0.13 psi 2021-02-13T00:59:26Z- handleDepth()-Depth_Keller msg: 1613177966084 epoch sec; seqNo:17547; depth 30.42 2021-02-13T00:59:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177966.10 epoch sec; seqNo:17547; 4060553.89 north; 593491.01 east 2021-02-13T00:59:26Z- AHRS_M2 msg: 1613177966.49 epoch sec; seqNo:17410 2021-02-13T00:59:26Z- AHRS_M2 msg: 0.52 phi; 0.06 theta; -0.13 psi 2021-02-13T00:59:26Z- handleDepth()-Depth_Keller msg: 1613177966493 epoch sec; seqNo:17548; depth 30.48 2021-02-13T00:59:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:26Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:59:26Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:59:26Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:59:26Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:59:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:59:26Z- handleDvl() - RDI_Pathfinder msg: 1613177966.49 epoch sec; seqNo:8463 2021-02-13T00:59:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:59:26Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177966.50 epoch sec; seqNo:17548; 4060554.27 north; 593491.00 east 2021-02-13T00:59:26Z- AHRS_M2 msg: 1613177966.89 epoch sec; seqNo:17411 2021-02-13T00:59:26Z- AHRS_M2 msg: 0.52 phi; 0.09 theta; -0.13 psi 2021-02-13T00:59:26Z- handleDepth()-Depth_Keller msg: 1613177966892 epoch sec; seqNo:17549; depth 30.51 2021-02-13T00:59:26Z- handleDvl() - RDI_Pathfinder msg: 1613177966.91 epoch sec; seqNo:8464 2021-02-13T00:59:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.53 , 78.51 , 0.00 , 64.99 2021-02-13T00:59:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , -0.07 2021-02-13T00:59:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177966.92 epoch sec; seqNo:17549; 4060554.65 north; 593491.00 east 2021-02-13T00:59:27Z- AHRS_M2 msg: 1613177967.29 epoch sec; seqNo:17412 2021-02-13T00:59:27Z- AHRS_M2 msg: 0.51 phi; 0.13 theta; -0.13 psi 2021-02-13T00:59:27Z- handleDepth()-Depth_Keller msg: 1613177967296 epoch sec; seqNo:17550; depth 30.54 2021-02-13T00:59:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177967.31 epoch sec; seqNo:17550; 4060555.03 north; 593490.99 east 2021-02-13T00:59:27Z- AHRS_M2 msg: 1613177967.69 epoch sec; seqNo:17413 2021-02-13T00:59:27Z- AHRS_M2 msg: 0.51 phi; 0.16 theta; -0.13 psi 2021-02-13T00:59:27Z- handleDepth()-Depth_Keller msg: 1613177967705 epoch sec; seqNo:17551; depth 30.55 2021-02-13T00:59:27Z- handleDvl() - RDI_Pathfinder msg: 1613177967.72 epoch sec; seqNo:8465 2021-02-13T00:59:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.12 , 0.00 , 0.00 , 62.48 2021-02-13T00:59:27Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177967.73 epoch sec; seqNo:17551; 4060555.41 north; 593490.99 east 2021-02-13T00:59:28Z- AHRS_M2 msg: 1613177968.10 epoch sec; seqNo:17414 2021-02-13T00:59:28Z- AHRS_M2 msg: 0.50 phi; 0.19 theta; -0.13 psi 2021-02-13T00:59:28Z- handleDepth()-Depth_Keller msg: 1613177968113 epoch sec; seqNo:17552; depth 30.54 2021-02-13T00:59:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177968.13 epoch sec; seqNo:17552; 4060555.79 north; 593490.98 east 2021-02-13T00:59:28Z- AHRS_M2 msg: 1613177968.50 epoch sec; seqNo:17415 2021-02-13T00:59:28Z- AHRS_M2 msg: 0.50 phi; 0.22 theta; -0.14 psi 2021-02-13T00:59:28Z- handleDepth()-Depth_Keller msg: 1613177968509 epoch sec; seqNo:17553; depth 30.53 2021-02-13T00:59:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177968.53 epoch sec; seqNo:17553; 4060555.80 north; 593492.21 east 2021-02-13T00:59:28Z- AHRS_M2 msg: 1613177968.91 epoch sec; seqNo:17416 2021-02-13T00:59:28Z- AHRS_M2 msg: 0.50 phi; 0.24 theta; -0.13 psi 2021-02-13T00:59:28Z- handleDepth()-Depth_Keller msg: 1613177968913 epoch sec; seqNo:17554; depth 30.50 2021-02-13T00:59:28Z- handleDvl() - RDI_Pathfinder msg: 1613177968.92 epoch sec; seqNo:8466 2021-02-13T00:59:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 64.32 , 0.00 , 0.00 , 59.63 2021-02-13T00:59:28Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177968.96 epoch sec; seqNo:17554; 4060556.18 north; 593492.20 east 2021-02-13T00:59:29Z- AHRS_M2 msg: 1613177969.31 epoch sec; seqNo:17417 2021-02-13T00:59:29Z- AHRS_M2 msg: 0.50 phi; 0.26 theta; -0.13 psi 2021-02-13T00:59:29Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T00:59:29Z- waiting for 1613177967.08, time is 1613177969.31 2021-02-13T00:59:29Z- LcmTrn::updateTrn() >>>> pose time 1613177969.31, meas time 1613177968.92 <<<< 2021-02-13T00:59:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177968.92 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:59:29Z- TerrainNav::motionUpdate() - currEstimate = (4060556, 593492.20, 30.50) 2021-02-13T00:59:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:59:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:59:29Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:59:29Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:59:29Z- TNavBF::Projecting Measurements 2021-02-13T00:59:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:59:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:59:29Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-13T00:59:29Z- TNavFilter::2 good beams. 2021-02-13T00:59:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:59:29Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:59:29Z- TNavBF:: beamIndex[1] = 3 2021-02-13T00:59:29Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:59:29Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:59:29Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-13T00:59:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:59:29Z- TNavBF::mmse selected from filter -1 2021-02-13T00:59:29Z- TerrainMapOctree: (vehN, vehE ) = (4060555.90, 593492.02). 2021-02-13T00:59:29Z- TerrainMapOctree: Min Distance = 1027.77. 2021-02-13T00:59:29Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:59:29Z- There is no specific good beam that all particles have in common. 2021-02-13T00:59:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:59:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.138 ExpectedMeasDiff: nan currMeas.covariance: 6.62 2021-02-13T00:59:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:59:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.134 ExpectedMeasDiff: nan currMeas.covariance: 5.69 2021-02-13T00:59:29Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:59:29Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:59:29Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:59:29Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:59:29Z- Filter 0 No beams used in subcloud update 2021-02-13T00:59:29Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:59:29Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:59:29Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 680.0 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:59:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177968.92 sec. 2021-02-13T00:59:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:59:29Z- TNavBF::mmse selected from filter -1 2021-02-13T00:59:29Z- variances([0], [2], and psi_berg) = 679.945067 832.988455 0.000000 2021-02-13T00:59:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:59:29Z- LcmTrn::publishEstimates() - MLE : 4060555.68 593510.70 30.50 2021-02-13T00:59:29Z- LcmTrn::publishEstimates() - MMSE : 4060556.18 593492.20 30.50 2021-02-13T00:59:29Z- LcmTrn::publishEstimates() - COVAR: 679.95 832.99 1.01 0.00 2021-02-13T00:59:29Z- handleDepth()-Depth_Keller msg: 1613177969385 epoch sec; seqNo:17555; depth 30.47 2021-02-13T00:59:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177969.40 epoch sec; seqNo:17555; 4060556.56 north; 593492.20 east 2021-02-13T00:59:29Z- AHRS_M2 msg: 1613177969.72 epoch sec; seqNo:17418 2021-02-13T00:59:29Z- AHRS_M2 msg: 0.50 phi; 0.28 theta; -0.12 psi 2021-02-13T00:59:29Z- handleDepth()-Depth_Keller msg: 1613177969729 epoch sec; seqNo:17556; depth 30.41 2021-02-13T00:59:29Z- handleDvl() - RDI_Pathfinder msg: 1613177969.74 epoch sec; seqNo:8467 2021-02-13T00:59:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 65.60 , 0.00 , 0.00 , 58.78 2021-02-13T00:59:29Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177969.75 epoch sec; seqNo:17556; 4060556.94 north; 593492.19 east 2021-02-13T00:59:30Z- AHRS_M2 msg: 1613177970.12 epoch sec; seqNo:17419 2021-02-13T00:59:30Z- AHRS_M2 msg: 0.50 phi; 0.30 theta; -0.11 psi 2021-02-13T00:59:30Z- handleDepth()-Depth_Keller msg: 1613177970139 epoch sec; seqNo:17557; depth 30.36 2021-02-13T00:59:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:59:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:59:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:59:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:59:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:59:30Z- handleDvl() - RDI_Pathfinder msg: 1613177970.14 epoch sec; seqNo:8468 2021-02-13T00:59:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:59:30Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177970.16 epoch sec; seqNo:17557; 4060557.32 north; 593492.19 east 2021-02-13T00:59:30Z- AHRS_M2 msg: 1613177970.53 epoch sec; seqNo:17420 2021-02-13T00:59:30Z- AHRS_M2 msg: 0.51 phi; 0.32 theta; -0.10 psi 2021-02-13T00:59:30Z- handleDepth()-Depth_Keller msg: 1613177970558 epoch sec; seqNo:17558; depth 30.29 2021-02-13T00:59:30Z- handleDvl() - RDI_Pathfinder msg: 1613177970.58 epoch sec; seqNo:8469 2021-02-13T00:59:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 67.30 , 0.00 , 0.00 , 59.63 2021-02-13T00:59:30Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177970.59 epoch sec; seqNo:17558; 4060557.70 north; 593492.18 east 2021-02-13T00:59:30Z- AHRS_M2 msg: 1613177970.93 epoch sec; seqNo:17421 2021-02-13T00:59:30Z- AHRS_M2 msg: 0.52 phi; 0.33 theta; -0.10 psi 2021-02-13T00:59:30Z- handleDepth()-Depth_Keller msg: 1613177970933 epoch sec; seqNo:17559; depth 30.19 2021-02-13T00:59:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177970.94 epoch sec; seqNo:17559; 4060557.70 north; 593492.18 east 2021-02-13T00:59:31Z- AHRS_M2 msg: 1613177971.33 epoch sec; seqNo:17422 2021-02-13T00:59:31Z- AHRS_M2 msg: 0.52 phi; 0.34 theta; -0.09 psi 2021-02-13T00:59:31Z- handleDepth()-Depth_Keller msg: 1613177971342 epoch sec; seqNo:17560; depth 30.12 2021-02-13T00:59:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:59:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:59:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:59:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:59:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:59:31Z- handleDvl() - RDI_Pathfinder msg: 1613177971.35 epoch sec; seqNo:8470 2021-02-13T00:59:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:59:31Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177971.36 epoch sec; seqNo:17560; 4060558.08 north; 593492.18 east 2021-02-13T00:59:31Z- AHRS_M2 msg: 1613177971.74 epoch sec; seqNo:17423 2021-02-13T00:59:31Z- AHRS_M2 msg: 0.53 phi; 0.35 theta; -0.09 psi 2021-02-13T00:59:31Z- handleDepth()-Depth_Keller msg: 1613177971741 epoch sec; seqNo:17561; depth 30.04 2021-02-13T00:59:31Z- handleDvl() - RDI_Pathfinder msg: 1613177971.76 epoch sec; seqNo:8471 2021-02-13T00:59:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:59:31Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177971.78 epoch sec; seqNo:17561; 4060558.45 north; 593492.18 east 2021-02-13T00:59:32Z- AHRS_M2 msg: 1613177972.15 epoch sec; seqNo:17424 2021-02-13T00:59:32Z- AHRS_M2 msg: 0.54 phi; 0.36 theta; -0.09 psi 2021-02-13T00:59:32Z- handleDepth()-Depth_Keller msg: 1613177972157 epoch sec; seqNo:17562; depth 29.94 2021-02-13T00:59:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177972.18 epoch sec; seqNo:17562; 4060558.85 north; 593493.39 east 2021-02-13T00:59:32Z- AHRS_M2 msg: 1613177972.55 epoch sec; seqNo:17425 2021-02-13T00:59:32Z- AHRS_M2 msg: 0.54 phi; 0.37 theta; -0.09 psi 2021-02-13T00:59:32Z- handleDepth()-Depth_Keller msg: 1613177972557 epoch sec; seqNo:17563; depth 29.84 2021-02-13T00:59:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:32Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:59:32Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:59:32Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:59:32Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:59:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:59:32Z- handleDvl() - RDI_Pathfinder msg: 1613177972.56 epoch sec; seqNo:8472 2021-02-13T00:59:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:59:32Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177972.57 epoch sec; seqNo:17563; 4060558.85 north; 593493.39 east 2021-02-13T00:59:32Z- AHRS_M2 msg: 1613177972.95 epoch sec; seqNo:17426 2021-02-13T00:59:32Z- AHRS_M2 msg: 0.55 phi; 0.37 theta; -0.09 psi 2021-02-13T00:59:33Z- handleDepth()-Depth_Keller msg: 1613177973017 epoch sec; seqNo:17564; depth 29.73 2021-02-13T00:59:33Z- handleDvl() - RDI_Pathfinder msg: 1613177973.03 epoch sec; seqNo:8473 2021-02-13T00:59:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 78.51 , 97.99 , 60.08 2021-02-13T00:59:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.10 , -0.02 2021-02-13T00:59:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177973.04 epoch sec; seqNo:17564; 4060559.23 north; 593493.39 east 2021-02-13T00:59:33Z- AHRS_M2 msg: 1613177973.36 epoch sec; seqNo:17427 2021-02-13T00:59:33Z- AHRS_M2 msg: 0.55 phi; 0.37 theta; -0.09 psi 2021-02-13T00:59:33Z- handleDepth()-Depth_Keller msg: 1613177973372 epoch sec; seqNo:17565; depth 29.60 2021-02-13T00:59:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177973.40 epoch sec; seqNo:17565; 4060559.60 north; 593493.38 east 2021-02-13T00:59:33Z- AHRS_M2 msg: 1613177973.76 epoch sec; seqNo:17428 2021-02-13T00:59:33Z- AHRS_M2 msg: 0.55 phi; 0.37 theta; -0.09 psi 2021-02-13T00:59:33Z- handleDepth()-Depth_Keller msg: 1613177973785 epoch sec; seqNo:17566; depth 29.51 2021-02-13T00:59:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:59:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:59:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:59:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:59:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:59:33Z- handleDvl() - RDI_Pathfinder msg: 1613177973.79 epoch sec; seqNo:8474 2021-02-13T00:59:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:59:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.10 , -0.02 2021-02-13T00:59:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177973.83 epoch sec; seqNo:17566; 4060559.98 north; 593493.38 east 2021-02-13T00:59:34Z- AHRS_M2 msg: 1613177974.16 epoch sec; seqNo:17429 2021-02-13T00:59:34Z- AHRS_M2 msg: 0.55 phi; 0.37 theta; -0.09 psi 2021-02-13T00:59:34Z- handleDepth()-Depth_Keller msg: 1613177974185 epoch sec; seqNo:17567; depth 29.37 2021-02-13T00:59:34Z- handleDvl() - RDI_Pathfinder msg: 1613177974.20 epoch sec; seqNo:8475 2021-02-13T00:59:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 75.61 , 97.99 , 58.86 2021-02-13T00:59:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.11 , 0.04 2021-02-13T00:59:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177974.22 epoch sec; seqNo:17567; 4060559.98 north; 593493.38 east 2021-02-13T00:59:34Z- LcmTrn::handleMessages() - handling 47 messages took 0.80 ms and 131 calls... 2021-02-13T00:59:34Z- waiting for 1613177972.31, time is 1613177974.22 2021-02-13T00:59:34Z- LcmTrn::updateTrn() equating pose 1613177974.22 and meas times 1613177974.22 2021-02-13T00:59:34Z- LcmTrn::updateTrn() >>>> pose time 1613177974.22, meas time 1613177974.22 <<<< 2021-02-13T00:59:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177974.22 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:59:34Z- TerrainNav::motionUpdate() - currEstimate = (4060560, 593493.38, 29.37) 2021-02-13T00:59:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:59:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:59:34Z- TNavBF::Projecting Measurements 2021-02-13T00:59:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:59:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:59:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:59:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:59:34Z- TNavFilter::4 good beams. 2021-02-13T00:59:34Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:59:34Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:59:34Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:59:34Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:59:34Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:59:34Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:59:34Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:59:34Z- TNavBF::mmse selected from filter -1 2021-02-13T00:59:34Z- TerrainMapOctree: (vehN, vehE ) = (4060559.98, 593493.38). 2021-02-13T00:59:34Z- TerrainMapOctree: Min Distance = 1024.01. 2021-02-13T00:59:34Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:59:34Z- There is no specific good beam that all particles have in common. 2021-02-13T00:59:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:59:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.115 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-13T00:59:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:59:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 9.15 2021-02-13T00:59:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:59:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:59:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:59:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 5.54 2021-02-13T00:59:34Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:59:34Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:59:34Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:59:34Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:59:34Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:59:34Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:59:34Z- Filter 0 No beams used in subcloud update 2021-02-13T00:59:34Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:59:34Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:59:34Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 679.9 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:59:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177974.22 sec. 2021-02-13T00:59:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:59:34Z- TNavBF::mmse selected from filter -1 2021-02-13T00:59:34Z- variances([0], [2], and psi_berg) = 679.901309 832.988436 0.000000 2021-02-13T00:59:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:59:34Z- LcmTrn::publishEstimates() - MLE : 4060559.48 593511.88 29.37 2021-02-13T00:59:34Z- LcmTrn::publishEstimates() - MMSE : 4060559.98 593493.38 29.37 2021-02-13T00:59:34Z- LcmTrn::publishEstimates() - COVAR: 679.90 832.99 0.82 0.00 2021-02-13T00:59:34Z- AHRS_M2 msg: 1613177974.56 epoch sec; seqNo:17430 2021-02-13T00:59:34Z- AHRS_M2 msg: 0.55 phi; 0.37 theta; -0.09 psi 2021-02-13T00:59:34Z- handleDepth()-Depth_Keller msg: 1613177974574 epoch sec; seqNo:17568; depth 29.25 2021-02-13T00:59:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177974.58 epoch sec; seqNo:17568; 4060560.36 north; 593493.38 east 2021-02-13T00:59:34Z- AHRS_M2 msg: 1613177974.97 epoch sec; seqNo:17431 2021-02-13T00:59:34Z- AHRS_M2 msg: 0.56 phi; 0.37 theta; -0.09 psi 2021-02-13T00:59:34Z- handleDepth()-Depth_Keller msg: 1613177974973 epoch sec; seqNo:17569; depth 29.14 2021-02-13T00:59:35Z- handleDvl() - RDI_Pathfinder msg: 1613177974.98 epoch sec; seqNo:8476 2021-02-13T00:59:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:59:35Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177975.01 epoch sec; seqNo:17569; 4060560.74 north; 593493.37 east 2021-02-13T00:59:35Z- AHRS_M2 msg: 1613177975.37 epoch sec; seqNo:17432 2021-02-13T00:59:35Z- AHRS_M2 msg: 0.56 phi; 0.37 theta; -0.09 psi 2021-02-13T00:59:35Z- handleDepth()-Depth_Keller msg: 1613177975376 epoch sec; seqNo:17570; depth 29.01 2021-02-13T00:59:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177975.39 epoch sec; seqNo:17570; 4060561.12 north; 593493.37 east 2021-02-13T00:59:35Z- AHRS_M2 msg: 1613177975.78 epoch sec; seqNo:17433 2021-02-13T00:59:35Z- AHRS_M2 msg: 0.57 phi; 0.37 theta; -0.09 psi 2021-02-13T00:59:35Z- handleDepth()-Depth_Keller msg: 1613177975786 epoch sec; seqNo:17571; depth 28.92 2021-02-13T00:59:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177975.80 epoch sec; seqNo:17571; 4060561.51 north; 593494.58 east 2021-02-13T00:59:36Z- AHRS_M2 msg: 1613177976.18 epoch sec; seqNo:17434 2021-02-13T00:59:36Z- AHRS_M2 msg: 0.57 phi; 0.37 theta; -0.09 psi 2021-02-13T00:59:36Z- handleDepth()-Depth_Keller msg: 1613177976185 epoch sec; seqNo:17572; depth 28.78 2021-02-13T00:59:36Z- handleDvl() - RDI_Pathfinder msg: 1613177976.20 epoch sec; seqNo:8477 2021-02-13T00:59:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 79.53 , 97.11 , 58.45 2021-02-13T00:59:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , 0.07 2021-02-13T00:59:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177976.21 epoch sec; seqNo:17572; 4060561.51 north; 593494.58 east 2021-02-13T00:59:36Z- AHRS_M2 msg: 1613177976.58 epoch sec; seqNo:17435 2021-02-13T00:59:36Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.09 psi 2021-02-13T00:59:36Z- handleDepth()-Depth_Keller msg: 1613177976589 epoch sec; seqNo:17573; depth 28.65 2021-02-13T00:59:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177976.60 epoch sec; seqNo:17573; 4060561.89 north; 593494.58 east 2021-02-13T00:59:36Z- AHRS_M2 msg: 1613177976.99 epoch sec; seqNo:17436 2021-02-13T00:59:36Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.09 psi 2021-02-13T00:59:36Z- handleDepth()-Depth_Keller msg: 1613177976997 epoch sec; seqNo:17574; depth 28.53 2021-02-13T00:59:37Z- handleDvl() - RDI_Pathfinder msg: 1613177977.01 epoch sec; seqNo:8478 2021-02-13T00:59:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.71 , 78.51 , 97.99 , 61.31 2021-02-13T00:59:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , 0.01 2021-02-13T00:59:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177977.02 epoch sec; seqNo:17574; 4060562.27 north; 593494.58 east 2021-02-13T00:59:37Z- AHRS_M2 msg: 1613177977.39 epoch sec; seqNo:17437 2021-02-13T00:59:37Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.09 psi 2021-02-13T00:59:37Z- handleDepth()-Depth_Keller msg: 1613177977398 epoch sec; seqNo:17575; depth 28.41 2021-02-13T00:59:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:59:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:59:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:59:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:59:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:59:37Z- handleDvl() - RDI_Pathfinder msg: 1613177977.40 epoch sec; seqNo:8479 2021-02-13T00:59:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:59:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , 0.01 2021-02-13T00:59:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177977.41 epoch sec; seqNo:17575; 4060562.65 north; 593494.57 east 2021-02-13T00:59:37Z- AHRS_M2 msg: 1613177977.80 epoch sec; seqNo:17438 2021-02-13T00:59:37Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.09 psi 2021-02-13T00:59:37Z- handleDepth()-Depth_Keller msg: 1613177977800 epoch sec; seqNo:17576; depth 28.28 2021-02-13T00:59:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177977.81 epoch sec; seqNo:17576; 4060562.65 north; 593494.57 east 2021-02-13T00:59:38Z- AHRS_M2 msg: 1613177978.20 epoch sec; seqNo:17439 2021-02-13T00:59:38Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.09 psi 2021-02-13T00:59:38Z- handleDepth()-Depth_Keller msg: 1613177978205 epoch sec; seqNo:17577; depth 28.15 2021-02-13T00:59:38Z- handleDvl() - RDI_Pathfinder msg: 1613177978.22 epoch sec; seqNo:8480 2021-02-13T00:59:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.71 , 79.59 , 97.99 , 61.31 2021-02-13T00:59:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , -0.06 2021-02-13T00:59:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177978.23 epoch sec; seqNo:17577; 4060563.04 north; 593495.79 east 2021-02-13T00:59:38Z- AHRS_M2 msg: 1613177978.60 epoch sec; seqNo:17440 2021-02-13T00:59:38Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.10 psi 2021-02-13T00:59:38Z- handleDepth()-Depth_Keller msg: 1613177978609 epoch sec; seqNo:17578; depth 28.04 2021-02-13T00:59:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:38Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:59:38Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:59:38Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:59:38Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:59:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:59:38Z- handleDvl() - RDI_Pathfinder msg: 1613177978.61 epoch sec; seqNo:8481 2021-02-13T00:59:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:59:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , -0.06 2021-02-13T00:59:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177978.62 epoch sec; seqNo:17578; 4060563.42 north; 593495.78 east 2021-02-13T00:59:39Z- AHRS_M2 msg: 1613177979.01 epoch sec; seqNo:17441 2021-02-13T00:59:39Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.11 psi 2021-02-13T00:59:39Z- handleDepth()-Depth_Keller msg: 1613177979021 epoch sec; seqNo:17579; depth 27.91 2021-02-13T00:59:39Z- handleDvl() - RDI_Pathfinder msg: 1613177979.03 epoch sec; seqNo:8482 2021-02-13T00:59:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 0.00 , 97.99 , 61.72 2021-02-13T00:59:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , -0.13 2021-02-13T00:59:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177979.05 epoch sec; seqNo:17579; 4060563.42 north; 593495.78 east 2021-02-13T00:59:39Z- AHRS_M2 msg: 1613177979.41 epoch sec; seqNo:17442 2021-02-13T00:59:39Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.12 psi 2021-02-13T00:59:39Z- handleDepth()-Depth_Keller msg: 1613177979421 epoch sec; seqNo:17580; depth 27.76 2021-02-13T00:59:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177979.44 epoch sec; seqNo:17580; 4060563.80 north; 593495.78 east 2021-02-13T00:59:39Z- LcmTrn::handleMessages() - handling 46 messages took 0.68 ms and 133 calls... 2021-02-13T00:59:39Z- waiting for 1613177977.22, time is 1613177979.44 2021-02-13T00:59:39Z- LcmTrn::updateTrn() >>>> pose time 1613177979.44, meas time 1613177979.03 <<<< 2021-02-13T00:59:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177979.03 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:59:39Z- TerrainNav::motionUpdate() - currEstimate = (4060564, 593495.78, 27.76) 2021-02-13T00:59:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:59:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:59:39Z- TNavBF::Projecting Measurements 2021-02-13T00:59:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:59:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:59:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:59:39Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T00:59:39Z- TNavFilter::3 good beams. 2021-02-13T00:59:39Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:59:39Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:59:39Z- TNavBF:: beamIndex[1] = 2 2021-02-13T00:59:39Z- TNavBF:: beamIndex[2] = 3 2021-02-13T00:59:39Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:59:39Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T00:59:39Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:59:39Z- TNavBF::mmse selected from filter -1 2021-02-13T00:59:39Z- TerrainMapOctree: (vehN, vehE ) = (4060563.50, 593495.59). 2021-02-13T00:59:39Z- TerrainMapOctree: Min Distance = 1020.01. 2021-02-13T00:59:39Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:59:39Z- There is no specific good beam that all particles have in common. 2021-02-13T00:59:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:59:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T00:59:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:59:39Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:59:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:59:39Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 6.09 2021-02-13T00:59:39Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:59:39Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:59:39Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:59:39Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:59:39Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:59:39Z- Filter 0 No beams used in subcloud update 2021-02-13T00:59:39Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:59:39Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:59:39Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 679.9 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T00:59:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177979.03 sec. 2021-02-13T00:59:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:59:39Z- TNavBF::mmse selected from filter -1 2021-02-13T00:59:39Z- variances([0], [2], and psi_berg) = 679.843638 832.988412 0.000000 2021-02-13T00:59:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:59:39Z- LcmTrn::publishEstimates() - MLE : 4060563.30 593514.28 27.76 2021-02-13T00:59:39Z- LcmTrn::publishEstimates() - MMSE : 4060563.80 593495.78 27.76 2021-02-13T00:59:39Z- LcmTrn::publishEstimates() - COVAR: 679.84 832.99 0.82 0.00 2021-02-13T00:59:39Z- AHRS_M2 msg: 1613177979.82 epoch sec; seqNo:17443 2021-02-13T00:59:39Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.13 psi 2021-02-13T00:59:39Z- handleDepth()-Depth_Keller msg: 1613177979820 epoch sec; seqNo:17581; depth 27.63 2021-02-13T00:59:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T00:59:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T00:59:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T00:59:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T00:59:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T00:59:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T00:59:39Z- handleDvl() - RDI_Pathfinder msg: 1613177979.82 epoch sec; seqNo:8483 2021-02-13T00:59:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:59:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , -0.13 2021-02-13T00:59:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177979.83 epoch sec; seqNo:17581; 4060564.18 north; 593495.78 east 2021-02-13T00:59:40Z- AHRS_M2 msg: 1613177980.22 epoch sec; seqNo:17444 2021-02-13T00:59:40Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.14 psi 2021-02-13T00:59:40Z- handleDepth()-Depth_Keller msg: 1613177980225 epoch sec; seqNo:17582; depth 27.51 2021-02-13T00:59:40Z- handleDvl() - RDI_Pathfinder msg: 1613177980.24 epoch sec; seqNo:8484 2021-02-13T00:59:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 82.84 , 97.99 , 61.31 2021-02-13T00:59:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , -0.11 2021-02-13T00:59:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177980.25 epoch sec; seqNo:17582; 4060564.18 north; 593495.78 east 2021-02-13T00:59:40Z- AHRS_M2 msg: 1613177980.62 epoch sec; seqNo:17445 2021-02-13T00:59:40Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.15 psi 2021-02-13T00:59:40Z- handleDepth()-Depth_Keller msg: 1613177980631 epoch sec; seqNo:17583; depth 27.37 2021-02-13T00:59:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177980.64 epoch sec; seqNo:17583; 4060564.56 north; 593495.77 east 2021-02-13T00:59:41Z- AHRS_M2 msg: 1613177981.03 epoch sec; seqNo:17446 2021-02-13T00:59:41Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-13T00:59:41Z- handleDepth()-Depth_Keller msg: 1613177981032 epoch sec; seqNo:17584; depth 27.24 2021-02-13T00:59:41Z- handleDvl() - RDI_Pathfinder msg: 1613177981.05 epoch sec; seqNo:8485 2021-02-13T00:59:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 82.84 , 97.99 , 60.90 2021-02-13T00:59:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.13 , -0.03 2021-02-13T00:59:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177981.06 epoch sec; seqNo:17584; 4060564.93 north; 593495.77 east 2021-02-13T00:59:41Z- AHRS_M2 msg: 1613177981.44 epoch sec; seqNo:17447 2021-02-13T00:59:41Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-13T00:59:41Z- handleDepth()-Depth_Keller msg: 1613177981442 epoch sec; seqNo:17585; depth 27.10 2021-02-13T00:59:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177981.47 epoch sec; seqNo:17585; 4060565.33 north; 593496.98 east 2021-02-13T00:59:41Z- AHRS_M2 msg: 1613177981.84 epoch sec; seqNo:17448 2021-02-13T00:59:41Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T00:59:41Z- handleDepth()-Depth_Keller msg: 1613177981841 epoch sec; seqNo:17586; depth 26.98 2021-02-13T00:59:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177981.85 epoch sec; seqNo:17586; 4060565.33 north; 593496.98 east 2021-02-13T00:59:42Z- AHRS_M2 msg: 1613177982.24 epoch sec; seqNo:17449 2021-02-13T00:59:42Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T00:59:42Z- handleDepth()-Depth_Keller msg: 1613177982253 epoch sec; seqNo:17587; depth 26.87 2021-02-13T00:59:42Z- handleDvl() - RDI_Pathfinder msg: 1613177982.27 epoch sec; seqNo:8486 2021-02-13T00:59:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 83.85 , 97.92 , 60.90 2021-02-13T00:59:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.15 , 0.05 2021-02-13T00:59:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177982.28 epoch sec; seqNo:17587; 4060565.71 north; 593496.98 east 2021-02-13T00:59:42Z- AHRS_M2 msg: 1613177982.64 epoch sec; seqNo:17450 2021-02-13T00:59:42Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.14 psi 2021-02-13T00:59:42Z- handleDepth()-Depth_Keller msg: 1613177982648 epoch sec; seqNo:17588; depth 26.72 2021-02-13T00:59:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177982.67 epoch sec; seqNo:17588; 4060566.08 north; 593496.98 east 2021-02-13T00:59:43Z- AHRS_M2 msg: 1613177983.05 epoch sec; seqNo:17451 2021-02-13T00:59:43Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.13 psi 2021-02-13T00:59:43Z- handleDepth()-Depth_Keller msg: 1613177983117 epoch sec; seqNo:17589; depth 26.61 2021-02-13T00:59:43Z- handleDvl() - RDI_Pathfinder msg: 1613177983.13 epoch sec; seqNo:8487 2021-02-13T00:59:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 0.00 , 97.99 , 62.94 2021-02-13T00:59:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.15 , 0.10 2021-02-13T00:59:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177983.15 epoch sec; seqNo:17589; 4060566.46 north; 593496.97 east 2021-02-13T00:59:43Z- AHRS_M2 msg: 1613177983.45 epoch sec; seqNo:17452 2021-02-13T00:59:43Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.12 psi 2021-02-13T00:59:43Z- handleDepth()-Depth_Keller msg: 1613177983463 epoch sec; seqNo:17590; depth 26.46 2021-02-13T00:59:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177983.47 epoch sec; seqNo:17590; 4060566.84 north; 593496.97 east 2021-02-13T00:59:43Z- AHRS_M2 msg: 1613177983.86 epoch sec; seqNo:17453 2021-02-13T00:59:43Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.12 psi 2021-02-13T00:59:43Z- handleDepth()-Depth_Keller msg: 1613177983877 epoch sec; seqNo:17591; depth 26.36 2021-02-13T00:59:43Z- handleDvl() - RDI_Pathfinder msg: 1613177983.89 epoch sec; seqNo:8488 2021-02-13T00:59:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:59:43Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177983.90 epoch sec; seqNo:17591; 4060567.22 north; 593496.96 east 2021-02-13T00:59:44Z- AHRS_M2 msg: 1613177984.26 epoch sec; seqNo:17454 2021-02-13T00:59:44Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.12 psi 2021-02-13T00:59:44Z- handleDepth()-Depth_Keller msg: 1613177984289 epoch sec; seqNo:17592; depth 26.23 2021-02-13T00:59:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177984.30 epoch sec; seqNo:17592; 4060567.23 north; 593498.18 east 2021-02-13T00:59:44Z- LcmTrn::handleMessages() - handling 42 messages took 0.64 ms and 129 calls... 2021-02-13T00:59:44Z- waiting for 1613177982.44, time is 1613177984.30 2021-02-13T00:59:44Z- LcmTrn::updateTrn() >>>> pose time 1613177984.30, meas time 1613177983.89 <<<< 2021-02-13T00:59:44Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177983.89, ping # 0. 2021-02-13T00:59:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177983.89 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:59:44Z- TerrainNav::motionUpdate() - currEstimate = (4060567, 593498.18, 26.23) 2021-02-13T00:59:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:59:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:59:44Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:59:44Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:59:44Z- TNavBF::Projecting Measurements 2021-02-13T00:59:44Z- TNavFilter::Measurement from time = 1613177983.89 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:59:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:59:44Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:59:44Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:59:44Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:59:44Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:59:44Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:59:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:59:44Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 679.8 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T00:59:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:59:44Z- TNavBF::mmse selected from filter -1 2021-02-13T00:59:44Z- variances([0], [2], and psi_berg) = 679.787670 832.988388 0.000000 2021-02-13T00:59:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:59:44Z- LcmTrn::publishEstimates() - MLE : 4060566.73 593516.68 26.23 2021-02-13T00:59:44Z- LcmTrn::publishEstimates() - MMSE : 4060567.23 593498.18 26.23 2021-02-13T00:59:44Z- LcmTrn::publishEstimates() - COVAR: 679.79 832.99 1.04 0.00 2021-02-13T00:59:44Z- AHRS_M2 msg: 1613177984.66 epoch sec; seqNo:17455 2021-02-13T00:59:44Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.13 psi 2021-02-13T00:59:44Z- handleDepth()-Depth_Keller msg: 1613177984668 epoch sec; seqNo:17593; depth 26.12 2021-02-13T00:59:44Z- handleDvl() - RDI_Pathfinder msg: 1613177984.68 epoch sec; seqNo:8489 2021-02-13T00:59:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 71.30 , 0.00 , 0.00 , 62.56 2021-02-13T00:59:44Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177984.71 epoch sec; seqNo:17593; 4060567.61 north; 593498.18 east 2021-02-13T00:59:45Z- AHRS_M2 msg: 1613177985.07 epoch sec; seqNo:17456 2021-02-13T00:59:45Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.13 psi 2021-02-13T00:59:45Z- handleDepth()-Depth_Keller msg: 1613177985077 epoch sec; seqNo:17594; depth 25.99 2021-02-13T00:59:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177985.09 epoch sec; seqNo:17594; 4060567.99 north; 593498.18 east 2021-02-13T00:59:45Z- AHRS_M2 msg: 1613177985.47 epoch sec; seqNo:17457 2021-02-13T00:59:45Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-13T00:59:45Z- handleDepth()-Depth_Keller msg: 1613177985479 epoch sec; seqNo:17595; depth 25.86 2021-02-13T00:59:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177985.49 epoch sec; seqNo:17595; 4060568.37 north; 593498.17 east 2021-02-13T00:59:45Z- AHRS_M2 msg: 1613177985.88 epoch sec; seqNo:17458 2021-02-13T00:59:45Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-13T00:59:45Z- handleDepth()-Depth_Keller msg: 1613177985880 epoch sec; seqNo:17596; depth 25.75 2021-02-13T00:59:45Z- handleDvl() - RDI_Pathfinder msg: 1613177985.89 epoch sec; seqNo:8490 2021-02-13T00:59:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 63.48 2021-02-13T00:59:45Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177985.90 epoch sec; seqNo:17596; 4060568.75 north; 593498.17 east 2021-02-13T00:59:46Z- AHRS_M2 msg: 1613177986.28 epoch sec; seqNo:17459 2021-02-13T00:59:46Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.15 psi 2021-02-13T00:59:46Z- handleDepth()-Depth_Keller msg: 1613177986356 epoch sec; seqNo:17597; depth 25.63 2021-02-13T00:59:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177986.37 epoch sec; seqNo:17597; 4060569.13 north; 593498.16 east 2021-02-13T00:59:46Z- AHRS_M2 msg: 1613177986.68 epoch sec; seqNo:17460 2021-02-13T00:59:46Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-13T00:59:46Z- handleDepth()-Depth_Keller msg: 1613177986701 epoch sec; seqNo:17598; depth 25.50 2021-02-13T00:59:46Z- handleDvl() - RDI_Pathfinder msg: 1613177986.74 epoch sec; seqNo:8491 2021-02-13T00:59:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 70.84 , 0.00 , 0.00 , 63.48 2021-02-13T00:59:46Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177986.77 epoch sec; seqNo:17598; 4060569.13 north; 593498.16 east 2021-02-13T00:59:47Z- AHRS_M2 msg: 1613177987.09 epoch sec; seqNo:17461 2021-02-13T00:59:47Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-13T00:59:47Z- handleDepth()-Depth_Keller msg: 1613177987112 epoch sec; seqNo:17599; depth 25.38 2021-02-13T00:59:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177987.14 epoch sec; seqNo:17599; 4060569.51 north; 593498.16 east 2021-02-13T00:59:47Z- AHRS_M2 msg: 1613177987.49 epoch sec; seqNo:17462 2021-02-13T00:59:47Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-13T00:59:47Z- handleDepth()-Depth_Keller msg: 1613177987510 epoch sec; seqNo:17600; depth 25.28 2021-02-13T00:59:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177987.52 epoch sec; seqNo:17600; 4060569.90 north; 593499.37 east 2021-02-13T00:59:47Z- AHRS_M2 msg: 1613177987.90 epoch sec; seqNo:17463 2021-02-13T00:59:47Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-13T00:59:47Z- handleDepth()-Depth_Keller msg: 1613177987901 epoch sec; seqNo:17601; depth 25.17 2021-02-13T00:59:47Z- handleDvl() - RDI_Pathfinder msg: 1613177987.91 epoch sec; seqNo:8492 2021-02-13T00:59:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:59:47Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177987.95 epoch sec; seqNo:17601; 4060570.28 north; 593499.37 east 2021-02-13T00:59:48Z- AHRS_M2 msg: 1613177988.30 epoch sec; seqNo:17464 2021-02-13T00:59:48Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.17 psi 2021-02-13T00:59:48Z- handleDepth()-Depth_Keller msg: 1613177988304 epoch sec; seqNo:17602; depth 25.04 2021-02-13T00:59:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177988.32 epoch sec; seqNo:17602; 4060570.66 north; 593499.37 east 2021-02-13T00:59:48Z- AHRS_M2 msg: 1613177988.71 epoch sec; seqNo:17465 2021-02-13T00:59:48Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.18 psi 2021-02-13T00:59:48Z- handleDepth()-Depth_Keller msg: 1613177988719 epoch sec; seqNo:17603; depth 24.95 2021-02-13T00:59:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177988.73 epoch sec; seqNo:17603; 4060571.04 north; 593499.36 east 2021-02-13T00:59:49Z- AHRS_M2 msg: 1613177989.11 epoch sec; seqNo:17466 2021-02-13T00:59:49Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.18 psi 2021-02-13T00:59:49Z- handleDepth()-Depth_Keller msg: 1613177989115 epoch sec; seqNo:17604; depth 24.82 2021-02-13T00:59:49Z- handleDvl() - RDI_Pathfinder msg: 1613177989.14 epoch sec; seqNo:8493 2021-02-13T00:59:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:59:49Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177989.15 epoch sec; seqNo:17604; 4060571.04 north; 593499.36 east 2021-02-13T00:59:49Z- AHRS_M2 msg: 1613177989.52 epoch sec; seqNo:17467 2021-02-13T00:59:49Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.19 psi 2021-02-13T00:59:49Z- handleDepth()-Depth_Keller msg: 1613177989522 epoch sec; seqNo:17605; depth 24.72 2021-02-13T00:59:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177989.55 epoch sec; seqNo:17605; 4060571.41 north; 593499.36 east 2021-02-13T00:59:49Z- LcmTrn::handleMessages() - handling 44 messages took 0.77 ms and 129 calls... 2021-02-13T00:59:49Z- waiting for 1613177987.30, time is 1613177989.55 2021-02-13T00:59:49Z- LcmTrn::updateTrn() >>>> pose time 1613177989.55, meas time 1613177989.14 <<<< 2021-02-13T00:59:49Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177989.14, ping # 0. 2021-02-13T00:59:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177989.14 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:59:49Z- TerrainNav::motionUpdate() - currEstimate = (4060571, 593499.36, 24.72) 2021-02-13T00:59:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:59:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:59:49Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:59:49Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:59:49Z- TNavBF::Projecting Measurements 2021-02-13T00:59:49Z- TNavFilter::Measurement from time = 1613177989.14 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:59:49Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:59:49Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:59:49Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:59:49Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:59:49Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:59:49Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:59:49Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:59:49Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 679.7 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T00:59:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:59:49Z- TNavBF::mmse selected from filter -1 2021-02-13T00:59:49Z- variances([0], [2], and psi_berg) = 679.731117 832.988364 0.000000 2021-02-13T00:59:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:59:49Z- LcmTrn::publishEstimates() - MLE : 4060570.91 593515.86 24.72 2021-02-13T00:59:49Z- LcmTrn::publishEstimates() - MMSE : 4060571.41 593499.36 24.72 2021-02-13T00:59:49Z- LcmTrn::publishEstimates() - COVAR: 679.73 832.99 1.26 0.00 2021-02-13T00:59:49Z- AHRS_M2 msg: 1613177989.92 epoch sec; seqNo:17468 2021-02-13T00:59:49Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.19 psi 2021-02-13T00:59:49Z- handleDepth()-Depth_Keller msg: 1613177989922 epoch sec; seqNo:17606; depth 24.62 2021-02-13T00:59:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177989.93 epoch sec; seqNo:17606; 4060571.79 north; 593499.35 east 2021-02-13T00:59:50Z- AHRS_M2 msg: 1613177990.32 epoch sec; seqNo:17469 2021-02-13T00:59:50Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.19 psi 2021-02-13T00:59:50Z- handleDepth()-Depth_Keller msg: 1613177990333 epoch sec; seqNo:17607; depth 24.49 2021-02-13T00:59:50Z- handleDvl() - RDI_Pathfinder msg: 1613177990.35 epoch sec; seqNo:8494 2021-02-13T00:59:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.90 , 88.32 , 97.24 , 77.90 2021-02-13T00:59:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.03 2021-02-13T00:59:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177990.36 epoch sec; seqNo:17607; 4060572.19 north; 593500.57 east 2021-02-13T00:59:50Z- AHRS_M2 msg: 1613177990.72 epoch sec; seqNo:17470 2021-02-13T00:59:50Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.19 psi 2021-02-13T00:59:50Z- handleDepth()-Depth_Keller msg: 1613177990728 epoch sec; seqNo:17608; depth 24.41 2021-02-13T00:59:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177990.74 epoch sec; seqNo:17608; 4060572.19 north; 593500.57 east 2021-02-13T00:59:51Z- AHRS_M2 msg: 1613177991.13 epoch sec; seqNo:17471 2021-02-13T00:59:51Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.19 psi 2021-02-13T00:59:51Z- handleDepth()-Depth_Keller msg: 1613177991135 epoch sec; seqNo:17609; depth 24.29 2021-02-13T00:59:51Z- handleDvl() - RDI_Pathfinder msg: 1613177991.15 epoch sec; seqNo:8495 2021-02-13T00:59:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.30 , 127.40 , 98.19 , 65.40 2021-02-13T00:59:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , -0.04 2021-02-13T00:59:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177991.16 epoch sec; seqNo:17609; 4060572.56 north; 593500.57 east 2021-02-13T00:59:51Z- AHRS_M2 msg: 1613177991.53 epoch sec; seqNo:17472 2021-02-13T00:59:51Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.19 psi 2021-02-13T00:59:51Z- handleDepth()-Depth_Keller msg: 1613177991538 epoch sec; seqNo:17610; depth 24.17 2021-02-13T00:59:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177991.55 epoch sec; seqNo:17610; 4060572.94 north; 593500.56 east 2021-02-13T00:59:51Z- AHRS_M2 msg: 1613177991.94 epoch sec; seqNo:17473 2021-02-13T00:59:51Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.19 psi 2021-02-13T00:59:51Z- handleDepth()-Depth_Keller msg: 1613177991975 epoch sec; seqNo:17611; depth 24.07 2021-02-13T00:59:51Z- handleDvl() - RDI_Pathfinder msg: 1613177991.99 epoch sec; seqNo:8496 2021-02-13T00:59:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.71 , 89.20 , 97.71 , 64.17 2021-02-13T00:59:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , -0.04 2021-02-13T00:59:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177992.00 epoch sec; seqNo:17611; 4060573.32 north; 593500.56 east 2021-02-13T00:59:52Z- AHRS_M2 msg: 1613177992.34 epoch sec; seqNo:17474 2021-02-13T00:59:52Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.18 psi 2021-02-13T00:59:52Z- handleDepth()-Depth_Keller msg: 1613177992344 epoch sec; seqNo:17612; depth 23.95 2021-02-13T00:59:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177992.36 epoch sec; seqNo:17612; 4060573.32 north; 593500.56 east 2021-02-13T00:59:52Z- AHRS_M2 msg: 1613177992.74 epoch sec; seqNo:17475 2021-02-13T00:59:52Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-13T00:59:52Z- handleDepth()-Depth_Keller msg: 1613177992761 epoch sec; seqNo:17613; depth 23.86 2021-02-13T00:59:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177992.77 epoch sec; seqNo:17613; 4060573.71 north; 593501.77 east 2021-02-13T00:59:53Z- AHRS_M2 msg: 1613177993.15 epoch sec; seqNo:17476 2021-02-13T00:59:53Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-13T00:59:53Z- handleDepth()-Depth_Keller msg: 1613177993156 epoch sec; seqNo:17614; depth 23.74 2021-02-13T00:59:53Z- handleDvl() - RDI_Pathfinder msg: 1613177993.17 epoch sec; seqNo:8497 2021-02-13T00:59:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 0.00 , 97.92 , 65.81 2021-02-13T00:59:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , -0.07 2021-02-13T00:59:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177993.19 epoch sec; seqNo:17614; 4060574.09 north; 593501.77 east 2021-02-13T00:59:53Z- AHRS_M2 msg: 1613177993.55 epoch sec; seqNo:17477 2021-02-13T00:59:53Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-13T00:59:53Z- handleDepth()-Depth_Keller msg: 1613177993561 epoch sec; seqNo:17615; depth 23.63 2021-02-13T00:59:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177993.57 epoch sec; seqNo:17615; 4060574.47 north; 593501.77 east 2021-02-13T00:59:53Z- AHRS_M2 msg: 1613177993.96 epoch sec; seqNo:17478 2021-02-13T00:59:53Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-13T00:59:53Z- handleDepth()-Depth_Keller msg: 1613177993974 epoch sec; seqNo:17616; depth 23.52 2021-02-13T00:59:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177994.00 epoch sec; seqNo:17616; 4060574.47 north; 593501.77 east 2021-02-13T00:59:54Z- AHRS_M2 msg: 1613177994.36 epoch sec; seqNo:17479 2021-02-13T00:59:54Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-13T00:59:54Z- handleDepth()-Depth_Keller msg: 1613177994365 epoch sec; seqNo:17617; depth 23.43 2021-02-13T00:59:54Z- handleDvl() - RDI_Pathfinder msg: 1613177994.38 epoch sec; seqNo:8498 2021-02-13T00:59:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T00:59:54Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T00:59:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177994.39 epoch sec; seqNo:17617; 4060574.85 north; 593501.76 east 2021-02-13T00:59:54Z- LcmTrn::handleMessages() - handling 41 messages took 0.73 ms and 127 calls... 2021-02-13T00:59:54Z- waiting for 1613177992.55, time is 1613177994.39 2021-02-13T00:59:54Z- LcmTrn::updateTrn() equating pose 1613177994.39 and meas times 1613177994.39 2021-02-13T00:59:54Z- LcmTrn::updateTrn() >>>> pose time 1613177994.39, meas time 1613177994.39 <<<< 2021-02-13T00:59:54Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613177994.39, ping # 0. 2021-02-13T00:59:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177994.39 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:59:54Z- TerrainNav::motionUpdate() - currEstimate = (4060575, 593501.76, 23.43) 2021-02-13T00:59:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:59:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:59:54Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T00:59:54Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T00:59:54Z- TNavBF::Projecting Measurements 2021-02-13T00:59:54Z- TNavFilter::Measurement from time = 1613177994.39 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T00:59:54Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:59:54Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:59:54Z- TNavBF:: beamIndex[1] = 0 2021-02-13T00:59:54Z- TNavBF:: beamIndex[2] = 0 2021-02-13T00:59:54Z- TNavBF:: beamIndex[3] = 0 2021-02-13T00:59:54Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:59:54Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:59:54Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 679.7 EastVariance: 833.0 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-13T00:59:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:59:54Z- TNavBF::mmse selected from filter -1 2021-02-13T00:59:54Z- variances([0], [2], and psi_berg) = 679.686301 832.988345 0.000000 2021-02-13T00:59:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:59:54Z- LcmTrn::publishEstimates() - MLE : 4060574.35 593518.26 23.43 2021-02-13T00:59:54Z- LcmTrn::publishEstimates() - MMSE : 4060574.85 593501.76 23.43 2021-02-13T00:59:54Z- LcmTrn::publishEstimates() - COVAR: 679.69 832.99 1.47 0.00 2021-02-13T00:59:54Z- AHRS_M2 msg: 1613177994.76 epoch sec; seqNo:17480 2021-02-13T00:59:54Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-13T00:59:54Z- handleDepth()-Depth_Keller msg: 1613177994769 epoch sec; seqNo:17618; depth 23.31 2021-02-13T00:59:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177994.78 epoch sec; seqNo:17618; 4060575.23 north; 593501.76 east 2021-02-13T00:59:55Z- AHRS_M2 msg: 1613177995.17 epoch sec; seqNo:17481 2021-02-13T00:59:55Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.16 psi 2021-02-13T00:59:55Z- handleDepth()-Depth_Keller msg: 1613177995176 epoch sec; seqNo:17619; depth 23.20 2021-02-13T00:59:55Z- handleDvl() - RDI_Pathfinder msg: 1613177995.19 epoch sec; seqNo:8499 2021-02-13T00:59:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.71 , 86.69 , 97.51 , 65.40 2021-02-13T00:59:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.08 , -0.04 2021-02-13T00:59:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177995.20 epoch sec; seqNo:17619; 4060575.62 north; 593502.97 east 2021-02-13T00:59:55Z- AHRS_M2 msg: 1613177995.57 epoch sec; seqNo:17482 2021-02-13T00:59:55Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-13T00:59:55Z- handleDepth()-Depth_Keller msg: 1613177995578 epoch sec; seqNo:17620; depth 23.08 2021-02-13T00:59:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177995.59 epoch sec; seqNo:17620; 4060575.62 north; 593502.97 east 2021-02-13T00:59:55Z- AHRS_M2 msg: 1613177995.97 epoch sec; seqNo:17483 2021-02-13T00:59:55Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-13T00:59:55Z- handleDepth()-Depth_Keller msg: 1613177995985 epoch sec; seqNo:17621; depth 22.99 2021-02-13T00:59:56Z- handleDvl() - RDI_Pathfinder msg: 1613177996.00 epoch sec; seqNo:8500 2021-02-13T00:59:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.53 , 89.26 , 97.92 , 65.40 2021-02-13T00:59:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.08 , 0.04 2021-02-13T00:59:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177996.01 epoch sec; seqNo:17621; 4060576.00 north; 593502.97 east 2021-02-13T00:59:56Z- AHRS_M2 msg: 1613177996.38 epoch sec; seqNo:17484 2021-02-13T00:59:56Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-13T00:59:56Z- handleDepth()-Depth_Keller msg: 1613177996448 epoch sec; seqNo:17622; depth 22.88 2021-02-13T00:59:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177996.46 epoch sec; seqNo:17622; 4060576.38 north; 593502.97 east 2021-02-13T00:59:56Z- AHRS_M2 msg: 1613177996.79 epoch sec; seqNo:17485 2021-02-13T00:59:56Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-13T00:59:56Z- handleDepth()-Depth_Keller msg: 1613177996797 epoch sec; seqNo:17623; depth 22.76 2021-02-13T00:59:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177996.81 epoch sec; seqNo:17623; 4060576.76 north; 593502.96 east 2021-02-13T00:59:57Z- AHRS_M2 msg: 1613177997.19 epoch sec; seqNo:17486 2021-02-13T00:59:57Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-13T00:59:57Z- handleDepth()-Depth_Keller msg: 1613177997213 epoch sec; seqNo:17624; depth 22.67 2021-02-13T00:59:57Z- handleDvl() - RDI_Pathfinder msg: 1613177997.23 epoch sec; seqNo:8501 2021-02-13T00:59:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 0.00 , 98.93 , 65.81 2021-02-13T00:59:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , 0.09 2021-02-13T00:59:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177997.24 epoch sec; seqNo:17624; 4060577.14 north; 593502.96 east 2021-02-13T00:59:57Z- AHRS_M2 msg: 1613177997.59 epoch sec; seqNo:17487 2021-02-13T00:59:57Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.17 psi 2021-02-13T00:59:57Z- handleDepth()-Depth_Keller msg: 1613177997618 epoch sec; seqNo:17625; depth 22.54 2021-02-13T00:59:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177997.63 epoch sec; seqNo:17625; 4060577.52 north; 593502.95 east 2021-02-13T00:59:57Z- AHRS_M2 msg: 1613177998.00 epoch sec; seqNo:17488 2021-02-13T00:59:57Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.17 psi 2021-02-13T00:59:58Z- handleDepth()-Depth_Keller msg: 1613177998005 epoch sec; seqNo:17626; depth 22.43 2021-02-13T00:59:58Z- handleDvl() - RDI_Pathfinder msg: 1613177998.02 epoch sec; seqNo:8502 2021-02-13T00:59:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.35 , 0.00 , 97.85 , 67.03 2021-02-13T00:59:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , 0.12 2021-02-13T00:59:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177998.03 epoch sec; seqNo:17626; 4060577.53 north; 593504.17 east 2021-02-13T00:59:58Z- AHRS_M2 msg: 1613177998.40 epoch sec; seqNo:17489 2021-02-13T00:59:58Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.16 psi 2021-02-13T00:59:58Z- handleDepth()-Depth_Keller msg: 1613177998404 epoch sec; seqNo:17627; depth 22.33 2021-02-13T00:59:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177998.42 epoch sec; seqNo:17627; 4060577.91 north; 593504.17 east 2021-02-13T00:59:58Z- AHRS_M2 msg: 1613177998.80 epoch sec; seqNo:17490 2021-02-13T00:59:58Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.16 psi 2021-02-13T00:59:58Z- handleDepth()-Depth_Keller msg: 1613177998808 epoch sec; seqNo:17628; depth 22.22 2021-02-13T00:59:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177998.83 epoch sec; seqNo:17628; 4060578.29 north; 593504.17 east 2021-02-13T00:59:59Z- AHRS_M2 msg: 1613177999.21 epoch sec; seqNo:17491 2021-02-13T00:59:59Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.16 psi 2021-02-13T00:59:59Z- handleDepth()-Depth_Keller msg: 1613177999217 epoch sec; seqNo:17629; depth 22.12 2021-02-13T00:59:59Z- handleDvl() - RDI_Pathfinder msg: 1613177999.23 epoch sec; seqNo:8503 2021-02-13T00:59:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 87.17 , 97.99 , 66.62 2021-02-13T00:59:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.09 , 0.07 2021-02-13T00:59:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177999.25 epoch sec; seqNo:17629; 4060578.67 north; 593504.16 east 2021-02-13T00:59:59Z- AHRS_M2 msg: 1613177999.61 epoch sec; seqNo:17492 2021-02-13T00:59:59Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.16 psi 2021-02-13T00:59:59Z- handleDepth()-Depth_Keller msg: 1613177999680 epoch sec; seqNo:17630; depth 22.02 2021-02-13T00:59:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613177999.69 epoch sec; seqNo:17630; 4060578.67 north; 593504.16 east 2021-02-13T00:59:59Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-13T00:59:59Z- waiting for 1613177997.39, time is 1613177999.69 2021-02-13T00:59:59Z- LcmTrn::updateTrn() >>>> pose time 1613177999.69, meas time 1613177999.23 <<<< 2021-02-13T00:59:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613177999.23 sec, ping # 0; waiting for more recent INS data... 2021-02-13T00:59:59Z- TerrainNav::motionUpdate() - currEstimate = (4060579, 593504.16, 22.02) 2021-02-13T00:59:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T00:59:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T00:59:59Z- TNavBF::Projecting Measurements 2021-02-13T00:59:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T00:59:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T00:59:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T00:59:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T00:59:59Z- TNavFilter::4 good beams. 2021-02-13T00:59:59Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T00:59:59Z- TNavBF:: beamIndex[0] = 0 2021-02-13T00:59:59Z- TNavBF:: beamIndex[1] = 1 2021-02-13T00:59:59Z- TNavBF:: beamIndex[2] = 2 2021-02-13T00:59:59Z- TNavBF:: beamIndex[3] = 3 2021-02-13T00:59:59Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T00:59:59Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T00:59:59Z- TNavBF::mmse selected from filter -1 2021-02-13T00:59:59Z- TerrainMapOctree: (vehN, vehE ) = (4060578.33, 593503.95). 2021-02-13T00:59:59Z- TerrainMapOctree: Min Distance = 1003.85. 2021-02-13T00:59:59Z- TerrainMapOctree: Using tile 1. 2021-02-13T00:59:59Z- There is no specific good beam that all particles have in common. 2021-02-13T00:59:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T00:59:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.68 K: 0.142 ExpectedMeasDiff: nan currMeas.covariance: 8.47 2021-02-13T00:59:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T00:59:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.44 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 12.16 2021-02-13T00:59:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T00:59:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T00:59:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T00:59:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.121 ExpectedMeasDiff: nan currMeas.covariance: 7.10 2021-02-13T00:59:59Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T00:59:59Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T00:59:59Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T00:59:59Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T00:59:59Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T00:59:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T00:59:59Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T00:59:59Z- Filter 0 No beams used in subcloud update 2021-02-13T00:59:59Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T00:59:59Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T00:59:59Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 679.6 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T00:59:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613177999.23 sec. 2021-02-13T00:59:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T00:59:59Z- TNavBF::mmse selected from filter -1 2021-02-13T00:59:59Z- variances([0], [2], and psi_berg) = 679.628225 832.988321 0.000000 2021-02-13T00:59:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T00:59:59Z- LcmTrn::publishEstimates() - MLE : 4060578.17 593523.66 22.02 2021-02-13T00:59:59Z- LcmTrn::publishEstimates() - MMSE : 4060578.67 593504.16 22.02 2021-02-13T00:59:59Z- LcmTrn::publishEstimates() - COVAR: 679.63 832.99 1.09 0.00 2021-02-13T01:00:00Z- AHRS_M2 msg: 1613178000.02 epoch sec; seqNo:17493 2021-02-13T01:00:00Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.16 psi 2021-02-13T01:00:00Z- handleDepth()-Depth_Keller msg: 1613178000030 epoch sec; seqNo:17631; depth 21.89 2021-02-13T01:00:00Z- handleDvl() - RDI_Pathfinder msg: 1613178000.04 epoch sec; seqNo:8504 2021-02-13T01:00:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.57 , 0.00 , 97.85 , 68.26 2021-02-13T01:00:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.08 , 0.00 2021-02-13T01:00:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178000.05 epoch sec; seqNo:17631; 4060579.06 north; 593505.38 east 2021-02-13T01:00:00Z- AHRS_M2 msg: 1613178000.42 epoch sec; seqNo:17494 2021-02-13T01:00:00Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-13T01:00:00Z- handleDepth()-Depth_Keller msg: 1613178000449 epoch sec; seqNo:17632; depth 21.80 2021-02-13T01:00:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178000.46 epoch sec; seqNo:17632; 4060579.44 north; 593505.37 east 2021-02-13T01:00:00Z- AHRS_M2 msg: 1613178000.82 epoch sec; seqNo:17495 2021-02-13T01:00:00Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-13T01:00:00Z- handleDepth()-Depth_Keller msg: 1613178000849 epoch sec; seqNo:17633; depth 21.68 2021-02-13T01:00:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178000.86 epoch sec; seqNo:17633; 4060579.82 north; 593505.37 east 2021-02-13T01:00:01Z- AHRS_M2 msg: 1613178001.23 epoch sec; seqNo:17496 2021-02-13T01:00:01Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.15 psi 2021-02-13T01:00:01Z- handleDepth()-Depth_Keller msg: 1613178001235 epoch sec; seqNo:17634; depth 21.60 2021-02-13T01:00:01Z- handleDvl() - RDI_Pathfinder msg: 1613178001.25 epoch sec; seqNo:8505 2021-02-13T01:00:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 88.25 , 97.99 , 67.03 2021-02-13T01:00:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.12 , -0.07 2021-02-13T01:00:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178001.26 epoch sec; seqNo:17634; 4060579.82 north; 593505.37 east 2021-02-13T01:00:01Z- AHRS_M2 msg: 1613178001.63 epoch sec; seqNo:17497 2021-02-13T01:00:01Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.15 psi 2021-02-13T01:00:01Z- handleDepth()-Depth_Keller msg: 1613178001639 epoch sec; seqNo:17635; depth 21.48 2021-02-13T01:00:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178001.65 epoch sec; seqNo:17635; 4060580.19 north; 593505.37 east 2021-02-13T01:00:02Z- AHRS_M2 msg: 1613178002.04 epoch sec; seqNo:17498 2021-02-13T01:00:02Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.16 psi 2021-02-13T01:00:02Z- handleDepth()-Depth_Keller msg: 1613178002040 epoch sec; seqNo:17636; depth 21.38 2021-02-13T01:00:02Z- handleDvl() - RDI_Pathfinder msg: 1613178002.06 epoch sec; seqNo:8506 2021-02-13T01:00:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:02Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:00:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178002.07 epoch sec; seqNo:17636; 4060580.59 north; 593506.58 east 2021-02-13T01:00:02Z- AHRS_M2 msg: 1613178002.44 epoch sec; seqNo:17499 2021-02-13T01:00:02Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.16 psi 2021-02-13T01:00:02Z- handleDepth()-Depth_Keller msg: 1613178002444 epoch sec; seqNo:17637; depth 21.27 2021-02-13T01:00:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178002.45 epoch sec; seqNo:17637; 4060580.59 north; 593506.58 east 2021-02-13T01:00:02Z- AHRS_M2 msg: 1613178002.84 epoch sec; seqNo:17500 2021-02-13T01:00:02Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.16 psi 2021-02-13T01:00:02Z- handleDepth()-Depth_Keller msg: 1613178002848 epoch sec; seqNo:17638; depth 21.19 2021-02-13T01:00:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178002.86 epoch sec; seqNo:17638; 4060580.97 north; 593506.58 east 2021-02-13T01:00:03Z- AHRS_M2 msg: 1613178003.25 epoch sec; seqNo:17501 2021-02-13T01:00:03Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.16 psi 2021-02-13T01:00:03Z- handleDepth()-Depth_Keller msg: 1613178003268 epoch sec; seqNo:17639; depth 21.07 2021-02-13T01:00:03Z- handleDvl() - RDI_Pathfinder msg: 1613178003.28 epoch sec; seqNo:8507 2021-02-13T01:00:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.57 , 89.87 , 98.39 , 68.26 2021-02-13T01:00:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , -0.14 2021-02-13T01:00:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178003.29 epoch sec; seqNo:17639; 4060581.34 north; 593506.57 east 2021-02-13T01:00:03Z- AHRS_M2 msg: 1613178003.65 epoch sec; seqNo:17502 2021-02-13T01:00:03Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.16 psi 2021-02-13T01:00:03Z- handleDepth()-Depth_Keller msg: 1613178003657 epoch sec; seqNo:17640; depth 20.95 2021-02-13T01:00:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178003.67 epoch sec; seqNo:17640; 4060581.34 north; 593506.57 east 2021-02-13T01:00:04Z- AHRS_M2 msg: 1613178004.07 epoch sec; seqNo:17503 2021-02-13T01:00:04Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.16 psi 2021-02-13T01:00:04Z- handleDepth()-Depth_Keller msg: 1613178004072 epoch sec; seqNo:17641; depth 20.84 2021-02-13T01:00:04Z- handleDvl() - RDI_Pathfinder msg: 1613178004.08 epoch sec; seqNo:8508 2021-02-13T01:00:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 89.87 , 98.39 , 78.92 2021-02-13T01:00:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , -0.10 2021-02-13T01:00:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178004.09 epoch sec; seqNo:17641; 4060581.72 north; 593506.57 east 2021-02-13T01:00:04Z- AHRS_M2 msg: 1613178004.46 epoch sec; seqNo:17504 2021-02-13T01:00:04Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.16 psi 2021-02-13T01:00:04Z- handleDepth()-Depth_Keller msg: 1613178004473 epoch sec; seqNo:17642; depth 20.75 2021-02-13T01:00:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178004.48 epoch sec; seqNo:17642; 4060582.12 north; 593507.79 east 2021-02-13T01:00:04Z- LcmTrn::handleMessages() - handling 41 messages took 0.70 ms and 127 calls... 2021-02-13T01:00:04Z- waiting for 1613178002.69, time is 1613178004.48 2021-02-13T01:00:04Z- LcmTrn::updateTrn() >>>> pose time 1613178004.48, meas time 1613178004.08 <<<< 2021-02-13T01:00:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178004.08 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:00:04Z- TerrainNav::motionUpdate() - currEstimate = (4060582, 593507.79, 20.75) 2021-02-13T01:00:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:00:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:00:04Z- TNavBF::Projecting Measurements 2021-02-13T01:00:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:00:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:00:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:00:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:00:04Z- TNavFilter::4 good beams. 2021-02-13T01:00:04Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:00:04Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:00:04Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:00:04Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:00:04Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:00:04Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:00:04Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:00:04Z- TNavBF::mmse selected from filter -1 2021-02-13T01:00:04Z- TerrainMapOctree: (vehN, vehE ) = (4060581.83, 593507.48). 2021-02-13T01:00:04Z- TerrainMapOctree: Min Distance = 998.89. 2021-02-13T01:00:04Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:00:04Z- There is no specific good beam that all particles have in common. 2021-02-13T01:00:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:00:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.32 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 9.96 2021-02-13T01:00:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:00:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.075 ExpectedMeasDiff: nan currMeas.covariance: 12.92 2021-02-13T01:00:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:00:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.49 2021-02-13T01:00:04Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:00:04Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 9.96 2021-02-13T01:00:04Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:00:04Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:00:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:00:04Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:00:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:00:04Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:00:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:00:04Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:00:04Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:00:04Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:00:04Z- Filter 0 No beams used in subcloud update 2021-02-13T01:00:04Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:00:04Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:00:04Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 679.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:00:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178004.08 sec. 2021-02-13T01:00:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:00:04Z- TNavBF::mmse selected from filter -1 2021-02-13T01:00:04Z- variances([0], [2], and psi_berg) = 679.567290 832.988295 0.000000 2021-02-13T01:00:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:00:04Z- LcmTrn::publishEstimates() - MLE : 4060581.62 593527.29 20.75 2021-02-13T01:00:04Z- LcmTrn::publishEstimates() - MMSE : 4060582.12 593507.79 20.75 2021-02-13T01:00:04Z- LcmTrn::publishEstimates() - COVAR: 679.57 832.99 0.97 0.00 2021-02-13T01:00:04Z- AHRS_M2 msg: 1613178004.86 epoch sec; seqNo:17505 2021-02-13T01:00:04Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.17 psi 2021-02-13T01:00:04Z- handleDepth()-Depth_Keller msg: 1613178004873 epoch sec; seqNo:17643; depth 20.64 2021-02-13T01:00:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178004.89 epoch sec; seqNo:17643; 4060582.49 north; 593507.78 east 2021-02-13T01:00:05Z- AHRS_M2 msg: 1613178005.27 epoch sec; seqNo:17506 2021-02-13T01:00:05Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-13T01:00:05Z- handleDepth()-Depth_Keller msg: 1613178005272 epoch sec; seqNo:17644; depth 20.53 2021-02-13T01:00:05Z- handleDvl() - RDI_Pathfinder msg: 1613178005.28 epoch sec; seqNo:8509 2021-02-13T01:00:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 99.27 , 67.03 2021-02-13T01:00:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.12 , -0.02 2021-02-13T01:00:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178005.31 epoch sec; seqNo:17644; 4060582.49 north; 593507.78 east 2021-02-13T01:00:05Z- AHRS_M2 msg: 1613178005.67 epoch sec; seqNo:17507 2021-02-13T01:00:05Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-13T01:00:05Z- handleDepth()-Depth_Keller msg: 1613178005676 epoch sec; seqNo:17645; depth 20.42 2021-02-13T01:00:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178005.69 epoch sec; seqNo:17645; 4060582.87 north; 593507.78 east 2021-02-13T01:00:06Z- AHRS_M2 msg: 1613178006.08 epoch sec; seqNo:17508 2021-02-13T01:00:06Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.18 psi 2021-02-13T01:00:06Z- handleDepth()-Depth_Keller msg: 1613178006090 epoch sec; seqNo:17646; depth 20.32 2021-02-13T01:00:06Z- handleDvl() - RDI_Pathfinder msg: 1613178006.10 epoch sec; seqNo:8510 2021-02-13T01:00:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.39 , 89.60 , 100.56 , 69.07 2021-02-13T01:00:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.11 , 0.06 2021-02-13T01:00:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178006.11 epoch sec; seqNo:17646; 4060583.25 north; 593507.77 east 2021-02-13T01:00:06Z- AHRS_M2 msg: 1613178006.48 epoch sec; seqNo:17509 2021-02-13T01:00:06Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.18 psi 2021-02-13T01:00:06Z- handleDepth()-Depth_Keller msg: 1613178006490 epoch sec; seqNo:17647; depth 20.19 2021-02-13T01:00:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178006.51 epoch sec; seqNo:17647; 4060583.63 north; 593507.77 east 2021-02-13T01:00:06Z- AHRS_M2 msg: 1613178006.89 epoch sec; seqNo:17510 2021-02-13T01:00:06Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.18 psi 2021-02-13T01:00:06Z- handleDepth()-Depth_Keller msg: 1613178006893 epoch sec; seqNo:17648; depth 20.10 2021-02-13T01:00:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178006.90 epoch sec; seqNo:17648; 4060584.01 north; 593507.77 east 2021-02-13T01:00:07Z- AHRS_M2 msg: 1613178007.29 epoch sec; seqNo:17511 2021-02-13T01:00:07Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.18 psi 2021-02-13T01:00:07Z- handleDepth()-Depth_Keller msg: 1613178007292 epoch sec; seqNo:17649; depth 20.00 2021-02-13T01:00:07Z- handleDvl() - RDI_Pathfinder msg: 1613178007.30 epoch sec; seqNo:8511 2021-02-13T01:00:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 98.32 , 100.76 , 68.26 2021-02-13T01:00:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.04 , 0.11 2021-02-13T01:00:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178007.31 epoch sec; seqNo:17649; 4060584.40 north; 593508.98 east 2021-02-13T01:00:07Z- AHRS_M2 msg: 1613178007.69 epoch sec; seqNo:17512 2021-02-13T01:00:07Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.18 psi 2021-02-13T01:00:07Z- handleDepth()-Depth_Keller msg: 1613178007698 epoch sec; seqNo:17650; depth 19.88 2021-02-13T01:00:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178007.71 epoch sec; seqNo:17650; 4060584.40 north; 593508.98 east 2021-02-13T01:00:08Z- AHRS_M2 msg: 1613178008.10 epoch sec; seqNo:17513 2021-02-13T01:00:08Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.17 psi 2021-02-13T01:00:08Z- handleDepth()-Depth_Keller msg: 1613178008101 epoch sec; seqNo:17651; depth 19.79 2021-02-13T01:00:08Z- handleDvl() - RDI_Pathfinder msg: 1613178008.11 epoch sec; seqNo:8512 2021-02-13T01:00:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.39 , 98.05 , 101.71 , 0.00 2021-02-13T01:00:08Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.03 , 0.11 2021-02-13T01:00:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178008.14 epoch sec; seqNo:17651; 4060584.78 north; 593508.98 east 2021-02-13T01:00:08Z- AHRS_M2 msg: 1613178008.50 epoch sec; seqNo:17514 2021-02-13T01:00:08Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.17 psi 2021-02-13T01:00:08Z- handleDepth()-Depth_Keller msg: 1613178008517 epoch sec; seqNo:17652; depth 19.66 2021-02-13T01:00:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178008.53 epoch sec; seqNo:17652; 4060585.16 north; 593508.97 east 2021-02-13T01:00:08Z- AHRS_M2 msg: 1613178008.90 epoch sec; seqNo:17515 2021-02-13T01:00:08Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.17 psi 2021-02-13T01:00:08Z- handleDepth()-Depth_Keller msg: 1613178008910 epoch sec; seqNo:17653; depth 19.54 2021-02-13T01:00:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178008.92 epoch sec; seqNo:17653; 4060585.54 north; 593508.97 east 2021-02-13T01:00:09Z- AHRS_M2 msg: 1613178009.31 epoch sec; seqNo:17516 2021-02-13T01:00:09Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.16 psi 2021-02-13T01:00:09Z- handleDepth()-Depth_Keller msg: 1613178009317 epoch sec; seqNo:17654; depth 19.44 2021-02-13T01:00:09Z- handleDvl() - RDI_Pathfinder msg: 1613178009.34 epoch sec; seqNo:8513 2021-02-13T01:00:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.17 , 97.65 , 102.52 , 68.26 2021-02-13T01:00:09Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.06 , 0.04 2021-02-13T01:00:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178009.35 epoch sec; seqNo:17654; 4060585.92 north; 593508.97 east 2021-02-13T01:00:09Z- AHRS_M2 msg: 1613178009.71 epoch sec; seqNo:17517 2021-02-13T01:00:09Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.16 psi 2021-02-13T01:00:09Z- handleDepth()-Depth_Keller msg: 1613178009721 epoch sec; seqNo:17655; depth 19.34 2021-02-13T01:00:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178009.73 epoch sec; seqNo:17655; 4060586.31 north; 593510.18 east 2021-02-13T01:00:09Z- LcmTrn::handleMessages() - handling 44 messages took 0.61 ms and 132 calls... 2021-02-13T01:00:09Z- waiting for 1613178007.48, time is 1613178009.73 2021-02-13T01:00:09Z- LcmTrn::updateTrn() >>>> pose time 1613178009.73, meas time 1613178009.34 <<<< 2021-02-13T01:00:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178009.34 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:00:09Z- TerrainNav::motionUpdate() - currEstimate = (4060586, 593510.18, 19.34) 2021-02-13T01:00:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:00:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:00:09Z- TNavBF::Projecting Measurements 2021-02-13T01:00:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:00:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:00:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:00:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:00:09Z- TNavFilter::4 good beams. 2021-02-13T01:00:09Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:00:09Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:00:09Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:00:09Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:00:09Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:00:09Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:00:09Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:00:09Z- TNavBF::mmse selected from filter -1 2021-02-13T01:00:09Z- TerrainMapOctree: (vehN, vehE ) = (4060585.99, 593510.00). 2021-02-13T01:00:09Z- TerrainMapOctree: Min Distance = 994.24. 2021-02-13T01:00:09Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:00:09Z- There is no specific good beam that all particles have in common. 2021-02-13T01:00:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:00:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 9.78 2021-02-13T01:00:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:00:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.26 2021-02-13T01:00:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:00:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 16.82 2021-02-13T01:00:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:00:09Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 7.45 2021-02-13T01:00:09Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:00:09Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:00:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:00:09Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:00:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:00:10Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:00:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:00:10Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:00:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:00:10Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:00:10Z- Filter 0 No beams used in subcloud update 2021-02-13T01:00:10Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:00:10Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:00:10Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 679.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:00:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178009.34 sec. 2021-02-13T01:00:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:00:10Z- TNavBF::mmse selected from filter -1 2021-02-13T01:00:10Z- variances([0], [2], and psi_berg) = 679.507939 832.988270 0.000000 2021-02-13T01:00:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:00:10Z- LcmTrn::publishEstimates() - MLE : 4060585.81 593526.68 19.34 2021-02-13T01:00:10Z- LcmTrn::publishEstimates() - MMSE : 4060586.31 593510.18 19.34 2021-02-13T01:00:10Z- LcmTrn::publishEstimates() - COVAR: 679.51 832.99 0.89 0.00 2021-02-13T01:00:10Z- AHRS_M2 msg: 1613178010.12 epoch sec; seqNo:17518 2021-02-13T01:00:10Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.16 psi 2021-02-13T01:00:10Z- handleDepth()-Depth_Keller msg: 1613178010190 epoch sec; seqNo:17656; depth 19.21 2021-02-13T01:00:10Z- handleDvl() - RDI_Pathfinder msg: 1613178010.20 epoch sec; seqNo:8514 2021-02-13T01:00:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 104.14 , 69.07 2021-02-13T01:00:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.10 , -0.04 2021-02-13T01:00:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178010.22 epoch sec; seqNo:17656; 4060586.69 north; 593510.18 east 2021-02-13T01:00:10Z- AHRS_M2 msg: 1613178010.52 epoch sec; seqNo:17519 2021-02-13T01:00:10Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-13T01:00:10Z- handleDepth()-Depth_Keller msg: 1613178010534 epoch sec; seqNo:17657; depth 19.09 2021-02-13T01:00:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178010.55 epoch sec; seqNo:17657; 4060586.69 north; 593510.18 east 2021-02-13T01:00:10Z- AHRS_M2 msg: 1613178010.92 epoch sec; seqNo:17520 2021-02-13T01:00:10Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T01:00:10Z- handleDepth()-Depth_Keller msg: 1613178010946 epoch sec; seqNo:17658; depth 19.02 2021-02-13T01:00:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178010.96 epoch sec; seqNo:17658; 4060587.07 north; 593510.17 east 2021-02-13T01:00:11Z- AHRS_M2 msg: 1613178011.33 epoch sec; seqNo:17521 2021-02-13T01:00:11Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-13T01:00:11Z- handleDepth()-Depth_Keller msg: 1613178011358 epoch sec; seqNo:17659; depth 18.89 2021-02-13T01:00:11Z- handleDvl() - RDI_Pathfinder msg: 1613178011.37 epoch sec; seqNo:8515 2021-02-13T01:00:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.99 , 104.07 , 68.67 2021-02-13T01:00:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.06 , -0.11 2021-02-13T01:00:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178011.38 epoch sec; seqNo:17659; 4060587.45 north; 593510.17 east 2021-02-13T01:00:11Z- AHRS_M2 msg: 1613178011.73 epoch sec; seqNo:17522 2021-02-13T01:00:11Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-13T01:00:11Z- handleDepth()-Depth_Keller msg: 1613178011738 epoch sec; seqNo:17660; depth 18.78 2021-02-13T01:00:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178011.75 epoch sec; seqNo:17660; 4060587.46 north; 593511.39 east 2021-02-13T01:00:12Z- AHRS_M2 msg: 1613178012.14 epoch sec; seqNo:17523 2021-02-13T01:00:12Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-13T01:00:12Z- handleDepth()-Depth_Keller msg: 1613178012152 epoch sec; seqNo:17661; depth 18.68 2021-02-13T01:00:12Z- handleDvl() - RDI_Pathfinder msg: 1613178012.16 epoch sec; seqNo:8516 2021-02-13T01:00:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 97.99 , 104.07 , 78.51 2021-02-13T01:00:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , -0.09 2021-02-13T01:00:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178012.17 epoch sec; seqNo:17661; 4060587.84 north; 593511.39 east 2021-02-13T01:00:12Z- AHRS_M2 msg: 1613178012.54 epoch sec; seqNo:17524 2021-02-13T01:00:12Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T01:00:12Z- handleDepth()-Depth_Keller msg: 1613178012545 epoch sec; seqNo:17662; depth 18.55 2021-02-13T01:00:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178012.56 epoch sec; seqNo:17662; 4060588.22 north; 593511.38 east 2021-02-13T01:00:12Z- AHRS_M2 msg: 1613178012.94 epoch sec; seqNo:17525 2021-02-13T01:00:12Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T01:00:13Z- handleDepth()-Depth_Keller msg: 1613178013032 epoch sec; seqNo:17663; depth 18.45 2021-02-13T01:00:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178013.06 epoch sec; seqNo:17663; 4060588.60 north; 593511.38 east 2021-02-13T01:00:13Z- AHRS_M2 msg: 1613178013.35 epoch sec; seqNo:17526 2021-02-13T01:00:13Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-13T01:00:13Z- handleDepth()-Depth_Keller msg: 1613178013360 epoch sec; seqNo:17664; depth 18.32 2021-02-13T01:00:13Z- handleDvl() - RDI_Pathfinder msg: 1613178013.41 epoch sec; seqNo:8517 2021-02-13T01:00:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 97.85 , 103.94 , 69.07 2021-02-13T01:00:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.05 , -0.06 2021-02-13T01:00:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178013.43 epoch sec; seqNo:17664; 4060588.60 north; 593511.38 east 2021-02-13T01:00:13Z- AHRS_M2 msg: 1613178013.75 epoch sec; seqNo:17527 2021-02-13T01:00:13Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-13T01:00:13Z- handleDepth()-Depth_Keller msg: 1613178013778 epoch sec; seqNo:17665; depth 18.22 2021-02-13T01:00:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178013.79 epoch sec; seqNo:17665; 4060588.97 north; 593511.37 east 2021-02-13T01:00:14Z- AHRS_M2 msg: 1613178014.16 epoch sec; seqNo:17528 2021-02-13T01:00:14Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-13T01:00:14Z- handleDepth()-Depth_Keller msg: 1613178014185 epoch sec; seqNo:17666; depth 18.12 2021-02-13T01:00:14Z- handleDvl() - RDI_Pathfinder msg: 1613178014.20 epoch sec; seqNo:8518 2021-02-13T01:00:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 91.90 , 105.29 , 71.12 2021-02-13T01:00:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.08 , -0.04 2021-02-13T01:00:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178014.21 epoch sec; seqNo:17666; 4060589.37 north; 593512.59 east 2021-02-13T01:00:14Z- AHRS_M2 msg: 1613178014.56 epoch sec; seqNo:17529 2021-02-13T01:00:14Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-13T01:00:14Z- handleDepth()-Depth_Keller msg: 1613178014565 epoch sec; seqNo:17667; depth 17.99 2021-02-13T01:00:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178014.58 epoch sec; seqNo:17667; 4060589.37 north; 593512.59 east 2021-02-13T01:00:14Z- AHRS_M2 msg: 1613178014.96 epoch sec; seqNo:17530 2021-02-13T01:00:14Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.14 psi 2021-02-13T01:00:14Z- handleDepth()-Depth_Keller msg: 1613178014978 epoch sec; seqNo:17668; depth 17.90 2021-02-13T01:00:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178014.99 epoch sec; seqNo:17668; 4060589.75 north; 593512.59 east 2021-02-13T01:00:15Z- LcmTrn::handleMessages() - handling 44 messages took 0.86 ms and 127 calls... 2021-02-13T01:00:15Z- waiting for 1613178012.73, time is 1613178014.99 2021-02-13T01:00:15Z- LcmTrn::updateTrn() >>>> pose time 1613178014.99, meas time 1613178014.20 <<<< 2021-02-13T01:00:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178014.20 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:00:15Z- TerrainNav::motionUpdate() - currEstimate = (4060590, 593512.59, 17.90) 2021-02-13T01:00:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:00:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:00:15Z- TNavBF::Projecting Measurements 2021-02-13T01:00:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:00:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:00:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:00:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:00:15Z- TNavFilter::4 good beams. 2021-02-13T01:00:15Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:00:15Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:00:15Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:00:15Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:00:15Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:00:15Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:00:15Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:00:15Z- TNavBF::mmse selected from filter -1 2021-02-13T01:00:15Z- TerrainMapOctree: (vehN, vehE ) = (4060589.23, 593512.23). 2021-02-13T01:00:15Z- TerrainMapOctree: Min Distance = 990.42. 2021-02-13T01:00:15Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:00:15Z- There is no specific good beam that all particles have in common. 2021-02-13T01:00:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:00:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T01:00:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:00:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 13.51 2021-02-13T01:00:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:00:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 17.74 2021-02-13T01:00:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:00:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 8.09 2021-02-13T01:00:15Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:00:15Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:00:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:00:15Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:00:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:00:15Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:00:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:00:15Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:00:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:00:15Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:00:15Z- Filter 0 No beams used in subcloud update 2021-02-13T01:00:15Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:00:15Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:00:15Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 679.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T01:00:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178014.20 sec. 2021-02-13T01:00:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:00:15Z- TNavBF::mmse selected from filter -1 2021-02-13T01:00:15Z- variances([0], [2], and psi_berg) = 679.449435 832.988245 0.000000 2021-02-13T01:00:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:00:15Z- LcmTrn::publishEstimates() - MLE : 4060589.25 593532.09 17.90 2021-02-13T01:00:15Z- LcmTrn::publishEstimates() - MMSE : 4060589.75 593512.59 17.90 2021-02-13T01:00:15Z- LcmTrn::publishEstimates() - COVAR: 679.45 832.99 0.84 0.00 2021-02-13T01:00:15Z- AHRS_M2 msg: 1613178015.37 epoch sec; seqNo:17531 2021-02-13T01:00:15Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.13 psi 2021-02-13T01:00:15Z- handleDepth()-Depth_Keller msg: 1613178015372 epoch sec; seqNo:17669; depth 17.80 2021-02-13T01:00:15Z- handleDvl() - RDI_Pathfinder msg: 1613178015.38 epoch sec; seqNo:8519 2021-02-13T01:00:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:15Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:00:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178015.41 epoch sec; seqNo:17669; 4060589.75 north; 593512.59 east 2021-02-13T01:00:15Z- AHRS_M2 msg: 1613178015.77 epoch sec; seqNo:17532 2021-02-13T01:00:15Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.12 psi 2021-02-13T01:00:15Z- handleDepth()-Depth_Keller msg: 1613178015781 epoch sec; seqNo:17670; depth 17.67 2021-02-13T01:00:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178015.79 epoch sec; seqNo:17670; 4060590.12 north; 593512.58 east 2021-02-13T01:00:16Z- AHRS_M2 msg: 1613178016.18 epoch sec; seqNo:17533 2021-02-13T01:00:16Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.14 psi 2021-02-13T01:00:16Z- handleDepth()-Depth_Keller msg: 1613178016182 epoch sec; seqNo:17671; depth 17.54 2021-02-13T01:00:16Z- handleDvl() - RDI_Pathfinder msg: 1613178016.19 epoch sec; seqNo:8520 2021-02-13T01:00:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.35 , 97.71 , 105.15 , 69.48 2021-02-13T01:00:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , -0.07 2021-02-13T01:00:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178016.20 epoch sec; seqNo:17671; 4060590.50 north; 593512.58 east 2021-02-13T01:00:16Z- AHRS_M2 msg: 1613178016.58 epoch sec; seqNo:17534 2021-02-13T01:00:16Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.15 psi 2021-02-13T01:00:16Z- handleDepth()-Depth_Keller msg: 1613178016584 epoch sec; seqNo:17672; depth 17.42 2021-02-13T01:00:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178016.60 epoch sec; seqNo:17672; 4060590.52 north; 593513.80 east 2021-02-13T01:00:16Z- AHRS_M2 msg: 1613178016.98 epoch sec; seqNo:17535 2021-02-13T01:00:16Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.16 psi 2021-02-13T01:00:16Z- handleDepth()-Depth_Keller msg: 1613178016997 epoch sec; seqNo:17673; depth 17.32 2021-02-13T01:00:17Z- handleDvl() - RDI_Pathfinder msg: 1613178017.02 epoch sec; seqNo:8521 2021-02-13T01:00:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.92 , 106.44 , 70.71 2021-02-13T01:00:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , -0.05 2021-02-13T01:00:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178017.03 epoch sec; seqNo:17673; 4060590.90 north; 593513.79 east 2021-02-13T01:00:17Z- AHRS_M2 msg: 1613178017.39 epoch sec; seqNo:17536 2021-02-13T01:00:17Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-13T01:00:17Z- handleDepth()-Depth_Keller msg: 1613178017393 epoch sec; seqNo:17674; depth 17.19 2021-02-13T01:00:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178017.40 epoch sec; seqNo:17674; 4060591.27 north; 593513.79 east 2021-02-13T01:00:17Z- AHRS_M2 msg: 1613178017.79 epoch sec; seqNo:17537 2021-02-13T01:00:17Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.18 psi 2021-02-13T01:00:17Z- handleDepth()-Depth_Keller msg: 1613178017799 epoch sec; seqNo:17675; depth 17.08 2021-02-13T01:00:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178017.81 epoch sec; seqNo:17675; 4060591.65 north; 593513.79 east 2021-02-13T01:00:18Z- AHRS_M2 msg: 1613178018.19 epoch sec; seqNo:17538 2021-02-13T01:00:18Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.18 psi 2021-02-13T01:00:18Z- handleDepth()-Depth_Keller msg: 1613178018205 epoch sec; seqNo:17676; depth 16.96 2021-02-13T01:00:18Z- handleDvl() - RDI_Pathfinder msg: 1613178018.22 epoch sec; seqNo:8522 2021-02-13T01:00:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.07 , 97.65 , 109.82 , 78.17 2021-02-13T01:00:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.06 , -0.01 2021-02-13T01:00:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178018.23 epoch sec; seqNo:17676; 4060592.03 north; 593513.78 east 2021-02-13T01:00:18Z- AHRS_M2 msg: 1613178018.60 epoch sec; seqNo:17539 2021-02-13T01:00:18Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.18 psi 2021-02-13T01:00:18Z- handleDepth()-Depth_Keller msg: 1613178018604 epoch sec; seqNo:17677; depth 16.83 2021-02-13T01:00:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178018.62 epoch sec; seqNo:17677; 4060592.03 north; 593513.78 east 2021-02-13T01:00:19Z- AHRS_M2 msg: 1613178019.00 epoch sec; seqNo:17540 2021-02-13T01:00:19Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-13T01:00:19Z- handleDepth()-Depth_Keller msg: 1613178019009 epoch sec; seqNo:17678; depth 16.73 2021-02-13T01:00:19Z- handleDvl() - RDI_Pathfinder msg: 1613178019.02 epoch sec; seqNo:8523 2021-02-13T01:00:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.48 , 96.50 , 107.45 , 69.89 2021-02-13T01:00:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.07 , 0.03 2021-02-13T01:00:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178019.03 epoch sec; seqNo:17678; 4060592.41 north; 593513.78 east 2021-02-13T01:00:19Z- AHRS_M2 msg: 1613178019.41 epoch sec; seqNo:17541 2021-02-13T01:00:19Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T01:00:19Z- handleDepth()-Depth_Keller msg: 1613178019417 epoch sec; seqNo:17679; depth 16.60 2021-02-13T01:00:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178019.43 epoch sec; seqNo:17679; 4060592.80 north; 593514.99 east 2021-02-13T01:00:19Z- AHRS_M2 msg: 1613178019.81 epoch sec; seqNo:17542 2021-02-13T01:00:19Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T01:00:19Z- handleDepth()-Depth_Keller msg: 1613178019818 epoch sec; seqNo:17680; depth 16.49 2021-02-13T01:00:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:00:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:00:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:00:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:00:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:00:19Z- handleDvl() - RDI_Pathfinder msg: 1613178019.83 epoch sec; seqNo:8524 2021-02-13T01:00:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.07 , 0.03 2021-02-13T01:00:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178019.85 epoch sec; seqNo:17680; 4060593.18 north; 593514.99 east 2021-02-13T01:00:20Z- LcmTrn::handleMessages() - handling 42 messages took 0.63 ms and 130 calls... 2021-02-13T01:00:20Z- waiting for 1613178017.99, time is 1613178019.85 2021-02-13T01:00:20Z- LcmTrn::updateTrn() equating pose 1613178019.85 and meas times 1613178019.85 2021-02-13T01:00:20Z- LcmTrn::updateTrn() >>>> pose time 1613178019.85, meas time 1613178019.85 <<<< 2021-02-13T01:00:20Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613178019.85, ping # 0. 2021-02-13T01:00:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178019.85 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:00:20Z- TerrainNav::motionUpdate() - currEstimate = (4060593, 593514.99, 16.49) 2021-02-13T01:00:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:00:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:00:20Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:00:20Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:00:20Z- TNavBF::Projecting Measurements 2021-02-13T01:00:20Z- TNavFilter::Measurement from time = 1613178019.85 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T01:00:20Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:00:20Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:00:20Z- TNavBF:: beamIndex[1] = 0 2021-02-13T01:00:20Z- TNavBF:: beamIndex[2] = 0 2021-02-13T01:00:20Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:00:20Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:00:20Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:00:20Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 679.4 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T01:00:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:00:20Z- TNavBF::mmse selected from filter -1 2021-02-13T01:00:20Z- variances([0], [2], and psi_berg) = 679.404750 832.988226 0.000000 2021-02-13T01:00:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:00:20Z- LcmTrn::publishEstimates() - MLE : 4060592.68 593531.49 16.49 2021-02-13T01:00:20Z- LcmTrn::publishEstimates() - MMSE : 4060593.18 593514.99 16.49 2021-02-13T01:00:20Z- LcmTrn::publishEstimates() - COVAR: 679.40 832.99 1.06 0.00 2021-02-13T01:00:20Z- AHRS_M2 msg: 1613178020.22 epoch sec; seqNo:17543 2021-02-13T01:00:20Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.19 psi 2021-02-13T01:00:20Z- handleDepth()-Depth_Keller msg: 1613178020221 epoch sec; seqNo:17681; depth 16.36 2021-02-13T01:00:20Z- handleDvl() - RDI_Pathfinder msg: 1613178020.23 epoch sec; seqNo:8525 2021-02-13T01:00:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.66 , 97.92 , 111.31 , 78.44 2021-02-13T01:00:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.03 , 0.09 2021-02-13T01:00:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178020.26 epoch sec; seqNo:17681; 4060593.56 north; 593514.98 east 2021-02-13T01:00:20Z- AHRS_M2 msg: 1613178020.62 epoch sec; seqNo:17544 2021-02-13T01:00:20Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-13T01:00:20Z- handleDepth()-Depth_Keller msg: 1613178020625 epoch sec; seqNo:17682; depth 16.24 2021-02-13T01:00:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178020.63 epoch sec; seqNo:17682; 4060593.94 north; 593514.98 east 2021-02-13T01:00:21Z- AHRS_M2 msg: 1613178021.02 epoch sec; seqNo:17545 2021-02-13T01:00:21Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-13T01:00:21Z- handleDepth()-Depth_Keller msg: 1613178021037 epoch sec; seqNo:17683; depth 16.13 2021-02-13T01:00:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:21Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:00:21Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:00:21Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:00:21Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:00:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:00:21Z- handleDvl() - RDI_Pathfinder msg: 1613178021.04 epoch sec; seqNo:8526 2021-02-13T01:00:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.03 , 0.09 2021-02-13T01:00:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178021.05 epoch sec; seqNo:17683; 4060593.94 north; 593514.98 east 2021-02-13T01:00:21Z- AHRS_M2 msg: 1613178021.43 epoch sec; seqNo:17546 2021-02-13T01:00:21Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.18 psi 2021-02-13T01:00:21Z- handleDepth()-Depth_Keller msg: 1613178021433 epoch sec; seqNo:17684; depth 16.02 2021-02-13T01:00:21Z- handleDvl() - RDI_Pathfinder msg: 1613178021.46 epoch sec; seqNo:8527 2021-02-13T01:00:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.14 , 96.36 , 108.53 , 78.78 2021-02-13T01:00:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.05 , 0.09 2021-02-13T01:00:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178021.47 epoch sec; seqNo:17684; 4060594.32 north; 593514.98 east 2021-02-13T01:00:21Z- AHRS_M2 msg: 1613178021.83 epoch sec; seqNo:17547 2021-02-13T01:00:21Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.18 psi 2021-02-13T01:00:21Z- handleDepth()-Depth_Keller msg: 1613178021841 epoch sec; seqNo:17685; depth 15.92 2021-02-13T01:00:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178021.85 epoch sec; seqNo:17685; 4060594.71 north; 593516.19 east 2021-02-13T01:00:22Z- AHRS_M2 msg: 1613178022.24 epoch sec; seqNo:17548 2021-02-13T01:00:22Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.18 psi 2021-02-13T01:00:22Z- handleDepth()-Depth_Keller msg: 1613178022245 epoch sec; seqNo:17686; depth 15.79 2021-02-13T01:00:22Z- handleDvl() - RDI_Pathfinder msg: 1613178022.26 epoch sec; seqNo:8528 2021-02-13T01:00:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.71 , 109.89 , 78.24 2021-02-13T01:00:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.13 , 0.04 2021-02-13T01:00:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178022.27 epoch sec; seqNo:17686; 4060595.09 north; 593516.19 east 2021-02-13T01:00:22Z- AHRS_M2 msg: 1613178022.64 epoch sec; seqNo:17549 2021-02-13T01:00:22Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-13T01:00:22Z- handleDepth()-Depth_Keller msg: 1613178022653 epoch sec; seqNo:17687; depth 15.68 2021-02-13T01:00:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178022.67 epoch sec; seqNo:17687; 4060595.47 north; 593516.18 east 2021-02-13T01:00:23Z- AHRS_M2 msg: 1613178023.04 epoch sec; seqNo:17550 2021-02-13T01:00:23Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-13T01:00:23Z- handleDepth()-Depth_Keller msg: 1613178023049 epoch sec; seqNo:17688; depth 15.57 2021-02-13T01:00:23Z- handleDvl() - RDI_Pathfinder msg: 1613178023.07 epoch sec; seqNo:8529 2021-02-13T01:00:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.29 , 97.44 , 109.62 , 78.51 2021-02-13T01:00:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.15 , -0.03 2021-02-13T01:00:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178023.08 epoch sec; seqNo:17688; 4060595.47 north; 593516.18 east 2021-02-13T01:00:23Z- AHRS_M2 msg: 1613178023.45 epoch sec; seqNo:17551 2021-02-13T01:00:23Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-13T01:00:23Z- handleDepth()-Depth_Keller msg: 1613178023520 epoch sec; seqNo:17689; depth 15.47 2021-02-13T01:00:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:23Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:00:23Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:00:23Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:00:23Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:00:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:00:23Z- handleDvl() - RDI_Pathfinder msg: 1613178023.52 epoch sec; seqNo:8530 2021-02-13T01:00:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.15 , -0.03 2021-02-13T01:00:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178023.54 epoch sec; seqNo:17689; 4060595.85 north; 593516.18 east 2021-02-13T01:00:23Z- AHRS_M2 msg: 1613178023.87 epoch sec; seqNo:17552 2021-02-13T01:00:23Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-13T01:00:23Z- handleDepth()-Depth_Keller msg: 1613178023876 epoch sec; seqNo:17690; depth 15.36 2021-02-13T01:00:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178023.88 epoch sec; seqNo:17690; 4060596.24 north; 593517.40 east 2021-02-13T01:00:24Z- AHRS_M2 msg: 1613178024.26 epoch sec; seqNo:17553 2021-02-13T01:00:24Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-13T01:00:24Z- handleDepth()-Depth_Keller msg: 1613178024285 epoch sec; seqNo:17691; depth 15.26 2021-02-13T01:00:24Z- handleDvl() - RDI_Pathfinder msg: 1613178024.30 epoch sec; seqNo:8531 2021-02-13T01:00:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 97.65 , 109.82 , 72.35 2021-02-13T01:00:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.16 , -0.09 2021-02-13T01:00:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178024.33 epoch sec; seqNo:17691; 4060596.24 north; 593517.40 east 2021-02-13T01:00:24Z- AHRS_M2 msg: 1613178024.66 epoch sec; seqNo:17554 2021-02-13T01:00:24Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-13T01:00:24Z- handleDepth()-Depth_Keller msg: 1613178024685 epoch sec; seqNo:17692; depth 15.14 2021-02-13T01:00:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:24Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:00:24Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:00:24Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:00:24Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:00:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:00:24Z- handleDvl() - RDI_Pathfinder msg: 1613178024.69 epoch sec; seqNo:8532 2021-02-13T01:00:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.16 , -0.09 2021-02-13T01:00:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178024.70 epoch sec; seqNo:17692; 4060596.62 north; 593517.39 east 2021-02-13T01:00:25Z- AHRS_M2 msg: 1613178025.06 epoch sec; seqNo:17555 2021-02-13T01:00:25Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-13T01:00:25Z- handleDepth()-Depth_Keller msg: 1613178025069 epoch sec; seqNo:17693; depth 15.05 2021-02-13T01:00:25Z- handleDvl() - RDI_Pathfinder msg: 1613178025.08 epoch sec; seqNo:8533 2021-02-13T01:00:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.78 , 107.52 , 78.31 2021-02-13T01:00:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.12 , -0.13 2021-02-13T01:00:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178025.10 epoch sec; seqNo:17693; 4060597.00 north; 593517.39 east 2021-02-13T01:00:25Z- LcmTrn::handleMessages() - handling 48 messages took 0.73 ms and 134 calls... 2021-02-13T01:00:25Z- waiting for 1613178022.85, time is 1613178025.10 2021-02-13T01:00:25Z- LcmTrn::updateTrn() equating pose 1613178025.10 and meas times 1613178025.10 2021-02-13T01:00:25Z- LcmTrn::updateTrn() >>>> pose time 1613178025.10, meas time 1613178025.10 <<<< 2021-02-13T01:00:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178025.10 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:00:25Z- TerrainNav::motionUpdate() - currEstimate = (4060597, 593517.39, 15.05) 2021-02-13T01:00:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:00:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:00:25Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178025.10 sec.; No relevant navigation data available 2021-02-13T01:00:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:00:25Z- TNavBF::mmse selected from filter -1 2021-02-13T01:00:25Z- variances([0], [2], and psi_berg) = 679.358445 832.988207 0.000000 2021-02-13T01:00:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:00:25Z- LcmTrn::publishEstimates() - MLE : 4060596.50 593533.89 15.05 2021-02-13T01:00:25Z- LcmTrn::publishEstimates() - MMSE : 4060597.00 593517.39 15.05 2021-02-13T01:00:25Z- LcmTrn::publishEstimates() - COVAR: 679.36 832.99 1.28 0.00 2021-02-13T01:00:25Z- AHRS_M2 msg: 1613178025.47 epoch sec; seqNo:17556 2021-02-13T01:00:25Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-13T01:00:25Z- handleDepth()-Depth_Keller msg: 1613178025481 epoch sec; seqNo:17694; depth 14.95 2021-02-13T01:00:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178025.49 epoch sec; seqNo:17694; 4060597.00 north; 593517.39 east 2021-02-13T01:00:25Z- AHRS_M2 msg: 1613178025.87 epoch sec; seqNo:17557 2021-02-13T01:00:25Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-13T01:00:25Z- handleDepth()-Depth_Keller msg: 1613178025946 epoch sec; seqNo:17695; depth 14.83 2021-02-13T01:00:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:00:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:00:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:00:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:00:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:00:25Z- handleDvl() - RDI_Pathfinder msg: 1613178025.95 epoch sec; seqNo:8534 2021-02-13T01:00:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.12 , -0.13 2021-02-13T01:00:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178025.96 epoch sec; seqNo:17695; 4060597.39 north; 593518.60 east 2021-02-13T01:00:26Z- AHRS_M2 msg: 1613178026.28 epoch sec; seqNo:17558 2021-02-13T01:00:26Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-13T01:00:26Z- handleDepth()-Depth_Keller msg: 1613178026314 epoch sec; seqNo:17696; depth 14.71 2021-02-13T01:00:26Z- handleDvl() - RDI_Pathfinder msg: 1613178026.33 epoch sec; seqNo:8535 2021-02-13T01:00:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.51 , 111.04 , 78.58 2021-02-13T01:00:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.10 , -0.16 2021-02-13T01:00:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178026.34 epoch sec; seqNo:17696; 4060597.77 north; 593518.60 east 2021-02-13T01:00:26Z- AHRS_M2 msg: 1613178026.68 epoch sec; seqNo:17559 2021-02-13T01:00:26Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-13T01:00:26Z- handleDepth()-Depth_Keller msg: 1613178026705 epoch sec; seqNo:17697; depth 14.60 2021-02-13T01:00:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178026.72 epoch sec; seqNo:17697; 4060597.77 north; 593518.60 east 2021-02-13T01:00:27Z- AHRS_M2 msg: 1613178027.09 epoch sec; seqNo:17560 2021-02-13T01:00:27Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-13T01:00:27Z- handleDepth()-Depth_Keller msg: 1613178027109 epoch sec; seqNo:17698; depth 14.48 2021-02-13T01:00:27Z- handleDvl() - RDI_Pathfinder msg: 1613178027.12 epoch sec; seqNo:8536 2021-02-13T01:00:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.08 , 112.25 , 78.44 2021-02-13T01:00:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , -0.15 2021-02-13T01:00:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178027.15 epoch sec; seqNo:17698; 4060598.15 north; 593518.60 east 2021-02-13T01:00:27Z- AHRS_M2 msg: 1613178027.49 epoch sec; seqNo:17561 2021-02-13T01:00:27Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-13T01:00:27Z- handleDepth()-Depth_Keller msg: 1613178027494 epoch sec; seqNo:17699; depth 14.36 2021-02-13T01:00:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178027.51 epoch sec; seqNo:17699; 4060598.53 north; 593518.59 east 2021-02-13T01:00:27Z- AHRS_M2 msg: 1613178027.89 epoch sec; seqNo:17562 2021-02-13T01:00:27Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-13T01:00:27Z- handleDepth()-Depth_Keller msg: 1613178027901 epoch sec; seqNo:17700; depth 14.25 2021-02-13T01:00:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178027.92 epoch sec; seqNo:17700; 4060598.53 north; 593518.59 east 2021-02-13T01:00:28Z- AHRS_M2 msg: 1613178028.30 epoch sec; seqNo:17563 2021-02-13T01:00:28Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.17 psi 2021-02-13T01:00:28Z- handleDepth()-Depth_Keller msg: 1613178028300 epoch sec; seqNo:17701; depth 14.14 2021-02-13T01:00:28Z- handleDvl() - RDI_Pathfinder msg: 1613178028.31 epoch sec; seqNo:8537 2021-02-13T01:00:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.01 , 113.54 , 78.38 2021-02-13T01:00:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , -0.07 2021-02-13T01:00:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178028.34 epoch sec; seqNo:17701; 4060598.90 north; 593518.59 east 2021-02-13T01:00:28Z- AHRS_M2 msg: 1613178028.70 epoch sec; seqNo:17564 2021-02-13T01:00:28Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-13T01:00:28Z- handleDepth()-Depth_Keller msg: 1613178028704 epoch sec; seqNo:17702; depth 14.02 2021-02-13T01:00:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178028.72 epoch sec; seqNo:17702; 4060599.30 north; 593519.80 east 2021-02-13T01:00:29Z- AHRS_M2 msg: 1613178029.11 epoch sec; seqNo:17565 2021-02-13T01:00:29Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.16 psi 2021-02-13T01:00:29Z- handleDepth()-Depth_Keller msg: 1613178029110 epoch sec; seqNo:17703; depth 13.89 2021-02-13T01:00:29Z- handleDvl() - RDI_Pathfinder msg: 1613178029.12 epoch sec; seqNo:8538 2021-02-13T01:00:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.37 , 98.59 , 113.47 , 74.80 2021-02-13T01:00:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , 0.05 2021-02-13T01:00:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178029.14 epoch sec; seqNo:17703; 4060599.30 north; 593519.80 east 2021-02-13T01:00:29Z- AHRS_M2 msg: 1613178029.51 epoch sec; seqNo:17566 2021-02-13T01:00:29Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.16 psi 2021-02-13T01:00:29Z- handleDepth()-Depth_Keller msg: 1613178029512 epoch sec; seqNo:17704; depth 13.77 2021-02-13T01:00:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:00:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:00:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:00:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:00:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:00:29Z- handleDvl() - RDI_Pathfinder msg: 1613178029.51 epoch sec; seqNo:8539 2021-02-13T01:00:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , 0.05 2021-02-13T01:00:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178029.53 epoch sec; seqNo:17704; 4060599.68 north; 593519.80 east 2021-02-13T01:00:29Z- AHRS_M2 msg: 1613178029.92 epoch sec; seqNo:17567 2021-02-13T01:00:29Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.16 psi 2021-02-13T01:00:29Z- handleDepth()-Depth_Keller msg: 1613178029930 epoch sec; seqNo:17705; depth 13.64 2021-02-13T01:00:29Z- handleDvl() - RDI_Pathfinder msg: 1613178029.97 epoch sec; seqNo:8540 2021-02-13T01:00:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.71 , 112.59 , 74.80 2021-02-13T01:00:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , 0.12 2021-02-13T01:00:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178029.98 epoch sec; seqNo:17705; 4060600.05 north; 593519.80 east 2021-02-13T01:00:30Z- LcmTrn::handleMessages() - handling 43 messages took 0.76 ms and 128 calls... 2021-02-13T01:00:30Z- waiting for 1613178028.10, time is 1613178029.98 2021-02-13T01:00:30Z- LcmTrn::updateTrn() equating pose 1613178029.98 and meas times 1613178029.98 2021-02-13T01:00:30Z- LcmTrn::updateTrn() >>>> pose time 1613178029.98, meas time 1613178029.98 <<<< 2021-02-13T01:00:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178029.98 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:00:30Z- TerrainNav::motionUpdate() - currEstimate = (4060600, 593519.80, 13.64) 2021-02-13T01:00:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:00:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:00:30Z- TNavBF::Projecting Measurements 2021-02-13T01:00:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:00:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:00:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:00:30Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:00:30Z- TNavFilter::3 good beams. 2021-02-13T01:00:30Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:00:30Z- TNavBF:: beamIndex[0] = 1 2021-02-13T01:00:30Z- TNavBF:: beamIndex[1] = 2 2021-02-13T01:00:30Z- TNavBF:: beamIndex[2] = 3 2021-02-13T01:00:30Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:00:30Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:00:30Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:00:30Z- TNavBF::mmse selected from filter -1 2021-02-13T01:00:30Z- TerrainMapOctree: (vehN, vehE ) = (4060600.05, 593519.80). 2021-02-13T01:00:30Z- TerrainMapOctree: Min Distance = 977.56. 2021-02-13T01:00:30Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:00:30Z- There is no specific good beam that all particles have in common. 2021-02-13T01:00:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:00:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.48 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T01:00:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:00:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.36 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 20.28 2021-02-13T01:00:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:00:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.107 ExpectedMeasDiff: nan currMeas.covariance: 8.95 2021-02-13T01:00:30Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:00:30Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:00:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:00:30Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:00:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:00:30Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:00:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:00:30Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:00:30Z- Filter 0 No beams used in subcloud update 2021-02-13T01:00:30Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:00:30Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:00:30Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 679.3 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T01:00:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178029.98 sec. 2021-02-13T01:00:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:00:30Z- TNavBF::mmse selected from filter -1 2021-02-13T01:00:30Z- variances([0], [2], and psi_berg) = 679.315438 832.988189 0.000000 2021-02-13T01:00:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:00:30Z- LcmTrn::publishEstimates() - MLE : 4060599.55 593536.30 13.64 2021-02-13T01:00:30Z- LcmTrn::publishEstimates() - MMSE : 4060600.05 593519.80 13.64 2021-02-13T01:00:30Z- LcmTrn::publishEstimates() - COVAR: 679.32 832.99 1.15 0.00 2021-02-13T01:00:30Z- AHRS_M2 msg: 1613178030.32 epoch sec; seqNo:17568 2021-02-13T01:00:30Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.17 psi 2021-02-13T01:00:30Z- handleDepth()-Depth_Keller msg: 1613178030320 epoch sec; seqNo:17706; depth 13.51 2021-02-13T01:00:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178030.33 epoch sec; seqNo:17706; 4060600.43 north; 593519.79 east 2021-02-13T01:00:30Z- AHRS_M2 msg: 1613178030.72 epoch sec; seqNo:17569 2021-02-13T01:00:30Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.17 psi 2021-02-13T01:00:30Z- handleDepth()-Depth_Keller msg: 1613178030727 epoch sec; seqNo:17707; depth 13.40 2021-02-13T01:00:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:00:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:00:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:00:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:00:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:00:30Z- handleDvl() - RDI_Pathfinder msg: 1613178030.73 epoch sec; seqNo:8541 2021-02-13T01:00:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , 0.12 2021-02-13T01:00:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178030.74 epoch sec; seqNo:17707; 4060600.43 north; 593519.79 east 2021-02-13T01:00:31Z- AHRS_M2 msg: 1613178031.13 epoch sec; seqNo:17570 2021-02-13T01:00:31Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.17 psi 2021-02-13T01:00:31Z- handleDepth()-Depth_Keller msg: 1613178031131 epoch sec; seqNo:17708; depth 13.29 2021-02-13T01:00:31Z- handleDvl() - RDI_Pathfinder msg: 1613178031.15 epoch sec; seqNo:8542 2021-02-13T01:00:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 98.80 , 113.67 , 78.51 2021-02-13T01:00:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.12 , 0.07 2021-02-13T01:00:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178031.17 epoch sec; seqNo:17708; 4060600.83 north; 593521.01 east 2021-02-13T01:00:31Z- AHRS_M2 msg: 1613178031.53 epoch sec; seqNo:17571 2021-02-13T01:00:31Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.17 psi 2021-02-13T01:00:31Z- handleDepth()-Depth_Keller msg: 1613178031532 epoch sec; seqNo:17709; depth 13.14 2021-02-13T01:00:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178031.54 epoch sec; seqNo:17709; 4060601.20 north; 593521.00 east 2021-02-13T01:00:31Z- AHRS_M2 msg: 1613178031.94 epoch sec; seqNo:17572 2021-02-13T01:00:31Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.18 psi 2021-02-13T01:00:31Z- handleDepth()-Depth_Keller msg: 1613178031942 epoch sec; seqNo:17710; depth 13.03 2021-02-13T01:00:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178031.95 epoch sec; seqNo:17710; 4060601.58 north; 593521.00 east 2021-02-13T01:00:32Z- AHRS_M2 msg: 1613178032.34 epoch sec; seqNo:17573 2021-02-13T01:00:32Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.18 psi 2021-02-13T01:00:32Z- handleDepth()-Depth_Keller msg: 1613178032349 epoch sec; seqNo:17711; depth 12.92 2021-02-13T01:00:32Z- handleDvl() - RDI_Pathfinder msg: 1613178032.36 epoch sec; seqNo:8543 2021-02-13T01:00:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.70 , 98.93 , 115.16 , 78.65 2021-02-13T01:00:32Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.13 , -0.05 2021-02-13T01:00:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178032.37 epoch sec; seqNo:17711; 4060601.58 north; 593521.00 east 2021-02-13T01:00:32Z- AHRS_M2 msg: 1613178032.74 epoch sec; seqNo:17574 2021-02-13T01:00:32Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.18 psi 2021-02-13T01:00:32Z- handleDepth()-Depth_Keller msg: 1613178032744 epoch sec; seqNo:17712; depth 12.80 2021-02-13T01:00:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178032.76 epoch sec; seqNo:17712; 4060601.96 north; 593521.00 east 2021-02-13T01:00:33Z- AHRS_M2 msg: 1613178033.15 epoch sec; seqNo:17575 2021-02-13T01:00:33Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.17 psi 2021-02-13T01:00:33Z- handleDepth()-Depth_Keller msg: 1613178033153 epoch sec; seqNo:17713; depth 12.69 2021-02-13T01:00:33Z- handleDvl() - RDI_Pathfinder msg: 1613178033.16 epoch sec; seqNo:8544 2021-02-13T01:00:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 101.64 , 112.46 , 78.65 2021-02-13T01:00:33Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.04 , -0.14 2021-02-13T01:00:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178033.19 epoch sec; seqNo:17713; 4060602.35 north; 593522.21 east 2021-02-13T01:00:33Z- AHRS_M2 msg: 1613178033.55 epoch sec; seqNo:17576 2021-02-13T01:00:33Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.18 psi 2021-02-13T01:00:33Z- handleDepth()-Depth_Keller msg: 1613178033557 epoch sec; seqNo:17714; depth 12.56 2021-02-13T01:00:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178033.57 epoch sec; seqNo:17714; 4060602.73 north; 593522.21 east 2021-02-13T01:00:33Z- AHRS_M2 msg: 1613178033.95 epoch sec; seqNo:17577 2021-02-13T01:00:33Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.18 psi 2021-02-13T01:00:33Z- handleDepth()-Depth_Keller msg: 1613178033958 epoch sec; seqNo:17715; depth 12.46 2021-02-13T01:00:33Z- handleDvl() - RDI_Pathfinder msg: 1613178033.97 epoch sec; seqNo:8545 2021-02-13T01:00:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:33Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:00:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178033.98 epoch sec; seqNo:17715; 4060603.11 north; 593522.20 east 2021-02-13T01:00:34Z- AHRS_M2 msg: 1613178034.36 epoch sec; seqNo:17578 2021-02-13T01:00:34Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.17 psi 2021-02-13T01:00:34Z- handleDepth()-Depth_Keller msg: 1613178034370 epoch sec; seqNo:17716; depth 12.35 2021-02-13T01:00:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:00:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:00:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:00:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:00:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:00:34Z- handleDvl() - RDI_Pathfinder msg: 1613178034.37 epoch sec; seqNo:8546 2021-02-13T01:00:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:34Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:00:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178034.38 epoch sec; seqNo:17716; 4060603.11 north; 593522.20 east 2021-02-13T01:00:34Z- AHRS_M2 msg: 1613178034.76 epoch sec; seqNo:17579 2021-02-13T01:00:34Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.17 psi 2021-02-13T01:00:34Z- handleDepth()-Depth_Keller msg: 1613178034764 epoch sec; seqNo:17717; depth 12.23 2021-02-13T01:00:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178034.77 epoch sec; seqNo:17717; 4060603.49 north; 593522.20 east 2021-02-13T01:00:35Z- AHRS_M2 msg: 1613178035.16 epoch sec; seqNo:17580 2021-02-13T01:00:35Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.17 psi 2021-02-13T01:00:35Z- handleDepth()-Depth_Keller msg: 1613178035173 epoch sec; seqNo:17718; depth 12.11 2021-02-13T01:00:35Z- handleDvl() - RDI_Pathfinder msg: 1613178035.18 epoch sec; seqNo:8547 2021-02-13T01:00:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.57 , 115.09 , 73.98 2021-02-13T01:00:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.08 , 0.07 2021-02-13T01:00:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178035.19 epoch sec; seqNo:17718; 4060603.87 north; 593522.20 east 2021-02-13T01:00:35Z- LcmTrn::handleMessages() - handling 46 messages took 0.71 ms and 132 calls... 2021-02-13T01:00:35Z- waiting for 1613178032.98, time is 1613178035.19 2021-02-13T01:00:35Z- LcmTrn::updateTrn() equating pose 1613178035.19 and meas times 1613178035.19 2021-02-13T01:00:35Z- LcmTrn::updateTrn() >>>> pose time 1613178035.19, meas time 1613178035.19 <<<< 2021-02-13T01:00:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178035.19 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:00:35Z- TerrainNav::motionUpdate() - currEstimate = (4060604, 593522.20, 12.11) 2021-02-13T01:00:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:00:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:00:35Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178035.19 sec.; No relevant navigation data available 2021-02-13T01:00:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:00:35Z- TNavBF::mmse selected from filter -1 2021-02-13T01:00:35Z- variances([0], [2], and psi_berg) = 679.269176 832.988169 0.000000 2021-02-13T01:00:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:00:35Z- LcmTrn::publishEstimates() - MLE : 4060603.37 593541.70 12.11 2021-02-13T01:00:35Z- LcmTrn::publishEstimates() - MMSE : 4060603.87 593522.20 12.11 2021-02-13T01:00:35Z- LcmTrn::publishEstimates() - COVAR: 679.27 832.99 1.38 0.00 2021-02-13T01:00:35Z- AHRS_M2 msg: 1613178035.57 epoch sec; seqNo:17581 2021-02-13T01:00:35Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.17 psi 2021-02-13T01:00:35Z- handleDepth()-Depth_Keller msg: 1613178035579 epoch sec; seqNo:17719; depth 11.99 2021-02-13T01:00:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:35Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:00:35Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:00:35Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:00:35Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:00:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:00:35Z- handleDvl() - RDI_Pathfinder msg: 1613178035.58 epoch sec; seqNo:8548 2021-02-13T01:00:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.08 , 0.07 2021-02-13T01:00:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178035.60 epoch sec; seqNo:17719; 4060604.26 north; 593523.41 east 2021-02-13T01:00:36Z- AHRS_M2 msg: 1613178035.99 epoch sec; seqNo:17582 2021-02-13T01:00:36Z- AHRS_M2 msg: 0.55 phi; 0.36 theta; -0.17 psi 2021-02-13T01:00:36Z- handleDepth()-Depth_Keller msg: 1613178036016 epoch sec; seqNo:17720; depth 11.86 2021-02-13T01:00:36Z- handleDvl() - RDI_Pathfinder msg: 1613178036.03 epoch sec; seqNo:8549 2021-02-13T01:00:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.37 , 117.60 , 76.43 2021-02-13T01:00:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.03 , 0.16 2021-02-13T01:00:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178036.04 epoch sec; seqNo:17720; 4060604.64 north; 593523.41 east 2021-02-13T01:00:36Z- AHRS_M2 msg: 1613178036.38 epoch sec; seqNo:17583 2021-02-13T01:00:36Z- AHRS_M2 msg: 0.54 phi; 0.37 theta; -0.17 psi 2021-02-13T01:00:36Z- handleDepth()-Depth_Keller msg: 1613178036383 epoch sec; seqNo:17721; depth 11.72 2021-02-13T01:00:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178036.39 epoch sec; seqNo:17721; 4060604.64 north; 593523.41 east 2021-02-13T01:00:36Z- AHRS_M2 msg: 1613178036.78 epoch sec; seqNo:17584 2021-02-13T01:00:36Z- AHRS_M2 msg: 0.54 phi; 0.37 theta; -0.16 psi 2021-02-13T01:00:36Z- handleDepth()-Depth_Keller msg: 1613178036849 epoch sec; seqNo:17722; depth 11.62 2021-02-13T01:00:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:00:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:00:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:00:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:00:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:00:36Z- handleDvl() - RDI_Pathfinder msg: 1613178036.85 epoch sec; seqNo:8550 2021-02-13T01:00:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.03 , 0.16 2021-02-13T01:00:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178036.86 epoch sec; seqNo:17722; 4060605.02 north; 593523.40 east 2021-02-13T01:00:37Z- AHRS_M2 msg: 1613178037.18 epoch sec; seqNo:17585 2021-02-13T01:00:37Z- AHRS_M2 msg: 0.54 phi; 0.37 theta; -0.15 psi 2021-02-13T01:00:37Z- handleDepth()-Depth_Keller msg: 1613178037212 epoch sec; seqNo:17723; depth 11.49 2021-02-13T01:00:37Z- handleDvl() - RDI_Pathfinder msg: 1613178037.23 epoch sec; seqNo:8551 2021-02-13T01:00:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.23 , 118.81 , 76.02 2021-02-13T01:00:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.23 , 0.10 2021-02-13T01:00:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178037.25 epoch sec; seqNo:17723; 4060605.40 north; 593523.40 east 2021-02-13T01:00:37Z- AHRS_M2 msg: 1613178037.59 epoch sec; seqNo:17586 2021-02-13T01:00:37Z- AHRS_M2 msg: 0.55 phi; 0.38 theta; -0.14 psi 2021-02-13T01:00:37Z- handleDepth()-Depth_Keller msg: 1613178037614 epoch sec; seqNo:17724; depth 11.38 2021-02-13T01:00:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178037.63 epoch sec; seqNo:17724; 4060605.79 north; 593524.62 east 2021-02-13T01:00:37Z- AHRS_M2 msg: 1613178037.99 epoch sec; seqNo:17587 2021-02-13T01:00:37Z- AHRS_M2 msg: 0.56 phi; 0.38 theta; -0.14 psi 2021-02-13T01:00:38Z- handleDepth()-Depth_Keller msg: 1613178038017 epoch sec; seqNo:17725; depth 11.23 2021-02-13T01:00:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:38Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:00:38Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:00:38Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:00:38Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:00:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:00:38Z- handleDvl() - RDI_Pathfinder msg: 1613178038.02 epoch sec; seqNo:8552 2021-02-13T01:00:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.23 , 0.10 2021-02-13T01:00:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178038.04 epoch sec; seqNo:17725; 4060606.17 north; 593524.61 east 2021-02-13T01:00:38Z- AHRS_M2 msg: 1613178038.42 epoch sec; seqNo:17588 2021-02-13T01:00:38Z- AHRS_M2 msg: 0.57 phi; 0.37 theta; -0.14 psi 2021-02-13T01:00:38Z- handleDepth()-Depth_Keller msg: 1613178038425 epoch sec; seqNo:17726; depth 11.11 2021-02-13T01:00:38Z- handleDvl() - RDI_Pathfinder msg: 1613178038.44 epoch sec; seqNo:8553 2021-02-13T01:00:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.17 , 99.88 , 118.81 , 78.24 2021-02-13T01:00:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.29 , -0.11 2021-02-13T01:00:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178038.46 epoch sec; seqNo:17726; 4060606.17 north; 593524.61 east 2021-02-13T01:00:38Z- AHRS_M2 msg: 1613178038.80 epoch sec; seqNo:17589 2021-02-13T01:00:38Z- AHRS_M2 msg: 0.58 phi; 0.37 theta; -0.14 psi 2021-02-13T01:00:38Z- handleDepth()-Depth_Keller msg: 1613178038811 epoch sec; seqNo:17727; depth 10.99 2021-02-13T01:00:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178038.82 epoch sec; seqNo:17727; 4060606.55 north; 593524.61 east 2021-02-13T01:00:39Z- AHRS_M2 msg: 1613178039.20 epoch sec; seqNo:17590 2021-02-13T01:00:39Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.14 psi 2021-02-13T01:00:39Z- handleDepth()-Depth_Keller msg: 1613178039277 epoch sec; seqNo:17728; depth 10.85 2021-02-13T01:00:39Z- handleDvl() - RDI_Pathfinder msg: 1613178039.29 epoch sec; seqNo:8554 2021-02-13T01:00:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 99.88 , 124.22 , 76.43 2021-02-13T01:00:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.18 , -0.28 2021-02-13T01:00:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178039.30 epoch sec; seqNo:17728; 4060606.55 north; 593524.61 east 2021-02-13T01:00:39Z- AHRS_M2 msg: 1613178039.61 epoch sec; seqNo:17591 2021-02-13T01:00:39Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.15 psi 2021-02-13T01:00:39Z- handleDepth()-Depth_Keller msg: 1613178039621 epoch sec; seqNo:17729; depth 10.70 2021-02-13T01:00:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178039.63 epoch sec; seqNo:17729; 4060606.93 north; 593524.60 east 2021-02-13T01:00:40Z- AHRS_M2 msg: 1613178040.01 epoch sec; seqNo:17592 2021-02-13T01:00:40Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.15 psi 2021-02-13T01:00:40Z- handleDepth()-Depth_Keller msg: 1613178040042 epoch sec; seqNo:17730; depth 10.60 2021-02-13T01:00:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178040.08 epoch sec; seqNo:17730; 4060606.94 north; 593525.82 east 2021-02-13T01:00:40Z- LcmTrn::handleMessages() - handling 43 messages took 0.75 ms and 128 calls... 2021-02-13T01:00:40Z- waiting for 1613178038.19, time is 1613178040.08 2021-02-13T01:00:40Z- LcmTrn::updateTrn() >>>> pose time 1613178040.08, meas time 1613178039.29 <<<< 2021-02-13T01:00:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178039.29 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:00:40Z- TerrainNav::motionUpdate() - currEstimate = (4060607, 593525.82, 10.60) 2021-02-13T01:00:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:00:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:00:40Z- TNavBF::Projecting Measurements 2021-02-13T01:00:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:00:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:00:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:00:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:00:40Z- TNavFilter::4 good beams. 2021-02-13T01:00:40Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:00:40Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:00:40Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:00:40Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:00:40Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:00:40Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:00:40Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:00:40Z- TNavBF::mmse selected from filter -1 2021-02-13T01:00:40Z- TerrainMapOctree: (vehN, vehE ) = (4060606.45, 593525.24). 2021-02-13T01:00:40Z- TerrainMapOctree: Min Distance = 969.25. 2021-02-13T01:00:40Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:00:40Z- There is no specific good beam that all particles have in common. 2021-02-13T01:00:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:00:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.58 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 12.69 2021-02-13T01:00:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:00:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.42 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 15.96 2021-02-13T01:00:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:00:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.32 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 24.69 2021-02-13T01:00:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:00:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 9.35 2021-02-13T01:00:40Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:00:40Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:00:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:00:40Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:00:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:00:40Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:00:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:00:40Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:00:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:00:40Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:00:40Z- Filter 0 No beams used in subcloud update 2021-02-13T01:00:40Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:00:40Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:00:40Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 679.2 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T01:00:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178039.29 sec. 2021-02-13T01:00:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:00:40Z- TNavBF::mmse selected from filter -1 2021-02-13T01:00:40Z- variances([0], [2], and psi_berg) = 679.206636 832.988143 0.000000 2021-02-13T01:00:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:00:40Z- LcmTrn::publishEstimates() - MLE : 4060606.44 593545.32 10.60 2021-02-13T01:00:40Z- LcmTrn::publishEstimates() - MMSE : 4060606.94 593525.82 10.60 2021-02-13T01:00:40Z- LcmTrn::publishEstimates() - COVAR: 679.21 832.99 1.17 0.00 2021-02-13T01:00:40Z- AHRS_M2 msg: 1613178040.42 epoch sec; seqNo:17593 2021-02-13T01:00:40Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.15 psi 2021-02-13T01:00:40Z- handleDepth()-Depth_Keller msg: 1613178040441 epoch sec; seqNo:17731; depth 10.45 2021-02-13T01:00:40Z- handleDvl() - RDI_Pathfinder msg: 1613178040.46 epoch sec; seqNo:8555 2021-02-13T01:00:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.30 , 118.88 , 78.31 2021-02-13T01:00:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , -0.26 2021-02-13T01:00:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178040.47 epoch sec; seqNo:17731; 4060607.32 north; 593525.82 east 2021-02-13T01:00:40Z- AHRS_M2 msg: 1613178040.83 epoch sec; seqNo:17594 2021-02-13T01:00:40Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T01:00:40Z- handleDepth()-Depth_Keller msg: 1613178040832 epoch sec; seqNo:17732; depth 10.32 2021-02-13T01:00:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178040.84 epoch sec; seqNo:17732; 4060607.70 north; 593525.82 east 2021-02-13T01:00:41Z- AHRS_M2 msg: 1613178041.24 epoch sec; seqNo:17595 2021-02-13T01:00:41Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T01:00:41Z- handleDepth()-Depth_Keller msg: 1613178041243 epoch sec; seqNo:17733; depth 10.21 2021-02-13T01:00:41Z- handleDvl() - RDI_Pathfinder msg: 1613178041.26 epoch sec; seqNo:8556 2021-02-13T01:00:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 101.16 , 117.39 , 79.53 2021-02-13T01:00:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.20 , -0.05 2021-02-13T01:00:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178041.27 epoch sec; seqNo:17733; 4060607.70 north; 593525.82 east 2021-02-13T01:00:41Z- AHRS_M2 msg: 1613178041.63 epoch sec; seqNo:17596 2021-02-13T01:00:41Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.15 psi 2021-02-13T01:00:41Z- handleDepth()-Depth_Keller msg: 1613178041632 epoch sec; seqNo:17734; depth 10.07 2021-02-13T01:00:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:00:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:00:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:00:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:00:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:00:41Z- handleDvl() - RDI_Pathfinder msg: 1613178041.63 epoch sec; seqNo:8557 2021-02-13T01:00:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.20 , -0.05 2021-02-13T01:00:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178041.65 epoch sec; seqNo:17734; 4060608.08 north; 593525.81 east 2021-02-13T01:00:42Z- AHRS_M2 msg: 1613178042.03 epoch sec; seqNo:17597 2021-02-13T01:00:42Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-13T01:00:42Z- handleDepth()-Depth_Keller msg: 1613178042043 epoch sec; seqNo:17735; depth 9.95 2021-02-13T01:00:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178042.08 epoch sec; seqNo:17735; 4060608.46 north; 593525.81 east 2021-02-13T01:00:42Z- AHRS_M2 msg: 1613178042.44 epoch sec; seqNo:17598 2021-02-13T01:00:42Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.16 psi 2021-02-13T01:00:42Z- handleDepth()-Depth_Keller msg: 1613178042442 epoch sec; seqNo:17736; depth 9.80 2021-02-13T01:00:42Z- handleDvl() - RDI_Pathfinder msg: 1613178042.45 epoch sec; seqNo:8558 2021-02-13T01:00:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.72 , 116.24 , 78.38 2021-02-13T01:00:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.15 , 0.10 2021-02-13T01:00:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178042.46 epoch sec; seqNo:17736; 4060608.83 north; 593525.80 east 2021-02-13T01:00:42Z- AHRS_M2 msg: 1613178042.84 epoch sec; seqNo:17599 2021-02-13T01:00:42Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.16 psi 2021-02-13T01:00:42Z- handleDepth()-Depth_Keller msg: 1613178042844 epoch sec; seqNo:17737; depth 9.69 2021-02-13T01:00:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:00:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:00:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:00:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:00:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:00:42Z- handleDvl() - RDI_Pathfinder msg: 1613178042.85 epoch sec; seqNo:8559 2021-02-13T01:00:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.15 , 0.10 2021-02-13T01:00:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178042.87 epoch sec; seqNo:17737; 4060609.21 north; 593525.80 east 2021-02-13T01:00:43Z- AHRS_M2 msg: 1613178043.24 epoch sec; seqNo:17600 2021-02-13T01:00:43Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.17 psi 2021-02-13T01:00:43Z- handleDepth()-Depth_Keller msg: 1613178043249 epoch sec; seqNo:17738; depth 9.56 2021-02-13T01:00:43Z- handleDvl() - RDI_Pathfinder msg: 1613178043.26 epoch sec; seqNo:8560 2021-02-13T01:00:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.16 , 117.39 , 75.61 2021-02-13T01:00:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.08 , 0.11 2021-02-13T01:00:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178043.28 epoch sec; seqNo:17738; 4060609.23 north; 593527.02 east 2021-02-13T01:00:43Z- AHRS_M2 msg: 1613178043.65 epoch sec; seqNo:17601 2021-02-13T01:00:43Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.18 psi 2021-02-13T01:00:43Z- handleDepth()-Depth_Keller msg: 1613178043652 epoch sec; seqNo:17739; depth 9.44 2021-02-13T01:00:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178043.66 epoch sec; seqNo:17739; 4060609.61 north; 593527.02 east 2021-02-13T01:00:44Z- AHRS_M2 msg: 1613178044.05 epoch sec; seqNo:17602 2021-02-13T01:00:44Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.18 psi 2021-02-13T01:00:44Z- handleDepth()-Depth_Keller msg: 1613178044058 epoch sec; seqNo:17740; depth 9.31 2021-02-13T01:00:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:00:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:00:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:00:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:00:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:00:44Z- handleDvl() - RDI_Pathfinder msg: 1613178044.06 epoch sec; seqNo:8561 2021-02-13T01:00:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.08 , 0.11 2021-02-13T01:00:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178044.07 epoch sec; seqNo:17740; 4060609.98 north; 593527.01 east 2021-02-13T01:00:44Z- AHRS_M2 msg: 1613178044.46 epoch sec; seqNo:17603 2021-02-13T01:00:44Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.18 psi 2021-02-13T01:00:44Z- handleDepth()-Depth_Keller msg: 1613178044460 epoch sec; seqNo:17741; depth 9.19 2021-02-13T01:00:44Z- handleDvl() - RDI_Pathfinder msg: 1613178044.48 epoch sec; seqNo:8562 2021-02-13T01:00:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.17 , 103.94 , 121.52 , 79.59 2021-02-13T01:00:44Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.00 , 0.11 2021-02-13T01:00:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178044.49 epoch sec; seqNo:17741; 4060610.36 north; 593527.01 east 2021-02-13T01:00:44Z- AHRS_M2 msg: 1613178044.86 epoch sec; seqNo:17604 2021-02-13T01:00:44Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.18 psi 2021-02-13T01:00:44Z- handleDepth()-Depth_Keller msg: 1613178044869 epoch sec; seqNo:17742; depth 9.05 2021-02-13T01:00:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178044.88 epoch sec; seqNo:17742; 4060610.74 north; 593527.00 east 2021-02-13T01:00:45Z- AHRS_M2 msg: 1613178045.26 epoch sec; seqNo:17605 2021-02-13T01:00:45Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.17 psi 2021-02-13T01:00:45Z- handleDepth()-Depth_Keller msg: 1613178045269 epoch sec; seqNo:17743; depth 8.95 2021-02-13T01:00:45Z- handleDvl() - RDI_Pathfinder msg: 1613178045.28 epoch sec; seqNo:8563 2021-02-13T01:00:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 106.44 , 124.02 , 79.39 2021-02-13T01:00:45Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.06 , 0.13 2021-02-13T01:00:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178045.29 epoch sec; seqNo:17743; 4060611.12 north; 593527.00 east 2021-02-13T01:00:45Z- LcmTrn::handleMessages() - handling 48 messages took 0.68 ms and 135 calls... 2021-02-13T01:00:45Z- waiting for 1613178043.08, time is 1613178045.29 2021-02-13T01:00:45Z- LcmTrn::updateTrn() equating pose 1613178045.29 and meas times 1613178045.29 2021-02-13T01:00:45Z- LcmTrn::updateTrn() >>>> pose time 1613178045.29, meas time 1613178045.29 <<<< 2021-02-13T01:00:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178045.29 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:00:45Z- TerrainNav::motionUpdate() - currEstimate = (4060611, 593527.00, 8.95) 2021-02-13T01:00:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:00:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:00:45Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178045.29 sec.; No relevant navigation data available 2021-02-13T01:00:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:00:45Z- TNavBF::mmse selected from filter -1 2021-02-13T01:00:45Z- variances([0], [2], and psi_berg) = 679.161277 832.988124 0.000000 2021-02-13T01:00:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:00:45Z- LcmTrn::publishEstimates() - MLE : 4060610.62 593543.50 8.95 2021-02-13T01:00:45Z- LcmTrn::publishEstimates() - MMSE : 4060611.12 593527.00 8.95 2021-02-13T01:00:45Z- LcmTrn::publishEstimates() - COVAR: 679.16 832.99 1.39 0.00 2021-02-13T01:00:45Z- AHRS_M2 msg: 1613178045.67 epoch sec; seqNo:17606 2021-02-13T01:00:45Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.15 psi 2021-02-13T01:00:45Z- handleDepth()-Depth_Keller msg: 1613178045675 epoch sec; seqNo:17744; depth 8.80 2021-02-13T01:00:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178045.69 epoch sec; seqNo:17744; 4060611.13 north; 593528.22 east 2021-02-13T01:00:46Z- AHRS_M2 msg: 1613178046.08 epoch sec; seqNo:17607 2021-02-13T01:00:46Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-13T01:00:46Z- handleDepth()-Depth_Keller msg: 1613178046106 epoch sec; seqNo:17745; depth 8.69 2021-02-13T01:00:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178046.13 epoch sec; seqNo:17745; 4060611.51 north; 593528.22 east 2021-02-13T01:00:46Z- AHRS_M2 msg: 1613178046.48 epoch sec; seqNo:17608 2021-02-13T01:00:46Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-13T01:00:46Z- handleDepth()-Depth_Keller msg: 1613178046485 epoch sec; seqNo:17746; depth 8.54 2021-02-13T01:00:46Z- handleDvl() - RDI_Pathfinder msg: 1613178046.50 epoch sec; seqNo:8564 2021-02-13T01:00:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 105.42 , 121.79 , 80.14 2021-02-13T01:00:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.20 , 0.09 2021-02-13T01:00:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178046.51 epoch sec; seqNo:17746; 4060611.89 north; 593528.21 east 2021-02-13T01:00:46Z- AHRS_M2 msg: 1613178046.89 epoch sec; seqNo:17609 2021-02-13T01:00:46Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-13T01:00:46Z- handleDepth()-Depth_Keller msg: 1613178046890 epoch sec; seqNo:17747; depth 8.40 2021-02-13T01:00:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178046.90 epoch sec; seqNo:17747; 4060612.27 north; 593528.21 east 2021-02-13T01:00:47Z- AHRS_M2 msg: 1613178047.28 epoch sec; seqNo:17610 2021-02-13T01:00:47Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-13T01:00:47Z- handleDepth()-Depth_Keller msg: 1613178047293 epoch sec; seqNo:17748; depth 8.26 2021-02-13T01:00:47Z- handleDvl() - RDI_Pathfinder msg: 1613178047.31 epoch sec; seqNo:8565 2021-02-13T01:00:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 103.94 , 120.30 , 0.00 2021-02-13T01:00:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.14 , -0.02 2021-02-13T01:00:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178047.32 epoch sec; seqNo:17748; 4060612.27 north; 593528.21 east 2021-02-13T01:00:47Z- AHRS_M2 msg: 1613178047.69 epoch sec; seqNo:17611 2021-02-13T01:00:47Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.19 psi 2021-02-13T01:00:47Z- handleDepth()-Depth_Keller msg: 1613178047692 epoch sec; seqNo:17749; depth 8.13 2021-02-13T01:00:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:00:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:00:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:00:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:00:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:00:47Z- handleDvl() - RDI_Pathfinder msg: 1613178047.69 epoch sec; seqNo:8566 2021-02-13T01:00:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.14 , -0.02 2021-02-13T01:00:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178047.71 epoch sec; seqNo:17749; 4060612.66 north; 593529.42 east 2021-02-13T01:00:48Z- AHRS_M2 msg: 1613178048.09 epoch sec; seqNo:17612 2021-02-13T01:00:48Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.20 psi 2021-02-13T01:00:48Z- handleDepth()-Depth_Keller msg: 1613178048103 epoch sec; seqNo:17750; depth 8.00 2021-02-13T01:00:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178048.11 epoch sec; seqNo:17750; 4060613.04 north; 593529.42 east 2021-02-13T01:00:48Z- AHRS_M2 msg: 1613178048.50 epoch sec; seqNo:17613 2021-02-13T01:00:48Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.19 psi 2021-02-13T01:00:48Z- handleDepth()-Depth_Keller msg: 1613178048503 epoch sec; seqNo:17751; depth 7.89 2021-02-13T01:00:48Z- handleDvl() - RDI_Pathfinder msg: 1613178048.51 epoch sec; seqNo:8567 2021-02-13T01:00:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.44 , 106.37 , 116.78 , 81.08 2021-02-13T01:00:48Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.12 , -0.04 2021-02-13T01:00:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178048.52 epoch sec; seqNo:17751; 4060613.04 north; 593529.42 east 2021-02-13T01:00:48Z- AHRS_M2 msg: 1613178048.90 epoch sec; seqNo:17614 2021-02-13T01:00:48Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.18 psi 2021-02-13T01:00:48Z- handleDepth()-Depth_Keller msg: 1613178048905 epoch sec; seqNo:17752; depth 7.76 2021-02-13T01:00:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:00:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:00:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:00:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:00:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:00:48Z- handleDvl() - RDI_Pathfinder msg: 1613178048.91 epoch sec; seqNo:8568 2021-02-13T01:00:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:48Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.12 , -0.04 2021-02-13T01:00:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178048.93 epoch sec; seqNo:17752; 4060613.42 north; 593529.42 east 2021-02-13T01:00:49Z- AHRS_M2 msg: 1613178049.30 epoch sec; seqNo:17615 2021-02-13T01:00:49Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.17 psi 2021-02-13T01:00:49Z- handleDepth()-Depth_Keller msg: 1613178049313 epoch sec; seqNo:17753; depth 7.65 2021-02-13T01:00:49Z- handleDvl() - RDI_Pathfinder msg: 1613178049.32 epoch sec; seqNo:8569 2021-02-13T01:00:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.86 , 109.14 , 124.02 , 80.74 2021-02-13T01:00:49Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , 0.15 , -0.12 2021-02-13T01:00:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178049.35 epoch sec; seqNo:17753; 4060613.80 north; 593529.41 east 2021-02-13T01:00:49Z- AHRS_M2 msg: 1613178049.71 epoch sec; seqNo:17616 2021-02-13T01:00:49Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.16 psi 2021-02-13T01:00:49Z- handleDepth()-Depth_Keller msg: 1613178049712 epoch sec; seqNo:17754; depth 7.54 2021-02-13T01:00:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178049.72 epoch sec; seqNo:17754; 4060614.19 north; 593530.63 east 2021-02-13T01:00:50Z- AHRS_M2 msg: 1613178050.11 epoch sec; seqNo:17617 2021-02-13T01:00:50Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.16 psi 2021-02-13T01:00:50Z- handleDepth()-Depth_Keller msg: 1613178050217 epoch sec; seqNo:17755; depth 7.44 2021-02-13T01:00:50Z- handleDvl() - RDI_Pathfinder msg: 1613178050.23 epoch sec; seqNo:8570 2021-02-13T01:00:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:50Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:00:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178050.24 epoch sec; seqNo:17755; 4060614.57 north; 593530.62 east 2021-02-13T01:00:50Z- LcmTrn::handleMessages() - handling 43 messages took 0.69 ms and 130 calls... 2021-02-13T01:00:50Z- waiting for 1613178048.29, time is 1613178050.24 2021-02-13T01:00:50Z- LcmTrn::updateTrn() equating pose 1613178050.24 and meas times 1613178050.24 2021-02-13T01:00:50Z- LcmTrn::updateTrn() >>>> pose time 1613178050.24, meas time 1613178050.24 <<<< 2021-02-13T01:00:50Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613178050.24, ping # 0. 2021-02-13T01:00:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178050.24 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:00:50Z- TerrainNav::motionUpdate() - currEstimate = (4060615, 593530.62, 7.44) 2021-02-13T01:00:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:00:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:00:50Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:00:50Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:00:50Z- TNavBF::Projecting Measurements 2021-02-13T01:00:50Z- TNavFilter::Measurement from time = 1613178050.24 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T01:00:50Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:00:50Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:00:50Z- TNavBF:: beamIndex[1] = 0 2021-02-13T01:00:50Z- TNavBF:: beamIndex[2] = 0 2021-02-13T01:00:50Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:00:50Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:00:50Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:00:50Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 679.1 EastVariance: 833.0 DepthVariance: 1.6 this->DepthBiasCov: 1.6 2021-02-13T01:00:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:00:50Z- TNavBF::mmse selected from filter -1 2021-02-13T01:00:50Z- variances([0], [2], and psi_berg) = 679.112611 832.988103 0.000000 2021-02-13T01:00:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:00:50Z- LcmTrn::publishEstimates() - MLE : 4060614.07 593547.12 7.44 2021-02-13T01:00:50Z- LcmTrn::publishEstimates() - MMSE : 4060614.57 593530.62 7.44 2021-02-13T01:00:50Z- LcmTrn::publishEstimates() - COVAR: 679.11 832.99 1.64 0.00 2021-02-13T01:00:50Z- AHRS_M2 msg: 1613178050.52 epoch sec; seqNo:17618 2021-02-13T01:00:50Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-13T01:00:50Z- handleDepth()-Depth_Keller msg: 1613178050528 epoch sec; seqNo:17756; depth 7.29 2021-02-13T01:00:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178050.56 epoch sec; seqNo:17756; 4060614.57 north; 593530.62 east 2021-02-13T01:00:50Z- AHRS_M2 msg: 1613178050.92 epoch sec; seqNo:17619 2021-02-13T01:00:50Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-13T01:00:50Z- handleDepth()-Depth_Keller msg: 1613178050945 epoch sec; seqNo:17757; depth 7.20 2021-02-13T01:00:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178050.97 epoch sec; seqNo:17757; 4060614.95 north; 593530.62 east 2021-02-13T01:00:51Z- AHRS_M2 msg: 1613178051.32 epoch sec; seqNo:17620 2021-02-13T01:00:51Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.18 psi 2021-02-13T01:00:51Z- handleDepth()-Depth_Keller msg: 1613178051349 epoch sec; seqNo:17758; depth 7.07 2021-02-13T01:00:51Z- handleDvl() - RDI_Pathfinder msg: 1613178051.36 epoch sec; seqNo:8571 2021-02-13T01:00:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 103.19 , 124.02 , 80.88 2021-02-13T01:00:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.12 , -0.27 2021-02-13T01:00:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178051.40 epoch sec; seqNo:17758; 4060615.33 north; 593530.62 east 2021-02-13T01:00:51Z- AHRS_M2 msg: 1613178051.73 epoch sec; seqNo:17621 2021-02-13T01:00:51Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.19 psi 2021-02-13T01:00:51Z- handleDepth()-Depth_Keller msg: 1613178051734 epoch sec; seqNo:17759; depth 6.93 2021-02-13T01:00:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178051.75 epoch sec; seqNo:17759; 4060615.34 north; 593531.84 east 2021-02-13T01:00:52Z- AHRS_M2 msg: 1613178052.13 epoch sec; seqNo:17622 2021-02-13T01:00:52Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.19 psi 2021-02-13T01:00:52Z- handleDepth()-Depth_Keller msg: 1613178052142 epoch sec; seqNo:17760; depth 6.83 2021-02-13T01:00:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178052.16 epoch sec; seqNo:17760; 4060615.72 north; 593531.83 east 2021-02-13T01:00:52Z- AHRS_M2 msg: 1613178052.54 epoch sec; seqNo:17623 2021-02-13T01:00:52Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-13T01:00:52Z- handleDepth()-Depth_Keller msg: 1613178052542 epoch sec; seqNo:17761; depth 6.71 2021-02-13T01:00:52Z- handleDvl() - RDI_Pathfinder msg: 1613178052.55 epoch sec; seqNo:8572 2021-02-13T01:00:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:52Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:00:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178052.59 epoch sec; seqNo:17761; 4060616.10 north; 593531.83 east 2021-02-13T01:00:52Z- AHRS_M2 msg: 1613178052.94 epoch sec; seqNo:17624 2021-02-13T01:00:52Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-13T01:00:52Z- handleDepth()-Depth_Keller msg: 1613178052945 epoch sec; seqNo:17762; depth 6.58 2021-02-13T01:00:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178052.96 epoch sec; seqNo:17762; 4060616.10 north; 593531.83 east 2021-02-13T01:00:53Z- AHRS_M2 msg: 1613178053.34 epoch sec; seqNo:17625 2021-02-13T01:00:53Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.17 psi 2021-02-13T01:00:53Z- handleDepth()-Depth_Keller msg: 1613178053435 epoch sec; seqNo:17763; depth 6.45 2021-02-13T01:00:53Z- handleDvl() - RDI_Pathfinder msg: 1613178053.45 epoch sec; seqNo:8573 2021-02-13T01:00:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 106.17 , 119.56 , 82.37 2021-02-13T01:00:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , 0.24 2021-02-13T01:00:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178053.47 epoch sec; seqNo:17763; 4060616.48 north; 593531.82 east 2021-02-13T01:00:53Z- AHRS_M2 msg: 1613178053.75 epoch sec; seqNo:17626 2021-02-13T01:00:53Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.17 psi 2021-02-13T01:00:53Z- handleDepth()-Depth_Keller msg: 1613178053760 epoch sec; seqNo:17764; depth 6.29 2021-02-13T01:00:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:00:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:00:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:00:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:00:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:00:53Z- handleDvl() - RDI_Pathfinder msg: 1613178053.78 epoch sec; seqNo:8574 2021-02-13T01:00:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , 0.24 2021-02-13T01:00:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178053.81 epoch sec; seqNo:17764; 4060616.86 north; 593531.82 east 2021-02-13T01:00:54Z- AHRS_M2 msg: 1613178054.15 epoch sec; seqNo:17627 2021-02-13T01:00:54Z- AHRS_M2 msg: 0.58 phi; 0.30 theta; -0.18 psi 2021-02-13T01:00:54Z- handleDepth()-Depth_Keller msg: 1613178054186 epoch sec; seqNo:17765; depth 6.17 2021-02-13T01:00:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178054.19 epoch sec; seqNo:17765; 4060617.24 north; 593531.82 east 2021-02-13T01:00:54Z- AHRS_M2 msg: 1613178054.56 epoch sec; seqNo:17628 2021-02-13T01:00:54Z- AHRS_M2 msg: 0.59 phi; 0.27 theta; -0.19 psi 2021-02-13T01:00:54Z- handleDepth()-Depth_Keller msg: 1613178054581 epoch sec; seqNo:17766; depth 6.05 2021-02-13T01:00:54Z- handleDvl() - RDI_Pathfinder msg: 1613178054.59 epoch sec; seqNo:8575 2021-02-13T01:00:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 105.42 , 120.30 , 83.11 2021-02-13T01:00:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.24 , 0.19 2021-02-13T01:00:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178054.61 epoch sec; seqNo:17766; 4060617.25 north; 593533.04 east 2021-02-13T01:00:54Z- AHRS_M2 msg: 1613178054.96 epoch sec; seqNo:17629 2021-02-13T01:00:54Z- AHRS_M2 msg: 0.59 phi; 0.23 theta; -0.20 psi 2021-02-13T01:00:54Z- handleDepth()-Depth_Keller msg: 1613178054970 epoch sec; seqNo:17767; depth 5.91 2021-02-13T01:00:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:00:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:00:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:00:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:00:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:00:54Z- handleDvl() - RDI_Pathfinder msg: 1613178054.97 epoch sec; seqNo:8576 2021-02-13T01:00:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.24 , 0.19 2021-02-13T01:00:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178054.98 epoch sec; seqNo:17767; 4060617.63 north; 593533.03 east 2021-02-13T01:00:55Z- AHRS_M2 msg: 1613178055.36 epoch sec; seqNo:17630 2021-02-13T01:00:55Z- AHRS_M2 msg: 0.60 phi; 0.19 theta; -0.21 psi 2021-02-13T01:00:55Z- handleDepth()-Depth_Keller msg: 1613178055368 epoch sec; seqNo:17768; depth 5.79 2021-02-13T01:00:55Z- handleDvl() - RDI_Pathfinder msg: 1613178055.38 epoch sec; seqNo:8577 2021-02-13T01:00:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.42 , 109.89 , 114.35 2021-02-13T01:00:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.52 , -0.22 , -0.23 2021-02-13T01:00:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178055.40 epoch sec; seqNo:17768; 4060617.63 north; 593533.03 east 2021-02-13T01:00:55Z- LcmTrn::handleMessages() - handling 46 messages took 0.89 ms and 129 calls... 2021-02-13T01:00:55Z- waiting for 1613178053.24, time is 1613178055.40 2021-02-13T01:00:55Z- LcmTrn::updateTrn() equating pose 1613178055.40 and meas times 1613178055.40 2021-02-13T01:00:55Z- LcmTrn::updateTrn() >>>> pose time 1613178055.40, meas time 1613178055.40 <<<< 2021-02-13T01:00:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178055.40 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:00:55Z- TerrainNav::motionUpdate() - currEstimate = (4060618, 593533.03, 5.79) 2021-02-13T01:00:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:00:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:00:55Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178055.40 sec.; No relevant navigation data available 2021-02-13T01:00:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:00:55Z- TNavBF::mmse selected from filter -1 2021-02-13T01:00:55Z- variances([0], [2], and psi_berg) = 679.069712 832.988085 0.000000 2021-02-13T01:00:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:00:55Z- LcmTrn::publishEstimates() - MLE : 4060617.13 593549.53 5.79 2021-02-13T01:00:55Z- LcmTrn::publishEstimates() - MMSE : 4060617.63 593533.03 5.79 2021-02-13T01:00:55Z- LcmTrn::publishEstimates() - COVAR: 679.07 832.99 1.84 0.00 2021-02-13T01:00:55Z- AHRS_M2 msg: 1613178055.77 epoch sec; seqNo:17631 2021-02-13T01:00:55Z- AHRS_M2 msg: 0.62 phi; 0.15 theta; -0.22 psi 2021-02-13T01:00:55Z- handleDepth()-Depth_Keller msg: 1613178055772 epoch sec; seqNo:17769; depth 5.66 2021-02-13T01:00:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178055.78 epoch sec; seqNo:17769; 4060618.01 north; 593533.03 east 2021-02-13T01:00:56Z- AHRS_M2 msg: 1613178056.17 epoch sec; seqNo:17632 2021-02-13T01:00:56Z- AHRS_M2 msg: 0.64 phi; 0.10 theta; -0.23 psi 2021-02-13T01:00:56Z- handleDepth()-Depth_Keller msg: 1613178056178 epoch sec; seqNo:17770; depth 5.57 2021-02-13T01:00:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:00:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:00:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:00:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:00:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:00:56Z- handleDvl() - RDI_Pathfinder msg: 1613178056.18 epoch sec; seqNo:8578 2021-02-13T01:00:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.52 , -0.22 , -0.23 2021-02-13T01:00:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178056.19 epoch sec; seqNo:17770; 4060618.39 north; 593533.02 east 2021-02-13T01:00:56Z- AHRS_M2 msg: 1613178056.58 epoch sec; seqNo:17633 2021-02-13T01:00:56Z- AHRS_M2 msg: 0.66 phi; 0.06 theta; -0.23 psi 2021-02-13T01:00:56Z- handleDepth()-Depth_Keller msg: 1613178056580 epoch sec; seqNo:17771; depth 5.48 2021-02-13T01:00:56Z- handleDvl() - RDI_Pathfinder msg: 1613178056.60 epoch sec; seqNo:8579 2021-02-13T01:00:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 106.17 , 103.19 , 97.24 2021-02-13T01:00:56Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , 0.00 , -0.06 2021-02-13T01:00:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178056.62 epoch sec; seqNo:17771; 4060618.39 north; 593533.02 east 2021-02-13T01:00:56Z- AHRS_M2 msg: 1613178056.98 epoch sec; seqNo:17634 2021-02-13T01:00:56Z- AHRS_M2 msg: 0.67 phi; 0.01 theta; -0.23 psi 2021-02-13T01:00:56Z- handleDepth()-Depth_Keller msg: 1613178056988 epoch sec; seqNo:17772; depth 5.42 2021-02-13T01:00:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178057.00 epoch sec; seqNo:17772; 4060618.76 north; 593533.02 east 2021-02-13T01:00:57Z- handleDepth()-Depth_Keller msg: 1613178057409 epoch sec; seqNo:17773; depth 5.37 2021-02-13T01:00:57Z- handleDvl() - RDI_Pathfinder msg: 1613178057.42 epoch sec; seqNo:8580 2021-02-13T01:00:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.53 , 108.33 , 97.92 , 97.92 2021-02-13T01:00:57Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.00 , 0.02 2021-02-13T01:00:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178057.43 epoch sec; seqNo:17773; 4060619.14 north; 593533.01 east 2021-02-13T01:00:57Z- AHRS_M2 msg: 1613178057.79 epoch sec; seqNo:17635 2021-02-13T01:00:57Z- AHRS_M2 msg: 0.70 phi; -0.07 theta; -0.17 psi 2021-02-13T01:00:57Z- handleDepth()-Depth_Keller msg: 1613178057799 epoch sec; seqNo:17774; depth 5.38 2021-02-13T01:00:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178057.81 epoch sec; seqNo:17774; 4060619.54 north; 593534.23 east 2021-02-13T01:00:58Z- AHRS_M2 msg: 1613178058.21 epoch sec; seqNo:17636 2021-02-13T01:00:58Z- AHRS_M2 msg: 0.73 phi; -0.14 theta; -0.13 psi 2021-02-13T01:00:58Z- handleDepth()-Depth_Keller msg: 1613178058210 epoch sec; seqNo:17775; depth 5.37 2021-02-13T01:00:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178058.22 epoch sec; seqNo:17775; 4060619.91 north; 593534.23 east 2021-02-13T01:00:58Z- AHRS_M2 msg: 1613178058.60 epoch sec; seqNo:17637 2021-02-13T01:00:58Z- AHRS_M2 msg: 0.74 phi; -0.18 theta; -0.14 psi 2021-02-13T01:00:58Z- handleDepth()-Depth_Keller msg: 1613178058604 epoch sec; seqNo:17776; depth 5.38 2021-02-13T01:00:58Z- handleDvl() - RDI_Pathfinder msg: 1613178058.62 epoch sec; seqNo:8581 2021-02-13T01:00:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 101.37 , 97.31 , 98.66 2021-02-13T01:00:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.10 , -0.07 2021-02-13T01:00:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178058.64 epoch sec; seqNo:17776; 4060620.29 north; 593534.22 east 2021-02-13T01:00:59Z- AHRS_M2 msg: 1613178059.00 epoch sec; seqNo:17638 2021-02-13T01:00:59Z- AHRS_M2 msg: 0.74 phi; -0.21 theta; -0.16 psi 2021-02-13T01:00:59Z- handleDepth()-Depth_Keller msg: 1613178059008 epoch sec; seqNo:17777; depth 5.38 2021-02-13T01:00:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178059.02 epoch sec; seqNo:17777; 4060620.29 north; 593534.22 east 2021-02-13T01:00:59Z- AHRS_M2 msg: 1613178059.40 epoch sec; seqNo:17639 2021-02-13T01:00:59Z- AHRS_M2 msg: 0.74 phi; -0.24 theta; -0.18 psi 2021-02-13T01:00:59Z- handleDepth()-Depth_Keller msg: 1613178059410 epoch sec; seqNo:17778; depth 5.41 2021-02-13T01:00:59Z- handleDvl() - RDI_Pathfinder msg: 1613178059.42 epoch sec; seqNo:8582 2021-02-13T01:00:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.43 , 86.56 , 104.14 2021-02-13T01:00:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.04 , -0.19 2021-02-13T01:00:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178059.43 epoch sec; seqNo:17778; 4060620.69 north; 593535.44 east 2021-02-13T01:00:59Z- AHRS_M2 msg: 1613178059.81 epoch sec; seqNo:17640 2021-02-13T01:00:59Z- AHRS_M2 msg: 0.74 phi; -0.26 theta; -0.20 psi 2021-02-13T01:00:59Z- handleDepth()-Depth_Keller msg: 1613178059817 epoch sec; seqNo:17779; depth 5.44 2021-02-13T01:00:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:00:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:00:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:00:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:00:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:00:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:00:59Z- handleDvl() - RDI_Pathfinder msg: 1613178059.82 epoch sec; seqNo:8583 2021-02-13T01:00:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:00:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.04 , -0.19 2021-02-13T01:00:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178059.84 epoch sec; seqNo:17779; 4060620.69 north; 593535.44 east 2021-02-13T01:01:00Z- AHRS_M2 msg: 1613178060.21 epoch sec; seqNo:17641 2021-02-13T01:01:00Z- AHRS_M2 msg: 0.73 phi; -0.28 theta; -0.22 psi 2021-02-13T01:01:00Z- handleDepth()-Depth_Keller msg: 1613178060217 epoch sec; seqNo:17780; depth 5.45 2021-02-13T01:01:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178060.23 epoch sec; seqNo:17780; 4060621.06 north; 593535.44 east 2021-02-13T01:01:00Z- LcmTrn::handleMessages() - handling 41 messages took 0.56 ms and 130 calls... 2021-02-13T01:01:00Z- waiting for 1613178058.40, time is 1613178060.23 2021-02-13T01:01:00Z- LcmTrn::updateTrn() >>>> pose time 1613178060.23, meas time 1613178059.82 <<<< 2021-02-13T01:01:00Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613178059.82, ping # 0. 2021-02-13T01:01:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178059.82 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:01:00Z- TerrainNav::motionUpdate() - currEstimate = (4060621, 593535.44, 5.45) 2021-02-13T01:01:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:01:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:01:00Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:01:00Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:01:00Z- TNavBF::Projecting Measurements 2021-02-13T01:01:00Z- TNavFilter::Measurement from time = 1613178059.82 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T01:01:00Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:01:00Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:01:00Z- TNavBF:: beamIndex[1] = 0 2021-02-13T01:01:00Z- TNavBF:: beamIndex[2] = 0 2021-02-13T01:01:00Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:01:00Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:01:00Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:01:00Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 679.0 EastVariance: 833.0 DepthVariance: 2.0 this->DepthBiasCov: 2.0 2021-02-13T01:01:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:01:00Z- TNavBF::mmse selected from filter -1 2021-02-13T01:01:00Z- variances([0], [2], and psi_berg) = 679.014196 832.988062 0.000000 2021-02-13T01:01:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:01:00Z- LcmTrn::publishEstimates() - MLE : 4060620.56 593551.94 5.45 2021-02-13T01:01:00Z- LcmTrn::publishEstimates() - MMSE : 4060621.06 593535.44 5.45 2021-02-13T01:01:00Z- LcmTrn::publishEstimates() - COVAR: 679.01 832.99 2.05 0.00 2021-02-13T01:01:00Z- AHRS_M2 msg: 1613178060.62 epoch sec; seqNo:17642 2021-02-13T01:01:00Z- AHRS_M2 msg: 0.71 phi; -0.30 theta; -0.24 psi 2021-02-13T01:01:00Z- handleDepth()-Depth_Keller msg: 1613178060620 epoch sec; seqNo:17781; depth 5.50 2021-02-13T01:01:00Z- handleDvl() - RDI_Pathfinder msg: 1613178060.63 epoch sec; seqNo:8584 2021-02-13T01:01:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.78 , 105.36 , 83.72 , 106.71 2021-02-13T01:01:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.09 , -0.04 2021-02-13T01:01:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178060.64 epoch sec; seqNo:17781; 4060621.44 north; 593535.43 east 2021-02-13T01:01:01Z- AHRS_M2 msg: 1613178061.03 epoch sec; seqNo:17643 2021-02-13T01:01:01Z- AHRS_M2 msg: 0.70 phi; -0.31 theta; -0.26 psi 2021-02-13T01:01:01Z- handleDepth()-Depth_Keller msg: 1613178061030 epoch sec; seqNo:17782; depth 5.57 2021-02-13T01:01:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:01Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:01Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:01Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:01Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:01Z- handleDvl() - RDI_Pathfinder msg: 1613178061.03 epoch sec; seqNo:8585 2021-02-13T01:01:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.09 , -0.04 2021-02-13T01:01:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178061.04 epoch sec; seqNo:17782; 4060621.44 north; 593535.43 east 2021-02-13T01:01:01Z- AHRS_M2 msg: 1613178061.42 epoch sec; seqNo:17644 2021-02-13T01:01:01Z- AHRS_M2 msg: 0.68 phi; -0.32 theta; -0.27 psi 2021-02-13T01:01:01Z- handleDepth()-Depth_Keller msg: 1613178061429 epoch sec; seqNo:17783; depth 5.64 2021-02-13T01:01:01Z- handleDvl() - RDI_Pathfinder msg: 1613178061.45 epoch sec; seqNo:8586 2021-02-13T01:01:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 116.31 , 83.85 , 120.37 2021-02-13T01:01:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.01 , 0.17 2021-02-13T01:01:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178061.46 epoch sec; seqNo:17783; 4060621.82 north; 593535.43 east 2021-02-13T01:01:01Z- AHRS_M2 msg: 1613178061.83 epoch sec; seqNo:17645 2021-02-13T01:01:01Z- AHRS_M2 msg: 0.68 phi; -0.33 theta; -0.28 psi 2021-02-13T01:01:01Z- handleDepth()-Depth_Keller msg: 1613178061837 epoch sec; seqNo:17784; depth 5.72 2021-02-13T01:01:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178061.85 epoch sec; seqNo:17784; 4060621.82 north; 593535.43 east 2021-02-13T01:01:02Z- AHRS_M2 msg: 1613178062.26 epoch sec; seqNo:17646 2021-02-13T01:01:02Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.28 psi 2021-02-13T01:01:02Z- handleDepth()-Depth_Keller msg: 1613178062264 epoch sec; seqNo:17785; depth 5.81 2021-02-13T01:01:02Z- handleDvl() - RDI_Pathfinder msg: 1613178062.29 epoch sec; seqNo:8587 2021-02-13T01:01:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 117.32 , 83.11 , 124.76 2021-02-13T01:01:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.10 , 0.10 2021-02-13T01:01:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178062.30 epoch sec; seqNo:17785; 4060622.21 north; 593536.64 east 2021-02-13T01:01:02Z- AHRS_M2 msg: 1613178062.64 epoch sec; seqNo:17647 2021-02-13T01:01:02Z- AHRS_M2 msg: 0.67 phi; -0.33 theta; -0.28 psi 2021-02-13T01:01:02Z- handleDepth()-Depth_Keller msg: 1613178062647 epoch sec; seqNo:17786; depth 5.92 2021-02-13T01:01:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178062.67 epoch sec; seqNo:17786; 4060622.21 north; 593536.64 east 2021-02-13T01:01:03Z- AHRS_M2 msg: 1613178063.04 epoch sec; seqNo:17648 2021-02-13T01:01:03Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.28 psi 2021-02-13T01:01:03Z- handleDepth()-Depth_Keller msg: 1613178063045 epoch sec; seqNo:17787; depth 6.02 2021-02-13T01:01:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178063.05 epoch sec; seqNo:17787; 4060622.59 north; 593536.64 east 2021-02-13T01:01:03Z- AHRS_M2 msg: 1613178063.46 epoch sec; seqNo:17649 2021-02-13T01:01:03Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.28 psi 2021-02-13T01:01:03Z- handleDepth()-Depth_Keller msg: 1613178063530 epoch sec; seqNo:17788; depth 6.13 2021-02-13T01:01:03Z- handleDvl() - RDI_Pathfinder msg: 1613178063.54 epoch sec; seqNo:8588 2021-02-13T01:01:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 118.68 , 84.46 , 123.14 2021-02-13T01:01:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.07 , -0.04 2021-02-13T01:01:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178063.55 epoch sec; seqNo:17788; 4060622.97 north; 593536.63 east 2021-02-13T01:01:03Z- AHRS_M2 msg: 1613178063.85 epoch sec; seqNo:17650 2021-02-13T01:01:03Z- AHRS_M2 msg: 0.66 phi; -0.34 theta; -0.27 psi 2021-02-13T01:01:03Z- handleDepth()-Depth_Keller msg: 1613178063860 epoch sec; seqNo:17789; depth 6.26 2021-02-13T01:01:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178063.87 epoch sec; seqNo:17789; 4060623.35 north; 593536.63 east 2021-02-13T01:01:04Z- AHRS_M2 msg: 1613178064.26 epoch sec; seqNo:17651 2021-02-13T01:01:04Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.27 psi 2021-02-13T01:01:04Z- handleDepth()-Depth_Keller msg: 1613178064285 epoch sec; seqNo:17790; depth 6.36 2021-02-13T01:01:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178064.30 epoch sec; seqNo:17790; 4060623.36 north; 593537.85 east 2021-02-13T01:01:04Z- AHRS_M2 msg: 1613178064.65 epoch sec; seqNo:17652 2021-02-13T01:01:04Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.26 psi 2021-02-13T01:01:04Z- handleDepth()-Depth_Keller msg: 1613178064681 epoch sec; seqNo:17791; depth 6.49 2021-02-13T01:01:04Z- handleDvl() - RDI_Pathfinder msg: 1613178064.69 epoch sec; seqNo:8589 2021-02-13T01:01:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.96 , 119.69 , 82.50 , 124.15 2021-02-13T01:01:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , -0.01 2021-02-13T01:01:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178064.70 epoch sec; seqNo:17791; 4060623.74 north; 593537.85 east 2021-02-13T01:01:05Z- AHRS_M2 msg: 1613178065.06 epoch sec; seqNo:17653 2021-02-13T01:01:05Z- AHRS_M2 msg: 0.64 phi; -0.34 theta; -0.25 psi 2021-02-13T01:01:05Z- handleDepth()-Depth_Keller msg: 1613178065065 epoch sec; seqNo:17792; depth 6.62 2021-02-13T01:01:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178065.08 epoch sec; seqNo:17792; 4060624.12 north; 593537.84 east 2021-02-13T01:01:05Z- AHRS_M2 msg: 1613178065.46 epoch sec; seqNo:17654 2021-02-13T01:01:05Z- AHRS_M2 msg: 0.63 phi; -0.34 theta; -0.25 psi 2021-02-13T01:01:05Z- handleDepth()-Depth_Keller msg: 1613178065469 epoch sec; seqNo:17793; depth 6.71 2021-02-13T01:01:05Z- handleDvl() - RDI_Pathfinder msg: 1613178065.48 epoch sec; seqNo:8590 2021-02-13T01:01:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 114.21 , 82.97 , 124.63 2021-02-13T01:01:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.16 2021-02-13T01:01:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178065.49 epoch sec; seqNo:17793; 4060624.12 north; 593537.84 east 2021-02-13T01:01:05Z- LcmTrn::handleMessages() - handling 46 messages took 0.74 ms and 132 calls... 2021-02-13T01:01:05Z- waiting for 1613178063.23, time is 1613178065.49 2021-02-13T01:01:05Z- LcmTrn::updateTrn() equating pose 1613178065.49 and meas times 1613178065.49 2021-02-13T01:01:05Z- LcmTrn::updateTrn() >>>> pose time 1613178065.49, meas time 1613178065.49 <<<< 2021-02-13T01:01:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178065.49 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:01:05Z- TerrainNav::motionUpdate() - currEstimate = (4060624, 593537.84, 6.71) 2021-02-13T01:01:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:01:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:01:05Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178065.49 sec.; No relevant navigation data available 2021-02-13T01:01:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:01:05Z- TNavBF::mmse selected from filter -1 2021-02-13T01:01:05Z- variances([0], [2], and psi_berg) = 678.971341 832.988044 0.000000 2021-02-13T01:01:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:01:05Z- LcmTrn::publishEstimates() - MLE : 4060623.62 593554.34 6.71 2021-02-13T01:01:05Z- LcmTrn::publishEstimates() - MMSE : 4060624.12 593537.84 6.71 2021-02-13T01:01:05Z- LcmTrn::publishEstimates() - COVAR: 678.97 832.99 2.25 0.00 2021-02-13T01:01:05Z- AHRS_M2 msg: 1613178065.87 epoch sec; seqNo:17655 2021-02-13T01:01:05Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.24 psi 2021-02-13T01:01:05Z- handleDepth()-Depth_Keller msg: 1613178065875 epoch sec; seqNo:17794; depth 6.84 2021-02-13T01:01:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:05Z- handleDvl() - RDI_Pathfinder msg: 1613178065.88 epoch sec; seqNo:8591 2021-02-13T01:01:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.16 2021-02-13T01:01:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178065.89 epoch sec; seqNo:17794; 4060624.50 north; 593537.84 east 2021-02-13T01:01:06Z- AHRS_M2 msg: 1613178066.29 epoch sec; seqNo:17656 2021-02-13T01:01:06Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.23 psi 2021-02-13T01:01:06Z- handleDepth()-Depth_Keller msg: 1613178066299 epoch sec; seqNo:17795; depth 6.96 2021-02-13T01:01:06Z- handleDvl() - RDI_Pathfinder msg: 1613178066.31 epoch sec; seqNo:8592 2021-02-13T01:01:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.51 , 113.88 , 0.00 , 124.29 2021-02-13T01:01:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.18 , 0.25 2021-02-13T01:01:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178066.32 epoch sec; seqNo:17795; 4060624.50 north; 593537.84 east 2021-02-13T01:01:06Z- AHRS_M2 msg: 1613178066.68 epoch sec; seqNo:17657 2021-02-13T01:01:06Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.21 psi 2021-02-13T01:01:06Z- handleDepth()-Depth_Keller msg: 1613178066753 epoch sec; seqNo:17796; depth 7.09 2021-02-13T01:01:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178066.77 epoch sec; seqNo:17796; 4060624.89 north; 593539.06 east 2021-02-13T01:01:07Z- AHRS_M2 msg: 1613178067.09 epoch sec; seqNo:17658 2021-02-13T01:01:07Z- AHRS_M2 msg: 0.61 phi; -0.36 theta; -0.20 psi 2021-02-13T01:01:07Z- handleDepth()-Depth_Keller msg: 1613178067100 epoch sec; seqNo:17797; depth 7.23 2021-02-13T01:01:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:07Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:07Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:07Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:07Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:07Z- handleDvl() - RDI_Pathfinder msg: 1613178067.10 epoch sec; seqNo:8593 2021-02-13T01:01:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.18 , 0.25 2021-02-13T01:01:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178067.11 epoch sec; seqNo:17797; 4060624.89 north; 593539.06 east 2021-02-13T01:01:07Z- AHRS_M2 msg: 1613178067.49 epoch sec; seqNo:17659 2021-02-13T01:01:07Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.19 psi 2021-02-13T01:01:07Z- handleDepth()-Depth_Keller msg: 1613178067517 epoch sec; seqNo:17798; depth 7.32 2021-02-13T01:01:07Z- handleDvl() - RDI_Pathfinder msg: 1613178067.53 epoch sec; seqNo:8594 2021-02-13T01:01:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 109.14 , 80.88 , 124.02 2021-02-13T01:01:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.36 , 0.08 2021-02-13T01:01:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178067.54 epoch sec; seqNo:17798; 4060625.27 north; 593539.05 east 2021-02-13T01:01:07Z- AHRS_M2 msg: 1613178067.90 epoch sec; seqNo:17660 2021-02-13T01:01:07Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.20 psi 2021-02-13T01:01:07Z- handleDepth()-Depth_Keller msg: 1613178067926 epoch sec; seqNo:17799; depth 7.45 2021-02-13T01:01:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178067.94 epoch sec; seqNo:17799; 4060625.27 north; 593539.05 east 2021-02-13T01:01:08Z- AHRS_M2 msg: 1613178068.31 epoch sec; seqNo:17661 2021-02-13T01:01:08Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.21 psi 2021-02-13T01:01:08Z- handleDepth()-Depth_Keller msg: 1613178068313 epoch sec; seqNo:17800; depth 7.56 2021-02-13T01:01:08Z- handleDvl() - RDI_Pathfinder msg: 1613178068.33 epoch sec; seqNo:8595 2021-02-13T01:01:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 108.26 , 0.00 , 126.11 2021-02-13T01:01:08Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.29 , -0.19 2021-02-13T01:01:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178068.35 epoch sec; seqNo:17800; 4060625.66 north; 593540.27 east 2021-02-13T01:01:08Z- AHRS_M2 msg: 1613178068.70 epoch sec; seqNo:17662 2021-02-13T01:01:08Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.23 psi 2021-02-13T01:01:08Z- handleDepth()-Depth_Keller msg: 1613178068711 epoch sec; seqNo:17801; depth 7.66 2021-02-13T01:01:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178068.72 epoch sec; seqNo:17801; 4060626.04 north; 593540.26 east 2021-02-13T01:01:09Z- AHRS_M2 msg: 1613178069.11 epoch sec; seqNo:17663 2021-02-13T01:01:09Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.23 psi 2021-02-13T01:01:09Z- handleDepth()-Depth_Keller msg: 1613178069117 epoch sec; seqNo:17802; depth 7.79 2021-02-13T01:01:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178069.14 epoch sec; seqNo:17802; 4060626.42 north; 593540.26 east 2021-02-13T01:01:09Z- AHRS_M2 msg: 1613178069.51 epoch sec; seqNo:17664 2021-02-13T01:01:09Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.24 psi 2021-02-13T01:01:09Z- handleDepth()-Depth_Keller msg: 1613178069517 epoch sec; seqNo:17803; depth 7.91 2021-02-13T01:01:09Z- handleDvl() - RDI_Pathfinder msg: 1613178069.53 epoch sec; seqNo:8596 2021-02-13T01:01:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.83 , 112.12 , 0.00 , 121.04 2021-02-13T01:01:09Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , 0.04 , -0.24 2021-02-13T01:01:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178069.54 epoch sec; seqNo:17803; 4060626.80 north; 593540.26 east 2021-02-13T01:01:09Z- AHRS_M2 msg: 1613178069.92 epoch sec; seqNo:17665 2021-02-13T01:01:09Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.24 psi 2021-02-13T01:01:09Z- handleDepth()-Depth_Keller msg: 1613178069922 epoch sec; seqNo:17804; depth 8.03 2021-02-13T01:01:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178069.93 epoch sec; seqNo:17804; 4060627.18 north; 593540.25 east 2021-02-13T01:01:10Z- AHRS_M2 msg: 1613178070.32 epoch sec; seqNo:17666 2021-02-13T01:01:10Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.24 psi 2021-02-13T01:01:10Z- handleDepth()-Depth_Keller msg: 1613178070330 epoch sec; seqNo:17805; depth 8.15 2021-02-13T01:01:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178070.34 epoch sec; seqNo:17805; 4060627.57 north; 593541.47 east 2021-02-13T01:01:10Z- LcmTrn::handleMessages() - handling 42 messages took 0.58 ms and 131 calls... 2021-02-13T01:01:10Z- waiting for 1613178068.49, time is 1613178070.34 2021-02-13T01:01:10Z- LcmTrn::updateTrn() >>>> pose time 1613178070.34, meas time 1613178069.53 <<<< 2021-02-13T01:01:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178069.53 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:01:10Z- TerrainNav::motionUpdate() - currEstimate = (4060628, 593541.47, 8.15) 2021-02-13T01:01:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:01:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:01:10Z- TNavBF::Projecting Measurements 2021-02-13T01:01:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:01:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:01:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:01:10Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:01:10Z- TNavFilter::3 good beams. 2021-02-13T01:01:10Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:01:10Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:01:10Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:01:10Z- TNavBF:: beamIndex[2] = 3 2021-02-13T01:01:10Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:01:10Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:01:10Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:01:10Z- TNavBF::mmse selected from filter -1 2021-02-13T01:01:10Z- TerrainMapOctree: (vehN, vehE ) = (4060626.99, 593540.86). 2021-02-13T01:01:10Z- TerrainMapOctree: Min Distance = 943.93. 2021-02-13T01:01:10Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:01:10Z- There is no specific good beam that all particles have in common. 2021-02-13T01:01:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:01:10Z- Old Depth Bias: 0.00 DepthBiasCov: 2.47 K: 0.152 ExpectedMeasDiff: nan currMeas.covariance: 12.06 2021-02-13T01:01:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:01:10Z- Old Depth Bias: 0.00 DepthBiasCov: 2.09 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 20.11 2021-02-13T01:01:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:01:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.91 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 23.44 2021-02-13T01:01:10Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:01:10Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:01:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:01:10Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:01:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:01:10Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:01:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:01:10Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:01:10Z- Filter 0 No beams used in subcloud update 2021-02-13T01:01:10Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:01:10Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:01:10Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 678.9 EastVariance: 833.0 DepthVariance: 1.8 this->DepthBiasCov: 1.8 2021-02-13T01:01:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178069.53 sec. 2021-02-13T01:01:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:01:10Z- TNavBF::mmse selected from filter -1 2021-02-13T01:01:10Z- variances([0], [2], and psi_berg) = 678.907169 832.988017 0.000000 2021-02-13T01:01:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:01:10Z- LcmTrn::publishEstimates() - MLE : 4060627.07 593557.97 8.15 2021-02-13T01:01:10Z- LcmTrn::publishEstimates() - MMSE : 4060627.57 593541.47 8.15 2021-02-13T01:01:10Z- LcmTrn::publishEstimates() - COVAR: 678.91 832.99 1.82 0.00 2021-02-13T01:01:10Z- AHRS_M2 msg: 1613178070.72 epoch sec; seqNo:17667 2021-02-13T01:01:10Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.23 psi 2021-02-13T01:01:10Z- handleDepth()-Depth_Keller msg: 1613178070728 epoch sec; seqNo:17806; depth 8.27 2021-02-13T01:01:10Z- handleDvl() - RDI_Pathfinder msg: 1613178070.75 epoch sec; seqNo:8597 2021-02-13T01:01:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.61 , 110.90 , 81.15 , 119.83 2021-02-13T01:01:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.09 , -0.15 2021-02-13T01:01:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178070.76 epoch sec; seqNo:17806; 4060627.95 north; 593541.46 east 2021-02-13T01:01:11Z- AHRS_M2 msg: 1613178071.13 epoch sec; seqNo:17668 2021-02-13T01:01:11Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.22 psi 2021-02-13T01:01:11Z- handleDepth()-Depth_Keller msg: 1613178071137 epoch sec; seqNo:17807; depth 8.40 2021-02-13T01:01:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178071.15 epoch sec; seqNo:17807; 4060628.33 north; 593541.46 east 2021-02-13T01:01:11Z- AHRS_M2 msg: 1613178071.53 epoch sec; seqNo:17669 2021-02-13T01:01:11Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.21 psi 2021-02-13T01:01:11Z- handleDepth()-Depth_Keller msg: 1613178071541 epoch sec; seqNo:17808; depth 8.51 2021-02-13T01:01:11Z- handleDvl() - RDI_Pathfinder msg: 1613178071.55 epoch sec; seqNo:8598 2021-02-13T01:01:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 109.28 , 78.88 , 118.74 2021-02-13T01:01:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.15 , -0.11 2021-02-13T01:01:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178071.56 epoch sec; seqNo:17808; 4060628.33 north; 593541.46 east 2021-02-13T01:01:11Z- AHRS_M2 msg: 1613178071.94 epoch sec; seqNo:17670 2021-02-13T01:01:11Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.20 psi 2021-02-13T01:01:11Z- handleDepth()-Depth_Keller msg: 1613178071942 epoch sec; seqNo:17809; depth 8.63 2021-02-13T01:01:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:11Z- handleDvl() - RDI_Pathfinder msg: 1613178071.94 epoch sec; seqNo:8599 2021-02-13T01:01:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.15 , -0.11 2021-02-13T01:01:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178071.95 epoch sec; seqNo:17809; 4060628.71 north; 593541.45 east 2021-02-13T01:01:12Z- AHRS_M2 msg: 1613178072.35 epoch sec; seqNo:17671 2021-02-13T01:01:12Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.19 psi 2021-02-13T01:01:12Z- handleDepth()-Depth_Keller msg: 1613178072359 epoch sec; seqNo:17810; depth 8.75 2021-02-13T01:01:12Z- handleDvl() - RDI_Pathfinder msg: 1613178072.39 epoch sec; seqNo:8600 2021-02-13T01:01:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.02 , 106.84 , 80.07 , 115.77 2021-02-13T01:01:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.18 , -0.07 2021-02-13T01:01:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178072.41 epoch sec; seqNo:17810; 4060629.09 north; 593541.45 east 2021-02-13T01:01:12Z- AHRS_M2 msg: 1613178072.74 epoch sec; seqNo:17672 2021-02-13T01:01:12Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.19 psi 2021-02-13T01:01:12Z- handleDepth()-Depth_Keller msg: 1613178072750 epoch sec; seqNo:17811; depth 8.89 2021-02-13T01:01:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178072.76 epoch sec; seqNo:17811; 4060629.47 north; 593541.45 east 2021-02-13T01:01:13Z- AHRS_M2 msg: 1613178073.15 epoch sec; seqNo:17673 2021-02-13T01:01:13Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.19 psi 2021-02-13T01:01:13Z- handleDepth()-Depth_Keller msg: 1613178073158 epoch sec; seqNo:17812; depth 9.00 2021-02-13T01:01:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:13Z- handleDvl() - RDI_Pathfinder msg: 1613178073.16 epoch sec; seqNo:8601 2021-02-13T01:01:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.18 , -0.07 2021-02-13T01:01:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178073.17 epoch sec; seqNo:17812; 4060629.84 north; 593541.44 east 2021-02-13T01:01:13Z- AHRS_M2 msg: 1613178073.55 epoch sec; seqNo:17674 2021-02-13T01:01:13Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.18 psi 2021-02-13T01:01:13Z- handleDepth()-Depth_Keller msg: 1613178073556 epoch sec; seqNo:17813; depth 9.13 2021-02-13T01:01:13Z- handleDvl() - RDI_Pathfinder msg: 1613178073.57 epoch sec; seqNo:8602 2021-02-13T01:01:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 105.42 , 78.07 , 116.24 2021-02-13T01:01:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.16 , 0.05 2021-02-13T01:01:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178073.59 epoch sec; seqNo:17813; 4060630.24 north; 593542.66 east 2021-02-13T01:01:13Z- AHRS_M2 msg: 1613178073.96 epoch sec; seqNo:17675 2021-02-13T01:01:13Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.17 psi 2021-02-13T01:01:13Z- handleDepth()-Depth_Keller msg: 1613178073965 epoch sec; seqNo:17814; depth 9.24 2021-02-13T01:01:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178073.97 epoch sec; seqNo:17814; 4060630.24 north; 593542.66 east 2021-02-13T01:01:14Z- AHRS_M2 msg: 1613178074.38 epoch sec; seqNo:17676 2021-02-13T01:01:14Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.17 psi 2021-02-13T01:01:14Z- handleDepth()-Depth_Keller msg: 1613178074395 epoch sec; seqNo:17815; depth 9.37 2021-02-13T01:01:14Z- handleDvl() - RDI_Pathfinder msg: 1613178074.42 epoch sec; seqNo:8603 2021-02-13T01:01:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 101.37 , 78.38 , 114.89 2021-02-13T01:01:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.03 , 0.14 2021-02-13T01:01:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178074.43 epoch sec; seqNo:17815; 4060630.62 north; 593542.65 east 2021-02-13T01:01:14Z- handleDepth()-Depth_Keller msg: 1613178074785 epoch sec; seqNo:17816; depth 9.52 2021-02-13T01:01:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178074.80 epoch sec; seqNo:17816; 4060630.99 north; 593542.65 east 2021-02-13T01:01:15Z- AHRS_M2 msg: 1613178075.17 epoch sec; seqNo:17677 2021-02-13T01:01:15Z- AHRS_M2 msg: 0.56 phi; -0.33 theta; -0.19 psi 2021-02-13T01:01:15Z- handleDepth()-Depth_Keller msg: 1613178075179 epoch sec; seqNo:17817; depth 9.61 2021-02-13T01:01:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178075.19 epoch sec; seqNo:17817; 4060630.99 north; 593542.65 east 2021-02-13T01:01:15Z- AHRS_M2 msg: 1613178075.57 epoch sec; seqNo:17678 2021-02-13T01:01:15Z- AHRS_M2 msg: 0.55 phi; -0.32 theta; -0.20 psi 2021-02-13T01:01:15Z- handleDepth()-Depth_Keller msg: 1613178075578 epoch sec; seqNo:17818; depth 9.73 2021-02-13T01:01:15Z- handleDvl() - RDI_Pathfinder msg: 1613178075.59 epoch sec; seqNo:8604 2021-02-13T01:01:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 104.21 , 78.51 , 117.73 2021-02-13T01:01:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.12 , 0.12 2021-02-13T01:01:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178075.60 epoch sec; seqNo:17818; 4060631.37 north; 593542.65 east 2021-02-13T01:01:15Z- LcmTrn::handleMessages() - handling 46 messages took 0.69 ms and 133 calls... 2021-02-13T01:01:15Z- waiting for 1613178073.34, time is 1613178075.60 2021-02-13T01:01:15Z- LcmTrn::updateTrn() equating pose 1613178075.60 and meas times 1613178075.60 2021-02-13T01:01:15Z- LcmTrn::updateTrn() >>>> pose time 1613178075.60, meas time 1613178075.60 <<<< 2021-02-13T01:01:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178075.60 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:01:15Z- TerrainNav::motionUpdate() - currEstimate = (4060631, 593542.65, 9.73) 2021-02-13T01:01:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:01:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:01:15Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178075.60 sec.; No relevant navigation data available 2021-02-13T01:01:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:01:15Z- TNavBF::mmse selected from filter -1 2021-02-13T01:01:15Z- variances([0], [2], and psi_berg) = 678.863876 832.987999 0.000000 2021-02-13T01:01:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:01:15Z- LcmTrn::publishEstimates() - MLE : 4060630.87 593559.15 9.73 2021-02-13T01:01:15Z- LcmTrn::publishEstimates() - MMSE : 4060631.37 593542.65 9.73 2021-02-13T01:01:15Z- LcmTrn::publishEstimates() - COVAR: 678.86 832.99 2.02 0.00 2021-02-13T01:01:15Z- AHRS_M2 msg: 1613178075.98 epoch sec; seqNo:17679 2021-02-13T01:01:15Z- AHRS_M2 msg: 0.55 phi; -0.31 theta; -0.21 psi 2021-02-13T01:01:15Z- handleDepth()-Depth_Keller msg: 1613178075982 epoch sec; seqNo:17819; depth 9.83 2021-02-13T01:01:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178075.99 epoch sec; seqNo:17819; 4060631.37 north; 593542.65 east 2021-02-13T01:01:16Z- AHRS_M2 msg: 1613178076.38 epoch sec; seqNo:17680 2021-02-13T01:01:16Z- AHRS_M2 msg: 0.55 phi; -0.31 theta; -0.22 psi 2021-02-13T01:01:16Z- handleDepth()-Depth_Keller msg: 1613178076385 epoch sec; seqNo:17820; depth 9.93 2021-02-13T01:01:16Z- handleDvl() - RDI_Pathfinder msg: 1613178076.41 epoch sec; seqNo:8605 2021-02-13T01:01:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 104.07 , 77.25 , 114.89 2021-02-13T01:01:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.13 , 0.08 2021-02-13T01:01:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178076.42 epoch sec; seqNo:17820; 4060631.77 north; 593543.86 east 2021-02-13T01:01:16Z- AHRS_M2 msg: 1613178076.78 epoch sec; seqNo:17681 2021-02-13T01:01:16Z- AHRS_M2 msg: 0.56 phi; -0.30 theta; -0.22 psi 2021-02-13T01:01:16Z- handleDepth()-Depth_Keller msg: 1613178076789 epoch sec; seqNo:17821; depth 10.06 2021-02-13T01:01:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:16Z- handleDvl() - RDI_Pathfinder msg: 1613178076.79 epoch sec; seqNo:8606 2021-02-13T01:01:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.13 , 0.08 2021-02-13T01:01:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178076.80 epoch sec; seqNo:17821; 4060632.14 north; 593543.86 east 2021-02-13T01:01:17Z- AHRS_M2 msg: 1613178077.19 epoch sec; seqNo:17682 2021-02-13T01:01:17Z- AHRS_M2 msg: 0.57 phi; -0.30 theta; -0.22 psi 2021-02-13T01:01:17Z- handleDepth()-Depth_Keller msg: 1613178077286 epoch sec; seqNo:17822; depth 10.16 2021-02-13T01:01:17Z- handleDvl() - RDI_Pathfinder msg: 1613178077.30 epoch sec; seqNo:8607 2021-02-13T01:01:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 106.91 , 79.86 , 116.38 2021-02-13T01:01:17Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.07 , 0.11 2021-02-13T01:01:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178077.31 epoch sec; seqNo:17822; 4060632.52 north; 593543.85 east 2021-02-13T01:01:17Z- AHRS_M2 msg: 1613178077.59 epoch sec; seqNo:17683 2021-02-13T01:01:17Z- AHRS_M2 msg: 0.57 phi; -0.30 theta; -0.21 psi 2021-02-13T01:01:17Z- handleDepth()-Depth_Keller msg: 1613178077605 epoch sec; seqNo:17823; depth 10.32 2021-02-13T01:01:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178077.62 epoch sec; seqNo:17823; 4060632.52 north; 593543.85 east 2021-02-13T01:01:17Z- AHRS_M2 msg: 1613178078.00 epoch sec; seqNo:17684 2021-02-13T01:01:17Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.19 psi 2021-02-13T01:01:18Z- handleDepth()-Depth_Keller msg: 1613178078021 epoch sec; seqNo:17824; depth 10.40 2021-02-13T01:01:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:18Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:18Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:18Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:18Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:18Z- handleDvl() - RDI_Pathfinder msg: 1613178078.02 epoch sec; seqNo:8608 2021-02-13T01:01:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:18Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.07 , 0.11 2021-02-13T01:01:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178078.03 epoch sec; seqNo:17824; 4060632.90 north; 593543.85 east 2021-02-13T01:01:18Z- AHRS_M2 msg: 1613178078.40 epoch sec; seqNo:17685 2021-02-13T01:01:18Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.17 psi 2021-02-13T01:01:18Z- handleDepth()-Depth_Keller msg: 1613178078447 epoch sec; seqNo:17825; depth 10.54 2021-02-13T01:01:18Z- handleDvl() - RDI_Pathfinder msg: 1613178078.47 epoch sec; seqNo:8609 2021-02-13T01:01:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.09 , 79.39 , 114.55 2021-02-13T01:01:18Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.06 , 0.12 2021-02-13T01:01:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178078.48 epoch sec; seqNo:17825; 4060633.29 north; 593545.07 east 2021-02-13T01:01:18Z- AHRS_M2 msg: 1613178078.80 epoch sec; seqNo:17686 2021-02-13T01:01:18Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.16 psi 2021-02-13T01:01:18Z- handleDepth()-Depth_Keller msg: 1613178078813 epoch sec; seqNo:17826; depth 10.67 2021-02-13T01:01:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178078.82 epoch sec; seqNo:17826; 4060633.67 north; 593545.06 east 2021-02-13T01:01:19Z- AHRS_M2 msg: 1613178079.21 epoch sec; seqNo:17687 2021-02-13T01:01:19Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.14 psi 2021-02-13T01:01:19Z- handleDepth()-Depth_Keller msg: 1613178079214 epoch sec; seqNo:17827; depth 10.78 2021-02-13T01:01:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:19Z- handleDvl() - RDI_Pathfinder msg: 1613178079.22 epoch sec; seqNo:8610 2021-02-13T01:01:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:19Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.06 , 0.12 2021-02-13T01:01:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178079.22 epoch sec; seqNo:17827; 4060633.67 north; 593545.06 east 2021-02-13T01:01:19Z- AHRS_M2 msg: 1613178079.61 epoch sec; seqNo:17688 2021-02-13T01:01:19Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.14 psi 2021-02-13T01:01:19Z- handleDepth()-Depth_Keller msg: 1613178079617 epoch sec; seqNo:17828; depth 10.87 2021-02-13T01:01:19Z- handleDvl() - RDI_Pathfinder msg: 1613178079.63 epoch sec; seqNo:8611 2021-02-13T01:01:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.87 , 99.74 , 78.11 , 110.56 2021-02-13T01:01:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.15 , 0.07 2021-02-13T01:01:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178079.65 epoch sec; seqNo:17828; 4060634.05 north; 593545.06 east 2021-02-13T01:01:20Z- AHRS_M2 msg: 1613178080.02 epoch sec; seqNo:17689 2021-02-13T01:01:20Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.14 psi 2021-02-13T01:01:20Z- handleDepth()-Depth_Keller msg: 1613178080021 epoch sec; seqNo:17829; depth 10.99 2021-02-13T01:01:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178080.03 epoch sec; seqNo:17829; 4060634.43 north; 593545.05 east 2021-02-13T01:01:20Z- AHRS_M2 msg: 1613178080.42 epoch sec; seqNo:17690 2021-02-13T01:01:20Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.15 psi 2021-02-13T01:01:20Z- handleDepth()-Depth_Keller msg: 1613178080488 epoch sec; seqNo:17830; depth 11.10 2021-02-13T01:01:20Z- handleDvl() - RDI_Pathfinder msg: 1613178080.50 epoch sec; seqNo:8612 2021-02-13T01:01:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.93 , 78.65 , 111.10 2021-02-13T01:01:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.16 , -0.05 2021-02-13T01:01:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178080.52 epoch sec; seqNo:17830; 4060634.82 north; 593546.27 east 2021-02-13T01:01:20Z- AHRS_M2 msg: 1613178080.82 epoch sec; seqNo:17691 2021-02-13T01:01:20Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.16 psi 2021-02-13T01:01:20Z- LcmTrn::handleMessages() - handling 45 messages took 0.80 ms and 129 calls... 2021-02-13T01:01:20Z- waiting for 1613178078.60, time is 1613178080.82 2021-02-13T01:01:20Z- LcmTrn::updateTrn() >>>> pose time 1613178080.82, meas time 1613178080.50 <<<< 2021-02-13T01:01:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178080.50 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:01:20Z- TerrainNav::motionUpdate() - currEstimate = (4060635, 593546.27, 11.10) 2021-02-13T01:01:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:01:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:01:20Z- TNavBF::Projecting Measurements 2021-02-13T01:01:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:01:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:01:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:01:20Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:01:20Z- TNavFilter::3 good beams. 2021-02-13T01:01:20Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:01:20Z- TNavBF:: beamIndex[0] = 1 2021-02-13T01:01:20Z- TNavBF:: beamIndex[1] = 2 2021-02-13T01:01:20Z- TNavBF:: beamIndex[2] = 3 2021-02-13T01:01:20Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:01:20Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:01:20Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:01:20Z- TNavBF::mmse selected from filter -1 2021-02-13T01:01:20Z- TerrainMapOctree: (vehN, vehE ) = (4060634.61, 593546.05). 2021-02-13T01:01:20Z- TerrainMapOctree: Min Distance = 935.02. 2021-02-13T01:01:20Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:01:20Z- There is no specific good beam that all particles have in common. 2021-02-13T01:01:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:01:20Z- Old Depth Bias: 0.00 DepthBiasCov: 2.26 K: 0.115 ExpectedMeasDiff: nan currMeas.covariance: 15.66 2021-02-13T01:01:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:01:20Z- Old Depth Bias: 0.00 DepthBiasCov: 2.00 K: 0.147 ExpectedMeasDiff: nan currMeas.covariance: 9.90 2021-02-13T01:01:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:01:20Z- Old Depth Bias: 0.00 DepthBiasCov: 1.71 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 19.75 2021-02-13T01:01:20Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:01:20Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:01:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:01:20Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:01:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:01:20Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:01:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:01:20Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:01:20Z- Filter 0 No beams used in subcloud update 2021-02-13T01:01:20Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:01:20Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:01:20Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 678.8 EastVariance: 833.0 DepthVariance: 1.6 this->DepthBiasCov: 1.6 2021-02-13T01:01:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178080.50 sec. 2021-02-13T01:01:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:01:20Z- TNavBF::mmse selected from filter -1 2021-02-13T01:01:20Z- variances([0], [2], and psi_berg) = 678.804812 832.987974 0.000000 2021-02-13T01:01:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:01:20Z- LcmTrn::publishEstimates() - MLE : 4060634.32 593563.77 11.10 2021-02-13T01:01:20Z- LcmTrn::publishEstimates() - MMSE : 4060634.82 593546.27 11.10 2021-02-13T01:01:20Z- LcmTrn::publishEstimates() - COVAR: 678.80 832.99 1.60 0.00 2021-02-13T01:01:20Z- handleDepth()-Depth_Keller msg: 1613178080840 epoch sec; seqNo:17831; depth 11.21 2021-02-13T01:01:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178080.85 epoch sec; seqNo:17831; 4060634.82 north; 593546.27 east 2021-02-13T01:01:21Z- AHRS_M2 msg: 1613178081.23 epoch sec; seqNo:17692 2021-02-13T01:01:21Z- AHRS_M2 msg: 0.60 phi; -0.30 theta; -0.17 psi 2021-02-13T01:01:21Z- handleDepth()-Depth_Keller msg: 1613178081258 epoch sec; seqNo:17832; depth 11.30 2021-02-13T01:01:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178081.27 epoch sec; seqNo:17832; 4060635.20 north; 593546.27 east 2021-02-13T01:01:21Z- AHRS_M2 msg: 1613178081.63 epoch sec; seqNo:17693 2021-02-13T01:01:21Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.17 psi 2021-02-13T01:01:21Z- handleDepth()-Depth_Keller msg: 1613178081657 epoch sec; seqNo:17833; depth 11.44 2021-02-13T01:01:21Z- handleDvl() - RDI_Pathfinder msg: 1613178081.68 epoch sec; seqNo:8613 2021-02-13T01:01:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.85 , 100.08 , 78.44 , 112.25 2021-02-13T01:01:21Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.10 , -0.16 2021-02-13T01:01:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178081.69 epoch sec; seqNo:17833; 4060635.58 north; 593546.26 east 2021-02-13T01:01:22Z- AHRS_M2 msg: 1613178082.04 epoch sec; seqNo:17694 2021-02-13T01:01:22Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.17 psi 2021-02-13T01:01:22Z- handleDepth()-Depth_Keller msg: 1613178082042 epoch sec; seqNo:17834; depth 11.53 2021-02-13T01:01:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178082.05 epoch sec; seqNo:17834; 4060635.96 north; 593546.26 east 2021-02-13T01:01:22Z- AHRS_M2 msg: 1613178082.44 epoch sec; seqNo:17695 2021-02-13T01:01:22Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.17 psi 2021-02-13T01:01:22Z- handleDepth()-Depth_Keller msg: 1613178082445 epoch sec; seqNo:17835; depth 11.63 2021-02-13T01:01:22Z- handleDvl() - RDI_Pathfinder msg: 1613178082.46 epoch sec; seqNo:8614 2021-02-13T01:01:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.29 , 75.61 , 109.75 2021-02-13T01:01:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.02 , -0.13 2021-02-13T01:01:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178082.47 epoch sec; seqNo:17835; 4060636.35 north; 593547.47 east 2021-02-13T01:01:22Z- AHRS_M2 msg: 1613178082.84 epoch sec; seqNo:17696 2021-02-13T01:01:22Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.16 psi 2021-02-13T01:01:22Z- handleDepth()-Depth_Keller msg: 1613178082848 epoch sec; seqNo:17836; depth 11.74 2021-02-13T01:01:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:22Z- handleDvl() - RDI_Pathfinder msg: 1613178082.85 epoch sec; seqNo:8615 2021-02-13T01:01:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.02 , -0.13 2021-02-13T01:01:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178082.86 epoch sec; seqNo:17836; 4060636.35 north; 593547.47 east 2021-02-13T01:01:23Z- AHRS_M2 msg: 1613178083.27 epoch sec; seqNo:17697 2021-02-13T01:01:23Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.16 psi 2021-02-13T01:01:23Z- handleDepth()-Depth_Keller msg: 1613178083287 epoch sec; seqNo:17837; depth 11.84 2021-02-13T01:01:23Z- handleDvl() - RDI_Pathfinder msg: 1613178083.30 epoch sec; seqNo:8616 2021-02-13T01:01:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.57 , 98.80 , 0.00 , 109.62 2021-02-13T01:01:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.04 , -0.06 2021-02-13T01:01:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178083.31 epoch sec; seqNo:17837; 4060636.73 north; 593547.47 east 2021-02-13T01:01:23Z- AHRS_M2 msg: 1613178083.65 epoch sec; seqNo:17698 2021-02-13T01:01:23Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.15 psi 2021-02-13T01:01:23Z- handleDepth()-Depth_Keller msg: 1613178083658 epoch sec; seqNo:17838; depth 11.96 2021-02-13T01:01:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178083.67 epoch sec; seqNo:17838; 4060637.11 north; 593547.47 east 2021-02-13T01:01:24Z- AHRS_M2 msg: 1613178084.07 epoch sec; seqNo:17699 2021-02-13T01:01:24Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.14 psi 2021-02-13T01:01:24Z- handleDepth()-Depth_Keller msg: 1613178084078 epoch sec; seqNo:17839; depth 12.06 2021-02-13T01:01:24Z- handleDvl() - RDI_Pathfinder msg: 1613178084.10 epoch sec; seqNo:8617 2021-02-13T01:01:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:24Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:01:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178084.12 epoch sec; seqNo:17839; 4060637.49 north; 593547.46 east 2021-02-13T01:01:24Z- AHRS_M2 msg: 1613178084.46 epoch sec; seqNo:17700 2021-02-13T01:01:24Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.14 psi 2021-02-13T01:01:24Z- handleDepth()-Depth_Keller msg: 1613178084464 epoch sec; seqNo:17840; depth 12.18 2021-02-13T01:01:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178084.48 epoch sec; seqNo:17840; 4060637.87 north; 593547.46 east 2021-02-13T01:01:24Z- AHRS_M2 msg: 1613178084.86 epoch sec; seqNo:17701 2021-02-13T01:01:24Z- AHRS_M2 msg: 0.63 phi; -0.30 theta; -0.14 psi 2021-02-13T01:01:24Z- handleDepth()-Depth_Keller msg: 1613178084869 epoch sec; seqNo:17841; depth 12.27 2021-02-13T01:01:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178084.88 epoch sec; seqNo:17841; 4060638.26 north; 593548.67 east 2021-02-13T01:01:25Z- AHRS_M2 msg: 1613178085.27 epoch sec; seqNo:17702 2021-02-13T01:01:25Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.16 psi 2021-02-13T01:01:25Z- handleDepth()-Depth_Keller msg: 1613178085274 epoch sec; seqNo:17842; depth 12.37 2021-02-13T01:01:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:25Z- handleDvl() - RDI_Pathfinder msg: 1613178085.28 epoch sec; seqNo:8618 2021-02-13T01:01:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:25Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:01:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178085.28 epoch sec; seqNo:17842; 4060638.26 north; 593548.67 east 2021-02-13T01:01:25Z- AHRS_M2 msg: 1613178085.67 epoch sec; seqNo:17703 2021-02-13T01:01:25Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.17 psi 2021-02-13T01:01:25Z- handleDepth()-Depth_Keller msg: 1613178085681 epoch sec; seqNo:17843; depth 12.47 2021-02-13T01:01:25Z- handleDvl() - RDI_Pathfinder msg: 1613178085.69 epoch sec; seqNo:8619 2021-02-13T01:01:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.79 , 97.31 , 78.38 , 110.83 2021-02-13T01:01:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , -0.07 2021-02-13T01:01:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178085.72 epoch sec; seqNo:17843; 4060638.64 north; 593548.67 east 2021-02-13T01:01:25Z- LcmTrn::handleMessages() - handling 45 messages took 0.78 ms and 130 calls... 2021-02-13T01:01:25Z- waiting for 1613178083.82, time is 1613178085.72 2021-02-13T01:01:25Z- LcmTrn::updateTrn() equating pose 1613178085.72 and meas times 1613178085.72 2021-02-13T01:01:25Z- LcmTrn::updateTrn() >>>> pose time 1613178085.72, meas time 1613178085.72 <<<< 2021-02-13T01:01:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178085.72 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:01:25Z- TerrainNav::motionUpdate() - currEstimate = (4060639, 593548.67, 12.47) 2021-02-13T01:01:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:01:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:01:25Z- TNavBF::Projecting Measurements 2021-02-13T01:01:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:01:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:01:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:01:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:01:25Z- TNavFilter::4 good beams. 2021-02-13T01:01:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:01:25Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:01:25Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:01:25Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:01:25Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:01:25Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:01:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:01:25Z- TNavBF::mmse selected from filter -1 2021-02-13T01:01:25Z- TerrainMapOctree: (vehN, vehE ) = (4060638.64, 593548.67). 2021-02-13T01:01:25Z- TerrainMapOctree: Min Distance = 930.39. 2021-02-13T01:01:25Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:01:25Z- There is no specific good beam that all particles have in common. 2021-02-13T01:01:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:01:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.83 K: 0.123 ExpectedMeasDiff: nan currMeas.covariance: 11.23 2021-02-13T01:01:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:01:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.60 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 15.15 2021-02-13T01:01:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:01:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.46 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 9.83 2021-02-13T01:01:25Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:01:25Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 19.65 2021-02-13T01:01:25Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:01:25Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:01:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:01:25Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:01:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:01:25Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:01:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:01:25Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:01:25Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:01:25Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:01:25Z- Filter 0 No beams used in subcloud update 2021-02-13T01:01:25Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:01:25Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:01:25Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 678.8 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T01:01:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178085.72 sec. 2021-02-13T01:01:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:01:26Z- TNavBF::mmse selected from filter -1 2021-02-13T01:01:26Z- variances([0], [2], and psi_berg) = 678.758793 832.987955 0.000000 2021-02-13T01:01:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:01:26Z- LcmTrn::publishEstimates() - MLE : 4060638.14 593565.17 12.47 2021-02-13T01:01:26Z- LcmTrn::publishEstimates() - MMSE : 4060638.64 593548.67 12.47 2021-02-13T01:01:26Z- LcmTrn::publishEstimates() - COVAR: 678.76 832.99 1.22 0.00 2021-02-13T01:01:26Z- AHRS_M2 msg: 1613178086.08 epoch sec; seqNo:17704 2021-02-13T01:01:26Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.18 psi 2021-02-13T01:01:26Z- handleDepth()-Depth_Keller msg: 1613178086085 epoch sec; seqNo:17844; depth 12.60 2021-02-13T01:01:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178086.10 epoch sec; seqNo:17844; 4060639.02 north; 593548.67 east 2021-02-13T01:01:26Z- AHRS_M2 msg: 1613178086.48 epoch sec; seqNo:17705 2021-02-13T01:01:26Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.18 psi 2021-02-13T01:01:26Z- handleDepth()-Depth_Keller msg: 1613178086486 epoch sec; seqNo:17845; depth 12.70 2021-02-13T01:01:26Z- handleDvl() - RDI_Pathfinder msg: 1613178086.50 epoch sec; seqNo:8620 2021-02-13T01:01:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.42 , 99.00 , 78.71 , 109.82 2021-02-13T01:01:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , -0.05 2021-02-13T01:01:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178086.51 epoch sec; seqNo:17845; 4060639.40 north; 593548.66 east 2021-02-13T01:01:26Z- AHRS_M2 msg: 1613178086.88 epoch sec; seqNo:17706 2021-02-13T01:01:26Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.18 psi 2021-02-13T01:01:26Z- handleDepth()-Depth_Keller msg: 1613178086893 epoch sec; seqNo:17846; depth 12.83 2021-02-13T01:01:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178086.90 epoch sec; seqNo:17846; 4060639.77 north; 593548.66 east 2021-02-13T01:01:27Z- AHRS_M2 msg: 1613178087.29 epoch sec; seqNo:17707 2021-02-13T01:01:27Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.17 psi 2021-02-13T01:01:27Z- handleDepth()-Depth_Keller msg: 1613178087297 epoch sec; seqNo:17847; depth 12.94 2021-02-13T01:01:27Z- handleDvl() - RDI_Pathfinder msg: 1613178087.31 epoch sec; seqNo:8621 2021-02-13T01:01:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.49 , 99.07 , 75.61 , 109.89 2021-02-13T01:01:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.09 , -0.01 2021-02-13T01:01:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178087.32 epoch sec; seqNo:17847; 4060639.79 north; 593549.88 east 2021-02-13T01:01:27Z- AHRS_M2 msg: 1613178087.69 epoch sec; seqNo:17708 2021-02-13T01:01:27Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.17 psi 2021-02-13T01:01:27Z- handleDepth()-Depth_Keller msg: 1613178087698 epoch sec; seqNo:17848; depth 13.06 2021-02-13T01:01:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:27Z- handleDvl() - RDI_Pathfinder msg: 1613178087.70 epoch sec; seqNo:8622 2021-02-13T01:01:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.09 , -0.01 2021-02-13T01:01:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178087.71 epoch sec; seqNo:17848; 4060640.17 north; 593549.87 east 2021-02-13T01:01:28Z- AHRS_M2 msg: 1613178088.10 epoch sec; seqNo:17709 2021-02-13T01:01:28Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.18 psi 2021-02-13T01:01:28Z- handleDepth()-Depth_Keller msg: 1613178088100 epoch sec; seqNo:17849; depth 13.16 2021-02-13T01:01:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178088.11 epoch sec; seqNo:17849; 4060640.55 north; 593549.87 east 2021-02-13T01:01:28Z- AHRS_M2 msg: 1613178088.50 epoch sec; seqNo:17710 2021-02-13T01:01:28Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.19 psi 2021-02-13T01:01:28Z- handleDepth()-Depth_Keller msg: 1613178088509 epoch sec; seqNo:17850; depth 13.28 2021-02-13T01:01:28Z- handleDvl() - RDI_Pathfinder msg: 1613178088.52 epoch sec; seqNo:8623 2021-02-13T01:01:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 98.86 , 72.75 , 112.25 2021-02-13T01:01:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.06 , 0.09 2021-02-13T01:01:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178088.53 epoch sec; seqNo:17850; 4060640.55 north; 593549.87 east 2021-02-13T01:01:28Z- AHRS_M2 msg: 1613178088.90 epoch sec; seqNo:17711 2021-02-13T01:01:28Z- AHRS_M2 msg: 0.62 phi; -0.32 theta; -0.19 psi 2021-02-13T01:01:28Z- handleDepth()-Depth_Keller msg: 1613178088911 epoch sec; seqNo:17851; depth 13.40 2021-02-13T01:01:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:28Z- handleDvl() - RDI_Pathfinder msg: 1613178088.91 epoch sec; seqNo:8624 2021-02-13T01:01:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.06 , 0.09 2021-02-13T01:01:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178088.92 epoch sec; seqNo:17851; 4060640.92 north; 593549.87 east 2021-02-13T01:01:29Z- AHRS_M2 msg: 1613178089.31 epoch sec; seqNo:17712 2021-02-13T01:01:29Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-13T01:01:29Z- handleDepth()-Depth_Keller msg: 1613178089317 epoch sec; seqNo:17852; depth 13.52 2021-02-13T01:01:29Z- handleDvl() - RDI_Pathfinder msg: 1613178089.34 epoch sec; seqNo:8625 2021-02-13T01:01:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 98.86 , 75.21 , 111.04 2021-02-13T01:01:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , 0.17 2021-02-13T01:01:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178089.35 epoch sec; seqNo:17852; 4060641.30 north; 593549.86 east 2021-02-13T01:01:29Z- AHRS_M2 msg: 1613178089.71 epoch sec; seqNo:17713 2021-02-13T01:01:29Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-13T01:01:29Z- handleDepth()-Depth_Keller msg: 1613178089721 epoch sec; seqNo:17853; depth 13.64 2021-02-13T01:01:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178089.75 epoch sec; seqNo:17853; 4060641.30 north; 593549.86 east 2021-02-13T01:01:30Z- AHRS_M2 msg: 1613178090.12 epoch sec; seqNo:17714 2021-02-13T01:01:30Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-13T01:01:30Z- handleDepth()-Depth_Keller msg: 1613178090123 epoch sec; seqNo:17854; depth 13.77 2021-02-13T01:01:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:30Z- handleDvl() - RDI_Pathfinder msg: 1613178090.13 epoch sec; seqNo:8626 2021-02-13T01:01:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , 0.17 2021-02-13T01:01:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178090.14 epoch sec; seqNo:17854; 4060641.70 north; 593551.08 east 2021-02-13T01:01:30Z- AHRS_M2 msg: 1613178090.52 epoch sec; seqNo:17715 2021-02-13T01:01:30Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-13T01:01:30Z- handleDepth()-Depth_Keller msg: 1613178090594 epoch sec; seqNo:17855; depth 13.88 2021-02-13T01:01:30Z- handleDvl() - RDI_Pathfinder msg: 1613178090.61 epoch sec; seqNo:8627 2021-02-13T01:01:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 100.08 , 73.57 , 112.25 2021-02-13T01:01:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.16 , 0.16 2021-02-13T01:01:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178090.62 epoch sec; seqNo:17855; 4060642.07 north; 593551.07 east 2021-02-13T01:01:30Z- AHRS_M2 msg: 1613178090.92 epoch sec; seqNo:17716 2021-02-13T01:01:30Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.19 psi 2021-02-13T01:01:30Z- handleDepth()-Depth_Keller msg: 1613178090957 epoch sec; seqNo:17856; depth 14.02 2021-02-13T01:01:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178090.97 epoch sec; seqNo:17856; 4060642.07 north; 593551.07 east 2021-02-13T01:01:31Z- LcmTrn::handleMessages() - handling 47 messages took 0.71 ms and 133 calls... 2021-02-13T01:01:31Z- waiting for 1613178088.72, time is 1613178090.97 2021-02-13T01:01:31Z- LcmTrn::updateTrn() >>>> pose time 1613178090.97, meas time 1613178090.61 <<<< 2021-02-13T01:01:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178090.61 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:01:31Z- TerrainNav::motionUpdate() - currEstimate = (4060642, 593551.07, 14.02) 2021-02-13T01:01:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:01:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:01:31Z- TNavBF::Projecting Measurements 2021-02-13T01:01:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:01:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:01:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:01:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:01:31Z- TNavFilter::4 good beams. 2021-02-13T01:01:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:01:31Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:01:31Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:01:31Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:01:31Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:01:31Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:01:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:01:31Z- TNavBF::mmse selected from filter -1 2021-02-13T01:01:31Z- TerrainMapOctree: (vehN, vehE ) = (4060641.83, 593550.91). 2021-02-13T01:01:31Z- TerrainMapOctree: Min Distance = 926.61. 2021-02-13T01:01:31Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:01:31Z- There is no specific good beam that all particles have in common. 2021-02-13T01:01:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:01:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.42 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 10.71 2021-02-13T01:01:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:01:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 16.03 2021-02-13T01:01:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:01:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 8.66 2021-02-13T01:01:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:01:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 20.16 2021-02-13T01:01:31Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:01:31Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:01:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:01:31Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:01:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:01:31Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:01:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:01:31Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:01:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:01:31Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:01:31Z- Filter 0 No beams used in subcloud update 2021-02-13T01:01:31Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:01:31Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:01:31Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 678.7 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T01:01:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178090.61 sec. 2021-02-13T01:01:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:01:31Z- TNavBF::mmse selected from filter -1 2021-02-13T01:01:31Z- variances([0], [2], and psi_berg) = 678.704290 832.987932 0.000000 2021-02-13T01:01:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:01:31Z- LcmTrn::publishEstimates() - MLE : 4060641.57 593567.57 14.02 2021-02-13T01:01:31Z- LcmTrn::publishEstimates() - MMSE : 4060642.07 593551.07 14.02 2021-02-13T01:01:31Z- LcmTrn::publishEstimates() - COVAR: 678.70 832.99 1.03 0.00 2021-02-13T01:01:31Z- AHRS_M2 msg: 1613178091.33 epoch sec; seqNo:17717 2021-02-13T01:01:31Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-13T01:01:31Z- handleDepth()-Depth_Keller msg: 1613178091356 epoch sec; seqNo:17857; depth 14.12 2021-02-13T01:01:31Z- handleDvl() - RDI_Pathfinder msg: 1613178091.37 epoch sec; seqNo:8628 2021-02-13T01:01:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.49 , 99.07 , 74.39 , 111.24 2021-02-13T01:01:31Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.19 , 0.07 2021-02-13T01:01:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178091.38 epoch sec; seqNo:17857; 4060642.45 north; 593551.07 east 2021-02-13T01:01:31Z- AHRS_M2 msg: 1613178091.73 epoch sec; seqNo:17718 2021-02-13T01:01:31Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-13T01:01:31Z- handleDepth()-Depth_Keller msg: 1613178091757 epoch sec; seqNo:17858; depth 14.25 2021-02-13T01:01:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178091.78 epoch sec; seqNo:17858; 4060642.83 north; 593551.07 east 2021-02-13T01:01:32Z- AHRS_M2 msg: 1613178092.15 epoch sec; seqNo:17719 2021-02-13T01:01:32Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.20 psi 2021-02-13T01:01:32Z- handleDepth()-Depth_Keller msg: 1613178092156 epoch sec; seqNo:17859; depth 14.37 2021-02-13T01:01:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178092.17 epoch sec; seqNo:17859; 4060642.85 north; 593552.29 east 2021-02-13T01:01:32Z- AHRS_M2 msg: 1613178092.55 epoch sec; seqNo:17720 2021-02-13T01:01:32Z- AHRS_M2 msg: 0.63 phi; -0.32 theta; -0.20 psi 2021-02-13T01:01:32Z- handleDepth()-Depth_Keller msg: 1613178092554 epoch sec; seqNo:17860; depth 14.50 2021-02-13T01:01:32Z- handleDvl() - RDI_Pathfinder msg: 1613178092.57 epoch sec; seqNo:8629 2021-02-13T01:01:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 100.08 , 78.17 , 109.82 2021-02-13T01:01:32Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.12 , -0.00 2021-02-13T01:01:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178092.58 epoch sec; seqNo:17860; 4060643.22 north; 593552.28 east 2021-02-13T01:01:32Z- AHRS_M2 msg: 1613178092.94 epoch sec; seqNo:17721 2021-02-13T01:01:32Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.20 psi 2021-02-13T01:01:32Z- handleDepth()-Depth_Keller msg: 1613178092953 epoch sec; seqNo:17861; depth 14.64 2021-02-13T01:01:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178092.96 epoch sec; seqNo:17861; 4060643.60 north; 593552.28 east 2021-02-13T01:01:33Z- AHRS_M2 msg: 1613178093.35 epoch sec; seqNo:17722 2021-02-13T01:01:33Z- AHRS_M2 msg: 0.64 phi; -0.32 theta; -0.19 psi 2021-02-13T01:01:33Z- handleDepth()-Depth_Keller msg: 1613178093353 epoch sec; seqNo:17862; depth 14.75 2021-02-13T01:01:33Z- handleDvl() - RDI_Pathfinder msg: 1613178093.36 epoch sec; seqNo:8630 2021-02-13T01:01:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.48 , 100.42 , 72.75 , 108.53 2021-02-13T01:01:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.05 , -0.03 2021-02-13T01:01:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178093.38 epoch sec; seqNo:17862; 4060643.98 north; 593552.27 east 2021-02-13T01:01:33Z- AHRS_M2 msg: 1613178093.75 epoch sec; seqNo:17723 2021-02-13T01:01:33Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.19 psi 2021-02-13T01:01:33Z- handleDepth()-Depth_Keller msg: 1613178093820 epoch sec; seqNo:17863; depth 14.87 2021-02-13T01:01:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:33Z- handleDvl() - RDI_Pathfinder msg: 1613178093.82 epoch sec; seqNo:8631 2021-02-13T01:01:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.05 , -0.03 2021-02-13T01:01:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178093.83 epoch sec; seqNo:17863; 4060643.99 north; 593553.49 east 2021-02-13T01:01:34Z- AHRS_M2 msg: 1613178094.16 epoch sec; seqNo:17724 2021-02-13T01:01:34Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-13T01:01:34Z- handleDepth()-Depth_Keller msg: 1613178094172 epoch sec; seqNo:17864; depth 15.02 2021-02-13T01:01:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178094.21 epoch sec; seqNo:17864; 4060644.37 north; 593553.49 east 2021-02-13T01:01:34Z- AHRS_M2 msg: 1613178094.56 epoch sec; seqNo:17725 2021-02-13T01:01:34Z- AHRS_M2 msg: 0.63 phi; -0.33 theta; -0.18 psi 2021-02-13T01:01:34Z- handleDepth()-Depth_Keller msg: 1613178094585 epoch sec; seqNo:17865; depth 15.12 2021-02-13T01:01:34Z- handleDvl() - RDI_Pathfinder msg: 1613178094.60 epoch sec; seqNo:8632 2021-02-13T01:01:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.74 , 99.00 , 73.57 , 107.52 2021-02-13T01:01:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.05 , -0.01 2021-02-13T01:01:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178094.61 epoch sec; seqNo:17865; 4060644.75 north; 593553.49 east 2021-02-13T01:01:34Z- AHRS_M2 msg: 1613178094.96 epoch sec; seqNo:17726 2021-02-13T01:01:34Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-13T01:01:34Z- handleDepth()-Depth_Keller msg: 1613178094989 epoch sec; seqNo:17866; depth 15.24 2021-02-13T01:01:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:34Z- handleDvl() - RDI_Pathfinder msg: 1613178094.99 epoch sec; seqNo:8633 2021-02-13T01:01:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.05 , -0.01 2021-02-13T01:01:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178095.00 epoch sec; seqNo:17866; 4060645.13 north; 593553.48 east 2021-02-13T01:01:35Z- AHRS_M2 msg: 1613178095.37 epoch sec; seqNo:17727 2021-02-13T01:01:35Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-13T01:01:35Z- handleDepth()-Depth_Keller msg: 1613178095381 epoch sec; seqNo:17867; depth 15.35 2021-02-13T01:01:35Z- handleDvl() - RDI_Pathfinder msg: 1613178095.40 epoch sec; seqNo:8634 2021-02-13T01:01:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.29 , 97.71 , 78.24 , 108.67 2021-02-13T01:01:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.10 , -0.02 2021-02-13T01:01:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178095.41 epoch sec; seqNo:17867; 4060645.13 north; 593553.48 east 2021-02-13T01:01:35Z- AHRS_M2 msg: 1613178095.77 epoch sec; seqNo:17728 2021-02-13T01:01:35Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.18 psi 2021-02-13T01:01:35Z- handleDepth()-Depth_Keller msg: 1613178095777 epoch sec; seqNo:17868; depth 15.46 2021-02-13T01:01:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178095.79 epoch sec; seqNo:17868; 4060645.51 north; 593553.48 east 2021-02-13T01:01:36Z- LcmTrn::handleMessages() - handling 43 messages took 0.68 ms and 130 calls... 2021-02-13T01:01:36Z- waiting for 1613178093.97, time is 1613178095.79 2021-02-13T01:01:36Z- LcmTrn::updateTrn() >>>> pose time 1613178095.79, meas time 1613178095.40 <<<< 2021-02-13T01:01:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178095.40 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:01:36Z- TerrainNav::motionUpdate() - currEstimate = (4060646, 593553.48, 15.46) 2021-02-13T01:01:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:01:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:01:36Z- TNavBF::Projecting Measurements 2021-02-13T01:01:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:01:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:01:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:01:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:01:36Z- TNavFilter::4 good beams. 2021-02-13T01:01:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:01:36Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:01:36Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:01:36Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:01:36Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:01:36Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:01:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:01:36Z- TNavBF::mmse selected from filter -1 2021-02-13T01:01:36Z- TerrainMapOctree: (vehN, vehE ) = (4060645.24, 593553.29). 2021-02-13T01:01:36Z- TerrainMapOctree: Min Distance = 922.58. 2021-02-13T01:01:36Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:01:36Z- There is no specific good beam that all particles have in common. 2021-02-13T01:01:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:01:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 10.06 2021-02-13T01:01:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:01:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T01:01:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:01:36Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 9.79 2021-02-13T01:01:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:01:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 18.89 2021-02-13T01:01:36Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:01:36Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:01:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:01:36Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:01:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:01:36Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:01:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:01:36Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:01:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:01:36Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:01:36Z- Filter 0 No beams used in subcloud update 2021-02-13T01:01:36Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:01:36Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:01:36Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 678.7 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:01:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178095.40 sec. 2021-02-13T01:01:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:01:36Z- TNavBF::mmse selected from filter -1 2021-02-13T01:01:36Z- variances([0], [2], and psi_berg) = 678.649235 832.987909 0.000000 2021-02-13T01:01:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:01:36Z- LcmTrn::publishEstimates() - MLE : 4060645.01 593569.98 15.46 2021-02-13T01:01:36Z- LcmTrn::publishEstimates() - MMSE : 4060645.51 593553.48 15.46 2021-02-13T01:01:36Z- LcmTrn::publishEstimates() - COVAR: 678.65 832.99 0.93 0.00 2021-02-13T01:01:36Z- AHRS_M2 msg: 1613178096.17 epoch sec; seqNo:17729 2021-02-13T01:01:36Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.19 psi 2021-02-13T01:01:36Z- handleDepth()-Depth_Keller msg: 1613178096188 epoch sec; seqNo:17869; depth 15.56 2021-02-13T01:01:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:36Z- handleDvl() - RDI_Pathfinder msg: 1613178096.19 epoch sec; seqNo:8635 2021-02-13T01:01:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.10 , -0.02 2021-02-13T01:01:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178096.20 epoch sec; seqNo:17869; 4060645.90 north; 593554.69 east 2021-02-13T01:01:36Z- AHRS_M2 msg: 1613178096.58 epoch sec; seqNo:17730 2021-02-13T01:01:36Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-13T01:01:36Z- handleDepth()-Depth_Keller msg: 1613178096584 epoch sec; seqNo:17870; depth 15.70 2021-02-13T01:01:36Z- handleDvl() - RDI_Pathfinder msg: 1613178096.60 epoch sec; seqNo:8636 2021-02-13T01:01:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 97.65 , 72.35 , 109.82 2021-02-13T01:01:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.12 , -0.07 2021-02-13T01:01:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178096.61 epoch sec; seqNo:17870; 4060645.90 north; 593554.69 east 2021-02-13T01:01:36Z- AHRS_M2 msg: 1613178096.98 epoch sec; seqNo:17731 2021-02-13T01:01:36Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-13T01:01:36Z- handleDepth()-Depth_Keller msg: 1613178096993 epoch sec; seqNo:17871; depth 15.82 2021-02-13T01:01:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178097.00 epoch sec; seqNo:17871; 4060646.28 north; 593554.69 east 2021-02-13T01:01:37Z- AHRS_M2 msg: 1613178097.39 epoch sec; seqNo:17732 2021-02-13T01:01:37Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-13T01:01:37Z- handleDepth()-Depth_Keller msg: 1613178097394 epoch sec; seqNo:17872; depth 15.93 2021-02-13T01:01:37Z- handleDvl() - RDI_Pathfinder msg: 1613178097.41 epoch sec; seqNo:8637 2021-02-13T01:01:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.53 , 99.00 , 72.35 , 108.74 2021-02-13T01:01:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.08 , -0.12 2021-02-13T01:01:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178097.42 epoch sec; seqNo:17872; 4060646.66 north; 593554.69 east 2021-02-13T01:01:37Z- AHRS_M2 msg: 1613178097.79 epoch sec; seqNo:17733 2021-02-13T01:01:37Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-13T01:01:37Z- handleDepth()-Depth_Keller msg: 1613178097796 epoch sec; seqNo:17873; depth 16.06 2021-02-13T01:01:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178097.81 epoch sec; seqNo:17873; 4060647.04 north; 593554.68 east 2021-02-13T01:01:38Z- AHRS_M2 msg: 1613178098.20 epoch sec; seqNo:17734 2021-02-13T01:01:38Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-13T01:01:38Z- handleDepth()-Depth_Keller msg: 1613178098211 epoch sec; seqNo:17874; depth 16.18 2021-02-13T01:01:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178098.22 epoch sec; seqNo:17874; 4060647.43 north; 593555.90 east 2021-02-13T01:01:38Z- handleDepth()-Depth_Keller msg: 1613178098621 epoch sec; seqNo:17875; depth 16.30 2021-02-13T01:01:38Z- handleDvl() - RDI_Pathfinder msg: 1613178098.63 epoch sec; seqNo:8638 2021-02-13T01:01:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.53 , 99.00 , 71.53 , 108.74 2021-02-13T01:01:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.01 , -0.12 2021-02-13T01:01:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178098.64 epoch sec; seqNo:17875; 4060647.81 north; 593555.89 east 2021-02-13T01:01:39Z- AHRS_M2 msg: 1613178099.00 epoch sec; seqNo:17735 2021-02-13T01:01:39Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.19 psi 2021-02-13T01:01:39Z- handleDepth()-Depth_Keller msg: 1613178099009 epoch sec; seqNo:17876; depth 16.42 2021-02-13T01:01:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178099.03 epoch sec; seqNo:17876; 4060647.81 north; 593555.89 east 2021-02-13T01:01:39Z- AHRS_M2 msg: 1613178099.41 epoch sec; seqNo:17736 2021-02-13T01:01:39Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-13T01:01:39Z- handleDepth()-Depth_Keller msg: 1613178099412 epoch sec; seqNo:17877; depth 16.54 2021-02-13T01:01:39Z- handleDvl() - RDI_Pathfinder msg: 1613178099.42 epoch sec; seqNo:8639 2021-02-13T01:01:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.02 , 97.78 , 69.89 , 107.52 2021-02-13T01:01:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , -0.07 2021-02-13T01:01:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178099.45 epoch sec; seqNo:17877; 4060648.19 north; 593555.89 east 2021-02-13T01:01:39Z- AHRS_M2 msg: 1613178099.81 epoch sec; seqNo:17737 2021-02-13T01:01:39Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.20 psi 2021-02-13T01:01:39Z- handleDepth()-Depth_Keller msg: 1613178099821 epoch sec; seqNo:17878; depth 16.67 2021-02-13T01:01:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:39Z- handleDvl() - RDI_Pathfinder msg: 1613178099.82 epoch sec; seqNo:8640 2021-02-13T01:01:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , -0.07 2021-02-13T01:01:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178099.83 epoch sec; seqNo:17878; 4060648.57 north; 593555.89 east 2021-02-13T01:01:40Z- AHRS_M2 msg: 1613178100.21 epoch sec; seqNo:17738 2021-02-13T01:01:40Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-13T01:01:40Z- handleDepth()-Depth_Keller msg: 1613178100228 epoch sec; seqNo:17879; depth 16.79 2021-02-13T01:01:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178100.24 epoch sec; seqNo:17879; 4060648.96 north; 593557.10 east 2021-02-13T01:01:40Z- AHRS_M2 msg: 1613178100.62 epoch sec; seqNo:17739 2021-02-13T01:01:40Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.20 psi 2021-02-13T01:01:40Z- handleDepth()-Depth_Keller msg: 1613178100625 epoch sec; seqNo:17880; depth 16.91 2021-02-13T01:01:40Z- handleDvl() - RDI_Pathfinder msg: 1613178100.64 epoch sec; seqNo:8641 2021-02-13T01:01:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.99 , 70.71 , 107.72 2021-02-13T01:01:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , -0.00 2021-02-13T01:01:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178100.65 epoch sec; seqNo:17880; 4060648.96 north; 593557.10 east 2021-02-13T01:01:41Z- AHRS_M2 msg: 1613178101.03 epoch sec; seqNo:17740 2021-02-13T01:01:41Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.20 psi 2021-02-13T01:01:41Z- handleDepth()-Depth_Keller msg: 1613178101034 epoch sec; seqNo:17881; depth 17.04 2021-02-13T01:01:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:41Z- handleDvl() - RDI_Pathfinder msg: 1613178101.04 epoch sec; seqNo:8642 2021-02-13T01:01:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , -0.00 2021-02-13T01:01:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178101.05 epoch sec; seqNo:17881; 4060649.34 north; 593557.10 east 2021-02-13T01:01:41Z- LcmTrn::handleMessages() - handling 46 messages took 0.64 ms and 133 calls... 2021-02-13T01:01:41Z- waiting for 1613178098.79, time is 1613178101.05 2021-02-13T01:01:41Z- LcmTrn::updateTrn() equating pose 1613178101.05 and meas times 1613178101.05 2021-02-13T01:01:41Z- LcmTrn::updateTrn() >>>> pose time 1613178101.05, meas time 1613178101.05 <<<< 2021-02-13T01:01:41Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613178101.05, ping # 0. 2021-02-13T01:01:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178101.05 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:01:41Z- TerrainNav::motionUpdate() - currEstimate = (4060649, 593557.10, 17.04) 2021-02-13T01:01:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:01:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:01:41Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:01:41Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:01:41Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178101.05 sec.; No relevant navigation data available 2021-02-13T01:01:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:01:41Z- TNavBF::mmse selected from filter -1 2021-02-13T01:01:41Z- variances([0], [2], and psi_berg) = 678.599557 832.987888 0.000000 2021-02-13T01:01:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:01:41Z- LcmTrn::publishEstimates() - MLE : 4060648.84 593573.60 17.04 2021-02-13T01:01:41Z- LcmTrn::publishEstimates() - MMSE : 4060649.34 593557.10 17.04 2021-02-13T01:01:41Z- LcmTrn::publishEstimates() - COVAR: 678.60 832.99 1.20 0.00 2021-02-13T01:01:41Z- AHRS_M2 msg: 1613178101.43 epoch sec; seqNo:17741 2021-02-13T01:01:41Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.20 psi 2021-02-13T01:01:41Z- handleDepth()-Depth_Keller msg: 1613178101433 epoch sec; seqNo:17882; depth 17.17 2021-02-13T01:01:41Z- handleDvl() - RDI_Pathfinder msg: 1613178101.44 epoch sec; seqNo:8643 2021-02-13T01:01:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.43 , 97.85 , 70.30 , 107.59 2021-02-13T01:01:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , 0.05 2021-02-13T01:01:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178101.46 epoch sec; seqNo:17882; 4060649.72 north; 593557.09 east 2021-02-13T01:01:41Z- AHRS_M2 msg: 1613178101.83 epoch sec; seqNo:17742 2021-02-13T01:01:41Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.21 psi 2021-02-13T01:01:41Z- handleDepth()-Depth_Keller msg: 1613178101845 epoch sec; seqNo:17883; depth 17.28 2021-02-13T01:01:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178101.86 epoch sec; seqNo:17883; 4060650.10 north; 593557.09 east 2021-02-13T01:01:42Z- AHRS_M2 msg: 1613178102.24 epoch sec; seqNo:17743 2021-02-13T01:01:42Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.21 psi 2021-02-13T01:01:42Z- handleDepth()-Depth_Keller msg: 1613178102246 epoch sec; seqNo:17884; depth 17.41 2021-02-13T01:01:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:42Z- handleDvl() - RDI_Pathfinder msg: 1613178102.25 epoch sec; seqNo:8644 2021-02-13T01:01:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , 0.05 2021-02-13T01:01:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178102.27 epoch sec; seqNo:17884; 4060650.10 north; 593557.09 east 2021-02-13T01:01:42Z- AHRS_M2 msg: 1613178102.64 epoch sec; seqNo:17744 2021-02-13T01:01:42Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.21 psi 2021-02-13T01:01:42Z- handleDepth()-Depth_Keller msg: 1613178102644 epoch sec; seqNo:17885; depth 17.52 2021-02-13T01:01:42Z- handleDvl() - RDI_Pathfinder msg: 1613178102.66 epoch sec; seqNo:8645 2021-02-13T01:01:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 97.92 , 70.71 , 107.65 2021-02-13T01:01:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.06 , 0.06 2021-02-13T01:01:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178102.67 epoch sec; seqNo:17885; 4060650.48 north; 593557.09 east 2021-02-13T01:01:43Z- AHRS_M2 msg: 1613178103.05 epoch sec; seqNo:17745 2021-02-13T01:01:43Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.21 psi 2021-02-13T01:01:43Z- handleDepth()-Depth_Keller msg: 1613178103050 epoch sec; seqNo:17886; depth 17.66 2021-02-13T01:01:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178103.06 epoch sec; seqNo:17886; 4060650.87 north; 593558.30 east 2021-02-13T01:01:43Z- AHRS_M2 msg: 1613178103.47 epoch sec; seqNo:17746 2021-02-13T01:01:43Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.21 psi 2021-02-13T01:01:43Z- handleDepth()-Depth_Keller msg: 1613178103540 epoch sec; seqNo:17887; depth 17.78 2021-02-13T01:01:43Z- handleDvl() - RDI_Pathfinder msg: 1613178103.55 epoch sec; seqNo:8646 2021-02-13T01:01:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 97.85 , 78.38 , 106.37 2021-02-13T01:01:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.07 , 0.03 2021-02-13T01:01:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178103.58 epoch sec; seqNo:17887; 4060651.25 north; 593558.30 east 2021-02-13T01:01:43Z- AHRS_M2 msg: 1613178103.85 epoch sec; seqNo:17747 2021-02-13T01:01:43Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.22 psi 2021-02-13T01:01:43Z- handleDepth()-Depth_Keller msg: 1613178103863 epoch sec; seqNo:17888; depth 17.93 2021-02-13T01:01:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178103.88 epoch sec; seqNo:17888; 4060651.25 north; 593558.30 east 2021-02-13T01:01:44Z- AHRS_M2 msg: 1613178104.26 epoch sec; seqNo:17748 2021-02-13T01:01:44Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.22 psi 2021-02-13T01:01:44Z- handleDepth()-Depth_Keller msg: 1613178104286 epoch sec; seqNo:17889; depth 18.02 2021-02-13T01:01:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178104.31 epoch sec; seqNo:17889; 4060651.63 north; 593558.29 east 2021-02-13T01:01:44Z- AHRS_M2 msg: 1613178104.66 epoch sec; seqNo:17749 2021-02-13T01:01:44Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.22 psi 2021-02-13T01:01:44Z- handleDepth()-Depth_Keller msg: 1613178104685 epoch sec; seqNo:17890; depth 18.15 2021-02-13T01:01:44Z- handleDvl() - RDI_Pathfinder msg: 1613178104.70 epoch sec; seqNo:8647 2021-02-13T01:01:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.24 , 98.93 , 69.07 , 107.45 2021-02-13T01:01:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.05 , -0.00 2021-02-13T01:01:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178104.71 epoch sec; seqNo:17890; 4060652.00 north; 593558.29 east 2021-02-13T01:01:45Z- AHRS_M2 msg: 1613178105.06 epoch sec; seqNo:17750 2021-02-13T01:01:45Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.22 psi 2021-02-13T01:01:45Z- handleDepth()-Depth_Keller msg: 1613178105073 epoch sec; seqNo:17891; depth 18.28 2021-02-13T01:01:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178105.10 epoch sec; seqNo:17891; 4060652.00 north; 593558.29 east 2021-02-13T01:01:45Z- AHRS_M2 msg: 1613178105.47 epoch sec; seqNo:17751 2021-02-13T01:01:45Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.21 psi 2021-02-13T01:01:45Z- handleDepth()-Depth_Keller msg: 1613178105473 epoch sec; seqNo:17892; depth 18.40 2021-02-13T01:01:45Z- handleDvl() - RDI_Pathfinder msg: 1613178105.48 epoch sec; seqNo:8648 2021-02-13T01:01:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 97.92 , 69.07 , 105.22 2021-02-13T01:01:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.00 , 0.03 2021-02-13T01:01:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178105.49 epoch sec; seqNo:17892; 4060652.40 north; 593559.51 east 2021-02-13T01:01:45Z- AHRS_M2 msg: 1613178105.87 epoch sec; seqNo:17752 2021-02-13T01:01:45Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.21 psi 2021-02-13T01:01:45Z- handleDepth()-Depth_Keller msg: 1613178105878 epoch sec; seqNo:17893; depth 18.54 2021-02-13T01:01:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:45Z- handleDvl() - RDI_Pathfinder msg: 1613178105.88 epoch sec; seqNo:8649 2021-02-13T01:01:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.00 , 0.03 2021-02-13T01:01:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178105.89 epoch sec; seqNo:17893; 4060652.78 north; 593559.50 east 2021-02-13T01:01:46Z- LcmTrn::handleMessages() - handling 43 messages took 0.71 ms and 129 calls... 2021-02-13T01:01:46Z- waiting for 1613178104.05, time is 1613178105.89 2021-02-13T01:01:46Z- LcmTrn::updateTrn() equating pose 1613178105.89 and meas times 1613178105.89 2021-02-13T01:01:46Z- LcmTrn::updateTrn() >>>> pose time 1613178105.89, meas time 1613178105.89 <<<< 2021-02-13T01:01:46Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613178105.89, ping # 0. 2021-02-13T01:01:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178105.89 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:01:46Z- TerrainNav::motionUpdate() - currEstimate = (4060653, 593559.50, 18.54) 2021-02-13T01:01:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:01:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:01:46Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:01:46Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:01:46Z- TNavBF::Projecting Measurements 2021-02-13T01:01:46Z- TNavFilter::Measurement from time = 1613178105.89 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T01:01:46Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:01:46Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:01:46Z- TNavBF:: beamIndex[1] = 0 2021-02-13T01:01:46Z- TNavBF:: beamIndex[2] = 0 2021-02-13T01:01:46Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:01:46Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:01:46Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:01:46Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 678.6 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T01:01:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:01:46Z- TNavBF::mmse selected from filter -1 2021-02-13T01:01:46Z- variances([0], [2], and psi_berg) = 678.555261 832.987870 0.000000 2021-02-13T01:01:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:01:46Z- LcmTrn::publishEstimates() - MLE : 4060652.28 593576.00 18.54 2021-02-13T01:01:46Z- LcmTrn::publishEstimates() - MMSE : 4060652.78 593559.50 18.54 2021-02-13T01:01:46Z- LcmTrn::publishEstimates() - COVAR: 678.56 832.99 1.42 0.00 2021-02-13T01:01:46Z- AHRS_M2 msg: 1613178106.28 epoch sec; seqNo:17753 2021-02-13T01:01:46Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.21 psi 2021-02-13T01:01:46Z- handleDepth()-Depth_Keller msg: 1613178106338 epoch sec; seqNo:17894; depth 18.65 2021-02-13T01:01:46Z- handleDvl() - RDI_Pathfinder msg: 1613178106.36 epoch sec; seqNo:8650 2021-02-13T01:01:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 96.84 , 69.07 , 106.57 2021-02-13T01:01:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.01 , 0.09 2021-02-13T01:01:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178106.37 epoch sec; seqNo:17894; 4060653.15 north; 593559.50 east 2021-02-13T01:01:46Z- AHRS_M2 msg: 1613178106.68 epoch sec; seqNo:17754 2021-02-13T01:01:46Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.20 psi 2021-02-13T01:01:46Z- handleDepth()-Depth_Keller msg: 1613178106698 epoch sec; seqNo:17895; depth 18.81 2021-02-13T01:01:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178106.73 epoch sec; seqNo:17895; 4060653.53 north; 593559.49 east 2021-02-13T01:01:47Z- AHRS_M2 msg: 1613178107.08 epoch sec; seqNo:17755 2021-02-13T01:01:47Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.19 psi 2021-02-13T01:01:47Z- handleDepth()-Depth_Keller msg: 1613178107088 epoch sec; seqNo:17896; depth 18.92 2021-02-13T01:01:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:47Z- handleDvl() - RDI_Pathfinder msg: 1613178107.09 epoch sec; seqNo:8651 2021-02-13T01:01:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.01 , 0.09 2021-02-13T01:01:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178107.10 epoch sec; seqNo:17896; 4060653.53 north; 593559.49 east 2021-02-13T01:01:47Z- AHRS_M2 msg: 1613178107.49 epoch sec; seqNo:17756 2021-02-13T01:01:47Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.19 psi 2021-02-13T01:01:47Z- handleDepth()-Depth_Keller msg: 1613178107493 epoch sec; seqNo:17897; depth 19.05 2021-02-13T01:01:47Z- handleDvl() - RDI_Pathfinder msg: 1613178107.51 epoch sec; seqNo:8652 2021-02-13T01:01:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 78.58 , 105.36 2021-02-13T01:01:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.06 , 0.13 2021-02-13T01:01:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178107.52 epoch sec; seqNo:17897; 4060653.91 north; 593559.49 east 2021-02-13T01:01:47Z- AHRS_M2 msg: 1613178107.89 epoch sec; seqNo:17757 2021-02-13T01:01:47Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.18 psi 2021-02-13T01:01:47Z- handleDepth()-Depth_Keller msg: 1613178107896 epoch sec; seqNo:17898; depth 19.19 2021-02-13T01:01:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178107.91 epoch sec; seqNo:17898; 4060654.30 north; 593560.71 east 2021-02-13T01:01:48Z- AHRS_M2 msg: 1613178108.30 epoch sec; seqNo:17758 2021-02-13T01:01:48Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.18 psi 2021-02-13T01:01:48Z- handleDepth()-Depth_Keller msg: 1613178108301 epoch sec; seqNo:17899; depth 19.30 2021-02-13T01:01:48Z- handleDvl() - RDI_Pathfinder msg: 1613178108.31 epoch sec; seqNo:8653 2021-02-13T01:01:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 92.92 , 69.89 , 103.87 2021-02-13T01:01:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.14 , 0.08 2021-02-13T01:01:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178108.34 epoch sec; seqNo:17899; 4060654.68 north; 593560.70 east 2021-02-13T01:01:48Z- AHRS_M2 msg: 1613178108.70 epoch sec; seqNo:17759 2021-02-13T01:01:48Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.18 psi 2021-02-13T01:01:48Z- handleDepth()-Depth_Keller msg: 1613178108704 epoch sec; seqNo:17900; depth 19.45 2021-02-13T01:01:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178108.72 epoch sec; seqNo:17900; 4060654.68 north; 593560.70 east 2021-02-13T01:01:49Z- AHRS_M2 msg: 1613178109.11 epoch sec; seqNo:17760 2021-02-13T01:01:49Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.17 psi 2021-02-13T01:01:49Z- handleDepth()-Depth_Keller msg: 1613178109117 epoch sec; seqNo:17901; depth 19.55 2021-02-13T01:01:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178109.13 epoch sec; seqNo:17901; 4060655.06 north; 593560.70 east 2021-02-13T01:01:49Z- AHRS_M2 msg: 1613178109.51 epoch sec; seqNo:17761 2021-02-13T01:01:49Z- AHRS_M2 msg: 0.56 phi; -0.33 theta; -0.18 psi 2021-02-13T01:01:49Z- handleDepth()-Depth_Keller msg: 1613178109518 epoch sec; seqNo:17902; depth 19.67 2021-02-13T01:01:49Z- handleDvl() - RDI_Pathfinder msg: 1613178109.54 epoch sec; seqNo:8654 2021-02-13T01:01:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.02 , 97.85 , 67.44 , 102.72 2021-02-13T01:01:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.18 , -0.03 2021-02-13T01:01:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178109.55 epoch sec; seqNo:17902; 4060655.44 north; 593560.69 east 2021-02-13T01:01:49Z- AHRS_M2 msg: 1613178109.91 epoch sec; seqNo:17762 2021-02-13T01:01:49Z- AHRS_M2 msg: 0.56 phi; -0.33 theta; -0.19 psi 2021-02-13T01:01:49Z- handleDepth()-Depth_Keller msg: 1613178109925 epoch sec; seqNo:17903; depth 19.81 2021-02-13T01:01:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178109.95 epoch sec; seqNo:17903; 4060655.45 north; 593561.91 east 2021-02-13T01:01:50Z- AHRS_M2 msg: 1613178110.32 epoch sec; seqNo:17763 2021-02-13T01:01:50Z- AHRS_M2 msg: 0.56 phi; -0.33 theta; -0.19 psi 2021-02-13T01:01:50Z- handleDepth()-Depth_Keller msg: 1613178110325 epoch sec; seqNo:17904; depth 19.92 2021-02-13T01:01:50Z- handleDvl() - RDI_Pathfinder msg: 1613178110.34 epoch sec; seqNo:8655 2021-02-13T01:01:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 93.25 , 69.07 , 104.21 2021-02-13T01:01:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.13 , -0.13 2021-02-13T01:01:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178110.35 epoch sec; seqNo:17904; 4060655.83 north; 593561.91 east 2021-02-13T01:01:50Z- AHRS_M2 msg: 1613178110.72 epoch sec; seqNo:17764 2021-02-13T01:01:50Z- AHRS_M2 msg: 0.56 phi; -0.33 theta; -0.20 psi 2021-02-13T01:01:50Z- handleDepth()-Depth_Keller msg: 1613178110725 epoch sec; seqNo:17905; depth 20.05 2021-02-13T01:01:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:50Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:50Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:50Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:50Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:50Z- handleDvl() - RDI_Pathfinder msg: 1613178110.73 epoch sec; seqNo:8656 2021-02-13T01:01:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.13 , -0.13 2021-02-13T01:01:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178110.74 epoch sec; seqNo:17905; 4060656.21 north; 593561.90 east 2021-02-13T01:01:51Z- AHRS_M2 msg: 1613178111.13 epoch sec; seqNo:17765 2021-02-13T01:01:51Z- AHRS_M2 msg: 0.56 phi; -0.33 theta; -0.21 psi 2021-02-13T01:01:51Z- handleDepth()-Depth_Keller msg: 1613178111139 epoch sec; seqNo:17906; depth 20.15 2021-02-13T01:01:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178111.15 epoch sec; seqNo:17906; 4060656.59 north; 593561.90 east 2021-02-13T01:01:51Z- LcmTrn::handleMessages() - handling 46 messages took 0.74 ms and 132 calls... 2021-02-13T01:01:51Z- waiting for 1613178108.89, time is 1613178111.15 2021-02-13T01:01:51Z- LcmTrn::updateTrn() >>>> pose time 1613178111.15, meas time 1613178110.73 <<<< 2021-02-13T01:01:51Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613178110.73, ping # 0. 2021-02-13T01:01:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178110.73 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:01:51Z- TerrainNav::motionUpdate() - currEstimate = (4060657, 593561.90, 20.15) 2021-02-13T01:01:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:01:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:01:51Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:01:51Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:01:51Z- TNavBF::Projecting Measurements 2021-02-13T01:01:51Z- TNavFilter::Measurement from time = 1613178110.73 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T01:01:51Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:01:51Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:01:51Z- TNavBF:: beamIndex[1] = 0 2021-02-13T01:01:51Z- TNavBF:: beamIndex[2] = 0 2021-02-13T01:01:51Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:01:51Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:01:51Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:01:51Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 678.5 EastVariance: 833.0 DepthVariance: 1.6 this->DepthBiasCov: 1.6 2021-02-13T01:01:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:01:51Z- TNavBF::mmse selected from filter -1 2021-02-13T01:01:51Z- variances([0], [2], and psi_berg) = 678.498233 832.987846 0.000000 2021-02-13T01:01:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:01:51Z- LcmTrn::publishEstimates() - MLE : 4060656.09 593578.40 20.15 2021-02-13T01:01:51Z- LcmTrn::publishEstimates() - MMSE : 4060656.59 593561.90 20.15 2021-02-13T01:01:51Z- LcmTrn::publishEstimates() - COVAR: 678.50 832.99 1.64 0.00 2021-02-13T01:01:51Z- AHRS_M2 msg: 1613178111.53 epoch sec; seqNo:17766 2021-02-13T01:01:51Z- AHRS_M2 msg: 0.56 phi; -0.32 theta; -0.21 psi 2021-02-13T01:01:51Z- handleDepth()-Depth_Keller msg: 1613178111532 epoch sec; seqNo:17907; depth 20.27 2021-02-13T01:01:51Z- handleDvl() - RDI_Pathfinder msg: 1613178111.54 epoch sec; seqNo:8657 2021-02-13T01:01:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 98.12 , 78.65 , 104.21 2021-02-13T01:01:51Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.02 , -0.17 2021-02-13T01:01:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178111.57 epoch sec; seqNo:17907; 4060656.97 north; 593561.90 east 2021-02-13T01:01:51Z- AHRS_M2 msg: 1613178111.93 epoch sec; seqNo:17767 2021-02-13T01:01:51Z- AHRS_M2 msg: 0.56 phi; -0.32 theta; -0.21 psi 2021-02-13T01:01:51Z- handleDepth()-Depth_Keller msg: 1613178111936 epoch sec; seqNo:17908; depth 20.40 2021-02-13T01:01:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:51Z- handleDvl() - RDI_Pathfinder msg: 1613178111.94 epoch sec; seqNo:8658 2021-02-13T01:01:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:51Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.02 , -0.17 2021-02-13T01:01:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178111.95 epoch sec; seqNo:17908; 4060657.36 north; 593563.11 east 2021-02-13T01:01:52Z- AHRS_M2 msg: 1613178112.34 epoch sec; seqNo:17768 2021-02-13T01:01:52Z- AHRS_M2 msg: 0.56 phi; -0.32 theta; -0.21 psi 2021-02-13T01:01:52Z- handleDepth()-Depth_Keller msg: 1613178112349 epoch sec; seqNo:17909; depth 20.52 2021-02-13T01:01:52Z- handleDvl() - RDI_Pathfinder msg: 1613178112.36 epoch sec; seqNo:8659 2021-02-13T01:01:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.57 , 97.78 , 66.62 , 102.65 2021-02-13T01:01:52Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.07 , -0.13 2021-02-13T01:01:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178112.37 epoch sec; seqNo:17909; 4060657.74 north; 593563.11 east 2021-02-13T01:01:52Z- AHRS_M2 msg: 1613178112.74 epoch sec; seqNo:17769 2021-02-13T01:01:52Z- AHRS_M2 msg: 0.56 phi; -0.32 theta; -0.20 psi 2021-02-13T01:01:52Z- handleDepth()-Depth_Keller msg: 1613178112744 epoch sec; seqNo:17910; depth 20.63 2021-02-13T01:01:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178112.76 epoch sec; seqNo:17910; 4060657.74 north; 593563.11 east 2021-02-13T01:01:53Z- AHRS_M2 msg: 1613178113.17 epoch sec; seqNo:17770 2021-02-13T01:01:53Z- AHRS_M2 msg: 0.56 phi; -0.32 theta; -0.19 psi 2021-02-13T01:01:53Z- handleDepth()-Depth_Keller msg: 1613178113172 epoch sec; seqNo:17911; depth 20.75 2021-02-13T01:01:53Z- handleDvl() - RDI_Pathfinder msg: 1613178113.19 epoch sec; seqNo:8660 2021-02-13T01:01:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.75 , 98.19 , 68.26 , 101.84 2021-02-13T01:01:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.14 , -0.05 2021-02-13T01:01:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178113.20 epoch sec; seqNo:17911; 4060658.12 north; 593563.10 east 2021-02-13T01:01:53Z- handleDepth()-Depth_Keller msg: 1613178113569 epoch sec; seqNo:17912; depth 20.91 2021-02-13T01:01:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178113.58 epoch sec; seqNo:17912; 4060658.50 north; 593563.10 east 2021-02-13T01:01:53Z- AHRS_M2 msg: 1613178113.95 epoch sec; seqNo:17771 2021-02-13T01:01:53Z- AHRS_M2 msg: 0.56 phi; -0.32 theta; -0.18 psi 2021-02-13T01:01:53Z- handleDepth()-Depth_Keller msg: 1613178113957 epoch sec; seqNo:17913; depth 21.01 2021-02-13T01:01:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178113.98 epoch sec; seqNo:17913; 4060658.88 north; 593563.10 east 2021-02-13T01:01:54Z- AHRS_M2 msg: 1613178114.36 epoch sec; seqNo:17772 2021-02-13T01:01:54Z- AHRS_M2 msg: 0.56 phi; -0.32 theta; -0.17 psi 2021-02-13T01:01:54Z- handleDepth()-Depth_Keller msg: 1613178114360 epoch sec; seqNo:17914; depth 21.15 2021-02-13T01:01:54Z- handleDvl() - RDI_Pathfinder msg: 1613178114.37 epoch sec; seqNo:8661 2021-02-13T01:01:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.17 , 98.19 , 67.86 , 100.63 2021-02-13T01:01:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , -0.02 2021-02-13T01:01:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178114.40 epoch sec; seqNo:17914; 4060659.26 north; 593563.09 east 2021-02-13T01:01:54Z- AHRS_M2 msg: 1613178114.76 epoch sec; seqNo:17773 2021-02-13T01:01:54Z- AHRS_M2 msg: 0.56 phi; -0.32 theta; -0.17 psi 2021-02-13T01:01:54Z- handleDepth()-Depth_Keller msg: 1613178114764 epoch sec; seqNo:17915; depth 21.25 2021-02-13T01:01:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178114.78 epoch sec; seqNo:17915; 4060659.65 north; 593564.31 east 2021-02-13T01:01:55Z- AHRS_M2 msg: 1613178115.18 epoch sec; seqNo:17774 2021-02-13T01:01:55Z- AHRS_M2 msg: 0.56 phi; -0.32 theta; -0.17 psi 2021-02-13T01:01:55Z- handleDepth()-Depth_Keller msg: 1613178115186 epoch sec; seqNo:17916; depth 21.37 2021-02-13T01:01:55Z- handleDvl() - RDI_Pathfinder msg: 1613178115.20 epoch sec; seqNo:8662 2021-02-13T01:01:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 0.00 , 65.81 , 99.41 2021-02-13T01:01:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.10 , 0.03 2021-02-13T01:01:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178115.21 epoch sec; seqNo:17916; 4060660.03 north; 593564.30 east 2021-02-13T01:01:55Z- AHRS_M2 msg: 1613178115.57 epoch sec; seqNo:17775 2021-02-13T01:01:55Z- AHRS_M2 msg: 0.56 phi; -0.32 theta; -0.17 psi 2021-02-13T01:01:55Z- handleDepth()-Depth_Keller msg: 1613178115571 epoch sec; seqNo:17917; depth 21.51 2021-02-13T01:01:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:55Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:55Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:55Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:55Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:55Z- handleDvl() - RDI_Pathfinder msg: 1613178115.57 epoch sec; seqNo:8663 2021-02-13T01:01:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.10 , 0.03 2021-02-13T01:01:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178115.60 epoch sec; seqNo:17917; 4060660.03 north; 593564.30 east 2021-02-13T01:01:55Z- AHRS_M2 msg: 1613178115.97 epoch sec; seqNo:17776 2021-02-13T01:01:55Z- AHRS_M2 msg: 0.56 phi; -0.32 theta; -0.17 psi 2021-02-13T01:01:55Z- handleDepth()-Depth_Keller msg: 1613178115981 epoch sec; seqNo:17918; depth 21.61 2021-02-13T01:01:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178115.99 epoch sec; seqNo:17918; 4060660.41 north; 593564.30 east 2021-02-13T01:01:56Z- LcmTrn::handleMessages() - handling 42 messages took 0.52 ms and 132 calls... 2021-02-13T01:01:56Z- waiting for 1613178114.15, time is 1613178115.99 2021-02-13T01:01:56Z- LcmTrn::updateTrn() >>>> pose time 1613178115.99, meas time 1613178115.57 <<<< 2021-02-13T01:01:56Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613178115.57, ping # 0. 2021-02-13T01:01:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178115.57 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:01:56Z- TerrainNav::motionUpdate() - currEstimate = (4060660, 593564.30, 21.61) 2021-02-13T01:01:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:01:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:01:56Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:01:56Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:01:56Z- TNavBF::Projecting Measurements 2021-02-13T01:01:56Z- TNavFilter::Measurement from time = 1613178115.57 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T01:01:56Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:01:56Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:01:56Z- TNavBF:: beamIndex[1] = 0 2021-02-13T01:01:56Z- TNavBF:: beamIndex[2] = 0 2021-02-13T01:01:56Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:01:56Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:01:56Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:01:56Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 678.5 EastVariance: 833.0 DepthVariance: 1.9 this->DepthBiasCov: 1.9 2021-02-13T01:01:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:01:56Z- TNavBF::mmse selected from filter -1 2021-02-13T01:01:56Z- variances([0], [2], and psi_berg) = 678.440933 832.987822 0.000000 2021-02-13T01:01:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:01:56Z- LcmTrn::publishEstimates() - MLE : 4060659.91 593580.80 21.61 2021-02-13T01:01:56Z- LcmTrn::publishEstimates() - MMSE : 4060660.41 593564.30 21.61 2021-02-13T01:01:56Z- LcmTrn::publishEstimates() - COVAR: 678.44 832.99 1.87 0.00 2021-02-13T01:01:56Z- AHRS_M2 msg: 1613178116.38 epoch sec; seqNo:17777 2021-02-13T01:01:56Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.17 psi 2021-02-13T01:01:56Z- handleDepth()-Depth_Keller msg: 1613178116385 epoch sec; seqNo:17919; depth 21.74 2021-02-13T01:01:56Z- handleDvl() - RDI_Pathfinder msg: 1613178116.40 epoch sec; seqNo:8664 2021-02-13T01:01:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.16 , 0.00 , 67.03 , 100.01 2021-02-13T01:01:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.07 , 0.05 2021-02-13T01:01:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178116.41 epoch sec; seqNo:17919; 4060660.78 north; 593564.30 east 2021-02-13T01:01:56Z- AHRS_M2 msg: 1613178116.78 epoch sec; seqNo:17778 2021-02-13T01:01:56Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.18 psi 2021-02-13T01:01:56Z- handleDepth()-Depth_Keller msg: 1613178116786 epoch sec; seqNo:17920; depth 21.85 2021-02-13T01:01:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:56Z- handleDvl() - RDI_Pathfinder msg: 1613178116.79 epoch sec; seqNo:8665 2021-02-13T01:01:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.07 , 0.05 2021-02-13T01:01:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178116.80 epoch sec; seqNo:17920; 4060661.16 north; 593564.29 east 2021-02-13T01:01:57Z- AHRS_M2 msg: 1613178117.19 epoch sec; seqNo:17779 2021-02-13T01:01:57Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.18 psi 2021-02-13T01:01:57Z- handleDepth()-Depth_Keller msg: 1613178117197 epoch sec; seqNo:17921; depth 21.99 2021-02-13T01:01:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178117.22 epoch sec; seqNo:17921; 4060661.16 north; 593564.29 east 2021-02-13T01:01:57Z- AHRS_M2 msg: 1613178117.59 epoch sec; seqNo:17780 2021-02-13T01:01:57Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.18 psi 2021-02-13T01:01:57Z- handleDepth()-Depth_Keller msg: 1613178117592 epoch sec; seqNo:17922; depth 22.10 2021-02-13T01:01:57Z- handleDvl() - RDI_Pathfinder msg: 1613178117.60 epoch sec; seqNo:8666 2021-02-13T01:01:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 89.26 , 66.62 , 100.08 2021-02-13T01:01:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , 0.09 2021-02-13T01:01:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178117.61 epoch sec; seqNo:17922; 4060661.54 north; 593564.29 east 2021-02-13T01:01:57Z- AHRS_M2 msg: 1613178117.99 epoch sec; seqNo:17781 2021-02-13T01:01:57Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.18 psi 2021-02-13T01:01:57Z- handleDepth()-Depth_Keller msg: 1613178117998 epoch sec; seqNo:17923; depth 22.22 2021-02-13T01:01:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:01:58Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:01:58Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:01:58Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:01:58Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:01:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:01:58Z- handleDvl() - RDI_Pathfinder msg: 1613178118.00 epoch sec; seqNo:8667 2021-02-13T01:01:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:01:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , 0.09 2021-02-13T01:01:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178118.01 epoch sec; seqNo:17923; 4060661.93 north; 593565.50 east 2021-02-13T01:01:58Z- AHRS_M2 msg: 1613178118.39 epoch sec; seqNo:17782 2021-02-13T01:01:58Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.18 psi 2021-02-13T01:01:58Z- handleDepth()-Depth_Keller msg: 1613178118399 epoch sec; seqNo:17924; depth 22.35 2021-02-13T01:01:58Z- handleDvl() - RDI_Pathfinder msg: 1613178118.42 epoch sec; seqNo:8668 2021-02-13T01:01:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 89.80 , 65.81 , 99.54 2021-02-13T01:01:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.13 2021-02-13T01:01:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178118.43 epoch sec; seqNo:17924; 4060662.31 north; 593565.50 east 2021-02-13T01:01:58Z- AHRS_M2 msg: 1613178118.80 epoch sec; seqNo:17783 2021-02-13T01:01:58Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.18 psi 2021-02-13T01:01:58Z- handleDepth()-Depth_Keller msg: 1613178118804 epoch sec; seqNo:17925; depth 22.48 2021-02-13T01:01:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178118.82 epoch sec; seqNo:17925; 4060662.31 north; 593565.50 east 2021-02-13T01:01:59Z- AHRS_M2 msg: 1613178119.20 epoch sec; seqNo:17784 2021-02-13T01:01:59Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.18 psi 2021-02-13T01:01:59Z- handleDepth()-Depth_Keller msg: 1613178119219 epoch sec; seqNo:17926; depth 22.59 2021-02-13T01:01:59Z- handleDvl() - RDI_Pathfinder msg: 1613178119.23 epoch sec; seqNo:8669 2021-02-13T01:01:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.94 , 88.45 , 66.21 , 98.19 2021-02-13T01:01:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , 0.14 2021-02-13T01:01:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178119.24 epoch sec; seqNo:17926; 4060662.69 north; 593565.50 east 2021-02-13T01:01:59Z- AHRS_M2 msg: 1613178119.61 epoch sec; seqNo:17785 2021-02-13T01:01:59Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.18 psi 2021-02-13T01:01:59Z- handleDepth()-Depth_Keller msg: 1613178119615 epoch sec; seqNo:17927; depth 22.71 2021-02-13T01:01:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178119.63 epoch sec; seqNo:17927; 4060663.07 north; 593565.49 east 2021-02-13T01:02:00Z- AHRS_M2 msg: 1613178120.01 epoch sec; seqNo:17786 2021-02-13T01:02:00Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.17 psi 2021-02-13T01:02:00Z- handleDepth()-Depth_Keller msg: 1613178120016 epoch sec; seqNo:17928; depth 22.83 2021-02-13T01:02:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178120.04 epoch sec; seqNo:17928; 4060663.45 north; 593565.49 east 2021-02-13T01:02:00Z- AHRS_M2 msg: 1613178120.41 epoch sec; seqNo:17787 2021-02-13T01:02:00Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.17 psi 2021-02-13T01:02:00Z- handleDepth()-Depth_Keller msg: 1613178120425 epoch sec; seqNo:17929; depth 22.95 2021-02-13T01:02:00Z- handleDvl() - RDI_Pathfinder msg: 1613178120.44 epoch sec; seqNo:8670 2021-02-13T01:02:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 169.25 , 78.38 , 98.93 2021-02-13T01:02:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.46 , -1.84 , 0.39 2021-02-13T01:02:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178120.46 epoch sec; seqNo:17929; 4060663.83 north; 593565.48 east 2021-02-13T01:02:00Z- AHRS_M2 msg: 1613178120.82 epoch sec; seqNo:17788 2021-02-13T01:02:00Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.17 psi 2021-02-13T01:02:00Z- handleDepth()-Depth_Keller msg: 1613178120824 epoch sec; seqNo:17930; depth 23.09 2021-02-13T01:02:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178120.84 epoch sec; seqNo:17930; 4060664.19 north; 593564.26 east 2021-02-13T01:02:01Z- AHRS_M2 msg: 1613178121.22 epoch sec; seqNo:17789 2021-02-13T01:02:01Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.17 psi 2021-02-13T01:02:01Z- handleDepth()-Depth_Keller msg: 1613178121234 epoch sec; seqNo:17931; depth 23.21 2021-02-13T01:02:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178121.24 epoch sec; seqNo:17931; 4060664.57 north; 593564.25 east 2021-02-13T01:02:01Z- LcmTrn::handleMessages() - handling 46 messages took 0.78 ms and 131 calls... 2021-02-13T01:02:01Z- waiting for 1613178118.99, time is 1613178121.24 2021-02-13T01:02:01Z- LcmTrn::updateTrn() >>>> pose time 1613178121.24, meas time 1613178120.44 <<<< 2021-02-13T01:02:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178120.44 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:02:01Z- TerrainNav::motionUpdate() - currEstimate = (4060665, 593564.25, 23.21) 2021-02-13T01:02:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:02:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:02:01Z- TNavBF::Projecting Measurements 2021-02-13T01:02:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:02:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:02:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:02:01Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:02:01Z- TNavFilter::3 good beams. 2021-02-13T01:02:01Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:02:01Z- TNavBF:: beamIndex[0] = 1 2021-02-13T01:02:01Z- TNavBF:: beamIndex[1] = 2 2021-02-13T01:02:01Z- TNavBF:: beamIndex[2] = 3 2021-02-13T01:02:01Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:02:01Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:02:01Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:02:01Z- TNavBF::mmse selected from filter -1 2021-02-13T01:02:01Z- TerrainMapOctree: (vehN, vehE ) = (4060663.93, 593564.26). 2021-02-13T01:02:01Z- TerrainMapOctree: Min Distance = 902.06. 2021-02-13T01:02:01Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:02:01Z- There is no specific good beam that all particles have in common. 2021-02-13T01:02:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:02:01Z- Old Depth Bias: 0.00 DepthBiasCov: 2.05 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 45.84 2021-02-13T01:02:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:02:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.97 K: 0.146 ExpectedMeasDiff: nan currMeas.covariance: 9.83 2021-02-13T01:02:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:02:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.68 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 15.66 2021-02-13T01:02:01Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:02:01Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:02:01Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:02:01Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:02:01Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:02:01Z- Filter 0 No beams used in subcloud update 2021-02-13T01:02:01Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:02:01Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:02:01Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 678.4 EastVariance: 833.0 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-13T01:02:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178120.44 sec. 2021-02-13T01:02:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:02:01Z- TNavBF::mmse selected from filter -1 2021-02-13T01:02:01Z- variances([0], [2], and psi_berg) = 678.383113 832.987798 0.000000 2021-02-13T01:02:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:02:01Z- LcmTrn::publishEstimates() - MLE : 4060664.07 593580.75 23.21 2021-02-13T01:02:01Z- LcmTrn::publishEstimates() - MMSE : 4060664.57 593564.25 23.21 2021-02-13T01:02:01Z- LcmTrn::publishEstimates() - COVAR: 678.38 832.99 1.57 0.00 2021-02-13T01:02:01Z- AHRS_M2 msg: 1613178121.63 epoch sec; seqNo:17790 2021-02-13T01:02:01Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.17 psi 2021-02-13T01:02:01Z- handleDepth()-Depth_Keller msg: 1613178121637 epoch sec; seqNo:17932; depth 23.33 2021-02-13T01:02:01Z- handleDvl() - RDI_Pathfinder msg: 1613178121.66 epoch sec; seqNo:8671 2021-02-13T01:02:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.29 , 97.24 , 63.76 , 100.22 2021-02-13T01:02:01Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.10 , 0.06 2021-02-13T01:02:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178121.67 epoch sec; seqNo:17932; 4060664.95 north; 593564.25 east 2021-02-13T01:02:02Z- AHRS_M2 msg: 1613178122.03 epoch sec; seqNo:17791 2021-02-13T01:02:02Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.17 psi 2021-02-13T01:02:02Z- handleDepth()-Depth_Keller msg: 1613178122045 epoch sec; seqNo:17933; depth 23.46 2021-02-13T01:02:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178122.06 epoch sec; seqNo:17933; 4060665.33 north; 593564.25 east 2021-02-13T01:02:02Z- AHRS_M2 msg: 1613178122.44 epoch sec; seqNo:17792 2021-02-13T01:02:02Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.17 psi 2021-02-13T01:02:02Z- handleDepth()-Depth_Keller msg: 1613178122440 epoch sec; seqNo:17934; depth 23.57 2021-02-13T01:02:02Z- handleDvl() - RDI_Pathfinder msg: 1613178122.46 epoch sec; seqNo:8672 2021-02-13T01:02:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:02Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178122.48 epoch sec; seqNo:17934; 4060665.72 north; 593565.46 east 2021-02-13T01:02:02Z- AHRS_M2 msg: 1613178122.84 epoch sec; seqNo:17793 2021-02-13T01:02:02Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.17 psi 2021-02-13T01:02:02Z- handleDepth()-Depth_Keller msg: 1613178122844 epoch sec; seqNo:17935; depth 23.69 2021-02-13T01:02:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:02Z- handleDvl() - RDI_Pathfinder msg: 1613178122.85 epoch sec; seqNo:8673 2021-02-13T01:02:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:02Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178122.86 epoch sec; seqNo:17935; 4060665.72 north; 593565.46 east 2021-02-13T01:02:03Z- AHRS_M2 msg: 1613178123.24 epoch sec; seqNo:17794 2021-02-13T01:02:03Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.17 psi 2021-02-13T01:02:03Z- handleDepth()-Depth_Keller msg: 1613178123253 epoch sec; seqNo:17936; depth 23.82 2021-02-13T01:02:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178123.26 epoch sec; seqNo:17936; 4060666.10 north; 593565.46 east 2021-02-13T01:02:03Z- AHRS_M2 msg: 1613178123.65 epoch sec; seqNo:17795 2021-02-13T01:02:03Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.17 psi 2021-02-13T01:02:03Z- handleDepth()-Depth_Keller msg: 1613178123657 epoch sec; seqNo:17937; depth 23.95 2021-02-13T01:02:03Z- handleDvl() - RDI_Pathfinder msg: 1613178123.67 epoch sec; seqNo:8674 2021-02-13T01:02:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.17 , 88.59 , 63.76 , 99.00 2021-02-13T01:02:03Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.10 , -0.08 2021-02-13T01:02:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178123.68 epoch sec; seqNo:17937; 4060666.48 north; 593565.45 east 2021-02-13T01:02:04Z- AHRS_M2 msg: 1613178124.05 epoch sec; seqNo:17796 2021-02-13T01:02:04Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.17 psi 2021-02-13T01:02:04Z- handleDepth()-Depth_Keller msg: 1613178124056 epoch sec; seqNo:17938; depth 24.07 2021-02-13T01:02:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:04Z- handleDvl() - RDI_Pathfinder msg: 1613178124.06 epoch sec; seqNo:8675 2021-02-13T01:02:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:04Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.10 , -0.08 2021-02-13T01:02:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178124.07 epoch sec; seqNo:17938; 4060666.86 north; 593565.45 east 2021-02-13T01:02:04Z- AHRS_M2 msg: 1613178124.45 epoch sec; seqNo:17797 2021-02-13T01:02:04Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.17 psi 2021-02-13T01:02:04Z- handleDepth()-Depth_Keller msg: 1613178124465 epoch sec; seqNo:17939; depth 24.19 2021-02-13T01:02:04Z- handleDvl() - RDI_Pathfinder msg: 1613178124.48 epoch sec; seqNo:8676 2021-02-13T01:02:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.30 , 87.44 , 64.99 , 98.39 2021-02-13T01:02:04Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.02 , -0.13 2021-02-13T01:02:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178124.49 epoch sec; seqNo:17939; 4060667.25 north; 593566.67 east 2021-02-13T01:02:04Z- AHRS_M2 msg: 1613178124.86 epoch sec; seqNo:17798 2021-02-13T01:02:04Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.17 psi 2021-02-13T01:02:04Z- handleDepth()-Depth_Keller msg: 1613178124866 epoch sec; seqNo:17940; depth 24.31 2021-02-13T01:02:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178124.88 epoch sec; seqNo:17940; 4060667.25 north; 593566.67 east 2021-02-13T01:02:05Z- AHRS_M2 msg: 1613178125.26 epoch sec; seqNo:17799 2021-02-13T01:02:05Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.17 psi 2021-02-13T01:02:05Z- handleDepth()-Depth_Keller msg: 1613178125268 epoch sec; seqNo:17941; depth 24.43 2021-02-13T01:02:05Z- handleDvl() - RDI_Pathfinder msg: 1613178125.29 epoch sec; seqNo:8677 2021-02-13T01:02:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.71 , 87.10 , 64.58 , 97.92 2021-02-13T01:02:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , -0.14 2021-02-13T01:02:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178125.31 epoch sec; seqNo:17941; 4060667.63 north; 593566.66 east 2021-02-13T01:02:05Z- AHRS_M2 msg: 1613178125.67 epoch sec; seqNo:17800 2021-02-13T01:02:05Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T01:02:05Z- handleDepth()-Depth_Keller msg: 1613178125672 epoch sec; seqNo:17942; depth 24.59 2021-02-13T01:02:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178125.68 epoch sec; seqNo:17942; 4060668.01 north; 593566.66 east 2021-02-13T01:02:06Z- AHRS_M2 msg: 1613178126.07 epoch sec; seqNo:17801 2021-02-13T01:02:06Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T01:02:06Z- handleDepth()-Depth_Keller msg: 1613178126077 epoch sec; seqNo:17943; depth 24.69 2021-02-13T01:02:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178126.09 epoch sec; seqNo:17943; 4060668.39 north; 593566.65 east 2021-02-13T01:02:06Z- AHRS_M2 msg: 1613178126.48 epoch sec; seqNo:17802 2021-02-13T01:02:06Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.18 psi 2021-02-13T01:02:06Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T01:02:06Z- waiting for 1613178124.24, time is 1613178126.48 2021-02-13T01:02:06Z- LcmTrn::updateTrn() >>>> pose time 1613178126.48, meas time 1613178125.29 <<<< 2021-02-13T01:02:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178125.29 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:02:06Z- TerrainNav::motionUpdate() - currEstimate = (4060668, 593566.65, 24.69) 2021-02-13T01:02:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:02:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:02:06Z- TNavBF::Projecting Measurements 2021-02-13T01:02:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:02:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:02:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:02:06Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:02:06Z- TNavFilter::3 good beams. 2021-02-13T01:02:06Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:02:06Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:02:06Z- TNavBF:: beamIndex[1] = 2 2021-02-13T01:02:06Z- TNavBF:: beamIndex[2] = 3 2021-02-13T01:02:06Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:02:06Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:02:06Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:02:06Z- TNavBF::mmse selected from filter -1 2021-02-13T01:02:06Z- TerrainMapOctree: (vehN, vehE ) = (4060667.52, 593566.11). 2021-02-13T01:02:06Z- TerrainMapOctree: Min Distance = 898.33. 2021-02-13T01:02:06Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:02:06Z- There is no specific good beam that all particles have in common. 2021-02-13T01:02:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:02:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.74 K: 0.152 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-13T01:02:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:02:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.48 K: 0.150 ExpectedMeasDiff: nan currMeas.covariance: 6.67 2021-02-13T01:02:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:02:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.069 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T01:02:06Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:02:06Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:02:06Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:02:06Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:02:06Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:02:06Z- Filter 0 No beams used in subcloud update 2021-02-13T01:02:06Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:02:06Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:02:06Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 678.3 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T01:02:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178125.29 sec. 2021-02-13T01:02:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:02:06Z- TNavBF::mmse selected from filter -1 2021-02-13T01:02:06Z- variances([0], [2], and psi_berg) = 678.321004 832.987772 0.000000 2021-02-13T01:02:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:02:06Z- LcmTrn::publishEstimates() - MLE : 4060667.89 593583.15 24.69 2021-02-13T01:02:06Z- LcmTrn::publishEstimates() - MMSE : 4060668.39 593566.65 24.69 2021-02-13T01:02:06Z- LcmTrn::publishEstimates() - COVAR: 678.32 832.99 1.22 0.00 2021-02-13T01:02:06Z- handleDepth()-Depth_Keller msg: 1613178126491 epoch sec; seqNo:17944; depth 24.82 2021-02-13T01:02:06Z- handleDvl() - RDI_Pathfinder msg: 1613178126.51 epoch sec; seqNo:8678 2021-02-13T01:02:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.90 , 87.10 , 63.77 , 97.92 2021-02-13T01:02:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , -0.13 2021-02-13T01:02:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178126.52 epoch sec; seqNo:17944; 4060668.77 north; 593566.65 east 2021-02-13T01:02:06Z- AHRS_M2 msg: 1613178126.88 epoch sec; seqNo:17803 2021-02-13T01:02:06Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.19 psi 2021-02-13T01:02:06Z- handleDepth()-Depth_Keller msg: 1613178126889 epoch sec; seqNo:17945; depth 24.95 2021-02-13T01:02:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178126.90 epoch sec; seqNo:17945; 4060669.16 north; 593567.87 east 2021-02-13T01:02:07Z- AHRS_M2 msg: 1613178127.29 epoch sec; seqNo:17804 2021-02-13T01:02:07Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.19 psi 2021-02-13T01:02:07Z- handleDepth()-Depth_Keller msg: 1613178127292 epoch sec; seqNo:17946; depth 25.07 2021-02-13T01:02:07Z- handleDvl() - RDI_Pathfinder msg: 1613178127.31 epoch sec; seqNo:8679 2021-02-13T01:02:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.49 , 0.00 , 63.77 , 97.99 2021-02-13T01:02:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.11 , -0.06 2021-02-13T01:02:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178127.32 epoch sec; seqNo:17946; 4060669.16 north; 593567.87 east 2021-02-13T01:02:07Z- AHRS_M2 msg: 1613178127.69 epoch sec; seqNo:17805 2021-02-13T01:02:07Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.20 psi 2021-02-13T01:02:07Z- handleDepth()-Depth_Keller msg: 1613178127692 epoch sec; seqNo:17947; depth 25.19 2021-02-13T01:02:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:07Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:07Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:07Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:07Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:07Z- handleDvl() - RDI_Pathfinder msg: 1613178127.69 epoch sec; seqNo:8680 2021-02-13T01:02:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.11 , -0.06 2021-02-13T01:02:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178127.71 epoch sec; seqNo:17947; 4060669.54 north; 593567.86 east 2021-02-13T01:02:08Z- AHRS_M2 msg: 1613178128.10 epoch sec; seqNo:17806 2021-02-13T01:02:08Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.20 psi 2021-02-13T01:02:08Z- handleDepth()-Depth_Keller msg: 1613178128101 epoch sec; seqNo:17948; depth 25.32 2021-02-13T01:02:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178128.13 epoch sec; seqNo:17948; 4060669.92 north; 593567.86 east 2021-02-13T01:02:08Z- AHRS_M2 msg: 1613178128.50 epoch sec; seqNo:17807 2021-02-13T01:02:08Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.21 psi 2021-02-13T01:02:08Z- handleDepth()-Depth_Keller msg: 1613178128501 epoch sec; seqNo:17949; depth 25.46 2021-02-13T01:02:08Z- handleDvl() - RDI_Pathfinder msg: 1613178128.51 epoch sec; seqNo:8681 2021-02-13T01:02:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:08Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178128.52 epoch sec; seqNo:17949; 4060670.30 north; 593567.85 east 2021-02-13T01:02:08Z- AHRS_M2 msg: 1613178128.90 epoch sec; seqNo:17808 2021-02-13T01:02:08Z- AHRS_M2 msg: 0.56 phi; -0.34 theta; -0.21 psi 2021-02-13T01:02:08Z- handleDepth()-Depth_Keller msg: 1613178128906 epoch sec; seqNo:17950; depth 25.59 2021-02-13T01:02:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:08Z- handleDvl() - RDI_Pathfinder msg: 1613178128.91 epoch sec; seqNo:8682 2021-02-13T01:02:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:08Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178128.92 epoch sec; seqNo:17950; 4060670.67 north; 593567.85 east 2021-02-13T01:02:09Z- AHRS_M2 msg: 1613178129.31 epoch sec; seqNo:17809 2021-02-13T01:02:09Z- AHRS_M2 msg: 0.56 phi; -0.34 theta; -0.21 psi 2021-02-13T01:02:09Z- handleDepth()-Depth_Keller msg: 1613178129311 epoch sec; seqNo:17951; depth 25.71 2021-02-13T01:02:09Z- handleDvl() - RDI_Pathfinder msg: 1613178129.34 epoch sec; seqNo:8683 2021-02-13T01:02:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.85 , 0.00 , 62.94 , 97.31 2021-02-13T01:02:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.12 , 0.07 2021-02-13T01:02:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178129.35 epoch sec; seqNo:17951; 4060670.67 north; 593567.85 east 2021-02-13T01:02:09Z- AHRS_M2 msg: 1613178129.71 epoch sec; seqNo:17810 2021-02-13T01:02:09Z- AHRS_M2 msg: 0.56 phi; -0.34 theta; -0.20 psi 2021-02-13T01:02:09Z- handleDepth()-Depth_Keller msg: 1613178129713 epoch sec; seqNo:17952; depth 25.86 2021-02-13T01:02:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178129.73 epoch sec; seqNo:17952; 4060671.05 north; 593567.85 east 2021-02-13T01:02:10Z- AHRS_M2 msg: 1613178130.11 epoch sec; seqNo:17811 2021-02-13T01:02:10Z- AHRS_M2 msg: 0.56 phi; -0.34 theta; -0.20 psi 2021-02-13T01:02:10Z- handleDepth()-Depth_Keller msg: 1613178130116 epoch sec; seqNo:17953; depth 25.98 2021-02-13T01:02:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:10Z- handleDvl() - RDI_Pathfinder msg: 1613178130.12 epoch sec; seqNo:8684 2021-02-13T01:02:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.12 , 0.07 2021-02-13T01:02:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178130.13 epoch sec; seqNo:17953; 4060671.43 north; 593567.84 east 2021-02-13T01:02:10Z- AHRS_M2 msg: 1613178130.52 epoch sec; seqNo:17812 2021-02-13T01:02:10Z- AHRS_M2 msg: 0.56 phi; -0.34 theta; -0.20 psi 2021-02-13T01:02:10Z- handleDepth()-Depth_Keller msg: 1613178130526 epoch sec; seqNo:17954; depth 26.10 2021-02-13T01:02:10Z- handleDvl() - RDI_Pathfinder msg: 1613178130.54 epoch sec; seqNo:8685 2021-02-13T01:02:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.67 , 88.25 , 63.35 , 97.99 2021-02-13T01:02:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , 0.10 2021-02-13T01:02:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178130.55 epoch sec; seqNo:17954; 4060671.82 north; 593569.06 east 2021-02-13T01:02:10Z- AHRS_M2 msg: 1613178130.92 epoch sec; seqNo:17813 2021-02-13T01:02:10Z- AHRS_M2 msg: 0.56 phi; -0.34 theta; -0.19 psi 2021-02-13T01:02:10Z- handleDepth()-Depth_Keller msg: 1613178130924 epoch sec; seqNo:17955; depth 26.24 2021-02-13T01:02:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178130.93 epoch sec; seqNo:17955; 4060671.82 north; 593569.06 east 2021-02-13T01:02:11Z- AHRS_M2 msg: 1613178131.33 epoch sec; seqNo:17814 2021-02-13T01:02:11Z- AHRS_M2 msg: 0.56 phi; -0.34 theta; -0.19 psi 2021-02-13T01:02:11Z- handleDepth()-Depth_Keller msg: 1613178131333 epoch sec; seqNo:17956; depth 26.36 2021-02-13T01:02:11Z- handleDvl() - RDI_Pathfinder msg: 1613178131.35 epoch sec; seqNo:8686 2021-02-13T01:02:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:11Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178131.36 epoch sec; seqNo:17956; 4060672.20 north; 593569.05 east 2021-02-13T01:02:11Z- LcmTrn::handleMessages() - handling 47 messages took 0.65 ms and 134 calls... 2021-02-13T01:02:11Z- waiting for 1613178129.48, time is 1613178131.36 2021-02-13T01:02:11Z- LcmTrn::updateTrn() equating pose 1613178131.36 and meas times 1613178131.36 2021-02-13T01:02:11Z- LcmTrn::updateTrn() >>>> pose time 1613178131.36, meas time 1613178131.36 <<<< 2021-02-13T01:02:11Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613178131.36, ping # 0. 2021-02-13T01:02:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178131.36 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:02:11Z- TerrainNav::motionUpdate() - currEstimate = (4060672, 593569.05, 26.36) 2021-02-13T01:02:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:02:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:02:11Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:02:11Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:02:11Z- TNavBF::Projecting Measurements 2021-02-13T01:02:11Z- TNavFilter::Measurement from time = 1613178131.36 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T01:02:11Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:02:11Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:02:11Z- TNavBF:: beamIndex[1] = 0 2021-02-13T01:02:11Z- TNavBF:: beamIndex[2] = 0 2021-02-13T01:02:11Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:02:11Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:02:11Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:02:11Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 678.3 EastVariance: 833.0 DepthVariance: 1.5 this->DepthBiasCov: 1.5 2021-02-13T01:02:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:02:11Z- TNavBF::mmse selected from filter -1 2021-02-13T01:02:11Z- variances([0], [2], and psi_berg) = 678.275214 832.987753 0.000000 2021-02-13T01:02:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:02:11Z- LcmTrn::publishEstimates() - MLE : 4060671.70 593585.55 26.36 2021-02-13T01:02:11Z- LcmTrn::publishEstimates() - MMSE : 4060672.20 593569.05 26.36 2021-02-13T01:02:11Z- LcmTrn::publishEstimates() - COVAR: 678.28 832.99 1.45 0.00 2021-02-13T01:02:11Z- AHRS_M2 msg: 1613178131.73 epoch sec; seqNo:17815 2021-02-13T01:02:11Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.19 psi 2021-02-13T01:02:11Z- handleDepth()-Depth_Keller msg: 1613178131735 epoch sec; seqNo:17957; depth 26.47 2021-02-13T01:02:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178131.75 epoch sec; seqNo:17957; 4060672.58 north; 593569.05 east 2021-02-13T01:02:12Z- AHRS_M2 msg: 1613178132.13 epoch sec; seqNo:17816 2021-02-13T01:02:12Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.18 psi 2021-02-13T01:02:12Z- handleDepth()-Depth_Keller msg: 1613178132145 epoch sec; seqNo:17958; depth 26.61 2021-02-13T01:02:12Z- handleDvl() - RDI_Pathfinder msg: 1613178132.17 epoch sec; seqNo:8687 2021-02-13T01:02:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 68.54 , 0.00 , 62.56 , 0.00 2021-02-13T01:02:12Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178132.18 epoch sec; seqNo:17958; 4060672.58 north; 593569.05 east 2021-02-13T01:02:12Z- AHRS_M2 msg: 1613178132.54 epoch sec; seqNo:17817 2021-02-13T01:02:12Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.18 psi 2021-02-13T01:02:12Z- handleDepth()-Depth_Keller msg: 1613178132541 epoch sec; seqNo:17959; depth 26.74 2021-02-13T01:02:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:12Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:12Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:12Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:12Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:12Z- handleDvl() - RDI_Pathfinder msg: 1613178132.54 epoch sec; seqNo:8688 2021-02-13T01:02:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:12Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178132.56 epoch sec; seqNo:17959; 4060672.96 north; 593569.05 east 2021-02-13T01:02:12Z- AHRS_M2 msg: 1613178132.94 epoch sec; seqNo:17818 2021-02-13T01:02:12Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.18 psi 2021-02-13T01:02:12Z- handleDepth()-Depth_Keller msg: 1613178132949 epoch sec; seqNo:17960; depth 26.87 2021-02-13T01:02:12Z- handleDvl() - RDI_Pathfinder msg: 1613178132.96 epoch sec; seqNo:8689 2021-02-13T01:02:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 68.08 , 0.00 , 62.56 , 0.00 2021-02-13T01:02:12Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178132.98 epoch sec; seqNo:17960; 4060673.34 north; 593569.04 east 2021-02-13T01:02:13Z- AHRS_M2 msg: 1613178133.34 epoch sec; seqNo:17819 2021-02-13T01:02:13Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.18 psi 2021-02-13T01:02:13Z- handleDepth()-Depth_Keller msg: 1613178133353 epoch sec; seqNo:17961; depth 26.99 2021-02-13T01:02:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178133.38 epoch sec; seqNo:17961; 4060673.72 north; 593569.04 east 2021-02-13T01:02:13Z- AHRS_M2 msg: 1613178133.75 epoch sec; seqNo:17820 2021-02-13T01:02:13Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.17 psi 2021-02-13T01:02:13Z- handleDepth()-Depth_Keller msg: 1613178133757 epoch sec; seqNo:17962; depth 27.12 2021-02-13T01:02:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:13Z- handleDvl() - RDI_Pathfinder msg: 1613178133.76 epoch sec; seqNo:8690 2021-02-13T01:02:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:13Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178133.77 epoch sec; seqNo:17962; 4060673.73 north; 593570.26 east 2021-02-13T01:02:14Z- AHRS_M2 msg: 1613178134.15 epoch sec; seqNo:17821 2021-02-13T01:02:14Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.17 psi 2021-02-13T01:02:14Z- handleDepth()-Depth_Keller msg: 1613178134165 epoch sec; seqNo:17963; depth 27.25 2021-02-13T01:02:14Z- handleDvl() - RDI_Pathfinder msg: 1613178134.18 epoch sec; seqNo:8691 2021-02-13T01:02:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 67.62 , 0.00 , 61.18 , 0.00 2021-02-13T01:02:14Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178134.20 epoch sec; seqNo:17963; 4060674.11 north; 593570.25 east 2021-02-13T01:02:14Z- AHRS_M2 msg: 1613178134.56 epoch sec; seqNo:17822 2021-02-13T01:02:14Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.17 psi 2021-02-13T01:02:14Z- handleDepth()-Depth_Keller msg: 1613178134565 epoch sec; seqNo:17964; depth 27.36 2021-02-13T01:02:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178134.58 epoch sec; seqNo:17964; 4060674.49 north; 593570.25 east 2021-02-13T01:02:14Z- AHRS_M2 msg: 1613178134.96 epoch sec; seqNo:17823 2021-02-13T01:02:14Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.17 psi 2021-02-13T01:02:14Z- handleDepth()-Depth_Keller msg: 1613178134969 epoch sec; seqNo:17965; depth 27.47 2021-02-13T01:02:14Z- handleDvl() - RDI_Pathfinder msg: 1613178134.99 epoch sec; seqNo:8692 2021-02-13T01:02:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:14Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178135.00 epoch sec; seqNo:17965; 4060674.87 north; 593570.25 east 2021-02-13T01:02:15Z- AHRS_M2 msg: 1613178135.36 epoch sec; seqNo:17824 2021-02-13T01:02:15Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.17 psi 2021-02-13T01:02:15Z- handleDepth()-Depth_Keller msg: 1613178135373 epoch sec; seqNo:17966; depth 27.62 2021-02-13T01:02:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178135.38 epoch sec; seqNo:17966; 4060674.87 north; 593570.25 east 2021-02-13T01:02:15Z- AHRS_M2 msg: 1613178135.77 epoch sec; seqNo:17825 2021-02-13T01:02:15Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.17 psi 2021-02-13T01:02:15Z- handleDepth()-Depth_Keller msg: 1613178135772 epoch sec; seqNo:17967; depth 27.73 2021-02-13T01:02:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178135.78 epoch sec; seqNo:17967; 4060675.25 north; 593570.24 east 2021-02-13T01:02:16Z- AHRS_M2 msg: 1613178136.19 epoch sec; seqNo:17826 2021-02-13T01:02:16Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.17 psi 2021-02-13T01:02:16Z- handleDepth()-Depth_Keller msg: 1613178136198 epoch sec; seqNo:17968; depth 27.84 2021-02-13T01:02:16Z- handleDvl() - RDI_Pathfinder msg: 1613178136.21 epoch sec; seqNo:8693 2021-02-13T01:02:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.03 , 83.92 , 61.72 , 97.99 2021-02-13T01:02:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.05 , 0.01 2021-02-13T01:02:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178136.22 epoch sec; seqNo:17968; 4060675.63 north; 593570.24 east 2021-02-13T01:02:16Z- LcmTrn::handleMessages() - handling 43 messages took 0.59 ms and 131 calls... 2021-02-13T01:02:16Z- waiting for 1613178134.36, time is 1613178136.22 2021-02-13T01:02:16Z- LcmTrn::updateTrn() equating pose 1613178136.22 and meas times 1613178136.22 2021-02-13T01:02:16Z- LcmTrn::updateTrn() >>>> pose time 1613178136.22, meas time 1613178136.22 <<<< 2021-02-13T01:02:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178136.22 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:02:16Z- TerrainNav::motionUpdate() - currEstimate = (4060676, 593570.24, 27.84) 2021-02-13T01:02:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:02:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:02:16Z- TNavBF::Projecting Measurements 2021-02-13T01:02:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:02:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:02:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:02:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:02:16Z- TNavFilter::4 good beams. 2021-02-13T01:02:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:02:16Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:02:16Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:02:16Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:02:16Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:02:16Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:02:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:02:16Z- TNavBF::mmse selected from filter -1 2021-02-13T01:02:16Z- TerrainMapOctree: (vehN, vehE ) = (4060675.63, 593570.24). 2021-02-13T01:02:16Z- TerrainMapOctree: Min Distance = 889.93. 2021-02-13T01:02:16Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:02:16Z- There is no specific good beam that all particles have in common. 2021-02-13T01:02:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:02:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.64 K: 0.155 ExpectedMeasDiff: nan currMeas.covariance: 7.19 2021-02-13T01:02:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:02:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 11.27 2021-02-13T01:02:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:02:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.138 ExpectedMeasDiff: nan currMeas.covariance: 6.09 2021-02-13T01:02:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:02:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T01:02:16Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:02:16Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:02:16Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:02:16Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:02:16Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:02:16Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:02:16Z- Filter 0 No beams used in subcloud update 2021-02-13T01:02:16Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:02:16Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:02:16Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 678.2 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T01:02:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178136.22 sec. 2021-02-13T01:02:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:02:16Z- TNavBF::mmse selected from filter -1 2021-02-13T01:02:16Z- variances([0], [2], and psi_berg) = 678.234186 832.987736 0.000000 2021-02-13T01:02:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:02:16Z- LcmTrn::publishEstimates() - MLE : 4060675.13 593587.74 27.84 2021-02-13T01:02:16Z- LcmTrn::publishEstimates() - MMSE : 4060675.63 593570.24 27.84 2021-02-13T01:02:16Z- LcmTrn::publishEstimates() - COVAR: 678.23 832.99 1.01 0.00 2021-02-13T01:02:16Z- AHRS_M2 msg: 1613178136.58 epoch sec; seqNo:17827 2021-02-13T01:02:16Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.17 psi 2021-02-13T01:02:16Z- handleDepth()-Depth_Keller msg: 1613178136585 epoch sec; seqNo:17969; depth 27.97 2021-02-13T01:02:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178136.61 epoch sec; seqNo:17969; 4060675.63 north; 593570.24 east 2021-02-13T01:02:16Z- AHRS_M2 msg: 1613178136.98 epoch sec; seqNo:17828 2021-02-13T01:02:16Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.17 psi 2021-02-13T01:02:16Z- handleDepth()-Depth_Keller msg: 1613178136984 epoch sec; seqNo:17970; depth 28.08 2021-02-13T01:02:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178136.99 epoch sec; seqNo:17970; 4060676.02 north; 593571.45 east 2021-02-13T01:02:17Z- AHRS_M2 msg: 1613178137.39 epoch sec; seqNo:17829 2021-02-13T01:02:17Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.17 psi 2021-02-13T01:02:17Z- handleDepth()-Depth_Keller msg: 1613178137398 epoch sec; seqNo:17971; depth 28.19 2021-02-13T01:02:17Z- handleDvl() - RDI_Pathfinder msg: 1613178137.41 epoch sec; seqNo:8694 2021-02-13T01:02:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.62 , 81.76 , 61.31 , 97.99 2021-02-13T01:02:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , -0.04 2021-02-13T01:02:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178137.42 epoch sec; seqNo:17971; 4060676.40 north; 593571.45 east 2021-02-13T01:02:17Z- AHRS_M2 msg: 1613178137.79 epoch sec; seqNo:17830 2021-02-13T01:02:17Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.17 psi 2021-02-13T01:02:17Z- handleDepth()-Depth_Keller msg: 1613178137802 epoch sec; seqNo:17972; depth 28.31 2021-02-13T01:02:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178137.81 epoch sec; seqNo:17972; 4060676.78 north; 593571.45 east 2021-02-13T01:02:18Z- AHRS_M2 msg: 1613178138.20 epoch sec; seqNo:17831 2021-02-13T01:02:18Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.17 psi 2021-02-13T01:02:18Z- handleDepth()-Depth_Keller msg: 1613178138206 epoch sec; seqNo:17973; depth 28.42 2021-02-13T01:02:18Z- handleDvl() - RDI_Pathfinder msg: 1613178138.22 epoch sec; seqNo:8695 2021-02-13T01:02:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.21 , 82.84 , 0.00 , 97.99 2021-02-13T01:02:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , -0.08 2021-02-13T01:02:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178138.23 epoch sec; seqNo:17973; 4060676.78 north; 593571.45 east 2021-02-13T01:02:18Z- AHRS_M2 msg: 1613178138.60 epoch sec; seqNo:17832 2021-02-13T01:02:18Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.17 psi 2021-02-13T01:02:18Z- handleDepth()-Depth_Keller msg: 1613178138601 epoch sec; seqNo:17974; depth 28.54 2021-02-13T01:02:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:18Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:18Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:18Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:18Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:18Z- handleDvl() - RDI_Pathfinder msg: 1613178138.60 epoch sec; seqNo:8696 2021-02-13T01:02:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , -0.08 2021-02-13T01:02:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178138.62 epoch sec; seqNo:17974; 4060677.15 north; 593571.44 east 2021-02-13T01:02:19Z- AHRS_M2 msg: 1613178139.00 epoch sec; seqNo:17833 2021-02-13T01:02:19Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-13T01:02:19Z- handleDepth()-Depth_Keller msg: 1613178139006 epoch sec; seqNo:17975; depth 28.65 2021-02-13T01:02:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178139.02 epoch sec; seqNo:17975; 4060677.53 north; 593571.44 east 2021-02-13T01:02:19Z- handleDepth()-Depth_Keller msg: 1613178139425 epoch sec; seqNo:17976; depth 28.75 2021-02-13T01:02:19Z- handleDvl() - RDI_Pathfinder msg: 1613178139.44 epoch sec; seqNo:8697 2021-02-13T01:02:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.99 , 0.00 , 60.08 , 97.78 2021-02-13T01:02:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.08 , -0.08 2021-02-13T01:02:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178139.45 epoch sec; seqNo:17976; 4060677.93 north; 593572.65 east 2021-02-13T01:02:19Z- AHRS_M2 msg: 1613178139.81 epoch sec; seqNo:17834 2021-02-13T01:02:19Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-13T01:02:19Z- handleDepth()-Depth_Keller msg: 1613178139813 epoch sec; seqNo:17977; depth 28.85 2021-02-13T01:02:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:19Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:19Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:19Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:19Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:19Z- handleDvl() - RDI_Pathfinder msg: 1613178139.82 epoch sec; seqNo:8698 2021-02-13T01:02:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.08 , -0.08 2021-02-13T01:02:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178139.83 epoch sec; seqNo:17977; 4060678.30 north; 593572.65 east 2021-02-13T01:02:20Z- AHRS_M2 msg: 1613178140.21 epoch sec; seqNo:17835 2021-02-13T01:02:20Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-13T01:02:20Z- handleDepth()-Depth_Keller msg: 1613178140217 epoch sec; seqNo:17978; depth 28.97 2021-02-13T01:02:20Z- handleDvl() - RDI_Pathfinder msg: 1613178140.23 epoch sec; seqNo:8699 2021-02-13T01:02:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:20Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178140.24 epoch sec; seqNo:17978; 4060678.68 north; 593572.65 east 2021-02-13T01:02:20Z- AHRS_M2 msg: 1613178140.62 epoch sec; seqNo:17836 2021-02-13T01:02:20Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-13T01:02:20Z- handleDepth()-Depth_Keller msg: 1613178140620 epoch sec; seqNo:17979; depth 29.09 2021-02-13T01:02:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178140.64 epoch sec; seqNo:17979; 4060678.68 north; 593572.65 east 2021-02-13T01:02:21Z- AHRS_M2 msg: 1613178141.02 epoch sec; seqNo:17837 2021-02-13T01:02:21Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.18 psi 2021-02-13T01:02:21Z- handleDepth()-Depth_Keller msg: 1613178141024 epoch sec; seqNo:17980; depth 29.19 2021-02-13T01:02:21Z- handleDvl() - RDI_Pathfinder msg: 1613178141.04 epoch sec; seqNo:8700 2021-02-13T01:02:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 64.86 , 0.00 , 60.26 , 0.00 2021-02-13T01:02:21Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178141.06 epoch sec; seqNo:17980; 4060679.06 north; 593572.64 east 2021-02-13T01:02:21Z- AHRS_M2 msg: 1613178141.42 epoch sec; seqNo:17838 2021-02-13T01:02:21Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.18 psi 2021-02-13T01:02:21Z- handleDepth()-Depth_Keller msg: 1613178141428 epoch sec; seqNo:17981; depth 29.30 2021-02-13T01:02:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178141.44 epoch sec; seqNo:17981; 4060679.44 north; 593572.64 east 2021-02-13T01:02:21Z- LcmTrn::handleMessages() - handling 45 messages took 0.58 ms and 134 calls... 2021-02-13T01:02:21Z- waiting for 1613178139.22, time is 1613178141.44 2021-02-13T01:02:21Z- LcmTrn::updateTrn() >>>> pose time 1613178141.44, meas time 1613178141.04 <<<< 2021-02-13T01:02:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178141.04 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:02:21Z- TerrainNav::motionUpdate() - currEstimate = (4060679, 593572.64, 29.30) 2021-02-13T01:02:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:02:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:02:21Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:02:21Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:02:21Z- TNavBF::Projecting Measurements 2021-02-13T01:02:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:02:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:02:21Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-13T01:02:21Z- TNavFilter::2 good beams. 2021-02-13T01:02:21Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:02:21Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:02:21Z- TNavBF:: beamIndex[1] = 2 2021-02-13T01:02:21Z- TNavBF:: beamIndex[2] = 0 2021-02-13T01:02:21Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:02:21Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-13T01:02:21Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:02:21Z- TNavBF::mmse selected from filter -1 2021-02-13T01:02:21Z- TerrainMapOctree: (vehN, vehE ) = (4060679.15, 593572.45). 2021-02-13T01:02:21Z- TerrainMapOctree: Min Distance = 885.97. 2021-02-13T01:02:21Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:02:21Z- There is no specific good beam that all particles have in common. 2021-02-13T01:02:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:02:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.23 K: 0.127 ExpectedMeasDiff: nan currMeas.covariance: 6.73 2021-02-13T01:02:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:02:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.124 ExpectedMeasDiff: nan currMeas.covariance: 5.81 2021-02-13T01:02:21Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:02:21Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:02:21Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:02:21Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:02:21Z- Filter 0 No beams used in subcloud update 2021-02-13T01:02:21Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:02:21Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:02:21Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 678.2 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:02:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178141.04 sec. 2021-02-13T01:02:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:02:21Z- TNavBF::mmse selected from filter -1 2021-02-13T01:02:21Z- variances([0], [2], and psi_berg) = 678.177531 832.987712 0.000000 2021-02-13T01:02:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:02:21Z- LcmTrn::publishEstimates() - MLE : 4060678.94 593590.14 29.30 2021-02-13T01:02:21Z- LcmTrn::publishEstimates() - MMSE : 4060679.44 593572.64 29.30 2021-02-13T01:02:21Z- LcmTrn::publishEstimates() - COVAR: 678.18 832.99 0.96 0.00 2021-02-13T01:02:21Z- AHRS_M2 msg: 1613178141.83 epoch sec; seqNo:17839 2021-02-13T01:02:21Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.17 psi 2021-02-13T01:02:21Z- handleDepth()-Depth_Keller msg: 1613178141837 epoch sec; seqNo:17982; depth 29.42 2021-02-13T01:02:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178141.85 epoch sec; seqNo:17982; 4060679.82 north; 593572.63 east 2021-02-13T01:02:22Z- AHRS_M2 msg: 1613178142.23 epoch sec; seqNo:17840 2021-02-13T01:02:22Z- AHRS_M2 msg: 0.58 phi; -0.27 theta; -0.17 psi 2021-02-13T01:02:22Z- handleDepth()-Depth_Keller msg: 1613178142237 epoch sec; seqNo:17983; depth 29.55 2021-02-13T01:02:22Z- handleDvl() - RDI_Pathfinder msg: 1613178142.26 epoch sec; seqNo:8701 2021-02-13T01:02:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 64.41 , 0.00 , 60.27 , 0.00 2021-02-13T01:02:22Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178142.27 epoch sec; seqNo:17983; 4060680.21 north; 593573.85 east 2021-02-13T01:02:22Z- AHRS_M2 msg: 1613178142.64 epoch sec; seqNo:17841 2021-02-13T01:02:22Z- AHRS_M2 msg: 0.58 phi; -0.24 theta; -0.17 psi 2021-02-13T01:02:22Z- handleDepth()-Depth_Keller msg: 1613178142645 epoch sec; seqNo:17984; depth 29.67 2021-02-13T01:02:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178142.66 epoch sec; seqNo:17984; 4060680.59 north; 593573.84 east 2021-02-13T01:02:23Z- AHRS_M2 msg: 1613178143.04 epoch sec; seqNo:17842 2021-02-13T01:02:23Z- AHRS_M2 msg: 0.58 phi; -0.21 theta; -0.16 psi 2021-02-13T01:02:23Z- handleDepth()-Depth_Keller msg: 1613178143045 epoch sec; seqNo:17985; depth 29.78 2021-02-13T01:02:23Z- handleDvl() - RDI_Pathfinder msg: 1613178143.06 epoch sec; seqNo:8702 2021-02-13T01:02:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.03 , 0.00 , 60.73 , 0.00 2021-02-13T01:02:23Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178143.07 epoch sec; seqNo:17985; 4060680.97 north; 593573.84 east 2021-02-13T01:02:23Z- AHRS_M2 msg: 1613178143.44 epoch sec; seqNo:17843 2021-02-13T01:02:23Z- AHRS_M2 msg: 0.57 phi; -0.17 theta; -0.15 psi 2021-02-13T01:02:23Z- handleDepth()-Depth_Keller msg: 1613178143459 epoch sec; seqNo:17986; depth 29.89 2021-02-13T01:02:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:23Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:23Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:23Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:23Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:23Z- handleDvl() - RDI_Pathfinder msg: 1613178143.46 epoch sec; seqNo:8703 2021-02-13T01:02:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:23Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178143.47 epoch sec; seqNo:17986; 4060681.35 north; 593573.84 east 2021-02-13T01:02:23Z- AHRS_M2 msg: 1613178143.86 epoch sec; seqNo:17844 2021-02-13T01:02:23Z- AHRS_M2 msg: 0.57 phi; -0.09 theta; -0.14 psi 2021-02-13T01:02:23Z- handleDepth()-Depth_Keller msg: 1613178143897 epoch sec; seqNo:17987; depth 30.00 2021-02-13T01:02:23Z- handleDvl() - RDI_Pathfinder msg: 1613178143.91 epoch sec; seqNo:8704 2021-02-13T01:02:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:23Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178143.94 epoch sec; seqNo:17987; 4060681.73 north; 593573.83 east 2021-02-13T01:02:24Z- AHRS_M2 msg: 1613178144.25 epoch sec; seqNo:17845 2021-02-13T01:02:24Z- AHRS_M2 msg: 0.56 phi; -0.05 theta; -0.14 psi 2021-02-13T01:02:24Z- handleDepth()-Depth_Keller msg: 1613178144265 epoch sec; seqNo:17988; depth 30.12 2021-02-13T01:02:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178144.28 epoch sec; seqNo:17988; 4060682.11 north; 593573.83 east 2021-02-13T01:02:24Z- AHRS_M2 msg: 1613178144.66 epoch sec; seqNo:17846 2021-02-13T01:02:24Z- AHRS_M2 msg: 0.56 phi; -0.01 theta; -0.14 psi 2021-02-13T01:02:24Z- handleDepth()-Depth_Keller msg: 1613178144662 epoch sec; seqNo:17989; depth 30.16 2021-02-13T01:02:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:24Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:24Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:24Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:24Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:24Z- handleDvl() - RDI_Pathfinder msg: 1613178144.66 epoch sec; seqNo:8705 2021-02-13T01:02:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:24Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178144.67 epoch sec; seqNo:17989; 4060682.12 north; 593575.05 east 2021-02-13T01:02:25Z- AHRS_M2 msg: 1613178145.06 epoch sec; seqNo:17847 2021-02-13T01:02:25Z- AHRS_M2 msg: 0.55 phi; 0.03 theta; -0.14 psi 2021-02-13T01:02:25Z- handleDepth()-Depth_Keller msg: 1613178145064 epoch sec; seqNo:17990; depth 30.23 2021-02-13T01:02:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178145.09 epoch sec; seqNo:17990; 4060682.50 north; 593575.04 east 2021-02-13T01:02:25Z- AHRS_M2 msg: 1613178145.46 epoch sec; seqNo:17848 2021-02-13T01:02:25Z- AHRS_M2 msg: 0.55 phi; 0.07 theta; -0.15 psi 2021-02-13T01:02:25Z- handleDepth()-Depth_Keller msg: 1613178145469 epoch sec; seqNo:17991; depth 30.29 2021-02-13T01:02:25Z- handleDvl() - RDI_Pathfinder msg: 1613178145.48 epoch sec; seqNo:8706 2021-02-13T01:02:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:25Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178145.49 epoch sec; seqNo:17991; 4060682.88 north; 593575.04 east 2021-02-13T01:02:25Z- AHRS_M2 msg: 1613178145.87 epoch sec; seqNo:17849 2021-02-13T01:02:25Z- AHRS_M2 msg: 0.54 phi; 0.10 theta; -0.15 psi 2021-02-13T01:02:25Z- handleDepth()-Depth_Keller msg: 1613178145872 epoch sec; seqNo:17992; depth 30.33 2021-02-13T01:02:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:25Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:25Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:25Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:25Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:25Z- handleDvl() - RDI_Pathfinder msg: 1613178145.87 epoch sec; seqNo:8707 2021-02-13T01:02:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:25Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178145.89 epoch sec; seqNo:17992; 4060683.26 north; 593575.04 east 2021-02-13T01:02:26Z- AHRS_M2 msg: 1613178146.30 epoch sec; seqNo:17850 2021-02-13T01:02:26Z- AHRS_M2 msg: 0.54 phi; 0.13 theta; -0.16 psi 2021-02-13T01:02:26Z- handleDepth()-Depth_Keller msg: 1613178146307 epoch sec; seqNo:17993; depth 30.36 2021-02-13T01:02:26Z- handleDvl() - RDI_Pathfinder msg: 1613178146.32 epoch sec; seqNo:8708 2021-02-13T01:02:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.55 , 77.91 , 77.91 , 63.33 2021-02-13T01:02:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , 0.02 2021-02-13T01:02:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178146.35 epoch sec; seqNo:17993; 4060683.63 north; 593575.03 east 2021-02-13T01:02:26Z- AHRS_M2 msg: 1613178146.68 epoch sec; seqNo:17851 2021-02-13T01:02:26Z- AHRS_M2 msg: 0.53 phi; 0.16 theta; -0.16 psi 2021-02-13T01:02:26Z- handleDepth()-Depth_Keller msg: 1613178146680 epoch sec; seqNo:17994; depth 30.38 2021-02-13T01:02:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178146.69 epoch sec; seqNo:17994; 4060684.01 north; 593575.03 east 2021-02-13T01:02:26Z- LcmTrn::handleMessages() - handling 47 messages took 0.74 ms and 133 calls... 2021-02-13T01:02:26Z- waiting for 1613178144.44, time is 1613178146.69 2021-02-13T01:02:26Z- LcmTrn::updateTrn() >>>> pose time 1613178146.69, meas time 1613178146.32 <<<< 2021-02-13T01:02:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178146.32 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:02:26Z- TerrainNav::motionUpdate() - currEstimate = (4060684, 593575.03, 30.38) 2021-02-13T01:02:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:02:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:02:26Z- TNavBF::Projecting Measurements 2021-02-13T01:02:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:02:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:02:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:02:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:02:26Z- TNavFilter::4 good beams. 2021-02-13T01:02:26Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:02:26Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:02:26Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:02:26Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:02:26Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:02:26Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:02:26Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:02:26Z- TNavBF::mmse selected from filter -1 2021-02-13T01:02:26Z- TerrainMapOctree: (vehN, vehE ) = (4060683.69, 593574.86). 2021-02-13T01:02:26Z- TerrainMapOctree: Min Distance = 881.21. 2021-02-13T01:02:26Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:02:26Z- There is no specific good beam that all particles have in common. 2021-02-13T01:02:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:02:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.133 ExpectedMeasDiff: nan currMeas.covariance: 6.06 2021-02-13T01:02:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:02:26Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 9.71 2021-02-13T01:02:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:02:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 9.71 2021-02-13T01:02:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:02:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 6.42 2021-02-13T01:02:26Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:02:26Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:02:26Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:02:26Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:02:26Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:02:26Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:02:26Z- Filter 0 No beams used in subcloud update 2021-02-13T01:02:26Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:02:26Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:02:26Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 678.1 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T01:02:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178146.32 sec. 2021-02-13T01:02:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:02:26Z- TNavBF::mmse selected from filter -1 2021-02-13T01:02:26Z- variances([0], [2], and psi_berg) = 678.117356 832.987687 0.000000 2021-02-13T01:02:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:02:26Z- LcmTrn::publishEstimates() - MLE : 4060683.51 593592.53 30.38 2021-02-13T01:02:26Z- LcmTrn::publishEstimates() - MMSE : 4060684.01 593575.03 30.38 2021-02-13T01:02:26Z- LcmTrn::publishEstimates() - COVAR: 678.12 832.99 0.81 0.00 2021-02-13T01:02:27Z- AHRS_M2 msg: 1613178147.08 epoch sec; seqNo:17852 2021-02-13T01:02:27Z- AHRS_M2 msg: 0.52 phi; 0.18 theta; -0.17 psi 2021-02-13T01:02:27Z- handleDepth()-Depth_Keller msg: 1613178147084 epoch sec; seqNo:17995; depth 30.38 2021-02-13T01:02:27Z- handleDvl() - RDI_Pathfinder msg: 1613178147.10 epoch sec; seqNo:8709 2021-02-13T01:02:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.81 , 0.00 , 0.00 , 62.16 2021-02-13T01:02:27Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178147.11 epoch sec; seqNo:17995; 4060684.39 north; 593575.02 east 2021-02-13T01:02:27Z- AHRS_M2 msg: 1613178147.48 epoch sec; seqNo:17853 2021-02-13T01:02:27Z- AHRS_M2 msg: 0.52 phi; 0.21 theta; -0.17 psi 2021-02-13T01:02:27Z- handleDepth()-Depth_Keller msg: 1613178147501 epoch sec; seqNo:17996; depth 30.36 2021-02-13T01:02:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178147.51 epoch sec; seqNo:17996; 4060684.39 north; 593575.02 east 2021-02-13T01:02:27Z- AHRS_M2 msg: 1613178147.89 epoch sec; seqNo:17854 2021-02-13T01:02:27Z- AHRS_M2 msg: 0.51 phi; 0.23 theta; -0.17 psi 2021-02-13T01:02:27Z- handleDepth()-Depth_Keller msg: 1613178147892 epoch sec; seqNo:17997; depth 30.33 2021-02-13T01:02:27Z- handleDvl() - RDI_Pathfinder msg: 1613178147.91 epoch sec; seqNo:8710 2021-02-13T01:02:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 63.35 , 0.00 , 0.00 , 62.02 2021-02-13T01:02:27Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178147.92 epoch sec; seqNo:17997; 4060684.78 north; 593576.24 east 2021-02-13T01:02:28Z- AHRS_M2 msg: 1613178148.29 epoch sec; seqNo:17855 2021-02-13T01:02:28Z- AHRS_M2 msg: 0.51 phi; 0.24 theta; -0.17 psi 2021-02-13T01:02:28Z- handleDepth()-Depth_Keller msg: 1613178148296 epoch sec; seqNo:17998; depth 30.30 2021-02-13T01:02:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:28Z- handleDvl() - RDI_Pathfinder msg: 1613178148.30 epoch sec; seqNo:8711 2021-02-13T01:02:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:28Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178148.31 epoch sec; seqNo:17998; 4060685.16 north; 593576.24 east 2021-02-13T01:02:28Z- AHRS_M2 msg: 1613178148.70 epoch sec; seqNo:17856 2021-02-13T01:02:28Z- AHRS_M2 msg: 0.51 phi; 0.26 theta; -0.17 psi 2021-02-13T01:02:28Z- handleDepth()-Depth_Keller msg: 1613178148700 epoch sec; seqNo:17999; depth 30.26 2021-02-13T01:02:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178148.71 epoch sec; seqNo:17999; 4060685.54 north; 593576.23 east 2021-02-13T01:02:29Z- AHRS_M2 msg: 1613178149.10 epoch sec; seqNo:17857 2021-02-13T01:02:29Z- AHRS_M2 msg: 0.51 phi; 0.27 theta; -0.16 psi 2021-02-13T01:02:29Z- handleDepth()-Depth_Keller msg: 1613178149104 epoch sec; seqNo:18000; depth 30.21 2021-02-13T01:02:29Z- handleDvl() - RDI_Pathfinder msg: 1613178149.12 epoch sec; seqNo:8712 2021-02-13T01:02:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 64.68 , 0.00 , 0.00 , 60.69 2021-02-13T01:02:29Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178149.14 epoch sec; seqNo:18000; 4060685.54 north; 593576.23 east 2021-02-13T01:02:29Z- AHRS_M2 msg: 1613178149.50 epoch sec; seqNo:17858 2021-02-13T01:02:29Z- AHRS_M2 msg: 0.51 phi; 0.29 theta; -0.16 psi 2021-02-13T01:02:29Z- handleDepth()-Depth_Keller msg: 1613178149508 epoch sec; seqNo:18001; depth 30.15 2021-02-13T01:02:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:29Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:29Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:29Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:29Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:29Z- handleDvl() - RDI_Pathfinder msg: 1613178149.51 epoch sec; seqNo:8713 2021-02-13T01:02:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:29Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178149.54 epoch sec; seqNo:18001; 4060685.92 north; 593576.23 east 2021-02-13T01:02:29Z- AHRS_M2 msg: 1613178149.91 epoch sec; seqNo:17859 2021-02-13T01:02:29Z- AHRS_M2 msg: 0.51 phi; 0.30 theta; -0.16 psi 2021-02-13T01:02:29Z- handleDepth()-Depth_Keller msg: 1613178149917 epoch sec; seqNo:18002; depth 30.09 2021-02-13T01:02:29Z- handleDvl() - RDI_Pathfinder msg: 1613178149.93 epoch sec; seqNo:8714 2021-02-13T01:02:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 65.57 , 0.00 , 0.00 , 60.69 2021-02-13T01:02:29Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178149.96 epoch sec; seqNo:18002; 4060686.30 north; 593576.22 east 2021-02-13T01:02:30Z- AHRS_M2 msg: 1613178150.31 epoch sec; seqNo:17860 2021-02-13T01:02:30Z- AHRS_M2 msg: 0.51 phi; 0.31 theta; -0.16 psi 2021-02-13T01:02:30Z- handleDepth()-Depth_Keller msg: 1613178150317 epoch sec; seqNo:18003; depth 30.01 2021-02-13T01:02:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178150.33 epoch sec; seqNo:18003; 4060686.68 north; 593576.22 east 2021-02-13T01:02:30Z- AHRS_M2 msg: 1613178150.72 epoch sec; seqNo:17861 2021-02-13T01:02:30Z- AHRS_M2 msg: 0.51 phi; 0.32 theta; -0.15 psi 2021-02-13T01:02:30Z- handleDepth()-Depth_Keller msg: 1613178150720 epoch sec; seqNo:18004; depth 29.94 2021-02-13T01:02:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:30Z- handleDvl() - RDI_Pathfinder msg: 1613178150.72 epoch sec; seqNo:8715 2021-02-13T01:02:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:30Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178150.74 epoch sec; seqNo:18004; 4060686.68 north; 593576.22 east 2021-02-13T01:02:31Z- AHRS_M2 msg: 1613178151.12 epoch sec; seqNo:17862 2021-02-13T01:02:31Z- AHRS_M2 msg: 0.51 phi; 0.32 theta; -0.15 psi 2021-02-13T01:02:31Z- handleDepth()-Depth_Keller msg: 1613178151126 epoch sec; seqNo:18005; depth 29.86 2021-02-13T01:02:31Z- handleDvl() - RDI_Pathfinder msg: 1613178151.15 epoch sec; seqNo:8716 2021-02-13T01:02:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:31Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178151.16 epoch sec; seqNo:18005; 4060687.06 north; 593576.22 east 2021-02-13T01:02:31Z- AHRS_M2 msg: 1613178151.52 epoch sec; seqNo:17863 2021-02-13T01:02:31Z- AHRS_M2 msg: 0.52 phi; 0.33 theta; -0.15 psi 2021-02-13T01:02:31Z- handleDepth()-Depth_Keller msg: 1613178151531 epoch sec; seqNo:18006; depth 29.77 2021-02-13T01:02:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178151.54 epoch sec; seqNo:18006; 4060687.45 north; 593577.43 east 2021-02-13T01:02:31Z- LcmTrn::handleMessages() - handling 44 messages took 0.61 ms and 132 calls... 2021-02-13T01:02:31Z- waiting for 1613178149.69, time is 1613178151.54 2021-02-13T01:02:31Z- LcmTrn::updateTrn() >>>> pose time 1613178151.54, meas time 1613178151.15 <<<< 2021-02-13T01:02:31Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613178151.15, ping # 0. 2021-02-13T01:02:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178151.15 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:02:31Z- TerrainNav::motionUpdate() - currEstimate = (4060687, 593577.43, 29.77) 2021-02-13T01:02:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:02:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:02:31Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:02:31Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:02:31Z- TNavBF::Projecting Measurements 2021-02-13T01:02:31Z- TNavFilter::Measurement from time = 1613178151.15 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T01:02:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:02:31Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:02:31Z- TNavBF:: beamIndex[1] = 0 2021-02-13T01:02:31Z- TNavBF:: beamIndex[2] = 0 2021-02-13T01:02:31Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:02:31Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:02:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:02:31Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 678.1 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T01:02:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:02:31Z- TNavBF::mmse selected from filter -1 2021-02-13T01:02:31Z- variances([0], [2], and psi_berg) = 678.062585 832.987664 0.000000 2021-02-13T01:02:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:02:31Z- LcmTrn::publishEstimates() - MLE : 4060686.95 593594.93 29.77 2021-02-13T01:02:31Z- LcmTrn::publishEstimates() - MMSE : 4060687.45 593577.43 29.77 2021-02-13T01:02:31Z- LcmTrn::publishEstimates() - COVAR: 678.06 832.99 1.03 0.00 2021-02-13T01:02:31Z- AHRS_M2 msg: 1613178151.93 epoch sec; seqNo:17864 2021-02-13T01:02:31Z- AHRS_M2 msg: 0.52 phi; 0.33 theta; -0.14 psi 2021-02-13T01:02:31Z- handleDepth()-Depth_Keller msg: 1613178151934 epoch sec; seqNo:18007; depth 29.69 2021-02-13T01:02:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:31Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:31Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:31Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:31Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:31Z- handleDvl() - RDI_Pathfinder msg: 1613178151.94 epoch sec; seqNo:8717 2021-02-13T01:02:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:31Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178151.94 epoch sec; seqNo:18007; 4060687.45 north; 593577.43 east 2021-02-13T01:02:32Z- AHRS_M2 msg: 1613178152.33 epoch sec; seqNo:17865 2021-02-13T01:02:32Z- AHRS_M2 msg: 0.53 phi; 0.33 theta; -0.14 psi 2021-02-13T01:02:32Z- handleDepth()-Depth_Keller msg: 1613178152336 epoch sec; seqNo:18008; depth 29.59 2021-02-13T01:02:32Z- handleDvl() - RDI_Pathfinder msg: 1613178152.35 epoch sec; seqNo:8718 2021-02-13T01:02:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.04 , 81.76 , 97.99 , 58.86 2021-02-13T01:02:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , -0.11 2021-02-13T01:02:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178152.37 epoch sec; seqNo:18008; 4060687.83 north; 593577.43 east 2021-02-13T01:02:32Z- AHRS_M2 msg: 1613178152.74 epoch sec; seqNo:17866 2021-02-13T01:02:32Z- AHRS_M2 msg: 0.54 phi; 0.34 theta; -0.14 psi 2021-02-13T01:02:32Z- handleDepth()-Depth_Keller msg: 1613178152740 epoch sec; seqNo:18009; depth 29.50 2021-02-13T01:02:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178152.75 epoch sec; seqNo:18009; 4060688.21 north; 593577.42 east 2021-02-13T01:02:33Z- AHRS_M2 msg: 1613178153.15 epoch sec; seqNo:17867 2021-02-13T01:02:33Z- AHRS_M2 msg: 0.54 phi; 0.34 theta; -0.13 psi 2021-02-13T01:02:33Z- handleDepth()-Depth_Keller msg: 1613178153155 epoch sec; seqNo:18010; depth 29.38 2021-02-13T01:02:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:33Z- handleDvl() - RDI_Pathfinder msg: 1613178153.16 epoch sec; seqNo:8719 2021-02-13T01:02:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , -0.11 2021-02-13T01:02:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178153.20 epoch sec; seqNo:18010; 4060688.59 north; 593577.42 east 2021-02-13T01:02:33Z- AHRS_M2 msg: 1613178153.55 epoch sec; seqNo:17868 2021-02-13T01:02:33Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.13 psi 2021-02-13T01:02:33Z- handleDepth()-Depth_Keller msg: 1613178153553 epoch sec; seqNo:18011; depth 29.29 2021-02-13T01:02:33Z- handleDvl() - RDI_Pathfinder msg: 1613178153.57 epoch sec; seqNo:8720 2021-02-13T01:02:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.45 , 79.60 , 97.99 , 59.27 2021-02-13T01:02:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , -0.13 2021-02-13T01:02:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178153.58 epoch sec; seqNo:18011; 4060688.59 north; 593577.42 east 2021-02-13T01:02:33Z- AHRS_M2 msg: 1613178153.95 epoch sec; seqNo:17869 2021-02-13T01:02:33Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.13 psi 2021-02-13T01:02:33Z- handleDepth()-Depth_Keller msg: 1613178153952 epoch sec; seqNo:18012; depth 29.18 2021-02-13T01:02:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178153.97 epoch sec; seqNo:18012; 4060688.97 north; 593577.42 east 2021-02-13T01:02:34Z- AHRS_M2 msg: 1613178154.35 epoch sec; seqNo:17870 2021-02-13T01:02:34Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.13 psi 2021-02-13T01:02:34Z- handleDepth()-Depth_Keller msg: 1613178154356 epoch sec; seqNo:18013; depth 29.07 2021-02-13T01:02:34Z- handleDvl() - RDI_Pathfinder msg: 1613178154.37 epoch sec; seqNo:8721 2021-02-13T01:02:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.63 , 79.60 , 97.99 , 59.68 2021-02-13T01:02:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.09 , -0.10 2021-02-13T01:02:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178154.39 epoch sec; seqNo:18013; 4060689.34 north; 593577.41 east 2021-02-13T01:02:34Z- AHRS_M2 msg: 1613178154.76 epoch sec; seqNo:17871 2021-02-13T01:02:34Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.13 psi 2021-02-13T01:02:34Z- handleDepth()-Depth_Keller msg: 1613178154765 epoch sec; seqNo:18014; depth 28.97 2021-02-13T01:02:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178154.78 epoch sec; seqNo:18014; 4060689.34 north; 593577.41 east 2021-02-13T01:02:35Z- AHRS_M2 msg: 1613178155.19 epoch sec; seqNo:17872 2021-02-13T01:02:35Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.12 psi 2021-02-13T01:02:35Z- handleDepth()-Depth_Keller msg: 1613178155195 epoch sec; seqNo:18015; depth 28.87 2021-02-13T01:02:35Z- handleDvl() - RDI_Pathfinder msg: 1613178155.21 epoch sec; seqNo:8722 2021-02-13T01:02:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.26 , 80.68 , 97.99 , 60.50 2021-02-13T01:02:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.15 , -0.06 2021-02-13T01:02:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178155.22 epoch sec; seqNo:18015; 4060689.74 north; 593578.63 east 2021-02-13T01:02:35Z- AHRS_M2 msg: 1613178155.56 epoch sec; seqNo:17873 2021-02-13T01:02:35Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.12 psi 2021-02-13T01:02:35Z- handleDepth()-Depth_Keller msg: 1613178155578 epoch sec; seqNo:18016; depth 28.75 2021-02-13T01:02:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:35Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:35Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:35Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:35Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:35Z- handleDvl() - RDI_Pathfinder msg: 1613178155.58 epoch sec; seqNo:8723 2021-02-13T01:02:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.15 , -0.06 2021-02-13T01:02:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178155.59 epoch sec; seqNo:18016; 4060690.12 north; 593578.62 east 2021-02-13T01:02:35Z- AHRS_M2 msg: 1613178155.97 epoch sec; seqNo:17874 2021-02-13T01:02:35Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.12 psi 2021-02-13T01:02:35Z- handleDepth()-Depth_Keller msg: 1613178155972 epoch sec; seqNo:18017; depth 28.65 2021-02-13T01:02:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178155.98 epoch sec; seqNo:18017; 4060690.49 north; 593578.62 east 2021-02-13T01:02:36Z- AHRS_M2 msg: 1613178156.37 epoch sec; seqNo:17875 2021-02-13T01:02:36Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.11 psi 2021-02-13T01:02:36Z- handleDepth()-Depth_Keller msg: 1613178156381 epoch sec; seqNo:18018; depth 28.55 2021-02-13T01:02:36Z- handleDvl() - RDI_Pathfinder msg: 1613178156.39 epoch sec; seqNo:8724 2021-02-13T01:02:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:36Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178156.40 epoch sec; seqNo:18018; 4060690.49 north; 593578.62 east 2021-02-13T01:02:36Z- AHRS_M2 msg: 1613178156.78 epoch sec; seqNo:17876 2021-02-13T01:02:36Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.11 psi 2021-02-13T01:02:36Z- handleDepth()-Depth_Keller msg: 1613178156789 epoch sec; seqNo:18019; depth 28.43 2021-02-13T01:02:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:36Z- handleDvl() - RDI_Pathfinder msg: 1613178156.79 epoch sec; seqNo:8725 2021-02-13T01:02:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:36Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178156.80 epoch sec; seqNo:18019; 4060690.87 north; 593578.61 east 2021-02-13T01:02:36Z- LcmTrn::handleMessages() - handling 48 messages took 0.79 ms and 133 calls... 2021-02-13T01:02:36Z- waiting for 1613178154.54, time is 1613178156.80 2021-02-13T01:02:36Z- LcmTrn::updateTrn() equating pose 1613178156.80 and meas times 1613178156.80 2021-02-13T01:02:36Z- LcmTrn::updateTrn() >>>> pose time 1613178156.80, meas time 1613178156.80 <<<< 2021-02-13T01:02:36Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613178156.80, ping # 0. 2021-02-13T01:02:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178156.80 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:02:36Z- TerrainNav::motionUpdate() - currEstimate = (4060691, 593578.61, 28.43) 2021-02-13T01:02:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:02:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:02:36Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:02:36Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:02:36Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178156.80 sec.; No relevant navigation data available 2021-02-13T01:02:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:02:36Z- TNavBF::mmse selected from filter -1 2021-02-13T01:02:36Z- variances([0], [2], and psi_berg) = 678.021647 832.987647 0.000000 2021-02-13T01:02:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:02:36Z- LcmTrn::publishEstimates() - MLE : 4060690.37 593596.11 28.43 2021-02-13T01:02:36Z- LcmTrn::publishEstimates() - MMSE : 4060690.87 593578.61 28.43 2021-02-13T01:02:36Z- LcmTrn::publishEstimates() - COVAR: 678.02 832.99 1.21 0.00 2021-02-13T01:02:37Z- AHRS_M2 msg: 1613178157.18 epoch sec; seqNo:17877 2021-02-13T01:02:37Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.10 psi 2021-02-13T01:02:37Z- handleDepth()-Depth_Keller msg: 1613178157184 epoch sec; seqNo:18020; depth 28.32 2021-02-13T01:02:37Z- handleDvl() - RDI_Pathfinder msg: 1613178157.20 epoch sec; seqNo:8726 2021-02-13T01:02:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.90 , 0.00 , 98.19 , 60.50 2021-02-13T01:02:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.12 , 0.05 2021-02-13T01:02:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178157.21 epoch sec; seqNo:18020; 4060691.25 north; 593578.61 east 2021-02-13T01:02:37Z- AHRS_M2 msg: 1613178157.58 epoch sec; seqNo:17878 2021-02-13T01:02:37Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.10 psi 2021-02-13T01:02:37Z- handleDepth()-Depth_Keller msg: 1613178157588 epoch sec; seqNo:18021; depth 28.21 2021-02-13T01:02:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178157.60 epoch sec; seqNo:18021; 4060691.63 north; 593578.61 east 2021-02-13T01:02:37Z- AHRS_M2 msg: 1613178157.99 epoch sec; seqNo:17879 2021-02-13T01:02:37Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.09 psi 2021-02-13T01:02:37Z- handleDepth()-Depth_Keller msg: 1613178157997 epoch sec; seqNo:18022; depth 28.11 2021-02-13T01:02:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:37Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:37Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:37Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:37Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:37Z- handleDvl() - RDI_Pathfinder msg: 1613178158.00 epoch sec; seqNo:8727 2021-02-13T01:02:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.12 , 0.05 2021-02-13T01:02:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178158.01 epoch sec; seqNo:18022; 4060692.01 north; 593578.60 east 2021-02-13T01:02:38Z- AHRS_M2 msg: 1613178158.39 epoch sec; seqNo:17880 2021-02-13T01:02:38Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.09 psi 2021-02-13T01:02:38Z- handleDepth()-Depth_Keller msg: 1613178158405 epoch sec; seqNo:18023; depth 27.98 2021-02-13T01:02:38Z- handleDvl() - RDI_Pathfinder msg: 1613178158.42 epoch sec; seqNo:8728 2021-02-13T01:02:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.31 , 78.51 , 97.99 , 0.00 2021-02-13T01:02:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , 0.08 2021-02-13T01:02:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178158.43 epoch sec; seqNo:18023; 4060692.01 north; 593578.60 east 2021-02-13T01:02:38Z- AHRS_M2 msg: 1613178158.80 epoch sec; seqNo:17881 2021-02-13T01:02:38Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.09 psi 2021-02-13T01:02:38Z- handleDepth()-Depth_Keller msg: 1613178158805 epoch sec; seqNo:18024; depth 27.88 2021-02-13T01:02:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178158.82 epoch sec; seqNo:18024; 4060692.40 north; 593579.82 east 2021-02-13T01:02:39Z- AHRS_M2 msg: 1613178159.21 epoch sec; seqNo:17882 2021-02-13T01:02:39Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.08 psi 2021-02-13T01:02:39Z- handleDepth()-Depth_Keller msg: 1613178159212 epoch sec; seqNo:18025; depth 27.76 2021-02-13T01:02:39Z- handleDvl() - RDI_Pathfinder msg: 1613178159.22 epoch sec; seqNo:8729 2021-02-13T01:02:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 79.39 , 97.79 , 60.09 2021-02-13T01:02:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.06 2021-02-13T01:02:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178159.23 epoch sec; seqNo:18025; 4060692.78 north; 593579.81 east 2021-02-13T01:02:39Z- AHRS_M2 msg: 1613178159.60 epoch sec; seqNo:17883 2021-02-13T01:02:39Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.08 psi 2021-02-13T01:02:39Z- handleDepth()-Depth_Keller msg: 1613178159608 epoch sec; seqNo:18026; depth 27.64 2021-02-13T01:02:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178159.63 epoch sec; seqNo:18026; 4060693.16 north; 593579.81 east 2021-02-13T01:02:40Z- AHRS_M2 msg: 1613178160.01 epoch sec; seqNo:17884 2021-02-13T01:02:40Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.08 psi 2021-02-13T01:02:40Z- handleDepth()-Depth_Keller msg: 1613178160012 epoch sec; seqNo:18027; depth 27.53 2021-02-13T01:02:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178160.02 epoch sec; seqNo:18027; 4060693.16 north; 593579.81 east 2021-02-13T01:02:40Z- AHRS_M2 msg: 1613178160.41 epoch sec; seqNo:17885 2021-02-13T01:02:40Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.09 psi 2021-02-13T01:02:40Z- handleDepth()-Depth_Keller msg: 1613178160416 epoch sec; seqNo:18028; depth 27.43 2021-02-13T01:02:40Z- handleDvl() - RDI_Pathfinder msg: 1613178160.43 epoch sec; seqNo:8730 2021-02-13T01:02:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.53 , 78.51 , 97.99 , 62.13 2021-02-13T01:02:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.00 , 0.04 2021-02-13T01:02:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178160.46 epoch sec; seqNo:18028; 4060693.54 north; 593579.81 east 2021-02-13T01:02:40Z- AHRS_M2 msg: 1613178160.83 epoch sec; seqNo:17886 2021-02-13T01:02:40Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.10 psi 2021-02-13T01:02:40Z- handleDepth()-Depth_Keller msg: 1613178160831 epoch sec; seqNo:18029; depth 27.30 2021-02-13T01:02:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178160.84 epoch sec; seqNo:18029; 4060693.92 north; 593579.80 east 2021-02-13T01:02:41Z- AHRS_M2 msg: 1613178161.24 epoch sec; seqNo:17887 2021-02-13T01:02:41Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.11 psi 2021-02-13T01:02:41Z- handleDepth()-Depth_Keller msg: 1613178161249 epoch sec; seqNo:18030; depth 27.20 2021-02-13T01:02:41Z- handleDvl() - RDI_Pathfinder msg: 1613178161.27 epoch sec; seqNo:8731 2021-02-13T01:02:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 79.60 , 94.75 , 61.31 2021-02-13T01:02:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.00 , 0.01 2021-02-13T01:02:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178161.29 epoch sec; seqNo:18030; 4060693.92 north; 593579.80 east 2021-02-13T01:02:41Z- AHRS_M2 msg: 1613178161.62 epoch sec; seqNo:17888 2021-02-13T01:02:41Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.12 psi 2021-02-13T01:02:41Z- handleDepth()-Depth_Keller msg: 1613178161628 epoch sec; seqNo:18031; depth 27.06 2021-02-13T01:02:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:41Z- handleDvl() - RDI_Pathfinder msg: 1613178161.63 epoch sec; seqNo:8732 2021-02-13T01:02:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.00 , 0.01 2021-02-13T01:02:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178161.65 epoch sec; seqNo:18031; 4060694.31 north; 593581.02 east 2021-02-13T01:02:42Z- LcmTrn::handleMessages() - handling 43 messages took 0.68 ms and 130 calls... 2021-02-13T01:02:42Z- waiting for 1613178159.80, time is 1613178161.65 2021-02-13T01:02:42Z- LcmTrn::updateTrn() equating pose 1613178161.65 and meas times 1613178161.65 2021-02-13T01:02:42Z- LcmTrn::updateTrn() >>>> pose time 1613178161.65, meas time 1613178161.65 <<<< 2021-02-13T01:02:42Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613178161.65, ping # 0. 2021-02-13T01:02:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178161.65 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:02:42Z- TerrainNav::motionUpdate() - currEstimate = (4060694, 593581.02, 27.06) 2021-02-13T01:02:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:02:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:02:42Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:02:42Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:02:42Z- TNavBF::Projecting Measurements 2021-02-13T01:02:42Z- TNavFilter::Measurement from time = 1613178161.65 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T01:02:42Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:02:42Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:02:42Z- TNavBF:: beamIndex[1] = 0 2021-02-13T01:02:42Z- TNavBF:: beamIndex[2] = 0 2021-02-13T01:02:42Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:02:42Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:02:42Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:02:42Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 678.0 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T01:02:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:02:42Z- TNavBF::mmse selected from filter -1 2021-02-13T01:02:42Z- variances([0], [2], and psi_berg) = 677.977613 832.987629 0.000000 2021-02-13T01:02:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:02:42Z- LcmTrn::publishEstimates() - MLE : 4060693.81 593598.52 27.06 2021-02-13T01:02:42Z- LcmTrn::publishEstimates() - MMSE : 4060694.31 593581.02 27.06 2021-02-13T01:02:42Z- LcmTrn::publishEstimates() - COVAR: 677.98 832.99 1.42 0.00 2021-02-13T01:02:42Z- AHRS_M2 msg: 1613178162.03 epoch sec; seqNo:17889 2021-02-13T01:02:42Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.14 psi 2021-02-13T01:02:42Z- handleDepth()-Depth_Keller msg: 1613178162032 epoch sec; seqNo:18032; depth 26.95 2021-02-13T01:02:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178162.04 epoch sec; seqNo:18032; 4060694.69 north; 593581.01 east 2021-02-13T01:02:42Z- AHRS_M2 msg: 1613178162.43 epoch sec; seqNo:17890 2021-02-13T01:02:42Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T01:02:42Z- handleDepth()-Depth_Keller msg: 1613178162441 epoch sec; seqNo:18033; depth 26.85 2021-02-13T01:02:42Z- handleDvl() - RDI_Pathfinder msg: 1613178162.46 epoch sec; seqNo:8733 2021-02-13T01:02:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.72 , 81.76 , 97.99 , 62.95 2021-02-13T01:02:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.00 , 0.00 2021-02-13T01:02:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178162.47 epoch sec; seqNo:18033; 4060694.69 north; 593581.01 east 2021-02-13T01:02:42Z- AHRS_M2 msg: 1613178162.84 epoch sec; seqNo:17891 2021-02-13T01:02:42Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-13T01:02:42Z- handleDepth()-Depth_Keller msg: 1613178162842 epoch sec; seqNo:18034; depth 26.73 2021-02-13T01:02:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:42Z- handleDvl() - RDI_Pathfinder msg: 1613178162.84 epoch sec; seqNo:8734 2021-02-13T01:02:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.00 , 0.00 2021-02-13T01:02:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178162.86 epoch sec; seqNo:18034; 4060695.07 north; 593581.01 east 2021-02-13T01:02:43Z- AHRS_M2 msg: 1613178163.24 epoch sec; seqNo:17892 2021-02-13T01:02:43Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-13T01:02:43Z- handleDepth()-Depth_Keller msg: 1613178163245 epoch sec; seqNo:18035; depth 26.62 2021-02-13T01:02:43Z- handleDvl() - RDI_Pathfinder msg: 1613178163.26 epoch sec; seqNo:8735 2021-02-13T01:02:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.49 , 85.01 , 97.99 , 61.72 2021-02-13T01:02:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.01 , 0.00 2021-02-13T01:02:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178163.27 epoch sec; seqNo:18035; 4060695.45 north; 593581.01 east 2021-02-13T01:02:43Z- AHRS_M2 msg: 1613178163.64 epoch sec; seqNo:17893 2021-02-13T01:02:43Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-13T01:02:43Z- handleDepth()-Depth_Keller msg: 1613178163653 epoch sec; seqNo:18036; depth 26.49 2021-02-13T01:02:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178163.66 epoch sec; seqNo:18036; 4060695.45 north; 593581.01 east 2021-02-13T01:02:44Z- AHRS_M2 msg: 1613178164.05 epoch sec; seqNo:17894 2021-02-13T01:02:44Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-13T01:02:44Z- handleDepth()-Depth_Keller msg: 1613178164065 epoch sec; seqNo:18037; depth 26.39 2021-02-13T01:02:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:44Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:44Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:44Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:44Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:44Z- handleDvl() - RDI_Pathfinder msg: 1613178164.07 epoch sec; seqNo:8736 2021-02-13T01:02:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.01 , 0.00 2021-02-13T01:02:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178164.08 epoch sec; seqNo:18037; 4060695.84 north; 593582.22 east 2021-02-13T01:02:44Z- AHRS_M2 msg: 1613178164.45 epoch sec; seqNo:17895 2021-02-13T01:02:44Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-13T01:02:44Z- handleDepth()-Depth_Keller msg: 1613178164465 epoch sec; seqNo:18038; depth 26.27 2021-02-13T01:02:44Z- handleDvl() - RDI_Pathfinder msg: 1613178164.49 epoch sec; seqNo:8737 2021-02-13T01:02:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.90 , 86.09 , 97.99 , 61.72 2021-02-13T01:02:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.01 , 0.01 2021-02-13T01:02:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178164.50 epoch sec; seqNo:18038; 4060696.22 north; 593582.22 east 2021-02-13T01:02:44Z- AHRS_M2 msg: 1613178164.86 epoch sec; seqNo:17896 2021-02-13T01:02:44Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-13T01:02:44Z- handleDepth()-Depth_Keller msg: 1613178164859 epoch sec; seqNo:18039; depth 26.15 2021-02-13T01:02:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178164.87 epoch sec; seqNo:18039; 4060696.22 north; 593582.22 east 2021-02-13T01:02:45Z- AHRS_M2 msg: 1613178165.26 epoch sec; seqNo:17897 2021-02-13T01:02:45Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-13T01:02:45Z- handleDepth()-Depth_Keller msg: 1613178165265 epoch sec; seqNo:18040; depth 26.04 2021-02-13T01:02:45Z- handleDvl() - RDI_Pathfinder msg: 1613178165.28 epoch sec; seqNo:8738 2021-02-13T01:02:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.31 , 85.01 , 97.99 , 64.18 2021-02-13T01:02:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.02 , 0.02 2021-02-13T01:02:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178165.29 epoch sec; seqNo:18040; 4060696.60 north; 593582.21 east 2021-02-13T01:02:45Z- AHRS_M2 msg: 1613178165.66 epoch sec; seqNo:17898 2021-02-13T01:02:45Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-13T01:02:45Z- handleDepth()-Depth_Keller msg: 1613178165677 epoch sec; seqNo:18041; depth 25.92 2021-02-13T01:02:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178165.69 epoch sec; seqNo:18041; 4060696.97 north; 593582.21 east 2021-02-13T01:02:46Z- AHRS_M2 msg: 1613178166.07 epoch sec; seqNo:17899 2021-02-13T01:02:46Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-13T01:02:46Z- handleDepth()-Depth_Keller msg: 1613178166077 epoch sec; seqNo:18042; depth 25.81 2021-02-13T01:02:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178166.09 epoch sec; seqNo:18042; 4060696.97 north; 593582.21 east 2021-02-13T01:02:46Z- AHRS_M2 msg: 1613178166.48 epoch sec; seqNo:17900 2021-02-13T01:02:46Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.16 psi 2021-02-13T01:02:46Z- handleDepth()-Depth_Keller msg: 1613178166481 epoch sec; seqNo:18043; depth 25.68 2021-02-13T01:02:46Z- handleDvl() - RDI_Pathfinder msg: 1613178166.49 epoch sec; seqNo:8739 2021-02-13T01:02:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 85.01 , 97.99 , 63.77 2021-02-13T01:02:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.02 , -0.01 2021-02-13T01:02:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178166.50 epoch sec; seqNo:18043; 4060697.35 north; 593582.21 east 2021-02-13T01:02:46Z- AHRS_M2 msg: 1613178166.88 epoch sec; seqNo:17901 2021-02-13T01:02:46Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.15 psi 2021-02-13T01:02:46Z- handleDepth()-Depth_Keller msg: 1613178166885 epoch sec; seqNo:18044; depth 25.58 2021-02-13T01:02:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178166.90 epoch sec; seqNo:18044; 4060697.75 north; 593583.42 east 2021-02-13T01:02:47Z- LcmTrn::handleMessages() - handling 46 messages took 0.68 ms and 133 calls... 2021-02-13T01:02:47Z- waiting for 1613178164.65, time is 1613178166.90 2021-02-13T01:02:47Z- LcmTrn::updateTrn() >>>> pose time 1613178166.90, meas time 1613178166.49 <<<< 2021-02-13T01:02:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178166.49 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:02:47Z- TerrainNav::motionUpdate() - currEstimate = (4060698, 593583.42, 25.58) 2021-02-13T01:02:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:02:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:02:47Z- TNavBF::Projecting Measurements 2021-02-13T01:02:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:02:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:02:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:02:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:02:47Z- TNavFilter::4 good beams. 2021-02-13T01:02:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:02:47Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:02:47Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:02:47Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:02:47Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:02:47Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:02:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:02:47Z- TNavBF::mmse selected from filter -1 2021-02-13T01:02:47Z- TerrainMapOctree: (vehN, vehE ) = (4060697.48, 593583.24). 2021-02-13T01:02:47Z- TerrainMapOctree: Min Distance = 865.94. 2021-02-13T01:02:47Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:02:47Z- There is no specific good beam that all particles have in common. 2021-02-13T01:02:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:02:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.62 K: 0.123 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T01:02:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:02:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.42 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 11.56 2021-02-13T01:02:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:02:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.29 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T01:02:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:02:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.127 ExpectedMeasDiff: nan currMeas.covariance: 6.51 2021-02-13T01:02:47Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:02:47Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:02:47Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:02:47Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:02:47Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:02:47Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:02:47Z- Filter 0 No beams used in subcloud update 2021-02-13T01:02:47Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:02:47Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:02:47Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 677.9 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T01:02:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178166.49 sec. 2021-02-13T01:02:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:02:47Z- TNavBF::mmse selected from filter -1 2021-02-13T01:02:47Z- variances([0], [2], and psi_berg) = 677.923133 832.987606 0.000000 2021-02-13T01:02:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:02:47Z- LcmTrn::publishEstimates() - MLE : 4060697.25 593600.92 25.58 2021-02-13T01:02:47Z- LcmTrn::publishEstimates() - MMSE : 4060697.75 593583.42 25.58 2021-02-13T01:02:47Z- LcmTrn::publishEstimates() - COVAR: 677.92 832.99 1.06 0.00 2021-02-13T01:02:47Z- AHRS_M2 msg: 1613178167.28 epoch sec; seqNo:17902 2021-02-13T01:02:47Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-13T01:02:47Z- handleDepth()-Depth_Keller msg: 1613178167288 epoch sec; seqNo:18045; depth 25.45 2021-02-13T01:02:47Z- handleDvl() - RDI_Pathfinder msg: 1613178167.31 epoch sec; seqNo:8740 2021-02-13T01:02:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 84.87 , 97.86 , 63.36 2021-02-13T01:02:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , -0.05 2021-02-13T01:02:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178167.32 epoch sec; seqNo:18045; 4060697.75 north; 593583.42 east 2021-02-13T01:02:47Z- AHRS_M2 msg: 1613178167.69 epoch sec; seqNo:17903 2021-02-13T01:02:47Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-13T01:02:47Z- handleDepth()-Depth_Keller msg: 1613178167697 epoch sec; seqNo:18046; depth 25.32 2021-02-13T01:02:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:47Z- handleDvl() - RDI_Pathfinder msg: 1613178167.70 epoch sec; seqNo:8741 2021-02-13T01:02:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , -0.05 2021-02-13T01:02:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178167.72 epoch sec; seqNo:18046; 4060698.12 north; 593583.42 east 2021-02-13T01:02:48Z- AHRS_M2 msg: 1613178168.09 epoch sec; seqNo:17904 2021-02-13T01:02:48Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-13T01:02:48Z- handleDepth()-Depth_Keller msg: 1613178168101 epoch sec; seqNo:18047; depth 25.22 2021-02-13T01:02:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178168.11 epoch sec; seqNo:18047; 4060698.50 north; 593583.41 east 2021-02-13T01:02:48Z- AHRS_M2 msg: 1613178168.52 epoch sec; seqNo:17905 2021-02-13T01:02:48Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.15 psi 2021-02-13T01:02:48Z- handleDepth()-Depth_Keller msg: 1613178168541 epoch sec; seqNo:18048; depth 25.08 2021-02-13T01:02:48Z- handleDvl() - RDI_Pathfinder msg: 1613178168.55 epoch sec; seqNo:8742 2021-02-13T01:02:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 86.09 , 97.99 , 63.36 2021-02-13T01:02:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , -0.07 2021-02-13T01:02:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178168.56 epoch sec; seqNo:18048; 4060698.88 north; 593583.41 east 2021-02-13T01:02:48Z- AHRS_M2 msg: 1613178168.90 epoch sec; seqNo:17906 2021-02-13T01:02:48Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.14 psi 2021-02-13T01:02:48Z- handleDepth()-Depth_Keller msg: 1613178168904 epoch sec; seqNo:18049; depth 24.96 2021-02-13T01:02:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:48Z- handleDvl() - RDI_Pathfinder msg: 1613178168.91 epoch sec; seqNo:8743 2021-02-13T01:02:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , -0.07 2021-02-13T01:02:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178168.93 epoch sec; seqNo:18049; 4060698.88 north; 593583.41 east 2021-02-13T01:02:49Z- AHRS_M2 msg: 1613178169.30 epoch sec; seqNo:17907 2021-02-13T01:02:49Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.14 psi 2021-02-13T01:02:49Z- handleDepth()-Depth_Keller msg: 1613178169308 epoch sec; seqNo:18050; depth 24.83 2021-02-13T01:02:49Z- handleDvl() - RDI_Pathfinder msg: 1613178169.32 epoch sec; seqNo:8744 2021-02-13T01:02:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.53 , 85.01 , 100.15 , 64.99 2021-02-13T01:02:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.13 , -0.04 2021-02-13T01:02:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178169.33 epoch sec; seqNo:18050; 4060699.26 north; 593583.41 east 2021-02-13T01:02:49Z- AHRS_M2 msg: 1613178169.71 epoch sec; seqNo:17908 2021-02-13T01:02:49Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.13 psi 2021-02-13T01:02:49Z- handleDepth()-Depth_Keller msg: 1613178169713 epoch sec; seqNo:18051; depth 24.72 2021-02-13T01:02:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178169.72 epoch sec; seqNo:18051; 4060699.65 north; 593584.62 east 2021-02-13T01:02:50Z- AHRS_M2 msg: 1613178170.11 epoch sec; seqNo:17909 2021-02-13T01:02:50Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.13 psi 2021-02-13T01:02:50Z- handleDepth()-Depth_Keller msg: 1613178170118 epoch sec; seqNo:18052; depth 24.61 2021-02-13T01:02:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:50Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:50Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:50Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:50Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:50Z- handleDvl() - RDI_Pathfinder msg: 1613178170.12 epoch sec; seqNo:8745 2021-02-13T01:02:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.13 , -0.04 2021-02-13T01:02:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178170.13 epoch sec; seqNo:18052; 4060700.03 north; 593584.62 east 2021-02-13T01:02:50Z- AHRS_M2 msg: 1613178170.52 epoch sec; seqNo:17910 2021-02-13T01:02:50Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.12 psi 2021-02-13T01:02:50Z- handleDepth()-Depth_Keller msg: 1613178170521 epoch sec; seqNo:18053; depth 24.49 2021-02-13T01:02:50Z- handleDvl() - RDI_Pathfinder msg: 1613178170.54 epoch sec; seqNo:8746 2021-02-13T01:02:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 0.00 , 99.00 , 64.58 2021-02-13T01:02:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.21 , 0.03 2021-02-13T01:02:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178170.56 epoch sec; seqNo:18053; 4060700.03 north; 593584.62 east 2021-02-13T01:02:50Z- AHRS_M2 msg: 1613178170.92 epoch sec; seqNo:17911 2021-02-13T01:02:50Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.12 psi 2021-02-13T01:02:50Z- handleDepth()-Depth_Keller msg: 1613178170929 epoch sec; seqNo:18054; depth 24.36 2021-02-13T01:02:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178170.94 epoch sec; seqNo:18054; 4060700.41 north; 593584.61 east 2021-02-13T01:02:51Z- AHRS_M2 msg: 1613178171.33 epoch sec; seqNo:17912 2021-02-13T01:02:51Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.12 psi 2021-02-13T01:02:51Z- handleDepth()-Depth_Keller msg: 1613178171337 epoch sec; seqNo:18055; depth 24.26 2021-02-13T01:02:51Z- handleDvl() - RDI_Pathfinder msg: 1613178171.35 epoch sec; seqNo:8747 2021-02-13T01:02:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.58 , 83.86 , 103.33 , 65.40 2021-02-13T01:02:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , 0.08 2021-02-13T01:02:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178171.36 epoch sec; seqNo:18055; 4060700.79 north; 593584.61 east 2021-02-13T01:02:51Z- AHRS_M2 msg: 1613178171.73 epoch sec; seqNo:17913 2021-02-13T01:02:51Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.12 psi 2021-02-13T01:02:51Z- handleDepth()-Depth_Keller msg: 1613178171732 epoch sec; seqNo:18056; depth 24.12 2021-02-13T01:02:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178171.75 epoch sec; seqNo:18056; 4060701.17 north; 593584.61 east 2021-02-13T01:02:52Z- AHRS_M2 msg: 1613178172.14 epoch sec; seqNo:17914 2021-02-13T01:02:52Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.13 psi 2021-02-13T01:02:52Z- LcmTrn::handleMessages() - handling 45 messages took 0.66 ms and 132 calls... 2021-02-13T01:02:52Z- waiting for 1613178169.90, time is 1613178172.14 2021-02-13T01:02:52Z- LcmTrn::updateTrn() >>>> pose time 1613178172.14, meas time 1613178171.35 <<<< 2021-02-13T01:02:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178171.35 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:02:52Z- TerrainNav::motionUpdate() - currEstimate = (4060701, 593584.61, 24.12) 2021-02-13T01:02:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:02:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:02:52Z- TNavBF::Projecting Measurements 2021-02-13T01:02:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:02:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:02:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:02:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:02:52Z- TNavFilter::4 good beams. 2021-02-13T01:02:52Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:02:52Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:02:52Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:02:52Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:02:52Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:02:52Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:02:52Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:02:52Z- TNavBF::mmse selected from filter -1 2021-02-13T01:02:52Z- TerrainMapOctree: (vehN, vehE ) = (4060700.65, 593584.43). 2021-02-13T01:02:52Z- TerrainMapOctree: Min Distance = 863.00. 2021-02-13T01:02:52Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:02:52Z- There is no specific good beam that all particles have in common. 2021-02-13T01:02:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:02:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 8.66 2021-02-13T01:02:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:02:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 11.25 2021-02-13T01:02:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:02:52Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 17.08 2021-02-13T01:02:52Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:02:52Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-13T01:02:52Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:02:52Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:02:52Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:02:52Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:02:52Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:52Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:02:52Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:02:52Z- Filter 0 No beams used in subcloud update 2021-02-13T01:02:52Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:02:52Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:02:52Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 677.9 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:02:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178171.35 sec. 2021-02-13T01:02:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:02:52Z- TNavBF::mmse selected from filter -1 2021-02-13T01:02:52Z- variances([0], [2], and psi_berg) = 677.869606 832.987584 0.000000 2021-02-13T01:02:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:02:52Z- LcmTrn::publishEstimates() - MLE : 4060700.67 593601.11 24.12 2021-02-13T01:02:52Z- LcmTrn::publishEstimates() - MMSE : 4060701.17 593584.61 24.12 2021-02-13T01:02:52Z- LcmTrn::publishEstimates() - COVAR: 677.87 832.99 0.89 0.00 2021-02-13T01:02:52Z- handleDepth()-Depth_Keller msg: 1613178172143 epoch sec; seqNo:18057; depth 24.02 2021-02-13T01:02:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178172.18 epoch sec; seqNo:18057; 4060701.55 north; 593584.60 east 2021-02-13T01:02:52Z- AHRS_M2 msg: 1613178172.54 epoch sec; seqNo:17915 2021-02-13T01:02:52Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.13 psi 2021-02-13T01:02:52Z- handleDepth()-Depth_Keller msg: 1613178172541 epoch sec; seqNo:18058; depth 23.89 2021-02-13T01:02:52Z- handleDvl() - RDI_Pathfinder msg: 1613178172.55 epoch sec; seqNo:8748 2021-02-13T01:02:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.99 , 84.87 , 98.94 , 65.40 2021-02-13T01:02:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.00 , 0.07 2021-02-13T01:02:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178172.56 epoch sec; seqNo:18058; 4060701.55 north; 593584.60 east 2021-02-13T01:02:52Z- AHRS_M2 msg: 1613178172.94 epoch sec; seqNo:17916 2021-02-13T01:02:52Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-13T01:02:52Z- handleDepth()-Depth_Keller msg: 1613178172949 epoch sec; seqNo:18059; depth 23.80 2021-02-13T01:02:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178172.96 epoch sec; seqNo:18059; 4060701.94 north; 593585.82 east 2021-02-13T01:02:53Z- AHRS_M2 msg: 1613178173.34 epoch sec; seqNo:17917 2021-02-13T01:02:53Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.15 psi 2021-02-13T01:02:53Z- handleDepth()-Depth_Keller msg: 1613178173348 epoch sec; seqNo:18060; depth 23.67 2021-02-13T01:02:53Z- handleDvl() - RDI_Pathfinder msg: 1613178173.36 epoch sec; seqNo:8749 2021-02-13T01:02:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 84.94 , 97.92 , 65.40 2021-02-13T01:02:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.08 , 0.03 2021-02-13T01:02:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178173.38 epoch sec; seqNo:18060; 4060702.32 north; 593585.81 east 2021-02-13T01:02:53Z- AHRS_M2 msg: 1613178173.75 epoch sec; seqNo:17918 2021-02-13T01:02:53Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-13T01:02:53Z- handleDepth()-Depth_Keller msg: 1613178173757 epoch sec; seqNo:18061; depth 23.57 2021-02-13T01:02:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:53Z- handleDvl() - RDI_Pathfinder msg: 1613178173.76 epoch sec; seqNo:8750 2021-02-13T01:02:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.08 , 0.03 2021-02-13T01:02:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178173.77 epoch sec; seqNo:18061; 4060702.32 north; 593585.81 east 2021-02-13T01:02:54Z- AHRS_M2 msg: 1613178174.16 epoch sec; seqNo:17919 2021-02-13T01:02:54Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-13T01:02:54Z- handleDepth()-Depth_Keller msg: 1613178174170 epoch sec; seqNo:18062; depth 23.46 2021-02-13T01:02:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178174.21 epoch sec; seqNo:18062; 4060702.70 north; 593585.81 east 2021-02-13T01:02:54Z- AHRS_M2 msg: 1613178174.56 epoch sec; seqNo:17920 2021-02-13T01:02:54Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.17 psi 2021-02-13T01:02:54Z- handleDepth()-Depth_Keller msg: 1613178174562 epoch sec; seqNo:18063; depth 23.36 2021-02-13T01:02:54Z- handleDvl() - RDI_Pathfinder msg: 1613178174.57 epoch sec; seqNo:8751 2021-02-13T01:02:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.35 , 86.97 , 97.79 , 66.22 2021-02-13T01:02:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.11 , -0.02 2021-02-13T01:02:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178174.58 epoch sec; seqNo:18063; 4060702.70 north; 593585.81 east 2021-02-13T01:02:54Z- AHRS_M2 msg: 1613178174.96 epoch sec; seqNo:17921 2021-02-13T01:02:54Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.17 psi 2021-02-13T01:02:54Z- handleDepth()-Depth_Keller msg: 1613178174964 epoch sec; seqNo:18064; depth 23.23 2021-02-13T01:02:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:54Z- handleDvl() - RDI_Pathfinder msg: 1613178174.97 epoch sec; seqNo:8752 2021-02-13T01:02:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.11 , -0.02 2021-02-13T01:02:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178174.99 epoch sec; seqNo:18064; 4060703.09 north; 593587.03 east 2021-02-13T01:02:55Z- AHRS_M2 msg: 1613178175.36 epoch sec; seqNo:17922 2021-02-13T01:02:55Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.17 psi 2021-02-13T01:02:55Z- handleDepth()-Depth_Keller msg: 1613178175368 epoch sec; seqNo:18065; depth 23.13 2021-02-13T01:02:55Z- handleDvl() - RDI_Pathfinder msg: 1613178175.38 epoch sec; seqNo:8753 2021-02-13T01:02:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 0.00 , 100.02 , 64.99 2021-02-13T01:02:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.11 , -0.05 2021-02-13T01:02:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178175.39 epoch sec; seqNo:18065; 4060703.47 north; 593587.02 east 2021-02-13T01:02:55Z- AHRS_M2 msg: 1613178175.77 epoch sec; seqNo:17923 2021-02-13T01:02:55Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.18 psi 2021-02-13T01:02:55Z- handleDepth()-Depth_Keller msg: 1613178175777 epoch sec; seqNo:18066; depth 23.00 2021-02-13T01:02:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178175.79 epoch sec; seqNo:18066; 4060703.47 north; 593587.02 east 2021-02-13T01:02:56Z- AHRS_M2 msg: 1613178176.17 epoch sec; seqNo:17924 2021-02-13T01:02:56Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.18 psi 2021-02-13T01:02:56Z- handleDepth()-Depth_Keller msg: 1613178176176 epoch sec; seqNo:18067; depth 22.90 2021-02-13T01:02:56Z- handleDvl() - RDI_Pathfinder msg: 1613178176.19 epoch sec; seqNo:8754 2021-02-13T01:02:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.35 , 0.00 , 97.86 , 66.22 2021-02-13T01:02:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.06 , -0.02 2021-02-13T01:02:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178176.22 epoch sec; seqNo:18067; 4060703.85 north; 593587.02 east 2021-02-13T01:02:56Z- AHRS_M2 msg: 1613178176.58 epoch sec; seqNo:17925 2021-02-13T01:02:56Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.18 psi 2021-02-13T01:02:56Z- handleDepth()-Depth_Keller msg: 1613178176580 epoch sec; seqNo:18068; depth 22.80 2021-02-13T01:02:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178176.60 epoch sec; seqNo:18068; 4060703.85 north; 593587.02 east 2021-02-13T01:02:56Z- AHRS_M2 msg: 1613178176.98 epoch sec; seqNo:17926 2021-02-13T01:02:56Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-13T01:02:56Z- handleDepth()-Depth_Keller msg: 1613178176989 epoch sec; seqNo:18069; depth 22.69 2021-02-13T01:02:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178177.01 epoch sec; seqNo:18069; 4060704.23 north; 593587.01 east 2021-02-13T01:02:57Z- LcmTrn::handleMessages() - handling 45 messages took 0.68 ms and 132 calls... 2021-02-13T01:02:57Z- waiting for 1613178175.14, time is 1613178177.01 2021-02-13T01:02:57Z- LcmTrn::updateTrn() >>>> pose time 1613178177.01, meas time 1613178176.19 <<<< 2021-02-13T01:02:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178176.19 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:02:57Z- TerrainNav::motionUpdate() - currEstimate = (4060704, 593587.01, 22.69) 2021-02-13T01:02:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:02:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:02:57Z- TNavBF::Projecting Measurements 2021-02-13T01:02:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:02:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:02:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:02:57Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:02:57Z- TNavFilter::3 good beams. 2021-02-13T01:02:57Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:02:57Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:02:57Z- TNavBF:: beamIndex[1] = 2 2021-02-13T01:02:57Z- TNavBF:: beamIndex[2] = 3 2021-02-13T01:02:57Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:02:57Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:02:57Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:02:57Z- TNavBF::mmse selected from filter -1 2021-02-13T01:02:57Z- TerrainMapOctree: (vehN, vehE ) = (4060703.71, 593586.61). 2021-02-13T01:02:57Z- TerrainMapOctree: Min Distance = 859.37. 2021-02-13T01:02:57Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:02:57Z- There is no specific good beam that all particles have in common. 2021-02-13T01:02:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:02:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 8.38 2021-02-13T01:02:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:02:57Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-13T01:02:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:02:57Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 7.02 2021-02-13T01:02:57Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:02:57Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:02:57Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:02:57Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:02:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:02:57Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:02:57Z- Filter 0 No beams used in subcloud update 2021-02-13T01:02:57Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:02:57Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:02:57Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 677.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T01:02:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178176.19 sec. 2021-02-13T01:02:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:02:57Z- TNavBF::mmse selected from filter -1 2021-02-13T01:02:57Z- variances([0], [2], and psi_berg) = 677.813623 832.987561 0.000000 2021-02-13T01:02:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:02:57Z- LcmTrn::publishEstimates() - MLE : 4060703.73 593603.51 22.69 2021-02-13T01:02:57Z- LcmTrn::publishEstimates() - MMSE : 4060704.23 593587.01 22.69 2021-02-13T01:02:57Z- LcmTrn::publishEstimates() - COVAR: 677.81 832.99 0.85 0.00 2021-02-13T01:02:57Z- AHRS_M2 msg: 1613178177.38 epoch sec; seqNo:17927 2021-02-13T01:02:57Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-13T01:02:57Z- handleDepth()-Depth_Keller msg: 1613178177388 epoch sec; seqNo:18070; depth 22.58 2021-02-13T01:02:57Z- handleDvl() - RDI_Pathfinder msg: 1613178177.41 epoch sec; seqNo:8755 2021-02-13T01:02:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 89.33 , 100.15 , 65.81 2021-02-13T01:02:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.04 , 0.01 2021-02-13T01:02:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178177.42 epoch sec; seqNo:18070; 4060704.62 north; 593588.23 east 2021-02-13T01:02:57Z- AHRS_M2 msg: 1613178177.79 epoch sec; seqNo:17928 2021-02-13T01:02:57Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-13T01:02:57Z- handleDepth()-Depth_Keller msg: 1613178177796 epoch sec; seqNo:18071; depth 22.47 2021-02-13T01:02:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178177.81 epoch sec; seqNo:18071; 4060704.62 north; 593588.23 east 2021-02-13T01:02:58Z- AHRS_M2 msg: 1613178178.22 epoch sec; seqNo:17929 2021-02-13T01:02:58Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.18 psi 2021-02-13T01:02:58Z- handleDepth()-Depth_Keller msg: 1613178178228 epoch sec; seqNo:18072; depth 22.37 2021-02-13T01:02:58Z- handleDvl() - RDI_Pathfinder msg: 1613178178.26 epoch sec; seqNo:8756 2021-02-13T01:02:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:58Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178178.27 epoch sec; seqNo:18072; 4060705.00 north; 593588.23 east 2021-02-13T01:02:58Z- AHRS_M2 msg: 1613178178.60 epoch sec; seqNo:17930 2021-02-13T01:02:58Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.18 psi 2021-02-13T01:02:58Z- handleDepth()-Depth_Keller msg: 1613178178601 epoch sec; seqNo:18073; depth 22.26 2021-02-13T01:02:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:58Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:58Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:58Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:58Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:58Z- handleDvl() - RDI_Pathfinder msg: 1613178178.60 epoch sec; seqNo:8757 2021-02-13T01:02:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:58Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:02:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178178.62 epoch sec; seqNo:18073; 4060705.38 north; 593588.22 east 2021-02-13T01:02:59Z- AHRS_M2 msg: 1613178179.00 epoch sec; seqNo:17931 2021-02-13T01:02:59Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.18 psi 2021-02-13T01:02:59Z- handleDepth()-Depth_Keller msg: 1613178179003 epoch sec; seqNo:18074; depth 22.16 2021-02-13T01:02:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178179.01 epoch sec; seqNo:18074; 4060705.38 north; 593588.22 east 2021-02-13T01:02:59Z- AHRS_M2 msg: 1613178179.40 epoch sec; seqNo:17932 2021-02-13T01:02:59Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.18 psi 2021-02-13T01:02:59Z- handleDepth()-Depth_Keller msg: 1613178179414 epoch sec; seqNo:18075; depth 22.05 2021-02-13T01:02:59Z- handleDvl() - RDI_Pathfinder msg: 1613178179.43 epoch sec; seqNo:8758 2021-02-13T01:02:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.18 , 90.08 , 100.49 , 0.00 2021-02-13T01:02:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , -0.03 2021-02-13T01:02:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178179.44 epoch sec; seqNo:18075; 4060705.75 north; 593588.22 east 2021-02-13T01:02:59Z- AHRS_M2 msg: 1613178179.81 epoch sec; seqNo:17933 2021-02-13T01:02:59Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-13T01:02:59Z- handleDepth()-Depth_Keller msg: 1613178179817 epoch sec; seqNo:18076; depth 21.96 2021-02-13T01:02:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:02:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:02:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:02:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:02:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:02:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:02:59Z- handleDvl() - RDI_Pathfinder msg: 1613178179.82 epoch sec; seqNo:8759 2021-02-13T01:02:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:02:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , -0.03 2021-02-13T01:02:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178179.84 epoch sec; seqNo:18076; 4060706.15 north; 593589.43 east 2021-02-13T01:03:00Z- AHRS_M2 msg: 1613178180.21 epoch sec; seqNo:17934 2021-02-13T01:03:00Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T01:03:00Z- handleDepth()-Depth_Keller msg: 1613178180221 epoch sec; seqNo:18077; depth 21.84 2021-02-13T01:03:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178180.23 epoch sec; seqNo:18077; 4060706.15 north; 593589.43 east 2021-02-13T01:03:00Z- AHRS_M2 msg: 1613178180.62 epoch sec; seqNo:17935 2021-02-13T01:03:00Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-13T01:03:00Z- handleDepth()-Depth_Keller msg: 1613178180622 epoch sec; seqNo:18078; depth 21.75 2021-02-13T01:03:00Z- handleDvl() - RDI_Pathfinder msg: 1613178180.63 epoch sec; seqNo:8760 2021-02-13T01:03:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 89.27 , 100.22 , 65.81 2021-02-13T01:03:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , -0.07 2021-02-13T01:03:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178180.64 epoch sec; seqNo:18078; 4060706.53 north; 593589.43 east 2021-02-13T01:03:01Z- AHRS_M2 msg: 1613178181.02 epoch sec; seqNo:17936 2021-02-13T01:03:01Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-13T01:03:01Z- handleDepth()-Depth_Keller msg: 1613178181024 epoch sec; seqNo:18079; depth 21.64 2021-02-13T01:03:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:01Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:01Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:01Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:01Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:01Z- handleDvl() - RDI_Pathfinder msg: 1613178181.03 epoch sec; seqNo:8761 2021-02-13T01:03:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , -0.07 2021-02-13T01:03:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178181.05 epoch sec; seqNo:18079; 4060706.90 north; 593589.43 east 2021-02-13T01:03:01Z- AHRS_M2 msg: 1613178181.42 epoch sec; seqNo:17937 2021-02-13T01:03:01Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T01:03:01Z- handleDepth()-Depth_Keller msg: 1613178181428 epoch sec; seqNo:18080; depth 21.54 2021-02-13T01:03:01Z- handleDvl() - RDI_Pathfinder msg: 1613178181.44 epoch sec; seqNo:8762 2021-02-13T01:03:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.58 , 98.33 , 100.76 , 68.26 2021-02-13T01:03:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , -0.09 2021-02-13T01:03:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178181.45 epoch sec; seqNo:18080; 4060706.90 north; 593589.43 east 2021-02-13T01:03:01Z- AHRS_M2 msg: 1613178181.83 epoch sec; seqNo:17938 2021-02-13T01:03:01Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-13T01:03:01Z- handleDepth()-Depth_Keller msg: 1613178181834 epoch sec; seqNo:18081; depth 21.44 2021-02-13T01:03:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178181.84 epoch sec; seqNo:18081; 4060707.28 north; 593589.42 east 2021-02-13T01:03:02Z- AHRS_M2 msg: 1613178182.23 epoch sec; seqNo:17939 2021-02-13T01:03:02Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-13T01:03:02Z- handleDepth()-Depth_Keller msg: 1613178182250 epoch sec; seqNo:18082; depth 21.34 2021-02-13T01:03:02Z- handleDvl() - RDI_Pathfinder msg: 1613178182.27 epoch sec; seqNo:8763 2021-02-13T01:03:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.58 , 98.19 , 101.85 , 67.45 2021-02-13T01:03:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.12 , -0.07 2021-02-13T01:03:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178182.28 epoch sec; seqNo:18082; 4060707.66 north; 593589.42 east 2021-02-13T01:03:02Z- LcmTrn::handleMessages() - handling 48 messages took 0.70 ms and 134 calls... 2021-02-13T01:03:02Z- waiting for 1613178180.01, time is 1613178182.28 2021-02-13T01:03:02Z- LcmTrn::updateTrn() equating pose 1613178182.28 and meas times 1613178182.28 2021-02-13T01:03:02Z- LcmTrn::updateTrn() >>>> pose time 1613178182.28, meas time 1613178182.28 <<<< 2021-02-13T01:03:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178182.28 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:03:02Z- TerrainNav::motionUpdate() - currEstimate = (4060708, 593589.42, 21.34) 2021-02-13T01:03:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:03:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:03:02Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178182.28 sec.; No relevant navigation data available 2021-02-13T01:03:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:03:02Z- TNavBF::mmse selected from filter -1 2021-02-13T01:03:02Z- variances([0], [2], and psi_berg) = 677.769683 832.987543 0.000000 2021-02-13T01:03:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:03:02Z- LcmTrn::publishEstimates() - MLE : 4060707.16 593605.92 21.34 2021-02-13T01:03:02Z- LcmTrn::publishEstimates() - MMSE : 4060707.66 593589.42 21.34 2021-02-13T01:03:02Z- LcmTrn::publishEstimates() - COVAR: 677.77 832.99 1.06 0.00 2021-02-13T01:03:02Z- AHRS_M2 msg: 1613178182.64 epoch sec; seqNo:17940 2021-02-13T01:03:02Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-13T01:03:02Z- handleDepth()-Depth_Keller msg: 1613178182640 epoch sec; seqNo:18083; depth 21.24 2021-02-13T01:03:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178182.65 epoch sec; seqNo:18083; 4060708.05 north; 593590.63 east 2021-02-13T01:03:03Z- AHRS_M2 msg: 1613178183.04 epoch sec; seqNo:17941 2021-02-13T01:03:03Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-13T01:03:03Z- handleDepth()-Depth_Keller msg: 1613178183049 epoch sec; seqNo:18084; depth 21.15 2021-02-13T01:03:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178183.06 epoch sec; seqNo:18084; 4060708.05 north; 593590.63 east 2021-02-13T01:03:03Z- AHRS_M2 msg: 1613178183.44 epoch sec; seqNo:17942 2021-02-13T01:03:03Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-13T01:03:03Z- handleDepth()-Depth_Keller msg: 1613178183449 epoch sec; seqNo:18085; depth 21.04 2021-02-13T01:03:03Z- handleDvl() - RDI_Pathfinder msg: 1613178183.46 epoch sec; seqNo:8764 2021-02-13T01:03:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 98.19 , 99.41 , 68.67 2021-02-13T01:03:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.07 , -0.04 2021-02-13T01:03:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178183.47 epoch sec; seqNo:18085; 4060708.43 north; 593590.63 east 2021-02-13T01:03:03Z- AHRS_M2 msg: 1613178183.85 epoch sec; seqNo:17943 2021-02-13T01:03:03Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-13T01:03:03Z- handleDepth()-Depth_Keller msg: 1613178183853 epoch sec; seqNo:18086; depth 20.93 2021-02-13T01:03:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178183.88 epoch sec; seqNo:18086; 4060708.81 north; 593590.63 east 2021-02-13T01:03:04Z- AHRS_M2 msg: 1613178184.25 epoch sec; seqNo:17944 2021-02-13T01:03:04Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-13T01:03:04Z- handleDepth()-Depth_Keller msg: 1613178184255 epoch sec; seqNo:18087; depth 20.83 2021-02-13T01:03:04Z- handleDvl() - RDI_Pathfinder msg: 1613178184.27 epoch sec; seqNo:8765 2021-02-13T01:03:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 89.61 , 99.34 , 67.86 2021-02-13T01:03:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , 0.05 2021-02-13T01:03:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178184.28 epoch sec; seqNo:18087; 4060709.19 north; 593590.62 east 2021-02-13T01:03:04Z- AHRS_M2 msg: 1613178184.65 epoch sec; seqNo:17945 2021-02-13T01:03:04Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-13T01:03:04Z- handleDepth()-Depth_Keller msg: 1613178184662 epoch sec; seqNo:18088; depth 20.73 2021-02-13T01:03:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:04Z- handleDvl() - RDI_Pathfinder msg: 1613178184.66 epoch sec; seqNo:8766 2021-02-13T01:03:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , 0.05 2021-02-13T01:03:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178184.67 epoch sec; seqNo:18088; 4060709.19 north; 593590.62 east 2021-02-13T01:03:05Z- AHRS_M2 msg: 1613178185.06 epoch sec; seqNo:17946 2021-02-13T01:03:05Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-13T01:03:05Z- handleDepth()-Depth_Keller msg: 1613178185063 epoch sec; seqNo:18089; depth 20.65 2021-02-13T01:03:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178185.07 epoch sec; seqNo:18089; 4060709.57 north; 593590.62 east 2021-02-13T01:03:05Z- AHRS_M2 msg: 1613178185.46 epoch sec; seqNo:17947 2021-02-13T01:03:05Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-13T01:03:05Z- handleDepth()-Depth_Keller msg: 1613178185473 epoch sec; seqNo:18090; depth 20.53 2021-02-13T01:03:05Z- handleDvl() - RDI_Pathfinder msg: 1613178185.48 epoch sec; seqNo:8767 2021-02-13T01:03:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 0.00 , 103.27 , 68.67 2021-02-13T01:03:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.11 2021-02-13T01:03:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178185.51 epoch sec; seqNo:18090; 4060709.95 north; 593590.61 east 2021-02-13T01:03:05Z- AHRS_M2 msg: 1613178185.87 epoch sec; seqNo:17948 2021-02-13T01:03:05Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.14 psi 2021-02-13T01:03:05Z- handleDepth()-Depth_Keller msg: 1613178185873 epoch sec; seqNo:18091; depth 20.45 2021-02-13T01:03:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:05Z- handleDvl() - RDI_Pathfinder msg: 1613178185.88 epoch sec; seqNo:8768 2021-02-13T01:03:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.11 2021-02-13T01:03:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178185.89 epoch sec; seqNo:18091; 4060710.34 north; 593591.83 east 2021-02-13T01:03:06Z- AHRS_M2 msg: 1613178186.27 epoch sec; seqNo:17949 2021-02-13T01:03:06Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.14 psi 2021-02-13T01:03:06Z- handleDepth()-Depth_Keller msg: 1613178186281 epoch sec; seqNo:18092; depth 20.34 2021-02-13T01:03:06Z- handleDvl() - RDI_Pathfinder msg: 1613178186.30 epoch sec; seqNo:8769 2021-02-13T01:03:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 0.00 , 101.98 , 68.67 2021-02-13T01:03:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.09 , 0.12 2021-02-13T01:03:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178186.31 epoch sec; seqNo:18092; 4060710.34 north; 593591.83 east 2021-02-13T01:03:06Z- AHRS_M2 msg: 1613178186.68 epoch sec; seqNo:17950 2021-02-13T01:03:06Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.14 psi 2021-02-13T01:03:06Z- handleDepth()-Depth_Keller msg: 1613178186680 epoch sec; seqNo:18093; depth 20.25 2021-02-13T01:03:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178186.69 epoch sec; seqNo:18093; 4060710.72 north; 593591.82 east 2021-02-13T01:03:07Z- AHRS_M2 msg: 1613178187.08 epoch sec; seqNo:17951 2021-02-13T01:03:07Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.14 psi 2021-02-13T01:03:07Z- handleDepth()-Depth_Keller msg: 1613178187086 epoch sec; seqNo:18094; depth 20.15 2021-02-13T01:03:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:07Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:07Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:07Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:07Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:07Z- handleDvl() - RDI_Pathfinder msg: 1613178187.09 epoch sec; seqNo:8770 2021-02-13T01:03:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.09 , 0.12 2021-02-13T01:03:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178187.10 epoch sec; seqNo:18094; 4060711.10 north; 593591.82 east 2021-02-13T01:03:07Z- LcmTrn::handleMessages() - handling 43 messages took 0.60 ms and 131 calls... 2021-02-13T01:03:07Z- waiting for 1613178185.28, time is 1613178187.10 2021-02-13T01:03:07Z- LcmTrn::updateTrn() equating pose 1613178187.10 and meas times 1613178187.10 2021-02-13T01:03:07Z- LcmTrn::updateTrn() >>>> pose time 1613178187.10, meas time 1613178187.10 <<<< 2021-02-13T01:03:07Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613178187.10, ping # 0. 2021-02-13T01:03:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178187.10 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:03:07Z- TerrainNav::motionUpdate() - currEstimate = (4060711, 593591.82, 20.15) 2021-02-13T01:03:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:03:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:03:07Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:03:07Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:03:07Z- TNavBF::Projecting Measurements 2021-02-13T01:03:07Z- TNavFilter::Measurement from time = 1613178187.10 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T01:03:07Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:03:07Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:03:07Z- TNavBF:: beamIndex[1] = 0 2021-02-13T01:03:07Z- TNavBF:: beamIndex[2] = 0 2021-02-13T01:03:07Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:03:07Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:03:07Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:03:07Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 677.7 EastVariance: 833.0 DepthVariance: 1.3 this->DepthBiasCov: 1.3 2021-02-13T01:03:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:03:07Z- TNavBF::mmse selected from filter -1 2021-02-13T01:03:07Z- variances([0], [2], and psi_berg) = 677.725764 832.987524 0.000000 2021-02-13T01:03:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:03:07Z- LcmTrn::publishEstimates() - MLE : 4060710.60 593608.32 20.15 2021-02-13T01:03:07Z- LcmTrn::publishEstimates() - MMSE : 4060711.10 593591.82 20.15 2021-02-13T01:03:07Z- LcmTrn::publishEstimates() - COVAR: 677.73 832.99 1.28 0.00 2021-02-13T01:03:07Z- AHRS_M2 msg: 1613178187.48 epoch sec; seqNo:17952 2021-02-13T01:03:07Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.14 psi 2021-02-13T01:03:07Z- handleDepth()-Depth_Keller msg: 1613178187488 epoch sec; seqNo:18095; depth 20.06 2021-02-13T01:03:07Z- handleDvl() - RDI_Pathfinder msg: 1613178187.51 epoch sec; seqNo:8771 2021-02-13T01:03:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 88.59 , 103.20 , 67.86 2021-02-13T01:03:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.22 , 0.03 2021-02-13T01:03:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178187.52 epoch sec; seqNo:18095; 4060711.48 north; 593591.82 east 2021-02-13T01:03:07Z- AHRS_M2 msg: 1613178187.89 epoch sec; seqNo:17953 2021-02-13T01:03:07Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.14 psi 2021-02-13T01:03:07Z- handleDepth()-Depth_Keller msg: 1613178187897 epoch sec; seqNo:18096; depth 19.94 2021-02-13T01:03:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178187.91 epoch sec; seqNo:18096; 4060711.49 north; 593593.04 east 2021-02-13T01:03:08Z- AHRS_M2 msg: 1613178188.29 epoch sec; seqNo:17954 2021-02-13T01:03:08Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.14 psi 2021-02-13T01:03:08Z- handleDepth()-Depth_Keller msg: 1613178188305 epoch sec; seqNo:18097; depth 19.86 2021-02-13T01:03:08Z- handleDvl() - RDI_Pathfinder msg: 1613178188.32 epoch sec; seqNo:8772 2021-02-13T01:03:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:08Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:03:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178188.33 epoch sec; seqNo:18097; 4060711.87 north; 593593.03 east 2021-02-13T01:03:08Z- AHRS_M2 msg: 1613178188.69 epoch sec; seqNo:17955 2021-02-13T01:03:08Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.14 psi 2021-02-13T01:03:08Z- handleDepth()-Depth_Keller msg: 1613178188713 epoch sec; seqNo:18098; depth 19.73 2021-02-13T01:03:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178188.72 epoch sec; seqNo:18098; 4060711.87 north; 593593.03 east 2021-02-13T01:03:09Z- AHRS_M2 msg: 1613178189.10 epoch sec; seqNo:17956 2021-02-13T01:03:09Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.14 psi 2021-02-13T01:03:09Z- handleDepth()-Depth_Keller msg: 1613178189109 epoch sec; seqNo:18099; depth 19.64 2021-02-13T01:03:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178189.13 epoch sec; seqNo:18099; 4060712.25 north; 593593.03 east 2021-02-13T01:03:09Z- AHRS_M2 msg: 1613178189.50 epoch sec; seqNo:17957 2021-02-13T01:03:09Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.15 psi 2021-02-13T01:03:09Z- handleDepth()-Depth_Keller msg: 1613178189508 epoch sec; seqNo:18100; depth 19.53 2021-02-13T01:03:09Z- handleDvl() - RDI_Pathfinder msg: 1613178189.52 epoch sec; seqNo:8773 2021-02-13T01:03:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.79 , 98.26 , 103.13 , 69.49 2021-02-13T01:03:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.10 , -0.21 2021-02-13T01:03:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178189.55 epoch sec; seqNo:18100; 4060712.64 north; 593594.25 east 2021-02-13T01:03:09Z- AHRS_M2 msg: 1613178189.91 epoch sec; seqNo:17958 2021-02-13T01:03:09Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-13T01:03:09Z- handleDepth()-Depth_Keller msg: 1613178189912 epoch sec; seqNo:18101; depth 19.42 2021-02-13T01:03:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178189.92 epoch sec; seqNo:18101; 4060712.64 north; 593594.25 east 2021-02-13T01:03:10Z- AHRS_M2 msg: 1613178190.32 epoch sec; seqNo:17959 2021-02-13T01:03:10Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.16 psi 2021-02-13T01:03:10Z- handleDepth()-Depth_Keller msg: 1613178190337 epoch sec; seqNo:18102; depth 19.30 2021-02-13T01:03:10Z- handleDvl() - RDI_Pathfinder msg: 1613178190.35 epoch sec; seqNo:8774 2021-02-13T01:03:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.99 , 105.29 , 67.86 2021-02-13T01:03:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.20 2021-02-13T01:03:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178190.36 epoch sec; seqNo:18102; 4060713.02 north; 593594.24 east 2021-02-13T01:03:10Z- AHRS_M2 msg: 1613178190.72 epoch sec; seqNo:17960 2021-02-13T01:03:10Z- AHRS_M2 msg: 0.65 phi; 0.33 theta; -0.16 psi 2021-02-13T01:03:10Z- handleDepth()-Depth_Keller msg: 1613178190720 epoch sec; seqNo:18103; depth 19.19 2021-02-13T01:03:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:10Z- handleDvl() - RDI_Pathfinder msg: 1613178190.72 epoch sec; seqNo:8775 2021-02-13T01:03:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.20 2021-02-13T01:03:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178190.74 epoch sec; seqNo:18103; 4060713.02 north; 593594.24 east 2021-02-13T01:03:11Z- AHRS_M2 msg: 1613178191.12 epoch sec; seqNo:17961 2021-02-13T01:03:11Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.16 psi 2021-02-13T01:03:11Z- handleDepth()-Depth_Keller msg: 1613178191129 epoch sec; seqNo:18104; depth 19.08 2021-02-13T01:03:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178191.14 epoch sec; seqNo:18104; 4060713.40 north; 593594.24 east 2021-02-13T01:03:11Z- AHRS_M2 msg: 1613178191.52 epoch sec; seqNo:17962 2021-02-13T01:03:11Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.17 psi 2021-02-13T01:03:11Z- handleDepth()-Depth_Keller msg: 1613178191536 epoch sec; seqNo:18105; depth 18.98 2021-02-13T01:03:11Z- handleDvl() - RDI_Pathfinder msg: 1613178191.55 epoch sec; seqNo:8776 2021-02-13T01:03:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 93.12 , 104.08 , 68.26 2021-02-13T01:03:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.11 , -0.07 2021-02-13T01:03:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178191.56 epoch sec; seqNo:18105; 4060713.78 north; 593594.23 east 2021-02-13T01:03:11Z- AHRS_M2 msg: 1613178191.93 epoch sec; seqNo:17963 2021-02-13T01:03:11Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T01:03:11Z- handleDepth()-Depth_Keller msg: 1613178191932 epoch sec; seqNo:18106; depth 18.85 2021-02-13T01:03:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:11Z- handleDvl() - RDI_Pathfinder msg: 1613178191.93 epoch sec; seqNo:8777 2021-02-13T01:03:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.11 , -0.07 2021-02-13T01:03:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178191.96 epoch sec; seqNo:18106; 4060714.16 north; 593594.23 east 2021-02-13T01:03:12Z- AHRS_M2 msg: 1613178192.33 epoch sec; seqNo:17964 2021-02-13T01:03:12Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-13T01:03:12Z- handleDepth()-Depth_Keller msg: 1613178192337 epoch sec; seqNo:18107; depth 18.75 2021-02-13T01:03:12Z- handleDvl() - RDI_Pathfinder msg: 1613178192.35 epoch sec; seqNo:8778 2021-02-13T01:03:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 98.13 , 106.65 , 78.65 2021-02-13T01:03:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.10 , 0.07 2021-02-13T01:03:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178192.36 epoch sec; seqNo:18107; 4060714.16 north; 593594.23 east 2021-02-13T01:03:12Z- LcmTrn::handleMessages() - handling 47 messages took 0.70 ms and 133 calls... 2021-02-13T01:03:12Z- waiting for 1613178190.10, time is 1613178192.36 2021-02-13T01:03:12Z- LcmTrn::updateTrn() equating pose 1613178192.36 and meas times 1613178192.36 2021-02-13T01:03:12Z- LcmTrn::updateTrn() >>>> pose time 1613178192.36, meas time 1613178192.36 <<<< 2021-02-13T01:03:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178192.36 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:03:12Z- TerrainNav::motionUpdate() - currEstimate = (4060714, 593594.23, 18.75) 2021-02-13T01:03:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:03:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:03:12Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178192.36 sec.; No relevant navigation data available 2021-02-13T01:03:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:03:12Z- TNavBF::mmse selected from filter -1 2021-02-13T01:03:12Z- variances([0], [2], and psi_berg) = 677.683474 832.987507 0.000000 2021-02-13T01:03:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:03:12Z- LcmTrn::publishEstimates() - MLE : 4060713.66 593610.73 18.75 2021-02-13T01:03:12Z- LcmTrn::publishEstimates() - MMSE : 4060714.16 593594.23 18.75 2021-02-13T01:03:12Z- LcmTrn::publishEstimates() - COVAR: 677.68 832.99 1.48 0.00 2021-02-13T01:03:12Z- AHRS_M2 msg: 1613178192.74 epoch sec; seqNo:17965 2021-02-13T01:03:12Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T01:03:12Z- handleDepth()-Depth_Keller msg: 1613178192741 epoch sec; seqNo:18108; depth 18.63 2021-02-13T01:03:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178192.75 epoch sec; seqNo:18108; 4060714.55 north; 593595.44 east 2021-02-13T01:03:13Z- AHRS_M2 msg: 1613178193.14 epoch sec; seqNo:17966 2021-02-13T01:03:13Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T01:03:13Z- handleDepth()-Depth_Keller msg: 1613178193150 epoch sec; seqNo:18109; depth 18.54 2021-02-13T01:03:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:13Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:13Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:13Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:13Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:13Z- handleDvl() - RDI_Pathfinder msg: 1613178193.15 epoch sec; seqNo:8779 2021-02-13T01:03:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.10 , 0.07 2021-02-13T01:03:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178193.16 epoch sec; seqNo:18109; 4060714.93 north; 593595.44 east 2021-02-13T01:03:13Z- AHRS_M2 msg: 1613178193.55 epoch sec; seqNo:17967 2021-02-13T01:03:13Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.17 psi 2021-02-13T01:03:13Z- handleDepth()-Depth_Keller msg: 1613178193566 epoch sec; seqNo:18110; depth 18.42 2021-02-13T01:03:13Z- handleDvl() - RDI_Pathfinder msg: 1613178193.59 epoch sec; seqNo:8780 2021-02-13T01:03:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.26 , 97.72 , 106.24 , 78.24 2021-02-13T01:03:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.02 , 0.14 2021-02-13T01:03:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178193.60 epoch sec; seqNo:18110; 4060715.31 north; 593595.44 east 2021-02-13T01:03:13Z- AHRS_M2 msg: 1613178193.95 epoch sec; seqNo:17968 2021-02-13T01:03:13Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.17 psi 2021-02-13T01:03:13Z- handleDepth()-Depth_Keller msg: 1613178193952 epoch sec; seqNo:18111; depth 18.30 2021-02-13T01:03:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178193.96 epoch sec; seqNo:18111; 4060715.31 north; 593595.44 east 2021-02-13T01:03:14Z- AHRS_M2 msg: 1613178194.35 epoch sec; seqNo:17969 2021-02-13T01:03:14Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.17 psi 2021-02-13T01:03:14Z- handleDepth()-Depth_Keller msg: 1613178194357 epoch sec; seqNo:18112; depth 18.20 2021-02-13T01:03:14Z- handleDvl() - RDI_Pathfinder msg: 1613178194.37 epoch sec; seqNo:8781 2021-02-13T01:03:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 97.79 , 105.09 , 69.90 2021-02-13T01:03:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.11 , 0.10 2021-02-13T01:03:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178194.38 epoch sec; seqNo:18112; 4060715.68 north; 593595.43 east 2021-02-13T01:03:14Z- AHRS_M2 msg: 1613178194.76 epoch sec; seqNo:17970 2021-02-13T01:03:14Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.16 psi 2021-02-13T01:03:14Z- handleDepth()-Depth_Keller msg: 1613178194777 epoch sec; seqNo:18113; depth 18.07 2021-02-13T01:03:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178194.79 epoch sec; seqNo:18113; 4060716.06 north; 593595.43 east 2021-02-13T01:03:15Z- AHRS_M2 msg: 1613178195.17 epoch sec; seqNo:17971 2021-02-13T01:03:15Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-13T01:03:15Z- handleDepth()-Depth_Keller msg: 1613178195172 epoch sec; seqNo:18114; depth 17.97 2021-02-13T01:03:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178195.19 epoch sec; seqNo:18114; 4060716.08 north; 593596.65 east 2021-02-13T01:03:15Z- AHRS_M2 msg: 1613178195.57 epoch sec; seqNo:17972 2021-02-13T01:03:15Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-13T01:03:15Z- handleDepth()-Depth_Keller msg: 1613178195572 epoch sec; seqNo:18115; depth 17.86 2021-02-13T01:03:15Z- handleDvl() - RDI_Pathfinder msg: 1613178195.59 epoch sec; seqNo:8782 2021-02-13T01:03:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.73 , 93.12 , 105.29 , 71.53 2021-02-13T01:03:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.15 , -0.00 2021-02-13T01:03:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178195.61 epoch sec; seqNo:18115; 4060716.46 north; 593596.64 east 2021-02-13T01:03:15Z- AHRS_M2 msg: 1613178195.97 epoch sec; seqNo:17973 2021-02-13T01:03:15Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.16 psi 2021-02-13T01:03:15Z- handleDepth()-Depth_Keller msg: 1613178195973 epoch sec; seqNo:18116; depth 17.73 2021-02-13T01:03:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178195.98 epoch sec; seqNo:18116; 4060716.83 north; 593596.64 east 2021-02-13T01:03:16Z- AHRS_M2 msg: 1613178196.37 epoch sec; seqNo:17974 2021-02-13T01:03:16Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.16 psi 2021-02-13T01:03:16Z- handleDepth()-Depth_Keller msg: 1613178196386 epoch sec; seqNo:18117; depth 17.65 2021-02-13T01:03:16Z- handleDvl() - RDI_Pathfinder msg: 1613178196.40 epoch sec; seqNo:8783 2021-02-13T01:03:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.73 , 97.99 , 106.51 , 71.53 2021-02-13T01:03:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.11 , -0.08 2021-02-13T01:03:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178196.41 epoch sec; seqNo:18117; 4060716.83 north; 593596.64 east 2021-02-13T01:03:16Z- AHRS_M2 msg: 1613178196.78 epoch sec; seqNo:17975 2021-02-13T01:03:16Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-13T01:03:16Z- handleDepth()-Depth_Keller msg: 1613178196780 epoch sec; seqNo:18118; depth 17.52 2021-02-13T01:03:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:16Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:16Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:16Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:16Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:16Z- handleDvl() - RDI_Pathfinder msg: 1613178196.78 epoch sec; seqNo:8784 2021-02-13T01:03:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.11 , -0.08 2021-02-13T01:03:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178196.80 epoch sec; seqNo:18118; 4060717.21 north; 593596.64 east 2021-02-13T01:03:17Z- AHRS_M2 msg: 1613178197.18 epoch sec; seqNo:17976 2021-02-13T01:03:17Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.18 psi 2021-02-13T01:03:17Z- handleDepth()-Depth_Keller msg: 1613178197185 epoch sec; seqNo:18119; depth 17.41 2021-02-13T01:03:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178197.19 epoch sec; seqNo:18119; 4060717.23 north; 593597.86 east 2021-02-13T01:03:17Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-13T01:03:17Z- waiting for 1613178195.36, time is 1613178197.19 2021-02-13T01:03:17Z- LcmTrn::updateTrn() >>>> pose time 1613178197.19, meas time 1613178196.78 <<<< 2021-02-13T01:03:17Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613178196.78, ping # 0. 2021-02-13T01:03:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178196.78 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:03:17Z- TerrainNav::motionUpdate() - currEstimate = (4060717, 593597.86, 17.41) 2021-02-13T01:03:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:03:17Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:03:17Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:03:17Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:03:17Z- TNavBF::Projecting Measurements 2021-02-13T01:03:17Z- TNavFilter::Measurement from time = 1613178196.78 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T01:03:17Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:03:17Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:03:17Z- TNavBF:: beamIndex[1] = 0 2021-02-13T01:03:17Z- TNavBF:: beamIndex[2] = 0 2021-02-13T01:03:17Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:03:17Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:03:17Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:03:17Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 677.6 EastVariance: 833.0 DepthVariance: 1.7 this->DepthBiasCov: 1.7 2021-02-13T01:03:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:03:17Z- TNavBF::mmse selected from filter -1 2021-02-13T01:03:17Z- variances([0], [2], and psi_berg) = 677.625160 832.987483 0.000000 2021-02-13T01:03:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:03:17Z- LcmTrn::publishEstimates() - MLE : 4060716.73 593614.36 17.41 2021-02-13T01:03:17Z- LcmTrn::publishEstimates() - MMSE : 4060717.23 593597.86 17.41 2021-02-13T01:03:17Z- LcmTrn::publishEstimates() - COVAR: 677.63 832.99 1.72 0.00 2021-02-13T01:03:17Z- AHRS_M2 msg: 1613178197.58 epoch sec; seqNo:17977 2021-02-13T01:03:17Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.18 psi 2021-02-13T01:03:17Z- handleDepth()-Depth_Keller msg: 1613178197591 epoch sec; seqNo:18120; depth 17.30 2021-02-13T01:03:17Z- handleDvl() - RDI_Pathfinder msg: 1613178197.61 epoch sec; seqNo:8785 2021-02-13T01:03:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.60 , 97.86 , 108.81 , 69.49 2021-02-13T01:03:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.10 2021-02-13T01:03:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178197.62 epoch sec; seqNo:18120; 4060717.61 north; 593597.85 east 2021-02-13T01:03:17Z- AHRS_M2 msg: 1613178197.99 epoch sec; seqNo:17978 2021-02-13T01:03:17Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.19 psi 2021-02-13T01:03:17Z- handleDepth()-Depth_Keller msg: 1613178197997 epoch sec; seqNo:18121; depth 17.18 2021-02-13T01:03:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:18Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:18Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:18Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:18Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:18Z- handleDvl() - RDI_Pathfinder msg: 1613178198.00 epoch sec; seqNo:8786 2021-02-13T01:03:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.10 2021-02-13T01:03:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178198.01 epoch sec; seqNo:18121; 4060717.98 north; 593597.85 east 2021-02-13T01:03:18Z- AHRS_M2 msg: 1613178198.40 epoch sec; seqNo:17979 2021-02-13T01:03:18Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.19 psi 2021-02-13T01:03:18Z- handleDepth()-Depth_Keller msg: 1613178198400 epoch sec; seqNo:18122; depth 17.07 2021-02-13T01:03:18Z- handleDvl() - RDI_Pathfinder msg: 1613178198.41 epoch sec; seqNo:8787 2021-02-13T01:03:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.85 , 97.86 , 107.59 , 70.31 2021-02-13T01:03:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , -0.06 2021-02-13T01:03:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178198.42 epoch sec; seqNo:18122; 4060718.36 north; 593597.84 east 2021-02-13T01:03:18Z- AHRS_M2 msg: 1613178198.80 epoch sec; seqNo:17980 2021-02-13T01:03:18Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.19 psi 2021-02-13T01:03:18Z- handleDepth()-Depth_Keller msg: 1613178198805 epoch sec; seqNo:18123; depth 16.95 2021-02-13T01:03:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178198.82 epoch sec; seqNo:18123; 4060718.36 north; 593597.84 east 2021-02-13T01:03:19Z- AHRS_M2 msg: 1613178199.22 epoch sec; seqNo:17981 2021-02-13T01:03:19Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.19 psi 2021-02-13T01:03:19Z- handleDepth()-Depth_Keller msg: 1613178199235 epoch sec; seqNo:18124; depth 16.85 2021-02-13T01:03:19Z- handleDvl() - RDI_Pathfinder msg: 1613178199.25 epoch sec; seqNo:8788 2021-02-13T01:03:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.26 , 97.92 , 110.10 , 72.76 2021-02-13T01:03:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.03 2021-02-13T01:03:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178199.26 epoch sec; seqNo:18124; 4060718.74 north; 593597.84 east 2021-02-13T01:03:19Z- AHRS_M2 msg: 1613178199.61 epoch sec; seqNo:17982 2021-02-13T01:03:19Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.19 psi 2021-02-13T01:03:19Z- handleDepth()-Depth_Keller msg: 1613178199617 epoch sec; seqNo:18125; depth 16.72 2021-02-13T01:03:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178199.63 epoch sec; seqNo:18125; 4060719.12 north; 593597.84 east 2021-02-13T01:03:20Z- AHRS_M2 msg: 1613178200.01 epoch sec; seqNo:17983 2021-02-13T01:03:20Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.19 psi 2021-02-13T01:03:20Z- handleDepth()-Depth_Keller msg: 1613178200011 epoch sec; seqNo:18126; depth 16.62 2021-02-13T01:03:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178200.02 epoch sec; seqNo:18126; 4060719.51 north; 593599.05 east 2021-02-13T01:03:20Z- AHRS_M2 msg: 1613178200.41 epoch sec; seqNo:17984 2021-02-13T01:03:20Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.19 psi 2021-02-13T01:03:20Z- handleDepth()-Depth_Keller msg: 1613178200416 epoch sec; seqNo:18127; depth 16.53 2021-02-13T01:03:20Z- handleDvl() - RDI_Pathfinder msg: 1613178200.43 epoch sec; seqNo:8789 2021-02-13T01:03:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.53 , 97.79 , 107.53 , 78.31 2021-02-13T01:03:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , 0.10 2021-02-13T01:03:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178200.44 epoch sec; seqNo:18127; 4060719.51 north; 593599.05 east 2021-02-13T01:03:20Z- AHRS_M2 msg: 1613178200.82 epoch sec; seqNo:17985 2021-02-13T01:03:20Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.18 psi 2021-02-13T01:03:20Z- handleDepth()-Depth_Keller msg: 1613178200821 epoch sec; seqNo:18128; depth 16.40 2021-02-13T01:03:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178200.83 epoch sec; seqNo:18128; 4060719.89 north; 593599.05 east 2021-02-13T01:03:21Z- AHRS_M2 msg: 1613178201.22 epoch sec; seqNo:17986 2021-02-13T01:03:21Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.18 psi 2021-02-13T01:03:21Z- handleDepth()-Depth_Keller msg: 1613178201234 epoch sec; seqNo:18129; depth 16.29 2021-02-13T01:03:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178201.24 epoch sec; seqNo:18129; 4060720.27 north; 593599.04 east 2021-02-13T01:03:21Z- AHRS_M2 msg: 1613178201.62 epoch sec; seqNo:17987 2021-02-13T01:03:21Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.17 psi 2021-02-13T01:03:21Z- handleDepth()-Depth_Keller msg: 1613178201628 epoch sec; seqNo:18130; depth 16.20 2021-02-13T01:03:21Z- handleDvl() - RDI_Pathfinder msg: 1613178201.64 epoch sec; seqNo:8790 2021-02-13T01:03:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 97.72 , 108.68 , 71.94 2021-02-13T01:03:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.15 , 0.07 2021-02-13T01:03:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178201.67 epoch sec; seqNo:18130; 4060720.65 north; 593599.04 east 2021-02-13T01:03:22Z- AHRS_M2 msg: 1613178202.03 epoch sec; seqNo:17988 2021-02-13T01:03:22Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.17 psi 2021-02-13T01:03:22Z- handleDepth()-Depth_Keller msg: 1613178202032 epoch sec; seqNo:18131; depth 16.09 2021-02-13T01:03:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178202.04 epoch sec; seqNo:18131; 4060720.66 north; 593600.26 east 2021-02-13T01:03:22Z- AHRS_M2 msg: 1613178202.43 epoch sec; seqNo:17989 2021-02-13T01:03:22Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.16 psi 2021-02-13T01:03:22Z- LcmTrn::handleMessages() - handling 43 messages took 0.66 ms and 130 calls... 2021-02-13T01:03:22Z- waiting for 1613178200.19, time is 1613178202.43 2021-02-13T01:03:22Z- LcmTrn::updateTrn() >>>> pose time 1613178202.43, meas time 1613178201.64 <<<< 2021-02-13T01:03:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178201.64 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:03:22Z- TerrainNav::motionUpdate() - currEstimate = (4060721, 593600.26, 16.09) 2021-02-13T01:03:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:03:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:03:22Z- TNavBF::Projecting Measurements 2021-02-13T01:03:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:03:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:03:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:03:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:03:22Z- TNavFilter::4 good beams. 2021-02-13T01:03:22Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:03:22Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:03:22Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:03:22Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:03:22Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:03:22Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:03:22Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:03:22Z- TNavBF::mmse selected from filter -1 2021-02-13T01:03:22Z- TerrainMapOctree: (vehN, vehE ) = (4060720.14, 593599.90). 2021-02-13T01:03:22Z- TerrainMapOctree: Min Distance = 838.68. 2021-02-13T01:03:22Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:03:22Z- There is no specific good beam that all particles have in common. 2021-02-13T01:03:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:03:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.90 K: 0.135 ExpectedMeasDiff: nan currMeas.covariance: 10.41 2021-02-13T01:03:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:03:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.64 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T01:03:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:03:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.50 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 18.90 2021-02-13T01:03:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:03:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.122 ExpectedMeasDiff: nan currMeas.covariance: 8.28 2021-02-13T01:03:22Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:03:22Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:03:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:03:22Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:03:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:03:22Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:03:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:03:22Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:03:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:03:22Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:03:22Z- Filter 0 No beams used in subcloud update 2021-02-13T01:03:22Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:03:22Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:03:22Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 677.6 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T01:03:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178201.64 sec. 2021-02-13T01:03:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:03:22Z- TNavBF::mmse selected from filter -1 2021-02-13T01:03:22Z- variances([0], [2], and psi_berg) = 677.567707 832.987459 0.000000 2021-02-13T01:03:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:03:22Z- LcmTrn::publishEstimates() - MLE : 4060720.16 593616.76 16.09 2021-02-13T01:03:22Z- LcmTrn::publishEstimates() - MMSE : 4060720.66 593600.26 16.09 2021-02-13T01:03:22Z- LcmTrn::publishEstimates() - COVAR: 677.57 832.99 1.26 0.00 2021-02-13T01:03:22Z- handleDepth()-Depth_Keller msg: 1613178202437 epoch sec; seqNo:18132; depth 16.00 2021-02-13T01:03:22Z- handleDvl() - RDI_Pathfinder msg: 1613178202.45 epoch sec; seqNo:8791 2021-02-13T01:03:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.75 , 97.79 , 107.53 , 78.31 2021-02-13T01:03:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.19 , -0.06 2021-02-13T01:03:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178202.46 epoch sec; seqNo:18132; 4060721.04 north; 593600.26 east 2021-02-13T01:03:22Z- AHRS_M2 msg: 1613178202.84 epoch sec; seqNo:17990 2021-02-13T01:03:22Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.16 psi 2021-02-13T01:03:22Z- handleDepth()-Depth_Keller msg: 1613178202839 epoch sec; seqNo:18133; depth 15.88 2021-02-13T01:03:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:22Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:22Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:22Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:22Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:22Z- handleDvl() - RDI_Pathfinder msg: 1613178202.84 epoch sec; seqNo:8792 2021-02-13T01:03:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.19 , -0.06 2021-02-13T01:03:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178202.85 epoch sec; seqNo:18133; 4060721.42 north; 593600.25 east 2021-02-13T01:03:23Z- AHRS_M2 msg: 1613178203.24 epoch sec; seqNo:17991 2021-02-13T01:03:23Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T01:03:23Z- handleDepth()-Depth_Keller msg: 1613178203247 epoch sec; seqNo:18134; depth 15.79 2021-02-13T01:03:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178203.26 epoch sec; seqNo:18134; 4060721.42 north; 593600.25 east 2021-02-13T01:03:23Z- AHRS_M2 msg: 1613178203.64 epoch sec; seqNo:17992 2021-02-13T01:03:23Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T01:03:23Z- handleDepth()-Depth_Keller msg: 1613178203661 epoch sec; seqNo:18135; depth 15.69 2021-02-13T01:03:23Z- handleDvl() - RDI_Pathfinder msg: 1613178203.68 epoch sec; seqNo:8793 2021-02-13T01:03:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 95.22 , 108.61 , 71.12 2021-02-13T01:03:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.14 , -0.20 2021-02-13T01:03:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178203.69 epoch sec; seqNo:18135; 4060721.80 north; 593600.25 east 2021-02-13T01:03:24Z- AHRS_M2 msg: 1613178204.05 epoch sec; seqNo:17993 2021-02-13T01:03:24Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-13T01:03:24Z- handleDepth()-Depth_Keller msg: 1613178204052 epoch sec; seqNo:18136; depth 15.56 2021-02-13T01:03:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:24Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:24Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:24Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:24Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:24Z- handleDvl() - RDI_Pathfinder msg: 1613178204.05 epoch sec; seqNo:8794 2021-02-13T01:03:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.14 , -0.20 2021-02-13T01:03:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178204.07 epoch sec; seqNo:18136; 4060721.80 north; 593600.25 east 2021-02-13T01:03:24Z- AHRS_M2 msg: 1613178204.45 epoch sec; seqNo:17994 2021-02-13T01:03:24Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-13T01:03:24Z- handleDepth()-Depth_Keller msg: 1613178204456 epoch sec; seqNo:18137; depth 15.48 2021-02-13T01:03:24Z- handleDvl() - RDI_Pathfinder msg: 1613178204.47 epoch sec; seqNo:8795 2021-02-13T01:03:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 95.42 , 107.59 , 70.72 2021-02-13T01:03:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.04 , -0.22 2021-02-13T01:03:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178204.48 epoch sec; seqNo:18137; 4060722.19 north; 593601.46 east 2021-02-13T01:03:24Z- AHRS_M2 msg: 1613178204.86 epoch sec; seqNo:17995 2021-02-13T01:03:24Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.18 psi 2021-02-13T01:03:24Z- handleDepth()-Depth_Keller msg: 1613178204869 epoch sec; seqNo:18138; depth 15.36 2021-02-13T01:03:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178204.88 epoch sec; seqNo:18138; 4060722.57 north; 593601.46 east 2021-02-13T01:03:25Z- AHRS_M2 msg: 1613178205.26 epoch sec; seqNo:17996 2021-02-13T01:03:25Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.18 psi 2021-02-13T01:03:25Z- handleDepth()-Depth_Keller msg: 1613178205272 epoch sec; seqNo:18139; depth 15.25 2021-02-13T01:03:25Z- handleDvl() - RDI_Pathfinder msg: 1613178205.31 epoch sec; seqNo:8796 2021-02-13T01:03:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.89 , 97.65 , 108.61 , 78.18 2021-02-13T01:03:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.13 , -0.09 2021-02-13T01:03:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178205.32 epoch sec; seqNo:18139; 4060722.95 north; 593601.46 east 2021-02-13T01:03:25Z- AHRS_M2 msg: 1613178205.66 epoch sec; seqNo:17997 2021-02-13T01:03:25Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-13T01:03:25Z- handleDepth()-Depth_Keller msg: 1613178205668 epoch sec; seqNo:18140; depth 15.14 2021-02-13T01:03:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178205.68 epoch sec; seqNo:18140; 4060722.95 north; 593601.46 east 2021-02-13T01:03:26Z- AHRS_M2 msg: 1613178206.07 epoch sec; seqNo:17998 2021-02-13T01:03:26Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.17 psi 2021-02-13T01:03:26Z- handleDepth()-Depth_Keller msg: 1613178206073 epoch sec; seqNo:18141; depth 15.04 2021-02-13T01:03:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178206.08 epoch sec; seqNo:18141; 4060723.33 north; 593601.45 east 2021-02-13T01:03:26Z- AHRS_M2 msg: 1613178206.47 epoch sec; seqNo:17999 2021-02-13T01:03:26Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.16 psi 2021-02-13T01:03:26Z- handleDepth()-Depth_Keller msg: 1613178206476 epoch sec; seqNo:18142; depth 14.92 2021-02-13T01:03:26Z- handleDvl() - RDI_Pathfinder msg: 1613178206.49 epoch sec; seqNo:8797 2021-02-13T01:03:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 97.72 , 105.02 , 0.00 2021-02-13T01:03:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , 0.07 2021-02-13T01:03:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178206.51 epoch sec; seqNo:18142; 4060723.71 north; 593601.45 east 2021-02-13T01:03:26Z- AHRS_M2 msg: 1613178206.88 epoch sec; seqNo:18000 2021-02-13T01:03:26Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.16 psi 2021-02-13T01:03:26Z- handleDepth()-Depth_Keller msg: 1613178206880 epoch sec; seqNo:18143; depth 14.81 2021-02-13T01:03:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178206.89 epoch sec; seqNo:18143; 4060724.09 north; 593601.44 east 2021-02-13T01:03:27Z- AHRS_M2 msg: 1613178207.28 epoch sec; seqNo:18001 2021-02-13T01:03:27Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.16 psi 2021-02-13T01:03:27Z- handleDepth()-Depth_Keller msg: 1613178207288 epoch sec; seqNo:18144; depth 14.71 2021-02-13T01:03:27Z- handleDvl() - RDI_Pathfinder msg: 1613178207.30 epoch sec; seqNo:8798 2021-02-13T01:03:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.64 , 97.52 , 108.34 , 73.58 2021-02-13T01:03:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , 0.12 2021-02-13T01:03:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178207.31 epoch sec; seqNo:18144; 4060724.09 north; 593601.44 east 2021-02-13T01:03:27Z- LcmTrn::handleMessages() - handling 46 messages took 0.70 ms and 132 calls... 2021-02-13T01:03:27Z- waiting for 1613178205.43, time is 1613178207.31 2021-02-13T01:03:27Z- LcmTrn::updateTrn() equating pose 1613178207.31 and meas times 1613178207.31 2021-02-13T01:03:27Z- LcmTrn::updateTrn() >>>> pose time 1613178207.31, meas time 1613178207.31 <<<< 2021-02-13T01:03:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178207.31 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:03:27Z- TerrainNav::motionUpdate() - currEstimate = (4060724, 593601.44, 14.71) 2021-02-13T01:03:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:03:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:03:27Z- TNavBF::Projecting Measurements 2021-02-13T01:03:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:03:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:03:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:03:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:03:27Z- TNavFilter::4 good beams. 2021-02-13T01:03:27Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:03:27Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:03:27Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:03:27Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:03:27Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:03:27Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:03:27Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:03:27Z- TNavBF::mmse selected from filter -1 2021-02-13T01:03:27Z- TerrainMapOctree: (vehN, vehE ) = (4060724.09, 593601.44). 2021-02-13T01:03:27Z- TerrainMapOctree: Min Distance = 834.99. 2021-02-13T01:03:27Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:03:27Z- There is no specific good beam that all particles have in common. 2021-02-13T01:03:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:03:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.44 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 10.93 2021-02-13T01:03:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:03:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 15.22 2021-02-13T01:03:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:03:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 18.78 2021-02-13T01:03:27Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:03:27Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 8.66 2021-02-13T01:03:27Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:03:27Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:03:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:03:27Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:03:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:03:27Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:03:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:03:27Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:03:27Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:03:27Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:03:27Z- Filter 0 No beams used in subcloud update 2021-02-13T01:03:27Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:03:27Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:03:27Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 677.5 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T01:03:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178207.31 sec. 2021-02-13T01:03:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:03:27Z- TNavBF::mmse selected from filter -1 2021-02-13T01:03:27Z- variances([0], [2], and psi_berg) = 677.526977 832.987442 0.000000 2021-02-13T01:03:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:03:27Z- LcmTrn::publishEstimates() - MLE : 4060723.59 593617.94 14.71 2021-02-13T01:03:27Z- LcmTrn::publishEstimates() - MMSE : 4060724.09 593601.44 14.71 2021-02-13T01:03:27Z- LcmTrn::publishEstimates() - COVAR: 677.53 832.99 1.02 0.00 2021-02-13T01:03:27Z- AHRS_M2 msg: 1613178207.68 epoch sec; seqNo:18002 2021-02-13T01:03:27Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T01:03:27Z- handleDepth()-Depth_Keller msg: 1613178207689 epoch sec; seqNo:18145; depth 14.60 2021-02-13T01:03:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:27Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:27Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:27Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:27Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:27Z- handleDvl() - RDI_Pathfinder msg: 1613178207.69 epoch sec; seqNo:8799 2021-02-13T01:03:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , 0.12 2021-02-13T01:03:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178207.70 epoch sec; seqNo:18145; 4060724.48 north; 593602.66 east 2021-02-13T01:03:28Z- AHRS_M2 msg: 1613178208.09 epoch sec; seqNo:18003 2021-02-13T01:03:28Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T01:03:28Z- handleDepth()-Depth_Keller msg: 1613178208092 epoch sec; seqNo:18146; depth 14.49 2021-02-13T01:03:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178208.11 epoch sec; seqNo:18146; 4060724.86 north; 593602.66 east 2021-02-13T01:03:28Z- AHRS_M2 msg: 1613178208.49 epoch sec; seqNo:18004 2021-02-13T01:03:28Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.14 psi 2021-02-13T01:03:28Z- handleDepth()-Depth_Keller msg: 1613178208497 epoch sec; seqNo:18147; depth 14.38 2021-02-13T01:03:28Z- handleDvl() - RDI_Pathfinder msg: 1613178208.51 epoch sec; seqNo:8800 2021-02-13T01:03:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.11 , 97.72 , 109.89 , 78.24 2021-02-13T01:03:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.10 , 0.08 2021-02-13T01:03:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178208.55 epoch sec; seqNo:18147; 4060725.24 north; 593602.65 east 2021-02-13T01:03:28Z- AHRS_M2 msg: 1613178208.90 epoch sec; seqNo:18005 2021-02-13T01:03:28Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.14 psi 2021-02-13T01:03:28Z- handleDepth()-Depth_Keller msg: 1613178208900 epoch sec; seqNo:18148; depth 14.27 2021-02-13T01:03:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:28Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:28Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:28Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:28Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:28Z- handleDvl() - RDI_Pathfinder msg: 1613178208.90 epoch sec; seqNo:8801 2021-02-13T01:03:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.10 , 0.08 2021-02-13T01:03:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178208.91 epoch sec; seqNo:18148; 4060725.24 north; 593602.65 east 2021-02-13T01:03:29Z- AHRS_M2 msg: 1613178209.30 epoch sec; seqNo:18006 2021-02-13T01:03:29Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-13T01:03:29Z- handleDepth()-Depth_Keller msg: 1613178209313 epoch sec; seqNo:18149; depth 14.17 2021-02-13T01:03:29Z- handleDvl() - RDI_Pathfinder msg: 1613178209.33 epoch sec; seqNo:8802 2021-02-13T01:03:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.72 , 109.89 , 78.79 2021-02-13T01:03:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.10 , 0.01 2021-02-13T01:03:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178209.34 epoch sec; seqNo:18149; 4060725.61 north; 593602.65 east 2021-02-13T01:03:29Z- AHRS_M2 msg: 1613178209.70 epoch sec; seqNo:18007 2021-02-13T01:03:29Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.15 psi 2021-02-13T01:03:29Z- handleDepth()-Depth_Keller msg: 1613178209708 epoch sec; seqNo:18150; depth 14.04 2021-02-13T01:03:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178209.72 epoch sec; seqNo:18150; 4060726.01 north; 593603.86 east 2021-02-13T01:03:30Z- AHRS_M2 msg: 1613178210.11 epoch sec; seqNo:18008 2021-02-13T01:03:30Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-13T01:03:30Z- handleDepth()-Depth_Keller msg: 1613178210112 epoch sec; seqNo:18151; depth 13.92 2021-02-13T01:03:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:30Z- handleDvl() - RDI_Pathfinder msg: 1613178210.11 epoch sec; seqNo:8803 2021-02-13T01:03:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.10 , 0.01 2021-02-13T01:03:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178210.12 epoch sec; seqNo:18151; 4060726.01 north; 593603.86 east 2021-02-13T01:03:30Z- AHRS_M2 msg: 1613178210.52 epoch sec; seqNo:18009 2021-02-13T01:03:30Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-13T01:03:30Z- handleDepth()-Depth_Keller msg: 1613178210520 epoch sec; seqNo:18152; depth 13.81 2021-02-13T01:03:30Z- handleDvl() - RDI_Pathfinder msg: 1613178210.53 epoch sec; seqNo:8804 2021-02-13T01:03:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.57 , 98.80 , 112.33 , 78.51 2021-02-13T01:03:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.06 , -0.02 2021-02-13T01:03:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178210.54 epoch sec; seqNo:18152; 4060726.39 north; 593603.86 east 2021-02-13T01:03:30Z- AHRS_M2 msg: 1613178210.91 epoch sec; seqNo:18010 2021-02-13T01:03:30Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.17 psi 2021-02-13T01:03:30Z- handleDepth()-Depth_Keller msg: 1613178210919 epoch sec; seqNo:18153; depth 13.69 2021-02-13T01:03:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178210.94 epoch sec; seqNo:18153; 4060726.76 north; 593603.86 east 2021-02-13T01:03:31Z- AHRS_M2 msg: 1613178211.34 epoch sec; seqNo:18011 2021-02-13T01:03:31Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-13T01:03:31Z- handleDepth()-Depth_Keller msg: 1613178211353 epoch sec; seqNo:18154; depth 13.58 2021-02-13T01:03:31Z- handleDvl() - RDI_Pathfinder msg: 1613178211.37 epoch sec; seqNo:8805 2021-02-13T01:03:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:31Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:03:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178211.38 epoch sec; seqNo:18154; 4060727.14 north; 593603.85 east 2021-02-13T01:03:31Z- AHRS_M2 msg: 1613178211.72 epoch sec; seqNo:18012 2021-02-13T01:03:31Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.18 psi 2021-02-13T01:03:31Z- handleDepth()-Depth_Keller msg: 1613178211728 epoch sec; seqNo:18155; depth 13.47 2021-02-13T01:03:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178211.74 epoch sec; seqNo:18155; 4060727.14 north; 593603.85 east 2021-02-13T01:03:32Z- AHRS_M2 msg: 1613178212.14 epoch sec; seqNo:18013 2021-02-13T01:03:32Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-13T01:03:32Z- handleDepth()-Depth_Keller msg: 1613178212143 epoch sec; seqNo:18156; depth 13.36 2021-02-13T01:03:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178212.15 epoch sec; seqNo:18156; 4060727.54 north; 593605.07 east 2021-02-13T01:03:32Z- AHRS_M2 msg: 1613178212.54 epoch sec; seqNo:18014 2021-02-13T01:03:32Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.17 psi 2021-02-13T01:03:32Z- handleDepth()-Depth_Keller msg: 1613178212540 epoch sec; seqNo:18157; depth 13.26 2021-02-13T01:03:32Z- handleDvl() - RDI_Pathfinder msg: 1613178212.56 epoch sec; seqNo:8806 2021-02-13T01:03:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.98 , 100.83 , 112.73 , 78.51 2021-02-13T01:03:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.07 , 0.02 2021-02-13T01:03:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178212.57 epoch sec; seqNo:18157; 4060727.91 north; 593605.06 east 2021-02-13T01:03:32Z- LcmTrn::handleMessages() - handling 47 messages took 0.75 ms and 132 calls... 2021-02-13T01:03:32Z- waiting for 1613178210.31, time is 1613178212.57 2021-02-13T01:03:32Z- LcmTrn::updateTrn() equating pose 1613178212.57 and meas times 1613178212.57 2021-02-13T01:03:32Z- LcmTrn::updateTrn() >>>> pose time 1613178212.57, meas time 1613178212.57 <<<< 2021-02-13T01:03:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178212.57 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:03:32Z- TerrainNav::motionUpdate() - currEstimate = (4060728, 593605.06, 13.26) 2021-02-13T01:03:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:03:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:03:32Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178212.57 sec.; No relevant navigation data available 2021-02-13T01:03:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:03:32Z- TNavBF::mmse selected from filter -1 2021-02-13T01:03:32Z- variances([0], [2], and psi_berg) = 677.477870 832.987422 0.000000 2021-02-13T01:03:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:03:32Z- LcmTrn::publishEstimates() - MLE : 4060727.41 593620.56 13.26 2021-02-13T01:03:32Z- LcmTrn::publishEstimates() - MMSE : 4060727.91 593605.06 13.26 2021-02-13T01:03:32Z- LcmTrn::publishEstimates() - COVAR: 677.48 832.99 1.29 0.00 2021-02-13T01:03:32Z- AHRS_M2 msg: 1613178212.94 epoch sec; seqNo:18015 2021-02-13T01:03:32Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.16 psi 2021-02-13T01:03:32Z- handleDepth()-Depth_Keller msg: 1613178212945 epoch sec; seqNo:18158; depth 13.14 2021-02-13T01:03:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178212.96 epoch sec; seqNo:18158; 4060728.29 north; 593605.06 east 2021-02-13T01:03:33Z- AHRS_M2 msg: 1613178213.34 epoch sec; seqNo:18016 2021-02-13T01:03:33Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-13T01:03:33Z- handleDepth()-Depth_Keller msg: 1613178213349 epoch sec; seqNo:18159; depth 13.04 2021-02-13T01:03:33Z- handleDvl() - RDI_Pathfinder msg: 1613178213.36 epoch sec; seqNo:8807 2021-02-13T01:03:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 100.29 , 115.17 , 75.62 2021-02-13T01:03:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.16 , -0.01 2021-02-13T01:03:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178213.37 epoch sec; seqNo:18159; 4060728.29 north; 593605.06 east 2021-02-13T01:03:33Z- AHRS_M2 msg: 1613178213.74 epoch sec; seqNo:18017 2021-02-13T01:03:33Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-13T01:03:33Z- handleDepth()-Depth_Keller msg: 1613178213749 epoch sec; seqNo:18160; depth 12.92 2021-02-13T01:03:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:33Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:33Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:33Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:33Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:33Z- handleDvl() - RDI_Pathfinder msg: 1613178213.75 epoch sec; seqNo:8808 2021-02-13T01:03:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.16 , -0.01 2021-02-13T01:03:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178213.76 epoch sec; seqNo:18160; 4060728.67 north; 593605.06 east 2021-02-13T01:03:34Z- AHRS_M2 msg: 1613178214.15 epoch sec; seqNo:18018 2021-02-13T01:03:34Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-13T01:03:34Z- handleDepth()-Depth_Keller msg: 1613178214155 epoch sec; seqNo:18161; depth 12.82 2021-02-13T01:03:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178214.16 epoch sec; seqNo:18161; 4060729.06 north; 593606.27 east 2021-02-13T01:03:34Z- AHRS_M2 msg: 1613178214.55 epoch sec; seqNo:18019 2021-02-13T01:03:34Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-13T01:03:34Z- handleDepth()-Depth_Keller msg: 1613178214556 epoch sec; seqNo:18162; depth 12.71 2021-02-13T01:03:34Z- handleDvl() - RDI_Pathfinder msg: 1613178214.57 epoch sec; seqNo:8809 2021-02-13T01:03:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.41 , 98.94 , 113.82 , 75.62 2021-02-13T01:03:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.17 , -0.11 2021-02-13T01:03:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178214.59 epoch sec; seqNo:18162; 4060729.06 north; 593606.27 east 2021-02-13T01:03:34Z- AHRS_M2 msg: 1613178214.96 epoch sec; seqNo:18020 2021-02-13T01:03:34Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-13T01:03:34Z- handleDepth()-Depth_Keller msg: 1613178214973 epoch sec; seqNo:18163; depth 12.59 2021-02-13T01:03:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:34Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:34Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:34Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:34Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:34Z- handleDvl() - RDI_Pathfinder msg: 1613178214.98 epoch sec; seqNo:8810 2021-02-13T01:03:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.17 , -0.11 2021-02-13T01:03:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178214.99 epoch sec; seqNo:18163; 4060729.44 north; 593606.27 east 2021-02-13T01:03:35Z- AHRS_M2 msg: 1613178215.36 epoch sec; seqNo:18021 2021-02-13T01:03:35Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-13T01:03:35Z- handleDepth()-Depth_Keller msg: 1613178215369 epoch sec; seqNo:18164; depth 12.46 2021-02-13T01:03:35Z- handleDvl() - RDI_Pathfinder msg: 1613178215.38 epoch sec; seqNo:8811 2021-02-13T01:03:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.29 , 115.17 , 73.99 2021-02-13T01:03:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.05 , -0.19 2021-02-13T01:03:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178215.39 epoch sec; seqNo:18164; 4060729.44 north; 593606.27 east 2021-02-13T01:03:35Z- AHRS_M2 msg: 1613178215.77 epoch sec; seqNo:18022 2021-02-13T01:03:35Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.18 psi 2021-02-13T01:03:35Z- handleDepth()-Depth_Keller msg: 1613178215770 epoch sec; seqNo:18165; depth 12.36 2021-02-13T01:03:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178215.78 epoch sec; seqNo:18165; 4060729.82 north; 593606.26 east 2021-02-13T01:03:36Z- AHRS_M2 msg: 1613178216.17 epoch sec; seqNo:18023 2021-02-13T01:03:36Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.18 psi 2021-02-13T01:03:36Z- handleDepth()-Depth_Keller msg: 1613178216172 epoch sec; seqNo:18166; depth 12.24 2021-02-13T01:03:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:36Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:36Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:36Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:36Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:36Z- handleDvl() - RDI_Pathfinder msg: 1613178216.17 epoch sec; seqNo:8812 2021-02-13T01:03:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.05 , -0.19 2021-02-13T01:03:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178216.19 epoch sec; seqNo:18166; 4060730.20 north; 593606.26 east 2021-02-13T01:03:36Z- AHRS_M2 msg: 1613178216.57 epoch sec; seqNo:18024 2021-02-13T01:03:36Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.18 psi 2021-02-13T01:03:36Z- handleDepth()-Depth_Keller msg: 1613178216576 epoch sec; seqNo:18167; depth 12.12 2021-02-13T01:03:36Z- handleDvl() - RDI_Pathfinder msg: 1613178216.59 epoch sec; seqNo:8813 2021-02-13T01:03:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 101.37 , 114.90 , 78.38 2021-02-13T01:03:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.06 , -0.17 2021-02-13T01:03:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178216.61 epoch sec; seqNo:18167; 4060730.21 north; 593607.48 east 2021-02-13T01:03:36Z- AHRS_M2 msg: 1613178216.98 epoch sec; seqNo:18025 2021-02-13T01:03:36Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.18 psi 2021-02-13T01:03:36Z- handleDepth()-Depth_Keller msg: 1613178216982 epoch sec; seqNo:18168; depth 12.00 2021-02-13T01:03:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178216.99 epoch sec; seqNo:18168; 4060730.59 north; 593607.48 east 2021-02-13T01:03:37Z- AHRS_M2 msg: 1613178217.38 epoch sec; seqNo:18026 2021-02-13T01:03:37Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.19 psi 2021-02-13T01:03:37Z- handleDepth()-Depth_Keller msg: 1613178217389 epoch sec; seqNo:18169; depth 11.89 2021-02-13T01:03:37Z- handleDvl() - RDI_Pathfinder msg: 1613178217.41 epoch sec; seqNo:8814 2021-02-13T01:03:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.86 , 110.97 , 76.85 2021-02-13T01:03:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.16 , -0.09 2021-02-13T01:03:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178217.42 epoch sec; seqNo:18169; 4060730.97 north; 593607.47 east 2021-02-13T01:03:37Z- LcmTrn::handleMessages() - handling 44 messages took 0.63 ms and 132 calls... 2021-02-13T01:03:37Z- waiting for 1613178215.57, time is 1613178217.42 2021-02-13T01:03:37Z- LcmTrn::updateTrn() equating pose 1613178217.42 and meas times 1613178217.42 2021-02-13T01:03:37Z- LcmTrn::updateTrn() >>>> pose time 1613178217.42, meas time 1613178217.42 <<<< 2021-02-13T01:03:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178217.42 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:03:37Z- TerrainNav::motionUpdate() - currEstimate = (4060731, 593607.47, 11.89) 2021-02-13T01:03:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:03:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:03:37Z- TNavBF::Projecting Measurements 2021-02-13T01:03:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:03:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:03:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:03:37Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:03:37Z- TNavFilter::3 good beams. 2021-02-13T01:03:37Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:03:37Z- TNavBF:: beamIndex[0] = 1 2021-02-13T01:03:37Z- TNavBF:: beamIndex[1] = 2 2021-02-13T01:03:37Z- TNavBF:: beamIndex[2] = 3 2021-02-13T01:03:37Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:03:37Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:03:37Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:03:37Z- TNavBF::mmse selected from filter -1 2021-02-13T01:03:37Z- TerrainMapOctree: (vehN, vehE ) = (4060730.97, 593607.47). 2021-02-13T01:03:37Z- TerrainMapOctree: Min Distance = 825.97. 2021-02-13T01:03:37Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:03:37Z- There is no specific good beam that all particles have in common. 2021-02-13T01:03:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:03:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.49 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 16.93 2021-02-13T01:03:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:03:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 19.70 2021-02-13T01:03:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:03:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 9.45 2021-02-13T01:03:37Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:03:37Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:03:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:03:37Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:03:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:03:37Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:03:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:03:37Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:03:37Z- Filter 0 No beams used in subcloud update 2021-02-13T01:03:37Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:03:37Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:03:37Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 677.4 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T01:03:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178217.42 sec. 2021-02-13T01:03:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:03:37Z- TNavBF::mmse selected from filter -1 2021-02-13T01:03:37Z- variances([0], [2], and psi_berg) = 677.435688 832.987404 0.000000 2021-02-13T01:03:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:03:37Z- LcmTrn::publishEstimates() - MLE : 4060730.47 593623.97 11.89 2021-02-13T01:03:37Z- LcmTrn::publishEstimates() - MMSE : 4060730.97 593607.47 11.89 2021-02-13T01:03:37Z- LcmTrn::publishEstimates() - COVAR: 677.44 832.99 1.16 0.00 2021-02-13T01:03:37Z- AHRS_M2 msg: 1613178217.78 epoch sec; seqNo:18027 2021-02-13T01:03:37Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.19 psi 2021-02-13T01:03:37Z- handleDepth()-Depth_Keller msg: 1613178217788 epoch sec; seqNo:18170; depth 11.76 2021-02-13T01:03:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178217.80 epoch sec; seqNo:18170; 4060731.35 north; 593607.47 east 2021-02-13T01:03:38Z- AHRS_M2 msg: 1613178218.19 epoch sec; seqNo:18028 2021-02-13T01:03:38Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.18 psi 2021-02-13T01:03:38Z- handleDepth()-Depth_Keller msg: 1613178218194 epoch sec; seqNo:18171; depth 11.65 2021-02-13T01:03:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178218.20 epoch sec; seqNo:18171; 4060731.35 north; 593607.47 east 2021-02-13T01:03:38Z- AHRS_M2 msg: 1613178218.59 epoch sec; seqNo:18029 2021-02-13T01:03:38Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.19 psi 2021-02-13T01:03:38Z- handleDepth()-Depth_Keller msg: 1613178218601 epoch sec; seqNo:18172; depth 11.55 2021-02-13T01:03:38Z- handleDvl() - RDI_Pathfinder msg: 1613178218.61 epoch sec; seqNo:8815 2021-02-13T01:03:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:38Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:03:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178218.64 epoch sec; seqNo:18172; 4060731.73 north; 593607.46 east 2021-02-13T01:03:38Z- AHRS_M2 msg: 1613178219.00 epoch sec; seqNo:18030 2021-02-13T01:03:38Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.19 psi 2021-02-13T01:03:39Z- handleDepth()-Depth_Keller msg: 1613178219000 epoch sec; seqNo:18173; depth 11.42 2021-02-13T01:03:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178219.01 epoch sec; seqNo:18173; 4060732.11 north; 593607.46 east 2021-02-13T01:03:39Z- AHRS_M2 msg: 1613178219.40 epoch sec; seqNo:18031 2021-02-13T01:03:39Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.19 psi 2021-02-13T01:03:39Z- handleDepth()-Depth_Keller msg: 1613178219410 epoch sec; seqNo:18174; depth 11.32 2021-02-13T01:03:39Z- handleDvl() - RDI_Pathfinder msg: 1613178219.42 epoch sec; seqNo:8816 2021-02-13T01:03:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.85 , 103.19 , 118.07 , 75.72 2021-02-13T01:03:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , 0.11 2021-02-13T01:03:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178219.43 epoch sec; seqNo:18174; 4060732.49 north; 593607.45 east 2021-02-13T01:03:39Z- AHRS_M2 msg: 1613178219.80 epoch sec; seqNo:18032 2021-02-13T01:03:39Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.19 psi 2021-02-13T01:03:39Z- handleDepth()-Depth_Keller msg: 1613178219813 epoch sec; seqNo:18175; depth 11.19 2021-02-13T01:03:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:39Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:39Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:39Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:39Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:39Z- handleDvl() - RDI_Pathfinder msg: 1613178219.82 epoch sec; seqNo:8817 2021-02-13T01:03:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , 0.11 2021-02-13T01:03:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178219.84 epoch sec; seqNo:18175; 4060732.87 north; 593607.45 east 2021-02-13T01:03:40Z- AHRS_M2 msg: 1613178220.21 epoch sec; seqNo:18033 2021-02-13T01:03:40Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.18 psi 2021-02-13T01:03:40Z- handleDepth()-Depth_Keller msg: 1613178220212 epoch sec; seqNo:18176; depth 11.08 2021-02-13T01:03:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178220.22 epoch sec; seqNo:18176; 4060732.88 north; 593608.67 east 2021-02-13T01:03:40Z- AHRS_M2 msg: 1613178220.62 epoch sec; seqNo:18034 2021-02-13T01:03:40Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.18 psi 2021-02-13T01:03:40Z- handleDepth()-Depth_Keller msg: 1613178220622 epoch sec; seqNo:18177; depth 10.97 2021-02-13T01:03:40Z- handleDvl() - RDI_Pathfinder msg: 1613178220.63 epoch sec; seqNo:8818 2021-02-13T01:03:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.27 , 102.85 , 119.08 , 78.51 2021-02-13T01:03:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , 0.12 2021-02-13T01:03:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178220.64 epoch sec; seqNo:18177; 4060733.26 north; 593608.67 east 2021-02-13T01:03:41Z- AHRS_M2 msg: 1613178221.02 epoch sec; seqNo:18035 2021-02-13T01:03:41Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.18 psi 2021-02-13T01:03:41Z- handleDepth()-Depth_Keller msg: 1613178221020 epoch sec; seqNo:18178; depth 10.85 2021-02-13T01:03:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:41Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:41Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:41Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:41Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:41Z- handleDvl() - RDI_Pathfinder msg: 1613178221.03 epoch sec; seqNo:8819 2021-02-13T01:03:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , 0.12 2021-02-13T01:03:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178221.04 epoch sec; seqNo:18178; 4060733.64 north; 593608.66 east 2021-02-13T01:03:41Z- AHRS_M2 msg: 1613178221.42 epoch sec; seqNo:18036 2021-02-13T01:03:41Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.18 psi 2021-02-13T01:03:41Z- handleDepth()-Depth_Keller msg: 1613178221429 epoch sec; seqNo:18179; depth 10.73 2021-02-13T01:03:41Z- handleDvl() - RDI_Pathfinder msg: 1613178221.45 epoch sec; seqNo:8820 2021-02-13T01:03:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 102.79 , 114.96 , 78.45 2021-02-13T01:03:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.05 , 0.11 2021-02-13T01:03:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178221.46 epoch sec; seqNo:18179; 4060734.02 north; 593608.66 east 2021-02-13T01:03:41Z- AHRS_M2 msg: 1613178221.82 epoch sec; seqNo:18037 2021-02-13T01:03:41Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.18 psi 2021-02-13T01:03:41Z- handleDepth()-Depth_Keller msg: 1613178221831 epoch sec; seqNo:18180; depth 10.61 2021-02-13T01:03:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178221.84 epoch sec; seqNo:18180; 4060734.02 north; 593608.66 east 2021-02-13T01:03:42Z- AHRS_M2 msg: 1613178222.23 epoch sec; seqNo:18038 2021-02-13T01:03:42Z- AHRS_M2 msg: 0.55 phi; 0.35 theta; -0.18 psi 2021-02-13T01:03:42Z- handleDepth()-Depth_Keller msg: 1613178222233 epoch sec; seqNo:18181; depth 10.50 2021-02-13T01:03:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:42Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:42Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:42Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:42Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:42Z- handleDvl() - RDI_Pathfinder msg: 1613178222.23 epoch sec; seqNo:8821 2021-02-13T01:03:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.05 , 0.11 2021-02-13T01:03:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178222.24 epoch sec; seqNo:18181; 4060734.39 north; 593608.65 east 2021-02-13T01:03:42Z- AHRS_M2 msg: 1613178222.63 epoch sec; seqNo:18039 2021-02-13T01:03:42Z- AHRS_M2 msg: 0.55 phi; 0.35 theta; -0.17 psi 2021-02-13T01:03:42Z- handleDepth()-Depth_Keller msg: 1613178222638 epoch sec; seqNo:18182; depth 10.36 2021-02-13T01:03:42Z- handleDvl() - RDI_Pathfinder msg: 1613178222.65 epoch sec; seqNo:8822 2021-02-13T01:03:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 102.79 , 117.66 , 0.00 2021-02-13T01:03:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.08 , 0.09 2021-02-13T01:03:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178222.66 epoch sec; seqNo:18182; 4060734.79 north; 593609.87 east 2021-02-13T01:03:42Z- LcmTrn::handleMessages() - handling 47 messages took 0.67 ms and 134 calls... 2021-02-13T01:03:42Z- waiting for 1613178220.42, time is 1613178222.66 2021-02-13T01:03:42Z- LcmTrn::updateTrn() equating pose 1613178222.66 and meas times 1613178222.66 2021-02-13T01:03:42Z- LcmTrn::updateTrn() >>>> pose time 1613178222.66, meas time 1613178222.66 <<<< 2021-02-13T01:03:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178222.66 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:03:42Z- TerrainNav::motionUpdate() - currEstimate = (4060735, 593609.87, 10.36) 2021-02-13T01:03:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:03:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:03:42Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178222.66 sec.; No relevant navigation data available 2021-02-13T01:03:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:03:42Z- TNavBF::mmse selected from filter -1 2021-02-13T01:03:42Z- variances([0], [2], and psi_berg) = 677.390312 832.987386 0.000000 2021-02-13T01:03:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:03:42Z- LcmTrn::publishEstimates() - MLE : 4060734.29 593626.37 10.36 2021-02-13T01:03:42Z- LcmTrn::publishEstimates() - MMSE : 4060734.79 593609.87 10.36 2021-02-13T01:03:42Z- LcmTrn::publishEstimates() - COVAR: 677.39 832.99 1.39 0.00 2021-02-13T01:03:43Z- AHRS_M2 msg: 1613178223.04 epoch sec; seqNo:18040 2021-02-13T01:03:43Z- AHRS_M2 msg: 0.55 phi; 0.36 theta; -0.17 psi 2021-02-13T01:03:43Z- handleDepth()-Depth_Keller msg: 1613178223041 epoch sec; seqNo:18183; depth 10.25 2021-02-13T01:03:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178223.05 epoch sec; seqNo:18183; 4060735.17 north; 593609.87 east 2021-02-13T01:03:43Z- AHRS_M2 msg: 1613178223.44 epoch sec; seqNo:18041 2021-02-13T01:03:43Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.16 psi 2021-02-13T01:03:43Z- handleDepth()-Depth_Keller msg: 1613178223444 epoch sec; seqNo:18184; depth 10.13 2021-02-13T01:03:43Z- handleDvl() - RDI_Pathfinder msg: 1613178223.46 epoch sec; seqNo:8823 2021-02-13T01:03:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.36 , 120.23 , 80.07 2021-02-13T01:03:43Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.13 , 0.07 2021-02-13T01:03:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178223.47 epoch sec; seqNo:18184; 4060735.17 north; 593609.87 east 2021-02-13T01:03:43Z- AHRS_M2 msg: 1613178223.84 epoch sec; seqNo:18042 2021-02-13T01:03:43Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.16 psi 2021-02-13T01:03:43Z- handleDepth()-Depth_Keller msg: 1613178223848 epoch sec; seqNo:18185; depth 10.01 2021-02-13T01:03:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178223.86 epoch sec; seqNo:18185; 4060735.54 north; 593609.86 east 2021-02-13T01:03:44Z- AHRS_M2 msg: 1613178224.25 epoch sec; seqNo:18043 2021-02-13T01:03:44Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.15 psi 2021-02-13T01:03:44Z- handleDepth()-Depth_Keller msg: 1613178224267 epoch sec; seqNo:18186; depth 9.90 2021-02-13T01:03:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178224.29 epoch sec; seqNo:18186; 4060735.92 north; 593609.86 east 2021-02-13T01:03:44Z- AHRS_M2 msg: 1613178224.65 epoch sec; seqNo:18044 2021-02-13T01:03:44Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.14 psi 2021-02-13T01:03:44Z- handleDepth()-Depth_Keller msg: 1613178224656 epoch sec; seqNo:18187; depth 9.77 2021-02-13T01:03:44Z- handleDvl() - RDI_Pathfinder msg: 1613178224.67 epoch sec; seqNo:8824 2021-02-13T01:03:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 103.94 , 120.31 , 76.44 2021-02-13T01:03:44Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.16 , 0.01 2021-02-13T01:03:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178224.69 epoch sec; seqNo:18187; 4060736.32 north; 593611.07 east 2021-02-13T01:03:45Z- AHRS_M2 msg: 1613178225.06 epoch sec; seqNo:18045 2021-02-13T01:03:45Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.13 psi 2021-02-13T01:03:45Z- handleDepth()-Depth_Keller msg: 1613178225065 epoch sec; seqNo:18188; depth 9.65 2021-02-13T01:03:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178225.08 epoch sec; seqNo:18188; 4060736.32 north; 593611.07 east 2021-02-13T01:03:45Z- AHRS_M2 msg: 1613178225.46 epoch sec; seqNo:18046 2021-02-13T01:03:45Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.12 psi 2021-02-13T01:03:45Z- handleDepth()-Depth_Keller msg: 1613178225467 epoch sec; seqNo:18189; depth 9.54 2021-02-13T01:03:45Z- handleDvl() - RDI_Pathfinder msg: 1613178225.48 epoch sec; seqNo:8825 2021-02-13T01:03:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.25 , 102.66 , 117.54 , 78.31 2021-02-13T01:03:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.16 , -0.09 2021-02-13T01:03:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178225.49 epoch sec; seqNo:18189; 4060736.69 north; 593611.07 east 2021-02-13T01:03:45Z- AHRS_M2 msg: 1613178225.86 epoch sec; seqNo:18047 2021-02-13T01:03:45Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.11 psi 2021-02-13T01:03:45Z- handleDepth()-Depth_Keller msg: 1613178225868 epoch sec; seqNo:18190; depth 9.43 2021-02-13T01:03:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:45Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:45Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:45Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:45Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:45Z- handleDvl() - RDI_Pathfinder msg: 1613178225.87 epoch sec; seqNo:8826 2021-02-13T01:03:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.16 , -0.09 2021-02-13T01:03:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178225.89 epoch sec; seqNo:18190; 4060737.07 north; 593611.07 east 2021-02-13T01:03:46Z- AHRS_M2 msg: 1613178226.27 epoch sec; seqNo:18048 2021-02-13T01:03:46Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.11 psi 2021-02-13T01:03:46Z- handleDepth()-Depth_Keller msg: 1613178226273 epoch sec; seqNo:18191; depth 9.31 2021-02-13T01:03:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178226.28 epoch sec; seqNo:18191; 4060737.45 north; 593611.06 east 2021-02-13T01:03:46Z- AHRS_M2 msg: 1613178226.67 epoch sec; seqNo:18049 2021-02-13T01:03:46Z- AHRS_M2 msg: 0.62 phi; 0.35 theta; -0.11 psi 2021-02-13T01:03:46Z- handleDepth()-Depth_Keller msg: 1613178226677 epoch sec; seqNo:18192; depth 9.20 2021-02-13T01:03:46Z- handleDvl() - RDI_Pathfinder msg: 1613178226.69 epoch sec; seqNo:8827 2021-02-13T01:03:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 101.16 , 121.45 , 78.17 2021-02-13T01:03:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.11 , -0.21 2021-02-13T01:03:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178226.70 epoch sec; seqNo:18192; 4060737.47 north; 593612.28 east 2021-02-13T01:03:47Z- AHRS_M2 msg: 1613178227.08 epoch sec; seqNo:18050 2021-02-13T01:03:47Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.11 psi 2021-02-13T01:03:47Z- handleDepth()-Depth_Keller msg: 1613178227081 epoch sec; seqNo:18193; depth 9.07 2021-02-13T01:03:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:47Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:47Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:47Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:47Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:47Z- handleDvl() - RDI_Pathfinder msg: 1613178227.08 epoch sec; seqNo:8828 2021-02-13T01:03:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.11 , -0.21 2021-02-13T01:03:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178227.09 epoch sec; seqNo:18193; 4060737.84 north; 593612.28 east 2021-02-13T01:03:47Z- AHRS_M2 msg: 1613178227.48 epoch sec; seqNo:18051 2021-02-13T01:03:47Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.11 psi 2021-02-13T01:03:47Z- handleDepth()-Depth_Keller msg: 1613178227484 epoch sec; seqNo:18194; depth 8.95 2021-02-13T01:03:47Z- handleDvl() - RDI_Pathfinder msg: 1613178227.50 epoch sec; seqNo:8829 2021-02-13T01:03:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 101.16 , 121.45 , 79.53 2021-02-13T01:03:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.08 , -0.26 2021-02-13T01:03:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178227.51 epoch sec; seqNo:18194; 4060737.84 north; 593612.28 east 2021-02-13T01:03:47Z- LcmTrn::handleMessages() - handling 43 messages took 0.68 ms and 130 calls... 2021-02-13T01:03:47Z- waiting for 1613178225.66, time is 1613178227.51 2021-02-13T01:03:47Z- LcmTrn::updateTrn() equating pose 1613178227.51 and meas times 1613178227.51 2021-02-13T01:03:47Z- LcmTrn::updateTrn() >>>> pose time 1613178227.51, meas time 1613178227.51 <<<< 2021-02-13T01:03:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178227.51 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:03:47Z- TerrainNav::motionUpdate() - currEstimate = (4060738, 593612.28, 8.95) 2021-02-13T01:03:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:03:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:03:47Z- TNavBF::Projecting Measurements 2021-02-13T01:03:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:03:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:03:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:03:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:03:47Z- TNavFilter::4 good beams. 2021-02-13T01:03:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:03:47Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:03:47Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:03:47Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:03:47Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:03:47Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:03:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:03:47Z- TNavBF::mmse selected from filter -1 2021-02-13T01:03:47Z- TerrainMapOctree: (vehN, vehE ) = (4060737.84, 593612.28). 2021-02-13T01:03:47Z- TerrainMapOctree: Min Distance = 817.92. 2021-02-13T01:03:47Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:03:47Z- There is no specific good beam that all particles have in common. 2021-02-13T01:03:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:03:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.59 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-13T01:03:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:03:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.45 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 16.37 2021-02-13T01:03:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:03:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.35 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 23.60 2021-02-13T01:03:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:03:47Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 10.12 2021-02-13T01:03:47Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:03:47Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:03:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:03:47Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:03:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:03:47Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:03:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:03:47Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:03:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:03:47Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:03:47Z- Filter 0 No beams used in subcloud update 2021-02-13T01:03:47Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:03:47Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:03:47Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 677.3 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T01:03:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178227.51 sec. 2021-02-13T01:03:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:03:47Z- TNavBF::mmse selected from filter -1 2021-02-13T01:03:47Z- variances([0], [2], and psi_berg) = 677.348168 832.987368 0.000000 2021-02-13T01:03:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:03:47Z- LcmTrn::publishEstimates() - MLE : 4060737.34 593628.78 8.95 2021-02-13T01:03:47Z- LcmTrn::publishEstimates() - MMSE : 4060737.84 593612.28 8.95 2021-02-13T01:03:47Z- LcmTrn::publishEstimates() - COVAR: 677.35 832.99 1.15 0.00 2021-02-13T01:03:47Z- AHRS_M2 msg: 1613178227.88 epoch sec; seqNo:18052 2021-02-13T01:03:47Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.12 psi 2021-02-13T01:03:47Z- handleDepth()-Depth_Keller msg: 1613178227888 epoch sec; seqNo:18195; depth 8.82 2021-02-13T01:03:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178227.90 epoch sec; seqNo:18195; 4060738.22 north; 593612.27 east 2021-02-13T01:03:48Z- AHRS_M2 msg: 1613178228.29 epoch sec; seqNo:18053 2021-02-13T01:03:48Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.13 psi 2021-02-13T01:03:48Z- handleDepth()-Depth_Keller msg: 1613178228293 epoch sec; seqNo:18196; depth 8.69 2021-02-13T01:03:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:48Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:48Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:48Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:48Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:48Z- handleDvl() - RDI_Pathfinder msg: 1613178228.30 epoch sec; seqNo:8830 2021-02-13T01:03:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.08 , -0.26 2021-02-13T01:03:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178228.30 epoch sec; seqNo:18196; 4060738.22 north; 593612.27 east 2021-02-13T01:03:48Z- AHRS_M2 msg: 1613178228.69 epoch sec; seqNo:18054 2021-02-13T01:03:48Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.15 psi 2021-02-13T01:03:48Z- handleDepth()-Depth_Keller msg: 1613178228701 epoch sec; seqNo:18197; depth 8.57 2021-02-13T01:03:48Z- handleDvl() - RDI_Pathfinder msg: 1613178228.72 epoch sec; seqNo:8831 2021-02-13T01:03:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 102.45 , 118.68 , 79.46 2021-02-13T01:03:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.07 , -0.22 2021-02-13T01:03:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178228.73 epoch sec; seqNo:18197; 4060738.60 north; 593612.27 east 2021-02-13T01:03:49Z- AHRS_M2 msg: 1613178229.10 epoch sec; seqNo:18055 2021-02-13T01:03:49Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.18 psi 2021-02-13T01:03:49Z- handleDepth()-Depth_Keller msg: 1613178229107 epoch sec; seqNo:18198; depth 8.43 2021-02-13T01:03:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178229.13 epoch sec; seqNo:18198; 4060738.60 north; 593612.27 east 2021-02-13T01:03:49Z- AHRS_M2 msg: 1613178229.51 epoch sec; seqNo:18056 2021-02-13T01:03:49Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-13T01:03:49Z- handleDepth()-Depth_Keller msg: 1613178229510 epoch sec; seqNo:18199; depth 8.31 2021-02-13T01:03:49Z- handleDvl() - RDI_Pathfinder msg: 1613178229.52 epoch sec; seqNo:8832 2021-02-13T01:03:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 106.58 , 121.46 , 80.88 2021-02-13T01:03:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.21 , -0.02 2021-02-13T01:03:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178229.55 epoch sec; seqNo:18199; 4060738.98 north; 593612.27 east 2021-02-13T01:03:49Z- AHRS_M2 msg: 1613178229.91 epoch sec; seqNo:18057 2021-02-13T01:03:49Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.19 psi 2021-02-13T01:03:49Z- handleDepth()-Depth_Keller msg: 1613178229912 epoch sec; seqNo:18200; depth 8.20 2021-02-13T01:03:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178229.92 epoch sec; seqNo:18200; 4060739.37 north; 593613.48 east 2021-02-13T01:03:50Z- AHRS_M2 msg: 1613178230.31 epoch sec; seqNo:18058 2021-02-13T01:03:50Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T01:03:50Z- handleDepth()-Depth_Keller msg: 1613178230314 epoch sec; seqNo:18201; depth 8.07 2021-02-13T01:03:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178230.33 epoch sec; seqNo:18201; 4060739.75 north; 593613.48 east 2021-02-13T01:03:50Z- AHRS_M2 msg: 1613178230.71 epoch sec; seqNo:18059 2021-02-13T01:03:50Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-13T01:03:50Z- handleDepth()-Depth_Keller msg: 1613178230717 epoch sec; seqNo:18202; depth 7.94 2021-02-13T01:03:50Z- handleDvl() - RDI_Pathfinder msg: 1613178230.74 epoch sec; seqNo:8833 2021-02-13T01:03:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.44 , 108.27 , 124.64 , 81.49 2021-02-13T01:03:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.20 , 0.20 2021-02-13T01:03:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178230.76 epoch sec; seqNo:18202; 4060740.13 north; 593613.47 east 2021-02-13T01:03:51Z- AHRS_M2 msg: 1613178231.12 epoch sec; seqNo:18060 2021-02-13T01:03:51Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-13T01:03:51Z- handleDepth()-Depth_Keller msg: 1613178231121 epoch sec; seqNo:18203; depth 7.82 2021-02-13T01:03:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178231.14 epoch sec; seqNo:18203; 4060740.13 north; 593613.47 east 2021-02-13T01:03:51Z- AHRS_M2 msg: 1613178231.52 epoch sec; seqNo:18061 2021-02-13T01:03:51Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T01:03:51Z- handleDepth()-Depth_Keller msg: 1613178231529 epoch sec; seqNo:18204; depth 7.71 2021-02-13T01:03:51Z- handleDvl() - RDI_Pathfinder msg: 1613178231.54 epoch sec; seqNo:8834 2021-02-13T01:03:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.98 , 105.09 , 122.68 , 80.75 2021-02-13T01:03:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , 0.32 2021-02-13T01:03:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178231.55 epoch sec; seqNo:18204; 4060740.51 north; 593613.47 east 2021-02-13T01:03:51Z- AHRS_M2 msg: 1613178231.93 epoch sec; seqNo:18062 2021-02-13T01:03:51Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.14 psi 2021-02-13T01:03:51Z- handleDepth()-Depth_Keller msg: 1613178231930 epoch sec; seqNo:18205; depth 7.58 2021-02-13T01:03:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:51Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:51Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:51Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:51Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:51Z- handleDvl() - RDI_Pathfinder msg: 1613178231.93 epoch sec; seqNo:8835 2021-02-13T01:03:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , 0.32 2021-02-13T01:03:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178231.94 epoch sec; seqNo:18205; 4060740.89 north; 593613.47 east 2021-02-13T01:03:52Z- AHRS_M2 msg: 1613178232.33 epoch sec; seqNo:18063 2021-02-13T01:03:52Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.13 psi 2021-02-13T01:03:52Z- handleDepth()-Depth_Keller msg: 1613178232336 epoch sec; seqNo:18206; depth 7.48 2021-02-13T01:03:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178232.36 epoch sec; seqNo:18206; 4060741.27 north; 593613.46 east 2021-02-13T01:03:52Z- AHRS_M2 msg: 1613178232.73 epoch sec; seqNo:18064 2021-02-13T01:03:52Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.13 psi 2021-02-13T01:03:52Z- handleDepth()-Depth_Keller msg: 1613178232736 epoch sec; seqNo:18207; depth 7.36 2021-02-13T01:03:52Z- handleDvl() - RDI_Pathfinder msg: 1613178232.77 epoch sec; seqNo:8836 2021-02-13T01:03:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.55 , 103.94 , 118.82 , 81.62 2021-02-13T01:03:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.19 , 0.26 2021-02-13T01:03:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178232.78 epoch sec; seqNo:18207; 4060741.66 north; 593614.68 east 2021-02-13T01:03:52Z- LcmTrn::handleMessages() - handling 46 messages took 0.67 ms and 133 calls... 2021-02-13T01:03:52Z- waiting for 1613178230.51, time is 1613178232.78 2021-02-13T01:03:52Z- LcmTrn::updateTrn() equating pose 1613178232.78 and meas times 1613178232.78 2021-02-13T01:03:52Z- LcmTrn::updateTrn() >>>> pose time 1613178232.78, meas time 1613178232.78 <<<< 2021-02-13T01:03:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178232.78 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:03:52Z- TerrainNav::motionUpdate() - currEstimate = (4060742, 593614.68, 7.36) 2021-02-13T01:03:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:03:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:03:52Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178232.78 sec.; No relevant navigation data available 2021-02-13T01:03:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:03:52Z- TNavBF::mmse selected from filter -1 2021-02-13T01:03:52Z- variances([0], [2], and psi_berg) = 677.302834 832.987350 0.000000 2021-02-13T01:03:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:03:52Z- LcmTrn::publishEstimates() - MLE : 4060741.16 593631.18 7.36 2021-02-13T01:03:52Z- LcmTrn::publishEstimates() - MMSE : 4060741.66 593614.68 7.36 2021-02-13T01:03:52Z- LcmTrn::publishEstimates() - COVAR: 677.30 832.99 1.38 0.00 2021-02-13T01:03:53Z- AHRS_M2 msg: 1613178233.14 epoch sec; seqNo:18065 2021-02-13T01:03:53Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.14 psi 2021-02-13T01:03:53Z- handleDepth()-Depth_Keller msg: 1613178233146 epoch sec; seqNo:18208; depth 7.25 2021-02-13T01:03:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:53Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:53Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:53Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:53Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:53Z- handleDvl() - RDI_Pathfinder msg: 1613178233.15 epoch sec; seqNo:8837 2021-02-13T01:03:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.19 , 0.26 2021-02-13T01:03:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178233.16 epoch sec; seqNo:18208; 4060741.66 north; 593614.68 east 2021-02-13T01:03:53Z- AHRS_M2 msg: 1613178233.54 epoch sec; seqNo:18066 2021-02-13T01:03:53Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.14 psi 2021-02-13T01:03:53Z- handleDepth()-Depth_Keller msg: 1613178233544 epoch sec; seqNo:18209; depth 7.14 2021-02-13T01:03:53Z- handleDvl() - RDI_Pathfinder msg: 1613178233.59 epoch sec; seqNo:8838 2021-02-13T01:03:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.42 , 102.59 , 121.53 , 80.95 2021-02-13T01:03:53Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.30 , 0.07 2021-02-13T01:03:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178233.61 epoch sec; seqNo:18209; 4060742.04 north; 593614.67 east 2021-02-13T01:03:53Z- AHRS_M2 msg: 1613178233.94 epoch sec; seqNo:18067 2021-02-13T01:03:53Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-13T01:03:53Z- handleDepth()-Depth_Keller msg: 1613178233952 epoch sec; seqNo:18210; depth 7.04 2021-02-13T01:03:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178233.96 epoch sec; seqNo:18210; 4060742.42 north; 593614.67 east 2021-02-13T01:03:54Z- AHRS_M2 msg: 1613178234.35 epoch sec; seqNo:18068 2021-02-13T01:03:54Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-13T01:03:54Z- handleDepth()-Depth_Keller msg: 1613178234358 epoch sec; seqNo:18211; depth 6.95 2021-02-13T01:03:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:54Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:54Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:54Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:54Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:54Z- handleDvl() - RDI_Pathfinder msg: 1613178234.36 epoch sec; seqNo:8839 2021-02-13T01:03:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:54Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.30 , 0.07 2021-02-13T01:03:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178234.37 epoch sec; seqNo:18211; 4060742.43 north; 593615.89 east 2021-02-13T01:03:54Z- AHRS_M2 msg: 1613178234.76 epoch sec; seqNo:18069 2021-02-13T01:03:54Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-13T01:03:54Z- handleDepth()-Depth_Keller msg: 1613178234766 epoch sec; seqNo:18212; depth 6.84 2021-02-13T01:03:54Z- handleDvl() - RDI_Pathfinder msg: 1613178234.78 epoch sec; seqNo:8840 2021-02-13T01:03:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 103.88 , 118.75 , 82.23 2021-02-13T01:03:54Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.28 , -0.19 2021-02-13T01:03:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178234.79 epoch sec; seqNo:18212; 4060742.81 north; 593615.89 east 2021-02-13T01:03:55Z- AHRS_M2 msg: 1613178235.16 epoch sec; seqNo:18070 2021-02-13T01:03:55Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-13T01:03:55Z- handleDepth()-Depth_Keller msg: 1613178235174 epoch sec; seqNo:18213; depth 6.76 2021-02-13T01:03:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178235.18 epoch sec; seqNo:18213; 4060743.19 north; 593615.88 east 2021-02-13T01:03:55Z- AHRS_M2 msg: 1613178235.56 epoch sec; seqNo:18071 2021-02-13T01:03:55Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.18 psi 2021-02-13T01:03:55Z- handleDepth()-Depth_Keller msg: 1613178235564 epoch sec; seqNo:18214; depth 6.65 2021-02-13T01:03:55Z- handleDvl() - RDI_Pathfinder msg: 1613178235.58 epoch sec; seqNo:8841 2021-02-13T01:03:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.98 , 106.44 , 121.32 , 82.10 2021-02-13T01:03:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.08 , -0.41 2021-02-13T01:03:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178235.59 epoch sec; seqNo:18214; 4060743.19 north; 593615.88 east 2021-02-13T01:03:55Z- AHRS_M2 msg: 1613178235.96 epoch sec; seqNo:18072 2021-02-13T01:03:55Z- AHRS_M2 msg: 0.62 phi; 0.29 theta; -0.19 psi 2021-02-13T01:03:55Z- handleDepth()-Depth_Keller msg: 1613178235977 epoch sec; seqNo:18215; depth 6.57 2021-02-13T01:03:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178235.99 epoch sec; seqNo:18215; 4060743.57 north; 593615.88 east 2021-02-13T01:03:56Z- AHRS_M2 msg: 1613178236.37 epoch sec; seqNo:18073 2021-02-13T01:03:56Z- AHRS_M2 msg: 0.61 phi; 0.28 theta; -0.20 psi 2021-02-13T01:03:56Z- handleDepth()-Depth_Keller msg: 1613178236372 epoch sec; seqNo:18216; depth 6.48 2021-02-13T01:03:56Z- handleDvl() - RDI_Pathfinder msg: 1613178236.39 epoch sec; seqNo:8842 2021-02-13T01:03:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 106.91 , 115.84 , 0.00 2021-02-13T01:03:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.24 , -0.35 2021-02-13T01:03:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178236.41 epoch sec; seqNo:18216; 4060743.96 north; 593617.09 east 2021-02-13T01:03:56Z- AHRS_M2 msg: 1613178236.77 epoch sec; seqNo:18074 2021-02-13T01:03:56Z- AHRS_M2 msg: 0.60 phi; 0.27 theta; -0.21 psi 2021-02-13T01:03:56Z- handleDepth()-Depth_Keller msg: 1613178236776 epoch sec; seqNo:18217; depth 6.38 2021-02-13T01:03:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:56Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:56Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:56Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:56Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:56Z- handleDvl() - RDI_Pathfinder msg: 1613178236.78 epoch sec; seqNo:8843 2021-02-13T01:03:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.24 , -0.35 2021-02-13T01:03:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178236.79 epoch sec; seqNo:18217; 4060743.96 north; 593617.09 east 2021-02-13T01:03:57Z- AHRS_M2 msg: 1613178237.18 epoch sec; seqNo:18075 2021-02-13T01:03:57Z- AHRS_M2 msg: 0.59 phi; 0.27 theta; -0.21 psi 2021-02-13T01:03:57Z- handleDepth()-Depth_Keller msg: 1613178237180 epoch sec; seqNo:18218; depth 6.30 2021-02-13T01:03:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178237.19 epoch sec; seqNo:18218; 4060744.34 north; 593617.09 east 2021-02-13T01:03:57Z- AHRS_M2 msg: 1613178237.58 epoch sec; seqNo:18076 2021-02-13T01:03:57Z- AHRS_M2 msg: 0.59 phi; 0.28 theta; -0.19 psi 2021-02-13T01:03:57Z- handleDepth()-Depth_Keller msg: 1613178237593 epoch sec; seqNo:18219; depth 6.21 2021-02-13T01:03:57Z- handleDvl() - RDI_Pathfinder msg: 1613178237.60 epoch sec; seqNo:8844 2021-02-13T01:03:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 109.41 , 113.88 , 82.64 2021-02-13T01:03:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.28 , 0.03 2021-02-13T01:03:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178237.61 epoch sec; seqNo:18219; 4060744.72 north; 593617.09 east 2021-02-13T01:03:57Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-13T01:03:57Z- waiting for 1613178235.78, time is 1613178237.61 2021-02-13T01:03:57Z- LcmTrn::updateTrn() equating pose 1613178237.61 and meas times 1613178237.61 2021-02-13T01:03:57Z- LcmTrn::updateTrn() >>>> pose time 1613178237.61, meas time 1613178237.61 <<<< 2021-02-13T01:03:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178237.61 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:03:57Z- TerrainNav::motionUpdate() - currEstimate = (4060745, 593617.09, 6.21) 2021-02-13T01:03:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:03:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:03:57Z- TNavBF::Projecting Measurements 2021-02-13T01:03:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:03:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:03:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:03:57Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:03:57Z- TNavFilter::3 good beams. 2021-02-13T01:03:57Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:03:57Z- TNavBF:: beamIndex[0] = 1 2021-02-13T01:03:57Z- TNavBF:: beamIndex[1] = 2 2021-02-13T01:03:57Z- TNavBF:: beamIndex[2] = 3 2021-02-13T01:03:57Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:03:57Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:03:57Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:03:57Z- TNavBF::mmse selected from filter -1 2021-02-13T01:03:57Z- TerrainMapOctree: (vehN, vehE ) = (4060744.72, 593617.09). 2021-02-13T01:03:57Z- TerrainMapOctree: Min Distance = 809.87. 2021-02-13T01:03:57Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:03:57Z- There is no specific good beam that all particles have in common. 2021-02-13T01:03:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:03:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.58 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 19.15 2021-02-13T01:03:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:03:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.47 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 20.75 2021-02-13T01:03:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:03:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 10.93 2021-02-13T01:03:58Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:03:58Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:03:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:03:58Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:03:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:03:58Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:03:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:03:58Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:03:58Z- Filter 0 No beams used in subcloud update 2021-02-13T01:03:58Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:03:58Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:03:58Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 677.3 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T01:03:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178237.61 sec. 2021-02-13T01:03:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:03:58Z- TNavBF::mmse selected from filter -1 2021-02-13T01:03:58Z- variances([0], [2], and psi_berg) = 677.260727 832.987332 0.000000 2021-02-13T01:03:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:03:58Z- LcmTrn::publishEstimates() - MLE : 4060744.22 593633.59 6.21 2021-02-13T01:03:58Z- LcmTrn::publishEstimates() - MMSE : 4060744.72 593617.09 6.21 2021-02-13T01:03:58Z- LcmTrn::publishEstimates() - COVAR: 677.26 832.99 1.24 0.00 2021-02-13T01:03:58Z- AHRS_M2 msg: 1613178237.98 epoch sec; seqNo:18077 2021-02-13T01:03:58Z- AHRS_M2 msg: 0.58 phi; 0.29 theta; -0.17 psi 2021-02-13T01:03:58Z- handleDepth()-Depth_Keller msg: 1613178237993 epoch sec; seqNo:18220; depth 6.13 2021-02-13T01:03:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:58Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:58Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:58Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:58Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:58Z- handleDvl() - RDI_Pathfinder msg: 1613178237.99 epoch sec; seqNo:8845 2021-02-13T01:03:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.28 , 0.03 2021-02-13T01:03:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178238.01 epoch sec; seqNo:18220; 4060745.10 north; 593617.08 east 2021-02-13T01:03:58Z- AHRS_M2 msg: 1613178238.39 epoch sec; seqNo:18078 2021-02-13T01:03:58Z- AHRS_M2 msg: 0.58 phi; 0.30 theta; -0.16 psi 2021-02-13T01:03:58Z- handleDepth()-Depth_Keller msg: 1613178238394 epoch sec; seqNo:18221; depth 6.05 2021-02-13T01:03:58Z- handleDvl() - RDI_Pathfinder msg: 1613178238.41 epoch sec; seqNo:8846 2021-02-13T01:03:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 106.84 , 114.28 , 83.04 2021-02-13T01:03:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.12 , 0.29 2021-02-13T01:03:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178238.43 epoch sec; seqNo:18221; 4060745.47 north; 593617.08 east 2021-02-13T01:03:58Z- AHRS_M2 msg: 1613178238.80 epoch sec; seqNo:18079 2021-02-13T01:03:58Z- AHRS_M2 msg: 0.58 phi; 0.30 theta; -0.14 psi 2021-02-13T01:03:58Z- handleDepth()-Depth_Keller msg: 1613178238803 epoch sec; seqNo:18222; depth 5.96 2021-02-13T01:03:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178238.81 epoch sec; seqNo:18222; 4060745.47 north; 593617.08 east 2021-02-13T01:03:59Z- AHRS_M2 msg: 1613178239.20 epoch sec; seqNo:18080 2021-02-13T01:03:59Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.14 psi 2021-02-13T01:03:59Z- handleDepth()-Depth_Keller msg: 1613178239203 epoch sec; seqNo:18223; depth 5.88 2021-02-13T01:03:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:03:59Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:03:59Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:03:59Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:03:59Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:03:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:03:59Z- handleDvl() - RDI_Pathfinder msg: 1613178239.20 epoch sec; seqNo:8847 2021-02-13T01:03:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:03:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.12 , 0.29 2021-02-13T01:03:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178239.21 epoch sec; seqNo:18223; 4060745.85 north; 593617.07 east 2021-02-13T01:03:59Z- AHRS_M2 msg: 1613178239.60 epoch sec; seqNo:18081 2021-02-13T01:03:59Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.14 psi 2021-02-13T01:03:59Z- handleDepth()-Depth_Keller msg: 1613178239604 epoch sec; seqNo:18224; depth 5.79 2021-02-13T01:03:59Z- handleDvl() - RDI_Pathfinder msg: 1613178239.62 epoch sec; seqNo:8848 2021-02-13T01:03:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.17 , 103.94 , 118.81 , 82.30 2021-02-13T01:03:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.34 , 0.11 2021-02-13T01:03:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178239.64 epoch sec; seqNo:18224; 4060746.25 north; 593618.29 east 2021-02-13T01:04:00Z- AHRS_M2 msg: 1613178240.00 epoch sec; seqNo:18082 2021-02-13T01:04:00Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-13T01:04:00Z- handleDepth()-Depth_Keller msg: 1613178240009 epoch sec; seqNo:18225; depth 5.67 2021-02-13T01:04:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178240.03 epoch sec; seqNo:18225; 4060746.25 north; 593618.29 east 2021-02-13T01:04:00Z- AHRS_M2 msg: 1613178240.42 epoch sec; seqNo:18083 2021-02-13T01:04:00Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-13T01:04:00Z- handleDepth()-Depth_Keller msg: 1613178240428 epoch sec; seqNo:18226; depth 5.59 2021-02-13T01:04:00Z- handleDvl() - RDI_Pathfinder msg: 1613178240.44 epoch sec; seqNo:8849 2021-02-13T01:04:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.98 , 105.09 , 117.26 , 83.45 2021-02-13T01:04:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.19 , -0.09 2021-02-13T01:04:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178240.45 epoch sec; seqNo:18226; 4060746.62 north; 593618.29 east 2021-02-13T01:04:00Z- AHRS_M2 msg: 1613178240.81 epoch sec; seqNo:18084 2021-02-13T01:04:00Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-13T01:04:00Z- handleDepth()-Depth_Keller msg: 1613178240816 epoch sec; seqNo:18227; depth 5.45 2021-02-13T01:04:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178240.84 epoch sec; seqNo:18227; 4060746.62 north; 593618.29 east 2021-02-13T01:04:01Z- AHRS_M2 msg: 1613178241.22 epoch sec; seqNo:18085 2021-02-13T01:04:01Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.16 psi 2021-02-13T01:04:01Z- handleDepth()-Depth_Keller msg: 1613178241220 epoch sec; seqNo:18228; depth 5.36 2021-02-13T01:04:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178241.23 epoch sec; seqNo:18228; 4060747.00 north; 593618.28 east 2021-02-13T01:04:01Z- AHRS_M2 msg: 1613178241.62 epoch sec; seqNo:18086 2021-02-13T01:04:01Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.17 psi 2021-02-13T01:04:01Z- handleDepth()-Depth_Keller msg: 1613178241624 epoch sec; seqNo:18229; depth 5.25 2021-02-13T01:04:01Z- handleDvl() - RDI_Pathfinder msg: 1613178241.64 epoch sec; seqNo:8850 2021-02-13T01:04:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.57 , 107.80 , 117.26 , 83.45 2021-02-13T01:04:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , -0.06 2021-02-13T01:04:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178241.65 epoch sec; seqNo:18229; 4060747.02 north; 593619.50 east 2021-02-13T01:04:02Z- AHRS_M2 msg: 1613178242.03 epoch sec; seqNo:18087 2021-02-13T01:04:02Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.17 psi 2021-02-13T01:04:02Z- handleDepth()-Depth_Keller msg: 1613178242034 epoch sec; seqNo:18230; depth 5.13 2021-02-13T01:04:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178242.05 epoch sec; seqNo:18230; 4060747.40 north; 593619.50 east 2021-02-13T01:04:02Z- AHRS_M2 msg: 1613178242.43 epoch sec; seqNo:18088 2021-02-13T01:04:02Z- AHRS_M2 msg: 0.57 phi; 0.31 theta; -0.17 psi 2021-02-13T01:04:02Z- handleDepth()-Depth_Keller msg: 1613178242432 epoch sec; seqNo:18231; depth 5.02 2021-02-13T01:04:02Z- handleDvl() - RDI_Pathfinder msg: 1613178242.45 epoch sec; seqNo:8851 2021-02-13T01:04:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.28 , 106.51 , 120.03 , 0.00 2021-02-13T01:04:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.08 , 0.03 2021-02-13T01:04:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178242.46 epoch sec; seqNo:18231; 4060747.77 north; 593619.49 east 2021-02-13T01:04:02Z- AHRS_M2 msg: 1613178242.83 epoch sec; seqNo:18089 2021-02-13T01:04:02Z- AHRS_M2 msg: 0.57 phi; 0.29 theta; -0.16 psi 2021-02-13T01:04:02Z- handleDepth()-Depth_Keller msg: 1613178242841 epoch sec; seqNo:18232; depth 4.90 2021-02-13T01:04:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:04:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:04:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:04:02Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:04:02Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:04:02Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:04:02Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:04:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:04:02Z- handleDvl() - RDI_Pathfinder msg: 1613178242.84 epoch sec; seqNo:8852 2021-02-13T01:04:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:04:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.08 , 0.03 2021-02-13T01:04:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178242.85 epoch sec; seqNo:18232; 4060748.15 north; 593619.49 east 2021-02-13T01:04:03Z- LcmTrn::handleMessages() - handling 47 messages took 0.62 ms and 135 calls... 2021-02-13T01:04:03Z- waiting for 1613178240.61, time is 1613178242.85 2021-02-13T01:04:03Z- LcmTrn::updateTrn() equating pose 1613178242.85 and meas times 1613178242.85 2021-02-13T01:04:03Z- LcmTrn::updateTrn() >>>> pose time 1613178242.85, meas time 1613178242.85 <<<< 2021-02-13T01:04:03Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613178242.85, ping # 0. 2021-02-13T01:04:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178242.85 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:04:03Z- TerrainNav::motionUpdate() - currEstimate = (4060748, 593619.49, 4.90) 2021-02-13T01:04:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:04:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:04:03Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:04:03Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:04:03Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178242.85 sec.; No relevant navigation data available 2021-02-13T01:04:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:04:03Z- TNavBF::mmse selected from filter -1 2021-02-13T01:04:03Z- variances([0], [2], and psi_berg) = 677.217037 832.987314 0.000000 2021-02-13T01:04:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:04:03Z- LcmTrn::publishEstimates() - MLE : 4060747.65 593635.99 4.90 2021-02-13T01:04:03Z- LcmTrn::publishEstimates() - MMSE : 4060748.15 593619.49 4.90 2021-02-13T01:04:03Z- LcmTrn::publishEstimates() - COVAR: 677.22 832.99 1.46 0.00 2021-02-13T01:04:03Z- AHRS_M2 msg: 1613178243.24 epoch sec; seqNo:18090 2021-02-13T01:04:03Z- AHRS_M2 msg: 0.58 phi; 0.26 theta; -0.15 psi 2021-02-13T01:04:03Z- handleDepth()-Depth_Keller msg: 1613178243241 epoch sec; seqNo:18233; depth 4.78 2021-02-13T01:04:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178243.25 epoch sec; seqNo:18233; 4060748.53 north; 593619.49 east 2021-02-13T01:04:03Z- AHRS_M2 msg: 1613178243.64 epoch sec; seqNo:18091 2021-02-13T01:04:03Z- AHRS_M2 msg: 0.58 phi; 0.23 theta; -0.15 psi 2021-02-13T01:04:03Z- handleDepth()-Depth_Keller msg: 1613178243644 epoch sec; seqNo:18234; depth 4.66 2021-02-13T01:04:03Z- handleDvl() - RDI_Pathfinder msg: 1613178243.66 epoch sec; seqNo:8853 2021-02-13T01:04:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.65 , 105.09 , 115.50 , 84.26 2021-02-13T01:04:03Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.03 , 0.08 2021-02-13T01:04:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178243.67 epoch sec; seqNo:18234; 4060748.91 north; 593619.48 east 2021-02-13T01:04:04Z- AHRS_M2 msg: 1613178244.04 epoch sec; seqNo:18092 2021-02-13T01:04:04Z- AHRS_M2 msg: 0.59 phi; 0.19 theta; -0.15 psi 2021-02-13T01:04:04Z- handleDepth()-Depth_Keller msg: 1613178244048 epoch sec; seqNo:18235; depth 4.55 2021-02-13T01:04:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:04:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:04:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:04:04Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:04:04Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:04:04Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:04:04Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:04:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:04:04Z- handleDvl() - RDI_Pathfinder msg: 1613178244.05 epoch sec; seqNo:8854 2021-02-13T01:04:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:04:04Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.03 , 0.08 2021-02-13T01:04:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178244.07 epoch sec; seqNo:18235; 4060749.30 north; 593620.70 east 2021-02-13T01:04:04Z- AHRS_M2 msg: 1613178244.46 epoch sec; seqNo:18093 2021-02-13T01:04:04Z- AHRS_M2 msg: 0.59 phi; 0.15 theta; -0.16 psi 2021-02-13T01:04:04Z- handleDepth()-Depth_Keller msg: 1613178244466 epoch sec; seqNo:18236; depth 4.44 2021-02-13T01:04:04Z- handleDvl() - RDI_Pathfinder msg: 1613178244.48 epoch sec; seqNo:8855 2021-02-13T01:04:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 103.73 , 110.50 , 86.15 2021-02-13T01:04:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.11 , 0.03 2021-02-13T01:04:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178244.49 epoch sec; seqNo:18236; 4060749.30 north; 593620.70 east 2021-02-13T01:04:04Z- AHRS_M2 msg: 1613178244.85 epoch sec; seqNo:18094 2021-02-13T01:04:04Z- AHRS_M2 msg: 0.60 phi; 0.11 theta; -0.16 psi 2021-02-13T01:04:04Z- handleDepth()-Depth_Keller msg: 1613178244865 epoch sec; seqNo:18237; depth 4.36 2021-02-13T01:04:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178244.88 epoch sec; seqNo:18237; 4060749.68 north; 593620.69 east 2021-02-13T01:04:05Z- AHRS_M2 msg: 1613178245.26 epoch sec; seqNo:18095 2021-02-13T01:04:05Z- AHRS_M2 msg: 0.60 phi; 0.07 theta; -0.16 psi 2021-02-13T01:04:05Z- handleDepth()-Depth_Keller msg: 1613178245260 epoch sec; seqNo:18238; depth 4.28 2021-02-13T01:04:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:04:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:04:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:04:05Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:04:05Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:04:05Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:04:05Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:04:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:04:05Z- handleDvl() - RDI_Pathfinder msg: 1613178245.26 epoch sec; seqNo:8856 2021-02-13T01:04:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:04:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.11 , 0.03 2021-02-13T01:04:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178245.28 epoch sec; seqNo:18238; 4060750.06 north; 593620.69 east 2021-02-13T01:04:05Z- AHRS_M2 msg: 1613178245.66 epoch sec; seqNo:18096 2021-02-13T01:04:05Z- AHRS_M2 msg: 0.62 phi; 0.03 theta; -0.15 psi 2021-02-13T01:04:05Z- handleDepth()-Depth_Keller msg: 1613178245669 epoch sec; seqNo:18239; depth 4.20 2021-02-13T01:04:05Z- handleDvl() - RDI_Pathfinder msg: 1613178245.68 epoch sec; seqNo:8857 2021-02-13T01:04:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.99 , 102.52 , 99.81 , 90.35 2021-02-13T01:04:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.22 , -0.07 2021-02-13T01:04:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178245.70 epoch sec; seqNo:18239; 4060750.44 north; 593620.69 east 2021-02-13T01:04:06Z- AHRS_M2 msg: 1613178246.06 epoch sec; seqNo:18097 2021-02-13T01:04:06Z- AHRS_M2 msg: 0.63 phi; -0.01 theta; -0.15 psi 2021-02-13T01:04:06Z- handleDepth()-Depth_Keller msg: 1613178246068 epoch sec; seqNo:18240; depth 4.15 2021-02-13T01:04:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178246.08 epoch sec; seqNo:18240; 4060750.45 north; 593621.91 east 2021-02-13T01:04:06Z- AHRS_M2 msg: 1613178246.47 epoch sec; seqNo:18098 2021-02-13T01:04:06Z- AHRS_M2 msg: 0.64 phi; -0.06 theta; -0.15 psi 2021-02-13T01:04:06Z- handleDepth()-Depth_Keller msg: 1613178246478 epoch sec; seqNo:18241; depth 4.11 2021-02-13T01:04:06Z- handleDvl() - RDI_Pathfinder msg: 1613178246.49 epoch sec; seqNo:8858 2021-02-13T01:04:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.98 , 101.50 , 97.44 , 97.44 2021-02-13T01:04:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.17 , -0.29 2021-02-13T01:04:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178246.50 epoch sec; seqNo:18241; 4060750.83 north; 593621.90 east 2021-02-13T01:04:06Z- AHRS_M2 msg: 1613178246.88 epoch sec; seqNo:18099 2021-02-13T01:04:06Z- AHRS_M2 msg: 0.65 phi; -0.14 theta; -0.21 psi 2021-02-13T01:04:06Z- handleDepth()-Depth_Keller msg: 1613178246886 epoch sec; seqNo:18242; depth 4.05 2021-02-13T01:04:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178246.90 epoch sec; seqNo:18242; 4060751.21 north; 593621.90 east 2021-02-13T01:04:07Z- AHRS_M2 msg: 1613178247.28 epoch sec; seqNo:18100 2021-02-13T01:04:07Z- AHRS_M2 msg: 0.65 phi; -0.17 theta; -0.25 psi 2021-02-13T01:04:07Z- handleDepth()-Depth_Keller msg: 1613178247281 epoch sec; seqNo:18243; depth 4.02 2021-02-13T01:04:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178247.29 epoch sec; seqNo:18243; 4060751.21 north; 593621.90 east 2021-02-13T01:04:07Z- AHRS_M2 msg: 1613178247.69 epoch sec; seqNo:18101 2021-02-13T01:04:07Z- AHRS_M2 msg: 0.66 phi; -0.19 theta; -0.26 psi 2021-02-13T01:04:07Z- handleDepth()-Depth_Keller msg: 1613178247692 epoch sec; seqNo:18244; depth 3.97 2021-02-13T01:04:07Z- handleDvl() - RDI_Pathfinder msg: 1613178247.71 epoch sec; seqNo:8859 2021-02-13T01:04:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 105.49 , 97.38 , 102.79 2021-02-13T01:04:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.21 , -0.27 2021-02-13T01:04:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178247.73 epoch sec; seqNo:18244; 4060751.59 north; 593621.89 east 2021-02-13T01:04:08Z- LcmTrn::handleMessages() - handling 43 messages took 0.66 ms and 130 calls... 2021-02-13T01:04:08Z- waiting for 1613178245.85, time is 1613178247.73 2021-02-13T01:04:08Z- LcmTrn::updateTrn() equating pose 1613178247.73 and meas times 1613178247.73 2021-02-13T01:04:08Z- LcmTrn::updateTrn() >>>> pose time 1613178247.73, meas time 1613178247.73 <<<< 2021-02-13T01:04:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178247.73 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:04:08Z- TerrainNav::motionUpdate() - currEstimate = (4060752, 593621.89, 3.97) 2021-02-13T01:04:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:04:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:04:08Z- TNavBF::Projecting Measurements 2021-02-13T01:04:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:04:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:04:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:04:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:04:08Z- TNavFilter::4 good beams. 2021-02-13T01:04:08Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:04:08Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:04:08Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:04:08Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:04:08Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:04:08Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:04:08Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:04:08Z- TNavBF::mmse selected from filter -1 2021-02-13T01:04:08Z- TerrainMapOctree: (vehN, vehE ) = (4060751.59, 593621.89). 2021-02-13T01:04:08Z- TerrainMapOctree: Min Distance = 801.83. 2021-02-13T01:04:08Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:04:08Z- There is no specific good beam that all particles have in common. 2021-02-13T01:04:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:04:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.67 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 11.99 2021-02-13T01:04:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:04:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.49 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 17.81 2021-02-13T01:04:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:04:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 15.17 2021-02-13T01:04:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:04:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 16.90 2021-02-13T01:04:08Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:04:08Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:04:08Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:04:08Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:04:08Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:04:08Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:04:08Z- Filter 0 No beams used in subcloud update 2021-02-13T01:04:08Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:04:08Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:04:08Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 677.2 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T01:04:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178247.73 sec. 2021-02-13T01:04:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:04:08Z- TNavBF::mmse selected from filter -1 2021-02-13T01:04:08Z- variances([0], [2], and psi_berg) = 677.173367 832.987296 0.000000 2021-02-13T01:04:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:04:08Z- LcmTrn::publishEstimates() - MLE : 4060751.09 593638.39 3.97 2021-02-13T01:04:08Z- LcmTrn::publishEstimates() - MMSE : 4060751.59 593621.89 3.97 2021-02-13T01:04:08Z- LcmTrn::publishEstimates() - COVAR: 677.17 832.99 1.20 0.00 2021-02-13T01:04:08Z- AHRS_M2 msg: 1613178248.09 epoch sec; seqNo:18102 2021-02-13T01:04:08Z- AHRS_M2 msg: 0.67 phi; -0.22 theta; -0.25 psi 2021-02-13T01:04:08Z- handleDepth()-Depth_Keller msg: 1613178248089 epoch sec; seqNo:18245; depth 3.99 2021-02-13T01:04:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178248.10 epoch sec; seqNo:18245; 4060751.97 north; 593621.89 east 2021-02-13T01:04:08Z- AHRS_M2 msg: 1613178248.49 epoch sec; seqNo:18103 2021-02-13T01:04:08Z- AHRS_M2 msg: 0.68 phi; -0.25 theta; -0.23 psi 2021-02-13T01:04:08Z- handleDepth()-Depth_Keller msg: 1613178248492 epoch sec; seqNo:18246; depth 4.02 2021-02-13T01:04:08Z- handleDvl() - RDI_Pathfinder msg: 1613178248.51 epoch sec; seqNo:8860 2021-02-13T01:04:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 113.40 , 87.71 , 110.70 2021-02-13T01:04:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.26 , 0.18 2021-02-13T01:04:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178248.52 epoch sec; seqNo:18246; 4060752.36 north; 593623.11 east 2021-02-13T01:04:08Z- AHRS_M2 msg: 1613178248.89 epoch sec; seqNo:18104 2021-02-13T01:04:08Z- AHRS_M2 msg: 0.68 phi; -0.28 theta; -0.20 psi 2021-02-13T01:04:08Z- handleDepth()-Depth_Keller msg: 1613178248896 epoch sec; seqNo:18247; depth 4.05 2021-02-13T01:04:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:04:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:04:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:04:08Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:04:08Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:04:08Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:04:08Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:04:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:04:08Z- handleDvl() - RDI_Pathfinder msg: 1613178248.90 epoch sec; seqNo:8861 2021-02-13T01:04:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:04:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.26 , 0.18 2021-02-13T01:04:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178248.91 epoch sec; seqNo:18247; 4060752.74 north; 593623.10 east 2021-02-13T01:04:09Z- AHRS_M2 msg: 1613178249.30 epoch sec; seqNo:18105 2021-02-13T01:04:09Z- AHRS_M2 msg: 0.69 phi; -0.30 theta; -0.17 psi 2021-02-13T01:04:09Z- handleDepth()-Depth_Keller msg: 1613178249305 epoch sec; seqNo:18248; depth 4.11 2021-02-13T01:04:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178249.32 epoch sec; seqNo:18248; 4060753.12 north; 593623.10 east 2021-02-13T01:04:09Z- AHRS_M2 msg: 1613178249.70 epoch sec; seqNo:18106 2021-02-13T01:04:09Z- AHRS_M2 msg: 0.69 phi; -0.32 theta; -0.15 psi 2021-02-13T01:04:09Z- handleDepth()-Depth_Keller msg: 1613178249713 epoch sec; seqNo:18249; depth 4.14 2021-02-13T01:04:09Z- handleDvl() - RDI_Pathfinder msg: 1613178249.72 epoch sec; seqNo:8862 2021-02-13T01:04:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 109.28 , 0.00 , 113.33 2021-02-13T01:04:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , 0.07 , 0.26 2021-02-13T01:04:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178249.73 epoch sec; seqNo:18249; 4060753.12 north; 593623.10 east 2021-02-13T01:04:10Z- AHRS_M2 msg: 1613178250.10 epoch sec; seqNo:18107 2021-02-13T01:04:10Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.17 psi 2021-02-13T01:04:10Z- handleDepth()-Depth_Keller msg: 1613178250108 epoch sec; seqNo:18250; depth 4.21 2021-02-13T01:04:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:04:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:04:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:04:10Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:04:10Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:04:10Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:04:10Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:04:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:04:10Z- handleDvl() - RDI_Pathfinder msg: 1613178250.11 epoch sec; seqNo:8863 2021-02-13T01:04:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:04:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , 0.07 , 0.26 2021-02-13T01:04:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178250.13 epoch sec; seqNo:18250; 4060753.12 north; 593623.10 east 2021-02-13T01:04:10Z- AHRS_M2 msg: 1613178250.51 epoch sec; seqNo:18108 2021-02-13T01:04:10Z- AHRS_M2 msg: 0.69 phi; -0.35 theta; -0.19 psi 2021-02-13T01:04:10Z- handleDepth()-Depth_Keller msg: 1613178250512 epoch sec; seqNo:18251; depth 4.28 2021-02-13T01:04:10Z- handleDvl() - RDI_Pathfinder msg: 1613178250.52 epoch sec; seqNo:8864 2021-02-13T01:04:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.58 , 107.99 , 84.19 , 118.41 2021-02-13T01:04:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , 0.14 , 0.08 2021-02-13T01:04:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178250.55 epoch sec; seqNo:18251; 4060753.50 north; 593623.09 east 2021-02-13T01:04:10Z- AHRS_M2 msg: 1613178250.91 epoch sec; seqNo:18109 2021-02-13T01:04:10Z- AHRS_M2 msg: 0.70 phi; -0.36 theta; -0.20 psi 2021-02-13T01:04:10Z- handleDepth()-Depth_Keller msg: 1613178250916 epoch sec; seqNo:18252; depth 4.34 2021-02-13T01:04:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178250.93 epoch sec; seqNo:18252; 4060753.50 north; 593623.09 east 2021-02-13T01:04:11Z- AHRS_M2 msg: 1613178251.32 epoch sec; seqNo:18110 2021-02-13T01:04:11Z- AHRS_M2 msg: 0.70 phi; -0.36 theta; -0.21 psi 2021-02-13T01:04:11Z- handleDepth()-Depth_Keller msg: 1613178251322 epoch sec; seqNo:18253; depth 4.42 2021-02-13T01:04:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:04:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:04:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:04:11Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:04:11Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:04:11Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:04:11Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:04:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:04:11Z- handleDvl() - RDI_Pathfinder msg: 1613178251.33 epoch sec; seqNo:8865 2021-02-13T01:04:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:04:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , 0.14 , 0.08 2021-02-13T01:04:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178251.34 epoch sec; seqNo:18253; 4060753.51 north; 593624.31 east 2021-02-13T01:04:11Z- AHRS_M2 msg: 1613178251.72 epoch sec; seqNo:18111 2021-02-13T01:04:11Z- AHRS_M2 msg: 0.70 phi; -0.37 theta; -0.22 psi 2021-02-13T01:04:11Z- handleDepth()-Depth_Keller msg: 1613178251729 epoch sec; seqNo:18254; depth 4.51 2021-02-13T01:04:11Z- handleDvl() - RDI_Pathfinder msg: 1613178251.75 epoch sec; seqNo:8866 2021-02-13T01:04:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 112.79 , 83.04 , 124.69 2021-02-13T01:04:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , 0.07 , 0.09 2021-02-13T01:04:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178251.76 epoch sec; seqNo:18254; 4060753.89 north; 593624.31 east 2021-02-13T01:04:12Z- AHRS_M2 msg: 1613178252.12 epoch sec; seqNo:18112 2021-02-13T01:04:12Z- AHRS_M2 msg: 0.69 phi; -0.38 theta; -0.22 psi 2021-02-13T01:04:12Z- handleDepth()-Depth_Keller msg: 1613178252137 epoch sec; seqNo:18255; depth 4.62 2021-02-13T01:04:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178252.16 epoch sec; seqNo:18255; 4060753.89 north; 593624.31 east 2021-02-13T01:04:12Z- AHRS_M2 msg: 1613178252.53 epoch sec; seqNo:18113 2021-02-13T01:04:12Z- AHRS_M2 msg: 0.68 phi; -0.38 theta; -0.22 psi 2021-02-13T01:04:12Z- handleDepth()-Depth_Keller msg: 1613178252532 epoch sec; seqNo:18256; depth 4.72 2021-02-13T01:04:12Z- handleDvl() - RDI_Pathfinder msg: 1613178252.54 epoch sec; seqNo:8867 2021-02-13T01:04:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 117.05 , 82.84 , 130.44 2021-02-13T01:04:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , 0.11 , 0.14 2021-02-13T01:04:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178252.55 epoch sec; seqNo:18256; 4060754.27 north; 593624.31 east 2021-02-13T01:04:12Z- AHRS_M2 msg: 1613178252.93 epoch sec; seqNo:18114 2021-02-13T01:04:12Z- AHRS_M2 msg: 0.67 phi; -0.39 theta; -0.22 psi 2021-02-13T01:04:12Z- handleDepth()-Depth_Keller msg: 1613178252937 epoch sec; seqNo:18257; depth 4.83 2021-02-13T01:04:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178252.95 epoch sec; seqNo:18257; 4060754.27 north; 593624.31 east 2021-02-13T01:04:13Z- LcmTrn::handleMessages() - handling 47 messages took 0.64 ms and 135 calls... 2021-02-13T01:04:13Z- waiting for 1613178250.73, time is 1613178252.95 2021-02-13T01:04:13Z- LcmTrn::updateTrn() >>>> pose time 1613178252.95, meas time 1613178252.54 <<<< 2021-02-13T01:04:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178252.54 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:04:13Z- TerrainNav::motionUpdate() - currEstimate = (4060754, 593624.31, 4.83) 2021-02-13T01:04:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:04:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:04:13Z- TNavBF::Projecting Measurements 2021-02-13T01:04:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:04:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:04:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:04:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:04:13Z- TNavFilter::4 good beams. 2021-02-13T01:04:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:04:13Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:04:13Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:04:13Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:04:13Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:04:13Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:04:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:04:13Z- TNavBF::mmse selected from filter -1 2021-02-13T01:04:13Z- TerrainMapOctree: (vehN, vehE ) = (4060754.06, 593624.12). 2021-02-13T01:04:13Z- TerrainMapOctree: Min Distance = 798.55. 2021-02-13T01:04:13Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:04:13Z- There is no specific good beam that all particles have in common. 2021-02-13T01:04:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:04:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T01:04:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:04:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 21.92 2021-02-13T01:04:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:04:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 10.98 2021-02-13T01:04:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:04:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 27.22 2021-02-13T01:04:13Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:04:13Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:04:13Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:04:13Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:04:13Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:04:13Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:04:13Z- Filter 0 No beams used in subcloud update 2021-02-13T01:04:13Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:04:13Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:04:13Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 677.1 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T01:04:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178252.54 sec. 2021-02-13T01:04:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:04:13Z- TNavBF::mmse selected from filter -1 2021-02-13T01:04:13Z- variances([0], [2], and psi_berg) = 677.123232 832.987276 0.000000 2021-02-13T01:04:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:04:13Z- LcmTrn::publishEstimates() - MLE : 4060753.77 593640.81 4.83 2021-02-13T01:04:13Z- LcmTrn::publishEstimates() - MMSE : 4060754.27 593624.31 4.83 2021-02-13T01:04:13Z- LcmTrn::publishEstimates() - COVAR: 677.12 832.99 1.07 0.00 2021-02-13T01:04:13Z- AHRS_M2 msg: 1613178253.34 epoch sec; seqNo:18115 2021-02-13T01:04:13Z- AHRS_M2 msg: 0.66 phi; -0.39 theta; -0.23 psi 2021-02-13T01:04:13Z- handleDepth()-Depth_Keller msg: 1613178253355 epoch sec; seqNo:18258; depth 4.93 2021-02-13T01:04:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178253.36 epoch sec; seqNo:18258; 4060754.65 north; 593624.30 east 2021-02-13T01:04:13Z- handleDepth()-Depth_Keller msg: 1613178253762 epoch sec; seqNo:18259; depth 5.03 2021-02-13T01:04:13Z- handleDvl() - RDI_Pathfinder msg: 1613178253.78 epoch sec; seqNo:8868 2021-02-13T01:04:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.51 , 112.39 , 82.64 , 134.70 2021-02-13T01:04:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.19 , 0.07 2021-02-13T01:04:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178253.79 epoch sec; seqNo:18259; 4060754.66 north; 593625.52 east 2021-02-13T01:04:14Z- AHRS_M2 msg: 1613178254.15 epoch sec; seqNo:18116 2021-02-13T01:04:14Z- AHRS_M2 msg: 0.64 phi; -0.38 theta; -0.24 psi 2021-02-13T01:04:14Z- handleDepth()-Depth_Keller msg: 1613178254153 epoch sec; seqNo:18260; depth 5.15 2021-02-13T01:04:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178254.17 epoch sec; seqNo:18260; 4060755.04 north; 593625.52 east 2021-02-13T01:04:14Z- AHRS_M2 msg: 1613178254.57 epoch sec; seqNo:18117 2021-02-13T01:04:14Z- AHRS_M2 msg: 0.64 phi; -0.37 theta; -0.24 psi 2021-02-13T01:04:14Z- handleDepth()-Depth_Keller msg: 1613178254575 epoch sec; seqNo:18261; depth 5.28 2021-02-13T01:04:14Z- handleDvl() - RDI_Pathfinder msg: 1613178254.59 epoch sec; seqNo:8869 2021-02-13T01:04:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.51 , 115.36 , 84.12 , 137.68 2021-02-13T01:04:14Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.15 , -0.01 2021-02-13T01:04:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178254.61 epoch sec; seqNo:18261; 4060755.42 north; 593625.51 east 2021-02-13T01:04:14Z- AHRS_M2 msg: 1613178254.95 epoch sec; seqNo:18118 2021-02-13T01:04:14Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.25 psi 2021-02-13T01:04:14Z- handleDepth()-Depth_Keller msg: 1613178254956 epoch sec; seqNo:18262; depth 5.39 2021-02-13T01:04:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178254.97 epoch sec; seqNo:18262; 4060755.42 north; 593625.51 east 2021-02-13T01:04:15Z- AHRS_M2 msg: 1613178255.36 epoch sec; seqNo:18119 2021-02-13T01:04:15Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.25 psi 2021-02-13T01:04:15Z- handleDepth()-Depth_Keller msg: 1613178255366 epoch sec; seqNo:18263; depth 5.53 2021-02-13T01:04:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178255.37 epoch sec; seqNo:18263; 4060755.80 north; 593625.51 east 2021-02-13T01:04:15Z- AHRS_M2 msg: 1613178255.76 epoch sec; seqNo:18120 2021-02-13T01:04:15Z- AHRS_M2 msg: 0.64 phi; -0.36 theta; -0.24 psi 2021-02-13T01:04:15Z- handleDepth()-Depth_Keller msg: 1613178255764 epoch sec; seqNo:18264; depth 5.64 2021-02-13T01:04:15Z- handleDvl() - RDI_Pathfinder msg: 1613178255.78 epoch sec; seqNo:8870 2021-02-13T01:04:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.94 , 119.69 , 83.99 , 137.54 2021-02-13T01:04:15Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.07 , 0.01 2021-02-13T01:04:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178255.79 epoch sec; seqNo:18264; 4060756.19 north; 593626.73 east 2021-02-13T01:04:16Z- AHRS_M2 msg: 1613178256.17 epoch sec; seqNo:18121 2021-02-13T01:04:16Z- AHRS_M2 msg: 0.63 phi; -0.37 theta; -0.23 psi 2021-02-13T01:04:16Z- handleDepth()-Depth_Keller msg: 1613178256173 epoch sec; seqNo:18265; depth 5.79 2021-02-13T01:04:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178256.18 epoch sec; seqNo:18265; 4060756.19 north; 593626.73 east 2021-02-13T01:04:16Z- AHRS_M2 msg: 1613178256.57 epoch sec; seqNo:18122 2021-02-13T01:04:16Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.22 psi 2021-02-13T01:04:16Z- handleDepth()-Depth_Keller msg: 1613178256582 epoch sec; seqNo:18266; depth 5.92 2021-02-13T01:04:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178256.61 epoch sec; seqNo:18266; 4060756.57 north; 593626.72 east 2021-02-13T01:04:16Z- AHRS_M2 msg: 1613178256.97 epoch sec; seqNo:18123 2021-02-13T01:04:16Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.21 psi 2021-02-13T01:04:16Z- handleDepth()-Depth_Keller msg: 1613178256981 epoch sec; seqNo:18267; depth 6.03 2021-02-13T01:04:17Z- handleDvl() - RDI_Pathfinder msg: 1613178257.00 epoch sec; seqNo:8871 2021-02-13T01:04:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.44 , 113.81 , 82.57 , 130.17 2021-02-13T01:04:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.11 , 0.04 2021-02-13T01:04:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178257.01 epoch sec; seqNo:18267; 4060756.95 north; 593626.72 east 2021-02-13T01:04:17Z- AHRS_M2 msg: 1613178257.38 epoch sec; seqNo:18124 2021-02-13T01:04:17Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.21 psi 2021-02-13T01:04:17Z- handleDepth()-Depth_Keller msg: 1613178257380 epoch sec; seqNo:18268; depth 6.16 2021-02-13T01:04:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178257.39 epoch sec; seqNo:18268; 4060757.33 north; 593626.71 east 2021-02-13T01:04:17Z- AHRS_M2 msg: 1613178257.78 epoch sec; seqNo:18125 2021-02-13T01:04:17Z- AHRS_M2 msg: 0.59 phi; -0.37 theta; -0.22 psi 2021-02-13T01:04:17Z- handleDepth()-Depth_Keller msg: 1613178257789 epoch sec; seqNo:18269; depth 6.28 2021-02-13T01:04:17Z- handleDvl() - RDI_Pathfinder msg: 1613178257.80 epoch sec; seqNo:8872 2021-02-13T01:04:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.58 , 112.46 , 0.00 , 130.31 2021-02-13T01:04:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.12 , -0.14 2021-02-13T01:04:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178257.81 epoch sec; seqNo:18269; 4060757.34 north; 593627.93 east 2021-02-13T01:04:18Z- AHRS_M2 msg: 1613178258.18 epoch sec; seqNo:18126 2021-02-13T01:04:18Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.24 psi 2021-02-13T01:04:18Z- handleDepth()-Depth_Keller msg: 1613178258188 epoch sec; seqNo:18270; depth 6.40 2021-02-13T01:04:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178258.21 epoch sec; seqNo:18270; 4060757.72 north; 593627.93 east 2021-02-13T01:04:18Z- LcmTrn::handleMessages() - handling 43 messages took 0.72 ms and 129 calls... 2021-02-13T01:04:18Z- waiting for 1613178255.95, time is 1613178258.21 2021-02-13T01:04:18Z- LcmTrn::updateTrn() >>>> pose time 1613178258.21, meas time 1613178257.80 <<<< 2021-02-13T01:04:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178257.80 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:04:18Z- TerrainNav::motionUpdate() - currEstimate = (4060758, 593627.93, 6.40) 2021-02-13T01:04:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:04:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:04:18Z- TNavBF::Projecting Measurements 2021-02-13T01:04:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:04:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:04:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:04:18Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:04:18Z- TNavFilter::3 good beams. 2021-02-13T01:04:18Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:04:18Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:04:18Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:04:18Z- TNavBF:: beamIndex[2] = 3 2021-02-13T01:04:18Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:04:18Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:04:18Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:04:18Z- TNavBF::mmse selected from filter -1 2021-02-13T01:04:18Z- TerrainMapOctree: (vehN, vehE ) = (4060757.45, 593627.65). 2021-02-13T01:04:18Z- TerrainMapOctree: Min Distance = 793.68. 2021-02-13T01:04:18Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:04:18Z- There is no specific good beam that all particles have in common. 2021-02-13T01:04:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:04:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 15.23 2021-02-13T01:04:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:04:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 20.23 2021-02-13T01:04:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:04:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 27.17 2021-02-13T01:04:18Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:04:18Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:04:18Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:04:18Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:18Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:04:18Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:04:18Z- Filter 0 No beams used in subcloud update 2021-02-13T01:04:18Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:04:18Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:04:18Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 677.1 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T01:04:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178257.80 sec. 2021-02-13T01:04:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:04:18Z- TNavBF::mmse selected from filter -1 2021-02-13T01:04:18Z- variances([0], [2], and psi_berg) = 677.064228 832.987251 0.000000 2021-02-13T01:04:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:04:18Z- LcmTrn::publishEstimates() - MLE : 4060757.22 593644.43 6.40 2021-02-13T01:04:18Z- LcmTrn::publishEstimates() - MMSE : 4060757.72 593627.93 6.40 2021-02-13T01:04:18Z- LcmTrn::publishEstimates() - COVAR: 677.06 832.99 1.13 0.00 2021-02-13T01:04:18Z- AHRS_M2 msg: 1613178258.59 epoch sec; seqNo:18127 2021-02-13T01:04:18Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.25 psi 2021-02-13T01:04:18Z- handleDepth()-Depth_Keller msg: 1613178258604 epoch sec; seqNo:18271; depth 6.52 2021-02-13T01:04:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178258.64 epoch sec; seqNo:18271; 4060758.10 north; 593627.93 east 2021-02-13T01:04:18Z- AHRS_M2 msg: 1613178258.99 epoch sec; seqNo:18128 2021-02-13T01:04:18Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.25 psi 2021-02-13T01:04:19Z- handleDepth()-Depth_Keller msg: 1613178259001 epoch sec; seqNo:18272; depth 6.63 2021-02-13T01:04:19Z- handleDvl() - RDI_Pathfinder msg: 1613178259.01 epoch sec; seqNo:8873 2021-02-13T01:04:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 113.61 , 82.37 , 128.48 2021-02-13T01:04:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.11 , -0.19 2021-02-13T01:04:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178259.04 epoch sec; seqNo:18272; 4060758.48 north; 593627.92 east 2021-02-13T01:04:19Z- AHRS_M2 msg: 1613178259.40 epoch sec; seqNo:18129 2021-02-13T01:04:19Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.24 psi 2021-02-13T01:04:19Z- handleDepth()-Depth_Keller msg: 1613178259404 epoch sec; seqNo:18273; depth 6.76 2021-02-13T01:04:19Z- handleDvl() - RDI_Pathfinder msg: 1613178259.42 epoch sec; seqNo:8874 2021-02-13T01:04:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:04:19Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:04:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178259.43 epoch sec; seqNo:18273; 4060758.85 north; 593627.92 east 2021-02-13T01:04:19Z- AHRS_M2 msg: 1613178259.80 epoch sec; seqNo:18130 2021-02-13T01:04:19Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.22 psi 2021-02-13T01:04:19Z- handleDepth()-Depth_Keller msg: 1613178259804 epoch sec; seqNo:18274; depth 6.87 2021-02-13T01:04:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178259.82 epoch sec; seqNo:18274; 4060759.23 north; 593627.91 east 2021-02-13T01:04:20Z- AHRS_M2 msg: 1613178260.20 epoch sec; seqNo:18131 2021-02-13T01:04:20Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.21 psi 2021-02-13T01:04:20Z- handleDepth()-Depth_Keller msg: 1613178260213 epoch sec; seqNo:18275; depth 6.99 2021-02-13T01:04:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178260.22 epoch sec; seqNo:18275; 4060759.63 north; 593629.13 east 2021-02-13T01:04:20Z- AHRS_M2 msg: 1613178260.61 epoch sec; seqNo:18132 2021-02-13T01:04:20Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.19 psi 2021-02-13T01:04:20Z- handleDepth()-Depth_Keller msg: 1613178260617 epoch sec; seqNo:18276; depth 7.11 2021-02-13T01:04:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178260.62 epoch sec; seqNo:18276; 4060759.63 north; 593629.13 east 2021-02-13T01:04:21Z- AHRS_M2 msg: 1613178261.01 epoch sec; seqNo:18133 2021-02-13T01:04:21Z- AHRS_M2 msg: 0.57 phi; -0.33 theta; -0.17 psi 2021-02-13T01:04:21Z- handleDepth()-Depth_Keller msg: 1613178261018 epoch sec; seqNo:18277; depth 7.21 2021-02-13T01:04:21Z- handleDvl() - RDI_Pathfinder msg: 1613178261.03 epoch sec; seqNo:8875 2021-02-13T01:04:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.24 , 108.06 , 81.28 , 119.96 2021-02-13T01:04:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.15 , 0.16 2021-02-13T01:04:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178261.04 epoch sec; seqNo:18277; 4060760.00 north; 593629.13 east 2021-02-13T01:04:21Z- AHRS_M2 msg: 1613178261.42 epoch sec; seqNo:18134 2021-02-13T01:04:21Z- AHRS_M2 msg: 0.56 phi; -0.33 theta; -0.17 psi 2021-02-13T01:04:21Z- handleDepth()-Depth_Keller msg: 1613178261423 epoch sec; seqNo:18278; depth 7.31 2021-02-13T01:04:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178261.44 epoch sec; seqNo:18278; 4060760.38 north; 593629.12 east 2021-02-13T01:04:21Z- AHRS_M2 msg: 1613178261.82 epoch sec; seqNo:18135 2021-02-13T01:04:21Z- AHRS_M2 msg: 0.55 phi; -0.33 theta; -0.16 psi 2021-02-13T01:04:21Z- handleDepth()-Depth_Keller msg: 1613178261824 epoch sec; seqNo:18279; depth 7.43 2021-02-13T01:04:21Z- handleDvl() - RDI_Pathfinder msg: 1613178261.84 epoch sec; seqNo:8876 2021-02-13T01:04:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.64 , 105.22 , 80.88 , 118.74 2021-02-13T01:04:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.01 , 0.19 2021-02-13T01:04:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178261.87 epoch sec; seqNo:18279; 4060760.76 north; 593629.12 east 2021-02-13T01:04:22Z- AHRS_M2 msg: 1613178262.22 epoch sec; seqNo:18136 2021-02-13T01:04:22Z- AHRS_M2 msg: 0.55 phi; -0.33 theta; -0.17 psi 2021-02-13T01:04:22Z- handleDepth()-Depth_Keller msg: 1613178262229 epoch sec; seqNo:18280; depth 7.55 2021-02-13T01:04:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178262.24 epoch sec; seqNo:18280; 4060760.76 north; 593629.12 east 2021-02-13T01:04:22Z- AHRS_M2 msg: 1613178262.63 epoch sec; seqNo:18137 2021-02-13T01:04:22Z- AHRS_M2 msg: 0.55 phi; -0.32 theta; -0.17 psi 2021-02-13T01:04:22Z- handleDepth()-Depth_Keller msg: 1613178262633 epoch sec; seqNo:18281; depth 7.63 2021-02-13T01:04:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178262.64 epoch sec; seqNo:18281; 4060761.14 north; 593629.11 east 2021-02-13T01:04:23Z- AHRS_M2 msg: 1613178263.03 epoch sec; seqNo:18138 2021-02-13T01:04:23Z- AHRS_M2 msg: 0.54 phi; -0.32 theta; -0.18 psi 2021-02-13T01:04:23Z- handleDepth()-Depth_Keller msg: 1613178263036 epoch sec; seqNo:18282; depth 7.74 2021-02-13T01:04:23Z- handleDvl() - RDI_Pathfinder msg: 1613178263.06 epoch sec; seqNo:8877 2021-02-13T01:04:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 105.29 , 80.95 , 121.52 2021-02-13T01:04:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.17 , 0.13 2021-02-13T01:04:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178263.07 epoch sec; seqNo:18282; 4060761.14 north; 593629.11 east 2021-02-13T01:04:23Z- LcmTrn::handleMessages() - handling 41 messages took 0.74 ms and 126 calls... 2021-02-13T01:04:23Z- waiting for 1613178261.21, time is 1613178263.07 2021-02-13T01:04:23Z- LcmTrn::updateTrn() equating pose 1613178263.07 and meas times 1613178263.07 2021-02-13T01:04:23Z- LcmTrn::updateTrn() >>>> pose time 1613178263.07, meas time 1613178263.07 <<<< 2021-02-13T01:04:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178263.07 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:04:23Z- TerrainNav::motionUpdate() - currEstimate = (4060761, 593629.11, 7.74) 2021-02-13T01:04:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:04:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:04:23Z- TNavBF::Projecting Measurements 2021-02-13T01:04:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:04:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:04:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:04:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:04:23Z- TNavFilter::4 good beams. 2021-02-13T01:04:23Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:04:23Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:04:23Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:04:23Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:04:23Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:04:23Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:04:23Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:04:23Z- TNavBF::mmse selected from filter -1 2021-02-13T01:04:23Z- TerrainMapOctree: (vehN, vehE ) = (4060761.14, 593629.11). 2021-02-13T01:04:23Z- TerrainMapOctree: Min Distance = 790.24. 2021-02-13T01:04:23Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:04:23Z- There is no specific good beam that all particles have in common. 2021-02-13T01:04:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:04:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.31 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 12.69 2021-02-13T01:04:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:04:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 17.74 2021-02-13T01:04:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:04:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 10.48 2021-02-13T01:04:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:04:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 23.63 2021-02-13T01:04:23Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:04:23Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:04:23Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:04:23Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:04:23Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:04:23Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:04:23Z- Filter 0 No beams used in subcloud update 2021-02-13T01:04:23Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:04:23Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:04:23Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 677.0 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T01:04:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178263.07 sec. 2021-02-13T01:04:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:04:23Z- TNavBF::mmse selected from filter -1 2021-02-13T01:04:23Z- variances([0], [2], and psi_berg) = 677.023708 832.987235 0.000000 2021-02-13T01:04:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:04:23Z- LcmTrn::publishEstimates() - MLE : 4060760.64 593645.61 7.74 2021-02-13T01:04:23Z- LcmTrn::publishEstimates() - MMSE : 4060761.14 593629.11 7.74 2021-02-13T01:04:23Z- LcmTrn::publishEstimates() - COVAR: 677.02 832.99 0.99 0.00 2021-02-13T01:04:23Z- AHRS_M2 msg: 1613178263.44 epoch sec; seqNo:18139 2021-02-13T01:04:23Z- AHRS_M2 msg: 0.55 phi; -0.32 theta; -0.18 psi 2021-02-13T01:04:23Z- handleDepth()-Depth_Keller msg: 1613178263441 epoch sec; seqNo:18283; depth 7.85 2021-02-13T01:04:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178263.47 epoch sec; seqNo:18283; 4060761.53 north; 593630.33 east 2021-02-13T01:04:23Z- AHRS_M2 msg: 1613178263.85 epoch sec; seqNo:18140 2021-02-13T01:04:23Z- AHRS_M2 msg: 0.56 phi; -0.32 theta; -0.18 psi 2021-02-13T01:04:23Z- handleDepth()-Depth_Keller msg: 1613178263857 epoch sec; seqNo:18284; depth 7.94 2021-02-13T01:04:23Z- handleDvl() - RDI_Pathfinder msg: 1613178263.89 epoch sec; seqNo:8878 2021-02-13T01:04:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 103.87 , 80.88 , 118.74 2021-02-13T01:04:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.22 , 0.04 2021-02-13T01:04:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178263.90 epoch sec; seqNo:18284; 4060761.91 north; 593630.32 east 2021-02-13T01:04:24Z- AHRS_M2 msg: 1613178264.25 epoch sec; seqNo:18141 2021-02-13T01:04:24Z- AHRS_M2 msg: 0.57 phi; -0.32 theta; -0.18 psi 2021-02-13T01:04:24Z- handleDepth()-Depth_Keller msg: 1613178264254 epoch sec; seqNo:18285; depth 8.08 2021-02-13T01:04:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178264.27 epoch sec; seqNo:18285; 4060761.91 north; 593630.32 east 2021-02-13T01:04:24Z- AHRS_M2 msg: 1613178264.65 epoch sec; seqNo:18142 2021-02-13T01:04:24Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.17 psi 2021-02-13T01:04:24Z- handleDepth()-Depth_Keller msg: 1613178264652 epoch sec; seqNo:18286; depth 8.17 2021-02-13T01:04:24Z- handleDvl() - RDI_Pathfinder msg: 1613178264.66 epoch sec; seqNo:8879 2021-02-13T01:04:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 106.51 , 0.00 , 118.68 2021-02-13T01:04:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.22 , -0.04 2021-02-13T01:04:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178264.68 epoch sec; seqNo:18286; 4060762.29 north; 593630.32 east 2021-02-13T01:04:25Z- AHRS_M2 msg: 1613178265.05 epoch sec; seqNo:18143 2021-02-13T01:04:25Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.16 psi 2021-02-13T01:04:25Z- handleDepth()-Depth_Keller msg: 1613178265057 epoch sec; seqNo:18287; depth 8.28 2021-02-13T01:04:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178265.07 epoch sec; seqNo:18287; 4060762.68 north; 593631.54 east 2021-02-13T01:04:25Z- AHRS_M2 msg: 1613178265.46 epoch sec; seqNo:18144 2021-02-13T01:04:25Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.14 psi 2021-02-13T01:04:25Z- handleDepth()-Depth_Keller msg: 1613178265462 epoch sec; seqNo:18288; depth 8.40 2021-02-13T01:04:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178265.47 epoch sec; seqNo:18288; 4060762.68 north; 593631.54 east 2021-02-13T01:04:25Z- AHRS_M2 msg: 1613178265.86 epoch sec; seqNo:18145 2021-02-13T01:04:25Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.14 psi 2021-02-13T01:04:25Z- handleDepth()-Depth_Keller msg: 1613178265864 epoch sec; seqNo:18289; depth 8.53 2021-02-13T01:04:25Z- handleDvl() - RDI_Pathfinder msg: 1613178265.89 epoch sec; seqNo:8880 2021-02-13T01:04:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.30 , 103.67 , 79.86 , 117.05 2021-02-13T01:04:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.17 , -0.10 2021-02-13T01:04:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178265.90 epoch sec; seqNo:18289; 4060763.06 north; 593631.53 east 2021-02-13T01:04:26Z- AHRS_M2 msg: 1613178266.26 epoch sec; seqNo:18146 2021-02-13T01:04:26Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.14 psi 2021-02-13T01:04:26Z- handleDepth()-Depth_Keller msg: 1613178266273 epoch sec; seqNo:18290; depth 8.65 2021-02-13T01:04:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178266.28 epoch sec; seqNo:18290; 4060763.44 north; 593631.53 east 2021-02-13T01:04:26Z- AHRS_M2 msg: 1613178266.67 epoch sec; seqNo:18147 2021-02-13T01:04:26Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.14 psi 2021-02-13T01:04:26Z- handleDepth()-Depth_Keller msg: 1613178266673 epoch sec; seqNo:18291; depth 8.76 2021-02-13T01:04:26Z- handleDvl() - RDI_Pathfinder msg: 1613178266.69 epoch sec; seqNo:8881 2021-02-13T01:04:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.84 , 102.72 , 0.00 , 118.95 2021-02-13T01:04:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.07 , -0.16 2021-02-13T01:04:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178266.70 epoch sec; seqNo:18291; 4060763.44 north; 593631.53 east 2021-02-13T01:04:27Z- AHRS_M2 msg: 1613178267.07 epoch sec; seqNo:18148 2021-02-13T01:04:27Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.14 psi 2021-02-13T01:04:27Z- handleDepth()-Depth_Keller msg: 1613178267076 epoch sec; seqNo:18292; depth 8.89 2021-02-13T01:04:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178267.09 epoch sec; seqNo:18292; 4060763.83 north; 593632.75 east 2021-02-13T01:04:27Z- AHRS_M2 msg: 1613178267.48 epoch sec; seqNo:18149 2021-02-13T01:04:27Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.15 psi 2021-02-13T01:04:27Z- handleDepth()-Depth_Keller msg: 1613178267481 epoch sec; seqNo:18293; depth 9.01 2021-02-13T01:04:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178267.49 epoch sec; seqNo:18293; 4060764.21 north; 593632.74 east 2021-02-13T01:04:27Z- AHRS_M2 msg: 1613178267.88 epoch sec; seqNo:18150 2021-02-13T01:04:27Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.17 psi 2021-02-13T01:04:27Z- handleDepth()-Depth_Keller msg: 1613178267887 epoch sec; seqNo:18294; depth 9.15 2021-02-13T01:04:27Z- handleDvl() - RDI_Pathfinder msg: 1613178267.90 epoch sec; seqNo:8882 2021-02-13T01:04:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.44 , 103.80 , 78.48 , 118.68 2021-02-13T01:04:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , -0.16 2021-02-13T01:04:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178267.91 epoch sec; seqNo:18294; 4060764.59 north; 593632.74 east 2021-02-13T01:04:28Z- AHRS_M2 msg: 1613178268.28 epoch sec; seqNo:18151 2021-02-13T01:04:28Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.19 psi 2021-02-13T01:04:28Z- handleDepth()-Depth_Keller msg: 1613178268290 epoch sec; seqNo:18295; depth 9.29 2021-02-13T01:04:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178268.30 epoch sec; seqNo:18295; 4060764.97 north; 593632.73 east 2021-02-13T01:04:28Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 131 calls... 2021-02-13T01:04:28Z- waiting for 1613178266.07, time is 1613178268.30 2021-02-13T01:04:28Z- LcmTrn::updateTrn() >>>> pose time 1613178268.30, meas time 1613178267.90 <<<< 2021-02-13T01:04:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178267.90 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:04:28Z- TerrainNav::motionUpdate() - currEstimate = (4060765, 593632.73, 9.29) 2021-02-13T01:04:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:04:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:04:28Z- TNavBF::Projecting Measurements 2021-02-13T01:04:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:04:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:04:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:04:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:04:28Z- TNavFilter::4 good beams. 2021-02-13T01:04:28Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:04:28Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:04:28Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:04:28Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:04:28Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:04:28Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:04:28Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:04:28Z- TNavBF::mmse selected from filter -1 2021-02-13T01:04:28Z- TerrainMapOctree: (vehN, vehE ) = (4060764.67, 593632.45). 2021-02-13T01:04:28Z- TerrainMapOctree: Min Distance = 785.43. 2021-02-13T01:04:28Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:04:28Z- There is no specific good beam that all particles have in common. 2021-02-13T01:04:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:04:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.24 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 12.23 2021-02-13T01:04:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:04:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 17.24 2021-02-13T01:04:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:04:28Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 9.85 2021-02-13T01:04:28Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:04:28Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 22.53 2021-02-13T01:04:28Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:04:28Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:04:28Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:04:28Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:04:28Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:28Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:04:28Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:04:28Z- Filter 0 No beams used in subcloud update 2021-02-13T01:04:28Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:04:28Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:04:28Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 677.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:04:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178267.90 sec. 2021-02-13T01:04:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:04:28Z- TNavBF::mmse selected from filter -1 2021-02-13T01:04:28Z- variances([0], [2], and psi_berg) = 676.963234 832.987210 0.000000 2021-02-13T01:04:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:04:28Z- LcmTrn::publishEstimates() - MLE : 4060764.47 593649.23 9.29 2021-02-13T01:04:28Z- LcmTrn::publishEstimates() - MMSE : 4060764.97 593632.73 9.29 2021-02-13T01:04:28Z- LcmTrn::publishEstimates() - COVAR: 676.96 832.99 0.97 0.00 2021-02-13T01:04:28Z- handleDepth()-Depth_Keller msg: 1613178268709 epoch sec; seqNo:18296; depth 9.41 2021-02-13T01:04:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178268.73 epoch sec; seqNo:18296; 4060764.97 north; 593632.73 east 2021-02-13T01:04:29Z- AHRS_M2 msg: 1613178269.09 epoch sec; seqNo:18152 2021-02-13T01:04:29Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.17 psi 2021-02-13T01:04:29Z- handleDepth()-Depth_Keller msg: 1613178269105 epoch sec; seqNo:18297; depth 9.55 2021-02-13T01:04:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178269.12 epoch sec; seqNo:18297; 4060765.35 north; 593632.73 east 2021-02-13T01:04:29Z- AHRS_M2 msg: 1613178269.51 epoch sec; seqNo:18153 2021-02-13T01:04:29Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.16 psi 2021-02-13T01:04:29Z- handleDepth()-Depth_Keller msg: 1613178269509 epoch sec; seqNo:18298; depth 9.67 2021-02-13T01:04:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178269.52 epoch sec; seqNo:18298; 4060765.73 north; 593632.72 east 2021-02-13T01:04:29Z- AHRS_M2 msg: 1613178269.90 epoch sec; seqNo:18154 2021-02-13T01:04:29Z- AHRS_M2 msg: 0.60 phi; -0.33 theta; -0.17 psi 2021-02-13T01:04:29Z- handleDepth()-Depth_Keller msg: 1613178269907 epoch sec; seqNo:18299; depth 9.80 2021-02-13T01:04:29Z- handleDvl() - RDI_Pathfinder msg: 1613178269.93 epoch sec; seqNo:8883 2021-02-13T01:04:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.08 , 103.94 , 78.65 , 120.30 2021-02-13T01:04:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , 0.11 2021-02-13T01:04:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178269.94 epoch sec; seqNo:18299; 4060766.12 north; 593633.94 east 2021-02-13T01:04:30Z- AHRS_M2 msg: 1613178270.30 epoch sec; seqNo:18155 2021-02-13T01:04:30Z- AHRS_M2 msg: 0.59 phi; -0.32 theta; -0.16 psi 2021-02-13T01:04:30Z- handleDepth()-Depth_Keller msg: 1613178270313 epoch sec; seqNo:18300; depth 9.93 2021-02-13T01:04:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178270.32 epoch sec; seqNo:18300; 4060766.12 north; 593633.94 east 2021-02-13T01:04:30Z- AHRS_M2 msg: 1613178270.71 epoch sec; seqNo:18156 2021-02-13T01:04:30Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.16 psi 2021-02-13T01:04:30Z- handleDepth()-Depth_Keller msg: 1613178270714 epoch sec; seqNo:18301; depth 10.04 2021-02-13T01:04:30Z- handleDvl() - RDI_Pathfinder msg: 1613178270.73 epoch sec; seqNo:8884 2021-02-13T01:04:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.65 , 78.31 , 116.18 2021-02-13T01:04:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , 0.14 2021-02-13T01:04:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178270.74 epoch sec; seqNo:18301; 4060766.50 north; 593633.94 east 2021-02-13T01:04:31Z- AHRS_M2 msg: 1613178271.11 epoch sec; seqNo:18157 2021-02-13T01:04:31Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.17 psi 2021-02-13T01:04:31Z- handleDepth()-Depth_Keller msg: 1613178271117 epoch sec; seqNo:18302; depth 10.16 2021-02-13T01:04:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178271.13 epoch sec; seqNo:18302; 4060766.88 north; 593633.93 east 2021-02-13T01:04:31Z- AHRS_M2 msg: 1613178271.52 epoch sec; seqNo:18158 2021-02-13T01:04:31Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-13T01:04:31Z- handleDepth()-Depth_Keller msg: 1613178271520 epoch sec; seqNo:18303; depth 10.27 2021-02-13T01:04:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178271.53 epoch sec; seqNo:18303; 4060766.88 north; 593633.93 east 2021-02-13T01:04:31Z- AHRS_M2 msg: 1613178271.92 epoch sec; seqNo:18159 2021-02-13T01:04:31Z- AHRS_M2 msg: 0.57 phi; -0.31 theta; -0.19 psi 2021-02-13T01:04:31Z- handleDepth()-Depth_Keller msg: 1613178271937 epoch sec; seqNo:18304; depth 10.40 2021-02-13T01:04:31Z- handleDvl() - RDI_Pathfinder msg: 1613178271.95 epoch sec; seqNo:8885 2021-02-13T01:04:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.52 , 78.17 , 114.69 2021-02-13T01:04:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.15 , 0.10 2021-02-13T01:04:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178271.96 epoch sec; seqNo:18304; 4060767.26 north; 593633.93 east 2021-02-13T01:04:32Z- AHRS_M2 msg: 1613178272.32 epoch sec; seqNo:18160 2021-02-13T01:04:32Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.19 psi 2021-02-13T01:04:32Z- handleDepth()-Depth_Keller msg: 1613178272328 epoch sec; seqNo:18305; depth 10.51 2021-02-13T01:04:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178272.35 epoch sec; seqNo:18305; 4060767.65 north; 593635.14 east 2021-02-13T01:04:32Z- AHRS_M2 msg: 1613178272.73 epoch sec; seqNo:18161 2021-02-13T01:04:32Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.19 psi 2021-02-13T01:04:32Z- handleDepth()-Depth_Keller msg: 1613178272732 epoch sec; seqNo:18306; depth 10.61 2021-02-13T01:04:32Z- handleDvl() - RDI_Pathfinder msg: 1613178272.74 epoch sec; seqNo:8886 2021-02-13T01:04:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 102.58 , 76.84 , 114.75 2021-02-13T01:04:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.12 , -0.02 2021-02-13T01:04:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178272.77 epoch sec; seqNo:18306; 4060768.03 north; 593635.14 east 2021-02-13T01:04:33Z- AHRS_M2 msg: 1613178273.13 epoch sec; seqNo:18162 2021-02-13T01:04:33Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; -0.19 psi 2021-02-13T01:04:33Z- handleDepth()-Depth_Keller msg: 1613178273139 epoch sec; seqNo:18307; depth 10.74 2021-02-13T01:04:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178273.15 epoch sec; seqNo:18307; 4060768.03 north; 593635.14 east 2021-02-13T01:04:33Z- LcmTrn::handleMessages() - handling 39 messages took 0.62 ms and 127 calls... 2021-02-13T01:04:33Z- waiting for 1613178271.30, time is 1613178273.15 2021-02-13T01:04:33Z- LcmTrn::updateTrn() >>>> pose time 1613178273.15, meas time 1613178272.74 <<<< 2021-02-13T01:04:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178272.74 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:04:33Z- TerrainNav::motionUpdate() - currEstimate = (4060768, 593635.14, 10.74) 2021-02-13T01:04:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:04:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:04:33Z- TNavBF::Projecting Measurements 2021-02-13T01:04:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:04:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:04:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:04:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:04:33Z- TNavFilter::4 good beams. 2021-02-13T01:04:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:04:33Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:04:33Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:04:33Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:04:33Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:04:33Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:04:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:04:33Z- TNavBF::mmse selected from filter -1 2021-02-13T01:04:33Z- TerrainMapOctree: (vehN, vehE ) = (4060767.77, 593634.94). 2021-02-13T01:04:33Z- TerrainMapOctree: Min Distance = 781.57. 2021-02-13T01:04:33Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:04:33Z- There is no specific good beam that all particles have in common. 2021-02-13T01:04:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:04:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.15 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 11.56 2021-02-13T01:04:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:04:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 16.84 2021-02-13T01:04:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:04:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 9.45 2021-02-13T01:04:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:04:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 21.07 2021-02-13T01:04:33Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:04:33Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:04:33Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:04:33Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:04:33Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:04:33Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:04:33Z- Filter 0 No beams used in subcloud update 2021-02-13T01:04:33Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:04:33Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:04:33Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 676.9 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:04:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178272.74 sec. 2021-02-13T01:04:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:04:33Z- TNavBF::mmse selected from filter -1 2021-02-13T01:04:33Z- variances([0], [2], and psi_berg) = 676.910895 832.987188 0.000000 2021-02-13T01:04:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:04:33Z- LcmTrn::publishEstimates() - MLE : 4060767.53 593651.64 10.74 2021-02-13T01:04:33Z- LcmTrn::publishEstimates() - MMSE : 4060768.03 593635.14 10.74 2021-02-13T01:04:33Z- LcmTrn::publishEstimates() - COVAR: 676.91 832.99 0.90 0.00 2021-02-13T01:04:33Z- AHRS_M2 msg: 1613178273.55 epoch sec; seqNo:18163 2021-02-13T01:04:33Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.19 psi 2021-02-13T01:04:33Z- handleDepth()-Depth_Keller msg: 1613178273565 epoch sec; seqNo:18308; depth 10.85 2021-02-13T01:04:33Z- handleDvl() - RDI_Pathfinder msg: 1613178273.61 epoch sec; seqNo:8887 2021-02-13T01:04:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 104.27 , 78.58 , 113.74 2021-02-13T01:04:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.06 , -0.09 2021-02-13T01:04:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178273.62 epoch sec; seqNo:18308; 4060768.41 north; 593635.14 east 2021-02-13T01:04:33Z- AHRS_M2 msg: 1613178273.94 epoch sec; seqNo:18164 2021-02-13T01:04:33Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.19 psi 2021-02-13T01:04:33Z- handleDepth()-Depth_Keller msg: 1613178273944 epoch sec; seqNo:18309; depth 10.98 2021-02-13T01:04:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178273.97 epoch sec; seqNo:18309; 4060768.78 north; 593635.13 east 2021-02-13T01:04:34Z- AHRS_M2 msg: 1613178274.34 epoch sec; seqNo:18165 2021-02-13T01:04:34Z- AHRS_M2 msg: 0.59 phi; -0.30 theta; -0.19 psi 2021-02-13T01:04:34Z- handleDepth()-Depth_Keller msg: 1613178274348 epoch sec; seqNo:18310; depth 11.08 2021-02-13T01:04:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178274.36 epoch sec; seqNo:18310; 4060769.18 north; 593636.35 east 2021-02-13T01:04:34Z- AHRS_M2 msg: 1613178274.75 epoch sec; seqNo:18166 2021-02-13T01:04:34Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.18 psi 2021-02-13T01:04:34Z- handleDepth()-Depth_Keller msg: 1613178274760 epoch sec; seqNo:18311; depth 11.18 2021-02-13T01:04:34Z- handleDvl() - RDI_Pathfinder msg: 1613178274.78 epoch sec; seqNo:8888 2021-02-13T01:04:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.52 , 78.17 , 111.98 2021-02-13T01:04:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , -0.05 2021-02-13T01:04:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178274.79 epoch sec; seqNo:18311; 4060769.18 north; 593636.35 east 2021-02-13T01:04:35Z- AHRS_M2 msg: 1613178275.15 epoch sec; seqNo:18167 2021-02-13T01:04:35Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.18 psi 2021-02-13T01:04:35Z- handleDepth()-Depth_Keller msg: 1613178275163 epoch sec; seqNo:18312; depth 11.30 2021-02-13T01:04:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178275.17 epoch sec; seqNo:18312; 4060769.56 north; 593636.34 east 2021-02-13T01:04:35Z- AHRS_M2 msg: 1613178275.56 epoch sec; seqNo:18168 2021-02-13T01:04:35Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.17 psi 2021-02-13T01:04:35Z- handleDepth()-Depth_Keller msg: 1613178275560 epoch sec; seqNo:18313; depth 11.40 2021-02-13T01:04:35Z- handleDvl() - RDI_Pathfinder msg: 1613178275.57 epoch sec; seqNo:8889 2021-02-13T01:04:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 101.30 , 75.61 , 110.77 2021-02-13T01:04:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.06 2021-02-13T01:04:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178275.58 epoch sec; seqNo:18313; 4060769.93 north; 593636.34 east 2021-02-13T01:04:35Z- AHRS_M2 msg: 1613178275.96 epoch sec; seqNo:18169 2021-02-13T01:04:35Z- AHRS_M2 msg: 0.58 phi; -0.29 theta; -0.17 psi 2021-02-13T01:04:35Z- handleDepth()-Depth_Keller msg: 1613178275970 epoch sec; seqNo:18314; depth 11.51 2021-02-13T01:04:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178275.98 epoch sec; seqNo:18314; 4060770.31 north; 593636.34 east 2021-02-13T01:04:36Z- AHRS_M2 msg: 1613178276.36 epoch sec; seqNo:18170 2021-02-13T01:04:36Z- AHRS_M2 msg: 0.58 phi; -0.29 theta; -0.16 psi 2021-02-13T01:04:36Z- handleDepth()-Depth_Keller msg: 1613178276368 epoch sec; seqNo:18315; depth 11.63 2021-02-13T01:04:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178276.38 epoch sec; seqNo:18315; 4060770.31 north; 593636.34 east 2021-02-13T01:04:36Z- AHRS_M2 msg: 1613178276.77 epoch sec; seqNo:18171 2021-02-13T01:04:36Z- AHRS_M2 msg: 0.58 phi; -0.29 theta; -0.16 psi 2021-02-13T01:04:36Z- handleDepth()-Depth_Keller msg: 1613178276772 epoch sec; seqNo:18316; depth 11.72 2021-02-13T01:04:36Z- handleDvl() - RDI_Pathfinder msg: 1613178276.78 epoch sec; seqNo:8890 2021-02-13T01:04:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.85 , 100.08 , 0.00 , 109.55 2021-02-13T01:04:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.09 , 0.09 2021-02-13T01:04:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178276.81 epoch sec; seqNo:18316; 4060770.69 north; 593636.33 east 2021-02-13T01:04:37Z- AHRS_M2 msg: 1613178277.17 epoch sec; seqNo:18172 2021-02-13T01:04:37Z- AHRS_M2 msg: 0.58 phi; -0.29 theta; -0.16 psi 2021-02-13T01:04:37Z- handleDepth()-Depth_Keller msg: 1613178277177 epoch sec; seqNo:18317; depth 11.83 2021-02-13T01:04:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178277.19 epoch sec; seqNo:18317; 4060771.08 north; 593637.55 east 2021-02-13T01:04:37Z- AHRS_M2 msg: 1613178277.58 epoch sec; seqNo:18173 2021-02-13T01:04:37Z- AHRS_M2 msg: 0.58 phi; -0.29 theta; -0.16 psi 2021-02-13T01:04:37Z- handleDepth()-Depth_Keller msg: 1613178277582 epoch sec; seqNo:18318; depth 11.94 2021-02-13T01:04:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178277.59 epoch sec; seqNo:18318; 4060771.08 north; 593637.55 east 2021-02-13T01:04:37Z- AHRS_M2 msg: 1613178277.98 epoch sec; seqNo:18174 2021-02-13T01:04:37Z- AHRS_M2 msg: 0.58 phi; -0.29 theta; -0.16 psi 2021-02-13T01:04:37Z- handleDepth()-Depth_Keller msg: 1613178277984 epoch sec; seqNo:18319; depth 12.04 2021-02-13T01:04:38Z- handleDvl() - RDI_Pathfinder msg: 1613178278.00 epoch sec; seqNo:8891 2021-02-13T01:04:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.72 , 98.59 , 78.31 , 108.06 2021-02-13T01:04:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.23 , 0.02 2021-02-13T01:04:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178278.01 epoch sec; seqNo:18319; 4060771.46 north; 593637.54 east 2021-02-13T01:04:38Z- AHRS_M2 msg: 1613178278.38 epoch sec; seqNo:18175 2021-02-13T01:04:38Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.16 psi 2021-02-13T01:04:38Z- handleDepth()-Depth_Keller msg: 1613178278393 epoch sec; seqNo:18320; depth 12.14 2021-02-13T01:04:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178278.40 epoch sec; seqNo:18320; 4060771.84 north; 593637.54 east 2021-02-13T01:04:38Z- LcmTrn::handleMessages() - handling 44 messages took 0.61 ms and 132 calls... 2021-02-13T01:04:38Z- waiting for 1613178276.15, time is 1613178278.40 2021-02-13T01:04:38Z- LcmTrn::updateTrn() >>>> pose time 1613178278.40, meas time 1613178278.00 <<<< 2021-02-13T01:04:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178278.00 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:04:38Z- TerrainNav::motionUpdate() - currEstimate = (4060772, 593637.54, 12.14) 2021-02-13T01:04:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:04:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:04:38Z- TNavBF::Projecting Measurements 2021-02-13T01:04:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:04:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:04:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:04:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:04:38Z- TNavFilter::4 good beams. 2021-02-13T01:04:38Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:04:38Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:04:38Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:04:38Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:04:38Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:04:38Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:04:38Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:04:38Z- TNavBF::mmse selected from filter -1 2021-02-13T01:04:38Z- TerrainMapOctree: (vehN, vehE ) = (4060771.55, 593637.36). 2021-02-13T01:04:38Z- TerrainMapOctree: Min Distance = 777.33. 2021-02-13T01:04:38Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:04:38Z- There is no specific good beam that all particles have in common. 2021-02-13T01:04:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:04:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 11.21 2021-02-13T01:04:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:04:38Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 15.55 2021-02-13T01:04:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:04:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 9.81 2021-02-13T01:04:38Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:04:38Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 18.68 2021-02-13T01:04:38Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:04:38Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:04:38Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:04:38Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:04:38Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:38Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:04:38Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:04:38Z- Filter 0 No beams used in subcloud update 2021-02-13T01:04:38Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:04:38Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:04:38Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 676.9 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:04:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178278.00 sec. 2021-02-13T01:04:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:04:38Z- TNavBF::mmse selected from filter -1 2021-02-13T01:04:38Z- variances([0], [2], and psi_berg) = 676.855044 832.987165 0.000000 2021-02-13T01:04:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:04:38Z- LcmTrn::publishEstimates() - MLE : 4060771.34 593654.04 12.14 2021-02-13T01:04:38Z- LcmTrn::publishEstimates() - MMSE : 4060771.84 593637.54 12.14 2021-02-13T01:04:38Z- LcmTrn::publishEstimates() - COVAR: 676.86 832.99 0.87 0.00 2021-02-13T01:04:38Z- AHRS_M2 msg: 1613178278.79 epoch sec; seqNo:18176 2021-02-13T01:04:38Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.16 psi 2021-02-13T01:04:38Z- handleDepth()-Depth_Keller msg: 1613178278794 epoch sec; seqNo:18321; depth 12.24 2021-02-13T01:04:38Z- handleDvl() - RDI_Pathfinder msg: 1613178278.81 epoch sec; seqNo:8892 2021-02-13T01:04:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 99.07 , 76.84 , 112.59 2021-02-13T01:04:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.20 , -0.12 2021-02-13T01:04:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178278.82 epoch sec; seqNo:18321; 4060771.84 north; 593637.54 east 2021-02-13T01:04:39Z- AHRS_M2 msg: 1613178279.19 epoch sec; seqNo:18177 2021-02-13T01:04:39Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.17 psi 2021-02-13T01:04:39Z- handleDepth()-Depth_Keller msg: 1613178279204 epoch sec; seqNo:18322; depth 12.34 2021-02-13T01:04:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178279.22 epoch sec; seqNo:18322; 4060772.23 north; 593638.76 east 2021-02-13T01:04:39Z- AHRS_M2 msg: 1613178279.60 epoch sec; seqNo:18178 2021-02-13T01:04:39Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.17 psi 2021-02-13T01:04:39Z- handleDepth()-Depth_Keller msg: 1613178279600 epoch sec; seqNo:18323; depth 12.47 2021-02-13T01:04:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178279.61 epoch sec; seqNo:18323; 4060772.61 north; 593638.75 east 2021-02-13T01:04:40Z- AHRS_M2 msg: 1613178280.00 epoch sec; seqNo:18179 2021-02-13T01:04:40Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.17 psi 2021-02-13T01:04:40Z- handleDepth()-Depth_Keller msg: 1613178280006 epoch sec; seqNo:18324; depth 12.55 2021-02-13T01:04:40Z- handleDvl() - RDI_Pathfinder msg: 1613178280.02 epoch sec; seqNo:8893 2021-02-13T01:04:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.50 , 98.73 , 78.44 , 109.55 2021-02-13T01:04:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.06 , -0.19 2021-02-13T01:04:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178280.03 epoch sec; seqNo:18324; 4060772.99 north; 593638.75 east 2021-02-13T01:04:40Z- AHRS_M2 msg: 1613178280.41 epoch sec; seqNo:18180 2021-02-13T01:04:40Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.18 psi 2021-02-13T01:04:40Z- handleDepth()-Depth_Keller msg: 1613178280424 epoch sec; seqNo:18325; depth 12.67 2021-02-13T01:04:40Z- handleDvl() - RDI_Pathfinder msg: 1613178280.43 epoch sec; seqNo:8894 2021-02-13T01:04:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:04:40Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:04:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178280.44 epoch sec; seqNo:18325; 4060772.99 north; 593638.75 east 2021-02-13T01:04:40Z- AHRS_M2 msg: 1613178280.81 epoch sec; seqNo:18181 2021-02-13T01:04:40Z- AHRS_M2 msg: 0.62 phi; -0.29 theta; -0.18 psi 2021-02-13T01:04:40Z- handleDepth()-Depth_Keller msg: 1613178280815 epoch sec; seqNo:18326; depth 12.78 2021-02-13T01:04:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178280.83 epoch sec; seqNo:18326; 4060773.37 north; 593638.74 east 2021-02-13T01:04:41Z- AHRS_M2 msg: 1613178281.22 epoch sec; seqNo:18182 2021-02-13T01:04:41Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.17 psi 2021-02-13T01:04:41Z- handleDepth()-Depth_Keller msg: 1613178281223 epoch sec; seqNo:18327; depth 12.88 2021-02-13T01:04:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178281.23 epoch sec; seqNo:18327; 4060773.76 north; 593639.96 east 2021-02-13T01:04:41Z- AHRS_M2 msg: 1613178281.62 epoch sec; seqNo:18183 2021-02-13T01:04:41Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.17 psi 2021-02-13T01:04:41Z- handleDepth()-Depth_Keller msg: 1613178281626 epoch sec; seqNo:18328; depth 12.99 2021-02-13T01:04:41Z- handleDvl() - RDI_Pathfinder msg: 1613178281.64 epoch sec; seqNo:8895 2021-02-13T01:04:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 99.07 , 78.78 , 107.18 2021-02-13T01:04:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.18 , 0.05 2021-02-13T01:04:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178281.65 epoch sec; seqNo:18328; 4060774.14 north; 593639.96 east 2021-02-13T01:04:42Z- AHRS_M2 msg: 1613178282.02 epoch sec; seqNo:18184 2021-02-13T01:04:42Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.17 psi 2021-02-13T01:04:42Z- handleDepth()-Depth_Keller msg: 1613178282030 epoch sec; seqNo:18329; depth 13.10 2021-02-13T01:04:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178282.04 epoch sec; seqNo:18329; 4060774.52 north; 593639.95 east 2021-02-13T01:04:42Z- AHRS_M2 msg: 1613178282.42 epoch sec; seqNo:18185 2021-02-13T01:04:42Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.16 psi 2021-02-13T01:04:42Z- handleDepth()-Depth_Keller msg: 1613178282428 epoch sec; seqNo:18330; depth 13.21 2021-02-13T01:04:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178282.44 epoch sec; seqNo:18330; 4060774.90 north; 593639.95 east 2021-02-13T01:04:42Z- AHRS_M2 msg: 1613178282.83 epoch sec; seqNo:18186 2021-02-13T01:04:42Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.16 psi 2021-02-13T01:04:42Z- handleDepth()-Depth_Keller msg: 1613178282832 epoch sec; seqNo:18331; depth 13.31 2021-02-13T01:04:42Z- handleDvl() - RDI_Pathfinder msg: 1613178282.84 epoch sec; seqNo:8896 2021-02-13T01:04:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.57 , 98.80 , 78.51 , 110.97 2021-02-13T01:04:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.11 , 0.21 2021-02-13T01:04:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178282.85 epoch sec; seqNo:18331; 4060774.90 north; 593639.95 east 2021-02-13T01:04:43Z- AHRS_M2 msg: 1613178283.23 epoch sec; seqNo:18187 2021-02-13T01:04:43Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.16 psi 2021-02-13T01:04:43Z- handleDepth()-Depth_Keller msg: 1613178283241 epoch sec; seqNo:18332; depth 13.42 2021-02-13T01:04:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178283.25 epoch sec; seqNo:18332; 4060775.28 north; 593639.94 east 2021-02-13T01:04:43Z- AHRS_M2 msg: 1613178283.65 epoch sec; seqNo:18188 2021-02-13T01:04:43Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.16 psi 2021-02-13T01:04:43Z- handleDepth()-Depth_Keller msg: 1613178283653 epoch sec; seqNo:18333; depth 13.54 2021-02-13T01:04:43Z- handleDvl() - RDI_Pathfinder msg: 1613178283.66 epoch sec; seqNo:8897 2021-02-13T01:04:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 97.65 , 78.71 , 109.82 2021-02-13T01:04:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.06 , 0.20 2021-02-13T01:04:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178283.67 epoch sec; seqNo:18333; 4060775.66 north; 593639.94 east 2021-02-13T01:04:43Z- LcmTrn::handleMessages() - handling 45 messages took 0.68 ms and 132 calls... 2021-02-13T01:04:43Z- waiting for 1613178281.40, time is 1613178283.67 2021-02-13T01:04:43Z- LcmTrn::updateTrn() equating pose 1613178283.67 and meas times 1613178283.67 2021-02-13T01:04:43Z- LcmTrn::updateTrn() >>>> pose time 1613178283.67, meas time 1613178283.67 <<<< 2021-02-13T01:04:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178283.67 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:04:43Z- TerrainNav::motionUpdate() - currEstimate = (4060776, 593639.94, 13.54) 2021-02-13T01:04:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:04:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:04:43Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178283.67 sec.; No relevant navigation data available 2021-02-13T01:04:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:04:43Z- TNavBF::mmse selected from filter -1 2021-02-13T01:04:43Z- variances([0], [2], and psi_berg) = 676.809939 832.987147 0.000000 2021-02-13T01:04:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:04:43Z- LcmTrn::publishEstimates() - MLE : 4060775.16 593656.44 13.54 2021-02-13T01:04:43Z- LcmTrn::publishEstimates() - MMSE : 4060775.66 593639.94 13.54 2021-02-13T01:04:43Z- LcmTrn::publishEstimates() - COVAR: 676.81 832.99 1.10 0.00 2021-02-13T01:04:44Z- AHRS_M2 msg: 1613178284.04 epoch sec; seqNo:18189 2021-02-13T01:04:44Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.16 psi 2021-02-13T01:04:44Z- handleDepth()-Depth_Keller msg: 1613178284049 epoch sec; seqNo:18334; depth 13.65 2021-02-13T01:04:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178284.07 epoch sec; seqNo:18334; 4060775.67 north; 593641.16 east 2021-02-13T01:04:44Z- AHRS_M2 msg: 1613178284.44 epoch sec; seqNo:18190 2021-02-13T01:04:44Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.16 psi 2021-02-13T01:04:44Z- handleDepth()-Depth_Keller msg: 1613178284453 epoch sec; seqNo:18335; depth 13.76 2021-02-13T01:04:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178284.46 epoch sec; seqNo:18335; 4060776.05 north; 593641.16 east 2021-02-13T01:04:44Z- AHRS_M2 msg: 1613178284.85 epoch sec; seqNo:18191 2021-02-13T01:04:44Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.16 psi 2021-02-13T01:04:44Z- handleDepth()-Depth_Keller msg: 1613178284852 epoch sec; seqNo:18336; depth 13.87 2021-02-13T01:04:44Z- handleDvl() - RDI_Pathfinder msg: 1613178284.86 epoch sec; seqNo:8898 2021-02-13T01:04:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.49 , 97.71 , 78.78 , 107.18 2021-02-13T01:04:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.18 , 0.06 2021-02-13T01:04:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178284.87 epoch sec; seqNo:18336; 4060776.43 north; 593641.15 east 2021-02-13T01:04:45Z- AHRS_M2 msg: 1613178285.25 epoch sec; seqNo:18192 2021-02-13T01:04:45Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.17 psi 2021-02-13T01:04:45Z- handleDepth()-Depth_Keller msg: 1613178285261 epoch sec; seqNo:18337; depth 13.99 2021-02-13T01:04:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178285.27 epoch sec; seqNo:18337; 4060776.43 north; 593641.15 east 2021-02-13T01:04:45Z- AHRS_M2 msg: 1613178285.67 epoch sec; seqNo:18193 2021-02-13T01:04:45Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.17 psi 2021-02-13T01:04:45Z- handleDepth()-Depth_Keller msg: 1613178285676 epoch sec; seqNo:18338; depth 14.10 2021-02-13T01:04:45Z- handleDvl() - RDI_Pathfinder msg: 1613178285.69 epoch sec; seqNo:8899 2021-02-13T01:04:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.82 , 97.65 , 78.17 , 109.82 2021-02-13T01:04:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.16 , -0.11 2021-02-13T01:04:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178285.70 epoch sec; seqNo:18338; 4060776.81 north; 593641.15 east 2021-02-13T01:04:46Z- AHRS_M2 msg: 1613178286.06 epoch sec; seqNo:18194 2021-02-13T01:04:46Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.18 psi 2021-02-13T01:04:46Z- handleDepth()-Depth_Keller msg: 1613178286066 epoch sec; seqNo:18339; depth 14.21 2021-02-13T01:04:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178286.08 epoch sec; seqNo:18339; 4060777.20 north; 593642.36 east 2021-02-13T01:04:46Z- AHRS_M2 msg: 1613178286.46 epoch sec; seqNo:18195 2021-02-13T01:04:46Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.19 psi 2021-02-13T01:04:46Z- handleDepth()-Depth_Keller msg: 1613178286468 epoch sec; seqNo:18340; depth 14.32 2021-02-13T01:04:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178286.48 epoch sec; seqNo:18340; 4060777.20 north; 593642.36 east 2021-02-13T01:04:46Z- AHRS_M2 msg: 1613178286.87 epoch sec; seqNo:18196 2021-02-13T01:04:46Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.19 psi 2021-02-13T01:04:46Z- handleDepth()-Depth_Keller msg: 1613178286881 epoch sec; seqNo:18341; depth 14.44 2021-02-13T01:04:46Z- handleDvl() - RDI_Pathfinder msg: 1613178286.89 epoch sec; seqNo:8900 2021-02-13T01:04:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.71 , 78.78 , 105.83 2021-02-13T01:04:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.06 , -0.17 2021-02-13T01:04:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178286.90 epoch sec; seqNo:18341; 4060777.58 north; 593642.36 east 2021-02-13T01:04:47Z- AHRS_M2 msg: 1613178287.27 epoch sec; seqNo:18197 2021-02-13T01:04:47Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.19 psi 2021-02-13T01:04:47Z- handleDepth()-Depth_Keller msg: 1613178287281 epoch sec; seqNo:18342; depth 14.53 2021-02-13T01:04:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178287.29 epoch sec; seqNo:18342; 4060777.96 north; 593642.36 east 2021-02-13T01:04:47Z- AHRS_M2 msg: 1613178287.68 epoch sec; seqNo:18198 2021-02-13T01:04:47Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.19 psi 2021-02-13T01:04:47Z- handleDepth()-Depth_Keller msg: 1613178287680 epoch sec; seqNo:18343; depth 14.64 2021-02-13T01:04:47Z- handleDvl() - RDI_Pathfinder msg: 1613178287.70 epoch sec; seqNo:8901 2021-02-13T01:04:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.93 , 100.22 , 78.58 , 106.98 2021-02-13T01:04:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , -0.10 2021-02-13T01:04:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178287.73 epoch sec; seqNo:18343; 4060778.34 north; 593642.35 east 2021-02-13T01:04:48Z- AHRS_M2 msg: 1613178288.08 epoch sec; seqNo:18199 2021-02-13T01:04:48Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.19 psi 2021-02-13T01:04:48Z- handleDepth()-Depth_Keller msg: 1613178288085 epoch sec; seqNo:18344; depth 14.77 2021-02-13T01:04:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178288.10 epoch sec; seqNo:18344; 4060778.73 north; 593643.57 east 2021-02-13T01:04:48Z- AHRS_M2 msg: 1613178288.48 epoch sec; seqNo:18200 2021-02-13T01:04:48Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.20 psi 2021-02-13T01:04:48Z- handleDepth()-Depth_Keller msg: 1613178288488 epoch sec; seqNo:18345; depth 14.88 2021-02-13T01:04:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178288.50 epoch sec; seqNo:18345; 4060778.73 north; 593643.57 east 2021-02-13T01:04:48Z- LcmTrn::handleMessages() - handling 40 messages took 0.61 ms and 128 calls... 2021-02-13T01:04:48Z- waiting for 1613178286.67, time is 1613178288.50 2021-02-13T01:04:48Z- LcmTrn::updateTrn() >>>> pose time 1613178288.50, meas time 1613178287.70 <<<< 2021-02-13T01:04:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178287.70 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:04:48Z- TerrainNav::motionUpdate() - currEstimate = (4060779, 593643.57, 14.88) 2021-02-13T01:04:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:04:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:04:48Z- TNavBF::Projecting Measurements 2021-02-13T01:04:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:04:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:04:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:04:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:04:48Z- TNavFilter::4 good beams. 2021-02-13T01:04:48Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:04:48Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:04:48Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:04:48Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:04:48Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:04:48Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:04:48Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:04:48Z- TNavBF::mmse selected from filter -1 2021-02-13T01:04:48Z- TerrainMapOctree: (vehN, vehE ) = (4060778.22, 593642.97). 2021-02-13T01:04:48Z- TerrainMapOctree: Min Distance = 768.81. 2021-02-13T01:04:48Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:04:48Z- There is no specific good beam that all particles have in common. 2021-02-13T01:04:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:04:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 10.22 2021-02-13T01:04:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:04:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 16.07 2021-02-13T01:04:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:04:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.087 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-13T01:04:48Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:04:48Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 18.31 2021-02-13T01:04:48Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:04:48Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:04:48Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:04:48Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:04:48Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:48Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:04:48Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:04:48Z- Filter 0 No beams used in subcloud update 2021-02-13T01:04:48Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:04:48Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:04:48Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 676.8 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T01:04:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178287.70 sec. 2021-02-13T01:04:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:04:48Z- TNavBF::mmse selected from filter -1 2021-02-13T01:04:48Z- variances([0], [2], and psi_berg) = 676.748818 832.987122 0.000000 2021-02-13T01:04:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:04:48Z- LcmTrn::publishEstimates() - MLE : 4060778.23 593660.07 14.88 2021-02-13T01:04:48Z- LcmTrn::publishEstimates() - MMSE : 4060778.73 593643.57 14.88 2021-02-13T01:04:48Z- LcmTrn::publishEstimates() - COVAR: 676.75 832.99 1.00 0.00 2021-02-13T01:04:48Z- AHRS_M2 msg: 1613178288.89 epoch sec; seqNo:18201 2021-02-13T01:04:48Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.19 psi 2021-02-13T01:04:48Z- handleDepth()-Depth_Keller msg: 1613178288897 epoch sec; seqNo:18346; depth 14.99 2021-02-13T01:04:48Z- handleDvl() - RDI_Pathfinder msg: 1613178288.91 epoch sec; seqNo:8902 2021-02-13T01:04:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.86 , 78.17 , 108.60 2021-02-13T01:04:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , 0.03 2021-02-13T01:04:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178288.92 epoch sec; seqNo:18346; 4060779.11 north; 593643.56 east 2021-02-13T01:04:49Z- AHRS_M2 msg: 1613178289.29 epoch sec; seqNo:18202 2021-02-13T01:04:49Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.18 psi 2021-02-13T01:04:49Z- handleDepth()-Depth_Keller msg: 1613178289301 epoch sec; seqNo:18347; depth 15.11 2021-02-13T01:04:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178289.31 epoch sec; seqNo:18347; 4060779.49 north; 593643.56 east 2021-02-13T01:04:49Z- AHRS_M2 msg: 1613178289.70 epoch sec; seqNo:18203 2021-02-13T01:04:49Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.18 psi 2021-02-13T01:04:49Z- handleDepth()-Depth_Keller msg: 1613178289704 epoch sec; seqNo:18348; depth 15.20 2021-02-13T01:04:49Z- handleDvl() - RDI_Pathfinder msg: 1613178289.72 epoch sec; seqNo:8903 2021-02-13T01:04:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.86 , 97.44 , 71.94 , 106.91 2021-02-13T01:04:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.11 2021-02-13T01:04:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178289.73 epoch sec; seqNo:18348; 4060779.86 north; 593643.56 east 2021-02-13T01:04:50Z- AHRS_M2 msg: 1613178290.10 epoch sec; seqNo:18204 2021-02-13T01:04:50Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.18 psi 2021-02-13T01:04:50Z- handleDepth()-Depth_Keller msg: 1613178290105 epoch sec; seqNo:18349; depth 15.32 2021-02-13T01:04:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178290.12 epoch sec; seqNo:18349; 4060779.86 north; 593643.56 east 2021-02-13T01:04:50Z- AHRS_M2 msg: 1613178290.50 epoch sec; seqNo:18205 2021-02-13T01:04:50Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.18 psi 2021-02-13T01:04:50Z- handleDepth()-Depth_Keller msg: 1613178290513 epoch sec; seqNo:18350; depth 15.42 2021-02-13T01:04:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178290.52 epoch sec; seqNo:18350; 4060780.24 north; 593643.55 east 2021-02-13T01:04:50Z- AHRS_M2 msg: 1613178290.91 epoch sec; seqNo:18206 2021-02-13T01:04:50Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.18 psi 2021-02-13T01:04:50Z- handleDepth()-Depth_Keller msg: 1613178290914 epoch sec; seqNo:18351; depth 15.54 2021-02-13T01:04:50Z- handleDvl() - RDI_Pathfinder msg: 1613178290.92 epoch sec; seqNo:8904 2021-02-13T01:04:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.85 , 78.38 , 105.15 2021-02-13T01:04:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.07 , 0.13 2021-02-13T01:04:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178290.93 epoch sec; seqNo:18351; 4060780.64 north; 593644.77 east 2021-02-13T01:04:51Z- AHRS_M2 msg: 1613178291.31 epoch sec; seqNo:18207 2021-02-13T01:04:51Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.18 psi 2021-02-13T01:04:51Z- handleDepth()-Depth_Keller msg: 1613178291316 epoch sec; seqNo:18352; depth 15.66 2021-02-13T01:04:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178291.34 epoch sec; seqNo:18352; 4060780.64 north; 593644.77 east 2021-02-13T01:04:51Z- AHRS_M2 msg: 1613178291.72 epoch sec; seqNo:18208 2021-02-13T01:04:51Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.17 psi 2021-02-13T01:04:51Z- handleDepth()-Depth_Keller msg: 1613178291723 epoch sec; seqNo:18353; depth 15.77 2021-02-13T01:04:51Z- handleDvl() - RDI_Pathfinder msg: 1613178291.73 epoch sec; seqNo:8905 2021-02-13T01:04:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.00 , 97.58 , 78.65 , 107.05 2021-02-13T01:04:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.12 , 0.07 2021-02-13T01:04:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178291.76 epoch sec; seqNo:18353; 4060781.01 north; 593644.76 east 2021-02-13T01:04:52Z- AHRS_M2 msg: 1613178292.12 epoch sec; seqNo:18209 2021-02-13T01:04:52Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.17 psi 2021-02-13T01:04:52Z- handleDepth()-Depth_Keller msg: 1613178292126 epoch sec; seqNo:18354; depth 15.89 2021-02-13T01:04:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178292.14 epoch sec; seqNo:18354; 4060781.39 north; 593644.76 east 2021-02-13T01:04:52Z- AHRS_M2 msg: 1613178292.53 epoch sec; seqNo:18210 2021-02-13T01:04:52Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.17 psi 2021-02-13T01:04:52Z- handleDepth()-Depth_Keller msg: 1613178292533 epoch sec; seqNo:18355; depth 15.98 2021-02-13T01:04:52Z- handleDvl() - RDI_Pathfinder msg: 1613178292.55 epoch sec; seqNo:8906 2021-02-13T01:04:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.07 , 97.71 , 0.00 , 106.24 2021-02-13T01:04:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.14 , -0.02 2021-02-13T01:04:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178292.56 epoch sec; seqNo:18355; 4060781.77 north; 593644.75 east 2021-02-13T01:04:52Z- AHRS_M2 msg: 1613178292.93 epoch sec; seqNo:18211 2021-02-13T01:04:52Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.17 psi 2021-02-13T01:04:52Z- handleDepth()-Depth_Keller msg: 1613178292937 epoch sec; seqNo:18356; depth 16.11 2021-02-13T01:04:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178292.95 epoch sec; seqNo:18356; 4060781.79 north; 593645.98 east 2021-02-13T01:04:53Z- AHRS_M2 msg: 1613178293.33 epoch sec; seqNo:18212 2021-02-13T01:04:53Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.18 psi 2021-02-13T01:04:53Z- handleDepth()-Depth_Keller msg: 1613178293341 epoch sec; seqNo:18357; depth 16.21 2021-02-13T01:04:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178293.35 epoch sec; seqNo:18357; 4060782.16 north; 593645.97 east 2021-02-13T01:04:53Z- AHRS_M2 msg: 1613178293.74 epoch sec; seqNo:18213 2021-02-13T01:04:53Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.20 psi 2021-02-13T01:04:53Z- handleDepth()-Depth_Keller msg: 1613178293742 epoch sec; seqNo:18358; depth 16.34 2021-02-13T01:04:53Z- handleDvl() - RDI_Pathfinder msg: 1613178293.75 epoch sec; seqNo:8907 2021-02-13T01:04:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.48 , 96.23 , 71.94 , 104.34 2021-02-13T01:04:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.09 , -0.09 2021-02-13T01:04:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178293.76 epoch sec; seqNo:18358; 4060782.54 north; 593645.97 east 2021-02-13T01:04:53Z- LcmTrn::handleMessages() - handling 45 messages took 0.67 ms and 132 calls... 2021-02-13T01:04:53Z- waiting for 1613178291.50, time is 1613178293.76 2021-02-13T01:04:53Z- LcmTrn::updateTrn() equating pose 1613178293.76 and meas times 1613178293.76 2021-02-13T01:04:53Z- LcmTrn::updateTrn() >>>> pose time 1613178293.76, meas time 1613178293.76 <<<< 2021-02-13T01:04:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178293.76 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:04:53Z- TerrainNav::motionUpdate() - currEstimate = (4060783, 593645.97, 16.34) 2021-02-13T01:04:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:04:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:04:53Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178293.76 sec.; No relevant navigation data available 2021-02-13T01:04:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:04:53Z- TNavBF::mmse selected from filter -1 2021-02-13T01:04:53Z- variances([0], [2], and psi_berg) = 676.703762 832.987103 0.000000 2021-02-13T01:04:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:04:53Z- LcmTrn::publishEstimates() - MLE : 4060782.04 593661.47 16.34 2021-02-13T01:04:53Z- LcmTrn::publishEstimates() - MMSE : 4060782.54 593645.97 16.34 2021-02-13T01:04:53Z- LcmTrn::publishEstimates() - COVAR: 676.70 832.99 1.23 0.00 2021-02-13T01:04:54Z- AHRS_M2 msg: 1613178294.14 epoch sec; seqNo:18214 2021-02-13T01:04:54Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.21 psi 2021-02-13T01:04:54Z- handleDepth()-Depth_Keller msg: 1613178294147 epoch sec; seqNo:18359; depth 16.45 2021-02-13T01:04:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178294.16 epoch sec; seqNo:18359; 4060782.92 north; 593645.96 east 2021-02-13T01:04:54Z- AHRS_M2 msg: 1613178294.54 epoch sec; seqNo:18215 2021-02-13T01:04:54Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.21 psi 2021-02-13T01:04:54Z- handleDepth()-Depth_Keller msg: 1613178294548 epoch sec; seqNo:18360; depth 16.55 2021-02-13T01:04:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178294.56 epoch sec; seqNo:18360; 4060782.92 north; 593645.96 east 2021-02-13T01:04:54Z- AHRS_M2 msg: 1613178294.95 epoch sec; seqNo:18216 2021-02-13T01:04:54Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.21 psi 2021-02-13T01:04:54Z- handleDepth()-Depth_Keller msg: 1613178294965 epoch sec; seqNo:18361; depth 16.66 2021-02-13T01:04:54Z- handleDvl() - RDI_Pathfinder msg: 1613178294.98 epoch sec; seqNo:8908 2021-02-13T01:04:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 99.20 , 78.51 , 105.29 2021-02-13T01:04:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , -0.10 2021-02-13T01:04:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178294.99 epoch sec; seqNo:18361; 4060783.30 north; 593645.96 east 2021-02-13T01:04:55Z- AHRS_M2 msg: 1613178295.35 epoch sec; seqNo:18217 2021-02-13T01:04:55Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.21 psi 2021-02-13T01:04:55Z- handleDepth()-Depth_Keller msg: 1613178295366 epoch sec; seqNo:18362; depth 16.78 2021-02-13T01:04:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178295.38 epoch sec; seqNo:18362; 4060783.69 north; 593647.18 east 2021-02-13T01:04:55Z- AHRS_M2 msg: 1613178295.76 epoch sec; seqNo:18218 2021-02-13T01:04:55Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.21 psi 2021-02-13T01:04:55Z- handleDepth()-Depth_Keller msg: 1613178295760 epoch sec; seqNo:18363; depth 16.90 2021-02-13T01:04:55Z- handleDvl() - RDI_Pathfinder msg: 1613178295.77 epoch sec; seqNo:8909 2021-02-13T01:04:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.24 , 98.93 , 70.30 , 106.24 2021-02-13T01:04:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.08 , -0.03 2021-02-13T01:04:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178295.78 epoch sec; seqNo:18363; 4060784.07 north; 593647.17 east 2021-02-13T01:04:56Z- AHRS_M2 msg: 1613178296.16 epoch sec; seqNo:18219 2021-02-13T01:04:56Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.20 psi 2021-02-13T01:04:56Z- handleDepth()-Depth_Keller msg: 1613178296170 epoch sec; seqNo:18364; depth 17.01 2021-02-13T01:04:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178296.18 epoch sec; seqNo:18364; 4060784.45 north; 593647.17 east 2021-02-13T01:04:56Z- AHRS_M2 msg: 1613178296.56 epoch sec; seqNo:18220 2021-02-13T01:04:56Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.19 psi 2021-02-13T01:04:56Z- handleDepth()-Depth_Keller msg: 1613178296568 epoch sec; seqNo:18365; depth 17.16 2021-02-13T01:04:56Z- handleDvl() - RDI_Pathfinder msg: 1613178296.59 epoch sec; seqNo:8910 2021-02-13T01:04:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.92 , 71.94 , 104.00 2021-02-13T01:04:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.12 , 0.03 2021-02-13T01:04:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178296.60 epoch sec; seqNo:18365; 4060784.83 north; 593647.16 east 2021-02-13T01:04:56Z- AHRS_M2 msg: 1613178296.97 epoch sec; seqNo:18221 2021-02-13T01:04:56Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.19 psi 2021-02-13T01:04:56Z- handleDepth()-Depth_Keller msg: 1613178296972 epoch sec; seqNo:18366; depth 17.25 2021-02-13T01:04:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178296.98 epoch sec; seqNo:18366; 4060784.83 north; 593647.16 east 2021-02-13T01:04:57Z- AHRS_M2 msg: 1613178297.37 epoch sec; seqNo:18222 2021-02-13T01:04:57Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-13T01:04:57Z- handleDepth()-Depth_Keller msg: 1613178297376 epoch sec; seqNo:18367; depth 17.38 2021-02-13T01:04:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178297.38 epoch sec; seqNo:18367; 4060785.21 north; 593647.16 east 2021-02-13T01:04:57Z- AHRS_M2 msg: 1613178297.78 epoch sec; seqNo:18223 2021-02-13T01:04:57Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-13T01:04:57Z- handleDepth()-Depth_Keller msg: 1613178297780 epoch sec; seqNo:18368; depth 17.50 2021-02-13T01:04:57Z- handleDvl() - RDI_Pathfinder msg: 1613178297.80 epoch sec; seqNo:8911 2021-02-13T01:04:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.02 , 97.65 , 71.94 , 103.73 2021-02-13T01:04:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.06 , 0.08 2021-02-13T01:04:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178297.81 epoch sec; seqNo:18368; 4060785.59 north; 593647.15 east 2021-02-13T01:04:58Z- AHRS_M2 msg: 1613178298.19 epoch sec; seqNo:18224 2021-02-13T01:04:58Z- AHRS_M2 msg: 0.57 phi; -0.31 theta; -0.18 psi 2021-02-13T01:04:58Z- handleDepth()-Depth_Keller msg: 1613178298190 epoch sec; seqNo:18369; depth 17.62 2021-02-13T01:04:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178298.20 epoch sec; seqNo:18369; 4060785.98 north; 593648.37 east 2021-02-13T01:04:58Z- AHRS_M2 msg: 1613178298.61 epoch sec; seqNo:18225 2021-02-13T01:04:58Z- AHRS_M2 msg: 0.57 phi; -0.31 theta; -0.18 psi 2021-02-13T01:04:58Z- handleDepth()-Depth_Keller msg: 1613178298625 epoch sec; seqNo:18370; depth 17.73 2021-02-13T01:04:58Z- handleDvl() - RDI_Pathfinder msg: 1613178298.65 epoch sec; seqNo:8912 2021-02-13T01:04:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.99 , 69.89 , 102.85 2021-02-13T01:04:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.02 , 0.04 2021-02-13T01:04:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178298.66 epoch sec; seqNo:18370; 4060786.36 north; 593648.37 east 2021-02-13T01:04:58Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-13T01:04:58Z- waiting for 1613178296.76, time is 1613178298.66 2021-02-13T01:04:58Z- LcmTrn::updateTrn() equating pose 1613178298.66 and meas times 1613178298.66 2021-02-13T01:04:58Z- LcmTrn::updateTrn() >>>> pose time 1613178298.66, meas time 1613178298.66 <<<< 2021-02-13T01:04:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178298.66 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:04:58Z- TerrainNav::motionUpdate() - currEstimate = (4060786, 593648.37, 17.73) 2021-02-13T01:04:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:04:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:04:58Z- TNavBF::Projecting Measurements 2021-02-13T01:04:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:04:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:04:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:04:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:04:58Z- TNavFilter::4 good beams. 2021-02-13T01:04:58Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:04:58Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:04:58Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:04:58Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:04:58Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:04:58Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:04:58Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:04:58Z- TNavBF::mmse selected from filter -1 2021-02-13T01:04:58Z- TerrainMapOctree: (vehN, vehE ) = (4060786.36, 593648.37). 2021-02-13T01:04:58Z- TerrainMapOctree: Min Distance = 759.56. 2021-02-13T01:04:58Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:04:58Z- There is no specific good beam that all particles have in common. 2021-02-13T01:04:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:04:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.46 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T01:04:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:04:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.070 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T01:04:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:04:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 7.82 2021-02-13T01:04:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:04:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 16.93 2021-02-13T01:04:59Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:04:59Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:04:59Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:04:59Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:04:59Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:04:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:04:59Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:04:59Z- Filter 0 No beams used in subcloud update 2021-02-13T01:04:59Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:04:59Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:04:59Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 676.7 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T01:04:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178298.66 sec. 2021-02-13T01:04:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:04:59Z- TNavBF::mmse selected from filter -1 2021-02-13T01:04:59Z- variances([0], [2], and psi_berg) = 676.658727 832.987085 0.000000 2021-02-13T01:04:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:04:59Z- LcmTrn::publishEstimates() - MLE : 4060785.86 593663.87 17.73 2021-02-13T01:04:59Z- LcmTrn::publishEstimates() - MMSE : 4060786.36 593648.37 17.73 2021-02-13T01:04:59Z- LcmTrn::publishEstimates() - COVAR: 676.66 832.99 1.01 0.00 2021-02-13T01:04:59Z- AHRS_M2 msg: 1613178298.99 epoch sec; seqNo:18226 2021-02-13T01:04:59Z- AHRS_M2 msg: 0.57 phi; -0.31 theta; -0.19 psi 2021-02-13T01:04:59Z- handleDepth()-Depth_Keller msg: 1613178299002 epoch sec; seqNo:18371; depth 17.86 2021-02-13T01:04:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178299.01 epoch sec; seqNo:18371; 4060786.36 north; 593648.37 east 2021-02-13T01:04:59Z- AHRS_M2 msg: 1613178299.39 epoch sec; seqNo:18227 2021-02-13T01:04:59Z- AHRS_M2 msg: 0.57 phi; -0.31 theta; -0.19 psi 2021-02-13T01:04:59Z- handleDepth()-Depth_Keller msg: 1613178299401 epoch sec; seqNo:18372; depth 17.96 2021-02-13T01:04:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178299.41 epoch sec; seqNo:18372; 4060786.74 north; 593648.36 east 2021-02-13T01:04:59Z- AHRS_M2 msg: 1613178299.80 epoch sec; seqNo:18228 2021-02-13T01:04:59Z- AHRS_M2 msg: 0.57 phi; -0.31 theta; -0.19 psi 2021-02-13T01:04:59Z- handleDepth()-Depth_Keller msg: 1613178299801 epoch sec; seqNo:18373; depth 18.06 2021-02-13T01:04:59Z- handleDvl() - RDI_Pathfinder msg: 1613178299.81 epoch sec; seqNo:8913 2021-02-13T01:04:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.99 , 70.30 , 104.07 2021-02-13T01:04:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , -0.02 2021-02-13T01:04:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178299.82 epoch sec; seqNo:18373; 4060787.12 north; 593648.36 east 2021-02-13T01:05:00Z- AHRS_M2 msg: 1613178300.20 epoch sec; seqNo:18229 2021-02-13T01:05:00Z- AHRS_M2 msg: 0.57 phi; -0.31 theta; -0.19 psi 2021-02-13T01:05:00Z- handleDepth()-Depth_Keller msg: 1613178300204 epoch sec; seqNo:18374; depth 18.18 2021-02-13T01:05:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178300.22 epoch sec; seqNo:18374; 4060787.49 north; 593648.35 east 2021-02-13T01:05:00Z- AHRS_M2 msg: 1613178300.60 epoch sec; seqNo:18230 2021-02-13T01:05:00Z- AHRS_M2 msg: 0.57 phi; -0.31 theta; -0.19 psi 2021-02-13T01:05:00Z- handleDepth()-Depth_Keller msg: 1613178300608 epoch sec; seqNo:18375; depth 18.31 2021-02-13T01:05:00Z- handleDvl() - RDI_Pathfinder msg: 1613178300.62 epoch sec; seqNo:8914 2021-02-13T01:05:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.99 , 70.30 , 101.64 2021-02-13T01:05:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , -0.06 2021-02-13T01:05:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178300.64 epoch sec; seqNo:18375; 4060787.51 north; 593649.57 east 2021-02-13T01:05:01Z- AHRS_M2 msg: 1613178301.01 epoch sec; seqNo:18231 2021-02-13T01:05:01Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.19 psi 2021-02-13T01:05:01Z- handleDepth()-Depth_Keller msg: 1613178301012 epoch sec; seqNo:18376; depth 18.43 2021-02-13T01:05:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178301.02 epoch sec; seqNo:18376; 4060787.89 north; 593649.57 east 2021-02-13T01:05:01Z- AHRS_M2 msg: 1613178301.41 epoch sec; seqNo:18232 2021-02-13T01:05:01Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.19 psi 2021-02-13T01:05:01Z- handleDepth()-Depth_Keller msg: 1613178301421 epoch sec; seqNo:18377; depth 18.54 2021-02-13T01:05:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178301.43 epoch sec; seqNo:18377; 4060788.27 north; 593649.57 east 2021-02-13T01:05:01Z- AHRS_M2 msg: 1613178301.82 epoch sec; seqNo:18233 2021-02-13T01:05:01Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-13T01:05:01Z- handleDepth()-Depth_Keller msg: 1613178301823 epoch sec; seqNo:18378; depth 18.64 2021-02-13T01:05:01Z- handleDvl() - RDI_Pathfinder msg: 1613178301.85 epoch sec; seqNo:8915 2021-02-13T01:05:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 93.12 , 78.51 , 107.72 2021-02-13T01:05:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.06 2021-02-13T01:05:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178301.86 epoch sec; seqNo:18378; 4060788.64 north; 593649.56 east 2021-02-13T01:05:02Z- AHRS_M2 msg: 1613178302.22 epoch sec; seqNo:18234 2021-02-13T01:05:02Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-13T01:05:02Z- handleDepth()-Depth_Keller msg: 1613178302229 epoch sec; seqNo:18379; depth 18.76 2021-02-13T01:05:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178302.24 epoch sec; seqNo:18379; 4060789.02 north; 593649.56 east 2021-02-13T01:05:02Z- AHRS_M2 msg: 1613178302.62 epoch sec; seqNo:18235 2021-02-13T01:05:02Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-13T01:05:02Z- handleDepth()-Depth_Keller msg: 1613178302633 epoch sec; seqNo:18380; depth 18.87 2021-02-13T01:05:02Z- handleDvl() - RDI_Pathfinder msg: 1613178302.64 epoch sec; seqNo:8916 2021-02-13T01:05:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.78 , 78.31 , 102.65 2021-02-13T01:05:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , -0.03 2021-02-13T01:05:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178302.65 epoch sec; seqNo:18380; 4060789.02 north; 593649.56 east 2021-02-13T01:05:03Z- AHRS_M2 msg: 1613178303.03 epoch sec; seqNo:18236 2021-02-13T01:05:03Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-13T01:05:03Z- handleDepth()-Depth_Keller msg: 1613178303034 epoch sec; seqNo:18381; depth 18.99 2021-02-13T01:05:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178303.05 epoch sec; seqNo:18381; 4060789.40 north; 593649.55 east 2021-02-13T01:05:03Z- AHRS_M2 msg: 1613178303.43 epoch sec; seqNo:18237 2021-02-13T01:05:03Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-13T01:05:03Z- handleDepth()-Depth_Keller msg: 1613178303439 epoch sec; seqNo:18382; depth 19.10 2021-02-13T01:05:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178303.47 epoch sec; seqNo:18382; 4060789.79 north; 593650.77 east 2021-02-13T01:05:03Z- AHRS_M2 msg: 1613178303.84 epoch sec; seqNo:18238 2021-02-13T01:05:03Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.18 psi 2021-02-13T01:05:03Z- handleDepth()-Depth_Keller msg: 1613178303845 epoch sec; seqNo:18383; depth 19.20 2021-02-13T01:05:03Z- handleDvl() - RDI_Pathfinder msg: 1613178303.86 epoch sec; seqNo:8917 2021-02-13T01:05:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 97.65 , 69.07 , 101.71 2021-02-13T01:05:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , 0.02 2021-02-13T01:05:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178303.88 epoch sec; seqNo:18383; 4060790.17 north; 593650.77 east 2021-02-13T01:05:04Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 132 calls... 2021-02-13T01:05:04Z- waiting for 1613178301.66, time is 1613178303.88 2021-02-13T01:05:04Z- LcmTrn::updateTrn() equating pose 1613178303.88 and meas times 1613178303.88 2021-02-13T01:05:04Z- LcmTrn::updateTrn() >>>> pose time 1613178303.88, meas time 1613178303.88 <<<< 2021-02-13T01:05:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178303.88 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:05:04Z- TerrainNav::motionUpdate() - currEstimate = (4060790, 593650.77, 19.20) 2021-02-13T01:05:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:05:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:05:04Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178303.88 sec.; No relevant navigation data available 2021-02-13T01:05:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:05:04Z- TNavBF::mmse selected from filter -1 2021-02-13T01:05:04Z- variances([0], [2], and psi_berg) = 676.613713 832.987067 0.000000 2021-02-13T01:05:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:05:04Z- LcmTrn::publishEstimates() - MLE : 4060789.67 593638.27 19.20 2021-02-13T01:05:04Z- LcmTrn::publishEstimates() - MMSE : 4060790.17 593650.77 19.20 2021-02-13T01:05:04Z- LcmTrn::publishEstimates() - COVAR: 676.61 832.99 1.24 0.00 2021-02-13T01:05:04Z- AHRS_M2 msg: 1613178304.24 epoch sec; seqNo:18239 2021-02-13T01:05:04Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.17 psi 2021-02-13T01:05:04Z- handleDepth()-Depth_Keller msg: 1613178304248 epoch sec; seqNo:18384; depth 19.32 2021-02-13T01:05:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178304.26 epoch sec; seqNo:18384; 4060790.55 north; 593650.76 east 2021-02-13T01:05:04Z- AHRS_M2 msg: 1613178304.64 epoch sec; seqNo:18240 2021-02-13T01:05:04Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.17 psi 2021-02-13T01:05:04Z- handleDepth()-Depth_Keller msg: 1613178304647 epoch sec; seqNo:18385; depth 19.42 2021-02-13T01:05:04Z- handleDvl() - RDI_Pathfinder msg: 1613178304.67 epoch sec; seqNo:8918 2021-02-13T01:05:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.21 , 98.12 , 69.89 , 100.56 2021-02-13T01:05:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , 0.04 2021-02-13T01:05:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178304.68 epoch sec; seqNo:18385; 4060790.55 north; 593650.76 east 2021-02-13T01:05:05Z- AHRS_M2 msg: 1613178305.05 epoch sec; seqNo:18241 2021-02-13T01:05:05Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.17 psi 2021-02-13T01:05:05Z- handleDepth()-Depth_Keller msg: 1613178305057 epoch sec; seqNo:18386; depth 19.55 2021-02-13T01:05:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178305.07 epoch sec; seqNo:18386; 4060790.93 north; 593650.76 east 2021-02-13T01:05:05Z- AHRS_M2 msg: 1613178305.45 epoch sec; seqNo:18242 2021-02-13T01:05:05Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.17 psi 2021-02-13T01:05:05Z- handleDepth()-Depth_Keller msg: 1613178305461 epoch sec; seqNo:18387; depth 19.65 2021-02-13T01:05:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178305.47 epoch sec; seqNo:18387; 4060791.31 north; 593650.75 east 2021-02-13T01:05:05Z- AHRS_M2 msg: 1613178305.86 epoch sec; seqNo:18243 2021-02-13T01:05:05Z- AHRS_M2 msg: 0.59 phi; -0.31 theta; -0.16 psi 2021-02-13T01:05:05Z- handleDepth()-Depth_Keller msg: 1613178305861 epoch sec; seqNo:18388; depth 19.77 2021-02-13T01:05:05Z- handleDvl() - RDI_Pathfinder msg: 1613178305.87 epoch sec; seqNo:8919 2021-02-13T01:05:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 98.12 , 69.07 , 101.77 2021-02-13T01:05:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.00 , 0.05 2021-02-13T01:05:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178305.88 epoch sec; seqNo:18388; 4060791.70 north; 593651.97 east 2021-02-13T01:05:06Z- AHRS_M2 msg: 1613178306.26 epoch sec; seqNo:18244 2021-02-13T01:05:06Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.16 psi 2021-02-13T01:05:06Z- handleDepth()-Depth_Keller msg: 1613178306269 epoch sec; seqNo:18389; depth 19.88 2021-02-13T01:05:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178306.28 epoch sec; seqNo:18389; 4060791.70 north; 593651.97 east 2021-02-13T01:05:06Z- AHRS_M2 msg: 1613178306.67 epoch sec; seqNo:18245 2021-02-13T01:05:06Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.15 psi 2021-02-13T01:05:06Z- handleDepth()-Depth_Keller msg: 1613178306675 epoch sec; seqNo:18390; depth 20.00 2021-02-13T01:05:06Z- handleDvl() - RDI_Pathfinder msg: 1613178306.69 epoch sec; seqNo:8920 2021-02-13T01:05:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:05:06Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:05:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178306.70 epoch sec; seqNo:18390; 4060792.08 north; 593651.97 east 2021-02-13T01:05:07Z- AHRS_M2 msg: 1613178307.07 epoch sec; seqNo:18246 2021-02-13T01:05:07Z- AHRS_M2 msg: 0.60 phi; -0.31 theta; -0.15 psi 2021-02-13T01:05:07Z- handleDepth()-Depth_Keller msg: 1613178307078 epoch sec; seqNo:18391; depth 20.11 2021-02-13T01:05:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178307.09 epoch sec; seqNo:18391; 4060792.46 north; 593651.96 east 2021-02-13T01:05:07Z- AHRS_M2 msg: 1613178307.48 epoch sec; seqNo:18247 2021-02-13T01:05:07Z- AHRS_M2 msg: 0.60 phi; -0.32 theta; -0.15 psi 2021-02-13T01:05:07Z- handleDepth()-Depth_Keller msg: 1613178307492 epoch sec; seqNo:18392; depth 20.21 2021-02-13T01:05:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178307.50 epoch sec; seqNo:18392; 4060792.84 north; 593651.96 east 2021-02-13T01:05:07Z- AHRS_M2 msg: 1613178307.88 epoch sec; seqNo:18248 2021-02-13T01:05:07Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.15 psi 2021-02-13T01:05:07Z- handleDepth()-Depth_Keller msg: 1613178307880 epoch sec; seqNo:18393; depth 20.34 2021-02-13T01:05:07Z- handleDvl() - RDI_Pathfinder msg: 1613178307.89 epoch sec; seqNo:8921 2021-02-13T01:05:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.57 , 97.65 , 67.44 , 102.11 2021-02-13T01:05:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , 0.02 2021-02-13T01:05:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178307.90 epoch sec; seqNo:18393; 4060792.84 north; 593651.96 east 2021-02-13T01:05:08Z- AHRS_M2 msg: 1613178308.28 epoch sec; seqNo:18249 2021-02-13T01:05:08Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.16 psi 2021-02-13T01:05:08Z- handleDepth()-Depth_Keller msg: 1613178308289 epoch sec; seqNo:18394; depth 20.44 2021-02-13T01:05:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178308.30 epoch sec; seqNo:18394; 4060793.23 north; 593653.17 east 2021-02-13T01:05:08Z- AHRS_M2 msg: 1613178308.68 epoch sec; seqNo:18250 2021-02-13T01:05:08Z- AHRS_M2 msg: 0.61 phi; -0.32 theta; -0.16 psi 2021-02-13T01:05:08Z- handleDepth()-Depth_Keller msg: 1613178308697 epoch sec; seqNo:18395; depth 20.55 2021-02-13T01:05:08Z- handleDvl() - RDI_Pathfinder msg: 1613178308.71 epoch sec; seqNo:8922 2021-02-13T01:05:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 0.00 , 78.71 , 100.62 2021-02-13T01:05:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.01 2021-02-13T01:05:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178308.72 epoch sec; seqNo:18395; 4060793.61 north; 593653.17 east 2021-02-13T01:05:09Z- AHRS_M2 msg: 1613178309.09 epoch sec; seqNo:18251 2021-02-13T01:05:09Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; -0.17 psi 2021-02-13T01:05:09Z- LcmTrn::handleMessages() - handling 42 messages took 0.64 ms and 129 calls... 2021-02-13T01:05:09Z- waiting for 1613178306.88, time is 1613178309.09 2021-02-13T01:05:09Z- LcmTrn::updateTrn() >>>> pose time 1613178309.09, meas time 1613178308.71 <<<< 2021-02-13T01:05:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178308.71 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:05:09Z- TerrainNav::motionUpdate() - currEstimate = (4060794, 593653.17, 20.55) 2021-02-13T01:05:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:05:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:05:09Z- TNavBF::Projecting Measurements 2021-02-13T01:05:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:05:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:05:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:05:09Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:05:09Z- TNavFilter::3 good beams. 2021-02-13T01:05:09Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:05:09Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:05:09Z- TNavBF:: beamIndex[1] = 2 2021-02-13T01:05:09Z- TNavBF:: beamIndex[2] = 3 2021-02-13T01:05:09Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:05:09Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:05:09Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:05:09Z- TNavBF::mmse selected from filter -1 2021-02-13T01:05:09Z- TerrainMapOctree: (vehN, vehE ) = (4060793.36, 593652.99). 2021-02-13T01:05:09Z- TerrainMapOctree: Min Distance = 751.63. 2021-02-13T01:05:09Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:05:09Z- There is no specific good beam that all particles have in common. 2021-02-13T01:05:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:05:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.44 K: 0.119 ExpectedMeasDiff: nan currMeas.covariance: 8.95 2021-02-13T01:05:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:05:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 9.91 2021-02-13T01:05:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:05:09Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 16.20 2021-02-13T01:05:09Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:05:09Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:05:09Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:05:09Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:05:09Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:05:09Z- Filter 0 No beams used in subcloud update 2021-02-13T01:05:09Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:05:09Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:05:09Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 676.6 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T01:05:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178308.71 sec. 2021-02-13T01:05:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:05:09Z- TNavBF::mmse selected from filter -1 2021-02-13T01:05:09Z- variances([0], [2], and psi_berg) = 676.560168 832.987045 0.000000 2021-02-13T01:05:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:05:09Z- LcmTrn::publishEstimates() - MLE : 4060793.11 593671.67 20.55 2021-02-13T01:05:09Z- LcmTrn::publishEstimates() - MMSE : 4060793.61 593653.17 20.55 2021-02-13T01:05:09Z- LcmTrn::publishEstimates() - COVAR: 676.56 832.99 1.09 0.00 2021-02-13T01:05:09Z- handleDepth()-Depth_Keller msg: 1613178309097 epoch sec; seqNo:18396; depth 20.66 2021-02-13T01:05:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178309.11 epoch sec; seqNo:18396; 4060793.99 north; 593653.17 east 2021-02-13T01:05:09Z- AHRS_M2 msg: 1613178309.50 epoch sec; seqNo:18252 2021-02-13T01:05:09Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.17 psi 2021-02-13T01:05:09Z- handleDepth()-Depth_Keller msg: 1613178309509 epoch sec; seqNo:18397; depth 20.78 2021-02-13T01:05:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178309.54 epoch sec; seqNo:18397; 4060793.99 north; 593653.17 east 2021-02-13T01:05:09Z- AHRS_M2 msg: 1613178309.90 epoch sec; seqNo:18253 2021-02-13T01:05:09Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.17 psi 2021-02-13T01:05:09Z- handleDepth()-Depth_Keller msg: 1613178309901 epoch sec; seqNo:18398; depth 20.89 2021-02-13T01:05:09Z- handleDvl() - RDI_Pathfinder msg: 1613178309.91 epoch sec; seqNo:8923 2021-02-13T01:05:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.78 , 98.26 , 67.44 , 101.91 2021-02-13T01:05:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , -0.03 2021-02-13T01:05:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178309.92 epoch sec; seqNo:18398; 4060794.37 north; 593653.16 east 2021-02-13T01:05:10Z- AHRS_M2 msg: 1613178310.30 epoch sec; seqNo:18254 2021-02-13T01:05:10Z- AHRS_M2 msg: 0.62 phi; -0.33 theta; -0.17 psi 2021-02-13T01:05:10Z- handleDepth()-Depth_Keller msg: 1613178310304 epoch sec; seqNo:18399; depth 21.01 2021-02-13T01:05:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178310.32 epoch sec; seqNo:18399; 4060794.75 north; 593653.16 east 2021-02-13T01:05:10Z- AHRS_M2 msg: 1613178310.70 epoch sec; seqNo:18255 2021-02-13T01:05:10Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.17 psi 2021-02-13T01:05:10Z- handleDepth()-Depth_Keller msg: 1613178310713 epoch sec; seqNo:18400; depth 21.11 2021-02-13T01:05:10Z- handleDvl() - RDI_Pathfinder msg: 1613178310.72 epoch sec; seqNo:8924 2021-02-13T01:05:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 90.75 , 65.81 , 102.92 2021-02-13T01:05:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.00 2021-02-13T01:05:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178310.73 epoch sec; seqNo:18400; 4060795.14 north; 593654.37 east 2021-02-13T01:05:11Z- AHRS_M2 msg: 1613178311.11 epoch sec; seqNo:18256 2021-02-13T01:05:11Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.17 psi 2021-02-13T01:05:11Z- handleDepth()-Depth_Keller msg: 1613178311125 epoch sec; seqNo:18401; depth 21.24 2021-02-13T01:05:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178311.14 epoch sec; seqNo:18401; 4060795.14 north; 593654.37 east 2021-02-13T01:05:11Z- AHRS_M2 msg: 1613178311.53 epoch sec; seqNo:18257 2021-02-13T01:05:11Z- AHRS_M2 msg: 0.62 phi; -0.34 theta; -0.17 psi 2021-02-13T01:05:11Z- handleDepth()-Depth_Keller msg: 1613178311530 epoch sec; seqNo:18402; depth 21.34 2021-02-13T01:05:11Z- handleDvl() - RDI_Pathfinder msg: 1613178311.54 epoch sec; seqNo:8925 2021-02-13T01:05:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.98 , 98.05 , 66.62 , 102.92 2021-02-13T01:05:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , 0.04 2021-02-13T01:05:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178311.55 epoch sec; seqNo:18402; 4060795.52 north; 593654.37 east 2021-02-13T01:05:11Z- AHRS_M2 msg: 1613178311.92 epoch sec; seqNo:18258 2021-02-13T01:05:11Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.17 psi 2021-02-13T01:05:11Z- handleDepth()-Depth_Keller msg: 1613178311920 epoch sec; seqNo:18403; depth 21.47 2021-02-13T01:05:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178311.93 epoch sec; seqNo:18403; 4060795.90 north; 593654.37 east 2021-02-13T01:05:12Z- AHRS_M2 msg: 1613178312.32 epoch sec; seqNo:18259 2021-02-13T01:05:12Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.17 psi 2021-02-13T01:05:12Z- handleDepth()-Depth_Keller msg: 1613178312324 epoch sec; seqNo:18404; depth 21.57 2021-02-13T01:05:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178312.34 epoch sec; seqNo:18404; 4060796.27 north; 593654.36 east 2021-02-13T01:05:12Z- AHRS_M2 msg: 1613178312.72 epoch sec; seqNo:18260 2021-02-13T01:05:12Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.17 psi 2021-02-13T01:05:12Z- handleDepth()-Depth_Keller msg: 1613178312730 epoch sec; seqNo:18405; depth 21.68 2021-02-13T01:05:12Z- handleDvl() - RDI_Pathfinder msg: 1613178312.74 epoch sec; seqNo:8926 2021-02-13T01:05:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.98 , 98.19 , 67.03 , 103.06 2021-02-13T01:05:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , 0.08 2021-02-13T01:05:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178312.75 epoch sec; seqNo:18405; 4060796.27 north; 593654.36 east 2021-02-13T01:05:13Z- AHRS_M2 msg: 1613178313.13 epoch sec; seqNo:18261 2021-02-13T01:05:13Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.17 psi 2021-02-13T01:05:13Z- handleDepth()-Depth_Keller msg: 1613178313136 epoch sec; seqNo:18406; depth 21.81 2021-02-13T01:05:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178313.16 epoch sec; seqNo:18406; 4060796.65 north; 593654.36 east 2021-02-13T01:05:13Z- AHRS_M2 msg: 1613178313.54 epoch sec; seqNo:18262 2021-02-13T01:05:13Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.18 psi 2021-02-13T01:05:13Z- handleDepth()-Depth_Keller msg: 1613178313543 epoch sec; seqNo:18407; depth 21.93 2021-02-13T01:05:13Z- handleDvl() - RDI_Pathfinder msg: 1613178313.55 epoch sec; seqNo:8927 2021-02-13T01:05:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 98.12 , 65.40 , 102.99 2021-02-13T01:05:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , 0.09 2021-02-13T01:05:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178313.58 epoch sec; seqNo:18407; 4060797.05 north; 593655.57 east 2021-02-13T01:05:13Z- AHRS_M2 msg: 1613178313.94 epoch sec; seqNo:18263 2021-02-13T01:05:13Z- AHRS_M2 msg: 0.62 phi; -0.36 theta; -0.18 psi 2021-02-13T01:05:13Z- handleDepth()-Depth_Keller msg: 1613178313940 epoch sec; seqNo:18408; depth 22.06 2021-02-13T01:05:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178313.95 epoch sec; seqNo:18408; 4060797.42 north; 593655.57 east 2021-02-13T01:05:14Z- LcmTrn::handleMessages() - handling 43 messages took 0.63 ms and 131 calls... 2021-02-13T01:05:14Z- waiting for 1613178312.09, time is 1613178313.95 2021-02-13T01:05:14Z- LcmTrn::updateTrn() >>>> pose time 1613178313.95, meas time 1613178313.55 <<<< 2021-02-13T01:05:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178313.55 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:05:14Z- TerrainNav::motionUpdate() - currEstimate = (4060797, 593655.57, 22.06) 2021-02-13T01:05:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:05:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:05:14Z- TNavBF::Projecting Measurements 2021-02-13T01:05:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:05:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:05:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:05:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:05:14Z- TNavFilter::4 good beams. 2021-02-13T01:05:14Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:05:14Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:05:14Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:05:14Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:05:14Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:05:14Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:05:14Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:05:14Z- TNavBF::mmse selected from filter -1 2021-02-13T01:05:14Z- TerrainMapOctree: (vehN, vehE ) = (4060797.11, 593655.37). 2021-02-13T01:05:14Z- TerrainMapOctree: Min Distance = 747.45. 2021-02-13T01:05:14Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:05:14Z- There is no specific good beam that all particles have in common. 2021-02-13T01:05:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:05:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.30 K: 0.101 ExpectedMeasDiff: nan currMeas.covariance: 9.90 2021-02-13T01:05:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:05:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.17 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 15.40 2021-02-13T01:05:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:05:14Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.113 ExpectedMeasDiff: nan currMeas.covariance: 6.84 2021-02-13T01:05:14Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:05:14Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 16.97 2021-02-13T01:05:14Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:05:14Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:05:14Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:05:14Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:05:14Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:14Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:05:14Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:05:14Z- Filter 0 No beams used in subcloud update 2021-02-13T01:05:14Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:05:14Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:05:14Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 676.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:05:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178313.55 sec. 2021-02-13T01:05:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:05:14Z- TNavBF::mmse selected from filter -1 2021-02-13T01:05:14Z- variances([0], [2], and psi_berg) = 676.504228 832.987022 0.000000 2021-02-13T01:05:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:05:14Z- LcmTrn::publishEstimates() - MLE : 4060796.92 593643.07 22.06 2021-02-13T01:05:14Z- LcmTrn::publishEstimates() - MMSE : 4060797.42 593655.57 22.06 2021-02-13T01:05:14Z- LcmTrn::publishEstimates() - COVAR: 676.50 832.99 0.94 0.00 2021-02-13T01:05:14Z- AHRS_M2 msg: 1613178314.34 epoch sec; seqNo:18264 2021-02-13T01:05:14Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.18 psi 2021-02-13T01:05:14Z- handleDepth()-Depth_Keller msg: 1613178314346 epoch sec; seqNo:18409; depth 22.18 2021-02-13T01:05:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178314.36 epoch sec; seqNo:18409; 4060797.42 north; 593655.57 east 2021-02-13T01:05:14Z- AHRS_M2 msg: 1613178314.74 epoch sec; seqNo:18265 2021-02-13T01:05:14Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.18 psi 2021-02-13T01:05:14Z- handleDepth()-Depth_Keller msg: 1613178314753 epoch sec; seqNo:18410; depth 22.31 2021-02-13T01:05:14Z- handleDvl() - RDI_Pathfinder msg: 1613178314.78 epoch sec; seqNo:8928 2021-02-13T01:05:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.05 , 64.58 , 104.14 2021-02-13T01:05:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.07 , 0.07 2021-02-13T01:05:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178314.79 epoch sec; seqNo:18410; 4060797.80 north; 593655.57 east 2021-02-13T01:05:15Z- AHRS_M2 msg: 1613178315.15 epoch sec; seqNo:18266 2021-02-13T01:05:15Z- AHRS_M2 msg: 0.62 phi; -0.37 theta; -0.19 psi 2021-02-13T01:05:15Z- handleDepth()-Depth_Keller msg: 1613178315159 epoch sec; seqNo:18411; depth 22.44 2021-02-13T01:05:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178315.17 epoch sec; seqNo:18411; 4060798.18 north; 593655.56 east 2021-02-13T01:05:15Z- AHRS_M2 msg: 1613178315.57 epoch sec; seqNo:18267 2021-02-13T01:05:15Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.19 psi 2021-02-13T01:05:15Z- handleDepth()-Depth_Keller msg: 1613178315577 epoch sec; seqNo:18412; depth 22.58 2021-02-13T01:05:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178315.60 epoch sec; seqNo:18412; 4060798.20 north; 593656.78 east 2021-02-13T01:05:15Z- AHRS_M2 msg: 1613178315.96 epoch sec; seqNo:18268 2021-02-13T01:05:15Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.19 psi 2021-02-13T01:05:15Z- handleDepth()-Depth_Keller msg: 1613178315969 epoch sec; seqNo:18413; depth 22.70 2021-02-13T01:05:15Z- handleDvl() - RDI_Pathfinder msg: 1613178315.98 epoch sec; seqNo:8929 2021-02-13T01:05:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.35 , 97.85 , 78.38 , 105.15 2021-02-13T01:05:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.05 , 0.06 2021-02-13T01:05:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178315.99 epoch sec; seqNo:18413; 4060798.57 north; 593656.78 east 2021-02-13T01:05:16Z- AHRS_M2 msg: 1613178316.36 epoch sec; seqNo:18269 2021-02-13T01:05:16Z- AHRS_M2 msg: 0.63 phi; -0.39 theta; -0.19 psi 2021-02-13T01:05:16Z- handleDepth()-Depth_Keller msg: 1613178316364 epoch sec; seqNo:18414; depth 22.85 2021-02-13T01:05:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178316.38 epoch sec; seqNo:18414; 4060798.95 north; 593656.77 east 2021-02-13T01:05:16Z- AHRS_M2 msg: 1613178316.77 epoch sec; seqNo:18270 2021-02-13T01:05:16Z- AHRS_M2 msg: 0.63 phi; -0.39 theta; -0.19 psi 2021-02-13T01:05:16Z- handleDepth()-Depth_Keller msg: 1613178316771 epoch sec; seqNo:18415; depth 22.96 2021-02-13T01:05:16Z- handleDvl() - RDI_Pathfinder msg: 1613178316.78 epoch sec; seqNo:8930 2021-02-13T01:05:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.16 , 97.92 , 65.81 , 106.44 2021-02-13T01:05:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.04 , -0.04 2021-02-13T01:05:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178316.79 epoch sec; seqNo:18415; 4060798.95 north; 593656.77 east 2021-02-13T01:05:17Z- AHRS_M2 msg: 1613178317.17 epoch sec; seqNo:18271 2021-02-13T01:05:17Z- AHRS_M2 msg: 0.63 phi; -0.39 theta; -0.19 psi 2021-02-13T01:05:17Z- handleDepth()-Depth_Keller msg: 1613178317178 epoch sec; seqNo:18416; depth 23.10 2021-02-13T01:05:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178317.19 epoch sec; seqNo:18416; 4060799.33 north; 593656.77 east 2021-02-13T01:05:17Z- AHRS_M2 msg: 1613178317.57 epoch sec; seqNo:18272 2021-02-13T01:05:17Z- AHRS_M2 msg: 0.63 phi; -0.40 theta; -0.19 psi 2021-02-13T01:05:17Z- handleDepth()-Depth_Keller msg: 1613178317576 epoch sec; seqNo:18417; depth 23.24 2021-02-13T01:05:17Z- handleDvl() - RDI_Pathfinder msg: 1613178317.59 epoch sec; seqNo:8931 2021-02-13T01:05:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.65 , 98.12 , 64.17 , 106.64 2021-02-13T01:05:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , -0.09 2021-02-13T01:05:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178317.60 epoch sec; seqNo:18417; 4060799.71 north; 593656.76 east 2021-02-13T01:05:17Z- AHRS_M2 msg: 1613178317.98 epoch sec; seqNo:18273 2021-02-13T01:05:17Z- AHRS_M2 msg: 0.63 phi; -0.40 theta; -0.19 psi 2021-02-13T01:05:17Z- handleDepth()-Depth_Keller msg: 1613178317980 epoch sec; seqNo:18418; depth 23.38 2021-02-13T01:05:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178317.99 epoch sec; seqNo:18418; 4060800.10 north; 593657.98 east 2021-02-13T01:05:18Z- AHRS_M2 msg: 1613178318.38 epoch sec; seqNo:18274 2021-02-13T01:05:18Z- AHRS_M2 msg: 0.63 phi; -0.40 theta; -0.20 psi 2021-02-13T01:05:18Z- handleDepth()-Depth_Keller msg: 1613178318389 epoch sec; seqNo:18419; depth 23.52 2021-02-13T01:05:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178318.40 epoch sec; seqNo:18419; 4060800.10 north; 593657.98 east 2021-02-13T01:05:18Z- AHRS_M2 msg: 1613178318.79 epoch sec; seqNo:18275 2021-02-13T01:05:18Z- AHRS_M2 msg: 0.63 phi; -0.41 theta; -0.20 psi 2021-02-13T01:05:18Z- handleDepth()-Depth_Keller msg: 1613178318789 epoch sec; seqNo:18420; depth 23.65 2021-02-13T01:05:18Z- handleDvl() - RDI_Pathfinder msg: 1613178318.80 epoch sec; seqNo:8932 2021-02-13T01:05:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.94 , 98.05 , 0.00 , 106.57 2021-02-13T01:05:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , -0.09 2021-02-13T01:05:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178318.81 epoch sec; seqNo:18420; 4060800.48 north; 593657.98 east 2021-02-13T01:05:19Z- AHRS_M2 msg: 1613178319.19 epoch sec; seqNo:18276 2021-02-13T01:05:19Z- AHRS_M2 msg: 0.62 phi; -0.41 theta; -0.20 psi 2021-02-13T01:05:19Z- handleDepth()-Depth_Keller msg: 1613178319193 epoch sec; seqNo:18421; depth 23.80 2021-02-13T01:05:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178319.22 epoch sec; seqNo:18421; 4060800.86 north; 593657.97 east 2021-02-13T01:05:19Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-13T01:05:19Z- waiting for 1613178316.95, time is 1613178319.22 2021-02-13T01:05:19Z- LcmTrn::updateTrn() >>>> pose time 1613178319.22, meas time 1613178318.80 <<<< 2021-02-13T01:05:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178318.80 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:05:19Z- TerrainNav::motionUpdate() - currEstimate = (4060801, 593657.97, 23.80) 2021-02-13T01:05:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:05:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:05:19Z- TNavBF::Projecting Measurements 2021-02-13T01:05:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:05:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:05:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:05:19Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:05:19Z- TNavFilter::3 good beams. 2021-02-13T01:05:19Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:05:19Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:05:19Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:05:19Z- TNavBF:: beamIndex[2] = 3 2021-02-13T01:05:19Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:05:19Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:05:19Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:05:19Z- TNavBF::mmse selected from filter -1 2021-02-13T01:05:19Z- TerrainMapOctree: (vehN, vehE ) = (4060800.59, 593657.78). 2021-02-13T01:05:19Z- TerrainMapOctree: Min Distance = 743.43. 2021-02-13T01:05:19Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:05:19Z- There is no specific good beam that all particles have in common. 2021-02-13T01:05:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:05:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 8.28 2021-02-13T01:05:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:05:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 15.38 2021-02-13T01:05:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:05:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 18.17 2021-02-13T01:05:19Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:05:19Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:05:19Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:05:19Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:05:19Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:05:19Z- Filter 0 No beams used in subcloud update 2021-02-13T01:05:19Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:05:19Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:05:19Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 676.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:05:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178318.80 sec. 2021-02-13T01:05:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:05:19Z- TNavBF::mmse selected from filter -1 2021-02-13T01:05:19Z- variances([0], [2], and psi_berg) = 676.450445 832.987000 0.000000 2021-02-13T01:05:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:05:19Z- LcmTrn::publishEstimates() - MLE : 4060800.36 593645.47 23.80 2021-02-13T01:05:19Z- LcmTrn::publishEstimates() - MMSE : 4060800.86 593657.97 23.80 2021-02-13T01:05:19Z- LcmTrn::publishEstimates() - COVAR: 676.45 832.99 0.94 0.00 2021-02-13T01:05:19Z- AHRS_M2 msg: 1613178319.59 epoch sec; seqNo:18277 2021-02-13T01:05:19Z- AHRS_M2 msg: 0.62 phi; -0.41 theta; -0.20 psi 2021-02-13T01:05:19Z- handleDepth()-Depth_Keller msg: 1613178319601 epoch sec; seqNo:18422; depth 23.95 2021-02-13T01:05:19Z- handleDvl() - RDI_Pathfinder msg: 1613178319.62 epoch sec; seqNo:8933 2021-02-13T01:05:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.35 , 97.71 , 61.72 , 107.18 2021-02-13T01:05:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , -0.05 2021-02-13T01:05:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178319.63 epoch sec; seqNo:18422; 4060801.24 north; 593657.97 east 2021-02-13T01:05:19Z- AHRS_M2 msg: 1613178320.00 epoch sec; seqNo:18278 2021-02-13T01:05:19Z- AHRS_M2 msg: 0.61 phi; -0.41 theta; -0.21 psi 2021-02-13T01:05:20Z- handleDepth()-Depth_Keller msg: 1613178320000 epoch sec; seqNo:18423; depth 24.09 2021-02-13T01:05:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178320.01 epoch sec; seqNo:18423; 4060801.24 north; 593657.97 east 2021-02-13T01:05:20Z- AHRS_M2 msg: 1613178320.41 epoch sec; seqNo:18279 2021-02-13T01:05:20Z- AHRS_M2 msg: 0.61 phi; -0.41 theta; -0.21 psi 2021-02-13T01:05:20Z- handleDepth()-Depth_Keller msg: 1613178320418 epoch sec; seqNo:18424; depth 24.25 2021-02-13T01:05:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178320.44 epoch sec; seqNo:18424; 4060801.62 north; 593657.96 east 2021-02-13T01:05:20Z- AHRS_M2 msg: 1613178320.80 epoch sec; seqNo:18280 2021-02-13T01:05:20Z- AHRS_M2 msg: 0.60 phi; -0.41 theta; -0.21 psi 2021-02-13T01:05:20Z- handleDepth()-Depth_Keller msg: 1613178320813 epoch sec; seqNo:18425; depth 24.40 2021-02-13T01:05:20Z- handleDvl() - RDI_Pathfinder msg: 1613178320.83 epoch sec; seqNo:8934 2021-02-13T01:05:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.94 , 92.10 , 62.94 , 109.14 2021-02-13T01:05:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , 0.03 2021-02-13T01:05:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178320.84 epoch sec; seqNo:18425; 4060802.01 north; 593659.18 east 2021-02-13T01:05:21Z- AHRS_M2 msg: 1613178321.21 epoch sec; seqNo:18281 2021-02-13T01:05:21Z- AHRS_M2 msg: 0.60 phi; -0.41 theta; -0.21 psi 2021-02-13T01:05:21Z- handleDepth()-Depth_Keller msg: 1613178321214 epoch sec; seqNo:18426; depth 24.55 2021-02-13T01:05:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178321.23 epoch sec; seqNo:18426; 4060802.39 north; 593659.18 east 2021-02-13T01:05:21Z- AHRS_M2 msg: 1613178321.61 epoch sec; seqNo:18282 2021-02-13T01:05:21Z- AHRS_M2 msg: 0.59 phi; -0.41 theta; -0.21 psi 2021-02-13T01:05:21Z- handleDepth()-Depth_Keller msg: 1613178321618 epoch sec; seqNo:18427; depth 24.70 2021-02-13T01:05:21Z- handleDvl() - RDI_Pathfinder msg: 1613178321.63 epoch sec; seqNo:8935 2021-02-13T01:05:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.78 , 98.26 , 63.35 , 110.43 2021-02-13T01:05:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.10 , 0.08 2021-02-13T01:05:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178321.64 epoch sec; seqNo:18427; 4060802.39 north; 593659.18 east 2021-02-13T01:05:22Z- AHRS_M2 msg: 1613178322.02 epoch sec; seqNo:18283 2021-02-13T01:05:22Z- AHRS_M2 msg: 0.59 phi; -0.41 theta; -0.21 psi 2021-02-13T01:05:22Z- handleDepth()-Depth_Keller msg: 1613178322020 epoch sec; seqNo:18428; depth 24.85 2021-02-13T01:05:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178322.04 epoch sec; seqNo:18428; 4060802.77 north; 593659.17 east 2021-02-13T01:05:22Z- AHRS_M2 msg: 1613178322.42 epoch sec; seqNo:18284 2021-02-13T01:05:22Z- AHRS_M2 msg: 0.58 phi; -0.41 theta; -0.21 psi 2021-02-13T01:05:22Z- handleDepth()-Depth_Keller msg: 1613178322426 epoch sec; seqNo:18429; depth 25.00 2021-02-13T01:05:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178322.43 epoch sec; seqNo:18429; 4060803.15 north; 593659.17 east 2021-02-13T01:05:22Z- AHRS_M2 msg: 1613178322.82 epoch sec; seqNo:18285 2021-02-13T01:05:22Z- AHRS_M2 msg: 0.58 phi; -0.41 theta; -0.21 psi 2021-02-13T01:05:22Z- handleDepth()-Depth_Keller msg: 1613178322828 epoch sec; seqNo:18430; depth 25.14 2021-02-13T01:05:22Z- handleDvl() - RDI_Pathfinder msg: 1613178322.84 epoch sec; seqNo:8936 2021-02-13T01:05:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.99 , 64.58 , 107.73 2021-02-13T01:05:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.09 2021-02-13T01:05:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178322.85 epoch sec; seqNo:18430; 4060803.15 north; 593659.17 east 2021-02-13T01:05:23Z- AHRS_M2 msg: 1613178323.23 epoch sec; seqNo:18286 2021-02-13T01:05:23Z- AHRS_M2 msg: 0.58 phi; -0.41 theta; -0.21 psi 2021-02-13T01:05:23Z- handleDepth()-Depth_Keller msg: 1613178323234 epoch sec; seqNo:18431; depth 25.30 2021-02-13T01:05:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178323.24 epoch sec; seqNo:18431; 4060803.53 north; 593659.16 east 2021-02-13T01:05:23Z- AHRS_M2 msg: 1613178323.66 epoch sec; seqNo:18287 2021-02-13T01:05:23Z- AHRS_M2 msg: 0.57 phi; -0.41 theta; -0.21 psi 2021-02-13T01:05:23Z- handleDepth()-Depth_Keller msg: 1613178323662 epoch sec; seqNo:18432; depth 25.46 2021-02-13T01:05:23Z- handleDvl() - RDI_Pathfinder msg: 1613178323.67 epoch sec; seqNo:8937 2021-02-13T01:05:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.72 , 91.90 , 63.36 , 108.95 2021-02-13T01:05:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.05 2021-02-13T01:05:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178323.68 epoch sec; seqNo:18432; 4060803.92 north; 593660.38 east 2021-02-13T01:05:24Z- AHRS_M2 msg: 1613178324.04 epoch sec; seqNo:18288 2021-02-13T01:05:24Z- AHRS_M2 msg: 0.57 phi; -0.41 theta; -0.21 psi 2021-02-13T01:05:24Z- handleDepth()-Depth_Keller msg: 1613178324040 epoch sec; seqNo:18433; depth 25.61 2021-02-13T01:05:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178324.05 epoch sec; seqNo:18433; 4060804.30 north; 593660.38 east 2021-02-13T01:05:24Z- LcmTrn::handleMessages() - handling 41 messages took 0.59 ms and 130 calls... 2021-02-13T01:05:24Z- waiting for 1613178322.22, time is 1613178324.05 2021-02-13T01:05:24Z- LcmTrn::updateTrn() >>>> pose time 1613178324.05, meas time 1613178323.67 <<<< 2021-02-13T01:05:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178323.67 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:05:24Z- TerrainNav::motionUpdate() - currEstimate = (4060804, 593660.38, 25.61) 2021-02-13T01:05:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:05:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:05:24Z- TNavBF::Projecting Measurements 2021-02-13T01:05:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:05:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:05:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:05:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:05:24Z- TNavFilter::4 good beams. 2021-02-13T01:05:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:05:24Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:05:24Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:05:24Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:05:24Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:05:24Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:05:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:05:24Z- TNavBF::mmse selected from filter -1 2021-02-13T01:05:24Z- TerrainMapOctree: (vehN, vehE ) = (4060804.03, 593660.19). 2021-02-13T01:05:24Z- TerrainMapOctree: Min Distance = 739.44. 2021-02-13T01:05:24Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:05:24Z- There is no specific good beam that all particles have in common. 2021-02-13T01:05:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:05:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-13T01:05:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:05:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 13.51 2021-02-13T01:05:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:05:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 6.42 2021-02-13T01:05:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:05:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 18.99 2021-02-13T01:05:24Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:05:24Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:05:24Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:05:24Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:05:24Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:05:24Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:05:24Z- Filter 0 No beams used in subcloud update 2021-02-13T01:05:24Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:05:24Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:05:24Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 676.4 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T01:05:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178323.67 sec. 2021-02-13T01:05:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:05:24Z- TNavBF::mmse selected from filter -1 2021-02-13T01:05:24Z- variances([0], [2], and psi_berg) = 676.396747 832.986978 0.000000 2021-02-13T01:05:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:05:24Z- LcmTrn::publishEstimates() - MLE : 4060803.80 593647.88 25.61 2021-02-13T01:05:24Z- LcmTrn::publishEstimates() - MMSE : 4060804.30 593660.38 25.61 2021-02-13T01:05:24Z- LcmTrn::publishEstimates() - COVAR: 676.40 832.99 0.84 0.00 2021-02-13T01:05:24Z- AHRS_M2 msg: 1613178324.44 epoch sec; seqNo:18289 2021-02-13T01:05:24Z- AHRS_M2 msg: 0.58 phi; -0.41 theta; -0.21 psi 2021-02-13T01:05:24Z- handleDepth()-Depth_Keller msg: 1613178324446 epoch sec; seqNo:18434; depth 25.76 2021-02-13T01:05:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178324.45 epoch sec; seqNo:18434; 4060804.30 north; 593660.38 east 2021-02-13T01:05:24Z- AHRS_M2 msg: 1613178324.84 epoch sec; seqNo:18290 2021-02-13T01:05:24Z- AHRS_M2 msg: 0.58 phi; -0.40 theta; -0.21 psi 2021-02-13T01:05:24Z- handleDepth()-Depth_Keller msg: 1613178324850 epoch sec; seqNo:18435; depth 25.91 2021-02-13T01:05:24Z- handleDvl() - RDI_Pathfinder msg: 1613178324.86 epoch sec; seqNo:8938 2021-02-13T01:05:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 89.80 , 61.72 , 108.06 2021-02-13T01:05:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , 0.02 2021-02-13T01:05:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178324.87 epoch sec; seqNo:18435; 4060804.68 north; 593660.37 east 2021-02-13T01:05:25Z- AHRS_M2 msg: 1613178325.25 epoch sec; seqNo:18291 2021-02-13T01:05:25Z- AHRS_M2 msg: 0.58 phi; -0.40 theta; -0.21 psi 2021-02-13T01:05:25Z- handleDepth()-Depth_Keller msg: 1613178325257 epoch sec; seqNo:18436; depth 26.06 2021-02-13T01:05:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178325.27 epoch sec; seqNo:18436; 4060805.05 north; 593660.37 east 2021-02-13T01:05:25Z- AHRS_M2 msg: 1613178325.65 epoch sec; seqNo:18292 2021-02-13T01:05:25Z- AHRS_M2 msg: 0.58 phi; -0.40 theta; -0.20 psi 2021-02-13T01:05:25Z- handleDepth()-Depth_Keller msg: 1613178325661 epoch sec; seqNo:18437; depth 26.22 2021-02-13T01:05:25Z- handleDvl() - RDI_Pathfinder msg: 1613178325.67 epoch sec; seqNo:8939 2021-02-13T01:05:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.78 , 0.00 , 62.53 , 104.34 2021-02-13T01:05:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , -0.03 2021-02-13T01:05:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178325.68 epoch sec; seqNo:18437; 4060805.05 north; 593660.37 east 2021-02-13T01:05:26Z- AHRS_M2 msg: 1613178326.06 epoch sec; seqNo:18293 2021-02-13T01:05:26Z- AHRS_M2 msg: 0.58 phi; -0.40 theta; -0.20 psi 2021-02-13T01:05:26Z- handleDepth()-Depth_Keller msg: 1613178326062 epoch sec; seqNo:18438; depth 26.36 2021-02-13T01:05:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178326.07 epoch sec; seqNo:18438; 4060805.43 north; 593660.36 east 2021-02-13T01:05:26Z- AHRS_M2 msg: 1613178326.46 epoch sec; seqNo:18294 2021-02-13T01:05:26Z- AHRS_M2 msg: 0.58 phi; -0.39 theta; -0.20 psi 2021-02-13T01:05:26Z- handleDepth()-Depth_Keller msg: 1613178326469 epoch sec; seqNo:18439; depth 26.51 2021-02-13T01:05:26Z- handleDvl() - RDI_Pathfinder msg: 1613178326.48 epoch sec; seqNo:8940 2021-02-13T01:05:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.67 , 0.00 , 62.13 , 102.11 2021-02-13T01:05:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.08 , -0.05 2021-02-13T01:05:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178326.50 epoch sec; seqNo:18439; 4060805.83 north; 593661.58 east 2021-02-13T01:05:26Z- AHRS_M2 msg: 1613178326.86 epoch sec; seqNo:18295 2021-02-13T01:05:26Z- AHRS_M2 msg: 0.58 phi; -0.39 theta; -0.20 psi 2021-02-13T01:05:26Z- handleDepth()-Depth_Keller msg: 1613178326873 epoch sec; seqNo:18440; depth 26.66 2021-02-13T01:05:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178326.88 epoch sec; seqNo:18440; 4060806.20 north; 593661.58 east 2021-02-13T01:05:27Z- AHRS_M2 msg: 1613178327.27 epoch sec; seqNo:18296 2021-02-13T01:05:27Z- AHRS_M2 msg: 0.58 phi; -0.39 theta; -0.19 psi 2021-02-13T01:05:27Z- handleDepth()-Depth_Keller msg: 1613178327274 epoch sec; seqNo:18441; depth 26.81 2021-02-13T01:05:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178327.28 epoch sec; seqNo:18441; 4060806.58 north; 593661.57 east 2021-02-13T01:05:27Z- AHRS_M2 msg: 1613178327.67 epoch sec; seqNo:18297 2021-02-13T01:05:27Z- AHRS_M2 msg: 0.58 phi; -0.38 theta; -0.19 psi 2021-02-13T01:05:27Z- handleDepth()-Depth_Keller msg: 1613178327676 epoch sec; seqNo:18442; depth 26.95 2021-02-13T01:05:27Z- handleDvl() - RDI_Pathfinder msg: 1613178327.69 epoch sec; seqNo:8941 2021-02-13T01:05:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 151.00 , 60.49 , 102.31 2021-02-13T01:05:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , -0.05 2021-02-13T01:05:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178327.70 epoch sec; seqNo:18442; 4060806.58 north; 593661.57 east 2021-02-13T01:05:28Z- AHRS_M2 msg: 1613178328.08 epoch sec; seqNo:18298 2021-02-13T01:05:28Z- AHRS_M2 msg: 0.58 phi; -0.38 theta; -0.19 psi 2021-02-13T01:05:28Z- handleDepth()-Depth_Keller msg: 1613178328080 epoch sec; seqNo:18443; depth 27.10 2021-02-13T01:05:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178328.09 epoch sec; seqNo:18443; 4060806.96 north; 593661.57 east 2021-02-13T01:05:28Z- AHRS_M2 msg: 1613178328.48 epoch sec; seqNo:18299 2021-02-13T01:05:28Z- AHRS_M2 msg: 0.59 phi; -0.38 theta; -0.18 psi 2021-02-13T01:05:28Z- handleDepth()-Depth_Keller msg: 1613178328493 epoch sec; seqNo:18444; depth 27.25 2021-02-13T01:05:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178328.52 epoch sec; seqNo:18444; 4060807.34 north; 593661.56 east 2021-02-13T01:05:28Z- AHRS_M2 msg: 1613178328.88 epoch sec; seqNo:18300 2021-02-13T01:05:28Z- AHRS_M2 msg: 0.59 phi; -0.38 theta; -0.18 psi 2021-02-13T01:05:28Z- handleDepth()-Depth_Keller msg: 1613178328890 epoch sec; seqNo:18445; depth 27.41 2021-02-13T01:05:28Z- handleDvl() - RDI_Pathfinder msg: 1613178328.91 epoch sec; seqNo:8942 2021-02-13T01:05:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.26 , 87.31 , 61.31 , 100.70 2021-02-13T01:05:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , -0.05 2021-02-13T01:05:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178328.92 epoch sec; seqNo:18445; 4060807.72 north; 593661.56 east 2021-02-13T01:05:29Z- AHRS_M2 msg: 1613178329.29 epoch sec; seqNo:18301 2021-02-13T01:05:29Z- AHRS_M2 msg: 0.58 phi; -0.37 theta; -0.18 psi 2021-02-13T01:05:29Z- handleDepth()-Depth_Keller msg: 1613178329297 epoch sec; seqNo:18446; depth 27.56 2021-02-13T01:05:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178329.32 epoch sec; seqNo:18446; 4060808.11 north; 593662.78 east 2021-02-13T01:05:29Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 132 calls... 2021-02-13T01:05:29Z- waiting for 1613178327.05, time is 1613178329.32 2021-02-13T01:05:29Z- LcmTrn::updateTrn() >>>> pose time 1613178329.32, meas time 1613178328.91 <<<< 2021-02-13T01:05:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178328.91 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:05:29Z- TerrainNav::motionUpdate() - currEstimate = (4060808, 593662.78, 27.56) 2021-02-13T01:05:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:05:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:05:29Z- TNavBF::Projecting Measurements 2021-02-13T01:05:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:05:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:05:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:05:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:05:29Z- TNavFilter::4 good beams. 2021-02-13T01:05:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:05:29Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:05:29Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:05:29Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:05:29Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:05:29Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:05:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:05:29Z- TNavBF::mmse selected from filter -1 2021-02-13T01:05:29Z- TerrainMapOctree: (vehN, vehE ) = (4060807.82, 593662.59). 2021-02-13T01:05:29Z- TerrainMapOctree: Min Distance = 735.24. 2021-02-13T01:05:29Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:05:29Z- There is no specific good beam that all particles have in common. 2021-02-13T01:05:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:05:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 7.46 2021-02-13T01:05:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:05:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 12.20 2021-02-13T01:05:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:05:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 6.02 2021-02-13T01:05:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:05:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.79 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 16.22 2021-02-13T01:05:29Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:05:29Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:05:29Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:05:29Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:05:29Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:05:29Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:05:29Z- Filter 0 No beams used in subcloud update 2021-02-13T01:05:29Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:05:29Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:05:29Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 676.4 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T01:05:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178328.91 sec. 2021-02-13T01:05:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:05:29Z- TNavBF::mmse selected from filter -1 2021-02-13T01:05:29Z- variances([0], [2], and psi_berg) = 676.341126 832.986955 0.000000 2021-02-13T01:05:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:05:29Z- LcmTrn::publishEstimates() - MLE : 4060807.61 593650.28 27.56 2021-02-13T01:05:29Z- LcmTrn::publishEstimates() - MMSE : 4060808.11 593662.78 27.56 2021-02-13T01:05:29Z- LcmTrn::publishEstimates() - COVAR: 676.34 832.99 0.78 0.00 2021-02-13T01:05:29Z- AHRS_M2 msg: 1613178329.69 epoch sec; seqNo:18302 2021-02-13T01:05:29Z- AHRS_M2 msg: 0.58 phi; -0.37 theta; -0.18 psi 2021-02-13T01:05:29Z- handleDepth()-Depth_Keller msg: 1613178329696 epoch sec; seqNo:18447; depth 27.69 2021-02-13T01:05:29Z- handleDvl() - RDI_Pathfinder msg: 1613178329.71 epoch sec; seqNo:8943 2021-02-13T01:05:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 0.00 , 62.13 , 102.32 2021-02-13T01:05:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.10 , -0.04 2021-02-13T01:05:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178329.72 epoch sec; seqNo:18447; 4060808.11 north; 593662.78 east 2021-02-13T01:05:30Z- AHRS_M2 msg: 1613178330.10 epoch sec; seqNo:18303 2021-02-13T01:05:30Z- AHRS_M2 msg: 0.58 phi; -0.37 theta; -0.18 psi 2021-02-13T01:05:30Z- handleDepth()-Depth_Keller msg: 1613178330100 epoch sec; seqNo:18448; depth 27.82 2021-02-13T01:05:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178330.12 epoch sec; seqNo:18448; 4060808.49 north; 593662.77 east 2021-02-13T01:05:30Z- AHRS_M2 msg: 1613178330.50 epoch sec; seqNo:18304 2021-02-13T01:05:30Z- AHRS_M2 msg: 0.58 phi; -0.37 theta; -0.18 psi 2021-02-13T01:05:30Z- handleDepth()-Depth_Keller msg: 1613178330509 epoch sec; seqNo:18449; depth 27.97 2021-02-13T01:05:30Z- handleDvl() - RDI_Pathfinder msg: 1613178330.52 epoch sec; seqNo:8944 2021-02-13T01:05:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:05:30Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:05:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178330.56 epoch sec; seqNo:18449; 4060808.87 north; 593662.77 east 2021-02-13T01:05:30Z- AHRS_M2 msg: 1613178330.90 epoch sec; seqNo:18305 2021-02-13T01:05:30Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.18 psi 2021-02-13T01:05:30Z- handleDepth()-Depth_Keller msg: 1613178330908 epoch sec; seqNo:18450; depth 28.13 2021-02-13T01:05:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178330.92 epoch sec; seqNo:18450; 4060808.87 north; 593662.77 east 2021-02-13T01:05:31Z- AHRS_M2 msg: 1613178331.31 epoch sec; seqNo:18306 2021-02-13T01:05:31Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.18 psi 2021-02-13T01:05:31Z- handleDepth()-Depth_Keller msg: 1613178331313 epoch sec; seqNo:18451; depth 28.25 2021-02-13T01:05:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178331.32 epoch sec; seqNo:18451; 4060809.25 north; 593662.76 east 2021-02-13T01:05:31Z- AHRS_M2 msg: 1613178331.71 epoch sec; seqNo:18307 2021-02-13T01:05:31Z- AHRS_M2 msg: 0.57 phi; -0.36 theta; -0.18 psi 2021-02-13T01:05:31Z- handleDepth()-Depth_Keller msg: 1613178331726 epoch sec; seqNo:18452; depth 28.40 2021-02-13T01:05:31Z- handleDvl() - RDI_Pathfinder msg: 1613178331.74 epoch sec; seqNo:8945 2021-02-13T01:05:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.85 , 83.79 , 61.31 , 97.85 2021-02-13T01:05:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , 0.02 2021-02-13T01:05:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178331.75 epoch sec; seqNo:18452; 4060809.64 north; 593663.98 east 2021-02-13T01:05:32Z- AHRS_M2 msg: 1613178332.12 epoch sec; seqNo:18308 2021-02-13T01:05:32Z- AHRS_M2 msg: 0.57 phi; -0.36 theta; -0.18 psi 2021-02-13T01:05:32Z- handleDepth()-Depth_Keller msg: 1613178332121 epoch sec; seqNo:18453; depth 28.53 2021-02-13T01:05:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178332.15 epoch sec; seqNo:18453; 4060810.02 north; 593663.98 east 2021-02-13T01:05:32Z- AHRS_M2 msg: 1613178332.52 epoch sec; seqNo:18309 2021-02-13T01:05:32Z- AHRS_M2 msg: 0.57 phi; -0.36 theta; -0.18 psi 2021-02-13T01:05:32Z- handleDepth()-Depth_Keller msg: 1613178332529 epoch sec; seqNo:18454; depth 28.66 2021-02-13T01:05:32Z- handleDvl() - RDI_Pathfinder msg: 1613178332.54 epoch sec; seqNo:8946 2021-02-13T01:05:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:05:32Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:05:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178332.55 epoch sec; seqNo:18454; 4060810.02 north; 593663.98 east 2021-02-13T01:05:32Z- AHRS_M2 msg: 1613178332.93 epoch sec; seqNo:18310 2021-02-13T01:05:32Z- AHRS_M2 msg: 0.57 phi; -0.35 theta; -0.18 psi 2021-02-13T01:05:32Z- handleDepth()-Depth_Keller msg: 1613178332935 epoch sec; seqNo:18455; depth 28.81 2021-02-13T01:05:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178332.95 epoch sec; seqNo:18455; 4060810.40 north; 593663.97 east 2021-02-13T01:05:33Z- AHRS_M2 msg: 1613178333.33 epoch sec; seqNo:18311 2021-02-13T01:05:33Z- AHRS_M2 msg: 0.57 phi; -0.34 theta; -0.18 psi 2021-02-13T01:05:33Z- handleDepth()-Depth_Keller msg: 1613178333337 epoch sec; seqNo:18456; depth 28.94 2021-02-13T01:05:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178333.35 epoch sec; seqNo:18456; 4060810.78 north; 593663.97 east 2021-02-13T01:05:33Z- AHRS_M2 msg: 1613178333.73 epoch sec; seqNo:18312 2021-02-13T01:05:33Z- AHRS_M2 msg: 0.56 phi; -0.33 theta; -0.18 psi 2021-02-13T01:05:33Z- handleDepth()-Depth_Keller msg: 1613178333737 epoch sec; seqNo:18457; depth 29.08 2021-02-13T01:05:33Z- handleDvl() - RDI_Pathfinder msg: 1613178333.75 epoch sec; seqNo:8947 2021-02-13T01:05:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.81 , 0.00 , 163.79 , 98.06 2021-02-13T01:05:33Z- handleDvl() - RDI_Pathfinder msg: velocities 1.45 , 0.57 , -0.31 2021-02-13T01:05:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178333.78 epoch sec; seqNo:18457; 4060811.17 north; 593665.18 east 2021-02-13T01:05:34Z- AHRS_M2 msg: 1613178334.14 epoch sec; seqNo:18313 2021-02-13T01:05:34Z- AHRS_M2 msg: 0.56 phi; -0.31 theta; -0.18 psi 2021-02-13T01:05:34Z- handleDepth()-Depth_Keller msg: 1613178334145 epoch sec; seqNo:18458; depth 29.22 2021-02-13T01:05:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178334.16 epoch sec; seqNo:18458; 4060811.55 north; 593665.18 east 2021-02-13T01:05:34Z- AHRS_M2 msg: 1613178334.55 epoch sec; seqNo:18314 2021-02-13T01:05:34Z- AHRS_M2 msg: 0.55 phi; -0.27 theta; -0.18 psi 2021-02-13T01:05:34Z- handleDepth()-Depth_Keller msg: 1613178334552 epoch sec; seqNo:18459; depth 29.36 2021-02-13T01:05:34Z- handleDvl() - RDI_Pathfinder msg: 1613178334.57 epoch sec; seqNo:8948 2021-02-13T01:05:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.65 , 0.00 , 58.01 , 91.36 2021-02-13T01:05:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.01 2021-02-13T01:05:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178334.58 epoch sec; seqNo:18459; 4060811.93 north; 593665.18 east 2021-02-13T01:05:34Z- LcmTrn::handleMessages() - handling 45 messages took 0.73 ms and 131 calls... 2021-02-13T01:05:34Z- waiting for 1613178332.32, time is 1613178334.58 2021-02-13T01:05:34Z- LcmTrn::updateTrn() equating pose 1613178334.58 and meas times 1613178334.58 2021-02-13T01:05:34Z- LcmTrn::updateTrn() >>>> pose time 1613178334.58, meas time 1613178334.58 <<<< 2021-02-13T01:05:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178334.58 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:05:34Z- TerrainNav::motionUpdate() - currEstimate = (4060812, 593665.18, 29.36) 2021-02-13T01:05:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:05:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:05:34Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178334.58 sec.; No relevant navigation data available 2021-02-13T01:05:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:05:34Z- TNavBF::mmse selected from filter -1 2021-02-13T01:05:34Z- variances([0], [2], and psi_berg) = 676.296258 832.986937 0.000000 2021-02-13T01:05:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:05:34Z- LcmTrn::publishEstimates() - MLE : 4060811.43 593682.68 29.36 2021-02-13T01:05:34Z- LcmTrn::publishEstimates() - MMSE : 4060811.93 593665.18 29.36 2021-02-13T01:05:34Z- LcmTrn::publishEstimates() - COVAR: 676.30 832.99 1.01 0.00 2021-02-13T01:05:34Z- AHRS_M2 msg: 1613178334.94 epoch sec; seqNo:18315 2021-02-13T01:05:34Z- AHRS_M2 msg: 0.55 phi; -0.24 theta; -0.17 psi 2021-02-13T01:05:34Z- handleDepth()-Depth_Keller msg: 1613178334949 epoch sec; seqNo:18460; depth 29.50 2021-02-13T01:05:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178334.97 epoch sec; seqNo:18460; 4060812.31 north; 593665.17 east 2021-02-13T01:05:35Z- AHRS_M2 msg: 1613178335.35 epoch sec; seqNo:18316 2021-02-13T01:05:35Z- AHRS_M2 msg: 0.54 phi; -0.19 theta; -0.15 psi 2021-02-13T01:05:35Z- handleDepth()-Depth_Keller msg: 1613178335353 epoch sec; seqNo:18461; depth 29.64 2021-02-13T01:05:35Z- handleDvl() - RDI_Pathfinder msg: 1613178335.36 epoch sec; seqNo:8949 2021-02-13T01:05:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:05:35Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:05:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178335.38 epoch sec; seqNo:18461; 4060812.31 north; 593665.17 east 2021-02-13T01:05:35Z- AHRS_M2 msg: 1613178335.75 epoch sec; seqNo:18317 2021-02-13T01:05:35Z- AHRS_M2 msg: 0.54 phi; -0.15 theta; -0.15 psi 2021-02-13T01:05:35Z- handleDepth()-Depth_Keller msg: 1613178335765 epoch sec; seqNo:18462; depth 29.78 2021-02-13T01:05:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178335.78 epoch sec; seqNo:18462; 4060812.69 north; 593665.17 east 2021-02-13T01:05:36Z- AHRS_M2 msg: 1613178336.17 epoch sec; seqNo:18318 2021-02-13T01:05:36Z- AHRS_M2 msg: 0.53 phi; -0.11 theta; -0.14 psi 2021-02-13T01:05:36Z- handleDepth()-Depth_Keller msg: 1613178336176 epoch sec; seqNo:18463; depth 29.88 2021-02-13T01:05:36Z- handleDvl() - RDI_Pathfinder msg: 1613178336.19 epoch sec; seqNo:8950 2021-02-13T01:05:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.91 , 0.00 , 63.35 , 0.00 2021-02-13T01:05:36Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:05:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178336.20 epoch sec; seqNo:18463; 4060813.08 north; 593666.38 east 2021-02-13T01:05:36Z- AHRS_M2 msg: 1613178336.56 epoch sec; seqNo:18319 2021-02-13T01:05:36Z- AHRS_M2 msg: 0.52 phi; -0.07 theta; -0.14 psi 2021-02-13T01:05:36Z- handleDepth()-Depth_Keller msg: 1613178336564 epoch sec; seqNo:18464; depth 30.00 2021-02-13T01:05:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178336.58 epoch sec; seqNo:18464; 4060813.46 north; 593666.38 east 2021-02-13T01:05:36Z- AHRS_M2 msg: 1613178336.96 epoch sec; seqNo:18320 2021-02-13T01:05:36Z- AHRS_M2 msg: 0.51 phi; -0.02 theta; -0.14 psi 2021-02-13T01:05:36Z- handleDepth()-Depth_Keller msg: 1613178336973 epoch sec; seqNo:18465; depth 30.08 2021-02-13T01:05:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178336.98 epoch sec; seqNo:18465; 4060813.83 north; 593666.38 east 2021-02-13T01:05:37Z- AHRS_M2 msg: 1613178337.37 epoch sec; seqNo:18321 2021-02-13T01:05:37Z- AHRS_M2 msg: 0.51 phi; 0.02 theta; -0.13 psi 2021-02-13T01:05:37Z- handleDepth()-Depth_Keller msg: 1613178337373 epoch sec; seqNo:18466; depth 30.17 2021-02-13T01:05:37Z- handleDvl() - RDI_Pathfinder msg: 1613178337.39 epoch sec; seqNo:8951 2021-02-13T01:05:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.47 , 0.00 , 66.90 , 0.00 2021-02-13T01:05:37Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:05:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178337.40 epoch sec; seqNo:18466; 4060813.83 north; 593666.38 east 2021-02-13T01:05:37Z- AHRS_M2 msg: 1613178337.77 epoch sec; seqNo:18322 2021-02-13T01:05:37Z- AHRS_M2 msg: 0.50 phi; 0.05 theta; -0.13 psi 2021-02-13T01:05:37Z- handleDepth()-Depth_Keller msg: 1613178337776 epoch sec; seqNo:18467; depth 30.23 2021-02-13T01:05:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178337.79 epoch sec; seqNo:18467; 4060814.21 north; 593666.37 east 2021-02-13T01:05:38Z- AHRS_M2 msg: 1613178338.18 epoch sec; seqNo:18323 2021-02-13T01:05:38Z- AHRS_M2 msg: 0.50 phi; 0.09 theta; -0.13 psi 2021-02-13T01:05:38Z- handleDepth()-Depth_Keller msg: 1613178338180 epoch sec; seqNo:18468; depth 30.27 2021-02-13T01:05:38Z- handleDvl() - RDI_Pathfinder msg: 1613178338.19 epoch sec; seqNo:8952 2021-02-13T01:05:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 62.46 , 0.00 , 0.00 , 64.68 2021-02-13T01:05:38Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:05:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178338.24 epoch sec; seqNo:18468; 4060814.59 north; 593666.37 east 2021-02-13T01:05:38Z- AHRS_M2 msg: 1613178338.58 epoch sec; seqNo:18324 2021-02-13T01:05:38Z- AHRS_M2 msg: 0.50 phi; 0.12 theta; -0.12 psi 2021-02-13T01:05:38Z- handleDepth()-Depth_Keller msg: 1613178338589 epoch sec; seqNo:18469; depth 30.32 2021-02-13T01:05:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178338.60 epoch sec; seqNo:18469; 4060814.97 north; 593666.36 east 2021-02-13T01:05:38Z- AHRS_M2 msg: 1613178338.98 epoch sec; seqNo:18325 2021-02-13T01:05:38Z- AHRS_M2 msg: 0.49 phi; 0.15 theta; -0.12 psi 2021-02-13T01:05:38Z- handleDepth()-Depth_Keller msg: 1613178338997 epoch sec; seqNo:18470; depth 30.35 2021-02-13T01:05:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178339.01 epoch sec; seqNo:18470; 4060815.36 north; 593667.58 east 2021-02-13T01:05:39Z- AHRS_M2 msg: 1613178339.39 epoch sec; seqNo:18326 2021-02-13T01:05:39Z- AHRS_M2 msg: 0.49 phi; 0.18 theta; -0.11 psi 2021-02-13T01:05:39Z- handleDepth()-Depth_Keller msg: 1613178339397 epoch sec; seqNo:18471; depth 30.35 2021-02-13T01:05:39Z- handleDvl() - RDI_Pathfinder msg: 1613178339.41 epoch sec; seqNo:8953 2021-02-13T01:05:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:05:39Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:05:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178339.43 epoch sec; seqNo:18471; 4060815.74 north; 593667.57 east 2021-02-13T01:05:39Z- LcmTrn::handleMessages() - handling 41 messages took 0.68 ms and 128 calls... 2021-02-13T01:05:39Z- waiting for 1613178337.58, time is 1613178339.43 2021-02-13T01:05:39Z- LcmTrn::updateTrn() equating pose 1613178339.43 and meas times 1613178339.43 2021-02-13T01:05:39Z- LcmTrn::updateTrn() >>>> pose time 1613178339.43, meas time 1613178339.43 <<<< 2021-02-13T01:05:39Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613178339.43, ping # 0. 2021-02-13T01:05:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178339.43 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:05:39Z- TerrainNav::motionUpdate() - currEstimate = (4060816, 593667.57, 30.35) 2021-02-13T01:05:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:05:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:05:39Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:05:39Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:05:39Z- TNavBF::Projecting Measurements 2021-02-13T01:05:39Z- TNavFilter::Measurement from time = 1613178339.43 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T01:05:39Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:05:39Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:05:39Z- TNavBF:: beamIndex[1] = 0 2021-02-13T01:05:39Z- TNavBF:: beamIndex[2] = 0 2021-02-13T01:05:39Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:05:39Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:05:39Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:05:39Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 676.3 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T01:05:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:05:39Z- TNavBF::mmse selected from filter -1 2021-02-13T01:05:39Z- variances([0], [2], and psi_berg) = 676.251412 832.986919 0.000000 2021-02-13T01:05:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:05:39Z- LcmTrn::publishEstimates() - MLE : 4060815.24 593685.07 30.35 2021-02-13T01:05:39Z- LcmTrn::publishEstimates() - MMSE : 4060815.74 593667.57 30.35 2021-02-13T01:05:39Z- LcmTrn::publishEstimates() - COVAR: 676.25 832.99 1.23 0.00 2021-02-13T01:05:39Z- AHRS_M2 msg: 1613178339.79 epoch sec; seqNo:18327 2021-02-13T01:05:39Z- AHRS_M2 msg: 0.50 phi; 0.21 theta; -0.11 psi 2021-02-13T01:05:39Z- handleDepth()-Depth_Keller msg: 1613178339796 epoch sec; seqNo:18472; depth 30.35 2021-02-13T01:05:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178339.81 epoch sec; seqNo:18472; 4060815.74 north; 593667.57 east 2021-02-13T01:05:40Z- AHRS_M2 msg: 1613178340.20 epoch sec; seqNo:18328 2021-02-13T01:05:40Z- AHRS_M2 msg: 0.50 phi; 0.23 theta; -0.11 psi 2021-02-13T01:05:40Z- handleDepth()-Depth_Keller msg: 1613178340206 epoch sec; seqNo:18473; depth 30.32 2021-02-13T01:05:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178340.22 epoch sec; seqNo:18473; 4060816.12 north; 593667.57 east 2021-02-13T01:05:40Z- AHRS_M2 msg: 1613178340.61 epoch sec; seqNo:18329 2021-02-13T01:05:40Z- AHRS_M2 msg: 0.50 phi; 0.25 theta; -0.10 psi 2021-02-13T01:05:40Z- handleDepth()-Depth_Keller msg: 1613178340610 epoch sec; seqNo:18474; depth 30.29 2021-02-13T01:05:40Z- handleDvl() - RDI_Pathfinder msg: 1613178340.62 epoch sec; seqNo:8954 2021-02-13T01:05:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.99 , 72.76 , 81.76 , 60.50 2021-02-13T01:05:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.05 2021-02-13T01:05:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178340.63 epoch sec; seqNo:18474; 4060816.50 north; 593667.57 east 2021-02-13T01:05:41Z- AHRS_M2 msg: 1613178341.00 epoch sec; seqNo:18330 2021-02-13T01:05:41Z- AHRS_M2 msg: 0.51 phi; 0.27 theta; -0.10 psi 2021-02-13T01:05:41Z- handleDepth()-Depth_Keller msg: 1613178341008 epoch sec; seqNo:18475; depth 30.25 2021-02-13T01:05:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178341.03 epoch sec; seqNo:18475; 4060816.88 north; 593667.56 east 2021-02-13T01:05:41Z- AHRS_M2 msg: 1613178341.42 epoch sec; seqNo:18331 2021-02-13T01:05:41Z- AHRS_M2 msg: 0.51 phi; 0.28 theta; -0.10 psi 2021-02-13T01:05:41Z- handleDepth()-Depth_Keller msg: 1613178341423 epoch sec; seqNo:18476; depth 30.19 2021-02-13T01:05:41Z- handleDvl() - RDI_Pathfinder msg: 1613178341.44 epoch sec; seqNo:8955 2021-02-13T01:05:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.04 , 0.00 , 132.62 , 60.09 2021-02-13T01:05:41Z- handleDvl() - RDI_Pathfinder msg: velocities -0.59 , -1.46 , 0.74 2021-02-13T01:05:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178341.45 epoch sec; seqNo:18476; 4060816.88 north; 593667.56 east 2021-02-13T01:05:41Z- AHRS_M2 msg: 1613178341.81 epoch sec; seqNo:18332 2021-02-13T01:05:41Z- AHRS_M2 msg: 0.52 phi; 0.30 theta; -0.10 psi 2021-02-13T01:05:41Z- handleDepth()-Depth_Keller msg: 1613178341816 epoch sec; seqNo:18477; depth 30.13 2021-02-13T01:05:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178341.83 epoch sec; seqNo:18477; 4060817.24 north; 593666.34 east 2021-02-13T01:05:42Z- AHRS_M2 msg: 1613178342.22 epoch sec; seqNo:18333 2021-02-13T01:05:42Z- AHRS_M2 msg: 0.52 phi; 0.31 theta; -0.09 psi 2021-02-13T01:05:42Z- handleDepth()-Depth_Keller msg: 1613178342226 epoch sec; seqNo:18478; depth 30.09 2021-02-13T01:05:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178342.23 epoch sec; seqNo:18478; 4060817.24 north; 593666.34 east 2021-02-13T01:05:42Z- AHRS_M2 msg: 1613178342.62 epoch sec; seqNo:18334 2021-02-13T01:05:42Z- AHRS_M2 msg: 0.52 phi; 0.32 theta; -0.09 psi 2021-02-13T01:05:42Z- handleDepth()-Depth_Keller msg: 1613178342634 epoch sec; seqNo:18479; depth 30.01 2021-02-13T01:05:42Z- handleDvl() - RDI_Pathfinder msg: 1613178342.64 epoch sec; seqNo:8956 2021-02-13T01:05:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 73.58 , 85.89 , 59.27 2021-02-13T01:05:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.03 , -0.03 2021-02-13T01:05:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178342.65 epoch sec; seqNo:18479; 4060817.62 north; 593666.33 east 2021-02-13T01:05:43Z- AHRS_M2 msg: 1613178343.02 epoch sec; seqNo:18335 2021-02-13T01:05:43Z- AHRS_M2 msg: 0.53 phi; 0.33 theta; -0.10 psi 2021-02-13T01:05:43Z- handleDepth()-Depth_Keller msg: 1613178343033 epoch sec; seqNo:18480; depth 29.93 2021-02-13T01:05:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178343.04 epoch sec; seqNo:18480; 4060817.62 north; 593666.33 east 2021-02-13T01:05:43Z- AHRS_M2 msg: 1613178343.43 epoch sec; seqNo:18336 2021-02-13T01:05:43Z- AHRS_M2 msg: 0.53 phi; 0.33 theta; -0.10 psi 2021-02-13T01:05:43Z- handleDepth()-Depth_Keller msg: 1613178343432 epoch sec; seqNo:18481; depth 29.86 2021-02-13T01:05:43Z- handleDvl() - RDI_Pathfinder msg: 1613178343.44 epoch sec; seqNo:8957 2021-02-13T01:05:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.45 , 78.51 , 0.00 , 60.09 2021-02-13T01:05:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.06 , 0.00 2021-02-13T01:05:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178343.45 epoch sec; seqNo:18481; 4060818.00 north; 593666.33 east 2021-02-13T01:05:43Z- AHRS_M2 msg: 1613178343.83 epoch sec; seqNo:18337 2021-02-13T01:05:43Z- AHRS_M2 msg: 0.53 phi; 0.34 theta; -0.10 psi 2021-02-13T01:05:43Z- handleDepth()-Depth_Keller msg: 1613178343845 epoch sec; seqNo:18482; depth 29.76 2021-02-13T01:05:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178343.86 epoch sec; seqNo:18482; 4060818.38 north; 593666.33 east 2021-02-13T01:05:44Z- AHRS_M2 msg: 1613178344.24 epoch sec; seqNo:18338 2021-02-13T01:05:44Z- AHRS_M2 msg: 0.53 phi; 0.35 theta; -0.10 psi 2021-02-13T01:05:44Z- handleDepth()-Depth_Keller msg: 1613178344257 epoch sec; seqNo:18483; depth 29.67 2021-02-13T01:05:44Z- handleDvl() - RDI_Pathfinder msg: 1613178344.27 epoch sec; seqNo:8958 2021-02-13T01:05:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 68.96 , 0.00 , 0.00 , 60.54 2021-02-13T01:05:44Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:05:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178344.28 epoch sec; seqNo:18483; 4060818.76 north; 593666.32 east 2021-02-13T01:05:44Z- AHRS_M2 msg: 1613178344.64 epoch sec; seqNo:18339 2021-02-13T01:05:44Z- AHRS_M2 msg: 0.53 phi; 0.35 theta; -0.10 psi 2021-02-13T01:05:44Z- handleDepth()-Depth_Keller msg: 1613178344644 epoch sec; seqNo:18484; depth 29.56 2021-02-13T01:05:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178344.65 epoch sec; seqNo:18484; 4060818.77 north; 593667.54 east 2021-02-13T01:05:44Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T01:05:44Z- waiting for 1613178342.43, time is 1613178344.65 2021-02-13T01:05:44Z- LcmTrn::updateTrn() >>>> pose time 1613178344.65, meas time 1613178344.27 <<<< 2021-02-13T01:05:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178344.27 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:05:44Z- TerrainNav::motionUpdate() - currEstimate = (4060819, 593667.54, 29.56) 2021-02-13T01:05:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:05:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:05:44Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:05:44Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:05:44Z- TNavBF::Projecting Measurements 2021-02-13T01:05:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:05:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:05:44Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-13T01:05:44Z- TNavFilter::2 good beams. 2021-02-13T01:05:44Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:05:44Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:05:44Z- TNavBF:: beamIndex[1] = 3 2021-02-13T01:05:44Z- TNavBF:: beamIndex[2] = 0 2021-02-13T01:05:44Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:05:44Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-13T01:05:44Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:05:44Z- TNavBF::mmse selected from filter -1 2021-02-13T01:05:44Z- TerrainMapOctree: (vehN, vehE ) = (4060818.55, 593667.54). 2021-02-13T01:05:44Z- TerrainMapOctree: Min Distance = 724.85. 2021-02-13T01:05:44Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:05:44Z- There is no specific good beam that all particles have in common. 2021-02-13T01:05:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:05:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.38 K: 0.129 ExpectedMeasDiff: nan currMeas.covariance: 7.61 2021-02-13T01:05:44Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:05:44Z- Old Depth Bias: 0.00 DepthBiasCov: 1.20 K: 0.136 ExpectedMeasDiff: nan currMeas.covariance: 5.86 2021-02-13T01:05:44Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:05:44Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:05:44Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:44Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:05:44Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:05:44Z- Filter 0 No beams used in subcloud update 2021-02-13T01:05:44Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:05:44Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:05:44Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 676.2 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T01:05:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178344.27 sec. 2021-02-13T01:05:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:05:44Z- TNavBF::mmse selected from filter -1 2021-02-13T01:05:44Z- variances([0], [2], and psi_berg) = 676.206051 832.986900 0.000000 2021-02-13T01:05:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:05:44Z- LcmTrn::publishEstimates() - MLE : 4060818.27 593684.04 29.56 2021-02-13T01:05:44Z- LcmTrn::publishEstimates() - MMSE : 4060818.77 593667.54 29.56 2021-02-13T01:05:44Z- LcmTrn::publishEstimates() - COVAR: 676.21 832.99 1.05 0.00 2021-02-13T01:05:45Z- AHRS_M2 msg: 1613178345.04 epoch sec; seqNo:18340 2021-02-13T01:05:45Z- AHRS_M2 msg: 0.54 phi; 0.35 theta; -0.10 psi 2021-02-13T01:05:45Z- handleDepth()-Depth_Keller msg: 1613178345048 epoch sec; seqNo:18485; depth 29.47 2021-02-13T01:05:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178345.06 epoch sec; seqNo:18485; 4060819.15 north; 593667.54 east 2021-02-13T01:05:45Z- AHRS_M2 msg: 1613178345.45 epoch sec; seqNo:18341 2021-02-13T01:05:45Z- AHRS_M2 msg: 0.54 phi; 0.36 theta; -0.09 psi 2021-02-13T01:05:45Z- handleDepth()-Depth_Keller msg: 1613178345453 epoch sec; seqNo:18486; depth 29.36 2021-02-13T01:05:45Z- handleDvl() - RDI_Pathfinder msg: 1613178345.46 epoch sec; seqNo:8959 2021-02-13T01:05:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 69.43 , 0.00 , 0.00 , 60.49 2021-02-13T01:05:45Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:05:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178345.48 epoch sec; seqNo:18486; 4060819.53 north; 593667.53 east 2021-02-13T01:05:45Z- AHRS_M2 msg: 1613178345.85 epoch sec; seqNo:18342 2021-02-13T01:05:45Z- AHRS_M2 msg: 0.55 phi; 0.36 theta; -0.09 psi 2021-02-13T01:05:45Z- handleDepth()-Depth_Keller msg: 1613178345857 epoch sec; seqNo:18487; depth 29.26 2021-02-13T01:05:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178345.88 epoch sec; seqNo:18487; 4060819.53 north; 593667.53 east 2021-02-13T01:05:46Z- AHRS_M2 msg: 1613178346.26 epoch sec; seqNo:18343 2021-02-13T01:05:46Z- AHRS_M2 msg: 0.55 phi; 0.36 theta; -0.08 psi 2021-02-13T01:05:46Z- handleDepth()-Depth_Keller msg: 1613178346265 epoch sec; seqNo:18488; depth 29.15 2021-02-13T01:05:46Z- handleDvl() - RDI_Pathfinder msg: 1613178346.29 epoch sec; seqNo:8960 2021-02-13T01:05:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 69.86 , 0.00 , 0.00 , 60.06 2021-02-13T01:05:46Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:05:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178346.30 epoch sec; seqNo:18488; 4060819.91 north; 593667.53 east 2021-02-13T01:05:46Z- AHRS_M2 msg: 1613178346.66 epoch sec; seqNo:18344 2021-02-13T01:05:46Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.08 psi 2021-02-13T01:05:46Z- handleDepth()-Depth_Keller msg: 1613178346669 epoch sec; seqNo:18489; depth 29.05 2021-02-13T01:05:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178346.68 epoch sec; seqNo:18489; 4060820.29 north; 593667.53 east 2021-02-13T01:05:47Z- AHRS_M2 msg: 1613178347.06 epoch sec; seqNo:18345 2021-02-13T01:05:47Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.08 psi 2021-02-13T01:05:47Z- handleDepth()-Depth_Keller msg: 1613178347070 epoch sec; seqNo:18490; depth 28.94 2021-02-13T01:05:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178347.08 epoch sec; seqNo:18490; 4060820.29 north; 593667.53 east 2021-02-13T01:05:47Z- AHRS_M2 msg: 1613178347.47 epoch sec; seqNo:18346 2021-02-13T01:05:47Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.08 psi 2021-02-13T01:05:47Z- handleDepth()-Depth_Keller msg: 1613178347473 epoch sec; seqNo:18491; depth 28.82 2021-02-13T01:05:47Z- handleDvl() - RDI_Pathfinder msg: 1613178347.49 epoch sec; seqNo:8961 2021-02-13T01:05:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 59.64 2021-02-13T01:05:47Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:05:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178347.50 epoch sec; seqNo:18491; 4060820.67 north; 593667.52 east 2021-02-13T01:05:47Z- AHRS_M2 msg: 1613178347.87 epoch sec; seqNo:18347 2021-02-13T01:05:47Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.07 psi 2021-02-13T01:05:47Z- handleDepth()-Depth_Keller msg: 1613178347881 epoch sec; seqNo:18492; depth 28.70 2021-02-13T01:05:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178347.89 epoch sec; seqNo:18492; 4060821.05 north; 593667.52 east 2021-02-13T01:05:48Z- AHRS_M2 msg: 1613178348.28 epoch sec; seqNo:18348 2021-02-13T01:05:48Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.07 psi 2021-02-13T01:05:48Z- handleDepth()-Depth_Keller msg: 1613178348283 epoch sec; seqNo:18493; depth 28.60 2021-02-13T01:05:48Z- handleDvl() - RDI_Pathfinder msg: 1613178348.30 epoch sec; seqNo:8962 2021-02-13T01:05:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:05:48Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:05:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178348.31 epoch sec; seqNo:18493; 4060821.06 north; 593668.74 east 2021-02-13T01:05:48Z- AHRS_M2 msg: 1613178348.69 epoch sec; seqNo:18349 2021-02-13T01:05:48Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.07 psi 2021-02-13T01:05:48Z- handleDepth()-Depth_Keller msg: 1613178348692 epoch sec; seqNo:18494; depth 28.48 2021-02-13T01:05:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178348.70 epoch sec; seqNo:18494; 4060821.44 north; 593668.73 east 2021-02-13T01:05:49Z- AHRS_M2 msg: 1613178349.08 epoch sec; seqNo:18350 2021-02-13T01:05:49Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.08 psi 2021-02-13T01:05:49Z- handleDepth()-Depth_Keller msg: 1613178349088 epoch sec; seqNo:18495; depth 28.38 2021-02-13T01:05:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178349.10 epoch sec; seqNo:18495; 4060821.82 north; 593668.73 east 2021-02-13T01:05:49Z- AHRS_M2 msg: 1613178349.49 epoch sec; seqNo:18351 2021-02-13T01:05:49Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.09 psi 2021-02-13T01:05:49Z- handleDepth()-Depth_Keller msg: 1613178349502 epoch sec; seqNo:18496; depth 28.25 2021-02-13T01:05:49Z- handleDvl() - RDI_Pathfinder msg: 1613178349.52 epoch sec; seqNo:8963 2021-02-13T01:05:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.31 , 79.60 , 97.99 , 60.50 2021-02-13T01:05:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , -0.07 2021-02-13T01:05:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178349.53 epoch sec; seqNo:18496; 4060821.82 north; 593668.73 east 2021-02-13T01:05:49Z- LcmTrn::handleMessages() - handling 41 messages took 0.67 ms and 128 calls... 2021-02-13T01:05:49Z- waiting for 1613178347.65, time is 1613178349.53 2021-02-13T01:05:49Z- LcmTrn::updateTrn() equating pose 1613178349.53 and meas times 1613178349.53 2021-02-13T01:05:49Z- LcmTrn::updateTrn() >>>> pose time 1613178349.53, meas time 1613178349.53 <<<< 2021-02-13T01:05:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178349.53 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:05:49Z- TerrainNav::motionUpdate() - currEstimate = (4060822, 593668.73, 28.25) 2021-02-13T01:05:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:05:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:05:49Z- TNavBF::Projecting Measurements 2021-02-13T01:05:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:05:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:05:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:05:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:05:49Z- TNavFilter::4 good beams. 2021-02-13T01:05:49Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:05:49Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:05:49Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:05:49Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:05:49Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:05:49Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:05:49Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:05:49Z- TNavBF::mmse selected from filter -1 2021-02-13T01:05:49Z- TerrainMapOctree: (vehN, vehE ) = (4060821.82, 593668.73). 2021-02-13T01:05:49Z- TerrainMapOctree: Min Distance = 721.95. 2021-02-13T01:05:49Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:05:49Z- There is no specific good beam that all particles have in common. 2021-02-13T01:05:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:05:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 7.91 2021-02-13T01:05:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:05:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.083 ExpectedMeasDiff: nan currMeas.covariance: 10.14 2021-02-13T01:05:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:05:49Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T01:05:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:05:49Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.110 ExpectedMeasDiff: nan currMeas.covariance: 5.86 2021-02-13T01:05:49Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:05:49Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:05:49Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:05:49Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:05:49Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:05:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:05:49Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:05:49Z- Filter 0 No beams used in subcloud update 2021-02-13T01:05:49Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:05:49Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:05:49Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 676.2 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T01:05:49Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178349.53 sec. 2021-02-13T01:05:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:05:49Z- TNavBF::mmse selected from filter -1 2021-02-13T01:05:49Z- variances([0], [2], and psi_berg) = 676.167901 832.986885 0.000000 2021-02-13T01:05:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:05:49Z- LcmTrn::publishEstimates() - MLE : 4060821.32 593685.23 28.25 2021-02-13T01:05:49Z- LcmTrn::publishEstimates() - MMSE : 4060821.82 593668.73 28.25 2021-02-13T01:05:49Z- LcmTrn::publishEstimates() - COVAR: 676.17 832.99 0.83 0.00 2021-02-13T01:05:49Z- AHRS_M2 msg: 1613178349.90 epoch sec; seqNo:18352 2021-02-13T01:05:49Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.11 psi 2021-02-13T01:05:49Z- handleDepth()-Depth_Keller msg: 1613178349909 epoch sec; seqNo:18497; depth 28.14 2021-02-13T01:05:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178349.92 epoch sec; seqNo:18497; 4060822.20 north; 593668.72 east 2021-02-13T01:05:50Z- AHRS_M2 msg: 1613178350.30 epoch sec; seqNo:18353 2021-02-13T01:05:50Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.11 psi 2021-02-13T01:05:50Z- handleDepth()-Depth_Keller msg: 1613178350303 epoch sec; seqNo:18498; depth 28.02 2021-02-13T01:05:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178350.31 epoch sec; seqNo:18498; 4060822.58 north; 593668.72 east 2021-02-13T01:05:50Z- AHRS_M2 msg: 1613178350.70 epoch sec; seqNo:18354 2021-02-13T01:05:50Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.12 psi 2021-02-13T01:05:50Z- handleDepth()-Depth_Keller msg: 1613178350705 epoch sec; seqNo:18499; depth 27.92 2021-02-13T01:05:50Z- handleDvl() - RDI_Pathfinder msg: 1613178350.72 epoch sec; seqNo:8964 2021-02-13T01:05:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 79.60 , 97.99 , 61.31 2021-02-13T01:05:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.08 , -0.05 2021-02-13T01:05:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178350.73 epoch sec; seqNo:18499; 4060822.58 north; 593668.72 east 2021-02-13T01:05:51Z- AHRS_M2 msg: 1613178351.10 epoch sec; seqNo:18355 2021-02-13T01:05:51Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.13 psi 2021-02-13T01:05:51Z- handleDepth()-Depth_Keller msg: 1613178351117 epoch sec; seqNo:18500; depth 27.78 2021-02-13T01:05:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178351.13 epoch sec; seqNo:18500; 4060822.95 north; 593668.72 east 2021-02-13T01:05:51Z- AHRS_M2 msg: 1613178351.51 epoch sec; seqNo:18356 2021-02-13T01:05:51Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.14 psi 2021-02-13T01:05:51Z- handleDepth()-Depth_Keller msg: 1613178351512 epoch sec; seqNo:18501; depth 27.68 2021-02-13T01:05:51Z- handleDvl() - RDI_Pathfinder msg: 1613178351.53 epoch sec; seqNo:8965 2021-02-13T01:05:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.49 , 81.76 , 97.99 , 60.91 2021-02-13T01:05:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.11 , 0.01 2021-02-13T01:05:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178351.54 epoch sec; seqNo:18501; 4060823.35 north; 593669.93 east 2021-02-13T01:05:51Z- AHRS_M2 msg: 1613178351.91 epoch sec; seqNo:18357 2021-02-13T01:05:51Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-13T01:05:51Z- handleDepth()-Depth_Keller msg: 1613178351916 epoch sec; seqNo:18502; depth 27.54 2021-02-13T01:05:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178351.93 epoch sec; seqNo:18502; 4060823.72 north; 593669.93 east 2021-02-13T01:05:52Z- AHRS_M2 msg: 1613178352.32 epoch sec; seqNo:18358 2021-02-13T01:05:52Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-13T01:05:52Z- handleDepth()-Depth_Keller msg: 1613178352321 epoch sec; seqNo:18503; depth 27.43 2021-02-13T01:05:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178352.33 epoch sec; seqNo:18503; 4060823.72 north; 593669.93 east 2021-02-13T01:05:52Z- AHRS_M2 msg: 1613178352.72 epoch sec; seqNo:18359 2021-02-13T01:05:52Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-13T01:05:52Z- handleDepth()-Depth_Keller msg: 1613178352726 epoch sec; seqNo:18504; depth 27.32 2021-02-13T01:05:52Z- handleDvl() - RDI_Pathfinder msg: 1613178352.74 epoch sec; seqNo:8966 2021-02-13T01:05:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.08 , 82.84 , 97.99 , 61.31 2021-02-13T01:05:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , 0.08 2021-02-13T01:05:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178352.75 epoch sec; seqNo:18504; 4060824.10 north; 593669.92 east 2021-02-13T01:05:53Z- AHRS_M2 msg: 1613178353.12 epoch sec; seqNo:18360 2021-02-13T01:05:53Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.15 psi 2021-02-13T01:05:53Z- handleDepth()-Depth_Keller msg: 1613178353130 epoch sec; seqNo:18505; depth 27.21 2021-02-13T01:05:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178353.14 epoch sec; seqNo:18505; 4060824.48 north; 593669.92 east 2021-02-13T01:05:53Z- AHRS_M2 msg: 1613178353.53 epoch sec; seqNo:18361 2021-02-13T01:05:53Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.15 psi 2021-02-13T01:05:53Z- handleDepth()-Depth_Keller msg: 1613178353538 epoch sec; seqNo:18506; depth 27.09 2021-02-13T01:05:53Z- handleDvl() - RDI_Pathfinder msg: 1613178353.55 epoch sec; seqNo:8967 2021-02-13T01:05:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.08 , 0.00 , 97.99 , 62.95 2021-02-13T01:05:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , 0.10 2021-02-13T01:05:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178353.56 epoch sec; seqNo:18506; 4060824.86 north; 593669.92 east 2021-02-13T01:05:53Z- AHRS_M2 msg: 1613178353.93 epoch sec; seqNo:18362 2021-02-13T01:05:53Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.15 psi 2021-02-13T01:05:53Z- handleDepth()-Depth_Keller msg: 1613178353948 epoch sec; seqNo:18507; depth 26.97 2021-02-13T01:05:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178353.96 epoch sec; seqNo:18507; 4060824.86 north; 593669.92 east 2021-02-13T01:05:54Z- AHRS_M2 msg: 1613178354.34 epoch sec; seqNo:18363 2021-02-13T01:05:54Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-13T01:05:54Z- handleDepth()-Depth_Keller msg: 1613178354340 epoch sec; seqNo:18508; depth 26.83 2021-02-13T01:05:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178354.35 epoch sec; seqNo:18508; 4060825.25 north; 593671.13 east 2021-02-13T01:05:54Z- AHRS_M2 msg: 1613178354.74 epoch sec; seqNo:18364 2021-02-13T01:05:54Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-13T01:05:54Z- handleDepth()-Depth_Keller msg: 1613178354753 epoch sec; seqNo:18509; depth 26.74 2021-02-13T01:05:54Z- handleDvl() - RDI_Pathfinder msg: 1613178354.77 epoch sec; seqNo:8968 2021-02-13T01:05:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.31 , 0.00 , 97.99 , 140.19 2021-02-13T01:05:54Z- handleDvl() - RDI_Pathfinder msg: velocities 1.73 , -0.82 , 0.55 2021-02-13T01:05:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178354.78 epoch sec; seqNo:18509; 4060826.01 north; 593671.12 east 2021-02-13T01:05:54Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-13T01:05:54Z- waiting for 1613178352.53, time is 1613178354.78 2021-02-13T01:05:54Z- LcmTrn::updateTrn() equating pose 1613178354.78 and meas times 1613178354.78 2021-02-13T01:05:54Z- LcmTrn::updateTrn() >>>> pose time 1613178354.78, meas time 1613178354.78 <<<< 2021-02-13T01:05:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178354.78 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:05:54Z- TerrainNav::motionUpdate() - currEstimate = (4060826, 593671.12, 26.74) 2021-02-13T01:05:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:05:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:05:54Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178354.78 sec.; No relevant navigation data available 2021-02-13T01:05:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:05:54Z- TNavBF::mmse selected from filter -1 2021-02-13T01:05:54Z- variances([0], [2], and psi_berg) = 676.121537 832.986866 0.000000 2021-02-13T01:05:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:05:54Z- LcmTrn::publishEstimates() - MLE : 4060825.51 593687.62 26.74 2021-02-13T01:05:54Z- LcmTrn::publishEstimates() - MMSE : 4060826.01 593671.12 26.74 2021-02-13T01:05:54Z- LcmTrn::publishEstimates() - COVAR: 676.12 832.99 1.08 0.00 2021-02-13T01:05:55Z- AHRS_M2 msg: 1613178355.15 epoch sec; seqNo:18365 2021-02-13T01:05:55Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.15 psi 2021-02-13T01:05:55Z- handleDepth()-Depth_Keller msg: 1613178355157 epoch sec; seqNo:18510; depth 26.60 2021-02-13T01:05:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178355.17 epoch sec; seqNo:18510; 4060826.77 north; 593671.12 east 2021-02-13T01:05:55Z- AHRS_M2 msg: 1613178355.55 epoch sec; seqNo:18366 2021-02-13T01:05:55Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.15 psi 2021-02-13T01:05:55Z- handleDepth()-Depth_Keller msg: 1613178355552 epoch sec; seqNo:18511; depth 26.47 2021-02-13T01:05:55Z- handleDvl() - RDI_Pathfinder msg: 1613178355.56 epoch sec; seqNo:8969 2021-02-13T01:05:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:05:55Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:05:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178355.57 epoch sec; seqNo:18511; 4060827.53 north; 593671.11 east 2021-02-13T01:05:55Z- AHRS_M2 msg: 1613178355.95 epoch sec; seqNo:18367 2021-02-13T01:05:55Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.16 psi 2021-02-13T01:05:55Z- handleDepth()-Depth_Keller msg: 1613178355957 epoch sec; seqNo:18512; depth 26.38 2021-02-13T01:05:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178355.97 epoch sec; seqNo:18512; 4060828.28 north; 593671.10 east 2021-02-13T01:05:56Z- AHRS_M2 msg: 1613178356.36 epoch sec; seqNo:18368 2021-02-13T01:05:56Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.16 psi 2021-02-13T01:05:56Z- handleDepth()-Depth_Keller msg: 1613178356360 epoch sec; seqNo:18513; depth 26.25 2021-02-13T01:05:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178356.38 epoch sec; seqNo:18513; 4060829.04 north; 593671.09 east 2021-02-13T01:05:56Z- AHRS_M2 msg: 1613178356.76 epoch sec; seqNo:18369 2021-02-13T01:05:56Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.16 psi 2021-02-13T01:05:56Z- handleDepth()-Depth_Keller msg: 1613178356763 epoch sec; seqNo:18514; depth 26.14 2021-02-13T01:05:56Z- handleDvl() - RDI_Pathfinder msg: 1613178356.78 epoch sec; seqNo:8970 2021-02-13T01:05:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.90 , 85.95 , 97.86 , 62.13 2021-02-13T01:05:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , -0.04 2021-02-13T01:05:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178356.81 epoch sec; seqNo:18514; 4060829.42 north; 593671.09 east 2021-02-13T01:05:57Z- AHRS_M2 msg: 1613178357.16 epoch sec; seqNo:18370 2021-02-13T01:05:57Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-13T01:05:57Z- handleDepth()-Depth_Keller msg: 1613178357169 epoch sec; seqNo:18515; depth 26.00 2021-02-13T01:05:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178357.18 epoch sec; seqNo:18515; 4060829.81 north; 593672.30 east 2021-02-13T01:05:57Z- AHRS_M2 msg: 1613178357.57 epoch sec; seqNo:18371 2021-02-13T01:05:57Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-13T01:05:57Z- handleDepth()-Depth_Keller msg: 1613178357574 epoch sec; seqNo:18516; depth 25.88 2021-02-13T01:05:57Z- handleDvl() - RDI_Pathfinder msg: 1613178357.59 epoch sec; seqNo:8971 2021-02-13T01:05:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 87.17 , 100.15 , 63.36 2021-02-13T01:05:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.03 2021-02-13T01:05:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178357.60 epoch sec; seqNo:18516; 4060830.19 north; 593672.30 east 2021-02-13T01:05:57Z- AHRS_M2 msg: 1613178357.97 epoch sec; seqNo:18372 2021-02-13T01:05:57Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-13T01:05:57Z- handleDepth()-Depth_Keller msg: 1613178357976 epoch sec; seqNo:18517; depth 25.76 2021-02-13T01:05:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178358.00 epoch sec; seqNo:18517; 4060830.19 north; 593672.30 east 2021-02-13T01:05:58Z- AHRS_M2 msg: 1613178358.38 epoch sec; seqNo:18373 2021-02-13T01:05:58Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-13T01:05:58Z- handleDepth()-Depth_Keller msg: 1613178358380 epoch sec; seqNo:18518; depth 25.63 2021-02-13T01:05:58Z- handleDvl() - RDI_Pathfinder msg: 1613178358.39 epoch sec; seqNo:8972 2021-02-13T01:05:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:05:58Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:05:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178358.41 epoch sec; seqNo:18518; 4060830.57 north; 593672.30 east 2021-02-13T01:05:58Z- AHRS_M2 msg: 1613178358.78 epoch sec; seqNo:18374 2021-02-13T01:05:58Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.17 psi 2021-02-13T01:05:58Z- handleDepth()-Depth_Keller msg: 1613178358784 epoch sec; seqNo:18519; depth 25.51 2021-02-13T01:05:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178358.80 epoch sec; seqNo:18519; 4060830.95 north; 593672.29 east 2021-02-13T01:05:59Z- AHRS_M2 msg: 1613178359.18 epoch sec; seqNo:18375 2021-02-13T01:05:59Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.16 psi 2021-02-13T01:05:59Z- handleDepth()-Depth_Keller msg: 1613178359190 epoch sec; seqNo:18520; depth 25.38 2021-02-13T01:05:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178359.20 epoch sec; seqNo:18520; 4060830.95 north; 593672.29 east 2021-02-13T01:05:59Z- AHRS_M2 msg: 1613178359.59 epoch sec; seqNo:18376 2021-02-13T01:05:59Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.16 psi 2021-02-13T01:05:59Z- handleDepth()-Depth_Keller msg: 1613178359597 epoch sec; seqNo:18521; depth 25.26 2021-02-13T01:05:59Z- handleDvl() - RDI_Pathfinder msg: 1613178359.62 epoch sec; seqNo:8973 2021-02-13T01:05:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 0.00 , 99.95 , 62.54 2021-02-13T01:05:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.07 , 0.05 2021-02-13T01:05:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178359.62 epoch sec; seqNo:18521; 4060831.33 north; 593672.29 east 2021-02-13T01:05:59Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-13T01:05:59Z- waiting for 1613178357.78, time is 1613178359.62 2021-02-13T01:05:59Z- LcmTrn::updateTrn() equating pose 1613178359.62 and meas times 1613178359.62 2021-02-13T01:05:59Z- LcmTrn::updateTrn() >>>> pose time 1613178359.62, meas time 1613178359.62 <<<< 2021-02-13T01:05:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178359.62 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:05:59Z- TerrainNav::motionUpdate() - currEstimate = (4060831, 593672.29, 25.26) 2021-02-13T01:05:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:05:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:05:59Z- TNavBF::Projecting Measurements 2021-02-13T01:05:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:05:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:05:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:05:59Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:05:59Z- TNavFilter::3 good beams. 2021-02-13T01:05:59Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:05:59Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:05:59Z- TNavBF:: beamIndex[1] = 2 2021-02-13T01:05:59Z- TNavBF:: beamIndex[2] = 3 2021-02-13T01:05:59Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:05:59Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:05:59Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:05:59Z- TNavBF::mmse selected from filter -1 2021-02-13T01:05:59Z- TerrainMapOctree: (vehN, vehE ) = (4060831.33, 593672.29). 2021-02-13T01:05:59Z- TerrainMapOctree: Min Distance = 713.47. 2021-02-13T01:05:59Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:05:59Z- There is no specific good beam that all particles have in common. 2021-02-13T01:05:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:05:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.36 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 9.81 2021-02-13T01:06:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:06:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 15.98 2021-02-13T01:06:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:06:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.124 ExpectedMeasDiff: nan currMeas.covariance: 6.26 2021-02-13T01:06:00Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:06:00Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:06:00Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:06:00Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:06:00Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:06:00Z- Filter 0 No beams used in subcloud update 2021-02-13T01:06:00Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:06:00Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:06:00Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 676.1 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T01:06:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178359.62 sec. 2021-02-13T01:06:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:06:00Z- TNavBF::mmse selected from filter -1 2021-02-13T01:06:00Z- variances([0], [2], and psi_berg) = 676.072342 832.986846 0.000000 2021-02-13T01:06:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:06:00Z- LcmTrn::publishEstimates() - MLE : 4060830.83 593659.79 25.26 2021-02-13T01:06:00Z- LcmTrn::publishEstimates() - MMSE : 4060831.33 593672.29 25.26 2021-02-13T01:06:00Z- LcmTrn::publishEstimates() - COVAR: 676.07 832.99 0.99 0.00 2021-02-13T01:06:00Z- AHRS_M2 msg: 1613178359.99 epoch sec; seqNo:18377 2021-02-13T01:06:00Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.15 psi 2021-02-13T01:06:00Z- handleDepth()-Depth_Keller msg: 1613178360001 epoch sec; seqNo:18522; depth 25.13 2021-02-13T01:06:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178360.01 epoch sec; seqNo:18522; 4060831.72 north; 593673.50 east 2021-02-13T01:06:00Z- AHRS_M2 msg: 1613178360.40 epoch sec; seqNo:18378 2021-02-13T01:06:00Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.15 psi 2021-02-13T01:06:00Z- handleDepth()-Depth_Keller msg: 1613178360402 epoch sec; seqNo:18523; depth 25.00 2021-02-13T01:06:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178360.41 epoch sec; seqNo:18523; 4060832.10 north; 593673.50 east 2021-02-13T01:06:00Z- AHRS_M2 msg: 1613178360.80 epoch sec; seqNo:18379 2021-02-13T01:06:00Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.14 psi 2021-02-13T01:06:00Z- handleDepth()-Depth_Keller msg: 1613178360809 epoch sec; seqNo:18524; depth 24.88 2021-02-13T01:06:00Z- handleDvl() - RDI_Pathfinder msg: 1613178360.83 epoch sec; seqNo:8974 2021-02-13T01:06:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.35 , 86.02 , 100.08 , 64.58 2021-02-13T01:06:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , 0.04 2021-02-13T01:06:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178360.84 epoch sec; seqNo:18524; 4060832.10 north; 593673.50 east 2021-02-13T01:06:01Z- AHRS_M2 msg: 1613178361.20 epoch sec; seqNo:18380 2021-02-13T01:06:01Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.14 psi 2021-02-13T01:06:01Z- handleDepth()-Depth_Keller msg: 1613178361213 epoch sec; seqNo:18525; depth 24.75 2021-02-13T01:06:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178361.22 epoch sec; seqNo:18525; 4060832.48 north; 593673.49 east 2021-02-13T01:06:01Z- AHRS_M2 msg: 1613178361.61 epoch sec; seqNo:18381 2021-02-13T01:06:01Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.13 psi 2021-02-13T01:06:01Z- handleDepth()-Depth_Keller msg: 1613178361617 epoch sec; seqNo:18526; depth 24.64 2021-02-13T01:06:01Z- handleDvl() - RDI_Pathfinder msg: 1613178361.63 epoch sec; seqNo:8975 2021-02-13T01:06:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.44 , 86.02 , 101.16 , 64.99 2021-02-13T01:06:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.00 2021-02-13T01:06:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178361.64 epoch sec; seqNo:18526; 4060832.86 north; 593673.49 east 2021-02-13T01:06:02Z- AHRS_M2 msg: 1613178362.01 epoch sec; seqNo:18382 2021-02-13T01:06:02Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.13 psi 2021-02-13T01:06:02Z- handleDepth()-Depth_Keller msg: 1613178362018 epoch sec; seqNo:18527; depth 24.50 2021-02-13T01:06:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178362.04 epoch sec; seqNo:18527; 4060833.24 north; 593673.49 east 2021-02-13T01:06:02Z- AHRS_M2 msg: 1613178362.42 epoch sec; seqNo:18383 2021-02-13T01:06:02Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.12 psi 2021-02-13T01:06:02Z- handleDepth()-Depth_Keller msg: 1613178362424 epoch sec; seqNo:18528; depth 24.38 2021-02-13T01:06:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178362.47 epoch sec; seqNo:18528; 4060833.25 north; 593674.71 east 2021-02-13T01:06:02Z- AHRS_M2 msg: 1613178362.82 epoch sec; seqNo:18384 2021-02-13T01:06:02Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.12 psi 2021-02-13T01:06:02Z- handleDepth()-Depth_Keller msg: 1613178362824 epoch sec; seqNo:18529; depth 24.26 2021-02-13T01:06:02Z- handleDvl() - RDI_Pathfinder msg: 1613178362.84 epoch sec; seqNo:8976 2021-02-13T01:06:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.92 , 0.00 , 98.40 , 62.95 2021-02-13T01:06:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , -0.02 2021-02-13T01:06:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178362.85 epoch sec; seqNo:18529; 4060833.63 north; 593674.70 east 2021-02-13T01:06:03Z- AHRS_M2 msg: 1613178363.22 epoch sec; seqNo:18385 2021-02-13T01:06:03Z- AHRS_M2 msg: 0.64 phi; 0.36 theta; -0.12 psi 2021-02-13T01:06:03Z- handleDepth()-Depth_Keller msg: 1613178363239 epoch sec; seqNo:18530; depth 24.13 2021-02-13T01:06:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178363.25 epoch sec; seqNo:18530; 4060834.01 north; 593674.70 east 2021-02-13T01:06:03Z- AHRS_M2 msg: 1613178363.63 epoch sec; seqNo:18386 2021-02-13T01:06:03Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.11 psi 2021-02-13T01:06:03Z- handleDepth()-Depth_Keller msg: 1613178363632 epoch sec; seqNo:18531; depth 23.99 2021-02-13T01:06:03Z- handleDvl() - RDI_Pathfinder msg: 1613178363.65 epoch sec; seqNo:8977 2021-02-13T01:06:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:06:03Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:06:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178363.66 epoch sec; seqNo:18531; 4060834.01 north; 593674.70 east 2021-02-13T01:06:04Z- handleDepth()-Depth_Keller msg: 1613178364054 epoch sec; seqNo:18532; depth 23.88 2021-02-13T01:06:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178364.07 epoch sec; seqNo:18532; 4060834.39 north; 593674.69 east 2021-02-13T01:06:04Z- AHRS_M2 msg: 1613178364.43 epoch sec; seqNo:18387 2021-02-13T01:06:04Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.11 psi 2021-02-13T01:06:04Z- handleDepth()-Depth_Keller msg: 1613178364441 epoch sec; seqNo:18533; depth 23.74 2021-02-13T01:06:04Z- handleDvl() - RDI_Pathfinder msg: 1613178364.45 epoch sec; seqNo:8978 2021-02-13T01:06:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.99 , 0.00 , 102.25 , 64.99 2021-02-13T01:06:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.02 , -0.02 2021-02-13T01:06:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178364.46 epoch sec; seqNo:18533; 4060834.76 north; 593674.69 east 2021-02-13T01:06:04Z- AHRS_M2 msg: 1613178364.84 epoch sec; seqNo:18388 2021-02-13T01:06:04Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.11 psi 2021-02-13T01:06:04Z- handleDepth()-Depth_Keller msg: 1613178364845 epoch sec; seqNo:18534; depth 23.62 2021-02-13T01:06:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178364.86 epoch sec; seqNo:18534; 4060834.76 north; 593674.69 east 2021-02-13T01:06:05Z- LcmTrn::handleMessages() - handling 43 messages took 0.66 ms and 130 calls... 2021-02-13T01:06:05Z- waiting for 1613178362.62, time is 1613178364.86 2021-02-13T01:06:05Z- LcmTrn::updateTrn() >>>> pose time 1613178364.86, meas time 1613178364.45 <<<< 2021-02-13T01:06:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178364.45 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:06:05Z- TerrainNav::motionUpdate() - currEstimate = (4060835, 593674.69, 23.62) 2021-02-13T01:06:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:06:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:06:05Z- TNavBF::Projecting Measurements 2021-02-13T01:06:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:06:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:06:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:06:05Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:06:05Z- TNavFilter::3 good beams. 2021-02-13T01:06:05Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:06:05Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:06:05Z- TNavBF:: beamIndex[1] = 2 2021-02-13T01:06:05Z- TNavBF:: beamIndex[2] = 3 2021-02-13T01:06:05Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:06:05Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:06:05Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:06:05Z- TNavBF::mmse selected from filter -1 2021-02-13T01:06:05Z- TerrainMapOctree: (vehN, vehE ) = (4060834.50, 593674.51). 2021-02-13T01:06:05Z- TerrainMapOctree: Min Distance = 709.81. 2021-02-13T01:06:05Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:06:05Z- There is no specific good beam that all particles have in common. 2021-02-13T01:06:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:06:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.102 ExpectedMeasDiff: nan currMeas.covariance: 8.76 2021-02-13T01:06:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:06:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 16.73 2021-02-13T01:06:05Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:06:05Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 6.76 2021-02-13T01:06:05Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:06:05Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:06:05Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:06:05Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:05Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:06:05Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:06:05Z- Filter 0 No beams used in subcloud update 2021-02-13T01:06:05Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:06:05Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:06:05Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 676.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:06:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178364.45 sec. 2021-02-13T01:06:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:06:05Z- TNavBF::mmse selected from filter -1 2021-02-13T01:06:05Z- variances([0], [2], and psi_berg) = 676.018905 832.986824 0.000000 2021-02-13T01:06:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:06:05Z- LcmTrn::publishEstimates() - MLE : 4060834.26 593691.19 23.62 2021-02-13T01:06:05Z- LcmTrn::publishEstimates() - MMSE : 4060834.76 593674.69 23.62 2021-02-13T01:06:05Z- LcmTrn::publishEstimates() - COVAR: 676.02 832.99 0.92 0.00 2021-02-13T01:06:05Z- AHRS_M2 msg: 1613178365.28 epoch sec; seqNo:18389 2021-02-13T01:06:05Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.11 psi 2021-02-13T01:06:05Z- handleDepth()-Depth_Keller msg: 1613178365318 epoch sec; seqNo:18535; depth 23.50 2021-02-13T01:06:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178365.33 epoch sec; seqNo:18535; 4060835.16 north; 593675.91 east 2021-02-13T01:06:05Z- AHRS_M2 msg: 1613178365.65 epoch sec; seqNo:18390 2021-02-13T01:06:05Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.12 psi 2021-02-13T01:06:05Z- handleDepth()-Depth_Keller msg: 1613178365657 epoch sec; seqNo:18536; depth 23.35 2021-02-13T01:06:05Z- handleDvl() - RDI_Pathfinder msg: 1613178365.67 epoch sec; seqNo:8979 2021-02-13T01:06:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.45 , 0.00 , 99.00 , 64.58 2021-02-13T01:06:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.02 2021-02-13T01:06:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178365.69 epoch sec; seqNo:18536; 4060835.54 north; 593675.90 east 2021-02-13T01:06:06Z- AHRS_M2 msg: 1613178366.05 epoch sec; seqNo:18391 2021-02-13T01:06:06Z- AHRS_M2 msg: 0.65 phi; 0.35 theta; -0.13 psi 2021-02-13T01:06:06Z- handleDepth()-Depth_Keller msg: 1613178366061 epoch sec; seqNo:18537; depth 23.25 2021-02-13T01:06:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178366.07 epoch sec; seqNo:18537; 4060835.54 north; 593675.90 east 2021-02-13T01:06:06Z- AHRS_M2 msg: 1613178366.48 epoch sec; seqNo:18392 2021-02-13T01:06:06Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.16 psi 2021-02-13T01:06:06Z- handleDepth()-Depth_Keller msg: 1613178366489 epoch sec; seqNo:18538; depth 23.14 2021-02-13T01:06:06Z- handleDvl() - RDI_Pathfinder msg: 1613178366.50 epoch sec; seqNo:8980 2021-02-13T01:06:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.58 , 87.10 , 101.17 , 66.63 2021-02-13T01:06:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , 0.05 2021-02-13T01:06:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178366.52 epoch sec; seqNo:18538; 4060835.91 north; 593675.90 east 2021-02-13T01:06:06Z- handleDepth()-Depth_Keller msg: 1613178366881 epoch sec; seqNo:18539; depth 22.99 2021-02-13T01:06:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178366.89 epoch sec; seqNo:18539; 4060836.29 north; 593675.89 east 2021-02-13T01:06:07Z- AHRS_M2 msg: 1613178367.27 epoch sec; seqNo:18393 2021-02-13T01:06:07Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.17 psi 2021-02-13T01:06:07Z- handleDepth()-Depth_Keller msg: 1613178367283 epoch sec; seqNo:18540; depth 22.88 2021-02-13T01:06:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178367.30 epoch sec; seqNo:18540; 4060836.67 north; 593675.89 east 2021-02-13T01:06:07Z- AHRS_M2 msg: 1613178367.67 epoch sec; seqNo:18394 2021-02-13T01:06:07Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.18 psi 2021-02-13T01:06:07Z- handleDepth()-Depth_Keller msg: 1613178367672 epoch sec; seqNo:18541; depth 22.77 2021-02-13T01:06:07Z- handleDvl() - RDI_Pathfinder msg: 1613178367.68 epoch sec; seqNo:8981 2021-02-13T01:06:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 0.00 , 102.12 , 65.40 2021-02-13T01:06:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.07 2021-02-13T01:06:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178367.69 epoch sec; seqNo:18541; 4060836.69 north; 593677.11 east 2021-02-13T01:06:08Z- AHRS_M2 msg: 1613178368.07 epoch sec; seqNo:18395 2021-02-13T01:06:08Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.18 psi 2021-02-13T01:06:08Z- handleDepth()-Depth_Keller msg: 1613178368085 epoch sec; seqNo:18542; depth 22.64 2021-02-13T01:06:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178368.10 epoch sec; seqNo:18542; 4060837.06 north; 593677.11 east 2021-02-13T01:06:08Z- AHRS_M2 msg: 1613178368.48 epoch sec; seqNo:18396 2021-02-13T01:06:08Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.18 psi 2021-02-13T01:06:08Z- handleDepth()-Depth_Keller msg: 1613178368485 epoch sec; seqNo:18543; depth 22.52 2021-02-13T01:06:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178368.50 epoch sec; seqNo:18543; 4060837.44 north; 593677.10 east 2021-02-13T01:06:08Z- AHRS_M2 msg: 1613178368.88 epoch sec; seqNo:18397 2021-02-13T01:06:08Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.18 psi 2021-02-13T01:06:08Z- handleDepth()-Depth_Keller msg: 1613178368884 epoch sec; seqNo:18544; depth 22.41 2021-02-13T01:06:08Z- handleDvl() - RDI_Pathfinder msg: 1613178368.90 epoch sec; seqNo:8982 2021-02-13T01:06:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 97.79 , 102.12 , 64.99 2021-02-13T01:06:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , 0.05 2021-02-13T01:06:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178368.92 epoch sec; seqNo:18544; 4060837.44 north; 593677.10 east 2021-02-13T01:06:09Z- AHRS_M2 msg: 1613178369.28 epoch sec; seqNo:18398 2021-02-13T01:06:09Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.18 psi 2021-02-13T01:06:09Z- handleDepth()-Depth_Keller msg: 1613178369294 epoch sec; seqNo:18545; depth 22.28 2021-02-13T01:06:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178369.31 epoch sec; seqNo:18545; 4060837.82 north; 593677.10 east 2021-02-13T01:06:09Z- AHRS_M2 msg: 1613178369.69 epoch sec; seqNo:18399 2021-02-13T01:06:09Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.18 psi 2021-02-13T01:06:09Z- handleDepth()-Depth_Keller msg: 1613178369693 epoch sec; seqNo:18546; depth 22.16 2021-02-13T01:06:09Z- handleDvl() - RDI_Pathfinder msg: 1613178369.70 epoch sec; seqNo:8983 2021-02-13T01:06:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.58 , 90.82 , 101.78 , 67.04 2021-02-13T01:06:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.07 , 0.04 2021-02-13T01:06:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178369.71 epoch sec; seqNo:18546; 4060838.20 north; 593677.09 east 2021-02-13T01:06:10Z- AHRS_M2 msg: 1613178370.09 epoch sec; seqNo:18400 2021-02-13T01:06:10Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.18 psi 2021-02-13T01:06:10Z- handleDepth()-Depth_Keller msg: 1613178370096 epoch sec; seqNo:18547; depth 22.05 2021-02-13T01:06:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178370.11 epoch sec; seqNo:18547; 4060838.21 north; 593678.31 east 2021-02-13T01:06:10Z- LcmTrn::handleMessages() - handling 43 messages took 0.63 ms and 131 calls... 2021-02-13T01:06:10Z- waiting for 1613178367.86, time is 1613178370.11 2021-02-13T01:06:10Z- LcmTrn::updateTrn() >>>> pose time 1613178370.11, meas time 1613178369.70 <<<< 2021-02-13T01:06:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178369.70 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:06:10Z- TerrainNav::motionUpdate() - currEstimate = (4060838, 593678.31, 22.05) 2021-02-13T01:06:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:06:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:06:10Z- TNavBF::Projecting Measurements 2021-02-13T01:06:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:06:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:06:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:06:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:06:10Z- TNavFilter::4 good beams. 2021-02-13T01:06:10Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:06:10Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:06:10Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:06:10Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:06:10Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:06:10Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:06:10Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:06:10Z- TNavBF::mmse selected from filter -1 2021-02-13T01:06:10Z- TerrainMapOctree: (vehN, vehE ) = (4060837.95, 593678.03). 2021-02-13T01:06:10Z- TerrainMapOctree: Min Distance = 704.94. 2021-02-13T01:06:10Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:06:10Z- There is no specific good beam that all particles have in common. 2021-02-13T01:06:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:06:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 8.66 2021-02-13T01:06:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:06:10Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.065 ExpectedMeasDiff: nan currMeas.covariance: 13.20 2021-02-13T01:06:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:06:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 16.57 2021-02-13T01:06:10Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:06:10Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 7.19 2021-02-13T01:06:10Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:06:10Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:06:10Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:06:10Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:06:10Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:10Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:06:10Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:06:10Z- Filter 0 No beams used in subcloud update 2021-02-13T01:06:10Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:06:10Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:06:10Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 676.0 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T01:06:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178369.70 sec. 2021-02-13T01:06:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:06:10Z- TNavBF::mmse selected from filter -1 2021-02-13T01:06:10Z- variances([0], [2], and psi_berg) = 675.960532 832.986800 0.000000 2021-02-13T01:06:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:06:10Z- LcmTrn::publishEstimates() - MLE : 4060837.71 593694.81 22.05 2021-02-13T01:06:10Z- LcmTrn::publishEstimates() - MMSE : 4060838.21 593678.31 22.05 2021-02-13T01:06:10Z- LcmTrn::publishEstimates() - COVAR: 675.96 832.99 0.86 0.00 2021-02-13T01:06:10Z- AHRS_M2 msg: 1613178370.50 epoch sec; seqNo:18401 2021-02-13T01:06:10Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.18 psi 2021-02-13T01:06:10Z- handleDepth()-Depth_Keller msg: 1613178370506 epoch sec; seqNo:18548; depth 21.92 2021-02-13T01:06:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178370.54 epoch sec; seqNo:18548; 4060838.59 north; 593678.31 east 2021-02-13T01:06:10Z- AHRS_M2 msg: 1613178370.90 epoch sec; seqNo:18402 2021-02-13T01:06:10Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T01:06:10Z- handleDepth()-Depth_Keller msg: 1613178370909 epoch sec; seqNo:18549; depth 21.81 2021-02-13T01:06:10Z- handleDvl() - RDI_Pathfinder msg: 1613178370.92 epoch sec; seqNo:8984 2021-02-13T01:06:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 98.33 , 101.98 , 0.00 2021-02-13T01:06:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , 0.06 2021-02-13T01:06:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178370.93 epoch sec; seqNo:18549; 4060838.97 north; 593678.31 east 2021-02-13T01:06:11Z- AHRS_M2 msg: 1613178371.31 epoch sec; seqNo:18403 2021-02-13T01:06:11Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T01:06:11Z- handleDepth()-Depth_Keller msg: 1613178371310 epoch sec; seqNo:18550; depth 21.69 2021-02-13T01:06:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178371.32 epoch sec; seqNo:18550; 4060838.97 north; 593678.31 east 2021-02-13T01:06:11Z- AHRS_M2 msg: 1613178371.71 epoch sec; seqNo:18404 2021-02-13T01:06:11Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-13T01:06:11Z- handleDepth()-Depth_Keller msg: 1613178371717 epoch sec; seqNo:18551; depth 21.56 2021-02-13T01:06:11Z- handleDvl() - RDI_Pathfinder msg: 1613178371.73 epoch sec; seqNo:8985 2021-02-13T01:06:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 97.65 , 101.71 , 67.86 2021-02-13T01:06:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.10 , 0.02 2021-02-13T01:06:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178371.75 epoch sec; seqNo:18551; 4060839.35 north; 593678.30 east 2021-02-13T01:06:12Z- AHRS_M2 msg: 1613178372.11 epoch sec; seqNo:18405 2021-02-13T01:06:12Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-13T01:06:12Z- handleDepth()-Depth_Keller msg: 1613178372116 epoch sec; seqNo:18552; depth 21.44 2021-02-13T01:06:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178372.13 epoch sec; seqNo:18552; 4060839.73 north; 593678.30 east 2021-02-13T01:06:12Z- AHRS_M2 msg: 1613178372.53 epoch sec; seqNo:18406 2021-02-13T01:06:12Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.17 psi 2021-02-13T01:06:12Z- handleDepth()-Depth_Keller msg: 1613178372529 epoch sec; seqNo:18553; depth 21.33 2021-02-13T01:06:12Z- handleDvl() - RDI_Pathfinder msg: 1613178372.54 epoch sec; seqNo:8986 2021-02-13T01:06:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.06 , 102.93 , 66.63 2021-02-13T01:06:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , -0.02 2021-02-13T01:06:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178372.55 epoch sec; seqNo:18553; 4060840.12 north; 593679.51 east 2021-02-13T01:06:12Z- AHRS_M2 msg: 1613178372.92 epoch sec; seqNo:18407 2021-02-13T01:06:12Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-13T01:06:12Z- handleDepth()-Depth_Keller msg: 1613178372927 epoch sec; seqNo:18554; depth 21.18 2021-02-13T01:06:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178372.94 epoch sec; seqNo:18554; 4060840.12 north; 593679.51 east 2021-02-13T01:06:13Z- AHRS_M2 msg: 1613178373.32 epoch sec; seqNo:18408 2021-02-13T01:06:13Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.17 psi 2021-02-13T01:06:13Z- handleDepth()-Depth_Keller msg: 1613178373331 epoch sec; seqNo:18555; depth 21.08 2021-02-13T01:06:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178373.34 epoch sec; seqNo:18555; 4060840.50 north; 593679.51 east 2021-02-13T01:06:13Z- AHRS_M2 msg: 1613178373.73 epoch sec; seqNo:18409 2021-02-13T01:06:13Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-13T01:06:13Z- handleDepth()-Depth_Keller msg: 1613178373741 epoch sec; seqNo:18556; depth 20.95 2021-02-13T01:06:13Z- handleDvl() - RDI_Pathfinder msg: 1613178373.75 epoch sec; seqNo:8987 2021-02-13T01:06:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.86 , 105.16 , 68.26 2021-02-13T01:06:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.02 , -0.08 2021-02-13T01:06:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178373.76 epoch sec; seqNo:18556; 4060840.88 north; 593679.51 east 2021-02-13T01:06:14Z- AHRS_M2 msg: 1613178374.14 epoch sec; seqNo:18410 2021-02-13T01:06:14Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-13T01:06:14Z- handleDepth()-Depth_Keller msg: 1613178374145 epoch sec; seqNo:18557; depth 20.83 2021-02-13T01:06:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178374.16 epoch sec; seqNo:18557; 4060841.26 north; 593679.50 east 2021-02-13T01:06:14Z- AHRS_M2 msg: 1613178374.54 epoch sec; seqNo:18411 2021-02-13T01:06:14Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-13T01:06:14Z- handleDepth()-Depth_Keller msg: 1613178374548 epoch sec; seqNo:18558; depth 20.72 2021-02-13T01:06:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178374.58 epoch sec; seqNo:18558; 4060841.26 north; 593679.50 east 2021-02-13T01:06:14Z- AHRS_M2 msg: 1613178374.94 epoch sec; seqNo:18412 2021-02-13T01:06:14Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T01:06:14Z- handleDepth()-Depth_Keller msg: 1613178374948 epoch sec; seqNo:18559; depth 20.58 2021-02-13T01:06:14Z- handleDvl() - RDI_Pathfinder msg: 1613178374.96 epoch sec; seqNo:8988 2021-02-13T01:06:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.24 , 97.72 , 105.02 , 66.63 2021-02-13T01:06:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , -0.12 2021-02-13T01:06:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178374.98 epoch sec; seqNo:18559; 4060841.65 north; 593680.72 east 2021-02-13T01:06:15Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-13T01:06:15Z- waiting for 1613178373.11, time is 1613178374.98 2021-02-13T01:06:15Z- LcmTrn::updateTrn() equating pose 1613178374.98 and meas times 1613178374.98 2021-02-13T01:06:15Z- LcmTrn::updateTrn() >>>> pose time 1613178374.98, meas time 1613178374.98 <<<< 2021-02-13T01:06:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178374.98 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:06:15Z- TerrainNav::motionUpdate() - currEstimate = (4060842, 593680.72, 20.58) 2021-02-13T01:06:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:06:15Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:06:15Z- TNavBF::Projecting Measurements 2021-02-13T01:06:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:06:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:06:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:06:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:06:15Z- TNavFilter::4 good beams. 2021-02-13T01:06:15Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:06:15Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:06:15Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:06:15Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:06:15Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:06:15Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:06:15Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:06:15Z- TNavBF::mmse selected from filter -1 2021-02-13T01:06:15Z- TerrainMapOctree: (vehN, vehE ) = (4060841.65, 593680.72). 2021-02-13T01:06:15Z- TerrainMapOctree: Min Distance = 700.59. 2021-02-13T01:06:15Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:06:15Z- There is no specific good beam that all particles have in common. 2021-02-13T01:06:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:06:15Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.085 ExpectedMeasDiff: nan currMeas.covariance: 9.80 2021-02-13T01:06:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:06:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.28 2021-02-13T01:06:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:06:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.046 ExpectedMeasDiff: nan currMeas.covariance: 17.65 2021-02-13T01:06:15Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:06:15Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 7.10 2021-02-13T01:06:15Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:06:15Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:06:15Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:06:15Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:06:15Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:15Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:06:15Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:06:15Z- Filter 0 No beams used in subcloud update 2021-02-13T01:06:15Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:06:15Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:06:15Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 675.9 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T01:06:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178374.98 sec. 2021-02-13T01:06:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:06:15Z- TNavBF::mmse selected from filter -1 2021-02-13T01:06:15Z- variances([0], [2], and psi_berg) = 675.917422 832.986783 0.000000 2021-02-13T01:06:15Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:06:15Z- LcmTrn::publishEstimates() - MLE : 4060841.15 593668.22 20.58 2021-02-13T01:06:15Z- LcmTrn::publishEstimates() - MMSE : 4060841.65 593680.72 20.58 2021-02-13T01:06:15Z- LcmTrn::publishEstimates() - COVAR: 675.92 832.99 0.80 0.00 2021-02-13T01:06:15Z- AHRS_M2 msg: 1613178375.35 epoch sec; seqNo:18413 2021-02-13T01:06:15Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T01:06:15Z- handleDepth()-Depth_Keller msg: 1613178375358 epoch sec; seqNo:18560; depth 20.46 2021-02-13T01:06:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178375.37 epoch sec; seqNo:18560; 4060842.03 north; 593680.71 east 2021-02-13T01:06:15Z- AHRS_M2 msg: 1613178375.75 epoch sec; seqNo:18414 2021-02-13T01:06:15Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T01:06:15Z- handleDepth()-Depth_Keller msg: 1613178375758 epoch sec; seqNo:18561; depth 20.34 2021-02-13T01:06:15Z- handleDvl() - RDI_Pathfinder msg: 1613178375.77 epoch sec; seqNo:8989 2021-02-13T01:06:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 93.19 , 104.14 , 68.26 2021-02-13T01:06:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.09 , -0.10 2021-02-13T01:06:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178375.78 epoch sec; seqNo:18561; 4060842.41 north; 593680.71 east 2021-02-13T01:06:16Z- AHRS_M2 msg: 1613178376.16 epoch sec; seqNo:18415 2021-02-13T01:06:16Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-13T01:06:16Z- handleDepth()-Depth_Keller msg: 1613178376174 epoch sec; seqNo:18562; depth 20.23 2021-02-13T01:06:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178376.18 epoch sec; seqNo:18562; 4060842.41 north; 593680.71 east 2021-02-13T01:06:16Z- AHRS_M2 msg: 1613178376.56 epoch sec; seqNo:18416 2021-02-13T01:06:16Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-13T01:06:16Z- handleDepth()-Depth_Keller msg: 1613178376564 epoch sec; seqNo:18563; depth 20.11 2021-02-13T01:06:16Z- handleDvl() - RDI_Pathfinder msg: 1613178376.58 epoch sec; seqNo:8990 2021-02-13T01:06:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 98.13 , 101.78 , 69.49 2021-02-13T01:06:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.10 , -0.03 2021-02-13T01:06:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178376.60 epoch sec; seqNo:18563; 4060842.79 north; 593680.71 east 2021-02-13T01:06:16Z- AHRS_M2 msg: 1613178376.96 epoch sec; seqNo:18417 2021-02-13T01:06:16Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.17 psi 2021-02-13T01:06:16Z- handleDepth()-Depth_Keller msg: 1613178376968 epoch sec; seqNo:18564; depth 19.99 2021-02-13T01:06:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178376.98 epoch sec; seqNo:18564; 4060843.17 north; 593680.70 east 2021-02-13T01:06:17Z- AHRS_M2 msg: 1613178377.37 epoch sec; seqNo:18418 2021-02-13T01:06:17Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T01:06:17Z- handleDepth()-Depth_Keller msg: 1613178377383 epoch sec; seqNo:18565; depth 19.87 2021-02-13T01:06:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178377.39 epoch sec; seqNo:18565; 4060843.55 north; 593680.70 east 2021-02-13T01:06:17Z- AHRS_M2 msg: 1613178377.77 epoch sec; seqNo:18419 2021-02-13T01:06:17Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.19 psi 2021-02-13T01:06:17Z- handleDepth()-Depth_Keller msg: 1613178377777 epoch sec; seqNo:18566; depth 19.75 2021-02-13T01:06:17Z- handleDvl() - RDI_Pathfinder msg: 1613178377.79 epoch sec; seqNo:8991 2021-02-13T01:06:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.21 , 98.13 , 103.00 , 69.49 2021-02-13T01:06:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , 0.04 2021-02-13T01:06:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178377.81 epoch sec; seqNo:18566; 4060843.55 north; 593680.70 east 2021-02-13T01:06:18Z- AHRS_M2 msg: 1613178378.17 epoch sec; seqNo:18420 2021-02-13T01:06:18Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.18 psi 2021-02-13T01:06:18Z- handleDepth()-Depth_Keller msg: 1613178378185 epoch sec; seqNo:18567; depth 19.62 2021-02-13T01:06:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178378.19 epoch sec; seqNo:18567; 4060843.94 north; 593681.91 east 2021-02-13T01:06:18Z- AHRS_M2 msg: 1613178378.58 epoch sec; seqNo:18421 2021-02-13T01:06:18Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.17 psi 2021-02-13T01:06:18Z- handleDepth()-Depth_Keller msg: 1613178378586 epoch sec; seqNo:18568; depth 19.53 2021-02-13T01:06:18Z- handleDvl() - RDI_Pathfinder msg: 1613178378.60 epoch sec; seqNo:8992 2021-02-13T01:06:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.21 , 98.06 , 102.93 , 70.31 2021-02-13T01:06:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.07 2021-02-13T01:06:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178378.61 epoch sec; seqNo:18568; 4060844.32 north; 593681.91 east 2021-02-13T01:06:19Z- handleDepth()-Depth_Keller msg: 1613178379005 epoch sec; seqNo:18569; depth 19.42 2021-02-13T01:06:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178379.02 epoch sec; seqNo:18569; 4060844.69 north; 593681.91 east 2021-02-13T01:06:19Z- AHRS_M2 msg: 1613178379.39 epoch sec; seqNo:18422 2021-02-13T01:06:19Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.16 psi 2021-02-13T01:06:19Z- handleDepth()-Depth_Keller msg: 1613178379410 epoch sec; seqNo:18570; depth 19.29 2021-02-13T01:06:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178379.42 epoch sec; seqNo:18570; 4060844.69 north; 593681.91 east 2021-02-13T01:06:19Z- AHRS_M2 msg: 1613178379.79 epoch sec; seqNo:18423 2021-02-13T01:06:19Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.16 psi 2021-02-13T01:06:19Z- handleDepth()-Depth_Keller msg: 1613178379798 epoch sec; seqNo:18571; depth 19.19 2021-02-13T01:06:19Z- handleDvl() - RDI_Pathfinder msg: 1613178379.81 epoch sec; seqNo:8993 2021-02-13T01:06:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.06 , 101.71 , 78.58 2021-02-13T01:06:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , 0.06 2021-02-13T01:06:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178379.82 epoch sec; seqNo:18571; 4060845.07 north; 593681.90 east 2021-02-13T01:06:20Z- AHRS_M2 msg: 1613178380.20 epoch sec; seqNo:18424 2021-02-13T01:06:20Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T01:06:20Z- handleDepth()-Depth_Keller msg: 1613178380200 epoch sec; seqNo:18572; depth 19.07 2021-02-13T01:06:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178380.21 epoch sec; seqNo:18572; 4060845.45 north; 593681.90 east 2021-02-13T01:06:20Z- LcmTrn::handleMessages() - handling 43 messages took 0.66 ms and 130 calls... 2021-02-13T01:06:20Z- waiting for 1613178377.98, time is 1613178380.21 2021-02-13T01:06:20Z- LcmTrn::updateTrn() >>>> pose time 1613178380.21, meas time 1613178379.81 <<<< 2021-02-13T01:06:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178379.81 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:06:20Z- TerrainNav::motionUpdate() - currEstimate = (4060845, 593681.90, 19.07) 2021-02-13T01:06:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:06:20Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:06:20Z- TNavBF::Projecting Measurements 2021-02-13T01:06:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:06:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:06:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:06:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:06:20Z- TNavFilter::4 good beams. 2021-02-13T01:06:20Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:06:20Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:06:20Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:06:20Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:06:20Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:06:20Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:06:20Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:06:20Z- TNavBF::mmse selected from filter -1 2021-02-13T01:06:20Z- TerrainMapOctree: (vehN, vehE ) = (4060845.16, 593681.81). 2021-02-13T01:06:20Z- TerrainMapOctree: Min Distance = 697.67. 2021-02-13T01:06:20Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:06:20Z- There is no specific good beam that all particles have in common. 2021-02-13T01:06:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:06:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-13T01:06:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:06:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 15.38 2021-02-13T01:06:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:06:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 16.55 2021-02-13T01:06:20Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:06:20Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-13T01:06:20Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:06:20Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:06:20Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:06:20Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:06:20Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:20Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:06:20Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:06:20Z- Filter 0 No beams used in subcloud update 2021-02-13T01:06:20Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:06:20Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:06:20Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 675.9 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T01:06:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178379.81 sec. 2021-02-13T01:06:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:06:20Z- TNavBF::mmse selected from filter -1 2021-02-13T01:06:20Z- variances([0], [2], and psi_berg) = 675.865463 832.986762 0.000000 2021-02-13T01:06:20Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:06:20Z- LcmTrn::publishEstimates() - MLE : 4060844.95 593670.40 19.07 2021-02-13T01:06:20Z- LcmTrn::publishEstimates() - MMSE : 4060845.45 593681.90 19.07 2021-02-13T01:06:20Z- LcmTrn::publishEstimates() - COVAR: 675.87 832.99 0.79 0.00 2021-02-13T01:06:20Z- AHRS_M2 msg: 1613178380.61 epoch sec; seqNo:18425 2021-02-13T01:06:20Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T01:06:20Z- handleDepth()-Depth_Keller msg: 1613178380609 epoch sec; seqNo:18573; depth 18.96 2021-02-13T01:06:20Z- handleDvl() - RDI_Pathfinder msg: 1613178380.62 epoch sec; seqNo:8994 2021-02-13T01:06:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 97.86 , 102.72 , 70.72 2021-02-13T01:06:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.04 , 0.05 2021-02-13T01:06:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178380.64 epoch sec; seqNo:18573; 4060845.47 north; 593683.12 east 2021-02-13T01:06:21Z- AHRS_M2 msg: 1613178381.00 epoch sec; seqNo:18426 2021-02-13T01:06:21Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T01:06:21Z- handleDepth()-Depth_Keller msg: 1613178381008 epoch sec; seqNo:18574; depth 18.85 2021-02-13T01:06:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178381.02 epoch sec; seqNo:18574; 4060845.84 north; 593683.11 east 2021-02-13T01:06:21Z- AHRS_M2 msg: 1613178381.41 epoch sec; seqNo:18427 2021-02-13T01:06:21Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T01:06:21Z- handleDepth()-Depth_Keller msg: 1613178381412 epoch sec; seqNo:18575; depth 18.74 2021-02-13T01:06:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178381.43 epoch sec; seqNo:18575; 4060846.22 north; 593683.11 east 2021-02-13T01:06:21Z- AHRS_M2 msg: 1613178381.81 epoch sec; seqNo:18428 2021-02-13T01:06:21Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.15 psi 2021-02-13T01:06:21Z- handleDepth()-Depth_Keller msg: 1613178381821 epoch sec; seqNo:18576; depth 18.64 2021-02-13T01:06:21Z- handleDvl() - RDI_Pathfinder msg: 1613178381.84 epoch sec; seqNo:8995 2021-02-13T01:06:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.99 , 104.08 , 78.51 2021-02-13T01:06:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , 0.06 2021-02-13T01:06:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178381.85 epoch sec; seqNo:18576; 4060846.60 north; 593683.11 east 2021-02-13T01:06:22Z- AHRS_M2 msg: 1613178382.22 epoch sec; seqNo:18429 2021-02-13T01:06:22Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T01:06:22Z- handleDepth()-Depth_Keller msg: 1613178382221 epoch sec; seqNo:18577; depth 18.51 2021-02-13T01:06:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178382.24 epoch sec; seqNo:18577; 4060846.60 north; 593683.11 east 2021-02-13T01:06:22Z- AHRS_M2 msg: 1613178382.62 epoch sec; seqNo:18430 2021-02-13T01:06:22Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.15 psi 2021-02-13T01:06:22Z- handleDepth()-Depth_Keller msg: 1613178382625 epoch sec; seqNo:18578; depth 18.41 2021-02-13T01:06:22Z- handleDvl() - RDI_Pathfinder msg: 1613178382.64 epoch sec; seqNo:8996 2021-02-13T01:06:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.79 , 106.31 , 69.49 2021-02-13T01:06:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , 0.08 2021-02-13T01:06:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178382.66 epoch sec; seqNo:18578; 4060846.98 north; 593683.10 east 2021-02-13T01:06:23Z- AHRS_M2 msg: 1613178383.02 epoch sec; seqNo:18431 2021-02-13T01:06:23Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.15 psi 2021-02-13T01:06:23Z- handleDepth()-Depth_Keller msg: 1613178383033 epoch sec; seqNo:18579; depth 18.29 2021-02-13T01:06:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178383.05 epoch sec; seqNo:18579; 4060847.37 north; 593684.32 east 2021-02-13T01:06:23Z- AHRS_M2 msg: 1613178383.43 epoch sec; seqNo:18432 2021-02-13T01:06:23Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.15 psi 2021-02-13T01:06:23Z- handleDepth()-Depth_Keller msg: 1613178383447 epoch sec; seqNo:18580; depth 18.18 2021-02-13T01:06:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178383.46 epoch sec; seqNo:18580; 4060847.75 north; 593684.31 east 2021-02-13T01:06:23Z- AHRS_M2 msg: 1613178383.83 epoch sec; seqNo:18433 2021-02-13T01:06:23Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.15 psi 2021-02-13T01:06:23Z- handleDepth()-Depth_Keller msg: 1613178383850 epoch sec; seqNo:18581; depth 18.06 2021-02-13T01:06:23Z- handleDvl() - RDI_Pathfinder msg: 1613178383.86 epoch sec; seqNo:8997 2021-02-13T01:06:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.67 , 97.65 , 106.17 , 69.90 2021-02-13T01:06:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.10 , 0.10 2021-02-13T01:06:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178383.87 epoch sec; seqNo:18581; 4060847.75 north; 593684.31 east 2021-02-13T01:06:24Z- AHRS_M2 msg: 1613178384.24 epoch sec; seqNo:18434 2021-02-13T01:06:24Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.15 psi 2021-02-13T01:06:24Z- handleDepth()-Depth_Keller msg: 1613178384246 epoch sec; seqNo:18582; depth 17.95 2021-02-13T01:06:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178384.26 epoch sec; seqNo:18582; 4060848.13 north; 593684.31 east 2021-02-13T01:06:24Z- AHRS_M2 msg: 1613178384.64 epoch sec; seqNo:18435 2021-02-13T01:06:24Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.15 psi 2021-02-13T01:06:24Z- handleDepth()-Depth_Keller msg: 1613178384653 epoch sec; seqNo:18583; depth 17.85 2021-02-13T01:06:24Z- handleDvl() - RDI_Pathfinder msg: 1613178384.66 epoch sec; seqNo:8998 2021-02-13T01:06:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:06:24Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:06:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178384.67 epoch sec; seqNo:18583; 4060848.51 north; 593684.30 east 2021-02-13T01:06:25Z- AHRS_M2 msg: 1613178385.04 epoch sec; seqNo:18436 2021-02-13T01:06:25Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.15 psi 2021-02-13T01:06:25Z- handleDepth()-Depth_Keller msg: 1613178385053 epoch sec; seqNo:18584; depth 17.72 2021-02-13T01:06:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178385.06 epoch sec; seqNo:18584; 4060848.89 north; 593684.30 east 2021-02-13T01:06:25Z- AHRS_M2 msg: 1613178385.45 epoch sec; seqNo:18437 2021-02-13T01:06:25Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.14 psi 2021-02-13T01:06:25Z- LcmTrn::handleMessages() - handling 42 messages took 0.64 ms and 129 calls... 2021-02-13T01:06:25Z- waiting for 1613178383.21, time is 1613178385.45 2021-02-13T01:06:25Z- LcmTrn::updateTrn() >>>> pose time 1613178385.45, meas time 1613178384.66 <<<< 2021-02-13T01:06:25Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613178384.66, ping # 0. 2021-02-13T01:06:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178384.66 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:06:25Z- TerrainNav::motionUpdate() - currEstimate = (4060849, 593684.30, 17.72) 2021-02-13T01:06:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:06:25Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:06:25Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:06:25Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:06:25Z- TNavBF::Projecting Measurements 2021-02-13T01:06:25Z- TNavFilter::Measurement from time = 1613178384.66 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T01:06:25Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:06:25Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:06:25Z- TNavBF:: beamIndex[1] = 0 2021-02-13T01:06:25Z- TNavBF:: beamIndex[2] = 0 2021-02-13T01:06:25Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:06:25Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:06:25Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:06:25Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 675.8 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T01:06:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:06:25Z- TNavBF::mmse selected from filter -1 2021-02-13T01:06:25Z- variances([0], [2], and psi_berg) = 675.809098 832.986739 0.000000 2021-02-13T01:06:25Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:06:25Z- LcmTrn::publishEstimates() - MLE : 4060848.39 593672.80 17.72 2021-02-13T01:06:25Z- LcmTrn::publishEstimates() - MMSE : 4060848.89 593684.30 17.72 2021-02-13T01:06:25Z- LcmTrn::publishEstimates() - COVAR: 675.81 832.99 1.00 0.00 2021-02-13T01:06:25Z- handleDepth()-Depth_Keller msg: 1613178385458 epoch sec; seqNo:18585; depth 17.60 2021-02-13T01:06:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178385.47 epoch sec; seqNo:18585; 4060848.90 north; 593685.52 east 2021-02-13T01:06:25Z- AHRS_M2 msg: 1613178385.85 epoch sec; seqNo:18438 2021-02-13T01:06:25Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.14 psi 2021-02-13T01:06:25Z- handleDepth()-Depth_Keller msg: 1613178385859 epoch sec; seqNo:18586; depth 17.49 2021-02-13T01:06:25Z- handleDvl() - RDI_Pathfinder msg: 1613178385.87 epoch sec; seqNo:8999 2021-02-13T01:06:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.99 , 106.51 , 78.51 2021-02-13T01:06:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.16 , -0.04 2021-02-13T01:06:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178385.88 epoch sec; seqNo:18586; 4060849.28 north; 593685.52 east 2021-02-13T01:06:26Z- AHRS_M2 msg: 1613178386.26 epoch sec; seqNo:18439 2021-02-13T01:06:26Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.12 psi 2021-02-13T01:06:26Z- handleDepth()-Depth_Keller msg: 1613178386260 epoch sec; seqNo:18587; depth 17.38 2021-02-13T01:06:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178386.28 epoch sec; seqNo:18587; 4060849.66 north; 593685.51 east 2021-02-13T01:06:26Z- AHRS_M2 msg: 1613178386.66 epoch sec; seqNo:18440 2021-02-13T01:06:26Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.12 psi 2021-02-13T01:06:26Z- handleDepth()-Depth_Keller msg: 1613178386669 epoch sec; seqNo:18588; depth 17.26 2021-02-13T01:06:26Z- handleDvl() - RDI_Pathfinder msg: 1613178386.68 epoch sec; seqNo:9000 2021-02-13T01:06:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.93 , 93.46 , 108.34 , 78.58 2021-02-13T01:06:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.13 , -0.15 2021-02-13T01:06:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178386.70 epoch sec; seqNo:18588; 4060849.66 north; 593685.51 east 2021-02-13T01:06:27Z- AHRS_M2 msg: 1613178387.06 epoch sec; seqNo:18441 2021-02-13T01:06:27Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.12 psi 2021-02-13T01:06:27Z- handleDepth()-Depth_Keller msg: 1613178387068 epoch sec; seqNo:18589; depth 17.14 2021-02-13T01:06:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178387.08 epoch sec; seqNo:18589; 4060850.04 north; 593685.51 east 2021-02-13T01:06:27Z- AHRS_M2 msg: 1613178387.47 epoch sec; seqNo:18442 2021-02-13T01:06:27Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.12 psi 2021-02-13T01:06:27Z- handleDepth()-Depth_Keller msg: 1613178387474 epoch sec; seqNo:18590; depth 17.01 2021-02-13T01:06:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178387.48 epoch sec; seqNo:18590; 4060850.05 north; 593686.73 east 2021-02-13T01:06:27Z- AHRS_M2 msg: 1613178387.87 epoch sec; seqNo:18443 2021-02-13T01:06:27Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.13 psi 2021-02-13T01:06:27Z- handleDepth()-Depth_Keller msg: 1613178387876 epoch sec; seqNo:18591; depth 16.90 2021-02-13T01:06:27Z- handleDvl() - RDI_Pathfinder msg: 1613178387.89 epoch sec; seqNo:9001 2021-02-13T01:06:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.75 , 97.79 , 106.31 , 78.31 2021-02-13T01:06:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , -0.20 2021-02-13T01:06:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178387.91 epoch sec; seqNo:18591; 4060850.43 north; 593686.73 east 2021-02-13T01:06:28Z- AHRS_M2 msg: 1613178388.28 epoch sec; seqNo:18444 2021-02-13T01:06:28Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.14 psi 2021-02-13T01:06:28Z- handleDepth()-Depth_Keller msg: 1613178388280 epoch sec; seqNo:18592; depth 16.77 2021-02-13T01:06:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178388.29 epoch sec; seqNo:18592; 4060850.81 north; 593686.72 east 2021-02-13T01:06:28Z- AHRS_M2 msg: 1613178388.69 epoch sec; seqNo:18445 2021-02-13T01:06:28Z- AHRS_M2 msg: 0.64 phi; 0.32 theta; -0.14 psi 2021-02-13T01:06:28Z- handleDepth()-Depth_Keller msg: 1613178388716 epoch sec; seqNo:18593; depth 16.65 2021-02-13T01:06:28Z- handleDvl() - RDI_Pathfinder msg: 1613178388.73 epoch sec; seqNo:9002 2021-02-13T01:06:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 94.07 , 106.24 , 78.24 2021-02-13T01:06:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , -0.17 2021-02-13T01:06:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178388.75 epoch sec; seqNo:18593; 4060851.19 north; 593686.72 east 2021-02-13T01:06:29Z- AHRS_M2 msg: 1613178389.08 epoch sec; seqNo:18446 2021-02-13T01:06:29Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-13T01:06:29Z- handleDepth()-Depth_Keller msg: 1613178389093 epoch sec; seqNo:18594; depth 16.53 2021-02-13T01:06:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178389.11 epoch sec; seqNo:18594; 4060851.19 north; 593686.72 east 2021-02-13T01:06:29Z- AHRS_M2 msg: 1613178389.49 epoch sec; seqNo:18447 2021-02-13T01:06:29Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.16 psi 2021-02-13T01:06:29Z- handleDepth()-Depth_Keller msg: 1613178389492 epoch sec; seqNo:18595; depth 16.43 2021-02-13T01:06:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178389.50 epoch sec; seqNo:18595; 4060851.57 north; 593686.71 east 2021-02-13T01:06:29Z- AHRS_M2 msg: 1613178389.89 epoch sec; seqNo:18448 2021-02-13T01:06:29Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-13T01:06:29Z- handleDepth()-Depth_Keller msg: 1613178389901 epoch sec; seqNo:18596; depth 16.31 2021-02-13T01:06:29Z- handleDvl() - RDI_Pathfinder msg: 1613178389.91 epoch sec; seqNo:9003 2021-02-13T01:06:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.75 , 97.79 , 107.53 , 72.35 2021-02-13T01:06:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.10 , -0.07 2021-02-13T01:06:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178389.92 epoch sec; seqNo:18596; 4060851.95 north; 593686.71 east 2021-02-13T01:06:30Z- AHRS_M2 msg: 1613178390.30 epoch sec; seqNo:18449 2021-02-13T01:06:30Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-13T01:06:30Z- handleDepth()-Depth_Keller msg: 1613178390309 epoch sec; seqNo:18597; depth 16.20 2021-02-13T01:06:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178390.32 epoch sec; seqNo:18597; 4060851.96 north; 593687.93 east 2021-02-13T01:06:30Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 130 calls... 2021-02-13T01:06:30Z- waiting for 1613178388.45, time is 1613178390.32 2021-02-13T01:06:30Z- LcmTrn::updateTrn() >>>> pose time 1613178390.32, meas time 1613178389.91 <<<< 2021-02-13T01:06:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178389.91 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:06:30Z- TerrainNav::motionUpdate() - currEstimate = (4060852, 593687.93, 16.20) 2021-02-13T01:06:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:06:30Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:06:30Z- TNavBF::Projecting Measurements 2021-02-13T01:06:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:06:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:06:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:06:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:06:30Z- TNavFilter::4 good beams. 2021-02-13T01:06:30Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:06:30Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:06:30Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:06:30Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:06:30Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:06:30Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:06:30Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:06:30Z- TNavBF::mmse selected from filter -1 2021-02-13T01:06:30Z- TerrainMapOctree: (vehN, vehE ) = (4060851.70, 593687.63). 2021-02-13T01:06:30Z- TerrainMapOctree: Min Distance = 689.13. 2021-02-13T01:06:30Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:06:30Z- There is no specific good beam that all particles have in common. 2021-02-13T01:06:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:06:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.22 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 10.43 2021-02-13T01:06:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:06:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-13T01:06:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:06:30Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 18.50 2021-02-13T01:06:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:06:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 8.38 2021-02-13T01:06:30Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:06:30Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:06:30Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:06:30Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:06:30Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:06:30Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:06:30Z- Filter 0 No beams used in subcloud update 2021-02-13T01:06:30Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:06:30Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:06:30Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 675.8 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:06:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178389.91 sec. 2021-02-13T01:06:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:06:30Z- TNavBF::mmse selected from filter -1 2021-02-13T01:06:30Z- variances([0], [2], and psi_berg) = 675.751990 832.986716 0.000000 2021-02-13T01:06:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:06:30Z- LcmTrn::publishEstimates() - MLE : 4060851.46 593702.43 16.20 2021-02-13T01:06:30Z- LcmTrn::publishEstimates() - MMSE : 4060851.96 593687.93 16.20 2021-02-13T01:06:30Z- LcmTrn::publishEstimates() - COVAR: 675.75 832.99 0.92 0.00 2021-02-13T01:06:30Z- AHRS_M2 msg: 1613178390.70 epoch sec; seqNo:18450 2021-02-13T01:06:30Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T01:06:30Z- handleDepth()-Depth_Keller msg: 1613178390704 epoch sec; seqNo:18598; depth 16.08 2021-02-13T01:06:30Z- handleDvl() - RDI_Pathfinder msg: 1613178390.72 epoch sec; seqNo:9004 2021-02-13T01:06:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.79 , 106.31 , 78.31 2021-02-13T01:06:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.09 , 0.03 2021-02-13T01:06:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178390.73 epoch sec; seqNo:18598; 4060852.34 north; 593687.92 east 2021-02-13T01:06:31Z- AHRS_M2 msg: 1613178391.10 epoch sec; seqNo:18451 2021-02-13T01:06:31Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T01:06:31Z- handleDepth()-Depth_Keller msg: 1613178391108 epoch sec; seqNo:18599; depth 15.97 2021-02-13T01:06:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178391.12 epoch sec; seqNo:18599; 4060852.72 north; 593687.92 east 2021-02-13T01:06:31Z- AHRS_M2 msg: 1613178391.51 epoch sec; seqNo:18452 2021-02-13T01:06:31Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T01:06:31Z- handleDepth()-Depth_Keller msg: 1613178391516 epoch sec; seqNo:18600; depth 15.86 2021-02-13T01:06:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178391.53 epoch sec; seqNo:18600; 4060853.10 north; 593687.92 east 2021-02-13T01:06:31Z- AHRS_M2 msg: 1613178391.91 epoch sec; seqNo:18453 2021-02-13T01:06:31Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-13T01:06:31Z- handleDepth()-Depth_Keller msg: 1613178391918 epoch sec; seqNo:18601; depth 15.75 2021-02-13T01:06:31Z- handleDvl() - RDI_Pathfinder msg: 1613178391.93 epoch sec; seqNo:9005 2021-02-13T01:06:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.93 , 96.17 , 108.34 , 72.76 2021-02-13T01:06:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.02 , 0.10 2021-02-13T01:06:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178391.94 epoch sec; seqNo:18601; 4060853.10 north; 593687.92 east 2021-02-13T01:06:32Z- AHRS_M2 msg: 1613178392.32 epoch sec; seqNo:18454 2021-02-13T01:06:32Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T01:06:32Z- handleDepth()-Depth_Keller msg: 1613178392320 epoch sec; seqNo:18602; depth 15.64 2021-02-13T01:06:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178392.34 epoch sec; seqNo:18602; 4060853.48 north; 593687.91 east 2021-02-13T01:06:32Z- AHRS_M2 msg: 1613178392.72 epoch sec; seqNo:18455 2021-02-13T01:06:32Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.17 psi 2021-02-13T01:06:32Z- handleDepth()-Depth_Keller msg: 1613178392724 epoch sec; seqNo:18603; depth 15.53 2021-02-13T01:06:32Z- handleDvl() - RDI_Pathfinder msg: 1613178392.73 epoch sec; seqNo:9006 2021-02-13T01:06:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.86 , 106.38 , 78.38 2021-02-13T01:06:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.04 , 0.12 2021-02-13T01:06:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178392.76 epoch sec; seqNo:18603; 4060853.85 north; 593687.91 east 2021-02-13T01:06:33Z- AHRS_M2 msg: 1613178393.12 epoch sec; seqNo:18456 2021-02-13T01:06:33Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.17 psi 2021-02-13T01:06:33Z- handleDepth()-Depth_Keller msg: 1613178393131 epoch sec; seqNo:18604; depth 15.41 2021-02-13T01:06:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178393.14 epoch sec; seqNo:18604; 4060853.87 north; 593689.13 east 2021-02-13T01:06:33Z- AHRS_M2 msg: 1613178393.53 epoch sec; seqNo:18457 2021-02-13T01:06:33Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.17 psi 2021-02-13T01:06:33Z- handleDepth()-Depth_Keller msg: 1613178393538 epoch sec; seqNo:18605; depth 15.30 2021-02-13T01:06:33Z- handleDvl() - RDI_Pathfinder msg: 1613178393.55 epoch sec; seqNo:9007 2021-02-13T01:06:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.71 , 97.52 , 106.99 , 74.40 2021-02-13T01:06:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.08 , 0.09 2021-02-13T01:06:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178393.56 epoch sec; seqNo:18605; 4060854.25 north; 593689.12 east 2021-02-13T01:06:33Z- AHRS_M2 msg: 1613178393.93 epoch sec; seqNo:18458 2021-02-13T01:06:33Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.17 psi 2021-02-13T01:06:33Z- handleDepth()-Depth_Keller msg: 1613178393947 epoch sec; seqNo:18606; depth 15.20 2021-02-13T01:06:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178393.96 epoch sec; seqNo:18606; 4060854.63 north; 593689.12 east 2021-02-13T01:06:34Z- AHRS_M2 msg: 1613178394.34 epoch sec; seqNo:18459 2021-02-13T01:06:34Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.18 psi 2021-02-13T01:06:34Z- handleDepth()-Depth_Keller msg: 1613178394346 epoch sec; seqNo:18607; depth 15.07 2021-02-13T01:06:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178394.37 epoch sec; seqNo:18607; 4060855.00 north; 593689.12 east 2021-02-13T01:06:34Z- AHRS_M2 msg: 1613178394.74 epoch sec; seqNo:18460 2021-02-13T01:06:34Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.18 psi 2021-02-13T01:06:34Z- handleDepth()-Depth_Keller msg: 1613178394749 epoch sec; seqNo:18608; depth 14.98 2021-02-13T01:06:34Z- handleDvl() - RDI_Pathfinder msg: 1613178394.76 epoch sec; seqNo:9008 2021-02-13T01:06:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.75 , 97.79 , 108.74 , 78.31 2021-02-13T01:06:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , 0.06 2021-02-13T01:06:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178394.77 epoch sec; seqNo:18608; 4060855.00 north; 593689.12 east 2021-02-13T01:06:35Z- AHRS_M2 msg: 1613178395.15 epoch sec; seqNo:18461 2021-02-13T01:06:35Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.18 psi 2021-02-13T01:06:35Z- handleDepth()-Depth_Keller msg: 1613178395154 epoch sec; seqNo:18609; depth 14.87 2021-02-13T01:06:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178395.16 epoch sec; seqNo:18609; 4060855.38 north; 593689.11 east 2021-02-13T01:06:35Z- AHRS_M2 msg: 1613178395.55 epoch sec; seqNo:18462 2021-02-13T01:06:35Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.18 psi 2021-02-13T01:06:35Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-13T01:06:35Z- waiting for 1613178393.32, time is 1613178395.55 2021-02-13T01:06:35Z- LcmTrn::updateTrn() >>>> pose time 1613178395.55, meas time 1613178394.76 <<<< 2021-02-13T01:06:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178394.76 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:06:35Z- TerrainNav::motionUpdate() - currEstimate = (4060855, 593689.11, 14.87) 2021-02-13T01:06:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:06:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:06:35Z- TNavBF::Projecting Measurements 2021-02-13T01:06:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:06:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:06:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:06:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:06:35Z- TNavFilter::4 good beams. 2021-02-13T01:06:35Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:06:35Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:06:35Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:06:35Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:06:35Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:06:35Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:06:35Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:06:35Z- TNavBF::mmse selected from filter -1 2021-02-13T01:06:35Z- TerrainMapOctree: (vehN, vehE ) = (4060854.87, 593688.93). 2021-02-13T01:06:35Z- TerrainMapOctree: Min Distance = 686.24. 2021-02-13T01:06:35Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:06:35Z- There is no specific good beam that all particles have in common. 2021-02-13T01:06:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:06:35Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 10.43 2021-02-13T01:06:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:06:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-13T01:06:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:06:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 18.92 2021-02-13T01:06:35Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:06:35Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 9.81 2021-02-13T01:06:35Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:06:35Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:06:35Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:06:35Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:06:35Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:35Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:06:35Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:06:35Z- Filter 0 No beams used in subcloud update 2021-02-13T01:06:35Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:06:35Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:06:35Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 675.7 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T01:06:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178394.76 sec. 2021-02-13T01:06:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:06:35Z- TNavBF::mmse selected from filter -1 2021-02-13T01:06:35Z- variances([0], [2], and psi_berg) = 675.699620 832.986694 0.000000 2021-02-13T01:06:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:06:35Z- LcmTrn::publishEstimates() - MLE : 4060854.88 593703.61 14.87 2021-02-13T01:06:35Z- LcmTrn::publishEstimates() - MMSE : 4060855.38 593689.11 14.87 2021-02-13T01:06:35Z- LcmTrn::publishEstimates() - COVAR: 675.70 832.99 0.86 0.00 2021-02-13T01:06:35Z- handleDepth()-Depth_Keller msg: 1613178395570 epoch sec; seqNo:18610; depth 14.77 2021-02-13T01:06:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178395.60 epoch sec; seqNo:18610; 4060855.77 north; 593690.33 east 2021-02-13T01:06:35Z- AHRS_M2 msg: 1613178395.95 epoch sec; seqNo:18463 2021-02-13T01:06:35Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.18 psi 2021-02-13T01:06:35Z- handleDepth()-Depth_Keller msg: 1613178395956 epoch sec; seqNo:18611; depth 14.67 2021-02-13T01:06:35Z- handleDvl() - RDI_Pathfinder msg: 1613178395.97 epoch sec; seqNo:9009 2021-02-13T01:06:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.83 , 97.65 , 109.83 , 72.76 2021-02-13T01:06:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.08 , 0.01 2021-02-13T01:06:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178395.99 epoch sec; seqNo:18611; 4060855.77 north; 593690.33 east 2021-02-13T01:06:36Z- AHRS_M2 msg: 1613178396.36 epoch sec; seqNo:18464 2021-02-13T01:06:36Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.18 psi 2021-02-13T01:06:36Z- handleDepth()-Depth_Keller msg: 1613178396360 epoch sec; seqNo:18612; depth 14.57 2021-02-13T01:06:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178396.37 epoch sec; seqNo:18612; 4060856.15 north; 593690.32 east 2021-02-13T01:06:36Z- AHRS_M2 msg: 1613178396.76 epoch sec; seqNo:18465 2021-02-13T01:06:36Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.18 psi 2021-02-13T01:06:36Z- handleDepth()-Depth_Keller msg: 1613178396766 epoch sec; seqNo:18613; depth 14.46 2021-02-13T01:06:36Z- handleDvl() - RDI_Pathfinder msg: 1613178396.78 epoch sec; seqNo:9010 2021-02-13T01:06:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.68 , 100.15 , 108.67 , 73.57 2021-02-13T01:06:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.09 , -0.05 2021-02-13T01:06:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178396.79 epoch sec; seqNo:18613; 4060856.53 north; 593690.32 east 2021-02-13T01:06:37Z- AHRS_M2 msg: 1613178397.16 epoch sec; seqNo:18466 2021-02-13T01:06:37Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.18 psi 2021-02-13T01:06:37Z- handleDepth()-Depth_Keller msg: 1613178397169 epoch sec; seqNo:18614; depth 14.38 2021-02-13T01:06:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178397.18 epoch sec; seqNo:18614; 4060856.53 north; 593690.32 east 2021-02-13T01:06:37Z- AHRS_M2 msg: 1613178397.59 epoch sec; seqNo:18467 2021-02-13T01:06:37Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.17 psi 2021-02-13T01:06:37Z- handleDepth()-Depth_Keller msg: 1613178397596 epoch sec; seqNo:18615; depth 14.28 2021-02-13T01:06:37Z- handleDvl() - RDI_Pathfinder msg: 1613178397.61 epoch sec; seqNo:9011 2021-02-13T01:06:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.29 , 98.93 , 107.45 , 78.24 2021-02-13T01:06:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , -0.06 2021-02-13T01:06:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178397.62 epoch sec; seqNo:18615; 4060856.92 north; 593691.54 east 2021-02-13T01:06:37Z- AHRS_M2 msg: 1613178397.97 epoch sec; seqNo:18468 2021-02-13T01:06:37Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.17 psi 2021-02-13T01:06:37Z- handleDepth()-Depth_Keller msg: 1613178397977 epoch sec; seqNo:18616; depth 14.17 2021-02-13T01:06:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178397.99 epoch sec; seqNo:18616; 4060857.30 north; 593691.53 east 2021-02-13T01:06:38Z- AHRS_M2 msg: 1613178398.38 epoch sec; seqNo:18469 2021-02-13T01:06:38Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.17 psi 2021-02-13T01:06:38Z- handleDepth()-Depth_Keller msg: 1613178398382 epoch sec; seqNo:18617; depth 14.08 2021-02-13T01:06:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178398.39 epoch sec; seqNo:18617; 4060857.68 north; 593691.53 east 2021-02-13T01:06:38Z- AHRS_M2 msg: 1613178398.78 epoch sec; seqNo:18470 2021-02-13T01:06:38Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.17 psi 2021-02-13T01:06:38Z- handleDepth()-Depth_Keller msg: 1613178398784 epoch sec; seqNo:18618; depth 13.98 2021-02-13T01:06:38Z- handleDvl() - RDI_Pathfinder msg: 1613178398.80 epoch sec; seqNo:9012 2021-02-13T01:06:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.05 , 97.65 , 109.83 , 78.18 2021-02-13T01:06:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.01 2021-02-13T01:06:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178398.81 epoch sec; seqNo:18618; 4060857.68 north; 593691.53 east 2021-02-13T01:06:39Z- AHRS_M2 msg: 1613178399.19 epoch sec; seqNo:18471 2021-02-13T01:06:39Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.17 psi 2021-02-13T01:06:39Z- handleDepth()-Depth_Keller msg: 1613178399190 epoch sec; seqNo:18619; depth 13.87 2021-02-13T01:06:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178399.20 epoch sec; seqNo:18619; 4060858.06 north; 593691.52 east 2021-02-13T01:06:39Z- AHRS_M2 msg: 1613178399.59 epoch sec; seqNo:18472 2021-02-13T01:06:39Z- AHRS_M2 msg: 0.56 phi; 0.33 theta; -0.17 psi 2021-02-13T01:06:39Z- handleDepth()-Depth_Keller msg: 1613178399603 epoch sec; seqNo:18620; depth 13.77 2021-02-13T01:06:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178399.64 epoch sec; seqNo:18620; 4060858.44 north; 593691.52 east 2021-02-13T01:06:40Z- AHRS_M2 msg: 1613178400.00 epoch sec; seqNo:18473 2021-02-13T01:06:40Z- AHRS_M2 msg: 0.56 phi; 0.33 theta; -0.17 psi 2021-02-13T01:06:40Z- handleDepth()-Depth_Keller msg: 1613178400003 epoch sec; seqNo:18621; depth 13.66 2021-02-13T01:06:40Z- handleDvl() - RDI_Pathfinder msg: 1613178400.01 epoch sec; seqNo:9013 2021-02-13T01:06:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 96.37 , 111.25 , 78.79 2021-02-13T01:06:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.08 , 0.03 2021-02-13T01:06:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178400.03 epoch sec; seqNo:18621; 4060858.83 north; 593692.74 east 2021-02-13T01:06:40Z- AHRS_M2 msg: 1613178400.39 epoch sec; seqNo:18474 2021-02-13T01:06:40Z- AHRS_M2 msg: 0.55 phi; 0.33 theta; -0.17 psi 2021-02-13T01:06:40Z- handleDepth()-Depth_Keller msg: 1613178400400 epoch sec; seqNo:18622; depth 13.57 2021-02-13T01:06:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178400.42 epoch sec; seqNo:18622; 4060858.83 north; 593692.74 east 2021-02-13T01:06:40Z- LcmTrn::handleMessages() - handling 43 messages took 0.69 ms and 130 calls... 2021-02-13T01:06:40Z- waiting for 1613178398.55, time is 1613178400.42 2021-02-13T01:06:40Z- LcmTrn::updateTrn() >>>> pose time 1613178400.42, meas time 1613178400.01 <<<< 2021-02-13T01:06:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178400.01 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:06:40Z- TerrainNav::motionUpdate() - currEstimate = (4060859, 593692.74, 13.57) 2021-02-13T01:06:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:06:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:06:40Z- TNavBF::Projecting Measurements 2021-02-13T01:06:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:06:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:06:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:06:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:06:40Z- TNavFilter::4 good beams. 2021-02-13T01:06:40Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:06:40Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:06:40Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:06:40Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:06:40Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:06:40Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:06:40Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:06:40Z- TNavBF::mmse selected from filter -1 2021-02-13T01:06:40Z- TerrainMapOctree: (vehN, vehE ) = (4060858.54, 593692.43). 2021-02-13T01:06:40Z- TerrainMapOctree: Min Distance = 681.27. 2021-02-13T01:06:40Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:06:40Z- There is no specific good beam that all particles have in common. 2021-02-13T01:06:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:06:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 10.98 2021-02-13T01:06:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:06:40Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 14.86 2021-02-13T01:06:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:06:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 19.80 2021-02-13T01:06:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:06:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 9.93 2021-02-13T01:06:40Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:06:40Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:06:40Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:06:40Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:06:40Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:06:40Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:06:40Z- Filter 0 No beams used in subcloud update 2021-02-13T01:06:40Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:06:40Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:06:40Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 675.7 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T01:06:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178400.01 sec. 2021-02-13T01:06:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:06:40Z- TNavBF::mmse selected from filter -1 2021-02-13T01:06:40Z- variances([0], [2], and psi_berg) = 675.641049 832.986671 0.000000 2021-02-13T01:06:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:06:40Z- LcmTrn::publishEstimates() - MLE : 4060858.33 593680.24 13.57 2021-02-13T01:06:40Z- LcmTrn::publishEstimates() - MMSE : 4060858.83 593692.74 13.57 2021-02-13T01:06:40Z- LcmTrn::publishEstimates() - COVAR: 675.64 832.99 0.87 0.00 2021-02-13T01:06:40Z- AHRS_M2 msg: 1613178400.80 epoch sec; seqNo:18475 2021-02-13T01:06:40Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.17 psi 2021-02-13T01:06:40Z- handleDepth()-Depth_Keller msg: 1613178400804 epoch sec; seqNo:18623; depth 13.47 2021-02-13T01:06:40Z- handleDvl() - RDI_Pathfinder msg: 1613178400.82 epoch sec; seqNo:9014 2021-02-13T01:06:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 100.43 , 113.95 , 75.62 2021-02-13T01:06:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.19 , -0.03 2021-02-13T01:06:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178400.84 epoch sec; seqNo:18623; 4060859.21 north; 593692.73 east 2021-02-13T01:06:41Z- AHRS_M2 msg: 1613178401.21 epoch sec; seqNo:18476 2021-02-13T01:06:41Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.16 psi 2021-02-13T01:06:41Z- handleDepth()-Depth_Keller msg: 1613178401214 epoch sec; seqNo:18624; depth 13.35 2021-02-13T01:06:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178401.22 epoch sec; seqNo:18624; 4060859.59 north; 593692.73 east 2021-02-13T01:06:41Z- AHRS_M2 msg: 1613178401.61 epoch sec; seqNo:18477 2021-02-13T01:06:41Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.15 psi 2021-02-13T01:06:41Z- handleDepth()-Depth_Keller msg: 1613178401618 epoch sec; seqNo:18625; depth 13.26 2021-02-13T01:06:41Z- handleDvl() - RDI_Pathfinder msg: 1613178401.63 epoch sec; seqNo:9015 2021-02-13T01:06:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.94 , 112.46 , 78.65 2021-02-13T01:06:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.17 , -0.19 2021-02-13T01:06:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178401.64 epoch sec; seqNo:18625; 4060859.59 north; 593692.73 east 2021-02-13T01:06:42Z- AHRS_M2 msg: 1613178402.01 epoch sec; seqNo:18478 2021-02-13T01:06:42Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.15 psi 2021-02-13T01:06:42Z- handleDepth()-Depth_Keller msg: 1613178402021 epoch sec; seqNo:18626; depth 13.13 2021-02-13T01:06:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178402.03 epoch sec; seqNo:18626; 4060859.97 north; 593692.72 east 2021-02-13T01:06:42Z- AHRS_M2 msg: 1613178402.42 epoch sec; seqNo:18479 2021-02-13T01:06:42Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.16 psi 2021-02-13T01:06:42Z- handleDepth()-Depth_Keller msg: 1613178402420 epoch sec; seqNo:18627; depth 13.02 2021-02-13T01:06:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178402.43 epoch sec; seqNo:18627; 4060860.36 north; 593693.94 east 2021-02-13T01:06:42Z- AHRS_M2 msg: 1613178402.82 epoch sec; seqNo:18480 2021-02-13T01:06:42Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.16 psi 2021-02-13T01:06:42Z- handleDepth()-Depth_Keller msg: 1613178402826 epoch sec; seqNo:18628; depth 12.92 2021-02-13T01:06:42Z- handleDvl() - RDI_Pathfinder msg: 1613178402.84 epoch sec; seqNo:9016 2021-02-13T01:06:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 97.32 , 113.54 , 73.58 2021-02-13T01:06:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , -0.26 2021-02-13T01:06:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178402.85 epoch sec; seqNo:18628; 4060860.36 north; 593693.94 east 2021-02-13T01:06:43Z- AHRS_M2 msg: 1613178403.22 epoch sec; seqNo:18481 2021-02-13T01:06:43Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.16 psi 2021-02-13T01:06:43Z- handleDepth()-Depth_Keller msg: 1613178403229 epoch sec; seqNo:18629; depth 12.79 2021-02-13T01:06:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178403.24 epoch sec; seqNo:18629; 4060860.74 north; 593693.94 east 2021-02-13T01:06:43Z- AHRS_M2 msg: 1613178403.64 epoch sec; seqNo:18482 2021-02-13T01:06:43Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.17 psi 2021-02-13T01:06:43Z- handleDepth()-Depth_Keller msg: 1613178403648 epoch sec; seqNo:18630; depth 12.65 2021-02-13T01:06:43Z- handleDvl() - RDI_Pathfinder msg: 1613178403.66 epoch sec; seqNo:9017 2021-02-13T01:06:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:06:43Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:06:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178403.68 epoch sec; seqNo:18630; 4060861.12 north; 593693.93 east 2021-02-13T01:06:44Z- AHRS_M2 msg: 1613178404.03 epoch sec; seqNo:18483 2021-02-13T01:06:44Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.17 psi 2021-02-13T01:06:44Z- handleDepth()-Depth_Keller msg: 1613178404038 epoch sec; seqNo:18631; depth 12.53 2021-02-13T01:06:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178404.05 epoch sec; seqNo:18631; 4060861.12 north; 593693.93 east 2021-02-13T01:06:44Z- AHRS_M2 msg: 1613178404.43 epoch sec; seqNo:18484 2021-02-13T01:06:44Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.16 psi 2021-02-13T01:06:44Z- handleDepth()-Depth_Keller msg: 1613178404441 epoch sec; seqNo:18632; depth 12.42 2021-02-13T01:06:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178404.46 epoch sec; seqNo:18632; 4060861.50 north; 593693.93 east 2021-02-13T01:06:44Z- AHRS_M2 msg: 1613178404.84 epoch sec; seqNo:18485 2021-02-13T01:06:44Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.15 psi 2021-02-13T01:06:44Z- handleDepth()-Depth_Keller msg: 1613178404849 epoch sec; seqNo:18633; depth 12.32 2021-02-13T01:06:44Z- handleDvl() - RDI_Pathfinder msg: 1613178404.86 epoch sec; seqNo:9018 2021-02-13T01:06:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 103.20 , 115.10 , 72.62 2021-02-13T01:06:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.23 , 0.07 2021-02-13T01:06:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178404.89 epoch sec; seqNo:18633; 4060861.88 north; 593693.92 east 2021-02-13T01:06:45Z- AHRS_M2 msg: 1613178405.25 epoch sec; seqNo:18486 2021-02-13T01:06:45Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.14 psi 2021-02-13T01:06:45Z- handleDepth()-Depth_Keller msg: 1613178405250 epoch sec; seqNo:18634; depth 12.19 2021-02-13T01:06:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178405.26 epoch sec; seqNo:18634; 4060862.26 north; 593693.92 east 2021-02-13T01:06:45Z- AHRS_M2 msg: 1613178405.65 epoch sec; seqNo:18487 2021-02-13T01:06:45Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.14 psi 2021-02-13T01:06:45Z- handleDepth()-Depth_Keller msg: 1613178405657 epoch sec; seqNo:18635; depth 12.09 2021-02-13T01:06:45Z- handleDvl() - RDI_Pathfinder msg: 1613178405.67 epoch sec; seqNo:9019 2021-02-13T01:06:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.67 , 114.90 , 76.03 2021-02-13T01:06:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.09 , 0.22 2021-02-13T01:06:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178405.68 epoch sec; seqNo:18635; 4060862.65 north; 593695.14 east 2021-02-13T01:06:45Z- LcmTrn::handleMessages() - handling 45 messages took 0.62 ms and 133 calls... 2021-02-13T01:06:45Z- waiting for 1613178403.42, time is 1613178405.68 2021-02-13T01:06:45Z- LcmTrn::updateTrn() equating pose 1613178405.68 and meas times 1613178405.68 2021-02-13T01:06:45Z- LcmTrn::updateTrn() >>>> pose time 1613178405.68, meas time 1613178405.68 <<<< 2021-02-13T01:06:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178405.68 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:06:45Z- TerrainNav::motionUpdate() - currEstimate = (4060863, 593695.14, 12.09) 2021-02-13T01:06:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:06:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:06:45Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178405.68 sec.; No relevant navigation data available 2021-02-13T01:06:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:06:45Z- TNavBF::mmse selected from filter -1 2021-02-13T01:06:45Z- variances([0], [2], and psi_berg) = 675.596503 832.986653 0.000000 2021-02-13T01:06:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:06:45Z- LcmTrn::publishEstimates() - MLE : 4060862.15 593710.64 12.09 2021-02-13T01:06:45Z- LcmTrn::publishEstimates() - MMSE : 4060862.65 593695.14 12.09 2021-02-13T01:06:45Z- LcmTrn::publishEstimates() - COVAR: 675.60 832.99 1.09 0.00 2021-02-13T01:06:46Z- AHRS_M2 msg: 1613178406.05 epoch sec; seqNo:18488 2021-02-13T01:06:46Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.12 psi 2021-02-13T01:06:46Z- handleDepth()-Depth_Keller msg: 1613178406057 epoch sec; seqNo:18636; depth 11.95 2021-02-13T01:06:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178406.07 epoch sec; seqNo:18636; 4060862.65 north; 593695.14 east 2021-02-13T01:06:46Z- AHRS_M2 msg: 1613178406.46 epoch sec; seqNo:18489 2021-02-13T01:06:46Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.11 psi 2021-02-13T01:06:46Z- handleDepth()-Depth_Keller msg: 1613178406461 epoch sec; seqNo:18637; depth 11.85 2021-02-13T01:06:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178406.47 epoch sec; seqNo:18637; 4060863.03 north; 593695.13 east 2021-02-13T01:06:46Z- AHRS_M2 msg: 1613178406.86 epoch sec; seqNo:18490 2021-02-13T01:06:46Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.11 psi 2021-02-13T01:06:46Z- handleDepth()-Depth_Keller msg: 1613178406868 epoch sec; seqNo:18638; depth 11.73 2021-02-13T01:06:46Z- handleDvl() - RDI_Pathfinder msg: 1613178406.89 epoch sec; seqNo:9020 2021-02-13T01:06:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.50 , 95.96 , 113.54 , 74.40 2021-02-13T01:06:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.09 , 0.22 2021-02-13T01:06:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178406.90 epoch sec; seqNo:18638; 4060863.41 north; 593695.13 east 2021-02-13T01:06:47Z- AHRS_M2 msg: 1613178407.26 epoch sec; seqNo:18491 2021-02-13T01:06:47Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.11 psi 2021-02-13T01:06:47Z- handleDepth()-Depth_Keller msg: 1613178407268 epoch sec; seqNo:18639; depth 11.61 2021-02-13T01:06:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178407.28 epoch sec; seqNo:18639; 4060863.78 north; 593695.12 east 2021-02-13T01:06:47Z- AHRS_M2 msg: 1613178407.67 epoch sec; seqNo:18492 2021-02-13T01:06:47Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.11 psi 2021-02-13T01:06:47Z- handleDepth()-Depth_Keller msg: 1613178407672 epoch sec; seqNo:18640; depth 11.50 2021-02-13T01:06:47Z- handleDvl() - RDI_Pathfinder msg: 1613178407.68 epoch sec; seqNo:9021 2021-02-13T01:06:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.18 , 97.65 , 112.53 , 78.72 2021-02-13T01:06:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.19 , 0.10 2021-02-13T01:06:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178407.69 epoch sec; seqNo:18640; 4060863.78 north; 593695.12 east 2021-02-13T01:06:48Z- AHRS_M2 msg: 1613178408.07 epoch sec; seqNo:18493 2021-02-13T01:06:48Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.11 psi 2021-02-13T01:06:48Z- handleDepth()-Depth_Keller msg: 1613178408085 epoch sec; seqNo:18641; depth 11.37 2021-02-13T01:06:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178408.10 epoch sec; seqNo:18641; 4060864.18 north; 593696.34 east 2021-02-13T01:06:48Z- AHRS_M2 msg: 1613178408.47 epoch sec; seqNo:18494 2021-02-13T01:06:48Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.11 psi 2021-02-13T01:06:48Z- handleDepth()-Depth_Keller msg: 1613178408498 epoch sec; seqNo:18642; depth 11.25 2021-02-13T01:06:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178408.51 epoch sec; seqNo:18642; 4060864.18 north; 593696.34 east 2021-02-13T01:06:48Z- handleDepth()-Depth_Keller msg: 1613178408901 epoch sec; seqNo:18643; depth 11.13 2021-02-13T01:06:48Z- handleDvl() - RDI_Pathfinder msg: 1613178408.91 epoch sec; seqNo:9022 2021-02-13T01:06:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.05 , 97.52 , 117.81 , 78.58 2021-02-13T01:06:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.14 , -0.08 2021-02-13T01:06:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178408.92 epoch sec; seqNo:18643; 4060864.56 north; 593696.34 east 2021-02-13T01:06:49Z- AHRS_M2 msg: 1613178409.28 epoch sec; seqNo:18495 2021-02-13T01:06:49Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.12 psi 2021-02-13T01:06:49Z- handleDepth()-Depth_Keller msg: 1613178409293 epoch sec; seqNo:18644; depth 11.00 2021-02-13T01:06:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178409.30 epoch sec; seqNo:18644; 4060864.93 north; 593696.33 east 2021-02-13T01:06:49Z- AHRS_M2 msg: 1613178409.69 epoch sec; seqNo:18496 2021-02-13T01:06:49Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-13T01:06:49Z- handleDepth()-Depth_Keller msg: 1613178409701 epoch sec; seqNo:18645; depth 10.88 2021-02-13T01:06:49Z- handleDvl() - RDI_Pathfinder msg: 1613178409.71 epoch sec; seqNo:9023 2021-02-13T01:06:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.98 , 98.80 , 116.39 , 0.00 2021-02-13T01:06:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.12 2021-02-13T01:06:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178409.72 epoch sec; seqNo:18645; 4060864.93 north; 593696.33 east 2021-02-13T01:06:50Z- AHRS_M2 msg: 1613178410.09 epoch sec; seqNo:18497 2021-02-13T01:06:50Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.14 psi 2021-02-13T01:06:50Z- handleDepth()-Depth_Keller msg: 1613178410096 epoch sec; seqNo:18646; depth 10.77 2021-02-13T01:06:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178410.12 epoch sec; seqNo:18646; 4060865.31 north; 593696.33 east 2021-02-13T01:06:50Z- AHRS_M2 msg: 1613178410.50 epoch sec; seqNo:18498 2021-02-13T01:06:50Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.15 psi 2021-02-13T01:06:50Z- handleDepth()-Depth_Keller msg: 1613178410503 epoch sec; seqNo:18647; depth 10.66 2021-02-13T01:06:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178410.51 epoch sec; seqNo:18647; 4060865.71 north; 593697.54 east 2021-02-13T01:06:50Z- LcmTrn::handleMessages() - handling 39 messages took 0.61 ms and 127 calls... 2021-02-13T01:06:50Z- waiting for 1613178408.68, time is 1613178410.51 2021-02-13T01:06:50Z- LcmTrn::updateTrn() >>>> pose time 1613178410.51, meas time 1613178409.71 <<<< 2021-02-13T01:06:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178409.71 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:06:50Z- TerrainNav::motionUpdate() - currEstimate = (4060866, 593697.54, 10.66) 2021-02-13T01:06:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:06:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:06:50Z- TNavBF::Projecting Measurements 2021-02-13T01:06:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:06:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:06:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:06:50Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:06:50Z- TNavFilter::3 good beams. 2021-02-13T01:06:50Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:06:50Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:06:50Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:06:50Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:06:50Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:06:50Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:06:50Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:06:50Z- TNavBF::mmse selected from filter -1 2021-02-13T01:06:50Z- TerrainMapOctree: (vehN, vehE ) = (4060865.20, 593697.14). 2021-02-13T01:06:50Z- TerrainMapOctree: Min Distance = 673.59. 2021-02-13T01:06:50Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:06:50Z- There is no specific good beam that all particles have in common. 2021-02-13T01:06:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:06:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 12.39 2021-02-13T01:06:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:06:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 15.62 2021-02-13T01:06:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:06:50Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.044 ExpectedMeasDiff: nan currMeas.covariance: 21.67 2021-02-13T01:06:50Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:06:50Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:06:50Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:06:50Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:06:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:06:50Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:06:50Z- Filter 0 No beams used in subcloud update 2021-02-13T01:06:50Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:06:50Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:06:50Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 675.6 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T01:06:50Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178409.71 sec. 2021-02-13T01:06:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:06:50Z- TNavBF::mmse selected from filter -1 2021-02-13T01:06:50Z- variances([0], [2], and psi_berg) = 675.541875 832.986630 0.000000 2021-02-13T01:06:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:06:50Z- LcmTrn::publishEstimates() - MLE : 4060865.21 593713.04 10.66 2021-02-13T01:06:50Z- LcmTrn::publishEstimates() - MMSE : 4060865.71 593697.54 10.66 2021-02-13T01:06:50Z- LcmTrn::publishEstimates() - COVAR: 675.54 832.99 1.07 0.00 2021-02-13T01:06:50Z- AHRS_M2 msg: 1613178410.90 epoch sec; seqNo:18499 2021-02-13T01:06:50Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-13T01:06:50Z- handleDepth()-Depth_Keller msg: 1613178410909 epoch sec; seqNo:18648; depth 10.54 2021-02-13T01:06:50Z- handleDvl() - RDI_Pathfinder msg: 1613178410.92 epoch sec; seqNo:9024 2021-02-13T01:06:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.51 , 101.03 , 118.62 , 78.04 2021-02-13T01:06:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , -0.03 2021-02-13T01:06:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178410.93 epoch sec; seqNo:18648; 4060866.08 north; 593697.54 east 2021-02-13T01:06:51Z- AHRS_M2 msg: 1613178411.31 epoch sec; seqNo:18500 2021-02-13T01:06:51Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.16 psi 2021-02-13T01:06:51Z- handleDepth()-Depth_Keller msg: 1613178411309 epoch sec; seqNo:18649; depth 10.41 2021-02-13T01:06:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178411.32 epoch sec; seqNo:18649; 4060866.08 north; 593697.54 east 2021-02-13T01:06:51Z- AHRS_M2 msg: 1613178411.71 epoch sec; seqNo:18501 2021-02-13T01:06:51Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.16 psi 2021-02-13T01:06:51Z- handleDepth()-Depth_Keller msg: 1613178411711 epoch sec; seqNo:18650; depth 10.30 2021-02-13T01:06:51Z- handleDvl() - RDI_Pathfinder msg: 1613178411.73 epoch sec; seqNo:9025 2021-02-13T01:06:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:06:51Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:06:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178411.74 epoch sec; seqNo:18650; 4060866.46 north; 593697.53 east 2021-02-13T01:06:52Z- AHRS_M2 msg: 1613178412.11 epoch sec; seqNo:18502 2021-02-13T01:06:52Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.16 psi 2021-02-13T01:06:52Z- handleDepth()-Depth_Keller msg: 1613178412121 epoch sec; seqNo:18651; depth 10.18 2021-02-13T01:06:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178412.15 epoch sec; seqNo:18651; 4060866.84 north; 593697.53 east 2021-02-13T01:06:52Z- AHRS_M2 msg: 1613178412.52 epoch sec; seqNo:18503 2021-02-13T01:06:52Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.16 psi 2021-02-13T01:06:52Z- handleDepth()-Depth_Keller msg: 1613178412520 epoch sec; seqNo:18652; depth 10.07 2021-02-13T01:06:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178412.53 epoch sec; seqNo:18652; 4060867.22 north; 593697.53 east 2021-02-13T01:06:52Z- AHRS_M2 msg: 1613178412.92 epoch sec; seqNo:18504 2021-02-13T01:06:52Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.16 psi 2021-02-13T01:06:52Z- handleDepth()-Depth_Keller msg: 1613178412933 epoch sec; seqNo:18653; depth 9.95 2021-02-13T01:06:52Z- handleDvl() - RDI_Pathfinder msg: 1613178412.94 epoch sec; seqNo:9026 2021-02-13T01:06:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.83 , 101.71 , 116.59 , 0.00 2021-02-13T01:06:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.15 , 0.07 2021-02-13T01:06:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178412.95 epoch sec; seqNo:18653; 4060867.23 north; 593698.75 east 2021-02-13T01:06:53Z- AHRS_M2 msg: 1613178413.33 epoch sec; seqNo:18505 2021-02-13T01:06:53Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.16 psi 2021-02-13T01:06:53Z- handleDepth()-Depth_Keller msg: 1613178413334 epoch sec; seqNo:18654; depth 9.84 2021-02-13T01:06:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178413.38 epoch sec; seqNo:18654; 4060867.61 north; 593698.74 east 2021-02-13T01:06:53Z- AHRS_M2 msg: 1613178413.73 epoch sec; seqNo:18506 2021-02-13T01:06:53Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.17 psi 2021-02-13T01:06:53Z- handleDepth()-Depth_Keller msg: 1613178413734 epoch sec; seqNo:18655; depth 9.70 2021-02-13T01:06:53Z- handleDvl() - RDI_Pathfinder msg: 1613178413.77 epoch sec; seqNo:9027 2021-02-13T01:06:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:06:53Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:06:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178413.78 epoch sec; seqNo:18655; 4060867.99 north; 593698.74 east 2021-02-13T01:06:54Z- AHRS_M2 msg: 1613178414.13 epoch sec; seqNo:18507 2021-02-13T01:06:54Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.17 psi 2021-02-13T01:06:54Z- handleDepth()-Depth_Keller msg: 1613178414145 epoch sec; seqNo:18656; depth 9.59 2021-02-13T01:06:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178414.16 epoch sec; seqNo:18656; 4060867.99 north; 593698.74 east 2021-02-13T01:06:54Z- AHRS_M2 msg: 1613178414.54 epoch sec; seqNo:18508 2021-02-13T01:06:54Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T01:06:54Z- handleDepth()-Depth_Keller msg: 1613178414558 epoch sec; seqNo:18657; depth 9.49 2021-02-13T01:06:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178414.60 epoch sec; seqNo:18657; 4060868.37 north; 593698.73 east 2021-02-13T01:06:54Z- AHRS_M2 msg: 1613178414.94 epoch sec; seqNo:18509 2021-02-13T01:06:54Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.17 psi 2021-02-13T01:06:54Z- handleDepth()-Depth_Keller msg: 1613178414943 epoch sec; seqNo:18658; depth 9.36 2021-02-13T01:06:54Z- handleDvl() - RDI_Pathfinder msg: 1613178414.96 epoch sec; seqNo:9028 2021-02-13T01:06:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 105.43 , 120.31 , 80.14 2021-02-13T01:06:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.09 , -0.15 2021-02-13T01:06:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178414.98 epoch sec; seqNo:18658; 4060868.76 north; 593699.95 east 2021-02-13T01:06:55Z- AHRS_M2 msg: 1613178415.34 epoch sec; seqNo:18510 2021-02-13T01:06:55Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-13T01:06:55Z- handleDepth()-Depth_Keller msg: 1613178415347 epoch sec; seqNo:18659; depth 9.24 2021-02-13T01:06:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178415.36 epoch sec; seqNo:18659; 4060868.76 north; 593699.95 east 2021-02-13T01:06:55Z- AHRS_M2 msg: 1613178415.75 epoch sec; seqNo:18511 2021-02-13T01:06:55Z- AHRS_M2 msg: 0.62 phi; 0.33 theta; -0.17 psi 2021-02-13T01:06:55Z- handleDepth()-Depth_Keller msg: 1613178415764 epoch sec; seqNo:18660; depth 9.13 2021-02-13T01:06:55Z- handleDvl() - RDI_Pathfinder msg: 1613178415.78 epoch sec; seqNo:9029 2021-02-13T01:06:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 103.87 , 115.77 , 127.67 2021-02-13T01:06:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.04 , -0.98 , -0.66 2021-02-13T01:06:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178415.79 epoch sec; seqNo:18660; 4060869.13 north; 593698.73 east 2021-02-13T01:06:55Z- LcmTrn::handleMessages() - handling 45 messages took 0.76 ms and 130 calls... 2021-02-13T01:06:55Z- waiting for 1613178413.51, time is 1613178415.79 2021-02-13T01:06:55Z- LcmTrn::updateTrn() equating pose 1613178415.79 and meas times 1613178415.79 2021-02-13T01:06:55Z- LcmTrn::updateTrn() >>>> pose time 1613178415.79, meas time 1613178415.79 <<<< 2021-02-13T01:06:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178415.79 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:06:55Z- TerrainNav::motionUpdate() - currEstimate = (4060869, 593698.73, 9.13) 2021-02-13T01:06:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:06:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:06:55Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178415.79 sec.; No relevant navigation data available 2021-02-13T01:06:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:06:55Z- TNavBF::mmse selected from filter -1 2021-02-13T01:06:55Z- variances([0], [2], and psi_berg) = 675.501984 832.986614 0.000000 2021-02-13T01:06:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:06:55Z- LcmTrn::publishEstimates() - MLE : 4060868.63 593686.23 9.13 2021-02-13T01:06:55Z- LcmTrn::publishEstimates() - MMSE : 4060869.13 593698.73 9.13 2021-02-13T01:06:55Z- LcmTrn::publishEstimates() - COVAR: 675.50 832.99 1.25 0.00 2021-02-13T01:06:56Z- AHRS_M2 msg: 1613178416.15 epoch sec; seqNo:18512 2021-02-13T01:06:56Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-13T01:06:56Z- handleDepth()-Depth_Keller msg: 1613178416158 epoch sec; seqNo:18661; depth 9.03 2021-02-13T01:06:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178416.19 epoch sec; seqNo:18661; 4060869.51 north; 593698.72 east 2021-02-13T01:06:56Z- AHRS_M2 msg: 1613178416.56 epoch sec; seqNo:18513 2021-02-13T01:06:56Z- AHRS_M2 msg: 0.61 phi; 0.33 theta; -0.16 psi 2021-02-13T01:06:56Z- handleDepth()-Depth_Keller msg: 1613178416560 epoch sec; seqNo:18662; depth 8.91 2021-02-13T01:06:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178416.57 epoch sec; seqNo:18662; 4060869.51 north; 593698.72 east 2021-02-13T01:06:56Z- AHRS_M2 msg: 1613178416.96 epoch sec; seqNo:18514 2021-02-13T01:06:56Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T01:06:56Z- handleDepth()-Depth_Keller msg: 1613178416966 epoch sec; seqNo:18663; depth 8.79 2021-02-13T01:06:56Z- handleDvl() - RDI_Pathfinder msg: 1613178416.98 epoch sec; seqNo:9030 2021-02-13T01:06:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 104.68 , 118.07 , 80.88 2021-02-13T01:06:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.06 , -0.08 2021-02-13T01:06:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178416.99 epoch sec; seqNo:18663; 4060869.89 north; 593698.72 east 2021-02-13T01:06:57Z- AHRS_M2 msg: 1613178417.36 epoch sec; seqNo:18515 2021-02-13T01:06:57Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T01:06:57Z- handleDepth()-Depth_Keller msg: 1613178417368 epoch sec; seqNo:18664; depth 8.69 2021-02-13T01:06:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178417.39 epoch sec; seqNo:18664; 4060870.26 north; 593698.71 east 2021-02-13T01:06:57Z- AHRS_M2 msg: 1613178417.77 epoch sec; seqNo:18516 2021-02-13T01:06:57Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.16 psi 2021-02-13T01:06:57Z- handleDepth()-Depth_Keller msg: 1613178417786 epoch sec; seqNo:18665; depth 8.60 2021-02-13T01:06:57Z- handleDvl() - RDI_Pathfinder msg: 1613178417.80 epoch sec; seqNo:9031 2021-02-13T01:06:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.58 , 103.81 , 118.68 , 80.81 2021-02-13T01:06:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , -0.04 2021-02-13T01:06:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178417.81 epoch sec; seqNo:18665; 4060870.64 north; 593698.71 east 2021-02-13T01:06:58Z- AHRS_M2 msg: 1613178418.17 epoch sec; seqNo:18517 2021-02-13T01:06:58Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.15 psi 2021-02-13T01:06:58Z- handleDepth()-Depth_Keller msg: 1613178418181 epoch sec; seqNo:18666; depth 8.48 2021-02-13T01:06:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178418.19 epoch sec; seqNo:18666; 4060870.64 north; 593698.71 east 2021-02-13T01:06:58Z- AHRS_M2 msg: 1613178418.59 epoch sec; seqNo:18518 2021-02-13T01:06:58Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.16 psi 2021-02-13T01:06:58Z- handleDepth()-Depth_Keller msg: 1613178418591 epoch sec; seqNo:18667; depth 8.38 2021-02-13T01:06:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178418.60 epoch sec; seqNo:18667; 4060871.04 north; 593699.93 east 2021-02-13T01:06:58Z- AHRS_M2 msg: 1613178418.98 epoch sec; seqNo:18519 2021-02-13T01:06:58Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.17 psi 2021-02-13T01:06:58Z- handleDepth()-Depth_Keller msg: 1613178418989 epoch sec; seqNo:18668; depth 8.27 2021-02-13T01:06:59Z- handleDvl() - RDI_Pathfinder msg: 1613178419.01 epoch sec; seqNo:9032 2021-02-13T01:06:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.25 , 104.01 , 117.54 , 81.02 2021-02-13T01:06:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.03 , 0.04 2021-02-13T01:06:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178419.02 epoch sec; seqNo:18668; 4060871.41 north; 593699.92 east 2021-02-13T01:06:59Z- AHRS_M2 msg: 1613178419.39 epoch sec; seqNo:18520 2021-02-13T01:06:59Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-13T01:06:59Z- handleDepth()-Depth_Keller msg: 1613178419393 epoch sec; seqNo:18669; depth 8.17 2021-02-13T01:06:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178419.40 epoch sec; seqNo:18669; 4060871.41 north; 593699.92 east 2021-02-13T01:06:59Z- AHRS_M2 msg: 1613178419.79 epoch sec; seqNo:18521 2021-02-13T01:06:59Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-13T01:06:59Z- handleDepth()-Depth_Keller msg: 1613178419794 epoch sec; seqNo:18670; depth 8.07 2021-02-13T01:06:59Z- handleDvl() - RDI_Pathfinder msg: 1613178419.81 epoch sec; seqNo:9033 2021-02-13T01:06:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 103.81 , 121.39 , 0.00 2021-02-13T01:06:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.07 , 0.06 2021-02-13T01:06:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178419.82 epoch sec; seqNo:18670; 4060871.79 north; 593699.92 east 2021-02-13T01:07:00Z- AHRS_M2 msg: 1613178420.19 epoch sec; seqNo:18522 2021-02-13T01:07:00Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T01:07:00Z- handleDepth()-Depth_Keller msg: 1613178420201 epoch sec; seqNo:18671; depth 7.97 2021-02-13T01:07:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178420.21 epoch sec; seqNo:18671; 4060872.17 north; 593699.91 east 2021-02-13T01:07:00Z- AHRS_M2 msg: 1613178420.60 epoch sec; seqNo:18523 2021-02-13T01:07:00Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T01:07:00Z- handleDepth()-Depth_Keller msg: 1613178420602 epoch sec; seqNo:18672; depth 7.87 2021-02-13T01:07:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178420.61 epoch sec; seqNo:18672; 4060872.17 north; 593699.91 east 2021-02-13T01:07:00Z- LcmTrn::handleMessages() - handling 40 messages took 0.60 ms and 128 calls... 2021-02-13T01:07:00Z- waiting for 1613178418.79, time is 1613178420.61 2021-02-13T01:07:00Z- LcmTrn::updateTrn() >>>> pose time 1613178420.61, meas time 1613178419.81 <<<< 2021-02-13T01:07:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178419.81 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:07:00Z- TerrainNav::motionUpdate() - currEstimate = (4060872, 593699.91, 7.87) 2021-02-13T01:07:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:07:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:07:00Z- TNavBF::Projecting Measurements 2021-02-13T01:07:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:07:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:07:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:07:00Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:07:00Z- TNavFilter::3 good beams. 2021-02-13T01:07:00Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:07:00Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:07:00Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:07:00Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:07:00Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:07:00Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:07:00Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:07:00Z- TNavBF::mmse selected from filter -1 2021-02-13T01:07:00Z- TerrainMapOctree: (vehN, vehE ) = (4060871.66, 593699.72). 2021-02-13T01:07:00Z- TerrainMapOctree: Min Distance = 667.80. 2021-02-13T01:07:00Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:07:00Z- There is no specific good beam that all particles have in common. 2021-02-13T01:07:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:07:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.39 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 12.65 2021-02-13T01:07:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:07:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.27 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 17.24 2021-02-13T01:07:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:07:00Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 23.58 2021-02-13T01:07:00Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:07:00Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:07:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:07:00Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:07:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:07:00Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:07:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:07:00Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:07:00Z- Filter 0 No beams used in subcloud update 2021-02-13T01:07:00Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:07:00Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:07:00Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 675.5 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T01:07:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178419.81 sec. 2021-02-13T01:07:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:07:00Z- TNavBF::mmse selected from filter -1 2021-02-13T01:07:00Z- variances([0], [2], and psi_berg) = 675.451947 832.986594 0.000000 2021-02-13T01:07:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:07:00Z- LcmTrn::publishEstimates() - MLE : 4060871.67 593715.41 7.87 2021-02-13T01:07:00Z- LcmTrn::publishEstimates() - MMSE : 4060872.17 593699.91 7.87 2021-02-13T01:07:00Z- LcmTrn::publishEstimates() - COVAR: 675.45 832.99 1.16 0.00 2021-02-13T01:07:01Z- AHRS_M2 msg: 1613178421.00 epoch sec; seqNo:18524 2021-02-13T01:07:01Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.18 psi 2021-02-13T01:07:01Z- handleDepth()-Depth_Keller msg: 1613178421006 epoch sec; seqNo:18673; depth 7.76 2021-02-13T01:07:01Z- handleDvl() - RDI_Pathfinder msg: 1613178421.02 epoch sec; seqNo:9034 2021-02-13T01:07:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.72 , 105.16 , 118.55 , 81.36 2021-02-13T01:07:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.07 , -0.00 2021-02-13T01:07:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178421.03 epoch sec; seqNo:18673; 4060872.56 north; 593701.13 east 2021-02-13T01:07:01Z- AHRS_M2 msg: 1613178421.41 epoch sec; seqNo:18525 2021-02-13T01:07:01Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.20 psi 2021-02-13T01:07:01Z- handleDepth()-Depth_Keller msg: 1613178421412 epoch sec; seqNo:18674; depth 7.63 2021-02-13T01:07:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178421.43 epoch sec; seqNo:18674; 4060872.56 north; 593701.13 east 2021-02-13T01:07:01Z- AHRS_M2 msg: 1613178421.81 epoch sec; seqNo:18526 2021-02-13T01:07:01Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.21 psi 2021-02-13T01:07:01Z- handleDepth()-Depth_Keller msg: 1613178421812 epoch sec; seqNo:18675; depth 7.53 2021-02-13T01:07:01Z- handleDvl() - RDI_Pathfinder msg: 1613178421.82 epoch sec; seqNo:9035 2021-02-13T01:07:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.98 , 107.80 , 118.62 , 82.10 2021-02-13T01:07:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , -0.03 2021-02-13T01:07:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178421.85 epoch sec; seqNo:18675; 4060872.94 north; 593701.13 east 2021-02-13T01:07:02Z- AHRS_M2 msg: 1613178422.21 epoch sec; seqNo:18527 2021-02-13T01:07:02Z- AHRS_M2 msg: 0.58 phi; 0.33 theta; -0.21 psi 2021-02-13T01:07:02Z- handleDepth()-Depth_Keller msg: 1613178422216 epoch sec; seqNo:18676; depth 7.43 2021-02-13T01:07:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178422.23 epoch sec; seqNo:18676; 4060873.32 north; 593701.12 east 2021-02-13T01:07:02Z- AHRS_M2 msg: 1613178422.62 epoch sec; seqNo:18528 2021-02-13T01:07:02Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.20 psi 2021-02-13T01:07:02Z- handleDepth()-Depth_Keller msg: 1613178422620 epoch sec; seqNo:18677; depth 7.33 2021-02-13T01:07:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178422.63 epoch sec; seqNo:18677; 4060873.70 north; 593701.12 east 2021-02-13T01:07:03Z- AHRS_M2 msg: 1613178423.02 epoch sec; seqNo:18529 2021-02-13T01:07:03Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.19 psi 2021-02-13T01:07:03Z- handleDepth()-Depth_Keller msg: 1613178423029 epoch sec; seqNo:18678; depth 7.22 2021-02-13T01:07:03Z- handleDvl() - RDI_Pathfinder msg: 1613178423.04 epoch sec; seqNo:9036 2021-02-13T01:07:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.58 , 109.89 , 121.80 , 83.11 2021-02-13T01:07:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , 0.04 2021-02-13T01:07:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178423.06 epoch sec; seqNo:18678; 4060874.08 north; 593701.11 east 2021-02-13T01:07:03Z- AHRS_M2 msg: 1613178423.42 epoch sec; seqNo:18530 2021-02-13T01:07:03Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.18 psi 2021-02-13T01:07:03Z- handleDepth()-Depth_Keller msg: 1613178423428 epoch sec; seqNo:18679; depth 7.12 2021-02-13T01:07:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178423.44 epoch sec; seqNo:18679; 4060874.08 north; 593701.11 east 2021-02-13T01:07:03Z- AHRS_M2 msg: 1613178423.83 epoch sec; seqNo:18531 2021-02-13T01:07:03Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.18 psi 2021-02-13T01:07:03Z- handleDepth()-Depth_Keller msg: 1613178423835 epoch sec; seqNo:18680; depth 7.01 2021-02-13T01:07:03Z- handleDvl() - RDI_Pathfinder msg: 1613178423.85 epoch sec; seqNo:9037 2021-02-13T01:07:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:07:03Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:07:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178423.86 epoch sec; seqNo:18680; 4060874.47 north; 593702.33 east 2021-02-13T01:07:04Z- AHRS_M2 msg: 1613178424.23 epoch sec; seqNo:18532 2021-02-13T01:07:04Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.18 psi 2021-02-13T01:07:04Z- handleDepth()-Depth_Keller msg: 1613178424241 epoch sec; seqNo:18681; depth 6.90 2021-02-13T01:07:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178424.26 epoch sec; seqNo:18681; 4060874.85 north; 593702.33 east 2021-02-13T01:07:04Z- AHRS_M2 msg: 1613178424.64 epoch sec; seqNo:18533 2021-02-13T01:07:04Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.18 psi 2021-02-13T01:07:04Z- handleDepth()-Depth_Keller msg: 1613178424640 epoch sec; seqNo:18682; depth 6.78 2021-02-13T01:07:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178424.65 epoch sec; seqNo:18682; 4060875.23 north; 593702.32 east 2021-02-13T01:07:05Z- AHRS_M2 msg: 1613178425.04 epoch sec; seqNo:18534 2021-02-13T01:07:05Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.18 psi 2021-02-13T01:07:05Z- handleDepth()-Depth_Keller msg: 1613178425049 epoch sec; seqNo:18683; depth 6.68 2021-02-13T01:07:05Z- handleDvl() - RDI_Pathfinder msg: 1613178425.06 epoch sec; seqNo:9038 2021-02-13T01:07:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.81 , 109.15 , 124.03 , 82.37 2021-02-13T01:07:05Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.08 , 0.04 2021-02-13T01:07:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178425.07 epoch sec; seqNo:18683; 4060875.61 north; 593702.32 east 2021-02-13T01:07:05Z- AHRS_M2 msg: 1613178425.44 epoch sec; seqNo:18535 2021-02-13T01:07:05Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.16 psi 2021-02-13T01:07:05Z- handleDepth()-Depth_Keller msg: 1613178425457 epoch sec; seqNo:18684; depth 6.56 2021-02-13T01:07:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178425.47 epoch sec; seqNo:18684; 4060875.61 north; 593702.32 east 2021-02-13T01:07:05Z- AHRS_M2 msg: 1613178425.85 epoch sec; seqNo:18536 2021-02-13T01:07:05Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.15 psi 2021-02-13T01:07:05Z- handleDepth()-Depth_Keller msg: 1613178425852 epoch sec; seqNo:18685; depth 6.47 2021-02-13T01:07:05Z- handleDvl() - RDI_Pathfinder msg: 1613178425.86 epoch sec; seqNo:9039 2021-02-13T01:07:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.55 , 106.92 , 126.26 , 81.62 2021-02-13T01:07:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.16 , -0.02 2021-02-13T01:07:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178425.88 epoch sec; seqNo:18685; 4060876.00 north; 593703.53 east 2021-02-13T01:07:05Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-13T01:07:05Z- waiting for 1613178423.61, time is 1613178425.88 2021-02-13T01:07:05Z- LcmTrn::updateTrn() equating pose 1613178425.88 and meas times 1613178425.88 2021-02-13T01:07:05Z- LcmTrn::updateTrn() >>>> pose time 1613178425.88, meas time 1613178425.88 <<<< 2021-02-13T01:07:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178425.88 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:07:05Z- TerrainNav::motionUpdate() - currEstimate = (4060876, 593703.53, 6.47) 2021-02-13T01:07:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:07:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:07:05Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178425.88 sec.; No relevant navigation data available 2021-02-13T01:07:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:07:05Z- TNavBF::mmse selected from filter -1 2021-02-13T01:07:05Z- variances([0], [2], and psi_berg) = 675.403876 832.986575 0.000000 2021-02-13T01:07:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:07:05Z- LcmTrn::publishEstimates() - MLE : 4060875.50 593719.03 6.47 2021-02-13T01:07:05Z- LcmTrn::publishEstimates() - MMSE : 4060876.00 593703.53 6.47 2021-02-13T01:07:05Z- LcmTrn::publishEstimates() - COVAR: 675.40 832.99 1.43 0.00 2021-02-13T01:07:06Z- AHRS_M2 msg: 1613178426.25 epoch sec; seqNo:18537 2021-02-13T01:07:06Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.13 psi 2021-02-13T01:07:06Z- handleDepth()-Depth_Keller msg: 1613178426263 epoch sec; seqNo:18686; depth 6.34 2021-02-13T01:07:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178426.27 epoch sec; seqNo:18686; 4060876.38 north; 593703.53 east 2021-02-13T01:07:06Z- AHRS_M2 msg: 1613178426.66 epoch sec; seqNo:18538 2021-02-13T01:07:06Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.11 psi 2021-02-13T01:07:06Z- handleDepth()-Depth_Keller msg: 1613178426665 epoch sec; seqNo:18687; depth 6.25 2021-02-13T01:07:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178426.67 epoch sec; seqNo:18687; 4060876.38 north; 593703.53 east 2021-02-13T01:07:07Z- AHRS_M2 msg: 1613178427.06 epoch sec; seqNo:18539 2021-02-13T01:07:07Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.11 psi 2021-02-13T01:07:07Z- handleDepth()-Depth_Keller msg: 1613178427072 epoch sec; seqNo:18688; depth 6.12 2021-02-13T01:07:07Z- handleDvl() - RDI_Pathfinder msg: 1613178427.08 epoch sec; seqNo:9040 2021-02-13T01:07:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 105.43 , 124.77 , 83.11 2021-02-13T01:07:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.17 , -0.16 2021-02-13T01:07:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178427.10 epoch sec; seqNo:18688; 4060876.76 north; 593703.53 east 2021-02-13T01:07:07Z- AHRS_M2 msg: 1613178427.46 epoch sec; seqNo:18540 2021-02-13T01:07:07Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.12 psi 2021-02-13T01:07:07Z- handleDepth()-Depth_Keller msg: 1613178427468 epoch sec; seqNo:18689; depth 6.00 2021-02-13T01:07:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178427.48 epoch sec; seqNo:18689; 4060876.76 north; 593703.53 east 2021-02-13T01:07:07Z- AHRS_M2 msg: 1613178427.87 epoch sec; seqNo:18541 2021-02-13T01:07:07Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.14 psi 2021-02-13T01:07:07Z- handleDepth()-Depth_Keller msg: 1613178427872 epoch sec; seqNo:18690; depth 5.87 2021-02-13T01:07:07Z- handleDvl() - RDI_Pathfinder msg: 1613178427.88 epoch sec; seqNo:9041 2021-02-13T01:07:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 105.29 , 121.65 , 81.49 2021-02-13T01:07:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.21 2021-02-13T01:07:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178427.89 epoch sec; seqNo:18690; 4060877.14 north; 593703.52 east 2021-02-13T01:07:08Z- AHRS_M2 msg: 1613178428.27 epoch sec; seqNo:18542 2021-02-13T01:07:08Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.15 psi 2021-02-13T01:07:08Z- handleDepth()-Depth_Keller msg: 1613178428279 epoch sec; seqNo:18691; depth 5.75 2021-02-13T01:07:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178428.29 epoch sec; seqNo:18691; 4060877.53 north; 593704.74 east 2021-02-13T01:07:08Z- AHRS_M2 msg: 1613178428.68 epoch sec; seqNo:18543 2021-02-13T01:07:08Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.15 psi 2021-02-13T01:07:08Z- handleDepth()-Depth_Keller msg: 1613178428682 epoch sec; seqNo:18692; depth 5.61 2021-02-13T01:07:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178428.69 epoch sec; seqNo:18692; 4060877.53 north; 593704.74 east 2021-02-13T01:07:09Z- AHRS_M2 msg: 1613178429.08 epoch sec; seqNo:18544 2021-02-13T01:07:09Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.15 psi 2021-02-13T01:07:09Z- handleDepth()-Depth_Keller msg: 1613178429084 epoch sec; seqNo:18693; depth 5.48 2021-02-13T01:07:09Z- handleDvl() - RDI_Pathfinder msg: 1613178429.10 epoch sec; seqNo:9042 2021-02-13T01:07:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.04 , 108.40 , 123.28 , 81.62 2021-02-13T01:07:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.14 , -0.03 2021-02-13T01:07:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178429.11 epoch sec; seqNo:18693; 4060877.91 north; 593704.73 east 2021-02-13T01:07:09Z- AHRS_M2 msg: 1613178429.48 epoch sec; seqNo:18545 2021-02-13T01:07:09Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.14 psi 2021-02-13T01:07:09Z- handleDepth()-Depth_Keller msg: 1613178429489 epoch sec; seqNo:18694; depth 5.33 2021-02-13T01:07:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178429.51 epoch sec; seqNo:18694; 4060878.29 north; 593704.73 east 2021-02-13T01:07:09Z- AHRS_M2 msg: 1613178429.89 epoch sec; seqNo:18546 2021-02-13T01:07:09Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.13 psi 2021-02-13T01:07:09Z- handleDepth()-Depth_Keller msg: 1613178429893 epoch sec; seqNo:18695; depth 5.20 2021-02-13T01:07:09Z- handleDvl() - RDI_Pathfinder msg: 1613178429.90 epoch sec; seqNo:9043 2021-02-13T01:07:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:07:09Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:07:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178429.91 epoch sec; seqNo:18695; 4060878.67 north; 593704.73 east 2021-02-13T01:07:10Z- AHRS_M2 msg: 1613178430.29 epoch sec; seqNo:18547 2021-02-13T01:07:10Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.12 psi 2021-02-13T01:07:10Z- handleDepth()-Depth_Keller msg: 1613178430302 epoch sec; seqNo:18696; depth 5.07 2021-02-13T01:07:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178430.31 epoch sec; seqNo:18696; 4060879.04 north; 593704.72 east 2021-02-13T01:07:10Z- AHRS_M2 msg: 1613178430.70 epoch sec; seqNo:18548 2021-02-13T01:07:10Z- AHRS_M2 msg: 0.65 phi; 0.27 theta; -0.11 psi 2021-02-13T01:07:10Z- handleDepth()-Depth_Keller msg: 1613178430705 epoch sec; seqNo:18697; depth 4.94 2021-02-13T01:07:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178430.71 epoch sec; seqNo:18697; 4060879.42 north; 593704.72 east 2021-02-13T01:07:11Z- LcmTrn::handleMessages() - handling 40 messages took 0.61 ms and 128 calls... 2021-02-13T01:07:11Z- waiting for 1613178428.88, time is 1613178430.71 2021-02-13T01:07:11Z- LcmTrn::updateTrn() >>>> pose time 1613178430.71, meas time 1613178429.90 <<<< 2021-02-13T01:07:11Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613178429.90, ping # 0. 2021-02-13T01:07:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178429.90 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:07:11Z- TerrainNav::motionUpdate() - currEstimate = (4060879, 593704.72, 4.94) 2021-02-13T01:07:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:07:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:07:11Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:07:11Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:07:11Z- TNavBF::Projecting Measurements 2021-02-13T01:07:11Z- TNavFilter::Measurement from time = 1613178429.90 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T01:07:11Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:07:11Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:07:11Z- TNavBF:: beamIndex[1] = 0 2021-02-13T01:07:11Z- TNavBF:: beamIndex[2] = 0 2021-02-13T01:07:11Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:07:11Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:07:11Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:07:11Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 675.4 EastVariance: 833.0 DepthVariance: 1.6 this->DepthBiasCov: 1.6 2021-02-13T01:07:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:07:11Z- TNavBF::mmse selected from filter -1 2021-02-13T01:07:11Z- variances([0], [2], and psi_berg) = 675.351241 832.986553 0.000000 2021-02-13T01:07:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:07:11Z- LcmTrn::publishEstimates() - MLE : 4060878.92 593692.22 4.94 2021-02-13T01:07:11Z- LcmTrn::publishEstimates() - MMSE : 4060879.42 593704.72 4.94 2021-02-13T01:07:11Z- LcmTrn::publishEstimates() - COVAR: 675.35 832.99 1.62 0.00 2021-02-13T01:07:11Z- AHRS_M2 msg: 1613178431.10 epoch sec; seqNo:18549 2021-02-13T01:07:11Z- AHRS_M2 msg: 0.68 phi; 0.22 theta; -0.11 psi 2021-02-13T01:07:11Z- handleDepth()-Depth_Keller msg: 1613178431104 epoch sec; seqNo:18698; depth 4.80 2021-02-13T01:07:11Z- handleDvl() - RDI_Pathfinder msg: 1613178431.12 epoch sec; seqNo:9044 2021-02-13T01:07:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.25 , 106.17 , 122.54 , 83.86 2021-02-13T01:07:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.15 , -0.19 2021-02-13T01:07:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178431.13 epoch sec; seqNo:18698; 4060879.44 north; 593705.94 east 2021-02-13T01:07:11Z- AHRS_M2 msg: 1613178431.50 epoch sec; seqNo:18550 2021-02-13T01:07:11Z- AHRS_M2 msg: 0.70 phi; 0.16 theta; -0.11 psi 2021-02-13T01:07:11Z- handleDepth()-Depth_Keller msg: 1613178431508 epoch sec; seqNo:18699; depth 4.68 2021-02-13T01:07:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178431.53 epoch sec; seqNo:18699; 4060879.82 north; 593705.93 east 2021-02-13T01:07:11Z- AHRS_M2 msg: 1613178431.91 epoch sec; seqNo:18551 2021-02-13T01:07:11Z- AHRS_M2 msg: 0.71 phi; 0.10 theta; -0.12 psi 2021-02-13T01:07:11Z- handleDepth()-Depth_Keller msg: 1613178431915 epoch sec; seqNo:18700; depth 4.55 2021-02-13T01:07:11Z- handleDvl() - RDI_Pathfinder msg: 1613178431.93 epoch sec; seqNo:9045 2021-02-13T01:07:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.48 , 100.89 , 108.33 , 133.62 2021-02-13T01:07:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.58 , -0.05 , -0.17 2021-02-13T01:07:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178431.95 epoch sec; seqNo:18700; 4060879.82 north; 593705.93 east 2021-02-13T01:07:12Z- AHRS_M2 msg: 1613178432.31 epoch sec; seqNo:18552 2021-02-13T01:07:12Z- AHRS_M2 msg: 0.72 phi; 0.04 theta; -0.13 psi 2021-02-13T01:07:12Z- handleDepth()-Depth_Keller msg: 1613178432321 epoch sec; seqNo:18701; depth 4.41 2021-02-13T01:07:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178432.33 epoch sec; seqNo:18701; 4060880.19 north; 593705.93 east 2021-02-13T01:07:12Z- AHRS_M2 msg: 1613178432.72 epoch sec; seqNo:18553 2021-02-13T01:07:12Z- AHRS_M2 msg: 0.72 phi; -0.01 theta; -0.14 psi 2021-02-13T01:07:12Z- handleDepth()-Depth_Keller msg: 1613178432725 epoch sec; seqNo:18702; depth 4.33 2021-02-13T01:07:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178432.73 epoch sec; seqNo:18702; 4060880.19 north; 593705.93 east 2021-02-13T01:07:13Z- AHRS_M2 msg: 1613178433.12 epoch sec; seqNo:18554 2021-02-13T01:07:13Z- AHRS_M2 msg: 0.71 phi; -0.06 theta; -0.16 psi 2021-02-13T01:07:13Z- handleDepth()-Depth_Keller msg: 1613178433129 epoch sec; seqNo:18703; depth 4.24 2021-02-13T01:07:13Z- handleDvl() - RDI_Pathfinder msg: 1613178433.15 epoch sec; seqNo:9046 2021-02-13T01:07:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.96 , 100.35 , 97.38 , 97.38 2021-02-13T01:07:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.60 , -0.16 , 0.15 2021-02-13T01:07:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178433.16 epoch sec; seqNo:18703; 4060880.57 north; 593705.92 east 2021-02-13T01:07:13Z- AHRS_M2 msg: 1613178433.52 epoch sec; seqNo:18555 2021-02-13T01:07:13Z- AHRS_M2 msg: 0.71 phi; -0.10 theta; -0.19 psi 2021-02-13T01:07:13Z- handleDepth()-Depth_Keller msg: 1613178433528 epoch sec; seqNo:18704; depth 4.17 2021-02-13T01:07:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178433.54 epoch sec; seqNo:18704; 4060880.95 north; 593705.92 east 2021-02-13T01:07:13Z- AHRS_M2 msg: 1613178433.93 epoch sec; seqNo:18556 2021-02-13T01:07:13Z- AHRS_M2 msg: 0.71 phi; -0.13 theta; -0.21 psi 2021-02-13T01:07:13Z- handleDepth()-Depth_Keller msg: 1613178433933 epoch sec; seqNo:18705; depth 4.14 2021-02-13T01:07:13Z- handleDvl() - RDI_Pathfinder msg: 1613178433.94 epoch sec; seqNo:9047 2021-02-13T01:07:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.63 , 106.92 , 97.45 , 100.15 2021-02-13T01:07:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.02 , 0.46 2021-02-13T01:07:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178433.95 epoch sec; seqNo:18705; 4060880.95 north; 593705.92 east 2021-02-13T01:07:14Z- AHRS_M2 msg: 1613178434.33 epoch sec; seqNo:18557 2021-02-13T01:07:14Z- AHRS_M2 msg: 0.70 phi; -0.16 theta; -0.22 psi 2021-02-13T01:07:14Z- handleDepth()-Depth_Keller msg: 1613178434337 epoch sec; seqNo:18706; depth 4.10 2021-02-13T01:07:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178434.35 epoch sec; seqNo:18706; 4060881.34 north; 593707.14 east 2021-02-13T01:07:14Z- AHRS_M2 msg: 1613178434.74 epoch sec; seqNo:18558 2021-02-13T01:07:14Z- AHRS_M2 msg: 0.70 phi; -0.19 theta; -0.23 psi 2021-02-13T01:07:14Z- handleDepth()-Depth_Keller msg: 1613178434740 epoch sec; seqNo:18707; depth 4.11 2021-02-13T01:07:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178434.75 epoch sec; seqNo:18707; 4060881.34 north; 593707.14 east 2021-02-13T01:07:15Z- AHRS_M2 msg: 1613178435.14 epoch sec; seqNo:18559 2021-02-13T01:07:15Z- AHRS_M2 msg: 0.69 phi; -0.21 theta; -0.24 psi 2021-02-13T01:07:15Z- handleDepth()-Depth_Keller msg: 1613178435154 epoch sec; seqNo:18708; depth 4.13 2021-02-13T01:07:15Z- handleDvl() - RDI_Pathfinder msg: 1613178435.16 epoch sec; seqNo:9048 2021-02-13T01:07:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.43 , 110.77 , 97.25 , 106.71 2021-02-13T01:07:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.26 , 0.41 2021-02-13T01:07:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178435.17 epoch sec; seqNo:18708; 4060881.34 north; 593707.14 east 2021-02-13T01:07:15Z- AHRS_M2 msg: 1613178435.55 epoch sec; seqNo:18560 2021-02-13T01:07:15Z- AHRS_M2 msg: 0.68 phi; -0.24 theta; -0.25 psi 2021-02-13T01:07:15Z- handleDepth()-Depth_Keller msg: 1613178435556 epoch sec; seqNo:18709; depth 4.14 2021-02-13T01:07:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178435.57 epoch sec; seqNo:18709; 4060881.72 north; 593707.13 east 2021-02-13T01:07:15Z- AHRS_M2 msg: 1613178435.95 epoch sec; seqNo:18561 2021-02-13T01:07:15Z- AHRS_M2 msg: 0.69 phi; -0.26 theta; -0.26 psi 2021-02-13T01:07:15Z- handleDepth()-Depth_Keller msg: 1613178435962 epoch sec; seqNo:18710; depth 4.19 2021-02-13T01:07:15Z- handleDvl() - RDI_Pathfinder msg: 1613178435.97 epoch sec; seqNo:9049 2021-02-13T01:07:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.57 , 115.97 , 0.00 , 114.62 2021-02-13T01:07:15Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.38 , 0.09 2021-02-13T01:07:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178435.98 epoch sec; seqNo:18710; 4060881.74 north; 593708.35 east 2021-02-13T01:07:16Z- LcmTrn::handleMessages() - handling 45 messages took 0.70 ms and 131 calls... 2021-02-13T01:07:16Z- waiting for 1613178433.71, time is 1613178435.98 2021-02-13T01:07:16Z- LcmTrn::updateTrn() equating pose 1613178435.98 and meas times 1613178435.98 2021-02-13T01:07:16Z- LcmTrn::updateTrn() >>>> pose time 1613178435.98, meas time 1613178435.98 <<<< 2021-02-13T01:07:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178435.98 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:07:16Z- TerrainNav::motionUpdate() - currEstimate = (4060882, 593708.35, 4.19) 2021-02-13T01:07:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:07:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:07:16Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178435.98 sec.; No relevant navigation data available 2021-02-13T01:07:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:07:16Z- TNavBF::mmse selected from filter -1 2021-02-13T01:07:16Z- variances([0], [2], and psi_berg) = 675.307810 832.986536 0.000000 2021-02-13T01:07:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:07:16Z- LcmTrn::publishEstimates() - MLE : 4060881.24 593695.85 4.19 2021-02-13T01:07:16Z- LcmTrn::publishEstimates() - MMSE : 4060881.74 593708.35 4.19 2021-02-13T01:07:16Z- LcmTrn::publishEstimates() - COVAR: 675.31 832.99 1.84 0.00 2021-02-13T01:07:16Z- AHRS_M2 msg: 1613178436.35 epoch sec; seqNo:18562 2021-02-13T01:07:16Z- AHRS_M2 msg: 0.70 phi; -0.27 theta; -0.26 psi 2021-02-13T01:07:16Z- handleDepth()-Depth_Keller msg: 1613178436358 epoch sec; seqNo:18711; depth 4.26 2021-02-13T01:07:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178436.37 epoch sec; seqNo:18711; 4060882.12 north; 593708.35 east 2021-02-13T01:07:16Z- AHRS_M2 msg: 1613178436.76 epoch sec; seqNo:18563 2021-02-13T01:07:16Z- AHRS_M2 msg: 0.70 phi; -0.28 theta; -0.26 psi 2021-02-13T01:07:16Z- handleDepth()-Depth_Keller msg: 1613178436761 epoch sec; seqNo:18712; depth 4.31 2021-02-13T01:07:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178436.77 epoch sec; seqNo:18712; 4060882.12 north; 593708.35 east 2021-02-13T01:07:17Z- AHRS_M2 msg: 1613178437.16 epoch sec; seqNo:18564 2021-02-13T01:07:17Z- AHRS_M2 msg: 0.70 phi; -0.29 theta; -0.26 psi 2021-02-13T01:07:17Z- handleDepth()-Depth_Keller msg: 1613178437166 epoch sec; seqNo:18713; depth 4.37 2021-02-13T01:07:17Z- handleDvl() - RDI_Pathfinder msg: 1613178437.18 epoch sec; seqNo:9050 2021-02-13T01:07:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.17 , 113.94 , 87.17 , 118.41 2021-02-13T01:07:17Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , 0.28 , -0.20 2021-02-13T01:07:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178437.19 epoch sec; seqNo:18713; 4060882.49 north; 593708.34 east 2021-02-13T01:07:17Z- AHRS_M2 msg: 1613178437.56 epoch sec; seqNo:18565 2021-02-13T01:07:17Z- AHRS_M2 msg: 0.70 phi; -0.30 theta; -0.27 psi 2021-02-13T01:07:17Z- handleDepth()-Depth_Keller msg: 1613178437568 epoch sec; seqNo:18714; depth 4.46 2021-02-13T01:07:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178437.58 epoch sec; seqNo:18714; 4060882.89 north; 593709.56 east 2021-02-13T01:07:17Z- AHRS_M2 msg: 1613178437.97 epoch sec; seqNo:18566 2021-02-13T01:07:17Z- AHRS_M2 msg: 0.70 phi; -0.31 theta; -0.26 psi 2021-02-13T01:07:17Z- handleDepth()-Depth_Keller msg: 1613178437972 epoch sec; seqNo:18715; depth 4.53 2021-02-13T01:07:17Z- handleDvl() - RDI_Pathfinder msg: 1613178437.98 epoch sec; seqNo:9051 2021-02-13T01:07:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 120.31 , 86.09 , 121.80 2021-02-13T01:07:17Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , -0.02 , -0.21 2021-02-13T01:07:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178437.99 epoch sec; seqNo:18715; 4060883.27 north; 593709.56 east 2021-02-13T01:07:18Z- AHRS_M2 msg: 1613178438.38 epoch sec; seqNo:18567 2021-02-13T01:07:18Z- AHRS_M2 msg: 0.70 phi; -0.32 theta; -0.25 psi 2021-02-13T01:07:18Z- handleDepth()-Depth_Keller msg: 1613178438382 epoch sec; seqNo:18716; depth 4.63 2021-02-13T01:07:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178438.39 epoch sec; seqNo:18716; 4060883.64 north; 593709.55 east 2021-02-13T01:07:18Z- AHRS_M2 msg: 1613178438.78 epoch sec; seqNo:18568 2021-02-13T01:07:18Z- AHRS_M2 msg: 0.69 phi; -0.34 theta; -0.23 psi 2021-02-13T01:07:18Z- handleDepth()-Depth_Keller msg: 1613178438780 epoch sec; seqNo:18717; depth 4.73 2021-02-13T01:07:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178438.80 epoch sec; seqNo:18717; 4060884.02 north; 593709.55 east 2021-02-13T01:07:19Z- AHRS_M2 msg: 1613178439.18 epoch sec; seqNo:18569 2021-02-13T01:07:19Z- AHRS_M2 msg: 0.68 phi; -0.35 theta; -0.21 psi 2021-02-13T01:07:19Z- handleDepth()-Depth_Keller msg: 1613178439184 epoch sec; seqNo:18718; depth 4.84 2021-02-13T01:07:19Z- handleDvl() - RDI_Pathfinder msg: 1613178439.20 epoch sec; seqNo:9052 2021-02-13T01:07:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.90 , 116.66 , 85.41 , 122.61 2021-02-13T01:07:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , 0.01 2021-02-13T01:07:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178439.22 epoch sec; seqNo:18718; 4060884.04 north; 593710.77 east 2021-02-13T01:07:19Z- AHRS_M2 msg: 1613178439.58 epoch sec; seqNo:18570 2021-02-13T01:07:19Z- AHRS_M2 msg: 0.67 phi; -0.36 theta; -0.20 psi 2021-02-13T01:07:19Z- handleDepth()-Depth_Keller msg: 1613178439593 epoch sec; seqNo:18719; depth 4.94 2021-02-13T01:07:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178439.60 epoch sec; seqNo:18719; 4060884.42 north; 593710.77 east 2021-02-13T01:07:19Z- AHRS_M2 msg: 1613178439.99 epoch sec; seqNo:18571 2021-02-13T01:07:19Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.20 psi 2021-02-13T01:07:19Z- handleDepth()-Depth_Keller msg: 1613178439993 epoch sec; seqNo:18720; depth 5.04 2021-02-13T01:07:20Z- handleDvl() - RDI_Pathfinder msg: 1613178440.00 epoch sec; seqNo:9053 2021-02-13T01:07:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.41 , 114.21 , 82.97 , 124.63 2021-02-13T01:07:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.14 , -0.10 2021-02-13T01:07:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178440.01 epoch sec; seqNo:18720; 4060884.79 north; 593710.76 east 2021-02-13T01:07:20Z- AHRS_M2 msg: 1613178440.39 epoch sec; seqNo:18572 2021-02-13T01:07:20Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.22 psi 2021-02-13T01:07:20Z- handleDepth()-Depth_Keller msg: 1613178440396 epoch sec; seqNo:18721; depth 5.16 2021-02-13T01:07:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178440.41 epoch sec; seqNo:18721; 4060884.79 north; 593710.76 east 2021-02-13T01:07:20Z- AHRS_M2 msg: 1613178440.80 epoch sec; seqNo:18573 2021-02-13T01:07:20Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.24 psi 2021-02-13T01:07:20Z- handleDepth()-Depth_Keller msg: 1613178440805 epoch sec; seqNo:18722; depth 5.27 2021-02-13T01:07:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178440.81 epoch sec; seqNo:18722; 4060885.17 north; 593710.76 east 2021-02-13T01:07:21Z- LcmTrn::handleMessages() - handling 40 messages took 0.61 ms and 128 calls... 2021-02-13T01:07:21Z- waiting for 1613178438.98, time is 1613178440.81 2021-02-13T01:07:21Z- LcmTrn::updateTrn() >>>> pose time 1613178440.81, meas time 1613178440.00 <<<< 2021-02-13T01:07:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178440.00 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:07:21Z- TerrainNav::motionUpdate() - currEstimate = (4060885, 593710.76, 5.27) 2021-02-13T01:07:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:07:21Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:07:21Z- TNavBF::Projecting Measurements 2021-02-13T01:07:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:07:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:07:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:07:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:07:21Z- TNavFilter::4 good beams. 2021-02-13T01:07:21Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:07:21Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:07:21Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:07:21Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:07:21Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:07:21Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:07:21Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:07:21Z- TNavBF::mmse selected from filter -1 2021-02-13T01:07:21Z- TerrainMapOctree: (vehN, vehE ) = (4060884.60, 593710.35). 2021-02-13T01:07:21Z- TerrainMapOctree: Min Distance = 651.72. 2021-02-13T01:07:21Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:07:21Z- There is no specific good beam that all particles have in common. 2021-02-13T01:07:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:07:21Z- Old Depth Bias: 0.00 DepthBiasCov: 2.02 K: 0.120 ExpectedMeasDiff: nan currMeas.covariance: 13.08 2021-02-13T01:07:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:07:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.77 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 20.87 2021-02-13T01:07:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:07:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.64 K: 0.114 ExpectedMeasDiff: nan currMeas.covariance: 11.02 2021-02-13T01:07:21Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:07:21Z- Old Depth Bias: 0.00 DepthBiasCov: 1.46 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 24.85 2021-02-13T01:07:21Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:07:21Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:07:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:07:21Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:07:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:07:21Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:07:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:07:21Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:07:21Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:07:21Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:07:21Z- Filter 0 No beams used in subcloud update 2021-02-13T01:07:21Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:07:21Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:07:21Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 675.3 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T01:07:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178440.00 sec. 2021-02-13T01:07:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:07:21Z- TNavBF::mmse selected from filter -1 2021-02-13T01:07:21Z- variances([0], [2], and psi_berg) = 675.251220 832.986513 0.000000 2021-02-13T01:07:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:07:21Z- LcmTrn::publishEstimates() - MLE : 4060884.67 593727.26 5.27 2021-02-13T01:07:21Z- LcmTrn::publishEstimates() - MMSE : 4060885.17 593710.76 5.27 2021-02-13T01:07:21Z- LcmTrn::publishEstimates() - COVAR: 675.25 832.99 1.42 0.00 2021-02-13T01:07:21Z- AHRS_M2 msg: 1613178441.20 epoch sec; seqNo:18574 2021-02-13T01:07:21Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.26 psi 2021-02-13T01:07:21Z- handleDepth()-Depth_Keller msg: 1613178441204 epoch sec; seqNo:18723; depth 5.36 2021-02-13T01:07:21Z- handleDvl() - RDI_Pathfinder msg: 1613178441.21 epoch sec; seqNo:9054 2021-02-13T01:07:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.41 , 114.21 , 82.97 , 129.09 2021-02-13T01:07:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , -0.29 2021-02-13T01:07:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178441.23 epoch sec; seqNo:18723; 4060885.55 north; 593710.75 east 2021-02-13T01:07:21Z- AHRS_M2 msg: 1613178441.61 epoch sec; seqNo:18575 2021-02-13T01:07:21Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.25 psi 2021-02-13T01:07:21Z- handleDepth()-Depth_Keller msg: 1613178441610 epoch sec; seqNo:18724; depth 5.49 2021-02-13T01:07:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178441.62 epoch sec; seqNo:18724; 4060885.57 north; 593711.97 east 2021-02-13T01:07:22Z- AHRS_M2 msg: 1613178442.01 epoch sec; seqNo:18576 2021-02-13T01:07:22Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.24 psi 2021-02-13T01:07:22Z- handleDepth()-Depth_Keller msg: 1613178442012 epoch sec; seqNo:18725; depth 5.61 2021-02-13T01:07:22Z- handleDvl() - RDI_Pathfinder msg: 1613178442.02 epoch sec; seqNo:9055 2021-02-13T01:07:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.37 , 120.11 , 82.91 , 126.06 2021-02-13T01:07:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.30 , -0.01 2021-02-13T01:07:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178442.05 epoch sec; seqNo:18725; 4060885.94 north; 593711.97 east 2021-02-13T01:07:22Z- AHRS_M2 msg: 1613178442.41 epoch sec; seqNo:18577 2021-02-13T01:07:22Z- AHRS_M2 msg: 0.66 phi; -0.37 theta; -0.22 psi 2021-02-13T01:07:22Z- handleDepth()-Depth_Keller msg: 1613178442416 epoch sec; seqNo:18726; depth 5.72 2021-02-13T01:07:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178442.43 epoch sec; seqNo:18726; 4060886.32 north; 593711.96 east 2021-02-13T01:07:22Z- AHRS_M2 msg: 1613178442.84 epoch sec; seqNo:18578 2021-02-13T01:07:22Z- AHRS_M2 msg: 0.66 phi; -0.38 theta; -0.20 psi 2021-02-13T01:07:22Z- handleDepth()-Depth_Keller msg: 1613178442849 epoch sec; seqNo:18727; depth 5.85 2021-02-13T01:07:22Z- handleDvl() - RDI_Pathfinder msg: 1613178442.86 epoch sec; seqNo:9056 2021-02-13T01:07:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.86 , 115.64 , 0.00 , 166.23 2021-02-13T01:07:22Z- handleDvl() - RDI_Pathfinder msg: velocities -0.35 , -0.07 , 0.32 2021-02-13T01:07:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178442.88 epoch sec; seqNo:18727; 4060886.32 north; 593711.96 east 2021-02-13T01:07:23Z- AHRS_M2 msg: 1613178443.22 epoch sec; seqNo:18579 2021-02-13T01:07:23Z- AHRS_M2 msg: 0.65 phi; -0.38 theta; -0.17 psi 2021-02-13T01:07:23Z- handleDepth()-Depth_Keller msg: 1613178443225 epoch sec; seqNo:18728; depth 6.00 2021-02-13T01:07:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178443.23 epoch sec; seqNo:18728; 4060885.93 north; 593710.75 east 2021-02-13T01:07:23Z- AHRS_M2 msg: 1613178443.62 epoch sec; seqNo:18580 2021-02-13T01:07:23Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.16 psi 2021-02-13T01:07:23Z- handleDepth()-Depth_Keller msg: 1613178443628 epoch sec; seqNo:18729; depth 6.11 2021-02-13T01:07:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178443.64 epoch sec; seqNo:18729; 4060885.93 north; 593710.75 east 2021-02-13T01:07:24Z- AHRS_M2 msg: 1613178444.03 epoch sec; seqNo:18581 2021-02-13T01:07:24Z- AHRS_M2 msg: 0.61 phi; -0.38 theta; -0.16 psi 2021-02-13T01:07:24Z- handleDepth()-Depth_Keller msg: 1613178444034 epoch sec; seqNo:18730; depth 6.23 2021-02-13T01:07:24Z- handleDvl() - RDI_Pathfinder msg: 1613178444.05 epoch sec; seqNo:9057 2021-02-13T01:07:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.29 , 107.65 , 82.37 , 124.02 2021-02-13T01:07:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.25 , 0.28 2021-02-13T01:07:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178444.06 epoch sec; seqNo:18730; 4060885.94 north; 593711.97 east 2021-02-13T01:07:24Z- AHRS_M2 msg: 1613178444.43 epoch sec; seqNo:18582 2021-02-13T01:07:24Z- AHRS_M2 msg: 0.59 phi; -0.38 theta; -0.18 psi 2021-02-13T01:07:24Z- handleDepth()-Depth_Keller msg: 1613178444436 epoch sec; seqNo:18731; depth 6.35 2021-02-13T01:07:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178444.45 epoch sec; seqNo:18731; 4060886.32 north; 593711.96 east 2021-02-13T01:07:24Z- AHRS_M2 msg: 1613178444.84 epoch sec; seqNo:18583 2021-02-13T01:07:24Z- AHRS_M2 msg: 0.58 phi; -0.38 theta; -0.20 psi 2021-02-13T01:07:24Z- handleDepth()-Depth_Keller msg: 1613178444840 epoch sec; seqNo:18732; depth 6.48 2021-02-13T01:07:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178444.85 epoch sec; seqNo:18732; 4060886.32 north; 593711.96 east 2021-02-13T01:07:25Z- AHRS_M2 msg: 1613178445.24 epoch sec; seqNo:18584 2021-02-13T01:07:25Z- AHRS_M2 msg: 0.58 phi; -0.37 theta; -0.22 psi 2021-02-13T01:07:25Z- handleDepth()-Depth_Keller msg: 1613178445245 epoch sec; seqNo:18733; depth 6.60 2021-02-13T01:07:25Z- handleDvl() - RDI_Pathfinder msg: 1613178445.26 epoch sec; seqNo:9058 2021-02-13T01:07:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 111.37 , 81.62 , 127.74 2021-02-13T01:07:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.36 , -0.01 2021-02-13T01:07:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178445.27 epoch sec; seqNo:18733; 4060886.70 north; 593711.96 east 2021-02-13T01:07:25Z- AHRS_M2 msg: 1613178445.64 epoch sec; seqNo:18585 2021-02-13T01:07:25Z- AHRS_M2 msg: 0.57 phi; -0.37 theta; -0.24 psi 2021-02-13T01:07:25Z- handleDepth()-Depth_Keller msg: 1613178445659 epoch sec; seqNo:18734; depth 6.73 2021-02-13T01:07:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178445.67 epoch sec; seqNo:18734; 4060886.70 north; 593711.96 east 2021-02-13T01:07:26Z- AHRS_M2 msg: 1613178446.05 epoch sec; seqNo:18586 2021-02-13T01:07:26Z- AHRS_M2 msg: 0.57 phi; -0.36 theta; -0.26 psi 2021-02-13T01:07:26Z- handleDepth()-Depth_Keller msg: 1613178446051 epoch sec; seqNo:18735; depth 6.86 2021-02-13T01:07:26Z- handleDvl() - RDI_Pathfinder msg: 1613178446.06 epoch sec; seqNo:9059 2021-02-13T01:07:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.71 , 117.05 , 82.84 , 131.93 2021-02-13T01:07:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.13 , -0.20 2021-02-13T01:07:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178446.08 epoch sec; seqNo:18735; 4060887.09 north; 593713.18 east 2021-02-13T01:07:26Z- LcmTrn::handleMessages() - handling 45 messages took 0.68 ms and 132 calls... 2021-02-13T01:07:26Z- waiting for 1613178443.81, time is 1613178446.08 2021-02-13T01:07:26Z- LcmTrn::updateTrn() equating pose 1613178446.08 and meas times 1613178446.08 2021-02-13T01:07:26Z- LcmTrn::updateTrn() >>>> pose time 1613178446.08, meas time 1613178446.08 <<<< 2021-02-13T01:07:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178446.08 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:07:26Z- TerrainNav::motionUpdate() - currEstimate = (4060887, 593713.18, 6.86) 2021-02-13T01:07:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:07:26Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:07:26Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178446.08 sec.; No relevant navigation data available 2021-02-13T01:07:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:07:26Z- TNavBF::mmse selected from filter -1 2021-02-13T01:07:26Z- variances([0], [2], and psi_berg) = 675.214484 832.986498 0.000000 2021-02-13T01:07:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:07:26Z- LcmTrn::publishEstimates() - MLE : 4060886.59 593700.68 6.86 2021-02-13T01:07:26Z- LcmTrn::publishEstimates() - MMSE : 4060887.09 593713.18 6.86 2021-02-13T01:07:26Z- LcmTrn::publishEstimates() - COVAR: 675.21 832.99 1.57 0.00 2021-02-13T01:07:26Z- AHRS_M2 msg: 1613178446.45 epoch sec; seqNo:18587 2021-02-13T01:07:26Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.25 psi 2021-02-13T01:07:26Z- handleDepth()-Depth_Keller msg: 1613178446456 epoch sec; seqNo:18736; depth 6.99 2021-02-13T01:07:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178446.47 epoch sec; seqNo:18736; 4060887.47 north; 593713.17 east 2021-02-13T01:07:26Z- AHRS_M2 msg: 1613178446.86 epoch sec; seqNo:18588 2021-02-13T01:07:26Z- AHRS_M2 msg: 0.58 phi; -0.34 theta; -0.23 psi 2021-02-13T01:07:26Z- handleDepth()-Depth_Keller msg: 1613178446860 epoch sec; seqNo:18737; depth 7.13 2021-02-13T01:07:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178446.87 epoch sec; seqNo:18737; 4060887.85 north; 593713.17 east 2021-02-13T01:07:27Z- AHRS_M2 msg: 1613178447.26 epoch sec; seqNo:18589 2021-02-13T01:07:27Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.20 psi 2021-02-13T01:07:27Z- handleDepth()-Depth_Keller msg: 1613178447265 epoch sec; seqNo:18738; depth 7.27 2021-02-13T01:07:27Z- handleDvl() - RDI_Pathfinder msg: 1613178447.28 epoch sec; seqNo:9060 2021-02-13T01:07:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 115.30 , 82.57 , 127.20 2021-02-13T01:07:27Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.09 , -0.10 2021-02-13T01:07:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178447.29 epoch sec; seqNo:18738; 4060888.23 north; 593713.16 east 2021-02-13T01:07:27Z- AHRS_M2 msg: 1613178447.67 epoch sec; seqNo:18590 2021-02-13T01:07:27Z- AHRS_M2 msg: 0.59 phi; -0.34 theta; -0.18 psi 2021-02-13T01:07:27Z- handleDepth()-Depth_Keller msg: 1613178447671 epoch sec; seqNo:18739; depth 7.41 2021-02-13T01:07:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178447.69 epoch sec; seqNo:18739; 4060888.23 north; 593713.16 east 2021-02-13T01:07:28Z- AHRS_M2 msg: 1613178448.07 epoch sec; seqNo:18591 2021-02-13T01:07:28Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.15 psi 2021-02-13T01:07:28Z- handleDepth()-Depth_Keller msg: 1613178448071 epoch sec; seqNo:18740; depth 7.55 2021-02-13T01:07:28Z- handleDvl() - RDI_Pathfinder msg: 1613178448.08 epoch sec; seqNo:9061 2021-02-13T01:07:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.45 , 109.28 , 81.01 , 121.18 2021-02-13T01:07:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.12 , 0.08 2021-02-13T01:07:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178448.09 epoch sec; seqNo:18740; 4060888.61 north; 593713.16 east 2021-02-13T01:07:28Z- AHRS_M2 msg: 1613178448.47 epoch sec; seqNo:18592 2021-02-13T01:07:28Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.13 psi 2021-02-13T01:07:28Z- handleDepth()-Depth_Keller msg: 1613178448478 epoch sec; seqNo:18741; depth 7.69 2021-02-13T01:07:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178448.49 epoch sec; seqNo:18741; 4060889.00 north; 593714.38 east 2021-02-13T01:07:28Z- AHRS_M2 msg: 1613178448.88 epoch sec; seqNo:18593 2021-02-13T01:07:28Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.12 psi 2021-02-13T01:07:28Z- handleDepth()-Depth_Keller msg: 1613178448890 epoch sec; seqNo:18742; depth 7.81 2021-02-13T01:07:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178448.90 epoch sec; seqNo:18742; 4060889.38 north; 593714.37 east 2021-02-13T01:07:29Z- AHRS_M2 msg: 1613178449.28 epoch sec; seqNo:18594 2021-02-13T01:07:29Z- AHRS_M2 msg: 0.57 phi; -0.36 theta; -0.11 psi 2021-02-13T01:07:29Z- handleDepth()-Depth_Keller msg: 1613178449284 epoch sec; seqNo:18743; depth 7.94 2021-02-13T01:07:29Z- handleDvl() - RDI_Pathfinder msg: 1613178449.30 epoch sec; seqNo:9062 2021-02-13T01:07:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.28 , 103.80 , 79.46 , 120.03 2021-02-13T01:07:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.08 , 0.18 2021-02-13T01:07:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178449.32 epoch sec; seqNo:18743; 4060889.38 north; 593714.37 east 2021-02-13T01:07:29Z- AHRS_M2 msg: 1613178449.68 epoch sec; seqNo:18595 2021-02-13T01:07:29Z- AHRS_M2 msg: 0.56 phi; -0.36 theta; -0.12 psi 2021-02-13T01:07:29Z- handleDepth()-Depth_Keller msg: 1613178449690 epoch sec; seqNo:18744; depth 8.06 2021-02-13T01:07:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178449.70 epoch sec; seqNo:18744; 4060889.76 north; 593714.37 east 2021-02-13T01:07:30Z- AHRS_M2 msg: 1613178450.09 epoch sec; seqNo:18596 2021-02-13T01:07:30Z- AHRS_M2 msg: 0.55 phi; -0.35 theta; -0.13 psi 2021-02-13T01:07:30Z- handleDepth()-Depth_Keller msg: 1613178450094 epoch sec; seqNo:18745; depth 8.18 2021-02-13T01:07:30Z- handleDvl() - RDI_Pathfinder msg: 1613178450.11 epoch sec; seqNo:9063 2021-02-13T01:07:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.17 , 103.94 , 79.59 , 124.22 2021-02-13T01:07:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.26 , 0.11 2021-02-13T01:07:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178450.12 epoch sec; seqNo:18745; 4060889.76 north; 593714.37 east 2021-02-13T01:07:30Z- AHRS_M2 msg: 1613178450.49 epoch sec; seqNo:18597 2021-02-13T01:07:30Z- AHRS_M2 msg: 0.55 phi; -0.34 theta; -0.14 psi 2021-02-13T01:07:30Z- handleDepth()-Depth_Keller msg: 1613178450496 epoch sec; seqNo:18746; depth 8.30 2021-02-13T01:07:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178450.51 epoch sec; seqNo:18746; 4060890.14 north; 593714.36 east 2021-02-13T01:07:30Z- AHRS_M2 msg: 1613178450.91 epoch sec; seqNo:18598 2021-02-13T01:07:30Z- AHRS_M2 msg: 0.55 phi; -0.34 theta; -0.15 psi 2021-02-13T01:07:30Z- handleDepth()-Depth_Keller msg: 1613178450923 epoch sec; seqNo:18747; depth 8.44 2021-02-13T01:07:30Z- handleDvl() - RDI_Pathfinder msg: 1613178450.95 epoch sec; seqNo:9064 2021-02-13T01:07:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 102.45 , 0.00 , 120.30 2021-02-13T01:07:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.29 , -0.08 2021-02-13T01:07:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178450.96 epoch sec; seqNo:18747; 4060890.53 north; 593715.58 east 2021-02-13T01:07:31Z- LcmTrn::handleMessages() - handling 41 messages took 0.61 ms and 129 calls... 2021-02-13T01:07:31Z- waiting for 1613178449.08, time is 1613178450.96 2021-02-13T01:07:31Z- LcmTrn::updateTrn() equating pose 1613178450.96 and meas times 1613178450.96 2021-02-13T01:07:31Z- LcmTrn::updateTrn() >>>> pose time 1613178450.96, meas time 1613178450.96 <<<< 2021-02-13T01:07:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178450.96 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:07:31Z- TerrainNav::motionUpdate() - currEstimate = (4060891, 593715.58, 8.44) 2021-02-13T01:07:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:07:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:07:31Z- TNavBF::Projecting Measurements 2021-02-13T01:07:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:07:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:07:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:07:31Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:07:31Z- TNavFilter::3 good beams. 2021-02-13T01:07:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:07:31Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:07:31Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:07:31Z- TNavBF:: beamIndex[2] = 3 2021-02-13T01:07:31Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:07:31Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:07:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:07:31Z- TNavBF::mmse selected from filter -1 2021-02-13T01:07:31Z- TerrainMapOctree: (vehN, vehE ) = (4060890.53, 593715.58). 2021-02-13T01:07:31Z- TerrainMapOctree: Min Distance = 644.07. 2021-02-13T01:07:31Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:07:31Z- There is no specific good beam that all particles have in common. 2021-02-13T01:07:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:07:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.79 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T01:07:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:07:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.62 K: 0.080 ExpectedMeasDiff: nan currMeas.covariance: 16.79 2021-02-13T01:07:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:07:31Z- Old Depth Bias: 0.00 DepthBiasCov: 1.49 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 23.16 2021-02-13T01:07:31Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:07:31Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:07:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:07:31Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:07:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:07:31Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:07:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:07:31Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:07:31Z- Filter 0 No beams used in subcloud update 2021-02-13T01:07:31Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:07:31Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:07:31Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 675.2 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T01:07:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178450.96 sec. 2021-02-13T01:07:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:07:31Z- TNavBF::mmse selected from filter -1 2021-02-13T01:07:31Z- variances([0], [2], and psi_berg) = 675.171702 832.986481 0.000000 2021-02-13T01:07:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:07:31Z- LcmTrn::publishEstimates() - MLE : 4060890.03 593702.08 8.44 2021-02-13T01:07:31Z- LcmTrn::publishEstimates() - MMSE : 4060890.53 593715.58 8.44 2021-02-13T01:07:31Z- LcmTrn::publishEstimates() - COVAR: 675.17 832.99 1.40 0.00 2021-02-13T01:07:31Z- AHRS_M2 msg: 1613178451.30 epoch sec; seqNo:18599 2021-02-13T01:07:31Z- AHRS_M2 msg: 0.55 phi; -0.33 theta; -0.16 psi 2021-02-13T01:07:31Z- handleDepth()-Depth_Keller msg: 1613178451306 epoch sec; seqNo:18748; depth 8.60 2021-02-13T01:07:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178451.32 epoch sec; seqNo:18748; 4060890.53 north; 593715.58 east 2021-02-13T01:07:31Z- AHRS_M2 msg: 1613178451.70 epoch sec; seqNo:18600 2021-02-13T01:07:31Z- AHRS_M2 msg: 0.55 phi; -0.32 theta; -0.17 psi 2021-02-13T01:07:31Z- handleDepth()-Depth_Keller msg: 1613178451708 epoch sec; seqNo:18749; depth 8.70 2021-02-13T01:07:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178451.72 epoch sec; seqNo:18749; 4060890.91 north; 593715.58 east 2021-02-13T01:07:32Z- AHRS_M2 msg: 1613178452.11 epoch sec; seqNo:18601 2021-02-13T01:07:32Z- AHRS_M2 msg: 0.55 phi; -0.32 theta; -0.18 psi 2021-02-13T01:07:32Z- handleDepth()-Depth_Keller msg: 1613178452112 epoch sec; seqNo:18750; depth 8.82 2021-02-13T01:07:32Z- handleDvl() - RDI_Pathfinder msg: 1613178452.13 epoch sec; seqNo:9065 2021-02-13T01:07:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 103.46 , 79.66 , 115.36 2021-02-13T01:07:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.19 , -0.23 2021-02-13T01:07:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178452.20 epoch sec; seqNo:18750; 4060891.29 north; 593715.57 east 2021-02-13T01:07:32Z- AHRS_M2 msg: 1613178452.51 epoch sec; seqNo:18602 2021-02-13T01:07:32Z- AHRS_M2 msg: 0.55 phi; -0.32 theta; -0.19 psi 2021-02-13T01:07:32Z- handleDepth()-Depth_Keller msg: 1613178452518 epoch sec; seqNo:18751; depth 8.97 2021-02-13T01:07:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178452.53 epoch sec; seqNo:18751; 4060891.68 north; 593716.79 east 2021-02-13T01:07:32Z- AHRS_M2 msg: 1613178452.94 epoch sec; seqNo:18603 2021-02-13T01:07:32Z- AHRS_M2 msg: 0.55 phi; -0.31 theta; -0.19 psi 2021-02-13T01:07:32Z- handleDepth()-Depth_Keller msg: 1613178452950 epoch sec; seqNo:18752; depth 9.06 2021-02-13T01:07:32Z- handleDvl() - RDI_Pathfinder msg: 1613178452.96 epoch sec; seqNo:9066 2021-02-13T01:07:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.29 , 106.57 , 0.00 , 114.69 2021-02-13T01:07:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , -0.25 2021-02-13T01:07:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178452.97 epoch sec; seqNo:18752; 4060892.06 north; 593716.78 east 2021-02-13T01:07:33Z- AHRS_M2 msg: 1613178453.32 epoch sec; seqNo:18604 2021-02-13T01:07:33Z- AHRS_M2 msg: 0.55 phi; -0.31 theta; -0.19 psi 2021-02-13T01:07:33Z- handleDepth()-Depth_Keller msg: 1613178453326 epoch sec; seqNo:18753; depth 9.19 2021-02-13T01:07:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178453.34 epoch sec; seqNo:18753; 4060892.06 north; 593716.78 east 2021-02-13T01:07:33Z- AHRS_M2 msg: 1613178453.73 epoch sec; seqNo:18605 2021-02-13T01:07:33Z- AHRS_M2 msg: 0.55 phi; -0.30 theta; -0.19 psi 2021-02-13T01:07:33Z- handleDepth()-Depth_Keller msg: 1613178453730 epoch sec; seqNo:18754; depth 9.30 2021-02-13T01:07:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178453.77 epoch sec; seqNo:18754; 4060892.44 north; 593716.78 east 2021-02-13T01:07:34Z- AHRS_M2 msg: 1613178454.14 epoch sec; seqNo:18606 2021-02-13T01:07:34Z- AHRS_M2 msg: 0.55 phi; -0.30 theta; -0.18 psi 2021-02-13T01:07:34Z- handleDepth()-Depth_Keller msg: 1613178454143 epoch sec; seqNo:18755; depth 9.43 2021-02-13T01:07:34Z- handleDvl() - RDI_Pathfinder msg: 1613178454.16 epoch sec; seqNo:9067 2021-02-13T01:07:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.02 , 105.36 , 0.00 , 114.28 2021-02-13T01:07:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.12 , -0.15 2021-02-13T01:07:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178454.17 epoch sec; seqNo:18755; 4060892.82 north; 593716.78 east 2021-02-13T01:07:34Z- AHRS_M2 msg: 1613178454.53 epoch sec; seqNo:18607 2021-02-13T01:07:34Z- AHRS_M2 msg: 0.55 phi; -0.30 theta; -0.18 psi 2021-02-13T01:07:34Z- handleDepth()-Depth_Keller msg: 1613178454536 epoch sec; seqNo:18756; depth 9.54 2021-02-13T01:07:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178454.55 epoch sec; seqNo:18756; 4060893.20 north; 593716.77 east 2021-02-13T01:07:34Z- AHRS_M2 msg: 1613178454.94 epoch sec; seqNo:18608 2021-02-13T01:07:34Z- AHRS_M2 msg: 0.55 phi; -0.29 theta; -0.17 psi 2021-02-13T01:07:34Z- handleDepth()-Depth_Keller msg: 1613178454942 epoch sec; seqNo:18757; depth 9.66 2021-02-13T01:07:34Z- handleDvl() - RDI_Pathfinder msg: 1613178454.95 epoch sec; seqNo:9068 2021-02-13T01:07:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 103.94 , 80.14 , 112.86 2021-02-13T01:07:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.20 , -0.03 2021-02-13T01:07:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178454.97 epoch sec; seqNo:18757; 4060893.57 north; 593716.77 east 2021-02-13T01:07:35Z- AHRS_M2 msg: 1613178455.34 epoch sec; seqNo:18609 2021-02-13T01:07:35Z- AHRS_M2 msg: 0.55 phi; -0.29 theta; -0.17 psi 2021-02-13T01:07:35Z- handleDepth()-Depth_Keller msg: 1613178455344 epoch sec; seqNo:18758; depth 9.78 2021-02-13T01:07:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178455.36 epoch sec; seqNo:18758; 4060893.95 north; 593716.76 east 2021-02-13T01:07:35Z- AHRS_M2 msg: 1613178455.75 epoch sec; seqNo:18610 2021-02-13T01:07:35Z- AHRS_M2 msg: 0.55 phi; -0.29 theta; -0.17 psi 2021-02-13T01:07:35Z- handleDepth()-Depth_Keller msg: 1613178455754 epoch sec; seqNo:18759; depth 9.89 2021-02-13T01:07:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178455.76 epoch sec; seqNo:18759; 4060894.35 north; 593717.98 east 2021-02-13T01:07:36Z- AHRS_M2 msg: 1613178456.15 epoch sec; seqNo:18611 2021-02-13T01:07:36Z- AHRS_M2 msg: 0.56 phi; -0.28 theta; -0.16 psi 2021-02-13T01:07:36Z- handleDepth()-Depth_Keller msg: 1613178456156 epoch sec; seqNo:18760; depth 10.02 2021-02-13T01:07:36Z- handleDvl() - RDI_Pathfinder msg: 1613178456.17 epoch sec; seqNo:9069 2021-02-13T01:07:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.94 , 103.87 , 79.53 , 111.98 2021-02-13T01:07:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.19 , 0.12 2021-02-13T01:07:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178456.18 epoch sec; seqNo:18760; 4060894.72 north; 593717.98 east 2021-02-13T01:07:36Z- LcmTrn::handleMessages() - handling 44 messages took 0.65 ms and 131 calls... 2021-02-13T01:07:36Z- waiting for 1613178453.96, time is 1613178456.18 2021-02-13T01:07:36Z- LcmTrn::updateTrn() equating pose 1613178456.18 and meas times 1613178456.18 2021-02-13T01:07:36Z- LcmTrn::updateTrn() >>>> pose time 1613178456.18, meas time 1613178456.18 <<<< 2021-02-13T01:07:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178456.18 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:07:36Z- TerrainNav::motionUpdate() - currEstimate = (4060895, 593717.98, 10.02) 2021-02-13T01:07:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:07:36Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:07:36Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178456.18 sec.; No relevant navigation data available 2021-02-13T01:07:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:07:36Z- TNavBF::mmse selected from filter -1 2021-02-13T01:07:36Z- variances([0], [2], and psi_berg) = 675.125810 832.986463 0.000000 2021-02-13T01:07:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:07:36Z- LcmTrn::publishEstimates() - MLE : 4060894.22 593704.48 10.02 2021-02-13T01:07:36Z- LcmTrn::publishEstimates() - MMSE : 4060894.72 593717.98 10.02 2021-02-13T01:07:36Z- LcmTrn::publishEstimates() - COVAR: 675.13 832.99 1.65 0.00 2021-02-13T01:07:36Z- AHRS_M2 msg: 1613178456.55 epoch sec; seqNo:18612 2021-02-13T01:07:36Z- AHRS_M2 msg: 0.56 phi; -0.28 theta; -0.16 psi 2021-02-13T01:07:36Z- handleDepth()-Depth_Keller msg: 1613178456561 epoch sec; seqNo:18761; depth 10.13 2021-02-13T01:07:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178456.57 epoch sec; seqNo:18761; 4060895.10 north; 593717.97 east 2021-02-13T01:07:36Z- AHRS_M2 msg: 1613178456.96 epoch sec; seqNo:18613 2021-02-13T01:07:36Z- AHRS_M2 msg: 0.56 phi; -0.28 theta; -0.15 psi 2021-02-13T01:07:36Z- handleDepth()-Depth_Keller msg: 1613178456963 epoch sec; seqNo:18762; depth 10.23 2021-02-13T01:07:36Z- handleDvl() - RDI_Pathfinder msg: 1613178456.97 epoch sec; seqNo:9070 2021-02-13T01:07:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 100.15 , 79.86 , 109.62 2021-02-13T01:07:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.09 , 0.22 2021-02-13T01:07:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178456.98 epoch sec; seqNo:18762; 4060895.10 north; 593717.97 east 2021-02-13T01:07:37Z- AHRS_M2 msg: 1613178457.36 epoch sec; seqNo:18614 2021-02-13T01:07:37Z- AHRS_M2 msg: 0.56 phi; -0.28 theta; -0.14 psi 2021-02-13T01:07:37Z- handleDepth()-Depth_Keller msg: 1613178457364 epoch sec; seqNo:18763; depth 10.33 2021-02-13T01:07:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178457.38 epoch sec; seqNo:18763; 4060895.48 north; 593717.97 east 2021-02-13T01:07:37Z- AHRS_M2 msg: 1613178457.76 epoch sec; seqNo:18615 2021-02-13T01:07:37Z- AHRS_M2 msg: 0.55 phi; -0.27 theta; -0.13 psi 2021-02-13T01:07:37Z- handleDepth()-Depth_Keller msg: 1613178457768 epoch sec; seqNo:18764; depth 10.43 2021-02-13T01:07:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178457.78 epoch sec; seqNo:18764; 4060895.86 north; 593717.96 east 2021-02-13T01:07:38Z- AHRS_M2 msg: 1613178458.17 epoch sec; seqNo:18616 2021-02-13T01:07:38Z- AHRS_M2 msg: 0.55 phi; -0.27 theta; -0.14 psi 2021-02-13T01:07:38Z- handleDepth()-Depth_Keller msg: 1613178458174 epoch sec; seqNo:18765; depth 10.52 2021-02-13T01:07:38Z- handleDvl() - RDI_Pathfinder msg: 1613178458.19 epoch sec; seqNo:9071 2021-02-13T01:07:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.14 , 97.31 , 78.38 , 109.48 2021-02-13T01:07:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.09 , 0.18 2021-02-13T01:07:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178458.19 epoch sec; seqNo:18765; 4060895.86 north; 593717.96 east 2021-02-13T01:07:38Z- AHRS_M2 msg: 1613178458.57 epoch sec; seqNo:18617 2021-02-13T01:07:38Z- AHRS_M2 msg: 0.55 phi; -0.27 theta; -0.14 psi 2021-02-13T01:07:38Z- handleDepth()-Depth_Keller msg: 1613178458576 epoch sec; seqNo:18766; depth 10.63 2021-02-13T01:07:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178458.60 epoch sec; seqNo:18766; 4060896.24 north; 593717.96 east 2021-02-13T01:07:38Z- AHRS_M2 msg: 1613178458.98 epoch sec; seqNo:18618 2021-02-13T01:07:38Z- AHRS_M2 msg: 0.56 phi; -0.27 theta; -0.15 psi 2021-02-13T01:07:38Z- handleDepth()-Depth_Keller msg: 1613178458982 epoch sec; seqNo:18767; depth 10.73 2021-02-13T01:07:38Z- handleDvl() - RDI_Pathfinder msg: 1613178458.99 epoch sec; seqNo:9072 2021-02-13T01:07:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.48 , 97.65 , 78.71 , 105.76 2021-02-13T01:07:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.14 , 0.05 2021-02-13T01:07:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178459.02 epoch sec; seqNo:18767; 4060896.63 north; 593719.18 east 2021-02-13T01:07:39Z- AHRS_M2 msg: 1613178459.38 epoch sec; seqNo:18619 2021-02-13T01:07:39Z- AHRS_M2 msg: 0.56 phi; -0.27 theta; -0.15 psi 2021-02-13T01:07:39Z- handleDepth()-Depth_Keller msg: 1613178459384 epoch sec; seqNo:18768; depth 10.82 2021-02-13T01:07:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178459.39 epoch sec; seqNo:18768; 4060896.63 north; 593719.18 east 2021-02-13T01:07:39Z- AHRS_M2 msg: 1613178459.79 epoch sec; seqNo:18620 2021-02-13T01:07:39Z- AHRS_M2 msg: 0.56 phi; -0.27 theta; -0.16 psi 2021-02-13T01:07:39Z- handleDepth()-Depth_Keller msg: 1613178459794 epoch sec; seqNo:18769; depth 10.91 2021-02-13T01:07:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178459.80 epoch sec; seqNo:18769; 4060897.01 north; 593719.17 east 2021-02-13T01:07:40Z- AHRS_M2 msg: 1613178460.19 epoch sec; seqNo:18621 2021-02-13T01:07:40Z- AHRS_M2 msg: 0.57 phi; -0.27 theta; -0.16 psi 2021-02-13T01:07:40Z- handleDepth()-Depth_Keller msg: 1613178460192 epoch sec; seqNo:18770; depth 11.01 2021-02-13T01:07:40Z- handleDvl() - RDI_Pathfinder msg: 1613178460.20 epoch sec; seqNo:9073 2021-02-13T01:07:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 99.07 , 78.78 , 107.18 2021-02-13T01:07:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.14 , -0.03 2021-02-13T01:07:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178460.22 epoch sec; seqNo:18770; 4060897.39 north; 593719.17 east 2021-02-13T01:07:40Z- AHRS_M2 msg: 1613178460.59 epoch sec; seqNo:18622 2021-02-13T01:07:40Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.16 psi 2021-02-13T01:07:40Z- handleDepth()-Depth_Keller msg: 1613178460596 epoch sec; seqNo:18771; depth 11.09 2021-02-13T01:07:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178460.61 epoch sec; seqNo:18771; 4060897.77 north; 593719.16 east 2021-02-13T01:07:40Z- AHRS_M2 msg: 1613178461.00 epoch sec; seqNo:18623 2021-02-13T01:07:40Z- AHRS_M2 msg: 0.58 phi; -0.28 theta; -0.16 psi 2021-02-13T01:07:41Z- handleDepth()-Depth_Keller msg: 1613178461001 epoch sec; seqNo:18772; depth 11.19 2021-02-13T01:07:41Z- handleDvl() - RDI_Pathfinder msg: 1613178461.02 epoch sec; seqNo:9074 2021-02-13T01:07:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 99.07 , 76.84 , 108.53 2021-02-13T01:07:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.12 , -0.05 2021-02-13T01:07:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178461.03 epoch sec; seqNo:18772; 4060897.78 north; 593720.38 east 2021-02-13T01:07:41Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-13T01:07:41Z- waiting for 1613178459.18, time is 1613178461.03 2021-02-13T01:07:41Z- LcmTrn::updateTrn() equating pose 1613178461.03 and meas times 1613178461.03 2021-02-13T01:07:41Z- LcmTrn::updateTrn() >>>> pose time 1613178461.03, meas time 1613178461.03 <<<< 2021-02-13T01:07:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178461.03 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:07:41Z- TerrainNav::motionUpdate() - currEstimate = (4060898, 593720.38, 11.19) 2021-02-13T01:07:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:07:41Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:07:41Z- TNavBF::Projecting Measurements 2021-02-13T01:07:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:07:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:07:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:07:41Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:07:41Z- TNavFilter::3 good beams. 2021-02-13T01:07:41Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:07:41Z- TNavBF:: beamIndex[0] = 1 2021-02-13T01:07:41Z- TNavBF:: beamIndex[1] = 2 2021-02-13T01:07:41Z- TNavBF:: beamIndex[2] = 3 2021-02-13T01:07:41Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:07:41Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:07:41Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:07:41Z- TNavBF::mmse selected from filter -1 2021-02-13T01:07:41Z- TerrainMapOctree: (vehN, vehE ) = (4060897.78, 593720.38). 2021-02-13T01:07:41Z- TerrainMapOctree: Min Distance = 636.04. 2021-02-13T01:07:41Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:07:41Z- There is no specific good beam that all particles have in common. 2021-02-13T01:07:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:07:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.85 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 15.70 2021-02-13T01:07:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:07:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.67 K: 0.130 ExpectedMeasDiff: nan currMeas.covariance: 9.45 2021-02-13T01:07:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:07:41Z- Old Depth Bias: 0.00 DepthBiasCov: 1.45 K: 0.066 ExpectedMeasDiff: nan currMeas.covariance: 18.85 2021-02-13T01:07:41Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:07:41Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:07:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:07:41Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:07:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:07:41Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:07:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:07:41Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:07:41Z- Filter 0 No beams used in subcloud update 2021-02-13T01:07:41Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:07:41Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:07:41Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 675.1 EastVariance: 833.0 DepthVariance: 1.4 this->DepthBiasCov: 1.4 2021-02-13T01:07:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178461.03 sec. 2021-02-13T01:07:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:07:41Z- TNavBF::mmse selected from filter -1 2021-02-13T01:07:41Z- variances([0], [2], and psi_berg) = 675.084635 832.986446 0.000000 2021-02-13T01:07:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:07:41Z- LcmTrn::publishEstimates() - MLE : 4060897.28 593734.88 11.19 2021-02-13T01:07:41Z- LcmTrn::publishEstimates() - MMSE : 4060897.78 593720.38 11.19 2021-02-13T01:07:41Z- LcmTrn::publishEstimates() - COVAR: 675.08 832.99 1.36 0.00 2021-02-13T01:07:41Z- AHRS_M2 msg: 1613178461.40 epoch sec; seqNo:18624 2021-02-13T01:07:41Z- AHRS_M2 msg: 0.59 phi; -0.29 theta; -0.16 psi 2021-02-13T01:07:41Z- handleDepth()-Depth_Keller msg: 1613178461409 epoch sec; seqNo:18773; depth 11.30 2021-02-13T01:07:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178461.42 epoch sec; seqNo:18773; 4060898.16 north; 593720.38 east 2021-02-13T01:07:41Z- AHRS_M2 msg: 1613178461.80 epoch sec; seqNo:18625 2021-02-13T01:07:41Z- AHRS_M2 msg: 0.60 phi; -0.29 theta; -0.15 psi 2021-02-13T01:07:41Z- handleDepth()-Depth_Keller msg: 1613178461808 epoch sec; seqNo:18774; depth 11.41 2021-02-13T01:07:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178461.82 epoch sec; seqNo:18774; 4060898.54 north; 593720.38 east 2021-02-13T01:07:42Z- AHRS_M2 msg: 1613178462.21 epoch sec; seqNo:18626 2021-02-13T01:07:42Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.15 psi 2021-02-13T01:07:42Z- handleDepth()-Depth_Keller msg: 1613178462214 epoch sec; seqNo:18775; depth 11.49 2021-02-13T01:07:42Z- handleDvl() - RDI_Pathfinder msg: 1613178462.23 epoch sec; seqNo:9075 2021-02-13T01:07:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 98.86 , 0.00 , 108.33 2021-02-13T01:07:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.08 , -0.09 2021-02-13T01:07:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178462.24 epoch sec; seqNo:18775; 4060898.92 north; 593720.37 east 2021-02-13T01:07:42Z- AHRS_M2 msg: 1613178462.61 epoch sec; seqNo:18627 2021-02-13T01:07:42Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.13 psi 2021-02-13T01:07:42Z- handleDepth()-Depth_Keller msg: 1613178462616 epoch sec; seqNo:18776; depth 11.59 2021-02-13T01:07:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178462.64 epoch sec; seqNo:18776; 4060898.92 north; 593720.37 east 2021-02-13T01:07:43Z- AHRS_M2 msg: 1613178463.02 epoch sec; seqNo:18628 2021-02-13T01:07:43Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.12 psi 2021-02-13T01:07:43Z- handleDepth()-Depth_Keller msg: 1613178463025 epoch sec; seqNo:18777; depth 11.70 2021-02-13T01:07:43Z- handleDvl() - RDI_Pathfinder msg: 1613178463.04 epoch sec; seqNo:9076 2021-02-13T01:07:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.31 , 78.38 , 108.13 2021-02-13T01:07:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , -0.09 2021-02-13T01:07:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178463.06 epoch sec; seqNo:18777; 4060899.31 north; 593721.59 east 2021-02-13T01:07:43Z- AHRS_M2 msg: 1613178463.42 epoch sec; seqNo:18629 2021-02-13T01:07:43Z- AHRS_M2 msg: 0.62 phi; -0.30 theta; -0.12 psi 2021-02-13T01:07:43Z- handleDepth()-Depth_Keller msg: 1613178463424 epoch sec; seqNo:18778; depth 11.81 2021-02-13T01:07:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178463.43 epoch sec; seqNo:18778; 4060899.69 north; 593721.58 east 2021-02-13T01:07:43Z- AHRS_M2 msg: 1613178463.82 epoch sec; seqNo:18630 2021-02-13T01:07:43Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.13 psi 2021-02-13T01:07:43Z- handleDepth()-Depth_Keller msg: 1613178463833 epoch sec; seqNo:18779; depth 11.89 2021-02-13T01:07:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178463.85 epoch sec; seqNo:18779; 4060900.07 north; 593721.58 east 2021-02-13T01:07:44Z- AHRS_M2 msg: 1613178464.23 epoch sec; seqNo:18631 2021-02-13T01:07:44Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.14 psi 2021-02-13T01:07:44Z- handleDepth()-Depth_Keller msg: 1613178464237 epoch sec; seqNo:18780; depth 11.99 2021-02-13T01:07:44Z- handleDvl() - RDI_Pathfinder msg: 1613178464.26 epoch sec; seqNo:9077 2021-02-13T01:07:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 97.51 , 76.02 , 108.33 2021-02-13T01:07:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , -0.02 2021-02-13T01:07:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178464.27 epoch sec; seqNo:18780; 4060900.07 north; 593721.58 east 2021-02-13T01:07:44Z- AHRS_M2 msg: 1613178464.63 epoch sec; seqNo:18632 2021-02-13T01:07:44Z- AHRS_M2 msg: 0.61 phi; -0.29 theta; -0.15 psi 2021-02-13T01:07:44Z- handleDepth()-Depth_Keller msg: 1613178464636 epoch sec; seqNo:18781; depth 12.09 2021-02-13T01:07:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178464.65 epoch sec; seqNo:18781; 4060900.45 north; 593721.57 east 2021-02-13T01:07:45Z- AHRS_M2 msg: 1613178465.04 epoch sec; seqNo:18633 2021-02-13T01:07:45Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.16 psi 2021-02-13T01:07:45Z- handleDepth()-Depth_Keller msg: 1613178465042 epoch sec; seqNo:18782; depth 12.17 2021-02-13T01:07:45Z- handleDvl() - RDI_Pathfinder msg: 1613178465.05 epoch sec; seqNo:9078 2021-02-13T01:07:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.65 , 78.17 , 108.60 2021-02-13T01:07:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.04 , 0.09 2021-02-13T01:07:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178465.06 epoch sec; seqNo:18782; 4060900.83 north; 593721.57 east 2021-02-13T01:07:45Z- AHRS_M2 msg: 1613178465.44 epoch sec; seqNo:18634 2021-02-13T01:07:45Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.15 psi 2021-02-13T01:07:45Z- handleDepth()-Depth_Keller msg: 1613178465445 epoch sec; seqNo:18783; depth 12.29 2021-02-13T01:07:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178465.46 epoch sec; seqNo:18783; 4060901.22 north; 593722.79 east 2021-02-13T01:07:45Z- AHRS_M2 msg: 1613178465.85 epoch sec; seqNo:18635 2021-02-13T01:07:45Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.15 psi 2021-02-13T01:07:45Z- handleDepth()-Depth_Keller msg: 1613178465852 epoch sec; seqNo:18784; depth 12.37 2021-02-13T01:07:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178465.87 epoch sec; seqNo:18784; 4060901.60 north; 593722.78 east 2021-02-13T01:07:46Z- AHRS_M2 msg: 1613178466.25 epoch sec; seqNo:18636 2021-02-13T01:07:46Z- AHRS_M2 msg: 0.61 phi; -0.30 theta; -0.15 psi 2021-02-13T01:07:46Z- handleDepth()-Depth_Keller msg: 1613178466261 epoch sec; seqNo:18785; depth 12.50 2021-02-13T01:07:46Z- handleDvl() - RDI_Pathfinder msg: 1613178466.27 epoch sec; seqNo:9079 2021-02-13T01:07:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.51 , 78.58 , 109.68 2021-02-13T01:07:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , 0.14 2021-02-13T01:07:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178466.28 epoch sec; seqNo:18785; 4060901.60 north; 593722.78 east 2021-02-13T01:07:46Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-13T01:07:46Z- waiting for 1613178464.03, time is 1613178466.28 2021-02-13T01:07:46Z- LcmTrn::updateTrn() equating pose 1613178466.28 and meas times 1613178466.28 2021-02-13T01:07:46Z- LcmTrn::updateTrn() >>>> pose time 1613178466.28, meas time 1613178466.28 <<<< 2021-02-13T01:07:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178466.28 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:07:46Z- TerrainNav::motionUpdate() - currEstimate = (4060902, 593722.78, 12.50) 2021-02-13T01:07:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:07:46Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:07:46Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178466.28 sec.; No relevant navigation data available 2021-02-13T01:07:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:07:46Z- TNavBF::mmse selected from filter -1 2021-02-13T01:07:46Z- variances([0], [2], and psi_berg) = 675.040343 832.986428 0.000000 2021-02-13T01:07:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:07:46Z- LcmTrn::publishEstimates() - MLE : 4060901.10 593710.28 12.50 2021-02-13T01:07:46Z- LcmTrn::publishEstimates() - MMSE : 4060901.60 593722.78 12.50 2021-02-13T01:07:46Z- LcmTrn::publishEstimates() - COVAR: 675.04 832.99 1.59 0.00 2021-02-13T01:07:46Z- AHRS_M2 msg: 1613178466.65 epoch sec; seqNo:18637 2021-02-13T01:07:46Z- AHRS_M2 msg: 0.61 phi; -0.31 theta; -0.14 psi 2021-02-13T01:07:46Z- handleDepth()-Depth_Keller msg: 1613178466656 epoch sec; seqNo:18786; depth 12.60 2021-02-13T01:07:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178466.67 epoch sec; seqNo:18786; 4060901.98 north; 593722.78 east 2021-02-13T01:07:47Z- AHRS_M2 msg: 1613178467.06 epoch sec; seqNo:18638 2021-02-13T01:07:47Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.13 psi 2021-02-13T01:07:47Z- handleDepth()-Depth_Keller msg: 1613178467060 epoch sec; seqNo:18787; depth 12.70 2021-02-13T01:07:47Z- handleDvl() - RDI_Pathfinder msg: 1613178467.07 epoch sec; seqNo:9080 2021-02-13T01:07:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.92 , 97.44 , 76.02 , 109.62 2021-02-13T01:07:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.13 , 0.07 2021-02-13T01:07:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178467.08 epoch sec; seqNo:18787; 4060902.35 north; 593722.77 east 2021-02-13T01:07:47Z- AHRS_M2 msg: 1613178467.46 epoch sec; seqNo:18639 2021-02-13T01:07:47Z- AHRS_M2 msg: 0.62 phi; -0.31 theta; -0.13 psi 2021-02-13T01:07:47Z- handleDepth()-Depth_Keller msg: 1613178467465 epoch sec; seqNo:18788; depth 12.80 2021-02-13T01:07:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178467.48 epoch sec; seqNo:18788; 4060902.35 north; 593722.77 east 2021-02-13T01:07:47Z- AHRS_M2 msg: 1613178467.86 epoch sec; seqNo:18640 2021-02-13T01:07:47Z- AHRS_M2 msg: 0.63 phi; -0.31 theta; -0.13 psi 2021-02-13T01:07:47Z- handleDepth()-Depth_Keller msg: 1613178467869 epoch sec; seqNo:18789; depth 12.91 2021-02-13T01:07:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178467.89 epoch sec; seqNo:18789; 4060902.75 north; 593723.99 east 2021-02-13T01:07:48Z- AHRS_M2 msg: 1613178468.27 epoch sec; seqNo:18641 2021-02-13T01:07:48Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.14 psi 2021-02-13T01:07:48Z- handleDepth()-Depth_Keller msg: 1613178468273 epoch sec; seqNo:18790; depth 13.02 2021-02-13T01:07:48Z- handleDvl() - RDI_Pathfinder msg: 1613178468.28 epoch sec; seqNo:9081 2021-02-13T01:07:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 97.71 , 78.78 , 109.89 2021-02-13T01:07:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.14 , -0.07 2021-02-13T01:07:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178468.29 epoch sec; seqNo:18790; 4060903.13 north; 593723.99 east 2021-02-13T01:07:48Z- AHRS_M2 msg: 1613178468.67 epoch sec; seqNo:18642 2021-02-13T01:07:48Z- AHRS_M2 msg: 0.64 phi; -0.31 theta; -0.14 psi 2021-02-13T01:07:48Z- handleDepth()-Depth_Keller msg: 1613178468677 epoch sec; seqNo:18791; depth 13.12 2021-02-13T01:07:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178468.69 epoch sec; seqNo:18791; 4060903.50 north; 593723.98 east 2021-02-13T01:07:49Z- AHRS_M2 msg: 1613178469.08 epoch sec; seqNo:18643 2021-02-13T01:07:49Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.16 psi 2021-02-13T01:07:49Z- handleDepth()-Depth_Keller msg: 1613178469080 epoch sec; seqNo:18792; depth 13.23 2021-02-13T01:07:49Z- handleDvl() - RDI_Pathfinder msg: 1613178469.10 epoch sec; seqNo:9082 2021-02-13T01:07:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.77 , 97.65 , 73.57 , 113.88 2021-02-13T01:07:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , -0.17 2021-02-13T01:07:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178469.12 epoch sec; seqNo:18792; 4060903.50 north; 593723.98 east 2021-02-13T01:07:49Z- AHRS_M2 msg: 1613178469.48 epoch sec; seqNo:18644 2021-02-13T01:07:49Z- AHRS_M2 msg: 0.65 phi; -0.32 theta; -0.17 psi 2021-02-13T01:07:49Z- handleDepth()-Depth_Keller msg: 1613178469484 epoch sec; seqNo:18793; depth 13.35 2021-02-13T01:07:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178469.50 epoch sec; seqNo:18793; 4060903.90 north; 593725.20 east 2021-02-13T01:07:49Z- AHRS_M2 msg: 1613178469.90 epoch sec; seqNo:18645 2021-02-13T01:07:49Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.19 psi 2021-02-13T01:07:49Z- handleDepth()-Depth_Keller msg: 1613178469903 epoch sec; seqNo:18794; depth 13.44 2021-02-13T01:07:49Z- handleDvl() - RDI_Pathfinder msg: 1613178469.92 epoch sec; seqNo:9083 2021-02-13T01:07:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.70 , 99.07 , 78.78 , 107.18 2021-02-13T01:07:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.14 , -0.10 2021-02-13T01:07:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178469.95 epoch sec; seqNo:18794; 4060904.28 north; 593725.19 east 2021-02-13T01:07:50Z- AHRS_M2 msg: 1613178470.29 epoch sec; seqNo:18646 2021-02-13T01:07:50Z- AHRS_M2 msg: 0.65 phi; -0.33 theta; -0.19 psi 2021-02-13T01:07:50Z- handleDepth()-Depth_Keller msg: 1613178470305 epoch sec; seqNo:18795; depth 13.57 2021-02-13T01:07:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178470.32 epoch sec; seqNo:18795; 4060904.65 north; 593725.19 east 2021-02-13T01:07:50Z- AHRS_M2 msg: 1613178470.69 epoch sec; seqNo:18647 2021-02-13T01:07:50Z- AHRS_M2 msg: 0.65 phi; -0.34 theta; -0.20 psi 2021-02-13T01:07:50Z- handleDepth()-Depth_Keller msg: 1613178470696 epoch sec; seqNo:18796; depth 13.67 2021-02-13T01:07:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178470.70 epoch sec; seqNo:18796; 4060905.03 north; 593725.19 east 2021-02-13T01:07:51Z- AHRS_M2 msg: 1613178471.10 epoch sec; seqNo:18648 2021-02-13T01:07:51Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.20 psi 2021-02-13T01:07:51Z- handleDepth()-Depth_Keller msg: 1613178471100 epoch sec; seqNo:18797; depth 13.78 2021-02-13T01:07:51Z- handleDvl() - RDI_Pathfinder msg: 1613178471.11 epoch sec; seqNo:9084 2021-02-13T01:07:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.29 , 100.08 , 78.17 , 112.25 2021-02-13T01:07:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.12 , 0.08 2021-02-13T01:07:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178471.12 epoch sec; seqNo:18797; 4060905.03 north; 593725.19 east 2021-02-13T01:07:51Z- LcmTrn::handleMessages() - handling 41 messages took 0.69 ms and 127 calls... 2021-02-13T01:07:51Z- waiting for 1613178469.28, time is 1613178471.12 2021-02-13T01:07:51Z- LcmTrn::updateTrn() equating pose 1613178471.12 and meas times 1613178471.12 2021-02-13T01:07:51Z- LcmTrn::updateTrn() >>>> pose time 1613178471.12, meas time 1613178471.12 <<<< 2021-02-13T01:07:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178471.12 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:07:51Z- TerrainNav::motionUpdate() - currEstimate = (4060905, 593725.19, 13.78) 2021-02-13T01:07:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:07:51Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:07:51Z- TNavBF::Projecting Measurements 2021-02-13T01:07:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:07:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:07:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:07:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:07:51Z- TNavFilter::4 good beams. 2021-02-13T01:07:51Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:07:51Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:07:51Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:07:51Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:07:51Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:07:51Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:07:51Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:07:51Z- TNavBF::mmse selected from filter -1 2021-02-13T01:07:51Z- TerrainMapOctree: (vehN, vehE ) = (4060905.03, 593725.19). 2021-02-13T01:07:51Z- TerrainMapOctree: Min Distance = 628.04. 2021-02-13T01:07:51Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:07:51Z- There is no specific good beam that all particles have in common. 2021-02-13T01:07:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:07:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.80 K: 0.133 ExpectedMeasDiff: nan currMeas.covariance: 10.06 2021-02-13T01:07:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:07:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.56 K: 0.081 ExpectedMeasDiff: nan currMeas.covariance: 16.03 2021-02-13T01:07:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:07:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.44 K: 0.111 ExpectedMeasDiff: nan currMeas.covariance: 9.78 2021-02-13T01:07:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:07:51Z- Old Depth Bias: 0.00 DepthBiasCov: 1.28 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 20.16 2021-02-13T01:07:51Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:07:51Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:07:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:07:51Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:07:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:07:51Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:07:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:07:51Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:07:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:07:51Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:07:51Z- Filter 0 No beams used in subcloud update 2021-02-13T01:07:51Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:07:51Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:07:51Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 675.0 EastVariance: 833.0 DepthVariance: 1.2 this->DepthBiasCov: 1.2 2021-02-13T01:07:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178471.12 sec. 2021-02-13T01:07:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:07:51Z- TNavBF::mmse selected from filter -1 2021-02-13T01:07:51Z- variances([0], [2], and psi_berg) = 674.997638 832.986411 0.000000 2021-02-13T01:07:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:07:51Z- LcmTrn::publishEstimates() - MLE : 4060904.53 593711.69 13.78 2021-02-13T01:07:51Z- LcmTrn::publishEstimates() - MMSE : 4060905.03 593725.19 13.78 2021-02-13T01:07:51Z- LcmTrn::publishEstimates() - COVAR: 675.00 832.99 1.21 0.00 2021-02-13T01:07:51Z- AHRS_M2 msg: 1613178471.50 epoch sec; seqNo:18649 2021-02-13T01:07:51Z- AHRS_M2 msg: 0.64 phi; -0.35 theta; -0.20 psi 2021-02-13T01:07:51Z- handleDepth()-Depth_Keller msg: 1613178471505 epoch sec; seqNo:18798; depth 13.89 2021-02-13T01:07:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178471.52 epoch sec; seqNo:18798; 4060905.41 north; 593725.18 east 2021-02-13T01:07:51Z- AHRS_M2 msg: 1613178471.90 epoch sec; seqNo:18650 2021-02-13T01:07:51Z- AHRS_M2 msg: 0.63 phi; -0.35 theta; -0.20 psi 2021-02-13T01:07:51Z- handleDepth()-Depth_Keller msg: 1613178471912 epoch sec; seqNo:18799; depth 14.00 2021-02-13T01:07:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178471.94 epoch sec; seqNo:18799; 4060905.79 north; 593725.18 east 2021-02-13T01:07:52Z- AHRS_M2 msg: 1613178472.31 epoch sec; seqNo:18651 2021-02-13T01:07:52Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.19 psi 2021-02-13T01:07:52Z- handleDepth()-Depth_Keller msg: 1613178472317 epoch sec; seqNo:18800; depth 14.11 2021-02-13T01:07:52Z- handleDvl() - RDI_Pathfinder msg: 1613178472.34 epoch sec; seqNo:9085 2021-02-13T01:07:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.49 , 100.42 , 78.78 , 113.94 2021-02-13T01:07:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.02 , 0.21 2021-02-13T01:07:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178472.35 epoch sec; seqNo:18800; 4060905.79 north; 593725.18 east 2021-02-13T01:07:52Z- AHRS_M2 msg: 1613178472.71 epoch sec; seqNo:18652 2021-02-13T01:07:52Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-13T01:07:52Z- handleDepth()-Depth_Keller msg: 1613178472716 epoch sec; seqNo:18801; depth 14.24 2021-02-13T01:07:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178472.73 epoch sec; seqNo:18801; 4060906.18 north; 593726.39 east 2021-02-13T01:07:53Z- AHRS_M2 msg: 1613178473.12 epoch sec; seqNo:18653 2021-02-13T01:07:53Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-13T01:07:53Z- handleDepth()-Depth_Keller msg: 1613178473123 epoch sec; seqNo:18802; depth 14.34 2021-02-13T01:07:53Z- handleDvl() - RDI_Pathfinder msg: 1613178473.14 epoch sec; seqNo:9086 2021-02-13T01:07:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.70 , 100.29 , 71.53 , 113.81 2021-02-13T01:07:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.15 , 0.17 2021-02-13T01:07:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178473.15 epoch sec; seqNo:18802; 4060906.18 north; 593726.39 east 2021-02-13T01:07:53Z- AHRS_M2 msg: 1613178473.52 epoch sec; seqNo:18654 2021-02-13T01:07:53Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-13T01:07:53Z- handleDepth()-Depth_Keller msg: 1613178473524 epoch sec; seqNo:18803; depth 14.46 2021-02-13T01:07:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178473.54 epoch sec; seqNo:18803; 4060906.56 north; 593726.39 east 2021-02-13T01:07:53Z- AHRS_M2 msg: 1613178473.93 epoch sec; seqNo:18655 2021-02-13T01:07:53Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-13T01:07:53Z- handleDepth()-Depth_Keller msg: 1613178473939 epoch sec; seqNo:18804; depth 14.59 2021-02-13T01:07:53Z- handleDvl() - RDI_Pathfinder msg: 1613178473.97 epoch sec; seqNo:9087 2021-02-13T01:07:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 99.07 , 78.78 , 112.59 2021-02-13T01:07:53Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.18 , 0.05 2021-02-13T01:07:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178473.98 epoch sec; seqNo:18804; 4060906.94 north; 593726.39 east 2021-02-13T01:07:54Z- AHRS_M2 msg: 1613178474.33 epoch sec; seqNo:18656 2021-02-13T01:07:54Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-13T01:07:54Z- handleDepth()-Depth_Keller msg: 1613178474332 epoch sec; seqNo:18805; depth 14.73 2021-02-13T01:07:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178474.34 epoch sec; seqNo:18805; 4060906.94 north; 593726.39 east 2021-02-13T01:07:54Z- AHRS_M2 msg: 1613178474.73 epoch sec; seqNo:18657 2021-02-13T01:07:54Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.18 psi 2021-02-13T01:07:54Z- handleDepth()-Depth_Keller msg: 1613178474737 epoch sec; seqNo:18806; depth 14.83 2021-02-13T01:07:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178474.75 epoch sec; seqNo:18806; 4060907.33 north; 593727.60 east 2021-02-13T01:07:55Z- AHRS_M2 msg: 1613178475.14 epoch sec; seqNo:18658 2021-02-13T01:07:55Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.19 psi 2021-02-13T01:07:55Z- handleDepth()-Depth_Keller msg: 1613178475143 epoch sec; seqNo:18807; depth 14.96 2021-02-13T01:07:55Z- handleDvl() - RDI_Pathfinder msg: 1613178475.17 epoch sec; seqNo:9088 2021-02-13T01:07:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.35 , 98.93 , 78.65 , 115.16 2021-02-13T01:07:55Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.10 , -0.06 2021-02-13T01:07:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178475.18 epoch sec; seqNo:18807; 4060907.71 north; 593727.60 east 2021-02-13T01:07:55Z- AHRS_M2 msg: 1613178475.54 epoch sec; seqNo:18659 2021-02-13T01:07:55Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T01:07:55Z- handleDepth()-Depth_Keller msg: 1613178475544 epoch sec; seqNo:18808; depth 15.09 2021-02-13T01:07:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178475.56 epoch sec; seqNo:18808; 4060908.09 north; 593727.59 east 2021-02-13T01:07:55Z- AHRS_M2 msg: 1613178475.96 epoch sec; seqNo:18660 2021-02-13T01:07:55Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.21 psi 2021-02-13T01:07:55Z- handleDepth()-Depth_Keller msg: 1613178475972 epoch sec; seqNo:18809; depth 15.21 2021-02-13T01:07:55Z- handleDvl() - RDI_Pathfinder msg: 1613178475.98 epoch sec; seqNo:9089 2021-02-13T01:07:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.00 , 101.64 , 78.65 , 111.10 2021-02-13T01:07:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , -0.10 2021-02-13T01:07:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178476.00 epoch sec; seqNo:18809; 4060908.47 north; 593727.59 east 2021-02-13T01:07:56Z- AHRS_M2 msg: 1613178476.35 epoch sec; seqNo:18661 2021-02-13T01:07:56Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.21 psi 2021-02-13T01:07:56Z- handleDepth()-Depth_Keller msg: 1613178476357 epoch sec; seqNo:18810; depth 15.35 2021-02-13T01:07:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178476.37 epoch sec; seqNo:18810; 4060908.47 north; 593727.59 east 2021-02-13T01:07:56Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-13T01:07:56Z- waiting for 1613178474.12, time is 1613178476.37 2021-02-13T01:07:56Z- LcmTrn::updateTrn() >>>> pose time 1613178476.37, meas time 1613178475.98 <<<< 2021-02-13T01:07:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178475.98 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:07:56Z- TerrainNav::motionUpdate() - currEstimate = (4060908, 593727.59, 15.35) 2021-02-13T01:07:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:07:56Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:07:56Z- TNavBF::Projecting Measurements 2021-02-13T01:07:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:07:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:07:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:07:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:07:56Z- TNavFilter::4 good beams. 2021-02-13T01:07:56Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:07:56Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:07:56Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:07:56Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:07:56Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:07:56Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:07:56Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:07:56Z- TNavBF::mmse selected from filter -1 2021-02-13T01:07:56Z- TerrainMapOctree: (vehN, vehE ) = (4060908.22, 593727.41). 2021-02-13T01:07:56Z- TerrainMapOctree: Min Distance = 624.43. 2021-02-13T01:07:56Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:07:56Z- There is no specific good beam that all particles have in common. 2021-02-13T01:07:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:07:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.40 K: 0.105 ExpectedMeasDiff: nan currMeas.covariance: 10.24 2021-02-13T01:07:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:07:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 16.53 2021-02-13T01:07:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:07:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.18 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 9.90 2021-02-13T01:07:56Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:07:56Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 19.75 2021-02-13T01:07:56Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:07:56Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:07:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:07:56Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:07:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:07:56Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:07:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:07:56Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:07:56Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:07:56Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:07:56Z- Filter 0 No beams used in subcloud update 2021-02-13T01:07:56Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:07:56Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:07:56Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 675.0 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T01:07:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178475.98 sec. 2021-02-13T01:07:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:07:56Z- TNavBF::mmse selected from filter -1 2021-02-13T01:07:56Z- variances([0], [2], and psi_berg) = 674.944999 832.986390 0.000000 2021-02-13T01:07:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:07:56Z- LcmTrn::publishEstimates() - MLE : 4060907.97 593717.09 15.35 2021-02-13T01:07:56Z- LcmTrn::publishEstimates() - MMSE : 4060908.47 593727.59 15.35 2021-02-13T01:07:56Z- LcmTrn::publishEstimates() - COVAR: 674.94 832.99 1.03 0.00 2021-02-13T01:07:56Z- AHRS_M2 msg: 1613178476.75 epoch sec; seqNo:18662 2021-02-13T01:07:56Z- AHRS_M2 msg: 0.62 phi; -0.35 theta; -0.20 psi 2021-02-13T01:07:56Z- handleDepth()-Depth_Keller msg: 1613178476757 epoch sec; seqNo:18811; depth 15.46 2021-02-13T01:07:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178476.77 epoch sec; seqNo:18811; 4060908.86 north; 593728.81 east 2021-02-13T01:07:57Z- AHRS_M2 msg: 1613178477.16 epoch sec; seqNo:18663 2021-02-13T01:07:57Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-13T01:07:57Z- handleDepth()-Depth_Keller msg: 1613178477162 epoch sec; seqNo:18812; depth 15.59 2021-02-13T01:07:57Z- handleDvl() - RDI_Pathfinder msg: 1613178477.18 epoch sec; seqNo:9090 2021-02-13T01:07:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.07 , 100.35 , 78.71 , 109.82 2021-02-13T01:07:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.00 , -0.07 2021-02-13T01:07:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178477.19 epoch sec; seqNo:18812; 4060909.24 north; 593728.80 east 2021-02-13T01:07:57Z- AHRS_M2 msg: 1613178477.56 epoch sec; seqNo:18664 2021-02-13T01:07:57Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.20 psi 2021-02-13T01:07:57Z- handleDepth()-Depth_Keller msg: 1613178477566 epoch sec; seqNo:18813; depth 15.70 2021-02-13T01:07:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178477.58 epoch sec; seqNo:18813; 4060909.62 north; 593728.80 east 2021-02-13T01:07:57Z- AHRS_M2 msg: 1613178477.97 epoch sec; seqNo:18665 2021-02-13T01:07:57Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.20 psi 2021-02-13T01:07:57Z- handleDepth()-Depth_Keller msg: 1613178477976 epoch sec; seqNo:18814; depth 15.83 2021-02-13T01:07:57Z- handleDvl() - RDI_Pathfinder msg: 1613178477.99 epoch sec; seqNo:9091 2021-02-13T01:07:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.14 , 100.42 , 78.78 , 112.59 2021-02-13T01:07:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.00 , -0.00 2021-02-13T01:07:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178478.00 epoch sec; seqNo:18814; 4060909.62 north; 593728.80 east 2021-02-13T01:07:58Z- AHRS_M2 msg: 1613178478.37 epoch sec; seqNo:18666 2021-02-13T01:07:58Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.21 psi 2021-02-13T01:07:58Z- handleDepth()-Depth_Keller msg: 1613178478372 epoch sec; seqNo:18815; depth 15.94 2021-02-13T01:07:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178478.38 epoch sec; seqNo:18815; 4060910.00 north; 593728.79 east 2021-02-13T01:07:58Z- AHRS_M2 msg: 1613178478.78 epoch sec; seqNo:18667 2021-02-13T01:07:58Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.21 psi 2021-02-13T01:07:58Z- handleDepth()-Depth_Keller msg: 1613178478791 epoch sec; seqNo:18816; depth 16.06 2021-02-13T01:07:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178478.80 epoch sec; seqNo:18816; 4060910.38 north; 593728.79 east 2021-02-13T01:07:59Z- AHRS_M2 msg: 1613178479.17 epoch sec; seqNo:18668 2021-02-13T01:07:59Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.22 psi 2021-02-13T01:07:59Z- handleDepth()-Depth_Keller msg: 1613178479180 epoch sec; seqNo:18817; depth 16.18 2021-02-13T01:07:59Z- handleDvl() - RDI_Pathfinder msg: 1613178479.20 epoch sec; seqNo:9092 2021-02-13T01:07:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.07 , 100.35 , 71.53 , 112.52 2021-02-13T01:07:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , 0.06 2021-02-13T01:07:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178479.21 epoch sec; seqNo:18817; 4060910.39 north; 593730.01 east 2021-02-13T01:07:59Z- AHRS_M2 msg: 1613178479.58 epoch sec; seqNo:18669 2021-02-13T01:07:59Z- AHRS_M2 msg: 0.57 phi; -0.35 theta; -0.22 psi 2021-02-13T01:07:59Z- handleDepth()-Depth_Keller msg: 1613178479589 epoch sec; seqNo:18818; depth 16.29 2021-02-13T01:07:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178479.60 epoch sec; seqNo:18818; 4060910.77 north; 593730.01 east 2021-02-13T01:07:59Z- AHRS_M2 msg: 1613178479.98 epoch sec; seqNo:18670 2021-02-13T01:07:59Z- AHRS_M2 msg: 0.57 phi; -0.35 theta; -0.22 psi 2021-02-13T01:07:59Z- handleDepth()-Depth_Keller msg: 1613178479993 epoch sec; seqNo:18819; depth 16.41 2021-02-13T01:08:00Z- handleDvl() - RDI_Pathfinder msg: 1613178480.00 epoch sec; seqNo:9093 2021-02-13T01:08:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.07 , 100.08 , 78.17 , 112.25 2021-02-13T01:08:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.05 , 0.11 2021-02-13T01:08:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178480.02 epoch sec; seqNo:18819; 4060911.15 north; 593730.00 east 2021-02-13T01:08:00Z- AHRS_M2 msg: 1613178480.39 epoch sec; seqNo:18671 2021-02-13T01:08:00Z- AHRS_M2 msg: 0.56 phi; -0.35 theta; -0.22 psi 2021-02-13T01:08:00Z- handleDepth()-Depth_Keller msg: 1613178480394 epoch sec; seqNo:18820; depth 16.53 2021-02-13T01:08:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178480.40 epoch sec; seqNo:18820; 4060911.53 north; 593730.00 east 2021-02-13T01:08:00Z- AHRS_M2 msg: 1613178480.83 epoch sec; seqNo:18672 2021-02-13T01:08:00Z- AHRS_M2 msg: 0.56 phi; -0.35 theta; -0.21 psi 2021-02-13T01:08:00Z- handleDepth()-Depth_Keller msg: 1613178480830 epoch sec; seqNo:18821; depth 16.66 2021-02-13T01:08:00Z- handleDvl() - RDI_Pathfinder msg: 1613178480.84 epoch sec; seqNo:9094 2021-02-13T01:08:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.07 , 99.07 , 72.35 , 113.94 2021-02-13T01:08:00Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.08 , 0.09 2021-02-13T01:08:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178480.85 epoch sec; seqNo:18821; 4060911.53 north; 593730.00 east 2021-02-13T01:08:01Z- AHRS_M2 msg: 1613178481.20 epoch sec; seqNo:18673 2021-02-13T01:08:01Z- AHRS_M2 msg: 0.56 phi; -0.35 theta; -0.21 psi 2021-02-13T01:08:01Z- handleDepth()-Depth_Keller msg: 1613178481203 epoch sec; seqNo:18822; depth 16.78 2021-02-13T01:08:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178481.22 epoch sec; seqNo:18822; 4060911.91 north; 593729.99 east 2021-02-13T01:08:01Z- AHRS_M2 msg: 1613178481.60 epoch sec; seqNo:18674 2021-02-13T01:08:01Z- AHRS_M2 msg: 0.57 phi; -0.35 theta; -0.21 psi 2021-02-13T01:08:01Z- handleDepth()-Depth_Keller msg: 1613178481605 epoch sec; seqNo:18823; depth 16.88 2021-02-13T01:08:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178481.62 epoch sec; seqNo:18823; 4060912.30 north; 593731.21 east 2021-02-13T01:08:01Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-13T01:08:01Z- waiting for 1613178479.37, time is 1613178481.62 2021-02-13T01:08:01Z- LcmTrn::updateTrn() >>>> pose time 1613178481.62, meas time 1613178480.84 <<<< 2021-02-13T01:08:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178480.84 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:08:01Z- TerrainNav::motionUpdate() - currEstimate = (4060912, 593731.21, 16.88) 2021-02-13T01:08:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:08:01Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:08:01Z- TNavBF::Projecting Measurements 2021-02-13T01:08:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:08:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:08:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:08:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:08:01Z- TNavFilter::4 good beams. 2021-02-13T01:08:01Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:08:01Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:08:01Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:08:01Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:08:01Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:08:01Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:08:01Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:08:01Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:01Z- TerrainMapOctree: (vehN, vehE ) = (4060911.73, 593730.67). 2021-02-13T01:08:01Z- TerrainMapOctree: Min Distance = 619.78. 2021-02-13T01:08:01Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:08:01Z- There is no specific good beam that all particles have in common. 2021-02-13T01:08:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:08:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 9.75 2021-02-13T01:08:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:08:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 15.70 2021-02-13T01:08:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:08:01Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 8.37 2021-02-13T01:08:01Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:08:01Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 20.77 2021-02-13T01:08:01Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:08:01Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:08:01Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:08:01Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:08:01Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:01Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:08:01Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:08:01Z- Filter 0 No beams used in subcloud update 2021-02-13T01:08:01Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:08:01Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:08:01Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 674.9 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:08:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178480.84 sec. 2021-02-13T01:08:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:08:01Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:01Z- variances([0], [2], and psi_berg) = 674.882470 832.986365 0.000000 2021-02-13T01:08:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:08:01Z- LcmTrn::publishEstimates() - MLE : 4060911.80 593720.71 16.88 2021-02-13T01:08:01Z- LcmTrn::publishEstimates() - MMSE : 4060912.30 593731.21 16.88 2021-02-13T01:08:01Z- LcmTrn::publishEstimates() - COVAR: 674.88 832.99 0.97 0.00 2021-02-13T01:08:02Z- AHRS_M2 msg: 1613178482.00 epoch sec; seqNo:18675 2021-02-13T01:08:02Z- AHRS_M2 msg: 0.57 phi; -0.35 theta; -0.21 psi 2021-02-13T01:08:02Z- handleDepth()-Depth_Keller msg: 1613178482009 epoch sec; seqNo:18824; depth 17.02 2021-02-13T01:08:02Z- handleDvl() - RDI_Pathfinder msg: 1613178482.03 epoch sec; seqNo:9095 2021-02-13T01:08:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.25 , 98.93 , 78.24 , 109.89 2021-02-13T01:08:02Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.07 , -0.02 2021-02-13T01:08:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178482.04 epoch sec; seqNo:18824; 4060912.68 north; 593731.21 east 2021-02-13T01:08:02Z- AHRS_M2 msg: 1613178482.41 epoch sec; seqNo:18676 2021-02-13T01:08:02Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.20 psi 2021-02-13T01:08:02Z- handleDepth()-Depth_Keller msg: 1613178482412 epoch sec; seqNo:18825; depth 17.15 2021-02-13T01:08:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178482.42 epoch sec; seqNo:18825; 4060913.06 north; 593731.20 east 2021-02-13T01:08:02Z- AHRS_M2 msg: 1613178482.81 epoch sec; seqNo:18677 2021-02-13T01:08:02Z- AHRS_M2 msg: 0.58 phi; -0.35 theta; -0.19 psi 2021-02-13T01:08:02Z- handleDepth()-Depth_Keller msg: 1613178482818 epoch sec; seqNo:18826; depth 17.27 2021-02-13T01:08:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178482.83 epoch sec; seqNo:18826; 4060913.06 north; 593731.20 east 2021-02-13T01:08:03Z- AHRS_M2 msg: 1613178483.22 epoch sec; seqNo:18678 2021-02-13T01:08:03Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.19 psi 2021-02-13T01:08:03Z- handleDepth()-Depth_Keller msg: 1613178483222 epoch sec; seqNo:18827; depth 17.41 2021-02-13T01:08:03Z- handleDvl() - RDI_Pathfinder msg: 1613178483.23 epoch sec; seqNo:9096 2021-02-13T01:08:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.17 , 98.86 , 71.94 , 109.82 2021-02-13T01:08:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.03 , -0.09 2021-02-13T01:08:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178483.24 epoch sec; seqNo:18827; 4060913.43 north; 593731.20 east 2021-02-13T01:08:03Z- AHRS_M2 msg: 1613178483.62 epoch sec; seqNo:18679 2021-02-13T01:08:03Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.18 psi 2021-02-13T01:08:03Z- handleDepth()-Depth_Keller msg: 1613178483629 epoch sec; seqNo:18828; depth 17.54 2021-02-13T01:08:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178483.64 epoch sec; seqNo:18828; 4060913.83 north; 593732.41 east 2021-02-13T01:08:04Z- AHRS_M2 msg: 1613178484.02 epoch sec; seqNo:18680 2021-02-13T01:08:04Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.18 psi 2021-02-13T01:08:04Z- handleDepth()-Depth_Keller msg: 1613178484033 epoch sec; seqNo:18829; depth 17.66 2021-02-13T01:08:04Z- handleDvl() - RDI_Pathfinder msg: 1613178484.05 epoch sec; seqNo:9097 2021-02-13T01:08:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.31 , 97.78 , 70.71 , 108.74 2021-02-13T01:08:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.07 , -0.06 2021-02-13T01:08:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178484.06 epoch sec; seqNo:18829; 4060914.21 north; 593732.41 east 2021-02-13T01:08:04Z- AHRS_M2 msg: 1613178484.43 epoch sec; seqNo:18681 2021-02-13T01:08:04Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.17 psi 2021-02-13T01:08:04Z- handleDepth()-Depth_Keller msg: 1613178484434 epoch sec; seqNo:18830; depth 17.80 2021-02-13T01:08:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178484.44 epoch sec; seqNo:18830; 4060914.58 north; 593732.41 east 2021-02-13T01:08:04Z- AHRS_M2 msg: 1613178484.83 epoch sec; seqNo:18682 2021-02-13T01:08:04Z- AHRS_M2 msg: 0.60 phi; -0.36 theta; -0.17 psi 2021-02-13T01:08:04Z- handleDepth()-Depth_Keller msg: 1613178484836 epoch sec; seqNo:18831; depth 17.92 2021-02-13T01:08:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178484.84 epoch sec; seqNo:18831; 4060914.58 north; 593732.41 east 2021-02-13T01:08:05Z- AHRS_M2 msg: 1613178485.24 epoch sec; seqNo:18683 2021-02-13T01:08:05Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.16 psi 2021-02-13T01:08:05Z- handleDepth()-Depth_Keller msg: 1613178485240 epoch sec; seqNo:18832; depth 18.05 2021-02-13T01:08:05Z- handleDvl() - RDI_Pathfinder msg: 1613178485.25 epoch sec; seqNo:9098 2021-02-13T01:08:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.84 , 95.42 , 78.38 , 110.02 2021-02-13T01:08:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.08 , 0.04 2021-02-13T01:08:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178485.26 epoch sec; seqNo:18832; 4060914.96 north; 593732.40 east 2021-02-13T01:08:05Z- AHRS_M2 msg: 1613178485.64 epoch sec; seqNo:18684 2021-02-13T01:08:05Z- AHRS_M2 msg: 0.59 phi; -0.37 theta; -0.16 psi 2021-02-13T01:08:05Z- handleDepth()-Depth_Keller msg: 1613178485646 epoch sec; seqNo:18833; depth 18.19 2021-02-13T01:08:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178485.66 epoch sec; seqNo:18833; 4060915.34 north; 593732.40 east 2021-02-13T01:08:06Z- AHRS_M2 msg: 1613178486.04 epoch sec; seqNo:18685 2021-02-13T01:08:06Z- AHRS_M2 msg: 0.59 phi; -0.37 theta; -0.15 psi 2021-02-13T01:08:06Z- handleDepth()-Depth_Keller msg: 1613178486053 epoch sec; seqNo:18834; depth 18.31 2021-02-13T01:08:06Z- handleDvl() - RDI_Pathfinder msg: 1613178486.08 epoch sec; seqNo:9099 2021-02-13T01:08:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.88 , 97.78 , 68.67 , 107.52 2021-02-13T01:08:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.00 , 0.10 2021-02-13T01:08:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178486.09 epoch sec; seqNo:18834; 4060915.72 north; 593732.39 east 2021-02-13T01:08:06Z- AHRS_M2 msg: 1613178486.45 epoch sec; seqNo:18686 2021-02-13T01:08:06Z- AHRS_M2 msg: 0.58 phi; -0.37 theta; -0.15 psi 2021-02-13T01:08:06Z- handleDepth()-Depth_Keller msg: 1613178486452 epoch sec; seqNo:18835; depth 18.44 2021-02-13T01:08:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178486.47 epoch sec; seqNo:18835; 4060915.72 north; 593732.39 east 2021-02-13T01:08:06Z- LcmTrn::handleMessages() - handling 41 messages took 0.61 ms and 129 calls... 2021-02-13T01:08:06Z- waiting for 1613178484.62, time is 1613178486.47 2021-02-13T01:08:06Z- LcmTrn::updateTrn() >>>> pose time 1613178486.47, meas time 1613178486.08 <<<< 2021-02-13T01:08:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178486.08 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:08:06Z- TerrainNav::motionUpdate() - currEstimate = (4060916, 593732.39, 18.44) 2021-02-13T01:08:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:08:06Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:08:06Z- TNavBF::Projecting Measurements 2021-02-13T01:08:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:08:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:08:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:08:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:08:06Z- TNavFilter::4 good beams. 2021-02-13T01:08:06Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:08:06Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:08:06Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:08:06Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:08:06Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:08:06Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:08:06Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:08:06Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:06Z- TerrainMapOctree: (vehN, vehE ) = (4060915.44, 593732.30). 2021-02-13T01:08:06Z- TerrainMapOctree: Min Distance = 616.40. 2021-02-13T01:08:06Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:08:06Z- There is no specific good beam that all particles have in common. 2021-02-13T01:08:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:08:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 9.96 2021-02-13T01:08:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:08:06Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-13T01:08:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:08:06Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 7.54 2021-02-13T01:08:06Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:08:06Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 18.50 2021-02-13T01:08:06Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:08:06Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:08:06Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:08:06Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:08:06Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:06Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:08:06Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:08:06Z- Filter 0 No beams used in subcloud update 2021-02-13T01:08:06Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:08:06Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:08:06Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 674.8 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T01:08:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178486.08 sec. 2021-02-13T01:08:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:08:06Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:06Z- variances([0], [2], and psi_berg) = 674.833223 832.986345 0.000000 2021-02-13T01:08:06Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:08:06Z- LcmTrn::publishEstimates() - MLE : 4060915.22 593721.89 18.44 2021-02-13T01:08:06Z- LcmTrn::publishEstimates() - MMSE : 4060915.72 593732.39 18.44 2021-02-13T01:08:06Z- LcmTrn::publishEstimates() - COVAR: 674.83 832.99 0.86 0.00 2021-02-13T01:08:06Z- AHRS_M2 msg: 1613178486.87 epoch sec; seqNo:18687 2021-02-13T01:08:06Z- AHRS_M2 msg: 0.57 phi; -0.37 theta; -0.16 psi 2021-02-13T01:08:06Z- handleDepth()-Depth_Keller msg: 1613178486874 epoch sec; seqNo:18836; depth 18.56 2021-02-13T01:08:06Z- handleDvl() - RDI_Pathfinder msg: 1613178486.89 epoch sec; seqNo:9100 2021-02-13T01:08:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.88 , 97.92 , 0.00 , 108.87 2021-02-13T01:08:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.09 , 0.08 2021-02-13T01:08:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178486.91 epoch sec; seqNo:18836; 4060916.11 north; 593733.61 east 2021-02-13T01:08:07Z- AHRS_M2 msg: 1613178487.27 epoch sec; seqNo:18688 2021-02-13T01:08:07Z- AHRS_M2 msg: 0.57 phi; -0.37 theta; -0.17 psi 2021-02-13T01:08:07Z- handleDepth()-Depth_Keller msg: 1613178487282 epoch sec; seqNo:18837; depth 18.71 2021-02-13T01:08:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178487.29 epoch sec; seqNo:18837; 4060916.49 north; 593733.61 east 2021-02-13T01:08:07Z- AHRS_M2 msg: 1613178487.66 epoch sec; seqNo:18689 2021-02-13T01:08:07Z- AHRS_M2 msg: 0.57 phi; -0.37 theta; -0.17 psi 2021-02-13T01:08:07Z- handleDepth()-Depth_Keller msg: 1613178487667 epoch sec; seqNo:18838; depth 18.83 2021-02-13T01:08:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178487.68 epoch sec; seqNo:18838; 4060916.49 north; 593733.61 east 2021-02-13T01:08:08Z- AHRS_M2 msg: 1613178488.06 epoch sec; seqNo:18690 2021-02-13T01:08:08Z- AHRS_M2 msg: 0.57 phi; -0.36 theta; -0.18 psi 2021-02-13T01:08:08Z- handleDepth()-Depth_Keller msg: 1613178488073 epoch sec; seqNo:18839; depth 18.95 2021-02-13T01:08:08Z- handleDvl() - RDI_Pathfinder msg: 1613178488.08 epoch sec; seqNo:9101 2021-02-13T01:08:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.07 , 97.44 , 69.89 , 110.97 2021-02-13T01:08:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.11 , -0.01 2021-02-13T01:08:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178488.11 epoch sec; seqNo:18839; 4060916.87 north; 593733.60 east 2021-02-13T01:08:08Z- AHRS_M2 msg: 1613178488.47 epoch sec; seqNo:18691 2021-02-13T01:08:08Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.19 psi 2021-02-13T01:08:08Z- handleDepth()-Depth_Keller msg: 1613178488477 epoch sec; seqNo:18840; depth 19.09 2021-02-13T01:08:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178488.49 epoch sec; seqNo:18840; 4060917.25 north; 593733.60 east 2021-02-13T01:08:08Z- AHRS_M2 msg: 1613178488.88 epoch sec; seqNo:18692 2021-02-13T01:08:08Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.20 psi 2021-02-13T01:08:08Z- handleDepth()-Depth_Keller msg: 1613178488886 epoch sec; seqNo:18841; depth 19.21 2021-02-13T01:08:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178488.91 epoch sec; seqNo:18841; 4060917.64 north; 593734.81 east 2021-02-13T01:08:09Z- AHRS_M2 msg: 1613178489.28 epoch sec; seqNo:18693 2021-02-13T01:08:09Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.20 psi 2021-02-13T01:08:09Z- handleDepth()-Depth_Keller msg: 1613178489283 epoch sec; seqNo:18842; depth 19.35 2021-02-13T01:08:09Z- handleDvl() - RDI_Pathfinder msg: 1613178489.31 epoch sec; seqNo:9102 2021-02-13T01:08:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.86 , 78.38 , 110.03 2021-02-13T01:08:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.02 , -0.09 2021-02-13T01:08:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178489.32 epoch sec; seqNo:18842; 4060917.64 north; 593734.81 east 2021-02-13T01:08:09Z- AHRS_M2 msg: 1613178489.68 epoch sec; seqNo:18694 2021-02-13T01:08:09Z- AHRS_M2 msg: 0.58 phi; -0.36 theta; -0.20 psi 2021-02-13T01:08:09Z- handleDepth()-Depth_Keller msg: 1613178489684 epoch sec; seqNo:18843; depth 19.49 2021-02-13T01:08:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178489.69 epoch sec; seqNo:18843; 4060918.02 north; 593734.81 east 2021-02-13T01:08:10Z- AHRS_M2 msg: 1613178490.08 epoch sec; seqNo:18695 2021-02-13T01:08:10Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-13T01:08:10Z- handleDepth()-Depth_Keller msg: 1613178490088 epoch sec; seqNo:18844; depth 19.62 2021-02-13T01:08:10Z- handleDvl() - RDI_Pathfinder msg: 1613178490.10 epoch sec; seqNo:9103 2021-02-13T01:08:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.03 , 97.45 , 69.49 , 106.92 2021-02-13T01:08:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.05 , -0.07 2021-02-13T01:08:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178490.11 epoch sec; seqNo:18844; 4060918.40 north; 593734.80 east 2021-02-13T01:08:10Z- AHRS_M2 msg: 1613178490.49 epoch sec; seqNo:18696 2021-02-13T01:08:10Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.19 psi 2021-02-13T01:08:10Z- handleDepth()-Depth_Keller msg: 1613178490501 epoch sec; seqNo:18845; depth 19.76 2021-02-13T01:08:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178490.51 epoch sec; seqNo:18845; 4060918.78 north; 593734.80 east 2021-02-13T01:08:10Z- AHRS_M2 msg: 1613178490.91 epoch sec; seqNo:18697 2021-02-13T01:08:10Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.18 psi 2021-02-13T01:08:10Z- handleDepth()-Depth_Keller msg: 1613178490923 epoch sec; seqNo:18846; depth 19.88 2021-02-13T01:08:10Z- handleDvl() - RDI_Pathfinder msg: 1613178490.93 epoch sec; seqNo:9104 2021-02-13T01:08:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.99 , 0.00 , 106.51 2021-02-13T01:08:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.11 , 0.00 2021-02-13T01:08:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178490.94 epoch sec; seqNo:18846; 4060919.16 north; 593734.80 east 2021-02-13T01:08:11Z- AHRS_M2 msg: 1613178491.30 epoch sec; seqNo:18698 2021-02-13T01:08:11Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.18 psi 2021-02-13T01:08:11Z- handleDepth()-Depth_Keller msg: 1613178491302 epoch sec; seqNo:18847; depth 20.01 2021-02-13T01:08:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178491.31 epoch sec; seqNo:18847; 4060919.54 north; 593734.79 east 2021-02-13T01:08:11Z- AHRS_M2 msg: 1613178491.70 epoch sec; seqNo:18699 2021-02-13T01:08:11Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.17 psi 2021-02-13T01:08:11Z- handleDepth()-Depth_Keller msg: 1613178491710 epoch sec; seqNo:18848; depth 20.14 2021-02-13T01:08:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178491.72 epoch sec; seqNo:18848; 4060919.55 north; 593736.01 east 2021-02-13T01:08:11Z- LcmTrn::handleMessages() - handling 44 messages took 0.61 ms and 132 calls... 2021-02-13T01:08:11Z- waiting for 1613178489.47, time is 1613178491.72 2021-02-13T01:08:11Z- LcmTrn::updateTrn() >>>> pose time 1613178491.72, meas time 1613178490.93 <<<< 2021-02-13T01:08:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178490.93 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:08:11Z- TerrainNav::motionUpdate() - currEstimate = (4060920, 593736.01, 20.14) 2021-02-13T01:08:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:08:11Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:08:11Z- TNavBF::Projecting Measurements 2021-02-13T01:08:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:08:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:08:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:08:11Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:08:11Z- TNavFilter::3 good beams. 2021-02-13T01:08:11Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:08:11Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:08:11Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:08:11Z- TNavBF:: beamIndex[2] = 3 2021-02-13T01:08:11Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:08:11Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:08:11Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:08:11Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:11Z- TerrainMapOctree: (vehN, vehE ) = (4060918.98, 593735.47). 2021-02-13T01:08:11Z- TerrainMapOctree: Min Distance = 611.81. 2021-02-13T01:08:11Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:08:11Z- There is no specific good beam that all particles have in common. 2021-02-13T01:08:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:08:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.086 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T01:08:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:08:11Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T01:08:11Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:08:11Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 18.15 2021-02-13T01:08:11Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:08:11Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:08:11Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:08:11Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:11Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:08:11Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:08:11Z- Filter 0 No beams used in subcloud update 2021-02-13T01:08:11Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:08:11Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:08:11Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 674.8 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:08:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178490.93 sec. 2021-02-13T01:08:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:08:11Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:11Z- variances([0], [2], and psi_berg) = 674.770712 832.986320 0.000000 2021-02-13T01:08:11Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:08:11Z- LcmTrn::publishEstimates() - MLE : 4060919.05 593725.51 20.14 2021-02-13T01:08:11Z- LcmTrn::publishEstimates() - MMSE : 4060919.55 593736.01 20.14 2021-02-13T01:08:11Z- LcmTrn::publishEstimates() - COVAR: 674.77 832.99 0.94 0.00 2021-02-13T01:08:12Z- AHRS_M2 msg: 1613178492.10 epoch sec; seqNo:18700 2021-02-13T01:08:12Z- AHRS_M2 msg: 0.59 phi; -0.35 theta; -0.17 psi 2021-02-13T01:08:12Z- handleDepth()-Depth_Keller msg: 1613178492109 epoch sec; seqNo:18849; depth 20.28 2021-02-13T01:08:12Z- handleDvl() - RDI_Pathfinder msg: 1613178492.13 epoch sec; seqNo:9105 2021-02-13T01:08:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 97.52 , 68.67 , 105.63 2021-02-13T01:08:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.12 , 0.06 2021-02-13T01:08:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178492.14 epoch sec; seqNo:18849; 4060919.93 north; 593736.01 east 2021-02-13T01:08:12Z- AHRS_M2 msg: 1613178492.51 epoch sec; seqNo:18701 2021-02-13T01:08:12Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.17 psi 2021-02-13T01:08:12Z- handleDepth()-Depth_Keller msg: 1613178492515 epoch sec; seqNo:18850; depth 20.40 2021-02-13T01:08:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178492.53 epoch sec; seqNo:18850; 4060920.31 north; 593736.00 east 2021-02-13T01:08:12Z- AHRS_M2 msg: 1613178492.93 epoch sec; seqNo:18702 2021-02-13T01:08:12Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.17 psi 2021-02-13T01:08:12Z- handleDepth()-Depth_Keller msg: 1613178492932 epoch sec; seqNo:18851; depth 20.54 2021-02-13T01:08:12Z- handleDvl() - RDI_Pathfinder msg: 1613178492.94 epoch sec; seqNo:9106 2021-02-13T01:08:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.51 , 97.99 , 67.86 , 105.29 2021-02-13T01:08:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , 0.12 2021-02-13T01:08:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178492.95 epoch sec; seqNo:18851; 4060920.69 north; 593736.00 east 2021-02-13T01:08:13Z- AHRS_M2 msg: 1613178493.32 epoch sec; seqNo:18703 2021-02-13T01:08:13Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.16 psi 2021-02-13T01:08:13Z- handleDepth()-Depth_Keller msg: 1613178493321 epoch sec; seqNo:18852; depth 20.67 2021-02-13T01:08:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178493.34 epoch sec; seqNo:18852; 4060920.69 north; 593736.00 east 2021-02-13T01:08:13Z- AHRS_M2 msg: 1613178493.72 epoch sec; seqNo:18704 2021-02-13T01:08:13Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.16 psi 2021-02-13T01:08:13Z- handleDepth()-Depth_Keller msg: 1613178493724 epoch sec; seqNo:18853; depth 20.78 2021-02-13T01:08:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178493.73 epoch sec; seqNo:18853; 4060921.06 north; 593736.00 east 2021-02-13T01:08:14Z- AHRS_M2 msg: 1613178494.12 epoch sec; seqNo:18705 2021-02-13T01:08:14Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.15 psi 2021-02-13T01:08:14Z- handleDepth()-Depth_Keller msg: 1613178494130 epoch sec; seqNo:18854; depth 20.90 2021-02-13T01:08:14Z- handleDvl() - RDI_Pathfinder msg: 1613178494.14 epoch sec; seqNo:9107 2021-02-13T01:08:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.99 , 66.63 , 106.51 2021-02-13T01:08:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.00 , 0.13 2021-02-13T01:08:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178494.16 epoch sec; seqNo:18854; 4060921.44 north; 593735.99 east 2021-02-13T01:08:14Z- AHRS_M2 msg: 1613178494.53 epoch sec; seqNo:18706 2021-02-13T01:08:14Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.15 psi 2021-02-13T01:08:14Z- handleDepth()-Depth_Keller msg: 1613178494545 epoch sec; seqNo:18855; depth 21.04 2021-02-13T01:08:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178494.56 epoch sec; seqNo:18855; 4060921.84 north; 593737.21 east 2021-02-13T01:08:14Z- AHRS_M2 msg: 1613178494.93 epoch sec; seqNo:18707 2021-02-13T01:08:14Z- AHRS_M2 msg: 0.60 phi; -0.37 theta; -0.15 psi 2021-02-13T01:08:14Z- handleDepth()-Depth_Keller msg: 1613178494941 epoch sec; seqNo:18856; depth 21.19 2021-02-13T01:08:14Z- handleDvl() - RDI_Pathfinder msg: 1613178494.95 epoch sec; seqNo:9108 2021-02-13T01:08:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.21 , 97.71 , 67.44 , 105.02 2021-02-13T01:08:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.04 , 0.07 2021-02-13T01:08:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178494.97 epoch sec; seqNo:18856; 4060921.84 north; 593737.21 east 2021-02-13T01:08:15Z- AHRS_M2 msg: 1613178495.34 epoch sec; seqNo:18708 2021-02-13T01:08:15Z- AHRS_M2 msg: 0.61 phi; -0.37 theta; -0.16 psi 2021-02-13T01:08:15Z- handleDepth()-Depth_Keller msg: 1613178495340 epoch sec; seqNo:18857; depth 21.31 2021-02-13T01:08:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178495.35 epoch sec; seqNo:18857; 4060922.21 north; 593737.20 east 2021-02-13T01:08:15Z- AHRS_M2 msg: 1613178495.74 epoch sec; seqNo:18709 2021-02-13T01:08:15Z- AHRS_M2 msg: 0.61 phi; -0.38 theta; -0.16 psi 2021-02-13T01:08:15Z- handleDepth()-Depth_Keller msg: 1613178495745 epoch sec; seqNo:18858; depth 21.44 2021-02-13T01:08:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178495.75 epoch sec; seqNo:18858; 4060922.59 north; 593737.20 east 2021-02-13T01:08:16Z- AHRS_M2 msg: 1613178496.15 epoch sec; seqNo:18710 2021-02-13T01:08:16Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.17 psi 2021-02-13T01:08:16Z- handleDepth()-Depth_Keller msg: 1613178496152 epoch sec; seqNo:18859; depth 21.58 2021-02-13T01:08:16Z- handleDvl() - RDI_Pathfinder msg: 1613178496.17 epoch sec; seqNo:9109 2021-02-13T01:08:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.58 , 98.05 , 65.81 , 107.79 2021-02-13T01:08:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.08 , 0.03 2021-02-13T01:08:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178496.18 epoch sec; seqNo:18859; 4060922.97 north; 593737.20 east 2021-02-13T01:08:16Z- AHRS_M2 msg: 1613178496.55 epoch sec; seqNo:18711 2021-02-13T01:08:16Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.17 psi 2021-02-13T01:08:16Z- handleDepth()-Depth_Keller msg: 1613178496552 epoch sec; seqNo:18860; depth 21.73 2021-02-13T01:08:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178496.56 epoch sec; seqNo:18860; 4060922.99 north; 593738.42 east 2021-02-13T01:08:16Z- AHRS_M2 msg: 1613178496.95 epoch sec; seqNo:18712 2021-02-13T01:08:16Z- AHRS_M2 msg: 0.63 phi; -0.38 theta; -0.16 psi 2021-02-13T01:08:16Z- LcmTrn::handleMessages() - handling 42 messages took 0.61 ms and 130 calls... 2021-02-13T01:08:16Z- waiting for 1613178494.72, time is 1613178496.95 2021-02-13T01:08:16Z- LcmTrn::updateTrn() >>>> pose time 1613178496.95, meas time 1613178496.17 <<<< 2021-02-13T01:08:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178496.17 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:08:16Z- TerrainNav::motionUpdate() - currEstimate = (4060923, 593738.42, 21.73) 2021-02-13T01:08:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:08:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:08:16Z- TNavBF::Projecting Measurements 2021-02-13T01:08:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:08:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:08:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:08:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:08:16Z- TNavFilter::4 good beams. 2021-02-13T01:08:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:08:16Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:08:16Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:08:16Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:08:16Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:08:16Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:08:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:08:16Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:16Z- TerrainMapOctree: (vehN, vehE ) = (4060922.47, 593738.06). 2021-02-13T01:08:16Z- TerrainMapOctree: Min Distance = 607.75. 2021-02-13T01:08:16Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:08:16Z- There is no specific good beam that all particles have in common. 2021-02-13T01:08:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:08:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 9.88 2021-02-13T01:08:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:08:16Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 15.38 2021-02-13T01:08:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:08:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 6.93 2021-02-13T01:08:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:08:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 18.59 2021-02-13T01:08:17Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:08:17Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:08:17Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:08:17Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:08:17Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:17Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:08:17Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:08:17Z- Filter 0 No beams used in subcloud update 2021-02-13T01:08:17Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:08:17Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:08:17Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 674.7 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T01:08:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178496.17 sec. 2021-02-13T01:08:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:08:17Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:17Z- variances([0], [2], and psi_berg) = 674.714986 832.986297 0.000000 2021-02-13T01:08:17Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:08:17Z- LcmTrn::publishEstimates() - MLE : 4060922.49 593751.92 21.73 2021-02-13T01:08:17Z- LcmTrn::publishEstimates() - MMSE : 4060922.99 593738.42 21.73 2021-02-13T01:08:17Z- LcmTrn::publishEstimates() - COVAR: 674.71 832.99 0.86 0.00 2021-02-13T01:08:17Z- handleDepth()-Depth_Keller msg: 1613178496957 epoch sec; seqNo:18861; depth 21.86 2021-02-13T01:08:17Z- handleDvl() - RDI_Pathfinder msg: 1613178496.98 epoch sec; seqNo:9110 2021-02-13T01:08:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.21 , 97.99 , 67.44 , 106.51 2021-02-13T01:08:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.07 , -0.03 2021-02-13T01:08:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178497.02 epoch sec; seqNo:18861; 4060923.36 north; 593738.41 east 2021-02-13T01:08:17Z- AHRS_M2 msg: 1613178497.36 epoch sec; seqNo:18713 2021-02-13T01:08:17Z- AHRS_M2 msg: 0.63 phi; -0.39 theta; -0.16 psi 2021-02-13T01:08:17Z- handleDepth()-Depth_Keller msg: 1613178497373 epoch sec; seqNo:18862; depth 22.03 2021-02-13T01:08:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178497.40 epoch sec; seqNo:18862; 4060923.74 north; 593738.41 east 2021-02-13T01:08:17Z- AHRS_M2 msg: 1613178497.76 epoch sec; seqNo:18714 2021-02-13T01:08:17Z- AHRS_M2 msg: 0.63 phi; -0.39 theta; -0.16 psi 2021-02-13T01:08:17Z- handleDepth()-Depth_Keller msg: 1613178497765 epoch sec; seqNo:18863; depth 22.14 2021-02-13T01:08:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178497.80 epoch sec; seqNo:18863; 4060924.12 north; 593738.40 east 2021-02-13T01:08:18Z- AHRS_M2 msg: 1613178498.16 epoch sec; seqNo:18715 2021-02-13T01:08:18Z- AHRS_M2 msg: 0.63 phi; -0.39 theta; -0.17 psi 2021-02-13T01:08:18Z- handleDepth()-Depth_Keller msg: 1613178498169 epoch sec; seqNo:18864; depth 22.27 2021-02-13T01:08:18Z- handleDvl() - RDI_Pathfinder msg: 1613178498.18 epoch sec; seqNo:9111 2021-02-13T01:08:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.21 , 90.75 , 66.21 , 106.57 2021-02-13T01:08:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.05 , -0.04 2021-02-13T01:08:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178498.21 epoch sec; seqNo:18864; 4060924.12 north; 593738.40 east 2021-02-13T01:08:18Z- AHRS_M2 msg: 1613178498.57 epoch sec; seqNo:18716 2021-02-13T01:08:18Z- AHRS_M2 msg: 0.64 phi; -0.39 theta; -0.17 psi 2021-02-13T01:08:18Z- handleDepth()-Depth_Keller msg: 1613178498573 epoch sec; seqNo:18865; depth 22.44 2021-02-13T01:08:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178498.60 epoch sec; seqNo:18865; 4060924.50 north; 593738.40 east 2021-02-13T01:08:18Z- AHRS_M2 msg: 1613178498.97 epoch sec; seqNo:18717 2021-02-13T01:08:18Z- AHRS_M2 msg: 0.64 phi; -0.39 theta; -0.17 psi 2021-02-13T01:08:18Z- handleDepth()-Depth_Keller msg: 1613178498981 epoch sec; seqNo:18866; depth 22.57 2021-02-13T01:08:19Z- handleDvl() - RDI_Pathfinder msg: 1613178499.01 epoch sec; seqNo:9112 2021-02-13T01:08:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 98.26 , 65.81 , 106.78 2021-02-13T01:08:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , -0.05 2021-02-13T01:08:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178499.02 epoch sec; seqNo:18866; 4060924.89 north; 593739.62 east 2021-02-13T01:08:19Z- AHRS_M2 msg: 1613178499.38 epoch sec; seqNo:18718 2021-02-13T01:08:19Z- AHRS_M2 msg: 0.64 phi; -0.39 theta; -0.17 psi 2021-02-13T01:08:19Z- handleDepth()-Depth_Keller msg: 1613178499385 epoch sec; seqNo:18867; depth 22.72 2021-02-13T01:08:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178499.41 epoch sec; seqNo:18867; 4060925.27 north; 593739.61 east 2021-02-13T01:08:19Z- AHRS_M2 msg: 1613178499.78 epoch sec; seqNo:18719 2021-02-13T01:08:19Z- AHRS_M2 msg: 0.64 phi; -0.39 theta; -0.17 psi 2021-02-13T01:08:19Z- handleDepth()-Depth_Keller msg: 1613178499785 epoch sec; seqNo:18868; depth 22.86 2021-02-13T01:08:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178499.81 epoch sec; seqNo:18868; 4060925.27 north; 593739.61 east 2021-02-13T01:08:20Z- AHRS_M2 msg: 1613178500.18 epoch sec; seqNo:18720 2021-02-13T01:08:20Z- AHRS_M2 msg: 0.64 phi; -0.39 theta; -0.17 psi 2021-02-13T01:08:20Z- handleDepth()-Depth_Keller msg: 1613178500189 epoch sec; seqNo:18869; depth 23.01 2021-02-13T01:08:20Z- handleDvl() - RDI_Pathfinder msg: 1613178500.20 epoch sec; seqNo:9113 2021-02-13T01:08:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.17 , 90.75 , 66.22 , 106.58 2021-02-13T01:08:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.00 , -0.08 2021-02-13T01:08:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178500.21 epoch sec; seqNo:18869; 4060925.65 north; 593739.61 east 2021-02-13T01:08:20Z- AHRS_M2 msg: 1613178500.59 epoch sec; seqNo:18721 2021-02-13T01:08:20Z- AHRS_M2 msg: 0.64 phi; -0.39 theta; -0.17 psi 2021-02-13T01:08:20Z- handleDepth()-Depth_Keller msg: 1613178500597 epoch sec; seqNo:18870; depth 23.16 2021-02-13T01:08:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178500.63 epoch sec; seqNo:18870; 4060926.03 north; 593739.60 east 2021-02-13T01:08:20Z- AHRS_M2 msg: 1613178500.99 epoch sec; seqNo:18722 2021-02-13T01:08:20Z- AHRS_M2 msg: 0.64 phi; -0.39 theta; -0.18 psi 2021-02-13T01:08:20Z- handleDepth()-Depth_Keller msg: 1613178500997 epoch sec; seqNo:18871; depth 23.30 2021-02-13T01:08:21Z- handleDvl() - RDI_Pathfinder msg: 1613178501.02 epoch sec; seqNo:9114 2021-02-13T01:08:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.57 , 98.26 , 65.81 , 106.78 2021-02-13T01:08:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , -0.09 2021-02-13T01:08:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178501.04 epoch sec; seqNo:18871; 4060926.42 north; 593740.82 east 2021-02-13T01:08:21Z- AHRS_M2 msg: 1613178501.40 epoch sec; seqNo:18723 2021-02-13T01:08:21Z- AHRS_M2 msg: 0.64 phi; -0.39 theta; -0.18 psi 2021-02-13T01:08:21Z- handleDepth()-Depth_Keller msg: 1613178501401 epoch sec; seqNo:18872; depth 23.46 2021-02-13T01:08:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178501.44 epoch sec; seqNo:18872; 4060926.42 north; 593740.82 east 2021-02-13T01:08:21Z- AHRS_M2 msg: 1613178501.80 epoch sec; seqNo:18724 2021-02-13T01:08:21Z- AHRS_M2 msg: 0.63 phi; -0.39 theta; -0.19 psi 2021-02-13T01:08:21Z- handleDepth()-Depth_Keller msg: 1613178501809 epoch sec; seqNo:18873; depth 23.59 2021-02-13T01:08:21Z- handleDvl() - RDI_Pathfinder msg: 1613178501.82 epoch sec; seqNo:9115 2021-02-13T01:08:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.16 , 98.12 , 64.99 , 106.64 2021-02-13T01:08:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.10 , -0.06 2021-02-13T01:08:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178501.86 epoch sec; seqNo:18873; 4060926.80 north; 593740.82 east 2021-02-13T01:08:22Z- LcmTrn::handleMessages() - handling 44 messages took 0.82 ms and 128 calls... 2021-02-13T01:08:22Z- waiting for 1613178499.95, time is 1613178501.86 2021-02-13T01:08:22Z- LcmTrn::updateTrn() equating pose 1613178501.86 and meas times 1613178501.86 2021-02-13T01:08:22Z- LcmTrn::updateTrn() >>>> pose time 1613178501.86, meas time 1613178501.86 <<<< 2021-02-13T01:08:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178501.86 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:08:22Z- TerrainNav::motionUpdate() - currEstimate = (4060927, 593740.82, 23.59) 2021-02-13T01:08:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:08:22Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:08:22Z- TNavBF::Projecting Measurements 2021-02-13T01:08:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:08:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:08:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:08:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:08:22Z- TNavFilter::4 good beams. 2021-02-13T01:08:22Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:08:22Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:08:22Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:08:22Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:08:22Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:08:22Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:08:22Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:08:22Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:22Z- TerrainMapOctree: (vehN, vehE ) = (4060926.80, 593740.82). 2021-02-13T01:08:22Z- TerrainMapOctree: Min Distance = 603.10. 2021-02-13T01:08:22Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:08:22Z- There is no specific good beam that all particles have in common. 2021-02-13T01:08:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:08:22Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.096 ExpectedMeasDiff: nan currMeas.covariance: 8.56 2021-02-13T01:08:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:08:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.40 2021-02-13T01:08:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:08:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.099 ExpectedMeasDiff: nan currMeas.covariance: 6.76 2021-02-13T01:08:22Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:08:22Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 18.20 2021-02-13T01:08:22Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:08:22Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:08:22Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:08:22Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:08:22Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:22Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:08:22Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:08:22Z- Filter 0 No beams used in subcloud update 2021-02-13T01:08:22Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:08:22Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:08:22Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 674.7 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T01:08:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178501.86 sec. 2021-02-13T01:08:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:08:22Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:22Z- variances([0], [2], and psi_berg) = 674.670862 832.986280 0.000000 2021-02-13T01:08:22Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:08:22Z- LcmTrn::publishEstimates() - MLE : 4060926.30 593731.32 23.59 2021-02-13T01:08:22Z- LcmTrn::publishEstimates() - MMSE : 4060926.80 593740.82 23.59 2021-02-13T01:08:22Z- LcmTrn::publishEstimates() - COVAR: 674.67 832.99 0.81 0.00 2021-02-13T01:08:22Z- AHRS_M2 msg: 1613178502.20 epoch sec; seqNo:18725 2021-02-13T01:08:22Z- AHRS_M2 msg: 0.63 phi; -0.39 theta; -0.19 psi 2021-02-13T01:08:22Z- handleDepth()-Depth_Keller msg: 1613178502217 epoch sec; seqNo:18874; depth 23.75 2021-02-13T01:08:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178502.23 epoch sec; seqNo:18874; 4060927.18 north; 593740.81 east 2021-02-13T01:08:22Z- AHRS_M2 msg: 1613178502.61 epoch sec; seqNo:18726 2021-02-13T01:08:22Z- AHRS_M2 msg: 0.62 phi; -0.38 theta; -0.20 psi 2021-02-13T01:08:22Z- handleDepth()-Depth_Keller msg: 1613178502617 epoch sec; seqNo:18875; depth 23.90 2021-02-13T01:08:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178502.64 epoch sec; seqNo:18875; 4060927.56 north; 593740.81 east 2021-02-13T01:08:23Z- AHRS_M2 msg: 1613178503.01 epoch sec; seqNo:18727 2021-02-13T01:08:23Z- AHRS_M2 msg: 0.61 phi; -0.38 theta; -0.20 psi 2021-02-13T01:08:23Z- handleDepth()-Depth_Keller msg: 1613178503017 epoch sec; seqNo:18876; depth 24.04 2021-02-13T01:08:23Z- handleDvl() - RDI_Pathfinder msg: 1613178503.03 epoch sec; seqNo:9116 2021-02-13T01:08:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.94 , 90.82 , 63.76 , 104.21 2021-02-13T01:08:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.14 , -0.01 2021-02-13T01:08:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178503.07 epoch sec; seqNo:18876; 4060927.94 north; 593740.80 east 2021-02-13T01:08:23Z- AHRS_M2 msg: 1613178503.42 epoch sec; seqNo:18728 2021-02-13T01:08:23Z- AHRS_M2 msg: 0.60 phi; -0.38 theta; -0.21 psi 2021-02-13T01:08:23Z- handleDepth()-Depth_Keller msg: 1613178503429 epoch sec; seqNo:18877; depth 24.19 2021-02-13T01:08:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178503.46 epoch sec; seqNo:18877; 4060927.94 north; 593740.80 east 2021-02-13T01:08:23Z- AHRS_M2 msg: 1613178503.82 epoch sec; seqNo:18729 2021-02-13T01:08:23Z- AHRS_M2 msg: 0.60 phi; -0.38 theta; -0.21 psi 2021-02-13T01:08:23Z- handleDepth()-Depth_Keller msg: 1613178503829 epoch sec; seqNo:18878; depth 24.34 2021-02-13T01:08:23Z- handleDvl() - RDI_Pathfinder msg: 1613178503.85 epoch sec; seqNo:9117 2021-02-13T01:08:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.53 , 98.39 , 65.40 , 104.48 2021-02-13T01:08:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.10 , 0.05 2021-02-13T01:08:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178503.90 epoch sec; seqNo:18878; 4060928.33 north; 593742.02 east 2021-02-13T01:08:24Z- AHRS_M2 msg: 1613178504.22 epoch sec; seqNo:18730 2021-02-13T01:08:24Z- AHRS_M2 msg: 0.59 phi; -0.38 theta; -0.22 psi 2021-02-13T01:08:24Z- handleDepth()-Depth_Keller msg: 1613178504233 epoch sec; seqNo:18879; depth 24.49 2021-02-13T01:08:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178504.27 epoch sec; seqNo:18879; 4060928.71 north; 593742.02 east 2021-02-13T01:08:24Z- AHRS_M2 msg: 1613178504.63 epoch sec; seqNo:18731 2021-02-13T01:08:24Z- AHRS_M2 msg: 0.58 phi; -0.37 theta; -0.22 psi 2021-02-13T01:08:24Z- handleDepth()-Depth_Keller msg: 1613178504638 epoch sec; seqNo:18880; depth 24.63 2021-02-13T01:08:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178504.66 epoch sec; seqNo:18880; 4060929.09 north; 593742.01 east 2021-02-13T01:08:25Z- AHRS_M2 msg: 1613178505.03 epoch sec; seqNo:18732 2021-02-13T01:08:25Z- AHRS_M2 msg: 0.58 phi; -0.37 theta; -0.22 psi 2021-02-13T01:08:25Z- handleDepth()-Depth_Keller msg: 1613178505041 epoch sec; seqNo:18881; depth 24.77 2021-02-13T01:08:25Z- handleDvl() - RDI_Pathfinder msg: 1613178505.07 epoch sec; seqNo:9118 2021-02-13T01:08:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 98.19 , 64.99 , 104.28 2021-02-13T01:08:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.09 , 0.08 2021-02-13T01:08:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178505.08 epoch sec; seqNo:18881; 4060929.09 north; 593742.01 east 2021-02-13T01:08:25Z- AHRS_M2 msg: 1613178505.44 epoch sec; seqNo:18733 2021-02-13T01:08:25Z- AHRS_M2 msg: 0.58 phi; -0.37 theta; -0.22 psi 2021-02-13T01:08:25Z- handleDepth()-Depth_Keller msg: 1613178505441 epoch sec; seqNo:18882; depth 24.94 2021-02-13T01:08:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178505.46 epoch sec; seqNo:18882; 4060929.47 north; 593742.01 east 2021-02-13T01:08:25Z- AHRS_M2 msg: 1613178505.85 epoch sec; seqNo:18734 2021-02-13T01:08:25Z- AHRS_M2 msg: 0.58 phi; -0.37 theta; -0.21 psi 2021-02-13T01:08:25Z- handleDepth()-Depth_Keller msg: 1613178505858 epoch sec; seqNo:18883; depth 25.06 2021-02-13T01:08:25Z- handleDvl() - RDI_Pathfinder msg: 1613178505.88 epoch sec; seqNo:9119 2021-02-13T01:08:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.31 , 98.06 , 63.77 , 102.93 2021-02-13T01:08:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , 0.06 2021-02-13T01:08:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178505.89 epoch sec; seqNo:18883; 4060929.85 north; 593742.00 east 2021-02-13T01:08:26Z- AHRS_M2 msg: 1613178506.24 epoch sec; seqNo:18735 2021-02-13T01:08:26Z- AHRS_M2 msg: 0.58 phi; -0.37 theta; -0.21 psi 2021-02-13T01:08:26Z- handleDepth()-Depth_Keller msg: 1613178506248 epoch sec; seqNo:18884; depth 25.21 2021-02-13T01:08:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178506.26 epoch sec; seqNo:18884; 4060930.22 north; 593742.00 east 2021-02-13T01:08:26Z- AHRS_M2 msg: 1613178506.65 epoch sec; seqNo:18736 2021-02-13T01:08:26Z- AHRS_M2 msg: 0.58 phi; -0.37 theta; -0.21 psi 2021-02-13T01:08:26Z- handleDepth()-Depth_Keller msg: 1613178506654 epoch sec; seqNo:18885; depth 25.34 2021-02-13T01:08:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178506.66 epoch sec; seqNo:18885; 4060930.22 north; 593742.00 east 2021-02-13T01:08:27Z- AHRS_M2 msg: 1613178507.05 epoch sec; seqNo:18737 2021-02-13T01:08:27Z- AHRS_M2 msg: 0.58 phi; -0.37 theta; -0.20 psi 2021-02-13T01:08:27Z- handleDepth()-Depth_Keller msg: 1613178507056 epoch sec; seqNo:18886; depth 25.48 2021-02-13T01:08:27Z- handleDvl() - RDI_Pathfinder msg: 1613178507.07 epoch sec; seqNo:9120 2021-02-13T01:08:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.85 , 89.81 , 64.18 , 101.98 2021-02-13T01:08:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , 0.03 2021-02-13T01:08:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178507.09 epoch sec; seqNo:18886; 4060930.62 north; 593743.22 east 2021-02-13T01:08:27Z- LcmTrn::handleMessages() - handling 44 messages took 0.84 ms and 128 calls... 2021-02-13T01:08:27Z- waiting for 1613178504.86, time is 1613178507.09 2021-02-13T01:08:27Z- LcmTrn::updateTrn() equating pose 1613178507.09 and meas times 1613178507.09 2021-02-13T01:08:27Z- LcmTrn::updateTrn() >>>> pose time 1613178507.09, meas time 1613178507.09 <<<< 2021-02-13T01:08:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178507.09 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:08:27Z- TerrainNav::motionUpdate() - currEstimate = (4060931, 593743.22, 25.48) 2021-02-13T01:08:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:08:27Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:08:27Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178507.09 sec.; No relevant navigation data available 2021-02-13T01:08:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:08:27Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:27Z- variances([0], [2], and psi_berg) = 674.626757 832.986262 0.000000 2021-02-13T01:08:27Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:08:27Z- LcmTrn::publishEstimates() - MLE : 4060930.12 593756.72 25.48 2021-02-13T01:08:27Z- LcmTrn::publishEstimates() - MMSE : 4060930.62 593743.22 25.48 2021-02-13T01:08:27Z- LcmTrn::publishEstimates() - COVAR: 674.63 832.99 1.04 0.00 2021-02-13T01:08:27Z- AHRS_M2 msg: 1613178507.46 epoch sec; seqNo:18738 2021-02-13T01:08:27Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.20 psi 2021-02-13T01:08:27Z- handleDepth()-Depth_Keller msg: 1613178507463 epoch sec; seqNo:18887; depth 25.61 2021-02-13T01:08:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178507.47 epoch sec; seqNo:18887; 4060931.00 north; 593743.21 east 2021-02-13T01:08:27Z- AHRS_M2 msg: 1613178507.86 epoch sec; seqNo:18739 2021-02-13T01:08:27Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.19 psi 2021-02-13T01:08:27Z- handleDepth()-Depth_Keller msg: 1613178507870 epoch sec; seqNo:18888; depth 25.77 2021-02-13T01:08:27Z- handleDvl() - RDI_Pathfinder msg: 1613178507.88 epoch sec; seqNo:9121 2021-02-13T01:08:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:08:27Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:08:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178507.89 epoch sec; seqNo:18888; 4060931.00 north; 593743.21 east 2021-02-13T01:08:28Z- AHRS_M2 msg: 1613178508.26 epoch sec; seqNo:18740 2021-02-13T01:08:28Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.19 psi 2021-02-13T01:08:28Z- handleDepth()-Depth_Keller msg: 1613178508269 epoch sec; seqNo:18889; depth 25.91 2021-02-13T01:08:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178508.29 epoch sec; seqNo:18889; 4060931.37 north; 593743.21 east 2021-02-13T01:08:28Z- AHRS_M2 msg: 1613178508.67 epoch sec; seqNo:18741 2021-02-13T01:08:28Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.19 psi 2021-02-13T01:08:28Z- handleDepth()-Depth_Keller msg: 1613178508673 epoch sec; seqNo:18890; depth 26.04 2021-02-13T01:08:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178508.68 epoch sec; seqNo:18890; 4060931.75 north; 593743.20 east 2021-02-13T01:08:29Z- AHRS_M2 msg: 1613178509.07 epoch sec; seqNo:18742 2021-02-13T01:08:29Z- AHRS_M2 msg: 0.59 phi; -0.36 theta; -0.19 psi 2021-02-13T01:08:29Z- handleDepth()-Depth_Keller msg: 1613178509086 epoch sec; seqNo:18891; depth 26.17 2021-02-13T01:08:29Z- handleDvl() - RDI_Pathfinder msg: 1613178509.10 epoch sec; seqNo:9122 2021-02-13T01:08:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.31 , 87.78 , 62.54 , 98.60 2021-02-13T01:08:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , -0.03 2021-02-13T01:08:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178509.11 epoch sec; seqNo:18891; 4060932.13 north; 593743.20 east 2021-02-13T01:08:29Z- AHRS_M2 msg: 1613178509.48 epoch sec; seqNo:18743 2021-02-13T01:08:29Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.18 psi 2021-02-13T01:08:29Z- handleDepth()-Depth_Keller msg: 1613178509480 epoch sec; seqNo:18892; depth 26.32 2021-02-13T01:08:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178509.49 epoch sec; seqNo:18892; 4060932.14 north; 593744.42 east 2021-02-13T01:08:29Z- AHRS_M2 msg: 1613178509.88 epoch sec; seqNo:18744 2021-02-13T01:08:29Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.18 psi 2021-02-13T01:08:29Z- handleDepth()-Depth_Keller msg: 1613178509889 epoch sec; seqNo:18893; depth 26.44 2021-02-13T01:08:29Z- handleDvl() - RDI_Pathfinder msg: 1613178509.90 epoch sec; seqNo:9123 2021-02-13T01:08:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.49 , 0.00 , 62.95 , 97.99 2021-02-13T01:08:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , -0.02 2021-02-13T01:08:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178509.91 epoch sec; seqNo:18893; 4060932.52 north; 593744.41 east 2021-02-13T01:08:30Z- AHRS_M2 msg: 1613178510.28 epoch sec; seqNo:18745 2021-02-13T01:08:30Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.18 psi 2021-02-13T01:08:30Z- handleDepth()-Depth_Keller msg: 1613178510297 epoch sec; seqNo:18894; depth 26.58 2021-02-13T01:08:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178510.31 epoch sec; seqNo:18894; 4060932.90 north; 593744.41 east 2021-02-13T01:08:30Z- AHRS_M2 msg: 1613178510.69 epoch sec; seqNo:18746 2021-02-13T01:08:30Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.18 psi 2021-02-13T01:08:30Z- handleDepth()-Depth_Keller msg: 1613178510692 epoch sec; seqNo:18895; depth 26.70 2021-02-13T01:08:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178510.71 epoch sec; seqNo:18895; 4060933.28 north; 593744.41 east 2021-02-13T01:08:31Z- AHRS_M2 msg: 1613178511.09 epoch sec; seqNo:18747 2021-02-13T01:08:31Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.18 psi 2021-02-13T01:08:31Z- handleDepth()-Depth_Keller msg: 1613178511097 epoch sec; seqNo:18896; depth 26.84 2021-02-13T01:08:31Z- handleDvl() - RDI_Pathfinder msg: 1613178511.11 epoch sec; seqNo:9124 2021-02-13T01:08:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 86.08 , 61.72 , 97.99 2021-02-13T01:08:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , -0.01 2021-02-13T01:08:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178511.14 epoch sec; seqNo:18896; 4060933.28 north; 593744.41 east 2021-02-13T01:08:31Z- AHRS_M2 msg: 1613178511.50 epoch sec; seqNo:18748 2021-02-13T01:08:31Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.17 psi 2021-02-13T01:08:31Z- handleDepth()-Depth_Keller msg: 1613178511500 epoch sec; seqNo:18897; depth 26.96 2021-02-13T01:08:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178511.51 epoch sec; seqNo:18897; 4060933.66 north; 593744.40 east 2021-02-13T01:08:31Z- AHRS_M2 msg: 1613178511.90 epoch sec; seqNo:18749 2021-02-13T01:08:31Z- AHRS_M2 msg: 0.60 phi; -0.35 theta; -0.17 psi 2021-02-13T01:08:31Z- handleDepth()-Depth_Keller msg: 1613178511906 epoch sec; seqNo:18898; depth 27.09 2021-02-13T01:08:31Z- handleDvl() - RDI_Pathfinder msg: 1613178511.92 epoch sec; seqNo:9125 2021-02-13T01:08:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.07 , 85.00 , 62.94 , 97.99 2021-02-13T01:08:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , -0.02 2021-02-13T01:08:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178511.93 epoch sec; seqNo:18898; 4060934.05 north; 593745.62 east 2021-02-13T01:08:32Z- LcmTrn::handleMessages() - handling 41 messages took 0.63 ms and 129 calls... 2021-02-13T01:08:32Z- waiting for 1613178510.09, time is 1613178511.93 2021-02-13T01:08:32Z- LcmTrn::updateTrn() equating pose 1613178511.93 and meas times 1613178511.93 2021-02-13T01:08:32Z- LcmTrn::updateTrn() >>>> pose time 1613178511.93, meas time 1613178511.93 <<<< 2021-02-13T01:08:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178511.93 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:08:32Z- TerrainNav::motionUpdate() - currEstimate = (4060934, 593745.62, 27.09) 2021-02-13T01:08:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:08:32Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:08:32Z- TNavBF::Projecting Measurements 2021-02-13T01:08:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:08:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:08:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:08:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:08:32Z- TNavFilter::4 good beams. 2021-02-13T01:08:32Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:08:32Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:08:32Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:08:32Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:08:32Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:08:32Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:08:32Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:08:32Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:32Z- TerrainMapOctree: (vehN, vehE ) = (4060934.05, 593745.62). 2021-02-13T01:08:32Z- TerrainMapOctree: Min Distance = 595.17. 2021-02-13T01:08:32Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:08:32Z- There is no specific good beam that all particles have in common. 2021-02-13T01:08:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:08:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.118 ExpectedMeasDiff: nan currMeas.covariance: 7.63 2021-02-13T01:08:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:08:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.077 ExpectedMeasDiff: nan currMeas.covariance: 11.56 2021-02-13T01:08:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:08:32Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.112 ExpectedMeasDiff: nan currMeas.covariance: 6.34 2021-02-13T01:08:32Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:08:32Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T01:08:32Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:08:32Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:08:32Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:08:32Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:08:32Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:32Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:08:32Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:08:32Z- Filter 0 No beams used in subcloud update 2021-02-13T01:08:32Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:08:32Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:08:32Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 674.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:08:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178511.93 sec. 2021-02-13T01:08:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:08:32Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:32Z- variances([0], [2], and psi_berg) = 674.584233 832.986245 0.000000 2021-02-13T01:08:32Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:08:32Z- LcmTrn::publishEstimates() - MLE : 4060933.55 593737.12 27.09 2021-02-13T01:08:32Z- LcmTrn::publishEstimates() - MMSE : 4060934.05 593745.62 27.09 2021-02-13T01:08:32Z- LcmTrn::publishEstimates() - COVAR: 674.58 832.99 0.86 0.00 2021-02-13T01:08:32Z- AHRS_M2 msg: 1613178512.30 epoch sec; seqNo:18750 2021-02-13T01:08:32Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.17 psi 2021-02-13T01:08:32Z- handleDepth()-Depth_Keller msg: 1613178512308 epoch sec; seqNo:18899; depth 27.23 2021-02-13T01:08:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178512.33 epoch sec; seqNo:18899; 4060934.43 north; 593745.61 east 2021-02-13T01:08:32Z- AHRS_M2 msg: 1613178512.71 epoch sec; seqNo:18751 2021-02-13T01:08:32Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.17 psi 2021-02-13T01:08:32Z- handleDepth()-Depth_Keller msg: 1613178512712 epoch sec; seqNo:18900; depth 27.35 2021-02-13T01:08:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178512.72 epoch sec; seqNo:18900; 4060934.43 north; 593745.61 east 2021-02-13T01:08:33Z- AHRS_M2 msg: 1613178513.11 epoch sec; seqNo:18752 2021-02-13T01:08:33Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.17 psi 2021-02-13T01:08:33Z- handleDepth()-Depth_Keller msg: 1613178513116 epoch sec; seqNo:18901; depth 27.48 2021-02-13T01:08:33Z- handleDvl() - RDI_Pathfinder msg: 1613178513.13 epoch sec; seqNo:9126 2021-02-13T01:08:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.67 , 85.00 , 62.53 , 97.99 2021-02-13T01:08:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , -0.03 2021-02-13T01:08:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178513.14 epoch sec; seqNo:18901; 4060934.81 north; 593745.61 east 2021-02-13T01:08:33Z- AHRS_M2 msg: 1613178513.52 epoch sec; seqNo:18753 2021-02-13T01:08:33Z- AHRS_M2 msg: 0.61 phi; -0.35 theta; -0.17 psi 2021-02-13T01:08:33Z- handleDepth()-Depth_Keller msg: 1613178513522 epoch sec; seqNo:18902; depth 27.61 2021-02-13T01:08:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178513.53 epoch sec; seqNo:18902; 4060935.19 north; 593745.61 east 2021-02-13T01:08:33Z- AHRS_M2 msg: 1613178513.93 epoch sec; seqNo:18754 2021-02-13T01:08:33Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.17 psi 2021-02-13T01:08:33Z- handleDepth()-Depth_Keller msg: 1613178513952 epoch sec; seqNo:18903; depth 27.74 2021-02-13T01:08:33Z- handleDvl() - RDI_Pathfinder msg: 1613178513.96 epoch sec; seqNo:9127 2021-02-13T01:08:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.85 , 83.92 , 61.31 , 97.99 2021-02-13T01:08:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.06 2021-02-13T01:08:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178513.97 epoch sec; seqNo:18903; 4060935.57 north; 593745.60 east 2021-02-13T01:08:34Z- AHRS_M2 msg: 1613178514.32 epoch sec; seqNo:18755 2021-02-13T01:08:34Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.17 psi 2021-02-13T01:08:34Z- handleDepth()-Depth_Keller msg: 1613178514328 epoch sec; seqNo:18904; depth 27.88 2021-02-13T01:08:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178514.34 epoch sec; seqNo:18904; 4060935.58 north; 593746.82 east 2021-02-13T01:08:34Z- AHRS_M2 msg: 1613178514.73 epoch sec; seqNo:18756 2021-02-13T01:08:34Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.17 psi 2021-02-13T01:08:34Z- handleDepth()-Depth_Keller msg: 1613178514741 epoch sec; seqNo:18905; depth 27.96 2021-02-13T01:08:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178514.75 epoch sec; seqNo:18905; 4060935.96 north; 593746.82 east 2021-02-13T01:08:35Z- AHRS_M2 msg: 1613178515.13 epoch sec; seqNo:18757 2021-02-13T01:08:35Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.18 psi 2021-02-13T01:08:35Z- handleDepth()-Depth_Keller msg: 1613178515140 epoch sec; seqNo:18906; depth 28.11 2021-02-13T01:08:35Z- handleDvl() - RDI_Pathfinder msg: 1613178515.16 epoch sec; seqNo:9128 2021-02-13T01:08:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.85 , 83.92 , 60.90 , 97.99 2021-02-13T01:08:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , -0.06 2021-02-13T01:08:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178515.17 epoch sec; seqNo:18906; 4060936.34 north; 593746.81 east 2021-02-13T01:08:35Z- AHRS_M2 msg: 1613178515.54 epoch sec; seqNo:18758 2021-02-13T01:08:35Z- AHRS_M2 msg: 0.61 phi; -0.34 theta; -0.18 psi 2021-02-13T01:08:35Z- handleDepth()-Depth_Keller msg: 1613178515540 epoch sec; seqNo:18907; depth 28.24 2021-02-13T01:08:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178515.55 epoch sec; seqNo:18907; 4060936.72 north; 593746.81 east 2021-02-13T01:08:35Z- AHRS_M2 msg: 1613178515.94 epoch sec; seqNo:18759 2021-02-13T01:08:35Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T01:08:35Z- handleDepth()-Depth_Keller msg: 1613178515944 epoch sec; seqNo:18908; depth 28.35 2021-02-13T01:08:35Z- handleDvl() - RDI_Pathfinder msg: 1613178515.96 epoch sec; seqNo:9129 2021-02-13T01:08:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.03 , 82.84 , 61.31 , 97.99 2021-02-13T01:08:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.10 , -0.02 2021-02-13T01:08:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178515.97 epoch sec; seqNo:18908; 4060937.10 north; 593746.81 east 2021-02-13T01:08:36Z- AHRS_M2 msg: 1613178516.35 epoch sec; seqNo:18760 2021-02-13T01:08:36Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T01:08:36Z- handleDepth()-Depth_Keller msg: 1613178516349 epoch sec; seqNo:18909; depth 28.48 2021-02-13T01:08:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178516.36 epoch sec; seqNo:18909; 4060937.10 north; 593746.81 east 2021-02-13T01:08:36Z- AHRS_M2 msg: 1613178516.75 epoch sec; seqNo:18761 2021-02-13T01:08:36Z- AHRS_M2 msg: 0.60 phi; -0.34 theta; -0.18 psi 2021-02-13T01:08:36Z- handleDepth()-Depth_Keller msg: 1613178516763 epoch sec; seqNo:18910; depth 28.60 2021-02-13T01:08:36Z- handleDvl() - RDI_Pathfinder msg: 1613178516.77 epoch sec; seqNo:9130 2021-02-13T01:08:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.63 , 0.00 , 60.91 , 97.99 2021-02-13T01:08:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.12 , 0.01 2021-02-13T01:08:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178516.79 epoch sec; seqNo:18910; 4060937.48 north; 593746.80 east 2021-02-13T01:08:37Z- AHRS_M2 msg: 1613178517.15 epoch sec; seqNo:18762 2021-02-13T01:08:37Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-13T01:08:37Z- handleDepth()-Depth_Keller msg: 1613178517159 epoch sec; seqNo:18911; depth 28.72 2021-02-13T01:08:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178517.17 epoch sec; seqNo:18911; 4060937.85 north; 593746.80 east 2021-02-13T01:08:37Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-13T01:08:37Z- waiting for 1613178514.93, time is 1613178517.17 2021-02-13T01:08:37Z- LcmTrn::updateTrn() >>>> pose time 1613178517.17, meas time 1613178516.77 <<<< 2021-02-13T01:08:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178516.77 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:08:37Z- TerrainNav::motionUpdate() - currEstimate = (4060938, 593746.80, 28.72) 2021-02-13T01:08:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:08:37Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:08:37Z- TNavBF::Projecting Measurements 2021-02-13T01:08:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:08:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:08:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:08:37Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:08:37Z- TNavFilter::3 good beams. 2021-02-13T01:08:37Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:08:37Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:08:37Z- TNavBF:: beamIndex[1] = 2 2021-02-13T01:08:37Z- TNavBF:: beamIndex[2] = 3 2021-02-13T01:08:37Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:08:37Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:08:37Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:08:37Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:37Z- TerrainMapOctree: (vehN, vehE ) = (4060937.57, 593746.71). 2021-02-13T01:08:37Z- TerrainMapOctree: Min Distance = 592.38. 2021-02-13T01:08:37Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:08:37Z- There is no specific good beam that all particles have in common. 2021-02-13T01:08:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:08:37Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.106 ExpectedMeasDiff: nan currMeas.covariance: 7.10 2021-02-13T01:08:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:08:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.109 ExpectedMeasDiff: nan currMeas.covariance: 5.94 2021-02-13T01:08:37Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:08:37Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 15.36 2021-02-13T01:08:37Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:08:37Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:08:37Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:08:37Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:37Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:08:37Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:08:37Z- Filter 0 No beams used in subcloud update 2021-02-13T01:08:37Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:08:37Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:08:37Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 674.5 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T01:08:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178516.77 sec. 2021-02-13T01:08:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:08:37Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:37Z- variances([0], [2], and psi_berg) = 674.533037 832.986225 0.000000 2021-02-13T01:08:37Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:08:37Z- LcmTrn::publishEstimates() - MLE : 4060937.35 593738.30 28.72 2021-02-13T01:08:37Z- LcmTrn::publishEstimates() - MMSE : 4060937.85 593746.80 28.72 2021-02-13T01:08:37Z- LcmTrn::publishEstimates() - COVAR: 674.53 832.99 0.81 0.00 2021-02-13T01:08:37Z- AHRS_M2 msg: 1613178517.56 epoch sec; seqNo:18763 2021-02-13T01:08:37Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-13T01:08:37Z- handleDepth()-Depth_Keller msg: 1613178517560 epoch sec; seqNo:18912; depth 28.82 2021-02-13T01:08:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178517.57 epoch sec; seqNo:18912; 4060938.25 north; 593748.01 east 2021-02-13T01:08:37Z- AHRS_M2 msg: 1613178517.96 epoch sec; seqNo:18764 2021-02-13T01:08:37Z- AHRS_M2 msg: 0.59 phi; -0.33 theta; -0.19 psi 2021-02-13T01:08:37Z- handleDepth()-Depth_Keller msg: 1613178517971 epoch sec; seqNo:18913; depth 28.95 2021-02-13T01:08:37Z- handleDvl() - RDI_Pathfinder msg: 1613178517.98 epoch sec; seqNo:9131 2021-02-13T01:08:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.81 , 83.93 , 60.50 , 97.99 2021-02-13T01:08:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.08 , 0.04 2021-02-13T01:08:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178517.99 epoch sec; seqNo:18913; 4060938.63 north; 593748.01 east 2021-02-13T01:08:38Z- AHRS_M2 msg: 1613178518.36 epoch sec; seqNo:18765 2021-02-13T01:08:38Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.19 psi 2021-02-13T01:08:38Z- handleDepth()-Depth_Keller msg: 1613178518368 epoch sec; seqNo:18914; depth 29.09 2021-02-13T01:08:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178518.39 epoch sec; seqNo:18914; 4060938.63 north; 593748.01 east 2021-02-13T01:08:38Z- AHRS_M2 msg: 1613178518.77 epoch sec; seqNo:18766 2021-02-13T01:08:38Z- AHRS_M2 msg: 0.58 phi; -0.33 theta; -0.19 psi 2021-02-13T01:08:38Z- handleDepth()-Depth_Keller msg: 1613178518779 epoch sec; seqNo:18915; depth 29.20 2021-02-13T01:08:38Z- handleDvl() - RDI_Pathfinder msg: 1613178518.85 epoch sec; seqNo:9132 2021-02-13T01:08:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.81 , 83.93 , 60.09 , 92.58 2021-02-13T01:08:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , 0.04 2021-02-13T01:08:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178518.86 epoch sec; seqNo:18915; 4060939.00 north; 593748.01 east 2021-02-13T01:08:39Z- AHRS_M2 msg: 1613178519.17 epoch sec; seqNo:18767 2021-02-13T01:08:39Z- AHRS_M2 msg: 0.58 phi; -0.32 theta; -0.19 psi 2021-02-13T01:08:39Z- handleDepth()-Depth_Keller msg: 1613178519176 epoch sec; seqNo:18916; depth 29.36 2021-02-13T01:08:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178519.19 epoch sec; seqNo:18916; 4060939.38 north; 593748.00 east 2021-02-13T01:08:39Z- AHRS_M2 msg: 1613178519.59 epoch sec; seqNo:18768 2021-02-13T01:08:39Z- AHRS_M2 msg: 0.58 phi; -0.31 theta; -0.19 psi 2021-02-13T01:08:39Z- handleDepth()-Depth_Keller msg: 1613178519589 epoch sec; seqNo:18917; depth 29.45 2021-02-13T01:08:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178519.60 epoch sec; seqNo:18917; 4060939.76 north; 593748.00 east 2021-02-13T01:08:39Z- AHRS_M2 msg: 1613178519.98 epoch sec; seqNo:18769 2021-02-13T01:08:39Z- AHRS_M2 msg: 0.57 phi; -0.28 theta; -0.19 psi 2021-02-13T01:08:39Z- handleDepth()-Depth_Keller msg: 1613178519984 epoch sec; seqNo:18918; depth 29.58 2021-02-13T01:08:40Z- handleDvl() - RDI_Pathfinder msg: 1613178520.01 epoch sec; seqNo:9133 2021-02-13T01:08:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:08:40Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:08:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178520.03 epoch sec; seqNo:18918; 4060940.14 north; 593747.99 east 2021-02-13T01:08:40Z- AHRS_M2 msg: 1613178520.38 epoch sec; seqNo:18770 2021-02-13T01:08:40Z- AHRS_M2 msg: 0.56 phi; -0.25 theta; -0.18 psi 2021-02-13T01:08:40Z- handleDepth()-Depth_Keller msg: 1613178520388 epoch sec; seqNo:18919; depth 29.74 2021-02-13T01:08:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178520.40 epoch sec; seqNo:18919; 4060940.15 north; 593749.21 east 2021-02-13T01:08:40Z- AHRS_M2 msg: 1613178520.80 epoch sec; seqNo:18771 2021-02-13T01:08:40Z- AHRS_M2 msg: 0.56 phi; -0.21 theta; -0.17 psi 2021-02-13T01:08:40Z- handleDepth()-Depth_Keller msg: 1613178520815 epoch sec; seqNo:18920; depth 29.86 2021-02-13T01:08:40Z- handleDvl() - RDI_Pathfinder msg: 1613178520.84 epoch sec; seqNo:9134 2021-02-13T01:08:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 116.18 , 79.39 , 61.31 , 0.00 2021-02-13T01:08:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.03 , 0.07 2021-02-13T01:08:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178520.85 epoch sec; seqNo:18920; 4060940.53 north; 593749.21 east 2021-02-13T01:08:41Z- AHRS_M2 msg: 1613178521.19 epoch sec; seqNo:18772 2021-02-13T01:08:41Z- AHRS_M2 msg: 0.55 phi; -0.17 theta; -0.16 psi 2021-02-13T01:08:41Z- handleDepth()-Depth_Keller msg: 1613178521197 epoch sec; seqNo:18921; depth 29.99 2021-02-13T01:08:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178521.21 epoch sec; seqNo:18921; 4060940.91 north; 593749.21 east 2021-02-13T01:08:41Z- AHRS_M2 msg: 1613178521.60 epoch sec; seqNo:18773 2021-02-13T01:08:41Z- AHRS_M2 msg: 0.53 phi; -0.08 theta; -0.14 psi 2021-02-13T01:08:41Z- handleDepth()-Depth_Keller msg: 1613178521611 epoch sec; seqNo:18922; depth 30.10 2021-02-13T01:08:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178521.62 epoch sec; seqNo:18922; 4060941.29 north; 593749.20 east 2021-02-13T01:08:42Z- AHRS_M2 msg: 1613178522.00 epoch sec; seqNo:18774 2021-02-13T01:08:42Z- AHRS_M2 msg: 0.53 phi; -0.04 theta; -0.14 psi 2021-02-13T01:08:42Z- handleDepth()-Depth_Keller msg: 1613178522013 epoch sec; seqNo:18923; depth 30.19 2021-02-13T01:08:42Z- handleDvl() - RDI_Pathfinder msg: 1613178522.04 epoch sec; seqNo:9135 2021-02-13T01:08:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.95 , 78.65 , 63.36 , 78.65 2021-02-13T01:08:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.02 2021-02-13T01:08:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178522.05 epoch sec; seqNo:18923; 4060941.67 north; 593749.20 east 2021-02-13T01:08:42Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-13T01:08:42Z- waiting for 1613178520.17, time is 1613178522.05 2021-02-13T01:08:42Z- LcmTrn::updateTrn() equating pose 1613178522.05 and meas times 1613178522.05 2021-02-13T01:08:42Z- LcmTrn::updateTrn() >>>> pose time 1613178522.05, meas time 1613178522.05 <<<< 2021-02-13T01:08:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178522.05 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:08:42Z- TerrainNav::motionUpdate() - currEstimate = (4060942, 593749.20, 30.19) 2021-02-13T01:08:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:08:42Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:08:42Z- TNavBF::Projecting Measurements 2021-02-13T01:08:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:08:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:08:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:08:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:08:42Z- TNavFilter::4 good beams. 2021-02-13T01:08:42Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:08:42Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:08:42Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:08:42Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:08:42Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:08:42Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:08:42Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:08:42Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:42Z- TerrainMapOctree: (vehN, vehE ) = (4060941.67, 593749.20). 2021-02-13T01:08:42Z- TerrainMapOctree: Min Distance = 588.11. 2021-02-13T01:08:42Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:08:42Z- There is no specific good beam that all particles have in common. 2021-02-13T01:08:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:08:42Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.114 ExpectedMeasDiff: nan currMeas.covariance: 6.34 2021-02-13T01:08:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:08:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.073 ExpectedMeasDiff: nan currMeas.covariance: 9.90 2021-02-13T01:08:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:08:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.85 K: 0.095 ExpectedMeasDiff: nan currMeas.covariance: 6.42 2021-02-13T01:08:42Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:08:42Z- Old Depth Bias: 0.00 DepthBiasCov: 0.77 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 9.90 2021-02-13T01:08:42Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:08:42Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:08:42Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:08:42Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:08:42Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:42Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:08:42Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:08:42Z- Filter 0 No beams used in subcloud update 2021-02-13T01:08:42Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:08:42Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:08:42Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 674.5 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T01:08:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178522.05 sec. 2021-02-13T01:08:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:08:42Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:42Z- variances([0], [2], and psi_berg) = 674.488994 832.986207 0.000000 2021-02-13T01:08:42Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:08:42Z- LcmTrn::publishEstimates() - MLE : 4060941.17 593740.70 30.19 2021-02-13T01:08:42Z- LcmTrn::publishEstimates() - MMSE : 4060941.67 593749.20 30.19 2021-02-13T01:08:42Z- LcmTrn::publishEstimates() - COVAR: 674.49 832.99 0.72 0.00 2021-02-13T01:08:42Z- AHRS_M2 msg: 1613178522.41 epoch sec; seqNo:18775 2021-02-13T01:08:42Z- AHRS_M2 msg: 0.52 phi; 0.00 theta; -0.14 psi 2021-02-13T01:08:42Z- handleDepth()-Depth_Keller msg: 1613178522422 epoch sec; seqNo:18924; depth 30.28 2021-02-13T01:08:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178522.45 epoch sec; seqNo:18924; 4060941.67 north; 593749.20 east 2021-02-13T01:08:42Z- AHRS_M2 msg: 1613178522.81 epoch sec; seqNo:18776 2021-02-13T01:08:42Z- AHRS_M2 msg: 0.51 phi; 0.05 theta; -0.13 psi 2021-02-13T01:08:42Z- handleDepth()-Depth_Keller msg: 1613178522812 epoch sec; seqNo:18925; depth 30.34 2021-02-13T01:08:42Z- handleDvl() - RDI_Pathfinder msg: 1613178522.83 epoch sec; seqNo:9136 2021-02-13T01:08:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.04 , 0.00 , 66.79 , 67.82 2021-02-13T01:08:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.02 , 0.02 2021-02-13T01:08:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178522.84 epoch sec; seqNo:18925; 4060942.05 north; 593749.19 east 2021-02-13T01:08:43Z- AHRS_M2 msg: 1613178523.21 epoch sec; seqNo:18777 2021-02-13T01:08:43Z- AHRS_M2 msg: 0.51 phi; 0.09 theta; -0.13 psi 2021-02-13T01:08:43Z- handleDepth()-Depth_Keller msg: 1613178523217 epoch sec; seqNo:18926; depth 30.40 2021-02-13T01:08:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178523.23 epoch sec; seqNo:18926; 4060942.44 north; 593750.41 east 2021-02-13T01:08:43Z- AHRS_M2 msg: 1613178523.62 epoch sec; seqNo:18778 2021-02-13T01:08:43Z- AHRS_M2 msg: 0.50 phi; 0.12 theta; -0.12 psi 2021-02-13T01:08:43Z- handleDepth()-Depth_Keller msg: 1613178523620 epoch sec; seqNo:18927; depth 30.44 2021-02-13T01:08:43Z- handleDvl() - RDI_Pathfinder msg: 1613178523.63 epoch sec; seqNo:9137 2021-02-13T01:08:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.79 , 0.00 , 71.16 , 64.71 2021-02-13T01:08:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.00 , 0.02 2021-02-13T01:08:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178523.64 epoch sec; seqNo:18927; 4060942.82 north; 593750.41 east 2021-02-13T01:08:44Z- AHRS_M2 msg: 1613178524.02 epoch sec; seqNo:18779 2021-02-13T01:08:44Z- AHRS_M2 msg: 0.50 phi; 0.16 theta; -0.11 psi 2021-02-13T01:08:44Z- handleDepth()-Depth_Keller msg: 1613178524029 epoch sec; seqNo:18928; depth 30.47 2021-02-13T01:08:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178524.04 epoch sec; seqNo:18928; 4060942.82 north; 593750.41 east 2021-02-13T01:08:44Z- AHRS_M2 msg: 1613178524.43 epoch sec; seqNo:18780 2021-02-13T01:08:44Z- AHRS_M2 msg: 0.50 phi; 0.19 theta; -0.11 psi 2021-02-13T01:08:44Z- handleDepth()-Depth_Keller msg: 1613178524439 epoch sec; seqNo:18929; depth 30.47 2021-02-13T01:08:44Z- handleDvl() - RDI_Pathfinder msg: 1613178524.45 epoch sec; seqNo:9138 2021-02-13T01:08:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.71 , 0.00 , 76.06 , 62.26 2021-02-13T01:08:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.03 , -0.01 2021-02-13T01:08:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178524.46 epoch sec; seqNo:18929; 4060943.20 north; 593750.40 east 2021-02-13T01:08:44Z- AHRS_M2 msg: 1613178524.83 epoch sec; seqNo:18781 2021-02-13T01:08:44Z- AHRS_M2 msg: 0.50 phi; 0.21 theta; -0.11 psi 2021-02-13T01:08:44Z- handleDepth()-Depth_Keller msg: 1613178524832 epoch sec; seqNo:18930; depth 30.46 2021-02-13T01:08:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178524.84 epoch sec; seqNo:18930; 4060943.58 north; 593750.40 east 2021-02-13T01:08:45Z- AHRS_M2 msg: 1613178525.23 epoch sec; seqNo:18782 2021-02-13T01:08:45Z- AHRS_M2 msg: 0.51 phi; 0.24 theta; -0.11 psi 2021-02-13T01:08:45Z- handleDepth()-Depth_Keller msg: 1613178525245 epoch sec; seqNo:18931; depth 30.44 2021-02-13T01:08:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178525.25 epoch sec; seqNo:18931; 4060943.96 north; 593750.39 east 2021-02-13T01:08:45Z- AHRS_M2 msg: 1613178525.64 epoch sec; seqNo:18783 2021-02-13T01:08:45Z- AHRS_M2 msg: 0.51 phi; 0.26 theta; -0.10 psi 2021-02-13T01:08:45Z- handleDepth()-Depth_Keller msg: 1613178525642 epoch sec; seqNo:18932; depth 30.41 2021-02-13T01:08:45Z- handleDvl() - RDI_Pathfinder msg: 1613178525.65 epoch sec; seqNo:9139 2021-02-13T01:08:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 66.04 , 0.00 , 0.00 , 60.53 2021-02-13T01:08:45Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:08:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178525.66 epoch sec; seqNo:18932; 4060944.33 north; 593750.39 east 2021-02-13T01:08:46Z- AHRS_M2 msg: 1613178526.04 epoch sec; seqNo:18784 2021-02-13T01:08:46Z- AHRS_M2 msg: 0.51 phi; 0.27 theta; -0.10 psi 2021-02-13T01:08:46Z- handleDepth()-Depth_Keller msg: 1613178526044 epoch sec; seqNo:18933; depth 30.37 2021-02-13T01:08:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178526.06 epoch sec; seqNo:18933; 4060944.35 north; 593751.61 east 2021-02-13T01:08:46Z- AHRS_M2 msg: 1613178526.46 epoch sec; seqNo:18785 2021-02-13T01:08:46Z- AHRS_M2 msg: 0.52 phi; 0.29 theta; -0.10 psi 2021-02-13T01:08:46Z- handleDepth()-Depth_Keller msg: 1613178526469 epoch sec; seqNo:18934; depth 30.33 2021-02-13T01:08:46Z- handleDvl() - RDI_Pathfinder msg: 1613178526.49 epoch sec; seqNo:9140 2021-02-13T01:08:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 67.31 , 0.00 , 0.00 , 60.92 2021-02-13T01:08:46Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:08:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178526.51 epoch sec; seqNo:18934; 4060944.73 north; 593751.60 east 2021-02-13T01:08:46Z- AHRS_M2 msg: 1613178526.85 epoch sec; seqNo:18786 2021-02-13T01:08:46Z- AHRS_M2 msg: 0.53 phi; 0.30 theta; -0.09 psi 2021-02-13T01:08:46Z- handleDepth()-Depth_Keller msg: 1613178526861 epoch sec; seqNo:18935; depth 30.26 2021-02-13T01:08:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178526.87 epoch sec; seqNo:18935; 4060945.11 north; 593751.60 east 2021-02-13T01:08:47Z- AHRS_M2 msg: 1613178527.25 epoch sec; seqNo:18787 2021-02-13T01:08:47Z- AHRS_M2 msg: 0.53 phi; 0.31 theta; -0.09 psi 2021-02-13T01:08:47Z- handleDepth()-Depth_Keller msg: 1613178527256 epoch sec; seqNo:18936; depth 30.18 2021-02-13T01:08:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178527.26 epoch sec; seqNo:18936; 4060945.11 north; 593751.60 east 2021-02-13T01:08:47Z- LcmTrn::handleMessages() - handling 44 messages took 0.63 ms and 132 calls... 2021-02-13T01:08:47Z- waiting for 1613178525.05, time is 1613178527.26 2021-02-13T01:08:47Z- LcmTrn::updateTrn() >>>> pose time 1613178527.26, meas time 1613178526.49 <<<< 2021-02-13T01:08:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178526.49 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:08:47Z- TerrainNav::motionUpdate() - currEstimate = (4060945, 593751.60, 30.18) 2021-02-13T01:08:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:08:47Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:08:47Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:08:47Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:08:47Z- TNavBF::Projecting Measurements 2021-02-13T01:08:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:08:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:08:47Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-13T01:08:47Z- TNavFilter::2 good beams. 2021-02-13T01:08:47Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:08:47Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:08:47Z- TNavBF:: beamIndex[1] = 3 2021-02-13T01:08:47Z- TNavBF:: beamIndex[2] = 0 2021-02-13T01:08:47Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:08:47Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-13T01:08:47Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:08:47Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:47Z- TerrainMapOctree: (vehN, vehE ) = (4060944.60, 593751.25). 2021-02-13T01:08:47Z- TerrainMapOctree: Min Distance = 584.83. 2021-02-13T01:08:47Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:08:47Z- There is no specific good beam that all particles have in common. 2021-02-13T01:08:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:08:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 7.25 2021-02-13T01:08:47Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:08:47Z- Old Depth Bias: 0.00 DepthBiasCov: 0.82 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 5.94 2021-02-13T01:08:47Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:08:47Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:08:47Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:47Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:08:47Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:08:47Z- Filter 0 No beams used in subcloud update 2021-02-13T01:08:47Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:08:47Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:08:47Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 674.4 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T01:08:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178526.49 sec. 2021-02-13T01:08:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:08:47Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:47Z- variances([0], [2], and psi_berg) = 674.433483 832.986185 0.000000 2021-02-13T01:08:47Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:08:47Z- LcmTrn::publishEstimates() - MLE : 4060944.61 593743.10 30.18 2021-02-13T01:08:47Z- LcmTrn::publishEstimates() - MMSE : 4060945.11 593751.60 30.18 2021-02-13T01:08:47Z- LcmTrn::publishEstimates() - COVAR: 674.43 832.99 0.78 0.00 2021-02-13T01:08:47Z- AHRS_M2 msg: 1613178527.65 epoch sec; seqNo:18788 2021-02-13T01:08:47Z- AHRS_M2 msg: 0.54 phi; 0.32 theta; -0.09 psi 2021-02-13T01:08:47Z- handleDepth()-Depth_Keller msg: 1613178527660 epoch sec; seqNo:18937; depth 30.12 2021-02-13T01:08:47Z- handleDvl() - RDI_Pathfinder msg: 1613178527.67 epoch sec; seqNo:9141 2021-02-13T01:08:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 68.58 , 0.00 , 0.00 , 60.06 2021-02-13T01:08:47Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:08:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178527.68 epoch sec; seqNo:18937; 4060945.48 north; 593751.60 east 2021-02-13T01:08:48Z- AHRS_M2 msg: 1613178528.06 epoch sec; seqNo:18789 2021-02-13T01:08:48Z- AHRS_M2 msg: 0.55 phi; 0.33 theta; -0.09 psi 2021-02-13T01:08:48Z- handleDepth()-Depth_Keller msg: 1613178528065 epoch sec; seqNo:18938; depth 30.04 2021-02-13T01:08:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178528.08 epoch sec; seqNo:18938; 4060945.86 north; 593751.59 east 2021-02-13T01:08:48Z- AHRS_M2 msg: 1613178528.48 epoch sec; seqNo:18790 2021-02-13T01:08:48Z- AHRS_M2 msg: 0.55 phi; 0.34 theta; -0.09 psi 2021-02-13T01:08:48Z- handleDepth()-Depth_Keller msg: 1613178528489 epoch sec; seqNo:18939; depth 29.94 2021-02-13T01:08:48Z- handleDvl() - RDI_Pathfinder msg: 1613178528.50 epoch sec; seqNo:9142 2021-02-13T01:08:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 69.43 , 0.00 , 0.00 , 60.49 2021-02-13T01:08:48Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:08:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178528.51 epoch sec; seqNo:18939; 4060946.24 north; 593751.59 east 2021-02-13T01:08:48Z- AHRS_M2 msg: 1613178528.87 epoch sec; seqNo:18791 2021-02-13T01:08:48Z- AHRS_M2 msg: 0.56 phi; 0.34 theta; -0.09 psi 2021-02-13T01:08:48Z- handleDepth()-Depth_Keller msg: 1613178528872 epoch sec; seqNo:18940; depth 29.84 2021-02-13T01:08:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178528.88 epoch sec; seqNo:18940; 4060946.26 north; 593752.81 east 2021-02-13T01:08:49Z- AHRS_M2 msg: 1613178529.27 epoch sec; seqNo:18792 2021-02-13T01:08:49Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.09 psi 2021-02-13T01:08:49Z- handleDepth()-Depth_Keller msg: 1613178529276 epoch sec; seqNo:18941; depth 29.76 2021-02-13T01:08:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178529.28 epoch sec; seqNo:18941; 4060946.63 north; 593752.80 east 2021-02-13T01:08:49Z- AHRS_M2 msg: 1613178529.68 epoch sec; seqNo:18793 2021-02-13T01:08:49Z- AHRS_M2 msg: 0.56 phi; 0.35 theta; -0.09 psi 2021-02-13T01:08:49Z- handleDepth()-Depth_Keller msg: 1613178529681 epoch sec; seqNo:18942; depth 29.67 2021-02-13T01:08:49Z- handleDvl() - RDI_Pathfinder msg: 1613178529.69 epoch sec; seqNo:9143 2021-02-13T01:08:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.43 , 76.25 , 0.00 , 60.06 2021-02-13T01:08:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.14 , -0.09 2021-02-13T01:08:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178529.70 epoch sec; seqNo:18942; 4060947.01 north; 593752.80 east 2021-02-13T01:08:50Z- AHRS_M2 msg: 1613178530.08 epoch sec; seqNo:18794 2021-02-13T01:08:50Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.09 psi 2021-02-13T01:08:50Z- handleDepth()-Depth_Keller msg: 1613178530084 epoch sec; seqNo:18943; depth 29.56 2021-02-13T01:08:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178530.10 epoch sec; seqNo:18943; 4060947.39 north; 593752.80 east 2021-02-13T01:08:50Z- AHRS_M2 msg: 1613178530.48 epoch sec; seqNo:18795 2021-02-13T01:08:50Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.09 psi 2021-02-13T01:08:50Z- handleDepth()-Depth_Keller msg: 1613178530488 epoch sec; seqNo:18944; depth 29.44 2021-02-13T01:08:50Z- handleDvl() - RDI_Pathfinder msg: 1613178530.50 epoch sec; seqNo:9144 2021-02-13T01:08:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 69.93 , 0.00 , 0.00 , 60.54 2021-02-13T01:08:50Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:08:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178530.53 epoch sec; seqNo:18944; 4060947.39 north; 593752.80 east 2021-02-13T01:08:50Z- AHRS_M2 msg: 1613178530.89 epoch sec; seqNo:18796 2021-02-13T01:08:50Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.10 psi 2021-02-13T01:08:50Z- handleDepth()-Depth_Keller msg: 1613178530897 epoch sec; seqNo:18945; depth 29.34 2021-02-13T01:08:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178530.91 epoch sec; seqNo:18945; 4060947.77 north; 593752.79 east 2021-02-13T01:08:51Z- AHRS_M2 msg: 1613178531.29 epoch sec; seqNo:18797 2021-02-13T01:08:51Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.10 psi 2021-02-13T01:08:51Z- handleDepth()-Depth_Keller msg: 1613178531297 epoch sec; seqNo:18946; depth 29.23 2021-02-13T01:08:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178531.31 epoch sec; seqNo:18946; 4060948.15 north; 593752.79 east 2021-02-13T01:08:51Z- AHRS_M2 msg: 1613178531.70 epoch sec; seqNo:18798 2021-02-13T01:08:51Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.10 psi 2021-02-13T01:08:51Z- handleDepth()-Depth_Keller msg: 1613178531705 epoch sec; seqNo:18947; depth 29.13 2021-02-13T01:08:51Z- handleDvl() - RDI_Pathfinder msg: 1613178531.72 epoch sec; seqNo:9145 2021-02-13T01:08:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.86 , 76.68 , 0.00 , 60.06 2021-02-13T01:08:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.13 , 0.04 2021-02-13T01:08:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178531.73 epoch sec; seqNo:18947; 4060948.53 north; 593752.78 east 2021-02-13T01:08:52Z- AHRS_M2 msg: 1613178532.10 epoch sec; seqNo:18799 2021-02-13T01:08:52Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.10 psi 2021-02-13T01:08:52Z- handleDepth()-Depth_Keller msg: 1613178532109 epoch sec; seqNo:18948; depth 29.02 2021-02-13T01:08:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178532.12 epoch sec; seqNo:18948; 4060948.53 north; 593752.78 east 2021-02-13T01:08:52Z- AHRS_M2 msg: 1613178532.50 epoch sec; seqNo:18800 2021-02-13T01:08:52Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.10 psi 2021-02-13T01:08:52Z- handleDepth()-Depth_Keller msg: 1613178532508 epoch sec; seqNo:18949; depth 28.90 2021-02-13T01:08:52Z- handleDvl() - RDI_Pathfinder msg: 1613178532.52 epoch sec; seqNo:9146 2021-02-13T01:08:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 70.58 , 0.00 , 0.00 , 60.54 2021-02-13T01:08:52Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:08:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178532.53 epoch sec; seqNo:18949; 4060948.91 north; 593752.78 east 2021-02-13T01:08:52Z- LcmTrn::handleMessages() - handling 45 messages took 0.66 ms and 132 calls... 2021-02-13T01:08:52Z- waiting for 1613178530.26, time is 1613178532.53 2021-02-13T01:08:52Z- LcmTrn::updateTrn() equating pose 1613178532.53 and meas times 1613178532.53 2021-02-13T01:08:52Z- LcmTrn::updateTrn() >>>> pose time 1613178532.53, meas time 1613178532.53 <<<< 2021-02-13T01:08:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178532.53 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:08:52Z- TerrainNav::motionUpdate() - currEstimate = (4060949, 593752.78, 28.90) 2021-02-13T01:08:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:08:52Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:08:52Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:08:52Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:08:52Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178532.53 sec.; No relevant navigation data available 2021-02-13T01:08:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:08:52Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:52Z- variances([0], [2], and psi_berg) = 674.392135 832.986168 0.000000 2021-02-13T01:08:52Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:08:52Z- LcmTrn::publishEstimates() - MLE : 4060948.41 593744.28 28.90 2021-02-13T01:08:52Z- LcmTrn::publishEstimates() - MMSE : 4060948.91 593752.78 28.90 2021-02-13T01:08:52Z- LcmTrn::publishEstimates() - COVAR: 674.39 832.99 0.98 0.00 2021-02-13T01:08:52Z- AHRS_M2 msg: 1613178532.91 epoch sec; seqNo:18801 2021-02-13T01:08:52Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.10 psi 2021-02-13T01:08:52Z- handleDepth()-Depth_Keller msg: 1613178532918 epoch sec; seqNo:18950; depth 28.79 2021-02-13T01:08:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178532.93 epoch sec; seqNo:18950; 4060949.30 north; 593754.00 east 2021-02-13T01:08:53Z- AHRS_M2 msg: 1613178533.31 epoch sec; seqNo:18802 2021-02-13T01:08:53Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.10 psi 2021-02-13T01:08:53Z- handleDepth()-Depth_Keller msg: 1613178533321 epoch sec; seqNo:18951; depth 28.66 2021-02-13T01:08:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178533.33 epoch sec; seqNo:18951; 4060949.68 north; 593753.99 east 2021-02-13T01:08:53Z- AHRS_M2 msg: 1613178533.71 epoch sec; seqNo:18803 2021-02-13T01:08:53Z- AHRS_M2 msg: 0.58 phi; 0.36 theta; -0.10 psi 2021-02-13T01:08:53Z- handleDepth()-Depth_Keller msg: 1613178533720 epoch sec; seqNo:18952; depth 28.56 2021-02-13T01:08:53Z- handleDvl() - RDI_Pathfinder msg: 1613178533.73 epoch sec; seqNo:9147 2021-02-13T01:08:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 70.71 , 0.00 , 0.00 , 60.92 2021-02-13T01:08:53Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:08:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178533.74 epoch sec; seqNo:18952; 4060949.68 north; 593753.99 east 2021-02-13T01:08:54Z- AHRS_M2 msg: 1613178534.12 epoch sec; seqNo:18804 2021-02-13T01:08:54Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.10 psi 2021-02-13T01:08:54Z- handleDepth()-Depth_Keller msg: 1613178534135 epoch sec; seqNo:18953; depth 28.44 2021-02-13T01:08:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178534.16 epoch sec; seqNo:18953; 4060950.06 north; 593753.99 east 2021-02-13T01:08:54Z- handleDepth()-Depth_Keller msg: 1613178534553 epoch sec; seqNo:18954; depth 28.32 2021-02-13T01:08:54Z- handleDvl() - RDI_Pathfinder msg: 1613178534.56 epoch sec; seqNo:9148 2021-02-13T01:08:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.14 , 77.53 , 0.00 , 60.92 2021-02-13T01:08:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.03 , 0.09 2021-02-13T01:08:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178534.58 epoch sec; seqNo:18954; 4060950.44 north; 593753.98 east 2021-02-13T01:08:54Z- AHRS_M2 msg: 1613178534.93 epoch sec; seqNo:18805 2021-02-13T01:08:54Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.09 psi 2021-02-13T01:08:54Z- handleDepth()-Depth_Keller msg: 1613178534933 epoch sec; seqNo:18955; depth 28.20 2021-02-13T01:08:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178534.94 epoch sec; seqNo:18955; 4060950.44 north; 593753.98 east 2021-02-13T01:08:55Z- AHRS_M2 msg: 1613178535.33 epoch sec; seqNo:18806 2021-02-13T01:08:55Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.09 psi 2021-02-13T01:08:55Z- handleDepth()-Depth_Keller msg: 1613178535337 epoch sec; seqNo:18956; depth 28.09 2021-02-13T01:08:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178535.35 epoch sec; seqNo:18956; 4060950.82 north; 593753.98 east 2021-02-13T01:08:55Z- AHRS_M2 msg: 1613178535.73 epoch sec; seqNo:18807 2021-02-13T01:08:55Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.09 psi 2021-02-13T01:08:55Z- handleDepth()-Depth_Keller msg: 1613178535750 epoch sec; seqNo:18957; depth 27.97 2021-02-13T01:08:55Z- handleDvl() - RDI_Pathfinder msg: 1613178535.76 epoch sec; seqNo:9149 2021-02-13T01:08:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 71.55 , 0.00 , 0.00 , 62.81 2021-02-13T01:08:55Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:08:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178535.77 epoch sec; seqNo:18957; 4060951.21 north; 593755.20 east 2021-02-13T01:08:56Z- AHRS_M2 msg: 1613178536.14 epoch sec; seqNo:18808 2021-02-13T01:08:56Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.09 psi 2021-02-13T01:08:56Z- handleDepth()-Depth_Keller msg: 1613178536153 epoch sec; seqNo:18958; depth 27.84 2021-02-13T01:08:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178536.17 epoch sec; seqNo:18958; 4060951.59 north; 593755.19 east 2021-02-13T01:08:56Z- AHRS_M2 msg: 1613178536.54 epoch sec; seqNo:18809 2021-02-13T01:08:56Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.09 psi 2021-02-13T01:08:56Z- handleDepth()-Depth_Keller msg: 1613178536553 epoch sec; seqNo:18959; depth 27.72 2021-02-13T01:08:56Z- handleDvl() - RDI_Pathfinder msg: 1613178536.57 epoch sec; seqNo:9150 2021-02-13T01:08:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 71.77 , 0.00 , 0.00 , 62.47 2021-02-13T01:08:56Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:08:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178536.58 epoch sec; seqNo:18959; 4060951.59 north; 593755.19 east 2021-02-13T01:08:56Z- AHRS_M2 msg: 1613178536.95 epoch sec; seqNo:18810 2021-02-13T01:08:56Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.10 psi 2021-02-13T01:08:56Z- handleDepth()-Depth_Keller msg: 1613178536957 epoch sec; seqNo:18960; depth 27.60 2021-02-13T01:08:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178536.97 epoch sec; seqNo:18960; 4060951.97 north; 593755.19 east 2021-02-13T01:08:57Z- AHRS_M2 msg: 1613178537.35 epoch sec; seqNo:18811 2021-02-13T01:08:57Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.11 psi 2021-02-13T01:08:57Z- handleDepth()-Depth_Keller msg: 1613178537366 epoch sec; seqNo:18961; depth 27.47 2021-02-13T01:08:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178537.37 epoch sec; seqNo:18961; 4060952.34 north; 593755.18 east 2021-02-13T01:08:57Z- LcmTrn::handleMessages() - handling 39 messages took 0.68 ms and 126 calls... 2021-02-13T01:08:57Z- waiting for 1613178535.53, time is 1613178537.37 2021-02-13T01:08:57Z- LcmTrn::updateTrn() >>>> pose time 1613178537.37, meas time 1613178536.57 <<<< 2021-02-13T01:08:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178536.57 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:08:57Z- TerrainNav::motionUpdate() - currEstimate = (4060952, 593755.18, 27.47) 2021-02-13T01:08:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:08:57Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:08:57Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:08:57Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:08:57Z- TNavBF::Projecting Measurements 2021-02-13T01:08:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:08:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:08:57Z- TNavFilter::Excluded 2 beam(s) from correlation due to poor sonar data. 2021-02-13T01:08:57Z- TNavFilter::2 good beams. 2021-02-13T01:08:57Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:08:57Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:08:57Z- TNavBF:: beamIndex[1] = 3 2021-02-13T01:08:57Z- TNavBF:: beamIndex[2] = 0 2021-02-13T01:08:57Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:08:57Z- TNavBF:: *** beamsVF.Ncols() = 2 *** 2021-02-13T01:08:57Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:08:57Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:57Z- TerrainMapOctree: (vehN, vehE ) = (4060951.77, 593754.78). 2021-02-13T01:08:57Z- TerrainMapOctree: Min Distance = 578.08. 2021-02-13T01:08:57Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:08:57Z- There is no specific good beam that all particles have in common. 2021-02-13T01:08:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:08:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.16 K: 0.104 ExpectedMeasDiff: nan currMeas.covariance: 8.24 2021-02-13T01:08:57Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:08:57Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.115 ExpectedMeasDiff: nan currMeas.covariance: 6.24 2021-02-13T01:08:57Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:08:57Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:08:57Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:08:57Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:08:57Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:08:57Z- Filter 0 No beams used in subcloud update 2021-02-13T01:08:57Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:08:57Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:08:57Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 674.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:08:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178536.57 sec. 2021-02-13T01:08:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:08:57Z- TNavBF::mmse selected from filter -1 2021-02-13T01:08:57Z- variances([0], [2], and psi_berg) = 674.336089 832.986146 0.000000 2021-02-13T01:08:57Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:08:57Z- LcmTrn::publishEstimates() - MLE : 4060951.84 593746.68 27.47 2021-02-13T01:08:57Z- LcmTrn::publishEstimates() - MMSE : 4060952.34 593755.18 27.47 2021-02-13T01:08:57Z- LcmTrn::publishEstimates() - COVAR: 674.34 832.99 0.95 0.00 2021-02-13T01:08:57Z- AHRS_M2 msg: 1613178537.87 epoch sec; seqNo:18812 2021-02-13T01:08:57Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.12 psi 2021-02-13T01:08:57Z- handleDepth()-Depth_Keller msg: 1613178537877 epoch sec; seqNo:18962; depth 27.34 2021-02-13T01:08:57Z- handleDvl() - RDI_Pathfinder msg: 1613178537.90 epoch sec; seqNo:9151 2021-02-13T01:08:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 71.77 , 0.00 , 0.00 , 63.35 2021-02-13T01:08:57Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:08:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178537.91 epoch sec; seqNo:18962; 4060952.74 north; 593756.40 east 2021-02-13T01:08:58Z- handleDepth()-Depth_Keller msg: 1613178538185 epoch sec; seqNo:18963; depth 27.19 2021-02-13T01:08:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178538.20 epoch sec; seqNo:18963; 4060952.74 north; 593756.40 east 2021-02-13T01:08:58Z- AHRS_M2 msg: 1613178538.57 epoch sec; seqNo:18813 2021-02-13T01:08:58Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.13 psi 2021-02-13T01:08:58Z- handleDepth()-Depth_Keller msg: 1613178538577 epoch sec; seqNo:18964; depth 27.10 2021-02-13T01:08:58Z- handleDvl() - RDI_Pathfinder msg: 1613178538.59 epoch sec; seqNo:9152 2021-02-13T01:08:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.33 , 81.96 , 0.00 , 62.47 2021-02-13T01:08:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.04 , -0.10 2021-02-13T01:08:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178538.60 epoch sec; seqNo:18964; 4060953.12 north; 593756.40 east 2021-02-13T01:08:58Z- AHRS_M2 msg: 1613178538.97 epoch sec; seqNo:18814 2021-02-13T01:08:58Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.14 psi 2021-02-13T01:08:58Z- handleDepth()-Depth_Keller msg: 1613178538976 epoch sec; seqNo:18965; depth 26.97 2021-02-13T01:08:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178538.99 epoch sec; seqNo:18965; 4060953.12 north; 593756.40 east 2021-02-13T01:08:59Z- AHRS_M2 msg: 1613178539.37 epoch sec; seqNo:18815 2021-02-13T01:08:59Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.14 psi 2021-02-13T01:08:59Z- handleDepth()-Depth_Keller msg: 1613178539376 epoch sec; seqNo:18966; depth 26.86 2021-02-13T01:08:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178539.38 epoch sec; seqNo:18966; 4060953.49 north; 593756.39 east 2021-02-13T01:08:59Z- AHRS_M2 msg: 1613178539.78 epoch sec; seqNo:18816 2021-02-13T01:08:59Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.15 psi 2021-02-13T01:08:59Z- handleDepth()-Depth_Keller msg: 1613178539798 epoch sec; seqNo:18967; depth 26.74 2021-02-13T01:08:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178539.83 epoch sec; seqNo:18967; 4060953.87 north; 593756.39 east 2021-02-13T01:09:00Z- AHRS_M2 msg: 1613178540.18 epoch sec; seqNo:18817 2021-02-13T01:09:00Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.16 psi 2021-02-13T01:09:00Z- handleDepth()-Depth_Keller msg: 1613178540186 epoch sec; seqNo:18968; depth 26.59 2021-02-13T01:09:00Z- handleDvl() - RDI_Pathfinder msg: 1613178540.20 epoch sec; seqNo:9153 2021-02-13T01:09:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.12 , 85.01 , 101.24 , 63.36 2021-02-13T01:09:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.09 2021-02-13T01:09:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178540.21 epoch sec; seqNo:18968; 4060953.87 north; 593756.39 east 2021-02-13T01:09:00Z- AHRS_M2 msg: 1613178540.58 epoch sec; seqNo:18818 2021-02-13T01:09:00Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.17 psi 2021-02-13T01:09:00Z- handleDepth()-Depth_Keller msg: 1613178540593 epoch sec; seqNo:18969; depth 26.47 2021-02-13T01:09:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178540.61 epoch sec; seqNo:18969; 4060954.26 north; 593757.60 east 2021-02-13T01:09:00Z- AHRS_M2 msg: 1613178540.99 epoch sec; seqNo:18819 2021-02-13T01:09:00Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.17 psi 2021-02-13T01:09:00Z- handleDepth()-Depth_Keller msg: 1613178540996 epoch sec; seqNo:18970; depth 26.36 2021-02-13T01:09:01Z- handleDvl() - RDI_Pathfinder msg: 1613178541.01 epoch sec; seqNo:9154 2021-02-13T01:09:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.72 , 87.10 , 96.84 , 63.77 2021-02-13T01:09:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.05 , -0.06 2021-02-13T01:09:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178541.03 epoch sec; seqNo:18970; 4060954.64 north; 593757.60 east 2021-02-13T01:09:01Z- AHRS_M2 msg: 1613178541.39 epoch sec; seqNo:18820 2021-02-13T01:09:01Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.18 psi 2021-02-13T01:09:01Z- handleDepth()-Depth_Keller msg: 1613178541396 epoch sec; seqNo:18971; depth 26.22 2021-02-13T01:09:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178541.41 epoch sec; seqNo:18971; 4060954.64 north; 593757.60 east 2021-02-13T01:09:01Z- AHRS_M2 msg: 1613178541.82 epoch sec; seqNo:18821 2021-02-13T01:09:01Z- AHRS_M2 msg: 0.63 phi; 0.37 theta; -0.18 psi 2021-02-13T01:09:01Z- handleDepth()-Depth_Keller msg: 1613178541822 epoch sec; seqNo:18972; depth 26.10 2021-02-13T01:09:01Z- handleDvl() - RDI_Pathfinder msg: 1613178541.83 epoch sec; seqNo:9155 2021-02-13T01:09:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.72 , 87.17 , 96.91 , 63.36 2021-02-13T01:09:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.08 , -0.01 2021-02-13T01:09:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178541.84 epoch sec; seqNo:18972; 4060955.02 north; 593757.60 east 2021-02-13T01:09:02Z- AHRS_M2 msg: 1613178542.20 epoch sec; seqNo:18822 2021-02-13T01:09:02Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.19 psi 2021-02-13T01:09:02Z- handleDepth()-Depth_Keller msg: 1613178542204 epoch sec; seqNo:18973; depth 25.97 2021-02-13T01:09:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178542.21 epoch sec; seqNo:18973; 4060955.40 north; 593757.59 east 2021-02-13T01:09:02Z- AHRS_M2 msg: 1613178542.60 epoch sec; seqNo:18823 2021-02-13T01:09:02Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.19 psi 2021-02-13T01:09:02Z- handleDepth()-Depth_Keller msg: 1613178542613 epoch sec; seqNo:18974; depth 25.85 2021-02-13T01:09:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178542.62 epoch sec; seqNo:18974; 4060955.40 north; 593757.59 east 2021-02-13T01:09:02Z- LcmTrn::handleMessages() - handling 43 messages took 0.64 ms and 131 calls... 2021-02-13T01:09:02Z- waiting for 1613178540.37, time is 1613178542.62 2021-02-13T01:09:02Z- LcmTrn::updateTrn() >>>> pose time 1613178542.62, meas time 1613178541.83 <<<< 2021-02-13T01:09:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178541.83 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:09:02Z- TerrainNav::motionUpdate() - currEstimate = (4060955, 593757.59, 25.85) 2021-02-13T01:09:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:09:02Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:09:02Z- TNavBF::Projecting Measurements 2021-02-13T01:09:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:09:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:09:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:09:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:09:02Z- TNavFilter::4 good beams. 2021-02-13T01:09:02Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:09:02Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:09:02Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:09:02Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:09:02Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:09:02Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:09:02Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:09:02Z- TNavBF::mmse selected from filter -1 2021-02-13T01:09:02Z- TerrainMapOctree: (vehN, vehE ) = (4060954.94, 593757.23). 2021-02-13T01:09:02Z- TerrainMapOctree: Min Distance = 574.35. 2021-02-13T01:09:02Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:09:02Z- There is no specific good beam that all particles have in common. 2021-02-13T01:09:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:09:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.103 ExpectedMeasDiff: nan currMeas.covariance: 8.00 2021-02-13T01:09:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:09:02Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 12.16 2021-02-13T01:09:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:09:02Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 15.03 2021-02-13T01:09:02Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:09:02Z- Old Depth Bias: 0.00 DepthBiasCov: 0.89 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 6.42 2021-02-13T01:09:02Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:09:02Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:09:02Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:09:02Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:09:02Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:02Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:09:02Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:09:02Z- Filter 0 No beams used in subcloud update 2021-02-13T01:09:02Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:09:02Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:09:02Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 674.3 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T01:09:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178541.83 sec. 2021-02-13T01:09:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:09:02Z- TNavBF::mmse selected from filter -1 2021-02-13T01:09:02Z- variances([0], [2], and psi_berg) = 674.282611 832.986125 0.000000 2021-02-13T01:09:02Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:09:02Z- LcmTrn::publishEstimates() - MLE : 4060954.90 593772.09 25.85 2021-02-13T01:09:02Z- LcmTrn::publishEstimates() - MMSE : 4060955.40 593757.59 25.85 2021-02-13T01:09:02Z- LcmTrn::publishEstimates() - COVAR: 674.28 832.99 0.83 0.00 2021-02-13T01:09:03Z- AHRS_M2 msg: 1613178543.01 epoch sec; seqNo:18824 2021-02-13T01:09:03Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.19 psi 2021-02-13T01:09:03Z- handleDepth()-Depth_Keller msg: 1613178543014 epoch sec; seqNo:18975; depth 25.73 2021-02-13T01:09:03Z- handleDvl() - RDI_Pathfinder msg: 1613178543.03 epoch sec; seqNo:9156 2021-02-13T01:09:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.90 , 90.42 , 103.40 , 62.54 2021-02-13T01:09:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.12 , 0.02 2021-02-13T01:09:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178543.04 epoch sec; seqNo:18975; 4060955.78 north; 593757.59 east 2021-02-13T01:09:03Z- AHRS_M2 msg: 1613178543.41 epoch sec; seqNo:18825 2021-02-13T01:09:03Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.19 psi 2021-02-13T01:09:03Z- handleDepth()-Depth_Keller msg: 1613178543421 epoch sec; seqNo:18976; depth 25.60 2021-02-13T01:09:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178543.43 epoch sec; seqNo:18976; 4060956.16 north; 593757.58 east 2021-02-13T01:09:03Z- AHRS_M2 msg: 1613178543.82 epoch sec; seqNo:18826 2021-02-13T01:09:03Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.19 psi 2021-02-13T01:09:03Z- handleDepth()-Depth_Keller msg: 1613178543821 epoch sec; seqNo:18977; depth 25.48 2021-02-13T01:09:03Z- handleDvl() - RDI_Pathfinder msg: 1613178543.87 epoch sec; seqNo:9157 2021-02-13T01:09:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.72 , 0.00 , 96.77 , 64.18 2021-02-13T01:09:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.09 , 0.05 2021-02-13T01:09:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178543.90 epoch sec; seqNo:18977; 4060956.55 north; 593758.80 east 2021-02-13T01:09:04Z- AHRS_M2 msg: 1613178544.22 epoch sec; seqNo:18827 2021-02-13T01:09:04Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.19 psi 2021-02-13T01:09:04Z- handleDepth()-Depth_Keller msg: 1613178544229 epoch sec; seqNo:18978; depth 25.35 2021-02-13T01:09:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178544.24 epoch sec; seqNo:18978; 4060956.55 north; 593758.80 east 2021-02-13T01:09:04Z- AHRS_M2 msg: 1613178544.62 epoch sec; seqNo:18828 2021-02-13T01:09:04Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.19 psi 2021-02-13T01:09:04Z- handleDepth()-Depth_Keller msg: 1613178544629 epoch sec; seqNo:18979; depth 25.22 2021-02-13T01:09:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178544.64 epoch sec; seqNo:18979; 4060956.93 north; 593758.79 east 2021-02-13T01:09:05Z- AHRS_M2 msg: 1613178545.03 epoch sec; seqNo:18829 2021-02-13T01:09:05Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.19 psi 2021-02-13T01:09:05Z- handleDepth()-Depth_Keller msg: 1613178545035 epoch sec; seqNo:18980; depth 25.12 2021-02-13T01:09:05Z- handleDvl() - RDI_Pathfinder msg: 1613178545.05 epoch sec; seqNo:9158 2021-02-13T01:09:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.31 , 90.42 , 101.24 , 64.18 2021-02-13T01:09:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , 0.06 2021-02-13T01:09:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178545.06 epoch sec; seqNo:18980; 4060957.31 north; 593758.79 east 2021-02-13T01:09:05Z- AHRS_M2 msg: 1613178545.43 epoch sec; seqNo:18830 2021-02-13T01:09:05Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.19 psi 2021-02-13T01:09:05Z- handleDepth()-Depth_Keller msg: 1613178545441 epoch sec; seqNo:18981; depth 24.98 2021-02-13T01:09:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178545.46 epoch sec; seqNo:18981; 4060957.69 north; 593758.79 east 2021-02-13T01:09:05Z- AHRS_M2 msg: 1613178545.84 epoch sec; seqNo:18831 2021-02-13T01:09:05Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.19 psi 2021-02-13T01:09:05Z- handleDepth()-Depth_Keller msg: 1613178545850 epoch sec; seqNo:18982; depth 24.85 2021-02-13T01:09:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178545.89 epoch sec; seqNo:18982; 4060957.69 north; 593758.79 east 2021-02-13T01:09:06Z- AHRS_M2 msg: 1613178546.24 epoch sec; seqNo:18832 2021-02-13T01:09:06Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.19 psi 2021-02-13T01:09:06Z- handleDepth()-Depth_Keller msg: 1613178546249 epoch sec; seqNo:18983; depth 24.75 2021-02-13T01:09:06Z- handleDvl() - RDI_Pathfinder msg: 1613178546.26 epoch sec; seqNo:9159 2021-02-13T01:09:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.38 , 90.28 , 96.77 , 64.58 2021-02-13T01:09:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , 0.08 2021-02-13T01:09:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178546.27 epoch sec; seqNo:18983; 4060958.07 north; 593758.78 east 2021-02-13T01:09:06Z- AHRS_M2 msg: 1613178546.65 epoch sec; seqNo:18833 2021-02-13T01:09:06Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.19 psi 2021-02-13T01:09:06Z- handleDepth()-Depth_Keller msg: 1613178546657 epoch sec; seqNo:18984; depth 24.62 2021-02-13T01:09:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178546.67 epoch sec; seqNo:18984; 4060958.45 north; 593758.78 east 2021-02-13T01:09:07Z- AHRS_M2 msg: 1613178547.05 epoch sec; seqNo:18834 2021-02-13T01:09:07Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.18 psi 2021-02-13T01:09:07Z- handleDepth()-Depth_Keller msg: 1613178547058 epoch sec; seqNo:18985; depth 24.50 2021-02-13T01:09:07Z- handleDvl() - RDI_Pathfinder msg: 1613178547.08 epoch sec; seqNo:9160 2021-02-13T01:09:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.94 , 89.88 , 103.27 , 63.77 2021-02-13T01:09:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.08 2021-02-13T01:09:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178547.09 epoch sec; seqNo:18985; 4060958.84 north; 593759.99 east 2021-02-13T01:09:07Z- AHRS_M2 msg: 1613178547.45 epoch sec; seqNo:18835 2021-02-13T01:09:07Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.18 psi 2021-02-13T01:09:07Z- handleDepth()-Depth_Keller msg: 1613178547456 epoch sec; seqNo:18986; depth 24.38 2021-02-13T01:09:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178547.47 epoch sec; seqNo:18986; 4060958.84 north; 593759.99 east 2021-02-13T01:09:07Z- LcmTrn::handleMessages() - handling 41 messages took 0.66 ms and 128 calls... 2021-02-13T01:09:07Z- waiting for 1613178545.62, time is 1613178547.47 2021-02-13T01:09:07Z- LcmTrn::updateTrn() >>>> pose time 1613178547.47, meas time 1613178547.08 <<<< 2021-02-13T01:09:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178547.08 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:09:07Z- TerrainNav::motionUpdate() - currEstimate = (4060959, 593759.99, 24.38) 2021-02-13T01:09:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:09:07Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:09:07Z- TNavBF::Projecting Measurements 2021-02-13T01:09:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:09:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:09:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:09:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:09:07Z- TNavFilter::4 good beams. 2021-02-13T01:09:07Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:09:07Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:09:07Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:09:07Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:09:07Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:09:07Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:09:07Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:09:07Z- TNavBF::mmse selected from filter -1 2021-02-13T01:09:07Z- TerrainMapOctree: (vehN, vehE ) = (4060958.56, 593759.80). 2021-02-13T01:09:07Z- TerrainMapOctree: Min Distance = 570.29. 2021-02-13T01:09:07Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:09:07Z- There is no specific good beam that all particles have in common. 2021-02-13T01:09:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:09:07Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 8.28 2021-02-13T01:09:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:09:07Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 12.92 2021-02-13T01:09:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:09:07Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 17.06 2021-02-13T01:09:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:09:07Z- Old Depth Bias: 0.00 DepthBiasCov: 0.84 K: 0.092 ExpectedMeasDiff: nan currMeas.covariance: 6.51 2021-02-13T01:09:07Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:09:07Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:09:07Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:09:07Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:09:07Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:09:07Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:09:07Z- Filter 0 No beams used in subcloud update 2021-02-13T01:09:07Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:09:07Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:09:07Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 674.2 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T01:09:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178547.08 sec. 2021-02-13T01:09:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:09:07Z- TNavBF::mmse selected from filter -1 2021-02-13T01:09:07Z- variances([0], [2], and psi_berg) = 674.230005 832.986104 0.000000 2021-02-13T01:09:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:09:07Z- LcmTrn::publishEstimates() - MLE : 4060958.34 593751.49 24.38 2021-02-13T01:09:07Z- LcmTrn::publishEstimates() - MMSE : 4060958.84 593759.99 24.38 2021-02-13T01:09:07Z- LcmTrn::publishEstimates() - COVAR: 674.23 832.99 0.78 0.00 2021-02-13T01:09:07Z- AHRS_M2 msg: 1613178547.86 epoch sec; seqNo:18836 2021-02-13T01:09:07Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.17 psi 2021-02-13T01:09:07Z- handleDepth()-Depth_Keller msg: 1613178547865 epoch sec; seqNo:18987; depth 24.25 2021-02-13T01:09:07Z- handleDvl() - RDI_Pathfinder msg: 1613178547.88 epoch sec; seqNo:9161 2021-02-13T01:09:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.94 , 90.21 , 96.71 , 64.99 2021-02-13T01:09:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.01 , 0.07 2021-02-13T01:09:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178547.89 epoch sec; seqNo:18987; 4060959.22 north; 593759.99 east 2021-02-13T01:09:08Z- AHRS_M2 msg: 1613178548.26 epoch sec; seqNo:18837 2021-02-13T01:09:08Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.17 psi 2021-02-13T01:09:08Z- handleDepth()-Depth_Keller msg: 1613178548269 epoch sec; seqNo:18988; depth 24.12 2021-02-13T01:09:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178548.28 epoch sec; seqNo:18988; 4060959.60 north; 593759.99 east 2021-02-13T01:09:08Z- AHRS_M2 msg: 1613178548.66 epoch sec; seqNo:18838 2021-02-13T01:09:08Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.16 psi 2021-02-13T01:09:08Z- handleDepth()-Depth_Keller msg: 1613178548669 epoch sec; seqNo:18989; depth 24.00 2021-02-13T01:09:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178548.68 epoch sec; seqNo:18989; 4060959.60 north; 593759.99 east 2021-02-13T01:09:09Z- AHRS_M2 msg: 1613178549.08 epoch sec; seqNo:18839 2021-02-13T01:09:09Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.15 psi 2021-02-13T01:09:09Z- handleDepth()-Depth_Keller msg: 1613178549081 epoch sec; seqNo:18990; depth 23.89 2021-02-13T01:09:09Z- handleDvl() - RDI_Pathfinder msg: 1613178549.09 epoch sec; seqNo:9162 2021-02-13T01:09:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.99 , 0.00 , 104.35 , 65.40 2021-02-13T01:09:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.03 , 0.04 2021-02-13T01:09:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178549.10 epoch sec; seqNo:18990; 4060959.97 north; 593759.98 east 2021-02-13T01:09:09Z- AHRS_M2 msg: 1613178549.47 epoch sec; seqNo:18840 2021-02-13T01:09:09Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.15 psi 2021-02-13T01:09:09Z- handleDepth()-Depth_Keller msg: 1613178549476 epoch sec; seqNo:18991; depth 23.75 2021-02-13T01:09:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178549.49 epoch sec; seqNo:18991; 4060960.37 north; 593761.20 east 2021-02-13T01:09:09Z- AHRS_M2 msg: 1613178549.88 epoch sec; seqNo:18841 2021-02-13T01:09:09Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.14 psi 2021-02-13T01:09:09Z- handleDepth()-Depth_Keller msg: 1613178549885 epoch sec; seqNo:18992; depth 23.63 2021-02-13T01:09:09Z- handleDvl() - RDI_Pathfinder msg: 1613178549.90 epoch sec; seqNo:9163 2021-02-13T01:09:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.40 , 0.00 , 104.35 , 65.40 2021-02-13T01:09:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.08 , 0.01 2021-02-13T01:09:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178549.91 epoch sec; seqNo:18992; 4060960.37 north; 593761.20 east 2021-02-13T01:09:10Z- AHRS_M2 msg: 1613178550.28 epoch sec; seqNo:18842 2021-02-13T01:09:10Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.14 psi 2021-02-13T01:09:10Z- handleDepth()-Depth_Keller msg: 1613178550293 epoch sec; seqNo:18993; depth 23.52 2021-02-13T01:09:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178550.30 epoch sec; seqNo:18993; 4060960.75 north; 593761.19 east 2021-02-13T01:09:10Z- AHRS_M2 msg: 1613178550.68 epoch sec; seqNo:18843 2021-02-13T01:09:10Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.14 psi 2021-02-13T01:09:10Z- handleDepth()-Depth_Keller msg: 1613178550689 epoch sec; seqNo:18994; depth 23.39 2021-02-13T01:09:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178550.70 epoch sec; seqNo:18994; 4060961.12 north; 593761.19 east 2021-02-13T01:09:11Z- AHRS_M2 msg: 1613178551.09 epoch sec; seqNo:18844 2021-02-13T01:09:11Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.14 psi 2021-02-13T01:09:11Z- handleDepth()-Depth_Keller msg: 1613178551102 epoch sec; seqNo:18995; depth 23.28 2021-02-13T01:09:11Z- handleDvl() - RDI_Pathfinder msg: 1613178551.11 epoch sec; seqNo:9164 2021-02-13T01:09:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 89.20 , 104.35 , 64.99 2021-02-13T01:09:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.10 , -0.02 2021-02-13T01:09:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178551.13 epoch sec; seqNo:18995; 4060961.12 north; 593761.19 east 2021-02-13T01:09:11Z- AHRS_M2 msg: 1613178551.49 epoch sec; seqNo:18845 2021-02-13T01:09:11Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.14 psi 2021-02-13T01:09:11Z- handleDepth()-Depth_Keller msg: 1613178551501 epoch sec; seqNo:18996; depth 23.14 2021-02-13T01:09:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178551.52 epoch sec; seqNo:18996; 4060961.50 north; 593761.19 east 2021-02-13T01:09:11Z- AHRS_M2 msg: 1613178551.89 epoch sec; seqNo:18846 2021-02-13T01:09:11Z- AHRS_M2 msg: 0.62 phi; 0.36 theta; -0.14 psi 2021-02-13T01:09:11Z- handleDepth()-Depth_Keller msg: 1613178551900 epoch sec; seqNo:18997; depth 23.03 2021-02-13T01:09:11Z- handleDvl() - RDI_Pathfinder msg: 1613178551.91 epoch sec; seqNo:9165 2021-02-13T01:09:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.21 , 88.59 , 104.42 , 66.63 2021-02-13T01:09:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.08 , -0.06 2021-02-13T01:09:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178551.92 epoch sec; seqNo:18997; 4060961.90 north; 593762.40 east 2021-02-13T01:09:12Z- AHRS_M2 msg: 1613178552.31 epoch sec; seqNo:18847 2021-02-13T01:09:12Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.14 psi 2021-02-13T01:09:12Z- handleDepth()-Depth_Keller msg: 1613178552310 epoch sec; seqNo:18998; depth 22.90 2021-02-13T01:09:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178552.32 epoch sec; seqNo:18998; 4060961.90 north; 593762.40 east 2021-02-13T01:09:12Z- AHRS_M2 msg: 1613178552.70 epoch sec; seqNo:18848 2021-02-13T01:09:12Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.14 psi 2021-02-13T01:09:12Z- handleDepth()-Depth_Keller msg: 1613178552717 epoch sec; seqNo:18999; depth 22.77 2021-02-13T01:09:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178552.72 epoch sec; seqNo:18999; 4060962.27 north; 593762.40 east 2021-02-13T01:09:12Z- LcmTrn::handleMessages() - handling 44 messages took 0.62 ms and 132 calls... 2021-02-13T01:09:12Z- waiting for 1613178550.47, time is 1613178552.72 2021-02-13T01:09:12Z- LcmTrn::updateTrn() >>>> pose time 1613178552.72, meas time 1613178551.91 <<<< 2021-02-13T01:09:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178551.91 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:09:12Z- TerrainNav::motionUpdate() - currEstimate = (4060962, 593762.40, 22.77) 2021-02-13T01:09:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:09:12Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:09:12Z- TNavBF::Projecting Measurements 2021-02-13T01:09:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:09:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:09:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:09:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:09:12Z- TNavFilter::4 good beams. 2021-02-13T01:09:12Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:09:12Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:09:12Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:09:12Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:09:12Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:09:12Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:09:12Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:09:12Z- TNavBF::mmse selected from filter -1 2021-02-13T01:09:12Z- TerrainMapOctree: (vehN, vehE ) = (4060961.74, 593762.03). 2021-02-13T01:09:12Z- TerrainMapOctree: Min Distance = 566.76. 2021-02-13T01:09:12Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:09:12Z- There is no specific good beam that all particles have in common. 2021-02-13T01:09:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:09:12Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 9.05 2021-02-13T01:09:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:09:12Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 12.56 2021-02-13T01:09:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:09:12Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 17.44 2021-02-13T01:09:12Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:09:12Z- Old Depth Bias: 0.00 DepthBiasCov: 0.79 K: 0.082 ExpectedMeasDiff: nan currMeas.covariance: 7.10 2021-02-13T01:09:12Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:09:12Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:09:12Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:09:12Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:09:12Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:12Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:09:12Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:09:12Z- Filter 0 No beams used in subcloud update 2021-02-13T01:09:12Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:09:12Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:09:12Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 674.2 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T01:09:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178551.91 sec. 2021-02-13T01:09:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:09:13Z- TNavBF::mmse selected from filter -1 2021-02-13T01:09:13Z- variances([0], [2], and psi_berg) = 674.174413 832.986081 0.000000 2021-02-13T01:09:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:09:13Z- LcmTrn::publishEstimates() - MLE : 4060961.77 593753.90 22.77 2021-02-13T01:09:13Z- LcmTrn::publishEstimates() - MMSE : 4060962.27 593762.40 22.77 2021-02-13T01:09:13Z- LcmTrn::publishEstimates() - COVAR: 674.17 832.99 0.76 0.00 2021-02-13T01:09:13Z- AHRS_M2 msg: 1613178553.11 epoch sec; seqNo:18849 2021-02-13T01:09:13Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.14 psi 2021-02-13T01:09:13Z- handleDepth()-Depth_Keller msg: 1613178553117 epoch sec; seqNo:19000; depth 22.65 2021-02-13T01:09:13Z- handleDvl() - RDI_Pathfinder msg: 1613178553.14 epoch sec; seqNo:9166 2021-02-13T01:09:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 88.59 , 103.20 , 67.04 2021-02-13T01:09:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.03 , -0.10 2021-02-13T01:09:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178553.15 epoch sec; seqNo:19000; 4060962.65 north; 593762.39 east 2021-02-13T01:09:13Z- AHRS_M2 msg: 1613178553.51 epoch sec; seqNo:18850 2021-02-13T01:09:13Z- AHRS_M2 msg: 0.63 phi; 0.35 theta; -0.15 psi 2021-02-13T01:09:13Z- handleDepth()-Depth_Keller msg: 1613178553516 epoch sec; seqNo:19001; depth 22.51 2021-02-13T01:09:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178553.53 epoch sec; seqNo:19001; 4060962.65 north; 593762.39 east 2021-02-13T01:09:13Z- AHRS_M2 msg: 1613178553.92 epoch sec; seqNo:18851 2021-02-13T01:09:13Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.15 psi 2021-02-13T01:09:13Z- handleDepth()-Depth_Keller msg: 1613178553922 epoch sec; seqNo:19002; depth 22.42 2021-02-13T01:09:13Z- handleDvl() - RDI_Pathfinder msg: 1613178553.93 epoch sec; seqNo:9167 2021-02-13T01:09:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.80 , 88.59 , 101.98 , 67.86 2021-02-13T01:09:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.01 , -0.12 2021-02-13T01:09:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178553.94 epoch sec; seqNo:19002; 4060963.03 north; 593762.39 east 2021-02-13T01:09:14Z- AHRS_M2 msg: 1613178554.32 epoch sec; seqNo:18852 2021-02-13T01:09:14Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.16 psi 2021-02-13T01:09:14Z- handleDepth()-Depth_Keller msg: 1613178554325 epoch sec; seqNo:19003; depth 22.29 2021-02-13T01:09:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178554.34 epoch sec; seqNo:19003; 4060963.05 north; 593763.61 east 2021-02-13T01:09:14Z- AHRS_M2 msg: 1613178554.72 epoch sec; seqNo:18853 2021-02-13T01:09:14Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.16 psi 2021-02-13T01:09:14Z- handleDepth()-Depth_Keller msg: 1613178554728 epoch sec; seqNo:19004; depth 22.17 2021-02-13T01:09:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178554.74 epoch sec; seqNo:19004; 4060963.42 north; 593763.61 east 2021-02-13T01:09:15Z- AHRS_M2 msg: 1613178555.13 epoch sec; seqNo:18854 2021-02-13T01:09:15Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.17 psi 2021-02-13T01:09:15Z- handleDepth()-Depth_Keller msg: 1613178555135 epoch sec; seqNo:19005; depth 22.06 2021-02-13T01:09:15Z- handleDvl() - RDI_Pathfinder msg: 1613178555.15 epoch sec; seqNo:9168 2021-02-13T01:09:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.99 , 88.59 , 101.98 , 67.04 2021-02-13T01:09:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.08 , -0.09 2021-02-13T01:09:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178555.18 epoch sec; seqNo:19005; 4060963.80 north; 593763.60 east 2021-02-13T01:09:15Z- AHRS_M2 msg: 1613178555.53 epoch sec; seqNo:18855 2021-02-13T01:09:15Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.18 psi 2021-02-13T01:09:15Z- handleDepth()-Depth_Keller msg: 1613178555545 epoch sec; seqNo:19006; depth 21.96 2021-02-13T01:09:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178555.56 epoch sec; seqNo:19006; 4060963.80 north; 593763.60 east 2021-02-13T01:09:15Z- AHRS_M2 msg: 1613178555.94 epoch sec; seqNo:18856 2021-02-13T01:09:15Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.18 psi 2021-02-13T01:09:15Z- handleDepth()-Depth_Keller msg: 1613178555953 epoch sec; seqNo:19007; depth 21.84 2021-02-13T01:09:15Z- handleDvl() - RDI_Pathfinder msg: 1613178555.97 epoch sec; seqNo:9169 2021-02-13T01:09:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.99 , 98.40 , 102.05 , 67.04 2021-02-13T01:09:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.09 , -0.06 2021-02-13T01:09:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178555.98 epoch sec; seqNo:19007; 4060964.18 north; 593763.60 east 2021-02-13T01:09:16Z- AHRS_M2 msg: 1613178556.34 epoch sec; seqNo:18857 2021-02-13T01:09:16Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-13T01:09:16Z- handleDepth()-Depth_Keller msg: 1613178556348 epoch sec; seqNo:19008; depth 21.72 2021-02-13T01:09:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178556.36 epoch sec; seqNo:19008; 4060964.56 north; 593763.59 east 2021-02-13T01:09:16Z- AHRS_M2 msg: 1613178556.75 epoch sec; seqNo:18858 2021-02-13T01:09:16Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.19 psi 2021-02-13T01:09:16Z- handleDepth()-Depth_Keller msg: 1613178556759 epoch sec; seqNo:19009; depth 21.61 2021-02-13T01:09:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178556.79 epoch sec; seqNo:19009; 4060964.94 north; 593763.59 east 2021-02-13T01:09:17Z- AHRS_M2 msg: 1613178557.15 epoch sec; seqNo:18859 2021-02-13T01:09:17Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.19 psi 2021-02-13T01:09:17Z- handleDepth()-Depth_Keller msg: 1613178557161 epoch sec; seqNo:19010; depth 21.50 2021-02-13T01:09:17Z- handleDvl() - RDI_Pathfinder msg: 1613178557.17 epoch sec; seqNo:9170 2021-02-13T01:09:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.58 , 92.11 , 101.85 , 68.26 2021-02-13T01:09:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.10 , 0.02 2021-02-13T01:09:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178557.18 epoch sec; seqNo:19010; 4060964.94 north; 593763.59 east 2021-02-13T01:09:17Z- AHRS_M2 msg: 1613178557.55 epoch sec; seqNo:18860 2021-02-13T01:09:17Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.18 psi 2021-02-13T01:09:17Z- handleDepth()-Depth_Keller msg: 1613178557561 epoch sec; seqNo:19011; depth 21.39 2021-02-13T01:09:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178557.57 epoch sec; seqNo:19011; 4060965.32 north; 593763.59 east 2021-02-13T01:09:17Z- AHRS_M2 msg: 1613178557.96 epoch sec; seqNo:18861 2021-02-13T01:09:17Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.18 psi 2021-02-13T01:09:17Z- handleDepth()-Depth_Keller msg: 1613178557960 epoch sec; seqNo:19012; depth 21.29 2021-02-13T01:09:17Z- handleDvl() - RDI_Pathfinder msg: 1613178557.97 epoch sec; seqNo:9171 2021-02-13T01:09:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.58 , 90.96 , 100.70 , 68.26 2021-02-13T01:09:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.10 , 0.07 2021-02-13T01:09:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178557.98 epoch sec; seqNo:19012; 4060965.71 north; 593764.80 east 2021-02-13T01:09:18Z- LcmTrn::handleMessages() - handling 45 messages took 0.67 ms and 132 calls... 2021-02-13T01:09:18Z- waiting for 1613178555.72, time is 1613178557.98 2021-02-13T01:09:18Z- LcmTrn::updateTrn() equating pose 1613178557.98 and meas times 1613178557.98 2021-02-13T01:09:18Z- LcmTrn::updateTrn() >>>> pose time 1613178557.98, meas time 1613178557.98 <<<< 2021-02-13T01:09:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178557.98 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:09:18Z- TerrainNav::motionUpdate() - currEstimate = (4060966, 593764.80, 21.29) 2021-02-13T01:09:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:09:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:09:18Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178557.98 sec.; No relevant navigation data available 2021-02-13T01:09:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:09:18Z- TNavBF::mmse selected from filter -1 2021-02-13T01:09:18Z- variances([0], [2], and psi_berg) = 674.132086 832.986065 0.000000 2021-02-13T01:09:18Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:09:18Z- LcmTrn::publishEstimates() - MLE : 4060965.21 593779.30 21.29 2021-02-13T01:09:18Z- LcmTrn::publishEstimates() - MMSE : 4060965.71 593764.80 21.29 2021-02-13T01:09:18Z- LcmTrn::publishEstimates() - COVAR: 674.13 832.99 0.98 0.00 2021-02-13T01:09:18Z- AHRS_M2 msg: 1613178558.37 epoch sec; seqNo:18862 2021-02-13T01:09:18Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.18 psi 2021-02-13T01:09:18Z- handleDepth()-Depth_Keller msg: 1613178558370 epoch sec; seqNo:19013; depth 21.20 2021-02-13T01:09:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178558.38 epoch sec; seqNo:19013; 4060965.71 north; 593764.80 east 2021-02-13T01:09:18Z- AHRS_M2 msg: 1613178558.79 epoch sec; seqNo:18863 2021-02-13T01:09:18Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.18 psi 2021-02-13T01:09:18Z- handleDepth()-Depth_Keller msg: 1613178558793 epoch sec; seqNo:19014; depth 21.10 2021-02-13T01:09:18Z- handleDvl() - RDI_Pathfinder msg: 1613178558.80 epoch sec; seqNo:9172 2021-02-13T01:09:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.99 , 98.33 , 97.11 , 69.08 2021-02-13T01:09:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.06 , 0.13 2021-02-13T01:09:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178558.81 epoch sec; seqNo:19014; 4060966.09 north; 593764.80 east 2021-02-13T01:09:19Z- AHRS_M2 msg: 1613178559.17 epoch sec; seqNo:18864 2021-02-13T01:09:19Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.18 psi 2021-02-13T01:09:19Z- handleDepth()-Depth_Keller msg: 1613178559172 epoch sec; seqNo:19015; depth 20.98 2021-02-13T01:09:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178559.18 epoch sec; seqNo:19015; 4060966.47 north; 593764.79 east 2021-02-13T01:09:19Z- AHRS_M2 msg: 1613178559.57 epoch sec; seqNo:18865 2021-02-13T01:09:19Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.18 psi 2021-02-13T01:09:19Z- handleDepth()-Depth_Keller msg: 1613178559585 epoch sec; seqNo:19016; depth 20.89 2021-02-13T01:09:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178559.59 epoch sec; seqNo:19016; 4060966.85 north; 593764.79 east 2021-02-13T01:09:19Z- AHRS_M2 msg: 1613178559.98 epoch sec; seqNo:18866 2021-02-13T01:09:19Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.18 psi 2021-02-13T01:09:19Z- handleDepth()-Depth_Keller msg: 1613178559985 epoch sec; seqNo:19017; depth 20.80 2021-02-13T01:09:19Z- handleDvl() - RDI_Pathfinder msg: 1613178560.00 epoch sec; seqNo:9173 2021-02-13T01:09:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.40 , 93.39 , 99.48 , 0.00 2021-02-13T01:09:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , 0.16 2021-02-13T01:09:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178560.02 epoch sec; seqNo:19017; 4060966.85 north; 593764.79 east 2021-02-13T01:09:20Z- AHRS_M2 msg: 1613178560.39 epoch sec; seqNo:18867 2021-02-13T01:09:20Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.18 psi 2021-02-13T01:09:20Z- handleDepth()-Depth_Keller msg: 1613178560397 epoch sec; seqNo:19018; depth 20.69 2021-02-13T01:09:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178560.41 epoch sec; seqNo:19018; 4060967.23 north; 593764.78 east 2021-02-13T01:09:20Z- AHRS_M2 msg: 1613178560.79 epoch sec; seqNo:18868 2021-02-13T01:09:20Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.17 psi 2021-02-13T01:09:20Z- handleDepth()-Depth_Keller msg: 1613178560789 epoch sec; seqNo:19019; depth 20.62 2021-02-13T01:09:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178560.80 epoch sec; seqNo:19019; 4060967.62 north; 593766.00 east 2021-02-13T01:09:21Z- AHRS_M2 msg: 1613178561.19 epoch sec; seqNo:18869 2021-02-13T01:09:21Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.17 psi 2021-02-13T01:09:21Z- handleDepth()-Depth_Keller msg: 1613178561192 epoch sec; seqNo:19020; depth 20.53 2021-02-13T01:09:21Z- handleDvl() - RDI_Pathfinder msg: 1613178561.21 epoch sec; seqNo:9174 2021-02-13T01:09:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.40 , 91.77 , 96.64 , 67.45 2021-02-13T01:09:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.06 , 0.15 2021-02-13T01:09:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178561.22 epoch sec; seqNo:19020; 4060968.00 north; 593766.00 east 2021-02-13T01:09:21Z- AHRS_M2 msg: 1613178561.59 epoch sec; seqNo:18870 2021-02-13T01:09:21Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.17 psi 2021-02-13T01:09:21Z- handleDepth()-Depth_Keller msg: 1613178561596 epoch sec; seqNo:19021; depth 20.41 2021-02-13T01:09:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178561.61 epoch sec; seqNo:19021; 4060968.38 north; 593765.99 east 2021-02-13T01:09:21Z- AHRS_M2 msg: 1613178561.99 epoch sec; seqNo:18871 2021-02-13T01:09:21Z- AHRS_M2 msg: 0.59 phi; 0.31 theta; -0.16 psi 2021-02-13T01:09:22Z- handleDepth()-Depth_Keller msg: 1613178562005 epoch sec; seqNo:19022; depth 20.33 2021-02-13T01:09:22Z- handleDvl() - RDI_Pathfinder msg: 1613178562.02 epoch sec; seqNo:9175 2021-02-13T01:09:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.62 , 92.31 , 102.05 , 69.49 2021-02-13T01:09:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.15 , 0.06 2021-02-13T01:09:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178562.03 epoch sec; seqNo:19022; 4060968.38 north; 593765.99 east 2021-02-13T01:09:22Z- AHRS_M2 msg: 1613178562.40 epoch sec; seqNo:18872 2021-02-13T01:09:22Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.16 psi 2021-02-13T01:09:22Z- handleDepth()-Depth_Keller msg: 1613178562413 epoch sec; seqNo:19023; depth 20.23 2021-02-13T01:09:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178562.43 epoch sec; seqNo:19023; 4060968.75 north; 593765.99 east 2021-02-13T01:09:22Z- AHRS_M2 msg: 1613178562.80 epoch sec; seqNo:18873 2021-02-13T01:09:22Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.15 psi 2021-02-13T01:09:22Z- handleDepth()-Depth_Keller msg: 1613178562808 epoch sec; seqNo:19024; depth 20.15 2021-02-13T01:09:22Z- handleDvl() - RDI_Pathfinder msg: 1613178562.83 epoch sec; seqNo:9176 2021-02-13T01:09:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.03 , 98.19 , 96.98 , 69.90 2021-02-13T01:09:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.22 , -0.08 2021-02-13T01:09:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178562.84 epoch sec; seqNo:19024; 4060969.15 north; 593767.21 east 2021-02-13T01:09:23Z- LcmTrn::handleMessages() - handling 41 messages took 0.59 ms and 130 calls... 2021-02-13T01:09:23Z- waiting for 1613178560.98, time is 1613178562.84 2021-02-13T01:09:23Z- LcmTrn::updateTrn() equating pose 1613178562.84 and meas times 1613178562.84 2021-02-13T01:09:23Z- LcmTrn::updateTrn() >>>> pose time 1613178562.84, meas time 1613178562.84 <<<< 2021-02-13T01:09:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178562.84 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:09:23Z- TerrainNav::motionUpdate() - currEstimate = (4060969, 593767.21, 20.15) 2021-02-13T01:09:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:09:23Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:09:23Z- TNavBF::Projecting Measurements 2021-02-13T01:09:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:09:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:09:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:09:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:09:23Z- TNavFilter::4 good beams. 2021-02-13T01:09:23Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:09:23Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:09:23Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:09:23Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:09:23Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:09:23Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:09:23Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:09:23Z- TNavBF::mmse selected from filter -1 2021-02-13T01:09:23Z- TerrainMapOctree: (vehN, vehE ) = (4060969.15, 593767.21). 2021-02-13T01:09:23Z- TerrainMapOctree: Min Distance = 558.54. 2021-02-13T01:09:23Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:09:23Z- There is no specific good beam that all particles have in common. 2021-02-13T01:09:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:09:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.098 ExpectedMeasDiff: nan currMeas.covariance: 9.25 2021-02-13T01:09:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:09:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 15.43 2021-02-13T01:09:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:09:23Z- Old Depth Bias: 0.00 DepthBiasCov: 1.01 K: 0.057 ExpectedMeasDiff: nan currMeas.covariance: 15.05 2021-02-13T01:09:23Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:09:23Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.091 ExpectedMeasDiff: nan currMeas.covariance: 7.82 2021-02-13T01:09:23Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:09:23Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:09:23Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:09:23Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:09:23Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:23Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:09:23Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:09:23Z- Filter 0 No beams used in subcloud update 2021-02-13T01:09:23Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:09:23Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:09:23Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 674.1 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:09:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178562.84 sec. 2021-02-13T01:09:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:09:23Z- TNavBF::mmse selected from filter -1 2021-02-13T01:09:23Z- variances([0], [2], and psi_berg) = 674.089777 832.986048 0.000000 2021-02-13T01:09:23Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:09:23Z- LcmTrn::publishEstimates() - MLE : 4060968.65 593781.71 20.15 2021-02-13T01:09:23Z- LcmTrn::publishEstimates() - MMSE : 4060969.15 593767.21 20.15 2021-02-13T01:09:23Z- LcmTrn::publishEstimates() - COVAR: 674.09 832.99 0.87 0.00 2021-02-13T01:09:23Z- AHRS_M2 msg: 1613178563.21 epoch sec; seqNo:18874 2021-02-13T01:09:23Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.14 psi 2021-02-13T01:09:23Z- handleDepth()-Depth_Keller msg: 1613178563217 epoch sec; seqNo:19025; depth 20.06 2021-02-13T01:09:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178563.23 epoch sec; seqNo:19025; 4060969.15 north; 593767.21 east 2021-02-13T01:09:23Z- AHRS_M2 msg: 1613178563.61 epoch sec; seqNo:18875 2021-02-13T01:09:23Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.14 psi 2021-02-13T01:09:23Z- handleDepth()-Depth_Keller msg: 1613178563616 epoch sec; seqNo:19026; depth 19.95 2021-02-13T01:09:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178563.62 epoch sec; seqNo:19026; 4060969.53 north; 593767.20 east 2021-02-13T01:09:24Z- AHRS_M2 msg: 1613178564.02 epoch sec; seqNo:18876 2021-02-13T01:09:24Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.14 psi 2021-02-13T01:09:24Z- handleDepth()-Depth_Keller msg: 1613178564029 epoch sec; seqNo:19027; depth 19.87 2021-02-13T01:09:24Z- handleDvl() - RDI_Pathfinder msg: 1613178564.04 epoch sec; seqNo:9177 2021-02-13T01:09:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.44 , 98.19 , 101.85 , 69.49 2021-02-13T01:09:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.11 , -0.19 2021-02-13T01:09:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178564.05 epoch sec; seqNo:19027; 4060969.53 north; 593767.20 east 2021-02-13T01:09:24Z- AHRS_M2 msg: 1613178564.42 epoch sec; seqNo:18877 2021-02-13T01:09:24Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-13T01:09:24Z- handleDepth()-Depth_Keller msg: 1613178564429 epoch sec; seqNo:19028; depth 19.76 2021-02-13T01:09:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178564.45 epoch sec; seqNo:19028; 4060969.90 north; 593767.20 east 2021-02-13T01:09:24Z- AHRS_M2 msg: 1613178564.85 epoch sec; seqNo:18878 2021-02-13T01:09:24Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.15 psi 2021-02-13T01:09:24Z- handleDepth()-Depth_Keller msg: 1613178564853 epoch sec; seqNo:19029; depth 19.68 2021-02-13T01:09:24Z- handleDvl() - RDI_Pathfinder msg: 1613178564.87 epoch sec; seqNo:9178 2021-02-13T01:09:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.44 , 91.97 , 100.49 , 68.67 2021-02-13T01:09:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.00 , -0.21 2021-02-13T01:09:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178564.88 epoch sec; seqNo:19029; 4060970.30 north; 593768.41 east 2021-02-13T01:09:25Z- AHRS_M2 msg: 1613178565.23 epoch sec; seqNo:18879 2021-02-13T01:09:25Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.16 psi 2021-02-13T01:09:25Z- handleDepth()-Depth_Keller msg: 1613178565234 epoch sec; seqNo:19030; depth 19.57 2021-02-13T01:09:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178565.24 epoch sec; seqNo:19030; 4060970.30 north; 593768.41 east 2021-02-13T01:09:25Z- AHRS_M2 msg: 1613178565.63 epoch sec; seqNo:18880 2021-02-13T01:09:25Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.16 psi 2021-02-13T01:09:25Z- handleDepth()-Depth_Keller msg: 1613178565641 epoch sec; seqNo:19031; depth 19.47 2021-02-13T01:09:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178565.66 epoch sec; seqNo:19031; 4060970.68 north; 593768.41 east 2021-02-13T01:09:26Z- AHRS_M2 msg: 1613178566.04 epoch sec; seqNo:18881 2021-02-13T01:09:26Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.16 psi 2021-02-13T01:09:26Z- handleDepth()-Depth_Keller msg: 1613178566040 epoch sec; seqNo:19032; depth 19.37 2021-02-13T01:09:26Z- handleDvl() - RDI_Pathfinder msg: 1613178566.05 epoch sec; seqNo:9179 2021-02-13T01:09:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.21 , 93.39 , 97.04 , 70.72 2021-02-13T01:09:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.12 , -0.11 2021-02-13T01:09:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178566.08 epoch sec; seqNo:19032; 4060970.68 north; 593768.41 east 2021-02-13T01:09:26Z- AHRS_M2 msg: 1613178566.44 epoch sec; seqNo:18882 2021-02-13T01:09:26Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.16 psi 2021-02-13T01:09:26Z- handleDepth()-Depth_Keller msg: 1613178566444 epoch sec; seqNo:19033; depth 19.27 2021-02-13T01:09:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178566.45 epoch sec; seqNo:19033; 4060971.05 north; 593768.40 east 2021-02-13T01:09:26Z- AHRS_M2 msg: 1613178566.85 epoch sec; seqNo:18883 2021-02-13T01:09:26Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.16 psi 2021-02-13T01:09:26Z- handleDepth()-Depth_Keller msg: 1613178566853 epoch sec; seqNo:19034; depth 19.17 2021-02-13T01:09:26Z- handleDvl() - RDI_Pathfinder msg: 1613178566.87 epoch sec; seqNo:9180 2021-02-13T01:09:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.62 , 98.26 , 100.70 , 71.12 2021-02-13T01:09:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , 0.01 2021-02-13T01:09:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178566.87 epoch sec; seqNo:19034; 4060971.43 north; 593768.40 east 2021-02-13T01:09:27Z- AHRS_M2 msg: 1613178567.25 epoch sec; seqNo:18884 2021-02-13T01:09:27Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.16 psi 2021-02-13T01:09:27Z- handleDepth()-Depth_Keller msg: 1613178567253 epoch sec; seqNo:19035; depth 19.07 2021-02-13T01:09:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178567.27 epoch sec; seqNo:19035; 4060971.81 north; 593768.40 east 2021-02-13T01:09:27Z- AHRS_M2 msg: 1613178567.65 epoch sec; seqNo:18885 2021-02-13T01:09:27Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.15 psi 2021-02-13T01:09:27Z- handleDepth()-Depth_Keller msg: 1613178567661 epoch sec; seqNo:19036; depth 18.97 2021-02-13T01:09:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178567.67 epoch sec; seqNo:19036; 4060971.81 north; 593768.40 east 2021-02-13T01:09:28Z- AHRS_M2 msg: 1613178568.06 epoch sec; seqNo:18886 2021-02-13T01:09:28Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.15 psi 2021-02-13T01:09:28Z- handleDepth()-Depth_Keller msg: 1613178568063 epoch sec; seqNo:19037; depth 18.88 2021-02-13T01:09:28Z- handleDvl() - RDI_Pathfinder msg: 1613178568.08 epoch sec; seqNo:9181 2021-02-13T01:09:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.62 , 93.19 , 102.93 , 70.31 2021-02-13T01:09:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.02 , 0.08 2021-02-13T01:09:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178568.09 epoch sec; seqNo:19037; 4060972.20 north; 593769.61 east 2021-02-13T01:09:28Z- LcmTrn::handleMessages() - handling 44 messages took 0.77 ms and 129 calls... 2021-02-13T01:09:28Z- waiting for 1613178565.84, time is 1613178568.09 2021-02-13T01:09:28Z- LcmTrn::updateTrn() equating pose 1613178568.09 and meas times 1613178568.09 2021-02-13T01:09:28Z- LcmTrn::updateTrn() >>>> pose time 1613178568.09, meas time 1613178568.09 <<<< 2021-02-13T01:09:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178568.09 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:09:28Z- TerrainNav::motionUpdate() - currEstimate = (4060972, 593769.61, 18.88) 2021-02-13T01:09:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:09:28Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:09:28Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178568.09 sec.; No relevant navigation data available 2021-02-13T01:09:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:09:28Z- TNavBF::mmse selected from filter -1 2021-02-13T01:09:28Z- variances([0], [2], and psi_berg) = 674.049037 832.986031 0.000000 2021-02-13T01:09:28Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:09:28Z- LcmTrn::publishEstimates() - MLE : 4060971.70 593784.11 18.88 2021-02-13T01:09:28Z- LcmTrn::publishEstimates() - MMSE : 4060972.20 593769.61 18.88 2021-02-13T01:09:28Z- LcmTrn::publishEstimates() - COVAR: 674.05 832.99 1.06 0.00 2021-02-13T01:09:28Z- AHRS_M2 msg: 1613178568.46 epoch sec; seqNo:18887 2021-02-13T01:09:28Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.14 psi 2021-02-13T01:09:28Z- handleDepth()-Depth_Keller msg: 1613178568464 epoch sec; seqNo:19038; depth 18.77 2021-02-13T01:09:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178568.48 epoch sec; seqNo:19038; 4060972.58 north; 593769.61 east 2021-02-13T01:09:28Z- AHRS_M2 msg: 1613178568.87 epoch sec; seqNo:18888 2021-02-13T01:09:28Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.14 psi 2021-02-13T01:09:28Z- handleDepth()-Depth_Keller msg: 1613178568895 epoch sec; seqNo:19039; depth 18.68 2021-02-13T01:09:28Z- handleDvl() - RDI_Pathfinder msg: 1613178568.91 epoch sec; seqNo:9182 2021-02-13T01:09:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.85 , 98.13 , 101.78 , 0.00 2021-02-13T01:09:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , 0.06 2021-02-13T01:09:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178568.92 epoch sec; seqNo:19039; 4060972.96 north; 593769.60 east 2021-02-13T01:09:29Z- AHRS_M2 msg: 1613178569.27 epoch sec; seqNo:18889 2021-02-13T01:09:29Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.13 psi 2021-02-13T01:09:29Z- handleDepth()-Depth_Keller msg: 1613178569272 epoch sec; seqNo:19040; depth 18.57 2021-02-13T01:09:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178569.28 epoch sec; seqNo:19040; 4060972.96 north; 593769.60 east 2021-02-13T01:09:29Z- AHRS_M2 msg: 1613178569.67 epoch sec; seqNo:18890 2021-02-13T01:09:29Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.13 psi 2021-02-13T01:09:29Z- handleDepth()-Depth_Keller msg: 1613178569677 epoch sec; seqNo:19041; depth 18.49 2021-02-13T01:09:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178569.69 epoch sec; seqNo:19041; 4060973.34 north; 593769.60 east 2021-02-13T01:09:30Z- AHRS_M2 msg: 1613178570.08 epoch sec; seqNo:18891 2021-02-13T01:09:30Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.12 psi 2021-02-13T01:09:30Z- handleDepth()-Depth_Keller msg: 1613178570085 epoch sec; seqNo:19042; depth 18.41 2021-02-13T01:09:30Z- handleDvl() - RDI_Pathfinder msg: 1613178570.10 epoch sec; seqNo:9183 2021-02-13T01:09:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.89 , 89.61 , 103.13 , 68.67 2021-02-13T01:09:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.07 , -0.05 2021-02-13T01:09:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178570.11 epoch sec; seqNo:19042; 4060973.72 north; 593769.60 east 2021-02-13T01:09:30Z- AHRS_M2 msg: 1613178570.48 epoch sec; seqNo:18892 2021-02-13T01:09:30Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.12 psi 2021-02-13T01:09:30Z- handleDepth()-Depth_Keller msg: 1613178570489 epoch sec; seqNo:19043; depth 18.32 2021-02-13T01:09:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178570.50 epoch sec; seqNo:19043; 4060973.72 north; 593769.60 east 2021-02-13T01:09:30Z- AHRS_M2 msg: 1613178570.89 epoch sec; seqNo:18893 2021-02-13T01:09:30Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.12 psi 2021-02-13T01:09:30Z- handleDepth()-Depth_Keller msg: 1613178570893 epoch sec; seqNo:19044; depth 18.20 2021-02-13T01:09:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178570.91 epoch sec; seqNo:19044; 4060974.11 north; 593770.81 east 2021-02-13T01:09:31Z- AHRS_M2 msg: 1613178571.29 epoch sec; seqNo:18894 2021-02-13T01:09:31Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.12 psi 2021-02-13T01:09:31Z- handleDepth()-Depth_Keller msg: 1613178571297 epoch sec; seqNo:19045; depth 18.11 2021-02-13T01:09:31Z- handleDvl() - RDI_Pathfinder msg: 1613178571.31 epoch sec; seqNo:9184 2021-02-13T01:09:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.72 , 89.67 , 103.06 , 78.72 2021-02-13T01:09:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.03 , -0.14 2021-02-13T01:09:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178571.32 epoch sec; seqNo:19045; 4060974.49 north; 593770.81 east 2021-02-13T01:09:31Z- AHRS_M2 msg: 1613178571.69 epoch sec; seqNo:18895 2021-02-13T01:09:31Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.13 psi 2021-02-13T01:09:31Z- handleDepth()-Depth_Keller msg: 1613178571701 epoch sec; seqNo:19046; depth 18.01 2021-02-13T01:09:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178571.72 epoch sec; seqNo:19046; 4060974.49 north; 593770.81 east 2021-02-13T01:09:32Z- AHRS_M2 msg: 1613178572.10 epoch sec; seqNo:18896 2021-02-13T01:09:32Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.13 psi 2021-02-13T01:09:32Z- handleDepth()-Depth_Keller msg: 1613178572102 epoch sec; seqNo:19047; depth 17.92 2021-02-13T01:09:32Z- handleDvl() - RDI_Pathfinder msg: 1613178572.11 epoch sec; seqNo:9185 2021-02-13T01:09:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.67 , 97.99 , 104.08 , 71.12 2021-02-13T01:09:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.12 , -0.14 2021-02-13T01:09:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178572.14 epoch sec; seqNo:19047; 4060974.87 north; 593770.80 east 2021-02-13T01:09:32Z- AHRS_M2 msg: 1613178572.50 epoch sec; seqNo:18897 2021-02-13T01:09:32Z- AHRS_M2 msg: 0.64 phi; 0.30 theta; -0.14 psi 2021-02-13T01:09:32Z- handleDepth()-Depth_Keller msg: 1613178572509 epoch sec; seqNo:19048; depth 17.81 2021-02-13T01:09:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178572.53 epoch sec; seqNo:19048; 4060975.25 north; 593770.80 east 2021-02-13T01:09:32Z- AHRS_M2 msg: 1613178572.90 epoch sec; seqNo:18898 2021-02-13T01:09:32Z- AHRS_M2 msg: 0.63 phi; 0.30 theta; -0.15 psi 2021-02-13T01:09:32Z- handleDepth()-Depth_Keller msg: 1613178572915 epoch sec; seqNo:19049; depth 17.70 2021-02-13T01:09:32Z- handleDvl() - RDI_Pathfinder msg: 1613178572.93 epoch sec; seqNo:9186 2021-02-13T01:09:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.26 , 93.12 , 102.86 , 71.94 2021-02-13T01:09:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.18 , -0.02 2021-02-13T01:09:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178572.94 epoch sec; seqNo:19049; 4060975.63 north; 593770.80 east 2021-02-13T01:09:33Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-13T01:09:33Z- waiting for 1613178571.09, time is 1613178572.94 2021-02-13T01:09:33Z- LcmTrn::updateTrn() equating pose 1613178572.94 and meas times 1613178572.94 2021-02-13T01:09:33Z- LcmTrn::updateTrn() >>>> pose time 1613178572.94, meas time 1613178572.94 <<<< 2021-02-13T01:09:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178572.94 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:09:33Z- TerrainNav::motionUpdate() - currEstimate = (4060976, 593770.80, 17.70) 2021-02-13T01:09:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:09:33Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:09:33Z- TNavBF::Projecting Measurements 2021-02-13T01:09:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:09:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:09:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:09:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:09:33Z- TNavFilter::4 good beams. 2021-02-13T01:09:33Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:09:33Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:09:33Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:09:33Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:09:33Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:09:33Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:09:33Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:09:33Z- TNavBF::mmse selected from filter -1 2021-02-13T01:09:33Z- TerrainMapOctree: (vehN, vehE ) = (4060975.63, 593770.80). 2021-02-13T01:09:33Z- TerrainMapOctree: Min Distance = 552.19. 2021-02-13T01:09:33Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:09:33Z- There is no specific good beam that all particles have in common. 2021-02-13T01:09:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:09:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.25 K: 0.100 ExpectedMeasDiff: nan currMeas.covariance: 9.55 2021-02-13T01:09:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:09:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 13.87 2021-02-13T01:09:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:09:33Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 16.93 2021-02-13T01:09:33Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:09:33Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.090 ExpectedMeasDiff: nan currMeas.covariance: 8.28 2021-02-13T01:09:33Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:09:33Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:09:33Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:09:33Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:09:33Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:33Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:09:33Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:09:33Z- Filter 0 No beams used in subcloud update 2021-02-13T01:09:33Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:09:33Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:09:33Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 674.0 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:09:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178572.94 sec. 2021-02-13T01:09:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:09:33Z- TNavBF::mmse selected from filter -1 2021-02-13T01:09:33Z- variances([0], [2], and psi_berg) = 674.009752 832.986016 0.000000 2021-02-13T01:09:33Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:09:33Z- LcmTrn::publishEstimates() - MLE : 4060975.13 593785.30 17.70 2021-02-13T01:09:33Z- LcmTrn::publishEstimates() - MMSE : 4060975.63 593770.80 17.70 2021-02-13T01:09:33Z- LcmTrn::publishEstimates() - COVAR: 674.01 832.99 0.90 0.00 2021-02-13T01:09:33Z- AHRS_M2 msg: 1613178573.31 epoch sec; seqNo:18899 2021-02-13T01:09:33Z- AHRS_M2 msg: 0.62 phi; 0.30 theta; -0.16 psi 2021-02-13T01:09:33Z- handleDepth()-Depth_Keller msg: 1613178573321 epoch sec; seqNo:19050; depth 17.62 2021-02-13T01:09:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178573.34 epoch sec; seqNo:19050; 4060975.63 north; 593770.80 east 2021-02-13T01:09:33Z- AHRS_M2 msg: 1613178573.72 epoch sec; seqNo:18900 2021-02-13T01:09:33Z- AHRS_M2 msg: 0.61 phi; 0.30 theta; -0.16 psi 2021-02-13T01:09:33Z- handleDepth()-Depth_Keller msg: 1613178573723 epoch sec; seqNo:19051; depth 17.53 2021-02-13T01:09:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178573.73 epoch sec; seqNo:19051; 4060976.01 north; 593770.79 east 2021-02-13T01:09:34Z- AHRS_M2 msg: 1613178574.12 epoch sec; seqNo:18901 2021-02-13T01:09:34Z- AHRS_M2 msg: 0.60 phi; 0.30 theta; -0.16 psi 2021-02-13T01:09:34Z- handleDepth()-Depth_Keller msg: 1613178574125 epoch sec; seqNo:19052; depth 17.43 2021-02-13T01:09:34Z- handleDvl() - RDI_Pathfinder msg: 1613178574.14 epoch sec; seqNo:9187 2021-02-13T01:09:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 98.06 , 101.71 , 78.58 2021-02-13T01:09:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.14 , 0.19 2021-02-13T01:09:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178574.15 epoch sec; seqNo:19052; 4060976.40 north; 593772.01 east 2021-02-13T01:09:34Z- AHRS_M2 msg: 1613178574.52 epoch sec; seqNo:18902 2021-02-13T01:09:34Z- AHRS_M2 msg: 0.59 phi; 0.30 theta; -0.16 psi 2021-02-13T01:09:34Z- handleDepth()-Depth_Keller msg: 1613178574524 epoch sec; seqNo:19053; depth 17.33 2021-02-13T01:09:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178574.54 epoch sec; seqNo:19053; 4060976.78 north; 593772.00 east 2021-02-13T01:09:34Z- AHRS_M2 msg: 1613178574.92 epoch sec; seqNo:18903 2021-02-13T01:09:34Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.15 psi 2021-02-13T01:09:34Z- handleDepth()-Depth_Keller msg: 1613178574929 epoch sec; seqNo:19054; depth 17.23 2021-02-13T01:09:34Z- handleDvl() - RDI_Pathfinder msg: 1613178574.94 epoch sec; seqNo:9188 2021-02-13T01:09:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.65 , 103.06 , 71.12 2021-02-13T01:09:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.07 , 0.30 2021-02-13T01:09:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178574.96 epoch sec; seqNo:19054; 4060977.16 north; 593772.00 east 2021-02-13T01:09:35Z- AHRS_M2 msg: 1613178575.33 epoch sec; seqNo:18904 2021-02-13T01:09:35Z- AHRS_M2 msg: 0.58 phi; 0.31 theta; -0.15 psi 2021-02-13T01:09:35Z- handleDepth()-Depth_Keller msg: 1613178575337 epoch sec; seqNo:19055; depth 17.14 2021-02-13T01:09:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178575.35 epoch sec; seqNo:19055; 4060977.16 north; 593772.00 east 2021-02-13T01:09:35Z- AHRS_M2 msg: 1613178575.73 epoch sec; seqNo:18905 2021-02-13T01:09:35Z- AHRS_M2 msg: 0.57 phi; 0.31 theta; -0.16 psi 2021-02-13T01:09:35Z- handleDepth()-Depth_Keller msg: 1613178575737 epoch sec; seqNo:19056; depth 17.04 2021-02-13T01:09:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178575.74 epoch sec; seqNo:19056; 4060977.53 north; 593772.00 east 2021-02-13T01:09:36Z- AHRS_M2 msg: 1613178576.14 epoch sec; seqNo:18906 2021-02-13T01:09:36Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.16 psi 2021-02-13T01:09:36Z- handleDepth()-Depth_Keller msg: 1613178576145 epoch sec; seqNo:19057; depth 16.94 2021-02-13T01:09:36Z- handleDvl() - RDI_Pathfinder msg: 1613178576.17 epoch sec; seqNo:9189 2021-02-13T01:09:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.30 , 94.07 , 105.02 , 71.12 2021-02-13T01:09:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.23 , 0.22 2021-02-13T01:09:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178576.18 epoch sec; seqNo:19057; 4060977.93 north; 593773.21 east 2021-02-13T01:09:36Z- AHRS_M2 msg: 1613178576.54 epoch sec; seqNo:18907 2021-02-13T01:09:36Z- AHRS_M2 msg: 0.57 phi; 0.32 theta; -0.16 psi 2021-02-13T01:09:36Z- handleDepth()-Depth_Keller msg: 1613178576549 epoch sec; seqNo:19058; depth 16.82 2021-02-13T01:09:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178576.56 epoch sec; seqNo:19058; 4060978.31 north; 593773.21 east 2021-02-13T01:09:36Z- AHRS_M2 msg: 1613178576.94 epoch sec; seqNo:18908 2021-02-13T01:09:36Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.16 psi 2021-02-13T01:09:36Z- handleDepth()-Depth_Keller msg: 1613178576957 epoch sec; seqNo:19059; depth 16.74 2021-02-13T01:09:36Z- handleDvl() - RDI_Pathfinder msg: 1613178576.97 epoch sec; seqNo:9190 2021-02-13T01:09:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 94.34 , 106.51 , 73.17 2021-02-13T01:09:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.32 , -0.02 2021-02-13T01:09:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178576.98 epoch sec; seqNo:19059; 4060978.31 north; 593773.21 east 2021-02-13T01:09:37Z- AHRS_M2 msg: 1613178577.35 epoch sec; seqNo:18909 2021-02-13T01:09:37Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.17 psi 2021-02-13T01:09:37Z- handleDepth()-Depth_Keller msg: 1613178577354 epoch sec; seqNo:19060; depth 16.64 2021-02-13T01:09:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178577.37 epoch sec; seqNo:19060; 4060978.68 north; 593773.20 east 2021-02-13T01:09:37Z- AHRS_M2 msg: 1613178577.75 epoch sec; seqNo:18910 2021-02-13T01:09:37Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.17 psi 2021-02-13T01:09:37Z- handleDepth()-Depth_Keller msg: 1613178577761 epoch sec; seqNo:19061; depth 16.55 2021-02-13T01:09:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178577.78 epoch sec; seqNo:19061; 4060978.70 north; 593774.42 east 2021-02-13T01:09:38Z- AHRS_M2 msg: 1613178578.16 epoch sec; seqNo:18911 2021-02-13T01:09:38Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.17 psi 2021-02-13T01:09:38Z- handleDepth()-Depth_Keller msg: 1613178578169 epoch sec; seqNo:19062; depth 16.43 2021-02-13T01:09:38Z- handleDvl() - RDI_Pathfinder msg: 1613178578.18 epoch sec; seqNo:9191 2021-02-13T01:09:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.61 , 97.65 , 106.17 , 71.94 2021-02-13T01:09:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.23 , -0.25 2021-02-13T01:09:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178578.20 epoch sec; seqNo:19062; 4060979.08 north; 593774.42 east 2021-02-13T01:09:38Z- LcmTrn::handleMessages() - handling 44 messages took 0.68 ms and 131 calls... 2021-02-13T01:09:38Z- waiting for 1613178575.94, time is 1613178578.20 2021-02-13T01:09:38Z- LcmTrn::updateTrn() equating pose 1613178578.20 and meas times 1613178578.20 2021-02-13T01:09:38Z- LcmTrn::updateTrn() >>>> pose time 1613178578.20, meas time 1613178578.20 <<<< 2021-02-13T01:09:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178578.20 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:09:38Z- TerrainNav::motionUpdate() - currEstimate = (4060979, 593774.42, 16.43) 2021-02-13T01:09:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:09:38Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:09:38Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178578.20 sec.; No relevant navigation data available 2021-02-13T01:09:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:09:38Z- TNavBF::mmse selected from filter -1 2021-02-13T01:09:38Z- variances([0], [2], and psi_berg) = 673.963596 832.985997 0.000000 2021-02-13T01:09:38Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:09:38Z- LcmTrn::publishEstimates() - MLE : 4060978.58 593788.92 16.43 2021-02-13T01:09:38Z- LcmTrn::publishEstimates() - MMSE : 4060979.08 593774.42 16.43 2021-02-13T01:09:38Z- LcmTrn::publishEstimates() - COVAR: 673.96 832.99 1.16 0.00 2021-02-13T01:09:38Z- AHRS_M2 msg: 1613178578.56 epoch sec; seqNo:18912 2021-02-13T01:09:38Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-13T01:09:38Z- handleDepth()-Depth_Keller msg: 1613178578568 epoch sec; seqNo:19063; depth 16.32 2021-02-13T01:09:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178578.58 epoch sec; seqNo:19063; 4060979.08 north; 593774.42 east 2021-02-13T01:09:38Z- AHRS_M2 msg: 1613178578.96 epoch sec; seqNo:18913 2021-02-13T01:09:38Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.17 psi 2021-02-13T01:09:38Z- handleDepth()-Depth_Keller msg: 1613178578977 epoch sec; seqNo:19064; depth 16.19 2021-02-13T01:09:38Z- handleDvl() - RDI_Pathfinder msg: 1613178578.99 epoch sec; seqNo:9192 2021-02-13T01:09:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.67 , 96.64 , 107.59 , 72.35 2021-02-13T01:09:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.02 , -0.33 2021-02-13T01:09:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178579.00 epoch sec; seqNo:19064; 4060979.46 north; 593774.42 east 2021-02-13T01:09:39Z- AHRS_M2 msg: 1613178579.37 epoch sec; seqNo:18914 2021-02-13T01:09:39Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-13T01:09:39Z- handleDepth()-Depth_Keller msg: 1613178579386 epoch sec; seqNo:19065; depth 16.09 2021-02-13T01:09:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178579.40 epoch sec; seqNo:19065; 4060979.83 north; 593774.41 east 2021-02-13T01:09:39Z- AHRS_M2 msg: 1613178579.77 epoch sec; seqNo:18915 2021-02-13T01:09:39Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.17 psi 2021-02-13T01:09:39Z- handleDepth()-Depth_Keller msg: 1613178579777 epoch sec; seqNo:19066; depth 15.98 2021-02-13T01:09:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178579.79 epoch sec; seqNo:19066; 4060979.83 north; 593774.41 east 2021-02-13T01:09:40Z- AHRS_M2 msg: 1613178580.18 epoch sec; seqNo:18916 2021-02-13T01:09:40Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-13T01:09:40Z- handleDepth()-Depth_Keller msg: 1613178580180 epoch sec; seqNo:19067; depth 15.88 2021-02-13T01:09:40Z- handleDvl() - RDI_Pathfinder msg: 1613178580.19 epoch sec; seqNo:9193 2021-02-13T01:09:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.30 , 96.71 , 107.66 , 73.58 2021-02-13T01:09:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.15 , -0.21 2021-02-13T01:09:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178580.21 epoch sec; seqNo:19067; 4060980.21 north; 593774.41 east 2021-02-13T01:09:40Z- AHRS_M2 msg: 1613178580.58 epoch sec; seqNo:18917 2021-02-13T01:09:40Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.15 psi 2021-02-13T01:09:40Z- handleDepth()-Depth_Keller msg: 1613178580585 epoch sec; seqNo:19068; depth 15.76 2021-02-13T01:09:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178580.60 epoch sec; seqNo:19068; 4060980.61 north; 593775.62 east 2021-02-13T01:09:40Z- AHRS_M2 msg: 1613178580.98 epoch sec; seqNo:18918 2021-02-13T01:09:40Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.15 psi 2021-02-13T01:09:40Z- handleDepth()-Depth_Keller msg: 1613178580987 epoch sec; seqNo:19069; depth 15.66 2021-02-13T01:09:41Z- handleDvl() - RDI_Pathfinder msg: 1613178581.01 epoch sec; seqNo:9194 2021-02-13T01:09:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.81 , 96.64 , 106.38 , 72.76 2021-02-13T01:09:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.20 , -0.03 2021-02-13T01:09:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178581.02 epoch sec; seqNo:19069; 4060980.98 north; 593775.62 east 2021-02-13T01:09:41Z- AHRS_M2 msg: 1613178581.39 epoch sec; seqNo:18919 2021-02-13T01:09:41Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-13T01:09:41Z- handleDepth()-Depth_Keller msg: 1613178581392 epoch sec; seqNo:19070; depth 15.53 2021-02-13T01:09:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178581.40 epoch sec; seqNo:19070; 4060980.98 north; 593775.62 east 2021-02-13T01:09:41Z- AHRS_M2 msg: 1613178581.79 epoch sec; seqNo:18920 2021-02-13T01:09:41Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.15 psi 2021-02-13T01:09:41Z- handleDepth()-Depth_Keller msg: 1613178581801 epoch sec; seqNo:19071; depth 15.43 2021-02-13T01:09:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178581.81 epoch sec; seqNo:19071; 4060981.36 north; 593775.62 east 2021-02-13T01:09:42Z- AHRS_M2 msg: 1613178582.20 epoch sec; seqNo:18921 2021-02-13T01:09:42Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T01:09:42Z- handleDepth()-Depth_Keller msg: 1613178582211 epoch sec; seqNo:19072; depth 15.32 2021-02-13T01:09:42Z- handleDvl() - RDI_Pathfinder msg: 1613178582.22 epoch sec; seqNo:9195 2021-02-13T01:09:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.03 , 97.86 , 106.38 , 72.35 2021-02-13T01:09:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.13 , 0.12 2021-02-13T01:09:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178582.23 epoch sec; seqNo:19072; 4060981.74 north; 593775.61 east 2021-02-13T01:09:42Z- AHRS_M2 msg: 1613178582.60 epoch sec; seqNo:18922 2021-02-13T01:09:42Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T01:09:42Z- handleDepth()-Depth_Keller msg: 1613178582604 epoch sec; seqNo:19073; depth 15.21 2021-02-13T01:09:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178582.62 epoch sec; seqNo:19073; 4060982.12 north; 593775.61 east 2021-02-13T01:09:43Z- AHRS_M2 msg: 1613178583.00 epoch sec; seqNo:18923 2021-02-13T01:09:43Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.16 psi 2021-02-13T01:09:43Z- handleDepth()-Depth_Keller msg: 1613178583013 epoch sec; seqNo:19074; depth 15.10 2021-02-13T01:09:43Z- handleDvl() - RDI_Pathfinder msg: 1613178583.02 epoch sec; seqNo:9196 2021-02-13T01:09:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.96 , 96.57 , 106.31 , 73.17 2021-02-13T01:09:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , 0.16 2021-02-13T01:09:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178583.04 epoch sec; seqNo:19074; 4060982.50 north; 593775.60 east 2021-02-13T01:09:43Z- LcmTrn::handleMessages() - handling 41 messages took 0.62 ms and 129 calls... 2021-02-13T01:09:43Z- waiting for 1613178581.20, time is 1613178583.04 2021-02-13T01:09:43Z- LcmTrn::updateTrn() equating pose 1613178583.04 and meas times 1613178583.04 2021-02-13T01:09:43Z- LcmTrn::updateTrn() >>>> pose time 1613178583.04, meas time 1613178583.04 <<<< 2021-02-13T01:09:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178583.04 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:09:43Z- TerrainNav::motionUpdate() - currEstimate = (4060982, 593775.60, 15.10) 2021-02-13T01:09:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:09:43Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:09:43Z- TNavBF::Projecting Measurements 2021-02-13T01:09:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:09:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:09:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:09:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:09:43Z- TNavFilter::4 good beams. 2021-02-13T01:09:43Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:09:43Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:09:43Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:09:43Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:09:43Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:09:43Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:09:43Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:09:43Z- TNavBF::mmse selected from filter -1 2021-02-13T01:09:43Z- TerrainMapOctree: (vehN, vehE ) = (4060982.50, 593775.60). 2021-02-13T01:09:43Z- TerrainMapOctree: Min Distance = 544.62. 2021-02-13T01:09:43Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:09:43Z- There is no specific good beam that all particles have in common. 2021-02-13T01:09:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:09:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.34 K: 0.097 ExpectedMeasDiff: nan currMeas.covariance: 10.75 2021-02-13T01:09:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:09:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.068 ExpectedMeasDiff: nan currMeas.covariance: 14.92 2021-02-13T01:09:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:09:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.13 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 18.08 2021-02-13T01:09:43Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:09:43Z- Old Depth Bias: 0.00 DepthBiasCov: 1.07 K: 0.094 ExpectedMeasDiff: nan currMeas.covariance: 8.57 2021-02-13T01:09:43Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:09:43Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:09:43Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:09:43Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:09:43Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:43Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:09:43Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:09:43Z- Filter 0 No beams used in subcloud update 2021-02-13T01:09:43Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:09:43Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:09:43Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 673.9 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T01:09:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178583.04 sec. 2021-02-13T01:09:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:09:43Z- TNavBF::mmse selected from filter -1 2021-02-13T01:09:43Z- variances([0], [2], and psi_berg) = 673.924346 832.985982 0.000000 2021-02-13T01:09:43Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:09:43Z- LcmTrn::publishEstimates() - MLE : 4060982.00 593790.10 15.10 2021-02-13T01:09:43Z- LcmTrn::publishEstimates() - MMSE : 4060982.50 593775.60 15.10 2021-02-13T01:09:43Z- LcmTrn::publishEstimates() - COVAR: 673.92 832.99 0.97 0.00 2021-02-13T01:09:43Z- AHRS_M2 msg: 1613178583.41 epoch sec; seqNo:18924 2021-02-13T01:09:43Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.16 psi 2021-02-13T01:09:43Z- handleDepth()-Depth_Keller msg: 1613178583412 epoch sec; seqNo:19075; depth 14.99 2021-02-13T01:09:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178583.42 epoch sec; seqNo:19075; 4060982.50 north; 593775.60 east 2021-02-13T01:09:43Z- AHRS_M2 msg: 1613178583.81 epoch sec; seqNo:18925 2021-02-13T01:09:43Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T01:09:43Z- handleDepth()-Depth_Keller msg: 1613178583817 epoch sec; seqNo:19076; depth 14.89 2021-02-13T01:09:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178583.83 epoch sec; seqNo:19076; 4060982.89 north; 593776.82 east 2021-02-13T01:09:44Z- AHRS_M2 msg: 1613178584.22 epoch sec; seqNo:18926 2021-02-13T01:09:44Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T01:09:44Z- handleDepth()-Depth_Keller msg: 1613178584221 epoch sec; seqNo:19077; depth 14.78 2021-02-13T01:09:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178584.26 epoch sec; seqNo:19077; 4060983.27 north; 593776.81 east 2021-02-13T01:09:44Z- AHRS_M2 msg: 1613178584.62 epoch sec; seqNo:18927 2021-02-13T01:09:44Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.16 psi 2021-02-13T01:09:44Z- handleDepth()-Depth_Keller msg: 1613178584628 epoch sec; seqNo:19078; depth 14.69 2021-02-13T01:09:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178584.64 epoch sec; seqNo:19078; 4060983.65 north; 593776.81 east 2021-02-13T01:09:45Z- AHRS_M2 msg: 1613178585.03 epoch sec; seqNo:18928 2021-02-13T01:09:45Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-13T01:09:45Z- handleDepth()-Depth_Keller msg: 1613178585030 epoch sec; seqNo:19079; depth 14.58 2021-02-13T01:09:45Z- handleDvl() - RDI_Pathfinder msg: 1613178585.04 epoch sec; seqNo:9197 2021-02-13T01:09:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 97.72 , 109.89 , 73.99 2021-02-13T01:09:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.10 , -0.03 2021-02-13T01:09:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178585.05 epoch sec; seqNo:19079; 4060983.65 north; 593776.81 east 2021-02-13T01:09:45Z- AHRS_M2 msg: 1613178585.43 epoch sec; seqNo:18929 2021-02-13T01:09:45Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.15 psi 2021-02-13T01:09:45Z- handleDepth()-Depth_Keller msg: 1613178585434 epoch sec; seqNo:19080; depth 14.46 2021-02-13T01:09:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178585.44 epoch sec; seqNo:19080; 4060984.03 north; 593776.81 east 2021-02-13T01:09:45Z- AHRS_M2 msg: 1613178585.83 epoch sec; seqNo:18930 2021-02-13T01:09:45Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.16 psi 2021-02-13T01:09:45Z- handleDepth()-Depth_Keller msg: 1613178585836 epoch sec; seqNo:19081; depth 14.37 2021-02-13T01:09:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178585.85 epoch sec; seqNo:19081; 4060984.42 north; 593778.02 east 2021-02-13T01:09:46Z- AHRS_M2 msg: 1613178586.24 epoch sec; seqNo:18931 2021-02-13T01:09:46Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.16 psi 2021-02-13T01:09:46Z- handleDepth()-Depth_Keller msg: 1613178586249 epoch sec; seqNo:19082; depth 14.27 2021-02-13T01:09:46Z- handleDvl() - RDI_Pathfinder msg: 1613178586.26 epoch sec; seqNo:9198 2021-02-13T01:09:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.71 , 96.23 , 109.76 , 73.58 2021-02-13T01:09:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , -0.10 2021-02-13T01:09:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178586.27 epoch sec; seqNo:19082; 4060984.42 north; 593778.02 east 2021-02-13T01:09:46Z- AHRS_M2 msg: 1613178586.64 epoch sec; seqNo:18932 2021-02-13T01:09:46Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-13T01:09:46Z- handleDepth()-Depth_Keller msg: 1613178586646 epoch sec; seqNo:19083; depth 14.15 2021-02-13T01:09:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178586.66 epoch sec; seqNo:19083; 4060984.80 north; 593778.02 east 2021-02-13T01:09:47Z- AHRS_M2 msg: 1613178587.04 epoch sec; seqNo:18933 2021-02-13T01:09:47Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-13T01:09:47Z- handleDepth()-Depth_Keller msg: 1613178587049 epoch sec; seqNo:19084; depth 14.07 2021-02-13T01:09:47Z- handleDvl() - RDI_Pathfinder msg: 1613178587.06 epoch sec; seqNo:9199 2021-02-13T01:09:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.90 , 96.50 , 109.89 , 74.40 2021-02-13T01:09:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , -0.05 2021-02-13T01:09:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178587.09 epoch sec; seqNo:19084; 4060985.18 north; 593778.01 east 2021-02-13T01:09:47Z- AHRS_M2 msg: 1613178587.45 epoch sec; seqNo:18934 2021-02-13T01:09:47Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.17 psi 2021-02-13T01:09:47Z- handleDepth()-Depth_Keller msg: 1613178587452 epoch sec; seqNo:19085; depth 13.95 2021-02-13T01:09:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178587.46 epoch sec; seqNo:19085; 4060985.18 north; 593778.01 east 2021-02-13T01:09:47Z- AHRS_M2 msg: 1613178587.85 epoch sec; seqNo:18935 2021-02-13T01:09:47Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.17 psi 2021-02-13T01:09:47Z- handleDepth()-Depth_Keller msg: 1613178587857 epoch sec; seqNo:19086; depth 13.84 2021-02-13T01:09:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178587.87 epoch sec; seqNo:19086; 4060985.56 north; 593778.01 east 2021-02-13T01:09:48Z- AHRS_M2 msg: 1613178588.26 epoch sec; seqNo:18936 2021-02-13T01:09:48Z- AHRS_M2 msg: 0.61 phi; 0.32 theta; -0.17 psi 2021-02-13T01:09:48Z- handleDepth()-Depth_Keller msg: 1613178588260 epoch sec; seqNo:19087; depth 13.75 2021-02-13T01:09:48Z- handleDvl() - RDI_Pathfinder msg: 1613178588.28 epoch sec; seqNo:9200 2021-02-13T01:09:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 81.90 , 96.50 , 109.89 , 73.58 2021-02-13T01:09:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , 0.07 2021-02-13T01:09:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178588.29 epoch sec; seqNo:19087; 4060985.94 north; 593778.01 east 2021-02-13T01:09:48Z- LcmTrn::handleMessages() - handling 43 messages took 0.66 ms and 130 calls... 2021-02-13T01:09:48Z- waiting for 1613178586.04, time is 1613178588.29 2021-02-13T01:09:48Z- LcmTrn::updateTrn() equating pose 1613178588.29 and meas times 1613178588.29 2021-02-13T01:09:48Z- LcmTrn::updateTrn() >>>> pose time 1613178588.29, meas time 1613178588.29 <<<< 2021-02-13T01:09:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178588.29 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:09:48Z- TerrainNav::motionUpdate() - currEstimate = (4060986, 593778.01, 13.75) 2021-02-13T01:09:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:09:48Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:09:48Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178588.29 sec.; No relevant navigation data available 2021-02-13T01:09:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:09:48Z- TNavBF::mmse selected from filter -1 2021-02-13T01:09:48Z- variances([0], [2], and psi_berg) = 673.882127 832.985965 0.000000 2021-02-13T01:09:48Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:09:48Z- LcmTrn::publishEstimates() - MLE : 4060985.44 593794.51 13.75 2021-02-13T01:09:48Z- LcmTrn::publishEstimates() - MMSE : 4060985.94 593778.01 13.75 2021-02-13T01:09:48Z- LcmTrn::publishEstimates() - COVAR: 673.88 832.99 1.18 0.00 2021-02-13T01:09:48Z- AHRS_M2 msg: 1613178588.66 epoch sec; seqNo:18937 2021-02-13T01:09:48Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.16 psi 2021-02-13T01:09:48Z- handleDepth()-Depth_Keller msg: 1613178588664 epoch sec; seqNo:19088; depth 13.65 2021-02-13T01:09:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178588.67 epoch sec; seqNo:19088; 4060986.33 north; 593779.22 east 2021-02-13T01:09:49Z- AHRS_M2 msg: 1613178589.06 epoch sec; seqNo:18938 2021-02-13T01:09:49Z- AHRS_M2 msg: 0.60 phi; 0.32 theta; -0.15 psi 2021-02-13T01:09:49Z- handleDepth()-Depth_Keller msg: 1613178589074 epoch sec; seqNo:19089; depth 13.56 2021-02-13T01:09:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178589.10 epoch sec; seqNo:19089; 4060986.33 north; 593779.22 east 2021-02-13T01:09:49Z- AHRS_M2 msg: 1613178589.47 epoch sec; seqNo:18939 2021-02-13T01:09:49Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-13T01:09:49Z- handleDepth()-Depth_Keller msg: 1613178589472 epoch sec; seqNo:19090; depth 13.43 2021-02-13T01:09:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178589.48 epoch sec; seqNo:19090; 4060986.71 north; 593779.22 east 2021-02-13T01:09:49Z- AHRS_M2 msg: 1613178589.87 epoch sec; seqNo:18940 2021-02-13T01:09:49Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.15 psi 2021-02-13T01:09:49Z- handleDepth()-Depth_Keller msg: 1613178589876 epoch sec; seqNo:19091; depth 13.34 2021-02-13T01:09:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178589.89 epoch sec; seqNo:19091; 4060987.09 north; 593779.21 east 2021-02-13T01:09:50Z- AHRS_M2 msg: 1613178590.28 epoch sec; seqNo:18941 2021-02-13T01:09:50Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.16 psi 2021-02-13T01:09:50Z- handleDepth()-Depth_Keller msg: 1613178590293 epoch sec; seqNo:19092; depth 13.23 2021-02-13T01:09:50Z- handleDvl() - RDI_Pathfinder msg: 1613178590.31 epoch sec; seqNo:9201 2021-02-13T01:09:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 96.37 , 111.25 , 78.79 2021-02-13T01:09:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.19 , 0.00 2021-02-13T01:09:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178590.32 epoch sec; seqNo:19092; 4060987.09 north; 593779.21 east 2021-02-13T01:09:50Z- AHRS_M2 msg: 1613178590.68 epoch sec; seqNo:18942 2021-02-13T01:09:50Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.16 psi 2021-02-13T01:09:50Z- handleDepth()-Depth_Keller msg: 1613178590689 epoch sec; seqNo:19093; depth 13.12 2021-02-13T01:09:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178590.70 epoch sec; seqNo:19093; 4060987.47 north; 593779.21 east 2021-02-13T01:09:51Z- AHRS_M2 msg: 1613178591.08 epoch sec; seqNo:18943 2021-02-13T01:09:51Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-13T01:09:51Z- handleDepth()-Depth_Keller msg: 1613178591093 epoch sec; seqNo:19094; depth 12.99 2021-02-13T01:09:51Z- handleDvl() - RDI_Pathfinder msg: 1613178591.11 epoch sec; seqNo:9202 2021-02-13T01:09:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.19 , 99.07 , 112.60 , 78.79 2021-02-13T01:09:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.14 , -0.15 2021-02-13T01:09:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178591.12 epoch sec; seqNo:19094; 4060987.86 north; 593780.43 east 2021-02-13T01:09:51Z- AHRS_M2 msg: 1613178591.49 epoch sec; seqNo:18944 2021-02-13T01:09:51Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-13T01:09:51Z- handleDepth()-Depth_Keller msg: 1613178591498 epoch sec; seqNo:19095; depth 12.88 2021-02-13T01:09:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178591.51 epoch sec; seqNo:19095; 4060987.86 north; 593780.43 east 2021-02-13T01:09:51Z- AHRS_M2 msg: 1613178591.90 epoch sec; seqNo:18945 2021-02-13T01:09:51Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.16 psi 2021-02-13T01:09:51Z- handleDepth()-Depth_Keller msg: 1613178591907 epoch sec; seqNo:19096; depth 12.78 2021-02-13T01:09:51Z- handleDvl() - RDI_Pathfinder msg: 1613178591.92 epoch sec; seqNo:9203 2021-02-13T01:09:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.06 , 98.94 , 112.46 , 75.62 2021-02-13T01:09:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.17 2021-02-13T01:09:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178591.93 epoch sec; seqNo:19096; 4060988.24 north; 593780.42 east 2021-02-13T01:09:52Z- AHRS_M2 msg: 1613178592.30 epoch sec; seqNo:18946 2021-02-13T01:09:52Z- AHRS_M2 msg: 0.63 phi; 0.33 theta; -0.15 psi 2021-02-13T01:09:52Z- handleDepth()-Depth_Keller msg: 1613178592300 epoch sec; seqNo:19097; depth 12.65 2021-02-13T01:09:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178592.31 epoch sec; seqNo:19097; 4060988.24 north; 593780.42 east 2021-02-13T01:09:52Z- AHRS_M2 msg: 1613178592.70 epoch sec; seqNo:18947 2021-02-13T01:09:52Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-13T01:09:52Z- handleDepth()-Depth_Keller msg: 1613178592709 epoch sec; seqNo:19098; depth 12.56 2021-02-13T01:09:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178592.72 epoch sec; seqNo:19098; 4060988.61 north; 593780.42 east 2021-02-13T01:09:53Z- AHRS_M2 msg: 1613178593.10 epoch sec; seqNo:18948 2021-02-13T01:09:53Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.15 psi 2021-02-13T01:09:53Z- handleDepth()-Depth_Keller msg: 1613178593110 epoch sec; seqNo:19099; depth 12.45 2021-02-13T01:09:53Z- handleDvl() - RDI_Pathfinder msg: 1613178593.13 epoch sec; seqNo:9204 2021-02-13T01:09:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 96.30 , 111.18 , 104.42 2021-02-13T01:09:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.26 , -0.62 , -0.41 2021-02-13T01:09:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178593.14 epoch sec; seqNo:19099; 4060988.99 north; 593780.41 east 2021-02-13T01:09:53Z- AHRS_M2 msg: 1613178593.51 epoch sec; seqNo:18949 2021-02-13T01:09:53Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.14 psi 2021-02-13T01:09:53Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-13T01:09:53Z- waiting for 1613178591.29, time is 1613178593.51 2021-02-13T01:09:53Z- LcmTrn::updateTrn() >>>> pose time 1613178593.51, meas time 1613178593.13 <<<< 2021-02-13T01:09:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178593.13 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:09:53Z- TerrainNav::motionUpdate() - currEstimate = (4060989, 593780.41, 12.45) 2021-02-13T01:09:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:09:53Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:09:53Z- TNavBF::Projecting Measurements 2021-02-13T01:09:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:09:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:09:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:09:53Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:09:53Z- TNavFilter::3 good beams. 2021-02-13T01:09:53Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:09:53Z- TNavBF:: beamIndex[0] = 1 2021-02-13T01:09:53Z- TNavBF:: beamIndex[1] = 2 2021-02-13T01:09:53Z- TNavBF:: beamIndex[2] = 3 2021-02-13T01:09:53Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:09:53Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:09:53Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:09:53Z- TNavBF::mmse selected from filter -1 2021-02-13T01:09:53Z- TerrainMapOctree: (vehN, vehE ) = (4060988.77, 593780.24). 2021-02-13T01:09:53Z- TerrainMapOctree: Min Distance = 537.51. 2021-02-13T01:09:53Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:09:53Z- There is no specific good beam that all particles have in common. 2021-02-13T01:09:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:09:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.37 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 14.84 2021-02-13T01:09:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:09:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.26 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 19.78 2021-02-13T01:09:53Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:09:53Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.059 ExpectedMeasDiff: nan currMeas.covariance: 17.44 2021-02-13T01:09:53Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:09:53Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:09:53Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:09:53Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:53Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:09:53Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:09:53Z- Filter 0 No beams used in subcloud update 2021-02-13T01:09:53Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:09:53Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:09:53Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 673.8 EastVariance: 833.0 DepthVariance: 1.1 this->DepthBiasCov: 1.1 2021-02-13T01:09:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178593.13 sec. 2021-02-13T01:09:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:09:53Z- TNavBF::mmse selected from filter -1 2021-02-13T01:09:53Z- variances([0], [2], and psi_berg) = 673.831993 832.985945 0.000000 2021-02-13T01:09:53Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:09:53Z- LcmTrn::publishEstimates() - MLE : 4060988.49 593796.91 12.45 2021-02-13T01:09:53Z- LcmTrn::publishEstimates() - MMSE : 4060988.99 593780.41 12.45 2021-02-13T01:09:53Z- LcmTrn::publishEstimates() - COVAR: 673.83 832.99 1.14 0.00 2021-02-13T01:09:53Z- handleDepth()-Depth_Keller msg: 1613178593512 epoch sec; seqNo:19100; depth 12.34 2021-02-13T01:09:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178593.53 epoch sec; seqNo:19100; 4060988.99 north; 593780.41 east 2021-02-13T01:09:53Z- AHRS_M2 msg: 1613178593.94 epoch sec; seqNo:18950 2021-02-13T01:09:53Z- AHRS_M2 msg: 0.63 phi; 0.32 theta; -0.14 psi 2021-02-13T01:09:53Z- handleDepth()-Depth_Keller msg: 1613178593952 epoch sec; seqNo:19101; depth 12.27 2021-02-13T01:09:53Z- handleDvl() - RDI_Pathfinder msg: 1613178593.97 epoch sec; seqNo:9205 2021-02-13T01:09:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 84.40 , 99.28 , 112.67 , 74.80 2021-02-13T01:09:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.03 , -0.02 2021-02-13T01:09:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178593.98 epoch sec; seqNo:19101; 4060989.37 north; 593780.41 east 2021-02-13T01:09:54Z- AHRS_M2 msg: 1613178594.32 epoch sec; seqNo:18951 2021-02-13T01:09:54Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.14 psi 2021-02-13T01:09:54Z- handleDepth()-Depth_Keller msg: 1613178594320 epoch sec; seqNo:19102; depth 12.14 2021-02-13T01:09:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178594.33 epoch sec; seqNo:19102; 4060989.75 north; 593780.41 east 2021-02-13T01:09:54Z- AHRS_M2 msg: 1613178594.72 epoch sec; seqNo:18952 2021-02-13T01:09:54Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.14 psi 2021-02-13T01:09:54Z- handleDepth()-Depth_Keller msg: 1613178594724 epoch sec; seqNo:19103; depth 12.05 2021-02-13T01:09:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178594.73 epoch sec; seqNo:19103; 4060989.75 north; 593780.41 east 2021-02-13T01:09:55Z- AHRS_M2 msg: 1613178595.12 epoch sec; seqNo:18953 2021-02-13T01:09:55Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.14 psi 2021-02-13T01:09:55Z- handleDepth()-Depth_Keller msg: 1613178595138 epoch sec; seqNo:19104; depth 11.94 2021-02-13T01:09:55Z- handleDvl() - RDI_Pathfinder msg: 1613178595.16 epoch sec; seqNo:9206 2021-02-13T01:09:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 99.00 , 111.18 , 75.21 2021-02-13T01:09:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.01 , -0.05 2021-02-13T01:09:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178595.18 epoch sec; seqNo:19104; 4060990.14 north; 593781.62 east 2021-02-13T01:09:55Z- AHRS_M2 msg: 1613178595.53 epoch sec; seqNo:18954 2021-02-13T01:09:55Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.15 psi 2021-02-13T01:09:55Z- handleDepth()-Depth_Keller msg: 1613178595532 epoch sec; seqNo:19105; depth 11.84 2021-02-13T01:09:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178595.54 epoch sec; seqNo:19105; 4060990.14 north; 593781.62 east 2021-02-13T01:09:55Z- AHRS_M2 msg: 1613178595.93 epoch sec; seqNo:18955 2021-02-13T01:09:55Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.16 psi 2021-02-13T01:09:55Z- handleDepth()-Depth_Keller msg: 1613178595946 epoch sec; seqNo:19106; depth 11.75 2021-02-13T01:09:55Z- handleDvl() - RDI_Pathfinder msg: 1613178595.96 epoch sec; seqNo:9207 2021-02-13T01:09:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.28 , 96.10 , 109.62 , 76.44 2021-02-13T01:09:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , -0.05 2021-02-13T01:09:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178595.97 epoch sec; seqNo:19106; 4060990.52 north; 593781.62 east 2021-02-13T01:09:56Z- AHRS_M2 msg: 1613178596.34 epoch sec; seqNo:18956 2021-02-13T01:09:56Z- AHRS_M2 msg: 0.63 phi; 0.31 theta; -0.17 psi 2021-02-13T01:09:56Z- handleDepth()-Depth_Keller msg: 1613178596339 epoch sec; seqNo:19107; depth 11.64 2021-02-13T01:09:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178596.35 epoch sec; seqNo:19107; 4060990.90 north; 593781.61 east 2021-02-13T01:09:56Z- AHRS_M2 msg: 1613178596.74 epoch sec; seqNo:18957 2021-02-13T01:09:56Z- AHRS_M2 msg: 0.62 phi; 0.31 theta; -0.19 psi 2021-02-13T01:09:56Z- handleDepth()-Depth_Keller msg: 1613178596744 epoch sec; seqNo:19108; depth 11.53 2021-02-13T01:09:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178596.75 epoch sec; seqNo:19108; 4060990.90 north; 593781.61 east 2021-02-13T01:09:57Z- AHRS_M2 msg: 1613178597.15 epoch sec; seqNo:18958 2021-02-13T01:09:57Z- AHRS_M2 msg: 0.61 phi; 0.31 theta; -0.20 psi 2021-02-13T01:09:57Z- handleDepth()-Depth_Keller msg: 1613178597160 epoch sec; seqNo:19109; depth 11.44 2021-02-13T01:09:57Z- handleDvl() - RDI_Pathfinder msg: 1613178597.17 epoch sec; seqNo:9208 2021-02-13T01:09:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 82.84 , 101.78 , 113.95 , 78.79 2021-02-13T01:09:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.16 , 0.07 2021-02-13T01:09:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178597.18 epoch sec; seqNo:19109; 4060991.28 north; 593781.61 east 2021-02-13T01:09:57Z- AHRS_M2 msg: 1613178597.55 epoch sec; seqNo:18959 2021-02-13T01:09:57Z- AHRS_M2 msg: 0.59 phi; 0.32 theta; -0.20 psi 2021-02-13T01:09:57Z- handleDepth()-Depth_Keller msg: 1613178597557 epoch sec; seqNo:19110; depth 11.33 2021-02-13T01:09:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178597.58 epoch sec; seqNo:19110; 4060991.66 north; 593781.61 east 2021-02-13T01:09:57Z- AHRS_M2 msg: 1613178597.95 epoch sec; seqNo:18960 2021-02-13T01:09:57Z- AHRS_M2 msg: 0.58 phi; 0.32 theta; -0.20 psi 2021-02-13T01:09:57Z- handleDepth()-Depth_Keller msg: 1613178597956 epoch sec; seqNo:19111; depth 11.22 2021-02-13T01:09:57Z- handleDvl() - RDI_Pathfinder msg: 1613178597.97 epoch sec; seqNo:9209 2021-02-13T01:09:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 83.99 , 102.93 , 112.40 , 76.44 2021-02-13T01:09:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.10 , 0.19 2021-02-13T01:09:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178597.98 epoch sec; seqNo:19111; 4060992.05 north; 593782.82 east 2021-02-13T01:09:58Z- AHRS_M2 msg: 1613178598.36 epoch sec; seqNo:18961 2021-02-13T01:09:58Z- AHRS_M2 msg: 0.57 phi; 0.33 theta; -0.19 psi 2021-02-13T01:09:58Z- handleDepth()-Depth_Keller msg: 1613178598369 epoch sec; seqNo:19112; depth 11.12 2021-02-13T01:09:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178598.38 epoch sec; seqNo:19112; 4060992.43 north; 593782.82 east 2021-02-13T01:09:58Z- LcmTrn::handleMessages() - handling 43 messages took 0.65 ms and 130 calls... 2021-02-13T01:09:58Z- waiting for 1613178596.51, time is 1613178598.38 2021-02-13T01:09:58Z- LcmTrn::updateTrn() >>>> pose time 1613178598.38, meas time 1613178597.97 <<<< 2021-02-13T01:09:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178597.97 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:09:58Z- TerrainNav::motionUpdate() - currEstimate = (4060992, 593782.82, 11.12) 2021-02-13T01:09:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:09:58Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:09:58Z- TNavBF::Projecting Measurements 2021-02-13T01:09:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:09:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:09:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:09:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:09:58Z- TNavFilter::4 good beams. 2021-02-13T01:09:58Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:09:58Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:09:58Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:09:58Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:09:58Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:09:58Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:09:58Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:09:58Z- TNavBF::mmse selected from filter -1 2021-02-13T01:09:58Z- TerrainMapOctree: (vehN, vehE ) = (4060992.14, 593782.61). 2021-02-13T01:09:58Z- TerrainMapOctree: Min Distance = 533.80. 2021-02-13T01:09:58Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:09:58Z- There is no specific good beam that all particles have in common. 2021-02-13T01:09:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:09:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.33 K: 0.093 ExpectedMeasDiff: nan currMeas.covariance: 11.29 2021-02-13T01:09:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:09:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.21 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 16.95 2021-02-13T01:09:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:09:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.14 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 20.21 2021-02-13T01:09:58Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:09:58Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.089 ExpectedMeasDiff: nan currMeas.covariance: 9.35 2021-02-13T01:09:58Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:09:58Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:09:58Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:09:58Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:09:58Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:09:58Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:09:58Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:09:58Z- Filter 0 No beams used in subcloud update 2021-02-13T01:09:58Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:09:58Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:09:58Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 673.8 EastVariance: 833.0 DepthVariance: 1.0 this->DepthBiasCov: 1.0 2021-02-13T01:09:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178597.97 sec. 2021-02-13T01:09:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:09:58Z- TNavBF::mmse selected from filter -1 2021-02-13T01:09:58Z- variances([0], [2], and psi_berg) = 673.779377 832.985924 0.000000 2021-02-13T01:09:58Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:09:58Z- LcmTrn::publishEstimates() - MLE : 4060991.93 593798.32 11.12 2021-02-13T01:09:58Z- LcmTrn::publishEstimates() - MMSE : 4060992.43 593782.82 11.12 2021-02-13T01:09:58Z- LcmTrn::publishEstimates() - COVAR: 673.78 832.99 1.00 0.00 2021-02-13T01:09:58Z- AHRS_M2 msg: 1613178598.76 epoch sec; seqNo:18962 2021-02-13T01:09:58Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.19 psi 2021-02-13T01:09:58Z- handleDepth()-Depth_Keller msg: 1613178598764 epoch sec; seqNo:19113; depth 11.03 2021-02-13T01:09:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178598.79 epoch sec; seqNo:19113; 4060992.81 north; 593782.81 east 2021-02-13T01:09:59Z- AHRS_M2 msg: 1613178599.16 epoch sec; seqNo:18963 2021-02-13T01:09:59Z- AHRS_M2 msg: 0.57 phi; 0.34 theta; -0.18 psi 2021-02-13T01:09:59Z- handleDepth()-Depth_Keller msg: 1613178599170 epoch sec; seqNo:19114; depth 10.92 2021-02-13T01:09:59Z- handleDvl() - RDI_Pathfinder msg: 1613178599.18 epoch sec; seqNo:9210 2021-02-13T01:09:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 102.99 , 113.81 , 76.02 2021-02-13T01:09:59Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.11 , 0.21 2021-02-13T01:09:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178599.19 epoch sec; seqNo:19114; 4060993.19 north; 593782.81 east 2021-02-13T01:09:59Z- AHRS_M2 msg: 1613178599.57 epoch sec; seqNo:18964 2021-02-13T01:09:59Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.17 psi 2021-02-13T01:09:59Z- handleDepth()-Depth_Keller msg: 1613178599572 epoch sec; seqNo:19115; depth 10.83 2021-02-13T01:09:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178599.58 epoch sec; seqNo:19115; 4060993.57 north; 593782.81 east 2021-02-13T01:09:59Z- AHRS_M2 msg: 1613178599.97 epoch sec; seqNo:18965 2021-02-13T01:09:59Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.16 psi 2021-02-13T01:09:59Z- handleDepth()-Depth_Keller msg: 1613178599978 epoch sec; seqNo:19116; depth 10.73 2021-02-13T01:09:59Z- handleDvl() - RDI_Pathfinder msg: 1613178599.99 epoch sec; seqNo:9211 2021-02-13T01:09:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.78 , 104.00 , 120.23 , 76.84 2021-02-13T01:09:59Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.30 , 0.07 2021-02-13T01:10:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178600.00 epoch sec; seqNo:19116; 4060993.58 north; 593784.03 east 2021-02-13T01:10:00Z- AHRS_M2 msg: 1613178600.38 epoch sec; seqNo:18966 2021-02-13T01:10:00Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.16 psi 2021-02-13T01:10:00Z- handleDepth()-Depth_Keller msg: 1613178600379 epoch sec; seqNo:19117; depth 10.63 2021-02-13T01:10:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178600.40 epoch sec; seqNo:19117; 4060993.96 north; 593784.02 east 2021-02-13T01:10:00Z- AHRS_M2 msg: 1613178600.78 epoch sec; seqNo:18967 2021-02-13T01:10:00Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.15 psi 2021-02-13T01:10:00Z- handleDepth()-Depth_Keller msg: 1613178600787 epoch sec; seqNo:19118; depth 10.54 2021-02-13T01:10:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178600.80 epoch sec; seqNo:19118; 4060994.34 north; 593784.02 east 2021-02-13T01:10:01Z- AHRS_M2 msg: 1613178601.18 epoch sec; seqNo:18968 2021-02-13T01:10:01Z- AHRS_M2 msg: 0.60 phi; 0.35 theta; -0.14 psi 2021-02-13T01:10:01Z- handleDepth()-Depth_Keller msg: 1613178601197 epoch sec; seqNo:19119; depth 10.42 2021-02-13T01:10:01Z- handleDvl() - RDI_Pathfinder msg: 1613178601.21 epoch sec; seqNo:9212 2021-02-13T01:10:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.20 , 102.72 , 117.60 , 77.26 2021-02-13T01:10:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.37 , -0.13 2021-02-13T01:10:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178601.22 epoch sec; seqNo:19119; 4060994.34 north; 593784.02 east 2021-02-13T01:10:01Z- AHRS_M2 msg: 1613178601.59 epoch sec; seqNo:18969 2021-02-13T01:10:01Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.13 psi 2021-02-13T01:10:01Z- handleDepth()-Depth_Keller msg: 1613178601593 epoch sec; seqNo:19120; depth 10.31 2021-02-13T01:10:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178601.60 epoch sec; seqNo:19120; 4060994.73 north; 593785.23 east 2021-02-13T01:10:01Z- AHRS_M2 msg: 1613178601.99 epoch sec; seqNo:18970 2021-02-13T01:10:01Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.13 psi 2021-02-13T01:10:01Z- handleDepth()-Depth_Keller msg: 1613178601996 epoch sec; seqNo:19121; depth 10.20 2021-02-13T01:10:02Z- handleDvl() - RDI_Pathfinder msg: 1613178602.01 epoch sec; seqNo:9213 2021-02-13T01:10:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 95.96 , 118.96 , 78.48 2021-02-13T01:10:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , 0.26 , -0.27 2021-02-13T01:10:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178602.02 epoch sec; seqNo:19121; 4060994.73 north; 593785.23 east 2021-02-13T01:10:02Z- AHRS_M2 msg: 1613178602.40 epoch sec; seqNo:18971 2021-02-13T01:10:02Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-13T01:10:02Z- handleDepth()-Depth_Keller msg: 1613178602405 epoch sec; seqNo:19122; depth 10.06 2021-02-13T01:10:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178602.42 epoch sec; seqNo:19122; 4060994.73 north; 593785.23 east 2021-02-13T01:10:02Z- AHRS_M2 msg: 1613178602.80 epoch sec; seqNo:18972 2021-02-13T01:10:02Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-13T01:10:02Z- handleDepth()-Depth_Keller msg: 1613178602809 epoch sec; seqNo:19123; depth 9.97 2021-02-13T01:10:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178602.82 epoch sec; seqNo:19123; 4060995.11 north; 593785.23 east 2021-02-13T01:10:03Z- AHRS_M2 msg: 1613178603.21 epoch sec; seqNo:18973 2021-02-13T01:10:03Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-13T01:10:03Z- handleDepth()-Depth_Keller msg: 1613178603219 epoch sec; seqNo:19124; depth 9.83 2021-02-13T01:10:03Z- handleDvl() - RDI_Pathfinder msg: 1613178603.23 epoch sec; seqNo:9214 2021-02-13T01:10:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.78 , 102.66 , 117.54 , 78.48 2021-02-13T01:10:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , 0.07 , -0.26 2021-02-13T01:10:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178603.24 epoch sec; seqNo:19124; 4060995.11 north; 593785.23 east 2021-02-13T01:10:03Z- AHRS_M2 msg: 1613178603.61 epoch sec; seqNo:18974 2021-02-13T01:10:03Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.15 psi 2021-02-13T01:10:03Z- handleDepth()-Depth_Keller msg: 1613178603612 epoch sec; seqNo:19125; depth 9.73 2021-02-13T01:10:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178603.62 epoch sec; seqNo:19125; 4060995.49 north; 593785.23 east 2021-02-13T01:10:03Z- LcmTrn::handleMessages() - handling 44 messages took 0.72 ms and 130 calls... 2021-02-13T01:10:03Z- waiting for 1613178601.38, time is 1613178603.62 2021-02-13T01:10:03Z- LcmTrn::updateTrn() >>>> pose time 1613178603.62, meas time 1613178603.23 <<<< 2021-02-13T01:10:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178603.23 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:10:03Z- TerrainNav::motionUpdate() - currEstimate = (4060995, 593785.23, 9.73) 2021-02-13T01:10:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:10:03Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:10:03Z- TNavBF::Projecting Measurements 2021-02-13T01:10:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:10:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:10:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:10:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:10:03Z- TNavFilter::4 good beams. 2021-02-13T01:10:03Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:10:03Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:10:03Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:10:03Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:10:03Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:10:03Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:10:03Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:10:03Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:03Z- TerrainMapOctree: (vehN, vehE ) = (4060995.26, 593785.05). 2021-02-13T01:10:03Z- TerrainMapOctree: Min Distance = 530.17. 2021-02-13T01:10:03Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:10:03Z- There is no specific good beam that all particles have in common. 2021-02-13T01:10:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:10:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.19 K: 0.078 ExpectedMeasDiff: nan currMeas.covariance: 12.33 2021-02-13T01:10:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:10:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 16.86 2021-02-13T01:10:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:10:03Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.042 ExpectedMeasDiff: nan currMeas.covariance: 22.10 2021-02-13T01:10:03Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:10:03Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.079 ExpectedMeasDiff: nan currMeas.covariance: 9.86 2021-02-13T01:10:03Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:10:03Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:10:03Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:10:03Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:10:03Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:03Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:10:03Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:10:03Z- Filter 0 No beams used in subcloud update 2021-02-13T01:10:03Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:10:03Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:10:03Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 673.7 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:10:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178603.23 sec. 2021-02-13T01:10:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:10:03Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:03Z- variances([0], [2], and psi_berg) = 673.729214 832.985904 0.000000 2021-02-13T01:10:03Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:10:03Z- LcmTrn::publishEstimates() - MLE : 4060994.99 593800.73 9.73 2021-02-13T01:10:03Z- LcmTrn::publishEstimates() - MMSE : 4060995.49 593785.23 9.73 2021-02-13T01:10:03Z- LcmTrn::publishEstimates() - COVAR: 673.73 832.99 0.93 0.00 2021-02-13T01:10:04Z- AHRS_M2 msg: 1613178604.03 epoch sec; seqNo:18975 2021-02-13T01:10:04Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.16 psi 2021-02-13T01:10:04Z- handleDepth()-Depth_Keller msg: 1613178604036 epoch sec; seqNo:19126; depth 9.61 2021-02-13T01:10:04Z- handleDvl() - RDI_Pathfinder msg: 1613178604.05 epoch sec; seqNo:9215 2021-02-13T01:10:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.50 , 102.72 , 116.25 , 79.73 2021-02-13T01:10:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.06 , -0.16 2021-02-13T01:10:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178604.06 epoch sec; seqNo:19126; 4060995.49 north; 593785.23 east 2021-02-13T01:10:04Z- AHRS_M2 msg: 1613178604.42 epoch sec; seqNo:18976 2021-02-13T01:10:04Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.16 psi 2021-02-13T01:10:04Z- handleDepth()-Depth_Keller msg: 1613178604425 epoch sec; seqNo:19127; depth 9.49 2021-02-13T01:10:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178604.44 epoch sec; seqNo:19127; 4060995.88 north; 593786.44 east 2021-02-13T01:10:04Z- AHRS_M2 msg: 1613178604.82 epoch sec; seqNo:18977 2021-02-13T01:10:04Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-13T01:10:04Z- handleDepth()-Depth_Keller msg: 1613178604826 epoch sec; seqNo:19128; depth 9.37 2021-02-13T01:10:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178604.83 epoch sec; seqNo:19128; 4060996.26 north; 593786.44 east 2021-02-13T01:10:05Z- AHRS_M2 msg: 1613178605.22 epoch sec; seqNo:18978 2021-02-13T01:10:05Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-13T01:10:05Z- handleDepth()-Depth_Keller msg: 1613178605243 epoch sec; seqNo:19129; depth 9.26 2021-02-13T01:10:05Z- handleDvl() - RDI_Pathfinder msg: 1613178605.26 epoch sec; seqNo:9216 2021-02-13T01:10:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.56 , 104.14 , 119.01 , 78.88 2021-02-13T01:10:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.13 , -0.10 2021-02-13T01:10:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178605.27 epoch sec; seqNo:19129; 4060996.26 north; 593786.44 east 2021-02-13T01:10:05Z- AHRS_M2 msg: 1613178605.63 epoch sec; seqNo:18979 2021-02-13T01:10:05Z- AHRS_M2 msg: 0.61 phi; 0.34 theta; -0.17 psi 2021-02-13T01:10:05Z- handleDepth()-Depth_Keller msg: 1613178605637 epoch sec; seqNo:19130; depth 9.15 2021-02-13T01:10:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178605.65 epoch sec; seqNo:19130; 4060996.64 north; 593786.43 east 2021-02-13T01:10:06Z- AHRS_M2 msg: 1613178606.03 epoch sec; seqNo:18980 2021-02-13T01:10:06Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.17 psi 2021-02-13T01:10:06Z- handleDepth()-Depth_Keller msg: 1613178606036 epoch sec; seqNo:19131; depth 9.03 2021-02-13T01:10:06Z- handleDvl() - RDI_Pathfinder msg: 1613178606.05 epoch sec; seqNo:9217 2021-02-13T01:10:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.49 , 104.07 , 118.95 , 78.88 2021-02-13T01:10:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.18 , -0.03 2021-02-13T01:10:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178606.06 epoch sec; seqNo:19131; 4060997.02 north; 593786.43 east 2021-02-13T01:10:06Z- AHRS_M2 msg: 1613178606.44 epoch sec; seqNo:18981 2021-02-13T01:10:06Z- AHRS_M2 msg: 0.60 phi; 0.34 theta; -0.18 psi 2021-02-13T01:10:06Z- handleDepth()-Depth_Keller msg: 1613178606449 epoch sec; seqNo:19132; depth 8.91 2021-02-13T01:10:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178606.46 epoch sec; seqNo:19132; 4060997.40 north; 593786.42 east 2021-02-13T01:10:06Z- AHRS_M2 msg: 1613178606.84 epoch sec; seqNo:18982 2021-02-13T01:10:06Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-13T01:10:06Z- handleDepth()-Depth_Keller msg: 1613178606844 epoch sec; seqNo:19133; depth 8.81 2021-02-13T01:10:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178606.85 epoch sec; seqNo:19133; 4060997.77 north; 593786.42 east 2021-02-13T01:10:07Z- AHRS_M2 msg: 1613178607.24 epoch sec; seqNo:18983 2021-02-13T01:10:07Z- AHRS_M2 msg: 0.59 phi; 0.34 theta; -0.18 psi 2021-02-13T01:10:07Z- handleDepth()-Depth_Keller msg: 1613178607253 epoch sec; seqNo:19134; depth 8.70 2021-02-13T01:10:07Z- handleDvl() - RDI_Pathfinder msg: 1613178607.27 epoch sec; seqNo:9218 2021-02-13T01:10:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.84 , 105.42 , 118.95 , 79.73 2021-02-13T01:10:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.14 , 0.09 2021-02-13T01:10:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178607.28 epoch sec; seqNo:19134; 4060997.77 north; 593786.42 east 2021-02-13T01:10:07Z- AHRS_M2 msg: 1613178607.65 epoch sec; seqNo:18984 2021-02-13T01:10:07Z- AHRS_M2 msg: 0.58 phi; 0.34 theta; -0.17 psi 2021-02-13T01:10:07Z- handleDepth()-Depth_Keller msg: 1613178607659 epoch sec; seqNo:19135; depth 8.58 2021-02-13T01:10:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178607.67 epoch sec; seqNo:19135; 4060998.15 north; 593786.42 east 2021-02-13T01:10:08Z- AHRS_M2 msg: 1613178608.05 epoch sec; seqNo:18985 2021-02-13T01:10:08Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.17 psi 2021-02-13T01:10:08Z- handleDepth()-Depth_Keller msg: 1613178608057 epoch sec; seqNo:19136; depth 8.46 2021-02-13T01:10:08Z- handleDvl() - RDI_Pathfinder msg: 1613178608.08 epoch sec; seqNo:9219 2021-02-13T01:10:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.78 , 105.36 , 121.58 , 78.07 2021-02-13T01:10:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.20 2021-02-13T01:10:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178608.09 epoch sec; seqNo:19136; 4060998.55 north; 593787.63 east 2021-02-13T01:10:08Z- AHRS_M2 msg: 1613178608.45 epoch sec; seqNo:18986 2021-02-13T01:10:08Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.16 psi 2021-02-13T01:10:08Z- handleDepth()-Depth_Keller msg: 1613178608465 epoch sec; seqNo:19137; depth 8.35 2021-02-13T01:10:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178608.47 epoch sec; seqNo:19137; 4060998.92 north; 593787.63 east 2021-02-13T01:10:08Z- LcmTrn::handleMessages() - handling 41 messages took 0.61 ms and 129 calls... 2021-02-13T01:10:08Z- waiting for 1613178606.62, time is 1613178608.47 2021-02-13T01:10:08Z- LcmTrn::updateTrn() >>>> pose time 1613178608.47, meas time 1613178608.08 <<<< 2021-02-13T01:10:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178608.08 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:10:08Z- TerrainNav::motionUpdate() - currEstimate = (4060999, 593787.63, 8.35) 2021-02-13T01:10:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:10:08Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:10:08Z- TNavBF::Projecting Measurements 2021-02-13T01:10:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:10:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:10:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:10:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:10:08Z- TNavFilter::4 good beams. 2021-02-13T01:10:08Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:10:08Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:10:08Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:10:08Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:10:08Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:10:08Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:10:08Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:10:08Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:08Z- TerrainMapOctree: (vehN, vehE ) = (4060998.64, 593787.43). 2021-02-13T01:10:08Z- TerrainMapOctree: Min Distance = 526.45. 2021-02-13T01:10:08Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:10:08Z- There is no specific good beam that all particles have in common. 2021-02-13T01:10:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:10:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.074 ExpectedMeasDiff: nan currMeas.covariance: 12.33 2021-02-13T01:10:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:10:08Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 17.76 2021-02-13T01:10:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:10:08Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 23.65 2021-02-13T01:10:08Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:10:08Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.076 ExpectedMeasDiff: nan currMeas.covariance: 9.75 2021-02-13T01:10:08Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:10:08Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:10:08Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:10:08Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:10:08Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:08Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:10:08Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:10:08Z- Filter 0 No beams used in subcloud update 2021-02-13T01:10:08Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:10:08Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:10:08Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 673.7 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:10:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178608.08 sec. 2021-02-13T01:10:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:10:08Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:08Z- variances([0], [2], and psi_berg) = 673.676762 832.985884 0.000000 2021-02-13T01:10:08Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:10:08Z- LcmTrn::publishEstimates() - MLE : 4060998.42 593803.13 8.35 2021-02-13T01:10:08Z- LcmTrn::publishEstimates() - MMSE : 4060998.92 593787.63 8.35 2021-02-13T01:10:08Z- LcmTrn::publishEstimates() - COVAR: 673.68 832.99 0.90 0.00 2021-02-13T01:10:08Z- AHRS_M2 msg: 1613178608.86 epoch sec; seqNo:18987 2021-02-13T01:10:08Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.16 psi 2021-02-13T01:10:08Z- handleDepth()-Depth_Keller msg: 1613178608864 epoch sec; seqNo:19138; depth 8.24 2021-02-13T01:10:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178608.87 epoch sec; seqNo:19138; 4060999.30 north; 593787.62 east 2021-02-13T01:10:09Z- AHRS_M2 msg: 1613178609.26 epoch sec; seqNo:18988 2021-02-13T01:10:09Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.16 psi 2021-02-13T01:10:09Z- handleDepth()-Depth_Keller msg: 1613178609277 epoch sec; seqNo:19139; depth 8.13 2021-02-13T01:10:09Z- handleDvl() - RDI_Pathfinder msg: 1613178609.29 epoch sec; seqNo:9220 2021-02-13T01:10:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.06 , 105.29 , 121.52 , 80.95 2021-02-13T01:10:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.13 , 0.20 2021-02-13T01:10:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178609.30 epoch sec; seqNo:19139; 4060999.30 north; 593787.62 east 2021-02-13T01:10:09Z- AHRS_M2 msg: 1613178609.67 epoch sec; seqNo:18989 2021-02-13T01:10:09Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.16 psi 2021-02-13T01:10:09Z- handleDepth()-Depth_Keller msg: 1613178609677 epoch sec; seqNo:19140; depth 8.00 2021-02-13T01:10:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178609.69 epoch sec; seqNo:19140; 4060999.68 north; 593787.62 east 2021-02-13T01:10:10Z- AHRS_M2 msg: 1613178610.07 epoch sec; seqNo:18990 2021-02-13T01:10:10Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.17 psi 2021-02-13T01:10:10Z- handleDepth()-Depth_Keller msg: 1613178610076 epoch sec; seqNo:19141; depth 7.89 2021-02-13T01:10:10Z- handleDvl() - RDI_Pathfinder msg: 1613178610.09 epoch sec; seqNo:9221 2021-02-13T01:10:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.35 , 106.57 , 120.10 , 79.70 2021-02-13T01:10:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.14 , 0.06 2021-02-13T01:10:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178610.11 epoch sec; seqNo:19141; 4061000.07 north; 593788.84 east 2021-02-13T01:10:10Z- AHRS_M2 msg: 1613178610.48 epoch sec; seqNo:18991 2021-02-13T01:10:10Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.17 psi 2021-02-13T01:10:10Z- handleDepth()-Depth_Keller msg: 1613178610485 epoch sec; seqNo:19142; depth 7.77 2021-02-13T01:10:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178610.50 epoch sec; seqNo:19142; 4061000.07 north; 593788.84 east 2021-02-13T01:10:10Z- AHRS_M2 msg: 1613178610.88 epoch sec; seqNo:18992 2021-02-13T01:10:10Z- AHRS_M2 msg: 0.58 phi; 0.35 theta; -0.19 psi 2021-02-13T01:10:10Z- handleDepth()-Depth_Keller msg: 1613178610886 epoch sec; seqNo:19143; depth 7.65 2021-02-13T01:10:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178610.89 epoch sec; seqNo:19143; 4061000.45 north; 593788.83 east 2021-02-13T01:10:11Z- AHRS_M2 msg: 1613178611.28 epoch sec; seqNo:18993 2021-02-13T01:10:11Z- AHRS_M2 msg: 0.57 phi; 0.35 theta; -0.19 psi 2021-02-13T01:10:11Z- handleDepth()-Depth_Keller msg: 1613178611288 epoch sec; seqNo:19144; depth 7.53 2021-02-13T01:10:11Z- handleDvl() - RDI_Pathfinder msg: 1613178611.32 epoch sec; seqNo:9222 2021-02-13T01:10:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.35 , 107.93 , 121.45 , 80.88 2021-02-13T01:10:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , -0.04 2021-02-13T01:10:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178611.33 epoch sec; seqNo:19144; 4061000.83 north; 593788.83 east 2021-02-13T01:10:11Z- AHRS_M2 msg: 1613178611.69 epoch sec; seqNo:18994 2021-02-13T01:10:11Z- AHRS_M2 msg: 0.57 phi; 0.36 theta; -0.18 psi 2021-02-13T01:10:11Z- handleDepth()-Depth_Keller msg: 1613178611697 epoch sec; seqNo:19145; depth 7.42 2021-02-13T01:10:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178611.71 epoch sec; seqNo:19145; 4061000.83 north; 593788.83 east 2021-02-13T01:10:12Z- AHRS_M2 msg: 1613178612.09 epoch sec; seqNo:18995 2021-02-13T01:10:12Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.18 psi 2021-02-13T01:10:12Z- handleDepth()-Depth_Keller msg: 1613178612105 epoch sec; seqNo:19146; depth 7.30 2021-02-13T01:10:12Z- handleDvl() - RDI_Pathfinder msg: 1613178612.12 epoch sec; seqNo:9223 2021-02-13T01:10:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 107.86 , 121.38 , 82.16 2021-02-13T01:10:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.00 , 0.02 2021-02-13T01:10:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178612.13 epoch sec; seqNo:19146; 4061001.21 north; 593788.82 east 2021-02-13T01:10:12Z- AHRS_M2 msg: 1613178612.50 epoch sec; seqNo:18996 2021-02-13T01:10:12Z- AHRS_M2 msg: 0.56 phi; 0.36 theta; -0.17 psi 2021-02-13T01:10:12Z- handleDepth()-Depth_Keller msg: 1613178612502 epoch sec; seqNo:19147; depth 7.18 2021-02-13T01:10:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178612.51 epoch sec; seqNo:19147; 4061001.60 north; 593790.04 east 2021-02-13T01:10:12Z- AHRS_M2 msg: 1613178612.90 epoch sec; seqNo:18997 2021-02-13T01:10:12Z- AHRS_M2 msg: 0.55 phi; 0.37 theta; -0.17 psi 2021-02-13T01:10:12Z- handleDepth()-Depth_Keller msg: 1613178612904 epoch sec; seqNo:19148; depth 7.08 2021-02-13T01:10:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178612.91 epoch sec; seqNo:19148; 4061001.98 north; 593790.04 east 2021-02-13T01:10:13Z- AHRS_M2 msg: 1613178613.30 epoch sec; seqNo:18998 2021-02-13T01:10:13Z- AHRS_M2 msg: 0.55 phi; 0.37 theta; -0.15 psi 2021-02-13T01:10:13Z- handleDepth()-Depth_Keller msg: 1613178613317 epoch sec; seqNo:19149; depth 6.96 2021-02-13T01:10:13Z- handleDvl() - RDI_Pathfinder msg: 1613178613.33 epoch sec; seqNo:9224 2021-02-13T01:10:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.63 , 107.86 , 125.44 , 80.81 2021-02-13T01:10:13Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.12 , 0.08 2021-02-13T01:10:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178613.35 epoch sec; seqNo:19149; 4061002.36 north; 593790.03 east 2021-02-13T01:10:13Z- AHRS_M2 msg: 1613178613.71 epoch sec; seqNo:18999 2021-02-13T01:10:13Z- AHRS_M2 msg: 0.56 phi; 0.38 theta; -0.14 psi 2021-02-13T01:10:13Z- handleDepth()-Depth_Keller msg: 1613178613713 epoch sec; seqNo:19150; depth 6.84 2021-02-13T01:10:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178613.72 epoch sec; seqNo:19150; 4061002.36 north; 593790.03 east 2021-02-13T01:10:13Z- LcmTrn::handleMessages() - handling 44 messages took 0.75 ms and 129 calls... 2021-02-13T01:10:13Z- waiting for 1613178611.47, time is 1613178613.72 2021-02-13T01:10:13Z- LcmTrn::updateTrn() >>>> pose time 1613178613.72, meas time 1613178613.33 <<<< 2021-02-13T01:10:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178613.33 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:10:13Z- TerrainNav::motionUpdate() - currEstimate = (4061002, 593790.03, 6.84) 2021-02-13T01:10:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:10:13Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:10:13Z- TNavBF::Projecting Measurements 2021-02-13T01:10:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:10:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:10:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:10:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:10:13Z- TNavFilter::4 good beams. 2021-02-13T01:10:13Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:10:13Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:10:13Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:10:13Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:10:13Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:10:13Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:10:13Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:10:13Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:13Z- TerrainMapOctree: (vehN, vehE ) = (4061002.10, 593789.85). 2021-02-13T01:10:13Z- TerrainMapOctree: Min Distance = 522.68. 2021-02-13T01:10:13Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:10:13Z- There is no specific good beam that all particles have in common. 2021-02-13T01:10:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:10:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.09 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 13.43 2021-02-13T01:10:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:10:13Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 18.61 2021-02-13T01:10:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:10:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.97 K: 0.035 ExpectedMeasDiff: nan currMeas.covariance: 25.18 2021-02-13T01:10:13Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:10:13Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.071 ExpectedMeasDiff: nan currMeas.covariance: 10.45 2021-02-13T01:10:13Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:10:13Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:10:13Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:10:13Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:10:13Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:13Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:10:13Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:10:13Z- Filter 0 No beams used in subcloud update 2021-02-13T01:10:13Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:10:13Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:10:13Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 673.6 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:10:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178613.33 sec. 2021-02-13T01:10:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:10:13Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:13Z- variances([0], [2], and psi_berg) = 673.624708 832.985863 0.000000 2021-02-13T01:10:13Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:10:13Z- LcmTrn::publishEstimates() - MLE : 4061001.86 593805.53 6.84 2021-02-13T01:10:13Z- LcmTrn::publishEstimates() - MMSE : 4061002.36 593790.03 6.84 2021-02-13T01:10:13Z- LcmTrn::publishEstimates() - COVAR: 673.62 832.99 0.89 0.00 2021-02-13T01:10:14Z- AHRS_M2 msg: 1613178614.11 epoch sec; seqNo:19000 2021-02-13T01:10:14Z- AHRS_M2 msg: 0.57 phi; 0.38 theta; -0.13 psi 2021-02-13T01:10:14Z- handleDepth()-Depth_Keller msg: 1613178614128 epoch sec; seqNo:19151; depth 6.74 2021-02-13T01:10:14Z- handleDvl() - RDI_Pathfinder msg: 1613178614.14 epoch sec; seqNo:9225 2021-02-13T01:10:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 106.92 , 127.75 , 78.89 2021-02-13T01:10:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.30 , 0.02 2021-02-13T01:10:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178614.15 epoch sec; seqNo:19151; 4061002.74 north; 593790.03 east 2021-02-13T01:10:14Z- AHRS_M2 msg: 1613178614.52 epoch sec; seqNo:19001 2021-02-13T01:10:14Z- AHRS_M2 msg: 0.57 phi; 0.38 theta; -0.12 psi 2021-02-13T01:10:14Z- handleDepth()-Depth_Keller msg: 1613178614525 epoch sec; seqNo:19152; depth 6.63 2021-02-13T01:10:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178614.54 epoch sec; seqNo:19152; 4061003.13 north; 593791.24 east 2021-02-13T01:10:14Z- AHRS_M2 msg: 1613178614.93 epoch sec; seqNo:19002 2021-02-13T01:10:14Z- AHRS_M2 msg: 0.58 phi; 0.38 theta; -0.12 psi 2021-02-13T01:10:14Z- handleDepth()-Depth_Keller msg: 1613178614937 epoch sec; seqNo:19153; depth 6.50 2021-02-13T01:10:14Z- handleDvl() - RDI_Pathfinder msg: 1613178614.95 epoch sec; seqNo:9226 2021-02-13T01:10:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.39 , 105.29 , 126.12 , 78.48 2021-02-13T01:10:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.29 , -0.18 2021-02-13T01:10:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178614.97 epoch sec; seqNo:19153; 4061003.13 north; 593791.24 east 2021-02-13T01:10:15Z- AHRS_M2 msg: 1613178615.33 epoch sec; seqNo:19003 2021-02-13T01:10:15Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.12 psi 2021-02-13T01:10:15Z- handleDepth()-Depth_Keller msg: 1613178615341 epoch sec; seqNo:19154; depth 6.37 2021-02-13T01:10:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178615.36 epoch sec; seqNo:19154; 4061003.51 north; 593791.24 east 2021-02-13T01:10:15Z- AHRS_M2 msg: 1613178615.73 epoch sec; seqNo:19004 2021-02-13T01:10:15Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.13 psi 2021-02-13T01:10:15Z- handleDepth()-Depth_Keller msg: 1613178615737 epoch sec; seqNo:19155; depth 6.24 2021-02-13T01:10:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178615.75 epoch sec; seqNo:19155; 4061003.51 north; 593791.24 east 2021-02-13T01:10:16Z- AHRS_M2 msg: 1613178616.13 epoch sec; seqNo:19005 2021-02-13T01:10:16Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.15 psi 2021-02-13T01:10:16Z- handleDepth()-Depth_Keller msg: 1613178616139 epoch sec; seqNo:19156; depth 6.11 2021-02-13T01:10:16Z- handleDvl() - RDI_Pathfinder msg: 1613178616.15 epoch sec; seqNo:9227 2021-02-13T01:10:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.27 , 106.44 , 126.72 , 78.48 2021-02-13T01:10:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.07 , -0.29 2021-02-13T01:10:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178616.17 epoch sec; seqNo:19156; 4061003.89 north; 593791.24 east 2021-02-13T01:10:16Z- AHRS_M2 msg: 1613178616.54 epoch sec; seqNo:19006 2021-02-13T01:10:16Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.16 psi 2021-02-13T01:10:16Z- handleDepth()-Depth_Keller msg: 1613178616546 epoch sec; seqNo:19157; depth 5.97 2021-02-13T01:10:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178616.56 epoch sec; seqNo:19157; 4061003.89 north; 593791.24 east 2021-02-13T01:10:16Z- AHRS_M2 msg: 1613178616.94 epoch sec; seqNo:19007 2021-02-13T01:10:16Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.16 psi 2021-02-13T01:10:16Z- handleDepth()-Depth_Keller msg: 1613178616949 epoch sec; seqNo:19158; depth 5.86 2021-02-13T01:10:16Z- handleDvl() - RDI_Pathfinder msg: 1613178616.97 epoch sec; seqNo:9228 2021-02-13T01:10:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.04 , 106.91 , 127.74 , 79.70 2021-02-13T01:10:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.07 , -0.18 2021-02-13T01:10:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178616.98 epoch sec; seqNo:19158; 4061004.28 north; 593792.45 east 2021-02-13T01:10:17Z- AHRS_M2 msg: 1613178617.35 epoch sec; seqNo:19008 2021-02-13T01:10:17Z- AHRS_M2 msg: 0.59 phi; 0.37 theta; -0.16 psi 2021-02-13T01:10:17Z- handleDepth()-Depth_Keller msg: 1613178617354 epoch sec; seqNo:19159; depth 5.73 2021-02-13T01:10:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178617.37 epoch sec; seqNo:19159; 4061004.28 north; 593792.45 east 2021-02-13T01:10:17Z- AHRS_M2 msg: 1613178617.75 epoch sec; seqNo:19009 2021-02-13T01:10:17Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.14 psi 2021-02-13T01:10:17Z- handleDepth()-Depth_Keller msg: 1613178617754 epoch sec; seqNo:19160; depth 5.61 2021-02-13T01:10:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178617.76 epoch sec; seqNo:19160; 4061004.66 north; 593792.45 east 2021-02-13T01:10:18Z- AHRS_M2 msg: 1613178618.15 epoch sec; seqNo:19010 2021-02-13T01:10:18Z- AHRS_M2 msg: 0.62 phi; 0.37 theta; -0.13 psi 2021-02-13T01:10:18Z- handleDepth()-Depth_Keller msg: 1613178618159 epoch sec; seqNo:19161; depth 5.51 2021-02-13T01:10:18Z- handleDvl() - RDI_Pathfinder msg: 1613178618.18 epoch sec; seqNo:9229 2021-02-13T01:10:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.52 , 108.40 , 129.23 , 78.88 2021-02-13T01:10:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.04 , -0.05 2021-02-13T01:10:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178618.19 epoch sec; seqNo:19161; 4061005.04 north; 593792.44 east 2021-02-13T01:10:18Z- AHRS_M2 msg: 1613178618.56 epoch sec; seqNo:19011 2021-02-13T01:10:18Z- AHRS_M2 msg: 0.63 phi; 0.36 theta; -0.12 psi 2021-02-13T01:10:18Z- handleDepth()-Depth_Keller msg: 1613178618559 epoch sec; seqNo:19162; depth 5.37 2021-02-13T01:10:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178618.57 epoch sec; seqNo:19162; 4061005.04 north; 593792.44 east 2021-02-13T01:10:18Z- AHRS_M2 msg: 1613178618.96 epoch sec; seqNo:19012 2021-02-13T01:10:18Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.11 psi 2021-02-13T01:10:18Z- LcmTrn::handleMessages() - handling 42 messages took 0.65 ms and 129 calls... 2021-02-13T01:10:18Z- waiting for 1613178616.72, time is 1613178618.96 2021-02-13T01:10:18Z- LcmTrn::updateTrn() >>>> pose time 1613178618.96, meas time 1613178618.18 <<<< 2021-02-13T01:10:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178618.18 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:10:18Z- TerrainNav::motionUpdate() - currEstimate = (4061005, 593792.44, 5.37) 2021-02-13T01:10:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:10:18Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:10:18Z- TNavBF::Projecting Measurements 2021-02-13T01:10:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:10:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:10:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:10:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:10:18Z- TNavFilter::4 good beams. 2021-02-13T01:10:18Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:10:18Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:10:18Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:10:18Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:10:18Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:10:18Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:10:18Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:10:18Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:18Z- TerrainMapOctree: (vehN, vehE ) = (4061004.64, 593792.08). 2021-02-13T01:10:18Z- TerrainMapOctree: Min Distance = 519.51. 2021-02-13T01:10:18Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:10:18Z- There is no specific good beam that all particles have in common. 2021-02-13T01:10:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:10:18Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 13.99 2021-02-13T01:10:18Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:10:18Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 18.80 2021-02-13T01:10:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:10:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.032 ExpectedMeasDiff: nan currMeas.covariance: 26.72 2021-02-13T01:10:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:10:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.90 K: 0.072 ExpectedMeasDiff: nan currMeas.covariance: 9.96 2021-02-13T01:10:19Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:10:19Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:10:19Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:10:19Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:10:19Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:10:19Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:10:19Z- Filter 0 No beams used in subcloud update 2021-02-13T01:10:19Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:10:19Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:10:19Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 673.6 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T01:10:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178618.18 sec. 2021-02-13T01:10:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:10:19Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:19Z- variances([0], [2], and psi_berg) = 673.573625 832.985843 0.000000 2021-02-13T01:10:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:10:19Z- LcmTrn::publishEstimates() - MLE : 4061004.54 593807.94 5.37 2021-02-13T01:10:19Z- LcmTrn::publishEstimates() - MMSE : 4061005.04 593792.44 5.37 2021-02-13T01:10:19Z- LcmTrn::publishEstimates() - COVAR: 673.57 832.99 0.87 0.00 2021-02-13T01:10:19Z- handleDepth()-Depth_Keller msg: 1613178618967 epoch sec; seqNo:19163; depth 5.23 2021-02-13T01:10:19Z- handleDvl() - RDI_Pathfinder msg: 1613178618.98 epoch sec; seqNo:9230 2021-02-13T01:10:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.94 , 105.36 , 126.18 , 78.88 2021-02-13T01:10:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , -0.06 , -0.07 2021-02-13T01:10:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178618.99 epoch sec; seqNo:19163; 4061005.42 north; 593792.44 east 2021-02-13T01:10:19Z- AHRS_M2 msg: 1613178619.36 epoch sec; seqNo:19013 2021-02-13T01:10:19Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.11 psi 2021-02-13T01:10:19Z- handleDepth()-Depth_Keller msg: 1613178619380 epoch sec; seqNo:19164; depth 5.11 2021-02-13T01:10:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178619.39 epoch sec; seqNo:19164; 4061005.42 north; 593792.44 east 2021-02-13T01:10:19Z- AHRS_M2 msg: 1613178619.77 epoch sec; seqNo:19014 2021-02-13T01:10:19Z- AHRS_M2 msg: 0.64 phi; 0.34 theta; -0.12 psi 2021-02-13T01:10:19Z- handleDepth()-Depth_Keller msg: 1613178619777 epoch sec; seqNo:19165; depth 4.97 2021-02-13T01:10:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178619.79 epoch sec; seqNo:19165; 4061005.80 north; 593792.44 east 2021-02-13T01:10:20Z- AHRS_M2 msg: 1613178620.17 epoch sec; seqNo:19015 2021-02-13T01:10:20Z- AHRS_M2 msg: 0.63 phi; 0.34 theta; -0.14 psi 2021-02-13T01:10:20Z- handleDepth()-Depth_Keller msg: 1613178620180 epoch sec; seqNo:19166; depth 4.83 2021-02-13T01:10:20Z- handleDvl() - RDI_Pathfinder msg: 1613178620.19 epoch sec; seqNo:9231 2021-02-13T01:10:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.53 , 105.43 , 124.77 , 81.62 2021-02-13T01:10:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.31 , 0.00 2021-02-13T01:10:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178620.20 epoch sec; seqNo:19166; 4061006.18 north; 593792.43 east 2021-02-13T01:10:20Z- AHRS_M2 msg: 1613178620.58 epoch sec; seqNo:19016 2021-02-13T01:10:20Z- AHRS_M2 msg: 0.62 phi; 0.34 theta; -0.17 psi 2021-02-13T01:10:20Z- handleDepth()-Depth_Keller msg: 1613178620583 epoch sec; seqNo:19167; depth 4.70 2021-02-13T01:10:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178620.59 epoch sec; seqNo:19167; 4061006.55 north; 593792.43 east 2021-02-13T01:10:21Z- AHRS_M2 msg: 1613178621.00 epoch sec; seqNo:19017 2021-02-13T01:10:21Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.17 psi 2021-02-13T01:10:21Z- handleDepth()-Depth_Keller msg: 1613178621006 epoch sec; seqNo:19168; depth 4.57 2021-02-13T01:10:21Z- handleDvl() - RDI_Pathfinder msg: 1613178621.02 epoch sec; seqNo:9232 2021-02-13T01:10:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.04 , 109.89 , 124.77 , 83.11 2021-02-13T01:10:21Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.22 , 0.31 2021-02-13T01:10:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178621.03 epoch sec; seqNo:19168; 4061006.95 north; 593793.64 east 2021-02-13T01:10:21Z- AHRS_M2 msg: 1613178621.38 epoch sec; seqNo:19018 2021-02-13T01:10:21Z- AHRS_M2 msg: 0.61 phi; 0.36 theta; -0.16 psi 2021-02-13T01:10:21Z- handleDepth()-Depth_Keller msg: 1613178621389 epoch sec; seqNo:19169; depth 4.45 2021-02-13T01:10:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178621.40 epoch sec; seqNo:19169; 4061007.33 north; 593793.64 east 2021-02-13T01:10:21Z- AHRS_M2 msg: 1613178621.79 epoch sec; seqNo:19019 2021-02-13T01:10:21Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.13 psi 2021-02-13T01:10:21Z- handleDepth()-Depth_Keller msg: 1613178621797 epoch sec; seqNo:19170; depth 4.34 2021-02-13T01:10:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178621.81 epoch sec; seqNo:19170; 4061007.70 north; 593793.64 east 2021-02-13T01:10:22Z- AHRS_M2 msg: 1613178622.19 epoch sec; seqNo:19020 2021-02-13T01:10:22Z- AHRS_M2 msg: 0.62 phi; 0.38 theta; -0.10 psi 2021-02-13T01:10:22Z- handleDepth()-Depth_Keller msg: 1613178622198 epoch sec; seqNo:19171; depth 4.24 2021-02-13T01:10:22Z- handleDvl() - RDI_Pathfinder msg: 1613178622.21 epoch sec; seqNo:9233 2021-02-13T01:10:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.94 , 108.33 , 126.18 , 81.56 2021-02-13T01:10:22Z- handleDvl() - RDI_Pathfinder msg: velocities 1.10 , 0.25 , 0.33 2021-02-13T01:10:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178622.22 epoch sec; seqNo:19171; 4061008.08 north; 593793.63 east 2021-02-13T01:10:22Z- AHRS_M2 msg: 1613178622.60 epoch sec; seqNo:19021 2021-02-13T01:10:22Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.09 psi 2021-02-13T01:10:22Z- handleDepth()-Depth_Keller msg: 1613178622610 epoch sec; seqNo:19172; depth 4.13 2021-02-13T01:10:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178622.62 epoch sec; seqNo:19172; 4061008.08 north; 593793.63 east 2021-02-13T01:10:23Z- AHRS_M2 msg: 1613178623.01 epoch sec; seqNo:19022 2021-02-13T01:10:23Z- AHRS_M2 msg: 0.65 phi; 0.36 theta; -0.09 psi 2021-02-13T01:10:23Z- handleDepth()-Depth_Keller msg: 1613178623013 epoch sec; seqNo:19173; depth 4.03 2021-02-13T01:10:23Z- handleDvl() - RDI_Pathfinder msg: 1613178623.03 epoch sec; seqNo:9234 2021-02-13T01:10:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 105.29 , 130.58 , 82.97 2021-02-13T01:10:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.38 , -0.05 2021-02-13T01:10:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178623.05 epoch sec; seqNo:19173; 4061008.48 north; 593794.85 east 2021-02-13T01:10:23Z- AHRS_M2 msg: 1613178623.41 epoch sec; seqNo:19023 2021-02-13T01:10:23Z- AHRS_M2 msg: 0.66 phi; 0.35 theta; -0.10 psi 2021-02-13T01:10:23Z- handleDepth()-Depth_Keller msg: 1613178623414 epoch sec; seqNo:19174; depth 3.90 2021-02-13T01:10:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178623.42 epoch sec; seqNo:19174; 4061008.48 north; 593794.85 east 2021-02-13T01:10:23Z- AHRS_M2 msg: 1613178623.81 epoch sec; seqNo:19024 2021-02-13T01:10:23Z- AHRS_M2 msg: 0.66 phi; 0.33 theta; -0.13 psi 2021-02-13T01:10:23Z- handleDepth()-Depth_Keller msg: 1613178623813 epoch sec; seqNo:19175; depth 3.78 2021-02-13T01:10:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178623.82 epoch sec; seqNo:19175; 4061008.85 north; 593794.84 east 2021-02-13T01:10:24Z- LcmTrn::handleMessages() - handling 43 messages took 0.57 ms and 132 calls... 2021-02-13T01:10:24Z- waiting for 1613178621.96, time is 1613178623.82 2021-02-13T01:10:24Z- LcmTrn::updateTrn() >>>> pose time 1613178623.82, meas time 1613178623.03 <<<< 2021-02-13T01:10:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178623.03 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:10:24Z- TerrainNav::motionUpdate() - currEstimate = (4061009, 593794.84, 3.78) 2021-02-13T01:10:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:10:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:10:24Z- TNavBF::Projecting Measurements 2021-02-13T01:10:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:10:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:10:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:10:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:10:24Z- TNavFilter::4 good beams. 2021-02-13T01:10:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:10:24Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:10:24Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:10:24Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:10:24Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:10:24Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:10:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:10:24Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:24Z- TerrainMapOctree: (vehN, vehE ) = (4061008.23, 593794.45). 2021-02-13T01:10:24Z- TerrainMapOctree: Min Distance = 515.73. 2021-02-13T01:10:24Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:10:24Z- There is no specific good beam that all particles have in common. 2021-02-13T01:10:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:10:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.06 K: 0.058 ExpectedMeasDiff: nan currMeas.covariance: 15.32 2021-02-13T01:10:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:10:24Z- Old Depth Bias: 0.00 DepthBiasCov: 1.00 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 17.74 2021-02-13T01:10:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:10:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.032 ExpectedMeasDiff: nan currMeas.covariance: 27.28 2021-02-13T01:10:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:10:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.067 ExpectedMeasDiff: nan currMeas.covariance: 11.02 2021-02-13T01:10:24Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:10:24Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:10:24Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:10:24Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:10:24Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:10:24Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:10:24Z- Filter 0 No beams used in subcloud update 2021-02-13T01:10:24Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:10:24Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:10:24Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 673.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:10:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178623.03 sec. 2021-02-13T01:10:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:10:24Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:24Z- variances([0], [2], and psi_berg) = 673.516133 832.985820 0.000000 2021-02-13T01:10:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:10:24Z- LcmTrn::publishEstimates() - MLE : 4061008.35 593810.34 3.78 2021-02-13T01:10:24Z- LcmTrn::publishEstimates() - MMSE : 4061008.85 593794.84 3.78 2021-02-13T01:10:24Z- LcmTrn::publishEstimates() - COVAR: 673.52 832.99 0.89 0.00 2021-02-13T01:10:24Z- AHRS_M2 msg: 1613178624.21 epoch sec; seqNo:19025 2021-02-13T01:10:24Z- AHRS_M2 msg: 0.65 phi; 0.32 theta; -0.17 psi 2021-02-13T01:10:24Z- handleDepth()-Depth_Keller msg: 1613178624216 epoch sec; seqNo:19176; depth 3.65 2021-02-13T01:10:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178624.24 epoch sec; seqNo:19176; 4061008.87 north; 593796.06 east 2021-02-13T01:10:24Z- AHRS_M2 msg: 1613178624.62 epoch sec; seqNo:19026 2021-02-13T01:10:24Z- AHRS_M2 msg: 0.61 phi; 0.35 theta; -0.20 psi 2021-02-13T01:10:24Z- handleDepth()-Depth_Keller msg: 1613178624626 epoch sec; seqNo:19177; depth 3.51 2021-02-13T01:10:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178624.63 epoch sec; seqNo:19177; 4061009.25 north; 593796.06 east 2021-02-13T01:10:25Z- AHRS_M2 msg: 1613178625.03 epoch sec; seqNo:19027 2021-02-13T01:10:25Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.18 psi 2021-02-13T01:10:25Z- handleDepth()-Depth_Keller msg: 1613178625029 epoch sec; seqNo:19178; depth 3.40 2021-02-13T01:10:25Z- handleDvl() - RDI_Pathfinder msg: 1613178625.05 epoch sec; seqNo:9235 2021-02-13T01:10:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.81 , 106.71 , 124.56 , 84.39 2021-02-13T01:10:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , 0.03 , -0.14 2021-02-13T01:10:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178625.07 epoch sec; seqNo:19178; 4061009.25 north; 593796.06 east 2021-02-13T01:10:25Z- AHRS_M2 msg: 1613178625.42 epoch sec; seqNo:19028 2021-02-13T01:10:25Z- AHRS_M2 msg: 0.61 phi; 0.40 theta; -0.16 psi 2021-02-13T01:10:25Z- handleDepth()-Depth_Keller msg: 1613178625430 epoch sec; seqNo:19179; depth 3.26 2021-02-13T01:10:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178625.44 epoch sec; seqNo:19179; 4061009.63 north; 593796.06 east 2021-02-13T01:10:25Z- AHRS_M2 msg: 1613178625.83 epoch sec; seqNo:19029 2021-02-13T01:10:25Z- AHRS_M2 msg: 0.61 phi; 0.40 theta; -0.16 psi 2021-02-13T01:10:25Z- handleDepth()-Depth_Keller msg: 1613178625832 epoch sec; seqNo:19180; depth 3.17 2021-02-13T01:10:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178625.85 epoch sec; seqNo:19180; 4061009.63 north; 593796.06 east 2021-02-13T01:10:26Z- AHRS_M2 msg: 1613178626.23 epoch sec; seqNo:19030 2021-02-13T01:10:26Z- AHRS_M2 msg: 0.63 phi; 0.39 theta; -0.17 psi 2021-02-13T01:10:26Z- handleDepth()-Depth_Keller msg: 1613178626241 epoch sec; seqNo:19181; depth 3.05 2021-02-13T01:10:26Z- handleDvl() - RDI_Pathfinder msg: 1613178626.25 epoch sec; seqNo:9236 2021-02-13T01:10:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 109.68 , 134.97 , 84.39 2021-02-13T01:10:26Z- handleDvl() - RDI_Pathfinder msg: velocities 1.11 , 0.40 , -0.07 2021-02-13T01:10:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178626.27 epoch sec; seqNo:19181; 4061010.00 north; 593796.05 east 2021-02-13T01:10:26Z- AHRS_M2 msg: 1613178626.64 epoch sec; seqNo:19031 2021-02-13T01:10:26Z- AHRS_M2 msg: 0.64 phi; 0.37 theta; -0.18 psi 2021-02-13T01:10:26Z- handleDepth()-Depth_Keller msg: 1613178626645 epoch sec; seqNo:19182; depth 2.93 2021-02-13T01:10:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178626.65 epoch sec; seqNo:19182; 4061010.02 north; 593797.27 east 2021-02-13T01:10:27Z- AHRS_M2 msg: 1613178627.06 epoch sec; seqNo:19032 2021-02-13T01:10:27Z- AHRS_M2 msg: 0.64 phi; 0.35 theta; -0.20 psi 2021-02-13T01:10:27Z- handleDepth()-Depth_Keller msg: 1613178627072 epoch sec; seqNo:19183; depth 2.80 2021-02-13T01:10:27Z- handleDvl() - RDI_Pathfinder msg: 1613178627.08 epoch sec; seqNo:9237 2021-02-13T01:10:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.03 , 116.85 , 131.73 , 84.12 2021-02-13T01:10:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.08 , -0.52 2021-02-13T01:10:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178627.10 epoch sec; seqNo:19183; 4061010.40 north; 593797.27 east 2021-02-13T01:10:27Z- AHRS_M2 msg: 1613178627.44 epoch sec; seqNo:19033 2021-02-13T01:10:27Z- AHRS_M2 msg: 0.64 phi; 0.33 theta; -0.20 psi 2021-02-13T01:10:27Z- handleDepth()-Depth_Keller msg: 1613178627448 epoch sec; seqNo:19184; depth 2.66 2021-02-13T01:10:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178627.46 epoch sec; seqNo:19184; 4061010.78 north; 593797.26 east 2021-02-13T01:10:27Z- AHRS_M2 msg: 1613178627.85 epoch sec; seqNo:19034 2021-02-13T01:10:27Z- AHRS_M2 msg: 0.62 phi; 0.32 theta; -0.18 psi 2021-02-13T01:10:27Z- handleDepth()-Depth_Keller msg: 1613178627852 epoch sec; seqNo:19185; depth 2.53 2021-02-13T01:10:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178627.87 epoch sec; seqNo:19185; 4061010.78 north; 593797.26 east 2021-02-13T01:10:28Z- AHRS_M2 msg: 1613178628.25 epoch sec; seqNo:19035 2021-02-13T01:10:28Z- AHRS_M2 msg: 0.60 phi; 0.33 theta; -0.17 psi 2021-02-13T01:10:28Z- handleDepth()-Depth_Keller msg: 1613178628258 epoch sec; seqNo:19186; depth 2.42 2021-02-13T01:10:28Z- handleDvl() - RDI_Pathfinder msg: 1613178628.27 epoch sec; seqNo:9238 2021-02-13T01:10:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.05 , 116.85 , 127.26 , 85.61 2021-02-13T01:10:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.13 , -0.05 2021-02-13T01:10:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178628.28 epoch sec; seqNo:19186; 4061011.15 north; 593797.26 east 2021-02-13T01:10:28Z- AHRS_M2 msg: 1613178628.66 epoch sec; seqNo:19036 2021-02-13T01:10:28Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.18 psi 2021-02-13T01:10:28Z- handleDepth()-Depth_Keller msg: 1613178628660 epoch sec; seqNo:19187; depth 2.28 2021-02-13T01:10:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178628.72 epoch sec; seqNo:19187; 4061011.53 north; 593797.26 east 2021-02-13T01:10:29Z- AHRS_M2 msg: 1613178629.06 epoch sec; seqNo:19037 2021-02-13T01:10:29Z- AHRS_M2 msg: 0.59 phi; 0.36 theta; -0.17 psi 2021-02-13T01:10:29Z- handleDepth()-Depth_Keller msg: 1613178629067 epoch sec; seqNo:19188; depth 2.16 2021-02-13T01:10:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178629.08 epoch sec; seqNo:19188; 4061011.53 north; 593797.26 east 2021-02-13T01:10:29Z- LcmTrn::handleMessages() - handling 43 messages took 0.69 ms and 129 calls... 2021-02-13T01:10:29Z- waiting for 1613178626.82, time is 1613178629.08 2021-02-13T01:10:29Z- LcmTrn::updateTrn() >>>> pose time 1613178629.08, meas time 1613178628.27 <<<< 2021-02-13T01:10:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178628.27 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:10:29Z- TerrainNav::motionUpdate() - currEstimate = (4061012, 593797.26, 2.16) 2021-02-13T01:10:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:10:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:10:29Z- TNavBF::Projecting Measurements 2021-02-13T01:10:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:10:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:10:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:10:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:10:29Z- TNavFilter::4 good beams. 2021-02-13T01:10:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:10:29Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:10:29Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:10:29Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:10:29Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:10:29Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:10:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:10:29Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:29Z- TerrainMapOctree: (vehN, vehE ) = (4061011.12, 593796.88). 2021-02-13T01:10:29Z- TerrainMapOctree: Min Distance = 512.23. 2021-02-13T01:10:29Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:10:29Z- There is no specific good beam that all particles have in common. 2021-02-13T01:10:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:10:29Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 13.85 2021-02-13T01:10:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:10:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 21.85 2021-02-13T01:10:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:10:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.033 ExpectedMeasDiff: nan currMeas.covariance: 25.91 2021-02-13T01:10:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:10:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.063 ExpectedMeasDiff: nan currMeas.covariance: 11.73 2021-02-13T01:10:29Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:10:29Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:10:29Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:10:29Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:10:29Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:29Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:10:29Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:10:29Z- Filter 0 No beams used in subcloud update 2021-02-13T01:10:29Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:10:29Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:10:29Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 673.5 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:10:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178628.27 sec. 2021-02-13T01:10:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:10:29Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:29Z- variances([0], [2], and psi_berg) = 673.465003 832.985800 0.000000 2021-02-13T01:10:29Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:10:29Z- LcmTrn::publishEstimates() - MLE : 4061011.03 593811.76 2.16 2021-02-13T01:10:29Z- LcmTrn::publishEstimates() - MMSE : 4061011.53 593797.26 2.16 2021-02-13T01:10:29Z- LcmTrn::publishEstimates() - COVAR: 673.47 832.99 0.88 0.00 2021-02-13T01:10:29Z- AHRS_M2 msg: 1613178629.46 epoch sec; seqNo:19038 2021-02-13T01:10:29Z- AHRS_M2 msg: 0.60 phi; 0.36 theta; -0.17 psi 2021-02-13T01:10:29Z- handleDepth()-Depth_Keller msg: 1613178629468 epoch sec; seqNo:19189; depth 2.04 2021-02-13T01:10:29Z- handleDvl() - RDI_Pathfinder msg: 1613178629.48 epoch sec; seqNo:9239 2021-02-13T01:10:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.18 , 114.01 , 127.40 , 84.26 2021-02-13T01:10:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.02 , 0.18 2021-02-13T01:10:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178629.49 epoch sec; seqNo:19189; 4061011.92 north; 593798.47 east 2021-02-13T01:10:29Z- AHRS_M2 msg: 1613178629.87 epoch sec; seqNo:19039 2021-02-13T01:10:29Z- AHRS_M2 msg: 0.61 phi; 0.37 theta; -0.17 psi 2021-02-13T01:10:29Z- handleDepth()-Depth_Keller msg: 1613178629877 epoch sec; seqNo:19190; depth 1.94 2021-02-13T01:10:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178629.89 epoch sec; seqNo:19190; 4061012.30 north; 593798.47 east 2021-02-13T01:10:30Z- AHRS_M2 msg: 1613178630.27 epoch sec; seqNo:19040 2021-02-13T01:10:30Z- AHRS_M2 msg: 0.62 phi; 0.38 theta; -0.16 psi 2021-02-13T01:10:30Z- handleDepth()-Depth_Keller msg: 1613178630278 epoch sec; seqNo:19191; depth 1.82 2021-02-13T01:10:30Z- handleDvl() - RDI_Pathfinder msg: 1613178630.29 epoch sec; seqNo:9240 2021-02-13T01:10:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.74 , 112.59 , 130.44 , 85.82 2021-02-13T01:10:30Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , 0.02 , 0.00 2021-02-13T01:10:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178630.30 epoch sec; seqNo:19191; 4061012.68 north; 593798.46 east 2021-02-13T01:10:30Z- AHRS_M2 msg: 1613178630.67 epoch sec; seqNo:19041 2021-02-13T01:10:30Z- AHRS_M2 msg: 0.63 phi; 0.38 theta; -0.13 psi 2021-02-13T01:10:30Z- handleDepth()-Depth_Keller msg: 1613178630685 epoch sec; seqNo:19192; depth 1.73 2021-02-13T01:10:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178630.70 epoch sec; seqNo:19192; 4061012.68 north; 593798.46 east 2021-02-13T01:10:31Z- AHRS_M2 msg: 1613178631.08 epoch sec; seqNo:19042 2021-02-13T01:10:31Z- AHRS_M2 msg: 0.66 phi; 0.37 theta; -0.10 psi 2021-02-13T01:10:31Z- handleDepth()-Depth_Keller msg: 1613178631084 epoch sec; seqNo:19193; depth 1.65 2021-02-13T01:10:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178631.09 epoch sec; seqNo:19193; 4061013.06 north; 593798.46 east 2021-02-13T01:10:31Z- AHRS_M2 msg: 1613178631.48 epoch sec; seqNo:19043 2021-02-13T01:10:31Z- AHRS_M2 msg: 0.67 phi; 0.34 theta; -0.07 psi 2021-02-13T01:10:31Z- handleDepth()-Depth_Keller msg: 1613178631497 epoch sec; seqNo:19194; depth 1.57 2021-02-13T01:10:31Z- handleDvl() - RDI_Pathfinder msg: 1613178631.51 epoch sec; seqNo:9241 2021-02-13T01:10:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.58 , 113.94 , 137.74 , 87.17 2021-02-13T01:10:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.15 , 0.01 2021-02-13T01:10:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178631.52 epoch sec; seqNo:19194; 4061013.45 north; 593799.68 east 2021-02-13T01:10:31Z- AHRS_M2 msg: 1613178631.89 epoch sec; seqNo:19044 2021-02-13T01:10:31Z- AHRS_M2 msg: 0.64 phi; 0.31 theta; -0.08 psi 2021-02-13T01:10:31Z- handleDepth()-Depth_Keller msg: 1613178631892 epoch sec; seqNo:19195; depth 1.46 2021-02-13T01:10:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178631.91 epoch sec; seqNo:19195; 4061013.45 north; 593799.68 east 2021-02-13T01:10:32Z- AHRS_M2 msg: 1613178632.29 epoch sec; seqNo:19045 2021-02-13T01:10:32Z- AHRS_M2 msg: 0.60 phi; 0.31 theta; -0.13 psi 2021-02-13T01:10:32Z- handleDepth()-Depth_Keller msg: 1613178632301 epoch sec; seqNo:19196; depth 1.29 2021-02-13T01:10:32Z- handleDvl() - RDI_Pathfinder msg: 1613178632.32 epoch sec; seqNo:9242 2021-02-13T01:10:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 100.29 , 106.24 , 127.06 , 86.90 2021-02-13T01:10:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.38 , 0.11 , -0.36 2021-02-13T01:10:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178632.33 epoch sec; seqNo:19196; 4061013.45 north; 593799.68 east 2021-02-13T01:10:32Z- AHRS_M2 msg: 1613178632.70 epoch sec; seqNo:19046 2021-02-13T01:10:32Z- AHRS_M2 msg: 0.59 phi; 0.33 theta; -0.19 psi 2021-02-13T01:10:32Z- handleDepth()-Depth_Keller msg: 1613178632700 epoch sec; seqNo:19197; depth 1.13 2021-02-13T01:10:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178632.71 epoch sec; seqNo:19197; 4061013.83 north; 593799.67 east 2021-02-13T01:10:33Z- AHRS_M2 msg: 1613178633.10 epoch sec; seqNo:19047 2021-02-13T01:10:33Z- AHRS_M2 msg: 0.59 phi; 0.35 theta; -0.20 psi 2021-02-13T01:10:33Z- handleDepth()-Depth_Keller msg: 1613178633106 epoch sec; seqNo:19198; depth 1.00 2021-02-13T01:10:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178633.12 epoch sec; seqNo:19198; 4061013.83 north; 593799.67 east 2021-02-13T01:10:33Z- AHRS_M2 msg: 1613178633.51 epoch sec; seqNo:19048 2021-02-13T01:10:33Z- AHRS_M2 msg: 0.60 phi; 0.37 theta; -0.19 psi 2021-02-13T01:10:33Z- handleDepth()-Depth_Keller msg: 1613178633528 epoch sec; seqNo:19199; depth 0.89 2021-02-13T01:10:33Z- handleDvl() - RDI_Pathfinder msg: 1613178633.54 epoch sec; seqNo:9243 2021-02-13T01:10:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.67 , 115.50 , 127.40 , 85.75 2021-02-13T01:10:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.10 , -0.01 2021-02-13T01:10:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178633.55 epoch sec; seqNo:19199; 4061014.21 north; 593799.67 east 2021-02-13T01:10:33Z- AHRS_M2 msg: 1613178633.91 epoch sec; seqNo:19049 2021-02-13T01:10:33Z- AHRS_M2 msg: 0.60 phi; 0.40 theta; -0.20 psi 2021-02-13T01:10:33Z- handleDepth()-Depth_Keller msg: 1613178633912 epoch sec; seqNo:19200; depth 0.76 2021-02-13T01:10:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178633.92 epoch sec; seqNo:19200; 4061014.21 north; 593799.67 east 2021-02-13T01:10:34Z- LcmTrn::handleMessages() - handling 41 messages took 0.65 ms and 128 calls... 2021-02-13T01:10:34Z- waiting for 1613178632.08, time is 1613178633.92 2021-02-13T01:10:34Z- LcmTrn::updateTrn() >>>> pose time 1613178633.92, meas time 1613178633.54 <<<< 2021-02-13T01:10:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178633.54 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:10:34Z- TerrainNav::motionUpdate() - currEstimate = (4061014, 593799.67, 0.76) 2021-02-13T01:10:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:10:34Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:10:34Z- TNavBF::Projecting Measurements 2021-02-13T01:10:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:10:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:10:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:10:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:10:34Z- TNavFilter::4 good beams. 2021-02-13T01:10:34Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:10:34Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:10:34Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:10:34Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:10:34Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:10:34Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:10:34Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:10:34Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:34Z- TerrainMapOctree: (vehN, vehE ) = (4061014.00, 593799.48). 2021-02-13T01:10:34Z- TerrainMapOctree: Min Distance = 508.59. 2021-02-13T01:10:34Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:10:34Z- There is no specific good beam that all particles have in common. 2021-02-13T01:10:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:10:34Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.061 ExpectedMeasDiff: nan currMeas.covariance: 14.34 2021-02-13T01:10:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:10:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 21.34 2021-02-13T01:10:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:10:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.95 K: 0.033 ExpectedMeasDiff: nan currMeas.covariance: 25.97 2021-02-13T01:10:34Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:10:34Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.064 ExpectedMeasDiff: nan currMeas.covariance: 11.76 2021-02-13T01:10:34Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:10:34Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:10:34Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:10:34Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:10:34Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:34Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:10:34Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:10:34Z- Filter 0 No beams used in subcloud update 2021-02-13T01:10:34Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:10:34Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:10:34Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 673.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:10:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178633.54 sec. 2021-02-13T01:10:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:10:34Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:34Z- variances([0], [2], and psi_berg) = 673.416670 832.985781 0.000000 2021-02-13T01:10:34Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:10:34Z- LcmTrn::publishEstimates() - MLE : 4061013.71 593815.17 0.76 2021-02-13T01:10:34Z- LcmTrn::publishEstimates() - MMSE : 4061014.21 593799.67 0.76 2021-02-13T01:10:34Z- LcmTrn::publishEstimates() - COVAR: 673.42 832.99 0.87 0.00 2021-02-13T01:10:34Z- AHRS_M2 msg: 1613178634.31 epoch sec; seqNo:19050 2021-02-13T01:10:34Z- AHRS_M2 msg: 0.61 phi; 0.41 theta; -0.18 psi 2021-02-13T01:10:34Z- handleDepth()-Depth_Keller msg: 1613178634317 epoch sec; seqNo:19201; depth 0.66 2021-02-13T01:10:34Z- handleDvl() - RDI_Pathfinder msg: 1613178634.33 epoch sec; seqNo:9244 2021-02-13T01:10:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.09 , 118.41 , 139.23 , 87.17 2021-02-13T01:10:34Z- handleDvl() - RDI_Pathfinder msg: velocities 1.18 , -0.09 , 0.21 2021-02-13T01:10:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178634.35 epoch sec; seqNo:19201; 4061014.97 north; 593799.66 east 2021-02-13T01:10:34Z- AHRS_M2 msg: 1613178634.71 epoch sec; seqNo:19051 2021-02-13T01:10:34Z- AHRS_M2 msg: 0.63 phi; 0.40 theta; -0.11 psi 2021-02-13T01:10:34Z- handleDepth()-Depth_Keller msg: 1613178634729 epoch sec; seqNo:19202; depth 0.56 2021-02-13T01:10:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178634.76 epoch sec; seqNo:19202; 4061015.36 north; 593800.87 east 2021-02-13T01:10:35Z- AHRS_M2 msg: 1613178635.12 epoch sec; seqNo:19052 2021-02-13T01:10:35Z- AHRS_M2 msg: 0.65 phi; 0.34 theta; -0.04 psi 2021-02-13T01:10:35Z- handleDepth()-Depth_Keller msg: 1613178635126 epoch sec; seqNo:19203; depth 0.46 2021-02-13T01:10:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178635.15 epoch sec; seqNo:19203; 4061015.74 north; 593800.87 east 2021-02-13T01:10:35Z- AHRS_M2 msg: 1613178635.52 epoch sec; seqNo:19053 2021-02-13T01:10:35Z- AHRS_M2 msg: 0.64 phi; 0.28 theta; 0.01 psi 2021-02-13T01:10:35Z- handleDepth()-Depth_Keller msg: 1613178635528 epoch sec; seqNo:19204; depth 0.40 2021-02-13T01:10:35Z- handleDvl() - RDI_Pathfinder msg: 1613178635.56 epoch sec; seqNo:9245 2021-02-13T01:10:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.71 , 113.61 , 0.00 , 85.34 2021-02-13T01:10:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.16 , 0.10 2021-02-13T01:10:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178635.57 epoch sec; seqNo:19204; 4061015.74 north; 593800.87 east 2021-02-13T01:10:35Z- AHRS_M2 msg: 1613178635.93 epoch sec; seqNo:19054 2021-02-13T01:10:35Z- AHRS_M2 msg: 0.61 phi; 0.24 theta; 0.01 psi 2021-02-13T01:10:35Z- handleDepth()-Depth_Keller msg: 1613178635932 epoch sec; seqNo:19205; depth 0.30 2021-02-13T01:10:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178635.94 epoch sec; seqNo:19205; 4061016.12 north; 593800.87 east 2021-02-13T01:10:36Z- AHRS_M2 msg: 1613178636.33 epoch sec; seqNo:19055 2021-02-13T01:10:36Z- AHRS_M2 msg: 0.62 phi; 0.18 theta; -0.00 psi 2021-02-13T01:10:36Z- handleDepth()-Depth_Keller msg: 1613178636345 epoch sec; seqNo:19206; depth 0.26 2021-02-13T01:10:36Z- handleDvl() - RDI_Pathfinder msg: 1613178636.36 epoch sec; seqNo:9246 2021-02-13T01:10:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 102.45 , 95.69 , 120.03 , 88.93 2021-02-13T01:10:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , -0.01 2021-02-13T01:10:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178636.37 epoch sec; seqNo:19206; 4061016.50 north; 593800.86 east 2021-02-13T01:10:36Z- AHRS_M2 msg: 1613178636.74 epoch sec; seqNo:19056 2021-02-13T01:10:36Z- AHRS_M2 msg: 0.65 phi; 0.11 theta; -0.01 psi 2021-02-13T01:10:36Z- handleDepth()-Depth_Keller msg: 1613178636740 epoch sec; seqNo:19207; depth 0.23 2021-02-13T01:10:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178636.76 epoch sec; seqNo:19207; 4061016.89 north; 593802.08 east 2021-02-13T01:10:37Z- AHRS_M2 msg: 1613178637.14 epoch sec; seqNo:19057 2021-02-13T01:10:37Z- AHRS_M2 msg: 0.64 phi; 0.07 theta; -0.01 psi 2021-02-13T01:10:37Z- handleDepth()-Depth_Keller msg: 1613178637144 epoch sec; seqNo:19208; depth 0.22 2021-02-13T01:10:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178637.19 epoch sec; seqNo:19208; 4061016.89 north; 593802.08 east 2021-02-13T01:10:37Z- AHRS_M2 msg: 1613178637.54 epoch sec; seqNo:19058 2021-02-13T01:10:37Z- AHRS_M2 msg: 0.63 phi; 0.09 theta; 0.07 psi 2021-02-13T01:10:37Z- handleDepth()-Depth_Keller msg: 1613178637548 epoch sec; seqNo:19209; depth 0.21 2021-02-13T01:10:37Z- handleDvl() - RDI_Pathfinder msg: 1613178637.57 epoch sec; seqNo:9247 2021-02-13T01:10:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.48 , 96.50 , 105.43 , 97.99 2021-02-13T01:10:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , 0.29 , 0.03 2021-02-13T01:10:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178637.60 epoch sec; seqNo:19209; 4061017.27 north; 593802.07 east 2021-02-13T01:10:37Z- AHRS_M2 msg: 1613178637.95 epoch sec; seqNo:19059 2021-02-13T01:10:37Z- AHRS_M2 msg: 0.64 phi; 0.11 theta; 0.15 psi 2021-02-13T01:10:37Z- handleDepth()-Depth_Keller msg: 1613178637954 epoch sec; seqNo:19210; depth 0.24 2021-02-13T01:10:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178637.98 epoch sec; seqNo:19210; 4061017.27 north; 593802.07 east 2021-02-13T01:10:38Z- AHRS_M2 msg: 1613178638.35 epoch sec; seqNo:19060 2021-02-13T01:10:38Z- AHRS_M2 msg: 0.65 phi; 0.06 theta; 0.10 psi 2021-02-13T01:10:38Z- handleDepth()-Depth_Keller msg: 1613178638356 epoch sec; seqNo:19211; depth 0.20 2021-02-13T01:10:38Z- handleDvl() - RDI_Pathfinder msg: 1613178638.38 epoch sec; seqNo:9248 2021-02-13T01:10:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 107.86 , 92.99 , 107.86 , 97.04 2021-02-13T01:10:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.50 , 0.92 , -0.37 2021-02-13T01:10:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178638.39 epoch sec; seqNo:19211; 4061017.27 north; 593802.07 east 2021-02-13T01:10:38Z- AHRS_M2 msg: 1613178638.76 epoch sec; seqNo:19061 2021-02-13T01:10:38Z- AHRS_M2 msg: 0.63 phi; -0.01 theta; -0.04 psi 2021-02-13T01:10:38Z- handleDepth()-Depth_Keller msg: 1613178638760 epoch sec; seqNo:19212; depth 0.16 2021-02-13T01:10:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178638.77 epoch sec; seqNo:19212; 4061017.28 north; 593803.30 east 2021-02-13T01:10:39Z- AHRS_M2 msg: 1613178639.16 epoch sec; seqNo:19062 2021-02-13T01:10:39Z- AHRS_M2 msg: 0.58 phi; 0.03 theta; -0.13 psi 2021-02-13T01:10:39Z- handleDepth()-Depth_Keller msg: 1613178639171 epoch sec; seqNo:19213; depth 0.13 2021-02-13T01:10:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178639.18 epoch sec; seqNo:19213; 4061017.28 north; 593803.30 east 2021-02-13T01:10:39Z- LcmTrn::handleMessages() - handling 44 messages took 0.66 ms and 131 calls... 2021-02-13T01:10:39Z- waiting for 1613178636.92, time is 1613178639.18 2021-02-13T01:10:39Z- LcmTrn::updateTrn() >>>> pose time 1613178639.18, meas time 1613178638.38 <<<< 2021-02-13T01:10:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178638.38 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:10:39Z- TerrainNav::motionUpdate() - currEstimate = (4061017, 593803.30, 0.13) 2021-02-13T01:10:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:10:39Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:10:39Z- TNavBF::Projecting Measurements 2021-02-13T01:10:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:10:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:10:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:10:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:10:39Z- TNavFilter::4 good beams. 2021-02-13T01:10:39Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:10:39Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:10:39Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:10:39Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:10:39Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:10:39Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:10:39Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:10:39Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:39Z- TerrainMapOctree: (vehN, vehE ) = (4061016.82, 593802.74). 2021-02-13T01:10:39Z- TerrainMapOctree: Min Distance = 504.39. 2021-02-13T01:10:39Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:10:39Z- There is no specific good beam that all particles have in common. 2021-02-13T01:10:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:10:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.08 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 18.62 2021-02-13T01:10:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:10:39Z- Old Depth Bias: 0.00 DepthBiasCov: 1.02 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 13.83 2021-02-13T01:10:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:10:39Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 18.62 2021-02-13T01:10:39Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:10:39Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 15.07 2021-02-13T01:10:39Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:10:39Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:10:39Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:10:39Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:10:39Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:39Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:10:39Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:10:39Z- Filter 0 No beams used in subcloud update 2021-02-13T01:10:39Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:10:39Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:10:39Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 673.4 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:10:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178638.38 sec. 2021-02-13T01:10:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:10:39Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:39Z- variances([0], [2], and psi_berg) = 673.358081 832.985757 0.000000 2021-02-13T01:10:39Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:10:39Z- LcmTrn::publishEstimates() - MLE : 4061016.78 593818.80 0.13 2021-02-13T01:10:39Z- LcmTrn::publishEstimates() - MMSE : 4061017.28 593803.30 0.13 2021-02-13T01:10:39Z- LcmTrn::publishEstimates() - COVAR: 673.36 832.99 0.91 0.00 2021-02-13T01:10:39Z- handleDepth()-Depth_Keller msg: 1613178639593 epoch sec; seqNo:19214; depth 0.07 2021-02-13T01:10:39Z- handleDvl() - RDI_Pathfinder msg: 1613178639.61 epoch sec; seqNo:9249 2021-02-13T01:10:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 99.47 , 97.99 , 97.99 2021-02-13T01:10:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.50 , -0.19 , -0.55 2021-02-13T01:10:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178639.63 epoch sec; seqNo:19214; 4061017.28 north; 593803.30 east 2021-02-13T01:10:39Z- AHRS_M2 msg: 1613178639.97 epoch sec; seqNo:19063 2021-02-13T01:10:40Z- AHRS_M2 msg: 0.56 phi; 0.02 theta; 0.03 psi 2021-02-13T01:10:40Z- handleDepth()-Depth_Keller msg: 1613178639973 epoch sec; seqNo:19215; depth 0.10 2021-02-13T01:10:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178639.99 epoch sec; seqNo:19215; 4061017.66 north; 593803.29 east 2021-02-13T01:10:40Z- AHRS_M2 msg: 1613178640.37 epoch sec; seqNo:19064 2021-02-13T01:10:40Z- AHRS_M2 msg: 0.56 phi; 0.03 theta; 0.12 psi 2021-02-13T01:10:40Z- handleDepth()-Depth_Keller msg: 1613178640376 epoch sec; seqNo:19216; depth 0.09 2021-02-13T01:10:40Z- handleDvl() - RDI_Pathfinder msg: 1613178640.39 epoch sec; seqNo:9250 2021-02-13T01:10:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.17 , 97.17 , 98.53 , 97.17 2021-02-13T01:10:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.57 , -0.31 , -0.22 2021-02-13T01:10:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178640.41 epoch sec; seqNo:19216; 4061018.04 north; 593803.29 east 2021-02-13T01:10:40Z- AHRS_M2 msg: 1613178640.78 epoch sec; seqNo:19065 2021-02-13T01:10:40Z- AHRS_M2 msg: 0.61 phi; -0.00 theta; 0.15 psi 2021-02-13T01:10:40Z- handleDepth()-Depth_Keller msg: 1613178640780 epoch sec; seqNo:19217; depth 0.05 2021-02-13T01:10:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178640.79 epoch sec; seqNo:19217; 4061018.04 north; 593803.29 east 2021-02-13T01:10:41Z- AHRS_M2 msg: 1613178641.18 epoch sec; seqNo:19066 2021-02-13T01:10:41Z- AHRS_M2 msg: 0.62 phi; -0.12 theta; 0.11 psi 2021-02-13T01:10:41Z- handleDepth()-Depth_Keller msg: 1613178641185 epoch sec; seqNo:19218; depth 0.06 2021-02-13T01:10:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178641.20 epoch sec; seqNo:19218; 4061018.42 north; 593803.28 east 2021-02-13T01:10:41Z- AHRS_M2 msg: 1613178641.58 epoch sec; seqNo:19067 2021-02-13T01:10:41Z- AHRS_M2 msg: 0.54 phi; -0.13 theta; 0.05 psi 2021-02-13T01:10:41Z- handleDepth()-Depth_Keller msg: 1613178641604 epoch sec; seqNo:19219; depth 0.05 2021-02-13T01:10:41Z- handleDvl() - RDI_Pathfinder msg: 1613178641.62 epoch sec; seqNo:9251 2021-02-13T01:10:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 106.71 , 97.24 , 97.24 , 101.30 2021-02-13T01:10:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.13 , -0.16 , -0.21 2021-02-13T01:10:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178641.63 epoch sec; seqNo:19219; 4061018.42 north; 593803.28 east 2021-02-13T01:10:41Z- AHRS_M2 msg: 1613178641.99 epoch sec; seqNo:19068 2021-02-13T01:10:41Z- AHRS_M2 msg: 0.49 phi; -0.08 theta; 0.03 psi 2021-02-13T01:10:41Z- handleDepth()-Depth_Keller msg: 1613178641998 epoch sec; seqNo:19220; depth 0.03 2021-02-13T01:10:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178642.03 epoch sec; seqNo:19220; 4061018.42 north; 593803.28 east 2021-02-13T01:10:42Z- AHRS_M2 msg: 1613178642.40 epoch sec; seqNo:19069 2021-02-13T01:10:42Z- AHRS_M2 msg: 0.46 phi; -0.04 theta; 0.24 psi 2021-02-13T01:10:42Z- handleDepth()-Depth_Keller msg: 1613178642400 epoch sec; seqNo:19221; depth 0.07 2021-02-13T01:10:42Z- handleDvl() - RDI_Pathfinder msg: 1613178642.43 epoch sec; seqNo:9252 2021-02-13T01:10:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.68 , 96.97 , 92.92 , 105.09 2021-02-13T01:10:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.60 , -0.37 , 0.70 2021-02-13T01:10:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178642.45 epoch sec; seqNo:19221; 4061018.80 north; 593803.28 east 2021-02-13T01:10:42Z- AHRS_M2 msg: 1613178642.80 epoch sec; seqNo:19070 2021-02-13T01:10:42Z- AHRS_M2 msg: 0.45 phi; 0.01 theta; 0.34 psi 2021-02-13T01:10:42Z- handleDepth()-Depth_Keller msg: 1613178642809 epoch sec; seqNo:19222; depth 0.12 2021-02-13T01:10:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178642.82 epoch sec; seqNo:19222; 4061019.18 north; 593803.27 east 2021-02-13T01:10:43Z- AHRS_M2 msg: 1613178643.20 epoch sec; seqNo:19071 2021-02-13T01:10:43Z- AHRS_M2 msg: 0.49 phi; 0.00 theta; 0.22 psi 2021-02-13T01:10:43Z- handleDepth()-Depth_Keller msg: 1613178643204 epoch sec; seqNo:19223; depth 0.14 2021-02-13T01:10:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178643.21 epoch sec; seqNo:19223; 4061019.56 north; 593803.27 east 2021-02-13T01:10:43Z- AHRS_M2 msg: 1613178643.60 epoch sec; seqNo:19072 2021-02-13T01:10:43Z- AHRS_M2 msg: 0.50 phi; -0.05 theta; 0.07 psi 2021-02-13T01:10:43Z- handleDepth()-Depth_Keller msg: 1613178643608 epoch sec; seqNo:19224; depth 0.13 2021-02-13T01:10:43Z- handleDvl() - RDI_Pathfinder msg: 1613178643.62 epoch sec; seqNo:9253 2021-02-13T01:10:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 122.87 , 97.17 , 99.88 , 101.23 2021-02-13T01:10:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , 0.76 , 0.78 2021-02-13T01:10:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178643.63 epoch sec; seqNo:19224; 4061019.56 north; 593803.27 east 2021-02-13T01:10:44Z- handleDepth()-Depth_Keller msg: 1613178644033 epoch sec; seqNo:19225; depth 0.10 2021-02-13T01:10:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178644.05 epoch sec; seqNo:19225; 4061019.57 north; 593804.49 east 2021-02-13T01:10:44Z- AHRS_M2 msg: 1613178644.41 epoch sec; seqNo:19073 2021-02-13T01:10:44Z- AHRS_M2 msg: 0.44 phi; -0.09 theta; -0.08 psi 2021-02-13T01:10:44Z- handleDepth()-Depth_Keller msg: 1613178644417 epoch sec; seqNo:19226; depth 0.06 2021-02-13T01:10:44Z- handleDvl() - RDI_Pathfinder msg: 1613178644.44 epoch sec; seqNo:9254 2021-02-13T01:10:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.60 , 97.65 , 97.65 , 102.11 2021-02-13T01:10:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.37 , 0.32 , -0.55 2021-02-13T01:10:44Z- LcmTrn::handleMessages() - handling 42 messages took 1.01 ms and 122 calls... 2021-02-13T01:10:44Z- waiting for 1613178642.18, time is 1613178644.44 2021-02-13T01:10:44Z- LcmTrn::updateTrn() equating pose 1613178644.44 and meas times 1613178644.44 2021-02-13T01:10:44Z- LcmTrn::updateTrn() >>>> pose time 1613178644.44, meas time 1613178644.44 <<<< 2021-02-13T01:10:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178644.44 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:10:44Z- TerrainNav::motionUpdate() - currEstimate = (4061020, 593804.49, 0.06) 2021-02-13T01:10:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:10:44Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:10:44Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178644.44 sec.; No relevant navigation data available 2021-02-13T01:10:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:10:44Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:44Z- variances([0], [2], and psi_berg) = 673.325156 832.985744 0.000000 2021-02-13T01:10:44Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:10:44Z- LcmTrn::publishEstimates() - MLE : 4061019.07 593819.99 0.06 2021-02-13T01:10:44Z- LcmTrn::publishEstimates() - MMSE : 4061019.57 593804.49 0.06 2021-02-13T01:10:44Z- LcmTrn::publishEstimates() - COVAR: 673.33 832.99 1.04 0.00 2021-02-13T01:10:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178644.45 epoch sec; seqNo:19226; 4061019.57 north; 593804.49 east 2021-02-13T01:10:44Z- AHRS_M2 msg: 1613178644.82 epoch sec; seqNo:19074 2021-02-13T01:10:44Z- AHRS_M2 msg: 0.40 phi; -0.02 theta; -0.04 psi 2021-02-13T01:10:44Z- handleDepth()-Depth_Keller msg: 1613178644821 epoch sec; seqNo:19227; depth 0.02 2021-02-13T01:10:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178644.83 epoch sec; seqNo:19227; 4061019.95 north; 593804.49 east 2021-02-13T01:10:45Z- AHRS_M2 msg: 1613178645.22 epoch sec; seqNo:19075 2021-02-13T01:10:45Z- AHRS_M2 msg: 0.41 phi; 0.04 theta; 0.11 psi 2021-02-13T01:10:45Z- handleDepth()-Depth_Keller msg: 1613178645225 epoch sec; seqNo:19228; depth 0.03 2021-02-13T01:10:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178645.23 epoch sec; seqNo:19228; 4061019.95 north; 593804.49 east 2021-02-13T01:10:45Z- AHRS_M2 msg: 1613178645.62 epoch sec; seqNo:19076 2021-02-13T01:10:45Z- AHRS_M2 msg: 0.46 phi; -0.01 theta; 0.21 psi 2021-02-13T01:10:45Z- handleDepth()-Depth_Keller msg: 1613178645638 epoch sec; seqNo:19229; depth 0.08 2021-02-13T01:10:45Z- handleDvl() - RDI_Pathfinder msg: 1613178645.65 epoch sec; seqNo:9255 2021-02-13T01:10:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.69 , 102.45 , 94.33 , 103.80 2021-02-13T01:10:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.61 , -0.01 , 0.22 2021-02-13T01:10:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178645.66 epoch sec; seqNo:19229; 4061020.33 north; 593804.48 east 2021-02-13T01:10:46Z- AHRS_M2 msg: 1613178646.03 epoch sec; seqNo:19077 2021-02-13T01:10:46Z- AHRS_M2 msg: 0.50 phi; -0.08 theta; 0.20 psi 2021-02-13T01:10:46Z- handleDepth()-Depth_Keller msg: 1613178646032 epoch sec; seqNo:19230; depth 0.09 2021-02-13T01:10:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178646.04 epoch sec; seqNo:19230; 4061020.33 north; 593804.48 east 2021-02-13T01:10:46Z- AHRS_M2 msg: 1613178646.43 epoch sec; seqNo:19078 2021-02-13T01:10:46Z- AHRS_M2 msg: 0.46 phi; -0.08 theta; 0.08 psi 2021-02-13T01:10:46Z- handleDepth()-Depth_Keller msg: 1613178646436 epoch sec; seqNo:19231; depth 0.05 2021-02-13T01:10:46Z- handleDvl() - RDI_Pathfinder msg: 1613178646.47 epoch sec; seqNo:9256 2021-02-13T01:10:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 113.33 , 97.11 , 99.81 , 101.16 2021-02-13T01:10:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.18 , 0.26 , -0.43 2021-02-13T01:10:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178646.49 epoch sec; seqNo:19231; 4061020.33 north; 593804.48 east 2021-02-13T01:10:46Z- AHRS_M2 msg: 1613178646.84 epoch sec; seqNo:19079 2021-02-13T01:10:46Z- AHRS_M2 msg: 0.45 phi; -0.03 theta; 0.01 psi 2021-02-13T01:10:46Z- handleDepth()-Depth_Keller msg: 1613178646840 epoch sec; seqNo:19232; depth 0.06 2021-02-13T01:10:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178646.87 epoch sec; seqNo:19232; 4061020.33 north; 593804.48 east 2021-02-13T01:10:47Z- AHRS_M2 msg: 1613178647.24 epoch sec; seqNo:19080 2021-02-13T01:10:47Z- AHRS_M2 msg: 0.49 phi; -0.03 theta; 0.00 psi 2021-02-13T01:10:47Z- handleDepth()-Depth_Keller msg: 1613178647251 epoch sec; seqNo:19233; depth 0.08 2021-02-13T01:10:47Z- handleDvl() - RDI_Pathfinder msg: 1613178647.30 epoch sec; seqNo:9257 2021-02-13T01:10:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.34 , 97.85 , 94.88 , 99.34 2021-02-13T01:10:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.34 , 0.25 , 0.35 2021-02-13T01:10:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178647.31 epoch sec; seqNo:19233; 4061020.34 north; 593805.70 east 2021-02-13T01:10:47Z- AHRS_M2 msg: 1613178647.64 epoch sec; seqNo:19081 2021-02-13T01:10:47Z- AHRS_M2 msg: 0.54 phi; -0.07 theta; -0.04 psi 2021-02-13T01:10:47Z- handleDepth()-Depth_Keller msg: 1613178647648 epoch sec; seqNo:19234; depth 0.08 2021-02-13T01:10:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178647.68 epoch sec; seqNo:19234; 4061020.72 north; 593805.70 east 2021-02-13T01:10:48Z- AHRS_M2 msg: 1613178648.05 epoch sec; seqNo:19082 2021-02-13T01:10:48Z- AHRS_M2 msg: 0.54 phi; -0.08 theta; -0.12 psi 2021-02-13T01:10:48Z- handleDepth()-Depth_Keller msg: 1613178648052 epoch sec; seqNo:19235; depth 0.05 2021-02-13T01:10:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178648.06 epoch sec; seqNo:19235; 4061020.72 north; 593805.70 east 2021-02-13T01:10:48Z- AHRS_M2 msg: 1613178648.45 epoch sec; seqNo:19083 2021-02-13T01:10:48Z- AHRS_M2 msg: 0.59 phi; -0.02 theta; -0.11 psi 2021-02-13T01:10:48Z- handleDepth()-Depth_Keller msg: 1613178648458 epoch sec; seqNo:19236; depth 0.01 2021-02-13T01:10:48Z- handleDvl() - RDI_Pathfinder msg: 1613178648.47 epoch sec; seqNo:9258 2021-02-13T01:10:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 99.95 , 97.24 , 104.00 2021-02-13T01:10:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.37 , -0.05 , -0.47 2021-02-13T01:10:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178648.48 epoch sec; seqNo:19236; 4061020.72 north; 593805.70 east 2021-02-13T01:10:48Z- AHRS_M2 msg: 1613178648.86 epoch sec; seqNo:19084 2021-02-13T01:10:48Z- AHRS_M2 msg: 0.64 phi; -0.03 theta; 0.00 psi 2021-02-13T01:10:48Z- handleDepth()-Depth_Keller msg: 1613178648860 epoch sec; seqNo:19237; depth 0.06 2021-02-13T01:10:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178648.88 epoch sec; seqNo:19237; 4061020.72 north; 593805.70 east 2021-02-13T01:10:49Z- AHRS_M2 msg: 1613178649.26 epoch sec; seqNo:19085 2021-02-13T01:10:49Z- AHRS_M2 msg: 0.59 phi; -0.15 theta; 0.13 psi 2021-02-13T01:10:49Z- handleDepth()-Depth_Keller msg: 1613178649267 epoch sec; seqNo:19238; depth 0.07 2021-02-13T01:10:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178649.29 epoch sec; seqNo:19238; 4061021.10 north; 593805.69 east 2021-02-13T01:10:49Z- LcmTrn::handleMessages() - handling 41 messages took 0.76 ms and 126 calls... 2021-02-13T01:10:49Z- waiting for 1613178647.44, time is 1613178649.29 2021-02-13T01:10:49Z- LcmTrn::updateTrn() >>>> pose time 1613178649.29, meas time 1613178648.47 <<<< 2021-02-13T01:10:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178648.47 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:10:49Z- TerrainNav::motionUpdate() - currEstimate = (4061021, 593805.69, 0.07) 2021-02-13T01:10:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:10:49Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:10:49Z- TNavBF::Projecting Measurements 2021-02-13T01:10:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:10:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:10:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:10:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:10:49Z- TNavFilter::4 good beams. 2021-02-13T01:10:49Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:10:49Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:10:49Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:10:49Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:10:49Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:10:49Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:10:49Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:10:49Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:49Z- TerrainMapOctree: (vehN, vehE ) = (4061020.84, 593805.49). 2021-02-13T01:10:49Z- TerrainMapOctree: Min Distance = 500.10. 2021-02-13T01:10:49Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:10:49Z- There is no specific good beam that all particles have in common. 2021-02-13T01:10:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:10:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.12 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-13T01:10:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:10:49Z- Old Depth Bias: 0.00 DepthBiasCov: 1.05 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 15.98 2021-02-13T01:10:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:10:49Z- Old Depth Bias: 0.00 DepthBiasCov: 0.99 K: 0.056 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-13T01:10:49Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:10:49Z- Old Depth Bias: 0.00 DepthBiasCov: 0.94 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 17.31 2021-02-13T01:10:49Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:10:49Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:10:49Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:10:49Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:10:49Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:49Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:10:49Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:10:49Z- Filter 0 No beams used in subcloud update 2021-02-13T01:10:49Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:10:49Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:10:49Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 673.3 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:10:49Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178648.47 sec. 2021-02-13T01:10:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:10:49Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:49Z- variances([0], [2], and psi_berg) = 673.287378 832.985730 0.000000 2021-02-13T01:10:49Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:10:49Z- LcmTrn::publishEstimates() - MLE : 4061020.60 593821.19 0.07 2021-02-13T01:10:49Z- LcmTrn::publishEstimates() - MMSE : 4061021.10 593805.69 0.07 2021-02-13T01:10:49Z- LcmTrn::publishEstimates() - COVAR: 673.29 832.99 0.91 0.00 2021-02-13T01:10:49Z- AHRS_M2 msg: 1613178649.67 epoch sec; seqNo:19086 2021-02-13T01:10:49Z- AHRS_M2 msg: 0.49 phi; -0.09 theta; 0.21 psi 2021-02-13T01:10:49Z- handleDepth()-Depth_Keller msg: 1613178649672 epoch sec; seqNo:19239; depth 0.07 2021-02-13T01:10:49Z- handleDvl() - RDI_Pathfinder msg: 1613178649.68 epoch sec; seqNo:9259 2021-02-13T01:10:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 101.10 , 97.04 , 97.04 2021-02-13T01:10:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.11 , -0.16 , 0.25 2021-02-13T01:10:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178649.69 epoch sec; seqNo:19239; 4061021.10 north; 593805.69 east 2021-02-13T01:10:50Z- AHRS_M2 msg: 1613178650.07 epoch sec; seqNo:19087 2021-02-13T01:10:50Z- AHRS_M2 msg: 0.53 phi; 0.08 theta; 0.23 psi 2021-02-13T01:10:50Z- handleDepth()-Depth_Keller msg: 1613178650077 epoch sec; seqNo:19240; depth 0.07 2021-02-13T01:10:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178650.09 epoch sec; seqNo:19240; 4061021.10 north; 593805.69 east 2021-02-13T01:10:50Z- AHRS_M2 msg: 1613178650.47 epoch sec; seqNo:19088 2021-02-13T01:10:50Z- AHRS_M2 msg: 0.62 phi; 0.08 theta; 0.20 psi 2021-02-13T01:10:50Z- handleDepth()-Depth_Keller msg: 1613178650479 epoch sec; seqNo:19241; depth 0.06 2021-02-13T01:10:50Z- handleDvl() - RDI_Pathfinder msg: 1613178650.50 epoch sec; seqNo:9260 2021-02-13T01:10:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 116.11 , 97.17 , 97.17 , 105.29 2021-02-13T01:10:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.04 , 0.75 2021-02-13T01:10:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178650.51 epoch sec; seqNo:19241; 4061021.48 north; 593805.69 east 2021-02-13T01:10:50Z- AHRS_M2 msg: 1613178650.88 epoch sec; seqNo:19089 2021-02-13T01:10:50Z- AHRS_M2 msg: 0.67 phi; 0.02 theta; 0.08 psi 2021-02-13T01:10:50Z- handleDepth()-Depth_Keller msg: 1613178650880 epoch sec; seqNo:19242; depth 0.06 2021-02-13T01:10:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178650.89 epoch sec; seqNo:19242; 4061021.86 north; 593805.69 east 2021-02-13T01:10:51Z- AHRS_M2 msg: 1613178651.29 epoch sec; seqNo:19090 2021-02-13T01:10:51Z- AHRS_M2 msg: 0.68 phi; -0.03 theta; -0.09 psi 2021-02-13T01:10:51Z- handleDepth()-Depth_Keller msg: 1613178651294 epoch sec; seqNo:19243; depth 0.02 2021-02-13T01:10:51Z- handleDvl() - RDI_Pathfinder msg: 1613178651.32 epoch sec; seqNo:9261 2021-02-13T01:10:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 106.71 , 97.78 , 102.25 , 99.27 2021-02-13T01:10:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.35 , 0.49 , -0.22 2021-02-13T01:10:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178651.34 epoch sec; seqNo:19243; 4061021.86 north; 593805.69 east 2021-02-13T01:10:51Z- AHRS_M2 msg: 1613178651.68 epoch sec; seqNo:19091 2021-02-13T01:10:51Z- AHRS_M2 msg: 0.71 phi; -0.00 theta; -0.16 psi 2021-02-13T01:10:51Z- handleDepth()-Depth_Keller msg: 1613178651698 epoch sec; seqNo:19244; depth 0.00 2021-02-13T01:10:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178651.73 epoch sec; seqNo:19244; 4061021.87 north; 593806.91 east 2021-02-13T01:10:52Z- AHRS_M2 msg: 1613178652.09 epoch sec; seqNo:19092 2021-02-13T01:10:52Z- AHRS_M2 msg: 0.75 phi; 0.05 theta; -0.07 psi 2021-02-13T01:10:52Z- handleDepth()-Depth_Keller msg: 1613178652097 epoch sec; seqNo:19245; depth 0.03 2021-02-13T01:10:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178652.13 epoch sec; seqNo:19245; 4061021.87 north; 593806.91 east 2021-02-13T01:10:52Z- AHRS_M2 msg: 1613178652.49 epoch sec; seqNo:19093 2021-02-13T01:10:52Z- AHRS_M2 msg: 0.80 phi; 0.03 theta; 0.04 psi 2021-02-13T01:10:52Z- handleDepth()-Depth_Keller msg: 1613178652499 epoch sec; seqNo:19246; depth 0.05 2021-02-13T01:10:52Z- handleDvl() - RDI_Pathfinder msg: 1613178652.52 epoch sec; seqNo:9262 2021-02-13T01:10:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 106.91 , 99.47 , 97.99 2021-02-13T01:10:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.12 , 0.40 , 0.04 2021-02-13T01:10:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178652.55 epoch sec; seqNo:19246; 4061021.49 north; 593806.91 east 2021-02-13T01:10:52Z- AHRS_M2 msg: 1613178652.90 epoch sec; seqNo:19094 2021-02-13T01:10:52Z- AHRS_M2 msg: 0.84 phi; -0.14 theta; 0.09 psi 2021-02-13T01:10:52Z- handleDepth()-Depth_Keller msg: 1613178652901 epoch sec; seqNo:19247; depth 0.06 2021-02-13T01:10:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178652.91 epoch sec; seqNo:19247; 4061021.49 north; 593806.91 east 2021-02-13T01:10:53Z- AHRS_M2 msg: 1613178653.32 epoch sec; seqNo:19095 2021-02-13T01:10:53Z- AHRS_M2 msg: 0.78 phi; -0.22 theta; 0.05 psi 2021-02-13T01:10:53Z- handleDepth()-Depth_Keller msg: 1613178653326 epoch sec; seqNo:19248; depth 0.05 2021-02-13T01:10:53Z- handleDvl() - RDI_Pathfinder msg: 1613178653.34 epoch sec; seqNo:9263 2021-02-13T01:10:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 105.09 , 96.97 , 96.97 , 103.73 2021-02-13T01:10:53Z- handleDvl() - RDI_Pathfinder msg: velocities -0.47 , 0.55 , -0.58 2021-02-13T01:10:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178653.35 epoch sec; seqNo:19248; 4061021.11 north; 593806.91 east 2021-02-13T01:10:53Z- AHRS_M2 msg: 1613178653.70 epoch sec; seqNo:19096 2021-02-13T01:10:53Z- AHRS_M2 msg: 0.72 phi; -0.11 theta; -0.03 psi 2021-02-13T01:10:53Z- handleDepth()-Depth_Keller msg: 1613178653708 epoch sec; seqNo:19249; depth 0.02 2021-02-13T01:10:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178653.72 epoch sec; seqNo:19249; 4061021.11 north; 593806.91 east 2021-02-13T01:10:54Z- AHRS_M2 msg: 1613178654.11 epoch sec; seqNo:19097 2021-02-13T01:10:54Z- AHRS_M2 msg: 0.73 phi; 0.01 theta; -0.00 psi 2021-02-13T01:10:54Z- handleDepth()-Depth_Keller msg: 1613178654112 epoch sec; seqNo:19250; depth 0.04 2021-02-13T01:10:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178654.12 epoch sec; seqNo:19250; 4061020.73 north; 593806.92 east 2021-02-13T01:10:54Z- AHRS_M2 msg: 1613178654.51 epoch sec; seqNo:19098 2021-02-13T01:10:54Z- AHRS_M2 msg: 0.80 phi; 0.01 theta; 0.07 psi 2021-02-13T01:10:54Z- handleDepth()-Depth_Keller msg: 1613178654522 epoch sec; seqNo:19251; depth 0.08 2021-02-13T01:10:54Z- handleDvl() - RDI_Pathfinder msg: 1613178654.54 epoch sec; seqNo:9264 2021-02-13T01:10:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 99.68 , 96.97 , 96.97 2021-02-13T01:10:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , -0.32 , 0.24 2021-02-13T01:10:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178654.55 epoch sec; seqNo:19251; 4061021.11 north; 593806.91 east 2021-02-13T01:10:54Z- LcmTrn::handleMessages() - handling 45 messages took 0.74 ms and 130 calls... 2021-02-13T01:10:54Z- waiting for 1613178652.29, time is 1613178654.55 2021-02-13T01:10:54Z- LcmTrn::updateTrn() equating pose 1613178654.55 and meas times 1613178654.55 2021-02-13T01:10:54Z- LcmTrn::updateTrn() >>>> pose time 1613178654.55, meas time 1613178654.55 <<<< 2021-02-13T01:10:54Z- TerrainNav:: Throwing out all beams because more than two are equal to 96.972 at t=1613178654.55, ping # 0. 2021-02-13T01:10:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178654.55 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:10:54Z- TerrainNav::motionUpdate() - currEstimate = (4061021, 593806.91, 0.08) 2021-02-13T01:10:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:10:54Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:10:54Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178654.55 sec.; No relevant navigation data available 2021-02-13T01:10:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:10:54Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:54Z- variances([0], [2], and psi_berg) = 673.264750 832.985721 0.000000 2021-02-13T01:10:54Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:10:54Z- LcmTrn::publishEstimates() - MLE : 4061020.61 593822.41 0.08 2021-02-13T01:10:54Z- LcmTrn::publishEstimates() - MMSE : 4061021.11 593806.91 0.08 2021-02-13T01:10:54Z- LcmTrn::publishEstimates() - COVAR: 673.26 832.99 0.97 0.00 2021-02-13T01:10:54Z- AHRS_M2 msg: 1613178654.92 epoch sec; seqNo:19099 2021-02-13T01:10:54Z- AHRS_M2 msg: 0.83 phi; -0.10 theta; 0.12 psi 2021-02-13T01:10:54Z- handleDepth()-Depth_Keller msg: 1613178654920 epoch sec; seqNo:19252; depth 0.08 2021-02-13T01:10:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178654.94 epoch sec; seqNo:19252; 4061021.49 north; 593806.91 east 2021-02-13T01:10:55Z- AHRS_M2 msg: 1613178655.33 epoch sec; seqNo:19100 2021-02-13T01:10:55Z- AHRS_M2 msg: 0.79 phi; -0.13 theta; 0.09 psi 2021-02-13T01:10:55Z- handleDepth()-Depth_Keller msg: 1613178655332 epoch sec; seqNo:19253; depth 0.05 2021-02-13T01:10:55Z- handleDvl() - RDI_Pathfinder msg: 1613178655.35 epoch sec; seqNo:9265 2021-02-13T01:10:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 105.36 , 91.83 , 97.24 , 102.65 2021-02-13T01:10:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.20 , -0.23 , -0.01 2021-02-13T01:10:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178655.36 epoch sec; seqNo:19253; 4061021.49 north; 593806.91 east 2021-02-13T01:10:55Z- AHRS_M2 msg: 1613178655.72 epoch sec; seqNo:19101 2021-02-13T01:10:55Z- AHRS_M2 msg: 0.77 phi; -0.05 theta; 0.01 psi 2021-02-13T01:10:55Z- handleDepth()-Depth_Keller msg: 1613178655728 epoch sec; seqNo:19254; depth 0.02 2021-02-13T01:10:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178655.74 epoch sec; seqNo:19254; 4061021.49 north; 593806.91 east 2021-02-13T01:10:56Z- AHRS_M2 msg: 1613178656.13 epoch sec; seqNo:19102 2021-02-13T01:10:56Z- AHRS_M2 msg: 0.77 phi; 0.02 theta; 0.02 psi 2021-02-13T01:10:56Z- handleDepth()-Depth_Keller msg: 1613178656138 epoch sec; seqNo:19255; depth 0.03 2021-02-13T01:10:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178656.17 epoch sec; seqNo:19255; 4061021.87 north; 593806.91 east 2021-02-13T01:10:56Z- AHRS_M2 msg: 1613178656.53 epoch sec; seqNo:19103 2021-02-13T01:10:56Z- AHRS_M2 msg: 0.78 phi; 0.03 theta; 0.10 psi 2021-02-13T01:10:56Z- handleDepth()-Depth_Keller msg: 1613178656536 epoch sec; seqNo:19256; depth 0.06 2021-02-13T01:10:56Z- handleDvl() - RDI_Pathfinder msg: 1613178656.56 epoch sec; seqNo:9266 2021-02-13T01:10:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.46 , 95.75 , 98.46 , 95.75 2021-02-13T01:10:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.25 , 0.84 2021-02-13T01:10:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178656.57 epoch sec; seqNo:19256; 4061021.87 north; 593806.91 east 2021-02-13T01:10:56Z- AHRS_M2 msg: 1613178656.94 epoch sec; seqNo:19104 2021-02-13T01:10:56Z- AHRS_M2 msg: 0.81 phi; -0.00 theta; 0.18 psi 2021-02-13T01:10:56Z- handleDepth()-Depth_Keller msg: 1613178656940 epoch sec; seqNo:19257; depth 0.07 2021-02-13T01:10:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178656.95 epoch sec; seqNo:19257; 4061022.25 north; 593806.90 east 2021-02-13T01:10:57Z- AHRS_M2 msg: 1613178657.34 epoch sec; seqNo:19105 2021-02-13T01:10:57Z- AHRS_M2 msg: 0.83 phi; 0.02 theta; 0.17 psi 2021-02-13T01:10:57Z- handleDepth()-Depth_Keller msg: 1613178657344 epoch sec; seqNo:19258; depth 0.08 2021-02-13T01:10:57Z- handleDvl() - RDI_Pathfinder msg: 1613178657.36 epoch sec; seqNo:9267 2021-02-13T01:10:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 113.54 , 90.55 , 100.01 , 100.01 2021-02-13T01:10:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.32 , 0.94 , 0.96 2021-02-13T01:10:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178657.37 epoch sec; seqNo:19258; 4061022.26 north; 593808.12 east 2021-02-13T01:10:57Z- AHRS_M2 msg: 1613178657.74 epoch sec; seqNo:19106 2021-02-13T01:10:57Z- AHRS_M2 msg: 0.85 phi; 0.01 theta; 0.14 psi 2021-02-13T01:10:57Z- handleDepth()-Depth_Keller msg: 1613178657749 epoch sec; seqNo:19259; depth 0.06 2021-02-13T01:10:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178657.76 epoch sec; seqNo:19259; 4061021.88 north; 593808.13 east 2021-02-13T01:10:58Z- AHRS_M2 msg: 1613178658.15 epoch sec; seqNo:19107 2021-02-13T01:10:58Z- AHRS_M2 msg: 0.80 phi; -0.05 theta; 0.15 psi 2021-02-13T01:10:58Z- handleDepth()-Depth_Keller msg: 1613178658156 epoch sec; seqNo:19260; depth 0.10 2021-02-13T01:10:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178658.18 epoch sec; seqNo:19260; 4061021.88 north; 593808.13 east 2021-02-13T01:10:58Z- AHRS_M2 msg: 1613178658.55 epoch sec; seqNo:19108 2021-02-13T01:10:58Z- AHRS_M2 msg: 0.73 phi; -0.07 theta; 0.07 psi 2021-02-13T01:10:58Z- handleDepth()-Depth_Keller msg: 1613178658556 epoch sec; seqNo:19261; depth 0.09 2021-02-13T01:10:58Z- handleDvl() - RDI_Pathfinder msg: 1613178658.57 epoch sec; seqNo:9268 2021-02-13T01:10:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 110.50 , 96.97 , 101.03 , 99.68 2021-02-13T01:10:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.02 , 1.99 , -0.96 2021-02-13T01:10:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178658.59 epoch sec; seqNo:19261; 4061021.52 north; 593809.35 east 2021-02-13T01:10:59Z- handleDepth()-Depth_Keller msg: 1613178659009 epoch sec; seqNo:19262; depth 0.09 2021-02-13T01:10:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178659.02 epoch sec; seqNo:19262; 4061020.76 north; 593809.36 east 2021-02-13T01:10:59Z- AHRS_M2 msg: 1613178659.37 epoch sec; seqNo:19109 2021-02-13T01:10:59Z- AHRS_M2 msg: 0.73 phi; -0.12 theta; -0.01 psi 2021-02-13T01:10:59Z- handleDepth()-Depth_Keller msg: 1613178659371 epoch sec; seqNo:19263; depth 0.14 2021-02-13T01:10:59Z- handleDvl() - RDI_Pathfinder msg: 1613178659.39 epoch sec; seqNo:9269 2021-02-13T01:10:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 103.87 , 97.92 , 99.41 2021-02-13T01:10:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.31 , -0.57 , -1.06 2021-02-13T01:10:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178659.40 epoch sec; seqNo:19263; 4061021.14 north; 593809.36 east 2021-02-13T01:10:59Z- LcmTrn::handleMessages() - handling 40 messages took 0.64 ms and 127 calls... 2021-02-13T01:10:59Z- waiting for 1613178657.55, time is 1613178659.40 2021-02-13T01:10:59Z- LcmTrn::updateTrn() equating pose 1613178659.40 and meas times 1613178659.40 2021-02-13T01:10:59Z- LcmTrn::updateTrn() >>>> pose time 1613178659.40, meas time 1613178659.40 <<<< 2021-02-13T01:10:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178659.40 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:10:59Z- TerrainNav::motionUpdate() - currEstimate = (4061021, 593809.36, 0.14) 2021-02-13T01:10:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:10:59Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:10:59Z- TNavBF::Projecting Measurements 2021-02-13T01:10:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:10:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:10:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:10:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:10:59Z- TNavFilter::4 good beams. 2021-02-13T01:10:59Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:10:59Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:10:59Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:10:59Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:10:59Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:10:59Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:10:59Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:10:59Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:59Z- TerrainMapOctree: (vehN, vehE ) = (4061021.14, 593809.36). 2021-02-13T01:10:59Z- TerrainMapOctree: Min Distance = 496.53. 2021-02-13T01:10:59Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:10:59Z- There is no specific good beam that all particles have in common. 2021-02-13T01:10:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:10:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.10 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T01:10:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:10:59Z- Old Depth Bias: 0.00 DepthBiasCov: 1.03 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 17.26 2021-02-13T01:10:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:10:59Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.054 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T01:10:59Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:10:59Z- Old Depth Bias: 0.00 DepthBiasCov: 0.92 K: 0.050 ExpectedMeasDiff: nan currMeas.covariance: 15.81 2021-02-13T01:10:59Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:10:59Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:10:59Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:10:59Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:10:59Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:10:59Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:10:59Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:10:59Z- Filter 0 No beams used in subcloud update 2021-02-13T01:10:59Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:10:59Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:10:59Z- Filter 0 NorthBias: 0.0 EastBias: -0.0 DepthBias: 0.0 NorthVariance: 673.2 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:10:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178659.40 sec. 2021-02-13T01:10:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:10:59Z- TNavBF::mmse selected from filter -1 2021-02-13T01:10:59Z- variances([0], [2], and psi_berg) = 673.232757 832.985708 0.000000 2021-02-13T01:10:59Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:10:59Z- LcmTrn::publishEstimates() - MLE : 4061020.64 593824.86 0.14 2021-02-13T01:10:59Z- LcmTrn::publishEstimates() - MMSE : 4061021.14 593809.36 0.14 2021-02-13T01:10:59Z- LcmTrn::publishEstimates() - COVAR: 673.23 832.99 0.88 0.00 2021-02-13T01:10:59Z- AHRS_M2 msg: 1613178659.76 epoch sec; seqNo:19110 2021-02-13T01:10:59Z- AHRS_M2 msg: 0.68 phi; -0.12 theta; 0.19 psi 2021-02-13T01:10:59Z- handleDepth()-Depth_Keller msg: 1613178659768 epoch sec; seqNo:19264; depth 0.21 2021-02-13T01:10:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178659.80 epoch sec; seqNo:19264; 4061021.14 north; 593809.36 east 2021-02-13T01:11:00Z- AHRS_M2 msg: 1613178660.17 epoch sec; seqNo:19111 2021-02-13T01:11:00Z- AHRS_M2 msg: 0.70 phi; -0.07 theta; 0.24 psi 2021-02-13T01:11:00Z- handleDepth()-Depth_Keller msg: 1613178660172 epoch sec; seqNo:19265; depth 0.25 2021-02-13T01:11:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178660.18 epoch sec; seqNo:19265; 4061021.52 north; 593809.35 east 2021-02-13T01:11:00Z- AHRS_M2 msg: 1613178660.58 epoch sec; seqNo:19112 2021-02-13T01:11:00Z- AHRS_M2 msg: 0.76 phi; -0.07 theta; 0.17 psi 2021-02-13T01:11:00Z- handleDepth()-Depth_Keller msg: 1613178660583 epoch sec; seqNo:19266; depth 0.30 2021-02-13T01:11:00Z- handleDvl() - RDI_Pathfinder msg: 1613178660.60 epoch sec; seqNo:9270 2021-02-13T01:11:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.74 , 97.04 , 97.04 , 106.51 2021-02-13T01:11:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.43 , -0.49 , 0.34 2021-02-13T01:11:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178660.62 epoch sec; seqNo:19266; 4061021.90 north; 593809.35 east 2021-02-13T01:11:00Z- AHRS_M2 msg: 1613178660.98 epoch sec; seqNo:19113 2021-02-13T01:11:00Z- AHRS_M2 msg: 0.77 phi; -0.15 theta; 0.10 psi 2021-02-13T01:11:00Z- handleDepth()-Depth_Keller msg: 1613178660981 epoch sec; seqNo:19267; depth 0.32 2021-02-13T01:11:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178660.99 epoch sec; seqNo:19267; 4061021.90 north; 593809.35 east 2021-02-13T01:11:01Z- AHRS_M2 msg: 1613178661.38 epoch sec; seqNo:19114 2021-02-13T01:11:01Z- AHRS_M2 msg: 0.72 phi; -0.23 theta; 0.11 psi 2021-02-13T01:11:01Z- handleDepth()-Depth_Keller msg: 1613178661385 epoch sec; seqNo:19268; depth 0.32 2021-02-13T01:11:01Z- handleDvl() - RDI_Pathfinder msg: 1613178661.40 epoch sec; seqNo:9271 2021-02-13T01:11:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 113.27 , 97.04 , 97.04 , 103.80 2021-02-13T01:11:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.05 , -0.05 , 0.24 2021-02-13T01:11:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178661.41 epoch sec; seqNo:19268; 4061021.90 north; 593809.35 east 2021-02-13T01:11:01Z- AHRS_M2 msg: 1613178661.78 epoch sec; seqNo:19115 2021-02-13T01:11:01Z- AHRS_M2 msg: 0.68 phi; -0.24 theta; 0.16 psi 2021-02-13T01:11:01Z- handleDepth()-Depth_Keller msg: 1613178661789 epoch sec; seqNo:19269; depth 0.33 2021-02-13T01:11:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178661.81 epoch sec; seqNo:19269; 4061021.90 north; 593809.35 east 2021-02-13T01:11:02Z- AHRS_M2 msg: 1613178662.19 epoch sec; seqNo:19116 2021-02-13T01:11:02Z- AHRS_M2 msg: 0.66 phi; -0.20 theta; 0.15 psi 2021-02-13T01:11:02Z- handleDepth()-Depth_Keller msg: 1613178662192 epoch sec; seqNo:19270; depth 0.35 2021-02-13T01:11:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178662.20 epoch sec; seqNo:19270; 4061021.90 north; 593809.35 east 2021-02-13T01:11:02Z- AHRS_M2 msg: 1613178662.59 epoch sec; seqNo:19117 2021-02-13T01:11:02Z- AHRS_M2 msg: 0.68 phi; -0.22 theta; 0.03 psi 2021-02-13T01:11:02Z- handleDepth()-Depth_Keller msg: 1613178662598 epoch sec; seqNo:19271; depth 0.35 2021-02-13T01:11:02Z- handleDvl() - RDI_Pathfinder msg: 1613178662.61 epoch sec; seqNo:9272 2021-02-13T01:11:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 112.73 , 93.39 , 97.85 , 117.19 2021-02-13T01:11:02Z- handleDvl() - RDI_Pathfinder msg: velocities -0.04 , 0.74 , 0.31 2021-02-13T01:11:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178662.62 epoch sec; seqNo:19271; 4061021.90 north; 593809.35 east 2021-02-13T01:11:02Z- AHRS_M2 msg: 1613178663.00 epoch sec; seqNo:19118 2021-02-13T01:11:02Z- AHRS_M2 msg: 0.67 phi; -0.27 theta; -0.15 psi 2021-02-13T01:11:03Z- handleDepth()-Depth_Keller msg: 1613178663000 epoch sec; seqNo:19272; depth 0.35 2021-02-13T01:11:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178663.02 epoch sec; seqNo:19272; 4061021.53 north; 593810.57 east 2021-02-13T01:11:03Z- AHRS_M2 msg: 1613178663.40 epoch sec; seqNo:19119 2021-02-13T01:11:03Z- AHRS_M2 msg: 0.64 phi; -0.30 theta; -0.25 psi 2021-02-13T01:11:03Z- handleDepth()-Depth_Keller msg: 1613178663404 epoch sec; seqNo:19273; depth 0.33 2021-02-13T01:11:03Z- handleDvl() - RDI_Pathfinder msg: 1613178663.42 epoch sec; seqNo:9273 2021-02-13T01:11:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 102.11 , 102.11 , 97.65 , 109.55 2021-02-13T01:11:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.04 , 0.43 , -0.73 2021-02-13T01:11:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178663.44 epoch sec; seqNo:19273; 4061021.53 north; 593810.57 east 2021-02-13T01:11:03Z- AHRS_M2 msg: 1613178663.80 epoch sec; seqNo:19120 2021-02-13T01:11:03Z- AHRS_M2 msg: 0.60 phi; -0.28 theta; -0.20 psi 2021-02-13T01:11:03Z- handleDepth()-Depth_Keller msg: 1613178663808 epoch sec; seqNo:19274; depth 0.30 2021-02-13T01:11:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178663.82 epoch sec; seqNo:19274; 4061021.53 north; 593810.57 east 2021-02-13T01:11:04Z- AHRS_M2 msg: 1613178664.21 epoch sec; seqNo:19121 2021-02-13T01:11:04Z- AHRS_M2 msg: 0.62 phi; -0.26 theta; -0.06 psi 2021-02-13T01:11:04Z- handleDepth()-Depth_Keller msg: 1613178664213 epoch sec; seqNo:19275; depth 0.30 2021-02-13T01:11:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178664.22 epoch sec; seqNo:19275; 4061021.53 north; 593810.57 east 2021-02-13T01:11:04Z- AHRS_M2 msg: 1613178664.61 epoch sec; seqNo:19122 2021-02-13T01:11:04Z- AHRS_M2 msg: 0.65 phi; -0.26 theta; 0.01 psi 2021-02-13T01:11:04Z- handleDepth()-Depth_Keller msg: 1613178664621 epoch sec; seqNo:19276; depth 0.32 2021-02-13T01:11:04Z- handleDvl() - RDI_Pathfinder msg: 1613178664.63 epoch sec; seqNo:9274 2021-02-13T01:11:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 115.70 , 97.85 , 120.16 2021-02-13T01:11:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.44 , -0.32 , 0.19 2021-02-13T01:11:04Z- LcmTrn::handleMessages() - handling 43 messages took 0.70 ms and 129 calls... 2021-02-13T01:11:04Z- waiting for 1613178662.40, time is 1613178664.63 2021-02-13T01:11:04Z- LcmTrn::updateTrn() equating pose 1613178664.63 and meas times 1613178664.63 2021-02-13T01:11:04Z- LcmTrn::updateTrn() >>>> pose time 1613178664.63, meas time 1613178664.63 <<<< 2021-02-13T01:11:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178664.63 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:11:04Z- TerrainNav::motionUpdate() - currEstimate = (4061022, 593810.57, 0.32) 2021-02-13T01:11:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:11:04Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:11:04Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178664.63 sec.; No relevant navigation data available 2021-02-13T01:11:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:11:04Z- TNavBF::mmse selected from filter -1 2021-02-13T01:11:04Z- variances([0], [2], and psi_berg) = 673.209616 832.985699 0.000000 2021-02-13T01:11:04Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:11:04Z- LcmTrn::publishEstimates() - MLE : 4061021.03 593826.07 0.32 2021-02-13T01:11:04Z- LcmTrn::publishEstimates() - MMSE : 4061021.53 593810.57 0.32 2021-02-13T01:11:04Z- LcmTrn::publishEstimates() - COVAR: 673.21 832.99 0.94 0.00 2021-02-13T01:11:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178664.65 epoch sec; seqNo:19276; 4061021.91 north; 593810.57 east 2021-02-13T01:11:05Z- AHRS_M2 msg: 1613178665.02 epoch sec; seqNo:19123 2021-02-13T01:11:05Z- AHRS_M2 msg: 0.65 phi; -0.30 theta; 0.03 psi 2021-02-13T01:11:05Z- handleDepth()-Depth_Keller msg: 1613178665020 epoch sec; seqNo:19277; depth 0.32 2021-02-13T01:11:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178665.03 epoch sec; seqNo:19277; 4061021.91 north; 593810.57 east 2021-02-13T01:11:05Z- AHRS_M2 msg: 1613178665.42 epoch sec; seqNo:19124 2021-02-13T01:11:05Z- AHRS_M2 msg: 0.61 phi; -0.33 theta; 0.05 psi 2021-02-13T01:11:05Z- handleDepth()-Depth_Keller msg: 1613178665425 epoch sec; seqNo:19278; depth 0.33 2021-02-13T01:11:05Z- handleDvl() - RDI_Pathfinder msg: 1613178665.44 epoch sec; seqNo:9275 2021-02-13T01:11:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.91 , 98.93 , 87.03 , 115.30 2021-02-13T01:11:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.28 , -0.37 , 0.12 2021-02-13T01:11:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178665.45 epoch sec; seqNo:19278; 4061022.29 north; 593810.56 east 2021-02-13T01:11:05Z- AHRS_M2 msg: 1613178665.82 epoch sec; seqNo:19125 2021-02-13T01:11:05Z- AHRS_M2 msg: 0.58 phi; -0.30 theta; 0.05 psi 2021-02-13T01:11:05Z- handleDepth()-Depth_Keller msg: 1613178665829 epoch sec; seqNo:19279; depth 0.32 2021-02-13T01:11:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178665.85 epoch sec; seqNo:19279; 4061022.29 north; 593810.56 east 2021-02-13T01:11:06Z- AHRS_M2 msg: 1613178666.23 epoch sec; seqNo:19126 2021-02-13T01:11:06Z- AHRS_M2 msg: 0.62 phi; -0.26 theta; 0.00 psi 2021-02-13T01:11:06Z- handleDepth()-Depth_Keller msg: 1613178666239 epoch sec; seqNo:19280; depth 0.34 2021-02-13T01:11:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178666.25 epoch sec; seqNo:19280; 4061022.67 north; 593810.56 east 2021-02-13T01:11:06Z- AHRS_M2 msg: 1613178666.63 epoch sec; seqNo:19127 2021-02-13T01:11:06Z- AHRS_M2 msg: 0.68 phi; -0.29 theta; -0.05 psi 2021-02-13T01:11:06Z- handleDepth()-Depth_Keller msg: 1613178666636 epoch sec; seqNo:19281; depth 0.35 2021-02-13T01:11:06Z- handleDvl() - RDI_Pathfinder msg: 1613178666.66 epoch sec; seqNo:9276 2021-02-13T01:11:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 106.44 , 96.97 , 87.50 , 122.67 2021-02-13T01:11:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.28 , 0.10 , 0.45 2021-02-13T01:11:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178666.68 epoch sec; seqNo:19281; 4061022.67 north; 593810.56 east 2021-02-13T01:11:07Z- AHRS_M2 msg: 1613178667.04 epoch sec; seqNo:19128 2021-02-13T01:11:07Z- AHRS_M2 msg: 0.67 phi; -0.37 theta; -0.05 psi 2021-02-13T01:11:07Z- handleDepth()-Depth_Keller msg: 1613178667041 epoch sec; seqNo:19282; depth 0.35 2021-02-13T01:11:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178667.07 epoch sec; seqNo:19282; 4061022.67 north; 593810.56 east 2021-02-13T01:11:07Z- AHRS_M2 msg: 1613178667.44 epoch sec; seqNo:19129 2021-02-13T01:11:07Z- AHRS_M2 msg: 0.61 phi; -0.42 theta; -0.00 psi 2021-02-13T01:11:07Z- handleDepth()-Depth_Keller msg: 1613178667444 epoch sec; seqNo:19283; depth 0.36 2021-02-13T01:11:07Z- handleDvl() - RDI_Pathfinder msg: 1613178667.46 epoch sec; seqNo:9277 2021-02-13T01:11:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 100.83 , 103.80 , 0.00 , 124.63 2021-02-13T01:11:07Z- handleDvl() - RDI_Pathfinder msg: velocities -0.36 , 0.34 , 0.09 2021-02-13T01:11:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178667.47 epoch sec; seqNo:19283; 4061022.29 north; 593810.56 east 2021-02-13T01:11:07Z- AHRS_M2 msg: 1613178667.84 epoch sec; seqNo:19130 2021-02-13T01:11:07Z- AHRS_M2 msg: 0.56 phi; -0.46 theta; 0.03 psi 2021-02-13T01:11:07Z- handleDepth()-Depth_Keller msg: 1613178667848 epoch sec; seqNo:19284; depth 0.37 2021-02-13T01:11:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178667.86 epoch sec; seqNo:19284; 4061022.29 north; 593810.56 east 2021-02-13T01:11:08Z- AHRS_M2 msg: 1613178668.25 epoch sec; seqNo:19131 2021-02-13T01:11:08Z- AHRS_M2 msg: 0.53 phi; -0.50 theta; -0.04 psi 2021-02-13T01:11:08Z- handleDepth()-Depth_Keller msg: 1613178668257 epoch sec; seqNo:19285; depth 0.37 2021-02-13T01:11:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178668.27 epoch sec; seqNo:19285; 4061021.91 north; 593810.57 east 2021-02-13T01:11:08Z- AHRS_M2 msg: 1613178668.65 epoch sec; seqNo:19132 2021-02-13T01:11:08Z- AHRS_M2 msg: 0.51 phi; -0.44 theta; -0.12 psi 2021-02-13T01:11:08Z- handleDepth()-Depth_Keller msg: 1613178668658 epoch sec; seqNo:19286; depth 0.34 2021-02-13T01:11:08Z- handleDvl() - RDI_Pathfinder msg: 1613178668.67 epoch sec; seqNo:9278 2021-02-13T01:11:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 110.63 , 104.68 , 85.34 , 0.00 2021-02-13T01:11:08Z- handleDvl() - RDI_Pathfinder msg: velocities -0.09 , 0.43 , -0.10 2021-02-13T01:11:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178668.68 epoch sec; seqNo:19286; 4061021.91 north; 593810.57 east 2021-02-13T01:11:09Z- AHRS_M2 msg: 1613178669.06 epoch sec; seqNo:19133 2021-02-13T01:11:09Z- AHRS_M2 msg: 0.51 phi; -0.36 theta; -0.13 psi 2021-02-13T01:11:09Z- handleDepth()-Depth_Keller msg: 1613178669060 epoch sec; seqNo:19287; depth 0.31 2021-02-13T01:11:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178669.07 epoch sec; seqNo:19287; 4061021.91 north; 593810.57 east 2021-02-13T01:11:09Z- AHRS_M2 msg: 1613178669.46 epoch sec; seqNo:19134 2021-02-13T01:11:09Z- AHRS_M2 msg: 0.54 phi; -0.40 theta; -0.11 psi 2021-02-13T01:11:09Z- handleDepth()-Depth_Keller msg: 1613178669464 epoch sec; seqNo:19288; depth 0.29 2021-02-13T01:11:09Z- handleDvl() - RDI_Pathfinder msg: 1613178669.48 epoch sec; seqNo:9279 2021-02-13T01:11:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.39 , 111.92 , 86.22 , 0.00 2021-02-13T01:11:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , 0.34 , 0.31 2021-02-13T01:11:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178669.49 epoch sec; seqNo:19288; 4061021.91 north; 593810.57 east 2021-02-13T01:11:09Z- LcmTrn::handleMessages() - handling 42 messages took 0.64 ms and 129 calls... 2021-02-13T01:11:09Z- waiting for 1613178667.63, time is 1613178669.49 2021-02-13T01:11:09Z- LcmTrn::updateTrn() equating pose 1613178669.49 and meas times 1613178669.49 2021-02-13T01:11:09Z- LcmTrn::updateTrn() >>>> pose time 1613178669.49, meas time 1613178669.49 <<<< 2021-02-13T01:11:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178669.49 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:11:09Z- TerrainNav::motionUpdate() - currEstimate = (4061022, 593810.57, 0.29) 2021-02-13T01:11:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:11:09Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:11:09Z- TNavBF::Projecting Measurements 2021-02-13T01:11:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:11:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:11:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:11:09Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:11:09Z- TNavFilter::3 good beams. 2021-02-13T01:11:09Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:11:09Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:11:09Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:11:09Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:11:09Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:11:09Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:11:09Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:11:09Z- TNavBF::mmse selected from filter -1 2021-02-13T01:11:09Z- TerrainMapOctree: (vehN, vehE ) = (4061021.91, 593810.57). 2021-02-13T01:11:09Z- TerrainMapOctree: Min Distance = 495.10. 2021-02-13T01:11:09Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:11:09Z- There is no specific good beam that all particles have in common. 2021-02-13T01:11:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:11:09Z- Old Depth Bias: 0.00 DepthBiasCov: 0.96 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 15.49 2021-02-13T01:11:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:11:09Z- Old Depth Bias: 0.00 DepthBiasCov: 0.91 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 20.04 2021-02-13T01:11:09Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:11:09Z- Old Depth Bias: 0.00 DepthBiasCov: 0.87 K: 0.060 ExpectedMeasDiff: nan currMeas.covariance: 11.89 2021-02-13T01:11:09Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:11:09Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:11:09Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:11:09Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:09Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:11:09Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:11:09Z- Filter 0 No beams used in subcloud update 2021-02-13T01:11:09Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:11:09Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:11:09Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 673.2 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T01:11:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178669.49 sec. 2021-02-13T01:11:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:11:09Z- TNavBF::mmse selected from filter -1 2021-02-13T01:11:09Z- variances([0], [2], and psi_berg) = 673.197018 832.985694 0.000000 2021-02-13T01:11:09Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:11:09Z- LcmTrn::publishEstimates() - MLE : 4061021.41 593826.07 0.29 2021-02-13T01:11:09Z- LcmTrn::publishEstimates() - MMSE : 4061021.91 593810.57 0.29 2021-02-13T01:11:09Z- LcmTrn::publishEstimates() - COVAR: 673.20 832.99 0.82 0.00 2021-02-13T01:11:09Z- AHRS_M2 msg: 1613178669.87 epoch sec; seqNo:19135 2021-02-13T01:11:09Z- AHRS_M2 msg: 0.54 phi; -0.50 theta; -0.04 psi 2021-02-13T01:11:09Z- handleDepth()-Depth_Keller msg: 1613178669869 epoch sec; seqNo:19289; depth 0.31 2021-02-13T01:11:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178669.88 epoch sec; seqNo:19289; 4061021.92 north; 593811.79 east 2021-02-13T01:11:10Z- AHRS_M2 msg: 1613178670.27 epoch sec; seqNo:19136 2021-02-13T01:11:10Z- AHRS_M2 msg: 0.50 phi; -0.51 theta; 0.06 psi 2021-02-13T01:11:10Z- handleDepth()-Depth_Keller msg: 1613178670275 epoch sec; seqNo:19290; depth 0.32 2021-02-13T01:11:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178670.32 epoch sec; seqNo:19290; 4061022.30 north; 593811.78 east 2021-02-13T01:11:10Z- AHRS_M2 msg: 1613178670.67 epoch sec; seqNo:19137 2021-02-13T01:11:10Z- AHRS_M2 msg: 0.48 phi; -0.43 theta; 0.04 psi 2021-02-13T01:11:10Z- handleDepth()-Depth_Keller msg: 1613178670676 epoch sec; seqNo:19291; depth 0.32 2021-02-13T01:11:10Z- handleDvl() - RDI_Pathfinder msg: 1613178670.69 epoch sec; seqNo:9280 2021-02-13T01:11:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.87 , 110.63 , 0.00 , 166.08 2021-02-13T01:11:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.24 , 0.24 , -0.21 2021-02-13T01:11:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178670.71 epoch sec; seqNo:19291; 4061022.30 north; 593811.78 east 2021-02-13T01:11:11Z- AHRS_M2 msg: 1613178671.09 epoch sec; seqNo:19138 2021-02-13T01:11:11Z- AHRS_M2 msg: 0.51 phi; -0.38 theta; -0.09 psi 2021-02-13T01:11:11Z- handleDepth()-Depth_Keller msg: 1613178671091 epoch sec; seqNo:19292; depth 0.27 2021-02-13T01:11:11Z- handleDvl() - RDI_Pathfinder msg: 1613178671.13 epoch sec; seqNo:9281 2021-02-13T01:11:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:11:11Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:11:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178671.14 epoch sec; seqNo:19292; 4061022.30 north; 593811.78 east 2021-02-13T01:11:11Z- AHRS_M2 msg: 1613178671.48 epoch sec; seqNo:19139 2021-02-13T01:11:11Z- AHRS_M2 msg: 0.56 phi; -0.41 theta; -0.13 psi 2021-02-13T01:11:11Z- handleDepth()-Depth_Keller msg: 1613178671486 epoch sec; seqNo:19293; depth 0.26 2021-02-13T01:11:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178671.51 epoch sec; seqNo:19293; 4061022.30 north; 593811.78 east 2021-02-13T01:11:11Z- AHRS_M2 msg: 1613178671.89 epoch sec; seqNo:19140 2021-02-13T01:11:11Z- AHRS_M2 msg: 0.54 phi; -0.48 theta; -0.06 psi 2021-02-13T01:11:11Z- handleDepth()-Depth_Keller msg: 1613178671892 epoch sec; seqNo:19294; depth 0.30 2021-02-13T01:11:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178671.91 epoch sec; seqNo:19294; 4061022.30 north; 593811.78 east 2021-02-13T01:11:12Z- AHRS_M2 msg: 1613178672.29 epoch sec; seqNo:19141 2021-02-13T01:11:12Z- AHRS_M2 msg: 0.48 phi; -0.50 theta; 0.01 psi 2021-02-13T01:11:12Z- handleDepth()-Depth_Keller msg: 1613178672292 epoch sec; seqNo:19295; depth 0.32 2021-02-13T01:11:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178672.30 epoch sec; seqNo:19295; 4061022.68 north; 593811.78 east 2021-02-13T01:11:12Z- AHRS_M2 msg: 1613178672.69 epoch sec; seqNo:19142 2021-02-13T01:11:12Z- AHRS_M2 msg: 0.42 phi; -0.47 theta; 0.02 psi 2021-02-13T01:11:12Z- handleDepth()-Depth_Keller msg: 1613178672696 epoch sec; seqNo:19296; depth 0.31 2021-02-13T01:11:12Z- handleDvl() - RDI_Pathfinder msg: 1613178672.71 epoch sec; seqNo:9282 2021-02-13T01:11:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:11:12Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:11:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178672.72 epoch sec; seqNo:19296; 4061022.68 north; 593811.78 east 2021-02-13T01:11:13Z- AHRS_M2 msg: 1613178673.10 epoch sec; seqNo:19143 2021-02-13T01:11:13Z- AHRS_M2 msg: 0.39 phi; -0.44 theta; -0.04 psi 2021-02-13T01:11:13Z- handleDepth()-Depth_Keller msg: 1613178673101 epoch sec; seqNo:19297; depth 0.29 2021-02-13T01:11:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178673.11 epoch sec; seqNo:19297; 4061022.69 north; 593813.00 east 2021-02-13T01:11:13Z- AHRS_M2 msg: 1613178673.50 epoch sec; seqNo:19144 2021-02-13T01:11:13Z- AHRS_M2 msg: 0.37 phi; -0.38 theta; -0.08 psi 2021-02-13T01:11:13Z- handleDepth()-Depth_Keller msg: 1613178673514 epoch sec; seqNo:19298; depth 0.27 2021-02-13T01:11:13Z- handleDvl() - RDI_Pathfinder msg: 1613178673.53 epoch sec; seqNo:9283 2021-02-13T01:11:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:11:13Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:11:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178673.54 epoch sec; seqNo:19298; 4061022.69 north; 593813.00 east 2021-02-13T01:11:13Z- AHRS_M2 msg: 1613178673.90 epoch sec; seqNo:19145 2021-02-13T01:11:13Z- AHRS_M2 msg: 0.40 phi; -0.33 theta; -0.08 psi 2021-02-13T01:11:13Z- handleDepth()-Depth_Keller msg: 1613178673908 epoch sec; seqNo:19299; depth 0.23 2021-02-13T01:11:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178673.92 epoch sec; seqNo:19299; 4061022.69 north; 593813.00 east 2021-02-13T01:11:14Z- AHRS_M2 msg: 1613178674.31 epoch sec; seqNo:19146 2021-02-13T01:11:14Z- AHRS_M2 msg: 0.42 phi; -0.37 theta; -0.03 psi 2021-02-13T01:11:14Z- handleDepth()-Depth_Keller msg: 1613178674312 epoch sec; seqNo:19300; depth 0.25 2021-02-13T01:11:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178674.32 epoch sec; seqNo:19300; 4061023.07 north; 593813.00 east 2021-02-13T01:11:14Z- AHRS_M2 msg: 1613178674.71 epoch sec; seqNo:19147 2021-02-13T01:11:14Z- AHRS_M2 msg: 0.40 phi; -0.42 theta; 0.09 psi 2021-02-13T01:11:14Z- handleDepth()-Depth_Keller msg: 1613178674718 epoch sec; seqNo:19301; depth 0.28 2021-02-13T01:11:14Z- LcmTrn::handleMessages() - handling 42 messages took 0.79 ms and 126 calls... 2021-02-13T01:11:14Z- waiting for 1613178672.49, time is 1613178674.71 2021-02-13T01:11:14Z- LcmTrn::updateTrn() >>>> pose time 1613178674.71, meas time 1613178673.53 <<<< 2021-02-13T01:11:14Z- TerrainNav:: Throwing out all beams because more than two are equal to 0.000 at t=1613178673.53, ping # 0. 2021-02-13T01:11:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178673.53 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:11:14Z- TerrainNav::motionUpdate() - currEstimate = (4061023, 593813.00, 0.28) 2021-02-13T01:11:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:11:14Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:11:14Z- TerrainNav::motionUpdate() - Invalid velocity 2021-02-13T01:11:14Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2021-02-13T01:11:14Z- TNavBF::Projecting Measurements 2021-02-13T01:11:14Z- TNavFilter::Measurement from time = 1613178673.53 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T01:11:14Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:11:14Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:11:14Z- TNavBF:: beamIndex[1] = 0 2021-02-13T01:11:14Z- TNavBF:: beamIndex[2] = 0 2021-02-13T01:11:14Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:11:14Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:11:14Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:11:14Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 673.2 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:11:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:11:14Z- TNavBF::mmse selected from filter -1 2021-02-13T01:11:14Z- variances([0], [2], and psi_berg) = 673.151507 832.985676 0.000000 2021-02-13T01:11:14Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:11:14Z- LcmTrn::publishEstimates() - MLE : 4061022.57 593827.50 0.28 2021-02-13T01:11:14Z- LcmTrn::publishEstimates() - MMSE : 4061023.07 593813.00 0.28 2021-02-13T01:11:14Z- LcmTrn::publishEstimates() - COVAR: 673.15 832.99 0.96 0.00 2021-02-13T01:11:14Z- handleDvl() - RDI_Pathfinder msg: 1613178674.73 epoch sec; seqNo:9284 2021-02-13T01:11:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 100.22 , 104.68 , 86.83 , 126.99 2021-02-13T01:11:14Z- handleDvl() - RDI_Pathfinder msg: velocities -0.07 , 0.24 , 0.34 2021-02-13T01:11:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178674.74 epoch sec; seqNo:19301; 4061022.69 north; 593813.00 east 2021-02-13T01:11:15Z- AHRS_M2 msg: 1613178675.12 epoch sec; seqNo:19148 2021-02-13T01:11:15Z- AHRS_M2 msg: 0.38 phi; -0.46 theta; 0.14 psi 2021-02-13T01:11:15Z- handleDepth()-Depth_Keller msg: 1613178675125 epoch sec; seqNo:19302; depth 0.28 2021-02-13T01:11:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178675.15 epoch sec; seqNo:19302; 4061022.69 north; 593813.00 east 2021-02-13T01:11:15Z- AHRS_M2 msg: 1613178675.52 epoch sec; seqNo:19149 2021-02-13T01:11:15Z- AHRS_M2 msg: 0.33 phi; -0.51 theta; 0.07 psi 2021-02-13T01:11:15Z- handleDepth()-Depth_Keller msg: 1613178675524 epoch sec; seqNo:19303; depth 0.30 2021-02-13T01:11:15Z- handleDvl() - RDI_Pathfinder msg: 1613178675.56 epoch sec; seqNo:9285 2021-02-13T01:11:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 123.00 , 100.69 , 85.82 , 0.00 2021-02-13T01:11:15Z- handleDvl() - RDI_Pathfinder msg: velocities -0.37 , 0.65 , -0.51 2021-02-13T01:11:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178675.57 epoch sec; seqNo:19303; 4061022.69 north; 593813.00 east 2021-02-13T01:11:15Z- AHRS_M2 msg: 1613178675.92 epoch sec; seqNo:19150 2021-02-13T01:11:15Z- AHRS_M2 msg: 0.25 phi; -0.45 theta; -0.05 psi 2021-02-13T01:11:15Z- handleDepth()-Depth_Keller msg: 1613178675928 epoch sec; seqNo:19304; depth 0.26 2021-02-13T01:11:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178675.97 epoch sec; seqNo:19304; 4061022.31 north; 593813.00 east 2021-02-13T01:11:16Z- AHRS_M2 msg: 1613178676.34 epoch sec; seqNo:19151 2021-02-13T01:11:16Z- AHRS_M2 msg: 0.26 phi; -0.30 theta; -0.22 psi 2021-02-13T01:11:16Z- handleDepth()-Depth_Keller msg: 1613178676339 epoch sec; seqNo:19305; depth 0.18 2021-02-13T01:11:16Z- handleDvl() - RDI_Pathfinder msg: 1613178676.37 epoch sec; seqNo:9286 2021-02-13T01:11:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:11:16Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:11:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178676.40 epoch sec; seqNo:19305; 4061022.33 north; 593814.22 east 2021-02-13T01:11:16Z- AHRS_M2 msg: 1613178676.73 epoch sec; seqNo:19152 2021-02-13T01:11:16Z- AHRS_M2 msg: 0.28 phi; -0.31 theta; -0.06 psi 2021-02-13T01:11:16Z- handleDepth()-Depth_Keller msg: 1613178676739 epoch sec; seqNo:19306; depth 0.16 2021-02-13T01:11:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178676.77 epoch sec; seqNo:19306; 4061022.33 north; 593814.22 east 2021-02-13T01:11:17Z- AHRS_M2 msg: 1613178677.14 epoch sec; seqNo:19153 2021-02-13T01:11:17Z- AHRS_M2 msg: 0.25 phi; -0.32 theta; 0.12 psi 2021-02-13T01:11:17Z- handleDepth()-Depth_Keller msg: 1613178677146 epoch sec; seqNo:19307; depth 0.19 2021-02-13T01:11:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178677.16 epoch sec; seqNo:19307; 4061021.95 north; 593814.23 east 2021-02-13T01:11:17Z- AHRS_M2 msg: 1613178677.54 epoch sec; seqNo:19154 2021-02-13T01:11:17Z- AHRS_M2 msg: 0.21 phi; -0.33 theta; 0.23 psi 2021-02-13T01:11:17Z- handleDepth()-Depth_Keller msg: 1613178677544 epoch sec; seqNo:19308; depth 0.21 2021-02-13T01:11:17Z- handleDvl() - RDI_Pathfinder msg: 1613178677.56 epoch sec; seqNo:9287 2021-02-13T01:11:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:11:17Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:11:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178677.57 epoch sec; seqNo:19308; 4061021.95 north; 593814.23 east 2021-02-13T01:11:17Z- AHRS_M2 msg: 1613178677.94 epoch sec; seqNo:19155 2021-02-13T01:11:17Z- AHRS_M2 msg: 0.16 phi; -0.32 theta; 0.21 psi 2021-02-13T01:11:17Z- handleDepth()-Depth_Keller msg: 1613178677950 epoch sec; seqNo:19309; depth 0.19 2021-02-13T01:11:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178677.96 epoch sec; seqNo:19309; 4061021.95 north; 593814.23 east 2021-02-13T01:11:18Z- AHRS_M2 msg: 1613178678.35 epoch sec; seqNo:19156 2021-02-13T01:11:18Z- AHRS_M2 msg: 0.14 phi; -0.27 theta; 0.09 psi 2021-02-13T01:11:18Z- handleDepth()-Depth_Keller msg: 1613178678352 epoch sec; seqNo:19310; depth 0.20 2021-02-13T01:11:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178678.37 epoch sec; seqNo:19310; 4061021.58 north; 593815.45 east 2021-02-13T01:11:18Z- AHRS_M2 msg: 1613178678.75 epoch sec; seqNo:19157 2021-02-13T01:11:18Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; -0.09 psi 2021-02-13T01:11:18Z- handleDepth()-Depth_Keller msg: 1613178678756 epoch sec; seqNo:19311; depth 0.19 2021-02-13T01:11:18Z- handleDvl() - RDI_Pathfinder msg: 1613178678.77 epoch sec; seqNo:9288 2021-02-13T01:11:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 121.65 , 93.39 , 90.41 , 97.85 2021-02-13T01:11:18Z- handleDvl() - RDI_Pathfinder msg: velocities -0.00 , 0.56 , 0.49 2021-02-13T01:11:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178678.79 epoch sec; seqNo:19311; 4061021.58 north; 593815.45 east 2021-02-13T01:11:19Z- AHRS_M2 msg: 1613178679.16 epoch sec; seqNo:19158 2021-02-13T01:11:19Z- AHRS_M2 msg: 0.15 phi; -0.25 theta; -0.28 psi 2021-02-13T01:11:19Z- handleDepth()-Depth_Keller msg: 1613178679169 epoch sec; seqNo:19312; depth 0.18 2021-02-13T01:11:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178679.18 epoch sec; seqNo:19312; 4061021.58 north; 593815.45 east 2021-02-13T01:11:19Z- AHRS_M2 msg: 1613178679.56 epoch sec; seqNo:19159 2021-02-13T01:11:19Z- AHRS_M2 msg: 0.16 phi; -0.22 theta; -0.27 psi 2021-02-13T01:11:19Z- handleDepth()-Depth_Keller msg: 1613178679565 epoch sec; seqNo:19313; depth 0.11 2021-02-13T01:11:19Z- handleDvl() - RDI_Pathfinder msg: 1613178679.58 epoch sec; seqNo:9289 2021-02-13T01:11:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 103.73 , 96.97 , 0.00 2021-02-13T01:11:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.04 , 0.17 , -0.53 2021-02-13T01:11:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178679.59 epoch sec; seqNo:19313; 4061021.58 north; 593815.45 east 2021-02-13T01:11:19Z- LcmTrn::handleMessages() - handling 43 messages took 0.75 ms and 128 calls... 2021-02-13T01:11:19Z- waiting for 1613178677.71, time is 1613178679.59 2021-02-13T01:11:19Z- LcmTrn::updateTrn() equating pose 1613178679.59 and meas times 1613178679.59 2021-02-13T01:11:19Z- LcmTrn::updateTrn() >>>> pose time 1613178679.59, meas time 1613178679.59 <<<< 2021-02-13T01:11:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178679.59 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:11:19Z- TerrainNav::motionUpdate() - currEstimate = (4061022, 593815.45, 0.11) 2021-02-13T01:11:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:11:19Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:11:19Z- TNavBF::Projecting Measurements 2021-02-13T01:11:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:11:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:11:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:11:19Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:11:19Z- TNavFilter::3 good beams. 2021-02-13T01:11:19Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:11:19Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:11:19Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:11:19Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:11:19Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:11:19Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:11:19Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:11:19Z- TNavBF::mmse selected from filter -1 2021-02-13T01:11:19Z- TerrainMapOctree: (vehN, vehE ) = (4061021.58, 593815.45). 2021-02-13T01:11:19Z- TerrainMapOctree: Min Distance = 490.92. 2021-02-13T01:11:19Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:11:19Z- There is no specific good beam that all particles have in common. 2021-02-13T01:11:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:11:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.11 K: 0.062 ExpectedMeasDiff: nan currMeas.covariance: 15.05 2021-02-13T01:11:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:11:19Z- Old Depth Bias: 0.00 DepthBiasCov: 1.04 K: 0.052 ExpectedMeasDiff: nan currMeas.covariance: 17.22 2021-02-13T01:11:19Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:11:19Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.055 ExpectedMeasDiff: nan currMeas.covariance: 15.05 2021-02-13T01:11:19Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:11:19Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:11:19Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:11:19Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:19Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:11:19Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:11:19Z- Filter 0 No beams used in subcloud update 2021-02-13T01:11:19Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:11:19Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:11:19Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 673.1 EastVariance: 833.0 DepthVariance: 0.9 this->DepthBiasCov: 0.9 2021-02-13T01:11:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178679.59 sec. 2021-02-13T01:11:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:11:19Z- TNavBF::mmse selected from filter -1 2021-02-13T01:11:19Z- variances([0], [2], and psi_berg) = 673.116837 832.985662 0.000000 2021-02-13T01:11:19Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:11:19Z- LcmTrn::publishEstimates() - MLE : 4061021.08 593829.95 0.11 2021-02-13T01:11:19Z- LcmTrn::publishEstimates() - MMSE : 4061021.58 593815.45 0.11 2021-02-13T01:11:19Z- LcmTrn::publishEstimates() - COVAR: 673.12 832.99 0.93 0.00 2021-02-13T01:11:19Z- AHRS_M2 msg: 1613178679.96 epoch sec; seqNo:19160 2021-02-13T01:11:19Z- AHRS_M2 msg: 0.18 phi; -0.21 theta; -0.10 psi 2021-02-13T01:11:19Z- handleDepth()-Depth_Keller msg: 1613178679978 epoch sec; seqNo:19314; depth 0.08 2021-02-13T01:11:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178679.99 epoch sec; seqNo:19314; 4061021.58 north; 593815.45 east 2021-02-13T01:11:20Z- AHRS_M2 msg: 1613178680.37 epoch sec; seqNo:19161 2021-02-13T01:11:20Z- AHRS_M2 msg: 0.14 phi; -0.31 theta; 0.11 psi 2021-02-13T01:11:20Z- handleDepth()-Depth_Keller msg: 1613178680372 epoch sec; seqNo:19315; depth 0.09 2021-02-13T01:11:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178680.41 epoch sec; seqNo:19315; 4061021.58 north; 593815.45 east 2021-02-13T01:11:20Z- AHRS_M2 msg: 1613178680.77 epoch sec; seqNo:19162 2021-02-13T01:11:20Z- AHRS_M2 msg: 0.05 phi; -0.27 theta; 0.22 psi 2021-02-13T01:11:20Z- handleDepth()-Depth_Keller msg: 1613178680776 epoch sec; seqNo:19316; depth 0.11 2021-02-13T01:11:20Z- handleDvl() - RDI_Pathfinder msg: 1613178680.80 epoch sec; seqNo:9290 2021-02-13T01:11:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 103.87 , 97.11 , 111.98 2021-02-13T01:11:20Z- handleDvl() - RDI_Pathfinder msg: velocities -0.09 , 0.14 , 0.31 2021-02-13T01:11:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178680.81 epoch sec; seqNo:19316; 4061021.58 north; 593815.45 east 2021-02-13T01:11:21Z- AHRS_M2 msg: 1613178681.18 epoch sec; seqNo:19163 2021-02-13T01:11:21Z- AHRS_M2 msg: 0.07 phi; -0.12 theta; 0.14 psi 2021-02-13T01:11:21Z- handleDepth()-Depth_Keller msg: 1613178681182 epoch sec; seqNo:19317; depth 0.12 2021-02-13T01:11:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178681.21 epoch sec; seqNo:19317; 4061021.58 north; 593815.45 east 2021-02-13T01:11:21Z- AHRS_M2 msg: 1613178681.58 epoch sec; seqNo:19164 2021-02-13T01:11:21Z- AHRS_M2 msg: 0.10 phi; -0.09 theta; -0.02 psi 2021-02-13T01:11:21Z- handleDepth()-Depth_Keller msg: 1613178681585 epoch sec; seqNo:19318; depth 0.07 2021-02-13T01:11:21Z- handleDvl() - RDI_Pathfinder msg: 1613178681.61 epoch sec; seqNo:9291 2021-02-13T01:11:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 118.74 , 97.92 , 97.92 , 117.26 2021-02-13T01:11:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.35 , 0.06 , 0.15 2021-02-13T01:11:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178681.62 epoch sec; seqNo:19318; 4061021.58 north; 593815.45 east 2021-02-13T01:11:21Z- AHRS_M2 msg: 1613178681.98 epoch sec; seqNo:19165 2021-02-13T01:11:21Z- AHRS_M2 msg: 0.09 phi; -0.07 theta; -0.07 psi 2021-02-13T01:11:21Z- handleDepth()-Depth_Keller msg: 1613178681988 epoch sec; seqNo:19319; depth 0.04 2021-02-13T01:11:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178682.00 epoch sec; seqNo:19319; 4061021.96 north; 593815.45 east 2021-02-13T01:11:22Z- AHRS_M2 msg: 1613178682.39 epoch sec; seqNo:19166 2021-02-13T01:11:22Z- AHRS_M2 msg: 0.11 phi; -0.06 theta; -0.02 psi 2021-02-13T01:11:22Z- handleDepth()-Depth_Keller msg: 1613178682392 epoch sec; seqNo:19320; depth 0.03 2021-02-13T01:11:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178682.40 epoch sec; seqNo:19320; 4061021.96 north; 593815.45 east 2021-02-13T01:11:22Z- AHRS_M2 msg: 1613178682.79 epoch sec; seqNo:19167 2021-02-13T01:11:22Z- AHRS_M2 msg: 0.13 phi; -0.12 theta; 0.02 psi 2021-02-13T01:11:22Z- handleDepth()-Depth_Keller msg: 1613178682797 epoch sec; seqNo:19321; depth 0.04 2021-02-13T01:11:22Z- handleDvl() - RDI_Pathfinder msg: 1613178682.81 epoch sec; seqNo:9292 2021-02-13T01:11:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.96 , 95.96 , 95.96 , 101.91 2021-02-13T01:11:22Z- handleDvl() - RDI_Pathfinder msg: velocities -0.09 , 0.36 , 0.70 2021-02-13T01:11:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178682.82 epoch sec; seqNo:19321; 4061021.96 north; 593815.45 east 2021-02-13T01:11:23Z- AHRS_M2 msg: 1613178683.20 epoch sec; seqNo:19168 2021-02-13T01:11:23Z- AHRS_M2 msg: 0.09 phi; -0.14 theta; 0.05 psi 2021-02-13T01:11:23Z- handleDepth()-Depth_Keller msg: 1613178683201 epoch sec; seqNo:19322; depth 0.02 2021-02-13T01:11:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178683.22 epoch sec; seqNo:19322; 4061021.96 north; 593815.45 east 2021-02-13T01:11:23Z- AHRS_M2 msg: 1613178683.60 epoch sec; seqNo:19169 2021-02-13T01:11:23Z- AHRS_M2 msg: 0.02 phi; -0.14 theta; 0.03 psi 2021-02-13T01:11:23Z- handleDepth()-Depth_Keller msg: 1613178683604 epoch sec; seqNo:19323; depth 0.01 2021-02-13T01:11:23Z- handleDvl() - RDI_Pathfinder msg: 1613178683.62 epoch sec; seqNo:9293 2021-02-13T01:11:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.30 , 95.89 , 97.24 , 106.71 2021-02-13T01:11:23Z- handleDvl() - RDI_Pathfinder msg: velocities -0.30 , 0.55 , -0.11 2021-02-13T01:11:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178683.64 epoch sec; seqNo:19323; 4061021.60 north; 593816.67 east 2021-02-13T01:11:24Z- AHRS_M2 msg: 1613178684.01 epoch sec; seqNo:19170 2021-02-13T01:11:24Z- AHRS_M2 msg: 0.02 phi; -0.08 theta; -0.02 psi 2021-02-13T01:11:24Z- handleDepth()-Depth_Keller msg: 1613178684017 epoch sec; seqNo:19324; depth 0.02 2021-02-13T01:11:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178684.03 epoch sec; seqNo:19324; 4061021.60 north; 593816.67 east 2021-02-13T01:11:24Z- AHRS_M2 msg: 1613178684.41 epoch sec; seqNo:19171 2021-02-13T01:11:24Z- AHRS_M2 msg: 0.05 phi; 0.02 theta; -0.03 psi 2021-02-13T01:11:24Z- handleDepth()-Depth_Keller msg: 1613178684416 epoch sec; seqNo:19325; depth 0.02 2021-02-13T01:11:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178684.42 epoch sec; seqNo:19325; 4061021.60 north; 593816.67 east 2021-02-13T01:11:24Z- AHRS_M2 msg: 1613178684.81 epoch sec; seqNo:19172 2021-02-13T01:11:24Z- AHRS_M2 msg: 0.07 phi; 0.05 theta; -0.04 psi 2021-02-13T01:11:24Z- LcmTrn::handleMessages() - handling 41 messages took 0.69 ms and 127 calls... 2021-02-13T01:11:24Z- waiting for 1613178682.59, time is 1613178684.81 2021-02-13T01:11:24Z- LcmTrn::updateTrn() >>>> pose time 1613178684.81, meas time 1613178683.62 <<<< 2021-02-13T01:11:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178683.62 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:11:24Z- TerrainNav::motionUpdate() - currEstimate = (4061022, 593816.67, 0.02) 2021-02-13T01:11:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:11:24Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:11:24Z- TNavBF::Projecting Measurements 2021-02-13T01:11:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:11:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:11:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:11:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:11:24Z- TNavFilter::4 good beams. 2021-02-13T01:11:24Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:11:24Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:11:24Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:11:24Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:11:24Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:11:24Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:11:24Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:11:24Z- TNavBF::mmse selected from filter -1 2021-02-13T01:11:24Z- TerrainMapOctree: (vehN, vehE ) = (4061021.59, 593816.40). 2021-02-13T01:11:24Z- TerrainMapOctree: Min Distance = 490.08. 2021-02-13T01:11:24Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:11:24Z- There is no specific good beam that all particles have in common. 2021-02-13T01:11:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:11:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.98 K: 0.051 ExpectedMeasDiff: nan currMeas.covariance: 16.42 2021-02-13T01:11:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:11:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.93 K: 0.053 ExpectedMeasDiff: nan currMeas.covariance: 14.71 2021-02-13T01:11:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:11:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.88 K: 0.049 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-13T01:11:24Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:11:24Z- Old Depth Bias: 0.00 DepthBiasCov: 0.83 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 18.22 2021-02-13T01:11:24Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:11:24Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:11:24Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:11:24Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:11:24Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:24Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:11:24Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:11:24Z- Filter 0 No beams used in subcloud update 2021-02-13T01:11:24Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:11:24Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:11:24Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 673.1 EastVariance: 833.0 DepthVariance: 0.8 this->DepthBiasCov: 0.8 2021-02-13T01:11:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178683.62 sec. 2021-02-13T01:11:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:11:24Z- TNavBF::mmse selected from filter -1 2021-02-13T01:11:24Z- variances([0], [2], and psi_berg) = 673.086205 832.985650 0.000000 2021-02-13T01:11:24Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:11:24Z- LcmTrn::publishEstimates() - MLE : 4061021.10 593831.17 0.02 2021-02-13T01:11:24Z- LcmTrn::publishEstimates() - MMSE : 4061021.60 593816.67 0.02 2021-02-13T01:11:24Z- LcmTrn::publishEstimates() - COVAR: 673.09 832.99 0.81 0.00 2021-02-13T01:11:24Z- handleDepth()-Depth_Keller msg: 1613178684816 epoch sec; seqNo:19326; depth 0.07 2021-02-13T01:11:24Z- handleDvl() - RDI_Pathfinder msg: 1613178684.83 epoch sec; seqNo:9294 2021-02-13T01:11:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 97.04 , 97.04 , 103.80 2021-02-13T01:11:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.51 , 0.47 , -0.73 2021-02-13T01:11:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178684.85 epoch sec; seqNo:19326; 4061021.60 north; 593816.67 east 2021-02-13T01:11:25Z- AHRS_M2 msg: 1613178685.22 epoch sec; seqNo:19173 2021-02-13T01:11:25Z- AHRS_M2 msg: 0.08 phi; -0.01 theta; 0.01 psi 2021-02-13T01:11:25Z- handleDepth()-Depth_Keller msg: 1613178685223 epoch sec; seqNo:19327; depth 0.10 2021-02-13T01:11:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178685.26 epoch sec; seqNo:19327; 4061021.98 north; 593816.67 east 2021-02-13T01:11:25Z- AHRS_M2 msg: 1613178685.62 epoch sec; seqNo:19174 2021-02-13T01:11:25Z- AHRS_M2 msg: 0.08 phi; -0.02 theta; 0.11 psi 2021-02-13T01:11:25Z- handleDepth()-Depth_Keller msg: 1613178685628 epoch sec; seqNo:19328; depth 0.14 2021-02-13T01:11:25Z- handleDvl() - RDI_Pathfinder msg: 1613178685.65 epoch sec; seqNo:9295 2021-02-13T01:11:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.17 , 97.17 , 95.82 , 97.17 2021-02-13T01:11:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.09 , -0.53 , -0.26 2021-02-13T01:11:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178685.68 epoch sec; seqNo:19328; 4061021.98 north; 593816.67 east 2021-02-13T01:11:26Z- AHRS_M2 msg: 1613178686.02 epoch sec; seqNo:19175 2021-02-13T01:11:26Z- AHRS_M2 msg: 0.07 phi; 0.00 theta; 0.15 psi 2021-02-13T01:11:26Z- handleDepth()-Depth_Keller msg: 1613178686029 epoch sec; seqNo:19329; depth 0.17 2021-02-13T01:11:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178686.04 epoch sec; seqNo:19329; 4061021.98 north; 593816.67 east 2021-02-13T01:11:26Z- AHRS_M2 msg: 1613178686.43 epoch sec; seqNo:19176 2021-02-13T01:11:26Z- AHRS_M2 msg: 0.04 phi; 0.02 theta; 0.09 psi 2021-02-13T01:11:26Z- handleDepth()-Depth_Keller msg: 1613178686433 epoch sec; seqNo:19330; depth 0.17 2021-02-13T01:11:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178686.44 epoch sec; seqNo:19330; 4061021.98 north; 593816.67 east 2021-02-13T01:11:26Z- AHRS_M2 msg: 1613178686.83 epoch sec; seqNo:19177 2021-02-13T01:11:26Z- AHRS_M2 msg: 0.04 phi; -0.01 theta; -0.01 psi 2021-02-13T01:11:26Z- handleDepth()-Depth_Keller msg: 1613178686841 epoch sec; seqNo:19331; depth 0.16 2021-02-13T01:11:26Z- handleDvl() - RDI_Pathfinder msg: 1613178686.85 epoch sec; seqNo:9296 2021-02-13T01:11:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 108.13 , 90.55 , 98.66 , 95.96 2021-02-13T01:11:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.03 , -0.22 , 0.16 2021-02-13T01:11:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178686.88 epoch sec; seqNo:19331; 4061021.98 north; 593816.67 east 2021-02-13T01:11:27Z- AHRS_M2 msg: 1613178687.24 epoch sec; seqNo:19178 2021-02-13T01:11:27Z- AHRS_M2 msg: 0.05 phi; -0.09 theta; -0.07 psi 2021-02-13T01:11:27Z- handleDepth()-Depth_Keller msg: 1613178687241 epoch sec; seqNo:19332; depth 0.15 2021-02-13T01:11:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178687.25 epoch sec; seqNo:19332; 4061021.98 north; 593816.67 east 2021-02-13T01:11:27Z- AHRS_M2 msg: 1613178687.64 epoch sec; seqNo:19179 2021-02-13T01:11:27Z- AHRS_M2 msg: 0.01 phi; -0.12 theta; -0.03 psi 2021-02-13T01:11:27Z- handleDepth()-Depth_Keller msg: 1613178687659 epoch sec; seqNo:19333; depth 0.15 2021-02-13T01:11:27Z- handleDvl() - RDI_Pathfinder msg: 1613178687.68 epoch sec; seqNo:9297 2021-02-13T01:11:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.17 , 98.53 , 97.17 , 99.88 2021-02-13T01:11:27Z- handleDvl() - RDI_Pathfinder msg: velocities -0.26 , 0.04 , 0.20 2021-02-13T01:11:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178687.69 epoch sec; seqNo:19333; 4061021.98 north; 593816.67 east 2021-02-13T01:11:28Z- AHRS_M2 msg: 1613178688.04 epoch sec; seqNo:19180 2021-02-13T01:11:28Z- AHRS_M2 msg: 6.25 phi; -0.07 theta; 0.04 psi 2021-02-13T01:11:28Z- handleDepth()-Depth_Keller msg: 1613178688048 epoch sec; seqNo:19334; depth 0.13 2021-02-13T01:11:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178688.06 epoch sec; seqNo:19334; 4061021.60 north; 593816.67 east 2021-02-13T01:11:28Z- AHRS_M2 msg: 1613178688.45 epoch sec; seqNo:19181 2021-02-13T01:11:28Z- AHRS_M2 msg: 6.28 phi; -0.06 theta; 0.07 psi 2021-02-13T01:11:28Z- handleDepth()-Depth_Keller msg: 1613178688452 epoch sec; seqNo:19335; depth 0.09 2021-02-13T01:11:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178688.46 epoch sec; seqNo:19335; 4061021.60 north; 593816.67 east 2021-02-13T01:11:28Z- AHRS_M2 msg: 1613178688.85 epoch sec; seqNo:19182 2021-02-13T01:11:28Z- AHRS_M2 msg: 0.03 phi; -0.15 theta; 0.05 psi 2021-02-13T01:11:28Z- handleDepth()-Depth_Keller msg: 1613178688857 epoch sec; seqNo:19336; depth 0.05 2021-02-13T01:11:28Z- handleDvl() - RDI_Pathfinder msg: 1613178688.87 epoch sec; seqNo:9298 2021-02-13T01:11:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.23 , 94.47 , 97.17 , 102.58 2021-02-13T01:11:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.06 , 0.25 , 0.46 2021-02-13T01:11:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178688.88 epoch sec; seqNo:19336; 4061021.60 north; 593816.67 east 2021-02-13T01:11:29Z- AHRS_M2 msg: 1613178689.26 epoch sec; seqNo:19183 2021-02-13T01:11:29Z- AHRS_M2 msg: 6.27 phi; -0.21 theta; 0.04 psi 2021-02-13T01:11:29Z- handleDepth()-Depth_Keller msg: 1613178689260 epoch sec; seqNo:19337; depth 0.06 2021-02-13T01:11:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178689.28 epoch sec; seqNo:19337; 4061021.60 north; 593816.67 east 2021-02-13T01:11:29Z- AHRS_M2 msg: 1613178689.66 epoch sec; seqNo:19184 2021-02-13T01:11:29Z- AHRS_M2 msg: 6.20 phi; -0.17 theta; 0.01 psi 2021-02-13T01:11:29Z- handleDepth()-Depth_Keller msg: 1613178689669 epoch sec; seqNo:19338; depth 0.06 2021-02-13T01:11:29Z- handleDvl() - RDI_Pathfinder msg: 1613178689.69 epoch sec; seqNo:9299 2021-02-13T01:11:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.87 , 95.75 , 90.35 , 113.33 2021-02-13T01:11:29Z- handleDvl() - RDI_Pathfinder msg: velocities -0.16 , 0.43 , -0.35 2021-02-13T01:11:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178689.70 epoch sec; seqNo:19338; 4061021.60 north; 593816.67 east 2021-02-13T01:11:29Z- LcmTrn::handleMessages() - handling 44 messages took 0.64 ms and 132 calls... 2021-02-13T01:11:29Z- waiting for 1613178687.81, time is 1613178689.70 2021-02-13T01:11:29Z- LcmTrn::updateTrn() equating pose 1613178689.70 and meas times 1613178689.70 2021-02-13T01:11:29Z- LcmTrn::updateTrn() >>>> pose time 1613178689.70, meas time 1613178689.70 <<<< 2021-02-13T01:11:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178689.70 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:11:29Z- TerrainNav::motionUpdate() - currEstimate = (4061022, 593816.67, 0.06) 2021-02-13T01:11:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:11:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:11:29Z- TNavBF::Projecting Measurements 2021-02-13T01:11:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:11:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:11:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:11:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:11:29Z- TNavFilter::4 good beams. 2021-02-13T01:11:29Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:11:29Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:11:29Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:11:29Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:11:29Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:11:29Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:11:29Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:11:29Z- TNavBF::mmse selected from filter -1 2021-02-13T01:11:29Z- TerrainMapOctree: (vehN, vehE ) = (4061021.60, 593816.67). 2021-02-13T01:11:29Z- TerrainMapOctree: Min Distance = 489.84. 2021-02-13T01:11:29Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:11:29Z- There is no specific good beam that all particles have in common. 2021-02-13T01:11:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:11:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.81 K: 0.041 ExpectedMeasDiff: nan currMeas.covariance: 17.26 2021-02-13T01:11:29Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:11:29Z- Old Depth Bias: 0.00 DepthBiasCov: 0.78 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 14.67 2021-02-13T01:11:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:11:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.75 K: 0.048 ExpectedMeasDiff: nan currMeas.covariance: 13.06 2021-02-13T01:11:30Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:11:30Z- Old Depth Bias: 0.00 DepthBiasCov: 0.71 K: 0.031 ExpectedMeasDiff: nan currMeas.covariance: 20.55 2021-02-13T01:11:30Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:11:30Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:11:30Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:11:30Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:11:30Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:30Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:11:30Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:11:30Z- Filter 0 No beams used in subcloud update 2021-02-13T01:11:30Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:11:30Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:11:30Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 673.1 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T01:11:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178689.70 sec. 2021-02-13T01:11:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:11:30Z- TNavBF::mmse selected from filter -1 2021-02-13T01:11:30Z- variances([0], [2], and psi_berg) = 673.086205 832.985650 0.000000 2021-02-13T01:11:30Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:11:30Z- LcmTrn::publishEstimates() - MLE : 4061021.10 593831.17 0.06 2021-02-13T01:11:30Z- LcmTrn::publishEstimates() - MMSE : 4061021.60 593816.67 0.06 2021-02-13T01:11:30Z- LcmTrn::publishEstimates() - COVAR: 673.09 832.99 0.69 0.00 2021-02-13T01:11:30Z- AHRS_M2 msg: 1613178690.06 epoch sec; seqNo:19185 2021-02-13T01:11:30Z- AHRS_M2 msg: 6.20 phi; -0.10 theta; -0.06 psi 2021-02-13T01:11:30Z- handleDepth()-Depth_Keller msg: 1613178690073 epoch sec; seqNo:19339; depth 0.03 2021-02-13T01:11:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:11:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:11:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2021-02-13T01:11:30Z- handleDVL() - Beam1Range item not found in dvl msg 2021-02-13T01:11:30Z- handleDVL() - Beam2Range item not found in dvl msg 2021-02-13T01:11:30Z- handleDVL() - Beam3Range item not found in dvl msg 2021-02-13T01:11:30Z- handleDVL() - Beam4Range item not found in dvl msg 2021-02-13T01:11:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2021-02-13T01:11:30Z- handleDvl() - RDI_Pathfinder msg: 1613178690.08 epoch sec; seqNo:9300 2021-02-13T01:11:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:11:30Z- handleDvl() - RDI_Pathfinder msg: velocities -0.16 , 0.43 , -0.35 2021-02-13T01:11:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178690.09 epoch sec; seqNo:19339; 4061021.60 north; 593816.67 east 2021-02-13T01:11:30Z- AHRS_M2 msg: 1613178690.47 epoch sec; seqNo:19186 2021-02-13T01:11:30Z- AHRS_M2 msg: 6.22 phi; -0.04 theta; -0.07 psi 2021-02-13T01:11:30Z- handleDepth()-Depth_Keller msg: 1613178690475 epoch sec; seqNo:19340; depth 0.03 2021-02-13T01:11:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178690.48 epoch sec; seqNo:19340; 4061021.60 north; 593816.67 east 2021-02-13T01:11:30Z- AHRS_M2 msg: 1613178690.87 epoch sec; seqNo:19187 2021-02-13T01:11:30Z- AHRS_M2 msg: 6.20 phi; -0.02 theta; -0.02 psi 2021-02-13T01:11:30Z- handleDepth()-Depth_Keller msg: 1613178690886 epoch sec; seqNo:19341; depth 0.09 2021-02-13T01:11:30Z- handleDvl() - RDI_Pathfinder msg: 1613178690.90 epoch sec; seqNo:9301 2021-02-13T01:11:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.62 , 99.68 , 92.92 , 106.44 2021-02-13T01:11:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.21 , 0.00 , -0.14 2021-02-13T01:11:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178690.91 epoch sec; seqNo:19341; 4061021.60 north; 593816.67 east 2021-02-13T01:11:31Z- AHRS_M2 msg: 1613178691.28 epoch sec; seqNo:19188 2021-02-13T01:11:31Z- AHRS_M2 msg: 6.14 phi; 0.00 theta; 0.01 psi 2021-02-13T01:11:31Z- handleDepth()-Depth_Keller msg: 1613178691280 epoch sec; seqNo:19342; depth 0.09 2021-02-13T01:11:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178691.29 epoch sec; seqNo:19342; 4061021.60 north; 593816.67 east 2021-02-13T01:11:31Z- AHRS_M2 msg: 1613178691.68 epoch sec; seqNo:19189 2021-02-13T01:11:31Z- AHRS_M2 msg: 6.12 phi; 0.00 theta; 0.01 psi 2021-02-13T01:11:31Z- handleDepth()-Depth_Keller msg: 1613178691688 epoch sec; seqNo:19343; depth 0.08 2021-02-13T01:11:31Z- handleDvl() - RDI_Pathfinder msg: 1613178691.71 epoch sec; seqNo:9302 2021-02-13T01:11:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.82 , 95.82 , 97.17 , 95.82 2021-02-13T01:11:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.08 , -0.35 , 0.03 2021-02-13T01:11:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178691.74 epoch sec; seqNo:19343; 4061021.60 north; 593816.67 east 2021-02-13T01:11:32Z- AHRS_M2 msg: 1613178692.09 epoch sec; seqNo:19190 2021-02-13T01:11:32Z- AHRS_M2 msg: 6.18 phi; 0.03 theta; 0.02 psi 2021-02-13T01:11:32Z- handleDepth()-Depth_Keller msg: 1613178692089 epoch sec; seqNo:19344; depth 0.09 2021-02-13T01:11:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178692.12 epoch sec; seqNo:19344; 4061021.60 north; 593816.67 east 2021-02-13T01:11:32Z- AHRS_M2 msg: 1613178692.49 epoch sec; seqNo:19191 2021-02-13T01:11:32Z- AHRS_M2 msg: 6.23 phi; 0.10 theta; 0.06 psi 2021-02-13T01:11:32Z- handleDepth()-Depth_Keller msg: 1613178692492 epoch sec; seqNo:19345; depth 0.11 2021-02-13T01:11:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178692.52 epoch sec; seqNo:19345; 4061021.60 north; 593816.67 east 2021-02-13T01:11:32Z- AHRS_M2 msg: 1613178692.89 epoch sec; seqNo:19192 2021-02-13T01:11:32Z- AHRS_M2 msg: 6.22 phi; 0.15 theta; 0.04 psi 2021-02-13T01:11:32Z- handleDepth()-Depth_Keller msg: 1613178692901 epoch sec; seqNo:19346; depth 0.08 2021-02-13T01:11:32Z- handleDvl() - RDI_Pathfinder msg: 1613178692.92 epoch sec; seqNo:9303 2021-02-13T01:11:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.73 , 94.67 , 98.73 , 97.38 2021-02-13T01:11:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.35 , 0.22 , 0.16 2021-02-13T01:11:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178692.93 epoch sec; seqNo:19346; 4061021.98 north; 593816.67 east 2021-02-13T01:11:33Z- AHRS_M2 msg: 1613178693.31 epoch sec; seqNo:19193 2021-02-13T01:11:33Z- AHRS_M2 msg: 6.20 phi; -0.10 theta; 0.08 psi 2021-02-13T01:11:33Z- handleDepth()-Depth_Keller msg: 1613178693313 epoch sec; seqNo:19347; depth 0.03 2021-02-13T01:11:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178693.33 epoch sec; seqNo:19347; 4061021.98 north; 593816.67 east 2021-02-13T01:11:33Z- AHRS_M2 msg: 1613178693.71 epoch sec; seqNo:19194 2021-02-13T01:11:33Z- AHRS_M2 msg: 6.14 phi; -0.21 theta; 0.13 psi 2021-02-13T01:11:33Z- handleDepth()-Depth_Keller msg: 1613178693714 epoch sec; seqNo:19348; depth 0.05 2021-02-13T01:11:33Z- handleDvl() - RDI_Pathfinder msg: 1613178693.73 epoch sec; seqNo:9304 2021-02-13T01:11:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:11:33Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:11:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178693.74 epoch sec; seqNo:19348; 4061021.98 north; 593816.67 east 2021-02-13T01:11:34Z- AHRS_M2 msg: 1613178694.10 epoch sec; seqNo:19195 2021-02-13T01:11:34Z- AHRS_M2 msg: 6.07 phi; -0.16 theta; 0.15 psi 2021-02-13T01:11:34Z- handleDepth()-Depth_Keller msg: 1613178694113 epoch sec; seqNo:19349; depth 0.03 2021-02-13T01:11:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178694.13 epoch sec; seqNo:19349; 4061022.35 north; 593816.67 east 2021-02-13T01:11:34Z- AHRS_M2 msg: 1613178694.51 epoch sec; seqNo:19196 2021-02-13T01:11:34Z- AHRS_M2 msg: 6.02 phi; -0.06 theta; 0.04 psi 2021-02-13T01:11:34Z- handleDepth()-Depth_Keller msg: 1613178694512 epoch sec; seqNo:19350; depth 0.03 2021-02-13T01:11:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178694.53 epoch sec; seqNo:19350; 4061022.35 north; 593816.67 east 2021-02-13T01:11:34Z- AHRS_M2 msg: 1613178694.91 epoch sec; seqNo:19197 2021-02-13T01:11:34Z- AHRS_M2 msg: 5.96 phi; 0.01 theta; -0.14 psi 2021-02-13T01:11:34Z- handleDepth()-Depth_Keller msg: 1613178694916 epoch sec; seqNo:19351; depth 0.08 2021-02-13T01:11:34Z- handleDvl() - RDI_Pathfinder msg: 1613178694.93 epoch sec; seqNo:9305 2021-02-13T01:11:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 110.70 , 93.12 , 97.17 , 114.75 2021-02-13T01:11:34Z- handleDvl() - RDI_Pathfinder msg: velocities -0.16 , 0.55 , -0.35 2021-02-13T01:11:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178694.94 epoch sec; seqNo:19351; 4061022.35 north; 593816.67 east 2021-02-13T01:11:35Z- LcmTrn::handleMessages() - handling 45 messages took 0.74 ms and 131 calls... 2021-02-13T01:11:35Z- waiting for 1613178692.70, time is 1613178694.94 2021-02-13T01:11:35Z- LcmTrn::updateTrn() equating pose 1613178694.94 and meas times 1613178694.94 2021-02-13T01:11:35Z- LcmTrn::updateTrn() >>>> pose time 1613178694.94, meas time 1613178694.94 <<<< 2021-02-13T01:11:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178694.94 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:11:35Z- TerrainNav::motionUpdate() - currEstimate = (4061022, 593816.67, 0.08) 2021-02-13T01:11:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:11:35Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:11:35Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178694.94 sec.; No relevant navigation data available 2021-02-13T01:11:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:11:35Z- TNavBF::mmse selected from filter -1 2021-02-13T01:11:35Z- variances([0], [2], and psi_berg) = 673.068412 832.985643 0.000000 2021-02-13T01:11:35Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:11:35Z- LcmTrn::publishEstimates() - MLE : 4061021.85 593832.17 0.08 2021-02-13T01:11:35Z- LcmTrn::publishEstimates() - MMSE : 4061022.35 593816.67 0.08 2021-02-13T01:11:35Z- LcmTrn::publishEstimates() - COVAR: 673.07 832.99 0.73 0.00 2021-02-13T01:11:35Z- AHRS_M2 msg: 1613178695.32 epoch sec; seqNo:19198 2021-02-13T01:11:35Z- AHRS_M2 msg: 5.97 phi; 0.02 theta; -0.20 psi 2021-02-13T01:11:35Z- handleDepth()-Depth_Keller msg: 1613178695321 epoch sec; seqNo:19352; depth 0.12 2021-02-13T01:11:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178695.33 epoch sec; seqNo:19352; 4061022.37 north; 593817.89 east 2021-02-13T01:11:35Z- AHRS_M2 msg: 1613178695.72 epoch sec; seqNo:19199 2021-02-13T01:11:35Z- AHRS_M2 msg: 5.97 phi; -0.00 theta; -0.10 psi 2021-02-13T01:11:35Z- handleDepth()-Depth_Keller msg: 1613178695725 epoch sec; seqNo:19353; depth 0.16 2021-02-13T01:11:35Z- handleDvl() - RDI_Pathfinder msg: 1613178695.74 epoch sec; seqNo:9306 2021-02-13T01:11:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.78 , 100.76 , 97.78 , 100.76 2021-02-13T01:11:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.22 , -0.24 , -0.48 2021-02-13T01:11:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178695.75 epoch sec; seqNo:19353; 4061022.35 north; 593816.67 east 2021-02-13T01:11:36Z- AHRS_M2 msg: 1613178696.12 epoch sec; seqNo:19200 2021-02-13T01:11:36Z- AHRS_M2 msg: 5.96 phi; 0.02 theta; 0.04 psi 2021-02-13T01:11:36Z- handleDepth()-Depth_Keller msg: 1613178696137 epoch sec; seqNo:19354; depth 0.22 2021-02-13T01:11:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178696.16 epoch sec; seqNo:19354; 4061022.73 north; 593816.66 east 2021-02-13T01:11:36Z- AHRS_M2 msg: 1613178696.53 epoch sec; seqNo:19201 2021-02-13T01:11:36Z- AHRS_M2 msg: 5.96 phi; 0.04 theta; 0.14 psi 2021-02-13T01:11:36Z- handleDepth()-Depth_Keller msg: 1613178696540 epoch sec; seqNo:19355; depth 0.25 2021-02-13T01:11:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178696.58 epoch sec; seqNo:19355; 4061022.73 north; 593816.66 east 2021-02-13T01:11:36Z- AHRS_M2 msg: 1613178696.93 epoch sec; seqNo:19202 2021-02-13T01:11:36Z- AHRS_M2 msg: 5.97 phi; 0.04 theta; 0.13 psi 2021-02-13T01:11:36Z- handleDepth()-Depth_Keller msg: 1613178696939 epoch sec; seqNo:19356; depth 0.28 2021-02-13T01:11:36Z- handleDvl() - RDI_Pathfinder msg: 1613178696.95 epoch sec; seqNo:9307 2021-02-13T01:11:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 98.46 , 99.81 , 97.11 2021-02-13T01:11:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.21 , -0.05 , 0.01 2021-02-13T01:11:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178696.96 epoch sec; seqNo:19356; 4061022.73 north; 593816.66 east 2021-02-13T01:11:37Z- AHRS_M2 msg: 1613178697.34 epoch sec; seqNo:19203 2021-02-13T01:11:37Z- AHRS_M2 msg: 5.98 phi; -0.01 theta; -0.01 psi 2021-02-13T01:11:37Z- handleDepth()-Depth_Keller msg: 1613178697341 epoch sec; seqNo:19357; depth 0.26 2021-02-13T01:11:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178697.36 epoch sec; seqNo:19357; 4061022.73 north; 593816.66 east 2021-02-13T01:11:37Z- AHRS_M2 msg: 1613178697.74 epoch sec; seqNo:19204 2021-02-13T01:11:37Z- AHRS_M2 msg: 5.94 phi; -0.05 theta; -0.13 psi 2021-02-13T01:11:37Z- handleDepth()-Depth_Keller msg: 1613178697745 epoch sec; seqNo:19358; depth 0.24 2021-02-13T01:11:37Z- handleDvl() - RDI_Pathfinder msg: 1613178697.76 epoch sec; seqNo:9308 2021-02-13T01:11:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 107.99 , 90.41 , 102.58 , 95.82 2021-02-13T01:11:37Z- handleDvl() - RDI_Pathfinder msg: velocities -0.00 , 0.25 , 0.18 2021-02-13T01:11:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178697.78 epoch sec; seqNo:19358; 4061022.73 north; 593816.66 east 2021-02-13T01:11:38Z- AHRS_M2 msg: 1613178698.15 epoch sec; seqNo:19205 2021-02-13T01:11:38Z- AHRS_M2 msg: 5.91 phi; -0.10 theta; -0.15 psi 2021-02-13T01:11:38Z- handleDepth()-Depth_Keller msg: 1613178698151 epoch sec; seqNo:19359; depth 0.23 2021-02-13T01:11:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178698.16 epoch sec; seqNo:19359; 4061022.73 north; 593816.66 east 2021-02-13T01:11:38Z- AHRS_M2 msg: 1613178698.58 epoch sec; seqNo:19206 2021-02-13T01:11:38Z- AHRS_M2 msg: 5.88 phi; -0.18 theta; -0.11 psi 2021-02-13T01:11:38Z- handleDepth()-Depth_Keller msg: 1613178698579 epoch sec; seqNo:19360; depth 0.21 2021-02-13T01:11:38Z- handleDvl() - RDI_Pathfinder msg: 1613178698.59 epoch sec; seqNo:9309 2021-02-13T01:11:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.70 , 105.22 , 97.11 , 101.16 2021-02-13T01:11:38Z- handleDvl() - RDI_Pathfinder msg: velocities -0.44 , 0.19 , 0.15 2021-02-13T01:11:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178698.62 epoch sec; seqNo:19360; 4061022.75 north; 593817.88 east 2021-02-13T01:11:38Z- AHRS_M2 msg: 1613178698.95 epoch sec; seqNo:19207 2021-02-13T01:11:38Z- AHRS_M2 msg: 5.90 phi; -0.19 theta; -0.09 psi 2021-02-13T01:11:38Z- handleDepth()-Depth_Keller msg: 1613178698960 epoch sec; seqNo:19361; depth 0.23 2021-02-13T01:11:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178698.98 epoch sec; seqNo:19361; 4061022.37 north; 593817.89 east 2021-02-13T01:11:39Z- AHRS_M2 msg: 1613178699.36 epoch sec; seqNo:19208 2021-02-13T01:11:39Z- AHRS_M2 msg: 5.94 phi; -0.13 theta; -0.08 psi 2021-02-13T01:11:39Z- handleDepth()-Depth_Keller msg: 1613178699365 epoch sec; seqNo:19362; depth 0.22 2021-02-13T01:11:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178699.37 epoch sec; seqNo:19362; 4061022.37 north; 593817.89 east 2021-02-13T01:11:39Z- AHRS_M2 msg: 1613178699.76 epoch sec; seqNo:19209 2021-02-13T01:11:39Z- AHRS_M2 msg: 5.97 phi; -0.12 theta; -0.06 psi 2021-02-13T01:11:39Z- handleDepth()-Depth_Keller msg: 1613178699769 epoch sec; seqNo:19363; depth 0.17 2021-02-13T01:11:39Z- handleDvl() - RDI_Pathfinder msg: 1613178699.79 epoch sec; seqNo:9310 2021-02-13T01:11:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 102.52 , 90.35 , 111.98 2021-02-13T01:11:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.10 , 0.13 , -0.58 2021-02-13T01:11:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178699.80 epoch sec; seqNo:19363; 4061022.37 north; 593817.89 east 2021-02-13T01:11:40Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 129 calls... 2021-02-13T01:11:40Z- waiting for 1613178697.94, time is 1613178699.80 2021-02-13T01:11:40Z- LcmTrn::updateTrn() equating pose 1613178699.80 and meas times 1613178699.80 2021-02-13T01:11:40Z- LcmTrn::updateTrn() >>>> pose time 1613178699.80, meas time 1613178699.80 <<<< 2021-02-13T01:11:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178699.80 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:11:40Z- TerrainNav::motionUpdate() - currEstimate = (4061022, 593817.89, 0.17) 2021-02-13T01:11:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:11:40Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:11:40Z- TNavBF::Projecting Measurements 2021-02-13T01:11:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:11:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:11:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:11:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:11:40Z- TNavFilter::4 good beams. 2021-02-13T01:11:40Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:11:40Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:11:40Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:11:40Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:11:40Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:11:40Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:11:40Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:11:40Z- TNavBF::mmse selected from filter -1 2021-02-13T01:11:40Z- TerrainMapOctree: (vehN, vehE ) = (4061022.37, 593817.89). 2021-02-13T01:11:40Z- TerrainMapOctree: Min Distance = 488.40. 2021-02-13T01:11:40Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:11:40Z- There is no specific good beam that all particles have in common. 2021-02-13T01:11:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:11:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.79 K: 0.045 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-13T01:11:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:11:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.75 K: 0.039 ExpectedMeasDiff: nan currMeas.covariance: 16.82 2021-02-13T01:11:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:11:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.72 K: 0.047 ExpectedMeasDiff: nan currMeas.covariance: 13.06 2021-02-13T01:11:40Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:11:40Z- Old Depth Bias: 0.00 DepthBiasCov: 0.69 K: 0.031 ExpectedMeasDiff: nan currMeas.covariance: 20.06 2021-02-13T01:11:40Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:11:40Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:11:40Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:11:40Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:11:40Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:40Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:11:40Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:11:40Z- Filter 0 No beams used in subcloud update 2021-02-13T01:11:40Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:11:40Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:11:40Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 673.0 EastVariance: 833.0 DepthVariance: 0.7 this->DepthBiasCov: 0.7 2021-02-13T01:11:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178699.80 sec. 2021-02-13T01:11:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:11:40Z- TNavBF::mmse selected from filter -1 2021-02-13T01:11:40Z- variances([0], [2], and psi_berg) = 673.045835 832.985634 0.000000 2021-02-13T01:11:40Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:11:40Z- LcmTrn::publishEstimates() - MLE : 4061021.87 593833.39 0.17 2021-02-13T01:11:40Z- LcmTrn::publishEstimates() - MMSE : 4061022.37 593817.89 0.17 2021-02-13T01:11:40Z- LcmTrn::publishEstimates() - COVAR: 673.05 832.99 0.67 0.00 2021-02-13T01:11:40Z- AHRS_M2 msg: 1613178700.17 epoch sec; seqNo:19210 2021-02-13T01:11:40Z- AHRS_M2 msg: 5.94 phi; -0.17 theta; -0.03 psi 2021-02-13T01:11:40Z- handleDepth()-Depth_Keller msg: 1613178700177 epoch sec; seqNo:19364; depth 0.12 2021-02-13T01:11:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178700.19 epoch sec; seqNo:19364; 4061022.75 north; 593817.88 east 2021-02-13T01:11:40Z- AHRS_M2 msg: 1613178700.59 epoch sec; seqNo:19211 2021-02-13T01:11:40Z- AHRS_M2 msg: 5.92 phi; -0.22 theta; -0.01 psi 2021-02-13T01:11:40Z- handleDepth()-Depth_Keller msg: 1613178700593 epoch sec; seqNo:19365; depth 0.11 2021-02-13T01:11:40Z- handleDvl() - RDI_Pathfinder msg: 1613178700.61 epoch sec; seqNo:9311 2021-02-13T01:11:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 95.62 , 91.56 , 106.44 2021-02-13T01:11:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.04 , 0.03 , 0.11 2021-02-13T01:11:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178700.62 epoch sec; seqNo:19365; 4061022.75 north; 593817.88 east 2021-02-13T01:11:40Z- AHRS_M2 msg: 1613178700.97 epoch sec; seqNo:19212 2021-02-13T01:11:40Z- AHRS_M2 msg: 5.95 phi; -0.21 theta; -0.03 psi 2021-02-13T01:11:40Z- handleDepth()-Depth_Keller msg: 1613178700981 epoch sec; seqNo:19366; depth 0.10 2021-02-13T01:11:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178701.02 epoch sec; seqNo:19366; 4061022.75 north; 593817.88 east 2021-02-13T01:11:41Z- AHRS_M2 msg: 1613178701.38 epoch sec; seqNo:19213 2021-02-13T01:11:41Z- AHRS_M2 msg: 5.98 phi; -0.22 theta; -0.04 psi 2021-02-13T01:11:41Z- handleDepth()-Depth_Keller msg: 1613178701381 epoch sec; seqNo:19367; depth 0.06 2021-02-13T01:11:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178701.41 epoch sec; seqNo:19367; 4061022.75 north; 593817.88 east 2021-02-13T01:11:41Z- AHRS_M2 msg: 1613178701.78 epoch sec; seqNo:19214 2021-02-13T01:11:41Z- AHRS_M2 msg: 5.92 phi; -0.22 theta; -0.02 psi 2021-02-13T01:11:41Z- handleDepth()-Depth_Keller msg: 1613178701794 epoch sec; seqNo:19368; depth 0.09 2021-02-13T01:11:41Z- handleDvl() - RDI_Pathfinder msg: 1613178701.81 epoch sec; seqNo:9312 2021-02-13T01:11:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.59 , 95.89 , 89.13 , 113.47 2021-02-13T01:11:41Z- handleDvl() - RDI_Pathfinder msg: velocities -0.13 , -0.04 , 0.02 2021-02-13T01:11:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178701.82 epoch sec; seqNo:19368; 4061022.75 north; 593817.88 east 2021-02-13T01:11:42Z- AHRS_M2 msg: 1613178702.18 epoch sec; seqNo:19215 2021-02-13T01:11:42Z- AHRS_M2 msg: 5.90 phi; -0.15 theta; -0.05 psi 2021-02-13T01:11:42Z- handleDepth()-Depth_Keller msg: 1613178702193 epoch sec; seqNo:19369; depth 0.08 2021-02-13T01:11:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178702.20 epoch sec; seqNo:19369; 4061022.37 north; 593817.89 east 2021-02-13T01:11:42Z- AHRS_M2 msg: 1613178702.59 epoch sec; seqNo:19216 2021-02-13T01:11:42Z- AHRS_M2 msg: 5.93 phi; -0.11 theta; -0.06 psi 2021-02-13T01:11:42Z- handleDepth()-Depth_Keller msg: 1613178702593 epoch sec; seqNo:19370; depth 0.06 2021-02-13T01:11:42Z- handleDvl() - RDI_Pathfinder msg: 1613178702.61 epoch sec; seqNo:9313 2021-02-13T01:11:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.46 , 98.46 , 88.99 , 111.98 2021-02-13T01:11:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.07 , -0.25 , -0.06 2021-02-13T01:11:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178702.62 epoch sec; seqNo:19370; 4061022.37 north; 593817.89 east 2021-02-13T01:11:42Z- AHRS_M2 msg: 1613178702.99 epoch sec; seqNo:19217 2021-02-13T01:11:42Z- AHRS_M2 msg: 5.94 phi; -0.06 theta; 0.00 psi 2021-02-13T01:11:42Z- handleDepth()-Depth_Keller msg: 1613178702997 epoch sec; seqNo:19371; depth 0.06 2021-02-13T01:11:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178703.01 epoch sec; seqNo:19371; 4061022.37 north; 593817.89 east 2021-02-13T01:11:43Z- AHRS_M2 msg: 1613178703.40 epoch sec; seqNo:19218 2021-02-13T01:11:43Z- AHRS_M2 msg: 5.98 phi; -0.05 theta; 0.08 psi 2021-02-13T01:11:43Z- handleDepth()-Depth_Keller msg: 1613178703410 epoch sec; seqNo:19372; depth 0.04 2021-02-13T01:11:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178703.42 epoch sec; seqNo:19372; 4061022.37 north; 593817.89 east 2021-02-13T01:11:43Z- AHRS_M2 msg: 1613178703.80 epoch sec; seqNo:19219 2021-02-13T01:11:43Z- AHRS_M2 msg: 5.98 phi; -0.10 theta; 0.16 psi 2021-02-13T01:11:43Z- handleDepth()-Depth_Keller msg: 1613178703809 epoch sec; seqNo:19373; depth 0.03 2021-02-13T01:11:43Z- handleDvl() - RDI_Pathfinder msg: 1613178703.83 epoch sec; seqNo:9314 2021-02-13T01:11:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.75 , 97.11 , 94.40 , 99.81 2021-02-13T01:11:43Z- handleDvl() - RDI_Pathfinder msg: velocities -0.07 , -0.14 , 0.38 2021-02-13T01:11:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178703.84 epoch sec; seqNo:19373; 4061022.35 north; 593816.67 east 2021-02-13T01:11:44Z- AHRS_M2 msg: 1613178704.20 epoch sec; seqNo:19220 2021-02-13T01:11:44Z- AHRS_M2 msg: 5.90 phi; -0.09 theta; 0.22 psi 2021-02-13T01:11:44Z- handleDepth()-Depth_Keller msg: 1613178704213 epoch sec; seqNo:19374; depth 0.03 2021-02-13T01:11:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178704.22 epoch sec; seqNo:19374; 4061022.35 north; 593816.67 east 2021-02-13T01:11:44Z- AHRS_M2 msg: 1613178704.61 epoch sec; seqNo:19221 2021-02-13T01:11:44Z- AHRS_M2 msg: 5.87 phi; -0.06 theta; 0.14 psi 2021-02-13T01:11:44Z- handleDepth()-Depth_Keller msg: 1613178704615 epoch sec; seqNo:19375; depth 0.03 2021-02-13T01:11:44Z- handleDvl() - RDI_Pathfinder msg: 1613178704.63 epoch sec; seqNo:9315 2021-02-13T01:11:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 113.54 , 89.20 , 97.31 , 106.78 2021-02-13T01:11:44Z- handleDvl() - RDI_Pathfinder msg: velocities -0.14 , 0.45 , 0.40 2021-02-13T01:11:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178704.64 epoch sec; seqNo:19375; 4061022.37 north; 593817.89 east 2021-02-13T01:11:45Z- AHRS_M2 msg: 1613178705.01 epoch sec; seqNo:19222 2021-02-13T01:11:45Z- AHRS_M2 msg: 5.86 phi; -0.02 theta; 0.00 psi 2021-02-13T01:11:45Z- handleDepth()-Depth_Keller msg: 1613178705022 epoch sec; seqNo:19376; depth 0.04 2021-02-13T01:11:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178705.03 epoch sec; seqNo:19376; 4061022.37 north; 593817.89 east 2021-02-13T01:11:45Z- LcmTrn::handleMessages() - handling 44 messages took 0.63 ms and 132 calls... 2021-02-13T01:11:45Z- waiting for 1613178702.80, time is 1613178705.03 2021-02-13T01:11:45Z- LcmTrn::updateTrn() >>>> pose time 1613178705.03, meas time 1613178704.63 <<<< 2021-02-13T01:11:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178704.63 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:11:45Z- TerrainNav::motionUpdate() - currEstimate = (4061022, 593817.89, 0.04) 2021-02-13T01:11:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:11:45Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:11:45Z- TNavBF::Projecting Measurements 2021-02-13T01:11:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:11:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:11:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:11:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:11:45Z- TNavFilter::4 good beams. 2021-02-13T01:11:45Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:11:45Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:11:45Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:11:45Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:11:45Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:11:45Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:11:45Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:11:45Z- TNavBF::mmse selected from filter -1 2021-02-13T01:11:45Z- TerrainMapOctree: (vehN, vehE ) = (4061022.37, 593817.89). 2021-02-13T01:11:45Z- TerrainMapOctree: Min Distance = 488.40. 2021-02-13T01:11:45Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:11:45Z- There is no specific good beam that all particles have in common. 2021-02-13T01:11:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:11:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.67 K: 0.029 ExpectedMeasDiff: nan currMeas.covariance: 20.63 2021-02-13T01:11:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:11:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.65 K: 0.043 ExpectedMeasDiff: nan currMeas.covariance: 12.73 2021-02-13T01:11:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:11:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.62 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 15.15 2021-02-13T01:11:45Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:11:45Z- Old Depth Bias: 0.00 DepthBiasCov: 0.60 K: 0.029 ExpectedMeasDiff: nan currMeas.covariance: 18.24 2021-02-13T01:11:45Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:11:45Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:11:45Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:11:45Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:11:45Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:45Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:11:45Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:11:45Z- Filter 0 No beams used in subcloud update 2021-02-13T01:11:45Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:11:45Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:11:45Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -0.0 NorthVariance: 673.0 EastVariance: 833.0 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-13T01:11:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178704.63 sec. 2021-02-13T01:11:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:11:45Z- TNavBF::mmse selected from filter -1 2021-02-13T01:11:45Z- variances([0], [2], and psi_berg) = 673.045835 832.985634 0.000000 2021-02-13T01:11:45Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:11:45Z- LcmTrn::publishEstimates() - MLE : 4061021.87 593833.39 0.04 2021-02-13T01:11:45Z- LcmTrn::publishEstimates() - MMSE : 4061022.37 593817.89 0.04 2021-02-13T01:11:45Z- LcmTrn::publishEstimates() - COVAR: 673.05 832.99 0.58 0.00 2021-02-13T01:11:45Z- AHRS_M2 msg: 1613178705.42 epoch sec; seqNo:19223 2021-02-13T01:11:45Z- AHRS_M2 msg: 5.83 phi; 0.05 theta; -0.07 psi 2021-02-13T01:11:45Z- handleDepth()-Depth_Keller msg: 1613178705425 epoch sec; seqNo:19377; depth 0.06 2021-02-13T01:11:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178705.45 epoch sec; seqNo:19377; 4061022.37 north; 593817.89 east 2021-02-13T01:11:45Z- AHRS_M2 msg: 1613178705.82 epoch sec; seqNo:19224 2021-02-13T01:11:45Z- AHRS_M2 msg: 5.81 phi; 0.05 theta; -0.09 psi 2021-02-13T01:11:45Z- handleDepth()-Depth_Keller msg: 1613178705824 epoch sec; seqNo:19378; depth 0.07 2021-02-13T01:11:45Z- handleDvl() - RDI_Pathfinder msg: 1613178705.85 epoch sec; seqNo:9316 2021-02-13T01:11:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 97.99 , 97.99 , 97.99 2021-02-13T01:11:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.21 , -0.02 , -0.03 2021-02-13T01:11:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178705.88 epoch sec; seqNo:19378; 4061022.37 north; 593817.89 east 2021-02-13T01:11:46Z- AHRS_M2 msg: 1613178706.23 epoch sec; seqNo:19225 2021-02-13T01:11:46Z- AHRS_M2 msg: 5.83 phi; -0.02 theta; -0.01 psi 2021-02-13T01:11:46Z- handleDepth()-Depth_Keller msg: 1613178706230 epoch sec; seqNo:19379; depth 0.09 2021-02-13T01:11:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178706.24 epoch sec; seqNo:19379; 4061022.75 north; 593817.88 east 2021-02-13T01:11:46Z- AHRS_M2 msg: 1613178706.63 epoch sec; seqNo:19226 2021-02-13T01:11:46Z- AHRS_M2 msg: 5.87 phi; -0.12 theta; 0.11 psi 2021-02-13T01:11:46Z- handleDepth()-Depth_Keller msg: 1613178706632 epoch sec; seqNo:19380; depth 0.12 2021-02-13T01:11:46Z- handleDvl() - RDI_Pathfinder msg: 1613178706.65 epoch sec; seqNo:9317 2021-02-13T01:11:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.24 , 99.95 , 98.59 , 98.59 2021-02-13T01:11:46Z- handleDvl() - RDI_Pathfinder msg: velocities -0.41 , 0.07 , 0.04 2021-02-13T01:11:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178706.66 epoch sec; seqNo:19380; 4061022.37 north; 593817.89 east 2021-02-13T01:11:47Z- AHRS_M2 msg: 1613178707.03 epoch sec; seqNo:19227 2021-02-13T01:11:47Z- AHRS_M2 msg: 5.84 phi; -0.16 theta; 0.12 psi 2021-02-13T01:11:47Z- handleDepth()-Depth_Keller msg: 1613178707041 epoch sec; seqNo:19381; depth 0.12 2021-02-13T01:11:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178707.05 epoch sec; seqNo:19381; 4061022.37 north; 593817.89 east 2021-02-13T01:11:47Z- AHRS_M2 msg: 1613178707.44 epoch sec; seqNo:19228 2021-02-13T01:11:47Z- AHRS_M2 msg: 5.79 phi; -0.12 theta; 0.07 psi 2021-02-13T01:11:47Z- handleDepth()-Depth_Keller msg: 1613178707440 epoch sec; seqNo:19382; depth 0.09 2021-02-13T01:11:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178707.45 epoch sec; seqNo:19382; 4061021.99 north; 593817.89 east 2021-02-13T01:11:47Z- AHRS_M2 msg: 1613178707.84 epoch sec; seqNo:19229 2021-02-13T01:11:47Z- AHRS_M2 msg: 5.78 phi; -0.11 theta; -0.02 psi 2021-02-13T01:11:47Z- handleDepth()-Depth_Keller msg: 1613178707852 epoch sec; seqNo:19383; depth 0.07 2021-02-13T01:11:47Z- handleDvl() - RDI_Pathfinder msg: 1613178707.87 epoch sec; seqNo:9318 2021-02-13T01:11:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 107.99 , 93.12 , 91.76 , 109.35 2021-02-13T01:11:47Z- handleDvl() - RDI_Pathfinder msg: velocities -0.07 , 0.09 , 0.04 2021-02-13T01:11:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178707.87 epoch sec; seqNo:19383; 4061021.99 north; 593817.89 east 2021-02-13T01:11:48Z- AHRS_M2 msg: 1613178708.24 epoch sec; seqNo:19230 2021-02-13T01:11:48Z- AHRS_M2 msg: 5.80 phi; -0.15 theta; -0.11 psi 2021-02-13T01:11:48Z- handleDepth()-Depth_Keller msg: 1613178708249 epoch sec; seqNo:19384; depth 0.06 2021-02-13T01:11:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178708.27 epoch sec; seqNo:19384; 4061021.99 north; 593817.89 east 2021-02-13T01:11:48Z- AHRS_M2 msg: 1613178708.65 epoch sec; seqNo:19231 2021-02-13T01:11:48Z- AHRS_M2 msg: 5.80 phi; -0.19 theta; -0.09 psi 2021-02-13T01:11:48Z- handleDepth()-Depth_Keller msg: 1613178708653 epoch sec; seqNo:19385; depth 0.08 2021-02-13T01:11:48Z- handleDvl() - RDI_Pathfinder msg: 1613178708.66 epoch sec; seqNo:9319 2021-02-13T01:11:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.97 , 102.38 , 91.56 , 106.44 2021-02-13T01:11:48Z- handleDvl() - RDI_Pathfinder msg: velocities -0.33 , 0.28 , -0.10 2021-02-13T01:11:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178708.67 epoch sec; seqNo:19385; 4061021.99 north; 593817.89 east 2021-02-13T01:11:49Z- AHRS_M2 msg: 1613178709.06 epoch sec; seqNo:19232 2021-02-13T01:11:49Z- AHRS_M2 msg: 5.77 phi; -0.20 theta; -0.03 psi 2021-02-13T01:11:49Z- handleDepth()-Depth_Keller msg: 1613178709059 epoch sec; seqNo:19386; depth 0.10 2021-02-13T01:11:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178709.07 epoch sec; seqNo:19386; 4061021.99 north; 593817.89 east 2021-02-13T01:11:49Z- AHRS_M2 msg: 1613178709.46 epoch sec; seqNo:19233 2021-02-13T01:11:49Z- AHRS_M2 msg: 5.75 phi; -0.20 theta; 0.01 psi 2021-02-13T01:11:49Z- handleDepth()-Depth_Keller msg: 1613178709469 epoch sec; seqNo:19387; depth 0.11 2021-02-13T01:11:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178709.50 epoch sec; seqNo:19387; 4061022.00 north; 593819.11 east 2021-02-13T01:11:49Z- AHRS_M2 msg: 1613178709.86 epoch sec; seqNo:19234 2021-02-13T01:11:49Z- AHRS_M2 msg: 5.73 phi; -0.18 theta; 0.02 psi 2021-02-13T01:11:49Z- handleDepth()-Depth_Keller msg: 1613178709864 epoch sec; seqNo:19388; depth 0.09 2021-02-13T01:11:49Z- handleDvl() - RDI_Pathfinder msg: 1613178709.90 epoch sec; seqNo:9320 2021-02-13T01:11:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.75 , 99.81 , 88.99 , 111.98 2021-02-13T01:11:49Z- handleDvl() - RDI_Pathfinder msg: velocities -0.19 , -0.06 , -0.40 2021-02-13T01:11:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178709.91 epoch sec; seqNo:19388; 4061022.00 north; 593819.11 east 2021-02-13T01:11:50Z- AHRS_M2 msg: 1613178710.26 epoch sec; seqNo:19235 2021-02-13T01:11:50Z- AHRS_M2 msg: 5.71 phi; -0.16 theta; 0.02 psi 2021-02-13T01:11:50Z- handleDepth()-Depth_Keller msg: 1613178710274 epoch sec; seqNo:19389; depth 0.10 2021-02-13T01:11:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178710.29 epoch sec; seqNo:19389; 4061022.00 north; 593819.11 east 2021-02-13T01:11:50Z- LcmTrn::handleMessages() - handling 44 messages took 0.74 ms and 130 calls... 2021-02-13T01:11:50Z- waiting for 1613178708.03, time is 1613178710.29 2021-02-13T01:11:50Z- LcmTrn::updateTrn() >>>> pose time 1613178710.29, meas time 1613178709.90 <<<< 2021-02-13T01:11:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178709.90 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:11:50Z- TerrainNav::motionUpdate() - currEstimate = (4061022, 593819.11, 0.10) 2021-02-13T01:11:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:11:50Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:11:50Z- TNavBF::Projecting Measurements 2021-02-13T01:11:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:11:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:11:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:11:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:11:50Z- TNavFilter::4 good beams. 2021-02-13T01:11:50Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:11:50Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:11:50Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:11:50Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:11:50Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:11:50Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:11:50Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:11:50Z- TNavBF::mmse selected from filter -1 2021-02-13T01:11:50Z- TerrainMapOctree: (vehN, vehE ) = (4061022.03, 593819.02). 2021-02-13T01:11:50Z- TerrainMapOctree: Min Distance = 487.56. 2021-02-13T01:11:50Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:11:50Z- There is no specific good beam that all particles have in common. 2021-02-13T01:11:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:11:50Z- Old Depth Bias: 0.00 DepthBiasCov: 0.64 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 14.67 2021-02-13T01:11:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:11:50Z- Old Depth Bias: 0.00 DepthBiasCov: 0.62 K: 0.034 ExpectedMeasDiff: nan currMeas.covariance: 15.94 2021-02-13T01:11:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:11:50Z- Old Depth Bias: 0.00 DepthBiasCov: 0.60 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 12.67 2021-02-13T01:11:50Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:11:50Z- Old Depth Bias: 0.00 DepthBiasCov: 0.57 K: 0.026 ExpectedMeasDiff: nan currMeas.covariance: 20.06 2021-02-13T01:11:50Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:11:50Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:11:50Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:11:50Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:11:50Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:50Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:11:50Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:11:50Z- Filter 0 No beams used in subcloud update 2021-02-13T01:11:50Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:11:50Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:11:50Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 673.0 EastVariance: 833.0 DepthVariance: 0.6 this->DepthBiasCov: 0.6 2021-02-13T01:11:50Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178709.90 sec. 2021-02-13T01:11:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:11:50Z- TNavBF::mmse selected from filter -1 2021-02-13T01:11:50Z- variances([0], [2], and psi_berg) = 673.017315 832.985623 0.000000 2021-02-13T01:11:50Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:11:50Z- LcmTrn::publishEstimates() - MLE : 4061021.50 593834.61 0.10 2021-02-13T01:11:50Z- LcmTrn::publishEstimates() - MMSE : 4061022.00 593819.11 0.10 2021-02-13T01:11:50Z- LcmTrn::publishEstimates() - COVAR: 673.02 832.99 0.56 0.00 2021-02-13T01:11:50Z- AHRS_M2 msg: 1613178710.68 epoch sec; seqNo:19236 2021-02-13T01:11:50Z- AHRS_M2 msg: 5.70 phi; -0.13 theta; -0.04 psi 2021-02-13T01:11:50Z- handleDepth()-Depth_Keller msg: 1613178710682 epoch sec; seqNo:19390; depth 0.08 2021-02-13T01:11:50Z- handleDvl() - RDI_Pathfinder msg: 1613178710.69 epoch sec; seqNo:9321 2021-02-13T01:11:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.16 , 95.75 , 88.99 , 110.63 2021-02-13T01:11:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.21 , -0.03 , -0.03 2021-02-13T01:11:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178710.70 epoch sec; seqNo:19390; 4061021.99 north; 593817.89 east 2021-02-13T01:11:51Z- AHRS_M2 msg: 1613178711.08 epoch sec; seqNo:19237 2021-02-13T01:11:51Z- AHRS_M2 msg: 5.67 phi; -0.16 theta; -0.03 psi 2021-02-13T01:11:51Z- handleDepth()-Depth_Keller msg: 1613178711081 epoch sec; seqNo:19391; depth 0.07 2021-02-13T01:11:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178711.09 epoch sec; seqNo:19391; 4061021.99 north; 593817.89 east 2021-02-13T01:11:51Z- AHRS_M2 msg: 1613178711.48 epoch sec; seqNo:19238 2021-02-13T01:11:51Z- AHRS_M2 msg: 5.67 phi; -0.22 theta; 0.01 psi 2021-02-13T01:11:51Z- handleDepth()-Depth_Keller msg: 1613178711485 epoch sec; seqNo:19392; depth 0.06 2021-02-13T01:11:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178711.50 epoch sec; seqNo:19392; 4061021.99 north; 593817.89 east 2021-02-13T01:11:51Z- AHRS_M2 msg: 1613178711.88 epoch sec; seqNo:19239 2021-02-13T01:11:51Z- AHRS_M2 msg: 5.68 phi; -0.19 theta; 0.06 psi 2021-02-13T01:11:51Z- handleDepth()-Depth_Keller msg: 1613178711885 epoch sec; seqNo:19393; depth 0.09 2021-02-13T01:11:51Z- handleDvl() - RDI_Pathfinder msg: 1613178711.90 epoch sec; seqNo:9322 2021-02-13T01:11:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.75 , 95.75 , 91.70 , 106.57 2021-02-13T01:11:51Z- handleDvl() - RDI_Pathfinder msg: velocities -0.17 , -0.01 , 0.32 2021-02-13T01:11:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178711.91 epoch sec; seqNo:19393; 4061021.99 north; 593817.89 east 2021-02-13T01:11:52Z- AHRS_M2 msg: 1613178712.28 epoch sec; seqNo:19240 2021-02-13T01:11:52Z- AHRS_M2 msg: 5.66 phi; -0.06 theta; 0.03 psi 2021-02-13T01:11:52Z- handleDepth()-Depth_Keller msg: 1613178712293 epoch sec; seqNo:19394; depth 0.11 2021-02-13T01:11:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178712.31 epoch sec; seqNo:19394; 4061021.99 north; 593817.89 east 2021-02-13T01:11:52Z- AHRS_M2 msg: 1613178712.69 epoch sec; seqNo:19241 2021-02-13T01:11:52Z- AHRS_M2 msg: 5.65 phi; 0.01 theta; -0.04 psi 2021-02-13T01:11:52Z- handleDepth()-Depth_Keller msg: 1613178712692 epoch sec; seqNo:19395; depth 0.06 2021-02-13T01:11:52Z- handleDvl() - RDI_Pathfinder msg: 1613178712.71 epoch sec; seqNo:9323 2021-02-13T01:11:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 104.00 , 97.24 , 97.24 , 112.12 2021-02-13T01:11:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.12 , 0.36 , -0.45 2021-02-13T01:11:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178712.72 epoch sec; seqNo:19395; 4061022.00 north; 593819.11 east 2021-02-13T01:11:53Z- AHRS_M2 msg: 1613178713.09 epoch sec; seqNo:19242 2021-02-13T01:11:53Z- AHRS_M2 msg: 5.65 phi; -0.03 theta; -0.08 psi 2021-02-13T01:11:53Z- handleDepth()-Depth_Keller msg: 1613178713097 epoch sec; seqNo:19396; depth 0.02 2021-02-13T01:11:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178713.11 epoch sec; seqNo:19396; 4061022.00 north; 593819.11 east 2021-02-13T01:11:53Z- AHRS_M2 msg: 1613178713.50 epoch sec; seqNo:19243 2021-02-13T01:11:53Z- AHRS_M2 msg: 5.60 phi; -0.10 theta; -0.08 psi 2021-02-13T01:11:53Z- handleDepth()-Depth_Keller msg: 1613178713509 epoch sec; seqNo:19397; depth 0.01 2021-02-13T01:11:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178713.54 epoch sec; seqNo:19397; 4061022.38 north; 593819.11 east 2021-02-13T01:11:53Z- AHRS_M2 msg: 1613178713.90 epoch sec; seqNo:19244 2021-02-13T01:11:53Z- AHRS_M2 msg: 5.57 phi; -0.08 theta; -0.10 psi 2021-02-13T01:11:53Z- handleDepth()-Depth_Keller msg: 1613178713912 epoch sec; seqNo:19398; depth 0.03 2021-02-13T01:11:53Z- handleDvl() - RDI_Pathfinder msg: 1613178713.94 epoch sec; seqNo:9324 2021-02-13T01:11:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 99.41 , 96.43 , 96.43 2021-02-13T01:11:53Z- handleDvl() - RDI_Pathfinder msg: velocities -0.25 , 0.23 , 0.49 2021-02-13T01:11:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178713.95 epoch sec; seqNo:19398; 4061022.38 north; 593819.11 east 2021-02-13T01:11:54Z- AHRS_M2 msg: 1613178714.30 epoch sec; seqNo:19245 2021-02-13T01:11:54Z- AHRS_M2 msg: 5.58 phi; -0.00 theta; -0.08 psi 2021-02-13T01:11:54Z- handleDepth()-Depth_Keller msg: 1613178714308 epoch sec; seqNo:19399; depth -0.02 2021-02-13T01:11:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178714.34 epoch sec; seqNo:19399; 4061022.38 north; 593819.11 east 2021-02-13T01:11:54Z- AHRS_M2 msg: 1613178714.71 epoch sec; seqNo:19246 2021-02-13T01:11:54Z- AHRS_M2 msg: 5.57 phi; 0.05 theta; -0.03 psi 2021-02-13T01:11:54Z- handleDepth()-Depth_Keller msg: 1613178714712 epoch sec; seqNo:19400; depth 0.03 2021-02-13T01:11:54Z- handleDvl() - RDI_Pathfinder msg: 1613178714.72 epoch sec; seqNo:9325 2021-02-13T01:11:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.83 , 102.65 , 97.24 , 104.00 2021-02-13T01:11:54Z- handleDvl() - RDI_Pathfinder msg: velocities -0.18 , -0.22 , -0.17 2021-02-13T01:11:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178714.77 epoch sec; seqNo:19400; 4061022.00 north; 593819.11 east 2021-02-13T01:11:55Z- AHRS_M2 msg: 1613178715.11 epoch sec; seqNo:19247 2021-02-13T01:11:55Z- AHRS_M2 msg: 5.55 phi; 0.02 theta; -0.05 psi 2021-02-13T01:11:55Z- handleDepth()-Depth_Keller msg: 1613178715116 epoch sec; seqNo:19401; depth 0.06 2021-02-13T01:11:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178715.13 epoch sec; seqNo:19401; 4061022.00 north; 593819.11 east 2021-02-13T01:11:55Z- AHRS_M2 msg: 1613178715.52 epoch sec; seqNo:19248 2021-02-13T01:11:55Z- AHRS_M2 msg: 5.53 phi; -0.07 theta; -0.05 psi 2021-02-13T01:11:55Z- handleDepth()-Depth_Keller msg: 1613178715525 epoch sec; seqNo:19402; depth 0.05 2021-02-13T01:11:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178715.54 epoch sec; seqNo:19402; 4061022.00 north; 593819.11 east 2021-02-13T01:11:55Z- LcmTrn::handleMessages() - handling 44 messages took 0.79 ms and 129 calls... 2021-02-13T01:11:55Z- waiting for 1613178713.29, time is 1613178715.54 2021-02-13T01:11:55Z- LcmTrn::updateTrn() >>>> pose time 1613178715.54, meas time 1613178714.72 <<<< 2021-02-13T01:11:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178714.72 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:11:55Z- TerrainNav::motionUpdate() - currEstimate = (4061022, 593819.11, 0.05) 2021-02-13T01:11:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:11:55Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:11:55Z- TNavBF::Projecting Measurements 2021-02-13T01:11:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:11:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:11:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:11:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:11:55Z- TNavFilter::4 good beams. 2021-02-13T01:11:55Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:11:55Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:11:55Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:11:55Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:11:55Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:11:55Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:11:55Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:11:55Z- TNavBF::mmse selected from filter -1 2021-02-13T01:11:55Z- TerrainMapOctree: (vehN, vehE ) = (4061022.00, 593819.11). 2021-02-13T01:11:55Z- TerrainMapOctree: Min Distance = 487.49. 2021-02-13T01:11:55Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:11:55Z- There is no specific good beam that all particles have in common. 2021-02-13T01:11:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:11:55Z- Old Depth Bias: 0.00 DepthBiasCov: 0.56 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 13.49 2021-02-13T01:11:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:11:55Z- Old Depth Bias: 0.00 DepthBiasCov: 0.54 K: 0.028 ExpectedMeasDiff: nan currMeas.covariance: 16.86 2021-02-13T01:11:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:11:55Z- Old Depth Bias: 0.00 DepthBiasCov: 0.53 K: 0.030 ExpectedMeasDiff: nan currMeas.covariance: 15.13 2021-02-13T01:11:55Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:11:55Z- Old Depth Bias: 0.00 DepthBiasCov: 0.51 K: 0.026 ExpectedMeasDiff: nan currMeas.covariance: 17.31 2021-02-13T01:11:55Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:11:55Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:11:55Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:11:55Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:11:55Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:11:55Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:11:55Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:11:55Z- Filter 0 No beams used in subcloud update 2021-02-13T01:11:55Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:11:55Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:11:55Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: 0.0 NorthVariance: 673.0 EastVariance: 833.0 DepthVariance: 0.5 this->DepthBiasCov: 0.5 2021-02-13T01:11:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178714.72 sec. 2021-02-13T01:11:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:11:55Z- TNavBF::mmse selected from filter -1 2021-02-13T01:11:55Z- variances([0], [2], and psi_berg) = 673.017315 832.985623 0.000000 2021-02-13T01:11:55Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:11:55Z- LcmTrn::publishEstimates() - MLE : 4061021.50 593834.61 0.05 2021-02-13T01:11:55Z- LcmTrn::publishEstimates() - MMSE : 4061022.00 593819.11 0.05 2021-02-13T01:11:55Z- LcmTrn::publishEstimates() - COVAR: 673.02 832.99 0.50 0.00 2021-02-13T01:11:55Z- AHRS_M2 msg: 1613178715.92 epoch sec; seqNo:19249 2021-02-13T01:11:55Z- AHRS_M2 msg: 5.56 phi; -0.11 theta; -0.02 psi 2021-02-13T01:11:55Z- handleDepth()-Depth_Keller msg: 1613178715929 epoch sec; seqNo:19403; depth 0.06 2021-02-13T01:11:55Z- handleDvl() - RDI_Pathfinder msg: 1613178715.95 epoch sec; seqNo:9326 2021-02-13T01:11:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.75 , 95.75 , 99.81 , 95.75 2021-02-13T01:11:55Z- handleDvl() - RDI_Pathfinder msg: velocities -0.22 , -0.25 , -0.07 2021-02-13T01:11:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178715.96 epoch sec; seqNo:19403; 4061022.00 north; 593819.11 east 2021-02-13T01:11:56Z- AHRS_M2 msg: 1613178716.32 epoch sec; seqNo:19250 2021-02-13T01:11:56Z- AHRS_M2 msg: 5.58 phi; -0.01 theta; 0.05 psi 2021-02-13T01:11:56Z- handleDepth()-Depth_Keller msg: 1613178716328 epoch sec; seqNo:19404; depth 0.09 2021-02-13T01:11:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178716.34 epoch sec; seqNo:19404; 4061021.62 north; 593819.11 east 2021-02-13T01:11:56Z- AHRS_M2 msg: 1613178716.73 epoch sec; seqNo:19251 2021-02-13T01:11:56Z- AHRS_M2 msg: 5.60 phi; 0.13 theta; 0.11 psi 2021-02-13T01:11:56Z- handleDepth()-Depth_Keller msg: 1613178716732 epoch sec; seqNo:19405; depth 0.11 2021-02-13T01:11:56Z- handleDvl() - RDI_Pathfinder msg: 1613178716.74 epoch sec; seqNo:9327 2021-02-13T01:11:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.31 , 95.96 , 91.90 , 104.07 2021-02-13T01:11:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.50 , -0.21 , -0.38 2021-02-13T01:11:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178716.76 epoch sec; seqNo:19405; 4061021.62 north; 593819.11 east 2021-02-13T01:11:57Z- AHRS_M2 msg: 1613178717.14 epoch sec; seqNo:19252 2021-02-13T01:11:57Z- AHRS_M2 msg: 5.60 phi; 0.15 theta; 0.10 psi 2021-02-13T01:11:57Z- handleDepth()-Depth_Keller msg: 1613178717142 epoch sec; seqNo:19406; depth 0.08 2021-02-13T01:11:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178717.16 epoch sec; seqNo:19406; 4061022.00 north; 593819.11 east 2021-02-13T01:11:57Z- AHRS_M2 msg: 1613178717.56 epoch sec; seqNo:19253 2021-02-13T01:11:57Z- AHRS_M2 msg: 5.57 phi; 0.02 theta; 0.01 psi 2021-02-13T01:11:57Z- handleDepth()-Depth_Keller msg: 1613178717569 epoch sec; seqNo:19407; depth 0.03 2021-02-13T01:11:57Z- handleDvl() - RDI_Pathfinder msg: 1613178717.58 epoch sec; seqNo:9328 2021-02-13T01:11:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 106.71 , 97.24 , 109.41 , 97.24 2021-02-13T01:11:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.29 , -0.03 , 1.01 2021-02-13T01:11:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178717.59 epoch sec; seqNo:19407; 4061021.99 north; 593817.89 east 2021-02-13T01:11:57Z- AHRS_M2 msg: 1613178717.94 epoch sec; seqNo:19254 2021-02-13T01:11:57Z- AHRS_M2 msg: 5.56 phi; -0.07 theta; -0.09 psi 2021-02-13T01:11:57Z- handleDepth()-Depth_Keller msg: 1613178717949 epoch sec; seqNo:19408; depth 0.05 2021-02-13T01:11:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178717.96 epoch sec; seqNo:19408; 4061021.99 north; 593817.89 east 2021-02-13T01:11:58Z- AHRS_M2 msg: 1613178718.35 epoch sec; seqNo:19255 2021-02-13T01:11:58Z- AHRS_M2 msg: 5.57 phi; -0.01 theta; -0.14 psi 2021-02-13T01:11:58Z- handleDepth()-Depth_Keller msg: 1613178718349 epoch sec; seqNo:19409; depth 0.04 2021-02-13T01:11:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178718.36 epoch sec; seqNo:19409; 4061021.99 north; 593817.89 east 2021-02-13T01:11:58Z- AHRS_M2 msg: 1613178718.75 epoch sec; seqNo:19256 2021-02-13T01:11:58Z- AHRS_M2 msg: 5.58 phi; 0.04 theta; -0.12 psi 2021-02-13T01:11:58Z- handleDepth()-Depth_Keller msg: 1613178718758 epoch sec; seqNo:19410; depth 0.02 2021-02-13T01:11:58Z- handleDvl() - RDI_Pathfinder msg: 1613178718.77 epoch sec; seqNo:9329 2021-02-13T01:11:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.46 , 102.38 , 97.92 , 102.38 2021-02-13T01:11:58Z- handleDvl() - RDI_Pathfinder msg: velocities -0.58 , 0.15 , 0.02 2021-02-13T01:11:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178718.78 epoch sec; seqNo:19410; 4061022.00 north; 593819.11 east 2021-02-13T01:11:59Z- AHRS_M2 msg: 1613178719.15 epoch sec; seqNo:19257 2021-02-13T01:11:59Z- AHRS_M2 msg: 5.61 phi; -0.08 theta; -0.00 psi 2021-02-13T01:11:59Z- handleDepth()-Depth_Keller msg: 1613178719158 epoch sec; seqNo:19411; depth -0.00 2021-02-13T01:11:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178719.18 epoch sec; seqNo:19411; 4061022.00 north; 593819.11 east 2021-02-13T01:11:59Z- AHRS_M2 msg: 1613178719.58 epoch sec; seqNo:19258 2021-02-13T01:11:59Z- AHRS_M2 msg: 5.59 phi; -0.21 theta; 0.13 psi 2021-02-13T01:11:59Z- handleDepth()-Depth_Keller msg: 1613178719586 epoch sec; seqNo:19412; depth 0.04 2021-02-13T01:11:59Z- handleDvl() - RDI_Pathfinder msg: 1613178719.60 epoch sec; seqNo:9330 2021-02-13T01:11:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 99.81 , 98.46 , 98.46 2021-02-13T01:11:59Z- handleDvl() - RDI_Pathfinder msg: velocities -0.90 , 0.50 , -0.10 2021-02-13T01:11:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178719.61 epoch sec; seqNo:19412; 4061021.62 north; 593819.11 east 2021-02-13T01:11:59Z- AHRS_M2 msg: 1613178719.96 epoch sec; seqNo:19259 2021-02-13T01:11:59Z- AHRS_M2 msg: 5.54 phi; -0.13 theta; 0.20 psi 2021-02-13T01:11:59Z- handleDepth()-Depth_Keller msg: 1613178719965 epoch sec; seqNo:19413; depth 0.08 2021-02-13T01:11:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178719.98 epoch sec; seqNo:19413; 4061021.62 north; 593819.11 east 2021-02-13T01:12:00Z- AHRS_M2 msg: 1613178720.37 epoch sec; seqNo:19260 2021-02-13T01:12:00Z- AHRS_M2 msg: 5.48 phi; 0.03 theta; 0.11 psi 2021-02-13T01:12:00Z- handleDepth()-Depth_Keller msg: 1613178720372 epoch sec; seqNo:19414; depth 0.10 2021-02-13T01:12:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178720.38 epoch sec; seqNo:19414; 4061021.64 north; 593820.34 east 2021-02-13T01:12:00Z- LcmTrn::handleMessages() - handling 41 messages took 0.66 ms and 128 calls... 2021-02-13T01:12:00Z- waiting for 1613178718.54, time is 1613178720.38 2021-02-13T01:12:00Z- LcmTrn::updateTrn() >>>> pose time 1613178720.38, meas time 1613178719.60 <<<< 2021-02-13T01:12:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178719.60 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:12:00Z- TerrainNav::motionUpdate() - currEstimate = (4061022, 593820.34, 0.10) 2021-02-13T01:12:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:12:00Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:12:00Z- TNavBF::Projecting Measurements 2021-02-13T01:12:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:12:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:12:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:12:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:12:00Z- TNavFilter::4 good beams. 2021-02-13T01:12:00Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:12:00Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:12:00Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:12:00Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:12:00Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:12:00Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:12:00Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:12:00Z- TNavBF::mmse selected from filter -1 2021-02-13T01:12:00Z- TerrainMapOctree: (vehN, vehE ) = (4061021.70, 593820.14). 2021-02-13T01:12:00Z- TerrainMapOctree: Min Distance = 486.73. 2021-02-13T01:12:00Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:12:00Z- There is no specific good beam that all particles have in common. 2021-02-13T01:12:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:12:00Z- Old Depth Bias: 0.00 DepthBiasCov: 0.55 K: 0.032 ExpectedMeasDiff: nan currMeas.covariance: 15.09 2021-02-13T01:12:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:12:00Z- Old Depth Bias: 0.00 DepthBiasCov: 0.54 K: 0.029 ExpectedMeasDiff: nan currMeas.covariance: 15.94 2021-02-13T01:12:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:12:00Z- Old Depth Bias: 0.00 DepthBiasCov: 0.52 K: 0.029 ExpectedMeasDiff: nan currMeas.covariance: 15.51 2021-02-13T01:12:00Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:12:00Z- Old Depth Bias: 0.00 DepthBiasCov: 0.50 K: 0.028 ExpectedMeasDiff: nan currMeas.covariance: 15.51 2021-02-13T01:12:00Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:12:00Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:12:00Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:12:00Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:12:00Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:00Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:12:00Z- Filter 0 SubcloudNIS: 0.000000 2021-02-13T01:12:00Z- Filter 0 No beams used in subcloud update 2021-02-13T01:12:00Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:12:00Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:12:00Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -0.0 NorthVariance: 673.0 EastVariance: 833.0 DepthVariance: 0.5 this->DepthBiasCov: 0.5 2021-02-13T01:12:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178719.60 sec. 2021-02-13T01:12:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:12:00Z- TNavBF::mmse selected from filter -1 2021-02-13T01:12:00Z- variances([0], [2], and psi_berg) = 672.986883 832.985611 0.000000 2021-02-13T01:12:00Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:12:00Z- LcmTrn::publishEstimates() - MLE : 4061021.14 593835.84 0.10 2021-02-13T01:12:00Z- LcmTrn::publishEstimates() - MMSE : 4061021.64 593820.34 0.10 2021-02-13T01:12:00Z- LcmTrn::publishEstimates() - COVAR: 672.99 832.99 0.50 0.00 2021-02-13T01:12:00Z- AHRS_M2 msg: 1613178720.77 epoch sec; seqNo:19261 2021-02-13T01:12:00Z- AHRS_M2 msg: 5.43 phi; 0.08 theta; -0.05 psi 2021-02-13T01:12:00Z- handleDepth()-Depth_Keller msg: 1613178720776 epoch sec; seqNo:19415; depth 0.11 2021-02-13T01:12:00Z- handleDvl() - RDI_Pathfinder msg: 1613178720.80 epoch sec; seqNo:9331 2021-02-13T01:12:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 113.40 , 97.17 , 97.17 , 107.99 2021-02-13T01:12:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.41 , 0.40 , -0.34 2021-02-13T01:12:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178720.81 epoch sec; seqNo:19415; 4061021.64 north; 593820.34 east 2021-02-13T01:12:01Z- AHRS_M2 msg: 1613178721.17 epoch sec; seqNo:19262 2021-02-13T01:12:01Z- AHRS_M2 msg: 5.40 phi; 0.05 theta; -0.09 psi 2021-02-13T01:12:01Z- handleDepth()-Depth_Keller msg: 1613178721181 epoch sec; seqNo:19416; depth 0.13 2021-02-13T01:12:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178721.19 epoch sec; seqNo:19416; 4061022.02 north; 593820.33 east 2021-02-13T01:12:01Z- AHRS_M2 msg: 1613178721.58 epoch sec; seqNo:19263 2021-02-13T01:12:01Z- AHRS_M2 msg: 5.43 phi; 0.00 theta; -0.12 psi 2021-02-13T01:12:01Z- handleDepth()-Depth_Keller msg: 1613178721585 epoch sec; seqNo:19417; depth 0.12 2021-02-13T01:12:01Z- handleDvl() - RDI_Pathfinder msg: 1613178721.60 epoch sec; seqNo:9332 2021-02-13T01:12:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.81 , 102.25 , 100.76 , 94.81 2021-02-13T01:12:01Z- handleDvl() - RDI_Pathfinder msg: velocities -0.09 , -0.16 , 0.12 2021-02-13T01:12:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178721.61 epoch sec; seqNo:19417; 4061022.02 north; 593820.33 east 2021-02-13T01:12:01Z- AHRS_M2 msg: 1613178721.98 epoch sec; seqNo:19264 2021-02-13T01:12:01Z- AHRS_M2 msg: 5.47 phi; -0.02 theta; -0.09 psi 2021-02-13T01:12:01Z- handleDepth()-Depth_Keller msg: 1613178721993 epoch sec; seqNo:19418; depth 0.12 2021-02-13T01:12:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178722.00 epoch sec; seqNo:19418; 4061021.64 north; 593820.34 east 2021-02-13T01:12:02Z- AHRS_M2 msg: 1613178722.38 epoch sec; seqNo:19265 2021-02-13T01:12:02Z- AHRS_M2 msg: 5.44 phi; -0.00 theta; 0.02 psi 2021-02-13T01:12:02Z- handleDepth()-Depth_Keller msg: 1613178722399 epoch sec; seqNo:19419; depth 0.16 2021-02-13T01:12:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178722.43 epoch sec; seqNo:19419; 4061021.64 north; 593820.34 east 2021-02-13T01:12:02Z- AHRS_M2 msg: 1613178722.79 epoch sec; seqNo:19266 2021-02-13T01:12:02Z- AHRS_M2 msg: 5.41 phi; 0.04 theta; 0.06 psi 2021-02-13T01:12:02Z- handleDepth()-Depth_Keller msg: 1613178722792 epoch sec; seqNo:19420; depth 0.13 2021-02-13T01:12:02Z- handleDvl() - RDI_Pathfinder msg: 1613178722.83 epoch sec; seqNo:9333 2021-02-13T01:12:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.17 , 101.23 , 97.17 , 98.53 2021-02-13T01:12:02Z- handleDvl() - RDI_Pathfinder msg: velocities -0.05 , -0.04 , -0.10 2021-02-13T01:12:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178722.84 epoch sec; seqNo:19420; 4061021.64 north; 593820.34 east 2021-02-13T01:12:03Z- AHRS_M2 msg: 1613178723.19 epoch sec; seqNo:19267 2021-02-13T01:12:03Z- AHRS_M2 msg: 5.40 phi; 0.05 theta; 0.01 psi 2021-02-13T01:12:03Z- handleDepth()-Depth_Keller msg: 1613178723197 epoch sec; seqNo:19421; depth 0.12 2021-02-13T01:12:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178723.21 epoch sec; seqNo:19421; 4061021.64 north; 593820.34 east 2021-02-13T01:12:03Z- AHRS_M2 msg: 1613178723.60 epoch sec; seqNo:19268 2021-02-13T01:12:03Z- AHRS_M2 msg: 5.42 phi; 0.01 theta; -0.05 psi 2021-02-13T01:12:03Z- handleDepth()-Depth_Keller msg: 1613178723602 epoch sec; seqNo:19422; depth 0.08 2021-02-13T01:12:03Z- handleDvl() - RDI_Pathfinder msg: 1613178723.61 epoch sec; seqNo:9334 2021-02-13T01:12:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.89 , 95.83 , 99.89 , 97.18 2021-02-13T01:12:03Z- handleDvl() - RDI_Pathfinder msg: velocities -0.28 , -0.27 , 0.49 2021-02-13T01:12:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178723.62 epoch sec; seqNo:19422; 4061021.64 north; 593820.34 east 2021-02-13T01:12:04Z- AHRS_M2 msg: 1613178724.00 epoch sec; seqNo:19269 2021-02-13T01:12:04Z- AHRS_M2 msg: 5.43 phi; -0.08 theta; -0.05 psi 2021-02-13T01:12:04Z- handleDepth()-Depth_Keller msg: 1613178724008 epoch sec; seqNo:19423; depth 0.06 2021-02-13T01:12:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178724.06 epoch sec; seqNo:19423; 4061021.26 north; 593820.34 east 2021-02-13T01:12:04Z- AHRS_M2 msg: 1613178724.40 epoch sec; seqNo:19270 2021-02-13T01:12:04Z- AHRS_M2 msg: 5.43 phi; -0.15 theta; 0.00 psi 2021-02-13T01:12:04Z- handleDepth()-Depth_Keller msg: 1613178724413 epoch sec; seqNo:19424; depth 0.07 2021-02-13T01:12:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178724.45 epoch sec; seqNo:19424; 4061021.26 north; 593820.34 east 2021-02-13T01:12:04Z- AHRS_M2 msg: 1613178724.82 epoch sec; seqNo:19271 2021-02-13T01:12:04Z- AHRS_M2 msg: 5.41 phi; -0.22 theta; 0.08 psi 2021-02-13T01:12:04Z- handleDepth()-Depth_Keller msg: 1613178724824 epoch sec; seqNo:19425; depth 0.05 2021-02-13T01:12:04Z- handleDvl() - RDI_Pathfinder msg: 1613178724.84 epoch sec; seqNo:9335 2021-02-13T01:12:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.83 , 95.83 , 93.12 , 105.29 2021-02-13T01:12:04Z- handleDvl() - RDI_Pathfinder msg: velocities -0.95 , 0.21 , -0.11 2021-02-13T01:12:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178724.85 epoch sec; seqNo:19425; 4061020.88 north; 593820.34 east 2021-02-13T01:12:05Z- AHRS_M2 msg: 1613178725.21 epoch sec; seqNo:19272 2021-02-13T01:12:05Z- AHRS_M2 msg: 5.35 phi; -0.28 theta; 0.07 psi 2021-02-13T01:12:05Z- handleDepth()-Depth_Keller msg: 1613178725216 epoch sec; seqNo:19426; depth 0.06 2021-02-13T01:12:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178725.24 epoch sec; seqNo:19426; 4061020.88 north; 593820.34 east 2021-02-13T01:12:05Z- AHRS_M2 msg: 1613178725.62 epoch sec; seqNo:19273 2021-02-13T01:12:05Z- AHRS_M2 msg: 5.32 phi; -0.21 theta; -0.00 psi 2021-02-13T01:12:05Z- handleDepth()-Depth_Keller msg: 1613178725621 epoch sec; seqNo:19427; depth 0.04 2021-02-13T01:12:05Z- handleDvl() - RDI_Pathfinder msg: 1613178725.63 epoch sec; seqNo:9336 2021-02-13T01:12:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 108.06 , 97.24 , 87.78 , 118.88 2021-02-13T01:12:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.01 , 0.19 , -1.23 2021-02-13T01:12:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178725.64 epoch sec; seqNo:19427; 4061020.88 north; 593820.34 east 2021-02-13T01:12:05Z- LcmTrn::handleMessages() - handling 45 messages took 0.64 ms and 133 calls... 2021-02-13T01:12:05Z- waiting for 1613178723.38, time is 1613178725.64 2021-02-13T01:12:05Z- LcmTrn::updateTrn() equating pose 1613178725.64 and meas times 1613178725.64 2021-02-13T01:12:05Z- LcmTrn::updateTrn() >>>> pose time 1613178725.64, meas time 1613178725.64 <<<< 2021-02-13T01:12:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178725.64 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:12:05Z- TerrainNav::motionUpdate() - currEstimate = (4061021, 593820.34, 0.04) 2021-02-13T01:12:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:12:05Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:12:05Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178725.64 sec.; No relevant navigation data available 2021-02-13T01:12:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:12:05Z- TNavBF::mmse selected from filter -1 2021-02-13T01:12:05Z- variances([0], [2], and psi_berg) = 672.969108 832.985604 0.000000 2021-02-13T01:12:05Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:12:05Z- LcmTrn::publishEstimates() - MLE : 4061020.38 593835.84 0.04 2021-02-13T01:12:05Z- LcmTrn::publishEstimates() - MMSE : 4061020.88 593820.34 0.04 2021-02-13T01:12:05Z- LcmTrn::publishEstimates() - COVAR: 672.97 832.99 0.54 0.00 2021-02-13T01:12:06Z- AHRS_M2 msg: 1613178726.02 epoch sec; seqNo:19274 2021-02-13T01:12:06Z- AHRS_M2 msg: 5.31 phi; -0.12 theta; -0.10 psi 2021-02-13T01:12:06Z- handleDepth()-Depth_Keller msg: 1613178726029 epoch sec; seqNo:19428; depth 0.03 2021-02-13T01:12:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178726.04 epoch sec; seqNo:19428; 4061021.26 north; 593820.34 east 2021-02-13T01:12:06Z- AHRS_M2 msg: 1613178726.42 epoch sec; seqNo:19275 2021-02-13T01:12:06Z- AHRS_M2 msg: 5.30 phi; -0.05 theta; -0.17 psi 2021-02-13T01:12:06Z- handleDepth()-Depth_Keller msg: 1613178726430 epoch sec; seqNo:19429; depth 0.05 2021-02-13T01:12:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178726.44 epoch sec; seqNo:19429; 4061021.26 north; 593820.34 east 2021-02-13T01:12:06Z- AHRS_M2 msg: 1613178726.83 epoch sec; seqNo:19276 2021-02-13T01:12:06Z- AHRS_M2 msg: 5.27 phi; 0.07 theta; -0.12 psi 2021-02-13T01:12:06Z- handleDepth()-Depth_Keller msg: 1613178726833 epoch sec; seqNo:19430; depth 0.09 2021-02-13T01:12:06Z- handleDvl() - RDI_Pathfinder msg: 1613178726.86 epoch sec; seqNo:9337 2021-02-13T01:12:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 103.80 , 97.85 , 105.29 2021-02-13T01:12:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.19 , -0.17 2021-02-13T01:12:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178726.87 epoch sec; seqNo:19430; 4061021.26 north; 593820.34 east 2021-02-13T01:12:07Z- AHRS_M2 msg: 1613178727.23 epoch sec; seqNo:19277 2021-02-13T01:12:07Z- AHRS_M2 msg: 5.27 phi; 0.12 theta; -0.03 psi 2021-02-13T01:12:07Z- handleDepth()-Depth_Keller msg: 1613178727241 epoch sec; seqNo:19431; depth 0.10 2021-02-13T01:12:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178727.26 epoch sec; seqNo:19431; 4061021.26 north; 593820.34 east 2021-02-13T01:12:07Z- AHRS_M2 msg: 1613178727.64 epoch sec; seqNo:19278 2021-02-13T01:12:07Z- AHRS_M2 msg: 5.25 phi; 0.17 theta; -0.04 psi 2021-02-13T01:12:07Z- handleDepth()-Depth_Keller msg: 1613178727645 epoch sec; seqNo:19432; depth 0.08 2021-02-13T01:12:07Z- handleDvl() - RDI_Pathfinder msg: 1613178727.66 epoch sec; seqNo:9338 2021-02-13T01:12:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.01 , 99.47 , 105.42 , 90.55 2021-02-13T01:12:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.58 , -0.01 , 0.86 2021-02-13T01:12:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178727.67 epoch sec; seqNo:19432; 4061021.62 north; 593819.11 east 2021-02-13T01:12:08Z- AHRS_M2 msg: 1613178728.05 epoch sec; seqNo:19279 2021-02-13T01:12:08Z- AHRS_M2 msg: 5.24 phi; 0.08 theta; -0.01 psi 2021-02-13T01:12:08Z- handleDepth()-Depth_Keller msg: 1613178728051 epoch sec; seqNo:19433; depth 0.06 2021-02-13T01:12:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178728.08 epoch sec; seqNo:19433; 4061021.62 north; 593819.11 east 2021-02-13T01:12:08Z- AHRS_M2 msg: 1613178728.44 epoch sec; seqNo:19280 2021-02-13T01:12:08Z- AHRS_M2 msg: 5.19 phi; 0.08 theta; -0.01 psi 2021-02-13T01:12:08Z- handleDepth()-Depth_Keller msg: 1613178728453 epoch sec; seqNo:19434; depth 0.07 2021-02-13T01:12:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178728.48 epoch sec; seqNo:19434; 4061021.62 north; 593819.11 east 2021-02-13T01:12:08Z- AHRS_M2 msg: 1613178728.85 epoch sec; seqNo:19281 2021-02-13T01:12:08Z- AHRS_M2 msg: 5.14 phi; -0.01 theta; -0.08 psi 2021-02-13T01:12:08Z- handleDepth()-Depth_Keller msg: 1613178728853 epoch sec; seqNo:19435; depth 0.06 2021-02-13T01:12:08Z- handleDvl() - RDI_Pathfinder msg: 1613178728.87 epoch sec; seqNo:9339 2021-02-13T01:12:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.89 , 95.89 , 108.06 , 97.24 2021-02-13T01:12:08Z- handleDvl() - RDI_Pathfinder msg: velocities -0.44 , 0.23 , 0.60 2021-02-13T01:12:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178728.88 epoch sec; seqNo:19435; 4061021.62 north; 593819.11 east 2021-02-13T01:12:09Z- AHRS_M2 msg: 1613178729.25 epoch sec; seqNo:19282 2021-02-13T01:12:09Z- AHRS_M2 msg: 5.15 phi; -0.20 theta; -0.17 psi 2021-02-13T01:12:09Z- handleDepth()-Depth_Keller msg: 1613178729258 epoch sec; seqNo:19436; depth 0.01 2021-02-13T01:12:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178729.27 epoch sec; seqNo:19436; 4061021.62 north; 593819.11 east 2021-02-13T01:12:09Z- AHRS_M2 msg: 1613178729.66 epoch sec; seqNo:19283 2021-02-13T01:12:09Z- AHRS_M2 msg: 5.16 phi; -0.26 theta; -0.15 psi 2021-02-13T01:12:09Z- handleDepth()-Depth_Keller msg: 1613178729660 epoch sec; seqNo:19437; depth -0.01 2021-02-13T01:12:09Z- handleDvl() - RDI_Pathfinder msg: 1613178729.67 epoch sec; seqNo:9340 2021-02-13T01:12:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 99.82 , 98.46 , 97.11 2021-02-13T01:12:09Z- handleDvl() - RDI_Pathfinder msg: velocities -1.06 , -0.41 , -0.10 2021-02-13T01:12:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178729.69 epoch sec; seqNo:19437; 4061021.26 north; 593820.34 east 2021-02-13T01:12:10Z- AHRS_M2 msg: 1613178730.06 epoch sec; seqNo:19284 2021-02-13T01:12:10Z- AHRS_M2 msg: 5.20 phi; -0.06 theta; -0.07 psi 2021-02-13T01:12:10Z- handleDepth()-Depth_Keller msg: 1613178730064 epoch sec; seqNo:19438; depth 0.02 2021-02-13T01:12:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178730.08 epoch sec; seqNo:19438; 4061021.26 north; 593820.34 east 2021-02-13T01:12:10Z- AHRS_M2 msg: 1613178730.47 epoch sec; seqNo:19285 2021-02-13T01:12:10Z- AHRS_M2 msg: 5.22 phi; 0.06 theta; 0.05 psi 2021-02-13T01:12:10Z- handleDepth()-Depth_Keller msg: 1613178730470 epoch sec; seqNo:19439; depth 0.06 2021-02-13T01:12:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178730.48 epoch sec; seqNo:19439; 4061020.88 north; 593820.34 east 2021-02-13T01:12:10Z- AHRS_M2 msg: 1613178730.87 epoch sec; seqNo:19286 2021-02-13T01:12:10Z- AHRS_M2 msg: 5.20 phi; 0.02 theta; 0.08 psi 2021-02-13T01:12:10Z- handleDepth()-Depth_Keller msg: 1613178730873 epoch sec; seqNo:19440; depth 0.14 2021-02-13T01:12:10Z- handleDvl() - RDI_Pathfinder msg: 1613178730.89 epoch sec; seqNo:9341 2021-02-13T01:12:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.18 , 106.65 , 97.18 , 114.76 2021-02-13T01:12:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.52 , -0.10 , -0.29 2021-02-13T01:12:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178730.90 epoch sec; seqNo:19440; 4061020.88 north; 593820.34 east 2021-02-13T01:12:10Z- LcmTrn::handleMessages() - handling 44 messages took 0.67 ms and 131 calls... 2021-02-13T01:12:10Z- waiting for 1613178728.64, time is 1613178730.90 2021-02-13T01:12:10Z- LcmTrn::updateTrn() equating pose 1613178730.90 and meas times 1613178730.90 2021-02-13T01:12:10Z- LcmTrn::updateTrn() >>>> pose time 1613178730.90, meas time 1613178730.90 <<<< 2021-02-13T01:12:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178730.90 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:12:10Z- TerrainNav::motionUpdate() - currEstimate = (4061021, 593820.34, 0.14) 2021-02-13T01:12:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:12:10Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:12:10Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178730.90 sec.; No relevant navigation data available 2021-02-13T01:12:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:12:10Z- TNavBF::mmse selected from filter -1 2021-02-13T01:12:10Z- variances([0], [2], and psi_berg) = 672.969108 832.985604 0.000000 2021-02-13T01:12:10Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:12:10Z- LcmTrn::publishEstimates() - MLE : 4061020.38 593835.84 0.14 2021-02-13T01:12:10Z- LcmTrn::publishEstimates() - MMSE : 4061020.88 593820.34 0.14 2021-02-13T01:12:10Z- LcmTrn::publishEstimates() - COVAR: 672.97 832.99 0.54 0.00 2021-02-13T01:12:11Z- AHRS_M2 msg: 1613178731.27 epoch sec; seqNo:19287 2021-02-13T01:12:11Z- AHRS_M2 msg: 5.17 phi; -0.02 theta; 0.01 psi 2021-02-13T01:12:11Z- handleDepth()-Depth_Keller msg: 1613178731281 epoch sec; seqNo:19441; depth 0.21 2021-02-13T01:12:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178731.29 epoch sec; seqNo:19441; 4061020.88 north; 593820.34 east 2021-02-13T01:12:11Z- AHRS_M2 msg: 1613178731.68 epoch sec; seqNo:19288 2021-02-13T01:12:11Z- AHRS_M2 msg: 5.14 phi; -0.05 theta; -0.13 psi 2021-02-13T01:12:11Z- handleDepth()-Depth_Keller msg: 1613178731683 epoch sec; seqNo:19442; depth 0.23 2021-02-13T01:12:11Z- handleDvl() - RDI_Pathfinder msg: 1613178731.70 epoch sec; seqNo:9342 2021-02-13T01:12:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.74 , 94.81 , 99.28 , 97.79 2021-02-13T01:12:11Z- handleDvl() - RDI_Pathfinder msg: velocities -0.14 , 0.26 , 0.22 2021-02-13T01:12:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178731.71 epoch sec; seqNo:19442; 4061020.88 north; 593820.34 east 2021-02-13T01:12:12Z- AHRS_M2 msg: 1613178732.08 epoch sec; seqNo:19289 2021-02-13T01:12:12Z- AHRS_M2 msg: 5.09 phi; -0.09 theta; -0.26 psi 2021-02-13T01:12:12Z- handleDepth()-Depth_Keller msg: 1613178732089 epoch sec; seqNo:19443; depth 0.24 2021-02-13T01:12:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178732.11 epoch sec; seqNo:19443; 4061020.88 north; 593820.34 east 2021-02-13T01:12:12Z- AHRS_M2 msg: 1613178732.48 epoch sec; seqNo:19290 2021-02-13T01:12:12Z- AHRS_M2 msg: 5.07 phi; -0.14 theta; -0.27 psi 2021-02-13T01:12:12Z- handleDepth()-Depth_Keller msg: 1613178732493 epoch sec; seqNo:19444; depth 0.23 2021-02-13T01:12:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178732.53 epoch sec; seqNo:19444; 4061021.26 north; 593820.34 east 2021-02-13T01:12:12Z- AHRS_M2 msg: 1613178732.89 epoch sec; seqNo:19291 2021-02-13T01:12:12Z- AHRS_M2 msg: 5.11 phi; -0.19 theta; -0.15 psi 2021-02-13T01:12:12Z- handleDepth()-Depth_Keller msg: 1613178732893 epoch sec; seqNo:19445; depth 0.26 2021-02-13T01:12:12Z- handleDvl() - RDI_Pathfinder msg: 1613178732.92 epoch sec; seqNo:9343 2021-02-13T01:12:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 113.34 , 97.11 , 103.88 2021-02-13T01:12:12Z- handleDvl() - RDI_Pathfinder msg: velocities -0.53 , -0.37 , 0.12 2021-02-13T01:12:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178732.93 epoch sec; seqNo:19445; 4061020.88 north; 593820.34 east 2021-02-13T01:12:13Z- AHRS_M2 msg: 1613178733.29 epoch sec; seqNo:19292 2021-02-13T01:12:13Z- AHRS_M2 msg: 5.14 phi; -0.20 theta; 0.01 psi 2021-02-13T01:12:13Z- handleDepth()-Depth_Keller msg: 1613178733297 epoch sec; seqNo:19446; depth 0.27 2021-02-13T01:12:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178733.33 epoch sec; seqNo:19446; 4061020.88 north; 593820.34 east 2021-02-13T01:12:13Z- AHRS_M2 msg: 1613178733.70 epoch sec; seqNo:19293 2021-02-13T01:12:13Z- AHRS_M2 msg: 5.14 phi; -0.22 theta; 0.11 psi 2021-02-13T01:12:13Z- handleDepth()-Depth_Keller msg: 1613178733700 epoch sec; seqNo:19447; depth 0.29 2021-02-13T01:12:13Z- handleDvl() - RDI_Pathfinder msg: 1613178733.73 epoch sec; seqNo:9344 2021-02-13T01:12:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.85 , 112.73 , 97.85 , 112.73 2021-02-13T01:12:13Z- handleDvl() - RDI_Pathfinder msg: velocities -0.31 , -0.12 , -0.67 2021-02-13T01:12:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178733.74 epoch sec; seqNo:19447; 4061020.50 north; 593820.35 east 2021-02-13T01:12:14Z- AHRS_M2 msg: 1613178734.10 epoch sec; seqNo:19294 2021-02-13T01:12:14Z- AHRS_M2 msg: 5.15 phi; -0.24 theta; 0.07 psi 2021-02-13T01:12:14Z- handleDepth()-Depth_Keller msg: 1613178734109 epoch sec; seqNo:19448; depth 0.30 2021-02-13T01:12:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178734.12 epoch sec; seqNo:19448; 4061020.50 north; 593820.35 east 2021-02-13T01:12:14Z- AHRS_M2 msg: 1613178734.50 epoch sec; seqNo:19295 2021-02-13T01:12:14Z- AHRS_M2 msg: 5.16 phi; -0.24 theta; -0.07 psi 2021-02-13T01:12:14Z- handleDepth()-Depth_Keller msg: 1613178734509 epoch sec; seqNo:19449; depth 0.27 2021-02-13T01:12:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178734.52 epoch sec; seqNo:19449; 4061020.50 north; 593820.35 east 2021-02-13T01:12:14Z- AHRS_M2 msg: 1613178734.91 epoch sec; seqNo:19296 2021-02-13T01:12:14Z- AHRS_M2 msg: 5.16 phi; -0.24 theta; -0.17 psi 2021-02-13T01:12:14Z- handleDepth()-Depth_Keller msg: 1613178734914 epoch sec; seqNo:19450; depth 0.26 2021-02-13T01:12:14Z- handleDvl() - RDI_Pathfinder msg: 1613178734.93 epoch sec; seqNo:9345 2021-02-13T01:12:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 106.57 , 97.65 , 88.72 , 115.50 2021-02-13T01:12:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.16 , 0.22 , -0.33 2021-02-13T01:12:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178734.94 epoch sec; seqNo:19450; 4061020.50 north; 593820.35 east 2021-02-13T01:12:15Z- AHRS_M2 msg: 1613178735.31 epoch sec; seqNo:19297 2021-02-13T01:12:15Z- AHRS_M2 msg: 5.17 phi; -0.32 theta; -0.19 psi 2021-02-13T01:12:15Z- handleDepth()-Depth_Keller msg: 1613178735334 epoch sec; seqNo:19451; depth 0.21 2021-02-13T01:12:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178735.35 epoch sec; seqNo:19451; 4061020.88 north; 593820.34 east 2021-02-13T01:12:15Z- AHRS_M2 msg: 1613178735.72 epoch sec; seqNo:19298 2021-02-13T01:12:15Z- AHRS_M2 msg: 5.18 phi; -0.37 theta; -0.14 psi 2021-02-13T01:12:15Z- handleDepth()-Depth_Keller msg: 1613178735720 epoch sec; seqNo:19452; depth 0.22 2021-02-13T01:12:15Z- handleDvl() - RDI_Pathfinder msg: 1613178735.73 epoch sec; seqNo:9346 2021-02-13T01:12:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 111.31 , 89.00 , 117.26 2021-02-13T01:12:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.12 , 0.20 , -0.06 2021-02-13T01:12:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178735.76 epoch sec; seqNo:19452; 4061020.88 north; 593820.34 east 2021-02-13T01:12:16Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 129 calls... 2021-02-13T01:12:16Z- waiting for 1613178733.90, time is 1613178735.76 2021-02-13T01:12:16Z- LcmTrn::updateTrn() equating pose 1613178735.76 and meas times 1613178735.76 2021-02-13T01:12:16Z- LcmTrn::updateTrn() >>>> pose time 1613178735.76, meas time 1613178735.76 <<<< 2021-02-13T01:12:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178735.76 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:12:16Z- TerrainNav::motionUpdate() - currEstimate = (4061021, 593820.34, 0.22) 2021-02-13T01:12:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:12:16Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:12:16Z- TNavBF::Projecting Measurements 2021-02-13T01:12:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:12:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:12:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:12:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:12:16Z- TNavFilter::4 good beams. 2021-02-13T01:12:16Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:12:16Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:12:16Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:12:16Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:12:16Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:12:16Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:12:16Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:12:16Z- TNavBF::mmse selected from filter -1 2021-02-13T01:12:16Z- TerrainMapOctree: (vehN, vehE ) = (4061020.88, 593820.34). 2021-02-13T01:12:16Z- TerrainMapOctree: Min Distance = 486.93. 2021-02-13T01:12:16Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:12:16Z- There is no specific good beam that all particles have in common. 2021-02-13T01:12:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:12:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.54 K: 0.031 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T01:12:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:12:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.52 K: 0.024 ExpectedMeasDiff: nan currMeas.covariance: 19.83 2021-02-13T01:12:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:12:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.51 K: 0.034 ExpectedMeasDiff: nan currMeas.covariance: 12.67 2021-02-13T01:12:16Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:12:16Z- Old Depth Bias: 0.00 DepthBiasCov: 0.49 K: 0.020 ExpectedMeasDiff: nan currMeas.covariance: 22.00 2021-02-13T01:12:16Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:12:16Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:12:16Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:12:16Z- Filter 0 beam number: 2 num in subcloud: 2208 num not in subcloud: 7792 2021-02-13T01:12:16Z- Filter 0 partialDeltaRmsComputation: 153005.539667 terrainVariance: 24.952722 1-SumSquareWeights: 0.999489 2021-02-13T01:12:16Z- Filter 0 meanExpectedMeasDiff: -391.127328 delta_rms_squared: 24.942727 alpha: 0.999199 2021-02-13T01:12:16Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:16Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:12:16Z- Filter 0 SubcloudNIS: 0.354994 2021-02-13T01:12:16Z- Filter 0 Subcloud Weighting FAILED due to sumWeights == 0. 2021-02-13T01:12:16Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:12:16Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:12:16Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -2.7 NorthVariance: 673.0 EastVariance: 833.0 DepthVariance: 30.5 this->DepthBiasCov: 0.5 2021-02-13T01:12:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178735.76 sec. 2021-02-13T01:12:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:12:16Z- TNavBF::mmse selected from filter -1 2021-02-13T01:12:16Z- variances([0], [2], and psi_berg) = 672.969108 832.985604 0.000000 2021-02-13T01:12:16Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:12:16Z- LcmTrn::publishEstimates() - MLE : 4061020.38 593835.84 0.22 2021-02-13T01:12:16Z- LcmTrn::publishEstimates() - MMSE : 4061020.88 593820.34 -2.47 2021-02-13T01:12:16Z- LcmTrn::publishEstimates() - COVAR: 672.97 832.99 30.51 0.00 2021-02-13T01:12:16Z- AHRS_M2 msg: 1613178736.12 epoch sec; seqNo:19299 2021-02-13T01:12:16Z- AHRS_M2 msg: 5.15 phi; -0.35 theta; -0.05 psi 2021-02-13T01:12:16Z- handleDepth()-Depth_Keller msg: 1613178736125 epoch sec; seqNo:19453; depth 0.23 2021-02-13T01:12:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178736.14 epoch sec; seqNo:19453; 4061020.88 north; 593820.34 east 2021-02-13T01:12:16Z- AHRS_M2 msg: 1613178736.53 epoch sec; seqNo:19300 2021-02-13T01:12:16Z- AHRS_M2 msg: 5.13 phi; -0.37 theta; -0.04 psi 2021-02-13T01:12:16Z- handleDepth()-Depth_Keller msg: 1613178736542 epoch sec; seqNo:19454; depth 0.24 2021-02-13T01:12:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178736.57 epoch sec; seqNo:19454; 4061020.88 north; 593820.34 east 2021-02-13T01:12:16Z- AHRS_M2 msg: 1613178736.93 epoch sec; seqNo:19301 2021-02-13T01:12:16Z- AHRS_M2 msg: 5.14 phi; -0.39 theta; -0.11 psi 2021-02-13T01:12:16Z- handleDepth()-Depth_Keller msg: 1613178736933 epoch sec; seqNo:19455; depth 0.19 2021-02-13T01:12:16Z- handleDvl() - RDI_Pathfinder msg: 1613178736.95 epoch sec; seqNo:9347 2021-02-13T01:12:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.30 , 106.71 , 85.89 , 129.03 2021-02-13T01:12:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.01 , 0.08 , 0.29 2021-02-13T01:12:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178736.96 epoch sec; seqNo:19455; 4061020.88 north; 593820.34 east 2021-02-13T01:12:17Z- AHRS_M2 msg: 1613178737.34 epoch sec; seqNo:19302 2021-02-13T01:12:17Z- AHRS_M2 msg: 5.14 phi; -0.39 theta; -0.14 psi 2021-02-13T01:12:17Z- handleDepth()-Depth_Keller msg: 1613178737349 epoch sec; seqNo:19456; depth 0.18 2021-02-13T01:12:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178737.38 epoch sec; seqNo:19456; 4061021.26 north; 593820.34 east 2021-02-13T01:12:17Z- AHRS_M2 msg: 1613178737.74 epoch sec; seqNo:19303 2021-02-13T01:12:17Z- AHRS_M2 msg: 5.19 phi; -0.39 theta; -0.18 psi 2021-02-13T01:12:17Z- handleDepth()-Depth_Keller msg: 1613178737741 epoch sec; seqNo:19457; depth 0.19 2021-02-13T01:12:17Z- handleDvl() - RDI_Pathfinder msg: 1613178737.77 epoch sec; seqNo:9348 2021-02-13T01:12:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.23 , 114.08 , 87.30 , 133.42 2021-02-13T01:12:17Z- handleDvl() - RDI_Pathfinder msg: velocities -0.44 , -0.23 , 0.07 2021-02-13T01:12:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178737.79 epoch sec; seqNo:19457; 4061020.88 north; 593820.34 east 2021-02-13T01:12:18Z- AHRS_M2 msg: 1613178738.14 epoch sec; seqNo:19304 2021-02-13T01:12:18Z- AHRS_M2 msg: 5.23 phi; -0.36 theta; -0.16 psi 2021-02-13T01:12:18Z- handleDepth()-Depth_Keller msg: 1613178738149 epoch sec; seqNo:19458; depth 0.22 2021-02-13T01:12:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178738.16 epoch sec; seqNo:19458; 4061020.88 north; 593820.34 east 2021-02-13T01:12:18Z- AHRS_M2 msg: 1613178738.54 epoch sec; seqNo:19305 2021-02-13T01:12:18Z- AHRS_M2 msg: 5.24 phi; -0.34 theta; -0.02 psi 2021-02-13T01:12:18Z- handleDepth()-Depth_Keller msg: 1613178738549 epoch sec; seqNo:19459; depth 0.24 2021-02-13T01:12:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178738.56 epoch sec; seqNo:19459; 4061020.51 north; 593821.57 east 2021-02-13T01:12:18Z- AHRS_M2 msg: 1613178738.95 epoch sec; seqNo:19306 2021-02-13T01:12:18Z- AHRS_M2 msg: 5.22 phi; -0.33 theta; 0.10 psi 2021-02-13T01:12:18Z- handleDepth()-Depth_Keller msg: 1613178738957 epoch sec; seqNo:19460; depth 0.26 2021-02-13T01:12:18Z- handleDvl() - RDI_Pathfinder msg: 1613178738.97 epoch sec; seqNo:9349 2021-02-13T01:12:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.96 , 110.83 , 87.03 , 131.66 2021-02-13T01:12:18Z- handleDvl() - RDI_Pathfinder msg: velocities -0.35 , -0.15 , -0.29 2021-02-13T01:12:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178738.99 epoch sec; seqNo:19460; 4061020.51 north; 593821.57 east 2021-02-13T01:12:19Z- AHRS_M2 msg: 1613178739.35 epoch sec; seqNo:19307 2021-02-13T01:12:19Z- AHRS_M2 msg: 5.22 phi; -0.30 theta; 0.07 psi 2021-02-13T01:12:19Z- handleDepth()-Depth_Keller msg: 1613178739357 epoch sec; seqNo:19461; depth 0.29 2021-02-13T01:12:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178739.37 epoch sec; seqNo:19461; 4061020.51 north; 593821.57 east 2021-02-13T01:12:19Z- AHRS_M2 msg: 1613178739.76 epoch sec; seqNo:19308 2021-02-13T01:12:19Z- AHRS_M2 msg: 5.20 phi; -0.22 theta; -0.04 psi 2021-02-13T01:12:19Z- handleDepth()-Depth_Keller msg: 1613178739769 epoch sec; seqNo:19462; depth 0.25 2021-02-13T01:12:19Z- handleDvl() - RDI_Pathfinder msg: 1613178739.79 epoch sec; seqNo:9350 2021-02-13T01:12:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 109.49 , 96.10 , 87.17 , 121.39 2021-02-13T01:12:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.21 , 0.03 , -0.40 2021-02-13T01:12:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178739.80 epoch sec; seqNo:19462; 4061020.51 north; 593821.57 east 2021-02-13T01:12:20Z- AHRS_M2 msg: 1613178740.16 epoch sec; seqNo:19309 2021-02-13T01:12:20Z- AHRS_M2 msg: 5.20 phi; -0.17 theta; -0.12 psi 2021-02-13T01:12:20Z- handleDepth()-Depth_Keller msg: 1613178740167 epoch sec; seqNo:19463; depth 0.21 2021-02-13T01:12:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178740.19 epoch sec; seqNo:19463; 4061020.50 north; 593820.35 east 2021-02-13T01:12:20Z- AHRS_M2 msg: 1613178740.56 epoch sec; seqNo:19310 2021-02-13T01:12:20Z- AHRS_M2 msg: 5.24 phi; -0.17 theta; -0.15 psi 2021-02-13T01:12:20Z- handleDepth()-Depth_Keller msg: 1613178740568 epoch sec; seqNo:19464; depth 0.16 2021-02-13T01:12:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178740.58 epoch sec; seqNo:19464; 4061020.50 north; 593820.35 east 2021-02-13T01:12:20Z- AHRS_M2 msg: 1613178740.97 epoch sec; seqNo:19311 2021-02-13T01:12:20Z- AHRS_M2 msg: 5.25 phi; -0.18 theta; -0.10 psi 2021-02-13T01:12:20Z- handleDepth()-Depth_Keller msg: 1613178740973 epoch sec; seqNo:19465; depth 0.15 2021-02-13T01:12:20Z- handleDvl() - RDI_Pathfinder msg: 1613178740.98 epoch sec; seqNo:9351 2021-02-13T01:12:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.32 , 105.23 , 90.35 , 108.20 2021-02-13T01:12:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.43 , -0.05 , 0.37 2021-02-13T01:12:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178740.99 epoch sec; seqNo:19465; 4061020.50 north; 593820.35 east 2021-02-13T01:12:21Z- LcmTrn::handleMessages() - handling 44 messages took 0.71 ms and 130 calls... 2021-02-13T01:12:21Z- waiting for 1613178738.76, time is 1613178740.99 2021-02-13T01:12:21Z- LcmTrn::updateTrn() equating pose 1613178740.99 and meas times 1613178740.99 2021-02-13T01:12:21Z- LcmTrn::updateTrn() >>>> pose time 1613178740.99, meas time 1613178740.99 <<<< 2021-02-13T01:12:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178740.99 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:12:21Z- TerrainNav::motionUpdate() - currEstimate = (4061021, 593820.35, 0.15) 2021-02-13T01:12:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:12:21Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:12:21Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178740.99 sec.; No relevant navigation data available 2021-02-13T01:12:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:12:21Z- TNavBF::mmse selected from filter -1 2021-02-13T01:12:21Z- variances([0], [2], and psi_berg) = 672.956542 832.985599 0.000000 2021-02-13T01:12:21Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:12:21Z- LcmTrn::publishEstimates() - MLE : 4061020.00 593835.85 0.15 2021-02-13T01:12:21Z- LcmTrn::publishEstimates() - MMSE : 4061020.50 593820.35 -2.53 2021-02-13T01:12:21Z- LcmTrn::publishEstimates() - COVAR: 672.96 832.99 30.53 0.00 2021-02-13T01:12:21Z- AHRS_M2 msg: 1613178741.37 epoch sec; seqNo:19312 2021-02-13T01:12:21Z- AHRS_M2 msg: 5.22 phi; -0.15 theta; -0.00 psi 2021-02-13T01:12:21Z- handleDepth()-Depth_Keller msg: 1613178741385 epoch sec; seqNo:19466; depth 0.12 2021-02-13T01:12:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178741.40 epoch sec; seqNo:19466; 4061020.50 north; 593820.35 east 2021-02-13T01:12:21Z- AHRS_M2 msg: 1613178741.78 epoch sec; seqNo:19313 2021-02-13T01:12:21Z- AHRS_M2 msg: 5.22 phi; -0.15 theta; 0.02 psi 2021-02-13T01:12:21Z- handleDepth()-Depth_Keller msg: 1613178741780 epoch sec; seqNo:19467; depth 0.09 2021-02-13T01:12:21Z- handleDvl() - RDI_Pathfinder msg: 1613178741.80 epoch sec; seqNo:9352 2021-02-13T01:12:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 99.81 , 97.11 , 107.93 2021-02-13T01:12:21Z- handleDvl() - RDI_Pathfinder msg: velocities -0.06 , -0.00 , 0.79 2021-02-13T01:12:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178741.81 epoch sec; seqNo:19467; 4061020.50 north; 593820.35 east 2021-02-13T01:12:22Z- AHRS_M2 msg: 1613178742.18 epoch sec; seqNo:19314 2021-02-13T01:12:22Z- AHRS_M2 msg: 5.23 phi; -0.21 theta; 0.01 psi 2021-02-13T01:12:22Z- handleDepth()-Depth_Keller msg: 1613178742189 epoch sec; seqNo:19468; depth 0.08 2021-02-13T01:12:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178742.20 epoch sec; seqNo:19468; 4061020.50 north; 593820.35 east 2021-02-13T01:12:22Z- AHRS_M2 msg: 1613178742.59 epoch sec; seqNo:19315 2021-02-13T01:12:22Z- AHRS_M2 msg: 5.26 phi; -0.24 theta; -0.05 psi 2021-02-13T01:12:22Z- handleDepth()-Depth_Keller msg: 1613178742589 epoch sec; seqNo:19469; depth 0.06 2021-02-13T01:12:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178742.60 epoch sec; seqNo:19469; 4061020.50 north; 593820.35 east 2021-02-13T01:12:22Z- AHRS_M2 msg: 1613178742.99 epoch sec; seqNo:19316 2021-02-13T01:12:22Z- AHRS_M2 msg: 5.27 phi; -0.21 theta; -0.08 psi 2021-02-13T01:12:23Z- handleDepth()-Depth_Keller msg: 1613178743001 epoch sec; seqNo:19470; depth 0.07 2021-02-13T01:12:23Z- handleDvl() - RDI_Pathfinder msg: 1613178743.01 epoch sec; seqNo:9353 2021-02-13T01:12:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.03 , 95.62 , 90.21 , 113.20 2021-02-13T01:12:23Z- handleDvl() - RDI_Pathfinder msg: velocities -0.88 , 0.32 , 0.32 2021-02-13T01:12:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178743.02 epoch sec; seqNo:19470; 4061020.51 north; 593821.57 east 2021-02-13T01:12:23Z- AHRS_M2 msg: 1613178743.39 epoch sec; seqNo:19317 2021-02-13T01:12:23Z- AHRS_M2 msg: 5.23 phi; -0.23 theta; -0.08 psi 2021-02-13T01:12:23Z- handleDepth()-Depth_Keller msg: 1613178743399 epoch sec; seqNo:19471; depth 0.13 2021-02-13T01:12:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178743.42 epoch sec; seqNo:19471; 4061020.51 north; 593821.57 east 2021-02-13T01:12:23Z- AHRS_M2 msg: 1613178743.80 epoch sec; seqNo:19318 2021-02-13T01:12:23Z- AHRS_M2 msg: 5.16 phi; -0.20 theta; -0.12 psi 2021-02-13T01:12:23Z- handleDepth()-Depth_Keller msg: 1613178743805 epoch sec; seqNo:19472; depth 0.10 2021-02-13T01:12:23Z- handleDvl() - RDI_Pathfinder msg: 1613178743.82 epoch sec; seqNo:9354 2021-02-13T01:12:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 103.81 , 90.28 , 113.28 2021-02-13T01:12:23Z- handleDvl() - RDI_Pathfinder msg: velocities -0.52 , 0.40 , -0.68 2021-02-13T01:12:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178743.85 epoch sec; seqNo:19472; 4061020.51 north; 593821.57 east 2021-02-13T01:12:24Z- AHRS_M2 msg: 1613178744.20 epoch sec; seqNo:19319 2021-02-13T01:12:24Z- AHRS_M2 msg: 5.12 phi; -0.04 theta; -0.20 psi 2021-02-13T01:12:24Z- handleDepth()-Depth_Keller msg: 1613178744209 epoch sec; seqNo:19473; depth 0.03 2021-02-13T01:12:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178744.22 epoch sec; seqNo:19473; 4061020.51 north; 593821.57 east 2021-02-13T01:12:24Z- AHRS_M2 msg: 1613178744.61 epoch sec; seqNo:19320 2021-02-13T01:12:24Z- AHRS_M2 msg: 5.13 phi; 0.06 theta; -0.19 psi 2021-02-13T01:12:24Z- handleDepth()-Depth_Keller msg: 1613178744618 epoch sec; seqNo:19474; depth 0.01 2021-02-13T01:12:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178744.65 epoch sec; seqNo:19474; 4061020.53 north; 593822.79 east 2021-02-13T01:12:25Z- AHRS_M2 msg: 1613178745.01 epoch sec; seqNo:19321 2021-02-13T01:12:25Z- AHRS_M2 msg: 5.19 phi; 0.04 theta; 0.01 psi 2021-02-13T01:12:25Z- handleDepth()-Depth_Keller msg: 1613178745023 epoch sec; seqNo:19475; depth -0.00 2021-02-13T01:12:25Z- handleDvl() - RDI_Pathfinder msg: 1613178745.04 epoch sec; seqNo:9355 2021-02-13T01:12:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.98 , 113.21 , 96.98 , 101.03 2021-02-13T01:12:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.37 , -0.32 , 0.29 2021-02-13T01:12:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178745.05 epoch sec; seqNo:19475; 4061020.51 north; 593821.57 east 2021-02-13T01:12:25Z- AHRS_M2 msg: 1613178745.42 epoch sec; seqNo:19322 2021-02-13T01:12:25Z- AHRS_M2 msg: 5.15 phi; -0.06 theta; 0.04 psi 2021-02-13T01:12:25Z- handleDepth()-Depth_Keller msg: 1613178745423 epoch sec; seqNo:19476; depth -0.01 2021-02-13T01:12:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178745.45 epoch sec; seqNo:19476; 4061020.51 north; 593821.57 east 2021-02-13T01:12:25Z- AHRS_M2 msg: 1613178745.82 epoch sec; seqNo:19323 2021-02-13T01:12:25Z- AHRS_M2 msg: 5.11 phi; -0.09 theta; 0.01 psi 2021-02-13T01:12:25Z- handleDepth()-Depth_Keller msg: 1613178745820 epoch sec; seqNo:19477; depth 0.03 2021-02-13T01:12:25Z- handleDvl() - RDI_Pathfinder msg: 1613178745.83 epoch sec; seqNo:9356 2021-02-13T01:12:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 99.41 , 102.39 , 97.92 2021-02-13T01:12:25Z- handleDvl() - RDI_Pathfinder msg: velocities -0.45 , 0.06 , 0.19 2021-02-13T01:12:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178745.84 epoch sec; seqNo:19477; 4061020.51 north; 593821.57 east 2021-02-13T01:12:26Z- AHRS_M2 msg: 1613178746.22 epoch sec; seqNo:19324 2021-02-13T01:12:26Z- AHRS_M2 msg: 5.06 phi; -0.10 theta; -0.08 psi 2021-02-13T01:12:26Z- LcmTrn::handleMessages() - handling 42 messages took 0.76 ms and 127 calls... 2021-02-13T01:12:26Z- waiting for 1613178743.99, time is 1613178746.22 2021-02-13T01:12:26Z- LcmTrn::updateTrn() >>>> pose time 1613178746.22, meas time 1613178745.83 <<<< 2021-02-13T01:12:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178745.83 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:12:26Z- TerrainNav::motionUpdate() - currEstimate = (4061021, 593821.57, 0.03) 2021-02-13T01:12:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:12:26Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:12:26Z- TNavBF::Projecting Measurements 2021-02-13T01:12:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:12:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:12:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:12:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:12:26Z- TNavFilter::4 good beams. 2021-02-13T01:12:26Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:12:26Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:12:26Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:12:26Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:12:26Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:12:26Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:12:26Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:12:26Z- TNavBF::mmse selected from filter -1 2021-02-13T01:12:26Z- TerrainMapOctree: (vehN, vehE ) = (4061020.51, 593821.48). 2021-02-13T01:12:26Z- TerrainMapOctree: Min Distance = 486.10. 2021-02-13T01:12:26Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:12:26Z- There is no specific good beam that all particles have in common. 2021-02-13T01:12:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:12:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.56 K: 0.032 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T01:12:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:12:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.54 K: 0.030 ExpectedMeasDiff: nan currMeas.covariance: 15.81 2021-02-13T01:12:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:12:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.53 K: 0.028 ExpectedMeasDiff: nan currMeas.covariance: 16.77 2021-02-13T01:12:26Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:12:26Z- Old Depth Bias: 0.00 DepthBiasCov: 0.51 K: 0.029 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T01:12:26Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:12:26Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:12:26Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:12:26Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:12:26Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:26Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:12:26Z- Filter 0 SubcloudNIS: 0.355002 2021-02-13T01:12:26Z- Filter 0 No beams used in subcloud update 2021-02-13T01:12:26Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:12:26Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:12:26Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -2.7 NorthVariance: 672.9 EastVariance: 833.0 DepthVariance: 30.5 this->DepthBiasCov: 0.5 2021-02-13T01:12:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178745.83 sec. 2021-02-13T01:12:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:12:26Z- TNavBF::mmse selected from filter -1 2021-02-13T01:12:26Z- variances([0], [2], and psi_berg) = 672.928690 832.985588 0.000000 2021-02-13T01:12:26Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:12:26Z- LcmTrn::publishEstimates() - MLE : 4061020.01 593837.07 0.03 2021-02-13T01:12:26Z- LcmTrn::publishEstimates() - MMSE : 4061020.51 593821.57 -2.66 2021-02-13T01:12:26Z- LcmTrn::publishEstimates() - COVAR: 672.93 832.99 30.53 0.00 2021-02-13T01:12:26Z- handleDepth()-Depth_Keller msg: 1613178746226 epoch sec; seqNo:19478; depth 0.01 2021-02-13T01:12:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178746.24 epoch sec; seqNo:19478; 4061020.15 north; 593822.79 east 2021-02-13T01:12:26Z- AHRS_M2 msg: 1613178746.65 epoch sec; seqNo:19325 2021-02-13T01:12:26Z- AHRS_M2 msg: 5.08 phi; -0.17 theta; -0.19 psi 2021-02-13T01:12:26Z- handleDepth()-Depth_Keller msg: 1613178746659 epoch sec; seqNo:19479; depth 0.02 2021-02-13T01:12:26Z- handleDvl() - RDI_Pathfinder msg: 1613178746.67 epoch sec; seqNo:9357 2021-02-13T01:12:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.46 , 98.46 , 94.41 , 102.52 2021-02-13T01:12:26Z- handleDvl() - RDI_Pathfinder msg: velocities -0.38 , -0.21 , -0.23 2021-02-13T01:12:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178746.68 epoch sec; seqNo:19479; 4061020.15 north; 593822.79 east 2021-02-13T01:12:27Z- AHRS_M2 msg: 1613178747.03 epoch sec; seqNo:19326 2021-02-13T01:12:27Z- AHRS_M2 msg: 5.15 phi; -0.17 theta; -0.17 psi 2021-02-13T01:12:27Z- handleDepth()-Depth_Keller msg: 1613178747037 epoch sec; seqNo:19480; depth 0.03 2021-02-13T01:12:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178747.06 epoch sec; seqNo:19480; 4061020.15 north; 593822.79 east 2021-02-13T01:12:27Z- AHRS_M2 msg: 1613178747.43 epoch sec; seqNo:19327 2021-02-13T01:12:27Z- AHRS_M2 msg: 5.14 phi; -0.15 theta; -0.04 psi 2021-02-13T01:12:27Z- handleDepth()-Depth_Keller msg: 1613178747437 epoch sec; seqNo:19481; depth 0.05 2021-02-13T01:12:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178747.45 epoch sec; seqNo:19481; 4061019.77 north; 593822.80 east 2021-02-13T01:12:27Z- AHRS_M2 msg: 1613178747.84 epoch sec; seqNo:19328 2021-02-13T01:12:27Z- AHRS_M2 msg: 5.14 phi; -0.18 theta; 0.02 psi 2021-02-13T01:12:27Z- handleDepth()-Depth_Keller msg: 1613178747841 epoch sec; seqNo:19482; depth 0.08 2021-02-13T01:12:27Z- handleDvl() - RDI_Pathfinder msg: 1613178747.85 epoch sec; seqNo:9358 2021-02-13T01:12:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.77 , 113.41 , 97.18 , 109.35 2021-02-13T01:12:27Z- handleDvl() - RDI_Pathfinder msg: velocities -0.02 , -0.12 , -0.47 2021-02-13T01:12:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178747.86 epoch sec; seqNo:19482; 4061019.77 north; 593822.80 east 2021-02-13T01:12:28Z- AHRS_M2 msg: 1613178748.24 epoch sec; seqNo:19329 2021-02-13T01:12:28Z- AHRS_M2 msg: 5.15 phi; -0.17 theta; 0.02 psi 2021-02-13T01:12:28Z- handleDepth()-Depth_Keller msg: 1613178748245 epoch sec; seqNo:19483; depth 0.07 2021-02-13T01:12:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178748.26 epoch sec; seqNo:19483; 4061019.77 north; 593822.80 east 2021-02-13T01:12:28Z- AHRS_M2 msg: 1613178748.64 epoch sec; seqNo:19330 2021-02-13T01:12:28Z- AHRS_M2 msg: 5.13 phi; -0.10 theta; -0.04 psi 2021-02-13T01:12:28Z- handleDepth()-Depth_Keller msg: 1613178748648 epoch sec; seqNo:19484; depth 0.11 2021-02-13T01:12:28Z- handleDvl() - RDI_Pathfinder msg: 1613178748.66 epoch sec; seqNo:9359 2021-02-13T01:12:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 100.83 , 97.86 , 97.86 , 109.76 2021-02-13T01:12:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.33 , 0.08 , -0.36 2021-02-13T01:12:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178748.68 epoch sec; seqNo:19484; 4061019.77 north; 593822.80 east 2021-02-13T01:12:29Z- AHRS_M2 msg: 1613178749.05 epoch sec; seqNo:19331 2021-02-13T01:12:29Z- AHRS_M2 msg: 5.11 phi; -0.11 theta; -0.14 psi 2021-02-13T01:12:29Z- handleDepth()-Depth_Keller msg: 1613178749063 epoch sec; seqNo:19485; depth 0.09 2021-02-13T01:12:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178749.11 epoch sec; seqNo:19485; 4061020.13 north; 593821.57 east 2021-02-13T01:12:29Z- AHRS_M2 msg: 1613178749.45 epoch sec; seqNo:19332 2021-02-13T01:12:29Z- AHRS_M2 msg: 5.09 phi; -0.21 theta; -0.22 psi 2021-02-13T01:12:29Z- handleDepth()-Depth_Keller msg: 1613178749458 epoch sec; seqNo:19486; depth 0.07 2021-02-13T01:12:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178749.49 epoch sec; seqNo:19486; 4061020.13 north; 593821.57 east 2021-02-13T01:12:29Z- AHRS_M2 msg: 1613178749.86 epoch sec; seqNo:19333 2021-02-13T01:12:29Z- AHRS_M2 msg: 5.13 phi; -0.21 theta; -0.22 psi 2021-02-13T01:12:29Z- handleDepth()-Depth_Keller msg: 1613178749860 epoch sec; seqNo:19487; depth 0.09 2021-02-13T01:12:29Z- handleDvl() - RDI_Pathfinder msg: 1613178749.88 epoch sec; seqNo:9360 2021-02-13T01:12:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 103.94 , 97.99 , 103.94 2021-02-13T01:12:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.10 , -0.01 , 0.25 2021-02-13T01:12:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178749.89 epoch sec; seqNo:19487; 4061020.13 north; 593821.57 east 2021-02-13T01:12:30Z- AHRS_M2 msg: 1613178750.26 epoch sec; seqNo:19334 2021-02-13T01:12:30Z- AHRS_M2 msg: 5.17 phi; -0.13 theta; -0.13 psi 2021-02-13T01:12:30Z- handleDepth()-Depth_Keller msg: 1613178750269 epoch sec; seqNo:19488; depth 0.07 2021-02-13T01:12:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178750.28 epoch sec; seqNo:19488; 4061020.13 north; 593821.57 east 2021-02-13T01:12:30Z- AHRS_M2 msg: 1613178750.67 epoch sec; seqNo:19335 2021-02-13T01:12:30Z- AHRS_M2 msg: 5.16 phi; -0.10 theta; -0.02 psi 2021-02-13T01:12:30Z- handleDepth()-Depth_Keller msg: 1613178750674 epoch sec; seqNo:19489; depth 0.09 2021-02-13T01:12:30Z- handleDvl() - RDI_Pathfinder msg: 1613178750.69 epoch sec; seqNo:9361 2021-02-13T01:12:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.98 , 110.50 , 96.98 , 111.86 2021-02-13T01:12:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.25 , 0.21 , -0.06 2021-02-13T01:12:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178750.70 epoch sec; seqNo:19489; 4061020.13 north; 593821.57 east 2021-02-13T01:12:31Z- AHRS_M2 msg: 1613178751.07 epoch sec; seqNo:19336 2021-02-13T01:12:31Z- AHRS_M2 msg: 5.12 phi; -0.11 theta; -0.01 psi 2021-02-13T01:12:31Z- handleDepth()-Depth_Keller msg: 1613178751075 epoch sec; seqNo:19490; depth 0.08 2021-02-13T01:12:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178751.09 epoch sec; seqNo:19490; 4061020.51 north; 593821.57 east 2021-02-13T01:12:31Z- LcmTrn::handleMessages() - handling 43 messages took 0.71 ms and 129 calls... 2021-02-13T01:12:31Z- waiting for 1613178749.22, time is 1613178751.09 2021-02-13T01:12:31Z- LcmTrn::updateTrn() >>>> pose time 1613178751.09, meas time 1613178750.69 <<<< 2021-02-13T01:12:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178750.69 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:12:31Z- TerrainNav::motionUpdate() - currEstimate = (4061021, 593821.57, 0.08) 2021-02-13T01:12:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:12:31Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:12:31Z- TNavBF::Projecting Measurements 2021-02-13T01:12:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:12:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:12:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:12:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:12:31Z- TNavFilter::4 good beams. 2021-02-13T01:12:31Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:12:31Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:12:31Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:12:31Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:12:31Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:12:31Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:12:31Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:12:31Z- TNavBF::mmse selected from filter -1 2021-02-13T01:12:31Z- TerrainMapOctree: (vehN, vehE ) = (4061020.51, 593821.57). 2021-02-13T01:12:31Z- TerrainMapOctree: Min Distance = 486.02. 2021-02-13T01:12:31Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:12:31Z- There is no specific good beam that all particles have in common. 2021-02-13T01:12:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:12:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.50 K: 0.029 ExpectedMeasDiff: nan currMeas.covariance: 15.05 2021-02-13T01:12:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:12:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.49 K: 0.022 ExpectedMeasDiff: nan currMeas.covariance: 19.54 2021-02-13T01:12:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:12:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.48 K: 0.028 ExpectedMeasDiff: nan currMeas.covariance: 15.05 2021-02-13T01:12:31Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:12:31Z- Old Depth Bias: 0.00 DepthBiasCov: 0.46 K: 0.021 ExpectedMeasDiff: nan currMeas.covariance: 20.02 2021-02-13T01:12:31Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:12:31Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:12:31Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:12:31Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:12:31Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:31Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:12:31Z- Filter 0 SubcloudNIS: 0.355003 2021-02-13T01:12:31Z- Filter 0 No beams used in subcloud update 2021-02-13T01:12:31Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:12:31Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:12:31Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -2.7 NorthVariance: 672.9 EastVariance: 833.0 DepthVariance: 30.5 this->DepthBiasCov: 0.5 2021-02-13T01:12:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178750.69 sec. 2021-02-13T01:12:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:12:31Z- TNavBF::mmse selected from filter -1 2021-02-13T01:12:31Z- variances([0], [2], and psi_berg) = 672.928690 832.985588 0.000000 2021-02-13T01:12:31Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:12:31Z- LcmTrn::publishEstimates() - MLE : 4061020.01 593837.07 0.08 2021-02-13T01:12:31Z- LcmTrn::publishEstimates() - MMSE : 4061020.51 593821.57 -2.60 2021-02-13T01:12:31Z- LcmTrn::publishEstimates() - COVAR: 672.93 832.99 30.48 0.00 2021-02-13T01:12:31Z- AHRS_M2 msg: 1613178751.47 epoch sec; seqNo:19337 2021-02-13T01:12:31Z- AHRS_M2 msg: 5.05 phi; -0.11 theta; -0.13 psi 2021-02-13T01:12:31Z- handleDepth()-Depth_Keller msg: 1613178751490 epoch sec; seqNo:19491; depth 0.07 2021-02-13T01:12:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178751.50 epoch sec; seqNo:19491; 4061020.51 north; 593821.57 east 2021-02-13T01:12:31Z- AHRS_M2 msg: 1613178751.88 epoch sec; seqNo:19338 2021-02-13T01:12:31Z- AHRS_M2 msg: 5.07 phi; -0.11 theta; -0.29 psi 2021-02-13T01:12:31Z- handleDepth()-Depth_Keller msg: 1613178751880 epoch sec; seqNo:19492; depth 0.04 2021-02-13T01:12:31Z- handleDvl() - RDI_Pathfinder msg: 1613178751.90 epoch sec; seqNo:9362 2021-02-13T01:12:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.79 , 97.79 , 97.79 , 105.23 2021-02-13T01:12:31Z- handleDvl() - RDI_Pathfinder msg: velocities -0.09 , -0.02 , 0.72 2021-02-13T01:12:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178751.93 epoch sec; seqNo:19492; 4061020.51 north; 593821.57 east 2021-02-13T01:12:32Z- AHRS_M2 msg: 1613178752.28 epoch sec; seqNo:19339 2021-02-13T01:12:32Z- AHRS_M2 msg: 5.11 phi; -0.13 theta; -0.26 psi 2021-02-13T01:12:32Z- handleDepth()-Depth_Keller msg: 1613178752284 epoch sec; seqNo:19493; depth 0.02 2021-02-13T01:12:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178752.30 epoch sec; seqNo:19493; 4061020.51 north; 593821.57 east 2021-02-13T01:12:32Z- AHRS_M2 msg: 1613178752.69 epoch sec; seqNo:19340 2021-02-13T01:12:32Z- AHRS_M2 msg: 5.12 phi; -0.05 theta; -0.11 psi 2021-02-13T01:12:32Z- handleDepth()-Depth_Keller msg: 1613178752690 epoch sec; seqNo:19494; depth 0.06 2021-02-13T01:12:32Z- handleDvl() - RDI_Pathfinder msg: 1613178752.70 epoch sec; seqNo:9363 2021-02-13T01:12:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.93 , 122.74 , 97.04 , 111.92 2021-02-13T01:12:32Z- handleDvl() - RDI_Pathfinder msg: velocities -0.55 , -0.31 , 0.39 2021-02-13T01:12:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178752.71 epoch sec; seqNo:19494; 4061020.13 north; 593821.57 east 2021-02-13T01:12:33Z- AHRS_M2 msg: 1613178753.09 epoch sec; seqNo:19341 2021-02-13T01:12:33Z- AHRS_M2 msg: 5.16 phi; 0.02 theta; 0.02 psi 2021-02-13T01:12:33Z- handleDepth()-Depth_Keller msg: 1613178753095 epoch sec; seqNo:19495; depth 0.05 2021-02-13T01:12:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178753.14 epoch sec; seqNo:19495; 4061020.15 north; 593822.79 east 2021-02-13T01:12:33Z- AHRS_M2 msg: 1613178753.49 epoch sec; seqNo:19342 2021-02-13T01:12:33Z- AHRS_M2 msg: 5.20 phi; -0.05 theta; 0.14 psi 2021-02-13T01:12:33Z- handleDepth()-Depth_Keller msg: 1613178753499 epoch sec; seqNo:19496; depth 0.03 2021-02-13T01:12:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178753.53 epoch sec; seqNo:19496; 4061019.77 north; 593822.80 east 2021-02-13T01:12:33Z- AHRS_M2 msg: 1613178753.90 epoch sec; seqNo:19343 2021-02-13T01:12:33Z- AHRS_M2 msg: 5.18 phi; -0.12 theta; 0.16 psi 2021-02-13T01:12:33Z- handleDepth()-Depth_Keller msg: 1613178753904 epoch sec; seqNo:19497; depth 0.05 2021-02-13T01:12:33Z- handleDvl() - RDI_Pathfinder msg: 1613178753.94 epoch sec; seqNo:9364 2021-02-13T01:12:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.14 , 97.65 , 97.65 , 97.65 2021-02-13T01:12:33Z- handleDvl() - RDI_Pathfinder msg: velocities -0.76 , 0.01 , 0.55 2021-02-13T01:12:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178753.95 epoch sec; seqNo:19497; 4061019.77 north; 593822.80 east 2021-02-13T01:12:34Z- AHRS_M2 msg: 1613178754.30 epoch sec; seqNo:19344 2021-02-13T01:12:34Z- AHRS_M2 msg: 5.15 phi; -0.18 theta; 0.08 psi 2021-02-13T01:12:34Z- handleDepth()-Depth_Keller msg: 1613178754313 epoch sec; seqNo:19498; depth 0.02 2021-02-13T01:12:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178754.34 epoch sec; seqNo:19498; 4061019.39 north; 593822.80 east 2021-02-13T01:12:34Z- AHRS_M2 msg: 1613178754.71 epoch sec; seqNo:19345 2021-02-13T01:12:34Z- AHRS_M2 msg: 5.13 phi; -0.15 theta; -0.07 psi 2021-02-13T01:12:34Z- handleDepth()-Depth_Keller msg: 1613178754711 epoch sec; seqNo:19499; depth 0.03 2021-02-13T01:12:34Z- handleDvl() - RDI_Pathfinder msg: 1613178754.73 epoch sec; seqNo:9365 2021-02-13T01:12:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 109.35 , 93.12 , 97.18 , 110.71 2021-02-13T01:12:34Z- handleDvl() - RDI_Pathfinder msg: velocities -0.74 , 0.51 , -0.75 2021-02-13T01:12:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178754.74 epoch sec; seqNo:19499; 4061019.77 north; 593822.80 east 2021-02-13T01:12:35Z- AHRS_M2 msg: 1613178755.11 epoch sec; seqNo:19346 2021-02-13T01:12:35Z- AHRS_M2 msg: 5.12 phi; -0.00 theta; -0.21 psi 2021-02-13T01:12:35Z- handleDepth()-Depth_Keller msg: 1613178755117 epoch sec; seqNo:19500; depth 0.01 2021-02-13T01:12:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178755.13 epoch sec; seqNo:19500; 4061019.78 north; 593824.02 east 2021-02-13T01:12:35Z- AHRS_M2 msg: 1613178755.52 epoch sec; seqNo:19347 2021-02-13T01:12:35Z- AHRS_M2 msg: 5.08 phi; 0.10 theta; -0.26 psi 2021-02-13T01:12:35Z- handleDepth()-Depth_Keller msg: 1613178755522 epoch sec; seqNo:19501; depth 0.06 2021-02-13T01:12:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178755.53 epoch sec; seqNo:19501; 4061019.78 north; 593824.02 east 2021-02-13T01:12:35Z- AHRS_M2 msg: 1613178755.92 epoch sec; seqNo:19348 2021-02-13T01:12:35Z- AHRS_M2 msg: 5.03 phi; 0.06 theta; -0.27 psi 2021-02-13T01:12:35Z- handleDepth()-Depth_Keller msg: 1613178755934 epoch sec; seqNo:19502; depth 0.10 2021-02-13T01:12:35Z- handleDvl() - RDI_Pathfinder msg: 1613178755.95 epoch sec; seqNo:9366 2021-02-13T01:12:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 109.83 , 99.41 , 97.92 2021-02-13T01:12:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.16 , 0.10 , -0.23 2021-02-13T01:12:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178755.96 epoch sec; seqNo:19502; 4061019.78 north; 593824.02 east 2021-02-13T01:12:36Z- AHRS_M2 msg: 1613178756.32 epoch sec; seqNo:19349 2021-02-13T01:12:36Z- AHRS_M2 msg: 5.01 phi; -0.05 theta; -0.23 psi 2021-02-13T01:12:36Z- handleDepth()-Depth_Keller msg: 1613178756324 epoch sec; seqNo:19503; depth 0.12 2021-02-13T01:12:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178756.34 epoch sec; seqNo:19503; 4061019.78 north; 593824.02 east 2021-02-13T01:12:36Z- LcmTrn::handleMessages() - handling 44 messages took 0.79 ms and 128 calls... 2021-02-13T01:12:36Z- waiting for 1613178754.09, time is 1613178756.34 2021-02-13T01:12:36Z- LcmTrn::updateTrn() >>>> pose time 1613178756.34, meas time 1613178755.95 <<<< 2021-02-13T01:12:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178755.95 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:12:36Z- TerrainNav::motionUpdate() - currEstimate = (4061020, 593824.02, 0.12) 2021-02-13T01:12:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:12:36Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:12:36Z- TNavBF::Projecting Measurements 2021-02-13T01:12:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:12:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:12:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:12:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:12:36Z- TNavFilter::4 good beams. 2021-02-13T01:12:36Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:12:36Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:12:36Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:12:36Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:12:36Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:12:36Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:12:36Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:12:36Z- TNavBF::mmse selected from filter -1 2021-02-13T01:12:36Z- TerrainMapOctree: (vehN, vehE ) = (4061019.84, 593823.84). 2021-02-13T01:12:36Z- TerrainMapOctree: Min Distance = 484.35. 2021-02-13T01:12:36Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:12:36Z- There is no specific good beam that all particles have in common. 2021-02-13T01:12:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:12:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.57 K: 0.033 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T01:12:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:12:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.55 K: 0.026 ExpectedMeasDiff: nan currMeas.covariance: 19.30 2021-02-13T01:12:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:12:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.54 K: 0.030 ExpectedMeasDiff: nan currMeas.covariance: 15.81 2021-02-13T01:12:36Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:12:36Z- Old Depth Bias: 0.00 DepthBiasCov: 0.52 K: 0.030 ExpectedMeasDiff: nan currMeas.covariance: 15.34 2021-02-13T01:12:36Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:12:36Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:12:36Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:12:36Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:12:36Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:36Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:12:36Z- Filter 0 SubcloudNIS: 0.355010 2021-02-13T01:12:36Z- Filter 0 No beams used in subcloud update 2021-02-13T01:12:36Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:12:36Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:12:36Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -2.7 NorthVariance: 672.9 EastVariance: 833.0 DepthVariance: 30.5 this->DepthBiasCov: 0.5 2021-02-13T01:12:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178755.95 sec. 2021-02-13T01:12:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:12:36Z- TNavBF::mmse selected from filter -1 2021-02-13T01:12:36Z- variances([0], [2], and psi_berg) = 672.888419 832.985572 0.000000 2021-02-13T01:12:36Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:12:36Z- LcmTrn::publishEstimates() - MLE : 4061019.28 593839.52 0.12 2021-02-13T01:12:36Z- LcmTrn::publishEstimates() - MMSE : 4061019.78 593824.02 -2.57 2021-02-13T01:12:36Z- LcmTrn::publishEstimates() - COVAR: 672.89 832.99 30.54 0.00 2021-02-13T01:12:36Z- AHRS_M2 msg: 1613178756.72 epoch sec; seqNo:19350 2021-02-13T01:12:36Z- AHRS_M2 msg: 5.04 phi; -0.05 theta; -0.13 psi 2021-02-13T01:12:36Z- handleDepth()-Depth_Keller msg: 1613178756733 epoch sec; seqNo:19504; depth 0.14 2021-02-13T01:12:36Z- handleDvl() - RDI_Pathfinder msg: 1613178756.74 epoch sec; seqNo:9367 2021-02-13T01:12:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 115.85 , 99.48 , 99.48 2021-02-13T01:12:36Z- handleDvl() - RDI_Pathfinder msg: velocities -0.28 , -0.63 , -0.58 2021-02-13T01:12:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178756.76 epoch sec; seqNo:19504; 4061019.40 north; 593824.02 east 2021-02-13T01:12:37Z- AHRS_M2 msg: 1613178757.13 epoch sec; seqNo:19351 2021-02-13T01:12:37Z- AHRS_M2 msg: 5.07 phi; 0.01 theta; -0.04 psi 2021-02-13T01:12:37Z- handleDepth()-Depth_Keller msg: 1613178757134 epoch sec; seqNo:19505; depth 0.14 2021-02-13T01:12:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178757.14 epoch sec; seqNo:19505; 4061019.02 north; 593824.03 east 2021-02-13T01:12:37Z- AHRS_M2 msg: 1613178757.53 epoch sec; seqNo:19352 2021-02-13T01:12:37Z- AHRS_M2 msg: 5.12 phi; 0.07 theta; 0.07 psi 2021-02-13T01:12:37Z- handleDepth()-Depth_Keller msg: 1613178757537 epoch sec; seqNo:19506; depth 0.16 2021-02-13T01:12:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178757.56 epoch sec; seqNo:19506; 4061018.65 north; 593824.03 east 2021-02-13T01:12:37Z- AHRS_M2 msg: 1613178757.94 epoch sec; seqNo:19353 2021-02-13T01:12:37Z- AHRS_M2 msg: 5.14 phi; 0.13 theta; 0.11 psi 2021-02-13T01:12:37Z- handleDepth()-Depth_Keller msg: 1613178757940 epoch sec; seqNo:19507; depth 0.17 2021-02-13T01:12:37Z- handleDvl() - RDI_Pathfinder msg: 1613178757.98 epoch sec; seqNo:9368 2021-02-13T01:12:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.72 , 97.72 , 97.72 , 97.72 2021-02-13T01:12:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.62 , -0.08 , -0.30 2021-02-13T01:12:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178757.99 epoch sec; seqNo:19507; 4061019.02 north; 593824.03 east 2021-02-13T01:12:38Z- AHRS_M2 msg: 1613178758.34 epoch sec; seqNo:19354 2021-02-13T01:12:38Z- AHRS_M2 msg: 5.12 phi; 0.15 theta; 0.02 psi 2021-02-13T01:12:38Z- handleDepth()-Depth_Keller msg: 1613178758344 epoch sec; seqNo:19508; depth 0.12 2021-02-13T01:12:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178758.36 epoch sec; seqNo:19508; 4061019.02 north; 593824.03 east 2021-02-13T01:12:38Z- AHRS_M2 msg: 1613178758.75 epoch sec; seqNo:19355 2021-02-13T01:12:38Z- AHRS_M2 msg: 5.11 phi; 0.10 theta; -0.13 psi 2021-02-13T01:12:38Z- handleDepth()-Depth_Keller msg: 1613178758761 epoch sec; seqNo:19509; depth 0.09 2021-02-13T01:12:38Z- handleDvl() - RDI_Pathfinder msg: 1613178758.77 epoch sec; seqNo:9369 2021-02-13T01:12:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 102.66 , 97.25 , 105.36 , 97.25 2021-02-13T01:12:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.24 , 0.22 , 0.66 2021-02-13T01:12:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178758.78 epoch sec; seqNo:19509; 4061019.02 north; 593824.03 east 2021-02-13T01:12:39Z- AHRS_M2 msg: 1613178759.15 epoch sec; seqNo:19356 2021-02-13T01:12:39Z- AHRS_M2 msg: 5.09 phi; 0.06 theta; -0.25 psi 2021-02-13T01:12:39Z- handleDepth()-Depth_Keller msg: 1613178759155 epoch sec; seqNo:19510; depth 0.08 2021-02-13T01:12:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178759.17 epoch sec; seqNo:19510; 4061019.39 north; 593822.80 east 2021-02-13T01:12:39Z- AHRS_M2 msg: 1613178759.55 epoch sec; seqNo:19357 2021-02-13T01:12:39Z- AHRS_M2 msg: 5.10 phi; -0.01 theta; -0.29 psi 2021-02-13T01:12:39Z- handleDepth()-Depth_Keller msg: 1613178759556 epoch sec; seqNo:19511; depth 0.02 2021-02-13T01:12:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178759.57 epoch sec; seqNo:19511; 4061019.39 north; 593822.80 east 2021-02-13T01:12:39Z- AHRS_M2 msg: 1613178759.96 epoch sec; seqNo:19358 2021-02-13T01:12:39Z- AHRS_M2 msg: 5.13 phi; -0.05 theta; -0.23 psi 2021-02-13T01:12:39Z- handleDepth()-Depth_Keller msg: 1613178759962 epoch sec; seqNo:19512; depth 0.02 2021-02-13T01:12:39Z- handleDvl() - RDI_Pathfinder msg: 1613178759.97 epoch sec; seqNo:9370 2021-02-13T01:12:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.98 , 117.26 , 103.74 , 96.98 2021-02-13T01:12:39Z- handleDvl() - RDI_Pathfinder msg: velocities -0.49 , -0.08 , 0.30 2021-02-13T01:12:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178759.98 epoch sec; seqNo:19512; 4061019.40 north; 593824.02 east 2021-02-13T01:12:40Z- AHRS_M2 msg: 1613178760.36 epoch sec; seqNo:19359 2021-02-13T01:12:40Z- AHRS_M2 msg: 5.13 phi; -0.01 theta; -0.06 psi 2021-02-13T01:12:40Z- handleDepth()-Depth_Keller msg: 1613178760369 epoch sec; seqNo:19513; depth 0.04 2021-02-13T01:12:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178760.38 epoch sec; seqNo:19513; 4061019.40 north; 593824.02 east 2021-02-13T01:12:40Z- AHRS_M2 msg: 1613178760.76 epoch sec; seqNo:19360 2021-02-13T01:12:40Z- AHRS_M2 msg: 5.14 phi; -0.02 theta; 0.04 psi 2021-02-13T01:12:40Z- handleDepth()-Depth_Keller msg: 1613178760778 epoch sec; seqNo:19514; depth 0.04 2021-02-13T01:12:40Z- handleDvl() - RDI_Pathfinder msg: 1613178760.79 epoch sec; seqNo:9371 2021-02-13T01:12:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.79 , 100.76 , 97.79 , 99.28 2021-02-13T01:12:40Z- handleDvl() - RDI_Pathfinder msg: velocities -0.13 , 0.01 , 0.09 2021-02-13T01:12:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178760.81 epoch sec; seqNo:19514; 4061019.40 north; 593824.02 east 2021-02-13T01:12:41Z- AHRS_M2 msg: 1613178761.17 epoch sec; seqNo:19361 2021-02-13T01:12:41Z- AHRS_M2 msg: 5.14 phi; -0.09 theta; 0.05 psi 2021-02-13T01:12:41Z- handleDepth()-Depth_Keller msg: 1613178761173 epoch sec; seqNo:19515; depth 0.08 2021-02-13T01:12:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178761.20 epoch sec; seqNo:19515; 4061019.40 north; 593824.02 east 2021-02-13T01:12:41Z- LcmTrn::handleMessages() - handling 41 messages took 0.65 ms and 128 calls... 2021-02-13T01:12:41Z- waiting for 1613178759.34, time is 1613178761.20 2021-02-13T01:12:41Z- LcmTrn::updateTrn() >>>> pose time 1613178761.20, meas time 1613178760.79 <<<< 2021-02-13T01:12:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178760.79 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:12:41Z- TerrainNav::motionUpdate() - currEstimate = (4061019, 593824.02, 0.08) 2021-02-13T01:12:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:12:41Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:12:41Z- TNavBF::Projecting Measurements 2021-02-13T01:12:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:12:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:12:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:12:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:12:41Z- TNavFilter::4 good beams. 2021-02-13T01:12:41Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:12:41Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:12:41Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:12:41Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:12:41Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:12:41Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:12:41Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:12:41Z- TNavBF::mmse selected from filter -1 2021-02-13T01:12:41Z- TerrainMapOctree: (vehN, vehE ) = (4061019.44, 593824.02). 2021-02-13T01:12:41Z- TerrainMapOctree: Min Distance = 484.37. 2021-02-13T01:12:41Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:12:41Z- There is no specific good beam that all particles have in common. 2021-02-13T01:12:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:12:41Z- Old Depth Bias: 0.00 DepthBiasCov: 0.54 K: 0.031 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-13T01:12:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:12:41Z- Old Depth Bias: 0.00 DepthBiasCov: 0.52 K: 0.028 ExpectedMeasDiff: nan currMeas.covariance: 16.25 2021-02-13T01:12:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:12:41Z- Old Depth Bias: 0.00 DepthBiasCov: 0.51 K: 0.029 ExpectedMeasDiff: nan currMeas.covariance: 15.30 2021-02-13T01:12:41Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:12:41Z- Old Depth Bias: 0.00 DepthBiasCov: 0.49 K: 0.027 ExpectedMeasDiff: nan currMeas.covariance: 15.77 2021-02-13T01:12:41Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:12:41Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:12:41Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:12:41Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:12:41Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:41Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:12:41Z- Filter 0 SubcloudNIS: 0.355015 2021-02-13T01:12:41Z- Filter 0 No beams used in subcloud update 2021-02-13T01:12:41Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:12:41Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:12:41Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -2.7 NorthVariance: 672.9 EastVariance: 833.0 DepthVariance: 30.5 this->DepthBiasCov: 0.5 2021-02-13T01:12:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178760.79 sec. 2021-02-13T01:12:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:12:41Z- TNavBF::mmse selected from filter -1 2021-02-13T01:12:41Z- variances([0], [2], and psi_berg) = 672.872745 832.985566 0.000000 2021-02-13T01:12:41Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:12:41Z- LcmTrn::publishEstimates() - MLE : 4061018.90 593839.52 0.08 2021-02-13T01:12:41Z- LcmTrn::publishEstimates() - MMSE : 4061019.40 593824.02 -2.60 2021-02-13T01:12:41Z- LcmTrn::publishEstimates() - COVAR: 672.87 832.99 30.50 0.00 2021-02-13T01:12:41Z- AHRS_M2 msg: 1613178761.57 epoch sec; seqNo:19362 2021-02-13T01:12:41Z- AHRS_M2 msg: 5.13 phi; -0.05 theta; -0.03 psi 2021-02-13T01:12:41Z- handleDepth()-Depth_Keller msg: 1613178761577 epoch sec; seqNo:19516; depth 0.09 2021-02-13T01:12:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178761.59 epoch sec; seqNo:19516; 4061019.40 north; 593824.02 east 2021-02-13T01:12:41Z- AHRS_M2 msg: 1613178761.99 epoch sec; seqNo:19363 2021-02-13T01:12:41Z- AHRS_M2 msg: 5.07 phi; 0.05 theta; -0.28 psi 2021-02-13T01:12:41Z- handleDepth()-Depth_Keller msg: 1613178761995 epoch sec; seqNo:19517; depth 0.04 2021-02-13T01:12:42Z- handleDvl() - RDI_Pathfinder msg: 1613178762.01 epoch sec; seqNo:9372 2021-02-13T01:12:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 102.52 , 94.41 , 97.11 , 103.88 2021-02-13T01:12:42Z- handleDvl() - RDI_Pathfinder msg: velocities -0.07 , 0.13 , 0.16 2021-02-13T01:12:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178762.02 epoch sec; seqNo:19517; 4061019.40 north; 593824.02 east 2021-02-13T01:12:42Z- AHRS_M2 msg: 1613178762.38 epoch sec; seqNo:19364 2021-02-13T01:12:42Z- AHRS_M2 msg: 5.06 phi; -0.02 theta; -0.27 psi 2021-02-13T01:12:42Z- handleDepth()-Depth_Keller msg: 1613178762396 epoch sec; seqNo:19518; depth 0.00 2021-02-13T01:12:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178762.41 epoch sec; seqNo:19518; 4061019.40 north; 593824.02 east 2021-02-13T01:12:42Z- AHRS_M2 msg: 1613178762.79 epoch sec; seqNo:19365 2021-02-13T01:12:42Z- AHRS_M2 msg: 5.11 phi; -0.15 theta; -0.15 psi 2021-02-13T01:12:42Z- handleDepth()-Depth_Keller msg: 1613178762789 epoch sec; seqNo:19519; depth 0.03 2021-02-13T01:12:42Z- handleDvl() - RDI_Pathfinder msg: 1613178762.80 epoch sec; seqNo:9373 2021-02-13T01:12:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.00 , 118.75 , 105.23 , 98.46 2021-02-13T01:12:42Z- handleDvl() - RDI_Pathfinder msg: velocities -0.66 , -0.03 , 0.82 2021-02-13T01:12:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178762.82 epoch sec; seqNo:19519; 4061019.40 north; 593824.02 east 2021-02-13T01:12:43Z- AHRS_M2 msg: 1613178763.19 epoch sec; seqNo:19366 2021-02-13T01:12:43Z- AHRS_M2 msg: 5.14 phi; -0.23 theta; -0.06 psi 2021-02-13T01:12:43Z- handleDepth()-Depth_Keller msg: 1613178763197 epoch sec; seqNo:19520; depth 0.05 2021-02-13T01:12:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178763.21 epoch sec; seqNo:19520; 4061019.02 north; 593824.03 east 2021-02-13T01:12:43Z- AHRS_M2 msg: 1613178763.60 epoch sec; seqNo:19367 2021-02-13T01:12:43Z- AHRS_M2 msg: 5.15 phi; -0.25 theta; -0.03 psi 2021-02-13T01:12:43Z- handleDepth()-Depth_Keller msg: 1613178763600 epoch sec; seqNo:19521; depth 0.06 2021-02-13T01:12:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178763.61 epoch sec; seqNo:19521; 4061019.04 north; 593825.25 east 2021-02-13T01:12:43Z- AHRS_M2 msg: 1613178763.99 epoch sec; seqNo:19368 2021-02-13T01:12:43Z- AHRS_M2 msg: 5.22 phi; -0.14 theta; -0.02 psi 2021-02-13T01:12:44Z- handleDepth()-Depth_Keller msg: 1613178764010 epoch sec; seqNo:19522; depth 0.10 2021-02-13T01:12:44Z- handleDvl() - RDI_Pathfinder msg: 1613178764.02 epoch sec; seqNo:9374 2021-02-13T01:12:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.86 , 105.29 , 97.86 , 109.76 2021-02-13T01:12:44Z- handleDvl() - RDI_Pathfinder msg: velocities -1.07 , -0.12 , -1.10 2021-02-13T01:12:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178764.03 epoch sec; seqNo:19522; 4061018.66 north; 593825.25 east 2021-02-13T01:12:44Z- AHRS_M2 msg: 1613178764.40 epoch sec; seqNo:19369 2021-02-13T01:12:44Z- AHRS_M2 msg: 5.28 phi; -0.01 theta; -0.03 psi 2021-02-13T01:12:44Z- handleDepth()-Depth_Keller msg: 1613178764406 epoch sec; seqNo:19523; depth 0.06 2021-02-13T01:12:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178764.42 epoch sec; seqNo:19523; 4061018.66 north; 593825.25 east 2021-02-13T01:12:44Z- AHRS_M2 msg: 1613178764.80 epoch sec; seqNo:19370 2021-02-13T01:12:44Z- AHRS_M2 msg: 5.25 phi; 0.01 theta; -0.07 psi 2021-02-13T01:12:44Z- handleDepth()-Depth_Keller msg: 1613178764813 epoch sec; seqNo:19524; depth 0.05 2021-02-13T01:12:44Z- handleDvl() - RDI_Pathfinder msg: 1613178764.83 epoch sec; seqNo:9375 2021-02-13T01:12:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2021-02-13T01:12:44Z- handleDvl() - RDI_Pathfinder msg: velocities -32.77 , -32.77 , -32.77 2021-02-13T01:12:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178764.84 epoch sec; seqNo:19524; 4061018.29 north; 593826.47 east 2021-02-13T01:12:45Z- AHRS_M2 msg: 1613178765.21 epoch sec; seqNo:19371 2021-02-13T01:12:45Z- AHRS_M2 msg: 5.23 phi; 0.03 theta; -0.18 psi 2021-02-13T01:12:45Z- handleDepth()-Depth_Keller msg: 1613178765214 epoch sec; seqNo:19525; depth 0.04 2021-02-13T01:12:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178765.23 epoch sec; seqNo:19525; 4061017.92 north; 593826.48 east 2021-02-13T01:12:45Z- AHRS_M2 msg: 1613178765.61 epoch sec; seqNo:19372 2021-02-13T01:12:45Z- AHRS_M2 msg: 5.26 phi; 0.10 theta; -0.29 psi 2021-02-13T01:12:45Z- handleDepth()-Depth_Keller msg: 1613178765621 epoch sec; seqNo:19526; depth 0.04 2021-02-13T01:12:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178765.64 epoch sec; seqNo:19526; 4061017.54 north; 593826.48 east 2021-02-13T01:12:46Z- AHRS_M2 msg: 1613178766.02 epoch sec; seqNo:19373 2021-02-13T01:12:46Z- AHRS_M2 msg: 5.27 phi; 0.18 theta; -0.23 psi 2021-02-13T01:12:46Z- handleDepth()-Depth_Keller msg: 1613178766025 epoch sec; seqNo:19527; depth 0.03 2021-02-13T01:12:46Z- handleDvl() - RDI_Pathfinder msg: 1613178766.04 epoch sec; seqNo:9376 2021-02-13T01:12:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.25 , 98.60 , 97.25 , 97.25 2021-02-13T01:12:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.52 , 0.07 , 0.43 2021-02-13T01:12:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178766.06 epoch sec; seqNo:19527; 4061017.54 north; 593826.48 east 2021-02-13T01:12:46Z- AHRS_M2 msg: 1613178766.42 epoch sec; seqNo:19374 2021-02-13T01:12:46Z- AHRS_M2 msg: 5.29 phi; 0.17 theta; -0.12 psi 2021-02-13T01:12:46Z- handleDepth()-Depth_Keller msg: 1613178766429 epoch sec; seqNo:19528; depth 0.05 2021-02-13T01:12:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178766.44 epoch sec; seqNo:19528; 4061017.92 north; 593826.48 east 2021-02-13T01:12:46Z- LcmTrn::handleMessages() - handling 44 messages took 0.63 ms and 132 calls... 2021-02-13T01:12:46Z- waiting for 1613178764.20, time is 1613178766.44 2021-02-13T01:12:46Z- LcmTrn::updateTrn() >>>> pose time 1613178766.44, meas time 1613178766.04 <<<< 2021-02-13T01:12:46Z- TerrainNav:: Throwing out all beams because more than two are equal to 97.247 at t=1613178766.04, ping # 0. 2021-02-13T01:12:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178766.04 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:12:46Z- TerrainNav::motionUpdate() - currEstimate = (4061018, 593826.48, 0.05) 2021-02-13T01:12:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:12:46Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:12:46Z- TNavBF::Projecting Measurements 2021-02-13T01:12:46Z- TNavFilter::Measurement from time = 1613178766.04 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T01:12:46Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:12:46Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:12:46Z- TNavBF:: beamIndex[1] = 0 2021-02-13T01:12:46Z- TNavBF:: beamIndex[2] = 0 2021-02-13T01:12:46Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:12:46Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:12:46Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:12:46Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -2.7 NorthVariance: 672.8 EastVariance: 833.0 DepthVariance: 30.6 this->DepthBiasCov: 0.6 2021-02-13T01:12:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:12:46Z- TNavBF::mmse selected from filter -1 2021-02-13T01:12:46Z- variances([0], [2], and psi_berg) = 672.829952 832.985549 0.000000 2021-02-13T01:12:46Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:12:46Z- LcmTrn::publishEstimates() - MLE : 4061017.42 593841.98 0.05 2021-02-13T01:12:46Z- LcmTrn::publishEstimates() - MMSE : 4061017.92 593826.48 -2.63 2021-02-13T01:12:46Z- LcmTrn::publishEstimates() - COVAR: 672.83 832.99 30.65 0.00 2021-02-13T01:12:46Z- AHRS_M2 msg: 1613178766.83 epoch sec; seqNo:19375 2021-02-13T01:12:46Z- AHRS_M2 msg: 5.31 phi; 0.05 theta; -0.01 psi 2021-02-13T01:12:46Z- handleDepth()-Depth_Keller msg: 1613178766830 epoch sec; seqNo:19529; depth 0.01 2021-02-13T01:12:46Z- handleDvl() - RDI_Pathfinder msg: 1613178766.84 epoch sec; seqNo:9377 2021-02-13T01:12:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.25 , 117.54 , 114.83 , 97.25 2021-02-13T01:12:46Z- handleDvl() - RDI_Pathfinder msg: velocities -0.14 , 0.12 , 1.50 2021-02-13T01:12:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178766.85 epoch sec; seqNo:19529; 4061017.92 north; 593826.48 east 2021-02-13T01:12:47Z- AHRS_M2 msg: 1613178767.23 epoch sec; seqNo:19376 2021-02-13T01:12:47Z- AHRS_M2 msg: 5.26 phi; -0.07 theta; 0.07 psi 2021-02-13T01:12:47Z- handleDepth()-Depth_Keller msg: 1613178767233 epoch sec; seqNo:19530; depth 0.04 2021-02-13T01:12:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178767.26 epoch sec; seqNo:19530; 4061017.92 north; 593826.48 east 2021-02-13T01:12:47Z- AHRS_M2 msg: 1613178767.63 epoch sec; seqNo:19377 2021-02-13T01:12:47Z- AHRS_M2 msg: 5.26 phi; -0.13 theta; 0.05 psi 2021-02-13T01:12:47Z- handleDepth()-Depth_Keller msg: 1613178767636 epoch sec; seqNo:19531; depth 0.04 2021-02-13T01:12:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178767.64 epoch sec; seqNo:19531; 4061017.92 north; 593826.48 east 2021-02-13T01:12:48Z- AHRS_M2 msg: 1613178768.04 epoch sec; seqNo:19378 2021-02-13T01:12:48Z- AHRS_M2 msg: 5.31 phi; -0.17 theta; -0.06 psi 2021-02-13T01:12:48Z- handleDepth()-Depth_Keller msg: 1613178768040 epoch sec; seqNo:19532; depth -0.01 2021-02-13T01:12:48Z- handleDvl() - RDI_Pathfinder msg: 1613178768.05 epoch sec; seqNo:9378 2021-02-13T01:12:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 103.47 , 97.52 , 100.49 , 99.00 2021-02-13T01:12:48Z- handleDvl() - RDI_Pathfinder msg: velocities -1.52 , -0.12 , -0.27 2021-02-13T01:12:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178768.06 epoch sec; seqNo:19532; 4061017.54 north; 593826.48 east 2021-02-13T01:12:48Z- AHRS_M2 msg: 1613178768.44 epoch sec; seqNo:19379 2021-02-13T01:12:48Z- AHRS_M2 msg: 5.34 phi; -0.17 theta; -0.10 psi 2021-02-13T01:12:48Z- handleDepth()-Depth_Keller msg: 1613178768453 epoch sec; seqNo:19533; depth -0.01 2021-02-13T01:12:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178768.46 epoch sec; seqNo:19533; 4061017.17 north; 593827.71 east 2021-02-13T01:12:48Z- AHRS_M2 msg: 1613178768.84 epoch sec; seqNo:19380 2021-02-13T01:12:48Z- AHRS_M2 msg: 5.34 phi; -0.09 theta; -0.10 psi 2021-02-13T01:12:48Z- handleDepth()-Depth_Keller msg: 1613178768858 epoch sec; seqNo:19534; depth 0.03 2021-02-13T01:12:48Z- handleDvl() - RDI_Pathfinder msg: 1613178768.87 epoch sec; seqNo:9379 2021-02-13T01:12:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.69 , 102.45 , 97.04 , 110.57 2021-02-13T01:12:48Z- handleDvl() - RDI_Pathfinder msg: velocities -0.27 , 0.04 , -1.23 2021-02-13T01:12:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178768.88 epoch sec; seqNo:19534; 4061016.79 north; 593827.71 east 2021-02-13T01:12:49Z- AHRS_M2 msg: 1613178769.25 epoch sec; seqNo:19381 2021-02-13T01:12:49Z- AHRS_M2 msg: 5.32 phi; 0.01 theta; -0.14 psi 2021-02-13T01:12:49Z- handleDepth()-Depth_Keller msg: 1613178769257 epoch sec; seqNo:19535; depth 0.07 2021-02-13T01:12:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178769.27 epoch sec; seqNo:19535; 4061016.79 north; 593827.71 east 2021-02-13T01:12:49Z- AHRS_M2 msg: 1613178769.66 epoch sec; seqNo:19382 2021-02-13T01:12:49Z- AHRS_M2 msg: 5.33 phi; 0.03 theta; -0.14 psi 2021-02-13T01:12:49Z- handleDepth()-Depth_Keller msg: 1613178769663 epoch sec; seqNo:19536; depth 0.08 2021-02-13T01:12:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178769.70 epoch sec; seqNo:19536; 4061016.79 north; 593827.71 east 2021-02-13T01:12:50Z- AHRS_M2 msg: 1613178770.06 epoch sec; seqNo:19383 2021-02-13T01:12:50Z- AHRS_M2 msg: 5.36 phi; -0.00 theta; -0.05 psi 2021-02-13T01:12:50Z- handleDepth()-Depth_Keller msg: 1613178770062 epoch sec; seqNo:19537; depth 0.09 2021-02-13T01:12:50Z- handleDvl() - RDI_Pathfinder msg: 1613178770.07 epoch sec; seqNo:9380 2021-02-13T01:12:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.99 , 103.94 , 97.99 , 99.48 2021-02-13T01:12:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.60 , -0.14 , -0.12 2021-02-13T01:12:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178770.08 epoch sec; seqNo:19537; 4061016.79 north; 593827.71 east 2021-02-13T01:12:50Z- AHRS_M2 msg: 1613178770.46 epoch sec; seqNo:19384 2021-02-13T01:12:50Z- AHRS_M2 msg: 5.36 phi; -0.03 theta; 0.04 psi 2021-02-13T01:12:50Z- handleDepth()-Depth_Keller msg: 1613178770469 epoch sec; seqNo:19538; depth 0.11 2021-02-13T01:12:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178770.49 epoch sec; seqNo:19538; 4061016.79 north; 593827.71 east 2021-02-13T01:12:50Z- AHRS_M2 msg: 1613178770.86 epoch sec; seqNo:19385 2021-02-13T01:12:50Z- AHRS_M2 msg: 5.35 phi; -0.04 theta; 0.08 psi 2021-02-13T01:12:50Z- handleDepth()-Depth_Keller msg: 1613178770868 epoch sec; seqNo:19539; depth 0.12 2021-02-13T01:12:50Z- handleDvl() - RDI_Pathfinder msg: 1613178770.88 epoch sec; seqNo:9381 2021-02-13T01:12:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 98.40 , 95.69 , 97.04 2021-02-13T01:12:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.02 , 0.01 , 0.18 2021-02-13T01:12:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178770.90 epoch sec; seqNo:19539; 4061016.78 north; 593826.49 east 2021-02-13T01:12:51Z- AHRS_M2 msg: 1613178771.27 epoch sec; seqNo:19386 2021-02-13T01:12:51Z- AHRS_M2 msg: 5.32 phi; -0.07 theta; 0.01 psi 2021-02-13T01:12:51Z- handleDepth()-Depth_Keller msg: 1613178771272 epoch sec; seqNo:19540; depth 0.12 2021-02-13T01:12:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178771.28 epoch sec; seqNo:19540; 4061016.78 north; 593826.49 east 2021-02-13T01:12:51Z- AHRS_M2 msg: 1613178771.68 epoch sec; seqNo:19387 2021-02-13T01:12:51Z- AHRS_M2 msg: 5.35 phi; -0.13 theta; -0.17 psi 2021-02-13T01:12:51Z- handleDepth()-Depth_Keller msg: 1613178771681 epoch sec; seqNo:19541; depth 0.09 2021-02-13T01:12:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178771.69 epoch sec; seqNo:19541; 4061016.79 north; 593827.71 east 2021-02-13T01:12:51Z- LcmTrn::handleMessages() - handling 44 messages took 0.74 ms and 130 calls... 2021-02-13T01:12:51Z- waiting for 1613178769.44, time is 1613178771.69 2021-02-13T01:12:51Z- LcmTrn::updateTrn() >>>> pose time 1613178771.69, meas time 1613178770.88 <<<< 2021-02-13T01:12:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178770.88 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:12:51Z- TerrainNav::motionUpdate() - currEstimate = (4061017, 593827.71, 0.09) 2021-02-13T01:12:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:12:51Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:12:51Z- TNavBF::Projecting Measurements 2021-02-13T01:12:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:12:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:12:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2021-02-13T01:12:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:12:51Z- TNavFilter::4 good beams. 2021-02-13T01:12:51Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:12:51Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:12:51Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:12:51Z- TNavBF:: beamIndex[2] = 2 2021-02-13T01:12:51Z- TNavBF:: beamIndex[3] = 3 2021-02-13T01:12:51Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:12:51Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:12:51Z- TNavBF::mmse selected from filter -1 2021-02-13T01:12:51Z- TerrainMapOctree: (vehN, vehE ) = (4061016.97, 593827.52). 2021-02-13T01:12:51Z- TerrainMapOctree: Min Distance = 482.49. 2021-02-13T01:12:51Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:12:51Z- There is no specific good beam that all particles have in common. 2021-02-13T01:12:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:12:51Z- Old Depth Bias: 0.00 DepthBiasCov: 0.70 K: 0.040 ExpectedMeasDiff: nan currMeas.covariance: 15.07 2021-02-13T01:12:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:12:51Z- Old Depth Bias: 0.00 DepthBiasCov: 0.67 K: 0.037 ExpectedMeasDiff: nan currMeas.covariance: 15.49 2021-02-13T01:12:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:12:51Z- Old Depth Bias: 0.00 DepthBiasCov: 0.64 K: 0.038 ExpectedMeasDiff: nan currMeas.covariance: 14.65 2021-02-13T01:12:51Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 3 2021-02-13T01:12:51Z- Old Depth Bias: 0.00 DepthBiasCov: 0.62 K: 0.036 ExpectedMeasDiff: nan currMeas.covariance: 15.07 2021-02-13T01:12:51Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:12:51Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:12:51Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:12:51Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:12:51Z- Filter 0 beam number: 3 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:12:51Z- Filter 0 insufficient particles or particle weight in subcloud for beam 3 2021-02-13T01:12:51Z- Filter 0 SubcloudNIS: 0.355030 2021-02-13T01:12:51Z- Filter 0 No beams used in subcloud update 2021-02-13T01:12:51Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:12:51Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:12:51Z- Filter 0 NorthBias: -0.0 EastBias: -0.0 DepthBias: -2.7 NorthVariance: 672.8 EastVariance: 833.0 DepthVariance: 30.6 this->DepthBiasCov: 0.6 2021-02-13T01:12:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178770.88 sec. 2021-02-13T01:12:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:12:51Z- TNavBF::mmse selected from filter -1 2021-02-13T01:12:51Z- variances([0], [2], and psi_berg) = 672.795389 832.985536 0.000000 2021-02-13T01:12:51Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:12:51Z- LcmTrn::publishEstimates() - MLE : 4061016.29 593843.21 0.09 2021-02-13T01:12:51Z- LcmTrn::publishEstimates() - MMSE : 4061016.79 593827.71 -2.59 2021-02-13T01:12:51Z- LcmTrn::publishEstimates() - COVAR: 672.80 832.99 30.63 0.00 2021-02-13T01:12:52Z- AHRS_M2 msg: 1613178772.08 epoch sec; seqNo:19388 2021-02-13T01:12:52Z- AHRS_M2 msg: 5.42 phi; -0.13 theta; -0.26 psi 2021-02-13T01:12:52Z- handleDepth()-Depth_Keller msg: 1613178772080 epoch sec; seqNo:19542; depth 0.07 2021-02-13T01:12:52Z- handleDvl() - RDI_Pathfinder msg: 1613178772.10 epoch sec; seqNo:9382 2021-02-13T01:12:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.37 , 97.32 , 97.32 , 97.32 2021-02-13T01:12:52Z- handleDvl() - RDI_Pathfinder msg: velocities -0.34 , 0.07 , 0.50 2021-02-13T01:12:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178772.11 epoch sec; seqNo:19542; 4061016.79 north; 593827.71 east 2021-02-13T01:12:52Z- AHRS_M2 msg: 1613178772.48 epoch sec; seqNo:19389 2021-02-13T01:12:52Z- AHRS_M2 msg: 5.45 phi; -0.09 theta; -0.16 psi 2021-02-13T01:12:52Z- handleDepth()-Depth_Keller msg: 1613178772489 epoch sec; seqNo:19543; depth 0.04 2021-02-13T01:12:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178772.52 epoch sec; seqNo:19543; 4061016.79 north; 593827.71 east 2021-02-13T01:12:52Z- AHRS_M2 msg: 1613178772.88 epoch sec; seqNo:19390 2021-02-13T01:12:52Z- AHRS_M2 msg: 5.38 phi; -0.14 theta; -0.07 psi 2021-02-13T01:12:52Z- handleDepth()-Depth_Keller msg: 1613178772893 epoch sec; seqNo:19544; depth 0.06 2021-02-13T01:12:53Z- handleDvl() - RDI_Pathfinder msg: 1613178773.22 epoch sec; seqNo:9383 2021-02-13T01:12:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.18 , 114.76 , 97.18 , 106.65 2021-02-13T01:12:53Z- handleDvl() - RDI_Pathfinder msg: velocities -0.30 , 0.19 , -0.06 2021-02-13T01:12:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178773.25 epoch sec; seqNo:19544; 4061016.79 north; 593827.71 east 2021-02-13T01:12:53Z- AHRS_M2 msg: 1613178773.35 epoch sec; seqNo:19391 2021-02-13T01:12:53Z- AHRS_M2 msg: 5.38 phi; -0.19 theta; -0.11 psi 2021-02-13T01:12:53Z- handleDepth()-Depth_Keller msg: 1613178773362 epoch sec; seqNo:19545; depth 0.05 2021-02-13T01:12:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178773.38 epoch sec; seqNo:19545; 4061016.79 north; 593827.71 east 2021-02-13T01:12:53Z- AHRS_M2 msg: 1613178773.74 epoch sec; seqNo:19392 2021-02-13T01:12:53Z- AHRS_M2 msg: 5.46 phi; -0.14 theta; -0.15 psi 2021-02-13T01:12:53Z- handleDepth()-Depth_Keller msg: 1613178773748 epoch sec; seqNo:19546; depth 0.03 2021-02-13T01:12:53Z- handleDvl() - RDI_Pathfinder msg: 1613178773.78 epoch sec; seqNo:9384 2021-02-13T01:12:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.79 , 103.74 , 90.35 , 109.69 2021-02-13T01:12:53Z- handleDvl() - RDI_Pathfinder msg: velocities -0.40 , -0.27 , -0.49 2021-02-13T01:12:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178773.79 epoch sec; seqNo:19546; 4061016.79 north; 593827.71 east 2021-02-13T01:12:54Z- AHRS_M2 msg: 1613178774.15 epoch sec; seqNo:19393 2021-02-13T01:12:54Z- AHRS_M2 msg: 5.53 phi; -0.09 theta; -0.11 psi 2021-02-13T01:12:54Z- handleDepth()-Depth_Keller msg: 1613178774153 epoch sec; seqNo:19547; depth -0.01 2021-02-13T01:12:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178774.19 epoch sec; seqNo:19547; 4061008.21 north; 593874.19 east 2021-02-13T01:12:54Z- AHRS_M2 msg: 1613178774.55 epoch sec; seqNo:19394 2021-02-13T01:12:54Z- AHRS_M2 msg: 5.61 phi; -0.05 theta; 0.01 psi 2021-02-13T01:12:54Z- handleDepth()-Depth_Keller msg: 1613178774565 epoch sec; seqNo:19548; depth 0.02 2021-02-13T01:12:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178774.60 epoch sec; seqNo:19548; 4061007.83 north; 593874.19 east 2021-02-13T01:12:55Z- AHRS_M2 msg: 1613178775.01 epoch sec; seqNo:19395 2021-02-13T01:12:55Z- AHRS_M2 msg: 5.51 phi; 0.09 theta; -0.01 psi 2021-02-13T01:12:55Z- handleDepth()-Depth_Keller msg: 1613178775017 epoch sec; seqNo:19549; depth 0.06 2021-02-13T01:12:55Z- handleDvl() - RDI_Pathfinder msg: 1613178775.03 epoch sec; seqNo:9385 2021-02-13T01:12:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.98 , 99.68 , 94.27 , 96.98 2021-02-13T01:12:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.20 , 0.38 , -0.06 2021-02-13T01:12:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178775.04 epoch sec; seqNo:19549; 4061008.21 north; 593874.19 east 2021-02-13T01:12:55Z- AHRS_M2 msg: 1613178775.41 epoch sec; seqNo:19396 2021-02-13T01:12:55Z- AHRS_M2 msg: 5.54 phi; 0.07 theta; -0.03 psi 2021-02-13T01:12:55Z- handleDepth()-Depth_Keller msg: 1613178775422 epoch sec; seqNo:19550; depth 0.06 2021-02-13T01:12:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178775.43 epoch sec; seqNo:19550; 4061008.21 north; 593874.19 east 2021-02-13T01:12:55Z- AHRS_M2 msg: 1613178775.81 epoch sec; seqNo:19397 2021-02-13T01:12:55Z- AHRS_M2 msg: 5.61 phi; -0.01 theta; 0.01 psi 2021-02-13T01:12:55Z- handleDepth()-Depth_Keller msg: 1613178775821 epoch sec; seqNo:19551; depth 0.03 2021-02-13T01:12:55Z- handleDvl() - RDI_Pathfinder msg: 1613178775.84 epoch sec; seqNo:9386 2021-02-13T01:12:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.32 , 97.32 , 102.72 , 97.32 2021-02-13T01:12:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.09 , -0.33 , 0.53 2021-02-13T01:12:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178775.85 epoch sec; seqNo:19551; 4061008.21 north; 593874.19 east 2021-02-13T01:12:56Z- AHRS_M2 msg: 1613178776.21 epoch sec; seqNo:19398 2021-02-13T01:12:56Z- AHRS_M2 msg: 5.61 phi; 0.03 theta; 0.09 psi 2021-02-13T01:12:56Z- handleDepth()-Depth_Keller msg: 1613178776216 epoch sec; seqNo:19552; depth 0.09 2021-02-13T01:12:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178776.24 epoch sec; seqNo:19552; 4061008.19 north; 593872.97 east 2021-02-13T01:12:56Z- AHRS_M2 msg: 1613178776.62 epoch sec; seqNo:19399 2021-02-13T01:12:56Z- AHRS_M2 msg: 5.59 phi; 0.08 theta; 0.04 psi 2021-02-13T01:12:56Z- handleDepth()-Depth_Keller msg: 1613178776621 epoch sec; seqNo:19553; depth 0.11 2021-02-13T01:12:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178776.65 epoch sec; seqNo:19553; 4061008.19 north; 593872.97 east 2021-02-13T01:12:56Z- LcmTrn::handleMessages() - handling 41 messages took 0.64 ms and 128 calls... 2021-02-13T01:12:56Z- waiting for 1613178774.69, time is 1613178776.65 2021-02-13T01:12:56Z- LcmTrn::updateTrn() >>>> pose time 1613178776.65, meas time 1613178775.84 <<<< 2021-02-13T01:12:56Z- TerrainNav:: Throwing out all beams because more than two are equal to 97.315 at t=1613178775.84, ping # 0. 2021-02-13T01:12:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178775.84 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:12:56Z- TerrainNav::motionUpdate() - currEstimate = (4061008, 593872.97, 0.11) 2021-02-13T01:12:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:12:56Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:12:56Z- TNavBF::Projecting Measurements 2021-02-13T01:12:56Z- TNavFilter::Measurement from time = 1613178775.84 sec, ping # 0 not included; there are no good beams from the sonar. 2021-02-13T01:12:56Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:12:56Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:12:56Z- TNavBF:: beamIndex[1] = 0 2021-02-13T01:12:56Z- TNavBF:: beamIndex[2] = 0 2021-02-13T01:12:56Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:12:56Z- TNavBF:: *** beamsVF.Ncols() = 4 *** 2021-02-13T01:12:56Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:12:56Z- Filter 0 NorthBias: -0.0 EastBias: 0.0 DepthBias: -2.7 NorthVariance: 672.7 EastVariance: 833.0 DepthVariance: 32.6 this->DepthBiasCov: 2.6 2021-02-13T01:12:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:12:56Z- TNavBF::mmse selected from filter -1 2021-02-13T01:12:56Z- variances([0], [2], and psi_berg) = 672.612922 832.985464 0.000000 2021-02-13T01:12:56Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:12:56Z- LcmTrn::publishEstimates() - MLE : 4061007.69 593862.47 0.11 2021-02-13T01:12:56Z- LcmTrn::publishEstimates() - MMSE : 4061008.19 593872.97 -2.58 2021-02-13T01:12:56Z- LcmTrn::publishEstimates() - COVAR: 672.61 832.99 32.98 0.00 2021-02-13T01:12:57Z- AHRS_M2 msg: 1613178777.02 epoch sec; seqNo:19400 2021-02-13T01:12:57Z- AHRS_M2 msg: 5.56 phi; 0.03 theta; -0.10 psi 2021-02-13T01:12:57Z- handleDepth()-Depth_Keller msg: 1613178777024 epoch sec; seqNo:19554; depth 0.04 2021-02-13T01:12:57Z- handleDvl() - RDI_Pathfinder msg: 1613178777.05 epoch sec; seqNo:9387 2021-02-13T01:12:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.89 , 94.47 , 98.53 , 97.18 2021-02-13T01:12:57Z- handleDvl() - RDI_Pathfinder msg: velocities -0.37 , 0.34 , 0.64 2021-02-13T01:12:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178777.06 epoch sec; seqNo:19554; 4061008.19 north; 593872.97 east 2021-02-13T01:12:57Z- AHRS_M2 msg: 1613178777.42 epoch sec; seqNo:19401 2021-02-13T01:12:57Z- AHRS_M2 msg: 5.55 phi; -0.04 theta; -0.17 psi 2021-02-13T01:12:57Z- handleDepth()-Depth_Keller msg: 1613178777433 epoch sec; seqNo:19555; depth 0.01 2021-02-13T01:12:57Z- handleDvl() - RDI_Pathfinder msg: 1613178777.98 epoch sec; seqNo:9388 2021-02-13T01:12:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.25 , 102.66 , 102.66 , 97.25 2021-02-13T01:12:57Z- handleDvl() - RDI_Pathfinder msg: velocities -0.91 , 0.11 , -0.00 2021-02-13T01:12:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178777.99 epoch sec; seqNo:19555; 4061007.45 north; 593874.20 east 2021-02-13T01:12:58Z- AHRS_M2 msg: 1613178778.09 epoch sec; seqNo:19402 2021-02-13T01:12:58Z- AHRS_M2 msg: 5.64 phi; -0.14 theta; -0.18 psi 2021-02-13T01:12:58Z- handleDepth()-Depth_Keller msg: 1613178778100 epoch sec; seqNo:19556; depth -0.01 2021-02-13T01:13:00Z- handleDvl() - RDI_Pathfinder msg: 1613178780.15 epoch sec; seqNo:9389 2021-02-13T01:13:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 105.16 , 97.04 , 107.86 2021-02-13T01:13:00Z- handleDvl() - RDI_Pathfinder msg: velocities -0.23 , 0.06 , -1.18 2021-02-13T01:13:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178780.17 epoch sec; seqNo:19556; 4061007.07 north; 593874.20 east 2021-02-13T01:13:00Z- AHRS_M2 msg: 1613178780.28 epoch sec; seqNo:19403 2021-02-13T01:13:00Z- AHRS_M2 msg: 5.71 phi; -0.00 theta; -0.06 psi 2021-02-13T01:13:00Z- handleDepth()-Depth_Keller msg: 1613178780284 epoch sec; seqNo:19557; depth 0.20 2021-02-13T01:13:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178780.30 epoch sec; seqNo:19557; 4061007.07 north; 593874.20 east 2021-02-13T01:13:00Z- AHRS_M2 msg: 1613178780.67 epoch sec; seqNo:19404 2021-02-13T01:13:00Z- AHRS_M2 msg: 5.74 phi; -0.01 theta; -0.01 psi 2021-02-13T01:13:00Z- handleDepth()-Depth_Keller msg: 1613178780673 epoch sec; seqNo:19558; depth 0.18 2021-02-13T01:13:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178780.70 epoch sec; seqNo:19558; 4061007.07 north; 593874.20 east 2021-02-13T01:13:01Z- AHRS_M2 msg: 1613178781.07 epoch sec; seqNo:19405 2021-02-13T01:13:01Z- AHRS_M2 msg: 5.75 phi; -0.11 theta; 0.05 psi 2021-02-13T01:13:01Z- handleDepth()-Depth_Keller msg: 1613178781081 epoch sec; seqNo:19559; depth 0.22 2021-02-13T01:13:01Z- handleDvl() - RDI_Pathfinder msg: 1613178781.09 epoch sec; seqNo:9390 2021-02-13T01:13:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.11 , 95.76 , 99.82 , 97.11 2021-02-13T01:13:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.56 , -0.15 , 0.83 2021-02-13T01:13:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178781.11 epoch sec; seqNo:19559; 4061007.07 north; 593874.20 east 2021-02-13T01:13:01Z- AHRS_M2 msg: 1613178781.48 epoch sec; seqNo:19406 2021-02-13T01:13:01Z- AHRS_M2 msg: 5.72 phi; -0.17 theta; 0.07 psi 2021-02-13T01:13:01Z- handleDepth()-Depth_Keller msg: 1613178781480 epoch sec; seqNo:19560; depth 0.26 2021-02-13T01:13:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178781.49 epoch sec; seqNo:19560; 4061007.45 north; 593874.20 east 2021-02-13T01:13:01Z- AHRS_M2 msg: 1613178781.88 epoch sec; seqNo:19407 2021-02-13T01:13:01Z- AHRS_M2 msg: 5.71 phi; -0.22 theta; 0.02 psi 2021-02-13T01:13:01Z- handleDepth()-Depth_Keller msg: 1613178781886 epoch sec; seqNo:19561; depth 0.26 2021-02-13T01:13:01Z- handleDvl() - RDI_Pathfinder msg: 1613178781.90 epoch sec; seqNo:9391 2021-02-13T01:13:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.31 , 97.25 , 93.19 , 105.36 2021-02-13T01:13:01Z- handleDvl() - RDI_Pathfinder msg: velocities -0.79 , 0.35 , 0.36 2021-02-13T01:13:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178781.91 epoch sec; seqNo:19561; 4061007.07 north; 593874.20 east 2021-02-13T01:13:01Z- LcmTrn::handleMessages() - handling 29 messages took 0.43 ms and 121 calls... 2021-02-13T01:13:01Z- waiting for 1613178779.65, time is 1613178781.91 2021-02-13T01:13:01Z- LcmTrn::updateTrn() equating pose 1613178781.91 and meas times 1613178781.91 2021-02-13T01:13:01Z- LcmTrn::updateTrn() >>>> pose time 1613178781.91, meas time 1613178781.91 <<<< 2021-02-13T01:13:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178781.91 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:13:01Z- TerrainNav::motionUpdate() - currEstimate = (4061007, 593874.20, 0.26) 2021-02-13T01:13:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:13:01Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:13:01Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178781.91 sec.; No relevant navigation data available 2021-02-13T01:13:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:13:01Z- TNavBF::mmse selected from filter -1 2021-02-13T01:13:01Z- variances([0], [2], and psi_berg) = 672.586653 832.985454 0.000000 2021-02-13T01:13:01Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:13:01Z- LcmTrn::publishEstimates() - MLE : 4061006.57 593863.70 0.26 2021-02-13T01:13:01Z- LcmTrn::publishEstimates() - MMSE : 4061007.07 593874.20 -2.43 2021-02-13T01:13:01Z- LcmTrn::publishEstimates() - COVAR: 672.59 832.99 33.06 0.00 2021-02-13T01:13:02Z- AHRS_M2 msg: 1613178782.28 epoch sec; seqNo:19408 2021-02-13T01:13:02Z- AHRS_M2 msg: 5.74 phi; -0.25 theta; -0.06 psi 2021-02-13T01:13:02Z- handleDepth()-Depth_Keller msg: 1613178782289 epoch sec; seqNo:19562; depth 0.23 2021-02-13T01:13:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178782.31 epoch sec; seqNo:19562; 4061007.07 north; 593874.20 east 2021-02-13T01:13:02Z- AHRS_M2 msg: 1613178782.69 epoch sec; seqNo:19409 2021-02-13T01:13:02Z- AHRS_M2 msg: 5.77 phi; -0.27 theta; -0.12 psi 2021-02-13T01:13:02Z- handleDepth()-Depth_Keller msg: 1613178782701 epoch sec; seqNo:19563; depth 0.22 2021-02-13T01:13:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178782.75 epoch sec; seqNo:19563; 4061006.69 north; 593874.20 east 2021-02-13T01:13:03Z- AHRS_M2 msg: 1613178783.09 epoch sec; seqNo:19410 2021-02-13T01:13:03Z- AHRS_M2 msg: 5.74 phi; -0.28 theta; -0.11 psi 2021-02-13T01:13:03Z- handleDepth()-Depth_Keller msg: 1613178783096 epoch sec; seqNo:19564; depth 0.21 2021-02-13T01:13:03Z- handleDvl() - RDI_Pathfinder msg: 1613178783.11 epoch sec; seqNo:9392 2021-02-13T01:13:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 99.82 , 101.17 , 89.00 , 116.05 2021-02-13T01:13:03Z- handleDvl() - RDI_Pathfinder msg: velocities -0.59 , 0.47 , -0.51 2021-02-13T01:13:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178783.12 epoch sec; seqNo:19564; 4061006.71 north; 593875.42 east 2021-02-13T01:13:03Z- AHRS_M2 msg: 1613178783.49 epoch sec; seqNo:19411 2021-02-13T01:13:03Z- AHRS_M2 msg: 5.70 phi; -0.28 theta; -0.11 psi 2021-02-13T01:13:03Z- handleDepth()-Depth_Keller msg: 1613178783509 epoch sec; seqNo:19565; depth 0.20 2021-02-13T01:13:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178783.52 epoch sec; seqNo:19565; 4061006.71 north; 593875.42 east 2021-02-13T01:13:03Z- AHRS_M2 msg: 1613178783.90 epoch sec; seqNo:19412 2021-02-13T01:13:03Z- AHRS_M2 msg: 5.70 phi; -0.34 theta; -0.13 psi 2021-02-13T01:13:03Z- handleDepth()-Depth_Keller msg: 1613178783909 epoch sec; seqNo:19566; depth 0.16 2021-02-13T01:13:03Z- handleDvl() - RDI_Pathfinder msg: 1613178783.93 epoch sec; seqNo:9393 2021-02-13T01:13:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.50 , 106.92 , 87.58 , 120.31 2021-02-13T01:13:03Z- handleDvl() - RDI_Pathfinder msg: velocities -0.06 , -0.06 , -0.59 2021-02-13T01:13:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178783.94 epoch sec; seqNo:19566; 4061006.71 north; 593875.42 east 2021-02-13T01:13:04Z- handleDepth()-Depth_Keller msg: 1613178784325 epoch sec; seqNo:19567; depth 0.16 2021-02-13T01:13:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178784.34 epoch sec; seqNo:19567; 4061006.71 north; 593875.42 east 2021-02-13T01:13:04Z- AHRS_M2 msg: 1613178784.71 epoch sec; seqNo:19413 2021-02-13T01:13:04Z- AHRS_M2 msg: 5.74 phi; -0.41 theta; 0.06 psi 2021-02-13T01:13:04Z- handleDepth()-Depth_Keller msg: 1613178784721 epoch sec; seqNo:19568; depth 0.19 2021-02-13T01:13:04Z- handleDvl() - RDI_Pathfinder msg: 1613178784.73 epoch sec; seqNo:9394 2021-02-13T01:13:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 100.83 , 112.73 , 85.95 , 0.00 2021-02-13T01:13:04Z- handleDvl() - RDI_Pathfinder msg: velocities -0.06 , -0.26 , -0.42 2021-02-13T01:13:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178784.74 epoch sec; seqNo:19568; 4061006.71 north; 593875.42 east 2021-02-13T01:13:05Z- AHRS_M2 msg: 1613178785.11 epoch sec; seqNo:19414 2021-02-13T01:13:05Z- AHRS_M2 msg: 5.74 phi; -0.30 theta; 0.09 psi 2021-02-13T01:13:05Z- handleDepth()-Depth_Keller msg: 1613178785119 epoch sec; seqNo:19569; depth 0.22 2021-02-13T01:13:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178785.13 epoch sec; seqNo:19569; 4061006.71 north; 593875.42 east 2021-02-13T01:13:05Z- AHRS_M2 msg: 1613178785.52 epoch sec; seqNo:19415 2021-02-13T01:13:05Z- AHRS_M2 msg: 5.73 phi; -0.19 theta; 0.03 psi 2021-02-13T01:13:05Z- handleDepth()-Depth_Keller msg: 1613178785525 epoch sec; seqNo:19570; depth 0.19 2021-02-13T01:13:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178785.54 epoch sec; seqNo:19570; 4061006.69 north; 593874.20 east 2021-02-13T01:13:05Z- AHRS_M2 msg: 1613178785.92 epoch sec; seqNo:19416 2021-02-13T01:13:05Z- AHRS_M2 msg: 5.74 phi; -0.16 theta; -0.07 psi 2021-02-13T01:13:05Z- handleDepth()-Depth_Keller msg: 1613178785924 epoch sec; seqNo:19571; depth 0.15 2021-02-13T01:13:05Z- handleDvl() - RDI_Pathfinder msg: 1613178785.94 epoch sec; seqNo:9395 2021-02-13T01:13:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 107.86 , 97.04 , 0.00 , 117.33 2021-02-13T01:13:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.49 , -0.21 , 0.41 2021-02-13T01:13:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178785.96 epoch sec; seqNo:19571; 4061006.69 north; 593874.20 east 2021-02-13T01:13:06Z- AHRS_M2 msg: 1613178786.32 epoch sec; seqNo:19417 2021-02-13T01:13:06Z- AHRS_M2 msg: 5.76 phi; -0.22 theta; -0.08 psi 2021-02-13T01:13:06Z- handleDepth()-Depth_Keller msg: 1613178786331 epoch sec; seqNo:19572; depth 0.13 2021-02-13T01:13:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178786.34 epoch sec; seqNo:19572; 4061006.69 north; 593874.20 east 2021-02-13T01:13:06Z- AHRS_M2 msg: 1613178786.73 epoch sec; seqNo:19418 2021-02-13T01:13:06Z- AHRS_M2 msg: 5.74 phi; -0.26 theta; 0.01 psi 2021-02-13T01:13:06Z- handleDepth()-Depth_Keller msg: 1613178786737 epoch sec; seqNo:19573; depth 0.24 2021-02-13T01:13:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178786.76 epoch sec; seqNo:19573; 4061006.69 north; 593874.20 east 2021-02-13T01:13:07Z- LcmTrn::handleMessages() - handling 39 messages took 0.69 ms and 126 calls... 2021-02-13T01:13:07Z- waiting for 1613178784.91, time is 1613178786.76 2021-02-13T01:13:07Z- LcmTrn::updateTrn() >>>> pose time 1613178786.76, meas time 1613178785.94 <<<< 2021-02-13T01:13:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178785.94 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:13:07Z- TerrainNav::motionUpdate() - currEstimate = (4061007, 593874.20, 0.24) 2021-02-13T01:13:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:13:07Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:13:07Z- TNavBF::Projecting Measurements 2021-02-13T01:13:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2021-02-13T01:13:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2021-02-13T01:13:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2021-02-13T01:13:07Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2021-02-13T01:13:07Z- TNavFilter::3 good beams. 2021-02-13T01:13:07Z- TNavBF::Measurements Projected, beam correspondences: 2021-02-13T01:13:07Z- TNavBF:: beamIndex[0] = 0 2021-02-13T01:13:07Z- TNavBF:: beamIndex[1] = 1 2021-02-13T01:13:07Z- TNavBF:: beamIndex[2] = 3 2021-02-13T01:13:07Z- TNavBF:: beamIndex[3] = 0 2021-02-13T01:13:07Z- TNavBF:: *** beamsVF.Ncols() = 3 *** 2021-02-13T01:13:07Z- TNavBF:: *** currMeas.numMeas = 4 *** 2021-02-13T01:13:07Z- TNavBF::mmse selected from filter -1 2021-02-13T01:13:07Z- TerrainMapOctree: (vehN, vehE ) = (4061006.76, 593874.20). 2021-02-13T01:13:07Z- TerrainMapOctree: Min Distance = 447.65. 2021-02-13T01:13:07Z- TerrainMapOctree: Using tile 1. 2021-02-13T01:13:07Z- There is no specific good beam that all particles have in common. 2021-02-13T01:13:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 0 2021-02-13T01:13:07Z- Old Depth Bias: 0.00 DepthBiasCov: 3.06 K: 0.131 ExpectedMeasDiff: nan currMeas.covariance: 18.62 2021-02-13T01:13:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 1 2021-02-13T01:13:07Z- Old Depth Bias: 0.00 DepthBiasCov: 2.66 K: 0.137 ExpectedMeasDiff: nan currMeas.covariance: 15.07 2021-02-13T01:13:07Z- Filter 0 deapthBias would have been set to Nan. indexP: 0 indexM: 2 2021-02-13T01:13:07Z- Old Depth Bias: 0.00 DepthBiasCov: 2.30 K: 0.088 ExpectedMeasDiff: nan currMeas.covariance: 22.03 2021-02-13T01:13:07Z- Filter 0 Weighting particles with subcloud comparison 2021-02-13T01:13:07Z- Filter 0 beam number: 0 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:13:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 0 2021-02-13T01:13:07Z- Filter 0 beam number: 1 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:13:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 1 2021-02-13T01:13:07Z- Filter 0 beam number: 2 num in subcloud: 0 num not in subcloud: 10000 2021-02-13T01:13:07Z- Filter 0 insufficient particles or particle weight in subcloud for beam 2 2021-02-13T01:13:07Z- Filter 0 SubcloudNIS: 0.355082 2021-02-13T01:13:07Z- Filter 0 No beams used in subcloud update 2021-02-13T01:13:07Z- TNavBF:: Filter 0 sumSquaredError = 0.000000 2021-02-13T01:13:07Z- TNavBF:: Filter 0 sumWeights = 1.000000 2021-02-13T01:13:07Z- Filter 0 NorthBias: 0.0 EastBias: 0.0 DepthBias: -2.7 NorthVariance: 672.6 EastVariance: 833.0 DepthVariance: 32.1 this->DepthBiasCov: 2.1 2021-02-13T01:13:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1613178785.94 sec. 2021-02-13T01:13:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:13:07Z- TNavBF::mmse selected from filter -1 2021-02-13T01:13:07Z- variances([0], [2], and psi_berg) = 672.570093 832.985448 0.000000 2021-02-13T01:13:07Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:13:07Z- LcmTrn::publishEstimates() - MLE : 4061006.19 593888.70 0.24 2021-02-13T01:13:07Z- LcmTrn::publishEstimates() - MMSE : 4061006.69 593874.20 -2.44 2021-02-13T01:13:07Z- LcmTrn::publishEstimates() - COVAR: 672.57 832.99 32.11 0.00 2021-02-13T01:13:07Z- AHRS_M2 msg: 1613178787.13 epoch sec; seqNo:19419 2021-02-13T01:13:07Z- AHRS_M2 msg: 5.74 phi; -0.20 theta; 0.05 psi 2021-02-13T01:13:07Z- handleDepth()-Depth_Keller msg: 1613178787147 epoch sec; seqNo:19574; depth 0.23 2021-02-13T01:13:07Z- handleDvl() - RDI_Pathfinder msg: 1613178787.16 epoch sec; seqNo:9396 2021-02-13T01:13:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.45 , 103.40 , 97.45 , 109.35 2021-02-13T01:13:07Z- handleDvl() - RDI_Pathfinder msg: velocities -0.69 , 0.27 , 0.48 2021-02-13T01:13:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178787.17 epoch sec; seqNo:19574; 4061006.69 north; 593874.20 east 2021-02-13T01:13:07Z- AHRS_M2 msg: 1613178787.54 epoch sec; seqNo:19420 2021-02-13T01:13:07Z- AHRS_M2 msg: 5.72 phi; -0.14 theta; 0.04 psi 2021-02-13T01:13:07Z- handleDepth()-Depth_Keller msg: 1613178787541 epoch sec; seqNo:19575; depth 0.21 2021-02-13T01:13:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178787.55 epoch sec; seqNo:19575; 4061006.31 north; 593874.21 east 2021-02-13T01:13:07Z- AHRS_M2 msg: 1613178787.94 epoch sec; seqNo:19421 2021-02-13T01:13:07Z- AHRS_M2 msg: 5.70 phi; -0.10 theta; -0.03 psi 2021-02-13T01:13:07Z- handleDepth()-Depth_Keller msg: 1613178787949 epoch sec; seqNo:19576; depth 0.19 2021-02-13T01:13:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178788.00 epoch sec; seqNo:19576; 4061006.33 north; 593875.43 east 2021-02-13T01:13:08Z- AHRS_M2 msg: 1613178788.34 epoch sec; seqNo:19422 2021-02-13T01:13:08Z- AHRS_M2 msg: 5.69 phi; -0.07 theta; -0.14 psi 2021-02-13T01:13:08Z- handleDepth()-Depth_Keller msg: 1613178788350 epoch sec; seqNo:19577; depth 0.16 2021-02-13T01:13:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178788.36 epoch sec; seqNo:19577; 4061005.95 north; 593875.43 east 2021-02-13T01:13:08Z- AHRS_M2 msg: 1613178788.76 epoch sec; seqNo:19423 2021-02-13T01:13:08Z- AHRS_M2 msg: 5.73 phi; -0.06 theta; -0.18 psi 2021-02-13T01:13:08Z- handleDepth()-Depth_Keller msg: 1613178788763 epoch sec; seqNo:19578; depth 0.13 2021-02-13T01:13:08Z- handleDvl() - RDI_Pathfinder msg: 1613178788.78 epoch sec; seqNo:9397 2021-02-13T01:13:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.92 , 103.88 , 94.95 , 102.39 2021-02-13T01:13:08Z- handleDvl() - RDI_Pathfinder msg: velocities -0.23 , -0.09 , -0.14 2021-02-13T01:13:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178788.79 epoch sec; seqNo:19578; 4061005.95 north; 593875.43 east 2021-02-13T01:13:09Z- AHRS_M2 msg: 1613178789.15 epoch sec; seqNo:19424 2021-02-13T01:13:09Z- AHRS_M2 msg: 5.77 phi; -0.07 theta; -0.07 psi 2021-02-13T01:13:09Z- handleDepth()-Depth_Keller msg: 1613178789161 epoch sec; seqNo:19579; depth 0.11 2021-02-13T01:13:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178789.18 epoch sec; seqNo:19579; 4061005.95 north; 593875.43 east 2021-02-13T01:13:09Z- AHRS_M2 msg: 1613178789.56 epoch sec; seqNo:19425 2021-02-13T01:13:09Z- AHRS_M2 msg: 5.77 phi; -0.13 theta; 0.08 psi 2021-02-13T01:13:09Z- handleDepth()-Depth_Keller msg: 1613178789561 epoch sec; seqNo:19580; depth 0.14 2021-02-13T01:13:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178789.57 epoch sec; seqNo:19580; 4061005.95 north; 593875.43 east 2021-02-13T01:13:09Z- AHRS_M2 msg: 1613178789.96 epoch sec; seqNo:19426 2021-02-13T01:13:09Z- AHRS_M2 msg: 5.75 phi; -0.16 theta; 0.13 psi 2021-02-13T01:13:09Z- handleDepth()-Depth_Keller msg: 1613178789985 epoch sec; seqNo:19581; depth 0.12 2021-02-13T01:13:10Z- handleDvl() - RDI_Pathfinder msg: 1613178790.09 epoch sec; seqNo:9398 2021-02-13T01:13:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 97.04 , 97.04 , 99.75 2021-02-13T01:13:10Z- handleDvl() - RDI_Pathfinder msg: velocities -0.38 , 0.26 , -0.44 2021-02-13T01:13:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178790.10 epoch sec; seqNo:19581; 4061005.57 north; 593875.44 east 2021-02-13T01:13:10Z- AHRS_M2 msg: 1613178790.36 epoch sec; seqNo:19427 2021-02-13T01:13:10Z- AHRS_M2 msg: 5.72 phi; -0.12 theta; 0.05 psi 2021-02-13T01:13:10Z- handleDepth()-Depth_Keller msg: 1613178790368 epoch sec; seqNo:19582; depth 0.05 2021-02-13T01:13:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178790.55 epoch sec; seqNo:19582; 4061005.57 north; 593875.44 east 2021-02-13T01:13:10Z- AHRS_M2 msg: 1613178790.82 epoch sec; seqNo:19428 2021-02-13T01:13:10Z- AHRS_M2 msg: 5.73 phi; -0.03 theta; -0.16 psi 2021-02-13T01:13:10Z- handleDepth()-Depth_Keller msg: 1613178790827 epoch sec; seqNo:19583; depth 0.02 2021-02-13T01:13:10Z- handleDvl() - RDI_Pathfinder msg: 1613178790.86 epoch sec; seqNo:9399 2021-02-13T01:13:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 101.31 , 97.25 , 97.25 , 105.36 2021-02-13T01:13:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.34 , -0.20 , -0.25 2021-02-13T01:13:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178790.88 epoch sec; seqNo:19583; 4061005.57 north; 593875.44 east 2021-02-13T01:13:11Z- handleDepth()-Depth_Keller msg: 1613178791190 epoch sec; seqNo:19584; depth 0.03 2021-02-13T01:13:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178791.23 epoch sec; seqNo:19584; 4061005.95 north; 593875.43 east 2021-02-13T01:13:11Z- AHRS_M2 msg: 1613178791.58 epoch sec; seqNo:19429 2021-02-13T01:13:11Z- AHRS_M2 msg: 5.72 phi; 0.07 theta; -0.14 psi 2021-02-13T01:13:11Z- handleDepth()-Depth_Keller msg: 1613178791581 epoch sec; seqNo:19585; depth 0.02 2021-02-13T01:13:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178791.60 epoch sec; seqNo:19585; 4061005.95 north; 593875.43 east 2021-02-13T01:13:11Z- AHRS_M2 msg: 1613178791.99 epoch sec; seqNo:19430 2021-02-13T01:13:11Z- AHRS_M2 msg: 5.77 phi; 0.07 theta; 0.12 psi 2021-02-13T01:13:11Z- handleDepth()-Depth_Keller msg: 1613178791993 epoch sec; seqNo:19586; depth 0.05 2021-02-13T01:13:12Z- handleDvl() - RDI_Pathfinder msg: 1613178792.00 epoch sec; seqNo:9400 2021-02-13T01:13:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 97.04 , 107.86 , 99.75 , 97.04 2021-02-13T01:13:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.48 , -0.34 , 0.87 2021-02-13T01:13:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1613178792.03 epoch sec; seqNo:19586; 4061005.95 north; 593875.43 east 2021-02-13T01:13:12Z- LcmTrn::handleMessages() - handling 43 messages took 0.69 ms and 129 calls... 2021-02-13T01:13:12Z- waiting for 1613178789.76, time is 1613178792.03 2021-02-13T01:13:12Z- LcmTrn::updateTrn() equating pose 1613178792.03 and meas times 1613178792.03 2021-02-13T01:13:12Z- LcmTrn::updateTrn() >>>> pose time 1613178792.03, meas time 1613178792.03 <<<< 2021-02-13T01:13:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1613178792.03 sec, ping # 0; waiting for more recent INS data... 2021-02-13T01:13:12Z- TerrainNav::motionUpdate() - currEstimate = (4061006, 593875.43, 0.05) 2021-02-13T01:13:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2021-02-13T01:13:12Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2021-02-13T01:13:12Z- TerrainNav::Measurement type 1 not incorporated from time = 1613178792.03 sec.; No relevant navigation data available 2021-02-13T01:13:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2021-02-13T01:13:12Z- TNavBF::mmse selected from filter -1 2021-02-13T01:13:12Z- variances([0], [2], and psi_berg) = 672.545724 832.985438 0.000000 2021-02-13T01:13:12Z- LcmTrn::publishEstimates() - reinits:0 filterstate:0 2021-02-13T01:13:12Z- LcmTrn::publishEstimates() - MLE : 4061005.45 593890.93 0.05 2021-02-13T01:13:12Z- LcmTrn::publishEstimates() - MMSE : 4061005.95 593875.43 -2.64 2021-02-13T01:13:12Z- LcmTrn::publishEstimates() - COVAR: 672.55 832.99 32.19 0.00 2021-02-13T01:13:12Z- AHRS_M2 msg: 1613178792.39 epoch sec; seqNo:19431 2021-02-13T01:13:12Z- AHRS_M2 msg: 5.82 phi; 0.15 theta; 0.20 psi 2021-02-13T01:13:12Z- handleDepth()-Depth_Keller msg: 1613178792395 epoch sec; seqNo:19587; depth 0.10